From aca9764ea3ed7ec7b55c774922e6460dd601f2d6 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 19 Sep 2021 21:57:18 -0500 Subject: [PATCH 001/947] In the middle of tracking down fairly egregious mesh reduction bugs, likely implying additional triangle test bugs. --- BepuPhysics/BoundingBoxHelpers.cs | 3 +- .../CollisionTasks/ConvexMeshContinuations.cs | 1 + .../CompoundMeshReduction.cs | 2 +- .../CollisionDetection/MeshReduction.cs | 117 ++++++++++++++++-- 4 files changed, 109 insertions(+), 14 deletions(-) diff --git a/BepuPhysics/BoundingBoxHelpers.cs b/BepuPhysics/BoundingBoxHelpers.cs index 5929164ae..8e452afc5 100644 --- a/BepuPhysics/BoundingBoxHelpers.cs +++ b/BepuPhysics/BoundingBoxHelpers.cs @@ -216,7 +216,8 @@ public static unsafe void ExpandLocalBoundingBoxes(ref Vector3Wide min, ref Vect [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void EpsilonExpandLocalBoundingBoxes(Vector maximumRadius, ref Vector3Wide min, ref Vector3Wide max) { - var expansion = maximumRadius * new Vector(1e-4f); + //var expansion = maximumRadius * new Vector(1e-4f); + var expansion = maximumRadius + new Vector(10f); Vector3Wide.Subtract(min, expansion, out min); Vector3Wide.Add(max, expansion, out max); } diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexMeshContinuations.cs b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexMeshContinuations.cs index 3774372e6..3a9b73349 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexMeshContinuations.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexMeshContinuations.cs @@ -20,6 +20,7 @@ public ref MeshReduction CreateContinuation( continuation.RequiresFlip = pair.FlipMask == 0; continuation.QueryBounds.Min = pairQuery.Min; continuation.QueryBounds.Max = pairQuery.Max; + unsafe { continuation.DebugMesh = Unsafe.AsRef(pairQuery.Container); } return ref continuation; } diff --git a/BepuPhysics/CollisionDetection/CompoundMeshReduction.cs b/BepuPhysics/CollisionDetection/CompoundMeshReduction.cs index 196cd0430..cea81e3aa 100644 --- a/BepuPhysics/CollisionDetection/CompoundMeshReduction.cs +++ b/BepuPhysics/CollisionDetection/CompoundMeshReduction.cs @@ -56,7 +56,7 @@ public unsafe bool TryFlush(int pairId, ref CollisionBatcher 0) { - MeshReduction.ReduceManifolds(ref Triangles, ref Inner.Children, region.Start, region.Count, RequiresFlip, QueryBounds[i], meshOrientation, meshInverseOrientation); + MeshReduction.ReduceManifolds(ref Triangles, ref Inner.Children, region.Start, region.Count, RequiresFlip, QueryBounds[i], meshOrientation, meshInverseOrientation, default); } } diff --git a/BepuPhysics/CollisionDetection/MeshReduction.cs b/BepuPhysics/CollisionDetection/MeshReduction.cs index 9fdf53e26..33f6ec7ff 100644 --- a/BepuPhysics/CollisionDetection/MeshReduction.cs +++ b/BepuPhysics/CollisionDetection/MeshReduction.cs @@ -1,4 +1,5 @@ using BepuPhysics.Collidables; +using BepuPhysics.Trees; using BepuUtilities; using BepuUtilities.Collections; using BepuUtilities.Memory; @@ -31,6 +32,8 @@ public struct MeshReduction : ICollisionTestContinuation //This uses all of the nonconvex reduction's logic, so we just nest it. public NonconvexReduction Inner; + public Mesh DebugMesh; + public void Create(int childManifoldCount, BufferPool pool) { Inner.Create(childManifoldCount, pool); @@ -75,7 +78,7 @@ private static unsafe void ComputeMeshSpaceContacts(ref ConvexContactManifold ma struct TestTriangle { - //The test triangle contains AOS-ified layouts for quicker per contact testing. + //The test triangle contains SOA-ified layouts for quicker per contact testing. public Vector4 AnchorX; public Vector4 AnchorY; public Vector4 AnchorZ; @@ -166,7 +169,7 @@ private static unsafe bool ShouldBlockNormal(in TestTriangle triangle, in Vector //Note that we are stricter about being on the edge than we were about being nearby. //That's because infringement checks require a normal infringement along every edge that the contact is on; //being too aggressive about edge classification would cause infringements to sometimes be ignored. - var negativeThreshold = triangle.DistanceThreshold * -1e-2f; + var negativeThreshold = triangle.DistanceThreshold * -1e-1f; var onAB = distanceAlongNormal.Y >= negativeThreshold; var onBC = distanceAlongNormal.Z >= negativeThreshold; var onCA = distanceAlongNormal.W >= negativeThreshold; @@ -193,7 +196,7 @@ private static unsafe bool ShouldBlockNormal(in TestTriangle triangle, in Vector //the normal is alinged with an edge. if ((onAB && normalDot.Y > infringementEpsilon) || (onBC && normalDot.Z > infringementEpsilon) || (onCA && normalDot.W > infringementEpsilon)) { - const float secondaryInfringementEpsilon = -1e-3f; + const float secondaryInfringementEpsilon = -1e-2f; //At least one edge is infringed. Are all contact-touched edges at least nearly infringed? if ((!onAB || normalDot.Y > secondaryInfringementEpsilon) && (!onBC || normalDot.Z > secondaryInfringementEpsilon) && (!onCA || normalDot.W > secondaryInfringementEpsilon)) { @@ -205,8 +208,18 @@ private static unsafe bool ShouldBlockNormal(in TestTriangle triangle, in Vector return false; } + struct DebugLeafEnumerator : IBreakableForEach + { + public QuickList List; + public bool LoopBody(int i) + { + List.AllocateUnsafely() = i; + return true; + } + } + public unsafe static void ReduceManifolds(ref Buffer continuationTriangles, ref Buffer continuationChildren, int start, int count, - bool requiresFlip, in BoundingBox queryBounds, in Matrix3x3 meshOrientation, in Matrix3x3 meshInverseOrientation) + bool requiresFlip, in BoundingBox queryBounds, in Matrix3x3 meshOrientation, in Matrix3x3 meshInverseOrientation, Mesh debugMesh) { //Before handing responsibility off to the nonconvex reduction, make sure that no contacts create nasty 'bumps' at the border of triangles. //Bumps can occur when an isolated triangle test detects a contact pointing outward, like when a box hits the side. This is fine when the triangle truly is isolated, @@ -243,22 +256,34 @@ public unsafe static void ReduceManifolds(ref Buffer continuationTrian continuationChildren.Slice(start, count, out var children); //Allocate enough space for all potential triangles, even though we're only going to be enumerating over the subset which actually have contacts. //Note that the count is limited by the above early-out; there are limits to how much this can allocate on the stack. - int activeChildCount = 0; + //int activeChildCount = 0; var memory = stackalloc TestTriangle[count]; var activeTriangles = new Buffer(memory, count); for (int i = 0; i < count; ++i) { - if (children[i].Manifold.Count > 0) + //if (children[i].Manifold.Count > 0) { - activeTriangles[activeChildCount] = new TestTriangle(triangles[i], i); - ++activeChildCount; + activeTriangles[i] = new TestTriangle(triangles[i], i); + //++activeChildCount; } } + var debugOverlapMemory = stackalloc int[count]; + var debugOverlapBuffer = new Buffer(debugOverlapMemory, count); + var debugKeyMemory = stackalloc int[count]; + var debugKeys = new Buffer(debugKeyMemory, count); + var debugValueMemory = stackalloc TestTriangle[count]; + var debugValues = new Buffer(debugValueMemory, count); + var debugTableSize = 1 << SpanHelper.GetContainingPowerOf2(count * 4); + var debugTableMemory = stackalloc int[debugTableSize]; + var debugTable = new Buffer(debugTableMemory, debugTableSize); + QuickDictionary> testTriangles = new(ref debugKeys, ref debugValues, ref debugTable); var meshSpaceContacts = stackalloc Vector3[4]; - for (int i = 0; i < activeChildCount; ++i) + for (int i = 0; i < count; ++i) { ref var sourceTriangle = ref activeTriangles[i]; ref var sourceChild = ref children[sourceTriangle.ChildIndex]; + if (sourceChild.Manifold.Count == 0) + continue; //Can't correct contacts that were created by face collisions. if ((sourceChild.Manifold.Contact0.FeatureId & FaceCollisionFlag) == 0) { @@ -283,7 +308,69 @@ public unsafe static void ReduceManifolds(ref Buffer continuationTrian } } ref var meshSpaceContact = ref meshSpaceContacts[deepestIndex]; - for (int j = 0; j < activeChildCount; ++j) + + { + ref var debugSourceTriangle = ref debugMesh.Triangles[sourceChild.ChildIndexB]; + var inverse = 1f / Vector3.Cross(debugSourceTriangle.B - debugSourceTriangle.A, debugSourceTriangle.C - debugSourceTriangle.A).Length(); + var wa = Vector3.Cross(debugSourceTriangle.B - debugSourceTriangle.A, meshSpaceContact - debugSourceTriangle.A).Length() * inverse; + var wb = Vector3.Cross(debugSourceTriangle.C - debugSourceTriangle.B, meshSpaceContact - debugSourceTriangle.B).Length() * inverse; + var wc = Vector3.Cross(debugSourceTriangle.A - debugSourceTriangle.C, meshSpaceContact - debugSourceTriangle.C).Length() * inverse; + if (wa + wb + wc > 1 + 1e-3f) + Console.WriteLine($"EXTERNAL source contact {wa}, {wb}, {wc}"); + else if (wa > 1e-3f && wb > 1e-3f && wc > 1e-3f) + Console.WriteLine($"Internal source contact {wa}, {wb}, {wc}"); + } + + var debugMin = meshSpaceContact - new Vector3(1e-3f); + var debugMax = meshSpaceContact + new Vector3(1e-3f); + var debugLeafEnumerator = new DebugLeafEnumerator(); + debugLeafEnumerator.List = new QuickList(debugOverlapBuffer); + debugMesh.Tree.GetOverlaps(debugMin, debugMax, ref debugLeafEnumerator); + for (int j = 0; j < debugLeafEnumerator.List.Count; ++j) + { + TestTriangle targetTriangle; + if (!testTriangles.TryGetValue(debugLeafEnumerator.List[j], out targetTriangle)) + { + //Search for the child... + int childIndexForTargetTriangle = -1; + for (int k = 0; k < children.Length; ++k) + { + if (children[k].ChildIndexB == debugLeafEnumerator.List[j]) + { + childIndexForTargetTriangle = k; + break; + } + } + if (childIndexForTargetTriangle == -1) + { + Console.WriteLine("Bad news bears: a child index that should have existed by query does not exist according to the mesh reduction child set."); + } + targetTriangle = new TestTriangle(debugMesh.Triangles[debugLeafEnumerator.List[j]], childIndexForTargetTriangle); + testTriangles.AddUnsafely(debugLeafEnumerator.List[j], targetTriangle); + } + + { + ref var debugTriangle = ref debugMesh.Triangles[debugLeafEnumerator.List[j]]; + var inverse = 1f / Vector3.Cross(debugTriangle.B - debugTriangle.A, debugTriangle.C - debugTriangle.A).Length(); + var wa = Vector3.Cross(debugTriangle.B - debugTriangle.A, meshSpaceContact - debugTriangle.A).Length() * inverse; + var wb = Vector3.Cross(debugTriangle.C - debugTriangle.B, meshSpaceContact - debugTriangle.B).Length() * inverse; + var wc = Vector3.Cross(debugTriangle.A - debugTriangle.C, meshSpaceContact - debugTriangle.C).Length() * inverse; + } + + if (ShouldBlockNormal(targetTriangle, meshSpaceContact, meshSpaceNormal)) + { + if (targetTriangle.ChildIndex != sourceTriangle.ChildIndex) + { + sourceTriangle.Blocked = true; + sourceTriangle.CorrectedNormal = new Vector3(targetTriangle.NX.X, targetTriangle.NY.X, targetTriangle.NZ.X); + //Even if the target manifold gets blocked, it should not necessarily be deleted. We made use of it as a blocker. + targetTriangle.ForceDeletionOnBlock = false; + break; + } + } + } + + for (int j = 0; j < count; ++j) { //No point in trying to check a normal against its own triangle. if (i != j) @@ -315,6 +402,12 @@ public unsafe static void ReduceManifolds(ref Buffer continuationTrian ConvexContactManifold.FastRemoveAt(ref sourceChild.Manifold, j); } } + + var testDot = Vector3.Dot(meshSpaceNormal, new Vector3(sourceTriangle.NX.X, sourceTriangle.NY.X, sourceTriangle.NZ.X)); + if (MathF.Abs(testDot) < 0.3f && !sourceTriangle.Blocked && sourceChild.Manifold.Count > 0) + { + Console.WriteLine($"Iffy dot: {testDot} NOT BLOCKED"); + } } else { @@ -326,7 +419,7 @@ public unsafe static void ReduceManifolds(ref Buffer continuationTrian } } } - for (int i = 0; i < activeChildCount; ++i) + for (int i = 0; i < count; ++i) { ref var triangle = ref activeTriangles[i]; if (triangle.Blocked) @@ -376,7 +469,7 @@ public unsafe bool TryFlush(int pairId, ref CollisionBatcher Date: Mon, 20 Sep 2021 21:19:04 -0500 Subject: [PATCH 002/947] Trying alternative to include vertex normals to satisfy meshreduction. --- .../CollisionTasks/TrianglePairTester.cs | 99 ++++++++++++++++--- 1 file changed, 86 insertions(+), 13 deletions(-) diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs index cdea34fcb..659cc9f24 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs @@ -42,6 +42,43 @@ static void TestEdgeEdge( //Protect against bad normals. depth = Vector.ConditionalSelect(Vector.LessThan(normalLength, new Vector(1e-10f)), new Vector(float.MaxValue), depth); } + + static void TestEdgeEdge2( + in Vector3Wide edgeStartA, in Vector3Wide edgeOffsetA, in Vector edgeOffsetALengthSquared, in Vector inverseEdgeOffsetALengthSquared, + in Vector3Wide edgeStartB, in Vector3Wide edgeOffsetB, in Vector edgeOffsetBLengthSquared, in Vector inverseEdgeOffsetBLengthSquared, + in Vector3Wide aA, in Vector3Wide bA, in Vector3Wide cA, in Vector3Wide aB, in Vector3Wide bB, in Vector3Wide cB, + out Vector depth, out Vector3Wide normal) + { + Vector3Wide.Subtract(edgeStartB, edgeStartA, out var aStartToBStart); + Vector3Wide.Dot(edgeOffsetA, aStartToBStart, out var oADotAB); + Vector3Wide.Dot(edgeOffsetB, aStartToBStart, out var oBDotAB); + Vector3Wide.Dot(edgeOffsetA, edgeOffsetB, out var oADotOB); + var denominator = edgeOffsetALengthSquared * edgeOffsetBLengthSquared - oADotOB * oADotOB; //TODO: div 0 guard. + //Compute the first guess for tA and clamp it. + var tA = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (oADotOB * oBDotAB - oADotAB * edgeOffsetBLengthSquared) / denominator)); + //Compute the closest point on B to the first guess. + var tB = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (oADotOB * tA + oBDotAB) * inverseEdgeOffsetBLengthSquared)); + //Finally, compute the true tA. + tA = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (tB * oADotOB - oADotAB) * inverseEdgeOffsetALengthSquared)); + + Vector3Wide.Scale(edgeOffsetA, tA, out var startToClosestA); + Vector3Wide.Add(edgeStartA, startToClosestA, out var a); + Vector3Wide.Scale(edgeOffsetB, tB, out var startToClosestB); + Vector3Wide.Add(edgeStartB, startToClosestB, out var b); + + //If the segments touch, then fall back to using the edge direction as the normal candidate. + Vector3Wide.Subtract(a, b, out var ba); + Vector3Wide.LengthSquared(ba, out var baLengthSquared); + var useFallback = Vector.LessThan(baLengthSquared, new Vector(1e-14f)); + Vector3Wide.CrossWithoutOverlap(edgeOffsetA, edgeOffsetB, out var fallbackNormal); + Vector3Wide.ConditionalSelect(useFallback, fallbackNormal, ba, out normal); + Vector3Wide.Length(normal, out var normalLength); + Vector3Wide.Scale(normal, Vector.One / normalLength, out normal); + GetDepthForNormal(aA, bA, cA, aB, bB, cB, normal, out depth); + //Protect against bad normals. + depth = Vector.ConditionalSelect(Vector.LessThan(normalLength, new Vector(1e-10f)), new Vector(float.MaxValue), depth); + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] static void Select( ref Vector depth, ref Vector3Wide normal, @@ -259,27 +296,63 @@ public unsafe void Test( Vector3Wide.Subtract(a.C, a.B, out var bcA); Vector3Wide.Subtract(a.A, a.C, out var caA); + ////A AB x * + //TestEdgeEdge(abA, abB, a.A, a.B, a.C, bA, bB, bC, out var depth, out var localNormal); + //TestEdgeEdge(abA, bcB, a.A, a.B, a.C, bA, bB, bC, out var depthCandidate, out var localNormalCandidate); + //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + //TestEdgeEdge(abA, caB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + + ////A BC x * + //TestEdgeEdge(bcA, abB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + //TestEdgeEdge(bcA, bcB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + //TestEdgeEdge(bcA, caB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + + ////A CA x * + //TestEdgeEdge(caA, abB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + //TestEdgeEdge(caA, bcB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + //TestEdgeEdge(caA, caB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + + Vector3Wide.LengthSquared(abA, out var abALengthSquared); + Vector3Wide.LengthSquared(bcA, out var bcALengthSquared); + Vector3Wide.LengthSquared(caA, out var caALengthSquared); + Vector3Wide.LengthSquared(abB, out var abBLengthSquared); + Vector3Wide.LengthSquared(bcB, out var bcBLengthSquared); + Vector3Wide.LengthSquared(caB, out var caBLengthSquared); + var inverseABALengthSquared = Vector.One / abALengthSquared; + var inverseBCALengthSquared = Vector.One / bcALengthSquared; + var inverseCAALengthSquared = Vector.One / caALengthSquared; + var inverseABBLengthSquared = Vector.One / abBLengthSquared; + var inverseBCBLengthSquared = Vector.One / bcBLengthSquared; + var inverseCABLengthSquared = Vector.One / caBLengthSquared; + //A AB x * - TestEdgeEdge(abA, abB, a.A, a.B, a.C, bA, bB, bC, out var depth, out var localNormal); - TestEdgeEdge(abA, bcB, a.A, a.B, a.C, bA, bB, bC, out var depthCandidate, out var localNormalCandidate); + TestEdgeEdge2(a.A, abA, abALengthSquared, inverseABALengthSquared, bA, abB, abBLengthSquared, inverseABBLengthSquared, a.A, a.B, a.C, bA, bB, bC, out var depth, out var localNormal); + TestEdgeEdge2(a.A, abA, abALengthSquared, inverseABALengthSquared, bB, bcB, bcBLengthSquared, inverseBCBLengthSquared, a.A, a.B, a.C, bA, bB, bC, out var depthCandidate, out var localNormalCandidate); Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - TestEdgeEdge(abA, caB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + TestEdgeEdge2(a.A, abA, abALengthSquared, inverseABALengthSquared, bC, caB, caBLengthSquared, inverseCABLengthSquared, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); //A BC x * - TestEdgeEdge(bcA, abB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + TestEdgeEdge2(a.B, bcA, bcALengthSquared, inverseBCALengthSquared, bA, abB, abBLengthSquared, inverseABBLengthSquared, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - TestEdgeEdge(bcA, bcB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + TestEdgeEdge2(a.B, bcA, bcALengthSquared, inverseBCALengthSquared, bB, bcB, bcBLengthSquared, inverseBCBLengthSquared, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - TestEdgeEdge(bcA, caB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + TestEdgeEdge2(a.B, bcA, bcALengthSquared, inverseBCALengthSquared, bC, caB, caBLengthSquared, inverseCABLengthSquared, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); //A CA x * - TestEdgeEdge(caA, abB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + TestEdgeEdge2(a.C, caA, caALengthSquared, inverseCAALengthSquared, bA, abB, abBLengthSquared, inverseABBLengthSquared, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - TestEdgeEdge(caA, bcB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + TestEdgeEdge2(a.C, caA, caALengthSquared, inverseCAALengthSquared, bB, bcB, bcBLengthSquared, inverseBCBLengthSquared, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - TestEdgeEdge(caA, caB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + TestEdgeEdge2(a.C, caA, caALengthSquared, inverseCAALengthSquared, bC, caB, caBLengthSquared, inverseCABLengthSquared, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); //Face normals @@ -357,7 +430,7 @@ public unsafe void Test( //We will be working on the surface of triangleB, but we'd still like a 2d parameterization of the surface for contact reduction. //So, we'll create tangent axes from the edge and edge x normal. - Vector3Wide.LengthSquared(abB, out var abBLengthSquared); + //Vector3Wide.LengthSquared(abB, out var abBLengthSquared); Vector3Wide.Scale(abB, Vector.One / Vector.SquareRoot(abBLengthSquared), out var tangentBX); Vector3Wide.CrossWithoutOverlap(tangentBX, faceNormalB, out var tangentBY); @@ -408,9 +481,9 @@ public unsafe void Test( } //Create a scale-sensitive epsilon for comparisons based on the size of the involved shapes. This helps avoid varying behavior based on how large involved objects are. - Vector3Wide.LengthSquared(abA, out var abALengthSquared); - Vector3Wide.LengthSquared(caA, out var caALengthSquared); - Vector3Wide.LengthSquared(caB, out var caBLengthSquared); + //Vector3Wide.LengthSquared(abA, out var abALengthSquared); + //Vector3Wide.LengthSquared(caA, out var caALengthSquared); + //Vector3Wide.LengthSquared(caB, out var caBLengthSquared); var epsilonScale = Vector.SquareRoot(Vector.Min( Vector.Max(abALengthSquared, caALengthSquared), Vector.Max(abBLengthSquared, caBLengthSquared))); From 5896cd048d51377540a53b7586fc26e41d44ba3a Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 20 Sep 2021 21:19:18 -0500 Subject: [PATCH 003/947] Typo. --- BepuPhysics/CollisionDetection/MeshReduction.cs | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/BepuPhysics/CollisionDetection/MeshReduction.cs b/BepuPhysics/CollisionDetection/MeshReduction.cs index 33f6ec7ff..a43487dea 100644 --- a/BepuPhysics/CollisionDetection/MeshReduction.cs +++ b/BepuPhysics/CollisionDetection/MeshReduction.cs @@ -193,7 +193,7 @@ private static unsafe bool ShouldBlockNormal(in TestTriangle triangle, in Vector //Further, note that we require nonzero positive infringement; otherwise, we'd end up blocking the contacts of a flat neighbor. //But we are a little more aggressive about blocking the *second* edge infringement- if it's merely parallel, we count it as infringing. //Otherwise you could get into situations where a contact on the vertex of a bunch of different triangles isn't blocked by any of them because - //the normal is alinged with an edge. + //the normal is aligned with an edge. if ((onAB && normalDot.Y > infringementEpsilon) || (onBC && normalDot.Z > infringementEpsilon) || (onCA && normalDot.W > infringementEpsilon)) { const float secondaryInfringementEpsilon = -1e-2f; @@ -267,13 +267,14 @@ public unsafe static void ReduceManifolds(ref Buffer continuationTrian //++activeChildCount; } } - var debugOverlapMemory = stackalloc int[count]; - var debugOverlapBuffer = new Buffer(debugOverlapMemory, count); - var debugKeyMemory = stackalloc int[count]; - var debugKeys = new Buffer(debugKeyMemory, count); - var debugValueMemory = stackalloc TestTriangle[count]; - var debugValues = new Buffer(debugValueMemory, count); - var debugTableSize = 1 << SpanHelper.GetContainingPowerOf2(count * 4); + var allocatedCount = count * 3; + var debugOverlapMemory = stackalloc int[allocatedCount]; + var debugOverlapBuffer = new Buffer(debugOverlapMemory, allocatedCount); + var debugKeyMemory = stackalloc int[allocatedCount]; + var debugKeys = new Buffer(debugKeyMemory, allocatedCount); + var debugValueMemory = stackalloc TestTriangle[allocatedCount]; + var debugValues = new Buffer(debugValueMemory, allocatedCount); + var debugTableSize = 1 << SpanHelper.GetContainingPowerOf2(allocatedCount * 4); var debugTableMemory = stackalloc int[debugTableSize]; var debugTable = new Buffer(debugTableMemory, debugTableSize); QuickDictionary> testTriangles = new(ref debugKeys, ref debugValues, ref debugTable); From f77d86de56fd53dbfa445dd8bb00c858e6f463e7 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 21 Sep 2021 15:53:57 -0500 Subject: [PATCH 004/947] Fixed edgedge2 sign bug and added more bounding box debug logic. --- BepuPhysics/BoundingBoxHelpers.cs | 9 +++++++-- .../CollisionTasks/TrianglePairTester.cs | 10 +++++----- 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/BepuPhysics/BoundingBoxHelpers.cs b/BepuPhysics/BoundingBoxHelpers.cs index 8e452afc5..2120a08fc 100644 --- a/BepuPhysics/BoundingBoxHelpers.cs +++ b/BepuPhysics/BoundingBoxHelpers.cs @@ -113,6 +113,9 @@ public static void ExpandBoundingBoxes(ref Vector3Wide min, ref Vector3Wide max, //The maximum expansion passed into this function is the speculative margin for discrete mode collidables, and ~infinity for passive or continuous ones. Vector3Wide.Max(-maximumExpansion, minDisplacement, out minDisplacement); Vector3Wide.Min(maximumExpansion, maxDisplacement, out maxDisplacement); + Vector3Wide.Broadcast(new Vector3(5), out var debug); + Vector3Wide.Subtract(minDisplacement, debug, out minDisplacement); + Vector3Wide.Add(maxDisplacement, debug, out maxDisplacement); Vector3Wide.Add(min, minDisplacement, out min); Vector3Wide.Add(max, maxDisplacement, out max); @@ -197,6 +200,9 @@ public static unsafe void ExpandLocalBoundingBoxes(ref Vector3Wide min, ref Vect //Clamp the expansion to the pair imposed limit. Discrete pairs don't need to look beyond their speculative margin. Vector3Wide.Min(maximumAllowedExpansion, maxExpansion, out maxExpansion); Vector3Wide.Max(-maximumAllowedExpansion, minExpansion, out minExpansion); + Vector3Wide.Broadcast(new Vector3(5), out var debug); + Vector3Wide.Subtract(minExpansion, debug, out minExpansion); + Vector3Wide.Add(maxExpansion, debug, out maxExpansion); Vector3Wide.Add(minExpansion, min, out min); Vector3Wide.Add(maxExpansion, max, out max); @@ -216,8 +222,7 @@ public static unsafe void ExpandLocalBoundingBoxes(ref Vector3Wide min, ref Vect [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void EpsilonExpandLocalBoundingBoxes(Vector maximumRadius, ref Vector3Wide min, ref Vector3Wide max) { - //var expansion = maximumRadius * new Vector(1e-4f); - var expansion = maximumRadius + new Vector(10f); + var expansion = maximumRadius * new Vector(1e-4f); Vector3Wide.Subtract(min, expansion, out min); Vector3Wide.Add(max, expansion, out max); } diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs index 659cc9f24..e392732e3 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs @@ -49,11 +49,11 @@ static void TestEdgeEdge2( in Vector3Wide aA, in Vector3Wide bA, in Vector3Wide cA, in Vector3Wide aB, in Vector3Wide bB, in Vector3Wide cB, out Vector depth, out Vector3Wide normal) { - Vector3Wide.Subtract(edgeStartB, edgeStartA, out var aStartToBStart); - Vector3Wide.Dot(edgeOffsetA, aStartToBStart, out var oADotAB); - Vector3Wide.Dot(edgeOffsetB, aStartToBStart, out var oBDotAB); + Vector3Wide.Subtract(edgeStartA, edgeStartB, out var bStartToAStart); + Vector3Wide.Dot(edgeOffsetA, bStartToAStart, out var oADotAB); + Vector3Wide.Dot(edgeOffsetB, bStartToAStart, out var oBDotAB); Vector3Wide.Dot(edgeOffsetA, edgeOffsetB, out var oADotOB); - var denominator = edgeOffsetALengthSquared * edgeOffsetBLengthSquared - oADotOB * oADotOB; //TODO: div 0 guard. + var denominator = edgeOffsetALengthSquared * edgeOffsetBLengthSquared - oADotOB * oADotOB; //TODO: div 0 guard if edge offsets are parallel. //Compute the first guess for tA and clamp it. var tA = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (oADotOB * oBDotAB - oADotAB * edgeOffsetBLengthSquared) / denominator)); //Compute the closest point on B to the first guess. @@ -69,7 +69,7 @@ static void TestEdgeEdge2( //If the segments touch, then fall back to using the edge direction as the normal candidate. Vector3Wide.Subtract(a, b, out var ba); Vector3Wide.LengthSquared(ba, out var baLengthSquared); - var useFallback = Vector.LessThan(baLengthSquared, new Vector(1e-14f)); + var useFallback = Vector.LessThan(baLengthSquared, new Vector(1e-10f)); Vector3Wide.CrossWithoutOverlap(edgeOffsetA, edgeOffsetB, out var fallbackNormal); Vector3Wide.ConditionalSelect(useFallback, fallbackNormal, ba, out normal); Vector3Wide.Length(normal, out var normalLength); From f1685776eae1547549f4d1747914e26030eb5169 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 21 Sep 2021 20:05:18 -0500 Subject: [PATCH 005/947] Tried out explicit brute force vertex testing. Comparable to fancy edge-edge. --- .../CollisionTasks/TrianglePairTester.cs | 248 ++++++++++++++---- .../SpecializedTests/BatchedCollisionTests.cs | 89 ++++--- Demos/SpecializedTests/TriangleTestDemo.cs | 89 ++++--- 3 files changed, 285 insertions(+), 141 deletions(-) diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs index e392732e3..fc4d48fce 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs @@ -43,6 +43,7 @@ static void TestEdgeEdge( depth = Vector.ConditionalSelect(Vector.LessThan(normalLength, new Vector(1e-10f)), new Vector(float.MaxValue), depth); } + [MethodImpl(MethodImplOptions.AggressiveInlining)] static void TestEdgeEdge2( in Vector3Wide edgeStartA, in Vector3Wide edgeOffsetA, in Vector edgeOffsetALengthSquared, in Vector inverseEdgeOffsetALengthSquared, in Vector3Wide edgeStartB, in Vector3Wide edgeOffsetB, in Vector edgeOffsetBLengthSquared, in Vector inverseEdgeOffsetBLengthSquared, @@ -69,7 +70,7 @@ static void TestEdgeEdge2( //If the segments touch, then fall back to using the edge direction as the normal candidate. Vector3Wide.Subtract(a, b, out var ba); Vector3Wide.LengthSquared(ba, out var baLengthSquared); - var useFallback = Vector.LessThan(baLengthSquared, new Vector(1e-10f)); + var useFallback = Vector.LessThan(baLengthSquared, new Vector(1e-12f)); Vector3Wide.CrossWithoutOverlap(edgeOffsetA, edgeOffsetB, out var fallbackNormal); Vector3Wide.ConditionalSelect(useFallback, fallbackNormal, ba, out normal); Vector3Wide.Length(normal, out var normalLength); @@ -79,6 +80,105 @@ static void TestEdgeEdge2( depth = Vector.ConditionalSelect(Vector.LessThan(normalLength, new Vector(1e-10f)), new Vector(float.MaxValue), depth); } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + static void TestVertexNormal(in Vector3Wide vertex, + in Vector3Wide a, in Vector3Wide b, in Vector3Wide c, + in Vector3Wide opposingA, in Vector3Wide opposingB, in Vector3Wide opposingC, + in Vector3Wide opposingAB, in Vector3Wide opposingBC, in Vector3Wide opposingCA, + in Vector inverseLengthSquaredAB, in Vector inverseLengthSquaredBC, in Vector inverseLengthSquaredCA, + out Vector depth, out Vector3Wide normal) + { + //Project the vertex onto all three edges. Take the closest approach as a normal candidate. + //Note that this will try normals that cross over the triangle's face, but that's fine- it'll just be a crappy normal candidate and another option will be chosen instead. + Vector3Wide.Subtract(vertex, opposingA, out var av); + Vector3Wide.Subtract(vertex, opposingB, out var bv); + Vector3Wide.Subtract(vertex, opposingC, out var cv); + Vector3Wide.Dot(av, opposingAB, out var avDotAB); + Vector3Wide.Dot(bv, opposingBC, out var bvDotBC); + Vector3Wide.Dot(cv, opposingCA, out var cvDotCA); + var tAB = Vector.Max(Vector.Zero, Vector.Min(Vector.One, avDotAB * inverseLengthSquaredAB)); + var tBC = Vector.Max(Vector.Zero, Vector.Min(Vector.One, bvDotBC * inverseLengthSquaredBC)); + var tCA = Vector.Max(Vector.Zero, Vector.Min(Vector.One, cvDotCA * inverseLengthSquaredCA)); + Vector3Wide vToAB, vToBC, vToCA; + vToAB.X = opposingA.X + opposingAB.X * tAB - vertex.X; + vToAB.Y = opposingA.Y + opposingAB.Y * tAB - vertex.Y; + vToAB.Z = opposingA.Z + opposingAB.Z * tAB - vertex.Z; + vToBC.X = opposingB.X + opposingBC.X * tBC - vertex.X; + vToBC.Y = opposingB.Y + opposingBC.Y * tBC - vertex.Y; + vToBC.Z = opposingB.Z + opposingBC.Z * tBC - vertex.Z; + vToCA.X = opposingC.X + opposingCA.X * tCA - vertex.X; + vToCA.Y = opposingC.Y + opposingCA.Y * tCA - vertex.Y; + vToCA.Z = opposingC.Z + opposingCA.Z * tCA - vertex.Z; + Vector3Wide.LengthSquared(vToAB, out var abDistanceSquared); + Vector3Wide.LengthSquared(vToBC, out var bcDistanceSquared); + Vector3Wide.LengthSquared(vToCA, out var caDistanceSquared); + + var distanceSquared = Vector.Min(abDistanceSquared, Vector.Min(bcDistanceSquared, caDistanceSquared)); + Vector3Wide.ConditionalSelect(Vector.Equals(distanceSquared, abDistanceSquared), vToAB, vToCA, out normal); + Vector3Wide.ConditionalSelect(Vector.Equals(distanceSquared, bcDistanceSquared), vToBC, normal, out normal); + + Vector3Wide.Scale(normal, Vector.One / Vector.SquareRoot(distanceSquared), out normal); + GetDepthForNormal(a, b, c, opposingA, opposingB, opposingC, normal, out depth); + //Protect against bad normals. + depth = Vector.ConditionalSelect(Vector.LessThan(distanceSquared, new Vector(1e-12f)), new Vector(float.MaxValue), depth); + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + static void TestVertexNormal2(in Vector3Wide vertex, + in Vector3Wide opposingA, in Vector3Wide opposingB, in Vector3Wide opposingC, + in Vector3Wide edgePlaneNormalAB, in Vector3Wide edgePlaneNormalBC, in Vector3Wide edgePlaneNormalCA, + in Vector3Wide triangleNormal, in Vector inverseTriangleNormalLength, + in Vector inverseLengthSquaredAB, in Vector inverseLengthSquaredBC, in Vector inverseLengthSquaredCA) + { + Vector3Wide.Subtract(vertex, opposingA, out var av); + Vector3Wide.Subtract(vertex, opposingB, out var bv); + Vector3Wide.Subtract(vertex, opposingC, out var cv); + Vector3Wide.Dot(av, edgePlaneNormalAB, out var avDotPlaneAB); + Vector3Wide.Dot(bv, edgePlaneNormalBC, out var bvDotPlaneBC); + Vector3Wide.Dot(cv, edgePlaneNormalCA, out var cvDotPlaneCA); + //Push the vertex down to the triangle plane. + Vector3Wide.Dot(av, triangleNormal, out var flattenDot); + Vector3Wide flattenedVertex; + flattenedVertex.X = vertex.X - flattenDot * triangleNormal.X; + flattenedVertex.Y = vertex.Y - flattenDot * triangleNormal.Y; + flattenedVertex.Z = vertex.Z - flattenDot * triangleNormal.Z; + //Edge plane normals should have the same magnitude as the edge lengths, since they're just rotated 90 degrees. Triangle normal is perfectly perpendicular to the edges by construction. + var tAB = avDotPlaneAB * inverseLengthSquaredAB; + var tBC = bvDotPlaneBC * inverseLengthSquaredBC; + var tCA = cvDotPlaneCA * inverseLengthSquaredCA; + Vector3Wide onAB, onBC, onCA; + onAB.X = flattenedVertex.X - edgePlaneNormalAB.X * tAB; + onAB.Y = flattenedVertex.Y - edgePlaneNormalAB.Y * tAB; + onAB.Z = flattenedVertex.Z - edgePlaneNormalAB.Z * tAB; + onBC.X = flattenedVertex.X - edgePlaneNormalBC.X * tBC; + onBC.Y = flattenedVertex.Y - edgePlaneNormalBC.Y * tBC; + onBC.Z = flattenedVertex.Z - edgePlaneNormalBC.Z * tBC; + onCA.X = flattenedVertex.X - edgePlaneNormalCA.X * tCA; + onCA.Y = flattenedVertex.Y - edgePlaneNormalCA.Y * tCA; + onCA.Z = flattenedVertex.Z - edgePlaneNormalCA.Z * tCA; + + //If the vertex is outside one edge plane, use the vertex + //If the vertex is outside two edge planes, it is on the shared vertex. + var outsideAB = Vector.GreaterThanOrEqual(avDotPlaneAB, Vector.Zero); + var outsideBC = Vector.GreaterThanOrEqual(bvDotPlaneBC, Vector.Zero); + var outsideCA = Vector.GreaterThanOrEqual(cvDotPlaneCA, Vector.Zero); + var outsideA = Vector.BitwiseAnd(outsideAB, outsideCA); + var outsideB = Vector.BitwiseAnd(outsideAB, outsideBC); + var outsideC = Vector.BitwiseAnd(outsideBC, outsideCA); + + //Vector3Wide.ConditionalSelect + + //var wa = bvDotPlaneBC * inverseTriangleNormalLength; + //var wb = cvDotPlaneCA * inverseTriangleNormalLength; + //var wc = Vector.One - wa - wb; + ////dot(ab x N, av) = dot(av x ab, N) + ////barycentricCoordinate = dot(ab x N * ||ab x ca||, av) / ||ab x ca||^2 = dot(ab x N, av) / ||ab x ca|| + //avDotPlaneAB = Vector.Max(Vector.Zero, Vector.Min(Vector.One, avDotPlaneAB * inverseLengthSquaredAB)); + //bvDotPlaneBC = Vector.Max(Vector.Zero, Vector.Min(Vector.One, bvDotPlaneBC * inverseLengthSquaredBC)); + //cvDotPlaneCA = Vector.Max(Vector.Zero, Vector.Min(Vector.One, cvDotPlaneCA * inverseLengthSquaredCA)); + + + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] static void Select( ref Vector depth, ref Vector3Wide normal, @@ -160,16 +260,6 @@ private static void ClipEdge( var tA = ((intersectionPointX - edgeStartA.X) * edgeOffsetA.X + (intersectionPointY - edgeStartA.Y) * edgeOffsetA.Y) * inverseEdgeLengthSquaredA; intersectionExists = Vector.BitwiseAnd(Vector.GreaterThanOrEqual(tA, Vector.Zero), Vector.LessThanOrEqual(tA, Vector.One)); depthContributionA = edgeStartADotNormal + edgeOffsetADotNormal * tA; - - - //var minValue = new Vector(float.MinValue); - //var maxValue = new Vector(float.MaxValue); - //entry = Vector.ConditionalSelect(isEntry, t, minValue); - //exit = Vector.ConditionalSelect(isEntry, maxValue, t); - ////If the edges are parallel and the edge is outside the plane, then this edge can't contribute. - //var edgeParallelAndOutside = Vector.BitwiseAnd(Vector.GreaterThan(edgePlaneNormalDot, Vector.Zero), Vector.LessThan(Vector.Abs(velocity), new Vector(1e-14f))); - //entry = Vector.ConditionalSelect(edgeParallelAndOutside, maxValue, entry); - //exit = Vector.ConditionalSelect(edgeParallelAndOutside, minValue, exit); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -296,63 +386,29 @@ public unsafe void Test( Vector3Wide.Subtract(a.C, a.B, out var bcA); Vector3Wide.Subtract(a.A, a.C, out var caA); - ////A AB x * - //TestEdgeEdge(abA, abB, a.A, a.B, a.C, bA, bB, bC, out var depth, out var localNormal); - //TestEdgeEdge(abA, bcB, a.A, a.B, a.C, bA, bB, bC, out var depthCandidate, out var localNormalCandidate); - //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - //TestEdgeEdge(abA, caB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); - //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - - ////A BC x * - //TestEdgeEdge(bcA, abB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); - //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - //TestEdgeEdge(bcA, bcB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); - //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - //TestEdgeEdge(bcA, caB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); - //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - - ////A CA x * - //TestEdgeEdge(caA, abB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); - //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - //TestEdgeEdge(caA, bcB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); - //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - //TestEdgeEdge(caA, caB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); - //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - - Vector3Wide.LengthSquared(abA, out var abALengthSquared); - Vector3Wide.LengthSquared(bcA, out var bcALengthSquared); - Vector3Wide.LengthSquared(caA, out var caALengthSquared); - Vector3Wide.LengthSquared(abB, out var abBLengthSquared); - Vector3Wide.LengthSquared(bcB, out var bcBLengthSquared); - Vector3Wide.LengthSquared(caB, out var caBLengthSquared); - var inverseABALengthSquared = Vector.One / abALengthSquared; - var inverseBCALengthSquared = Vector.One / bcALengthSquared; - var inverseCAALengthSquared = Vector.One / caALengthSquared; - var inverseABBLengthSquared = Vector.One / abBLengthSquared; - var inverseBCBLengthSquared = Vector.One / bcBLengthSquared; - var inverseCABLengthSquared = Vector.One / caBLengthSquared; + ManifoldCandidateHelper.CreateActiveMask(pairCount, out var allowContacts); //A AB x * - TestEdgeEdge2(a.A, abA, abALengthSquared, inverseABALengthSquared, bA, abB, abBLengthSquared, inverseABBLengthSquared, a.A, a.B, a.C, bA, bB, bC, out var depth, out var localNormal); - TestEdgeEdge2(a.A, abA, abALengthSquared, inverseABALengthSquared, bB, bcB, bcBLengthSquared, inverseBCBLengthSquared, a.A, a.B, a.C, bA, bB, bC, out var depthCandidate, out var localNormalCandidate); + TestEdgeEdge(abA, abB, a.A, a.B, a.C, bA, bB, bC, out var depth, out var localNormal); + TestEdgeEdge(abA, bcB, a.A, a.B, a.C, bA, bB, bC, out var depthCandidate, out var localNormalCandidate); Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - TestEdgeEdge2(a.A, abA, abALengthSquared, inverseABALengthSquared, bC, caB, caBLengthSquared, inverseCABLengthSquared, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + TestEdgeEdge(abA, caB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); //A BC x * - TestEdgeEdge2(a.B, bcA, bcALengthSquared, inverseBCALengthSquared, bA, abB, abBLengthSquared, inverseABBLengthSquared, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + TestEdgeEdge(bcA, abB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - TestEdgeEdge2(a.B, bcA, bcALengthSquared, inverseBCALengthSquared, bB, bcB, bcBLengthSquared, inverseBCBLengthSquared, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + TestEdgeEdge(bcA, bcB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - TestEdgeEdge2(a.B, bcA, bcALengthSquared, inverseBCALengthSquared, bC, caB, caBLengthSquared, inverseCABLengthSquared, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + TestEdgeEdge(bcA, caB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); //A CA x * - TestEdgeEdge2(a.C, caA, caALengthSquared, inverseCAALengthSquared, bA, abB, abBLengthSquared, inverseABBLengthSquared, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + TestEdgeEdge(caA, abB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - TestEdgeEdge2(a.C, caA, caALengthSquared, inverseCAALengthSquared, bB, bcB, bcBLengthSquared, inverseBCBLengthSquared, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + TestEdgeEdge(caA, bcB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - TestEdgeEdge2(a.C, caA, caALengthSquared, inverseCAALengthSquared, bC, caB, caBLengthSquared, inverseCABLengthSquared, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + TestEdgeEdge(caA, caB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); //Face normals @@ -367,6 +423,85 @@ public unsafe void Test( GetDepthForNormal(a.A, a.B, a.C, bA, bB, bC, faceNormalB, out var faceDepthB); Select(ref depth, ref localNormal, faceDepthB, faceNormalB); + + var tryVertexNormals = Vector.BitwiseAnd(Vector.LessThan(depth, Vector.Zero), allowContacts); + Vector3Wide.LengthSquared(abA, out var abALengthSquared); + Vector3Wide.LengthSquared(abB, out var abBLengthSquared); + Vector3Wide.LengthSquared(caA, out var caALengthSquared); + Vector3Wide.LengthSquared(caB, out var caBLengthSquared); + if (Vector.LessThanAny(tryVertexNormals, Vector.Zero)) + { + //Vertex normals are not required to determine penetration versus separation. They are only used to ensure correct separated speculative normals. + //This isn't strictly required for behavior in the general case, but MeshReduction depends on it. + Vector3Wide.LengthSquared(bcA, out var bcALengthSquared); + Vector3Wide.LengthSquared(bcB, out var bcBLengthSquared); + var inverseABALengthSquared = Vector.One / abALengthSquared; + var inverseBCALengthSquared = Vector.One / bcALengthSquared; + var inverseCAALengthSquared = Vector.One / caALengthSquared; + var inverseABBLengthSquared = Vector.One / abBLengthSquared; + var inverseBCBLengthSquared = Vector.One / bcBLengthSquared; + var inverseCABLengthSquared = Vector.One / caBLengthSquared; + TestVertexNormal(a.A, a.A, a.B, a.C, bA, bB, bC, abB, bcB, caB, inverseABBLengthSquared, inverseBCBLengthSquared, inverseCABLengthSquared, out depthCandidate, out localNormalCandidate); + Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + TestVertexNormal(a.B, a.A, a.B, a.C, bA, bB, bC, abB, bcB, caB, inverseABBLengthSquared, inverseBCBLengthSquared, inverseCABLengthSquared, out depthCandidate, out localNormalCandidate); + Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + TestVertexNormal(a.C, a.A, a.B, a.C, bA, bB, bC, abB, bcB, caB, inverseABBLengthSquared, inverseBCBLengthSquared, inverseCABLengthSquared, out depthCandidate, out localNormalCandidate); + Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + TestVertexNormal(bA, bA, bB, bC, a.A, a.B, a.C, abA, bcA, caA, inverseABALengthSquared, inverseBCALengthSquared, inverseCAALengthSquared, out depthCandidate, out localNormalCandidate); + Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + TestVertexNormal(bB, bA, bB, bC, a.A, a.B, a.C, abA, bcA, caA, inverseABALengthSquared, inverseBCALengthSquared, inverseCAALengthSquared, out depthCandidate, out localNormalCandidate); + Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + TestVertexNormal(bC, bA, bB, bC, a.A, a.B, a.C, abA, bcA, caA, inverseABALengthSquared, inverseBCALengthSquared, inverseCAALengthSquared, out depthCandidate, out localNormalCandidate); + Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + } + + //Vector3Wide.LengthSquared(abA, out var abALengthSquared); + //Vector3Wide.LengthSquared(abB, out var abBLengthSquared); + //Vector3Wide.LengthSquared(caA, out var caALengthSquared); + //Vector3Wide.LengthSquared(caB, out var caBLengthSquared); + //Vector3Wide.LengthSquared(bcA, out var bcALengthSquared); + //Vector3Wide.LengthSquared(bcB, out var bcBLengthSquared); + //var inverseABALengthSquared = Vector.One / abALengthSquared; + //var inverseBCALengthSquared = Vector.One / bcALengthSquared; + //var inverseCAALengthSquared = Vector.One / caALengthSquared; + //var inverseABBLengthSquared = Vector.One / abBLengthSquared; + //var inverseBCBLengthSquared = Vector.One / bcBLengthSquared; + //var inverseCABLengthSquared = Vector.One / caBLengthSquared; + ////A AB x * + //TestEdgeEdge2(a.A, abA, abALengthSquared, inverseABALengthSquared, bA, abB, abBLengthSquared, inverseABBLengthSquared, a.A, a.B, a.C, bA, bB, bC, out var depth, out var localNormal); + //TestEdgeEdge2(a.A, abA, abALengthSquared, inverseABALengthSquared, bB, bcB, bcBLengthSquared, inverseBCBLengthSquared, a.A, a.B, a.C, bA, bB, bC, out var depthCandidate, out var localNormalCandidate); + //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + //TestEdgeEdge2(a.A, abA, abALengthSquared, inverseABALengthSquared, bC, caB, caBLengthSquared, inverseCABLengthSquared, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + + ////A BC x * + //TestEdgeEdge2(a.B, bcA, bcALengthSquared, inverseBCALengthSquared, bA, abB, abBLengthSquared, inverseABBLengthSquared, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + //TestEdgeEdge2(a.B, bcA, bcALengthSquared, inverseBCALengthSquared, bB, bcB, bcBLengthSquared, inverseBCBLengthSquared, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + //TestEdgeEdge2(a.B, bcA, bcALengthSquared, inverseBCALengthSquared, bC, caB, caBLengthSquared, inverseCABLengthSquared, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + + ////A CA x * + //TestEdgeEdge2(a.C, caA, caALengthSquared, inverseCAALengthSquared, bA, abB, abBLengthSquared, inverseABBLengthSquared, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + //TestEdgeEdge2(a.C, caA, caALengthSquared, inverseCAALengthSquared, bB, bcB, bcBLengthSquared, inverseBCBLengthSquared, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + //TestEdgeEdge2(a.C, caA, caALengthSquared, inverseCAALengthSquared, bC, caB, caBLengthSquared, inverseCABLengthSquared, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + + ////Face normals + //Vector3Wide.CrossWithoutOverlap(abA, caA, out var faceNormalA); + //Vector3Wide.Length(faceNormalA, out var faceNormalALength); + //Vector3Wide.Scale(faceNormalA, Vector.One / faceNormalALength, out faceNormalA); + //GetDepthForNormal(a.A, a.B, a.C, bA, bB, bC, faceNormalA, out depthCandidate); + //Select(ref depth, ref localNormal, depthCandidate, faceNormalA); + //Vector3Wide.CrossWithoutOverlap(abB, caB, out var faceNormalB); + //Vector3Wide.Length(faceNormalB, out var faceNormalBLength); + //Vector3Wide.Scale(faceNormalB, Vector.One / faceNormalBLength, out faceNormalB); + //GetDepthForNormal(a.A, a.B, a.C, bA, bB, bC, faceNormalB, out var faceDepthB); + //Select(ref depth, ref localNormal, faceDepthB, faceNormalB); + //Point the normal from B to A by convention. Vector3Wide.Subtract(localTriangleCenterB, localTriangleCenterA, out var centerAToCenterB); Vector3Wide.Dot(localNormal, centerAToCenterB, out var calibrationDot); @@ -377,8 +512,7 @@ public unsafe void Test( Vector3Wide.Dot(localNormal, faceNormalA, out var localNormalDotFaceNormalA); Vector3Wide.Dot(localNormal, faceNormalB, out var localNormalDotFaceNormalB); - ManifoldCandidateHelper.CreateActiveMask(pairCount, out var activeLanes); - var allowContacts = Vector.BitwiseAnd(activeLanes, Vector.BitwiseAnd( + allowContacts = Vector.BitwiseAnd(allowContacts, Vector.BitwiseAnd( Vector.LessThan(localNormalDotFaceNormalA, new Vector(-SphereTriangleTester.BackfaceNormalDotRejectionThreshold)), Vector.GreaterThan(localNormalDotFaceNormalB, new Vector(SphereTriangleTester.BackfaceNormalDotRejectionThreshold)))); if (Vector.EqualsAll(allowContacts, Vector.Zero)) diff --git a/Demos/SpecializedTests/BatchedCollisionTests.cs b/Demos/SpecializedTests/BatchedCollisionTests.cs index fee374dd2..66876859d 100644 --- a/Demos/SpecializedTests/BatchedCollisionTests.cs +++ b/Demos/SpecializedTests/BatchedCollisionTests.cs @@ -23,9 +23,16 @@ unsafe struct TestCollisionCallbacks : ICollisionCallbacks public unsafe void OnPairCompleted(int pairId, ref TManifold manifold) where TManifold : unmanaged, IContactManifold { - manifold.GetContact(0, out var offset, out var normal, out var depth, out var featureId); - var extra = 1e-16 * (depth + offset.X + normal.X); - *Count += 1 + (int)extra; + if (manifold.Count > 0) + { + manifold.GetContact(0, out var offset, out var normal, out var depth, out var featureId); + var extra = 1e-16 * (depth + offset.X + normal.X); + *Count += 1 + (int)extra; + } + else + { + ++*Count; + } } public unsafe void OnChildPairCompleted(int pairId, int childA, int childB, ref ConvexContactManifold manifold) @@ -160,7 +167,7 @@ public static void Test() points.AllocateUnsafely() = new Vector3((float)random.NextDouble(), 1 * (float)random.NextDouble(), (float)random.NextDouble()); //points.AllocateUnsafely() = new Vector3(0, 1, 0) + Vector3.Normalize(new Vector3((float)random.NextDouble() * 2 - 1, (float)random.NextDouble() * 2 - 1, (float)random.NextDouble() * 2 - 1)) * (float)random.NextDouble(); } - + var pointsBuffer = points.Span.Slice(points.Count); ConvexHullHelper.CreateShape(pointsBuffer, pool, out _, out var convexHull); @@ -178,44 +185,44 @@ public static void Test() } - Test(ref sphere, ref sphere, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref sphere, ref capsule, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref sphere, ref box, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref sphere, ref triangle, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref sphere, ref cylinder, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref sphere, ref convexHull, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref capsule, ref capsule, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref capsule, ref box, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref capsule, ref triangle, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref capsule, ref cylinder, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref capsule, ref convexHull, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref box, ref box, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref box, ref triangle, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref box, ref cylinder, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref box, ref convexHull, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + //Test(ref sphere, ref sphere, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + //Test(ref sphere, ref capsule, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + //Test(ref sphere, ref box, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + //Test(ref sphere, ref triangle, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + //Test(ref sphere, ref cylinder, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + //Test(ref sphere, ref convexHull, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + //Test(ref capsule, ref capsule, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + //Test(ref capsule, ref box, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + //Test(ref capsule, ref triangle, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + //Test(ref capsule, ref cylinder, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + //Test(ref capsule, ref convexHull, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + //Test(ref box, ref box, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + //Test(ref box, ref triangle, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + //Test(ref box, ref cylinder, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + //Test(ref box, ref convexHull, ref posesA, ref posesB, pool, shapes, registry, iterationCount); Test(ref triangle, ref triangle, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref triangle, ref cylinder, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref triangle, ref convexHull, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref cylinder, ref cylinder, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref cylinder, ref convexHull, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref convexHull, ref convexHull, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - - - Test(sphere, sphere, ref posesA, ref posesB, iterationCount); - Test(sphere, capsule, ref posesA, ref posesB, iterationCount); - Test(sphere, box, ref posesA, ref posesB, iterationCount); - Test(sphere, triangle, ref posesA, ref posesB, iterationCount); - Test(sphere, cylinder, ref posesA, ref posesB, iterationCount); - Test(capsule, capsule, ref posesA, ref posesB, iterationCount); - Test(capsule, box, ref posesA, ref posesB, iterationCount); - Test>(capsule, triangle, ref posesA, ref posesB, iterationCount); - Test>(capsule, cylinder, ref posesA, ref posesB, iterationCount); - Test>(cylinder, box, ref posesA, ref posesB, iterationCount); - Test>(cylinder, triangle, ref posesA, ref posesB, iterationCount); - Test>(cylinder, cylinder, ref posesA, ref posesB, iterationCount); - Test>(box, box, ref posesA, ref posesB, iterationCount); - Test>(box, triangle, ref posesA, ref posesB, iterationCount); - Test>(triangle, triangle, ref posesA, ref posesB, iterationCount); + //Test(ref triangle, ref cylinder, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + //Test(ref triangle, ref convexHull, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + //Test(ref cylinder, ref cylinder, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + //Test(ref cylinder, ref convexHull, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + //Test(ref convexHull, ref convexHull, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + + + //Test(sphere, sphere, ref posesA, ref posesB, iterationCount); + //Test(sphere, capsule, ref posesA, ref posesB, iterationCount); + //Test(sphere, box, ref posesA, ref posesB, iterationCount); + //Test(sphere, triangle, ref posesA, ref posesB, iterationCount); + //Test(sphere, cylinder, ref posesA, ref posesB, iterationCount); + //Test(capsule, capsule, ref posesA, ref posesB, iterationCount); + //Test(capsule, box, ref posesA, ref posesB, iterationCount); + //Test>(capsule, triangle, ref posesA, ref posesB, iterationCount); + //Test>(capsule, cylinder, ref posesA, ref posesB, iterationCount); + //Test>(cylinder, box, ref posesA, ref posesB, iterationCount); + //Test>(cylinder, triangle, ref posesA, ref posesB, iterationCount); + //Test>(cylinder, cylinder, ref posesA, ref posesB, iterationCount); + //Test>(box, box, ref posesA, ref posesB, iterationCount); + //Test>(box, triangle, ref posesA, ref posesB, iterationCount); + //Test>(triangle, triangle, ref posesA, ref posesB, iterationCount); Console.WriteLine($"Done. Hit enter to exit."); Console.ReadLine(); } diff --git a/Demos/SpecializedTests/TriangleTestDemo.cs b/Demos/SpecializedTests/TriangleTestDemo.cs index ceabc0aca..a55e6e4c2 100644 --- a/Demos/SpecializedTests/TriangleTestDemo.cs +++ b/Demos/SpecializedTests/TriangleTestDemo.cs @@ -92,45 +92,47 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); - var triangleDescription = new StaticDescription - { - Pose = new RigidPose - { - Position = new Vector3(2, 0, 2), - Orientation = QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 3.2345f) - }, - Collidable = new CollidableDescription - { - Shape = Simulation.Shapes.Add(new Triangle( - new Vector3(-3, -0.5f, -3), - new Vector3(3, 0, -3), - new Vector3(-3, 0, 3))), - SpeculativeMargin = 10.1f - } - }; - Simulation.Statics.Add(triangleDescription); - - var shape = new Triangle(new Vector3(0, 0, 3), new Vector3(0, 0, 0), new Vector3(-3, 3, 0)); - var bodyDescription = new BodyDescription - { - Collidable = new CollidableDescription { Shape = Simulation.Shapes.Add(shape), SpeculativeMargin = 0.1f }, - Activity = new BodyActivityDescription { SleepThreshold = -1 }, - Pose = new RigidPose - { - Position = new Vector3(1, -0.01f, 1), - Orientation = QuaternionEx.CreateFromAxisAngle(Vector3.Normalize(new Vector3(1, 0, 1)), 0) - //Orientation = BepuUtilities.Quaternion.Identity - } - }; - shape.ComputeInertia(1, out bodyDescription.LocalInertia); - //bodyDescription.LocalInertia.InverseInertiaTensor = new Triangular3x3(); - Simulation.Bodies.Add(bodyDescription); + //var triangleDescription = new StaticDescription + //{ + // Pose = new RigidPose + // { + // Position = new Vector3(2 - 10, 0, 2), + // Orientation = QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 3.2345f) + // }, + // Collidable = new CollidableDescription + // { + // Shape = Simulation.Shapes.Add(new Triangle( + // new Vector3(-3, -0.5f, -3), + // new Vector3(3, 0, -3), + // new Vector3(-3, 0, 3))), + // SpeculativeMargin = 10.1f + // } + //}; + //Simulation.Statics.Add(triangleDescription); + + //var shape = new Triangle(new Vector3(0, 0, 3), new Vector3(0, 0, 0), new Vector3(-3, 3, 0)); + //var bodyDescription = new BodyDescription + //{ + // Collidable = new CollidableDescription { Shape = Simulation.Shapes.Add(shape), SpeculativeMargin = 0.1f }, + // Activity = new BodyActivityDescription { SleepThreshold = -1 }, + // Pose = new RigidPose + // { + // Position = new Vector3(1 - 10, -0.01f, 1), + // Orientation = QuaternionEx.CreateFromAxisAngle(Vector3.Normalize(new Vector3(1, 0, 1)), 0) + // //Orientation = BepuUtilities.Quaternion.Identity + // } + //}; + //shape.ComputeInertia(1, out bodyDescription.LocalInertia); + ////bodyDescription.LocalInertia.InverseInertiaTensor = new Triangular3x3(); + //Simulation.Bodies.Add(bodyDescription); Simulation.Statics.Add(new StaticDescription(new Vector3(0, -3, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(200, 1, 200)), 0.1f))); Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 2, 0), new BodyInertia { InverseMass = 1 }, new CollidableDescription(Simulation.Shapes.Add(new Sphere(1.75f)), 0.1f), new BodyActivityDescription(-1))); Simulation.Bodies.Add(BodyDescription.CreateDynamic(new RigidPose(new Vector3(20, 2, 3), Quaternion.CreateFromYawPitchRoll(0f, 1.745329E-05f, 0f)), new BodyInertia { InverseMass = 1 }, new CollidableDescription(Simulation.Shapes.Add(new Capsule(1, 2)), 0.1f), new BodyActivityDescription(-1))); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 2, 6), new BodyInertia { InverseMass = 1 }, new CollidableDescription(Simulation.Shapes.Add(new Box(2, 3, 2)), 0.1f), new BodyActivityDescription(-1))); + var testBox = new Box(2, 3, 2); + testBox.ComputeInertia(1, out var testBoxInertia); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 2, 6), testBoxInertia, new CollidableDescription(Simulation.Shapes.Add(testBox), 10.1f), new BodyActivityDescription(-1))); var cylinder = new Cylinder(1.75f, 2); cylinder.ComputeInertia(1, out var cylinderInertia); @@ -160,7 +162,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) builder.BuildDynamicCompound(out var children, out var compoundInertia); //compoundInertia.InverseInertiaTensor = default; var compound = new Compound(children); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 3, 14), compoundInertia, new CollidableDescription(Simulation.Shapes.Add(compound), 0.1f), new BodyActivityDescription(-1))); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 3, 14), compoundInertia, new CollidableDescription(Simulation.Shapes.Add(compound), 10.1f), new BodyActivityDescription(-1))); var triangles = new QuickList(4, BufferPool); var v0 = new Vector3(0, 1.75f, 0); @@ -173,16 +175,17 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //triangles.AllocateUnsafely() = new Triangle { A = v1, B = v2, C = v3 }; var testMesh = new Mesh(triangles, Vector3.One, BufferPool); - Simulation.Statics.Add(new StaticDescription(new Vector3(30, -2.5f, 0), new CollidableDescription(Simulation.Shapes.Add(testMesh), 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(30, -2.5f, 0), new CollidableDescription(Simulation.Shapes.Add(testMesh), 10.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -2.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Triangle(v2, v0, v1)), 10.1f))); - DemoMeshHelper.LoadModel(content, BufferPool, "Content\\newt.obj", new Vector3(3), out var mesh); - var collidable = new CollidableDescription(Simulation.Shapes.Add(mesh), 0.2f); - mesh.ComputeClosedInertia(1, out var newtInertia); - for (int i = 0; i < 5; ++i) - { - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(-20, 5 + i * 5, 0), newtInertia, collidable, new BodyActivityDescription(1e-2f))); - } + //DemoMeshHelper.LoadModel(content, BufferPool, "Content\\newt.obj", new Vector3(3), out var mesh); + //var collidable = new CollidableDescription(Simulation.Shapes.Add(mesh), 0.2f); + //mesh.ComputeClosedInertia(1, out var newtInertia); + //for (int i = 0; i < 5; ++i) + //{ + // Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(-20, 5 + i * 5, 0), newtInertia, collidable, new BodyActivityDescription(1e-2f))); + //} } } From c79746c45e66340dbe345538aeb2dc68fdca3ead Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 22 Sep 2021 18:16:18 -0500 Subject: [PATCH 006/947] Interesting approach. Fundamentally broken, but interesting. --- .../CollisionTasks/TrianglePairTester.cs | 193 ++++++++++++++---- 1 file changed, 156 insertions(+), 37 deletions(-) diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs index fc4d48fce..5774ed9d7 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs @@ -42,6 +42,26 @@ static void TestEdgeEdge( //Protect against bad normals. depth = Vector.ConditionalSelect(Vector.LessThan(normalLength, new Vector(1e-10f)), new Vector(float.MaxValue), depth); } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + static void TestEdgeEdge3( + in Vector3Wide edgeDirectionA, in Vector3Wide edgeDirectionB, in Vector edgeIndexA, in Vector edgeIndexB, + in Vector3Wide aA, in Vector3Wide bA, in Vector3Wide cA, in Vector3Wide aB, in Vector3Wide bB, in Vector3Wide cB, + ref Vector depth, ref Vector3Wide normal, ref Vector minimumEdgeA, ref Vector minimumEdgeB) + { + //Calibrate the normal to point from the triangle to the box while normalizing. + Vector3Wide.CrossWithoutOverlap(edgeDirectionA, edgeDirectionB, out var normalCandidate); + Vector3Wide.Length(normalCandidate, out var normalLength); + //Note that we do not calibrate yet. The depth calculation does not rely on calibration, so we punt it until after all normals have been tested. + Vector3Wide.Scale(normalCandidate, Vector.One / normalLength, out normalCandidate); + GetDepthForNormal(aA, bA, cA, aB, bB, cB, normalCandidate, out var depthCandidate); + //Protect against bad normals. + depthCandidate = Vector.ConditionalSelect(Vector.LessThan(normalLength, new Vector(1e-10f)), new Vector(float.MaxValue), depthCandidate); + var useCandidate = Vector.LessThan(depthCandidate, depth); + depth = Vector.Min(depthCandidate, depth); + Vector3Wide.ConditionalSelect(useCandidate, normalCandidate, normal, out normal); + minimumEdgeA = Vector.ConditionalSelect(useCandidate, edgeIndexA, minimumEdgeA); + minimumEdgeB = Vector.ConditionalSelect(useCandidate, edgeIndexB, minimumEdgeB); + } [MethodImpl(MethodImplOptions.AggressiveInlining)] static void TestEdgeEdge2( @@ -79,6 +99,38 @@ static void TestEdgeEdge2( //Protect against bad normals. depth = Vector.ConditionalSelect(Vector.LessThan(normalLength, new Vector(1e-10f)), new Vector(float.MaxValue), depth); } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + static void TestEdgeEdge4( + in Vector3Wide edgeStartA, in Vector3Wide edgeOffsetA, in Vector edgeOffsetALengthSquared, in Vector inverseEdgeOffsetALengthSquared, + in Vector3Wide edgeStartB, in Vector3Wide edgeOffsetB, in Vector edgeOffsetBLengthSquared, in Vector inverseEdgeOffsetBLengthSquared, + in Vector3Wide aA, in Vector3Wide bA, in Vector3Wide cA, in Vector3Wide aB, in Vector3Wide bB, in Vector3Wide cB, + out Vector depth, out Vector3Wide normal) + { + Vector3Wide.Subtract(edgeStartA, edgeStartB, out var bStartToAStart); + Vector3Wide.Dot(edgeOffsetA, bStartToAStart, out var oADotAB); + Vector3Wide.Dot(edgeOffsetB, bStartToAStart, out var oBDotAB); + Vector3Wide.Dot(edgeOffsetA, edgeOffsetB, out var oADotOB); + var denominator = edgeOffsetALengthSquared * edgeOffsetBLengthSquared - oADotOB * oADotOB; //TODO: div 0 guard if edge offsets are parallel. + //Compute the first guess for tA and clamp it. + var tA = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (oADotOB * oBDotAB - oADotAB * edgeOffsetBLengthSquared) / denominator)); + //Compute the closest point on B to the first guess. + var tB = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (oADotOB * tA + oBDotAB) * inverseEdgeOffsetBLengthSquared)); + //Finally, compute the true tA. + tA = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (tB * oADotOB - oADotAB) * inverseEdgeOffsetALengthSquared)); + + Vector3Wide.Scale(edgeOffsetA, tA, out var startToClosestA); + Vector3Wide.Add(edgeStartA, startToClosestA, out var a); + Vector3Wide.Scale(edgeOffsetB, tB, out var startToClosestB); + Vector3Wide.Add(edgeStartB, startToClosestB, out var b); + + //Note that this will produce poor results if the edges overlap, but in that case, the vertex normals aren't going to be relevant anyway so it's fine to ignore their contribution. + Vector3Wide.Subtract(a, b, out normal); + Vector3Wide.Length(normal, out var normalLength); + Vector3Wide.Scale(normal, Vector.One / normalLength, out normal); + GetDepthForNormal(aA, bA, cA, aB, bB, cB, normal, out depth); + //Protect against bad normals. + depth = Vector.ConditionalSelect(Vector.LessThan(normalLength, new Vector(1e-7f)), new Vector(float.MaxValue), depth); + } [MethodImpl(MethodImplOptions.AggressiveInlining)] static void TestVertexNormal(in Vector3Wide vertex, @@ -388,34 +440,31 @@ public unsafe void Test( ManifoldCandidateHelper.CreateActiveMask(pairCount, out var allowContacts); + var two = new Vector(2); + var depth = new Vector(float.MaxValue); + Unsafe.SkipInit(out Vector3Wide localNormal); + Unsafe.SkipInit(out Vector minimumEdgeA); + Unsafe.SkipInit(out Vector minimumEdgeB); //A AB x * - TestEdgeEdge(abA, abB, a.A, a.B, a.C, bA, bB, bC, out var depth, out var localNormal); - TestEdgeEdge(abA, bcB, a.A, a.B, a.C, bA, bB, bC, out var depthCandidate, out var localNormalCandidate); - Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - TestEdgeEdge(abA, caB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); - Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + TestEdgeEdge3(abA, abB, Vector.Zero, Vector.Zero, a.A, a.B, a.C, bA, bB, bC, ref depth, ref localNormal, ref minimumEdgeA, ref minimumEdgeB); + TestEdgeEdge3(abA, bcB, Vector.Zero, Vector.One, a.A, a.B, a.C, bA, bB, bC, ref depth, ref localNormal, ref minimumEdgeA, ref minimumEdgeB); + TestEdgeEdge3(abA, caB, Vector.Zero, two, a.A, a.B, a.C, bA, bB, bC, ref depth, ref localNormal, ref minimumEdgeA, ref minimumEdgeB); //A BC x * - TestEdgeEdge(bcA, abB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); - Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - TestEdgeEdge(bcA, bcB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); - Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - TestEdgeEdge(bcA, caB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); - Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + TestEdgeEdge3(bcA, abB, Vector.One, Vector.Zero, a.A, a.B, a.C, bA, bB, bC, ref depth, ref localNormal, ref minimumEdgeA, ref minimumEdgeB); + TestEdgeEdge3(bcA, bcB, Vector.One, Vector.One, a.A, a.B, a.C, bA, bB, bC, ref depth, ref localNormal, ref minimumEdgeA, ref minimumEdgeB); + TestEdgeEdge3(bcA, caB, Vector.One, two, a.A, a.B, a.C, bA, bB, bC, ref depth, ref localNormal, ref minimumEdgeA, ref minimumEdgeB); //A CA x * - TestEdgeEdge(caA, abB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); - Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - TestEdgeEdge(caA, bcB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); - Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - TestEdgeEdge(caA, caB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); - Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + TestEdgeEdge3(caA, abB, two, Vector.Zero, a.A, a.B, a.C, bA, bB, bC, ref depth, ref localNormal, ref minimumEdgeA, ref minimumEdgeB); + TestEdgeEdge3(caA, bcB, two, Vector.One, a.A, a.B, a.C, bA, bB, bC, ref depth, ref localNormal, ref minimumEdgeA, ref minimumEdgeB); + TestEdgeEdge3(caA, caB, two, two, a.A, a.B, a.C, bA, bB, bC, ref depth, ref localNormal, ref minimumEdgeA, ref minimumEdgeB); //Face normals Vector3Wide.CrossWithoutOverlap(abA, caA, out var faceNormalA); Vector3Wide.Length(faceNormalA, out var faceNormalALength); Vector3Wide.Scale(faceNormalA, Vector.One / faceNormalALength, out faceNormalA); - GetDepthForNormal(a.A, a.B, a.C, bA, bB, bC, faceNormalA, out depthCandidate); + GetDepthForNormal(a.A, a.B, a.C, bA, bB, bC, faceNormalA, out var depthCandidate); Select(ref depth, ref localNormal, depthCandidate, faceNormalA); Vector3Wide.CrossWithoutOverlap(abB, caB, out var faceNormalB); Vector3Wide.Length(faceNormalB, out var faceNormalBLength); @@ -433,28 +482,98 @@ public unsafe void Test( { //Vertex normals are not required to determine penetration versus separation. They are only used to ensure correct separated speculative normals. //This isn't strictly required for behavior in the general case, but MeshReduction depends on it. - Vector3Wide.LengthSquared(bcA, out var bcALengthSquared); - Vector3Wide.LengthSquared(bcB, out var bcBLengthSquared); - var inverseABALengthSquared = Vector.One / abALengthSquared; - var inverseBCALengthSquared = Vector.One / bcALengthSquared; - var inverseCAALengthSquared = Vector.One / caALengthSquared; - var inverseABBLengthSquared = Vector.One / abBLengthSquared; - var inverseBCBLengthSquared = Vector.One / bcBLengthSquared; - var inverseCABLengthSquared = Vector.One / caBLengthSquared; - TestVertexNormal(a.A, a.A, a.B, a.C, bA, bB, bC, abB, bcB, caB, inverseABBLengthSquared, inverseBCBLengthSquared, inverseCABLengthSquared, out depthCandidate, out localNormalCandidate); - Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - TestVertexNormal(a.B, a.A, a.B, a.C, bA, bB, bC, abB, bcB, caB, inverseABBLengthSquared, inverseBCBLengthSquared, inverseCABLengthSquared, out depthCandidate, out localNormalCandidate); - Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - TestVertexNormal(a.C, a.A, a.B, a.C, bA, bB, bC, abB, bcB, caB, inverseABBLengthSquared, inverseBCBLengthSquared, inverseCABLengthSquared, out depthCandidate, out localNormalCandidate); - Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - TestVertexNormal(bA, bA, bB, bC, a.A, a.B, a.C, abA, bcA, caA, inverseABALengthSquared, inverseBCALengthSquared, inverseCAALengthSquared, out depthCandidate, out localNormalCandidate); - Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - TestVertexNormal(bB, bA, bB, bC, a.A, a.B, a.C, abA, bcA, caA, inverseABALengthSquared, inverseBCALengthSquared, inverseCAALengthSquared, out depthCandidate, out localNormalCandidate); - Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - TestVertexNormal(bC, bA, bB, bC, a.A, a.B, a.C, abA, bcA, caA, inverseABALengthSquared, inverseBCALengthSquared, inverseCAALengthSquared, out depthCandidate, out localNormalCandidate); + //To generate them, the above edge-edge tests tracked the edge pair which generated the lowest depth. + //If a vertex contributes a better normal, then it will be associated with those minimal edges. + //So, just perform a closest points query on the minimum edge pair. + var useABOnA = Vector.Equals(minimumEdgeA, Vector.Zero); + var useBCOnA = Vector.Equals(minimumEdgeA, Vector.One); + var useCAOnA = Vector.Equals(minimumEdgeA, two); + var useABOnB = Vector.Equals(minimumEdgeB, Vector.Zero); + var useBCOnB = Vector.Equals(minimumEdgeB, Vector.One); + var useCAOnB = Vector.Equals(minimumEdgeB, two); + Vector3Wide.ConditionalSelect(useABOnA, a.A, a.C, out var edgeStartA); + Vector3Wide.ConditionalSelect(useBCOnA, a.B, edgeStartA, out edgeStartA); + Vector3Wide.ConditionalSelect(useABOnA, abA, caA, out var edgeOffsetA); + Vector3Wide.ConditionalSelect(useBCOnA, bcA, edgeOffsetA, out edgeOffsetA); + Vector3Wide.ConditionalSelect(useABOnB, bA, bC, out var edgeStartB); + Vector3Wide.ConditionalSelect(useBCOnB, bB, edgeStartB, out edgeStartB); + Vector3Wide.ConditionalSelect(useABOnB, abB, caB, out var edgeOffsetB); + Vector3Wide.ConditionalSelect(useBCOnB, bcB, edgeOffsetB, out edgeOffsetB); + Vector3Wide.LengthSquared(edgeOffsetA, out var edgeOffsetALengthSquared); + Vector3Wide.LengthSquared(edgeOffsetB, out var edgeOffsetBLengthSquared); + var inverseEdgeOffsetALengthSquared = Vector.One / edgeOffsetALengthSquared; + var inverseEdgeOffsetBLengthSquared = Vector.One / edgeOffsetBLengthSquared; + TestEdgeEdge4(edgeStartA, edgeOffsetA, edgeOffsetALengthSquared, inverseEdgeOffsetALengthSquared, edgeStartB, edgeOffsetB, edgeOffsetBLengthSquared, inverseEdgeOffsetBLengthSquared, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out var localNormalCandidate); Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); } + ////A AB x * + //TestEdgeEdge(abA, abB, a.A, a.B, a.C, bA, bB, bC, out var depth, out var localNormal); + //TestEdgeEdge(abA, bcB, a.A, a.B, a.C, bA, bB, bC, out var depthCandidate, out var localNormalCandidate); + //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + //TestEdgeEdge(abA, caB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + + ////A BC x * + //TestEdgeEdge(bcA, abB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + //TestEdgeEdge(bcA, bcB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + //TestEdgeEdge(bcA, caB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + + ////A CA x * + //TestEdgeEdge(caA, abB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + //TestEdgeEdge(caA, bcB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + //TestEdgeEdge(caA, caB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + + ////Face normals + //Vector3Wide.CrossWithoutOverlap(abA, caA, out var faceNormalA); + //Vector3Wide.Length(faceNormalA, out var faceNormalALength); + //Vector3Wide.Scale(faceNormalA, Vector.One / faceNormalALength, out faceNormalA); + //GetDepthForNormal(a.A, a.B, a.C, bA, bB, bC, faceNormalA, out depthCandidate); + //Select(ref depth, ref localNormal, depthCandidate, faceNormalA); + //Vector3Wide.CrossWithoutOverlap(abB, caB, out var faceNormalB); + //Vector3Wide.Length(faceNormalB, out var faceNormalBLength); + //Vector3Wide.Scale(faceNormalB, Vector.One / faceNormalBLength, out faceNormalB); + //GetDepthForNormal(a.A, a.B, a.C, bA, bB, bC, faceNormalB, out var faceDepthB); + //Select(ref depth, ref localNormal, faceDepthB, faceNormalB); + + + //var tryVertexNormals = Vector.BitwiseAnd(Vector.LessThan(depth, Vector.Zero), allowContacts); + //Vector3Wide.LengthSquared(abA, out var abALengthSquared); + //Vector3Wide.LengthSquared(abB, out var abBLengthSquared); + //Vector3Wide.LengthSquared(caA, out var caALengthSquared); + //Vector3Wide.LengthSquared(caB, out var caBLengthSquared); + //if (Vector.LessThanAny(tryVertexNormals, Vector.Zero)) + //{ + // //Vertex normals are not required to determine penetration versus separation. They are only used to ensure correct separated speculative normals. + // //This isn't strictly required for behavior in the general case, but MeshReduction depends on it. + // Vector3Wide.LengthSquared(bcA, out var bcALengthSquared); + // Vector3Wide.LengthSquared(bcB, out var bcBLengthSquared); + // var inverseABALengthSquared = Vector.One / abALengthSquared; + // var inverseBCALengthSquared = Vector.One / bcALengthSquared; + // var inverseCAALengthSquared = Vector.One / caALengthSquared; + // var inverseABBLengthSquared = Vector.One / abBLengthSquared; + // var inverseBCBLengthSquared = Vector.One / bcBLengthSquared; + // var inverseCABLengthSquared = Vector.One / caBLengthSquared; + // TestVertexNormal(a.A, a.A, a.B, a.C, bA, bB, bC, abB, bcB, caB, inverseABBLengthSquared, inverseBCBLengthSquared, inverseCABLengthSquared, out depthCandidate, out localNormalCandidate); + // Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + // TestVertexNormal(a.B, a.A, a.B, a.C, bA, bB, bC, abB, bcB, caB, inverseABBLengthSquared, inverseBCBLengthSquared, inverseCABLengthSquared, out depthCandidate, out localNormalCandidate); + // Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + // TestVertexNormal(a.C, a.A, a.B, a.C, bA, bB, bC, abB, bcB, caB, inverseABBLengthSquared, inverseBCBLengthSquared, inverseCABLengthSquared, out depthCandidate, out localNormalCandidate); + // Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + // TestVertexNormal(bA, bA, bB, bC, a.A, a.B, a.C, abA, bcA, caA, inverseABALengthSquared, inverseBCALengthSquared, inverseCAALengthSquared, out depthCandidate, out localNormalCandidate); + // Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + // TestVertexNormal(bB, bA, bB, bC, a.A, a.B, a.C, abA, bcA, caA, inverseABALengthSquared, inverseBCALengthSquared, inverseCAALengthSquared, out depthCandidate, out localNormalCandidate); + // Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + // TestVertexNormal(bC, bA, bB, bC, a.A, a.B, a.C, abA, bcA, caA, inverseABALengthSquared, inverseBCALengthSquared, inverseCAALengthSquared, out depthCandidate, out localNormalCandidate); + // Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + //} + //Vector3Wide.LengthSquared(abA, out var abALengthSquared); //Vector3Wide.LengthSquared(abB, out var abBLengthSquared); //Vector3Wide.LengthSquared(caA, out var caALengthSquared); From ed3ee1535b79f1982de499cc5a8208123814da55 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 22 Sep 2021 21:17:56 -0500 Subject: [PATCH 007/947] Another attempt. --- .../CollisionTasks/TrianglePairTester.cs | 216 +++++++++--------- 1 file changed, 111 insertions(+), 105 deletions(-) diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs index 5774ed9d7..9572e19e4 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs @@ -42,26 +42,7 @@ static void TestEdgeEdge( //Protect against bad normals. depth = Vector.ConditionalSelect(Vector.LessThan(normalLength, new Vector(1e-10f)), new Vector(float.MaxValue), depth); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - static void TestEdgeEdge3( - in Vector3Wide edgeDirectionA, in Vector3Wide edgeDirectionB, in Vector edgeIndexA, in Vector edgeIndexB, - in Vector3Wide aA, in Vector3Wide bA, in Vector3Wide cA, in Vector3Wide aB, in Vector3Wide bB, in Vector3Wide cB, - ref Vector depth, ref Vector3Wide normal, ref Vector minimumEdgeA, ref Vector minimumEdgeB) - { - //Calibrate the normal to point from the triangle to the box while normalizing. - Vector3Wide.CrossWithoutOverlap(edgeDirectionA, edgeDirectionB, out var normalCandidate); - Vector3Wide.Length(normalCandidate, out var normalLength); - //Note that we do not calibrate yet. The depth calculation does not rely on calibration, so we punt it until after all normals have been tested. - Vector3Wide.Scale(normalCandidate, Vector.One / normalLength, out normalCandidate); - GetDepthForNormal(aA, bA, cA, aB, bB, cB, normalCandidate, out var depthCandidate); - //Protect against bad normals. - depthCandidate = Vector.ConditionalSelect(Vector.LessThan(normalLength, new Vector(1e-10f)), new Vector(float.MaxValue), depthCandidate); - var useCandidate = Vector.LessThan(depthCandidate, depth); - depth = Vector.Min(depthCandidate, depth); - Vector3Wide.ConditionalSelect(useCandidate, normalCandidate, normal, out normal); - minimumEdgeA = Vector.ConditionalSelect(useCandidate, edgeIndexA, minimumEdgeA); - minimumEdgeB = Vector.ConditionalSelect(useCandidate, edgeIndexB, minimumEdgeB); - } + [MethodImpl(MethodImplOptions.AggressiveInlining)] static void TestEdgeEdge2( @@ -99,38 +80,6 @@ static void TestEdgeEdge2( //Protect against bad normals. depth = Vector.ConditionalSelect(Vector.LessThan(normalLength, new Vector(1e-10f)), new Vector(float.MaxValue), depth); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - static void TestEdgeEdge4( - in Vector3Wide edgeStartA, in Vector3Wide edgeOffsetA, in Vector edgeOffsetALengthSquared, in Vector inverseEdgeOffsetALengthSquared, - in Vector3Wide edgeStartB, in Vector3Wide edgeOffsetB, in Vector edgeOffsetBLengthSquared, in Vector inverseEdgeOffsetBLengthSquared, - in Vector3Wide aA, in Vector3Wide bA, in Vector3Wide cA, in Vector3Wide aB, in Vector3Wide bB, in Vector3Wide cB, - out Vector depth, out Vector3Wide normal) - { - Vector3Wide.Subtract(edgeStartA, edgeStartB, out var bStartToAStart); - Vector3Wide.Dot(edgeOffsetA, bStartToAStart, out var oADotAB); - Vector3Wide.Dot(edgeOffsetB, bStartToAStart, out var oBDotAB); - Vector3Wide.Dot(edgeOffsetA, edgeOffsetB, out var oADotOB); - var denominator = edgeOffsetALengthSquared * edgeOffsetBLengthSquared - oADotOB * oADotOB; //TODO: div 0 guard if edge offsets are parallel. - //Compute the first guess for tA and clamp it. - var tA = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (oADotOB * oBDotAB - oADotAB * edgeOffsetBLengthSquared) / denominator)); - //Compute the closest point on B to the first guess. - var tB = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (oADotOB * tA + oBDotAB) * inverseEdgeOffsetBLengthSquared)); - //Finally, compute the true tA. - tA = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (tB * oADotOB - oADotAB) * inverseEdgeOffsetALengthSquared)); - - Vector3Wide.Scale(edgeOffsetA, tA, out var startToClosestA); - Vector3Wide.Add(edgeStartA, startToClosestA, out var a); - Vector3Wide.Scale(edgeOffsetB, tB, out var startToClosestB); - Vector3Wide.Add(edgeStartB, startToClosestB, out var b); - - //Note that this will produce poor results if the edges overlap, but in that case, the vertex normals aren't going to be relevant anyway so it's fine to ignore their contribution. - Vector3Wide.Subtract(a, b, out normal); - Vector3Wide.Length(normal, out var normalLength); - Vector3Wide.Scale(normal, Vector.One / normalLength, out normal); - GetDepthForNormal(aA, bA, cA, aB, bB, cB, normal, out depth); - //Protect against bad normals. - depth = Vector.ConditionalSelect(Vector.LessThan(normalLength, new Vector(1e-7f)), new Vector(float.MaxValue), depth); - } [MethodImpl(MethodImplOptions.AggressiveInlining)] static void TestVertexNormal(in Vector3Wide vertex, @@ -189,26 +138,26 @@ static void TestVertexNormal2(in Vector3Wide vertex, Vector3Wide.Dot(cv, edgePlaneNormalCA, out var cvDotPlaneCA); //Push the vertex down to the triangle plane. Vector3Wide.Dot(av, triangleNormal, out var flattenDot); - Vector3Wide flattenedVertex; - flattenedVertex.X = vertex.X - flattenDot * triangleNormal.X; - flattenedVertex.Y = vertex.Y - flattenDot * triangleNormal.Y; - flattenedVertex.Z = vertex.Z - flattenDot * triangleNormal.Z; + Vector3Wide vertexToTrianglePlane; + vertexToTrianglePlane.X = flattenDot * triangleNormal.X; + vertexToTrianglePlane.Y = flattenDot * triangleNormal.Y; + vertexToTrianglePlane.Z = flattenDot * triangleNormal.Z; //Edge plane normals should have the same magnitude as the edge lengths, since they're just rotated 90 degrees. Triangle normal is perfectly perpendicular to the edges by construction. var tAB = avDotPlaneAB * inverseLengthSquaredAB; var tBC = bvDotPlaneBC * inverseLengthSquaredBC; var tCA = cvDotPlaneCA * inverseLengthSquaredCA; - Vector3Wide onAB, onBC, onCA; - onAB.X = flattenedVertex.X - edgePlaneNormalAB.X * tAB; - onAB.Y = flattenedVertex.Y - edgePlaneNormalAB.Y * tAB; - onAB.Z = flattenedVertex.Z - edgePlaneNormalAB.Z * tAB; - onBC.X = flattenedVertex.X - edgePlaneNormalBC.X * tBC; - onBC.Y = flattenedVertex.Y - edgePlaneNormalBC.Y * tBC; - onBC.Z = flattenedVertex.Z - edgePlaneNormalBC.Z * tBC; - onCA.X = flattenedVertex.X - edgePlaneNormalCA.X * tCA; - onCA.Y = flattenedVertex.Y - edgePlaneNormalCA.Y * tCA; - onCA.Z = flattenedVertex.Z - edgePlaneNormalCA.Z * tCA; - - //If the vertex is outside one edge plane, use the vertex + Vector3Wide toAB, toBC, toCA; + toAB.X = vertexToTrianglePlane.X - edgePlaneNormalAB.X * tAB; + toAB.Y = vertexToTrianglePlane.Y - edgePlaneNormalAB.Y * tAB; + toAB.Z = vertexToTrianglePlane.Z - edgePlaneNormalAB.Z * tAB; + toBC.X = vertexToTrianglePlane.X - edgePlaneNormalBC.X * tBC; + toBC.Y = vertexToTrianglePlane.Y - edgePlaneNormalBC.Y * tBC; + toBC.Z = vertexToTrianglePlane.Z - edgePlaneNormalBC.Z * tBC; + toCA.X = vertexToTrianglePlane.X - edgePlaneNormalCA.X * tCA; + toCA.Y = vertexToTrianglePlane.Y - edgePlaneNormalCA.Y * tCA; + toCA.Z = vertexToTrianglePlane.Z - edgePlaneNormalCA.Z * tCA; + + //If the vertex is outside one edge plane, use the vertex projected onto that edge. //If the vertex is outside two edge planes, it is on the shared vertex. var outsideAB = Vector.GreaterThanOrEqual(avDotPlaneAB, Vector.Zero); var outsideBC = Vector.GreaterThanOrEqual(bvDotPlaneBC, Vector.Zero); @@ -231,6 +180,52 @@ static void TestVertexNormal2(in Vector3Wide vertex, } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + static void TestVertexNormal3(in Vector3Wide vertex, + in Vector3Wide opposingA, in Vector3Wide opposingB, in Vector3Wide opposingC, + in Vector3Wide opposingAB, in Vector3Wide opposingBC, in Vector3Wide opposingCA, + in Vector3Wide opposingEdgePlaneAB, in Vector3Wide opposingEdgePlaneBC, + in Vector inverseLengthSquaredAB, in Vector inverseLengthSquaredBC, in Vector inverseLengthSquaredCA, in Vector triangleNormalLength, + out Vector distanceSquared, out Vector3Wide offset) + { + //Project the vertex onto all three edges. Take the closest approach as a normal candidate. + //Note that this will try normals that cross over the triangle's face, but that's fine- it'll just be a crappy normal candidate and another option will be chosen instead. + Vector3Wide.Subtract(vertex, opposingA, out var av); + Vector3Wide.Subtract(vertex, opposingB, out var bv); + Vector3Wide.Subtract(vertex, opposingC, out var cv); + Vector3Wide.Dot(av, opposingEdgePlaneAB, out var avDotEdgePlaneAB); + Vector3Wide.Dot(bv, opposingEdgePlaneBC, out var bvDotEdgePlaneBC); + //Because of the equivalence between the edge plane tests and barycentric coordinates, we can compute the last edge plane dot implicitly: + var cvDotEdgePlaneCA = triangleNormalLength - avDotEdgePlaneAB - bvDotEdgePlaneBC; + var outsideTriangle = Vector.BitwiseOr(Vector.GreaterThan(avDotEdgePlaneAB, Vector.Zero), Vector.BitwiseOr(Vector.GreaterThan(bvDotEdgePlaneBC, Vector.Zero), Vector.GreaterThan(cvDotEdgePlaneCA, Vector.Zero))); + Vector3Wide.Dot(av, opposingAB, out var avDotAB); + Vector3Wide.Dot(bv, opposingBC, out var bvDotBC); + Vector3Wide.Dot(cv, opposingCA, out var cvDotCA); + var tAB = Vector.Max(Vector.Zero, Vector.Min(Vector.One, avDotAB * inverseLengthSquaredAB)); + var tBC = Vector.Max(Vector.Zero, Vector.Min(Vector.One, bvDotBC * inverseLengthSquaredBC)); + var tCA = Vector.Max(Vector.Zero, Vector.Min(Vector.One, cvDotCA * inverseLengthSquaredCA)); + Vector3Wide vToAB, vToBC, vToCA; + vToAB.X = opposingA.X + opposingAB.X * tAB - vertex.X; + vToAB.Y = opposingA.Y + opposingAB.Y * tAB - vertex.Y; + vToAB.Z = opposingA.Z + opposingAB.Z * tAB - vertex.Z; + vToBC.X = opposingB.X + opposingBC.X * tBC - vertex.X; + vToBC.Y = opposingB.Y + opposingBC.Y * tBC - vertex.Y; + vToBC.Z = opposingB.Z + opposingBC.Z * tBC - vertex.Z; + vToCA.X = opposingC.X + opposingCA.X * tCA - vertex.X; + vToCA.Y = opposingC.Y + opposingCA.Y * tCA - vertex.Y; + vToCA.Z = opposingC.Z + opposingCA.Z * tCA - vertex.Z; + Vector3Wide.LengthSquared(vToAB, out var abDistanceSquared); + Vector3Wide.LengthSquared(vToBC, out var bcDistanceSquared); + Vector3Wide.LengthSquared(vToCA, out var caDistanceSquared); + + distanceSquared = Vector.Min(abDistanceSquared, Vector.Min(bcDistanceSquared, caDistanceSquared)); + Vector3Wide.ConditionalSelect(Vector.Equals(distanceSquared, abDistanceSquared), vToAB, vToCA, out offset); + Vector3Wide.ConditionalSelect(Vector.Equals(distanceSquared, bcDistanceSquared), vToBC, offset, out offset); + + //Ignore this vertex if it's contained within the triangle. We'll detect distances that are too close to generate normals outside. + distanceSquared = Vector.ConditionalSelect(outsideTriangle, distanceSquared, new Vector(float.MaxValue)); + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] static void Select( ref Vector depth, ref Vector3Wide normal, @@ -440,31 +435,34 @@ public unsafe void Test( ManifoldCandidateHelper.CreateActiveMask(pairCount, out var allowContacts); - var two = new Vector(2); - var depth = new Vector(float.MaxValue); - Unsafe.SkipInit(out Vector3Wide localNormal); - Unsafe.SkipInit(out Vector minimumEdgeA); - Unsafe.SkipInit(out Vector minimumEdgeB); //A AB x * - TestEdgeEdge3(abA, abB, Vector.Zero, Vector.Zero, a.A, a.B, a.C, bA, bB, bC, ref depth, ref localNormal, ref minimumEdgeA, ref minimumEdgeB); - TestEdgeEdge3(abA, bcB, Vector.Zero, Vector.One, a.A, a.B, a.C, bA, bB, bC, ref depth, ref localNormal, ref minimumEdgeA, ref minimumEdgeB); - TestEdgeEdge3(abA, caB, Vector.Zero, two, a.A, a.B, a.C, bA, bB, bC, ref depth, ref localNormal, ref minimumEdgeA, ref minimumEdgeB); + TestEdgeEdge(abA, abB, a.A, a.B, a.C, bA, bB, bC, out var depth, out var localNormal); + TestEdgeEdge(abA, bcB, a.A, a.B, a.C, bA, bB, bC, out var depthCandidate, out var localNormalCandidate); + Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + TestEdgeEdge(abA, caB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); //A BC x * - TestEdgeEdge3(bcA, abB, Vector.One, Vector.Zero, a.A, a.B, a.C, bA, bB, bC, ref depth, ref localNormal, ref minimumEdgeA, ref minimumEdgeB); - TestEdgeEdge3(bcA, bcB, Vector.One, Vector.One, a.A, a.B, a.C, bA, bB, bC, ref depth, ref localNormal, ref minimumEdgeA, ref minimumEdgeB); - TestEdgeEdge3(bcA, caB, Vector.One, two, a.A, a.B, a.C, bA, bB, bC, ref depth, ref localNormal, ref minimumEdgeA, ref minimumEdgeB); + TestEdgeEdge(bcA, abB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + TestEdgeEdge(bcA, bcB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + TestEdgeEdge(bcA, caB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); //A CA x * - TestEdgeEdge3(caA, abB, two, Vector.Zero, a.A, a.B, a.C, bA, bB, bC, ref depth, ref localNormal, ref minimumEdgeA, ref minimumEdgeB); - TestEdgeEdge3(caA, bcB, two, Vector.One, a.A, a.B, a.C, bA, bB, bC, ref depth, ref localNormal, ref minimumEdgeA, ref minimumEdgeB); - TestEdgeEdge3(caA, caB, two, two, a.A, a.B, a.C, bA, bB, bC, ref depth, ref localNormal, ref minimumEdgeA, ref minimumEdgeB); + TestEdgeEdge(caA, abB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + TestEdgeEdge(caA, bcB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + TestEdgeEdge(caA, caB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); //Face normals Vector3Wide.CrossWithoutOverlap(abA, caA, out var faceNormalA); Vector3Wide.Length(faceNormalA, out var faceNormalALength); Vector3Wide.Scale(faceNormalA, Vector.One / faceNormalALength, out faceNormalA); - GetDepthForNormal(a.A, a.B, a.C, bA, bB, bC, faceNormalA, out var depthCandidate); + GetDepthForNormal(a.A, a.B, a.C, bA, bB, bC, faceNormalA, out depthCandidate); Select(ref depth, ref localNormal, depthCandidate, faceNormalA); Vector3Wide.CrossWithoutOverlap(abB, caB, out var faceNormalB); Vector3Wide.Length(faceNormalB, out var faceNormalBLength); @@ -482,28 +480,36 @@ public unsafe void Test( { //Vertex normals are not required to determine penetration versus separation. They are only used to ensure correct separated speculative normals. //This isn't strictly required for behavior in the general case, but MeshReduction depends on it. - //To generate them, the above edge-edge tests tracked the edge pair which generated the lowest depth. - //If a vertex contributes a better normal, then it will be associated with those minimal edges. - //So, just perform a closest points query on the minimum edge pair. - var useABOnA = Vector.Equals(minimumEdgeA, Vector.Zero); - var useBCOnA = Vector.Equals(minimumEdgeA, Vector.One); - var useCAOnA = Vector.Equals(minimumEdgeA, two); - var useABOnB = Vector.Equals(minimumEdgeB, Vector.Zero); - var useBCOnB = Vector.Equals(minimumEdgeB, Vector.One); - var useCAOnB = Vector.Equals(minimumEdgeB, two); - Vector3Wide.ConditionalSelect(useABOnA, a.A, a.C, out var edgeStartA); - Vector3Wide.ConditionalSelect(useBCOnA, a.B, edgeStartA, out edgeStartA); - Vector3Wide.ConditionalSelect(useABOnA, abA, caA, out var edgeOffsetA); - Vector3Wide.ConditionalSelect(useBCOnA, bcA, edgeOffsetA, out edgeOffsetA); - Vector3Wide.ConditionalSelect(useABOnB, bA, bC, out var edgeStartB); - Vector3Wide.ConditionalSelect(useBCOnB, bB, edgeStartB, out edgeStartB); - Vector3Wide.ConditionalSelect(useABOnB, abB, caB, out var edgeOffsetB); - Vector3Wide.ConditionalSelect(useBCOnB, bcB, edgeOffsetB, out edgeOffsetB); - Vector3Wide.LengthSquared(edgeOffsetA, out var edgeOffsetALengthSquared); - Vector3Wide.LengthSquared(edgeOffsetB, out var edgeOffsetBLengthSquared); - var inverseEdgeOffsetALengthSquared = Vector.One / edgeOffsetALengthSquared; - var inverseEdgeOffsetBLengthSquared = Vector.One / edgeOffsetBLengthSquared; - TestEdgeEdge4(edgeStartA, edgeOffsetA, edgeOffsetALengthSquared, inverseEdgeOffsetALengthSquared, edgeStartB, edgeOffsetB, edgeOffsetBLengthSquared, inverseEdgeOffsetBLengthSquared, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out var localNormalCandidate); + Vector3Wide.LengthSquared(bcA, out var bcALengthSquared); + Vector3Wide.LengthSquared(bcB, out var bcBLengthSquared); + var inverseABALengthSquared = Vector.One / abALengthSquared; + var inverseBCALengthSquared = Vector.One / bcALengthSquared; + var inverseCAALengthSquared = Vector.One / caALengthSquared; + var inverseABBLengthSquared = Vector.One / abBLengthSquared; + var inverseBCBLengthSquared = Vector.One / bcBLengthSquared; + var inverseCABLengthSquared = Vector.One / caBLengthSquared; + Vector3Wide.Cross(abA, faceNormalA, out var edgeNormalABOnA); + Vector3Wide.Cross(bcA, faceNormalA, out var edgeNormalBCOnA); + Vector3Wide.Cross(abB, faceNormalB, out var edgeNormalABOnB); + Vector3Wide.Cross(bcB, faceNormalB, out var edgeNormalBCOnB); + TestVertexNormal3(a.A, bA, bB, bC, abB, bcB, caB, edgeNormalABOnB, edgeNormalBCOnB, inverseABBLengthSquared, inverseBCBLengthSquared, inverseCABLengthSquared, faceNormalBLength, out var distanceSquared, out var offset); + Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + TestVertexNormal3(a.B, bA, bB, bC, abB, bcB, caB, edgeNormalABOnB, edgeNormalBCOnB, inverseABBLengthSquared, inverseBCBLengthSquared, inverseCABLengthSquared, faceNormalBLength, out var distanceSquaredCandidate, out var offsetCandidate); + Select(ref distanceSquared, ref offset, distanceSquaredCandidate, offsetCandidate); + TestVertexNormal3(a.C, bA, bB, bC, abB, bcB, caB, edgeNormalABOnB, edgeNormalBCOnB, inverseABBLengthSquared, inverseBCBLengthSquared, inverseCABLengthSquared, faceNormalBLength, out distanceSquaredCandidate, out offsetCandidate); + Select(ref distanceSquared, ref offset, distanceSquaredCandidate, offsetCandidate); + TestVertexNormal3(bA, a.A, a.B, a.C, abA, bcA, caA, edgeNormalABOnA, edgeNormalBCOnA, inverseABALengthSquared, inverseBCALengthSquared, inverseCAALengthSquared, faceNormalALength, out distanceSquaredCandidate, out offsetCandidate); + Select(ref distanceSquared, ref offset, distanceSquaredCandidate, offsetCandidate); + TestVertexNormal3(bB, a.A, a.B, a.C, abA, bcA, caA, edgeNormalABOnA, edgeNormalBCOnA, inverseABALengthSquared, inverseBCALengthSquared, inverseCAALengthSquared, faceNormalALength, out distanceSquaredCandidate, out offsetCandidate); + Select(ref distanceSquared, ref offset, distanceSquaredCandidate, offsetCandidate); + TestVertexNormal3(bC, a.A, a.B, a.C, abA, bcA, caA, edgeNormalABOnA, edgeNormalBCOnA, inverseABALengthSquared, inverseBCALengthSquared, inverseCAALengthSquared, faceNormalALength, out distanceSquaredCandidate, out offsetCandidate); + Select(ref distanceSquared, ref offset, distanceSquaredCandidate, offsetCandidate); + + var distance = Vector.SquareRoot(distanceSquared); + Vector3Wide.Scale(offset, Vector.One / distance, out localNormalCandidate); + //Don't try to use distances that are so small that the resulting normal will be numerically bad. That's close enough to intersecting that the previous normals will handle it. + GetDepthForNormal(a.A, a.B, a.C, bA, bB, bC, localNormalCandidate, out depthCandidate); + depthCandidate = Vector.ConditionalSelect(Vector.GreaterThan(distance, new Vector(1e-7f)), depthCandidate, new Vector(float.MaxValue)); Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); } From dacfdc46161181ea72d216214d1ee5b4b617e0ca Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 22 Sep 2021 21:51:53 -0500 Subject: [PATCH 008/947] Tuning and one final slight variant. --- .../CollisionTasks/TrianglePairTester.cs | 160 ++++++++++++++++-- 1 file changed, 142 insertions(+), 18 deletions(-) diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs index 9572e19e4..1ac242115 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs @@ -44,7 +44,7 @@ static void TestEdgeEdge( } - [MethodImpl(MethodImplOptions.AggressiveInlining)] + //[MethodImpl(MethodImplOptions.AggressiveInlining)] static void TestEdgeEdge2( in Vector3Wide edgeStartA, in Vector3Wide edgeOffsetA, in Vector edgeOffsetALengthSquared, in Vector inverseEdgeOffsetALengthSquared, in Vector3Wide edgeStartB, in Vector3Wide edgeOffsetB, in Vector edgeOffsetBLengthSquared, in Vector inverseEdgeOffsetBLengthSquared, @@ -81,7 +81,7 @@ static void TestEdgeEdge2( depth = Vector.ConditionalSelect(Vector.LessThan(normalLength, new Vector(1e-10f)), new Vector(float.MaxValue), depth); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] + //[MethodImpl(MethodImplOptions.AggressiveInlining)] static void TestVertexNormal(in Vector3Wide vertex, in Vector3Wide a, in Vector3Wide b, in Vector3Wide c, in Vector3Wide opposingA, in Vector3Wide opposingB, in Vector3Wide opposingC, @@ -180,7 +180,7 @@ static void TestVertexNormal2(in Vector3Wide vertex, } - [MethodImpl(MethodImplOptions.AggressiveInlining)] + //[MethodImpl(MethodImplOptions.AggressiveInlining)] static void TestVertexNormal3(in Vector3Wide vertex, in Vector3Wide opposingA, in Vector3Wide opposingB, in Vector3Wide opposingC, in Vector3Wide opposingAB, in Vector3Wide opposingBC, in Vector3Wide opposingCA, @@ -226,6 +226,54 @@ static void TestVertexNormal3(in Vector3Wide vertex, distanceSquared = Vector.ConditionalSelect(outsideTriangle, distanceSquared, new Vector(float.MaxValue)); } + //[MethodImpl(MethodImplOptions.AggressiveInlining)] + static void TestVertexNormal4(in Vector3Wide vertex, + in Vector3Wide opposingA, in Vector3Wide opposingB, in Vector3Wide opposingC, + in Vector3Wide opposingAB, in Vector3Wide opposingBC, in Vector3Wide opposingCA, + in Vector3Wide opposingEdgePlaneAB, in Vector3Wide opposingEdgePlaneBC, + in Vector inverseLengthSquaredAB, in Vector inverseLengthSquaredBC, in Vector inverseLengthSquaredCA, in Vector triangleNormalLength, + ref Vector distanceSquared, ref Vector3Wide offset) + { + //Project the vertex onto all three edges. Take the closest approach as a normal candidate. + //Note that this will try normals that cross over the triangle's face, but that's fine- it'll just be a crappy normal candidate and another option will be chosen instead. + Vector3Wide.Subtract(vertex, opposingA, out var av); + Vector3Wide.Subtract(vertex, opposingB, out var bv); + Vector3Wide.Subtract(vertex, opposingC, out var cv); + Vector3Wide.Dot(av, opposingEdgePlaneAB, out var avDotEdgePlaneAB); + Vector3Wide.Dot(bv, opposingEdgePlaneBC, out var bvDotEdgePlaneBC); + //Because of the equivalence between the edge plane tests and barycentric coordinates, we can compute the last edge plane dot implicitly: + var cvDotEdgePlaneCA = triangleNormalLength - avDotEdgePlaneAB - bvDotEdgePlaneBC; + var outsideTriangle = Vector.BitwiseOr(Vector.GreaterThan(avDotEdgePlaneAB, Vector.Zero), Vector.BitwiseOr(Vector.GreaterThan(bvDotEdgePlaneBC, Vector.Zero), Vector.GreaterThan(cvDotEdgePlaneCA, Vector.Zero))); + Vector3Wide.Dot(av, opposingAB, out var avDotAB); + Vector3Wide.Dot(bv, opposingBC, out var bvDotBC); + Vector3Wide.Dot(cv, opposingCA, out var cvDotCA); + var tAB = Vector.Max(Vector.Zero, Vector.Min(Vector.One, avDotAB * inverseLengthSquaredAB)); + var tBC = Vector.Max(Vector.Zero, Vector.Min(Vector.One, bvDotBC * inverseLengthSquaredBC)); + var tCA = Vector.Max(Vector.Zero, Vector.Min(Vector.One, cvDotCA * inverseLengthSquaredCA)); + Vector3Wide vToAB, vToBC, vToCA; + vToAB.X = opposingA.X + opposingAB.X * tAB - vertex.X; + vToAB.Y = opposingA.Y + opposingAB.Y * tAB - vertex.Y; + vToAB.Z = opposingA.Z + opposingAB.Z * tAB - vertex.Z; + vToBC.X = opposingB.X + opposingBC.X * tBC - vertex.X; + vToBC.Y = opposingB.Y + opposingBC.Y * tBC - vertex.Y; + vToBC.Z = opposingB.Z + opposingBC.Z * tBC - vertex.Z; + vToCA.X = opposingC.X + opposingCA.X * tCA - vertex.X; + vToCA.Y = opposingC.Y + opposingCA.Y * tCA - vertex.Y; + vToCA.Z = opposingC.Z + opposingCA.Z * tCA - vertex.Z; + Vector3Wide.LengthSquared(vToAB, out var abDistanceSquared); + Vector3Wide.LengthSquared(vToBC, out var bcDistanceSquared); + Vector3Wide.LengthSquared(vToCA, out var caDistanceSquared); + + var distanceSquaredCandidate = Vector.Min(abDistanceSquared, Vector.Min(bcDistanceSquared, caDistanceSquared)); + Vector3Wide.ConditionalSelect(Vector.Equals(distanceSquaredCandidate, abDistanceSquared), vToAB, vToCA, out var offsetCandidate); + Vector3Wide.ConditionalSelect(Vector.Equals(distanceSquaredCandidate, bcDistanceSquared), vToBC, offsetCandidate, out offsetCandidate); + + //Ignore this vertex if it's contained within the triangle. We'll detect distances that are too close to generate normals outside. + var useCandidate = Vector.BitwiseAnd(Vector.LessThan(distanceSquaredCandidate, distanceSquared), outsideTriangle); + distanceSquared = Vector.ConditionalSelect(useCandidate, distanceSquaredCandidate, distanceSquared); + Vector3Wide.ConditionalSelect(useCandidate, offsetCandidate, offset, out offset); + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] static void Select( ref Vector depth, ref Vector3Wide normal, @@ -287,7 +335,7 @@ private static void TryAddTriangleAVertex(in Vector3Wide vertex, in Vector2Wide ManifoldCandidateHelper.AddCandidateWithDepth(ref candidates, ref candidateCount, candidate, Vector.BitwiseAnd(Vector.GreaterThanOrEqual(candidate.Depth, minimumDepth), Vector.BitwiseAnd(allowContacts, contained)), pairCount); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] + //[MethodImpl(MethodImplOptions.AggressiveInlining)] private static void ClipEdge( in Vector2Wide edgeStartB, in Vector2Wide edgeOffsetB, in Vector2Wide edgeStartA, in Vector2Wide edgeOffsetA, in Vector inverseEdgeLengthSquaredA, in Vector edgeStartADotNormal, in Vector edgeOffsetADotNormal, @@ -309,7 +357,7 @@ private static void ClipEdge( depthContributionA = edgeStartADotNormal + edgeOffsetADotNormal * tA; } - [MethodImpl(MethodImplOptions.AggressiveInlining)] + //[MethodImpl(MethodImplOptions.AggressiveInlining)] private static void ClipBEdgeAgainstABounds( //maybe we should have.. MORE parameters in Vector2Wide aA, in Vector2Wide aB, in Vector2Wide aC, in Vector2Wide edgeOffsetABOnA, in Vector2Wide edgeOffsetBCOnA, in Vector2Wide edgeOffsetCAOnA, @@ -399,7 +447,7 @@ private static void ClipBEdgeAgainstABounds( //maybe we should have.. MORE param - [MethodImpl(MethodImplOptions.AggressiveInlining)] + //[MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe void Test( ref TriangleWide a, ref TriangleWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, @@ -492,18 +540,14 @@ public unsafe void Test( Vector3Wide.Cross(bcA, faceNormalA, out var edgeNormalBCOnA); Vector3Wide.Cross(abB, faceNormalB, out var edgeNormalABOnB); Vector3Wide.Cross(bcB, faceNormalB, out var edgeNormalBCOnB); - TestVertexNormal3(a.A, bA, bB, bC, abB, bcB, caB, edgeNormalABOnB, edgeNormalBCOnB, inverseABBLengthSquared, inverseBCBLengthSquared, inverseCABLengthSquared, faceNormalBLength, out var distanceSquared, out var offset); - Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - TestVertexNormal3(a.B, bA, bB, bC, abB, bcB, caB, edgeNormalABOnB, edgeNormalBCOnB, inverseABBLengthSquared, inverseBCBLengthSquared, inverseCABLengthSquared, faceNormalBLength, out var distanceSquaredCandidate, out var offsetCandidate); - Select(ref distanceSquared, ref offset, distanceSquaredCandidate, offsetCandidate); - TestVertexNormal3(a.C, bA, bB, bC, abB, bcB, caB, edgeNormalABOnB, edgeNormalBCOnB, inverseABBLengthSquared, inverseBCBLengthSquared, inverseCABLengthSquared, faceNormalBLength, out distanceSquaredCandidate, out offsetCandidate); - Select(ref distanceSquared, ref offset, distanceSquaredCandidate, offsetCandidate); - TestVertexNormal3(bA, a.A, a.B, a.C, abA, bcA, caA, edgeNormalABOnA, edgeNormalBCOnA, inverseABALengthSquared, inverseBCALengthSquared, inverseCAALengthSquared, faceNormalALength, out distanceSquaredCandidate, out offsetCandidate); - Select(ref distanceSquared, ref offset, distanceSquaredCandidate, offsetCandidate); - TestVertexNormal3(bB, a.A, a.B, a.C, abA, bcA, caA, edgeNormalABOnA, edgeNormalBCOnA, inverseABALengthSquared, inverseBCALengthSquared, inverseCAALengthSquared, faceNormalALength, out distanceSquaredCandidate, out offsetCandidate); - Select(ref distanceSquared, ref offset, distanceSquaredCandidate, offsetCandidate); - TestVertexNormal3(bC, a.A, a.B, a.C, abA, bcA, caA, edgeNormalABOnA, edgeNormalBCOnA, inverseABALengthSquared, inverseBCALengthSquared, inverseCAALengthSquared, faceNormalALength, out distanceSquaredCandidate, out offsetCandidate); - Select(ref distanceSquared, ref offset, distanceSquaredCandidate, offsetCandidate); + var distanceSquared = new Vector(float.MaxValue); + Unsafe.SkipInit(out Vector3Wide offset); + TestVertexNormal4(a.A, bA, bB, bC, abB, bcB, caB, edgeNormalABOnB, edgeNormalBCOnB, inverseABBLengthSquared, inverseBCBLengthSquared, inverseCABLengthSquared, faceNormalBLength, ref distanceSquared, ref offset); + TestVertexNormal4(a.B, bA, bB, bC, abB, bcB, caB, edgeNormalABOnB, edgeNormalBCOnB, inverseABBLengthSquared, inverseBCBLengthSquared, inverseCABLengthSquared, faceNormalBLength, ref distanceSquared, ref offset); + TestVertexNormal4(a.C, bA, bB, bC, abB, bcB, caB, edgeNormalABOnB, edgeNormalBCOnB, inverseABBLengthSquared, inverseBCBLengthSquared, inverseCABLengthSquared, faceNormalBLength, ref distanceSquared, ref offset); + TestVertexNormal4(bA, a.A, a.B, a.C, abA, bcA, caA, edgeNormalABOnA, edgeNormalBCOnA, inverseABALengthSquared, inverseBCALengthSquared, inverseCAALengthSquared, faceNormalALength, ref distanceSquared, ref offset); + TestVertexNormal4(bB, a.A, a.B, a.C, abA, bcA, caA, edgeNormalABOnA, edgeNormalBCOnA, inverseABALengthSquared, inverseBCALengthSquared, inverseCAALengthSquared, faceNormalALength, ref distanceSquared, ref offset); + TestVertexNormal4(bC, a.A, a.B, a.C, abA, bcA, caA, edgeNormalABOnA, edgeNormalBCOnA, inverseABALengthSquared, inverseBCALengthSquared, inverseCAALengthSquared, faceNormalALength, ref distanceSquared, ref offset); var distance = Vector.SquareRoot(distanceSquared); Vector3Wide.Scale(offset, Vector.One / distance, out localNormalCandidate); @@ -549,6 +593,86 @@ public unsafe void Test( //Select(ref depth, ref localNormal, faceDepthB, faceNormalB); + //var tryVertexNormals = Vector.BitwiseAnd(Vector.LessThan(depth, Vector.Zero), allowContacts); + //Vector3Wide.LengthSquared(abA, out var abALengthSquared); + //Vector3Wide.LengthSquared(abB, out var abBLengthSquared); + //Vector3Wide.LengthSquared(caA, out var caALengthSquared); + //Vector3Wide.LengthSquared(caB, out var caBLengthSquared); + //if (Vector.LessThanAny(tryVertexNormals, Vector.Zero)) + //{ + // //Vertex normals are not required to determine penetration versus separation. They are only used to ensure correct separated speculative normals. + // //This isn't strictly required for behavior in the general case, but MeshReduction depends on it. + // Vector3Wide.LengthSquared(bcA, out var bcALengthSquared); + // Vector3Wide.LengthSquared(bcB, out var bcBLengthSquared); + // var inverseABALengthSquared = Vector.One / abALengthSquared; + // var inverseBCALengthSquared = Vector.One / bcALengthSquared; + // var inverseCAALengthSquared = Vector.One / caALengthSquared; + // var inverseABBLengthSquared = Vector.One / abBLengthSquared; + // var inverseBCBLengthSquared = Vector.One / bcBLengthSquared; + // var inverseCABLengthSquared = Vector.One / caBLengthSquared; + // Vector3Wide.Cross(abA, faceNormalA, out var edgeNormalABOnA); + // Vector3Wide.Cross(bcA, faceNormalA, out var edgeNormalBCOnA); + // Vector3Wide.Cross(abB, faceNormalB, out var edgeNormalABOnB); + // Vector3Wide.Cross(bcB, faceNormalB, out var edgeNormalBCOnB); + // var distanceSquared = new Vector(float.MaxValue); + // Unsafe.SkipInit(out Vector3Wide offset); + // TestVertexNormal3(a.A, bA, bB, bC, abB, bcB, caB, edgeNormalABOnB, edgeNormalBCOnB, inverseABBLengthSquared, inverseBCBLengthSquared, inverseCABLengthSquared, faceNormalBLength, ref distanceSquared, ref offset); + // //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + // TestVertexNormal3(a.B, bA, bB, bC, abB, bcB, caB, edgeNormalABOnB, edgeNormalBCOnB, inverseABBLengthSquared, inverseBCBLengthSquared, inverseCABLengthSquared, faceNormalBLength, ref distanceSquaredCandidate, out var offsetCandidate); + // Select(ref distanceSquared, ref offset, distanceSquaredCandidate, offsetCandidate); + // TestVertexNormal3(a.C, bA, bB, bC, abB, bcB, caB, edgeNormalABOnB, edgeNormalBCOnB, inverseABBLengthSquared, inverseBCBLengthSquared, inverseCABLengthSquared, faceNormalBLength, out distanceSquaredCandidate, out offsetCandidate); + // Select(ref distanceSquared, ref offset, distanceSquaredCandidate, offsetCandidate); + // TestVertexNormal3(bA, a.A, a.B, a.C, abA, bcA, caA, edgeNormalABOnA, edgeNormalBCOnA, inverseABALengthSquared, inverseBCALengthSquared, inverseCAALengthSquared, faceNormalALength, out distanceSquaredCandidate, out offsetCandidate); + // Select(ref distanceSquared, ref offset, distanceSquaredCandidate, offsetCandidate); + // TestVertexNormal3(bB, a.A, a.B, a.C, abA, bcA, caA, edgeNormalABOnA, edgeNormalBCOnA, inverseABALengthSquared, inverseBCALengthSquared, inverseCAALengthSquared, faceNormalALength, out distanceSquaredCandidate, out offsetCandidate); + // Select(ref distanceSquared, ref offset, distanceSquaredCandidate, offsetCandidate); + // TestVertexNormal3(bC, a.A, a.B, a.C, abA, bcA, caA, edgeNormalABOnA, edgeNormalBCOnA, inverseABALengthSquared, inverseBCALengthSquared, inverseCAALengthSquared, faceNormalALength, out distanceSquaredCandidate, out offsetCandidate); + // Select(ref distanceSquared, ref offset, distanceSquaredCandidate, offsetCandidate); + + // var distance = Vector.SquareRoot(distanceSquared); + // Vector3Wide.Scale(offset, Vector.One / distance, out localNormalCandidate); + // //Don't try to use distances that are so small that the resulting normal will be numerically bad. That's close enough to intersecting that the previous normals will handle it. + // GetDepthForNormal(a.A, a.B, a.C, bA, bB, bC, localNormalCandidate, out depthCandidate); + // depthCandidate = Vector.ConditionalSelect(Vector.GreaterThan(distance, new Vector(1e-7f)), depthCandidate, new Vector(float.MaxValue)); + // Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + //} + + ////A AB x * + //TestEdgeEdge(abA, abB, a.A, a.B, a.C, bA, bB, bC, out var depth, out var localNormal); + //TestEdgeEdge(abA, bcB, a.A, a.B, a.C, bA, bB, bC, out var depthCandidate, out var localNormalCandidate); + //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + //TestEdgeEdge(abA, caB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + + ////A BC x * + //TestEdgeEdge(bcA, abB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + //TestEdgeEdge(bcA, bcB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + //TestEdgeEdge(bcA, caB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + + ////A CA x * + //TestEdgeEdge(caA, abB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + //TestEdgeEdge(caA, bcB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + //TestEdgeEdge(caA, caB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); + //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + + ////Face normals + //Vector3Wide.CrossWithoutOverlap(abA, caA, out var faceNormalA); + //Vector3Wide.Length(faceNormalA, out var faceNormalALength); + //Vector3Wide.Scale(faceNormalA, Vector.One / faceNormalALength, out faceNormalA); + //GetDepthForNormal(a.A, a.B, a.C, bA, bB, bC, faceNormalA, out depthCandidate); + //Select(ref depth, ref localNormal, depthCandidate, faceNormalA); + //Vector3Wide.CrossWithoutOverlap(abB, caB, out var faceNormalB); + //Vector3Wide.Length(faceNormalB, out var faceNormalBLength); + //Vector3Wide.Scale(faceNormalB, Vector.One / faceNormalBLength, out faceNormalB); + //GetDepthForNormal(a.A, a.B, a.C, bA, bB, bC, faceNormalB, out var faceDepthB); + //Select(ref depth, ref localNormal, faceDepthB, faceNormalB); + + //var tryVertexNormals = Vector.BitwiseAnd(Vector.LessThan(depth, Vector.Zero), allowContacts); //Vector3Wide.LengthSquared(abA, out var abALengthSquared); //Vector3Wide.LengthSquared(abB, out var abBLengthSquared); From d277a80e671e517ad70c6fa4650f1e5d1b1f7558 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 22 Sep 2021 21:54:28 -0500 Subject: [PATCH 009/947] Variants purged. --- .../CollisionTasks/TrianglePairTester.cs | 389 +----------------- 1 file changed, 6 insertions(+), 383 deletions(-) diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs index 1ac242115..5df0a4540 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs @@ -43,191 +43,8 @@ static void TestEdgeEdge( depth = Vector.ConditionalSelect(Vector.LessThan(normalLength, new Vector(1e-10f)), new Vector(float.MaxValue), depth); } - - //[MethodImpl(MethodImplOptions.AggressiveInlining)] - static void TestEdgeEdge2( - in Vector3Wide edgeStartA, in Vector3Wide edgeOffsetA, in Vector edgeOffsetALengthSquared, in Vector inverseEdgeOffsetALengthSquared, - in Vector3Wide edgeStartB, in Vector3Wide edgeOffsetB, in Vector edgeOffsetBLengthSquared, in Vector inverseEdgeOffsetBLengthSquared, - in Vector3Wide aA, in Vector3Wide bA, in Vector3Wide cA, in Vector3Wide aB, in Vector3Wide bB, in Vector3Wide cB, - out Vector depth, out Vector3Wide normal) - { - Vector3Wide.Subtract(edgeStartA, edgeStartB, out var bStartToAStart); - Vector3Wide.Dot(edgeOffsetA, bStartToAStart, out var oADotAB); - Vector3Wide.Dot(edgeOffsetB, bStartToAStart, out var oBDotAB); - Vector3Wide.Dot(edgeOffsetA, edgeOffsetB, out var oADotOB); - var denominator = edgeOffsetALengthSquared * edgeOffsetBLengthSquared - oADotOB * oADotOB; //TODO: div 0 guard if edge offsets are parallel. - //Compute the first guess for tA and clamp it. - var tA = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (oADotOB * oBDotAB - oADotAB * edgeOffsetBLengthSquared) / denominator)); - //Compute the closest point on B to the first guess. - var tB = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (oADotOB * tA + oBDotAB) * inverseEdgeOffsetBLengthSquared)); - //Finally, compute the true tA. - tA = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (tB * oADotOB - oADotAB) * inverseEdgeOffsetALengthSquared)); - - Vector3Wide.Scale(edgeOffsetA, tA, out var startToClosestA); - Vector3Wide.Add(edgeStartA, startToClosestA, out var a); - Vector3Wide.Scale(edgeOffsetB, tB, out var startToClosestB); - Vector3Wide.Add(edgeStartB, startToClosestB, out var b); - - //If the segments touch, then fall back to using the edge direction as the normal candidate. - Vector3Wide.Subtract(a, b, out var ba); - Vector3Wide.LengthSquared(ba, out var baLengthSquared); - var useFallback = Vector.LessThan(baLengthSquared, new Vector(1e-12f)); - Vector3Wide.CrossWithoutOverlap(edgeOffsetA, edgeOffsetB, out var fallbackNormal); - Vector3Wide.ConditionalSelect(useFallback, fallbackNormal, ba, out normal); - Vector3Wide.Length(normal, out var normalLength); - Vector3Wide.Scale(normal, Vector.One / normalLength, out normal); - GetDepthForNormal(aA, bA, cA, aB, bB, cB, normal, out depth); - //Protect against bad normals. - depth = Vector.ConditionalSelect(Vector.LessThan(normalLength, new Vector(1e-10f)), new Vector(float.MaxValue), depth); - } - //[MethodImpl(MethodImplOptions.AggressiveInlining)] static void TestVertexNormal(in Vector3Wide vertex, - in Vector3Wide a, in Vector3Wide b, in Vector3Wide c, - in Vector3Wide opposingA, in Vector3Wide opposingB, in Vector3Wide opposingC, - in Vector3Wide opposingAB, in Vector3Wide opposingBC, in Vector3Wide opposingCA, - in Vector inverseLengthSquaredAB, in Vector inverseLengthSquaredBC, in Vector inverseLengthSquaredCA, - out Vector depth, out Vector3Wide normal) - { - //Project the vertex onto all three edges. Take the closest approach as a normal candidate. - //Note that this will try normals that cross over the triangle's face, but that's fine- it'll just be a crappy normal candidate and another option will be chosen instead. - Vector3Wide.Subtract(vertex, opposingA, out var av); - Vector3Wide.Subtract(vertex, opposingB, out var bv); - Vector3Wide.Subtract(vertex, opposingC, out var cv); - Vector3Wide.Dot(av, opposingAB, out var avDotAB); - Vector3Wide.Dot(bv, opposingBC, out var bvDotBC); - Vector3Wide.Dot(cv, opposingCA, out var cvDotCA); - var tAB = Vector.Max(Vector.Zero, Vector.Min(Vector.One, avDotAB * inverseLengthSquaredAB)); - var tBC = Vector.Max(Vector.Zero, Vector.Min(Vector.One, bvDotBC * inverseLengthSquaredBC)); - var tCA = Vector.Max(Vector.Zero, Vector.Min(Vector.One, cvDotCA * inverseLengthSquaredCA)); - Vector3Wide vToAB, vToBC, vToCA; - vToAB.X = opposingA.X + opposingAB.X * tAB - vertex.X; - vToAB.Y = opposingA.Y + opposingAB.Y * tAB - vertex.Y; - vToAB.Z = opposingA.Z + opposingAB.Z * tAB - vertex.Z; - vToBC.X = opposingB.X + opposingBC.X * tBC - vertex.X; - vToBC.Y = opposingB.Y + opposingBC.Y * tBC - vertex.Y; - vToBC.Z = opposingB.Z + opposingBC.Z * tBC - vertex.Z; - vToCA.X = opposingC.X + opposingCA.X * tCA - vertex.X; - vToCA.Y = opposingC.Y + opposingCA.Y * tCA - vertex.Y; - vToCA.Z = opposingC.Z + opposingCA.Z * tCA - vertex.Z; - Vector3Wide.LengthSquared(vToAB, out var abDistanceSquared); - Vector3Wide.LengthSquared(vToBC, out var bcDistanceSquared); - Vector3Wide.LengthSquared(vToCA, out var caDistanceSquared); - - var distanceSquared = Vector.Min(abDistanceSquared, Vector.Min(bcDistanceSquared, caDistanceSquared)); - Vector3Wide.ConditionalSelect(Vector.Equals(distanceSquared, abDistanceSquared), vToAB, vToCA, out normal); - Vector3Wide.ConditionalSelect(Vector.Equals(distanceSquared, bcDistanceSquared), vToBC, normal, out normal); - - Vector3Wide.Scale(normal, Vector.One / Vector.SquareRoot(distanceSquared), out normal); - GetDepthForNormal(a, b, c, opposingA, opposingB, opposingC, normal, out depth); - //Protect against bad normals. - depth = Vector.ConditionalSelect(Vector.LessThan(distanceSquared, new Vector(1e-12f)), new Vector(float.MaxValue), depth); - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - static void TestVertexNormal2(in Vector3Wide vertex, - in Vector3Wide opposingA, in Vector3Wide opposingB, in Vector3Wide opposingC, - in Vector3Wide edgePlaneNormalAB, in Vector3Wide edgePlaneNormalBC, in Vector3Wide edgePlaneNormalCA, - in Vector3Wide triangleNormal, in Vector inverseTriangleNormalLength, - in Vector inverseLengthSquaredAB, in Vector inverseLengthSquaredBC, in Vector inverseLengthSquaredCA) - { - Vector3Wide.Subtract(vertex, opposingA, out var av); - Vector3Wide.Subtract(vertex, opposingB, out var bv); - Vector3Wide.Subtract(vertex, opposingC, out var cv); - Vector3Wide.Dot(av, edgePlaneNormalAB, out var avDotPlaneAB); - Vector3Wide.Dot(bv, edgePlaneNormalBC, out var bvDotPlaneBC); - Vector3Wide.Dot(cv, edgePlaneNormalCA, out var cvDotPlaneCA); - //Push the vertex down to the triangle plane. - Vector3Wide.Dot(av, triangleNormal, out var flattenDot); - Vector3Wide vertexToTrianglePlane; - vertexToTrianglePlane.X = flattenDot * triangleNormal.X; - vertexToTrianglePlane.Y = flattenDot * triangleNormal.Y; - vertexToTrianglePlane.Z = flattenDot * triangleNormal.Z; - //Edge plane normals should have the same magnitude as the edge lengths, since they're just rotated 90 degrees. Triangle normal is perfectly perpendicular to the edges by construction. - var tAB = avDotPlaneAB * inverseLengthSquaredAB; - var tBC = bvDotPlaneBC * inverseLengthSquaredBC; - var tCA = cvDotPlaneCA * inverseLengthSquaredCA; - Vector3Wide toAB, toBC, toCA; - toAB.X = vertexToTrianglePlane.X - edgePlaneNormalAB.X * tAB; - toAB.Y = vertexToTrianglePlane.Y - edgePlaneNormalAB.Y * tAB; - toAB.Z = vertexToTrianglePlane.Z - edgePlaneNormalAB.Z * tAB; - toBC.X = vertexToTrianglePlane.X - edgePlaneNormalBC.X * tBC; - toBC.Y = vertexToTrianglePlane.Y - edgePlaneNormalBC.Y * tBC; - toBC.Z = vertexToTrianglePlane.Z - edgePlaneNormalBC.Z * tBC; - toCA.X = vertexToTrianglePlane.X - edgePlaneNormalCA.X * tCA; - toCA.Y = vertexToTrianglePlane.Y - edgePlaneNormalCA.Y * tCA; - toCA.Z = vertexToTrianglePlane.Z - edgePlaneNormalCA.Z * tCA; - - //If the vertex is outside one edge plane, use the vertex projected onto that edge. - //If the vertex is outside two edge planes, it is on the shared vertex. - var outsideAB = Vector.GreaterThanOrEqual(avDotPlaneAB, Vector.Zero); - var outsideBC = Vector.GreaterThanOrEqual(bvDotPlaneBC, Vector.Zero); - var outsideCA = Vector.GreaterThanOrEqual(cvDotPlaneCA, Vector.Zero); - var outsideA = Vector.BitwiseAnd(outsideAB, outsideCA); - var outsideB = Vector.BitwiseAnd(outsideAB, outsideBC); - var outsideC = Vector.BitwiseAnd(outsideBC, outsideCA); - - //Vector3Wide.ConditionalSelect - - //var wa = bvDotPlaneBC * inverseTriangleNormalLength; - //var wb = cvDotPlaneCA * inverseTriangleNormalLength; - //var wc = Vector.One - wa - wb; - ////dot(ab x N, av) = dot(av x ab, N) - ////barycentricCoordinate = dot(ab x N * ||ab x ca||, av) / ||ab x ca||^2 = dot(ab x N, av) / ||ab x ca|| - //avDotPlaneAB = Vector.Max(Vector.Zero, Vector.Min(Vector.One, avDotPlaneAB * inverseLengthSquaredAB)); - //bvDotPlaneBC = Vector.Max(Vector.Zero, Vector.Min(Vector.One, bvDotPlaneBC * inverseLengthSquaredBC)); - //cvDotPlaneCA = Vector.Max(Vector.Zero, Vector.Min(Vector.One, cvDotPlaneCA * inverseLengthSquaredCA)); - - - } - - //[MethodImpl(MethodImplOptions.AggressiveInlining)] - static void TestVertexNormal3(in Vector3Wide vertex, - in Vector3Wide opposingA, in Vector3Wide opposingB, in Vector3Wide opposingC, - in Vector3Wide opposingAB, in Vector3Wide opposingBC, in Vector3Wide opposingCA, - in Vector3Wide opposingEdgePlaneAB, in Vector3Wide opposingEdgePlaneBC, - in Vector inverseLengthSquaredAB, in Vector inverseLengthSquaredBC, in Vector inverseLengthSquaredCA, in Vector triangleNormalLength, - out Vector distanceSquared, out Vector3Wide offset) - { - //Project the vertex onto all three edges. Take the closest approach as a normal candidate. - //Note that this will try normals that cross over the triangle's face, but that's fine- it'll just be a crappy normal candidate and another option will be chosen instead. - Vector3Wide.Subtract(vertex, opposingA, out var av); - Vector3Wide.Subtract(vertex, opposingB, out var bv); - Vector3Wide.Subtract(vertex, opposingC, out var cv); - Vector3Wide.Dot(av, opposingEdgePlaneAB, out var avDotEdgePlaneAB); - Vector3Wide.Dot(bv, opposingEdgePlaneBC, out var bvDotEdgePlaneBC); - //Because of the equivalence between the edge plane tests and barycentric coordinates, we can compute the last edge plane dot implicitly: - var cvDotEdgePlaneCA = triangleNormalLength - avDotEdgePlaneAB - bvDotEdgePlaneBC; - var outsideTriangle = Vector.BitwiseOr(Vector.GreaterThan(avDotEdgePlaneAB, Vector.Zero), Vector.BitwiseOr(Vector.GreaterThan(bvDotEdgePlaneBC, Vector.Zero), Vector.GreaterThan(cvDotEdgePlaneCA, Vector.Zero))); - Vector3Wide.Dot(av, opposingAB, out var avDotAB); - Vector3Wide.Dot(bv, opposingBC, out var bvDotBC); - Vector3Wide.Dot(cv, opposingCA, out var cvDotCA); - var tAB = Vector.Max(Vector.Zero, Vector.Min(Vector.One, avDotAB * inverseLengthSquaredAB)); - var tBC = Vector.Max(Vector.Zero, Vector.Min(Vector.One, bvDotBC * inverseLengthSquaredBC)); - var tCA = Vector.Max(Vector.Zero, Vector.Min(Vector.One, cvDotCA * inverseLengthSquaredCA)); - Vector3Wide vToAB, vToBC, vToCA; - vToAB.X = opposingA.X + opposingAB.X * tAB - vertex.X; - vToAB.Y = opposingA.Y + opposingAB.Y * tAB - vertex.Y; - vToAB.Z = opposingA.Z + opposingAB.Z * tAB - vertex.Z; - vToBC.X = opposingB.X + opposingBC.X * tBC - vertex.X; - vToBC.Y = opposingB.Y + opposingBC.Y * tBC - vertex.Y; - vToBC.Z = opposingB.Z + opposingBC.Z * tBC - vertex.Z; - vToCA.X = opposingC.X + opposingCA.X * tCA - vertex.X; - vToCA.Y = opposingC.Y + opposingCA.Y * tCA - vertex.Y; - vToCA.Z = opposingC.Z + opposingCA.Z * tCA - vertex.Z; - Vector3Wide.LengthSquared(vToAB, out var abDistanceSquared); - Vector3Wide.LengthSquared(vToBC, out var bcDistanceSquared); - Vector3Wide.LengthSquared(vToCA, out var caDistanceSquared); - - distanceSquared = Vector.Min(abDistanceSquared, Vector.Min(bcDistanceSquared, caDistanceSquared)); - Vector3Wide.ConditionalSelect(Vector.Equals(distanceSquared, abDistanceSquared), vToAB, vToCA, out offset); - Vector3Wide.ConditionalSelect(Vector.Equals(distanceSquared, bcDistanceSquared), vToBC, offset, out offset); - - //Ignore this vertex if it's contained within the triangle. We'll detect distances that are too close to generate normals outside. - distanceSquared = Vector.ConditionalSelect(outsideTriangle, distanceSquared, new Vector(float.MaxValue)); - } - - //[MethodImpl(MethodImplOptions.AggressiveInlining)] - static void TestVertexNormal4(in Vector3Wide vertex, in Vector3Wide opposingA, in Vector3Wide opposingB, in Vector3Wide opposingC, in Vector3Wide opposingAB, in Vector3Wide opposingBC, in Vector3Wide opposingCA, in Vector3Wide opposingEdgePlaneAB, in Vector3Wide opposingEdgePlaneBC, @@ -542,12 +359,12 @@ public unsafe void Test( Vector3Wide.Cross(bcB, faceNormalB, out var edgeNormalBCOnB); var distanceSquared = new Vector(float.MaxValue); Unsafe.SkipInit(out Vector3Wide offset); - TestVertexNormal4(a.A, bA, bB, bC, abB, bcB, caB, edgeNormalABOnB, edgeNormalBCOnB, inverseABBLengthSquared, inverseBCBLengthSquared, inverseCABLengthSquared, faceNormalBLength, ref distanceSquared, ref offset); - TestVertexNormal4(a.B, bA, bB, bC, abB, bcB, caB, edgeNormalABOnB, edgeNormalBCOnB, inverseABBLengthSquared, inverseBCBLengthSquared, inverseCABLengthSquared, faceNormalBLength, ref distanceSquared, ref offset); - TestVertexNormal4(a.C, bA, bB, bC, abB, bcB, caB, edgeNormalABOnB, edgeNormalBCOnB, inverseABBLengthSquared, inverseBCBLengthSquared, inverseCABLengthSquared, faceNormalBLength, ref distanceSquared, ref offset); - TestVertexNormal4(bA, a.A, a.B, a.C, abA, bcA, caA, edgeNormalABOnA, edgeNormalBCOnA, inverseABALengthSquared, inverseBCALengthSquared, inverseCAALengthSquared, faceNormalALength, ref distanceSquared, ref offset); - TestVertexNormal4(bB, a.A, a.B, a.C, abA, bcA, caA, edgeNormalABOnA, edgeNormalBCOnA, inverseABALengthSquared, inverseBCALengthSquared, inverseCAALengthSquared, faceNormalALength, ref distanceSquared, ref offset); - TestVertexNormal4(bC, a.A, a.B, a.C, abA, bcA, caA, edgeNormalABOnA, edgeNormalBCOnA, inverseABALengthSquared, inverseBCALengthSquared, inverseCAALengthSquared, faceNormalALength, ref distanceSquared, ref offset); + TestVertexNormal(a.A, bA, bB, bC, abB, bcB, caB, edgeNormalABOnB, edgeNormalBCOnB, inverseABBLengthSquared, inverseBCBLengthSquared, inverseCABLengthSquared, faceNormalBLength, ref distanceSquared, ref offset); + TestVertexNormal(a.B, bA, bB, bC, abB, bcB, caB, edgeNormalABOnB, edgeNormalBCOnB, inverseABBLengthSquared, inverseBCBLengthSquared, inverseCABLengthSquared, faceNormalBLength, ref distanceSquared, ref offset); + TestVertexNormal(a.C, bA, bB, bC, abB, bcB, caB, edgeNormalABOnB, edgeNormalBCOnB, inverseABBLengthSquared, inverseBCBLengthSquared, inverseCABLengthSquared, faceNormalBLength, ref distanceSquared, ref offset); + TestVertexNormal(bA, a.A, a.B, a.C, abA, bcA, caA, edgeNormalABOnA, edgeNormalBCOnA, inverseABALengthSquared, inverseBCALengthSquared, inverseCAALengthSquared, faceNormalALength, ref distanceSquared, ref offset); + TestVertexNormal(bB, a.A, a.B, a.C, abA, bcA, caA, edgeNormalABOnA, edgeNormalBCOnA, inverseABALengthSquared, inverseBCALengthSquared, inverseCAALengthSquared, faceNormalALength, ref distanceSquared, ref offset); + TestVertexNormal(bC, a.A, a.B, a.C, abA, bcA, caA, edgeNormalABOnA, edgeNormalBCOnA, inverseABALengthSquared, inverseBCALengthSquared, inverseCAALengthSquared, faceNormalALength, ref distanceSquared, ref offset); var distance = Vector.SquareRoot(distanceSquared); Vector3Wide.Scale(offset, Vector.One / distance, out localNormalCandidate); @@ -557,200 +374,6 @@ public unsafe void Test( Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); } - ////A AB x * - //TestEdgeEdge(abA, abB, a.A, a.B, a.C, bA, bB, bC, out var depth, out var localNormal); - //TestEdgeEdge(abA, bcB, a.A, a.B, a.C, bA, bB, bC, out var depthCandidate, out var localNormalCandidate); - //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - //TestEdgeEdge(abA, caB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); - //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - - ////A BC x * - //TestEdgeEdge(bcA, abB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); - //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - //TestEdgeEdge(bcA, bcB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); - //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - //TestEdgeEdge(bcA, caB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); - //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - - ////A CA x * - //TestEdgeEdge(caA, abB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); - //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - //TestEdgeEdge(caA, bcB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); - //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - //TestEdgeEdge(caA, caB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); - //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - - ////Face normals - //Vector3Wide.CrossWithoutOverlap(abA, caA, out var faceNormalA); - //Vector3Wide.Length(faceNormalA, out var faceNormalALength); - //Vector3Wide.Scale(faceNormalA, Vector.One / faceNormalALength, out faceNormalA); - //GetDepthForNormal(a.A, a.B, a.C, bA, bB, bC, faceNormalA, out depthCandidate); - //Select(ref depth, ref localNormal, depthCandidate, faceNormalA); - //Vector3Wide.CrossWithoutOverlap(abB, caB, out var faceNormalB); - //Vector3Wide.Length(faceNormalB, out var faceNormalBLength); - //Vector3Wide.Scale(faceNormalB, Vector.One / faceNormalBLength, out faceNormalB); - //GetDepthForNormal(a.A, a.B, a.C, bA, bB, bC, faceNormalB, out var faceDepthB); - //Select(ref depth, ref localNormal, faceDepthB, faceNormalB); - - - //var tryVertexNormals = Vector.BitwiseAnd(Vector.LessThan(depth, Vector.Zero), allowContacts); - //Vector3Wide.LengthSquared(abA, out var abALengthSquared); - //Vector3Wide.LengthSquared(abB, out var abBLengthSquared); - //Vector3Wide.LengthSquared(caA, out var caALengthSquared); - //Vector3Wide.LengthSquared(caB, out var caBLengthSquared); - //if (Vector.LessThanAny(tryVertexNormals, Vector.Zero)) - //{ - // //Vertex normals are not required to determine penetration versus separation. They are only used to ensure correct separated speculative normals. - // //This isn't strictly required for behavior in the general case, but MeshReduction depends on it. - // Vector3Wide.LengthSquared(bcA, out var bcALengthSquared); - // Vector3Wide.LengthSquared(bcB, out var bcBLengthSquared); - // var inverseABALengthSquared = Vector.One / abALengthSquared; - // var inverseBCALengthSquared = Vector.One / bcALengthSquared; - // var inverseCAALengthSquared = Vector.One / caALengthSquared; - // var inverseABBLengthSquared = Vector.One / abBLengthSquared; - // var inverseBCBLengthSquared = Vector.One / bcBLengthSquared; - // var inverseCABLengthSquared = Vector.One / caBLengthSquared; - // Vector3Wide.Cross(abA, faceNormalA, out var edgeNormalABOnA); - // Vector3Wide.Cross(bcA, faceNormalA, out var edgeNormalBCOnA); - // Vector3Wide.Cross(abB, faceNormalB, out var edgeNormalABOnB); - // Vector3Wide.Cross(bcB, faceNormalB, out var edgeNormalBCOnB); - // var distanceSquared = new Vector(float.MaxValue); - // Unsafe.SkipInit(out Vector3Wide offset); - // TestVertexNormal3(a.A, bA, bB, bC, abB, bcB, caB, edgeNormalABOnB, edgeNormalBCOnB, inverseABBLengthSquared, inverseBCBLengthSquared, inverseCABLengthSquared, faceNormalBLength, ref distanceSquared, ref offset); - // //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - // TestVertexNormal3(a.B, bA, bB, bC, abB, bcB, caB, edgeNormalABOnB, edgeNormalBCOnB, inverseABBLengthSquared, inverseBCBLengthSquared, inverseCABLengthSquared, faceNormalBLength, ref distanceSquaredCandidate, out var offsetCandidate); - // Select(ref distanceSquared, ref offset, distanceSquaredCandidate, offsetCandidate); - // TestVertexNormal3(a.C, bA, bB, bC, abB, bcB, caB, edgeNormalABOnB, edgeNormalBCOnB, inverseABBLengthSquared, inverseBCBLengthSquared, inverseCABLengthSquared, faceNormalBLength, out distanceSquaredCandidate, out offsetCandidate); - // Select(ref distanceSquared, ref offset, distanceSquaredCandidate, offsetCandidate); - // TestVertexNormal3(bA, a.A, a.B, a.C, abA, bcA, caA, edgeNormalABOnA, edgeNormalBCOnA, inverseABALengthSquared, inverseBCALengthSquared, inverseCAALengthSquared, faceNormalALength, out distanceSquaredCandidate, out offsetCandidate); - // Select(ref distanceSquared, ref offset, distanceSquaredCandidate, offsetCandidate); - // TestVertexNormal3(bB, a.A, a.B, a.C, abA, bcA, caA, edgeNormalABOnA, edgeNormalBCOnA, inverseABALengthSquared, inverseBCALengthSquared, inverseCAALengthSquared, faceNormalALength, out distanceSquaredCandidate, out offsetCandidate); - // Select(ref distanceSquared, ref offset, distanceSquaredCandidate, offsetCandidate); - // TestVertexNormal3(bC, a.A, a.B, a.C, abA, bcA, caA, edgeNormalABOnA, edgeNormalBCOnA, inverseABALengthSquared, inverseBCALengthSquared, inverseCAALengthSquared, faceNormalALength, out distanceSquaredCandidate, out offsetCandidate); - // Select(ref distanceSquared, ref offset, distanceSquaredCandidate, offsetCandidate); - - // var distance = Vector.SquareRoot(distanceSquared); - // Vector3Wide.Scale(offset, Vector.One / distance, out localNormalCandidate); - // //Don't try to use distances that are so small that the resulting normal will be numerically bad. That's close enough to intersecting that the previous normals will handle it. - // GetDepthForNormal(a.A, a.B, a.C, bA, bB, bC, localNormalCandidate, out depthCandidate); - // depthCandidate = Vector.ConditionalSelect(Vector.GreaterThan(distance, new Vector(1e-7f)), depthCandidate, new Vector(float.MaxValue)); - // Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - //} - - ////A AB x * - //TestEdgeEdge(abA, abB, a.A, a.B, a.C, bA, bB, bC, out var depth, out var localNormal); - //TestEdgeEdge(abA, bcB, a.A, a.B, a.C, bA, bB, bC, out var depthCandidate, out var localNormalCandidate); - //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - //TestEdgeEdge(abA, caB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); - //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - - ////A BC x * - //TestEdgeEdge(bcA, abB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); - //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - //TestEdgeEdge(bcA, bcB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); - //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - //TestEdgeEdge(bcA, caB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); - //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - - ////A CA x * - //TestEdgeEdge(caA, abB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); - //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - //TestEdgeEdge(caA, bcB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); - //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - //TestEdgeEdge(caA, caB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); - //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - - ////Face normals - //Vector3Wide.CrossWithoutOverlap(abA, caA, out var faceNormalA); - //Vector3Wide.Length(faceNormalA, out var faceNormalALength); - //Vector3Wide.Scale(faceNormalA, Vector.One / faceNormalALength, out faceNormalA); - //GetDepthForNormal(a.A, a.B, a.C, bA, bB, bC, faceNormalA, out depthCandidate); - //Select(ref depth, ref localNormal, depthCandidate, faceNormalA); - //Vector3Wide.CrossWithoutOverlap(abB, caB, out var faceNormalB); - //Vector3Wide.Length(faceNormalB, out var faceNormalBLength); - //Vector3Wide.Scale(faceNormalB, Vector.One / faceNormalBLength, out faceNormalB); - //GetDepthForNormal(a.A, a.B, a.C, bA, bB, bC, faceNormalB, out var faceDepthB); - //Select(ref depth, ref localNormal, faceDepthB, faceNormalB); - - - //var tryVertexNormals = Vector.BitwiseAnd(Vector.LessThan(depth, Vector.Zero), allowContacts); - //Vector3Wide.LengthSquared(abA, out var abALengthSquared); - //Vector3Wide.LengthSquared(abB, out var abBLengthSquared); - //Vector3Wide.LengthSquared(caA, out var caALengthSquared); - //Vector3Wide.LengthSquared(caB, out var caBLengthSquared); - //if (Vector.LessThanAny(tryVertexNormals, Vector.Zero)) - //{ - // //Vertex normals are not required to determine penetration versus separation. They are only used to ensure correct separated speculative normals. - // //This isn't strictly required for behavior in the general case, but MeshReduction depends on it. - // Vector3Wide.LengthSquared(bcA, out var bcALengthSquared); - // Vector3Wide.LengthSquared(bcB, out var bcBLengthSquared); - // var inverseABALengthSquared = Vector.One / abALengthSquared; - // var inverseBCALengthSquared = Vector.One / bcALengthSquared; - // var inverseCAALengthSquared = Vector.One / caALengthSquared; - // var inverseABBLengthSquared = Vector.One / abBLengthSquared; - // var inverseBCBLengthSquared = Vector.One / bcBLengthSquared; - // var inverseCABLengthSquared = Vector.One / caBLengthSquared; - // TestVertexNormal(a.A, a.A, a.B, a.C, bA, bB, bC, abB, bcB, caB, inverseABBLengthSquared, inverseBCBLengthSquared, inverseCABLengthSquared, out depthCandidate, out localNormalCandidate); - // Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - // TestVertexNormal(a.B, a.A, a.B, a.C, bA, bB, bC, abB, bcB, caB, inverseABBLengthSquared, inverseBCBLengthSquared, inverseCABLengthSquared, out depthCandidate, out localNormalCandidate); - // Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - // TestVertexNormal(a.C, a.A, a.B, a.C, bA, bB, bC, abB, bcB, caB, inverseABBLengthSquared, inverseBCBLengthSquared, inverseCABLengthSquared, out depthCandidate, out localNormalCandidate); - // Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - // TestVertexNormal(bA, bA, bB, bC, a.A, a.B, a.C, abA, bcA, caA, inverseABALengthSquared, inverseBCALengthSquared, inverseCAALengthSquared, out depthCandidate, out localNormalCandidate); - // Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - // TestVertexNormal(bB, bA, bB, bC, a.A, a.B, a.C, abA, bcA, caA, inverseABALengthSquared, inverseBCALengthSquared, inverseCAALengthSquared, out depthCandidate, out localNormalCandidate); - // Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - // TestVertexNormal(bC, bA, bB, bC, a.A, a.B, a.C, abA, bcA, caA, inverseABALengthSquared, inverseBCALengthSquared, inverseCAALengthSquared, out depthCandidate, out localNormalCandidate); - // Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - //} - - //Vector3Wide.LengthSquared(abA, out var abALengthSquared); - //Vector3Wide.LengthSquared(abB, out var abBLengthSquared); - //Vector3Wide.LengthSquared(caA, out var caALengthSquared); - //Vector3Wide.LengthSquared(caB, out var caBLengthSquared); - //Vector3Wide.LengthSquared(bcA, out var bcALengthSquared); - //Vector3Wide.LengthSquared(bcB, out var bcBLengthSquared); - //var inverseABALengthSquared = Vector.One / abALengthSquared; - //var inverseBCALengthSquared = Vector.One / bcALengthSquared; - //var inverseCAALengthSquared = Vector.One / caALengthSquared; - //var inverseABBLengthSquared = Vector.One / abBLengthSquared; - //var inverseBCBLengthSquared = Vector.One / bcBLengthSquared; - //var inverseCABLengthSquared = Vector.One / caBLengthSquared; - ////A AB x * - //TestEdgeEdge2(a.A, abA, abALengthSquared, inverseABALengthSquared, bA, abB, abBLengthSquared, inverseABBLengthSquared, a.A, a.B, a.C, bA, bB, bC, out var depth, out var localNormal); - //TestEdgeEdge2(a.A, abA, abALengthSquared, inverseABALengthSquared, bB, bcB, bcBLengthSquared, inverseBCBLengthSquared, a.A, a.B, a.C, bA, bB, bC, out var depthCandidate, out var localNormalCandidate); - //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - //TestEdgeEdge2(a.A, abA, abALengthSquared, inverseABALengthSquared, bC, caB, caBLengthSquared, inverseCABLengthSquared, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); - //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - - ////A BC x * - //TestEdgeEdge2(a.B, bcA, bcALengthSquared, inverseBCALengthSquared, bA, abB, abBLengthSquared, inverseABBLengthSquared, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); - //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - //TestEdgeEdge2(a.B, bcA, bcALengthSquared, inverseBCALengthSquared, bB, bcB, bcBLengthSquared, inverseBCBLengthSquared, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); - //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - //TestEdgeEdge2(a.B, bcA, bcALengthSquared, inverseBCALengthSquared, bC, caB, caBLengthSquared, inverseCABLengthSquared, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); - //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - - ////A CA x * - //TestEdgeEdge2(a.C, caA, caALengthSquared, inverseCAALengthSquared, bA, abB, abBLengthSquared, inverseABBLengthSquared, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); - //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - //TestEdgeEdge2(a.C, caA, caALengthSquared, inverseCAALengthSquared, bB, bcB, bcBLengthSquared, inverseBCBLengthSquared, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); - //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - //TestEdgeEdge2(a.C, caA, caALengthSquared, inverseCAALengthSquared, bC, caB, caBLengthSquared, inverseCABLengthSquared, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate); - //Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - - ////Face normals - //Vector3Wide.CrossWithoutOverlap(abA, caA, out var faceNormalA); - //Vector3Wide.Length(faceNormalA, out var faceNormalALength); - //Vector3Wide.Scale(faceNormalA, Vector.One / faceNormalALength, out faceNormalA); - //GetDepthForNormal(a.A, a.B, a.C, bA, bB, bC, faceNormalA, out depthCandidate); - //Select(ref depth, ref localNormal, depthCandidate, faceNormalA); - //Vector3Wide.CrossWithoutOverlap(abB, caB, out var faceNormalB); - //Vector3Wide.Length(faceNormalB, out var faceNormalBLength); - //Vector3Wide.Scale(faceNormalB, Vector.One / faceNormalBLength, out faceNormalB); - //GetDepthForNormal(a.A, a.B, a.C, bA, bB, bC, faceNormalB, out var faceDepthB); - //Select(ref depth, ref localNormal, faceDepthB, faceNormalB); - //Point the normal from B to A by convention. Vector3Wide.Subtract(localTriangleCenterB, localTriangleCenterA, out var centerAToCenterB); Vector3Wide.Dot(localNormal, centerAToCenterB, out var calibrationDot); From bdbea9625ee5b553bd94daa0f7a8daf7cc36c1f7 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 23 Sep 2021 17:16:34 -0500 Subject: [PATCH 010/947] Box-triangle first pass vertex normals. --- .../CollisionTasks/BoxTriangleTester.cs | 241 +++++++++++++++++- 1 file changed, 240 insertions(+), 1 deletion(-) diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/BoxTriangleTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/BoxTriangleTester.cs index 3c1af2f06..aecb594be 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/BoxTriangleTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/BoxTriangleTester.cs @@ -333,9 +333,248 @@ public unsafe void Test( Vector.Abs(triangleNormal.X) * a.HalfWidth + Vector.Abs(triangleNormal.Y) * a.HalfHeight + Vector.Abs(triangleNormal.Z) * a.HalfLength - Vector.Abs(trianglePlaneOffset); Select(ref depth, ref localNormal, triangleFaceDepth, calibratedTriangleNormal); + ManifoldCandidateHelper.CreateActiveMask(pairCount, out var activeLanes); + var testVertexNormals = Vector.BitwiseAnd(activeLanes, Vector.LessThan(depth, Vector.Zero)); + if (Vector.LessThanAny(testVertexNormals, Vector.Zero)) + { + //At least one lane contains a separating pair. Mesh reduction relies on separating normals being minimal (or very very close to it), so test 7 candidate normals. + //First examine the 3 triangle vertices. + var negativeHalfWidth = -a.HalfWidth; + var negativeHalfHeight = -a.HalfHeight; + var negativeHalfLength = -a.HalfLength; + var boxToAX = vA.X - Vector.Min(a.HalfWidth, Vector.Max(negativeHalfWidth, vA.X)); + var boxToAY = vA.Y - Vector.Min(a.HalfHeight, Vector.Max(negativeHalfHeight, vA.Y)); + var boxToAZ = vA.Z - Vector.Min(a.HalfLength, Vector.Max(negativeHalfLength, vA.Z)); + var boxToBX = vB.X - Vector.Min(a.HalfWidth, Vector.Max(negativeHalfWidth, vB.X)); + var boxToBY = vB.Y - Vector.Min(a.HalfHeight, Vector.Max(negativeHalfHeight, vB.Y)); + var boxToBZ = vB.Z - Vector.Min(a.HalfLength, Vector.Max(negativeHalfLength, vB.Z)); + var boxToCX = vC.X - Vector.Min(a.HalfWidth, Vector.Max(negativeHalfWidth, vC.X)); + var boxToCY = vC.Y - Vector.Min(a.HalfHeight, Vector.Max(negativeHalfHeight, vC.Y)); + var boxToCZ = vC.Z - Vector.Min(a.HalfLength, Vector.Max(negativeHalfLength, vC.Z)); + + var distanceSquaredA = boxToAX * boxToAX + boxToAY * boxToAY + boxToAZ * boxToAZ; + var distanceSquaredB = boxToBX * boxToBX + boxToBY * boxToBY + boxToBZ * boxToBZ; + var distanceSquaredC = boxToCX * boxToCX + boxToCY * boxToCY + boxToCZ * boxToCZ; + var distanceSquared = Vector.Min(distanceSquaredA, Vector.Min(distanceSquaredB, distanceSquaredC)); + var useA = Vector.Equals(distanceSquared, distanceSquaredA); + var useB = Vector.Equals(distanceSquared, distanceSquaredB); + var offsetX = Vector.ConditionalSelect(useA, boxToAX, Vector.ConditionalSelect(useB, boxToBX, boxToCX)); + var offsetY = Vector.ConditionalSelect(useA, boxToAY, Vector.ConditionalSelect(useB, boxToBY, boxToCY)); + var offsetZ = Vector.ConditionalSelect(useA, boxToAZ, Vector.ConditionalSelect(useB, boxToBZ, boxToCZ)); + + //Now examine the 4 box vertices from the best box face. + //The box face is spanned by two edge directions which are aligned with the working space's axes. + //To check a box vertex for containment: + //dot(edgePlaneNormalAB, boxVertex - vA), which is proportional to barycentric weight, scaled up by the triangle normal length. + //That is, barycentricWeightC = dot(edgePlaneNormalAB, boxVertex - vA) / triangleNormalLength. + //To detect containment, we can compute two barycentric weights and derive the third. If all are positive, then the box vertex is contained and should not contribute. + //(It would be redundant with the triangle face depth result, so there's no reason to go through the effort to reconstruct the exact point on the triangle's surface.) + //Note that: boxVertex = faceOffset +- boxEdgeOffsetX +- boxEdgeOffsetY + //So we can split the above barycentric weight calculation into pieces. Leaving it scaled for succinctness: + //scaledBarycentricWeightC = dot(edgePlaneNormalAB, boxVertex) - dot(edgePlaneNormalAB, vA) + //scaledBarycentricWeightC = dot(edgePlaneNormalAB, faceOffset) +- dot(edgePlaneNormalAB, boxEdgeOffsetX) +- dot(edgePlaneNormalAB, boxEdgeOffsetY) - dot(edgePlaneNormalAB, vA) + //So we can share quite a few operations across the 4 box vertices. + + //This gives us containment, but does not provide the closest point on the triangle directly. For simplicity, we'll just compute the closest point on each edge directly: + //tClosestPointOnAB = clamp(dot(edgeOffsetAB, boxVertex - vA) / ||edgeOffsetAB||^2, 0, 1) + // = clamp((dot(edgeOffsetAB, faceOffset) +- dot(edgeOffsetAB, boxEdgeOffsetX) +- dot(edgeOffsetAB, boxEdgeOffsetY) - dot(edgeOffsetAB, vA)) / ||edgeOffsetAB||^2, 0, 1) + + var absNormalX = Vector.Abs(localNormal.X); + var absNormalY = Vector.Abs(localNormal.Y); + var absNormalZ = Vector.Abs(localNormal.Z); + var useFaceX = Vector.BitwiseAnd(Vector.GreaterThan(absNormalX, absNormalY), Vector.GreaterThan(absNormalX, absNormalZ)); + var useFaceY = Vector.AndNot(Vector.GreaterThan(absNormalY, absNormalZ), useFaceX); + var faceLocalNormalComponent = Vector.ConditionalSelect(useFaceX, localNormal.X, Vector.ConditionalSelect(useFaceY, localNormal.Y, localNormal.Z)); + var faceOffsetMagnitude = Vector.ConditionalSelect(useFaceX, a.HalfWidth, Vector.ConditionalSelect(useFaceY, a.HalfHeight, a.HalfLength)); + var xOffset = Vector.ConditionalSelect(useFaceX, a.HalfHeight, Vector.ConditionalSelect(useFaceY, a.HalfLength, a.HalfWidth)); + var yOffset = Vector.ConditionalSelect(useFaceX, a.HalfLength, Vector.ConditionalSelect(useFaceY, a.HalfWidth, a.HalfHeight)); + var faceOffset = Vector.ConditionalSelect(Vector.LessThan(faceLocalNormalComponent, Vector.Zero), faceOffsetMagnitude, -faceOffsetMagnitude); + + //Thanks to axis alignment, all the box component dot products squish down to component selections. + var edgeABDotFaceOffset = faceOffset * Vector.ConditionalSelect(useFaceX, ab.X, Vector.ConditionalSelect(useFaceY, ab.Y, ab.Z)); + var edgeBCDotFaceOffset = faceOffset * Vector.ConditionalSelect(useFaceX, bc.X, Vector.ConditionalSelect(useFaceY, bc.Y, bc.Z)); + var edgeCADotFaceOffset = faceOffset * Vector.ConditionalSelect(useFaceX, ca.X, Vector.ConditionalSelect(useFaceY, ca.Y, ca.Z)); + var edgeABDotBoxEdgeX = xOffset * Vector.ConditionalSelect(useFaceX, ab.Y, Vector.ConditionalSelect(useFaceY, ab.Z, ab.X)); + var edgeBCDotBoxEdgeX = xOffset * Vector.ConditionalSelect(useFaceX, bc.Y, Vector.ConditionalSelect(useFaceY, bc.Z, bc.X)); + var edgeCADotBoxEdgeX = xOffset * Vector.ConditionalSelect(useFaceX, ca.Y, Vector.ConditionalSelect(useFaceY, ca.Z, ca.X)); + var edgeABDotBoxEdgeY = yOffset * Vector.ConditionalSelect(useFaceX, ab.Z, Vector.ConditionalSelect(useFaceY, ab.X, ab.Y)); + var edgeBCDotBoxEdgeY = yOffset * Vector.ConditionalSelect(useFaceX, bc.Z, Vector.ConditionalSelect(useFaceY, bc.X, bc.Y)); + var edgeCADotBoxEdgeY = yOffset * Vector.ConditionalSelect(useFaceX, ca.Z, Vector.ConditionalSelect(useFaceY, ca.X, ca.Y)); + Vector3Wide.Dot(ab, vA, out var abDotA); + Vector3Wide.Dot(bc, vB, out var bcDotB); + Vector3Wide.Dot(ca, vC, out var caDotC); + + var inverseLengthSquaredAB = Vector.One / (ab.X * ab.X + ab.Y * ab.Y + ab.Z * ab.Z); + var inverseLengthSquaredBC = Vector.One / (bc.X * bc.X + bc.Y * bc.Y + bc.Z * bc.Z); + var inverseLengthSquaredCA = Vector.One / (ca.X * ca.X + ca.Y * ca.Y + ca.Z * ca.Z); + var abToFaceDot = edgeABDotFaceOffset - abDotA; + var bcToFaceDot = edgeBCDotFaceOffset - bcDotB; + var caToFaceDot = edgeCADotFaceOffset - caDotC; + var tClosestOnAB00 = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (abToFaceDot - edgeABDotBoxEdgeX - edgeABDotBoxEdgeY) * inverseLengthSquaredAB)); + var tClosestOnAB01 = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (abToFaceDot - edgeABDotBoxEdgeX + edgeABDotBoxEdgeY) * inverseLengthSquaredAB)); + var tClosestOnAB10 = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (abToFaceDot + edgeABDotBoxEdgeX - edgeABDotBoxEdgeY) * inverseLengthSquaredAB)); + var tClosestOnAB11 = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (abToFaceDot + edgeABDotBoxEdgeX + edgeABDotBoxEdgeY) * inverseLengthSquaredAB)); + var tClosestOnBC00 = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (bcToFaceDot - edgeBCDotBoxEdgeX - edgeBCDotBoxEdgeY) * inverseLengthSquaredBC)); + var tClosestOnBC01 = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (bcToFaceDot - edgeBCDotBoxEdgeX + edgeBCDotBoxEdgeY) * inverseLengthSquaredBC)); + var tClosestOnBC10 = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (bcToFaceDot + edgeBCDotBoxEdgeX - edgeBCDotBoxEdgeY) * inverseLengthSquaredBC)); + var tClosestOnBC11 = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (bcToFaceDot + edgeBCDotBoxEdgeX + edgeBCDotBoxEdgeY) * inverseLengthSquaredBC)); + var tClosestOnCA00 = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (caToFaceDot - edgeCADotBoxEdgeX - edgeCADotBoxEdgeY) * inverseLengthSquaredCA)); + var tClosestOnCA01 = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (caToFaceDot - edgeCADotBoxEdgeX + edgeCADotBoxEdgeY) * inverseLengthSquaredCA)); + var tClosestOnCA10 = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (caToFaceDot + edgeCADotBoxEdgeX - edgeCADotBoxEdgeY) * inverseLengthSquaredCA)); + var tClosestOnCA11 = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (caToFaceDot + edgeCADotBoxEdgeX + edgeCADotBoxEdgeY) * inverseLengthSquaredCA)); + + //We now have the t value of every box vertex on each triangle edge. + //Find the closest pair. + //offsetAB00 = a + ab * t00 - (faceOffset - edgeOffsetX - edgeOffsetY) + var ab00X = vA.X + ab.X * tClosestOnAB00; + var ab00Y = vA.Y + ab.Y * tClosestOnAB00; + var ab00Z = vA.Z + ab.Z * tClosestOnAB00; + var ab01X = vA.X + ab.X * tClosestOnAB01; + var ab01Y = vA.Y + ab.Y * tClosestOnAB01; + var ab01Z = vA.Z + ab.Z * tClosestOnAB01; + var ab10X = vA.X + ab.X * tClosestOnAB10; + var ab10Y = vA.Y + ab.Y * tClosestOnAB10; + var ab10Z = vA.Z + ab.Z * tClosestOnAB10; + var ab11X = vA.X + ab.X * tClosestOnAB11; + var ab11Y = vA.Y + ab.Y * tClosestOnAB11; + var ab11Z = vA.Z + ab.Z * tClosestOnAB11; + ab00X = ab00X - Vector.Min(a.HalfWidth, Vector.Max(negativeHalfWidth, ab00X)); + ab00Y = ab00Y - Vector.Min(a.HalfHeight, Vector.Max(negativeHalfHeight, ab00Y)); + ab00Z = ab00Z - Vector.Min(a.HalfLength, Vector.Max(negativeHalfLength, ab00Z)); + ab01X = ab01X - Vector.Min(a.HalfWidth, Vector.Max(negativeHalfWidth, ab01X)); + ab01Y = ab01Y - Vector.Min(a.HalfHeight, Vector.Max(negativeHalfHeight, ab01Y)); + ab01Z = ab01Z - Vector.Min(a.HalfLength, Vector.Max(negativeHalfLength, ab01Z)); + ab10X = ab10X - Vector.Min(a.HalfWidth, Vector.Max(negativeHalfWidth, ab10X)); + ab10Y = ab10Y - Vector.Min(a.HalfHeight, Vector.Max(negativeHalfHeight, ab10Y)); + ab10Z = ab10Z - Vector.Min(a.HalfLength, Vector.Max(negativeHalfLength, ab10Z)); + ab11X = ab11X - Vector.Min(a.HalfWidth, Vector.Max(negativeHalfWidth, ab11X)); + ab11Y = ab11Y - Vector.Min(a.HalfHeight, Vector.Max(negativeHalfHeight, ab11Y)); + ab11Z = ab11Z - Vector.Min(a.HalfLength, Vector.Max(negativeHalfLength, ab11Z)); + var distanceSquaredAB00 = ab00X * ab00X + ab00Y * ab00Y + ab00Z * ab00Z; + var distanceSquaredAB01 = ab01X * ab01X + ab01Y * ab01Y + ab01Z * ab01Z; + var distanceSquaredAB10 = ab10X * ab10X + ab10Y * ab10Y + ab10Z * ab10Z; + var distanceSquaredAB11 = ab11X * ab11X + ab11Y * ab11Y + ab11Z * ab11Z; + distanceSquared = Vector.Min(distanceSquared, Vector.Min(Vector.Min(distanceSquaredAB00, distanceSquaredAB01), Vector.Min(distanceSquaredAB10, distanceSquaredAB11))); + var useAB00 = Vector.Equals(distanceSquared, distanceSquaredAB00); + var useAB01 = Vector.Equals(distanceSquared, distanceSquaredAB01); + var useAB10 = Vector.Equals(distanceSquared, distanceSquaredAB10); + var useAB11 = Vector.Equals(distanceSquared, distanceSquaredAB11); + offsetX = Vector.ConditionalSelect(useAB00, ab00X, Vector.ConditionalSelect(useAB01, ab01X, Vector.ConditionalSelect(useAB10, ab10X, Vector.ConditionalSelect(useAB11, ab11X, offsetX)))); + offsetY = Vector.ConditionalSelect(useAB00, ab00Y, Vector.ConditionalSelect(useAB01, ab01Y, Vector.ConditionalSelect(useAB10, ab10Y, Vector.ConditionalSelect(useAB11, ab11Y, offsetY)))); + offsetZ = Vector.ConditionalSelect(useAB00, ab00Z, Vector.ConditionalSelect(useAB01, ab01Z, Vector.ConditionalSelect(useAB10, ab10Z, Vector.ConditionalSelect(useAB11, ab11Z, offsetZ)))); + + var bc00X = vB.X + bc.X * tClosestOnBC00; + var bc00Y = vB.Y + bc.Y * tClosestOnBC00; + var bc00Z = vB.Z + bc.Z * tClosestOnBC00; + var bc01X = vB.X + bc.X * tClosestOnBC01; + var bc01Y = vB.Y + bc.Y * tClosestOnBC01; + var bc01Z = vB.Z + bc.Z * tClosestOnBC01; + var bc10X = vB.X + bc.X * tClosestOnBC10; + var bc10Y = vB.Y + bc.Y * tClosestOnBC10; + var bc10Z = vB.Z + bc.Z * tClosestOnBC10; + var bc11X = vB.X + bc.X * tClosestOnBC11; + var bc11Y = vB.Y + bc.Y * tClosestOnBC11; + var bc11Z = vB.Z + bc.Z * tClosestOnBC11; + bc00X = bc00X - Vector.Min(a.HalfWidth, Vector.Max(negativeHalfWidth, bc00X)); + bc00Y = bc00Y - Vector.Min(a.HalfHeight, Vector.Max(negativeHalfHeight, bc00Y)); + bc00Z = bc00Z - Vector.Min(a.HalfLength, Vector.Max(negativeHalfLength, bc00Z)); + bc01X = bc01X - Vector.Min(a.HalfWidth, Vector.Max(negativeHalfWidth, bc01X)); + bc01Y = bc01Y - Vector.Min(a.HalfHeight, Vector.Max(negativeHalfHeight, bc01Y)); + bc01Z = bc01Z - Vector.Min(a.HalfLength, Vector.Max(negativeHalfLength, bc01Z)); + bc10X = bc10X - Vector.Min(a.HalfWidth, Vector.Max(negativeHalfWidth, bc10X)); + bc10Y = bc10Y - Vector.Min(a.HalfHeight, Vector.Max(negativeHalfHeight, bc10Y)); + bc10Z = bc10Z - Vector.Min(a.HalfLength, Vector.Max(negativeHalfLength, bc10Z)); + bc11X = bc11X - Vector.Min(a.HalfWidth, Vector.Max(negativeHalfWidth, bc11X)); + bc11Y = bc11Y - Vector.Min(a.HalfHeight, Vector.Max(negativeHalfHeight, bc11Y)); + bc11Z = bc11Z - Vector.Min(a.HalfLength, Vector.Max(negativeHalfLength, bc11Z)); + var distanceSquaredBC00 = bc00X * bc00X + bc00Y * bc00Y + bc00Z * bc00Z; + var distanceSquaredBC01 = bc01X * bc01X + bc01Y * bc01Y + bc01Z * bc01Z; + var distanceSquaredBC10 = bc10X * bc10X + bc10Y * bc10Y + bc10Z * bc10Z; + var distanceSquaredBC11 = bc11X * bc11X + bc11Y * bc11Y + bc11Z * bc11Z; + distanceSquared = Vector.Min(distanceSquared, Vector.Min(Vector.Min(distanceSquaredBC00, distanceSquaredBC01), Vector.Min(distanceSquaredBC10, distanceSquaredBC11))); + var useBC00 = Vector.Equals(distanceSquared, distanceSquaredBC00); + var useBC01 = Vector.Equals(distanceSquared, distanceSquaredBC01); + var useBC10 = Vector.Equals(distanceSquared, distanceSquaredBC10); + var useBC11 = Vector.Equals(distanceSquared, distanceSquaredBC11); + offsetX = Vector.ConditionalSelect(useBC00, bc00X, Vector.ConditionalSelect(useBC01, bc01X, Vector.ConditionalSelect(useBC10, bc10X, Vector.ConditionalSelect(useBC11, bc11X, offsetX)))); + offsetY = Vector.ConditionalSelect(useBC00, bc00Y, Vector.ConditionalSelect(useBC01, bc01Y, Vector.ConditionalSelect(useBC10, bc10Y, Vector.ConditionalSelect(useBC11, bc11Y, offsetY)))); + offsetZ = Vector.ConditionalSelect(useBC00, bc00Z, Vector.ConditionalSelect(useBC01, bc01Z, Vector.ConditionalSelect(useBC10, bc10Z, Vector.ConditionalSelect(useBC11, bc11Z, offsetZ)))); + + var ca00X = vC.X + ca.X * tClosestOnCA00; + var ca00Y = vC.Y + ca.Y * tClosestOnCA00; + var ca00Z = vC.Z + ca.Z * tClosestOnCA00; + var ca01X = vC.X + ca.X * tClosestOnCA01; + var ca01Y = vC.Y + ca.Y * tClosestOnCA01; + var ca01Z = vC.Z + ca.Z * tClosestOnCA01; + var ca10X = vC.X + ca.X * tClosestOnCA10; + var ca10Y = vC.Y + ca.Y * tClosestOnCA10; + var ca10Z = vC.Z + ca.Z * tClosestOnCA10; + var ca11X = vC.X + ca.X * tClosestOnCA11; + var ca11Y = vC.Y + ca.Y * tClosestOnCA11; + var ca11Z = vC.Z + ca.Z * tClosestOnCA11; + ca00X = ca00X - Vector.Min(a.HalfWidth, Vector.Max(negativeHalfWidth, ca00X)); + ca00Y = ca00Y - Vector.Min(a.HalfHeight, Vector.Max(negativeHalfHeight, ca00Y)); + ca00Z = ca00Z - Vector.Min(a.HalfLength, Vector.Max(negativeHalfLength, ca00Z)); + ca01X = ca01X - Vector.Min(a.HalfWidth, Vector.Max(negativeHalfWidth, ca01X)); + ca01Y = ca01Y - Vector.Min(a.HalfHeight, Vector.Max(negativeHalfHeight, ca01Y)); + ca01Z = ca01Z - Vector.Min(a.HalfLength, Vector.Max(negativeHalfLength, ca01Z)); + ca10X = ca10X - Vector.Min(a.HalfWidth, Vector.Max(negativeHalfWidth, ca10X)); + ca10Y = ca10Y - Vector.Min(a.HalfHeight, Vector.Max(negativeHalfHeight, ca10Y)); + ca10Z = ca10Z - Vector.Min(a.HalfLength, Vector.Max(negativeHalfLength, ca10Z)); + ca11X = ca11X - Vector.Min(a.HalfWidth, Vector.Max(negativeHalfWidth, ca11X)); + ca11Y = ca11Y - Vector.Min(a.HalfHeight, Vector.Max(negativeHalfHeight, ca11Y)); + ca11Z = ca11Z - Vector.Min(a.HalfLength, Vector.Max(negativeHalfLength, ca11Z)); + var distanceSquaredCA00 = ca00X * ca00X + ca00Y * ca00Y + ca00Z * ca00Z; + var distanceSquaredCA01 = ca01X * ca01X + ca01Y * ca01Y + ca01Z * ca01Z; + var distanceSquaredCA10 = ca10X * ca10X + ca10Y * ca10Y + ca10Z * ca10Z; + var distanceSquaredCA11 = ca11X * ca11X + ca11Y * ca11Y + ca11Z * ca11Z; + distanceSquared = Vector.Min(distanceSquared, Vector.Min(Vector.Min(distanceSquaredCA00, distanceSquaredCA01), Vector.Min(distanceSquaredCA10, distanceSquaredCA11))); + var useCA00 = Vector.Equals(distanceSquared, distanceSquaredCA00); + var useCA01 = Vector.Equals(distanceSquared, distanceSquaredCA01); + var useCA10 = Vector.Equals(distanceSquared, distanceSquaredCA10); + var useCA11 = Vector.Equals(distanceSquared, distanceSquaredCA11); + offsetX = Vector.ConditionalSelect(useCA00, ca00X, Vector.ConditionalSelect(useCA01, ca01X, Vector.ConditionalSelect(useCA10, ca10X, Vector.ConditionalSelect(useCA11, ca11X, offsetX)))); + offsetY = Vector.ConditionalSelect(useCA00, ca00Y, Vector.ConditionalSelect(useCA01, ca01Y, Vector.ConditionalSelect(useCA10, ca10Y, Vector.ConditionalSelect(useCA11, ca11Y, offsetY)))); + offsetZ = Vector.ConditionalSelect(useCA00, ca00Z, Vector.ConditionalSelect(useCA01, ca01Z, Vector.ConditionalSelect(useCA10, ca10Z, Vector.ConditionalSelect(useCA11, ca11Z, offsetZ)))); + + var distance = Vector.SquareRoot(distanceSquared); + var inverseDistance = new Vector(-1f) / distance; + localNormalCandidate.X = offsetX * inverseDistance; + localNormalCandidate.Y = offsetY * inverseDistance; + localNormalCandidate.Z = offsetZ * inverseDistance; + Vector3Wide.Length(localNormalCandidate, out var length); + Vector3Wide.Dot(localNormalCandidate, vA, out var nVA); + Vector3Wide.Dot(localNormalCandidate, vB, out var nVB); + Vector3Wide.Dot(localNormalCandidate, vC, out var nVC); + var extremeA = Vector.Abs(localNormalCandidate.X) * a.HalfWidth + Vector.Abs(localNormalCandidate.Y) * a.HalfHeight + Vector.Abs(localNormalCandidate.Z) * a.HalfLength; + GetDepthForInterval(extremeA, nVA, nVB, nVC, out depthCandidate); + //Guard against division by zero. + depthCandidate = Vector.ConditionalSelect(Vector.GreaterThan(distanceSquared, new Vector(1e-6f)), depthCandidate, new Vector(float.MaxValue)); + Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + + //Vector3Wide.Cross(ab, triangleNormal, out var edgePlaneNormalAB); + //Vector3Wide.Cross(bc, triangleNormal, out var edgePlaneNormalBC); + //Vector3Wide.Cross(ca, triangleNormal, out var edgePlaneNormalCA); + //var edgePlaneNormalABDotFaceOffset = faceOffset * Vector.ConditionalSelect(useFaceX, edgePlaneNormalAB.X, Vector.ConditionalSelect(useFaceY, edgePlaneNormalAB.Y, edgePlaneNormalAB.Z)); + //var edgePlaneNormalBCDotFaceOffset = faceOffset * Vector.ConditionalSelect(useFaceX, edgePlaneNormalBC.X, Vector.ConditionalSelect(useFaceY, edgePlaneNormalBC.Y, edgePlaneNormalBC.Z)); + //var edgePlaneNormalCADotFaceOffset = faceOffset * Vector.ConditionalSelect(useFaceX, edgePlaneNormalCA.X, Vector.ConditionalSelect(useFaceY, edgePlaneNormalCA.Y, edgePlaneNormalCA.Z)); + //var edgePlaneNormalABDotBoxEdgeX = xOffset * Vector.ConditionalSelect(useFaceX, edgePlaneNormalAB.Y, Vector.ConditionalSelect(useFaceY, edgePlaneNormalAB.Z, edgePlaneNormalAB.X)); + //var edgePlaneNormalBCDotBoxEdgeX = xOffset * Vector.ConditionalSelect(useFaceX, edgePlaneNormalBC.Y, Vector.ConditionalSelect(useFaceY, edgePlaneNormalBC.Z, edgePlaneNormalBC.X)); + //var edgePlaneNormalCADotBoxEdgeX = xOffset * Vector.ConditionalSelect(useFaceX, edgePlaneNormalCA.Y, Vector.ConditionalSelect(useFaceY, edgePlaneNormalCA.Z, edgePlaneNormalCA.X)); + //var edgePlaneNormalABDotBoxEdgeY = yOffset * Vector.ConditionalSelect(useFaceX, edgePlaneNormalAB.Z, Vector.ConditionalSelect(useFaceY, edgePlaneNormalAB.X, edgePlaneNormalAB.Y)); + //var edgePlaneNormalBCDotBoxEdgeY = yOffset * Vector.ConditionalSelect(useFaceX, edgePlaneNormalBC.Z, Vector.ConditionalSelect(useFaceY, edgePlaneNormalBC.X, edgePlaneNormalBC.Y)); + //var edgePlaneNormalCADotBoxEdgeY = yOffset * Vector.ConditionalSelect(useFaceX, edgePlaneNormalCA.Z, Vector.ConditionalSelect(useFaceY, edgePlaneNormalCA.X, edgePlaneNormalCA.Y)); + + + + + + + } + + //If the local normal points against the triangle normal, then it's on the backside and should not collide. Vector3Wide.Dot(localNormal, triangleNormal, out var normalDot); - ManifoldCandidateHelper.CreateActiveMask(pairCount, out var activeLanes); var allowContacts = Vector.BitwiseAnd(Vector.GreaterThanOrEqual(normalDot, new Vector(SphereTriangleTester.BackfaceNormalDotRejectionThreshold)), activeLanes); if (Vector.EqualsAll(allowContacts, Vector.Zero)) { From 38fc4be4ebb145c096dd30c907571356c6ea74fb Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 23 Sep 2021 17:31:20 -0500 Subject: [PATCH 011/947] Simplified triangle-triangle vertex case (no need for edge plane normal containment testing if a depth interval is explicitly calculated). Cleaned up box-triangle. --- .../CollisionTasks/BoxTriangleTester.cs | 38 +++---------------- .../CollisionTasks/TrianglePairTester.cs | 29 +++++--------- 2 files changed, 14 insertions(+), 53 deletions(-) diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/BoxTriangleTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/BoxTriangleTester.cs index aecb594be..49dc01721 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/BoxTriangleTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/BoxTriangleTester.cs @@ -363,21 +363,12 @@ public unsafe void Test( var offsetZ = Vector.ConditionalSelect(useA, boxToAZ, Vector.ConditionalSelect(useB, boxToBZ, boxToCZ)); //Now examine the 4 box vertices from the best box face. - //The box face is spanned by two edge directions which are aligned with the working space's axes. - //To check a box vertex for containment: - //dot(edgePlaneNormalAB, boxVertex - vA), which is proportional to barycentric weight, scaled up by the triangle normal length. - //That is, barycentricWeightC = dot(edgePlaneNormalAB, boxVertex - vA) / triangleNormalLength. - //To detect containment, we can compute two barycentric weights and derive the third. If all are positive, then the box vertex is contained and should not contribute. - //(It would be redundant with the triangle face depth result, so there's no reason to go through the effort to reconstruct the exact point on the triangle's surface.) - //Note that: boxVertex = faceOffset +- boxEdgeOffsetX +- boxEdgeOffsetY - //So we can split the above barycentric weight calculation into pieces. Leaving it scaled for succinctness: - //scaledBarycentricWeightC = dot(edgePlaneNormalAB, boxVertex) - dot(edgePlaneNormalAB, vA) - //scaledBarycentricWeightC = dot(edgePlaneNormalAB, faceOffset) +- dot(edgePlaneNormalAB, boxEdgeOffsetX) +- dot(edgePlaneNormalAB, boxEdgeOffsetY) - dot(edgePlaneNormalAB, vA) - //So we can share quite a few operations across the 4 box vertices. - - //This gives us containment, but does not provide the closest point on the triangle directly. For simplicity, we'll just compute the closest point on each edge directly: + //For simplicity, we'll just compute the closest point on each edge directly: //tClosestPointOnAB = clamp(dot(edgeOffsetAB, boxVertex - vA) / ||edgeOffsetAB||^2, 0, 1) - // = clamp((dot(edgeOffsetAB, faceOffset) +- dot(edgeOffsetAB, boxEdgeOffsetX) +- dot(edgeOffsetAB, boxEdgeOffsetY) - dot(edgeOffsetAB, vA)) / ||edgeOffsetAB||^2, 0, 1) + //Note that: boxVertex = faceOffset +- boxEdgeOffsetX +- boxEdgeOffsetY + //So we can split the above calculation into pieces. Leaving it scaled for succinctness: + //tClosestPointOnAB = clamp((dot(edgeOffsetAB, faceOffset) +- dot(edgeOffsetAB, boxEdgeOffsetX) +- dot(edgeOffsetAB, boxEdgeOffsetY) - dot(edgeOffsetAB, vA)) / ||edgeOffsetAB||^2, 0, 1) + //So we can share quite a few operations across the 4 box vertices. var absNormalX = Vector.Abs(localNormal.X); var absNormalY = Vector.Abs(localNormal.Y); @@ -551,25 +542,6 @@ public unsafe void Test( //Guard against division by zero. depthCandidate = Vector.ConditionalSelect(Vector.GreaterThan(distanceSquared, new Vector(1e-6f)), depthCandidate, new Vector(float.MaxValue)); Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - - //Vector3Wide.Cross(ab, triangleNormal, out var edgePlaneNormalAB); - //Vector3Wide.Cross(bc, triangleNormal, out var edgePlaneNormalBC); - //Vector3Wide.Cross(ca, triangleNormal, out var edgePlaneNormalCA); - //var edgePlaneNormalABDotFaceOffset = faceOffset * Vector.ConditionalSelect(useFaceX, edgePlaneNormalAB.X, Vector.ConditionalSelect(useFaceY, edgePlaneNormalAB.Y, edgePlaneNormalAB.Z)); - //var edgePlaneNormalBCDotFaceOffset = faceOffset * Vector.ConditionalSelect(useFaceX, edgePlaneNormalBC.X, Vector.ConditionalSelect(useFaceY, edgePlaneNormalBC.Y, edgePlaneNormalBC.Z)); - //var edgePlaneNormalCADotFaceOffset = faceOffset * Vector.ConditionalSelect(useFaceX, edgePlaneNormalCA.X, Vector.ConditionalSelect(useFaceY, edgePlaneNormalCA.Y, edgePlaneNormalCA.Z)); - //var edgePlaneNormalABDotBoxEdgeX = xOffset * Vector.ConditionalSelect(useFaceX, edgePlaneNormalAB.Y, Vector.ConditionalSelect(useFaceY, edgePlaneNormalAB.Z, edgePlaneNormalAB.X)); - //var edgePlaneNormalBCDotBoxEdgeX = xOffset * Vector.ConditionalSelect(useFaceX, edgePlaneNormalBC.Y, Vector.ConditionalSelect(useFaceY, edgePlaneNormalBC.Z, edgePlaneNormalBC.X)); - //var edgePlaneNormalCADotBoxEdgeX = xOffset * Vector.ConditionalSelect(useFaceX, edgePlaneNormalCA.Y, Vector.ConditionalSelect(useFaceY, edgePlaneNormalCA.Z, edgePlaneNormalCA.X)); - //var edgePlaneNormalABDotBoxEdgeY = yOffset * Vector.ConditionalSelect(useFaceX, edgePlaneNormalAB.Z, Vector.ConditionalSelect(useFaceY, edgePlaneNormalAB.X, edgePlaneNormalAB.Y)); - //var edgePlaneNormalBCDotBoxEdgeY = yOffset * Vector.ConditionalSelect(useFaceX, edgePlaneNormalBC.Z, Vector.ConditionalSelect(useFaceY, edgePlaneNormalBC.X, edgePlaneNormalBC.Y)); - //var edgePlaneNormalCADotBoxEdgeY = yOffset * Vector.ConditionalSelect(useFaceX, edgePlaneNormalCA.Z, Vector.ConditionalSelect(useFaceY, edgePlaneNormalCA.X, edgePlaneNormalCA.Y)); - - - - - - } diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs index 5df0a4540..62f70338f 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs @@ -47,8 +47,7 @@ static void TestEdgeEdge( static void TestVertexNormal(in Vector3Wide vertex, in Vector3Wide opposingA, in Vector3Wide opposingB, in Vector3Wide opposingC, in Vector3Wide opposingAB, in Vector3Wide opposingBC, in Vector3Wide opposingCA, - in Vector3Wide opposingEdgePlaneAB, in Vector3Wide opposingEdgePlaneBC, - in Vector inverseLengthSquaredAB, in Vector inverseLengthSquaredBC, in Vector inverseLengthSquaredCA, in Vector triangleNormalLength, + in Vector inverseLengthSquaredAB, in Vector inverseLengthSquaredBC, in Vector inverseLengthSquaredCA, ref Vector distanceSquared, ref Vector3Wide offset) { //Project the vertex onto all three edges. Take the closest approach as a normal candidate. @@ -56,11 +55,6 @@ static void TestVertexNormal(in Vector3Wide vertex, Vector3Wide.Subtract(vertex, opposingA, out var av); Vector3Wide.Subtract(vertex, opposingB, out var bv); Vector3Wide.Subtract(vertex, opposingC, out var cv); - Vector3Wide.Dot(av, opposingEdgePlaneAB, out var avDotEdgePlaneAB); - Vector3Wide.Dot(bv, opposingEdgePlaneBC, out var bvDotEdgePlaneBC); - //Because of the equivalence between the edge plane tests and barycentric coordinates, we can compute the last edge plane dot implicitly: - var cvDotEdgePlaneCA = triangleNormalLength - avDotEdgePlaneAB - bvDotEdgePlaneBC; - var outsideTriangle = Vector.BitwiseOr(Vector.GreaterThan(avDotEdgePlaneAB, Vector.Zero), Vector.BitwiseOr(Vector.GreaterThan(bvDotEdgePlaneBC, Vector.Zero), Vector.GreaterThan(cvDotEdgePlaneCA, Vector.Zero))); Vector3Wide.Dot(av, opposingAB, out var avDotAB); Vector3Wide.Dot(bv, opposingBC, out var bvDotBC); Vector3Wide.Dot(cv, opposingCA, out var cvDotCA); @@ -85,9 +79,8 @@ static void TestVertexNormal(in Vector3Wide vertex, Vector3Wide.ConditionalSelect(Vector.Equals(distanceSquaredCandidate, abDistanceSquared), vToAB, vToCA, out var offsetCandidate); Vector3Wide.ConditionalSelect(Vector.Equals(distanceSquaredCandidate, bcDistanceSquared), vToBC, offsetCandidate, out offsetCandidate); - //Ignore this vertex if it's contained within the triangle. We'll detect distances that are too close to generate normals outside. - var useCandidate = Vector.BitwiseAnd(Vector.LessThan(distanceSquaredCandidate, distanceSquared), outsideTriangle); - distanceSquared = Vector.ConditionalSelect(useCandidate, distanceSquaredCandidate, distanceSquared); + var useCandidate = Vector.LessThan(distanceSquaredCandidate, distanceSquared); + distanceSquared = Vector.Min(distanceSquaredCandidate, distanceSquared); Vector3Wide.ConditionalSelect(useCandidate, offsetCandidate, offset, out offset); } @@ -353,18 +346,14 @@ public unsafe void Test( var inverseABBLengthSquared = Vector.One / abBLengthSquared; var inverseBCBLengthSquared = Vector.One / bcBLengthSquared; var inverseCABLengthSquared = Vector.One / caBLengthSquared; - Vector3Wide.Cross(abA, faceNormalA, out var edgeNormalABOnA); - Vector3Wide.Cross(bcA, faceNormalA, out var edgeNormalBCOnA); - Vector3Wide.Cross(abB, faceNormalB, out var edgeNormalABOnB); - Vector3Wide.Cross(bcB, faceNormalB, out var edgeNormalBCOnB); var distanceSquared = new Vector(float.MaxValue); Unsafe.SkipInit(out Vector3Wide offset); - TestVertexNormal(a.A, bA, bB, bC, abB, bcB, caB, edgeNormalABOnB, edgeNormalBCOnB, inverseABBLengthSquared, inverseBCBLengthSquared, inverseCABLengthSquared, faceNormalBLength, ref distanceSquared, ref offset); - TestVertexNormal(a.B, bA, bB, bC, abB, bcB, caB, edgeNormalABOnB, edgeNormalBCOnB, inverseABBLengthSquared, inverseBCBLengthSquared, inverseCABLengthSquared, faceNormalBLength, ref distanceSquared, ref offset); - TestVertexNormal(a.C, bA, bB, bC, abB, bcB, caB, edgeNormalABOnB, edgeNormalBCOnB, inverseABBLengthSquared, inverseBCBLengthSquared, inverseCABLengthSquared, faceNormalBLength, ref distanceSquared, ref offset); - TestVertexNormal(bA, a.A, a.B, a.C, abA, bcA, caA, edgeNormalABOnA, edgeNormalBCOnA, inverseABALengthSquared, inverseBCALengthSquared, inverseCAALengthSquared, faceNormalALength, ref distanceSquared, ref offset); - TestVertexNormal(bB, a.A, a.B, a.C, abA, bcA, caA, edgeNormalABOnA, edgeNormalBCOnA, inverseABALengthSquared, inverseBCALengthSquared, inverseCAALengthSquared, faceNormalALength, ref distanceSquared, ref offset); - TestVertexNormal(bC, a.A, a.B, a.C, abA, bcA, caA, edgeNormalABOnA, edgeNormalBCOnA, inverseABALengthSquared, inverseBCALengthSquared, inverseCAALengthSquared, faceNormalALength, ref distanceSquared, ref offset); + TestVertexNormal(a.A, bA, bB, bC, abB, bcB, caB, inverseABBLengthSquared, inverseBCBLengthSquared, inverseCABLengthSquared, ref distanceSquared, ref offset); + TestVertexNormal(a.B, bA, bB, bC, abB, bcB, caB, inverseABBLengthSquared, inverseBCBLengthSquared, inverseCABLengthSquared, ref distanceSquared, ref offset); + TestVertexNormal(a.C, bA, bB, bC, abB, bcB, caB, inverseABBLengthSquared, inverseBCBLengthSquared, inverseCABLengthSquared, ref distanceSquared, ref offset); + TestVertexNormal(bA, a.A, a.B, a.C, abA, bcA, caA, inverseABALengthSquared, inverseBCALengthSquared, inverseCAALengthSquared, ref distanceSquared, ref offset); + TestVertexNormal(bB, a.A, a.B, a.C, abA, bcA, caA, inverseABALengthSquared, inverseBCALengthSquared, inverseCAALengthSquared, ref distanceSquared, ref offset); + TestVertexNormal(bC, a.A, a.B, a.C, abA, bcA, caA, inverseABALengthSquared, inverseBCALengthSquared, inverseCAALengthSquared, ref distanceSquared, ref offset); var distance = Vector.SquareRoot(distanceSquared); Vector3Wide.Scale(offset, Vector.One / distance, out localNormalCandidate); From 442f0ae314e79c452b8f098b358340dd4c1b476f Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 23 Sep 2021 19:07:54 -0500 Subject: [PATCH 012/947] On the other hand, what if we erased 3 days of work and did a simpler and faster thing? --- .../CollisionTasks/BoxTriangleTester.cs | 422 +++++++++--------- .../CollisionTasks/TrianglePairTester.cs | 66 ++- .../CollisionDetection/MeshReduction.cs | 13 +- 3 files changed, 252 insertions(+), 249 deletions(-) diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/BoxTriangleTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/BoxTriangleTester.cs index 49dc01721..f9d31de9b 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/BoxTriangleTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/BoxTriangleTester.cs @@ -334,215 +334,219 @@ public unsafe void Test( Select(ref depth, ref localNormal, triangleFaceDepth, calibratedTriangleNormal); ManifoldCandidateHelper.CreateActiveMask(pairCount, out var activeLanes); - var testVertexNormals = Vector.BitwiseAnd(activeLanes, Vector.LessThan(depth, Vector.Zero)); - if (Vector.LessThanAny(testVertexNormals, Vector.Zero)) - { - //At least one lane contains a separating pair. Mesh reduction relies on separating normals being minimal (or very very close to it), so test 7 candidate normals. - //First examine the 3 triangle vertices. - var negativeHalfWidth = -a.HalfWidth; - var negativeHalfHeight = -a.HalfHeight; - var negativeHalfLength = -a.HalfLength; - var boxToAX = vA.X - Vector.Min(a.HalfWidth, Vector.Max(negativeHalfWidth, vA.X)); - var boxToAY = vA.Y - Vector.Min(a.HalfHeight, Vector.Max(negativeHalfHeight, vA.Y)); - var boxToAZ = vA.Z - Vector.Min(a.HalfLength, Vector.Max(negativeHalfLength, vA.Z)); - var boxToBX = vB.X - Vector.Min(a.HalfWidth, Vector.Max(negativeHalfWidth, vB.X)); - var boxToBY = vB.Y - Vector.Min(a.HalfHeight, Vector.Max(negativeHalfHeight, vB.Y)); - var boxToBZ = vB.Z - Vector.Min(a.HalfLength, Vector.Max(negativeHalfLength, vB.Z)); - var boxToCX = vC.X - Vector.Min(a.HalfWidth, Vector.Max(negativeHalfWidth, vC.X)); - var boxToCY = vC.Y - Vector.Min(a.HalfHeight, Vector.Max(negativeHalfHeight, vC.Y)); - var boxToCZ = vC.Z - Vector.Min(a.HalfLength, Vector.Max(negativeHalfLength, vC.Z)); - - var distanceSquaredA = boxToAX * boxToAX + boxToAY * boxToAY + boxToAZ * boxToAZ; - var distanceSquaredB = boxToBX * boxToBX + boxToBY * boxToBY + boxToBZ * boxToBZ; - var distanceSquaredC = boxToCX * boxToCX + boxToCY * boxToCY + boxToCZ * boxToCZ; - var distanceSquared = Vector.Min(distanceSquaredA, Vector.Min(distanceSquaredB, distanceSquaredC)); - var useA = Vector.Equals(distanceSquared, distanceSquaredA); - var useB = Vector.Equals(distanceSquared, distanceSquaredB); - var offsetX = Vector.ConditionalSelect(useA, boxToAX, Vector.ConditionalSelect(useB, boxToBX, boxToCX)); - var offsetY = Vector.ConditionalSelect(useA, boxToAY, Vector.ConditionalSelect(useB, boxToBY, boxToCY)); - var offsetZ = Vector.ConditionalSelect(useA, boxToAZ, Vector.ConditionalSelect(useB, boxToBZ, boxToCZ)); - - //Now examine the 4 box vertices from the best box face. - //For simplicity, we'll just compute the closest point on each edge directly: - //tClosestPointOnAB = clamp(dot(edgeOffsetAB, boxVertex - vA) / ||edgeOffsetAB||^2, 0, 1) - //Note that: boxVertex = faceOffset +- boxEdgeOffsetX +- boxEdgeOffsetY - //So we can split the above calculation into pieces. Leaving it scaled for succinctness: - //tClosestPointOnAB = clamp((dot(edgeOffsetAB, faceOffset) +- dot(edgeOffsetAB, boxEdgeOffsetX) +- dot(edgeOffsetAB, boxEdgeOffsetY) - dot(edgeOffsetAB, vA)) / ||edgeOffsetAB||^2, 0, 1) - //So we can share quite a few operations across the 4 box vertices. - - var absNormalX = Vector.Abs(localNormal.X); - var absNormalY = Vector.Abs(localNormal.Y); - var absNormalZ = Vector.Abs(localNormal.Z); - var useFaceX = Vector.BitwiseAnd(Vector.GreaterThan(absNormalX, absNormalY), Vector.GreaterThan(absNormalX, absNormalZ)); - var useFaceY = Vector.AndNot(Vector.GreaterThan(absNormalY, absNormalZ), useFaceX); - var faceLocalNormalComponent = Vector.ConditionalSelect(useFaceX, localNormal.X, Vector.ConditionalSelect(useFaceY, localNormal.Y, localNormal.Z)); - var faceOffsetMagnitude = Vector.ConditionalSelect(useFaceX, a.HalfWidth, Vector.ConditionalSelect(useFaceY, a.HalfHeight, a.HalfLength)); - var xOffset = Vector.ConditionalSelect(useFaceX, a.HalfHeight, Vector.ConditionalSelect(useFaceY, a.HalfLength, a.HalfWidth)); - var yOffset = Vector.ConditionalSelect(useFaceX, a.HalfLength, Vector.ConditionalSelect(useFaceY, a.HalfWidth, a.HalfHeight)); - var faceOffset = Vector.ConditionalSelect(Vector.LessThan(faceLocalNormalComponent, Vector.Zero), faceOffsetMagnitude, -faceOffsetMagnitude); - - //Thanks to axis alignment, all the box component dot products squish down to component selections. - var edgeABDotFaceOffset = faceOffset * Vector.ConditionalSelect(useFaceX, ab.X, Vector.ConditionalSelect(useFaceY, ab.Y, ab.Z)); - var edgeBCDotFaceOffset = faceOffset * Vector.ConditionalSelect(useFaceX, bc.X, Vector.ConditionalSelect(useFaceY, bc.Y, bc.Z)); - var edgeCADotFaceOffset = faceOffset * Vector.ConditionalSelect(useFaceX, ca.X, Vector.ConditionalSelect(useFaceY, ca.Y, ca.Z)); - var edgeABDotBoxEdgeX = xOffset * Vector.ConditionalSelect(useFaceX, ab.Y, Vector.ConditionalSelect(useFaceY, ab.Z, ab.X)); - var edgeBCDotBoxEdgeX = xOffset * Vector.ConditionalSelect(useFaceX, bc.Y, Vector.ConditionalSelect(useFaceY, bc.Z, bc.X)); - var edgeCADotBoxEdgeX = xOffset * Vector.ConditionalSelect(useFaceX, ca.Y, Vector.ConditionalSelect(useFaceY, ca.Z, ca.X)); - var edgeABDotBoxEdgeY = yOffset * Vector.ConditionalSelect(useFaceX, ab.Z, Vector.ConditionalSelect(useFaceY, ab.X, ab.Y)); - var edgeBCDotBoxEdgeY = yOffset * Vector.ConditionalSelect(useFaceX, bc.Z, Vector.ConditionalSelect(useFaceY, bc.X, bc.Y)); - var edgeCADotBoxEdgeY = yOffset * Vector.ConditionalSelect(useFaceX, ca.Z, Vector.ConditionalSelect(useFaceY, ca.X, ca.Y)); - Vector3Wide.Dot(ab, vA, out var abDotA); - Vector3Wide.Dot(bc, vB, out var bcDotB); - Vector3Wide.Dot(ca, vC, out var caDotC); - - var inverseLengthSquaredAB = Vector.One / (ab.X * ab.X + ab.Y * ab.Y + ab.Z * ab.Z); - var inverseLengthSquaredBC = Vector.One / (bc.X * bc.X + bc.Y * bc.Y + bc.Z * bc.Z); - var inverseLengthSquaredCA = Vector.One / (ca.X * ca.X + ca.Y * ca.Y + ca.Z * ca.Z); - var abToFaceDot = edgeABDotFaceOffset - abDotA; - var bcToFaceDot = edgeBCDotFaceOffset - bcDotB; - var caToFaceDot = edgeCADotFaceOffset - caDotC; - var tClosestOnAB00 = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (abToFaceDot - edgeABDotBoxEdgeX - edgeABDotBoxEdgeY) * inverseLengthSquaredAB)); - var tClosestOnAB01 = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (abToFaceDot - edgeABDotBoxEdgeX + edgeABDotBoxEdgeY) * inverseLengthSquaredAB)); - var tClosestOnAB10 = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (abToFaceDot + edgeABDotBoxEdgeX - edgeABDotBoxEdgeY) * inverseLengthSquaredAB)); - var tClosestOnAB11 = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (abToFaceDot + edgeABDotBoxEdgeX + edgeABDotBoxEdgeY) * inverseLengthSquaredAB)); - var tClosestOnBC00 = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (bcToFaceDot - edgeBCDotBoxEdgeX - edgeBCDotBoxEdgeY) * inverseLengthSquaredBC)); - var tClosestOnBC01 = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (bcToFaceDot - edgeBCDotBoxEdgeX + edgeBCDotBoxEdgeY) * inverseLengthSquaredBC)); - var tClosestOnBC10 = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (bcToFaceDot + edgeBCDotBoxEdgeX - edgeBCDotBoxEdgeY) * inverseLengthSquaredBC)); - var tClosestOnBC11 = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (bcToFaceDot + edgeBCDotBoxEdgeX + edgeBCDotBoxEdgeY) * inverseLengthSquaredBC)); - var tClosestOnCA00 = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (caToFaceDot - edgeCADotBoxEdgeX - edgeCADotBoxEdgeY) * inverseLengthSquaredCA)); - var tClosestOnCA01 = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (caToFaceDot - edgeCADotBoxEdgeX + edgeCADotBoxEdgeY) * inverseLengthSquaredCA)); - var tClosestOnCA10 = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (caToFaceDot + edgeCADotBoxEdgeX - edgeCADotBoxEdgeY) * inverseLengthSquaredCA)); - var tClosestOnCA11 = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (caToFaceDot + edgeCADotBoxEdgeX + edgeCADotBoxEdgeY) * inverseLengthSquaredCA)); - - //We now have the t value of every box vertex on each triangle edge. - //Find the closest pair. - //offsetAB00 = a + ab * t00 - (faceOffset - edgeOffsetX - edgeOffsetY) - var ab00X = vA.X + ab.X * tClosestOnAB00; - var ab00Y = vA.Y + ab.Y * tClosestOnAB00; - var ab00Z = vA.Z + ab.Z * tClosestOnAB00; - var ab01X = vA.X + ab.X * tClosestOnAB01; - var ab01Y = vA.Y + ab.Y * tClosestOnAB01; - var ab01Z = vA.Z + ab.Z * tClosestOnAB01; - var ab10X = vA.X + ab.X * tClosestOnAB10; - var ab10Y = vA.Y + ab.Y * tClosestOnAB10; - var ab10Z = vA.Z + ab.Z * tClosestOnAB10; - var ab11X = vA.X + ab.X * tClosestOnAB11; - var ab11Y = vA.Y + ab.Y * tClosestOnAB11; - var ab11Z = vA.Z + ab.Z * tClosestOnAB11; - ab00X = ab00X - Vector.Min(a.HalfWidth, Vector.Max(negativeHalfWidth, ab00X)); - ab00Y = ab00Y - Vector.Min(a.HalfHeight, Vector.Max(negativeHalfHeight, ab00Y)); - ab00Z = ab00Z - Vector.Min(a.HalfLength, Vector.Max(negativeHalfLength, ab00Z)); - ab01X = ab01X - Vector.Min(a.HalfWidth, Vector.Max(negativeHalfWidth, ab01X)); - ab01Y = ab01Y - Vector.Min(a.HalfHeight, Vector.Max(negativeHalfHeight, ab01Y)); - ab01Z = ab01Z - Vector.Min(a.HalfLength, Vector.Max(negativeHalfLength, ab01Z)); - ab10X = ab10X - Vector.Min(a.HalfWidth, Vector.Max(negativeHalfWidth, ab10X)); - ab10Y = ab10Y - Vector.Min(a.HalfHeight, Vector.Max(negativeHalfHeight, ab10Y)); - ab10Z = ab10Z - Vector.Min(a.HalfLength, Vector.Max(negativeHalfLength, ab10Z)); - ab11X = ab11X - Vector.Min(a.HalfWidth, Vector.Max(negativeHalfWidth, ab11X)); - ab11Y = ab11Y - Vector.Min(a.HalfHeight, Vector.Max(negativeHalfHeight, ab11Y)); - ab11Z = ab11Z - Vector.Min(a.HalfLength, Vector.Max(negativeHalfLength, ab11Z)); - var distanceSquaredAB00 = ab00X * ab00X + ab00Y * ab00Y + ab00Z * ab00Z; - var distanceSquaredAB01 = ab01X * ab01X + ab01Y * ab01Y + ab01Z * ab01Z; - var distanceSquaredAB10 = ab10X * ab10X + ab10Y * ab10Y + ab10Z * ab10Z; - var distanceSquaredAB11 = ab11X * ab11X + ab11Y * ab11Y + ab11Z * ab11Z; - distanceSquared = Vector.Min(distanceSquared, Vector.Min(Vector.Min(distanceSquaredAB00, distanceSquaredAB01), Vector.Min(distanceSquaredAB10, distanceSquaredAB11))); - var useAB00 = Vector.Equals(distanceSquared, distanceSquaredAB00); - var useAB01 = Vector.Equals(distanceSquared, distanceSquaredAB01); - var useAB10 = Vector.Equals(distanceSquared, distanceSquaredAB10); - var useAB11 = Vector.Equals(distanceSquared, distanceSquaredAB11); - offsetX = Vector.ConditionalSelect(useAB00, ab00X, Vector.ConditionalSelect(useAB01, ab01X, Vector.ConditionalSelect(useAB10, ab10X, Vector.ConditionalSelect(useAB11, ab11X, offsetX)))); - offsetY = Vector.ConditionalSelect(useAB00, ab00Y, Vector.ConditionalSelect(useAB01, ab01Y, Vector.ConditionalSelect(useAB10, ab10Y, Vector.ConditionalSelect(useAB11, ab11Y, offsetY)))); - offsetZ = Vector.ConditionalSelect(useAB00, ab00Z, Vector.ConditionalSelect(useAB01, ab01Z, Vector.ConditionalSelect(useAB10, ab10Z, Vector.ConditionalSelect(useAB11, ab11Z, offsetZ)))); - - var bc00X = vB.X + bc.X * tClosestOnBC00; - var bc00Y = vB.Y + bc.Y * tClosestOnBC00; - var bc00Z = vB.Z + bc.Z * tClosestOnBC00; - var bc01X = vB.X + bc.X * tClosestOnBC01; - var bc01Y = vB.Y + bc.Y * tClosestOnBC01; - var bc01Z = vB.Z + bc.Z * tClosestOnBC01; - var bc10X = vB.X + bc.X * tClosestOnBC10; - var bc10Y = vB.Y + bc.Y * tClosestOnBC10; - var bc10Z = vB.Z + bc.Z * tClosestOnBC10; - var bc11X = vB.X + bc.X * tClosestOnBC11; - var bc11Y = vB.Y + bc.Y * tClosestOnBC11; - var bc11Z = vB.Z + bc.Z * tClosestOnBC11; - bc00X = bc00X - Vector.Min(a.HalfWidth, Vector.Max(negativeHalfWidth, bc00X)); - bc00Y = bc00Y - Vector.Min(a.HalfHeight, Vector.Max(negativeHalfHeight, bc00Y)); - bc00Z = bc00Z - Vector.Min(a.HalfLength, Vector.Max(negativeHalfLength, bc00Z)); - bc01X = bc01X - Vector.Min(a.HalfWidth, Vector.Max(negativeHalfWidth, bc01X)); - bc01Y = bc01Y - Vector.Min(a.HalfHeight, Vector.Max(negativeHalfHeight, bc01Y)); - bc01Z = bc01Z - Vector.Min(a.HalfLength, Vector.Max(negativeHalfLength, bc01Z)); - bc10X = bc10X - Vector.Min(a.HalfWidth, Vector.Max(negativeHalfWidth, bc10X)); - bc10Y = bc10Y - Vector.Min(a.HalfHeight, Vector.Max(negativeHalfHeight, bc10Y)); - bc10Z = bc10Z - Vector.Min(a.HalfLength, Vector.Max(negativeHalfLength, bc10Z)); - bc11X = bc11X - Vector.Min(a.HalfWidth, Vector.Max(negativeHalfWidth, bc11X)); - bc11Y = bc11Y - Vector.Min(a.HalfHeight, Vector.Max(negativeHalfHeight, bc11Y)); - bc11Z = bc11Z - Vector.Min(a.HalfLength, Vector.Max(negativeHalfLength, bc11Z)); - var distanceSquaredBC00 = bc00X * bc00X + bc00Y * bc00Y + bc00Z * bc00Z; - var distanceSquaredBC01 = bc01X * bc01X + bc01Y * bc01Y + bc01Z * bc01Z; - var distanceSquaredBC10 = bc10X * bc10X + bc10Y * bc10Y + bc10Z * bc10Z; - var distanceSquaredBC11 = bc11X * bc11X + bc11Y * bc11Y + bc11Z * bc11Z; - distanceSquared = Vector.Min(distanceSquared, Vector.Min(Vector.Min(distanceSquaredBC00, distanceSquaredBC01), Vector.Min(distanceSquaredBC10, distanceSquaredBC11))); - var useBC00 = Vector.Equals(distanceSquared, distanceSquaredBC00); - var useBC01 = Vector.Equals(distanceSquared, distanceSquaredBC01); - var useBC10 = Vector.Equals(distanceSquared, distanceSquaredBC10); - var useBC11 = Vector.Equals(distanceSquared, distanceSquaredBC11); - offsetX = Vector.ConditionalSelect(useBC00, bc00X, Vector.ConditionalSelect(useBC01, bc01X, Vector.ConditionalSelect(useBC10, bc10X, Vector.ConditionalSelect(useBC11, bc11X, offsetX)))); - offsetY = Vector.ConditionalSelect(useBC00, bc00Y, Vector.ConditionalSelect(useBC01, bc01Y, Vector.ConditionalSelect(useBC10, bc10Y, Vector.ConditionalSelect(useBC11, bc11Y, offsetY)))); - offsetZ = Vector.ConditionalSelect(useBC00, bc00Z, Vector.ConditionalSelect(useBC01, bc01Z, Vector.ConditionalSelect(useBC10, bc10Z, Vector.ConditionalSelect(useBC11, bc11Z, offsetZ)))); - - var ca00X = vC.X + ca.X * tClosestOnCA00; - var ca00Y = vC.Y + ca.Y * tClosestOnCA00; - var ca00Z = vC.Z + ca.Z * tClosestOnCA00; - var ca01X = vC.X + ca.X * tClosestOnCA01; - var ca01Y = vC.Y + ca.Y * tClosestOnCA01; - var ca01Z = vC.Z + ca.Z * tClosestOnCA01; - var ca10X = vC.X + ca.X * tClosestOnCA10; - var ca10Y = vC.Y + ca.Y * tClosestOnCA10; - var ca10Z = vC.Z + ca.Z * tClosestOnCA10; - var ca11X = vC.X + ca.X * tClosestOnCA11; - var ca11Y = vC.Y + ca.Y * tClosestOnCA11; - var ca11Z = vC.Z + ca.Z * tClosestOnCA11; - ca00X = ca00X - Vector.Min(a.HalfWidth, Vector.Max(negativeHalfWidth, ca00X)); - ca00Y = ca00Y - Vector.Min(a.HalfHeight, Vector.Max(negativeHalfHeight, ca00Y)); - ca00Z = ca00Z - Vector.Min(a.HalfLength, Vector.Max(negativeHalfLength, ca00Z)); - ca01X = ca01X - Vector.Min(a.HalfWidth, Vector.Max(negativeHalfWidth, ca01X)); - ca01Y = ca01Y - Vector.Min(a.HalfHeight, Vector.Max(negativeHalfHeight, ca01Y)); - ca01Z = ca01Z - Vector.Min(a.HalfLength, Vector.Max(negativeHalfLength, ca01Z)); - ca10X = ca10X - Vector.Min(a.HalfWidth, Vector.Max(negativeHalfWidth, ca10X)); - ca10Y = ca10Y - Vector.Min(a.HalfHeight, Vector.Max(negativeHalfHeight, ca10Y)); - ca10Z = ca10Z - Vector.Min(a.HalfLength, Vector.Max(negativeHalfLength, ca10Z)); - ca11X = ca11X - Vector.Min(a.HalfWidth, Vector.Max(negativeHalfWidth, ca11X)); - ca11Y = ca11Y - Vector.Min(a.HalfHeight, Vector.Max(negativeHalfHeight, ca11Y)); - ca11Z = ca11Z - Vector.Min(a.HalfLength, Vector.Max(negativeHalfLength, ca11Z)); - var distanceSquaredCA00 = ca00X * ca00X + ca00Y * ca00Y + ca00Z * ca00Z; - var distanceSquaredCA01 = ca01X * ca01X + ca01Y * ca01Y + ca01Z * ca01Z; - var distanceSquaredCA10 = ca10X * ca10X + ca10Y * ca10Y + ca10Z * ca10Z; - var distanceSquaredCA11 = ca11X * ca11X + ca11Y * ca11Y + ca11Z * ca11Z; - distanceSquared = Vector.Min(distanceSquared, Vector.Min(Vector.Min(distanceSquaredCA00, distanceSquaredCA01), Vector.Min(distanceSquaredCA10, distanceSquaredCA11))); - var useCA00 = Vector.Equals(distanceSquared, distanceSquaredCA00); - var useCA01 = Vector.Equals(distanceSquared, distanceSquaredCA01); - var useCA10 = Vector.Equals(distanceSquared, distanceSquaredCA10); - var useCA11 = Vector.Equals(distanceSquared, distanceSquaredCA11); - offsetX = Vector.ConditionalSelect(useCA00, ca00X, Vector.ConditionalSelect(useCA01, ca01X, Vector.ConditionalSelect(useCA10, ca10X, Vector.ConditionalSelect(useCA11, ca11X, offsetX)))); - offsetY = Vector.ConditionalSelect(useCA00, ca00Y, Vector.ConditionalSelect(useCA01, ca01Y, Vector.ConditionalSelect(useCA10, ca10Y, Vector.ConditionalSelect(useCA11, ca11Y, offsetY)))); - offsetZ = Vector.ConditionalSelect(useCA00, ca00Z, Vector.ConditionalSelect(useCA01, ca01Z, Vector.ConditionalSelect(useCA10, ca10Z, Vector.ConditionalSelect(useCA11, ca11Z, offsetZ)))); - - var distance = Vector.SquareRoot(distanceSquared); - var inverseDistance = new Vector(-1f) / distance; - localNormalCandidate.X = offsetX * inverseDistance; - localNormalCandidate.Y = offsetY * inverseDistance; - localNormalCandidate.Z = offsetZ * inverseDistance; - Vector3Wide.Length(localNormalCandidate, out var length); - Vector3Wide.Dot(localNormalCandidate, vA, out var nVA); - Vector3Wide.Dot(localNormalCandidate, vB, out var nVB); - Vector3Wide.Dot(localNormalCandidate, vC, out var nVC); - var extremeA = Vector.Abs(localNormalCandidate.X) * a.HalfWidth + Vector.Abs(localNormalCandidate.Y) * a.HalfHeight + Vector.Abs(localNormalCandidate.Z) * a.HalfLength; - GetDepthForInterval(extremeA, nVA, nVB, nVC, out depthCandidate); - //Guard against division by zero. - depthCandidate = Vector.ConditionalSelect(Vector.GreaterThan(distanceSquared, new Vector(1e-6f)), depthCandidate, new Vector(float.MaxValue)); - Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - } + //The following was created for MeshReduction when it demanded all contact normals be correct during separation. + //Other pairs don't have that requirement, and we ended modifying MeshReduction to be a little less picky. + //This remains for posterity because, hey, it works, and if you need it, there it is. + //var testVertexNormals = Vector.BitwiseAnd(activeLanes, Vector.LessThan(depth, Vector.Zero)); + //if (Vector.LessThanAny(testVertexNormals, Vector.Zero)) + //{ + // //At least one lane contains a separating pair. Mesh reduction relies on separating normals being minimal (or very very close to it), so test 7 candidate normals. + // //First examine the 3 triangle vertices. + // var negativeHalfWidth = -a.HalfWidth; + // var negativeHalfHeight = -a.HalfHeight; + // var negativeHalfLength = -a.HalfLength; + // var boxToAX = vA.X - Vector.Min(a.HalfWidth, Vector.Max(negativeHalfWidth, vA.X)); + // var boxToAY = vA.Y - Vector.Min(a.HalfHeight, Vector.Max(negativeHalfHeight, vA.Y)); + // var boxToAZ = vA.Z - Vector.Min(a.HalfLength, Vector.Max(negativeHalfLength, vA.Z)); + // var boxToBX = vB.X - Vector.Min(a.HalfWidth, Vector.Max(negativeHalfWidth, vB.X)); + // var boxToBY = vB.Y - Vector.Min(a.HalfHeight, Vector.Max(negativeHalfHeight, vB.Y)); + // var boxToBZ = vB.Z - Vector.Min(a.HalfLength, Vector.Max(negativeHalfLength, vB.Z)); + // var boxToCX = vC.X - Vector.Min(a.HalfWidth, Vector.Max(negativeHalfWidth, vC.X)); + // var boxToCY = vC.Y - Vector.Min(a.HalfHeight, Vector.Max(negativeHalfHeight, vC.Y)); + // var boxToCZ = vC.Z - Vector.Min(a.HalfLength, Vector.Max(negativeHalfLength, vC.Z)); + + // var distanceSquaredA = boxToAX * boxToAX + boxToAY * boxToAY + boxToAZ * boxToAZ; + // var distanceSquaredB = boxToBX * boxToBX + boxToBY * boxToBY + boxToBZ * boxToBZ; + // var distanceSquaredC = boxToCX * boxToCX + boxToCY * boxToCY + boxToCZ * boxToCZ; + // var distanceSquared = Vector.Min(distanceSquaredA, Vector.Min(distanceSquaredB, distanceSquaredC)); + // var useA = Vector.Equals(distanceSquared, distanceSquaredA); + // var useB = Vector.Equals(distanceSquared, distanceSquaredB); + // var offsetX = Vector.ConditionalSelect(useA, boxToAX, Vector.ConditionalSelect(useB, boxToBX, boxToCX)); + // var offsetY = Vector.ConditionalSelect(useA, boxToAY, Vector.ConditionalSelect(useB, boxToBY, boxToCY)); + // var offsetZ = Vector.ConditionalSelect(useA, boxToAZ, Vector.ConditionalSelect(useB, boxToBZ, boxToCZ)); + + // //Now examine the 4 box vertices from the best box face. + // //For simplicity, we'll just compute the closest point on each edge directly: + // //tClosestPointOnAB = clamp(dot(edgeOffsetAB, boxVertex - vA) / ||edgeOffsetAB||^2, 0, 1) + // //Note that: boxVertex = faceOffset +- boxEdgeOffsetX +- boxEdgeOffsetY + // //So we can split the above calculation into pieces. Leaving it scaled for succinctness: + // //tClosestPointOnAB = clamp((dot(edgeOffsetAB, faceOffset) +- dot(edgeOffsetAB, boxEdgeOffsetX) +- dot(edgeOffsetAB, boxEdgeOffsetY) - dot(edgeOffsetAB, vA)) / ||edgeOffsetAB||^2, 0, 1) + // //So we can share quite a few operations across the 4 box vertices. + // //Likely some better options here. + + // var absNormalX = Vector.Abs(localNormal.X); + // var absNormalY = Vector.Abs(localNormal.Y); + // var absNormalZ = Vector.Abs(localNormal.Z); + // var useFaceX = Vector.BitwiseAnd(Vector.GreaterThan(absNormalX, absNormalY), Vector.GreaterThan(absNormalX, absNormalZ)); + // var useFaceY = Vector.AndNot(Vector.GreaterThan(absNormalY, absNormalZ), useFaceX); + // var faceLocalNormalComponent = Vector.ConditionalSelect(useFaceX, localNormal.X, Vector.ConditionalSelect(useFaceY, localNormal.Y, localNormal.Z)); + // var faceOffsetMagnitude = Vector.ConditionalSelect(useFaceX, a.HalfWidth, Vector.ConditionalSelect(useFaceY, a.HalfHeight, a.HalfLength)); + // var xOffset = Vector.ConditionalSelect(useFaceX, a.HalfHeight, Vector.ConditionalSelect(useFaceY, a.HalfLength, a.HalfWidth)); + // var yOffset = Vector.ConditionalSelect(useFaceX, a.HalfLength, Vector.ConditionalSelect(useFaceY, a.HalfWidth, a.HalfHeight)); + // var faceOffset = Vector.ConditionalSelect(Vector.LessThan(faceLocalNormalComponent, Vector.Zero), faceOffsetMagnitude, -faceOffsetMagnitude); + + // //Thanks to axis alignment, all the box component dot products squish down to component selections. + // var edgeABDotFaceOffset = faceOffset * Vector.ConditionalSelect(useFaceX, ab.X, Vector.ConditionalSelect(useFaceY, ab.Y, ab.Z)); + // var edgeBCDotFaceOffset = faceOffset * Vector.ConditionalSelect(useFaceX, bc.X, Vector.ConditionalSelect(useFaceY, bc.Y, bc.Z)); + // var edgeCADotFaceOffset = faceOffset * Vector.ConditionalSelect(useFaceX, ca.X, Vector.ConditionalSelect(useFaceY, ca.Y, ca.Z)); + // var edgeABDotBoxEdgeX = xOffset * Vector.ConditionalSelect(useFaceX, ab.Y, Vector.ConditionalSelect(useFaceY, ab.Z, ab.X)); + // var edgeBCDotBoxEdgeX = xOffset * Vector.ConditionalSelect(useFaceX, bc.Y, Vector.ConditionalSelect(useFaceY, bc.Z, bc.X)); + // var edgeCADotBoxEdgeX = xOffset * Vector.ConditionalSelect(useFaceX, ca.Y, Vector.ConditionalSelect(useFaceY, ca.Z, ca.X)); + // var edgeABDotBoxEdgeY = yOffset * Vector.ConditionalSelect(useFaceX, ab.Z, Vector.ConditionalSelect(useFaceY, ab.X, ab.Y)); + // var edgeBCDotBoxEdgeY = yOffset * Vector.ConditionalSelect(useFaceX, bc.Z, Vector.ConditionalSelect(useFaceY, bc.X, bc.Y)); + // var edgeCADotBoxEdgeY = yOffset * Vector.ConditionalSelect(useFaceX, ca.Z, Vector.ConditionalSelect(useFaceY, ca.X, ca.Y)); + // Vector3Wide.Dot(ab, vA, out var abDotA); + // Vector3Wide.Dot(bc, vB, out var bcDotB); + // Vector3Wide.Dot(ca, vC, out var caDotC); + + // var inverseLengthSquaredAB = Vector.One / (ab.X * ab.X + ab.Y * ab.Y + ab.Z * ab.Z); + // var inverseLengthSquaredBC = Vector.One / (bc.X * bc.X + bc.Y * bc.Y + bc.Z * bc.Z); + // var inverseLengthSquaredCA = Vector.One / (ca.X * ca.X + ca.Y * ca.Y + ca.Z * ca.Z); + // var abToFaceDot = edgeABDotFaceOffset - abDotA; + // var bcToFaceDot = edgeBCDotFaceOffset - bcDotB; + // var caToFaceDot = edgeCADotFaceOffset - caDotC; + // var tClosestOnAB00 = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (abToFaceDot - edgeABDotBoxEdgeX - edgeABDotBoxEdgeY) * inverseLengthSquaredAB)); + // var tClosestOnAB01 = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (abToFaceDot - edgeABDotBoxEdgeX + edgeABDotBoxEdgeY) * inverseLengthSquaredAB)); + // var tClosestOnAB10 = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (abToFaceDot + edgeABDotBoxEdgeX - edgeABDotBoxEdgeY) * inverseLengthSquaredAB)); + // var tClosestOnAB11 = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (abToFaceDot + edgeABDotBoxEdgeX + edgeABDotBoxEdgeY) * inverseLengthSquaredAB)); + // var tClosestOnBC00 = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (bcToFaceDot - edgeBCDotBoxEdgeX - edgeBCDotBoxEdgeY) * inverseLengthSquaredBC)); + // var tClosestOnBC01 = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (bcToFaceDot - edgeBCDotBoxEdgeX + edgeBCDotBoxEdgeY) * inverseLengthSquaredBC)); + // var tClosestOnBC10 = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (bcToFaceDot + edgeBCDotBoxEdgeX - edgeBCDotBoxEdgeY) * inverseLengthSquaredBC)); + // var tClosestOnBC11 = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (bcToFaceDot + edgeBCDotBoxEdgeX + edgeBCDotBoxEdgeY) * inverseLengthSquaredBC)); + // var tClosestOnCA00 = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (caToFaceDot - edgeCADotBoxEdgeX - edgeCADotBoxEdgeY) * inverseLengthSquaredCA)); + // var tClosestOnCA01 = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (caToFaceDot - edgeCADotBoxEdgeX + edgeCADotBoxEdgeY) * inverseLengthSquaredCA)); + // var tClosestOnCA10 = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (caToFaceDot + edgeCADotBoxEdgeX - edgeCADotBoxEdgeY) * inverseLengthSquaredCA)); + // var tClosestOnCA11 = Vector.Max(Vector.Zero, Vector.Min(Vector.One, (caToFaceDot + edgeCADotBoxEdgeX + edgeCADotBoxEdgeY) * inverseLengthSquaredCA)); + + // //We now have the t value of every box vertex on each triangle edge. + // //Find the closest pair. + // //offsetAB00 = a + ab * t00 - (faceOffset - edgeOffsetX - edgeOffsetY) + // var ab00X = vA.X + ab.X * tClosestOnAB00; + // var ab00Y = vA.Y + ab.Y * tClosestOnAB00; + // var ab00Z = vA.Z + ab.Z * tClosestOnAB00; + // var ab01X = vA.X + ab.X * tClosestOnAB01; + // var ab01Y = vA.Y + ab.Y * tClosestOnAB01; + // var ab01Z = vA.Z + ab.Z * tClosestOnAB01; + // var ab10X = vA.X + ab.X * tClosestOnAB10; + // var ab10Y = vA.Y + ab.Y * tClosestOnAB10; + // var ab10Z = vA.Z + ab.Z * tClosestOnAB10; + // var ab11X = vA.X + ab.X * tClosestOnAB11; + // var ab11Y = vA.Y + ab.Y * tClosestOnAB11; + // var ab11Z = vA.Z + ab.Z * tClosestOnAB11; + // ab00X = ab00X - Vector.Min(a.HalfWidth, Vector.Max(negativeHalfWidth, ab00X)); + // ab00Y = ab00Y - Vector.Min(a.HalfHeight, Vector.Max(negativeHalfHeight, ab00Y)); + // ab00Z = ab00Z - Vector.Min(a.HalfLength, Vector.Max(negativeHalfLength, ab00Z)); + // ab01X = ab01X - Vector.Min(a.HalfWidth, Vector.Max(negativeHalfWidth, ab01X)); + // ab01Y = ab01Y - Vector.Min(a.HalfHeight, Vector.Max(negativeHalfHeight, ab01Y)); + // ab01Z = ab01Z - Vector.Min(a.HalfLength, Vector.Max(negativeHalfLength, ab01Z)); + // ab10X = ab10X - Vector.Min(a.HalfWidth, Vector.Max(negativeHalfWidth, ab10X)); + // ab10Y = ab10Y - Vector.Min(a.HalfHeight, Vector.Max(negativeHalfHeight, ab10Y)); + // ab10Z = ab10Z - Vector.Min(a.HalfLength, Vector.Max(negativeHalfLength, ab10Z)); + // ab11X = ab11X - Vector.Min(a.HalfWidth, Vector.Max(negativeHalfWidth, ab11X)); + // ab11Y = ab11Y - Vector.Min(a.HalfHeight, Vector.Max(negativeHalfHeight, ab11Y)); + // ab11Z = ab11Z - Vector.Min(a.HalfLength, Vector.Max(negativeHalfLength, ab11Z)); + // var distanceSquaredAB00 = ab00X * ab00X + ab00Y * ab00Y + ab00Z * ab00Z; + // var distanceSquaredAB01 = ab01X * ab01X + ab01Y * ab01Y + ab01Z * ab01Z; + // var distanceSquaredAB10 = ab10X * ab10X + ab10Y * ab10Y + ab10Z * ab10Z; + // var distanceSquaredAB11 = ab11X * ab11X + ab11Y * ab11Y + ab11Z * ab11Z; + // distanceSquared = Vector.Min(distanceSquared, Vector.Min(Vector.Min(distanceSquaredAB00, distanceSquaredAB01), Vector.Min(distanceSquaredAB10, distanceSquaredAB11))); + // var useAB00 = Vector.Equals(distanceSquared, distanceSquaredAB00); + // var useAB01 = Vector.Equals(distanceSquared, distanceSquaredAB01); + // var useAB10 = Vector.Equals(distanceSquared, distanceSquaredAB10); + // var useAB11 = Vector.Equals(distanceSquared, distanceSquaredAB11); + // offsetX = Vector.ConditionalSelect(useAB00, ab00X, Vector.ConditionalSelect(useAB01, ab01X, Vector.ConditionalSelect(useAB10, ab10X, Vector.ConditionalSelect(useAB11, ab11X, offsetX)))); + // offsetY = Vector.ConditionalSelect(useAB00, ab00Y, Vector.ConditionalSelect(useAB01, ab01Y, Vector.ConditionalSelect(useAB10, ab10Y, Vector.ConditionalSelect(useAB11, ab11Y, offsetY)))); + // offsetZ = Vector.ConditionalSelect(useAB00, ab00Z, Vector.ConditionalSelect(useAB01, ab01Z, Vector.ConditionalSelect(useAB10, ab10Z, Vector.ConditionalSelect(useAB11, ab11Z, offsetZ)))); + + // var bc00X = vB.X + bc.X * tClosestOnBC00; + // var bc00Y = vB.Y + bc.Y * tClosestOnBC00; + // var bc00Z = vB.Z + bc.Z * tClosestOnBC00; + // var bc01X = vB.X + bc.X * tClosestOnBC01; + // var bc01Y = vB.Y + bc.Y * tClosestOnBC01; + // var bc01Z = vB.Z + bc.Z * tClosestOnBC01; + // var bc10X = vB.X + bc.X * tClosestOnBC10; + // var bc10Y = vB.Y + bc.Y * tClosestOnBC10; + // var bc10Z = vB.Z + bc.Z * tClosestOnBC10; + // var bc11X = vB.X + bc.X * tClosestOnBC11; + // var bc11Y = vB.Y + bc.Y * tClosestOnBC11; + // var bc11Z = vB.Z + bc.Z * tClosestOnBC11; + // bc00X = bc00X - Vector.Min(a.HalfWidth, Vector.Max(negativeHalfWidth, bc00X)); + // bc00Y = bc00Y - Vector.Min(a.HalfHeight, Vector.Max(negativeHalfHeight, bc00Y)); + // bc00Z = bc00Z - Vector.Min(a.HalfLength, Vector.Max(negativeHalfLength, bc00Z)); + // bc01X = bc01X - Vector.Min(a.HalfWidth, Vector.Max(negativeHalfWidth, bc01X)); + // bc01Y = bc01Y - Vector.Min(a.HalfHeight, Vector.Max(negativeHalfHeight, bc01Y)); + // bc01Z = bc01Z - Vector.Min(a.HalfLength, Vector.Max(negativeHalfLength, bc01Z)); + // bc10X = bc10X - Vector.Min(a.HalfWidth, Vector.Max(negativeHalfWidth, bc10X)); + // bc10Y = bc10Y - Vector.Min(a.HalfHeight, Vector.Max(negativeHalfHeight, bc10Y)); + // bc10Z = bc10Z - Vector.Min(a.HalfLength, Vector.Max(negativeHalfLength, bc10Z)); + // bc11X = bc11X - Vector.Min(a.HalfWidth, Vector.Max(negativeHalfWidth, bc11X)); + // bc11Y = bc11Y - Vector.Min(a.HalfHeight, Vector.Max(negativeHalfHeight, bc11Y)); + // bc11Z = bc11Z - Vector.Min(a.HalfLength, Vector.Max(negativeHalfLength, bc11Z)); + // var distanceSquaredBC00 = bc00X * bc00X + bc00Y * bc00Y + bc00Z * bc00Z; + // var distanceSquaredBC01 = bc01X * bc01X + bc01Y * bc01Y + bc01Z * bc01Z; + // var distanceSquaredBC10 = bc10X * bc10X + bc10Y * bc10Y + bc10Z * bc10Z; + // var distanceSquaredBC11 = bc11X * bc11X + bc11Y * bc11Y + bc11Z * bc11Z; + // distanceSquared = Vector.Min(distanceSquared, Vector.Min(Vector.Min(distanceSquaredBC00, distanceSquaredBC01), Vector.Min(distanceSquaredBC10, distanceSquaredBC11))); + // var useBC00 = Vector.Equals(distanceSquared, distanceSquaredBC00); + // var useBC01 = Vector.Equals(distanceSquared, distanceSquaredBC01); + // var useBC10 = Vector.Equals(distanceSquared, distanceSquaredBC10); + // var useBC11 = Vector.Equals(distanceSquared, distanceSquaredBC11); + // offsetX = Vector.ConditionalSelect(useBC00, bc00X, Vector.ConditionalSelect(useBC01, bc01X, Vector.ConditionalSelect(useBC10, bc10X, Vector.ConditionalSelect(useBC11, bc11X, offsetX)))); + // offsetY = Vector.ConditionalSelect(useBC00, bc00Y, Vector.ConditionalSelect(useBC01, bc01Y, Vector.ConditionalSelect(useBC10, bc10Y, Vector.ConditionalSelect(useBC11, bc11Y, offsetY)))); + // offsetZ = Vector.ConditionalSelect(useBC00, bc00Z, Vector.ConditionalSelect(useBC01, bc01Z, Vector.ConditionalSelect(useBC10, bc10Z, Vector.ConditionalSelect(useBC11, bc11Z, offsetZ)))); + + // var ca00X = vC.X + ca.X * tClosestOnCA00; + // var ca00Y = vC.Y + ca.Y * tClosestOnCA00; + // var ca00Z = vC.Z + ca.Z * tClosestOnCA00; + // var ca01X = vC.X + ca.X * tClosestOnCA01; + // var ca01Y = vC.Y + ca.Y * tClosestOnCA01; + // var ca01Z = vC.Z + ca.Z * tClosestOnCA01; + // var ca10X = vC.X + ca.X * tClosestOnCA10; + // var ca10Y = vC.Y + ca.Y * tClosestOnCA10; + // var ca10Z = vC.Z + ca.Z * tClosestOnCA10; + // var ca11X = vC.X + ca.X * tClosestOnCA11; + // var ca11Y = vC.Y + ca.Y * tClosestOnCA11; + // var ca11Z = vC.Z + ca.Z * tClosestOnCA11; + // ca00X = ca00X - Vector.Min(a.HalfWidth, Vector.Max(negativeHalfWidth, ca00X)); + // ca00Y = ca00Y - Vector.Min(a.HalfHeight, Vector.Max(negativeHalfHeight, ca00Y)); + // ca00Z = ca00Z - Vector.Min(a.HalfLength, Vector.Max(negativeHalfLength, ca00Z)); + // ca01X = ca01X - Vector.Min(a.HalfWidth, Vector.Max(negativeHalfWidth, ca01X)); + // ca01Y = ca01Y - Vector.Min(a.HalfHeight, Vector.Max(negativeHalfHeight, ca01Y)); + // ca01Z = ca01Z - Vector.Min(a.HalfLength, Vector.Max(negativeHalfLength, ca01Z)); + // ca10X = ca10X - Vector.Min(a.HalfWidth, Vector.Max(negativeHalfWidth, ca10X)); + // ca10Y = ca10Y - Vector.Min(a.HalfHeight, Vector.Max(negativeHalfHeight, ca10Y)); + // ca10Z = ca10Z - Vector.Min(a.HalfLength, Vector.Max(negativeHalfLength, ca10Z)); + // ca11X = ca11X - Vector.Min(a.HalfWidth, Vector.Max(negativeHalfWidth, ca11X)); + // ca11Y = ca11Y - Vector.Min(a.HalfHeight, Vector.Max(negativeHalfHeight, ca11Y)); + // ca11Z = ca11Z - Vector.Min(a.HalfLength, Vector.Max(negativeHalfLength, ca11Z)); + // var distanceSquaredCA00 = ca00X * ca00X + ca00Y * ca00Y + ca00Z * ca00Z; + // var distanceSquaredCA01 = ca01X * ca01X + ca01Y * ca01Y + ca01Z * ca01Z; + // var distanceSquaredCA10 = ca10X * ca10X + ca10Y * ca10Y + ca10Z * ca10Z; + // var distanceSquaredCA11 = ca11X * ca11X + ca11Y * ca11Y + ca11Z * ca11Z; + // distanceSquared = Vector.Min(distanceSquared, Vector.Min(Vector.Min(distanceSquaredCA00, distanceSquaredCA01), Vector.Min(distanceSquaredCA10, distanceSquaredCA11))); + // var useCA00 = Vector.Equals(distanceSquared, distanceSquaredCA00); + // var useCA01 = Vector.Equals(distanceSquared, distanceSquaredCA01); + // var useCA10 = Vector.Equals(distanceSquared, distanceSquaredCA10); + // var useCA11 = Vector.Equals(distanceSquared, distanceSquaredCA11); + // offsetX = Vector.ConditionalSelect(useCA00, ca00X, Vector.ConditionalSelect(useCA01, ca01X, Vector.ConditionalSelect(useCA10, ca10X, Vector.ConditionalSelect(useCA11, ca11X, offsetX)))); + // offsetY = Vector.ConditionalSelect(useCA00, ca00Y, Vector.ConditionalSelect(useCA01, ca01Y, Vector.ConditionalSelect(useCA10, ca10Y, Vector.ConditionalSelect(useCA11, ca11Y, offsetY)))); + // offsetZ = Vector.ConditionalSelect(useCA00, ca00Z, Vector.ConditionalSelect(useCA01, ca01Z, Vector.ConditionalSelect(useCA10, ca10Z, Vector.ConditionalSelect(useCA11, ca11Z, offsetZ)))); + + // var distance = Vector.SquareRoot(distanceSquared); + // var inverseDistance = new Vector(-1f) / distance; + // localNormalCandidate.X = offsetX * inverseDistance; + // localNormalCandidate.Y = offsetY * inverseDistance; + // localNormalCandidate.Z = offsetZ * inverseDistance; + // Vector3Wide.Length(localNormalCandidate, out var length); + // Vector3Wide.Dot(localNormalCandidate, vA, out var nVA); + // Vector3Wide.Dot(localNormalCandidate, vB, out var nVB); + // Vector3Wide.Dot(localNormalCandidate, vC, out var nVC); + // var extremeA = Vector.Abs(localNormalCandidate.X) * a.HalfWidth + Vector.Abs(localNormalCandidate.Y) * a.HalfHeight + Vector.Abs(localNormalCandidate.Z) * a.HalfLength; + // GetDepthForInterval(extremeA, nVA, nVB, nVC, out depthCandidate); + // //Guard against division by zero. + // depthCandidate = Vector.ConditionalSelect(Vector.GreaterThan(distanceSquared, new Vector(1e-6f)), depthCandidate, new Vector(float.MaxValue)); + // Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + //} //If the local normal points against the triangle normal, then it's on the backside and should not collide. diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs index 62f70338f..927f32ef6 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs @@ -328,40 +328,42 @@ public unsafe void Test( GetDepthForNormal(a.A, a.B, a.C, bA, bB, bC, faceNormalB, out var faceDepthB); Select(ref depth, ref localNormal, faceDepthB, faceNormalB); - - var tryVertexNormals = Vector.BitwiseAnd(Vector.LessThan(depth, Vector.Zero), allowContacts); Vector3Wide.LengthSquared(abA, out var abALengthSquared); Vector3Wide.LengthSquared(abB, out var abBLengthSquared); Vector3Wide.LengthSquared(caA, out var caALengthSquared); Vector3Wide.LengthSquared(caB, out var caBLengthSquared); - if (Vector.LessThanAny(tryVertexNormals, Vector.Zero)) - { - //Vertex normals are not required to determine penetration versus separation. They are only used to ensure correct separated speculative normals. - //This isn't strictly required for behavior in the general case, but MeshReduction depends on it. - Vector3Wide.LengthSquared(bcA, out var bcALengthSquared); - Vector3Wide.LengthSquared(bcB, out var bcBLengthSquared); - var inverseABALengthSquared = Vector.One / abALengthSquared; - var inverseBCALengthSquared = Vector.One / bcALengthSquared; - var inverseCAALengthSquared = Vector.One / caALengthSquared; - var inverseABBLengthSquared = Vector.One / abBLengthSquared; - var inverseBCBLengthSquared = Vector.One / bcBLengthSquared; - var inverseCABLengthSquared = Vector.One / caBLengthSquared; - var distanceSquared = new Vector(float.MaxValue); - Unsafe.SkipInit(out Vector3Wide offset); - TestVertexNormal(a.A, bA, bB, bC, abB, bcB, caB, inverseABBLengthSquared, inverseBCBLengthSquared, inverseCABLengthSquared, ref distanceSquared, ref offset); - TestVertexNormal(a.B, bA, bB, bC, abB, bcB, caB, inverseABBLengthSquared, inverseBCBLengthSquared, inverseCABLengthSquared, ref distanceSquared, ref offset); - TestVertexNormal(a.C, bA, bB, bC, abB, bcB, caB, inverseABBLengthSquared, inverseBCBLengthSquared, inverseCABLengthSquared, ref distanceSquared, ref offset); - TestVertexNormal(bA, a.A, a.B, a.C, abA, bcA, caA, inverseABALengthSquared, inverseBCALengthSquared, inverseCAALengthSquared, ref distanceSquared, ref offset); - TestVertexNormal(bB, a.A, a.B, a.C, abA, bcA, caA, inverseABALengthSquared, inverseBCALengthSquared, inverseCAALengthSquared, ref distanceSquared, ref offset); - TestVertexNormal(bC, a.A, a.B, a.C, abA, bcA, caA, inverseABALengthSquared, inverseBCALengthSquared, inverseCAALengthSquared, ref distanceSquared, ref offset); - - var distance = Vector.SquareRoot(distanceSquared); - Vector3Wide.Scale(offset, Vector.One / distance, out localNormalCandidate); - //Don't try to use distances that are so small that the resulting normal will be numerically bad. That's close enough to intersecting that the previous normals will handle it. - GetDepthForNormal(a.A, a.B, a.C, bA, bB, bC, localNormalCandidate, out depthCandidate); - depthCandidate = Vector.ConditionalSelect(Vector.GreaterThan(distance, new Vector(1e-7f)), depthCandidate, new Vector(float.MaxValue)); - Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); - } + //The following was created for MeshReduction when it demanded all contact normals be correct during separation. + //Other pairs don't have that requirement, and we ended modifying MeshReduction to be a little less picky. + //This remains for posterity because, hey, it works, and if you need it, there it is. + //var tryVertexNormals = Vector.BitwiseAnd(Vector.LessThan(depth, Vector.Zero), allowContacts); + //if (Vector.LessThanAny(tryVertexNormals, Vector.Zero)) + //{ + // //Vertex normals are not required to determine penetration versus separation. They are only used to ensure correct separated speculative normals. + // //This isn't strictly required for behavior in the general case, but MeshReduction depends on it. + // Vector3Wide.LengthSquared(bcA, out var bcALengthSquared); + // Vector3Wide.LengthSquared(bcB, out var bcBLengthSquared); + // var inverseABALengthSquared = Vector.One / abALengthSquared; + // var inverseBCALengthSquared = Vector.One / bcALengthSquared; + // var inverseCAALengthSquared = Vector.One / caALengthSquared; + // var inverseABBLengthSquared = Vector.One / abBLengthSquared; + // var inverseBCBLengthSquared = Vector.One / bcBLengthSquared; + // var inverseCABLengthSquared = Vector.One / caBLengthSquared; + // var distanceSquared = new Vector(float.MaxValue); + // Unsafe.SkipInit(out Vector3Wide offset); + // TestVertexNormal(a.A, bA, bB, bC, abB, bcB, caB, inverseABBLengthSquared, inverseBCBLengthSquared, inverseCABLengthSquared, ref distanceSquared, ref offset); + // TestVertexNormal(a.B, bA, bB, bC, abB, bcB, caB, inverseABBLengthSquared, inverseBCBLengthSquared, inverseCABLengthSquared, ref distanceSquared, ref offset); + // TestVertexNormal(a.C, bA, bB, bC, abB, bcB, caB, inverseABBLengthSquared, inverseBCBLengthSquared, inverseCABLengthSquared, ref distanceSquared, ref offset); + // TestVertexNormal(bA, a.A, a.B, a.C, abA, bcA, caA, inverseABALengthSquared, inverseBCALengthSquared, inverseCAALengthSquared, ref distanceSquared, ref offset); + // TestVertexNormal(bB, a.A, a.B, a.C, abA, bcA, caA, inverseABALengthSquared, inverseBCALengthSquared, inverseCAALengthSquared, ref distanceSquared, ref offset); + // TestVertexNormal(bC, a.A, a.B, a.C, abA, bcA, caA, inverseABALengthSquared, inverseBCALengthSquared, inverseCAALengthSquared, ref distanceSquared, ref offset); + + // var distance = Vector.SquareRoot(distanceSquared); + // Vector3Wide.Scale(offset, Vector.One / distance, out localNormalCandidate); + // //Don't try to use distances that are so small that the resulting normal will be numerically bad. That's close enough to intersecting that the previous normals will handle it. + // GetDepthForNormal(a.A, a.B, a.C, bA, bB, bC, localNormalCandidate, out depthCandidate); + // depthCandidate = Vector.ConditionalSelect(Vector.GreaterThan(distance, new Vector(1e-7f)), depthCandidate, new Vector(float.MaxValue)); + // Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate); + //} //Point the normal from B to A by convention. Vector3Wide.Subtract(localTriangleCenterB, localTriangleCenterA, out var centerAToCenterB); @@ -425,7 +427,6 @@ public unsafe void Test( //We will be working on the surface of triangleB, but we'd still like a 2d parameterization of the surface for contact reduction. //So, we'll create tangent axes from the edge and edge x normal. - //Vector3Wide.LengthSquared(abB, out var abBLengthSquared); Vector3Wide.Scale(abB, Vector.One / Vector.SquareRoot(abBLengthSquared), out var tangentBX); Vector3Wide.CrossWithoutOverlap(tangentBX, faceNormalB, out var tangentBY); @@ -476,9 +477,6 @@ public unsafe void Test( } //Create a scale-sensitive epsilon for comparisons based on the size of the involved shapes. This helps avoid varying behavior based on how large involved objects are. - //Vector3Wide.LengthSquared(abA, out var abALengthSquared); - //Vector3Wide.LengthSquared(caA, out var caALengthSquared); - //Vector3Wide.LengthSquared(caB, out var caBLengthSquared); var epsilonScale = Vector.SquareRoot(Vector.Min( Vector.Max(abALengthSquared, caALengthSquared), Vector.Max(abBLengthSquared, caBLengthSquared))); diff --git a/BepuPhysics/CollisionDetection/MeshReduction.cs b/BepuPhysics/CollisionDetection/MeshReduction.cs index a43487dea..4265c2abd 100644 --- a/BepuPhysics/CollisionDetection/MeshReduction.cs +++ b/BepuPhysics/CollisionDetection/MeshReduction.cs @@ -169,7 +169,7 @@ private static unsafe bool ShouldBlockNormal(in TestTriangle triangle, in Vector //Note that we are stricter about being on the edge than we were about being nearby. //That's because infringement checks require a normal infringement along every edge that the contact is on; //being too aggressive about edge classification would cause infringements to sometimes be ignored. - var negativeThreshold = triangle.DistanceThreshold * -1e-1f; + var negativeThreshold = triangle.DistanceThreshold * -1e-2f; var onAB = distanceAlongNormal.Y >= negativeThreshold; var onBC = distanceAlongNormal.Z >= negativeThreshold; var onCA = distanceAlongNormal.W >= negativeThreshold; @@ -267,7 +267,7 @@ public unsafe static void ReduceManifolds(ref Buffer continuationTrian //++activeChildCount; } } - var allocatedCount = count * 3; + var allocatedCount = count * 10; var debugOverlapMemory = stackalloc int[allocatedCount]; var debugOverlapBuffer = new Buffer(debugOverlapMemory, allocatedCount); var debugKeyMemory = stackalloc int[allocatedCount]; @@ -360,7 +360,7 @@ public unsafe static void ReduceManifolds(ref Buffer continuationTrian if (ShouldBlockNormal(targetTriangle, meshSpaceContact, meshSpaceNormal)) { - if (targetTriangle.ChildIndex != sourceTriangle.ChildIndex) + //if (targetTriangle.ChildIndex != sourceTriangle.ChildIndex) { sourceTriangle.Blocked = true; sourceTriangle.CorrectedNormal = new Vector3(targetTriangle.NX.X, targetTriangle.NY.X, targetTriangle.NZ.X); @@ -374,7 +374,7 @@ public unsafe static void ReduceManifolds(ref Buffer continuationTrian for (int j = 0; j < count; ++j) { //No point in trying to check a normal against its own triangle. - if (i != j) + //if (i != j) { ref var targetTriangle = ref activeTriangles[j]; if (ShouldBlockNormal(targetTriangle, meshSpaceContact, meshSpaceNormal)) @@ -448,8 +448,9 @@ public unsafe static void ReduceManifolds(ref Buffer continuationTrian //case- consider what happens when an objects wedges itself into an edge between two triangles. Matrix3x3.Transform(requiresFlip ? triangle.CorrectedNormal : -triangle.CorrectedNormal, meshOrientation, out manifold.Normal); //Note that we do not modify the depth. - //The only time this situation should occur is when an object has somehow wedged between adjacent triangles such that the detected - //depths are *less* than the triangle face depths. So, using those depths is guaranteed not to introduce excessive energy. + //The only times this situation should occur is when either 1) an object has somehow wedged between adjacent triangles such that the detected + //depths are *less* than the triangle face depths, or 2) a source triangle generated an internal contact, and the face depth is guaranteed to be less. + //So, using those depths is guaranteed not to introduce excessive energy. } else { From cad4959b9ff4d95d47bec37910e54d7f4d16735d Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 23 Sep 2021 20:14:06 -0500 Subject: [PATCH 013/947] Incremental cleanup of debug stuff. --- BepuPhysics/BoundingBoxHelpers.cs | 6 - .../CollisionDetection/MeshReduction.cs | 132 ++++++++---------- .../SpecializedTests/BatchedCollisionTests.cs | 74 +++++----- 3 files changed, 92 insertions(+), 120 deletions(-) diff --git a/BepuPhysics/BoundingBoxHelpers.cs b/BepuPhysics/BoundingBoxHelpers.cs index 2120a08fc..5929164ae 100644 --- a/BepuPhysics/BoundingBoxHelpers.cs +++ b/BepuPhysics/BoundingBoxHelpers.cs @@ -113,9 +113,6 @@ public static void ExpandBoundingBoxes(ref Vector3Wide min, ref Vector3Wide max, //The maximum expansion passed into this function is the speculative margin for discrete mode collidables, and ~infinity for passive or continuous ones. Vector3Wide.Max(-maximumExpansion, minDisplacement, out minDisplacement); Vector3Wide.Min(maximumExpansion, maxDisplacement, out maxDisplacement); - Vector3Wide.Broadcast(new Vector3(5), out var debug); - Vector3Wide.Subtract(minDisplacement, debug, out minDisplacement); - Vector3Wide.Add(maxDisplacement, debug, out maxDisplacement); Vector3Wide.Add(min, minDisplacement, out min); Vector3Wide.Add(max, maxDisplacement, out max); @@ -200,9 +197,6 @@ public static unsafe void ExpandLocalBoundingBoxes(ref Vector3Wide min, ref Vect //Clamp the expansion to the pair imposed limit. Discrete pairs don't need to look beyond their speculative margin. Vector3Wide.Min(maximumAllowedExpansion, maxExpansion, out maxExpansion); Vector3Wide.Max(-maximumAllowedExpansion, minExpansion, out minExpansion); - Vector3Wide.Broadcast(new Vector3(5), out var debug); - Vector3Wide.Subtract(minExpansion, debug, out minExpansion); - Vector3Wide.Add(maxExpansion, debug, out maxExpansion); Vector3Wide.Add(minExpansion, min, out min); Vector3Wide.Add(maxExpansion, max, out max); diff --git a/BepuPhysics/CollisionDetection/MeshReduction.cs b/BepuPhysics/CollisionDetection/MeshReduction.cs index 4265c2abd..7f37eb038 100644 --- a/BepuPhysics/CollisionDetection/MeshReduction.cs +++ b/BepuPhysics/CollisionDetection/MeshReduction.cs @@ -254,18 +254,13 @@ public unsafe static void ReduceManifolds(ref Buffer continuationTrian //Narrow the region of interest. continuationTriangles.Slice(start, count, out var triangles); continuationChildren.Slice(start, count, out var children); - //Allocate enough space for all potential triangles, even though we're only going to be enumerating over the subset which actually have contacts. + //Allocate enough space for all potential triangles. //Note that the count is limited by the above early-out; there are limits to how much this can allocate on the stack. - //int activeChildCount = 0; var memory = stackalloc TestTriangle[count]; var activeTriangles = new Buffer(memory, count); for (int i = 0; i < count; ++i) { - //if (children[i].Manifold.Count > 0) - { - activeTriangles[i] = new TestTriangle(triangles[i], i); - //++activeChildCount; - } + activeTriangles[i] = new TestTriangle(triangles[i], i); } var allocatedCount = count * 10; var debugOverlapMemory = stackalloc int[allocatedCount]; @@ -309,82 +304,65 @@ public unsafe static void ReduceManifolds(ref Buffer continuationTrian } } ref var meshSpaceContact = ref meshSpaceContacts[deepestIndex]; + //var debugMin = meshSpaceContact - new Vector3(1e-3f); + //var debugMax = meshSpaceContact + new Vector3(1e-3f); + //var debugLeafEnumerator = new DebugLeafEnumerator(); + //debugLeafEnumerator.List = new QuickList(debugOverlapBuffer); + //debugMesh.Tree.GetOverlaps(debugMin, debugMax, ref debugLeafEnumerator); + //for (int j = 0; j < debugLeafEnumerator.List.Count; ++j) + //{ + // TestTriangle targetTriangle; + // if (!testTriangles.TryGetValue(debugLeafEnumerator.List[j], out targetTriangle)) + // { + // //Search for the child... + // int childIndexForTargetTriangle = -1; + // for (int k = 0; k < children.Length; ++k) + // { + // if (children[k].ChildIndexB == debugLeafEnumerator.List[j]) + // { + // childIndexForTargetTriangle = k; + // break; + // } + // } + // if (childIndexForTargetTriangle == -1) + // { + // Console.WriteLine("Bad news bears: a child index that should have existed by query does not exist according to the mesh reduction child set."); + // } + // targetTriangle = new TestTriangle(debugMesh.Triangles[debugLeafEnumerator.List[j]], childIndexForTargetTriangle); + // testTriangles.AddUnsafely(debugLeafEnumerator.List[j], targetTriangle); + // } - { - ref var debugSourceTriangle = ref debugMesh.Triangles[sourceChild.ChildIndexB]; - var inverse = 1f / Vector3.Cross(debugSourceTriangle.B - debugSourceTriangle.A, debugSourceTriangle.C - debugSourceTriangle.A).Length(); - var wa = Vector3.Cross(debugSourceTriangle.B - debugSourceTriangle.A, meshSpaceContact - debugSourceTriangle.A).Length() * inverse; - var wb = Vector3.Cross(debugSourceTriangle.C - debugSourceTriangle.B, meshSpaceContact - debugSourceTriangle.B).Length() * inverse; - var wc = Vector3.Cross(debugSourceTriangle.A - debugSourceTriangle.C, meshSpaceContact - debugSourceTriangle.C).Length() * inverse; - if (wa + wb + wc > 1 + 1e-3f) - Console.WriteLine($"EXTERNAL source contact {wa}, {wb}, {wc}"); - else if (wa > 1e-3f && wb > 1e-3f && wc > 1e-3f) - Console.WriteLine($"Internal source contact {wa}, {wb}, {wc}"); - } - - var debugMin = meshSpaceContact - new Vector3(1e-3f); - var debugMax = meshSpaceContact + new Vector3(1e-3f); - var debugLeafEnumerator = new DebugLeafEnumerator(); - debugLeafEnumerator.List = new QuickList(debugOverlapBuffer); - debugMesh.Tree.GetOverlaps(debugMin, debugMax, ref debugLeafEnumerator); - for (int j = 0; j < debugLeafEnumerator.List.Count; ++j) - { - TestTriangle targetTriangle; - if (!testTriangles.TryGetValue(debugLeafEnumerator.List[j], out targetTriangle)) - { - //Search for the child... - int childIndexForTargetTriangle = -1; - for (int k = 0; k < children.Length; ++k) - { - if (children[k].ChildIndexB == debugLeafEnumerator.List[j]) - { - childIndexForTargetTriangle = k; - break; - } - } - if (childIndexForTargetTriangle == -1) - { - Console.WriteLine("Bad news bears: a child index that should have existed by query does not exist according to the mesh reduction child set."); - } - targetTriangle = new TestTriangle(debugMesh.Triangles[debugLeafEnumerator.List[j]], childIndexForTargetTriangle); - testTriangles.AddUnsafely(debugLeafEnumerator.List[j], targetTriangle); - } - - { - ref var debugTriangle = ref debugMesh.Triangles[debugLeafEnumerator.List[j]]; - var inverse = 1f / Vector3.Cross(debugTriangle.B - debugTriangle.A, debugTriangle.C - debugTriangle.A).Length(); - var wa = Vector3.Cross(debugTriangle.B - debugTriangle.A, meshSpaceContact - debugTriangle.A).Length() * inverse; - var wb = Vector3.Cross(debugTriangle.C - debugTriangle.B, meshSpaceContact - debugTriangle.B).Length() * inverse; - var wc = Vector3.Cross(debugTriangle.A - debugTriangle.C, meshSpaceContact - debugTriangle.C).Length() * inverse; - } + // { + // ref var debugTriangle = ref debugMesh.Triangles[debugLeafEnumerator.List[j]]; + // var inverse = 1f / Vector3.Cross(debugTriangle.B - debugTriangle.A, debugTriangle.C - debugTriangle.A).Length(); + // var wa = Vector3.Cross(debugTriangle.B - debugTriangle.A, meshSpaceContact - debugTriangle.A).Length() * inverse; + // var wb = Vector3.Cross(debugTriangle.C - debugTriangle.B, meshSpaceContact - debugTriangle.B).Length() * inverse; + // var wc = Vector3.Cross(debugTriangle.A - debugTriangle.C, meshSpaceContact - debugTriangle.C).Length() * inverse; + // } - if (ShouldBlockNormal(targetTriangle, meshSpaceContact, meshSpaceNormal)) - { - //if (targetTriangle.ChildIndex != sourceTriangle.ChildIndex) - { - sourceTriangle.Blocked = true; - sourceTriangle.CorrectedNormal = new Vector3(targetTriangle.NX.X, targetTriangle.NY.X, targetTriangle.NZ.X); - //Even if the target manifold gets blocked, it should not necessarily be deleted. We made use of it as a blocker. - targetTriangle.ForceDeletionOnBlock = false; - break; - } - } - } + // if (ShouldBlockNormal(targetTriangle, meshSpaceContact, meshSpaceNormal)) + // { + // //if (targetTriangle.ChildIndex != sourceTriangle.ChildIndex) + // { + // sourceTriangle.Blocked = true; + // sourceTriangle.CorrectedNormal = new Vector3(targetTriangle.NX.X, targetTriangle.NY.X, targetTriangle.NZ.X); + // //Even if the target manifold gets blocked, it should not necessarily be deleted. We made use of it as a blocker. + // targetTriangle.ForceDeletionOnBlock = false; + // break; + // } + // } + //} for (int j = 0; j < count; ++j) { - //No point in trying to check a normal against its own triangle. - //if (i != j) + ref var targetTriangle = ref activeTriangles[j]; + if (ShouldBlockNormal(targetTriangle, meshSpaceContact, meshSpaceNormal)) { - ref var targetTriangle = ref activeTriangles[j]; - if (ShouldBlockNormal(targetTriangle, meshSpaceContact, meshSpaceNormal)) - { - sourceTriangle.Blocked = true; - sourceTriangle.CorrectedNormal = new Vector3(targetTriangle.NX.X, targetTriangle.NY.X, targetTriangle.NZ.X); - //Even if the target manifold gets blocked, it should not necessarily be deleted. We made use of it as a blocker. - targetTriangle.ForceDeletionOnBlock = false; - break; - } + sourceTriangle.Blocked = true; + sourceTriangle.CorrectedNormal = new Vector3(targetTriangle.NX.X, targetTriangle.NY.X, targetTriangle.NZ.X); + //Even if the target manifold gets blocked, it should not necessarily be deleted. We made use of it as a blocker. + targetTriangle.ForceDeletionOnBlock = false; + break; } } //Note that the removal had to be deferred until after blocking analysis. diff --git a/Demos/SpecializedTests/BatchedCollisionTests.cs b/Demos/SpecializedTests/BatchedCollisionTests.cs index 66876859d..58d6ce4dd 100644 --- a/Demos/SpecializedTests/BatchedCollisionTests.cs +++ b/Demos/SpecializedTests/BatchedCollisionTests.cs @@ -185,44 +185,44 @@ public static void Test() } - //Test(ref sphere, ref sphere, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - //Test(ref sphere, ref capsule, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - //Test(ref sphere, ref box, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - //Test(ref sphere, ref triangle, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - //Test(ref sphere, ref cylinder, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - //Test(ref sphere, ref convexHull, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - //Test(ref capsule, ref capsule, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - //Test(ref capsule, ref box, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - //Test(ref capsule, ref triangle, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - //Test(ref capsule, ref cylinder, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - //Test(ref capsule, ref convexHull, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - //Test(ref box, ref box, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - //Test(ref box, ref triangle, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - //Test(ref box, ref cylinder, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - //Test(ref box, ref convexHull, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref sphere, ref sphere, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref sphere, ref capsule, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref sphere, ref box, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref sphere, ref triangle, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref sphere, ref cylinder, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref sphere, ref convexHull, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref capsule, ref capsule, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref capsule, ref box, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref capsule, ref triangle, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref capsule, ref cylinder, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref capsule, ref convexHull, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref box, ref box, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref box, ref triangle, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref box, ref cylinder, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref box, ref convexHull, ref posesA, ref posesB, pool, shapes, registry, iterationCount); Test(ref triangle, ref triangle, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - //Test(ref triangle, ref cylinder, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - //Test(ref triangle, ref convexHull, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - //Test(ref cylinder, ref cylinder, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - //Test(ref cylinder, ref convexHull, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - //Test(ref convexHull, ref convexHull, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - - - //Test(sphere, sphere, ref posesA, ref posesB, iterationCount); - //Test(sphere, capsule, ref posesA, ref posesB, iterationCount); - //Test(sphere, box, ref posesA, ref posesB, iterationCount); - //Test(sphere, triangle, ref posesA, ref posesB, iterationCount); - //Test(sphere, cylinder, ref posesA, ref posesB, iterationCount); - //Test(capsule, capsule, ref posesA, ref posesB, iterationCount); - //Test(capsule, box, ref posesA, ref posesB, iterationCount); - //Test>(capsule, triangle, ref posesA, ref posesB, iterationCount); - //Test>(capsule, cylinder, ref posesA, ref posesB, iterationCount); - //Test>(cylinder, box, ref posesA, ref posesB, iterationCount); - //Test>(cylinder, triangle, ref posesA, ref posesB, iterationCount); - //Test>(cylinder, cylinder, ref posesA, ref posesB, iterationCount); - //Test>(box, box, ref posesA, ref posesB, iterationCount); - //Test>(box, triangle, ref posesA, ref posesB, iterationCount); - //Test>(triangle, triangle, ref posesA, ref posesB, iterationCount); + Test(ref triangle, ref cylinder, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref triangle, ref convexHull, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref cylinder, ref cylinder, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref cylinder, ref convexHull, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref convexHull, ref convexHull, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + + + Test(sphere, sphere, ref posesA, ref posesB, iterationCount); + Test(sphere, capsule, ref posesA, ref posesB, iterationCount); + Test(sphere, box, ref posesA, ref posesB, iterationCount); + Test(sphere, triangle, ref posesA, ref posesB, iterationCount); + Test(sphere, cylinder, ref posesA, ref posesB, iterationCount); + Test(capsule, capsule, ref posesA, ref posesB, iterationCount); + Test(capsule, box, ref posesA, ref posesB, iterationCount); + Test>(capsule, triangle, ref posesA, ref posesB, iterationCount); + Test>(capsule, cylinder, ref posesA, ref posesB, iterationCount); + Test>(cylinder, box, ref posesA, ref posesB, iterationCount); + Test>(cylinder, triangle, ref posesA, ref posesB, iterationCount); + Test>(cylinder, cylinder, ref posesA, ref posesB, iterationCount); + Test>(box, box, ref posesA, ref posesB, iterationCount); + Test>(box, triangle, ref posesA, ref posesB, iterationCount); + Test>(triangle, triangle, ref posesA, ref posesB, iterationCount); Console.WriteLine($"Done. Hit enter to exit."); Console.ReadLine(); } From e08d0613aff2fdc26b112a489b5b4823ce3f39b8 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 23 Sep 2021 22:05:09 -0500 Subject: [PATCH 014/947] Trying a shape-aware MeshReduction. --- .../CompoundMeshContinuations.cs | 2 + .../CollisionTasks/ConvexMeshContinuations.cs | 5 +- .../CompoundMeshReduction.cs | 6 +- .../CollisionDetection/MeshReduction.cs | 377 +++++++++--------- 4 files changed, 208 insertions(+), 182 deletions(-) diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundMeshContinuations.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundMeshContinuations.cs index 0620f2874..155d251b9 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundMeshContinuations.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundMeshContinuations.cs @@ -23,6 +23,8 @@ public ref CompoundMeshReduction CreateContinuation( collisionBatcher.Pool.Take(pairOverlaps.Length, out continuation.QueryBounds); continuation.RegionCount = pairOverlaps.Length; continuation.MeshOrientation = pair.OrientationB; + //TODO: This is not flexible with respect to different mesh types. Not a problem right now, but it will be in the future. + continuation.Mesh = (Mesh*)pair.B; //A flip is required in mesh reduction whenever contacts are being generated as if the triangle is in slot B, which is whenever this pair has *not* been flipped. continuation.RequiresFlip = pair.FlipMask == 0; diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexMeshContinuations.cs b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexMeshContinuations.cs index 3a9b73349..3d968c8b4 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexMeshContinuations.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexMeshContinuations.cs @@ -8,7 +8,7 @@ public struct ConvexMeshContinuations : IConvexCompoundContinuationHandle public CollisionContinuationType CollisionContinuationType => CollisionContinuationType.MeshReduction; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref MeshReduction CreateContinuation( + public unsafe ref MeshReduction CreateContinuation( ref CollisionBatcher collisionBatcher, int childCount, in BoundsTestedPair pair, in OverlapQueryForPair pairQuery, out int continuationIndex) where TCallbacks : struct, ICollisionCallbacks { @@ -20,7 +20,8 @@ public ref MeshReduction CreateContinuation( continuation.RequiresFlip = pair.FlipMask == 0; continuation.QueryBounds.Min = pairQuery.Min; continuation.QueryBounds.Max = pairQuery.Max; - unsafe { continuation.DebugMesh = Unsafe.AsRef(pairQuery.Container); } + //TODO: This is not flexible with respect to different mesh types. Not a problem right now, but it will be in the future. + continuation.Mesh = pairQuery.Container; return ref continuation; } diff --git a/BepuPhysics/CollisionDetection/CompoundMeshReduction.cs b/BepuPhysics/CollisionDetection/CompoundMeshReduction.cs index cea81e3aa..e13d5f53e 100644 --- a/BepuPhysics/CollisionDetection/CompoundMeshReduction.cs +++ b/BepuPhysics/CollisionDetection/CompoundMeshReduction.cs @@ -11,7 +11,7 @@ namespace BepuPhysics.CollisionDetection { - public struct CompoundMeshReduction : ICollisionTestContinuation + public unsafe struct CompoundMeshReduction : ICollisionTestContinuation { public int RegionCount; public Buffer<(int Start, int Count)> ChildManifoldRegions; @@ -25,6 +25,8 @@ public struct CompoundMeshReduction : ICollisionTestContinuation //This uses all of the nonconvex reduction's logic, so we just nest it. public NonconvexReduction Inner; + public Mesh* Mesh; //TODO: This is not flexible with respect to different mesh types. Not a problem right now, but it will be in the future. + public void Create(int childManifoldCount, BufferPool pool) { Inner.Create(childManifoldCount, pool); @@ -56,7 +58,7 @@ public unsafe bool TryFlush(int pairId, ref CollisionBatcher 0) { - MeshReduction.ReduceManifolds(ref Triangles, ref Inner.Children, region.Start, region.Count, RequiresFlip, QueryBounds[i], meshOrientation, meshInverseOrientation, default); + MeshReduction.ReduceManifolds(ref Triangles, ref Inner.Children, region.Start, region.Count, RequiresFlip, QueryBounds[i], meshOrientation, meshInverseOrientation, Mesh, batcher.Pool); } } diff --git a/BepuPhysics/CollisionDetection/MeshReduction.cs b/BepuPhysics/CollisionDetection/MeshReduction.cs index 7f37eb038..c6fff064b 100644 --- a/BepuPhysics/CollisionDetection/MeshReduction.cs +++ b/BepuPhysics/CollisionDetection/MeshReduction.cs @@ -12,7 +12,7 @@ namespace BepuPhysics.CollisionDetection { - public struct MeshReduction : ICollisionTestContinuation + public unsafe struct MeshReduction : ICollisionTestContinuation { /// /// Flag used to mark a contact as being generated by the face of a triangle in its feature id. @@ -32,7 +32,7 @@ public struct MeshReduction : ICollisionTestContinuation //This uses all of the nonconvex reduction's logic, so we just nest it. public NonconvexReduction Inner; - public Mesh DebugMesh; + public void* Mesh; //TODO: This is not flexible with respect to different mesh types. Not a problem right now, but it will be in the future. public void Create(int childManifoldCount, BufferPool pool) { @@ -53,25 +53,38 @@ public void OnChildCompletedEmpty(ref PairContinuation report, ref C } [MethodImpl(MethodImplOptions.AggressiveInlining)] - private static unsafe void ComputeMeshSpaceContacts(ref ConvexContactManifold manifold, in Matrix3x3 inverseMeshOrientation, bool requiresFlip, Vector3* meshSpaceContacts, out Vector3 meshSpaceNormal) + private static unsafe void ComputeMeshSpaceContact(ref ConvexContactManifold manifold, in Matrix3x3 inverseMeshOrientation, bool requiresFlip, out Vector3 meshSpaceContact, out Vector3 meshSpaceNormal) { + //Select the deepest contact out of the manifold. Our goal is to find a contact on the representative feature of the source triangle. + //Recall that triangle collision tests will generate speculative contacts elsewhere on the triangle, both on the face and potentially on edges + //other than the deepest edge. + //The *normal*, however, is most directly associated with the deepest contact. The fact that the normal is 'infringing' on some other edge doesn't really matter. + //(Why doesn't it matter? MeshReduction operates on single convex-mesh pairs at a time. The *convex* shape cannot generate genuinely infringing contacts on two sides of a triangle at once. + //The opposing edge's contact will actually point *away* from that edge toward the interior of the source triangle. For the same reason that we never block face contacts, it doesn't make sense to + //block based on those incidental contacts.) + //This is equivalent to using the normal to determine the manifold voronoi region, except the contact position lets us deal with more arbitrary content. + var deepestIndex = 0; + var deepestDepth = manifold.Contact0.Depth; + for (int j = 1; j < manifold.Count; ++j) + { + var depth = Unsafe.Add(ref manifold.Contact0, j).Depth; + if (deepestDepth < depth) + { + deepestDepth = depth; + deepestIndex = j; + } + } //First, if the manifold considers the mesh and its triangles to be shape B, then we need to flip it. if (requiresFlip) { //If the manifold considers the mesh and its triangles to be shape B, it needs to be flipped before being transformed. - for (int i = 0; i < manifold.Count; ++i) - { - Matrix3x3.Transform(Unsafe.Add(ref manifold.Contact0, i).Offset - manifold.OffsetB, inverseMeshOrientation, out meshSpaceContacts[i]); - } + Matrix3x3.Transform(Unsafe.Add(ref manifold.Contact0, deepestIndex).Offset - manifold.OffsetB, inverseMeshOrientation, out meshSpaceContact); Matrix3x3.Transform(-manifold.Normal, inverseMeshOrientation, out meshSpaceNormal); } else { //No flip required. - for (int i = 0; i < manifold.Count; ++i) - { - Matrix3x3.Transform(Unsafe.Add(ref manifold.Contact0, i).Offset, inverseMeshOrientation, out meshSpaceContacts[i]); - } + Matrix3x3.Transform(Unsafe.Add(ref manifold.Contact0, deepestIndex).Offset, inverseMeshOrientation, out meshSpaceContact); Matrix3x3.Transform(manifold.Normal, inverseMeshOrientation, out meshSpaceNormal); } } @@ -208,18 +221,80 @@ private static unsafe bool ShouldBlockNormal(in TestTriangle triangle, in Vector return false; } - struct DebugLeafEnumerator : IBreakableForEach + //static void RemoveContacts() + //{ + // //Note that the removal had to be deferred until after blocking analysis. + // //This manifold will not be considered for the remainder of this loop, so modifying it is fine. + // for (int j = sourceChild.Manifold.Count - 1; j >= 0; --j) + // { + // //If a contact is outside of the mesh space bounding box that found the triangles to test, then two things are true: + // //1) The contact is almost certainly not productive; the bounding box included a frame of integrated motion and this contact was outside of it. + // //2) The contact may have been created with a triangle whose neighbor was not in the query bounds, and so the neighbor won't contribute any blocking. + // //The result is that such contacts have a tendency to cause ghost collisions. We'd rather not force the use of very small speculative margins, + // //so instead we explicitly kill off contacts which are outside the queried bounds. + // ref var contactToCheck = ref meshSpaceContacts[j]; + // if (Vector3.Min(contactToCheck, queryBounds.Min) != queryBounds.Min || + // Vector3.Max(contactToCheck, queryBounds.Max) != queryBounds.Max) + // { + // ConvexContactManifold.FastRemoveAt(ref sourceChild.Manifold, j); + // } + // } + //} + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + static void TryApplyBlockToTriangle(ref TestTriangle triangle, Buffer children, in Matrix3x3 meshOrientation, bool requiresFlip) + { + if (triangle.Blocked) + { + ref var manifold = ref children[triangle.ChildIndex].Manifold; + if (triangle.ForceDeletionOnBlock) + { + //The manifold was infringing, and no other manifold infringed upon it. Can safely just ignore the manifold completely. + manifold.Count = 0; + } + else + { + var manifoldHasPositiveDepth = false; + for (int j = 0; j < manifold.Count; ++j) + { + if (Unsafe.Add(ref manifold.Contact0, j).Depth > 0) + { + manifoldHasPositiveDepth = true; + break; + } + } + if (manifoldHasPositiveDepth) + { + //The manifold was infringing, but another manifold was infringing upon it. We can't safely delete such a manifold since it's likely a mutually infringing + //case- consider what happens when an objects wedges itself into an edge between two triangles. + Matrix3x3.Transform(requiresFlip ? triangle.CorrectedNormal : -triangle.CorrectedNormal, meshOrientation, out manifold.Normal); + //Note that we do not modify the depth. + //The only times this situation should occur is when either 1) an object has somehow wedged between adjacent triangles such that the detected + //depths are *less* than the triangle face depths, or 2) a source triangle generated an internal contact, and the face depth is guaranteed to be less. + //So, using those depths is guaranteed not to introduce excessive energy. + } + else + { + //The manifold has zero or negative depth; it's clearly not a case where a shape is wedged between triangles. Just get rid of it. + manifold.Count = 0; + } + } + } + } + + struct ChildEnumerator : IBreakableForEach { public QuickList List; + public BufferPool Pool; public bool LoopBody(int i) { - List.AllocateUnsafely() = i; + List.Allocate(Pool) = i; return true; } } public unsafe static void ReduceManifolds(ref Buffer continuationTriangles, ref Buffer continuationChildren, int start, int count, - bool requiresFlip, in BoundingBox queryBounds, in Matrix3x3 meshOrientation, in Matrix3x3 meshInverseOrientation, Mesh debugMesh) + bool requiresFlip, in BoundingBox queryBounds, in Matrix3x3 meshOrientation, in Matrix3x3 meshInverseOrientation, Mesh* mesh, BufferPool pool) { //Before handing responsibility off to the nonconvex reduction, make sure that no contacts create nasty 'bumps' at the border of triangles. //Bumps can occur when an isolated triangle test detects a contact pointing outward, like when a box hits the side. This is fine when the triangle truly is isolated, @@ -241,203 +316,149 @@ public unsafe static void ReduceManifolds(ref Buffer continuationTrian //Contacts generated by face collisions are marked with a special feature id flag. If it is present, we can skip the contact. The collision tester also provided unique feature ids //beyond that flag, so we can strip the flag now. (We effectively just hijacked the feature id to store some temporary metadata.) - //TODO: Note that we perform contact correction prior to reduction. Reduction depends on normals to compute its 'distinctiveness' heuristic. - //You could sacrifice a little bit of reduction quality for faster contact correction (since reduction outputs a low fixed number of contacts), but - //we should only pursue that if contact correction is a meaningful cost. + //If you don't want to run mesh reduction at all for sufficiently complex pairs, you could simply early out here like so: + //if (count > 1024) + // return; - //Note that we don't bother performing any reduction on pairs that have pathological numbers of triangles. - //The current quadratic scaling behavior of this reduction can be explosively bad as the count rises into the thousands. - //Ideally we'll do https://github.com/bepu/bepuphysics2/issues/66 so this will become a nonissue. - //Until then, attempting to reduce absurdo-manifolds is likely misguided. Better to have some bumps than a multi-second hang. - if (count > 1024) - return; //Narrow the region of interest. continuationTriangles.Slice(start, count, out var triangles); continuationChildren.Slice(start, count, out var children); - //Allocate enough space for all potential triangles. - //Note that the count is limited by the above early-out; there are limits to how much this can allocate on the stack. - var memory = stackalloc TestTriangle[count]; - var activeTriangles = new Buffer(memory, count); - for (int i = 0; i < count; ++i) + const int bruteForceThreshold = 32; + if (false)//count < bruteForceThreshold) { - activeTriangles[i] = new TestTriangle(triangles[i], i); - } - var allocatedCount = count * 10; - var debugOverlapMemory = stackalloc int[allocatedCount]; - var debugOverlapBuffer = new Buffer(debugOverlapMemory, allocatedCount); - var debugKeyMemory = stackalloc int[allocatedCount]; - var debugKeys = new Buffer(debugKeyMemory, allocatedCount); - var debugValueMemory = stackalloc TestTriangle[allocatedCount]; - var debugValues = new Buffer(debugValueMemory, allocatedCount); - var debugTableSize = 1 << SpanHelper.GetContainingPowerOf2(allocatedCount * 4); - var debugTableMemory = stackalloc int[debugTableSize]; - var debugTable = new Buffer(debugTableMemory, debugTableSize); - QuickDictionary> testTriangles = new(ref debugKeys, ref debugValues, ref debugTable); - var meshSpaceContacts = stackalloc Vector3[4]; - for (int i = 0; i < count; ++i) - { - ref var sourceTriangle = ref activeTriangles[i]; - ref var sourceChild = ref children[sourceTriangle.ChildIndex]; - if (sourceChild.Manifold.Count == 0) - continue; - //Can't correct contacts that were created by face collisions. - if ((sourceChild.Manifold.Contact0.FeatureId & FaceCollisionFlag) == 0) + var memory = stackalloc TestTriangle[count]; + var activeTriangles = new Buffer(memory, count); + for (int i = 0; i < count; ++i) + { + activeTriangles[i] = new TestTriangle(triangles[i], i); + } + + for (int i = 0; i < count; ++i) { - ComputeMeshSpaceContacts(ref sourceChild.Manifold, meshInverseOrientation, requiresFlip, meshSpaceContacts, out var meshSpaceNormal); - //Select the deepest contact out of the manifold. Our goal is to find a contact on the representative feature of the source triangle. - //Recall that triangle collision tests will generate speculative contacts elsewhere on the triangle, both on the face and potentially on edges - //other than the deepest edge. - //The *normal*, however, is most directly associated with the deepest contact. The fact that the normal is 'infringing' on some other edge doesn't really matter. - //(Why doesn't it matter? MeshReduction operates on single convex-mesh pairs at a time. The *convex* shape cannot generate genuinely infringing contacts on two sides of a triangle at once. - //The opposing edge's contact will actually point *away* from that edge toward the interior of the source triangle. For the same reason that we never block face contacts, it doesn't make sense to - //block based on those incidental contacts.) - //This is equivalent to using the normal to determine the manifold voronoi region, except the contact position lets us deal with more arbitrary content. - var deepestIndex = 0; - var deepestDepth = sourceChild.Manifold.Contact0.Depth; - for (int j = 1; j < sourceChild.Manifold.Count; ++j) + ref var sourceTriangle = ref activeTriangles[i]; + ref var sourceChild = ref children[sourceTriangle.ChildIndex]; + //Can't correct contacts that were created by face collisions. + var faceFlagUnset = (sourceChild.Manifold.Contact0.FeatureId & FaceCollisionFlag) == 0; + if (faceFlagUnset && sourceChild.Manifold.Count > 0) { - var depth = Unsafe.Add(ref sourceChild.Manifold.Contact0, j).Depth; - if (deepestDepth < depth) + ComputeMeshSpaceContact(ref sourceChild.Manifold, meshInverseOrientation, requiresFlip, out var meshSpaceContact, out var meshSpaceNormal); + + for (int j = 0; j < count; ++j) { - deepestDepth = depth; - deepestIndex = j; + ref var targetTriangle = ref activeTriangles[j]; + if (ShouldBlockNormal(targetTriangle, meshSpaceContact, meshSpaceNormal)) + { + sourceTriangle.Blocked = true; + sourceTriangle.CorrectedNormal = new Vector3(targetTriangle.NX.X, targetTriangle.NY.X, targetTriangle.NZ.X); + //Even if the target manifold gets blocked, it should not necessarily be deleted. We made use of it as a blocker. + targetTriangle.ForceDeletionOnBlock = false; + break; + } } - } - ref var meshSpaceContact = ref meshSpaceContacts[deepestIndex]; - //var debugMin = meshSpaceContact - new Vector3(1e-3f); - //var debugMax = meshSpaceContact + new Vector3(1e-3f); - //var debugLeafEnumerator = new DebugLeafEnumerator(); - //debugLeafEnumerator.List = new QuickList(debugOverlapBuffer); - //debugMesh.Tree.GetOverlaps(debugMin, debugMax, ref debugLeafEnumerator); - //for (int j = 0; j < debugLeafEnumerator.List.Count; ++j) - //{ - // TestTriangle targetTriangle; - // if (!testTriangles.TryGetValue(debugLeafEnumerator.List[j], out targetTriangle)) - // { - // //Search for the child... - // int childIndexForTargetTriangle = -1; - // for (int k = 0; k < children.Length; ++k) - // { - // if (children[k].ChildIndexB == debugLeafEnumerator.List[j]) - // { - // childIndexForTargetTriangle = k; - // break; - // } - // } - // if (childIndexForTargetTriangle == -1) - // { - // Console.WriteLine("Bad news bears: a child index that should have existed by query does not exist according to the mesh reduction child set."); - // } - // targetTriangle = new TestTriangle(debugMesh.Triangles[debugLeafEnumerator.List[j]], childIndexForTargetTriangle); - // testTriangles.AddUnsafely(debugLeafEnumerator.List[j], targetTriangle); - // } - - // { - // ref var debugTriangle = ref debugMesh.Triangles[debugLeafEnumerator.List[j]]; - // var inverse = 1f / Vector3.Cross(debugTriangle.B - debugTriangle.A, debugTriangle.C - debugTriangle.A).Length(); - // var wa = Vector3.Cross(debugTriangle.B - debugTriangle.A, meshSpaceContact - debugTriangle.A).Length() * inverse; - // var wb = Vector3.Cross(debugTriangle.C - debugTriangle.B, meshSpaceContact - debugTriangle.B).Length() * inverse; - // var wc = Vector3.Cross(debugTriangle.A - debugTriangle.C, meshSpaceContact - debugTriangle.C).Length() * inverse; - // } - - // if (ShouldBlockNormal(targetTriangle, meshSpaceContact, meshSpaceNormal)) - // { - // //if (targetTriangle.ChildIndex != sourceTriangle.ChildIndex) - // { - // sourceTriangle.Blocked = true; - // sourceTriangle.CorrectedNormal = new Vector3(targetTriangle.NX.X, targetTriangle.NY.X, targetTriangle.NZ.X); - // //Even if the target manifold gets blocked, it should not necessarily be deleted. We made use of it as a blocker. - // targetTriangle.ForceDeletionOnBlock = false; - // break; - // } - // } - //} - - for (int j = 0; j < count; ++j) - { - ref var targetTriangle = ref activeTriangles[j]; - if (ShouldBlockNormal(targetTriangle, meshSpaceContact, meshSpaceNormal)) + + //RemoveContacts(); + + var testDot = Vector3.Dot(meshSpaceNormal, new Vector3(sourceTriangle.NX.X, sourceTriangle.NY.X, sourceTriangle.NZ.X)); + if (MathF.Abs(testDot) < 0.3f && !sourceTriangle.Blocked && sourceChild.Manifold.Count > 0) { - sourceTriangle.Blocked = true; - sourceTriangle.CorrectedNormal = new Vector3(targetTriangle.NX.X, targetTriangle.NY.X, targetTriangle.NZ.X); - //Even if the target manifold gets blocked, it should not necessarily be deleted. We made use of it as a blocker. - targetTriangle.ForceDeletionOnBlock = false; - break; + Console.WriteLine($"Iffy dot: {testDot} NOT BLOCKED"); } } - //Note that the removal had to be deferred until after blocking analysis. - //This manifold will not be considered for the remainder of this loop, so modifying it is fine. - for (int j = sourceChild.Manifold.Count - 1; j >= 0; --j) + else if (!faceFlagUnset) { - //If a contact is outside of the mesh space bounding box that found the triangles to test, then two things are true: - //1) The contact is almost certainly not productive; the bounding box included a frame of integrated motion and this contact was outside of it. - //2) The contact may have been created with a triangle whose neighbor was not in the query bounds, and so the neighbor won't contribute any blocking. - //The result is that such contacts have a tendency to cause ghost collisions. We'd rather not force the use of very small speculative margins, - //so instead we explicitly kill off contacts which are outside the queried bounds. - ref var contactToCheck = ref meshSpaceContacts[j]; - if (Vector3.Min(contactToCheck, queryBounds.Min) != queryBounds.Min || - Vector3.Max(contactToCheck, queryBounds.Max) != queryBounds.Max) + //Clear the face flags. This isn't *required* since they're coherent enough anyway and the accumulated impulse redistributor is a decent fallback, + //but it costs basically nothing to do this. + for (int k = 0; k < sourceChild.Manifold.Count; ++k) { - ConvexContactManifold.FastRemoveAt(ref sourceChild.Manifold, j); + Unsafe.Add(ref sourceChild.Manifold.Contact0, k).FeatureId &= ~FaceCollisionFlag; } } - - var testDot = Vector3.Dot(meshSpaceNormal, new Vector3(sourceTriangle.NX.X, sourceTriangle.NY.X, sourceTriangle.NZ.X)); - if (MathF.Abs(testDot) < 0.3f && !sourceTriangle.Blocked && sourceChild.Manifold.Count > 0) - { - Console.WriteLine($"Iffy dot: {testDot} NOT BLOCKED"); - } } - else + for (int i = 0; i < count; ++i) { - //Clear the face flags. This isn't *required* since they're coherent enough anyway and the accumulated impulse redistributor is a decent fallback, - //but it costs basically nothing to do this. - for (int k = 0; k < sourceChild.Manifold.Count; ++k) - { - Unsafe.Add(ref sourceChild.Manifold.Contact0, k).FeatureId &= ~FaceCollisionFlag; - } + TryApplyBlockToTriangle(ref activeTriangles[i], children, meshOrientation, requiresFlip); } } - for (int i = 0; i < count; ++i) + else { - ref var triangle = ref activeTriangles[i]; - if (triangle.Blocked) + + ChildEnumerator enumerator; + enumerator.List = new QuickList(count, pool); + enumerator.Pool = pool; + pool.Take(count, out var overlapBuffer); + QuickDictionary> testTriangles = new(count, pool); + + //We're likely to encounter all the triangles that we collected, so go ahead and create their entries. + //Note that this is also used to keep the indices lined up for the TryApplyBlockToTriangle loop. + for (int i = 0; i < count; ++i) { - ref var manifold = ref children[triangle.ChildIndex].Manifold; - if (triangle.ForceDeletionOnBlock) - { - //The manifold was infringing, and no other manifold infringed upon it. Can safely just ignore the manifold completely. - manifold.Count = 0; - } - else + var childIndex = children[i].ChildIndexB; + testTriangles.AddUnsafely(childIndex, new TestTriangle(mesh->Triangles[childIndex], i)); + } + for (int i = 0; i < count; ++i) + { + ref var sourceTriangle = ref testTriangles.Values[i]; + ref var sourceChild = ref children[sourceTriangle.ChildIndex]; + //Can't correct contacts that were created by face collisions. + var faceFlagUnset = (sourceChild.Manifold.Contact0.FeatureId & FaceCollisionFlag) == 0; + if (faceFlagUnset && sourceChild.Manifold.Count > 0) { - var manifoldHasPositiveDepth = false; - for (int j = 0; j < manifold.Count; ++j) + ComputeMeshSpaceContact(ref sourceChild.Manifold, meshInverseOrientation, requiresFlip, out var meshSpaceContact, out var meshSpaceNormal); + var contactQueryMin = meshSpaceContact - new Vector3(1e-3f); + var contactQueryMax = meshSpaceContact + new Vector3(1e-3f); + enumerator.List.Count = 0; + mesh->Tree.GetOverlaps(contactQueryMin, contactQueryMax, ref enumerator); + for (int j = 0; j < enumerator.List.Count; ++j) { - if (Unsafe.Add(ref manifold.Contact0, j).Depth > 0) + //Note that the test triangles detected by querying may exceed the count in extremely rare cases, so it's not safe to use AllocateUnsafely. + //Resizing invalidates table indices, so do any that ahead of time. + testTriangles.EnsureCapacity(testTriangles.Count + 1, pool); + if (!testTriangles.GetTableIndices(ref enumerator.List[j], out var tableIndex, out var elementIndex)) + { + testTriangles.Values[testTriangles.Count] = new TestTriangle(mesh->Triangles[enumerator.List[j]], testTriangles.Count); + testTriangles.Keys[testTriangles.Count] = enumerator.List[j]; + testTriangles.Table[tableIndex] = 1 + testTriangles.Count; + elementIndex = testTriangles.Count; + ++testTriangles.Count; + } + ref var targetTriangle = ref testTriangles.Values[elementIndex]; + + if (ShouldBlockNormal(targetTriangle, meshSpaceContact, meshSpaceNormal)) { - manifoldHasPositiveDepth = true; + sourceTriangle.Blocked = true; + sourceTriangle.CorrectedNormal = new Vector3(targetTriangle.NX.X, targetTriangle.NY.X, targetTriangle.NZ.X); + //Even if the target manifold gets blocked, it should not necessarily be deleted. We made use of it as a blocker. + targetTriangle.ForceDeletionOnBlock = false; break; } } - if (manifoldHasPositiveDepth) + var testDot = Vector3.Dot(meshSpaceNormal, new Vector3(sourceTriangle.NX.X, sourceTriangle.NY.X, sourceTriangle.NZ.X)); + if (MathF.Abs(testDot) < 0.3f && !sourceTriangle.Blocked && sourceChild.Manifold.Count > 0) { - //The manifold was infringing, but another manifold was infringing upon it. We can't safely delete such a manifold since it's likely a mutually infringing - //case- consider what happens when an objects wedges itself into an edge between two triangles. - Matrix3x3.Transform(requiresFlip ? triangle.CorrectedNormal : -triangle.CorrectedNormal, meshOrientation, out manifold.Normal); - //Note that we do not modify the depth. - //The only times this situation should occur is when either 1) an object has somehow wedged between adjacent triangles such that the detected - //depths are *less* than the triangle face depths, or 2) a source triangle generated an internal contact, and the face depth is guaranteed to be less. - //So, using those depths is guaranteed not to introduce excessive energy. + Console.WriteLine($"Iffy dot: {testDot} NOT BLOCKED"); } - else + } + else if (!faceFlagUnset) + { + //Clear the face flags. This isn't *required* since they're coherent enough anyway and the accumulated impulse redistributor is a decent fallback, + //but it costs basically nothing to do this. + for (int k = 0; k < sourceChild.Manifold.Count; ++k) { - //The manifold has zero or negative depth; it's clearly not a case where a shape is wedged between triangles. Just get rid of it. - manifold.Count = 0; + Unsafe.Add(ref sourceChild.Manifold.Contact0, k).FeatureId &= ~FaceCollisionFlag; } } } + + for (int i = 0; i < count; ++i) + { + TryApplyBlockToTriangle(ref testTriangles.Values[i], children, meshOrientation, requiresFlip); + } + + testTriangles.Dispose(pool); + enumerator.List.Dispose(pool); } + + } //[MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -448,8 +469,8 @@ public unsafe bool TryFlush(int pairId, ref CollisionBatcher Date: Fri, 24 Sep 2021 15:23:53 -0500 Subject: [PATCH 015/947] Query epsilon adjusted. Debug stuff trimmed. --- .../CollisionDetection/MeshReduction.cs | 47 +++++++++++-------- 1 file changed, 27 insertions(+), 20 deletions(-) diff --git a/BepuPhysics/CollisionDetection/MeshReduction.cs b/BepuPhysics/CollisionDetection/MeshReduction.cs index c6fff064b..0af6e8b4f 100644 --- a/BepuPhysics/CollisionDetection/MeshReduction.cs +++ b/BepuPhysics/CollisionDetection/MeshReduction.cs @@ -323,8 +323,9 @@ public unsafe static void ReduceManifolds(ref Buffer continuationTrian //Narrow the region of interest. continuationTriangles.Slice(start, count, out var triangles); continuationChildren.Slice(start, count, out var children); - const int bruteForceThreshold = 32; - if (false)//count < bruteForceThreshold) + const int bruteForceThreshold = 16; + //Console.WriteLine($"count: {count}"); + if (count < bruteForceThreshold) { var memory = stackalloc TestTriangle[count]; var activeTriangles = new Buffer(memory, count); @@ -358,11 +359,11 @@ public unsafe static void ReduceManifolds(ref Buffer continuationTrian //RemoveContacts(); - var testDot = Vector3.Dot(meshSpaceNormal, new Vector3(sourceTriangle.NX.X, sourceTriangle.NY.X, sourceTriangle.NZ.X)); - if (MathF.Abs(testDot) < 0.3f && !sourceTriangle.Blocked && sourceChild.Manifold.Count > 0) - { - Console.WriteLine($"Iffy dot: {testDot} NOT BLOCKED"); - } + //var testDot = Vector3.Dot(meshSpaceNormal, new Vector3(sourceTriangle.NX.X, sourceTriangle.NY.X, sourceTriangle.NZ.X)); + //if (MathF.Abs(testDot) < 0.3f && !sourceTriangle.Blocked && sourceChild.Manifold.Count > 0) + //{ + // Console.WriteLine($"Iffy dot: {testDot} NOT BLOCKED"); + //} } else if (!faceFlagUnset) { @@ -383,10 +384,16 @@ public unsafe static void ReduceManifolds(ref Buffer continuationTrian { ChildEnumerator enumerator; - enumerator.List = new QuickList(count, pool); + //Queries can sometimes find triangles that are just barely outside the original child set. It's rare, but there's no reason to force a resize if it does happen. + //Allocate a bit more to make resizes almost-but-not-quite impossible. + var allocationSize = count * 2; enumerator.Pool = pool; - pool.Take(count, out var overlapBuffer); - QuickDictionary> testTriangles = new(count, pool); + enumerator.List = new QuickList(allocationSize, pool); + QuickDictionary> testTriangles = new(allocationSize, pool); + //For numerical reasons, expand each contact by an epsilon to capture relevant triangles. + var span = queryBounds.Max - queryBounds.Min; + var maxSpan = MathF.Max(span.X, MathF.Max(span.Y, span.Z)); + var contactExpansion = new Vector3(maxSpan * 1e-4f); //We're likely to encounter all the triangles that we collected, so go ahead and create their entries. //Note that this is also used to keep the indices lined up for the TryApplyBlockToTriangle loop. @@ -404,15 +411,15 @@ public unsafe static void ReduceManifolds(ref Buffer continuationTrian if (faceFlagUnset && sourceChild.Manifold.Count > 0) { ComputeMeshSpaceContact(ref sourceChild.Manifold, meshInverseOrientation, requiresFlip, out var meshSpaceContact, out var meshSpaceNormal); - var contactQueryMin = meshSpaceContact - new Vector3(1e-3f); - var contactQueryMax = meshSpaceContact + new Vector3(1e-3f); + var contactQueryMin = meshSpaceContact - contactExpansion; + var contactQueryMax = meshSpaceContact + contactExpansion; enumerator.List.Count = 0; mesh->Tree.GetOverlaps(contactQueryMin, contactQueryMax, ref enumerator); + //Note that the test triangles detected by querying may exceed the count in extremely rare cases, so it's not safe to use AllocateUnsafely without some extra work. + //Resizing invalidates table indices, so do any that ahead of time. + testTriangles.EnsureCapacity(testTriangles.Count + enumerator.List.Count, pool); for (int j = 0; j < enumerator.List.Count; ++j) { - //Note that the test triangles detected by querying may exceed the count in extremely rare cases, so it's not safe to use AllocateUnsafely. - //Resizing invalidates table indices, so do any that ahead of time. - testTriangles.EnsureCapacity(testTriangles.Count + 1, pool); if (!testTriangles.GetTableIndices(ref enumerator.List[j], out var tableIndex, out var elementIndex)) { testTriangles.Values[testTriangles.Count] = new TestTriangle(mesh->Triangles[enumerator.List[j]], testTriangles.Count); @@ -432,11 +439,11 @@ public unsafe static void ReduceManifolds(ref Buffer continuationTrian break; } } - var testDot = Vector3.Dot(meshSpaceNormal, new Vector3(sourceTriangle.NX.X, sourceTriangle.NY.X, sourceTriangle.NZ.X)); - if (MathF.Abs(testDot) < 0.3f && !sourceTriangle.Blocked && sourceChild.Manifold.Count > 0) - { - Console.WriteLine($"Iffy dot: {testDot} NOT BLOCKED"); - } + //var testDot = Vector3.Dot(meshSpaceNormal, new Vector3(sourceTriangle.NX.X, sourceTriangle.NY.X, sourceTriangle.NZ.X)); + //if (MathF.Abs(testDot) < 0.3f && !sourceTriangle.Blocked && sourceChild.Manifold.Count > 0) + //{ + // Console.WriteLine($"Iffy dot: {testDot} NOT BLOCKED"); + //} } else if (!faceFlagUnset) { From c5c6062a730806c4bbb19595eb009f2a186dfd37 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 24 Sep 2021 16:51:53 -0500 Subject: [PATCH 016/947] Added dictionary FindOrAllocateSlot, abandoned pretense of ever trying to directly support VB. --- BepuPhysics/Collidables/ConvexHullHelper.cs | 2 +- .../CollisionDetection/MeshReduction.cs | 11 +- BepuPhysics/CollisionDetection/PairCache.cs | 6 +- .../CollisionDetection/PairCache_Activity.cs | 4 +- BepuPhysics/FallbackBatch.cs | 26 ++--- BepuPhysics/IslandAwakener.cs | 2 +- BepuPhysics/IslandSleeper.cs | 2 +- BepuUtilities/Collections/QuickDictionary.cs | 109 +++++++++++++++--- BepuUtilities/Collections/QuickList.cs | 12 +- BepuUtilities/Collections/QuickSet.cs | 30 ++--- 10 files changed, 139 insertions(+), 65 deletions(-) diff --git a/BepuPhysics/Collidables/ConvexHullHelper.cs b/BepuPhysics/Collidables/ConvexHullHelper.cs index 29d54acda..759a4f606 100644 --- a/BepuPhysics/Collidables/ConvexHullHelper.cs +++ b/BepuPhysics/Collidables/ConvexHullHelper.cs @@ -585,7 +585,7 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa edgeToAdd.Endpoints.B = reducedFaceIndices[i]; edgeToAdd.FaceNormal = initialFaceNormal; edgeToAdd.FaceIndex = 0; - edgeFaceCounts.AddRef(ref edgeToAdd.Endpoints, 1, pool); + edgeFaceCounts.Add(ref edgeToAdd.Endpoints, 1, pool); } //Since an actual face was found, we go ahead and output it into the face set. earlyFaceStartIndices.Allocate(pool) = earlyFaceIndices.Count; diff --git a/BepuPhysics/CollisionDetection/MeshReduction.cs b/BepuPhysics/CollisionDetection/MeshReduction.cs index 0af6e8b4f..99a746bcb 100644 --- a/BepuPhysics/CollisionDetection/MeshReduction.cs +++ b/BepuPhysics/CollisionDetection/MeshReduction.cs @@ -420,15 +420,12 @@ public unsafe static void ReduceManifolds(ref Buffer continuationTrian testTriangles.EnsureCapacity(testTriangles.Count + enumerator.List.Count, pool); for (int j = 0; j < enumerator.List.Count; ++j) { - if (!testTriangles.GetTableIndices(ref enumerator.List[j], out var tableIndex, out var elementIndex)) + var triangleIndexInMesh = enumerator.List[j]; + if (!testTriangles.FindOrAllocateSlotUnsafely(triangleIndexInMesh, out var testTriangleIndex)) { - testTriangles.Values[testTriangles.Count] = new TestTriangle(mesh->Triangles[enumerator.List[j]], testTriangles.Count); - testTriangles.Keys[testTriangles.Count] = enumerator.List[j]; - testTriangles.Table[tableIndex] = 1 + testTriangles.Count; - elementIndex = testTriangles.Count; - ++testTriangles.Count; + testTriangles.Values[testTriangleIndex] = new TestTriangle(mesh->Triangles[triangleIndexInMesh], testTriangleIndex); } - ref var targetTriangle = ref testTriangles.Values[elementIndex]; + ref var targetTriangle = ref testTriangles.Values[testTriangleIndex]; if (ShouldBlockNormal(targetTriangle, meshSpaceContact, meshSpaceNormal)) { diff --git a/BepuPhysics/CollisionDetection/PairCache.cs b/BepuPhysics/CollisionDetection/PairCache.cs index 340ebbc01..d6d0f11ab 100644 --- a/BepuPhysics/CollisionDetection/PairCache.cs +++ b/BepuPhysics/CollisionDetection/PairCache.cs @@ -251,13 +251,13 @@ public unsafe void FlushMappingChanges() //Walk backwards on the off chance that a swap can be avoided. for (int j = cache.PendingRemoves.Count - 1; j >= 0; --j) { - var removed = Mapping.FastRemoveRef(ref cache.PendingRemoves[j]); + var removed = Mapping.FastRemove(ref cache.PendingRemoves[j]); Debug.Assert(removed); } for (int j = 0; j < cache.PendingAdds.Count; ++j) { ref var pending = ref cache.PendingAdds[j]; - var added = Mapping.AddUnsafelyRef(ref pending.Pair, pending.Pointers); + var added = Mapping.AddUnsafely(ref pending.Pair, pending.Pointers); Debug.Assert(added); } } @@ -350,7 +350,7 @@ public void Dispose() [MethodImpl(MethodImplOptions.AggressiveInlining)] public int IndexOf(ref CollidablePair pair) { - return Mapping.IndexOfRef(ref pair); + return Mapping.IndexOf(ref pair); } [MethodImpl(MethodImplOptions.AggressiveInlining)] diff --git a/BepuPhysics/CollisionDetection/PairCache_Activity.cs b/BepuPhysics/CollisionDetection/PairCache_Activity.cs index 6fa8aea11..0d0d3c9c2 100644 --- a/BepuPhysics/CollisionDetection/PairCache_Activity.cs +++ b/BepuPhysics/CollisionDetection/PairCache_Activity.cs @@ -174,7 +174,7 @@ internal unsafe void AwakenSet(int setIndex) { pointers.CollisionDetectionCache = new PairCacheIndex(); } - Mapping.AddUnsafelyRef(ref pair.Pair, pointers); + Mapping.AddUnsafely(ref pair.Pair, pointers); } } } @@ -183,7 +183,7 @@ internal void RemoveReferenceIfContactConstraint(ConstraintHandle handle, int ty { if (NarrowPhase.IsContactConstraintType(typeId)) { - var removed = Mapping.FastRemoveRef(ref ConstraintHandleToPair[handle.Value].Pair); + var removed = Mapping.FastRemove(ref ConstraintHandleToPair[handle.Value].Pair); Debug.Assert(removed, "If a contact constraint is being directly removed, it must exist within the pair mapping- " + "all *active* contact constraints do, and it's not valid to attempt to remove an inactive constraint."); } diff --git a/BepuPhysics/FallbackBatch.cs b/BepuPhysics/FallbackBatch.cs index 0b88cefb0..03ccb557b 100644 --- a/BepuPhysics/FallbackBatch.cs +++ b/BepuPhysics/FallbackBatch.cs @@ -88,7 +88,7 @@ unsafe void Allocate(ConstraintHandle constraintHandle, Sp ++bodyConstraintReferences.Count; } var fallbackReference = new FallbackReference { ConstraintHandle = constraintHandle, IndexInConstraint = i }; - constraintReferences.AddRef(ref fallbackReference, pool); + constraintReferences.Add(ref fallbackReference, pool); } } @@ -139,7 +139,7 @@ internal unsafe void Remove(int bodyReference, ConstraintHandle constraintHandle ref var constraintReferences = ref bodyConstraintReferences.Values[bodyReferencesIndex]; //TODO: Should really just be using a dictionary here. var dummy = new FallbackReference { ConstraintHandle = constraintHandle }; - var removed = constraintReferences.FastRemoveRef(ref dummy); + var removed = constraintReferences.FastRemove(ref dummy); Debug.Assert(removed, "If a constraint removal was requested, it must exist within the referenced body's constraint set."); if (constraintReferences.Count == 0) { @@ -241,7 +241,7 @@ public void GetJacobiScaleForBodies(ref Vector references, int count, out V ref var countsStart = ref Unsafe.As, int>(ref counts); for (int i = 0; i < count; ++i) { - var index = bodyConstraintReferences.IndexOfRef(ref Unsafe.Add(ref start, i)); + var index = bodyConstraintReferences.IndexOf(ref Unsafe.Add(ref start, i)); Debug.Assert(index >= 0, "If a prestep is looking up constraint counts associated with a body, it better be in the jacobi batch!"); Unsafe.Add(ref countsStart, i) = bodyConstraintReferences.Values[index].Count; } @@ -259,8 +259,8 @@ public void GetJacobiScaleForBodies(ref TwoBodyReferences references, int count, ref var countsBStart = ref Unsafe.As, int>(ref countsB); for (int i = 0; i < count; ++i) { - var indexA = bodyConstraintReferences.IndexOfRef(ref Unsafe.Add(ref startA, i)); - var indexB = bodyConstraintReferences.IndexOfRef(ref Unsafe.Add(ref startB, i)); + var indexA = bodyConstraintReferences.IndexOf(ref Unsafe.Add(ref startA, i)); + var indexB = bodyConstraintReferences.IndexOf(ref Unsafe.Add(ref startB, i)); Debug.Assert(indexA >= 0 && indexB >= 0, "If a prestep is looking up constraint counts associated with a body, it better be in the jacobi batch!"); Unsafe.Add(ref countsAStart, i) = bodyConstraintReferences.Values[indexA].Count; Unsafe.Add(ref countsBStart, i) = bodyConstraintReferences.Values[indexB].Count; @@ -284,9 +284,9 @@ public void GetJacobiScaleForBodies(ref ThreeBodyReferences references, int coun ref var countsCStart = ref Unsafe.As, int>(ref countsC); for (int i = 0; i < count; ++i) { - var indexA = bodyConstraintReferences.IndexOfRef(ref Unsafe.Add(ref startA, i)); - var indexB = bodyConstraintReferences.IndexOfRef(ref Unsafe.Add(ref startB, i)); - var indexC = bodyConstraintReferences.IndexOfRef(ref Unsafe.Add(ref startC, i)); + var indexA = bodyConstraintReferences.IndexOf(ref Unsafe.Add(ref startA, i)); + var indexB = bodyConstraintReferences.IndexOf(ref Unsafe.Add(ref startB, i)); + var indexC = bodyConstraintReferences.IndexOf(ref Unsafe.Add(ref startC, i)); Debug.Assert(indexA >= 0 && indexB >= 0, "If a prestep is looking up constraint counts associated with a body, it better be in the jacobi batch!"); Unsafe.Add(ref countsAStart, i) = bodyConstraintReferences.Values[indexA].Count; Unsafe.Add(ref countsBStart, i) = bodyConstraintReferences.Values[indexB].Count; @@ -315,10 +315,10 @@ public void GetJacobiScaleForBodies(ref FourBodyReferences references, int count ref var countsDStart = ref Unsafe.As, int>(ref countsD); for (int i = 0; i < count; ++i) { - var indexA = bodyConstraintReferences.IndexOfRef(ref Unsafe.Add(ref startA, i)); - var indexB = bodyConstraintReferences.IndexOfRef(ref Unsafe.Add(ref startB, i)); - var indexC = bodyConstraintReferences.IndexOfRef(ref Unsafe.Add(ref startC, i)); - var indexD = bodyConstraintReferences.IndexOfRef(ref Unsafe.Add(ref startD, i)); + var indexA = bodyConstraintReferences.IndexOf(ref Unsafe.Add(ref startA, i)); + var indexB = bodyConstraintReferences.IndexOf(ref Unsafe.Add(ref startB, i)); + var indexC = bodyConstraintReferences.IndexOf(ref Unsafe.Add(ref startC, i)); + var indexD = bodyConstraintReferences.IndexOf(ref Unsafe.Add(ref startD, i)); Debug.Assert(indexA >= 0 && indexB >= 0, "If a prestep is looking up constraint counts associated with a body, it better be in the jacobi batch!"); Unsafe.Add(ref countsAStart, i) = bodyConstraintReferences.Values[indexA].Count; Unsafe.Add(ref countsBStart, i) = bodyConstraintReferences.Values[indexB].Count; @@ -453,7 +453,7 @@ internal void UpdateForBodyMemoryMove(int originalBodyIndex, int newBodyLocation bodyConstraintReferences.GetTableIndices(ref originalBodyIndex, out var tableIndex, out var elementIndex); var references = bodyConstraintReferences.Values[elementIndex]; bodyConstraintReferences.FastRemove(tableIndex, elementIndex); - bodyConstraintReferences.AddUnsafelyRef(ref newBodyLocation, references); + bodyConstraintReferences.AddUnsafely(ref newBodyLocation, references); } internal void UpdateForBodyMemorySwap(int a, int b) diff --git a/BepuPhysics/IslandAwakener.cs b/BepuPhysics/IslandAwakener.cs index df1e04cc7..79974ad7d 100644 --- a/BepuPhysics/IslandAwakener.cs +++ b/BepuPhysics/IslandAwakener.cs @@ -256,7 +256,7 @@ internal unsafe void ExecutePhaseOneJob(int index) //The HandleToLocation was updated during job setup, so we can use it. ref var bodyLocation = ref bodies.HandleToLocation[source.bodyConstraintReferences.Keys[j]]; Debug.Assert(bodyLocation.SetIndex == 0, "Any batch moved into the active set should be dealing with bodies which have already been moved into the active set."); - var added = target.bodyConstraintReferences.AddUnsafelyRef(ref bodyLocation.Index, source.bodyConstraintReferences.Values[j]); + var added = target.bodyConstraintReferences.AddUnsafely(ref bodyLocation.Index, source.bodyConstraintReferences.Values[j]); Debug.Assert(added, "Any body moving from an inactive set to the active set should not already be present in the active set's fallback batch."); } //We've reused the lists. Set the count to zero so they don't get disposed later. diff --git a/BepuPhysics/IslandSleeper.cs b/BepuPhysics/IslandSleeper.cs index 52e6f52cf..edb4440f5 100644 --- a/BepuPhysics/IslandSleeper.cs +++ b/BepuPhysics/IslandSleeper.cs @@ -573,7 +573,7 @@ unsafe void PrintIsland(ref IslandScaffold island) ref solverBatch.TypeBatches[solverBatch.TypeIndexToTypeBatchIndex[location.TypeId]], location.IndexInTypeBatch, ref bodyIndexEnumerator); for (int i = 0; i < typeProcessor.BodiesPerConstraint; ++i) { - constraintReferencedBodyHandles.AddRef(ref bodies.ActiveSet.IndexToHandle[references[i]].Value, pool); + constraintReferencedBodyHandles.Add(ref bodies.ActiveSet.IndexToHandle[references[i]].Value, pool); } Console.Write($"{handle}, "); } diff --git a/BepuUtilities/Collections/QuickDictionary.cs b/BepuUtilities/Collections/QuickDictionary.cs index 1a6742d74..a57decbfe 100644 --- a/BepuUtilities/Collections/QuickDictionary.cs +++ b/BepuUtilities/Collections/QuickDictionary.cs @@ -239,7 +239,7 @@ public void Resize(ref Buffer newKeySpan, ref Buffer newValueSpan, { //We assume that ref adds will get inlined reasonably here. That's not actually guaranteed, but we'll bite the bullet. //(You could technically branch on the Unsafe.SizeOf, which should result in a compile time specialized zero overhead implementation... but meh!) - AddUnsafelyRef(ref oldDictionary.Keys[i], oldDictionary.Values[i]); + AddUnsafely(ref oldDictionary.Keys[i], oldDictionary.Values[i]); } oldKeySpan = oldDictionary.Keys; oldValueSpan = oldDictionary.Values; @@ -364,7 +364,7 @@ public int IndexOf(TKey key) /// /// Key to get the index of. /// The index of the key if the key exists in the dictionary, -1 otherwise. - public int IndexOfRef(ref TKey key) + public int IndexOf(ref TKey key) { Validate(); GetTableIndices(ref key, out int tableIndex, out int objectIndex); @@ -387,7 +387,7 @@ public bool ContainsKey(TKey key) /// /// Key to test for. /// True if the key already belongs to the dictionary, false otherwise. - public bool ContainsKeyRef(ref TKey key) + public bool ContainsKey(ref TKey key) { Validate(); return GetTableIndices(ref key, out int tableIndex, out int objectIndex); @@ -417,7 +417,7 @@ public bool TryGetValue(TKey key, out TValue value) /// Key to look up. /// Value associated with the specified key. /// True if a value was found, false otherwise. - public bool TryGetValueRef(ref TKey key, out TValue value) + public bool TryGetValue(ref TKey key, out TValue value) { Validate(); if (GetTableIndices(ref key, out int tableIndex, out int elementIndex)) @@ -429,6 +429,83 @@ public bool TryGetValueRef(ref TKey key, out TValue value) return false; } + /// + /// Attempts to find the index of the given key. If it is present, outputs the index and returns true. If it is not present, it allocates a slot for it, outputs the index of that new slot, and returns false. + /// If a new slot is allocated, the value stored in the slot is undefined. + /// + /// Key to find or allocate a slot for. + /// Index of the found or allocated slot. + /// True if the key was already present in the dictionary, false otherwise. + public bool FindOrAllocateSlotUnsafely(ref TKey key, out int slotIndex) + { + Validate(); + ValidateUnsafeAdd(); + if (GetTableIndices(ref key, out int tableIndex, out slotIndex)) + return true; + //It wasn't in the dictionary. Allocate it! + slotIndex = Count++; + Keys[slotIndex] = key; + //Use the encoding- all indices are offset by 1 since 0 represents 'empty'. + Table[tableIndex] = Count; + return false; + } + + /// + /// Attempts to find the index of the given key. If it is present, outputs the index and returns true. If it is not present, it allocates a slot for it, outputs the index of that new slot, and returns false. + /// If a new slot is allocated, the value stored in the slot is undefined. + /// + /// Key to find or allocate a slot for. + /// Index of the found or allocated slot. + /// True if the key was already present in the dictionary, false otherwise. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool FindOrAllocateSlotUnsafely(TKey key, out int slotIndex) + { + return FindOrAllocateSlotUnsafely(ref key, out slotIndex); + } + + /// + /// Attempts to find the index of the given key. If it is present, outputs the index and returns true. If it is not present, it allocates a slot for it, outputs the index of that new slot, and returns false. + /// If a new slot is allocated, the value stored in the slot is undefined. + /// + /// Key to find or allocate a slot for. + /// Pool used to resize the container if necessary to allocate. + /// Index of the found or allocated slot. + /// True if the key was already present in the dictionary, false otherwise. + public bool FindOrAllocateSlot(ref TKey key, BufferPool pool, out int slotIndex) + { + Validate(); + if (Count == Keys.Length) + { + //There's no room left; resize. + Resize(Count * 2, pool); + //Note that this is tested before any indices are found. + //If we resized only after determining that it was going to be added, + //the potential resize would invalidate the computed indices. + } + if (GetTableIndices(ref key, out int tableIndex, out slotIndex)) + return true; + //It wasn't in the dictionary. Allocate it! + slotIndex = Count++; + Keys[slotIndex] = key; + //Use the encoding- all indices are offset by 1 since 0 represents 'empty'. + Table[tableIndex] = Count; + return false; + } + + /// + /// Attempts to find the index of the given key. If it is present, outputs the index and returns true. If it is not present, it allocates a slot for it, outputs the index of that new slot, and returns false. + /// If a new slot is allocated, the value stored in the slot is undefined. + /// + /// Key to find or allocate a slot for. + /// Pool used to resize the container if necessary to allocate. + /// Index of the found or allocated slot. + /// True if the key was already present in the dictionary, false otherwise. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool FindOrAllocateSlot(TKey key, BufferPool pool, out int slotIndex) + { + return FindOrAllocateSlot(ref key, pool, out slotIndex); + } + /// /// Adds a pair to the dictionary. If a version of the key (same hash code, 'equal' by comparer) is already present, /// the existing pair is replaced by the given version. @@ -437,7 +514,7 @@ public bool TryGetValueRef(ref TKey key, out TValue value) /// Value of the pair to add. /// True if the pair was added to the dictionary, false if the key was already present and its pair was replaced. //[MethodImpl(MethodImplOptions.AggressiveInlining)] //TODO: Test performance of full chain inline. - public bool AddAndReplaceUnsafelyRef(ref TKey key, in TValue value) + public bool AddAndReplaceUnsafely(ref TKey key, in TValue value) { Validate(); ValidateUnsafeAdd(); @@ -468,7 +545,7 @@ public bool AddAndReplaceUnsafelyRef(ref TKey key, in TValue value) [MethodImpl(MethodImplOptions.AggressiveInlining)] public bool AddAndReplaceUnsafely(TKey key, in TValue value) { - return AddAndReplaceUnsafelyRef(ref key, value); + return AddAndReplaceUnsafely(ref key, value); } /// @@ -478,7 +555,7 @@ public bool AddAndReplaceUnsafely(TKey key, in TValue value) /// Value of the pair to add. /// True if the pair was added to the dictionary, false if the key was already present. //[MethodImpl(MethodImplOptions.AggressiveInlining)] //TODO: Test performance of full chain inline. - public bool AddUnsafelyRef(ref TKey key, in TValue value) + public bool AddUnsafely(ref TKey key, in TValue value) { Validate(); ValidateUnsafeAdd(); @@ -505,7 +582,7 @@ public bool AddUnsafelyRef(ref TKey key, in TValue value) [MethodImpl(MethodImplOptions.AggressiveInlining)] public bool AddUnsafely(TKey key, in TValue value) { - return AddUnsafelyRef(ref key, value); + return AddUnsafely(ref key, value); } /// @@ -517,7 +594,7 @@ public bool AddUnsafely(TKey key, in TValue value) /// Pool used for spans. /// True if the pair was added to the dictionary, false if the key was already present and its pair was replaced. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool AddAndReplaceRef(ref TKey key, in TValue value, IUnmanagedMemoryPool pool) + public bool AddAndReplace(ref TKey key, in TValue value, IUnmanagedMemoryPool pool) { if (Count == Keys.Length) { @@ -528,7 +605,7 @@ public bool AddAndReplaceRef(ref TKey key, in TValue value, IUnmanagedMemoryPool //If we resized only after determining that it was going to be added, //the potential resize would invalidate the computed indices. } - return AddAndReplaceUnsafelyRef(ref key, value); + return AddAndReplaceUnsafely(ref key, value); } /// @@ -542,7 +619,7 @@ public bool AddAndReplaceRef(ref TKey key, in TValue value, IUnmanagedMemoryPool [MethodImpl(MethodImplOptions.AggressiveInlining)] public bool AddAndReplace(TKey key, in TValue value, IUnmanagedMemoryPool pool) { - return AddAndReplaceRef(ref key, value, pool); + return AddAndReplace(ref key, value, pool); } /// @@ -554,7 +631,7 @@ public bool AddAndReplace(TKey key, in TValue value, IUnmanagedMemoryPool pool) /// Type of the pool used for spans. /// True if the pair was added to the dictionary, false if the key was already present. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool AddRef(ref TKey key, in TValue value, IUnmanagedMemoryPool pool) + public bool Add(ref TKey key, in TValue value, IUnmanagedMemoryPool pool) { Validate(); @@ -567,7 +644,7 @@ public bool AddRef(ref TKey key, in TValue value, IUnmanagedMemoryPool pool) //If we resized only after determining that it was going to be added, //the potential resize would invalidate the computed indices. } - return AddUnsafelyRef(ref key, value); + return AddUnsafely(ref key, value); } /// @@ -581,7 +658,7 @@ public bool AddRef(ref TKey key, in TValue value, IUnmanagedMemoryPool pool) [MethodImpl(MethodImplOptions.AggressiveInlining)] public bool Add(TKey key, in TValue value, IUnmanagedMemoryPool pool) { - return AddRef(ref key, value, pool); + return Add(ref key, value, pool); } //Note: the reason this is named "FastRemove" instead of just "Remove" despite it being the only remove present is that @@ -646,7 +723,7 @@ public void FastRemove(int tableIndex, int elementIndex) /// /// Key of the pair to remove. /// True if the key was found and removed, false otherwise. - public bool FastRemoveRef(ref TKey key) + public bool FastRemove(ref TKey key) { Validate(); //Find it. @@ -668,7 +745,7 @@ public bool FastRemoveRef(ref TKey key) [MethodImpl(MethodImplOptions.AggressiveInlining)] public bool FastRemove(TKey key) { - return FastRemoveRef(ref key); + return FastRemove(ref key); } /// diff --git a/BepuUtilities/Collections/QuickList.cs b/BepuUtilities/Collections/QuickList.cs index dd6f0b3f9..68f0e0caf 100644 --- a/BepuUtilities/Collections/QuickList.cs +++ b/BepuUtilities/Collections/QuickList.cs @@ -435,7 +435,7 @@ public int IndexOf(T element) /// Element to find. /// Index of the element in the list if present, -1 otherwise. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public int IndexOfRef(ref T element) + public int IndexOf(ref T element) { Validate(); return Span.IndexOf(ref element, 0, Count); @@ -462,7 +462,7 @@ public int IndexOf(ref TPredicate predicate) where TPredicate : IPre public bool Remove(ref T element) { Validate(); - var index = IndexOfRef(ref element); + var index = IndexOf(ref element); if (index >= 0) { RemoveAt(index); @@ -508,7 +508,7 @@ public bool Remove(ref TPredicate predicate) where TPredicate : IPre /// Element to remove from the list. /// True if the element was present and was removed, false otherwise. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool FastRemoveRef(ref T element) + public bool FastRemove(ref T element) { Validate(); var index = IndexOf(element); @@ -528,7 +528,7 @@ public bool FastRemoveRef(ref T element) [MethodImpl(MethodImplOptions.AggressiveInlining)] public bool FastRemove(T element) { - return FastRemoveRef(ref element); + return FastRemove(ref element); } /// @@ -640,9 +640,9 @@ public bool Contains(T element) /// /// The object to locate in the collection. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool ContainsRef(ref T element) + public bool Contains(ref T element) { - return IndexOfRef(ref element) >= 0; + return IndexOf(ref element) >= 0; } /// diff --git a/BepuUtilities/Collections/QuickSet.cs b/BepuUtilities/Collections/QuickSet.cs index fa8656fde..d757b2201 100644 --- a/BepuUtilities/Collections/QuickSet.cs +++ b/BepuUtilities/Collections/QuickSet.cs @@ -175,7 +175,7 @@ public void Resize(ref Buffer newSpan, ref Buffer newTableSpan, out Buff { //We assume that ref adds will get inlined reasonably here. That's not actually guaranteed, but we'll bite the bullet. //(You could technically branch on the Unsafe.SizeOf, which should result in a compile time specialized zero overhead implementation... but meh!) - AddUnsafelyRef(ref oldSet.Span[i]); + AddUnsafely(ref oldSet.Span[i]); } oldSpan = oldSet.Span; oldTableSpan = oldSet.Table; @@ -289,7 +289,7 @@ public int IndexOf(T element) /// Element to get the index of. /// The index of the element if the element exists in the set, -1 otherwise. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public int IndexOfRef(ref T element) + public int IndexOf(ref T element) { GetTableIndices(ref element, out int tableIndex, out int objectIndex); return objectIndex; @@ -312,7 +312,7 @@ public bool Contains(T element) /// Element to test for. /// True if the element already belongs to the set, false otherwise. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool ContainsRef(ref T element) + public bool Contains(ref T element) { return GetTableIndices(ref element, out int tableIndex, out int objectIndex); } @@ -325,7 +325,7 @@ public bool ContainsRef(ref T element) /// Element to add. /// True if the element was added to the set, false if the element was already present and was instead replaced. //[MethodImpl(MethodImplOptions.AggressiveInlining)] //TODO: Test performance of full chain inline. - public bool AddAndReplaceUnsafelyRef(ref T element) + public bool AddAndReplaceUnsafely(ref T element) { Validate(); if (GetTableIndices(ref element, out int tableIndex, out int elementIndex)) @@ -353,7 +353,7 @@ public bool AddAndReplaceUnsafelyRef(ref T element) [MethodImpl(MethodImplOptions.AggressiveInlining)] public bool AddAndReplaceUnsafely(T element) { - return AddAndReplaceUnsafelyRef(ref element); + return AddAndReplaceUnsafely(ref element); } /// @@ -363,7 +363,7 @@ public bool AddAndReplaceUnsafely(T element) /// Element to add. /// True if the element was added to the set, false if the element was already present. //[MethodImpl(MethodImplOptions.AggressiveInlining)] //TODO: Test performance of full chain inline. - public bool AddUnsafelyRef(ref T element) + public bool AddUnsafely(ref T element) { Validate(); if (GetTableIndices(ref element, out int tableIndex, out int elementIndex)) @@ -389,7 +389,7 @@ public bool AddUnsafelyRef(ref T element) [MethodImpl(MethodImplOptions.AggressiveInlining)] public bool AddUnsafely(T element) { - return AddUnsafelyRef(ref element); + return AddUnsafely(ref element); } /// @@ -400,7 +400,7 @@ public bool AddUnsafely(T element) /// Pool used for spans. /// True if the element was added to the set, false if the element was already present and was instead replaced. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool AddAndReplaceRef(ref T element, IUnmanagedMemoryPool pool) + public bool AddAndReplace(ref T element, IUnmanagedMemoryPool pool) { if (Count == Span.Length) { @@ -411,7 +411,7 @@ public bool AddAndReplaceRef(ref T element, IUnmanagedMemoryPool pool) //If we resized only after determining that it was going to be added, //the potential resize would invalidate the computed indices. } - return AddAndReplaceUnsafelyRef(ref element); + return AddAndReplaceUnsafely(ref element); } @@ -422,7 +422,7 @@ public bool AddAndReplaceRef(ref T element, IUnmanagedMemoryPool pool) /// Pool used for spans. /// True if the element was added to the set, false if the element was already present. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool AddRef(ref T element, IUnmanagedMemoryPool pool) + public bool Add(ref T element, IUnmanagedMemoryPool pool) { if (Count == Span.Length) { @@ -433,7 +433,7 @@ public bool AddRef(ref T element, IUnmanagedMemoryPool pool) //If we resized only after determining that it was going to be added, //the potential resize would invalidate the computed indices. } - return AddUnsafelyRef(ref element); + return AddUnsafely(ref element); } @@ -447,7 +447,7 @@ public bool AddRef(ref T element, IUnmanagedMemoryPool pool) [MethodImpl(MethodImplOptions.AggressiveInlining)] public bool AddAndReplace(T element, IUnmanagedMemoryPool pool) { - return AddAndReplaceUnsafelyRef(ref element); + return AddAndReplaceUnsafely(ref element); } @@ -460,7 +460,7 @@ public bool AddAndReplace(T element, IUnmanagedMemoryPool pool) [MethodImpl(MethodImplOptions.AggressiveInlining)] public bool Add(T element, IUnmanagedMemoryPool pool) { - return AddRef(ref element, pool); + return Add(ref element, pool); } //Note: the reason this is named "FastRemove" instead of just "Remove" despite it being the only remove present is that @@ -524,7 +524,7 @@ public void FastRemove(int tableIndex, int elementIndex) /// /// Element to remove. /// True if the element was found and removed, false otherwise. - public bool FastRemoveRef(ref T element) + public bool FastRemove(ref T element) { Validate(); //Find it. @@ -546,7 +546,7 @@ public bool FastRemoveRef(ref T element) [MethodImpl(MethodImplOptions.AggressiveInlining)] public bool FastRemove(T element) { - return FastRemoveRef(ref element); + return FastRemove(ref element); } From 139ca2e2fcda79ee3831c7ef83975f69d3955247 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 24 Sep 2021 17:20:05 -0500 Subject: [PATCH 017/947] Fixed a couple of invalid mesh buffer accesses in MeshReduction. --- BepuPhysics/CollisionDetection/MeshReduction.cs | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/BepuPhysics/CollisionDetection/MeshReduction.cs b/BepuPhysics/CollisionDetection/MeshReduction.cs index 99a746bcb..861ac8fa9 100644 --- a/BepuPhysics/CollisionDetection/MeshReduction.cs +++ b/BepuPhysics/CollisionDetection/MeshReduction.cs @@ -400,7 +400,7 @@ public unsafe static void ReduceManifolds(ref Buffer continuationTrian for (int i = 0; i < count; ++i) { var childIndex = children[i].ChildIndexB; - testTriangles.AddUnsafely(childIndex, new TestTriangle(mesh->Triangles[childIndex], i)); + testTriangles.AddUnsafely(childIndex, new TestTriangle(triangles[i], i)); } for (int i = 0; i < count; ++i) { @@ -421,11 +421,17 @@ public unsafe static void ReduceManifolds(ref Buffer continuationTrian for (int j = 0; j < enumerator.List.Count; ++j) { var triangleIndexInMesh = enumerator.List[j]; - if (!testTriangles.FindOrAllocateSlotUnsafely(triangleIndexInMesh, out var testTriangleIndex)) + if (!testTriangles.FindOrAllocateSlotUnsafely(triangleIndexInMesh, out var triangleIndex)) { - testTriangles.Values[testTriangleIndex] = new TestTriangle(mesh->Triangles[triangleIndexInMesh], testTriangleIndex); + //Note that this does not try to do a direct lookup of the triangle data in the Mesh's triangles buffer! + //That's invalid for two reasons: + //1) in the long term, the mesh type will be abstracted away, and we might be dealing with a type that doesn't have a Triangles buffer at all. + //2) the Mesh applies a scale to the stored triangles! That's why we have the continuation triangles explicitly stored rather than just looking them all up in the mesh- + //the convex-triangle tests that preceded this reduction had to have somewhere they could load the 'baked' triangle data from. + mesh->GetLocalChild(triangleIndexInMesh, out var triangle); + testTriangles.Values[triangleIndex] = new TestTriangle(triangle, triangleIndex); } - ref var targetTriangle = ref testTriangles.Values[testTriangleIndex]; + ref var targetTriangle = ref testTriangles.Values[triangleIndex]; if (ShouldBlockNormal(targetTriangle, meshSpaceContact, meshSpaceNormal)) { From 34d7f0ddf13f2a85549c474e15d699ce1ed815c3 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 24 Sep 2021 17:28:11 -0500 Subject: [PATCH 018/947] Purged epsilon expansion, since MeshReduction has moved away from contact filtering anyway. --- BepuPhysics/BoundingBoxHelpers.cs | 18 ------------------ .../CompoundPairCollisionTask.cs | 8 ++------ .../CompoundPairOverlapFinder.cs | 7 +------ .../ConvexCompoundCollisionTask.cs | 6 +----- .../ConvexCompoundOverlapFinder.cs | 19 ++----------------- .../CollisionTasks/MeshPairOverlapFinder.cs | 7 +------ 6 files changed, 7 insertions(+), 58 deletions(-) diff --git a/BepuPhysics/BoundingBoxHelpers.cs b/BepuPhysics/BoundingBoxHelpers.cs index 5929164ae..9dbd285b5 100644 --- a/BepuPhysics/BoundingBoxHelpers.cs +++ b/BepuPhysics/BoundingBoxHelpers.cs @@ -204,24 +204,6 @@ public static unsafe void ExpandLocalBoundingBoxes(ref Vector3Wide min, ref Vect Vector3Wide.Add(max, localPositionA, out max); } - /// - /// Expands a bounding box by an extremely small amount heuristically. - /// - /// Maximum radius of the shape to base the expansion on. - /// Minimum bounds of the shape to expand. - /// Maximum bounds of the shape to expand. - /// This exists as a byproduct of the MeshReduction's contact filtering policy. Any contacts generated outside the query bounds are ignored. - /// While this works okay for the most part, there are cases where a real contact is just *barely* outside the bounds for numerical reasons and gets removed. - /// In pathological cases that results in a contact with positive depth being removed. Incrementally expanding the bounding boxes avoids this. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void EpsilonExpandLocalBoundingBoxes(Vector maximumRadius, ref Vector3Wide min, ref Vector3Wide max) - { - var expansion = maximumRadius * new Vector(1e-4f); - Vector3Wide.Subtract(min, expansion, out min); - Vector3Wide.Add(max, expansion, out max); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe static void ExpandBoundingBox(in Vector3 expansion, ref Vector3 min, ref Vector3 max) { diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairCollisionTask.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairCollisionTask.cs index 6ea2163dd..df41dd863 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairCollisionTask.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairCollisionTask.cs @@ -11,7 +11,7 @@ namespace BepuPhysics.CollisionDetection.CollisionTasks { public interface ICompoundPairOverlapFinder { - void FindLocalOverlaps(ref Buffer pairs, int pairCount, BufferPool pool, Shapes shapes, float dt, out CompoundPairOverlaps overlaps) where TOverlapTestingOptions : unmanaged, IOverlapTestingOptions; + void FindLocalOverlaps(ref Buffer pairs, int pairCount, BufferPool pool, Shapes shapes, float dt, out CompoundPairOverlaps overlaps); } public unsafe interface ICompoundPairContinuationHandler where TContinuation : struct, ICollisionTestContinuation @@ -53,11 +53,7 @@ public unsafe override void ExecuteBatch(ref UntypedList batch, ref Unsafe.SkipInit(out TContinuationHandler continuationHandler); //We perform all necessary bounding box computations and lookups up front. This helps avoid some instruction pipeline pressure at the cost of some extra data cache requirements. //Because of this, you need to be careful with the batch size on this collision task. - CompoundPairOverlaps overlaps; - if (continuationHandler.CollisionContinuationType == CollisionContinuationType.CompoundMeshReduction || continuationHandler.CollisionContinuationType == CollisionContinuationType.MeshReduction) - overlapFinder.FindLocalOverlaps(ref pairs, batch.Count, batcher.Pool, batcher.Shapes, batcher.Dt, out overlaps); - else - overlapFinder.FindLocalOverlaps(ref pairs, batch.Count, batcher.Pool, batcher.Shapes, batcher.Dt, out overlaps); + overlapFinder.FindLocalOverlaps(ref pairs, batch.Count, batcher.Pool, batcher.Shapes, batcher.Dt, out var overlaps); for (int pairIndex = 0; pairIndex < batch.Count; ++pairIndex) { diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlapFinder.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlapFinder.cs index a23fc5d0e..55ffe5dcb 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlapFinder.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlapFinder.cs @@ -20,7 +20,7 @@ public struct CompoundPairOverlapFinder : ICompoundPairO where TCompoundB : struct, IBoundsQueryableCompound { - public unsafe void FindLocalOverlaps(ref Buffer pairs, int pairCount, BufferPool pool, Shapes shapes, float dt, out CompoundPairOverlaps overlaps) where TOverlapTestingOptions : unmanaged, IOverlapTestingOptions + public unsafe void FindLocalOverlaps(ref Buffer pairs, int pairCount, BufferPool pool, Shapes shapes, float dt, out CompoundPairOverlaps overlaps) { var totalCompoundChildCount = 0; for (int i = 0; i < pairCount; ++i) @@ -59,7 +59,6 @@ public unsafe void FindLocalOverlaps(ref Buffer.Count) { var count = totalCompoundChildCount - i; @@ -106,10 +105,6 @@ out GatherScatter.Get(ref maximumRadius, j), Vector3Wide.Length(localOffsetA, out var radiusA); BoundingBoxHelpers.ExpandLocalBoundingBoxes(ref mins, ref maxes, radiusA, localPositionsA, localRelativeLinearVelocityA, angularVelocityA, angularVelocityB, dt, maximumRadius, maximumAngularExpansion, maximumAllowedExpansion); - if (overlapTestingOptions.EpsilonExpandBounds) - { - BoundingBoxHelpers.EpsilonExpandLocalBoundingBoxes(maximumRadius, ref mins, ref maxes); - } for (int j = 0; j < count; ++j) { diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundCollisionTask.cs b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundCollisionTask.cs index 2a88a7187..6bbed0a5b 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundCollisionTask.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundCollisionTask.cs @@ -45,11 +45,7 @@ public unsafe override void ExecuteBatch(ref UntypedList batch, ref Unsafe.SkipInit(out TContinuationHandler continuationHandler); //We perform all necessary bounding box computations and lookups up front. This helps avoid some instruction pipeline pressure at the cost of some extra data cache requirements. //Because of this, you need to be careful with the batch size on this collision task. - ConvexCompoundTaskOverlaps overlaps; - if (continuationHandler.CollisionContinuationType == CollisionContinuationType.MeshReduction || continuationHandler.CollisionContinuationType == CollisionContinuationType.CompoundMeshReduction) - overlapFinder.FindLocalOverlaps(ref pairs, batch.Count, batcher.Pool, batcher.Shapes, batcher.Dt, out overlaps); - else - overlapFinder.FindLocalOverlaps(ref pairs, batch.Count, batcher.Pool, batcher.Shapes, batcher.Dt, out overlaps); + overlapFinder.FindLocalOverlaps(ref pairs, batch.Count, batcher.Pool, batcher.Shapes, batcher.Dt, out var overlaps); for (int i = 0; i < batch.Count; ++i) { ref var pairOverlaps = ref overlaps.GetOverlapsForPair(i); diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundOverlapFinder.cs b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundOverlapFinder.cs index da5dfe1fd..c1b843e52 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundOverlapFinder.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundOverlapFinder.cs @@ -22,19 +22,9 @@ unsafe void FindLocalOverlaps(ref Buffer(in Vector3 min, in Vector3 max, in Vector3 sweep, float maximumT, BufferPool pool, Shapes shapes, void* overlaps) where TOverlaps : ICollisionTaskSubpairOverlaps; } - public interface IOverlapTestingOptions - { - /// - /// Returns true if pairs should epsilon-expand the query bounds for the sake of a later reduction process, like MeshReduction. - /// This helps avoid situations where contacts that are just barely contained within the bounding box can get filtered out incorrectly by the MeshReduction's heuristics. - /// - bool EpsilonExpandBounds { get; } - } - public struct UseEpsilonBoundsExpansion : IOverlapTestingOptions { public readonly bool EpsilonExpandBounds => true; } - public struct DontUseEpsilonBoundsExpansion : IOverlapTestingOptions { public readonly bool EpsilonExpandBounds => false; } public interface IConvexCompoundOverlapFinder { - void FindLocalOverlaps(ref Buffer pairs, int pairCount, BufferPool pool, Shapes shapes, float dt, out ConvexCompoundTaskOverlaps overlaps) where TOverlapTestingOptions : unmanaged, IOverlapTestingOptions; + void FindLocalOverlaps(ref Buffer pairs, int pairCount, BufferPool pool, Shapes shapes, float dt, out ConvexCompoundTaskOverlaps overlaps); } public struct ConvexCompoundOverlapFinder : IConvexCompoundOverlapFinder @@ -42,7 +32,7 @@ public struct ConvexCompoundOverlapFinder : ICo where TConvexWide : struct, IShapeWide where TCompound : struct, IBoundsQueryableCompound { - public unsafe void FindLocalOverlaps(ref Buffer pairs, int pairCount, BufferPool pool, Shapes shapes, float dt, out ConvexCompoundTaskOverlaps overlaps) where TOverlapTestingOptions : unmanaged, IOverlapTestingOptions + public unsafe void FindLocalOverlaps(ref Buffer pairs, int pairCount, BufferPool pool, Shapes shapes, float dt, out ConvexCompoundTaskOverlaps overlaps) { overlaps = new ConvexCompoundTaskOverlaps(pool, pairCount); ref var pairsToTest = ref overlaps.subpairQueries; @@ -54,7 +44,6 @@ public unsafe void FindLocalOverlaps(ref Buffer maximumAllowedExpansion); Unsafe.SkipInit(out TConvexWide convexWide); - Unsafe.SkipInit(out TOverlapTestingOptions overlapTestingOptions); if (convexWide.InternalAllocationSize > 0) { var memory = stackalloc byte[convexWide.InternalAllocationSize]; @@ -95,10 +84,6 @@ public unsafe void FindLocalOverlaps(ref Buffer.Zero, localPositionA, localRelativeLinearVelocityA, angularVelocityA, angularVelocityB, dt, maximumRadius, maximumAngularExpansion, maximumAllowedExpansion); - if (overlapTestingOptions.EpsilonExpandBounds) - { - BoundingBoxHelpers.EpsilonExpandLocalBoundingBoxes(maximumRadius, ref min, ref max); - } for (int j = 0; j < count; ++j) { diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/MeshPairOverlapFinder.cs b/BepuPhysics/CollisionDetection/CollisionTasks/MeshPairOverlapFinder.cs index f1aaeb6aa..73f356c1a 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/MeshPairOverlapFinder.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/MeshPairOverlapFinder.cs @@ -15,7 +15,7 @@ public struct MeshPairOverlapFinder : ICompoundPairOverlapFinder where TMeshB : struct, IHomogeneousCompoundShape { - public unsafe void FindLocalOverlaps(ref Buffer pairs, int pairCount, BufferPool pool, Shapes shapes, float dt, out CompoundPairOverlaps overlaps) where TOverlapTestingOptions : unmanaged, IOverlapTestingOptions + public unsafe void FindLocalOverlaps(ref Buffer pairs, int pairCount, BufferPool pool, Shapes shapes, float dt, out CompoundPairOverlaps overlaps) { var totalCompoundChildCount = 0; for (int i = 0; i < pairCount; ++i) @@ -40,7 +40,6 @@ public unsafe void FindLocalOverlaps(ref Buffer(ref Buffer Date: Fri, 24 Sep 2021 18:49:49 -0500 Subject: [PATCH 019/947] All triangle pairs now consistent with triangle degeneracy testing and early depth early outs. --- BepuPhysics/Collidables/Triangle.cs | 47 +++++++++++++------ .../CollisionTasks/BoxTriangleTester.cs | 15 +++--- .../CollisionTasks/CapsuleTriangleTester.cs | 22 +++++---- .../CollisionTasks/SphereTriangleTester.cs | 12 ++--- .../TriangleConvexHullTester.cs | 6 +-- .../CollisionTasks/TriangleCylinderTester.cs | 6 +-- .../CollisionTasks/TrianglePairTester.cs | 20 ++++---- .../CollisionDetection/MeshReduction.cs | 3 +- 8 files changed, 79 insertions(+), 52 deletions(-) diff --git a/BepuPhysics/Collidables/Triangle.cs b/BepuPhysics/Collidables/Triangle.cs index 5574f3cd1..2695353e3 100644 --- a/BepuPhysics/Collidables/Triangle.cs +++ b/BepuPhysics/Collidables/Triangle.cs @@ -155,28 +155,47 @@ public void WriteFirst(in Triangle source) public int InternalAllocationSize => 0; public void Initialize(in RawBuffer memory) { } - /// - /// Provides an estimate of the scale of a shape. - /// - /// Approximate scale of the shape for use in epsilons. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void EstimateEpsilonScale(out Vector epsilonScale) + public void WriteSlot(int index, in Triangle source) { - var minX = Vector.Min(A.X, Vector.Min(B.X, C.X)); - var maxX = Vector.Max(A.X, Vector.Max(B.X, C.X)); - var minY = Vector.Min(A.Y, Vector.Min(B.Y, C.Y)); - var maxY = Vector.Max(A.Y, Vector.Max(B.Y, C.Y)); - var minZ = Vector.Min(A.Z, Vector.Min(B.Z, C.Z)); - var maxZ = Vector.Max(A.Z, Vector.Max(B.Z, C.Z)); - epsilonScale = Vector.Max(maxX - minX, Vector.Max(maxY - minY, maxZ - minZ)); + GatherScatter.GetOffsetInstance(ref this, index).WriteFirst(source); } + /// + /// Minimum dot product between the detected local normal and the face normal of a triangle necessary to create contacts. + /// + public const float BackfaceNormalDotRejectionThreshold = -1e-2f; + /// + /// Epsilon to apply to testing triangles for degeneracy (which will be scaled by a pair-determined epsilon scale). Degenerate triangles do not have well defined normals and should not contribute + /// + public const float DegenerateTriangleEpsilon = 1e-6f; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WriteSlot(int index, in Triangle source) + public static void ComputeTriangleEpsilonScale(in Vector abLengthSquared, in Vector caLengthSquared, out Vector epsilonScale) { - GatherScatter.GetOffsetInstance(ref this, index).WriteFirst(source); + epsilonScale = Vector.SquareRoot(Vector.Max(abLengthSquared, caLengthSquared)); } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void ComputeDegenerateTriangleEpsilon(in Vector abLengthSquared, in Vector caLengthSquared, out Vector epsilonScale, out Vector epsilon) + { + ComputeTriangleEpsilonScale(abLengthSquared, caLengthSquared, out epsilonScale); + epsilon = new Vector(DegenerateTriangleEpsilon) * epsilonScale; + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void ComputeNondegenerateTriangleMask(in Vector3Wide ab, in Vector3Wide ca, in Vector triangleNormalLength, out Vector epsilonScale, out Vector nondegenerateMask) + { + Vector3Wide.LengthSquared(ab, out var abLengthSquared); + Vector3Wide.LengthSquared(ca, out var caLengthSquared); + ComputeDegenerateTriangleEpsilon(abLengthSquared, caLengthSquared, out epsilonScale, out var epsilon); + nondegenerateMask = Vector.GreaterThan(triangleNormalLength, epsilon); + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void ComputeNondegenerateTriangleMask(in Vector abLengthSquared, in Vector caLengthSquared, in Vector triangleNormalLength, out Vector epsilonScale, out Vector nondegenerateMask) + { + ComputeDegenerateTriangleEpsilon(abLengthSquared, caLengthSquared, out epsilonScale, out var epsilon); + nondegenerateMask = Vector.GreaterThan(triangleNormalLength, epsilon); + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] public void GetBounds(ref QuaternionWide orientations, int countInBundle, out Vector maximumRadius, out Vector maximumAngularExpansion, out Vector3Wide min, out Vector3Wide max) diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/BoxTriangleTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/BoxTriangleTester.cs index f9d31de9b..2c5103b21 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/BoxTriangleTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/BoxTriangleTester.cs @@ -551,7 +551,13 @@ public unsafe void Test( //If the local normal points against the triangle normal, then it's on the backside and should not collide. Vector3Wide.Dot(localNormal, triangleNormal, out var normalDot); - var allowContacts = Vector.BitwiseAnd(Vector.GreaterThanOrEqual(normalDot, new Vector(SphereTriangleTester.BackfaceNormalDotRejectionThreshold)), activeLanes); + var minimumDepth = -speculativeMargin; + Vector3Wide.LengthSquared(ab, out var abLengthSquared); + Vector3Wide.LengthSquared(ca, out var caLengthSquared); + TriangleWide.ComputeNondegenerateTriangleMask(abLengthSquared, caLengthSquared, triangleNormalLength, out var triangleEpsilonScale, out var nondegenerateMask); + var allowContacts = Vector.BitwiseAnd( + Vector.BitwiseAnd(nondegenerateMask, Vector.GreaterThanOrEqual(normalDot, new Vector(TriangleWide.BackfaceNormalDotRejectionThreshold))), + Vector.BitwiseAnd(Vector.GreaterThanOrEqual(depth, minimumDepth), activeLanes)); if (Vector.EqualsAll(allowContacts, Vector.Zero)) { //All lanes are inactive; early out. @@ -622,12 +628,9 @@ public unsafe void Test( //Note that using a raw absolute epsilon would have a varying effect based on the scale of the involved shapes. //The minimum across the maxes is intended to avoid cases like a huge box being used as a plane, causing a massive size disparity. //Using its sizes as a threshold would tend to kill off perfectly valid contacts. - Vector3Wide.LengthSquared(ab, out var abLengthSquared); - Vector3Wide.LengthSquared(bc, out var bcLengthSquared); - Vector3Wide.LengthSquared(ca, out var caLengthSquared); var epsilonScale = Vector.Min( Vector.Max(a.HalfWidth, Vector.Max(a.HalfHeight, a.HalfLength)), - Vector.SquareRoot(Vector.Max(abLengthSquared, Vector.Max(bcLengthSquared, caLengthSquared)))); + triangleEpsilonScale); //We will be working on the surface of the triangle, but we'd still like a 2d parameterization of the surface for contact reduction. //So, we'll create tangent axes from the edge and edge x normal. @@ -662,7 +665,7 @@ public unsafe void Test( Vector3Wide.Subtract(boxFaceCenter, localTriangleCenter, out var faceCenterBToFaceCenterA); Vector3Wide.Dot(boxFaceNormal, localNormal, out var faceNormalDotNormal); - ManifoldCandidateHelper.Reduce(ref candidates, candidateCount, 6, boxFaceNormal, Vector.One / faceNormalDotNormal, faceCenterBToFaceCenterA, triangleTangentX, triangleTangentY, epsilonScale, -speculativeMargin, pairCount, + ManifoldCandidateHelper.Reduce(ref candidates, candidateCount, 6, boxFaceNormal, Vector.One / faceNormalDotNormal, faceCenterBToFaceCenterA, triangleTangentX, triangleTangentY, epsilonScale, minimumDepth, pairCount, out var contact0, out var contact1, out var contact2, out var contact3, out manifold.Contact0Exists, out manifold.Contact1Exists, out manifold.Contact2Exists, out manifold.Contact3Exists); diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleTriangleTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleTriangleTester.cs index 08d27c424..8d31facdd 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleTriangleTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleTriangleTester.cs @@ -151,8 +151,8 @@ public void Test( Vector3Wide.Subtract(b.B, b.A, out var ab); Vector3Wide.CrossWithoutOverlap(ac, ab, out var acxab); - Vector3Wide.LengthSquared(acxab, out var faceNormalLengthSquared); - Vector3Wide.Scale(acxab, Vector.One / Vector.SquareRoot(faceNormalLengthSquared), out var faceNormal); + Vector3Wide.Length(acxab, out var faceNormalLength); + Vector3Wide.Scale(acxab, Vector.One / faceNormalLength, out var faceNormal); //The depth along the face normal is unaffected by the triangle's extent- the triangle has no extent along its own normal. But the capsule does. Vector3Wide.Dot(faceNormal, localCapsuleAxis, out var nDotAxis); @@ -192,8 +192,12 @@ public void Test( var useEdge = Vector.LessThan(edgeDepth, faceDepth); Vector3Wide.ConditionalSelect(useEdge, edgeNormal, faceNormal, out var localNormal); Vector3Wide.Dot(localNormal, faceNormal, out var localNormalDotFaceNormal); - var collidingWithSolidSide = Vector.GreaterThanOrEqual(localNormalDotFaceNormal, new Vector(SphereTriangleTester.BackfaceNormalDotRejectionThreshold)); - if (Vector.EqualsAll(Vector.BitwiseAnd(collidingWithSolidSide, Vector.GreaterThanOrEqual(depth + a.Radius, -speculativeMargin)), Vector.Zero)) + var collidingWithSolidSide = Vector.GreaterThanOrEqual(localNormalDotFaceNormal, new Vector(TriangleWide.BackfaceNormalDotRejectionThreshold)); + ManifoldCandidateHelper.CreateActiveMask(pairCount, out var activeLanes); + TriangleWide.ComputeNondegenerateTriangleMask(ab, ac, faceNormalLength, out _, out var nondegenerateMask); + var negativeMargin = -speculativeMargin; + var allowContacts = Vector.BitwiseAnd(Vector.BitwiseAnd(Vector.GreaterThanOrEqual(depth + a.Radius, negativeMargin), activeLanes), Vector.BitwiseAnd(collidingWithSolidSide, nondegenerateMask)); + if (Vector.EqualsAll(allowContacts, Vector.Zero)) { //All contact normals are on the back of the triangle or the distance is too large for the margin, so we can immediately quit. manifold.Contact0Exists = Vector.Zero; @@ -203,7 +207,8 @@ public void Test( Unsafe.SkipInit(out Vector3Wide b0); Unsafe.SkipInit(out Vector3Wide b1); Vector contactCount; - if (Vector.EqualsAny(useEdge, new Vector(-1))) + useEdge = Vector.BitwiseAnd(useEdge, allowContacts); + if (Vector.LessThanAny(useEdge, Vector.Zero)) { //At least one of the paths uses edges, so go ahead and create all edge contact related information. //Borrowing from capsule-capsule again: @@ -253,7 +258,7 @@ public void Test( //The bounds check can share the clipping done for face contacts. //3) If an edge contact has generated two contacts, then no additional contacts are required. - if (Vector.LessThanOrEqualAny(contactCount, Vector.One)) + if (Vector.LessThanAny(Vector.BitwiseAnd(Vector.LessThanOrEqual(contactCount, Vector.One), allowContacts), Vector.Zero)) { ClipAgainstEdgePlane(triangle.A, ab, faceNormal, localOffsetA, localCapsuleAxis, out var abEntry, out var abExit); ClipAgainstEdgePlane(triangle.B, bc, faceNormal, localOffsetA, localCapsuleAxis, out var bcEntry, out var bcExit); @@ -338,11 +343,10 @@ public void Test( //In this situation, using more than one contact is pretty pointless anyway, so collapse the manifold to only one point and use the previously computed depth. var collapse = Vector.LessThan(Vector.Abs(faceNormalADotLocalNormal), new Vector(1e-7f)); manifold.Depth0 = Vector.ConditionalSelect(collapse, a.Radius + depth, manifold.Depth0); - var negativeMargin = -speculativeMargin; //If the normal we found points away from the triangle normal, then it it's hitting the wrong side and should be ignored. (Note that we had an early out for this earlier.) contactCount = Vector.ConditionalSelect(collidingWithSolidSide, contactCount, Vector.Zero); - manifold.Contact0Exists = Vector.BitwiseAnd(Vector.GreaterThan(contactCount, Vector.Zero), Vector.GreaterThan(manifold.Depth0, negativeMargin)); - manifold.Contact1Exists = Vector.BitwiseAnd(Vector.AndNot(Vector.Equals(contactCount, new Vector(2)), collapse), Vector.GreaterThan(manifold.Depth1, negativeMargin)); + manifold.Contact0Exists = Vector.BitwiseAnd(allowContacts, Vector.BitwiseAnd(Vector.GreaterThan(contactCount, Vector.Zero), Vector.GreaterThan(manifold.Depth0, negativeMargin))); + manifold.Contact1Exists = Vector.BitwiseAnd(allowContacts, Vector.BitwiseAnd(Vector.AndNot(Vector.Equals(contactCount, new Vector(2)), collapse), Vector.GreaterThan(manifold.Depth1, negativeMargin))); //For feature ids, note that we have a few different potential sources of contacts. While we could go through and force each potential source to output ids, //there is a useful single unifying factor: where the contacts occur on the capsule axis. Using this, it doesn't matter if contacts are generated from face or edge cases, diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/SphereTriangleTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/SphereTriangleTester.cs index ace11812f..f4b8a0bc5 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/SphereTriangleTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/SphereTriangleTester.cs @@ -11,11 +11,6 @@ public struct SphereTriangleTester : IPairTester 32; - /// - /// Minimum dot product between the detected local normal and the face normal of a triangle necessary to create contacts. - /// - public const float BackfaceNormalDotRejectionThreshold = -1e-2f; - public void Test(ref SphereWide a, ref TriangleWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex1ContactManifoldWide manifold) { @@ -115,10 +110,13 @@ public void Test(ref SphereWide a, ref TriangleWide b, ref Vector specula //In the event that the sphere's center point is touching the triangle, the normal is undefined. In that case, the 'correct' normal would be the triangle's normal. //However, given that this is a pretty rare degenerate case and that we already treat triangle backfaces as noncolliding, we'll treat zero distance as a backface non-collision. Vector3Wide.Dot(localTriangleNormal, manifold.Normal, out var faceNormalDotLocalNormal); + TriangleWide.ComputeNondegenerateTriangleMask(ab, ac, triangleNormalLength, out _, out var nondegenerateMask); manifold.ContactExists = Vector.BitwiseAnd( - Vector.GreaterThan(distance, Vector.Zero), Vector.BitwiseAnd( - Vector.LessThanOrEqual(faceNormalDotLocalNormal, new Vector(-BackfaceNormalDotRejectionThreshold)), + Vector.GreaterThan(distance, Vector.Zero), + nondegenerateMask), + Vector.BitwiseAnd( + Vector.LessThanOrEqual(faceNormalDotLocalNormal, new Vector(-TriangleWide.BackfaceNormalDotRejectionThreshold)), Vector.GreaterThanOrEqual(manifold.Depth, -speculativeMargin))); } diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/TriangleConvexHullTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/TriangleConvexHullTester.cs index b77fa11aa..eb6b4f7bf 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/TriangleConvexHullTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/TriangleConvexHullTester.cs @@ -61,11 +61,11 @@ public unsafe void Test(ref TriangleWide a, ref ConvexHullWide b, ref Vector(-SphereTriangleTester.BackfaceNormalDotRejectionThreshold)), Vector.LessThan(depth, depthThreshold))); + inactiveLanes = Vector.BitwiseOr(inactiveLanes, Vector.BitwiseOr(Vector.GreaterThan(triangleNormalDotLocalNormal, new Vector(-TriangleWide.BackfaceNormalDotRejectionThreshold)), Vector.LessThan(depth, depthThreshold))); if (Vector.LessThanAll(inactiveLanes, Vector.Zero)) { //No contacts generated. diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/TriangleCylinderTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/TriangleCylinderTester.cs index bc7ec690a..59b369a82 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/TriangleCylinderTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/TriangleCylinderTester.cs @@ -161,8 +161,8 @@ public unsafe void Test( var cylinderInsideAndBelowTriangle = Vector.BitwiseAnd(cylinderInsideTriangleEdgePlanes, cylinderBelowPlane); ManifoldCandidateHelper.CreateInactiveMask(pairCount, out var inactiveLanes); - var degenerate = Vector.LessThan(triangleNormalLength, new Vector(1e-10f)); - inactiveLanes = Vector.BitwiseOr(degenerate, inactiveLanes); + TriangleWide.ComputeNondegenerateTriangleMask(triangleAB, triangleCA, triangleNormalLength, out var triangleEpsilonScale, out var nondegenerateMask); + inactiveLanes = Vector.BitwiseOr(Vector.OnesComplement(nondegenerateMask), inactiveLanes); inactiveLanes = Vector.BitwiseOr(cylinderInsideAndBelowTriangle, inactiveLanes); if (Vector.LessThanAll(inactiveLanes, Vector.Zero)) { @@ -218,7 +218,7 @@ public unsafe void Test( //If the cylinder is too far away or if it's on the backside of the triangle, don't generate any contacts. Vector3Wide.Dot(triangleNormal, localNormal, out var faceNormalADotNormal); - inactiveLanes = Vector.BitwiseOr(inactiveLanes, Vector.BitwiseOr(Vector.GreaterThan(faceNormalADotNormal, new Vector(-SphereTriangleTester.BackfaceNormalDotRejectionThreshold)), Vector.LessThan(depth, depthThreshold))); + inactiveLanes = Vector.BitwiseOr(inactiveLanes, Vector.BitwiseOr(Vector.GreaterThan(faceNormalADotNormal, new Vector(-TriangleWide.BackfaceNormalDotRejectionThreshold)), Vector.LessThan(depth, depthThreshold))); if (Vector.LessThanAll(inactiveLanes, Vector.Zero)) { //All lanes are either inactive or were found to have a depth lower than the speculative margin, so we can just quit early. diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs index 927f32ef6..9a073430c 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs @@ -291,7 +291,6 @@ public unsafe void Test( Vector3Wide.Subtract(a.C, a.B, out var bcA); Vector3Wide.Subtract(a.A, a.C, out var caA); - ManifoldCandidateHelper.CreateActiveMask(pairCount, out var allowContacts); //A AB x * TestEdgeEdge(abA, abB, a.A, a.B, a.C, bA, bB, bC, out var depth, out var localNormal); @@ -332,6 +331,7 @@ public unsafe void Test( Vector3Wide.LengthSquared(abB, out var abBLengthSquared); Vector3Wide.LengthSquared(caA, out var caALengthSquared); Vector3Wide.LengthSquared(caB, out var caBLengthSquared); + ManifoldCandidateHelper.CreateActiveMask(pairCount, out var allowContacts); //The following was created for MeshReduction when it demanded all contact normals be correct during separation. //Other pairs don't have that requirement, and we ended modifying MeshReduction to be a little less picky. //This remains for posterity because, hey, it works, and if you need it, there it is. @@ -373,11 +373,18 @@ public unsafe void Test( localNormal.Y = Vector.ConditionalSelect(shouldFlip, -localNormal.Y, localNormal.Y); localNormal.Z = Vector.ConditionalSelect(shouldFlip, -localNormal.Z, localNormal.Z); + var minimumDepth = -speculativeMargin; Vector3Wide.Dot(localNormal, faceNormalA, out var localNormalDotFaceNormalA); Vector3Wide.Dot(localNormal, faceNormalB, out var localNormalDotFaceNormalB); - allowContacts = Vector.BitwiseAnd(allowContacts, Vector.BitwiseAnd( - Vector.LessThan(localNormalDotFaceNormalA, new Vector(-SphereTriangleTester.BackfaceNormalDotRejectionThreshold)), - Vector.GreaterThan(localNormalDotFaceNormalB, new Vector(SphereTriangleTester.BackfaceNormalDotRejectionThreshold)))); + TriangleWide.ComputeNondegenerateTriangleMask(abALengthSquared, caALengthSquared, faceNormalALength, out var epsilonScaleA, out var nondegenerateMaskA); + TriangleWide.ComputeNondegenerateTriangleMask(abBLengthSquared, caBLengthSquared, faceNormalBLength, out var epsilonScaleB, out var nondegenerateMaskB); + allowContacts = Vector.BitwiseAnd( + Vector.BitwiseAnd(nondegenerateMaskA, nondegenerateMaskB), + Vector.BitwiseAnd( + Vector.BitwiseAnd(Vector.GreaterThanOrEqual(depth, minimumDepth), allowContacts), + Vector.BitwiseAnd( + Vector.LessThan(localNormalDotFaceNormalA, new Vector(-TriangleWide.BackfaceNormalDotRejectionThreshold)), + Vector.GreaterThan(localNormalDotFaceNormalB, new Vector(TriangleWide.BackfaceNormalDotRejectionThreshold))))); if (Vector.EqualsAll(allowContacts, Vector.Zero)) { manifold.Contact0Exists = default; @@ -436,7 +443,6 @@ public unsafe void Test( var candidateCount = Vector.Zero; ref var candidates = ref *buffer; - var minimumDepth = -speculativeMargin; if (Vector.LessThanAny(useFaceCaseForB, Vector.Zero)) { //While the edge clipping will find any edge-edge or bVertex-aFace contacts, it will not find aVertex-bFace contacts. @@ -477,9 +483,7 @@ public unsafe void Test( } //Create a scale-sensitive epsilon for comparisons based on the size of the involved shapes. This helps avoid varying behavior based on how large involved objects are. - var epsilonScale = Vector.SquareRoot(Vector.Min( - Vector.Max(abALengthSquared, caALengthSquared), - Vector.Max(abBLengthSquared, caBLengthSquared))); + var epsilonScale = Vector.Min(epsilonScaleA, epsilonScaleB); var edgeEpsilon = new Vector(1e-5f) * epsilonScale; ManifoldCandidateHelper.ReduceWithoutComputingDepths(ref candidates, candidateCount, 6, epsilonScale, minimumDepth, pairCount, out var contact0, out var contact1, out var contact2, out var contact3, diff --git a/BepuPhysics/CollisionDetection/MeshReduction.cs b/BepuPhysics/CollisionDetection/MeshReduction.cs index 861ac8fa9..b63f817f5 100644 --- a/BepuPhysics/CollisionDetection/MeshReduction.cs +++ b/BepuPhysics/CollisionDetection/MeshReduction.cs @@ -399,8 +399,7 @@ public unsafe static void ReduceManifolds(ref Buffer continuationTrian //Note that this is also used to keep the indices lined up for the TryApplyBlockToTriangle loop. for (int i = 0; i < count; ++i) { - var childIndex = children[i].ChildIndexB; - testTriangles.AddUnsafely(childIndex, new TestTriangle(triangles[i], i)); + testTriangles.AddUnsafely(children[i].ChildIndexB, new TestTriangle(triangles[i], i)); } for (int i = 0; i < count; ++i) { From fafcfc3f7815b227f40594a7ae3be97227258a34 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 24 Sep 2021 19:14:28 -0500 Subject: [PATCH 020/947] Added box-box depth early out and allowContacts infrastructure. Fixed bug in MeshReduction when dealing with nonstatic meshes. --- .../CollisionTasks/BoxPairTester.cs | 59 ++++++++++++------- .../CollisionDetection/MeshReduction.cs | 15 ++++- 2 files changed, 49 insertions(+), 25 deletions(-) diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/BoxPairTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/BoxPairTester.cs index af1e6fafb..15377bac2 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/BoxPairTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/BoxPairTester.cs @@ -103,7 +103,7 @@ static void Select( [MethodImpl(MethodImplOptions.AggressiveInlining)] static void AddBoxAVertex(in Vector3Wide vertex, in Vector featureId, in Vector3Wide faceNormalB, in Vector3Wide contactNormal, in Vector inverseContactNormalDotFaceNormalB, in Vector3Wide faceCenterB, in Vector3Wide faceTangentBX, in Vector3Wide faceTangentBY, in Vector halfSpanBX, in Vector halfSpanBY, - ref ManifoldCandidate candidates, ref Vector candidateCount, int pairCount) + ref ManifoldCandidate candidates, ref Vector candidateCount, int pairCount, in Vector allowContacts) { //Cast a ray from the box A vertex up to the box B face along the contact normal. Vector3Wide.Subtract(vertex, faceCenterB, out var pointOnBToVertex); @@ -127,7 +127,7 @@ static void AddBoxAVertex(in Vector3Wide vertex, in Vector featureId, in Ve //Rather than assuming our numerical epsilon is guaranteed to always work, explicitly clamp the count. This should essentially never be needed, //but it is very cheap and guarantees no memory stomping with a pretty reasonable fallback. var belowBufferCapacity = Vector.LessThan(candidateCount, new Vector(8)); - var contactExists = Vector.BitwiseAnd(contained, belowBufferCapacity); + var contactExists = Vector.BitwiseAnd(allowContacts, Vector.BitwiseAnd(contained, belowBufferCapacity)); ManifoldCandidateHelper.AddCandidate(ref candidates, ref candidateCount, candidate, contactExists, pairCount); } @@ -136,7 +136,7 @@ private static void AddBoxAVertices(in Vector3Wide faceCenterB, in Vector3Wide f in Vector3Wide faceNormalB, in Vector3Wide contactNormal, in Vector3Wide v00, in Vector3Wide v01, in Vector3Wide v10, in Vector3Wide v11, in Vector f00, in Vector f01, in Vector f10, in Vector f11, - ref ManifoldCandidate candidates, ref Vector candidateCount, int pairCount) + ref ManifoldCandidate candidates, ref Vector candidateCount, int pairCount, in Vector allowContacts) { Vector3Wide.Dot(faceNormalB, contactNormal, out var normalDot); #if DEBUG @@ -150,13 +150,13 @@ private static void AddBoxAVertices(in Vector3Wide faceCenterB, in Vector3Wide f var inverseContactNormalDotFaceNormalB = Vector.ConditionalSelect(Vector.GreaterThan(Vector.Abs(normalDot), new Vector(1e-10f)), Vector.One / normalDot, new Vector(float.MaxValue)); AddBoxAVertex(v00, f00, faceNormalB, contactNormal, inverseContactNormalDotFaceNormalB, faceCenterB, faceTangentBX, faceTangentBY, halfSpanBX, halfSpanBY, - ref candidates, ref candidateCount, pairCount); + ref candidates, ref candidateCount, pairCount, allowContacts); AddBoxAVertex(v01, f01, faceNormalB, contactNormal, inverseContactNormalDotFaceNormalB, faceCenterB, faceTangentBX, faceTangentBY, halfSpanBX, halfSpanBY, - ref candidates, ref candidateCount, pairCount); + ref candidates, ref candidateCount, pairCount, allowContacts); AddBoxAVertex(v10, f10, faceNormalB, contactNormal, inverseContactNormalDotFaceNormalB, faceCenterB, faceTangentBX, faceTangentBY, halfSpanBX, halfSpanBY, - ref candidates, ref candidateCount, pairCount); + ref candidates, ref candidateCount, pairCount, allowContacts); AddBoxAVertex(v11, f11, faceNormalB, contactNormal, inverseContactNormalDotFaceNormalB, faceCenterB, faceTangentBX, faceTangentBY, halfSpanBX, halfSpanBY, - ref candidates, ref candidateCount, pairCount); + ref candidates, ref candidateCount, pairCount, allowContacts); } //[MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -221,19 +221,23 @@ private static void ClipBoxBEdgesAgainstBoxAFace(in Vector3Wide edgeStartB0, in [MethodImpl(MethodImplOptions.AggressiveInlining)] private static void AddContactsForEdge(in Vector min, in ManifoldCandidate minCandidate, in Vector max, in ManifoldCandidate maxCandidate, in Vector halfSpanB, - in Vector epsilon, ref ManifoldCandidate candidates, ref Vector candidateCount, int pairCount) + in Vector epsilon, ref ManifoldCandidate candidates, ref Vector candidateCount, in Vector allowContacts, int pairCount) { //If -halfSpanepsilon for an edge, use the min intersection as a contact. //If -halfSpan<=max<=halfSpan && max>=min, use the max intersection as a contact. //Note the comparisons: if the max lies on a face vertex, it is used, but if the min lies on a face vertex, it is not. This avoids redundant entries. var minExists = Vector.BitwiseAnd( - Vector.GreaterThan(max - min, epsilon), - Vector.LessThan(Vector.Abs(min), halfSpanB)); + allowContacts, + Vector.BitwiseAnd( + Vector.GreaterThan(max - min, epsilon), + Vector.LessThan(Vector.Abs(min), halfSpanB))); ManifoldCandidateHelper.AddCandidate(ref candidates, ref candidateCount, minCandidate, minExists, pairCount); var maxExists = Vector.BitwiseAnd( - Vector.GreaterThanOrEqual(max, min), - Vector.LessThanOrEqual(Vector.Abs(max), halfSpanB)); + allowContacts, + Vector.BitwiseAnd( + Vector.GreaterThanOrEqual(max, min), + Vector.LessThanOrEqual(Vector.Abs(max), halfSpanB))); ManifoldCandidateHelper.AddCandidate(ref candidates, ref candidateCount, maxCandidate, maxExists, pairCount); } @@ -242,7 +246,7 @@ private static void CreateEdgeContacts( in Vector3Wide faceCenterB, in Vector3Wide faceTangentBX, in Vector3Wide faceTangentBY, in Vector halfSpanBX, in Vector halfSpanBY, in Vector3Wide vertexA00, in Vector3Wide vertexA11, in Vector3Wide faceTangentAX, in Vector3Wide faceTangentAY, in Vector3Wide contactNormal, in Vector featureIdX0, in Vector featureIdX1, in Vector featureIdY0, in Vector featureIdY1, - in Vector epsilonScale, ref ManifoldCandidate candidates, ref Vector candidateCount, int pairCount) + in Vector epsilonScale, ref ManifoldCandidate candidates, ref Vector candidateCount, int pairCount, in Vector allowContacts) { //The critical observation here is that we are working in a contact plane defined by the contact normal- not the triangle face normal or the box face normal. //So, when performing clipping, we actually want to clip on the contact normal plane. @@ -284,7 +288,7 @@ private static void CreateEdgeContacts( max.FeatureId = featureIdX0 + edgeFeatureIdOffset; max.X = maxX0; max.Y = min.Y; - AddContactsForEdge(minX0, min, maxX0, max, halfSpanBX, epsilon, ref candidates, ref candidateCount, pairCount); + AddContactsForEdge(minX0, min, maxX0, max, halfSpanBX, epsilon, ref candidates, ref candidateCount, allowContacts, pairCount); //Y1 min.FeatureId = featureIdY1; @@ -293,7 +297,7 @@ private static void CreateEdgeContacts( max.FeatureId = featureIdY1 + edgeFeatureIdOffset; max.X = halfSpanBX; max.Y = maxY1; - AddContactsForEdge(minY1, min, maxY1, max, halfSpanBY, epsilon, ref candidates, ref candidateCount, pairCount); + AddContactsForEdge(minY1, min, maxY1, max, halfSpanBY, epsilon, ref candidates, ref candidateCount, allowContacts, pairCount); //X1 min.FeatureId = featureIdX1; @@ -302,7 +306,7 @@ private static void CreateEdgeContacts( max.FeatureId = featureIdX1 + edgeFeatureIdOffset; max.X = unflippedMinX1; max.Y = halfSpanBY; - AddContactsForEdge(minX1, min, maxX1, max, halfSpanBX, epsilon, ref candidates, ref candidateCount, pairCount); + AddContactsForEdge(minX1, min, maxX1, max, halfSpanBX, epsilon, ref candidates, ref candidateCount, allowContacts, pairCount); //Y0 min.FeatureId = featureIdY0; @@ -311,7 +315,7 @@ private static void CreateEdgeContacts( max.FeatureId = featureIdY0 + edgeFeatureIdOffset; max.X = min.X; max.Y = unflippedMinY0; - AddContactsForEdge(minY0, min, maxY0, max, halfSpanBY, epsilon, ref candidates, ref candidateCount, pairCount); + AddContactsForEdge(minY0, min, maxY0, max, halfSpanBY, epsilon, ref candidates, ref candidateCount, allowContacts, pairCount); } @@ -378,6 +382,17 @@ public unsafe void Test( var faceBZDepth = b.HalfLength + a.HalfWidth * absRBZ.X + a.HalfHeight * absRBZ.Y + a.HalfLength * absRBZ.Z - Vector.Abs(bLocalOffsetB.Z); Select(ref depth, ref localNormal, ref faceBZDepth, ref rB.Z.X, ref rB.Z.Y, ref rB.Z.Z); + ManifoldCandidateHelper.CreateActiveMask(pairCount, out var activeLanes); + var minimumDepth = -speculativeMargin; + var allowContacts = Vector.BitwiseAnd(activeLanes, Vector.GreaterThanOrEqual(depth, minimumDepth)); + if (Vector.EqualsAll(allowContacts, Vector.Zero)) + { + manifold.Contact0Exists = default; + manifold.Contact1Exists = default; + manifold.Contact2Exists = default; + manifold.Contact3Exists = default; + return; + } //Calibrate the normal to point from B to A, matching convention. Vector3Wide.Dot(localNormal, localOffsetB, out var normalDotOffsetB); var shouldNegateNormal = Vector.GreaterThan(normalDotOffsetB, Vector.Zero); @@ -486,7 +501,7 @@ public unsafe void Test( var edgeIdBY1 = axisIdBX * three + twiceAxisIdBY + axisZEdgeIdContribution; var candidateCount = Vector.Zero; CreateEdgeContacts(faceCenterB, tangentBX, tangentBY, halfSpanBX, halfSpanBY, vertexA00, vertexA11, tangentAX, tangentAY, manifold.Normal, - edgeIdBX0, edgeIdBX1, edgeIdBY0, edgeIdBY1, epsilonScale, ref candidates, ref candidateCount, pairCount); + edgeIdBX0, edgeIdBX1, edgeIdBY0, edgeIdBY1, epsilonScale, ref candidates, ref candidateCount, pairCount, allowContacts); //Face A vertices //Vertex ids only have two states per axis, so scale id by 0 or 1 before adding. Equivalent to conditional or. @@ -498,11 +513,11 @@ public unsafe void Test( var vertexId10 = -(axisIdAZ + axisIdAX); var vertexId11 = -(axisIdAZ + axisIdAX + axisIdAY); AddBoxAVertices(faceCenterB, tangentBX, tangentBY, halfSpanBX, halfSpanBY, normalB, manifold.Normal, - vertexA00, vertexA01, vertexA10, vertexA11, vertexId00, vertexId01, vertexId10, vertexId11, ref candidates, ref candidateCount, pairCount); + vertexA00, vertexA01, vertexA10, vertexA11, vertexId00, vertexId01, vertexId10, vertexId11, ref candidates, ref candidateCount, pairCount, allowContacts); - ManifoldCandidateHelper.Reduce(ref candidates, candidateCount, 8, normalA, new Vector(-1f) / Vector.Abs(calibrationDotA), faceCenterBToFaceCenterA, tangentBX, tangentBY, epsilonScale, -speculativeMargin, pairCount, - out var contact0, out var contact1, out var contact2, out var contact3, - out manifold.Contact0Exists, out manifold.Contact1Exists, out manifold.Contact2Exists, out manifold.Contact3Exists); + ManifoldCandidateHelper.Reduce(ref candidates, candidateCount, 8, normalA, new Vector(-1f) / Vector.Abs(calibrationDotA), faceCenterBToFaceCenterA, tangentBX, tangentBY, epsilonScale, minimumDepth, pairCount, + out var contact0, out var contact1, out var contact2, out var contact3, + out manifold.Contact0Exists, out manifold.Contact1Exists, out manifold.Contact2Exists, out manifold.Contact3Exists); //Transform the contacts into the manifold. TransformContactToManifold(ref contact0, ref faceCenterB, ref tangentBX, ref tangentBY, ref manifold.OffsetA0, ref manifold.Depth0, ref manifold.FeatureId0); diff --git a/BepuPhysics/CollisionDetection/MeshReduction.cs b/BepuPhysics/CollisionDetection/MeshReduction.cs index b63f817f5..aed1ec34c 100644 --- a/BepuPhysics/CollisionDetection/MeshReduction.cs +++ b/BepuPhysics/CollisionDetection/MeshReduction.cs @@ -382,7 +382,6 @@ public unsafe static void ReduceManifolds(ref Buffer continuationTrian } else { - ChildEnumerator enumerator; //Queries can sometimes find triangles that are just barely outside the original child set. It's rare, but there's no reason to force a resize if it does happen. //Allocate a bit more to make resizes almost-but-not-quite impossible. @@ -397,9 +396,19 @@ public unsafe static void ReduceManifolds(ref Buffer continuationTrian //We're likely to encounter all the triangles that we collected, so go ahead and create their entries. //Note that this is also used to keep the indices lined up for the TryApplyBlockToTriangle loop. - for (int i = 0; i < count; ++i) + if (requiresFlip) { - testTriangles.AddUnsafely(children[i].ChildIndexB, new TestTriangle(triangles[i], i)); + for (int i = 0; i < count; ++i) + { + testTriangles.AddUnsafely(children[i].ChildIndexB, new TestTriangle(triangles[i], i)); + } + } + else + { + for (int i = 0; i < count; ++i) + { + testTriangles.AddUnsafely(children[i].ChildIndexA, new TestTriangle(triangles[i], i)); + } } for (int i = 0; i < count; ++i) { From 39b51b5890b33c1c1d512c8ce9cb9142ee3d2609 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 24 Sep 2021 19:52:13 -0500 Subject: [PATCH 021/947] Avoided creating test triangles in MeshReduction upfront for children without contacts in query codepath. --- .../CollisionDetection/MeshReduction.cs | 26 +++++++++++-------- 1 file changed, 15 insertions(+), 11 deletions(-) diff --git a/BepuPhysics/CollisionDetection/MeshReduction.cs b/BepuPhysics/CollisionDetection/MeshReduction.cs index aed1ec34c..56fddc633 100644 --- a/BepuPhysics/CollisionDetection/MeshReduction.cs +++ b/BepuPhysics/CollisionDetection/MeshReduction.cs @@ -336,12 +336,12 @@ public unsafe static void ReduceManifolds(ref Buffer continuationTrian for (int i = 0; i < count; ++i) { - ref var sourceTriangle = ref activeTriangles[i]; - ref var sourceChild = ref children[sourceTriangle.ChildIndex]; + ref var sourceChild = ref children[i]; //Can't correct contacts that were created by face collisions. var faceFlagUnset = (sourceChild.Manifold.Contact0.FeatureId & FaceCollisionFlag) == 0; if (faceFlagUnset && sourceChild.Manifold.Count > 0) { + ref var sourceTriangle = ref activeTriangles[i]; ComputeMeshSpaceContact(ref sourceChild.Manifold, meshInverseOrientation, requiresFlip, out var meshSpaceContact, out var meshSpaceNormal); for (int j = 0; j < count; ++j) @@ -394,29 +394,33 @@ public unsafe static void ReduceManifolds(ref Buffer continuationTrian var maxSpan = MathF.Max(span.X, MathF.Max(span.Y, span.Z)); var contactExpansion = new Vector3(maxSpan * 1e-4f); - //We're likely to encounter all the triangles that we collected, so go ahead and create their entries. - //Note that this is also used to keep the indices lined up for the TryApplyBlockToTriangle loop. + //We're guaranteed to encounter all the triangles with contacts that we collected, so go ahead and create their entries. if (requiresFlip) { for (int i = 0; i < count; ++i) { - testTriangles.AddUnsafely(children[i].ChildIndexB, new TestTriangle(triangles[i], i)); + ref var child = ref children[i]; + if (child.Manifold.Count > 0) + testTriangles.AddUnsafely(child.ChildIndexB, new TestTriangle(triangles[i], i)); } } else { for (int i = 0; i < count; ++i) { - testTriangles.AddUnsafely(children[i].ChildIndexA, new TestTriangle(triangles[i], i)); + ref var child = ref children[i]; + if (child.Manifold.Count > 0) + testTriangles.AddUnsafely(child.ChildIndexA, new TestTriangle(triangles[i], i)); } } - for (int i = 0; i < count; ++i) + var activeChildCount = testTriangles.Count; + //Console.WriteLine($"active child count: {activeChildCount}"); + for (int i = 0; i < activeChildCount; ++i) { ref var sourceTriangle = ref testTriangles.Values[i]; ref var sourceChild = ref children[sourceTriangle.ChildIndex]; //Can't correct contacts that were created by face collisions. - var faceFlagUnset = (sourceChild.Manifold.Contact0.FeatureId & FaceCollisionFlag) == 0; - if (faceFlagUnset && sourceChild.Manifold.Count > 0) + if ((sourceChild.Manifold.Contact0.FeatureId & FaceCollisionFlag) == 0) { ComputeMeshSpaceContact(ref sourceChild.Manifold, meshInverseOrientation, requiresFlip, out var meshSpaceContact, out var meshSpaceNormal); var contactQueryMin = meshSpaceContact - contactExpansion; @@ -456,7 +460,7 @@ public unsafe static void ReduceManifolds(ref Buffer continuationTrian // Console.WriteLine($"Iffy dot: {testDot} NOT BLOCKED"); //} } - else if (!faceFlagUnset) + else { //Clear the face flags. This isn't *required* since they're coherent enough anyway and the accumulated impulse redistributor is a decent fallback, //but it costs basically nothing to do this. @@ -467,7 +471,7 @@ public unsafe static void ReduceManifolds(ref Buffer continuationTrian } } - for (int i = 0; i < count; ++i) + for (int i = 0; i < activeChildCount; ++i) { TryApplyBlockToTriangle(ref testTriangles.Values[i], children, meshOrientation, requiresFlip); } From 3fe924e146b0f88f80127e8e1054d764e16e4058 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 24 Sep 2021 20:23:52 -0500 Subject: [PATCH 022/947] Mesh reduction tuning; reintroduced maximum count early out. Added MeshReduction stress test demo. --- .../CollisionDetection/MeshReduction.cs | 6 +- Demos/DemoCallbacks.cs | 11 +- .../SpecializedTests/MeshReductionTestDemo.cs | 194 ++++++++++++++++++ Demos/SpecializedTests/TriangleTestDemo.cs | 6 +- 4 files changed, 205 insertions(+), 12 deletions(-) create mode 100644 Demos/SpecializedTests/MeshReductionTestDemo.cs diff --git a/BepuPhysics/CollisionDetection/MeshReduction.cs b/BepuPhysics/CollisionDetection/MeshReduction.cs index 56fddc633..4d1a101d3 100644 --- a/BepuPhysics/CollisionDetection/MeshReduction.cs +++ b/BepuPhysics/CollisionDetection/MeshReduction.cs @@ -317,13 +317,13 @@ public unsafe static void ReduceManifolds(ref Buffer continuationTrian //beyond that flag, so we can strip the flag now. (We effectively just hijacked the feature id to store some temporary metadata.) //If you don't want to run mesh reduction at all for sufficiently complex pairs, you could simply early out here like so: - //if (count > 1024) - // return; + if (count > 1024) + return; //Narrow the region of interest. continuationTriangles.Slice(start, count, out var triangles); continuationChildren.Slice(start, count, out var children); - const int bruteForceThreshold = 16; + const int bruteForceThreshold = 128; //Console.WriteLine($"count: {count}"); if (count < bruteForceThreshold) { diff --git a/Demos/DemoCallbacks.cs b/Demos/DemoCallbacks.cs index 504bc35fb..f00a08e61 100644 --- a/Demos/DemoCallbacks.cs +++ b/Demos/DemoCallbacks.cs @@ -83,13 +83,12 @@ public void IntegrateVelocity(int bodyIndex, in RigidPose pose, in BodyInertia l } public unsafe struct DemoNarrowPhaseCallbacks : INarrowPhaseCallbacks { - public SpringSettings ContactSpringiness; + public SpringSettings ContactSpringiness = new(30, 1); + public float MaximumRecoveryVelocity = 2f; + public float FrictionCoefficient = 1f; public void Initialize(Simulation simulation) { - //Use a default if the springiness value wasn't initialized. - if (ContactSpringiness.AngularFrequency == 0 && ContactSpringiness.TwiceDampingRatio == 0) - ContactSpringiness = new SpringSettings(30, 1); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -110,8 +109,8 @@ public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int chi [MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold { - pairMaterial.FrictionCoefficient = 1f; - pairMaterial.MaximumRecoveryVelocity = 2f; + pairMaterial.FrictionCoefficient = FrictionCoefficient; + pairMaterial.MaximumRecoveryVelocity = MaximumRecoveryVelocity; pairMaterial.SpringSettings = ContactSpringiness; return true; } diff --git a/Demos/SpecializedTests/MeshReductionTestDemo.cs b/Demos/SpecializedTests/MeshReductionTestDemo.cs new file mode 100644 index 000000000..c3e54a157 --- /dev/null +++ b/Demos/SpecializedTests/MeshReductionTestDemo.cs @@ -0,0 +1,194 @@ +using System; +using System.Numerics; +using BepuPhysics; +using BepuPhysics.Collidables; +using BepuPhysics.Constraints; +using BepuUtilities; +using BepuUtilities.Collections; +using BepuUtilities.Memory; +using DemoContentLoader; +using DemoRenderer; +using DemoRenderer.UI; +using DemoUtilities; +using OpenTK.Input; + +namespace Demos.SpecializedTests +{ + public class MeshReductionTestDemo : Demo + { + + public override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(0, 5, 10); + camera.Yaw = 0; + camera.Pitch = 0; + + //The PositionFirstTimestepper is the simplest timestepping mode, but since it integrates velocity into position at the start of the frame, directly modified velocities outside of the timestep + //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. + //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks() { FrictionCoefficient = 0 }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + + var builder = new CompoundBuilder(BufferPool, Simulation.Shapes, 2); + builder.Add(new Box(1.85f, 0.7f, 4.73f), RigidPose.Identity, 10); + builder.Add(new Box(1.85f, 0.6f, 2.5f), new RigidPose(new Vector3(0, 0.65f, -0.35f)), 0.5f); + builder.BuildDynamicCompound(out var children, out var bodyInertia, out _); + builder.Dispose(); + var bodyShape = new Compound(children); + var bodyShapeIndex = Simulation.Shapes.Add(bodyShape); + var wheelShape = new Cylinder(0.4f, .18f); + wheelShape.ComputeInertia(0.25f, out var wheelInertia); + var wheelShapeIndex = Simulation.Shapes.Add(wheelShape); + + + + const int planeWidth = 257; + const float scale = 3; + Vector2 terrainPosition = new Vector2(1 - planeWidth, 1 - planeWidth) * scale * 0.5f; + + + Vector3 min = new Vector3(-planeWidth * scale * 0.45f, 10, -planeWidth * scale * 0.45f); + Vector3 span = new Vector3(planeWidth * scale * 0.9f, 15, planeWidth * scale * 0.9f); + + + DemoMeshHelper.CreateDeformedPlane(planeWidth, planeWidth, + (int vX, int vY) => + { + var octave0 = (MathF.Sin((vX + 5f) * 0.05f) + MathF.Sin((vY + 11) * 0.05f)) * 1.8f; + var octave1 = (MathF.Sin((vX + 17) * 0.15f) + MathF.Sin((vY + 19) * 0.15f)) * 0.9f; + var octave2 = (MathF.Sin((vX + 37) * 0.35f) + MathF.Sin((vY + 93) * 0.35f)) * 0.4f; + var octave3 = (MathF.Sin((vX + 53) * 0.65f) + MathF.Sin((vY + 47) * 0.65f)) * 0.2f; + var octave4 = (MathF.Sin((vX + 67) * 1.50f) + MathF.Sin((vY + 13) * 1.5f)) * 0.125f; + var distanceToEdge = planeWidth / 2 - Math.Max(Math.Abs(vX - planeWidth / 2), Math.Abs(vY - planeWidth / 2)); + var edgeRamp = 25f / (distanceToEdge + 1); + var terrainHeight = octave0 + octave1 + octave2 + octave3 + octave4; + var vertexPosition = new Vector2(vX * scale, vY * scale) + terrainPosition; + return new Vector3(vertexPosition.X, terrainHeight + edgeRamp, vertexPosition.Y); + + }, new Vector3(1, 1, 1), BufferPool, out var planeMesh); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -15, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), + new CollidableDescription(Simulation.Shapes.Add(planeMesh), 0.1f))); + + var testBox = new Box(3, 3, 3); + testBox.ComputeInertia(1, out var testBoxInertia); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 10, 0), testBoxInertia, new CollidableDescription(Simulation.Shapes.Add(testBox), 10f), new BodyActivityDescription(-1))); + var testSphere = new Sphere(.1f); + testSphere.ComputeInertia(1, out var testSphereInertia); + //testSphereInertia.InverseInertiaTensor = default; + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(10, 10, 0), testSphereInertia, new CollidableDescription(Simulation.Shapes.Add(testSphere), 10f), new BodyActivityDescription(-1))); + var testCylinder = new Cylinder(1.5f, 2f); + testCylinder.ComputeInertia(1, out var testCylinderInertia); + //testCylinderInertia.InverseInertiaTensor = default; + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(15, 10, 0), testCylinderInertia, new CollidableDescription(Simulation.Shapes.Add(testCylinder), 10f), new BodyActivityDescription(-1))); + var testCapsule = new Capsule(.1f, 2f); + testCapsule.ComputeInertia(1, out var testCapsuleInertia); + //testCapsuleInertia.InverseInertiaTensor = default; + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(18, 10, 0), testCapsuleInertia, new CollidableDescription(Simulation.Shapes.Add(testCapsule), 10f), new BodyActivityDescription(-1))); + + var points = new QuickList(8, BufferPool); + points.AllocateUnsafely() = new Vector3(0, 0, 0); + points.AllocateUnsafely() = new Vector3(0, 0, 2); + points.AllocateUnsafely() = new Vector3(2, 0, 0); + points.AllocateUnsafely() = new Vector3(2, 0, 2); + points.AllocateUnsafely() = new Vector3(0, 2, 0); + points.AllocateUnsafely() = new Vector3(0, 2, 2); + points.AllocateUnsafely() = new Vector3(2, 2, 0); + points.AllocateUnsafely() = new Vector3(2, 2, 2); + var convexHull = new ConvexHull(points, BufferPool, out _); + convexHull.ComputeInertia(1, out var convexHullInertia); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(23, 10, 0), convexHullInertia, new CollidableDescription(Simulation.Shapes.Add(convexHull), 10f), new BodyActivityDescription(-1))); + + //var sphere = new Sphere(1.5f); + //var capsule = new Capsule(1f, 1f); + //var box = new Box(32f, 32f, 32f); + //var cylinder = new Cylinder(1.5f, 0.3f); + //const int pointCount = 32; + //var points = new QuickList(pointCount, BufferPool); + ////points.Allocate(BufferPool) = new Vector3(0, 0, 0); + ////points.Allocate(BufferPool) = new Vector3(0, 0, 1); + ////points.Allocate(BufferPool) = new Vector3(0, 1, 0); + ////points.Allocate(BufferPool) = new Vector3(0, 1, 1); + ////points.Allocate(BufferPool) = new Vector3(1, 0, 0); + ////points.Allocate(BufferPool) = new Vector3(1, 0, 1); + ////points.Allocate(BufferPool) = new Vector3(1, 1, 0); + ////points.Allocate(BufferPool) = new Vector3(1, 1, 1); + //var random = new Random(5); + //for (int i = 0; i < pointCount; ++i) + //{ + // points.AllocateUnsafely() = new Vector3(3 * (float)random.NextDouble(), 1 * (float)random.NextDouble(), 3 * (float)random.NextDouble()); + // //points.AllocateUnsafely() = new Vector3(0, 1, 0) + Vector3.Normalize(new Vector3((float)random.NextDouble() * 2 - 1, (float)random.NextDouble() * 2 - 1, (float)random.NextDouble() * 2 - 1)) * (float)random.NextDouble(); + //} + //var convexHull = new ConvexHull(points.Span.Slice(points.Count), BufferPool, out _); + //box.ComputeInertia(1, out var boxInertia); + //capsule.ComputeInertia(1, out var capsuleInertia); + //sphere.ComputeInertia(1, out var sphereInertia); + //cylinder.ComputeInertia(1, out var cylinderInertia); + //convexHull.ComputeInertia(1, out var hullInertia); + //var boxIndex = Simulation.Shapes.Add(box); + //var capsuleIndex = Simulation.Shapes.Add(capsule); + //var sphereIndex = Simulation.Shapes.Add(sphere); + //var cylinderIndex = Simulation.Shapes.Add(cylinder); + //var hullIndex = Simulation.Shapes.Add(convexHull); + + const int width = 12; + const int height = 1; + const int length = 12; + var shapeCount = 0; + var random = new Random(5); + for (int i = 0; i < width; ++i) + { + for (int j = 0; j < height; ++j) + { + for (int k = 0; k < length; ++k) + { + var location = new Vector3(70, 35, 70) * new Vector3(i, j, k) + new Vector3(-width * 70 / 2f, 5f, -length * 70 / 2f); + var bodyDescription = new BodyDescription + { + Activity = new BodyActivityDescription(0.01f), + Pose = new RigidPose + { + Orientation = Quaternion.Identity, + Position = location + }, + Collidable = new CollidableDescription + { + Continuity = new ContinuousDetectionSettings { Mode = ContinuousDetectionMode.Discrete }, + SpeculativeMargin = 0.1f + } + }; + var index = shapeCount++; + switch (index % 5) + { + //case 0: + // bodyDescription.Collidable.Shape = sphereIndex; + // bodyDescription.LocalInertia = sphereInertia; + // break; + //case 1: + // bodyDescription.Collidable.Shape = capsuleIndex; + // bodyDescription.LocalInertia = capsuleInertia; + // break; + case 2: + default: + var box = new Box(1 + 128 * (float)random.NextDouble(), 1 + 128 * (float)random.NextDouble(), 1 + 128 * (float)random.NextDouble()); + box.ComputeInertia(1, out var boxInertia); + bodyDescription.Collidable.Shape = Simulation.Shapes.Add(box); + bodyDescription.LocalInertia = boxInertia; + break; + //case 3: + // bodyDescription.Collidable.Shape = cylinderIndex; + // bodyDescription.LocalInertia = cylinderInertia; + // break; + //case 4: + // bodyDescription.Collidable.Shape = hullIndex; + // bodyDescription.LocalInertia = hullInertia; + // break; + } + var bodyHandle = Simulation.Bodies.Add(bodyDescription); + } + } + } + + + } + + } +} \ No newline at end of file diff --git a/Demos/SpecializedTests/TriangleTestDemo.cs b/Demos/SpecializedTests/TriangleTestDemo.cs index a55e6e4c2..067ba3140 100644 --- a/Demos/SpecializedTests/TriangleTestDemo.cs +++ b/Demos/SpecializedTests/TriangleTestDemo.cs @@ -170,9 +170,9 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var v2 = new Vector3(0, 1.75f, 50); var v3 = new Vector3(8, 1.75f, 50); triangles.AllocateUnsafely() = new Triangle { A = v2, B = v0, C = v1 }; - //triangles.AllocateUnsafely() = new Triangle { A = v2, B = v1, C = v3 }; - //triangles.AllocateUnsafely() = new Triangle { A = v0, B = v2, C = v1 }; - //triangles.AllocateUnsafely() = new Triangle { A = v1, B = v2, C = v3 }; + triangles.AllocateUnsafely() = new Triangle { A = v2, B = v1, C = v3 }; + triangles.AllocateUnsafely() = new Triangle { A = v0, B = v2, C = v1 }; + triangles.AllocateUnsafely() = new Triangle { A = v1, B = v2, C = v3 }; var testMesh = new Mesh(triangles, Vector3.One, BufferPool); Simulation.Statics.Add(new StaticDescription(new Vector3(30, -2.5f, 0), new CollidableDescription(Simulation.Shapes.Add(testMesh), 10.1f))); From 8a3461db2b1650f2a37d4b80f2b2d0566fcd82f3 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 24 Sep 2021 20:53:39 -0500 Subject: [PATCH 023/947] Fixed tooling compatibility oopsy. --- Demos/DemoCallbacks.cs | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/Demos/DemoCallbacks.cs b/Demos/DemoCallbacks.cs index f00a08e61..a9e6e0745 100644 --- a/Demos/DemoCallbacks.cs +++ b/Demos/DemoCallbacks.cs @@ -83,12 +83,19 @@ public void IntegrateVelocity(int bodyIndex, in RigidPose pose, in BodyInertia l } public unsafe struct DemoNarrowPhaseCallbacks : INarrowPhaseCallbacks { - public SpringSettings ContactSpringiness = new(30, 1); - public float MaximumRecoveryVelocity = 2f; - public float FrictionCoefficient = 1f; + public SpringSettings ContactSpringiness; + public float MaximumRecoveryVelocity; + public float FrictionCoefficient; public void Initialize(Simulation simulation) { + //Use a default if the springiness value wasn't initialized... at least until struct field initializers are supported outside of previews. + if (ContactSpringiness.AngularFrequency == 0 && ContactSpringiness.TwiceDampingRatio == 0) + { + ContactSpringiness = new(30, 1); + MaximumRecoveryVelocity = 2f; + FrictionCoefficient = 1f; + } } [MethodImpl(MethodImplOptions.AggressiveInlining)] From b28c0db72c7d8c0a0fbc88132ca19dc329a6b2f6 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 24 Sep 2021 21:48:41 -0500 Subject: [PATCH 024/947] Fixed missing pair properties for some demos. --- Demos/Demos/ContinuousCollisionDetectionDemo.cs | 4 +++- Demos/Demos/RopeStabilityDemo.cs | 4 +++- Demos/Demos/SubsteppingDemo.cs | 4 +++- 3 files changed, 9 insertions(+), 3 deletions(-) diff --git a/Demos/Demos/ContinuousCollisionDetectionDemo.cs b/Demos/Demos/ContinuousCollisionDetectionDemo.cs index e23f2346c..2e503fb98 100644 --- a/Demos/Demos/ContinuousCollisionDetectionDemo.cs +++ b/Demos/Demos/ContinuousCollisionDetectionDemo.cs @@ -52,7 +52,9 @@ public override void Initialize(ContentArchive content, Camera camera) //Also note that the PositionFirstTimestepper is the simplest timestepping mode, but since it integrates velocity into position at the start of the frame, directly modified velocities outside of the timestep //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(120, 1) }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + Simulation = Simulation.Create(BufferPool, + new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(240, 1), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 1f }, + new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); var shape = new Box(1, 1, 1); shape.ComputeInertia(1, out var inertia); diff --git a/Demos/Demos/RopeStabilityDemo.cs b/Demos/Demos/RopeStabilityDemo.cs index fd517de87..f04a174f8 100644 --- a/Demos/Demos/RopeStabilityDemo.cs +++ b/Demos/Demos/RopeStabilityDemo.cs @@ -95,7 +95,9 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(120, 1) }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); //So, even though you can avoid the need for these kinds of hacks, it's good to know that they exist should you find yourself in a circumstance where substepping isn't viable. - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(120, 1) }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + Simulation = Simulation.Create(BufferPool, + new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(120, 1), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 2f }, + new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); rolloverInfo = new RolloverInfo(); var smallWreckingBall = new Sphere(1); diff --git a/Demos/Demos/SubsteppingDemo.cs b/Demos/Demos/SubsteppingDemo.cs index 54bbe44d3..53e7185d8 100644 --- a/Demos/Demos/SubsteppingDemo.cs +++ b/Demos/Demos/SubsteppingDemo.cs @@ -24,7 +24,9 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = 0; camera.Pitch = 0; timestepper = new SubsteppingTimestepper(8); - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(120, 120) }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), timestepper, 8); + Simulation = Simulation.Create(BufferPool, + new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(120, 120), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 2f }, + new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), timestepper, 8); rolloverInfo = new RolloverInfo(); { From 6eb87d2dd76572b0d03496c1fb3f4f5dc0bc7f0d Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 25 Sep 2021 12:26:57 -0500 Subject: [PATCH 025/947] Fixed some documentation errors and a filename swap. --- Demos/SpecializedTests/Media/NewtVideoDemo.cs | 72 +++++++++---------- .../Media/ShrinkwrappedNewtsVideoDemo.cs | 72 ++++++++++--------- Documentation/GettingStarted.md | 2 +- Documentation/StabilityTips.md | 2 +- 4 files changed, 74 insertions(+), 74 deletions(-) diff --git a/Demos/SpecializedTests/Media/NewtVideoDemo.cs b/Demos/SpecializedTests/Media/NewtVideoDemo.cs index 6d713144f..fa5e42e58 100644 --- a/Demos/SpecializedTests/Media/NewtVideoDemo.cs +++ b/Demos/SpecializedTests/Media/NewtVideoDemo.cs @@ -1,70 +1,68 @@ using BepuPhysics; using BepuPhysics.Collidables; +using BepuPhysics.Constraints; using BepuUtilities; -using BepuUtilities.Collections; +using BepuUtilities.Memory; using DemoContentLoader; using DemoRenderer; +using Demos.Demos; using DemoUtilities; using System; using System.Numerics; namespace Demos.SpecializedTests { - public class ShrinkwrappedNewtsVideoDemo : Demo + public class NewtVideoDemo : Demo { public unsafe override void Initialize(ContentArchive content, Camera camera) { - camera.Position = new Vector3(25f, 1.5f, 15f); - camera.Yaw = 3 * MathHelper.Pi / 4; - camera.Pitch = 0;// MathHelper.Pi * 0.15f; + camera.Position = new Vector3(-5f, 5.5f, 5f); + camera.Yaw = MathHelper.Pi / 4; + camera.Pitch = MathHelper.Pi * 0.15f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + var filters = new CollidableProperty(); + Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); var meshContent = content.Load("Content\\newt.obj"); - - //This is actually a pretty good example of how *not* to make a convex hull shape. - //Generating it directly from a graphical data source tends to have way more surface complexity than needed, - //and it tends to have a lot of near-but-not-quite-coplanar surfaces which can make the contact manifold less stable. - //Prefer a simpler source with more distinct features, possibly created with an automated content-time tool. - var points = new QuickList(meshContent.Triangles.Length * 3, BufferPool); - for (int i = 0; i < meshContent.Triangles.Length; ++i) + float cellSize = 0.1f; + DumbTetrahedralizer.Tetrahedralize(meshContent.Triangles, cellSize, BufferPool, + out var vertices, out var vertexSpatialIndices, out var cellVertexIndices, out var tetrahedraVertexIndices); + var weldSpringiness = new SpringSettings(30f, 0); + var volumeSpringiness = new SpringSettings(30f, 1); + for (int i = 0; i < 5; ++i) { - ref var triangle = ref meshContent.Triangles[i]; - //resisting the urge to just reinterpret the memory - points.AllocateUnsafely() = triangle.A * new Vector3(1, 1.5f, 1); - points.AllocateUnsafely() = triangle.B * new Vector3(1, 1.5f, 1); - points.AllocateUnsafely() = triangle.C * new Vector3(1, 1.5f, 1); + NewtDemo.CreateDeformable(Simulation, new Vector3(i * 3, 5 + i * 1.5f, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), MathF.PI * (i * 0.55f)), 1f, cellSize, weldSpringiness, volumeSpringiness, i, filters, ref vertices, ref vertexSpatialIndices, ref cellVertexIndices, ref tetrahedraVertexIndices); } - var newtHull = new ConvexHull(points.Span.Slice(points.Count), BufferPool, out _); - var bodyDescription = BodyDescription.CreateConvexDynamic(RigidPose.Identity, 1, Simulation.Shapes, newtHull); - Random random = new Random(5); - var poseBounds = new BoundingBox { Min = new Vector3(-20, 1, 5), Max = new Vector3(20, 10, 50) }; - for (int i = 0; i < 512; ++i) - { - bodyDescription.Pose = TestHelpers.CreateRandomPose(random, poseBounds); - Simulation.Bodies.Add(bodyDescription); - } + BufferPool.Return(ref vertices); + vertexSpatialIndices.Dispose(BufferPool); + BufferPool.Return(ref cellVertexIndices); + BufferPool.Return(ref tetrahedraVertexIndices); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -30, 250), new CollidableDescription(Simulation.Shapes.Add(new Box(1000, 60, 500)), 0.1f))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -60, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(1000, 1, 1000)), 0.1f))); + Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 100, -.5f), 10, Simulation.Shapes, new Sphere(5))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(1500, 1, 1500)), 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -1.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Sphere(3)), 0.1f))); + var bulletShape = new Sphere(0.5f); + bulletShape.ComputeInertia(.25f, out var bulletInertia); + bulletDescription = BodyDescription.CreateDynamic(RigidPose.Identity, bulletInertia, new CollidableDescription(Simulation.Shapes.Add(bulletShape), 1f), new BodyActivityDescription(0.01f)); - DemoMeshHelper.LoadModel(content, BufferPool, "Content\\newt.obj", new Vector3(1, 1.5f, 1), out mesh); - Simulation.Statics.Add(new StaticDescription(new Vector3(30, 0, 20), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, -3 * MathHelper.PiOver4), new CollidableDescription(Simulation.Shapes.Add(mesh), 0.1f))); + DemoMeshHelper.LoadModel(content, BufferPool, "Content\\newt.obj", new Vector3(20), out var mesh); + Simulation.Statics.Add(new StaticDescription(new Vector3(200, 0.5f, 120), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, -3 * MathHelper.PiOver4), new CollidableDescription(Simulation.Shapes.Add(mesh), 0.1f))); } - - Mesh mesh; - + BodyDescription bulletDescription; public override void Update(Window window, Camera camera, Input input, float dt) { - if(input.WasPushed(OpenTK.Input.Key.Z)) + if (input.WasPushed(OpenTK.Input.Key.Z)) { - mesh.Scale = new Vector3(30); - Simulation.Statics.Add(new StaticDescription(new Vector3(70, 0, 50), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, -3.1f * MathHelper.PiOver4), new CollidableDescription(Simulation.Shapes.Add(mesh), 0.1f))); + bulletDescription.Pose.Position = camera.Position; + bulletDescription.Velocity.Linear = camera.Forward * 40; + Simulation.Bodies.Add(bulletDescription); } base.Update(window, camera, input, dt); } + + } } diff --git a/Demos/SpecializedTests/Media/ShrinkwrappedNewtsVideoDemo.cs b/Demos/SpecializedTests/Media/ShrinkwrappedNewtsVideoDemo.cs index fa5e42e58..6d713144f 100644 --- a/Demos/SpecializedTests/Media/ShrinkwrappedNewtsVideoDemo.cs +++ b/Demos/SpecializedTests/Media/ShrinkwrappedNewtsVideoDemo.cs @@ -1,68 +1,70 @@ using BepuPhysics; using BepuPhysics.Collidables; -using BepuPhysics.Constraints; using BepuUtilities; -using BepuUtilities.Memory; +using BepuUtilities.Collections; using DemoContentLoader; using DemoRenderer; -using Demos.Demos; using DemoUtilities; using System; using System.Numerics; namespace Demos.SpecializedTests { - public class NewtVideoDemo : Demo + public class ShrinkwrappedNewtsVideoDemo : Demo { public unsafe override void Initialize(ContentArchive content, Camera camera) { - camera.Position = new Vector3(-5f, 5.5f, 5f); - camera.Yaw = MathHelper.Pi / 4; - camera.Pitch = MathHelper.Pi * 0.15f; + camera.Position = new Vector3(25f, 1.5f, 15f); + camera.Yaw = 3 * MathHelper.Pi / 4; + camera.Pitch = 0;// MathHelper.Pi * 0.15f; - var filters = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); var meshContent = content.Load("Content\\newt.obj"); - float cellSize = 0.1f; - DumbTetrahedralizer.Tetrahedralize(meshContent.Triangles, cellSize, BufferPool, - out var vertices, out var vertexSpatialIndices, out var cellVertexIndices, out var tetrahedraVertexIndices); - var weldSpringiness = new SpringSettings(30f, 0); - var volumeSpringiness = new SpringSettings(30f, 1); - for (int i = 0; i < 5; ++i) + + //This is actually a pretty good example of how *not* to make a convex hull shape. + //Generating it directly from a graphical data source tends to have way more surface complexity than needed, + //and it tends to have a lot of near-but-not-quite-coplanar surfaces which can make the contact manifold less stable. + //Prefer a simpler source with more distinct features, possibly created with an automated content-time tool. + var points = new QuickList(meshContent.Triangles.Length * 3, BufferPool); + for (int i = 0; i < meshContent.Triangles.Length; ++i) { - NewtDemo.CreateDeformable(Simulation, new Vector3(i * 3, 5 + i * 1.5f, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), MathF.PI * (i * 0.55f)), 1f, cellSize, weldSpringiness, volumeSpringiness, i, filters, ref vertices, ref vertexSpatialIndices, ref cellVertexIndices, ref tetrahedraVertexIndices); + ref var triangle = ref meshContent.Triangles[i]; + //resisting the urge to just reinterpret the memory + points.AllocateUnsafely() = triangle.A * new Vector3(1, 1.5f, 1); + points.AllocateUnsafely() = triangle.B * new Vector3(1, 1.5f, 1); + points.AllocateUnsafely() = triangle.C * new Vector3(1, 1.5f, 1); } - BufferPool.Return(ref vertices); - vertexSpatialIndices.Dispose(BufferPool); - BufferPool.Return(ref cellVertexIndices); - BufferPool.Return(ref tetrahedraVertexIndices); + var newtHull = new ConvexHull(points.Span.Slice(points.Count), BufferPool, out _); + var bodyDescription = BodyDescription.CreateConvexDynamic(RigidPose.Identity, 1, Simulation.Shapes, newtHull); + Random random = new Random(5); + var poseBounds = new BoundingBox { Min = new Vector3(-20, 1, 5), Max = new Vector3(20, 10, 50) }; + for (int i = 0; i < 512; ++i) + { + bodyDescription.Pose = TestHelpers.CreateRandomPose(random, poseBounds); + Simulation.Bodies.Add(bodyDescription); + } - Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 100, -.5f), 10, Simulation.Shapes, new Sphere(5))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -30, 250), new CollidableDescription(Simulation.Shapes.Add(new Box(1000, 60, 500)), 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -60, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(1000, 1, 1000)), 0.1f))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(1500, 1, 1500)), 0.1f))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -1.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Sphere(3)), 0.1f))); - var bulletShape = new Sphere(0.5f); - bulletShape.ComputeInertia(.25f, out var bulletInertia); - bulletDescription = BodyDescription.CreateDynamic(RigidPose.Identity, bulletInertia, new CollidableDescription(Simulation.Shapes.Add(bulletShape), 1f), new BodyActivityDescription(0.01f)); - DemoMeshHelper.LoadModel(content, BufferPool, "Content\\newt.obj", new Vector3(20), out var mesh); - Simulation.Statics.Add(new StaticDescription(new Vector3(200, 0.5f, 120), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, -3 * MathHelper.PiOver4), new CollidableDescription(Simulation.Shapes.Add(mesh), 0.1f))); + DemoMeshHelper.LoadModel(content, BufferPool, "Content\\newt.obj", new Vector3(1, 1.5f, 1), out mesh); + Simulation.Statics.Add(new StaticDescription(new Vector3(30, 0, 20), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, -3 * MathHelper.PiOver4), new CollidableDescription(Simulation.Shapes.Add(mesh), 0.1f))); } - BodyDescription bulletDescription; + + Mesh mesh; + public override void Update(Window window, Camera camera, Input input, float dt) { - if (input.WasPushed(OpenTK.Input.Key.Z)) + if(input.WasPushed(OpenTK.Input.Key.Z)) { - bulletDescription.Pose.Position = camera.Position; - bulletDescription.Velocity.Linear = camera.Forward * 40; - Simulation.Bodies.Add(bulletDescription); + mesh.Scale = new Vector3(30); + Simulation.Statics.Add(new StaticDescription(new Vector3(70, 0, 50), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, -3.1f * MathHelper.PiOver4), new CollidableDescription(Simulation.Shapes.Add(mesh), 0.1f))); } base.Update(window, camera, input, dt); } - - } } diff --git a/Documentation/GettingStarted.md b/Documentation/GettingStarted.md index c4575a8b2..a7bff84da 100644 --- a/Documentation/GettingStarted.md +++ b/Documentation/GettingStarted.md @@ -118,4 +118,4 @@ As the above suggests, the engine uses a lot of idioms which are historically un In summary, it's a very low level API. The intent is to maximize performance, then expose as much as possible to let application-specific convenient abstractions be built on top. -All of this puts a heavier burden on users. They must be familiar with value type semantics, new performance minded language features, pointers, and all sorts of other unusual-for-C# stuff. If you've got questions, feel free to post them on the [forum](https://forum.bepuentertainment.com/). \ No newline at end of file +All of this puts a heavier burden on users. They must be familiar with value type semantics, new performance minded language features, pointers, and all sorts of other unusual-for-C# stuff. If you've got questions, feel free to post them on the [forum](https://github.com/bepu/bepuphysics2/discussions). \ No newline at end of file diff --git a/Documentation/StabilityTips.md b/Documentation/StabilityTips.md index c8f4b7fa0..3d3186612 100644 --- a/Documentation/StabilityTips.md +++ b/Documentation/StabilityTips.md @@ -13,7 +13,7 @@ An example of what it looks like when the solver needs more iterations: ![bounceybounce](images/lowiterationcount.gif) -One notable pathological case for the solver is high mass ratios. Very heavy objects rigidly depending on very light objects can make it nearly impossible for the solver to converge in a reasonable number of velocity iterations. One common example of this is a wrecking ball at the end of a rope composed of a bunch linked bodies. With constraint stiffness configured high enough to hold the wrecking ball, it's unlikely that a 60hz solver update rate and 8 velocity iterations will be sufficient to keep things stable at a 100:1 mass ratio. +One notable pathological case for the solver is high mass ratios. Very heavy objects rigidly depending on very light objects can make it nearly impossible for the solver to converge in a reasonable number of velocity iterations. One common example of this is a wrecking ball at the end of a rope composed of a bunch of linked bodies. With constraint stiffness configured high enough to hold the wrecking ball, it's unlikely that a 60hz solver update rate and 8 velocity iterations will be sufficient to keep things stable at a 100:1 mass ratio. There are ways around this issue, though. Reducing lever arms, adjusting inertias, and adding more paths for the solver to propagate impulses through are useful tricks that can stabilize even some fairly extreme cases. Check out the [RopeStabilityDemo](../Demos/Demos/RopeStabilityDemo.cs) for details. From c45927df1641c0b50d5067e0ae4e4eb6b38a0bd4 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 26 Sep 2021 19:39:39 -0500 Subject: [PATCH 026/947] Fixed some mesh reduction oopsies involving backfacing or distant triangles. --- .../CollisionTasks/MeshPairContinuations.cs | 2 + .../CollisionDetection/MeshReduction.cs | 11 ++-- .../SpecializedTests/MeshReductionTestDemo.cs | 4 +- Demos/SpecializedTests/TriangleTestDemo.cs | 65 ++++++++++++------- 4 files changed, 52 insertions(+), 30 deletions(-) diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/MeshPairContinuations.cs b/BepuPhysics/CollisionDetection/CollisionTasks/MeshPairContinuations.cs index faa0e3dcf..f5f1bead0 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/MeshPairContinuations.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/MeshPairContinuations.cs @@ -29,6 +29,8 @@ public ref CompoundMeshReduction CreateContinuation( continuation.MeshOrientation = pair.OrientationB; //A flip is required in mesh reduction whenever contacts are being generated as if the triangle is in slot B, which is whenever this pair has *not* been flipped. continuation.RequiresFlip = pair.FlipMask == 0; + //TODO: This is not flexible with respect to different mesh types. Not a problem right now, but it will be in the future. + continuation.Mesh = (Mesh*)pair.B; //All regions must be assigned ahead of time. Some trailing regions may be empty, so the dispatch may occur before all children are visited in the later loop. //That would result in potentially uninitialized values in region counts. diff --git a/BepuPhysics/CollisionDetection/MeshReduction.cs b/BepuPhysics/CollisionDetection/MeshReduction.cs index 4d1a101d3..ac827c186 100644 --- a/BepuPhysics/CollisionDetection/MeshReduction.cs +++ b/BepuPhysics/CollisionDetection/MeshReduction.cs @@ -74,7 +74,6 @@ private static unsafe void ComputeMeshSpaceContact(ref ConvexContactManifold man deepestIndex = j; } } - //First, if the manifold considers the mesh and its triangles to be shape B, then we need to flip it. if (requiresFlip) { //If the manifold considers the mesh and its triangles to be shape B, it needs to be flipped before being transformed. @@ -170,7 +169,7 @@ private static unsafe bool ShouldBlockNormal(in TestTriangle triangle, in Vector var distanceAlongNormal = offsetX * triangle.NX + offsetY * triangle.NY + offsetZ * triangle.NZ; //Note that very very thin triangles can result in questionable acceptance due to not checking for true distance- //a position might be way outside a vertex, but still within edge plane thresholds. We're assuming that the impact of this problem will be minimal. - if (distanceAlongNormal.X <= triangle.DistanceThreshold && + if (MathF.Abs(distanceAlongNormal.X) <= triangle.DistanceThreshold && distanceAlongNormal.Y <= triangle.DistanceThreshold && distanceAlongNormal.Z <= triangle.DistanceThreshold && distanceAlongNormal.W <= triangle.DistanceThreshold) @@ -186,10 +185,13 @@ private static unsafe bool ShouldBlockNormal(in TestTriangle triangle, in Vector var onAB = distanceAlongNormal.Y >= negativeThreshold; var onBC = distanceAlongNormal.Z >= negativeThreshold; var onCA = distanceAlongNormal.W >= negativeThreshold; + var normalDot = triangle.NX * meshSpaceNormal.X + triangle.NY * meshSpaceNormal.Y + triangle.NZ * meshSpaceNormal.Z; + //If the normal points in any direction not on the triangle's solid side, then it can't be infringing. + if (normalDot.X > -TriangleWide.BackfaceNormalDotRejectionThreshold) + return false; if (!onAB && !onBC && !onCA) { - //The contact is within the triangle. - //If this contact resulted in a correction, we can skip the remaining contacts in this manifold. + //The contact is within the triangle. return true; } else @@ -198,7 +200,6 @@ private static unsafe bool ShouldBlockNormal(in TestTriangle triangle, in Vector //Remember, the contact has been pushed into mesh space. The position is on the surface of the triangle, and the normal points from convex to mesh. //The edge plane normals point outward from the triangle, so if the contact normal is detected as pointing along the edge plane normal, //then it is infringing. - var normalDot = triangle.NX * meshSpaceNormal.X + triangle.NY * meshSpaceNormal.Y + triangle.NZ * meshSpaceNormal.Z; const float infringementEpsilon = 1e-6f; //In order to block a contact, it must be infringing on every edge that it is on top of. //In other words, when a contact is on a vertex, it's not good enough to infringe only one of the edges; in that case, the contact normal isn't diff --git a/Demos/SpecializedTests/MeshReductionTestDemo.cs b/Demos/SpecializedTests/MeshReductionTestDemo.cs index c3e54a157..e147d0dc0 100644 --- a/Demos/SpecializedTests/MeshReductionTestDemo.cs +++ b/Demos/SpecializedTests/MeshReductionTestDemo.cs @@ -26,7 +26,9 @@ public override void Initialize(ContentArchive content, Camera camera) //The PositionFirstTimestepper is the simplest timestepping mode, but since it integrates velocity into position at the start of the frame, directly modified velocities outside of the timestep //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks() { FrictionCoefficient = 0 }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + Simulation = Simulation.Create(BufferPool, + new DemoNarrowPhaseCallbacks() { ContactSpringiness = new(30, 1), MaximumRecoveryVelocity = 2, FrictionCoefficient = 0 }, + new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); var builder = new CompoundBuilder(BufferPool, Simulation.Shapes, 2); builder.Add(new Box(1.85f, 0.7f, 4.73f), RigidPose.Identity, 10); diff --git a/Demos/SpecializedTests/TriangleTestDemo.cs b/Demos/SpecializedTests/TriangleTestDemo.cs index 067ba3140..09a7f8017 100644 --- a/Demos/SpecializedTests/TriangleTestDemo.cs +++ b/Demos/SpecializedTests/TriangleTestDemo.cs @@ -90,7 +90,9 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathF.PI; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + Simulation = Simulation.Create(BufferPool, + new DemoNarrowPhaseCallbacks() { ContactSpringiness = new BepuPhysics.Constraints.SpringSettings(30, 1), FrictionCoefficient = 1, MaximumRecoveryVelocity = 1 }, + new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); //var triangleDescription = new StaticDescription //{ @@ -126,7 +128,8 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) ////bodyDescription.LocalInertia.InverseInertiaTensor = new Triangular3x3(); //Simulation.Bodies.Add(bodyDescription); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -3, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(200, 1, 200)), 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(200, 5, 200)), 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(10, -2, 30), new CollidableDescription(Simulation.Shapes.Add(new Box(10, 5, 10)), 0.1f))); Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 2, 0), new BodyInertia { InverseMass = 1 }, new CollidableDescription(Simulation.Shapes.Add(new Sphere(1.75f)), 0.1f), new BodyActivityDescription(-1))); Simulation.Bodies.Add(BodyDescription.CreateDynamic(new RigidPose(new Vector3(20, 2, 3), Quaternion.CreateFromYawPitchRoll(0f, 1.745329E-05f, 0f)), new BodyInertia { InverseMass = 1 }, new CollidableDescription(Simulation.Shapes.Add(new Capsule(1, 2)), 0.1f), new BodyActivityDescription(-1))); @@ -164,28 +167,42 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var compound = new Compound(children); Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 3, 14), compoundInertia, new CollidableDescription(Simulation.Shapes.Add(compound), 10.1f), new BodyActivityDescription(-1))); - var triangles = new QuickList(4, BufferPool); - var v0 = new Vector3(0, 1.75f, 0); - var v1 = new Vector3(8, 1.75f, 0); - var v2 = new Vector3(0, 1.75f, 50); - var v3 = new Vector3(8, 1.75f, 50); - triangles.AllocateUnsafely() = new Triangle { A = v2, B = v0, C = v1 }; - triangles.AllocateUnsafely() = new Triangle { A = v2, B = v1, C = v3 }; - triangles.AllocateUnsafely() = new Triangle { A = v0, B = v2, C = v1 }; - triangles.AllocateUnsafely() = new Triangle { A = v1, B = v2, C = v3 }; - var testMesh = new Mesh(triangles, Vector3.One, BufferPool); - - Simulation.Statics.Add(new StaticDescription(new Vector3(30, -2.5f, 0), new CollidableDescription(Simulation.Shapes.Add(testMesh), 10.1f))); - - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -2.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Triangle(v2, v0, v1)), 10.1f))); - - //DemoMeshHelper.LoadModel(content, BufferPool, "Content\\newt.obj", new Vector3(3), out var mesh); - //var collidable = new CollidableDescription(Simulation.Shapes.Add(mesh), 0.2f); - //mesh.ComputeClosedInertia(1, out var newtInertia); - //for (int i = 0; i < 5; ++i) - //{ - // Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(-20, 5 + i * 5, 0), newtInertia, collidable, new BodyActivityDescription(1e-2f))); - //} + { + var triangles = new QuickList(4, BufferPool); + var v0 = new Vector3(0, 1.75f, 0); + var v1 = new Vector3(8, 1.75f, 0); + var v2 = new Vector3(0, 1.75f, 50); + var v3 = new Vector3(8, 1.75f, 50); + triangles.AllocateUnsafely() = new Triangle { A = v2, B = v0, C = v1 }; + triangles.AllocateUnsafely() = new Triangle { A = v2, B = v1, C = v3 }; + triangles.AllocateUnsafely() = new Triangle { A = v0, B = v2, C = v1 }; + triangles.AllocateUnsafely() = new Triangle { A = v1, B = v2, C = v3 }; + var testMesh = new Mesh(triangles, Vector3.One, BufferPool); + Simulation.Statics.Add(new StaticDescription(new Vector3(30, -2.5f, 0), new CollidableDescription(Simulation.Shapes.Add(testMesh), 10.1f))); + + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -2.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Triangle(v2, v0, v1)), 10.1f))); + } + + + DemoMeshHelper.LoadModel(content, BufferPool, "Content\\newt.obj", new Vector3(3), out var mesh); + var collidable = new CollidableDescription(Simulation.Shapes.Add(mesh), 2f); + mesh.ComputeClosedInertia(1, out var newtInertia); + for (int i = 0; i < 5; ++i) + { + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(-20, 5 + i * 5, 0), newtInertia, collidable, new BodyActivityDescription(-1e-2f))); + } + + { + var triangles = new QuickList(4, BufferPool); + var v0 = new Vector3(3, 1f, 0); + var v1 = new Vector3(3, 0, 2); + var v2 = new Vector3(0, 1f, 2); + var v3 = new Vector3(2, 2, 1); + triangles.AllocateUnsafely() = new Triangle { A = v0, B = v2, C = v1 }; + triangles.AllocateUnsafely() = new Triangle { A = v1, B = v2, C = v3 }; + var testMesh = new Mesh(triangles, Vector3.One, BufferPool); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(22, -2.5f, 0), new BodyInertia { InverseMass = 1 }, new CollidableDescription(Simulation.Shapes.Add(testMesh), 10.1f), new BodyActivityDescription(-1f))); + } } } From f6e9c0022205497109107c2460886b6628ade85c Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 26 Sep 2021 21:30:02 -0500 Subject: [PATCH 027/947] Bumped version number. --- BepuPhysics/BepuPhysics.csproj | 2 +- BepuUtilities/BepuUtilities.csproj | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/BepuPhysics/BepuPhysics.csproj b/BepuPhysics/BepuPhysics.csproj index 35d575690..e61e8342f 100644 --- a/BepuPhysics/BepuPhysics.csproj +++ b/BepuPhysics/BepuPhysics.csproj @@ -1,7 +1,7 @@  net5.0 - 2.4.0-beta6 + 2.4.0-beta7 Bepu Entertainment LLC Ross Nordby Speedy real time physics simulation library. diff --git a/BepuUtilities/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj index b6a1b7c25..ff1f08850 100644 --- a/BepuUtilities/BepuUtilities.csproj +++ b/BepuUtilities/BepuUtilities.csproj @@ -3,7 +3,7 @@ BepuUtilities BepuUtilities net5.0 - 2.4.0-beta6 + 2.4.0-beta7 Bepu Entertainment LLC Ross Nordby Supporting utilities library for BEPUphysics v2. From 13b1df148fad5436b33d93277be764326de6273d Mon Sep 17 00:00:00 2001 From: Yevhenii Vitiuk Date: Sat, 2 Oct 2021 22:40:42 +0300 Subject: [PATCH 028/947] Ackerman angle --- Demos/Demos/Cars/CarDemo.cs | 11 ++++-- Demos/Demos/Cars/SimpleCarController.cs | 51 +++++++++++++++++++++++-- 2 files changed, 56 insertions(+), 6 deletions(-) diff --git a/Demos/Demos/Cars/CarDemo.cs b/Demos/Demos/Cars/CarDemo.cs index dd4f07a98..b0bbe4ded 100644 --- a/Demos/Demos/Cars/CarDemo.cs +++ b/Demos/Demos/Cars/CarDemo.cs @@ -61,10 +61,14 @@ public override void Initialize(ContentArchive content, Camera camera) const float y = -0.1f; const float frontZ = 1.7f; const float backZ = -1.7f; + const float wheelBaseWidth = x * 2; + const float wheelBaseLength = frontZ - backZ; + playerController = new SimpleCarController(SimpleCar.Create(Simulation, properties, new RigidPose(new Vector3(0, 10, 0), Quaternion.Identity), bodyShapeIndex, bodyInertia, 0.5f, wheelShapeIndex, wheelInertia, 2f, new Vector3(-x, y, frontZ), new Vector3(x, y, frontZ), new Vector3(-x, y, backZ), new Vector3(x, y, backZ), new Vector3(0, -1, 0), 0.25f, new SpringSettings(5f, 0.7f), QuaternionEx.CreateFromAxisAngle(Vector3.UnitZ, MathF.PI * 0.5f)), - forwardSpeed: 75, forwardForce: 6, zoomMultiplier: 2, backwardSpeed: 30, backwardForce: 4, idleForce: 0.25f, brakeForce: 7, steeringSpeed: 1.5f, maximumSteeringAngle: MathF.PI * 0.23f); + forwardSpeed: 75, forwardForce: 6, zoomMultiplier: 2, backwardSpeed: 30, backwardForce: 4, idleForce: 0.25f, brakeForce: 7, steeringSpeed: 1.5f, maximumSteeringAngle: MathF.PI * 0.23f, + wheelBaseLength: wheelBaseLength, wheelBaseWidth: wheelBaseWidth, ackermanSteering: 1); //Create a bunch of AI cars to race against. const int aiCount = 384; @@ -104,11 +108,12 @@ public override void Initialize(ContentArchive content, Camera camera) aiControllers[i].Controller = new SimpleCarController(SimpleCar.Create(Simulation, properties, new RigidPose(position, orientation), bodyShapeIndex, bodyInertia, 0.5f, wheelShapeIndex, wheelInertia, 2f, new Vector3(-x, y, frontZ), new Vector3(x, y, frontZ), new Vector3(-x, y, backZ), new Vector3(x, y, backZ), new Vector3(0, -1, 0), 0.25f, new SpringSettings(5, 0.7f), QuaternionEx.CreateFromAxisAngle(Vector3.UnitZ, MathF.PI * 0.5f)), - forwardSpeed: 50, forwardForce: 5, zoomMultiplier: 2, backwardSpeed: 10, backwardForce: 4, idleForce: 0.25f, brakeForce: 7, steeringSpeed: 1.5f, maximumSteeringAngle: MathF.PI * 0.23f); + forwardSpeed: 50, forwardForce: 5, zoomMultiplier: 2, backwardSpeed: 10, backwardForce: 4, idleForce: 0.25f, brakeForce: 7, steeringSpeed: 1.5f, maximumSteeringAngle: MathF.PI * 0.23f, + wheelBaseLength: wheelBaseLength, wheelBaseWidth: wheelBaseWidth, ackermanSteering: 1); + aiControllers[i].LaneOffset = (float)random.NextDouble() * 20 - 10; } - DemoMeshHelper.CreateDeformedPlane(planeWidth, planeWidth, (int vX, int vY) => { diff --git a/Demos/Demos/Cars/SimpleCarController.cs b/Demos/Demos/Cars/SimpleCarController.cs index b46bf2665..ffefcfcc9 100644 --- a/Demos/Demos/Cars/SimpleCarController.cs +++ b/Demos/Demos/Cars/SimpleCarController.cs @@ -8,6 +8,7 @@ struct SimpleCarController public SimpleCar Car; private float steeringAngle; + private float wheelBaseHalfWidth; public readonly float SteeringAngle { get { return steeringAngle; } } @@ -21,6 +22,9 @@ struct SimpleCarController public float BackwardForce; public float IdleForce; public float BrakeForce; + public float WheelBaseLength; + public float WheelBaseWidth; + public float AckermanSteering; //Track the previous state to force wakeups if the constraint targets have changed. private float previousTargetSpeed; @@ -28,7 +32,7 @@ struct SimpleCarController public SimpleCarController(SimpleCar car, float forwardSpeed, float forwardForce, float zoomMultiplier, float backwardSpeed, float backwardForce, float idleForce, float brakeForce, - float steeringSpeed, float maximumSteeringAngle) + float steeringSpeed, float maximumSteeringAngle, float wheelBaseLength, float wheelBaseWidth, float ackermanSteering) { Car = car; ForwardSpeed = forwardSpeed; @@ -40,24 +44,65 @@ public SimpleCarController(SimpleCar car, BrakeForce = brakeForce; SteeringSpeed = steeringSpeed; MaximumSteeringAngle = maximumSteeringAngle; + WheelBaseLength = wheelBaseLength; + WheelBaseWidth = wheelBaseWidth; + AckermanSteering = ackermanSteering; + + wheelBaseHalfWidth = WheelBaseWidth * 0.5f; steeringAngle = 0; previousTargetForce = 0; previousTargetSpeed = 0; } + public void Update(Simulation simulation, float dt, float targetSteeringAngle, float targetSpeedFraction, bool zoom, bool brake) { var steeringAngleDifference = targetSteeringAngle - steeringAngle; var maximumChange = SteeringSpeed * dt; var steeringAngleChange = MathF.Min(maximumChange, MathF.Max(-maximumChange, steeringAngleDifference)); var previousSteeringAngle = steeringAngle; + steeringAngle = MathF.Min(MaximumSteeringAngle, MathF.Max(-MaximumSteeringAngle, steeringAngle + steeringAngleChange)); if (steeringAngle != previousSteeringAngle) { + float leftSteeringAngle; + float rightSteeringAngle; + + if (AckermanSteering > 0 && Math.Abs(steeringAngle) > 1e-6) + { + float turnRadius = MathF.Abs(WheelBaseLength * MathF.Tan(MathF.PI * 0.5f - MathF.Abs(steeringAngle))); + + if (steeringAngle > 0) + { + rightSteeringAngle = MathF.Atan(WheelBaseLength / (turnRadius - wheelBaseHalfWidth)); + rightSteeringAngle = (rightSteeringAngle - MathF.Abs(steeringAngle)) * AckermanSteering + MathF.Abs(steeringAngle); + rightSteeringAngle = MathF.Sign(steeringAngle) * rightSteeringAngle; + + leftSteeringAngle = MathF.Atan(WheelBaseLength / (turnRadius + wheelBaseHalfWidth)); + leftSteeringAngle = (leftSteeringAngle - MathF.Abs(steeringAngle)) * AckermanSteering + MathF.Abs(steeringAngle); + leftSteeringAngle = MathF.Sign(steeringAngle) * leftSteeringAngle; + } + else + { + rightSteeringAngle = MathF.Atan(WheelBaseLength / (turnRadius + wheelBaseHalfWidth)); + rightSteeringAngle = (rightSteeringAngle - MathF.Abs(steeringAngle)) * AckermanSteering + MathF.Abs(steeringAngle); + rightSteeringAngle = MathF.Sign(steeringAngle) * rightSteeringAngle; + + leftSteeringAngle = MathF.Atan(WheelBaseLength / (turnRadius - wheelBaseHalfWidth)); + leftSteeringAngle = (leftSteeringAngle - MathF.Abs(steeringAngle)) * AckermanSteering + MathF.Abs(steeringAngle); + leftSteeringAngle = MathF.Sign(steeringAngle) * leftSteeringAngle; + } + } + else + { + leftSteeringAngle = steeringAngle; + rightSteeringAngle = steeringAngle; + } + //By guarding the constraint modifications behind a state test, we avoid waking up the car every single frame. //(We could have also used the ApplyDescriptionWithoutWaking function and then explicitly woke the car up when changes occur.) - Car.Steer(simulation, Car.FrontLeftWheel, steeringAngle); - Car.Steer(simulation, Car.FrontRightWheel, steeringAngle); + Car.Steer(simulation, Car.FrontLeftWheel, leftSteeringAngle); + Car.Steer(simulation, Car.FrontRightWheel, rightSteeringAngle); } float newTargetSpeed, newTargetForce; bool allWheels; From b5f7fb454f586f2683de74a0195d74e5423cb0b5 Mon Sep 17 00:00:00 2001 From: Yevhenii Vitiuk Date: Sat, 2 Oct 2021 23:19:57 +0300 Subject: [PATCH 029/947] Calculate abs steering angle once --- Demos/Demos/Cars/SimpleCarController.cs | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/Demos/Demos/Cars/SimpleCarController.cs b/Demos/Demos/Cars/SimpleCarController.cs index ffefcfcc9..edee064c6 100644 --- a/Demos/Demos/Cars/SimpleCarController.cs +++ b/Demos/Demos/Cars/SimpleCarController.cs @@ -68,28 +68,30 @@ public void Update(Simulation simulation, float dt, float targetSteeringAngle, f float leftSteeringAngle; float rightSteeringAngle; - if (AckermanSteering > 0 && Math.Abs(steeringAngle) > 1e-6) + float steeringAngleAbs = MathF.Abs(steeringAngle); + + if (AckermanSteering > 0 && steeringAngleAbs > 1e-6) { - float turnRadius = MathF.Abs(WheelBaseLength * MathF.Tan(MathF.PI * 0.5f - MathF.Abs(steeringAngle))); + float turnRadius = MathF.Abs(WheelBaseLength * MathF.Tan(MathF.PI * 0.5f - steeringAngleAbs)); if (steeringAngle > 0) { rightSteeringAngle = MathF.Atan(WheelBaseLength / (turnRadius - wheelBaseHalfWidth)); - rightSteeringAngle = (rightSteeringAngle - MathF.Abs(steeringAngle)) * AckermanSteering + MathF.Abs(steeringAngle); + rightSteeringAngle = (rightSteeringAngle - steeringAngleAbs) * AckermanSteering + steeringAngleAbs; rightSteeringAngle = MathF.Sign(steeringAngle) * rightSteeringAngle; leftSteeringAngle = MathF.Atan(WheelBaseLength / (turnRadius + wheelBaseHalfWidth)); - leftSteeringAngle = (leftSteeringAngle - MathF.Abs(steeringAngle)) * AckermanSteering + MathF.Abs(steeringAngle); + leftSteeringAngle = (leftSteeringAngle - steeringAngleAbs) * AckermanSteering + steeringAngleAbs; leftSteeringAngle = MathF.Sign(steeringAngle) * leftSteeringAngle; } else { rightSteeringAngle = MathF.Atan(WheelBaseLength / (turnRadius + wheelBaseHalfWidth)); - rightSteeringAngle = (rightSteeringAngle - MathF.Abs(steeringAngle)) * AckermanSteering + MathF.Abs(steeringAngle); + rightSteeringAngle = (rightSteeringAngle - steeringAngleAbs) * AckermanSteering + steeringAngleAbs; rightSteeringAngle = MathF.Sign(steeringAngle) * rightSteeringAngle; leftSteeringAngle = MathF.Atan(WheelBaseLength / (turnRadius - wheelBaseHalfWidth)); - leftSteeringAngle = (leftSteeringAngle - MathF.Abs(steeringAngle)) * AckermanSteering + MathF.Abs(steeringAngle); + leftSteeringAngle = (leftSteeringAngle - steeringAngleAbs) * AckermanSteering + steeringAngleAbs; leftSteeringAngle = MathF.Sign(steeringAngle) * leftSteeringAngle; } } From 6fe04bdc659ca9cb02ebe50143d6f2622d1cb849 Mon Sep 17 00:00:00 2001 From: Yevhenii Vitiuk Date: Sat, 2 Oct 2021 23:23:38 +0300 Subject: [PATCH 030/947] Simplify logic --- Demos/Demos/Cars/SimpleCarController.cs | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/Demos/Demos/Cars/SimpleCarController.cs b/Demos/Demos/Cars/SimpleCarController.cs index edee064c6..e1f43296a 100644 --- a/Demos/Demos/Cars/SimpleCarController.cs +++ b/Demos/Demos/Cars/SimpleCarController.cs @@ -78,21 +78,19 @@ public void Update(Simulation simulation, float dt, float targetSteeringAngle, f { rightSteeringAngle = MathF.Atan(WheelBaseLength / (turnRadius - wheelBaseHalfWidth)); rightSteeringAngle = (rightSteeringAngle - steeringAngleAbs) * AckermanSteering + steeringAngleAbs; - rightSteeringAngle = MathF.Sign(steeringAngle) * rightSteeringAngle; - + leftSteeringAngle = MathF.Atan(WheelBaseLength / (turnRadius + wheelBaseHalfWidth)); leftSteeringAngle = (leftSteeringAngle - steeringAngleAbs) * AckermanSteering + steeringAngleAbs; - leftSteeringAngle = MathF.Sign(steeringAngle) * leftSteeringAngle; } else { rightSteeringAngle = MathF.Atan(WheelBaseLength / (turnRadius + wheelBaseHalfWidth)); rightSteeringAngle = (rightSteeringAngle - steeringAngleAbs) * AckermanSteering + steeringAngleAbs; - rightSteeringAngle = MathF.Sign(steeringAngle) * rightSteeringAngle; + rightSteeringAngle *= -1; leftSteeringAngle = MathF.Atan(WheelBaseLength / (turnRadius - wheelBaseHalfWidth)); leftSteeringAngle = (leftSteeringAngle - steeringAngleAbs) * AckermanSteering + steeringAngleAbs; - leftSteeringAngle = MathF.Sign(steeringAngle) * leftSteeringAngle; + leftSteeringAngle *= -1; } } else From d152d696175f425e7c9fadfa034eadc4450dc322 Mon Sep 17 00:00:00 2001 From: Yevhenii Vitiuk Date: Sat, 2 Oct 2021 23:35:21 +0300 Subject: [PATCH 031/947] refactoring --- Demos/Demos/Cars/SimpleCarController.cs | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/Demos/Demos/Cars/SimpleCarController.cs b/Demos/Demos/Cars/SimpleCarController.cs index e1f43296a..7cc4bce07 100644 --- a/Demos/Demos/Cars/SimpleCarController.cs +++ b/Demos/Demos/Cars/SimpleCarController.cs @@ -77,20 +77,18 @@ public void Update(Simulation simulation, float dt, float targetSteeringAngle, f if (steeringAngle > 0) { rightSteeringAngle = MathF.Atan(WheelBaseLength / (turnRadius - wheelBaseHalfWidth)); - rightSteeringAngle = (rightSteeringAngle - steeringAngleAbs) * AckermanSteering + steeringAngleAbs; + rightSteeringAngle = steeringAngle + (rightSteeringAngle - steeringAngleAbs) * AckermanSteering; leftSteeringAngle = MathF.Atan(WheelBaseLength / (turnRadius + wheelBaseHalfWidth)); - leftSteeringAngle = (leftSteeringAngle - steeringAngleAbs) * AckermanSteering + steeringAngleAbs; + leftSteeringAngle = steeringAngle + (leftSteeringAngle - steeringAngleAbs) * AckermanSteering; } else { rightSteeringAngle = MathF.Atan(WheelBaseLength / (turnRadius + wheelBaseHalfWidth)); - rightSteeringAngle = (rightSteeringAngle - steeringAngleAbs) * AckermanSteering + steeringAngleAbs; - rightSteeringAngle *= -1; + rightSteeringAngle = steeringAngle - (rightSteeringAngle - steeringAngleAbs) * AckermanSteering; leftSteeringAngle = MathF.Atan(WheelBaseLength / (turnRadius - wheelBaseHalfWidth)); - leftSteeringAngle = (leftSteeringAngle - steeringAngleAbs) * AckermanSteering + steeringAngleAbs; - leftSteeringAngle *= -1; + leftSteeringAngle = steeringAngle - (leftSteeringAngle - steeringAngleAbs) * AckermanSteering; } } else From eb10ea1b9e59c488c1306c72fa543312ad31243c Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 2 Oct 2021 19:28:22 -0500 Subject: [PATCH 032/947] Tiny carfiddles. --- Demos/Demos/Cars/SimpleCarController.cs | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/Demos/Demos/Cars/SimpleCarController.cs b/Demos/Demos/Cars/SimpleCarController.cs index 7cc4bce07..c1d6a809f 100644 --- a/Demos/Demos/Cars/SimpleCarController.cs +++ b/Demos/Demos/Cars/SimpleCarController.cs @@ -8,7 +8,6 @@ struct SimpleCarController public SimpleCar Car; private float steeringAngle; - private float wheelBaseHalfWidth; public readonly float SteeringAngle { get { return steeringAngle; } } @@ -24,6 +23,9 @@ struct SimpleCarController public float BrakeForce; public float WheelBaseLength; public float WheelBaseWidth; + /// + /// Fraction of Ackerman steering angle to apply to wheels. Using 0 does not modify the the steering angle at all, leaving the wheels pointed exactly along the steering angle, while 1 uses the full Ackerman angle. + /// public float AckermanSteering; //Track the previous state to force wakeups if the constraint targets have changed. @@ -48,8 +50,6 @@ public SimpleCarController(SimpleCar car, WheelBaseWidth = wheelBaseWidth; AckermanSteering = ackermanSteering; - wheelBaseHalfWidth = WheelBaseWidth * 0.5f; - steeringAngle = 0; previousTargetForce = 0; previousTargetSpeed = 0; @@ -73,7 +73,7 @@ public void Update(Simulation simulation, float dt, float targetSteeringAngle, f if (AckermanSteering > 0 && steeringAngleAbs > 1e-6) { float turnRadius = MathF.Abs(WheelBaseLength * MathF.Tan(MathF.PI * 0.5f - steeringAngleAbs)); - + var wheelBaseHalfWidth = WheelBaseWidth * 0.5f; if (steeringAngle > 0) { rightSteeringAngle = MathF.Atan(WheelBaseLength / (turnRadius - wheelBaseHalfWidth)); From b7566c2aa0bfa0ecb6b878bdf64e0a59dce86664 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 12 Oct 2021 15:06:40 -0500 Subject: [PATCH 033/947] Contacts blocked by triangles without contacts now treat it as a correction rather than a deletion. --- .../CollisionDetection/MeshReduction.cs | 25 +++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/BepuPhysics/CollisionDetection/MeshReduction.cs b/BepuPhysics/CollisionDetection/MeshReduction.cs index ac827c186..d6bd584ec 100644 --- a/BepuPhysics/CollisionDetection/MeshReduction.cs +++ b/BepuPhysics/CollisionDetection/MeshReduction.cs @@ -328,6 +328,18 @@ public unsafe static void ReduceManifolds(ref Buffer continuationTrian //Console.WriteLine($"count: {count}"); if (count < bruteForceThreshold) { + //Console.WriteLine($"Mesh reduction child count: {count}"); + //for (int i = 0; i < count; ++i) + //{ + // var maxDepth = float.MinValue; + // for (int j = 0; j < children[i].Manifold.Count; ++j) + // { + // var depth = children[i].Manifold.GetDepth(ref children[i].Manifold, j); + // if (depth > maxDepth) + // maxDepth = depth; + // } + // Console.WriteLine($"Contact count in child {i}: {children[i].Manifold.Count}, maximum depth: {maxDepth}"); + //} var memory = stackalloc TestTriangle[count]; var activeTriangles = new Buffer(memory, count); for (int i = 0; i < count; ++i) @@ -352,8 +364,13 @@ public unsafe static void ReduceManifolds(ref Buffer continuationTrian { sourceTriangle.Blocked = true; sourceTriangle.CorrectedNormal = new Vector3(targetTriangle.NX.X, targetTriangle.NY.X, targetTriangle.NZ.X); + //If the blocker had no contacts, it's possible that a collision could exist that has all its contacts deleted. That's not ideal. + //Don't force deletion in that case. The contact normal will be corrected instead. + var correctInsteadOfDeleteIfBlocked = !sourceTriangle.ForceDeletionOnBlock || children[targetTriangle.ChildIndex].Manifold.Count == 0; + sourceTriangle.ForceDeletionOnBlock = !correctInsteadOfDeleteIfBlocked; //Even if the target manifold gets blocked, it should not necessarily be deleted. We made use of it as a blocker. targetTriangle.ForceDeletionOnBlock = false; + //Console.WriteLine($"Child {i} blocked by {j}"); break; } } @@ -376,6 +393,10 @@ public unsafe static void ReduceManifolds(ref Buffer continuationTrian } } } + //for (int i = 0; i < count; ++i) + //{ + // Console.WriteLine($"Child {i} blocked: {activeTriangles[i].Blocked}, force delete: {activeTriangles[i].ForceDeletionOnBlock}"); + //} for (int i = 0; i < count; ++i) { TryApplyBlockToTriangle(ref activeTriangles[i], children, meshOrientation, requiresFlip); @@ -450,6 +471,10 @@ public unsafe static void ReduceManifolds(ref Buffer continuationTrian { sourceTriangle.Blocked = true; sourceTriangle.CorrectedNormal = new Vector3(targetTriangle.NX.X, targetTriangle.NY.X, targetTriangle.NZ.X); + //If the blocker had no contacts, it's possible that a collision could exist that has all its contacts deleted. That's not ideal. + //Don't force deletion in that case. The contact normal will be corrected instead. + var correctInsteadOfDeleteIfBlocked = !sourceTriangle.ForceDeletionOnBlock || (targetTriangle.ChildIndex < activeChildCount && children[targetTriangle.ChildIndex].Manifold.Count == 0); + sourceTriangle.ForceDeletionOnBlock = !correctInsteadOfDeleteIfBlocked; //Even if the target manifold gets blocked, it should not necessarily be deleted. We made use of it as a blocker. targetTriangle.ForceDeletionOnBlock = false; break; From 070c71fcd7ab024039dba520eb3d13bcf60192f2 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 20 Oct 2021 17:00:23 -0500 Subject: [PATCH 034/947] Fixed determinism failure caused by lane interference in triangle-cylinder. --- .../CollisionTasks/TriangleCylinderTester.cs | 32 +++++++++---------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/TriangleCylinderTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/TriangleCylinderTester.cs index 59b369a82..0337c5116 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/TriangleCylinderTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/TriangleCylinderTester.cs @@ -531,17 +531,17 @@ public unsafe void Test( exitBC = Vector.ConditionalSelect(bcIsDominant, exitBC * restrictWeight + unrestrictWeight, exitBC); exitCA = Vector.ConditionalSelect(caIsDominant, exitCA * restrictWeight + unrestrictWeight, exitCA); - cylinderTMin = Vector.Max(entryAB, Vector.Max(entryBC, entryCA)); - cylinderTMax = Vector.Min(exitAB, Vector.Min(exitBC, exitCA)); + var sideTriangleCylinderTMin = Vector.Max(entryAB, Vector.Max(entryBC, entryCA)); + var sideTriangleCylinderTMax = Vector.Min(exitAB, Vector.Min(exitBC, exitCA)); //Note that the local normal should have been created such that projecting the cylinder edge down along it *should * result in an intersection with the triangle. //That would mean exitT >= entryT.However, due to numerical error, that is not strictly guaranteed. //This will typically happen in a vertex case. //We can choose the vertex in these cases by examining which edges contributed the intersections forming the bounds of the interval. - var useVertexFallback = Vector.BitwiseAnd(Vector.LessThan(cylinderTMax, cylinderTMin), useSideTriangleFace); - var abContributedBound = Vector.BitwiseOr(Vector.Equals(edgeTAB, cylinderTMin), Vector.Equals(edgeTAB, cylinderTMax)); - var bcContributedBound = Vector.BitwiseOr(Vector.Equals(edgeTBC, cylinderTMin), Vector.Equals(edgeTBC, cylinderTMax)); - var caContributedBound = Vector.BitwiseOr(Vector.Equals(edgeTCA, cylinderTMin), Vector.Equals(edgeTCA, cylinderTMax)); + var useVertexFallback = Vector.BitwiseAnd(Vector.LessThan(sideTriangleCylinderTMax, sideTriangleCylinderTMin), useSideTriangleFace); + var abContributedBound = Vector.BitwiseOr(Vector.Equals(edgeTAB, sideTriangleCylinderTMin), Vector.Equals(edgeTAB, sideTriangleCylinderTMax)); + var bcContributedBound = Vector.BitwiseOr(Vector.Equals(edgeTBC, sideTriangleCylinderTMin), Vector.Equals(edgeTBC, sideTriangleCylinderTMax)); + var caContributedBound = Vector.BitwiseOr(Vector.Equals(edgeTCA, sideTriangleCylinderTMin), Vector.Equals(edgeTCA, sideTriangleCylinderTMax)); var useA = Vector.BitwiseAnd(caContributedBound, abContributedBound); var useB = Vector.BitwiseAnd(abContributedBound, bcContributedBound); //var useC = Vector.BitwiseAnd(bcContributedBound, caContributedBound); @@ -549,22 +549,22 @@ public unsafe void Test( Vector3Wide.ConditionalSelect(useB, triangleB, vertexFallback, out vertexFallback); //Bound the interval to the cylinder's extent. - cylinderTMin = Vector.Max(Vector.Zero, Vector.Min(Vector.One, cylinderTMin)); - cylinderTMax = Vector.Max(Vector.Zero, Vector.Min(Vector.One, cylinderTMax)); - localOffsetB0.X = Vector.ConditionalSelect(useSideTriangleFace, minOnTriangle.X + minToMax.X * cylinderTMin, localOffsetB0.X); - localOffsetB0.Y = Vector.ConditionalSelect(useSideTriangleFace, minOnTriangle.Y + minToMax.Y * cylinderTMin, localOffsetB0.Y); - localOffsetB0.Z = Vector.ConditionalSelect(useSideTriangleFace, minOnTriangle.Z + minToMax.Z * cylinderTMin, localOffsetB0.Z); - localOffsetB1.X = Vector.ConditionalSelect(useSideTriangleFace, minOnTriangle.X + minToMax.X * cylinderTMax, localOffsetB1.X); - localOffsetB1.Y = Vector.ConditionalSelect(useSideTriangleFace, minOnTriangle.Y + minToMax.Y * cylinderTMax, localOffsetB1.Y); - localOffsetB1.Z = Vector.ConditionalSelect(useSideTriangleFace, minOnTriangle.Z + minToMax.Z * cylinderTMax, localOffsetB1.Z); + cylinderTMin = Vector.ConditionalSelect(useSideTriangleFace, Vector.Max(Vector.Zero, Vector.Min(Vector.One, sideTriangleCylinderTMin)), cylinderTMin); + cylinderTMax = Vector.ConditionalSelect(useSideTriangleFace, Vector.Max(Vector.Zero, Vector.Min(Vector.One, sideTriangleCylinderTMax)), cylinderTMax); + localOffsetB0.X = Vector.ConditionalSelect(useSideTriangleFace, minOnTriangle.X + minToMax.X * sideTriangleCylinderTMin, localOffsetB0.X); + localOffsetB0.Y = Vector.ConditionalSelect(useSideTriangleFace, minOnTriangle.Y + minToMax.Y * sideTriangleCylinderTMin, localOffsetB0.Y); + localOffsetB0.Z = Vector.ConditionalSelect(useSideTriangleFace, minOnTriangle.Z + minToMax.Z * sideTriangleCylinderTMin, localOffsetB0.Z); + localOffsetB1.X = Vector.ConditionalSelect(useSideTriangleFace, minOnTriangle.X + minToMax.X * sideTriangleCylinderTMax, localOffsetB1.X); + localOffsetB1.Y = Vector.ConditionalSelect(useSideTriangleFace, minOnTriangle.Y + minToMax.Y * sideTriangleCylinderTMax, localOffsetB1.Y); + localOffsetB1.Z = Vector.ConditionalSelect(useSideTriangleFace, minOnTriangle.Z + minToMax.Z * sideTriangleCylinderTMax, localOffsetB1.Z); Vector3Wide.ConditionalSelect(useVertexFallback, vertexFallback, localOffsetB0, out localOffsetB0); //Ray cast back to the cylinder's side to compute the depth for the contact. //t = dot(localNormal.X0Z, cylinderSideEdgeCenter - {entry, exit}) / dot(localNormal.X0Z, localNormal) var inverseDepthDenominator = Vector.One / (localNormal.X * localNormal.X + localNormal.Z * localNormal.Z); - depthTMin = (localNormal.X * (closestOnB.X - localOffsetB0.X) + localNormal.Z * (closestOnB.Z - localOffsetB0.Z)) * inverseDepthDenominator; - depthTMax = (localNormal.X * (closestOnB.X - localOffsetB1.X) + localNormal.Z * (closestOnB.Z - localOffsetB1.Z)) * inverseDepthDenominator; + depthTMin = Vector.ConditionalSelect(useSideTriangleFace, (localNormal.X * (closestOnB.X - localOffsetB0.X) + localNormal.Z * (closestOnB.Z - localOffsetB0.Z)) * inverseDepthDenominator, depthTMin); + depthTMax = Vector.ConditionalSelect(useSideTriangleFace, (localNormal.X * (closestOnB.X - localOffsetB1.X) + localNormal.Z * (closestOnB.Z - localOffsetB1.Z)) * inverseDepthDenominator, depthTMax); } manifold.FeatureId0 = Vector.ConditionalSelect(useSide, Vector.Zero, manifold.FeatureId0); manifold.FeatureId1 = Vector.ConditionalSelect(useSide, Vector.One, manifold.FeatureId1); From 31463ba67aee28314d33fc009dfa2f024b65130e Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 25 Oct 2021 18:27:49 -0500 Subject: [PATCH 035/947] Ungoofed triangle-cylinder interval clamping. --- .../CollisionTasks/TriangleCylinderTester.cs | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/TriangleCylinderTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/TriangleCylinderTester.cs index 0337c5116..9877add11 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/TriangleCylinderTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/TriangleCylinderTester.cs @@ -551,12 +551,12 @@ public unsafe void Test( //Bound the interval to the cylinder's extent. cylinderTMin = Vector.ConditionalSelect(useSideTriangleFace, Vector.Max(Vector.Zero, Vector.Min(Vector.One, sideTriangleCylinderTMin)), cylinderTMin); cylinderTMax = Vector.ConditionalSelect(useSideTriangleFace, Vector.Max(Vector.Zero, Vector.Min(Vector.One, sideTriangleCylinderTMax)), cylinderTMax); - localOffsetB0.X = Vector.ConditionalSelect(useSideTriangleFace, minOnTriangle.X + minToMax.X * sideTriangleCylinderTMin, localOffsetB0.X); - localOffsetB0.Y = Vector.ConditionalSelect(useSideTriangleFace, minOnTriangle.Y + minToMax.Y * sideTriangleCylinderTMin, localOffsetB0.Y); - localOffsetB0.Z = Vector.ConditionalSelect(useSideTriangleFace, minOnTriangle.Z + minToMax.Z * sideTriangleCylinderTMin, localOffsetB0.Z); - localOffsetB1.X = Vector.ConditionalSelect(useSideTriangleFace, minOnTriangle.X + minToMax.X * sideTriangleCylinderTMax, localOffsetB1.X); - localOffsetB1.Y = Vector.ConditionalSelect(useSideTriangleFace, minOnTriangle.Y + minToMax.Y * sideTriangleCylinderTMax, localOffsetB1.Y); - localOffsetB1.Z = Vector.ConditionalSelect(useSideTriangleFace, minOnTriangle.Z + minToMax.Z * sideTriangleCylinderTMax, localOffsetB1.Z); + localOffsetB0.X = Vector.ConditionalSelect(useSideTriangleFace, minOnTriangle.X + minToMax.X * cylinderTMin, localOffsetB0.X); + localOffsetB0.Y = Vector.ConditionalSelect(useSideTriangleFace, minOnTriangle.Y + minToMax.Y * cylinderTMin, localOffsetB0.Y); + localOffsetB0.Z = Vector.ConditionalSelect(useSideTriangleFace, minOnTriangle.Z + minToMax.Z * cylinderTMin, localOffsetB0.Z); + localOffsetB1.X = Vector.ConditionalSelect(useSideTriangleFace, minOnTriangle.X + minToMax.X * cylinderTMax, localOffsetB1.X); + localOffsetB1.Y = Vector.ConditionalSelect(useSideTriangleFace, minOnTriangle.Y + minToMax.Y * cylinderTMax, localOffsetB1.Y); + localOffsetB1.Z = Vector.ConditionalSelect(useSideTriangleFace, minOnTriangle.Z + minToMax.Z * cylinderTMax, localOffsetB1.Z); Vector3Wide.ConditionalSelect(useVertexFallback, vertexFallback, localOffsetB0, out localOffsetB0); From e07cf8840c3c522ce475a15e4dabf8680bd02e68 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 1 Nov 2021 17:07:50 -0500 Subject: [PATCH 036/947] Improved sleeping/static handling in ContactEventsDemo and made reported collidable order for flush-triggered pairs consistent with the narrow phase order. --- BepuPhysics/CollisionDetection/NarrowPhase.cs | 44 +++++++---- Demos/Demos/ContactEventsDemo.cs | 76 ++++++++++++------- 2 files changed, 78 insertions(+), 42 deletions(-) diff --git a/BepuPhysics/CollisionDetection/NarrowPhase.cs b/BepuPhysics/CollisionDetection/NarrowPhase.cs index e89c4fe7b..93f279a02 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhase.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhase.cs @@ -297,9 +297,33 @@ public void Dispose() protected abstract void OnDispose(); - - //TODO: Configurable memory usage. It automatically adapts based on last frame state, but it's nice to be able to specify minimums when more information is known. - + /// + /// Sorts references to guarantee that two collidables in the same pair will always be in the same order. + /// + /// First collidable reference to sort. + /// First collidable reference to sort. + /// Mobility extracted from collidable A. + /// Mobility extracted from collidable B. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void SortCollidableReferencesForPair(CollidableReference a, CollidableReference b, out CollidableMobility aMobility, out CollidableMobility bMobility, out CollidableReference sortedA, out CollidableReference sortedB) + { + //In order to guarantee contact manifold and constraint consistency across multiple frames, the order of collidables submitted to collision testing must be + //the same every time. Since the provided handles do not move for the lifespan of the collidable in the simulation, they can be used as an ordering. + //Between two bodies, simply put the lower handle in slot A always. + //If one of the two objects is static, stick it in the second slot. + aMobility = a.Mobility; + bMobility = b.Mobility; + if ((aMobility != CollidableMobility.Static && bMobility != CollidableMobility.Static && a.BodyHandle.Value > b.BodyHandle.Value) || aMobility == CollidableMobility.Static) + { + sortedA = b; + sortedB = a; + } + else + { + sortedA = a; + sortedB = b; + } + } } /// @@ -378,19 +402,7 @@ protected override void OnDispose() public unsafe void HandleOverlap(int workerIndex, CollidableReference a, CollidableReference b) { Debug.Assert(a.Packed != b.Packed, "Excuse me, broad phase, but an object cannot collide with itself!"); - //In order to guarantee contact manifold and constraint consistency across multiple frames, we must guarantee that the order of collidables submitted - //is the same every time. Since the provided handles do not move for the lifespan of the collidable in the simulation, they can be used as an ordering. - //Between two bodies, simply put the lower handle in slot A always. - //If one of the two objects is static, stick it in the second slot. - var aMobility = a.Mobility; - var bMobility = b.Mobility; - if ((aMobility != CollidableMobility.Static && bMobility != CollidableMobility.Static && a.BodyHandle.Value > b.BodyHandle.Value) || - aMobility == CollidableMobility.Static) - { - var temp = b; - b = a; - a = temp; - } + SortCollidableReferencesForPair(a, b, out var aMobility, out var bMobility, out a, out b); Debug.Assert(aMobility != CollidableMobility.Static || bMobility != CollidableMobility.Static, "Broad phase should not be able to generate static-static pairs."); if (!Callbacks.AllowContactGeneration(workerIndex, a, b)) return; diff --git a/Demos/Demos/ContactEventsDemo.cs b/Demos/Demos/ContactEventsDemo.cs index c1fbdc5f9..2461b19f4 100644 --- a/Demos/Demos/ContactEventsDemo.cs +++ b/Demos/Demos/ContactEventsDemo.cs @@ -188,7 +188,6 @@ struct PreviousCollision struct Listener { public CollidableReference Source; - public bool WasAwake; public IContactEventHandler Handler; public QuickList PreviousCollisions; } @@ -227,6 +226,7 @@ public void Initialize(Simulation simulation) this.simulation = simulation; if (pool == null) pool = simulation.BufferPool; + simulation.Timestepper.BeforeCollisionDetection += SetFreshnessForCurrentActivityStatus; listenerIndices = new CollidableProperty(simulation, pool); pendingWorkerAdds = new QuickList[threadDispatcher == null ? 1 : threadDispatcher.ThreadCount]; } @@ -337,6 +337,47 @@ public bool IsListener(CollidableReference collidable) } } + /// + /// Callback attached to the simulation's ITimestepper which executes just prior to collision detection to take a snapshot of activity states to determine which pairs we should expect updates in. + /// + void SetFreshnessForCurrentActivityStatus(float dt, IThreadDispatcher threadDispatcher) + { + //Every single pair tracked by the contact events has a 'freshness' flag. If the final flush sees a pair that is stale, it'll remove it + //and any necessary events to represent the end of that pair are reported. + //HandleManifoldForCollidable sets 'Fresh' to true for any processed pair, but pairs between sleeping or static bodies will not show up in HandleManifoldForCollidable since they're not active. + //We don't want Flush to report that sleeping pairs have stopped colliding, so we pre-initialize any such sleeping/static pair as 'fresh'. + + //This could be multithreaded reasonably easily if there are a ton of listeners or collisions, but that would be a pretty high bar. + //For simplicity, the demo will keep it single threaded. + var bodyHandleToLocation = simulation.Bodies.HandleToLocation; + for (int listenerIndex = 0; listenerIndex < listenerCount; ++listenerIndex) + { + ref var listener = ref listeners[listenerIndex]; + var source = listener.Source; + //If it's a body, and it's in the active set (index 0), then every pair associated with the listener should expect updates. + var sourceExpectsUpdates = source.Mobility != CollidableMobility.Static && bodyHandleToLocation[source.BodyHandle.Value].SetIndex == 0; + if (sourceExpectsUpdates) + { + var previousCollisions = listeners[listenerIndex].PreviousCollisions; + for (int j = 0; j < previousCollisions.Count; ++j) + { + //Pair updates will set the 'freshness' to true when they happen, so that they won't be considered 'stale' in the flush and removed. + previousCollisions[j].Fresh = false; + } + } + else + { + //The listener is either static or sleeping. We should only expect updates if the other collidable is awake. + var previousCollisions = listeners[listenerIndex].PreviousCollisions; + for (int j = 0; j < previousCollisions.Count; ++j) + { + ref var previousCollision = ref previousCollisions[j]; + previousCollision.Fresh = previousCollision.Collidable.Mobility == CollidableMobility.Static || bodyHandleToLocation[previousCollision.Collidable.BodyHandle.Value].SetIndex > 0; + } + } + } + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] void UpdatePreviousCollision(ref PreviousCollision collision, ref TManifold manifold, bool isTouching) where TManifold : unmanaged, IContactManifold { @@ -362,17 +403,6 @@ void HandleManifoldForCollidable(int workerIndex, CollidableReference //This collidable is registered. Is the opposing collidable present? ref var listener = ref listeners[listenerIndex]; - //If this listener is sleeping or static and is getting HandleManifoldForCollidable called on it, it means that there's an active body interacting with it. - //That doesn't mean the listener is active *yet*- all other pairs that are only between sleeping bodies and/or statics will not receive any updates. - //We don't want those to be considered 'stale' by the postpass for a lack of updating, so we store a note that this listener was sleeping. - //That's necessary because if contacts constraints are allowed, the listener's going to be woken up. - - //(There's a risk of thread access contention here. Not wonderfully ideal, but for the purposes of the demo, it's simpler to do this than - //to introduce another phase which checks sleeping status as a prepass.) - var awake = listener.Source.Mobility != CollidableMobility.Static && simulation.Bodies.GetBodyReference(listener.Source.BodyHandle).Awake; - if (awake != listener.WasAwake) - listener.WasAwake = awake; - int previousCollisionIndex = -1; bool isTouching = false; for (int i = 0; i < listener.PreviousCollisions.Count; ++i) @@ -494,23 +524,15 @@ public void Flush() for (int i = 0; i < listenerCount; ++i) { ref var listener = ref listeners[i]; - //If the listener fell asleep at the start of the frame and didn't get woken up, then it wouldn't have been informed that is now asleep, so we'll check for that here. - if (listener.WasAwake) - listener.WasAwake = listener.Source.Mobility == CollidableMobility.Static || simulation.Bodies.HandleToLocation[listener.Source.BodyHandle.Value].SetIndex == 0; //Note reverse order. We remove during iteration. for (int j = listener.PreviousCollisions.Count - 1; j >= 0; --j) { ref var collision = ref listener.PreviousCollisions[j]; - //At least one of the two bodies in a pair must have been awake to warrant examination. - //Pairs involved with only inactive bodies or statics do not need to be checked for freshness. If we did, it would result in inactive manifolds being considered a removal, and - //more contact added events would fire when the bodies woke up. - if (!listener.WasAwake && (collision.Collidable.Mobility == CollidableMobility.Static || simulation.Bodies.HandleToLocation[collision.Collidable.BodyHandle.Value].SetIndex > 0)) - { - continue; - } if (!collision.Fresh) { - var pair = new CollidablePair(listener.Source, collision.Collidable); + //Sort the references to be consistent with the direct narrow phase results. + CollidablePair pair; + NarrowPhase.SortCollidableReferencesForPair(listener.Source, collision.Collidable, out _, out _, out pair.A, out pair.B); if (collision.ContactCount > 0) { var emptyManifold = new EmptyManifold(); @@ -557,9 +579,12 @@ public void Flush() public void Dispose() { - bodyListenerFlags.Dispose(pool); - staticListenerFlags.Dispose(pool); + if (bodyListenerFlags.Flags.Allocated) + bodyListenerFlags.Dispose(pool); + if (staticListenerFlags.Flags.Allocated) + staticListenerFlags.Dispose(pool); listenerIndices.Dispose(); + simulation.Timestepper.BeforeCollisionDetection -= SetFreshnessForCurrentActivityStatus; for (int i = 0; i < pendingWorkerAdds.Length; ++i) { Debug.Assert(!pendingWorkerAdds[i].Span.Allocated, "The pending worker adds should have been disposed by the previous flush."); @@ -692,7 +717,6 @@ public override void Initialize(ContentArchive content, Camera camera) Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(30, 1, 30)), 0.04f))); Simulation.Statics.Add(new StaticDescription(new Vector3(0, 3, 15), new CollidableDescription(Simulation.Shapes.Add(new Box(30, 5, 1)), 0.04f))); - } public override void Update(Window window, Camera camera, Input input, float dt) From a5049c6cb06797a9bb3dfad9a6cb86775cf3f91b Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 2 Nov 2021 15:04:10 -0500 Subject: [PATCH 037/947] Bumped beta version number and generated doc file (oooOOPS). --- BepuPhysics/BepuPhysics.csproj | 6 +++--- BepuUtilities/BepuUtilities.csproj | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/BepuPhysics/BepuPhysics.csproj b/BepuPhysics/BepuPhysics.csproj index e61e8342f..c8cb55114 100644 --- a/BepuPhysics/BepuPhysics.csproj +++ b/BepuPhysics/BepuPhysics.csproj @@ -1,7 +1,7 @@  net5.0 - 2.4.0-beta7 + 2.4.0-beta8 Bepu Entertainment LLC Ross Nordby Speedy real time physics simulation library. @@ -13,11 +13,11 @@ Debug;Release;ReleaseNoProfiling latest physics;3d;rigid body;real time;simulation - True - + True true false key.snk + true diff --git a/BepuUtilities/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj index ff1f08850..a6ba9614d 100644 --- a/BepuUtilities/BepuUtilities.csproj +++ b/BepuUtilities/BepuUtilities.csproj @@ -3,7 +3,7 @@ BepuUtilities BepuUtilities net5.0 - 2.4.0-beta7 + 2.4.0-beta8 Bepu Entertainment LLC Ross Nordby Supporting utilities library for BEPUphysics v2. @@ -14,11 +14,11 @@ bepuphysicslogo256.png Debug;Release latest - True - + True true false key.snk + true From 796c2e573ce9b3ee97cdbb1f0d4d1986114cfe89 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 8 Feb 2021 22:13:23 -0600 Subject: [PATCH 038/947] Added embedded substepping solver. Alas, warm starting. --- .../Constraints/FourBodyTypeProcessor.cs | 64 ++++++++ .../Constraints/OneBodyTypeProcessor.cs | 53 +++++++ .../Constraints/ThreeBodyTypeProcessor.cs | 62 ++++++++ .../Constraints/TwoBodyTypeProcessor.cs | 57 +++++++ BepuPhysics/Constraints/TypeProcessor.cs | 3 + BepuPhysics/EmbeddedSubsteppingTimestepper.cs | 109 +++++++++++++ BepuPhysics/Solver.cs | 1 + BepuPhysics/Solver_Intermediate.cs | 108 ------------- BepuPhysics/Solver_Naive.cs | 120 --------------- BepuPhysics/Solver_SubsteppingSolve.cs | 143 ++++++++++++++++++ 10 files changed, 492 insertions(+), 228 deletions(-) create mode 100644 BepuPhysics/EmbeddedSubsteppingTimestepper.cs delete mode 100644 BepuPhysics/Solver_Intermediate.cs delete mode 100644 BepuPhysics/Solver_Naive.cs create mode 100644 BepuPhysics/Solver_SubsteppingSolve.cs diff --git a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs index 93dc7c486..e04dad99e 100644 --- a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs @@ -221,5 +221,69 @@ public unsafe override void JacobiSolveIteration(ref TypeBatch typeBatch, ref Bu } } + public unsafe override void SolveStep(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + { + ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); + ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); + ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); + ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); + ref var bodyVelocities = ref bodies.ActiveSet.Velocities; + var function = default(TConstraintFunctions); + for (int i = startBundle; i < exclusiveEndBundle; ++i) + { + ref var prestep = ref Unsafe.Add(ref prestepBase, i); + ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); + ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); + ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); + var count = GetCountInBundle(ref typeBatch, i); + bodies.GatherInertia(ref references, count, out var inertiaA, out var inertiaB, out var inertiaC, out var inertiaD); + function.Prestep(bodies, ref references, count, dt, inverseDt, ref inertiaA, ref inertiaB, ref inertiaC, ref inertiaD, ref prestep, out var projection); + Bodies.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out var wsvA, out var wsvB, out var wsvC, out var wsvD); + function.WarmStart(ref wsvA, ref wsvB, ref wsvC, ref wsvD, ref projection, ref accumulatedImpulses); + function.Solve(ref wsvA, ref wsvB, ref wsvC, ref wsvD, ref projection, ref accumulatedImpulses); + Bodies.ScatterVelocities(ref wsvA, ref wsvB, ref wsvC, ref wsvD, ref bodyVelocities, ref bodyReferences, count); + } + } + + public unsafe override void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + { + ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); + ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); + ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); + ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); + ref var bodyVelocities = ref bodies.ActiveSet.Velocities; + var function = default(TConstraintFunctions); + ref var jacobiResultsBundlesA = ref jacobiResults.GetVelocitiesForBody(0); + ref var jacobiResultsBundlesB = ref jacobiResults.GetVelocitiesForBody(1); + ref var jacobiResultsBundlesC = ref jacobiResults.GetVelocitiesForBody(2); + ref var jacobiResultsBundlesD = ref jacobiResults.GetVelocitiesForBody(3); + for (int i = startBundle; i < exclusiveEndBundle; ++i) + { + ref var prestep = ref Unsafe.Add(ref prestepBase, i); + ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); + ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); + ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); + var count = GetCountInBundle(ref typeBatch, i); + bodies.GatherInertia(ref references, count, out var inertiaA, out var inertiaB, out var inertiaC, out var inertiaD); + //Jacobi batches split affected bodies into multiple pieces to guarantee convergence. + jacobiBatch.GetJacobiScaleForBodies(ref references, count, out var jacobiScaleA, out var jacobiScaleB, out var jacobiScaleC, out var jacobiScaleD); + Symmetric3x3Wide.Scale(inertiaA.InverseInertiaTensor, jacobiScaleA, out inertiaA.InverseInertiaTensor); + inertiaA.InverseMass *= jacobiScaleA; + Symmetric3x3Wide.Scale(inertiaB.InverseInertiaTensor, jacobiScaleB, out inertiaB.InverseInertiaTensor); + inertiaB.InverseMass *= jacobiScaleB; + Symmetric3x3Wide.Scale(inertiaC.InverseInertiaTensor, jacobiScaleC, out inertiaC.InverseInertiaTensor); + inertiaC.InverseMass *= jacobiScaleC; + Symmetric3x3Wide.Scale(inertiaD.InverseInertiaTensor, jacobiScaleD, out inertiaD.InverseInertiaTensor); + inertiaD.InverseMass *= jacobiScaleD; + function.Prestep(bodies, ref references, count, dt, inverseDt, ref inertiaA, ref inertiaB, ref inertiaC, ref inertiaD, ref prestep, out var projection); + ref var wsvA = ref jacobiResultsBundlesA[i]; + ref var wsvB = ref jacobiResultsBundlesB[i]; + ref var wsvC = ref jacobiResultsBundlesC[i]; + ref var wsvD = ref jacobiResultsBundlesD[i]; + Bodies.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out wsvA, out wsvB, out wsvC, out wsvD); + function.WarmStart(ref wsvA, ref wsvB, ref wsvC, ref wsvD, ref projection, ref accumulatedImpulses); + function.Solve(ref wsvA, ref wsvB, ref wsvC, ref wsvD, ref projection, ref accumulatedImpulses); + } + } } } diff --git a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs index b75c99d34..3c6b575da 100644 --- a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs @@ -199,6 +199,59 @@ public unsafe override void JacobiSolveIteration(ref TypeBatch typeBatch, ref Bu } } + public unsafe override void SolveStep(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + { + ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); + ref var bodyReferencesBase = ref Unsafe.AsRef>(typeBatch.BodyReferences.Memory); + ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); + ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); + ref var bodyVelocities = ref bodies.ActiveSet.Velocities; + var function = default(TConstraintFunctions); + for (int i = startBundle; i < exclusiveEndBundle; ++i) + { + ref var prestep = ref Unsafe.Add(ref prestepBase, i); + ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); + ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); + ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); + var count = GetCountInBundle(ref typeBatch, i); + bodies.GatherInertia(ref references, count, out var inertiaA); + function.Prestep(bodies, ref references, count, dt, inverseDt, ref inertiaA, ref prestep, out var projection); + Bodies.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out var wsvA); + function.WarmStart(ref wsvA, ref projection, ref accumulatedImpulses); + function.Solve(ref wsvA, ref projection, ref accumulatedImpulses); + Bodies.ScatterVelocities(ref wsvA, ref bodyVelocities, ref bodyReferences, count); + } + } + + public unsafe override void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + { + ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); + ref var bodyReferencesBase = ref Unsafe.AsRef>(typeBatch.BodyReferences.Memory); + ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); + ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); + ref var bodyVelocities = ref bodies.ActiveSet.Velocities; + var function = default(TConstraintFunctions); + ref var jacobiResultsBundlesA = ref jacobiResults.GetVelocitiesForBody(0); + for (int i = startBundle; i < exclusiveEndBundle; ++i) + { + ref var prestep = ref Unsafe.Add(ref prestepBase, i); + ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); + ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); + ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); + var count = GetCountInBundle(ref typeBatch, i); + bodies.GatherInertia(ref references, count, out var inertiaA); + //Jacobi batches split affected bodies into multiple pieces to guarantee convergence. + jacobiBatch.GetJacobiScaleForBodies(ref references, count, out var jacobiScale); + Symmetric3x3Wide.Scale(inertiaA.InverseInertiaTensor, jacobiScale, out inertiaA.InverseInertiaTensor); + inertiaA.InverseMass *= jacobiScale; + function.Prestep(bodies, ref references, count, dt, inverseDt, ref inertiaA, ref prestep, out var projection); + ref var wsvA = ref jacobiResultsBundlesA[i]; + Bodies.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out wsvA); + function.WarmStart(ref wsvA, ref projection, ref accumulatedImpulses); + function.Solve(ref wsvA, ref projection, ref accumulatedImpulses); + } + } + } public abstract class OneBodyContactTypeProcessor diff --git a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs index 833ad367a..9899c8e68 100644 --- a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs @@ -213,5 +213,67 @@ public unsafe override void JacobiSolveIteration(ref TypeBatch typeBatch, ref Bu } } + + public unsafe override void SolveStep(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + { + ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); + ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); + ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); + ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); + ref var bodyVelocities = ref bodies.ActiveSet.Velocities; + var function = default(TConstraintFunctions); + for (int i = startBundle; i < exclusiveEndBundle; ++i) + { + ref var prestep = ref Unsafe.Add(ref prestepBase, i); + ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); + ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); + ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); + var count = GetCountInBundle(ref typeBatch, i); + bodies.GatherInertia(ref references, count, out var inertiaA, out var inertiaB, out var inertiaC); + function.Prestep(bodies, ref references, count, dt, inverseDt, ref inertiaA, ref inertiaB, ref inertiaC, ref prestep, out var projection); + Bodies.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out var wsvA, out var wsvB, out var wsvC); + function.WarmStart(ref wsvA, ref wsvB, ref wsvC, ref projection, ref accumulatedImpulses); + function.Solve(ref wsvA, ref wsvB, ref wsvC, ref projection, ref accumulatedImpulses); + Bodies.ScatterVelocities(ref wsvA, ref wsvB, ref wsvC, ref bodyVelocities, ref bodyReferences, count); + } + } + + public unsafe override void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + { + ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); + ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); + ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); + ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); + ref var bodyVelocities = ref bodies.ActiveSet.Velocities; + var function = default(TConstraintFunctions); + ref var jacobiResultsBundlesA = ref jacobiResults.GetVelocitiesForBody(0); + ref var jacobiResultsBundlesB = ref jacobiResults.GetVelocitiesForBody(1); + ref var jacobiResultsBundlesC = ref jacobiResults.GetVelocitiesForBody(2); + for (int i = startBundle; i < exclusiveEndBundle; ++i) + { + ref var prestep = ref Unsafe.Add(ref prestepBase, i); + ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); + ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); + ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); + var count = GetCountInBundle(ref typeBatch, i); + bodies.GatherInertia(ref references, count, out var inertiaA, out var inertiaB, out var inertiaC); + //Jacobi batches split affected bodies into multiple pieces to guarantee convergence. + jacobiBatch.GetJacobiScaleForBodies(ref references, count, out var jacobiScaleA, out var jacobiScaleB, out var jacobiScaleC); + Symmetric3x3Wide.Scale(inertiaA.InverseInertiaTensor, jacobiScaleA, out inertiaA.InverseInertiaTensor); + inertiaA.InverseMass *= jacobiScaleA; + Symmetric3x3Wide.Scale(inertiaB.InverseInertiaTensor, jacobiScaleB, out inertiaB.InverseInertiaTensor); + inertiaB.InverseMass *= jacobiScaleB; + Symmetric3x3Wide.Scale(inertiaC.InverseInertiaTensor, jacobiScaleC, out inertiaC.InverseInertiaTensor); + inertiaC.InverseMass *= jacobiScaleC; + function.Prestep(bodies, ref references, count, dt, inverseDt, ref inertiaA, ref inertiaB, ref inertiaC, ref prestep, out var projection); + ref var wsvA = ref jacobiResultsBundlesA[i]; + ref var wsvB = ref jacobiResultsBundlesB[i]; + ref var wsvC = ref jacobiResultsBundlesC[i]; + Bodies.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out wsvA, out wsvB, out wsvC); + function.WarmStart(ref wsvA, ref wsvB, ref wsvC, ref projection, ref accumulatedImpulses); + function.Solve(ref wsvA, ref wsvB, ref wsvC, ref projection, ref accumulatedImpulses); + } + } + } } diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 7a65e7720..e65a75390 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -243,6 +243,63 @@ public unsafe override void JacobiSolveIteration(ref TypeBatch typeBatch, ref Bu } } + public unsafe override void SolveStep(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + { + ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); + ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); + ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); + ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); + ref var bodyVelocities = ref bodies.ActiveSet.Velocities; + var function = default(TConstraintFunctions); + for (int i = startBundle; i < exclusiveEndBundle; ++i) + { + ref var prestep = ref Unsafe.Add(ref prestepBase, i); + ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); + ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); + ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); + var count = GetCountInBundle(ref typeBatch, i); + bodies.GatherInertia(ref references, count, out var inertiaA, out var inertiaB); + function.Prestep(bodies, ref references, count, dt, inverseDt, ref inertiaA, ref inertiaB, ref prestep, out var projection); + Bodies.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out var wsvA, out var wsvB); + function.WarmStart(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); + function.Solve(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); + Bodies.ScatterVelocities(ref wsvA, ref wsvB, ref bodyVelocities, ref bodyReferences, count); + } + } + + public unsafe override void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + { + ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); + ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); + ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); + ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); + ref var bodyVelocities = ref bodies.ActiveSet.Velocities; + var function = default(TConstraintFunctions); + ref var jacobiResultsBundlesA = ref jacobiResults.GetVelocitiesForBody(0); + ref var jacobiResultsBundlesB = ref jacobiResults.GetVelocitiesForBody(1); + for (int i = startBundle; i < exclusiveEndBundle; ++i) + { + ref var prestep = ref Unsafe.Add(ref prestepBase, i); + ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); + ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); + ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); + var count = GetCountInBundle(ref typeBatch, i); + bodies.GatherInertia(ref references, count, out var inertiaA, out var inertiaB); + //Jacobi batches split affected bodies into multiple pieces to guarantee convergence. + jacobiBatch.GetJacobiScaleForBodies(ref references, count, out var jacobiScaleA, out var jacobiScaleB); + Symmetric3x3Wide.Scale(inertiaA.InverseInertiaTensor, jacobiScaleA, out inertiaA.InverseInertiaTensor); + inertiaA.InverseMass *= jacobiScaleA; + Symmetric3x3Wide.Scale(inertiaB.InverseInertiaTensor, jacobiScaleB, out inertiaB.InverseInertiaTensor); + inertiaB.InverseMass *= jacobiScaleB; + function.Prestep(bodies, ref references, count, dt, inverseDt, ref inertiaA, ref inertiaB, ref prestep, out var projection); + ref var wsvA = ref jacobiResultsBundlesA[i]; + ref var wsvB = ref jacobiResultsBundlesB[i]; + Bodies.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out wsvA, out wsvB); + function.WarmStart(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); + function.Solve(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); + } + } + } public abstract class TwoBodyContactTypeProcessor diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index e80e20550..cd375011f 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -132,6 +132,9 @@ internal unsafe abstract void CopySleepingToActive( public abstract void JacobiWarmStart(ref TypeBatch typeBatch, ref Buffer bodyVelocities, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle); public abstract void JacobiSolveIteration(ref TypeBatch typeBatch, ref Buffer bodyVelocities, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle); + public abstract void SolveStep(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); + public abstract void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); + public virtual void IncrementallyUpdateContactData(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int end) { Debug.Fail("A contact data update was scheduled for a type batch that does not have a contact data update implementation."); diff --git a/BepuPhysics/EmbeddedSubsteppingTimestepper.cs b/BepuPhysics/EmbeddedSubsteppingTimestepper.cs new file mode 100644 index 000000000..c60f9e9e7 --- /dev/null +++ b/BepuPhysics/EmbeddedSubsteppingTimestepper.cs @@ -0,0 +1,109 @@ +using System; +using System.Collections.Generic; +using System.Diagnostics; +using System.Text; +using BepuUtilities; + +namespace BepuPhysics +{ + /// + /// Updates the simulation in the order of: sleeper -> predict body bounding boxes -> collision detection -> LOOP { contact data update (if on iteration > 0) -> integrate body velocities -> solver -> integrate body poses } -> data structure optimization. + /// Each inner loop execution simulates a sub-timestep of length dt/substepCount. + /// Useful for simulations with difficult to solve constraint systems that need shorter timestep durations but which don't require high frequency collision detection. + /// + public class EmbeddedSubsteppingTimestepper : ITimestepper + { + /// + /// Gets or sets the number of substeps to execute during each timestep. + /// + public int SubstepCount { get; set; } + + /// + /// Fires after the sleeper completes and before bodies are integrated. + /// + public event TimestepperStageHandler Slept; + /// + /// Fires after bodies have their bounding boxes updated for the frame's predicted motion and before collision detection. + /// + public event TimestepperStageHandler BeforeCollisionDetection; + /// + /// Fires after all collisions have been identified, but before the substep loop begins. + /// + public event TimestepperStageHandler CollisionsDetected; + /// + /// Fires at the beginning of a substep. + /// + public event TimestepperSubstepStageHandler SubstepStarted; + /// + /// Fires after contact constraints are incrementally updated at the beginning of substeps after the first and before velocities are integrated. + /// + public event TimestepperSubstepStageHandler ContactConstraintsUpdatedForSubstep; + /// + /// Fires after bodies have their velocities integrated and before the solver executes. + /// + public event TimestepperSubstepStageHandler VelocitiesIntegrated; + /// + /// Fires after the solver executes and before body poses are integrated. + /// + public event TimestepperSubstepStageHandler ConstraintsSolved; + /// + /// Fires after bodies have their poses integrated and before the substep ends. + /// + public event TimestepperSubstepStageHandler PosesIntegrated; + /// + /// Fires at the end of a substep. + /// + public event TimestepperSubstepStageHandler SubstepEnded; + /// + /// Fires after all substeps are finished executing and before data structures are incrementally optimized. + /// + public event TimestepperStageHandler SubstepsComplete; + + public EmbeddedSubsteppingTimestepper(int substepCount) + { + SubstepCount = substepCount; + } + + public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDispatcher = null) + { + simulation.Sleep(threadDispatcher); + Slept?.Invoke(dt, threadDispatcher); + + simulation.PredictBoundingBoxes(dt, threadDispatcher); + BeforeCollisionDetection?.Invoke(dt, threadDispatcher); + + simulation.CollisionDetection(dt, threadDispatcher); + CollisionsDetected?.Invoke(dt, threadDispatcher); + + Debug.Assert(SubstepCount >= 0, "Substep count should be positive."); + var substepDt = dt / SubstepCount; + + for (int substepIndex = 0; substepIndex < SubstepCount; ++substepIndex) + { + SubstepStarted?.Invoke(substepIndex, dt, threadDispatcher); + if (substepIndex > 0) + { + //This takes the place of collision detection for the substeps. It uses the current velocity to update penetration depths. + //It's definitely an approximation, but it's important for avoiding some obviously weird behavior. + //Note that we do not run this on the first iteration- the actual collision detection above takes care of it. + simulation.IncrementallyUpdateContactConstraints(substepDt, threadDispatcher); + ContactConstraintsUpdatedForSubstep?.Invoke(substepIndex, substepDt, threadDispatcher); + } + simulation.IntegrateVelocitiesAndUpdateInertias(substepDt, threadDispatcher); + VelocitiesIntegrated?.Invoke(substepIndex, substepDt, threadDispatcher); + + simulation.Profiler.Start(simulation.Solver); + simulation.Solver.SolveStep(substepDt, threadDispatcher); + simulation.Profiler.End(simulation.Solver); + ConstraintsSolved?.Invoke(substepIndex, substepDt, threadDispatcher); + + simulation.IntegratePoses(substepDt, threadDispatcher); + PosesIntegrated?.Invoke(substepIndex, substepDt, threadDispatcher); + SubstepEnded?.Invoke(substepIndex, substepDt, threadDispatcher); + } + SubstepsComplete?.Invoke(dt, threadDispatcher); + + simulation.IncrementallyOptimizeDataStructures(threadDispatcher); + } + } +} diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index 0d7d73763..c257b5aba 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -196,6 +196,7 @@ public Solver(Bodies bodies, BufferPool pool, int iterationCount, int fallbackBa batchReferencedHandles = new QuickList(fallbackBatchThreshold + 1, pool); ResizeHandleCapacity(initialCapacity); solveWorker = SolveWorker; + solveStepWorker = SolveStepWorker; incrementalContactUpdateWorker = IncrementalContactUpdateWorker; } diff --git a/BepuPhysics/Solver_Intermediate.cs b/BepuPhysics/Solver_Intermediate.cs deleted file mode 100644 index 5e303156d..000000000 --- a/BepuPhysics/Solver_Intermediate.cs +++ /dev/null @@ -1,108 +0,0 @@ -//using BepuUtilities; -//using BepuUtilities.Memory; -//using BepuPhysics.Constraints; -//using System; -//using System.Collections.Generic; -//using System.Diagnostics; -//using System.Text; -//using System.Threading; - -//namespace BepuPhysics -//{ -// public partial class Solver -// { - -// public Buffer StageIndices; //Used by the intermediate dispatcher. -// void IntermediateWork(int workerIndex) -// { -// int syncStage = 0; -// int blockIndex; -// var endIndex = context.WorkBlocks.Count; -// var inverseDt = 1f / context.Dt; -// ref var activeSet = ref ActiveSet; -// while ((blockIndex = Interlocked.Increment(ref StageIndices[syncStage])) <= endIndex) -// { -// ref var block = ref context.WorkBlocks[blockIndex - 1]; -// ref var typeBatch = ref activeSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; -// TypeProcessors[typeBatch.TypeId].Prestep(ref typeBatch, bodies, context.Dt, inverseDt, block.StartBundle, block.End); -// } - -// InterstageSync(ref syncStage); - -// for (int batchIndex = 0; batchIndex < activeSet.Batches.Count; ++batchIndex) -// { -// endIndex = context.BatchBoundaries[batchIndex]; -// while ((blockIndex = Interlocked.Increment(ref StageIndices[syncStage])) <= endIndex) -// { -// ref var block = ref context.WorkBlocks[blockIndex - 1]; -// ref var typeBatch = ref activeSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; -// TypeProcessors[typeBatch.TypeId].WarmStart(ref typeBatch, ref bodies.ActiveSet.Velocities, block.StartBundle, block.End); -// } -// InterstageSync(ref syncStage); -// } - -// for (int iterationIndex = 0; iterationIndex < iterationCount; ++iterationIndex) -// { -// for (int batchIndex = 0; batchIndex < activeSet.Batches.Count; ++batchIndex) -// { -// endIndex = context.BatchBoundaries[batchIndex]; -// while ((blockIndex = Interlocked.Increment(ref StageIndices[syncStage])) <= endIndex) -// { -// ref var block = ref context.WorkBlocks[blockIndex - 1]; -// ref var typeBatch = ref activeSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; -// TypeProcessors[typeBatch.TypeId].SolveIteration(ref typeBatch, ref bodies.ActiveSet.Velocities, block.StartBundle, block.End); -// } -// InterstageSync(ref syncStage); -// } -// } -// } - - - - - -// public double IntermediateMultithreadedUpdate(IThreadDispatcher threadPool, BufferPool bufferPool, float dt, float inverseDt) -// { -// var workerCount = context.WorkerCount = threadPool.ThreadCount; -// context.WorkerCompletedCount = 0; -// context.Dt = dt; -// //First build a set of work blocks. -// //The block size should be relatively small to give the workstealer something to do, but we don't want to go crazy with the number of blocks. -// //These values are found by empirical tuning. The optimal values may vary by architecture. -// //The goal here is to have just enough blocks that, in the event that we end up some underpowered threads (due to competition or hyperthreading), -// //there are enough blocks that workstealing will still generally allow the extra threads to be useful. -// const int targetBlocksPerBatchPerWorker = 16; -// const int minimumBlockSizeInBundles = 4; - -// var maximumBlocksPerBatch = workerCount * targetBlocksPerBatchPerWorker; -// BuildWorkBlocks(bufferPool, minimumBlockSizeInBundles, maximumBlocksPerBatch); -// ValidateWorkBlocks(); - -// ref var activeSet = ref ActiveSet; -// var stageCount = 1 + activeSet.Batches.Count * (iterationCount + 1); -// bufferPool.SpecializeFor().Take(stageCount, out StageIndices); - -// StageIndices[0] = 0; -// int stageIndex = 1; -// for (int i = 0; i < iterationCount + 1; ++i) -// { -// for (int batchIndex = 0; batchIndex < activeSet.Batches.Count; ++batchIndex) -// { -// StageIndices[stageIndex++] = batchIndex > 0 ? context.BatchBoundaries[batchIndex - 1] : 0; -// } -// } - -// var start = Stopwatch.GetTimestamp(); -// threadPool.DispatchWorkers(IntermediateWork); -// var end = Stopwatch.GetTimestamp(); - -// bufferPool.SpecializeFor().Return(ref StageIndices); -// context.WorkBlocks.Dispose(bufferPool.SpecializeFor()); -// context.BatchBoundaries.Dispose(bufferPool.SpecializeFor()); -// return (end - start) / (double)Stopwatch.Frequency; -// } - - - -// } -//} \ No newline at end of file diff --git a/BepuPhysics/Solver_Naive.cs b/BepuPhysics/Solver_Naive.cs deleted file mode 100644 index 96eeeead6..000000000 --- a/BepuPhysics/Solver_Naive.cs +++ /dev/null @@ -1,120 +0,0 @@ -//using BepuUtilities; -//using BepuUtilities.Memory; -//using BepuPhysics.Constraints; -//using System; -//using System.Collections.Generic; -//using System.Diagnostics; -//using System.Text; -//using System.Threading; - -//namespace BepuPhysics -//{ -// public partial class Solver -// { - -// int manualNaiveBlockIndex; -// int manualNaiveExclusiveEndIndex; -// void ManualNaivePrestep(int workerIndex) -// { -// int blockIndex; -// ref var activeSet = ref ActiveSet; -// var inverseDt = 1f / context.Dt; -// while ((blockIndex = Interlocked.Increment(ref manualNaiveBlockIndex)) <= manualNaiveExclusiveEndIndex) -// { -// blockIndex -= 1; -// ref var block = ref context.ConstraintBlocks.Blocks[blockIndex]; -// ref var typeBatch = ref activeSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; -// if (block.BatchIndex < FallbackBatchThreshold) -// TypeProcessors[typeBatch.TypeId].Prestep(ref typeBatch, bodies, context.Dt, inverseDt, block.StartBundle, block.End); -// else -// TypeProcessors[typeBatch.TypeId].JacobiPrestep(ref typeBatch, bodies, ref ActiveSet.Fallback, context.Dt, inverseDt, block.StartBundle, block.End); -// } -// } -// void ManualNaiveWarmStart(int workBlockIndex) -// { -// int blockIndex; -// ref var activeSet = ref ActiveSet; -// while ((blockIndex = Interlocked.Increment(ref manualNaiveBlockIndex)) <= manualNaiveExclusiveEndIndex) -// { -// ref var block = ref context.ConstraintBlocks.Blocks[blockIndex - 1]; -// ref var typeBatch = ref activeSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; -// if (block.BatchIndex < FallbackBatchThreshold) -// { -// TypeProcessors[typeBatch.TypeId].JacobiWarmStart(ref typeBatch, ref bodies.ActiveSet.Velocities, ref context.FallbackResults[block.TypeBatchIndex], block.StartBundle, block.End); -// } -// else -// { -// TypeProcessors[typeBatch.TypeId].WarmStart(ref typeBatch, ref bodies.ActiveSet.Velocities, block.StartBundle, block.End); -// } -// } -// } - -// void ManualNaiveSolveIteration(int workBlockIndex) -// { -// int blockIndex; -// ref var activeSet = ref ActiveSet; -// while ((blockIndex = Interlocked.Increment(ref manualNaiveBlockIndex)) <= manualNaiveExclusiveEndIndex) -// { -// ref var block = ref context.ConstraintBlocks.Blocks[blockIndex - 1]; -// ref var typeBatch = ref activeSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; -// if (block.BatchIndex < FallbackBatchThreshold) -// { -// TypeProcessors[typeBatch.TypeId].SolveIteration(ref typeBatch, ref bodies.ActiveSet.Velocities, block.StartBundle, block.End); -// } -// else -// { -// TypeProcessors[typeBatch.TypeId].JacobiSolveIteration(ref typeBatch, ref bodies.ActiveSet.Velocities, ref context.FallbackResults[block.TypeBatchIndex], block.StartBundle, block.End); -// } -// } -// } - - - - -// public void ManualNaiveMultithreadedUpdate(IThreadDispatcher threadPool, BufferPool bufferPool, float dt, float inverseDt) -// { -// var workerCount = context.WorkerCount = threadPool.ThreadCount; -// context.Dt = dt; -// //First build a set of work blocks. -// //The block size should be relatively small to give the workstealer something to do, but we don't want to go crazy with the number of blocks. -// //These values are found by empirical tuning. The optimal values may vary by architecture. -// const int targetBlocksPerBatchPerWorker = 4; -// const int minimumBlockSizeInBundles = 4; -// //Note that on a 3770K, the most expensive constraint bundles tend to cost less than 500ns to execute an iteration for. The minimum block size -// //is trying to balance having pointless numbers of blocks versus the worst case length of worker idling. For example, with a block size of 8, -// //and assuming 500ns per bundle, we risk up to 4 microseconds per iteration-batch worth of idle time. -// //This issue isn't unique to the somewhat odd workstealing scheme we use- it would still be a concern regardless. -// var maximumBlocksPerBatch = workerCount * targetBlocksPerBatchPerWorker; -// var filter = new MainSolveFilter(); -// BuildWorkBlocks(bufferPool, minimumBlockSizeInBundles, maximumBlocksPerBatch, ref filter); -// ValidateWorkBlocks(ref filter); - -// manualNaiveBlockIndex = 0; -// manualNaiveExclusiveEndIndex = context.ConstraintBlocks.Blocks.Count; -// threadPool.DispatchWorkers(ManualNaivePrestep); - -// ref var activeSet = ref ActiveSet; -// for (int batchIndex = 0; batchIndex < activeSet.Batches.Count; ++batchIndex) -// { -// manualNaiveBlockIndex = batchIndex > 0 ? context.BatchBoundaries[batchIndex - 1] : 0; -// manualNaiveExclusiveEndIndex = context.BatchBoundaries[batchIndex]; -// threadPool.DispatchWorkers(ManualNaiveWarmStart); -// } - -// for (int iterationIndex = 0; iterationIndex < iterationCount; ++iterationIndex) -// { -// for (int batchIndex = 0; batchIndex < activeSet.Batches.Count; ++batchIndex) -// { -// manualNaiveBlockIndex = batchIndex > 0 ? context.BatchBoundaries[batchIndex - 1] : 0; -// manualNaiveExclusiveEndIndex = context.BatchBoundaries[batchIndex]; -// threadPool.DispatchWorkers(ManualNaiveSolveIteration); -// } -// } - - -// context.ConstraintBlocks.Blocks.Dispose(bufferPool); -// bufferPool.Return(ref context.BatchBoundaries); -// } - -// } -//} \ No newline at end of file diff --git a/BepuPhysics/Solver_SubsteppingSolve.cs b/BepuPhysics/Solver_SubsteppingSolve.cs new file mode 100644 index 000000000..a2f0885c7 --- /dev/null +++ b/BepuPhysics/Solver_SubsteppingSolve.cs @@ -0,0 +1,143 @@ +using BepuUtilities; +using BepuUtilities.Collections; +using BepuUtilities.Memory; +using BepuPhysics.Constraints; +using System; +using System.Collections.Generic; +using System.Diagnostics; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; +using System.Text; +using System.Threading; + +namespace BepuPhysics +{ + public partial class Solver + { + + struct SolveStepStageFunction : IStageFunction + { + public float Dt; + public float InverseDt; + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void Execute(Solver solver, int blockIndex) + { + ref var block = ref solver.context.ConstraintBlocks.Blocks[blockIndex]; + ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; + var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; + typeProcessor.SolveStep(ref typeBatch, solver.bodies, Dt, InverseDt, block.StartBundle, block.End); + } + } + + struct FallbackSolveStepStageFunction : IStageFunction + { + public float Dt; + public float InverseDt; + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void Execute(Solver solver, int blockIndex) + { + ref var block = ref solver.context.ConstraintBlocks.Blocks[blockIndex]; + ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; + var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; + typeProcessor.JacobiSolveStep(ref typeBatch, solver.bodies, ref solver.ActiveSet.Fallback, ref solver.context.FallbackResults[block.TypeBatchIndex], Dt, InverseDt, block.StartBundle, block.End); + } + } + + Action solveStepWorker; + + + void SolveStepWorker(int workerIndex) + { + int prestepStart = GetUniformlyDistributedStart(workerIndex, context.ConstraintBlocks.Blocks.Count, context.WorkerCount, 0); + int fallbackStart = GetUniformlyDistributedStart(workerIndex, context.FallbackBlocks.Blocks.Count, context.WorkerCount, 0); + Buffer batchStarts; + ref var activeSet = ref ActiveSet; + unsafe + { + //stackalloc is actually a little bit slow since the localsinit behavior forces a zeroing. + //Fortunately, this executes once per thread per frame. With 32 batches, it would add... a few nanoseconds per frame. We can accept that overhead. + //This is preferred over preallocating on the heap- we might write to these values and we don't want to risk false sharing for no reason. + //A single instance of false sharing would cost far more than the overhead of zeroing out the array. + var batchStartsData = stackalloc int[activeSet.Batches.Count]; + batchStarts = new Buffer(batchStartsData, activeSet.Batches.Count); + } + for (int batchIndex = 0; batchIndex < activeSet.Batches.Count; ++batchIndex) + { + var batchOffset = batchIndex > 0 ? context.BatchBoundaries[batchIndex - 1] : 0; + var batchCount = context.BatchBoundaries[batchIndex] - batchOffset; + batchStarts[batchIndex] = GetUniformlyDistributedStart(workerIndex, batchCount, context.WorkerCount, batchOffset); + } + + int syncStage = 0; + //The claimed and unclaimed state swap after every usage of both pingpong claims buffers. + int claimedState = 1; + int unclaimedState = 0; + var bounds = context.WorkerBoundsA; + var boundsBackBuffer = context.WorkerBoundsB; + //Note that every batch has a different start position. Each covers a different subset of constraints, so they require different start locations. + //The same concept applies to the prestep- the prestep covers all constraints at once, rather than batch by batch. + Debug.Assert(activeSet.Batches.Count > 0, "Don't dispatch if there are no constraints."); + GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); + + var solveStage = new SolveStepStageFunction(); + for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) + { + var batchOffset = batchIndex > 0 ? context.BatchBoundaries[batchIndex - 1] : 0; + ExecuteStage(ref solveStage, ref context.ConstraintBlocks, ref bounds, ref boundsBackBuffer, workerIndex, batchOffset, context.BatchBoundaries[batchIndex], + ref batchStarts[batchIndex], ref syncStage, claimedState, unclaimedState); + } + if (fallbackExists) + { + var solveFallbackStage = new FallbackSolveStepStageFunction(); + var fallbackScatterStage = new FallbackScatterStageFunction(); + var batchOffset = FallbackBatchThreshold > 0 ? context.BatchBoundaries[FallbackBatchThreshold - 1] : 0; + ExecuteStage(ref solveFallbackStage, ref context.ConstraintBlocks, ref bounds, ref boundsBackBuffer, workerIndex, batchOffset, context.BatchBoundaries[FallbackBatchThreshold], + ref batchStarts[FallbackBatchThreshold], ref syncStage, claimedState, unclaimedState); + ExecuteStage(ref fallbackScatterStage, ref context.FallbackBlocks, ref bounds, ref boundsBackBuffer, + workerIndex, 0, context.FallbackBlocks.Blocks.Count, ref fallbackStart, ref syncStage, claimedState, unclaimedState); + } + claimedState ^= 1; + unclaimedState ^= 1; + } + + public void SolveStep(float dt, IThreadDispatcher threadDispatcher = null) + { + if (threadDispatcher == null) + { + var inverseDt = 1f / dt; + ref var activeSet = ref ActiveSet; + GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); + Buffer fallbackResults = default; + + for (int i = 0; i < synchronizedBatchCount; ++i) + { + ref var batch = ref activeSet.Batches[i]; + for (int j = 0; j < batch.TypeBatches.Count; ++j) + { + ref var typeBatch = ref batch.TypeBatches[j]; + TypeProcessors[typeBatch.TypeId].SolveStep(ref typeBatch, bodies, dt, inverseDt, 0, typeBatch.BundleCount); + } + } + if (fallbackExists) + { + ref var batch = ref activeSet.Batches[FallbackBatchThreshold]; + FallbackBatch.AllocateResults(this, pool, ref batch, out fallbackResults); + for (int j = 0; j < batch.TypeBatches.Count; ++j) + { + ref var typeBatch = ref batch.TypeBatches[j]; + TypeProcessors[typeBatch.TypeId].JacobiSolveStep(ref typeBatch, bodies, ref activeSet.Fallback, ref fallbackResults[j], dt, inverseDt, 0, typeBatch.BundleCount); + } + activeSet.Fallback.ScatterVelocities(bodies, this, ref fallbackResults, 0, activeSet.Fallback.BodyCount); + FallbackBatch.DisposeResults(this, pool, ref activeSet.Batches[FallbackBatchThreshold], ref fallbackResults); + } + } + else + { + ExecuteMultithreaded(dt, threadDispatcher, solveStepWorker); + } + } + + } +} From 3a4f6cee043e4c13d9dcb5f0117d459123d5f92d Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 11 Feb 2021 13:49:36 -0600 Subject: [PATCH 039/947] Another path. Correctness verified. --- .../Constraints/FourBodyTypeProcessor.cs | 10 ++ .../Constraints/OneBodyTypeProcessor.cs | 45 +++++ .../Constraints/ThreeBodyTypeProcessor.cs | 8 + .../Constraints/TwoBodyTypeProcessor.cs | 45 ++++- BepuPhysics/Constraints/TypeProcessor.cs | 4 + .../EmbeddedSubsteppingTimestepper2.cs | 109 ++++++++++++ BepuPhysics/Solver_SubsteppingSolve.cs | 3 +- BepuPhysics/Solver_SubsteppingSolve2.cs | 159 ++++++++++++++++++ 8 files changed, 380 insertions(+), 3 deletions(-) create mode 100644 BepuPhysics/EmbeddedSubsteppingTimestepper2.cs create mode 100644 BepuPhysics/Solver_SubsteppingSolve2.cs diff --git a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs index e04dad99e..c7daa1ab4 100644 --- a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs @@ -285,5 +285,15 @@ public unsafe override void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodi function.Solve(ref wsvA, ref wsvB, ref wsvC, ref wsvD, ref projection, ref accumulatedImpulses); } } + + + public override void WarmStart2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + { + throw new NotImplementedException(); + } + public override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + { + throw new NotImplementedException(); + } } } diff --git a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs index 3c6b575da..fab776943 100644 --- a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs @@ -252,6 +252,51 @@ public unsafe override void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodi } } + + + public unsafe override void WarmStart2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + { + ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); + ref var bodyReferencesBase = ref Unsafe.AsRef>(typeBatch.BodyReferences.Memory); + ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); + ref var bodyVelocities = ref bodies.ActiveSet.Velocities; + var function = default(TConstraintFunctions); + for (int i = startBundle; i < exclusiveEndBundle; ++i) + { + ref var prestep = ref Unsafe.Add(ref prestepBase, i); + ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); + ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); + ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); + var count = GetCountInBundle(ref typeBatch, i); + bodies.GatherInertia(ref references, count, out var inertiaA); + Bodies.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out var wsvA); + function.Prestep(bodies, ref references, count, dt, inverseDt, ref inertiaA, ref prestep, out var projection); + function.WarmStart(ref wsvA, ref projection, ref accumulatedImpulses); + Bodies.ScatterVelocities(ref wsvA, ref bodyVelocities, ref bodyReferences, count); + } + } + public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + { + ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); + ref var bodyReferencesBase = ref Unsafe.AsRef>(typeBatch.BodyReferences.Memory); + ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); + ref var bodyVelocities = ref bodies.ActiveSet.Velocities; + var function = default(TConstraintFunctions); + for (int i = startBundle; i < exclusiveEndBundle; ++i) + { + ref var prestep = ref Unsafe.Add(ref prestepBase, i); + ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); + ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); + ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); + var count = GetCountInBundle(ref typeBatch, i); + bodies.GatherInertia(ref references, count, out var inertiaA); + Bodies.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out var wsvA); + function.Prestep(bodies, ref references, count, dt, inverseDt, ref inertiaA, ref prestep, out var projection); + function.Solve(ref wsvA, ref projection, ref accumulatedImpulses); + Bodies.ScatterVelocities(ref wsvA, ref bodyVelocities, ref bodyReferences, count); + } + } + } public abstract class OneBodyContactTypeProcessor diff --git a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs index 9899c8e68..112348fab 100644 --- a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs @@ -275,5 +275,13 @@ public unsafe override void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodi } } + public override void WarmStart2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + { + throw new NotImplementedException(); + } + public override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + { + throw new NotImplementedException(); + } } } diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index e65a75390..f8215174a 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -155,7 +155,7 @@ public unsafe override void WarmStart(ref TypeBatch typeBatch, ref Buffer(typeBatch.PrestepData.Memory); + ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); + ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); + ref var bodyVelocities = ref bodies.ActiveSet.Velocities; + var function = default(TConstraintFunctions); + for (int i = startBundle; i < exclusiveEndBundle; ++i) + { + ref var prestep = ref Unsafe.Add(ref prestepBase, i); + ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); + ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); + ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); + var count = GetCountInBundle(ref typeBatch, i); + bodies.GatherInertia(ref references, count, out var inertiaA, out var inertiaB); + Bodies.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out var wsvA, out var wsvB); + function.Prestep(bodies, ref references, count, dt, inverseDt, ref inertiaA, ref inertiaB, ref prestep, out var projection); + function.WarmStart(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); + Bodies.ScatterVelocities(ref wsvA, ref wsvB, ref bodyVelocities, ref bodyReferences, count); + } + } + public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + { + ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); + ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); + ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); + ref var bodyVelocities = ref bodies.ActiveSet.Velocities; + var function = default(TConstraintFunctions); + for (int i = startBundle; i < exclusiveEndBundle; ++i) + { + ref var prestep = ref Unsafe.Add(ref prestepBase, i); + ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); + ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); + ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); + var count = GetCountInBundle(ref typeBatch, i); + bodies.GatherInertia(ref references, count, out var inertiaA, out var inertiaB); + Bodies.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out var wsvA, out var wsvB); + function.Prestep(bodies, ref references, count, dt, inverseDt, ref inertiaA, ref inertiaB, ref prestep, out var projection); + function.Solve(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); + Bodies.ScatterVelocities(ref wsvA, ref wsvB, ref bodyVelocities, ref bodyReferences, count); + } + } } public abstract class TwoBodyContactTypeProcessor diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index cd375011f..5a991ee75 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -135,6 +135,10 @@ internal unsafe abstract void CopySleepingToActive( public abstract void SolveStep(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); public abstract void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); + + public abstract void WarmStart2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); + public abstract void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); + public virtual void IncrementallyUpdateContactData(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int end) { Debug.Fail("A contact data update was scheduled for a type batch that does not have a contact data update implementation."); diff --git a/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs b/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs new file mode 100644 index 000000000..2b4879ccd --- /dev/null +++ b/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs @@ -0,0 +1,109 @@ +using System; +using System.Collections.Generic; +using System.Diagnostics; +using System.Text; +using BepuUtilities; + +namespace BepuPhysics +{ + /// + /// Updates the simulation in the order of: sleeper -> predict body bounding boxes -> collision detection -> LOOP { contact data update (if on iteration > 0) -> integrate body velocities -> solver -> integrate body poses } -> data structure optimization. + /// Each inner loop execution simulates a sub-timestep of length dt/substepCount. + /// Useful for simulations with difficult to solve constraint systems that need shorter timestep durations but which don't require high frequency collision detection. + /// + public class EmbeddedSubsteppingTimestepper2 : ITimestepper + { + /// + /// Gets or sets the number of substeps to execute during each timestep. + /// + public int SubstepCount { get; set; } + + /// + /// Fires after the sleeper completes and before bodies are integrated. + /// + public event TimestepperStageHandler Slept; + /// + /// Fires after bodies have their bounding boxes updated for the frame's predicted motion and before collision detection. + /// + public event TimestepperStageHandler BeforeCollisionDetection; + /// + /// Fires after all collisions have been identified, but before the substep loop begins. + /// + public event TimestepperStageHandler CollisionsDetected; + /// + /// Fires at the beginning of a substep. + /// + public event TimestepperSubstepStageHandler SubstepStarted; + /// + /// Fires after contact constraints are incrementally updated at the beginning of substeps after the first and before velocities are integrated. + /// + public event TimestepperSubstepStageHandler ContactConstraintsUpdatedForSubstep; + /// + /// Fires after bodies have their velocities integrated and before the solver executes. + /// + public event TimestepperSubstepStageHandler VelocitiesIntegrated; + /// + /// Fires after the solver executes and before body poses are integrated. + /// + public event TimestepperSubstepStageHandler ConstraintsSolved; + /// + /// Fires after bodies have their poses integrated and before the substep ends. + /// + public event TimestepperSubstepStageHandler PosesIntegrated; + /// + /// Fires at the end of a substep. + /// + public event TimestepperSubstepStageHandler SubstepEnded; + /// + /// Fires after all substeps are finished executing and before data structures are incrementally optimized. + /// + public event TimestepperStageHandler SubstepsComplete; + + public EmbeddedSubsteppingTimestepper2(int substepCount) + { + SubstepCount = substepCount; + } + + public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDispatcher = null) + { + simulation.Sleep(threadDispatcher); + Slept?.Invoke(dt, threadDispatcher); + + simulation.PredictBoundingBoxes(dt, threadDispatcher); + BeforeCollisionDetection?.Invoke(dt, threadDispatcher); + + simulation.CollisionDetection(dt, threadDispatcher); + CollisionsDetected?.Invoke(dt, threadDispatcher); + + Debug.Assert(SubstepCount >= 0, "Substep count should be positive."); + var substepDt = dt / SubstepCount; + + for (int substepIndex = 0; substepIndex < SubstepCount; ++substepIndex) + { + SubstepStarted?.Invoke(substepIndex, dt, threadDispatcher); + if (substepIndex > 0) + { + //This takes the place of collision detection for the substeps. It uses the current velocity to update penetration depths. + //It's definitely an approximation, but it's important for avoiding some obviously weird behavior. + //Note that we do not run this on the first iteration- the actual collision detection above takes care of it. + simulation.IncrementallyUpdateContactConstraints(substepDt, threadDispatcher); + ContactConstraintsUpdatedForSubstep?.Invoke(substepIndex, substepDt, threadDispatcher); + } + simulation.IntegrateVelocitiesAndUpdateInertias(substepDt, threadDispatcher); + VelocitiesIntegrated?.Invoke(substepIndex, substepDt, threadDispatcher); + + simulation.Profiler.Start(simulation.Solver); + simulation.Solver.SolveStep2(substepDt, threadDispatcher); + simulation.Profiler.End(simulation.Solver); + ConstraintsSolved?.Invoke(substepIndex, substepDt, threadDispatcher); + + simulation.IntegratePoses(substepDt, threadDispatcher); + PosesIntegrated?.Invoke(substepIndex, substepDt, threadDispatcher); + SubstepEnded?.Invoke(substepIndex, substepDt, threadDispatcher); + } + SubstepsComplete?.Invoke(dt, threadDispatcher); + + simulation.IncrementallyOptimizeDataStructures(threadDispatcher); + } + } +} diff --git a/BepuPhysics/Solver_SubsteppingSolve.cs b/BepuPhysics/Solver_SubsteppingSolve.cs index a2f0885c7..5e375c0ae 100644 --- a/BepuPhysics/Solver_SubsteppingSolve.cs +++ b/BepuPhysics/Solver_SubsteppingSolve.cs @@ -109,7 +109,6 @@ public void SolveStep(float dt, IThreadDispatcher threadDispatcher = null) var inverseDt = 1f / dt; ref var activeSet = ref ActiveSet; GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); - Buffer fallbackResults = default; for (int i = 0; i < synchronizedBatchCount; ++i) { @@ -123,7 +122,7 @@ public void SolveStep(float dt, IThreadDispatcher threadDispatcher = null) if (fallbackExists) { ref var batch = ref activeSet.Batches[FallbackBatchThreshold]; - FallbackBatch.AllocateResults(this, pool, ref batch, out fallbackResults); + FallbackBatch.AllocateResults(this, pool, ref batch, out var fallbackResults); for (int j = 0; j < batch.TypeBatches.Count; ++j) { ref var typeBatch = ref batch.TypeBatches[j]; diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs new file mode 100644 index 000000000..d79b296be --- /dev/null +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -0,0 +1,159 @@ +using BepuUtilities; +using BepuUtilities.Collections; +using BepuUtilities.Memory; +using BepuPhysics.Constraints; +using System; +using System.Collections.Generic; +using System.Diagnostics; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; +using System.Text; +using System.Threading; + +namespace BepuPhysics +{ + public partial class Solver + { + //Split the solve process into a warmstart and solve, where warmstart doesn't try to store out anything. It just computes jacobians and modifies velocities according to the accumulated impulse. + //The solve step then *recomputes* jacobians from prestep data and pose information. + //Why? Memory bandwidth. Redoing the calculation is cheaper than storing it out. + //struct WarmStartStep2StageFunction : IStageFunction + //{ + // public float Dt; + // public float InverseDt; + + // [MethodImpl(MethodImplOptions.AggressiveInlining)] + // public void Execute(Solver solver, int blockIndex) + // { + // ref var block = ref solver.context.ConstraintBlocks.Blocks[blockIndex]; + // ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; + // var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; + // typeProcessor.WarmStart2(ref typeBatch, solver.bodies, Dt, InverseDt, block.StartBundle, block.End); + // } + //} + ////no fallback warmstart; the last constraint batch is always handled by the solve instead, and if the fallback batch exists, it's guaranteed to be the last batch. + + //struct SolveStep2StageFunction : IStageFunction + //{ + // public float Dt; + // public float InverseDt; + + // [MethodImpl(MethodImplOptions.AggressiveInlining)] + // public void Execute(Solver solver, int blockIndex) + // { + // ref var block = ref solver.context.ConstraintBlocks.Blocks[blockIndex]; + // ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; + // var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; + // typeProcessor.SolveStep2(ref typeBatch, solver.bodies, Dt, InverseDt, block.StartBundle, block.End); + // } + //} + + //struct FallbackSolveStep2StageFunction : IStageFunction + //{ + // public float Dt; + // public float InverseDt; + + // [MethodImpl(MethodImplOptions.AggressiveInlining)] + // public void Execute(Solver solver, int blockIndex) + // { + // ref var block = ref solver.context.ConstraintBlocks.Blocks[blockIndex]; + // ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; + // var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; + // typeProcessor.JacobiSolveStep2(ref typeBatch, solver.bodies, ref solver.ActiveSet.Fallback, ref solver.context.FallbackResults[block.TypeBatchIndex], Dt, InverseDt, block.StartBundle, block.End); + // } + //} + + //Action solveStep2Worker; + + + //void SolveStepWorker2(int workerIndex) + //{ + // int prestepStart = GetUniformlyDistributedStart(workerIndex, context.ConstraintBlocks.Blocks.Count, context.WorkerCount, 0); + // int fallbackStart = GetUniformlyDistributedStart(workerIndex, context.FallbackBlocks.Blocks.Count, context.WorkerCount, 0); + // Buffer batchStarts; + // ref var activeSet = ref ActiveSet; + // unsafe + // { + // //stackalloc is actually a little bit slow since the localsinit behavior forces a zeroing. + // //Fortunately, this executes once per thread per frame. With 32 batches, it would add... a few nanoseconds per frame. We can accept that overhead. + // //This is preferred over preallocating on the heap- we might write to these values and we don't want to risk false sharing for no reason. + // //A single instance of false sharing would cost far more than the overhead of zeroing out the array. + // var batchStartsData = stackalloc int[activeSet.Batches.Count]; + // batchStarts = new Buffer(batchStartsData, activeSet.Batches.Count); + // } + // for (int batchIndex = 0; batchIndex < activeSet.Batches.Count; ++batchIndex) + // { + // var batchOffset = batchIndex > 0 ? context.BatchBoundaries[batchIndex - 1] : 0; + // var batchCount = context.BatchBoundaries[batchIndex] - batchOffset; + // batchStarts[batchIndex] = GetUniformlyDistributedStart(workerIndex, batchCount, context.WorkerCount, batchOffset); + // } + + // int syncStage = 0; + // //The claimed and unclaimed state swap after every usage of both pingpong claims buffers. + // int claimedState = 1; + // int unclaimedState = 0; + // var bounds = context.WorkerBoundsA; + // var boundsBackBuffer = context.WorkerBoundsB; + // //Note that every batch has a different start position. Each covers a different subset of constraints, so they require different start locations. + // //The same concept applies to the prestep- the prestep covers all constraints at once, rather than batch by batch. + // Debug.Assert(activeSet.Batches.Count > 0, "Don't dispatch if there are no constraints."); + // GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); + + // var solveStage = new SolveStepStageFunction(); + // for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) + // { + // var batchOffset = batchIndex > 0 ? context.BatchBoundaries[batchIndex - 1] : 0; + // ExecuteStage(ref solveStage, ref context.ConstraintBlocks, ref bounds, ref boundsBackBuffer, workerIndex, batchOffset, context.BatchBoundaries[batchIndex], + // ref batchStarts[batchIndex], ref syncStage, claimedState, unclaimedState); + // } + // if (fallbackExists) + // { + // var solveFallbackStage = new FallbackSolveStepStageFunction(); + // var fallbackScatterStage = new FallbackScatterStageFunction(); + // var batchOffset = FallbackBatchThreshold > 0 ? context.BatchBoundaries[FallbackBatchThreshold - 1] : 0; + // ExecuteStage(ref solveFallbackStage, ref context.ConstraintBlocks, ref bounds, ref boundsBackBuffer, workerIndex, batchOffset, context.BatchBoundaries[FallbackBatchThreshold], + // ref batchStarts[FallbackBatchThreshold], ref syncStage, claimedState, unclaimedState); + // ExecuteStage(ref fallbackScatterStage, ref context.FallbackBlocks, ref bounds, ref boundsBackBuffer, + // workerIndex, 0, context.FallbackBlocks.Blocks.Count, ref fallbackStart, ref syncStage, claimedState, unclaimedState); + // } + // claimedState ^= 1; + // unclaimedState ^= 1; + //} + + public void SolveStep2(float dt, IThreadDispatcher threadDispatcher = null) + { + if (threadDispatcher == null) + { + var inverseDt = 1f / dt; + ref var activeSet = ref ActiveSet; + GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); + Debug.Assert(!fallbackExists, "Not handling this yet."); + + for (int i = 0; i < synchronizedBatchCount; ++i) + { + ref var batch = ref activeSet.Batches[i]; + for (int j = 0; j < batch.TypeBatches.Count; ++j) + { + ref var typeBatch = ref batch.TypeBatches[j]; + TypeProcessors[typeBatch.TypeId].WarmStart2(ref typeBatch, bodies, dt, inverseDt, 0, typeBatch.BundleCount); + } + } + + for (int i = 0; i < synchronizedBatchCount; ++i) + { + ref var batch = ref activeSet.Batches[i]; + for (int j = 0; j < batch.TypeBatches.Count; ++j) + { + ref var typeBatch = ref batch.TypeBatches[j]; + TypeProcessors[typeBatch.TypeId].SolveStep2(ref typeBatch, bodies, dt, inverseDt, 0, typeBatch.BundleCount); + } + } + } + else + { + //ExecuteMultithreaded(dt, threadDispatcher, solveStepWorker); + } + } + + } +} From 2b045b572c8feab9d2e7c95603de28fc62de6227 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 25 Jun 2021 13:21:39 -0500 Subject: [PATCH 040/947] Multithreaded substeppingsolve2 scaling test. --- BepuPhysics/Solver.cs | 1 + BepuPhysics/Solver_SubsteppingSolve.cs | 4 + BepuPhysics/Solver_SubsteppingSolve2.cs | 173 +++++++++++++----------- Demos/Demos/NewtDemo.cs | 21 +-- 4 files changed, 109 insertions(+), 90 deletions(-) diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index c257b5aba..f3338f01e 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -197,6 +197,7 @@ public Solver(Bodies bodies, BufferPool pool, int iterationCount, int fallbackBa ResizeHandleCapacity(initialCapacity); solveWorker = SolveWorker; solveStepWorker = SolveStepWorker; + solveStep2Worker = SolveStep2Worker; incrementalContactUpdateWorker = IncrementalContactUpdateWorker; } diff --git a/BepuPhysics/Solver_SubsteppingSolve.cs b/BepuPhysics/Solver_SubsteppingSolve.cs index 5e375c0ae..263df5cfa 100644 --- a/BepuPhysics/Solver_SubsteppingSolve.cs +++ b/BepuPhysics/Solver_SubsteppingSolve.cs @@ -82,6 +82,8 @@ void SolveStepWorker(int workerIndex) GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); var solveStage = new SolveStepStageFunction(); + solveStage.Dt = context.Dt; + solveStage.InverseDt = 1f / context.Dt; for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) { var batchOffset = batchIndex > 0 ? context.BatchBoundaries[batchIndex - 1] : 0; @@ -91,6 +93,8 @@ void SolveStepWorker(int workerIndex) if (fallbackExists) { var solveFallbackStage = new FallbackSolveStepStageFunction(); + solveFallbackStage.Dt = context.Dt; + solveFallbackStage.InverseDt = 1f / context.Dt; var fallbackScatterStage = new FallbackScatterStageFunction(); var batchOffset = FallbackBatchThreshold > 0 ? context.BatchBoundaries[FallbackBatchThreshold - 1] : 0; ExecuteStage(ref solveFallbackStage, ref context.ConstraintBlocks, ref bounds, ref boundsBackBuffer, workerIndex, batchOffset, context.BatchBoundaries[FallbackBatchThreshold], diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index d79b296be..a12a891b7 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -17,36 +17,36 @@ public partial class Solver //Split the solve process into a warmstart and solve, where warmstart doesn't try to store out anything. It just computes jacobians and modifies velocities according to the accumulated impulse. //The solve step then *recomputes* jacobians from prestep data and pose information. //Why? Memory bandwidth. Redoing the calculation is cheaper than storing it out. - //struct WarmStartStep2StageFunction : IStageFunction - //{ - // public float Dt; - // public float InverseDt; + struct WarmStartStep2StageFunction : IStageFunction + { + public float Dt; + public float InverseDt; - // [MethodImpl(MethodImplOptions.AggressiveInlining)] - // public void Execute(Solver solver, int blockIndex) - // { - // ref var block = ref solver.context.ConstraintBlocks.Blocks[blockIndex]; - // ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; - // var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; - // typeProcessor.WarmStart2(ref typeBatch, solver.bodies, Dt, InverseDt, block.StartBundle, block.End); - // } - //} - ////no fallback warmstart; the last constraint batch is always handled by the solve instead, and if the fallback batch exists, it's guaranteed to be the last batch. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void Execute(Solver solver, int blockIndex) + { + ref var block = ref solver.context.ConstraintBlocks.Blocks[blockIndex]; + ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; + var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; + typeProcessor.WarmStart2(ref typeBatch, solver.bodies, Dt, InverseDt, block.StartBundle, block.End); + } + } + //no fallback warmstart; the last constraint batch is always handled by the solve instead, and if the fallback batch exists, it's guaranteed to be the last batch. - //struct SolveStep2StageFunction : IStageFunction - //{ - // public float Dt; - // public float InverseDt; + struct SolveStep2StageFunction : IStageFunction + { + public float Dt; + public float InverseDt; - // [MethodImpl(MethodImplOptions.AggressiveInlining)] - // public void Execute(Solver solver, int blockIndex) - // { - // ref var block = ref solver.context.ConstraintBlocks.Blocks[blockIndex]; - // ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; - // var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; - // typeProcessor.SolveStep2(ref typeBatch, solver.bodies, Dt, InverseDt, block.StartBundle, block.End); - // } - //} + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void Execute(Solver solver, int blockIndex) + { + ref var block = ref solver.context.ConstraintBlocks.Blocks[blockIndex]; + ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; + var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; + typeProcessor.SolveStep2(ref typeBatch, solver.bodies, Dt, InverseDt, block.StartBundle, block.End); + } + } //struct FallbackSolveStep2StageFunction : IStageFunction //{ @@ -63,62 +63,75 @@ public partial class Solver // } //} - //Action solveStep2Worker; + Action solveStep2Worker; - //void SolveStepWorker2(int workerIndex) - //{ - // int prestepStart = GetUniformlyDistributedStart(workerIndex, context.ConstraintBlocks.Blocks.Count, context.WorkerCount, 0); - // int fallbackStart = GetUniformlyDistributedStart(workerIndex, context.FallbackBlocks.Blocks.Count, context.WorkerCount, 0); - // Buffer batchStarts; - // ref var activeSet = ref ActiveSet; - // unsafe - // { - // //stackalloc is actually a little bit slow since the localsinit behavior forces a zeroing. - // //Fortunately, this executes once per thread per frame. With 32 batches, it would add... a few nanoseconds per frame. We can accept that overhead. - // //This is preferred over preallocating on the heap- we might write to these values and we don't want to risk false sharing for no reason. - // //A single instance of false sharing would cost far more than the overhead of zeroing out the array. - // var batchStartsData = stackalloc int[activeSet.Batches.Count]; - // batchStarts = new Buffer(batchStartsData, activeSet.Batches.Count); - // } - // for (int batchIndex = 0; batchIndex < activeSet.Batches.Count; ++batchIndex) - // { - // var batchOffset = batchIndex > 0 ? context.BatchBoundaries[batchIndex - 1] : 0; - // var batchCount = context.BatchBoundaries[batchIndex] - batchOffset; - // batchStarts[batchIndex] = GetUniformlyDistributedStart(workerIndex, batchCount, context.WorkerCount, batchOffset); - // } + void SolveStep2Worker(int workerIndex) + { + int prestepStart = GetUniformlyDistributedStart(workerIndex, context.ConstraintBlocks.Blocks.Count, context.WorkerCount, 0); + int fallbackStart = GetUniformlyDistributedStart(workerIndex, context.FallbackBlocks.Blocks.Count, context.WorkerCount, 0); + Buffer batchStarts; + ref var activeSet = ref ActiveSet; + unsafe + { + //stackalloc is actually a little bit slow since the localsinit behavior forces a zeroing. + //Fortunately, this executes once per thread per frame. With 32 batches, it would add... a few nanoseconds per frame. We can accept that overhead. + //This is preferred over preallocating on the heap- we might write to these values and we don't want to risk false sharing for no reason. + //A single instance of false sharing would cost far more than the overhead of zeroing out the array. + var batchStartsData = stackalloc int[activeSet.Batches.Count]; + batchStarts = new Buffer(batchStartsData, activeSet.Batches.Count); + } + for (int batchIndex = 0; batchIndex < activeSet.Batches.Count; ++batchIndex) + { + var batchOffset = batchIndex > 0 ? context.BatchBoundaries[batchIndex - 1] : 0; + var batchCount = context.BatchBoundaries[batchIndex] - batchOffset; + batchStarts[batchIndex] = GetUniformlyDistributedStart(workerIndex, batchCount, context.WorkerCount, batchOffset); + } - // int syncStage = 0; - // //The claimed and unclaimed state swap after every usage of both pingpong claims buffers. - // int claimedState = 1; - // int unclaimedState = 0; - // var bounds = context.WorkerBoundsA; - // var boundsBackBuffer = context.WorkerBoundsB; - // //Note that every batch has a different start position. Each covers a different subset of constraints, so they require different start locations. - // //The same concept applies to the prestep- the prestep covers all constraints at once, rather than batch by batch. - // Debug.Assert(activeSet.Batches.Count > 0, "Don't dispatch if there are no constraints."); - // GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); + int syncStage = 0; + //The claimed and unclaimed state swap after every usage of both pingpong claims buffers. + int claimedState = 1; + int unclaimedState = 0; + var bounds = context.WorkerBoundsA; + var boundsBackBuffer = context.WorkerBoundsB; + //Note that every batch has a different start position. Each covers a different subset of constraints, so they require different start locations. + //The same concept applies to the prestep- the prestep covers all constraints at once, rather than batch by batch. + Debug.Assert(activeSet.Batches.Count > 0, "Don't dispatch if there are no constraints."); + GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); - // var solveStage = new SolveStepStageFunction(); - // for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) - // { - // var batchOffset = batchIndex > 0 ? context.BatchBoundaries[batchIndex - 1] : 0; - // ExecuteStage(ref solveStage, ref context.ConstraintBlocks, ref bounds, ref boundsBackBuffer, workerIndex, batchOffset, context.BatchBoundaries[batchIndex], - // ref batchStarts[batchIndex], ref syncStage, claimedState, unclaimedState); - // } - // if (fallbackExists) - // { - // var solveFallbackStage = new FallbackSolveStepStageFunction(); - // var fallbackScatterStage = new FallbackScatterStageFunction(); - // var batchOffset = FallbackBatchThreshold > 0 ? context.BatchBoundaries[FallbackBatchThreshold - 1] : 0; - // ExecuteStage(ref solveFallbackStage, ref context.ConstraintBlocks, ref bounds, ref boundsBackBuffer, workerIndex, batchOffset, context.BatchBoundaries[FallbackBatchThreshold], - // ref batchStarts[FallbackBatchThreshold], ref syncStage, claimedState, unclaimedState); - // ExecuteStage(ref fallbackScatterStage, ref context.FallbackBlocks, ref bounds, ref boundsBackBuffer, - // workerIndex, 0, context.FallbackBlocks.Blocks.Count, ref fallbackStart, ref syncStage, claimedState, unclaimedState); - // } - // claimedState ^= 1; - // unclaimedState ^= 1; - //} + var warmstartStage = new WarmStartStep2StageFunction(); + warmstartStage.Dt = context.Dt; + warmstartStage.InverseDt = 1f / context.Dt; + for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) + { + var batchOffset = batchIndex > 0 ? context.BatchBoundaries[batchIndex - 1] : 0; + ExecuteStage(ref warmstartStage, ref context.ConstraintBlocks, ref bounds, ref boundsBackBuffer, workerIndex, batchOffset, context.BatchBoundaries[batchIndex], + ref batchStarts[batchIndex], ref syncStage, claimedState, unclaimedState); + } + claimedState ^= 1; + unclaimedState ^= 1; + var solveStage = new SolveStep2StageFunction(); + solveStage.Dt = context.Dt; + solveStage.InverseDt = 1f / context.Dt; + for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) + { + var batchOffset = batchIndex > 0 ? context.BatchBoundaries[batchIndex - 1] : 0; + ExecuteStage(ref solveStage, ref context.ConstraintBlocks, ref bounds, ref boundsBackBuffer, workerIndex, batchOffset, context.BatchBoundaries[batchIndex], + ref batchStarts[batchIndex], ref syncStage, claimedState, unclaimedState); + } + //if (fallbackExists) + //{ + // var solveFallbackStage = new FallbackSolveStepStageFunction(); + // var fallbackScatterStage = new FallbackScatterStageFunction(); + // var batchOffset = FallbackBatchThreshold > 0 ? context.BatchBoundaries[FallbackBatchThreshold - 1] : 0; + // ExecuteStage(ref solveFallbackStage, ref context.ConstraintBlocks, ref bounds, ref boundsBackBuffer, workerIndex, batchOffset, context.BatchBoundaries[FallbackBatchThreshold], + // ref batchStarts[FallbackBatchThreshold], ref syncStage, claimedState, unclaimedState); + // ExecuteStage(ref fallbackScatterStage, ref context.FallbackBlocks, ref bounds, ref boundsBackBuffer, + // workerIndex, 0, context.FallbackBlocks.Blocks.Count, ref fallbackStart, ref syncStage, claimedState, unclaimedState); + //} + claimedState ^= 1; + unclaimedState ^= 1; + } public void SolveStep2(float dt, IThreadDispatcher threadDispatcher = null) { @@ -151,7 +164,7 @@ public void SolveStep2(float dt, IThreadDispatcher threadDispatcher = null) } else { - //ExecuteMultithreaded(dt, threadDispatcher, solveStepWorker); + ExecuteMultithreaded(dt, threadDispatcher, solveStep2Worker); } } diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index b5256b80a..153a9f39e 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -686,12 +686,12 @@ internal unsafe static void CreateDeformable(Simulation simulation, in Vector3 p //Simulation.Solver.Add(vertexHandles[edge.A], vertexHandles[edge.B], // new CenterDistanceConstraint(offset.Length(), weldSpringiness)); } - for (int i = 0; i < tetrahedraVertexIndices.Length; ++i) - { - ref var tetrahedron = ref tetrahedraVertexIndices[i]; - simulation.Solver.Add(vertexHandles[tetrahedron.A], vertexHandles[tetrahedron.B], vertexHandles[tetrahedron.C], vertexHandles[tetrahedron.D], - new VolumeConstraint(vertices[tetrahedron.A], vertices[tetrahedron.B], vertices[tetrahedron.C], vertices[tetrahedron.D], volumeSpringiness)); - } + //for (int i = 0; i < tetrahedraVertexIndices.Length; ++i) + //{ + // ref var tetrahedron = ref tetrahedraVertexIndices[i]; + // simulation.Solver.Add(vertexHandles[tetrahedron.A], vertexHandles[tetrahedron.B], vertexHandles[tetrahedron.C], vertexHandles[tetrahedron.D], + // new VolumeConstraint(vertices[tetrahedron.A], vertices[tetrahedron.B], vertices[tetrahedron.C], vertices[tetrahedron.D], volumeSpringiness)); + //} pool.Return(ref vertexEdgeCounts); edges.Dispose(pool); @@ -710,15 +710,16 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //The PositionFirstTimestepper is the simplest timestepping mode, but since it integrates velocity into position at the start of the frame, directly modified velocities outside of the timestep //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. - Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(8)); + //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper(), solverIterationCount: 20); var meshContent = content.Load("Content\\newt.obj"); float cellSize = 0.1f; DumbTetrahedralizer.Tetrahedralize(meshContent.Triangles, cellSize, BufferPool, out var vertices, out var vertexSpatialIndices, out var cellVertexIndices, out var tetrahedraVertexIndices); - var weldSpringiness = new SpringSettings(30f, 0); + var weldSpringiness = new SpringSettings(60f, 1f); var volumeSpringiness = new SpringSettings(30f, 1); - for (int i = 0; i < 5; ++i) + for (int i = 0; i < 8; ++i) { CreateDeformable(Simulation, new Vector3(i * 3, 5 + i * 1.5f, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), MathF.PI * (i * 0.55f)), 1f, cellSize, weldSpringiness, volumeSpringiness, i, filters, ref vertices, ref vertexSpatialIndices, ref cellVertexIndices, ref tetrahedraVertexIndices); } @@ -728,7 +729,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) BufferPool.Return(ref cellVertexIndices); BufferPool.Return(ref tetrahedraVertexIndices); - Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 100, -.5f), 10, Simulation.Shapes, new Sphere(5))); + //Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 100, -.5f), 10, Simulation.Shapes, new Sphere(5))); Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(1500, 1, 1500)), 0.1f))); Simulation.Statics.Add(new StaticDescription(new Vector3(0, -1.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Sphere(3)), 0.1f))); From 7fb53e15519ea9829c6040e1ee3bc37e9894a3b9 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 25 Jun 2021 19:01:39 -0500 Subject: [PATCH 041/947] Added a warmstart/prestep combined codepath. 2x faster for single iteration substeps compared to 2.3; similar to current proto-embedded substepper. --- .../Constraints/FourBodyTypeProcessor.cs | 10 + .../Constraints/OneBodyTypeProcessor.cs | 27 +++ .../Constraints/ThreeBodyTypeProcessor.cs | 9 + .../Constraints/TwoBodyTypeProcessor.cs | 28 ++- BepuPhysics/Constraints/TypeProcessor.cs | 4 + BepuPhysics/PositionFirstTimestepper2.cs | 74 +++++++ BepuPhysics/Solver.cs | 2 + BepuPhysics/Solver_Solve2.cs | 195 ++++++++++++++++++ BepuPhysics/SubsteppingTimestepper2.cs | 109 ++++++++++ Demos/Demo.cs | 3 +- Demos/DemoSet.cs | 2 +- Demos/Demos/NewtDemo.cs | 14 +- Demos/Program.cs | 3 + 13 files changed, 471 insertions(+), 9 deletions(-) create mode 100644 BepuPhysics/PositionFirstTimestepper2.cs create mode 100644 BepuPhysics/Solver_Solve2.cs create mode 100644 BepuPhysics/SubsteppingTimestepper2.cs diff --git a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs index c7daa1ab4..12a32528e 100644 --- a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs @@ -295,5 +295,15 @@ public override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, float dt { throw new NotImplementedException(); } + + + public override void Prestep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + { + throw new NotImplementedException(); + } + public override void JacobiPrestep2(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + { + throw new NotImplementedException(); + } } } diff --git a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs index fab776943..be8be221b 100644 --- a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs @@ -297,6 +297,33 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f } } + public unsafe override void Prestep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + { + ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); + ref var bodyReferencesBase = ref Unsafe.AsRef>(typeBatch.BodyReferences.Memory); + ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); + ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); + ref var bodyVelocities = ref bodies.ActiveSet.Velocities; + var function = default(TConstraintFunctions); + for (int i = startBundle; i < exclusiveEndBundle; ++i) + { + ref var prestep = ref Unsafe.Add(ref prestepBase, i); + ref var projection = ref Unsafe.Add(ref projectionBase, i); + ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); + ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); + var count = GetCountInBundle(ref typeBatch, i); + bodies.GatherInertia(ref references, count, out var inertiaA); + Bodies.GatherVelocities(ref bodyVelocities, ref references, count, out var wsvA); + function.Prestep(bodies, ref references, count, dt, inverseDt, ref inertiaA, ref prestep, out projection); + function.WarmStart(ref wsvA, ref projection, ref accumulatedImpulses); + Bodies.ScatterVelocities(ref wsvA, ref bodyVelocities, ref references, count); + } + } + public override void JacobiPrestep2(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + { + throw new NotImplementedException(); + } + } public abstract class OneBodyContactTypeProcessor diff --git a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs index 112348fab..18291e0df 100644 --- a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs @@ -283,5 +283,14 @@ public override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, float dt { throw new NotImplementedException(); } + + public override void Prestep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + { + throw new NotImplementedException(); + } + public override void JacobiPrestep2(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + { + throw new NotImplementedException(); + } } } diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index f8215174a..624f5a67a 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -300,7 +300,6 @@ public unsafe override void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodi } } - public unsafe override void WarmStart2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); @@ -343,6 +342,33 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f Bodies.ScatterVelocities(ref wsvA, ref wsvB, ref bodyVelocities, ref bodyReferences, count); } } + + public unsafe override void Prestep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + { + ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); + ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); + ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); + ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); + ref var bodyVelocities = ref bodies.ActiveSet.Velocities; + var function = default(TConstraintFunctions); + for (int i = startBundle; i < exclusiveEndBundle; ++i) + { + ref var prestep = ref Unsafe.Add(ref prestepBase, i); + ref var projection = ref Unsafe.Add(ref projectionBase, i); + ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); + ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); + var count = GetCountInBundle(ref typeBatch, i); + bodies.GatherInertia(ref references, count, out var inertiaA, out var inertiaB); + Bodies.GatherVelocities(ref bodyVelocities, ref references, count, out var wsvA, out var wsvB); + function.Prestep(bodies, ref references, count, dt, inverseDt, ref inertiaA, ref inertiaB, ref prestep, out projection); + function.WarmStart(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); + Bodies.ScatterVelocities(ref wsvA, ref wsvB, ref bodyVelocities, ref references, count); + } + } + public override void JacobiPrestep2(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + { + throw new NotImplementedException(); + } } public abstract class TwoBodyContactTypeProcessor diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index 5a991ee75..fe1aaa0ef 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -139,6 +139,10 @@ internal unsafe abstract void CopySleepingToActive( public abstract void WarmStart2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); public abstract void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); + + public abstract void Prestep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); + public abstract void JacobiPrestep2(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); + public virtual void IncrementallyUpdateContactData(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int end) { Debug.Fail("A contact data update was scheduled for a type batch that does not have a contact data update implementation."); diff --git a/BepuPhysics/PositionFirstTimestepper2.cs b/BepuPhysics/PositionFirstTimestepper2.cs new file mode 100644 index 000000000..6b358a272 --- /dev/null +++ b/BepuPhysics/PositionFirstTimestepper2.cs @@ -0,0 +1,74 @@ +using System; +using System.Collections.Generic; +using System.Text; +using BepuUtilities; + +namespace BepuPhysics +{ + /// + /// Updates the simulation in the order of: sleeper -> integrate body poses, velocity and bounding boxes -> collision detection -> solver -> data structure optimization. + /// + public class PositionFirstTimestepper2 : ITimestepper + { + /// + /// Fires after the sleeper completes and before bodies are integrated. + /// + public event TimestepperStageHandler Slept; + /// + /// Fires after bodies have had their position, velocity, and bounding boxes updated, but before collision detection begins. + /// + public event TimestepperStageHandler BeforeCollisionDetection; + /// + /// Fires after all collisions have been identified, but before constraints are solved. + /// + public event TimestepperStageHandler CollisionsDetected; + /// + /// Fires after the solver executes and before data structures are incrementally optimized. + /// + public event TimestepperStageHandler ConstraintsSolved; + + public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDispatcher = null) + { + //Note that there is a reason to put the sleep *after* velocity integration. That sounds a little weird, but there's a good reason: + //When the narrow phase activates a bunch of objects in a pile, their accumulated impulses will represent all forces acting on them at the time of sleep. + //That includes gravity. If we sleep objects *before* gravity is applied in a given frame, then when those bodies are awakened, the accumulated impulses + //will be less accurate because they assume that gravity has already been applied. This can cause a small bump. + //So, velocity integration (and deactivation candidacy management) could come before sleep. + + //Sleep at the start, on the other hand, stops some forms of unintuitive behavior when using direct awakenings. Just a matter of preference. + simulation.Sleep(threadDispatcher); + Slept?.Invoke(dt, threadDispatcher); + + //Note that pose integrator comes before collision detection and solving. This is a shift from v1, where collision detection went first. + //This is a tradeoff: + //1) Any externally set velocities will be integrated without input from the solver. The v1-style external velocity control won't work as well- + //the user would instead have to change velocities after the pose integrator runs. This isn't perfect either, since the pose integrator is also responsible + //for updating the bounding boxes used for collision detection. + //2) By bundling bounding box calculation with pose integration, you avoid redundant pose and velocity memory accesses. + //3) Generated contact positions are in sync with the integrated poses. + //That's often helpful for gameplay purposes- you don't have to reinterpret contact data when creating graphical effects or positioning sound sources. + + //#1 is a difficult problem, though. There is no fully 'correct' place to change velocities. We might just have to bite the bullet and create a + //inertia tensor/bounding box update separate from pose integration. If the cache gets evicted in between (virtually guaranteed unless no stages run), + //this basically means an extra 100-200 microseconds per frame on a processor with ~20GBps bandwidth simulating 32768 bodies. + + //Note that the reason why the pose integrator comes first instead of, say, the solver, is that the solver relies on world space inertias calculated by the pose integration. + //If the pose integrator doesn't run first, we either need + //1) complicated on demand updates of world inertia when objects are added or local inertias are changed or + //2) local->world inertia calculation before the solver. + simulation.IntegrateBodiesAndUpdateBoundingBoxes(dt, threadDispatcher); + BeforeCollisionDetection?.Invoke(dt, threadDispatcher); + + simulation.CollisionDetection(dt, threadDispatcher); + CollisionsDetected?.Invoke(dt, threadDispatcher); + + //simulation.Solve(dt, threadDispatcher); + simulation.Profiler.Start(simulation.Solver); + simulation.Solver.Solve2(dt, threadDispatcher); + simulation.Profiler.End(simulation.Solver); + ConstraintsSolved?.Invoke(dt, threadDispatcher); + + simulation.IncrementallyOptimizeDataStructures(threadDispatcher); + } + } +} diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index f3338f01e..adace0df0 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -179,6 +179,7 @@ public int CountConstraints() Action solveWorker; + Action solve2Worker; Action incrementalContactUpdateWorker; public Solver(Bodies bodies, BufferPool pool, int iterationCount, int fallbackBatchThreshold, int initialCapacity, @@ -196,6 +197,7 @@ public Solver(Bodies bodies, BufferPool pool, int iterationCount, int fallbackBa batchReferencedHandles = new QuickList(fallbackBatchThreshold + 1, pool); ResizeHandleCapacity(initialCapacity); solveWorker = SolveWorker; + solve2Worker = Solve2Worker; solveStepWorker = SolveStepWorker; solveStep2Worker = SolveStep2Worker; incrementalContactUpdateWorker = IncrementalContactUpdateWorker; diff --git a/BepuPhysics/Solver_Solve2.cs b/BepuPhysics/Solver_Solve2.cs new file mode 100644 index 000000000..d577d1c04 --- /dev/null +++ b/BepuPhysics/Solver_Solve2.cs @@ -0,0 +1,195 @@ +using BepuUtilities; +using BepuUtilities.Collections; +using BepuUtilities.Memory; +using BepuPhysics.Constraints; +using System; +using System.Collections.Generic; +using System.Diagnostics; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; +using System.Text; +using System.Threading; + +namespace BepuPhysics +{ + public partial class Solver + { + + + struct Prestep2StageFunction : IStageFunction + { + public float Dt; + public float InverseDt; + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void Execute(Solver solver, int blockIndex) + { + ref var block = ref solver.context.ConstraintBlocks.Blocks[blockIndex]; + ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; + var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; + typeProcessor.Prestep2(ref typeBatch, solver.bodies, Dt, InverseDt, block.StartBundle, block.End); + } + } + + struct Prestep2FallbackStageFunction : IStageFunction + { + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void Execute(Solver solver, int blockIndex) + { + ref var block = ref solver.context.ConstraintBlocks.Blocks[blockIndex]; + ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; + var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; + //typeProcessor.JacobiPrestep2(ref typeBatch, ref solver.bodies.ActiveSet.Velocities, solver ref solver.context.FallbackResults[block.TypeBatchIndex], block.StartBundle, block.End); + + } + } + + void Solve2Worker(int workerIndex) + { + int prestepStart = GetUniformlyDistributedStart(workerIndex, context.ConstraintBlocks.Blocks.Count, context.WorkerCount, 0); + int fallbackStart = GetUniformlyDistributedStart(workerIndex, context.FallbackBlocks.Blocks.Count, context.WorkerCount, 0); + Buffer batchStarts; + ref var activeSet = ref ActiveSet; + unsafe + { + //stackalloc is actually a little bit slow since the localsinit behavior forces a zeroing. + //Fortunately, this executes once per thread per frame. With 32 batches, it would add... a few nanoseconds per frame. We can accept that overhead. + //This is preferred over preallocating on the heap- we might write to these values and we don't want to risk false sharing for no reason. + //A single instance of false sharing would cost far more than the overhead of zeroing out the array. + var batchStartsData = stackalloc int[activeSet.Batches.Count]; + batchStarts = new Buffer(batchStartsData, activeSet.Batches.Count); + } + for (int batchIndex = 0; batchIndex < activeSet.Batches.Count; ++batchIndex) + { + var batchOffset = batchIndex > 0 ? context.BatchBoundaries[batchIndex - 1] : 0; + var batchCount = context.BatchBoundaries[batchIndex] - batchOffset; + batchStarts[batchIndex] = GetUniformlyDistributedStart(workerIndex, batchCount, context.WorkerCount, batchOffset); + } + + + int syncStage = 0; + //The claimed and unclaimed state swap after every usage of both pingpong claims buffers. + int claimedState = 1; + int unclaimedState = 0; + var bounds = context.WorkerBoundsA; + var boundsBackBuffer = context.WorkerBoundsB; + Debug.Assert(activeSet.Batches.Count > 0, "Don't dispatch if there are no constraints."); + + //Note that every batch has a different start position. Each covers a different subset of constraints, so they require different start locations. + GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); + //claimedState ^= 1; + //unclaimedState ^= 1; + var prestepStage = new Prestep2StageFunction { Dt = context.Dt, InverseDt = 1f / context.Dt }; + for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) + { + var batchOffset = batchIndex > 0 ? context.BatchBoundaries[batchIndex - 1] : 0; + //Don't use the warm start to guess at the solve iteration work distribution. + var workerBatchStartCopy = batchStarts[batchIndex]; + ExecuteStage(ref prestepStage, ref context.ConstraintBlocks, ref bounds, ref boundsBackBuffer, workerIndex, batchOffset, context.BatchBoundaries[batchIndex], + ref workerBatchStartCopy, ref syncStage, claimedState, unclaimedState); + } + var fallbackScatterStage = new FallbackScatterStageFunction(); + if (fallbackExists) + { + var prestepFallbackStage = new Prestep2FallbackStageFunction(); + var batchStart = FallbackBatchThreshold > 0 ? context.BatchBoundaries[FallbackBatchThreshold - 1] : 0; + //Don't use the warm start to guess at the solve iteration work distribution. + var workerBatchStartCopy = batchStarts[FallbackBatchThreshold]; + ExecuteStage(ref prestepFallbackStage, ref context.ConstraintBlocks, ref bounds, ref boundsBackBuffer, workerIndex, batchStart, context.BatchBoundaries[FallbackBatchThreshold], + ref workerBatchStartCopy, ref syncStage, claimedState, unclaimedState); + ExecuteStage(ref fallbackScatterStage, ref context.FallbackBlocks, ref bounds, ref boundsBackBuffer, + workerIndex, 0, context.FallbackBlocks.Blocks.Count, ref fallbackStart, ref syncStage, unclaimedState, claimedState); //note claim state swap: fallback scatter claims have no prestep, so it's off by one cycle + } + claimedState ^= 1; + unclaimedState ^= 1; + + var solveStage = new SolveStageFunction(); + var solveFallbackStage = new SolveFallbackStageFunction(); + for (int iterationIndex = 0; iterationIndex < iterationCount; ++iterationIndex) + { + for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) + { + var batchOffset = batchIndex > 0 ? context.BatchBoundaries[batchIndex - 1] : 0; + ExecuteStage(ref solveStage, ref context.ConstraintBlocks, ref bounds, ref boundsBackBuffer, workerIndex, batchOffset, context.BatchBoundaries[batchIndex], + ref batchStarts[batchIndex], ref syncStage, claimedState, unclaimedState); + } + if (fallbackExists) + { + var batchOffset = FallbackBatchThreshold > 0 ? context.BatchBoundaries[FallbackBatchThreshold - 1] : 0; + ExecuteStage(ref solveFallbackStage, ref context.ConstraintBlocks, ref bounds, ref boundsBackBuffer, workerIndex, batchOffset, context.BatchBoundaries[FallbackBatchThreshold], + ref batchStarts[FallbackBatchThreshold], ref syncStage, claimedState, unclaimedState); + ExecuteStage(ref fallbackScatterStage, ref context.FallbackBlocks, ref bounds, ref boundsBackBuffer, + workerIndex, 0, context.FallbackBlocks.Blocks.Count, ref fallbackStart, ref syncStage, unclaimedState, claimedState); //note claim state swap: fallback scatter claims have no prestep, so it's off by one cycle + } + claimedState ^= 1; + unclaimedState ^= 1; + } + } + + + + public void Solve2(float dt, IThreadDispatcher threadDispatcher = null) + { + if (threadDispatcher == null) + { + var inverseDt = 1f / dt; + ref var activeSet = ref ActiveSet; + GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); + //TODO: May want to consider executing warmstart immediately following the prestep. Multithreading can't do that, so there could be some bitwise differences introduced. + //On the upside, it would make use of cached data. + for (int i = 0; i < synchronizedBatchCount; ++i) + { + ref var batch = ref activeSet.Batches[i]; + for (int j = 0; j < batch.TypeBatches.Count; ++j) + { + ref var typeBatch = ref batch.TypeBatches[j]; + TypeProcessors[typeBatch.TypeId].Prestep2(ref typeBatch, bodies, dt, inverseDt, 0, typeBatch.BundleCount); + } + } + Buffer fallbackResults = default; + if (fallbackExists) + { + ref var batch = ref activeSet.Batches[FallbackBatchThreshold]; + FallbackBatch.AllocateResults(this, pool, ref batch, out fallbackResults); + for (int j = 0; j < batch.TypeBatches.Count; ++j) + { + ref var typeBatch = ref batch.TypeBatches[j]; + TypeProcessors[typeBatch.TypeId].JacobiPrestep2(ref typeBatch, bodies, ref activeSet.Fallback, ref fallbackResults[j], dt, inverseDt, 0, typeBatch.BundleCount); + } + activeSet.Fallback.ScatterVelocities(bodies, this, ref fallbackResults, 0, activeSet.Fallback.BodyCount); + } + for (int iterationIndex = 0; iterationIndex < iterationCount; ++iterationIndex) + { + for (int i = 0; i < synchronizedBatchCount; ++i) + { + ref var batch = ref activeSet.Batches[i]; + for (int j = 0; j < batch.TypeBatches.Count; ++j) + { + ref var typeBatch = ref batch.TypeBatches[j]; + TypeProcessors[typeBatch.TypeId].SolveIteration(ref typeBatch, ref bodies.ActiveSet.Velocities, 0, typeBatch.BundleCount); + } + } + if (fallbackExists) + { + ref var batch = ref activeSet.Batches[FallbackBatchThreshold]; + for (int j = 0; j < batch.TypeBatches.Count; ++j) + { + ref var typeBatch = ref batch.TypeBatches[j]; + TypeProcessors[typeBatch.TypeId].JacobiSolveIteration(ref typeBatch, ref bodies.ActiveSet.Velocities, ref fallbackResults[j], 0, typeBatch.BundleCount); + } + activeSet.Fallback.ScatterVelocities(bodies, this, ref fallbackResults, 0, activeSet.Fallback.BodyCount); + } + } + if (fallbackExists) + { + FallbackBatch.DisposeResults(this, pool, ref activeSet.Batches[FallbackBatchThreshold], ref fallbackResults); + } + } + else + { + ExecuteMultithreaded(dt, threadDispatcher, solve2Worker); + } + } + + } +} diff --git a/BepuPhysics/SubsteppingTimestepper2.cs b/BepuPhysics/SubsteppingTimestepper2.cs new file mode 100644 index 000000000..6854077df --- /dev/null +++ b/BepuPhysics/SubsteppingTimestepper2.cs @@ -0,0 +1,109 @@ +using System; +using System.Collections.Generic; +using System.Diagnostics; +using System.Text; +using BepuUtilities; + +namespace BepuPhysics +{ + /// + /// Updates the simulation in the order of: sleeper -> predict body bounding boxes -> collision detection -> LOOP { contact data update (if on iteration > 0) -> integrate body velocities -> solver -> integrate body poses } -> data structure optimization. + /// Each inner loop execution simulates a sub-timestep of length dt/substepCount. + /// Useful for simulations with difficult to solve constraint systems that need shorter timestep durations but which don't require high frequency collision detection. + /// + public class SubsteppingTimestepper2 : ITimestepper + { + /// + /// Gets or sets the number of substeps to execute during each timestep. + /// + public int SubstepCount { get; set; } + + /// + /// Fires after the sleeper completes and before bodies are integrated. + /// + public event TimestepperStageHandler Slept; + /// + /// Fires after bodies have their bounding boxes updated for the frame's predicted motion and before collision detection. + /// + public event TimestepperStageHandler BeforeCollisionDetection; + /// + /// Fires after all collisions have been identified, but before the substep loop begins. + /// + public event TimestepperStageHandler CollisionsDetected; + /// + /// Fires at the beginning of a substep. + /// + public event TimestepperSubstepStageHandler SubstepStarted; + /// + /// Fires after contact constraints are incrementally updated at the beginning of substeps after the first and before velocities are integrated. + /// + public event TimestepperSubstepStageHandler ContactConstraintsUpdatedForSubstep; + /// + /// Fires after bodies have their velocities integrated and before the solver executes. + /// + public event TimestepperSubstepStageHandler VelocitiesIntegrated; + /// + /// Fires after the solver executes and before body poses are integrated. + /// + public event TimestepperSubstepStageHandler ConstraintsSolved; + /// + /// Fires after bodies have their poses integrated and before the substep ends. + /// + public event TimestepperSubstepStageHandler PosesIntegrated; + /// + /// Fires at the end of a substep. + /// + public event TimestepperSubstepStageHandler SubstepEnded; + /// + /// Fires after all substeps are finished executing and before data structures are incrementally optimized. + /// + public event TimestepperStageHandler SubstepsComplete; + + public SubsteppingTimestepper2(int substepCount) + { + SubstepCount = substepCount; + } + + public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDispatcher = null) + { + simulation.Sleep(threadDispatcher); + Slept?.Invoke(dt, threadDispatcher); + + simulation.PredictBoundingBoxes(dt, threadDispatcher); + BeforeCollisionDetection?.Invoke(dt, threadDispatcher); + + simulation.CollisionDetection(dt, threadDispatcher); + CollisionsDetected?.Invoke(dt, threadDispatcher); + + Debug.Assert(SubstepCount >= 0, "Substep count should be positive."); + var substepDt = dt / SubstepCount; + + for (int substepIndex = 0; substepIndex < SubstepCount; ++substepIndex) + { + SubstepStarted?.Invoke(substepIndex, dt, threadDispatcher); + if (substepIndex > 0) + { + //This takes the place of collision detection for the substeps. It uses the current velocity to update penetration depths. + //It's definitely an approximation, but it's important for avoiding some obviously weird behavior. + //Note that we do not run this on the first iteration- the actual collision detection above takes care of it. + simulation.IncrementallyUpdateContactConstraints(substepDt, threadDispatcher); + ContactConstraintsUpdatedForSubstep?.Invoke(substepIndex, dt, threadDispatcher); + } + simulation.IntegrateVelocitiesAndUpdateInertias(substepDt, threadDispatcher); + VelocitiesIntegrated?.Invoke(substepIndex, dt, threadDispatcher); + + simulation.Profiler.Start(simulation.Solver); + simulation.Solver.Solve2(substepDt, threadDispatcher); + simulation.Profiler.End(simulation.Solver); + ConstraintsSolved?.Invoke(substepIndex, dt, threadDispatcher); + + simulation.IntegratePoses(substepDt, threadDispatcher); + PosesIntegrated?.Invoke(substepIndex, dt, threadDispatcher); + SubstepEnded?.Invoke(substepIndex, dt, threadDispatcher); + } + SubstepsComplete?.Invoke(dt, threadDispatcher); + + simulation.IncrementallyOptimizeDataStructures(threadDispatcher); + } + } +} diff --git a/Demos/Demo.cs b/Demos/Demo.cs index 4b57d7df9..a7fc0061f 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -42,7 +42,8 @@ protected Demo() //there won't be enough memory bandwidth to even feed half the physical cores. Using all 128 logical cores would just add overhead. //It may be worth using something like hwloc to extract extra information to reason about. - var targetThreadCount = Math.Max(1, Environment.ProcessorCount > 4 ? Environment.ProcessorCount - 2 : Environment.ProcessorCount - 1); + //var targetThreadCount = Math.Max(1, Environment.ProcessorCount > 4 ? Environment.ProcessorCount - 2 : Environment.ProcessorCount - 1); + var targetThreadCount = 64; ThreadDispatcher = new SimpleThreadDispatcher(targetThreadCount); } diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index cd152afdf..f90f14bc8 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -45,6 +45,7 @@ struct Option public DemoSet() { + AddOption(); AddOption(); AddOption(); AddOption(); @@ -53,7 +54,6 @@ public DemoSet() AddOption(); AddOption(); AddOption(); - AddOption(); AddOption(); AddOption(); AddOption(); diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index 153a9f39e..b7f09e815 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -667,7 +667,7 @@ internal unsafe static void CreateDeformable(Simulation simulation, in Vector3 p position + QuaternionEx.Transform(vertices[i], orientation), orientation), vertexInertia, //Bodies don't have to have collidables. Take advantage of this for all the internal vertices. new CollidableDescription(vertexEdgeCounts[i] == edgeCountForInternalVertex ? new TypedIndex() : vertexShapeIndex, cellSize * 0.5f), - new BodyActivityDescription(0.01f))); + new BodyActivityDescription(-0.01f))); ref var vertexSpatialIndex = ref vertexSpatialIndices[i]; filters.Allocate(vertexHandles[i]) = new DeformableCollisionFilter(vertexSpatialIndex.X, vertexSpatialIndex.Y, vertexSpatialIndex.Z, instanceId); } @@ -710,8 +710,9 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //The PositionFirstTimestepper is the simplest timestepping mode, but since it integrates velocity into position at the start of the frame, directly modified velocities outside of the timestep //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. - Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(8)); - //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper(), solverIterationCount: 20); + //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(6)); + //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper2(), solverIterationCount: 18); + Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(6), solverIterationCount: 1); var meshContent = content.Load("Content\\newt.obj"); float cellSize = 0.1f; @@ -719,9 +720,10 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) out var vertices, out var vertexSpatialIndices, out var cellVertexIndices, out var tetrahedraVertexIndices); var weldSpringiness = new SpringSettings(60f, 1f); var volumeSpringiness = new SpringSettings(30f, 1); - for (int i = 0; i < 8; ++i) + for (int i = 0; i < 40; ++i) { - CreateDeformable(Simulation, new Vector3(i * 3, 5 + i * 1.5f, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), MathF.PI * (i * 0.55f)), 1f, cellSize, weldSpringiness, volumeSpringiness, i, filters, ref vertices, ref vertexSpatialIndices, ref cellVertexIndices, ref tetrahedraVertexIndices); + //CreateDeformable(Simulation, new Vector3(i * 3, 5 + i * 1.5f, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), MathF.PI * (i * 0.55f)), 1f, cellSize, weldSpringiness, volumeSpringiness, i, filters, ref vertices, ref vertexSpatialIndices, ref cellVertexIndices, ref tetrahedraVertexIndices); + CreateDeformable(Simulation, new Vector3(i * 3, cellSize + i * 0f, 0), Quaternion.Identity, 1f, cellSize, weldSpringiness, volumeSpringiness, i, filters, ref vertices, ref vertexSpatialIndices, ref cellVertexIndices, ref tetrahedraVertexIndices); } BufferPool.Return(ref vertices); @@ -732,7 +734,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 100, -.5f), 10, Simulation.Shapes, new Sphere(5))); Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(1500, 1, 1500)), 0.1f))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -1.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Sphere(3)), 0.1f))); + //Simulation.Statics.Add(new StaticDescription(new Vector3(0, -1.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Sphere(3)), 0.1f))); } diff --git a/Demos/Program.cs b/Demos/Program.cs index 189962aa9..ba902a5cf 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -1,5 +1,7 @@ using BepuUtilities; using DemoContentLoader; +using Demos.Demos; +using Demos.SpecializedTests; using DemoUtilities; using OpenTK; @@ -17,6 +19,7 @@ static void Main(string[] args) { content = ContentArchive.Load(stream); } + HeadlessTest.Test(content, 2, 32, 512); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); From ffb7c8ce719a52b034c21db731febacb98db8651 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 27 Jun 2021 15:31:20 -0500 Subject: [PATCH 042/947] Finagling some teststuff. --- BepuPhysics/Solver_SubsteppingSolve.cs | 8 ++------ Demos/Demo.cs | 2 +- Demos/Demos/NewtDemo.cs | 15 +++++++++------ Demos/Program.cs | 2 +- 4 files changed, 13 insertions(+), 14 deletions(-) diff --git a/BepuPhysics/Solver_SubsteppingSolve.cs b/BepuPhysics/Solver_SubsteppingSolve.cs index 263df5cfa..7ad9aa930 100644 --- a/BepuPhysics/Solver_SubsteppingSolve.cs +++ b/BepuPhysics/Solver_SubsteppingSolve.cs @@ -81,9 +81,7 @@ void SolveStepWorker(int workerIndex) Debug.Assert(activeSet.Batches.Count > 0, "Don't dispatch if there are no constraints."); GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); - var solveStage = new SolveStepStageFunction(); - solveStage.Dt = context.Dt; - solveStage.InverseDt = 1f / context.Dt; + var solveStage = new SolveStepStageFunction { Dt = context.Dt, InverseDt = 1f / context.Dt }; for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) { var batchOffset = batchIndex > 0 ? context.BatchBoundaries[batchIndex - 1] : 0; @@ -92,9 +90,7 @@ void SolveStepWorker(int workerIndex) } if (fallbackExists) { - var solveFallbackStage = new FallbackSolveStepStageFunction(); - solveFallbackStage.Dt = context.Dt; - solveFallbackStage.InverseDt = 1f / context.Dt; + var solveFallbackStage = new FallbackSolveStepStageFunction { Dt = context.Dt, InverseDt = 1f / context.Dt }; var fallbackScatterStage = new FallbackScatterStageFunction(); var batchOffset = FallbackBatchThreshold > 0 ? context.BatchBoundaries[FallbackBatchThreshold - 1] : 0; ExecuteStage(ref solveFallbackStage, ref context.ConstraintBlocks, ref bounds, ref boundsBackBuffer, workerIndex, batchOffset, context.BatchBoundaries[FallbackBatchThreshold], diff --git a/Demos/Demo.cs b/Demos/Demo.cs index a7fc0061f..f00656836 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -43,7 +43,7 @@ protected Demo() //It may be worth using something like hwloc to extract extra information to reason about. //var targetThreadCount = Math.Max(1, Environment.ProcessorCount > 4 ? Environment.ProcessorCount - 2 : Environment.ProcessorCount - 1); - var targetThreadCount = 64; + var targetThreadCount = 32; ThreadDispatcher = new SimpleThreadDispatcher(targetThreadCount); } diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index b7f09e815..eb3da8f1f 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -683,8 +683,10 @@ internal unsafe static void CreateDeformable(Simulation simulation, in Vector3 p LocalOrientation = Quaternion.Identity, SpringSettings = weldSpringiness }); - //Simulation.Solver.Add(vertexHandles[edge.A], vertexHandles[edge.B], + //simulation.Solver.Add(vertexHandles[edge.A], vertexHandles[edge.B], // new CenterDistanceConstraint(offset.Length(), weldSpringiness)); + //simulation.Solver.Add(vertexHandles[edge.A], vertexHandles[edge.B], + // new BallSocket { LocalOffsetA = offset * 0.5f, LocalOffsetB = offset * -0.5f, SpringSettings = weldSpringiness }); } //for (int i = 0; i < tetrahedraVertexIndices.Length; ++i) //{ @@ -710,20 +712,21 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //The PositionFirstTimestepper is the simplest timestepping mode, but since it integrates velocity into position at the start of the frame, directly modified velocities outside of the timestep //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. - //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(6)); - //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper2(), solverIterationCount: 18); - Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(6), solverIterationCount: 1); + Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper(1)); + //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3)); + //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper2(), solverIterationCount: 5); + //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper2(3), solverIterationCount: 1); var meshContent = content.Load("Content\\newt.obj"); float cellSize = 0.1f; DumbTetrahedralizer.Tetrahedralize(meshContent.Triangles, cellSize, BufferPool, out var vertices, out var vertexSpatialIndices, out var cellVertexIndices, out var tetrahedraVertexIndices); - var weldSpringiness = new SpringSettings(60f, 1f); + var weldSpringiness = new SpringSettings(120f, 1f); var volumeSpringiness = new SpringSettings(30f, 1); for (int i = 0; i < 40; ++i) { //CreateDeformable(Simulation, new Vector3(i * 3, 5 + i * 1.5f, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), MathF.PI * (i * 0.55f)), 1f, cellSize, weldSpringiness, volumeSpringiness, i, filters, ref vertices, ref vertexSpatialIndices, ref cellVertexIndices, ref tetrahedraVertexIndices); - CreateDeformable(Simulation, new Vector3(i * 3, cellSize + i * 0f, 0), Quaternion.Identity, 1f, cellSize, weldSpringiness, volumeSpringiness, i, filters, ref vertices, ref vertexSpatialIndices, ref cellVertexIndices, ref tetrahedraVertexIndices); + CreateDeformable(Simulation, new Vector3(i * 3, cellSize * 2f + i * 0f, 0), Quaternion.Identity, 1f, cellSize, weldSpringiness, volumeSpringiness, i, filters, ref vertices, ref vertexSpatialIndices, ref cellVertexIndices, ref tetrahedraVertexIndices); } BufferPool.Return(ref vertices); diff --git a/Demos/Program.cs b/Demos/Program.cs index ba902a5cf..14a613027 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -19,7 +19,7 @@ static void Main(string[] args) { content = ContentArchive.Load(stream); } - HeadlessTest.Test(content, 2, 32, 512); + //HeadlessTest.Test(content, 2, 32, 512); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); From 11f6665ca209040f624ca10005e65f0532f8e219 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 28 Jun 2021 21:38:50 -0500 Subject: [PATCH 043/947] DOES NOT BUILD. In the middle of speculatively combining pose and velocity together for the embedded timestepper. --- BepuPhysics/Bodies.cs | 676 ++++++++---------- BepuPhysics/BodyDescription.cs | 25 +- BepuPhysics/BodyProperties.cs | 72 ++ BepuPhysics/BodyReference.cs | 19 +- BepuPhysics/BodySet.cs | 31 +- BepuPhysics/CollisionDetection/NarrowPhase.cs | 11 +- .../Constraints/FourBodyTypeProcessor.cs | 8 +- .../Constraints/OneBodyTypeProcessor.cs | 20 +- .../Constraints/ThreeBodyTypeProcessor.cs | 8 +- .../Constraints/TwoBodyTypeProcessor.cs | 18 +- BepuPhysics/Constraints/TypeProcessor.cs | 8 +- BepuPhysics/FallbackBatch.cs | 2 +- BepuPhysics/IslandAwakener.cs | 11 +- BepuPhysics/IslandSleeper.cs | 3 +- BepuPhysics/PoseIntegrator.cs | 104 ++- BepuPhysics/Simulation_Queries.cs | 2 +- BepuPhysics/Solver_Solve.cs | 16 +- BepuPhysics/Solver_Solve2.cs | 4 +- 18 files changed, 504 insertions(+), 534 deletions(-) diff --git a/BepuPhysics/Bodies.cs b/BepuPhysics/Bodies.cs index b2de36660..af9ce63dd 100644 --- a/BepuPhysics/Bodies.cs +++ b/BepuPhysics/Bodies.cs @@ -7,6 +7,7 @@ using BepuPhysics.Collidables; using BepuPhysics.CollisionDetection; using BepuUtilities; +using static BepuUtilities.GatherScatter; namespace BepuPhysics { @@ -113,7 +114,7 @@ public void UpdateBounds(BodyHandle bodyHandle) ref var collidable = ref set.Collidables[location.Index]; if (collidable.Shape.Exists) { - shapes.UpdateBounds(set.Poses[location.Index], ref collidable.Shape, out var bodyBounds); + shapes.UpdateBounds(set.MotionStates[location.Index].Pose, ref collidable.Shape, out var bodyBounds); if (location.SetIndex == 0) { broadPhase.UpdateActiveBounds(collidable.BroadPhaseIndex, bodyBounds.Min, bodyBounds.Max); @@ -174,7 +175,7 @@ public unsafe BodyHandle Add(in BodyDescription description) //Out of room; need to resize. ResizeHandles(HandleToLocation.Length << 1); } - Debug.Assert(Math.Abs(description.Pose.Orientation.Length() - 1) < 1e-6f, "Orientation should be initialized to a unit length quaternion."); + Debug.Assert(Math.Abs(description.MotionState.Pose.Orientation.Length() - 1) < 1e-6f, "Orientation should be initialized to a unit length quaternion."); //All new bodies are active for simplicity. Someday, it may be worth offering an optimized path for inactives, but it adds complexity. //(Directly adding inactive bodies can be helpful in some networked open world scenarios.) @@ -184,7 +185,7 @@ public unsafe BodyHandle Add(in BodyDescription description) if (description.Collidable.Shape.Exists) { - AddCollidableToBroadPhase(handle, description.Pose, description.LocalInertia, ref ActiveSet.Collidables[index]); + AddCollidableToBroadPhase(handle, description.MotionState.Pose, description.LocalInertia, ref ActiveSet.Collidables[index]); } return handle; } @@ -382,7 +383,7 @@ void UpdateForShapeChange(BodyHandle handle, int activeBodyIndex, TypedIndex old if (newShape.Exists) { //Add a collidable to the simulation for the new shape. - AddCollidableToBroadPhase(handle, set.Poses[activeBodyIndex], set.LocalInertias[activeBodyIndex], ref set.Collidables[activeBodyIndex]); + AddCollidableToBroadPhase(handle, set.MotionStates[activeBodyIndex].Pose, set.LocalInertias[activeBodyIndex], ref set.Collidables[activeBodyIndex]); } else { @@ -504,18 +505,17 @@ internal void ValidateMotionStates() { for (int j = 0; j < set.Count; ++j) { - ref var pose = ref set.Poses[j]; - ref var velocity = ref set.Velocities[j]; + ref var state = ref set.MotionStates[j]; try { - pose.Position.Validate(); - pose.Orientation.ValidateOrientation(); - velocity.Linear.Validate(); - velocity.Angular.Validate(); + state.Pose.Position.Validate(); + state.Pose.Orientation.ValidateOrientation(); + state.Velocity.Linear.Validate(); + state.Velocity.Angular.Validate(); } catch { - Console.WriteLine($"Validation failed on body {i} of set {j}. Position: {pose.Position}, orientation: {pose.Orientation}, linear: {velocity.Linear}, angular: {velocity.Angular}"); + Console.WriteLine($"Validation failed on body {i} of set {j}. Position: {state.Pose.Position}, orientation: {state.Pose.Orientation}, linear: {state.Velocity.Linear}, angular: {state.Velocity.Angular}"); throw; } @@ -525,185 +525,46 @@ internal void ValidateMotionStates() } [MethodImpl(MethodImplOptions.AggressiveInlining)] - private static void GatherInertiaForBody(ref BodyInertia source, ref BodyInertias targetSlot) + private static void WriteGatherInertia(ref int bundleBaseBodyIndexInSet, int bodyIndexInBundle, ref Buffer states, ref BodyInertias targetSlot) { - GatherScatter.GetFirst(ref targetSlot.InverseInertiaTensor.XX) = source.InverseInertiaTensor.XX; - GatherScatter.GetFirst(ref targetSlot.InverseInertiaTensor.YX) = source.InverseInertiaTensor.YX; - GatherScatter.GetFirst(ref targetSlot.InverseInertiaTensor.YY) = source.InverseInertiaTensor.YY; - GatherScatter.GetFirst(ref targetSlot.InverseInertiaTensor.ZX) = source.InverseInertiaTensor.ZX; - GatherScatter.GetFirst(ref targetSlot.InverseInertiaTensor.ZY) = source.InverseInertiaTensor.ZY; - GatherScatter.GetFirst(ref targetSlot.InverseInertiaTensor.ZZ) = source.InverseInertiaTensor.ZZ; - GatherScatter.GetFirst(ref targetSlot.InverseMass) = source.InverseMass; + ref var source = ref states[Unsafe.Add(ref bundleBaseBodyIndexInSet, bodyIndexInBundle)]; + GetFirst(ref targetSlot.InverseInertiaTensor.XX) = source.InverseInertiaTensor.XX; + GetFirst(ref targetSlot.InverseInertiaTensor.YX) = source.InverseInertiaTensor.YX; + GetFirst(ref targetSlot.InverseInertiaTensor.YY) = source.InverseInertiaTensor.YY; + GetFirst(ref targetSlot.InverseInertiaTensor.ZX) = source.InverseInertiaTensor.ZX; + GetFirst(ref targetSlot.InverseInertiaTensor.ZY) = source.InverseInertiaTensor.ZY; + GetFirst(ref targetSlot.InverseInertiaTensor.ZZ) = source.InverseInertiaTensor.ZZ; + GetFirst(ref targetSlot.InverseMass) = source.InverseMass; } - /// - /// Gathers inertia for one body bundle into an AOSOA bundle. - /// - /// Active body indices being gathered. - /// Number of bodies in the bundle. - /// Gathered inertia of body A. - /// Gathered inertia of body B. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void GatherInertia(ref Vector references, int count, - out BodyInertias inertiaA) - { - Unsafe.SkipInit(out inertiaA); - Debug.Assert(count >= 0 && count <= Vector.Count); - //Grab the base references for the body indices. Note that we make use of the references memory layout again. - ref var baseIndexA = ref Unsafe.As, int>(ref references); - for (int i = 0; i < count; ++i) - { - GatherInertiaForBody(ref Inertias[Unsafe.Add(ref baseIndexA, i)], ref GatherScatter.GetOffsetInstance(ref inertiaA, i)); - } - } - - /// - /// Gathers inertia for two body bundles into AOSOA bundles. - /// - /// Active body indices being gathered. - /// Number of bodies in the bundle. - /// Gathered inertia of body A. - /// Gathered inertia of body B. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void GatherInertia(ref TwoBodyReferences references, int count, out BodyInertias inertiaA, out BodyInertias inertiaB) - { - Unsafe.SkipInit(out inertiaA); - Unsafe.SkipInit(out inertiaB); - Debug.Assert(count >= 0 && count <= Vector.Count); - //Grab the base references for the body indices. Note that we make use of the references memory layout again. - ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); - ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); - for (int i = 0; i < count; ++i) - { - GatherInertiaForBody(ref Inertias[Unsafe.Add(ref baseIndexA, i)], ref GatherScatter.GetOffsetInstance(ref inertiaA, i)); - GatherInertiaForBody(ref Inertias[Unsafe.Add(ref baseIndexB, i)], ref GatherScatter.GetOffsetInstance(ref inertiaB, i)); - } - } - - /// - /// Gathers inertia for three body bundles into AOSOA bundles. - /// - /// Active body indices being gathered. - /// Number of bodies in the bundle. - /// Gathered inertia of body A. - /// Gathered inertia of body B. - /// Gathered inertia of body C. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void GatherInertia(ref ThreeBodyReferences references, int count, out BodyInertias inertiaA, out BodyInertias inertiaB, out BodyInertias inertiaC) - { - Unsafe.SkipInit(out inertiaA); - Unsafe.SkipInit(out inertiaB); - Unsafe.SkipInit(out inertiaC); - Debug.Assert(count >= 0 && count <= Vector.Count); - //Grab the base references for the body indices. Note that we make use of the references memory layout again. - ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); - ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); - ref var baseIndexC = ref Unsafe.As, int>(ref references.IndexC); - for (int i = 0; i < count; ++i) - { - GatherInertiaForBody(ref Inertias[Unsafe.Add(ref baseIndexA, i)], ref GatherScatter.GetOffsetInstance(ref inertiaA, i)); - GatherInertiaForBody(ref Inertias[Unsafe.Add(ref baseIndexB, i)], ref GatherScatter.GetOffsetInstance(ref inertiaB, i)); - GatherInertiaForBody(ref Inertias[Unsafe.Add(ref baseIndexC, i)], ref GatherScatter.GetOffsetInstance(ref inertiaC, i)); - } - } - - /// - /// Gathers inertia for four body bundles into AOSOA bundles. - /// - /// Active body indices being gathered. - /// Number of bodies in the bundle. - /// Gathered inertia of body A. - /// Gathered inertia of body B. - /// Gathered inertia of body C. - /// Gathered inertia of body D. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void GatherInertia(ref FourBodyReferences references, int count, out BodyInertias inertiaA, out BodyInertias inertiaB, out BodyInertias inertiaC, out BodyInertias inertiaD) - { - Unsafe.SkipInit(out inertiaA); - Unsafe.SkipInit(out inertiaB); - Unsafe.SkipInit(out inertiaC); - Unsafe.SkipInit(out inertiaD); - Debug.Assert(count >= 0 && count <= Vector.Count); - //Grab the base references for the body indices. Note that we make use of the references memory layout again. - ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); - ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); - ref var baseIndexC = ref Unsafe.As, int>(ref references.IndexC); - ref var baseIndexD = ref Unsafe.As, int>(ref references.IndexD); - for (int i = 0; i < count; ++i) - { - GatherInertiaForBody(ref Inertias[Unsafe.Add(ref baseIndexA, i)], ref GatherScatter.GetOffsetInstance(ref inertiaA, i)); - GatherInertiaForBody(ref Inertias[Unsafe.Add(ref baseIndexB, i)], ref GatherScatter.GetOffsetInstance(ref inertiaB, i)); - GatherInertiaForBody(ref Inertias[Unsafe.Add(ref baseIndexC, i)], ref GatherScatter.GetOffsetInstance(ref inertiaC, i)); - GatherInertiaForBody(ref Inertias[Unsafe.Add(ref baseIndexD, i)], ref GatherScatter.GetOffsetInstance(ref inertiaD, i)); - } - } - - /// - /// Gathers orientations for two body bundles into AOSOA bundles. - /// - /// Active body indices being gathered. - /// Number of body pairs in the bundle. - /// Gathered orientation of body A. - /// Gathered orientation of body B. - //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public void GatherOrientation(ref TwoBodyReferences references, int count, - out QuaternionWide orientationA, out QuaternionWide orientationB) - { - Unsafe.SkipInit(out orientationA); - Unsafe.SkipInit(out orientationB); - Debug.Assert(count >= 0 && count <= Vector.Count); - //Grab the base references for the body indices. Note that we make use of the references memory layout again. - ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); - ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); - - ref var poses = ref ActiveSet.Poses; - for (int i = 0; i < count; ++i) - { - ref var indexA = ref Unsafe.Add(ref baseIndexA, i); - QuaternionWide.WriteFirst(poses[indexA].Orientation, ref GatherScatter.GetOffsetInstance(ref orientationA, i)); - - ref var indexB = ref Unsafe.Add(ref baseIndexB, i); - QuaternionWide.WriteFirst(poses[indexB].Orientation, ref GatherScatter.GetOffsetInstance(ref orientationB, i)); - } - } - - /// - /// Gathers orientations for one body bundles into AOSOA bundles. - /// - /// Active body indices being gathered. - /// Number of body pairs in the bundle. - /// Gathered orientation of bodies in the bundle. - //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public void GatherOrientation(ref Vector references, int count, - out QuaternionWide orientation) + private static void WriteGatherState(ref int bundleBaseBodyIndexInSet, int bodyIndexInBundle, ref Buffer states, + ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocities velocity) { - Unsafe.SkipInit(out orientation); - Debug.Assert(count >= 0 && count <= Vector.Count); - //Grab the base references for the body indices. Note that we make use of the references memory layout again. - ref var baseIndexA = ref Unsafe.As, int>(ref references); - - ref var poses = ref ActiveSet.Poses; - for (int i = 0; i < count; ++i) - { - ref var indexA = ref Unsafe.Add(ref baseIndexA, i); - QuaternionWide.WriteFirst(poses[indexA].Orientation, ref GatherScatter.GetOffsetInstance(ref orientation, i)); - } + ref var state = ref states[Unsafe.Add(ref bundleBaseBodyIndexInSet, bodyIndexInBundle)]; + Vector3Wide.WriteFirst(state.Pose.Position, ref GetOffsetInstance(ref position, bodyIndexInBundle)); + QuaternionWide.WriteFirst(state.Pose.Orientation, ref GetOffsetInstance(ref orientation, bodyIndexInBundle)); + Vector3Wide.WriteFirst(state.Velocity.Linear, ref GetOffsetInstance(ref velocity.Linear, bodyIndexInBundle)); + Vector3Wide.WriteFirst(state.Velocity.Angular, ref GetOffsetInstance(ref velocity.Angular, bodyIndexInBundle)); } - - /// - /// Gathers pose information for a body bundle into an AOSOA bundle. + /// Gathers motion state information for a body bundle into an AOSOA bundle. /// /// Active body indices being gathered. /// Number of body pairs in the bundle. /// Gathered absolute position of the body. /// Gathered orientation of the body. + /// Gathered velocity of the body. + /// Gathered inertia of the body. //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public void GatherPose(ref Vector references, int count, out Vector3Wide position, out QuaternionWide orientation) + public void GatherState(ref Vector references, int count, + out Vector3Wide position, out QuaternionWide orientation, out BodyVelocities velocity, out BodyInertias inertia) { Unsafe.SkipInit(out position); Unsafe.SkipInit(out orientation); + Unsafe.SkipInit(out velocity); + Unsafe.SkipInit(out inertia); //TODO: This function and its users (which should be relatively few) is a problem for large world position precision. //It directly reports the position, thereby infecting vectorized logic with the high precision representation. //You might be able to redesign the users of this function to not need it, but that comes with its own difficulties @@ -712,14 +573,11 @@ public void GatherPose(ref Vector references, int count, out Vector3Wide po //Grab the base references for the body indices. Note that we make use of the references memory layout again. ref var baseIndex = ref Unsafe.As, int>(ref references); - ref var poses = ref ActiveSet.Poses; + ref var states = ref ActiveSet.MotionStates; for (int i = 0; i < count; ++i) { - ref var indexA = ref Unsafe.Add(ref baseIndex, i); - ref var poseA = ref poses[indexA]; - Vector3Wide.WriteFirst(poseA.Position, ref GatherScatter.GetOffsetInstance(ref position, i)); - QuaternionWide.WriteFirst(poseA.Orientation, ref GatherScatter.GetOffsetInstance(ref orientation, i)); - + WriteGatherState(ref baseIndex, i, ref states, ref position, ref orientation, ref velocity); + WriteGatherInertia(ref baseIndex, i, ref Inertias, ref inertia); } } @@ -728,34 +586,39 @@ public void GatherPose(ref Vector references, int count, out Vector3Wide po /// /// Active body indices being gathered. /// Number of body pairs in the bundle. - /// Gathered offsets from body A to body B. + /// Gathered offsets from body A to body B. /// Gathered orientation of body A. /// Gathered orientation of body B. + /// Gathered velocity of body A. + /// Gathered velocity of body B. + /// Gathered inertia of body A. + /// Gathered inertia of body B. //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public void GatherPose(ref TwoBodyReferences references, int count, - out Vector3Wide offsetB, out QuaternionWide orientationA, out QuaternionWide orientationB) + public void GatherState(ref TwoBodyReferences references, int count, + out QuaternionWide orientationA, out BodyVelocities velocityA, out BodyInertias inertiaA, + out Vector3Wide ab, + out QuaternionWide orientationB, out BodyVelocities velocityB, out BodyInertias inertiaB) { Unsafe.SkipInit(out Vector3Wide positionA); Unsafe.SkipInit(out Vector3Wide positionB); Unsafe.SkipInit(out orientationA); Unsafe.SkipInit(out orientationB); + Unsafe.SkipInit(out velocityA); + Unsafe.SkipInit(out velocityB); + Unsafe.SkipInit(out inertiaA); + Unsafe.SkipInit(out inertiaB); Debug.Assert(count >= 0 && count <= Vector.Count); //Grab the base references for the body indices. Note that we make use of the references memory layout again. ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); - ref var poses = ref ActiveSet.Poses; + ref var states = ref ActiveSet.MotionStates; for (int i = 0; i < count; ++i) { - ref var indexA = ref Unsafe.Add(ref baseIndexA, i); - ref var poseA = ref poses[indexA]; - Vector3Wide.WriteFirst(poseA.Position, ref GatherScatter.GetOffsetInstance(ref positionA, i)); - QuaternionWide.WriteFirst(poseA.Orientation, ref GatherScatter.GetOffsetInstance(ref orientationA, i)); - - ref var indexB = ref Unsafe.Add(ref baseIndexB, i); - ref var poseB = ref poses[indexB]; - Vector3Wide.WriteFirst(poseB.Position, ref GatherScatter.GetOffsetInstance(ref positionB, i)); - QuaternionWide.WriteFirst(poseB.Orientation, ref GatherScatter.GetOffsetInstance(ref orientationB, i)); + WriteGatherState(ref baseIndexA, i, ref states, ref positionA, ref orientationA, ref velocityA); + WriteGatherInertia(ref baseIndexA, i, ref Inertias, ref inertiaA); + WriteGatherState(ref baseIndexB, i, ref states, ref positionB, ref orientationB, ref velocityB); + WriteGatherInertia(ref baseIndexB, i, ref Inertias, ref inertiaB); } //TODO: In future versions, we will likely store the body position in different forms to allow for extremely large worlds. //That will be an opt-in feature. The default implementation will use the FP32 representation, but the user could choose to swap it out for a fp64 or fixed64 representation. @@ -765,80 +628,120 @@ public void GatherPose(ref TwoBodyReferences references, int count, //In order to support other absolute positions, we'll need alternate implementations of this and other functions. //But for the most part, we don't want to pay the overhead of an abstract invocation within the inner loop of the solver. //Given the current limits of C# and the compiler, the best option seems to be conditional compilation. - Vector3Wide.Subtract(positionB, positionA, out offsetB); - } - - /// - /// Gathers relative positions for a two body bundle into an AOSOA bundle. - /// - /// Active body indices being gathered. - /// Number of body pairs in the bundle. - /// Gathered offset from body A to of body B. - //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public void GatherOffsets(ref TwoBodyReferences references, int count, out Vector3Wide ab) - { - Unsafe.SkipInit(out Vector3Wide positionA); - Unsafe.SkipInit(out Vector3Wide positionB); - Debug.Assert(count >= 0 && count <= Vector.Count); - //Grab the base references for the body indices. Note that we make use of the references memory layout again. - ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); - ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); - - ref var poses = ref ActiveSet.Poses; - for (int i = 0; i < count; ++i) - { - Vector3Wide.WriteFirst(poses[Unsafe.Add(ref baseIndexA, i)].Position, ref GatherScatter.GetOffsetInstance(ref positionA, i)); - Vector3Wide.WriteFirst(poses[Unsafe.Add(ref baseIndexB, i)].Position, ref GatherScatter.GetOffsetInstance(ref positionB, i)); - } - //Same as other gather case; this is sensitive to changes in the representation of body position. In high precision modes, this'll need to change. Vector3Wide.Subtract(positionB, positionA, out ab); } + /// - /// Gathers relative positions for a three body bundle into an AOSOA bundle. + /// Gathers orientations and relative positions for a three body bundle into an AOSOA bundle. /// /// Active body indices being gathered. /// Number of body pairs in the bundle. - /// Gathered offset from body A to of body B. - /// Gathered offset from body A to of body C. + /// Gathered offsets from body A to body B. + /// Gathered offsets from body A to body C. + /// Gathered orientation of body A. + /// Gathered orientation of body B. + /// Gathered orientation of body C. + /// Gathered velocity of body A. + /// Gathered velocity of body B. + /// Gathered velocity of body C. + /// Gathered inertia of body A. + /// Gathered inertia of body B. + /// Gathered inertia of body C. //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public void GatherOffsets(ref ThreeBodyReferences references, int count, out Vector3Wide ab, out Vector3Wide ac) + public void GatherState(ref ThreeBodyReferences references, int count, + out QuaternionWide orientationA, out BodyVelocities velocityA, out BodyInertias inertiaA, + out Vector3Wide ab, + out QuaternionWide orientationB, out BodyVelocities velocityB, out BodyInertias inertiaB + out Vector3Wide ac, + out QuaternionWide orientationC, out BodyVelocities velocityC, out BodyInertias inertiaC) { Unsafe.SkipInit(out Vector3Wide positionA); Unsafe.SkipInit(out Vector3Wide positionB); Unsafe.SkipInit(out Vector3Wide positionC); + Unsafe.SkipInit(out orientationA); + Unsafe.SkipInit(out orientationB); + Unsafe.SkipInit(out orientationC); + Unsafe.SkipInit(out velocityA); + Unsafe.SkipInit(out velocityB); + Unsafe.SkipInit(out velocityC); + Unsafe.SkipInit(out inertiaA); + Unsafe.SkipInit(out inertiaB); + Unsafe.SkipInit(out inertiaC); Debug.Assert(count >= 0 && count <= Vector.Count); //Grab the base references for the body indices. Note that we make use of the references memory layout again. ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); ref var baseIndexC = ref Unsafe.As, int>(ref references.IndexC); - ref var poses = ref ActiveSet.Poses; + ref var states = ref ActiveSet.MotionStates; for (int i = 0; i < count; ++i) { - Vector3Wide.WriteFirst(poses[Unsafe.Add(ref baseIndexA, i)].Position, ref GatherScatter.GetOffsetInstance(ref positionA, i)); - Vector3Wide.WriteFirst(poses[Unsafe.Add(ref baseIndexB, i)].Position, ref GatherScatter.GetOffsetInstance(ref positionB, i)); - Vector3Wide.WriteFirst(poses[Unsafe.Add(ref baseIndexC, i)].Position, ref GatherScatter.GetOffsetInstance(ref positionC, i)); + WriteGatherState(ref baseIndexA, i, ref states, ref positionA, ref orientationA, ref velocityA); + WriteGatherInertia(ref baseIndexA, i, ref Inertias, ref inertiaA); + WriteGatherState(ref baseIndexB, i, ref states, ref positionB, ref orientationB, ref velocityB); + WriteGatherInertia(ref baseIndexB, i, ref Inertias, ref inertiaB); + WriteGatherState(ref baseIndexC, i, ref states, ref positionC, ref orientationC, ref velocityC); + WriteGatherInertia(ref baseIndexC, i, ref Inertias, ref inertiaC); } - //Same as two body case; this is sensitive to changes in the representation of body position. In high precision modes, this'll need to change. + //TODO: In future versions, we will likely store the body position in different forms to allow for extremely large worlds. + //That will be an opt-in feature. The default implementation will use the FP32 representation, but the user could choose to swap it out for a fp64 or fixed64 representation. + //This affects other systems- AABB calculation, pose integration, solving, and in extreme (64 bit) cases, the broadphase. + //We want to insulate other systems from direct knowledge about the implementation of positions when possible. + //These functions support the solver's needs while hiding absolute positions. + //In order to support other absolute positions, we'll need alternate implementations of this and other functions. + //But for the most part, we don't want to pay the overhead of an abstract invocation within the inner loop of the solver. + //Given the current limits of C# and the compiler, the best option seems to be conditional compilation. Vector3Wide.Subtract(positionB, positionA, out ab); Vector3Wide.Subtract(positionC, positionA, out ac); } + /// - /// Gathers relative positions for a four body bundle into an AOSOA bundle. + /// Gathers orientations and relative positions for a four body bundle into an AOSOA bundle. /// /// Active body indices being gathered. /// Number of body pairs in the bundle. - /// Gathered offset from body A to of body B. - /// Gathered offset from body A to of body C. - /// Gathered offset from body A to of body D. + /// Gathered offsets from body A to body B. + /// Gathered offsets from body A to body C. + /// Gathered offsets from body A to body D. + /// Gathered orientation of body A. + /// Gathered orientation of body B. + /// Gathered orientation of body C. + /// Gathered orientation of body D. + /// Gathered velocity of body A. + /// Gathered velocity of body B. + /// Gathered velocity of body C. + /// Gathered velocity of body D. + /// Gathered inertia of body A. + /// Gathered inertia of body B. + /// Gathered inertia of body C. + /// Gathered inertia of body C. //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public void GatherOffsets(ref FourBodyReferences references, int count, out Vector3Wide ab, out Vector3Wide ac, out Vector3Wide ad) + public void GatherState(ref FourBodyReferences references, int count, + out QuaternionWide orientationA, out BodyVelocities velocityA, out BodyInertias inertiaA, + out Vector3Wide ab, + out QuaternionWide orientationB, out BodyVelocities velocityB, out BodyInertias inertiaB, + out Vector3Wide ac, + out QuaternionWide orientationC, out BodyVelocities velocityC, out BodyInertias inertiaC, + out Vector3Wide ad, + out QuaternionWide orientationD, out BodyVelocities velocityD, out BodyInertias inertiaD) { Unsafe.SkipInit(out Vector3Wide positionA); Unsafe.SkipInit(out Vector3Wide positionB); Unsafe.SkipInit(out Vector3Wide positionC); Unsafe.SkipInit(out Vector3Wide positionD); + Unsafe.SkipInit(out orientationA); + Unsafe.SkipInit(out orientationB); + Unsafe.SkipInit(out orientationC); + Unsafe.SkipInit(out orientationD); + Unsafe.SkipInit(out velocityA); + Unsafe.SkipInit(out velocityB); + Unsafe.SkipInit(out velocityC); + Unsafe.SkipInit(out velocityD); + Unsafe.SkipInit(out inertiaA); + Unsafe.SkipInit(out inertiaB); + Unsafe.SkipInit(out inertiaC); + Unsafe.SkipInit(out inertiaD); Debug.Assert(count >= 0 && count <= Vector.Count); //Grab the base references for the body indices. Note that we make use of the references memory layout again. ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); @@ -846,146 +749,161 @@ public void GatherOffsets(ref FourBodyReferences references, int count, out Vect ref var baseIndexC = ref Unsafe.As, int>(ref references.IndexC); ref var baseIndexD = ref Unsafe.As, int>(ref references.IndexD); - ref var poses = ref ActiveSet.Poses; + ref var states = ref ActiveSet.MotionStates; for (int i = 0; i < count; ++i) { - Vector3Wide.WriteFirst(poses[Unsafe.Add(ref baseIndexA, i)].Position, ref GatherScatter.GetOffsetInstance(ref positionA, i)); - Vector3Wide.WriteFirst(poses[Unsafe.Add(ref baseIndexB, i)].Position, ref GatherScatter.GetOffsetInstance(ref positionB, i)); - Vector3Wide.WriteFirst(poses[Unsafe.Add(ref baseIndexC, i)].Position, ref GatherScatter.GetOffsetInstance(ref positionC, i)); - Vector3Wide.WriteFirst(poses[Unsafe.Add(ref baseIndexD, i)].Position, ref GatherScatter.GetOffsetInstance(ref positionD, i)); + WriteGatherState(ref baseIndexA, i, ref states, ref positionA, ref orientationA, ref velocityA); + WriteGatherInertia(ref baseIndexA, i, ref Inertias, ref inertiaA); + WriteGatherState(ref baseIndexB, i, ref states, ref positionB, ref orientationB, ref velocityB); + WriteGatherInertia(ref baseIndexB, i, ref Inertias, ref inertiaB); + WriteGatherState(ref baseIndexC, i, ref states, ref positionC, ref orientationC, ref velocityC); + WriteGatherInertia(ref baseIndexC, i, ref Inertias, ref inertiaC); + WriteGatherState(ref baseIndexD, i, ref states, ref positionD, ref orientationD, ref velocityD); + WriteGatherInertia(ref baseIndexD, i, ref Inertias, ref inertiaD); } - //Same as two body case; this is sensitive to changes in the representation of body position. In high precision modes, this'll need to change. + //TODO: In future versions, we will likely store the body position in different forms to allow for extremely large worlds. + //That will be an opt-in feature. The default implementation will use the FP32 representation, but the user could choose to swap it out for a fp64 or fixed64 representation. + //This affects other systems- AABB calculation, pose integration, solving, and in extreme (64 bit) cases, the broadphase. + //We want to insulate other systems from direct knowledge about the implementation of positions when possible. + //These functions support the solver's needs while hiding absolute positions. + //In order to support other absolute positions, we'll need alternate implementations of this and other functions. + //But for the most part, we don't want to pay the overhead of an abstract invocation within the inner loop of the solver. + //Given the current limits of C# and the compiler, the best option seems to be conditional compilation. Vector3Wide.Subtract(positionB, positionA, out ab); Vector3Wide.Subtract(positionC, positionA, out ac); - Vector3Wide.Subtract(positionD, positionA, out ad); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - private static unsafe void GatherVelocities(ref Buffer sources, ref BodyVelocities target, ref int baseIndex, int innerIndex) - { - ref var targetSlot = ref GatherScatter.GetOffsetInstance(ref target, innerIndex); - ref var source = ref sources[Unsafe.Add(ref baseIndex, innerIndex)]; - GatherScatter.GetFirst(ref targetSlot.Linear.X) = source.Linear.X; - GatherScatter.GetFirst(ref targetSlot.Linear.Y) = source.Linear.Y; - GatherScatter.GetFirst(ref targetSlot.Linear.Z) = source.Linear.Z; - GatherScatter.GetFirst(ref targetSlot.Angular.X) = source.Angular.X; - GatherScatter.GetFirst(ref targetSlot.Angular.Y) = source.Angular.Y; - GatherScatter.GetFirst(ref targetSlot.Angular.Z) = source.Angular.Z; + Vector3Wide.Subtract(positionC, positionA, out ad); } - /// - /// Gathers velocities for one body bundle and stores it into a velocity bundle. - /// - /// Active set indices of the bodies to gather velocity data for. - /// Number of bodies in the bundle. - /// Gathered velocities. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static unsafe void GatherVelocities(ref Buffer sourceVelocities, ref Vector references, int count, out BodyVelocities velocities) - { - Unsafe.SkipInit(out velocities); - //Grab the base references for the body indices. Note that we make use of the references memory layout again. - ref var baseIndex = ref Unsafe.As, int>(ref references); - for (int i = 0; i < count; ++i) - { - GatherVelocities(ref sourceVelocities, ref velocities, ref baseIndex, i); - } - } - - /// - /// Gathers velocities for two body bundles and stores it into velocity bundles. - /// - /// Active set indices of the bodies to gather velocity data for. - /// Number of body pairs in the bundle. - /// Gathered velocities of A bodies. - /// Gathered velocities of B bodies. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static unsafe void GatherVelocities(ref Buffer sourceVelocities, ref TwoBodyReferences references, int count, out BodyVelocities velocitiesA, out BodyVelocities velocitiesB) - { - Unsafe.SkipInit(out velocitiesA); - Unsafe.SkipInit(out velocitiesB); - Debug.Assert(count >= 0 && count <= Vector.Count); - //Grab the base references for the body indices. Note that we make use of the references memory layout again. - ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); - ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); - for (int i = 0; i < count; ++i) - { - GatherVelocities(ref sourceVelocities, ref velocitiesA, ref baseIndexA, i); - GatherVelocities(ref sourceVelocities, ref velocitiesB, ref baseIndexB, i); - } - } - - /// - /// Gathers velocities for three body bundles and stores it into velocity bundles. - /// - /// Active set indices of the bodies to gather velocity data for. - /// Number of body pairs in the bundle. - /// Gathered velocities of A bodies. - /// Gathered velocities of B bodies. - /// Gathered velocities of C bodies. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static unsafe void GatherVelocities(ref Buffer sourceVelocities, ref ThreeBodyReferences references, int count, - out BodyVelocities velocitiesA, out BodyVelocities velocitiesB, out BodyVelocities velocitiesC) - { - Unsafe.SkipInit(out velocitiesA); - Unsafe.SkipInit(out velocitiesB); - Unsafe.SkipInit(out velocitiesC); - Debug.Assert(count >= 0 && count <= Vector.Count); - //Grab the base references for the body indices. Note that we make use of the references memory layout again. - ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); - ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); - ref var baseIndexC = ref Unsafe.As, int>(ref references.IndexC); - for (int i = 0; i < count; ++i) - { - GatherVelocities(ref sourceVelocities, ref velocitiesA, ref baseIndexA, i); - GatherVelocities(ref sourceVelocities, ref velocitiesB, ref baseIndexB, i); - GatherVelocities(ref sourceVelocities, ref velocitiesC, ref baseIndexC, i); - } - } + //[MethodImpl(MethodImplOptions.AggressiveInlining)] + //private static void WriteLinearGatherState(ref int baseBodyIndexInSet, int bodyIndexInBundle, ref Buffer states, ref Vector3Wide linearVelocity, ref Vector3Wide position) + //{ + // ref var state = ref states[Unsafe.Add(ref baseBodyIndexInSet, bodyIndexInBundle)]; + // Vector3Wide.WriteFirst(state.Pose.Position, ref GetOffsetInstance(ref position, bodyIndexInBundle)); + // Vector3Wide.WriteFirst(state.Velocity.Linear, ref GetOffsetInstance(ref linearVelocity, bodyIndexInBundle)); + //} + ///// + ///// Gathers relative positions and linear velocities for a two body bundle into an AOSOA bundle. + ///// + ///// Active body indices being gathered. + ///// Number of body pairs in the bundle. + ///// Gathered offset from body A to body B. + ///// Linear velocity of body A. + ///// Linear velocity of body B. + ////[MethodImpl(MethodImplOptions.AggressiveInlining)] + //public void GatherLinearState(ref TwoBodyReferences references, int count, out Vector3Wide ab, out Vector3Wide linearVelocityA, out Vector3Wide linearVelocityB) + //{ + // Unsafe.SkipInit(out Vector3Wide positionA); + // Unsafe.SkipInit(out Vector3Wide positionB); + // Unsafe.SkipInit(out linearVelocityA); + // Unsafe.SkipInit(out linearVelocityB); + // Debug.Assert(count >= 0 && count <= Vector.Count); + // //Grab the base references for the body indices. Note that we make use of the references memory layout again. + // ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); + // ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); + + // ref var states = ref ActiveSet.MotionStates; + // for (int i = 0; i < count; ++i) + // { + // WriteLinearGatherState(ref baseIndexA, i, ref states, ref positionA, ref linearVelocityA); + // WriteLinearGatherState(ref baseIndexB, i, ref states, ref positionB, ref linearVelocityB); + // } + // //Same as other gather case; this is sensitive to changes in the representation of body position. In high precision modes, this'll need to change. + // Vector3Wide.Subtract(positionB, positionA, out ab); + //} + + + ///// + ///// Gathers relative positions and linear velocities for a three body bundle into an AOSOA bundle. + ///// + ///// Active body indices being gathered. + ///// Number of body pairs in the bundle. + ///// Gathered offset from body A to body B. + ///// Gathered offset from body A to body C. + ///// Linear velocity of body A. + ///// Linear velocity of body B. + ///// Linear velocity of body C. + ////[MethodImpl(MethodImplOptions.AggressiveInlining)] + //public void GatherLinearState(ref ThreeBodyReferences references, int count, out Vector3Wide ab, out Vector3Wide ac, out Vector3Wide linearVelocityA, out Vector3Wide linearVelocityB, out Vector3Wide linearVelocityC) + //{ + // Unsafe.SkipInit(out Vector3Wide positionA); + // Unsafe.SkipInit(out Vector3Wide positionB); + // Unsafe.SkipInit(out Vector3Wide positionC); + // Unsafe.SkipInit(out linearVelocityA); + // Unsafe.SkipInit(out linearVelocityB); + // Unsafe.SkipInit(out linearVelocityC); + // Debug.Assert(count >= 0 && count <= Vector.Count); + // //Grab the base references for the body indices. Note that we make use of the references memory layout again. + // ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); + // ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); + // ref var baseIndexC = ref Unsafe.As, int>(ref references.IndexC); + + // ref var states = ref ActiveSet.MotionStates; + // for (int i = 0; i < count; ++i) + // { + // WriteLinearGatherState(ref baseIndexA, i, ref states, ref positionA, ref linearVelocityA); + // WriteLinearGatherState(ref baseIndexB, i, ref states, ref positionB, ref linearVelocityB); + // WriteLinearGatherState(ref baseIndexC, i, ref states, ref positionC, ref linearVelocityC); + // } + // //Same as other gather case; this is sensitive to changes in the representation of body position. In high precision modes, this'll need to change. + // Vector3Wide.Subtract(positionB, positionA, out ab); + // Vector3Wide.Subtract(positionC, positionA, out ac); + //} + + + ///// + ///// Gathers relative positions and linear velocities for a four body bundle into an AOSOA bundle. + ///// + ///// Active body indices being gathered. + ///// Number of body pairs in the bundle. + ///// Gathered offset from body A to body B. + ///// Gathered offset from body A to body C. + ///// Gathered offset from body A to body D. + ///// Linear velocity of body A. + ///// Linear velocity of body B. + ///// Linear velocity of body C. + ///// Linear velocity of body D. + ////[MethodImpl(MethodImplOptions.AggressiveInlining)] + //public void GatherLinearState(ref FourBodyReferences references, int count, out Vector3Wide ab, out Vector3Wide ac, out Vector3Wide ad, + // out Vector3Wide linearVelocityA, out Vector3Wide linearVelocityB, out Vector3Wide linearVelocityC, out Vector3Wide linearVelocityD) + //{ + // Unsafe.SkipInit(out Vector3Wide positionA); + // Unsafe.SkipInit(out Vector3Wide positionB); + // Unsafe.SkipInit(out Vector3Wide positionC); + // Unsafe.SkipInit(out Vector3Wide positionD); + // Unsafe.SkipInit(out linearVelocityA); + // Unsafe.SkipInit(out linearVelocityB); + // Unsafe.SkipInit(out linearVelocityC); + // Unsafe.SkipInit(out linearVelocityD); + // Debug.Assert(count >= 0 && count <= Vector.Count); + // //Grab the base references for the body indices. Note that we make use of the references memory layout again. + // ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); + // ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); + // ref var baseIndexC = ref Unsafe.As, int>(ref references.IndexC); + // ref var baseIndexD = ref Unsafe.As, int>(ref references.IndexD); + + // ref var states = ref ActiveSet.MotionStates; + // for (int i = 0; i < count; ++i) + // { + // WriteLinearGatherState(ref baseIndexA, i, ref states, ref positionA, ref linearVelocityA); + // WriteLinearGatherState(ref baseIndexB, i, ref states, ref positionB, ref linearVelocityB); + // WriteLinearGatherState(ref baseIndexC, i, ref states, ref positionC, ref linearVelocityC); + // WriteLinearGatherState(ref baseIndexD, i, ref states, ref positionD, ref linearVelocityD); + // } + // //Same as other gather case; this is sensitive to changes in the representation of body position. In high precision modes, this'll need to change. + // Vector3Wide.Subtract(positionB, positionA, out ab); + // Vector3Wide.Subtract(positionC, positionA, out ac); + // Vector3Wide.Subtract(positionD, positionA, out ad); + //} - /// - /// Gathers velocities for four body bundles and stores it into velocity bundles. - /// - /// Active set indices of the bodies to gather velocity data for. - /// Number of body pairs in the bundle. - /// Gathered velocities of A bodies. - /// Gathered velocities of B bodies. - /// Gathered velocities of C bodies. - /// Gathered velocities of D bodies. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static unsafe void GatherVelocities(ref Buffer sourceVelocities, ref FourBodyReferences references, int count, - out BodyVelocities velocitiesA, out BodyVelocities velocitiesB, out BodyVelocities velocitiesC, out BodyVelocities velocitiesD) - { - Unsafe.SkipInit(out velocitiesA); - Unsafe.SkipInit(out velocitiesB); - Unsafe.SkipInit(out velocitiesC); - Unsafe.SkipInit(out velocitiesD); - Debug.Assert(count >= 0 && count <= Vector.Count); - //Grab the base references for the body indices. Note that we make use of the references memory layout again. - ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); - ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); - ref var baseIndexC = ref Unsafe.As, int>(ref references.IndexC); - ref var baseIndexD = ref Unsafe.As, int>(ref references.IndexD); - for (int i = 0; i < count; ++i) - { - GatherVelocities(ref sourceVelocities, ref velocitiesA, ref baseIndexA, i); - GatherVelocities(ref sourceVelocities, ref velocitiesB, ref baseIndexB, i); - GatherVelocities(ref sourceVelocities, ref velocitiesC, ref baseIndexC, i); - GatherVelocities(ref sourceVelocities, ref velocitiesD, ref baseIndexD, i); - } - } [MethodImpl(MethodImplOptions.AggressiveInlining)] - private static unsafe void ScatterVelocities(ref BodyVelocities sourceVelocities, ref Buffer targets, ref int baseIndex, int innerIndex) + private unsafe void ScatterVelocities(ref BodyVelocities sourceVelocities, ref int baseIndex, int innerIndex) { //TODO: How much value would there be in branching on kinematic state and avoiding a write? Depends a lot on the number of kinematics. - ref var sourceSlot = ref GatherScatter.GetOffsetInstance(ref sourceVelocities, innerIndex); - ref var target = ref targets[Unsafe.Add(ref baseIndex, innerIndex)]; - target.Linear.X = GatherScatter.GetFirst(ref sourceSlot.Linear.X); - target.Linear.Y = GatherScatter.GetFirst(ref sourceSlot.Linear.Y); - target.Linear.Z = GatherScatter.GetFirst(ref sourceSlot.Linear.Z); - target.Angular.X = GatherScatter.GetFirst(ref sourceSlot.Angular.X); - target.Angular.Y = GatherScatter.GetFirst(ref sourceSlot.Angular.Y); - target.Angular.Z = GatherScatter.GetFirst(ref sourceSlot.Angular.Z); + ref var sourceSlot = ref GetOffsetInstance(ref sourceVelocities, innerIndex); + ref var target = ref ActiveSet.MotionStates[Unsafe.Add(ref baseIndex, innerIndex)].Velocity; + target.Linear = new Vector3(sourceSlot.Linear.X[0], sourceSlot.Linear.Y[0], sourceSlot.Linear.Z[0]); + target.Angular = new Vector3(sourceSlot.Angular.X[0], sourceSlot.Angular.Y[0], sourceSlot.Angular.Z[0]); } @@ -996,14 +914,14 @@ private static unsafe void ScatterVelocities(ref BodyVelocities sourceVelocities /// Active set indices of the bodies to scatter velocity data to. /// Number of body pairs in the bundle. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static unsafe void ScatterVelocities(ref BodyVelocities sourceVelocities, ref Buffer targetVelocities, ref Vector references, int count) + public unsafe void ScatterVelocities(ref BodyVelocities sourceVelocities, ref Vector references, int count) { Debug.Assert(count >= 0 && count <= Vector.Count); //Grab the base references for the body indices. Note that we make use of the references memory layout again. ref var baseIndex = ref Unsafe.As, int>(ref references); for (int i = 0; i < count; ++i) { - ScatterVelocities(ref sourceVelocities, ref targetVelocities, ref baseIndex, i); + ScatterVelocities(ref sourceVelocities, ref baseIndex, i); } } @@ -1015,7 +933,7 @@ public static unsafe void ScatterVelocities(ref BodyVelocities sourceVelocities, /// Active set indices of the bodies to scatter velocity data to. /// Number of body pairs in the bundle. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static unsafe void ScatterVelocities(ref BodyVelocities sourceVelocitiesA, ref BodyVelocities sourceVelocitiesB, ref Buffer targetVelocities, + public unsafe void ScatterVelocities(ref BodyVelocities sourceVelocitiesA, ref BodyVelocities sourceVelocitiesB, ref TwoBodyReferences references, int count) { Debug.Assert(count >= 0 && count <= Vector.Count); @@ -1024,8 +942,8 @@ public static unsafe void ScatterVelocities(ref BodyVelocities sourceVelocitiesA ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); for (int i = 0; i < count; ++i) { - ScatterVelocities(ref sourceVelocitiesA, ref targetVelocities, ref baseIndexA, i); - ScatterVelocities(ref sourceVelocitiesB, ref targetVelocities, ref baseIndexB, i); + ScatterVelocities(ref sourceVelocitiesA, ref baseIndexA, i); + ScatterVelocities(ref sourceVelocitiesB, ref baseIndexB, i); } } @@ -1034,13 +952,13 @@ public static unsafe void ScatterVelocities(ref BodyVelocities sourceVelocitiesA /// /// Velocities of body bundle A to scatter. /// Velocities of body bundle B to scatter. - /// Velocities of body bundle A to scatter. + /// Velocities of body bundle C to scatter. /// Active set indices of the bodies to scatter velocity data to. /// Number of body pairs in the bundle. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static unsafe void ScatterVelocities( + public unsafe void ScatterVelocities( ref BodyVelocities sourceVelocitiesA, ref BodyVelocities sourceVelocitiesB, ref BodyVelocities sourceVelocitiesC, - ref Buffer targetVelocities, ref ThreeBodyReferences references, int count) + ref ThreeBodyReferences references, int count) { Debug.Assert(count >= 0 && count <= Vector.Count); //Grab the base references for the body indices. Note that we make use of the references memory layout again. @@ -1049,9 +967,9 @@ public static unsafe void ScatterVelocities( ref var baseIndexC = ref Unsafe.As, int>(ref references.IndexC); for (int i = 0; i < count; ++i) { - ScatterVelocities(ref sourceVelocitiesA, ref targetVelocities, ref baseIndexA, i); - ScatterVelocities(ref sourceVelocitiesB, ref targetVelocities, ref baseIndexB, i); - ScatterVelocities(ref sourceVelocitiesC, ref targetVelocities, ref baseIndexC, i); + ScatterVelocities(ref sourceVelocitiesA, ref baseIndexA, i); + ScatterVelocities(ref sourceVelocitiesB, ref baseIndexB, i); + ScatterVelocities(ref sourceVelocitiesC, ref baseIndexC, i); } } @@ -1060,14 +978,14 @@ public static unsafe void ScatterVelocities( /// /// Velocities of body bundle A to scatter. /// Velocities of body bundle B to scatter. - /// Velocities of body bundle A to scatter. - /// Velocities of body bundle B to scatter. + /// Velocities of body bundle C to scatter. + /// Velocities of body bundle D to scatter. /// Active set indices of the bodies to scatter velocity data to. /// Number of body pairs in the bundle. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static unsafe void ScatterVelocities( + public unsafe void ScatterVelocities( ref BodyVelocities sourceVelocitiesA, ref BodyVelocities sourceVelocitiesB, ref BodyVelocities sourceVelocitiesC, ref BodyVelocities sourceVelocitiesD, - ref Buffer targetVelocities, ref FourBodyReferences references, int count) + ref FourBodyReferences references, int count) { Debug.Assert(count >= 0 && count <= Vector.Count); //Grab the base references for the body indices. Note that we make use of the references memory layout again. @@ -1077,10 +995,10 @@ public static unsafe void ScatterVelocities( ref var baseIndexD = ref Unsafe.As, int>(ref references.IndexD); for (int i = 0; i < count; ++i) { - ScatterVelocities(ref sourceVelocitiesA, ref targetVelocities, ref baseIndexA, i); - ScatterVelocities(ref sourceVelocitiesB, ref targetVelocities, ref baseIndexB, i); - ScatterVelocities(ref sourceVelocitiesC, ref targetVelocities, ref baseIndexC, i); - ScatterVelocities(ref sourceVelocitiesD, ref targetVelocities, ref baseIndexD, i); + ScatterVelocities(ref sourceVelocitiesA, ref baseIndexA, i); + ScatterVelocities(ref sourceVelocitiesB, ref baseIndexB, i); + ScatterVelocities(ref sourceVelocitiesC, ref baseIndexC, i); + ScatterVelocities(ref sourceVelocitiesD, ref baseIndexD, i); } } diff --git a/BepuPhysics/BodyDescription.cs b/BepuPhysics/BodyDescription.cs index 73d57652d..baa47c5e6 100644 --- a/BepuPhysics/BodyDescription.cs +++ b/BepuPhysics/BodyDescription.cs @@ -34,9 +34,8 @@ public BodyActivityDescription(float sleepThreshold, byte minimumTimestepCountUn public struct BodyDescription { - public RigidPose Pose; + public MotionState MotionState; public BodyInertia LocalInertia; - public BodyVelocity Velocity; public CollidableDescription Collidable; public BodyActivityDescription Activity; @@ -81,7 +80,7 @@ public static BodyActivityDescription GetDefaultActivity(in TShape shape /// Constructed description for the body. public static BodyDescription CreateDynamic(in RigidPose pose, in BodyVelocity velocity, in BodyInertia inertia, in CollidableDescription collidable, in BodyActivityDescription activity) { - return new BodyDescription { Pose = pose, Velocity = velocity, LocalInertia = inertia, Activity = activity, Collidable = collidable }; + return new BodyDescription { MotionState = new (pose, velocity), LocalInertia = inertia, Activity = activity, Collidable = collidable }; } /// @@ -94,7 +93,7 @@ public static BodyDescription CreateDynamic(in RigidPose pose, in BodyVelocity v /// Constructed description for the body. public static BodyDescription CreateDynamic(in RigidPose pose, in BodyInertia inertia, in CollidableDescription collidable, in BodyActivityDescription activity) { - return new BodyDescription { Pose = pose, LocalInertia = inertia, Activity = activity, Collidable = collidable }; + return new BodyDescription { MotionState = new(pose), LocalInertia = inertia, Activity = activity, Collidable = collidable }; } /// @@ -108,7 +107,7 @@ public static BodyDescription CreateDynamic(in RigidPose pose, in BodyInertia in /// Constructed description for the body. public static BodyDescription CreateDynamic(in Vector3 position, in BodyVelocity velocity, in BodyInertia inertia, in CollidableDescription collidable, in BodyActivityDescription activity) { - return new BodyDescription { Pose = new RigidPose(position), Velocity = velocity, LocalInertia = inertia, Activity = activity, Collidable = collidable }; + return new BodyDescription { MotionState = new(position, velocity), LocalInertia = inertia, Activity = activity, Collidable = collidable }; } /// @@ -121,7 +120,7 @@ public static BodyDescription CreateDynamic(in Vector3 position, in BodyVelocity /// Constructed description for the body. public static BodyDescription CreateDynamic(in Vector3 position, in BodyInertia inertia, in CollidableDescription collidable, in BodyActivityDescription activity) { - return new BodyDescription { Pose = new RigidPose(position), LocalInertia = inertia, Activity = activity, Collidable = collidable }; + return new BodyDescription { MotionState = new(position), LocalInertia = inertia, Activity = activity, Collidable = collidable }; } /// @@ -140,8 +139,7 @@ public static BodyDescription CreateConvexDynamic( { var description = new BodyDescription { - Pose = pose, - Velocity = velocity, + MotionState = new(pose, velocity), Activity = GetDefaultActivity(shape), Collidable = new CollidableDescription(shapes.Add(shape), GetDefaultSpeculativeMargin(shape)) }; @@ -208,7 +206,7 @@ public static BodyDescription CreateConvexDynamic( /// Constructed description for the body. public static BodyDescription CreateKinematic(in RigidPose pose, in BodyVelocity velocity, in CollidableDescription collidable, in BodyActivityDescription activity) { - return new BodyDescription { Pose = pose, Velocity = velocity, Activity = activity, Collidable = collidable }; + return new BodyDescription { MotionState = new(pose, velocity), Activity = activity, Collidable = collidable }; } /// @@ -220,7 +218,7 @@ public static BodyDescription CreateKinematic(in RigidPose pose, in BodyVelocity /// Constructed description for the body. public static BodyDescription CreateKinematic(in RigidPose pose, in CollidableDescription collidable, in BodyActivityDescription activity) { - return new BodyDescription { Pose = pose, Activity = activity, Collidable = collidable }; + return new BodyDescription { MotionState = new(pose), Activity = activity, Collidable = collidable }; } /// @@ -233,7 +231,7 @@ public static BodyDescription CreateKinematic(in RigidPose pose, in CollidableDe /// Constructed description for the body. public static BodyDescription CreateKinematic(in Vector3 position, in BodyVelocity velocity, in CollidableDescription collidable, in BodyActivityDescription activity) { - return new BodyDescription { Pose = new RigidPose(position), Velocity = velocity, Activity = activity, Collidable = collidable }; + return new BodyDescription { MotionState = new(position, velocity), Activity = activity, Collidable = collidable }; } /// @@ -245,7 +243,7 @@ public static BodyDescription CreateKinematic(in Vector3 position, in BodyVeloci /// Constructed description for the body. public static BodyDescription CreateKinematic(in Vector3 position, in CollidableDescription collidable, in BodyActivityDescription activity) { - return new BodyDescription { Pose = new RigidPose(position), Activity = activity, Collidable = collidable }; + return new BodyDescription { MotionState = new(position), Activity = activity, Collidable = collidable }; } /// @@ -263,8 +261,7 @@ public static BodyDescription CreateConvexKinematic( { var description = new BodyDescription { - Pose = pose, - Velocity = velocity, + MotionState = new(pose, velocity), Activity = GetDefaultActivity(shape), Collidable = new CollidableDescription(shapes.Add(shape), GetDefaultSpeculativeMargin(shape)) }; diff --git a/BepuPhysics/BodyProperties.cs b/BepuPhysics/BodyProperties.cs index 180ff6936..d6b3e0055 100644 --- a/BepuPhysics/BodyProperties.cs +++ b/BepuPhysics/BodyProperties.cs @@ -8,6 +8,78 @@ namespace BepuPhysics { + /// + /// Describes the pose and velocity of a body. + /// + public struct MotionState + { + /// + /// Constructs a motion state for a body. + /// + /// Pose of the body. + /// Velocity of the body. + public MotionState(in RigidPose pose, in BodyVelocity velocity) + { + Pose = pose; + Velocity = velocity; + } + + /// + /// Constructs a motion state for a body with zero velocity. + /// + /// Pose of the body. + public MotionState(in RigidPose pose) + { + Pose = pose; + Velocity = default; + } + + /// + /// Constructs a motion state for a body with zero velocity and identity orientation. + /// + /// Position of the body. + public MotionState(in Vector3 position) + { + Pose.Position = position; + Pose.Orientation = Quaternion.Identity; + Velocity = default; + } + + /// + /// Constructs a motion state for a body with zero angular velocity and identity orientation. + /// + /// Position of the body. + /// Linear velocity of the body. + public MotionState(in Vector3 position, in Vector3 linearVelocity) + { + Pose.Position = position; + Pose.Orientation = Quaternion.Identity; + Velocity.Linear = linearVelocity; + Velocity.Angular = default; + } + + /// + /// Constructs a motion state for a body with identity orientation. + /// + /// Position of the body. + /// Velocity of the body. + public MotionState(in Vector3 position, in BodyVelocity velocity) + { + Pose.Position = position; + Pose.Orientation = Quaternion.Identity; + Velocity = velocity; + } + + /// + /// Pose of the body. + /// + public RigidPose Pose; + /// + /// Linear and angular velocity of the body. + /// + public BodyVelocity Velocity; + } + //TODO: It's a little odd that this exists alongside the BepuUtilities.RigidTransform. The original reasoning was that rigid poses may end up having a non-FP32 representation. //We haven't taken advantage of that, so right now it's pretty much a pure duplicate. //When/if we take advantage of larger sizes, we'll have to closely analyze every use case of RigidPose to see if we need the higher precision or not. diff --git a/BepuPhysics/BodyReference.cs b/BepuPhysics/BodyReference.cs index 2cd84a271..98b0fd203 100644 --- a/BepuPhysics/BodyReference.cs +++ b/BepuPhysics/BodyReference.cs @@ -98,7 +98,7 @@ public ref BodyVelocity Velocity get { ref var location = ref MemoryLocation; - return ref Bodies.Sets[location.SetIndex].Velocities[location.Index]; + return ref Bodies.Sets[location.SetIndex].MotionStates[location.Index].Velocity; } } @@ -111,7 +111,7 @@ public ref RigidPose Pose get { ref var location = ref MemoryLocation; - return ref Bodies.Sets[location.SetIndex].Poses[location.Index]; + return ref Bodies.Sets[location.SetIndex].MotionStates[location.Index].Pose; } } @@ -220,7 +220,7 @@ public void ComputeInverseInertia(out Symmetric3x3 inverseInertia) ref var location = ref MemoryLocation; ref var set = ref Bodies.Sets[MemoryLocation.SetIndex]; ref var localInertia = ref set.LocalInertias[location.Index]; - ref var pose = ref set.Poses[location.Index]; + ref var pose = ref set.MotionStates[location.Index].Pose; PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, pose.Orientation, out inverseInertia); } @@ -342,9 +342,8 @@ public static void ApplyImpulse(in Vector3 impulse, in Vector3 impulseOffset, re public static void ApplyImpulse(in BodySet set, int index, in Vector3 impulse, in Vector3 impulseOffset) { ref var localInertia = ref set.LocalInertias[index]; - ref var pose = ref set.Poses[index]; - ref var velocity = ref set.Velocities[index]; - ApplyImpulse(impulse, impulseOffset, ref localInertia, ref pose, ref velocity); + ref var state = ref set.MotionStates[index]; + ApplyImpulse(impulse, impulseOffset, ref localInertia, ref state.Pose, ref state.Velocity); } /// @@ -394,7 +393,7 @@ public void ApplyLinearImpulse(in Vector3 impulse) { ref var location = ref MemoryLocation; ref var set = ref Bodies.Sets[location.SetIndex]; - ApplyLinearImpulse(impulse, set.LocalInertias[location.Index].InverseMass, ref set.Velocities[location.Index].Linear); + ApplyLinearImpulse(impulse, set.LocalInertias[location.Index].InverseMass, ref set.MotionStates[location.Index].Velocity.Linear); } /// @@ -418,9 +417,9 @@ public void ApplyAngularImpulse(in Vector3 angularImpulse) ref var location = ref MemoryLocation; ref var set = ref Bodies.Sets[location.SetIndex]; ref var localInertia = ref set.LocalInertias[location.Index]; - ref var pose = ref set.Poses[location.Index]; - PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, pose.Orientation, out var inverseInertia); - ApplyAngularImpulse(angularImpulse, inverseInertia, ref set.Velocities[location.Index].Angular); + ref var state = ref set.MotionStates[location.Index]; + PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, state.Pose.Orientation, out var inverseInertia); + ApplyAngularImpulse(angularImpulse, inverseInertia, ref state.Velocity.Angular); } } } diff --git a/BepuPhysics/BodySet.cs b/BepuPhysics/BodySet.cs index 789096f4e..281bff34e 100644 --- a/BepuPhysics/BodySet.cs +++ b/BepuPhysics/BodySet.cs @@ -35,8 +35,7 @@ public struct BodySet /// public Buffer IndexToHandle; - public Buffer Poses; - public Buffer Velocities; + public Buffer MotionStates; public Buffer LocalInertias; /// @@ -89,8 +88,7 @@ internal bool RemoveAt(int bodyIndex, out BodyHandle handle, out int movedBodyIn { movedBodyIndex = Count; //Copy the memory state of the last element down. - Poses[bodyIndex] = Poses[movedBodyIndex]; - Velocities[bodyIndex] = Velocities[movedBodyIndex]; + MotionStates[bodyIndex] = MotionStates[movedBodyIndex]; LocalInertias[bodyIndex] = LocalInertias[movedBodyIndex]; Activity[bodyIndex] = Activity[movedBodyIndex]; Collidables[bodyIndex] = Collidables[movedBodyIndex]; @@ -113,10 +111,10 @@ internal bool RemoveAt(int bodyIndex, out BodyHandle handle, out int movedBodyIn internal void ApplyDescriptionByIndex(int index, in BodyDescription description) { - Debug.Assert(!MathChecker.IsInvalid(description.Pose.Position.LengthSquared()), $"Invalid body position: {description.Pose.Position}"); - Debug.Assert(Math.Abs(1 - description.Pose.Orientation.LengthSquared()) < 1e-3f, $"Body orientation not unit length: {description.Pose.Orientation}"); - Debug.Assert(!MathChecker.IsInvalid(description.Velocity.Linear.LengthSquared()), $"Invalid body linear velocity: {description.Velocity.Linear}"); - Debug.Assert(!MathChecker.IsInvalid(description.Velocity.Angular.LengthSquared()), $"Invalid body angular velocity: {description.Velocity.Angular}"); + Debug.Assert(!MathChecker.IsInvalid(description.MotionState.Pose.Position.LengthSquared()), $"Invalid body position: {description.MotionState.Pose.Position}"); + Debug.Assert(Math.Abs(1 - description.MotionState.Pose.Orientation.LengthSquared()) < 1e-3f, $"Body orientation not unit length: {description.MotionState.Pose.Orientation}"); + Debug.Assert(!MathChecker.IsInvalid(description.MotionState.Velocity.Linear.LengthSquared()), $"Invalid body linear velocity: {description.MotionState.Velocity.Linear}"); + Debug.Assert(!MathChecker.IsInvalid(description.MotionState.Velocity.Angular.LengthSquared()), $"Invalid body angular velocity: {description.MotionState.Velocity.Angular}"); Debug.Assert(!MathChecker.IsInvalid( description.LocalInertia.InverseInertiaTensor.XX * description.LocalInertia.InverseInertiaTensor.XX + description.LocalInertia.InverseInertiaTensor.YX * description.LocalInertia.InverseInertiaTensor.YX + @@ -126,8 +124,7 @@ internal void ApplyDescriptionByIndex(int index, in BodyDescription description) description.LocalInertia.InverseInertiaTensor.ZZ * description.LocalInertia.InverseInertiaTensor.ZZ), $"Invalid body inverse inertia tensor: {description.LocalInertia.InverseInertiaTensor}"); Debug.Assert(!MathChecker.IsInvalid(description.LocalInertia.InverseMass) && description.LocalInertia.InverseMass >= 0, $"Invalid body inverse mass: {description.LocalInertia.InverseMass}"); - Poses[index] = description.Pose; - Velocities[index] = description.Velocity; + MotionStates[index] = description.MotionState; LocalInertias[index] = description.LocalInertia; ref var collidable = ref Collidables[index]; collidable.Continuity = description.Collidable.Continuity; @@ -144,8 +141,7 @@ internal void ApplyDescriptionByIndex(int index, in BodyDescription description) public void GetDescription(int index, out BodyDescription description) { - description.Pose = Poses[index]; - description.Velocity = Velocities[index]; + description.MotionState = MotionStates[index]; description.LocalInertia = LocalInertias[index]; ref var collidable = ref Collidables[index]; description.Collidable.Continuity = collidable.Continuity; @@ -223,8 +219,7 @@ internal void Swap(int slotA, int slotB, ref Buffer handleTo handleToIndex[IndexToHandle[slotB].Value].Index = slotA; Helpers.Swap(ref IndexToHandle[slotA], ref IndexToHandle[slotB]); Helpers.Swap(ref Collidables[slotA], ref Collidables[slotB]); - Helpers.Swap(ref Poses[slotA], ref Poses[slotB]); - Helpers.Swap(ref Velocities[slotA], ref Velocities[slotB]); + Helpers.Swap(ref MotionStates[slotA], ref MotionStates[slotB]); Helpers.Swap(ref LocalInertias[slotA], ref LocalInertias[slotB]); Helpers.Swap(ref Activity[slotA], ref Activity[slotB]); Helpers.Swap(ref Constraints[slotA], ref Constraints[slotB]); @@ -236,9 +231,8 @@ internal unsafe void InternalResize(int targetBodyCapacity, BufferPool pool) //Note that we base the bundle capacities on post-resize capacity of the IndexToHandle array. This simplifies the conditions on allocation, but increases memory use. //You may want to change this in the future if memory use is concerning. targetBodyCapacity = BufferPool.GetCapacityForCount(targetBodyCapacity); - Debug.Assert(Poses.Length != BufferPool.GetCapacityForCount(targetBodyCapacity), "Should not try to use internal resize of the result won't change the size."); - pool.ResizeToAtLeast(ref Poses, targetBodyCapacity, Count); - pool.ResizeToAtLeast(ref Velocities, targetBodyCapacity, Count); + Debug.Assert(MotionStates.Length != BufferPool.GetCapacityForCount(targetBodyCapacity), "Should not try to use internal resize of the result won't change the size."); + pool.ResizeToAtLeast(ref MotionStates, targetBodyCapacity, Count); pool.ResizeToAtLeast(ref LocalInertias, targetBodyCapacity, Count); pool.ResizeToAtLeast(ref IndexToHandle, targetBodyCapacity, Count); pool.ResizeToAtLeast(ref Collidables, targetBodyCapacity, Count); @@ -261,8 +255,7 @@ public unsafe void Clear(BufferPool pool) /// Pool to return the set's top level buffers to. public void DisposeBuffers(BufferPool pool) { - pool.Return(ref Poses); - pool.Return(ref Velocities); + pool.Return(ref MotionStates); pool.Return(ref LocalInertias); pool.Return(ref IndexToHandle); pool.Return(ref Collidables); diff --git a/BepuPhysics/CollisionDetection/NarrowPhase.cs b/BepuPhysics/CollisionDetection/NarrowPhase.cs index 93f279a02..9c5da2a9d 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhase.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhase.cs @@ -416,10 +416,12 @@ public unsafe void HandleOverlap(int workerIndex, CollidableReference a, Collida Debug.Assert(bodyLocationA.SetIndex == 0 || bodyLocationB.SetIndex == 0, "One of the two bodies must be active. Otherwise, something is busted!"); ref var setA = ref Bodies.Sets[bodyLocationA.SetIndex]; ref var setB = ref Bodies.Sets[bodyLocationB.SetIndex]; + ref var stateA = ref setA.MotionStates[bodyLocationA.Index]; + ref var stateB = ref setB.MotionStates[bodyLocationB.Index]; AddBatchEntries(workerIndex, ref overlapWorker, ref pair, ref setA.Collidables[bodyLocationA.Index], ref setB.Collidables[bodyLocationB.Index], - ref setA.Poses[bodyLocationA.Index], ref setB.Poses[bodyLocationB.Index], - ref setA.Velocities[bodyLocationA.Index], ref setB.Velocities[bodyLocationB.Index]); + ref stateA.Pose, ref stateB.Pose, + ref stateA.Velocity, ref stateB.Velocity); } else { @@ -434,10 +436,11 @@ public unsafe void HandleOverlap(int workerIndex, CollidableReference a, Collida //TODO: Ideally, the compiler would see this and optimize away the relevant math in AddBatchEntries. That's a longshot, though. May want to abuse some generics to force it. var zeroVelocity = default(BodyVelocity); ref var bodySet = ref Bodies.ActiveSet; + ref var bodyState = ref bodySet.MotionStates[bodyLocation.Index]; AddBatchEntries(workerIndex, ref overlapWorker, ref pair, ref bodySet.Collidables[bodyLocation.Index], ref Statics.Collidables[staticIndex], - ref bodySet.Poses[bodyLocation.Index], ref Statics.Poses[staticIndex], - ref bodySet.Velocities[bodyLocation.Index], ref zeroVelocity); + ref bodyState.Pose, ref Statics.Poses[staticIndex], + ref bodyState.Velocity, ref zeroVelocity); } } diff --git a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs index 12a32528e..a527078a8 100644 --- a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs @@ -109,7 +109,7 @@ public unsafe override void Prestep(ref TypeBatch typeBatch, Bodies bodies, floa } } - public unsafe override void WarmStart(ref TypeBatch typeBatch, ref Buffer bodyVelocities, int startBundle, int exclusiveEndBundle) + public unsafe override void WarmStart(ref TypeBatch typeBatch, Bodies bodies, int startBundle, int exclusiveEndBundle) { ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); @@ -128,7 +128,7 @@ public unsafe override void WarmStart(ref TypeBatch typeBatch, ref Buffer bodyVelocities, int startBundle, int exclusiveEndBundle) + public unsafe override void SolveIteration(ref TypeBatch typeBatch, Bodies bodies, int startBundle, int exclusiveEndBundle) { ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); @@ -172,7 +172,7 @@ public unsafe override void JacobiPrestep(ref TypeBatch typeBatch, Bodies bodies function.Prestep(bodies, ref references, count, dt, inverseDt, ref inertiaA, ref inertiaB, ref inertiaC, ref inertiaD, ref prestep, out projection); } } - public unsafe override void JacobiWarmStart(ref TypeBatch typeBatch, ref Buffer bodyVelocities, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle) + public unsafe override void JacobiWarmStart(ref TypeBatch typeBatch, Bodies bodies, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle) { ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); @@ -196,7 +196,7 @@ public unsafe override void JacobiWarmStart(ref TypeBatch typeBatch, ref Buffer< function.WarmStart(ref wsvA, ref wsvB, ref wsvC, ref wsvD, ref projection, ref accumulatedImpulses); } } - public unsafe override void JacobiSolveIteration(ref TypeBatch typeBatch, ref Buffer bodyVelocities, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle) + public unsafe override void JacobiSolveIteration(ref TypeBatch typeBatch, Bodies bodies, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle) { ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); diff --git a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs index be8be221b..01c563919 100644 --- a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs @@ -101,12 +101,12 @@ public unsafe override void Prestep(ref TypeBatch typeBatch, Bodies bodies, floa ref var projection = ref Unsafe.Add(ref projectionBase, i); ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); var count = GetCountInBundle(ref typeBatch, i); - bodies.GatherInertia(ref references, count, out var inertiaA); - function.Prestep(bodies, ref references, count, dt, inverseDt, ref inertiaA, ref prestep, out projection); + bodies.GatherState(ref references, count, out var position, out var orientation, out _, out var inertia); + function.Prestep(position, orientation, ref references, count, dt, inverseDt, ref inertia, ref prestep, out projection); } } - public unsafe override void WarmStart(ref TypeBatch typeBatch, ref Buffer bodyVelocities, int startBundle, int exclusiveEndBundle) + public unsafe override void WarmStart(ref TypeBatch typeBatch, Bodies bodies, int startBundle, int exclusiveEndBundle) { ref var bodyReferencesBase = ref Unsafe.AsRef>(typeBatch.BodyReferences.Memory); ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); @@ -118,13 +118,13 @@ public unsafe override void WarmStart(ref TypeBatch typeBatch, ref Buffer bodyVelocities, int startBundle, int exclusiveEndBundle) + public unsafe override void SolveIteration(ref TypeBatch typeBatch, Bodies bodies, int startBundle, int exclusiveEndBundle) { ref var bodyReferencesBase = ref Unsafe.AsRef>(typeBatch.BodyReferences.Memory); ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); @@ -136,9 +136,9 @@ public unsafe override void SolveIteration(ref TypeBatch typeBatch, ref Buffer bodyVelocities, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle) + public unsafe override void JacobiWarmStart(ref TypeBatch typeBatch, Bodies bodies, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle) { ref var bodyReferencesBase = ref Unsafe.AsRef>(typeBatch.BodyReferences.Memory); ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); @@ -180,7 +180,7 @@ public unsafe override void JacobiWarmStart(ref TypeBatch typeBatch, ref Buffer< function.WarmStart(ref wsvA, ref projection, ref accumulatedImpulses); } } - public unsafe override void JacobiSolveIteration(ref TypeBatch typeBatch, ref Buffer bodyVelocities, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle) + public unsafe override void JacobiSolveIteration(ref TypeBatch typeBatch, Bodies bodies, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle) { ref var bodyReferencesBase = ref Unsafe.AsRef>(typeBatch.BodyReferences.Memory); ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); diff --git a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs index 18291e0df..41af32b98 100644 --- a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs @@ -107,7 +107,7 @@ public unsafe override void Prestep(ref TypeBatch typeBatch, Bodies bodies, floa } } - public unsafe override void WarmStart(ref TypeBatch typeBatch, ref Buffer bodyVelocities, int startBundle, int exclusiveEndBundle) + public unsafe override void WarmStart(ref TypeBatch typeBatch, Bodies bodies, int startBundle, int exclusiveEndBundle) { ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); @@ -126,7 +126,7 @@ public unsafe override void WarmStart(ref TypeBatch typeBatch, ref Buffer bodyVelocities, int startBundle, int exclusiveEndBundle) + public unsafe override void SolveIteration(ref TypeBatch typeBatch, Bodies bodies, int startBundle, int exclusiveEndBundle) { ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); @@ -168,7 +168,7 @@ public unsafe override void JacobiPrestep(ref TypeBatch typeBatch, Bodies bodies function.Prestep(bodies, ref references, count, dt, inverseDt, ref inertiaA, ref inertiaB, ref inertiaC, ref prestep, out projection); } } - public unsafe override void JacobiWarmStart(ref TypeBatch typeBatch, ref Buffer bodyVelocities, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle) + public unsafe override void JacobiWarmStart(ref TypeBatch typeBatch, Bodies bodies, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle) { ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); @@ -190,7 +190,7 @@ public unsafe override void JacobiWarmStart(ref TypeBatch typeBatch, ref Buffer< function.WarmStart(ref wsvA, ref wsvB, ref wsvC, ref projection, ref accumulatedImpulses); } } - public unsafe override void JacobiSolveIteration(ref TypeBatch typeBatch, ref Buffer bodyVelocities, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle) + public unsafe override void JacobiSolveIteration(ref TypeBatch typeBatch, Bodies bodies, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle) { ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 624f5a67a..826e29edc 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -143,7 +143,7 @@ public unsafe override void Prestep(ref TypeBatch typeBatch, Bodies bodies, floa } } - public unsafe override void WarmStart(ref TypeBatch typeBatch, ref Buffer bodyVelocities, int startBundle, int exclusiveEndBundle) + public unsafe override void WarmStart(ref TypeBatch typeBatch, Bodies bodies, int startBundle, int exclusiveEndBundle) { ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); @@ -162,7 +162,7 @@ public unsafe override void WarmStart(ref TypeBatch typeBatch, ref Buffer bodyVelocities, int startBundle, int exclusiveEndBundle) + public unsafe override void SolveIteration(ref TypeBatch typeBatch, Bodies bodies, int startBundle, int exclusiveEndBundle) { ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); @@ -202,7 +202,7 @@ public unsafe override void JacobiPrestep(ref TypeBatch typeBatch, Bodies bodies function.Prestep(bodies, ref references, count, dt, inverseDt, ref inertiaA, ref inertiaB, ref prestep, out projection); } } - public unsafe override void JacobiWarmStart(ref TypeBatch typeBatch, ref Buffer bodyVelocities, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle) + public unsafe override void JacobiWarmStart(ref TypeBatch typeBatch, Bodies bodies, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle) { ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); @@ -222,7 +222,7 @@ public unsafe override void JacobiWarmStart(ref TypeBatch typeBatch, ref Buffer< function.WarmStart(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); } } - public unsafe override void JacobiSolveIteration(ref TypeBatch typeBatch, ref Buffer bodyVelocities, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle) + public unsafe override void JacobiSolveIteration(ref TypeBatch typeBatch, Bodies bodies, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle) { ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); @@ -273,7 +273,7 @@ public unsafe override void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodi ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); - ref var bodyVelocities = ref bodies.ActiveSet.Velocities; + ref var motionStates = ref bodies.ActiveSet.MotionStates; var function = default(TConstraintFunctions); ref var jacobiResultsBundlesA = ref jacobiResults.GetVelocitiesForBody(0); ref var jacobiResultsBundlesB = ref jacobiResults.GetVelocitiesForBody(1); @@ -294,7 +294,7 @@ public unsafe override void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodi function.Prestep(bodies, ref references, count, dt, inverseDt, ref inertiaA, ref inertiaB, ref prestep, out var projection); ref var wsvA = ref jacobiResultsBundlesA[i]; ref var wsvB = ref jacobiResultsBundlesB[i]; - Bodies.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out wsvA, out wsvB); + Bodies.GatherVelocities(ref motionStates, ref bodyReferences, count, out wsvA, out wsvB); function.WarmStart(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); function.Solve(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); } @@ -305,7 +305,7 @@ public unsafe override void WarmStart2(ref TypeBatch typeBatch, Bodies bodies, f ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); - ref var bodyVelocities = ref bodies.ActiveSet.Velocities; + ref var states = ref bodies.ActiveSet.MotionStates; var function = default(TConstraintFunctions); for (int i = startBundle; i < exclusiveEndBundle; ++i) { @@ -315,10 +315,10 @@ public unsafe override void WarmStart2(ref TypeBatch typeBatch, Bodies bodies, f ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); var count = GetCountInBundle(ref typeBatch, i); bodies.GatherInertia(ref references, count, out var inertiaA, out var inertiaB); - Bodies.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out var wsvA, out var wsvB); + Bodies.GatherVelocities(ref states, ref bodyReferences, count, out var wsvA, out var wsvB); function.Prestep(bodies, ref references, count, dt, inverseDt, ref inertiaA, ref inertiaB, ref prestep, out var projection); function.WarmStart(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); - Bodies.ScatterVelocities(ref wsvA, ref wsvB, ref bodyVelocities, ref bodyReferences, count); + Bodies.ScatterVelocities(ref wsvA, ref wsvB, ref states, ref bodyReferences, count); } } public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index fe1aaa0ef..2e815db32 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -125,12 +125,12 @@ internal unsafe abstract void CopySleepingToActive( public abstract void Resize(ref TypeBatch typeBatch, int newCapacity, BufferPool pool); public abstract void Prestep(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); - public abstract void WarmStart(ref TypeBatch typeBatch, ref Buffer bodyVelocities, int startBundle, int exclusiveEndBundle); - public abstract void SolveIteration(ref TypeBatch typeBatch, ref Buffer bodyVelocities, int startBundle, int exclusiveEndBundle); + public abstract void WarmStart(ref TypeBatch typeBatch, Bodies bodies, int startBundle, int exclusiveEndBundle); + public abstract void SolveIteration(ref TypeBatch typeBatch, Bodies bodies, int startBundle, int exclusiveEndBundle); public abstract void JacobiPrestep(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); - public abstract void JacobiWarmStart(ref TypeBatch typeBatch, ref Buffer bodyVelocities, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle); - public abstract void JacobiSolveIteration(ref TypeBatch typeBatch, ref Buffer bodyVelocities, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle); + public abstract void JacobiWarmStart(ref TypeBatch typeBatch, Bodies bodies, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle); + public abstract void JacobiSolveIteration(ref TypeBatch typeBatch, Bodies bodies, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle); public abstract void SolveStep(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); public abstract void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); diff --git a/BepuPhysics/FallbackBatch.cs b/BepuPhysics/FallbackBatch.cs index 03ccb557b..fbcdf39e5 100644 --- a/BepuPhysics/FallbackBatch.cs +++ b/BepuPhysics/FallbackBatch.cs @@ -443,7 +443,7 @@ public void ScatterVelocities(Bodies bodies, Solver solver, ref BufferPose; shape = set.Collidables[location.Index].Shape; } } diff --git a/BepuPhysics/Solver_Solve.cs b/BepuPhysics/Solver_Solve.cs index f4f10ae7d..d2c421794 100644 --- a/BepuPhysics/Solver_Solve.cs +++ b/BepuPhysics/Solver_Solve.cs @@ -355,7 +355,7 @@ public void Execute(Solver solver, int blockIndex) ref var block = ref solver.context.ConstraintBlocks.Blocks[blockIndex]; ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; - typeProcessor.WarmStart(ref typeBatch, ref solver.bodies.ActiveSet.Velocities, block.StartBundle, block.End); + typeProcessor.WarmStart(ref typeBatch, solver.bodies, block.StartBundle, block.End); } } @@ -367,7 +367,7 @@ public void Execute(Solver solver, int blockIndex) ref var block = ref solver.context.ConstraintBlocks.Blocks[blockIndex]; ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; - typeProcessor.SolveIteration(ref typeBatch, ref solver.bodies.ActiveSet.Velocities, block.StartBundle, block.End); + typeProcessor.SolveIteration(ref typeBatch, solver.bodies, block.StartBundle, block.End); } } @@ -379,7 +379,7 @@ public void Execute(Solver solver, int blockIndex) ref var block = ref solver.context.ConstraintBlocks.Blocks[blockIndex]; ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; - typeProcessor.JacobiWarmStart(ref typeBatch, ref solver.bodies.ActiveSet.Velocities, ref solver.context.FallbackResults[block.TypeBatchIndex], block.StartBundle, block.End); + typeProcessor.JacobiWarmStart(ref typeBatch, solver.bodies, ref solver.context.FallbackResults[block.TypeBatchIndex], block.StartBundle, block.End); } } @@ -391,7 +391,7 @@ public void Execute(Solver solver, int blockIndex) ref var block = ref solver.context.ConstraintBlocks.Blocks[blockIndex]; ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; - typeProcessor.JacobiSolveIteration(ref typeBatch, ref solver.bodies.ActiveSet.Velocities, ref solver.context.FallbackResults[block.TypeBatchIndex], block.StartBundle, block.End); + typeProcessor.JacobiSolveIteration(ref typeBatch, solver.bodies, ref solver.context.FallbackResults[block.TypeBatchIndex], block.StartBundle, block.End); } } @@ -795,7 +795,7 @@ public void Solve(float dt, IThreadDispatcher threadDispatcher = null) for (int j = 0; j < batch.TypeBatches.Count; ++j) { ref var typeBatch = ref batch.TypeBatches[j]; - TypeProcessors[typeBatch.TypeId].WarmStart(ref typeBatch, ref bodies.ActiveSet.Velocities, 0, typeBatch.BundleCount); + TypeProcessors[typeBatch.TypeId].WarmStart(ref typeBatch, bodies, 0, typeBatch.BundleCount); } } Buffer fallbackResults = default; @@ -806,7 +806,7 @@ public void Solve(float dt, IThreadDispatcher threadDispatcher = null) for (int j = 0; j < batch.TypeBatches.Count; ++j) { ref var typeBatch = ref batch.TypeBatches[j]; - TypeProcessors[typeBatch.TypeId].JacobiWarmStart(ref typeBatch, ref bodies.ActiveSet.Velocities, ref fallbackResults[j], 0, typeBatch.BundleCount); + TypeProcessors[typeBatch.TypeId].JacobiWarmStart(ref typeBatch, bodies, ref fallbackResults[j], 0, typeBatch.BundleCount); } activeSet.Fallback.ScatterVelocities(bodies, this, ref fallbackResults, 0, activeSet.Fallback.BodyCount); } @@ -818,7 +818,7 @@ public void Solve(float dt, IThreadDispatcher threadDispatcher = null) for (int j = 0; j < batch.TypeBatches.Count; ++j) { ref var typeBatch = ref batch.TypeBatches[j]; - TypeProcessors[typeBatch.TypeId].SolveIteration(ref typeBatch, ref bodies.ActiveSet.Velocities, 0, typeBatch.BundleCount); + TypeProcessors[typeBatch.TypeId].SolveIteration(ref typeBatch, bodies, 0, typeBatch.BundleCount); } } if (fallbackExists) @@ -827,7 +827,7 @@ public void Solve(float dt, IThreadDispatcher threadDispatcher = null) for (int j = 0; j < batch.TypeBatches.Count; ++j) { ref var typeBatch = ref batch.TypeBatches[j]; - TypeProcessors[typeBatch.TypeId].JacobiSolveIteration(ref typeBatch, ref bodies.ActiveSet.Velocities, ref fallbackResults[j], 0, typeBatch.BundleCount); + TypeProcessors[typeBatch.TypeId].JacobiSolveIteration(ref typeBatch, bodies, ref fallbackResults[j], 0, typeBatch.BundleCount); } activeSet.Fallback.ScatterVelocities(bodies, this, ref fallbackResults, 0, activeSet.Fallback.BodyCount); } diff --git a/BepuPhysics/Solver_Solve2.cs b/BepuPhysics/Solver_Solve2.cs index d577d1c04..7ba9ceaa4 100644 --- a/BepuPhysics/Solver_Solve2.cs +++ b/BepuPhysics/Solver_Solve2.cs @@ -166,7 +166,7 @@ public void Solve2(float dt, IThreadDispatcher threadDispatcher = null) for (int j = 0; j < batch.TypeBatches.Count; ++j) { ref var typeBatch = ref batch.TypeBatches[j]; - TypeProcessors[typeBatch.TypeId].SolveIteration(ref typeBatch, ref bodies.ActiveSet.Velocities, 0, typeBatch.BundleCount); + TypeProcessors[typeBatch.TypeId].SolveIteration(ref typeBatch, bodies, 0, typeBatch.BundleCount); } } if (fallbackExists) @@ -175,7 +175,7 @@ public void Solve2(float dt, IThreadDispatcher threadDispatcher = null) for (int j = 0; j < batch.TypeBatches.Count; ++j) { ref var typeBatch = ref batch.TypeBatches[j]; - TypeProcessors[typeBatch.TypeId].JacobiSolveIteration(ref typeBatch, ref bodies.ActiveSet.Velocities, ref fallbackResults[j], 0, typeBatch.BundleCount); + TypeProcessors[typeBatch.TypeId].JacobiSolveIteration(ref typeBatch, bodies, ref fallbackResults[j], 0, typeBatch.BundleCount); } activeSet.Fallback.ScatterVelocities(bodies, this, ref fallbackResults, 0, activeSet.Fallback.BodyCount); } From 05f9fdcbfc98e7c1e5cf327afbb65e975b48c770 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 29 Jun 2021 14:03:43 -0500 Subject: [PATCH 044/947] GatherState pushed through type processors. --- BepuPhysics/Bodies.cs | 2 +- .../Constraints/FourBodyTypeProcessor.cs | 82 +++++++++++++------ .../Constraints/OneBodyTypeProcessor.cs | 57 ++++++------- .../Constraints/ThreeBodyTypeProcessor.cs | 70 ++++++++++------ .../Constraints/TwoBodyTypeProcessor.cs | 70 +++++++--------- BepuPhysics/Constraints/Weld.cs | 5 +- 6 files changed, 158 insertions(+), 128 deletions(-) diff --git a/BepuPhysics/Bodies.cs b/BepuPhysics/Bodies.cs index af9ce63dd..938b5627c 100644 --- a/BepuPhysics/Bodies.cs +++ b/BepuPhysics/Bodies.cs @@ -652,7 +652,7 @@ public void GatherState(ref TwoBodyReferences references, int count, public void GatherState(ref ThreeBodyReferences references, int count, out QuaternionWide orientationA, out BodyVelocities velocityA, out BodyInertias inertiaA, out Vector3Wide ab, - out QuaternionWide orientationB, out BodyVelocities velocityB, out BodyInertias inertiaB + out QuaternionWide orientationB, out BodyVelocities velocityB, out BodyInertias inertiaB, out Vector3Wide ac, out QuaternionWide orientationC, out BodyVelocities velocityC, out BodyInertias inertiaC) { diff --git a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs index a527078a8..d7fd88198 100644 --- a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs @@ -27,8 +27,12 @@ public struct FourBodyReferences /// Type of the projection to input. public interface IFourBodyConstraintFunctions { - void Prestep(Bodies bodies, ref FourBodyReferences bodyReferences, int count, float dt, float inverseDt, - ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref BodyInertias inertiaC, ref BodyInertias inertiaD, ref TPrestepData prestepData, out TProjection projection); + void Prestep( + in QuaternionWide orientationA, in BodyInertias inertiaA, + in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + in Vector3Wide ac, in QuaternionWide orientationC, in BodyInertias inertiaC, + in Vector3Wide ad, in QuaternionWide orientationD, in BodyInertias inertiaD, + float dt, float inverseDt, ref TPrestepData prestepData, out TProjection projection); void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref BodyVelocities velocityC, ref BodyVelocities velocityD, ref TProjection projection, ref TAccumulatedImpulse accumulatedImpulse); void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref BodyVelocities velocityC, ref BodyVelocities velocityD, ref TProjection projection, ref TAccumulatedImpulse accumulatedImpulse); } @@ -104,8 +108,12 @@ public unsafe override void Prestep(ref TypeBatch typeBatch, Bodies bodies, floa ref var projection = ref Unsafe.Add(ref projectionBase, i); ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); var count = GetCountInBundle(ref typeBatch, i); - bodies.GatherInertia(ref references, count, out var inertiaA, out var inertiaB, out var inertiaC, out var inertiaD); - function.Prestep(bodies, ref references, count, dt, inverseDt, ref inertiaA, ref inertiaB, ref inertiaC, ref inertiaD, ref prestep, out projection); + bodies.GatherState(ref references, count, + out var orientationA, out var wsvA, out var inertiaA, + out var ab, out var orientationB, out var wsvB, out var inertiaB, + out var ac, out var orientationC, out var wsvC, out var inertiaC, + out var ad, out var orientationD, out var wsvD, out var inertiaD); + function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, ac, orientationC, inertiaC, ad, orientationD, inertiaD, dt, inverseDt, ref prestep, out projection); } } @@ -121,9 +129,13 @@ public unsafe override void WarmStart(ref TypeBatch typeBatch, Bodies bodies, in ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); int count = GetCountInBundle(ref typeBatch, i); - Bodies.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out var wsvA, out var wsvB, out var wsvC, out var wsvD); + bodies.GatherState(ref bodyReferences, count, + out var orientationA, out var wsvA, out var inertiaA, + out var ab, out var orientationB, out var wsvB, out var inertiaB, + out var ac, out var orientationC, out var wsvC, out var inertiaC, + out var ad, out var orientationD, out var wsvD, out var inertiaD); function.WarmStart(ref wsvA, ref wsvB, ref wsvC, ref wsvD, ref projection, ref accumulatedImpulses); - Bodies.ScatterVelocities(ref wsvA, ref wsvB, ref wsvC, ref wsvD, ref bodyVelocities, ref bodyReferences, count); + bodies.ScatterVelocities(ref wsvA, ref wsvB, ref wsvC, ref wsvD, ref bodyReferences, count); } } @@ -140,9 +152,13 @@ public unsafe override void SolveIteration(ref TypeBatch typeBatch, Bodies bodie ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); int count = GetCountInBundle(ref typeBatch, i); - Bodies.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out var wsvA, out var wsvB, out var wsvC, out var wsvD); + bodies.GatherState(ref bodyReferences, count, + out var orientationA, out var wsvA, out var inertiaA, + out var ab, out var orientationB, out var wsvB, out var inertiaB, + out var ac, out var orientationC, out var wsvC, out var inertiaC, + out var ad, out var orientationD, out var wsvD, out var inertiaD); function.Solve(ref wsvA, ref wsvB, ref wsvC, ref wsvD, ref projection, ref accumulatedImpulses); - Bodies.ScatterVelocities(ref wsvA, ref wsvB, ref wsvC, ref wsvD, ref bodyVelocities, ref bodyReferences, count); + bodies.ScatterVelocities(ref wsvA, ref wsvB, ref wsvC, ref wsvD, ref bodyReferences, count); } } @@ -158,7 +174,11 @@ public unsafe override void JacobiPrestep(ref TypeBatch typeBatch, Bodies bodies ref var projection = ref Unsafe.Add(ref projectionBase, i); ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); var count = GetCountInBundle(ref typeBatch, i); - bodies.GatherInertia(ref references, count, out var inertiaA, out var inertiaB, out var inertiaC, out var inertiaD); + bodies.GatherState(ref references, count, + out var orientationA, out var wsvA, out var inertiaA, + out var ab, out var orientationB, out var wsvB, out var inertiaB, + out var ac, out var orientationC, out var wsvC, out var inertiaC, + out var ad, out var orientationD, out var wsvD, out var inertiaD); //Jacobi batches split affected bodies into multiple pieces to guarantee convergence. jacobiBatch.GetJacobiScaleForBodies(ref references, count, out var jacobiScaleA, out var jacobiScaleB, out var jacobiScaleC, out var jacobiScaleD); Symmetric3x3Wide.Scale(inertiaA.InverseInertiaTensor, jacobiScaleA, out inertiaA.InverseInertiaTensor); @@ -169,7 +189,7 @@ public unsafe override void JacobiPrestep(ref TypeBatch typeBatch, Bodies bodies inertiaC.InverseMass *= jacobiScaleC; Symmetric3x3Wide.Scale(inertiaD.InverseInertiaTensor, jacobiScaleD, out inertiaD.InverseInertiaTensor); inertiaD.InverseMass *= jacobiScaleD; - function.Prestep(bodies, ref references, count, dt, inverseDt, ref inertiaA, ref inertiaB, ref inertiaC, ref inertiaD, ref prestep, out projection); + function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, ac, orientationC, inertiaC, ad, orientationD, inertiaD, dt, inverseDt, ref prestep, out projection); } } public unsafe override void JacobiWarmStart(ref TypeBatch typeBatch, Bodies bodies, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle) @@ -192,7 +212,11 @@ public unsafe override void JacobiWarmStart(ref TypeBatch typeBatch, Bodies bodi ref var wsvB = ref jacobiResultsBundlesB[i]; ref var wsvC = ref jacobiResultsBundlesC[i]; ref var wsvD = ref jacobiResultsBundlesD[i]; - Bodies.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out wsvA, out wsvB, out wsvC, out wsvD); + bodies.GatherState(ref bodyReferences, count, + out var orientationA, out wsvA, out var inertiaA, + out var ab, out var orientationB, out wsvB, out var inertiaB, + out var ac, out var orientationC, out wsvC, out var inertiaC, + out var ad, out var orientationD, out wsvD, out var inertiaD); function.WarmStart(ref wsvA, ref wsvB, ref wsvC, ref wsvD, ref projection, ref accumulatedImpulses); } } @@ -216,7 +240,11 @@ public unsafe override void JacobiSolveIteration(ref TypeBatch typeBatch, Bodies ref var wsvB = ref jacobiResultsBundlesB[i]; ref var wsvC = ref jacobiResultsBundlesC[i]; ref var wsvD = ref jacobiResultsBundlesD[i]; - Bodies.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out wsvA, out wsvB, out wsvC, out wsvD); + bodies.GatherState(ref bodyReferences, count, + out var orientationA, out wsvA, out var inertiaA, + out var ab, out var orientationB, out wsvB, out var inertiaB, + out var ac, out var orientationC, out wsvC, out var inertiaC, + out var ad, out var orientationD, out wsvC, out var inertiaD); function.Solve(ref wsvA, ref wsvB, ref wsvC, ref wsvD, ref projection, ref accumulatedImpulses); } } @@ -227,7 +255,6 @@ public unsafe override void SolveStep(ref TypeBatch typeBatch, Bodies bodies, fl ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); - ref var bodyVelocities = ref bodies.ActiveSet.Velocities; var function = default(TConstraintFunctions); for (int i = startBundle; i < exclusiveEndBundle; ++i) { @@ -236,12 +263,15 @@ public unsafe override void SolveStep(ref TypeBatch typeBatch, Bodies bodies, fl ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); var count = GetCountInBundle(ref typeBatch, i); - bodies.GatherInertia(ref references, count, out var inertiaA, out var inertiaB, out var inertiaC, out var inertiaD); - function.Prestep(bodies, ref references, count, dt, inverseDt, ref inertiaA, ref inertiaB, ref inertiaC, ref inertiaD, ref prestep, out var projection); - Bodies.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out var wsvA, out var wsvB, out var wsvC, out var wsvD); + bodies.GatherState(ref references, count, + out var orientationA, out var wsvA, out var inertiaA, + out var ab, out var orientationB, out var wsvB, out var inertiaB, + out var ac, out var orientationC, out var wsvC, out var inertiaC, + out var ad, out var orientationD, out var wsvD, out var inertiaD); + function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, ac, orientationC, inertiaC, ad, orientationD, inertiaD, dt, inverseDt, ref prestep, out var projection); function.WarmStart(ref wsvA, ref wsvB, ref wsvC, ref wsvD, ref projection, ref accumulatedImpulses); function.Solve(ref wsvA, ref wsvB, ref wsvC, ref wsvD, ref projection, ref accumulatedImpulses); - Bodies.ScatterVelocities(ref wsvA, ref wsvB, ref wsvC, ref wsvD, ref bodyVelocities, ref bodyReferences, count); + bodies.ScatterVelocities(ref wsvA, ref wsvB, ref wsvC, ref wsvD, ref bodyReferences, count); } } @@ -251,7 +281,6 @@ public unsafe override void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodi ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); - ref var bodyVelocities = ref bodies.ActiveSet.Velocities; var function = default(TConstraintFunctions); ref var jacobiResultsBundlesA = ref jacobiResults.GetVelocitiesForBody(0); ref var jacobiResultsBundlesB = ref jacobiResults.GetVelocitiesForBody(1); @@ -264,7 +293,15 @@ public unsafe override void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodi ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); var count = GetCountInBundle(ref typeBatch, i); - bodies.GatherInertia(ref references, count, out var inertiaA, out var inertiaB, out var inertiaC, out var inertiaD); + ref var wsvA = ref jacobiResultsBundlesA[i]; + ref var wsvB = ref jacobiResultsBundlesB[i]; + ref var wsvC = ref jacobiResultsBundlesC[i]; + ref var wsvD = ref jacobiResultsBundlesD[i]; + bodies.GatherState(ref references, count, + out var orientationA, out wsvA, out var inertiaA, + out var ab, out var orientationB, out wsvB, out var inertiaB, + out var ac, out var orientationC, out wsvC, out var inertiaC, + out var ad, out var orientationD, out wsvC, out var inertiaD); //Jacobi batches split affected bodies into multiple pieces to guarantee convergence. jacobiBatch.GetJacobiScaleForBodies(ref references, count, out var jacobiScaleA, out var jacobiScaleB, out var jacobiScaleC, out var jacobiScaleD); Symmetric3x3Wide.Scale(inertiaA.InverseInertiaTensor, jacobiScaleA, out inertiaA.InverseInertiaTensor); @@ -275,12 +312,7 @@ public unsafe override void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodi inertiaC.InverseMass *= jacobiScaleC; Symmetric3x3Wide.Scale(inertiaD.InverseInertiaTensor, jacobiScaleD, out inertiaD.InverseInertiaTensor); inertiaD.InverseMass *= jacobiScaleD; - function.Prestep(bodies, ref references, count, dt, inverseDt, ref inertiaA, ref inertiaB, ref inertiaC, ref inertiaD, ref prestep, out var projection); - ref var wsvA = ref jacobiResultsBundlesA[i]; - ref var wsvB = ref jacobiResultsBundlesB[i]; - ref var wsvC = ref jacobiResultsBundlesC[i]; - ref var wsvD = ref jacobiResultsBundlesD[i]; - Bodies.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out wsvA, out wsvB, out wsvC, out wsvD); + function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, ac, orientationC, inertiaC, ad, orientationD, inertiaD, dt, inverseDt, ref prestep, out var projection); function.WarmStart(ref wsvA, ref wsvB, ref wsvC, ref wsvD, ref projection, ref accumulatedImpulses); function.Solve(ref wsvA, ref wsvB, ref wsvC, ref wsvD, ref projection, ref accumulatedImpulses); } diff --git a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs index 01c563919..21b7d50d5 100644 --- a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs @@ -16,7 +16,7 @@ namespace BepuPhysics.Constraints /// Type of the projection to input. public interface IOneBodyConstraintFunctions { - void Prestep(Bodies bodies, ref Vector bodyReferences, int count, float dt, float inverseDt, ref BodyInertias inertia, ref TPrestepData prestepData, out TProjection projection); + void Prestep(in Vector3Wide position, in QuaternionWide orientation, in BodyInertias inertia, float dt, float inverseDt, ref TPrestepData prestepData, out TProjection projection); void WarmStart(ref BodyVelocities velocity, ref TProjection projection, ref TAccumulatedImpulse accumulatedImpulse); void Solve(ref BodyVelocities velocity, ref TProjection projection, ref TAccumulatedImpulse accumulatedImpulse); } @@ -102,7 +102,7 @@ public unsafe override void Prestep(ref TypeBatch typeBatch, Bodies bodies, floa ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); var count = GetCountInBundle(ref typeBatch, i); bodies.GatherState(ref references, count, out var position, out var orientation, out _, out var inertia); - function.Prestep(position, orientation, ref references, count, dt, inverseDt, ref inertia, ref prestep, out projection); + function.Prestep(position, orientation, inertia, dt, inverseDt, ref prestep, out projection); } } @@ -154,12 +154,12 @@ public unsafe override void JacobiPrestep(ref TypeBatch typeBatch, Bodies bodies ref var projection = ref Unsafe.Add(ref projectionBase, i); ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); var count = GetCountInBundle(ref typeBatch, i); - bodies.GatherInertia(ref references, count, out var inertia); + bodies.GatherState(ref references, count, out var position, out var orientation, out _, out var inertia); //Jacobi batches split affected bodies into multiple pieces to guarantee convergence. jacobiBatch.GetJacobiScaleForBodies(ref references, count, out var jacobiScale); Symmetric3x3Wide.Scale(inertia.InverseInertiaTensor, jacobiScale, out inertia.InverseInertiaTensor); inertia.InverseMass *= jacobiScale; - function.Prestep(bodies, ref references, count, dt, inverseDt, ref inertia, ref prestep, out projection); + function.Prestep(position, orientation, inertia, dt, inverseDt, ref prestep, out projection); } } public unsafe override void JacobiWarmStart(ref TypeBatch typeBatch, Bodies bodies, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle) @@ -176,7 +176,7 @@ public unsafe override void JacobiWarmStart(ref TypeBatch typeBatch, Bodies bodi ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); int count = GetCountInBundle(ref typeBatch, i); ref var wsvA = ref jacobiResultsBundlesA[i]; - Bodies.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out wsvA); + bodies.GatherState(ref bodyReferences, count, out _, out _, out wsvA, out _); function.WarmStart(ref wsvA, ref projection, ref accumulatedImpulses); } } @@ -194,7 +194,7 @@ public unsafe override void JacobiSolveIteration(ref TypeBatch typeBatch, Bodies ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); int count = GetCountInBundle(ref typeBatch, i); ref var wsvA = ref jacobiResultsBundlesA[i]; - Bodies.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out wsvA); + bodies.GatherState(ref bodyReferences, count, out _, out _, out wsvA, out _); function.Solve(ref wsvA, ref projection, ref accumulatedImpulses); } } @@ -205,7 +205,6 @@ public unsafe override void SolveStep(ref TypeBatch typeBatch, Bodies bodies, fl ref var bodyReferencesBase = ref Unsafe.AsRef>(typeBatch.BodyReferences.Memory); ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); - ref var bodyVelocities = ref bodies.ActiveSet.Velocities; var function = default(TConstraintFunctions); for (int i = startBundle; i < exclusiveEndBundle; ++i) { @@ -214,12 +213,11 @@ public unsafe override void SolveStep(ref TypeBatch typeBatch, Bodies bodies, fl ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); var count = GetCountInBundle(ref typeBatch, i); - bodies.GatherInertia(ref references, count, out var inertiaA); - function.Prestep(bodies, ref references, count, dt, inverseDt, ref inertiaA, ref prestep, out var projection); - Bodies.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out var wsvA); + bodies.GatherState(ref bodyReferences, count, out var position, out var orientation, out var wsvA, out var inertiaA); + function.Prestep(position, orientation, inertiaA, dt, inverseDt, ref prestep, out var projection); function.WarmStart(ref wsvA, ref projection, ref accumulatedImpulses); function.Solve(ref wsvA, ref projection, ref accumulatedImpulses); - Bodies.ScatterVelocities(ref wsvA, ref bodyVelocities, ref bodyReferences, count); + bodies.ScatterVelocities(ref wsvA, ref bodyReferences, count); } } @@ -229,7 +227,6 @@ public unsafe override void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodi ref var bodyReferencesBase = ref Unsafe.AsRef>(typeBatch.BodyReferences.Memory); ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); - ref var bodyVelocities = ref bodies.ActiveSet.Velocities; var function = default(TConstraintFunctions); ref var jacobiResultsBundlesA = ref jacobiResults.GetVelocitiesForBody(0); for (int i = startBundle; i < exclusiveEndBundle; ++i) @@ -239,14 +236,13 @@ public unsafe override void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodi ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); var count = GetCountInBundle(ref typeBatch, i); - bodies.GatherInertia(ref references, count, out var inertiaA); + ref var wsvA = ref jacobiResultsBundlesA[i]; + bodies.GatherState(ref bodyReferences, count, out var position, out var orientation, out wsvA, out var inertiaA); //Jacobi batches split affected bodies into multiple pieces to guarantee convergence. jacobiBatch.GetJacobiScaleForBodies(ref references, count, out var jacobiScale); Symmetric3x3Wide.Scale(inertiaA.InverseInertiaTensor, jacobiScale, out inertiaA.InverseInertiaTensor); inertiaA.InverseMass *= jacobiScale; - function.Prestep(bodies, ref references, count, dt, inverseDt, ref inertiaA, ref prestep, out var projection); - ref var wsvA = ref jacobiResultsBundlesA[i]; - Bodies.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out wsvA); + function.Prestep(position, orientation, inertiaA, dt, inverseDt, ref prestep, out var projection); function.WarmStart(ref wsvA, ref projection, ref accumulatedImpulses); function.Solve(ref wsvA, ref projection, ref accumulatedImpulses); } @@ -259,7 +255,6 @@ public unsafe override void WarmStart2(ref TypeBatch typeBatch, Bodies bodies, f ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); ref var bodyReferencesBase = ref Unsafe.AsRef>(typeBatch.BodyReferences.Memory); ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); - ref var bodyVelocities = ref bodies.ActiveSet.Velocities; var function = default(TConstraintFunctions); for (int i = startBundle; i < exclusiveEndBundle; ++i) { @@ -268,11 +263,10 @@ public unsafe override void WarmStart2(ref TypeBatch typeBatch, Bodies bodies, f ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); var count = GetCountInBundle(ref typeBatch, i); - bodies.GatherInertia(ref references, count, out var inertiaA); - Bodies.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out var wsvA); - function.Prestep(bodies, ref references, count, dt, inverseDt, ref inertiaA, ref prestep, out var projection); + bodies.GatherState(ref bodyReferences, count, out var position, out var orientation, out var wsvA, out var inertiaA); + function.Prestep(position, orientation, inertiaA, dt, inverseDt, ref prestep, out var projection); function.WarmStart(ref wsvA, ref projection, ref accumulatedImpulses); - Bodies.ScatterVelocities(ref wsvA, ref bodyVelocities, ref bodyReferences, count); + bodies.ScatterVelocities(ref wsvA, ref bodyReferences, count); } } public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) @@ -280,7 +274,6 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); ref var bodyReferencesBase = ref Unsafe.AsRef>(typeBatch.BodyReferences.Memory); ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); - ref var bodyVelocities = ref bodies.ActiveSet.Velocities; var function = default(TConstraintFunctions); for (int i = startBundle; i < exclusiveEndBundle; ++i) { @@ -289,11 +282,10 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); var count = GetCountInBundle(ref typeBatch, i); - bodies.GatherInertia(ref references, count, out var inertiaA); - Bodies.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out var wsvA); - function.Prestep(bodies, ref references, count, dt, inverseDt, ref inertiaA, ref prestep, out var projection); + bodies.GatherState(ref bodyReferences, count, out var position, out var orientation, out var wsvA, out var inertiaA); + function.Prestep(position, orientation, inertiaA, dt, inverseDt, ref prestep, out var projection); function.Solve(ref wsvA, ref projection, ref accumulatedImpulses); - Bodies.ScatterVelocities(ref wsvA, ref bodyVelocities, ref bodyReferences, count); + bodies.ScatterVelocities(ref wsvA, ref bodyReferences, count); } } @@ -303,7 +295,6 @@ public unsafe override void Prestep2(ref TypeBatch typeBatch, Bodies bodies, flo ref var bodyReferencesBase = ref Unsafe.AsRef>(typeBatch.BodyReferences.Memory); ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); - ref var bodyVelocities = ref bodies.ActiveSet.Velocities; var function = default(TConstraintFunctions); for (int i = startBundle; i < exclusiveEndBundle; ++i) { @@ -312,11 +303,10 @@ public unsafe override void Prestep2(ref TypeBatch typeBatch, Bodies bodies, flo ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); var count = GetCountInBundle(ref typeBatch, i); - bodies.GatherInertia(ref references, count, out var inertiaA); - Bodies.GatherVelocities(ref bodyVelocities, ref references, count, out var wsvA); - function.Prestep(bodies, ref references, count, dt, inverseDt, ref inertiaA, ref prestep, out projection); + bodies.GatherState(ref references, count, out var position, out var orientation, out var wsvA, out var inertiaA); + function.Prestep(position, orientation, inertiaA, dt, inverseDt, ref prestep, out projection); function.WarmStart(ref wsvA, ref projection, ref accumulatedImpulses); - Bodies.ScatterVelocities(ref wsvA, ref bodyVelocities, ref references, count); + bodies.ScatterVelocities(ref wsvA, ref references, count); } } public override void JacobiPrestep2(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) @@ -335,7 +325,6 @@ public unsafe override void IncrementallyUpdateContactData(ref TypeBatch typeBat { ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); ref var bodyReferencesBase = ref Unsafe.AsRef>(typeBatch.BodyReferences.Memory); - ref var bodyVelocities = ref bodies.ActiveSet.Velocities; var function = default(TConstraintFunctions); var dtWide = new Vector(dt); for (int i = startBundle; i < exclusiveEndBundle; ++i) @@ -343,8 +332,8 @@ public unsafe override void IncrementallyUpdateContactData(ref TypeBatch typeBat ref var prestep = ref Unsafe.Add(ref prestepBase, i); ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); var count = GetCountInBundle(ref typeBatch, i); - Bodies.GatherVelocities(ref bodyVelocities, ref references, count, out var velocityA); - function.IncrementallyUpdateContactData(dtWide, velocityA, ref prestep); + bodies.GatherState(ref references, count, out _, out _, out var wsvA, out _); + function.IncrementallyUpdateContactData(dtWide, wsvA, ref prestep); } } } diff --git a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs index 41af32b98..ac8de87c5 100644 --- a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs @@ -26,8 +26,8 @@ public struct ThreeBodyReferences /// Type of the projection to input. public interface IThreeBodyConstraintFunctions { - void Prestep(Bodies bodies, ref ThreeBodyReferences bodyReferences, int count, float dt, float inverseDt, - ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref BodyInertias inertiaC, ref TPrestepData prestepData, out TProjection projection); + void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, in Vector3Wide ac, in QuaternionWide orientationC, in BodyInertias inertiaC, + float dt, float inverseDt, ref TPrestepData prestepData, out TProjection projection); void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref BodyVelocities velocityC, ref TProjection projection, ref TAccumulatedImpulse accumulatedImpulse); void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref BodyVelocities velocityC, ref TProjection projection, ref TAccumulatedImpulse accumulatedImpulse); } @@ -89,7 +89,7 @@ internal sealed override void VerifySortRegion(ref TypeBatch typeBatch, int bund { VerifySortRegion(ref typeBatch, bundleStartIndex, constraintCount, ref sortedKeys, ref sortedSourceIndices); } - + public unsafe override void Prestep(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); @@ -102,8 +102,11 @@ public unsafe override void Prestep(ref TypeBatch typeBatch, Bodies bodies, floa ref var projection = ref Unsafe.Add(ref projectionBase, i); ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); var count = GetCountInBundle(ref typeBatch, i); - bodies.GatherInertia(ref references, count, out var inertiaA, out var inertiaB, out var inertiaC); - function.Prestep(bodies, ref references, count, dt, inverseDt, ref inertiaA, ref inertiaB, ref inertiaC, ref prestep, out projection); + bodies.GatherState(ref references, count, + out var orientationA, out var wsvA, out var inertiaA, + out var ab, out var orientationB, out var wsvB, out var inertiaB, + out var ac, out var orientationC, out var wsvC, out var inertiaC); + function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, ac, orientationC, inertiaC, dt, inverseDt, ref prestep, out projection); } } @@ -119,9 +122,12 @@ public unsafe override void WarmStart(ref TypeBatch typeBatch, Bodies bodies, in ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); int count = GetCountInBundle(ref typeBatch, i); - Bodies.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out var wsvA, out var wsvB, out var wsvC); + bodies.GatherState(ref bodyReferences, count, + out var orientationA, out var wsvA, out var inertiaA, + out var ab, out var orientationB, out var wsvB, out var inertiaB, + out var ac, out var orientationC, out var wsvC, out var inertiaC); function.WarmStart(ref wsvA, ref wsvB, ref wsvC, ref projection, ref accumulatedImpulses); - Bodies.ScatterVelocities(ref wsvA, ref wsvB, ref wsvC, ref bodyVelocities, ref bodyReferences, count); + bodies.ScatterVelocities(ref wsvA, ref wsvB, ref wsvC, ref bodyReferences, count); } } @@ -138,9 +144,12 @@ public unsafe override void SolveIteration(ref TypeBatch typeBatch, Bodies bodie ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); int count = GetCountInBundle(ref typeBatch, i); - Bodies.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out var wsvA, out var wsvB, out var wsvC); + bodies.GatherState(ref bodyReferences, count, + out var orientationA, out var wsvA, out var inertiaA, + out var ab, out var orientationB, out var wsvB, out var inertiaB, + out var ac, out var orientationC, out var wsvC, out var inertiaC); function.Solve(ref wsvA, ref wsvB, ref wsvC, ref projection, ref accumulatedImpulses); - Bodies.ScatterVelocities(ref wsvA, ref wsvB, ref wsvC, ref bodyVelocities, ref bodyReferences, count); + bodies.ScatterVelocities(ref wsvA, ref wsvB, ref wsvC, ref bodyReferences, count); } } @@ -156,7 +165,10 @@ public unsafe override void JacobiPrestep(ref TypeBatch typeBatch, Bodies bodies ref var projection = ref Unsafe.Add(ref projectionBase, i); ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); var count = GetCountInBundle(ref typeBatch, i); - bodies.GatherInertia(ref references, count, out var inertiaA, out var inertiaB, out var inertiaC); + bodies.GatherState(ref references, count, + out var orientationA, out var wsvA, out var inertiaA, + out var ab, out var orientationB, out var wsvB, out var inertiaB, + out var ac, out var orientationC, out var wsvC, out var inertiaC); //Jacobi batches split affected bodies into multiple pieces to guarantee convergence. jacobiBatch.GetJacobiScaleForBodies(ref references, count, out var jacobiScaleA, out var jacobiScaleB, out var jacobiScaleC); Symmetric3x3Wide.Scale(inertiaA.InverseInertiaTensor, jacobiScaleA, out inertiaA.InverseInertiaTensor); @@ -165,7 +177,7 @@ public unsafe override void JacobiPrestep(ref TypeBatch typeBatch, Bodies bodies inertiaB.InverseMass *= jacobiScaleB; Symmetric3x3Wide.Scale(inertiaC.InverseInertiaTensor, jacobiScaleC, out inertiaC.InverseInertiaTensor); inertiaC.InverseMass *= jacobiScaleC; - function.Prestep(bodies, ref references, count, dt, inverseDt, ref inertiaA, ref inertiaB, ref inertiaC, ref prestep, out projection); + function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, ac, orientationC, inertiaC, dt, inverseDt, ref prestep, out projection); } } public unsafe override void JacobiWarmStart(ref TypeBatch typeBatch, Bodies bodies, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle) @@ -186,7 +198,10 @@ public unsafe override void JacobiWarmStart(ref TypeBatch typeBatch, Bodies bodi ref var wsvA = ref jacobiResultsBundlesA[i]; ref var wsvB = ref jacobiResultsBundlesB[i]; ref var wsvC = ref jacobiResultsBundlesC[i]; - Bodies.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out wsvA, out wsvB, out wsvC); + bodies.GatherState(ref bodyReferences, count, + out var orientationA, out wsvA, out var inertiaA, + out var ab, out var orientationB, out wsvB, out var inertiaB, + out var ac, out var orientationC, out wsvC, out var inertiaC); function.WarmStart(ref wsvA, ref wsvB, ref wsvC, ref projection, ref accumulatedImpulses); } } @@ -208,7 +223,10 @@ public unsafe override void JacobiSolveIteration(ref TypeBatch typeBatch, Bodies ref var wsvA = ref jacobiResultsBundlesA[i]; ref var wsvB = ref jacobiResultsBundlesB[i]; ref var wsvC = ref jacobiResultsBundlesC[i]; - Bodies.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out wsvA, out wsvB, out wsvC); + bodies.GatherState(ref bodyReferences, count, + out var orientationA, out wsvA, out var inertiaA, + out var ab, out var orientationB, out wsvB, out var inertiaB, + out var ac, out var orientationC, out wsvC, out var inertiaC); function.Solve(ref wsvA, ref wsvB, ref wsvC, ref projection, ref accumulatedImpulses); } } @@ -220,7 +238,6 @@ public unsafe override void SolveStep(ref TypeBatch typeBatch, Bodies bodies, fl ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); - ref var bodyVelocities = ref bodies.ActiveSet.Velocities; var function = default(TConstraintFunctions); for (int i = startBundle; i < exclusiveEndBundle; ++i) { @@ -229,12 +246,14 @@ public unsafe override void SolveStep(ref TypeBatch typeBatch, Bodies bodies, fl ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); var count = GetCountInBundle(ref typeBatch, i); - bodies.GatherInertia(ref references, count, out var inertiaA, out var inertiaB, out var inertiaC); - function.Prestep(bodies, ref references, count, dt, inverseDt, ref inertiaA, ref inertiaB, ref inertiaC, ref prestep, out var projection); - Bodies.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out var wsvA, out var wsvB, out var wsvC); + bodies.GatherState(ref references, count, + out var orientationA, out var wsvA, out var inertiaA, + out var ab, out var orientationB, out var wsvB, out var inertiaB, + out var ac, out var orientationC, out var wsvC, out var inertiaC); + function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, ac, orientationC, inertiaC, dt, inverseDt, ref prestep, out var projection); function.WarmStart(ref wsvA, ref wsvB, ref wsvC, ref projection, ref accumulatedImpulses); function.Solve(ref wsvA, ref wsvB, ref wsvC, ref projection, ref accumulatedImpulses); - Bodies.ScatterVelocities(ref wsvA, ref wsvB, ref wsvC, ref bodyVelocities, ref bodyReferences, count); + bodies.ScatterVelocities(ref wsvA, ref wsvB, ref wsvC, ref bodyReferences, count); } } @@ -244,7 +263,6 @@ public unsafe override void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodi ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); - ref var bodyVelocities = ref bodies.ActiveSet.Velocities; var function = default(TConstraintFunctions); ref var jacobiResultsBundlesA = ref jacobiResults.GetVelocitiesForBody(0); ref var jacobiResultsBundlesB = ref jacobiResults.GetVelocitiesForBody(1); @@ -256,7 +274,13 @@ public unsafe override void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodi ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); var count = GetCountInBundle(ref typeBatch, i); - bodies.GatherInertia(ref references, count, out var inertiaA, out var inertiaB, out var inertiaC); + ref var wsvA = ref jacobiResultsBundlesA[i]; + ref var wsvB = ref jacobiResultsBundlesB[i]; + ref var wsvC = ref jacobiResultsBundlesC[i]; + bodies.GatherState(ref references, count, + out var orientationA, out wsvA, out var inertiaA, + out var ab, out var orientationB, out wsvB, out var inertiaB, + out var ac, out var orientationC, out wsvC, out var inertiaC); //Jacobi batches split affected bodies into multiple pieces to guarantee convergence. jacobiBatch.GetJacobiScaleForBodies(ref references, count, out var jacobiScaleA, out var jacobiScaleB, out var jacobiScaleC); Symmetric3x3Wide.Scale(inertiaA.InverseInertiaTensor, jacobiScaleA, out inertiaA.InverseInertiaTensor); @@ -265,11 +289,7 @@ public unsafe override void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodi inertiaB.InverseMass *= jacobiScaleB; Symmetric3x3Wide.Scale(inertiaC.InverseInertiaTensor, jacobiScaleC, out inertiaC.InverseInertiaTensor); inertiaC.InverseMass *= jacobiScaleC; - function.Prestep(bodies, ref references, count, dt, inverseDt, ref inertiaA, ref inertiaB, ref inertiaC, ref prestep, out var projection); - ref var wsvA = ref jacobiResultsBundlesA[i]; - ref var wsvB = ref jacobiResultsBundlesB[i]; - ref var wsvC = ref jacobiResultsBundlesC[i]; - Bodies.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out wsvA, out wsvB, out wsvC); + function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, ac, orientationC, inertiaC, dt, inverseDt, ref prestep, out var projection); function.WarmStart(ref wsvA, ref wsvB, ref wsvC, ref projection, ref accumulatedImpulses); function.Solve(ref wsvA, ref wsvB, ref wsvC, ref projection, ref accumulatedImpulses); } diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 826e29edc..f944d4775 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -26,7 +26,7 @@ public struct TwoBodyReferences /// Type of the projection to input. public interface IConstraintFunctions { - void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int count, float dt, float inverseDt, ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref TPrestepData prestepData, out TProjection projection); + void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, float dt, float inverseDt, ref TPrestepData prestepData, out TProjection projection); void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref TProjection projection, ref TAccumulatedImpulse accumulatedImpulse); void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref TProjection projection, ref TAccumulatedImpulse accumulatedImpulse); } @@ -138,8 +138,8 @@ public unsafe override void Prestep(ref TypeBatch typeBatch, Bodies bodies, floa ref var projection = ref Unsafe.Add(ref projectionBase, i); ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); var count = GetCountInBundle(ref typeBatch, i); - bodies.GatherInertia(ref references, count, out var inertiaA, out var inertiaB); - function.Prestep(bodies, ref references, count, dt, inverseDt, ref inertiaA, ref inertiaB, ref prestep, out projection); + bodies.GatherState(ref references, count, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB); + function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, dt, inverseDt, ref prestep, out projection); } } @@ -155,9 +155,9 @@ public unsafe override void WarmStart(ref TypeBatch typeBatch, Bodies bodies, in ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); int count = GetCountInBundle(ref typeBatch, i); - Bodies.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out var wsvA, out var wsvB); + bodies.GatherState(ref bodyReferences, count, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB); function.WarmStart(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); - Bodies.ScatterVelocities(ref wsvA, ref wsvB, ref bodyVelocities, ref bodyReferences, count); + bodies.ScatterVelocities(ref wsvA, ref wsvB, ref bodyReferences, count); } } @@ -174,9 +174,9 @@ public unsafe override void SolveIteration(ref TypeBatch typeBatch, Bodies bodie ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); int count = GetCountInBundle(ref typeBatch, i); - Bodies.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out var wsvA, out var wsvB); + bodies.GatherState(ref bodyReferences, count, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB); function.Solve(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); - Bodies.ScatterVelocities(ref wsvA, ref wsvB, ref bodyVelocities, ref bodyReferences, count); + bodies.ScatterVelocities(ref wsvA, ref wsvB, ref bodyReferences, count); } } @@ -192,14 +192,14 @@ public unsafe override void JacobiPrestep(ref TypeBatch typeBatch, Bodies bodies ref var projection = ref Unsafe.Add(ref projectionBase, i); ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); var count = GetCountInBundle(ref typeBatch, i); - bodies.GatherInertia(ref references, count, out var inertiaA, out var inertiaB); + bodies.GatherState(ref references, count, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB); //Jacobi batches split affected bodies into multiple pieces to guarantee convergence. jacobiBatch.GetJacobiScaleForBodies(ref references, count, out var jacobiScaleA, out var jacobiScaleB); Symmetric3x3Wide.Scale(inertiaA.InverseInertiaTensor, jacobiScaleA, out inertiaA.InverseInertiaTensor); inertiaA.InverseMass *= jacobiScaleA; Symmetric3x3Wide.Scale(inertiaB.InverseInertiaTensor, jacobiScaleB, out inertiaB.InverseInertiaTensor); inertiaB.InverseMass *= jacobiScaleB; - function.Prestep(bodies, ref references, count, dt, inverseDt, ref inertiaA, ref inertiaB, ref prestep, out projection); + function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, dt, inverseDt, ref prestep, out projection); } } public unsafe override void JacobiWarmStart(ref TypeBatch typeBatch, Bodies bodies, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle) @@ -218,7 +218,7 @@ public unsafe override void JacobiWarmStart(ref TypeBatch typeBatch, Bodies bodi int count = GetCountInBundle(ref typeBatch, i); ref var wsvA = ref jacobiResultsBundlesA[i]; ref var wsvB = ref jacobiResultsBundlesB[i]; - Bodies.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out wsvA, out wsvB); + bodies.GatherState(ref bodyReferences, count, out var orientationA, out wsvA, out var inertiaA, out var ab, out var orientationB, out wsvB, out var inertiaB); function.WarmStart(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); } } @@ -238,7 +238,7 @@ public unsafe override void JacobiSolveIteration(ref TypeBatch typeBatch, Bodies int count = GetCountInBundle(ref typeBatch, i); ref var wsvA = ref jacobiResultsBundlesA[i]; ref var wsvB = ref jacobiResultsBundlesB[i]; - Bodies.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out wsvA, out wsvB); + bodies.GatherState(ref bodyReferences, count, out var orientationA, out wsvA, out var inertiaA, out var ab, out var orientationB, out wsvB, out var inertiaB); function.Solve(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); } } @@ -249,7 +249,6 @@ public unsafe override void SolveStep(ref TypeBatch typeBatch, Bodies bodies, fl ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); - ref var bodyVelocities = ref bodies.ActiveSet.Velocities; var function = default(TConstraintFunctions); for (int i = startBundle; i < exclusiveEndBundle; ++i) { @@ -258,12 +257,11 @@ public unsafe override void SolveStep(ref TypeBatch typeBatch, Bodies bodies, fl ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); var count = GetCountInBundle(ref typeBatch, i); - bodies.GatherInertia(ref references, count, out var inertiaA, out var inertiaB); - function.Prestep(bodies, ref references, count, dt, inverseDt, ref inertiaA, ref inertiaB, ref prestep, out var projection); - Bodies.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out var wsvA, out var wsvB); + bodies.GatherState(ref references, count, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB); + function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, dt, inverseDt, ref prestep, out var projection); function.WarmStart(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); function.Solve(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); - Bodies.ScatterVelocities(ref wsvA, ref wsvB, ref bodyVelocities, ref bodyReferences, count); + bodies.ScatterVelocities(ref wsvA, ref wsvB, ref bodyReferences, count); } } @@ -283,18 +281,17 @@ public unsafe override void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodi ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); - var count = GetCountInBundle(ref typeBatch, i); - bodies.GatherInertia(ref references, count, out var inertiaA, out var inertiaB); + var count = GetCountInBundle(ref typeBatch, i); + ref var wsvA = ref jacobiResultsBundlesA[i]; + ref var wsvB = ref jacobiResultsBundlesB[i]; + bodies.GatherState(ref references, count, out var orientationA, out wsvA, out var inertiaA, out var ab, out var orientationB, out wsvB, out var inertiaB); //Jacobi batches split affected bodies into multiple pieces to guarantee convergence. jacobiBatch.GetJacobiScaleForBodies(ref references, count, out var jacobiScaleA, out var jacobiScaleB); Symmetric3x3Wide.Scale(inertiaA.InverseInertiaTensor, jacobiScaleA, out inertiaA.InverseInertiaTensor); inertiaA.InverseMass *= jacobiScaleA; Symmetric3x3Wide.Scale(inertiaB.InverseInertiaTensor, jacobiScaleB, out inertiaB.InverseInertiaTensor); inertiaB.InverseMass *= jacobiScaleB; - function.Prestep(bodies, ref references, count, dt, inverseDt, ref inertiaA, ref inertiaB, ref prestep, out var projection); - ref var wsvA = ref jacobiResultsBundlesA[i]; - ref var wsvB = ref jacobiResultsBundlesB[i]; - Bodies.GatherVelocities(ref motionStates, ref bodyReferences, count, out wsvA, out wsvB); + function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, dt, inverseDt, ref prestep, out var projection); function.WarmStart(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); function.Solve(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); } @@ -305,7 +302,6 @@ public unsafe override void WarmStart2(ref TypeBatch typeBatch, Bodies bodies, f ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); - ref var states = ref bodies.ActiveSet.MotionStates; var function = default(TConstraintFunctions); for (int i = startBundle; i < exclusiveEndBundle; ++i) { @@ -314,11 +310,10 @@ public unsafe override void WarmStart2(ref TypeBatch typeBatch, Bodies bodies, f ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); var count = GetCountInBundle(ref typeBatch, i); - bodies.GatherInertia(ref references, count, out var inertiaA, out var inertiaB); - Bodies.GatherVelocities(ref states, ref bodyReferences, count, out var wsvA, out var wsvB); - function.Prestep(bodies, ref references, count, dt, inverseDt, ref inertiaA, ref inertiaB, ref prestep, out var projection); + bodies.GatherState(ref references, count, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB); + function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, dt, inverseDt, ref prestep, out var projection); function.WarmStart(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); - Bodies.ScatterVelocities(ref wsvA, ref wsvB, ref states, ref bodyReferences, count); + bodies.ScatterVelocities(ref wsvA, ref wsvB, ref bodyReferences, count); } } public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) @@ -326,7 +321,6 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); - ref var bodyVelocities = ref bodies.ActiveSet.Velocities; var function = default(TConstraintFunctions); for (int i = startBundle; i < exclusiveEndBundle; ++i) { @@ -335,11 +329,10 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); var count = GetCountInBundle(ref typeBatch, i); - bodies.GatherInertia(ref references, count, out var inertiaA, out var inertiaB); - Bodies.GatherVelocities(ref bodyVelocities, ref bodyReferences, count, out var wsvA, out var wsvB); - function.Prestep(bodies, ref references, count, dt, inverseDt, ref inertiaA, ref inertiaB, ref prestep, out var projection); + bodies.GatherState(ref references, count, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB); + function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, dt, inverseDt, ref prestep, out var projection); function.Solve(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); - Bodies.ScatterVelocities(ref wsvA, ref wsvB, ref bodyVelocities, ref bodyReferences, count); + bodies.ScatterVelocities(ref wsvA, ref wsvB, ref bodyReferences, count); } } @@ -349,7 +342,6 @@ public unsafe override void Prestep2(ref TypeBatch typeBatch, Bodies bodies, flo ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); - ref var bodyVelocities = ref bodies.ActiveSet.Velocities; var function = default(TConstraintFunctions); for (int i = startBundle; i < exclusiveEndBundle; ++i) { @@ -358,11 +350,10 @@ public unsafe override void Prestep2(ref TypeBatch typeBatch, Bodies bodies, flo ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); var count = GetCountInBundle(ref typeBatch, i); - bodies.GatherInertia(ref references, count, out var inertiaA, out var inertiaB); - Bodies.GatherVelocities(ref bodyVelocities, ref references, count, out var wsvA, out var wsvB); - function.Prestep(bodies, ref references, count, dt, inverseDt, ref inertiaA, ref inertiaB, ref prestep, out projection); + bodies.GatherState(ref references, count, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB); + function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, dt, inverseDt, ref prestep, out projection); function.WarmStart(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); - Bodies.ScatterVelocities(ref wsvA, ref wsvB, ref bodyVelocities, ref references, count); + bodies.ScatterVelocities(ref wsvA, ref wsvB, ref references, count); } } public override void JacobiPrestep2(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) @@ -380,7 +371,6 @@ public unsafe override void IncrementallyUpdateContactData(ref TypeBatch typeBat { ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); - ref var bodyVelocities = ref bodies.ActiveSet.Velocities; var function = default(TConstraintFunctions); var dtWide = new Vector(dt); for (int i = startBundle; i < exclusiveEndBundle; ++i) @@ -388,8 +378,8 @@ public unsafe override void IncrementallyUpdateContactData(ref TypeBatch typeBat ref var prestep = ref Unsafe.Add(ref prestepBase, i); ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); var count = GetCountInBundle(ref typeBatch, i); - Bodies.GatherVelocities(ref bodyVelocities, ref references, count, out var velocityA, out var velocityB); - function.IncrementallyUpdateContactData(dtWide, velocityA, velocityB, ref prestep); + bodies.GatherState(ref references, count, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB); + function.IncrementallyUpdateContactData(dtWide, wsvA, wsvB, ref prestep); } } } diff --git a/BepuPhysics/Constraints/Weld.cs b/BepuPhysics/Constraints/Weld.cs index 776f42bcc..074e53449 100644 --- a/BepuPhysics/Constraints/Weld.cs +++ b/BepuPhysics/Constraints/Weld.cs @@ -88,10 +88,9 @@ public struct WeldAccumulatedImpulses public struct WeldFunctions : IConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int count, float dt, float inverseDt, ref BodyInertias inertiaA, ref BodyInertias inertiaB, + public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, QuaternionWide orientationB, in BodyInertias inertiaB, float dt, float inverseDt, ref WeldPrestepData prestep, out WeldProjection projection) { - bodies.GatherPose(ref bodyReferences, count, out var localPositionB, out var orientationA, out var orientationB); projection.InertiaA = inertiaA; projection.InertiaB = inertiaB; @@ -128,7 +127,7 @@ public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int cou //Compute the current constraint error for all 6 degrees of freedom. //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. - Vector3Wide.Subtract(localPositionB, projection.Offset, out var positionError); + Vector3Wide.Subtract(ab, projection.Offset, out var positionError); QuaternionWide.ConcatenateWithoutOverlap(prestep.LocalOrientation, orientationA, out var targetOrientationB); QuaternionWide.Conjugate(targetOrientationB, out var inverseTarget); QuaternionWide.ConcatenateWithoutOverlap(inverseTarget, orientationB, out var rotationError); From 7b46855b7bdbd455d8a5066f385afebe5544c6fb Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 29 Jun 2021 14:26:14 -0500 Subject: [PATCH 045/947] Pushed through prestep signature refactor for all library constraints. --- .../Constraints/AngularAxisGearMotor.cs | 5 ++- BepuPhysics/Constraints/AngularAxisMotor.cs | 7 ++-- BepuPhysics/Constraints/AngularHinge.cs | 6 ++-- BepuPhysics/Constraints/AngularMotor.cs | 5 ++- BepuPhysics/Constraints/AngularServo.cs | 5 ++- BepuPhysics/Constraints/AngularSwivelHinge.cs | 6 ++-- BepuPhysics/Constraints/AreaConstraint.cs | 12 +++---- BepuPhysics/Constraints/BallSocket.cs | 9 +++--- BepuPhysics/Constraints/BallSocketMotor.cs | 9 +++--- BepuPhysics/Constraints/BallSocketServo.cs | 9 +++--- BepuPhysics/Constraints/BallSocketShared.cs | 2 +- .../Constraints/CenterDistanceConstraint.cs | 6 ++-- .../Constraints/Contact/ContactConvexTypes.cs | 32 +++++++++---------- .../Constraints/Contact/ContactConvexTypes.tt | 4 +-- .../Contact/ContactNonconvexCommon.cs | 8 ++--- BepuPhysics/Constraints/DistanceLimit.cs | 6 ++-- BepuPhysics/Constraints/DistanceServo.cs | 12 +++---- BepuPhysics/Constraints/Hinge.cs | 5 ++- BepuPhysics/Constraints/LinearAxisLimit.cs | 6 ++-- BepuPhysics/Constraints/LinearAxisMotor.cs | 6 ++-- BepuPhysics/Constraints/LinearAxisServo.cs | 12 +++---- .../Constraints/OneBodyAngularMotor.cs | 5 ++- .../Constraints/OneBodyAngularServo.cs | 5 ++- BepuPhysics/Constraints/OneBodyLinearMotor.cs | 9 +++--- BepuPhysics/Constraints/OneBodyLinearServo.cs | 11 +++---- BepuPhysics/Constraints/PointOnLineServo.cs | 7 ++-- BepuPhysics/Constraints/SwingLimit.cs | 6 ++-- BepuPhysics/Constraints/SwivelHinge.cs | 5 ++- BepuPhysics/Constraints/TwistLimit.cs | 6 ++-- BepuPhysics/Constraints/TwistMotor.cs | 5 ++- BepuPhysics/Constraints/TwistServo.cs | 11 +++---- BepuPhysics/Constraints/VolumeConstraint.cs | 11 ++++--- BepuPhysics/Constraints/Weld.cs | 2 +- 33 files changed, 114 insertions(+), 141 deletions(-) diff --git a/BepuPhysics/Constraints/AngularAxisGearMotor.cs b/BepuPhysics/Constraints/AngularAxisGearMotor.cs index 5584e3d17..e3fcaba40 100644 --- a/BepuPhysics/Constraints/AngularAxisGearMotor.cs +++ b/BepuPhysics/Constraints/AngularAxisGearMotor.cs @@ -81,13 +81,12 @@ public struct AngularAxisGearMotorProjection public struct AngularAxisGearMotorFunctions : IConstraintFunctions> { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int count, float dt, float inverseDt, ref BodyInertias inertiaA, ref BodyInertias inertiaB, - ref AngularAxisGearMotorPrestepData prestep, out AngularAxisGearMotorProjection projection) + public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + float dt, float inverseDt, ref AngularAxisGearMotorPrestepData prestep, out AngularAxisGearMotorProjection projection) { //Velocity level constraint that acts directly on the given axes. Jacobians just the axes, nothing complicated. 1DOF, so we do premultiplication. //This is mildly more complex than the AngularAxisMotor: //dot(wa, axis) - dot(wb, axis) * velocityScale = 0, so jacobianB is actually -axis * velocityScale, not just -axis. - bodies.GatherOrientation(ref bodyReferences, count, out var orientationA, out var orientationB); QuaternionWide.TransformWithoutOverlap(prestep.LocalAxisA, orientationA, out var axis); Vector3Wide.Scale(axis, prestep.VelocityScale, out var jA); Symmetric3x3Wide.TransformWithoutOverlap(jA, inertiaA.InverseInertiaTensor, out projection.ImpulseToVelocityA); diff --git a/BepuPhysics/Constraints/AngularAxisMotor.cs b/BepuPhysics/Constraints/AngularAxisMotor.cs index 633dae752..d955319c4 100644 --- a/BepuPhysics/Constraints/AngularAxisMotor.cs +++ b/BepuPhysics/Constraints/AngularAxisMotor.cs @@ -37,7 +37,7 @@ public readonly int ConstraintTypeId } public readonly Type TypeProcessorType => typeof(AngularAxisMotorTypeProcessor); - + public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { ConstraintChecker.AssertUnitLength(LocalAxisA, nameof(AngularAxisMotor), nameof(LocalAxisA)); @@ -80,11 +80,10 @@ public struct AngularAxisMotorProjection public struct AngularAxisMotorFunctions : IConstraintFunctions> { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int count, float dt, float inverseDt, ref BodyInertias inertiaA, ref BodyInertias inertiaB, - ref AngularAxisMotorPrestepData prestep, out AngularAxisMotorProjection projection) + public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + float dt, float inverseDt, ref AngularAxisMotorPrestepData prestep, out AngularAxisMotorProjection projection) { //Velocity level constraint that acts directly on the given axes. Jacobians just the axes, nothing complicated. 1DOF, so we do premultiplication. - bodies.GatherOrientation(ref bodyReferences, count, out var orientationA, out var orientationB); QuaternionWide.TransformWithoutOverlap(prestep.LocalAxisA, orientationA, out var axis); Symmetric3x3Wide.TransformWithoutOverlap(axis, inertiaA.InverseInertiaTensor, out projection.ImpulseToVelocityA); Vector3Wide.Dot(axis, projection.ImpulseToVelocityA, out var contributionA); diff --git a/BepuPhysics/Constraints/AngularHinge.cs b/BepuPhysics/Constraints/AngularHinge.cs index f73dd2ff9..384b52553 100644 --- a/BepuPhysics/Constraints/AngularHinge.cs +++ b/BepuPhysics/Constraints/AngularHinge.cs @@ -123,11 +123,9 @@ public static void GetErrorAngles(in Vector3Wide hingeAxisA, in Vector3Wide hing [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int count, float dt, float inverseDt, ref BodyInertias inertiaA, ref BodyInertias inertiaB, - ref AngularHingePrestepData prestep, out AngularHingeProjection projection) + public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + float dt, float inverseDt, ref AngularHingePrestepData prestep, out AngularHingeProjection projection) { - bodies.GatherOrientation(ref bodyReferences, count, out var orientationA, out var orientationB); - //Note that we build the tangents in local space first to avoid inconsistencies. Helpers.BuildOrthonormalBasis(prestep.LocalHingeAxisA, out var localAX, out var localAY); Matrix3x3Wide.CreateFromQuaternion(orientationA, out var orientationMatrixA); diff --git a/BepuPhysics/Constraints/AngularMotor.cs b/BepuPhysics/Constraints/AngularMotor.cs index f665a2702..1b2efc91a 100644 --- a/BepuPhysics/Constraints/AngularMotor.cs +++ b/BepuPhysics/Constraints/AngularMotor.cs @@ -71,10 +71,9 @@ public struct AngularMotorProjection public struct AngularMotorFunctions : IConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int count, float dt, float inverseDt, ref BodyInertias inertiaA, ref BodyInertias inertiaB, - ref AngularMotorPrestepData prestep, out AngularMotorProjection projection) + public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + float dt, float inverseDt, ref AngularMotorPrestepData prestep, out AngularMotorProjection projection) { - bodies.GatherOrientation(ref bodyReferences, count, out var orientationA, out var orientationB); projection.ImpulseToVelocityA = inertiaA.InverseInertiaTensor; projection.NegatedImpulseToVelocityB = inertiaB.InverseInertiaTensor; diff --git a/BepuPhysics/Constraints/AngularServo.cs b/BepuPhysics/Constraints/AngularServo.cs index 4afee7639..fe84af7c6 100644 --- a/BepuPhysics/Constraints/AngularServo.cs +++ b/BepuPhysics/Constraints/AngularServo.cs @@ -79,10 +79,9 @@ public struct AngularServoProjection public struct AngularServoFunctions : IConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int count, float dt, float inverseDt, ref BodyInertias inertiaA, ref BodyInertias inertiaB, - ref AngularServoPrestepData prestep, out AngularServoProjection projection) + public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + float dt, float inverseDt, ref AngularServoPrestepData prestep, out AngularServoProjection projection) { - bodies.GatherOrientation(ref bodyReferences, count, out var orientationA, out var orientationB); projection.ImpulseToVelocityA = inertiaA.InverseInertiaTensor; projection.NegatedImpulseToVelocityB = inertiaB.InverseInertiaTensor; diff --git a/BepuPhysics/Constraints/AngularSwivelHinge.cs b/BepuPhysics/Constraints/AngularSwivelHinge.cs index da2dea129..252a997ee 100644 --- a/BepuPhysics/Constraints/AngularSwivelHinge.cs +++ b/BepuPhysics/Constraints/AngularSwivelHinge.cs @@ -81,11 +81,9 @@ public struct AngularSwivelHingeProjection public struct AngularSwivelHingeFunctions : IConstraintFunctions> { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int count, float dt, float inverseDt, ref BodyInertias inertiaA, ref BodyInertias inertiaB, - ref AngularSwivelHingePrestepData prestep, out AngularSwivelHingeProjection projection) + public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + float dt, float inverseDt, ref AngularSwivelHingePrestepData prestep, out AngularSwivelHingeProjection projection) { - bodies.GatherOrientation(ref bodyReferences, count, out var orientationA, out var orientationB); - //The swivel hinge attempts to keep an axis on body A separated 90 degrees from an axis on body B. In other words, this is the same as a hinge joint, but with one fewer DOF. //C = dot(swivelA, hingeB) = 0 //C' = dot(d/dt(swivelA), hingeB) + dot(swivelA, d/dt(hingeB)) = 0 diff --git a/BepuPhysics/Constraints/AreaConstraint.cs b/BepuPhysics/Constraints/AreaConstraint.cs index 140072583..fe972f1e3 100644 --- a/BepuPhysics/Constraints/AreaConstraint.cs +++ b/BepuPhysics/Constraints/AreaConstraint.cs @@ -88,12 +88,12 @@ public struct AreaConstraintProjection public struct AreaConstraintFunctions : IThreeBodyConstraintFunctions> { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(Bodies bodies, ref ThreeBodyReferences bodyReferences, int count, float dt, float inverseDt, - ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref BodyInertias inertiaC, - ref AreaConstraintPrestepData prestep, out AreaConstraintProjection projection) + public void Prestep( + in QuaternionWide orientationA, in BodyInertias inertiaA, + in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + in Vector3Wide ac, in QuaternionWide orientationC, in BodyInertias inertiaC, + float dt, float inverseDt, ref AreaConstraintPrestepData prestep, out AreaConstraintProjection projection) { - bodies.GatherOffsets(ref bodyReferences, count, out var ab, out var ac); - //Area of a triangle with vertices a, b, and c is: //||ab x ac|| * 0.5 //So the constraint is: @@ -162,7 +162,7 @@ private static void ApplyImpulse(ref BodyVelocities velocityA, ref BodyVelocitie Vector3Wide.Add(velocityB.Linear, velocityChangeB, out velocityB.Linear); Vector3Wide.Add(velocityC.Linear, velocityChangeC, out velocityC.Linear); } - + [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref BodyVelocities velocityC, ref AreaConstraintProjection projection, ref Vector accumulatedImpulse) { diff --git a/BepuPhysics/Constraints/BallSocket.cs b/BepuPhysics/Constraints/BallSocket.cs index d8dad9704..463328315 100644 --- a/BepuPhysics/Constraints/BallSocket.cs +++ b/BepuPhysics/Constraints/BallSocket.cs @@ -78,10 +78,9 @@ public struct BallSocketProjection public struct BallSocketFunctions : IConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int count, float dt, float inverseDt, ref BodyInertias inertiaA, ref BodyInertias inertiaB, - ref BallSocketPrestepData prestep, out BallSocketProjection projection) + public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + float dt, float inverseDt, ref BallSocketPrestepData prestep, out BallSocketProjection projection) { - bodies.GatherPose(ref bodyReferences, count, out var offsetB, out var orientationA, out var orientationB); projection.InertiaA = inertiaA; projection.InertiaB = inertiaB; @@ -89,10 +88,10 @@ public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int cou QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetA, orientationA, out projection.OffsetA); QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetB, orientationB, out projection.OffsetB); SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); - BallSocketShared.ComputeEffectiveMass(ref inertiaA, ref inertiaB, ref projection.OffsetA, ref projection.OffsetB, ref effectiveMassCFMScale, out projection.EffectiveMass); + BallSocketShared.ComputeEffectiveMass(inertiaA, inertiaB, ref projection.OffsetA, ref projection.OffsetB, ref effectiveMassCFMScale, out projection.EffectiveMass); //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. - Vector3Wide.Add(offsetB, projection.OffsetB, out var anchorB); + Vector3Wide.Add(ab, projection.OffsetB, out var anchorB); Vector3Wide.Subtract(anchorB, projection.OffsetA, out var error); Vector3Wide.Scale(error, positionErrorToVelocity, out projection.BiasVelocity); } diff --git a/BepuPhysics/Constraints/BallSocketMotor.cs b/BepuPhysics/Constraints/BallSocketMotor.cs index 7704fe1df..08ae3730c 100644 --- a/BepuPhysics/Constraints/BallSocketMotor.cs +++ b/BepuPhysics/Constraints/BallSocketMotor.cs @@ -83,18 +83,17 @@ public struct BallSocketMotorProjection public struct BallSocketMotorFunctions : IConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int count, float dt, float inverseDt, ref BodyInertias inertiaA, ref BodyInertias inertiaB, - ref BallSocketMotorPrestepData prestep, out BallSocketMotorProjection projection) + public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + float dt, float inverseDt, ref BallSocketMotorPrestepData prestep, out BallSocketMotorProjection projection) { - bodies.GatherPose(ref bodyReferences, count, out var offsetFromACenterToBCenter, out var orientationA, out var orientationB); projection.InertiaA = inertiaA; projection.InertiaB = inertiaB; //The offset for A just goes directly to B's anchor. QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetB, orientationB, out projection.OffsetB); - Vector3Wide.Add(offsetFromACenterToBCenter, projection.OffsetB, out projection.OffsetA); + Vector3Wide.Add(ab, projection.OffsetB, out projection.OffsetA); MotorSettingsWide.ComputeSoftness(prestep.Settings, dt, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale, out projection.MaximumImpulse); - BallSocketShared.ComputeEffectiveMass(ref inertiaA, ref inertiaB, ref projection.OffsetA, ref projection.OffsetB, ref effectiveMassCFMScale, out projection.EffectiveMass); + BallSocketShared.ComputeEffectiveMass(inertiaA, inertiaB, ref projection.OffsetA, ref projection.OffsetB, ref effectiveMassCFMScale, out projection.EffectiveMass); QuaternionWide.Transform(prestep.TargetVelocityLocalA, orientationA, out projection.BiasVelocity); Vector3Wide.Negate(projection.BiasVelocity, out projection.BiasVelocity); diff --git a/BepuPhysics/Constraints/BallSocketServo.cs b/BepuPhysics/Constraints/BallSocketServo.cs index c49a244f9..2373f1123 100644 --- a/BepuPhysics/Constraints/BallSocketServo.cs +++ b/BepuPhysics/Constraints/BallSocketServo.cs @@ -87,10 +87,9 @@ public struct BallSocketServoProjection public struct BallSocketServoFunctions : IConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int count, float dt, float inverseDt, ref BodyInertias inertiaA, ref BodyInertias inertiaB, - ref BallSocketServoPrestepData prestep, out BallSocketServoProjection projection) + public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + float dt, float inverseDt, ref BallSocketServoPrestepData prestep, out BallSocketServoProjection projection) { - bodies.GatherPose(ref bodyReferences, count, out var offsetFromACenterToBCenter, out var orientationA, out var orientationB); projection.InertiaA = inertiaA; projection.InertiaB = inertiaB; @@ -98,10 +97,10 @@ public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int cou QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetA, orientationA, out projection.OffsetA); QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetB, orientationB, out projection.OffsetB); SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); - BallSocketShared.ComputeEffectiveMass(ref inertiaA, ref inertiaB, ref projection.OffsetA, ref projection.OffsetB, ref effectiveMassCFMScale, out projection.EffectiveMass); + BallSocketShared.ComputeEffectiveMass(inertiaA, inertiaB, ref projection.OffsetA, ref projection.OffsetB, ref effectiveMassCFMScale, out projection.EffectiveMass); //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. - Vector3Wide.Add(offsetFromACenterToBCenter, projection.OffsetB, out var anchorB); + Vector3Wide.Add(ab, projection.OffsetB, out var anchorB); Vector3Wide.Subtract(anchorB, projection.OffsetA, out var error); ServoSettingsWide.ComputeClampedBiasVelocity(error, positionErrorToVelocity, prestep.ServoSettings, dt, inverseDt, out projection.BiasVelocity, out projection.MaximumImpulse); } diff --git a/BepuPhysics/Constraints/BallSocketShared.cs b/BepuPhysics/Constraints/BallSocketShared.cs index a0a9a5a39..bde78daa5 100644 --- a/BepuPhysics/Constraints/BallSocketShared.cs +++ b/BepuPhysics/Constraints/BallSocketShared.cs @@ -16,7 +16,7 @@ public static class BallSocketShared //There are very few cases where a combo constraint will have less than 3DOFs...) //The only reason not to do that is codegen concerns. But we may want to stop holding back just because of some hopefully-not-permanent quirks in the JIT. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ComputeEffectiveMass(ref BodyInertias inertiaA, ref BodyInertias inertiaB, + public static void ComputeEffectiveMass(in BodyInertias inertiaA, in BodyInertias inertiaB, ref Vector3Wide offsetA, ref Vector3Wide offsetB, ref Vector effectiveMassCFMScale, out Symmetric3x3Wide effectiveMass) { //Anchor points attached to each body are constrained to stay in the same position, yielding a position constraint of: diff --git a/BepuPhysics/Constraints/CenterDistanceConstraint.cs b/BepuPhysics/Constraints/CenterDistanceConstraint.cs index aa7a3c557..8e404c131 100644 --- a/BepuPhysics/Constraints/CenterDistanceConstraint.cs +++ b/BepuPhysics/Constraints/CenterDistanceConstraint.cs @@ -79,11 +79,9 @@ public struct CenterDistanceProjection public struct CenterDistanceConstraintFunctions : IConstraintFunctions> { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int count, float dt, float inverseDt, ref BodyInertias inertiaA, ref BodyInertias inertiaB, - ref CenterDistancePrestepData prestep, out CenterDistanceProjection projection) + public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + float dt, float inverseDt, ref CenterDistancePrestepData prestep, out CenterDistanceProjection projection) { - bodies.GatherOffsets(ref bodyReferences, count, out var ab); - Vector3Wide.Length(ab, out var distance); Vector3Wide.Scale(ab, Vector.One / distance, out projection.JacobianA); diff --git a/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs b/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs index f77bd5172..95a06b327 100644 --- a/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs +++ b/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs @@ -303,8 +303,8 @@ public unsafe struct Contact1OneBodyProjection public struct Contact1OneBodyFunctions : IOneBodyContactConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(Bodies bodies, ref Vector bodyReferences, int count, - float dt, float inverseDt, ref BodyInertias inertiaA, ref Contact1OneBodyPrestepData prestep, out Contact1OneBodyProjection projection) + public void Prestep( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertias inertiaA, float dt, float inverseDt, ref Contact1OneBodyPrestepData prestep, out Contact1OneBodyProjection projection) { //Be careful about the execution order here. It should be aligned with the prestep data layout to ensure prefetching works well. projection.InertiaA = inertiaA; @@ -478,8 +478,8 @@ public unsafe struct Contact2OneBodyProjection public struct Contact2OneBodyFunctions : IOneBodyContactConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(Bodies bodies, ref Vector bodyReferences, int count, - float dt, float inverseDt, ref BodyInertias inertiaA, ref Contact2OneBodyPrestepData prestep, out Contact2OneBodyProjection projection) + public void Prestep( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertias inertiaA, float dt, float inverseDt, ref Contact2OneBodyPrestepData prestep, out Contact2OneBodyProjection projection) { //Be careful about the execution order here. It should be aligned with the prestep data layout to ensure prefetching works well. projection.InertiaA = inertiaA; @@ -666,8 +666,8 @@ public unsafe struct Contact3OneBodyProjection public struct Contact3OneBodyFunctions : IOneBodyContactConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(Bodies bodies, ref Vector bodyReferences, int count, - float dt, float inverseDt, ref BodyInertias inertiaA, ref Contact3OneBodyPrestepData prestep, out Contact3OneBodyProjection projection) + public void Prestep( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertias inertiaA, float dt, float inverseDt, ref Contact3OneBodyPrestepData prestep, out Contact3OneBodyProjection projection) { //Be careful about the execution order here. It should be aligned with the prestep data layout to ensure prefetching works well. projection.InertiaA = inertiaA; @@ -868,8 +868,8 @@ public unsafe struct Contact4OneBodyProjection public struct Contact4OneBodyFunctions : IOneBodyContactConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(Bodies bodies, ref Vector bodyReferences, int count, - float dt, float inverseDt, ref BodyInertias inertiaA, ref Contact4OneBodyPrestepData prestep, out Contact4OneBodyProjection projection) + public void Prestep( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertias inertiaA, float dt, float inverseDt, ref Contact4OneBodyPrestepData prestep, out Contact4OneBodyProjection projection) { //Be careful about the execution order here. It should be aligned with the prestep data layout to ensure prefetching works well. projection.InertiaA = inertiaA; @@ -1064,8 +1064,8 @@ public unsafe struct Contact1Projection public struct Contact1Functions : IContactConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int count, - float dt, float inverseDt, ref BodyInertias inertiaA, ref BodyInertias inertiaB,ref Contact1PrestepData prestep, out Contact1Projection projection) + public void Prestep( + in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, float dt, float inverseDt, ref Contact1PrestepData prestep, out Contact1Projection projection) { //Be careful about the execution order here. It should be aligned with the prestep data layout to ensure prefetching works well. projection.InertiaA = inertiaA; @@ -1255,8 +1255,8 @@ public unsafe struct Contact2Projection public struct Contact2Functions : IContactConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int count, - float dt, float inverseDt, ref BodyInertias inertiaA, ref BodyInertias inertiaB,ref Contact2PrestepData prestep, out Contact2Projection projection) + public void Prestep( + in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, float dt, float inverseDt, ref Contact2PrestepData prestep, out Contact2Projection projection) { //Be careful about the execution order here. It should be aligned with the prestep data layout to ensure prefetching works well. projection.InertiaA = inertiaA; @@ -1460,8 +1460,8 @@ public unsafe struct Contact3Projection public struct Contact3Functions : IContactConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int count, - float dt, float inverseDt, ref BodyInertias inertiaA, ref BodyInertias inertiaB,ref Contact3PrestepData prestep, out Contact3Projection projection) + public void Prestep( + in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, float dt, float inverseDt, ref Contact3PrestepData prestep, out Contact3Projection projection) { //Be careful about the execution order here. It should be aligned with the prestep data layout to ensure prefetching works well. projection.InertiaA = inertiaA; @@ -1680,8 +1680,8 @@ public unsafe struct Contact4Projection public struct Contact4Functions : IContactConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int count, - float dt, float inverseDt, ref BodyInertias inertiaA, ref BodyInertias inertiaB,ref Contact4PrestepData prestep, out Contact4Projection projection) + public void Prestep( + in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, float dt, float inverseDt, ref Contact4PrestepData prestep, out Contact4Projection projection) { //Be careful about the execution order here. It should be aligned with the prestep data layout to ensure prefetching works well. projection.InertiaA = inertiaA; diff --git a/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt b/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt index 90a97577e..833d55bde 100644 --- a/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt +++ b/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt @@ -238,8 +238,8 @@ for (int i = 0; i < contactCount; ++i) public struct Contact<#=contactCount#><#=suffix#>Functions : I<#=suffix#>ContactConstraintFunctions<#=suffix#>PrestepData, Contact<#=contactCount#><#=suffix#>Projection, Contact<#=contactCount#>AccumulatedImpulses> { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(Bodies bodies, ref <#=bodyReferencesType#> bodyReferences, int count, - float dt, float inverseDt, ref BodyInertias inertiaA, <#if(bodyCount == 2) {#>ref BodyInertias inertiaB,<#}#>ref Contact<#=contactCount#><#=suffix#>PrestepData prestep, out Contact<#=contactCount#><#=suffix#>Projection projection) + public void Prestep( + <#if(bodyCount == 1) {#>in Vector3Wide positionA,<#}#> in QuaternionWide orientationA, in BodyInertias inertiaA, <#if(bodyCount == 2) {#>in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, <#}#>float dt, float inverseDt, ref Contact<#=contactCount#><#=suffix#>PrestepData prestep, out Contact<#=contactCount#><#=suffix#>Projection projection) { //Be careful about the execution order here. It should be aligned with the prestep data layout to ensure prefetching works well. projection.InertiaA = inertiaA; diff --git a/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs b/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs index 9cbd0dea1..1e14a2bab 100644 --- a/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs +++ b/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs @@ -217,8 +217,8 @@ public struct ContactNonconvexOneBodyFunctions bodyReferences, int count, - float dt, float inverseDt, ref BodyInertias inertia, ref TPrestep prestep, out TProjection projection) + public void Prestep(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertias inertia, + float dt, float inverseDt, ref TPrestep prestep, out TProjection projection) { //TODO: This is another area where it's highly doubtful that the compiler will ever figure out that this initialization is unnecessary. //While we could jump through some nasty contortions now to resolve this, we'll instead opt for a little inefficient simplicity while waiting for generic pointer support @@ -302,8 +302,8 @@ public struct ContactNonconvexTwoBodyFunctions> { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int count, float dt, float inverseDt, ref BodyInertias inertiaA, ref BodyInertias inertiaB, - ref DistanceLimitPrestepData prestep, out DistanceLimitProjection projection) + public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + float dt, float inverseDt, ref DistanceLimitPrestepData prestep, out DistanceLimitProjection projection) { - DistanceServoFunctions.GetDistance(bodies, ref bodyReferences, count, prestep.LocalOffsetA, prestep.LocalOffsetB, + DistanceServoFunctions.GetDistance(orientationA, ab, orientationB, prestep.LocalOffsetA, prestep.LocalOffsetB, out var anchorOffsetA, out var anchorOffsetB, out var anchorOffset, out var distance); //If the current distance is closer to the minimum, calibrate for the minimum. Otherwise, calibrate for the maximum. var useMinimum = Vector.LessThan(Vector.Abs(distance - prestep.MinimumDistance), Vector.Abs(distance - prestep.MaximumDistance)); diff --git a/BepuPhysics/Constraints/DistanceServo.cs b/BepuPhysics/Constraints/DistanceServo.cs index 07a2ed414..949decdf4 100644 --- a/BepuPhysics/Constraints/DistanceServo.cs +++ b/BepuPhysics/Constraints/DistanceServo.cs @@ -120,14 +120,12 @@ public struct DistanceServoProjection public struct DistanceServoFunctions : IConstraintFunctions> { - public static void GetDistance(Bodies bodies, ref TwoBodyReferences bodyReferences, int count, in Vector3Wide localOffsetA, in Vector3Wide localOffsetB, + public static void GetDistance(in QuaternionWide orientationA, in Vector3Wide ab, in QuaternionWide orientationB, in Vector3Wide localOffsetA, in Vector3Wide localOffsetB, out Vector3Wide anchorOffsetA, out Vector3Wide anchorOffsetB, out Vector3Wide anchorOffset, out Vector distance) { - bodies.GatherPose(ref bodyReferences, count, out var offsetB, out var orientationA, out var orientationB); - QuaternionWide.TransformWithoutOverlap(localOffsetA, orientationA, out anchorOffsetA); QuaternionWide.TransformWithoutOverlap(localOffsetB, orientationB, out anchorOffsetB); - Vector3Wide.Add(anchorOffsetB, offsetB, out var anchorB); + Vector3Wide.Add(anchorOffsetB, ab, out var anchorB); Vector3Wide.Subtract(anchorB, anchorOffsetA, out anchorOffset); Vector3Wide.Length(anchorOffset, out distance); @@ -186,10 +184,10 @@ public static void ComputeTransforms( } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int count, float dt, float inverseDt, ref BodyInertias inertiaA, ref BodyInertias inertiaB, - ref DistanceServoPrestepData prestep, out DistanceServoProjection projection) + public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + float dt, float inverseDt, ref DistanceServoPrestepData prestep, out DistanceServoProjection projection) { - GetDistance(bodies, ref bodyReferences, count, prestep.LocalOffsetA, prestep.LocalOffsetB, out var anchorOffsetA, out var anchorOffsetB, out var anchorOffset, out var distance); + GetDistance(orientationA, ab, orientationB, prestep.LocalOffsetA, prestep.LocalOffsetB, out var anchorOffsetA, out var anchorOffsetB, out var anchorOffset, out var distance); Vector3Wide.Scale(anchorOffset, Vector.One / distance, out var direction); diff --git a/BepuPhysics/Constraints/Hinge.cs b/BepuPhysics/Constraints/Hinge.cs index 5e9ae27a1..120a202af 100644 --- a/BepuPhysics/Constraints/Hinge.cs +++ b/BepuPhysics/Constraints/Hinge.cs @@ -102,10 +102,9 @@ public struct HingeAccumulatedImpulses public struct HingeFunctions : IConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int count, float dt, float inverseDt, ref BodyInertias inertiaA, ref BodyInertias inertiaB, - ref HingePrestepData prestep, out HingeProjection projection) + public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + float dt, float inverseDt, ref HingePrestepData prestep, out HingeProjection projection) { - bodies.GatherPose(ref bodyReferences, count, out var ab, out var orientationA, out var orientationB); projection.InertiaA = inertiaA; projection.InertiaB = inertiaB; diff --git a/BepuPhysics/Constraints/LinearAxisLimit.cs b/BepuPhysics/Constraints/LinearAxisLimit.cs index 0bd08f21e..c2879e6f9 100644 --- a/BepuPhysics/Constraints/LinearAxisLimit.cs +++ b/BepuPhysics/Constraints/LinearAxisLimit.cs @@ -112,8 +112,8 @@ public void Modify(in Vector3Wide anchorA, in Vector3Wide anchorB, ref Vector3Wi } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int count, float dt, float inverseDt, ref BodyInertias inertiaA, ref BodyInertias inertiaB, - ref LinearAxisLimitPrestepData prestep, out LinearAxisServoProjection projection) + public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + float dt, float inverseDt, ref LinearAxisLimitPrestepData prestep, out LinearAxisServoProjection projection) { Unsafe.SkipInit(out projection); SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); @@ -121,7 +121,7 @@ public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int cou modifier.MinimumOffset = prestep.MinimumOffset; modifier.MaximumOffset = prestep.MaximumOffset; modifier.Error = default; - LinearAxisServoFunctions.ComputeTransforms(ref modifier, bodies, ref bodyReferences, count, prestep.LocalOffsetA, prestep.LocalOffsetB, prestep.LocalPlaneNormal, inertiaA, inertiaB, effectiveMassCFMScale, + LinearAxisServoFunctions.ComputeTransforms(ref modifier, prestep.LocalOffsetA, prestep.LocalOffsetB, prestep.LocalPlaneNormal, orientationA, inertiaA, ab, orientationB, inertiaB, effectiveMassCFMScale, out var anchorA, out var anchorB, out var normal, out var effectiveMass, out projection.LinearVelocityToImpulseA, out projection.AngularVelocityToImpulseA, out projection.AngularVelocityToImpulseB, out projection.LinearImpulseToVelocityA, out projection.AngularImpulseToVelocityA, out projection.NegatedLinearImpulseToVelocityB, out projection.AngularImpulseToVelocityB); diff --git a/BepuPhysics/Constraints/LinearAxisMotor.cs b/BepuPhysics/Constraints/LinearAxisMotor.cs index 5cb6d4069..439b216bf 100644 --- a/BepuPhysics/Constraints/LinearAxisMotor.cs +++ b/BepuPhysics/Constraints/LinearAxisMotor.cs @@ -82,12 +82,12 @@ public struct LinearAxisMotorPrestepData public struct LinearAxisMotorFunctions : IConstraintFunctions> { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int count, float dt, float inverseDt, ref BodyInertias inertiaA, ref BodyInertias inertiaB, - ref LinearAxisMotorPrestepData prestep, out LinearAxisServoProjection projection) + public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + float dt, float inverseDt, ref LinearAxisMotorPrestepData prestep, out LinearAxisServoProjection projection) { MotorSettingsWide.ComputeSoftness(prestep.Settings, dt, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale, out projection.MaximumImpulse); var modifier = new LinearAxisServoFunctions.NoChangeModifier(); - LinearAxisServoFunctions.ComputeTransforms(ref modifier, bodies, ref bodyReferences, count, prestep.LocalOffsetA, prestep.LocalOffsetB, prestep.LocalPlaneNormal, inertiaA, inertiaB, effectiveMassCFMScale, + LinearAxisServoFunctions.ComputeTransforms(ref modifier, prestep.LocalOffsetA, prestep.LocalOffsetB, prestep.LocalPlaneNormal, orientationA, inertiaA, ab, orientationB, inertiaB, effectiveMassCFMScale, out _, out _, out _, out var effectiveMass, out projection.LinearVelocityToImpulseA, out projection.AngularVelocityToImpulseA, out projection.AngularVelocityToImpulseB, out projection.LinearImpulseToVelocityA, out projection.AngularImpulseToVelocityA, out projection.NegatedLinearImpulseToVelocityB, out projection.AngularImpulseToVelocityB); diff --git a/BepuPhysics/Constraints/LinearAxisServo.cs b/BepuPhysics/Constraints/LinearAxisServo.cs index 4b46a458d..bcaab6857 100644 --- a/BepuPhysics/Constraints/LinearAxisServo.cs +++ b/BepuPhysics/Constraints/LinearAxisServo.cs @@ -116,9 +116,10 @@ public void Modify(in Vector3Wide anchorA, in Vector3Wide anchorB, ref Vector3Wi } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ComputeTransforms(ref TJacobianModifier jacobianModifier, Bodies bodies, ref TwoBodyReferences bodyReferences, int count, + public static void ComputeTransforms(ref TJacobianModifier jacobianModifier, in Vector3Wide localOffsetA, in Vector3Wide localOffsetB, in Vector3Wide localPlaneNormal, - in BodyInertias inertiaA, in BodyInertias inertiaB, in Vector effectiveMassCFMScale, + in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + in Vector effectiveMassCFMScale, out Vector3Wide anchorA, out Vector3Wide anchorB, out Vector3Wide normal, out Vector effectiveMass, out Vector3Wide linearVelocityToImpulseA, out Vector3Wide angularVelocityToImpulseA, out Vector3Wide angularVelocityToImpulseB, out Vector3Wide linearImpulseToVelocityA, out Vector3Wide angularImpulseToVelocityA, out Vector3Wide negatedLinearImpulseToVelocityB, out Vector3Wide angularImpulseToVelocityB) @@ -132,7 +133,6 @@ public static void ComputeTransforms(ref TJacobianModifier ja //dot(linearA + angularA x offsetA - linearB - angularB x offsetB), planeNormal) = 0 //dot(linearA - linearB, planeNormal) + dot(angularA x offsetA, planeNormal) + dot(offsetB x angularB, planeNormal) = 0 //dot(linearA - linearB, planeNormal) + dot(offsetA x planeNormal, angularA) + dot(planeNormal x offsetB, angularB) = 0 - bodies.GatherPose(ref bodyReferences, count, out var ab, out var orientationA, out var orientationB); //We'll just use the offset from a to anchorB as the 'offsetA' above. //(Note that there's no mathy reason why TargetOffset exists over just the LocalOffsetA alone; it's a usability thing.) Matrix3x3Wide.CreateFromQuaternion(orientationA, out var orientationMatrixA); @@ -161,12 +161,12 @@ public static void ComputeTransforms(ref TJacobianModifier ja } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int count, float dt, float inverseDt, ref BodyInertias inertiaA, ref BodyInertias inertiaB, - ref LinearAxisServoPrestepData prestep, out LinearAxisServoProjection projection) + public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + float dt, float inverseDt, ref LinearAxisServoPrestepData prestep, out LinearAxisServoProjection projection) { SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); var modifier = new NoChangeModifier(); - ComputeTransforms(ref modifier, bodies, ref bodyReferences, count, prestep.LocalOffsetA, prestep.LocalOffsetB, prestep.LocalPlaneNormal, inertiaA, inertiaB, effectiveMassCFMScale, + ComputeTransforms(ref modifier, prestep.LocalOffsetA, prestep.LocalOffsetB, prestep.LocalPlaneNormal, orientationA, inertiaA, ab, orientationB, inertiaB, effectiveMassCFMScale, out var anchorA, out var anchorB, out var normal, out var effectiveMass, out projection.LinearVelocityToImpulseA, out projection.AngularVelocityToImpulseA, out projection.AngularVelocityToImpulseB, out projection.LinearImpulseToVelocityA, out projection.AngularImpulseToVelocityA, out projection.NegatedLinearImpulseToVelocityB, out projection.AngularImpulseToVelocityB); diff --git a/BepuPhysics/Constraints/OneBodyAngularMotor.cs b/BepuPhysics/Constraints/OneBodyAngularMotor.cs index d467d3f57..91d4438de 100644 --- a/BepuPhysics/Constraints/OneBodyAngularMotor.cs +++ b/BepuPhysics/Constraints/OneBodyAngularMotor.cs @@ -60,10 +60,9 @@ public struct OneBodyAngularMotorPrestepData public struct OneBodyAngularMotorFunctions : IOneBodyConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(Bodies bodies, ref Vector bodyReferences, int count, float dt, float inverseDt, ref BodyInertias inertiaA, - ref OneBodyAngularMotorPrestepData prestep, out OneBodyAngularServoProjection projection) + public void Prestep(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertias inertiaA, + float dt, float inverseDt, ref OneBodyAngularMotorPrestepData prestep, out OneBodyAngularServoProjection projection) { - bodies.GatherOrientation(ref bodyReferences, count, out var orientationA); projection.ImpulseToVelocity = inertiaA.InverseInertiaTensor; //Jacobians are just the identity matrix. diff --git a/BepuPhysics/Constraints/OneBodyAngularServo.cs b/BepuPhysics/Constraints/OneBodyAngularServo.cs index c9abf2705..f30c180e0 100644 --- a/BepuPhysics/Constraints/OneBodyAngularServo.cs +++ b/BepuPhysics/Constraints/OneBodyAngularServo.cs @@ -77,10 +77,9 @@ public struct OneBodyAngularServoProjection public struct OneBodyAngularServoFunctions : IOneBodyConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(Bodies bodies, ref Vector bodyReferences, int count, float dt, float inverseDt, ref BodyInertias inertiaA, - ref OneBodyAngularServoPrestepData prestep, out OneBodyAngularServoProjection projection) + public void Prestep(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertias inertiaA, + float dt, float inverseDt, ref OneBodyAngularServoPrestepData prestep, out OneBodyAngularServoProjection projection) { - bodies.GatherOrientation(ref bodyReferences, count, out var orientationA); projection.ImpulseToVelocity = inertiaA.InverseInertiaTensor; //Jacobians are just the identity matrix. diff --git a/BepuPhysics/Constraints/OneBodyLinearMotor.cs b/BepuPhysics/Constraints/OneBodyLinearMotor.cs index 2af8b66a4..680297f44 100644 --- a/BepuPhysics/Constraints/OneBodyLinearMotor.cs +++ b/BepuPhysics/Constraints/OneBodyLinearMotor.cs @@ -67,16 +67,15 @@ public struct OneBodyLinearMotorPrestepData public struct OneBodyLinearMotorFunctions : IOneBodyConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(Bodies bodies, ref Vector bodyReferences, int count, float dt, float inverseDt, ref BodyInertias inertia, ref OneBodyLinearMotorPrestepData prestep, - out OneBodyLinearServoProjection projection) + public void Prestep(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertias inertiaA, + float dt, float inverseDt, ref OneBodyLinearMotorPrestepData prestep, out OneBodyLinearServoProjection projection) { //TODO: Note that this grabs a world position. That poses a problem for different position representations. - bodies.GatherPose(ref bodyReferences, count, out var position, out var orientation); - projection.Inertia = inertia; + projection.Inertia = inertiaA; MotorSettingsWide.ComputeSoftness(prestep.Settings, dt, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale, out projection.MaximumImpulse); - OneBodyLinearServoFunctions.ComputeTransforms(prestep.LocalOffset, orientation, effectiveMassCFMScale, inertia, out projection.Offset, out projection.EffectiveMass); + OneBodyLinearServoFunctions.ComputeTransforms(prestep.LocalOffset, orientationA, effectiveMassCFMScale, inertiaA, out projection.Offset, out projection.EffectiveMass); projection.BiasVelocity = prestep.TargetVelocity; } diff --git a/BepuPhysics/Constraints/OneBodyLinearServo.cs b/BepuPhysics/Constraints/OneBodyLinearServo.cs index 6fc8cd882..34f069f1e 100644 --- a/BepuPhysics/Constraints/OneBodyLinearServo.cs +++ b/BepuPhysics/Constraints/OneBodyLinearServo.cs @@ -103,19 +103,18 @@ public static void ComputeTransforms(in Vector3Wide localOffset, in QuaternionWi } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(Bodies bodies, ref Vector bodyReferences, int count, float dt, float inverseDt, ref BodyInertias inertia, ref OneBodyLinearServoPrestepData prestep, - out OneBodyLinearServoProjection projection) + public void Prestep(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertias inertiaA, + float dt, float inverseDt, ref OneBodyLinearServoPrestepData prestep, out OneBodyLinearServoProjection projection) { //TODO: Note that this grabs a world position. That poses a problem for different position representations. - bodies.GatherPose(ref bodyReferences, count, out var position, out var orientation); - projection.Inertia = inertia; + projection.Inertia = inertiaA; SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); - ComputeTransforms(prestep.LocalOffset, orientation, effectiveMassCFMScale, inertia, out projection.Offset, out projection.EffectiveMass); + ComputeTransforms(prestep.LocalOffset, orientationA, effectiveMassCFMScale, inertiaA, out projection.Offset, out projection.EffectiveMass); //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. - Vector3Wide.Add(projection.Offset, position, out var worldGrabPoint); + Vector3Wide.Add(projection.Offset, positionA, out var worldGrabPoint); Vector3Wide.Subtract(prestep.Target, worldGrabPoint, out var error); ServoSettingsWide.ComputeClampedBiasVelocity(error, positionErrorToVelocity, prestep.ServoSettings, dt, inverseDt, out projection.BiasVelocity, out projection.MaximumImpulse); } diff --git a/BepuPhysics/Constraints/PointOnLineServo.cs b/BepuPhysics/Constraints/PointOnLineServo.cs index 7c24afce0..fda30f883 100644 --- a/BepuPhysics/Constraints/PointOnLineServo.cs +++ b/BepuPhysics/Constraints/PointOnLineServo.cs @@ -103,8 +103,8 @@ static void GetAngularJacobians(in Matrix2x3Wide linearJacobians, in Vector3Wide } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int count, float dt, float inverseDt, ref BodyInertias inertiaA, ref BodyInertias inertiaB, - ref PointOnLineServoPrestepData prestep, out PointOnLineServoProjection projection) + public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + float dt, float inverseDt, ref PointOnLineServoPrestepData prestep, out PointOnLineServoProjection projection) { //This constrains a point on B to a line attached to A. It works on two degrees of freedom at the same time; those are the tangent axes to the line direction. //The error is measured as closest offset from the line. In other words: @@ -138,7 +138,6 @@ public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int cou //Instead, we'll go with #2. //#3 would be the fastest on a single core by virtue of requiring significantly less ALU work, but it requires more memory bandwidth. - bodies.GatherPose(ref bodyReferences, count, out var ab, out var orientationA, out var orientationB); Matrix3x3Wide.CreateFromQuaternion(orientationA, out var orientationMatrixA); Matrix3x3Wide.TransformWithoutOverlap(prestep.LocalOffsetA, orientationMatrixA, out var anchorA); QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetB, orientationB, out projection.OffsetB); @@ -178,7 +177,7 @@ public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int cou } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ApplyImpulse(ref BodyVelocities velocityA, ref BodyVelocities velocityB, + public static void ApplyImpulse(ref BodyVelocities velocityA, ref BodyVelocities velocityB, in Matrix2x3Wide linearJacobian, in Matrix2x3Wide angularJacobianA, in Matrix2x3Wide angularJacobianB, in BodyInertias inertiaA, in BodyInertias inertiaB, ref Vector2Wide csi) { Matrix2x3Wide.Transform(csi, linearJacobian, out var linearImpulseA); diff --git a/BepuPhysics/Constraints/SwingLimit.cs b/BepuPhysics/Constraints/SwingLimit.cs index 8f41e4eb7..cf1274bef 100644 --- a/BepuPhysics/Constraints/SwingLimit.cs +++ b/BepuPhysics/Constraints/SwingLimit.cs @@ -102,11 +102,9 @@ public struct SwingLimitProjection public struct SwingLimitFunctions : IConstraintFunctions> { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int count, float dt, float inverseDt, ref BodyInertias inertiaA, ref BodyInertias inertiaB, - ref SwingLimitPrestepData prestep, out SwingLimitProjection projection) + public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + float dt, float inverseDt, ref SwingLimitPrestepData prestep, out SwingLimitProjection projection) { - bodies.GatherOrientation(ref bodyReferences, count, out var orientationA, out var orientationB); - //The swing limit attempts to keep an axis on body A within from an axis on body B. In other words, this is the same as a hinge joint, but with one fewer DOF. //(Note that the jacobians are extremely similar to the AngularSwivelHinge; the difference is that this is a speculative inequality constraint.) //C = dot(axisA, axisB) >= MinimumDot diff --git a/BepuPhysics/Constraints/SwivelHinge.cs b/BepuPhysics/Constraints/SwivelHinge.cs index 60ff8052f..739371ff1 100644 --- a/BepuPhysics/Constraints/SwivelHinge.cs +++ b/BepuPhysics/Constraints/SwivelHinge.cs @@ -95,10 +95,9 @@ public struct SwivelHingeProjection public struct SwivelHingeFunctions : IConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int count, float dt, float inverseDt, ref BodyInertias inertiaA, ref BodyInertias inertiaB, - ref SwivelHingePrestepData prestep, out SwivelHingeProjection projection) + public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + float dt, float inverseDt, ref SwivelHingePrestepData prestep, out SwivelHingeProjection projection) { - bodies.GatherPose(ref bodyReferences, count, out var ab, out var orientationA, out var orientationB); projection.InertiaA = inertiaA; projection.InertiaB = inertiaB; diff --git a/BepuPhysics/Constraints/TwistLimit.cs b/BepuPhysics/Constraints/TwistLimit.cs index d977f4140..1a3bd35b5 100644 --- a/BepuPhysics/Constraints/TwistLimit.cs +++ b/BepuPhysics/Constraints/TwistLimit.cs @@ -95,11 +95,11 @@ public struct TwistLimitProjection public struct TwistLimitFunctions : IConstraintFunctions> { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int count, float dt, float inverseDt, ref BodyInertias inertiaA, ref BodyInertias inertiaB, - ref TwistLimitPrestepData prestep, out TwistLimitProjection projection) + public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + float dt, float inverseDt, ref TwistLimitPrestepData prestep, out TwistLimitProjection projection) { Unsafe.SkipInit(out projection); - TwistServoFunctions.ComputeJacobian(bodies, bodyReferences, count, prestep.LocalBasisA, prestep.LocalBasisB, + TwistServoFunctions.ComputeJacobian(orientationA, orientationB, prestep.LocalBasisA, prestep.LocalBasisB, out var basisBX, out var basisBZ, out var basisA, out var jacobianA); TwistServoFunctions.ComputeCurrentAngle(basisBX, basisBZ, basisA, out var angle); diff --git a/BepuPhysics/Constraints/TwistMotor.cs b/BepuPhysics/Constraints/TwistMotor.cs index 206bfb5c3..bb6109f43 100644 --- a/BepuPhysics/Constraints/TwistMotor.cs +++ b/BepuPhysics/Constraints/TwistMotor.cs @@ -87,11 +87,10 @@ public struct TwistMotorProjection public struct TwistMotorFunctions : IConstraintFunctions> { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int count, float dt, float inverseDt, ref BodyInertias inertiaA, ref BodyInertias inertiaB, - ref TwistMotorPrestepData prestep, out TwistMotorProjection projection) + public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + float dt, float inverseDt, ref TwistMotorPrestepData prestep, out TwistMotorProjection projection) { Unsafe.SkipInit(out projection); - bodies.GatherOrientation(ref bodyReferences, count, out var orientationA, out var orientationB); //We don't need any measurement basis in a velocity motor, so the prestep data needs only the axes. QuaternionWide.TransformWithoutOverlap(prestep.LocalAxisA, orientationA, out var axisA); QuaternionWide.TransformWithoutOverlap(prestep.LocalAxisB, orientationB, out var axisB); diff --git a/BepuPhysics/Constraints/TwistServo.cs b/BepuPhysics/Constraints/TwistServo.cs index c60ba3848..99b398ea6 100644 --- a/BepuPhysics/Constraints/TwistServo.cs +++ b/BepuPhysics/Constraints/TwistServo.cs @@ -96,12 +96,9 @@ public struct TwistServoProjection public struct TwistServoFunctions : IConstraintFunctions> { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ComputeJacobian( - Bodies bodies, TwoBodyReferences bodyReferences, int count, in QuaternionWide localBasisA, in QuaternionWide localBasisB, + public static void ComputeJacobian(in QuaternionWide orientationA, in QuaternionWide orientationB, in QuaternionWide localBasisA, in QuaternionWide localBasisB, out Vector3Wide basisBX, out Vector3Wide basisBZ, out Matrix3x3Wide basisA, out Vector3Wide jacobianA) { - bodies.GatherOrientation(ref bodyReferences, count, out var orientationA, out var orientationB); - //Twist joints attempt to match rotation around each body's local axis. //We'll use a basis attached to each of the two bodies. //B's basis will be transformed into alignment with A's basis for measurement. @@ -170,11 +167,11 @@ public static void ComputeEffectiveMass(float dt, in SpringSettingsWide springSe } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int count, float dt, float inverseDt, ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref TwistServoPrestepData prestep, - out TwistServoProjection projection) + public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + float dt, float inverseDt, ref TwistServoPrestepData prestep, out TwistServoProjection projection) { Unsafe.SkipInit(out projection); - ComputeJacobian(bodies, bodyReferences, count, prestep.LocalBasisA, prestep.LocalBasisB, + ComputeJacobian(orientationA, orientationB, prestep.LocalBasisA, prestep.LocalBasisB, out var basisBX, out var basisBZ, out var basisA, out var jacobianA); ComputeEffectiveMass(dt, prestep.SpringSettings, inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, jacobianA, diff --git a/BepuPhysics/Constraints/VolumeConstraint.cs b/BepuPhysics/Constraints/VolumeConstraint.cs index 576fe8381..1e6248230 100644 --- a/BepuPhysics/Constraints/VolumeConstraint.cs +++ b/BepuPhysics/Constraints/VolumeConstraint.cs @@ -90,12 +90,13 @@ public struct VolumeConstraintProjection public struct VolumeConstraintFunctions : IFourBodyConstraintFunctions> { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(Bodies bodies, ref FourBodyReferences bodyReferences, int count, float dt, float inverseDt, - ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref BodyInertias inertiaC, ref BodyInertias inertiaD, - ref VolumeConstraintPrestepData prestep, out VolumeConstraintProjection projection) + public void Prestep( + in QuaternionWide orientationA, in BodyInertias inertiaA, + in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + in Vector3Wide ac, in QuaternionWide orientationC, in BodyInertias inertiaC, + in Vector3Wide ad, in QuaternionWide orientationD, in BodyInertias inertiaD, + float dt, float inverseDt, ref VolumeConstraintPrestepData prestep, out VolumeConstraintProjection projection) { - bodies.GatherOffsets(ref bodyReferences, count, out var ab, out var ac, out var ad); - //Volume of parallelepiped with vertices a, b, c, d is: //(ab x ac) * ad //A tetrahedron with the same edges will have one sixth of this volume. As a constant factor, it's not relevant. So the constraint is just: diff --git a/BepuPhysics/Constraints/Weld.cs b/BepuPhysics/Constraints/Weld.cs index 074e53449..1092cab2a 100644 --- a/BepuPhysics/Constraints/Weld.cs +++ b/BepuPhysics/Constraints/Weld.cs @@ -88,7 +88,7 @@ public struct WeldAccumulatedImpulses public struct WeldFunctions : IConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, QuaternionWide orientationB, in BodyInertias inertiaB, float dt, float inverseDt, + public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, float dt, float inverseDt, ref WeldPrestepData prestep, out WeldProjection projection) { projection.InertiaA = inertiaA; From cf6b7214ea91a2be6eebe635b9b69ce4ce9e3641 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 29 Jun 2021 14:51:15 -0500 Subject: [PATCH 046/947] Pushed through the remaining motionstates refactoring. Builds, but does not run. --- BepuPhysics/Bodies.cs | 4 +- BepuPhysics/BodyDescription.cs | 25 +++++---- BepuPhysics/BodyProperties.cs | 54 +++++++++++++++++++ BepuPhysics/BodySet.cs | 16 +++--- .../AngularSwivelHingeLineExtractor.cs | 4 +- .../Constraints/BallSocketLineExtractor.cs | 4 +- .../BallSocketMotorLineExtractor.cs | 4 +- .../BallSocketServoLineExtractor.cs | 4 +- .../CenterDistanceLineExtractor.cs | 4 +- .../Constraints/ContactLineExtractors.cs | 28 +++++----- .../Constraints/ContactLineExtractors.tt | 2 +- .../Constraints/DistanceLimitLineExtractor.cs | 4 +- .../Constraints/DistanceServoLineExtractor.cs | 4 +- .../Constraints/HingeLineExtractor.cs | 4 +- .../LinearAxisServoLineExtractor.cs | 4 +- .../OneBodyLinearServoLineExtractor.cs | 2 +- .../Constraints/PointOnLineLineExtractor.cs | 4 +- .../Constraints/SwivelHingeLineExtractor.cs | 4 +- DemoRenderer/Constraints/WeldLineExtractor.cs | 4 +- DemoRenderer/ShapeDrawing/ShapesExtractor.cs | 2 +- .../ConstraintDescriptionMappingTests.cs | 2 +- DemoTests/TestUtilities.cs | 5 +- Demos/Demo.cs | 2 +- Demos/Demos/BlockChainDemo.cs | 7 +-- .../Demos/Characters/CharacterControllers.cs | 12 ++--- .../Characters/CharacterMotionConstraint.cs | 6 ++- .../Characters/CharacterMotionConstraint.tt | 3 +- Demos/Demos/CompoundTestDemo.cs | 2 +- Demos/Demos/FountainStressTestDemo.cs | 6 +-- Demos/SpecializedTests/CapsuleTestDemo.cs | 6 +-- Demos/SpecializedTests/CylinderTestDemo.cs | 10 ++-- Demos/SpecializedTests/DeterminismTest.cs | 2 +- Demos/SpecializedTests/MeshTestDemo.cs | 6 +-- Demos/SpecializedTests/TestHelpers.cs | 4 +- 34 files changed, 154 insertions(+), 100 deletions(-) diff --git a/BepuPhysics/Bodies.cs b/BepuPhysics/Bodies.cs index 938b5627c..ed5e8bde3 100644 --- a/BepuPhysics/Bodies.cs +++ b/BepuPhysics/Bodies.cs @@ -175,7 +175,7 @@ public unsafe BodyHandle Add(in BodyDescription description) //Out of room; need to resize. ResizeHandles(HandleToLocation.Length << 1); } - Debug.Assert(Math.Abs(description.MotionState.Pose.Orientation.Length() - 1) < 1e-6f, "Orientation should be initialized to a unit length quaternion."); + Debug.Assert(Math.Abs(description.Pose.Orientation.Length() - 1) < 1e-6f, "Orientation should be initialized to a unit length quaternion."); //All new bodies are active for simplicity. Someday, it may be worth offering an optimized path for inactives, but it adds complexity. //(Directly adding inactive bodies can be helpful in some networked open world scenarios.) @@ -185,7 +185,7 @@ public unsafe BodyHandle Add(in BodyDescription description) if (description.Collidable.Shape.Exists) { - AddCollidableToBroadPhase(handle, description.MotionState.Pose, description.LocalInertia, ref ActiveSet.Collidables[index]); + AddCollidableToBroadPhase(handle, description.Pose, description.LocalInertia, ref ActiveSet.Collidables[index]); } return handle; } diff --git a/BepuPhysics/BodyDescription.cs b/BepuPhysics/BodyDescription.cs index baa47c5e6..3268b8c2c 100644 --- a/BepuPhysics/BodyDescription.cs +++ b/BepuPhysics/BodyDescription.cs @@ -34,7 +34,8 @@ public BodyActivityDescription(float sleepThreshold, byte minimumTimestepCountUn public struct BodyDescription { - public MotionState MotionState; + public RigidPose Pose; + public BodyVelocity Velocity; public BodyInertia LocalInertia; public CollidableDescription Collidable; public BodyActivityDescription Activity; @@ -80,7 +81,7 @@ public static BodyActivityDescription GetDefaultActivity(in TShape shape /// Constructed description for the body. public static BodyDescription CreateDynamic(in RigidPose pose, in BodyVelocity velocity, in BodyInertia inertia, in CollidableDescription collidable, in BodyActivityDescription activity) { - return new BodyDescription { MotionState = new (pose, velocity), LocalInertia = inertia, Activity = activity, Collidable = collidable }; + return new BodyDescription { Pose = pose, Velocity = velocity, LocalInertia = inertia, Activity = activity, Collidable = collidable }; } /// @@ -93,7 +94,7 @@ public static BodyDescription CreateDynamic(in RigidPose pose, in BodyVelocity v /// Constructed description for the body. public static BodyDescription CreateDynamic(in RigidPose pose, in BodyInertia inertia, in CollidableDescription collidable, in BodyActivityDescription activity) { - return new BodyDescription { MotionState = new(pose), LocalInertia = inertia, Activity = activity, Collidable = collidable }; + return new BodyDescription { Pose = pose, LocalInertia = inertia, Activity = activity, Collidable = collidable }; } /// @@ -107,7 +108,7 @@ public static BodyDescription CreateDynamic(in RigidPose pose, in BodyInertia in /// Constructed description for the body. public static BodyDescription CreateDynamic(in Vector3 position, in BodyVelocity velocity, in BodyInertia inertia, in CollidableDescription collidable, in BodyActivityDescription activity) { - return new BodyDescription { MotionState = new(position, velocity), LocalInertia = inertia, Activity = activity, Collidable = collidable }; + return new BodyDescription { Pose = new(position), Velocity = velocity, LocalInertia = inertia, Activity = activity, Collidable = collidable }; } /// @@ -120,7 +121,7 @@ public static BodyDescription CreateDynamic(in Vector3 position, in BodyVelocity /// Constructed description for the body. public static BodyDescription CreateDynamic(in Vector3 position, in BodyInertia inertia, in CollidableDescription collidable, in BodyActivityDescription activity) { - return new BodyDescription { MotionState = new(position), LocalInertia = inertia, Activity = activity, Collidable = collidable }; + return new BodyDescription { Pose = new(position), LocalInertia = inertia, Activity = activity, Collidable = collidable }; } /// @@ -139,7 +140,8 @@ public static BodyDescription CreateConvexDynamic( { var description = new BodyDescription { - MotionState = new(pose, velocity), + Pose = pose, + Velocity = velocity, Activity = GetDefaultActivity(shape), Collidable = new CollidableDescription(shapes.Add(shape), GetDefaultSpeculativeMargin(shape)) }; @@ -206,7 +208,7 @@ public static BodyDescription CreateConvexDynamic( /// Constructed description for the body. public static BodyDescription CreateKinematic(in RigidPose pose, in BodyVelocity velocity, in CollidableDescription collidable, in BodyActivityDescription activity) { - return new BodyDescription { MotionState = new(pose, velocity), Activity = activity, Collidable = collidable }; + return new BodyDescription { Pose = pose, Velocity = velocity, Activity = activity, Collidable = collidable }; } /// @@ -218,7 +220,7 @@ public static BodyDescription CreateKinematic(in RigidPose pose, in BodyVelocity /// Constructed description for the body. public static BodyDescription CreateKinematic(in RigidPose pose, in CollidableDescription collidable, in BodyActivityDescription activity) { - return new BodyDescription { MotionState = new(pose), Activity = activity, Collidable = collidable }; + return new BodyDescription { Pose = pose, Activity = activity, Collidable = collidable }; } /// @@ -231,7 +233,7 @@ public static BodyDescription CreateKinematic(in RigidPose pose, in CollidableDe /// Constructed description for the body. public static BodyDescription CreateKinematic(in Vector3 position, in BodyVelocity velocity, in CollidableDescription collidable, in BodyActivityDescription activity) { - return new BodyDescription { MotionState = new(position, velocity), Activity = activity, Collidable = collidable }; + return new BodyDescription { Pose = new(position), Velocity = velocity, Activity = activity, Collidable = collidable }; } /// @@ -243,7 +245,7 @@ public static BodyDescription CreateKinematic(in Vector3 position, in BodyVeloci /// Constructed description for the body. public static BodyDescription CreateKinematic(in Vector3 position, in CollidableDescription collidable, in BodyActivityDescription activity) { - return new BodyDescription { MotionState = new(position), Activity = activity, Collidable = collidable }; + return new BodyDescription { Pose = new(position), Activity = activity, Collidable = collidable }; } /// @@ -261,7 +263,8 @@ public static BodyDescription CreateConvexKinematic( { var description = new BodyDescription { - MotionState = new(pose, velocity), + Pose = pose, + Velocity = velocity, Activity = GetDefaultActivity(shape), Collidable = new CollidableDescription(shapes.Add(shape), GetDefaultSpeculativeMargin(shape)) }; diff --git a/BepuPhysics/BodyProperties.cs b/BepuPhysics/BodyProperties.cs index d6b3e0055..f4f013a02 100644 --- a/BepuPhysics/BodyProperties.cs +++ b/BepuPhysics/BodyProperties.cs @@ -13,6 +13,60 @@ namespace BepuPhysics /// public struct MotionState { + /// + /// Gets a motion state with zero position, zero velocity, and identity orientation. + /// + public static MotionState Identity + { + get + { + MotionState m; + m.Velocity = default; + m.Pose.Position = default; + m.Pose.Orientation = Quaternion.Identity; + return m; + } + } + + /// + /// Constructs a motion state for a body. + /// + /// Position of the body. + /// Orientation of the body. + /// Velocity of the body. + public MotionState(in Vector3 position, in Quaternion orientation, in BodyVelocity velocity) + { + Pose.Position = position; + Pose.Orientation = orientation; + Velocity = velocity; + } + + /// + /// Constructs a motion state for a body with zero velocity. + /// + /// Position of the body. + /// Orientation of the body. + public MotionState(in Vector3 position, in Quaternion orientation) + { + Pose.Position = position; + Pose.Orientation = orientation; + Velocity = default; + } + + /// + /// Constructs a motion state for a body with zero angular velocity. + /// + /// Position of the body. + /// Orientation of the body. + /// Linear velocity of the body. + public MotionState(in Vector3 position, in Quaternion orientation, in Vector3 linearVelocity) + { + Pose.Position = position; + Pose.Orientation = orientation; + Velocity.Linear = linearVelocity; + Velocity.Angular = default; + } + /// /// Constructs a motion state for a body. /// diff --git a/BepuPhysics/BodySet.cs b/BepuPhysics/BodySet.cs index 281bff34e..8f822b0a3 100644 --- a/BepuPhysics/BodySet.cs +++ b/BepuPhysics/BodySet.cs @@ -111,10 +111,10 @@ internal bool RemoveAt(int bodyIndex, out BodyHandle handle, out int movedBodyIn internal void ApplyDescriptionByIndex(int index, in BodyDescription description) { - Debug.Assert(!MathChecker.IsInvalid(description.MotionState.Pose.Position.LengthSquared()), $"Invalid body position: {description.MotionState.Pose.Position}"); - Debug.Assert(Math.Abs(1 - description.MotionState.Pose.Orientation.LengthSquared()) < 1e-3f, $"Body orientation not unit length: {description.MotionState.Pose.Orientation}"); - Debug.Assert(!MathChecker.IsInvalid(description.MotionState.Velocity.Linear.LengthSquared()), $"Invalid body linear velocity: {description.MotionState.Velocity.Linear}"); - Debug.Assert(!MathChecker.IsInvalid(description.MotionState.Velocity.Angular.LengthSquared()), $"Invalid body angular velocity: {description.MotionState.Velocity.Angular}"); + Debug.Assert(!MathChecker.IsInvalid(description.Pose.Position.LengthSquared()), $"Invalid body position: {description.Pose.Position}"); + Debug.Assert(Math.Abs(1 - description.Pose.Orientation.LengthSquared()) < 1e-3f, $"Body orientation not unit length: {description.Pose.Orientation}"); + Debug.Assert(!MathChecker.IsInvalid(description.Velocity.Linear.LengthSquared()), $"Invalid body linear velocity: {description.Velocity.Linear}"); + Debug.Assert(!MathChecker.IsInvalid(description.Velocity.Angular.LengthSquared()), $"Invalid body angular velocity: {description.Velocity.Angular}"); Debug.Assert(!MathChecker.IsInvalid( description.LocalInertia.InverseInertiaTensor.XX * description.LocalInertia.InverseInertiaTensor.XX + description.LocalInertia.InverseInertiaTensor.YX * description.LocalInertia.InverseInertiaTensor.YX + @@ -124,7 +124,9 @@ internal void ApplyDescriptionByIndex(int index, in BodyDescription description) description.LocalInertia.InverseInertiaTensor.ZZ * description.LocalInertia.InverseInertiaTensor.ZZ), $"Invalid body inverse inertia tensor: {description.LocalInertia.InverseInertiaTensor}"); Debug.Assert(!MathChecker.IsInvalid(description.LocalInertia.InverseMass) && description.LocalInertia.InverseMass >= 0, $"Invalid body inverse mass: {description.LocalInertia.InverseMass}"); - MotionStates[index] = description.MotionState; + ref var state = ref MotionStates[index]; + state.Pose = description.Pose; + state.Velocity = description.Velocity; LocalInertias[index] = description.LocalInertia; ref var collidable = ref Collidables[index]; collidable.Continuity = description.Collidable.Continuity; @@ -141,7 +143,9 @@ internal void ApplyDescriptionByIndex(int index, in BodyDescription description) public void GetDescription(int index, out BodyDescription description) { - description.MotionState = MotionStates[index]; + ref var state = ref MotionStates[index]; + description.Pose = state.Pose; + description.Velocity = state.Velocity; description.LocalInertia = LocalInertias[index]; ref var collidable = ref Collidables[index]; description.Collidable.Continuity = collidable.Continuity; diff --git a/DemoRenderer/Constraints/AngularSwivelHingeLineExtractor.cs b/DemoRenderer/Constraints/AngularSwivelHingeLineExtractor.cs index a8f5e1340..2f84a3bf6 100644 --- a/DemoRenderer/Constraints/AngularSwivelHingeLineExtractor.cs +++ b/DemoRenderer/Constraints/AngularSwivelHingeLineExtractor.cs @@ -14,8 +14,8 @@ public unsafe void ExtractLines(ref AngularSwivelHingePrestepData prestepBundle, Bodies bodies, ref Vector3 tint, ref QuickList lines) { //Could do bundles of constraints at a time, but eh. - ref var poseA = ref bodies.Sets[setIndex].Poses[bodyIndices[0]]; - ref var poseB = ref bodies.Sets[setIndex].Poses[bodyIndices[1]]; + ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; + ref var poseB = ref bodies.Sets[setIndex].MotionStates[bodyIndices[1]].Pose; Vector3Wide.ReadFirst(prestepBundle.LocalSwivelAxisA, out var localSwivelAxisA); Vector3Wide.ReadFirst(prestepBundle.LocalHingeAxisB, out var localHingeAxisB); QuaternionEx.Transform(localSwivelAxisA, poseA.Orientation, out var swivelAxis); diff --git a/DemoRenderer/Constraints/BallSocketLineExtractor.cs b/DemoRenderer/Constraints/BallSocketLineExtractor.cs index 4f5ab7fdb..ecef23075 100644 --- a/DemoRenderer/Constraints/BallSocketLineExtractor.cs +++ b/DemoRenderer/Constraints/BallSocketLineExtractor.cs @@ -14,8 +14,8 @@ public unsafe void ExtractLines(ref BallSocketPrestepData prestepBundle, int set Bodies bodies, ref Vector3 tint, ref QuickList 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]]; + ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; + ref var poseB = ref bodies.Sets[setIndex].MotionStates[bodyIndices[1]].Pose; Vector3Wide.ReadFirst(prestepBundle.LocalOffsetA, out var localOffsetA); Vector3Wide.ReadFirst(prestepBundle.LocalOffsetB, out var localOffsetB); QuaternionEx.Transform(localOffsetA, poseA.Orientation, out var worldOffsetA); diff --git a/DemoRenderer/Constraints/BallSocketMotorLineExtractor.cs b/DemoRenderer/Constraints/BallSocketMotorLineExtractor.cs index b283de84c..65b6d47b7 100644 --- a/DemoRenderer/Constraints/BallSocketMotorLineExtractor.cs +++ b/DemoRenderer/Constraints/BallSocketMotorLineExtractor.cs @@ -14,8 +14,8 @@ public unsafe void ExtractLines(ref BallSocketMotorPrestepData prestepBundle, in Bodies bodies, ref Vector3 tint, ref QuickList 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]]; + ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; + ref var poseB = ref bodies.Sets[setIndex].MotionStates[bodyIndices[1]].Pose; Vector3Wide.ReadFirst(prestepBundle.LocalOffsetB, out var localOffsetB); QuaternionEx.Transform(localOffsetB, poseB.Orientation, out var worldOffsetB); var anchor = poseB.Position + worldOffsetB; diff --git a/DemoRenderer/Constraints/BallSocketServoLineExtractor.cs b/DemoRenderer/Constraints/BallSocketServoLineExtractor.cs index 276c06158..9f04324fd 100644 --- a/DemoRenderer/Constraints/BallSocketServoLineExtractor.cs +++ b/DemoRenderer/Constraints/BallSocketServoLineExtractor.cs @@ -14,8 +14,8 @@ public unsafe void ExtractLines(ref BallSocketServoPrestepData prestepBundle, in Bodies bodies, ref Vector3 tint, ref QuickList 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]]; + ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; + ref var poseB = ref bodies.Sets[setIndex].MotionStates[bodyIndices[1]].Pose; Vector3Wide.ReadFirst(prestepBundle.LocalOffsetA, out var localOffsetA); Vector3Wide.ReadFirst(prestepBundle.LocalOffsetB, out var localOffsetB); QuaternionEx.Transform(localOffsetA, poseA.Orientation, out var worldOffsetA); diff --git a/DemoRenderer/Constraints/CenterDistanceLineExtractor.cs b/DemoRenderer/Constraints/CenterDistanceLineExtractor.cs index 2ee8d7662..99f0f155f 100644 --- a/DemoRenderer/Constraints/CenterDistanceLineExtractor.cs +++ b/DemoRenderer/Constraints/CenterDistanceLineExtractor.cs @@ -15,8 +15,8 @@ public unsafe void ExtractLines(ref CenterDistancePrestepData prestepBundle, int Bodies bodies, ref Vector3 tint, ref QuickList 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]]; + ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; + ref var poseB = ref bodies.Sets[setIndex].MotionStates[bodyIndices[1]].Pose; var targetDistance = GatherScatter.GetFirst(ref prestepBundle.TargetDistance); var color = new Vector3(0.2f, 0.2f, 1f) * tint; var packedColor = Helpers.PackColor(color); diff --git a/DemoRenderer/Constraints/ContactLineExtractors.cs b/DemoRenderer/Constraints/ContactLineExtractors.cs index 0b614bd34..f8c127440 100644 --- a/DemoRenderer/Constraints/ContactLineExtractors.cs +++ b/DemoRenderer/Constraints/ContactLineExtractors.cs @@ -14,7 +14,7 @@ struct Contact1OneBodyLineExtractor : IConstraintLineExtractor lines) { - var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]]; + ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; ContactLines.Add(poseA, ref prestepBundle.Contact0.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact0.Depth, tint, ref lines); } } @@ -25,7 +25,7 @@ struct Contact2OneBodyLineExtractor : IConstraintLineExtractor lines) { - var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]]; + ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; ContactLines.Add(poseA, ref prestepBundle.Contact0.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact0.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact1.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact1.Depth, tint, ref lines); } @@ -37,7 +37,7 @@ struct Contact3OneBodyLineExtractor : IConstraintLineExtractor lines) { - var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]]; + ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; ContactLines.Add(poseA, ref prestepBundle.Contact0.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact0.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact1.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact1.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact2.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact2.Depth, tint, ref lines); @@ -50,7 +50,7 @@ struct Contact4OneBodyLineExtractor : IConstraintLineExtractor lines) { - var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]]; + ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; ContactLines.Add(poseA, ref prestepBundle.Contact0.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact0.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact1.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact1.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact2.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact2.Depth, tint, ref lines); @@ -64,7 +64,7 @@ struct Contact1LineExtractor : IConstraintLineExtractor public unsafe void ExtractLines(ref Contact1PrestepData prestepBundle, int setIndex, int* bodyIndices, Bodies bodies, ref Vector3 tint, ref QuickList lines) { - var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]]; + ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; ContactLines.Add(poseA, ref prestepBundle.Contact0.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact0.Depth, tint, ref lines); } } @@ -75,7 +75,7 @@ struct Contact2LineExtractor : IConstraintLineExtractor public unsafe void ExtractLines(ref Contact2PrestepData prestepBundle, int setIndex, int* bodyIndices, Bodies bodies, ref Vector3 tint, ref QuickList lines) { - var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]]; + ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; ContactLines.Add(poseA, ref prestepBundle.Contact0.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact0.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact1.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact1.Depth, tint, ref lines); } @@ -87,7 +87,7 @@ struct Contact3LineExtractor : IConstraintLineExtractor public unsafe void ExtractLines(ref Contact3PrestepData prestepBundle, int setIndex, int* bodyIndices, Bodies bodies, ref Vector3 tint, ref QuickList lines) { - var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]]; + ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; ContactLines.Add(poseA, ref prestepBundle.Contact0.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact0.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact1.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact1.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact2.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact2.Depth, tint, ref lines); @@ -100,7 +100,7 @@ struct Contact4LineExtractor : IConstraintLineExtractor public unsafe void ExtractLines(ref Contact4PrestepData prestepBundle, int setIndex, int* bodyIndices, Bodies bodies, ref Vector3 tint, ref QuickList lines) { - var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]]; + ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; ContactLines.Add(poseA, ref prestepBundle.Contact0.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact0.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact1.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact1.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact2.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact2.Depth, tint, ref lines); @@ -114,7 +114,7 @@ struct Contact2NonconvexOneBodyLineExtractor : IConstraintLineExtractor lines) { - var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]]; + ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; ContactLines.Add(poseA, ref prestepBundle.Contact0.Offset, ref prestepBundle.Contact0.Normal, ref prestepBundle.Contact0.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact1.Offset, ref prestepBundle.Contact1.Normal, ref prestepBundle.Contact1.Depth, tint, ref lines); } @@ -126,7 +126,7 @@ struct Contact3NonconvexOneBodyLineExtractor : IConstraintLineExtractor lines) { - var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]]; + ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; ContactLines.Add(poseA, ref prestepBundle.Contact0.Offset, ref prestepBundle.Contact0.Normal, ref prestepBundle.Contact0.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact1.Offset, ref prestepBundle.Contact1.Normal, ref prestepBundle.Contact1.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact2.Offset, ref prestepBundle.Contact2.Normal, ref prestepBundle.Contact2.Depth, tint, ref lines); @@ -139,7 +139,7 @@ struct Contact4NonconvexOneBodyLineExtractor : IConstraintLineExtractor lines) { - var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]]; + ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; ContactLines.Add(poseA, ref prestepBundle.Contact0.Offset, ref prestepBundle.Contact0.Normal, ref prestepBundle.Contact0.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact1.Offset, ref prestepBundle.Contact1.Normal, ref prestepBundle.Contact1.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact2.Offset, ref prestepBundle.Contact2.Normal, ref prestepBundle.Contact2.Depth, tint, ref lines); @@ -153,7 +153,7 @@ struct Contact2NonconvexLineExtractor : IConstraintLineExtractor lines) { - var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]]; + ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; ContactLines.Add(poseA, ref prestepBundle.Contact0.Offset, ref prestepBundle.Contact0.Normal, ref prestepBundle.Contact0.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact1.Offset, ref prestepBundle.Contact1.Normal, ref prestepBundle.Contact1.Depth, tint, ref lines); } @@ -165,7 +165,7 @@ struct Contact3NonconvexLineExtractor : IConstraintLineExtractor lines) { - var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]]; + ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; ContactLines.Add(poseA, ref prestepBundle.Contact0.Offset, ref prestepBundle.Contact0.Normal, ref prestepBundle.Contact0.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact1.Offset, ref prestepBundle.Contact1.Normal, ref prestepBundle.Contact1.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact2.Offset, ref prestepBundle.Contact2.Normal, ref prestepBundle.Contact2.Depth, tint, ref lines); @@ -178,7 +178,7 @@ struct Contact4NonconvexLineExtractor : IConstraintLineExtractor lines) { - var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]]; + ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; ContactLines.Add(poseA, ref prestepBundle.Contact0.Offset, ref prestepBundle.Contact0.Normal, ref prestepBundle.Contact0.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact1.Offset, ref prestepBundle.Contact1.Normal, ref prestepBundle.Contact1.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact2.Offset, ref prestepBundle.Contact2.Normal, ref prestepBundle.Contact2.Depth, tint, ref lines); diff --git a/DemoRenderer/Constraints/ContactLineExtractors.tt b/DemoRenderer/Constraints/ContactLineExtractors.tt index 1bfaa38ab..189f67641 100644 --- a/DemoRenderer/Constraints/ContactLineExtractors.tt +++ b/DemoRenderer/Constraints/ContactLineExtractors.tt @@ -31,7 +31,7 @@ for (int convexity = 0; convexity <= 1; ++convexity) public unsafe void ExtractLines(ref Contact<#=contactCount#><#=convexitySuffix#><#=bodySuffix#>PrestepData prestepBundle, int setIndex, int* bodyIndices, Bodies bodies, ref Vector3 tint, ref QuickList lines) { - var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]]; + ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; <#for (int i = 0; i < contactCount; ++i) { if(convex) {#> ContactLines.Add(poseA, ref prestepBundle.Contact<#=i#>.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact<#=i#>.Depth, tint, ref lines); <#} else {#> diff --git a/DemoRenderer/Constraints/DistanceLimitLineExtractor.cs b/DemoRenderer/Constraints/DistanceLimitLineExtractor.cs index deb985b28..7e85b96ca 100644 --- a/DemoRenderer/Constraints/DistanceLimitLineExtractor.cs +++ b/DemoRenderer/Constraints/DistanceLimitLineExtractor.cs @@ -14,8 +14,8 @@ public unsafe void ExtractLines(ref DistanceLimitPrestepData prestepBundle, int Bodies bodies, ref Vector3 tint, ref QuickList 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]]; + ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; + ref var poseB = ref bodies.Sets[setIndex].MotionStates[bodyIndices[1]].Pose; Vector3Wide.ReadFirst(prestepBundle.LocalOffsetA, out var localOffsetA); Vector3Wide.ReadFirst(prestepBundle.LocalOffsetB, out var localOffsetB); var minimumDistance = GatherScatter.GetFirst(ref prestepBundle.MinimumDistance); diff --git a/DemoRenderer/Constraints/DistanceServoLineExtractor.cs b/DemoRenderer/Constraints/DistanceServoLineExtractor.cs index 612d97d74..4eb058ef8 100644 --- a/DemoRenderer/Constraints/DistanceServoLineExtractor.cs +++ b/DemoRenderer/Constraints/DistanceServoLineExtractor.cs @@ -14,8 +14,8 @@ public unsafe void ExtractLines(ref DistanceServoPrestepData prestepBundle, int Bodies bodies, ref Vector3 tint, ref QuickList 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]]; + ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; + ref var poseB = ref bodies.Sets[setIndex].MotionStates[bodyIndices[1]].Pose; Vector3Wide.ReadFirst(prestepBundle.LocalOffsetA, out var localOffsetA); Vector3Wide.ReadFirst(prestepBundle.LocalOffsetB, out var localOffsetB); var targetDistance = GatherScatter.GetFirst(ref prestepBundle.TargetDistance); diff --git a/DemoRenderer/Constraints/HingeLineExtractor.cs b/DemoRenderer/Constraints/HingeLineExtractor.cs index a0aaf920e..7aa8163ac 100644 --- a/DemoRenderer/Constraints/HingeLineExtractor.cs +++ b/DemoRenderer/Constraints/HingeLineExtractor.cs @@ -14,8 +14,8 @@ public unsafe void ExtractLines(ref HingePrestepData prestepBundle, int setIndex Bodies bodies, ref Vector3 tint, ref QuickList lines) { //Could do bundles of constraints at a time, but eh. - ref var poseA = ref bodies.Sets[setIndex].Poses[bodyIndices[0]]; - ref var poseB = ref bodies.Sets[setIndex].Poses[bodyIndices[1]]; + ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; + ref var poseB = ref bodies.Sets[setIndex].MotionStates[bodyIndices[1]].Pose; Vector3Wide.ReadFirst(prestepBundle.LocalHingeAxisA, out var localHingeAxisA); Vector3Wide.ReadFirst(prestepBundle.LocalOffsetA, out var localOffsetA); Vector3Wide.ReadFirst(prestepBundle.LocalHingeAxisB, out var localHingeAxisB); diff --git a/DemoRenderer/Constraints/LinearAxisServoLineExtractor.cs b/DemoRenderer/Constraints/LinearAxisServoLineExtractor.cs index 41e0c98ea..b5fe69780 100644 --- a/DemoRenderer/Constraints/LinearAxisServoLineExtractor.cs +++ b/DemoRenderer/Constraints/LinearAxisServoLineExtractor.cs @@ -14,8 +14,8 @@ public unsafe void ExtractLines(ref LinearAxisServoPrestepData prestepBundle, in Bodies bodies, ref Vector3 tint, ref QuickList 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]]; + ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; + ref var poseB = ref bodies.Sets[setIndex].MotionStates[bodyIndices[1]].Pose; Vector3Wide.ReadFirst(prestepBundle.LocalOffsetA, out var localOffsetA); Vector3Wide.ReadFirst(prestepBundle.LocalOffsetB, out var localOffsetB); Vector3Wide.ReadFirst(prestepBundle.LocalPlaneNormal, out var localPlaneNormal); diff --git a/DemoRenderer/Constraints/OneBodyLinearServoLineExtractor.cs b/DemoRenderer/Constraints/OneBodyLinearServoLineExtractor.cs index 52a5cdbb9..84c66e3ba 100644 --- a/DemoRenderer/Constraints/OneBodyLinearServoLineExtractor.cs +++ b/DemoRenderer/Constraints/OneBodyLinearServoLineExtractor.cs @@ -14,7 +14,7 @@ public unsafe void ExtractLines(ref OneBodyLinearServoPrestepData prestepBundle, Bodies bodies, ref Vector3 tint, ref QuickList lines) { //Could do bundles of constraints at a time, but eh. - var pose = bodies.Sets[setIndex].Poses[*bodyIndices]; + ref var pose = ref bodies.Sets[setIndex].MotionStates[*bodyIndices].Pose; Vector3Wide.ReadFirst(prestepBundle.LocalOffset, out var localOffset); Vector3Wide.ReadFirst(prestepBundle.Target, out var target); QuaternionEx.Transform(localOffset, pose.Orientation, out var worldOffset); diff --git a/DemoRenderer/Constraints/PointOnLineLineExtractor.cs b/DemoRenderer/Constraints/PointOnLineLineExtractor.cs index 67c41272f..6bbd59988 100644 --- a/DemoRenderer/Constraints/PointOnLineLineExtractor.cs +++ b/DemoRenderer/Constraints/PointOnLineLineExtractor.cs @@ -14,8 +14,8 @@ public unsafe void ExtractLines(ref PointOnLineServoPrestepData prestepBundle, i Bodies bodies, ref Vector3 tint, ref QuickList 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]]; + ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; + ref var poseB = ref bodies.Sets[setIndex].MotionStates[bodyIndices[1]].Pose; Vector3Wide.ReadFirst(prestepBundle.LocalOffsetA, out var localOffsetA); Vector3Wide.ReadFirst(prestepBundle.LocalOffsetB, out var localOffsetB); Vector3Wide.ReadFirst(prestepBundle.LocalDirection, out var localDirection); diff --git a/DemoRenderer/Constraints/SwivelHingeLineExtractor.cs b/DemoRenderer/Constraints/SwivelHingeLineExtractor.cs index 3c6412c09..f880afa27 100644 --- a/DemoRenderer/Constraints/SwivelHingeLineExtractor.cs +++ b/DemoRenderer/Constraints/SwivelHingeLineExtractor.cs @@ -14,8 +14,8 @@ public unsafe void ExtractLines(ref SwivelHingePrestepData prestepBundle, int se Bodies bodies, ref Vector3 tint, ref QuickList lines) { //Could do bundles of constraints at a time, but eh. - ref var poseA = ref bodies.Sets[setIndex].Poses[bodyIndices[0]]; - ref var poseB = ref bodies.Sets[setIndex].Poses[bodyIndices[1]]; + ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; + ref var poseB = ref bodies.Sets[setIndex].MotionStates[bodyIndices[1]].Pose; Vector3Wide.ReadFirst(prestepBundle.LocalSwivelAxisA, out var localSwivelAxisA); Vector3Wide.ReadFirst(prestepBundle.LocalOffsetA, out var localOffsetA); Vector3Wide.ReadFirst(prestepBundle.LocalHingeAxisB, out var localHingeAxisB); diff --git a/DemoRenderer/Constraints/WeldLineExtractor.cs b/DemoRenderer/Constraints/WeldLineExtractor.cs index fec7ab02b..36d76f577 100644 --- a/DemoRenderer/Constraints/WeldLineExtractor.cs +++ b/DemoRenderer/Constraints/WeldLineExtractor.cs @@ -14,8 +14,8 @@ public unsafe void ExtractLines(ref WeldPrestepData prestepBundle, int setIndex, Bodies bodies, ref Vector3 tint, ref QuickList 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]]; + ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; + ref var poseB = ref bodies.Sets[setIndex].MotionStates[bodyIndices[1]].Pose; Vector3Wide.ReadFirst(prestepBundle.LocalOffset, out var localOffset); QuaternionEx.Transform(localOffset, poseA.Orientation, out var worldOffset); var bTarget = poseA.Position + worldOffset; diff --git a/DemoRenderer/ShapeDrawing/ShapesExtractor.cs b/DemoRenderer/ShapeDrawing/ShapesExtractor.cs index 04bbfb316..1e0addeab 100644 --- a/DemoRenderer/ShapeDrawing/ShapesExtractor.cs +++ b/DemoRenderer/ShapeDrawing/ShapesExtractor.cs @@ -266,7 +266,7 @@ void AddBodyShape(Shapes shapes, Bodies bodies, int setIndex, int indexInSet) color *= sleepTint; } - AddShape(shapes, set.Collidables[indexInSet].Shape, ref set.Poses[indexInSet], color); + AddShape(shapes, set.Collidables[indexInSet].Shape, ref set.MotionStates[indexInSet].Pose, color); } [MethodImpl(MethodImplOptions.AggressiveInlining)] diff --git a/DemoTests/ConstraintDescriptionMappingTests.cs b/DemoTests/ConstraintDescriptionMappingTests.cs index ad86fef63..7687662b5 100644 --- a/DemoTests/ConstraintDescriptionMappingTests.cs +++ b/DemoTests/ConstraintDescriptionMappingTests.cs @@ -31,7 +31,7 @@ static void Test(BufferPool pool, Random random, int constraintTypeBodyCount) const int bodyCount = 2048; for (int i = 0; i < bodyCount; ++i) { - var bodyDescription = new BodyDescription { LocalInertia = new BodyInertia { InverseMass = 1 }, Pose = new RigidPose { Orientation = Quaternion.Identity } }; + var bodyDescription = new BodyDescription { LocalInertia = new BodyInertia { InverseMass = 1 }, Pose = RigidPose.Identity }; simulation.Bodies.Add(bodyDescription); } diff --git a/DemoTests/TestUtilities.cs b/DemoTests/TestUtilities.cs index 542fb6c66..8ad454fa6 100644 --- a/DemoTests/TestUtilities.cs +++ b/DemoTests/TestUtilities.cs @@ -45,8 +45,9 @@ public static long ComputeHash(ref Vector3 v, long constant) { for (int bodyIndex = 0; bodyIndex < set.Count; ++bodyIndex) { - ref var pose = ref set.Poses[bodyIndex]; - ref var velocity = ref set.Velocities[bodyIndex]; + ref var state = ref set.MotionStates[bodyIndex]; + ref var pose = ref state.Pose; + ref var velocity = ref state.Velocity; var poseHash = ComputeHash(ref pose.Position, 89) + ComputeHash(ref pose.Orientation.X, 107) + ComputeHash(ref pose.Orientation.Y, 113) + ComputeHash(ref pose.Orientation.Z, 131) + ComputeHash(ref pose.Orientation.W, 149); var velocityHash = ComputeHash(ref velocity.Linear, 211) + ComputeHash(ref velocity.Angular, 397); hash += set.IndexToHandle[bodyIndex].Value * (poseHash + velocityHash); diff --git a/Demos/Demo.cs b/Demos/Demo.cs index f00656836..aba8044d8 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -62,7 +62,7 @@ public virtual void Update(Window window, Camera camera, Input input, float dt) //fully decouple simulation and rendering rates across different threads. //(In either case, you'd also want to interpolate or extrapolate simulation results during rendering for smoothness.) //Note that taking steps of variable length can reduce stability. Gradual or one-off changes can work reasonably well. - Simulation.Timestep(1 / 60f, ThreadDispatcher); + Simulation.Timestep(1 / 60f); ////Here's an example of how it would look to use more frequent updates, but still with a fixed amount of time simulated per update call: //const float timeToSimulate = 1 / 60f; diff --git a/Demos/Demos/BlockChainDemo.cs b/Demos/Demos/BlockChainDemo.cs index c95690d12..d6aa97b02 100644 --- a/Demos/Demos/BlockChainDemo.cs +++ b/Demos/Demos/BlockChainDemo.cs @@ -87,9 +87,10 @@ public override void Update(Window window, Camera camera, Input input, float dt) else direction = new Vector3(0, 1, 0); - coinDescription.Pose.Position = origin + direction * 10 * (float)random.NextDouble(); - coinDescription.Pose.Orientation = QuaternionEx.Normalize(new Quaternion(0.01f + (float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble())); - coinDescription.Velocity.Linear = direction * (5 + 30 * (float)random.NextDouble()); + coinDescription.Pose = new( + origin + direction * 10 * (float)random.NextDouble(), + QuaternionEx.Normalize(new Quaternion(0.01f + (float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()))); + coinDescription.Velocity = new(direction * (5 + 30 * (float)random.NextDouble())); Simulation.Bodies.Add(coinDescription); } } diff --git a/Demos/Demos/Characters/CharacterControllers.cs b/Demos/Demos/Characters/CharacterControllers.cs index bcb474824..0c4a03a65 100644 --- a/Demos/Demos/Characters/CharacterControllers.cs +++ b/Demos/Demos/Characters/CharacterControllers.cs @@ -279,7 +279,7 @@ bool TryReportContacts(CollidableReference characterCollidable, Colli //Have to take into account the current potentially inactive location. ref var bodyLocation = ref Simulation.Bodies.HandleToLocation[character.BodyHandle.Value]; ref var set = ref Simulation.Bodies.Sets[bodyLocation.SetIndex]; - ref var pose = ref set.Poses[bodyLocation.Index]; + ref var pose = ref set.MotionStates[bodyLocation.Index].Pose; QuaternionEx.Transform(character.LocalUp, pose.Orientation, out var up); //Note that this branch is compiled out- the generic constraints force type specialization. if (manifold.Convex) @@ -594,16 +594,16 @@ void AnalyzeContactsForCharacterRegion(int start, int exclusiveEnd, int workerIn //If the character is jumping, don't create a constraint. if (supportCandidate.Depth > float.MinValue && character.TryJump) { - QuaternionEx.Transform(character.LocalUp, Simulation.Bodies.ActiveSet.Poses[bodyLocation.Index].Orientation, out var characterUp); + QuaternionEx.Transform(character.LocalUp, Simulation.Bodies.ActiveSet.MotionStates[bodyLocation.Index].Pose.Orientation, out var characterUp); //Note that we assume that character orientations are constant. This isn't necessarily the case in all uses, but it's a decent approximation. - var characterUpVelocity = Vector3.Dot(Simulation.Bodies.ActiveSet.Velocities[bodyLocation.Index].Linear, characterUp); + var characterUpVelocity = Vector3.Dot(Simulation.Bodies.ActiveSet.MotionStates[bodyLocation.Index].Velocity.Linear, characterUp); //We don't want the character to be able to 'superboost' by simply adding jump speed on top of horizontal motion. //Instead, jumping targets a velocity change necessary to reach character.JumpVelocity along the up axis. if (character.Support.Mobility != CollidableMobility.Static) { ref var supportingBodyLocation = ref Simulation.Bodies.HandleToLocation[character.Support.BodyHandle.Value]; Debug.Assert(supportingBodyLocation.SetIndex == 0, "If the character is active, any support should be too."); - ref var supportVelocity = ref Simulation.Bodies.ActiveSet.Velocities[supportingBodyLocation.Index]; + ref var supportVelocity = ref Simulation.Bodies.ActiveSet.MotionStates[supportingBodyLocation.Index].Velocity; var wxr = Vector3.Cross(supportVelocity.Angular, supportCandidate.OffsetFromSupport); var supportContactVelocity = supportVelocity.Linear + wxr; var supportUpVelocity = Vector3.Dot(supportContactVelocity, characterUp); @@ -644,7 +644,7 @@ void AnalyzeContactsForCharacterRegion(int start, int exclusiveEnd, int workerIn Matrix3x3 surfaceBasis; surfaceBasis.Y = supportCandidate.Normal; //Note negation: we're using a right handed basis where -Z is forward, +Z is backward. - QuaternionEx.Transform(character.LocalUp, Simulation.Bodies.ActiveSet.Poses[bodyLocation.Index].Orientation, out var up); + QuaternionEx.Transform(character.LocalUp, Simulation.Bodies.ActiveSet.MotionStates[bodyLocation.Index].Pose.Orientation, out var up); var rayDistance = Vector3.Dot(character.ViewDirection, surfaceBasis.Y); var rayVelocity = Vector3.Dot(up, surfaceBasis.Y); Debug.Assert(rayVelocity > 0, @@ -828,7 +828,7 @@ void AnalyzeContacts(float dt, IThreadDispatcher threadDispatcher) for (int i = 0; i < workerCache.Jumps.Count; ++i) { ref var jump = ref workerCache.Jumps[i]; - activeSet.Velocities[jump.CharacterBodyIndex].Linear += jump.CharacterVelocityChange; + activeSet.MotionStates[jump.CharacterBodyIndex].Velocity.Linear += jump.CharacterVelocityChange; if (jump.SupportBodyIndex >= 0) { BodyReference.ApplyImpulse(Simulation.Bodies.ActiveSet, jump.SupportBodyIndex, jump.CharacterVelocityChange / -activeSet.LocalInertias[jump.CharacterBodyIndex].InverseMass, jump.SupportImpulseOffset); diff --git a/Demos/Demos/Characters/CharacterMotionConstraint.cs b/Demos/Demos/Characters/CharacterMotionConstraint.cs index b86ffabba..9e03162aa 100644 --- a/Demos/Demos/Characters/CharacterMotionConstraint.cs +++ b/Demos/Demos/Characters/CharacterMotionConstraint.cs @@ -166,7 +166,8 @@ static void ComputeJacobians(in Vector3Wide offsetA, in QuaternionWide basisQuat } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(Bodies bodies, ref Vector bodyReferences, int count, float dt, float inverseDt, ref BodyInertias inertiaA, ref StaticCharacterMotionPrestep prestepData, out StaticCharacterMotionProjection projection) + public void Prestep( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertias inertiaA, float dt, float inverseDt, ref StaticCharacterMotionPrestep prestepData, out StaticCharacterMotionProjection projection) { //The motion constraint is split into two parts: the horizontal constraint, and the vertical constraint. //The horizontal constraint acts almost exactly like the TangentFriction, but we'll duplicate some of the logic to keep this implementation self-contained. @@ -473,7 +474,8 @@ static void ComputeJacobians(in Vector3Wide offsetA, in Vector3Wide offsetB, in } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int count, float dt, float inverseDt, ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref DynamicCharacterMotionPrestep prestepData, out DynamicCharacterMotionProjection projection) + public void Prestep( + in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, float dt, float inverseDt, ref DynamicCharacterMotionPrestep prestepData, out DynamicCharacterMotionProjection projection) { //The motion constraint is split into two parts: the horizontal constraint, and the vertical constraint. //The horizontal constraint acts almost exactly like the TangentFriction, but we'll duplicate some of the logic to keep this implementation self-contained. diff --git a/Demos/Demos/Characters/CharacterMotionConstraint.tt b/Demos/Demos/Characters/CharacterMotionConstraint.tt index c627c2259..c724350e2 100644 --- a/Demos/Demos/Characters/CharacterMotionConstraint.tt +++ b/Demos/Demos/Characters/CharacterMotionConstraint.tt @@ -204,7 +204,8 @@ namespace Demos.Demos.Characters } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(Bodies bodies, ref <#=dynamic ? "TwoBodyReferences" : "Vector"#> bodyReferences, int count, float dt, float inverseDt, ref BodyInertias inertiaA, <#if (dynamic) {#>ref BodyInertias inertiaB, <#}#>ref <#=prefix#>CharacterMotionPrestep prestepData, out <#=prefix#>CharacterMotionProjection projection) + public void Prestep( + <#if(!dynamic) {#>in Vector3Wide positionA,<#}#> in QuaternionWide orientationA, in BodyInertias inertiaA, <#if(dynamic) {#>in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, <#}#>float dt, float inverseDt, ref <#=prefix#>CharacterMotionPrestep prestepData, out <#=prefix#>CharacterMotionProjection projection) { //The motion constraint is split into two parts: the horizontal constraint, and the vertical constraint. //The horizontal constraint acts almost exactly like the TangentFriction, but we'll duplicate some of the logic to keep this implementation self-contained. diff --git a/Demos/Demos/CompoundTestDemo.cs b/Demos/Demos/CompoundTestDemo.cs index 8a22ab1a1..a619ac8c9 100644 --- a/Demos/Demos/CompoundTestDemo.cs +++ b/Demos/Demos/CompoundTestDemo.cs @@ -96,7 +96,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) SpeculativeMargin = 0.1f, }, LocalInertia = tableInertia, - Pose = new RigidPose { Orientation = Quaternion.Identity } + Pose = RigidPose.Identity }; //Stack some tables. diff --git a/Demos/Demos/FountainStressTestDemo.cs b/Demos/Demos/FountainStressTestDemo.cs index 1a5032b9c..1d12db084 100644 --- a/Demos/Demos/FountainStressTestDemo.cs +++ b/Demos/Demos/FountainStressTestDemo.cs @@ -260,7 +260,7 @@ public override void Update(Window window, Camera camera, Input input, float dt) 16 + 16 * (float)Math.Cos(4 * (angle + t * 0.5)), radius * (float)Math.Sin(positionAngle)); - var correction = targetLocation - set.Poses[bodyLocation.Index].Position; + var correction = targetLocation - set.MotionStates[bodyLocation.Index].Pose.Position; var distance = correction.Length(); if (distance > 1e-4) { @@ -274,13 +274,13 @@ public override void Update(Window window, Camera camera, Input input, float dt) correction *= maxDisplacement / distance; } Debug.Assert(bodyLocation.SetIndex == 0); - Simulation.Bodies.ActiveSet.Velocities[bodyLocation.Index].Linear = correction * inverseDt; + Simulation.Bodies.ActiveSet.MotionStates[bodyLocation.Index].Velocity.Linear = correction * inverseDt; } else { if (bodyLocation.SetIndex == 0) { - Simulation.Bodies.ActiveSet.Velocities[bodyLocation.Index].Linear = new Vector3(); + Simulation.Bodies.ActiveSet.MotionStates[bodyLocation.Index].Velocity.Linear = new Vector3(); } } } diff --git a/Demos/SpecializedTests/CapsuleTestDemo.cs b/Demos/SpecializedTests/CapsuleTestDemo.cs index 452144035..cc0dc3b01 100644 --- a/Demos/SpecializedTests/CapsuleTestDemo.cs +++ b/Demos/SpecializedTests/CapsuleTestDemo.cs @@ -57,11 +57,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) { Activity = new BodyActivityDescription { MinimumTimestepCountUnderThreshold = 32, SleepThreshold = -0.01f }, LocalInertia = boxLocalInertia, - Pose = new RigidPose - { - Orientation = QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), 0), - Position = new Vector3(1, -0.5f, 0) - }, + Pose = new(new(1, -0.5f, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), 0)), Collidable = new CollidableDescription { SpeculativeMargin = 50.1f, Shape = Simulation.Shapes.Add(boxShape) } }; Simulation.Bodies.Add(boxDescription); diff --git a/Demos/SpecializedTests/CylinderTestDemo.cs b/Demos/SpecializedTests/CylinderTestDemo.cs index e5ed8e794..143dd131e 100644 --- a/Demos/SpecializedTests/CylinderTestDemo.cs +++ b/Demos/SpecializedTests/CylinderTestDemo.cs @@ -220,16 +220,12 @@ public override void Initialize(ContentArchive content, Camera camera) { for (int k = 0; k < length; ++k) { - var location = new Vector3(5, 3,5) * new Vector3(i, j, k) + new Vector3(-width * 1.5f, 2.5f, -30 - length * 1.5f); + var location = new Vector3(5, 3, 5) * new Vector3(i, j, k) + new Vector3(-width * 1.5f, 2.5f, -30 - length * 1.5f); var bodyDescription = new BodyDescription { Activity = new BodyActivityDescription(-0.01f), - Pose = new RigidPose - { - Orientation = Quaternion.Identity,// Quaternion.CreateFromAxisAngle(Vector3.Normalize(new Vector3(1, 1, 1)), MathF.PI * 0.79813f), - Position = location - }, - Collidable = new CollidableDescription + Pose = new(location), + Collidable = { Continuity = new ContinuousDetectionSettings { Mode = ContinuousDetectionMode.Discrete }, SpeculativeMargin = 0.1f diff --git a/Demos/SpecializedTests/DeterminismTest.cs b/Demos/SpecializedTests/DeterminismTest.cs index 77da433a0..4df939ef8 100644 --- a/Demos/SpecializedTests/DeterminismTest.cs +++ b/Demos/SpecializedTests/DeterminismTest.cs @@ -36,7 +36,7 @@ static Dictionary ExecuteSimulation(ContentArchive content, in { for (int bodyIndex = 0; bodyIndex < set.Count; ++bodyIndex) { - motionStates.Add(set.IndexToHandle[bodyIndex].Value, new MotionState { Pose = set.Poses[bodyIndex], Velocity = set.Velocities[bodyIndex] }); + motionStates.Add(set.IndexToHandle[bodyIndex].Value, new MotionState { Pose = set.MotionStates[bodyIndex].Pose, Velocity = set.MotionStates[bodyIndex].Velocity }); } } } diff --git a/Demos/SpecializedTests/MeshTestDemo.cs b/Demos/SpecializedTests/MeshTestDemo.cs index d8559f5a9..6c418850e 100644 --- a/Demos/SpecializedTests/MeshTestDemo.cs +++ b/Demos/SpecializedTests/MeshTestDemo.cs @@ -43,11 +43,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var bodyDescription = new BodyDescription { Activity = new BodyActivityDescription { MinimumTimestepCountUnderThreshold = 32, SleepThreshold = 0.01f }, - Pose = new RigidPose - { - Orientation = Quaternion.Identity, - Position = location - }, + Pose = new(location), Collidable = new CollidableDescription { Continuity = new ContinuousDetectionSettings { Mode = ContinuousDetectionMode.Discrete }, diff --git a/Demos/SpecializedTests/TestHelpers.cs b/Demos/SpecializedTests/TestHelpers.cs index b2ecb521a..e674d0e0b 100644 --- a/Demos/SpecializedTests/TestHelpers.cs +++ b/Demos/SpecializedTests/TestHelpers.cs @@ -17,8 +17,8 @@ public static float GetBodyEnergyHeuristic(Bodies bodies) float accumulated = 0; for (int index = 0; index < bodies.ActiveSet.Count; ++index) { - accumulated += Vector3.Dot(bodies.ActiveSet.Velocities[index].Linear, bodies.ActiveSet.Velocities[index].Linear); - accumulated += Vector3.Dot(bodies.ActiveSet.Velocities[index].Angular, bodies.ActiveSet.Velocities[index].Angular); + accumulated += Vector3.Dot(bodies.ActiveSet.MotionStates[index].Velocity.Linear, bodies.ActiveSet.MotionStates[index].Velocity.Linear); + accumulated += Vector3.Dot(bodies.ActiveSet.MotionStates[index].Velocity.Angular, bodies.ActiveSet.MotionStates[index].Velocity.Angular); } return accumulated; } From a2f23f7c25eca4d0f9fc24659ca8a97c78bf71a9 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 29 Jun 2021 16:24:21 -0500 Subject: [PATCH 047/947] Fixed bug in inertia gather. Overall, changes made it ever so slightly slower; now to refine. --- BepuPhysics/Bodies.cs | 3 ++- Demos/Demo.cs | 2 +- Demos/Demos/NewtDemo.cs | 6 +++--- Demos/Program.cs | 2 +- 4 files changed, 7 insertions(+), 6 deletions(-) diff --git a/BepuPhysics/Bodies.cs b/BepuPhysics/Bodies.cs index ed5e8bde3..efa3ce042 100644 --- a/BepuPhysics/Bodies.cs +++ b/BepuPhysics/Bodies.cs @@ -525,9 +525,10 @@ internal void ValidateMotionStates() } [MethodImpl(MethodImplOptions.AggressiveInlining)] - private static void WriteGatherInertia(ref int bundleBaseBodyIndexInSet, int bodyIndexInBundle, ref Buffer states, ref BodyInertias targetSlot) + private static void WriteGatherInertia(ref int bundleBaseBodyIndexInSet, int bodyIndexInBundle, ref Buffer states, ref BodyInertias gatheredInertias) { ref var source = ref states[Unsafe.Add(ref bundleBaseBodyIndexInSet, bodyIndexInBundle)]; + ref var targetSlot = ref GetOffsetInstance(ref gatheredInertias, bodyIndexInBundle); GetFirst(ref targetSlot.InverseInertiaTensor.XX) = source.InverseInertiaTensor.XX; GetFirst(ref targetSlot.InverseInertiaTensor.YX) = source.InverseInertiaTensor.YX; GetFirst(ref targetSlot.InverseInertiaTensor.YY) = source.InverseInertiaTensor.YY; diff --git a/Demos/Demo.cs b/Demos/Demo.cs index aba8044d8..f00656836 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -62,7 +62,7 @@ public virtual void Update(Window window, Camera camera, Input input, float dt) //fully decouple simulation and rendering rates across different threads. //(In either case, you'd also want to interpolate or extrapolate simulation results during rendering for smoothness.) //Note that taking steps of variable length can reduce stability. Gradual or one-off changes can work reasonably well. - Simulation.Timestep(1 / 60f); + Simulation.Timestep(1 / 60f, ThreadDispatcher); ////Here's an example of how it would look to use more frequent updates, but still with a fixed amount of time simulated per update call: //const float timeToSimulate = 1 / 60f; diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index eb3da8f1f..4f3f443e2 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -712,8 +712,8 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //The PositionFirstTimestepper is the simplest timestepping mode, but since it integrates velocity into position at the start of the frame, directly modified velocities outside of the timestep //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. - Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper(1)); - //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3)); + //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper(1)); + Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3)); //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper2(), solverIterationCount: 5); //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper2(3), solverIterationCount: 1); @@ -721,7 +721,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) float cellSize = 0.1f; DumbTetrahedralizer.Tetrahedralize(meshContent.Triangles, cellSize, BufferPool, out var vertices, out var vertexSpatialIndices, out var cellVertexIndices, out var tetrahedraVertexIndices); - var weldSpringiness = new SpringSettings(120f, 1f); + var weldSpringiness = new SpringSettings(30f, 1f); var volumeSpringiness = new SpringSettings(30f, 1); for (int i = 0; i < 40; ++i) { diff --git a/Demos/Program.cs b/Demos/Program.cs index 14a613027..ba902a5cf 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -19,7 +19,7 @@ static void Main(string[] args) { content = ContentArchive.Load(stream); } - //HeadlessTest.Test(content, 2, 32, 512); + HeadlessTest.Test(content, 2, 32, 512); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); From c8083d170c417f2e7fcaba27aa179006d82fe982 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 29 Jun 2021 16:36:38 -0500 Subject: [PATCH 048/947] Stronger alignment. --- BepuPhysics/BodyProperties.cs | 19 +++++++++++-------- BepuUtilities/Memory/BufferPool.cs | 13 +++++++------ Demos/Program.cs | 8 ++++++-- 3 files changed, 24 insertions(+), 16 deletions(-) diff --git a/BepuPhysics/BodyProperties.cs b/BepuPhysics/BodyProperties.cs index f4f013a02..3dc131b46 100644 --- a/BepuPhysics/BodyProperties.cs +++ b/BepuPhysics/BodyProperties.cs @@ -5,14 +5,25 @@ using System; using System.Numerics; using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; namespace BepuPhysics { /// /// Describes the pose and velocity of a body. /// + [StructLayout(LayoutKind.Sequential, Size = 64)] public struct MotionState { + /// + /// Pose of the body. + /// + public RigidPose Pose; + /// + /// Linear and angular velocity of the body. + /// + public BodyVelocity Velocity; + /// /// Gets a motion state with zero position, zero velocity, and identity orientation. /// @@ -124,14 +135,6 @@ public MotionState(in Vector3 position, in BodyVelocity velocity) Velocity = velocity; } - /// - /// Pose of the body. - /// - public RigidPose Pose; - /// - /// Linear and angular velocity of the body. - /// - public BodyVelocity Velocity; } //TODO: It's a little odd that this exists alongside the BepuUtilities.RigidTransform. The original reasoning was that rigid poses may end up having a non-FP32 representation. diff --git a/BepuUtilities/Memory/BufferPool.cs b/BepuUtilities/Memory/BufferPool.cs index 03b6d6f69..771a98307 100644 --- a/BepuUtilities/Memory/BufferPool.cs +++ b/BepuUtilities/Memory/BufferPool.cs @@ -23,18 +23,19 @@ unsafe struct Block public Block(int blockSize) { //While the runtime does have some alignment guarantees, we hedge against the possibility that the runtime could change (or another runtime is in use), - //or that the runtime isn't aligning to a size sufficiently large for wide SIMD types. I suspect that the combination of the jit's tendency to use unaligned - //instructions regardless and modern processors' performance on unaligned instructions will make this basically irrelevant, but it costs roughly nothing. + //or that the runtime isn't aligning to a size sufficiently large for wide SIMD types, or some type expects cache line size alignment. + //I suspect that the combination of the jit's tendency to use unaligned instructions regardless and modern processors' performance on unaligned instructions + //will make this *almost* irrelevant, but it costs roughly nothing. //Suballocations from the block will always occur on pow2 boundaries, so the only way for a suballocation to violate this alignment is if an individual //suballocation is smaller than the alignment- in which case it doesn't require the alignment to be that wide. Also, since the alignment and //suballocations are both pow2 sized, they won't drift out of sync. - int alignment = Vector.Count * sizeof(float); - Array = new byte[blockSize + alignment]; + const int cacheLineSize = 64; //Making an assumption here! But it's a reasonably good one. + Array = new byte[blockSize + cacheLineSize]; Handle = GCHandle.Alloc(Array, GCHandleType.Pinned); Pointer = (byte*)Handle.AddrOfPinnedObject(); - var mask = alignment - 1; + var mask = cacheLineSize - 1; var offset = (uint)Pointer & mask; - Pointer += alignment - offset; + Pointer += cacheLineSize - offset; } diff --git a/Demos/Program.cs b/Demos/Program.cs index ba902a5cf..eef4db3bb 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -1,9 +1,12 @@ -using BepuUtilities; +using BepuPhysics; +using BepuUtilities; using DemoContentLoader; using Demos.Demos; using Demos.SpecializedTests; using DemoUtilities; using OpenTK; +using System; +using System.Runtime.CompilerServices; namespace Demos { @@ -11,6 +14,7 @@ class Program { static void Main(string[] args) { + Console.WriteLine($"aasgh: {Unsafe.SizeOf()}"); var window = new Window("pretty cool multicolored window", new Int2((int)(DisplayDevice.Default.Width * 0.75f), (int)(DisplayDevice.Default.Height * 0.75f)), WindowMode.Windowed); var loop = new GameLoop(window); @@ -19,7 +23,7 @@ static void Main(string[] args) { content = ContentArchive.Load(stream); } - HeadlessTest.Test(content, 2, 32, 512); + HeadlessTest.Test(content, 8, 32, 512); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); From 8bc70134d33459c53e814eca1b7741c066aacf1f Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 29 Jun 2021 18:15:40 -0500 Subject: [PATCH 049/947] Prefetcher prototype. Actually helps! --- .../Constraints/TwoBodyTypeProcessor.cs | 85 +++++++++++++++---- 1 file changed, 67 insertions(+), 18 deletions(-) diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index f944d4775..31bbad444 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -5,6 +5,7 @@ using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; +using System.Runtime.Intrinsics.X86; namespace BepuPhysics.Constraints { @@ -175,7 +176,7 @@ public unsafe override void SolveIteration(ref TypeBatch typeBatch, Bodies bodie ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); int count = GetCountInBundle(ref typeBatch, i); bodies.GatherState(ref bodyReferences, count, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB); - function.Solve(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); + function.Solve(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); bodies.ScatterVelocities(ref wsvA, ref wsvB, ref bodyReferences, count); } } @@ -281,7 +282,7 @@ public unsafe override void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodi ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); - var count = GetCountInBundle(ref typeBatch, i); + var count = GetCountInBundle(ref typeBatch, i); ref var wsvA = ref jacobiResultsBundlesA[i]; ref var wsvB = ref jacobiResultsBundlesB[i]; bodies.GatherState(ref references, count, out var orientationA, out wsvA, out var inertiaA, out var ab, out var orientationB, out wsvB, out var inertiaB); @@ -297,42 +298,90 @@ public unsafe override void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodi } } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + static unsafe void Prefetch(void* address) + { + if (Sse.IsSupported) + { + Sse.Prefetch0(address); + } + //TODO: ARM? + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + static unsafe void PrefetchBundle(MotionState* baseAddress, ref TwoBodyReferences references, int countInBundle) + { + for (int i = 0; i < countInBundle; ++i) + { + Prefetch(baseAddress + references.IndexA[i]); + Prefetch(baseAddress + references.IndexB[i]); + } + } + + const int prefetchDistance = 8; + [MethodImpl(MethodImplOptions.AggressiveInlining)] + unsafe static void EarlyPrefetch(ref TypeBatch typeBatch, ref Buffer references, ref Buffer states, int startBundleIndex, int exclusiveEndBundleIndex) + { + exclusiveEndBundleIndex = Math.Min(exclusiveEndBundleIndex, startBundleIndex + prefetchDistance); + var lastBundleIndex = exclusiveEndBundleIndex - 1; + for (int i = startBundleIndex; i < lastBundleIndex; ++i) + { + PrefetchBundle(states.Memory, ref references[i], Vector.Count); + } + var countInBundle = GetCountInBundle(ref typeBatch, lastBundleIndex); + PrefetchBundle(states.Memory, ref references[lastBundleIndex], countInBundle); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + unsafe static void Prefetch(ref TypeBatch typeBatch, ref Buffer references, ref Buffer states, int bundleIndex, int exclusiveEndBundleIndex) + { + var targetIndex = bundleIndex + prefetchDistance; + if (targetIndex < exclusiveEndBundleIndex) + { + PrefetchBundle(states.Memory, ref references[targetIndex], GetCountInBundle(ref typeBatch, targetIndex)); + } + } + public unsafe override void WarmStart2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { - ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); - ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); - ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); + var prestepBundles = typeBatch.PrestepData.As(); + var bodyReferencesBundles = typeBatch.BodyReferences.As(); + var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); var function = default(TConstraintFunctions); + ref var motionStates = ref bodies.ActiveSet.MotionStates; + EarlyPrefetch(ref typeBatch, ref bodyReferencesBundles, ref motionStates, startBundle, exclusiveEndBundle); for (int i = startBundle; i < exclusiveEndBundle; ++i) { - ref var prestep = ref Unsafe.Add(ref prestepBase, i); - ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); - ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); - ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); + ref var prestep = ref prestepBundles[i]; + ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; + ref var references = ref bodyReferencesBundles[i]; var count = GetCountInBundle(ref typeBatch, i); + Prefetch(ref typeBatch, ref bodyReferencesBundles, ref motionStates, i, exclusiveEndBundle); bodies.GatherState(ref references, count, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB); function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, dt, inverseDt, ref prestep, out var projection); function.WarmStart(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); - bodies.ScatterVelocities(ref wsvA, ref wsvB, ref bodyReferences, count); + bodies.ScatterVelocities(ref wsvA, ref wsvB, ref references, count); } } public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { - ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); - ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); - ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); + var prestepBundles = typeBatch.PrestepData.As(); + var bodyReferencesBundles = typeBatch.BodyReferences.As(); + var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); var function = default(TConstraintFunctions); + ref var motionStates = ref bodies.ActiveSet.MotionStates; + EarlyPrefetch(ref typeBatch, ref bodyReferencesBundles, ref motionStates, startBundle, exclusiveEndBundle); for (int i = startBundle; i < exclusiveEndBundle; ++i) { - ref var prestep = ref Unsafe.Add(ref prestepBase, i); - ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); - ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); - ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); + ref var prestep = ref prestepBundles[i]; + ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; + ref var references = ref bodyReferencesBundles[i]; var count = GetCountInBundle(ref typeBatch, i); + Prefetch(ref typeBatch, ref bodyReferencesBundles, ref motionStates, i, exclusiveEndBundle); bodies.GatherState(ref references, count, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB); function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, dt, inverseDt, ref prestep, out var projection); function.Solve(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); - bodies.ScatterVelocities(ref wsvA, ref wsvB, ref bodyReferences, count); + bodies.ScatterVelocities(ref wsvA, ref wsvB, ref references, count); } } From 119c424f456fa7967743e1778d8929cf8c13d24c Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 29 Jun 2021 18:46:10 -0500 Subject: [PATCH 050/947] Packing inertia prototype. --- BepuPhysics/BodyProperties.cs | 162 ++++++++++++---------------------- Demos/Demos/NewtDemo.cs | 4 + Demos/Program.cs | 8 +- 3 files changed, 62 insertions(+), 112 deletions(-) diff --git a/BepuPhysics/BodyProperties.cs b/BepuPhysics/BodyProperties.cs index 3dc131b46..b5ec21f7d 100644 --- a/BepuPhysics/BodyProperties.cs +++ b/BepuPhysics/BodyProperties.cs @@ -10,131 +10,79 @@ namespace BepuPhysics { /// - /// Describes the pose and velocity of a body. + /// Stores the inertia of a body in half precision. /// - [StructLayout(LayoutKind.Sequential, Size = 64)] - public struct MotionState + [StructLayout(LayoutKind.Sequential, Size = 8, Pack = 1)] + public struct PackedInertia { - /// - /// Pose of the body. - /// - public RigidPose Pose; - /// - /// Linear and angular velocity of the body. - /// - public BodyVelocity Velocity; + //TODO: Temporarily ignoring off-diagonal inertia for testing. + public Half InverseMass; + public Half InverseInertiaXX; + //public Half InverseInertiaYX; + public Half InverseInertiaYY; + //public Half InverseInertiaZX; + //public Half InverseInertiaZY; + public Half InverseInertiaZZ; - /// - /// Gets a motion state with zero position, zero velocity, and identity orientation. - /// - public static MotionState Identity + public PackedInertia(in BodyInertia inertia) { - get - { - MotionState m; - m.Velocity = default; - m.Pose.Position = default; - m.Pose.Orientation = Quaternion.Identity; - return m; - } + InverseMass = (Half)inertia.InverseMass; + InverseInertiaXX = (Half)inertia.InverseInertiaTensor.XX; + //InverseInertiaYX = (Half)inertia.InverseInertiaTensor.YX; + InverseInertiaYY = (Half)inertia.InverseInertiaTensor.YY; + //InverseInertiaZX = (Half)inertia.InverseInertiaTensor.ZX; + //InverseInertiaZY = (Half)inertia.InverseInertiaTensor.ZY; + InverseInertiaZZ = (Half)inertia.InverseInertiaTensor.ZZ; } - /// - /// Constructs a motion state for a body. - /// - /// Position of the body. - /// Orientation of the body. - /// Velocity of the body. - public MotionState(in Vector3 position, in Quaternion orientation, in BodyVelocity velocity) - { - Pose.Position = position; - Pose.Orientation = orientation; - Velocity = velocity; - } - - /// - /// Constructs a motion state for a body with zero velocity. - /// - /// Position of the body. - /// Orientation of the body. - public MotionState(in Vector3 position, in Quaternion orientation) + public readonly void Unpack(out BodyInertia inertia) { - Pose.Position = position; - Pose.Orientation = orientation; - Velocity = default; + //TODO: not necessary in complete implementation + inertia = default; + inertia.InverseMass = (float)InverseMass; + inertia.InverseInertiaTensor.XX = (float)InverseInertiaXX; + //inertia.InverseInertiaTensor.YX = (float)InverseInertiaYX; + inertia.InverseInertiaTensor.YY = (float)InverseInertiaYY; + //inertia.InverseInertiaTensor.ZX = (float)InverseInertiaZX; + //inertia.InverseInertiaTensor.ZY = (float)InverseInertiaZY; + inertia.InverseInertiaTensor.ZZ = (float)InverseInertiaZZ; } - - /// - /// Constructs a motion state for a body with zero angular velocity. - /// - /// Position of the body. - /// Orientation of the body. - /// Linear velocity of the body. - public MotionState(in Vector3 position, in Quaternion orientation, in Vector3 linearVelocity) + public readonly BodyInertia Unpack() { - Pose.Position = position; - Pose.Orientation = orientation; - Velocity.Linear = linearVelocity; - Velocity.Angular = default; + BodyInertia inertia; + //TODO: not necessary in complete implementation + inertia = default; + inertia.InverseMass = (float)InverseMass; + inertia.InverseInertiaTensor.XX = (float)InverseInertiaXX; + //inertia.InverseInertiaTensor.YX = (float)InverseInertiaYX; + inertia.InverseInertiaTensor.YY = (float)InverseInertiaYY; + //inertia.InverseInertiaTensor.ZX = (float)InverseInertiaZX; + //inertia.InverseInertiaTensor.ZY = (float)InverseInertiaZY; + inertia.InverseInertiaTensor.ZZ = (float)InverseInertiaZZ; + return inertia; } - /// - /// Constructs a motion state for a body. - /// - /// Pose of the body. - /// Velocity of the body. - public MotionState(in RigidPose pose, in BodyVelocity velocity) - { - Pose = pose; - Velocity = velocity; - } - - /// - /// Constructs a motion state for a body with zero velocity. - /// - /// Pose of the body. - public MotionState(in RigidPose pose) - { - Pose = pose; - Velocity = default; - } + } + /// + /// Describes the pose and velocity of a body. + /// + [StructLayout(LayoutKind.Sequential, Size = 64, Pack = 1)] + public struct MotionState + { /// - /// Constructs a motion state for a body with zero velocity and identity orientation. + /// Pose of the body. /// - /// Position of the body. - public MotionState(in Vector3 position) - { - Pose.Position = position; - Pose.Orientation = Quaternion.Identity; - Velocity = default; - } - + public RigidPose Pose; /// - /// Constructs a motion state for a body with zero angular velocity and identity orientation. + /// Linear and angular velocity of the body. /// - /// Position of the body. - /// Linear velocity of the body. - public MotionState(in Vector3 position, in Vector3 linearVelocity) - { - Pose.Position = position; - Pose.Orientation = Quaternion.Identity; - Velocity.Linear = linearVelocity; - Velocity.Angular = default; - } + public BodyVelocity Velocity; /// - /// Constructs a motion state for a body with identity orientation. + /// Packed inertia of the body. /// - /// Position of the body. - /// Velocity of the body. - public MotionState(in Vector3 position, in BodyVelocity velocity) - { - Pose.Position = position; - Pose.Orientation = Quaternion.Identity; - Velocity = velocity; - } - + public PackedInertia PackedInertia; } //TODO: It's a little odd that this exists alongside the BepuUtilities.RigidTransform. The original reasoning was that rigid poses may end up having a non-FP32 representation. @@ -143,6 +91,7 @@ public MotionState(in Vector3 position, in BodyVelocity velocity) /// /// Represents a rigid transformation. /// + [StructLayout(LayoutKind.Sequential, Size = 28, Pack = 1)] public struct RigidPose { public Vector3 Position; @@ -218,6 +167,7 @@ public static void MultiplyWithoutOverlap(in RigidPose a, in RigidPose b, out Ri } } + [StructLayout(LayoutKind.Sequential, Size = 24, Pack = 1)] public struct BodyVelocity { public Vector3 Linear; diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index 4f3f443e2..1e8e4391b 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -704,6 +704,10 @@ internal unsafe static void CreateDeformable(Simulation simulation, in Vector3 p public unsafe override void Initialize(ContentArchive content, Camera camera) { + Console.WriteLine($"aasgh: {Unsafe.SizeOf()}"); + var stateTest = new MotionState(); + Console.WriteLine($"offset: {(byte*)&stateTest.Velocity - (byte*)&stateTest.Pose}"); + camera.Position = new Vector3(-5f, 5.5f, 5f); camera.Yaw = MathHelper.Pi / 4; camera.Pitch = MathHelper.Pi * 0.15f; diff --git a/Demos/Program.cs b/Demos/Program.cs index eef4db3bb..ba902a5cf 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -1,12 +1,9 @@ -using BepuPhysics; -using BepuUtilities; +using BepuUtilities; using DemoContentLoader; using Demos.Demos; using Demos.SpecializedTests; using DemoUtilities; using OpenTK; -using System; -using System.Runtime.CompilerServices; namespace Demos { @@ -14,7 +11,6 @@ class Program { static void Main(string[] args) { - Console.WriteLine($"aasgh: {Unsafe.SizeOf()}"); var window = new Window("pretty cool multicolored window", new Int2((int)(DisplayDevice.Default.Width * 0.75f), (int)(DisplayDevice.Default.Height * 0.75f)), WindowMode.Windowed); var loop = new GameLoop(window); @@ -23,7 +19,7 @@ static void Main(string[] args) { content = ContentArchive.Load(stream); } - HeadlessTest.Test(content, 8, 32, 512); + HeadlessTest.Test(content, 2, 32, 512); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); From c23815542cbead0953dfa7d2a67ff53ea37135aa Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 29 Jun 2021 19:29:13 -0500 Subject: [PATCH 051/947] Hacking around a bit. Using mass-normalized inertia tensor for packing to avoid range issues. --- BepuPhysics/Bodies.cs | 37 ++++++++++++++++++++--- BepuPhysics/BodyProperties.cs | 57 +++++++++++++++-------------------- BepuPhysics/BodySet.cs | 2 ++ Demos/Program.cs | 2 +- 4 files changed, 60 insertions(+), 38 deletions(-) diff --git a/BepuPhysics/Bodies.cs b/BepuPhysics/Bodies.cs index efa3ce042..caa590381 100644 --- a/BepuPhysics/Bodies.cs +++ b/BepuPhysics/Bodies.cs @@ -548,6 +548,33 @@ private static void WriteGatherState(ref int bundleBaseBodyIndexInSet, int bodyI Vector3Wide.WriteFirst(state.Velocity.Linear, ref GetOffsetInstance(ref velocity.Linear, bodyIndexInBundle)); Vector3Wide.WriteFirst(state.Velocity.Angular, ref GetOffsetInstance(ref velocity.Angular, bodyIndexInBundle)); } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + private static void WriteGatherState(ref int bundleBaseBodyIndexInSet, int bodyIndexInBundle, ref Buffer states, + ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocities velocity, ref BodyInertias inertia) + { + ref var state = ref states[Unsafe.Add(ref bundleBaseBodyIndexInSet, bodyIndexInBundle)]; + Vector3Wide.WriteFirst(state.Pose.Position, ref GetOffsetInstance(ref position, bodyIndexInBundle)); + QuaternionWide.WriteFirst(state.Pose.Orientation, ref GetOffsetInstance(ref orientation, bodyIndexInBundle)); + Vector3Wide.WriteFirst(state.Velocity.Linear, ref GetOffsetInstance(ref velocity.Linear, bodyIndexInBundle)); + Vector3Wide.WriteFirst(state.Velocity.Angular, ref GetOffsetInstance(ref velocity.Angular, bodyIndexInBundle)); + + //ref var targetSlot = ref GetOffsetInstance(ref inertia, bodyIndexInBundle); + //GetFirst(ref targetSlot.InverseInertiaTensor.XX) = (float)state.PackedLocalInertia.InverseNormalizedInertiaXX * state.PackedLocalInertia.InverseMass; + //GetFirst(ref targetSlot.InverseInertiaTensor.YX) = 0; + //GetFirst(ref targetSlot.InverseInertiaTensor.YY) = (float)state.PackedLocalInertia.InverseNormalizedInertiaYY * state.PackedLocalInertia.InverseMass; + //GetFirst(ref targetSlot.InverseInertiaTensor.ZX) = 0; + //GetFirst(ref targetSlot.InverseInertiaTensor.ZY) = 0; + //GetFirst(ref targetSlot.InverseInertiaTensor.ZZ) = (float)state.PackedLocalInertia.InverseNormalizedInertiaZZ * state.PackedLocalInertia.InverseMass; + //GetFirst(ref targetSlot.InverseMass) = state.PackedLocalInertia.InverseMass; + ref var targetSlot = ref GetOffsetInstance(ref inertia, bodyIndexInBundle); + GetFirst(ref targetSlot.InverseInertiaTensor.XX) = state.PackedLocalInertia.InverseMass; + GetFirst(ref targetSlot.InverseInertiaTensor.YX) = 0; + GetFirst(ref targetSlot.InverseInertiaTensor.YY) = state.PackedLocalInertia.InverseMass; + GetFirst(ref targetSlot.InverseInertiaTensor.ZX) = 0; + GetFirst(ref targetSlot.InverseInertiaTensor.ZY) = 0; + GetFirst(ref targetSlot.InverseInertiaTensor.ZZ) = state.PackedLocalInertia.InverseMass; + GetFirst(ref targetSlot.InverseMass) = state.PackedLocalInertia.InverseMass; + } /// /// Gathers motion state information for a body bundle into an AOSOA bundle. @@ -616,10 +643,12 @@ public void GatherState(ref TwoBodyReferences references, int count, ref var states = ref ActiveSet.MotionStates; for (int i = 0; i < count; ++i) { - WriteGatherState(ref baseIndexA, i, ref states, ref positionA, ref orientationA, ref velocityA); - WriteGatherInertia(ref baseIndexA, i, ref Inertias, ref inertiaA); - WriteGatherState(ref baseIndexB, i, ref states, ref positionB, ref orientationB, ref velocityB); - WriteGatherInertia(ref baseIndexB, i, ref Inertias, ref inertiaB); + //WriteGatherState(ref baseIndexA, i, ref states, ref positionA, ref orientationA, ref velocityA); + //WriteGatherInertia(ref baseIndexA, i, ref Inertias, ref inertiaA); + //WriteGatherState(ref baseIndexB, i, ref states, ref positionB, ref orientationB, ref velocityB); + //WriteGatherInertia(ref baseIndexB, i, ref Inertias, ref inertiaB); + WriteGatherState(ref baseIndexA, i, ref states, ref positionA, ref orientationA, ref velocityA, ref inertiaA); + WriteGatherState(ref baseIndexB, i, ref states, ref positionB, ref orientationB, ref velocityB, ref inertiaB); } //TODO: In future versions, we will likely store the body position in different forms to allow for extremely large worlds. //That will be an opt-in feature. The default implementation will use the FP32 representation, but the user could choose to swap it out for a fp64 or fixed64 representation. diff --git a/BepuPhysics/BodyProperties.cs b/BepuPhysics/BodyProperties.cs index b5ec21f7d..a610e0ff2 100644 --- a/BepuPhysics/BodyProperties.cs +++ b/BepuPhysics/BodyProperties.cs @@ -12,53 +12,44 @@ namespace BepuPhysics /// /// Stores the inertia of a body in half precision. /// - [StructLayout(LayoutKind.Sequential, Size = 8, Pack = 1)] + [StructLayout(LayoutKind.Sequential, Size = 10, Pack = 1)] public struct PackedInertia { //TODO: Temporarily ignoring off-diagonal inertia for testing. - public Half InverseMass; - public Half InverseInertiaXX; - //public Half InverseInertiaYX; - public Half InverseInertiaYY; - //public Half InverseInertiaZX; - //public Half InverseInertiaZY; - public Half InverseInertiaZZ; + public float InverseMass; + public Half InverseNormalizedInertiaXX; + //public Half InverseNormalizedInertiaYX; + public Half InverseNormalizedInertiaYY; + //public Half InverseNormalizedInertiaZX; + //public Half InverseNormalizedInertiaZY; + public Half InverseNormalizedInertiaZZ; public PackedInertia(in BodyInertia inertia) { - InverseMass = (Half)inertia.InverseMass; - InverseInertiaXX = (Half)inertia.InverseInertiaTensor.XX; - //InverseInertiaYX = (Half)inertia.InverseInertiaTensor.YX; - InverseInertiaYY = (Half)inertia.InverseInertiaTensor.YY; - //InverseInertiaZX = (Half)inertia.InverseInertiaTensor.ZX; - //InverseInertiaZY = (Half)inertia.InverseInertiaTensor.ZY; - InverseInertiaZZ = (Half)inertia.InverseInertiaTensor.ZZ; + InverseMass = inertia.InverseMass; + InverseNormalizedInertiaXX = (Half)(inertia.InverseInertiaTensor.XX / InverseMass); + //InverseNormalizedInertiaYX = (Half)(inertia.InverseInertiaTensor.YX/ InverseMass); + InverseNormalizedInertiaYY = (Half)(inertia.InverseInertiaTensor.YY / InverseMass); + //InverseNormalizedInertiaZX = (Half)(inertia.InverseInertiaTensor.ZX/ InverseMass); + //InverseNormalizedInertiaZY = (Half)(inertia.InverseInertiaTensor.ZY/ InverseMass); + InverseNormalizedInertiaZZ = (Half)(inertia.InverseInertiaTensor.ZZ / InverseMass); } public readonly void Unpack(out BodyInertia inertia) { //TODO: not necessary in complete implementation inertia = default; - inertia.InverseMass = (float)InverseMass; - inertia.InverseInertiaTensor.XX = (float)InverseInertiaXX; - //inertia.InverseInertiaTensor.YX = (float)InverseInertiaYX; - inertia.InverseInertiaTensor.YY = (float)InverseInertiaYY; - //inertia.InverseInertiaTensor.ZX = (float)InverseInertiaZX; - //inertia.InverseInertiaTensor.ZY = (float)InverseInertiaZY; - inertia.InverseInertiaTensor.ZZ = (float)InverseInertiaZZ; + inertia.InverseMass = InverseMass; + inertia.InverseInertiaTensor.XX = (float)InverseNormalizedInertiaXX * InverseMass; + //inertia.InverseInertiaTensor.YX = (float)InverseNormalizedInertiaYX * InverseMass; + inertia.InverseInertiaTensor.YY = (float)InverseNormalizedInertiaYY * InverseMass; + //inertia.InverseInertiaTensor.ZX = (float)InverseNormalizedInertiaZX * InverseMass; + //inertia.InverseInertiaTensor.ZY = (float)InverseNormalizedInertiaZY * InverseMass; + inertia.InverseInertiaTensor.ZZ = (float)InverseNormalizedInertiaZZ * InverseMass; } public readonly BodyInertia Unpack() { - BodyInertia inertia; - //TODO: not necessary in complete implementation - inertia = default; - inertia.InverseMass = (float)InverseMass; - inertia.InverseInertiaTensor.XX = (float)InverseInertiaXX; - //inertia.InverseInertiaTensor.YX = (float)InverseInertiaYX; - inertia.InverseInertiaTensor.YY = (float)InverseInertiaYY; - //inertia.InverseInertiaTensor.ZX = (float)InverseInertiaZX; - //inertia.InverseInertiaTensor.ZY = (float)InverseInertiaZY; - inertia.InverseInertiaTensor.ZZ = (float)InverseInertiaZZ; + Unpack(out var inertia); return inertia; } @@ -82,7 +73,7 @@ public struct MotionState /// /// Packed inertia of the body. /// - public PackedInertia PackedInertia; + public PackedInertia PackedLocalInertia; } //TODO: It's a little odd that this exists alongside the BepuUtilities.RigidTransform. The original reasoning was that rigid poses may end up having a non-FP32 representation. diff --git a/BepuPhysics/BodySet.cs b/BepuPhysics/BodySet.cs index 8f822b0a3..c1266fd35 100644 --- a/BepuPhysics/BodySet.cs +++ b/BepuPhysics/BodySet.cs @@ -127,6 +127,8 @@ internal void ApplyDescriptionByIndex(int index, in BodyDescription description) ref var state = ref MotionStates[index]; state.Pose = description.Pose; state.Velocity = description.Velocity; + //TODO: We're just trying this right now; note redundancy that we need to deal with. + state.PackedLocalInertia = new PackedInertia(description.LocalInertia); LocalInertias[index] = description.LocalInertia; ref var collidable = ref Collidables[index]; collidable.Continuity = description.Collidable.Continuity; diff --git a/Demos/Program.cs b/Demos/Program.cs index ba902a5cf..6c769ece5 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -19,7 +19,7 @@ static void Main(string[] args) { content = ContentArchive.Load(stream); } - HeadlessTest.Test(content, 2, 32, 512); + HeadlessTest.Test(content, 4, 32, 512); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); From 641328c206200e88399ca6f3c55877c8c256e075 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 30 Jun 2021 17:35:10 -0500 Subject: [PATCH 052/947] Gatherscatter refactor. --- BepuPhysics/Bodies.cs | 510 +-------------------------- BepuPhysics/Bodies_GatherScatter.cs | 526 ++++++++++++++++++++++++++++ Demos/Program.cs | 2 +- 3 files changed, 528 insertions(+), 510 deletions(-) create mode 100644 BepuPhysics/Bodies_GatherScatter.cs diff --git a/BepuPhysics/Bodies.cs b/BepuPhysics/Bodies.cs index caa590381..8614e3cc7 100644 --- a/BepuPhysics/Bodies.cs +++ b/BepuPhysics/Bodies.cs @@ -27,7 +27,7 @@ public struct BodyMemoryLocation /// /// Collection of all allocated bodies. /// - public class Bodies + public partial class Bodies { /// /// Remaps a body handle integer value to the actual array index of the body. @@ -524,514 +524,6 @@ internal void ValidateMotionStates() } } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - private static void WriteGatherInertia(ref int bundleBaseBodyIndexInSet, int bodyIndexInBundle, ref Buffer states, ref BodyInertias gatheredInertias) - { - ref var source = ref states[Unsafe.Add(ref bundleBaseBodyIndexInSet, bodyIndexInBundle)]; - ref var targetSlot = ref GetOffsetInstance(ref gatheredInertias, bodyIndexInBundle); - GetFirst(ref targetSlot.InverseInertiaTensor.XX) = source.InverseInertiaTensor.XX; - GetFirst(ref targetSlot.InverseInertiaTensor.YX) = source.InverseInertiaTensor.YX; - GetFirst(ref targetSlot.InverseInertiaTensor.YY) = source.InverseInertiaTensor.YY; - GetFirst(ref targetSlot.InverseInertiaTensor.ZX) = source.InverseInertiaTensor.ZX; - GetFirst(ref targetSlot.InverseInertiaTensor.ZY) = source.InverseInertiaTensor.ZY; - GetFirst(ref targetSlot.InverseInertiaTensor.ZZ) = source.InverseInertiaTensor.ZZ; - GetFirst(ref targetSlot.InverseMass) = source.InverseMass; - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - private static void WriteGatherState(ref int bundleBaseBodyIndexInSet, int bodyIndexInBundle, ref Buffer states, - ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocities velocity) - { - ref var state = ref states[Unsafe.Add(ref bundleBaseBodyIndexInSet, bodyIndexInBundle)]; - Vector3Wide.WriteFirst(state.Pose.Position, ref GetOffsetInstance(ref position, bodyIndexInBundle)); - QuaternionWide.WriteFirst(state.Pose.Orientation, ref GetOffsetInstance(ref orientation, bodyIndexInBundle)); - Vector3Wide.WriteFirst(state.Velocity.Linear, ref GetOffsetInstance(ref velocity.Linear, bodyIndexInBundle)); - Vector3Wide.WriteFirst(state.Velocity.Angular, ref GetOffsetInstance(ref velocity.Angular, bodyIndexInBundle)); - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - private static void WriteGatherState(ref int bundleBaseBodyIndexInSet, int bodyIndexInBundle, ref Buffer states, - ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocities velocity, ref BodyInertias inertia) - { - ref var state = ref states[Unsafe.Add(ref bundleBaseBodyIndexInSet, bodyIndexInBundle)]; - Vector3Wide.WriteFirst(state.Pose.Position, ref GetOffsetInstance(ref position, bodyIndexInBundle)); - QuaternionWide.WriteFirst(state.Pose.Orientation, ref GetOffsetInstance(ref orientation, bodyIndexInBundle)); - Vector3Wide.WriteFirst(state.Velocity.Linear, ref GetOffsetInstance(ref velocity.Linear, bodyIndexInBundle)); - Vector3Wide.WriteFirst(state.Velocity.Angular, ref GetOffsetInstance(ref velocity.Angular, bodyIndexInBundle)); - - //ref var targetSlot = ref GetOffsetInstance(ref inertia, bodyIndexInBundle); - //GetFirst(ref targetSlot.InverseInertiaTensor.XX) = (float)state.PackedLocalInertia.InverseNormalizedInertiaXX * state.PackedLocalInertia.InverseMass; - //GetFirst(ref targetSlot.InverseInertiaTensor.YX) = 0; - //GetFirst(ref targetSlot.InverseInertiaTensor.YY) = (float)state.PackedLocalInertia.InverseNormalizedInertiaYY * state.PackedLocalInertia.InverseMass; - //GetFirst(ref targetSlot.InverseInertiaTensor.ZX) = 0; - //GetFirst(ref targetSlot.InverseInertiaTensor.ZY) = 0; - //GetFirst(ref targetSlot.InverseInertiaTensor.ZZ) = (float)state.PackedLocalInertia.InverseNormalizedInertiaZZ * state.PackedLocalInertia.InverseMass; - //GetFirst(ref targetSlot.InverseMass) = state.PackedLocalInertia.InverseMass; - ref var targetSlot = ref GetOffsetInstance(ref inertia, bodyIndexInBundle); - GetFirst(ref targetSlot.InverseInertiaTensor.XX) = state.PackedLocalInertia.InverseMass; - GetFirst(ref targetSlot.InverseInertiaTensor.YX) = 0; - GetFirst(ref targetSlot.InverseInertiaTensor.YY) = state.PackedLocalInertia.InverseMass; - GetFirst(ref targetSlot.InverseInertiaTensor.ZX) = 0; - GetFirst(ref targetSlot.InverseInertiaTensor.ZY) = 0; - GetFirst(ref targetSlot.InverseInertiaTensor.ZZ) = state.PackedLocalInertia.InverseMass; - GetFirst(ref targetSlot.InverseMass) = state.PackedLocalInertia.InverseMass; - } - - /// - /// Gathers motion state information for a body bundle into an AOSOA bundle. - /// - /// Active body indices being gathered. - /// Number of body pairs in the bundle. - /// Gathered absolute position of the body. - /// Gathered orientation of the body. - /// Gathered velocity of the body. - /// Gathered inertia of the body. - //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public void GatherState(ref Vector references, int count, - out Vector3Wide position, out QuaternionWide orientation, out BodyVelocities velocity, out BodyInertias inertia) - { - Unsafe.SkipInit(out position); - Unsafe.SkipInit(out orientation); - Unsafe.SkipInit(out velocity); - Unsafe.SkipInit(out inertia); - //TODO: This function and its users (which should be relatively few) is a problem for large world position precision. - //It directly reports the position, thereby infecting vectorized logic with the high precision representation. - //You might be able to redesign the users of this function to not need it, but that comes with its own difficulties - //(for example, making the grab motor rely on having its goal offset updated every frame by the user). - Debug.Assert(count >= 0 && count <= Vector.Count); - //Grab the base references for the body indices. Note that we make use of the references memory layout again. - ref var baseIndex = ref Unsafe.As, int>(ref references); - - ref var states = ref ActiveSet.MotionStates; - for (int i = 0; i < count; ++i) - { - WriteGatherState(ref baseIndex, i, ref states, ref position, ref orientation, ref velocity); - WriteGatherInertia(ref baseIndex, i, ref Inertias, ref inertia); - } - } - - /// - /// Gathers orientations and relative positions for a two body bundle into an AOSOA bundle. - /// - /// Active body indices being gathered. - /// Number of body pairs in the bundle. - /// Gathered offsets from body A to body B. - /// Gathered orientation of body A. - /// Gathered orientation of body B. - /// Gathered velocity of body A. - /// Gathered velocity of body B. - /// Gathered inertia of body A. - /// Gathered inertia of body B. - //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public void GatherState(ref TwoBodyReferences references, int count, - out QuaternionWide orientationA, out BodyVelocities velocityA, out BodyInertias inertiaA, - out Vector3Wide ab, - out QuaternionWide orientationB, out BodyVelocities velocityB, out BodyInertias inertiaB) - { - Unsafe.SkipInit(out Vector3Wide positionA); - Unsafe.SkipInit(out Vector3Wide positionB); - Unsafe.SkipInit(out orientationA); - Unsafe.SkipInit(out orientationB); - Unsafe.SkipInit(out velocityA); - Unsafe.SkipInit(out velocityB); - Unsafe.SkipInit(out inertiaA); - Unsafe.SkipInit(out inertiaB); - Debug.Assert(count >= 0 && count <= Vector.Count); - //Grab the base references for the body indices. Note that we make use of the references memory layout again. - ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); - ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); - - ref var states = ref ActiveSet.MotionStates; - for (int i = 0; i < count; ++i) - { - //WriteGatherState(ref baseIndexA, i, ref states, ref positionA, ref orientationA, ref velocityA); - //WriteGatherInertia(ref baseIndexA, i, ref Inertias, ref inertiaA); - //WriteGatherState(ref baseIndexB, i, ref states, ref positionB, ref orientationB, ref velocityB); - //WriteGatherInertia(ref baseIndexB, i, ref Inertias, ref inertiaB); - WriteGatherState(ref baseIndexA, i, ref states, ref positionA, ref orientationA, ref velocityA, ref inertiaA); - WriteGatherState(ref baseIndexB, i, ref states, ref positionB, ref orientationB, ref velocityB, ref inertiaB); - } - //TODO: In future versions, we will likely store the body position in different forms to allow for extremely large worlds. - //That will be an opt-in feature. The default implementation will use the FP32 representation, but the user could choose to swap it out for a fp64 or fixed64 representation. - //This affects other systems- AABB calculation, pose integration, solving, and in extreme (64 bit) cases, the broadphase. - //We want to insulate other systems from direct knowledge about the implementation of positions when possible. - //These functions support the solver's needs while hiding absolute positions. - //In order to support other absolute positions, we'll need alternate implementations of this and other functions. - //But for the most part, we don't want to pay the overhead of an abstract invocation within the inner loop of the solver. - //Given the current limits of C# and the compiler, the best option seems to be conditional compilation. - Vector3Wide.Subtract(positionB, positionA, out ab); - } - - - /// - /// Gathers orientations and relative positions for a three body bundle into an AOSOA bundle. - /// - /// Active body indices being gathered. - /// Number of body pairs in the bundle. - /// Gathered offsets from body A to body B. - /// Gathered offsets from body A to body C. - /// Gathered orientation of body A. - /// Gathered orientation of body B. - /// Gathered orientation of body C. - /// Gathered velocity of body A. - /// Gathered velocity of body B. - /// Gathered velocity of body C. - /// Gathered inertia of body A. - /// Gathered inertia of body B. - /// Gathered inertia of body C. - //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public void GatherState(ref ThreeBodyReferences references, int count, - out QuaternionWide orientationA, out BodyVelocities velocityA, out BodyInertias inertiaA, - out Vector3Wide ab, - out QuaternionWide orientationB, out BodyVelocities velocityB, out BodyInertias inertiaB, - out Vector3Wide ac, - out QuaternionWide orientationC, out BodyVelocities velocityC, out BodyInertias inertiaC) - { - Unsafe.SkipInit(out Vector3Wide positionA); - Unsafe.SkipInit(out Vector3Wide positionB); - Unsafe.SkipInit(out Vector3Wide positionC); - Unsafe.SkipInit(out orientationA); - Unsafe.SkipInit(out orientationB); - Unsafe.SkipInit(out orientationC); - Unsafe.SkipInit(out velocityA); - Unsafe.SkipInit(out velocityB); - Unsafe.SkipInit(out velocityC); - Unsafe.SkipInit(out inertiaA); - Unsafe.SkipInit(out inertiaB); - Unsafe.SkipInit(out inertiaC); - Debug.Assert(count >= 0 && count <= Vector.Count); - //Grab the base references for the body indices. Note that we make use of the references memory layout again. - ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); - ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); - ref var baseIndexC = ref Unsafe.As, int>(ref references.IndexC); - - ref var states = ref ActiveSet.MotionStates; - for (int i = 0; i < count; ++i) - { - WriteGatherState(ref baseIndexA, i, ref states, ref positionA, ref orientationA, ref velocityA); - WriteGatherInertia(ref baseIndexA, i, ref Inertias, ref inertiaA); - WriteGatherState(ref baseIndexB, i, ref states, ref positionB, ref orientationB, ref velocityB); - WriteGatherInertia(ref baseIndexB, i, ref Inertias, ref inertiaB); - WriteGatherState(ref baseIndexC, i, ref states, ref positionC, ref orientationC, ref velocityC); - WriteGatherInertia(ref baseIndexC, i, ref Inertias, ref inertiaC); - } - //TODO: In future versions, we will likely store the body position in different forms to allow for extremely large worlds. - //That will be an opt-in feature. The default implementation will use the FP32 representation, but the user could choose to swap it out for a fp64 or fixed64 representation. - //This affects other systems- AABB calculation, pose integration, solving, and in extreme (64 bit) cases, the broadphase. - //We want to insulate other systems from direct knowledge about the implementation of positions when possible. - //These functions support the solver's needs while hiding absolute positions. - //In order to support other absolute positions, we'll need alternate implementations of this and other functions. - //But for the most part, we don't want to pay the overhead of an abstract invocation within the inner loop of the solver. - //Given the current limits of C# and the compiler, the best option seems to be conditional compilation. - Vector3Wide.Subtract(positionB, positionA, out ab); - Vector3Wide.Subtract(positionC, positionA, out ac); - } - - /// - /// Gathers orientations and relative positions for a four body bundle into an AOSOA bundle. - /// - /// Active body indices being gathered. - /// Number of body pairs in the bundle. - /// Gathered offsets from body A to body B. - /// Gathered offsets from body A to body C. - /// Gathered offsets from body A to body D. - /// Gathered orientation of body A. - /// Gathered orientation of body B. - /// Gathered orientation of body C. - /// Gathered orientation of body D. - /// Gathered velocity of body A. - /// Gathered velocity of body B. - /// Gathered velocity of body C. - /// Gathered velocity of body D. - /// Gathered inertia of body A. - /// Gathered inertia of body B. - /// Gathered inertia of body C. - /// Gathered inertia of body C. - //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public void GatherState(ref FourBodyReferences references, int count, - out QuaternionWide orientationA, out BodyVelocities velocityA, out BodyInertias inertiaA, - out Vector3Wide ab, - out QuaternionWide orientationB, out BodyVelocities velocityB, out BodyInertias inertiaB, - out Vector3Wide ac, - out QuaternionWide orientationC, out BodyVelocities velocityC, out BodyInertias inertiaC, - out Vector3Wide ad, - out QuaternionWide orientationD, out BodyVelocities velocityD, out BodyInertias inertiaD) - { - Unsafe.SkipInit(out Vector3Wide positionA); - Unsafe.SkipInit(out Vector3Wide positionB); - Unsafe.SkipInit(out Vector3Wide positionC); - Unsafe.SkipInit(out Vector3Wide positionD); - Unsafe.SkipInit(out orientationA); - Unsafe.SkipInit(out orientationB); - Unsafe.SkipInit(out orientationC); - Unsafe.SkipInit(out orientationD); - Unsafe.SkipInit(out velocityA); - Unsafe.SkipInit(out velocityB); - Unsafe.SkipInit(out velocityC); - Unsafe.SkipInit(out velocityD); - Unsafe.SkipInit(out inertiaA); - Unsafe.SkipInit(out inertiaB); - Unsafe.SkipInit(out inertiaC); - Unsafe.SkipInit(out inertiaD); - Debug.Assert(count >= 0 && count <= Vector.Count); - //Grab the base references for the body indices. Note that we make use of the references memory layout again. - ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); - ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); - ref var baseIndexC = ref Unsafe.As, int>(ref references.IndexC); - ref var baseIndexD = ref Unsafe.As, int>(ref references.IndexD); - - ref var states = ref ActiveSet.MotionStates; - for (int i = 0; i < count; ++i) - { - WriteGatherState(ref baseIndexA, i, ref states, ref positionA, ref orientationA, ref velocityA); - WriteGatherInertia(ref baseIndexA, i, ref Inertias, ref inertiaA); - WriteGatherState(ref baseIndexB, i, ref states, ref positionB, ref orientationB, ref velocityB); - WriteGatherInertia(ref baseIndexB, i, ref Inertias, ref inertiaB); - WriteGatherState(ref baseIndexC, i, ref states, ref positionC, ref orientationC, ref velocityC); - WriteGatherInertia(ref baseIndexC, i, ref Inertias, ref inertiaC); - WriteGatherState(ref baseIndexD, i, ref states, ref positionD, ref orientationD, ref velocityD); - WriteGatherInertia(ref baseIndexD, i, ref Inertias, ref inertiaD); - } - //TODO: In future versions, we will likely store the body position in different forms to allow for extremely large worlds. - //That will be an opt-in feature. The default implementation will use the FP32 representation, but the user could choose to swap it out for a fp64 or fixed64 representation. - //This affects other systems- AABB calculation, pose integration, solving, and in extreme (64 bit) cases, the broadphase. - //We want to insulate other systems from direct knowledge about the implementation of positions when possible. - //These functions support the solver's needs while hiding absolute positions. - //In order to support other absolute positions, we'll need alternate implementations of this and other functions. - //But for the most part, we don't want to pay the overhead of an abstract invocation within the inner loop of the solver. - //Given the current limits of C# and the compiler, the best option seems to be conditional compilation. - Vector3Wide.Subtract(positionB, positionA, out ab); - Vector3Wide.Subtract(positionC, positionA, out ac); - Vector3Wide.Subtract(positionC, positionA, out ad); - } - - //[MethodImpl(MethodImplOptions.AggressiveInlining)] - //private static void WriteLinearGatherState(ref int baseBodyIndexInSet, int bodyIndexInBundle, ref Buffer states, ref Vector3Wide linearVelocity, ref Vector3Wide position) - //{ - // ref var state = ref states[Unsafe.Add(ref baseBodyIndexInSet, bodyIndexInBundle)]; - // Vector3Wide.WriteFirst(state.Pose.Position, ref GetOffsetInstance(ref position, bodyIndexInBundle)); - // Vector3Wide.WriteFirst(state.Velocity.Linear, ref GetOffsetInstance(ref linearVelocity, bodyIndexInBundle)); - //} - ///// - ///// Gathers relative positions and linear velocities for a two body bundle into an AOSOA bundle. - ///// - ///// Active body indices being gathered. - ///// Number of body pairs in the bundle. - ///// Gathered offset from body A to body B. - ///// Linear velocity of body A. - ///// Linear velocity of body B. - ////[MethodImpl(MethodImplOptions.AggressiveInlining)] - //public void GatherLinearState(ref TwoBodyReferences references, int count, out Vector3Wide ab, out Vector3Wide linearVelocityA, out Vector3Wide linearVelocityB) - //{ - // Unsafe.SkipInit(out Vector3Wide positionA); - // Unsafe.SkipInit(out Vector3Wide positionB); - // Unsafe.SkipInit(out linearVelocityA); - // Unsafe.SkipInit(out linearVelocityB); - // Debug.Assert(count >= 0 && count <= Vector.Count); - // //Grab the base references for the body indices. Note that we make use of the references memory layout again. - // ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); - // ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); - - // ref var states = ref ActiveSet.MotionStates; - // for (int i = 0; i < count; ++i) - // { - // WriteLinearGatherState(ref baseIndexA, i, ref states, ref positionA, ref linearVelocityA); - // WriteLinearGatherState(ref baseIndexB, i, ref states, ref positionB, ref linearVelocityB); - // } - // //Same as other gather case; this is sensitive to changes in the representation of body position. In high precision modes, this'll need to change. - // Vector3Wide.Subtract(positionB, positionA, out ab); - //} - - - ///// - ///// Gathers relative positions and linear velocities for a three body bundle into an AOSOA bundle. - ///// - ///// Active body indices being gathered. - ///// Number of body pairs in the bundle. - ///// Gathered offset from body A to body B. - ///// Gathered offset from body A to body C. - ///// Linear velocity of body A. - ///// Linear velocity of body B. - ///// Linear velocity of body C. - ////[MethodImpl(MethodImplOptions.AggressiveInlining)] - //public void GatherLinearState(ref ThreeBodyReferences references, int count, out Vector3Wide ab, out Vector3Wide ac, out Vector3Wide linearVelocityA, out Vector3Wide linearVelocityB, out Vector3Wide linearVelocityC) - //{ - // Unsafe.SkipInit(out Vector3Wide positionA); - // Unsafe.SkipInit(out Vector3Wide positionB); - // Unsafe.SkipInit(out Vector3Wide positionC); - // Unsafe.SkipInit(out linearVelocityA); - // Unsafe.SkipInit(out linearVelocityB); - // Unsafe.SkipInit(out linearVelocityC); - // Debug.Assert(count >= 0 && count <= Vector.Count); - // //Grab the base references for the body indices. Note that we make use of the references memory layout again. - // ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); - // ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); - // ref var baseIndexC = ref Unsafe.As, int>(ref references.IndexC); - - // ref var states = ref ActiveSet.MotionStates; - // for (int i = 0; i < count; ++i) - // { - // WriteLinearGatherState(ref baseIndexA, i, ref states, ref positionA, ref linearVelocityA); - // WriteLinearGatherState(ref baseIndexB, i, ref states, ref positionB, ref linearVelocityB); - // WriteLinearGatherState(ref baseIndexC, i, ref states, ref positionC, ref linearVelocityC); - // } - // //Same as other gather case; this is sensitive to changes in the representation of body position. In high precision modes, this'll need to change. - // Vector3Wide.Subtract(positionB, positionA, out ab); - // Vector3Wide.Subtract(positionC, positionA, out ac); - //} - - - ///// - ///// Gathers relative positions and linear velocities for a four body bundle into an AOSOA bundle. - ///// - ///// Active body indices being gathered. - ///// Number of body pairs in the bundle. - ///// Gathered offset from body A to body B. - ///// Gathered offset from body A to body C. - ///// Gathered offset from body A to body D. - ///// Linear velocity of body A. - ///// Linear velocity of body B. - ///// Linear velocity of body C. - ///// Linear velocity of body D. - ////[MethodImpl(MethodImplOptions.AggressiveInlining)] - //public void GatherLinearState(ref FourBodyReferences references, int count, out Vector3Wide ab, out Vector3Wide ac, out Vector3Wide ad, - // out Vector3Wide linearVelocityA, out Vector3Wide linearVelocityB, out Vector3Wide linearVelocityC, out Vector3Wide linearVelocityD) - //{ - // Unsafe.SkipInit(out Vector3Wide positionA); - // Unsafe.SkipInit(out Vector3Wide positionB); - // Unsafe.SkipInit(out Vector3Wide positionC); - // Unsafe.SkipInit(out Vector3Wide positionD); - // Unsafe.SkipInit(out linearVelocityA); - // Unsafe.SkipInit(out linearVelocityB); - // Unsafe.SkipInit(out linearVelocityC); - // Unsafe.SkipInit(out linearVelocityD); - // Debug.Assert(count >= 0 && count <= Vector.Count); - // //Grab the base references for the body indices. Note that we make use of the references memory layout again. - // ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); - // ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); - // ref var baseIndexC = ref Unsafe.As, int>(ref references.IndexC); - // ref var baseIndexD = ref Unsafe.As, int>(ref references.IndexD); - - // ref var states = ref ActiveSet.MotionStates; - // for (int i = 0; i < count; ++i) - // { - // WriteLinearGatherState(ref baseIndexA, i, ref states, ref positionA, ref linearVelocityA); - // WriteLinearGatherState(ref baseIndexB, i, ref states, ref positionB, ref linearVelocityB); - // WriteLinearGatherState(ref baseIndexC, i, ref states, ref positionC, ref linearVelocityC); - // WriteLinearGatherState(ref baseIndexD, i, ref states, ref positionD, ref linearVelocityD); - // } - // //Same as other gather case; this is sensitive to changes in the representation of body position. In high precision modes, this'll need to change. - // Vector3Wide.Subtract(positionB, positionA, out ab); - // Vector3Wide.Subtract(positionC, positionA, out ac); - // Vector3Wide.Subtract(positionD, positionA, out ad); - //} - - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - private unsafe void ScatterVelocities(ref BodyVelocities sourceVelocities, ref int baseIndex, int innerIndex) - { - //TODO: How much value would there be in branching on kinematic state and avoiding a write? Depends a lot on the number of kinematics. - ref var sourceSlot = ref GetOffsetInstance(ref sourceVelocities, innerIndex); - ref var target = ref ActiveSet.MotionStates[Unsafe.Add(ref baseIndex, innerIndex)].Velocity; - target.Linear = new Vector3(sourceSlot.Linear.X[0], sourceSlot.Linear.Y[0], sourceSlot.Linear.Z[0]); - target.Angular = new Vector3(sourceSlot.Angular.X[0], sourceSlot.Angular.Y[0], sourceSlot.Angular.Z[0]); - } - - - /// - /// Scatters velocities for one body bundle into the active body set. - /// - /// Velocities of body bundle A to scatter. - /// Active set indices of the bodies to scatter velocity data to. - /// Number of body pairs in the bundle. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void ScatterVelocities(ref BodyVelocities sourceVelocities, ref Vector references, int count) - { - Debug.Assert(count >= 0 && count <= Vector.Count); - //Grab the base references for the body indices. Note that we make use of the references memory layout again. - ref var baseIndex = ref Unsafe.As, int>(ref references); - for (int i = 0; i < count; ++i) - { - ScatterVelocities(ref sourceVelocities, ref baseIndex, i); - } - } - - /// - /// Scatters velocities for two body bundles into the active body set. - /// - /// Velocities of body bundle A to scatter. - /// Velocities of body bundle B to scatter. - /// Active set indices of the bodies to scatter velocity data to. - /// Number of body pairs in the bundle. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void ScatterVelocities(ref BodyVelocities sourceVelocitiesA, ref BodyVelocities sourceVelocitiesB, - ref TwoBodyReferences references, int count) - { - Debug.Assert(count >= 0 && count <= Vector.Count); - //Grab the base references for the body indices. Note that we make use of the references memory layout again. - ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); - ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); - for (int i = 0; i < count; ++i) - { - ScatterVelocities(ref sourceVelocitiesA, ref baseIndexA, i); - ScatterVelocities(ref sourceVelocitiesB, ref baseIndexB, i); - } - } - - /// - /// Scatters velocities for three body bundles into the active body set. - /// - /// Velocities of body bundle A to scatter. - /// Velocities of body bundle B to scatter. - /// Velocities of body bundle C to scatter. - /// Active set indices of the bodies to scatter velocity data to. - /// Number of body pairs in the bundle. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void ScatterVelocities( - ref BodyVelocities sourceVelocitiesA, ref BodyVelocities sourceVelocitiesB, ref BodyVelocities sourceVelocitiesC, - ref ThreeBodyReferences references, int count) - { - Debug.Assert(count >= 0 && count <= Vector.Count); - //Grab the base references for the body indices. Note that we make use of the references memory layout again. - ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); - ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); - ref var baseIndexC = ref Unsafe.As, int>(ref references.IndexC); - for (int i = 0; i < count; ++i) - { - ScatterVelocities(ref sourceVelocitiesA, ref baseIndexA, i); - ScatterVelocities(ref sourceVelocitiesB, ref baseIndexB, i); - ScatterVelocities(ref sourceVelocitiesC, ref baseIndexC, i); - } - } - - /// - /// Scatters velocities for four body bundles into the active body set. - /// - /// Velocities of body bundle A to scatter. - /// Velocities of body bundle B to scatter. - /// Velocities of body bundle C to scatter. - /// Velocities of body bundle D to scatter. - /// Active set indices of the bodies to scatter velocity data to. - /// Number of body pairs in the bundle. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void ScatterVelocities( - ref BodyVelocities sourceVelocitiesA, ref BodyVelocities sourceVelocitiesB, ref BodyVelocities sourceVelocitiesC, ref BodyVelocities sourceVelocitiesD, - ref FourBodyReferences references, int count) - { - Debug.Assert(count >= 0 && count <= Vector.Count); - //Grab the base references for the body indices. Note that we make use of the references memory layout again. - ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); - ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); - ref var baseIndexC = ref Unsafe.As, int>(ref references.IndexC); - ref var baseIndexD = ref Unsafe.As, int>(ref references.IndexD); - for (int i = 0; i < count; ++i) - { - ScatterVelocities(ref sourceVelocitiesA, ref baseIndexA, i); - ScatterVelocities(ref sourceVelocitiesB, ref baseIndexB, i); - ScatterVelocities(ref sourceVelocitiesC, ref baseIndexC, i); - ScatterVelocities(ref sourceVelocitiesD, ref baseIndexD, i); - } - } - internal void ResizeSetsCapacity(int setsCapacity, int potentiallyAllocatedCount) { Debug.Assert(setsCapacity >= potentiallyAllocatedCount && potentiallyAllocatedCount <= Sets.Length); diff --git a/BepuPhysics/Bodies_GatherScatter.cs b/BepuPhysics/Bodies_GatherScatter.cs new file mode 100644 index 000000000..5fc46cece --- /dev/null +++ b/BepuPhysics/Bodies_GatherScatter.cs @@ -0,0 +1,526 @@ +using BepuUtilities.Memory; +using System; +using System.Diagnostics; +using System.Numerics; +using System.Runtime.CompilerServices; +using BepuPhysics.Constraints; +using BepuPhysics.Collidables; +using BepuPhysics.CollisionDetection; +using BepuUtilities; +using static BepuUtilities.GatherScatter; + +namespace BepuPhysics +{ + public partial class Bodies + { + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + private static void WriteGatherInertia(ref int bundleBaseBodyIndexInSet, int bodyIndexInBundle, ref Buffer states, ref BodyInertias gatheredInertias) + { + ref var source = ref states[Unsafe.Add(ref bundleBaseBodyIndexInSet, bodyIndexInBundle)]; + ref var targetSlot = ref GetOffsetInstance(ref gatheredInertias, bodyIndexInBundle); + GetFirst(ref targetSlot.InverseInertiaTensor.XX) = source.InverseInertiaTensor.XX; + GetFirst(ref targetSlot.InverseInertiaTensor.YX) = source.InverseInertiaTensor.YX; + GetFirst(ref targetSlot.InverseInertiaTensor.YY) = source.InverseInertiaTensor.YY; + GetFirst(ref targetSlot.InverseInertiaTensor.ZX) = source.InverseInertiaTensor.ZX; + GetFirst(ref targetSlot.InverseInertiaTensor.ZY) = source.InverseInertiaTensor.ZY; + GetFirst(ref targetSlot.InverseInertiaTensor.ZZ) = source.InverseInertiaTensor.ZZ; + GetFirst(ref targetSlot.InverseMass) = source.InverseMass; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + private static void WriteGatherState(ref int bundleBaseBodyIndexInSet, int bodyIndexInBundle, ref Buffer states, + ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocities velocity) + { + ref var state = ref states[Unsafe.Add(ref bundleBaseBodyIndexInSet, bodyIndexInBundle)]; + Vector3Wide.WriteFirst(state.Pose.Position, ref GetOffsetInstance(ref position, bodyIndexInBundle)); + QuaternionWide.WriteFirst(state.Pose.Orientation, ref GetOffsetInstance(ref orientation, bodyIndexInBundle)); + Vector3Wide.WriteFirst(state.Velocity.Linear, ref GetOffsetInstance(ref velocity.Linear, bodyIndexInBundle)); + Vector3Wide.WriteFirst(state.Velocity.Angular, ref GetOffsetInstance(ref velocity.Angular, bodyIndexInBundle)); + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + private static void WriteGatherState(ref int bundleBaseBodyIndexInSet, int bodyIndexInBundle, ref Buffer states, + ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocities velocity, ref BodyInertias inertia) + { + ref var state = ref states[Unsafe.Add(ref bundleBaseBodyIndexInSet, bodyIndexInBundle)]; + Vector3Wide.WriteFirst(state.Pose.Position, ref GetOffsetInstance(ref position, bodyIndexInBundle)); + QuaternionWide.WriteFirst(state.Pose.Orientation, ref GetOffsetInstance(ref orientation, bodyIndexInBundle)); + Vector3Wide.WriteFirst(state.Velocity.Linear, ref GetOffsetInstance(ref velocity.Linear, bodyIndexInBundle)); + Vector3Wide.WriteFirst(state.Velocity.Angular, ref GetOffsetInstance(ref velocity.Angular, bodyIndexInBundle)); + + //ref var targetSlot = ref GetOffsetInstance(ref inertia, bodyIndexInBundle); + //GetFirst(ref targetSlot.InverseInertiaTensor.XX) = (float)state.PackedLocalInertia.InverseNormalizedInertiaXX * state.PackedLocalInertia.InverseMass; + //GetFirst(ref targetSlot.InverseInertiaTensor.YX) = 0; + //GetFirst(ref targetSlot.InverseInertiaTensor.YY) = (float)state.PackedLocalInertia.InverseNormalizedInertiaYY * state.PackedLocalInertia.InverseMass; + //GetFirst(ref targetSlot.InverseInertiaTensor.ZX) = 0; + //GetFirst(ref targetSlot.InverseInertiaTensor.ZY) = 0; + //GetFirst(ref targetSlot.InverseInertiaTensor.ZZ) = (float)state.PackedLocalInertia.InverseNormalizedInertiaZZ * state.PackedLocalInertia.InverseMass; + //GetFirst(ref targetSlot.InverseMass) = state.PackedLocalInertia.InverseMass; + ref var targetSlot = ref GetOffsetInstance(ref inertia, bodyIndexInBundle); + GetFirst(ref targetSlot.InverseInertiaTensor.XX) = state.PackedLocalInertia.InverseMass; + GetFirst(ref targetSlot.InverseInertiaTensor.YX) = 0; + GetFirst(ref targetSlot.InverseInertiaTensor.YY) = state.PackedLocalInertia.InverseMass; + GetFirst(ref targetSlot.InverseInertiaTensor.ZX) = 0; + GetFirst(ref targetSlot.InverseInertiaTensor.ZY) = 0; + GetFirst(ref targetSlot.InverseInertiaTensor.ZZ) = state.PackedLocalInertia.InverseMass; + GetFirst(ref targetSlot.InverseMass) = state.PackedLocalInertia.InverseMass; + } + + /// + /// Gathers motion state information for a body bundle into an AOSOA bundle. + /// + /// Active body indices being gathered. + /// Number of body pairs in the bundle. + /// Gathered absolute position of the body. + /// Gathered orientation of the body. + /// Gathered velocity of the body. + /// Gathered inertia of the body. + //[MethodImpl(MethodImplOptions.AggressiveInlining)] + public void GatherState(ref Vector references, int count, + out Vector3Wide position, out QuaternionWide orientation, out BodyVelocities velocity, out BodyInertias inertia) + { + Unsafe.SkipInit(out position); + Unsafe.SkipInit(out orientation); + Unsafe.SkipInit(out velocity); + Unsafe.SkipInit(out inertia); + //TODO: This function and its users (which should be relatively few) is a problem for large world position precision. + //It directly reports the position, thereby infecting vectorized logic with the high precision representation. + //You might be able to redesign the users of this function to not need it, but that comes with its own difficulties + //(for example, making the grab motor rely on having its goal offset updated every frame by the user). + Debug.Assert(count >= 0 && count <= Vector.Count); + //Grab the base references for the body indices. Note that we make use of the references memory layout again. + ref var baseIndex = ref Unsafe.As, int>(ref references); + + ref var states = ref ActiveSet.MotionStates; + for (int i = 0; i < count; ++i) + { + WriteGatherState(ref baseIndex, i, ref states, ref position, ref orientation, ref velocity); + WriteGatherInertia(ref baseIndex, i, ref Inertias, ref inertia); + } + } + + /// + /// Gathers orientations and relative positions for a two body bundle into an AOSOA bundle. + /// + /// Active body indices being gathered. + /// Number of body pairs in the bundle. + /// Gathered offsets from body A to body B. + /// Gathered orientation of body A. + /// Gathered orientation of body B. + /// Gathered velocity of body A. + /// Gathered velocity of body B. + /// Gathered inertia of body A. + /// Gathered inertia of body B. + //[MethodImpl(MethodImplOptions.AggressiveInlining)] + public void GatherState(ref TwoBodyReferences references, int count, + out QuaternionWide orientationA, out BodyVelocities velocityA, out BodyInertias inertiaA, + out Vector3Wide ab, + out QuaternionWide orientationB, out BodyVelocities velocityB, out BodyInertias inertiaB) + { + Unsafe.SkipInit(out Vector3Wide positionA); + Unsafe.SkipInit(out Vector3Wide positionB); + Unsafe.SkipInit(out orientationA); + Unsafe.SkipInit(out orientationB); + Unsafe.SkipInit(out velocityA); + Unsafe.SkipInit(out velocityB); + Unsafe.SkipInit(out inertiaA); + Unsafe.SkipInit(out inertiaB); + Debug.Assert(count >= 0 && count <= Vector.Count); + //Grab the base references for the body indices. Note that we make use of the references memory layout again. + ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); + ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); + + ref var states = ref ActiveSet.MotionStates; + + for (int i = 0; i < count; ++i) + { + //WriteGatherState(ref baseIndexA, i, ref states, ref positionA, ref orientationA, ref velocityA); + //WriteGatherInertia(ref baseIndexA, i, ref Inertias, ref inertiaA); + //WriteGatherState(ref baseIndexB, i, ref states, ref positionB, ref orientationB, ref velocityB); + //WriteGatherInertia(ref baseIndexB, i, ref Inertias, ref inertiaB); + WriteGatherState(ref baseIndexA, i, ref states, ref positionA, ref orientationA, ref velocityA, ref inertiaA); + WriteGatherState(ref baseIndexB, i, ref states, ref positionB, ref orientationB, ref velocityB, ref inertiaB); + } + //TODO: In future versions, we will likely store the body position in different forms to allow for extremely large worlds. + //That will be an opt-in feature. The default implementation will use the FP32 representation, but the user could choose to swap it out for a fp64 or fixed64 representation. + //This affects other systems- AABB calculation, pose integration, solving, and in extreme (64 bit) cases, the broadphase. + //We want to insulate other systems from direct knowledge about the implementation of positions when possible. + //These functions support the solver's needs while hiding absolute positions. + //In order to support other absolute positions, we'll need alternate implementations of this and other functions. + //But for the most part, we don't want to pay the overhead of an abstract invocation within the inner loop of the solver. + //Given the current limits of C# and the compiler, the best option seems to be conditional compilation. + Vector3Wide.Subtract(positionB, positionA, out ab); + } + + + /// + /// Gathers orientations and relative positions for a three body bundle into an AOSOA bundle. + /// + /// Active body indices being gathered. + /// Number of body pairs in the bundle. + /// Gathered offsets from body A to body B. + /// Gathered offsets from body A to body C. + /// Gathered orientation of body A. + /// Gathered orientation of body B. + /// Gathered orientation of body C. + /// Gathered velocity of body A. + /// Gathered velocity of body B. + /// Gathered velocity of body C. + /// Gathered inertia of body A. + /// Gathered inertia of body B. + /// Gathered inertia of body C. + //[MethodImpl(MethodImplOptions.AggressiveInlining)] + public void GatherState(ref ThreeBodyReferences references, int count, + out QuaternionWide orientationA, out BodyVelocities velocityA, out BodyInertias inertiaA, + out Vector3Wide ab, + out QuaternionWide orientationB, out BodyVelocities velocityB, out BodyInertias inertiaB, + out Vector3Wide ac, + out QuaternionWide orientationC, out BodyVelocities velocityC, out BodyInertias inertiaC) + { + Unsafe.SkipInit(out Vector3Wide positionA); + Unsafe.SkipInit(out Vector3Wide positionB); + Unsafe.SkipInit(out Vector3Wide positionC); + Unsafe.SkipInit(out orientationA); + Unsafe.SkipInit(out orientationB); + Unsafe.SkipInit(out orientationC); + Unsafe.SkipInit(out velocityA); + Unsafe.SkipInit(out velocityB); + Unsafe.SkipInit(out velocityC); + Unsafe.SkipInit(out inertiaA); + Unsafe.SkipInit(out inertiaB); + Unsafe.SkipInit(out inertiaC); + Debug.Assert(count >= 0 && count <= Vector.Count); + //Grab the base references for the body indices. Note that we make use of the references memory layout again. + ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); + ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); + ref var baseIndexC = ref Unsafe.As, int>(ref references.IndexC); + + ref var states = ref ActiveSet.MotionStates; + for (int i = 0; i < count; ++i) + { + WriteGatherState(ref baseIndexA, i, ref states, ref positionA, ref orientationA, ref velocityA); + WriteGatherInertia(ref baseIndexA, i, ref Inertias, ref inertiaA); + WriteGatherState(ref baseIndexB, i, ref states, ref positionB, ref orientationB, ref velocityB); + WriteGatherInertia(ref baseIndexB, i, ref Inertias, ref inertiaB); + WriteGatherState(ref baseIndexC, i, ref states, ref positionC, ref orientationC, ref velocityC); + WriteGatherInertia(ref baseIndexC, i, ref Inertias, ref inertiaC); + } + //TODO: In future versions, we will likely store the body position in different forms to allow for extremely large worlds. + //That will be an opt-in feature. The default implementation will use the FP32 representation, but the user could choose to swap it out for a fp64 or fixed64 representation. + //This affects other systems- AABB calculation, pose integration, solving, and in extreme (64 bit) cases, the broadphase. + //We want to insulate other systems from direct knowledge about the implementation of positions when possible. + //These functions support the solver's needs while hiding absolute positions. + //In order to support other absolute positions, we'll need alternate implementations of this and other functions. + //But for the most part, we don't want to pay the overhead of an abstract invocation within the inner loop of the solver. + //Given the current limits of C# and the compiler, the best option seems to be conditional compilation. + Vector3Wide.Subtract(positionB, positionA, out ab); + Vector3Wide.Subtract(positionC, positionA, out ac); + } + + /// + /// Gathers orientations and relative positions for a four body bundle into an AOSOA bundle. + /// + /// Active body indices being gathered. + /// Number of body pairs in the bundle. + /// Gathered offsets from body A to body B. + /// Gathered offsets from body A to body C. + /// Gathered offsets from body A to body D. + /// Gathered orientation of body A. + /// Gathered orientation of body B. + /// Gathered orientation of body C. + /// Gathered orientation of body D. + /// Gathered velocity of body A. + /// Gathered velocity of body B. + /// Gathered velocity of body C. + /// Gathered velocity of body D. + /// Gathered inertia of body A. + /// Gathered inertia of body B. + /// Gathered inertia of body C. + /// Gathered inertia of body C. + //[MethodImpl(MethodImplOptions.AggressiveInlining)] + public void GatherState(ref FourBodyReferences references, int count, + out QuaternionWide orientationA, out BodyVelocities velocityA, out BodyInertias inertiaA, + out Vector3Wide ab, + out QuaternionWide orientationB, out BodyVelocities velocityB, out BodyInertias inertiaB, + out Vector3Wide ac, + out QuaternionWide orientationC, out BodyVelocities velocityC, out BodyInertias inertiaC, + out Vector3Wide ad, + out QuaternionWide orientationD, out BodyVelocities velocityD, out BodyInertias inertiaD) + { + Unsafe.SkipInit(out Vector3Wide positionA); + Unsafe.SkipInit(out Vector3Wide positionB); + Unsafe.SkipInit(out Vector3Wide positionC); + Unsafe.SkipInit(out Vector3Wide positionD); + Unsafe.SkipInit(out orientationA); + Unsafe.SkipInit(out orientationB); + Unsafe.SkipInit(out orientationC); + Unsafe.SkipInit(out orientationD); + Unsafe.SkipInit(out velocityA); + Unsafe.SkipInit(out velocityB); + Unsafe.SkipInit(out velocityC); + Unsafe.SkipInit(out velocityD); + Unsafe.SkipInit(out inertiaA); + Unsafe.SkipInit(out inertiaB); + Unsafe.SkipInit(out inertiaC); + Unsafe.SkipInit(out inertiaD); + Debug.Assert(count >= 0 && count <= Vector.Count); + //Grab the base references for the body indices. Note that we make use of the references memory layout again. + ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); + ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); + ref var baseIndexC = ref Unsafe.As, int>(ref references.IndexC); + ref var baseIndexD = ref Unsafe.As, int>(ref references.IndexD); + + ref var states = ref ActiveSet.MotionStates; + for (int i = 0; i < count; ++i) + { + WriteGatherState(ref baseIndexA, i, ref states, ref positionA, ref orientationA, ref velocityA); + WriteGatherInertia(ref baseIndexA, i, ref Inertias, ref inertiaA); + WriteGatherState(ref baseIndexB, i, ref states, ref positionB, ref orientationB, ref velocityB); + WriteGatherInertia(ref baseIndexB, i, ref Inertias, ref inertiaB); + WriteGatherState(ref baseIndexC, i, ref states, ref positionC, ref orientationC, ref velocityC); + WriteGatherInertia(ref baseIndexC, i, ref Inertias, ref inertiaC); + WriteGatherState(ref baseIndexD, i, ref states, ref positionD, ref orientationD, ref velocityD); + WriteGatherInertia(ref baseIndexD, i, ref Inertias, ref inertiaD); + } + //TODO: In future versions, we will likely store the body position in different forms to allow for extremely large worlds. + //That will be an opt-in feature. The default implementation will use the FP32 representation, but the user could choose to swap it out for a fp64 or fixed64 representation. + //This affects other systems- AABB calculation, pose integration, solving, and in extreme (64 bit) cases, the broadphase. + //We want to insulate other systems from direct knowledge about the implementation of positions when possible. + //These functions support the solver's needs while hiding absolute positions. + //In order to support other absolute positions, we'll need alternate implementations of this and other functions. + //But for the most part, we don't want to pay the overhead of an abstract invocation within the inner loop of the solver. + //Given the current limits of C# and the compiler, the best option seems to be conditional compilation. + Vector3Wide.Subtract(positionB, positionA, out ab); + Vector3Wide.Subtract(positionC, positionA, out ac); + Vector3Wide.Subtract(positionC, positionA, out ad); + } + + //[MethodImpl(MethodImplOptions.AggressiveInlining)] + //private static void WriteLinearGatherState(ref int baseBodyIndexInSet, int bodyIndexInBundle, ref Buffer states, ref Vector3Wide linearVelocity, ref Vector3Wide position) + //{ + // ref var state = ref states[Unsafe.Add(ref baseBodyIndexInSet, bodyIndexInBundle)]; + // Vector3Wide.WriteFirst(state.Pose.Position, ref GetOffsetInstance(ref position, bodyIndexInBundle)); + // Vector3Wide.WriteFirst(state.Velocity.Linear, ref GetOffsetInstance(ref linearVelocity, bodyIndexInBundle)); + //} + ///// + ///// Gathers relative positions and linear velocities for a two body bundle into an AOSOA bundle. + ///// + ///// Active body indices being gathered. + ///// Number of body pairs in the bundle. + ///// Gathered offset from body A to body B. + ///// Linear velocity of body A. + ///// Linear velocity of body B. + ////[MethodImpl(MethodImplOptions.AggressiveInlining)] + //public void GatherLinearState(ref TwoBodyReferences references, int count, out Vector3Wide ab, out Vector3Wide linearVelocityA, out Vector3Wide linearVelocityB) + //{ + // Unsafe.SkipInit(out Vector3Wide positionA); + // Unsafe.SkipInit(out Vector3Wide positionB); + // Unsafe.SkipInit(out linearVelocityA); + // Unsafe.SkipInit(out linearVelocityB); + // Debug.Assert(count >= 0 && count <= Vector.Count); + // //Grab the base references for the body indices. Note that we make use of the references memory layout again. + // ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); + // ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); + + // ref var states = ref ActiveSet.MotionStates; + // for (int i = 0; i < count; ++i) + // { + // WriteLinearGatherState(ref baseIndexA, i, ref states, ref positionA, ref linearVelocityA); + // WriteLinearGatherState(ref baseIndexB, i, ref states, ref positionB, ref linearVelocityB); + // } + // //Same as other gather case; this is sensitive to changes in the representation of body position. In high precision modes, this'll need to change. + // Vector3Wide.Subtract(positionB, positionA, out ab); + //} + + + ///// + ///// Gathers relative positions and linear velocities for a three body bundle into an AOSOA bundle. + ///// + ///// Active body indices being gathered. + ///// Number of body pairs in the bundle. + ///// Gathered offset from body A to body B. + ///// Gathered offset from body A to body C. + ///// Linear velocity of body A. + ///// Linear velocity of body B. + ///// Linear velocity of body C. + ////[MethodImpl(MethodImplOptions.AggressiveInlining)] + //public void GatherLinearState(ref ThreeBodyReferences references, int count, out Vector3Wide ab, out Vector3Wide ac, out Vector3Wide linearVelocityA, out Vector3Wide linearVelocityB, out Vector3Wide linearVelocityC) + //{ + // Unsafe.SkipInit(out Vector3Wide positionA); + // Unsafe.SkipInit(out Vector3Wide positionB); + // Unsafe.SkipInit(out Vector3Wide positionC); + // Unsafe.SkipInit(out linearVelocityA); + // Unsafe.SkipInit(out linearVelocityB); + // Unsafe.SkipInit(out linearVelocityC); + // Debug.Assert(count >= 0 && count <= Vector.Count); + // //Grab the base references for the body indices. Note that we make use of the references memory layout again. + // ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); + // ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); + // ref var baseIndexC = ref Unsafe.As, int>(ref references.IndexC); + + // ref var states = ref ActiveSet.MotionStates; + // for (int i = 0; i < count; ++i) + // { + // WriteLinearGatherState(ref baseIndexA, i, ref states, ref positionA, ref linearVelocityA); + // WriteLinearGatherState(ref baseIndexB, i, ref states, ref positionB, ref linearVelocityB); + // WriteLinearGatherState(ref baseIndexC, i, ref states, ref positionC, ref linearVelocityC); + // } + // //Same as other gather case; this is sensitive to changes in the representation of body position. In high precision modes, this'll need to change. + // Vector3Wide.Subtract(positionB, positionA, out ab); + // Vector3Wide.Subtract(positionC, positionA, out ac); + //} + + + ///// + ///// Gathers relative positions and linear velocities for a four body bundle into an AOSOA bundle. + ///// + ///// Active body indices being gathered. + ///// Number of body pairs in the bundle. + ///// Gathered offset from body A to body B. + ///// Gathered offset from body A to body C. + ///// Gathered offset from body A to body D. + ///// Linear velocity of body A. + ///// Linear velocity of body B. + ///// Linear velocity of body C. + ///// Linear velocity of body D. + ////[MethodImpl(MethodImplOptions.AggressiveInlining)] + //public void GatherLinearState(ref FourBodyReferences references, int count, out Vector3Wide ab, out Vector3Wide ac, out Vector3Wide ad, + // out Vector3Wide linearVelocityA, out Vector3Wide linearVelocityB, out Vector3Wide linearVelocityC, out Vector3Wide linearVelocityD) + //{ + // Unsafe.SkipInit(out Vector3Wide positionA); + // Unsafe.SkipInit(out Vector3Wide positionB); + // Unsafe.SkipInit(out Vector3Wide positionC); + // Unsafe.SkipInit(out Vector3Wide positionD); + // Unsafe.SkipInit(out linearVelocityA); + // Unsafe.SkipInit(out linearVelocityB); + // Unsafe.SkipInit(out linearVelocityC); + // Unsafe.SkipInit(out linearVelocityD); + // Debug.Assert(count >= 0 && count <= Vector.Count); + // //Grab the base references for the body indices. Note that we make use of the references memory layout again. + // ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); + // ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); + // ref var baseIndexC = ref Unsafe.As, int>(ref references.IndexC); + // ref var baseIndexD = ref Unsafe.As, int>(ref references.IndexD); + + // ref var states = ref ActiveSet.MotionStates; + // for (int i = 0; i < count; ++i) + // { + // WriteLinearGatherState(ref baseIndexA, i, ref states, ref positionA, ref linearVelocityA); + // WriteLinearGatherState(ref baseIndexB, i, ref states, ref positionB, ref linearVelocityB); + // WriteLinearGatherState(ref baseIndexC, i, ref states, ref positionC, ref linearVelocityC); + // WriteLinearGatherState(ref baseIndexD, i, ref states, ref positionD, ref linearVelocityD); + // } + // //Same as other gather case; this is sensitive to changes in the representation of body position. In high precision modes, this'll need to change. + // Vector3Wide.Subtract(positionB, positionA, out ab); + // Vector3Wide.Subtract(positionC, positionA, out ac); + // Vector3Wide.Subtract(positionD, positionA, out ad); + //} + + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + private unsafe void ScatterVelocities(ref BodyVelocities sourceVelocities, ref int baseIndex, int innerIndex) + { + //TODO: How much value would there be in branching on kinematic state and avoiding a write? Depends a lot on the number of kinematics. + ref var sourceSlot = ref GetOffsetInstance(ref sourceVelocities, innerIndex); + ref var target = ref ActiveSet.MotionStates[Unsafe.Add(ref baseIndex, innerIndex)].Velocity; + target.Linear = new Vector3(sourceSlot.Linear.X[0], sourceSlot.Linear.Y[0], sourceSlot.Linear.Z[0]); + target.Angular = new Vector3(sourceSlot.Angular.X[0], sourceSlot.Angular.Y[0], sourceSlot.Angular.Z[0]); + } + + + /// + /// Scatters velocities for one body bundle into the active body set. + /// + /// Velocities of body bundle A to scatter. + /// Active set indices of the bodies to scatter velocity data to. + /// Number of body pairs in the bundle. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public unsafe void ScatterVelocities(ref BodyVelocities sourceVelocities, ref Vector references, int count) + { + Debug.Assert(count >= 0 && count <= Vector.Count); + //Grab the base references for the body indices. Note that we make use of the references memory layout again. + ref var baseIndex = ref Unsafe.As, int>(ref references); + for (int i = 0; i < count; ++i) + { + ScatterVelocities(ref sourceVelocities, ref baseIndex, i); + } + } + + /// + /// Scatters velocities for two body bundles into the active body set. + /// + /// Velocities of body bundle A to scatter. + /// Velocities of body bundle B to scatter. + /// Active set indices of the bodies to scatter velocity data to. + /// Number of body pairs in the bundle. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public unsafe void ScatterVelocities(ref BodyVelocities sourceVelocitiesA, ref BodyVelocities sourceVelocitiesB, + ref TwoBodyReferences references, int count) + { + Debug.Assert(count >= 0 && count <= Vector.Count); + //Grab the base references for the body indices. Note that we make use of the references memory layout again. + ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); + ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); + for (int i = 0; i < count; ++i) + { + ScatterVelocities(ref sourceVelocitiesA, ref baseIndexA, i); + ScatterVelocities(ref sourceVelocitiesB, ref baseIndexB, i); + } + } + + /// + /// Scatters velocities for three body bundles into the active body set. + /// + /// Velocities of body bundle A to scatter. + /// Velocities of body bundle B to scatter. + /// Velocities of body bundle C to scatter. + /// Active set indices of the bodies to scatter velocity data to. + /// Number of body pairs in the bundle. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public unsafe void ScatterVelocities( + ref BodyVelocities sourceVelocitiesA, ref BodyVelocities sourceVelocitiesB, ref BodyVelocities sourceVelocitiesC, + ref ThreeBodyReferences references, int count) + { + Debug.Assert(count >= 0 && count <= Vector.Count); + //Grab the base references for the body indices. Note that we make use of the references memory layout again. + ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); + ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); + ref var baseIndexC = ref Unsafe.As, int>(ref references.IndexC); + for (int i = 0; i < count; ++i) + { + ScatterVelocities(ref sourceVelocitiesA, ref baseIndexA, i); + ScatterVelocities(ref sourceVelocitiesB, ref baseIndexB, i); + ScatterVelocities(ref sourceVelocitiesC, ref baseIndexC, i); + } + } + + /// + /// Scatters velocities for four body bundles into the active body set. + /// + /// Velocities of body bundle A to scatter. + /// Velocities of body bundle B to scatter. + /// Velocities of body bundle C to scatter. + /// Velocities of body bundle D to scatter. + /// Active set indices of the bodies to scatter velocity data to. + /// Number of body pairs in the bundle. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public unsafe void ScatterVelocities( + ref BodyVelocities sourceVelocitiesA, ref BodyVelocities sourceVelocitiesB, ref BodyVelocities sourceVelocitiesC, ref BodyVelocities sourceVelocitiesD, + ref FourBodyReferences references, int count) + { + Debug.Assert(count >= 0 && count <= Vector.Count); + //Grab the base references for the body indices. Note that we make use of the references memory layout again. + ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); + ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); + ref var baseIndexC = ref Unsafe.As, int>(ref references.IndexC); + ref var baseIndexD = ref Unsafe.As, int>(ref references.IndexD); + for (int i = 0; i < count; ++i) + { + ScatterVelocities(ref sourceVelocitiesA, ref baseIndexA, i); + ScatterVelocities(ref sourceVelocitiesB, ref baseIndexB, i); + ScatterVelocities(ref sourceVelocitiesC, ref baseIndexC, i); + ScatterVelocities(ref sourceVelocitiesD, ref baseIndexD, i); + } + } + } +} diff --git a/Demos/Program.cs b/Demos/Program.cs index 6c769ece5..7118826dd 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -19,7 +19,7 @@ static void Main(string[] args) { content = ContentArchive.Load(stream); } - HeadlessTest.Test(content, 4, 32, 512); + //HeadlessTest.Test(content, 4, 32, 512); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); From 4643f7ae6e5a4b64054a74a8c57104784b763da7 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 30 Jun 2021 21:02:33 -0500 Subject: [PATCH 053/947] AVX2 gather; expected levels of bleh --- BepuPhysics/Bodies_GatherScatter.cs | 106 +++++++++++++++++++++++++--- BepuPhysics/BodyProperties.cs | 18 +++++ Demos/Program.cs | 2 +- 3 files changed, 114 insertions(+), 12 deletions(-) diff --git a/BepuPhysics/Bodies_GatherScatter.cs b/BepuPhysics/Bodies_GatherScatter.cs index 5fc46cece..13fe6fe0f 100644 --- a/BepuPhysics/Bodies_GatherScatter.cs +++ b/BepuPhysics/Bodies_GatherScatter.cs @@ -8,11 +8,13 @@ using BepuPhysics.CollisionDetection; using BepuUtilities; using static BepuUtilities.GatherScatter; +using System.Runtime.Intrinsics.X86; +using System.Runtime.Intrinsics; namespace BepuPhysics -{ +{ public partial class Bodies - { + { [MethodImpl(MethodImplOptions.AggressiveInlining)] private static void WriteGatherInertia(ref int bundleBaseBodyIndexInSet, int bodyIndexInBundle, ref Buffer states, ref BodyInertias gatheredInertias) @@ -112,7 +114,7 @@ public void GatherState(ref Vector references, int count, /// Gathered inertia of body A. /// Gathered inertia of body B. //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public void GatherState(ref TwoBodyReferences references, int count, + public unsafe void GatherState(ref TwoBodyReferences references, int count, out QuaternionWide orientationA, out BodyVelocities velocityA, out BodyInertias inertiaA, out Vector3Wide ab, out QuaternionWide orientationB, out BodyVelocities velocityB, out BodyInertias inertiaB) @@ -131,15 +133,97 @@ public void GatherState(ref TwoBodyReferences references, int count, ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); ref var states = ref ActiveSet.MotionStates; - - for (int i = 0; i < count; ++i) + + + if (Avx2.IsSupported) + { + var maskMemory = stackalloc int[8]; + for (int i = 0; i < count; ++i) + { + maskMemory[i] = -1; + } + for (int i = count; i < 8; ++i) + { + maskMemory[i] = 0; + } + var mask = Avx.LoadVector256(maskMemory); + var statesMemory = (float*)states.Memory; + var indexA = Avx2.ShiftLeftLogical(references.IndexA.AsVector256(), 6); + var indexB = Avx2.ShiftLeftLogical(references.IndexB.AsVector256(), 6); + var zero = Vector256.Zero; + var offsetX = Vector256.Create(MotionState.OffsetToPositionX); + positionA.X = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetX), mask.AsSingle(), 1).AsVector(); + positionB.X = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetX), mask.AsSingle(), 1).AsVector(); + var offsetY = Vector256.Create(MotionState.OffsetToPositionY); + positionA.Y = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetY), mask.AsSingle(), 1).AsVector(); + positionB.Y = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetY), mask.AsSingle(), 1).AsVector(); + var offsetZ = Vector256.Create(MotionState.OffsetToPositionZ); + positionA.Z = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetZ), mask.AsSingle(), 1).AsVector(); + positionB.Z = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetZ), mask.AsSingle(), 1).AsVector(); + + var offsetOrientationX = Vector256.Create(MotionState.OffsetToOrientationX); + orientationA.X = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetOrientationX), mask.AsSingle(), 1).AsVector(); + orientationB.X = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetOrientationX), mask.AsSingle(), 1).AsVector(); + var offsetOrientationY = Vector256.Create(MotionState.OffsetToOrientationY); + orientationA.Y = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetOrientationY), mask.AsSingle(), 1).AsVector(); + orientationB.Y = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetOrientationY), mask.AsSingle(), 1).AsVector(); + var offsetOrientationZ = Vector256.Create(MotionState.OffsetToOrientationZ); + orientationA.Z = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetOrientationZ), mask.AsSingle(), 1).AsVector(); + orientationB.Z = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetOrientationZ), mask.AsSingle(), 1).AsVector(); + var offsetOrientationW = Vector256.Create(MotionState.OffsetToOrientationW); + orientationA.W = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetOrientationW), mask.AsSingle(), 1).AsVector(); + orientationB.W = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetOrientationW), mask.AsSingle(), 1).AsVector(); + + var offsetLinearX = Vector256.Create(MotionState.OffsetToLinearX); + velocityA.Linear.X = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetLinearX), mask.AsSingle(), 1).AsVector(); + velocityB.Linear.X = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetLinearX), mask.AsSingle(), 1).AsVector(); + var offsetLinearY = Vector256.Create(MotionState.OffsetToLinearY); + velocityA.Linear.Y = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetLinearY), mask.AsSingle(), 1).AsVector(); + velocityB.Linear.Y = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetLinearY), mask.AsSingle(), 1).AsVector(); + var offsetLinearZ = Vector256.Create(MotionState.OffsetToLinearZ); + velocityA.Linear.Z = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetLinearZ), mask.AsSingle(), 1).AsVector(); + velocityB.Linear.Z = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetLinearZ), mask.AsSingle(), 1).AsVector(); + + var offsetAngularX = Vector256.Create(MotionState.OffsetToAngularX); + velocityA.Angular.X = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetAngularX), mask.AsSingle(), 1).AsVector(); + velocityB.Angular.X = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetAngularX), mask.AsSingle(), 1).AsVector(); + var offsetAngularY = Vector256.Create(MotionState.OffsetToAngularY); + velocityA.Angular.Y = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetAngularY), mask.AsSingle(), 1).AsVector(); + velocityB.Angular.Y = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetAngularY), mask.AsSingle(), 1).AsVector(); + var offsetAngularZ = Vector256.Create(MotionState.OffsetToAngularZ); + velocityA.Angular.Z = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetAngularZ), mask.AsSingle(), 1).AsVector(); + velocityB.Angular.Z = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetAngularZ), mask.AsSingle(), 1).AsVector(); + + var offsetInverseMass = Vector256.Create(MotionState.OffsetToInverseMass); + inertiaA.InverseMass = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetInverseMass), mask.AsSingle(), 1).AsVector(); + inertiaB.InverseMass = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetInverseMass), mask.AsSingle(), 1).AsVector(); + var offsetInverseInertiaXX = Vector256.Create(MotionState.OffsetToInverseInertiaXX); + inertiaA.InverseInertiaTensor.XX = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetInverseInertiaXX), mask.AsSingle(), 1).AsVector(); + inertiaB.InverseInertiaTensor.XX = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetInverseInertiaXX), mask.AsSingle(), 1).AsVector(); + var offsetInverseInertiaYY = Vector256.Create(MotionState.OffsetToInverseInertiaYY); + inertiaA.InverseInertiaTensor.YY = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetInverseInertiaYY), mask.AsSingle(), 1).AsVector(); + inertiaB.InverseInertiaTensor.YY = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetInverseInertiaYY), mask.AsSingle(), 1).AsVector(); + var offsetInverseInertiaZZ = Vector256.Create(MotionState.OffsetToInverseInertiaZZ); + inertiaA.InverseInertiaTensor.ZZ = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetInverseInertiaZZ), mask.AsSingle(), 1).AsVector(); + inertiaB.InverseInertiaTensor.ZZ = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetInverseInertiaZZ), mask.AsSingle(), 1).AsVector(); + inertiaA.InverseInertiaTensor.YX = default; + inertiaB.InverseInertiaTensor.YX = default; + inertiaB.InverseInertiaTensor.ZX = default; + inertiaA.InverseInertiaTensor.ZX = default; + inertiaA.InverseInertiaTensor.ZY = default; + inertiaB.InverseInertiaTensor.ZY = default; + } + else { - //WriteGatherState(ref baseIndexA, i, ref states, ref positionA, ref orientationA, ref velocityA); - //WriteGatherInertia(ref baseIndexA, i, ref Inertias, ref inertiaA); - //WriteGatherState(ref baseIndexB, i, ref states, ref positionB, ref orientationB, ref velocityB); - //WriteGatherInertia(ref baseIndexB, i, ref Inertias, ref inertiaB); - WriteGatherState(ref baseIndexA, i, ref states, ref positionA, ref orientationA, ref velocityA, ref inertiaA); - WriteGatherState(ref baseIndexB, i, ref states, ref positionB, ref orientationB, ref velocityB, ref inertiaB); + for (int i = 0; i < count; ++i) + { + //WriteGatherState(ref baseIndexA, i, ref states, ref positionA, ref orientationA, ref velocityA); + //WriteGatherInertia(ref baseIndexA, i, ref Inertias, ref inertiaA); + //WriteGatherState(ref baseIndexB, i, ref states, ref positionB, ref orientationB, ref velocityB); + //WriteGatherInertia(ref baseIndexB, i, ref Inertias, ref inertiaB); + WriteGatherState(ref baseIndexA, i, ref states, ref positionA, ref orientationA, ref velocityA, ref inertiaA); + WriteGatherState(ref baseIndexB, i, ref states, ref positionB, ref orientationB, ref velocityB, ref inertiaB); + } } //TODO: In future versions, we will likely store the body position in different forms to allow for extremely large worlds. //That will be an opt-in feature. The default implementation will use the FP32 representation, but the user could choose to swap it out for a fp64 or fixed64 representation. diff --git a/BepuPhysics/BodyProperties.cs b/BepuPhysics/BodyProperties.cs index a610e0ff2..8861d3c6a 100644 --- a/BepuPhysics/BodyProperties.cs +++ b/BepuPhysics/BodyProperties.cs @@ -61,6 +61,24 @@ public readonly BodyInertia Unpack() [StructLayout(LayoutKind.Sequential, Size = 64, Pack = 1)] public struct MotionState { + internal const int OffsetToPositionX = 0; + internal const int OffsetToPositionY = 4; + internal const int OffsetToPositionZ = 8; + internal const int OffsetToOrientationX = 12; + internal const int OffsetToOrientationY = 16; + internal const int OffsetToOrientationZ = 20; + internal const int OffsetToOrientationW = 24; + internal const int OffsetToLinearX = 28; + internal const int OffsetToLinearY = 32; + internal const int OffsetToLinearZ = 36; + internal const int OffsetToAngularX = 40; + internal const int OffsetToAngularY = 44; + internal const int OffsetToAngularZ = 48; + internal const int OffsetToInverseMass = 52; + internal const int OffsetToInverseInertiaXX = OffsetToInverseMass; + internal const int OffsetToInverseInertiaYY = OffsetToInverseMass; + internal const int OffsetToInverseInertiaZZ = OffsetToInverseMass; + /// /// Pose of the body. /// diff --git a/Demos/Program.cs b/Demos/Program.cs index 7118826dd..6c769ece5 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -19,7 +19,7 @@ static void Main(string[] args) { content = ContentArchive.Load(stream); } - //HeadlessTest.Test(content, 4, 32, 512); + HeadlessTest.Test(content, 4, 32, 512); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); From 257cfed2d11e34d30e3fb6b7badc0a455e47def2 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 1 Jul 2021 11:49:24 -0500 Subject: [PATCH 054/947] Scalar gather baseline. --- BepuPhysics/Bodies_GatherScatter.cs | 114 +++++++++++++++++++++------- BepuPhysics/BodyProperties.cs | 45 +++++++---- 2 files changed, 117 insertions(+), 42 deletions(-) diff --git a/BepuPhysics/Bodies_GatherScatter.cs b/BepuPhysics/Bodies_GatherScatter.cs index 13fe6fe0f..24fefc39b 100644 --- a/BepuPhysics/Bodies_GatherScatter.cs +++ b/BepuPhysics/Bodies_GatherScatter.cs @@ -101,6 +101,58 @@ public void GatherState(ref Vector references, int count, } } + //[MethodImpl(MethodImplOptions.AggressiveInlining)] + unsafe static void ScalarGather(int count, MotionState* motionStates, ref Vector baseIndex, ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocities velocity, ref BodyInertias inertia) + { + var indices = (int*)Unsafe.AsPointer(ref baseIndex); + var pPositionX = (float*)Unsafe.AsPointer(ref position.X); + var pPositionY = (float*)Unsafe.AsPointer(ref position.Y); + var pPositionZ = (float*)Unsafe.AsPointer(ref position.Z); + var pOrientationX = (float*)Unsafe.AsPointer(ref orientation.X); + var pOrientationY = (float*)Unsafe.AsPointer(ref orientation.Y); + var pOrientationZ = (float*)Unsafe.AsPointer(ref orientation.Z); + var pOrientationW = (float*)Unsafe.AsPointer(ref orientation.W); + var pLinearX = (float*)Unsafe.AsPointer(ref velocity.Linear.X); + var pLinearY = (float*)Unsafe.AsPointer(ref velocity.Linear.Y); + var pLinearZ = (float*)Unsafe.AsPointer(ref velocity.Linear.Z); + var pAngularX = (float*)Unsafe.AsPointer(ref velocity.Angular.X); + var pAngularY = (float*)Unsafe.AsPointer(ref velocity.Angular.Y); + var pAngularZ = (float*)Unsafe.AsPointer(ref velocity.Angular.Z); + var pMass = (float*)Unsafe.AsPointer(ref inertia.InverseMass); + var pInertiaXX = (float*)Unsafe.AsPointer(ref inertia.InverseInertiaTensor.XX); + var pInertiaYX = (float*)Unsafe.AsPointer(ref inertia.InverseInertiaTensor.YX); + var pInertiaYY = (float*)Unsafe.AsPointer(ref inertia.InverseInertiaTensor.YY); + var pInertiaZX = (float*)Unsafe.AsPointer(ref inertia.InverseInertiaTensor.ZX); + var pInertiaZY = (float*)Unsafe.AsPointer(ref inertia.InverseInertiaTensor.ZY); + var pInertiaZZ = (float*)Unsafe.AsPointer(ref inertia.InverseInertiaTensor.ZZ); + + for (int i = 0; i < count; ++i) + { + var index = indices[i]; + var stateValues = (float*)(motionStates + index); + pPositionX[i] = stateValues[MotionState.OffsetToPositionX]; + pPositionY[i] = stateValues[MotionState.OffsetToPositionY]; + pPositionZ[i] = stateValues[MotionState.OffsetToPositionZ]; + pOrientationX[i] = stateValues[MotionState.OffsetToOrientationX]; + pOrientationY[i] = stateValues[MotionState.OffsetToOrientationY]; + pOrientationZ[i] = stateValues[MotionState.OffsetToOrientationZ]; + pOrientationW[i] = stateValues[MotionState.OffsetToOrientationW]; + pLinearX[i] = stateValues[MotionState.OffsetToLinearX]; + pLinearY[i] = stateValues[MotionState.OffsetToLinearY]; + pLinearZ[i] = stateValues[MotionState.OffsetToLinearZ]; + pAngularX[i] = stateValues[MotionState.OffsetToAngularX]; + pAngularY[i] = stateValues[MotionState.OffsetToAngularY]; + pAngularZ[i] = stateValues[MotionState.OffsetToAngularZ]; + pMass[i] = stateValues[MotionState.OffsetToInverseMass]; + pInertiaXX[i] = stateValues[MotionState.OffsetToInverseInertiaXX]; + pInertiaYX[i] = 0; + pInertiaYY[i] = stateValues[MotionState.OffsetToInverseInertiaYY]; + pInertiaZX[i] = 0; + pInertiaZY[i] = 0; + pInertiaZZ[i] = stateValues[MotionState.OffsetToInverseInertiaZZ]; + } + } + /// /// Gathers orientations and relative positions for a two body bundle into an AOSOA bundle. /// @@ -115,9 +167,9 @@ public void GatherState(ref Vector references, int count, /// Gathered inertia of body B. //[MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe void GatherState(ref TwoBodyReferences references, int count, - out QuaternionWide orientationA, out BodyVelocities velocityA, out BodyInertias inertiaA, - out Vector3Wide ab, - out QuaternionWide orientationB, out BodyVelocities velocityB, out BodyInertias inertiaB) + out QuaternionWide orientationA, out BodyVelocities velocityA, out BodyInertias inertiaA, + out Vector3Wide ab, + out QuaternionWide orientationB, out BodyVelocities velocityB, out BodyInertias inertiaB) { Unsafe.SkipInit(out Vector3Wide positionA); Unsafe.SkipInit(out Vector3Wide positionB); @@ -151,59 +203,59 @@ public unsafe void GatherState(ref TwoBodyReferences references, int count, var indexA = Avx2.ShiftLeftLogical(references.IndexA.AsVector256(), 6); var indexB = Avx2.ShiftLeftLogical(references.IndexB.AsVector256(), 6); var zero = Vector256.Zero; - var offsetX = Vector256.Create(MotionState.OffsetToPositionX); + var offsetX = Vector256.Create(MotionState.ByteOffsetToPositionX); positionA.X = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetX), mask.AsSingle(), 1).AsVector(); positionB.X = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetX), mask.AsSingle(), 1).AsVector(); - var offsetY = Vector256.Create(MotionState.OffsetToPositionY); + var offsetY = Vector256.Create(MotionState.ByteOffsetToPositionY); positionA.Y = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetY), mask.AsSingle(), 1).AsVector(); positionB.Y = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetY), mask.AsSingle(), 1).AsVector(); - var offsetZ = Vector256.Create(MotionState.OffsetToPositionZ); + var offsetZ = Vector256.Create(MotionState.ByteOffsetToPositionZ); positionA.Z = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetZ), mask.AsSingle(), 1).AsVector(); positionB.Z = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetZ), mask.AsSingle(), 1).AsVector(); - var offsetOrientationX = Vector256.Create(MotionState.OffsetToOrientationX); + var offsetOrientationX = Vector256.Create(MotionState.ByteOffsetToOrientationX); orientationA.X = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetOrientationX), mask.AsSingle(), 1).AsVector(); orientationB.X = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetOrientationX), mask.AsSingle(), 1).AsVector(); - var offsetOrientationY = Vector256.Create(MotionState.OffsetToOrientationY); + var offsetOrientationY = Vector256.Create(MotionState.ByteOffsetToOrientationY); orientationA.Y = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetOrientationY), mask.AsSingle(), 1).AsVector(); orientationB.Y = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetOrientationY), mask.AsSingle(), 1).AsVector(); - var offsetOrientationZ = Vector256.Create(MotionState.OffsetToOrientationZ); + var offsetOrientationZ = Vector256.Create(MotionState.ByteOffsetToOrientationZ); orientationA.Z = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetOrientationZ), mask.AsSingle(), 1).AsVector(); orientationB.Z = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetOrientationZ), mask.AsSingle(), 1).AsVector(); - var offsetOrientationW = Vector256.Create(MotionState.OffsetToOrientationW); + var offsetOrientationW = Vector256.Create(MotionState.ByteOffsetToOrientationW); orientationA.W = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetOrientationW), mask.AsSingle(), 1).AsVector(); orientationB.W = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetOrientationW), mask.AsSingle(), 1).AsVector(); - var offsetLinearX = Vector256.Create(MotionState.OffsetToLinearX); + var offsetLinearX = Vector256.Create(MotionState.ByteOffsetToLinearX); velocityA.Linear.X = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetLinearX), mask.AsSingle(), 1).AsVector(); velocityB.Linear.X = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetLinearX), mask.AsSingle(), 1).AsVector(); - var offsetLinearY = Vector256.Create(MotionState.OffsetToLinearY); + var offsetLinearY = Vector256.Create(MotionState.ByteOffsetToLinearY); velocityA.Linear.Y = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetLinearY), mask.AsSingle(), 1).AsVector(); velocityB.Linear.Y = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetLinearY), mask.AsSingle(), 1).AsVector(); - var offsetLinearZ = Vector256.Create(MotionState.OffsetToLinearZ); + var offsetLinearZ = Vector256.Create(MotionState.ByteOffsetToLinearZ); velocityA.Linear.Z = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetLinearZ), mask.AsSingle(), 1).AsVector(); velocityB.Linear.Z = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetLinearZ), mask.AsSingle(), 1).AsVector(); - var offsetAngularX = Vector256.Create(MotionState.OffsetToAngularX); + var offsetAngularX = Vector256.Create(MotionState.ByteOffsetToAngularX); velocityA.Angular.X = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetAngularX), mask.AsSingle(), 1).AsVector(); velocityB.Angular.X = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetAngularX), mask.AsSingle(), 1).AsVector(); - var offsetAngularY = Vector256.Create(MotionState.OffsetToAngularY); + var offsetAngularY = Vector256.Create(MotionState.ByteOffsetToAngularY); velocityA.Angular.Y = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetAngularY), mask.AsSingle(), 1).AsVector(); velocityB.Angular.Y = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetAngularY), mask.AsSingle(), 1).AsVector(); - var offsetAngularZ = Vector256.Create(MotionState.OffsetToAngularZ); + var offsetAngularZ = Vector256.Create(MotionState.ByteOffsetToAngularZ); velocityA.Angular.Z = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetAngularZ), mask.AsSingle(), 1).AsVector(); velocityB.Angular.Z = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetAngularZ), mask.AsSingle(), 1).AsVector(); - var offsetInverseMass = Vector256.Create(MotionState.OffsetToInverseMass); + var offsetInverseMass = Vector256.Create(MotionState.ByteOffsetToInverseMass); inertiaA.InverseMass = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetInverseMass), mask.AsSingle(), 1).AsVector(); inertiaB.InverseMass = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetInverseMass), mask.AsSingle(), 1).AsVector(); - var offsetInverseInertiaXX = Vector256.Create(MotionState.OffsetToInverseInertiaXX); + var offsetInverseInertiaXX = Vector256.Create(MotionState.ByteOffsetToInverseInertiaXX); inertiaA.InverseInertiaTensor.XX = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetInverseInertiaXX), mask.AsSingle(), 1).AsVector(); inertiaB.InverseInertiaTensor.XX = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetInverseInertiaXX), mask.AsSingle(), 1).AsVector(); - var offsetInverseInertiaYY = Vector256.Create(MotionState.OffsetToInverseInertiaYY); + var offsetInverseInertiaYY = Vector256.Create(MotionState.ByteOffsetToInverseInertiaYY); inertiaA.InverseInertiaTensor.YY = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetInverseInertiaYY), mask.AsSingle(), 1).AsVector(); inertiaB.InverseInertiaTensor.YY = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetInverseInertiaYY), mask.AsSingle(), 1).AsVector(); - var offsetInverseInertiaZZ = Vector256.Create(MotionState.OffsetToInverseInertiaZZ); + var offsetInverseInertiaZZ = Vector256.Create(MotionState.ByteOffsetToInverseInertiaZZ); inertiaA.InverseInertiaTensor.ZZ = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetInverseInertiaZZ), mask.AsSingle(), 1).AsVector(); inertiaB.InverseInertiaTensor.ZZ = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetInverseInertiaZZ), mask.AsSingle(), 1).AsVector(); inertiaA.InverseInertiaTensor.YX = default; @@ -215,15 +267,19 @@ public unsafe void GatherState(ref TwoBodyReferences references, int count, } else { - for (int i = 0; i < count; ++i) - { - //WriteGatherState(ref baseIndexA, i, ref states, ref positionA, ref orientationA, ref velocityA); - //WriteGatherInertia(ref baseIndexA, i, ref Inertias, ref inertiaA); - //WriteGatherState(ref baseIndexB, i, ref states, ref positionB, ref orientationB, ref velocityB); - //WriteGatherInertia(ref baseIndexB, i, ref Inertias, ref inertiaB); - WriteGatherState(ref baseIndexA, i, ref states, ref positionA, ref orientationA, ref velocityA, ref inertiaA); - WriteGatherState(ref baseIndexB, i, ref states, ref positionB, ref orientationB, ref velocityB, ref inertiaB); - } + ScalarGather(count, states.Memory, ref references.IndexA, ref positionA, ref orientationA, ref velocityA, ref inertiaA); + ScalarGather(count, states.Memory, ref references.IndexB, ref positionB, ref orientationB, ref velocityB, ref inertiaB); + + + //for (int i = 0; i < count; ++i) + //{ + // //WriteGatherState(ref baseIndexA, i, ref states, ref positionA, ref orientationA, ref velocityA); + // //WriteGatherInertia(ref baseIndexA, i, ref Inertias, ref inertiaA); + // //WriteGatherState(ref baseIndexB, i, ref states, ref positionB, ref orientationB, ref velocityB); + // //WriteGatherInertia(ref baseIndexB, i, ref Inertias, ref inertiaB); + // WriteGatherState(ref baseIndexA, i, ref states, ref positionA, ref orientationA, ref velocityA, ref inertiaA); + // WriteGatherState(ref baseIndexB, i, ref states, ref positionB, ref orientationB, ref velocityB, ref inertiaB); + //} } //TODO: In future versions, we will likely store the body position in different forms to allow for extremely large worlds. //That will be an opt-in feature. The default implementation will use the FP32 representation, but the user could choose to swap it out for a fp64 or fixed64 representation. diff --git a/BepuPhysics/BodyProperties.cs b/BepuPhysics/BodyProperties.cs index 8861d3c6a..3fd0ab132 100644 --- a/BepuPhysics/BodyProperties.cs +++ b/BepuPhysics/BodyProperties.cs @@ -62,23 +62,42 @@ public readonly BodyInertia Unpack() public struct MotionState { internal const int OffsetToPositionX = 0; - internal const int OffsetToPositionY = 4; - internal const int OffsetToPositionZ = 8; - internal const int OffsetToOrientationX = 12; - internal const int OffsetToOrientationY = 16; - internal const int OffsetToOrientationZ = 20; - internal const int OffsetToOrientationW = 24; - internal const int OffsetToLinearX = 28; - internal const int OffsetToLinearY = 32; - internal const int OffsetToLinearZ = 36; - internal const int OffsetToAngularX = 40; - internal const int OffsetToAngularY = 44; - internal const int OffsetToAngularZ = 48; - internal const int OffsetToInverseMass = 52; + internal const int OffsetToPositionY = 1; + internal const int OffsetToPositionZ = 2; + internal const int OffsetToOrientationX = 3; + internal const int OffsetToOrientationY = 4; + internal const int OffsetToOrientationZ = 5; + internal const int OffsetToOrientationW = 6; + internal const int OffsetToLinearX = 7; + internal const int OffsetToLinearY = 8; + internal const int OffsetToLinearZ = 9; + internal const int OffsetToAngularX = 10; + internal const int OffsetToAngularY = 11; + internal const int OffsetToAngularZ = 12; + internal const int OffsetToInverseMass = 13; internal const int OffsetToInverseInertiaXX = OffsetToInverseMass; internal const int OffsetToInverseInertiaYY = OffsetToInverseMass; internal const int OffsetToInverseInertiaZZ = OffsetToInverseMass; + internal const int ByteOffsetToPositionX = OffsetToPositionX * 4; + internal const int ByteOffsetToPositionY = OffsetToPositionY * 4; + internal const int ByteOffsetToPositionZ = OffsetToPositionZ * 4; + internal const int ByteOffsetToOrientationX = OffsetToOrientationX * 4; + internal const int ByteOffsetToOrientationY = OffsetToOrientationY * 4; + internal const int ByteOffsetToOrientationZ = OffsetToOrientationZ * 4; + internal const int ByteOffsetToOrientationW = OffsetToOrientationW * 4; + internal const int ByteOffsetToLinearX = OffsetToLinearX * 4; + internal const int ByteOffsetToLinearY = OffsetToLinearY * 4; + internal const int ByteOffsetToLinearZ = OffsetToLinearZ * 4; + internal const int ByteOffsetToAngularX = OffsetToAngularX * 4; + internal const int ByteOffsetToAngularY = OffsetToAngularY * 4; + internal const int ByteOffsetToAngularZ = OffsetToAngularZ * 4; + internal const int ByteOffsetToInverseMass = OffsetToInverseMass * 4; + internal const int ByteOffsetToInverseInertiaXX = ByteOffsetToInverseMass; + internal const int ByteOffsetToInverseInertiaYY = ByteOffsetToInverseMass; + internal const int ByteOffsetToInverseInertiaZZ = ByteOffsetToInverseMass; + + /// /// Pose of the body. /// From 3576962a1592413700dfa0741873f59d0628b6c4 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 1 Jul 2021 16:36:13 -0500 Subject: [PATCH 055/947] Beginnings of a transpose gather. --- BepuPhysics/Bodies_GatherScatter.cs | 72 +++++++++++++++++++++++++- Demos/Demo.cs | 2 +- Demos/Demos.csproj | 2 +- Demos/SpecializedTests/HeadlessDemo.cs | 2 +- 4 files changed, 74 insertions(+), 4 deletions(-) diff --git a/BepuPhysics/Bodies_GatherScatter.cs b/BepuPhysics/Bodies_GatherScatter.cs index 24fefc39b..828b40169 100644 --- a/BepuPhysics/Bodies_GatherScatter.cs +++ b/BepuPhysics/Bodies_GatherScatter.cs @@ -152,6 +152,72 @@ unsafe static void ScalarGather(int count, MotionState* motionStates, ref Vector pInertiaZZ[i] = stateValues[MotionState.OffsetToInverseInertiaZZ]; } } + unsafe static void TransposingGather(int count, MotionState* motionStates, ref Vector baseIndex, ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocities velocity, ref BodyInertias inertia) + { + var indices = (int*)Unsafe.AsPointer(ref baseIndex); + + var firstIndex = indices[0]; + var s0 = (float*)(motionStates + indices[0]); + var s1 = (float*)(motionStates + indices[1]); + var s2 = (float*)(motionStates + indices[2]); + var s3 = (float*)(motionStates + indices[3]); + var s4 = (float*)(motionStates + indices[4]); + var s5 = (float*)(motionStates + indices[5]); + var s6 = (float*)(motionStates + indices[6]); + var s7 = (float*)(motionStates + indices[7]); + + //for (int i = 0; i < 8; ++i) + //{ + // s0[i] = i; + // s1[i] = i + 100; + // s2[i] = i + 200; + // s3[i] = i + 300; + // s4[i] = i + 400; + // s5[i] = i + 500; + // s6[i] = i + 600; + // s7[i] = i + 700; + //} + + //Load every body for the first half of the motion state. + //Note that buffers are allocated on cache line boundaries, so we can use aligned loads for all that matters. + var m0 = Avx2.LoadVector256(s0); + var m1 = count > 1 ? Avx2.LoadAlignedVector256(s1) : Vector256.Zero; + var m2 = count > 2 ? Avx2.LoadAlignedVector256(s2) : Vector256.Zero; + var m3 = count > 3 ? Avx2.LoadAlignedVector256(s3) : Vector256.Zero; + var m4 = count > 4 ? Avx2.LoadAlignedVector256(s4) : Vector256.Zero; + var m5 = count > 5 ? Avx2.LoadAlignedVector256(s5) : Vector256.Zero; + var m6 = count > 6 ? Avx2.LoadAlignedVector256(s6) : Vector256.Zero; + var m7 = count > 7 ? Avx2.LoadAlignedVector256(s7) : Vector256.Zero; + + var n0 = Avx2.UnpackLow(m0, m1); + var n1 = Avx2.UnpackLow(m2, m3); + + var o0 = Avx2.Shuffle(n0, n1, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o4 = Avx2.Shuffle(n0, n1, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + + var n2 = Avx2.UnpackLow(m4, m5); + var n3 = Avx2.UnpackLow(m6, m7); + var o1 = Avx2.Shuffle(n2, n3, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + position.X = Avx2.Permute2x128(o0, o1, 0 | (2 << 4)).AsVector(); + orientation.Y = Avx2.Permute2x128(o0, o1, 1 | (3 << 4)).AsVector(); + var o5 = Avx2.Shuffle(n2, n3, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + position.Y = Avx2.Permute2x128(o4, o5, 0 | (2 << 4)).AsVector(); + orientation.Z = Avx2.Permute2x128(o4, o5, 1 | (3 << 4)).AsVector(); + + var n4 = Avx2.UnpackHigh(m0, m1); + var n5 = Avx2.UnpackHigh(m2, m3); + var n6 = Avx2.UnpackHigh(m4, m5); + var n7 = Avx2.UnpackHigh(m6, m7); + var o2 = Avx2.Shuffle(n4, n5, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o3 = Avx2.Shuffle(n6, n7, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + position.Z = Avx2.Permute2x128(o2, o3, 0 | (2 << 4)).AsVector(); + orientation.W = Avx2.Permute2x128(o2, o3, 1 | (3 << 4)).AsVector(); + var o6 = Avx2.Shuffle(n4, n5, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o7 = Avx2.Shuffle(n6, n7, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + orientation.X = Avx2.Permute2x128(o6, o7, 0 | (2 << 4)).AsVector(); + velocity.Linear.X = Avx2.Permute2x128(o6, o7, 1 | (3 << 4)).AsVector(); + + } /// /// Gathers orientations and relative positions for a two body bundle into an AOSOA bundle. @@ -187,7 +253,7 @@ public unsafe void GatherState(ref TwoBodyReferences references, int count, ref var states = ref ActiveSet.MotionStates; - if (Avx2.IsSupported) + if (false)//Avx2.IsSupported) { var maskMemory = stackalloc int[8]; for (int i = 0; i < count; ++i) @@ -265,6 +331,10 @@ public unsafe void GatherState(ref TwoBodyReferences references, int count, inertiaA.InverseInertiaTensor.ZY = default; inertiaB.InverseInertiaTensor.ZY = default; } + else if (Avx2.IsSupported) + { + TransposingGather(count, states.Memory, ref references.IndexA, ref positionA, ref orientationA, ref velocityA, ref inertiaA); + } else { ScalarGather(count, states.Memory, ref references.IndexA, ref positionA, ref orientationA, ref velocityA, ref inertiaA); diff --git a/Demos/Demo.cs b/Demos/Demo.cs index f00656836..aba8044d8 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -62,7 +62,7 @@ public virtual void Update(Window window, Camera camera, Input input, float dt) //fully decouple simulation and rendering rates across different threads. //(In either case, you'd also want to interpolate or extrapolate simulation results during rendering for smoothness.) //Note that taking steps of variable length can reduce stability. Gradual or one-off changes can work reasonably well. - Simulation.Timestep(1 / 60f, ThreadDispatcher); + Simulation.Timestep(1 / 60f); ////Here's an example of how it would look to use more frequent updates, but still with a fixed amount of time simulated per update call: //const float timeToSimulate = 1 / 60f; diff --git a/Demos/Demos.csproj b/Demos/Demos.csproj index 70b980cf3..4c7e0b045 100644 --- a/Demos/Demos.csproj +++ b/Demos/Demos.csproj @@ -5,7 +5,7 @@ True Debug;Release latest - + false diff --git a/Demos/SpecializedTests/HeadlessDemo.cs b/Demos/SpecializedTests/HeadlessDemo.cs index 08c56cfb4..ba3445f73 100644 --- a/Demos/SpecializedTests/HeadlessDemo.cs +++ b/Demos/SpecializedTests/HeadlessDemo.cs @@ -29,7 +29,7 @@ static class HeadlessTest Console.Write("Completed frames: "); for (int i = 0; i < frameCount; ++i) { - CacheBlaster.Blast(); + //CacheBlaster.Blast(); var start = Stopwatch.GetTimestamp(); demo.Update(null, null, null, 1 / 60f); var end = Stopwatch.GetTimestamp(); From 53633fa1f4f0f588dd57a2ac8b4488e0e6efb07e Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 1 Jul 2021 16:57:42 -0500 Subject: [PATCH 056/947] Filled out remaining transpose gather. --- BepuPhysics/Bodies_GatherScatter.cs | 120 +++++++++++++++++++--------- Demos/Program.cs | 2 +- 2 files changed, 83 insertions(+), 39 deletions(-) diff --git a/BepuPhysics/Bodies_GatherScatter.cs b/BepuPhysics/Bodies_GatherScatter.cs index 828b40169..3d5e7ea7d 100644 --- a/BepuPhysics/Bodies_GatherScatter.cs +++ b/BepuPhysics/Bodies_GatherScatter.cs @@ -178,45 +178,88 @@ unsafe static void TransposingGather(int count, MotionState* motionStates, ref V // s7[i] = i + 700; //} - //Load every body for the first half of the motion state. - //Note that buffers are allocated on cache line boundaries, so we can use aligned loads for all that matters. - var m0 = Avx2.LoadVector256(s0); - var m1 = count > 1 ? Avx2.LoadAlignedVector256(s1) : Vector256.Zero; - var m2 = count > 2 ? Avx2.LoadAlignedVector256(s2) : Vector256.Zero; - var m3 = count > 3 ? Avx2.LoadAlignedVector256(s3) : Vector256.Zero; - var m4 = count > 4 ? Avx2.LoadAlignedVector256(s4) : Vector256.Zero; - var m5 = count > 5 ? Avx2.LoadAlignedVector256(s5) : Vector256.Zero; - var m6 = count > 6 ? Avx2.LoadAlignedVector256(s6) : Vector256.Zero; - var m7 = count > 7 ? Avx2.LoadAlignedVector256(s7) : Vector256.Zero; - - var n0 = Avx2.UnpackLow(m0, m1); - var n1 = Avx2.UnpackLow(m2, m3); - - var o0 = Avx2.Shuffle(n0, n1, 0 | (1 << 2) | (0 << 4) | (1 << 6)); - var o4 = Avx2.Shuffle(n0, n1, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - - var n2 = Avx2.UnpackLow(m4, m5); - var n3 = Avx2.UnpackLow(m6, m7); - var o1 = Avx2.Shuffle(n2, n3, 0 | (1 << 2) | (0 << 4) | (1 << 6)); - position.X = Avx2.Permute2x128(o0, o1, 0 | (2 << 4)).AsVector(); - orientation.Y = Avx2.Permute2x128(o0, o1, 1 | (3 << 4)).AsVector(); - var o5 = Avx2.Shuffle(n2, n3, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - position.Y = Avx2.Permute2x128(o4, o5, 0 | (2 << 4)).AsVector(); - orientation.Z = Avx2.Permute2x128(o4, o5, 1 | (3 << 4)).AsVector(); - - var n4 = Avx2.UnpackHigh(m0, m1); - var n5 = Avx2.UnpackHigh(m2, m3); - var n6 = Avx2.UnpackHigh(m4, m5); - var n7 = Avx2.UnpackHigh(m6, m7); - var o2 = Avx2.Shuffle(n4, n5, 0 | (1 << 2) | (0 << 4) | (1 << 6)); - var o3 = Avx2.Shuffle(n6, n7, 0 | (1 << 2) | (0 << 4) | (1 << 6)); - position.Z = Avx2.Permute2x128(o2, o3, 0 | (2 << 4)).AsVector(); - orientation.W = Avx2.Permute2x128(o2, o3, 1 | (3 << 4)).AsVector(); - var o6 = Avx2.Shuffle(n4, n5, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - var o7 = Avx2.Shuffle(n6, n7, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - orientation.X = Avx2.Permute2x128(o6, o7, 0 | (2 << 4)).AsVector(); - velocity.Linear.X = Avx2.Permute2x128(o6, o7, 1 | (3 << 4)).AsVector(); + { + //Load every body for the first half of the motion state. + //Note that buffers are allocated on cache line boundaries, so we can use aligned loads for all that matters. + var m0 = Avx2.LoadVector256(s0); + var m1 = count > 1 ? Avx2.LoadAlignedVector256(s1) : Vector256.Zero; + var m2 = count > 2 ? Avx2.LoadAlignedVector256(s2) : Vector256.Zero; + var m3 = count > 3 ? Avx2.LoadAlignedVector256(s3) : Vector256.Zero; + var m4 = count > 4 ? Avx2.LoadAlignedVector256(s4) : Vector256.Zero; + var m5 = count > 5 ? Avx2.LoadAlignedVector256(s5) : Vector256.Zero; + var m6 = count > 6 ? Avx2.LoadAlignedVector256(s6) : Vector256.Zero; + var m7 = count > 7 ? Avx2.LoadAlignedVector256(s7) : Vector256.Zero; + + var n0 = Avx2.UnpackLow(m0, m1); + var n1 = Avx2.UnpackLow(m2, m3); + var n2 = Avx2.UnpackLow(m4, m5); + var n3 = Avx2.UnpackLow(m6, m7); + var n4 = Avx2.UnpackHigh(m0, m1); + var n5 = Avx2.UnpackHigh(m2, m3); + var n6 = Avx2.UnpackHigh(m4, m5); + var n7 = Avx2.UnpackHigh(m6, m7); + + var o0 = Avx2.Shuffle(n0, n1, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o1 = Avx2.Shuffle(n2, n3, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o2 = Avx2.Shuffle(n4, n5, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o3 = Avx2.Shuffle(n6, n7, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o4 = Avx2.Shuffle(n0, n1, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o5 = Avx2.Shuffle(n2, n3, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o6 = Avx2.Shuffle(n4, n5, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o7 = Avx2.Shuffle(n6, n7, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + + position.X = Avx2.Permute2x128(o0, o1, 0 | (2 << 4)).AsVector(); + position.Y = Avx2.Permute2x128(o4, o5, 0 | (2 << 4)).AsVector(); + position.Z = Avx2.Permute2x128(o2, o3, 0 | (2 << 4)).AsVector(); + orientation.X = Avx2.Permute2x128(o6, o7, 0 | (2 << 4)).AsVector(); + orientation.Y = Avx2.Permute2x128(o0, o1, 1 | (3 << 4)).AsVector(); + orientation.Z = Avx2.Permute2x128(o4, o5, 1 | (3 << 4)).AsVector(); + orientation.W = Avx2.Permute2x128(o2, o3, 1 | (3 << 4)).AsVector(); + velocity.Linear.X = Avx2.Permute2x128(o6, o7, 1 | (3 << 4)).AsVector(); + } + { + //Second half. + var m0 = Avx2.LoadVector256(s0 + 8); + var m1 = count > 1 ? Avx2.LoadAlignedVector256(s1 + 8) : Vector256.Zero; + var m2 = count > 2 ? Avx2.LoadAlignedVector256(s2 + 8) : Vector256.Zero; + var m3 = count > 3 ? Avx2.LoadAlignedVector256(s3 + 8) : Vector256.Zero; + var m4 = count > 4 ? Avx2.LoadAlignedVector256(s4 + 8) : Vector256.Zero; + var m5 = count > 5 ? Avx2.LoadAlignedVector256(s5 + 8) : Vector256.Zero; + var m6 = count > 6 ? Avx2.LoadAlignedVector256(s6 + 8) : Vector256.Zero; + var m7 = count > 7 ? Avx2.LoadAlignedVector256(s7 + 8) : Vector256.Zero; + + var n0 = Avx2.UnpackLow(m0, m1); + var n1 = Avx2.UnpackLow(m2, m3); + var n2 = Avx2.UnpackLow(m4, m5); + var n3 = Avx2.UnpackLow(m6, m7); + var n4 = Avx2.UnpackHigh(m0, m1); + var n5 = Avx2.UnpackHigh(m2, m3); + var n6 = Avx2.UnpackHigh(m4, m5); + var n7 = Avx2.UnpackHigh(m6, m7); + + var o0 = Avx2.Shuffle(n0, n1, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o1 = Avx2.Shuffle(n2, n3, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o2 = Avx2.Shuffle(n4, n5, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o3 = Avx2.Shuffle(n6, n7, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o4 = Avx2.Shuffle(n0, n1, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o5 = Avx2.Shuffle(n2, n3, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o6 = Avx2.Shuffle(n4, n5, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o7 = Avx2.Shuffle(n6, n7, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + + velocity.Linear.Y = Avx2.Permute2x128(o0, o1, 0 | (2 << 4)).AsVector(); + velocity.Linear.Z = Avx2.Permute2x128(o4, o5, 0 | (2 << 4)).AsVector(); + velocity.Angular.X = Avx2.Permute2x128(o2, o3, 0 | (2 << 4)).AsVector(); + velocity.Angular.Y = Avx2.Permute2x128(o6, o7, 0 | (2 << 4)).AsVector(); + velocity.Angular.Z = Avx2.Permute2x128(o0, o1, 1 | (3 << 4)).AsVector(); + inertia.InverseMass = Avx2.Permute2x128(o4, o5, 1 | (3 << 4)).AsVector(); + inertia.InverseInertiaTensor.XX = inertia.InverseMass; + inertia.InverseInertiaTensor.YY = inertia.InverseMass; + inertia.InverseInertiaTensor.ZZ = inertia.InverseMass; + inertia.InverseInertiaTensor.YX = default; + inertia.InverseInertiaTensor.ZX = default; + inertia.InverseInertiaTensor.ZY = default; + } } /// @@ -334,6 +377,7 @@ public unsafe void GatherState(ref TwoBodyReferences references, int count, else if (Avx2.IsSupported) { TransposingGather(count, states.Memory, ref references.IndexA, ref positionA, ref orientationA, ref velocityA, ref inertiaA); + TransposingGather(count, states.Memory, ref references.IndexB, ref positionB, ref orientationB, ref velocityB, ref inertiaB); } else { diff --git a/Demos/Program.cs b/Demos/Program.cs index 6c769ece5..7118826dd 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -19,7 +19,7 @@ static void Main(string[] args) { content = ContentArchive.Load(stream); } - HeadlessTest.Test(content, 4, 32, 512); + //HeadlessTest.Test(content, 4, 32, 512); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); From 0e193e2705a06119ffaa8b078f318a32080190ec Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 1 Jul 2021 19:13:23 -0500 Subject: [PATCH 057/947] Swippy swapped motion state layout so all 3 vectors fall on 16 byte boundaries and all velocities are in one AVX2 lane. --- BepuPhysics/Bodies_GatherScatter.cs | 126 ++++++---------------------- BepuPhysics/BodyProperties.cs | 79 +++++++++-------- BepuPhysics/Constraints/Weld.cs | 6 +- BepuUtilities/Symmetric6x6Wide.cs | 2 +- Demos/Demo.cs | 2 +- Demos/Program.cs | 2 +- 6 files changed, 70 insertions(+), 147 deletions(-) diff --git a/BepuPhysics/Bodies_GatherScatter.cs b/BepuPhysics/Bodies_GatherScatter.cs index 3d5e7ea7d..6c81965f6 100644 --- a/BepuPhysics/Bodies_GatherScatter.cs +++ b/BepuPhysics/Bodies_GatherScatter.cs @@ -152,6 +152,7 @@ unsafe static void ScalarGather(int count, MotionState* motionStates, ref Vector pInertiaZZ[i] = stateValues[MotionState.OffsetToInverseInertiaZZ]; } } + //[MethodImpl(MethodImplOptions.AggressiveInlining)] unsafe static void TransposingGather(int count, MotionState* motionStates, ref Vector baseIndex, ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocities velocity, ref BodyInertias inertia) { var indices = (int*)Unsafe.AsPointer(ref baseIndex); @@ -208,14 +209,20 @@ unsafe static void TransposingGather(int count, MotionState* motionStates, ref V var o6 = Avx2.Shuffle(n4, n5, 2 | (3 << 2) | (2 << 4) | (3 << 6)); var o7 = Avx2.Shuffle(n6, n7, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - position.X = Avx2.Permute2x128(o0, o1, 0 | (2 << 4)).AsVector(); - position.Y = Avx2.Permute2x128(o4, o5, 0 | (2 << 4)).AsVector(); - position.Z = Avx2.Permute2x128(o2, o3, 0 | (2 << 4)).AsVector(); - orientation.X = Avx2.Permute2x128(o6, o7, 0 | (2 << 4)).AsVector(); - orientation.Y = Avx2.Permute2x128(o0, o1, 1 | (3 << 4)).AsVector(); - orientation.Z = Avx2.Permute2x128(o4, o5, 1 | (3 << 4)).AsVector(); - orientation.W = Avx2.Permute2x128(o2, o3, 1 | (3 << 4)).AsVector(); - velocity.Linear.X = Avx2.Permute2x128(o6, o7, 1 | (3 << 4)).AsVector(); + orientation.X = Avx2.Permute2x128(o0, o1, 0 | (2 << 4)).AsVector(); + orientation.Y = Avx2.Permute2x128(o4, o5, 0 | (2 << 4)).AsVector(); + orientation.Z = Avx2.Permute2x128(o2, o3, 0 | (2 << 4)).AsVector(); + orientation.W = Avx2.Permute2x128(o6, o7, 0 | (2 << 4)).AsVector(); + position.X= Avx2.Permute2x128(o0, o1, 1 | (3 << 4)).AsVector(); + position.Y = Avx2.Permute2x128(o4, o5, 1 | (3 << 4)).AsVector(); + position.Z = Avx2.Permute2x128(o2, o3, 1 | (3 << 4)).AsVector(); + inertia.InverseMass = Avx2.Permute2x128(o6, o7, 1 | (3 << 4)).AsVector(); + inertia.InverseInertiaTensor.XX = inertia.InverseMass; + inertia.InverseInertiaTensor.YY = inertia.InverseMass; + inertia.InverseInertiaTensor.ZZ = inertia.InverseMass; + inertia.InverseInertiaTensor.YX = default; + inertia.InverseInertiaTensor.ZX = default; + inertia.InverseInertiaTensor.ZY = default; } { @@ -247,18 +254,13 @@ unsafe static void TransposingGather(int count, MotionState* motionStates, ref V var o6 = Avx2.Shuffle(n4, n5, 2 | (3 << 2) | (2 << 4) | (3 << 6)); var o7 = Avx2.Shuffle(n6, n7, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - velocity.Linear.Y = Avx2.Permute2x128(o0, o1, 0 | (2 << 4)).AsVector(); - velocity.Linear.Z = Avx2.Permute2x128(o4, o5, 0 | (2 << 4)).AsVector(); - velocity.Angular.X = Avx2.Permute2x128(o2, o3, 0 | (2 << 4)).AsVector(); - velocity.Angular.Y = Avx2.Permute2x128(o6, o7, 0 | (2 << 4)).AsVector(); - velocity.Angular.Z = Avx2.Permute2x128(o0, o1, 1 | (3 << 4)).AsVector(); - inertia.InverseMass = Avx2.Permute2x128(o4, o5, 1 | (3 << 4)).AsVector(); - inertia.InverseInertiaTensor.XX = inertia.InverseMass; - inertia.InverseInertiaTensor.YY = inertia.InverseMass; - inertia.InverseInertiaTensor.ZZ = inertia.InverseMass; - inertia.InverseInertiaTensor.YX = default; - inertia.InverseInertiaTensor.ZX = default; - inertia.InverseInertiaTensor.ZY = default; + velocity.Linear.X = Avx2.Permute2x128(o0, o1, 0 | (2 << 4)).AsVector(); + velocity.Linear.Y = Avx2.Permute2x128(o4, o5, 0 | (2 << 4)).AsVector(); + velocity.Linear.Z = Avx2.Permute2x128(o2, o3, 0 | (2 << 4)).AsVector(); + velocity.Angular.X = Avx2.Permute2x128(o6, o7, 0 | (2 << 4)).AsVector(); + velocity.Angular.Y = Avx2.Permute2x128(o0, o1, 1 | (3 << 4)).AsVector(); + velocity.Angular.Z = Avx2.Permute2x128(o4, o5, 1 | (3 << 4)).AsVector(); + } } @@ -296,85 +298,7 @@ public unsafe void GatherState(ref TwoBodyReferences references, int count, ref var states = ref ActiveSet.MotionStates; - if (false)//Avx2.IsSupported) - { - var maskMemory = stackalloc int[8]; - for (int i = 0; i < count; ++i) - { - maskMemory[i] = -1; - } - for (int i = count; i < 8; ++i) - { - maskMemory[i] = 0; - } - var mask = Avx.LoadVector256(maskMemory); - var statesMemory = (float*)states.Memory; - var indexA = Avx2.ShiftLeftLogical(references.IndexA.AsVector256(), 6); - var indexB = Avx2.ShiftLeftLogical(references.IndexB.AsVector256(), 6); - var zero = Vector256.Zero; - var offsetX = Vector256.Create(MotionState.ByteOffsetToPositionX); - positionA.X = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetX), mask.AsSingle(), 1).AsVector(); - positionB.X = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetX), mask.AsSingle(), 1).AsVector(); - var offsetY = Vector256.Create(MotionState.ByteOffsetToPositionY); - positionA.Y = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetY), mask.AsSingle(), 1).AsVector(); - positionB.Y = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetY), mask.AsSingle(), 1).AsVector(); - var offsetZ = Vector256.Create(MotionState.ByteOffsetToPositionZ); - positionA.Z = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetZ), mask.AsSingle(), 1).AsVector(); - positionB.Z = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetZ), mask.AsSingle(), 1).AsVector(); - - var offsetOrientationX = Vector256.Create(MotionState.ByteOffsetToOrientationX); - orientationA.X = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetOrientationX), mask.AsSingle(), 1).AsVector(); - orientationB.X = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetOrientationX), mask.AsSingle(), 1).AsVector(); - var offsetOrientationY = Vector256.Create(MotionState.ByteOffsetToOrientationY); - orientationA.Y = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetOrientationY), mask.AsSingle(), 1).AsVector(); - orientationB.Y = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetOrientationY), mask.AsSingle(), 1).AsVector(); - var offsetOrientationZ = Vector256.Create(MotionState.ByteOffsetToOrientationZ); - orientationA.Z = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetOrientationZ), mask.AsSingle(), 1).AsVector(); - orientationB.Z = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetOrientationZ), mask.AsSingle(), 1).AsVector(); - var offsetOrientationW = Vector256.Create(MotionState.ByteOffsetToOrientationW); - orientationA.W = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetOrientationW), mask.AsSingle(), 1).AsVector(); - orientationB.W = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetOrientationW), mask.AsSingle(), 1).AsVector(); - - var offsetLinearX = Vector256.Create(MotionState.ByteOffsetToLinearX); - velocityA.Linear.X = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetLinearX), mask.AsSingle(), 1).AsVector(); - velocityB.Linear.X = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetLinearX), mask.AsSingle(), 1).AsVector(); - var offsetLinearY = Vector256.Create(MotionState.ByteOffsetToLinearY); - velocityA.Linear.Y = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetLinearY), mask.AsSingle(), 1).AsVector(); - velocityB.Linear.Y = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetLinearY), mask.AsSingle(), 1).AsVector(); - var offsetLinearZ = Vector256.Create(MotionState.ByteOffsetToLinearZ); - velocityA.Linear.Z = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetLinearZ), mask.AsSingle(), 1).AsVector(); - velocityB.Linear.Z = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetLinearZ), mask.AsSingle(), 1).AsVector(); - - var offsetAngularX = Vector256.Create(MotionState.ByteOffsetToAngularX); - velocityA.Angular.X = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetAngularX), mask.AsSingle(), 1).AsVector(); - velocityB.Angular.X = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetAngularX), mask.AsSingle(), 1).AsVector(); - var offsetAngularY = Vector256.Create(MotionState.ByteOffsetToAngularY); - velocityA.Angular.Y = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetAngularY), mask.AsSingle(), 1).AsVector(); - velocityB.Angular.Y = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetAngularY), mask.AsSingle(), 1).AsVector(); - var offsetAngularZ = Vector256.Create(MotionState.ByteOffsetToAngularZ); - velocityA.Angular.Z = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetAngularZ), mask.AsSingle(), 1).AsVector(); - velocityB.Angular.Z = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetAngularZ), mask.AsSingle(), 1).AsVector(); - - var offsetInverseMass = Vector256.Create(MotionState.ByteOffsetToInverseMass); - inertiaA.InverseMass = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetInverseMass), mask.AsSingle(), 1).AsVector(); - inertiaB.InverseMass = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetInverseMass), mask.AsSingle(), 1).AsVector(); - var offsetInverseInertiaXX = Vector256.Create(MotionState.ByteOffsetToInverseInertiaXX); - inertiaA.InverseInertiaTensor.XX = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetInverseInertiaXX), mask.AsSingle(), 1).AsVector(); - inertiaB.InverseInertiaTensor.XX = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetInverseInertiaXX), mask.AsSingle(), 1).AsVector(); - var offsetInverseInertiaYY = Vector256.Create(MotionState.ByteOffsetToInverseInertiaYY); - inertiaA.InverseInertiaTensor.YY = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetInverseInertiaYY), mask.AsSingle(), 1).AsVector(); - inertiaB.InverseInertiaTensor.YY = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetInverseInertiaYY), mask.AsSingle(), 1).AsVector(); - var offsetInverseInertiaZZ = Vector256.Create(MotionState.ByteOffsetToInverseInertiaZZ); - inertiaA.InverseInertiaTensor.ZZ = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexA, offsetInverseInertiaZZ), mask.AsSingle(), 1).AsVector(); - inertiaB.InverseInertiaTensor.ZZ = Avx2.GatherMaskVector256(zero, statesMemory, Avx2.Add(indexB, offsetInverseInertiaZZ), mask.AsSingle(), 1).AsVector(); - inertiaA.InverseInertiaTensor.YX = default; - inertiaB.InverseInertiaTensor.YX = default; - inertiaB.InverseInertiaTensor.ZX = default; - inertiaA.InverseInertiaTensor.ZX = default; - inertiaA.InverseInertiaTensor.ZY = default; - inertiaB.InverseInertiaTensor.ZY = default; - } - else if (Avx2.IsSupported) + if (Avx2.IsSupported) { TransposingGather(count, states.Memory, ref references.IndexA, ref positionA, ref orientationA, ref velocityA, ref inertiaA); TransposingGather(count, states.Memory, ref references.IndexB, ref positionB, ref orientationB, ref velocityB, ref inertiaB); @@ -384,7 +308,6 @@ public unsafe void GatherState(ref TwoBodyReferences references, int count, ScalarGather(count, states.Memory, ref references.IndexA, ref positionA, ref orientationA, ref velocityA, ref inertiaA); ScalarGather(count, states.Memory, ref references.IndexB, ref positionB, ref orientationB, ref velocityB, ref inertiaB); - //for (int i = 0; i < count; ++i) //{ // //WriteGatherState(ref baseIndexA, i, ref states, ref positionA, ref orientationA, ref velocityA); @@ -707,7 +630,8 @@ public unsafe void ScatterVelocities(ref BodyVelocities sourceVelocities, ref Ve /// Velocities of body bundle B to scatter. /// Active set indices of the bodies to scatter velocity data to. /// Number of body pairs in the bundle. - [MethodImpl(MethodImplOptions.AggressiveInlining)] + //[MethodImpl(MethodImplOptions.AggressiveInlining)] + [MethodImpl(MethodImplOptions.NoInlining)] public unsafe void ScatterVelocities(ref BodyVelocities sourceVelocitiesA, ref BodyVelocities sourceVelocitiesB, ref TwoBodyReferences references, int count) { diff --git a/BepuPhysics/BodyProperties.cs b/BepuPhysics/BodyProperties.cs index 3fd0ab132..a9c71f4a4 100644 --- a/BepuPhysics/BodyProperties.cs +++ b/BepuPhysics/BodyProperties.cs @@ -12,27 +12,27 @@ namespace BepuPhysics /// /// Stores the inertia of a body in half precision. /// - [StructLayout(LayoutKind.Sequential, Size = 10, Pack = 1)] + [StructLayout(LayoutKind.Sequential, Size = 4, Pack = 1)] public struct PackedInertia { //TODO: Temporarily ignoring off-diagonal inertia for testing. public float InverseMass; - public Half InverseNormalizedInertiaXX; - //public Half InverseNormalizedInertiaYX; - public Half InverseNormalizedInertiaYY; - //public Half InverseNormalizedInertiaZX; - //public Half InverseNormalizedInertiaZY; - public Half InverseNormalizedInertiaZZ; + //public Half InverseNormalizedInertiaXX; + ////public Half InverseNormalizedInertiaYX; + //public Half InverseNormalizedInertiaYY; + ////public Half InverseNormalizedInertiaZX; + ////public Half InverseNormalizedInertiaZY; + //public Half InverseNormalizedInertiaZZ; public PackedInertia(in BodyInertia inertia) { InverseMass = inertia.InverseMass; - InverseNormalizedInertiaXX = (Half)(inertia.InverseInertiaTensor.XX / InverseMass); - //InverseNormalizedInertiaYX = (Half)(inertia.InverseInertiaTensor.YX/ InverseMass); - InverseNormalizedInertiaYY = (Half)(inertia.InverseInertiaTensor.YY / InverseMass); - //InverseNormalizedInertiaZX = (Half)(inertia.InverseInertiaTensor.ZX/ InverseMass); - //InverseNormalizedInertiaZY = (Half)(inertia.InverseInertiaTensor.ZY/ InverseMass); - InverseNormalizedInertiaZZ = (Half)(inertia.InverseInertiaTensor.ZZ / InverseMass); + //InverseNormalizedInertiaXX = (Half)(inertia.InverseInertiaTensor.XX / InverseMass); + ////InverseNormalizedInertiaYX = (Half)(inertia.InverseInertiaTensor.YX/ InverseMass); + //InverseNormalizedInertiaYY = (Half)(inertia.InverseInertiaTensor.YY / InverseMass); + ////InverseNormalizedInertiaZX = (Half)(inertia.InverseInertiaTensor.ZX/ InverseMass); + ////InverseNormalizedInertiaZY = (Half)(inertia.InverseInertiaTensor.ZY/ InverseMass); + //InverseNormalizedInertiaZZ = (Half)(inertia.InverseInertiaTensor.ZZ / InverseMass); } public readonly void Unpack(out BodyInertia inertia) @@ -40,12 +40,12 @@ public readonly void Unpack(out BodyInertia inertia) //TODO: not necessary in complete implementation inertia = default; inertia.InverseMass = InverseMass; - inertia.InverseInertiaTensor.XX = (float)InverseNormalizedInertiaXX * InverseMass; - //inertia.InverseInertiaTensor.YX = (float)InverseNormalizedInertiaYX * InverseMass; - inertia.InverseInertiaTensor.YY = (float)InverseNormalizedInertiaYY * InverseMass; - //inertia.InverseInertiaTensor.ZX = (float)InverseNormalizedInertiaZX * InverseMass; - //inertia.InverseInertiaTensor.ZY = (float)InverseNormalizedInertiaZY * InverseMass; - inertia.InverseInertiaTensor.ZZ = (float)InverseNormalizedInertiaZZ * InverseMass; + //inertia.InverseInertiaTensor.XX = (float)InverseNormalizedInertiaXX * InverseMass; + ////inertia.InverseInertiaTensor.YX = (float)InverseNormalizedInertiaYX * InverseMass; + //inertia.InverseInertiaTensor.YY = (float)InverseNormalizedInertiaYY * InverseMass; + ////inertia.InverseInertiaTensor.ZX = (float)InverseNormalizedInertiaZX * InverseMass; + ////inertia.InverseInertiaTensor.ZY = (float)InverseNormalizedInertiaZY * InverseMass; + //inertia.InverseInertiaTensor.ZZ = (float)InverseNormalizedInertiaZZ * InverseMass; } public readonly BodyInertia Unpack() { @@ -61,20 +61,20 @@ public readonly BodyInertia Unpack() [StructLayout(LayoutKind.Sequential, Size = 64, Pack = 1)] public struct MotionState { - internal const int OffsetToPositionX = 0; - internal const int OffsetToPositionY = 1; - internal const int OffsetToPositionZ = 2; - internal const int OffsetToOrientationX = 3; - internal const int OffsetToOrientationY = 4; - internal const int OffsetToOrientationZ = 5; - internal const int OffsetToOrientationW = 6; - internal const int OffsetToLinearX = 7; - internal const int OffsetToLinearY = 8; - internal const int OffsetToLinearZ = 9; - internal const int OffsetToAngularX = 10; - internal const int OffsetToAngularY = 11; - internal const int OffsetToAngularZ = 12; - internal const int OffsetToInverseMass = 13; + internal const int OffsetToOrientationX = 0; + internal const int OffsetToOrientationY = 1; + internal const int OffsetToOrientationZ = 2; + internal const int OffsetToOrientationW = 3; + internal const int OffsetToPositionX = 4; + internal const int OffsetToPositionY = 5; + internal const int OffsetToPositionZ = 6; + internal const int OffsetToInverseMass = 7; + internal const int OffsetToLinearX = 8; + internal const int OffsetToLinearY = 9; + internal const int OffsetToLinearZ = 10; + internal const int OffsetToAngularX = 12; + internal const int OffsetToAngularY = 13; + internal const int OffsetToAngularZ = 14; internal const int OffsetToInverseInertiaXX = OffsetToInverseMass; internal const int OffsetToInverseInertiaYY = OffsetToInverseMass; internal const int OffsetToInverseInertiaZZ = OffsetToInverseMass; @@ -102,15 +102,14 @@ public struct MotionState /// Pose of the body. /// public RigidPose Pose; - /// - /// Linear and angular velocity of the body. - /// - public BodyVelocity Velocity; - /// /// Packed inertia of the body. /// public PackedInertia PackedLocalInertia; + /// + /// Linear and angular velocity of the body. + /// + public BodyVelocity Velocity; } //TODO: It's a little odd that this exists alongside the BepuUtilities.RigidTransform. The original reasoning was that rigid poses may end up having a non-FP32 representation. @@ -122,12 +121,12 @@ public struct MotionState [StructLayout(LayoutKind.Sequential, Size = 28, Pack = 1)] public struct RigidPose { - public Vector3 Position; //Note that we store a quaternion rather than a matrix3x3. While this often requires some overhead when performing vector transforms or extracting basis vectors, //systems needing to interact directly with this representation are often terrifically memory bound. Spending the extra ALU time to convert to a basis can actually be faster //than loading the extra 5 elements needed to express the full 3x3 rotation matrix. Also, it's marginally easier to keep the rotation normalized over time. //There may be an argument for the matrix variant to ALSO be stored for some bandwidth-unconstrained stages, but don't worry about that until there's a reason to worry about it. public Quaternion Orientation; + public Vector3 Position; public static RigidPose Identity { get; } = new RigidPose(new Vector3()); @@ -195,7 +194,7 @@ public static void MultiplyWithoutOverlap(in RigidPose a, in RigidPose b, out Ri } } - [StructLayout(LayoutKind.Sequential, Size = 24, Pack = 1)] + [StructLayout(LayoutKind.Sequential, Size = 32, Pack = 16)] public struct BodyVelocity { public Vector3 Linear; diff --git a/BepuPhysics/Constraints/Weld.cs b/BepuPhysics/Constraints/Weld.cs index 1092cab2a..d851adbab 100644 --- a/BepuPhysics/Constraints/Weld.cs +++ b/BepuPhysics/Constraints/Weld.cs @@ -87,7 +87,7 @@ public struct WeldAccumulatedImpulses public struct WeldFunctions : IConstraintFunctions { - [MethodImpl(MethodImplOptions.AggressiveInlining)] + //[MethodImpl(MethodImplOptions.AggressiveInlining)] public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, float dt, float inverseDt, ref WeldPrestepData prestep, out WeldProjection projection) { @@ -167,13 +167,13 @@ private static void ApplyImpulse(ref BodyVelocities velocityA, ref BodyVelocitie Vector3Wide.Subtract(velocityB.Angular, negatedAngularChangeB, out velocityB.Angular); //note subtraction; the jacobian is -I } - [MethodImpl(MethodImplOptions.AggressiveInlining)] + //[MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref WeldProjection projection, ref WeldAccumulatedImpulses accumulatedImpulse) { ApplyImpulse(ref velocityA, ref velocityB, ref projection, ref accumulatedImpulse.Orientation, ref accumulatedImpulse.Offset); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] + //[MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref WeldProjection projection, ref WeldAccumulatedImpulses accumulatedImpulse) { //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); diff --git a/BepuUtilities/Symmetric6x6Wide.cs b/BepuUtilities/Symmetric6x6Wide.cs index d2c11f954..49ba1cef8 100644 --- a/BepuUtilities/Symmetric6x6Wide.cs +++ b/BepuUtilities/Symmetric6x6Wide.cs @@ -26,7 +26,7 @@ public static void Scale(in Symmetric6x6Wide m, in Vector scale, out Symm Symmetric3x3Wide.Scale(m.D, scale, out result.D); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] + //[MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Invert(in Symmetric3x3Wide a, in Matrix3x3Wide b, in Symmetric3x3Wide d, out Symmetric6x6Wide result) { // [ A B ]^-1 = [ (A - B * D^-1 * BT)^-1, -(A - B * D^-1 * BT)^-1 * B * D^-1 ] diff --git a/Demos/Demo.cs b/Demos/Demo.cs index aba8044d8..f00656836 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -62,7 +62,7 @@ public virtual void Update(Window window, Camera camera, Input input, float dt) //fully decouple simulation and rendering rates across different threads. //(In either case, you'd also want to interpolate or extrapolate simulation results during rendering for smoothness.) //Note that taking steps of variable length can reduce stability. Gradual or one-off changes can work reasonably well. - Simulation.Timestep(1 / 60f); + Simulation.Timestep(1 / 60f, ThreadDispatcher); ////Here's an example of how it would look to use more frequent updates, but still with a fixed amount of time simulated per update call: //const float timeToSimulate = 1 / 60f; diff --git a/Demos/Program.cs b/Demos/Program.cs index 7118826dd..6c769ece5 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -19,7 +19,7 @@ static void Main(string[] args) { content = ContentArchive.Load(stream); } - //HeadlessTest.Test(content, 4, 32, 512); + HeadlessTest.Test(content, 4, 32, 512); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); From 6e4443a41bc59a15e838bc52cee95c6d07de4bdd Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 1 Jul 2021 19:40:58 -0500 Subject: [PATCH 058/947] Transpose scatter. --- BepuPhysics/Bodies_GatherScatter.cs | 310 ++++++++++++++++------------ 1 file changed, 180 insertions(+), 130 deletions(-) diff --git a/BepuPhysics/Bodies_GatherScatter.cs b/BepuPhysics/Bodies_GatherScatter.cs index 6c81965f6..5c654d4ba 100644 --- a/BepuPhysics/Bodies_GatherScatter.cs +++ b/BepuPhysics/Bodies_GatherScatter.cs @@ -155,112 +155,117 @@ unsafe static void ScalarGather(int count, MotionState* motionStates, ref Vector //[MethodImpl(MethodImplOptions.AggressiveInlining)] unsafe static void TransposingGather(int count, MotionState* motionStates, ref Vector baseIndex, ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocities velocity, ref BodyInertias inertia) { - var indices = (int*)Unsafe.AsPointer(ref baseIndex); - - var firstIndex = indices[0]; - var s0 = (float*)(motionStates + indices[0]); - var s1 = (float*)(motionStates + indices[1]); - var s2 = (float*)(motionStates + indices[2]); - var s3 = (float*)(motionStates + indices[3]); - var s4 = (float*)(motionStates + indices[4]); - var s5 = (float*)(motionStates + indices[5]); - var s6 = (float*)(motionStates + indices[6]); - var s7 = (float*)(motionStates + indices[7]); - - //for (int i = 0; i < 8; ++i) - //{ - // s0[i] = i; - // s1[i] = i + 100; - // s2[i] = i + 200; - // s3[i] = i + 300; - // s4[i] = i + 400; - // s5[i] = i + 500; - // s6[i] = i + 600; - // s7[i] = i + 700; - //} - + if (Avx.IsSupported) { - //Load every body for the first half of the motion state. - //Note that buffers are allocated on cache line boundaries, so we can use aligned loads for all that matters. - var m0 = Avx2.LoadVector256(s0); - var m1 = count > 1 ? Avx2.LoadAlignedVector256(s1) : Vector256.Zero; - var m2 = count > 2 ? Avx2.LoadAlignedVector256(s2) : Vector256.Zero; - var m3 = count > 3 ? Avx2.LoadAlignedVector256(s3) : Vector256.Zero; - var m4 = count > 4 ? Avx2.LoadAlignedVector256(s4) : Vector256.Zero; - var m5 = count > 5 ? Avx2.LoadAlignedVector256(s5) : Vector256.Zero; - var m6 = count > 6 ? Avx2.LoadAlignedVector256(s6) : Vector256.Zero; - var m7 = count > 7 ? Avx2.LoadAlignedVector256(s7) : Vector256.Zero; - - var n0 = Avx2.UnpackLow(m0, m1); - var n1 = Avx2.UnpackLow(m2, m3); - var n2 = Avx2.UnpackLow(m4, m5); - var n3 = Avx2.UnpackLow(m6, m7); - var n4 = Avx2.UnpackHigh(m0, m1); - var n5 = Avx2.UnpackHigh(m2, m3); - var n6 = Avx2.UnpackHigh(m4, m5); - var n7 = Avx2.UnpackHigh(m6, m7); - - var o0 = Avx2.Shuffle(n0, n1, 0 | (1 << 2) | (0 << 4) | (1 << 6)); - var o1 = Avx2.Shuffle(n2, n3, 0 | (1 << 2) | (0 << 4) | (1 << 6)); - var o2 = Avx2.Shuffle(n4, n5, 0 | (1 << 2) | (0 << 4) | (1 << 6)); - var o3 = Avx2.Shuffle(n6, n7, 0 | (1 << 2) | (0 << 4) | (1 << 6)); - var o4 = Avx2.Shuffle(n0, n1, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - var o5 = Avx2.Shuffle(n2, n3, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - var o6 = Avx2.Shuffle(n4, n5, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - var o7 = Avx2.Shuffle(n6, n7, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - - orientation.X = Avx2.Permute2x128(o0, o1, 0 | (2 << 4)).AsVector(); - orientation.Y = Avx2.Permute2x128(o4, o5, 0 | (2 << 4)).AsVector(); - orientation.Z = Avx2.Permute2x128(o2, o3, 0 | (2 << 4)).AsVector(); - orientation.W = Avx2.Permute2x128(o6, o7, 0 | (2 << 4)).AsVector(); - position.X= Avx2.Permute2x128(o0, o1, 1 | (3 << 4)).AsVector(); - position.Y = Avx2.Permute2x128(o4, o5, 1 | (3 << 4)).AsVector(); - position.Z = Avx2.Permute2x128(o2, o3, 1 | (3 << 4)).AsVector(); - inertia.InverseMass = Avx2.Permute2x128(o6, o7, 1 | (3 << 4)).AsVector(); - inertia.InverseInertiaTensor.XX = inertia.InverseMass; - inertia.InverseInertiaTensor.YY = inertia.InverseMass; - inertia.InverseInertiaTensor.ZZ = inertia.InverseMass; - inertia.InverseInertiaTensor.YX = default; - inertia.InverseInertiaTensor.ZX = default; - inertia.InverseInertiaTensor.ZY = default; - } + var indices = (int*)Unsafe.AsPointer(ref baseIndex); + + var s0 = (float*)(motionStates + indices[0]); + var s1 = (float*)(motionStates + indices[1]); + var s2 = (float*)(motionStates + indices[2]); + var s3 = (float*)(motionStates + indices[3]); + var s4 = (float*)(motionStates + indices[4]); + var s5 = (float*)(motionStates + indices[5]); + var s6 = (float*)(motionStates + indices[6]); + var s7 = (float*)(motionStates + indices[7]); + + //for (int i = 0; i < 8; ++i) + //{ + // s0[i] = i; + // s1[i] = i + 100; + // s2[i] = i + 200; + // s3[i] = i + 300; + // s4[i] = i + 400; + // s5[i] = i + 500; + // s6[i] = i + 600; + // s7[i] = i + 700; + //} + { + //Load every body for the first half of the motion state. + //Note that buffers are allocated on cache line boundaries, so we can use aligned loads for all that matters. + var m0 = Avx.LoadVector256(s0); + var m1 = count > 1 ? Avx.LoadAlignedVector256(s1) : Vector256.Zero; + var m2 = count > 2 ? Avx.LoadAlignedVector256(s2) : Vector256.Zero; + var m3 = count > 3 ? Avx.LoadAlignedVector256(s3) : Vector256.Zero; + var m4 = count > 4 ? Avx.LoadAlignedVector256(s4) : Vector256.Zero; + var m5 = count > 5 ? Avx.LoadAlignedVector256(s5) : Vector256.Zero; + var m6 = count > 6 ? Avx.LoadAlignedVector256(s6) : Vector256.Zero; + var m7 = count > 7 ? Avx.LoadAlignedVector256(s7) : Vector256.Zero; + + var n0 = Avx.UnpackLow(m0, m1); + var n1 = Avx.UnpackLow(m2, m3); + var n2 = Avx.UnpackLow(m4, m5); + var n3 = Avx.UnpackLow(m6, m7); + var n4 = Avx.UnpackHigh(m0, m1); + var n5 = Avx.UnpackHigh(m2, m3); + var n6 = Avx.UnpackHigh(m4, m5); + var n7 = Avx.UnpackHigh(m6, m7); + + var o0 = Avx.Shuffle(n0, n1, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o1 = Avx.Shuffle(n2, n3, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o2 = Avx.Shuffle(n4, n5, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o3 = Avx.Shuffle(n6, n7, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o4 = Avx.Shuffle(n0, n1, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o5 = Avx.Shuffle(n2, n3, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o6 = Avx.Shuffle(n4, n5, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o7 = Avx.Shuffle(n6, n7, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + + orientation.X = Avx.Permute2x128(o0, o1, 0 | (2 << 4)).AsVector(); + orientation.Y = Avx.Permute2x128(o4, o5, 0 | (2 << 4)).AsVector(); + orientation.Z = Avx.Permute2x128(o2, o3, 0 | (2 << 4)).AsVector(); + orientation.W = Avx.Permute2x128(o6, o7, 0 | (2 << 4)).AsVector(); + position.X = Avx.Permute2x128(o0, o1, 1 | (3 << 4)).AsVector(); + position.Y = Avx.Permute2x128(o4, o5, 1 | (3 << 4)).AsVector(); + position.Z = Avx.Permute2x128(o2, o3, 1 | (3 << 4)).AsVector(); + inertia.InverseMass = Avx.Permute2x128(o6, o7, 1 | (3 << 4)).AsVector(); + inertia.InverseInertiaTensor.XX = inertia.InverseMass; + inertia.InverseInertiaTensor.YY = inertia.InverseMass; + inertia.InverseInertiaTensor.ZZ = inertia.InverseMass; + inertia.InverseInertiaTensor.YX = default; + inertia.InverseInertiaTensor.ZX = default; + inertia.InverseInertiaTensor.ZY = default; + } + + { + //Second half. + var m0 = Avx.LoadVector256(s0 + 8); + var m1 = count > 1 ? Avx.LoadAlignedVector256(s1 + 8) : Vector256.Zero; + var m2 = count > 2 ? Avx.LoadAlignedVector256(s2 + 8) : Vector256.Zero; + var m3 = count > 3 ? Avx.LoadAlignedVector256(s3 + 8) : Vector256.Zero; + var m4 = count > 4 ? Avx.LoadAlignedVector256(s4 + 8) : Vector256.Zero; + var m5 = count > 5 ? Avx.LoadAlignedVector256(s5 + 8) : Vector256.Zero; + var m6 = count > 6 ? Avx.LoadAlignedVector256(s6 + 8) : Vector256.Zero; + var m7 = count > 7 ? Avx.LoadAlignedVector256(s7 + 8) : Vector256.Zero; + + var n0 = Avx.UnpackLow(m0, m1); + var n1 = Avx.UnpackLow(m2, m3); + var n2 = Avx.UnpackLow(m4, m5); + var n3 = Avx.UnpackLow(m6, m7); + var n4 = Avx.UnpackHigh(m0, m1); + var n5 = Avx.UnpackHigh(m2, m3); + var n6 = Avx.UnpackHigh(m4, m5); + var n7 = Avx.UnpackHigh(m6, m7); + + var o0 = Avx.Shuffle(n0, n1, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o1 = Avx.Shuffle(n2, n3, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o2 = Avx.Shuffle(n4, n5, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o3 = Avx.Shuffle(n6, n7, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o4 = Avx.Shuffle(n0, n1, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o5 = Avx.Shuffle(n2, n3, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o6 = Avx.Shuffle(n4, n5, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o7 = Avx.Shuffle(n6, n7, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + + velocity.Linear.X = Avx.Permute2x128(o0, o1, 0 | (2 << 4)).AsVector(); + velocity.Linear.Y = Avx.Permute2x128(o4, o5, 0 | (2 << 4)).AsVector(); + velocity.Linear.Z = Avx.Permute2x128(o2, o3, 0 | (2 << 4)).AsVector(); + velocity.Angular.X = Avx.Permute2x128(o6, o7, 0 | (2 << 4)).AsVector(); + velocity.Angular.Y = Avx.Permute2x128(o0, o1, 1 | (3 << 4)).AsVector(); + velocity.Angular.Z = Avx.Permute2x128(o4, o5, 1 | (3 << 4)).AsVector(); + } + } + else { - //Second half. - var m0 = Avx2.LoadVector256(s0 + 8); - var m1 = count > 1 ? Avx2.LoadAlignedVector256(s1 + 8) : Vector256.Zero; - var m2 = count > 2 ? Avx2.LoadAlignedVector256(s2 + 8) : Vector256.Zero; - var m3 = count > 3 ? Avx2.LoadAlignedVector256(s3 + 8) : Vector256.Zero; - var m4 = count > 4 ? Avx2.LoadAlignedVector256(s4 + 8) : Vector256.Zero; - var m5 = count > 5 ? Avx2.LoadAlignedVector256(s5 + 8) : Vector256.Zero; - var m6 = count > 6 ? Avx2.LoadAlignedVector256(s6 + 8) : Vector256.Zero; - var m7 = count > 7 ? Avx2.LoadAlignedVector256(s7 + 8) : Vector256.Zero; - - var n0 = Avx2.UnpackLow(m0, m1); - var n1 = Avx2.UnpackLow(m2, m3); - var n2 = Avx2.UnpackLow(m4, m5); - var n3 = Avx2.UnpackLow(m6, m7); - var n4 = Avx2.UnpackHigh(m0, m1); - var n5 = Avx2.UnpackHigh(m2, m3); - var n6 = Avx2.UnpackHigh(m4, m5); - var n7 = Avx2.UnpackHigh(m6, m7); - - var o0 = Avx2.Shuffle(n0, n1, 0 | (1 << 2) | (0 << 4) | (1 << 6)); - var o1 = Avx2.Shuffle(n2, n3, 0 | (1 << 2) | (0 << 4) | (1 << 6)); - var o2 = Avx2.Shuffle(n4, n5, 0 | (1 << 2) | (0 << 4) | (1 << 6)); - var o3 = Avx2.Shuffle(n6, n7, 0 | (1 << 2) | (0 << 4) | (1 << 6)); - var o4 = Avx2.Shuffle(n0, n1, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - var o5 = Avx2.Shuffle(n2, n3, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - var o6 = Avx2.Shuffle(n4, n5, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - var o7 = Avx2.Shuffle(n6, n7, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - - velocity.Linear.X = Avx2.Permute2x128(o0, o1, 0 | (2 << 4)).AsVector(); - velocity.Linear.Y = Avx2.Permute2x128(o4, o5, 0 | (2 << 4)).AsVector(); - velocity.Linear.Z = Avx2.Permute2x128(o2, o3, 0 | (2 << 4)).AsVector(); - velocity.Angular.X = Avx2.Permute2x128(o6, o7, 0 | (2 << 4)).AsVector(); - velocity.Angular.Y = Avx2.Permute2x128(o0, o1, 1 | (3 << 4)).AsVector(); - velocity.Angular.Z = Avx2.Permute2x128(o4, o5, 1 | (3 << 4)).AsVector(); - + ScalarGather(count, motionStates, ref baseIndex, ref position, ref orientation, ref velocity, ref inertia); } } @@ -297,27 +302,19 @@ public unsafe void GatherState(ref TwoBodyReferences references, int count, ref var states = ref ActiveSet.MotionStates; + TransposingGather(count, states.Memory, ref references.IndexA, ref positionA, ref orientationA, ref velocityA, ref inertiaA); + TransposingGather(count, states.Memory, ref references.IndexB, ref positionB, ref orientationB, ref velocityB, ref inertiaB); - if (Avx2.IsSupported) - { - TransposingGather(count, states.Memory, ref references.IndexA, ref positionA, ref orientationA, ref velocityA, ref inertiaA); - TransposingGather(count, states.Memory, ref references.IndexB, ref positionB, ref orientationB, ref velocityB, ref inertiaB); - } - else - { - ScalarGather(count, states.Memory, ref references.IndexA, ref positionA, ref orientationA, ref velocityA, ref inertiaA); - ScalarGather(count, states.Memory, ref references.IndexB, ref positionB, ref orientationB, ref velocityB, ref inertiaB); + //for (int i = 0; i < count; ++i) + //{ + // //WriteGatherState(ref baseIndexA, i, ref states, ref positionA, ref orientationA, ref velocityA); + // //WriteGatherInertia(ref baseIndexA, i, ref Inertias, ref inertiaA); + // //WriteGatherState(ref baseIndexB, i, ref states, ref positionB, ref orientationB, ref velocityB); + // //WriteGatherInertia(ref baseIndexB, i, ref Inertias, ref inertiaB); + // WriteGatherState(ref baseIndexA, i, ref states, ref positionA, ref orientationA, ref velocityA, ref inertiaA); + // WriteGatherState(ref baseIndexB, i, ref states, ref positionB, ref orientationB, ref velocityB, ref inertiaB); + //} - //for (int i = 0; i < count; ++i) - //{ - // //WriteGatherState(ref baseIndexA, i, ref states, ref positionA, ref orientationA, ref velocityA); - // //WriteGatherInertia(ref baseIndexA, i, ref Inertias, ref inertiaA); - // //WriteGatherState(ref baseIndexB, i, ref states, ref positionB, ref orientationB, ref velocityB); - // //WriteGatherInertia(ref baseIndexB, i, ref Inertias, ref inertiaB); - // WriteGatherState(ref baseIndexA, i, ref states, ref positionA, ref orientationA, ref velocityA, ref inertiaA); - // WriteGatherState(ref baseIndexB, i, ref states, ref positionB, ref orientationB, ref velocityB, ref inertiaB); - //} - } //TODO: In future versions, we will likely store the body position in different forms to allow for extremely large worlds. //That will be an opt-in feature. The default implementation will use the FP32 representation, but the user could choose to swap it out for a fp64 or fixed64 representation. //This affects other systems- AABB calculation, pose integration, solving, and in extreme (64 bit) cases, the broadphase. @@ -604,6 +601,56 @@ private unsafe void ScatterVelocities(ref BodyVelocities sourceVelocities, ref i target.Angular = new Vector3(sourceSlot.Angular.X[0], sourceSlot.Angular.Y[0], sourceSlot.Angular.Z[0]); } + //[MethodImpl(MethodImplOptions.AggressiveInlining)] + private unsafe void TransposeScatterVelocities(ref BodyVelocities sourceVelocities, MotionState* motionStates, ref Vector references, int count) + { + if (Avx.IsSupported) + { + //Just like the gather but... transposed, we transpose from the wide representation into per-body velocities. + //The motion state struct puts the velocities into the second 32 byte chunk, so we can do a single write per body. + var m0 = sourceVelocities.Linear.X.AsVector256(); + var m1 = sourceVelocities.Linear.Y.AsVector256(); + var m2 = sourceVelocities.Linear.Z.AsVector256(); + var m3 = Vector256.Zero; + var m4 = sourceVelocities.Angular.X.AsVector256(); + var m5 = sourceVelocities.Angular.Y.AsVector256(); + var m6 = sourceVelocities.Angular.Z.AsVector256(); + var m7 = Vector256.Zero; + + var n0 = Avx.UnpackLow(m0, m1); + var n1 = Avx.UnpackLow(m2, m3); + var n2 = Avx.UnpackLow(m4, m5); + var n3 = Avx.UnpackLow(m6, m7); + var n4 = Avx.UnpackHigh(m0, m1); + var n5 = Avx.UnpackHigh(m2, m3); + var n6 = Avx.UnpackHigh(m4, m5); + var n7 = Avx.UnpackHigh(m6, m7); + + var o0 = Avx.Shuffle(n0, n1, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o1 = Avx.Shuffle(n2, n3, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o2 = Avx.Shuffle(n4, n5, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o3 = Avx.Shuffle(n6, n7, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o4 = Avx.Shuffle(n0, n1, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o5 = Avx.Shuffle(n2, n3, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o6 = Avx.Shuffle(n4, n5, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o7 = Avx.Shuffle(n6, n7, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + + var indices = (int*)Unsafe.AsPointer(ref references); + Avx.StoreAligned((float*)(motionStates + indices[0]) + 8, Avx.Permute2x128(o0, o1, 0 | (2 << 4))); + if (count > 1) Avx.StoreAligned((float*)(motionStates + indices[1]) + 8, Avx.Permute2x128(o4, o5, 0 | (2 << 4))); + if (count > 2) Avx.StoreAligned((float*)(motionStates + indices[2]) + 8, Avx.Permute2x128(o2, o3, 0 | (2 << 4))); + if (count > 3) Avx.StoreAligned((float*)(motionStates + indices[3]) + 8, Avx.Permute2x128(o6, o7, 0 | (2 << 4))); + if (count > 4) Avx.StoreAligned((float*)(motionStates + indices[4]) + 8, Avx.Permute2x128(o0, o1, 1 | (3 << 4))); + if (count > 5) Avx.StoreAligned((float*)(motionStates + indices[5]) + 8, Avx.Permute2x128(o4, o5, 1 | (3 << 4))); + if (count > 6) Avx.StoreAligned((float*)(motionStates + indices[6]) + 8, Avx.Permute2x128(o2, o3, 1 | (3 << 4))); + if (count > 7) Avx.StoreAligned((float*)(motionStates + indices[7]) + 8, Avx.Permute2x128(o6, o7, 1 | (3 << 4))); + } + else + { + + } + } + /// /// Scatters velocities for one body bundle into the active body set. @@ -636,14 +683,17 @@ public unsafe void ScatterVelocities(ref BodyVelocities sourceVelocitiesA, ref B ref TwoBodyReferences references, int count) { Debug.Assert(count >= 0 && count <= Vector.Count); - //Grab the base references for the body indices. Note that we make use of the references memory layout again. - ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); - ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); - for (int i = 0; i < count; ++i) - { - ScatterVelocities(ref sourceVelocitiesA, ref baseIndexA, i); - ScatterVelocities(ref sourceVelocitiesB, ref baseIndexB, i); - } + var motionStates = ActiveSet.MotionStates.Memory; + TransposeScatterVelocities(ref sourceVelocitiesA, motionStates, ref references.IndexA, count); + TransposeScatterVelocities(ref sourceVelocitiesB, motionStates, ref references.IndexB, count); + ////Grab the base references for the body indices. Note that we make use of the references memory layout again. + //ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); + //ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); + //for (int i = 0; i < count; ++i) + //{ + // ScatterVelocities(ref sourceVelocitiesA, ref baseIndexA, i); + // ScatterVelocities(ref sourceVelocitiesB, ref baseIndexB, i); + //} } /// From 2a0f68dc7863639f816233adfa0ec4d5eafd6d99 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 1 Jul 2021 21:38:29 -0500 Subject: [PATCH 059/947] In the middle of weld speculative revamp. --- BepuPhysics/BepuPhysics.csproj | 1 + BepuPhysics/Bodies_GatherScatter.cs | 13 ++- .../Constraints/TwoBodyTypeProcessor.cs | 12 ++- BepuPhysics/Constraints/Weld.cs | 99 ++++++++++++++++++- BepuUtilities/BepuUtilities.csproj | 1 + BepuUtilities/Symmetric3x3Wide.cs | 2 +- BepuUtilities/Symmetric6x6Wide.cs | 2 +- 7 files changed, 116 insertions(+), 14 deletions(-) diff --git a/BepuPhysics/BepuPhysics.csproj b/BepuPhysics/BepuPhysics.csproj index c8cb55114..bb1b09cdc 100644 --- a/BepuPhysics/BepuPhysics.csproj +++ b/BepuPhysics/BepuPhysics.csproj @@ -18,6 +18,7 @@ false key.snk true + false diff --git a/BepuPhysics/Bodies_GatherScatter.cs b/BepuPhysics/Bodies_GatherScatter.cs index 5c654d4ba..b5324ae96 100644 --- a/BepuPhysics/Bodies_GatherScatter.cs +++ b/BepuPhysics/Bodies_GatherScatter.cs @@ -601,7 +601,7 @@ private unsafe void ScatterVelocities(ref BodyVelocities sourceVelocities, ref i target.Angular = new Vector3(sourceSlot.Angular.X[0], sourceSlot.Angular.Y[0], sourceSlot.Angular.Z[0]); } - //[MethodImpl(MethodImplOptions.AggressiveInlining)] + [MethodImpl(MethodImplOptions.AggressiveInlining)] private unsafe void TransposeScatterVelocities(ref BodyVelocities sourceVelocities, MotionState* motionStates, ref Vector references, int count) { if (Avx.IsSupported) @@ -611,20 +611,19 @@ private unsafe void TransposeScatterVelocities(ref BodyVelocities sourceVelociti var m0 = sourceVelocities.Linear.X.AsVector256(); var m1 = sourceVelocities.Linear.Y.AsVector256(); var m2 = sourceVelocities.Linear.Z.AsVector256(); - var m3 = Vector256.Zero; var m4 = sourceVelocities.Angular.X.AsVector256(); var m5 = sourceVelocities.Angular.Y.AsVector256(); var m6 = sourceVelocities.Angular.Z.AsVector256(); - var m7 = Vector256.Zero; + //We're being a bit lazy here- you could reduce the instructions a bit more given the two empty source lanes. var n0 = Avx.UnpackLow(m0, m1); - var n1 = Avx.UnpackLow(m2, m3); + var n1 = Avx.UnpackLow(m2, m2); var n2 = Avx.UnpackLow(m4, m5); - var n3 = Avx.UnpackLow(m6, m7); + var n3 = Avx.UnpackLow(m6, m6); var n4 = Avx.UnpackHigh(m0, m1); - var n5 = Avx.UnpackHigh(m2, m3); + var n5 = Avx.UnpackHigh(m2, m2); var n6 = Avx.UnpackHigh(m4, m5); - var n7 = Avx.UnpackHigh(m6, m7); + var n7 = Avx.UnpackHigh(m6, m6); var o0 = Avx.Shuffle(n0, n1, 0 | (1 << 2) | (0 << 4) | (1 << 6)); var o1 = Avx.Shuffle(n2, n3, 0 | (1 << 2) | (0 << 4) | (1 << 6)); diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 31bbad444..f78ca8da0 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -358,8 +358,16 @@ public unsafe override void WarmStart2(ref TypeBatch typeBatch, Bodies bodies, f var count = GetCountInBundle(ref typeBatch, i); Prefetch(ref typeBatch, ref bodyReferencesBundles, ref motionStates, i, exclusiveEndBundle); bodies.GatherState(ref references, count, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB); - function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, dt, inverseDt, ref prestep, out var projection); - function.WarmStart(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); + if (typeof(TConstraintFunctions) == typeof(WeldFunctions)) + { + default(WeldFunctions).PrestepWarmstart(orientationA, inertiaA, ab, orientationB, inertiaB, dt, inverseDt, + Unsafe.As(ref prestep), Unsafe.As(ref accumulatedImpulses), ref wsvA, ref wsvB); + } + else + { + function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, dt, inverseDt, ref prestep, out var projection); + function.WarmStart(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); + } bodies.ScatterVelocities(ref wsvA, ref wsvB, ref references, count); } } diff --git a/BepuPhysics/Constraints/Weld.cs b/BepuPhysics/Constraints/Weld.cs index d851adbab..6e23b47b8 100644 --- a/BepuPhysics/Constraints/Weld.cs +++ b/BepuPhysics/Constraints/Weld.cs @@ -87,7 +87,7 @@ public struct WeldAccumulatedImpulses public struct WeldFunctions : IConstraintFunctions { - //[MethodImpl(MethodImplOptions.AggressiveInlining)] + [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, float dt, float inverseDt, ref WeldPrestepData prestep, out WeldProjection projection) { @@ -167,13 +167,13 @@ private static void ApplyImpulse(ref BodyVelocities velocityA, ref BodyVelocitie Vector3Wide.Subtract(velocityB.Angular, negatedAngularChangeB, out velocityB.Angular); //note subtraction; the jacobian is -I } - //[MethodImpl(MethodImplOptions.AggressiveInlining)] + [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref WeldProjection projection, ref WeldAccumulatedImpulses accumulatedImpulse) { ApplyImpulse(ref velocityA, ref velocityB, ref projection, ref accumulatedImpulse.Orientation, ref accumulatedImpulse.Offset); } - //[MethodImpl(MethodImplOptions.AggressiveInlining)] + [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref WeldProjection projection, ref WeldAccumulatedImpulses accumulatedImpulse) { //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); @@ -199,6 +199,99 @@ public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, re ApplyImpulse(ref velocityA, ref velocityB, ref projection, ref orientationCSI, ref offsetCSI); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + private static void ApplyImpulse(in BodyInertias inertiaA, in BodyInertias inertiaB, in Vector3Wide offset, in Vector3Wide orientationCSI, in Vector3Wide offsetCSI, ref BodyVelocities velocityA, ref BodyVelocities velocityB) + { + //Recall the jacobians: + //J = [ 0, I, 0, -I ] + // [ I, skewSymmetric(localOffset * orientationA), -I, 0 ] + //The velocity changes are: + // csi * J * I^-1 + //linearImpulseA = offsetCSI + //angularImpulseA = orientationCSI + worldOffset x offsetCSI + //linearImpulseB = -offsetCSI + //angularImpulseB = -orientationCSI + Vector3Wide.Scale(offsetCSI, inertiaA.InverseMass, out var linearChangeA); + Vector3Wide.Add(velocityA.Linear, linearChangeA, out velocityA.Linear); + + //Note order of cross relative to the SolveIteration. + //SolveIteration transforms velocity into constraint space velocity using JT, while this converts constraint space to world space using J. + //The elements are transposed, and transposed skew symmetric matrices are negated. Flipping the cross product is equivalent to a negation. + Vector3Wide.CrossWithoutOverlap(offset, offsetCSI, out var offsetWorldImpulse); + Vector3Wide.Add(offsetWorldImpulse, orientationCSI, out var angularImpulseA); + Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseA, inertiaA.InverseInertiaTensor, out var angularChangeA); + Vector3Wide.Add(velocityA.Angular, angularChangeA, out velocityA.Angular); + + Vector3Wide.Scale(offsetCSI, inertiaB.InverseMass, out var negatedLinearChangeB); + Vector3Wide.Subtract(velocityB.Linear, negatedLinearChangeB, out velocityB.Linear); //note subtraction; the jacobian is -I + + Symmetric3x3Wide.TransformWithoutOverlap(orientationCSI, inertiaB.InverseInertiaTensor, out var negatedAngularChangeB); + Vector3Wide.Subtract(velocityB.Angular, negatedAngularChangeB, out velocityB.Angular); //note subtraction; the jacobian is -I + } + + //[MethodImpl(MethodImplOptions.AggressiveInlining)] + public void WarmStart2(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, float dt, float inverseDt, + in WeldPrestepData prestep, in WeldAccumulatedImpulses accumulatedImpulses, ref BodyVelocities wsvA, ref BodyVelocities wsvB) + { + //The weld constraint handles 6 degrees of freedom simultaneously. The constraints are: + //localOrientation * orientationA = orientationB + //positionA + localOffset * orientationA = positionB + //The velocity derivatives: + //angularVelocityA = angularVelocityB + //linearVelocityA + angularVelocityA x (localOffset * orientationA) = linearVelocityB + //Note that the position constraint is similar a ball socket joint, except the anchor point is on top of the center of mass of object B. + QuaternionWide.TransformWithoutOverlap(prestep.LocalOffset, orientationA, out var offset); + ApplyImpulse(inertiaA, inertiaB, offset, accumulatedImpulses.Orientation, accumulatedImpulses.Offset, ref wsvA, ref wsvB); + } + public void Solve2(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, float dt, float inverseDt, + in WeldPrestepData prestep, in WeldAccumulatedImpulses accumulatedImpulses, ref BodyVelocities wsvA, ref BodyVelocities wsvB) + { + //The weld constraint handles 6 degrees of freedom simultaneously. The constraints are: + //localOrientation * orientationA = orientationB + //positionA + localOffset * orientationA = positionB + //The velocity derivatives: + //angularVelocityA = angularVelocityB + //linearVelocityA + angularVelocityA x (localOffset * orientationA) = linearVelocityB + //Note that the position constraint is similar a ball socket joint, except the anchor point is on top of the center of mass of object B. + + //From the above, the jacobians ordered as [linearA, angularA, linearB, angularB] are: + //J = [ 0, I, 0, -I ] + // [ I, skewSymmetric(localOffset * orientationA), -I, 0 ] + //where I is the 3x3 identity matrix. + //Effective mass = (J * M^-1 * JT)^-1, which is going to be a little tricky because J * M^-1 * JT is a 6x6 matrix: + //J * M^-1 * JT = [ Ia^-1 + Ib^-1, Ia^-1 * transpose(skewSymmetric(localOffset * orientationA)) ] + // [ skewSymmetric(localOffset * orientationA) * Ia^-1, Ma^-1 + Mb^-1 + skewSymmetric(localOffset * orientationA) * Ia^-1 * transpose(skewSymmetric(localOffset * orientationA)) ] + //where Ia^-1 and Ib^-1 are the inverse inertia tensors for a and b and Ma^-1 and Mb^-1 are the inverse masses of A and B expanded to 3x3 diagonal matrices. + Symmetric3x3Wide.Add(inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, out var jmjtA); + QuaternionWide.TransformWithoutOverlap(prestep.LocalOffset, orientationA, out var offset); + Matrix3x3Wide.CreateCrossProduct(offset, out var xAB); + Symmetric3x3Wide.Multiply(inertiaA.InverseInertiaTensor, xAB, out var jmjtB); + Symmetric3x3Wide.CompleteMatrixSandwichTranspose(xAB, jmjtB, out var jmjtD); + var diagonalAdd = inertiaA.InverseMass + inertiaB.InverseMass; + jmjtD.XX += diagonalAdd; + jmjtD.YY += diagonalAdd; + jmjtD.ZZ += diagonalAdd; + //Note that there is no need to invert that 6x6 chonk. We want to convert a constraint space velocity into a constraint space impulse, csi = csv * effectiveMass. + //This is equivalent to solving csi * effectiveMass^-1 = csv for csi, and since effectiveMass^-1 is symmetric positive semidefinite, we can use an LDLT decomposition to quickly solve it. + + SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); + + //Compute the current constraint error for all 6 degrees of freedom. + //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. + Vector3Wide.Subtract(ab, offset, out var positionError); + QuaternionWide.ConcatenateWithoutOverlap(prestep.LocalOrientation, orientationA, out var targetOrientationB); + QuaternionWide.Conjugate(targetOrientationB, out var inverseTarget); + QuaternionWide.ConcatenateWithoutOverlap(inverseTarget, orientationB, out var rotationError); + QuaternionWide.GetApproximateAxisAngleFromQuaternion(rotationError, out var rotationErrorAxis, out var rotationErrorLength); + + Vector3Wide.Scale(positionError, positionErrorToVelocity, out var offsetBiasVelocity); + Vector3Wide.Scale(rotationErrorAxis, rotationErrorLength * positionErrorToVelocity, out var orientationBiasVelocity); + + ApplyImpulse(inertiaA, inertiaB, ab, accumulatedImpulses.Orientation, accumulatedImpulses.Offset, ref wsvA, ref wsvB); + } + + } diff --git a/BepuUtilities/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj index a6ba9614d..ff2af19d1 100644 --- a/BepuUtilities/BepuUtilities.csproj +++ b/BepuUtilities/BepuUtilities.csproj @@ -19,6 +19,7 @@ false key.snk true + false diff --git a/BepuUtilities/Symmetric3x3Wide.cs b/BepuUtilities/Symmetric3x3Wide.cs index f9ea5307c..93f04a9f8 100644 --- a/BepuUtilities/Symmetric3x3Wide.cs +++ b/BepuUtilities/Symmetric3x3Wide.cs @@ -41,7 +41,7 @@ public struct Symmetric3x3Wide /// /// Symmetric matrix to invert. /// Inverse of the symmetric matrix. - [MethodImpl(MethodImplOptions.AggressiveInlining)] + //[MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Invert(in Symmetric3x3Wide m, out Symmetric3x3Wide inverse) { var xx = m.YY * m.ZZ - m.ZY * m.ZY; diff --git a/BepuUtilities/Symmetric6x6Wide.cs b/BepuUtilities/Symmetric6x6Wide.cs index 49ba1cef8..7594892e0 100644 --- a/BepuUtilities/Symmetric6x6Wide.cs +++ b/BepuUtilities/Symmetric6x6Wide.cs @@ -46,7 +46,7 @@ public static void Invert(in Symmetric3x3Wide a, in Matrix3x3Wide b, in Symmetri Symmetric3x3Wide.Add(result.D, invD, out result.D); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] + //[MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Invert(in Symmetric6x6Wide m, out Symmetric6x6Wide result) { Invert(m.A, m.B, m.D, out result); From 82a26e1f6471f6e9bf00710407f7720883f924fa Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 2 Jul 2021 12:50:19 -0500 Subject: [PATCH 060/947] Added less annoying functions and operators to Vector3Wide, thanks to .NET 5. --- .../Constraints/TwoBodyTypeProcessor.cs | 2 +- BepuUtilities/Vector3Wide.cs | 322 +++++++++++++++++- Demos/Demos/NewtDemo.cs | 21 +- 3 files changed, 341 insertions(+), 4 deletions(-) diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index f78ca8da0..88cdfad98 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -360,7 +360,7 @@ public unsafe override void WarmStart2(ref TypeBatch typeBatch, Bodies bodies, f bodies.GatherState(ref references, count, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB); if (typeof(TConstraintFunctions) == typeof(WeldFunctions)) { - default(WeldFunctions).PrestepWarmstart(orientationA, inertiaA, ab, orientationB, inertiaB, dt, inverseDt, + default(WeldFunctions).WarmStart2(orientationA, inertiaA, ab, orientationB, inertiaB, dt, inverseDt, Unsafe.As(ref prestep), Unsafe.As(ref accumulatedImpulses), ref wsvA, ref wsvB); } else diff --git a/BepuUtilities/Vector3Wide.cs b/BepuUtilities/Vector3Wide.cs index 30f530eb3..f95bebe9d 100644 --- a/BepuUtilities/Vector3Wide.cs +++ b/BepuUtilities/Vector3Wide.cs @@ -17,6 +17,13 @@ public Vector3Wide(ref Vector s) Y = s; Z = s; } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public Vector3Wide(Vector s) + { + X = s; + Y = s; + Z = s; + } [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Add(in Vector3Wide a, in Vector3Wide b, out Vector3Wide result) @@ -38,7 +45,52 @@ public static void Add(in Vector3Wide v, in Vector s, out Vector3Wide res result.Y = v.Y + s; result.Z = v.Z + s; } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3Wide operator +(Vector3Wide a, Vector3Wide b) + { + Vector3Wide result; + result.X = a.X + b.X; + result.Y = a.Y + b.Y; + result.Z = a.Z + b.Z; + return result; + } + /// + /// Finds the result of adding a scalar to every component of a vector. + /// + /// Vector to add to. + /// Scalar to add to every component of the vector. + /// Vector with components equal to the input vector added to the input scalar. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3Wide operator +(Vector3Wide v, Vector s) + { + Vector3Wide result; + result.X = v.X + s; + result.Y = v.Y + s; + result.Z = v.Z + s; + return result; + } + /// + /// Finds the result of adding a scalar to every component of a vector. + /// + /// Vector to add to. + /// Scalar to add to every component of the vector. + /// Vector with components equal to the input vector added to the input scalar. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3Wide operator +(Vector s, Vector3Wide v) + { + Vector3Wide result; + result.X = v.X + s; + result.Y = v.Y + s; + result.Z = v.Z + s; + return result; + } + /// + /// Subtracts one vector from another. + /// + /// Vector to subtract from. + /// Vector to subtract from the first vector. + /// Vector with components equal the input scalar subtracted from the input vector. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Subtract(in Vector3Wide a, in Vector3Wide b, out Vector3Wide result) { @@ -60,6 +112,23 @@ public static void Subtract(in Vector3Wide v, in Vector s, out Vector3Wid result.Y = v.Y - s; result.Z = v.Z - s; } + + /// + /// Subtracts one vector from another. + /// + /// Vector to subtract from. + /// Vector to subtract from the first vector. + /// Vector with components equal the input scalar subtracted from the input vector. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3Wide operator -(Vector3Wide a, Vector3Wide b) + { + Vector3Wide result; + result.X = a.X - b.X; + result.Y = a.Y - b.Y; + result.Z = a.Z - b.Z; + return result; + } + /// /// Finds the result of subtracting the components of a vector from a scalar. /// @@ -74,12 +143,35 @@ public static void Subtract(in Vector s, in Vector3Wide v, out Vector3Wid result.Z = s - v.Z; } + /// + /// Finds the result of subtracting the components of a vector from a scalar. + /// + /// Vector to subtract from the scalar. + /// Scalar to subtract from. + /// Vector with components equal the input vector subtracted from the input scalar. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3Wide operator -(Vector3Wide a, Vector b) + { + Vector3Wide result; + result.X = a.X - b; + result.Y = a.Y - b; + result.Z = a.Z - b; + return result; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Dot(in Vector3Wide a, in Vector3Wide b, out Vector result) { result = a.X * b.X + a.Y * b.Y + a.Z * b.Z; } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector Dot(Vector3Wide a, Vector3Wide b) + { + return a.X * b.X + a.Y * b.Y + a.Z * b.Z; + } + /// /// Computes the per-component minimum between a scalar value and the components of a vector. /// @@ -106,6 +198,38 @@ public static void Min(in Vector3Wide a, in Vector3Wide b, out Vector3Wide resul result.Y = Vector.Min(a.Y, b.Y); result.Z = Vector.Min(a.Z, b.Z); } + + /// + /// Computes the per-component minimum between a scalar value and the components of a vector. + /// + /// Scalar to compare to each vector component. + /// Vector whose components will be compared. + /// Vector with components matching the smaller of the scalar value and the input vector. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3Wide Min(Vector s, Vector3Wide v) + { + Vector3Wide result; + result.X = Vector.Min(s, v.X); + result.Y = Vector.Min(s, v.Y); + result.Z = Vector.Min(s, v.Z); + return result; + } + /// + /// Computes the per-component minimum of two vectors. + /// + /// First vector whose components will be compared. + /// Second vector whose components will be compared. + /// Vector with components matching the smaller of the two input vectors. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3Wide Min(Vector3Wide a, Vector3Wide b) + { + Vector3Wide result; + result.X = Vector.Min(a.X, b.X); + result.Y = Vector.Min(a.Y, b.Y); + result.Z = Vector.Min(a.Z, b.Z); + return result; + } + /// /// Computes the per-component maximum between a scalar value and the components of a vector. /// @@ -133,6 +257,37 @@ public static void Max(in Vector3Wide a, in Vector3Wide b, out Vector3Wide resul result.Z = Vector.Max(a.Z, b.Z); } + /// + /// Computes the per-component maximum between a scalar value and the components of a vector. + /// + /// Scalar to compare to each vector component. + /// Vector whose components will be compared. + /// Vector with components matching the larger of the scalar value and the input vector. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3Wide Max(Vector s, Vector3Wide v) + { + Vector3Wide result; + result.X = Vector.Max(s, v.X); + result.Y = Vector.Max(s, v.Y); + result.Z = Vector.Max(s, v.Z); + return result; + } + /// + /// Computes the per-component maximum of two vectors. + /// + /// First vector whose components will be compared. + /// Second vector whose components will be compared. + /// Vector with components matching the larger of the two input vectors. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3Wide Max(Vector3Wide a, Vector3Wide b) + { + Vector3Wide result; + result.X = Vector.Max(a.X, b.X); + result.Y = Vector.Max(a.Y, b.Y); + result.Z = Vector.Max(a.Z, b.Z); + return result; + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Scale(in Vector3Wide vector, in Vector scalar, out Vector3Wide result) @@ -142,6 +297,25 @@ public static void Scale(in Vector3Wide vector, in Vector scalar, out Vec result.Z = vector.Z * scalar; } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3Wide operator *(Vector3Wide vector, Vector scalar) + { + Vector3Wide result; + result.X = vector.X * scalar; + result.Y = vector.Y * scalar; + result.Z = vector.Z * scalar; + return result; + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3Wide operator *(Vector scalar, Vector3Wide vector) + { + Vector3Wide result; + result.X = vector.X * scalar; + result.Y = vector.Y * scalar; + result.Z = vector.Z * scalar; + return result; + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Abs(in Vector3Wide vector, out Vector3Wide result) { @@ -149,6 +323,15 @@ public static void Abs(in Vector3Wide vector, out Vector3Wide result) result.Y = Vector.Abs(vector.Y); result.Z = Vector.Abs(vector.Z); } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3Wide Abs(Vector3Wide vector) + { + Vector3Wide result; + result.X = Vector.Abs(vector.X); + result.Y = Vector.Abs(vector.Y); + result.Z = Vector.Abs(vector.Z); + return result; + } [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Negate(in Vector3Wide v, out Vector3Wide result) @@ -167,6 +350,17 @@ public static ref Vector3Wide Negate(ref Vector3Wide v) return ref v; } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3Wide operator -(Vector3Wide v) + { + Vector3Wide result; + result.X = -v.X; + result.Y = -v.Y; + result.Z = -v.Z; + return result; + } + + //TODO: Look into codegen for conditional select. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ConditionallyNegate(in Vector shouldNegate, ref Vector3Wide v) { @@ -183,6 +377,16 @@ public static void ConditionallyNegate(in Vector shouldNegate, in Vector3Wi negated.Z = Vector.ConditionalSelect(shouldNegate, -v.Z, v.Z); } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3Wide ConditionallyNegate(Vector shouldNegate, Vector3Wide v) + { + Vector3Wide negated; + negated.X = Vector.ConditionalSelect(shouldNegate, -v.X, v.X); + negated.Y = Vector.ConditionalSelect(shouldNegate, -v.Y, v.Y); + negated.Z = Vector.ConditionalSelect(shouldNegate, -v.Z, v.Z); + return negated; + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void CrossWithoutOverlap(in Vector3Wide a, in Vector3Wide b, out Vector3Wide result) { @@ -198,6 +402,16 @@ public static void Cross(in Vector3Wide a, in Vector3Wide b, out Vector3Wide res result = temp; } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3Wide Cross(Vector3Wide a, Vector3Wide b) + { + Vector3Wide result; + result.X = a.Y * b.Z - a.Z * b.Y; + result.Y = a.Z * b.X - a.X * b.Z; + result.Z = a.X * b.Y - a.Y * b.X; + return result; + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void LengthSquared(in Vector3Wide v, out Vector lengthSquared) @@ -209,6 +423,26 @@ public static void Length(in Vector3Wide v, out Vector length) { length = Vector.SquareRoot(v.X * v.X + v.Y * v.Y + v.Z * v.Z); } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector LengthSquared(Vector3Wide v) + { + return v.X * v.X + v.Y * v.Y + v.Z * v.Z; + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector Length(Vector3Wide v) + { + return Vector.SquareRoot(v.X * v.X + v.Y * v.Y + v.Z * v.Z); + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public Vector LengthSquared() + { + return X * X + Y * Y + Z * Z; + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public Vector Length() + { + return Vector.SquareRoot(X * X + Y * Y + Z * Z); + } [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Distance(in Vector3Wide a, in Vector3Wide b, out Vector distance) @@ -228,6 +462,25 @@ public static void DistanceSquared(in Vector3Wide a, in Vector3Wide b, out Vecto distanceSquared = x * x + y * y + z * z; } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector Distance(Vector3Wide a, Vector3Wide b) + { + var x = b.X - a.X; + var y = b.Y - a.Y; + var z = b.Z - a.Z; + return Vector.SquareRoot(x * x + y * y + z * z); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector DistanceSquared(Vector3Wide a, Vector3Wide b) + { + var x = b.X - a.X; + var y = b.Y - a.Y; + var z = b.Z - a.Z; + return x * x + y * y + z * z; + } + + //TODO: We have better intrinsics options here for a fast rsqrt path. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Normalize(in Vector3Wide v, out Vector3Wide result) { @@ -236,6 +489,14 @@ public static void Normalize(in Vector3Wide v, out Vector3Wide result) Scale(v, scale, out result); } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3Wide Normalize(Vector3Wide v) + { + Length(v, out var length); + var scale = Vector.One / length; + return v * scale; + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ConditionalSelect(in Vector condition, in Vector3Wide left, in Vector3Wide right, out Vector3Wide result) { @@ -244,6 +505,16 @@ public static void ConditionalSelect(in Vector condition, in Vector3Wide le result.Z = Vector.ConditionalSelect(condition, left.Z, right.Z); } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3Wide ConditionalSelect(Vector condition, Vector3Wide left, Vector3Wide right) + { + Vector3Wide result; + result.X = Vector.ConditionalSelect(condition, left.X, right.X); + result.Y = Vector.ConditionalSelect(condition, left.Y, right.Y); + result.Z = Vector.ConditionalSelect(condition, left.Z, right.Z); + return result; + } + /// /// Multiplies the components of one vector with another. /// @@ -258,6 +529,23 @@ public static void Multiply(in Vector3Wide a, in Vector3Wide b, out Vector3Wide result.Z = a.Z * b.Z; } + /// + /// Multiplies the components of one vector with another. + /// + /// First vector to multiply. + /// Second vector to multiply. + /// Result of the multiplication. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3Wide operator *(Vector3Wide a, Vector3Wide b) + { + Vector3Wide result; + result.X = a.X * b.X; + result.Y = a.Y * b.Y; + result.Z = a.Z * b.Z; + return result; + } + + //TODO: most of these gatherscattery functions are fallbacks in AOS->SOA conversions and should often be replaced by explicit vectorized transposes. /// /// Pulls one lane out of the wide representation. /// @@ -307,7 +595,7 @@ public static void WriteFirst(in Vector3 source, ref Vector3Wide targetSlot) [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void WriteSlot(in Vector3 source, int slotIndex, ref Vector3Wide target) { - WriteFirst(source, ref GatherScatter.GetOffsetInstance(ref target, slotIndex)); + WriteFirst(source, ref GatherScatter.GetOffsetInstance(ref target, slotIndex)); } /// @@ -323,6 +611,21 @@ public static void Broadcast(in Vector3 source, out Vector3Wide broadcasted) broadcasted.Z = new Vector(source.Z); } + /// + /// Expands each scalar value to every slot of the bundle. + /// + /// Source value to write to every bundle slot. + /// Bundle containing the source's components in every slot. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3Wide Broadcast(Vector3 source) + { + Vector3Wide broadcasted; + broadcasted.X = new Vector(source.X); + broadcasted.Y = new Vector(source.Y); + broadcasted.Z = new Vector(source.Z); + return broadcasted; + } + /// /// Takes a slot from the source vector and broadcasts it into all slots of the target vector. /// @@ -337,6 +640,23 @@ public static void Rebroadcast(in Vector3Wide source, int slotIndex, out Vector3 broadcasted.Z = new Vector(source.Z[slotIndex]); } + + /// + /// Takes a slot from the source vector and broadcasts it into all slots of the target vector. + /// + /// Vector to pull values from. + /// Slot in the source vectors to pull values from. + /// Target vector to be filled with the selected data. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3Wide Rebroadcast(Vector3Wide source, int slotIndex) + { + Vector3Wide broadcasted; + broadcasted.X = new Vector(source.X[slotIndex]); + broadcasted.Y = new Vector(source.Y[slotIndex]); + broadcasted.Z = new Vector(source.Z[slotIndex]); + return broadcasted; + } + /// /// Takes a slot from the source vector and places it into a slot of the target. /// diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index 1e8e4391b..fdf5fce3f 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -699,11 +699,28 @@ internal unsafe static void CreateDeformable(Simulation simulation, in Vector3 p edges.Dispose(pool); } - - + [MethodImpl(MethodImplOptions.NoInlining)] + void Test() + { + Unsafe.SkipInit(out Vector3Wide a); + Unsafe.SkipInit(out Vector3Wide b); + Unsafe.SkipInit(out Vector3Wide c); + Unsafe.SkipInit(out Vector d); + Unsafe.SkipInit(out Vector e); + + var r = Vector3Wide.Min(Vector3Wide.Max(Vector3Wide.Dot(a - b, b - c), a), b); + var p = Vector3Wide.Cross((d * r) * d, -Vector3Wide.Abs(a)); + var n1 = Vector3Wide.Length(Vector3Wide.ConditionallyNegate(e, p)); + var n2 = (Vector3Wide.ConditionallyNegate(e, b)).Length(); + var uh = n1 + n2; + uh += Vector3Wide.Dot(a * b, Vector3Wide.Broadcast(Vector3.Zero)); + + Console.WriteLine($"ree: {uh}"); + } public unsafe override void Initialize(ContentArchive content, Camera camera) { + Test(); Console.WriteLine($"aasgh: {Unsafe.SizeOf()}"); var stateTest = new MotionState(); Console.WriteLine($"offset: {(byte*)&stateTest.Velocity - (byte*)&stateTest.Pose}"); From 7edd2eb99ee9a496109326e718b9a6f53e2ecc39 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 2 Jul 2021 13:09:40 -0500 Subject: [PATCH 061/947] Quaternion wide operatorization. --- BepuPhysics/Collidables/Capsule.cs | 2 +- BepuPhysics/Collidables/Cylinder.cs | 2 +- .../CollisionTasks/CapsuleBoxTester.cs | 2 +- .../CollisionTasks/CapsulePairTester.cs | 2 +- .../CollisionTasks/CapsuleTriangleTester.cs | 2 +- .../SweepTasks/CapsuleBoxDistanceTester.cs | 2 +- .../CapsuleCylinderDistanceTester.cs | 2 +- .../SweepTasks/CapsulePairDistanceTester.cs | 4 +- BepuPhysics/PoseIntegrator.cs | 2 +- BepuUtilities/QuaternionWide.cs | 169 +++++++++++++++++- 10 files changed, 170 insertions(+), 19 deletions(-) diff --git a/BepuPhysics/Collidables/Capsule.cs b/BepuPhysics/Collidables/Capsule.cs index c6efb2d35..1eef29cde 100644 --- a/BepuPhysics/Collidables/Capsule.cs +++ b/BepuPhysics/Collidables/Capsule.cs @@ -221,7 +221,7 @@ public void WriteSlot(int index, in Capsule source) [MethodImpl(MethodImplOptions.AggressiveInlining)] public void GetBounds(ref QuaternionWide orientations, int countInBundle, out Vector maximumRadius, out Vector maximumAngularExpansion, out Vector3Wide min, out Vector3Wide max) { - QuaternionWide.TransformUnitY(orientations, out var segmentOffset); + var segmentOffset = QuaternionWide.TransformUnitY(orientations); Vector3Wide.Scale(segmentOffset, HalfLength, out segmentOffset); Vector3Wide.Abs(segmentOffset, out segmentOffset); diff --git a/BepuPhysics/Collidables/Cylinder.cs b/BepuPhysics/Collidables/Cylinder.cs index d94b189c1..dbda4024f 100644 --- a/BepuPhysics/Collidables/Cylinder.cs +++ b/BepuPhysics/Collidables/Cylinder.cs @@ -221,7 +221,7 @@ public void WriteSlot(int index, in Cylinder source) [MethodImpl(MethodImplOptions.AggressiveInlining)] public void GetBounds(ref QuaternionWide orientations, int countInBundle, out Vector maximumRadius, out Vector maximumAngularExpansion, out Vector3Wide min, out Vector3Wide max) { - QuaternionWide.TransformUnitY(orientations, out var y); + var y = QuaternionWide.TransformUnitY(orientations); Vector3Wide.Multiply(y, y, out var yy); Vector3Wide.Subtract(Vector.One, yy, out var squared); max.X = Vector.Abs(HalfLength * y.X) + Vector.SquareRoot(Vector.Max(Vector.Zero, squared.X)) * Radius; diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleBoxTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleBoxTester.cs index b860d480c..eff127193 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleBoxTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleBoxTester.cs @@ -19,7 +19,7 @@ internal static void Prepare( QuaternionWide.TransformWithoutOverlap(offsetB, toLocalB, out localOffsetA); Vector3Wide.Negate(ref localOffsetA); QuaternionWide.ConcatenateWithoutOverlap(orientationA, toLocalB, out var boxLocalOrientationA); - QuaternionWide.TransformUnitY(boxLocalOrientationA, out capsuleAxis); + capsuleAxis = QuaternionWide.TransformUnitY(boxLocalOrientationA); //Get the closest point on the capsule segment to the box center to choose which edge to use. //(Pointless to test the other 9; they're guaranteed to be further away.) diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CapsulePairTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CapsulePairTester.cs index 8cb066fe4..f35f15264 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CapsulePairTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CapsulePairTester.cs @@ -21,7 +21,7 @@ public void Test( //Taking the derivative with respect to ta and doing some algebra (taking into account ||da|| == ||db|| == 1) to solve for ta yields: //ta = (da * (b - a) + (db * (a - b)) * (da * db)) / (1 - ((da * db) * (da * db)) QuaternionWide.TransformUnitXY(orientationA, out var xa, out var da); - QuaternionWide.TransformUnitY(orientationB, out var db); + var db = QuaternionWide.TransformUnitY(orientationB); Vector3Wide.Dot(da, offsetB, out var daOffsetB); Vector3Wide.Dot(db, offsetB, out var dbOffsetB); Vector3Wide.Dot(da, db, out var dadb); diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleTriangleTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleTriangleTester.cs index 8d31facdd..3e1d139fb 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleTriangleTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleTriangleTester.cs @@ -127,7 +127,7 @@ public void Test( Vector3Wide.Subtract(b.B, localTriangleCenter, out triangle.B); Vector3Wide.Subtract(b.C, localTriangleCenter, out triangle.C); - QuaternionWide.TransformUnitY(orientationA, out var worldCapsuleAxis); + var worldCapsuleAxis = QuaternionWide.TransformUnitY(orientationA); Matrix3x3Wide.TransformByTransposedWithoutOverlap(worldCapsuleAxis, rB, out var localCapsuleAxis); //There are four sources of separating axis for deep contact, where the capsule axis intersects the triangle: diff --git a/BepuPhysics/CollisionDetection/SweepTasks/CapsuleBoxDistanceTester.cs b/BepuPhysics/CollisionDetection/SweepTasks/CapsuleBoxDistanceTester.cs index 85eaf85bd..f7f3db4d0 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/CapsuleBoxDistanceTester.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/CapsuleBoxDistanceTester.cs @@ -187,7 +187,7 @@ public void Test(in CapsuleWide a, in BoxWide b, in Vector3Wide offsetB, in Quat { //Bring the capsule into the box's local space. Matrix3x3Wide.CreateFromQuaternion(orientationB, out var rB); - QuaternionWide.TransformUnitY(orientationA, out var capsuleAxis); + var capsuleAxis = QuaternionWide.TransformUnitY(orientationA); Matrix3x3Wide.TransformByTransposedWithoutOverlap(capsuleAxis, rB, out var localCapsuleAxis); Matrix3x3Wide.TransformByTransposedWithoutOverlap(offsetB, rB, out var localOffsetB); Vector3Wide.Negate(localOffsetB, out var localOffsetA); diff --git a/BepuPhysics/CollisionDetection/SweepTasks/CapsuleCylinderDistanceTester.cs b/BepuPhysics/CollisionDetection/SweepTasks/CapsuleCylinderDistanceTester.cs index 9b4b69ae0..c092c51f8 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/CapsuleCylinderDistanceTester.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/CapsuleCylinderDistanceTester.cs @@ -14,7 +14,7 @@ public void Test(in CapsuleWide a, in CylinderWide b, in Vector3Wide offsetB, in { QuaternionWide.Conjugate(orientationB, out var inverseOrientationB); QuaternionWide.ConcatenateWithoutOverlap(orientationA, inverseOrientationB, out var localOrientationA); - QuaternionWide.TransformUnitY(localOrientationA, out var capsuleAxis); + var capsuleAxis = QuaternionWide.TransformUnitY(localOrientationA); QuaternionWide.TransformWithoutOverlap(offsetB, inverseOrientationB, out var localOffsetB); Vector3Wide.Negate(localOffsetB, out var localOffsetA); diff --git a/BepuPhysics/CollisionDetection/SweepTasks/CapsulePairDistanceTester.cs b/BepuPhysics/CollisionDetection/SweepTasks/CapsulePairDistanceTester.cs index e5f5540fe..495404da3 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/CapsulePairDistanceTester.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/CapsulePairDistanceTester.cs @@ -13,8 +13,8 @@ public void Test(in CapsuleWide a, in CapsuleWide b, in Vector3Wide offsetB, in //We want to minimize distance = ||(a + da * ta) - (b + db * tb)||. //Taking the derivative with respect to ta and doing some algebra (taking into account ||da|| == ||db|| == 1) to solve for ta yields: //ta = (da * (b - a) + (db * (a - b)) * (da * db)) / (1 - ((da * db) * (da * db)) - QuaternionWide.TransformUnitXY(orientationA, out var xa, out var da); - QuaternionWide.TransformUnitY(orientationB, out var db); + var da = QuaternionWide.TransformUnitY(orientationA); + var db = QuaternionWide.TransformUnitY(orientationB); Vector3Wide.Dot(da, offsetB, out var daOffsetB); Vector3Wide.Dot(db, offsetB, out var dbOffsetB); Vector3Wide.Dot(da, db, out var dadb); diff --git a/BepuPhysics/PoseIntegrator.cs b/BepuPhysics/PoseIntegrator.cs index 26e0d6252..f731542ae 100644 --- a/BepuPhysics/PoseIntegrator.cs +++ b/BepuPhysics/PoseIntegrator.cs @@ -135,7 +135,7 @@ public static void Integrate(in QuaternionWide start, in Vector3Wide angularVelo q.Z = angularVelocity.Z * scale; MathHelper.Cos(halfAngle, out q.W); QuaternionWide.ConcatenateWithoutOverlap(start, q, out var concatenated); - QuaternionWide.Normalize(concatenated, out integrated); + integrated = QuaternionWide.Normalize(concatenated); var speedValid = Vector.GreaterThan(speed, new Vector(1e-15f)); integrated.X = Vector.ConditionalSelect(speedValid, integrated.X, start.X); integrated.Y = Vector.ConditionalSelect(speedValid, integrated.Y, start.Y); diff --git a/BepuUtilities/QuaternionWide.cs b/BepuUtilities/QuaternionWide.cs index 58e901ae5..5f1843009 100644 --- a/BepuUtilities/QuaternionWide.cs +++ b/BepuUtilities/QuaternionWide.cs @@ -28,7 +28,7 @@ public static void Broadcast(in Quaternion source, out QuaternionWide broadcaste /// Target quaternion to be filled with the selected data. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Rebroadcast(in QuaternionWide source, int slotIndex, out QuaternionWide broadcasted) - { + { broadcasted.X = new Vector(source.X[slotIndex]); broadcasted.Y = new Vector(source.Y[slotIndex]); broadcasted.Z = new Vector(source.Z[slotIndex]); @@ -120,13 +120,16 @@ public static void GetLength(in QuaternionWide q, out Vector length) length = Vector.SquareRoot(q.X * q.X + q.Y * q.Y + q.Z * q.Z + q.W * q.W); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Normalize(in QuaternionWide q, out QuaternionWide normalized) + public static QuaternionWide Normalize(QuaternionWide q) { + //TODO: fast path is possible with intrinsics. var inverseNorm = Vector.One / Vector.SquareRoot(q.X * q.X + q.Y * q.Y + q.Z * q.Z + q.W * q.W); + QuaternionWide normalized; normalized.X = q.X * inverseNorm; normalized.Y = q.Y * inverseNorm; normalized.Z = q.Z * inverseNorm; normalized.W = q.W * inverseNorm; + return normalized; } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -138,6 +141,17 @@ public static void Negate(in QuaternionWide q, out QuaternionWide negated) negated.W = -q.W; } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static QuaternionWide operator-(QuaternionWide q) + { + QuaternionWide negated; + negated.X = -q.X; + negated.Y = -q.Y; + negated.Z = -q.Z; + negated.W = -q.W; + return negated; + } + /// /// Computes the quaternion rotation between two normalized vectors. /// @@ -167,7 +181,39 @@ public static void GetQuaternionBetweenNormalizedVectors(in Vector3Wide v1, in V q.Z = Vector.ConditionalSelect(useNormalCase, cross.Z, Vector.ConditionalSelect(xIsSmallest, v1.Y, Vector.ConditionalSelect(yIsSmaller, v1.X, Vector.Zero))); q.W = Vector.ConditionalSelect(useNormalCase, dot + Vector.One, Vector.Zero); - Normalize(q, out q); + q = Normalize(q); + } + + /// + /// Computes the quaternion rotation between two normalized vectors. + /// + /// First unit-length vector. + /// Second unit-length vector. + /// Quaternion representing the rotation from v1 to v2. + public static QuaternionWide GetQuaternionBetweenNormalizedVectors(Vector3Wide v1, Vector3Wide v2) + { + Vector3Wide.Dot(v1, v2, out var dot); + //For non-normal vectors, the multiplying the axes length squared would be necessary: + //float w = dot + Sqrt(v1.LengthSquared() * v2.LengthSquared()); + + + //There exists an ambiguity at dot == -1. If the directions point away from each other, there are an infinite number of shortest paths. + //One must be chosen arbitrarily. Here, we choose one by projecting onto the plane whose normal is associated with the smallest magnitude. + //Since this is a SIMD operation, the special case is always executed and its result is conditionally selected. + + var cross = Vector3Wide.Cross(v1, v2); + var useNormalCase = Vector.GreaterThan(dot, new Vector(-0.999999f)); + var absX = Vector.Abs(v1.X); + var absY = Vector.Abs(v1.Y); + var absZ = Vector.Abs(v1.Z); + var xIsSmallest = Vector.BitwiseAnd(Vector.LessThan(absX, absY), Vector.LessThan(absX, absZ)); + var yIsSmaller = Vector.LessThan(absY, absZ); + QuaternionWide q; + q.X = Vector.ConditionalSelect(useNormalCase, cross.X, Vector.ConditionalSelect(xIsSmallest, Vector.Zero, Vector.ConditionalSelect(yIsSmaller, -v1.Z, -v1.Y))); + q.Y = Vector.ConditionalSelect(useNormalCase, cross.Y, Vector.ConditionalSelect(xIsSmallest, -v1.Z, Vector.ConditionalSelect(yIsSmaller, Vector.Zero, v1.X))); + q.Z = Vector.ConditionalSelect(useNormalCase, cross.Z, Vector.ConditionalSelect(xIsSmallest, v1.Y, Vector.ConditionalSelect(yIsSmaller, v1.X, Vector.Zero))); + q.W = Vector.ConditionalSelect(useNormalCase, dot + Vector.One, Vector.Zero); + return Normalize(q); } /// @@ -239,13 +285,78 @@ public static void Transform(in Vector3Wide v, in QuaternionWide rotation, out V result = temp; } + /// + /// Transforms the vector using a quaternion. + /// + /// Vector to transform. + /// Rotation to apply to the vector. + /// Transformed vector. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3Wide Transform(Vector3Wide v, QuaternionWide rotation) + { + //This operation is an optimized-down version of v' = q * v * q^-1. + //The expanded form would be to treat v as an 'axis only' quaternion + //and perform standard quaternion multiplication. Assuming q is normalized, + //q^-1 can be replaced by a conjugation. + var x2 = rotation.X + rotation.X; + var y2 = rotation.Y + rotation.Y; + var z2 = rotation.Z + rotation.Z; + var xx2 = rotation.X * x2; + var xy2 = rotation.X * y2; + var xz2 = rotation.X * z2; + var yy2 = rotation.Y * y2; + var yz2 = rotation.Y * z2; + var zz2 = rotation.Z * z2; + var wx2 = rotation.W * x2; + var wy2 = rotation.W * y2; + var wz2 = rotation.W * z2; + Vector3Wide result; + result.X = v.X * (Vector.One - yy2 - zz2) + v.Y * (xy2 - wz2) + v.Z * (xz2 + wy2); + result.Y = v.X * (xy2 + wz2) + v.Y * (Vector.One - xx2 - zz2) + v.Z * (yz2 - wx2); + result.Z = v.X * (xz2 - wy2) + v.Y * (yz2 + wx2) + v.Z * (Vector.One - xx2 - yy2); + return result; + } + + /// + /// Transforms the vector using a quaternion. + /// + /// Vector to transform. + /// Rotation to apply to the vector. + /// Transformed vector. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3Wide TransformByConjugate(Vector3Wide v, QuaternionWide rotation) + { + //This operation is an optimized-down version of v' = q * v * q^-1. + //The expanded form would be to treat v as an 'axis only' quaternion + //and perform standard quaternion multiplication. Assuming q is normalized, + //q^-1 can be replaced by a conjugation. + var x2 = rotation.X + rotation.X; + var y2 = rotation.Y + rotation.Y; + var z2 = rotation.Z + rotation.Z; + var xx2 = rotation.X * x2; + var xy2 = rotation.X * y2; + var xz2 = rotation.X * z2; + var yy2 = rotation.Y * y2; + var yz2 = rotation.Y * z2; + var zz2 = rotation.Z * z2; + var nW = -rotation.W; + var wx2 = nW * x2; + var wy2 = nW * y2; + var wz2 = nW * z2; + Vector3Wide result; + result.X = v.X * (Vector.One - yy2 - zz2) + v.Y * (xy2 - wz2) + v.Z * (xz2 + wy2); + result.Y = v.X * (xy2 + wz2) + v.Y * (Vector.One - xx2 - zz2) + v.Z * (yz2 - wx2); + result.Z = v.X * (xz2 - wy2) + v.Y * (yz2 + wx2) + v.Z * (Vector.One - xx2 - yy2); + return result; + } + /// /// Transforms the unit X direction using a quaternion. /// /// Rotation to apply to the vector. - /// Transformed vector. + /// Transformed vector. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void TransformUnitX(in QuaternionWide rotation, out Vector3Wide result) + public static Vector3Wide TransformUnitX(QuaternionWide rotation) { var y2 = rotation.Y + rotation.Y; var z2 = rotation.Z + rotation.Z; @@ -255,18 +366,20 @@ public static void TransformUnitX(in QuaternionWide rotation, out Vector3Wide re var zz2 = rotation.Z * z2; var wy2 = rotation.W * y2; var wz2 = rotation.W * z2; + Vector3Wide result; result.X = Vector.One - yy2 - zz2; result.Y = xy2 + wz2; result.Z = xz2 - wy2; + return result; } /// /// Transforms the unit Y vector using a quaternion. /// /// Rotation to apply to the vector. - /// Transformed vector. + /// Transformed vector. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void TransformUnitY(in QuaternionWide rotation, out Vector3Wide result) + public static Vector3Wide TransformUnitY(QuaternionWide rotation) { var x2 = rotation.X + rotation.X; var y2 = rotation.Y + rotation.Y; @@ -277,18 +390,20 @@ public static void TransformUnitY(in QuaternionWide rotation, out Vector3Wide re var zz2 = rotation.Z * z2; var wx2 = rotation.W * x2; var wz2 = rotation.W * z2; + Vector3Wide result; result.X = xy2 - wz2; result.Y = Vector.One - xx2 - zz2; result.Z = yz2 + wx2; + return result; } /// /// Transforms the unit Z vector using a quaternion. /// /// Rotation to apply to the vector. - /// Transformed vector. + /// Transformed vector. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void TransformUnitZ(in QuaternionWide rotation, out Vector3Wide result) + public static Vector3Wide TransformUnitZ(QuaternionWide rotation) { var x2 = rotation.X + rotation.X; var y2 = rotation.Y + rotation.Y; @@ -299,9 +414,11 @@ public static void TransformUnitZ(in QuaternionWide rotation, out Vector3Wide re var yz2 = rotation.Y * z2; var wx2 = rotation.W * x2; var wy2 = rotation.W * y2; + Vector3Wide result; result.X = xz2 + wy2; result.Y = yz2 - wx2; result.Z = Vector.One - xx2 - yy2; + return result; } /// @@ -395,6 +512,24 @@ public static void Concatenate(in QuaternionWide a, in QuaternionWide b, out Qua result = tempResult; } + /// + /// Concatenates the transforms of two quaternions together such that the resulting quaternion, applied as an orientation to a vector v, is equivalent to + /// transformed = (v * a) * b. + /// + /// First quaternion to concatenate. + /// Second quaternion to concatenate. + /// Product of the concatenation. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static QuaternionWide operator*(QuaternionWide a, QuaternionWide b) + { + QuaternionWide result; + result.X = a.W * b.X + a.X * b.W + a.Z * b.Y - a.Y * b.Z; + result.Y = a.W * b.Y + a.Y * b.W + a.X * b.Z - a.Z * b.X; + result.Z = a.W * b.Z + a.Z * b.W + a.Y * b.X - a.X * b.Y; + result.W = a.W * b.W - a.X * b.X - a.Y * b.Y - a.Z * b.Z; + return result; + } + /// /// Computes the conjugate of the quaternion. /// @@ -409,6 +544,22 @@ public static void Conjugate(in QuaternionWide quaternion, out QuaternionWide re result.W = -quaternion.W; } + /// + /// Computes the conjugate of the quaternion. + /// + /// Quaternion to conjugate. + /// Conjugated quaternion. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static QuaternionWide Conjugate(in QuaternionWide quaternion) + { + QuaternionWide result; + result.X = quaternion.X; + result.Y = quaternion.Y; + result.Z = quaternion.Z; + result.W = -quaternion.W; + return result; + } + /// /// Gathers values from the first slot of a wide quaternion and puts them into a narrow representation. /// From deb9da2691aa90e7fd40a562fe5c5e63c75e0aea Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 2 Jul 2021 13:18:31 -0500 Subject: [PATCH 062/947] Few more missing bits on quaternionwide. --- BepuUtilities/QuaternionWide.cs | 20 ++++++++++++++------ Demos/Demos/NewtDemo.cs | 6 ++++++ 2 files changed, 20 insertions(+), 6 deletions(-) diff --git a/BepuUtilities/QuaternionWide.cs b/BepuUtilities/QuaternionWide.cs index 5f1843009..6c5867efc 100644 --- a/BepuUtilities/QuaternionWide.cs +++ b/BepuUtilities/QuaternionWide.cs @@ -110,15 +110,17 @@ public static void Scale(in QuaternionWide q, in Vector scale, out Quater } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void GetLengthSquared(in QuaternionWide q, out Vector lengthSquared) + public Vector LengthSquared() { - lengthSquared = q.X * q.X + q.Y * q.Y + q.Z * q.Z + q.W * q.W; + return X * X + Y * Y + Z * Z + W * W; } + [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void GetLength(in QuaternionWide q, out Vector length) + public Vector Length() { - length = Vector.SquareRoot(q.X * q.X + q.Y * q.Y + q.Z * q.Z + q.W * q.W); + return Vector.SquareRoot(X * X + Y * Y + Z * Z + W * W); } + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static QuaternionWide Normalize(QuaternionWide q) { @@ -142,7 +144,7 @@ public static void Negate(in QuaternionWide q, out QuaternionWide negated) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static QuaternionWide operator-(QuaternionWide q) + public static QuaternionWide operator -(QuaternionWide q) { QuaternionWide negated; negated.X = -q.X; @@ -350,6 +352,12 @@ public static Vector3Wide TransformByConjugate(Vector3Wide v, QuaternionWide rot return result; } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3Wide operator *(Vector3Wide v, QuaternionWide rotation) + { + return Transform(v, rotation); + } + /// /// Transforms the unit X direction using a quaternion. /// @@ -520,7 +528,7 @@ public static void Concatenate(in QuaternionWide a, in QuaternionWide b, out Qua /// Second quaternion to concatenate. /// Product of the concatenation. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static QuaternionWide operator*(QuaternionWide a, QuaternionWide b) + public static QuaternionWide operator *(QuaternionWide a, QuaternionWide b) { QuaternionWide result; result.X = a.W * b.X + a.X * b.W + a.Z * b.Y - a.Y * b.Z; diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index fdf5fce3f..c618a5c43 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -715,6 +715,12 @@ void Test() var uh = n1 + n2; uh += Vector3Wide.Dot(a * b, Vector3Wide.Broadcast(Vector3.Zero)); + Unsafe.SkipInit(out QuaternionWide q1); + Unsafe.SkipInit(out QuaternionWide q2); + + var ehe = Vector3Wide.Normalize(a * q1 * q2); + uh += ehe.LengthSquared() * q1.LengthSquared(); + Console.WriteLine($"ree: {uh}"); } From f2235e7337090be4bfc67aa295597702c02bf872 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 2 Jul 2021 15:42:10 -0500 Subject: [PATCH 063/947] More matrix operators. --- BepuPhysics/Constraints/Weld.cs | 5 +- BepuUtilities/Matrix3x3Wide.cs | 11 +++ BepuUtilities/Symmetric3x3Wide.cs | 132 +++++++++++++++++++++++++++++- Demos/Demos/NewtDemo.cs | 15 +++- 4 files changed, 155 insertions(+), 8 deletions(-) diff --git a/BepuPhysics/Constraints/Weld.cs b/BepuPhysics/Constraints/Weld.cs index 6e23b47b8..fc69da583 100644 --- a/BepuPhysics/Constraints/Weld.cs +++ b/BepuPhysics/Constraints/Weld.cs @@ -241,8 +241,7 @@ public void WarmStart2(in QuaternionWide orientationA, in BodyInertias inertiaA, //angularVelocityA = angularVelocityB //linearVelocityA + angularVelocityA x (localOffset * orientationA) = linearVelocityB //Note that the position constraint is similar a ball socket joint, except the anchor point is on top of the center of mass of object B. - QuaternionWide.TransformWithoutOverlap(prestep.LocalOffset, orientationA, out var offset); - ApplyImpulse(inertiaA, inertiaB, offset, accumulatedImpulses.Orientation, accumulatedImpulses.Offset, ref wsvA, ref wsvB); + ApplyImpulse(inertiaA, inertiaB, prestep.LocalOffset * orientationA, accumulatedImpulses.Orientation, accumulatedImpulses.Offset, ref wsvA, ref wsvB); } public void Solve2(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, float dt, float inverseDt, in WeldPrestepData prestep, in WeldAccumulatedImpulses accumulatedImpulses, ref BodyVelocities wsvA, ref BodyVelocities wsvB) @@ -263,7 +262,7 @@ public void Solve2(in QuaternionWide orientationA, in BodyInertias inertiaA, in //J * M^-1 * JT = [ Ia^-1 + Ib^-1, Ia^-1 * transpose(skewSymmetric(localOffset * orientationA)) ] // [ skewSymmetric(localOffset * orientationA) * Ia^-1, Ma^-1 + Mb^-1 + skewSymmetric(localOffset * orientationA) * Ia^-1 * transpose(skewSymmetric(localOffset * orientationA)) ] //where Ia^-1 and Ib^-1 are the inverse inertia tensors for a and b and Ma^-1 and Mb^-1 are the inverse masses of A and B expanded to 3x3 diagonal matrices. - Symmetric3x3Wide.Add(inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, out var jmjtA); + var jmjtA = inertiaA.InverseInertiaTensor + inertiaB.InverseInertiaTensor; QuaternionWide.TransformWithoutOverlap(prestep.LocalOffset, orientationA, out var offset); Matrix3x3Wide.CreateCrossProduct(offset, out var xAB); Symmetric3x3Wide.Multiply(inertiaA.InverseInertiaTensor, xAB, out var jmjtB); diff --git a/BepuUtilities/Matrix3x3Wide.cs b/BepuUtilities/Matrix3x3Wide.cs index a4afd9fca..79f53f5a3 100644 --- a/BepuUtilities/Matrix3x3Wide.cs +++ b/BepuUtilities/Matrix3x3Wide.cs @@ -113,6 +113,17 @@ public static void TransformWithoutOverlap(in Vector3Wide v, in Matrix3x3Wide m, result.Y = v.X * m.X.Y + v.Y * m.Y.Y + v.Z * m.Z.Y; result.Z = v.X * m.X.Z + v.Y * m.Y.Z + v.Z * m.Z.Z; } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3Wide operator *(Vector3Wide v, Matrix3x3Wide m) + { + Vector3Wide result; + result.X = v.X * m.X.X + v.Y * m.Y.X + v.Z * m.Z.X; + result.Y = v.X * m.X.Y + v.Y * m.Y.Y + v.Z * m.Z.Y; + result.Z = v.X * m.X.Z + v.Y * m.Y.Z + v.Z * m.Z.Z; + return result; + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void TransformByTransposedWithoutOverlap(in Vector3Wide v, in Matrix3x3Wide m, out Vector3Wide result) { diff --git a/BepuUtilities/Symmetric3x3Wide.cs b/BepuUtilities/Symmetric3x3Wide.cs index 93f04a9f8..484d0e20b 100644 --- a/BepuUtilities/Symmetric3x3Wide.cs +++ b/BepuUtilities/Symmetric3x3Wide.cs @@ -78,6 +78,25 @@ public static void Add(in Symmetric3x3Wide a, in Symmetric3x3Wide b, out Symmetr result.ZY = a.ZY + b.ZY; result.ZZ = a.ZZ + b.ZZ; } + /// + /// Adds the components of two symmetric matrices together. + /// + /// First matrix to add. + /// Second matrix to add. + /// Sum of the two input matrices. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Symmetric3x3Wide operator +(in Symmetric3x3Wide a, in Symmetric3x3Wide b) //TODO: without in decoration, this had some really peculiar codegen in .net 6 preview 5. + { + Symmetric3x3Wide result; + result.XX = a.XX + b.XX; + result.YX = a.YX + b.YX; + result.YY = a.YY + b.YY; + result.ZX = a.ZX + b.ZX; + result.ZY = a.ZY + b.ZY; + result.ZZ = a.ZZ + b.ZZ; + return result; + } + /// /// Subtracts one symmetric matrix's components from another. @@ -96,6 +115,24 @@ public static void Subtract(in Symmetric3x3Wide a, in Symmetric3x3Wide b, out Sy result.ZZ = a.ZZ - b.ZZ; } + /// + /// Subtracts one symmetric matrix's components from another. + /// + /// Matrix to be subtracted from. + /// Matrix to subtract from the first matrix. + /// Result of a - b. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Symmetric3x3Wide operator -(in Symmetric3x3Wide a, in Symmetric3x3Wide b) //TODO: without in decoration, this had some really peculiar codegen in .net 6 preview 5. + { + Symmetric3x3Wide result; + result.XX = a.XX - b.XX; + result.YX = a.YX - b.YX; + result.YY = a.YY - b.YY; + result.ZX = a.ZX - b.ZX; + result.ZY = a.ZY - b.ZY; + result.ZZ = a.ZZ - b.ZZ; + return result; + } [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Scale(in Symmetric3x3Wide m, in Vector scale, out Symmetric3x3Wide result) @@ -107,7 +144,20 @@ public static void Scale(in Symmetric3x3Wide m, in Vector scale, out Symm result.ZY = m.ZY * scale; result.ZZ = m.ZZ * scale; } - + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Symmetric3x3Wide operator *(in Symmetric3x3Wide m, in Vector scale) //TODO: without in decoration, this had some really peculiar codegen in .net 6 preview 5. + { + Symmetric3x3Wide result; + result.XX = m.XX * scale; + result.YX = m.YX * scale; + result.YY = m.YY * scale; + result.ZX = m.ZX * scale; + result.ZY = m.ZY * scale; + result.ZZ = m.ZZ * scale; + return result; + } + //If you ever need a triangular invert, a couple of options: //For matrices of the form: //[ 1 0 0 ] @@ -220,6 +270,25 @@ public static void MultiplyWithoutOverlap(in Matrix2x3Wide a, in Symmetric3x3Wid result.Y.Z = a.Y.X * b.ZX + a.Y.Y * b.ZY + a.Y.Z * b.ZZ; } + /// + /// Computes result = a * b, assuming that b represents a symmetric 3x3 matrix. Assumes that input parameters and output result do not overlap. + /// + /// First matrix of the pair to multiply. + /// Matrix to be reinterpreted as symmetric for the multiply. + /// Result of multiplying a * b. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix2x3Wide operator *(in Matrix2x3Wide a, in Symmetric3x3Wide b) //TODO: without in decoration, this had some really peculiar codegen in .net 6 preview 5. + { + Matrix2x3Wide result; + result.X.X = a.X.X * b.XX + a.X.Y * b.YX + a.X.Z * b.ZX; + result.X.Y = a.X.X * b.YX + a.X.Y * b.YY + a.X.Z * b.ZY; + result.X.Z = a.X.X * b.ZX + a.X.Y * b.ZY + a.X.Z * b.ZZ; + result.Y.X = a.Y.X * b.XX + a.Y.Y * b.YX + a.Y.Z * b.ZX; + result.Y.Y = a.Y.X * b.YX + a.Y.Y * b.YY + a.Y.Z * b.ZY; + result.Y.Z = a.Y.X * b.ZX + a.Y.Y * b.ZY + a.Y.Z * b.ZZ; + return result; + } + /// /// Computes result = a * b, assuming that b represents a symmetric 3x3 matrix. Assumes that input parameters and output result do not overlap. /// @@ -242,6 +311,31 @@ public static void MultiplyWithoutOverlap(in Matrix3x3Wide a, in Symmetric3x3Wid result.Z.Z = a.Z.X * b.ZX + a.Z.Y * b.ZY + a.Z.Z * b.ZZ; } + + /// + /// Computes result = a * b, assuming that b represents a symmetric 3x3 matrix. Assumes that input parameters and output result do not overlap. + /// + /// First matrix of the pair to multiply. + /// Matrix to be reinterpreted as symmetric for the multiply. + /// Result of multiplying a * b. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix3x3Wide operator *(in Matrix3x3Wide a, in Symmetric3x3Wide b) //TODO: without in decoration, this had some really peculiar codegen in .net 6 preview 5. + { + Matrix3x3Wide result; + result.X.X = a.X.X * b.XX + a.X.Y * b.YX + a.X.Z * b.ZX; + result.X.Y = a.X.X * b.YX + a.X.Y * b.YY + a.X.Z * b.ZY; + result.X.Z = a.X.X * b.ZX + a.X.Y * b.ZY + a.X.Z * b.ZZ; + + result.Y.X = a.Y.X * b.XX + a.Y.Y * b.YX + a.Y.Z * b.ZX; + result.Y.Y = a.Y.X * b.YX + a.Y.Y * b.YY + a.Y.Z * b.ZY; + result.Y.Z = a.Y.X * b.ZX + a.Y.Y * b.ZY + a.Y.Z * b.ZZ; + + result.Z.X = a.Z.X * b.XX + a.Z.Y * b.YX + a.Z.Z * b.ZX; + result.Z.Y = a.Z.X * b.YX + a.Z.Y * b.YY + a.Z.Z * b.ZY; + result.Z.Z = a.Z.X * b.ZX + a.Z.Y * b.ZY + a.Z.Z * b.ZZ; + return result; + } + /// /// Computes result = a * b, assuming that a represents a symmetric 3x3 matrix. Assumes that input parameters and output result do not overlap. /// @@ -264,6 +358,30 @@ public static void Multiply(in Symmetric3x3Wide a, in Matrix3x3Wide b, out Matri result.Z.Z = a.ZX * b.X.Z + a.ZY * b.Y.Z + a.ZZ * b.Z.Z; } + /// + /// Computes result = a * b, assuming that a represents a symmetric 3x3 matrix. Assumes that input parameters and output result do not overlap. + /// + /// Matrix to be reinterpreted as symmetric for the multiply. + /// Second matrix of the pair to multiply. + /// Result of multiplying a * b. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix3x3Wide operator *(in Symmetric3x3Wide a, in Matrix3x3Wide b) //TODO: without in decoration, this had some really peculiar codegen in .net 6 preview 5. + { + Matrix3x3Wide result; + result.X.X = a.XX * b.X.X + a.YX * b.Y.X + a.ZX * b.Z.X; + result.X.Y = a.XX * b.X.Y + a.YX * b.Y.Y + a.ZX * b.Z.Y; + result.X.Z = a.XX * b.X.Z + a.YX * b.Y.Z + a.ZX * b.Z.Z; + + result.Y.X = a.YX * b.X.X + a.YY * b.Y.X + a.ZY * b.Z.X; + result.Y.Y = a.YX * b.X.Y + a.YY * b.Y.Y + a.ZY * b.Z.Y; + result.Y.Z = a.YX * b.X.Z + a.YY * b.Y.Z + a.ZY * b.Z.Z; + + result.Z.X = a.ZX * b.X.X + a.ZY * b.Y.X + a.ZZ * b.Z.X; + result.Z.Y = a.ZX * b.X.Y + a.ZY * b.Y.Y + a.ZZ * b.Z.Y; + result.Z.Z = a.ZX * b.X.Z + a.ZY * b.Y.Z + a.ZZ * b.Z.Z; + return result; + } + /// /// Computes result = a * transpose(b). /// @@ -401,7 +519,7 @@ public static void CompleteMatrixSandwichTranspose(in Matrix3x3Wide a, in Matrix result.ZX = a.X.Z * b.X.X + a.Y.Z * b.Y.X + a.Z.Z * b.Z.X; result.ZY = a.X.Z * b.X.Y + a.Y.Z * b.Y.Y + a.Z.Z * b.Z.Y; result.ZZ = a.X.Z * b.X.Z + a.Y.Z * b.Y.Z + a.Z.Z * b.Z.Z; - } + } [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void TransformWithoutOverlap(in Vector3Wide v, in Symmetric3x3Wide m, out Vector3Wide result) @@ -411,6 +529,16 @@ public static void TransformWithoutOverlap(in Vector3Wide v, in Symmetric3x3Wide result.Z = v.X * m.ZX + v.Y * m.ZY + v.Z * m.ZZ; } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3Wide operator *(in Vector3Wide v, in Symmetric3x3Wide m) //TODO: without in decoration, this had some really peculiar codegen in .net 6 preview 5. + { + Vector3Wide result; + result.X = v.X * m.XX + v.Y * m.YX + v.Z * m.ZX; + result.Y = v.X * m.YX + v.Y * m.YY + v.Z * m.ZY; + result.Z = v.X * m.ZX + v.Y * m.ZY + v.Z * m.ZZ; + return result; + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void WriteFirst(in Symmetric3x3 scalar, ref Symmetric3x3Wide wide) { diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index c618a5c43..a49a08e41 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -707,6 +707,11 @@ void Test() Unsafe.SkipInit(out Vector3Wide c); Unsafe.SkipInit(out Vector d); Unsafe.SkipInit(out Vector e); + Unsafe.SkipInit(out Symmetric3x3Wide s1); + Unsafe.SkipInit(out Matrix3x3Wide s2); + Unsafe.SkipInit(out QuaternionWide q1); + Unsafe.SkipInit(out QuaternionWide q2); + var r = Vector3Wide.Min(Vector3Wide.Max(Vector3Wide.Dot(a - b, b - c), a), b); var p = Vector3Wide.Cross((d * r) * d, -Vector3Wide.Abs(a)); @@ -715,12 +720,16 @@ void Test() var uh = n1 + n2; uh += Vector3Wide.Dot(a * b, Vector3Wide.Broadcast(Vector3.Zero)); - Unsafe.SkipInit(out QuaternionWide q1); - Unsafe.SkipInit(out QuaternionWide q2); - var ehe = Vector3Wide.Normalize(a * q1 * q2); uh += ehe.LengthSquared() * q1.LengthSquared(); + var m0 = s1 * s2; + uh += Vector3Wide.Dot(a * m0, a); + + uh += Vector3Wide.Dot(a * s1, a); + var uh2 = a * s1; + + Console.WriteLine("$uah" + uh2); Console.WriteLine($"ree: {uh}"); } From 05107d751fba0312a6279d159448cc13989f3576 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 2 Jul 2021 16:22:48 -0500 Subject: [PATCH 064/947] Weld solve2 revamped a bit. LDLT solve still required. --- BepuPhysics/Constraints/Weld.cs | 63 +++++++++++++++++++-------------- BepuUtilities/Matrix3x3Wide.cs | 16 +++++++++ 2 files changed, 52 insertions(+), 27 deletions(-) diff --git a/BepuPhysics/Constraints/Weld.cs b/BepuPhysics/Constraints/Weld.cs index fc69da583..77689d26f 100644 --- a/BepuPhysics/Constraints/Weld.cs +++ b/BepuPhysics/Constraints/Weld.cs @@ -5,7 +5,13 @@ using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; +using static BepuUtilities.QuaternionWide; +using static BepuUtilities.Vector3Wide; +using static BepuUtilities.Symmetric3x3Wide; +using static BepuUtilities.Matrix3x3Wide; using static BepuUtilities.GatherScatter; + + namespace BepuPhysics.Constraints { /// @@ -234,17 +240,10 @@ private static void ApplyImpulse(in BodyInertias inertiaA, in BodyInertias inert public void WarmStart2(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, float dt, float inverseDt, in WeldPrestepData prestep, in WeldAccumulatedImpulses accumulatedImpulses, ref BodyVelocities wsvA, ref BodyVelocities wsvB) { - //The weld constraint handles 6 degrees of freedom simultaneously. The constraints are: - //localOrientation * orientationA = orientationB - //positionA + localOffset * orientationA = positionB - //The velocity derivatives: - //angularVelocityA = angularVelocityB - //linearVelocityA + angularVelocityA x (localOffset * orientationA) = linearVelocityB - //Note that the position constraint is similar a ball socket joint, except the anchor point is on top of the center of mass of object B. ApplyImpulse(inertiaA, inertiaB, prestep.LocalOffset * orientationA, accumulatedImpulses.Orientation, accumulatedImpulses.Offset, ref wsvA, ref wsvB); } public void Solve2(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, float dt, float inverseDt, - in WeldPrestepData prestep, in WeldAccumulatedImpulses accumulatedImpulses, ref BodyVelocities wsvA, ref BodyVelocities wsvB) + in WeldPrestepData prestep, ref WeldAccumulatedImpulses accumulatedImpulses, ref BodyVelocities wsvA, ref BodyVelocities wsvB) { //The weld constraint handles 6 degrees of freedom simultaneously. The constraints are: //localOrientation * orientationA = orientationB @@ -258,36 +257,46 @@ public void Solve2(in QuaternionWide orientationA, in BodyInertias inertiaA, in //J = [ 0, I, 0, -I ] // [ I, skewSymmetric(localOffset * orientationA), -I, 0 ] //where I is the 3x3 identity matrix. + + //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. + var offset = prestep.LocalOffset * orientationA; + var positionError = ab - offset; + var targetOrientationB = prestep.LocalOrientation * orientationA; + var rotationError = Conjugate(targetOrientationB) * orientationB; + GetApproximateAxisAngleFromQuaternion(rotationError, out var rotationErrorAxis, out var rotationErrorLength); + + SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); + var offsetBiasVelocity = positionError * positionErrorToVelocity; + var orientationBiasVelocity = rotationErrorAxis * (rotationErrorLength * positionErrorToVelocity); + + //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); + //csi = -accumulatedImpulse * projection.SoftnessImpulseScale - (-biasVelocity + csvaLinear + csvaAngular + csvbLinear + csvbAngular) * effectiveMass; + //csi = (biasVelocity - csvaLinear - csvaAngular - csvbLinear - csvbAngular) * effectiveMass - accumulatedImpulse * projection.SoftnessImpulseScale; + //csv = V * JT + var orientationCSV = orientationBiasVelocity - (wsvA.Angular - wsvB.Angular); + var offsetCSV = offsetBiasVelocity - (wsvA.Linear - wsvB.Linear + Cross(wsvA.Angular, offset)); + //Effective mass = (J * M^-1 * JT)^-1, which is going to be a little tricky because J * M^-1 * JT is a 6x6 matrix: //J * M^-1 * JT = [ Ia^-1 + Ib^-1, Ia^-1 * transpose(skewSymmetric(localOffset * orientationA)) ] // [ skewSymmetric(localOffset * orientationA) * Ia^-1, Ma^-1 + Mb^-1 + skewSymmetric(localOffset * orientationA) * Ia^-1 * transpose(skewSymmetric(localOffset * orientationA)) ] //where Ia^-1 and Ib^-1 are the inverse inertia tensors for a and b and Ma^-1 and Mb^-1 are the inverse masses of A and B expanded to 3x3 diagonal matrices. var jmjtA = inertiaA.InverseInertiaTensor + inertiaB.InverseInertiaTensor; - QuaternionWide.TransformWithoutOverlap(prestep.LocalOffset, orientationA, out var offset); - Matrix3x3Wide.CreateCrossProduct(offset, out var xAB); - Symmetric3x3Wide.Multiply(inertiaA.InverseInertiaTensor, xAB, out var jmjtB); - Symmetric3x3Wide.CompleteMatrixSandwichTranspose(xAB, jmjtB, out var jmjtD); + var xAB = CreateCrossProduct(offset); + var jmjtB = inertiaA.InverseInertiaTensor * xAB; + CompleteMatrixSandwichTranspose(xAB, jmjtB, out var jmjtD); var diagonalAdd = inertiaA.InverseMass + inertiaB.InverseMass; jmjtD.XX += diagonalAdd; jmjtD.YY += diagonalAdd; jmjtD.ZZ += diagonalAdd; - //Note that there is no need to invert that 6x6 chonk. We want to convert a constraint space velocity into a constraint space impulse, csi = csv * effectiveMass. + //Note that there is no need to invert the 6x6 inverse effective mass matrix chonk. We want to convert a constraint space velocity into a constraint space impulse, csi = csv * effectiveMass. //This is equivalent to solving csi * effectiveMass^-1 = csv for csi, and since effectiveMass^-1 is symmetric positive semidefinite, we can use an LDLT decomposition to quickly solve it. + Symmetric6x6Wide.LDLTSolve(orientationCSV, offsetCSV, jmjtA, jmjtB, jmjtD, out var orientationCSI, out var offsetCSI); + orientationCSI = orientationCSI * effectiveMassCFMScale - accumulatedImpulses.Orientation * softnessImpulseScale; + offsetCSI = offsetCSI * effectiveMassCFMScale - accumulatedImpulses.Offset * effectiveMassCFMScale; + accumulatedImpulses.Orientation += orientationCSI; + accumulatedImpulses.Offset += offsetCSI; - SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); - - //Compute the current constraint error for all 6 degrees of freedom. - //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. - Vector3Wide.Subtract(ab, offset, out var positionError); - QuaternionWide.ConcatenateWithoutOverlap(prestep.LocalOrientation, orientationA, out var targetOrientationB); - QuaternionWide.Conjugate(targetOrientationB, out var inverseTarget); - QuaternionWide.ConcatenateWithoutOverlap(inverseTarget, orientationB, out var rotationError); - QuaternionWide.GetApproximateAxisAngleFromQuaternion(rotationError, out var rotationErrorAxis, out var rotationErrorLength); - - Vector3Wide.Scale(positionError, positionErrorToVelocity, out var offsetBiasVelocity); - Vector3Wide.Scale(rotationErrorAxis, rotationErrorLength * positionErrorToVelocity, out var orientationBiasVelocity); - - ApplyImpulse(inertiaA, inertiaB, ab, accumulatedImpulses.Orientation, accumulatedImpulses.Offset, ref wsvA, ref wsvB); + ApplyImpulse(inertiaA, inertiaB, ab, orientationCSI, offsetCSI, ref wsvA, ref wsvB); } diff --git a/BepuUtilities/Matrix3x3Wide.cs b/BepuUtilities/Matrix3x3Wide.cs index 79f53f5a3..ed2f57945 100644 --- a/BepuUtilities/Matrix3x3Wide.cs +++ b/BepuUtilities/Matrix3x3Wide.cs @@ -180,6 +180,22 @@ public static void CreateCrossProduct(in Vector3Wide v, out Matrix3x3Wide skew) skew.Z.Z = Vector.Zero; } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix3x3Wide CreateCrossProduct(in Vector3Wide v) + { + Matrix3x3Wide skew; + skew.X.X = Vector.Zero; + skew.X.Y = -v.Z; + skew.X.Z = v.Y; + skew.Y.X = v.Z; + skew.Y.Y = Vector.Zero; + skew.Y.Z = -v.X; + skew.Z.X = -v.Y; + skew.Z.Y = v.X; + skew.Z.Z = Vector.Zero; + return skew; + } + /// /// Negates the components of a matrix. /// From e99677b15441836e2ecdcfca9a2ef3e277b8ce5f Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 2 Jul 2021 17:23:34 -0500 Subject: [PATCH 065/947] LDLTSolve progressing. --- BepuUtilities/Symmetric6x6Wide.cs | 46 +++++++++++++++++++++++++++++++ 1 file changed, 46 insertions(+) diff --git a/BepuUtilities/Symmetric6x6Wide.cs b/BepuUtilities/Symmetric6x6Wide.cs index 7594892e0..2cc591e87 100644 --- a/BepuUtilities/Symmetric6x6Wide.cs +++ b/BepuUtilities/Symmetric6x6Wide.cs @@ -71,5 +71,51 @@ public static void TransformWithoutOverlap(in Vector3Wide v0, in Vector3Wide v1, result1.Y = v0.X * m.B.X.Y + v0.Y * m.B.Y.Y + v0.Z * m.B.Z.Y + v1.X * m.D.YX + v1.Y * m.D.YY + v1.Z * m.D.ZY; result1.Z = v0.X * m.B.X.Z + v0.Y * m.B.Y.Z + v0.Z * m.B.Z.Z + v1.X * m.D.ZX + v1.Y * m.D.ZY + v1.Z * m.D.ZZ; } + + /// + /// Solves [vLower, vUpper] = [resultLower, resultUpper] * [[a, b], [bT, d]] for [resultLower, resultUpper] using LDLT decomposition. + /// [[a, b], [bT, d]] should be positive semidefinite. + /// + /// First 3 values of the 6 component input vector. + /// Second 3 values of the 6 component input vector. + /// Upper left 3x3 region of the matrix. + /// Upper right 3x3 region of the matrix. Also the lower left 3x3 region of the matrix, transposed. + /// Lower right 3x3 region of the matrix. + /// First 3 values of the result vector. + /// Second 3 values of the result vector. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void LDLTSolve( + in Vector3Wide v0, in Vector3Wide v1, in Symmetric3x3Wide a, in Matrix3x3Wide b, in Symmetric3x3Wide d, out Vector3Wide result0, out Vector3Wide result1) + { + var d1 = a.XX; + var inverseD1 = Vector.One / d1; + var l21 = inverseD1 * a.YX; + var l31 = inverseD1 * a.ZX; + var l41 = inverseD1 * b.X.X; + var l51 = inverseD1 * b.X.Y; + var l61 = inverseD1 * b.X.Z; + var d2 = a.YY - l21 * l21; + var inverseD2 = Vector.One / d2; + var l32 = inverseD2 * (a.ZY - l31 * l21 * d1); + var l42 = inverseD2 * (b.Y.X - l41 * l21 * d1); + var l52 = inverseD2 * (b.Y.Y - l51 * l21 * d1); + var l62 = inverseD2 * (b.Y.Z - l61 * l21 * d1); + var d3 = a.ZZ - l31 * l31 * d1 - l32 * l32 * d2; + var inverseD3 = Vector.One / d3; + var l43 = inverseD3 * (b.Z.X - l41 * l31 * d1 - l42 * l32 * d2); + var l53 = inverseD3 * (b.Z.Y - l51 * l31 * d1 - l52 * l32 * d2); + var l63 = inverseD3 * (b.Z.Y - l61 * l31 * d1 - l62 * l32 * d2); + var d4 = d.XX - l41 * l41 * d1 - l42 * l42 * d2 - l43 * l43 * d3; + var inverseD4 = Vector.One / d4; + var l54 = inverseD4 * (d.YX - l51 * l41 * d1 - l52 * l42 * d2 - l53 * l43 * d3); + var l64 = inverseD4 * (d.ZX - l61 * l41 * d1 - l62 * l42 * d2 - l63 * l43 * d3); + var d5 = d.YY - l51 * l51 * d1 - l52 * l52 * d2 - l53 * l53 * d3 - l54 * l54 * d4; + var inverseD5 = Vector.One / d5; + var l65 = inverseD5 * (d.ZY - l61 * l51 * d1 - l62 * l52 * d2 - l63 * l53 * d3 - l64 * l54 * d4); + var d6 = d.ZZ - l61 * l61 * d1 - l62 * l62 * d2 - l63 * l63 * d3 - l64 * l64 * d4 - l65 * l65 * d5; + var inverseD6 = Vector.One / d6; + + //We now have the components of L and D, so substitute. + } } } From 9974f24785885f555e1f6eef007c24615b123612 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 2 Jul 2021 19:52:41 -0500 Subject: [PATCH 066/947] LDLT solve completed, and... apparently working on first try? that doesn't sound right. --- BepuPhysics/Constraints/TwoBodyTypeProcessor.cs | 14 +++++++++++--- BepuPhysics/Constraints/Weld.cs | 2 ++ BepuUtilities/Symmetric6x6Wide.cs | 14 ++++++++++++++ 3 files changed, 27 insertions(+), 3 deletions(-) diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 88cdfad98..013ebdd26 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -360,7 +360,7 @@ public unsafe override void WarmStart2(ref TypeBatch typeBatch, Bodies bodies, f bodies.GatherState(ref references, count, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB); if (typeof(TConstraintFunctions) == typeof(WeldFunctions)) { - default(WeldFunctions).WarmStart2(orientationA, inertiaA, ab, orientationB, inertiaB, dt, inverseDt, + default(WeldFunctions).WarmStart2(orientationA, inertiaA, ab, orientationB, inertiaB, dt, inverseDt, Unsafe.As(ref prestep), Unsafe.As(ref accumulatedImpulses), ref wsvA, ref wsvB); } else @@ -387,8 +387,16 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f var count = GetCountInBundle(ref typeBatch, i); Prefetch(ref typeBatch, ref bodyReferencesBundles, ref motionStates, i, exclusiveEndBundle); bodies.GatherState(ref references, count, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB); - function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, dt, inverseDt, ref prestep, out var projection); - function.Solve(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); + if (typeof(TConstraintFunctions) == typeof(WeldFunctions)) + { + default(WeldFunctions).Solve2(orientationA, inertiaA, ab, orientationB, inertiaB, dt, inverseDt, + Unsafe.As(ref prestep), ref Unsafe.As(ref accumulatedImpulses), ref wsvA, ref wsvB); + } + else + { + function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, dt, inverseDt, ref prestep, out var projection); + function.Solve(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); + } bodies.ScatterVelocities(ref wsvA, ref wsvB, ref references, count); } } diff --git a/BepuPhysics/Constraints/Weld.cs b/BepuPhysics/Constraints/Weld.cs index 77689d26f..f190259fb 100644 --- a/BepuPhysics/Constraints/Weld.cs +++ b/BepuPhysics/Constraints/Weld.cs @@ -291,6 +291,8 @@ public void Solve2(in QuaternionWide orientationA, in BodyInertias inertiaA, in //Note that there is no need to invert the 6x6 inverse effective mass matrix chonk. We want to convert a constraint space velocity into a constraint space impulse, csi = csv * effectiveMass. //This is equivalent to solving csi * effectiveMass^-1 = csv for csi, and since effectiveMass^-1 is symmetric positive semidefinite, we can use an LDLT decomposition to quickly solve it. Symmetric6x6Wide.LDLTSolve(orientationCSV, offsetCSV, jmjtA, jmjtB, jmjtD, out var orientationCSI, out var offsetCSI); + //Symmetric6x6Wide.Invert(jmjtA, jmjtB, jmjtD, out var testEffectiveMass); + //Symmetric6x6Wide.TransformWithoutOverlap(orientationCSV, offsetCSV, testEffectiveMass, out var testOrientationCSI, out var testOffsetCSI); orientationCSI = orientationCSI * effectiveMassCFMScale - accumulatedImpulses.Orientation * softnessImpulseScale; offsetCSI = offsetCSI * effectiveMassCFMScale - accumulatedImpulses.Offset * effectiveMassCFMScale; accumulatedImpulses.Orientation += orientationCSI; diff --git a/BepuUtilities/Symmetric6x6Wide.cs b/BepuUtilities/Symmetric6x6Wide.cs index 2cc591e87..70baea18a 100644 --- a/BepuUtilities/Symmetric6x6Wide.cs +++ b/BepuUtilities/Symmetric6x6Wide.cs @@ -116,6 +116,20 @@ public static void LDLTSolve( var inverseD6 = Vector.One / d6; //We now have the components of L and D, so substitute. + result0.X = v0.X; + result0.Y = v0.Y - l21 * result0.X; + result0.Z = v0.Z - l31 * result0.X - l32 * result0.Y; + result1.X = v1.X - l41 * result0.X - l42 * result0.Y - l43 * result0.Z; + result1.Y = v1.Y - l51 * result0.X - l52 * result0.Y - l53 * result0.Z - l54 * result1.X; + result1.Z = v1.Z - l61 * result0.X - l62 * result0.Y - l63 * result0.Z - l64 * result1.X - l65 * result1.Y; + + result1.Z = result1.Z * inverseD6; + result1.Y = result1.Y * inverseD5 - l65 * result1.Z; + result1.X = result1.X * inverseD4 - l64 * result1.Z - l54 * result1.Y; + result0.Z = result0.Z * inverseD3 - l63 * result1.Z - l53 * result1.Y - l43 * result1.X; + result0.Y = result0.Y * inverseD2 - l62 * result1.Z - l52 * result1.Y - l42 * result1.X - l32 * result0.Z; + result0.X = result0.X * inverseD1 - l61 * result1.Z - l51 * result1.Y - l41 * result1.X - l31 * result0.Z - l21 * result0.Y; + } } } From ca96c2e91fef4c279d2004c7ed7a0d2de3bdd066 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 2 Jul 2021 21:04:24 -0500 Subject: [PATCH 067/947] Fiddling and tuning codegen. --- BepuPhysics/Constraints/Weld.cs | 26 ++++++++++---------------- BepuUtilities/Matrix3x3Wide.cs | 2 +- BepuUtilities/QuaternionWide.cs | 2 +- 3 files changed, 12 insertions(+), 18 deletions(-) diff --git a/BepuPhysics/Constraints/Weld.cs b/BepuPhysics/Constraints/Weld.cs index f190259fb..5b9374f75 100644 --- a/BepuPhysics/Constraints/Weld.cs +++ b/BepuPhysics/Constraints/Weld.cs @@ -187,7 +187,7 @@ public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, re Vector3Wide.Subtract(velocityA.Angular, velocityB.Angular, out var orientationCSV); Vector3Wide.Subtract(velocityA.Linear, velocityB.Linear, out var offsetCSV); - Vector3Wide.CrossWithoutOverlap(velocityA.Angular, projection.Offset, out var offsetAngularCSV); + Vector3Wide.CrossWithoutOverlap(velocityA.Angular, projection.Offset, out var offsetAngularCSV); Vector3Wide.Add(offsetCSV, offsetAngularCSV, out offsetCSV); //Note subtraction: this is computing biasVelocity - csv, and later we'll compute (biasVelocity-csv) - softness. @@ -206,7 +206,7 @@ public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, re } - [MethodImpl(MethodImplOptions.AggressiveInlining)] + //[MethodImpl(MethodImplOptions.AggressiveInlining)] private static void ApplyImpulse(in BodyInertias inertiaA, in BodyInertias inertiaB, in Vector3Wide offset, in Vector3Wide orientationCSI, in Vector3Wide offsetCSI, ref BodyVelocities velocityA, ref BodyVelocities velocityB) { //Recall the jacobians: @@ -218,22 +218,15 @@ private static void ApplyImpulse(in BodyInertias inertiaA, in BodyInertias inert //angularImpulseA = orientationCSI + worldOffset x offsetCSI //linearImpulseB = -offsetCSI //angularImpulseB = -orientationCSI - Vector3Wide.Scale(offsetCSI, inertiaA.InverseMass, out var linearChangeA); - Vector3Wide.Add(velocityA.Linear, linearChangeA, out velocityA.Linear); + velocityA.Linear += offsetCSI * inertiaA.InverseMass; //Note order of cross relative to the SolveIteration. //SolveIteration transforms velocity into constraint space velocity using JT, while this converts constraint space to world space using J. //The elements are transposed, and transposed skew symmetric matrices are negated. Flipping the cross product is equivalent to a negation. - Vector3Wide.CrossWithoutOverlap(offset, offsetCSI, out var offsetWorldImpulse); - Vector3Wide.Add(offsetWorldImpulse, orientationCSI, out var angularImpulseA); - Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseA, inertiaA.InverseInertiaTensor, out var angularChangeA); - Vector3Wide.Add(velocityA.Angular, angularChangeA, out velocityA.Angular); + velocityA.Angular += (Cross(offset, offsetCSI) + orientationCSI) * inertiaA.InverseInertiaTensor; - Vector3Wide.Scale(offsetCSI, inertiaB.InverseMass, out var negatedLinearChangeB); - Vector3Wide.Subtract(velocityB.Linear, negatedLinearChangeB, out velocityB.Linear); //note subtraction; the jacobian is -I - - Symmetric3x3Wide.TransformWithoutOverlap(orientationCSI, inertiaB.InverseInertiaTensor, out var negatedAngularChangeB); - Vector3Wide.Subtract(velocityB.Angular, negatedAngularChangeB, out velocityB.Angular); //note subtraction; the jacobian is -I + velocityB.Linear -= offsetCSI * inertiaB.InverseMass;//note subtraction; the jacobian is -I + velocityB.Angular -= orientationCSI * inertiaB.InverseInertiaTensor;//note subtraction; the jacobian is -I } //[MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -242,6 +235,7 @@ public void WarmStart2(in QuaternionWide orientationA, in BodyInertias inertiaA, { ApplyImpulse(inertiaA, inertiaB, prestep.LocalOffset * orientationA, accumulatedImpulses.Orientation, accumulatedImpulses.Offset, ref wsvA, ref wsvB); } + //[MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve2(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, float dt, float inverseDt, in WeldPrestepData prestep, ref WeldAccumulatedImpulses accumulatedImpulses, ref BodyVelocities wsvA, ref BodyVelocities wsvB) { @@ -261,7 +255,7 @@ public void Solve2(in QuaternionWide orientationA, in BodyInertias inertiaA, in //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. var offset = prestep.LocalOffset * orientationA; var positionError = ab - offset; - var targetOrientationB = prestep.LocalOrientation * orientationA; + var targetOrientationB = prestep.LocalOrientation * orientationA; var rotationError = Conjugate(targetOrientationB) * orientationB; GetApproximateAxisAngleFromQuaternion(rotationError, out var rotationErrorAxis, out var rotationErrorLength); @@ -281,7 +275,7 @@ public void Solve2(in QuaternionWide orientationA, in BodyInertias inertiaA, in // [ skewSymmetric(localOffset * orientationA) * Ia^-1, Ma^-1 + Mb^-1 + skewSymmetric(localOffset * orientationA) * Ia^-1 * transpose(skewSymmetric(localOffset * orientationA)) ] //where Ia^-1 and Ib^-1 are the inverse inertia tensors for a and b and Ma^-1 and Mb^-1 are the inverse masses of A and B expanded to 3x3 diagonal matrices. var jmjtA = inertiaA.InverseInertiaTensor + inertiaB.InverseInertiaTensor; - var xAB = CreateCrossProduct(offset); + CreateCrossProduct(offset, out var xAB); var jmjtB = inertiaA.InverseInertiaTensor * xAB; CompleteMatrixSandwichTranspose(xAB, jmjtB, out var jmjtD); var diagonalAdd = inertiaA.InverseMass + inertiaB.InverseMass; @@ -292,7 +286,7 @@ public void Solve2(in QuaternionWide orientationA, in BodyInertias inertiaA, in //This is equivalent to solving csi * effectiveMass^-1 = csv for csi, and since effectiveMass^-1 is symmetric positive semidefinite, we can use an LDLT decomposition to quickly solve it. Symmetric6x6Wide.LDLTSolve(orientationCSV, offsetCSV, jmjtA, jmjtB, jmjtD, out var orientationCSI, out var offsetCSI); //Symmetric6x6Wide.Invert(jmjtA, jmjtB, jmjtD, out var testEffectiveMass); - //Symmetric6x6Wide.TransformWithoutOverlap(orientationCSV, offsetCSV, testEffectiveMass, out var testOrientationCSI, out var testOffsetCSI); + //Symmetric6x6Wide.TransformWithoutOverlap(orientationCSV, offsetCSV, testEffectiveMass, out var orientationCSI, out var offsetCSI); orientationCSI = orientationCSI * effectiveMassCFMScale - accumulatedImpulses.Orientation * softnessImpulseScale; offsetCSI = offsetCSI * effectiveMassCFMScale - accumulatedImpulses.Offset * effectiveMassCFMScale; accumulatedImpulses.Orientation += orientationCSI; diff --git a/BepuUtilities/Matrix3x3Wide.cs b/BepuUtilities/Matrix3x3Wide.cs index ed2f57945..6b20578ec 100644 --- a/BepuUtilities/Matrix3x3Wide.cs +++ b/BepuUtilities/Matrix3x3Wide.cs @@ -181,7 +181,7 @@ public static void CreateCrossProduct(in Vector3Wide v, out Matrix3x3Wide skew) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Matrix3x3Wide CreateCrossProduct(in Vector3Wide v) + public static Matrix3x3Wide CreateCrossProduct(in Vector3Wide v) //TODO: this has some weird codegen on .NET 6 preview 5. { Matrix3x3Wide skew; skew.X.X = Vector.Zero; diff --git a/BepuUtilities/QuaternionWide.cs b/BepuUtilities/QuaternionWide.cs index 6c5867efc..492752005 100644 --- a/BepuUtilities/QuaternionWide.cs +++ b/BepuUtilities/QuaternionWide.cs @@ -240,7 +240,7 @@ public static void GetApproximateAxisAngleFromQuaternion(in QuaternionWide q, ou axis.Y = Vector.ConditionalSelect(useFallback, Vector.Zero, axis.Y); axis.Z = Vector.ConditionalSelect(useFallback, Vector.Zero, axis.Z); MathHelper.ApproximateAcos(qw, out var halfAngle); - angle = 2 * halfAngle; + angle = new Vector(2) * halfAngle; } /// From cbbeeddbad90f53e305ea88c24da9ff4b2e7be1c Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 3 Jul 2021 15:12:31 -0500 Subject: [PATCH 068/947] Fixed LDLTSolve typo. --- BepuPhysics/Constraints/Weld.cs | 2 +- BepuUtilities/Symmetric3x3Wide.cs | 2 +- BepuUtilities/Symmetric6x6Wide.cs | 84 +++++++++++++++++++++++++------ Demos/Demos/NewtDemo.cs | 63 +++++++++++++++++++++++ 4 files changed, 134 insertions(+), 17 deletions(-) diff --git a/BepuPhysics/Constraints/Weld.cs b/BepuPhysics/Constraints/Weld.cs index 5b9374f75..959cf87d4 100644 --- a/BepuPhysics/Constraints/Weld.cs +++ b/BepuPhysics/Constraints/Weld.cs @@ -286,7 +286,7 @@ public void Solve2(in QuaternionWide orientationA, in BodyInertias inertiaA, in //This is equivalent to solving csi * effectiveMass^-1 = csv for csi, and since effectiveMass^-1 is symmetric positive semidefinite, we can use an LDLT decomposition to quickly solve it. Symmetric6x6Wide.LDLTSolve(orientationCSV, offsetCSV, jmjtA, jmjtB, jmjtD, out var orientationCSI, out var offsetCSI); //Symmetric6x6Wide.Invert(jmjtA, jmjtB, jmjtD, out var testEffectiveMass); - //Symmetric6x6Wide.TransformWithoutOverlap(orientationCSV, offsetCSV, testEffectiveMass, out var orientationCSI, out var offsetCSI); + //Symmetric6x6Wide.TransformWithoutOverlap(orientationCSV, offsetCSV, testEffectiveMass, out var orientationCSI2, out var offsetCSI2); orientationCSI = orientationCSI * effectiveMassCFMScale - accumulatedImpulses.Orientation * softnessImpulseScale; offsetCSI = offsetCSI * effectiveMassCFMScale - accumulatedImpulses.Offset * effectiveMassCFMScale; accumulatedImpulses.Orientation += orientationCSI; diff --git a/BepuUtilities/Symmetric3x3Wide.cs b/BepuUtilities/Symmetric3x3Wide.cs index 484d0e20b..556a97ac4 100644 --- a/BepuUtilities/Symmetric3x3Wide.cs +++ b/BepuUtilities/Symmetric3x3Wide.cs @@ -85,7 +85,7 @@ public static void Add(in Symmetric3x3Wide a, in Symmetric3x3Wide b, out Symmetr /// Second matrix to add. /// Sum of the two input matrices. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Symmetric3x3Wide operator +(in Symmetric3x3Wide a, in Symmetric3x3Wide b) //TODO: without in decoration, this had some really peculiar codegen in .net 6 preview 5. + public static Symmetric3x3Wide operator +(in Symmetric3x3Wide a, in Symmetric3x3Wide b) { Symmetric3x3Wide result; result.XX = a.XX + b.XX; diff --git a/BepuUtilities/Symmetric6x6Wide.cs b/BepuUtilities/Symmetric6x6Wide.cs index 70baea18a..c5fde1d8e 100644 --- a/BepuUtilities/Symmetric6x6Wide.cs +++ b/BepuUtilities/Symmetric6x6Wide.cs @@ -83,10 +83,54 @@ public static void TransformWithoutOverlap(in Vector3Wide v0, in Vector3Wide v1, /// Lower right 3x3 region of the matrix. /// First 3 values of the result vector. /// Second 3 values of the result vector. - [MethodImpl(MethodImplOptions.AggressiveInlining)] + //[MethodImpl(MethodImplOptions.AggressiveInlining)] public static void LDLTSolve( in Vector3Wide v0, in Vector3Wide v1, in Symmetric3x3Wide a, in Matrix3x3Wide b, in Symmetric3x3Wide d, out Vector3Wide result0, out Vector3Wide result1) { + //var d1 = a.XX; + //var inverseD1 = Vector.One / d1; + //var l21 = inverseD1 * a.YX; + //var l31 = inverseD1 * a.ZX; + //var l41 = inverseD1 * b.X.X; + //var l51 = inverseD1 * b.X.Y; + //var l61 = inverseD1 * b.X.Z; + //var d2 = a.YY - l21 * l21 * d1; + //var inverseD2 = Vector.One / d2; + //var l32 = inverseD2 * (a.ZY - l31 * l21 * d1); + //var l42 = inverseD2 * (b.Y.X - l41 * l21 * d1); + //var l52 = inverseD2 * (b.Y.Y - l51 * l21 * d1); + //var l62 = inverseD2 * (b.Y.Z - l61 * l21 * d1); + //var d3 = a.ZZ - l31 * l31 * d1 - l32 * l32 * d2; + //var inverseD3 = Vector.One / d3; + //var l43 = inverseD3 * (b.Z.X - l41 * l31 * d1 - l42 * l32 * d2); + //var l53 = inverseD3 * (b.Z.Y - l51 * l31 * d1 - l52 * l32 * d2); + //var l63 = inverseD3 * (b.Z.Z - l61 * l31 * d1 - l62 * l32 * d2); + //var d4 = d.XX - l41 * l41 * d1 - l42 * l42 * d2 - l43 * l43 * d3; + //var inverseD4 = Vector.One / d4; + //var l54 = inverseD4 * (d.YX - l51 * l41 * d1 - l52 * l42 * d2 - l53 * l43 * d3); + //var l64 = inverseD4 * (d.ZX - l61 * l41 * d1 - l62 * l42 * d2 - l63 * l43 * d3); + //var d5 = d.YY - l51 * l51 * d1 - l52 * l52 * d2 - l53 * l53 * d3 - l54 * l54 * d4; + //var inverseD5 = Vector.One / d5; + //var l65 = inverseD5 * (d.ZY - l61 * l51 * d1 - l62 * l52 * d2 - l63 * l53 * d3 - l64 * l54 * d4); + //var d6 = d.ZZ - l61 * l61 * d1 - l62 * l62 * d2 - l63 * l63 * d3 - l64 * l64 * d4 - l65 * l65 * d5; + //var inverseD6 = Vector.One / d6; + + ////We now have the components of L and D, so substitute. + //result0.X = v0.X; + //result0.Y = v0.Y - l21 * result0.X; + //result0.Z = v0.Z - l31 * result0.X - l32 * result0.Y; + //result1.X = v1.X - l41 * result0.X - l42 * result0.Y - l43 * result0.Z; + //result1.Y = v1.Y - l51 * result0.X - l52 * result0.Y - l53 * result0.Z - l54 * result1.X; + //result1.Z = v1.Z - l61 * result0.X - l62 * result0.Y - l63 * result0.Z - l64 * result1.X - l65 * result1.Y; + + //result1.Z = result1.Z * inverseD6; + //result1.Y = result1.Y * inverseD5 - l65 * result1.Z; + //result1.X = result1.X * inverseD4 - l64 * result1.Z - l54 * result1.Y; + //result0.Z = result0.Z * inverseD3 - l63 * result1.Z - l53 * result1.Y - l43 * result1.X; + //result0.Y = result0.Y * inverseD2 - l62 * result1.Z - l52 * result1.Y - l42 * result1.X - l32 * result0.Z; + //result0.X = result0.X * inverseD1 - l61 * result1.Z - l51 * result1.Y - l41 * result1.X - l31 * result0.Z - l21 * result0.Y; + + var d1 = a.XX; var inverseD1 = Vector.One / d1; var l21 = inverseD1 * a.YX; @@ -94,24 +138,34 @@ public static void LDLTSolve( var l41 = inverseD1 * b.X.X; var l51 = inverseD1 * b.X.Y; var l61 = inverseD1 * b.X.Z; - var d2 = a.YY - l21 * l21; + var l21d1 = l21 * d1; + var d2 = a.YY - l21 * l21d1; var inverseD2 = Vector.One / d2; - var l32 = inverseD2 * (a.ZY - l31 * l21 * d1); - var l42 = inverseD2 * (b.Y.X - l41 * l21 * d1); - var l52 = inverseD2 * (b.Y.Y - l51 * l21 * d1); - var l62 = inverseD2 * (b.Y.Z - l61 * l21 * d1); - var d3 = a.ZZ - l31 * l31 * d1 - l32 * l32 * d2; + var l32 = inverseD2 * (a.ZY - l31 * l21d1); + var l42 = inverseD2 * (b.Y.X - l41 * l21d1); + var l52 = inverseD2 * (b.Y.Y - l51 * l21d1); + var l62 = inverseD2 * (b.Y.Z - l61 * l21d1); + var l31d1 = l31 * d1; + var l32d2 = l32 * d2; + var d3 = a.ZZ - l31 * l31d1 - l32 * l32d2; var inverseD3 = Vector.One / d3; - var l43 = inverseD3 * (b.Z.X - l41 * l31 * d1 - l42 * l32 * d2); - var l53 = inverseD3 * (b.Z.Y - l51 * l31 * d1 - l52 * l32 * d2); - var l63 = inverseD3 * (b.Z.Y - l61 * l31 * d1 - l62 * l32 * d2); - var d4 = d.XX - l41 * l41 * d1 - l42 * l42 * d2 - l43 * l43 * d3; + var l43 = inverseD3 * (b.Z.X - l41 * l31d1 - l42 * l32d2); + var l53 = inverseD3 * (b.Z.Y - l51 * l31d1 - l52 * l32d2); + var l63 = inverseD3 * (b.Z.Z - l61 * l31d1 - l62 * l32d2); + var l41d1 = l41 * d1; + var l42d2 = l42 * d2; + var l43d3 = l43 * d3; + var d4 = d.XX - l41 * l41d1 - l42 * l42d2 - l43 * l43d3; var inverseD4 = Vector.One / d4; - var l54 = inverseD4 * (d.YX - l51 * l41 * d1 - l52 * l42 * d2 - l53 * l43 * d3); - var l64 = inverseD4 * (d.ZX - l61 * l41 * d1 - l62 * l42 * d2 - l63 * l43 * d3); - var d5 = d.YY - l51 * l51 * d1 - l52 * l52 * d2 - l53 * l53 * d3 - l54 * l54 * d4; + var l54 = inverseD4 * (d.YX - l51 * l41d1 - l52 * l42d2 - l53 * l43d3); + var l64 = inverseD4 * (d.ZX - l61 * l41d1 - l62 * l42d2 - l63 * l43d3); + var l51d1 = l51 * d1; + var l52d2 = l52 * d2; + var l53d3 = l53 * d3; + var l54d4 = l54 * d4; + var d5 = d.YY - l51 * l51d1 - l52 * l52d2 - l53 * l53d3 - l54 * l54d4; var inverseD5 = Vector.One / d5; - var l65 = inverseD5 * (d.ZY - l61 * l51 * d1 - l62 * l52 * d2 - l63 * l53 * d3 - l64 * l54 * d4); + var l65 = inverseD5 * (d.ZY - l61 * l51d1 - l62 * l52d2 - l63 * l53d3 - l64 * l54d4); var d6 = d.ZZ - l61 * l61 * d1 - l62 * l62 * d2 - l63 * l63 * d3 - l64 * l64 * d4 - l65 * l65 * d5; var inverseD6 = Vector.One / d6; diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index a49a08e41..7f62daa1e 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -733,9 +733,72 @@ void Test() Console.WriteLine($"ree: {uh}"); } + void Test2() + { + const int pointCount = 16; + var points = new QuickList(pointCount * 2, BufferPool); + var random = new Random(5); + for (int i = 0; i < pointCount; ++i) + { + points.AllocateUnsafely() = new Vector3(3 * (float)random.NextDouble(), 1 * (float)random.NextDouble(), 3 * (float)random.NextDouble()); + } + var convexHull1 = new ConvexHull(points, BufferPool, out _); + points.Count = 0; + for (int i = 0; i < pointCount; ++i) + { + points.AllocateUnsafely() = new Vector3(3 * (float)random.NextDouble(), 1 * (float)random.NextDouble(), 3 * (float)random.NextDouble()); + } + var convexHull2 = new ConvexHull(points, BufferPool, out _); + convexHull1.ComputeInertia(1, out var scalarInertiaA); + convexHull2.ComputeInertia(1, out var scalarInertiaB); + + BodyInertias inertiaA; + inertiaA.InverseMass = new Vector(1); + inertiaA.InverseInertiaTensor.XX = new Vector(scalarInertiaA.InverseInertiaTensor.XX); + inertiaA.InverseInertiaTensor.YX = new Vector(scalarInertiaA.InverseInertiaTensor.YX); + inertiaA.InverseInertiaTensor.YY = new Vector(scalarInertiaA.InverseInertiaTensor.YY); + inertiaA.InverseInertiaTensor.ZX = new Vector(scalarInertiaA.InverseInertiaTensor.ZX); + inertiaA.InverseInertiaTensor.ZY = new Vector(scalarInertiaA.InverseInertiaTensor.ZY); + inertiaA.InverseInertiaTensor.ZZ = new Vector(scalarInertiaA.InverseInertiaTensor.ZZ); + Vector3Wide.Broadcast(new Vector3(1, 3, 4), out var offset); + BodyInertias inertiaB; + inertiaB.InverseMass = new Vector(1); + inertiaB.InverseInertiaTensor.XX = new Vector(scalarInertiaB.InverseInertiaTensor.XX); + inertiaB.InverseInertiaTensor.YX = new Vector(scalarInertiaB.InverseInertiaTensor.YX); + inertiaB.InverseInertiaTensor.YY = new Vector(scalarInertiaB.InverseInertiaTensor.YY); + inertiaB.InverseInertiaTensor.ZX = new Vector(scalarInertiaB.InverseInertiaTensor.ZX); + inertiaB.InverseInertiaTensor.ZY = new Vector(scalarInertiaB.InverseInertiaTensor.ZY); + inertiaB.InverseInertiaTensor.ZZ = new Vector(scalarInertiaB.InverseInertiaTensor.ZZ); + Vector3Wide.Broadcast(new Vector3(5, 4, 3), out var orientationCSV); + Vector3Wide.Broadcast(new Vector3(-5, -4, -3), out var offsetCSV); + + convexHull1.Dispose(BufferPool); + convexHull2.Dispose(BufferPool); + points.Dispose(BufferPool); + + var jmjtA = inertiaA.InverseInertiaTensor + inertiaB.InverseInertiaTensor; + Matrix3x3Wide.CreateCrossProduct(offset, out var xAB); + var jmjtB = inertiaA.InverseInertiaTensor * xAB; + Symmetric3x3Wide.CompleteMatrixSandwichTranspose(xAB, jmjtB, out var jmjtD); + var diagonalAdd = inertiaA.InverseMass + inertiaB.InverseMass; + jmjtD.XX += diagonalAdd; + jmjtD.YY += diagonalAdd; + jmjtD.ZZ += diagonalAdd; + + var inverseEffectiveMass = new Symmetric6x6Wide { A = jmjtA, B = jmjtB, D = jmjtD }; + + Symmetric6x6Wide.LDLTSolve(orientationCSV, offsetCSV, jmjtA, jmjtB, jmjtD, out var orientationCSI, out var offsetCSI); + Symmetric6x6Wide.TransformWithoutOverlap(orientationCSI, offsetCSI, inverseEffectiveMass, out var roundtripOrientationCSV, out var roundtripOffsetCSV); + + Symmetric6x6Wide.Invert(jmjtA, jmjtB, jmjtD, out var testEffectiveMass); + Symmetric6x6Wide.TransformWithoutOverlap(orientationCSV, offsetCSV, testEffectiveMass, out var orientationCSI2, out var offsetCSI2); + Symmetric6x6Wide.TransformWithoutOverlap(orientationCSI2, offsetCSI2, inverseEffectiveMass, out var roundtripOrientationCSV2, out var roundtripOffsetCSV2); + } + public unsafe override void Initialize(ContentArchive content, Camera camera) { Test(); + Test2(); Console.WriteLine($"aasgh: {Unsafe.SizeOf()}"); var stateTest = new MotionState(); Console.WriteLine($"offset: {(byte*)&stateTest.Velocity - (byte*)&stateTest.Pose}"); From 864db1c1590889079376d965595e94e42aa89ba2 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 3 Jul 2021 17:46:32 -0500 Subject: [PATCH 069/947] Alas, goofy codegen persists. --- BepuPhysics/Constraints/Weld.cs | 29 +++++++--- BepuUtilities/Symmetric6x6Wide.cs | 89 +++++++------------------------ Demos/Demos/NewtDemo.cs | 28 ++++++++-- 3 files changed, 63 insertions(+), 83 deletions(-) diff --git a/BepuPhysics/Constraints/Weld.cs b/BepuPhysics/Constraints/Weld.cs index 959cf87d4..2c09b04b4 100644 --- a/BepuPhysics/Constraints/Weld.cs +++ b/BepuPhysics/Constraints/Weld.cs @@ -229,11 +229,14 @@ private static void ApplyImpulse(in BodyInertias inertiaA, in BodyInertias inert velocityB.Angular -= orientationCSI * inertiaB.InverseInertiaTensor;//note subtraction; the jacobian is -I } - //[MethodImpl(MethodImplOptions.AggressiveInlining)] + //[MethodImpl(MethodImplOptions.NoInlining)] public void WarmStart2(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, float dt, float inverseDt, in WeldPrestepData prestep, in WeldAccumulatedImpulses accumulatedImpulses, ref BodyVelocities wsvA, ref BodyVelocities wsvB) { - ApplyImpulse(inertiaA, inertiaB, prestep.LocalOffset * orientationA, accumulatedImpulses.Orientation, accumulatedImpulses.Offset, ref wsvA, ref wsvB); + Transform(prestep.LocalOffset, orientationA, out var offset); + ApplyImpulse(inertiaA, inertiaB, offset, accumulatedImpulses.Orientation, accumulatedImpulses.Offset, ref wsvA, ref wsvB); + //var offset = prestep.LocalOffset * orientationA; + //ApplyImpulse(inertiaA, inertiaB, prestep.LocalOffset * orientationA, accumulatedImpulses.Orientation, accumulatedImpulses.Offset, ref wsvA, ref wsvB); } //[MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve2(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, float dt, float inverseDt, @@ -253,10 +256,12 @@ public void Solve2(in QuaternionWide orientationA, in BodyInertias inertiaA, in //where I is the 3x3 identity matrix. //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. - var offset = prestep.LocalOffset * orientationA; + //var offset = prestep.LocalOffset * orientationA; + QuaternionWide.Transform(prestep.LocalOffset, orientationA, out var offset); var positionError = ab - offset; var targetOrientationB = prestep.LocalOrientation * orientationA; - var rotationError = Conjugate(targetOrientationB) * orientationB; + //ConcatenateWithoutOverlap(prestep.LocalOrientation, orientationA, out var targetOrientationB); + ConcatenateWithoutOverlap(Conjugate(targetOrientationB), orientationB, out var rotationError); GetApproximateAxisAngleFromQuaternion(rotationError, out var rotationErrorAxis, out var rotationErrorLength); SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); @@ -274,9 +279,11 @@ public void Solve2(in QuaternionWide orientationA, in BodyInertias inertiaA, in //J * M^-1 * JT = [ Ia^-1 + Ib^-1, Ia^-1 * transpose(skewSymmetric(localOffset * orientationA)) ] // [ skewSymmetric(localOffset * orientationA) * Ia^-1, Ma^-1 + Mb^-1 + skewSymmetric(localOffset * orientationA) * Ia^-1 * transpose(skewSymmetric(localOffset * orientationA)) ] //where Ia^-1 and Ib^-1 are the inverse inertia tensors for a and b and Ma^-1 and Mb^-1 are the inverse masses of A and B expanded to 3x3 diagonal matrices. - var jmjtA = inertiaA.InverseInertiaTensor + inertiaB.InverseInertiaTensor; + //var jmjtA = inertiaA.InverseInertiaTensor + inertiaB.InverseInertiaTensor; + Symmetric3x3Wide.Add(inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, out var jmjtA); CreateCrossProduct(offset, out var xAB); - var jmjtB = inertiaA.InverseInertiaTensor * xAB; + Multiply(inertiaA.InverseInertiaTensor, xAB, out var jmjtB); + //var jmjtB = inertiaA.InverseInertiaTensor * xAB; CompleteMatrixSandwichTranspose(xAB, jmjtB, out var jmjtD); var diagonalAdd = inertiaA.InverseMass + inertiaB.InverseMass; jmjtD.XX += diagonalAdd; @@ -287,8 +294,16 @@ public void Solve2(in QuaternionWide orientationA, in BodyInertias inertiaA, in Symmetric6x6Wide.LDLTSolve(orientationCSV, offsetCSV, jmjtA, jmjtB, jmjtD, out var orientationCSI, out var offsetCSI); //Symmetric6x6Wide.Invert(jmjtA, jmjtB, jmjtD, out var testEffectiveMass); //Symmetric6x6Wide.TransformWithoutOverlap(orientationCSV, offsetCSV, testEffectiveMass, out var orientationCSI2, out var offsetCSI2); + //Scale(orientationCSI, effectiveMassCFMScale, out orientationCSI); + //Scale(accumulatedImpulses.Orientation, softnessImpulseScale, out var orientationSoftness); + //Subtract(orientationCSI, orientationSoftness, out orientationCSI); + //Add(accumulatedImpulses.Orientation, orientationCSI, out accumulatedImpulses.Orientation); + //Scale(offsetCSI, effectiveMassCFMScale, out offsetCSI); + //Scale(accumulatedImpulses.Offset, softnessImpulseScale, out var offsetSoftness); + //Subtract(offsetCSI, offsetSoftness, out offsetCSI); + //Add(accumulatedImpulses.Offset, offsetCSI, out accumulatedImpulses.Offset); orientationCSI = orientationCSI * effectiveMassCFMScale - accumulatedImpulses.Orientation * softnessImpulseScale; - offsetCSI = offsetCSI * effectiveMassCFMScale - accumulatedImpulses.Offset * effectiveMassCFMScale; + offsetCSI = offsetCSI * effectiveMassCFMScale - accumulatedImpulses.Offset * softnessImpulseScale; accumulatedImpulses.Orientation += orientationCSI; accumulatedImpulses.Offset += offsetCSI; diff --git a/BepuUtilities/Symmetric6x6Wide.cs b/BepuUtilities/Symmetric6x6Wide.cs index c5fde1d8e..deb81f7e4 100644 --- a/BepuUtilities/Symmetric6x6Wide.cs +++ b/BepuUtilities/Symmetric6x6Wide.cs @@ -2,6 +2,8 @@ using System.Collections.Generic; using System.Numerics; using System.Runtime.CompilerServices; +using System.Runtime.Intrinsics; +using System.Runtime.Intrinsics.X86; using System.Text; namespace BepuUtilities @@ -60,7 +62,7 @@ public static void Invert(in Symmetric6x6Wide m, out Symmetric6x6Wide result) /// Matrix to transform with. /// First half of the result. /// Second half of the result. - [MethodImpl(MethodImplOptions.AggressiveInlining)] + [MethodImpl(MethodImplOptions.NoInlining)] public static void TransformWithoutOverlap(in Vector3Wide v0, in Vector3Wide v1, in Symmetric6x6Wide m, out Vector3Wide result0, out Vector3Wide result1) { result0.X = v0.X * m.A.XX + v0.Y * m.A.YX + v0.Z * m.A.ZX + v1.X * m.B.X.X + v1.Y * m.B.X.Y + v1.Z * m.B.X.Z; @@ -83,54 +85,10 @@ public static void TransformWithoutOverlap(in Vector3Wide v0, in Vector3Wide v1, /// Lower right 3x3 region of the matrix. /// First 3 values of the result vector. /// Second 3 values of the result vector. - //[MethodImpl(MethodImplOptions.AggressiveInlining)] + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void LDLTSolve( in Vector3Wide v0, in Vector3Wide v1, in Symmetric3x3Wide a, in Matrix3x3Wide b, in Symmetric3x3Wide d, out Vector3Wide result0, out Vector3Wide result1) { - //var d1 = a.XX; - //var inverseD1 = Vector.One / d1; - //var l21 = inverseD1 * a.YX; - //var l31 = inverseD1 * a.ZX; - //var l41 = inverseD1 * b.X.X; - //var l51 = inverseD1 * b.X.Y; - //var l61 = inverseD1 * b.X.Z; - //var d2 = a.YY - l21 * l21 * d1; - //var inverseD2 = Vector.One / d2; - //var l32 = inverseD2 * (a.ZY - l31 * l21 * d1); - //var l42 = inverseD2 * (b.Y.X - l41 * l21 * d1); - //var l52 = inverseD2 * (b.Y.Y - l51 * l21 * d1); - //var l62 = inverseD2 * (b.Y.Z - l61 * l21 * d1); - //var d3 = a.ZZ - l31 * l31 * d1 - l32 * l32 * d2; - //var inverseD3 = Vector.One / d3; - //var l43 = inverseD3 * (b.Z.X - l41 * l31 * d1 - l42 * l32 * d2); - //var l53 = inverseD3 * (b.Z.Y - l51 * l31 * d1 - l52 * l32 * d2); - //var l63 = inverseD3 * (b.Z.Z - l61 * l31 * d1 - l62 * l32 * d2); - //var d4 = d.XX - l41 * l41 * d1 - l42 * l42 * d2 - l43 * l43 * d3; - //var inverseD4 = Vector.One / d4; - //var l54 = inverseD4 * (d.YX - l51 * l41 * d1 - l52 * l42 * d2 - l53 * l43 * d3); - //var l64 = inverseD4 * (d.ZX - l61 * l41 * d1 - l62 * l42 * d2 - l63 * l43 * d3); - //var d5 = d.YY - l51 * l51 * d1 - l52 * l52 * d2 - l53 * l53 * d3 - l54 * l54 * d4; - //var inverseD5 = Vector.One / d5; - //var l65 = inverseD5 * (d.ZY - l61 * l51 * d1 - l62 * l52 * d2 - l63 * l53 * d3 - l64 * l54 * d4); - //var d6 = d.ZZ - l61 * l61 * d1 - l62 * l62 * d2 - l63 * l63 * d3 - l64 * l64 * d4 - l65 * l65 * d5; - //var inverseD6 = Vector.One / d6; - - ////We now have the components of L and D, so substitute. - //result0.X = v0.X; - //result0.Y = v0.Y - l21 * result0.X; - //result0.Z = v0.Z - l31 * result0.X - l32 * result0.Y; - //result1.X = v1.X - l41 * result0.X - l42 * result0.Y - l43 * result0.Z; - //result1.Y = v1.Y - l51 * result0.X - l52 * result0.Y - l53 * result0.Z - l54 * result1.X; - //result1.Z = v1.Z - l61 * result0.X - l62 * result0.Y - l63 * result0.Z - l64 * result1.X - l65 * result1.Y; - - //result1.Z = result1.Z * inverseD6; - //result1.Y = result1.Y * inverseD5 - l65 * result1.Z; - //result1.X = result1.X * inverseD4 - l64 * result1.Z - l54 * result1.Y; - //result0.Z = result0.Z * inverseD3 - l63 * result1.Z - l53 * result1.Y - l43 * result1.X; - //result0.Y = result0.Y * inverseD2 - l62 * result1.Z - l52 * result1.Y - l42 * result1.X - l32 * result0.Z; - //result0.X = result0.X * inverseD1 - l61 * result1.Z - l51 * result1.Y - l41 * result1.X - l31 * result0.Z - l21 * result0.Y; - - var d1 = a.XX; var inverseD1 = Vector.One / d1; var l21 = inverseD1 * a.YX; @@ -138,34 +96,24 @@ public static void LDLTSolve( var l41 = inverseD1 * b.X.X; var l51 = inverseD1 * b.X.Y; var l61 = inverseD1 * b.X.Z; - var l21d1 = l21 * d1; - var d2 = a.YY - l21 * l21d1; + var d2 = a.YY - l21 * l21 * d1; var inverseD2 = Vector.One / d2; - var l32 = inverseD2 * (a.ZY - l31 * l21d1); - var l42 = inverseD2 * (b.Y.X - l41 * l21d1); - var l52 = inverseD2 * (b.Y.Y - l51 * l21d1); - var l62 = inverseD2 * (b.Y.Z - l61 * l21d1); - var l31d1 = l31 * d1; - var l32d2 = l32 * d2; - var d3 = a.ZZ - l31 * l31d1 - l32 * l32d2; + var l32 = inverseD2 * (a.ZY - l31 * l21 * d1); + var l42 = inverseD2 * (b.Y.X - l41 * l21 * d1); + var l52 = inverseD2 * (b.Y.Y - l51 * l21 * d1); + var l62 = inverseD2 * (b.Y.Z - l61 * l21 * d1); + var d3 = a.ZZ - l31 * l31 * d1 - l32 * l32 * d2; var inverseD3 = Vector.One / d3; - var l43 = inverseD3 * (b.Z.X - l41 * l31d1 - l42 * l32d2); - var l53 = inverseD3 * (b.Z.Y - l51 * l31d1 - l52 * l32d2); - var l63 = inverseD3 * (b.Z.Z - l61 * l31d1 - l62 * l32d2); - var l41d1 = l41 * d1; - var l42d2 = l42 * d2; - var l43d3 = l43 * d3; - var d4 = d.XX - l41 * l41d1 - l42 * l42d2 - l43 * l43d3; + var l43 = inverseD3 * (b.Z.X - l41 * l31 * d1 - l42 * l32 * d2); + var l53 = inverseD3 * (b.Z.Y - l51 * l31 * d1 - l52 * l32 * d2); + var l63 = inverseD3 * (b.Z.Z - l61 * l31 * d1 - l62 * l32 * d2); + var d4 = d.XX - l41 * l41 * d1 - l42 * l42 * d2 - l43 * l43 * d3; var inverseD4 = Vector.One / d4; - var l54 = inverseD4 * (d.YX - l51 * l41d1 - l52 * l42d2 - l53 * l43d3); - var l64 = inverseD4 * (d.ZX - l61 * l41d1 - l62 * l42d2 - l63 * l43d3); - var l51d1 = l51 * d1; - var l52d2 = l52 * d2; - var l53d3 = l53 * d3; - var l54d4 = l54 * d4; - var d5 = d.YY - l51 * l51d1 - l52 * l52d2 - l53 * l53d3 - l54 * l54d4; + var l54 = inverseD4 * (d.YX - l51 * l41 * d1 - l52 * l42 * d2 - l53 * l43 * d3); + var l64 = inverseD4 * (d.ZX - l61 * l41 * d1 - l62 * l42 * d2 - l63 * l43 * d3); + var d5 = d.YY - l51 * l51 * d1 - l52 * l52 * d2 - l53 * l53 * d3 - l54 * l54 * d4; var inverseD5 = Vector.One / d5; - var l65 = inverseD5 * (d.ZY - l61 * l51d1 - l62 * l52d2 - l63 * l53d3 - l64 * l54d4); + var l65 = inverseD5 * (d.ZY - l61 * l51 * d1 - l62 * l52 * d2 - l63 * l53 * d3 - l64 * l54 * d4); var d6 = d.ZZ - l61 * l61 * d1 - l62 * l62 * d2 - l63 * l63 * d3 - l64 * l64 * d4 - l65 * l65 * d5; var inverseD6 = Vector.One / d6; @@ -183,7 +131,6 @@ public static void LDLTSolve( result0.Z = result0.Z * inverseD3 - l63 * result1.Z - l53 * result1.Y - l43 * result1.X; result0.Y = result0.Y * inverseD2 - l62 * result1.Z - l52 * result1.Y - l42 * result1.X - l32 * result0.Z; result0.X = result0.X * inverseD1 - l61 * result1.Z - l51 * result1.Y - l41 * result1.X - l31 * result0.Z - l21 * result0.Y; - } } } diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index 7f62daa1e..a68c9938c 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -787,18 +787,36 @@ void Test2() var inverseEffectiveMass = new Symmetric6x6Wide { A = jmjtA, B = jmjtB, D = jmjtD }; - Symmetric6x6Wide.LDLTSolve(orientationCSV, offsetCSV, jmjtA, jmjtB, jmjtD, out var orientationCSI, out var offsetCSI); - Symmetric6x6Wide.TransformWithoutOverlap(orientationCSI, offsetCSI, inverseEffectiveMass, out var roundtripOrientationCSV, out var roundtripOffsetCSV); + const int iterationCount = 1000000; + var startLDLT = Stopwatch.GetTimestamp(); + for (int i = 0; i < iterationCount; ++i) + { + Symmetric6x6Wide.LDLTSolve(orientationCSV, offsetCSV, jmjtA, jmjtB, jmjtD, out var orientationCSI, out var offsetCSI); + //Symmetric6x6Wide.TransformWithoutOverlap(orientationCSI, offsetCSI, inverseEffectiveMass, out var roundtripOrientationCSV, out var roundtripOffsetCSV); + } + var endLDLT = Stopwatch.GetTimestamp(); + var startInverse = Stopwatch.GetTimestamp(); + for (int i = 0; i < iterationCount; ++i) + { + Symmetric6x6Wide.Invert(jmjtA, jmjtB, jmjtD, out var testEffectiveMass); + Symmetric6x6Wide.TransformWithoutOverlap(orientationCSV, offsetCSV, testEffectiveMass, out var orientationCSI2, out var offsetCSI2); + //Symmetric6x6Wide.TransformWithoutOverlap(orientationCSI2, offsetCSI2, inverseEffectiveMass, out var roundtripOrientationCSV2, out var roundtripOffsetCSV2); + } + var endInverse = Stopwatch.GetTimestamp(); + double ldltTime = (endLDLT - startLDLT) / (iterationCount * (double)Stopwatch.Frequency); + double inverseTime = (endInverse - startInverse) / (iterationCount * (double)Stopwatch.Frequency); + + Console.WriteLine($"LDLT time (ns): {ldltTime * 1e9}"); + Console.WriteLine($"Inverse time (ns): {inverseTime * 1e9}"); + Console.WriteLine($"inverse / ldlt: {inverseTime / ldltTime}"); - Symmetric6x6Wide.Invert(jmjtA, jmjtB, jmjtD, out var testEffectiveMass); - Symmetric6x6Wide.TransformWithoutOverlap(orientationCSV, offsetCSV, testEffectiveMass, out var orientationCSI2, out var offsetCSI2); - Symmetric6x6Wide.TransformWithoutOverlap(orientationCSI2, offsetCSI2, inverseEffectiveMass, out var roundtripOrientationCSV2, out var roundtripOffsetCSV2); } public unsafe override void Initialize(ContentArchive content, Camera camera) { Test(); Test2(); + Test2(); Console.WriteLine($"aasgh: {Unsafe.SizeOf()}"); var stateTest = new MotionState(); Console.WriteLine($"offset: {(byte*)&stateTest.Velocity - (byte*)&stateTest.Pose}"); From e873048b97a0c749c8a82151429b4af9edc80a45 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 3 Jul 2021 18:10:49 -0500 Subject: [PATCH 070/947] Manually inlining improves codegen and I am not terribly pleased. --- BepuPhysics/Constraints/Weld.cs | 41 ++++++++++++++++++++++++++++----- Demos/Demos/NewtDemo.cs | 7 ------ 2 files changed, 35 insertions(+), 13 deletions(-) diff --git a/BepuPhysics/Constraints/Weld.cs b/BepuPhysics/Constraints/Weld.cs index 2c09b04b4..56633631d 100644 --- a/BepuPhysics/Constraints/Weld.cs +++ b/BepuPhysics/Constraints/Weld.cs @@ -272,8 +272,21 @@ public void Solve2(in QuaternionWide orientationA, in BodyInertias inertiaA, in //csi = -accumulatedImpulse * projection.SoftnessImpulseScale - (-biasVelocity + csvaLinear + csvaAngular + csvbLinear + csvbAngular) * effectiveMass; //csi = (biasVelocity - csvaLinear - csvaAngular - csvbLinear - csvbAngular) * effectiveMass - accumulatedImpulse * projection.SoftnessImpulseScale; //csv = V * JT - var orientationCSV = orientationBiasVelocity - (wsvA.Angular - wsvB.Angular); - var offsetCSV = offsetBiasVelocity - (wsvA.Linear - wsvB.Linear + Cross(wsvA.Angular, offset)); + //var orientationCSV = orientationBiasVelocity - (wsvA.Angular - wsvB.Angular); + //var offsetCSV = offsetBiasVelocity - (wsvA.Linear - wsvB.Linear + Cross(wsvA.Angular, offset)); + + Vector3Wide orientationCSV, offsetCSV; + orientationCSV.X = orientationBiasVelocity.X - wsvA.Angular.X + wsvB.Angular.X; + orientationCSV.Y = orientationBiasVelocity.Y - wsvA.Angular.Y + wsvB.Angular.Y; + orientationCSV.Z = orientationBiasVelocity.Z - wsvA.Angular.Z + wsvB.Angular.Z; + + offsetCSV.X = offsetBiasVelocity.X - wsvA.Linear.X + wsvB.Linear.X - (wsvA.Angular.Y * offset.Z - wsvA.Angular.Z * offset.Y); + offsetCSV.Y = offsetBiasVelocity.Y - wsvA.Linear.Y + wsvB.Linear.Y - (wsvA.Angular.Z * offset.X - wsvA.Angular.X * offset.Z); + offsetCSV.Z = offsetBiasVelocity.Z - wsvA.Linear.Z + wsvB.Linear.Z - (wsvA.Angular.X * offset.Y - wsvA.Angular.Y * offset.X); + + //result.X = a.Y * b.Z - a.Z * b.Y; + //result.Y = a.Z * b.X - a.X * b.Z; + //result.Z = a.X * b.Y - a.Y * b.X; //Effective mass = (J * M^-1 * JT)^-1, which is going to be a little tricky because J * M^-1 * JT is a 6x6 matrix: //J * M^-1 * JT = [ Ia^-1 + Ib^-1, Ia^-1 * transpose(skewSymmetric(localOffset * orientationA)) ] @@ -294,6 +307,7 @@ public void Solve2(in QuaternionWide orientationA, in BodyInertias inertiaA, in Symmetric6x6Wide.LDLTSolve(orientationCSV, offsetCSV, jmjtA, jmjtB, jmjtD, out var orientationCSI, out var offsetCSI); //Symmetric6x6Wide.Invert(jmjtA, jmjtB, jmjtD, out var testEffectiveMass); //Symmetric6x6Wide.TransformWithoutOverlap(orientationCSV, offsetCSV, testEffectiveMass, out var orientationCSI2, out var offsetCSI2); + //Scale(orientationCSI, effectiveMassCFMScale, out orientationCSI); //Scale(accumulatedImpulses.Orientation, softnessImpulseScale, out var orientationSoftness); //Subtract(orientationCSI, orientationSoftness, out orientationCSI); @@ -302,10 +316,25 @@ public void Solve2(in QuaternionWide orientationA, in BodyInertias inertiaA, in //Scale(accumulatedImpulses.Offset, softnessImpulseScale, out var offsetSoftness); //Subtract(offsetCSI, offsetSoftness, out offsetCSI); //Add(accumulatedImpulses.Offset, offsetCSI, out accumulatedImpulses.Offset); - orientationCSI = orientationCSI * effectiveMassCFMScale - accumulatedImpulses.Orientation * softnessImpulseScale; - offsetCSI = offsetCSI * effectiveMassCFMScale - accumulatedImpulses.Offset * softnessImpulseScale; - accumulatedImpulses.Orientation += orientationCSI; - accumulatedImpulses.Offset += offsetCSI; + + //orientationCSI = orientationCSI * effectiveMassCFMScale - accumulatedImpulses.Orientation * softnessImpulseScale; + //offsetCSI = offsetCSI * effectiveMassCFMScale - accumulatedImpulses.Offset * softnessImpulseScale; + //accumulatedImpulses.Orientation += orientationCSI; + //accumulatedImpulses.Offset += offsetCSI; + + orientationCSI.X = orientationCSI.X * effectiveMassCFMScale - accumulatedImpulses.Orientation.X * softnessImpulseScale; + orientationCSI.Y = orientationCSI.Y * effectiveMassCFMScale - accumulatedImpulses.Orientation.Y * softnessImpulseScale; + orientationCSI.Z = orientationCSI.Z * effectiveMassCFMScale - accumulatedImpulses.Orientation.Z * softnessImpulseScale; + accumulatedImpulses.Orientation.X += orientationCSI.X; + accumulatedImpulses.Orientation.Y += orientationCSI.Y; + accumulatedImpulses.Orientation.Z += orientationCSI.Z; + + offsetCSI.X = offsetCSI.X * effectiveMassCFMScale - accumulatedImpulses.Offset.X * softnessImpulseScale; + offsetCSI.Y = offsetCSI.Y * effectiveMassCFMScale - accumulatedImpulses.Offset.Y * softnessImpulseScale; + offsetCSI.Z = offsetCSI.Z * effectiveMassCFMScale - accumulatedImpulses.Offset.Z * softnessImpulseScale; + accumulatedImpulses.Offset.X += offsetCSI.X; + accumulatedImpulses.Offset.Y += offsetCSI.Y; + accumulatedImpulses.Offset.Z += offsetCSI.Z; ApplyImpulse(inertiaA, inertiaB, ab, orientationCSI, offsetCSI, ref wsvA, ref wsvB); } diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index a68c9938c..4f78f6821 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -814,13 +814,6 @@ void Test2() public unsafe override void Initialize(ContentArchive content, Camera camera) { - Test(); - Test2(); - Test2(); - Console.WriteLine($"aasgh: {Unsafe.SizeOf()}"); - var stateTest = new MotionState(); - Console.WriteLine($"offset: {(byte*)&stateTest.Velocity - (byte*)&stateTest.Pose}"); - camera.Position = new Vector3(-5f, 5.5f, 5f); camera.Yaw = MathHelper.Pi / 4; camera.Pitch = MathHelper.Pi * 0.15f; From 3a03ab8330422a2b753499dde28770f13a3d6bdb Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 3 Jul 2021 21:00:28 -0500 Subject: [PATCH 071/947] Fiexd body velocity memory layout. --- BepuPhysics/Bodies_GatherScatter.cs | 133 +++------------------------- BepuPhysics/BodyProperties.cs | 5 +- BepuPhysics/Constraints/Weld.cs | 37 +++----- Demos/Program.cs | 2 +- 4 files changed, 28 insertions(+), 149 deletions(-) diff --git a/BepuPhysics/Bodies_GatherScatter.cs b/BepuPhysics/Bodies_GatherScatter.cs index b5324ae96..b4ef19327 100644 --- a/BepuPhysics/Bodies_GatherScatter.cs +++ b/BepuPhysics/Bodies_GatherScatter.cs @@ -469,128 +469,6 @@ public void GatherState(ref FourBodyReferences references, int count, Vector3Wide.Subtract(positionC, positionA, out ad); } - //[MethodImpl(MethodImplOptions.AggressiveInlining)] - //private static void WriteLinearGatherState(ref int baseBodyIndexInSet, int bodyIndexInBundle, ref Buffer states, ref Vector3Wide linearVelocity, ref Vector3Wide position) - //{ - // ref var state = ref states[Unsafe.Add(ref baseBodyIndexInSet, bodyIndexInBundle)]; - // Vector3Wide.WriteFirst(state.Pose.Position, ref GetOffsetInstance(ref position, bodyIndexInBundle)); - // Vector3Wide.WriteFirst(state.Velocity.Linear, ref GetOffsetInstance(ref linearVelocity, bodyIndexInBundle)); - //} - ///// - ///// Gathers relative positions and linear velocities for a two body bundle into an AOSOA bundle. - ///// - ///// Active body indices being gathered. - ///// Number of body pairs in the bundle. - ///// Gathered offset from body A to body B. - ///// Linear velocity of body A. - ///// Linear velocity of body B. - ////[MethodImpl(MethodImplOptions.AggressiveInlining)] - //public void GatherLinearState(ref TwoBodyReferences references, int count, out Vector3Wide ab, out Vector3Wide linearVelocityA, out Vector3Wide linearVelocityB) - //{ - // Unsafe.SkipInit(out Vector3Wide positionA); - // Unsafe.SkipInit(out Vector3Wide positionB); - // Unsafe.SkipInit(out linearVelocityA); - // Unsafe.SkipInit(out linearVelocityB); - // Debug.Assert(count >= 0 && count <= Vector.Count); - // //Grab the base references for the body indices. Note that we make use of the references memory layout again. - // ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); - // ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); - - // ref var states = ref ActiveSet.MotionStates; - // for (int i = 0; i < count; ++i) - // { - // WriteLinearGatherState(ref baseIndexA, i, ref states, ref positionA, ref linearVelocityA); - // WriteLinearGatherState(ref baseIndexB, i, ref states, ref positionB, ref linearVelocityB); - // } - // //Same as other gather case; this is sensitive to changes in the representation of body position. In high precision modes, this'll need to change. - // Vector3Wide.Subtract(positionB, positionA, out ab); - //} - - - ///// - ///// Gathers relative positions and linear velocities for a three body bundle into an AOSOA bundle. - ///// - ///// Active body indices being gathered. - ///// Number of body pairs in the bundle. - ///// Gathered offset from body A to body B. - ///// Gathered offset from body A to body C. - ///// Linear velocity of body A. - ///// Linear velocity of body B. - ///// Linear velocity of body C. - ////[MethodImpl(MethodImplOptions.AggressiveInlining)] - //public void GatherLinearState(ref ThreeBodyReferences references, int count, out Vector3Wide ab, out Vector3Wide ac, out Vector3Wide linearVelocityA, out Vector3Wide linearVelocityB, out Vector3Wide linearVelocityC) - //{ - // Unsafe.SkipInit(out Vector3Wide positionA); - // Unsafe.SkipInit(out Vector3Wide positionB); - // Unsafe.SkipInit(out Vector3Wide positionC); - // Unsafe.SkipInit(out linearVelocityA); - // Unsafe.SkipInit(out linearVelocityB); - // Unsafe.SkipInit(out linearVelocityC); - // Debug.Assert(count >= 0 && count <= Vector.Count); - // //Grab the base references for the body indices. Note that we make use of the references memory layout again. - // ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); - // ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); - // ref var baseIndexC = ref Unsafe.As, int>(ref references.IndexC); - - // ref var states = ref ActiveSet.MotionStates; - // for (int i = 0; i < count; ++i) - // { - // WriteLinearGatherState(ref baseIndexA, i, ref states, ref positionA, ref linearVelocityA); - // WriteLinearGatherState(ref baseIndexB, i, ref states, ref positionB, ref linearVelocityB); - // WriteLinearGatherState(ref baseIndexC, i, ref states, ref positionC, ref linearVelocityC); - // } - // //Same as other gather case; this is sensitive to changes in the representation of body position. In high precision modes, this'll need to change. - // Vector3Wide.Subtract(positionB, positionA, out ab); - // Vector3Wide.Subtract(positionC, positionA, out ac); - //} - - - ///// - ///// Gathers relative positions and linear velocities for a four body bundle into an AOSOA bundle. - ///// - ///// Active body indices being gathered. - ///// Number of body pairs in the bundle. - ///// Gathered offset from body A to body B. - ///// Gathered offset from body A to body C. - ///// Gathered offset from body A to body D. - ///// Linear velocity of body A. - ///// Linear velocity of body B. - ///// Linear velocity of body C. - ///// Linear velocity of body D. - ////[MethodImpl(MethodImplOptions.AggressiveInlining)] - //public void GatherLinearState(ref FourBodyReferences references, int count, out Vector3Wide ab, out Vector3Wide ac, out Vector3Wide ad, - // out Vector3Wide linearVelocityA, out Vector3Wide linearVelocityB, out Vector3Wide linearVelocityC, out Vector3Wide linearVelocityD) - //{ - // Unsafe.SkipInit(out Vector3Wide positionA); - // Unsafe.SkipInit(out Vector3Wide positionB); - // Unsafe.SkipInit(out Vector3Wide positionC); - // Unsafe.SkipInit(out Vector3Wide positionD); - // Unsafe.SkipInit(out linearVelocityA); - // Unsafe.SkipInit(out linearVelocityB); - // Unsafe.SkipInit(out linearVelocityC); - // Unsafe.SkipInit(out linearVelocityD); - // Debug.Assert(count >= 0 && count <= Vector.Count); - // //Grab the base references for the body indices. Note that we make use of the references memory layout again. - // ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); - // ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); - // ref var baseIndexC = ref Unsafe.As, int>(ref references.IndexC); - // ref var baseIndexD = ref Unsafe.As, int>(ref references.IndexD); - - // ref var states = ref ActiveSet.MotionStates; - // for (int i = 0; i < count; ++i) - // { - // WriteLinearGatherState(ref baseIndexA, i, ref states, ref positionA, ref linearVelocityA); - // WriteLinearGatherState(ref baseIndexB, i, ref states, ref positionB, ref linearVelocityB); - // WriteLinearGatherState(ref baseIndexC, i, ref states, ref positionC, ref linearVelocityC); - // WriteLinearGatherState(ref baseIndexD, i, ref states, ref positionD, ref linearVelocityD); - // } - // //Same as other gather case; this is sensitive to changes in the representation of body position. In high precision modes, this'll need to change. - // Vector3Wide.Subtract(positionB, positionA, out ab); - // Vector3Wide.Subtract(positionC, positionA, out ac); - // Vector3Wide.Subtract(positionD, positionA, out ad); - //} - - [MethodImpl(MethodImplOptions.AggressiveInlining)] private unsafe void ScatterVelocities(ref BodyVelocities sourceVelocities, ref int baseIndex, int innerIndex) { @@ -606,6 +484,16 @@ private unsafe void TransposeScatterVelocities(ref BodyVelocities sourceVelociti { if (Avx.IsSupported) { + //for (int i = 0; i < 8; ++i) + //{ + // Get(ref sourceVelocities.Linear.X, i) = i + 100; + // Get(ref sourceVelocities.Linear.Y, i) = i + 200; + // Get(ref sourceVelocities.Linear.Z, i) = i + 300; + // Get(ref sourceVelocities.Angular.X, i) = i + 500; + // Get(ref sourceVelocities.Angular.Y, i) = i + 600; + // Get(ref sourceVelocities.Angular.Z, i) = i + 700; + //} + //Just like the gather but... transposed, we transpose from the wide representation into per-body velocities. //The motion state struct puts the velocities into the second 32 byte chunk, so we can do a single write per body. var m0 = sourceVelocities.Linear.X.AsVector256(); @@ -646,7 +534,6 @@ private unsafe void TransposeScatterVelocities(ref BodyVelocities sourceVelociti } else { - } } diff --git a/BepuPhysics/BodyProperties.cs b/BepuPhysics/BodyProperties.cs index a9c71f4a4..04baa534a 100644 --- a/BepuPhysics/BodyProperties.cs +++ b/BepuPhysics/BodyProperties.cs @@ -194,10 +194,13 @@ public static void MultiplyWithoutOverlap(in RigidPose a, in RigidPose b, out Ri } } - [StructLayout(LayoutKind.Sequential, Size = 32, Pack = 16)] + [StructLayout(LayoutKind.Explicit, Size = 32)] public struct BodyVelocity { + [FieldOffset(0)] public Vector3 Linear; + + [FieldOffset(16)] public Vector3 Angular; [MethodImpl(MethodImplOptions.AggressiveInlining)] diff --git a/BepuPhysics/Constraints/Weld.cs b/BepuPhysics/Constraints/Weld.cs index 56633631d..0fc5ccefd 100644 --- a/BepuPhysics/Constraints/Weld.cs +++ b/BepuPhysics/Constraints/Weld.cs @@ -267,26 +267,25 @@ public void Solve2(in QuaternionWide orientationA, in BodyInertias inertiaA, in SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); var offsetBiasVelocity = positionError * positionErrorToVelocity; var orientationBiasVelocity = rotationErrorAxis * (rotationErrorLength * positionErrorToVelocity); + //offsetBiasVelocity = default; + //orientationBiasVelocity = default; //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); //csi = -accumulatedImpulse * projection.SoftnessImpulseScale - (-biasVelocity + csvaLinear + csvaAngular + csvbLinear + csvbAngular) * effectiveMass; //csi = (biasVelocity - csvaLinear - csvaAngular - csvbLinear - csvbAngular) * effectiveMass - accumulatedImpulse * projection.SoftnessImpulseScale; //csv = V * JT - //var orientationCSV = orientationBiasVelocity - (wsvA.Angular - wsvB.Angular); - //var offsetCSV = offsetBiasVelocity - (wsvA.Linear - wsvB.Linear + Cross(wsvA.Angular, offset)); + var orientationCSV = orientationBiasVelocity - (wsvA.Angular - wsvB.Angular); + var offsetCSV = offsetBiasVelocity - (wsvA.Linear - wsvB.Linear + Cross(wsvA.Angular, offset)); - Vector3Wide orientationCSV, offsetCSV; - orientationCSV.X = orientationBiasVelocity.X - wsvA.Angular.X + wsvB.Angular.X; - orientationCSV.Y = orientationBiasVelocity.Y - wsvA.Angular.Y + wsvB.Angular.Y; - orientationCSV.Z = orientationBiasVelocity.Z - wsvA.Angular.Z + wsvB.Angular.Z; + ////Unfortunately, manually inlining does actually improve the codegen meaningfully as of this writing. + //Vector3Wide orientationCSV, offsetCSV; + //orientationCSV.X = orientationBiasVelocity.X - wsvA.Angular.X + wsvB.Angular.X; + //orientationCSV.Y = orientationBiasVelocity.Y - wsvA.Angular.Y + wsvB.Angular.Y; + //orientationCSV.Z = orientationBiasVelocity.Z - wsvA.Angular.Z + wsvB.Angular.Z; - offsetCSV.X = offsetBiasVelocity.X - wsvA.Linear.X + wsvB.Linear.X - (wsvA.Angular.Y * offset.Z - wsvA.Angular.Z * offset.Y); - offsetCSV.Y = offsetBiasVelocity.Y - wsvA.Linear.Y + wsvB.Linear.Y - (wsvA.Angular.Z * offset.X - wsvA.Angular.X * offset.Z); - offsetCSV.Z = offsetBiasVelocity.Z - wsvA.Linear.Z + wsvB.Linear.Z - (wsvA.Angular.X * offset.Y - wsvA.Angular.Y * offset.X); - - //result.X = a.Y * b.Z - a.Z * b.Y; - //result.Y = a.Z * b.X - a.X * b.Z; - //result.Z = a.X * b.Y - a.Y * b.X; + //offsetCSV.X = offsetBiasVelocity.X - wsvA.Linear.X + wsvB.Linear.X - (wsvA.Angular.Y * offset.Z - wsvA.Angular.Z * offset.Y); + //offsetCSV.Y = offsetBiasVelocity.Y - wsvA.Linear.Y + wsvB.Linear.Y - (wsvA.Angular.Z * offset.X - wsvA.Angular.X * offset.Z); + //offsetCSV.Z = offsetBiasVelocity.Z - wsvA.Linear.Z + wsvB.Linear.Z - (wsvA.Angular.X * offset.Y - wsvA.Angular.Y * offset.X); //Effective mass = (J * M^-1 * JT)^-1, which is going to be a little tricky because J * M^-1 * JT is a 6x6 matrix: //J * M^-1 * JT = [ Ia^-1 + Ib^-1, Ia^-1 * transpose(skewSymmetric(localOffset * orientationA)) ] @@ -305,23 +304,13 @@ public void Solve2(in QuaternionWide orientationA, in BodyInertias inertiaA, in //Note that there is no need to invert the 6x6 inverse effective mass matrix chonk. We want to convert a constraint space velocity into a constraint space impulse, csi = csv * effectiveMass. //This is equivalent to solving csi * effectiveMass^-1 = csv for csi, and since effectiveMass^-1 is symmetric positive semidefinite, we can use an LDLT decomposition to quickly solve it. Symmetric6x6Wide.LDLTSolve(orientationCSV, offsetCSV, jmjtA, jmjtB, jmjtD, out var orientationCSI, out var offsetCSI); - //Symmetric6x6Wide.Invert(jmjtA, jmjtB, jmjtD, out var testEffectiveMass); - //Symmetric6x6Wide.TransformWithoutOverlap(orientationCSV, offsetCSV, testEffectiveMass, out var orientationCSI2, out var offsetCSI2); - - //Scale(orientationCSI, effectiveMassCFMScale, out orientationCSI); - //Scale(accumulatedImpulses.Orientation, softnessImpulseScale, out var orientationSoftness); - //Subtract(orientationCSI, orientationSoftness, out orientationCSI); - //Add(accumulatedImpulses.Orientation, orientationCSI, out accumulatedImpulses.Orientation); - //Scale(offsetCSI, effectiveMassCFMScale, out offsetCSI); - //Scale(accumulatedImpulses.Offset, softnessImpulseScale, out var offsetSoftness); - //Subtract(offsetCSI, offsetSoftness, out offsetCSI); - //Add(accumulatedImpulses.Offset, offsetCSI, out accumulatedImpulses.Offset); //orientationCSI = orientationCSI * effectiveMassCFMScale - accumulatedImpulses.Orientation * softnessImpulseScale; //offsetCSI = offsetCSI * effectiveMassCFMScale - accumulatedImpulses.Offset * softnessImpulseScale; //accumulatedImpulses.Orientation += orientationCSI; //accumulatedImpulses.Offset += offsetCSI; + //Unfortunately, manually inlining does actually improve the codegen meaningfully as of this writing. orientationCSI.X = orientationCSI.X * effectiveMassCFMScale - accumulatedImpulses.Orientation.X * softnessImpulseScale; orientationCSI.Y = orientationCSI.Y * effectiveMassCFMScale - accumulatedImpulses.Orientation.Y * softnessImpulseScale; orientationCSI.Z = orientationCSI.Z * effectiveMassCFMScale - accumulatedImpulses.Orientation.Z * softnessImpulseScale; diff --git a/Demos/Program.cs b/Demos/Program.cs index 6c769ece5..ba902a5cf 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -19,7 +19,7 @@ static void Main(string[] args) { content = ContentArchive.Load(stream); } - HeadlessTest.Test(content, 4, 32, 512); + HeadlessTest.Test(content, 2, 32, 512); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); From 4e5bb15625b75fc76b98805c45a77b2483ab49c6 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 3 Jul 2021 21:11:56 -0500 Subject: [PATCH 072/947] Fixed same bug, in gather. --- BepuPhysics/Bodies_GatherScatter.cs | 6 +++--- Demos/Demos/NewtDemo.cs | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/BepuPhysics/Bodies_GatherScatter.cs b/BepuPhysics/Bodies_GatherScatter.cs index b4ef19327..807870ee8 100644 --- a/BepuPhysics/Bodies_GatherScatter.cs +++ b/BepuPhysics/Bodies_GatherScatter.cs @@ -258,9 +258,9 @@ unsafe static void TransposingGather(int count, MotionState* motionStates, ref V velocity.Linear.X = Avx.Permute2x128(o0, o1, 0 | (2 << 4)).AsVector(); velocity.Linear.Y = Avx.Permute2x128(o4, o5, 0 | (2 << 4)).AsVector(); velocity.Linear.Z = Avx.Permute2x128(o2, o3, 0 | (2 << 4)).AsVector(); - velocity.Angular.X = Avx.Permute2x128(o6, o7, 0 | (2 << 4)).AsVector(); - velocity.Angular.Y = Avx.Permute2x128(o0, o1, 1 | (3 << 4)).AsVector(); - velocity.Angular.Z = Avx.Permute2x128(o4, o5, 1 | (3 << 4)).AsVector(); + velocity.Angular.X = Avx.Permute2x128(o0, o1, 1 | (3 << 4)).AsVector(); + velocity.Angular.Y = Avx.Permute2x128(o4, o5, 1 | (3 << 4)).AsVector(); + velocity.Angular.Z = Avx.Permute2x128(o2, o3, 1 | (3 << 4)).AsVector(); } } else diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index 4f78f6821..b8731d7bc 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -823,7 +823,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper(1)); - Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3)); + Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)), new EmbeddedSubsteppingTimestepper2(3)); //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper2(), solverIterationCount: 5); //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper2(3), solverIterationCount: 1); From fafb06c819f1663eb6d5cdf8e4771f1ab7a7e81c Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 3 Jul 2021 22:02:11 -0500 Subject: [PATCH 073/947] BallSocket partial 2ification. --- BepuPhysics/Constraints/BallSocket.cs | 31 +++++++++++++++++-- BepuPhysics/Constraints/BallSocketMotor.cs | 2 +- BepuPhysics/Constraints/BallSocketServo.cs | 2 +- BepuPhysics/Constraints/BallSocketShared.cs | 18 +++++------ .../Constraints/TwoBodyTypeProcessor.cs | 12 ++++++- BepuPhysics/Constraints/Weld.cs | 2 +- 6 files changed, 52 insertions(+), 15 deletions(-) diff --git a/BepuPhysics/Constraints/BallSocket.cs b/BepuPhysics/Constraints/BallSocket.cs index 463328315..9ef6b58a8 100644 --- a/BepuPhysics/Constraints/BallSocket.cs +++ b/BepuPhysics/Constraints/BallSocket.cs @@ -100,13 +100,40 @@ public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref BallSocketProjection projection, ref Vector3Wide accumulatedImpulse) { - BallSocketShared.ApplyImpulse(ref velocityA, ref velocityB, ref projection.OffsetA, ref projection.OffsetB, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulse); + BallSocketShared.ApplyImpulse(ref velocityA, ref velocityB, projection.OffsetA, projection.OffsetB, projection.InertiaA, projection.InertiaB, accumulatedImpulse); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref BallSocketProjection projection, ref Vector3Wide accumulatedImpulse) { - BallSocketShared.Solve(ref velocityA, ref velocityB, ref projection.OffsetA, ref projection.OffsetB, ref projection.BiasVelocity, ref projection.EffectiveMass, ref projection.SoftnessImpulseScale, ref accumulatedImpulse, ref projection.InertiaA, ref projection.InertiaB); + BallSocketShared.Solve(ref velocityA, ref velocityB, projection.OffsetA, projection.OffsetB, projection.BiasVelocity, projection.EffectiveMass, projection.SoftnessImpulseScale, ref accumulatedImpulse, projection.InertiaA, projection.InertiaB); + } + + public void WarmStart2(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + in BallSocketPrestepData prestep, in Vector3Wide accumulatedImpulses, ref BodyVelocities wsvA, ref BodyVelocities wsvB) + { + //Note that we must reconstruct the world offsets from the body orientations since we do not store world offsets. + QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetA, orientationA, out var offsetA); + QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetB, orientationB, out var offsetB); + BallSocketShared.ApplyImpulse(ref wsvA, ref wsvB, offsetA, offsetB, inertiaA, inertiaB, accumulatedImpulses); + } + + public void Solve2(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, float dt, float inverseDt, + in BallSocketPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocities wsvA, ref BodyVelocities wsvB) + { + //Note that we must reconstruct the world offsets from the body orientations since we do not store world offsets. + QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetA, orientationA, out var offsetA); + QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetB, orientationB, out var offsetB); + SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); + BallSocketShared.ComputeEffectiveMass(inertiaA, inertiaB, ref offsetA, ref offsetB, ref effectiveMassCFMScale, out var effectiveMass); + + //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. + Vector3Wide.Add(ab, offsetB, out var anchorB); + Vector3Wide.Subtract(anchorB, offsetA, out var error); + Vector3Wide.Scale(error, positionErrorToVelocity, out var biasVelocity); + + BallSocketShared.Solve(ref wsvA, ref wsvB, offsetA, offsetB, biasVelocity, effectiveMass, softnessImpulseScale, ref accumulatedImpulses, inertiaA, inertiaB); + } } diff --git a/BepuPhysics/Constraints/BallSocketMotor.cs b/BepuPhysics/Constraints/BallSocketMotor.cs index 08ae3730c..9fff482ec 100644 --- a/BepuPhysics/Constraints/BallSocketMotor.cs +++ b/BepuPhysics/Constraints/BallSocketMotor.cs @@ -102,7 +102,7 @@ public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref BallSocketMotorProjection projection, ref Vector3Wide accumulatedImpulse) { - BallSocketShared.ApplyImpulse(ref velocityA, ref velocityB, ref projection.OffsetA, ref projection.OffsetB, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulse); + BallSocketShared.ApplyImpulse(ref velocityA, ref velocityB, projection.OffsetA, projection.OffsetB, projection.InertiaA, projection.InertiaB, accumulatedImpulse); } [MethodImpl(MethodImplOptions.AggressiveInlining)] diff --git a/BepuPhysics/Constraints/BallSocketServo.cs b/BepuPhysics/Constraints/BallSocketServo.cs index 2373f1123..8ef15c2d0 100644 --- a/BepuPhysics/Constraints/BallSocketServo.cs +++ b/BepuPhysics/Constraints/BallSocketServo.cs @@ -109,7 +109,7 @@ public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref BallSocketServoProjection projection, ref Vector3Wide accumulatedImpulse) { - BallSocketShared.ApplyImpulse(ref velocityA, ref velocityB, ref projection.OffsetA, ref projection.OffsetB, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulse); + BallSocketShared.ApplyImpulse(ref velocityA, ref velocityB, projection.OffsetA, projection.OffsetB, projection.InertiaA, projection.InertiaB, accumulatedImpulse); } [MethodImpl(MethodImplOptions.AggressiveInlining)] diff --git a/BepuPhysics/Constraints/BallSocketShared.cs b/BepuPhysics/Constraints/BallSocketShared.cs index bde78daa5..df00d8572 100644 --- a/BepuPhysics/Constraints/BallSocketShared.cs +++ b/BepuPhysics/Constraints/BallSocketShared.cs @@ -75,7 +75,7 @@ public static void ComputeEffectiveMass(in BodyInertias inertiaA, in BodyInertia [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ApplyImpulse(ref BodyVelocities velocityA, ref BodyVelocities velocityB, - ref Vector3Wide offsetA, ref Vector3Wide offsetB, ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref Vector3Wide constraintSpaceImpulse) + in Vector3Wide offsetA, in Vector3Wide offsetB, in BodyInertias inertiaA, in BodyInertias inertiaB, in Vector3Wide constraintSpaceImpulse) { Vector3Wide.CrossWithoutOverlap(offsetA, constraintSpaceImpulse, out var wsi); Symmetric3x3Wide.TransformWithoutOverlap(wsi, inertiaA.InverseInertiaTensor, out var change); @@ -93,8 +93,8 @@ public static void ApplyImpulse(ref BodyVelocities velocityA, ref BodyVelocities } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ComputeCorrectiveImpulse(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref Vector3Wide offsetA, ref Vector3Wide offsetB, - ref Vector3Wide biasVelocity, ref Symmetric3x3Wide effectiveMass, ref Vector softnessImpulseScale, ref Vector3Wide accumulatedImpulse, out Vector3Wide correctiveImpulse) + public static void ComputeCorrectiveImpulse(ref BodyVelocities velocityA, ref BodyVelocities velocityB, in Vector3Wide offsetA, in Vector3Wide offsetB, + in Vector3Wide biasVelocity, in Symmetric3x3Wide effectiveMass, in Vector softnessImpulseScale, in Vector3Wide accumulatedImpulse, out Vector3Wide correctiveImpulse) { //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); //Note subtraction; jLinearB = -I. @@ -112,25 +112,25 @@ public static void ComputeCorrectiveImpulse(ref BodyVelocities velocityA, ref Bo } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref Vector3Wide offsetA, ref Vector3Wide offsetB, - ref Vector3Wide biasVelocity, ref Symmetric3x3Wide effectiveMass, ref Vector softnessImpulseScale, ref Vector3Wide accumulatedImpulse, ref BodyInertias inertiaA, ref BodyInertias inertiaB) + public static void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, in Vector3Wide offsetA, in Vector3Wide offsetB, + in Vector3Wide biasVelocity, in Symmetric3x3Wide effectiveMass, in Vector softnessImpulseScale, ref Vector3Wide accumulatedImpulse, in BodyInertias inertiaA, in BodyInertias inertiaB) { - ComputeCorrectiveImpulse(ref velocityA, ref velocityB, ref offsetA, ref offsetB, ref biasVelocity, ref effectiveMass, ref softnessImpulseScale, ref accumulatedImpulse, out var correctiveImpulse); + ComputeCorrectiveImpulse(ref velocityA, ref velocityB, offsetA, offsetB, biasVelocity, effectiveMass, softnessImpulseScale, accumulatedImpulse, out var correctiveImpulse); //This function does not have a maximum impulse limit, so no clamping is required. Vector3Wide.Add(accumulatedImpulse, correctiveImpulse, out accumulatedImpulse); - ApplyImpulse(ref velocityA, ref velocityB, ref offsetA, ref offsetB, ref inertiaA, ref inertiaB, ref correctiveImpulse); + ApplyImpulse(ref velocityA, ref velocityB, offsetA, offsetB, inertiaA, inertiaB, correctiveImpulse); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref Vector3Wide offsetA, ref Vector3Wide offsetB, ref Vector3Wide biasVelocity, ref Symmetric3x3Wide effectiveMass, ref Vector softnessImpulseScale, ref Vector maximumImpulse, ref Vector3Wide accumulatedImpulse, ref BodyInertias inertiaA, ref BodyInertias inertiaB) { - ComputeCorrectiveImpulse(ref velocityA, ref velocityB, ref offsetA, ref offsetB, ref biasVelocity, ref effectiveMass, ref softnessImpulseScale, ref accumulatedImpulse, out var correctiveImpulse); + ComputeCorrectiveImpulse(ref velocityA, ref velocityB, offsetA, offsetB, biasVelocity, effectiveMass, softnessImpulseScale, accumulatedImpulse, out var correctiveImpulse); //This function DOES have a maximum impulse limit. ServoSettingsWide.ClampImpulse(maximumImpulse, ref accumulatedImpulse, ref correctiveImpulse); - ApplyImpulse(ref velocityA, ref velocityB, ref offsetA, ref offsetB, ref inertiaA, ref inertiaB, ref correctiveImpulse); + ApplyImpulse(ref velocityA, ref velocityB, offsetA, offsetB, inertiaA, inertiaB, correctiveImpulse); } } diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 013ebdd26..8774bb129 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -360,9 +360,14 @@ public unsafe override void WarmStart2(ref TypeBatch typeBatch, Bodies bodies, f bodies.GatherState(ref references, count, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB); if (typeof(TConstraintFunctions) == typeof(WeldFunctions)) { - default(WeldFunctions).WarmStart2(orientationA, inertiaA, ab, orientationB, inertiaB, dt, inverseDt, + default(WeldFunctions).WarmStart2(orientationA, inertiaA, ab, orientationB, inertiaB, Unsafe.As(ref prestep), Unsafe.As(ref accumulatedImpulses), ref wsvA, ref wsvB); } + else if(typeof(TConstraintFunctions) == typeof(BallSocketFunctions)) + { + default(BallSocketFunctions).WarmStart2(orientationA, inertiaA, ab, orientationB, inertiaB, + Unsafe.As(ref prestep), Unsafe.As(ref accumulatedImpulses), ref wsvA, ref wsvB); + } else { function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, dt, inverseDt, ref prestep, out var projection); @@ -392,6 +397,11 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f default(WeldFunctions).Solve2(orientationA, inertiaA, ab, orientationB, inertiaB, dt, inverseDt, Unsafe.As(ref prestep), ref Unsafe.As(ref accumulatedImpulses), ref wsvA, ref wsvB); } + else if (typeof(TConstraintFunctions) == typeof(BallSocketFunctions)) + { + default(BallSocketFunctions).Solve2(orientationA, inertiaA, ab, orientationB, inertiaB, dt, inverseDt, + Unsafe.As(ref prestep), ref Unsafe.As(ref accumulatedImpulses), ref wsvA, ref wsvB); + } else { function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, dt, inverseDt, ref prestep, out var projection); diff --git a/BepuPhysics/Constraints/Weld.cs b/BepuPhysics/Constraints/Weld.cs index 0fc5ccefd..95b3a48fb 100644 --- a/BepuPhysics/Constraints/Weld.cs +++ b/BepuPhysics/Constraints/Weld.cs @@ -230,7 +230,7 @@ private static void ApplyImpulse(in BodyInertias inertiaA, in BodyInertias inert } //[MethodImpl(MethodImplOptions.NoInlining)] - public void WarmStart2(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, float dt, float inverseDt, + public void WarmStart2(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, in WeldPrestepData prestep, in WeldAccumulatedImpulses accumulatedImpulses, ref BodyVelocities wsvA, ref BodyVelocities wsvB) { Transform(prestep.LocalOffset, orientationA, out var offset); From e40222c8c5e105cca94ea6fc65ab92b202fa0865 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 4 Jul 2021 13:14:09 -0500 Subject: [PATCH 074/947] World inertia gather. --- BepuPhysics/Bodies_GatherScatter.cs | 202 +++++++++++------- BepuPhysics/BodyProperties.cs | 1 + .../Constraints/TwoBodyTypeProcessor.cs | 34 +-- Demos/Demo.cs | 2 +- Demos/Demos/NewtDemo.cs | 4 +- 5 files changed, 150 insertions(+), 93 deletions(-) diff --git a/BepuPhysics/Bodies_GatherScatter.cs b/BepuPhysics/Bodies_GatherScatter.cs index 807870ee8..8fb8e9a08 100644 --- a/BepuPhysics/Bodies_GatherScatter.cs +++ b/BepuPhysics/Bodies_GatherScatter.cs @@ -153,36 +153,128 @@ unsafe static void ScalarGather(int count, MotionState* motionStates, ref Vector } } //[MethodImpl(MethodImplOptions.AggressiveInlining)] - unsafe static void TransposingGather(int count, MotionState* motionStates, ref Vector baseIndex, ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocities velocity, ref BodyInertias inertia) + unsafe static void TransposingGather(int count, MotionState* motionStates, BodyInertia* inertias, ref Vector baseIndex, ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocities velocity, ref BodyInertias inertia) { if (Avx.IsSupported) { var indices = (int*)Unsafe.AsPointer(ref baseIndex); - var s0 = (float*)(motionStates + indices[0]); - var s1 = (float*)(motionStates + indices[1]); - var s2 = (float*)(motionStates + indices[2]); - var s3 = (float*)(motionStates + indices[3]); - var s4 = (float*)(motionStates + indices[4]); - var s5 = (float*)(motionStates + indices[5]); - var s6 = (float*)(motionStates + indices[6]); - var s7 = (float*)(motionStates + indices[7]); - - //for (int i = 0; i < 8; ++i) - //{ - // s0[i] = i; - // s1[i] = i + 100; - // s2[i] = i + 200; - // s3[i] = i + 300; - // s4[i] = i + 400; - // s5[i] = i + 500; - // s6[i] = i + 600; - // s7[i] = i + 700; - //} - { - //Load every body for the first half of the motion state. - //Note that buffers are allocated on cache line boundaries, so we can use aligned loads for all that matters. + var s0 = (float*)(motionStates + indices[0]); + var s1 = (float*)(motionStates + indices[1]); + var s2 = (float*)(motionStates + indices[2]); + var s3 = (float*)(motionStates + indices[3]); + var s4 = (float*)(motionStates + indices[4]); + var s5 = (float*)(motionStates + indices[5]); + var s6 = (float*)(motionStates + indices[6]); + var s7 = (float*)(motionStates + indices[7]); + + //for (int i = 0; i < 8; ++i) + //{ + // s0[i] = i; + // s1[i] = i + 100; + // s2[i] = i + 200; + // s3[i] = i + 300; + // s4[i] = i + 400; + // s5[i] = i + 500; + // s6[i] = i + 600; + // s7[i] = i + 700; + //} + + { + //Load every body for the first half of the motion state. + //Note that buffers are allocated on cache line boundaries, so we can use aligned loads for all that matters. + var m0 = Avx.LoadVector256(s0); + var m1 = count > 1 ? Avx.LoadAlignedVector256(s1) : Vector256.Zero; + var m2 = count > 2 ? Avx.LoadAlignedVector256(s2) : Vector256.Zero; + var m3 = count > 3 ? Avx.LoadAlignedVector256(s3) : Vector256.Zero; + var m4 = count > 4 ? Avx.LoadAlignedVector256(s4) : Vector256.Zero; + var m5 = count > 5 ? Avx.LoadAlignedVector256(s5) : Vector256.Zero; + var m6 = count > 6 ? Avx.LoadAlignedVector256(s6) : Vector256.Zero; + var m7 = count > 7 ? Avx.LoadAlignedVector256(s7) : Vector256.Zero; + + var n0 = Avx.UnpackLow(m0, m1); + var n1 = Avx.UnpackLow(m2, m3); + var n2 = Avx.UnpackLow(m4, m5); + var n3 = Avx.UnpackLow(m6, m7); + var n4 = Avx.UnpackHigh(m0, m1); + var n5 = Avx.UnpackHigh(m2, m3); + var n6 = Avx.UnpackHigh(m4, m5); + var n7 = Avx.UnpackHigh(m6, m7); + + var o0 = Avx.Shuffle(n0, n1, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o1 = Avx.Shuffle(n2, n3, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o2 = Avx.Shuffle(n4, n5, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o3 = Avx.Shuffle(n6, n7, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o4 = Avx.Shuffle(n0, n1, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o5 = Avx.Shuffle(n2, n3, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o6 = Avx.Shuffle(n4, n5, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o7 = Avx.Shuffle(n6, n7, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + + orientation.X = Avx.Permute2x128(o0, o1, 0 | (2 << 4)).AsVector(); + orientation.Y = Avx.Permute2x128(o4, o5, 0 | (2 << 4)).AsVector(); + orientation.Z = Avx.Permute2x128(o2, o3, 0 | (2 << 4)).AsVector(); + orientation.W = Avx.Permute2x128(o6, o7, 0 | (2 << 4)).AsVector(); + position.X = Avx.Permute2x128(o0, o1, 1 | (3 << 4)).AsVector(); + position.Y = Avx.Permute2x128(o4, o5, 1 | (3 << 4)).AsVector(); + position.Z = Avx.Permute2x128(o2, o3, 1 | (3 << 4)).AsVector(); + //inertia.InverseMass = Avx.Permute2x128(o6, o7, 1 | (3 << 4)).AsVector(); + //inertia.InverseInertiaTensor.XX = inertia.InverseMass; + //inertia.InverseInertiaTensor.YY = inertia.InverseMass; + //inertia.InverseInertiaTensor.ZZ = inertia.InverseMass; + //inertia.InverseInertiaTensor.YX = default; + //inertia.InverseInertiaTensor.ZX = default; + //inertia.InverseInertiaTensor.ZY = default; + } + + { + //Second half. + var m0 = Avx.LoadVector256(s0 + 8); + var m1 = count > 1 ? Avx.LoadAlignedVector256(s1 + 8) : Vector256.Zero; + var m2 = count > 2 ? Avx.LoadAlignedVector256(s2 + 8) : Vector256.Zero; + var m3 = count > 3 ? Avx.LoadAlignedVector256(s3 + 8) : Vector256.Zero; + var m4 = count > 4 ? Avx.LoadAlignedVector256(s4 + 8) : Vector256.Zero; + var m5 = count > 5 ? Avx.LoadAlignedVector256(s5 + 8) : Vector256.Zero; + var m6 = count > 6 ? Avx.LoadAlignedVector256(s6 + 8) : Vector256.Zero; + var m7 = count > 7 ? Avx.LoadAlignedVector256(s7 + 8) : Vector256.Zero; + + var n0 = Avx.UnpackLow(m0, m1); + var n1 = Avx.UnpackLow(m2, m3); + var n2 = Avx.UnpackLow(m4, m5); + var n3 = Avx.UnpackLow(m6, m7); + var n4 = Avx.UnpackHigh(m0, m1); + var n5 = Avx.UnpackHigh(m2, m3); + var n6 = Avx.UnpackHigh(m4, m5); + var n7 = Avx.UnpackHigh(m6, m7); + + var o0 = Avx.Shuffle(n0, n1, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o1 = Avx.Shuffle(n2, n3, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o2 = Avx.Shuffle(n4, n5, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o3 = Avx.Shuffle(n6, n7, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o4 = Avx.Shuffle(n0, n1, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o5 = Avx.Shuffle(n2, n3, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o6 = Avx.Shuffle(n4, n5, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o7 = Avx.Shuffle(n6, n7, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + + velocity.Linear.X = Avx.Permute2x128(o0, o1, 0 | (2 << 4)).AsVector(); + velocity.Linear.Y = Avx.Permute2x128(o4, o5, 0 | (2 << 4)).AsVector(); + velocity.Linear.Z = Avx.Permute2x128(o2, o3, 0 | (2 << 4)).AsVector(); + velocity.Angular.X = Avx.Permute2x128(o0, o1, 1 | (3 << 4)).AsVector(); + velocity.Angular.Y = Avx.Permute2x128(o4, o5, 1 | (3 << 4)).AsVector(); + velocity.Angular.Z = Avx.Permute2x128(o2, o3, 1 | (3 << 4)).AsVector(); + } + } + { + var s0 = (float*)(inertias + indices[0]); + var s1 = (float*)(inertias + indices[1]); + var s2 = (float*)(inertias + indices[2]); + var s3 = (float*)(inertias + indices[3]); + var s4 = (float*)(inertias + indices[4]); + var s5 = (float*)(inertias + indices[5]); + var s6 = (float*)(inertias + indices[6]); + var s7 = (float*)(inertias + indices[7]); + + //Load every inertia vector. var m0 = Avx.LoadVector256(s0); var m1 = count > 1 ? Avx.LoadAlignedVector256(s1) : Vector256.Zero; var m2 = count > 2 ? Avx.LoadAlignedVector256(s2) : Vector256.Zero; @@ -210,57 +302,13 @@ unsafe static void TransposingGather(int count, MotionState* motionStates, ref V var o6 = Avx.Shuffle(n4, n5, 2 | (3 << 2) | (2 << 4) | (3 << 6)); var o7 = Avx.Shuffle(n6, n7, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - orientation.X = Avx.Permute2x128(o0, o1, 0 | (2 << 4)).AsVector(); - orientation.Y = Avx.Permute2x128(o4, o5, 0 | (2 << 4)).AsVector(); - orientation.Z = Avx.Permute2x128(o2, o3, 0 | (2 << 4)).AsVector(); - orientation.W = Avx.Permute2x128(o6, o7, 0 | (2 << 4)).AsVector(); - position.X = Avx.Permute2x128(o0, o1, 1 | (3 << 4)).AsVector(); - position.Y = Avx.Permute2x128(o4, o5, 1 | (3 << 4)).AsVector(); - position.Z = Avx.Permute2x128(o2, o3, 1 | (3 << 4)).AsVector(); - inertia.InverseMass = Avx.Permute2x128(o6, o7, 1 | (3 << 4)).AsVector(); - inertia.InverseInertiaTensor.XX = inertia.InverseMass; - inertia.InverseInertiaTensor.YY = inertia.InverseMass; - inertia.InverseInertiaTensor.ZZ = inertia.InverseMass; - inertia.InverseInertiaTensor.YX = default; - inertia.InverseInertiaTensor.ZX = default; - inertia.InverseInertiaTensor.ZY = default; - } - - { - //Second half. - var m0 = Avx.LoadVector256(s0 + 8); - var m1 = count > 1 ? Avx.LoadAlignedVector256(s1 + 8) : Vector256.Zero; - var m2 = count > 2 ? Avx.LoadAlignedVector256(s2 + 8) : Vector256.Zero; - var m3 = count > 3 ? Avx.LoadAlignedVector256(s3 + 8) : Vector256.Zero; - var m4 = count > 4 ? Avx.LoadAlignedVector256(s4 + 8) : Vector256.Zero; - var m5 = count > 5 ? Avx.LoadAlignedVector256(s5 + 8) : Vector256.Zero; - var m6 = count > 6 ? Avx.LoadAlignedVector256(s6 + 8) : Vector256.Zero; - var m7 = count > 7 ? Avx.LoadAlignedVector256(s7 + 8) : Vector256.Zero; - - var n0 = Avx.UnpackLow(m0, m1); - var n1 = Avx.UnpackLow(m2, m3); - var n2 = Avx.UnpackLow(m4, m5); - var n3 = Avx.UnpackLow(m6, m7); - var n4 = Avx.UnpackHigh(m0, m1); - var n5 = Avx.UnpackHigh(m2, m3); - var n6 = Avx.UnpackHigh(m4, m5); - var n7 = Avx.UnpackHigh(m6, m7); - - var o0 = Avx.Shuffle(n0, n1, 0 | (1 << 2) | (0 << 4) | (1 << 6)); - var o1 = Avx.Shuffle(n2, n3, 0 | (1 << 2) | (0 << 4) | (1 << 6)); - var o2 = Avx.Shuffle(n4, n5, 0 | (1 << 2) | (0 << 4) | (1 << 6)); - var o3 = Avx.Shuffle(n6, n7, 0 | (1 << 2) | (0 << 4) | (1 << 6)); - var o4 = Avx.Shuffle(n0, n1, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - var o5 = Avx.Shuffle(n2, n3, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - var o6 = Avx.Shuffle(n4, n5, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - var o7 = Avx.Shuffle(n6, n7, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - - velocity.Linear.X = Avx.Permute2x128(o0, o1, 0 | (2 << 4)).AsVector(); - velocity.Linear.Y = Avx.Permute2x128(o4, o5, 0 | (2 << 4)).AsVector(); - velocity.Linear.Z = Avx.Permute2x128(o2, o3, 0 | (2 << 4)).AsVector(); - velocity.Angular.X = Avx.Permute2x128(o0, o1, 1 | (3 << 4)).AsVector(); - velocity.Angular.Y = Avx.Permute2x128(o4, o5, 1 | (3 << 4)).AsVector(); - velocity.Angular.Z = Avx.Permute2x128(o2, o3, 1 | (3 << 4)).AsVector(); + inertia.InverseInertiaTensor.XX = Avx.Permute2x128(o0, o1, 0 | (2 << 4)).AsVector(); + inertia.InverseInertiaTensor.YX = Avx.Permute2x128(o4, o5, 0 | (2 << 4)).AsVector(); + inertia.InverseInertiaTensor.YY = Avx.Permute2x128(o2, o3, 0 | (2 << 4)).AsVector(); + inertia.InverseInertiaTensor.ZX = Avx.Permute2x128(o6, o7, 0 | (2 << 4)).AsVector(); + inertia.InverseInertiaTensor.ZY = Avx.Permute2x128(o0, o1, 1 | (3 << 4)).AsVector(); + inertia.InverseInertiaTensor.ZZ = Avx.Permute2x128(o4, o5, 1 | (3 << 4)).AsVector(); + inertia.InverseMass = Avx.Permute2x128(o2, o3, 1 | (3 << 4)).AsVector(); } } else @@ -302,8 +350,8 @@ public unsafe void GatherState(ref TwoBodyReferences references, int count, ref var states = ref ActiveSet.MotionStates; - TransposingGather(count, states.Memory, ref references.IndexA, ref positionA, ref orientationA, ref velocityA, ref inertiaA); - TransposingGather(count, states.Memory, ref references.IndexB, ref positionB, ref orientationB, ref velocityB, ref inertiaB); + TransposingGather(count, states.Memory, Inertias.Memory, ref references.IndexA, ref positionA, ref orientationA, ref velocityA, ref inertiaA); + TransposingGather(count, states.Memory, Inertias.Memory, ref references.IndexB, ref positionB, ref orientationB, ref velocityB, ref inertiaB); //for (int i = 0; i < count; ++i) //{ diff --git a/BepuPhysics/BodyProperties.cs b/BepuPhysics/BodyProperties.cs index 04baa534a..bdf721f57 100644 --- a/BepuPhysics/BodyProperties.cs +++ b/BepuPhysics/BodyProperties.cs @@ -218,6 +218,7 @@ public BodyVelocity(in Vector3 linear, in Vector3 angular) } } + [StructLayout(LayoutKind.Sequential, Size = 32)] public struct BodyInertia { public Symmetric3x3 InverseInertiaTensor; diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 8774bb129..d8cbdc9b8 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -309,36 +309,42 @@ static unsafe void Prefetch(void* address) //TODO: ARM? } [MethodImpl(MethodImplOptions.AggressiveInlining)] - static unsafe void PrefetchBundle(MotionState* baseAddress, ref TwoBodyReferences references, int countInBundle) + static unsafe void PrefetchBundle(MotionState* motionBase, BodyInertia* inertiaBase, ref TwoBodyReferences references, int countInBundle) { + var indicesA = (int*)Unsafe.AsPointer(ref references.IndexA); + var indicesB = (int*)Unsafe.AsPointer(ref references.IndexB); for (int i = 0; i < countInBundle; ++i) { - Prefetch(baseAddress + references.IndexA[i]); - Prefetch(baseAddress + references.IndexB[i]); + var indexA = indicesA[i]; + var indexB = indicesA[i]; + Prefetch(motionBase + indexA); + Prefetch(inertiaBase + indexA); + Prefetch(motionBase + indexB); + Prefetch(inertiaBase + indexB); } } const int prefetchDistance = 8; [MethodImpl(MethodImplOptions.AggressiveInlining)] - unsafe static void EarlyPrefetch(ref TypeBatch typeBatch, ref Buffer references, ref Buffer states, int startBundleIndex, int exclusiveEndBundleIndex) + unsafe static void EarlyPrefetch(ref TypeBatch typeBatch, ref Buffer references, ref Buffer states, ref Buffer inertias, int startBundleIndex, int exclusiveEndBundleIndex) { exclusiveEndBundleIndex = Math.Min(exclusiveEndBundleIndex, startBundleIndex + prefetchDistance); var lastBundleIndex = exclusiveEndBundleIndex - 1; for (int i = startBundleIndex; i < lastBundleIndex; ++i) { - PrefetchBundle(states.Memory, ref references[i], Vector.Count); + PrefetchBundle(states.Memory, inertias.Memory, ref references[i], Vector.Count); } var countInBundle = GetCountInBundle(ref typeBatch, lastBundleIndex); - PrefetchBundle(states.Memory, ref references[lastBundleIndex], countInBundle); + PrefetchBundle(states.Memory, inertias.Memory, ref references[lastBundleIndex], countInBundle); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - unsafe static void Prefetch(ref TypeBatch typeBatch, ref Buffer references, ref Buffer states, int bundleIndex, int exclusiveEndBundleIndex) + unsafe static void Prefetch(ref TypeBatch typeBatch, ref Buffer references, ref Buffer states, ref Buffer inertias, int bundleIndex, int exclusiveEndBundleIndex) { var targetIndex = bundleIndex + prefetchDistance; if (targetIndex < exclusiveEndBundleIndex) { - PrefetchBundle(states.Memory, ref references[targetIndex], GetCountInBundle(ref typeBatch, targetIndex)); + PrefetchBundle(states.Memory, inertias.Memory, ref references[targetIndex], GetCountInBundle(ref typeBatch, targetIndex)); } } @@ -349,21 +355,22 @@ public unsafe override void WarmStart2(ref TypeBatch typeBatch, Bodies bodies, f var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); var function = default(TConstraintFunctions); ref var motionStates = ref bodies.ActiveSet.MotionStates; - EarlyPrefetch(ref typeBatch, ref bodyReferencesBundles, ref motionStates, startBundle, exclusiveEndBundle); + ref var inertias = ref bodies.Inertias; + EarlyPrefetch(ref typeBatch, ref bodyReferencesBundles, ref motionStates, ref inertias, startBundle, exclusiveEndBundle); for (int i = startBundle; i < exclusiveEndBundle; ++i) { ref var prestep = ref prestepBundles[i]; ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; ref var references = ref bodyReferencesBundles[i]; var count = GetCountInBundle(ref typeBatch, i); - Prefetch(ref typeBatch, ref bodyReferencesBundles, ref motionStates, i, exclusiveEndBundle); + Prefetch(ref typeBatch, ref bodyReferencesBundles, ref motionStates, ref inertias, i, exclusiveEndBundle); bodies.GatherState(ref references, count, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB); if (typeof(TConstraintFunctions) == typeof(WeldFunctions)) { default(WeldFunctions).WarmStart2(orientationA, inertiaA, ab, orientationB, inertiaB, Unsafe.As(ref prestep), Unsafe.As(ref accumulatedImpulses), ref wsvA, ref wsvB); } - else if(typeof(TConstraintFunctions) == typeof(BallSocketFunctions)) + else if (typeof(TConstraintFunctions) == typeof(BallSocketFunctions)) { default(BallSocketFunctions).WarmStart2(orientationA, inertiaA, ab, orientationB, inertiaB, Unsafe.As(ref prestep), Unsafe.As(ref accumulatedImpulses), ref wsvA, ref wsvB); @@ -383,14 +390,15 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); var function = default(TConstraintFunctions); ref var motionStates = ref bodies.ActiveSet.MotionStates; - EarlyPrefetch(ref typeBatch, ref bodyReferencesBundles, ref motionStates, startBundle, exclusiveEndBundle); + ref var inertias = ref bodies.Inertias; + EarlyPrefetch(ref typeBatch, ref bodyReferencesBundles, ref motionStates, ref inertias, startBundle, exclusiveEndBundle); for (int i = startBundle; i < exclusiveEndBundle; ++i) { ref var prestep = ref prestepBundles[i]; ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; ref var references = ref bodyReferencesBundles[i]; var count = GetCountInBundle(ref typeBatch, i); - Prefetch(ref typeBatch, ref bodyReferencesBundles, ref motionStates, i, exclusiveEndBundle); + Prefetch(ref typeBatch, ref bodyReferencesBundles, ref motionStates, ref inertias, i, exclusiveEndBundle); bodies.GatherState(ref references, count, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB); if (typeof(TConstraintFunctions) == typeof(WeldFunctions)) { diff --git a/Demos/Demo.cs b/Demos/Demo.cs index f00656836..aba8044d8 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -62,7 +62,7 @@ public virtual void Update(Window window, Camera camera, Input input, float dt) //fully decouple simulation and rendering rates across different threads. //(In either case, you'd also want to interpolate or extrapolate simulation results during rendering for smoothness.) //Note that taking steps of variable length can reduce stability. Gradual or one-off changes can work reasonably well. - Simulation.Timestep(1 / 60f, ThreadDispatcher); + Simulation.Timestep(1 / 60f); ////Here's an example of how it would look to use more frequent updates, but still with a fixed amount of time simulated per update call: //const float timeToSimulate = 1 / 60f; diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index b8731d7bc..0385085c7 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -823,8 +823,8 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper(1)); - Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)), new EmbeddedSubsteppingTimestepper2(3)); - //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper2(), solverIterationCount: 5); + //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)), new EmbeddedSubsteppingTimestepper2(3)); + Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)), new PositionFirstTimestepper2(), solverIterationCount: 5); //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper2(3), solverIterationCount: 1); var meshContent = content.Load("Content\\newt.obj"); From cb56fe94f0204eb0db10059aa9d3dc3ba2a090e8 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 4 Jul 2021 13:23:56 -0500 Subject: [PATCH 075/947] Docufix. --- BepuUtilities/Symmetric3x3.cs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/BepuUtilities/Symmetric3x3.cs b/BepuUtilities/Symmetric3x3.cs index 822866d1e..56f7e1306 100644 --- a/BepuUtilities/Symmetric3x3.cs +++ b/BepuUtilities/Symmetric3x3.cs @@ -38,11 +38,11 @@ public struct Symmetric3x3 public float ZZ; /// - /// Computes rT * m * r for a symmetric matrix m and a rotation matrix R. + /// Computes rT * m * r for a symmetric matrix m and a rotation matrix r. /// /// Rotation matrix to use as the sandwich bread. /// Succulent interior symmetric matrix. - /// Result of v * m * transpose(v) for a symmetric matrix m. + /// Result of transpose(r) * m * r. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void RotationSandwich(in Matrix3x3 r, in Symmetric3x3 m, out Symmetric3x3 sandwich) { From 928c404155a47975ea7774a33538412c013742a3 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 4 Jul 2021 18:02:58 -0500 Subject: [PATCH 076/947] Disabled incremental body/constraint optimization now that the solver benefits less from ordering. Shrank work block count in solver. --- BepuPhysics/BatchCompressor.cs | 2 +- .../Constraints/TwoBodyTypeProcessor.cs | 15 ++++++----- BepuPhysics/Constraints/Weld.cs | 26 +++++++++---------- BepuPhysics/Simulation.cs | 4 +-- BepuPhysics/Solver_Solve.cs | 4 +-- BepuPhysics/Solver_SubsteppingSolve2.cs | 5 ---- Demos/Demo.cs | 2 +- Demos/Demos/NewtDemo.cs | 4 +-- Demos/Program.cs | 2 +- 9 files changed, 30 insertions(+), 34 deletions(-) diff --git a/BepuPhysics/BatchCompressor.cs b/BepuPhysics/BatchCompressor.cs index 638b554a1..e0b78e558 100644 --- a/BepuPhysics/BatchCompressor.cs +++ b/BepuPhysics/BatchCompressor.cs @@ -97,7 +97,7 @@ struct AnalysisRegion Action analysisWorkerDelegate; - public BatchCompressor(Solver solver, Bodies bodies, float targetCandidateFraction = 0.01f, float maximumCompressionFraction = 0.0005f) + public BatchCompressor(Solver solver, Bodies bodies, float targetCandidateFraction = 0.005f, float maximumCompressionFraction = 0.0005f) { this.Solver = solver; this.Bodies = bodies; diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index d8cbdc9b8..a4a874f22 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -324,9 +324,10 @@ static unsafe void PrefetchBundle(MotionState* motionBase, BodyInertia* inertiaB } } - const int prefetchDistance = 8; + const int warmStartPrefetchDistance = 8; + const int solvePrefetchDistance = 4; [MethodImpl(MethodImplOptions.AggressiveInlining)] - unsafe static void EarlyPrefetch(ref TypeBatch typeBatch, ref Buffer references, ref Buffer states, ref Buffer inertias, int startBundleIndex, int exclusiveEndBundleIndex) + unsafe static void EarlyPrefetch(int prefetchDistance, ref TypeBatch typeBatch, ref Buffer references, ref Buffer states, ref Buffer inertias, int startBundleIndex, int exclusiveEndBundleIndex) { exclusiveEndBundleIndex = Math.Min(exclusiveEndBundleIndex, startBundleIndex + prefetchDistance); var lastBundleIndex = exclusiveEndBundleIndex - 1; @@ -339,7 +340,7 @@ unsafe static void EarlyPrefetch(ref TypeBatch typeBatch, ref Buffer references, ref Buffer states, ref Buffer inertias, int bundleIndex, int exclusiveEndBundleIndex) + unsafe static void Prefetch(int prefetchDistance, ref TypeBatch typeBatch, ref Buffer references, ref Buffer states, ref Buffer inertias, int bundleIndex, int exclusiveEndBundleIndex) { var targetIndex = bundleIndex + prefetchDistance; if (targetIndex < exclusiveEndBundleIndex) @@ -356,14 +357,14 @@ public unsafe override void WarmStart2(ref TypeBatch typeBatch, Bodies bodies, f var function = default(TConstraintFunctions); ref var motionStates = ref bodies.ActiveSet.MotionStates; ref var inertias = ref bodies.Inertias; - EarlyPrefetch(ref typeBatch, ref bodyReferencesBundles, ref motionStates, ref inertias, startBundle, exclusiveEndBundle); + EarlyPrefetch(warmStartPrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, ref inertias, startBundle, exclusiveEndBundle); for (int i = startBundle; i < exclusiveEndBundle; ++i) { ref var prestep = ref prestepBundles[i]; ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; ref var references = ref bodyReferencesBundles[i]; var count = GetCountInBundle(ref typeBatch, i); - Prefetch(ref typeBatch, ref bodyReferencesBundles, ref motionStates, ref inertias, i, exclusiveEndBundle); + Prefetch(warmStartPrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, ref inertias, i, exclusiveEndBundle); bodies.GatherState(ref references, count, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB); if (typeof(TConstraintFunctions) == typeof(WeldFunctions)) { @@ -391,14 +392,14 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f var function = default(TConstraintFunctions); ref var motionStates = ref bodies.ActiveSet.MotionStates; ref var inertias = ref bodies.Inertias; - EarlyPrefetch(ref typeBatch, ref bodyReferencesBundles, ref motionStates, ref inertias, startBundle, exclusiveEndBundle); + EarlyPrefetch(solvePrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, ref inertias, startBundle, exclusiveEndBundle); for (int i = startBundle; i < exclusiveEndBundle; ++i) { ref var prestep = ref prestepBundles[i]; ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; ref var references = ref bodyReferencesBundles[i]; var count = GetCountInBundle(ref typeBatch, i); - Prefetch(ref typeBatch, ref bodyReferencesBundles, ref motionStates, ref inertias, i, exclusiveEndBundle); + Prefetch(solvePrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, ref inertias, i, exclusiveEndBundle); bodies.GatherState(ref references, count, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB); if (typeof(TConstraintFunctions) == typeof(WeldFunctions)) { diff --git a/BepuPhysics/Constraints/Weld.cs b/BepuPhysics/Constraints/Weld.cs index 95b3a48fb..e68e2f4c5 100644 --- a/BepuPhysics/Constraints/Weld.cs +++ b/BepuPhysics/Constraints/Weld.cs @@ -257,7 +257,7 @@ public void Solve2(in QuaternionWide orientationA, in BodyInertias inertiaA, in //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. //var offset = prestep.LocalOffset * orientationA; - QuaternionWide.Transform(prestep.LocalOffset, orientationA, out var offset); + Transform(prestep.LocalOffset, orientationA, out var offset); var positionError = ab - offset; var targetOrientationB = prestep.LocalOrientation * orientationA; //ConcatenateWithoutOverlap(prestep.LocalOrientation, orientationA, out var targetOrientationB); @@ -274,18 +274,18 @@ public void Solve2(in QuaternionWide orientationA, in BodyInertias inertiaA, in //csi = -accumulatedImpulse * projection.SoftnessImpulseScale - (-biasVelocity + csvaLinear + csvaAngular + csvbLinear + csvbAngular) * effectiveMass; //csi = (biasVelocity - csvaLinear - csvaAngular - csvbLinear - csvbAngular) * effectiveMass - accumulatedImpulse * projection.SoftnessImpulseScale; //csv = V * JT - var orientationCSV = orientationBiasVelocity - (wsvA.Angular - wsvB.Angular); - var offsetCSV = offsetBiasVelocity - (wsvA.Linear - wsvB.Linear + Cross(wsvA.Angular, offset)); - - ////Unfortunately, manually inlining does actually improve the codegen meaningfully as of this writing. - //Vector3Wide orientationCSV, offsetCSV; - //orientationCSV.X = orientationBiasVelocity.X - wsvA.Angular.X + wsvB.Angular.X; - //orientationCSV.Y = orientationBiasVelocity.Y - wsvA.Angular.Y + wsvB.Angular.Y; - //orientationCSV.Z = orientationBiasVelocity.Z - wsvA.Angular.Z + wsvB.Angular.Z; - - //offsetCSV.X = offsetBiasVelocity.X - wsvA.Linear.X + wsvB.Linear.X - (wsvA.Angular.Y * offset.Z - wsvA.Angular.Z * offset.Y); - //offsetCSV.Y = offsetBiasVelocity.Y - wsvA.Linear.Y + wsvB.Linear.Y - (wsvA.Angular.Z * offset.X - wsvA.Angular.X * offset.Z); - //offsetCSV.Z = offsetBiasVelocity.Z - wsvA.Linear.Z + wsvB.Linear.Z - (wsvA.Angular.X * offset.Y - wsvA.Angular.Y * offset.X); + //var orientationCSV = orientationBiasVelocity - (wsvA.Angular - wsvB.Angular); + //var offsetCSV = offsetBiasVelocity - (wsvA.Linear - wsvB.Linear + Cross(wsvA.Angular, offset)); + + //Unfortunately, manually inlining does actually improve the codegen meaningfully as of this writing. + Vector3Wide orientationCSV, offsetCSV; + orientationCSV.X = orientationBiasVelocity.X - wsvA.Angular.X + wsvB.Angular.X; + orientationCSV.Y = orientationBiasVelocity.Y - wsvA.Angular.Y + wsvB.Angular.Y; + orientationCSV.Z = orientationBiasVelocity.Z - wsvA.Angular.Z + wsvB.Angular.Z; + + offsetCSV.X = offsetBiasVelocity.X - wsvA.Linear.X + wsvB.Linear.X - (wsvA.Angular.Y * offset.Z - wsvA.Angular.Z * offset.Y); + offsetCSV.Y = offsetBiasVelocity.Y - wsvA.Linear.Y + wsvB.Linear.Y - (wsvA.Angular.Z * offset.X - wsvA.Angular.X * offset.Z); + offsetCSV.Z = offsetBiasVelocity.Z - wsvA.Linear.Z + wsvB.Linear.Z - (wsvA.Angular.X * offset.Y - wsvA.Angular.Y * offset.X); //Effective mass = (J * M^-1 * JT)^-1, which is going to be a little tricky because J * M^-1 * JT is a 6x6 matrix: //J * M^-1 * JT = [ Ia^-1 + Ib^-1, Ia^-1 * transpose(skewSymmetric(localOffset * orientationA)) ] diff --git a/BepuPhysics/Simulation.cs b/BepuPhysics/Simulation.cs index cd7d13b81..b2ae97f80 100644 --- a/BepuPhysics/Simulation.cs +++ b/BepuPhysics/Simulation.cs @@ -324,11 +324,11 @@ public void IncrementallyOptimizeDataStructures(IThreadDispatcher threadDispatch //TODO: The order of these optimizer stages is performance relevant, even though they don't have any effect on correctness. //You may want to try them in different locations to see how they impact cache residency. profiler.Start(BodyLayoutOptimizer); - BodyLayoutOptimizer.IncrementalOptimize(); + //BodyLayoutOptimizer.IncrementalOptimize(); profiler.End(BodyLayoutOptimizer); profiler.Start(ConstraintLayoutOptimizer); - ConstraintLayoutOptimizer.Update(BufferPool, threadDispatcher); + //ConstraintLayoutOptimizer.Update(BufferPool, threadDispatcher); profiler.End(ConstraintLayoutOptimizer); profiler.Start(SolverBatchCompressor); diff --git a/BepuPhysics/Solver_Solve.cs b/BepuPhysics/Solver_Solve.cs index d2c421794..a7186f3b7 100644 --- a/BepuPhysics/Solver_Solve.cs +++ b/BepuPhysics/Solver_Solve.cs @@ -722,8 +722,8 @@ void ExecuteMultithreaded(float dt, IThreadDispatcher thr //These values are found by empirical tuning. The optimal values may vary by architecture. //The goal here is to have just enough blocks that, in the event that we end up some underpowered threads (due to competition or hyperthreading), //there are enough blocks that workstealing will still generally allow the extra threads to be useful. - const int targetBlocksPerBatchPerWorker = 16; - const int minimumBlockSizeInBundles = 3; + const int targetBlocksPerBatchPerWorker = 1; + const int minimumBlockSizeInBundles = 1; var targetBlocksPerBatch = workerCount * targetBlocksPerBatchPerWorker; BuildWorkBlocks(pool, minimumBlockSizeInBundles, targetBlocksPerBatch, ref filter); diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index a12a891b7..692a9381a 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -68,16 +68,11 @@ public void Execute(Solver solver, int blockIndex) void SolveStep2Worker(int workerIndex) { - int prestepStart = GetUniformlyDistributedStart(workerIndex, context.ConstraintBlocks.Blocks.Count, context.WorkerCount, 0); int fallbackStart = GetUniformlyDistributedStart(workerIndex, context.FallbackBlocks.Blocks.Count, context.WorkerCount, 0); Buffer batchStarts; ref var activeSet = ref ActiveSet; unsafe { - //stackalloc is actually a little bit slow since the localsinit behavior forces a zeroing. - //Fortunately, this executes once per thread per frame. With 32 batches, it would add... a few nanoseconds per frame. We can accept that overhead. - //This is preferred over preallocating on the heap- we might write to these values and we don't want to risk false sharing for no reason. - //A single instance of false sharing would cost far more than the overhead of zeroing out the array. var batchStartsData = stackalloc int[activeSet.Batches.Count]; batchStarts = new Buffer(batchStartsData, activeSet.Batches.Count); } diff --git a/Demos/Demo.cs b/Demos/Demo.cs index aba8044d8..f00656836 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -62,7 +62,7 @@ public virtual void Update(Window window, Camera camera, Input input, float dt) //fully decouple simulation and rendering rates across different threads. //(In either case, you'd also want to interpolate or extrapolate simulation results during rendering for smoothness.) //Note that taking steps of variable length can reduce stability. Gradual or one-off changes can work reasonably well. - Simulation.Timestep(1 / 60f); + Simulation.Timestep(1 / 60f, ThreadDispatcher); ////Here's an example of how it would look to use more frequent updates, but still with a fixed amount of time simulated per update call: //const float timeToSimulate = 1 / 60f; diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index 0385085c7..6592bfd6b 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -823,8 +823,8 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper(1)); - //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)), new EmbeddedSubsteppingTimestepper2(3)); - Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)), new PositionFirstTimestepper2(), solverIterationCount: 5); + Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)), new EmbeddedSubsteppingTimestepper2(3)); + //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)), new PositionFirstTimestepper2(), solverIterationCount: 5); //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper2(3), solverIterationCount: 1); var meshContent = content.Load("Content\\newt.obj"); diff --git a/Demos/Program.cs b/Demos/Program.cs index ba902a5cf..6c769ece5 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -19,7 +19,7 @@ static void Main(string[] args) { content = ContentArchive.Load(stream); } - HeadlessTest.Test(content, 2, 32, 512); + HeadlessTest.Test(content, 4, 32, 512); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); From 0ea9d06af3e1604d11418826bfb334a5e314014b Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 5 Jul 2021 16:46:14 -0500 Subject: [PATCH 077/947] Tiny solve block tuning. --- BepuPhysics/Solver_Solve.cs | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/BepuPhysics/Solver_Solve.cs b/BepuPhysics/Solver_Solve.cs index a7186f3b7..9a3729e3c 100644 --- a/BepuPhysics/Solver_Solve.cs +++ b/BepuPhysics/Solver_Solve.cs @@ -118,11 +118,13 @@ public bool AllowType(int typeId) } } - private unsafe void BuildWorkBlocks(BufferPool pool, int minimumBlockSizeInBundles, int targetBlocksPerBatch, ref TTypeBatchFilter typeBatchFilter) where TTypeBatchFilter : ITypeBatchSolveFilter + private unsafe void BuildWorkBlocks(BufferPool pool, int minimumBlockSizeInBundles, int maximumBlockSizeInBundles, int targetBlocksPerBatch, ref TTypeBatchFilter typeBatchFilter) where TTypeBatchFilter : ITypeBatchSolveFilter { ref var activeSet = ref ActiveSet; context.ConstraintBlocks.Blocks = new QuickList(targetBlocksPerBatch * activeSet.Batches.Count, pool); pool.Take(activeSet.Batches.Count, out context.BatchBoundaries); + var inverseMinimumBlockSizeInBundles = 1f / minimumBlockSizeInBundles; + var inverseMaximumBlockSizeInBundles = 1f / maximumBlockSizeInBundles; for (int batchIndex = 0; batchIndex < activeSet.Batches.Count; ++batchIndex) { ref var typeBatches = ref activeSet.Batches[batchIndex].TypeBatches; @@ -134,7 +136,6 @@ private unsafe void BuildWorkBlocks(BufferPool pool, int minim bundleCount += typeBatches[typeBatchIndex].BundleCount; } } - for (int typeBatchIndex = 0; typeBatchIndex < typeBatches.Count; ++typeBatchIndex) { ref var typeBatch = ref typeBatches[typeBatchIndex]; @@ -142,9 +143,10 @@ private unsafe void BuildWorkBlocks(BufferPool pool, int minim { continue; } - var typeBatchSizeFraction = typeBatch.BundleCount / (float)bundleCount; - var typeBatchMaximumBlockCount = typeBatch.BundleCount / (float)minimumBlockSizeInBundles; - var typeBatchBlockCount = Math.Max(1, (int)Math.Min(typeBatchMaximumBlockCount, targetBlocksPerBatch * typeBatchSizeFraction)); + var typeBatchSizeFraction = typeBatch.BundleCount / (float)bundleCount; //note: pre-inverting this doesn't necessarily work well due to numerical issues. + var typeBatchMaximumBlockCount = typeBatch.BundleCount * inverseMinimumBlockSizeInBundles; + var typeBatchMinimumBlockCount = typeBatch.BundleCount * inverseMaximumBlockSizeInBundles; + var typeBatchBlockCount = Math.Max(1, (int)Math.Min(typeBatchMaximumBlockCount, Math.Max(typeBatchMinimumBlockCount, targetBlocksPerBatch * typeBatchSizeFraction))); int previousEnd = 0; var baseBlockSizeInBundles = typeBatch.BundleCount / typeBatchBlockCount; var remainder = typeBatch.BundleCount - baseBlockSizeInBundles * typeBatchBlockCount; @@ -724,9 +726,10 @@ void ExecuteMultithreaded(float dt, IThreadDispatcher thr //there are enough blocks that workstealing will still generally allow the extra threads to be useful. const int targetBlocksPerBatchPerWorker = 1; const int minimumBlockSizeInBundles = 1; + const int maximumBlockSizeInBundles = 1024; var targetBlocksPerBatch = workerCount * targetBlocksPerBatchPerWorker; - BuildWorkBlocks(pool, minimumBlockSizeInBundles, targetBlocksPerBatch, ref filter); + BuildWorkBlocks(pool, minimumBlockSizeInBundles, maximumBlockSizeInBundles, targetBlocksPerBatch, ref filter); ValidateWorkBlocks(ref filter); //Note the clear; the block claims must be initialized to 0 so that the first worker stage knows that the data is available to claim. From fe7aa7a42a89948bbe90fa3c079abb3cc5073682 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 5 Jul 2021 20:30:34 -0500 Subject: [PATCH 078/947] Preparing for solver integration integration. --- BepuPhysics/BodyProperties.cs | 56 +++++++++++++++++++++++++++++++++++ BepuPhysics/BodySet.cs | 11 +++++++ 2 files changed, 67 insertions(+) diff --git a/BepuPhysics/BodyProperties.cs b/BepuPhysics/BodyProperties.cs index bdf721f57..25e0bbbf6 100644 --- a/BepuPhysics/BodyProperties.cs +++ b/BepuPhysics/BodyProperties.cs @@ -3,6 +3,7 @@ using BepuUtilities; using BepuUtilities.Memory; using System; +using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; using System.Runtime.InteropServices; @@ -298,4 +299,59 @@ public struct BodyActivity /// public bool SleepCandidate; } + + /// + /// Stores indices for choosing what system is responsible for integrating a body's velocities and pose. + /// + public struct BodyConstraintBatchRange + { + /// + /// Most significant bit encodes whether the body has any constraints. Set if it does, unset if it doesn't. + /// Least significant bits [0, 15) store the minimum batch index if the most significant bit is set. + /// Bits [16, 31) store the maximum batch index if the most significant bit is set. + /// Bits [0, 31) store the index into the active unconstrained integration set if the body is awake and the most significant bit is not set. + /// If the most significant bit is not set and the body is asleep, bits [0, 31) are undefined. + /// + public uint Packed; + + /// + /// Gets whether the body has any constraints in the solver. + /// Constrained bodies will have their velocities and poses integrated as a part of the constraint solve, unconstrained bodies will be independently integrated. + /// + public bool Constrained => (Packed & (1u << 31)) != 0; + /// + /// Gets the minimum constraint batch index that this body is associated with if the body has constraints. If it has no constraints, this property is undefined. + /// + public int MinimumBatch => (int)(Packed & 0x7FFF); + /// + /// Gets the maximum constraint batch index that this body is associated with if the body has constraints. If it has no constraints, this property is undefined. + /// + public int MaximumBatch => (int)((Packed >> 16) & 0x7FFF); + /// + /// Gets the index of the body in the unconstrained integration set if it has no constraints. If the body has constraints, this property is undefined. + /// + public int UnconstrainedIndex => (int)(Packed & 0x7FFF_FFFF); + + /// + /// Constructs an unconstrained body range. + /// + /// Index of the body in the unconstrained set. + public BodyConstraintBatchRange(int unconstrainedIndex) + { + Debug.Assert(unconstrainedIndex >= 0, "Unconstrained index must be positive. Did some data get corrupted?"); + Packed = (uint)unconstrainedIndex; + } + + /// + /// Constructs a constrained body range. + /// + /// Minimum constraint batch index the body is involved in. + /// Maximum constraint batch index the body is involved in. + public BodyConstraintBatchRange(int minimumBatch, int maximumBatch) + { + Debug.Assert(minimumBatch >= 0 && minimumBatch < (1 << 15) && maximumBatch >= 0 && maximumBatch < (1 << 15), + "Batch indices must fit within the packed ranges. Did some data get corrupted, or is the maximum batch count just way too high?"); + Packed = (1u << 31) | ((uint)maximumBatch << 16) | (uint)minimumBatch; + } + } } diff --git a/BepuPhysics/BodySet.cs b/BepuPhysics/BodySet.cs index c1266fd35..5dd2965aa 100644 --- a/BepuPhysics/BodySet.cs +++ b/BepuPhysics/BodySet.cs @@ -52,6 +52,13 @@ public struct BodySet /// public Buffer> Constraints; + /// + /// Range of constraint batches, or the unconstrained integration index, associated with each body. + /// + /// This is used to determine which system is responsible for integrating a body's velocities and pose. + /// If a body has constraints, the constraint solve will handle it. If it's unconstrained, a separate dedicated phase will. + public Buffer ConstraintBatchRanges; + public int Count; /// /// Gets whether this instance is backed by allocated memory. @@ -97,6 +104,7 @@ internal bool RemoveAt(int bodyIndex, out BodyHandle handle, out int movedBodyIn //During true removal, the caller is responsible for removing all constraints and disposing the list. //In sleeping, the reference to the list is simply copied into the sleeping set. Constraints[bodyIndex] = Constraints[movedBodyIndex]; + ConstraintBatchRanges[bodyIndex] = ConstraintBatchRanges[movedBodyIndex]; //Point the body handles at the new location. movedBodyHandle = IndexToHandle[movedBodyIndex]; IndexToHandle[bodyIndex] = movedBodyHandle; @@ -229,6 +237,7 @@ internal void Swap(int slotA, int slotB, ref Buffer handleTo Helpers.Swap(ref LocalInertias[slotA], ref LocalInertias[slotB]); Helpers.Swap(ref Activity[slotA], ref Activity[slotB]); Helpers.Swap(ref Constraints[slotA], ref Constraints[slotB]); + Helpers.Swap(ref ConstraintBatchRanges[slotA], ref ConstraintBatchRanges[slotB]); } internal unsafe void InternalResize(int targetBodyCapacity, BufferPool pool) @@ -244,6 +253,7 @@ internal unsafe void InternalResize(int targetBodyCapacity, BufferPool pool) pool.ResizeToAtLeast(ref Collidables, targetBodyCapacity, Count); pool.ResizeToAtLeast(ref Activity, targetBodyCapacity, Count); pool.ResizeToAtLeast(ref Constraints, targetBodyCapacity, Count); + pool.ResizeToAtLeast(ref ConstraintBatchRanges, targetBodyCapacity, Count); } public unsafe void Clear(BufferPool pool) @@ -267,6 +277,7 @@ public void DisposeBuffers(BufferPool pool) pool.Return(ref Collidables); pool.Return(ref Activity); pool.Return(ref Constraints); + pool.Return(ref ConstraintBatchRanges); } /// From 44a6571edf80505dfcba60bb1ecf67e9230ccffe Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 5 Jul 2021 21:18:55 -0500 Subject: [PATCH 079/947] Rejiggerment; one cache line, more data. --- BepuPhysics/Bodies.cs | 17 ++--- BepuPhysics/BodyProperties.cs | 54 +++++---------- BepuPhysics/BodyReference.cs | 2 +- BepuPhysics/BodySet.cs | 68 ++++++++++++------- .../CollisionDetection/FreshnessChecker.cs | 2 +- .../NarrowPhasePendingConstraintAdds.cs | 4 +- BepuPhysics/IslandSleeper.cs | 2 +- BepuPhysics/Solver.cs | 6 +- Demos/Program.cs | 2 +- .../SpecializedTests/SimulationScrambling.cs | 4 +- 10 files changed, 83 insertions(+), 78 deletions(-) diff --git a/BepuPhysics/Bodies.cs b/BepuPhysics/Bodies.cs index 8614e3cc7..1aa2407cf 100644 --- a/BepuPhysics/Bodies.cs +++ b/BepuPhysics/Bodies.cs @@ -223,7 +223,7 @@ internal BodyHandle RemoveFromActiveSet(int activeBodyIndex) public void RemoveAt(int activeBodyIndex) { //Constraints must be removed; we cannot leave 'orphans' in the solver because they will access invalid data. - ref var constraints = ref ActiveSet.Constraints[activeBodyIndex]; + ref var constraints = ref ActiveSet.Constraints[activeBodyIndex].References; for (int i = constraints.Count - 1; i >= 0; --i) { solver.Remove(constraints[i].ConnectingConstraintHandle); @@ -253,12 +253,13 @@ public void Remove(BodyHandle handle) /// /// Adds a constraint to an active body's constraint list. /// + /// Solver owning the constraint. /// Index of the body to add the constraint to. /// Handle of the constraint to add. /// Index of the body in the constraint. - internal void AddConstraint(int bodyIndex, ConstraintHandle constraintHandle, int indexInConstraint) + internal void AddConstraint(Solver solver, int bodyIndex, ConstraintHandle constraintHandle, int indexInConstraint) { - ActiveSet.AddConstraint(bodyIndex, constraintHandle, indexInConstraint, Pool); + ActiveSet.AddConstraint(solver, bodyIndex, constraintHandle, indexInConstraint, Pool); } /// @@ -331,7 +332,7 @@ void UpdateForKinematicStateChange(BodyHandle handle, ref BodyMemoryLocation loc } if (newlyKinematic) { - ref var constraints = ref set.Constraints[location.Index]; + ref var constraints = ref set.Constraints[location.Index].References; ConnectedDynamicCounter enumerator; enumerator.Bodies = this; for (int i = 0; i < constraints.Count; ++i) @@ -602,7 +603,7 @@ public void LoopBody(int connectedBodyHandle) [MethodImpl(MethodImplOptions.AggressiveInlining)] internal void EnumerateConnectedBodyIndices(int activeBodyIndex, ref TEnumerator enumerator) where TEnumerator : IForEach { - ref var list = ref ActiveSet.Constraints[activeBodyIndex]; + ref var list = ref ActiveSet.Constraints[activeBodyIndex].References; ActiveConstraintBodyIndicesEnumerator constraintBodiesEnumerator; constraintBodiesEnumerator.InnerEnumerator = enumerator; constraintBodiesEnumerator.SourceBodyIndex = activeBodyIndex; @@ -631,7 +632,7 @@ public void EnumerateConnectedBodies(BodyHandle bodyHandle, ref TEn { ref var bodyLocation = ref HandleToLocation[bodyHandle.Value]; ref var set = ref Sets[bodyLocation.SetIndex]; - ref var list = ref set.Constraints[bodyLocation.Index]; + ref var list = ref set.Constraints[bodyLocation.Index].References; //In the loops below, we still make use of the reversed iteration. Removing from within the context of an enumerator is a dangerous move, but it is permitted if the user //is careful. By maintaining the same convention across all of these enumerations, it makes it a little easier to do reliably. if (bodyLocation.SetIndex == 0) @@ -751,7 +752,7 @@ public void ResizeConstraintListCapacities() { for (int i = 0; i < ActiveSet.Count; ++i) { - ref var list = ref ActiveSet.Constraints[i]; + ref var list = ref ActiveSet.Constraints[i].References; var targetCapacity = BufferPool.GetCapacityForCount(list.Count > MinimumConstraintCapacityPerBody ? list.Count : MinimumConstraintCapacityPerBody); if (list.Span.Length != targetCapacity) list.Resize(targetCapacity, Pool); @@ -782,7 +783,7 @@ public void EnsureConstraintListCapacities() { for (int i = 0; i < ActiveSet.Count; ++i) { - ref var list = ref ActiveSet.Constraints[i]; + ref var list = ref ActiveSet.Constraints[i].References; if (list.Span.Length < MinimumConstraintCapacityPerBody) list.Resize(MinimumConstraintCapacityPerBody, Pool); } diff --git a/BepuPhysics/BodyProperties.cs b/BepuPhysics/BodyProperties.cs index 25e0bbbf6..22a0b74ee 100644 --- a/BepuPhysics/BodyProperties.cs +++ b/BepuPhysics/BodyProperties.cs @@ -1,6 +1,7 @@ using BepuPhysics.Collidables; using BepuPhysics.Constraints; using BepuUtilities; +using BepuUtilities.Collections; using BepuUtilities.Memory; using System; using System.Diagnostics; @@ -301,57 +302,38 @@ public struct BodyActivity } /// - /// Stores indices for choosing what system is responsible for integrating a body's velocities and pose. + /// Stores references to constraints connected to a body and additional data for choosing what system is responsible for integrating a body's velocities and pose. /// - public struct BodyConstraintBatchRange + public struct BodyConstraints { + public QuickList References; + /// - /// Most significant bit encodes whether the body has any constraints. Set if it does, unset if it doesn't. - /// Least significant bits [0, 15) store the minimum batch index if the most significant bit is set. - /// Bits [16, 31) store the maximum batch index if the most significant bit is set. - /// Bits [0, 31) store the index into the active unconstrained integration set if the body is awake and the most significant bit is not set. - /// If the most significant bit is not set and the body is asleep, bits [0, 31) are undefined. + /// Index of the body in the unconstrained integration set if it has no constraints. If the body has constraints, this value is undefined. /// - public uint Packed; + public int UnconstrainedIndex; /// - /// Gets whether the body has any constraints in the solver. - /// Constrained bodies will have their velocities and poses integrated as a part of the constraint solve, unconstrained bodies will be independently integrated. + /// Inclusive minimum constraint batch index that this body is associated with if the body has constraints. If it has no constraints, this value is undefined. /// - public bool Constrained => (Packed & (1u << 31)) != 0; + public int MinimumBatch; /// - /// Gets the minimum constraint batch index that this body is associated with if the body has constraints. If it has no constraints, this property is undefined. + /// Inclusive maximum constraint batch index that this body is associated with if the body has constraints. If it has no constraints, this value is undefined. /// - public int MinimumBatch => (int)(Packed & 0x7FFF); + public int MaximumBatch; + /// - /// Gets the maximum constraint batch index that this body is associated with if the body has constraints. If it has no constraints, this property is undefined. + /// Constraint connected to the body associated with the lowest batch index. If the body has no constraints, this value is undefined. /// - public int MaximumBatch => (int)((Packed >> 16) & 0x7FFF); + public ConstraintHandle MinimumConstraint; /// - /// Gets the index of the body in the unconstrained integration set if it has no constraints. If the body has constraints, this property is undefined. + /// Constraint connected to the body associated with the highest batch index. If the body has no constraints, this value is undefined. /// - public int UnconstrainedIndex => (int)(Packed & 0x7FFF_FFFF); + public ConstraintHandle MaximumConstraint; /// - /// Constructs an unconstrained body range. + /// Gets whether this body has any constraints based on the value in the UnconstrainedIndex field. /// - /// Index of the body in the unconstrained set. - public BodyConstraintBatchRange(int unconstrainedIndex) - { - Debug.Assert(unconstrainedIndex >= 0, "Unconstrained index must be positive. Did some data get corrupted?"); - Packed = (uint)unconstrainedIndex; - } - - /// - /// Constructs a constrained body range. - /// - /// Minimum constraint batch index the body is involved in. - /// Maximum constraint batch index the body is involved in. - public BodyConstraintBatchRange(int minimumBatch, int maximumBatch) - { - Debug.Assert(minimumBatch >= 0 && minimumBatch < (1 << 15) && maximumBatch >= 0 && maximumBatch < (1 << 15), - "Batch indices must fit within the packed ranges. Did some data get corrupted, or is the maximum batch count just way too high?"); - Packed = (1u << 31) | ((uint)maximumBatch << 16) | (uint)minimumBatch; - } + public bool Constrained => References.Count > 0; } } diff --git a/BepuPhysics/BodyReference.cs b/BepuPhysics/BodyReference.cs index 98b0fd203..651c19b85 100644 --- a/BepuPhysics/BodyReference.cs +++ b/BepuPhysics/BodyReference.cs @@ -163,7 +163,7 @@ public ref QuickList Constraints get { ref var location = ref MemoryLocation; - return ref Bodies.Sets[location.SetIndex].Constraints[location.Index]; + return ref Bodies.Sets[location.SetIndex].Constraints[location.Index].References; } } diff --git a/BepuPhysics/BodySet.cs b/BepuPhysics/BodySet.cs index 5dd2965aa..975854c41 100644 --- a/BepuPhysics/BodySet.cs +++ b/BepuPhysics/BodySet.cs @@ -48,16 +48,9 @@ public struct BodySet /// public Buffer Activity; /// - /// List of constraints associated with each body in the set. + /// List of constraints and constraint related data associated with each body in the set. /// - public Buffer> Constraints; - - /// - /// Range of constraint batches, or the unconstrained integration index, associated with each body. - /// - /// This is used to determine which system is responsible for integrating a body's velocities and pose. - /// If a body has constraints, the constraint solve will handle it. If it's unconstrained, a separate dedicated phase will. - public Buffer ConstraintBatchRanges; + public Buffer Constraints; public int Count; /// @@ -80,7 +73,8 @@ internal int Add(in BodyDescription bodyDescription, BodyHandle handle, int mini ++Count; IndexToHandle[index] = handle; //Collidable's broad phase index is left unset. The Bodies collection is responsible for attaching that data. - Constraints[index] = new QuickList(minimumConstraintCapacity, pool); + Constraints[index].References = new QuickList(minimumConstraintCapacity, pool); + ApplyDescriptionByIndex(index, bodyDescription); return index; } @@ -104,7 +98,6 @@ internal bool RemoveAt(int bodyIndex, out BodyHandle handle, out int movedBodyIn //During true removal, the caller is responsible for removing all constraints and disposing the list. //In sleeping, the reference to the list is simply copied into the sleeping set. Constraints[bodyIndex] = Constraints[movedBodyIndex]; - ConstraintBatchRanges[bodyIndex] = ConstraintBatchRanges[movedBodyIndex]; //Point the body handles at the new location. movedBodyHandle = IndexToHandle[movedBodyIndex]; IndexToHandle[bodyIndex] = movedBodyHandle; @@ -167,16 +160,48 @@ public void GetDescription(int index, out BodyDescription description) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - internal void AddConstraint(int bodyIndex, ConstraintHandle constraintHandle, int bodyIndexInConstraint, BufferPool pool) + internal void AddConstraint(Solver solver, int bodyIndex, ConstraintHandle constraintHandle, int bodyIndexInConstraint, BufferPool pool) { BodyConstraintReference constraint; constraint.ConnectingConstraintHandle = constraintHandle; constraint.BodyIndexInConstraint = bodyIndexInConstraint; ref var constraints = ref Constraints[bodyIndex]; - Debug.Assert(constraints.Span.Allocated, "Any time a body is created, a list should be built to support it."); - if (constraints.Span.Length == constraints.Count) - constraints.Resize(constraints.Span.Length * 2, pool); - constraints.AllocateUnsafely() = constraint; + Debug.Assert(constraints.References.Span.Allocated, "Any time a body is created, a list should be built to support it."); + if (constraints.References.Span.Length == constraints.References.Count) + constraints.References.Resize(constraints.References.Span.Length * 2, pool); + constraints.References.AllocateUnsafely() = constraint; + + var batchIndex = solver.HandleToConstraint[constraintHandle.Value].BatchIndex; + if (constraints.References.Count == 1) + { + //The body is transitioning from unconstrained to constrained. The constraint will now be responsible for its integration. + //UnconstrainedBodies.Remove(bodyIndex); + constraints.MinimumBatch = batchIndex; + constraints.MaximumBatch = batchIndex; + constraints.MinimumConstraint = constraintHandle; + constraints.MaximumConstraint = constraintHandle; + //solver.AddIntegrationResponsibilityToConstraint(constraintHandle, bodyIndex, bodyIndexInConstraint); + } + else + { + var minimum = constraints.MinimumBatch; + var maximum = constraints.MaximumBatch; + if (batchIndex < minimum) + { + //solver.RemoveIntegrationResponsibilityFromConstraint(constraints.MinimumConstraint, bodyIndex); + constraints.MinimumBatch = batchIndex; + constraints.MinimumConstraint = constraintHandle; + //solver.AddIntegrationResponsibilityToConstraint(constraintHandle, bodyIndex, bodyIndexInConstraint); + + } + else if (batchIndex > maximum) + { + //solver.RemoveIntegrationResponsibilityFromConstraint(constraints.MinimumConstraint, bodyIndex); + constraints.MaximumBatch = batchIndex; + constraints.MinimumConstraint = constraintHandle; + //solver.AddIntegrationResponsibilityToConstraint(constraintHandle, bodyIndex, bodyIndexInConstraint); + } + } } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -184,7 +209,7 @@ internal void RemoveConstraintReference(int bodyIndex, ConstraintHandle constrai { //This uses a linear search. That's fine; bodies will rarely have more than a handful of constraints associated with them. //Attempting to use something like a hash set for fast removes would just introduce more constant overhead and slow it down on average. - ref var list = ref Constraints[bodyIndex]; + ref var list = ref Constraints[bodyIndex].References; for (int i = 0; i < list.Count; ++i) { ref var element = ref list[i]; @@ -209,7 +234,7 @@ internal void RemoveConstraintReference(int bodyIndex, ConstraintHandle constrai public bool BodyIsConstrainedBy(int bodyIndex, ConstraintHandle constraintHandle) { - ref var list = ref Constraints[bodyIndex]; + ref var list = ref Constraints[bodyIndex].References; for (int i = 0; i < list.Count; ++i) { if (list[i].ConnectingConstraintHandle.Value == constraintHandle.Value) @@ -237,7 +262,6 @@ internal void Swap(int slotA, int slotB, ref Buffer handleTo Helpers.Swap(ref LocalInertias[slotA], ref LocalInertias[slotB]); Helpers.Swap(ref Activity[slotA], ref Activity[slotB]); Helpers.Swap(ref Constraints[slotA], ref Constraints[slotB]); - Helpers.Swap(ref ConstraintBatchRanges[slotA], ref ConstraintBatchRanges[slotB]); } internal unsafe void InternalResize(int targetBodyCapacity, BufferPool pool) @@ -253,14 +277,13 @@ internal unsafe void InternalResize(int targetBodyCapacity, BufferPool pool) pool.ResizeToAtLeast(ref Collidables, targetBodyCapacity, Count); pool.ResizeToAtLeast(ref Activity, targetBodyCapacity, Count); pool.ResizeToAtLeast(ref Constraints, targetBodyCapacity, Count); - pool.ResizeToAtLeast(ref ConstraintBatchRanges, targetBodyCapacity, Count); } public unsafe void Clear(BufferPool pool) { for (int i = 0; i < Count; ++i) { - Constraints[i].Dispose(pool); + Constraints[i].References.Dispose(pool); } Count = 0; } @@ -277,7 +300,6 @@ public void DisposeBuffers(BufferPool pool) pool.Return(ref Collidables); pool.Return(ref Activity); pool.Return(ref Constraints); - pool.Return(ref ConstraintBatchRanges); } /// @@ -288,7 +310,7 @@ public void Dispose(BufferPool pool) { for (int i = 0; i < Count; ++i) { - Constraints[i].Dispose(pool); + Constraints[i].References.Dispose(pool); } DisposeBuffers(pool); this = new BodySet(); diff --git a/BepuPhysics/CollisionDetection/FreshnessChecker.cs b/BepuPhysics/CollisionDetection/FreshnessChecker.cs index 903d22175..678fce0a5 100644 --- a/BepuPhysics/CollisionDetection/FreshnessChecker.cs +++ b/BepuPhysics/CollisionDetection/FreshnessChecker.cs @@ -149,7 +149,7 @@ unsafe void PrintRemovalInformation(ConstraintHandle constraintHandle) for (int i = 0; i < typeProcessor.BodiesPerConstraint; ++i) { var bodyHandle = constraintRemover.bodies.ActiveSet.IndexToHandle[references[i]]; - Console.Write($"{bodyHandle} ({constraintRemover.bodies.ActiveSet.Constraints[references[i]].Count}), "); + Console.Write($"{bodyHandle} ({constraintRemover.bodies.ActiveSet.Constraints[references[i]].References.Count}), "); } Console.WriteLine(); diff --git a/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs b/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs index 2bee68413..a8c98d6bf 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs @@ -130,12 +130,12 @@ static unsafe void AddToSimulationSpeculative(int bodyIndex, BufferPool pool, ref TTraversalPredicate predicate) where TTraversalPredicate : IPredicate { bodyEnumerator.SourceIndex = bodyIndex; - ref var list = ref bodies.ActiveSet.Constraints[bodyIndex]; + ref var list = ref bodies.ActiveSet.Constraints[bodyIndex].References; for (int i = 0; i < list.Count; ++i) { ref var entry = ref list[i]; diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index adace0df0..f4ca179cb 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -370,7 +370,7 @@ internal unsafe void ValidateExistingHandles(bool activeOnly = false) //Now for the body->constraint direction. for (int bodyIndex = 0; bodyIndex < bodySet.Count; ++bodyIndex) { - ref var constraintList = ref bodySet.Constraints[bodyIndex]; + ref var constraintList = ref bodySet.Constraints[bodyIndex].References; for (int constraintIndex = 0; constraintIndex < constraintList.Count; ++constraintIndex) { ref var constraintLocation = ref HandleToConstraint[constraintList[constraintIndex].ConnectingConstraintHandle.Value]; @@ -644,7 +644,7 @@ public ConstraintHandle Add(Span bodyHandles, ref TDes { var bodyHandle = bodyHandles[i]; bodies.ValidateExistingHandle(bodyHandle); - bodies.AddConstraint(bodies.HandleToLocation[bodyHandle.Value].Index, constraintHandle, i); + bodies.AddConstraint(this, bodies.HandleToLocation[bodyHandle.Value].Index, constraintHandle, i); } return constraintHandle; } @@ -909,7 +909,7 @@ private bool UpdateConstraintsForBodyMemoryMove(int originalIndex, int newIndex) //That's not impossible by any means, but consider that this function will tend to be called in a deferred way- we have control over how many cache optimizations //we perform. We do not, however, have any control over how many adds must be performed. Those must be performed immediately for correctness. //In other words, doing a little more work here can reduce the overall work required, in addition to simplifying the storage requirements. - ref var list = ref bodies.ActiveSet.Constraints[originalIndex]; + ref var list = ref bodies.ActiveSet.Constraints[originalIndex].References; bool bodyShouldBePresentInFallback = false; for (int i = 0; i < list.Count; ++i) { diff --git a/Demos/Program.cs b/Demos/Program.cs index 6c769ece5..7118826dd 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -19,7 +19,7 @@ static void Main(string[] args) { content = ContentArchive.Load(stream); } - HeadlessTest.Test(content, 4, 32, 512); + //HeadlessTest.Test(content, 4, 32, 512); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); diff --git a/Demos/SpecializedTests/SimulationScrambling.cs b/Demos/SpecializedTests/SimulationScrambling.cs index 0daee0dfd..70850960a 100644 --- a/Demos/SpecializedTests/SimulationScrambling.cs +++ b/Demos/SpecializedTests/SimulationScrambling.cs @@ -51,7 +51,7 @@ public static void ScrambleBodyConstraintLists(Simulation simulation) //Note that we cannot change the order of bodies within constraints! That would change behavior. for (int bodyIndex = 0; bodyIndex < simulation.Bodies.ActiveSet.Count; ++bodyIndex) { - ref var list = ref simulation.Bodies.ActiveSet.Constraints[bodyIndex]; + ref var list = ref simulation.Bodies.ActiveSet.Constraints[bodyIndex].References; for (int i = 0; i < list.Count - 1; ++i) { ref var currentSlot = ref list[i]; @@ -207,7 +207,7 @@ private static void ChurnRemoveBody(Simulation simulation, BodyHandle[] bodyH //Remove a body. var removedBodyIndex = random.Next(simulation.Bodies.ActiveSet.Count); //All constraints associated with the body have to be removed first. - ref var constraintList = ref simulation.Bodies.ActiveSet.Constraints[removedBodyIndex]; + ref var constraintList = ref simulation.Bodies.ActiveSet.Constraints[removedBodyIndex].References; for (int i = constraintList.Count - 1; i >= 0; --i) { WriteLine($"Removing constraint (handle: {constraintList[i].ConnectingConstraintHandle}) for a body removal."); From 04839531d3d6ceece04f3fc8b94af723571fedbf Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 7 Jul 2021 00:49:18 -0500 Subject: [PATCH 080/947] Broken state tracking. --- BepuPhysics/Bodies.cs | 19 ++- BepuPhysics/BodyProperties.cs | 9 ++ BepuPhysics/BodySet.cs | 141 +++++++++++++++--- .../CollisionDetection/ConstraintRemover.cs | 2 +- .../ConstraintGraphRemovalEnumerator.cs | 2 +- BepuPhysics/Solver.cs | 16 ++ BepuPhysics/Solver_Solve2.cs | 1 - BepuPhysics/UnconstrainedBodies.cs | 83 +++++++++++ Demos/Program.cs | 1 + 9 files changed, 246 insertions(+), 28 deletions(-) create mode 100644 BepuPhysics/UnconstrainedBodies.cs diff --git a/BepuPhysics/Bodies.cs b/BepuPhysics/Bodies.cs index 1aa2407cf..f38c805f2 100644 --- a/BepuPhysics/Bodies.cs +++ b/BepuPhysics/Bodies.cs @@ -60,6 +60,7 @@ public partial class Bodies internal Shapes shapes; internal BroadPhase broadPhase; internal Solver solver; + public UnconstrainedBodies UnconstrainedBodies { get; private set; } /// /// Gets or sets the minimum constraint capacity for each body. Future resizes or allocations will obey this minimum, but changing this does not immediately resize existing lists. @@ -90,6 +91,7 @@ public unsafe Bodies(BufferPool pool, Shapes shapes, BroadPhase broadPhase, this.shapes = shapes; this.broadPhase = broadPhase; MinimumConstraintCapacityPerBody = initialConstraintCapacityPerBody; + UnconstrainedBodies = new UnconstrainedBodies(initialBodyCapacity, pool); } /// @@ -180,7 +182,7 @@ public unsafe BodyHandle Add(in BodyDescription description) //All new bodies are active for simplicity. Someday, it may be worth offering an optimized path for inactives, but it adds complexity. //(Directly adding inactive bodies can be helpful in some networked open world scenarios.) var handle = new BodyHandle(handleIndex); - var index = ActiveSet.Add(description, handle, MinimumConstraintCapacityPerBody, Pool); + var index = ActiveSet.Add(description, handle, UnconstrainedBodies, MinimumConstraintCapacityPerBody, Pool); HandleToLocation[handleIndex] = new BodyMemoryLocation { SetIndex = 0, Index = index }; if (description.Collidable.Shape.Exists) @@ -205,7 +207,7 @@ internal BodyHandle RemoveFromActiveSet(int activeBodyIndex) RemoveCollidableFromBroadPhase(ref collidable); } - var bodyMoved = set.RemoveAt(activeBodyIndex, out var handle, out var movedBodyIndex, out var movedBodyHandle); + var bodyMoved = set.RemoveAt(activeBodyIndex, UnconstrainedBodies, out var handle, out var movedBodyIndex, out var movedBodyHandle); if (bodyMoved) { //While the removed body doesn't have any constraints associated with it, the body that gets moved to fill its slot might! @@ -259,17 +261,18 @@ public void Remove(BodyHandle handle) /// Index of the body in the constraint. internal void AddConstraint(Solver solver, int bodyIndex, ConstraintHandle constraintHandle, int indexInConstraint) { - ActiveSet.AddConstraint(solver, bodyIndex, constraintHandle, indexInConstraint, Pool); + ActiveSet.AddConstraint(solver, UnconstrainedBodies, bodyIndex, constraintHandle, indexInConstraint, Pool); } /// /// Removes a constraint from an active body's constraint list. /// + /// Solver to which the constraint belongs. /// Index of the active body. /// Handle of the constraint to remove. - internal void RemoveConstraintReference(int bodyIndex, ConstraintHandle constraintHandle) + internal void RemoveConstraintReference(Solver solver, int bodyIndex, ConstraintHandle constraintHandle) { - ActiveSet.RemoveConstraintReference(bodyIndex, constraintHandle, MinimumConstraintCapacityPerBody, Pool); + ActiveSet.RemoveConstraintReference(solver, UnconstrainedBodies, bodyIndex, constraintHandle, MinimumConstraintCapacityPerBody, Pool); } /// @@ -678,6 +681,7 @@ public unsafe void Clear() if (set.Allocated) set.Dispose(Pool); } + UnconstrainedBodies.Clear(); Unsafe.InitBlockUnaligned(HandleToLocation.Memory, 0xFF, (uint)(sizeof(BodyMemoryLocation) * HandleToLocation.Length)); HandlePool.Clear(); } @@ -737,6 +741,7 @@ public void Resize(int capacity) ActiveSet.InternalResize(targetBodyCapacity, Pool); } ResizeInertias(capacity); + UnconstrainedBodies.Resize(capacity, Pool); var targetHandleCapacity = BufferPool.GetCapacityForCount(Math.Max(capacity, HandlePool.HighestPossiblyClaimedId + 1)); if (HandleToLocation.Length != targetHandleCapacity) { @@ -762,13 +767,14 @@ public void ResizeConstraintListCapacities() /// /// Increases the size of active body buffers if needed to hold the target capacity. /// - /// Target data capacity. + /// Target body capacity. public void EnsureCapacity(int capacity) { if (ActiveSet.IndexToHandle.Length < capacity) { ActiveSet.InternalResize(capacity, Pool); } + UnconstrainedBodies.EnsureCapacity(capacity, Pool); EnsureInertiasCapacity(capacity); if (HandleToLocation.Length < capacity) { @@ -808,6 +814,7 @@ public void Dispose() Pool.Return(ref Inertias); Pool.Return(ref HandleToLocation); HandlePool.Dispose(Pool); + UnconstrainedBodies.Dispose(Pool); } } diff --git a/BepuPhysics/BodyProperties.cs b/BepuPhysics/BodyProperties.cs index 22a0b74ee..d9806a4e0 100644 --- a/BepuPhysics/BodyProperties.cs +++ b/BepuPhysics/BodyProperties.cs @@ -331,6 +331,15 @@ public struct BodyConstraints /// public ConstraintHandle MaximumConstraint; + /// + /// Index of the body within the minimum batch index constraint. If the body has no constraints, this value is undefined. + /// + public int MinimumIndexInConstraint; + /// + /// Index of the body within the maximum batch index constraint. If the body has no constraints, this value is undefined. + /// + public int MaximumIndexInConstraint; + /// /// Gets whether this body has any constraints based on the value in the UnconstrainedIndex field. /// diff --git a/BepuPhysics/BodySet.cs b/BepuPhysics/BodySet.cs index 975854c41..9de77a165 100644 --- a/BepuPhysics/BodySet.cs +++ b/BepuPhysics/BodySet.cs @@ -63,7 +63,7 @@ public BodySet(int initialCapacity, BufferPool pool) : this() InternalResize(initialCapacity, pool); } - internal int Add(in BodyDescription bodyDescription, BodyHandle handle, int minimumConstraintCapacity, BufferPool pool) + internal int Add(in BodyDescription bodyDescription, BodyHandle handle, UnconstrainedBodies unconstrainedBodies, int minimumConstraintCapacity, BufferPool pool) { var index = Count; if (index == IndexToHandle.Length) @@ -73,18 +73,26 @@ internal int Add(in BodyDescription bodyDescription, BodyHandle handle, int mini ++Count; IndexToHandle[index] = handle; //Collidable's broad phase index is left unset. The Bodies collection is responsible for attaching that data. - Constraints[index].References = new QuickList(minimumConstraintCapacity, pool); + ref var constraints = ref Constraints[index]; + constraints.References = new QuickList(minimumConstraintCapacity, pool); + constraints.UnconstrainedIndex = unconstrainedBodies.Add(index, pool); + ApplyDescriptionByIndex(index, bodyDescription); return index; } - internal bool RemoveAt(int bodyIndex, out BodyHandle handle, out int movedBodyIndex, out BodyHandle movedBodyHandle) + internal bool RemoveAt(int bodyIndex, UnconstrainedBodies unconstrainedBodies, out BodyHandle handle, out int movedBodyIndex, out BodyHandle movedBodyHandle) { handle = IndexToHandle[bodyIndex]; //Move the last body into the removed slot. --Count; bool bodyMoved = bodyIndex < Count; + ref var constraintsForRemovedSlot = ref Constraints[bodyIndex]; + if (constraintsForRemovedSlot.References.Count == 0 && unconstrainedBodies.RemoveAt(constraintsForRemovedSlot.UnconstrainedIndex, out var bodyMovedInUnconstrainedSet)) + { + Constraints[bodyMovedInUnconstrainedSet].UnconstrainedIndex = constraintsForRemovedSlot.UnconstrainedIndex; + } if (bodyMoved) { movedBodyIndex = Count; @@ -97,7 +105,11 @@ internal bool RemoveAt(int bodyIndex, out BodyHandle handle, out int movedBodyIn //The two callers for this function are 'true' removal, and sleeping. //During true removal, the caller is responsible for removing all constraints and disposing the list. //In sleeping, the reference to the list is simply copied into the sleeping set. - Constraints[bodyIndex] = Constraints[movedBodyIndex]; + constraintsForRemovedSlot = Constraints[movedBodyIndex]; + if (constraintsForRemovedSlot.References.Count == 0) + { + unconstrainedBodies.UpdateForBodyMemoryMove(constraintsForRemovedSlot.UnconstrainedIndex, bodyIndex); + } //Point the body handles at the new location. movedBodyHandle = IndexToHandle[movedBodyIndex]; IndexToHandle[bodyIndex] = movedBodyHandle; @@ -160,7 +172,7 @@ public void GetDescription(int index, out BodyDescription description) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - internal void AddConstraint(Solver solver, int bodyIndex, ConstraintHandle constraintHandle, int bodyIndexInConstraint, BufferPool pool) + internal void AddConstraint(Solver solver, UnconstrainedBodies unconstrainedBodies, int bodyIndex, ConstraintHandle constraintHandle, int bodyIndexInConstraint, BufferPool pool) { BodyConstraintReference constraint; constraint.ConnectingConstraintHandle = constraintHandle; @@ -171,16 +183,24 @@ internal void AddConstraint(Solver solver, int bodyIndex, ConstraintHandle const constraints.References.Resize(constraints.References.Span.Length * 2, pool); constraints.References.AllocateUnsafely() = constraint; - var batchIndex = solver.HandleToConstraint[constraintHandle.Value].BatchIndex; + ref var constraintLocation = ref solver.HandleToConstraint[constraintHandle.Value]; + Debug.Assert(constraintLocation.SetIndex == 0, "Constraints should only be added to active bodies."); + var batchIndex = constraintLocation.BatchIndex; if (constraints.References.Count == 1) { //The body is transitioning from unconstrained to constrained. The constraint will now be responsible for its integration. - //UnconstrainedBodies.Remove(bodyIndex); + if (unconstrainedBodies.RemoveAt(constraints.UnconstrainedIndex, out var movedUnconstrainedBodyIndex)) + { + Constraints[movedUnconstrainedBodyIndex].UnconstrainedIndex = constraints.UnconstrainedIndex; + } constraints.MinimumBatch = batchIndex; constraints.MaximumBatch = batchIndex; constraints.MinimumConstraint = constraintHandle; constraints.MaximumConstraint = constraintHandle; - //solver.AddIntegrationResponsibilityToConstraint(constraintHandle, bodyIndex, bodyIndexInConstraint); + constraints.MinimumIndexInConstraint = bodyIndexInConstraint; + constraints.MaximumIndexInConstraint = bodyIndexInConstraint; + solver.AddEarlyIntegrationResponsibilityToConstraint(constraintHandle, bodyIndexInConstraint, bodyIndex); + solver.AddLateIntegrationResponsibilityToConstraint(constraintHandle, bodyIndexInConstraint, bodyIndex); } else { @@ -188,35 +208,118 @@ internal void AddConstraint(Solver solver, int bodyIndex, ConstraintHandle const var maximum = constraints.MaximumBatch; if (batchIndex < minimum) { - //solver.RemoveIntegrationResponsibilityFromConstraint(constraints.MinimumConstraint, bodyIndex); + solver.RemoveEarlyIntegrationResponsibilityFromConstraint(constraints.MinimumConstraint, constraints.MinimumIndexInConstraint, bodyIndex); constraints.MinimumBatch = batchIndex; constraints.MinimumConstraint = constraintHandle; - //solver.AddIntegrationResponsibilityToConstraint(constraintHandle, bodyIndex, bodyIndexInConstraint); + constraints.MinimumIndexInConstraint = bodyIndexInConstraint; + solver.AddEarlyIntegrationResponsibilityToConstraint(constraintHandle, bodyIndexInConstraint, bodyIndex); } else if (batchIndex > maximum) { - //solver.RemoveIntegrationResponsibilityFromConstraint(constraints.MinimumConstraint, bodyIndex); + solver.RemoveLateIntegrationResponsibilityFromConstraint(constraints.MaximumConstraint, constraints.MaximumIndexInConstraint, bodyIndex); constraints.MaximumBatch = batchIndex; constraints.MinimumConstraint = constraintHandle; - //solver.AddIntegrationResponsibilityToConstraint(constraintHandle, bodyIndex, bodyIndexInConstraint); + constraints.MaximumIndexInConstraint = bodyIndexInConstraint; + solver.AddLateIntegrationResponsibilityToConstraint(constraintHandle, bodyIndexInConstraint, bodyIndex); } } } [MethodImpl(MethodImplOptions.AggressiveInlining)] - internal void RemoveConstraintReference(int bodyIndex, ConstraintHandle constraintHandle, int minimumConstraintCapacityPerBody, BufferPool pool) + internal void RemoveConstraintReference(Solver solver, UnconstrainedBodies unconstrainedBodies, int bodyIndex, ConstraintHandle constraintHandle, int minimumConstraintCapacityPerBody, BufferPool pool) { //This uses a linear search. That's fine; bodies will rarely have more than a handful of constraints associated with them. //Attempting to use something like a hash set for fast removes would just introduce more constant overhead and slow it down on average. - ref var list = ref Constraints[bodyIndex].References; - for (int i = 0; i < list.Count; ++i) + ref var constraints = ref Constraints[bodyIndex]; + ref var constraintLocation = ref solver.HandleToConstraint[constraintHandle.Value]; + Debug.Assert(constraintLocation.SetIndex == 0, "Removals must only occur on the active set."); + ref var list = ref constraints.References; + var isMinimum = constraintLocation.BatchIndex == constraints.MinimumBatch; + if (isMinimum || constraintLocation.BatchIndex == constraints.MaximumBatch) + { + //This constraint used to have integration responsibility for this body. + if (isMinimum) + { + solver.RemoveEarlyIntegrationResponsibilityFromConstraint(constraints.MinimumConstraint, constraints.MinimumIndexInConstraint, bodyIndex); + } + else + { + solver.RemoveLateIntegrationResponsibilityFromConstraint(constraints.MaximumConstraint, constraints.MaximumIndexInConstraint, bodyIndex); + } + if (list.Count == 1) + { + //Removing this constraint from the list will leave the body unconstrained, and it should enter the unconstrained integration set. + constraints.UnconstrainedIndex = unconstrainedBodies.Add(bodyIndex, pool); + list.Count = 0; + } + else + { + //We need to know which constraint is now responsible for the body. + + int newBatchIndex = int.MaxValue; + int newIndexInConstraint = -1; + ConstraintHandle newConstraintHandle = default; + if (isMinimum) + { + //Find the new minimum batch index, and remove the target constraint. + for (int i = list.Count - 1; i >= 0; --i) + { + ref var reference = ref list[i]; + if (reference.ConnectingConstraintHandle.Value == constraintHandle.Value) + { + list.FastRemoveAt(i); + } + else + { + var batchIndex = solver.HandleToConstraint[reference.ConnectingConstraintHandle.Value].BatchIndex; + if (batchIndex < newBatchIndex) + { + newBatchIndex = batchIndex; + newConstraintHandle = reference.ConnectingConstraintHandle; + newIndexInConstraint = reference.BodyIndexInConstraint; + } + } + } + solver.AddEarlyIntegrationResponsibilityToConstraint(newConstraintHandle, newIndexInConstraint, bodyIndex); + } + else + { + //Find the new maximum batch index, and remove the target constraint. + for (int i = list.Count - 1; i >= 0; --i) + { + ref var reference = ref list[i]; + if (reference.ConnectingConstraintHandle.Value == constraintHandle.Value) + { + list.FastRemoveAt(i); + } + else + { + var batchIndex = solver.HandleToConstraint[reference.ConnectingConstraintHandle.Value].BatchIndex; + if (batchIndex > newBatchIndex) + { + newBatchIndex = batchIndex; + newConstraintHandle = reference.ConnectingConstraintHandle; + newIndexInConstraint = reference.BodyIndexInConstraint; + } + } + } + solver.AddLateIntegrationResponsibilityToConstraint(newConstraintHandle, newIndexInConstraint, bodyIndex); + } + } + + } + else { - ref var element = ref list[i]; - if (element.ConnectingConstraintHandle.Value == constraintHandle.Value) + //This constraint did not have any integration responsibilities; we can remove it with no fanfare. + for (int i = 0; i < list.Count; ++i) { - list.FastRemoveAt(i); - break; + ref var element = ref list[i]; + if (element.ConnectingConstraintHandle.Value == constraintHandle.Value) + { + list.FastRemoveAt(i); + break; + } } } //Note the conservative resizing threshold. If the current capacity is 8, the minimum capacity is 4, and the current count is 4, it COULD resize, diff --git a/BepuPhysics/CollisionDetection/ConstraintRemover.cs b/BepuPhysics/CollisionDetection/ConstraintRemover.cs index 51b994ccf..dc58f4575 100644 --- a/BepuPhysics/CollisionDetection/ConstraintRemover.cs +++ b/BepuPhysics/CollisionDetection/ConstraintRemover.cs @@ -341,7 +341,7 @@ public void RemoveConstraintsFromBodyLists() for (int j = 0; j < removals.Count; ++j) { ref var target = ref removals[j]; - bodies.RemoveConstraintReference(target.BodyIndex, target.ConstraintHandle); + bodies.RemoveConstraintReference(solver, target.BodyIndex, target.ConstraintHandle); } } } diff --git a/BepuPhysics/ConstraintGraphRemovalEnumerator.cs b/BepuPhysics/ConstraintGraphRemovalEnumerator.cs index 2ab3de17e..9a07cff34 100644 --- a/BepuPhysics/ConstraintGraphRemovalEnumerator.cs +++ b/BepuPhysics/ConstraintGraphRemovalEnumerator.cs @@ -17,7 +17,7 @@ struct ConstraintGraphRemovalEnumerator : IForEach public void LoopBody(int bodyIndex) { //Note that this only looks in the active set. Directly removing inactive objects is unsupported- removals and adds activate all involved islands. - bodies.RemoveConstraintReference(bodyIndex, constraintHandle); + bodies.RemoveConstraintReference(bodies.solver, bodyIndex, constraintHandle); } } diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index f4ca179cb..3935442f2 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -1077,6 +1077,22 @@ internal void GetSynchronizedBatchCount(out int synchronizedBatchCount, out bool "There cannot be more than FallbackBatchThreshold + 1 constraint batches because that +1 is the fallback batch which contains all remaining constraints."); } + internal void RemoveLateIntegrationResponsibilityFromConstraint(ConstraintHandle constraint, int bodyIndexInConstraint, int bodyIndex) + { + } + + internal void AddLateIntegrationResponsibilityToConstraint(ConstraintHandle constraint, int bodyIndexInConstraint, int bodyIndex) + { + } + + internal void AddEarlyIntegrationResponsibilityToConstraint(ConstraintHandle constraint, int bodyIndexInConstraint, int bodyIndex) + { + } + + internal void RemoveEarlyIntegrationResponsibilityFromConstraint(ConstraintHandle constraint, int bodyIndexInConstraint, int bodyIndex) + { + } + //Note that none of these affect the constraint batch estimates or type batch estimates. The assumption is that those are too small to bother with. //In the worst case you might see a couple of kilobytes. The reason why these functions exist is to deal with the potential many *megabytes* worth of constraint and body buffers. //Maybe something weird happens where this assumption is invalidated later, but I doubt it. There is a cost in API complexity to support it, so we don't. diff --git a/BepuPhysics/Solver_Solve2.cs b/BepuPhysics/Solver_Solve2.cs index 7ba9ceaa4..b01833959 100644 --- a/BepuPhysics/Solver_Solve2.cs +++ b/BepuPhysics/Solver_Solve2.cs @@ -190,6 +190,5 @@ public void Solve2(float dt, IThreadDispatcher threadDispatcher = null) ExecuteMultithreaded(dt, threadDispatcher, solve2Worker); } } - } } diff --git a/BepuPhysics/UnconstrainedBodies.cs b/BepuPhysics/UnconstrainedBodies.cs new file mode 100644 index 000000000..16efed491 --- /dev/null +++ b/BepuPhysics/UnconstrainedBodies.cs @@ -0,0 +1,83 @@ +using BepuUtilities.Memory; +using System; +using System.Diagnostics; + +namespace BepuPhysics +{ + public class UnconstrainedBodies + { + /// + /// Indices of bodies in the active set that are unconstrained. + /// + public Buffer BodyIndices; + public int Count; + public UnconstrainedBodies(int initialCapacity, BufferPool pool) + { + pool.TakeAtLeast(initialCapacity, out BodyIndices); + } + + internal int Add(int bodyIndex, BufferPool pool) + { + if (Count == BodyIndices.Length) + { + pool.ResizeToAtLeast(ref BodyIndices, Count * 2, BodyIndices.Length); + } + var index = Count++; + BodyIndices[index] = bodyIndex; + return index; + } + + internal bool RemoveAt(int unconstrainedIndex, out int movedBodyIndex) + { + Debug.Assert(unconstrainedIndex >= 0 && unconstrainedIndex < Count, "Unconstrained index should fall within the unconstrained set."); + --Count; + if (unconstrainedIndex < Count) + { + //The removal target is not the last element, so we'll move the current last element to the removal index. + movedBodyIndex = BodyIndices[Count]; + return true; + } + else + { + movedBodyIndex = -1; + return false; + } + } + + internal void UpdateForBodyMemoryMove(int unconstrainedIndex, int bodyIndex) + { + BodyIndices[unconstrainedIndex] = bodyIndex; + } + + + public void EnsureCapacity(int capacity, BufferPool pool) + { + capacity = capacity > BodyIndices.Length ? capacity : BodyIndices.Length; + var target = BufferPool.GetCapacityForCount(capacity); + if (target != BodyIndices.Length) + { + pool.ResizeToAtLeast(ref BodyIndices, target, Count); + } + } + public void Resize(int capacity, BufferPool pool) + { + capacity = capacity > Count ? capacity : Count; + var target = BufferPool.GetCapacityForCount(capacity); + if (target != BodyIndices.Length) + { + pool.ResizeToAtLeast(ref BodyIndices, target, Count); + } + } + + public void Clear() + { + Count = 0; + } + + public void Dispose(BufferPool pool) + { + pool.Return(ref BodyIndices); + Count = 0; + } + } +} \ No newline at end of file diff --git a/Demos/Program.cs b/Demos/Program.cs index 7118826dd..88a7152e5 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -20,6 +20,7 @@ static void Main(string[] args) content = ContentArchive.Load(stream); } //HeadlessTest.Test(content, 4, 32, 512); + //DeterminismTest.Test(content, 2, 4096); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); From 135e565c81b290d05de41557e9a70a25b8b4e853 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 7 Jul 2021 11:35:36 -0500 Subject: [PATCH 081/947] Added validation for integration responsibilities. Couple of bugs fixed, more remain. --- BepuPhysics/Bodies.cs | 75 +++++++++++++++++++++++++++++- BepuPhysics/BodySet.cs | 19 ++++++-- BepuPhysics/Simulation.cs | 1 + BepuPhysics/UnconstrainedBodies.cs | 1 + Demos/Demo.cs | 2 +- Demos/Program.cs | 2 +- 6 files changed, 93 insertions(+), 7 deletions(-) diff --git a/BepuPhysics/Bodies.cs b/BepuPhysics/Bodies.cs index f38c805f2..5953368ff 100644 --- a/BepuPhysics/Bodies.cs +++ b/BepuPhysics/Bodies.cs @@ -216,8 +216,8 @@ internal BodyHandle RemoveFromActiveSet(int activeBodyIndex) HandleToLocation[movedBodyHandle.Value].Index = activeBodyIndex; } return handle; - } + /// /// Removes an active body by its index. Any constraints connected to this body will be removed. Assumes that the input location is valid. /// @@ -528,6 +528,79 @@ internal void ValidateMotionStates() } } + struct IntegrationValidationEnumerator : IForEach + { + public int BodyReference; + public int ExpectedIndexInConstraint; + public int EnumeratedCount; + public int FoundIndex; + + public void LoopBody(int i) + { + if (BodyReference == i) + { + FoundIndex = EnumeratedCount; + } + ++EnumeratedCount; + } + } + private void ValidateBodyConstraintIntegrationBounds(int bodyReference, in BodyConstraints constraints, int batchCount) + { + Debug.Assert(constraints.MinimumBatch >= 0 && constraints.MinimumBatch < batchCount && constraints.MinimumBatch <= constraints.MaximumBatch, + "Constrained bodies must have a minimum batch index that points back to a valid existing constraint batch."); + Debug.Assert(constraints.MaximumBatch >= 0 && constraints.MaximumBatch < batchCount, + "Constrained bodies must have a maximum batch index that points back to a valid existing constraint batch."); + IntegrationValidationEnumerator enumerator; + enumerator.EnumeratedCount = 0; + enumerator.ExpectedIndexInConstraint = constraints.MinimumIndexInConstraint; + enumerator.BodyReference = bodyReference; + enumerator.FoundIndex = -1; + solver.EnumerateConnectedBodies(constraints.MinimumConstraint, ref enumerator); + Debug.Assert(enumerator.FoundIndex == constraints.MinimumIndexInConstraint, "Body should be in the constraint that it listed as a integration-responsible bound."); + + enumerator.EnumeratedCount = 0; + enumerator.ExpectedIndexInConstraint = constraints.MaximumIndexInConstraint; + enumerator.FoundIndex = -1; + solver.EnumerateConnectedBodies(constraints.MaximumConstraint, ref enumerator); + Debug.Assert(enumerator.FoundIndex == constraints.MaximumIndexInConstraint, "Body should be in the constraint that it listed as a integration-responsible bound."); + } + + [Conditional("DEBUG")] + + internal void ValidateIntegrationResponsibilities() + { + Debug.Assert(UnconstrainedBodies.Count <= UnconstrainedBodies.BodyIndices.Length, "Unconstrained bodies count/length corrupted. Bad asynchronous access?"); + for (int bodyIndex = 0; bodyIndex < ActiveSet.Count; ++bodyIndex) + { + ref var constraints = ref ActiveSet.Constraints[bodyIndex]; + if (constraints.References.Count == 0) + { + Debug.Assert(constraints.UnconstrainedIndex >= 0 && constraints.UnconstrainedIndex < UnconstrainedBodies.Count, "Unconstrained bodies must point to a valid position in the unconstrained set."); + Debug.Assert(UnconstrainedBodies.BodyIndices[constraints.UnconstrainedIndex] == bodyIndex, "Bidirectional mapping between bodies and unconstrained set entries must match."); + } + else + { + ValidateBodyConstraintIntegrationBounds(bodyIndex, constraints, solver.ActiveSet.Batches.Count); + } + } + for (int setIndex = 1; setIndex < Sets.Length; ++setIndex) + { + ref var set = ref Sets[setIndex]; + if (set.Allocated) + { + for (int bodyIndex = 0; bodyIndex < set.Count; ++bodyIndex) + { + ref var constraints = ref set.Constraints[bodyIndex]; + if (constraints.References.Count > 0) + { + ValidateBodyConstraintIntegrationBounds(set.IndexToHandle[bodyIndex].Value, constraints, solver.Sets[setIndex].Batches.Count); + } + } + } + } + } + + internal void ResizeSetsCapacity(int setsCapacity, int potentiallyAllocatedCount) { Debug.Assert(setsCapacity >= potentiallyAllocatedCount && potentiallyAllocatedCount <= Sets.Length); diff --git a/BepuPhysics/BodySet.cs b/BepuPhysics/BodySet.cs index 9de77a165..d8aee9ae9 100644 --- a/BepuPhysics/BodySet.cs +++ b/BepuPhysics/BodySet.cs @@ -219,7 +219,7 @@ internal void AddConstraint(Solver solver, UnconstrainedBodies unconstrainedBodi { solver.RemoveLateIntegrationResponsibilityFromConstraint(constraints.MaximumConstraint, constraints.MaximumIndexInConstraint, bodyIndex); constraints.MaximumBatch = batchIndex; - constraints.MinimumConstraint = constraintHandle; + constraints.MaximumConstraint = constraintHandle; constraints.MaximumIndexInConstraint = bodyIndexInConstraint; solver.AddLateIntegrationResponsibilityToConstraint(constraintHandle, bodyIndexInConstraint, bodyIndex); } @@ -257,11 +257,11 @@ internal void RemoveConstraintReference(Solver solver, UnconstrainedBodies uncon { //We need to know which constraint is now responsible for the body. - int newBatchIndex = int.MaxValue; - int newIndexInConstraint = -1; - ConstraintHandle newConstraintHandle = default; if (isMinimum) { + int newBatchIndex = int.MaxValue; + int newIndexInConstraint = -1; + ConstraintHandle newConstraintHandle = default; //Find the new minimum batch index, and remove the target constraint. for (int i = list.Count - 1; i >= 0; --i) { @@ -281,10 +281,17 @@ internal void RemoveConstraintReference(Solver solver, UnconstrainedBodies uncon } } } + constraints.MinimumBatch = newBatchIndex; + constraints.MinimumConstraint = newConstraintHandle; + constraints.MinimumIndexInConstraint = newIndexInConstraint; + Debug.Assert(solver.HandleToConstraint[newConstraintHandle.Value].SetIndex >= 0); solver.AddEarlyIntegrationResponsibilityToConstraint(newConstraintHandle, newIndexInConstraint, bodyIndex); } else { + int newBatchIndex = -1; + int newIndexInConstraint = -1; + ConstraintHandle newConstraintHandle = default; //Find the new maximum batch index, and remove the target constraint. for (int i = list.Count - 1; i >= 0; --i) { @@ -304,6 +311,10 @@ internal void RemoveConstraintReference(Solver solver, UnconstrainedBodies uncon } } } + constraints.MaximumBatch = newBatchIndex; + constraints.MaximumConstraint = newConstraintHandle; + constraints.MaximumIndexInConstraint = newIndexInConstraint; + Debug.Assert(solver.HandleToConstraint[newConstraintHandle.Value].SetIndex >= 0); solver.AddLateIntegrationResponsibilityToConstraint(newConstraintHandle, newIndexInConstraint, bodyIndex); } } diff --git a/BepuPhysics/Simulation.cs b/BepuPhysics/Simulation.cs index b2ae97f80..c19037c36 100644 --- a/BepuPhysics/Simulation.cs +++ b/BepuPhysics/Simulation.cs @@ -353,6 +353,7 @@ public void Timestep(float dt, IThreadDispatcher threadDispatcher = null) profiler.Start(this); Timestepper.Timestep(this, dt, threadDispatcher); + Bodies.ValidateIntegrationResponsibilities(); profiler.End(this); } diff --git a/BepuPhysics/UnconstrainedBodies.cs b/BepuPhysics/UnconstrainedBodies.cs index 16efed491..266194722 100644 --- a/BepuPhysics/UnconstrainedBodies.cs +++ b/BepuPhysics/UnconstrainedBodies.cs @@ -35,6 +35,7 @@ internal bool RemoveAt(int unconstrainedIndex, out int movedBodyIndex) { //The removal target is not the last element, so we'll move the current last element to the removal index. movedBodyIndex = BodyIndices[Count]; + BodyIndices[unconstrainedIndex] = movedBodyIndex; return true; } else diff --git a/Demos/Demo.cs b/Demos/Demo.cs index f00656836..aba8044d8 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -62,7 +62,7 @@ public virtual void Update(Window window, Camera camera, Input input, float dt) //fully decouple simulation and rendering rates across different threads. //(In either case, you'd also want to interpolate or extrapolate simulation results during rendering for smoothness.) //Note that taking steps of variable length can reduce stability. Gradual or one-off changes can work reasonably well. - Simulation.Timestep(1 / 60f, ThreadDispatcher); + Simulation.Timestep(1 / 60f); ////Here's an example of how it would look to use more frequent updates, but still with a fixed amount of time simulated per update call: //const float timeToSimulate = 1 / 60f; diff --git a/Demos/Program.cs b/Demos/Program.cs index 88a7152e5..b992a8c48 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -20,7 +20,7 @@ static void Main(string[] args) content = ContentArchive.Load(stream); } //HeadlessTest.Test(content, 4, 32, 512); - //DeterminismTest.Test(content, 2, 4096); + DeterminismTest.Test(content, 4, 4096); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); From 07154990c45a079c2019d3af6d7aceffbaf9ff99 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 7 Jul 2021 13:01:13 -0500 Subject: [PATCH 082/947] More validation. --- BepuPhysics/Bodies.cs | 11 +++++++++++ BepuPhysics/BodySet.cs | 8 +++----- 2 files changed, 14 insertions(+), 5 deletions(-) diff --git a/BepuPhysics/Bodies.cs b/BepuPhysics/Bodies.cs index 5953368ff..805918d02 100644 --- a/BepuPhysics/Bodies.cs +++ b/BepuPhysics/Bodies.cs @@ -546,6 +546,17 @@ public void LoopBody(int i) } private void ValidateBodyConstraintIntegrationBounds(int bodyReference, in BodyConstraints constraints, int batchCount) { + int expectedMinimum = int.MaxValue; + int expectedMaximum = -1; + for (int i = 0; i < constraints.References.Count; ++i) + { + var batchIndex = solver.HandleToConstraint[constraints.References[i].ConnectingConstraintHandle.Value].BatchIndex; + if (batchIndex < expectedMinimum) + expectedMinimum = batchIndex; + if (batchIndex > expectedMaximum) + expectedMaximum = batchIndex; + } + Debug.Assert(constraints.MinimumBatch == expectedMinimum && constraints.MaximumBatch == expectedMaximum, "Minimum and maximum batch index bounds should match the brute force determined results."); Debug.Assert(constraints.MinimumBatch >= 0 && constraints.MinimumBatch < batchCount && constraints.MinimumBatch <= constraints.MaximumBatch, "Constrained bodies must have a minimum batch index that points back to a valid existing constraint batch."); Debug.Assert(constraints.MaximumBatch >= 0 && constraints.MaximumBatch < batchCount, diff --git a/BepuPhysics/BodySet.cs b/BepuPhysics/BodySet.cs index d8aee9ae9..301eb64d0 100644 --- a/BepuPhysics/BodySet.cs +++ b/BepuPhysics/BodySet.cs @@ -232,11 +232,10 @@ internal void RemoveConstraintReference(Solver solver, UnconstrainedBodies uncon //This uses a linear search. That's fine; bodies will rarely have more than a handful of constraints associated with them. //Attempting to use something like a hash set for fast removes would just introduce more constant overhead and slow it down on average. ref var constraints = ref Constraints[bodyIndex]; - ref var constraintLocation = ref solver.HandleToConstraint[constraintHandle.Value]; - Debug.Assert(constraintLocation.SetIndex == 0, "Removals must only occur on the active set."); + Debug.Assert(solver.HandleToConstraint[constraintHandle.Value].SetIndex == 0, "Removals must only occur on the active set."); ref var list = ref constraints.References; - var isMinimum = constraintLocation.BatchIndex == constraints.MinimumBatch; - if (isMinimum || constraintLocation.BatchIndex == constraints.MaximumBatch) + var isMinimum = constraintHandle == constraints.MinimumConstraint; + if (isMinimum || constraintHandle == constraints.MaximumConstraint) { //This constraint used to have integration responsibility for this body. if (isMinimum) @@ -256,7 +255,6 @@ internal void RemoveConstraintReference(Solver solver, UnconstrainedBodies uncon else { //We need to know which constraint is now responsible for the body. - if (isMinimum) { int newBatchIndex = int.MaxValue; From 52c1563652a33d0d9fe1ef293f8f963b5d079509 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 7 Jul 2021 16:16:26 -0500 Subject: [PATCH 083/947] More validation, fixed awakener not dealing with unconstrained indices. --- BepuPhysics/Bodies.cs | 2 +- BepuPhysics/BodySet.cs | 2 ++ BepuPhysics/IslandAwakener.cs | 19 +++++++++++++++++++ BepuPhysics/PositionFirstTimestepper.cs | 8 +++++++- BepuPhysics/Simulation.cs | 2 +- BepuPhysics/UnconstrainedBodies.cs | 12 ++++++++---- Demos/Demos/FountainStressTestDemo.cs | 7 +++++++ 7 files changed, 45 insertions(+), 7 deletions(-) diff --git a/BepuPhysics/Bodies.cs b/BepuPhysics/Bodies.cs index 805918d02..269693769 100644 --- a/BepuPhysics/Bodies.cs +++ b/BepuPhysics/Bodies.cs @@ -578,7 +578,7 @@ private void ValidateBodyConstraintIntegrationBounds(int bodyReference, in BodyC [Conditional("DEBUG")] - internal void ValidateIntegrationResponsibilities() + public void ValidateIntegrationResponsibilities() { Debug.Assert(UnconstrainedBodies.Count <= UnconstrainedBodies.BodyIndices.Length, "Unconstrained bodies count/length corrupted. Bad asynchronous access?"); for (int bodyIndex = 0; bodyIndex < ActiveSet.Count; ++bodyIndex) diff --git a/BepuPhysics/BodySet.cs b/BepuPhysics/BodySet.cs index 301eb64d0..290294ab5 100644 --- a/BepuPhysics/BodySet.cs +++ b/BepuPhysics/BodySet.cs @@ -189,6 +189,8 @@ internal void AddConstraint(Solver solver, UnconstrainedBodies unconstrainedBodi if (constraints.References.Count == 1) { //The body is transitioning from unconstrained to constrained. The constraint will now be responsible for its integration. + Debug.Assert(constraints.UnconstrainedIndex >= 0 && constraints.UnconstrainedIndex < unconstrainedBodies.Count); + Debug.Assert(unconstrainedBodies.BodyIndices[constraints.UnconstrainedIndex] == bodyIndex); if (unconstrainedBodies.RemoveAt(constraints.UnconstrainedIndex, out var movedUnconstrainedBodyIndex)) { Constraints[movedUnconstrainedBodyIndex].UnconstrainedIndex = constraints.UnconstrainedIndex; diff --git a/BepuPhysics/IslandAwakener.cs b/BepuPhysics/IslandAwakener.cs index a66177ead..41928b80c 100644 --- a/BepuPhysics/IslandAwakener.cs +++ b/BepuPhysics/IslandAwakener.cs @@ -178,6 +178,7 @@ enum PhaseTwoJobType { BroadPhase, CopyConstraintRegion, + UpdateUnconstrainedBodies, } struct PhaseTwoJob { @@ -375,6 +376,22 @@ internal unsafe void ExecutePhaseTwoJob(int index) job.SourceStart, job.TargetStart, job.Count, bodies, solver); } break; + case PhaseTwoJobType.UpdateUnconstrainedBodies: + { + //Unconstrained bodies are tracked so that they can be integrated without any of the constraints handling it for them. + //It requires modifying a shared list, so we couldn't do it as a part of the phase 1 copy job. Instead, it's deferred to a locally sequential job here. + //Note that the unconstrained body set's memory capacity was ensured on the main thread prior to the dispatch. + for (int i = 0; i < job.Count; ++i) + { + var bodyIndex = i + job.TargetStart; + ref var constraints = ref bodies.ActiveSet.Constraints[bodyIndex]; + if (constraints.References.Count == 0) + { + constraints.UnconstrainedIndex = bodies.UnconstrainedBodies.AddUnsafely(bodyIndex); + } + } + } + break; } } @@ -555,6 +572,7 @@ void AccumulatePairCacheTypeCounts(ref Buffer sourceTypeCaches, r //We now know how many new bodies, constraint batch entries, and pair cache entries are going to be added. //Ensure capacities on all systems: //bodies, + var preAwakeningBodyCount = bodies.ActiveSet.Count; bodies.EnsureCapacity(bodies.ActiveSet.Count + newBodyCount); //broad phase, (technically overestimating, not every body has a collidable, but vast majority do and shrug) broadPhase.EnsureCapacity(broadPhase.ActiveTree.LeafCount + newBodyCount, broadPhase.StaticTree.LeafCount); @@ -632,6 +650,7 @@ void EnsurePairCacheTypeCapacities(ref TypeAllocationSizes cache { phaseOneJobs.AllocateUnsafely() = new PhaseOneJob { Type = PhaseOneJobType.UpdateBatchReferencedHandles, BatchIndex = batchIndex }; } + phaseTwoJobs.AllocateUnsafely() = new PhaseTwoJob { Type = PhaseTwoJobType.UpdateUnconstrainedBodies, TargetStart = preAwakeningBodyCount, Count = newBodyCount }; phaseTwoJobs.AllocateUnsafely() = new PhaseTwoJob { Type = PhaseTwoJobType.BroadPhase }; ref var activeBodySet = ref bodies.ActiveSet; diff --git a/BepuPhysics/PositionFirstTimestepper.cs b/BepuPhysics/PositionFirstTimestepper.cs index 9fa619912..859b819a8 100644 --- a/BepuPhysics/PositionFirstTimestepper.cs +++ b/BepuPhysics/PositionFirstTimestepper.cs @@ -36,8 +36,10 @@ public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDi //So, velocity integration (and deactivation candidacy management) could come before sleep. //Sleep at the start, on the other hand, stops some forms of unintuitive behavior when using direct awakenings. Just a matter of preference. + simulation.Bodies.ValidateIntegrationResponsibilities(); simulation.Sleep(threadDispatcher); Slept?.Invoke(dt, threadDispatcher); + simulation.Bodies.ValidateIntegrationResponsibilities(); //Note that pose integrator comes before collision detection and solving. This is a shift from v1, where collision detection went first. //This is a tradeoff: @@ -47,7 +49,7 @@ public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDi //2) By bundling bounding box calculation with pose integration, you avoid redundant pose and velocity memory accesses. //3) Generated contact positions are in sync with the integrated poses. //That's often helpful for gameplay purposes- you don't have to reinterpret contact data when creating graphical effects or positioning sound sources. - + //#1 is a difficult problem, though. There is no fully 'correct' place to change velocities. We might just have to bite the bullet and create a //inertia tensor/bounding box update separate from pose integration. If the cache gets evicted in between (virtually guaranteed unless no stages run), //this basically means an extra 100-200 microseconds per frame on a processor with ~20GBps bandwidth simulating 32768 bodies. @@ -58,14 +60,18 @@ public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDi //2) local->world inertia calculation before the solver. simulation.IntegrateBodiesAndUpdateBoundingBoxes(dt, threadDispatcher); BeforeCollisionDetection?.Invoke(dt, threadDispatcher); + simulation.Bodies.ValidateIntegrationResponsibilities(); simulation.CollisionDetection(dt, threadDispatcher); CollisionsDetected?.Invoke(dt, threadDispatcher); + simulation.Bodies.ValidateIntegrationResponsibilities(); simulation.Solve(dt, threadDispatcher); ConstraintsSolved?.Invoke(dt, threadDispatcher); + simulation.Bodies.ValidateIntegrationResponsibilities(); simulation.IncrementallyOptimizeDataStructures(threadDispatcher); + simulation.Bodies.ValidateIntegrationResponsibilities(); } } } diff --git a/BepuPhysics/Simulation.cs b/BepuPhysics/Simulation.cs index c19037c36..9019617a1 100644 --- a/BepuPhysics/Simulation.cs +++ b/BepuPhysics/Simulation.cs @@ -332,7 +332,7 @@ public void IncrementallyOptimizeDataStructures(IThreadDispatcher threadDispatch profiler.End(ConstraintLayoutOptimizer); profiler.Start(SolverBatchCompressor); - SolverBatchCompressor.Compress(BufferPool, threadDispatcher, threadDispatcher != null && Deterministic); + //SolverBatchCompressor.Compress(BufferPool, threadDispatcher, threadDispatcher != null && Deterministic); profiler.End(SolverBatchCompressor); } diff --git a/BepuPhysics/UnconstrainedBodies.cs b/BepuPhysics/UnconstrainedBodies.cs index 266194722..f978f743e 100644 --- a/BepuPhysics/UnconstrainedBodies.cs +++ b/BepuPhysics/UnconstrainedBodies.cs @@ -15,6 +15,13 @@ public UnconstrainedBodies(int initialCapacity, BufferPool pool) { pool.TakeAtLeast(initialCapacity, out BodyIndices); } + internal int AddUnsafely(int bodyIndex) + { + Debug.Assert(Count < BodyIndices.Length); + var index = Count++; + BodyIndices[index] = bodyIndex; + return index; + } internal int Add(int bodyIndex, BufferPool pool) { @@ -22,11 +29,8 @@ internal int Add(int bodyIndex, BufferPool pool) { pool.ResizeToAtLeast(ref BodyIndices, Count * 2, BodyIndices.Length); } - var index = Count++; - BodyIndices[index] = bodyIndex; - return index; + return AddUnsafely(bodyIndex); } - internal bool RemoveAt(int unconstrainedIndex, out int movedBodyIndex) { Debug.Assert(unconstrainedIndex >= 0 && unconstrainedIndex < Count, "Unconstrained index should fall within the unconstrained set."); diff --git a/Demos/Demos/FountainStressTestDemo.cs b/Demos/Demos/FountainStressTestDemo.cs index 1d12db084..983a5f47f 100644 --- a/Demos/Demos/FountainStressTestDemo.cs +++ b/Demos/Demos/FountainStressTestDemo.cs @@ -247,6 +247,7 @@ public override void Update(Window window, Camera camera, Input input, float dt) var anglePerKinematic = MathHelper.TwoPi / kinematicHandles.Length; var maxDisplacement = 50 * timestepDuration; var inverseDt = 1f / timestepDuration; + Simulation.Bodies.ValidateIntegrationResponsibilities(); for (int i = 0; i < kinematicHandles.Length; ++i) { ref var bodyLocation = ref Simulation.Bodies.HandleToLocation[kinematicHandles[i].Value]; @@ -284,6 +285,7 @@ public override void Update(Window window, Camera camera, Input input, float dt) } } } + Simulation.Bodies.ValidateIntegrationResponsibilities(); //Remove some statics from the simulation. var missingStaticsAsymptote = 512; @@ -295,6 +297,7 @@ public override void Update(Window window, Camera camera, Input input, float dt) Simulation.Statics.RemoveAt(indexToRemove); removedStatics.Enqueue(staticDescription, BufferPool); } + Simulation.Bodies.ValidateIntegrationResponsibilities(); var staticApplyDescriptionsPerFrame = 8; for (int i = 0; i < staticApplyDescriptionsPerFrame; ++i) @@ -310,6 +313,7 @@ public override void Update(Window window, Camera camera, Input input, float dt) Simulation.Statics.ApplyDescription(handleToReapply, staticDescription); } + Simulation.Bodies.ValidateIntegrationResponsibilities(); //Add some of the missing static bodies back into the simulation. var staticAddCount = removedStatics.Count * (staticRemovalsPerFrame / (float)missingStaticsAsymptote); @@ -319,6 +323,7 @@ public override void Update(Window window, Camera camera, Input input, float dt) var staticDescription = removedStatics.Dequeue(); Simulation.Statics.Add(staticDescription); } + Simulation.Bodies.ValidateIntegrationResponsibilities(); //Spray some shapes! @@ -347,6 +352,7 @@ public override void Update(Window window, Camera camera, Input input, float dt) } } + Simulation.Bodies.ValidateIntegrationResponsibilities(); //Change some dynamic objects without adding/removing them to make sure all the state transition stuff works reasonably well. var dynamicApplyDescriptionsPerFrame = 8; for (int i = 0; i < dynamicApplyDescriptionsPerFrame; ++i) @@ -362,6 +368,7 @@ public override void Update(Window window, Camera camera, Input input, float dt) } Simulation.Bodies.ApplyDescription(handle, newDescription); } + Simulation.Bodies.ValidateIntegrationResponsibilities(); base.Update(window, camera, input, dt); From 72c4a08821f1f6f17d3df55ec06bcea90fd2a695 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 7 Jul 2021 20:09:19 -0500 Subject: [PATCH 084/947] Slowly debugging and refactoring the integration responsibilities. Removes currently use solver handle->constraint lookups, which isn't allowed due to narrow phase flush. Should extract all integration responsibilities code from bodyset and make it deferrable. --- BepuPhysics/Bodies.cs | 15 ------ BepuPhysics/BodyProperties.cs | 6 +-- BepuPhysics/CollisionDetection/NarrowPhase.cs | 5 ++ .../CollisionDetection/NarrowPhasePreflush.cs | 2 +- BepuPhysics/IslandAwakener.cs | 53 ++++++++++++------- BepuPhysics/UnconstrainedBodies.cs | 16 +++--- 6 files changed, 53 insertions(+), 44 deletions(-) diff --git a/BepuPhysics/Bodies.cs b/BepuPhysics/Bodies.cs index 269693769..b2cbbcab4 100644 --- a/BepuPhysics/Bodies.cs +++ b/BepuPhysics/Bodies.cs @@ -594,21 +594,6 @@ public void ValidateIntegrationResponsibilities() ValidateBodyConstraintIntegrationBounds(bodyIndex, constraints, solver.ActiveSet.Batches.Count); } } - for (int setIndex = 1; setIndex < Sets.Length; ++setIndex) - { - ref var set = ref Sets[setIndex]; - if (set.Allocated) - { - for (int bodyIndex = 0; bodyIndex < set.Count; ++bodyIndex) - { - ref var constraints = ref set.Constraints[bodyIndex]; - if (constraints.References.Count > 0) - { - ValidateBodyConstraintIntegrationBounds(set.IndexToHandle[bodyIndex].Value, constraints, solver.Sets[setIndex].Batches.Count); - } - } - } - } } diff --git a/BepuPhysics/BodyProperties.cs b/BepuPhysics/BodyProperties.cs index d9806a4e0..747d728c3 100644 --- a/BepuPhysics/BodyProperties.cs +++ b/BepuPhysics/BodyProperties.cs @@ -309,16 +309,16 @@ public struct BodyConstraints public QuickList References; /// - /// Index of the body in the unconstrained integration set if it has no constraints. If the body has constraints, this value is undefined. + /// Index of the body in the unconstrained integration set if it has no constraints. If the body has constraints or is asleep, this value is undefined. /// public int UnconstrainedIndex; /// - /// Inclusive minimum constraint batch index that this body is associated with if the body has constraints. If it has no constraints, this value is undefined. + /// Inclusive minimum constraint batch index that this body is associated with if the body has constraints. If it has no constraints or is asleep, this value is undefined. /// public int MinimumBatch; /// - /// Inclusive maximum constraint batch index that this body is associated with if the body has constraints. If it has no constraints, this value is undefined. + /// Inclusive maximum constraint batch index that this body is associated with if the body has constraints. If it has no constraints or is asleep, this value is undefined. /// public int MaximumBatch; diff --git a/BepuPhysics/CollisionDetection/NarrowPhase.cs b/BepuPhysics/CollisionDetection/NarrowPhase.cs index 9c5da2a9d..e6ff6afc2 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhase.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhase.cs @@ -237,7 +237,9 @@ void ExecuteFlushJob(ref NarrowPhaseFlushJob job, BufferPool threadPool) public void Flush(IThreadDispatcher threadDispatcher = null) { var deterministic = threadDispatcher != null && Simulation.Deterministic; + Simulation.Bodies.ValidateIntegrationResponsibilities(); OnPreflush(threadDispatcher, deterministic); + Simulation.Bodies.ValidateIntegrationResponsibilities(); //var start = Stopwatch.GetTimestamp(); flushJobs = new QuickList(128, Pool); PairCache.PrepareFlushJobs(ref flushJobs); @@ -258,6 +260,7 @@ public void Flush(IThreadDispatcher threadDispatcher = null) { flushJobs.AddUnsafely(new NarrowPhaseFlushJob { Type = NarrowPhaseFlushJobType.RemoveConstraintFromTypeBatch, Index = i }); } + Simulation.Bodies.ValidateIntegrationResponsibilities(); if (threadDispatcher == null) { @@ -273,6 +276,7 @@ public void Flush(IThreadDispatcher threadDispatcher = null) threadDispatcher.DispatchWorkers(flushWorkerLoop); this.threadDispatcher = null; } + Simulation.Bodies.ValidateIntegrationResponsibilities(); //var end = Stopwatch.GetTimestamp(); //Console.WriteLine($"Flush stage 3 time (us): {1e6 * (end - start) / Stopwatch.Frequency}"); flushJobs.Dispose(Pool); @@ -282,6 +286,7 @@ public void Flush(IThreadDispatcher threadDispatcher = null) ConstraintRemover.Postflush(); OnPostflush(threadDispatcher); + Simulation.Bodies.ValidateIntegrationResponsibilities(); } public void Clear() diff --git a/BepuPhysics/CollisionDetection/NarrowPhasePreflush.cs b/BepuPhysics/CollisionDetection/NarrowPhasePreflush.cs index 7c468a930..e9042b7b0 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhasePreflush.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhasePreflush.cs @@ -402,7 +402,7 @@ protected override void OnPreflush(IThreadDispatcher threadDispatcher, bool dete for (int i = 0; i < awakenerPhaseOneJobCount; ++i) Simulation.Awakener.ExecutePhaseOneJob(i); //Note that phase one of awakener must occur before the constraint flush. Phase one registers the newly awakened constraints in constraint batches. - //This this was not done, pending adds might end up in the same batches as newly awake constraints that share bodies. + //If this was not done, pending adds might end up in the same batches as newly awake constraints that share bodies. for (int i = 0; i < awakenerPhaseTwoJobCount; ++i) Simulation.Awakener.ExecutePhaseTwoJob(i); overlapWorkers[0].PendingConstraints.FlushSequentially(Simulation, PairCache); diff --git a/BepuPhysics/IslandAwakener.cs b/BepuPhysics/IslandAwakener.cs index 41928b80c..7725db955 100644 --- a/BepuPhysics/IslandAwakener.cs +++ b/BepuPhysics/IslandAwakener.cs @@ -178,7 +178,6 @@ enum PhaseTwoJobType { BroadPhase, CopyConstraintRegion, - UpdateUnconstrainedBodies, } struct PhaseTwoJob { @@ -273,7 +272,6 @@ internal unsafe void ExecutePhaseOneJob(int index) ref var sourceSet = ref bodies.Sets[job.SourceSet]; ref var targetSet = ref bodies.ActiveSet; sourceSet.Collidables.CopyTo(job.SourceStart, targetSet.Collidables, job.TargetStart, job.Count); - sourceSet.Constraints.CopyTo(job.SourceStart, targetSet.Constraints, job.TargetStart, job.Count); //The world inertias must be updated as well. They are stored outside the sets. //Note that we use a manual loop copy for the local inertias and motion state since we're accessing them during the world inertia calculation anyway. //This can worsen the copy codegen a little, but it means we only have to scan the memory once. @@ -287,10 +285,44 @@ internal unsafe void ExecutePhaseOneJob(int index) ref var targetLocalInertia = ref targetSet.LocalInertias[targetIndex]; ref var sourceState = ref sourceSet.MotionStates[sourceIndex]; ref var targetState = ref targetSet.MotionStates[targetIndex]; + ref var sourceConstraints = ref sourceSet.Constraints[sourceIndex]; + ref var targetConstraints = ref targetSet.Constraints[targetIndex]; targetState = sourceState; targetLocalInertia = sourceLocalInertia; + targetConstraints = sourceConstraints; PoseIntegration.RotateInverseInertia(sourceLocalInertia.InverseInertiaTensor, sourceState.Pose.Orientation, out targetWorldInertia.InverseInertiaTensor); targetWorldInertia.InverseMass = sourceLocalInertia.InverseMass; + if (sourceConstraints.References.Count > 0) + { + //Minimum/maximum batch indices and constraints were not updated when bodies went into sleep, so we update them as they wake up. + //It's necessary since sleeping potentially changes constraint batch indices. + //Note that the handle to constraint mapping is updated in the second phase as a part of the "CopyConstraintRegion" job, + //so the mapping currently points to the sleeping set as desired. + int minimumBatchIndex = int.MaxValue; + int maximumBatchIndex = -1; + for (int j = 0; j < sourceConstraints.References.Count; ++j) + { + var batchIndex = solver.HandleToConstraint[sourceConstraints.References[j].ConnectingConstraintHandle.Value].BatchIndex; + if (batchIndex < minimumBatchIndex) + minimumBatchIndex = batchIndex; + if (batchIndex > maximumBatchIndex) + maximumBatchIndex = batchIndex; + } + targetConstraints.MinimumBatch = minimumBatchIndex; + targetConstraints.MaximumBatch = maximumBatchIndex; + } + else + { + //We can't add an unconstrained body to the unconstrained bodies set from a multithreaded context without further work. + //The good news is that sleeping unconstrained bodies are actually pretty rare in practice- they'd have to be stationary and floating, which isn't a terribly common + //thing in most simulations thanks to gravity and momentum. + + //While we could defer this into the second phase, it would typically waste a lot of time enumerating over bodies that have constraints. + //We could write out a list of bodies which have constraints, but doing that is about as expensive as adding the body to the unconstrained set directly. + //So, we just have a special multithreading-safe add that internally performs an interlocked add. + //Note that the buffer capacity was ensured during the job creation phase. + targetConstraints.UnconstrainedIndex = bodies.UnconstrainedBodies.AddMultithreaded(targetIndex); + } } sourceSet.Activity.CopyTo(job.SourceStart, targetSet.Activity, job.TargetStart, job.Count); if (resetActivityStates) @@ -376,22 +408,6 @@ internal unsafe void ExecutePhaseTwoJob(int index) job.SourceStart, job.TargetStart, job.Count, bodies, solver); } break; - case PhaseTwoJobType.UpdateUnconstrainedBodies: - { - //Unconstrained bodies are tracked so that they can be integrated without any of the constraints handling it for them. - //It requires modifying a shared list, so we couldn't do it as a part of the phase 1 copy job. Instead, it's deferred to a locally sequential job here. - //Note that the unconstrained body set's memory capacity was ensured on the main thread prior to the dispatch. - for (int i = 0; i < job.Count; ++i) - { - var bodyIndex = i + job.TargetStart; - ref var constraints = ref bodies.ActiveSet.Constraints[bodyIndex]; - if (constraints.References.Count == 0) - { - constraints.UnconstrainedIndex = bodies.UnconstrainedBodies.AddUnsafely(bodyIndex); - } - } - } - break; } } @@ -650,7 +666,6 @@ void EnsurePairCacheTypeCapacities(ref TypeAllocationSizes cache { phaseOneJobs.AllocateUnsafely() = new PhaseOneJob { Type = PhaseOneJobType.UpdateBatchReferencedHandles, BatchIndex = batchIndex }; } - phaseTwoJobs.AllocateUnsafely() = new PhaseTwoJob { Type = PhaseTwoJobType.UpdateUnconstrainedBodies, TargetStart = preAwakeningBodyCount, Count = newBodyCount }; phaseTwoJobs.AllocateUnsafely() = new PhaseTwoJob { Type = PhaseTwoJobType.BroadPhase }; ref var activeBodySet = ref bodies.ActiveSet; diff --git a/BepuPhysics/UnconstrainedBodies.cs b/BepuPhysics/UnconstrainedBodies.cs index f978f743e..29d2d7b10 100644 --- a/BepuPhysics/UnconstrainedBodies.cs +++ b/BepuPhysics/UnconstrainedBodies.cs @@ -1,6 +1,7 @@ using BepuUtilities.Memory; using System; using System.Diagnostics; +using System.Threading; namespace BepuPhysics { @@ -15,12 +16,13 @@ public UnconstrainedBodies(int initialCapacity, BufferPool pool) { pool.TakeAtLeast(initialCapacity, out BodyIndices); } - internal int AddUnsafely(int bodyIndex) + + internal int AddMultithreaded(int bodyIndex) { - Debug.Assert(Count < BodyIndices.Length); - var index = Count++; - BodyIndices[index] = bodyIndex; - return index; + var unconstrainedIndex = Interlocked.Increment(ref Count) - 1; + Debug.Assert(unconstrainedIndex < BodyIndices.Length, "The multithreaded variant assumes that the body indices buffer has been resized to the maximum required size prior to execution."); + BodyIndices[unconstrainedIndex] = bodyIndex; + return unconstrainedIndex; } internal int Add(int bodyIndex, BufferPool pool) @@ -29,7 +31,9 @@ internal int Add(int bodyIndex, BufferPool pool) { pool.ResizeToAtLeast(ref BodyIndices, Count * 2, BodyIndices.Length); } - return AddUnsafely(bodyIndex); + var index = Count++; + BodyIndices[index] = bodyIndex; + return index; } internal bool RemoveAt(int unconstrainedIndex, out int movedBodyIndex) { From 6638de9a67df5a830be0e2fb2e714d5f86ff7814 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 7 Jul 2021 20:17:17 -0500 Subject: [PATCH 085/947] Pulled unconstrained set management out of bodyset for body add/remove. --- BepuPhysics/Bodies.cs | 15 +++++++++++++-- BepuPhysics/BodySet.cs | 19 ++++--------------- 2 files changed, 17 insertions(+), 17 deletions(-) diff --git a/BepuPhysics/Bodies.cs b/BepuPhysics/Bodies.cs index b2cbbcab4..9cef478e7 100644 --- a/BepuPhysics/Bodies.cs +++ b/BepuPhysics/Bodies.cs @@ -182,9 +182,11 @@ public unsafe BodyHandle Add(in BodyDescription description) //All new bodies are active for simplicity. Someday, it may be worth offering an optimized path for inactives, but it adds complexity. //(Directly adding inactive bodies can be helpful in some networked open world scenarios.) var handle = new BodyHandle(handleIndex); - var index = ActiveSet.Add(description, handle, UnconstrainedBodies, MinimumConstraintCapacityPerBody, Pool); + var index = ActiveSet.Add(description, handle, MinimumConstraintCapacityPerBody, Pool); + ActiveSet.Constraints[index].UnconstrainedIndex = UnconstrainedBodies.Add(index, Pool); HandleToLocation[handleIndex] = new BodyMemoryLocation { SetIndex = 0, Index = index }; + if (description.Collidable.Shape.Exists) { AddCollidableToBroadPhase(handle, description.Pose, description.LocalInertia, ref ActiveSet.Collidables[index]); @@ -207,11 +209,20 @@ internal BodyHandle RemoveFromActiveSet(int activeBodyIndex) RemoveCollidableFromBroadPhase(ref collidable); } - var bodyMoved = set.RemoveAt(activeBodyIndex, UnconstrainedBodies, out var handle, out var movedBodyIndex, out var movedBodyHandle); + ref var constraintsForRemovedSlot = ref set.Constraints[activeBodyIndex]; + if (constraintsForRemovedSlot.References.Count == 0 && UnconstrainedBodies.RemoveAt(constraintsForRemovedSlot.UnconstrainedIndex, out var bodyMovedInUnconstrainedSet)) + { + set.Constraints[bodyMovedInUnconstrainedSet].UnconstrainedIndex = constraintsForRemovedSlot.UnconstrainedIndex; + } + var bodyMoved = set.RemoveAt(activeBodyIndex, out var handle, out var movedBodyIndex, out var movedBodyHandle); if (bodyMoved) { //While the removed body doesn't have any constraints associated with it, the body that gets moved to fill its slot might! solver.UpdateForBodyMemoryMove(movedBodyIndex, activeBodyIndex); + if (constraintsForRemovedSlot.References.Count == 0) + { + UnconstrainedBodies.UpdateForBodyMemoryMove(constraintsForRemovedSlot.UnconstrainedIndex, activeBodyIndex); + } Debug.Assert(HandleToLocation[movedBodyHandle.Value].SetIndex == 0 && HandleToLocation[movedBodyHandle.Value].Index == movedBodyIndex); HandleToLocation[movedBodyHandle.Value].Index = activeBodyIndex; } diff --git a/BepuPhysics/BodySet.cs b/BepuPhysics/BodySet.cs index 290294ab5..18f43c635 100644 --- a/BepuPhysics/BodySet.cs +++ b/BepuPhysics/BodySet.cs @@ -63,7 +63,7 @@ public BodySet(int initialCapacity, BufferPool pool) : this() InternalResize(initialCapacity, pool); } - internal int Add(in BodyDescription bodyDescription, BodyHandle handle, UnconstrainedBodies unconstrainedBodies, int minimumConstraintCapacity, BufferPool pool) + internal int Add(in BodyDescription bodyDescription, BodyHandle handle, int minimumConstraintCapacity, BufferPool pool) { var index = Count; if (index == IndexToHandle.Length) @@ -73,26 +73,19 @@ internal int Add(in BodyDescription bodyDescription, BodyHandle handle, Unconstr ++Count; IndexToHandle[index] = handle; //Collidable's broad phase index is left unset. The Bodies collection is responsible for attaching that data. - ref var constraints = ref Constraints[index]; - constraints.References = new QuickList(minimumConstraintCapacity, pool); - constraints.UnconstrainedIndex = unconstrainedBodies.Add(index, pool); - + Constraints[index].References = new QuickList(minimumConstraintCapacity, pool); ApplyDescriptionByIndex(index, bodyDescription); return index; } - internal bool RemoveAt(int bodyIndex, UnconstrainedBodies unconstrainedBodies, out BodyHandle handle, out int movedBodyIndex, out BodyHandle movedBodyHandle) + internal bool RemoveAt(int bodyIndex, out BodyHandle handle, out int movedBodyIndex, out BodyHandle movedBodyHandle) { handle = IndexToHandle[bodyIndex]; //Move the last body into the removed slot. --Count; bool bodyMoved = bodyIndex < Count; ref var constraintsForRemovedSlot = ref Constraints[bodyIndex]; - if (constraintsForRemovedSlot.References.Count == 0 && unconstrainedBodies.RemoveAt(constraintsForRemovedSlot.UnconstrainedIndex, out var bodyMovedInUnconstrainedSet)) - { - Constraints[bodyMovedInUnconstrainedSet].UnconstrainedIndex = constraintsForRemovedSlot.UnconstrainedIndex; - } if (bodyMoved) { movedBodyIndex = Count; @@ -105,11 +98,7 @@ internal bool RemoveAt(int bodyIndex, UnconstrainedBodies unconstrainedBodies, o //The two callers for this function are 'true' removal, and sleeping. //During true removal, the caller is responsible for removing all constraints and disposing the list. //In sleeping, the reference to the list is simply copied into the sleeping set. - constraintsForRemovedSlot = Constraints[movedBodyIndex]; - if (constraintsForRemovedSlot.References.Count == 0) - { - unconstrainedBodies.UpdateForBodyMemoryMove(constraintsForRemovedSlot.UnconstrainedIndex, bodyIndex); - } + Constraints[bodyIndex] = Constraints[movedBodyIndex]; //Point the body handles at the new location. movedBodyHandle = IndexToHandle[movedBodyIndex]; IndexToHandle[bodyIndex] = movedBodyHandle; From 1e3a2d6dc26c4c30fe7a82922219077cd6927e74 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 8 Jul 2021 15:40:21 -0500 Subject: [PATCH 086/947] Added missing integration responsibility update to constraint remover. Refactored body-side constraint stuff to allow thread safe remover/flush stuff. --- BepuPhysics/Bodies.cs | 172 +++++++++++++++++- BepuPhysics/BodySet.cs | 150 +-------------- .../CollisionDetection/ConstraintRemover.cs | 27 ++- BepuPhysics/CollisionDetection/NarrowPhase.cs | 7 +- .../NarrowPhasePendingConstraintAdds.cs | 7 +- BepuPhysics/IslandAwakener.cs | 14 +- BepuPhysics/PositionFirstTimestepper.cs | 6 - BepuPhysics/Solver.cs | 3 +- Demos/Demos/FountainStressTestDemo.cs | 6 - 9 files changed, 212 insertions(+), 180 deletions(-) diff --git a/BepuPhysics/Bodies.cs b/BepuPhysics/Bodies.cs index 9cef478e7..94ffe868f 100644 --- a/BepuPhysics/Bodies.cs +++ b/BepuPhysics/Bodies.cs @@ -191,6 +191,10 @@ public unsafe BodyHandle Add(in BodyDescription description) { AddCollidableToBroadPhase(handle, description.Pose, description.LocalInertia, ref ActiveSet.Collidables[index]); } + lock (debugBodiesAdded) + { + debugBodiesAdded.Add(index); + } return handle; } @@ -260,6 +264,10 @@ public void Remove(BodyHandle handle) { ValidateExistingHandle(handle); awakener.AwakenBody(handle); + lock (debugBodiesRemoved) + { + debugBodiesRemoved.Add(HandleToLocation[handle.Value].Index); + } RemoveAt(HandleToLocation[handle.Value].Index); } @@ -269,21 +277,160 @@ public void Remove(BodyHandle handle) /// Solver owning the constraint. /// Index of the body to add the constraint to. /// Handle of the constraint to add. + /// Location of the constraint in memory. /// Index of the body in the constraint. - internal void AddConstraint(Solver solver, int bodyIndex, ConstraintHandle constraintHandle, int indexInConstraint) + /// We pass the reference to the constraint location explicitly to try to make abundantly clear that it is *used*, and this function should only be called + /// from contexts where that information is available. We don't currently use bodies.AddConstraint from any context where that's a risk, but this is the kind of thing + /// that I forget about a year later when trying to modify the bookkeeping somehow. + internal void AddConstraint(Solver solver, int bodyIndex, ConstraintHandle constraintHandle, ref ConstraintLocation constraintLocation, int indexInConstraint) { - ActiveSet.AddConstraint(solver, UnconstrainedBodies, bodyIndex, constraintHandle, indexInConstraint, Pool); + ref var constraints = ref ActiveSet.AddConstraint(bodyIndex, constraintHandle, indexInConstraint, Pool); + + //We handle integration responsibility stuff out here since it's only relevant for active constraints. + //It's okay to do this stuff immediately, because this function is only ever called from contexts that have a valid constraint location, which isn't the case for remove. + Debug.Assert(constraintLocation.SetIndex == 0, "Constraints should only be added to active bodies."); + var batchIndex = constraintLocation.BatchIndex; + if (constraints.References.Count == 1) + { + //The body is transitioning from unconstrained to constrained. The constraint will now be responsible for its integration. + Debug.Assert(constraints.UnconstrainedIndex >= 0 && constraints.UnconstrainedIndex < UnconstrainedBodies.Count); + Debug.Assert(UnconstrainedBodies.BodyIndices[constraints.UnconstrainedIndex] == bodyIndex); + if (UnconstrainedBodies.RemoveAt(constraints.UnconstrainedIndex, out var movedUnconstrainedBodyIndex)) + { + ActiveSet.Constraints[movedUnconstrainedBodyIndex].UnconstrainedIndex = constraints.UnconstrainedIndex; + } + constraints.MinimumBatch = batchIndex; + constraints.MaximumBatch = batchIndex; + constraints.MinimumConstraint = constraintHandle; + constraints.MaximumConstraint = constraintHandle; + constraints.MinimumIndexInConstraint = indexInConstraint; + constraints.MaximumIndexInConstraint = indexInConstraint; + solver.AddEarlyIntegrationResponsibilityToConstraint(constraintHandle, indexInConstraint, bodyIndex); + solver.AddLateIntegrationResponsibilityToConstraint(constraintHandle, indexInConstraint, bodyIndex); + } + else + { + var minimum = constraints.MinimumBatch; + var maximum = constraints.MaximumBatch; + if (batchIndex < minimum) + { + solver.RemoveEarlyIntegrationResponsibilityFromConstraint(constraints.MinimumConstraint, constraints.MinimumIndexInConstraint, bodyIndex); + constraints.MinimumBatch = batchIndex; + constraints.MinimumConstraint = constraintHandle; + constraints.MinimumIndexInConstraint = indexInConstraint; + solver.AddEarlyIntegrationResponsibilityToConstraint(constraintHandle, indexInConstraint, bodyIndex); + + } + else if (batchIndex > maximum) + { + solver.RemoveLateIntegrationResponsibilityFromConstraint(constraints.MaximumConstraint, constraints.MaximumIndexInConstraint, bodyIndex); + constraints.MaximumBatch = batchIndex; + constraints.MaximumConstraint = constraintHandle; + constraints.MaximumIndexInConstraint = indexInConstraint; + solver.AddLateIntegrationResponsibilityToConstraint(constraintHandle, indexInConstraint, bodyIndex); + } + } + lock (debugBodiesConstraintAdded) + { + debugBodiesConstraintAdded.Add(bodyIndex); + } } /// /// Removes a constraint from an active body's constraint list. /// - /// Solver to which the constraint belongs. + /// Index of the active body. + /// Handle of the constraint to remove. + /// True if the constraint had integration responsibilities for the body, false otherwise. + internal bool RemoveConstraintReference(int bodyIndex, ConstraintHandle constraintHandle) + { + ref var constraints = ref ActiveSet.RemoveConstraintReference(bodyIndex, constraintHandle, MinimumConstraintCapacityPerBody, Pool); + + //If this constraint is being removed, we have to get rid of references to it in the integration responsibilities. + //This function assumes a multithreaded context that means we can't directly handle everything here, so we return a bool indicating whether this body requires integration responsibility analysis. + bool requiresUpdateIfConstraintsRemain = constraintHandle == constraints.MinimumConstraint || constraintHandle == constraints.MaximumConstraint; + if (requiresUpdateIfConstraintsRemain) + { + //If either constraint is a bound, we just clear both bounds. + //Updating both bounds costs the same amount as updating one, and by clearing out both bounds, we stop later removals from possibly triggering another update for the second bound. + constraints.MinimumConstraint.Value = -1; + constraints.MaximumConstraint.Value = -1; + } + if (constraints.References.Count == 0) + { + //If there are no constraints left associated with the body, we can immediately add the body to the unconstrained set. + //This is only called from a locally sequential context, so it's safe to do inline, unlike the integration bounds update which needs to look at solver constraint locations. + constraints.UnconstrainedIndex = UnconstrainedBodies.Add(bodyIndex, Pool); + return false; + } + return requiresUpdateIfConstraintsRemain; + } + + + internal ref BodyConstraints UpdateIntegrationResponsibilitiesForBodyWithoutSolverModifications(Solver solver, int bodyIndex) + { + ref var constraints = ref ActiveSet.Constraints[bodyIndex]; + ref var list = ref constraints.References; + if (list.Count > 0) //It's possible that an update request came before the body had all its constraints removed, meaning there is no more need for an update. + { + //Note that there's no need to remove the integration responsibility of a constraint that's being removed. + constraints.MinimumBatch = int.MaxValue; + constraints.MaximumBatch = -1; + //Find the new minimum batch index. + for (int i = 0; i < list.Count; ++i) + { + ref var reference = ref list[i]; + var batchIndex = solver.HandleToConstraint[reference.ConnectingConstraintHandle.Value].BatchIndex; + if (batchIndex < constraints.MinimumBatch) + { + constraints.MinimumBatch = batchIndex; + constraints.MinimumConstraint = reference.ConnectingConstraintHandle; + constraints.MinimumIndexInConstraint = reference.BodyIndexInConstraint; + } + if (batchIndex > constraints.MaximumBatch) + { + constraints.MaximumBatch = batchIndex; + constraints.MaximumConstraint = reference.ConnectingConstraintHandle; + constraints.MaximumIndexInConstraint = reference.BodyIndexInConstraint; + } + } + } + lock (debugConstraintRemovedDirectlyOrIndirectly) + { + debugConstraintRemovedDirectlyOrIndirectly.Add(bodyIndex); + } + return ref constraints; + } + + internal void UpdateIntegrationResponsibilitiesForBodyWithSolverModifications(Solver solver, int bodyIndex) + { + ref var constraints = ref UpdateIntegrationResponsibilitiesForBodyWithoutSolverModifications(solver, bodyIndex); + if (constraints.References.Count > 0) //It's possible that an update request came before the body had all its constraints removed, meaning there is no more need for an update. + { + solver.AddEarlyIntegrationResponsibilityToConstraint(constraints.MinimumConstraint, constraints.MinimumIndexInConstraint, bodyIndex); + solver.AddLateIntegrationResponsibilityToConstraint(constraints.MaximumConstraint, constraints.MaximumIndexInConstraint, bodyIndex); + } + lock (debugConstraintRemovedDirectlyOrIndirectly) + { + debugConstraintRemovedDirectlyOrIndirectly.Add(bodyIndex); + } + } + + /// + /// Removes a constraint from an active body's constraint list. Immdiately modifies constraint integration responsibilities. + /// /// Index of the active body. /// Handle of the constraint to remove. internal void RemoveConstraintReference(Solver solver, int bodyIndex, ConstraintHandle constraintHandle) { - ActiveSet.RemoveConstraintReference(solver, UnconstrainedBodies, bodyIndex, constraintHandle, MinimumConstraintCapacityPerBody, Pool); + if (RemoveConstraintReference(bodyIndex, constraintHandle)) + { + UpdateIntegrationResponsibilitiesForBodyWithSolverModifications(solver, bodyIndex); + } + lock (debugBodiesConstraintRemovedDirectly) + { + debugBodiesConstraintRemovedDirectly.Add(bodyIndex); + } } /// @@ -555,8 +702,15 @@ public void LoopBody(int i) ++EnumeratedCount; } } + public System.Collections.Generic.HashSet debugConstraintRemovedDirectlyOrIndirectly = new(); + public System.Collections.Generic.HashSet debugBodiesConstraintAdded = new(); + public System.Collections.Generic.HashSet debugBodiesConstraintRemovedDirectly = new(); + public System.Collections.Generic.HashSet debugBodiesAdded = new(); + public System.Collections.Generic.HashSet debugBodiesRemoved = new(); private void ValidateBodyConstraintIntegrationBounds(int bodyReference, in BodyConstraints constraints, int batchCount) { + Debug.Assert(solver.HandleToConstraint[constraints.MinimumConstraint.Value].BatchIndex == constraints.MinimumBatch); + Debug.Assert(solver.HandleToConstraint[constraints.MaximumConstraint.Value].BatchIndex == constraints.MaximumBatch); int expectedMinimum = int.MaxValue; int expectedMaximum = -1; for (int i = 0; i < constraints.References.Count; ++i) @@ -567,6 +721,11 @@ private void ValidateBodyConstraintIntegrationBounds(int bodyReference, in BodyC if (batchIndex > expectedMaximum) expectedMaximum = batchIndex; } + var constraintHadBoundsUpdatedForRemovalDeferredOrImmediate = debugConstraintRemovedDirectlyOrIndirectly.Contains(bodyReference); + var bodyHadConstraintAdded = debugBodiesConstraintAdded.Contains(bodyReference); + var bodyHadConstraintRemoved = debugBodiesConstraintRemovedDirectly.Contains(bodyReference); + var bodyWasAdded = debugBodiesAdded.Contains(bodyReference); + var bodyWasRemoved = debugBodiesRemoved.Contains(bodyReference); Debug.Assert(constraints.MinimumBatch == expectedMinimum && constraints.MaximumBatch == expectedMaximum, "Minimum and maximum batch index bounds should match the brute force determined results."); Debug.Assert(constraints.MinimumBatch >= 0 && constraints.MinimumBatch < batchCount && constraints.MinimumBatch <= constraints.MaximumBatch, "Constrained bodies must have a minimum batch index that points back to a valid existing constraint batch."); @@ -605,6 +764,11 @@ public void ValidateIntegrationResponsibilities() ValidateBodyConstraintIntegrationBounds(bodyIndex, constraints, solver.ActiveSet.Batches.Count); } } + debugConstraintRemovedDirectlyOrIndirectly.Clear(); + debugBodiesConstraintAdded.Clear(); + debugBodiesConstraintRemovedDirectly.Clear(); + debugBodiesAdded.Clear(); + debugBodiesRemoved.Clear(); } diff --git a/BepuPhysics/BodySet.cs b/BepuPhysics/BodySet.cs index 18f43c635..4cffa94c0 100644 --- a/BepuPhysics/BodySet.cs +++ b/BepuPhysics/BodySet.cs @@ -161,7 +161,7 @@ public void GetDescription(int index, out BodyDescription description) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - internal void AddConstraint(Solver solver, UnconstrainedBodies unconstrainedBodies, int bodyIndex, ConstraintHandle constraintHandle, int bodyIndexInConstraint, BufferPool pool) + internal ref BodyConstraints AddConstraint(int bodyIndex, ConstraintHandle constraintHandle, int bodyIndexInConstraint, BufferPool pool) { BodyConstraintReference constraint; constraint.ConnectingConstraintHandle = constraintHandle; @@ -171,155 +171,24 @@ internal void AddConstraint(Solver solver, UnconstrainedBodies unconstrainedBodi if (constraints.References.Span.Length == constraints.References.Count) constraints.References.Resize(constraints.References.Span.Length * 2, pool); constraints.References.AllocateUnsafely() = constraint; - - ref var constraintLocation = ref solver.HandleToConstraint[constraintHandle.Value]; - Debug.Assert(constraintLocation.SetIndex == 0, "Constraints should only be added to active bodies."); - var batchIndex = constraintLocation.BatchIndex; - if (constraints.References.Count == 1) - { - //The body is transitioning from unconstrained to constrained. The constraint will now be responsible for its integration. - Debug.Assert(constraints.UnconstrainedIndex >= 0 && constraints.UnconstrainedIndex < unconstrainedBodies.Count); - Debug.Assert(unconstrainedBodies.BodyIndices[constraints.UnconstrainedIndex] == bodyIndex); - if (unconstrainedBodies.RemoveAt(constraints.UnconstrainedIndex, out var movedUnconstrainedBodyIndex)) - { - Constraints[movedUnconstrainedBodyIndex].UnconstrainedIndex = constraints.UnconstrainedIndex; - } - constraints.MinimumBatch = batchIndex; - constraints.MaximumBatch = batchIndex; - constraints.MinimumConstraint = constraintHandle; - constraints.MaximumConstraint = constraintHandle; - constraints.MinimumIndexInConstraint = bodyIndexInConstraint; - constraints.MaximumIndexInConstraint = bodyIndexInConstraint; - solver.AddEarlyIntegrationResponsibilityToConstraint(constraintHandle, bodyIndexInConstraint, bodyIndex); - solver.AddLateIntegrationResponsibilityToConstraint(constraintHandle, bodyIndexInConstraint, bodyIndex); - } - else - { - var minimum = constraints.MinimumBatch; - var maximum = constraints.MaximumBatch; - if (batchIndex < minimum) - { - solver.RemoveEarlyIntegrationResponsibilityFromConstraint(constraints.MinimumConstraint, constraints.MinimumIndexInConstraint, bodyIndex); - constraints.MinimumBatch = batchIndex; - constraints.MinimumConstraint = constraintHandle; - constraints.MinimumIndexInConstraint = bodyIndexInConstraint; - solver.AddEarlyIntegrationResponsibilityToConstraint(constraintHandle, bodyIndexInConstraint, bodyIndex); - - } - else if (batchIndex > maximum) - { - solver.RemoveLateIntegrationResponsibilityFromConstraint(constraints.MaximumConstraint, constraints.MaximumIndexInConstraint, bodyIndex); - constraints.MaximumBatch = batchIndex; - constraints.MaximumConstraint = constraintHandle; - constraints.MaximumIndexInConstraint = bodyIndexInConstraint; - solver.AddLateIntegrationResponsibilityToConstraint(constraintHandle, bodyIndexInConstraint, bodyIndex); - } - } + return ref constraints; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - internal void RemoveConstraintReference(Solver solver, UnconstrainedBodies unconstrainedBodies, int bodyIndex, ConstraintHandle constraintHandle, int minimumConstraintCapacityPerBody, BufferPool pool) + internal ref BodyConstraints RemoveConstraintReference(int bodyIndex, ConstraintHandle constraintHandle, int minimumConstraintCapacityPerBody, BufferPool pool) { //This uses a linear search. That's fine; bodies will rarely have more than a handful of constraints associated with them. //Attempting to use something like a hash set for fast removes would just introduce more constant overhead and slow it down on average. ref var constraints = ref Constraints[bodyIndex]; - Debug.Assert(solver.HandleToConstraint[constraintHandle.Value].SetIndex == 0, "Removals must only occur on the active set."); ref var list = ref constraints.References; - var isMinimum = constraintHandle == constraints.MinimumConstraint; - if (isMinimum || constraintHandle == constraints.MaximumConstraint) - { - //This constraint used to have integration responsibility for this body. - if (isMinimum) - { - solver.RemoveEarlyIntegrationResponsibilityFromConstraint(constraints.MinimumConstraint, constraints.MinimumIndexInConstraint, bodyIndex); - } - else - { - solver.RemoveLateIntegrationResponsibilityFromConstraint(constraints.MaximumConstraint, constraints.MaximumIndexInConstraint, bodyIndex); - } - if (list.Count == 1) - { - //Removing this constraint from the list will leave the body unconstrained, and it should enter the unconstrained integration set. - constraints.UnconstrainedIndex = unconstrainedBodies.Add(bodyIndex, pool); - list.Count = 0; - } - else - { - //We need to know which constraint is now responsible for the body. - if (isMinimum) - { - int newBatchIndex = int.MaxValue; - int newIndexInConstraint = -1; - ConstraintHandle newConstraintHandle = default; - //Find the new minimum batch index, and remove the target constraint. - for (int i = list.Count - 1; i >= 0; --i) - { - ref var reference = ref list[i]; - if (reference.ConnectingConstraintHandle.Value == constraintHandle.Value) - { - list.FastRemoveAt(i); - } - else - { - var batchIndex = solver.HandleToConstraint[reference.ConnectingConstraintHandle.Value].BatchIndex; - if (batchIndex < newBatchIndex) - { - newBatchIndex = batchIndex; - newConstraintHandle = reference.ConnectingConstraintHandle; - newIndexInConstraint = reference.BodyIndexInConstraint; - } - } - } - constraints.MinimumBatch = newBatchIndex; - constraints.MinimumConstraint = newConstraintHandle; - constraints.MinimumIndexInConstraint = newIndexInConstraint; - Debug.Assert(solver.HandleToConstraint[newConstraintHandle.Value].SetIndex >= 0); - solver.AddEarlyIntegrationResponsibilityToConstraint(newConstraintHandle, newIndexInConstraint, bodyIndex); - } - else - { - int newBatchIndex = -1; - int newIndexInConstraint = -1; - ConstraintHandle newConstraintHandle = default; - //Find the new maximum batch index, and remove the target constraint. - for (int i = list.Count - 1; i >= 0; --i) - { - ref var reference = ref list[i]; - if (reference.ConnectingConstraintHandle.Value == constraintHandle.Value) - { - list.FastRemoveAt(i); - } - else - { - var batchIndex = solver.HandleToConstraint[reference.ConnectingConstraintHandle.Value].BatchIndex; - if (batchIndex > newBatchIndex) - { - newBatchIndex = batchIndex; - newConstraintHandle = reference.ConnectingConstraintHandle; - newIndexInConstraint = reference.BodyIndexInConstraint; - } - } - } - constraints.MaximumBatch = newBatchIndex; - constraints.MaximumConstraint = newConstraintHandle; - constraints.MaximumIndexInConstraint = newIndexInConstraint; - Debug.Assert(solver.HandleToConstraint[newConstraintHandle.Value].SetIndex >= 0); - solver.AddLateIntegrationResponsibilityToConstraint(newConstraintHandle, newIndexInConstraint, bodyIndex); - } - } - - } - else + //This constraint did not have any integration responsibilities; we can remove it with no fanfare. + for (int i = 0; i < list.Count; ++i) { - //This constraint did not have any integration responsibilities; we can remove it with no fanfare. - for (int i = 0; i < list.Count; ++i) + ref var element = ref list[i]; + if (element.ConnectingConstraintHandle.Value == constraintHandle.Value) { - ref var element = ref list[i]; - if (element.ConnectingConstraintHandle.Value == constraintHandle.Value) - { - list.FastRemoveAt(i); - break; - } + list.FastRemoveAt(i); + break; } } //Note the conservative resizing threshold. If the current capacity is 8, the minimum capacity is 4, and the current count is 4, it COULD resize, @@ -333,6 +202,7 @@ internal void RemoveConstraintReference(Solver solver, UnconstrainedBodies uncon //The list can be trimmed down a bit while still holding all existing constraints and obeying the minimum capacity. list.Resize(targetCapacity, pool); } + return ref constraints; } public bool BodyIsConstrainedBy(int bodyIndex, ConstraintHandle constraintHandle) diff --git a/BepuPhysics/CollisionDetection/ConstraintRemover.cs b/BepuPhysics/CollisionDetection/ConstraintRemover.cs index dc58f4575..c0bb19155 100644 --- a/BepuPhysics/CollisionDetection/ConstraintRemover.cs +++ b/BepuPhysics/CollisionDetection/ConstraintRemover.cs @@ -247,7 +247,7 @@ public int Compare(ref TypeBatchIndex a, ref TypeBatchIndex b) //in sequence at that point- sequential removes would cost around 5us in that case, so any kind of multithreaded overhead can overwhelm the work being done. //Doubling the cost of the best case, resulting in handfuls of wasted microseconds, isn't concerning (and we could special case it if we really wanted to). //Cutting the cost of the worst case when thousands of constraints get removed by a factor of ~ThreadCount is worth this complexity. Frame spikes are evil! - + RemovalCache batches; /// /// Processes enqueued constraint removals and prepares removal jobs. @@ -330,21 +330,34 @@ public void ReturnConstraintHandles() } } - public void RemoveConstraintsFromBodyLists() + public void RemoveConstraintsFromBodyLists(BufferPool threadPool) { //While body list removal could technically be internally multithreaded, it would be pretty complex- you would have to do one dispatch per solver.Batches batch //to guarantee that no two threads hit the same body constraint list at the same time. //That is more complicated and would almost certainly be slower than this locally sequential version. + //Further, introducing the integration responsibility tracking here makes sequential processing ~required. + int updateCapacity = 0; + for (int i = 0; i < batches.BatchCount; ++i) + { + ref var removals = ref batches.RemovalsForTypeBatches[i].PerBodyRemovalTargets; + updateCapacity += removals.Count; + } + bodiesNeedingIntegrationResponsibilityUpdate.threadPool = threadPool; + bodiesNeedingIntegrationResponsibilityUpdate.bodies = new QuickList(updateCapacity, threadPool); for (int i = 0; i < batches.BatchCount; ++i) { ref var removals = ref batches.RemovalsForTypeBatches[i].PerBodyRemovalTargets; for (int j = 0; j < removals.Count; ++j) { ref var target = ref removals[j]; - bodies.RemoveConstraintReference(solver, target.BodyIndex, target.ConstraintHandle); + if (bodies.RemoveConstraintReference(target.BodyIndex, target.ConstraintHandle)) + { + bodiesNeedingIntegrationResponsibilityUpdate.bodies.AllocateUnsafely() = target.BodyIndex; + } } } } + (QuickList bodies, BufferPool threadPool) bodiesNeedingIntegrationResponsibilityUpdate; public void RemoveConstraintsFromBatchReferencedHandles() { @@ -440,6 +453,14 @@ public void MarkAffectedConstraintsAsRemovedFromSolver() solver.HandleToConstraint[batchHandles[j].Value].SetIndex = -1; } } + Debug.Assert(bodiesNeedingIntegrationResponsibilityUpdate.threadPool != null, "This should only be called from usages of the constraint remover that removed constraints from body lists."); + for (int i = 0; i < bodiesNeedingIntegrationResponsibilityUpdate.bodies.Count; ++i) + { + bodies.UpdateIntegrationResponsibilitiesForBodyWithoutSolverModifications(solver, bodiesNeedingIntegrationResponsibilityUpdate.bodies[i]); + } + //Console.WriteLine($"Number of updates: {bodiesNeedingIntegrationResponsibilityUpdate.bodies.Count}"); + bodiesNeedingIntegrationResponsibilityUpdate.bodies.Dispose(bodiesNeedingIntegrationResponsibilityUpdate.threadPool); + bodiesNeedingIntegrationResponsibilityUpdate = default; } public void Postflush() diff --git a/BepuPhysics/CollisionDetection/NarrowPhase.cs b/BepuPhysics/CollisionDetection/NarrowPhase.cs index e6ff6afc2..f4c72201f 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhase.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhase.cs @@ -213,7 +213,7 @@ void ExecuteFlushJob(ref NarrowPhaseFlushJob job, BufferPool threadPool) switch (job.Type) { case NarrowPhaseFlushJobType.RemoveConstraintsFromBodyLists: - ConstraintRemover.RemoveConstraintsFromBodyLists(); + ConstraintRemover.RemoveConstraintsFromBodyLists(threadPool); break; case NarrowPhaseFlushJobType.ReturnConstraintHandles: ConstraintRemover.ReturnConstraintHandles(); @@ -237,9 +237,7 @@ void ExecuteFlushJob(ref NarrowPhaseFlushJob job, BufferPool threadPool) public void Flush(IThreadDispatcher threadDispatcher = null) { var deterministic = threadDispatcher != null && Simulation.Deterministic; - Simulation.Bodies.ValidateIntegrationResponsibilities(); OnPreflush(threadDispatcher, deterministic); - Simulation.Bodies.ValidateIntegrationResponsibilities(); //var start = Stopwatch.GetTimestamp(); flushJobs = new QuickList(128, Pool); PairCache.PrepareFlushJobs(ref flushJobs); @@ -260,7 +258,6 @@ public void Flush(IThreadDispatcher threadDispatcher = null) { flushJobs.AddUnsafely(new NarrowPhaseFlushJob { Type = NarrowPhaseFlushJobType.RemoveConstraintFromTypeBatch, Index = i }); } - Simulation.Bodies.ValidateIntegrationResponsibilities(); if (threadDispatcher == null) { @@ -276,7 +273,6 @@ public void Flush(IThreadDispatcher threadDispatcher = null) threadDispatcher.DispatchWorkers(flushWorkerLoop); this.threadDispatcher = null; } - Simulation.Bodies.ValidateIntegrationResponsibilities(); //var end = Stopwatch.GetTimestamp(); //Console.WriteLine($"Flush stage 3 time (us): {1e6 * (end - start) / Stopwatch.Frequency}"); flushJobs.Dispose(Pool); @@ -286,7 +282,6 @@ public void Flush(IThreadDispatcher threadDispatcher = null) ConstraintRemover.Postflush(); OnPostflush(threadDispatcher); - Simulation.Bodies.ValidateIntegrationResponsibilities(); } public void Clear() diff --git a/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs b/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs index a8c98d6bf..c6d637b29 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs @@ -130,12 +130,15 @@ static unsafe void AddToSimulationSpeculative maximumBatchIndex) - maximumBatchIndex = batchIndex; - } - targetConstraints.MinimumBatch = minimumBatchIndex; - targetConstraints.MaximumBatch = maximumBatchIndex; + //The constraint copy is also responsible for initializing solver integration responsibility flags appropriately. + bodies.UpdateIntegrationResponsibilitiesForBodyWithoutSolverModifications(solver, targetIndex); } else { diff --git a/BepuPhysics/PositionFirstTimestepper.cs b/BepuPhysics/PositionFirstTimestepper.cs index 859b819a8..064f1282a 100644 --- a/BepuPhysics/PositionFirstTimestepper.cs +++ b/BepuPhysics/PositionFirstTimestepper.cs @@ -36,10 +36,8 @@ public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDi //So, velocity integration (and deactivation candidacy management) could come before sleep. //Sleep at the start, on the other hand, stops some forms of unintuitive behavior when using direct awakenings. Just a matter of preference. - simulation.Bodies.ValidateIntegrationResponsibilities(); simulation.Sleep(threadDispatcher); Slept?.Invoke(dt, threadDispatcher); - simulation.Bodies.ValidateIntegrationResponsibilities(); //Note that pose integrator comes before collision detection and solving. This is a shift from v1, where collision detection went first. //This is a tradeoff: @@ -60,18 +58,14 @@ public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDi //2) local->world inertia calculation before the solver. simulation.IntegrateBodiesAndUpdateBoundingBoxes(dt, threadDispatcher); BeforeCollisionDetection?.Invoke(dt, threadDispatcher); - simulation.Bodies.ValidateIntegrationResponsibilities(); simulation.CollisionDetection(dt, threadDispatcher); CollisionsDetected?.Invoke(dt, threadDispatcher); - simulation.Bodies.ValidateIntegrationResponsibilities(); simulation.Solve(dt, threadDispatcher); ConstraintsSolved?.Invoke(dt, threadDispatcher); - simulation.Bodies.ValidateIntegrationResponsibilities(); simulation.IncrementallyOptimizeDataStructures(threadDispatcher); - simulation.Bodies.ValidateIntegrationResponsibilities(); } } } diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index 3935442f2..99b1c0967 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -640,11 +640,12 @@ public ConstraintHandle Add(Span bodyHandles, ref TDes awakener.AwakenBody(bodyHandles[i]); } Add(bodyHandles, ref description, out var constraintHandle); + ref var location = ref HandleToConstraint[constraintHandle.Value]; for (int i = 0; i < bodyHandles.Length; ++i) { var bodyHandle = bodyHandles[i]; bodies.ValidateExistingHandle(bodyHandle); - bodies.AddConstraint(this, bodies.HandleToLocation[bodyHandle.Value].Index, constraintHandle, i); + bodies.AddConstraint(this, bodies.HandleToLocation[bodyHandle.Value].Index, constraintHandle, ref location, i); } return constraintHandle; } diff --git a/Demos/Demos/FountainStressTestDemo.cs b/Demos/Demos/FountainStressTestDemo.cs index 983a5f47f..4e923ac4b 100644 --- a/Demos/Demos/FountainStressTestDemo.cs +++ b/Demos/Demos/FountainStressTestDemo.cs @@ -247,7 +247,6 @@ public override void Update(Window window, Camera camera, Input input, float dt) var anglePerKinematic = MathHelper.TwoPi / kinematicHandles.Length; var maxDisplacement = 50 * timestepDuration; var inverseDt = 1f / timestepDuration; - Simulation.Bodies.ValidateIntegrationResponsibilities(); for (int i = 0; i < kinematicHandles.Length; ++i) { ref var bodyLocation = ref Simulation.Bodies.HandleToLocation[kinematicHandles[i].Value]; @@ -285,7 +284,6 @@ public override void Update(Window window, Camera camera, Input input, float dt) } } } - Simulation.Bodies.ValidateIntegrationResponsibilities(); //Remove some statics from the simulation. var missingStaticsAsymptote = 512; @@ -297,7 +295,6 @@ public override void Update(Window window, Camera camera, Input input, float dt) Simulation.Statics.RemoveAt(indexToRemove); removedStatics.Enqueue(staticDescription, BufferPool); } - Simulation.Bodies.ValidateIntegrationResponsibilities(); var staticApplyDescriptionsPerFrame = 8; for (int i = 0; i < staticApplyDescriptionsPerFrame; ++i) @@ -313,7 +310,6 @@ public override void Update(Window window, Camera camera, Input input, float dt) Simulation.Statics.ApplyDescription(handleToReapply, staticDescription); } - Simulation.Bodies.ValidateIntegrationResponsibilities(); //Add some of the missing static bodies back into the simulation. var staticAddCount = removedStatics.Count * (staticRemovalsPerFrame / (float)missingStaticsAsymptote); @@ -323,7 +319,6 @@ public override void Update(Window window, Camera camera, Input input, float dt) var staticDescription = removedStatics.Dequeue(); Simulation.Statics.Add(staticDescription); } - Simulation.Bodies.ValidateIntegrationResponsibilities(); //Spray some shapes! @@ -352,7 +347,6 @@ public override void Update(Window window, Camera camera, Input input, float dt) } } - Simulation.Bodies.ValidateIntegrationResponsibilities(); //Change some dynamic objects without adding/removing them to make sure all the state transition stuff works reasonably well. var dynamicApplyDescriptionsPerFrame = 8; for (int i = 0; i < dynamicApplyDescriptionsPerFrame; ++i) From 26a7671a776b27da57cbb555ed90416899a8986f Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 8 Jul 2021 16:32:14 -0500 Subject: [PATCH 087/947] Reenabled batch compressor with proper integration responsibility tracking. --- BepuPhysics/Constraints/TypeProcessor.cs | 14 +++++++++++++- BepuPhysics/HandyEnumerators.cs | 24 ++++++++++++++++++++++++ BepuPhysics/Simulation.cs | 2 +- Demos/Demo.cs | 2 +- 4 files changed, 39 insertions(+), 3 deletions(-) diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index 2e815db32..6088af4aa 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -341,8 +341,9 @@ public unsafe override void TransferConstraint(ref TypeBatch typeBatch, int sour //It's not exactly trivial to keep everything straight, especially over time- it becomes a maintenance nightmare. //So instead, given that compressions should generally be extremely rare (relatively speaking) and highly deferrable, we'll accept some minor overhead. int bodiesPerConstraint = InternalBodiesPerConstraint; + var bodyIndices = stackalloc int[bodiesPerConstraint]; var bodyHandles = stackalloc int[bodiesPerConstraint]; - var bodyHandleCollector = new ActiveConstraintBodyHandleCollector(bodies, bodyHandles); + var bodyHandleCollector = new ActiveConstraintBodyHandleAndIndexCollector(bodies, bodyHandles, bodyIndices); EnumerateConnectedBodyIndices(ref typeBatch, indexInTypeBatch, ref bodyHandleCollector); Debug.Assert(targetBatchIndex <= solver.FallbackBatchThreshold, "Constraint transfers should never target the fallback batch. It doesn't have any body handles so attempting to allocate in the same way wouldn't turn out well."); @@ -379,6 +380,17 @@ ref Buffer.Get(ref targetReference.TypeBatch.AccumulatedImp constraintLocation.IndexInTypeBatch = targetReference.IndexInTypeBatch; constraintLocation.TypeId = typeId; + //Now that the constraint's been moved, update the integration responsibilities for any involved constraints. + for (int i = 0; i < bodiesPerConstraint; ++i) + { + var bodyIndex = bodyIndices[i]; + ref var constraints = ref bodies.ActiveSet.Constraints[bodyIndex]; + if (constraints.MinimumBatch > targetBatchIndex || constraints.MaximumConstraint == constraintHandle) + { + bodies.UpdateIntegrationResponsibilitiesForBodyWithSolverModifications(solver, bodyIndex); + } + } + } void InternalResize(ref TypeBatch typeBatch, BufferPool pool, int constraintCapacity) diff --git a/BepuPhysics/HandyEnumerators.cs b/BepuPhysics/HandyEnumerators.cs index 6b05aa334..d501c2c89 100644 --- a/BepuPhysics/HandyEnumerators.cs +++ b/BepuPhysics/HandyEnumerators.cs @@ -29,6 +29,30 @@ public void LoopBody(int bodyIndex) Handles[Index++] = Bodies.ActiveSet.IndexToHandle[bodyIndex].Value; } } + /// + /// Collects body indices and body handles associated with an active constraint as integers. + /// + public unsafe struct ActiveConstraintBodyHandleAndIndexCollector : IForEach + { + public Bodies Bodies; + public int* Indices; + public int* Handles; + public int Index; + + public ActiveConstraintBodyHandleAndIndexCollector(Bodies bodies, int* handles, int* indices) + { + Bodies = bodies; + Indices = indices; + Handles = handles; + Index = 0; + } + + public void LoopBody(int bodyIndex) + { + Indices[Index] = bodyIndex; + Handles[Index++] = Bodies.ActiveSet.IndexToHandle[bodyIndex].Value; + } + } public unsafe struct ReferenceCollector : IForEach { diff --git a/BepuPhysics/Simulation.cs b/BepuPhysics/Simulation.cs index 9019617a1..c19037c36 100644 --- a/BepuPhysics/Simulation.cs +++ b/BepuPhysics/Simulation.cs @@ -332,7 +332,7 @@ public void IncrementallyOptimizeDataStructures(IThreadDispatcher threadDispatch profiler.End(ConstraintLayoutOptimizer); profiler.Start(SolverBatchCompressor); - //SolverBatchCompressor.Compress(BufferPool, threadDispatcher, threadDispatcher != null && Deterministic); + SolverBatchCompressor.Compress(BufferPool, threadDispatcher, threadDispatcher != null && Deterministic); profiler.End(SolverBatchCompressor); } diff --git a/Demos/Demo.cs b/Demos/Demo.cs index aba8044d8..f00656836 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -62,7 +62,7 @@ public virtual void Update(Window window, Camera camera, Input input, float dt) //fully decouple simulation and rendering rates across different threads. //(In either case, you'd also want to interpolate or extrapolate simulation results during rendering for smoothness.) //Note that taking steps of variable length can reduce stability. Gradual or one-off changes can work reasonably well. - Simulation.Timestep(1 / 60f); + Simulation.Timestep(1 / 60f, ThreadDispatcher); ////Here's an example of how it would look to use more frequent updates, but still with a fixed amount of time simulated per update call: //const float timeToSimulate = 1 / 60f; From d4d7fd39e3d5e753fab16f21fe5422f2c0dad3ab Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 8 Jul 2021 19:47:07 -0500 Subject: [PATCH 088/947] It occurs to me that I've wasted multiple days on this, and there's a vastly simpler alternative. --- BepuPhysics/Bodies.cs | 16 ++++----- BepuPhysics/Constraints/TypeBatch.cs | 20 +++++++++++ BepuPhysics/Constraints/TypeProcessor.cs | 46 ++++++++++++++++++++---- BepuPhysics/Solver.cs | 26 +++++++++++--- 4 files changed, 88 insertions(+), 20 deletions(-) diff --git a/BepuPhysics/Bodies.cs b/BepuPhysics/Bodies.cs index 94ffe868f..a04974169 100644 --- a/BepuPhysics/Bodies.cs +++ b/BepuPhysics/Bodies.cs @@ -305,8 +305,8 @@ internal void AddConstraint(Solver solver, int bodyIndex, ConstraintHandle const constraints.MaximumConstraint = constraintHandle; constraints.MinimumIndexInConstraint = indexInConstraint; constraints.MaximumIndexInConstraint = indexInConstraint; - solver.AddEarlyIntegrationResponsibilityToConstraint(constraintHandle, indexInConstraint, bodyIndex); - solver.AddLateIntegrationResponsibilityToConstraint(constraintHandle, indexInConstraint, bodyIndex); + solver.AddEarlyIntegrationResponsibilityToConstraint(constraintHandle, indexInConstraint); + solver.AddLateIntegrationResponsibilityToConstraint(constraintHandle, indexInConstraint); } else { @@ -314,20 +314,20 @@ internal void AddConstraint(Solver solver, int bodyIndex, ConstraintHandle const var maximum = constraints.MaximumBatch; if (batchIndex < minimum) { - solver.RemoveEarlyIntegrationResponsibilityFromConstraint(constraints.MinimumConstraint, constraints.MinimumIndexInConstraint, bodyIndex); + solver.RemoveEarlyIntegrationResponsibilityFromConstraint(constraints.MinimumConstraint, constraints.MinimumIndexInConstraint); constraints.MinimumBatch = batchIndex; constraints.MinimumConstraint = constraintHandle; constraints.MinimumIndexInConstraint = indexInConstraint; - solver.AddEarlyIntegrationResponsibilityToConstraint(constraintHandle, indexInConstraint, bodyIndex); + solver.AddEarlyIntegrationResponsibilityToConstraint(constraintHandle, indexInConstraint); } else if (batchIndex > maximum) { - solver.RemoveLateIntegrationResponsibilityFromConstraint(constraints.MaximumConstraint, constraints.MaximumIndexInConstraint, bodyIndex); + solver.RemoveLateIntegrationResponsibilityFromConstraint(constraints.MaximumConstraint, constraints.MaximumIndexInConstraint); constraints.MaximumBatch = batchIndex; constraints.MaximumConstraint = constraintHandle; constraints.MaximumIndexInConstraint = indexInConstraint; - solver.AddLateIntegrationResponsibilityToConstraint(constraintHandle, indexInConstraint, bodyIndex); + solver.AddLateIntegrationResponsibilityToConstraint(constraintHandle, indexInConstraint); } } lock (debugBodiesConstraintAdded) @@ -407,8 +407,8 @@ internal void UpdateIntegrationResponsibilitiesForBodyWithSolverModifications(So ref var constraints = ref UpdateIntegrationResponsibilitiesForBodyWithoutSolverModifications(solver, bodyIndex); if (constraints.References.Count > 0) //It's possible that an update request came before the body had all its constraints removed, meaning there is no more need for an update. { - solver.AddEarlyIntegrationResponsibilityToConstraint(constraints.MinimumConstraint, constraints.MinimumIndexInConstraint, bodyIndex); - solver.AddLateIntegrationResponsibilityToConstraint(constraints.MaximumConstraint, constraints.MaximumIndexInConstraint, bodyIndex); + solver.AddEarlyIntegrationResponsibilityToConstraint(constraints.MinimumConstraint, constraints.MinimumIndexInConstraint); + solver.AddLateIntegrationResponsibilityToConstraint(constraints.MaximumConstraint, constraints.MaximumIndexInConstraint); } lock (debugConstraintRemovedDirectlyOrIndirectly) { diff --git a/BepuPhysics/Constraints/TypeBatch.cs b/BepuPhysics/Constraints/TypeBatch.cs index 9b1944fc1..f21cd1e66 100644 --- a/BepuPhysics/Constraints/TypeBatch.cs +++ b/BepuPhysics/Constraints/TypeBatch.cs @@ -8,6 +8,18 @@ namespace BepuPhysics.Constraints { + public struct BodyIntegrationFlags + { + /// + /// Set of flags aligned with the constraints in the type batch which, if set, indicate that the constraint is the earliest one associated with the body. + /// + public IndexSet Early; + /// + /// Set of flags aligned with the constraints in the type batch which, if set, indicate that the constraint is the latest one associated with the body. + /// + public IndexSet Late; + + } /// /// Stores the raw AOSOA formatted data associated with constraints in a type batch. /// @@ -21,6 +33,7 @@ public struct TypeBatch //and even though we may end up not even persisting the allocation between frames. We may later pull this out and store it strictly ephemerally in the solver. public RawBuffer Projection; public Buffer IndexToHandle; + public Buffer IntegrationFlags; public int ConstraintCount; public int TypeId; @@ -40,6 +53,13 @@ public void Dispose(BufferPool pool) pool.Return(ref PrestepData); pool.Return(ref AccumulatedImpulses); pool.Return(ref IndexToHandle); + for (int i = 0; i < IntegrationFlags.Length; ++i) + { + ref var flags = ref IntegrationFlags[i]; + flags.Early.Dispose(pool); + flags.Late.Dispose(pool); + } + pool.Return(ref IntegrationFlags); } } } diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index 6088af4aa..a4b75cfac 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -387,7 +387,21 @@ ref Buffer.Get(ref targetReference.TypeBatch.AccumulatedImp ref var constraints = ref bodies.ActiveSet.Constraints[bodyIndex]; if (constraints.MinimumBatch > targetBatchIndex || constraints.MaximumConstraint == constraintHandle) { - bodies.UpdateIntegrationResponsibilitiesForBodyWithSolverModifications(solver, bodyIndex); + var previousMin = constraints.MinimumConstraint; + var previousMinIndexInConstraint = constraints.MinimumIndexInConstraint; + var previousMax = constraints.MaximumConstraint; + var previousMaxIndexInConstraint = constraints.MaximumIndexInConstraint; + bodies.UpdateIntegrationResponsibilitiesForBodyWithoutSolverModifications(solver, bodyIndex); + solver.AddEarlyIntegrationResponsibilityToConstraint(constraints.MinimumConstraint, constraints.MinimumIndexInConstraint); + solver.AddLateIntegrationResponsibilityToConstraint(constraints.MaximumConstraint, constraints.MaximumIndexInConstraint); + if (constraints.MinimumConstraint != previousMin) + { + solver.RemoveEarlyIntegrationResponsibilityFromConstraint(previousMin, previousMinIndexInConstraint); + } + if (constraints.MaximumConstraint != previousMax) + { + solver.RemoveLateIntegrationResponsibilityFromConstraint(previousMax, previousMaxIndexInConstraint); + } } } @@ -406,6 +420,13 @@ void InternalResize(ref TypeBatch typeBatch, BufferPool pool, int constraintCapa pool.ResizeToAtLeast(ref typeBatch.BodyReferences, bundleCapacity * Unsafe.SizeOf(), bundleCount * Unsafe.SizeOf()); pool.ResizeToAtLeast(ref typeBatch.PrestepData, bundleCapacity * Unsafe.SizeOf(), bundleCount * Unsafe.SizeOf()); pool.ResizeToAtLeast(ref typeBatch.AccumulatedImpulses, bundleCapacity * Unsafe.SizeOf(), bundleCount * Unsafe.SizeOf()); + pool.ResizeToAtLeast(ref typeBatch.AccumulatedImpulses, bundleCapacity * Unsafe.SizeOf(), bundleCount * Unsafe.SizeOf()); + for (int i = 0; i < typeBatch.IntegrationFlags.Length; ++i) + { + ref var flags = ref typeBatch.IntegrationFlags[i]; + flags.Early.Resize(constraintCapacity, pool); + flags.Late.Resize(constraintCapacity, pool); + } } public override void Initialize(ref TypeBatch typeBatch, int initialCapacity, BufferPool pool) @@ -414,6 +435,8 @@ public override void Initialize(ref TypeBatch typeBatch, int initialCapacity, Bu //Technically we could use an initialization-specific version, but it doesn't matter. typeBatch = new TypeBatch(); typeBatch.TypeId = TypeId; + pool.Take(bodiesPerConstraint, out typeBatch.IntegrationFlags); + typeBatch.IntegrationFlags.Clear(0, typeBatch.IntegrationFlags.Length); InternalResize(ref typeBatch, pool, initialCapacity); } @@ -633,12 +656,6 @@ internal unsafe sealed override void CopySleepingToActive( ref var sourceReferencesLaneStart = ref Unsafe.Add(ref Unsafe.As(ref Buffer.Get(ref sourceTypeBatch.BodyReferences, sourceBundle)), sourceInner); ref var targetReferencesLaneStart = ref Unsafe.Add(ref Unsafe.As(ref Buffer.Get(ref targetTypeBatch.BodyReferences, targetBundle)), targetInner); - var offset = 0; - for (int j = 0; j < bodiesPerConstraint; ++j) - { - Unsafe.Add(ref targetReferencesLaneStart, offset) = bodies.HandleToLocation[Unsafe.Add(ref sourceReferencesLaneStart, offset)].Index; - offset += Vector.Count; - } var constraintHandle = sourceTypeBatch.IndexToHandle[sourceIndex]; ref var location = ref solver.HandleToConstraint[constraintHandle.Value]; Debug.Assert(location.SetIndex == sourceSet); @@ -648,6 +665,21 @@ internal unsafe sealed override void CopySleepingToActive( location.IndexInTypeBatch = targetIndex; //This could be done with a bulk copy, but eh! We already touched the memory. targetTypeBatch.IndexToHandle[targetIndex] = constraintHandle; + + //While we update body references, also update constraint integration responsibilities. + var offset = 0; + for (int j = 0; j < bodiesPerConstraint; ++j) + { + var bodyIndex = bodies.HandleToLocation[Unsafe.Add(ref sourceReferencesLaneStart, offset)].Index; + //Note that the awakener called UpdateIntegrationResponsibilitiesForBodyWithoutSolverModifications already in phase 1. + //This constraint copy need only examine the results of that. + ref var bodyConstraints = ref bodies.ActiveSet.Constraints[bodyIndex]; + ref var flags = ref targetTypeBatch.IntegrationFlags[j]; + flags.Early.SetUnsafely(targetIndex, bodyConstraints.MinimumConstraint == constraintHandle); + flags.Late.SetUnsafely(targetIndex, bodyConstraints.MaximumConstraint == constraintHandle); + Unsafe.Add(ref targetReferencesLaneStart, offset) = bodyIndex; + offset += Vector.Count; + } } } diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index 99b1c0967..c9504edb7 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -1078,22 +1078,38 @@ internal void GetSynchronizedBatchCount(out int synchronizedBatchCount, out bool "There cannot be more than FallbackBatchThreshold + 1 constraint batches because that +1 is the fallback batch which contains all remaining constraints."); } - internal void RemoveLateIntegrationResponsibilityFromConstraint(ConstraintHandle constraint, int bodyIndexInConstraint, int bodyIndex) + internal void RemoveEarlyIntegrationResponsibilityFromConstraint(ConstraintHandle constraint, int bodyIndexInConstraint) { + ref var location = ref HandleToConstraint[constraint.Value]; + Debug.Assert(location.SetIndex == 0); + ref var typeBatch = ref ActiveSet.Batches[location.BatchIndex].GetTypeBatch(location.TypeId); + typeBatch.IntegrationFlags[bodyIndexInConstraint].Early.Remove(location.IndexInTypeBatch); } - - internal void AddLateIntegrationResponsibilityToConstraint(ConstraintHandle constraint, int bodyIndexInConstraint, int bodyIndex) + internal void RemoveLateIntegrationResponsibilityFromConstraint(ConstraintHandle constraint, int bodyIndexInConstraint) { + ref var location = ref HandleToConstraint[constraint.Value]; + Debug.Assert(location.SetIndex == 0); + ref var typeBatch = ref ActiveSet.Batches[location.BatchIndex].GetTypeBatch(location.TypeId); + typeBatch.IntegrationFlags[bodyIndexInConstraint].Late.Remove(location.IndexInTypeBatch); } - internal void AddEarlyIntegrationResponsibilityToConstraint(ConstraintHandle constraint, int bodyIndexInConstraint, int bodyIndex) + internal void AddEarlyIntegrationResponsibilityToConstraint(ConstraintHandle constraint, int bodyIndexInConstraint) { + ref var location = ref HandleToConstraint[constraint.Value]; + Debug.Assert(location.SetIndex == 0); + ref var typeBatch = ref ActiveSet.Batches[location.BatchIndex].GetTypeBatch(location.TypeId); + typeBatch.IntegrationFlags[bodyIndexInConstraint].Early.AddUnsafely(location.IndexInTypeBatch); } - internal void RemoveEarlyIntegrationResponsibilityFromConstraint(ConstraintHandle constraint, int bodyIndexInConstraint, int bodyIndex) + internal void AddLateIntegrationResponsibilityToConstraint(ConstraintHandle constraint, int bodyIndexInConstraint) { + ref var location = ref HandleToConstraint[constraint.Value]; + Debug.Assert(location.SetIndex == 0); + ref var typeBatch = ref ActiveSet.Batches[location.BatchIndex].GetTypeBatch(location.TypeId); + typeBatch.IntegrationFlags[bodyIndexInConstraint].Late.AddUnsafely(location.IndexInTypeBatch); } + //Note that none of these affect the constraint batch estimates or type batch estimates. The assumption is that those are too small to bother with. //In the worst case you might see a couple of kilobytes. The reason why these functions exist is to deal with the potential many *megabytes* worth of constraint and body buffers. //Maybe something weird happens where this assumption is invalidated later, but I doubt it. There is a cost in API complexity to support it, so we don't. From 6dff2af78e4ce03eb26ce15cfc915c5659d56693 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 9 Jul 2021 10:58:09 -0500 Subject: [PATCH 089/947] Everything related to integration responsibilties purged. --- BepuPhysics/Bodies.cs | 276 +----------------- BepuPhysics/BodyProperties.cs | 47 --- BepuPhysics/BodyReference.cs | 2 +- BepuPhysics/BodySet.cs | 32 +- .../CollisionDetection/ConstraintRemover.cs | 27 +- .../CollisionDetection/FreshnessChecker.cs | 2 +- BepuPhysics/CollisionDetection/NarrowPhase.cs | 2 +- .../NarrowPhasePendingConstraintAdds.cs | 7 +- .../CollisionDetection/NarrowPhasePreflush.cs | 2 +- .../ConstraintGraphRemovalEnumerator.cs | 2 +- BepuPhysics/Constraints/TypeBatch.cs | 20 -- BepuPhysics/Constraints/TypeProcessor.cs | 58 +--- BepuPhysics/HandyEnumerators.cs | 24 -- BepuPhysics/IslandAwakener.cs | 26 +- BepuPhysics/IslandSleeper.cs | 2 +- BepuPhysics/PositionFirstTimestepper.cs | 2 +- BepuPhysics/Simulation.cs | 1 - BepuPhysics/Solver.cs | 39 +-- BepuPhysics/Solver_Solve2.cs | 1 + BepuPhysics/UnconstrainedBodies.cs | 92 ------ Demos/Demos/FountainStressTestDemo.cs | 1 - Demos/Program.cs | 3 +- .../SpecializedTests/SimulationScrambling.cs | 4 +- 23 files changed, 52 insertions(+), 620 deletions(-) delete mode 100644 BepuPhysics/UnconstrainedBodies.cs diff --git a/BepuPhysics/Bodies.cs b/BepuPhysics/Bodies.cs index a04974169..8614e3cc7 100644 --- a/BepuPhysics/Bodies.cs +++ b/BepuPhysics/Bodies.cs @@ -60,7 +60,6 @@ public partial class Bodies internal Shapes shapes; internal BroadPhase broadPhase; internal Solver solver; - public UnconstrainedBodies UnconstrainedBodies { get; private set; } /// /// Gets or sets the minimum constraint capacity for each body. Future resizes or allocations will obey this minimum, but changing this does not immediately resize existing lists. @@ -91,7 +90,6 @@ public unsafe Bodies(BufferPool pool, Shapes shapes, BroadPhase broadPhase, this.shapes = shapes; this.broadPhase = broadPhase; MinimumConstraintCapacityPerBody = initialConstraintCapacityPerBody; - UnconstrainedBodies = new UnconstrainedBodies(initialBodyCapacity, pool); } /// @@ -183,18 +181,12 @@ public unsafe BodyHandle Add(in BodyDescription description) //(Directly adding inactive bodies can be helpful in some networked open world scenarios.) var handle = new BodyHandle(handleIndex); var index = ActiveSet.Add(description, handle, MinimumConstraintCapacityPerBody, Pool); - ActiveSet.Constraints[index].UnconstrainedIndex = UnconstrainedBodies.Add(index, Pool); HandleToLocation[handleIndex] = new BodyMemoryLocation { SetIndex = 0, Index = index }; - if (description.Collidable.Shape.Exists) { AddCollidableToBroadPhase(handle, description.Pose, description.LocalInertia, ref ActiveSet.Collidables[index]); } - lock (debugBodiesAdded) - { - debugBodiesAdded.Add(index); - } return handle; } @@ -213,26 +205,17 @@ internal BodyHandle RemoveFromActiveSet(int activeBodyIndex) RemoveCollidableFromBroadPhase(ref collidable); } - ref var constraintsForRemovedSlot = ref set.Constraints[activeBodyIndex]; - if (constraintsForRemovedSlot.References.Count == 0 && UnconstrainedBodies.RemoveAt(constraintsForRemovedSlot.UnconstrainedIndex, out var bodyMovedInUnconstrainedSet)) - { - set.Constraints[bodyMovedInUnconstrainedSet].UnconstrainedIndex = constraintsForRemovedSlot.UnconstrainedIndex; - } var bodyMoved = set.RemoveAt(activeBodyIndex, out var handle, out var movedBodyIndex, out var movedBodyHandle); if (bodyMoved) { //While the removed body doesn't have any constraints associated with it, the body that gets moved to fill its slot might! solver.UpdateForBodyMemoryMove(movedBodyIndex, activeBodyIndex); - if (constraintsForRemovedSlot.References.Count == 0) - { - UnconstrainedBodies.UpdateForBodyMemoryMove(constraintsForRemovedSlot.UnconstrainedIndex, activeBodyIndex); - } Debug.Assert(HandleToLocation[movedBodyHandle.Value].SetIndex == 0 && HandleToLocation[movedBodyHandle.Value].Index == movedBodyIndex); HandleToLocation[movedBodyHandle.Value].Index = activeBodyIndex; } return handle; - } + } /// /// Removes an active body by its index. Any constraints connected to this body will be removed. Assumes that the input location is valid. /// @@ -240,7 +223,7 @@ internal BodyHandle RemoveFromActiveSet(int activeBodyIndex) public void RemoveAt(int activeBodyIndex) { //Constraints must be removed; we cannot leave 'orphans' in the solver because they will access invalid data. - ref var constraints = ref ActiveSet.Constraints[activeBodyIndex].References; + ref var constraints = ref ActiveSet.Constraints[activeBodyIndex]; for (int i = constraints.Count - 1; i >= 0; --i) { solver.Remove(constraints[i].ConnectingConstraintHandle); @@ -264,76 +247,18 @@ public void Remove(BodyHandle handle) { ValidateExistingHandle(handle); awakener.AwakenBody(handle); - lock (debugBodiesRemoved) - { - debugBodiesRemoved.Add(HandleToLocation[handle.Value].Index); - } RemoveAt(HandleToLocation[handle.Value].Index); } /// /// Adds a constraint to an active body's constraint list. /// - /// Solver owning the constraint. /// Index of the body to add the constraint to. /// Handle of the constraint to add. - /// Location of the constraint in memory. /// Index of the body in the constraint. - /// We pass the reference to the constraint location explicitly to try to make abundantly clear that it is *used*, and this function should only be called - /// from contexts where that information is available. We don't currently use bodies.AddConstraint from any context where that's a risk, but this is the kind of thing - /// that I forget about a year later when trying to modify the bookkeeping somehow. - internal void AddConstraint(Solver solver, int bodyIndex, ConstraintHandle constraintHandle, ref ConstraintLocation constraintLocation, int indexInConstraint) + internal void AddConstraint(int bodyIndex, ConstraintHandle constraintHandle, int indexInConstraint) { - ref var constraints = ref ActiveSet.AddConstraint(bodyIndex, constraintHandle, indexInConstraint, Pool); - - //We handle integration responsibility stuff out here since it's only relevant for active constraints. - //It's okay to do this stuff immediately, because this function is only ever called from contexts that have a valid constraint location, which isn't the case for remove. - Debug.Assert(constraintLocation.SetIndex == 0, "Constraints should only be added to active bodies."); - var batchIndex = constraintLocation.BatchIndex; - if (constraints.References.Count == 1) - { - //The body is transitioning from unconstrained to constrained. The constraint will now be responsible for its integration. - Debug.Assert(constraints.UnconstrainedIndex >= 0 && constraints.UnconstrainedIndex < UnconstrainedBodies.Count); - Debug.Assert(UnconstrainedBodies.BodyIndices[constraints.UnconstrainedIndex] == bodyIndex); - if (UnconstrainedBodies.RemoveAt(constraints.UnconstrainedIndex, out var movedUnconstrainedBodyIndex)) - { - ActiveSet.Constraints[movedUnconstrainedBodyIndex].UnconstrainedIndex = constraints.UnconstrainedIndex; - } - constraints.MinimumBatch = batchIndex; - constraints.MaximumBatch = batchIndex; - constraints.MinimumConstraint = constraintHandle; - constraints.MaximumConstraint = constraintHandle; - constraints.MinimumIndexInConstraint = indexInConstraint; - constraints.MaximumIndexInConstraint = indexInConstraint; - solver.AddEarlyIntegrationResponsibilityToConstraint(constraintHandle, indexInConstraint); - solver.AddLateIntegrationResponsibilityToConstraint(constraintHandle, indexInConstraint); - } - else - { - var minimum = constraints.MinimumBatch; - var maximum = constraints.MaximumBatch; - if (batchIndex < minimum) - { - solver.RemoveEarlyIntegrationResponsibilityFromConstraint(constraints.MinimumConstraint, constraints.MinimumIndexInConstraint); - constraints.MinimumBatch = batchIndex; - constraints.MinimumConstraint = constraintHandle; - constraints.MinimumIndexInConstraint = indexInConstraint; - solver.AddEarlyIntegrationResponsibilityToConstraint(constraintHandle, indexInConstraint); - - } - else if (batchIndex > maximum) - { - solver.RemoveLateIntegrationResponsibilityFromConstraint(constraints.MaximumConstraint, constraints.MaximumIndexInConstraint); - constraints.MaximumBatch = batchIndex; - constraints.MaximumConstraint = constraintHandle; - constraints.MaximumIndexInConstraint = indexInConstraint; - solver.AddLateIntegrationResponsibilityToConstraint(constraintHandle, indexInConstraint); - } - } - lock (debugBodiesConstraintAdded) - { - debugBodiesConstraintAdded.Add(bodyIndex); - } + ActiveSet.AddConstraint(bodyIndex, constraintHandle, indexInConstraint, Pool); } /// @@ -341,96 +266,9 @@ internal void AddConstraint(Solver solver, int bodyIndex, ConstraintHandle const /// /// Index of the active body. /// Handle of the constraint to remove. - /// True if the constraint had integration responsibilities for the body, false otherwise. - internal bool RemoveConstraintReference(int bodyIndex, ConstraintHandle constraintHandle) - { - ref var constraints = ref ActiveSet.RemoveConstraintReference(bodyIndex, constraintHandle, MinimumConstraintCapacityPerBody, Pool); - - //If this constraint is being removed, we have to get rid of references to it in the integration responsibilities. - //This function assumes a multithreaded context that means we can't directly handle everything here, so we return a bool indicating whether this body requires integration responsibility analysis. - bool requiresUpdateIfConstraintsRemain = constraintHandle == constraints.MinimumConstraint || constraintHandle == constraints.MaximumConstraint; - if (requiresUpdateIfConstraintsRemain) - { - //If either constraint is a bound, we just clear both bounds. - //Updating both bounds costs the same amount as updating one, and by clearing out both bounds, we stop later removals from possibly triggering another update for the second bound. - constraints.MinimumConstraint.Value = -1; - constraints.MaximumConstraint.Value = -1; - } - if (constraints.References.Count == 0) - { - //If there are no constraints left associated with the body, we can immediately add the body to the unconstrained set. - //This is only called from a locally sequential context, so it's safe to do inline, unlike the integration bounds update which needs to look at solver constraint locations. - constraints.UnconstrainedIndex = UnconstrainedBodies.Add(bodyIndex, Pool); - return false; - } - return requiresUpdateIfConstraintsRemain; - } - - - internal ref BodyConstraints UpdateIntegrationResponsibilitiesForBodyWithoutSolverModifications(Solver solver, int bodyIndex) + internal void RemoveConstraintReference(int bodyIndex, ConstraintHandle constraintHandle) { - ref var constraints = ref ActiveSet.Constraints[bodyIndex]; - ref var list = ref constraints.References; - if (list.Count > 0) //It's possible that an update request came before the body had all its constraints removed, meaning there is no more need for an update. - { - //Note that there's no need to remove the integration responsibility of a constraint that's being removed. - constraints.MinimumBatch = int.MaxValue; - constraints.MaximumBatch = -1; - //Find the new minimum batch index. - for (int i = 0; i < list.Count; ++i) - { - ref var reference = ref list[i]; - var batchIndex = solver.HandleToConstraint[reference.ConnectingConstraintHandle.Value].BatchIndex; - if (batchIndex < constraints.MinimumBatch) - { - constraints.MinimumBatch = batchIndex; - constraints.MinimumConstraint = reference.ConnectingConstraintHandle; - constraints.MinimumIndexInConstraint = reference.BodyIndexInConstraint; - } - if (batchIndex > constraints.MaximumBatch) - { - constraints.MaximumBatch = batchIndex; - constraints.MaximumConstraint = reference.ConnectingConstraintHandle; - constraints.MaximumIndexInConstraint = reference.BodyIndexInConstraint; - } - } - } - lock (debugConstraintRemovedDirectlyOrIndirectly) - { - debugConstraintRemovedDirectlyOrIndirectly.Add(bodyIndex); - } - return ref constraints; - } - - internal void UpdateIntegrationResponsibilitiesForBodyWithSolverModifications(Solver solver, int bodyIndex) - { - ref var constraints = ref UpdateIntegrationResponsibilitiesForBodyWithoutSolverModifications(solver, bodyIndex); - if (constraints.References.Count > 0) //It's possible that an update request came before the body had all its constraints removed, meaning there is no more need for an update. - { - solver.AddEarlyIntegrationResponsibilityToConstraint(constraints.MinimumConstraint, constraints.MinimumIndexInConstraint); - solver.AddLateIntegrationResponsibilityToConstraint(constraints.MaximumConstraint, constraints.MaximumIndexInConstraint); - } - lock (debugConstraintRemovedDirectlyOrIndirectly) - { - debugConstraintRemovedDirectlyOrIndirectly.Add(bodyIndex); - } - } - - /// - /// Removes a constraint from an active body's constraint list. Immdiately modifies constraint integration responsibilities. - /// - /// Index of the active body. - /// Handle of the constraint to remove. - internal void RemoveConstraintReference(Solver solver, int bodyIndex, ConstraintHandle constraintHandle) - { - if (RemoveConstraintReference(bodyIndex, constraintHandle)) - { - UpdateIntegrationResponsibilitiesForBodyWithSolverModifications(solver, bodyIndex); - } - lock (debugBodiesConstraintRemovedDirectly) - { - debugBodiesConstraintRemovedDirectly.Add(bodyIndex); - } + ActiveSet.RemoveConstraintReference(bodyIndex, constraintHandle, MinimumConstraintCapacityPerBody, Pool); } /// @@ -493,7 +331,7 @@ void UpdateForKinematicStateChange(BodyHandle handle, ref BodyMemoryLocation loc } if (newlyKinematic) { - ref var constraints = ref set.Constraints[location.Index].References; + ref var constraints = ref set.Constraints[location.Index]; ConnectedDynamicCounter enumerator; enumerator.Bodies = this; for (int i = 0; i < constraints.Count; ++i) @@ -686,92 +524,6 @@ internal void ValidateMotionStates() } } - struct IntegrationValidationEnumerator : IForEach - { - public int BodyReference; - public int ExpectedIndexInConstraint; - public int EnumeratedCount; - public int FoundIndex; - - public void LoopBody(int i) - { - if (BodyReference == i) - { - FoundIndex = EnumeratedCount; - } - ++EnumeratedCount; - } - } - public System.Collections.Generic.HashSet debugConstraintRemovedDirectlyOrIndirectly = new(); - public System.Collections.Generic.HashSet debugBodiesConstraintAdded = new(); - public System.Collections.Generic.HashSet debugBodiesConstraintRemovedDirectly = new(); - public System.Collections.Generic.HashSet debugBodiesAdded = new(); - public System.Collections.Generic.HashSet debugBodiesRemoved = new(); - private void ValidateBodyConstraintIntegrationBounds(int bodyReference, in BodyConstraints constraints, int batchCount) - { - Debug.Assert(solver.HandleToConstraint[constraints.MinimumConstraint.Value].BatchIndex == constraints.MinimumBatch); - Debug.Assert(solver.HandleToConstraint[constraints.MaximumConstraint.Value].BatchIndex == constraints.MaximumBatch); - int expectedMinimum = int.MaxValue; - int expectedMaximum = -1; - for (int i = 0; i < constraints.References.Count; ++i) - { - var batchIndex = solver.HandleToConstraint[constraints.References[i].ConnectingConstraintHandle.Value].BatchIndex; - if (batchIndex < expectedMinimum) - expectedMinimum = batchIndex; - if (batchIndex > expectedMaximum) - expectedMaximum = batchIndex; - } - var constraintHadBoundsUpdatedForRemovalDeferredOrImmediate = debugConstraintRemovedDirectlyOrIndirectly.Contains(bodyReference); - var bodyHadConstraintAdded = debugBodiesConstraintAdded.Contains(bodyReference); - var bodyHadConstraintRemoved = debugBodiesConstraintRemovedDirectly.Contains(bodyReference); - var bodyWasAdded = debugBodiesAdded.Contains(bodyReference); - var bodyWasRemoved = debugBodiesRemoved.Contains(bodyReference); - Debug.Assert(constraints.MinimumBatch == expectedMinimum && constraints.MaximumBatch == expectedMaximum, "Minimum and maximum batch index bounds should match the brute force determined results."); - Debug.Assert(constraints.MinimumBatch >= 0 && constraints.MinimumBatch < batchCount && constraints.MinimumBatch <= constraints.MaximumBatch, - "Constrained bodies must have a minimum batch index that points back to a valid existing constraint batch."); - Debug.Assert(constraints.MaximumBatch >= 0 && constraints.MaximumBatch < batchCount, - "Constrained bodies must have a maximum batch index that points back to a valid existing constraint batch."); - IntegrationValidationEnumerator enumerator; - enumerator.EnumeratedCount = 0; - enumerator.ExpectedIndexInConstraint = constraints.MinimumIndexInConstraint; - enumerator.BodyReference = bodyReference; - enumerator.FoundIndex = -1; - solver.EnumerateConnectedBodies(constraints.MinimumConstraint, ref enumerator); - Debug.Assert(enumerator.FoundIndex == constraints.MinimumIndexInConstraint, "Body should be in the constraint that it listed as a integration-responsible bound."); - - enumerator.EnumeratedCount = 0; - enumerator.ExpectedIndexInConstraint = constraints.MaximumIndexInConstraint; - enumerator.FoundIndex = -1; - solver.EnumerateConnectedBodies(constraints.MaximumConstraint, ref enumerator); - Debug.Assert(enumerator.FoundIndex == constraints.MaximumIndexInConstraint, "Body should be in the constraint that it listed as a integration-responsible bound."); - } - - [Conditional("DEBUG")] - - public void ValidateIntegrationResponsibilities() - { - Debug.Assert(UnconstrainedBodies.Count <= UnconstrainedBodies.BodyIndices.Length, "Unconstrained bodies count/length corrupted. Bad asynchronous access?"); - for (int bodyIndex = 0; bodyIndex < ActiveSet.Count; ++bodyIndex) - { - ref var constraints = ref ActiveSet.Constraints[bodyIndex]; - if (constraints.References.Count == 0) - { - Debug.Assert(constraints.UnconstrainedIndex >= 0 && constraints.UnconstrainedIndex < UnconstrainedBodies.Count, "Unconstrained bodies must point to a valid position in the unconstrained set."); - Debug.Assert(UnconstrainedBodies.BodyIndices[constraints.UnconstrainedIndex] == bodyIndex, "Bidirectional mapping between bodies and unconstrained set entries must match."); - } - else - { - ValidateBodyConstraintIntegrationBounds(bodyIndex, constraints, solver.ActiveSet.Batches.Count); - } - } - debugConstraintRemovedDirectlyOrIndirectly.Clear(); - debugBodiesConstraintAdded.Clear(); - debugBodiesConstraintRemovedDirectly.Clear(); - debugBodiesAdded.Clear(); - debugBodiesRemoved.Clear(); - } - - internal void ResizeSetsCapacity(int setsCapacity, int potentiallyAllocatedCount) { Debug.Assert(setsCapacity >= potentiallyAllocatedCount && potentiallyAllocatedCount <= Sets.Length); @@ -850,7 +602,7 @@ public void LoopBody(int connectedBodyHandle) [MethodImpl(MethodImplOptions.AggressiveInlining)] internal void EnumerateConnectedBodyIndices(int activeBodyIndex, ref TEnumerator enumerator) where TEnumerator : IForEach { - ref var list = ref ActiveSet.Constraints[activeBodyIndex].References; + ref var list = ref ActiveSet.Constraints[activeBodyIndex]; ActiveConstraintBodyIndicesEnumerator constraintBodiesEnumerator; constraintBodiesEnumerator.InnerEnumerator = enumerator; constraintBodiesEnumerator.SourceBodyIndex = activeBodyIndex; @@ -879,7 +631,7 @@ public void EnumerateConnectedBodies(BodyHandle bodyHandle, ref TEn { ref var bodyLocation = ref HandleToLocation[bodyHandle.Value]; ref var set = ref Sets[bodyLocation.SetIndex]; - ref var list = ref set.Constraints[bodyLocation.Index].References; + ref var list = ref set.Constraints[bodyLocation.Index]; //In the loops below, we still make use of the reversed iteration. Removing from within the context of an enumerator is a dangerous move, but it is permitted if the user //is careful. By maintaining the same convention across all of these enumerations, it makes it a little easier to do reliably. if (bodyLocation.SetIndex == 0) @@ -925,7 +677,6 @@ public unsafe void Clear() if (set.Allocated) set.Dispose(Pool); } - UnconstrainedBodies.Clear(); Unsafe.InitBlockUnaligned(HandleToLocation.Memory, 0xFF, (uint)(sizeof(BodyMemoryLocation) * HandleToLocation.Length)); HandlePool.Clear(); } @@ -985,7 +736,6 @@ public void Resize(int capacity) ActiveSet.InternalResize(targetBodyCapacity, Pool); } ResizeInertias(capacity); - UnconstrainedBodies.Resize(capacity, Pool); var targetHandleCapacity = BufferPool.GetCapacityForCount(Math.Max(capacity, HandlePool.HighestPossiblyClaimedId + 1)); if (HandleToLocation.Length != targetHandleCapacity) { @@ -1001,7 +751,7 @@ public void ResizeConstraintListCapacities() { for (int i = 0; i < ActiveSet.Count; ++i) { - ref var list = ref ActiveSet.Constraints[i].References; + ref var list = ref ActiveSet.Constraints[i]; var targetCapacity = BufferPool.GetCapacityForCount(list.Count > MinimumConstraintCapacityPerBody ? list.Count : MinimumConstraintCapacityPerBody); if (list.Span.Length != targetCapacity) list.Resize(targetCapacity, Pool); @@ -1011,14 +761,13 @@ public void ResizeConstraintListCapacities() /// /// Increases the size of active body buffers if needed to hold the target capacity. /// - /// Target body capacity. + /// Target data capacity. public void EnsureCapacity(int capacity) { if (ActiveSet.IndexToHandle.Length < capacity) { ActiveSet.InternalResize(capacity, Pool); } - UnconstrainedBodies.EnsureCapacity(capacity, Pool); EnsureInertiasCapacity(capacity); if (HandleToLocation.Length < capacity) { @@ -1033,7 +782,7 @@ public void EnsureConstraintListCapacities() { for (int i = 0; i < ActiveSet.Count; ++i) { - ref var list = ref ActiveSet.Constraints[i].References; + ref var list = ref ActiveSet.Constraints[i]; if (list.Span.Length < MinimumConstraintCapacityPerBody) list.Resize(MinimumConstraintCapacityPerBody, Pool); } @@ -1058,7 +807,6 @@ public void Dispose() Pool.Return(ref Inertias); Pool.Return(ref HandleToLocation); HandlePool.Dispose(Pool); - UnconstrainedBodies.Dispose(Pool); } } diff --git a/BepuPhysics/BodyProperties.cs b/BepuPhysics/BodyProperties.cs index 747d728c3..bdf721f57 100644 --- a/BepuPhysics/BodyProperties.cs +++ b/BepuPhysics/BodyProperties.cs @@ -1,10 +1,8 @@ using BepuPhysics.Collidables; using BepuPhysics.Constraints; using BepuUtilities; -using BepuUtilities.Collections; using BepuUtilities.Memory; using System; -using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; using System.Runtime.InteropServices; @@ -300,49 +298,4 @@ public struct BodyActivity /// public bool SleepCandidate; } - - /// - /// Stores references to constraints connected to a body and additional data for choosing what system is responsible for integrating a body's velocities and pose. - /// - public struct BodyConstraints - { - public QuickList References; - - /// - /// Index of the body in the unconstrained integration set if it has no constraints. If the body has constraints or is asleep, this value is undefined. - /// - public int UnconstrainedIndex; - - /// - /// Inclusive minimum constraint batch index that this body is associated with if the body has constraints. If it has no constraints or is asleep, this value is undefined. - /// - public int MinimumBatch; - /// - /// Inclusive maximum constraint batch index that this body is associated with if the body has constraints. If it has no constraints or is asleep, this value is undefined. - /// - public int MaximumBatch; - - /// - /// Constraint connected to the body associated with the lowest batch index. If the body has no constraints, this value is undefined. - /// - public ConstraintHandle MinimumConstraint; - /// - /// Constraint connected to the body associated with the highest batch index. If the body has no constraints, this value is undefined. - /// - public ConstraintHandle MaximumConstraint; - - /// - /// Index of the body within the minimum batch index constraint. If the body has no constraints, this value is undefined. - /// - public int MinimumIndexInConstraint; - /// - /// Index of the body within the maximum batch index constraint. If the body has no constraints, this value is undefined. - /// - public int MaximumIndexInConstraint; - - /// - /// Gets whether this body has any constraints based on the value in the UnconstrainedIndex field. - /// - public bool Constrained => References.Count > 0; - } } diff --git a/BepuPhysics/BodyReference.cs b/BepuPhysics/BodyReference.cs index 651c19b85..98b0fd203 100644 --- a/BepuPhysics/BodyReference.cs +++ b/BepuPhysics/BodyReference.cs @@ -163,7 +163,7 @@ public ref QuickList Constraints get { ref var location = ref MemoryLocation; - return ref Bodies.Sets[location.SetIndex].Constraints[location.Index].References; + return ref Bodies.Sets[location.SetIndex].Constraints[location.Index]; } } diff --git a/BepuPhysics/BodySet.cs b/BepuPhysics/BodySet.cs index 4cffa94c0..c1266fd35 100644 --- a/BepuPhysics/BodySet.cs +++ b/BepuPhysics/BodySet.cs @@ -48,9 +48,9 @@ public struct BodySet /// public Buffer Activity; /// - /// List of constraints and constraint related data associated with each body in the set. + /// List of constraints associated with each body in the set. /// - public Buffer Constraints; + public Buffer> Constraints; public int Count; /// @@ -73,8 +73,7 @@ internal int Add(in BodyDescription bodyDescription, BodyHandle handle, int mini ++Count; IndexToHandle[index] = handle; //Collidable's broad phase index is left unset. The Bodies collection is responsible for attaching that data. - Constraints[index].References = new QuickList(minimumConstraintCapacity, pool); - + Constraints[index] = new QuickList(minimumConstraintCapacity, pool); ApplyDescriptionByIndex(index, bodyDescription); return index; } @@ -85,7 +84,6 @@ internal bool RemoveAt(int bodyIndex, out BodyHandle handle, out int movedBodyIn //Move the last body into the removed slot. --Count; bool bodyMoved = bodyIndex < Count; - ref var constraintsForRemovedSlot = ref Constraints[bodyIndex]; if (bodyMoved) { movedBodyIndex = Count; @@ -161,27 +159,24 @@ public void GetDescription(int index, out BodyDescription description) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - internal ref BodyConstraints AddConstraint(int bodyIndex, ConstraintHandle constraintHandle, int bodyIndexInConstraint, BufferPool pool) + internal void AddConstraint(int bodyIndex, ConstraintHandle constraintHandle, int bodyIndexInConstraint, BufferPool pool) { BodyConstraintReference constraint; constraint.ConnectingConstraintHandle = constraintHandle; constraint.BodyIndexInConstraint = bodyIndexInConstraint; ref var constraints = ref Constraints[bodyIndex]; - Debug.Assert(constraints.References.Span.Allocated, "Any time a body is created, a list should be built to support it."); - if (constraints.References.Span.Length == constraints.References.Count) - constraints.References.Resize(constraints.References.Span.Length * 2, pool); - constraints.References.AllocateUnsafely() = constraint; - return ref constraints; + Debug.Assert(constraints.Span.Allocated, "Any time a body is created, a list should be built to support it."); + if (constraints.Span.Length == constraints.Count) + constraints.Resize(constraints.Span.Length * 2, pool); + constraints.AllocateUnsafely() = constraint; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - internal ref BodyConstraints RemoveConstraintReference(int bodyIndex, ConstraintHandle constraintHandle, int minimumConstraintCapacityPerBody, BufferPool pool) + internal void RemoveConstraintReference(int bodyIndex, ConstraintHandle constraintHandle, int minimumConstraintCapacityPerBody, BufferPool pool) { //This uses a linear search. That's fine; bodies will rarely have more than a handful of constraints associated with them. //Attempting to use something like a hash set for fast removes would just introduce more constant overhead and slow it down on average. - ref var constraints = ref Constraints[bodyIndex]; - ref var list = ref constraints.References; - //This constraint did not have any integration responsibilities; we can remove it with no fanfare. + ref var list = ref Constraints[bodyIndex]; for (int i = 0; i < list.Count; ++i) { ref var element = ref list[i]; @@ -202,12 +197,11 @@ internal ref BodyConstraints RemoveConstraintReference(int bodyIndex, Constraint //The list can be trimmed down a bit while still holding all existing constraints and obeying the minimum capacity. list.Resize(targetCapacity, pool); } - return ref constraints; } public bool BodyIsConstrainedBy(int bodyIndex, ConstraintHandle constraintHandle) { - ref var list = ref Constraints[bodyIndex].References; + ref var list = ref Constraints[bodyIndex]; for (int i = 0; i < list.Count; ++i) { if (list[i].ConnectingConstraintHandle.Value == constraintHandle.Value) @@ -256,7 +250,7 @@ public unsafe void Clear(BufferPool pool) { for (int i = 0; i < Count; ++i) { - Constraints[i].References.Dispose(pool); + Constraints[i].Dispose(pool); } Count = 0; } @@ -283,7 +277,7 @@ public void Dispose(BufferPool pool) { for (int i = 0; i < Count; ++i) { - Constraints[i].References.Dispose(pool); + Constraints[i].Dispose(pool); } DisposeBuffers(pool); this = new BodySet(); diff --git a/BepuPhysics/CollisionDetection/ConstraintRemover.cs b/BepuPhysics/CollisionDetection/ConstraintRemover.cs index c0bb19155..51b994ccf 100644 --- a/BepuPhysics/CollisionDetection/ConstraintRemover.cs +++ b/BepuPhysics/CollisionDetection/ConstraintRemover.cs @@ -247,7 +247,7 @@ public int Compare(ref TypeBatchIndex a, ref TypeBatchIndex b) //in sequence at that point- sequential removes would cost around 5us in that case, so any kind of multithreaded overhead can overwhelm the work being done. //Doubling the cost of the best case, resulting in handfuls of wasted microseconds, isn't concerning (and we could special case it if we really wanted to). //Cutting the cost of the worst case when thousands of constraints get removed by a factor of ~ThreadCount is worth this complexity. Frame spikes are evil! - + RemovalCache batches; /// /// Processes enqueued constraint removals and prepares removal jobs. @@ -330,34 +330,21 @@ public void ReturnConstraintHandles() } } - public void RemoveConstraintsFromBodyLists(BufferPool threadPool) + public void RemoveConstraintsFromBodyLists() { //While body list removal could technically be internally multithreaded, it would be pretty complex- you would have to do one dispatch per solver.Batches batch //to guarantee that no two threads hit the same body constraint list at the same time. //That is more complicated and would almost certainly be slower than this locally sequential version. - //Further, introducing the integration responsibility tracking here makes sequential processing ~required. - int updateCapacity = 0; - for (int i = 0; i < batches.BatchCount; ++i) - { - ref var removals = ref batches.RemovalsForTypeBatches[i].PerBodyRemovalTargets; - updateCapacity += removals.Count; - } - bodiesNeedingIntegrationResponsibilityUpdate.threadPool = threadPool; - bodiesNeedingIntegrationResponsibilityUpdate.bodies = new QuickList(updateCapacity, threadPool); for (int i = 0; i < batches.BatchCount; ++i) { ref var removals = ref batches.RemovalsForTypeBatches[i].PerBodyRemovalTargets; for (int j = 0; j < removals.Count; ++j) { ref var target = ref removals[j]; - if (bodies.RemoveConstraintReference(target.BodyIndex, target.ConstraintHandle)) - { - bodiesNeedingIntegrationResponsibilityUpdate.bodies.AllocateUnsafely() = target.BodyIndex; - } + bodies.RemoveConstraintReference(target.BodyIndex, target.ConstraintHandle); } } } - (QuickList bodies, BufferPool threadPool) bodiesNeedingIntegrationResponsibilityUpdate; public void RemoveConstraintsFromBatchReferencedHandles() { @@ -453,14 +440,6 @@ public void MarkAffectedConstraintsAsRemovedFromSolver() solver.HandleToConstraint[batchHandles[j].Value].SetIndex = -1; } } - Debug.Assert(bodiesNeedingIntegrationResponsibilityUpdate.threadPool != null, "This should only be called from usages of the constraint remover that removed constraints from body lists."); - for (int i = 0; i < bodiesNeedingIntegrationResponsibilityUpdate.bodies.Count; ++i) - { - bodies.UpdateIntegrationResponsibilitiesForBodyWithoutSolverModifications(solver, bodiesNeedingIntegrationResponsibilityUpdate.bodies[i]); - } - //Console.WriteLine($"Number of updates: {bodiesNeedingIntegrationResponsibilityUpdate.bodies.Count}"); - bodiesNeedingIntegrationResponsibilityUpdate.bodies.Dispose(bodiesNeedingIntegrationResponsibilityUpdate.threadPool); - bodiesNeedingIntegrationResponsibilityUpdate = default; } public void Postflush() diff --git a/BepuPhysics/CollisionDetection/FreshnessChecker.cs b/BepuPhysics/CollisionDetection/FreshnessChecker.cs index 678fce0a5..903d22175 100644 --- a/BepuPhysics/CollisionDetection/FreshnessChecker.cs +++ b/BepuPhysics/CollisionDetection/FreshnessChecker.cs @@ -149,7 +149,7 @@ unsafe void PrintRemovalInformation(ConstraintHandle constraintHandle) for (int i = 0; i < typeProcessor.BodiesPerConstraint; ++i) { var bodyHandle = constraintRemover.bodies.ActiveSet.IndexToHandle[references[i]]; - Console.Write($"{bodyHandle} ({constraintRemover.bodies.ActiveSet.Constraints[references[i]].References.Count}), "); + Console.Write($"{bodyHandle} ({constraintRemover.bodies.ActiveSet.Constraints[references[i]].Count}), "); } Console.WriteLine(); diff --git a/BepuPhysics/CollisionDetection/NarrowPhase.cs b/BepuPhysics/CollisionDetection/NarrowPhase.cs index f4c72201f..9c5da2a9d 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhase.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhase.cs @@ -213,7 +213,7 @@ void ExecuteFlushJob(ref NarrowPhaseFlushJob job, BufferPool threadPool) switch (job.Type) { case NarrowPhaseFlushJobType.RemoveConstraintsFromBodyLists: - ConstraintRemover.RemoveConstraintsFromBodyLists(threadPool); + ConstraintRemover.RemoveConstraintsFromBodyLists(); break; case NarrowPhaseFlushJobType.ReturnConstraintHandles: ConstraintRemover.ReturnConstraintHandles(); diff --git a/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs b/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs index c6d637b29..2bee68413 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs @@ -130,15 +130,12 @@ static unsafe void AddToSimulationSpeculative public void LoopBody(int bodyIndex) { //Note that this only looks in the active set. Directly removing inactive objects is unsupported- removals and adds activate all involved islands. - bodies.RemoveConstraintReference(bodies.solver, bodyIndex, constraintHandle); + bodies.RemoveConstraintReference(bodyIndex, constraintHandle); } } diff --git a/BepuPhysics/Constraints/TypeBatch.cs b/BepuPhysics/Constraints/TypeBatch.cs index f21cd1e66..9b1944fc1 100644 --- a/BepuPhysics/Constraints/TypeBatch.cs +++ b/BepuPhysics/Constraints/TypeBatch.cs @@ -8,18 +8,6 @@ namespace BepuPhysics.Constraints { - public struct BodyIntegrationFlags - { - /// - /// Set of flags aligned with the constraints in the type batch which, if set, indicate that the constraint is the earliest one associated with the body. - /// - public IndexSet Early; - /// - /// Set of flags aligned with the constraints in the type batch which, if set, indicate that the constraint is the latest one associated with the body. - /// - public IndexSet Late; - - } /// /// Stores the raw AOSOA formatted data associated with constraints in a type batch. /// @@ -33,7 +21,6 @@ public struct TypeBatch //and even though we may end up not even persisting the allocation between frames. We may later pull this out and store it strictly ephemerally in the solver. public RawBuffer Projection; public Buffer IndexToHandle; - public Buffer IntegrationFlags; public int ConstraintCount; public int TypeId; @@ -53,13 +40,6 @@ public void Dispose(BufferPool pool) pool.Return(ref PrestepData); pool.Return(ref AccumulatedImpulses); pool.Return(ref IndexToHandle); - for (int i = 0; i < IntegrationFlags.Length; ++i) - { - ref var flags = ref IntegrationFlags[i]; - flags.Early.Dispose(pool); - flags.Late.Dispose(pool); - } - pool.Return(ref IntegrationFlags); } } } diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index a4b75cfac..2e815db32 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -341,9 +341,8 @@ public unsafe override void TransferConstraint(ref TypeBatch typeBatch, int sour //It's not exactly trivial to keep everything straight, especially over time- it becomes a maintenance nightmare. //So instead, given that compressions should generally be extremely rare (relatively speaking) and highly deferrable, we'll accept some minor overhead. int bodiesPerConstraint = InternalBodiesPerConstraint; - var bodyIndices = stackalloc int[bodiesPerConstraint]; var bodyHandles = stackalloc int[bodiesPerConstraint]; - var bodyHandleCollector = new ActiveConstraintBodyHandleAndIndexCollector(bodies, bodyHandles, bodyIndices); + var bodyHandleCollector = new ActiveConstraintBodyHandleCollector(bodies, bodyHandles); EnumerateConnectedBodyIndices(ref typeBatch, indexInTypeBatch, ref bodyHandleCollector); Debug.Assert(targetBatchIndex <= solver.FallbackBatchThreshold, "Constraint transfers should never target the fallback batch. It doesn't have any body handles so attempting to allocate in the same way wouldn't turn out well."); @@ -380,31 +379,6 @@ ref Buffer.Get(ref targetReference.TypeBatch.AccumulatedImp constraintLocation.IndexInTypeBatch = targetReference.IndexInTypeBatch; constraintLocation.TypeId = typeId; - //Now that the constraint's been moved, update the integration responsibilities for any involved constraints. - for (int i = 0; i < bodiesPerConstraint; ++i) - { - var bodyIndex = bodyIndices[i]; - ref var constraints = ref bodies.ActiveSet.Constraints[bodyIndex]; - if (constraints.MinimumBatch > targetBatchIndex || constraints.MaximumConstraint == constraintHandle) - { - var previousMin = constraints.MinimumConstraint; - var previousMinIndexInConstraint = constraints.MinimumIndexInConstraint; - var previousMax = constraints.MaximumConstraint; - var previousMaxIndexInConstraint = constraints.MaximumIndexInConstraint; - bodies.UpdateIntegrationResponsibilitiesForBodyWithoutSolverModifications(solver, bodyIndex); - solver.AddEarlyIntegrationResponsibilityToConstraint(constraints.MinimumConstraint, constraints.MinimumIndexInConstraint); - solver.AddLateIntegrationResponsibilityToConstraint(constraints.MaximumConstraint, constraints.MaximumIndexInConstraint); - if (constraints.MinimumConstraint != previousMin) - { - solver.RemoveEarlyIntegrationResponsibilityFromConstraint(previousMin, previousMinIndexInConstraint); - } - if (constraints.MaximumConstraint != previousMax) - { - solver.RemoveLateIntegrationResponsibilityFromConstraint(previousMax, previousMaxIndexInConstraint); - } - } - } - } void InternalResize(ref TypeBatch typeBatch, BufferPool pool, int constraintCapacity) @@ -420,13 +394,6 @@ void InternalResize(ref TypeBatch typeBatch, BufferPool pool, int constraintCapa pool.ResizeToAtLeast(ref typeBatch.BodyReferences, bundleCapacity * Unsafe.SizeOf(), bundleCount * Unsafe.SizeOf()); pool.ResizeToAtLeast(ref typeBatch.PrestepData, bundleCapacity * Unsafe.SizeOf(), bundleCount * Unsafe.SizeOf()); pool.ResizeToAtLeast(ref typeBatch.AccumulatedImpulses, bundleCapacity * Unsafe.SizeOf(), bundleCount * Unsafe.SizeOf()); - pool.ResizeToAtLeast(ref typeBatch.AccumulatedImpulses, bundleCapacity * Unsafe.SizeOf(), bundleCount * Unsafe.SizeOf()); - for (int i = 0; i < typeBatch.IntegrationFlags.Length; ++i) - { - ref var flags = ref typeBatch.IntegrationFlags[i]; - flags.Early.Resize(constraintCapacity, pool); - flags.Late.Resize(constraintCapacity, pool); - } } public override void Initialize(ref TypeBatch typeBatch, int initialCapacity, BufferPool pool) @@ -435,8 +402,6 @@ public override void Initialize(ref TypeBatch typeBatch, int initialCapacity, Bu //Technically we could use an initialization-specific version, but it doesn't matter. typeBatch = new TypeBatch(); typeBatch.TypeId = TypeId; - pool.Take(bodiesPerConstraint, out typeBatch.IntegrationFlags); - typeBatch.IntegrationFlags.Clear(0, typeBatch.IntegrationFlags.Length); InternalResize(ref typeBatch, pool, initialCapacity); } @@ -656,6 +621,12 @@ internal unsafe sealed override void CopySleepingToActive( ref var sourceReferencesLaneStart = ref Unsafe.Add(ref Unsafe.As(ref Buffer.Get(ref sourceTypeBatch.BodyReferences, sourceBundle)), sourceInner); ref var targetReferencesLaneStart = ref Unsafe.Add(ref Unsafe.As(ref Buffer.Get(ref targetTypeBatch.BodyReferences, targetBundle)), targetInner); + var offset = 0; + for (int j = 0; j < bodiesPerConstraint; ++j) + { + Unsafe.Add(ref targetReferencesLaneStart, offset) = bodies.HandleToLocation[Unsafe.Add(ref sourceReferencesLaneStart, offset)].Index; + offset += Vector.Count; + } var constraintHandle = sourceTypeBatch.IndexToHandle[sourceIndex]; ref var location = ref solver.HandleToConstraint[constraintHandle.Value]; Debug.Assert(location.SetIndex == sourceSet); @@ -665,21 +636,6 @@ internal unsafe sealed override void CopySleepingToActive( location.IndexInTypeBatch = targetIndex; //This could be done with a bulk copy, but eh! We already touched the memory. targetTypeBatch.IndexToHandle[targetIndex] = constraintHandle; - - //While we update body references, also update constraint integration responsibilities. - var offset = 0; - for (int j = 0; j < bodiesPerConstraint; ++j) - { - var bodyIndex = bodies.HandleToLocation[Unsafe.Add(ref sourceReferencesLaneStart, offset)].Index; - //Note that the awakener called UpdateIntegrationResponsibilitiesForBodyWithoutSolverModifications already in phase 1. - //This constraint copy need only examine the results of that. - ref var bodyConstraints = ref bodies.ActiveSet.Constraints[bodyIndex]; - ref var flags = ref targetTypeBatch.IntegrationFlags[j]; - flags.Early.SetUnsafely(targetIndex, bodyConstraints.MinimumConstraint == constraintHandle); - flags.Late.SetUnsafely(targetIndex, bodyConstraints.MaximumConstraint == constraintHandle); - Unsafe.Add(ref targetReferencesLaneStart, offset) = bodyIndex; - offset += Vector.Count; - } } } diff --git a/BepuPhysics/HandyEnumerators.cs b/BepuPhysics/HandyEnumerators.cs index d501c2c89..6b05aa334 100644 --- a/BepuPhysics/HandyEnumerators.cs +++ b/BepuPhysics/HandyEnumerators.cs @@ -29,30 +29,6 @@ public void LoopBody(int bodyIndex) Handles[Index++] = Bodies.ActiveSet.IndexToHandle[bodyIndex].Value; } } - /// - /// Collects body indices and body handles associated with an active constraint as integers. - /// - public unsafe struct ActiveConstraintBodyHandleAndIndexCollector : IForEach - { - public Bodies Bodies; - public int* Indices; - public int* Handles; - public int Index; - - public ActiveConstraintBodyHandleAndIndexCollector(Bodies bodies, int* handles, int* indices) - { - Bodies = bodies; - Indices = indices; - Handles = handles; - Index = 0; - } - - public void LoopBody(int bodyIndex) - { - Indices[Index] = bodyIndex; - Handles[Index++] = Bodies.ActiveSet.IndexToHandle[bodyIndex].Value; - } - } public unsafe struct ReferenceCollector : IForEach { diff --git a/BepuPhysics/IslandAwakener.cs b/BepuPhysics/IslandAwakener.cs index 07bfb4e7e..a66177ead 100644 --- a/BepuPhysics/IslandAwakener.cs +++ b/BepuPhysics/IslandAwakener.cs @@ -272,6 +272,7 @@ internal unsafe void ExecutePhaseOneJob(int index) ref var sourceSet = ref bodies.Sets[job.SourceSet]; ref var targetSet = ref bodies.ActiveSet; sourceSet.Collidables.CopyTo(job.SourceStart, targetSet.Collidables, job.TargetStart, job.Count); + sourceSet.Constraints.CopyTo(job.SourceStart, targetSet.Constraints, job.TargetStart, job.Count); //The world inertias must be updated as well. They are stored outside the sets. //Note that we use a manual loop copy for the local inertias and motion state since we're accessing them during the world inertia calculation anyway. //This can worsen the copy codegen a little, but it means we only have to scan the memory once. @@ -285,34 +286,10 @@ internal unsafe void ExecutePhaseOneJob(int index) ref var targetLocalInertia = ref targetSet.LocalInertias[targetIndex]; ref var sourceState = ref sourceSet.MotionStates[sourceIndex]; ref var targetState = ref targetSet.MotionStates[targetIndex]; - ref var sourceConstraints = ref sourceSet.Constraints[sourceIndex]; - ref var targetConstraints = ref targetSet.Constraints[targetIndex]; targetState = sourceState; targetLocalInertia = sourceLocalInertia; - targetConstraints = sourceConstraints; PoseIntegration.RotateInverseInertia(sourceLocalInertia.InverseInertiaTensor, sourceState.Pose.Orientation, out targetWorldInertia.InverseInertiaTensor); targetWorldInertia.InverseMass = sourceLocalInertia.InverseMass; - if (sourceConstraints.References.Count > 0) - { - //Minimum/maximum batch indices and constraints were not updated when bodies went into sleep, so we update them as they wake up. - //It's necessary since sleeping potentially changes constraint batch indices. - //Note that the handle to constraint mapping is updated in the second phase as a part of the "CopyConstraintRegion" job, - //so the mapping currently points to the sleeping set as desired. - //The constraint copy is also responsible for initializing solver integration responsibility flags appropriately. - bodies.UpdateIntegrationResponsibilitiesForBodyWithoutSolverModifications(solver, targetIndex); - } - else - { - //We can't add an unconstrained body to the unconstrained bodies set from a multithreaded context without further work. - //The good news is that sleeping unconstrained bodies are actually pretty rare in practice- they'd have to be stationary and floating, which isn't a terribly common - //thing in most simulations thanks to gravity and momentum. - - //While we could defer this into the second phase, it would typically waste a lot of time enumerating over bodies that have constraints. - //We could write out a list of bodies which have constraints, but doing that is about as expensive as adding the body to the unconstrained set directly. - //So, we just have a special multithreading-safe add that internally performs an interlocked add. - //Note that the buffer capacity was ensured during the job creation phase. - targetConstraints.UnconstrainedIndex = bodies.UnconstrainedBodies.AddMultithreaded(targetIndex); - } } sourceSet.Activity.CopyTo(job.SourceStart, targetSet.Activity, job.TargetStart, job.Count); if (resetActivityStates) @@ -578,7 +555,6 @@ void AccumulatePairCacheTypeCounts(ref Buffer sourceTypeCaches, r //We now know how many new bodies, constraint batch entries, and pair cache entries are going to be added. //Ensure capacities on all systems: //bodies, - var preAwakeningBodyCount = bodies.ActiveSet.Count; bodies.EnsureCapacity(bodies.ActiveSet.Count + newBodyCount); //broad phase, (technically overestimating, not every body has a collidable, but vast majority do and shrug) broadPhase.EnsureCapacity(broadPhase.ActiveTree.LeafCount + newBodyCount, broadPhase.StaticTree.LeafCount); diff --git a/BepuPhysics/IslandSleeper.cs b/BepuPhysics/IslandSleeper.cs index d7030b6ff..a8116ed76 100644 --- a/BepuPhysics/IslandSleeper.cs +++ b/BepuPhysics/IslandSleeper.cs @@ -140,7 +140,7 @@ bool EnqueueUnvisitedNeighbors(int bodyIndex, BufferPool pool, ref TTraversalPredicate predicate) where TTraversalPredicate : IPredicate { bodyEnumerator.SourceIndex = bodyIndex; - ref var list = ref bodies.ActiveSet.Constraints[bodyIndex].References; + ref var list = ref bodies.ActiveSet.Constraints[bodyIndex]; for (int i = 0; i < list.Count; ++i) { ref var entry = ref list[i]; diff --git a/BepuPhysics/PositionFirstTimestepper.cs b/BepuPhysics/PositionFirstTimestepper.cs index 064f1282a..9fa619912 100644 --- a/BepuPhysics/PositionFirstTimestepper.cs +++ b/BepuPhysics/PositionFirstTimestepper.cs @@ -47,7 +47,7 @@ public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDi //2) By bundling bounding box calculation with pose integration, you avoid redundant pose and velocity memory accesses. //3) Generated contact positions are in sync with the integrated poses. //That's often helpful for gameplay purposes- you don't have to reinterpret contact data when creating graphical effects or positioning sound sources. - + //#1 is a difficult problem, though. There is no fully 'correct' place to change velocities. We might just have to bite the bullet and create a //inertia tensor/bounding box update separate from pose integration. If the cache gets evicted in between (virtually guaranteed unless no stages run), //this basically means an extra 100-200 microseconds per frame on a processor with ~20GBps bandwidth simulating 32768 bodies. diff --git a/BepuPhysics/Simulation.cs b/BepuPhysics/Simulation.cs index c19037c36..b2ae97f80 100644 --- a/BepuPhysics/Simulation.cs +++ b/BepuPhysics/Simulation.cs @@ -353,7 +353,6 @@ public void Timestep(float dt, IThreadDispatcher threadDispatcher = null) profiler.Start(this); Timestepper.Timestep(this, dt, threadDispatcher); - Bodies.ValidateIntegrationResponsibilities(); profiler.End(this); } diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index c9504edb7..adace0df0 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -370,7 +370,7 @@ internal unsafe void ValidateExistingHandles(bool activeOnly = false) //Now for the body->constraint direction. for (int bodyIndex = 0; bodyIndex < bodySet.Count; ++bodyIndex) { - ref var constraintList = ref bodySet.Constraints[bodyIndex].References; + ref var constraintList = ref bodySet.Constraints[bodyIndex]; for (int constraintIndex = 0; constraintIndex < constraintList.Count; ++constraintIndex) { ref var constraintLocation = ref HandleToConstraint[constraintList[constraintIndex].ConnectingConstraintHandle.Value]; @@ -640,12 +640,11 @@ public ConstraintHandle Add(Span bodyHandles, ref TDes awakener.AwakenBody(bodyHandles[i]); } Add(bodyHandles, ref description, out var constraintHandle); - ref var location = ref HandleToConstraint[constraintHandle.Value]; for (int i = 0; i < bodyHandles.Length; ++i) { var bodyHandle = bodyHandles[i]; bodies.ValidateExistingHandle(bodyHandle); - bodies.AddConstraint(this, bodies.HandleToLocation[bodyHandle.Value].Index, constraintHandle, ref location, i); + bodies.AddConstraint(bodies.HandleToLocation[bodyHandle.Value].Index, constraintHandle, i); } return constraintHandle; } @@ -910,7 +909,7 @@ private bool UpdateConstraintsForBodyMemoryMove(int originalIndex, int newIndex) //That's not impossible by any means, but consider that this function will tend to be called in a deferred way- we have control over how many cache optimizations //we perform. We do not, however, have any control over how many adds must be performed. Those must be performed immediately for correctness. //In other words, doing a little more work here can reduce the overall work required, in addition to simplifying the storage requirements. - ref var list = ref bodies.ActiveSet.Constraints[originalIndex].References; + ref var list = ref bodies.ActiveSet.Constraints[originalIndex]; bool bodyShouldBePresentInFallback = false; for (int i = 0; i < list.Count; ++i) { @@ -1078,38 +1077,6 @@ internal void GetSynchronizedBatchCount(out int synchronizedBatchCount, out bool "There cannot be more than FallbackBatchThreshold + 1 constraint batches because that +1 is the fallback batch which contains all remaining constraints."); } - internal void RemoveEarlyIntegrationResponsibilityFromConstraint(ConstraintHandle constraint, int bodyIndexInConstraint) - { - ref var location = ref HandleToConstraint[constraint.Value]; - Debug.Assert(location.SetIndex == 0); - ref var typeBatch = ref ActiveSet.Batches[location.BatchIndex].GetTypeBatch(location.TypeId); - typeBatch.IntegrationFlags[bodyIndexInConstraint].Early.Remove(location.IndexInTypeBatch); - } - internal void RemoveLateIntegrationResponsibilityFromConstraint(ConstraintHandle constraint, int bodyIndexInConstraint) - { - ref var location = ref HandleToConstraint[constraint.Value]; - Debug.Assert(location.SetIndex == 0); - ref var typeBatch = ref ActiveSet.Batches[location.BatchIndex].GetTypeBatch(location.TypeId); - typeBatch.IntegrationFlags[bodyIndexInConstraint].Late.Remove(location.IndexInTypeBatch); - } - - internal void AddEarlyIntegrationResponsibilityToConstraint(ConstraintHandle constraint, int bodyIndexInConstraint) - { - ref var location = ref HandleToConstraint[constraint.Value]; - Debug.Assert(location.SetIndex == 0); - ref var typeBatch = ref ActiveSet.Batches[location.BatchIndex].GetTypeBatch(location.TypeId); - typeBatch.IntegrationFlags[bodyIndexInConstraint].Early.AddUnsafely(location.IndexInTypeBatch); - } - - internal void AddLateIntegrationResponsibilityToConstraint(ConstraintHandle constraint, int bodyIndexInConstraint) - { - ref var location = ref HandleToConstraint[constraint.Value]; - Debug.Assert(location.SetIndex == 0); - ref var typeBatch = ref ActiveSet.Batches[location.BatchIndex].GetTypeBatch(location.TypeId); - typeBatch.IntegrationFlags[bodyIndexInConstraint].Late.AddUnsafely(location.IndexInTypeBatch); - } - - //Note that none of these affect the constraint batch estimates or type batch estimates. The assumption is that those are too small to bother with. //In the worst case you might see a couple of kilobytes. The reason why these functions exist is to deal with the potential many *megabytes* worth of constraint and body buffers. //Maybe something weird happens where this assumption is invalidated later, but I doubt it. There is a cost in API complexity to support it, so we don't. diff --git a/BepuPhysics/Solver_Solve2.cs b/BepuPhysics/Solver_Solve2.cs index b01833959..7ba9ceaa4 100644 --- a/BepuPhysics/Solver_Solve2.cs +++ b/BepuPhysics/Solver_Solve2.cs @@ -190,5 +190,6 @@ public void Solve2(float dt, IThreadDispatcher threadDispatcher = null) ExecuteMultithreaded(dt, threadDispatcher, solve2Worker); } } + } } diff --git a/BepuPhysics/UnconstrainedBodies.cs b/BepuPhysics/UnconstrainedBodies.cs deleted file mode 100644 index 29d2d7b10..000000000 --- a/BepuPhysics/UnconstrainedBodies.cs +++ /dev/null @@ -1,92 +0,0 @@ -using BepuUtilities.Memory; -using System; -using System.Diagnostics; -using System.Threading; - -namespace BepuPhysics -{ - public class UnconstrainedBodies - { - /// - /// Indices of bodies in the active set that are unconstrained. - /// - public Buffer BodyIndices; - public int Count; - public UnconstrainedBodies(int initialCapacity, BufferPool pool) - { - pool.TakeAtLeast(initialCapacity, out BodyIndices); - } - - internal int AddMultithreaded(int bodyIndex) - { - var unconstrainedIndex = Interlocked.Increment(ref Count) - 1; - Debug.Assert(unconstrainedIndex < BodyIndices.Length, "The multithreaded variant assumes that the body indices buffer has been resized to the maximum required size prior to execution."); - BodyIndices[unconstrainedIndex] = bodyIndex; - return unconstrainedIndex; - } - - internal int Add(int bodyIndex, BufferPool pool) - { - if (Count == BodyIndices.Length) - { - pool.ResizeToAtLeast(ref BodyIndices, Count * 2, BodyIndices.Length); - } - var index = Count++; - BodyIndices[index] = bodyIndex; - return index; - } - internal bool RemoveAt(int unconstrainedIndex, out int movedBodyIndex) - { - Debug.Assert(unconstrainedIndex >= 0 && unconstrainedIndex < Count, "Unconstrained index should fall within the unconstrained set."); - --Count; - if (unconstrainedIndex < Count) - { - //The removal target is not the last element, so we'll move the current last element to the removal index. - movedBodyIndex = BodyIndices[Count]; - BodyIndices[unconstrainedIndex] = movedBodyIndex; - return true; - } - else - { - movedBodyIndex = -1; - return false; - } - } - - internal void UpdateForBodyMemoryMove(int unconstrainedIndex, int bodyIndex) - { - BodyIndices[unconstrainedIndex] = bodyIndex; - } - - - public void EnsureCapacity(int capacity, BufferPool pool) - { - capacity = capacity > BodyIndices.Length ? capacity : BodyIndices.Length; - var target = BufferPool.GetCapacityForCount(capacity); - if (target != BodyIndices.Length) - { - pool.ResizeToAtLeast(ref BodyIndices, target, Count); - } - } - public void Resize(int capacity, BufferPool pool) - { - capacity = capacity > Count ? capacity : Count; - var target = BufferPool.GetCapacityForCount(capacity); - if (target != BodyIndices.Length) - { - pool.ResizeToAtLeast(ref BodyIndices, target, Count); - } - } - - public void Clear() - { - Count = 0; - } - - public void Dispose(BufferPool pool) - { - pool.Return(ref BodyIndices); - Count = 0; - } - } -} \ No newline at end of file diff --git a/Demos/Demos/FountainStressTestDemo.cs b/Demos/Demos/FountainStressTestDemo.cs index 4e923ac4b..1d12db084 100644 --- a/Demos/Demos/FountainStressTestDemo.cs +++ b/Demos/Demos/FountainStressTestDemo.cs @@ -362,7 +362,6 @@ public override void Update(Window window, Camera camera, Input input, float dt) } Simulation.Bodies.ApplyDescription(handle, newDescription); } - Simulation.Bodies.ValidateIntegrationResponsibilities(); base.Update(window, camera, input, dt); diff --git a/Demos/Program.cs b/Demos/Program.cs index b992a8c48..6c769ece5 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -19,8 +19,7 @@ static void Main(string[] args) { content = ContentArchive.Load(stream); } - //HeadlessTest.Test(content, 4, 32, 512); - DeterminismTest.Test(content, 4, 4096); + HeadlessTest.Test(content, 4, 32, 512); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); diff --git a/Demos/SpecializedTests/SimulationScrambling.cs b/Demos/SpecializedTests/SimulationScrambling.cs index 70850960a..0daee0dfd 100644 --- a/Demos/SpecializedTests/SimulationScrambling.cs +++ b/Demos/SpecializedTests/SimulationScrambling.cs @@ -51,7 +51,7 @@ public static void ScrambleBodyConstraintLists(Simulation simulation) //Note that we cannot change the order of bodies within constraints! That would change behavior. for (int bodyIndex = 0; bodyIndex < simulation.Bodies.ActiveSet.Count; ++bodyIndex) { - ref var list = ref simulation.Bodies.ActiveSet.Constraints[bodyIndex].References; + ref var list = ref simulation.Bodies.ActiveSet.Constraints[bodyIndex]; for (int i = 0; i < list.Count - 1; ++i) { ref var currentSlot = ref list[i]; @@ -207,7 +207,7 @@ private static void ChurnRemoveBody(Simulation simulation, BodyHandle[] bodyH //Remove a body. var removedBodyIndex = random.Next(simulation.Bodies.ActiveSet.Count); //All constraints associated with the body have to be removed first. - ref var constraintList = ref simulation.Bodies.ActiveSet.Constraints[removedBodyIndex].References; + ref var constraintList = ref simulation.Bodies.ActiveSet.Constraints[removedBodyIndex]; for (int i = constraintList.Count - 1; i >= 0; --i) { WriteLine($"Removing constraint (handle: {constraintList[i].ConnectingConstraintHandle}) for a body removal."); From 4fc8b0036f8feb1873e1fe28e682210653067511 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 9 Jul 2021 11:58:00 -0500 Subject: [PATCH 090/947] Constraint integration responsibilities brute force set up. --- .../EmbeddedSubsteppingTimestepper2.cs | 2 + BepuPhysics/Solver_Solve2.cs | 69 +++++++++++++++++++ 2 files changed, 71 insertions(+) diff --git a/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs b/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs index 2b4879ccd..2c8d4a19a 100644 --- a/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs +++ b/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs @@ -78,6 +78,7 @@ public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDi Debug.Assert(SubstepCount >= 0, "Substep count should be positive."); var substepDt = dt / SubstepCount; + simulation.Solver.PrepareConstraintIntegrationResponsibilities(); for (int substepIndex = 0; substepIndex < SubstepCount; ++substepIndex) { SubstepStarted?.Invoke(substepIndex, dt, threadDispatcher); @@ -102,6 +103,7 @@ public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDi SubstepEnded?.Invoke(substepIndex, substepDt, threadDispatcher); } SubstepsComplete?.Invoke(dt, threadDispatcher); + simulation.Solver.DisposeConstraintIntegrationResponsibilities(); simulation.IncrementallyOptimizeDataStructures(threadDispatcher); } diff --git a/BepuPhysics/Solver_Solve2.cs b/BepuPhysics/Solver_Solve2.cs index 7ba9ceaa4..2b0519807 100644 --- a/BepuPhysics/Solver_Solve2.cs +++ b/BepuPhysics/Solver_Solve2.cs @@ -126,7 +126,76 @@ void Solve2Worker(int workerIndex) } } + Buffer>> integrationFlags; + public void PrepareConstraintIntegrationResponsibilities() + { + //var start = Stopwatch.GetTimestamp(); + pool.Take(ActiveSet.Batches.Count, out integrationFlags); + for (int i = 0; i < integrationFlags.Length; ++i) + { + ref var batch = ref ActiveSet.Batches[i]; + ref var flagsForBatch = ref integrationFlags[i]; + pool.Take(batch.TypeBatches.Count, out flagsForBatch); + for (int j = 0; j < flagsForBatch.Length; ++j) + { + ref var flagsForTypeBatch = ref flagsForBatch[j]; + ref var typeBatch = ref batch.TypeBatches[j]; + var bodiesPerConstraint = TypeProcessors[typeBatch.TypeId].BodiesPerConstraint; + pool.Take(bodiesPerConstraint, out flagsForTypeBatch); + for (int k = 0; k < bodiesPerConstraint; ++k) + { + flagsForTypeBatch[k] = new IndexSet(pool, typeBatch.ConstraintCount); + } + } + } + for (int i = 0; i < bodies.ActiveSet.Count; ++i) + { + ref var constraints = ref bodies.ActiveSet.Constraints[i]; + ConstraintHandle minimumConstraint; + minimumConstraint.Value = -1; + int minimumBatchIndex = int.MaxValue; + int minimumIndexInConstraint = -1; + for (int j = 0; j < constraints.Count; ++j) + { + ref var constraint = ref constraints[j]; + var batchIndex = HandleToConstraint[constraint.ConnectingConstraintHandle.Value].BatchIndex; + if (batchIndex < minimumBatchIndex) + { + minimumBatchIndex = batchIndex; + minimumIndexInConstraint = constraint.BodyIndexInConstraint; + minimumConstraint = constraint.ConnectingConstraintHandle; + } + } + if (minimumConstraint.Value >= 0) + { + ref var location = ref HandleToConstraint[minimumConstraint.Value]; + var typeBatchIndex = ActiveSet.Batches[location.BatchIndex].TypeIndexToTypeBatchIndex[location.TypeId]; + ref var indexSet = ref integrationFlags[location.BatchIndex][typeBatchIndex][minimumIndexInConstraint]; + indexSet.AddUnsafely(location.IndexInTypeBatch); + } + } + //var end = Stopwatch.GetTimestamp(); + //Console.WriteLine($"Brute force time (ms): {(end - start) * 1e3 / Stopwatch.Frequency}"); + } + public void DisposeConstraintIntegrationResponsibilities() + { + for (int i = 0; i < integrationFlags.Length; ++i) + { + ref var flagsForBatch = ref integrationFlags[i]; + for (int j = 0; j < flagsForBatch.Length; ++j) + { + ref var flagsForTypeBatch = ref flagsForBatch[j]; + for (int k = 0; k < flagsForTypeBatch.Length; ++k) + { + flagsForTypeBatch[k].Dispose(pool); + } + pool.Return(ref flagsForTypeBatch); + } + pool.Return(ref flagsForBatch); + } + pool.Return(ref integrationFlags); + } public void Solve2(float dt, IThreadDispatcher threadDispatcher = null) { From 71c7a38f65cda22c54462d0e6107f0fec507812a Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 9 Jul 2021 17:20:26 -0500 Subject: [PATCH 091/947] Oops, scooted. --- .../Constraints/FourBodyTypeProcessor.cs | 4 +- .../Constraints/OneBodyTypeProcessor.cs | 4 +- .../Constraints/ThreeBodyTypeProcessor.cs | 10 +-- .../Constraints/TwoBodyTypeProcessor.cs | 4 +- BepuPhysics/Constraints/TypeProcessor.cs | 4 +- BepuPhysics/Solver_Solve2.cs | 71 ---------------- BepuPhysics/Solver_SubsteppingSolve2.cs | 82 ++++++++++++++++++- 7 files changed, 91 insertions(+), 88 deletions(-) diff --git a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs index d7fd88198..0f6d8e837 100644 --- a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs @@ -319,11 +319,11 @@ public unsafe override void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodi } - public override void WarmStart2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + public override void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { throw new NotImplementedException(); } - public override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + public override void SolveStep2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs index 21b7d50d5..0540ff051 100644 --- a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs @@ -250,7 +250,7 @@ public unsafe override void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodi - public unsafe override void WarmStart2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + public unsafe override void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); ref var bodyReferencesBase = ref Unsafe.AsRef>(typeBatch.BodyReferences.Memory); @@ -269,7 +269,7 @@ public unsafe override void WarmStart2(ref TypeBatch typeBatch, Bodies bodies, f bodies.ScatterVelocities(ref wsvA, ref bodyReferences, count); } } - public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + public unsafe override void SolveStep2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); ref var bodyReferencesBase = ref Unsafe.AsRef>(typeBatch.BodyReferences.Memory); diff --git a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs index ac8de87c5..4fc926604 100644 --- a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs @@ -102,9 +102,9 @@ public unsafe override void Prestep(ref TypeBatch typeBatch, Bodies bodies, floa ref var projection = ref Unsafe.Add(ref projectionBase, i); ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); var count = GetCountInBundle(ref typeBatch, i); - bodies.GatherState(ref references, count, - out var orientationA, out var wsvA, out var inertiaA, - out var ab, out var orientationB, out var wsvB, out var inertiaB, + bodies.GatherState(ref references, count, + out var orientationA, out var wsvA, out var inertiaA, + out var ab, out var orientationB, out var wsvB, out var inertiaB, out var ac, out var orientationC, out var wsvC, out var inertiaC); function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, ac, orientationC, inertiaC, dt, inverseDt, ref prestep, out projection); } @@ -295,11 +295,11 @@ public unsafe override void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodi } } - public override void WarmStart2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + public override void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { throw new NotImplementedException(); } - public override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + public override void SolveStep2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index a4a874f22..cfb7c56d4 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -349,7 +349,7 @@ unsafe static void Prefetch(int prefetchDistance, ref TypeBatch typeBatch, ref B } } - public unsafe override void WarmStart2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + public unsafe override void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { var prestepBundles = typeBatch.PrestepData.As(); var bodyReferencesBundles = typeBatch.BodyReferences.As(); @@ -384,7 +384,7 @@ public unsafe override void WarmStart2(ref TypeBatch typeBatch, Bodies bodies, f bodies.ScatterVelocities(ref wsvA, ref wsvB, ref references, count); } } - public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + public unsafe override void SolveStep2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { var prestepBundles = typeBatch.PrestepData.As(); var bodyReferencesBundles = typeBatch.BodyReferences.As(); diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index 2e815db32..74aff11e0 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -136,8 +136,8 @@ internal unsafe abstract void CopySleepingToActive( public abstract void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); - public abstract void WarmStart2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); - public abstract void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); + public abstract void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); + public abstract void SolveStep2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); public abstract void Prestep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); diff --git a/BepuPhysics/Solver_Solve2.cs b/BepuPhysics/Solver_Solve2.cs index 2b0519807..558f96020 100644 --- a/BepuPhysics/Solver_Solve2.cs +++ b/BepuPhysics/Solver_Solve2.cs @@ -126,77 +126,6 @@ void Solve2Worker(int workerIndex) } } - Buffer>> integrationFlags; - - public void PrepareConstraintIntegrationResponsibilities() - { - //var start = Stopwatch.GetTimestamp(); - pool.Take(ActiveSet.Batches.Count, out integrationFlags); - for (int i = 0; i < integrationFlags.Length; ++i) - { - ref var batch = ref ActiveSet.Batches[i]; - ref var flagsForBatch = ref integrationFlags[i]; - pool.Take(batch.TypeBatches.Count, out flagsForBatch); - for (int j = 0; j < flagsForBatch.Length; ++j) - { - ref var flagsForTypeBatch = ref flagsForBatch[j]; - ref var typeBatch = ref batch.TypeBatches[j]; - var bodiesPerConstraint = TypeProcessors[typeBatch.TypeId].BodiesPerConstraint; - pool.Take(bodiesPerConstraint, out flagsForTypeBatch); - for (int k = 0; k < bodiesPerConstraint; ++k) - { - flagsForTypeBatch[k] = new IndexSet(pool, typeBatch.ConstraintCount); - } - } - } - for (int i = 0; i < bodies.ActiveSet.Count; ++i) - { - ref var constraints = ref bodies.ActiveSet.Constraints[i]; - ConstraintHandle minimumConstraint; - minimumConstraint.Value = -1; - int minimumBatchIndex = int.MaxValue; - int minimumIndexInConstraint = -1; - for (int j = 0; j < constraints.Count; ++j) - { - ref var constraint = ref constraints[j]; - var batchIndex = HandleToConstraint[constraint.ConnectingConstraintHandle.Value].BatchIndex; - if (batchIndex < minimumBatchIndex) - { - minimumBatchIndex = batchIndex; - minimumIndexInConstraint = constraint.BodyIndexInConstraint; - minimumConstraint = constraint.ConnectingConstraintHandle; - } - } - if (minimumConstraint.Value >= 0) - { - ref var location = ref HandleToConstraint[minimumConstraint.Value]; - var typeBatchIndex = ActiveSet.Batches[location.BatchIndex].TypeIndexToTypeBatchIndex[location.TypeId]; - ref var indexSet = ref integrationFlags[location.BatchIndex][typeBatchIndex][minimumIndexInConstraint]; - indexSet.AddUnsafely(location.IndexInTypeBatch); - } - } - //var end = Stopwatch.GetTimestamp(); - //Console.WriteLine($"Brute force time (ms): {(end - start) * 1e3 / Stopwatch.Frequency}"); - } - public void DisposeConstraintIntegrationResponsibilities() - { - for (int i = 0; i < integrationFlags.Length; ++i) - { - ref var flagsForBatch = ref integrationFlags[i]; - for (int j = 0; j < flagsForBatch.Length; ++j) - { - ref var flagsForTypeBatch = ref flagsForBatch[j]; - for (int k = 0; k < flagsForTypeBatch.Length; ++k) - { - flagsForTypeBatch[k].Dispose(pool); - } - pool.Return(ref flagsForTypeBatch); - } - pool.Return(ref flagsForBatch); - } - pool.Return(ref integrationFlags); - } - public void Solve2(float dt, IThreadDispatcher threadDispatcher = null) { if (threadDispatcher == null) diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index 692a9381a..1b613dc9f 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -28,7 +28,7 @@ public void Execute(Solver solver, int blockIndex) ref var block = ref solver.context.ConstraintBlocks.Blocks[blockIndex]; ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; - typeProcessor.WarmStart2(ref typeBatch, solver.bodies, Dt, InverseDt, block.StartBundle, block.End); + typeProcessor.WarmStart2(ref typeBatch, ref solver.integrationFlags[block.BatchIndex][block.TypeBatchIndex], solver.bodies, Dt, InverseDt, block.StartBundle, block.End); } } //no fallback warmstart; the last constraint batch is always handled by the solve instead, and if the fallback batch exists, it's guaranteed to be the last batch. @@ -44,7 +44,7 @@ public void Execute(Solver solver, int blockIndex) ref var block = ref solver.context.ConstraintBlocks.Blocks[blockIndex]; ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; - typeProcessor.SolveStep2(ref typeBatch, solver.bodies, Dt, InverseDt, block.StartBundle, block.End); + typeProcessor.SolveStep2(ref typeBatch, ref solver.integrationFlags[block.BatchIndex][block.TypeBatchIndex], solver.bodies, Dt, InverseDt, block.StartBundle, block.End); } } @@ -128,6 +128,78 @@ void SolveStep2Worker(int workerIndex) unclaimedState ^= 1; } + Buffer>> integrationFlags; + + public void PrepareConstraintIntegrationResponsibilities() + { + //var start = Stopwatch.GetTimestamp(); + pool.Take(ActiveSet.Batches.Count, out integrationFlags); + for (int i = 0; i < integrationFlags.Length; ++i) + { + ref var batch = ref ActiveSet.Batches[i]; + ref var flagsForBatch = ref integrationFlags[i]; + pool.Take(batch.TypeBatches.Count, out flagsForBatch); + for (int j = 0; j < flagsForBatch.Length; ++j) + { + ref var flagsForTypeBatch = ref flagsForBatch[j]; + ref var typeBatch = ref batch.TypeBatches[j]; + var bodiesPerConstraint = TypeProcessors[typeBatch.TypeId].BodiesPerConstraint; + pool.Take(bodiesPerConstraint, out flagsForTypeBatch); + for (int k = 0; k < bodiesPerConstraint; ++k) + { + flagsForTypeBatch[k] = new IndexSet(pool, typeBatch.ConstraintCount); + } + } + } + for (int i = 0; i < bodies.ActiveSet.Count; ++i) + { + ref var constraints = ref bodies.ActiveSet.Constraints[i]; + ConstraintHandle minimumConstraint; + minimumConstraint.Value = -1; + int minimumBatchIndex = int.MaxValue; + int minimumIndexInConstraint = -1; + for (int j = 0; j < constraints.Count; ++j) + { + ref var constraint = ref constraints[j]; + var batchIndex = HandleToConstraint[constraint.ConnectingConstraintHandle.Value].BatchIndex; + if (batchIndex < minimumBatchIndex) + { + minimumBatchIndex = batchIndex; + minimumIndexInConstraint = constraint.BodyIndexInConstraint; + minimumConstraint = constraint.ConnectingConstraintHandle; + } + } + if (minimumConstraint.Value >= 0) + { + ref var location = ref HandleToConstraint[minimumConstraint.Value]; + var typeBatchIndex = ActiveSet.Batches[location.BatchIndex].TypeIndexToTypeBatchIndex[location.TypeId]; + ref var indexSet = ref integrationFlags[location.BatchIndex][typeBatchIndex][minimumIndexInConstraint]; + indexSet.AddUnsafely(location.IndexInTypeBatch); + } + } + //Console.WriteLine($"body count: {bodies.ActiveSet.Count}, constraint count: {CountConstraints()}"); + //var end = Stopwatch.GetTimestamp(); + //Console.WriteLine($"Brute force time (ms): {(end - start) * 1e3 / Stopwatch.Frequency}"); + } + public void DisposeConstraintIntegrationResponsibilities() + { + for (int i = 0; i < integrationFlags.Length; ++i) + { + ref var flagsForBatch = ref integrationFlags[i]; + for (int j = 0; j < flagsForBatch.Length; ++j) + { + ref var flagsForTypeBatch = ref flagsForBatch[j]; + for (int k = 0; k < flagsForTypeBatch.Length; ++k) + { + flagsForTypeBatch[k].Dispose(pool); + } + pool.Return(ref flagsForTypeBatch); + } + pool.Return(ref flagsForBatch); + } + pool.Return(ref integrationFlags); + } + public void SolveStep2(float dt, IThreadDispatcher threadDispatcher = null) { if (threadDispatcher == null) @@ -140,20 +212,22 @@ public void SolveStep2(float dt, IThreadDispatcher threadDispatcher = null) for (int i = 0; i < synchronizedBatchCount; ++i) { ref var batch = ref activeSet.Batches[i]; + ref var integrationFlagsForBatch = ref integrationFlags[i]; for (int j = 0; j < batch.TypeBatches.Count; ++j) { ref var typeBatch = ref batch.TypeBatches[j]; - TypeProcessors[typeBatch.TypeId].WarmStart2(ref typeBatch, bodies, dt, inverseDt, 0, typeBatch.BundleCount); + TypeProcessors[typeBatch.TypeId].WarmStart2(ref typeBatch, ref integrationFlagsForBatch[j], bodies, dt, inverseDt, 0, typeBatch.BundleCount); } } for (int i = 0; i < synchronizedBatchCount; ++i) { ref var batch = ref activeSet.Batches[i]; + ref var integrationFlagsForBatch = ref integrationFlags[i]; for (int j = 0; j < batch.TypeBatches.Count; ++j) { ref var typeBatch = ref batch.TypeBatches[j]; - TypeProcessors[typeBatch.TypeId].SolveStep2(ref typeBatch, bodies, dt, inverseDt, 0, typeBatch.BundleCount); + TypeProcessors[typeBatch.TypeId].SolveStep2(ref typeBatch, ref integrationFlagsForBatch[j], bodies, dt, inverseDt, 0, typeBatch.BundleCount); } } } From 8745372b15c2c277707ac6830343c4e753f95f68 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 9 Jul 2021 17:27:38 -0500 Subject: [PATCH 092/947] Type renames to be consistent with math types, and to avoid collision with new BodyInertias. --- BepuPhysics/Bodies_GatherScatter.cs | 42 ++++----- BepuPhysics/BodyProperties.cs | 4 +- BepuPhysics/BoundingBoxHelpers.cs | 2 +- BepuPhysics/Collidables/BoundingBoxBatcher.cs | 2 +- .../Constraints/AngularAxisGearMotor.cs | 6 +- BepuPhysics/Constraints/AngularAxisMotor.cs | 6 +- BepuPhysics/Constraints/AngularHinge.cs | 6 +- BepuPhysics/Constraints/AngularMotor.cs | 6 +- BepuPhysics/Constraints/AngularServo.cs | 8 +- BepuPhysics/Constraints/AngularSwivelHinge.cs | 6 +- BepuPhysics/Constraints/AreaConstraint.cs | 12 +-- BepuPhysics/Constraints/BallSocket.cs | 18 ++-- BepuPhysics/Constraints/BallSocketMotor.cs | 10 +-- BepuPhysics/Constraints/BallSocketServo.cs | 10 +-- BepuPhysics/Constraints/BallSocketShared.cs | 16 ++-- .../Constraints/CenterDistanceConstraint.cs | 8 +- .../Constraints/Contact/ContactConvexTypes.cs | 88 +++++++++---------- .../Contact/ContactNonconvexCommon.cs | 22 ++--- .../Constraints/Contact/PenetrationLimit.cs | 18 ++-- .../Contact/PenetrationLimitOneBody.cs | 18 ++-- .../Constraints/Contact/TangentFriction.cs | 16 ++-- .../Contact/TangentFrictionOneBody.cs | 16 ++-- .../Constraints/Contact/TwistFriction.cs | 16 ++-- .../Contact/TwistFrictionOneBody.cs | 16 ++-- BepuPhysics/Constraints/DistanceLimit.cs | 6 +- BepuPhysics/Constraints/DistanceServo.cs | 10 +-- .../Constraints/FourBodyTypeProcessor.cs | 12 +-- BepuPhysics/Constraints/Hinge.cs | 12 +-- BepuPhysics/Constraints/Inequality1DOF.cs | 12 +-- BepuPhysics/Constraints/LinearAxisLimit.cs | 6 +- BepuPhysics/Constraints/LinearAxisMotor.cs | 6 +- BepuPhysics/Constraints/LinearAxisServo.cs | 12 +-- .../Constraints/OneBodyAngularMotor.cs | 8 +- .../Constraints/OneBodyAngularServo.cs | 8 +- BepuPhysics/Constraints/OneBodyLinearMotor.cs | 6 +- BepuPhysics/Constraints/OneBodyLinearServo.cs | 14 +-- .../Constraints/OneBodyTypeProcessor.cs | 8 +- BepuPhysics/Constraints/PointOnLineServo.cs | 14 +-- BepuPhysics/Constraints/SwingLimit.cs | 6 +- BepuPhysics/Constraints/SwivelHinge.cs | 12 +-- .../Constraints/ThreeBodyTypeProcessor.cs | 6 +- BepuPhysics/Constraints/TwistLimit.cs | 6 +- BepuPhysics/Constraints/TwistMotor.cs | 6 +- BepuPhysics/Constraints/TwistServo.cs | 6 +- .../Constraints/TwoBodyTypeProcessor.cs | 8 +- BepuPhysics/Constraints/VolumeConstraint.cs | 14 +-- BepuPhysics/Constraints/Weld.cs | 22 ++--- BepuPhysics/FallbackBatch.cs | 4 +- .../Characters/CharacterMotionConstraint.cs | 34 +++---- Demos/Demos/NewtDemo.cs | 4 +- 50 files changed, 317 insertions(+), 317 deletions(-) diff --git a/BepuPhysics/Bodies_GatherScatter.cs b/BepuPhysics/Bodies_GatherScatter.cs index 8fb8e9a08..0316f1508 100644 --- a/BepuPhysics/Bodies_GatherScatter.cs +++ b/BepuPhysics/Bodies_GatherScatter.cs @@ -17,7 +17,7 @@ public partial class Bodies { [MethodImpl(MethodImplOptions.AggressiveInlining)] - private static void WriteGatherInertia(ref int bundleBaseBodyIndexInSet, int bodyIndexInBundle, ref Buffer states, ref BodyInertias gatheredInertias) + private static void WriteGatherInertia(ref int bundleBaseBodyIndexInSet, int bodyIndexInBundle, ref Buffer states, ref BodyInertiaWide gatheredInertias) { ref var source = ref states[Unsafe.Add(ref bundleBaseBodyIndexInSet, bodyIndexInBundle)]; ref var targetSlot = ref GetOffsetInstance(ref gatheredInertias, bodyIndexInBundle); @@ -32,7 +32,7 @@ private static void WriteGatherInertia(ref int bundleBaseBodyIndexInSet, int bod [MethodImpl(MethodImplOptions.AggressiveInlining)] private static void WriteGatherState(ref int bundleBaseBodyIndexInSet, int bodyIndexInBundle, ref Buffer states, - ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocities velocity) + ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocityWide velocity) { ref var state = ref states[Unsafe.Add(ref bundleBaseBodyIndexInSet, bodyIndexInBundle)]; Vector3Wide.WriteFirst(state.Pose.Position, ref GetOffsetInstance(ref position, bodyIndexInBundle)); @@ -42,7 +42,7 @@ private static void WriteGatherState(ref int bundleBaseBodyIndexInSet, int bodyI } [MethodImpl(MethodImplOptions.AggressiveInlining)] private static void WriteGatherState(ref int bundleBaseBodyIndexInSet, int bodyIndexInBundle, ref Buffer states, - ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocities velocity, ref BodyInertias inertia) + ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocityWide velocity, ref BodyInertiaWide inertia) { ref var state = ref states[Unsafe.Add(ref bundleBaseBodyIndexInSet, bodyIndexInBundle)]; Vector3Wide.WriteFirst(state.Pose.Position, ref GetOffsetInstance(ref position, bodyIndexInBundle)); @@ -79,7 +79,7 @@ private static void WriteGatherState(ref int bundleBaseBodyIndexInSet, int bodyI /// Gathered inertia of the body. //[MethodImpl(MethodImplOptions.AggressiveInlining)] public void GatherState(ref Vector references, int count, - out Vector3Wide position, out QuaternionWide orientation, out BodyVelocities velocity, out BodyInertias inertia) + out Vector3Wide position, out QuaternionWide orientation, out BodyVelocityWide velocity, out BodyInertiaWide inertia) { Unsafe.SkipInit(out position); Unsafe.SkipInit(out orientation); @@ -102,7 +102,7 @@ public void GatherState(ref Vector references, int count, } //[MethodImpl(MethodImplOptions.AggressiveInlining)] - unsafe static void ScalarGather(int count, MotionState* motionStates, ref Vector baseIndex, ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocities velocity, ref BodyInertias inertia) + unsafe static void ScalarGather(int count, MotionState* motionStates, ref Vector baseIndex, ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocityWide velocity, ref BodyInertiaWide inertia) { var indices = (int*)Unsafe.AsPointer(ref baseIndex); var pPositionX = (float*)Unsafe.AsPointer(ref position.X); @@ -153,7 +153,7 @@ unsafe static void ScalarGather(int count, MotionState* motionStates, ref Vector } } //[MethodImpl(MethodImplOptions.AggressiveInlining)] - unsafe static void TransposingGather(int count, MotionState* motionStates, BodyInertia* inertias, ref Vector baseIndex, ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocities velocity, ref BodyInertias inertia) + unsafe static void TransposingGather(int count, MotionState* motionStates, BodyInertia* inertias, ref Vector baseIndex, ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocityWide velocity, ref BodyInertiaWide inertia) { if (Avx.IsSupported) { @@ -331,9 +331,9 @@ unsafe static void TransposingGather(int count, MotionState* motionStates, BodyI /// Gathered inertia of body B. //[MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe void GatherState(ref TwoBodyReferences references, int count, - out QuaternionWide orientationA, out BodyVelocities velocityA, out BodyInertias inertiaA, + out QuaternionWide orientationA, out BodyVelocityWide velocityA, out BodyInertiaWide inertiaA, out Vector3Wide ab, - out QuaternionWide orientationB, out BodyVelocities velocityB, out BodyInertias inertiaB) + out QuaternionWide orientationB, out BodyVelocityWide velocityB, out BodyInertiaWide inertiaB) { Unsafe.SkipInit(out Vector3Wide positionA); Unsafe.SkipInit(out Vector3Wide positionB); @@ -393,11 +393,11 @@ public unsafe void GatherState(ref TwoBodyReferences references, int count, /// Gathered inertia of body C. //[MethodImpl(MethodImplOptions.AggressiveInlining)] public void GatherState(ref ThreeBodyReferences references, int count, - out QuaternionWide orientationA, out BodyVelocities velocityA, out BodyInertias inertiaA, + out QuaternionWide orientationA, out BodyVelocityWide velocityA, out BodyInertiaWide inertiaA, out Vector3Wide ab, - out QuaternionWide orientationB, out BodyVelocities velocityB, out BodyInertias inertiaB, + out QuaternionWide orientationB, out BodyVelocityWide velocityB, out BodyInertiaWide inertiaB, out Vector3Wide ac, - out QuaternionWide orientationC, out BodyVelocities velocityC, out BodyInertias inertiaC) + out QuaternionWide orientationC, out BodyVelocityWide velocityC, out BodyInertiaWide inertiaC) { Unsafe.SkipInit(out Vector3Wide positionA); Unsafe.SkipInit(out Vector3Wide positionB); @@ -461,13 +461,13 @@ public void GatherState(ref ThreeBodyReferences references, int count, /// Gathered inertia of body C. //[MethodImpl(MethodImplOptions.AggressiveInlining)] public void GatherState(ref FourBodyReferences references, int count, - out QuaternionWide orientationA, out BodyVelocities velocityA, out BodyInertias inertiaA, + out QuaternionWide orientationA, out BodyVelocityWide velocityA, out BodyInertiaWide inertiaA, out Vector3Wide ab, - out QuaternionWide orientationB, out BodyVelocities velocityB, out BodyInertias inertiaB, + out QuaternionWide orientationB, out BodyVelocityWide velocityB, out BodyInertiaWide inertiaB, out Vector3Wide ac, - out QuaternionWide orientationC, out BodyVelocities velocityC, out BodyInertias inertiaC, + out QuaternionWide orientationC, out BodyVelocityWide velocityC, out BodyInertiaWide inertiaC, out Vector3Wide ad, - out QuaternionWide orientationD, out BodyVelocities velocityD, out BodyInertias inertiaD) + out QuaternionWide orientationD, out BodyVelocityWide velocityD, out BodyInertiaWide inertiaD) { Unsafe.SkipInit(out Vector3Wide positionA); Unsafe.SkipInit(out Vector3Wide positionB); @@ -518,7 +518,7 @@ public void GatherState(ref FourBodyReferences references, int count, } [MethodImpl(MethodImplOptions.AggressiveInlining)] - private unsafe void ScatterVelocities(ref BodyVelocities sourceVelocities, ref int baseIndex, int innerIndex) + private unsafe void ScatterVelocities(ref BodyVelocityWide sourceVelocities, ref int baseIndex, int innerIndex) { //TODO: How much value would there be in branching on kinematic state and avoiding a write? Depends a lot on the number of kinematics. ref var sourceSlot = ref GetOffsetInstance(ref sourceVelocities, innerIndex); @@ -528,7 +528,7 @@ private unsafe void ScatterVelocities(ref BodyVelocities sourceVelocities, ref i } [MethodImpl(MethodImplOptions.AggressiveInlining)] - private unsafe void TransposeScatterVelocities(ref BodyVelocities sourceVelocities, MotionState* motionStates, ref Vector references, int count) + private unsafe void TransposeScatterVelocities(ref BodyVelocityWide sourceVelocities, MotionState* motionStates, ref Vector references, int count) { if (Avx.IsSupported) { @@ -593,7 +593,7 @@ private unsafe void TransposeScatterVelocities(ref BodyVelocities sourceVelociti /// Active set indices of the bodies to scatter velocity data to. /// Number of body pairs in the bundle. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void ScatterVelocities(ref BodyVelocities sourceVelocities, ref Vector references, int count) + public unsafe void ScatterVelocities(ref BodyVelocityWide sourceVelocities, ref Vector references, int count) { Debug.Assert(count >= 0 && count <= Vector.Count); //Grab the base references for the body indices. Note that we make use of the references memory layout again. @@ -613,7 +613,7 @@ public unsafe void ScatterVelocities(ref BodyVelocities sourceVelocities, ref Ve /// Number of body pairs in the bundle. //[MethodImpl(MethodImplOptions.AggressiveInlining)] [MethodImpl(MethodImplOptions.NoInlining)] - public unsafe void ScatterVelocities(ref BodyVelocities sourceVelocitiesA, ref BodyVelocities sourceVelocitiesB, + public unsafe void ScatterVelocities(ref BodyVelocityWide sourceVelocitiesA, ref BodyVelocityWide sourceVelocitiesB, ref TwoBodyReferences references, int count) { Debug.Assert(count >= 0 && count <= Vector.Count); @@ -640,7 +640,7 @@ public unsafe void ScatterVelocities(ref BodyVelocities sourceVelocitiesA, ref B /// Number of body pairs in the bundle. [MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe void ScatterVelocities( - ref BodyVelocities sourceVelocitiesA, ref BodyVelocities sourceVelocitiesB, ref BodyVelocities sourceVelocitiesC, + ref BodyVelocityWide sourceVelocitiesA, ref BodyVelocityWide sourceVelocitiesB, ref BodyVelocityWide sourceVelocitiesC, ref ThreeBodyReferences references, int count) { Debug.Assert(count >= 0 && count <= Vector.Count); @@ -667,7 +667,7 @@ public unsafe void ScatterVelocities( /// Number of body pairs in the bundle. [MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe void ScatterVelocities( - ref BodyVelocities sourceVelocitiesA, ref BodyVelocities sourceVelocitiesB, ref BodyVelocities sourceVelocitiesC, ref BodyVelocities sourceVelocitiesD, + ref BodyVelocityWide sourceVelocitiesA, ref BodyVelocityWide sourceVelocitiesB, ref BodyVelocityWide sourceVelocitiesC, ref BodyVelocityWide sourceVelocitiesD, ref FourBodyReferences references, int count) { Debug.Assert(count >= 0 && count <= Vector.Count); diff --git a/BepuPhysics/BodyProperties.cs b/BepuPhysics/BodyProperties.cs index bdf721f57..6fea699d5 100644 --- a/BepuPhysics/BodyProperties.cs +++ b/BepuPhysics/BodyProperties.cs @@ -252,13 +252,13 @@ public static void ReadFirst(in RigidPoses poses, out RigidPose pose) } } - public struct BodyVelocities + public struct BodyVelocityWide { public Vector3Wide Linear; public Vector3Wide Angular; } - public struct BodyInertias + public struct BodyInertiaWide { public Symmetric3x3Wide InverseInertiaTensor; //Note that the inverse mass is included in the BodyInertias bundle. InverseMass is rotationally invariant, so it doesn't need to be updated... diff --git a/BepuPhysics/BoundingBoxHelpers.cs b/BepuPhysics/BoundingBoxHelpers.cs index 9dbd285b5..aa054b491 100644 --- a/BepuPhysics/BoundingBoxHelpers.cs +++ b/BepuPhysics/BoundingBoxHelpers.cs @@ -106,7 +106,7 @@ There are ways to address this- all of which are a bit expensive- but CCD as imp } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ExpandBoundingBoxes(ref Vector3Wide min, ref Vector3Wide max, ref BodyVelocities velocities, float dt, + public static void ExpandBoundingBoxes(ref Vector3Wide min, ref Vector3Wide max, ref BodyVelocityWide velocities, float dt, ref Vector maximumRadius, ref Vector maximumAngularExpansion, ref Vector maximumExpansion) { GetBoundsExpansion(velocities.Linear, velocities.Angular, dt, maximumRadius, maximumAngularExpansion, out var minDisplacement, out var maxDisplacement); diff --git a/BepuPhysics/Collidables/BoundingBoxBatcher.cs b/BepuPhysics/Collidables/BoundingBoxBatcher.cs index a9b11ee69..a6332ed81 100644 --- a/BepuPhysics/Collidables/BoundingBoxBatcher.cs +++ b/BepuPhysics/Collidables/BoundingBoxBatcher.cs @@ -80,7 +80,7 @@ public struct BoundingBoxInstanceWide where TShape : unmanag public TShapeWide Shape; public Vector MaximumExpansion; public RigidPoses Pose; - public BodyVelocities Velocities; + public BodyVelocityWide Velocities; } public struct BoundingBoxBatcher diff --git a/BepuPhysics/Constraints/AngularAxisGearMotor.cs b/BepuPhysics/Constraints/AngularAxisGearMotor.cs index e3fcaba40..757618ff3 100644 --- a/BepuPhysics/Constraints/AngularAxisGearMotor.cs +++ b/BepuPhysics/Constraints/AngularAxisGearMotor.cs @@ -81,7 +81,7 @@ public struct AngularAxisGearMotorProjection public struct AngularAxisGearMotorFunctions : IConstraintFunctions> { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref AngularAxisGearMotorPrestepData prestep, out AngularAxisGearMotorProjection projection) { //Velocity level constraint that acts directly on the given axes. Jacobians just the axes, nothing complicated. 1DOF, so we do premultiplication. @@ -111,14 +111,14 @@ public static void ApplyImpulse(ref Vector3Wide angularVelocityA, ref Vector3Wid } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref AngularAxisGearMotorProjection projection, ref Vector accumulatedImpulse) + public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref AngularAxisGearMotorProjection projection, ref Vector accumulatedImpulse) { ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, projection, accumulatedImpulse); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref AngularAxisGearMotorProjection projection, ref Vector accumulatedImpulse) + public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref AngularAxisGearMotorProjection projection, ref Vector accumulatedImpulse) { //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); Vector3Wide.Dot(velocityA.Angular, projection.NegatedVelocityToImpulseB, out var unscaledCSIA); diff --git a/BepuPhysics/Constraints/AngularAxisMotor.cs b/BepuPhysics/Constraints/AngularAxisMotor.cs index d955319c4..45f8d9007 100644 --- a/BepuPhysics/Constraints/AngularAxisMotor.cs +++ b/BepuPhysics/Constraints/AngularAxisMotor.cs @@ -80,7 +80,7 @@ public struct AngularAxisMotorProjection public struct AngularAxisMotorFunctions : IConstraintFunctions> { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref AngularAxisMotorPrestepData prestep, out AngularAxisMotorProjection projection) { //Velocity level constraint that acts directly on the given axes. Jacobians just the axes, nothing complicated. 1DOF, so we do premultiplication. @@ -107,14 +107,14 @@ public static void ApplyImpulse(ref Vector3Wide angularVelocityA, ref Vector3Wid } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref AngularAxisMotorProjection projection, ref Vector accumulatedImpulse) + public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref AngularAxisMotorProjection projection, ref Vector accumulatedImpulse) { ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, projection, accumulatedImpulse); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref AngularAxisMotorProjection projection, ref Vector accumulatedImpulse) + public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref AngularAxisMotorProjection projection, ref Vector accumulatedImpulse) { //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); Vector3Wide.Dot(velocityA.Angular, projection.VelocityToImpulseA, out var csiA); diff --git a/BepuPhysics/Constraints/AngularHinge.cs b/BepuPhysics/Constraints/AngularHinge.cs index 384b52553..af39f15d1 100644 --- a/BepuPhysics/Constraints/AngularHinge.cs +++ b/BepuPhysics/Constraints/AngularHinge.cs @@ -123,7 +123,7 @@ public static void GetErrorAngles(in Vector3Wide hingeAxisA, in Vector3Wide hing [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref AngularHingePrestepData prestep, out AngularHingeProjection projection) { //Note that we build the tangents in local space first to avoid inconsistencies. @@ -205,13 +205,13 @@ private static void ApplyImpulse(ref Vector3Wide angularVelocityA, ref Vector3Wi } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref AngularHingeProjection projection, ref Vector2Wide accumulatedImpulse) + public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref AngularHingeProjection projection, ref Vector2Wide accumulatedImpulse) { ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, ref projection, ref accumulatedImpulse); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref AngularHingeProjection projection, ref Vector2Wide accumulatedImpulse) + public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref AngularHingeProjection projection, ref Vector2Wide accumulatedImpulse) { //JB = -JA. This is (angularVelocityA * JA + angularVelocityB * JB) * effectiveMass => (angularVelocityA - angularVelocityB) * (JA * effectiveMass) Vector3Wide.Subtract(velocityA.Angular, velocityB.Angular, out var difference); diff --git a/BepuPhysics/Constraints/AngularMotor.cs b/BepuPhysics/Constraints/AngularMotor.cs index 1b2efc91a..e4947682f 100644 --- a/BepuPhysics/Constraints/AngularMotor.cs +++ b/BepuPhysics/Constraints/AngularMotor.cs @@ -71,7 +71,7 @@ public struct AngularMotorProjection public struct AngularMotorFunctions : IConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref AngularMotorPrestepData prestep, out AngularMotorProjection projection) { projection.ImpulseToVelocityA = inertiaA.InverseInertiaTensor; @@ -89,13 +89,13 @@ public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref AngularMotorProjection projection, ref Vector3Wide accumulatedImpulse) + public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref AngularMotorProjection projection, ref Vector3Wide accumulatedImpulse) { AngularServoFunctions.ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, projection.ImpulseToVelocityA, projection.NegatedImpulseToVelocityB, accumulatedImpulse); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref AngularMotorProjection projection, ref Vector3Wide accumulatedImpulse) + public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref AngularMotorProjection projection, ref Vector3Wide accumulatedImpulse) { AngularServoFunctions.Solve(ref velocityA, ref velocityB, projection.EffectiveMass, projection.SoftnessImpulseScale, projection.BiasImpulse, projection.MaximumImpulse, projection.ImpulseToVelocityA, projection.NegatedImpulseToVelocityB, ref accumulatedImpulse); diff --git a/BepuPhysics/Constraints/AngularServo.cs b/BepuPhysics/Constraints/AngularServo.cs index fe84af7c6..b9afba394 100644 --- a/BepuPhysics/Constraints/AngularServo.cs +++ b/BepuPhysics/Constraints/AngularServo.cs @@ -79,7 +79,7 @@ public struct AngularServoProjection public struct AngularServoFunctions : IConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref AngularServoPrestepData prestep, out AngularServoProjection projection) { projection.ImpulseToVelocityA = inertiaA.InverseInertiaTensor; @@ -112,13 +112,13 @@ public static void ApplyImpulse(ref Vector3Wide angularVelocityA, ref Vector3Wid } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref AngularServoProjection projection, ref Vector3Wide accumulatedImpulse) + public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref AngularServoProjection projection, ref Vector3Wide accumulatedImpulse) { ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, projection.ImpulseToVelocityA, projection.NegatedImpulseToVelocityB, accumulatedImpulse); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, + public static void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, in Symmetric3x3Wide effectiveMass, in Vector softnessImpulseScale, in Vector3Wide biasImpulse, in Vector maximumImpulse, in Symmetric3x3Wide impulseToVelocityA, in Symmetric3x3Wide negatedImpulseToVelocityB, ref Vector3Wide accumulatedImpulse) { @@ -136,7 +136,7 @@ public static void Solve(ref BodyVelocities velocityA, ref BodyVelocities veloci } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref AngularServoProjection projection, ref Vector3Wide accumulatedImpulse) + public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref AngularServoProjection projection, ref Vector3Wide accumulatedImpulse) { Solve(ref velocityA, ref velocityB, projection.EffectiveMass, projection.SoftnessImpulseScale, projection.BiasImpulse, projection.MaximumImpulse, projection.ImpulseToVelocityA, projection.NegatedImpulseToVelocityB, ref accumulatedImpulse); diff --git a/BepuPhysics/Constraints/AngularSwivelHinge.cs b/BepuPhysics/Constraints/AngularSwivelHinge.cs index 252a997ee..ee7f80f73 100644 --- a/BepuPhysics/Constraints/AngularSwivelHinge.cs +++ b/BepuPhysics/Constraints/AngularSwivelHinge.cs @@ -81,7 +81,7 @@ public struct AngularSwivelHingeProjection public struct AngularSwivelHingeFunctions : IConstraintFunctions> { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref AngularSwivelHingePrestepData prestep, out AngularSwivelHingeProjection projection) { //The swivel hinge attempts to keep an axis on body A separated 90 degrees from an axis on body B. In other words, this is the same as a hinge joint, but with one fewer DOF. @@ -136,13 +136,13 @@ private static void ApplyImpulse(ref Vector3Wide angularVelocityA, ref Vector3Wi } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref AngularSwivelHingeProjection projection, ref Vector accumulatedImpulse) + public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref AngularSwivelHingeProjection projection, ref Vector accumulatedImpulse) { ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, ref projection, ref accumulatedImpulse); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref AngularSwivelHingeProjection projection, ref Vector accumulatedImpulse) + public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref AngularSwivelHingeProjection projection, ref Vector accumulatedImpulse) { //JB = -JA. This is (angularVelocityA * JA + angularVelocityB * JB) * effectiveMass => (angularVelocityA - angularVelocityB) * (JA * effectiveMass) Vector3Wide.Subtract(velocityA.Angular, velocityB.Angular, out var difference); diff --git a/BepuPhysics/Constraints/AreaConstraint.cs b/BepuPhysics/Constraints/AreaConstraint.cs index fe972f1e3..577379737 100644 --- a/BepuPhysics/Constraints/AreaConstraint.cs +++ b/BepuPhysics/Constraints/AreaConstraint.cs @@ -89,9 +89,9 @@ public struct AreaConstraintFunctions : IThreeBodyConstraintFunctions impulse) { Vector3Wide.Scale(negatedJacobianA, projection.InverseMassA * impulse, out var negativeVelocityChangeA); @@ -164,14 +164,14 @@ private static void ApplyImpulse(ref BodyVelocities velocityA, ref BodyVelocitie } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref BodyVelocities velocityC, ref AreaConstraintProjection projection, ref Vector accumulatedImpulse) + public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref BodyVelocityWide velocityC, ref AreaConstraintProjection projection, ref Vector accumulatedImpulse) { Vector3Wide.Add(projection.JacobianB, projection.JacobianC, out var negatedJacobianA); ApplyImpulse(ref velocityA, ref velocityB, ref velocityC, ref projection, ref negatedJacobianA, ref accumulatedImpulse); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref BodyVelocities velocityC, ref AreaConstraintProjection projection, ref Vector accumulatedImpulse) + public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref BodyVelocityWide velocityC, ref AreaConstraintProjection projection, ref Vector accumulatedImpulse) { //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); Vector3Wide.Add(projection.JacobianB, projection.JacobianC, out var negatedJacobianA); diff --git a/BepuPhysics/Constraints/BallSocket.cs b/BepuPhysics/Constraints/BallSocket.cs index 9ef6b58a8..260402fb7 100644 --- a/BepuPhysics/Constraints/BallSocket.cs +++ b/BepuPhysics/Constraints/BallSocket.cs @@ -71,14 +71,14 @@ public struct BallSocketProjection public Vector3Wide BiasVelocity; public Symmetric3x3Wide EffectiveMass; public Vector SoftnessImpulseScale; - public BodyInertias InertiaA; - public BodyInertias InertiaB; + public BodyInertiaWide InertiaA; + public BodyInertiaWide InertiaB; } public struct BallSocketFunctions : IConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref BallSocketPrestepData prestep, out BallSocketProjection projection) { projection.InertiaA = inertiaA; @@ -98,19 +98,19 @@ public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref BallSocketProjection projection, ref Vector3Wide accumulatedImpulse) + public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref BallSocketProjection projection, ref Vector3Wide accumulatedImpulse) { BallSocketShared.ApplyImpulse(ref velocityA, ref velocityB, projection.OffsetA, projection.OffsetB, projection.InertiaA, projection.InertiaB, accumulatedImpulse); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref BallSocketProjection projection, ref Vector3Wide accumulatedImpulse) + public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref BallSocketProjection projection, ref Vector3Wide accumulatedImpulse) { BallSocketShared.Solve(ref velocityA, ref velocityB, projection.OffsetA, projection.OffsetB, projection.BiasVelocity, projection.EffectiveMass, projection.SoftnessImpulseScale, ref accumulatedImpulse, projection.InertiaA, projection.InertiaB); } - public void WarmStart2(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, - in BallSocketPrestepData prestep, in Vector3Wide accumulatedImpulses, ref BodyVelocities wsvA, ref BodyVelocities wsvB) + public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, + in BallSocketPrestepData prestep, in Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { //Note that we must reconstruct the world offsets from the body orientations since we do not store world offsets. QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetA, orientationA, out var offsetA); @@ -118,8 +118,8 @@ public void WarmStart2(in QuaternionWide orientationA, in BodyInertias inertiaA, BallSocketShared.ApplyImpulse(ref wsvA, ref wsvB, offsetA, offsetB, inertiaA, inertiaB, accumulatedImpulses); } - public void Solve2(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, float dt, float inverseDt, - in BallSocketPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocities wsvA, ref BodyVelocities wsvB) + public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, + in BallSocketPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { //Note that we must reconstruct the world offsets from the body orientations since we do not store world offsets. QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetA, orientationA, out var offsetA); diff --git a/BepuPhysics/Constraints/BallSocketMotor.cs b/BepuPhysics/Constraints/BallSocketMotor.cs index 9fff482ec..eaee44460 100644 --- a/BepuPhysics/Constraints/BallSocketMotor.cs +++ b/BepuPhysics/Constraints/BallSocketMotor.cs @@ -76,14 +76,14 @@ public struct BallSocketMotorProjection public Symmetric3x3Wide EffectiveMass; public Vector SoftnessImpulseScale; public Vector MaximumImpulse; - public BodyInertias InertiaA; - public BodyInertias InertiaB; + public BodyInertiaWide InertiaA; + public BodyInertiaWide InertiaB; } public struct BallSocketMotorFunctions : IConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref BallSocketMotorPrestepData prestep, out BallSocketMotorProjection projection) { projection.InertiaA = inertiaA; @@ -100,13 +100,13 @@ public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref BallSocketMotorProjection projection, ref Vector3Wide accumulatedImpulse) + public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref BallSocketMotorProjection projection, ref Vector3Wide accumulatedImpulse) { BallSocketShared.ApplyImpulse(ref velocityA, ref velocityB, projection.OffsetA, projection.OffsetB, projection.InertiaA, projection.InertiaB, accumulatedImpulse); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref BallSocketMotorProjection projection, ref Vector3Wide accumulatedImpulse) + public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref BallSocketMotorProjection projection, ref Vector3Wide accumulatedImpulse) { BallSocketShared.Solve(ref velocityA, ref velocityB, ref projection.OffsetA, ref projection.OffsetB, ref projection.BiasVelocity, ref projection.EffectiveMass, ref projection.SoftnessImpulseScale, ref projection.MaximumImpulse, ref accumulatedImpulse, ref projection.InertiaA, ref projection.InertiaB); } diff --git a/BepuPhysics/Constraints/BallSocketServo.cs b/BepuPhysics/Constraints/BallSocketServo.cs index 8ef15c2d0..108a2eef8 100644 --- a/BepuPhysics/Constraints/BallSocketServo.cs +++ b/BepuPhysics/Constraints/BallSocketServo.cs @@ -80,14 +80,14 @@ public struct BallSocketServoProjection public Symmetric3x3Wide EffectiveMass; public Vector SoftnessImpulseScale; public Vector MaximumImpulse; - public BodyInertias InertiaA; - public BodyInertias InertiaB; + public BodyInertiaWide InertiaA; + public BodyInertiaWide InertiaB; } public struct BallSocketServoFunctions : IConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref BallSocketServoPrestepData prestep, out BallSocketServoProjection projection) { projection.InertiaA = inertiaA; @@ -107,13 +107,13 @@ public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref BallSocketServoProjection projection, ref Vector3Wide accumulatedImpulse) + public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref BallSocketServoProjection projection, ref Vector3Wide accumulatedImpulse) { BallSocketShared.ApplyImpulse(ref velocityA, ref velocityB, projection.OffsetA, projection.OffsetB, projection.InertiaA, projection.InertiaB, accumulatedImpulse); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref BallSocketServoProjection projection, ref Vector3Wide accumulatedImpulse) + public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref BallSocketServoProjection projection, ref Vector3Wide accumulatedImpulse) { BallSocketShared.Solve(ref velocityA, ref velocityB, ref projection.OffsetA, ref projection.OffsetB, ref projection.BiasVelocity, ref projection.EffectiveMass, ref projection.SoftnessImpulseScale, ref projection.MaximumImpulse, ref accumulatedImpulse, ref projection.InertiaA, ref projection.InertiaB); } diff --git a/BepuPhysics/Constraints/BallSocketShared.cs b/BepuPhysics/Constraints/BallSocketShared.cs index df00d8572..309451a44 100644 --- a/BepuPhysics/Constraints/BallSocketShared.cs +++ b/BepuPhysics/Constraints/BallSocketShared.cs @@ -16,7 +16,7 @@ public static class BallSocketShared //There are very few cases where a combo constraint will have less than 3DOFs...) //The only reason not to do that is codegen concerns. But we may want to stop holding back just because of some hopefully-not-permanent quirks in the JIT. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ComputeEffectiveMass(in BodyInertias inertiaA, in BodyInertias inertiaB, + public static void ComputeEffectiveMass(in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, ref Vector3Wide offsetA, ref Vector3Wide offsetB, ref Vector effectiveMassCFMScale, out Symmetric3x3Wide effectiveMass) { //Anchor points attached to each body are constrained to stay in the same position, yielding a position constraint of: @@ -74,8 +74,8 @@ public static void ComputeEffectiveMass(in BodyInertias inertiaA, in BodyInertia } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ApplyImpulse(ref BodyVelocities velocityA, ref BodyVelocities velocityB, - in Vector3Wide offsetA, in Vector3Wide offsetB, in BodyInertias inertiaA, in BodyInertias inertiaB, in Vector3Wide constraintSpaceImpulse) + public static void ApplyImpulse(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, + in Vector3Wide offsetA, in Vector3Wide offsetB, in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, in Vector3Wide constraintSpaceImpulse) { Vector3Wide.CrossWithoutOverlap(offsetA, constraintSpaceImpulse, out var wsi); Symmetric3x3Wide.TransformWithoutOverlap(wsi, inertiaA.InverseInertiaTensor, out var change); @@ -93,7 +93,7 @@ public static void ApplyImpulse(ref BodyVelocities velocityA, ref BodyVelocities } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ComputeCorrectiveImpulse(ref BodyVelocities velocityA, ref BodyVelocities velocityB, in Vector3Wide offsetA, in Vector3Wide offsetB, + public static void ComputeCorrectiveImpulse(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, in Vector3Wide offsetA, in Vector3Wide offsetB, in Vector3Wide biasVelocity, in Symmetric3x3Wide effectiveMass, in Vector softnessImpulseScale, in Vector3Wide accumulatedImpulse, out Vector3Wide correctiveImpulse) { //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); @@ -112,8 +112,8 @@ public static void ComputeCorrectiveImpulse(ref BodyVelocities velocityA, ref Bo } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, in Vector3Wide offsetA, in Vector3Wide offsetB, - in Vector3Wide biasVelocity, in Symmetric3x3Wide effectiveMass, in Vector softnessImpulseScale, ref Vector3Wide accumulatedImpulse, in BodyInertias inertiaA, in BodyInertias inertiaB) + public static void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, in Vector3Wide offsetA, in Vector3Wide offsetB, + in Vector3Wide biasVelocity, in Symmetric3x3Wide effectiveMass, in Vector softnessImpulseScale, ref Vector3Wide accumulatedImpulse, in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB) { ComputeCorrectiveImpulse(ref velocityA, ref velocityB, offsetA, offsetB, biasVelocity, effectiveMass, softnessImpulseScale, accumulatedImpulse, out var correctiveImpulse); //This function does not have a maximum impulse limit, so no clamping is required. @@ -123,8 +123,8 @@ public static void Solve(ref BodyVelocities velocityA, ref BodyVelocities veloci } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref Vector3Wide offsetA, ref Vector3Wide offsetB, - ref Vector3Wide biasVelocity, ref Symmetric3x3Wide effectiveMass, ref Vector softnessImpulseScale, ref Vector maximumImpulse, ref Vector3Wide accumulatedImpulse, ref BodyInertias inertiaA, ref BodyInertias inertiaB) + public static void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref Vector3Wide offsetA, ref Vector3Wide offsetB, + ref Vector3Wide biasVelocity, ref Symmetric3x3Wide effectiveMass, ref Vector softnessImpulseScale, ref Vector maximumImpulse, ref Vector3Wide accumulatedImpulse, ref BodyInertiaWide inertiaA, ref BodyInertiaWide inertiaB) { ComputeCorrectiveImpulse(ref velocityA, ref velocityB, offsetA, offsetB, biasVelocity, effectiveMass, softnessImpulseScale, accumulatedImpulse, out var correctiveImpulse); //This function DOES have a maximum impulse limit. diff --git a/BepuPhysics/Constraints/CenterDistanceConstraint.cs b/BepuPhysics/Constraints/CenterDistanceConstraint.cs index 8e404c131..fe211ffac 100644 --- a/BepuPhysics/Constraints/CenterDistanceConstraint.cs +++ b/BepuPhysics/Constraints/CenterDistanceConstraint.cs @@ -79,7 +79,7 @@ public struct CenterDistanceProjection public struct CenterDistanceConstraintFunctions : IConstraintFunctions> { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref CenterDistancePrestepData prestep, out CenterDistanceProjection projection) { Vector3Wide.Length(ab, out var distance); @@ -102,7 +102,7 @@ public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in } [MethodImpl(MethodImplOptions.AggressiveInlining)] - static void ApplyImpulse(ref BodyVelocities a, ref BodyVelocities b, ref CenterDistanceProjection projection, ref Vector impulse) + static void ApplyImpulse(ref BodyVelocityWide a, ref BodyVelocityWide b, ref CenterDistanceProjection projection, ref Vector impulse) { Vector3Wide.Scale(projection.JacobianA, impulse * projection.InverseMassA, out var changeA); Vector3Wide.Scale(projection.JacobianA, impulse * projection.InverseMassB, out var negatedChangeB); @@ -111,13 +111,13 @@ static void ApplyImpulse(ref BodyVelocities a, ref BodyVelocities b, ref CenterD } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref CenterDistanceProjection projection, ref Vector accumulatedImpulse) + public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref CenterDistanceProjection projection, ref Vector accumulatedImpulse) { ApplyImpulse(ref velocityA, ref velocityB, ref projection, ref accumulatedImpulse); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref CenterDistanceProjection projection, ref Vector accumulatedImpulse) + public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref CenterDistanceProjection projection, ref Vector accumulatedImpulse) { //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); Vector3Wide.Dot(velocityA.Linear, projection.JacobianA, out var linearCSVA); diff --git a/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs b/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs index 95a06b327..d0a0a3ad6 100644 --- a/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs +++ b/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs @@ -289,7 +289,7 @@ public ref MaterialPropertiesWide GetMaterialProperties(ref Contact1OneBodyPrest public unsafe struct Contact1OneBodyProjection { - public BodyInertias InertiaA; + public BodyInertiaWide InertiaA; public Vector PremultipliedFrictionCoefficient; public Vector3Wide Normal; public TangentFrictionOneBody.Projection Tangent; @@ -304,7 +304,7 @@ public struct Contact1OneBodyFunctions : IOneBodyContactConstraintFunctions dt, in BodyVelocities velocityA, ref Contact1OneBodyPrestepData prestep) + public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityWide velocityA, ref Contact1OneBodyPrestepData prestep) { PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, velocityA, ref prestep.Contact0.Depth); } @@ -462,7 +462,7 @@ public ref MaterialPropertiesWide GetMaterialProperties(ref Contact2OneBodyPrest public unsafe struct Contact2OneBodyProjection { - public BodyInertias InertiaA; + public BodyInertiaWide InertiaA; public Vector PremultipliedFrictionCoefficient; public Vector3Wide Normal; public TangentFrictionOneBody.Projection Tangent; @@ -479,7 +479,7 @@ public struct Contact2OneBodyFunctions : IOneBodyContactConstraintFunctions dt, in BodyVelocities velocityA, ref Contact2OneBodyPrestepData prestep) + public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityWide velocityA, ref Contact2OneBodyPrestepData prestep) { PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, velocityA, ref prestep.Contact0.Depth); PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.Normal, velocityA, ref prestep.Contact1.Depth); @@ -648,7 +648,7 @@ public ref MaterialPropertiesWide GetMaterialProperties(ref Contact3OneBodyPrest public unsafe struct Contact3OneBodyProjection { - public BodyInertias InertiaA; + public BodyInertiaWide InertiaA; public Vector PremultipliedFrictionCoefficient; public Vector3Wide Normal; public TangentFrictionOneBody.Projection Tangent; @@ -667,7 +667,7 @@ public struct Contact3OneBodyFunctions : IOneBodyContactConstraintFunctions dt, in BodyVelocities velocityA, ref Contact3OneBodyPrestepData prestep) + public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityWide velocityA, ref Contact3OneBodyPrestepData prestep) { PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, velocityA, ref prestep.Contact0.Depth); PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.Normal, velocityA, ref prestep.Contact1.Depth); @@ -848,7 +848,7 @@ public ref MaterialPropertiesWide GetMaterialProperties(ref Contact4OneBodyPrest public unsafe struct Contact4OneBodyProjection { - public BodyInertias InertiaA; + public BodyInertiaWide InertiaA; public Vector PremultipliedFrictionCoefficient; public Vector3Wide Normal; public TangentFrictionOneBody.Projection Tangent; @@ -869,7 +869,7 @@ public struct Contact4OneBodyFunctions : IOneBodyContactConstraintFunctions dt, in BodyVelocities velocityA, ref Contact4OneBodyPrestepData prestep) + public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityWide velocityA, ref Contact4OneBodyPrestepData prestep) { PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, velocityA, ref prestep.Contact0.Depth); PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.Normal, velocityA, ref prestep.Contact1.Depth); @@ -1049,8 +1049,8 @@ public ref Vector3Wide GetOffsetB(ref Contact1PrestepData prestep) public unsafe struct Contact1Projection { - public BodyInertias InertiaA; - public BodyInertias InertiaB; + public BodyInertiaWide InertiaA; + public BodyInertiaWide InertiaB; public Vector PremultipliedFrictionCoefficient; public Vector3Wide Normal; public TangentFriction.Projection Tangent; @@ -1065,7 +1065,7 @@ public struct Contact1Functions : IContactConstraintFunctions dt, in BodyVelocities velocityA, in BodyVelocities velocityB, ref Contact1PrestepData prestep) + public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityWide velocityA, in BodyVelocityWide velocityB, ref Contact1PrestepData prestep) { PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact0.Depth); } @@ -1238,8 +1238,8 @@ public ref Vector3Wide GetOffsetB(ref Contact2PrestepData prestep) public unsafe struct Contact2Projection { - public BodyInertias InertiaA; - public BodyInertias InertiaB; + public BodyInertiaWide InertiaA; + public BodyInertiaWide InertiaB; public Vector PremultipliedFrictionCoefficient; public Vector3Wide Normal; public TangentFriction.Projection Tangent; @@ -1256,7 +1256,7 @@ public struct Contact2Functions : IContactConstraintFunctions dt, in BodyVelocities velocityA, in BodyVelocities velocityB, ref Contact2PrestepData prestep) + public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityWide velocityA, in BodyVelocityWide velocityB, ref Contact2PrestepData prestep) { PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact0.Depth); PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact1.Depth); @@ -1441,8 +1441,8 @@ public ref Vector3Wide GetOffsetB(ref Contact3PrestepData prestep) public unsafe struct Contact3Projection { - public BodyInertias InertiaA; - public BodyInertias InertiaB; + public BodyInertiaWide InertiaA; + public BodyInertiaWide InertiaB; public Vector PremultipliedFrictionCoefficient; public Vector3Wide Normal; public TangentFriction.Projection Tangent; @@ -1461,7 +1461,7 @@ public struct Contact3Functions : IContactConstraintFunctions dt, in BodyVelocities velocityA, in BodyVelocities velocityB, ref Contact3PrestepData prestep) + public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityWide velocityA, in BodyVelocityWide velocityB, ref Contact3PrestepData prestep) { PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact0.Depth); PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact1.Depth); @@ -1659,8 +1659,8 @@ public ref Vector3Wide GetOffsetB(ref Contact4PrestepData prestep) public unsafe struct Contact4Projection { - public BodyInertias InertiaA; - public BodyInertias InertiaB; + public BodyInertiaWide InertiaA; + public BodyInertiaWide InertiaB; public Vector PremultipliedFrictionCoefficient; public Vector3Wide Normal; public TangentFriction.Projection Tangent; @@ -1681,7 +1681,7 @@ public struct Contact4Functions : IContactConstraintFunctions dt, in BodyVelocities velocityA, in BodyVelocities velocityB, ref Contact4PrestepData prestep) + public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityWide velocityA, in BodyVelocityWide velocityB, ref Contact4PrestepData prestep) { PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact0.Depth); PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact1.Depth); diff --git a/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs b/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs index 1e14a2bab..27b05d31b 100644 --- a/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs +++ b/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs @@ -171,14 +171,14 @@ public struct NonconvexAccumulatedImpulses public struct NonconvexOneBodyProjectionCommon { - public BodyInertias InertiaA; + public BodyInertiaWide InertiaA; public Vector FrictionCoefficient; public Vector SoftnessImpulseScale; } public struct NonconvexTwoBodyProjectionCommon { - public BodyInertias InertiaA; - public BodyInertias InertiaB; + public BodyInertiaWide InertiaA; + public BodyInertiaWide InertiaB; public Vector FrictionCoefficient; public Vector SoftnessImpulseScale; } @@ -217,7 +217,7 @@ public struct ContactNonconvexOneBodyFunctionspenetration. //This is not for any principled reason- only simplicity. May want to reconsider later, but remember the significant change in access pattern. @@ -264,7 +264,7 @@ public unsafe void WarmStart(ref BodyVelocities wsvA, ref TProjection projection } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocities wsvA, ref TProjection projection, ref TAccumulatedImpulses accumulatedImpulses) + public void Solve(ref BodyVelocityWide wsvA, ref TProjection projection, ref TAccumulatedImpulses accumulatedImpulses) { //Note that, unlike convex manifolds, we simply solve every contact in sequence rather than tangent->penetration. //This is not for any principled reason- only simplicity. May want to reconsider later, but remember the significant change in access pattern. @@ -284,7 +284,7 @@ public void Solve(ref BodyVelocities wsvA, ref TProjection projection, ref TAccu } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocities velocity, ref TPrestep prestep) + public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityWide velocity, ref TPrestep prestep) { ref var prestepContactStart = ref prestep.GetContact(ref prestep, 0); for (int i = 0; i < prestep.ContactCount; ++i) @@ -302,7 +302,7 @@ public struct ContactNonconvexTwoBodyFunctionspenetration. //This is not for any principled reason- only simplicity. May want to reconsider later, but remember the significant change in access pattern. @@ -352,7 +352,7 @@ public unsafe void WarmStart(ref BodyVelocities wsvA, ref BodyVelocities wsvB, r } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocities wsvA, ref BodyVelocities wsvB, ref TProjection projection, ref TAccumulatedImpulses accumulatedImpulses) + public void Solve(ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref TProjection projection, ref TAccumulatedImpulses accumulatedImpulses) { //Note that, unlike convex manifolds, we simply solve every contact in sequence rather than tangent->penetration. //This is not for any principled reason- only simplicity. May want to reconsider later, but remember the significant change in access pattern. @@ -371,7 +371,7 @@ public void Solve(ref BodyVelocities wsvA, ref BodyVelocities wsvB, ref TProject } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocities velocityA, in BodyVelocities velocityB, ref TPrestep prestep) + public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityWide velocityA, in BodyVelocityWide velocityB, ref TPrestep prestep) { ref var prestepOffsetB = ref prestep.GetOffsetB(ref prestep); ref var prestepContactStart = ref prestep.GetContact(ref prestep, 0); diff --git a/BepuPhysics/Constraints/Contact/PenetrationLimit.cs b/BepuPhysics/Constraints/Contact/PenetrationLimit.cs index 56255bcc9..d108051bb 100644 --- a/BepuPhysics/Constraints/Contact/PenetrationLimit.cs +++ b/BepuPhysics/Constraints/Contact/PenetrationLimit.cs @@ -18,7 +18,7 @@ public struct PenetrationLimitProjection public static class PenetrationLimit { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Prestep(in BodyInertias inertiaA, in BodyInertias inertiaB, + public static void Prestep(in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, in Vector3Wide contactOffsetA, in Vector3Wide contactOffsetB, in Vector3Wide normal, in Vector depth, in Vector positionErrorToVelocity, in Vector effectiveMassCFMScale, in Vector maximumRecoveryVelocity, float inverseDt, out PenetrationLimitProjection projection) @@ -77,9 +77,9 @@ public static void Prestep(in BodyInertias inertiaA, in BodyInertias inertiaB, /// Transforms an impulse from constraint space to world space, uses it to modify the cached world space velocities of the bodies. /// [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ApplyImpulse(in PenetrationLimitProjection projection, in BodyInertias inertiaA, in BodyInertias inertiaB, in Vector3Wide normal, + public static void ApplyImpulse(in PenetrationLimitProjection projection, in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, in Vector3Wide normal, in Vector correctiveImpulse, - ref BodyVelocities wsvA, ref BodyVelocities wsvB) + ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { var linearVelocityChangeA = correctiveImpulse * inertiaA.InverseMass; Vector3Wide.Scale(normal, linearVelocityChangeA, out var correctiveVelocityALinearVelocity); @@ -99,14 +99,14 @@ public static void ApplyImpulse(in PenetrationLimitProjection projection, in Bod [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void WarmStart( - in PenetrationLimitProjection projection, in BodyInertias inertiaA, in BodyInertias inertiaB, in Vector3Wide normal, - in Vector accumulatedImpulse, ref BodyVelocities wsvA, ref BodyVelocities wsvB) + in PenetrationLimitProjection projection, in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, in Vector3Wide normal, + in Vector accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ApplyImpulse(projection, inertiaA, inertiaB, normal, accumulatedImpulse, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ComputeCorrectiveImpulse(in BodyVelocities wsvA, in BodyVelocities wsvB, + public static void ComputeCorrectiveImpulse(in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, in PenetrationLimitProjection projection, in Vector3Wide normal, in Vector softnessImpulseScale, ref Vector accumulatedImpulse, out Vector correctiveCSI) @@ -127,15 +127,15 @@ public static void ComputeCorrectiveImpulse(in BodyVelocities wsvA, in BodyVeloc } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Solve(in PenetrationLimitProjection projection, in BodyInertias inertiaA, in BodyInertias inertiaB, in Vector3Wide normal, - in Vector softnessImpulseScale, ref Vector accumulatedImpulse, ref BodyVelocities wsvA, ref BodyVelocities wsvB) + public static void Solve(in PenetrationLimitProjection projection, in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, in Vector3Wide normal, + in Vector softnessImpulseScale, ref Vector accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeCorrectiveImpulse(wsvA, wsvB, projection, normal, softnessImpulseScale, ref accumulatedImpulse, out var correctiveCSI); ApplyImpulse(projection, inertiaA, inertiaB, normal, correctiveCSI, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void UpdatePenetrationDepth(in Vector dt, in Vector3Wide contactOffsetA, in Vector3Wide offsetB, in Vector3Wide normal, in BodyVelocities velocityA, in BodyVelocities velocityB, ref Vector penetrationDepth) + public static void UpdatePenetrationDepth(in Vector dt, in Vector3Wide contactOffsetA, in Vector3Wide offsetB, in Vector3Wide normal, in BodyVelocityWide velocityA, in BodyVelocityWide velocityB, ref Vector penetrationDepth) { //The normal is calibrated to point from B to A. Any movement of A along N results in a decrease in depth. Any movement of B along N results in an increase in depth. //estimatedPenetrationDepthChange = dot(normal, velocityDtA.Linear + velocityDtA.Angular x contactOffsetA) - dot(normal, velocityDtB.Linear + velocityDtB.Angular x contactOffsetB) diff --git a/BepuPhysics/Constraints/Contact/PenetrationLimitOneBody.cs b/BepuPhysics/Constraints/Contact/PenetrationLimitOneBody.cs index 7a338825a..4b9ab5679 100644 --- a/BepuPhysics/Constraints/Contact/PenetrationLimitOneBody.cs +++ b/BepuPhysics/Constraints/Contact/PenetrationLimitOneBody.cs @@ -17,7 +17,7 @@ public struct PenetrationLimitOneBodyProjection public static class PenetrationLimitOneBody { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Prestep(in BodyInertias inertiaA, + public static void Prestep(in BodyInertiaWide inertiaA, in Vector3Wide contactOffsetA, in Vector3Wide normal, in Vector depth, in Vector positionErrorToVelocity, in Vector effectiveMassCFMScale, in Vector maximumRecoveryVelocity, float inverseDt, out PenetrationLimitOneBodyProjection projection) @@ -42,8 +42,8 @@ public static void Prestep(in BodyInertias inertiaA, /// Transforms an impulse from constraint space to world space, uses it to modify the cached world space velocities of the bodies. /// [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ApplyImpulse(in PenetrationLimitOneBodyProjection projection, in BodyInertias inertiaA, in Vector3Wide normal, - in Vector correctiveImpulse, ref BodyVelocities wsvA) + public static void ApplyImpulse(in PenetrationLimitOneBodyProjection projection, in BodyInertiaWide inertiaA, in Vector3Wide normal, + in Vector correctiveImpulse, ref BodyVelocityWide wsvA) { var linearVelocityChangeA = correctiveImpulse * inertiaA.InverseMass; Vector3Wide.Scale(normal, linearVelocityChangeA, out var correctiveVelocityALinearVelocity); @@ -56,14 +56,14 @@ public static void ApplyImpulse(in PenetrationLimitOneBodyProjection projection, [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void WarmStart( - in PenetrationLimitOneBodyProjection projection, in BodyInertias inertiaA, in Vector3Wide normal, - in Vector accumulatedImpulse, ref BodyVelocities wsvA) + in PenetrationLimitOneBodyProjection projection, in BodyInertiaWide inertiaA, in Vector3Wide normal, + in Vector accumulatedImpulse, ref BodyVelocityWide wsvA) { ApplyImpulse(projection, inertiaA, normal, accumulatedImpulse, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ComputeCorrectiveImpulse(in BodyVelocities wsvA, + public static void ComputeCorrectiveImpulse(in BodyVelocityWide wsvA, in PenetrationLimitOneBodyProjection projection, in Vector3Wide normal, in Vector softnessImpulseScale, ref Vector accumulatedImpulse, out Vector correctiveCSI) @@ -82,15 +82,15 @@ public static void ComputeCorrectiveImpulse(in BodyVelocities wsvA, } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Solve(in PenetrationLimitOneBodyProjection projection, in BodyInertias inertiaA, in Vector3Wide normal, - in Vector softnessImpulseScale, ref Vector accumulatedImpulse, ref BodyVelocities wsvA) + public static void Solve(in PenetrationLimitOneBodyProjection projection, in BodyInertiaWide inertiaA, in Vector3Wide normal, + in Vector softnessImpulseScale, ref Vector accumulatedImpulse, ref BodyVelocityWide wsvA) { ComputeCorrectiveImpulse(wsvA, projection, normal, softnessImpulseScale, ref accumulatedImpulse, out var correctiveCSI); ApplyImpulse(projection, inertiaA, normal, correctiveCSI, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void UpdatePenetrationDepth(in Vector dt, in Vector3Wide contactOffset, in Vector3Wide normal, in BodyVelocities velocity, ref Vector penetrationDepth) + public static void UpdatePenetrationDepth(in Vector dt, in Vector3Wide contactOffset, in Vector3Wide normal, in BodyVelocityWide velocity, ref Vector penetrationDepth) { //The normal is calibrated to point from B to A. Any movement of A along N results in a decrease in depth. Any movement of B along N results in an increase in depth. //But one body constraints have no B. diff --git a/BepuPhysics/Constraints/Contact/TangentFriction.cs b/BepuPhysics/Constraints/Contact/TangentFriction.cs index 87b3d8cc1..d48475247 100644 --- a/BepuPhysics/Constraints/Contact/TangentFriction.cs +++ b/BepuPhysics/Constraints/Contact/TangentFriction.cs @@ -65,7 +65,7 @@ public static void ComputeJacobians(ref Vector3Wide tangentX, ref Vector3Wide ta [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Prestep(ref Vector3Wide tangentX, ref Vector3Wide tangentY, ref Vector3Wide offsetA, ref Vector3Wide offsetB, - ref BodyInertias inertiaA, ref BodyInertias inertiaB, + ref BodyInertiaWide inertiaA, ref BodyInertiaWide inertiaB, out Projection projection) { ComputeJacobians(ref tangentX, ref tangentY, ref offsetA, ref offsetB, out var jacobians); @@ -91,13 +91,13 @@ public static void Prestep(ref Vector3Wide tangentX, ref Vector3Wide tangentY, r /// Transforms an impulse from constraint space to world space, uses it to modify the cached world space velocities of the bodies. /// [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ApplyImpulse(ref Jacobians jacobians, ref BodyInertias inertiaA, ref BodyInertias inertiaB, - ref Vector2Wide correctiveImpulse, ref BodyVelocities wsvA, ref BodyVelocities wsvB) + public static void ApplyImpulse(ref Jacobians jacobians, ref BodyInertiaWide inertiaA, ref BodyInertiaWide inertiaB, + ref Vector2Wide correctiveImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { Matrix2x3Wide.Transform(correctiveImpulse, jacobians.LinearA, out var linearImpulseA); Matrix2x3Wide.Transform(correctiveImpulse, jacobians.AngularA, out var angularImpulseA); Matrix2x3Wide.Transform(correctiveImpulse, jacobians.AngularB, out var angularImpulseB); - BodyVelocities correctiveVelocityA, correctiveVelocityB; + BodyVelocityWide correctiveVelocityA, correctiveVelocityB; Vector3Wide.Scale(linearImpulseA, inertiaA.InverseMass, out correctiveVelocityA.Linear); Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseA, inertiaA.InverseInertiaTensor, out correctiveVelocityA.Angular); Vector3Wide.Scale(linearImpulseA, inertiaB.InverseMass, out correctiveVelocityB.Linear); @@ -109,8 +109,8 @@ public static void ApplyImpulse(ref Jacobians jacobians, ref BodyInertias inerti } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void WarmStart(ref Vector3Wide tangentX, ref Vector3Wide tangentY, ref TangentFriction.Projection projection, ref BodyInertias inertiaA, ref BodyInertias inertiaB, - ref Vector2Wide accumulatedImpulse, ref BodyVelocities wsvA, ref BodyVelocities wsvB) + public static void WarmStart(ref Vector3Wide tangentX, ref Vector3Wide tangentY, ref TangentFriction.Projection projection, ref BodyInertiaWide inertiaA, ref BodyInertiaWide inertiaB, + ref Vector2Wide accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeJacobians(ref tangentX, ref tangentY, ref projection.OffsetA, ref projection.OffsetB, out var jacobians); //TODO: If the previous frame and current frame are associated with different time steps, the previous frame's solution won't be a good solution anymore. @@ -119,7 +119,7 @@ public static void WarmStart(ref Vector3Wide tangentX, ref Vector3Wide tangentY, } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ComputeCorrectiveImpulse(ref BodyVelocities wsvA, ref BodyVelocities wsvB, ref TangentFriction.Projection data, ref Jacobians jacobians, + public static void ComputeCorrectiveImpulse(ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref TangentFriction.Projection data, ref Jacobians jacobians, ref Vector maximumImpulse, ref Vector2Wide accumulatedImpulse, out Vector2Wide correctiveCSI) { Matrix2x3Wide.TransformByTransposeWithoutOverlap(wsvA.Linear, jacobians.LinearA, out var csvaLinear); @@ -149,7 +149,7 @@ public static void ComputeCorrectiveImpulse(ref BodyVelocities wsvA, ref BodyVel } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Solve(ref Vector3Wide tangentX, ref Vector3Wide tangentY, ref TangentFriction.Projection projection, ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref Vector maximumImpulse, ref Vector2Wide accumulatedImpulse, ref BodyVelocities wsvA, ref BodyVelocities wsvB) + public static void Solve(ref Vector3Wide tangentX, ref Vector3Wide tangentY, ref TangentFriction.Projection projection, ref BodyInertiaWide inertiaA, ref BodyInertiaWide inertiaB, ref Vector maximumImpulse, ref Vector2Wide accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeJacobians(ref tangentX, ref tangentY, ref projection.OffsetA, ref projection.OffsetB, out var jacobians); ComputeCorrectiveImpulse(ref wsvA, ref wsvB, ref projection, ref jacobians, ref maximumImpulse, ref accumulatedImpulse, out var correctiveCSI); diff --git a/BepuPhysics/Constraints/Contact/TangentFrictionOneBody.cs b/BepuPhysics/Constraints/Contact/TangentFrictionOneBody.cs index cec88f363..f6d357b48 100644 --- a/BepuPhysics/Constraints/Contact/TangentFrictionOneBody.cs +++ b/BepuPhysics/Constraints/Contact/TangentFrictionOneBody.cs @@ -35,7 +35,7 @@ public static void ComputeJacobians(ref Vector3Wide tangentX, ref Vector3Wide ta } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Prestep(ref Vector3Wide tangentX, ref Vector3Wide tangentY, ref Vector3Wide offsetA, ref BodyInertias inertiaA, + public static void Prestep(ref Vector3Wide tangentX, ref Vector3Wide tangentY, ref Vector3Wide offsetA, ref BodyInertiaWide inertiaA, out Projection projection) { ComputeJacobians(ref tangentX, ref tangentY, ref offsetA, out var jacobians); @@ -55,12 +55,12 @@ public static void Prestep(ref Vector3Wide tangentX, ref Vector3Wide tangentY, r /// Transforms an impulse from constraint space to world space, uses it to modify the cached world space velocities of the bodies. /// [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ApplyImpulse(ref Jacobians jacobians, ref BodyInertias inertiaA, - ref Vector2Wide correctiveImpulse, ref BodyVelocities wsvA) + public static void ApplyImpulse(ref Jacobians jacobians, ref BodyInertiaWide inertiaA, + ref Vector2Wide correctiveImpulse, ref BodyVelocityWide wsvA) { Matrix2x3Wide.Transform(correctiveImpulse, jacobians.LinearA, out var linearImpulseA); Matrix2x3Wide.Transform(correctiveImpulse, jacobians.AngularA, out var angularImpulseA); - BodyVelocities correctiveVelocityA; + BodyVelocityWide correctiveVelocityA; Vector3Wide.Scale(linearImpulseA, inertiaA.InverseMass, out correctiveVelocityA.Linear); Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseA, inertiaA.InverseInertiaTensor, out correctiveVelocityA.Angular); Vector3Wide.Add(wsvA.Linear, correctiveVelocityA.Linear, out wsvA.Linear); @@ -68,8 +68,8 @@ public static void ApplyImpulse(ref Jacobians jacobians, ref BodyInertias inerti } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void WarmStart(ref Vector3Wide tangentX, ref Vector3Wide tangentY, ref Projection projection, ref BodyInertias inertiaA, - ref Vector2Wide accumulatedImpulse, ref BodyVelocities wsvA) + public static void WarmStart(ref Vector3Wide tangentX, ref Vector3Wide tangentY, ref Projection projection, ref BodyInertiaWide inertiaA, + ref Vector2Wide accumulatedImpulse, ref BodyVelocityWide wsvA) { ComputeJacobians(ref tangentX, ref tangentY, ref projection.OffsetA, out var jacobians); //TODO: If the previous frame and current frame are associated with different time steps, the previous frame's solution won't be a good solution anymore. @@ -78,7 +78,7 @@ public static void WarmStart(ref Vector3Wide tangentX, ref Vector3Wide tangentY, } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ComputeCorrectiveImpulse(ref BodyVelocities wsvA, ref Projection data, ref Jacobians jacobians, + public static void ComputeCorrectiveImpulse(ref BodyVelocityWide wsvA, ref Projection data, ref Jacobians jacobians, ref Vector maximumImpulse, ref Vector2Wide accumulatedImpulse, out Vector2Wide correctiveCSI) { Matrix2x3Wide.TransformByTransposeWithoutOverlap(wsvA.Linear, jacobians.LinearA, out var csvaLinear); @@ -101,7 +101,7 @@ public static void ComputeCorrectiveImpulse(ref BodyVelocities wsvA, ref Project [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Solve(ref Vector3Wide tangentX, ref Vector3Wide tangentY, - ref Projection projection, ref BodyInertias inertiaA, ref Vector maximumImpulse, ref Vector2Wide accumulatedImpulse, ref BodyVelocities wsvA) + ref Projection projection, ref BodyInertiaWide inertiaA, ref Vector maximumImpulse, ref Vector2Wide accumulatedImpulse, ref BodyVelocityWide wsvA) { ComputeJacobians(ref tangentX, ref tangentY, ref projection.OffsetA, out var jacobians); ComputeCorrectiveImpulse(ref wsvA, ref projection, ref jacobians, ref maximumImpulse, ref accumulatedImpulse, out var correctiveCSI); diff --git a/BepuPhysics/Constraints/Contact/TwistFriction.cs b/BepuPhysics/Constraints/Contact/TwistFriction.cs index 7c956b551..286ccab70 100644 --- a/BepuPhysics/Constraints/Contact/TwistFriction.cs +++ b/BepuPhysics/Constraints/Contact/TwistFriction.cs @@ -20,7 +20,7 @@ public struct TwistFrictionProjection public static class TwistFriction { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Prestep(ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref Vector3Wide angularJacobianA, + public static void Prestep(ref BodyInertiaWide inertiaA, ref BodyInertiaWide inertiaB, ref Vector3Wide angularJacobianA, out TwistFrictionProjection projection) { //Compute effective mass matrix contributions. No linear contributions for the twist constraint. @@ -44,8 +44,8 @@ public static void Prestep(ref BodyInertias inertiaA, ref BodyInertias inertiaB, /// Transforms an impulse from constraint space to world space, uses it to modify the cached world space velocities of the bodies. /// [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ApplyImpulse(ref Vector3Wide angularJacobianA, ref BodyInertias inertiaA, ref BodyInertias inertiaB, - ref Vector correctiveImpulse, ref BodyVelocities wsvA, ref BodyVelocities wsvB) + public static void ApplyImpulse(ref Vector3Wide angularJacobianA, ref BodyInertiaWide inertiaA, ref BodyInertiaWide inertiaB, + ref Vector correctiveImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { Vector3Wide.Scale(angularJacobianA, correctiveImpulse, out var worldCorrectiveImpulseA); Symmetric3x3Wide.TransformWithoutOverlap(worldCorrectiveImpulseA, inertiaA.InverseInertiaTensor, out var worldCorrectiveVelocityA); @@ -55,15 +55,15 @@ public static void ApplyImpulse(ref Vector3Wide angularJacobianA, ref BodyInerti } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void WarmStart(ref Vector3Wide angularJacobianA, ref BodyInertias inertiaA, ref BodyInertias inertiaB, - ref Vector accumulatedImpulse, ref BodyVelocities wsvA, ref BodyVelocities wsvB) + public static void WarmStart(ref Vector3Wide angularJacobianA, ref BodyInertiaWide inertiaA, ref BodyInertiaWide inertiaB, + ref Vector accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ApplyImpulse(ref angularJacobianA, ref inertiaA, ref inertiaB, ref accumulatedImpulse, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ComputeCorrectiveImpulse(ref Vector3Wide angularJacobianA, ref TwistFrictionProjection projection, - ref BodyVelocities wsvA, ref BodyVelocities wsvB, ref Vector maximumImpulse, + ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref Vector maximumImpulse, ref Vector accumulatedImpulse, out Vector correctiveCSI) { Vector3Wide.Dot(wsvA.Angular, angularJacobianA, out var csvA); @@ -78,8 +78,8 @@ public static void ComputeCorrectiveImpulse(ref Vector3Wide angularJacobianA, re } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Solve(ref Vector3Wide angularJacobianA, ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref TwistFrictionProjection projection, - ref Vector maximumImpulse, ref Vector accumulatedImpulse, ref BodyVelocities wsvA, ref BodyVelocities wsvB) + public static void Solve(ref Vector3Wide angularJacobianA, ref BodyInertiaWide inertiaA, ref BodyInertiaWide inertiaB, ref TwistFrictionProjection projection, + ref Vector maximumImpulse, ref Vector accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeCorrectiveImpulse(ref angularJacobianA, ref projection, ref wsvA, ref wsvB, ref maximumImpulse, ref accumulatedImpulse, out var correctiveCSI); ApplyImpulse(ref angularJacobianA, ref inertiaA, ref inertiaB, ref correctiveCSI, ref wsvA, ref wsvB); diff --git a/BepuPhysics/Constraints/Contact/TwistFrictionOneBody.cs b/BepuPhysics/Constraints/Contact/TwistFrictionOneBody.cs index 7758f95e2..9d55168c8 100644 --- a/BepuPhysics/Constraints/Contact/TwistFrictionOneBody.cs +++ b/BepuPhysics/Constraints/Contact/TwistFrictionOneBody.cs @@ -13,7 +13,7 @@ namespace BepuPhysics.Constraints.Contact public static class TwistFrictionOneBody { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Prestep(ref BodyInertias inertiaA, ref Vector3Wide angularJacobianA, + public static void Prestep(ref BodyInertiaWide inertiaA, ref Vector3Wide angularJacobianA, out TwistFrictionProjection projection) { //Compute effective mass matrix contributions. No linear contributions for the twist constraint. @@ -36,8 +36,8 @@ public static void Prestep(ref BodyInertias inertiaA, ref Vector3Wide angularJac /// Transforms an impulse from constraint space to world space, uses it to modify the cached world space velocities of the bodies. /// [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ApplyImpulse(ref Vector3Wide angularJacobianA, ref BodyInertias inertiaA, - ref Vector correctiveImpulse, ref BodyVelocities wsvA) + public static void ApplyImpulse(ref Vector3Wide angularJacobianA, ref BodyInertiaWide inertiaA, + ref Vector correctiveImpulse, ref BodyVelocityWide wsvA) { Vector3Wide.Scale(angularJacobianA, correctiveImpulse, out var worldCorrectiveImpulseA); Symmetric3x3Wide.TransformWithoutOverlap(worldCorrectiveImpulseA, inertiaA.InverseInertiaTensor, out var worldCorrectiveVelocityA); @@ -45,15 +45,15 @@ public static void ApplyImpulse(ref Vector3Wide angularJacobianA, ref BodyInerti } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void WarmStart(ref Vector3Wide angularJacobianA, ref BodyInertias inertiaA, - ref Vector accumulatedImpulse, ref BodyVelocities wsvA) + public static void WarmStart(ref Vector3Wide angularJacobianA, ref BodyInertiaWide inertiaA, + ref Vector accumulatedImpulse, ref BodyVelocityWide wsvA) { ApplyImpulse(ref angularJacobianA, ref inertiaA, ref accumulatedImpulse, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ComputeCorrectiveImpulse(ref Vector3Wide angularJacobianA, ref TwistFrictionProjection projection, - ref BodyVelocities wsvA, ref Vector maximumImpulse, + ref BodyVelocityWide wsvA, ref Vector maximumImpulse, ref Vector accumulatedImpulse, out Vector correctiveCSI) { Vector3Wide.Dot(wsvA.Angular, angularJacobianA, out var csvA); @@ -68,8 +68,8 @@ public static void ComputeCorrectiveImpulse(ref Vector3Wide angularJacobianA, re } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Solve(ref Vector3Wide angularJacobianA, ref BodyInertias inertiaA, ref TwistFrictionProjection projection, - ref Vector maximumImpulse, ref Vector accumulatedImpulse, ref BodyVelocities wsvA) + public static void Solve(ref Vector3Wide angularJacobianA, ref BodyInertiaWide inertiaA, ref TwistFrictionProjection projection, + ref Vector maximumImpulse, ref Vector accumulatedImpulse, ref BodyVelocityWide wsvA) { ComputeCorrectiveImpulse(ref angularJacobianA, ref projection, ref wsvA, ref maximumImpulse, ref accumulatedImpulse, out var correctiveCSI); ApplyImpulse(ref angularJacobianA, ref inertiaA, ref correctiveCSI, ref wsvA); diff --git a/BepuPhysics/Constraints/DistanceLimit.cs b/BepuPhysics/Constraints/DistanceLimit.cs index e46379433..545800eb0 100644 --- a/BepuPhysics/Constraints/DistanceLimit.cs +++ b/BepuPhysics/Constraints/DistanceLimit.cs @@ -116,7 +116,7 @@ public struct DistanceLimitProjection public struct DistanceLimitFunctions : IConstraintFunctions> { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref DistanceLimitPrestepData prestep, out DistanceLimitProjection projection) { DistanceServoFunctions.GetDistance(orientationA, ab, orientationB, prestep.LocalOffsetA, prestep.LocalOffsetB, @@ -138,14 +138,14 @@ public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref DistanceLimitProjection projection, ref Vector accumulatedImpulse) + public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref DistanceLimitProjection projection, ref Vector accumulatedImpulse) { DistanceServoFunctions.ApplyImpulse(ref velocityA, ref velocityB, projection.LinearImpulseToVelocityA, projection.AngularImpulseToVelocityA, projection.LinearImpulseToVelocityB, projection.AngularImpulseToVelocityB, ref accumulatedImpulse); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref DistanceLimitProjection projection, ref Vector accumulatedImpulse) + public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref DistanceLimitProjection projection, ref Vector accumulatedImpulse) { //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); Vector3Wide.Dot(velocityA.Linear, projection.LinearVelocityToImpulseA, out var linearCSIA); diff --git a/BepuPhysics/Constraints/DistanceServo.cs b/BepuPhysics/Constraints/DistanceServo.cs index 949decdf4..e9e49e67d 100644 --- a/BepuPhysics/Constraints/DistanceServo.cs +++ b/BepuPhysics/Constraints/DistanceServo.cs @@ -133,7 +133,7 @@ public static void GetDistance(in QuaternionWide orientationA, in Vector3Wide ab [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ComputeTransforms( - in BodyInertias inertiaA, in BodyInertias inertiaB, in Vector3Wide anchorOffsetA, in Vector3Wide anchorOffsetB, in Vector distance, ref Vector3Wide direction, + in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, in Vector3Wide anchorOffsetA, in Vector3Wide anchorOffsetB, in Vector distance, ref Vector3Wide direction, float dt, in SpringSettingsWide springSettings, out Vector positionErrorToVelocity, out Vector softnessImpulseScale, out Vector effectiveMass, out Vector3Wide linearVelocityToImpulseA, out Vector3Wide angularVelocityToImpulseA, out Vector3Wide angularVelocityToImpulseB, @@ -184,7 +184,7 @@ public static void ComputeTransforms( } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref DistanceServoPrestepData prestep, out DistanceServoProjection projection) { GetDistance(orientationA, ab, orientationB, prestep.LocalOffsetA, prestep.LocalOffsetB, out var anchorOffsetA, out var anchorOffsetB, out var anchorOffset, out var distance); @@ -205,7 +205,7 @@ public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ApplyImpulse(ref BodyVelocities velocityA, ref BodyVelocities velocityB, + public static void ApplyImpulse(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, in Vector3Wide linearImpulseToVelocityA, in Vector3Wide angularImpulseToVelocityA, in Vector3Wide linearImpulseToVelocityB, in Vector3Wide angularImpulseToVelocityB, ref Vector csi) { @@ -220,14 +220,14 @@ public static void ApplyImpulse(ref BodyVelocities velocityA, ref BodyVelocities } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref DistanceServoProjection projection, ref Vector accumulatedImpulse) + public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref DistanceServoProjection projection, ref Vector accumulatedImpulse) { ApplyImpulse(ref velocityA, ref velocityB, projection.LinearImpulseToVelocityA, projection.AngularImpulseToVelocityA, projection.LinearImpulseToVelocityB, projection.AngularImpulseToVelocityB, ref accumulatedImpulse); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref DistanceServoProjection projection, ref Vector accumulatedImpulse) + public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref DistanceServoProjection projection, ref Vector accumulatedImpulse) { //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); Vector3Wide.Dot(velocityA.Linear, projection.LinearVelocityToImpulseA, out var linearCSIA); diff --git a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs index 0f6d8e837..041392f6e 100644 --- a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs @@ -28,13 +28,13 @@ public struct FourBodyReferences public interface IFourBodyConstraintFunctions { void Prestep( - in QuaternionWide orientationA, in BodyInertias inertiaA, - in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, - in Vector3Wide ac, in QuaternionWide orientationC, in BodyInertias inertiaC, - in Vector3Wide ad, in QuaternionWide orientationD, in BodyInertias inertiaD, + in QuaternionWide orientationA, in BodyInertiaWide inertiaA, + in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, + in Vector3Wide ac, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, + in Vector3Wide ad, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, float dt, float inverseDt, ref TPrestepData prestepData, out TProjection projection); - void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref BodyVelocities velocityC, ref BodyVelocities velocityD, ref TProjection projection, ref TAccumulatedImpulse accumulatedImpulse); - void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref BodyVelocities velocityC, ref BodyVelocities velocityD, ref TProjection projection, ref TAccumulatedImpulse accumulatedImpulse); + void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref BodyVelocityWide velocityC, ref BodyVelocityWide velocityD, ref TProjection projection, ref TAccumulatedImpulse accumulatedImpulse); + void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref BodyVelocityWide velocityC, ref BodyVelocityWide velocityD, ref TProjection projection, ref TAccumulatedImpulse accumulatedImpulse); } /// diff --git a/BepuPhysics/Constraints/Hinge.cs b/BepuPhysics/Constraints/Hinge.cs index 120a202af..16516ec81 100644 --- a/BepuPhysics/Constraints/Hinge.cs +++ b/BepuPhysics/Constraints/Hinge.cs @@ -89,8 +89,8 @@ public struct HingeProjection public Vector2Wide HingeBiasVelocity; public Symmetric5x5Wide EffectiveMass; public Vector SoftnessImpulseScale; - public BodyInertias InertiaA; - public BodyInertias InertiaB; + public BodyInertiaWide InertiaA; + public BodyInertiaWide InertiaB; } public struct HingeAccumulatedImpulses @@ -102,7 +102,7 @@ public struct HingeAccumulatedImpulses public struct HingeFunctions : IConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref HingePrestepData prestep, out HingeProjection projection) { projection.InertiaA = inertiaA; @@ -166,7 +166,7 @@ public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in } [MethodImpl(MethodImplOptions.AggressiveInlining)] - private static void ApplyImpulse(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref HingeProjection projection, ref HingeAccumulatedImpulses csi) + private static void ApplyImpulse(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref HingeProjection projection, ref HingeAccumulatedImpulses csi) { //[ csi ] * [ I, skew(offsetA), -I, -skew(offsetB) ] // [ 0, constraintAxisAX, 0, -constraintAxisAX ] @@ -190,13 +190,13 @@ private static void ApplyImpulse(ref BodyVelocities velocityA, ref BodyVelocitie } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref HingeProjection projection, ref HingeAccumulatedImpulses accumulatedImpulse) + public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref HingeProjection projection, ref HingeAccumulatedImpulses accumulatedImpulse) { ApplyImpulse(ref velocityA, ref velocityB, ref projection, ref accumulatedImpulse); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref HingeProjection projection, ref HingeAccumulatedImpulses accumulatedImpulse) + public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref HingeProjection projection, ref HingeAccumulatedImpulses accumulatedImpulse) { //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); // [ I, skew(offsetA), -I, -skew(offsetB) ] diff --git a/BepuPhysics/Constraints/Inequality1DOF.cs b/BepuPhysics/Constraints/Inequality1DOF.cs index 9deb29527..f0d88ca2b 100644 --- a/BepuPhysics/Constraints/Inequality1DOF.cs +++ b/BepuPhysics/Constraints/Inequality1DOF.cs @@ -47,7 +47,7 @@ public static class Inequality2Body1DOF { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Prestep(ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref TwoBody1DOFJacobians jacobians, ref SpringSettingsWide springSettings, ref Vector maximumRecoveryVelocity, + public static void Prestep(ref BodyInertiaWide inertiaA, ref BodyInertiaWide inertiaB, ref TwoBody1DOFJacobians jacobians, ref SpringSettingsWide springSettings, ref Vector maximumRecoveryVelocity, ref Vector positionError, float dt, float inverseDt, out Projection2Body1DOF projection) { //unsoftened effective mass = (J * M^-1 * JT)^-1 @@ -310,14 +310,14 @@ public static void Prestep(ref BodyInertias inertiaA, ref BodyInertias inertiaB, /// [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ApplyImpulse(ref Projection2Body1DOF data, ref Vector correctiveImpulse, - ref BodyVelocities wsvA, ref BodyVelocities wsvB) + ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { //Applying the impulse requires transforming the constraint space impulse into a world space velocity change. //The first step is to transform into a world space impulse, which requires transforming by the transposed jacobian //(transpose(jacobian) goes from world to constraint space, jacobian goes from constraint to world space). //That world space impulse is then converted to a corrective velocity change by scaling the impulse by the inverse mass/inertia. //As an optimization for constraints with smaller jacobians, the jacobian * (inertia or mass) transform is precomputed. - BodyVelocities correctiveVelocityA, correctiveVelocityB; + BodyVelocityWide correctiveVelocityA, correctiveVelocityB; Vector3Wide.Scale(data.CSIToWSVLinearA, correctiveImpulse, out correctiveVelocityA.Linear); Vector3Wide.Scale(data.CSIToWSVAngularA, correctiveImpulse, out correctiveVelocityA.Angular); Vector3Wide.Scale(data.CSIToWSVLinearB, correctiveImpulse, out correctiveVelocityB.Linear); @@ -329,7 +329,7 @@ public static void ApplyImpulse(ref Projection2Body1DOF data, ref Vector } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void WarmStart(ref Projection2Body1DOF data, ref Vector accumulatedImpulse, ref BodyVelocities wsvA, ref BodyVelocities wsvB) + public static void WarmStart(ref Projection2Body1DOF data, ref Vector accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { //TODO: If the previous frame and current frame are associated with different time steps, the previous frame's solution won't be a good solution anymore. //To compensate for this, the accumulated impulse should be scaled if dt changes. @@ -337,7 +337,7 @@ public static void WarmStart(ref Projection2Body1DOF data, ref Vector acc } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ComputeCorrectiveImpulse(ref BodyVelocities wsvA, ref BodyVelocities wsvB, ref Projection2Body1DOF projection, ref Vector accumulatedImpulse, + public static void ComputeCorrectiveImpulse(ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref Projection2Body1DOF projection, ref Vector accumulatedImpulse, out Vector correctiveCSI) { //Take the world space velocity of each body into constraint space by transforming by the transpose(jacobian). @@ -363,7 +363,7 @@ public static void ComputeCorrectiveImpulse(ref BodyVelocities wsvA, ref BodyVel } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Solve(ref Projection2Body1DOF projection, ref Vector accumulatedImpulse, ref BodyVelocities wsvA, ref BodyVelocities wsvB) + public static void Solve(ref Projection2Body1DOF projection, ref Vector accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeCorrectiveImpulse(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulse, out var correctiveCSI); ApplyImpulse(ref projection, ref correctiveCSI, ref wsvA, ref wsvB); diff --git a/BepuPhysics/Constraints/LinearAxisLimit.cs b/BepuPhysics/Constraints/LinearAxisLimit.cs index c2879e6f9..995830ba4 100644 --- a/BepuPhysics/Constraints/LinearAxisLimit.cs +++ b/BepuPhysics/Constraints/LinearAxisLimit.cs @@ -112,7 +112,7 @@ public void Modify(in Vector3Wide anchorA, in Vector3Wide anchorB, ref Vector3Wi } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref LinearAxisLimitPrestepData prestep, out LinearAxisServoProjection projection) { Unsafe.SkipInit(out projection); @@ -131,7 +131,7 @@ public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref LinearAxisServoProjection projection, ref Vector accumulatedImpulse) + public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref LinearAxisServoProjection projection, ref Vector accumulatedImpulse) { LinearAxisServoFunctions.ApplyImpulse(ref velocityA, ref velocityB, projection.LinearImpulseToVelocityA, projection.AngularImpulseToVelocityA, projection.NegatedLinearImpulseToVelocityB, projection.AngularImpulseToVelocityB, @@ -139,7 +139,7 @@ public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref LinearAxisServoProjection projection, ref Vector accumulatedImpulse) + public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref LinearAxisServoProjection projection, ref Vector accumulatedImpulse) { LinearAxisServoFunctions.ComputeCorrectiveImpulse(ref velocityA, ref velocityB, projection.LinearVelocityToImpulseA, projection.AngularVelocityToImpulseA, projection.AngularVelocityToImpulseB, projection.BiasImpulse, projection.SoftnessImpulseScale, accumulatedImpulse, out var csi); diff --git a/BepuPhysics/Constraints/LinearAxisMotor.cs b/BepuPhysics/Constraints/LinearAxisMotor.cs index 439b216bf..b3d9ce437 100644 --- a/BepuPhysics/Constraints/LinearAxisMotor.cs +++ b/BepuPhysics/Constraints/LinearAxisMotor.cs @@ -82,7 +82,7 @@ public struct LinearAxisMotorPrestepData public struct LinearAxisMotorFunctions : IConstraintFunctions> { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref LinearAxisMotorPrestepData prestep, out LinearAxisServoProjection projection) { MotorSettingsWide.ComputeSoftness(prestep.Settings, dt, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale, out projection.MaximumImpulse); @@ -96,7 +96,7 @@ public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref LinearAxisServoProjection projection, ref Vector accumulatedImpulse) + public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref LinearAxisServoProjection projection, ref Vector accumulatedImpulse) { LinearAxisServoFunctions.ApplyImpulse(ref velocityA, ref velocityB, projection.LinearImpulseToVelocityA, projection.AngularImpulseToVelocityA, projection.NegatedLinearImpulseToVelocityB, projection.AngularImpulseToVelocityB, @@ -104,7 +104,7 @@ public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref LinearAxisServoProjection projection, ref Vector accumulatedImpulse) + public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref LinearAxisServoProjection projection, ref Vector accumulatedImpulse) { LinearAxisServoFunctions.ComputeCorrectiveImpulse(ref velocityA, ref velocityB, projection.LinearVelocityToImpulseA, projection.AngularVelocityToImpulseA, projection.AngularVelocityToImpulseB, projection.BiasImpulse, projection.SoftnessImpulseScale, accumulatedImpulse, out var csi); diff --git a/BepuPhysics/Constraints/LinearAxisServo.cs b/BepuPhysics/Constraints/LinearAxisServo.cs index bcaab6857..4ffaa4c45 100644 --- a/BepuPhysics/Constraints/LinearAxisServo.cs +++ b/BepuPhysics/Constraints/LinearAxisServo.cs @@ -118,7 +118,7 @@ public void Modify(in Vector3Wide anchorA, in Vector3Wide anchorB, ref Vector3Wi [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ComputeTransforms(ref TJacobianModifier jacobianModifier, in Vector3Wide localOffsetA, in Vector3Wide localOffsetB, in Vector3Wide localPlaneNormal, - in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector effectiveMassCFMScale, out Vector3Wide anchorA, out Vector3Wide anchorB, out Vector3Wide normal, out Vector effectiveMass, out Vector3Wide linearVelocityToImpulseA, out Vector3Wide angularVelocityToImpulseA, out Vector3Wide angularVelocityToImpulseB, @@ -161,7 +161,7 @@ public static void ComputeTransforms(ref TJacobianModifier ja } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref LinearAxisServoPrestepData prestep, out LinearAxisServoProjection projection) { SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); @@ -181,7 +181,7 @@ public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ApplyImpulse(ref BodyVelocities velocityA, ref BodyVelocities velocityB, + public static void ApplyImpulse(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, in Vector3Wide linearImpulseToVelocityA, in Vector3Wide angularImpulseToVelocityA, in Vector3Wide negatedLinearImpulseToVelocityB, in Vector3Wide angularImpulseToVelocityB, ref Vector csi) { Vector3Wide.Scale(linearImpulseToVelocityA, csi, out var linearChangeA); @@ -196,14 +196,14 @@ public static void ApplyImpulse(ref BodyVelocities velocityA, ref BodyVelocities } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref LinearAxisServoProjection projection, ref Vector accumulatedImpulse) + public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref LinearAxisServoProjection projection, ref Vector accumulatedImpulse) { ApplyImpulse(ref velocityA, ref velocityB, projection.LinearImpulseToVelocityA, projection.AngularImpulseToVelocityA, projection.NegatedLinearImpulseToVelocityB, projection.AngularImpulseToVelocityB, ref accumulatedImpulse); } - public static void ComputeCorrectiveImpulse(ref BodyVelocities velocityA, ref BodyVelocities velocityB, + public static void ComputeCorrectiveImpulse(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, in Vector3Wide linearVelocityToImpulseA, in Vector3Wide angularVelocityToImpulseA, in Vector3Wide angularVelocityToImpulseB, in Vector biasImpulse, in Vector softnessImpulseScale, in Vector accumulatedImpulse, out Vector csi) { @@ -217,7 +217,7 @@ public static void ComputeCorrectiveImpulse(ref BodyVelocities velocityA, ref Bo } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref LinearAxisServoProjection projection, ref Vector accumulatedImpulse) + public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref LinearAxisServoProjection projection, ref Vector accumulatedImpulse) { ComputeCorrectiveImpulse(ref velocityA, ref velocityB, projection.LinearVelocityToImpulseA, projection.AngularVelocityToImpulseA, projection.AngularVelocityToImpulseB, projection.BiasImpulse, projection.SoftnessImpulseScale, accumulatedImpulse, out var csi); diff --git a/BepuPhysics/Constraints/OneBodyAngularMotor.cs b/BepuPhysics/Constraints/OneBodyAngularMotor.cs index 91d4438de..ab86f60e5 100644 --- a/BepuPhysics/Constraints/OneBodyAngularMotor.cs +++ b/BepuPhysics/Constraints/OneBodyAngularMotor.cs @@ -60,7 +60,7 @@ public struct OneBodyAngularMotorPrestepData public struct OneBodyAngularMotorFunctions : IOneBodyConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertias inertiaA, + public void Prestep(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref OneBodyAngularMotorPrestepData prestep, out OneBodyAngularServoProjection projection) { projection.ImpulseToVelocity = inertiaA.InverseInertiaTensor; @@ -81,13 +81,13 @@ public static void ApplyImpulse(ref Vector3Wide angularVelocity, in Symmetric3x3 } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocities velocityA, ref OneBodyAngularServoProjection projection, ref Vector3Wide accumulatedImpulse) + public void WarmStart(ref BodyVelocityWide velocityA, ref OneBodyAngularServoProjection projection, ref Vector3Wide accumulatedImpulse) { ApplyImpulse(ref velocityA.Angular, projection.ImpulseToVelocity, accumulatedImpulse); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Solve(ref BodyVelocities velocityA, + public static void Solve(ref BodyVelocityWide velocityA, in Symmetric3x3Wide effectiveMass, in Vector softnessImpulseScale, in Vector3Wide biasImpulse, in Vector maximumImpulse, in Symmetric3x3Wide impulseToVelocityA, ref Vector3Wide accumulatedImpulse) { @@ -104,7 +104,7 @@ public static void Solve(ref BodyVelocities velocityA, } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocities velocityA, ref OneBodyAngularServoProjection projection, ref Vector3Wide accumulatedImpulse) + public void Solve(ref BodyVelocityWide velocityA, ref OneBodyAngularServoProjection projection, ref Vector3Wide accumulatedImpulse) { Solve(ref velocityA, projection.VelocityToImpulse, projection.SoftnessImpulseScale, projection.BiasImpulse, projection.MaximumImpulse, projection.ImpulseToVelocity, ref accumulatedImpulse); diff --git a/BepuPhysics/Constraints/OneBodyAngularServo.cs b/BepuPhysics/Constraints/OneBodyAngularServo.cs index f30c180e0..9694a7827 100644 --- a/BepuPhysics/Constraints/OneBodyAngularServo.cs +++ b/BepuPhysics/Constraints/OneBodyAngularServo.cs @@ -77,7 +77,7 @@ public struct OneBodyAngularServoProjection public struct OneBodyAngularServoFunctions : IOneBodyConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertias inertiaA, + public void Prestep(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref OneBodyAngularServoPrestepData prestep, out OneBodyAngularServoProjection projection) { projection.ImpulseToVelocity = inertiaA.InverseInertiaTensor; @@ -105,13 +105,13 @@ public static void ApplyImpulse(ref Vector3Wide angularVelocity, in Symmetric3x3 } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocities velocityA, ref OneBodyAngularServoProjection projection, ref Vector3Wide accumulatedImpulse) + public void WarmStart(ref BodyVelocityWide velocityA, ref OneBodyAngularServoProjection projection, ref Vector3Wide accumulatedImpulse) { ApplyImpulse(ref velocityA.Angular, projection.ImpulseToVelocity, accumulatedImpulse); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Solve(ref BodyVelocities velocityA, + public static void Solve(ref BodyVelocityWide velocityA, in Symmetric3x3Wide effectiveMass, in Vector softnessImpulseScale, in Vector3Wide biasImpulse, in Vector maximumImpulse, in Symmetric3x3Wide impulseToVelocityA, ref Vector3Wide accumulatedImpulse) { @@ -128,7 +128,7 @@ public static void Solve(ref BodyVelocities velocityA, } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocities velocityA, ref OneBodyAngularServoProjection projection, ref Vector3Wide accumulatedImpulse) + public void Solve(ref BodyVelocityWide velocityA, ref OneBodyAngularServoProjection projection, ref Vector3Wide accumulatedImpulse) { Solve(ref velocityA, projection.VelocityToImpulse, projection.SoftnessImpulseScale, projection.BiasImpulse, projection.MaximumImpulse, projection.ImpulseToVelocity, ref accumulatedImpulse); diff --git a/BepuPhysics/Constraints/OneBodyLinearMotor.cs b/BepuPhysics/Constraints/OneBodyLinearMotor.cs index 680297f44..15678996a 100644 --- a/BepuPhysics/Constraints/OneBodyLinearMotor.cs +++ b/BepuPhysics/Constraints/OneBodyLinearMotor.cs @@ -67,7 +67,7 @@ public struct OneBodyLinearMotorPrestepData public struct OneBodyLinearMotorFunctions : IOneBodyConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertias inertiaA, + public void Prestep(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref OneBodyLinearMotorPrestepData prestep, out OneBodyLinearServoProjection projection) { //TODO: Note that this grabs a world position. That poses a problem for different position representations. @@ -81,13 +81,13 @@ public void Prestep(in Vector3Wide positionA, in QuaternionWide orientationA, in [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocities velocityA, ref OneBodyLinearServoProjection projection, ref Vector3Wide accumulatedImpulse) + public void WarmStart(ref BodyVelocityWide velocityA, ref OneBodyLinearServoProjection projection, ref Vector3Wide accumulatedImpulse) { OneBodyLinearServoFunctions.ApplyImpulse(ref velocityA, projection, ref accumulatedImpulse); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocities velocityA, ref OneBodyLinearServoProjection projection, ref Vector3Wide accumulatedImpulse) + public void Solve(ref BodyVelocityWide velocityA, ref OneBodyLinearServoProjection projection, ref Vector3Wide accumulatedImpulse) { OneBodyLinearServoFunctions.SharedSolve(ref velocityA, projection, ref accumulatedImpulse); } diff --git a/BepuPhysics/Constraints/OneBodyLinearServo.cs b/BepuPhysics/Constraints/OneBodyLinearServo.cs index 34f069f1e..106958395 100644 --- a/BepuPhysics/Constraints/OneBodyLinearServo.cs +++ b/BepuPhysics/Constraints/OneBodyLinearServo.cs @@ -81,14 +81,14 @@ public struct OneBodyLinearServoProjection public Symmetric3x3Wide EffectiveMass; public Vector SoftnessImpulseScale; public Vector MaximumImpulse; - public BodyInertias Inertia; + public BodyInertiaWide Inertia; } public struct OneBodyLinearServoFunctions : IOneBodyConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ComputeTransforms(in Vector3Wide localOffset, in QuaternionWide orientation, in Vector effectiveMassCFMScale, - in BodyInertias inertia, out Vector3Wide offset, out Symmetric3x3Wide effectiveMass) + in BodyInertiaWide inertia, out Vector3Wide offset, out Symmetric3x3Wide effectiveMass) { //The grabber is roughly equivalent to a ball socket joint with a nonzero goal (and only one body). QuaternionWide.TransformWithoutOverlap(localOffset, orientation, out offset); @@ -103,7 +103,7 @@ public static void ComputeTransforms(in Vector3Wide localOffset, in QuaternionWi } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertias inertiaA, + public void Prestep(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref OneBodyLinearServoPrestepData prestep, out OneBodyLinearServoProjection projection) { //TODO: Note that this grabs a world position. That poses a problem for different position representations. @@ -120,7 +120,7 @@ public void Prestep(in Vector3Wide positionA, in QuaternionWide orientationA, in } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ApplyImpulse(ref BodyVelocities velocityA, in OneBodyLinearServoProjection projection, ref Vector3Wide csi) + public static void ApplyImpulse(ref BodyVelocityWide velocityA, in OneBodyLinearServoProjection projection, ref Vector3Wide csi) { Vector3Wide.CrossWithoutOverlap(projection.Offset, csi, out var wsi); Symmetric3x3Wide.TransformWithoutOverlap(wsi, projection.Inertia.InverseInertiaTensor, out var change); @@ -131,13 +131,13 @@ public static void ApplyImpulse(ref BodyVelocities velocityA, in OneBodyLinearSe } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocities velocityA, ref OneBodyLinearServoProjection projection, ref Vector3Wide accumulatedImpulse) + public void WarmStart(ref BodyVelocityWide velocityA, ref OneBodyLinearServoProjection projection, ref Vector3Wide accumulatedImpulse) { ApplyImpulse(ref velocityA, projection, ref accumulatedImpulse); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void SharedSolve(ref BodyVelocities velocities, in OneBodyLinearServoProjection projection, ref Vector3Wide accumulatedImpulse) + public static void SharedSolve(ref BodyVelocityWide velocities, in OneBodyLinearServoProjection projection, ref Vector3Wide accumulatedImpulse) { //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular); Vector3Wide.CrossWithoutOverlap(velocities.Angular, projection.Offset, out var angularCSV); @@ -163,7 +163,7 @@ public static void SharedSolve(ref BodyVelocities velocities, in OneBodyLinearSe } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocities velocityA, ref OneBodyLinearServoProjection projection, ref Vector3Wide accumulatedImpulse) + public void Solve(ref BodyVelocityWide velocityA, ref OneBodyLinearServoProjection projection, ref Vector3Wide accumulatedImpulse) { SharedSolve(ref velocityA, projection, ref accumulatedImpulse); } diff --git a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs index 0540ff051..ffea07aef 100644 --- a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs @@ -16,9 +16,9 @@ namespace BepuPhysics.Constraints /// Type of the projection to input. public interface IOneBodyConstraintFunctions { - void Prestep(in Vector3Wide position, in QuaternionWide orientation, in BodyInertias inertia, float dt, float inverseDt, ref TPrestepData prestepData, out TProjection projection); - void WarmStart(ref BodyVelocities velocity, ref TProjection projection, ref TAccumulatedImpulse accumulatedImpulse); - void Solve(ref BodyVelocities velocity, ref TProjection projection, ref TAccumulatedImpulse accumulatedImpulse); + void Prestep(in Vector3Wide position, in QuaternionWide orientation, in BodyInertiaWide inertia, float dt, float inverseDt, ref TPrestepData prestepData, out TProjection projection); + void WarmStart(ref BodyVelocityWide velocity, ref TProjection projection, ref TAccumulatedImpulse accumulatedImpulse); + void Solve(ref BodyVelocityWide velocity, ref TProjection projection, ref TAccumulatedImpulse accumulatedImpulse); } /// @@ -29,7 +29,7 @@ public interface IOneBodyConstraintFunctionsType of the projection to input. public interface IOneBodyContactConstraintFunctions : IOneBodyConstraintFunctions { - void IncrementallyUpdateContactData(in Vector dt, in BodyVelocities velocity, ref TPrestepData prestepData); + void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityWide velocity, ref TPrestepData prestepData); } //Not a big fan of complex generic-filled inheritance hierarchies, but this is the shortest evolutionary step to removing duplicates. diff --git a/BepuPhysics/Constraints/PointOnLineServo.cs b/BepuPhysics/Constraints/PointOnLineServo.cs index fda30f883..214977b8f 100644 --- a/BepuPhysics/Constraints/PointOnLineServo.cs +++ b/BepuPhysics/Constraints/PointOnLineServo.cs @@ -88,8 +88,8 @@ public struct PointOnLineServoProjection public Symmetric2x2Wide EffectiveMass; public Vector SoftnessImpulseScale; public Vector MaximumImpulse; - public BodyInertias InertiaA; - public BodyInertias InertiaB; + public BodyInertiaWide InertiaA; + public BodyInertiaWide InertiaB; } public struct PointOnLineServoFunctions : IConstraintFunctions @@ -103,7 +103,7 @@ static void GetAngularJacobians(in Matrix2x3Wide linearJacobians, in Vector3Wide } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref PointOnLineServoPrestepData prestep, out PointOnLineServoProjection projection) { //This constrains a point on B to a line attached to A. It works on two degrees of freedom at the same time; those are the tangent axes to the line direction. @@ -177,8 +177,8 @@ public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ApplyImpulse(ref BodyVelocities velocityA, ref BodyVelocities velocityB, - in Matrix2x3Wide linearJacobian, in Matrix2x3Wide angularJacobianA, in Matrix2x3Wide angularJacobianB, in BodyInertias inertiaA, in BodyInertias inertiaB, ref Vector2Wide csi) + public static void ApplyImpulse(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, + in Matrix2x3Wide linearJacobian, in Matrix2x3Wide angularJacobianA, in Matrix2x3Wide angularJacobianB, in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, ref Vector2Wide csi) { Matrix2x3Wide.Transform(csi, linearJacobian, out var linearImpulseA); Matrix2x3Wide.Transform(csi, angularJacobianA, out var angularImpulseA); @@ -195,14 +195,14 @@ public static void ApplyImpulse(ref BodyVelocities velocityA, ref BodyVelocities } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref PointOnLineServoProjection projection, ref Vector2Wide accumulatedImpulse) + public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref PointOnLineServoProjection projection, ref Vector2Wide accumulatedImpulse) { GetAngularJacobians(projection.LinearJacobian, projection.OffsetA, projection.OffsetB, out var angularA, out var angularB); ApplyImpulse(ref velocityA, ref velocityB, projection.LinearJacobian, angularA, angularB, projection.InertiaA, projection.InertiaB, ref accumulatedImpulse); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref PointOnLineServoProjection projection, ref Vector2Wide accumulatedImpulse) + public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref PointOnLineServoProjection projection, ref Vector2Wide accumulatedImpulse) { //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); GetAngularJacobians(projection.LinearJacobian, projection.OffsetA, projection.OffsetB, out var angularA, out var angularB); diff --git a/BepuPhysics/Constraints/SwingLimit.cs b/BepuPhysics/Constraints/SwingLimit.cs index cf1274bef..3ecf6f08b 100644 --- a/BepuPhysics/Constraints/SwingLimit.cs +++ b/BepuPhysics/Constraints/SwingLimit.cs @@ -102,7 +102,7 @@ public struct SwingLimitProjection public struct SwingLimitFunctions : IConstraintFunctions> { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref SwingLimitPrestepData prestep, out SwingLimitProjection projection) { //The swing limit attempts to keep an axis on body A within from an axis on body B. In other words, this is the same as a hinge joint, but with one fewer DOF. @@ -159,13 +159,13 @@ private static void ApplyImpulse(ref Vector3Wide angularVelocityA, ref Vector3Wi } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref SwingLimitProjection projection, ref Vector accumulatedImpulse) + public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref SwingLimitProjection projection, ref Vector accumulatedImpulse) { ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, ref projection, ref accumulatedImpulse); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref SwingLimitProjection projection, ref Vector accumulatedImpulse) + public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref SwingLimitProjection projection, ref Vector accumulatedImpulse) { //JB = -JA. This is (angularVelocityA * JA + angularVelocityB * JB) * effectiveMass => (angularVelocityA - angularVelocityB) * (JA * effectiveMass) Vector3Wide.Subtract(velocityA.Angular, velocityB.Angular, out var difference); diff --git a/BepuPhysics/Constraints/SwivelHinge.cs b/BepuPhysics/Constraints/SwivelHinge.cs index 739371ff1..9a99c4cf4 100644 --- a/BepuPhysics/Constraints/SwivelHinge.cs +++ b/BepuPhysics/Constraints/SwivelHinge.cs @@ -88,14 +88,14 @@ public struct SwivelHingeProjection public Vector4Wide BiasVelocity; public Symmetric4x4Wide EffectiveMass; public Vector SoftnessImpulseScale; - public BodyInertias InertiaA; - public BodyInertias InertiaB; + public BodyInertiaWide InertiaA; + public BodyInertiaWide InertiaB; } public struct SwivelHingeFunctions : IConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref SwivelHingePrestepData prestep, out SwivelHingeProjection projection) { projection.InertiaA = inertiaA; @@ -155,7 +155,7 @@ public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in } [MethodImpl(MethodImplOptions.AggressiveInlining)] - private static void ApplyImpulse(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref SwivelHingeProjection projection, ref Vector4Wide csi) + private static void ApplyImpulse(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref SwivelHingeProjection projection, ref Vector4Wide csi) { //[ csi ] * [ I, skew(offsetA), -I, -skew(offsetB) ] // [ 0, swivelA x hingeB, 0, -swivelA x hingeB ] @@ -179,13 +179,13 @@ private static void ApplyImpulse(ref BodyVelocities velocityA, ref BodyVelocitie } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref SwivelHingeProjection projection, ref Vector4Wide accumulatedImpulse) + public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref SwivelHingeProjection projection, ref Vector4Wide accumulatedImpulse) { ApplyImpulse(ref velocityA, ref velocityB, ref projection, ref accumulatedImpulse); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref SwivelHingeProjection projection, ref Vector4Wide accumulatedImpulse) + public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref SwivelHingeProjection projection, ref Vector4Wide accumulatedImpulse) { //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); //[ csi ] * [ I, skew(offsetA), -I, -skew(offsetB) ] diff --git a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs index 4fc926604..439d8569f 100644 --- a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs @@ -26,10 +26,10 @@ public struct ThreeBodyReferences /// Type of the projection to input. public interface IThreeBodyConstraintFunctions { - void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, in Vector3Wide ac, in QuaternionWide orientationC, in BodyInertias inertiaC, + void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide ac, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, float dt, float inverseDt, ref TPrestepData prestepData, out TProjection projection); - void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref BodyVelocities velocityC, ref TProjection projection, ref TAccumulatedImpulse accumulatedImpulse); - void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref BodyVelocities velocityC, ref TProjection projection, ref TAccumulatedImpulse accumulatedImpulse); + void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref BodyVelocityWide velocityC, ref TProjection projection, ref TAccumulatedImpulse accumulatedImpulse); + void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref BodyVelocityWide velocityC, ref TProjection projection, ref TAccumulatedImpulse accumulatedImpulse); } /// diff --git a/BepuPhysics/Constraints/TwistLimit.cs b/BepuPhysics/Constraints/TwistLimit.cs index 1a3bd35b5..dc7811d3b 100644 --- a/BepuPhysics/Constraints/TwistLimit.cs +++ b/BepuPhysics/Constraints/TwistLimit.cs @@ -95,7 +95,7 @@ public struct TwistLimitProjection public struct TwistLimitFunctions : IConstraintFunctions> { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref TwistLimitPrestepData prestep, out TwistLimitProjection projection) { Unsafe.SkipInit(out projection); @@ -126,13 +126,13 @@ public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref TwistLimitProjection projection, ref Vector accumulatedImpulse) + public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref TwistLimitProjection projection, ref Vector accumulatedImpulse) { TwistServoFunctions.ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, projection.ImpulseToVelocityA, projection.NegatedImpulseToVelocityB, accumulatedImpulse); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref TwistLimitProjection projection, ref Vector accumulatedImpulse) + public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref TwistLimitProjection projection, ref Vector accumulatedImpulse) { Vector3Wide.Subtract(velocityA.Angular, velocityB.Angular, out var netVelocity); Vector3Wide.Dot(netVelocity, projection.VelocityToImpulseA, out var csiVelocityComponent); diff --git a/BepuPhysics/Constraints/TwistMotor.cs b/BepuPhysics/Constraints/TwistMotor.cs index bb6109f43..b4240ce84 100644 --- a/BepuPhysics/Constraints/TwistMotor.cs +++ b/BepuPhysics/Constraints/TwistMotor.cs @@ -87,7 +87,7 @@ public struct TwistMotorProjection public struct TwistMotorFunctions : IConstraintFunctions> { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref TwistMotorPrestepData prestep, out TwistMotorProjection projection) { Unsafe.SkipInit(out projection); @@ -111,13 +111,13 @@ public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref TwistMotorProjection projection, ref Vector accumulatedImpulse) + public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref TwistMotorProjection projection, ref Vector accumulatedImpulse) { TwistServoFunctions.ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, projection.ImpulseToVelocityA, projection.NegatedImpulseToVelocityB, accumulatedImpulse); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref TwistMotorProjection projection, ref Vector accumulatedImpulse) + public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref TwistMotorProjection projection, ref Vector accumulatedImpulse) { Vector3Wide.Subtract(velocityA.Angular, velocityB.Angular, out var netVelocity); Vector3Wide.Dot(netVelocity, projection.VelocityToImpulseA, out var csiVelocityComponent); diff --git a/BepuPhysics/Constraints/TwistServo.cs b/BepuPhysics/Constraints/TwistServo.cs index 99b398ea6..20461b123 100644 --- a/BepuPhysics/Constraints/TwistServo.cs +++ b/BepuPhysics/Constraints/TwistServo.cs @@ -167,7 +167,7 @@ public static void ComputeEffectiveMass(float dt, in SpringSettingsWide springSe } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, + public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref TwistServoPrestepData prestep, out TwistServoProjection projection) { Unsafe.SkipInit(out projection); @@ -196,13 +196,13 @@ public static void ApplyImpulse(ref Vector3Wide angularVelocityA, ref Vector3Wid } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref TwistServoProjection projection, ref Vector accumulatedImpulse) + public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref TwistServoProjection projection, ref Vector accumulatedImpulse) { ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, projection.ImpulseToVelocityA, projection.NegatedImpulseToVelocityB, accumulatedImpulse); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref TwistServoProjection projection, ref Vector accumulatedImpulse) + public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref TwistServoProjection projection, ref Vector accumulatedImpulse) { Vector3Wide.Subtract(velocityA.Angular, velocityB.Angular, out var netVelocity); Vector3Wide.Dot(netVelocity, projection.VelocityToImpulseA, out var csiVelocityComponent); diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index cfb7c56d4..5a43b32d4 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -27,9 +27,9 @@ public struct TwoBodyReferences /// Type of the projection to input. public interface IConstraintFunctions { - void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, float dt, float inverseDt, ref TPrestepData prestepData, out TProjection projection); - void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref TProjection projection, ref TAccumulatedImpulse accumulatedImpulse); - void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref TProjection projection, ref TAccumulatedImpulse accumulatedImpulse); + void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref TPrestepData prestepData, out TProjection projection); + void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref TProjection projection, ref TAccumulatedImpulse accumulatedImpulse); + void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref TProjection projection, ref TAccumulatedImpulse accumulatedImpulse); } /// @@ -40,7 +40,7 @@ public interface IConstraintFunctionsType of the projection to input. public interface IContactConstraintFunctions : IConstraintFunctions { - void IncrementallyUpdateContactData(in Vector dt, in BodyVelocities velocityA, in BodyVelocities velocityB, ref TPrestepData prestepData); + void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityWide velocityA, in BodyVelocityWide velocityB, ref TPrestepData prestepData); } //Not a big fan of complex generic-filled inheritance hierarchies, but this is the shortest evolutionary step to removing duplicates. diff --git a/BepuPhysics/Constraints/VolumeConstraint.cs b/BepuPhysics/Constraints/VolumeConstraint.cs index 1e6248230..124217cdc 100644 --- a/BepuPhysics/Constraints/VolumeConstraint.cs +++ b/BepuPhysics/Constraints/VolumeConstraint.cs @@ -91,10 +91,10 @@ public struct VolumeConstraintFunctions : IFourBodyConstraintFunctions impulse) { Vector3Wide.Scale(negatedJacobianA, projection.InverseMassA * impulse, out var negativeVelocityChangeA); @@ -176,7 +176,7 @@ private static void GetNegatedJacobianA(in VolumeConstraintProjection projection } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref BodyVelocities velocityC, ref BodyVelocities velocityD, ref VolumeConstraintProjection projection, ref Vector accumulatedImpulse) + public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref BodyVelocityWide velocityC, ref BodyVelocityWide velocityD, ref VolumeConstraintProjection projection, ref Vector accumulatedImpulse) { //Unlike most constraints, the jacobians in a volume constraint can change magnitude and direction wildly in some cases. //Reusing the previous frame's accumulated impulse can result in catastrophically wrong guesses which require many iterations to correct. @@ -191,7 +191,7 @@ public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref BodyVelocities velocityC, ref BodyVelocities velocityD, ref VolumeConstraintProjection projection, ref Vector accumulatedImpulse) + public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref BodyVelocityWide velocityC, ref BodyVelocityWide velocityD, ref VolumeConstraintProjection projection, ref Vector accumulatedImpulse) { //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); GetNegatedJacobianA(projection, out var negatedJacobianA); diff --git a/BepuPhysics/Constraints/Weld.cs b/BepuPhysics/Constraints/Weld.cs index e68e2f4c5..024a7dde3 100644 --- a/BepuPhysics/Constraints/Weld.cs +++ b/BepuPhysics/Constraints/Weld.cs @@ -81,8 +81,8 @@ public struct WeldProjection public Vector3Wide OrientationBiasVelocity; public Symmetric6x6Wide EffectiveMass; public Vector SoftnessImpulseScale; - public BodyInertias InertiaA; - public BodyInertias InertiaB; + public BodyInertiaWide InertiaA; + public BodyInertiaWide InertiaB; } public struct WeldAccumulatedImpulses @@ -94,7 +94,7 @@ public struct WeldAccumulatedImpulses public struct WeldFunctions : IConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, float dt, float inverseDt, + public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref WeldPrestepData prestep, out WeldProjection projection) { projection.InertiaA = inertiaA; @@ -144,7 +144,7 @@ public void Prestep(in QuaternionWide orientationA, in BodyInertias inertiaA, in } [MethodImpl(MethodImplOptions.AggressiveInlining)] - private static void ApplyImpulse(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref WeldProjection projection, ref Vector3Wide orientationCSI, ref Vector3Wide offsetCSI) + private static void ApplyImpulse(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref WeldProjection projection, ref Vector3Wide orientationCSI, ref Vector3Wide offsetCSI) { //Recall the jacobians: //J = [ 0, I, 0, -I ] @@ -174,13 +174,13 @@ private static void ApplyImpulse(ref BodyVelocities velocityA, ref BodyVelocitie } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref WeldProjection projection, ref WeldAccumulatedImpulses accumulatedImpulse) + public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref WeldProjection projection, ref WeldAccumulatedImpulses accumulatedImpulse) { ApplyImpulse(ref velocityA, ref velocityB, ref projection, ref accumulatedImpulse.Orientation, ref accumulatedImpulse.Offset); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref WeldProjection projection, ref WeldAccumulatedImpulses accumulatedImpulse) + public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref WeldProjection projection, ref WeldAccumulatedImpulses accumulatedImpulse) { //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); //csv = V * JT @@ -207,7 +207,7 @@ public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, re //[MethodImpl(MethodImplOptions.AggressiveInlining)] - private static void ApplyImpulse(in BodyInertias inertiaA, in BodyInertias inertiaB, in Vector3Wide offset, in Vector3Wide orientationCSI, in Vector3Wide offsetCSI, ref BodyVelocities velocityA, ref BodyVelocities velocityB) + private static void ApplyImpulse(in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, in Vector3Wide offset, in Vector3Wide orientationCSI, in Vector3Wide offsetCSI, ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB) { //Recall the jacobians: //J = [ 0, I, 0, -I ] @@ -230,8 +230,8 @@ private static void ApplyImpulse(in BodyInertias inertiaA, in BodyInertias inert } //[MethodImpl(MethodImplOptions.NoInlining)] - public void WarmStart2(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, - in WeldPrestepData prestep, in WeldAccumulatedImpulses accumulatedImpulses, ref BodyVelocities wsvA, ref BodyVelocities wsvB) + public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, + in WeldPrestepData prestep, in WeldAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { Transform(prestep.LocalOffset, orientationA, out var offset); ApplyImpulse(inertiaA, inertiaB, offset, accumulatedImpulses.Orientation, accumulatedImpulses.Offset, ref wsvA, ref wsvB); @@ -239,8 +239,8 @@ public void WarmStart2(in QuaternionWide orientationA, in BodyInertias inertiaA, //ApplyImpulse(inertiaA, inertiaB, prestep.LocalOffset * orientationA, accumulatedImpulses.Orientation, accumulatedImpulses.Offset, ref wsvA, ref wsvB); } //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve2(in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, float dt, float inverseDt, - in WeldPrestepData prestep, ref WeldAccumulatedImpulses accumulatedImpulses, ref BodyVelocities wsvA, ref BodyVelocities wsvB) + public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, + in WeldPrestepData prestep, ref WeldAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { //The weld constraint handles 6 degrees of freedom simultaneously. The constraints are: //localOrientation * orientationA = orientationB diff --git a/BepuPhysics/FallbackBatch.cs b/BepuPhysics/FallbackBatch.cs index fbcdf39e5..e4cb95bde 100644 --- a/BepuPhysics/FallbackBatch.cs +++ b/BepuPhysics/FallbackBatch.cs @@ -13,10 +13,10 @@ namespace BepuPhysics { public struct FallbackTypeBatchResults { - public Buffer> BodyVelocities; + public Buffer> BodyVelocities; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Buffer GetVelocitiesForBody(int slotIndex) + public ref Buffer GetVelocitiesForBody(int slotIndex) { return ref BodyVelocities[slotIndex]; } diff --git a/Demos/Demos/Characters/CharacterMotionConstraint.cs b/Demos/Demos/Characters/CharacterMotionConstraint.cs index 9e03162aa..4525cc950 100644 --- a/Demos/Demos/Characters/CharacterMotionConstraint.cs +++ b/Demos/Demos/Characters/CharacterMotionConstraint.cs @@ -127,7 +127,7 @@ public struct StaticCharacterMotionProjection public Vector2Wide TargetVelocity; public Symmetric2x2Wide HorizontalEffectiveMass; public Vector MaximumHorizontalImpulse; - public BodyInertias InertiaA; + public BodyInertiaWide InertiaA; public Vector VerticalBiasVelocity; public Vector VerticalEffectiveMass; public Vector MaximumVerticalForce; @@ -167,7 +167,7 @@ static void ComputeJacobians(in Vector3Wide offsetA, in QuaternionWide basisQuat [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Prestep( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertias inertiaA, float dt, float inverseDt, ref StaticCharacterMotionPrestep prestepData, out StaticCharacterMotionProjection projection) + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref StaticCharacterMotionPrestep prestepData, out StaticCharacterMotionProjection projection) { //The motion constraint is split into two parts: the horizontal constraint, and the vertical constraint. //The horizontal constraint acts almost exactly like the TangentFriction, but we'll duplicate some of the logic to keep this implementation self-contained. @@ -226,8 +226,8 @@ public void Prestep( [MethodImpl(MethodImplOptions.AggressiveInlining)] private static void ApplyHorizontalImpulse(in Matrix3x3Wide basis, in Matrix2x3Wide angularJacobianA, in Vector2Wide constraintSpaceImpulse, - in BodyInertias inertiaA, - ref BodyVelocities velocityA) + in BodyInertiaWide inertiaA, + ref BodyVelocityWide velocityA) { //Transform the constraint space impulse into world space by using the jacobian and then apply each body's inverse inertia to get the velocity change. Vector3Wide.Scale(basis.X, constraintSpaceImpulse.X, out var linearImpulseAX); @@ -244,8 +244,8 @@ private static void ApplyHorizontalImpulse(in Matrix3x3Wide basis, [MethodImpl(MethodImplOptions.AggressiveInlining)] private static void ApplyVerticalImpulse(in Matrix3x3Wide basis, in Vector3Wide angularJacobianA, in Vector constraintSpaceImpulse, - in BodyInertias inertiaA, - ref BodyVelocities velocityA) + in BodyInertiaWide inertiaA, + ref BodyVelocityWide velocityA) { Vector3Wide.Scale(basis.Y, constraintSpaceImpulse, out var linearImpulseA); Vector3Wide.Scale(linearImpulseA, inertiaA.InverseMass, out var linearChangeA); @@ -257,7 +257,7 @@ private static void ApplyVerticalImpulse(in Matrix3x3Wide basis, } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocities velocityA, ref StaticCharacterMotionProjection projection, ref CharacterMotionAccumulatedImpulse accumulatedImpulse) + public void WarmStart(ref BodyVelocityWide velocityA, ref StaticCharacterMotionProjection projection, ref CharacterMotionAccumulatedImpulse accumulatedImpulse) { ComputeJacobians(projection.OffsetFromCharacter, projection.SurfaceBasis, out var basis, out var horizontalAngularJacobianA, out var verticalAngularJacobianA); @@ -266,7 +266,7 @@ public void WarmStart(ref BodyVelocities velocityA, ref StaticCharacterMotionPro } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocities velocityA, ref StaticCharacterMotionProjection projection, ref CharacterMotionAccumulatedImpulse accumulatedImpulse) + public void Solve(ref BodyVelocityWide velocityA, ref StaticCharacterMotionProjection projection, ref CharacterMotionAccumulatedImpulse accumulatedImpulse) { ComputeJacobians(projection.OffsetFromCharacter, projection.SurfaceBasis, out var basis, out var horizontalAngularJacobianA, out var verticalAngularJacobianA); @@ -431,8 +431,8 @@ public struct DynamicCharacterMotionProjection public Vector2Wide TargetVelocity; public Symmetric2x2Wide HorizontalEffectiveMass; public Vector MaximumHorizontalImpulse; - public BodyInertias InertiaA; - public BodyInertias InertiaB; + public BodyInertiaWide InertiaA; + public BodyInertiaWide InertiaB; public Vector VerticalBiasVelocity; public Vector VerticalEffectiveMass; public Vector MaximumVerticalForce; @@ -475,7 +475,7 @@ static void ComputeJacobians(in Vector3Wide offsetA, in Vector3Wide offsetB, in [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Prestep( - in QuaternionWide orientationA, in BodyInertias inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, float dt, float inverseDt, ref DynamicCharacterMotionPrestep prestepData, out DynamicCharacterMotionProjection projection) + in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref DynamicCharacterMotionPrestep prestepData, out DynamicCharacterMotionProjection projection) { //The motion constraint is split into two parts: the horizontal constraint, and the vertical constraint. //The horizontal constraint acts almost exactly like the TangentFriction, but we'll duplicate some of the logic to keep this implementation self-contained. @@ -540,8 +540,8 @@ public void Prestep( [MethodImpl(MethodImplOptions.AggressiveInlining)] private static void ApplyHorizontalImpulse(in Matrix3x3Wide basis, in Matrix2x3Wide angularJacobianA, in Matrix2x3Wide angularJacobianB, in Vector2Wide constraintSpaceImpulse, - in BodyInertias inertiaA, in BodyInertias inertiaB, - ref BodyVelocities velocityA, ref BodyVelocities velocityB) + in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, + ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB) { //Transform the constraint space impulse into world space by using the jacobian and then apply each body's inverse inertia to get the velocity change. Vector3Wide.Scale(basis.X, constraintSpaceImpulse.X, out var linearImpulseAX); @@ -563,8 +563,8 @@ private static void ApplyHorizontalImpulse(in Matrix3x3Wide basis, [MethodImpl(MethodImplOptions.AggressiveInlining)] private static void ApplyVerticalImpulse(in Matrix3x3Wide basis, in Vector3Wide angularJacobianA, in Vector3Wide angularJacobianB, in Vector constraintSpaceImpulse, - in BodyInertias inertiaA, in BodyInertias inertiaB, - ref BodyVelocities velocityA, ref BodyVelocities velocityB) + in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, + ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB) { Vector3Wide.Scale(basis.Y, constraintSpaceImpulse, out var linearImpulseA); Vector3Wide.Scale(linearImpulseA, inertiaA.InverseMass, out var linearChangeA); @@ -581,7 +581,7 @@ private static void ApplyVerticalImpulse(in Matrix3x3Wide basis, } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref DynamicCharacterMotionProjection projection, ref CharacterMotionAccumulatedImpulse accumulatedImpulse) + public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref DynamicCharacterMotionProjection projection, ref CharacterMotionAccumulatedImpulse accumulatedImpulse) { ComputeJacobians(projection.OffsetFromCharacter, projection.OffsetFromSupport, projection.SurfaceBasis, out var basis, out var horizontalAngularJacobianA, out var horizontalAngularJacobianB, out var verticalAngularJacobianA, out var verticalAngularJacobianB); @@ -590,7 +590,7 @@ public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref DynamicCharacterMotionProjection projection, ref CharacterMotionAccumulatedImpulse accumulatedImpulse) + public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref DynamicCharacterMotionProjection projection, ref CharacterMotionAccumulatedImpulse accumulatedImpulse) { ComputeJacobians(projection.OffsetFromCharacter, projection.OffsetFromSupport, projection.SurfaceBasis, out var basis, out var horizontalAngularJacobianA, out var horizontalAngularJacobianB, out var verticalAngularJacobianA, out var verticalAngularJacobianB); diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index 6592bfd6b..248ff5466 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -752,7 +752,7 @@ void Test2() convexHull1.ComputeInertia(1, out var scalarInertiaA); convexHull2.ComputeInertia(1, out var scalarInertiaB); - BodyInertias inertiaA; + BodyInertiaWide inertiaA; inertiaA.InverseMass = new Vector(1); inertiaA.InverseInertiaTensor.XX = new Vector(scalarInertiaA.InverseInertiaTensor.XX); inertiaA.InverseInertiaTensor.YX = new Vector(scalarInertiaA.InverseInertiaTensor.YX); @@ -761,7 +761,7 @@ void Test2() inertiaA.InverseInertiaTensor.ZY = new Vector(scalarInertiaA.InverseInertiaTensor.ZY); inertiaA.InverseInertiaTensor.ZZ = new Vector(scalarInertiaA.InverseInertiaTensor.ZZ); Vector3Wide.Broadcast(new Vector3(1, 3, 4), out var offset); - BodyInertias inertiaB; + BodyInertiaWide inertiaB; inertiaB.InverseMass = new Vector(1); inertiaB.InverseInertiaTensor.XX = new Vector(scalarInertiaB.InverseInertiaTensor.XX); inertiaB.InverseInertiaTensor.YX = new Vector(scalarInertiaB.InverseInertiaTensor.YX); From f0d5369148bef04625f6c99d5fe4ba87a6ce725b Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 9 Jul 2021 19:10:12 -0500 Subject: [PATCH 093/947] Bundled local and world inertia together to avoid loading an extra cache line during solver gathers. --- BepuPhysics/Bodies.cs | 48 ++---------- BepuPhysics/Bodies_GatherScatter.cs | 74 ++++++------------- BepuPhysics/BodyProperties.cs | 71 +++++------------- BepuPhysics/BodyReference.cs | 18 +++-- BepuPhysics/BodySet.cs | 17 ++--- .../Constraints/TwoBodyTypeProcessor.cs | 10 +-- BepuPhysics/IslandAwakener.cs | 13 ++-- BepuPhysics/IslandSleeper.cs | 2 +- BepuPhysics/PoseIntegrator.cs | 57 ++++++-------- DemoRenderer/ShapeDrawing/ShapesExtractor.cs | 2 +- .../Demos/Characters/CharacterControllers.cs | 2 +- 11 files changed, 105 insertions(+), 209 deletions(-) diff --git a/BepuPhysics/Bodies.cs b/BepuPhysics/Bodies.cs index 8614e3cc7..d4ae8486c 100644 --- a/BepuPhysics/Bodies.cs +++ b/BepuPhysics/Bodies.cs @@ -46,13 +46,6 @@ public partial class Bodies /// Reference to the active body set. public unsafe ref BodySet ActiveSet { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { return ref *Sets.Memory; } } - //TODO: Having Inertias publicly exposed seems like a recipe for confusion, given its ephemeral nature. We may want to explicitly delete it after frame execution and - //never expose it. If the user really wants an up to date world space inertia, it's pretty easy for them to build it from the local inertia and orientation anyway. - /// - /// The world transformed inertias of active bodies as of the last update. Note that this is not automatically updated for direct orientation changes or for body memory moves. - /// It is only updated once during the frame. It should be treated as ephemeral information. - /// - public Buffer Inertias; public BufferPool Pool { get; private set; } internal IslandAwakener awakener; @@ -307,7 +300,7 @@ public void LoopBody(int bodyIndex) { //The solver's connected bodies enumeration directly provides the constraint-stored reference, which is an index in the active set for active constraints and a handle for inactive constraints. //We forced the dynamic active at the beginning of BecomeKinematic, so we don't have to worry about the inactive side of things. - if (!IsKinematic(Bodies.ActiveSet.LocalInertias[bodyIndex])) + if (!IsKinematic(Bodies.ActiveSet.Inertias[bodyIndex].Local)) ++DynamicCount; } } @@ -319,7 +312,7 @@ void UpdateForKinematicStateChange(BodyHandle handle, ref BodyMemoryLocation loc ref var collidable = ref set.Collidables[location.Index]; if (collidable.Shape.Exists) { - var mobility = IsKinematic(set.LocalInertias[location.Index]) ? CollidableMobility.Kinematic : CollidableMobility.Dynamic; + var mobility = IsKinematic(set.Inertias[location.Index].Local) ? CollidableMobility.Kinematic : CollidableMobility.Dynamic; if (location.SetIndex == 0) { broadPhase.activeLeaves[collidable.BroadPhaseIndex] = new CollidableReference(mobility, handle); @@ -369,8 +362,8 @@ public void SetLocalInertia(BodyHandle handle, in BodyInertia localInertia) } //Note that the HandleToLocation slot reference is still valid; it may have been updated, but handle slots don't move. ref var set = ref Sets[location.SetIndex]; - var newlyKinematic = IsKinematic(localInertia) && !IsKinematic(set.LocalInertias[location.Index]); - set.LocalInertias[location.Index] = localInertia; + var newlyKinematic = IsKinematic(localInertia) && !IsKinematic(set.Inertias[location.Index].Local); + set.Inertias[location.Index].Local = localInertia; UpdateForKinematicStateChange(handle, ref location, ref set, newlyKinematic); } @@ -383,7 +376,7 @@ void UpdateForShapeChange(BodyHandle handle, int activeBodyIndex, TypedIndex old if (newShape.Exists) { //Add a collidable to the simulation for the new shape. - AddCollidableToBroadPhase(handle, set.MotionStates[activeBodyIndex].Pose, set.LocalInertias[activeBodyIndex], ref set.Collidables[activeBodyIndex]); + AddCollidableToBroadPhase(handle, set.MotionStates[activeBodyIndex].Pose, set.Inertias[activeBodyIndex].Local, ref set.Collidables[activeBodyIndex]); } else { @@ -436,7 +429,7 @@ public void ApplyDescription(BodyHandle handle, in BodyDescription description) ref var set = ref Sets[location.SetIndex]; ref var collidable = ref set.Collidables[location.Index]; var oldShape = collidable.Shape; - var newlyKinematic = IsKinematic(description.LocalInertia) && !IsKinematic(set.LocalInertias[location.Index]); + var newlyKinematic = IsKinematic(description.LocalInertia) && !IsKinematic(set.Inertias[location.Index].Local); set.ApplyDescriptionByIndex(location.Index, description); UpdateForShapeChange(handle, location.Index, oldShape, description.Collidable.Shape); UpdateForKinematicStateChange(handle, ref location, ref set, newlyKinematic); @@ -698,31 +691,6 @@ unsafe void ResizeHandles(int newCapacity) } } } - //Note that these resize and ensure capacity functions affect only the active set. - //Inactive islands are created with minimal allocations. Since you cannot add to or remove from inactive islands, it is pointless to try to modify their allocation sizes. - /// - /// Reallocates the inertias buffer for the target capacity. Will not shrink below the size of the current active set. - /// - internal void ResizeInertias(int capacity) - { - var targetCapacity = BufferPool.GetCapacityForCount(Math.Max(capacity, ActiveSet.Count)); - if (Inertias.Length != targetCapacity) - { - Pool.ResizeToAtLeast(ref Inertias, targetCapacity, Math.Min(Inertias.Length, ActiveSet.Count)); - } - } - /// - /// Guarantees that the inertias capacity is sufficient for the given capacity. - /// - internal void EnsureInertiasCapacity(int capacity) - { - if (capacity < ActiveSet.Count) - capacity = ActiveSet.Count; - if (Inertias.Length < capacity) - { - Pool.ResizeToAtLeast(ref Inertias, capacity, Math.Min(Inertias.Length, ActiveSet.Count)); - } - } /// /// Resizes the allocated spans for active body data. Note that this is conservative; it will never orphan existing objects. @@ -735,7 +703,6 @@ public void Resize(int capacity) { ActiveSet.InternalResize(targetBodyCapacity, Pool); } - ResizeInertias(capacity); var targetHandleCapacity = BufferPool.GetCapacityForCount(Math.Max(capacity, HandlePool.HighestPossiblyClaimedId + 1)); if (HandleToLocation.Length != targetHandleCapacity) { @@ -768,7 +735,6 @@ public void EnsureCapacity(int capacity) { ActiveSet.InternalResize(capacity, Pool); } - EnsureInertiasCapacity(capacity); if (HandleToLocation.Length < capacity) { ResizeHandles(capacity); @@ -803,8 +769,6 @@ public void Dispose() } } Pool.Return(ref Sets); - if (Inertias.Allocated) - Pool.Return(ref Inertias); Pool.Return(ref HandleToLocation); HandlePool.Dispose(Pool); } diff --git a/BepuPhysics/Bodies_GatherScatter.cs b/BepuPhysics/Bodies_GatherScatter.cs index 0316f1508..66b2a8cd5 100644 --- a/BepuPhysics/Bodies_GatherScatter.cs +++ b/BepuPhysics/Bodies_GatherScatter.cs @@ -17,9 +17,9 @@ public partial class Bodies { [MethodImpl(MethodImplOptions.AggressiveInlining)] - private static void WriteGatherInertia(ref int bundleBaseBodyIndexInSet, int bodyIndexInBundle, ref Buffer states, ref BodyInertiaWide gatheredInertias) + private static void WriteGatherInertia(ref int bundleBaseBodyIndexInSet, int bodyIndexInBundle, ref Buffer inertias, ref BodyInertiaWide gatheredInertias) { - ref var source = ref states[Unsafe.Add(ref bundleBaseBodyIndexInSet, bodyIndexInBundle)]; + ref var source = ref inertias[Unsafe.Add(ref bundleBaseBodyIndexInSet, bodyIndexInBundle)].World; ref var targetSlot = ref GetOffsetInstance(ref gatheredInertias, bodyIndexInBundle); GetFirst(ref targetSlot.InverseInertiaTensor.XX) = source.InverseInertiaTensor.XX; GetFirst(ref targetSlot.InverseInertiaTensor.YX) = source.InverseInertiaTensor.YX; @@ -40,33 +40,6 @@ private static void WriteGatherState(ref int bundleBaseBodyIndexInSet, int bodyI Vector3Wide.WriteFirst(state.Velocity.Linear, ref GetOffsetInstance(ref velocity.Linear, bodyIndexInBundle)); Vector3Wide.WriteFirst(state.Velocity.Angular, ref GetOffsetInstance(ref velocity.Angular, bodyIndexInBundle)); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - private static void WriteGatherState(ref int bundleBaseBodyIndexInSet, int bodyIndexInBundle, ref Buffer states, - ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocityWide velocity, ref BodyInertiaWide inertia) - { - ref var state = ref states[Unsafe.Add(ref bundleBaseBodyIndexInSet, bodyIndexInBundle)]; - Vector3Wide.WriteFirst(state.Pose.Position, ref GetOffsetInstance(ref position, bodyIndexInBundle)); - QuaternionWide.WriteFirst(state.Pose.Orientation, ref GetOffsetInstance(ref orientation, bodyIndexInBundle)); - Vector3Wide.WriteFirst(state.Velocity.Linear, ref GetOffsetInstance(ref velocity.Linear, bodyIndexInBundle)); - Vector3Wide.WriteFirst(state.Velocity.Angular, ref GetOffsetInstance(ref velocity.Angular, bodyIndexInBundle)); - - //ref var targetSlot = ref GetOffsetInstance(ref inertia, bodyIndexInBundle); - //GetFirst(ref targetSlot.InverseInertiaTensor.XX) = (float)state.PackedLocalInertia.InverseNormalizedInertiaXX * state.PackedLocalInertia.InverseMass; - //GetFirst(ref targetSlot.InverseInertiaTensor.YX) = 0; - //GetFirst(ref targetSlot.InverseInertiaTensor.YY) = (float)state.PackedLocalInertia.InverseNormalizedInertiaYY * state.PackedLocalInertia.InverseMass; - //GetFirst(ref targetSlot.InverseInertiaTensor.ZX) = 0; - //GetFirst(ref targetSlot.InverseInertiaTensor.ZY) = 0; - //GetFirst(ref targetSlot.InverseInertiaTensor.ZZ) = (float)state.PackedLocalInertia.InverseNormalizedInertiaZZ * state.PackedLocalInertia.InverseMass; - //GetFirst(ref targetSlot.InverseMass) = state.PackedLocalInertia.InverseMass; - ref var targetSlot = ref GetOffsetInstance(ref inertia, bodyIndexInBundle); - GetFirst(ref targetSlot.InverseInertiaTensor.XX) = state.PackedLocalInertia.InverseMass; - GetFirst(ref targetSlot.InverseInertiaTensor.YX) = 0; - GetFirst(ref targetSlot.InverseInertiaTensor.YY) = state.PackedLocalInertia.InverseMass; - GetFirst(ref targetSlot.InverseInertiaTensor.ZX) = 0; - GetFirst(ref targetSlot.InverseInertiaTensor.ZY) = 0; - GetFirst(ref targetSlot.InverseInertiaTensor.ZZ) = state.PackedLocalInertia.InverseMass; - GetFirst(ref targetSlot.InverseMass) = state.PackedLocalInertia.InverseMass; - } /// /// Gathers motion state information for a body bundle into an AOSOA bundle. @@ -93,11 +66,10 @@ public void GatherState(ref Vector references, int count, //Grab the base references for the body indices. Note that we make use of the references memory layout again. ref var baseIndex = ref Unsafe.As, int>(ref references); - ref var states = ref ActiveSet.MotionStates; for (int i = 0; i < count; ++i) { - WriteGatherState(ref baseIndex, i, ref states, ref position, ref orientation, ref velocity); - WriteGatherInertia(ref baseIndex, i, ref Inertias, ref inertia); + WriteGatherState(ref baseIndex, i, ref ActiveSet.MotionStates, ref position, ref orientation, ref velocity); + WriteGatherInertia(ref baseIndex, i, ref ActiveSet.Inertias, ref inertia); } } @@ -153,7 +125,7 @@ unsafe static void ScalarGather(int count, MotionState* motionStates, ref Vector } } //[MethodImpl(MethodImplOptions.AggressiveInlining)] - unsafe static void TransposingGather(int count, MotionState* motionStates, BodyInertia* inertias, ref Vector baseIndex, ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocityWide velocity, ref BodyInertiaWide inertia) + unsafe static void TransposingGather(int count, MotionState* motionStates, BodyInertias* inertias, ref Vector baseIndex, ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocityWide velocity, ref BodyInertiaWide inertia) { if (Avx.IsSupported) { @@ -265,14 +237,14 @@ unsafe static void TransposingGather(int count, MotionState* motionStates, BodyI } } { - var s0 = (float*)(inertias + indices[0]); - var s1 = (float*)(inertias + indices[1]); - var s2 = (float*)(inertias + indices[2]); - var s3 = (float*)(inertias + indices[3]); - var s4 = (float*)(inertias + indices[4]); - var s5 = (float*)(inertias + indices[5]); - var s6 = (float*)(inertias + indices[6]); - var s7 = (float*)(inertias + indices[7]); + var s0 = (float*)(inertias + indices[0]) + 8; + var s1 = (float*)(inertias + indices[1]) + 8; + var s2 = (float*)(inertias + indices[2]) + 8; + var s3 = (float*)(inertias + indices[3]) + 8; + var s4 = (float*)(inertias + indices[4]) + 8; + var s5 = (float*)(inertias + indices[5]) + 8; + var s6 = (float*)(inertias + indices[6]) + 8; + var s7 = (float*)(inertias + indices[7]) + 8; //Load every inertia vector. var m0 = Avx.LoadVector256(s0); @@ -350,8 +322,8 @@ public unsafe void GatherState(ref TwoBodyReferences references, int count, ref var states = ref ActiveSet.MotionStates; - TransposingGather(count, states.Memory, Inertias.Memory, ref references.IndexA, ref positionA, ref orientationA, ref velocityA, ref inertiaA); - TransposingGather(count, states.Memory, Inertias.Memory, ref references.IndexB, ref positionB, ref orientationB, ref velocityB, ref inertiaB); + TransposingGather(count, states.Memory, ActiveSet.Inertias.Memory, ref references.IndexA, ref positionA, ref orientationA, ref velocityA, ref inertiaA); + TransposingGather(count, states.Memory, ActiveSet.Inertias.Memory, ref references.IndexB, ref positionB, ref orientationB, ref velocityB, ref inertiaB); //for (int i = 0; i < count; ++i) //{ @@ -418,14 +390,15 @@ public void GatherState(ref ThreeBodyReferences references, int count, ref var baseIndexC = ref Unsafe.As, int>(ref references.IndexC); ref var states = ref ActiveSet.MotionStates; + ref var inertias = ref ActiveSet.Inertias; for (int i = 0; i < count; ++i) { WriteGatherState(ref baseIndexA, i, ref states, ref positionA, ref orientationA, ref velocityA); - WriteGatherInertia(ref baseIndexA, i, ref Inertias, ref inertiaA); + WriteGatherInertia(ref baseIndexA, i, ref inertias, ref inertiaA); WriteGatherState(ref baseIndexB, i, ref states, ref positionB, ref orientationB, ref velocityB); - WriteGatherInertia(ref baseIndexB, i, ref Inertias, ref inertiaB); + WriteGatherInertia(ref baseIndexB, i, ref inertias, ref inertiaB); WriteGatherState(ref baseIndexC, i, ref states, ref positionC, ref orientationC, ref velocityC); - WriteGatherInertia(ref baseIndexC, i, ref Inertias, ref inertiaC); + WriteGatherInertia(ref baseIndexC, i, ref inertias, ref inertiaC); } //TODO: In future versions, we will likely store the body position in different forms to allow for extremely large worlds. //That will be an opt-in feature. The default implementation will use the FP32 representation, but the user could choose to swap it out for a fp64 or fixed64 representation. @@ -493,16 +466,17 @@ public void GatherState(ref FourBodyReferences references, int count, ref var baseIndexD = ref Unsafe.As, int>(ref references.IndexD); ref var states = ref ActiveSet.MotionStates; + ref var inertias = ref ActiveSet.Inertias; for (int i = 0; i < count; ++i) { WriteGatherState(ref baseIndexA, i, ref states, ref positionA, ref orientationA, ref velocityA); - WriteGatherInertia(ref baseIndexA, i, ref Inertias, ref inertiaA); + WriteGatherInertia(ref baseIndexA, i, ref inertias, ref inertiaA); WriteGatherState(ref baseIndexB, i, ref states, ref positionB, ref orientationB, ref velocityB); - WriteGatherInertia(ref baseIndexB, i, ref Inertias, ref inertiaB); + WriteGatherInertia(ref baseIndexB, i, ref inertias, ref inertiaB); WriteGatherState(ref baseIndexC, i, ref states, ref positionC, ref orientationC, ref velocityC); - WriteGatherInertia(ref baseIndexC, i, ref Inertias, ref inertiaC); + WriteGatherInertia(ref baseIndexC, i, ref inertias, ref inertiaC); WriteGatherState(ref baseIndexD, i, ref states, ref positionD, ref orientationD, ref velocityD); - WriteGatherInertia(ref baseIndexD, i, ref Inertias, ref inertiaD); + WriteGatherInertia(ref baseIndexD, i, ref inertias, ref inertiaD); } //TODO: In future versions, we will likely store the body position in different forms to allow for extremely large worlds. //That will be an opt-in feature. The default implementation will use the FP32 representation, but the user could choose to swap it out for a fp64 or fixed64 representation. diff --git a/BepuPhysics/BodyProperties.cs b/BepuPhysics/BodyProperties.cs index 6fea699d5..e6312cec8 100644 --- a/BepuPhysics/BodyProperties.cs +++ b/BepuPhysics/BodyProperties.cs @@ -9,52 +9,6 @@ namespace BepuPhysics { - /// - /// Stores the inertia of a body in half precision. - /// - [StructLayout(LayoutKind.Sequential, Size = 4, Pack = 1)] - public struct PackedInertia - { - //TODO: Temporarily ignoring off-diagonal inertia for testing. - public float InverseMass; - //public Half InverseNormalizedInertiaXX; - ////public Half InverseNormalizedInertiaYX; - //public Half InverseNormalizedInertiaYY; - ////public Half InverseNormalizedInertiaZX; - ////public Half InverseNormalizedInertiaZY; - //public Half InverseNormalizedInertiaZZ; - - public PackedInertia(in BodyInertia inertia) - { - InverseMass = inertia.InverseMass; - //InverseNormalizedInertiaXX = (Half)(inertia.InverseInertiaTensor.XX / InverseMass); - ////InverseNormalizedInertiaYX = (Half)(inertia.InverseInertiaTensor.YX/ InverseMass); - //InverseNormalizedInertiaYY = (Half)(inertia.InverseInertiaTensor.YY / InverseMass); - ////InverseNormalizedInertiaZX = (Half)(inertia.InverseInertiaTensor.ZX/ InverseMass); - ////InverseNormalizedInertiaZY = (Half)(inertia.InverseInertiaTensor.ZY/ InverseMass); - //InverseNormalizedInertiaZZ = (Half)(inertia.InverseInertiaTensor.ZZ / InverseMass); - } - - public readonly void Unpack(out BodyInertia inertia) - { - //TODO: not necessary in complete implementation - inertia = default; - inertia.InverseMass = InverseMass; - //inertia.InverseInertiaTensor.XX = (float)InverseNormalizedInertiaXX * InverseMass; - ////inertia.InverseInertiaTensor.YX = (float)InverseNormalizedInertiaYX * InverseMass; - //inertia.InverseInertiaTensor.YY = (float)InverseNormalizedInertiaYY * InverseMass; - ////inertia.InverseInertiaTensor.ZX = (float)InverseNormalizedInertiaZX * InverseMass; - ////inertia.InverseInertiaTensor.ZY = (float)InverseNormalizedInertiaZY * InverseMass; - //inertia.InverseInertiaTensor.ZZ = (float)InverseNormalizedInertiaZZ * InverseMass; - } - public readonly BodyInertia Unpack() - { - Unpack(out var inertia); - return inertia; - } - - } - /// /// Describes the pose and velocity of a body. /// @@ -103,10 +57,6 @@ public struct MotionState /// public RigidPose Pose; /// - /// Packed inertia of the body. - /// - public PackedInertia PackedLocalInertia; - /// /// Linear and angular velocity of the body. /// public BodyVelocity Velocity; @@ -118,7 +68,7 @@ public struct MotionState /// /// Represents a rigid transformation. /// - [StructLayout(LayoutKind.Sequential, Size = 28, Pack = 1)] + [StructLayout(LayoutKind.Sequential, Size = 32, Pack = 1)] public struct RigidPose { //Note that we store a quaternion rather than a matrix3x3. While this often requires some overhead when performing vector transforms or extracting basis vectors, @@ -225,6 +175,23 @@ public struct BodyInertia public float InverseMass; } + [StructLayout(LayoutKind.Sequential)] + public struct BodyInertias + { + /// + /// Local inertia of the body. + /// + public BodyInertia Local; + /// + /// Transformed world inertia of the body. Note that this is only valid between the velocity integration that updates it and the pose integration that follows. + /// Outside of that execution window, this should be considered undefined. + /// + /// + /// We cache this here because velocity integration wants both the local and world inertias, and any integration happening within the solver will do so without the benefit of sequential loads. + /// In that context, being able to load a single cache line to grab both local and world inertia helps quite a lot. + public BodyInertia World; + } + public struct RigidPoses { public Vector3Wide Position; @@ -261,7 +228,7 @@ public struct BodyVelocityWide public struct BodyInertiaWide { public Symmetric3x3Wide InverseInertiaTensor; - //Note that the inverse mass is included in the BodyInertias bundle. InverseMass is rotationally invariant, so it doesn't need to be updated... + //Note that the inverse mass is included in the bundle. InverseMass is rotationally invariant, so it doesn't need to be updated... //But it's included alongside the rotated inertia tensor because to split it out would require that constraint presteps suffer another cache miss when they //gather the inverse mass in isolation. (From the solver's perspective, inertia/mass gathering is incoherent.) public Vector InverseMass; diff --git a/BepuPhysics/BodyReference.cs b/BepuPhysics/BodyReference.cs index 98b0fd203..8ce00c2b4 100644 --- a/BepuPhysics/BodyReference.cs +++ b/BepuPhysics/BodyReference.cs @@ -137,7 +137,7 @@ public ref BodyInertia LocalInertia get { ref var location = ref MemoryLocation; - return ref Bodies.Sets[location.SetIndex].LocalInertias[location.Index]; + return ref Bodies.Sets[location.SetIndex].Inertias[location.Index].Local; } } @@ -219,9 +219,11 @@ public void ComputeInverseInertia(out Symmetric3x3 inverseInertia) { ref var location = ref MemoryLocation; ref var set = ref Bodies.Sets[MemoryLocation.SetIndex]; - ref var localInertia = ref set.LocalInertias[location.Index]; + //Note that inertia.World is ephemeral data packed into the same cache line for the benefit of the solver. + //It should not be assumed to contain up to date information outside of the velocity integration to pose integration interval, so this computes world inertia from scratch. + ref var inertia = ref set.Inertias[location.Index]; ref var pose = ref set.MotionStates[location.Index].Pose; - PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, pose.Orientation, out inverseInertia); + PoseIntegration.RotateInverseInertia(inertia.Local.InverseInertiaTensor, pose.Orientation, out inverseInertia); } /// @@ -341,7 +343,7 @@ public static void ApplyImpulse(in Vector3 impulse, in Vector3 impulseOffset, re [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ApplyImpulse(in BodySet set, int index, in Vector3 impulse, in Vector3 impulseOffset) { - ref var localInertia = ref set.LocalInertias[index]; + ref var localInertia = ref set.Inertias[index].Local; ref var state = ref set.MotionStates[index]; ApplyImpulse(impulse, impulseOffset, ref localInertia, ref state.Pose, ref state.Velocity); } @@ -393,7 +395,7 @@ public void ApplyLinearImpulse(in Vector3 impulse) { ref var location = ref MemoryLocation; ref var set = ref Bodies.Sets[location.SetIndex]; - ApplyLinearImpulse(impulse, set.LocalInertias[location.Index].InverseMass, ref set.MotionStates[location.Index].Velocity.Linear); + ApplyLinearImpulse(impulse, set.Inertias[location.Index].Local.InverseMass, ref set.MotionStates[location.Index].Velocity.Linear); } /// @@ -416,9 +418,11 @@ public void ApplyAngularImpulse(in Vector3 angularImpulse) { ref var location = ref MemoryLocation; ref var set = ref Bodies.Sets[location.SetIndex]; - ref var localInertia = ref set.LocalInertias[location.Index]; + ref var inertia = ref set.Inertias[location.Index]; ref var state = ref set.MotionStates[location.Index]; - PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, state.Pose.Orientation, out var inverseInertia); + //Note that inertia.World is ephemeral data packed into the same cache line for the benefit of the solver. + //It should not be assumed to contain up to date information outside of the velocity integration to pose integration interval, so this computes world inertia from scratch. + PoseIntegration.RotateInverseInertia(inertia.Local.InverseInertiaTensor, state.Pose.Orientation, out var inverseInertia); ApplyAngularImpulse(angularImpulse, inverseInertia, ref state.Velocity.Angular); } } diff --git a/BepuPhysics/BodySet.cs b/BepuPhysics/BodySet.cs index c1266fd35..38c933695 100644 --- a/BepuPhysics/BodySet.cs +++ b/BepuPhysics/BodySet.cs @@ -36,7 +36,7 @@ public struct BodySet public Buffer IndexToHandle; public Buffer MotionStates; - public Buffer LocalInertias; + public Buffer Inertias; /// /// The collidables owned by each body in the set. Speculative margins, continuity settings, and shape indices can be changed directly. @@ -89,7 +89,7 @@ internal bool RemoveAt(int bodyIndex, out BodyHandle handle, out int movedBodyIn movedBodyIndex = Count; //Copy the memory state of the last element down. MotionStates[bodyIndex] = MotionStates[movedBodyIndex]; - LocalInertias[bodyIndex] = LocalInertias[movedBodyIndex]; + Inertias[bodyIndex] = Inertias[movedBodyIndex]; Activity[bodyIndex] = Activity[movedBodyIndex]; Collidables[bodyIndex] = Collidables[movedBodyIndex]; //Note that the constraint list is NOT disposed before being overwritten. @@ -127,9 +127,8 @@ internal void ApplyDescriptionByIndex(int index, in BodyDescription description) ref var state = ref MotionStates[index]; state.Pose = description.Pose; state.Velocity = description.Velocity; - //TODO: We're just trying this right now; note redundancy that we need to deal with. - state.PackedLocalInertia = new PackedInertia(description.LocalInertia); - LocalInertias[index] = description.LocalInertia; + //Note that the world inertia is only valid in the velocity integration->pose integration interval, so we don't need to initialize it here. + Inertias[index].Local = description.LocalInertia; ref var collidable = ref Collidables[index]; collidable.Continuity = description.Collidable.Continuity; collidable.SpeculativeMargin = description.Collidable.SpeculativeMargin; @@ -148,7 +147,7 @@ public void GetDescription(int index, out BodyDescription description) ref var state = ref MotionStates[index]; description.Pose = state.Pose; description.Velocity = state.Velocity; - description.LocalInertia = LocalInertias[index]; + description.LocalInertia = Inertias[index].Local; ref var collidable = ref Collidables[index]; description.Collidable.Continuity = collidable.Continuity; description.Collidable.Shape = collidable.Shape; @@ -226,7 +225,7 @@ internal void Swap(int slotA, int slotB, ref Buffer handleTo Helpers.Swap(ref IndexToHandle[slotA], ref IndexToHandle[slotB]); Helpers.Swap(ref Collidables[slotA], ref Collidables[slotB]); Helpers.Swap(ref MotionStates[slotA], ref MotionStates[slotB]); - Helpers.Swap(ref LocalInertias[slotA], ref LocalInertias[slotB]); + Helpers.Swap(ref Inertias[slotA], ref Inertias[slotB]); Helpers.Swap(ref Activity[slotA], ref Activity[slotB]); Helpers.Swap(ref Constraints[slotA], ref Constraints[slotB]); } @@ -239,7 +238,7 @@ internal unsafe void InternalResize(int targetBodyCapacity, BufferPool pool) targetBodyCapacity = BufferPool.GetCapacityForCount(targetBodyCapacity); Debug.Assert(MotionStates.Length != BufferPool.GetCapacityForCount(targetBodyCapacity), "Should not try to use internal resize of the result won't change the size."); pool.ResizeToAtLeast(ref MotionStates, targetBodyCapacity, Count); - pool.ResizeToAtLeast(ref LocalInertias, targetBodyCapacity, Count); + pool.ResizeToAtLeast(ref Inertias, targetBodyCapacity, Count); pool.ResizeToAtLeast(ref IndexToHandle, targetBodyCapacity, Count); pool.ResizeToAtLeast(ref Collidables, targetBodyCapacity, Count); pool.ResizeToAtLeast(ref Activity, targetBodyCapacity, Count); @@ -262,7 +261,7 @@ public unsafe void Clear(BufferPool pool) public void DisposeBuffers(BufferPool pool) { pool.Return(ref MotionStates); - pool.Return(ref LocalInertias); + pool.Return(ref Inertias); pool.Return(ref IndexToHandle); pool.Return(ref Collidables); pool.Return(ref Activity); diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 5a43b32d4..1c3e11703 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -309,7 +309,7 @@ static unsafe void Prefetch(void* address) //TODO: ARM? } [MethodImpl(MethodImplOptions.AggressiveInlining)] - static unsafe void PrefetchBundle(MotionState* motionBase, BodyInertia* inertiaBase, ref TwoBodyReferences references, int countInBundle) + static unsafe void PrefetchBundle(MotionState* motionBase, BodyInertias* inertiaBase, ref TwoBodyReferences references, int countInBundle) { var indicesA = (int*)Unsafe.AsPointer(ref references.IndexA); var indicesB = (int*)Unsafe.AsPointer(ref references.IndexB); @@ -327,7 +327,7 @@ static unsafe void PrefetchBundle(MotionState* motionBase, BodyInertia* inertiaB const int warmStartPrefetchDistance = 8; const int solvePrefetchDistance = 4; [MethodImpl(MethodImplOptions.AggressiveInlining)] - unsafe static void EarlyPrefetch(int prefetchDistance, ref TypeBatch typeBatch, ref Buffer references, ref Buffer states, ref Buffer inertias, int startBundleIndex, int exclusiveEndBundleIndex) + unsafe static void EarlyPrefetch(int prefetchDistance, ref TypeBatch typeBatch, ref Buffer references, ref Buffer states, ref Buffer inertias, int startBundleIndex, int exclusiveEndBundleIndex) { exclusiveEndBundleIndex = Math.Min(exclusiveEndBundleIndex, startBundleIndex + prefetchDistance); var lastBundleIndex = exclusiveEndBundleIndex - 1; @@ -340,7 +340,7 @@ unsafe static void EarlyPrefetch(int prefetchDistance, ref TypeBatch typeBatch, } [MethodImpl(MethodImplOptions.AggressiveInlining)] - unsafe static void Prefetch(int prefetchDistance, ref TypeBatch typeBatch, ref Buffer references, ref Buffer states, ref Buffer inertias, int bundleIndex, int exclusiveEndBundleIndex) + unsafe static void Prefetch(int prefetchDistance, ref TypeBatch typeBatch, ref Buffer references, ref Buffer states, ref Buffer inertias, int bundleIndex, int exclusiveEndBundleIndex) { var targetIndex = bundleIndex + prefetchDistance; if (targetIndex < exclusiveEndBundleIndex) @@ -356,7 +356,7 @@ public unsafe override void WarmStart2(ref TypeBatch typeBatch, ref Buffer(); var function = default(TConstraintFunctions); ref var motionStates = ref bodies.ActiveSet.MotionStates; - ref var inertias = ref bodies.Inertias; + ref var inertias = ref bodies.ActiveSet.Inertias; EarlyPrefetch(warmStartPrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, ref inertias, startBundle, exclusiveEndBundle); for (int i = startBundle; i < exclusiveEndBundle; ++i) { @@ -391,7 +391,7 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, ref Buffer(); var function = default(TConstraintFunctions); ref var motionStates = ref bodies.ActiveSet.MotionStates; - ref var inertias = ref bodies.Inertias; + ref var inertias = ref bodies.ActiveSet.Inertias; EarlyPrefetch(solvePrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, ref inertias, startBundle, exclusiveEndBundle); for (int i = startBundle; i < exclusiveEndBundle; ++i) { diff --git a/BepuPhysics/IslandAwakener.cs b/BepuPhysics/IslandAwakener.cs index a66177ead..be9371318 100644 --- a/BepuPhysics/IslandAwakener.cs +++ b/BepuPhysics/IslandAwakener.cs @@ -281,15 +281,16 @@ internal unsafe void ExecutePhaseOneJob(int index) { var sourceIndex = job.SourceStart + i; var targetIndex = job.TargetStart + i; - ref var targetWorldInertia = ref bodies.Inertias[targetIndex]; - ref var sourceLocalInertia = ref sourceSet.LocalInertias[sourceIndex]; - ref var targetLocalInertia = ref targetSet.LocalInertias[targetIndex]; + ref var sourceInertia = ref sourceSet.Inertias[sourceIndex]; + ref var targetInertia = ref targetSet.Inertias[targetIndex]; ref var sourceState = ref sourceSet.MotionStates[sourceIndex]; ref var targetState = ref targetSet.MotionStates[targetIndex]; targetState = sourceState; - targetLocalInertia = sourceLocalInertia; - PoseIntegration.RotateInverseInertia(sourceLocalInertia.InverseInertiaTensor, sourceState.Pose.Orientation, out targetWorldInertia.InverseInertiaTensor); - targetWorldInertia.InverseMass = sourceLocalInertia.InverseMass; + targetInertia.Local = sourceInertia.Local; + //TODO: In principle, if velocity integration is always a part of the solver, then there is no need for this. That's not the case in 2.3.0, but embedded substepping should change that. + //Leaving this here for now so we don't break the other substeppers (and because it's a fairly tiny concern), but something to consider later. + PoseIntegration.RotateInverseInertia(sourceInertia.Local.InverseInertiaTensor, sourceState.Pose.Orientation, out targetInertia.World.InverseInertiaTensor); + targetInertia.World.InverseMass = sourceInertia.Local.InverseMass; } sourceSet.Activity.CopyTo(job.SourceStart, targetSet.Activity, job.TargetStart, job.Count); if (resetActivityStates) diff --git a/BepuPhysics/IslandSleeper.cs b/BepuPhysics/IslandSleeper.cs index a8116ed76..034102906 100644 --- a/BepuPhysics/IslandSleeper.cs +++ b/BepuPhysics/IslandSleeper.cs @@ -359,7 +359,7 @@ unsafe void Gather(int workerIndex) //Note that we are just copying the constraint list reference; we don't have to reallocate it. //Keep this in mind when removing the object from the active set. We don't want to dispose the list since we're still using it. targetSet.Constraints[targetIndex] = sourceSet.Constraints[sourceIndex]; - targetSet.LocalInertias[targetIndex] = sourceSet.LocalInertias[sourceIndex]; + targetSet.Inertias[targetIndex] = sourceSet.Inertias[sourceIndex]; targetSet.MotionStates[targetIndex] = sourceSet.MotionStates[sourceIndex]; if (sourceCollidable.Shape.Exists) diff --git a/BepuPhysics/PoseIntegrator.cs b/BepuPhysics/PoseIntegrator.cs index f731542ae..10d01b830 100644 --- a/BepuPhysics/PoseIntegrator.cs +++ b/BepuPhysics/PoseIntegrator.cs @@ -256,7 +256,7 @@ void IntegrateAngularVelocityConserving(in Quaternion previousOrientation, in Ri //Using localAngularVelocity0 as the first guess for localAngularVelocity1. Matrix3x3.TransformTranspose(angularVelocity, orientationMatrix, out var localAngularVelocity); Symmetric3x3.Invert(localInertia.InverseInertiaTensor, out var localInertiaTensor); - + Symmetric3x3.TransformWithoutOverlap(localAngularVelocity, localInertiaTensor, out var localAngularMomentum); var residual = dt * Vector3.Cross(localAngularMomentum, localAngularVelocity); @@ -307,8 +307,7 @@ void IntegrateAngularVelocity(in RigidPose pose, in BodyInertia localInertia, in unsafe void IntegrateBodiesAndUpdateBoundingBoxes(int startIndex, int endIndex, float dt, ref BoundingBoxBatcher boundingBoxBatcher, int workerIndex) { ref var baseStates = ref bodies.ActiveSet.MotionStates[0]; - ref var baseLocalInertia = ref bodies.ActiveSet.LocalInertias[0]; - ref var baseInertias = ref bodies.Inertias[0]; + ref var baseInertias = ref bodies.ActiveSet.Inertias[0]; ref var baseActivity = ref bodies.ActiveSet.Activity[0]; ref var baseCollidable = ref bodies.ActiveSet.Collidables[0]; for (int i = startIndex; i < endIndex; ++i) @@ -329,15 +328,15 @@ unsafe void IntegrateBodiesAndUpdateBoundingBoxes(int startIndex, int endIndex, //has to get is inertia tensors calculated elsewhere. Either they would need to be computed on addition or something- which is a bit gross, but doable- //or we would need to move this calculation to the beginning of the frame to guarantee that all inertias are up to date. //This would require a scan through all pose memory to support, but if you do it at the same time as AABB update, that's fine- that stage uses the pose too. - ref var localInertia = ref Unsafe.Add(ref baseLocalInertia, i); ref var inertia = ref Unsafe.Add(ref baseInertias, i); - PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, state.Pose.Orientation, out inertia.InverseInertiaTensor); + PoseIntegration.RotateInverseInertia(inertia.Local.InverseInertiaTensor, state.Pose.Orientation, out inertia.World.InverseInertiaTensor); //While it's a bit goofy just to copy over the inverse mass every frame even if it doesn't change, - //it's virtually always gathered together with the inertia tensor and it really isn't worth a whole extra external system to copy inverse masses only on demand. - inertia.InverseMass = localInertia.InverseMass; + //it's virtually always gathered together with the inertia tensor and having a duplicate means we can sometimes avoid loading a lane + //(i.e. loading only the last 32 bytes of the cache line into a Vector256). + inertia.World.InverseMass = inertia.Local.InverseMass; - IntegrateAngularVelocity(previousOrientation, state.Pose, localInertia, inertia, ref state.Velocity.Angular, dt); - Callbacks.IntegrateVelocity(i, state.Pose, localInertia, workerIndex, ref state.Velocity); + IntegrateAngularVelocity(previousOrientation, state.Pose, inertia.Local, inertia.World, ref state.Velocity.Angular, dt); + Callbacks.IntegrateVelocity(i, state.Pose, inertia.Local, workerIndex, ref state.Velocity); //Bounding boxes are accumulated in a scalar fashion, but the actual bounding box calculations are deferred until a sufficient number of collidables are accumulated to make //executing a bundle worthwhile. This does two things: @@ -361,7 +360,7 @@ unsafe void IntegrateBodiesAndUpdateBoundingBoxes(int startIndex, int endIndex, unsafe void PredictBoundingBoxes(int startIndex, int endIndex, float dt, ref BoundingBoxBatcher boundingBoxBatcher, int workerIndex) { ref var baseStates = ref bodies.ActiveSet.MotionStates[0]; - ref var baseLocalInertia = ref bodies.ActiveSet.LocalInertias[0]; + ref var baseInertia = ref bodies.ActiveSet.Inertias[0]; ref var baseActivity = ref bodies.ActiveSet.Activity[0]; ref var baseCollidable = ref bodies.ActiveSet.Collidables[0]; for (int i = startIndex; i < endIndex; ++i) @@ -376,7 +375,7 @@ unsafe void PredictBoundingBoxes(int startIndex, int endIndex, float dt, ref Bou //Bounding box prediction does not need to update inertia tensors. var integratedVelocity = state.Velocity; - Callbacks.IntegrateVelocity(i, state.Pose, Unsafe.Add(ref baseLocalInertia, i), workerIndex, ref integratedVelocity); + Callbacks.IntegrateVelocity(i, state.Pose, Unsafe.Add(ref baseInertia, i).Local, workerIndex, ref integratedVelocity); //Note that we do not include fancier angular integration for the bounding box prediction- it's not very important. boundingBoxBatcher.Add(i, state.Pose, integratedVelocity, Unsafe.Add(ref baseCollidable, i)); @@ -386,8 +385,7 @@ unsafe void PredictBoundingBoxes(int startIndex, int endIndex, float dt, ref Bou unsafe void IntegrateVelocitiesBoundsAndInertias(int startIndex, int endIndex, float dt, ref BoundingBoxBatcher boundingBoxBatcher, int workerIndex) { ref var baseStates = ref bodies.ActiveSet.MotionStates[0]; - ref var baseLocalInertia = ref bodies.ActiveSet.LocalInertias[0]; - ref var baseInertias = ref bodies.Inertias[0]; + ref var baseInertia = ref bodies.ActiveSet.Inertias[0]; ref var baseActivity = ref bodies.ActiveSet.Activity[0]; ref var baseCollidable = ref bodies.ActiveSet.Collidables[0]; for (int i = startIndex; i < endIndex; ++i) @@ -400,13 +398,12 @@ unsafe void IntegrateVelocitiesBoundsAndInertias(int startIndex, int endIndex, f UpdateSleepCandidacy(ref state.Velocity, ref Unsafe.Add(ref baseActivity, i)); - ref var localInertia = ref Unsafe.Add(ref baseLocalInertia, i); - ref var inertia = ref Unsafe.Add(ref baseInertias, i); - PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, state.Pose.Orientation, out inertia.InverseInertiaTensor); - inertia.InverseMass = localInertia.InverseMass; + ref var inertia = ref Unsafe.Add(ref baseInertia, i); + PoseIntegration.RotateInverseInertia(inertia.Local.InverseInertiaTensor, state.Pose.Orientation, out inertia.World.InverseInertiaTensor); + inertia.World.InverseMass = inertia.Local.InverseMass; - IntegrateAngularVelocity(state.Pose, localInertia, inertia, ref state.Velocity.Angular, dt); - Callbacks.IntegrateVelocity(i, state.Pose, localInertia, workerIndex, ref state.Velocity); + IntegrateAngularVelocity(state.Pose, inertia.Local, inertia.World, ref state.Velocity.Angular, dt); + Callbacks.IntegrateVelocity(i, state.Pose, inertia.Local, workerIndex, ref state.Velocity); boundingBoxBatcher.Add(i, state.Pose, state.Velocity, Unsafe.Add(ref baseCollidable, i)); } @@ -416,8 +413,7 @@ unsafe void IntegrateVelocitiesBoundsAndInertias(int startIndex, int endIndex, f unsafe void IntegrateVelocities(int startIndex, int endIndex, float dt, int workerIndex) { ref var baseStates = ref bodies.ActiveSet.MotionStates[0]; - ref var baseLocalInertia = ref bodies.ActiveSet.LocalInertias[0]; - ref var baseInertias = ref bodies.Inertias[0]; + ref var baseInertia = ref bodies.ActiveSet.Inertias[0]; for (int i = startIndex; i < endIndex; ++i) { ref var state = ref Unsafe.Add(ref baseStates, i); @@ -426,13 +422,12 @@ unsafe void IntegrateVelocities(int startIndex, int endIndex, float dt, int work state.Velocity.Linear.Validate(); state.Velocity.Angular.Validate(); - ref var localInertia = ref Unsafe.Add(ref baseLocalInertia, i); - ref var inertia = ref Unsafe.Add(ref baseInertias, i); - PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, state.Pose.Orientation, out inertia.InverseInertiaTensor); - inertia.InverseMass = localInertia.InverseMass; + ref var inertia = ref Unsafe.Add(ref baseInertia, i); + PoseIntegration.RotateInverseInertia(inertia.Local.InverseInertiaTensor, state.Pose.Orientation, out inertia.World.InverseInertiaTensor); + inertia.World.InverseMass = inertia.Local.InverseMass; - IntegrateAngularVelocity(state.Pose, localInertia, inertia, ref state.Velocity.Angular, dt); - Callbacks.IntegrateVelocity(i, state.Pose, localInertia, workerIndex, ref state.Velocity); + IntegrateAngularVelocity(state.Pose, inertia.Local, inertia.World, ref state.Velocity.Angular, dt); + Callbacks.IntegrateVelocity(i, state.Pose, inertia.Local, workerIndex, ref state.Velocity); } } @@ -546,9 +541,6 @@ void PrepareForMultithreadedExecution(float dt, int workerCount) public void IntegrateBodiesAndUpdateBoundingBoxes(float dt, BufferPool pool, IThreadDispatcher threadDispatcher = null) { - //Users of this codepath are expecting all integration related work to be done at once, so we need to update inertias. - bodies.EnsureInertiasCapacity(Math.Max(1, bodies.ActiveSet.Count)); - var workerCount = threadDispatcher == null ? 1 : threadDispatcher.ThreadCount; Callbacks.PrepareForIntegration(dt); @@ -600,8 +592,6 @@ public void PredictBoundingBoxes(float dt, BufferPool pool, IThreadDispatcher th public void IntegrateVelocitiesBoundsAndInertias(float dt, BufferPool pool, IThreadDispatcher threadDispatcher = null) { - bodies.EnsureInertiasCapacity(Math.Max(1, bodies.ActiveSet.Count)); - var workerCount = threadDispatcher == null ? 1 : threadDispatcher.ThreadCount; Callbacks.PrepareForIntegration(dt); @@ -622,9 +612,6 @@ public void IntegrateVelocitiesBoundsAndInertias(float dt, BufferPool pool, IThr public void IntegrateVelocitiesAndUpdateInertias(float dt, BufferPool pool, IThreadDispatcher threadDispatcher = null) { - //Isolated velocity integration is used by substeppers that also expect an inertia update. - bodies.EnsureInertiasCapacity(Math.Max(1, bodies.ActiveSet.Count)); - var workerCount = threadDispatcher == null ? 1 : threadDispatcher.ThreadCount; Callbacks.PrepareForIntegration(dt); diff --git a/DemoRenderer/ShapeDrawing/ShapesExtractor.cs b/DemoRenderer/ShapeDrawing/ShapesExtractor.cs index 1e0addeab..76b98bea0 100644 --- a/DemoRenderer/ShapeDrawing/ShapesExtractor.cs +++ b/DemoRenderer/ShapeDrawing/ShapesExtractor.cs @@ -236,7 +236,7 @@ void AddBodyShape(Shapes shapes, Bodies bodies, int setIndex, int indexInSet) //3) Activity state //The handle is hashed to get variation. ref var activity = ref set.Activity[indexInSet]; - ref var inertia = ref set.LocalInertias[indexInSet]; + ref var inertia = ref set.Inertias[indexInSet].Local; Vector3 color; Helpers.UnpackColor((uint)HashHelper.Rehash(handle.Value), out Vector3 colorVariation); if (Bodies.IsKinematic(inertia)) diff --git a/Demos/Demos/Characters/CharacterControllers.cs b/Demos/Demos/Characters/CharacterControllers.cs index 0c4a03a65..e3a32e97f 100644 --- a/Demos/Demos/Characters/CharacterControllers.cs +++ b/Demos/Demos/Characters/CharacterControllers.cs @@ -831,7 +831,7 @@ void AnalyzeContacts(float dt, IThreadDispatcher threadDispatcher) activeSet.MotionStates[jump.CharacterBodyIndex].Velocity.Linear += jump.CharacterVelocityChange; if (jump.SupportBodyIndex >= 0) { - BodyReference.ApplyImpulse(Simulation.Bodies.ActiveSet, jump.SupportBodyIndex, jump.CharacterVelocityChange / -activeSet.LocalInertias[jump.CharacterBodyIndex].InverseMass, jump.SupportImpulseOffset); + BodyReference.ApplyImpulse(Simulation.Bodies.ActiveSet, jump.SupportBodyIndex, jump.CharacterVelocityChange / -activeSet.Inertias[jump.CharacterBodyIndex].Local.InverseMass, jump.SupportImpulseOffset); } } workerCache.Dispose(pool); From 6c0e711816190d817264d3dc63d0106447dca62c Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 11 Jul 2021 13:06:55 -0500 Subject: [PATCH 094/947] Does not build. Pushing through solver-embedded pose integration. --- .../Constraints/TwoBodyTypeProcessor.cs | 65 ++++++++++++++++- BepuPhysics/Constraints/TypeProcessor.cs | 3 +- BepuPhysics/PoseIntegrator.cs | 71 +++++++++++++++++++ BepuPhysics/Solver.cs | 1 - BepuPhysics/Solver_Solve.cs | 46 ++++++------ BepuPhysics/Solver_SubsteppingSolve2.cs | 37 +++++++--- BepuUtilities/Matrix3x3Wide.cs | 29 ++++++++ BepuUtilities/Symmetric3x3Wide.cs | 33 +++++++++ 8 files changed, 249 insertions(+), 36 deletions(-) diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 1c3e11703..832b2733a 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -349,7 +349,66 @@ unsafe static void Prefetch(int prefetchDistance, ref TypeBatch typeBatch, ref B } } - public unsafe override void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + bool BundleShouldIntegrate(int bundleIndex, in IndexSet integrationFlags, out int integrationMask) + { + Debug.Assert(Vector.Count <= 32, "Wait, what? The integration mask isn't big enough to handle a vector this big."); + var constraintStartIndex = bundleIndex * Vector.Count; + var flagBundleIndex = constraintStartIndex >> 6; + var flagInnerIndex = constraintStartIndex - flagBundleIndex; + integrationMask = ((int)(integrationFlags.Flags[flagBundleIndex] >> flagInnerIndex)) & BundleIndexing.VectorMask; + return integrationMask > 0; + } + + unsafe void IntegratePoseAndVelocity( + ref TIntegratorCallbacks integratorCallbacks, ref Vector bodyIndices, ref BodyInertiaWide localInertia, float dt, Vector integrationMask, + ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocityWide velocity, + int workerIndex, + out BodyInertiaWide inertia) + where TIntegratorCallbacks : struct, IPoseIntegratorCallbacks + { + //Note that we integrate pose, then velocity. + //We only use this function where we can guarantee that the external-to-timestep view of velocities and poses looks like the frame starts on a velocity integration and ends on a pose integration. + //This ensures that velocities set externally are still solved before being integrated. + //So, the solver runs velocity integration alone on the first substep. All later substeps then run pose + velocity, and then after the last substep, a final pose integration. + //This is equivalent in ordering to running each substep as velocity, warmstart, solve, pose integration, but just shifting the execution context. + position += velocity.Linear * new Vector(dt); + + if (integratorCallbacks.AngularIntegrationMode == AngularIntegrationMode.ConserveMomentum) + { + var oldOrientation = orientation; + PoseIntegration.Integrate(orientation, velocity.Angular, new Vector(dt * 0.5f), out orientation); + PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, orientation, out inertia.InverseInertiaTensor); + inertia.InverseMass = localInertia.InverseMass; + PoseIntegration.IntegrateAngularVelocityConserveMomentum(oldOrientation, localInertia, inertia, ref velocity.Angular); + } + else if (integratorCallbacks.AngularIntegrationMode == AngularIntegrationMode.ConserveMomentumWithGyroscopicTorque) + { + PoseIntegration.Integrate(orientation, velocity.Angular, new Vector(dt * 0.5f), out orientation); + PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, orientation, out inertia.InverseInertiaTensor); + inertia.InverseMass = localInertia.InverseMass; + PoseIntegration.IntegrateAngularVelocityConserveMomentumWithGyroscopicTorque(orientation, localInertia, inertia, ref velocity.Angular, dt); + } + else + { + PoseIntegration.Integrate(orientation, velocity.Angular, new Vector(dt * 0.5f), out orientation); + PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, orientation, out inertia.InverseInertiaTensor); + inertia.InverseMass = localInertia.InverseMass; + } + integratorCallbacks.IntegrateVelocity(new ReadOnlySpan(Unsafe.AsPointer(ref bodyIndices), Vector.Count), position, orientation, localInertia, integrationMask, workerIndex, velocity, + out Vector3Wide linearChange, out Vector3Wide angularChange); + //It would be annoying to make the user handle masking velocity writes to inactive lanes, so we handle it internally. + velocity.Linear.X = Vector.ConditionalSelect(integrationMask, velocity.Linear.X + linearChange.X, velocity.Linear.X); + velocity.Linear.Y = Vector.ConditionalSelect(integrationMask, velocity.Linear.Y + linearChange.Y, velocity.Linear.Y); + velocity.Linear.Z = Vector.ConditionalSelect(integrationMask, velocity.Linear.Z + linearChange.Z, velocity.Linear.Z); + velocity.Angular.X = Vector.ConditionalSelect(integrationMask, velocity.Angular.X + angularChange.X, velocity.Linear.X); + velocity.Angular.Y = Vector.ConditionalSelect(integrationMask, velocity.Angular.Y + angularChange.Y, velocity.Linear.Y); + velocity.Angular.Z = Vector.ConditionalSelect(integrationMask, velocity.Angular.Z + angularChange.Z, velocity.Linear.Z); + + + } + + public unsafe override void WarmStart2( + ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { var prestepBundles = typeBatch.PrestepData.As(); var bodyReferencesBundles = typeBatch.BodyReferences.As(); @@ -366,6 +425,10 @@ public unsafe override void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); + public abstract void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TCallbacks poseIntegratorCallbacks, + float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) where TCallbacks : struct, IPoseIntegratorCallbacks; public abstract void SolveStep2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); diff --git a/BepuPhysics/PoseIntegrator.cs b/BepuPhysics/PoseIntegrator.cs index 10d01b830..7a5f6c2a0 100644 --- a/BepuPhysics/PoseIntegrator.cs +++ b/BepuPhysics/PoseIntegrator.cs @@ -143,6 +143,77 @@ public static void Integrate(in QuaternionWide start, in Vector3Wide angularVelo integrated.W = Vector.ConditionalSelect(speedValid, integrated.W, start.W); } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void RotateInverseInertia(in Symmetric3x3Wide localInverseInertiaTensor, in QuaternionWide orientation, out Symmetric3x3Wide rotatedInverseInertiaTensor) + { + Matrix3x3Wide.CreateFromQuaternion(orientation, out var orientationMatrix); + //I^-1 = RT * Ilocal^-1 * R + //NOTE: If you were willing to confuse users a little bit, the local inertia could be required to be diagonal. + //This would be totally fine for all the primitive types which happen to have diagonal inertias, but for more complex shapes (convex hulls, meshes), + //there would need to be a reorientation step. That could be confusing, and it's probably not worth it. + Symmetric3x3Wide.RotationSandwich(orientationMatrix, localInverseInertiaTensor, out rotatedInverseInertiaTensor); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void IntegrateAngularVelocityConserveMomentum(in QuaternionWide previousOrientation, in BodyInertiaWide localInertia, in BodyInertiaWide inertia, ref Vector3Wide angularVelocity) + { + //Note that this effectively recomputes the previous frame's inertia. There may not have been a previous inertia stored in the inertias buffer. + //This just avoids the need for quite a bit of complexity around keeping the world inertias buffer updated with adds/removes/moves and other state changes that we can't easily track. + //Also, even if it were cached, the memory bandwidth requirements of loading another inertia tensor would hurt multithreaded scaling enough to eliminate any performance advantage. + Matrix3x3Wide.CreateFromQuaternion(previousOrientation, out var previousOrientationMatrix); + Matrix3x3Wide.TransformByTransposedWithoutOverlap(angularVelocity, previousOrientationMatrix, out var localPreviousAngularVelocity); + Symmetric3x3Wide.Invert(localInertia.InverseInertiaTensor, out var localInertiaTensor); + Symmetric3x3Wide.TransformWithoutOverlap(localPreviousAngularVelocity, localInertiaTensor, out var localAngularMomentum); + Matrix3x3Wide.Transform(localAngularMomentum, previousOrientationMatrix, out var angularMomentum); + Symmetric3x3Wide.TransformWithoutOverlap(angularMomentum, inertia.InverseInertiaTensor, out angularVelocity); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void IntegrateAngularVelocityConserveMomentumWithGyroscopicTorque( + in QuaternionWide orientation, in BodyInertiaWide localInertia, in BodyInertiaWide inertia, ref Vector3Wide angularVelocity, float dt) + { + //Integrating the gyroscopic force explicitly can result in some instability, so we'll use an approximate implicit approach. + //angularVelocity1 * inertia1 = angularVelocity0 * inertia1 + dt * ((angularVelocity1 * inertia1) x angularVelocity1) + //Note that this includes a reference to inertia1 which doesn't exist yet. We do, however, have the local inertia, so we'll + //transform all velocities into local space using the current orientation for the calculation. + //So: + //localAngularVelocity1 * localInertia = localAngularVelocity0 * localInertia - dt * (localAngularVelocity1 x (localAngularVelocity1 * localInertia)) + //localAngularVelocity1 * localInertia - localAngularVelocity0 * localInertia + dt * (localAngularVelocity1 x (localAngularVelocity1 * localInertia)) = 0 + //f(localAngularVelocity1) = (localAngularVelocity1 - localAngularVelocity0) * localInertia + dt * (localAngularVelocity1 x (localAngularVelocity1 * localInertia)) + //Not trivial to solve for localAngularVelocity1 so we'll do so numerically with a newton iteration. + //(For readers familiar with Bullet's BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY, this is basically identical.) + + //We'll start with an initial guess of localAngularVelocity1 = localAngularVelocity0, and update with a newton step of f(localAngularVelocity1) * invert(df/dw1(localAngularVelocity1)) + //df/dw1x(localAngularVelocity1) * localInertia + dt * (df/dw1x(localAngularVelocity1) x (localAngularVelocity1 * localInertia) + localAngularVelocity1 x df/dw1x(localAngularVelocity1 * localInertia)) + //df/dw1x(localAngularVelocity1) = (1,0,0) + //df/dw1x(f(localAngularVelocity1)) = (1, 0, 0) * localInertia + dt * ((1, 0, 0) x (localAngularVelocity1 * localInertia) + localAngularVelocity1 x ((1, 0, 0) * localInertia)) + //df/dw1x(f(localAngularVelocity1)) = (0, 1, 0) * localInertia + dt * ((0, 1, 0) x (localAngularVelocity1 * localInertia) + localAngularVelocity1 x ((0, 1, 0) * localInertia)) + //df/dw1x(f(localAngularVelocity1)) = (0, 0, 1) * localInertia + dt * ((0, 0, 1) x (localAngularVelocity1 * localInertia) + localAngularVelocity1 x ((0, 0, 1) * localInertia)) + //This can be expressed a bit more concisely, given a x b = skew(a) * b, where skew(a) is a skew symmetric matrix representing a cross product: + //df/dw1(f(localAngularVelocity1)) = localInertia + dt * (skew(localAngularVelocity1) * localInertia - skew(localAngularVelocity1 * localInertia)) + Matrix3x3Wide.CreateFromQuaternion(orientation, out var orientationMatrix); + //Using localAngularVelocity0 as the first guess for localAngularVelocity1. + Matrix3x3Wide.TransformByTransposedWithoutOverlap(angularVelocity, orientationMatrix, out var localAngularVelocity); + Symmetric3x3Wide.Invert(localInertia.InverseInertiaTensor, out var localInertiaTensor); + + Symmetric3x3Wide.TransformWithoutOverlap(localAngularVelocity, localInertiaTensor, out var localAngularMomentum); + var dtWide = new Vector(dt); + var residual = dtWide * Vector3Wide.Cross(localAngularMomentum, localAngularVelocity); + + Matrix3x3Wide.CreateCrossProduct(localAngularMomentum, out var skewMomentum); + Matrix3x3Wide.CreateCrossProduct(localAngularVelocity, out var skewVelocity); + var transformedSkewVelocity = skewVelocity * localInertiaTensor; + Matrix3x3Wide.Subtract(transformedSkewVelocity, skewMomentum, out var changeOverDt); + Matrix3x3Wide.Scale(changeOverDt, dtWide, out var change); + var jacobian = localInertiaTensor + change; + + Matrix3x3Wide.Invert(jacobian, out var inverseJacobian); + Matrix3x3Wide.Transform(residual, inverseJacobian, out var newtonStep); + localAngularVelocity -= newtonStep; + + Matrix3x3Wide.Transform(localAngularVelocity, orientationMatrix, out angularVelocity); + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static unsafe void Integrate(in RigidPose pose, in BodyVelocity velocity, float dt, out RigidPose integratedPose) { diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index adace0df0..fbf8f7fc9 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -199,7 +199,6 @@ public Solver(Bodies bodies, BufferPool pool, int iterationCount, int fallbackBa solveWorker = SolveWorker; solve2Worker = Solve2Worker; solveStepWorker = SolveStepWorker; - solveStep2Worker = SolveStep2Worker; incrementalContactUpdateWorker = IncrementalContactUpdateWorker; } diff --git a/BepuPhysics/Solver_Solve.cs b/BepuPhysics/Solver_Solve.cs index 9a3729e3c..78cd1f93c 100644 --- a/BepuPhysics/Solver_Solve.cs +++ b/BepuPhysics/Solver_Solve.cs @@ -74,7 +74,7 @@ public partial class Solver //Without sticky scheduling, memory bandwidth use could skyrocket during iterations as the L3 gets missed over and over. - struct WorkBlock + protected struct WorkBlock { public int BatchIndex; public int TypeBatchIndex; @@ -88,19 +88,19 @@ struct WorkBlock public int End; } - struct FallbackScatterWorkBlock + protected struct FallbackScatterWorkBlock { public int Start; public int End; } - interface ITypeBatchSolveFilter + protected interface ITypeBatchSolveFilter { bool AllowFallback { get; } bool AllowType(int typeId); } - struct MainSolveFilter : ITypeBatchSolveFilter + protected struct MainSolveFilter : ITypeBatchSolveFilter { public bool AllowFallback { @@ -118,7 +118,7 @@ public bool AllowType(int typeId) } } - private unsafe void BuildWorkBlocks(BufferPool pool, int minimumBlockSizeInBundles, int maximumBlockSizeInBundles, int targetBlocksPerBatch, ref TTypeBatchFilter typeBatchFilter) where TTypeBatchFilter : ITypeBatchSolveFilter + protected unsafe void BuildWorkBlocks(BufferPool pool, int minimumBlockSizeInBundles, int maximumBlockSizeInBundles, int targetBlocksPerBatch, ref TTypeBatchFilter typeBatchFilter) where TTypeBatchFilter : ITypeBatchSolveFilter { ref var activeSet = ref ActiveSet; context.ConstraintBlocks.Blocks = new QuickList(targetBlocksPerBatch * activeSet.Batches.Count, pool); @@ -182,7 +182,7 @@ private unsafe void BuildWorkBlocks(BufferPool pool, int minim } - struct WorkerBounds + protected struct WorkerBounds { /// /// Inclusive start of blocks known to be claimed by any worker. @@ -216,7 +216,7 @@ public static void MergeIfTouching(ref WorkerBounds current, ref WorkerBounds ot } } - struct WorkBlocks where T : unmanaged + protected struct WorkBlocks where T : unmanaged { public QuickList Blocks; public Buffer Claims; @@ -234,7 +234,7 @@ public void Dispose(BufferPool pool) } //Just bundling these up to avoid polluting the this. intellisense. - struct MultithreadingParameters + protected struct MultithreadingParameters { public float Dt; public WorkBlocks ConstraintBlocks; @@ -248,7 +248,7 @@ struct MultithreadingParameters public Buffer WorkerBoundsB; } - MultithreadingParameters context; + protected MultithreadingParameters context; [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -278,7 +278,7 @@ int TraverseForwardUntilBlocked(ref TStageFunction stage highestLocallyClaimedIndex = blockIndex; bounds.Max = blockIndex + 1; //Exclusive bound. Debug.Assert(blockIndex < batchEnd); - stageFunction.Execute(this, blockIndex); + stageFunction.Execute(this, blockIndex, workerIndex); //Increment or exit. if (++blockIndex == batchEnd) break; @@ -310,7 +310,7 @@ int TraverseBackwardUntilBlocked(ref TStageFunction stag lowestLocallyClaimedIndex = blockIndex; bounds.Min = blockIndex; Debug.Assert(blockIndex >= batchStart); - stageFunction.Execute(this, blockIndex); + stageFunction.Execute(this, blockIndex, workerIndex); //Decrement or exit. if (blockIndex == batchStart) break; @@ -326,9 +326,9 @@ int TraverseBackwardUntilBlocked(ref TStageFunction stag MergeWorkerBounds(ref bounds, ref allWorkerBounds, workerIndex); return lowestLocallyClaimedIndex; } - interface IStageFunction + protected interface IStageFunction { - void Execute(Solver solver, int blockIndex); + void Execute(Solver solver, int blockIndex, int workerIndex); } struct PrestepStageFunction : IStageFunction { @@ -336,7 +336,7 @@ struct PrestepStageFunction : IStageFunction public float InverseDt; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Execute(Solver solver, int blockIndex) + public void Execute(Solver solver, int blockIndex, int workerIndex) { ref var block = ref solver.context.ConstraintBlocks.Blocks[blockIndex]; ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; @@ -352,7 +352,7 @@ public void Execute(Solver solver, int blockIndex) struct WarmStartStageFunction : IStageFunction { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Execute(Solver solver, int blockIndex) + public void Execute(Solver solver, int blockIndex, int workerIndex) { ref var block = ref solver.context.ConstraintBlocks.Blocks[blockIndex]; ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; @@ -364,7 +364,7 @@ public void Execute(Solver solver, int blockIndex) struct SolveStageFunction : IStageFunction { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Execute(Solver solver, int blockIndex) + public void Execute(Solver solver, int blockIndex, int workerIndex) { ref var block = ref solver.context.ConstraintBlocks.Blocks[blockIndex]; ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; @@ -376,7 +376,7 @@ public void Execute(Solver solver, int blockIndex) struct WarmStartFallbackStageFunction : IStageFunction { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Execute(Solver solver, int blockIndex) + public void Execute(Solver solver, int blockIndex, int workerIndex) { ref var block = ref solver.context.ConstraintBlocks.Blocks[blockIndex]; ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; @@ -388,7 +388,7 @@ public void Execute(Solver solver, int blockIndex) struct SolveFallbackStageFunction : IStageFunction { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Execute(Solver solver, int blockIndex) + public void Execute(Solver solver, int blockIndex, int workerIndex) { ref var block = ref solver.context.ConstraintBlocks.Blocks[blockIndex]; ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; @@ -400,7 +400,7 @@ public void Execute(Solver solver, int blockIndex) struct FallbackScatterStageFunction : IStageFunction { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Execute(Solver solver, int blockIndex) + public void Execute(Solver solver, int blockIndex, int workerIndex) { ref var block = ref solver.context.FallbackBlocks.Blocks[blockIndex]; solver.ActiveSet.Fallback.ScatterVelocities(solver.bodies, solver, ref solver.context.FallbackResults, block.Start, block.End); @@ -455,7 +455,7 @@ public void SpinOnce() } - void InterstageSync(ref int syncStageIndex) + protected void InterstageSync(ref int syncStageIndex) { //No more work is available to claim, but not every thread is necessarily done with the work they claimed. So we need a dedicated sync- upon completing its local work, //a worker increments the 'workerCompleted' counter, and the spins on that counter reaching workerCount * stageIndex. @@ -471,7 +471,7 @@ void InterstageSync(ref int syncStageIndex) } } - private void ExecuteStage(ref TStageFunction stageFunction, ref WorkBlocks blocks, + protected void ExecuteStage(ref TStageFunction stageFunction, ref WorkBlocks blocks, ref Buffer allWorkerBounds, ref Buffer previousWorkerBounds, int workerIndex, int batchStart, int batchEnd, ref int workerStart, ref int syncStage, int claimedState, int unclaimedState) @@ -572,7 +572,7 @@ private void ExecuteStage(ref TStageFunction stageFuncti } - static int GetUniformlyDistributedStart(int workerIndex, int blockCount, int workerCount, int offset) + protected static int GetUniformlyDistributedStart(int workerIndex, int blockCount, int workerCount, int offset) { if (blockCount <= workerCount) { @@ -713,7 +713,7 @@ void ValidateWorkBlocks(ref TTypeBatchSolveFilter filter) } - void ExecuteMultithreaded(float dt, IThreadDispatcher threadDispatcher, Action workDelegate) where TTypeBatchSolveFilter : struct, ITypeBatchSolveFilter + protected void ExecuteMultithreaded(float dt, IThreadDispatcher threadDispatcher, Action workDelegate) where TTypeBatchSolveFilter : struct, ITypeBatchSolveFilter { var filter = default(TTypeBatchSolveFilter); var workerCount = context.WorkerCount = threadDispatcher.ThreadCount; diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index 1b613dc9f..480825cd9 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -12,8 +12,20 @@ namespace BepuPhysics { - public partial class Solver + public class Solver : Solver where TIntegrationCallbacks : struct, IPoseIntegratorCallbacks { + public Solver(Bodies bodies, BufferPool pool, int iterationCount, int fallbackBatchThreshold, + int initialCapacity, + int initialIslandCapacity, + int minimumCapacityPerTypeBatch, PoseIntegrator poseIntegrator) + : base(bodies, pool, iterationCount, fallbackBatchThreshold, initialCapacity, initialIslandCapacity, minimumCapacityPerTypeBatch) + { + PoseIntegrator = poseIntegrator; + solveStep2Worker = SolveStep2Worker; + } + + public PoseIntegrator PoseIntegrator { get; private set; } + //Split the solve process into a warmstart and solve, where warmstart doesn't try to store out anything. It just computes jacobians and modifies velocities according to the accumulated impulse. //The solve step then *recomputes* jacobians from prestep data and pose information. //Why? Memory bandwidth. Redoing the calculation is cheaper than storing it out. @@ -21,14 +33,15 @@ struct WarmStartStep2StageFunction : IStageFunction { public float Dt; public float InverseDt; + Solver solver; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Execute(Solver solver, int blockIndex) + public void Execute(Solver solver, int blockIndex, int workerIndex ) { - ref var block = ref solver.context.ConstraintBlocks.Blocks[blockIndex]; + ref var block = ref this.solver.context.ConstraintBlocks.Blocks[blockIndex]; ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; - typeProcessor.WarmStart2(ref typeBatch, ref solver.integrationFlags[block.BatchIndex][block.TypeBatchIndex], solver.bodies, Dt, InverseDt, block.StartBundle, block.End); + typeProcessor.WarmStart2(ref typeBatch, ref this.solver.integrationFlags[block.BatchIndex][block.TypeBatchIndex], this.solver.bodies, ref this.solver.PoseIntegrator.Callbacks, Dt, InverseDt, block.StartBundle, block.End, workerIndex); } } //no fallback warmstart; the last constraint batch is always handled by the solve instead, and if the fallback batch exists, it's guaranteed to be the last batch. @@ -37,14 +50,15 @@ struct SolveStep2StageFunction : IStageFunction { public float Dt; public float InverseDt; + Solver solver; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Execute(Solver solver, int blockIndex) + public void Execute(Solver solver, int blockIndex, int workerIndex) { - ref var block = ref solver.context.ConstraintBlocks.Blocks[blockIndex]; + ref var block = ref this.solver.context.ConstraintBlocks.Blocks[blockIndex]; ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; - typeProcessor.SolveStep2(ref typeBatch, ref solver.integrationFlags[block.BatchIndex][block.TypeBatchIndex], solver.bodies, Dt, InverseDt, block.StartBundle, block.End); + typeProcessor.SolveStep2(ref typeBatch, ref this.solver.integrationFlags[block.BatchIndex][block.TypeBatchIndex], solver.bodies, Dt, InverseDt, block.StartBundle, block.End); } } @@ -64,8 +78,6 @@ public void Execute(Solver solver, int blockIndex) //} Action solveStep2Worker; - - void SolveStep2Worker(int workerIndex) { int fallbackStart = GetUniformlyDistributedStart(workerIndex, context.FallbackBlocks.Blocks.Count, context.WorkerCount, 0); @@ -216,7 +228,7 @@ public void SolveStep2(float dt, IThreadDispatcher threadDispatcher = null) for (int j = 0; j < batch.TypeBatches.Count; ++j) { ref var typeBatch = ref batch.TypeBatches[j]; - TypeProcessors[typeBatch.TypeId].WarmStart2(ref typeBatch, ref integrationFlagsForBatch[j], bodies, dt, inverseDt, 0, typeBatch.BundleCount); + TypeProcessors[typeBatch.TypeId].WarmStart2(ref typeBatch, ref integrationFlagsForBatch[j], bodies, ref PoseIntegrator.Callbacks, dt, inverseDt, 0, typeBatch.BundleCount, 0); } } @@ -236,6 +248,11 @@ public void SolveStep2(float dt, IThreadDispatcher threadDispatcher = null) ExecuteMultithreaded(dt, threadDispatcher, solveStep2Worker); } } + } + + public partial class Solver + { + } } diff --git a/BepuUtilities/Matrix3x3Wide.cs b/BepuUtilities/Matrix3x3Wide.cs index 6b20578ec..e197fdc5c 100644 --- a/BepuUtilities/Matrix3x3Wide.cs +++ b/BepuUtilities/Matrix3x3Wide.cs @@ -264,6 +264,35 @@ public static void CreateFromQuaternion(in QuaternionWide quaternion, out Matrix result.Z.Z = Vector.One - XX - YY; } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Subtract(in Matrix3x3Wide a, in Matrix3x3Wide b, out Matrix3x3Wide result) + { + result.X.X = a.X.X - b.X.X; + result.X.Y = a.X.Y - b.X.Y; + result.X.Z = a.X.Z - b.X.Z; + result.Y.X = a.Y.X - b.Y.X; + result.Y.Y = a.Y.Y - b.Y.Y; + result.Y.Z = a.Y.Z - b.Y.Z; + result.Z.X = a.Z.X - b.Z.X; + result.Z.Y = a.Z.Y - b.Z.Y; + result.Z.Z = a.Z.Z - b.Z.Z; + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix3x3Wide operator -(in Matrix3x3Wide a, in Matrix3x3Wide b) + { + Matrix3x3Wide result; + result.X.X = a.X.X - b.X.X; + result.X.Y = a.X.Y - b.X.Y; + result.X.Z = a.X.Z - b.X.Z; + result.Y.X = a.Y.X - b.Y.X; + result.Y.Y = a.Y.Y - b.Y.Y; + result.Y.Z = a.Y.Z - b.Y.Z; + result.Z.X = a.Z.X - b.Z.X; + result.Z.Y = a.Z.Y - b.Z.Y; + result.Z.Z = a.Z.Z - b.Z.Z; + return result; + } + /// /// Pulls one lane out of the wide representation. /// diff --git a/BepuUtilities/Symmetric3x3Wide.cs b/BepuUtilities/Symmetric3x3Wide.cs index 556a97ac4..c1caaabab 100644 --- a/BepuUtilities/Symmetric3x3Wide.cs +++ b/BepuUtilities/Symmetric3x3Wide.cs @@ -539,6 +539,39 @@ public static void TransformWithoutOverlap(in Vector3Wide v, in Symmetric3x3Wide return result; } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix3x3Wide operator +(in Symmetric3x3Wide a, in Matrix3x3Wide b) + { + Matrix3x3Wide result; + result.X.X = a.XX + b.X.X; + result.X.Y = a.YX + b.X.Y; + result.X.Z = a.ZX + b.X.Z; + result.Y.X = a.YX + b.Y.X; + result.Y.Y = a.YY + b.Y.Y; + result.Y.Z = a.ZY + b.Y.Z; + result.Z.X = a.ZX + b.Z.X; + result.Z.Y = a.ZY + b.Z.Y; + result.Z.Z = a.ZZ + b.Z.Z; + return result; + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix3x3Wide operator +(in Matrix3x3Wide a, in Symmetric3x3Wide b) + { + Matrix3x3Wide result; + result.X.X = a.X.X + b.XX; + result.X.Y = a.X.Y + b.YX; + result.X.Z = a.X.Z + b.ZX; + result.Y.X = a.Y.X + b.YX; + result.Y.Y = a.Y.Y + b.YY; + result.Y.Z = a.Y.Z + b.ZY; + result.Z.X = a.Z.X + b.ZX; + result.Z.Y = a.Z.Y + b.ZY; + result.Z.Z = a.Z.Z + b.ZZ; + return result; + } + + + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void WriteFirst(in Symmetric3x3 scalar, ref Symmetric3x3Wide wide) { From 6acedd7670a060a41ed33172f9487b4ffb72687a Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 11 Jul 2021 15:35:31 -0500 Subject: [PATCH 095/947] Does not build. More plumbing for integration. --- .../Constraints/FourBodyTypeProcessor.cs | 5 +- .../Constraints/OneBodyTypeProcessor.cs | 7 +-- .../Constraints/ThreeBodyTypeProcessor.cs | 5 +- .../Constraints/TwoBodyTypeProcessor.cs | 34 ++++++++++---- BepuPhysics/Constraints/TypeProcessor.cs | 6 +-- BepuPhysics/PoseIntegrator.cs | 19 ++++++++ .../Solver_IncrementalContactUpdate.cs | 2 +- BepuPhysics/Solver_Solve2.cs | 4 +- BepuPhysics/Solver_SubsteppingSolve.cs | 4 +- BepuPhysics/Solver_SubsteppingSolve2.cs | 46 +++++++++++++------ 10 files changed, 96 insertions(+), 36 deletions(-) diff --git a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs index 041392f6e..b31aabcc9 100644 --- a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs @@ -319,11 +319,12 @@ public unsafe override void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodi } - public override void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + public override void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks poseIntegratorCallbacks, + float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { throw new NotImplementedException(); } - public override void SolveStep2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + public override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs index ffea07aef..4edafea58 100644 --- a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs @@ -102,7 +102,7 @@ public unsafe override void Prestep(ref TypeBatch typeBatch, Bodies bodies, floa ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); var count = GetCountInBundle(ref typeBatch, i); bodies.GatherState(ref references, count, out var position, out var orientation, out _, out var inertia); - function.Prestep(position, orientation, inertia, dt, inverseDt, ref prestep, out projection); + function.Prestep(position, orientation, inertia, dt, inverseDt, ref prestep, out projection); } } @@ -250,7 +250,8 @@ public unsafe override void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodi - public unsafe override void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + public unsafe override void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks poseIntegratorCallbacks, + float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); ref var bodyReferencesBase = ref Unsafe.AsRef>(typeBatch.BodyReferences.Memory); @@ -269,7 +270,7 @@ public unsafe override void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); ref var bodyReferencesBase = ref Unsafe.AsRef>(typeBatch.BodyReferences.Memory); diff --git a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs index 439d8569f..8a1108920 100644 --- a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs @@ -295,11 +295,12 @@ public unsafe override void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodi } } - public override void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + public override void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks poseIntegratorCallbacks, + float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { throw new NotImplementedException(); } - public override void SolveStep2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + public override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 832b2733a..695cc834d 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -349,14 +349,33 @@ unsafe static void Prefetch(int prefetchDistance, ref TypeBatch typeBatch, ref B } } - bool BundleShouldIntegrate(int bundleIndex, in IndexSet integrationFlags, out int integrationMask) + [MethodImpl(MethodImplOptions.AggressiveInlining)] + bool BundleShouldIntegrate(int bundleIndex, in IndexSet integrationFlags, out Vector integrationMask) { Debug.Assert(Vector.Count <= 32, "Wait, what? The integration mask isn't big enough to handle a vector this big."); var constraintStartIndex = bundleIndex * Vector.Count; var flagBundleIndex = constraintStartIndex >> 6; var flagInnerIndex = constraintStartIndex - flagBundleIndex; - integrationMask = ((int)(integrationFlags.Flags[flagBundleIndex] >> flagInnerIndex)) & BundleIndexing.VectorMask; - return integrationMask > 0; + var scalarIntegrationMask = ((int)(integrationFlags.Flags[flagBundleIndex] >> flagInnerIndex)) & BundleIndexing.VectorMask; + if (scalarIntegrationMask == BundleIndexing.VectorMask) + { + //No need to carefully expand a bitstring into a vector mask if we know that a single broadcast will suffice. + integrationMask = new Vector(-1); + return true; + } + else if (scalarIntegrationMask > 0) + { + //TODO: This is bad prototype code. + Span mask = stackalloc int[Vector.Count]; + for (int i = 0; i < mask.Length; ++i) + { + mask[i] = (scalarIntegrationMask & (1 << i)) > 0 ? -1 : 0; + } + integrationMask = new Vector(scalarIntegrationMask); + return true; + } + integrationMask = default; + return false; } unsafe void IntegratePoseAndVelocity( @@ -372,7 +391,6 @@ unsafe void IntegratePoseAndVelocity( //So, the solver runs velocity integration alone on the first substep. All later substeps then run pose + velocity, and then after the last substep, a final pose integration. //This is equivalent in ordering to running each substep as velocity, warmstart, solve, pose integration, but just shifting the execution context. position += velocity.Linear * new Vector(dt); - if (integratorCallbacks.AngularIntegrationMode == AngularIntegrationMode.ConserveMomentum) { var oldOrientation = orientation; @@ -394,7 +412,7 @@ unsafe void IntegratePoseAndVelocity( PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, orientation, out inertia.InverseInertiaTensor); inertia.InverseMass = localInertia.InverseMass; } - integratorCallbacks.IntegrateVelocity(new ReadOnlySpan(Unsafe.AsPointer(ref bodyIndices), Vector.Count), position, orientation, localInertia, integrationMask, workerIndex, velocity, + integratorCallbacks.IntegrateVelocity(new ReadOnlySpan(Unsafe.AsPointer(ref bodyIndices), Vector.Count), position, orientation, localInertia, integrationMask, workerIndex, velocity, new Vector(dt), out Vector3Wide linearChange, out Vector3Wide angularChange); //It would be annoying to make the user handle masking velocity writes to inactive lanes, so we handle it internally. velocity.Linear.X = Vector.ConditionalSelect(integrationMask, velocity.Linear.X + linearChange.X, velocity.Linear.X); @@ -426,9 +444,9 @@ public unsafe override void WarmStart2( Prefetch(warmStartPrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, ref inertias, i, exclusiveEndBundle); bodies.GatherState(ref references, count, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB); if (BundleShouldIntegrate(i, integrationFlags[0], out var integrationMaskA)) - IntegratePoseAndVelocity(ref integratorCallbacks, localInertiaA, dt, integrationMaskA, ref positionA, ref orientationA, ref wsvA, workerIndex, out inertiaA); + IntegratePoseAndVelocity(ref integratorCallbacks, ref references.IndexA, localInertiaA, dt, integrationMaskA, ref positionA, ref orientationA, ref wsvA, workerIndex, out inertiaA); if (BundleShouldIntegrate(i, integrationFlags[1], out var integrationMaskB)) - IntegratePoseAndVelocity(ref integratorCallbacks, localInertiaB, dt, integrationMaskB, ref positionB, ref orientationB, ref wsvB, workerIndex, out inertiaB); + IntegratePoseAndVelocity(ref integratorCallbacks, ref references.IndexB, localInertiaB, dt, integrationMaskB, ref positionB, ref orientationB, ref wsvB, workerIndex, out inertiaB); if (typeof(TConstraintFunctions) == typeof(WeldFunctions)) { default(WeldFunctions).WarmStart2(orientationA, inertiaA, ab, orientationB, inertiaB, @@ -447,7 +465,7 @@ public unsafe override void WarmStart2( bodies.ScatterVelocities(ref wsvA, ref wsvB, ref references, count); } } - public unsafe override void SolveStep2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { var prestepBundles = typeBatch.PrestepData.As(); var bodyReferencesBundles = typeBatch.BodyReferences.As(); diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index fe48272ba..ec35e43e7 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -136,9 +136,9 @@ internal unsafe abstract void CopySleepingToActive( public abstract void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); - public abstract void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TCallbacks poseIntegratorCallbacks, - float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) where TCallbacks : struct, IPoseIntegratorCallbacks; - public abstract void SolveStep2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); + public abstract void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks poseIntegratorCallbacks, + float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) where TIntegratorCallbacks : struct, IPoseIntegratorCallbacks; + public abstract void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); public abstract void Prestep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); diff --git a/BepuPhysics/PoseIntegrator.cs b/BepuPhysics/PoseIntegrator.cs index 7a5f6c2a0..cce2f8880 100644 --- a/BepuPhysics/PoseIntegrator.cs +++ b/BepuPhysics/PoseIntegrator.cs @@ -69,6 +69,25 @@ public interface IPoseIntegratorCallbacks /// Index of the worker thread processing this body. /// Reference to the body's current velocity to integrate. void IntegrateVelocity(int bodyIndex, in RigidPose pose, in BodyInertia localInertia, int workerIndex, ref BodyVelocity velocity); + + + /// + /// Callback for a bundle of bodies being integrated. + /// + /// Indices of the bodies being integrated in this bundle. + /// Current body positions. + /// Current body orientations. + /// Body's current local inertia. + /// Mask indicating which lanes are active in the bundle. Active lanes will contain 0xFFFFFFFF, inactive lanes will contain 0. + /// Index of the worker thread processing this bundle. + /// Current velocity of bodies in the bundle. + /// Durations to integrate the velocity over. Can vary over lanes. + /// Change to apply to the linear velocity of bodies in the bundle. Any changes in inactive lanes will be ignored. + /// Change to apply to the angular velocity of bodies in the bundle. Any changes in inactive lanes will be ignored. + void IntegrateVelocity( + ReadOnlySpan bodyIndices, in Vector3Wide position, in QuaternionWide orientation, in BodyInertiaWide localInertia, + in Vector integrationMask, int workerIndex, in BodyVelocityWide velocity, in Vector dt, + out Vector3Wide linearChange, out Vector3Wide angularChange); } /// diff --git a/BepuPhysics/Solver_IncrementalContactUpdate.cs b/BepuPhysics/Solver_IncrementalContactUpdate.cs index 3234b9a24..118d4a4d9 100644 --- a/BepuPhysics/Solver_IncrementalContactUpdate.cs +++ b/BepuPhysics/Solver_IncrementalContactUpdate.cs @@ -28,7 +28,7 @@ struct IncrementalContactUpdateStageFunction : IStageFunction public float InverseDt; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Execute(Solver solver, int blockIndex) + public void Execute(Solver solver, int blockIndex, int workerIndex) { ref var block = ref solver.context.ConstraintBlocks.Blocks[blockIndex]; ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; diff --git a/BepuPhysics/Solver_Solve2.cs b/BepuPhysics/Solver_Solve2.cs index 558f96020..b6e9aed94 100644 --- a/BepuPhysics/Solver_Solve2.cs +++ b/BepuPhysics/Solver_Solve2.cs @@ -22,7 +22,7 @@ struct Prestep2StageFunction : IStageFunction public float InverseDt; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Execute(Solver solver, int blockIndex) + public void Execute(Solver solver, int blockIndex, int workerIndex) { ref var block = ref solver.context.ConstraintBlocks.Blocks[blockIndex]; ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; @@ -34,7 +34,7 @@ public void Execute(Solver solver, int blockIndex) struct Prestep2FallbackStageFunction : IStageFunction { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Execute(Solver solver, int blockIndex) + public void Execute(Solver solver, int blockIndex, int workerIndex) { ref var block = ref solver.context.ConstraintBlocks.Blocks[blockIndex]; ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; diff --git a/BepuPhysics/Solver_SubsteppingSolve.cs b/BepuPhysics/Solver_SubsteppingSolve.cs index 7ad9aa930..196675beb 100644 --- a/BepuPhysics/Solver_SubsteppingSolve.cs +++ b/BepuPhysics/Solver_SubsteppingSolve.cs @@ -21,7 +21,7 @@ struct SolveStepStageFunction : IStageFunction public float InverseDt; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Execute(Solver solver, int blockIndex) + public void Execute(Solver solver, int blockIndex, int workerIndex) { ref var block = ref solver.context.ConstraintBlocks.Blocks[blockIndex]; ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; @@ -36,7 +36,7 @@ struct FallbackSolveStepStageFunction : IStageFunction public float InverseDt; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Execute(Solver solver, int blockIndex) + public void Execute(Solver solver, int blockIndex, int workerIndex) { ref var block = ref solver.context.ConstraintBlocks.Blocks[blockIndex]; ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index 480825cd9..66f8a372f 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -12,6 +12,20 @@ namespace BepuPhysics { + public partial class Solver + { + public virtual void PrepareConstraintIntegrationResponsibilities() + { + } + public virtual void DisposeConstraintIntegrationResponsibilities() + { + } + + public virtual void SolveStep2(float dt, IThreadDispatcher threadDispatcher = null) + { + + } + } public class Solver : Solver where TIntegrationCallbacks : struct, IPoseIntegratorCallbacks { public Solver(Bodies bodies, BufferPool pool, int iterationCount, int fallbackBatchThreshold, @@ -33,7 +47,7 @@ struct WarmStartStep2StageFunction : IStageFunction { public float Dt; public float InverseDt; - Solver solver; + public Solver solver; [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Execute(Solver solver, int blockIndex, int workerIndex ) @@ -50,7 +64,7 @@ struct SolveStep2StageFunction : IStageFunction { public float Dt; public float InverseDt; - Solver solver; + public Solver solver; [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Execute(Solver solver, int blockIndex, int workerIndex) @@ -58,7 +72,7 @@ public void Execute(Solver solver, int blockIndex, int workerIndex) ref var block = ref this.solver.context.ConstraintBlocks.Blocks[blockIndex]; ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; - typeProcessor.SolveStep2(ref typeBatch, ref this.solver.integrationFlags[block.BatchIndex][block.TypeBatchIndex], solver.bodies, Dt, InverseDt, block.StartBundle, block.End); + typeProcessor.SolveStep2(ref typeBatch, solver.bodies, Dt, InverseDt, block.StartBundle, block.End); } } @@ -106,9 +120,12 @@ void SolveStep2Worker(int workerIndex) Debug.Assert(activeSet.Batches.Count > 0, "Don't dispatch if there are no constraints."); GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); - var warmstartStage = new WarmStartStep2StageFunction(); - warmstartStage.Dt = context.Dt; - warmstartStage.InverseDt = 1f / context.Dt; + var warmstartStage = new WarmStartStep2StageFunction + { + Dt = context.Dt, + InverseDt = 1f / context.Dt, + solver = this + }; for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) { var batchOffset = batchIndex > 0 ? context.BatchBoundaries[batchIndex - 1] : 0; @@ -117,9 +134,12 @@ void SolveStep2Worker(int workerIndex) } claimedState ^= 1; unclaimedState ^= 1; - var solveStage = new SolveStep2StageFunction(); - solveStage.Dt = context.Dt; - solveStage.InverseDt = 1f / context.Dt; + var solveStage = new SolveStep2StageFunction + { + Dt = context.Dt, + InverseDt = 1f / context.Dt, + solver = this + }; for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) { var batchOffset = batchIndex > 0 ? context.BatchBoundaries[batchIndex - 1] : 0; @@ -142,7 +162,7 @@ void SolveStep2Worker(int workerIndex) Buffer>> integrationFlags; - public void PrepareConstraintIntegrationResponsibilities() + public override void PrepareConstraintIntegrationResponsibilities() { //var start = Stopwatch.GetTimestamp(); pool.Take(ActiveSet.Batches.Count, out integrationFlags); @@ -193,7 +213,7 @@ public void PrepareConstraintIntegrationResponsibilities() //var end = Stopwatch.GetTimestamp(); //Console.WriteLine($"Brute force time (ms): {(end - start) * 1e3 / Stopwatch.Frequency}"); } - public void DisposeConstraintIntegrationResponsibilities() + public override void DisposeConstraintIntegrationResponsibilities() { for (int i = 0; i < integrationFlags.Length; ++i) { @@ -212,7 +232,7 @@ public void DisposeConstraintIntegrationResponsibilities() pool.Return(ref integrationFlags); } - public void SolveStep2(float dt, IThreadDispatcher threadDispatcher = null) + public override void SolveStep2(float dt, IThreadDispatcher threadDispatcher = null) { if (threadDispatcher == null) { @@ -239,7 +259,7 @@ public void SolveStep2(float dt, IThreadDispatcher threadDispatcher = null) for (int j = 0; j < batch.TypeBatches.Count; ++j) { ref var typeBatch = ref batch.TypeBatches[j]; - TypeProcessors[typeBatch.TypeId].SolveStep2(ref typeBatch, ref integrationFlagsForBatch[j], bodies, dt, inverseDt, 0, typeBatch.BundleCount); + TypeProcessors[typeBatch.TypeId].SolveStep2(ref typeBatch, bodies, dt, inverseDt, 0, typeBatch.BundleCount); } } } From 429444c92d9be0dc19cf1be9995998f6745a3374 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 11 Jul 2021 16:44:12 -0500 Subject: [PATCH 096/947] Does not build. Refactoring bodies gathering. --- BepuPhysics/Bodies_GatherScatter.cs | 304 ++++++++++-------- BepuPhysics/BodyProperties.cs | 8 - .../Constraints/TwoBodyTypeProcessor.cs | 74 +++-- BepuPhysics/PoseIntegrator.cs | 7 +- BepuUtilities/QuaternionWide.cs | 20 ++ 5 files changed, 244 insertions(+), 169 deletions(-) diff --git a/BepuPhysics/Bodies_GatherScatter.cs b/BepuPhysics/Bodies_GatherScatter.cs index 66b2a8cd5..4e8ba38b5 100644 --- a/BepuPhysics/Bodies_GatherScatter.cs +++ b/BepuPhysics/Bodies_GatherScatter.cs @@ -74,7 +74,7 @@ public void GatherState(ref Vector references, int count, } //[MethodImpl(MethodImplOptions.AggressiveInlining)] - unsafe static void ScalarGather(int count, MotionState* motionStates, ref Vector baseIndex, ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocityWide velocity, ref BodyInertiaWide inertia) + unsafe static void FallbackGatherMotionState(int count, MotionState* motionStates, ref Vector baseIndex, ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocityWide velocity) { var indices = (int*)Unsafe.AsPointer(ref baseIndex); var pPositionX = (float*)Unsafe.AsPointer(ref position.X); @@ -90,13 +90,6 @@ unsafe static void ScalarGather(int count, MotionState* motionStates, ref Vector var pAngularX = (float*)Unsafe.AsPointer(ref velocity.Angular.X); var pAngularY = (float*)Unsafe.AsPointer(ref velocity.Angular.Y); var pAngularZ = (float*)Unsafe.AsPointer(ref velocity.Angular.Z); - var pMass = (float*)Unsafe.AsPointer(ref inertia.InverseMass); - var pInertiaXX = (float*)Unsafe.AsPointer(ref inertia.InverseInertiaTensor.XX); - var pInertiaYX = (float*)Unsafe.AsPointer(ref inertia.InverseInertiaTensor.YX); - var pInertiaYY = (float*)Unsafe.AsPointer(ref inertia.InverseInertiaTensor.YY); - var pInertiaZX = (float*)Unsafe.AsPointer(ref inertia.InverseInertiaTensor.ZX); - var pInertiaZY = (float*)Unsafe.AsPointer(ref inertia.InverseInertiaTensor.ZY); - var pInertiaZZ = (float*)Unsafe.AsPointer(ref inertia.InverseInertiaTensor.ZZ); for (int i = 0; i < count; ++i) { @@ -115,136 +108,171 @@ unsafe static void ScalarGather(int count, MotionState* motionStates, ref Vector pAngularX[i] = stateValues[MotionState.OffsetToAngularX]; pAngularY[i] = stateValues[MotionState.OffsetToAngularY]; pAngularZ[i] = stateValues[MotionState.OffsetToAngularZ]; - pMass[i] = stateValues[MotionState.OffsetToInverseMass]; - pInertiaXX[i] = stateValues[MotionState.OffsetToInverseInertiaXX]; - pInertiaYX[i] = 0; - pInertiaYY[i] = stateValues[MotionState.OffsetToInverseInertiaYY]; - pInertiaZX[i] = 0; - pInertiaZY[i] = 0; - pInertiaZZ[i] = stateValues[MotionState.OffsetToInverseInertiaZZ]; + } + } + unsafe static void FallbackGatherInertia(int count, BodyInertias* inertias, ref Vector baseIndex, ref BodyInertiaWide inertia, int offsetInFloats) + { + var indices = (int*)Unsafe.AsPointer(ref baseIndex); + var pMass = (float*)Unsafe.AsPointer(ref inertia.InverseMass); + var pInertiaXX = (float*)Unsafe.AsPointer(ref inertia.InverseInertiaTensor.XX); + var pInertiaYX = (float*)Unsafe.AsPointer(ref inertia.InverseInertiaTensor.YX); + var pInertiaYY = (float*)Unsafe.AsPointer(ref inertia.InverseInertiaTensor.YY); + var pInertiaZX = (float*)Unsafe.AsPointer(ref inertia.InverseInertiaTensor.ZX); + var pInertiaZY = (float*)Unsafe.AsPointer(ref inertia.InverseInertiaTensor.ZY); + var pInertiaZZ = (float*)Unsafe.AsPointer(ref inertia.InverseInertiaTensor.ZZ); + + for (int i = 0; i < count; ++i) + { + var index = indices[i]; + var inertiaValues = (float*)(inertias + index) + offsetInFloats; + pInertiaXX[i] = inertiaValues[0]; + pInertiaYX[i] = inertiaValues[1]; + pInertiaYY[i] = inertiaValues[2]; + pInertiaZX[i] = inertiaValues[3]; + pInertiaZY[i] = inertiaValues[4]; + pInertiaZZ[i] = inertiaValues[5]; + pMass[i] = inertiaValues[6]; } } //[MethodImpl(MethodImplOptions.AggressiveInlining)] - unsafe static void TransposingGather(int count, MotionState* motionStates, BodyInertias* inertias, ref Vector baseIndex, ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocityWide velocity, ref BodyInertiaWide inertia) + public unsafe void GatherMotionState(ref Vector bodyIndices, int count, out Vector3Wide position, out QuaternionWide orientation, out BodyVelocityWide velocity) { + var motionStates = ActiveSet.MotionStates.Memory; if (Avx.IsSupported) { - var indices = (int*)Unsafe.AsPointer(ref baseIndex); + var indices = (int*)Unsafe.AsPointer(ref bodyIndices); + + var s0 = (float*)(motionStates + indices[0]); + var s1 = (float*)(motionStates + indices[1]); + var s2 = (float*)(motionStates + indices[2]); + var s3 = (float*)(motionStates + indices[3]); + var s4 = (float*)(motionStates + indices[4]); + var s5 = (float*)(motionStates + indices[5]); + var s6 = (float*)(motionStates + indices[6]); + var s7 = (float*)(motionStates + indices[7]); + + //for (int i = 0; i < 8; ++i) + //{ + // s0[i] = i; + // s1[i] = i + 100; + // s2[i] = i + 200; + // s3[i] = i + 300; + // s4[i] = i + 400; + // s5[i] = i + 500; + // s6[i] = i + 600; + // s7[i] = i + 700; + //} + + { + //Load every body for the first half of the motion state. + //Note that buffers are allocated on cache line boundaries, so we can use aligned loads for all that matters. + var m0 = Avx.LoadVector256(s0); + var m1 = count > 1 ? Avx.LoadAlignedVector256(s1) : Vector256.Zero; + var m2 = count > 2 ? Avx.LoadAlignedVector256(s2) : Vector256.Zero; + var m3 = count > 3 ? Avx.LoadAlignedVector256(s3) : Vector256.Zero; + var m4 = count > 4 ? Avx.LoadAlignedVector256(s4) : Vector256.Zero; + var m5 = count > 5 ? Avx.LoadAlignedVector256(s5) : Vector256.Zero; + var m6 = count > 6 ? Avx.LoadAlignedVector256(s6) : Vector256.Zero; + var m7 = count > 7 ? Avx.LoadAlignedVector256(s7) : Vector256.Zero; + + var n0 = Avx.UnpackLow(m0, m1); + var n1 = Avx.UnpackLow(m2, m3); + var n2 = Avx.UnpackLow(m4, m5); + var n3 = Avx.UnpackLow(m6, m7); + var n4 = Avx.UnpackHigh(m0, m1); + var n5 = Avx.UnpackHigh(m2, m3); + var n6 = Avx.UnpackHigh(m4, m5); + var n7 = Avx.UnpackHigh(m6, m7); + + var o0 = Avx.Shuffle(n0, n1, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o1 = Avx.Shuffle(n2, n3, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o2 = Avx.Shuffle(n4, n5, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o3 = Avx.Shuffle(n6, n7, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o4 = Avx.Shuffle(n0, n1, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o5 = Avx.Shuffle(n2, n3, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o6 = Avx.Shuffle(n4, n5, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o7 = Avx.Shuffle(n6, n7, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + + orientation.X = Avx.Permute2x128(o0, o1, 0 | (2 << 4)).AsVector(); + orientation.Y = Avx.Permute2x128(o4, o5, 0 | (2 << 4)).AsVector(); + orientation.Z = Avx.Permute2x128(o2, o3, 0 | (2 << 4)).AsVector(); + orientation.W = Avx.Permute2x128(o6, o7, 0 | (2 << 4)).AsVector(); + position.X = Avx.Permute2x128(o0, o1, 1 | (3 << 4)).AsVector(); + position.Y = Avx.Permute2x128(o4, o5, 1 | (3 << 4)).AsVector(); + position.Z = Avx.Permute2x128(o2, o3, 1 | (3 << 4)).AsVector(); + //inertia.InverseMass = Avx.Permute2x128(o6, o7, 1 | (3 << 4)).AsVector(); + //inertia.InverseInertiaTensor.XX = inertia.InverseMass; + //inertia.InverseInertiaTensor.YY = inertia.InverseMass; + //inertia.InverseInertiaTensor.ZZ = inertia.InverseMass; + //inertia.InverseInertiaTensor.YX = default; + //inertia.InverseInertiaTensor.ZX = default; + //inertia.InverseInertiaTensor.ZY = default; + } { - var s0 = (float*)(motionStates + indices[0]); - var s1 = (float*)(motionStates + indices[1]); - var s2 = (float*)(motionStates + indices[2]); - var s3 = (float*)(motionStates + indices[3]); - var s4 = (float*)(motionStates + indices[4]); - var s5 = (float*)(motionStates + indices[5]); - var s6 = (float*)(motionStates + indices[6]); - var s7 = (float*)(motionStates + indices[7]); - - //for (int i = 0; i < 8; ++i) - //{ - // s0[i] = i; - // s1[i] = i + 100; - // s2[i] = i + 200; - // s3[i] = i + 300; - // s4[i] = i + 400; - // s5[i] = i + 500; - // s6[i] = i + 600; - // s7[i] = i + 700; - //} - - { - //Load every body for the first half of the motion state. - //Note that buffers are allocated on cache line boundaries, so we can use aligned loads for all that matters. - var m0 = Avx.LoadVector256(s0); - var m1 = count > 1 ? Avx.LoadAlignedVector256(s1) : Vector256.Zero; - var m2 = count > 2 ? Avx.LoadAlignedVector256(s2) : Vector256.Zero; - var m3 = count > 3 ? Avx.LoadAlignedVector256(s3) : Vector256.Zero; - var m4 = count > 4 ? Avx.LoadAlignedVector256(s4) : Vector256.Zero; - var m5 = count > 5 ? Avx.LoadAlignedVector256(s5) : Vector256.Zero; - var m6 = count > 6 ? Avx.LoadAlignedVector256(s6) : Vector256.Zero; - var m7 = count > 7 ? Avx.LoadAlignedVector256(s7) : Vector256.Zero; - - var n0 = Avx.UnpackLow(m0, m1); - var n1 = Avx.UnpackLow(m2, m3); - var n2 = Avx.UnpackLow(m4, m5); - var n3 = Avx.UnpackLow(m6, m7); - var n4 = Avx.UnpackHigh(m0, m1); - var n5 = Avx.UnpackHigh(m2, m3); - var n6 = Avx.UnpackHigh(m4, m5); - var n7 = Avx.UnpackHigh(m6, m7); - - var o0 = Avx.Shuffle(n0, n1, 0 | (1 << 2) | (0 << 4) | (1 << 6)); - var o1 = Avx.Shuffle(n2, n3, 0 | (1 << 2) | (0 << 4) | (1 << 6)); - var o2 = Avx.Shuffle(n4, n5, 0 | (1 << 2) | (0 << 4) | (1 << 6)); - var o3 = Avx.Shuffle(n6, n7, 0 | (1 << 2) | (0 << 4) | (1 << 6)); - var o4 = Avx.Shuffle(n0, n1, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - var o5 = Avx.Shuffle(n2, n3, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - var o6 = Avx.Shuffle(n4, n5, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - var o7 = Avx.Shuffle(n6, n7, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - - orientation.X = Avx.Permute2x128(o0, o1, 0 | (2 << 4)).AsVector(); - orientation.Y = Avx.Permute2x128(o4, o5, 0 | (2 << 4)).AsVector(); - orientation.Z = Avx.Permute2x128(o2, o3, 0 | (2 << 4)).AsVector(); - orientation.W = Avx.Permute2x128(o6, o7, 0 | (2 << 4)).AsVector(); - position.X = Avx.Permute2x128(o0, o1, 1 | (3 << 4)).AsVector(); - position.Y = Avx.Permute2x128(o4, o5, 1 | (3 << 4)).AsVector(); - position.Z = Avx.Permute2x128(o2, o3, 1 | (3 << 4)).AsVector(); - //inertia.InverseMass = Avx.Permute2x128(o6, o7, 1 | (3 << 4)).AsVector(); - //inertia.InverseInertiaTensor.XX = inertia.InverseMass; - //inertia.InverseInertiaTensor.YY = inertia.InverseMass; - //inertia.InverseInertiaTensor.ZZ = inertia.InverseMass; - //inertia.InverseInertiaTensor.YX = default; - //inertia.InverseInertiaTensor.ZX = default; - //inertia.InverseInertiaTensor.ZY = default; - } - - { - //Second half. - var m0 = Avx.LoadVector256(s0 + 8); - var m1 = count > 1 ? Avx.LoadAlignedVector256(s1 + 8) : Vector256.Zero; - var m2 = count > 2 ? Avx.LoadAlignedVector256(s2 + 8) : Vector256.Zero; - var m3 = count > 3 ? Avx.LoadAlignedVector256(s3 + 8) : Vector256.Zero; - var m4 = count > 4 ? Avx.LoadAlignedVector256(s4 + 8) : Vector256.Zero; - var m5 = count > 5 ? Avx.LoadAlignedVector256(s5 + 8) : Vector256.Zero; - var m6 = count > 6 ? Avx.LoadAlignedVector256(s6 + 8) : Vector256.Zero; - var m7 = count > 7 ? Avx.LoadAlignedVector256(s7 + 8) : Vector256.Zero; - - var n0 = Avx.UnpackLow(m0, m1); - var n1 = Avx.UnpackLow(m2, m3); - var n2 = Avx.UnpackLow(m4, m5); - var n3 = Avx.UnpackLow(m6, m7); - var n4 = Avx.UnpackHigh(m0, m1); - var n5 = Avx.UnpackHigh(m2, m3); - var n6 = Avx.UnpackHigh(m4, m5); - var n7 = Avx.UnpackHigh(m6, m7); - - var o0 = Avx.Shuffle(n0, n1, 0 | (1 << 2) | (0 << 4) | (1 << 6)); - var o1 = Avx.Shuffle(n2, n3, 0 | (1 << 2) | (0 << 4) | (1 << 6)); - var o2 = Avx.Shuffle(n4, n5, 0 | (1 << 2) | (0 << 4) | (1 << 6)); - var o3 = Avx.Shuffle(n6, n7, 0 | (1 << 2) | (0 << 4) | (1 << 6)); - var o4 = Avx.Shuffle(n0, n1, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - var o5 = Avx.Shuffle(n2, n3, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - var o6 = Avx.Shuffle(n4, n5, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - var o7 = Avx.Shuffle(n6, n7, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - - velocity.Linear.X = Avx.Permute2x128(o0, o1, 0 | (2 << 4)).AsVector(); - velocity.Linear.Y = Avx.Permute2x128(o4, o5, 0 | (2 << 4)).AsVector(); - velocity.Linear.Z = Avx.Permute2x128(o2, o3, 0 | (2 << 4)).AsVector(); - velocity.Angular.X = Avx.Permute2x128(o0, o1, 1 | (3 << 4)).AsVector(); - velocity.Angular.Y = Avx.Permute2x128(o4, o5, 1 | (3 << 4)).AsVector(); - velocity.Angular.Z = Avx.Permute2x128(o2, o3, 1 | (3 << 4)).AsVector(); - } + //Second half. + var m0 = Avx.LoadVector256(s0 + 8); + var m1 = count > 1 ? Avx.LoadAlignedVector256(s1 + 8) : Vector256.Zero; + var m2 = count > 2 ? Avx.LoadAlignedVector256(s2 + 8) : Vector256.Zero; + var m3 = count > 3 ? Avx.LoadAlignedVector256(s3 + 8) : Vector256.Zero; + var m4 = count > 4 ? Avx.LoadAlignedVector256(s4 + 8) : Vector256.Zero; + var m5 = count > 5 ? Avx.LoadAlignedVector256(s5 + 8) : Vector256.Zero; + var m6 = count > 6 ? Avx.LoadAlignedVector256(s6 + 8) : Vector256.Zero; + var m7 = count > 7 ? Avx.LoadAlignedVector256(s7 + 8) : Vector256.Zero; + + var n0 = Avx.UnpackLow(m0, m1); + var n1 = Avx.UnpackLow(m2, m3); + var n2 = Avx.UnpackLow(m4, m5); + var n3 = Avx.UnpackLow(m6, m7); + var n4 = Avx.UnpackHigh(m0, m1); + var n5 = Avx.UnpackHigh(m2, m3); + var n6 = Avx.UnpackHigh(m4, m5); + var n7 = Avx.UnpackHigh(m6, m7); + + var o0 = Avx.Shuffle(n0, n1, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o1 = Avx.Shuffle(n2, n3, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o2 = Avx.Shuffle(n4, n5, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o3 = Avx.Shuffle(n6, n7, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o4 = Avx.Shuffle(n0, n1, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o5 = Avx.Shuffle(n2, n3, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o6 = Avx.Shuffle(n4, n5, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o7 = Avx.Shuffle(n6, n7, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + + velocity.Linear.X = Avx.Permute2x128(o0, o1, 0 | (2 << 4)).AsVector(); + velocity.Linear.Y = Avx.Permute2x128(o4, o5, 0 | (2 << 4)).AsVector(); + velocity.Linear.Z = Avx.Permute2x128(o2, o3, 0 | (2 << 4)).AsVector(); + velocity.Angular.X = Avx.Permute2x128(o0, o1, 1 | (3 << 4)).AsVector(); + velocity.Angular.Y = Avx.Permute2x128(o4, o5, 1 | (3 << 4)).AsVector(); + velocity.Angular.Z = Avx.Permute2x128(o2, o3, 1 | (3 << 4)).AsVector(); } + + } + else + { + Unsafe.SkipInit(out position); + Unsafe.SkipInit(out orientation); + Unsafe.SkipInit(out velocity); + FallbackGatherMotionState(count, motionStates, ref bodyIndices, ref position, ref orientation, ref velocity); + } + } + + + //[MethodImpl(MethodImplOptions.AggressiveInlining)] + unsafe void GatherInertia(ref Vector bodyIndices, int count, int offsetInFloats, out BodyInertiaWide inertia) + { + var inertias = ActiveSet.Inertias.Memory; + if (Avx.IsSupported) + { + var indices = (int*)Unsafe.AsPointer(ref bodyIndices); { - var s0 = (float*)(inertias + indices[0]) + 8; - var s1 = (float*)(inertias + indices[1]) + 8; - var s2 = (float*)(inertias + indices[2]) + 8; - var s3 = (float*)(inertias + indices[3]) + 8; - var s4 = (float*)(inertias + indices[4]) + 8; - var s5 = (float*)(inertias + indices[5]) + 8; - var s6 = (float*)(inertias + indices[6]) + 8; - var s7 = (float*)(inertias + indices[7]) + 8; + var s0 = (float*)(inertias + indices[0]) + offsetInFloats; + var s1 = (float*)(inertias + indices[1]) + offsetInFloats; + var s2 = (float*)(inertias + indices[2]) + offsetInFloats; + var s3 = (float*)(inertias + indices[3]) + offsetInFloats; + var s4 = (float*)(inertias + indices[4]) + offsetInFloats; + var s5 = (float*)(inertias + indices[5]) + offsetInFloats; + var s6 = (float*)(inertias + indices[6]) + offsetInFloats; + var s7 = (float*)(inertias + indices[7]) + offsetInFloats; //Load every inertia vector. var m0 = Avx.LoadVector256(s0); @@ -285,10 +313,22 @@ unsafe static void TransposingGather(int count, MotionState* motionStates, BodyI } else { - ScalarGather(count, motionStates, ref baseIndex, ref position, ref orientation, ref velocity, ref inertia); + Unsafe.SkipInit(out inertia); + FallbackGatherInertia(count, ActiveSet.Inertias.Memory, ref bodyIndices, ref inertia, offsetInFloats); } } + //[MethodImpl(MethodImplOptions.AggressiveInlining)] + public unsafe void GatherWorldInertia(ref Vector bodyIndices, int count, out BodyInertiaWide inertia) + { + GatherInertia(ref bodyIndices, count, 8, out inertia); + } + //[MethodImpl(MethodImplOptions.AggressiveInlining)] + public unsafe void GatherLocalInertia(ref Vector bodyIndices, int count, out BodyInertiaWide inertia) + { + GatherInertia(ref bodyIndices, count, 0, out inertia); + } + /// /// Gathers orientations and relative positions for a two body bundle into an AOSOA bundle. /// @@ -322,8 +362,10 @@ public unsafe void GatherState(ref TwoBodyReferences references, int count, ref var states = ref ActiveSet.MotionStates; - TransposingGather(count, states.Memory, ActiveSet.Inertias.Memory, ref references.IndexA, ref positionA, ref orientationA, ref velocityA, ref inertiaA); - TransposingGather(count, states.Memory, ActiveSet.Inertias.Memory, ref references.IndexB, ref positionB, ref orientationB, ref velocityB, ref inertiaB); + GatherMotionState(ref references.IndexA, count, out positionA, out orientationA, out velocityA); + GatherWorldInertia(ref references.IndexA, count, out inertiaA); + GatherMotionState(ref references.IndexB, count, out positionB, out orientationB, out velocityB); + GatherWorldInertia(ref references.IndexA, count, out inertiaB); //for (int i = 0; i < count; ++i) //{ diff --git a/BepuPhysics/BodyProperties.cs b/BepuPhysics/BodyProperties.cs index e6312cec8..3f993ae17 100644 --- a/BepuPhysics/BodyProperties.cs +++ b/BepuPhysics/BodyProperties.cs @@ -22,16 +22,12 @@ public struct MotionState internal const int OffsetToPositionX = 4; internal const int OffsetToPositionY = 5; internal const int OffsetToPositionZ = 6; - internal const int OffsetToInverseMass = 7; internal const int OffsetToLinearX = 8; internal const int OffsetToLinearY = 9; internal const int OffsetToLinearZ = 10; internal const int OffsetToAngularX = 12; internal const int OffsetToAngularY = 13; internal const int OffsetToAngularZ = 14; - internal const int OffsetToInverseInertiaXX = OffsetToInverseMass; - internal const int OffsetToInverseInertiaYY = OffsetToInverseMass; - internal const int OffsetToInverseInertiaZZ = OffsetToInverseMass; internal const int ByteOffsetToPositionX = OffsetToPositionX * 4; internal const int ByteOffsetToPositionY = OffsetToPositionY * 4; @@ -46,10 +42,6 @@ public struct MotionState internal const int ByteOffsetToAngularX = OffsetToAngularX * 4; internal const int ByteOffsetToAngularY = OffsetToAngularY * 4; internal const int ByteOffsetToAngularZ = OffsetToAngularZ * 4; - internal const int ByteOffsetToInverseMass = OffsetToInverseMass * 4; - internal const int ByteOffsetToInverseInertiaXX = ByteOffsetToInverseMass; - internal const int ByteOffsetToInverseInertiaYY = ByteOffsetToInverseMass; - internal const int ByteOffsetToInverseInertiaZZ = ByteOffsetToInverseMass; /// diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 695cc834d..b4991091e 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -350,7 +350,7 @@ unsafe static void Prefetch(int prefetchDistance, ref TypeBatch typeBatch, ref B } [MethodImpl(MethodImplOptions.AggressiveInlining)] - bool BundleShouldIntegrate(int bundleIndex, in IndexSet integrationFlags, out Vector integrationMask) + public static bool BundleShouldIntegrate(int bundleIndex, in IndexSet integrationFlags, out Vector integrationMask) { Debug.Assert(Vector.Count <= 32, "Wait, what? The integration mask isn't big enough to handle a vector this big."); var constraintStartIndex = bundleIndex * Vector.Count; @@ -378,8 +378,8 @@ bool BundleShouldIntegrate(int bundleIndex, in IndexSet integrationFlags, out Ve return false; } - unsafe void IntegratePoseAndVelocity( - ref TIntegratorCallbacks integratorCallbacks, ref Vector bodyIndices, ref BodyInertiaWide localInertia, float dt, Vector integrationMask, + public static unsafe void IntegratePoseAndVelocity( + ref TIntegratorCallbacks integratorCallbacks, ref Vector bodyIndices, in BodyInertiaWide localInertia, float dt, Vector integrationMask, ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocityWide velocity, int workerIndex, out BodyInertiaWide inertia) @@ -390,40 +390,62 @@ unsafe void IntegratePoseAndVelocity( //This ensures that velocities set externally are still solved before being integrated. //So, the solver runs velocity integration alone on the first substep. All later substeps then run pose + velocity, and then after the last substep, a final pose integration. //This is equivalent in ordering to running each substep as velocity, warmstart, solve, pose integration, but just shifting the execution context. - position += velocity.Linear * new Vector(dt); + var newPosition = position + velocity.Linear * new Vector(dt); + //Note that we only take results for slots which actually need integration. Reintegration would be an error. + Vector3Wide.ConditionalSelect(integrationMask, newPosition, position, out position); + QuaternionWide newOrientation; + inertia.InverseMass = localInertia.InverseMass; + var previousVelocity = velocity; if (integratorCallbacks.AngularIntegrationMode == AngularIntegrationMode.ConserveMomentum) { - var oldOrientation = orientation; - PoseIntegration.Integrate(orientation, velocity.Angular, new Vector(dt * 0.5f), out orientation); + var previousOrientation = orientation; + PoseIntegration.Integrate(orientation, velocity.Angular, new Vector(dt * 0.5f), out newOrientation); + QuaternionWide.ConditionalSelect(integrationMask, newOrientation, orientation, out orientation); PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, orientation, out inertia.InverseInertiaTensor); - inertia.InverseMass = localInertia.InverseMass; - PoseIntegration.IntegrateAngularVelocityConserveMomentum(oldOrientation, localInertia, inertia, ref velocity.Angular); + PoseIntegration.IntegrateAngularVelocityConserveMomentum(previousOrientation, localInertia, inertia, ref velocity.Angular); } else if (integratorCallbacks.AngularIntegrationMode == AngularIntegrationMode.ConserveMomentumWithGyroscopicTorque) { - PoseIntegration.Integrate(orientation, velocity.Angular, new Vector(dt * 0.5f), out orientation); + PoseIntegration.Integrate(orientation, velocity.Angular, new Vector(dt * 0.5f), out newOrientation); + QuaternionWide.ConditionalSelect(integrationMask, newOrientation, orientation, out orientation); PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, orientation, out inertia.InverseInertiaTensor); - inertia.InverseMass = localInertia.InverseMass; PoseIntegration.IntegrateAngularVelocityConserveMomentumWithGyroscopicTorque(orientation, localInertia, inertia, ref velocity.Angular, dt); } else { - PoseIntegration.Integrate(orientation, velocity.Angular, new Vector(dt * 0.5f), out orientation); + PoseIntegration.Integrate(orientation, velocity.Angular, new Vector(dt * 0.5f), out newOrientation); + QuaternionWide.ConditionalSelect(integrationMask, newOrientation, orientation, out orientation); PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, orientation, out inertia.InverseInertiaTensor); - inertia.InverseMass = localInertia.InverseMass; } - integratorCallbacks.IntegrateVelocity(new ReadOnlySpan(Unsafe.AsPointer(ref bodyIndices), Vector.Count), position, orientation, localInertia, integrationMask, workerIndex, velocity, new Vector(dt), - out Vector3Wide linearChange, out Vector3Wide angularChange); + + integratorCallbacks.IntegrateVelocity(new ReadOnlySpan(Unsafe.AsPointer(ref bodyIndices), Vector.Count), position, orientation, localInertia, integrationMask, workerIndex, new Vector(dt), ref velocity); //It would be annoying to make the user handle masking velocity writes to inactive lanes, so we handle it internally. - velocity.Linear.X = Vector.ConditionalSelect(integrationMask, velocity.Linear.X + linearChange.X, velocity.Linear.X); - velocity.Linear.Y = Vector.ConditionalSelect(integrationMask, velocity.Linear.Y + linearChange.Y, velocity.Linear.Y); - velocity.Linear.Z = Vector.ConditionalSelect(integrationMask, velocity.Linear.Z + linearChange.Z, velocity.Linear.Z); - velocity.Angular.X = Vector.ConditionalSelect(integrationMask, velocity.Angular.X + angularChange.X, velocity.Linear.X); - velocity.Angular.Y = Vector.ConditionalSelect(integrationMask, velocity.Angular.Y + angularChange.Y, velocity.Linear.Y); - velocity.Angular.Z = Vector.ConditionalSelect(integrationMask, velocity.Angular.Z + angularChange.Z, velocity.Linear.Z); + Vector3Wide.ConditionalSelect(integrationMask, velocity.Linear, previousVelocity.Linear, out velocity.Linear); + Vector3Wide.ConditionalSelect(integrationMask, velocity.Angular, previousVelocity.Angular, out velocity.Angular); } + public static unsafe void GatherAndIntegrate( + Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, ref IndexSet integrationFlags, float dt, int workerIndex, int bundleIndex, + ref Vector bodyIndices, int count, out Vector3Wide position, out QuaternionWide orientation, out BodyVelocityWide velocity, out BodyInertiaWide inertia) + where TIntegratorCallbacks : struct, IPoseIntegratorCallbacks + { + bodies.GatherMotionState(ref bodyIndices, count, out position, out orientation, out velocity); + if (BundleShouldIntegrate(bundleIndex, integrationFlags, out var integrationMask)) + { + //Note that if we take this codepath, the integration routine will reconstruct the world inertias from local inertia given the current pose. + //The changes to pose and velocity for integration inactive lanes will be masked out, so it'll just be identical to the world inertia if we had gathered it. + //Given that we're running the instructions in a bundle to build it, there's no reason to go out of our way to gather the world inertia. + bodies.GatherLocalInertia(ref bodyIndices, count, out var localInertia); + IntegratePoseAndVelocity(ref integratorCallbacks, ref bodyIndices, localInertia, dt, integrationMask, ref position, ref orientation, ref velocity, workerIndex, out inertia); + //The scatter will be able to ignore any lanes which have a zeroed integration mask. + bodies.ScatterMotionStateAndInertia(position, orientation, inertia, integrationMask, ref bodyIndices, count); + } + else + { + bodies.GatherWorldInertia(ref bodyIndices, count, out inertia); + } + } public unsafe override void WarmStart2( ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) @@ -442,11 +464,11 @@ public unsafe override void WarmStart2( ref var references = ref bodyReferencesBundles[i]; var count = GetCountInBundle(ref typeBatch, i); Prefetch(warmStartPrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, ref inertias, i, exclusiveEndBundle); - bodies.GatherState(ref references, count, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB); - if (BundleShouldIntegrate(i, integrationFlags[0], out var integrationMaskA)) - IntegratePoseAndVelocity(ref integratorCallbacks, ref references.IndexA, localInertiaA, dt, integrationMaskA, ref positionA, ref orientationA, ref wsvA, workerIndex, out inertiaA); - if (BundleShouldIntegrate(i, integrationFlags[1], out var integrationMaskB)) - IntegratePoseAndVelocity(ref integratorCallbacks, ref references.IndexB, localInertiaB, dt, integrationMaskB, ref positionB, ref orientationB, ref wsvB, workerIndex, out inertiaB); + GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags[0], dt, workerIndex, i, ref references.IndexA, count, + out var positionA, out var orientationA, out var wsvA, out var inertiaA); + GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags[1], dt, workerIndex, i, ref references.IndexB, count, + out var positionB, out var orientationB, out var wsvB, out var inertiaB); + var ab = positionB - positionA; if (typeof(TConstraintFunctions) == typeof(WeldFunctions)) { default(WeldFunctions).WarmStart2(orientationA, inertiaA, ab, orientationB, inertiaB, @@ -465,6 +487,8 @@ public unsafe override void WarmStart2( bodies.ScatterVelocities(ref wsvA, ref wsvB, ref references, count); } } + + public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { var prestepBundles = typeBatch.PrestepData.As(); diff --git a/BepuPhysics/PoseIntegrator.cs b/BepuPhysics/PoseIntegrator.cs index cce2f8880..915ddc902 100644 --- a/BepuPhysics/PoseIntegrator.cs +++ b/BepuPhysics/PoseIntegrator.cs @@ -80,14 +80,11 @@ public interface IPoseIntegratorCallbacks /// Body's current local inertia. /// Mask indicating which lanes are active in the bundle. Active lanes will contain 0xFFFFFFFF, inactive lanes will contain 0. /// Index of the worker thread processing this bundle. - /// Current velocity of bodies in the bundle. /// Durations to integrate the velocity over. Can vary over lanes. - /// Change to apply to the linear velocity of bodies in the bundle. Any changes in inactive lanes will be ignored. - /// Change to apply to the angular velocity of bodies in the bundle. Any changes in inactive lanes will be ignored. + /// Velocity of bodies in the bundle. Any changes to lanes which are not active by the integrationMask will be discarded. void IntegrateVelocity( ReadOnlySpan bodyIndices, in Vector3Wide position, in QuaternionWide orientation, in BodyInertiaWide localInertia, - in Vector integrationMask, int workerIndex, in BodyVelocityWide velocity, in Vector dt, - out Vector3Wide linearChange, out Vector3Wide angularChange); + in Vector integrationMask, int workerIndex, in Vector dt, ref BodyVelocityWide velocity); } /// diff --git a/BepuUtilities/QuaternionWide.cs b/BepuUtilities/QuaternionWide.cs index 492752005..117e20b7f 100644 --- a/BepuUtilities/QuaternionWide.cs +++ b/BepuUtilities/QuaternionWide.cs @@ -568,6 +568,26 @@ public static QuaternionWide Conjugate(in QuaternionWide quaternion) return result; } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void ConditionalSelect(in Vector condition, in QuaternionWide left, in QuaternionWide right, out QuaternionWide result) + { + result.X = Vector.ConditionalSelect(condition, left.X, right.X); + result.Y = Vector.ConditionalSelect(condition, left.Y, right.Y); + result.Z = Vector.ConditionalSelect(condition, left.Z, right.Z); + result.W = Vector.ConditionalSelect(condition, left.W, right.W); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static QuaternionWide ConditionalSelect(Vector condition, QuaternionWide left, QuaternionWide right) + { + QuaternionWide result; + result.X = Vector.ConditionalSelect(condition, left.X, right.X); + result.Y = Vector.ConditionalSelect(condition, left.Y, right.Y); + result.Z = Vector.ConditionalSelect(condition, left.Z, right.Z); + result.W = Vector.ConditionalSelect(condition, left.W, right.W); + return result; + } + /// /// Gathers values from the first slot of a wide quaternion and puts them into a narrow representation. /// From 8c713a4a0c7df8bd8f5b74a2d206621ac266efc2 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 11 Jul 2021 18:23:58 -0500 Subject: [PATCH 097/947] Pushed through remaining minimum required refactoring. Bugged. --- BepuPhysics/Bodies_GatherScatter.cs | 116 +++++++++++++++--- .../Constraints/TwoBodyTypeProcessor.cs | 2 +- .../EmbeddedSubsteppingTimestepper2.cs | 8 +- BepuPhysics/Simulation.cs | 58 +++++---- BepuPhysics/Solver_SubsteppingSolve2.cs | 6 - Demos/Demo.cs | 2 +- Demos/DemoCallbacks.cs | 4 + Demos/Demos/PlanetDemo.cs | 6 + Demos/Demos/SimpleSelfContainedDemo.cs | 4 + Demos/Program.cs | 2 +- Demos/SpecializedTests/GyroscopeTestDemo.cs | 4 + 11 files changed, 150 insertions(+), 62 deletions(-) diff --git a/BepuPhysics/Bodies_GatherScatter.cs b/BepuPhysics/Bodies_GatherScatter.cs index 4e8ba38b5..6a4df4feb 100644 --- a/BepuPhysics/Bodies_GatherScatter.cs +++ b/BepuPhysics/Bodies_GatherScatter.cs @@ -200,13 +200,6 @@ public unsafe void GatherMotionState(ref Vector bodyIndices, int count, out position.X = Avx.Permute2x128(o0, o1, 1 | (3 << 4)).AsVector(); position.Y = Avx.Permute2x128(o4, o5, 1 | (3 << 4)).AsVector(); position.Z = Avx.Permute2x128(o2, o3, 1 | (3 << 4)).AsVector(); - //inertia.InverseMass = Avx.Permute2x128(o6, o7, 1 | (3 << 4)).AsVector(); - //inertia.InverseInertiaTensor.XX = inertia.InverseMass; - //inertia.InverseInertiaTensor.YY = inertia.InverseMass; - //inertia.InverseInertiaTensor.ZZ = inertia.InverseMass; - //inertia.InverseInertiaTensor.YX = default; - //inertia.InverseInertiaTensor.ZX = default; - //inertia.InverseInertiaTensor.ZY = default; } { @@ -257,7 +250,7 @@ public unsafe void GatherMotionState(ref Vector bodyIndices, int count, out } - //[MethodImpl(MethodImplOptions.AggressiveInlining)] + [MethodImpl(MethodImplOptions.AggressiveInlining)] unsafe void GatherInertia(ref Vector bodyIndices, int count, int offsetInFloats, out BodyInertiaWide inertia) { var inertias = ActiveSet.Inertias.Memory; @@ -347,14 +340,6 @@ public unsafe void GatherState(ref TwoBodyReferences references, int count, out Vector3Wide ab, out QuaternionWide orientationB, out BodyVelocityWide velocityB, out BodyInertiaWide inertiaB) { - Unsafe.SkipInit(out Vector3Wide positionA); - Unsafe.SkipInit(out Vector3Wide positionB); - Unsafe.SkipInit(out orientationA); - Unsafe.SkipInit(out orientationB); - Unsafe.SkipInit(out velocityA); - Unsafe.SkipInit(out velocityB); - Unsafe.SkipInit(out inertiaA); - Unsafe.SkipInit(out inertiaB); Debug.Assert(count >= 0 && count <= Vector.Count); //Grab the base references for the body indices. Note that we make use of the references memory layout again. ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); @@ -362,9 +347,9 @@ public unsafe void GatherState(ref TwoBodyReferences references, int count, ref var states = ref ActiveSet.MotionStates; - GatherMotionState(ref references.IndexA, count, out positionA, out orientationA, out velocityA); + GatherMotionState(ref references.IndexA, count, out var positionA, out orientationA, out velocityA); GatherWorldInertia(ref references.IndexA, count, out inertiaA); - GatherMotionState(ref references.IndexB, count, out positionB, out orientationB, out velocityB); + GatherMotionState(ref references.IndexB, count, out var positionB, out orientationB, out velocityB); GatherWorldInertia(ref references.IndexA, count, out inertiaB); //for (int i = 0; i < count; ++i) @@ -533,6 +518,100 @@ public void GatherState(ref FourBodyReferences references, int count, Vector3Wide.Subtract(positionC, positionA, out ad); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public unsafe void ScatterPoseAndInertia( + ref Vector3Wide position, ref QuaternionWide orientation, ref BodyInertiaWide inertia, ref Vector references, ref Vector mask) + { + if (Avx.IsSupported) + { + { + var m0 = orientation.X.AsVector256(); + var m1 = orientation.Y.AsVector256(); + var m2 = orientation.Z.AsVector256(); + var m3 = orientation.W.AsVector256(); + var m4 = position.Y.AsVector256(); + var m5 = position.Y.AsVector256(); + var m6 = position.Z.AsVector256(); + + var n0 = Avx.UnpackLow(m0, m1); + var n1 = Avx.UnpackLow(m2, m3); + var n2 = Avx.UnpackLow(m4, m5); + var n3 = Avx.UnpackLow(m6, m6); + var n4 = Avx.UnpackHigh(m0, m1); + var n5 = Avx.UnpackHigh(m2, m3); + var n6 = Avx.UnpackHigh(m4, m5); + var n7 = Avx.UnpackHigh(m6, m6); //Laze alert. + + var o0 = Avx.Shuffle(n0, n1, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o1 = Avx.Shuffle(n2, n3, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o2 = Avx.Shuffle(n4, n5, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o3 = Avx.Shuffle(n6, n7, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o4 = Avx.Shuffle(n0, n1, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o5 = Avx.Shuffle(n2, n3, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o6 = Avx.Shuffle(n4, n5, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o7 = Avx.Shuffle(n6, n7, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + + var maskPointer = (int*)Unsafe.AsPointer(ref mask); + var indices = (int*)Unsafe.AsPointer(ref references); + var motionStates = ActiveSet.MotionStates.Memory; + if (maskPointer[0] != 0) Avx.StoreAligned((float*)(motionStates + indices[0]), Avx.Permute2x128(o0, o1, 0 | (2 << 4))); + if (maskPointer[1] != 0) Avx.StoreAligned((float*)(motionStates + indices[1]), Avx.Permute2x128(o4, o5, 0 | (2 << 4))); + if (maskPointer[2] != 0) Avx.StoreAligned((float*)(motionStates + indices[2]), Avx.Permute2x128(o2, o3, 0 | (2 << 4))); + if (maskPointer[3] != 0) Avx.StoreAligned((float*)(motionStates + indices[3]), Avx.Permute2x128(o6, o7, 0 | (2 << 4))); + if (maskPointer[4] != 0) Avx.StoreAligned((float*)(motionStates + indices[4]), Avx.Permute2x128(o0, o1, 1 | (3 << 4))); + if (maskPointer[5] != 0) Avx.StoreAligned((float*)(motionStates + indices[5]), Avx.Permute2x128(o4, o5, 1 | (3 << 4))); + if (maskPointer[6] != 0) Avx.StoreAligned((float*)(motionStates + indices[6]), Avx.Permute2x128(o2, o3, 1 | (3 << 4))); + if (maskPointer[7] != 0) Avx.StoreAligned((float*)(motionStates + indices[7]), Avx.Permute2x128(o6, o7, 1 | (3 << 4))); + } + { + var m0 = inertia.InverseInertiaTensor.XX.AsVector256(); + var m1 = inertia.InverseInertiaTensor.YX.AsVector256(); + var m2 = inertia.InverseInertiaTensor.YY.AsVector256(); + var m3 = inertia.InverseInertiaTensor.ZX.AsVector256(); + var m4 = inertia.InverseInertiaTensor.ZY.AsVector256(); + var m5 = inertia.InverseInertiaTensor.ZZ.AsVector256(); + var m6 = inertia.InverseMass.AsVector256(); + + var n0 = Avx.UnpackLow(m0, m1); + var n1 = Avx.UnpackLow(m2, m3); + var n2 = Avx.UnpackLow(m4, m5); + var n3 = Avx.UnpackLow(m6, m6); + var n4 = Avx.UnpackHigh(m0, m1); + var n5 = Avx.UnpackHigh(m2, m3); + var n6 = Avx.UnpackHigh(m4, m5); + var n7 = Avx.UnpackHigh(m6, m6); //Laze alert. + + var o0 = Avx.Shuffle(n0, n1, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o1 = Avx.Shuffle(n2, n3, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o2 = Avx.Shuffle(n4, n5, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o3 = Avx.Shuffle(n6, n7, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o4 = Avx.Shuffle(n0, n1, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o5 = Avx.Shuffle(n2, n3, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o6 = Avx.Shuffle(n4, n5, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o7 = Avx.Shuffle(n6, n7, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + + var maskPointer = (int*)Unsafe.AsPointer(ref mask); + var indices = (int*)Unsafe.AsPointer(ref references); + //Note the offset; we're scattering into the world inertias. + var inertias = ActiveSet.Inertias.Memory; + if (maskPointer[0] != 0) Avx.StoreAligned((float*)(inertias + indices[0]) + 8, Avx.Permute2x128(o0, o1, 0 | (2 << 4))); + if (maskPointer[1] != 0) Avx.StoreAligned((float*)(inertias + indices[1]) + 8, Avx.Permute2x128(o4, o5, 0 | (2 << 4))); + if (maskPointer[2] != 0) Avx.StoreAligned((float*)(inertias + indices[2]) + 8, Avx.Permute2x128(o2, o3, 0 | (2 << 4))); + if (maskPointer[3] != 0) Avx.StoreAligned((float*)(inertias + indices[3]) + 8, Avx.Permute2x128(o6, o7, 0 | (2 << 4))); + if (maskPointer[4] != 0) Avx.StoreAligned((float*)(inertias + indices[4]) + 8, Avx.Permute2x128(o0, o1, 1 | (3 << 4))); + if (maskPointer[5] != 0) Avx.StoreAligned((float*)(inertias + indices[5]) + 8, Avx.Permute2x128(o4, o5, 1 | (3 << 4))); + if (maskPointer[6] != 0) Avx.StoreAligned((float*)(inertias + indices[6]) + 8, Avx.Permute2x128(o2, o3, 1 | (3 << 4))); + if (maskPointer[7] != 0) Avx.StoreAligned((float*)(inertias + indices[7]) + 8, Avx.Permute2x128(o6, o7, 1 | (3 << 4))); + } + } + else + { + } + } + + + [MethodImpl(MethodImplOptions.AggressiveInlining)] private unsafe void ScatterVelocities(ref BodyVelocityWide sourceVelocities, ref int baseIndex, int innerIndex) { @@ -601,7 +680,6 @@ private unsafe void TransposeScatterVelocities(ref BodyVelocityWide sourceVeloci } } - /// /// Scatters velocities for one body bundle into the active body set. /// diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index b4991091e..0abfc25ad 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -439,7 +439,7 @@ public static unsafe void GatherAndIntegrate( bodies.GatherLocalInertia(ref bodyIndices, count, out var localInertia); IntegratePoseAndVelocity(ref integratorCallbacks, ref bodyIndices, localInertia, dt, integrationMask, ref position, ref orientation, ref velocity, workerIndex, out inertia); //The scatter will be able to ignore any lanes which have a zeroed integration mask. - bodies.ScatterMotionStateAndInertia(position, orientation, inertia, integrationMask, ref bodyIndices, count); + bodies.ScatterPoseAndInertia(ref position, ref orientation, ref inertia, ref bodyIndices, ref integrationMask); } else { diff --git a/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs b/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs index 2c8d4a19a..57a8539dc 100644 --- a/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs +++ b/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs @@ -90,16 +90,16 @@ public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDi simulation.IncrementallyUpdateContactConstraints(substepDt, threadDispatcher); ContactConstraintsUpdatedForSubstep?.Invoke(substepIndex, substepDt, threadDispatcher); } - simulation.IntegrateVelocitiesAndUpdateInertias(substepDt, threadDispatcher); - VelocitiesIntegrated?.Invoke(substepIndex, substepDt, threadDispatcher); + //simulation.IntegrateVelocitiesAndUpdateInertias(substepDt, threadDispatcher); + //VelocitiesIntegrated?.Invoke(substepIndex, substepDt, threadDispatcher); simulation.Profiler.Start(simulation.Solver); simulation.Solver.SolveStep2(substepDt, threadDispatcher); simulation.Profiler.End(simulation.Solver); ConstraintsSolved?.Invoke(substepIndex, substepDt, threadDispatcher); - simulation.IntegratePoses(substepDt, threadDispatcher); - PosesIntegrated?.Invoke(substepIndex, substepDt, threadDispatcher); + //simulation.IntegratePoses(substepDt, threadDispatcher); + //PosesIntegrated?.Invoke(substepIndex, substepDt, threadDispatcher); SubstepEnded?.Invoke(substepIndex, substepDt, threadDispatcher); } SubstepsComplete?.Invoke(dt, threadDispatcher); diff --git a/BepuPhysics/Simulation.cs b/BepuPhysics/Simulation.cs index b2ae97f80..22d2bc18b 100644 --- a/BepuPhysics/Simulation.cs +++ b/BepuPhysics/Simulation.cs @@ -59,35 +59,7 @@ public partial class Simulation : IDisposable /// public bool Deterministic { get; set; } - protected Simulation(BufferPool bufferPool, SimulationAllocationSizes initialAllocationSizes, int solverIterationCount, int solverFallbackBatchThreshold, ITimestepper timestepper) - { - BufferPool = bufferPool; - Shapes = new Shapes(bufferPool, initialAllocationSizes.ShapesPerType); - BroadPhase = new BroadPhase(bufferPool, initialAllocationSizes.Bodies, initialAllocationSizes.Bodies + initialAllocationSizes.Statics); - Bodies = new Bodies(bufferPool, Shapes, BroadPhase, - initialAllocationSizes.Bodies, - initialAllocationSizes.Islands, - initialAllocationSizes.ConstraintCountPerBodyEstimate); - Statics = new Statics(bufferPool, Shapes, Bodies, BroadPhase, initialAllocationSizes.Statics); - - Solver = new Solver(Bodies, BufferPool, solverIterationCount, solverFallbackBatchThreshold, - initialCapacity: initialAllocationSizes.Constraints, - initialIslandCapacity: initialAllocationSizes.Islands, - minimumCapacityPerTypeBatch: initialAllocationSizes.ConstraintsPerTypeBatch); - constraintRemover = new ConstraintRemover(BufferPool, Bodies, Solver); - Sleeper = new IslandSleeper(Bodies, Solver, BroadPhase, constraintRemover, BufferPool); - Awakener = new IslandAwakener(Bodies, Statics, Solver, BroadPhase, Sleeper, bufferPool); - Statics.awakener = Awakener; - Solver.awakener = Awakener; - Bodies.Initialize(Solver, Awakener, Sleeper); - SolverBatchCompressor = new BatchCompressor(Solver, Bodies); - BodyLayoutOptimizer = new BodyLayoutOptimizer(Bodies, BroadPhase, Solver, bufferPool); - ConstraintLayoutOptimizer = new ConstraintLayoutOptimizer(Bodies, Solver); - Timestepper = timestepper; - - } - - /// + /// /// Constructs a simulation supporting dynamic movement and constraints with the specified narrow phase callbacks. /// /// Buffer pool used to fill persistent structures and main thread ephemeral resources across the engine. @@ -117,9 +89,35 @@ public static Simulation Create }; } - var simulation = new Simulation(bufferPool, initialAllocationSizes.Value, solverIterationCount, solverFallbackBatchThreshold, timestepper); + //var simulation = new Simulation(bufferPool, initialAllocationSizes.Value, solverIterationCount, solverFallbackBatchThreshold, timestepper); + var simulation = new Simulation(); + simulation.BufferPool = bufferPool; + simulation.Shapes = new Shapes(bufferPool, initialAllocationSizes.Value.ShapesPerType); + simulation.BroadPhase = new BroadPhase(bufferPool, initialAllocationSizes.Value.Bodies, initialAllocationSizes.Value.Bodies + initialAllocationSizes.Value.Statics); + simulation.Bodies = new Bodies(bufferPool, simulation.Shapes, simulation.BroadPhase, + initialAllocationSizes.Value.Bodies, + initialAllocationSizes.Value.Islands, + initialAllocationSizes.Value.ConstraintCountPerBodyEstimate); + simulation.Statics = new Statics(bufferPool, simulation.Shapes, simulation.Bodies, simulation.BroadPhase, initialAllocationSizes.Value.Statics); + var poseIntegrator = new PoseIntegrator(simulation.Bodies, simulation.Shapes, simulation.BroadPhase, poseIntegratorCallbacks); simulation.PoseIntegrator = poseIntegrator; + + simulation.Solver = new Solver(simulation.Bodies, simulation.BufferPool, solverIterationCount, solverFallbackBatchThreshold, + initialCapacity: initialAllocationSizes.Value.Constraints, + initialIslandCapacity: initialAllocationSizes.Value.Islands, + minimumCapacityPerTypeBatch: initialAllocationSizes.Value.ConstraintsPerTypeBatch, poseIntegrator); + simulation.constraintRemover = new ConstraintRemover(simulation.BufferPool, simulation.Bodies, simulation.Solver); + simulation.Sleeper = new IslandSleeper(simulation.Bodies, simulation.Solver, simulation.BroadPhase, simulation.constraintRemover, simulation.BufferPool); + simulation.Awakener = new IslandAwakener(simulation.Bodies, simulation.Statics, simulation.Solver, simulation.BroadPhase, simulation.Sleeper, bufferPool); + simulation.Statics.awakener = simulation.Awakener; + simulation.Solver.awakener = simulation.Awakener; + simulation.Bodies.Initialize(simulation.Solver, simulation.Awakener, simulation.Sleeper); + simulation.SolverBatchCompressor = new BatchCompressor(simulation.Solver, simulation.Bodies); + simulation.BodyLayoutOptimizer = new BodyLayoutOptimizer(simulation.Bodies, simulation.BroadPhase, simulation.Solver, bufferPool); + simulation.ConstraintLayoutOptimizer = new ConstraintLayoutOptimizer(simulation.Bodies, simulation.Solver); + simulation.Timestepper = timestepper; + var narrowPhase = new NarrowPhase(simulation, DefaultTypes.CreateDefaultCollisionTaskRegistry(), DefaultTypes.CreateDefaultSweepTaskRegistry(), narrowPhaseCallbacks, initialAllocationSizes.Value.Islands + 1); diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index 66f8a372f..79857e7bd 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -269,10 +269,4 @@ public override void SolveStep2(float dt, IThreadDispatcher threadDispatcher = n } } } - - public partial class Solver - { - - - } } diff --git a/Demos/Demo.cs b/Demos/Demo.cs index f00656836..aba8044d8 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -62,7 +62,7 @@ public virtual void Update(Window window, Camera camera, Input input, float dt) //fully decouple simulation and rendering rates across different threads. //(In either case, you'd also want to interpolate or extrapolate simulation results during rendering for smoothness.) //Note that taking steps of variable length can reduce stability. Gradual or one-off changes can work reasonably well. - Simulation.Timestep(1 / 60f, ThreadDispatcher); + Simulation.Timestep(1 / 60f); ////Here's an example of how it would look to use more frequent updates, but still with a fixed amount of time simulated per update call: //const float timeToSimulate = 1 / 60f; diff --git a/Demos/DemoCallbacks.cs b/Demos/DemoCallbacks.cs index a9e6e0745..40d3d2d1f 100644 --- a/Demos/DemoCallbacks.cs +++ b/Demos/DemoCallbacks.cs @@ -80,6 +80,10 @@ public void IntegrateVelocity(int bodyIndex, in RigidPose pose, in BodyInertia l //This is also a handy spot to implement things like position dependent gravity or per-body damping. } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void IntegrateVelocity(ReadOnlySpan bodyIndices, in Vector3Wide position, in QuaternionWide orientation, in BodyInertiaWide localInertia, in Vector integrationMask, int workerIndex, in Vector dt, ref BodyVelocityWide velocity) + { + } } public unsafe struct DemoNarrowPhaseCallbacks : INarrowPhaseCallbacks { diff --git a/Demos/Demos/PlanetDemo.cs b/Demos/Demos/PlanetDemo.cs index c56008915..12685f5d1 100644 --- a/Demos/Demos/PlanetDemo.cs +++ b/Demos/Demos/PlanetDemo.cs @@ -49,8 +49,13 @@ public void IntegrateVelocity(int bodyIndex, in RigidPose pose, in BodyInertia l velocity.Linear -= gravityDt * offset / MathF.Max(1f, distance * distance * distance); } } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void IntegrateVelocity(ReadOnlySpan bodyIndices, in Vector3Wide position, in QuaternionWide orientation, in BodyInertiaWide localInertia, in Vector integrationMask, int workerIndex, in Vector dt, ref BodyVelocityWide velocity) + { + } } + public unsafe override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(0, 0, -300); @@ -85,5 +90,6 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) } + } } diff --git a/Demos/Demos/SimpleSelfContainedDemo.cs b/Demos/Demos/SimpleSelfContainedDemo.cs index a64d3844a..59e53db22 100644 --- a/Demos/Demos/SimpleSelfContainedDemo.cs +++ b/Demos/Demos/SimpleSelfContainedDemo.cs @@ -174,6 +174,10 @@ public void IntegrateVelocity(int bodyIndex, in RigidPose pose, in BodyInertia l velocity.Linear = velocity.Linear + gravityDt; } } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void IntegrateVelocity(ReadOnlySpan bodyIndices, in Vector3Wide position, in QuaternionWide orientation, in BodyInertiaWide localInertia, in Vector integrationMask, int workerIndex, in Vector dt, ref BodyVelocityWide velocity) + { + } } diff --git a/Demos/Program.cs b/Demos/Program.cs index 6c769ece5..7118826dd 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -19,7 +19,7 @@ static void Main(string[] args) { content = ContentArchive.Load(stream); } - HeadlessTest.Test(content, 4, 32, 512); + //HeadlessTest.Test(content, 4, 32, 512); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); diff --git a/Demos/SpecializedTests/GyroscopeTestDemo.cs b/Demos/SpecializedTests/GyroscopeTestDemo.cs index 77a05cebf..95ed8f4a7 100644 --- a/Demos/SpecializedTests/GyroscopeTestDemo.cs +++ b/Demos/SpecializedTests/GyroscopeTestDemo.cs @@ -41,6 +41,10 @@ public void IntegrateVelocity(int bodyIndex, in RigidPose pose, in BodyInertia l { innerCallbacks.IntegrateVelocity(bodyIndex, pose, localInertia, workerIndex, ref velocity); } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void IntegrateVelocity(ReadOnlySpan bodyIndices, in Vector3Wide position, in QuaternionWide orientation, in BodyInertiaWide localInertia, in Vector integrationMask, int workerIndex, in Vector dt, ref BodyVelocityWide velocity) + { + } } From 5a42882c7d2754f37e94e66e0ce6d5222b8f9131 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 12 Jul 2021 15:39:56 -0500 Subject: [PATCH 098/947] Multiple tinybugs fixed. Twobody-only solving now works as expected. --- BepuPhysics/Bodies_GatherScatter.cs | 10 +++++----- BepuPhysics/Constraints/TwoBodyTypeProcessor.cs | 10 +++++----- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/BepuPhysics/Bodies_GatherScatter.cs b/BepuPhysics/Bodies_GatherScatter.cs index 6a4df4feb..737477256 100644 --- a/BepuPhysics/Bodies_GatherScatter.cs +++ b/BepuPhysics/Bodies_GatherScatter.cs @@ -350,7 +350,7 @@ public unsafe void GatherState(ref TwoBodyReferences references, int count, GatherMotionState(ref references.IndexA, count, out var positionA, out orientationA, out velocityA); GatherWorldInertia(ref references.IndexA, count, out inertiaA); GatherMotionState(ref references.IndexB, count, out var positionB, out orientationB, out velocityB); - GatherWorldInertia(ref references.IndexA, count, out inertiaB); + GatherWorldInertia(ref references.IndexB, count, out inertiaB); //for (int i = 0; i < count; ++i) //{ @@ -530,14 +530,14 @@ public unsafe void ScatterPoseAndInertia( var m1 = orientation.Y.AsVector256(); var m2 = orientation.Z.AsVector256(); var m3 = orientation.W.AsVector256(); - var m4 = position.Y.AsVector256(); + var m4 = position.X.AsVector256(); var m5 = position.Y.AsVector256(); var m6 = position.Z.AsVector256(); var n0 = Avx.UnpackLow(m0, m1); var n1 = Avx.UnpackLow(m2, m3); var n2 = Avx.UnpackLow(m4, m5); - var n3 = Avx.UnpackLow(m6, m6); + var n3 = Avx.UnpackLow(m6, m6); //Laze alert. var n4 = Avx.UnpackHigh(m0, m1); var n5 = Avx.UnpackHigh(m2, m3); var n6 = Avx.UnpackHigh(m4, m5); @@ -562,7 +562,7 @@ public unsafe void ScatterPoseAndInertia( if (maskPointer[4] != 0) Avx.StoreAligned((float*)(motionStates + indices[4]), Avx.Permute2x128(o0, o1, 1 | (3 << 4))); if (maskPointer[5] != 0) Avx.StoreAligned((float*)(motionStates + indices[5]), Avx.Permute2x128(o4, o5, 1 | (3 << 4))); if (maskPointer[6] != 0) Avx.StoreAligned((float*)(motionStates + indices[6]), Avx.Permute2x128(o2, o3, 1 | (3 << 4))); - if (maskPointer[7] != 0) Avx.StoreAligned((float*)(motionStates + indices[7]), Avx.Permute2x128(o6, o7, 1 | (3 << 4))); + if (maskPointer[7] != 0) Avx.StoreAligned((float*)(motionStates + indices[7]), Avx.Permute2x128(o6, o7, 1 | (3 << 4))); } { var m0 = inertia.InverseInertiaTensor.XX.AsVector256(); @@ -576,7 +576,7 @@ public unsafe void ScatterPoseAndInertia( var n0 = Avx.UnpackLow(m0, m1); var n1 = Avx.UnpackLow(m2, m3); var n2 = Avx.UnpackLow(m4, m5); - var n3 = Avx.UnpackLow(m6, m6); + var n3 = Avx.UnpackLow(m6, m6); //Laze alert. var n4 = Avx.UnpackHigh(m0, m1); var n5 = Avx.UnpackHigh(m2, m3); var n6 = Avx.UnpackHigh(m4, m5); diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 0abfc25ad..9507c7be6 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -355,9 +355,10 @@ public static bool BundleShouldIntegrate(int bundleIndex, in IndexSet integratio Debug.Assert(Vector.Count <= 32, "Wait, what? The integration mask isn't big enough to handle a vector this big."); var constraintStartIndex = bundleIndex * Vector.Count; var flagBundleIndex = constraintStartIndex >> 6; - var flagInnerIndex = constraintStartIndex - flagBundleIndex; - var scalarIntegrationMask = ((int)(integrationFlags.Flags[flagBundleIndex] >> flagInnerIndex)) & BundleIndexing.VectorMask; - if (scalarIntegrationMask == BundleIndexing.VectorMask) + var flagInnerIndex = constraintStartIndex - (flagBundleIndex << 6); + var flagMask = (1 << Vector.Count) - 1; + var scalarIntegrationMask = ((int)(integrationFlags.Flags[flagBundleIndex] >> flagInnerIndex)) & flagMask; + if (scalarIntegrationMask == flagMask) { //No need to carefully expand a bitstring into a vector mask if we know that a single broadcast will suffice. integrationMask = new Vector(-1); @@ -371,7 +372,7 @@ public static bool BundleShouldIntegrate(int bundleIndex, in IndexSet integratio { mask[i] = (scalarIntegrationMask & (1 << i)) > 0 ? -1 : 0; } - integrationMask = new Vector(scalarIntegrationMask); + integrationMask = new Vector(mask); return true; } integrationMask = default; @@ -417,7 +418,6 @@ public static unsafe void IntegratePoseAndVelocity( QuaternionWide.ConditionalSelect(integrationMask, newOrientation, orientation, out orientation); PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, orientation, out inertia.InverseInertiaTensor); } - integratorCallbacks.IntegrateVelocity(new ReadOnlySpan(Unsafe.AsPointer(ref bodyIndices), Vector.Count), position, orientation, localInertia, integrationMask, workerIndex, new Vector(dt), ref velocity); //It would be annoying to make the user handle masking velocity writes to inactive lanes, so we handle it internally. Vector3Wide.ConditionalSelect(integrationMask, velocity.Linear, previousVelocity.Linear, out velocity.Linear); From fe5008f330fa84f209bf4b7c4b90f9ff4046a5f5 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 12 Jul 2021 17:46:26 -0500 Subject: [PATCH 099/947] Added batch 0 optimization- no condition required. --- BepuPhysics/Bodies_GatherScatter.cs | 67 +++++++++++----- .../Constraints/FourBodyTypeProcessor.cs | 2 +- .../Constraints/IBatchIntegrationMode.cs | 37 +++++++++ .../Constraints/OneBodyTypeProcessor.cs | 2 +- .../Constraints/ThreeBodyTypeProcessor.cs | 2 +- .../Constraints/TwoBodyTypeProcessor.cs | 76 +++++++++++++++---- BepuPhysics/Constraints/TypeProcessor.cs | 8 +- BepuPhysics/PoseIntegrator.cs | 16 ++-- BepuPhysics/Solver_SubsteppingSolve2.cs | 26 ++++++- Demos/Demo.cs | 2 +- Demos/Program.cs | 2 +- 11 files changed, 189 insertions(+), 51 deletions(-) create mode 100644 BepuPhysics/Constraints/IBatchIntegrationMode.cs diff --git a/BepuPhysics/Bodies_GatherScatter.cs b/BepuPhysics/Bodies_GatherScatter.cs index 737477256..b18cc81e3 100644 --- a/BepuPhysics/Bodies_GatherScatter.cs +++ b/BepuPhysics/Bodies_GatherScatter.cs @@ -520,8 +520,9 @@ public void GatherState(ref FourBodyReferences references, int count, [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void ScatterPoseAndInertia( - ref Vector3Wide position, ref QuaternionWide orientation, ref BodyInertiaWide inertia, ref Vector references, ref Vector mask) + public unsafe void ScatterPoseAndInertia( + ref Vector3Wide position, ref QuaternionWide orientation, ref BodyInertiaWide inertia, ref Vector references, int count, ref Vector mask) + where TBatchIntegrationMode : struct, IBatchIntegrationMode { if (Avx.IsSupported) { @@ -555,14 +556,29 @@ public unsafe void ScatterPoseAndInertia( var maskPointer = (int*)Unsafe.AsPointer(ref mask); var indices = (int*)Unsafe.AsPointer(ref references); var motionStates = ActiveSet.MotionStates.Memory; - if (maskPointer[0] != 0) Avx.StoreAligned((float*)(motionStates + indices[0]), Avx.Permute2x128(o0, o1, 0 | (2 << 4))); - if (maskPointer[1] != 0) Avx.StoreAligned((float*)(motionStates + indices[1]), Avx.Permute2x128(o4, o5, 0 | (2 << 4))); - if (maskPointer[2] != 0) Avx.StoreAligned((float*)(motionStates + indices[2]), Avx.Permute2x128(o2, o3, 0 | (2 << 4))); - if (maskPointer[3] != 0) Avx.StoreAligned((float*)(motionStates + indices[3]), Avx.Permute2x128(o6, o7, 0 | (2 << 4))); - if (maskPointer[4] != 0) Avx.StoreAligned((float*)(motionStates + indices[4]), Avx.Permute2x128(o0, o1, 1 | (3 << 4))); - if (maskPointer[5] != 0) Avx.StoreAligned((float*)(motionStates + indices[5]), Avx.Permute2x128(o4, o5, 1 | (3 << 4))); - if (maskPointer[6] != 0) Avx.StoreAligned((float*)(motionStates + indices[6]), Avx.Permute2x128(o2, o3, 1 | (3 << 4))); - if (maskPointer[7] != 0) Avx.StoreAligned((float*)(motionStates + indices[7]), Avx.Permute2x128(o6, o7, 1 | (3 << 4))); + if (typeof(TBatchIntegrationMode) == typeof(BatchShouldAlwaysIntegrate)) + { + //If we are in an 'always integrate' batch, then the mask is not guaranteed to mask out out-of-range lanes, since we don't otherwise need the mask. + Avx.StoreAligned((float*)(motionStates + indices[0]), Avx.Permute2x128(o0, o1, 0 | (2 << 4))); + if (count > 1) Avx.StoreAligned((float*)(motionStates + indices[1]), Avx.Permute2x128(o4, o5, 0 | (2 << 4))); + if (count > 2) Avx.StoreAligned((float*)(motionStates + indices[2]), Avx.Permute2x128(o2, o3, 0 | (2 << 4))); + if (count > 3) Avx.StoreAligned((float*)(motionStates + indices[3]), Avx.Permute2x128(o6, o7, 0 | (2 << 4))); + if (count > 4) Avx.StoreAligned((float*)(motionStates + indices[4]), Avx.Permute2x128(o0, o1, 1 | (3 << 4))); + if (count > 5) Avx.StoreAligned((float*)(motionStates + indices[5]), Avx.Permute2x128(o4, o5, 1 | (3 << 4))); + if (count > 6) Avx.StoreAligned((float*)(motionStates + indices[6]), Avx.Permute2x128(o2, o3, 1 | (3 << 4))); + if (count > 7) Avx.StoreAligned((float*)(motionStates + indices[7]), Avx.Permute2x128(o6, o7, 1 | (3 << 4))); + } + else + { + if (maskPointer[0] != 0) Avx.StoreAligned((float*)(motionStates + indices[0]), Avx.Permute2x128(o0, o1, 0 | (2 << 4))); + if (maskPointer[1] != 0) Avx.StoreAligned((float*)(motionStates + indices[1]), Avx.Permute2x128(o4, o5, 0 | (2 << 4))); + if (maskPointer[2] != 0) Avx.StoreAligned((float*)(motionStates + indices[2]), Avx.Permute2x128(o2, o3, 0 | (2 << 4))); + if (maskPointer[3] != 0) Avx.StoreAligned((float*)(motionStates + indices[3]), Avx.Permute2x128(o6, o7, 0 | (2 << 4))); + if (maskPointer[4] != 0) Avx.StoreAligned((float*)(motionStates + indices[4]), Avx.Permute2x128(o0, o1, 1 | (3 << 4))); + if (maskPointer[5] != 0) Avx.StoreAligned((float*)(motionStates + indices[5]), Avx.Permute2x128(o4, o5, 1 | (3 << 4))); + if (maskPointer[6] != 0) Avx.StoreAligned((float*)(motionStates + indices[6]), Avx.Permute2x128(o2, o3, 1 | (3 << 4))); + if (maskPointer[7] != 0) Avx.StoreAligned((float*)(motionStates + indices[7]), Avx.Permute2x128(o6, o7, 1 | (3 << 4))); + } } { var m0 = inertia.InverseInertiaTensor.XX.AsVector256(); @@ -595,14 +611,29 @@ public unsafe void ScatterPoseAndInertia( var indices = (int*)Unsafe.AsPointer(ref references); //Note the offset; we're scattering into the world inertias. var inertias = ActiveSet.Inertias.Memory; - if (maskPointer[0] != 0) Avx.StoreAligned((float*)(inertias + indices[0]) + 8, Avx.Permute2x128(o0, o1, 0 | (2 << 4))); - if (maskPointer[1] != 0) Avx.StoreAligned((float*)(inertias + indices[1]) + 8, Avx.Permute2x128(o4, o5, 0 | (2 << 4))); - if (maskPointer[2] != 0) Avx.StoreAligned((float*)(inertias + indices[2]) + 8, Avx.Permute2x128(o2, o3, 0 | (2 << 4))); - if (maskPointer[3] != 0) Avx.StoreAligned((float*)(inertias + indices[3]) + 8, Avx.Permute2x128(o6, o7, 0 | (2 << 4))); - if (maskPointer[4] != 0) Avx.StoreAligned((float*)(inertias + indices[4]) + 8, Avx.Permute2x128(o0, o1, 1 | (3 << 4))); - if (maskPointer[5] != 0) Avx.StoreAligned((float*)(inertias + indices[5]) + 8, Avx.Permute2x128(o4, o5, 1 | (3 << 4))); - if (maskPointer[6] != 0) Avx.StoreAligned((float*)(inertias + indices[6]) + 8, Avx.Permute2x128(o2, o3, 1 | (3 << 4))); - if (maskPointer[7] != 0) Avx.StoreAligned((float*)(inertias + indices[7]) + 8, Avx.Permute2x128(o6, o7, 1 | (3 << 4))); + if (typeof(TBatchIntegrationMode) == typeof(BatchShouldAlwaysIntegrate)) + { + //If we are in an 'always integrate' batch, then the mask is not guaranteed to mask out out-of-range lanes, since we don't otherwise need the mask. + Avx.StoreAligned((float*)(inertias + indices[0]) + 8, Avx.Permute2x128(o0, o1, 0 | (2 << 4))); + if (count > 1) Avx.StoreAligned((float*)(inertias + indices[1]) + 8, Avx.Permute2x128(o4, o5, 0 | (2 << 4))); + if (count > 2) Avx.StoreAligned((float*)(inertias + indices[2]) + 8, Avx.Permute2x128(o2, o3, 0 | (2 << 4))); + if (count > 3) Avx.StoreAligned((float*)(inertias + indices[3]) + 8, Avx.Permute2x128(o6, o7, 0 | (2 << 4))); + if (count > 4) Avx.StoreAligned((float*)(inertias + indices[4]) + 8, Avx.Permute2x128(o0, o1, 1 | (3 << 4))); + if (count > 5) Avx.StoreAligned((float*)(inertias + indices[5]) + 8, Avx.Permute2x128(o4, o5, 1 | (3 << 4))); + if (count > 6) Avx.StoreAligned((float*)(inertias + indices[6]) + 8, Avx.Permute2x128(o2, o3, 1 | (3 << 4))); + if (count > 7) Avx.StoreAligned((float*)(inertias + indices[7]) + 8, Avx.Permute2x128(o6, o7, 1 | (3 << 4))); + } + else + { + if (maskPointer[0] != 0) Avx.StoreAligned((float*)(inertias + indices[0]) + 8, Avx.Permute2x128(o0, o1, 0 | (2 << 4))); + if (maskPointer[1] != 0) Avx.StoreAligned((float*)(inertias + indices[1]) + 8, Avx.Permute2x128(o4, o5, 0 | (2 << 4))); + if (maskPointer[2] != 0) Avx.StoreAligned((float*)(inertias + indices[2]) + 8, Avx.Permute2x128(o2, o3, 0 | (2 << 4))); + if (maskPointer[3] != 0) Avx.StoreAligned((float*)(inertias + indices[3]) + 8, Avx.Permute2x128(o6, o7, 0 | (2 << 4))); + if (maskPointer[4] != 0) Avx.StoreAligned((float*)(inertias + indices[4]) + 8, Avx.Permute2x128(o0, o1, 1 | (3 << 4))); + if (maskPointer[5] != 0) Avx.StoreAligned((float*)(inertias + indices[5]) + 8, Avx.Permute2x128(o4, o5, 1 | (3 << 4))); + if (maskPointer[6] != 0) Avx.StoreAligned((float*)(inertias + indices[6]) + 8, Avx.Permute2x128(o2, o3, 1 | (3 << 4))); + if (maskPointer[7] != 0) Avx.StoreAligned((float*)(inertias + indices[7]) + 8, Avx.Permute2x128(o6, o7, 1 | (3 << 4))); + } } } else diff --git a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs index b31aabcc9..c72077b6d 100644 --- a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs @@ -319,7 +319,7 @@ public unsafe override void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodi } - public override void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks poseIntegratorCallbacks, + public override void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks poseIntegratorCallbacks, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { throw new NotImplementedException(); diff --git a/BepuPhysics/Constraints/IBatchIntegrationMode.cs b/BepuPhysics/Constraints/IBatchIntegrationMode.cs new file mode 100644 index 000000000..e37c0844e --- /dev/null +++ b/BepuPhysics/Constraints/IBatchIntegrationMode.cs @@ -0,0 +1,37 @@ +namespace BepuPhysics.Constraints +{ + //These are used as compile time specialization constants. The solver can preprocess constraint references to know whether a given batch has any constraints that need integration. + //The first batch, for example, is known to require integration for every single constraint, since the first batch is necessarily the first time you see any body. + //Similarly, there is a number of batches beyond which no constraints will have any integration responsibilities. + //By marking those ahead of time, we avoid the nonzero cost of checking the integration flags. + + /// + /// Marks a type as determining the integration mode for a solver batch. + /// + public interface IBatchIntegrationMode + { + } + + /// + /// The batch was determined to have only constraints with integration responsibilities, so there's no need to check. + /// + public struct BatchShouldAlwaysIntegrate : IBatchIntegrationMode + { + + } + /// + /// The batch was determined to have no constraints with integration responsibilities, so there's no need to check. + /// + public struct BatchShouldNeverIntegrate : IBatchIntegrationMode + { + + } + /// + /// The batch was determined to have some constraints with integration responsibilities. + /// + public struct BatchShouldConditionallyIntegrate : IBatchIntegrationMode + { + + } + +} diff --git a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs index 4edafea58..47f55c8eb 100644 --- a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs @@ -250,7 +250,7 @@ public unsafe override void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodi - public unsafe override void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks poseIntegratorCallbacks, + public unsafe override void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks poseIntegratorCallbacks, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); diff --git a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs index 8a1108920..5fe12b6ae 100644 --- a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs @@ -295,7 +295,7 @@ public unsafe override void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodi } } - public override void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks poseIntegratorCallbacks, + public override void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks poseIntegratorCallbacks, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { throw new NotImplementedException(); diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 9507c7be6..d5d7e5665 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -380,7 +380,7 @@ public static bool BundleShouldIntegrate(int bundleIndex, in IndexSet integratio } public static unsafe void IntegratePoseAndVelocity( - ref TIntegratorCallbacks integratorCallbacks, ref Vector bodyIndices, in BodyInertiaWide localInertia, float dt, Vector integrationMask, + ref TIntegratorCallbacks integratorCallbacks, ref Vector bodyIndices, int count, in BodyInertiaWide localInertia, float dt, Vector integrationMask, ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocityWide velocity, int workerIndex, out BodyInertiaWide inertia) @@ -418,36 +418,84 @@ public static unsafe void IntegratePoseAndVelocity( QuaternionWide.ConditionalSelect(integrationMask, newOrientation, orientation, out orientation); PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, orientation, out inertia.InverseInertiaTensor); } - integratorCallbacks.IntegrateVelocity(new ReadOnlySpan(Unsafe.AsPointer(ref bodyIndices), Vector.Count), position, orientation, localInertia, integrationMask, workerIndex, new Vector(dt), ref velocity); + integratorCallbacks.IntegrateVelocity(new ReadOnlySpan(Unsafe.AsPointer(ref bodyIndices), count), position, orientation, localInertia, integrationMask, workerIndex, new Vector(dt), ref velocity); //It would be annoying to make the user handle masking velocity writes to inactive lanes, so we handle it internally. Vector3Wide.ConditionalSelect(integrationMask, velocity.Linear, previousVelocity.Linear, out velocity.Linear); Vector3Wide.ConditionalSelect(integrationMask, velocity.Angular, previousVelocity.Angular, out velocity.Angular); + } - + public static unsafe void IntegratePoseAndVelocity( + ref TIntegratorCallbacks integratorCallbacks, ref Vector bodyIndices, int count, in BodyInertiaWide localInertia, float dt, + ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocityWide velocity, + int workerIndex, + out BodyInertiaWide inertia) + where TIntegratorCallbacks : struct, IPoseIntegratorCallbacks + { + //This is identical to the other IntegratePoseAndVelocity, but it avoids any masking because we know ahead of time that the entire bundle is integrating. + position += velocity.Linear * new Vector(dt); + inertia.InverseMass = localInertia.InverseMass; + if (integratorCallbacks.AngularIntegrationMode == AngularIntegrationMode.ConserveMomentum) + { + var previousOrientation = orientation; + PoseIntegration.Integrate(orientation, velocity.Angular, new Vector(dt * 0.5f), out orientation); + PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, orientation, out inertia.InverseInertiaTensor); + PoseIntegration.IntegrateAngularVelocityConserveMomentum(previousOrientation, localInertia, inertia, ref velocity.Angular); + } + else if (integratorCallbacks.AngularIntegrationMode == AngularIntegrationMode.ConserveMomentumWithGyroscopicTorque) + { + PoseIntegration.Integrate(orientation, velocity.Angular, new Vector(dt * 0.5f), out orientation); + PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, orientation, out inertia.InverseInertiaTensor); + PoseIntegration.IntegrateAngularVelocityConserveMomentumWithGyroscopicTorque(orientation, localInertia, inertia, ref velocity.Angular, dt); + } + else + { + PoseIntegration.Integrate(orientation, velocity.Angular, new Vector(dt * 0.5f), out orientation); + PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, orientation, out inertia.InverseInertiaTensor); + } + integratorCallbacks.IntegrateVelocity(new ReadOnlySpan(Unsafe.AsPointer(ref bodyIndices), count), position, orientation, localInertia, new Vector(-1), workerIndex, new Vector(dt), ref velocity); } - public static unsafe void GatherAndIntegrate( + + public static unsafe void GatherAndIntegrate( Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, ref IndexSet integrationFlags, float dt, int workerIndex, int bundleIndex, ref Vector bodyIndices, int count, out Vector3Wide position, out QuaternionWide orientation, out BodyVelocityWide velocity, out BodyInertiaWide inertia) where TIntegratorCallbacks : struct, IPoseIntegratorCallbacks + where TBatchIntegrationMode : struct, IBatchIntegrationMode { bodies.GatherMotionState(ref bodyIndices, count, out position, out orientation, out velocity); - if (BundleShouldIntegrate(bundleIndex, integrationFlags, out var integrationMask)) + if (typeof(TBatchIntegrationMode) == typeof(BatchShouldAlwaysIntegrate)) { - //Note that if we take this codepath, the integration routine will reconstruct the world inertias from local inertia given the current pose. - //The changes to pose and velocity for integration inactive lanes will be masked out, so it'll just be identical to the world inertia if we had gathered it. - //Given that we're running the instructions in a bundle to build it, there's no reason to go out of our way to gather the world inertia. bodies.GatherLocalInertia(ref bodyIndices, count, out var localInertia); - IntegratePoseAndVelocity(ref integratorCallbacks, ref bodyIndices, localInertia, dt, integrationMask, ref position, ref orientation, ref velocity, workerIndex, out inertia); + var integrationMask = new Vector(-1); + //IntegratePoseAndVelocity(ref integratorCallbacks, ref bodyIndices, count, localInertia, dt, integrationMask, ref position, ref orientation, ref velocity, workerIndex, out inertia); + IntegratePoseAndVelocity(ref integratorCallbacks, ref bodyIndices, count, localInertia, dt, ref position, ref orientation, ref velocity, workerIndex, out inertia); //The scatter will be able to ignore any lanes which have a zeroed integration mask. - bodies.ScatterPoseAndInertia(ref position, ref orientation, ref inertia, ref bodyIndices, ref integrationMask); + bodies.ScatterPoseAndInertia(ref position, ref orientation, ref inertia, ref bodyIndices, count, ref integrationMask); } - else + else if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) { bodies.GatherWorldInertia(ref bodyIndices, count, out inertia); } + else + { + Debug.Assert(typeof(TBatchIntegrationMode) == typeof(BatchShouldConditionallyIntegrate)); + if (BundleShouldIntegrate(bundleIndex, integrationFlags, out var integrationMask)) + { + //Note that if we take this codepath, the integration routine will reconstruct the world inertias from local inertia given the current pose. + //The changes to pose and velocity for integration inactive lanes will be masked out, so it'll just be identical to the world inertia if we had gathered it. + //Given that we're running the instructions in a bundle to build it, there's no reason to go out of our way to gather the world inertia. + bodies.GatherLocalInertia(ref bodyIndices, count, out var localInertia); + IntegratePoseAndVelocity(ref integratorCallbacks, ref bodyIndices, count, localInertia, dt, integrationMask, ref position, ref orientation, ref velocity, workerIndex, out inertia); + //The scatter will be able to ignore any lanes which have a zeroed integration mask. + bodies.ScatterPoseAndInertia(ref position, ref orientation, ref inertia, ref bodyIndices, count, ref integrationMask); + } + else + { + bodies.GatherWorldInertia(ref bodyIndices, count, out inertia); + } + } } - public unsafe override void WarmStart2( + public unsafe override void WarmStart2( ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { var prestepBundles = typeBatch.PrestepData.As(); @@ -464,9 +512,9 @@ public unsafe override void WarmStart2( ref var references = ref bodyReferencesBundles[i]; var count = GetCountInBundle(ref typeBatch, i); Prefetch(warmStartPrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, ref inertias, i, exclusiveEndBundle); - GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags[0], dt, workerIndex, i, ref references.IndexA, count, + GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags[0], dt, workerIndex, i, ref references.IndexA, count, out var positionA, out var orientationA, out var wsvA, out var inertiaA); - GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags[1], dt, workerIndex, i, ref references.IndexB, count, + GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags[1], dt, workerIndex, i, ref references.IndexB, count, out var positionB, out var orientationB, out var wsvB, out var inertiaB); var ab = positionB - positionA; if (typeof(TConstraintFunctions) == typeof(WeldFunctions)) diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index ec35e43e7..5ce59279e 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -8,6 +8,8 @@ namespace BepuPhysics.Constraints { + + /// /// Superclass of constraint type batch processors. Responsible for interpreting raw type batches for the purposes of bookkeeping and solving. /// @@ -136,8 +138,10 @@ internal unsafe abstract void CopySleepingToActive( public abstract void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); - public abstract void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks poseIntegratorCallbacks, - float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) where TIntegratorCallbacks : struct, IPoseIntegratorCallbacks; + public abstract void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks poseIntegratorCallbacks, + float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) + where TIntegratorCallbacks : struct, IPoseIntegratorCallbacks + where TBatchIntegrationMode : struct, IBatchIntegrationMode; public abstract void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); diff --git a/BepuPhysics/PoseIntegrator.cs b/BepuPhysics/PoseIntegrator.cs index 915ddc902..e8c0f21fa 100644 --- a/BepuPhysics/PoseIntegrator.cs +++ b/BepuPhysics/PoseIntegrator.cs @@ -78,7 +78,7 @@ public interface IPoseIntegratorCallbacks /// Current body positions. /// Current body orientations. /// Body's current local inertia. - /// Mask indicating which lanes are active in the bundle. Active lanes will contain 0xFFFFFFFF, inactive lanes will contain 0. + /// Mask indicating which lanes are active in the bundle. Active lanes will contain 0xFFFFFFFF, inactive lanes will contain 0. Lanes beyond bodyIndices.Length are undefined. /// Index of the worker thread processing this bundle. /// Durations to integrate the velocity over. Can vary over lanes. /// Velocity of bodies in the bundle. Any changes to lanes which are not active by the integrationMask will be discarded. @@ -139,8 +139,6 @@ public static void Integrate(in Quaternion orientation, in Vector3 angularVeloci [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Integrate(in QuaternionWide start, in Vector3Wide angularVelocity, in Vector halfDt, out QuaternionWide integrated) { - start.Validate(); - angularVelocity.Validate(); Vector3Wide.Length(angularVelocity, out var speed); var halfAngle = speed * halfDt; QuaternionWide q; @@ -150,13 +148,13 @@ public static void Integrate(in QuaternionWide start, in Vector3Wide angularVelo q.Y = angularVelocity.Y * scale; q.Z = angularVelocity.Z * scale; MathHelper.Cos(halfAngle, out q.W); - QuaternionWide.ConcatenateWithoutOverlap(start, q, out var concatenated); - integrated = QuaternionWide.Normalize(concatenated); + QuaternionWide.ConcatenateWithoutOverlap(start, q, out var end); + end = QuaternionWide.Normalize(end); var speedValid = Vector.GreaterThan(speed, new Vector(1e-15f)); - integrated.X = Vector.ConditionalSelect(speedValid, integrated.X, start.X); - integrated.Y = Vector.ConditionalSelect(speedValid, integrated.Y, start.Y); - integrated.Z = Vector.ConditionalSelect(speedValid, integrated.Z, start.Z); - integrated.W = Vector.ConditionalSelect(speedValid, integrated.W, start.W); + integrated.X = Vector.ConditionalSelect(speedValid, end.X, start.X); + integrated.Y = Vector.ConditionalSelect(speedValid, end.Y, start.Y); + integrated.Z = Vector.ConditionalSelect(speedValid, end.Z, start.Z); + integrated.W = Vector.ConditionalSelect(speedValid, end.W, start.W); } [MethodImpl(MethodImplOptions.AggressiveInlining)] diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index 79857e7bd..f3f6d346e 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -50,12 +50,23 @@ struct WarmStartStep2StageFunction : IStageFunction public Solver solver; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Execute(Solver solver, int blockIndex, int workerIndex ) + public void Execute(Solver solver, int blockIndex, int workerIndex) { ref var block = ref this.solver.context.ConstraintBlocks.Blocks[blockIndex]; ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; - typeProcessor.WarmStart2(ref typeBatch, ref this.solver.integrationFlags[block.BatchIndex][block.TypeBatchIndex], this.solver.bodies, ref this.solver.PoseIntegrator.Callbacks, Dt, InverseDt, block.StartBundle, block.End, workerIndex); + if (block.BatchIndex == 0) + { + typeProcessor.WarmStart2( + ref typeBatch, ref this.solver.integrationFlags[block.BatchIndex][block.TypeBatchIndex], this.solver.bodies, ref this.solver.PoseIntegrator.Callbacks, + Dt, InverseDt, block.StartBundle, block.End, workerIndex); + } + else + { + typeProcessor.WarmStart2( + ref typeBatch, ref this.solver.integrationFlags[block.BatchIndex][block.TypeBatchIndex], this.solver.bodies, ref this.solver.PoseIntegrator.Callbacks, + Dt, InverseDt, block.StartBundle, block.End, workerIndex); + } } } //no fallback warmstart; the last constraint batch is always handled by the solve instead, and if the fallback batch exists, it's guaranteed to be the last batch. @@ -248,7 +259,16 @@ public override void SolveStep2(float dt, IThreadDispatcher threadDispatcher = n for (int j = 0; j < batch.TypeBatches.Count; ++j) { ref var typeBatch = ref batch.TypeBatches[j]; - TypeProcessors[typeBatch.TypeId].WarmStart2(ref typeBatch, ref integrationFlagsForBatch[j], bodies, ref PoseIntegrator.Callbacks, dt, inverseDt, 0, typeBatch.BundleCount, 0); + if (i == 0) + { + TypeProcessors[typeBatch.TypeId].WarmStart2(ref typeBatch, ref integrationFlagsForBatch[j], bodies, ref PoseIntegrator.Callbacks, + dt, inverseDt, 0, typeBatch.BundleCount, 0); + } + else + { + TypeProcessors[typeBatch.TypeId].WarmStart2(ref typeBatch, ref integrationFlagsForBatch[j], bodies, ref PoseIntegrator.Callbacks, + dt, inverseDt, 0, typeBatch.BundleCount, 0); + } } } diff --git a/Demos/Demo.cs b/Demos/Demo.cs index aba8044d8..f00656836 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -62,7 +62,7 @@ public virtual void Update(Window window, Camera camera, Input input, float dt) //fully decouple simulation and rendering rates across different threads. //(In either case, you'd also want to interpolate or extrapolate simulation results during rendering for smoothness.) //Note that taking steps of variable length can reduce stability. Gradual or one-off changes can work reasonably well. - Simulation.Timestep(1 / 60f); + Simulation.Timestep(1 / 60f, ThreadDispatcher); ////Here's an example of how it would look to use more frequent updates, but still with a fixed amount of time simulated per update call: //const float timeToSimulate = 1 / 60f; diff --git a/Demos/Program.cs b/Demos/Program.cs index 7118826dd..6c769ece5 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -19,7 +19,7 @@ static void Main(string[] args) { content = ContentArchive.Load(stream); } - //HeadlessTest.Test(content, 4, 32, 512); + HeadlessTest.Test(content, 4, 32, 512); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); From 7ac4e92b2be4705be8b3f07ec477fd48c7e4f386 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 12 Jul 2021 17:47:34 -0500 Subject: [PATCH 100/947] Forgot a necessary vector support test. --- BepuPhysics/Bodies_GatherScatter.cs | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/BepuPhysics/Bodies_GatherScatter.cs b/BepuPhysics/Bodies_GatherScatter.cs index b18cc81e3..f37dd036f 100644 --- a/BepuPhysics/Bodies_GatherScatter.cs +++ b/BepuPhysics/Bodies_GatherScatter.cs @@ -138,7 +138,7 @@ unsafe static void FallbackGatherInertia(int count, BodyInertias* inertias, ref public unsafe void GatherMotionState(ref Vector bodyIndices, int count, out Vector3Wide position, out QuaternionWide orientation, out BodyVelocityWide velocity) { var motionStates = ActiveSet.MotionStates.Memory; - if (Avx.IsSupported) + if (Avx.IsSupported && Vector.Count == 8) { var indices = (int*)Unsafe.AsPointer(ref bodyIndices); @@ -254,7 +254,7 @@ public unsafe void GatherMotionState(ref Vector bodyIndices, int count, out unsafe void GatherInertia(ref Vector bodyIndices, int count, int offsetInFloats, out BodyInertiaWide inertia) { var inertias = ActiveSet.Inertias.Memory; - if (Avx.IsSupported) + if (Avx.IsSupported && Vector.Count == 8) { var indices = (int*)Unsafe.AsPointer(ref bodyIndices); { @@ -524,7 +524,7 @@ public unsafe void ScatterPoseAndInertia( ref Vector3Wide position, ref QuaternionWide orientation, ref BodyInertiaWide inertia, ref Vector references, int count, ref Vector mask) where TBatchIntegrationMode : struct, IBatchIntegrationMode { - if (Avx.IsSupported) + if (Avx.IsSupported && Vector.Count == 8) { { var m0 = orientation.X.AsVector256(); @@ -656,7 +656,7 @@ private unsafe void ScatterVelocities(ref BodyVelocityWide sourceVelocities, ref [MethodImpl(MethodImplOptions.AggressiveInlining)] private unsafe void TransposeScatterVelocities(ref BodyVelocityWide sourceVelocities, MotionState* motionStates, ref Vector references, int count) { - if (Avx.IsSupported) + if (Avx.IsSupported && Vector.Count == 8) { //for (int i = 0; i < 8; ++i) //{ From 7beed6bd8b4948c9c05b336c4b1aa544fa69fcaa Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 13 Jul 2021 10:39:41 -0500 Subject: [PATCH 101/947] Dynamically uniform integration dispatch. --- .../Constraints/TwoBodyTypeProcessor.cs | 67 +++++++++++++------ BepuPhysics/Solver_SubsteppingSolve2.cs | 1 + 2 files changed, 47 insertions(+), 21 deletions(-) diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index d5d7e5665..0bf774e00 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -349,8 +349,15 @@ unsafe static void Prefetch(int prefetchDistance, ref TypeBatch typeBatch, ref B } } + public enum BundleIntegrationMode + { + None = 0, + Partial = 1, + All = 2 + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static bool BundleShouldIntegrate(int bundleIndex, in IndexSet integrationFlags, out Vector integrationMask) + public static BundleIntegrationMode BundleShouldIntegrate(int bundleIndex, in IndexSet integrationFlags, out Vector integrationMask) { Debug.Assert(Vector.Count <= 32, "Wait, what? The integration mask isn't big enough to handle a vector this big."); var constraintStartIndex = bundleIndex * Vector.Count; @@ -362,7 +369,7 @@ public static bool BundleShouldIntegrate(int bundleIndex, in IndexSet integratio { //No need to carefully expand a bitstring into a vector mask if we know that a single broadcast will suffice. integrationMask = new Vector(-1); - return true; + return BundleIntegrationMode.All; } else if (scalarIntegrationMask > 0) { @@ -373,10 +380,10 @@ public static bool BundleShouldIntegrate(int bundleIndex, in IndexSet integratio mask[i] = (scalarIntegrationMask & (1 << i)) > 0 ? -1 : 0; } integrationMask = new Vector(mask); - return true; + return BundleIntegrationMode.Partial; } integrationMask = default; - return false; + return BundleIntegrationMode.None; } public static unsafe void IntegratePoseAndVelocity( @@ -455,6 +462,15 @@ public static unsafe void IntegratePoseAndVelocity( integratorCallbacks.IntegrateVelocity(new ReadOnlySpan(Unsafe.AsPointer(ref bodyIndices), count), position, orientation, localInertia, new Vector(-1), workerIndex, new Vector(dt), ref velocity); } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + private static unsafe void IntegrateUniformly( + Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, float dt, int workerIndex, ref Vector bodyIndices, int count, ref Vector integrationMask, + ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocityWide velocity, out BodyInertiaWide inertia) where TIntegratorCallbacks : struct, IPoseIntegratorCallbacks + { + bodies.GatherLocalInertia(ref bodyIndices, count, out var localInertia); + IntegratePoseAndVelocity(ref integratorCallbacks, ref bodyIndices, count, localInertia, dt, ref position, ref orientation, ref velocity, workerIndex, out inertia); + bodies.ScatterPoseAndInertia(ref position, ref orientation, ref inertia, ref bodyIndices, count, ref integrationMask); + } public static unsafe void GatherAndIntegrate( Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, ref IndexSet integrationFlags, float dt, int workerIndex, int bundleIndex, ref Vector bodyIndices, int count, out Vector3Wide position, out QuaternionWide orientation, out BodyVelocityWide velocity, out BodyInertiaWide inertia) @@ -464,12 +480,8 @@ public static unsafe void GatherAndIntegrate(-1); - //IntegratePoseAndVelocity(ref integratorCallbacks, ref bodyIndices, count, localInertia, dt, integrationMask, ref position, ref orientation, ref velocity, workerIndex, out inertia); - IntegratePoseAndVelocity(ref integratorCallbacks, ref bodyIndices, count, localInertia, dt, ref position, ref orientation, ref velocity, workerIndex, out inertia); - //The scatter will be able to ignore any lanes which have a zeroed integration mask. - bodies.ScatterPoseAndInertia(ref position, ref orientation, ref inertia, ref bodyIndices, count, ref integrationMask); + IntegrateUniformly(bodies, ref integratorCallbacks, dt, workerIndex, ref bodyIndices, count, ref integrationMask, ref position, ref orientation, ref velocity, out inertia); } else if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) { @@ -478,23 +490,36 @@ public static unsafe void GatherAndIntegrate(ref position, ref orientation, ref inertia, ref bodyIndices, count, ref integrationMask); - } - else - { - bodies.GatherWorldInertia(ref bodyIndices, count, out inertia); + case BundleIntegrationMode.All: + { + IntegrateUniformly(bodies, ref integratorCallbacks, dt, workerIndex, ref bodyIndices, count, ref integrationMask, ref position, ref orientation, ref velocity, out inertia); + } + break; + case BundleIntegrationMode.Partial: + { + //Note that if we take this codepath, the integration routine will reconstruct the world inertias from local inertia given the current pose. + //The changes to pose and velocity for integration inactive lanes will be masked out, so it'll just be identical to the world inertia if we had gathered it. + //Given that we're running the instructions in a bundle to build it, there's no reason to go out of our way to gather the world inertia. + bodies.GatherLocalInertia(ref bodyIndices, count, out var localInertia); + IntegratePoseAndVelocity(ref integratorCallbacks, ref bodyIndices, count, localInertia, dt, integrationMask, ref position, ref orientation, ref velocity, workerIndex, out inertia); + //The scatter will be able to ignore any lanes which have a zeroed integration mask. + bodies.ScatterPoseAndInertia(ref position, ref orientation, ref inertia, ref bodyIndices, count, ref integrationMask); + } + break; + default: + { + bodies.GatherWorldInertia(ref bodyIndices, count, out inertia); + } + break; } } } + public unsafe override void WarmStart2( ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index f3f6d346e..eb38557d0 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -9,6 +9,7 @@ using System.Runtime.InteropServices; using System.Text; using System.Threading; +using System.Runtime.Intrinsics.X86; namespace BepuPhysics { From c24e7d613e0b742969b1e1fe6ff2b4146a0d17bc Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 13 Jul 2021 11:55:35 -0500 Subject: [PATCH 102/947] Added vectorized integration mask generator. --- .../Constraints/TwoBodyTypeProcessor.cs | 35 +++++++++++++++---- 1 file changed, 29 insertions(+), 6 deletions(-) diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 0bf774e00..293fe223c 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -5,7 +5,9 @@ using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; +using System.Runtime.Intrinsics; using System.Runtime.Intrinsics.X86; +using System.Runtime.Intrinsics.Arm; namespace BepuPhysics.Constraints { @@ -356,7 +358,7 @@ public enum BundleIntegrationMode All = 2 } - [MethodImpl(MethodImplOptions.AggressiveInlining)] + //[MethodImpl(MethodImplOptions.AggressiveInlining)] public static BundleIntegrationMode BundleShouldIntegrate(int bundleIndex, in IndexSet integrationFlags, out Vector integrationMask) { Debug.Assert(Vector.Count <= 32, "Wait, what? The integration mask isn't big enough to handle a vector this big."); @@ -373,13 +375,33 @@ public static BundleIntegrationMode BundleShouldIntegrate(int bundleIndex, in In } else if (scalarIntegrationMask > 0) { - //TODO: This is bad prototype code. - Span mask = stackalloc int[Vector.Count]; - for (int i = 0; i < mask.Length; ++i) + if (Vector.Count == 4 || Vector.Count == 8) + { + Vector selectors; + if (Vector.Count == 8) + { + selectors = Vector256.Create(1, 2, 4, 8, 16, 32, 64, 128).AsVector(); + } + else + { + selectors = Vector128.Create(1, 2, 4, 8).AsVector(); + } + var scalarBroadcast = new Vector(scalarIntegrationMask); + var selected = Vector.BitwiseAnd(selectors, scalarBroadcast); + integrationMask = Vector.Equals(selected, selectors); + } + else { - mask[i] = (scalarIntegrationMask & (1 << i)) > 0 ? -1 : 0; + //This is not a good implementation, but I don't know of any target platforms that will hit this. + //TODO: AVX512 being enabled by the runtime could force this path to be taken; it'll require an update! + Debug.Assert(Vector.Count < 16, "The vector path assumes that AVX512 is not supported, so this is hitting a fallback path."); + Span mask = stackalloc int[Vector.Count]; + for (int i = 0; i < Vector.Count; ++i) + { + mask[i] = (scalarIntegrationMask & (1 << i)) > 0 ? -1 : 0; + } + integrationMask = new Vector(mask); } - integrationMask = new Vector(mask); return BundleIntegrationMode.Partial; } integrationMask = default; @@ -478,6 +500,7 @@ public static unsafe void GatherAndIntegrate(-1); From 51ae808289bc2253dba4e0468db4bdb77618d407 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 13 Jul 2021 13:24:16 -0500 Subject: [PATCH 103/947] Better assert. --- BepuPhysics/Constraints/TwoBodyTypeProcessor.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 293fe223c..bb3ee5331 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -394,7 +394,7 @@ public static BundleIntegrationMode BundleShouldIntegrate(int bundleIndex, in In { //This is not a good implementation, but I don't know of any target platforms that will hit this. //TODO: AVX512 being enabled by the runtime could force this path to be taken; it'll require an update! - Debug.Assert(Vector.Count < 16, "The vector path assumes that AVX512 is not supported, so this is hitting a fallback path."); + Debug.Assert(Vector.Count <= 8, "The vector path assumes that AVX512 is not supported, so this is hitting a fallback path."); Span mask = stackalloc int[Vector.Count]; for (int i = 0; i < Vector.Count; ++i) { From 8b4a98b12c0d124bd33422771aba7aabac3d91e5 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 13 Jul 2021 17:13:17 -0500 Subject: [PATCH 104/947] First pass batch merging based integration responsibility. --- .../Constraints/TwoBodyTypeProcessor.cs | 8 +- BepuPhysics/Solver_SubsteppingSolve2.cs | 195 ++++++++++++++---- 2 files changed, 158 insertions(+), 45 deletions(-) diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index bb3ee5331..2b4de587e 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -494,7 +494,7 @@ private static unsafe void IntegrateUniformly( bodies.ScatterPoseAndInertia(ref position, ref orientation, ref inertia, ref bodyIndices, count, ref integrationMask); } public static unsafe void GatherAndIntegrate( - Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, ref IndexSet integrationFlags, float dt, int workerIndex, int bundleIndex, + Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, ref Buffer integrationFlags, int bodyIndexInConstraint, float dt, int workerIndex, int bundleIndex, ref Vector bodyIndices, int count, out Vector3Wide position, out QuaternionWide orientation, out BodyVelocityWide velocity, out BodyInertiaWide inertia) where TIntegratorCallbacks : struct, IPoseIntegratorCallbacks where TBatchIntegrationMode : struct, IBatchIntegrationMode @@ -515,7 +515,7 @@ public static unsafe void GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags[0], dt, workerIndex, i, ref references.IndexA, count, + GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 0, dt, workerIndex, i, ref references.IndexA, count, out var positionA, out var orientationA, out var wsvA, out var inertiaA); - GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags[1], dt, workerIndex, i, ref references.IndexB, count, + GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 1, dt, workerIndex, i, ref references.IndexB, count, out var positionB, out var orientationB, out var wsvB, out var inertiaB); var ab = positionB - positionA; if (typeof(TConstraintFunctions) == typeof(WeldFunctions)) diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index eb38557d0..a2d279103 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -10,6 +10,7 @@ using System.Text; using System.Threading; using System.Runtime.Intrinsics.X86; +using System.Numerics; namespace BepuPhysics { @@ -58,8 +59,9 @@ public void Execute(Solver solver, int blockIndex, int workerIndex) var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; if (block.BatchIndex == 0) { + Buffer noFlagsRequired = default; typeProcessor.WarmStart2( - ref typeBatch, ref this.solver.integrationFlags[block.BatchIndex][block.TypeBatchIndex], this.solver.bodies, ref this.solver.PoseIntegrator.Callbacks, + ref typeBatch, ref noFlagsRequired, this.solver.bodies, ref this.solver.PoseIntegrator.Callbacks, Dt, InverseDt, block.StartBundle, block.End, workerIndex); } else @@ -174,68 +176,179 @@ void SolveStep2Worker(int workerIndex) Buffer>> integrationFlags; - public override void PrepareConstraintIntegrationResponsibilities() + public override unsafe void PrepareConstraintIntegrationResponsibilities() { //var start = Stopwatch.GetTimestamp(); pool.Take(ActiveSet.Batches.Count, out integrationFlags); - for (int i = 0; i < integrationFlags.Length; ++i) + integrationFlags[0] = default; + for (int batchIndex = 1; batchIndex < integrationFlags.Length; ++batchIndex) { - ref var batch = ref ActiveSet.Batches[i]; - ref var flagsForBatch = ref integrationFlags[i]; + ref var batch = ref ActiveSet.Batches[batchIndex]; + ref var flagsForBatch = ref integrationFlags[batchIndex]; pool.Take(batch.TypeBatches.Count, out flagsForBatch); - for (int j = 0; j < flagsForBatch.Length; ++j) + for (int typeBatchIndex = 0; typeBatchIndex < flagsForBatch.Length; ++typeBatchIndex) { - ref var flagsForTypeBatch = ref flagsForBatch[j]; - ref var typeBatch = ref batch.TypeBatches[j]; + ref var flagsForTypeBatch = ref flagsForBatch[typeBatchIndex]; + ref var typeBatch = ref batch.TypeBatches[typeBatchIndex]; var bodiesPerConstraint = TypeProcessors[typeBatch.TypeId].BodiesPerConstraint; pool.Take(bodiesPerConstraint, out flagsForTypeBatch); - for (int k = 0; k < bodiesPerConstraint; ++k) + for (int bodyIndexInConstraint = 0; bodyIndexInConstraint < bodiesPerConstraint; ++bodyIndexInConstraint) { - flagsForTypeBatch[k] = new IndexSet(pool, typeBatch.ConstraintCount); + flagsForTypeBatch[bodyIndexInConstraint] = new IndexSet(pool, typeBatch.ConstraintCount); } } } - for (int i = 0; i < bodies.ActiveSet.Count; ++i) + //for (int i = 0; i < bodies.ActiveSet.Count; ++i) + //{ + // ref var constraints = ref bodies.ActiveSet.Constraints[i]; + // ConstraintHandle minimumConstraint; + // minimumConstraint.Value = -1; + // int minimumBatchIndex = int.MaxValue; + // int minimumIndexInConstraint = -1; + // for (int j = 0; j < constraints.Count; ++j) + // { + // ref var constraint = ref constraints[j]; + // var batchIndex = HandleToConstraint[constraint.ConnectingConstraintHandle.Value].BatchIndex; + // if (batchIndex < minimumBatchIndex) + // { + // minimumBatchIndex = batchIndex; + // minimumIndexInConstraint = constraint.BodyIndexInConstraint; + // minimumConstraint = constraint.ConnectingConstraintHandle; + // } + // } + // if (minimumConstraint.Value >= 0) + // { + // ref var location = ref HandleToConstraint[minimumConstraint.Value]; + // var typeBatchIndex = ActiveSet.Batches[location.BatchIndex].TypeIndexToTypeBatchIndex[location.TypeId]; + // ref var indexSet = ref integrationFlags[location.BatchIndex][typeBatchIndex][minimumIndexInConstraint]; + // indexSet.AddUnsafely(location.IndexInTypeBatch); + // } + //} + //Console.WriteLine($"body count: {bodies.ActiveSet.Count}, constraint count: {CountConstraints()}"); + //var end = Stopwatch.GetTimestamp(); + //Console.WriteLine($"Brute force time (ms): {(end - start) * 1e3 / Stopwatch.Frequency}"); + + pool.Take(batchReferencedHandles.Count, out var bodiesFirstObservedInBatches); + IndexSet merged; + //We don't have to consider the first batch, since we know ahead of time that the first batch will be the first time we see any bodies in it. + //Just copy directly from the first batch into the merged to initialize it. + pool.Take((bodies.HandlePool.HighestPossiblyClaimedId + 63) / 64, out merged.Flags); + var copyLength = Math.Min(merged.Flags.Length, batchReferencedHandles[0].Flags.Length); + batchReferencedHandles[0].Flags.CopyTo(0, merged.Flags, 0, copyLength); + batchReferencedHandles[0].Flags.Clear(copyLength, batchReferencedHandles[0].Flags.Length - copyLength); + + //Yup, we're just leaving the first slot unallocated to avoid having to offset indices all over the place. Slight wonk, but not a big deal. + bodiesFirstObservedInBatches[0] = default; + for (int batchIndex = 1; batchIndex < bodiesFirstObservedInBatches.Length; ++batchIndex) + { + ref var batchHandles = ref batchReferencedHandles[batchIndex]; + var bundleCount = Math.Min(merged.Flags.Length, batchHandles.Flags.Length); + //Note that we bypass the constructor to avoid zeroing unnecessarily. Every bundle will be fully assigned. + pool.Take(bundleCount, out bodiesFirstObservedInBatches[batchIndex].Flags); + } + for (int batchIndex = 1; batchIndex < ActiveSet.Batches.Count; ++batchIndex) { - ref var constraints = ref bodies.ActiveSet.Constraints[i]; - ConstraintHandle minimumConstraint; - minimumConstraint.Value = -1; - int minimumBatchIndex = int.MaxValue; - int minimumIndexInConstraint = -1; - for (int j = 0; j < constraints.Count; ++j) + ref var batchHandles = ref batchReferencedHandles[batchIndex]; + ref var firstObservedInBatch = ref bodiesFirstObservedInBatches[batchIndex]; + var bundleCount = Math.Min(merged.Flags.Length, batchHandles.Flags.Length); + for (int flagBundleIndex = 0; flagBundleIndex < bundleCount; ++flagBundleIndex) { - ref var constraint = ref constraints[j]; - var batchIndex = HandleToConstraint[constraint.ConnectingConstraintHandle.Value].BatchIndex; - if (batchIndex < minimumBatchIndex) - { - minimumBatchIndex = batchIndex; - minimumIndexInConstraint = constraint.BodyIndexInConstraint; - minimumConstraint = constraint.ConnectingConstraintHandle; - } + var mergeBundle = merged.Flags[flagBundleIndex]; + var batchBundle = batchHandles.Flags[flagBundleIndex]; + merged.Flags[flagBundleIndex] = mergeBundle | batchBundle; + //If this batch contains a body, and the merged set does not, then it's the first batch that sees a body and it will have integration responsibility. + firstObservedInBatch.Flags[flagBundleIndex] = ~mergeBundle & batchBundle; } - if (minimumConstraint.Value >= 0) + } + var start = Stopwatch.GetTimestamp(); + //We now have index sets representing the first time each body handle is observed in a batch. + for (int batchIndex = 1; batchIndex < bodiesFirstObservedInBatches.Length; ++batchIndex) + { + ref var integrationFlagsForBatch = ref integrationFlags[batchIndex]; + ref var firstObservedForBatch = ref bodiesFirstObservedInBatches[batchIndex]; + ref var batch = ref ActiveSet.Batches[batchIndex]; + for (int typeBatchIndex = 0; typeBatchIndex < batch.TypeBatches.Count; ++typeBatchIndex) { - ref var location = ref HandleToConstraint[minimumConstraint.Value]; - var typeBatchIndex = ActiveSet.Batches[location.BatchIndex].TypeIndexToTypeBatchIndex[location.TypeId]; - ref var indexSet = ref integrationFlags[location.BatchIndex][typeBatchIndex][minimumIndexInConstraint]; - indexSet.AddUnsafely(location.IndexInTypeBatch); + ref var integrationFlagsForTypeBatch = ref integrationFlagsForBatch[typeBatchIndex]; + ref var typeBatch = ref batch.TypeBatches[typeBatchIndex]; + var typeBatchBodyReferences = typeBatch.BodyReferences.As(); + var bodiesPerConstraintInTypeBatch = TypeProcessors[typeBatch.TypeId].BodiesPerConstraint; + var intsPerBundle = Vector.Count * bodiesPerConstraintInTypeBatch; + for (int bundleIndex = 0; bundleIndex < typeBatch.BundleCount; ++bundleIndex) + { + int bundleStartIndexInConstraints = bundleIndex * Vector.Count; + int countInBundle = Math.Min(Vector.Count, typeBatch.ConstraintCount - bundleStartIndexInConstraints); + //Body references are stored in AOSOA layout. + var bundleBodyReferencesStart = typeBatchBodyReferences.Memory + bundleIndex * intsPerBundle; + for (int bodyIndexInConstraint = 0; bodyIndexInConstraint < bodiesPerConstraintInTypeBatch; ++bodyIndexInConstraint) + { + ref var integrationFlagsForBodyInConstraint = ref integrationFlagsForTypeBatch[bodyIndexInConstraint]; + var bundleStart = bundleBodyReferencesStart + bodyIndexInConstraint * Vector.Count; + for (int bundleInnerIndex = 0; bundleInnerIndex < countInBundle; ++bundleInnerIndex) + { + //Constraints refer to bodies by index when they're in the active set, so we need to transform to handle to look up our merged batch results. + var bodyHandle = bodies.ActiveSet.IndexToHandle[bundleStart[bundleInnerIndex]].Value; + if (firstObservedForBatch.Contains(bodyHandle)) + { + integrationFlagsForBodyInConstraint.AddUnsafely(bundleStartIndexInConstraints + bundleInnerIndex); + } + } + } + } } } - //Console.WriteLine($"body count: {bodies.ActiveSet.Count}, constraint count: {CountConstraints()}"); - //var end = Stopwatch.GetTimestamp(); - //Console.WriteLine($"Brute force time (ms): {(end - start) * 1e3 / Stopwatch.Frequency}"); + var end = Stopwatch.GetTimestamp(); + //Console.WriteLine($"Time (ms): {(end - start) * 1e3 / Stopwatch.Frequency}"); + //for (int i = 0; i < bodies.ActiveSet.Count; ++i) + //{ + // ref var constraints = ref bodies.ActiveSet.Constraints[i]; + // ConstraintHandle minimumConstraint; + // minimumConstraint.Value = -1; + // int minimumBatchIndex = int.MaxValue; + // int minimumIndexInConstraint = -1; + // for (int j = 0; j < constraints.Count; ++j) + // { + // ref var constraint = ref constraints[j]; + // var batchIndex = HandleToConstraint[constraint.ConnectingConstraintHandle.Value].BatchIndex; + // if (batchIndex < minimumBatchIndex) + // { + // minimumBatchIndex = batchIndex; + // minimumIndexInConstraint = constraint.BodyIndexInConstraint; + // minimumConstraint = constraint.ConnectingConstraintHandle; + // } + // } + // if (minimumConstraint.Value >= 0) + // { + // ref var location = ref HandleToConstraint[minimumConstraint.Value]; + // var typeBatchIndex = ActiveSet.Batches[location.BatchIndex].TypeIndexToTypeBatchIndex[location.TypeId]; + // if (location.BatchIndex > 0) + // { + // ref var indexSet = ref integrationFlags[location.BatchIndex][typeBatchIndex][minimumIndexInConstraint]; + // Debug.Assert(indexSet.Contains(location.IndexInTypeBatch)); + // } + // } + //} + + merged.Dispose(pool); + Debug.Assert(!bodiesFirstObservedInBatches[0].Flags.Allocated, "Remember, we're assuming we're just leaving the first batch's slot empty to avoid indexing complexity."); + for (int batchIndex = 1; batchIndex < bodiesFirstObservedInBatches.Length; ++batchIndex) + { + bodiesFirstObservedInBatches[batchIndex].Dispose(pool); + } + pool.Return(ref bodiesFirstObservedInBatches); } public override void DisposeConstraintIntegrationResponsibilities() { - for (int i = 0; i < integrationFlags.Length; ++i) + Debug.Assert(!integrationFlags[0].Allocated, "Remember, we're assuming we're just leaving the first batch's slot empty to avoid indexing complexity."); + for (int batchIndex = 1; batchIndex < integrationFlags.Length; ++batchIndex) { - ref var flagsForBatch = ref integrationFlags[i]; - for (int j = 0; j < flagsForBatch.Length; ++j) + ref var flagsForBatch = ref integrationFlags[batchIndex]; + for (int typeBatchIndex = 0; typeBatchIndex < flagsForBatch.Length; ++typeBatchIndex) { - ref var flagsForTypeBatch = ref flagsForBatch[j]; - for (int k = 0; k < flagsForTypeBatch.Length; ++k) + ref var flagsForTypeBatch = ref flagsForBatch[typeBatchIndex]; + for (int bodyIndexInConstraint = 0; bodyIndexInConstraint < flagsForTypeBatch.Length; ++bodyIndexInConstraint) { - flagsForTypeBatch[k].Dispose(pool); + flagsForTypeBatch[bodyIndexInConstraint].Dispose(pool); } pool.Return(ref flagsForTypeBatch); } @@ -262,7 +375,8 @@ public override void SolveStep2(float dt, IThreadDispatcher threadDispatcher = n ref var typeBatch = ref batch.TypeBatches[j]; if (i == 0) { - TypeProcessors[typeBatch.TypeId].WarmStart2(ref typeBatch, ref integrationFlagsForBatch[j], bodies, ref PoseIntegrator.Callbacks, + Buffer noFlagsRequired = default; + TypeProcessors[typeBatch.TypeId].WarmStart2(ref typeBatch, ref noFlagsRequired, bodies, ref PoseIntegrator.Callbacks, dt, inverseDt, 0, typeBatch.BundleCount, 0); } else @@ -276,7 +390,6 @@ public override void SolveStep2(float dt, IThreadDispatcher threadDispatcher = n for (int i = 0; i < synchronizedBatchCount; ++i) { ref var batch = ref activeSet.Batches[i]; - ref var integrationFlagsForBatch = ref integrationFlags[i]; for (int j = 0; j < batch.TypeBatches.Count; ++j) { ref var typeBatch = ref batch.TypeBatches[j]; From c679c4646a4cd408f51a9c7856e26f71eb4bbe00 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 13 Jul 2021 20:35:26 -0500 Subject: [PATCH 105/947] Added more integration responsibility filtering. Type batches get individually filtered, and the prepass is faster. --- BepuPhysics/Solver_SubsteppingSolve2.cs | 116 ++++++++++++++++++++++-- BepuUtilities/Collections/IndexSet.cs | 2 +- 2 files changed, 111 insertions(+), 7 deletions(-) diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index a2d279103..36f5af6de 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -66,9 +66,18 @@ public void Execute(Solver solver, int blockIndex, int workerIndex) } else { - typeProcessor.WarmStart2( - ref typeBatch, ref this.solver.integrationFlags[block.BatchIndex][block.TypeBatchIndex], this.solver.bodies, ref this.solver.PoseIntegrator.Callbacks, - Dt, InverseDt, block.StartBundle, block.End, workerIndex); + if (this.solver.coarseBatchIntegrationResponsibilities[block.BatchIndex][block.TypeBatchIndex]) + { + typeProcessor.WarmStart2( + ref typeBatch, ref this.solver.integrationFlags[block.BatchIndex][block.TypeBatchIndex], this.solver.bodies, ref this.solver.PoseIntegrator.Callbacks, + Dt, InverseDt, block.StartBundle, block.End, workerIndex); + } + else + { + typeProcessor.WarmStart2( + ref typeBatch, ref this.solver.integrationFlags[block.BatchIndex][block.TypeBatchIndex], this.solver.bodies, ref this.solver.PoseIntegrator.Callbacks, + Dt, InverseDt, block.StartBundle, block.End, workerIndex); + } } } } @@ -175,17 +184,24 @@ void SolveStep2Worker(int workerIndex) } Buffer>> integrationFlags; + /// + /// Caches a single bool for whether type batches within batches have constraints with any integration responsibilities. + /// Type batches with no integration responsibilities can use a codepath with no integration checks at all. + /// + Buffer> coarseBatchIntegrationResponsibilities; public override unsafe void PrepareConstraintIntegrationResponsibilities() { //var start = Stopwatch.GetTimestamp(); pool.Take(ActiveSet.Batches.Count, out integrationFlags); integrationFlags[0] = default; + pool.Take(ActiveSet.Batches.Count, out coarseBatchIntegrationResponsibilities); for (int batchIndex = 1; batchIndex < integrationFlags.Length; ++batchIndex) { ref var batch = ref ActiveSet.Batches[batchIndex]; ref var flagsForBatch = ref integrationFlags[batchIndex]; pool.Take(batch.TypeBatches.Count, out flagsForBatch); + pool.Take(batch.TypeBatches.Count, out coarseBatchIntegrationResponsibilities[batchIndex]); for (int typeBatchIndex = 0; typeBatchIndex < flagsForBatch.Length; ++typeBatchIndex) { ref var flagsForTypeBatch = ref flagsForBatch[typeBatchIndex]; @@ -239,6 +255,7 @@ public override unsafe void PrepareConstraintIntegrationResponsibilities() //Yup, we're just leaving the first slot unallocated to avoid having to offset indices all over the place. Slight wonk, but not a big deal. bodiesFirstObservedInBatches[0] = default; + pool.Take(batchReferencedHandles.Count, out var batchHasAnyIntegrationResponsibilities); for (int batchIndex = 1; batchIndex < bodiesFirstObservedInBatches.Length; ++batchIndex) { ref var batchHandles = ref batchReferencedHandles[batchIndex]; @@ -251,22 +268,34 @@ public override unsafe void PrepareConstraintIntegrationResponsibilities() ref var batchHandles = ref batchReferencedHandles[batchIndex]; ref var firstObservedInBatch = ref bodiesFirstObservedInBatches[batchIndex]; var bundleCount = Math.Min(merged.Flags.Length, batchHandles.Flags.Length); + ulong horizontalMerge = 0; for (int flagBundleIndex = 0; flagBundleIndex < bundleCount; ++flagBundleIndex) { var mergeBundle = merged.Flags[flagBundleIndex]; var batchBundle = batchHandles.Flags[flagBundleIndex]; merged.Flags[flagBundleIndex] = mergeBundle | batchBundle; //If this batch contains a body, and the merged set does not, then it's the first batch that sees a body and it will have integration responsibility. - firstObservedInBatch.Flags[flagBundleIndex] = ~mergeBundle & batchBundle; + var firstObservedBundle = ~mergeBundle & batchBundle; + horizontalMerge |= firstObservedBundle; + firstObservedInBatch.Flags[flagBundleIndex] = firstObservedBundle; } + batchHasAnyIntegrationResponsibilities[batchIndex] = horizontalMerge != 0; } var start = Stopwatch.GetTimestamp(); //We now have index sets representing the first time each body handle is observed in a batch. for (int batchIndex = 1; batchIndex < bodiesFirstObservedInBatches.Length; ++batchIndex) { + if (!batchHasAnyIntegrationResponsibilities[batchIndex]) + continue; ref var integrationFlagsForBatch = ref integrationFlags[batchIndex]; ref var firstObservedForBatch = ref bodiesFirstObservedInBatches[batchIndex]; ref var batch = ref ActiveSet.Batches[batchIndex]; + + //ulong totalConstraintCount = 0; + //ulong integratingConstraintCount = 0; + //ulong totalLanes = 0; + //ulong integratingLanes = 0; + for (int typeBatchIndex = 0; typeBatchIndex < batch.TypeBatches.Count; ++typeBatchIndex) { ref var integrationFlagsForTypeBatch = ref integrationFlagsForBatch[typeBatchIndex]; @@ -295,8 +324,73 @@ public override unsafe void PrepareConstraintIntegrationResponsibilities() } } } + //Precompute which type batches have *any* integration responsibilities, allowing us to use a all-or-nothing test before dispatching a workblock. + var flagBundleCount = IndexSet.GetBundleCapacity(typeBatch.ConstraintCount); + ulong mergedFlagBundles = 0; + for (int bodyIndexInConstraint = 0; bodyIndexInConstraint < bodiesPerConstraintInTypeBatch; ++bodyIndexInConstraint) + { + ref var integrationFlagsForBodyInTypeBatch = ref integrationFlagsForTypeBatch[bodyIndexInConstraint]; + for (int i = 0; i < flagBundleCount; ++i) + { + mergedFlagBundles |= integrationFlagsForBodyInTypeBatch.Flags[i]; + } + } + coarseBatchIntegrationResponsibilities[batchIndex][typeBatchIndex] = mergedFlagBundles != 0; + + //for (int i = 0; i < flagBundleCount; ++i) + //{ + // ulong countMerge = 0; + // for (int bodyIndexInConstraint = 0; bodyIndexInConstraint < bodiesPerConstraintInTypeBatch; ++bodyIndexInConstraint) + // { + // var flagsForBody = integrationFlagsForTypeBatch[bodyIndexInConstraint].Flags[i]; + // countMerge |= flagsForBody; + // integratingLanes += Popcnt.X64.PopCount(flagsForBody); + // totalLanes += (ulong)Math.Min(64, typeBatch.ConstraintCount - i * 64); + // } + // integratingConstraintCount += Popcnt.X64.PopCount(countMerge); + // totalConstraintCount += (ulong)Math.Min(64, typeBatch.ConstraintCount - i * 64); + //} } + //Console.WriteLine($"Batch {batchIndex} integrating constraints: {integratingConstraintCount} over {totalConstraintCount}, {integratingConstraintCount / (double)totalConstraintCount}"); + //Console.WriteLine($"Batch {batchIndex} integrating lanes: {integratingLanes} over {totalLanes}, {integratingLanes / (double)totalLanes}"); } + pool.Return(ref batchHasAnyIntegrationResponsibilities); + //for (int batchIndex = 1; batchIndex < bodiesFirstObservedInBatches.Length; ++batchIndex) + //{ + // ref var integrationFlagsForBatch = ref integrationFlags[batchIndex]; + // ref var firstObservedForBatch = ref bodiesFirstObservedInBatches[batchIndex]; + // ref var batch = ref ActiveSet.Batches[batchIndex]; + // for (int typeBatchIndex = 0; typeBatchIndex < batch.TypeBatches.Count; ++typeBatchIndex) + // { + // ref var integrationFlagsForTypeBatch = ref integrationFlagsForBatch[typeBatchIndex]; + // ref var typeBatch = ref batch.TypeBatches[typeBatchIndex]; + // var typeBatchBodyReferences = typeBatch.BodyReferences.As(); + // var bodiesPerConstraintInTypeBatch = TypeProcessors[typeBatch.TypeId].BodiesPerConstraint; + // var intsPerBundle = Vector.Count * bodiesPerConstraintInTypeBatch; + // //We process over one strip of bodies in the constraint in each pass, constructing contiguous flag sequences as we go. + // for (int bodyIndexInConstraint = 0; bodyIndexInConstraint < bodiesPerConstraintInTypeBatch; ++bodyIndexInConstraint) + // { + // ref var integrationFlagsForBodyInConstraint = ref integrationFlagsForTypeBatch[bodyIndexInConstraint]; + // for (int flagBundleIndex = 0; flagBundleIndex < integrationFlagsForBodyInConstraint.Flags.Length; ++flagBundleIndex) + // { + // ulong flagBundle = 0; + // var constraintBundleStartIndexInConstraints = flagBundleIndex * 64; + // int countInFlagBundle = Math.Min(64, typeBatch.ConstraintCount - constraintBundleStartIndexInConstraints); + // for (int indexInFlagBundle = 0; indexInFlagBundle < countInFlagBundle; ++indexInFlagBundle) + // { + // var constraintIndex = constraintBundleStartIndexInConstraints + indexInFlagBundle; + // BundleIndexing.GetBundleIndices(constraintIndex, out var constraintBundleIndex, out var constraintIndexInBundle); + // var bodyHandle = bodies.ActiveSet.IndexToHandle[typeBatchBodyReferences.Memory[constraintBundleIndex * intsPerBundle + Vector.Count * bodyIndexInConstraint]].Value; + // if (firstObservedForBatch.Contains(bodyHandle)) + // { + // integrationFlagsForBodyInConstraint.AddUnsafely(constraintIndex); + // } + // } + // integrationFlagsForBodyInConstraint.Flags[flagBundleIndex] = flagBundle; + // } + // } + // } + //} var end = Stopwatch.GetTimestamp(); //Console.WriteLine($"Time (ms): {(end - start) * 1e3 / Stopwatch.Frequency}"); //for (int i = 0; i < bodies.ActiveSet.Count; ++i) @@ -353,8 +447,10 @@ public override void DisposeConstraintIntegrationResponsibilities() pool.Return(ref flagsForTypeBatch); } pool.Return(ref flagsForBatch); + pool.Return(ref coarseBatchIntegrationResponsibilities[batchIndex]); } pool.Return(ref integrationFlags); + pool.Return(ref coarseBatchIntegrationResponsibilities); } public override void SolveStep2(float dt, IThreadDispatcher threadDispatcher = null) @@ -381,8 +477,16 @@ public override void SolveStep2(float dt, IThreadDispatcher threadDispatcher = n } else { - TypeProcessors[typeBatch.TypeId].WarmStart2(ref typeBatch, ref integrationFlagsForBatch[j], bodies, ref PoseIntegrator.Callbacks, - dt, inverseDt, 0, typeBatch.BundleCount, 0); + if (coarseBatchIntegrationResponsibilities[i][j]) + { + TypeProcessors[typeBatch.TypeId].WarmStart2(ref typeBatch, ref integrationFlagsForBatch[j], bodies, ref PoseIntegrator.Callbacks, + dt, inverseDt, 0, typeBatch.BundleCount, 0); + } + else + { + TypeProcessors[typeBatch.TypeId].WarmStart2(ref typeBatch, ref integrationFlagsForBatch[j], bodies, ref PoseIntegrator.Callbacks, + dt, inverseDt, 0, typeBatch.BundleCount, 0); + } } } } diff --git a/BepuUtilities/Collections/IndexSet.cs b/BepuUtilities/Collections/IndexSet.cs index b8d934df7..dadb1da0e 100644 --- a/BepuUtilities/Collections/IndexSet.cs +++ b/BepuUtilities/Collections/IndexSet.cs @@ -25,7 +25,7 @@ public struct IndexSet const int mask = 63; [MethodImpl(MethodImplOptions.AggressiveInlining)] - static int GetBundleCapacity(int count) + public static int GetBundleCapacity(int count) { return (count + mask) >> shift; } From 2b09d8e36b2a3f18c5fa0d691e4b61bad2be7da8 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 14 Jul 2021 13:57:44 -0500 Subject: [PATCH 106/947] AVX batch merging. 74000 bodies 8 batches takes 5 microseconds, so... --- BepuPhysics/Solver_SubsteppingSolve2.cs | 56 ++++++++++++++++++++----- Demos/Demos/NewtDemo.cs | 2 + 2 files changed, 47 insertions(+), 11 deletions(-) diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index 36f5af6de..728034110 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -11,6 +11,7 @@ using System.Threading; using System.Runtime.Intrinsics.X86; using System.Numerics; +using System.Runtime.Intrinsics; namespace BepuPhysics { @@ -267,19 +268,52 @@ public override unsafe void PrepareConstraintIntegrationResponsibilities() { ref var batchHandles = ref batchReferencedHandles[batchIndex]; ref var firstObservedInBatch = ref bodiesFirstObservedInBatches[batchIndex]; - var bundleCount = Math.Min(merged.Flags.Length, batchHandles.Flags.Length); - ulong horizontalMerge = 0; - for (int flagBundleIndex = 0; flagBundleIndex < bundleCount; ++flagBundleIndex) + var flagBundleCount = Math.Min(merged.Flags.Length, batchHandles.Flags.Length); + if (Avx2.IsSupported) { - var mergeBundle = merged.Flags[flagBundleIndex]; - var batchBundle = batchHandles.Flags[flagBundleIndex]; - merged.Flags[flagBundleIndex] = mergeBundle | batchBundle; - //If this batch contains a body, and the merged set does not, then it's the first batch that sees a body and it will have integration responsibility. - var firstObservedBundle = ~mergeBundle & batchBundle; - horizontalMerge |= firstObservedBundle; - firstObservedInBatch.Flags[flagBundleIndex] = firstObservedBundle; + var avxBundleCount = flagBundleCount / 4; + var horizontalAvxMerge = Vector256.Zero; + for (int avxBundleIndex = 0; avxBundleIndex < avxBundleCount; ++avxBundleIndex) + { + var mergeBundle = ((Vector256*)merged.Flags.Memory)[avxBundleIndex]; + var batchBundle = ((Vector256*)batchHandles.Flags.Memory)[avxBundleIndex]; + ((Vector256*)merged.Flags.Memory)[avxBundleIndex] = Avx2.Or(mergeBundle, batchBundle); + //If this batch contains a body, and the merged set does not, then it's the first batch that sees a body and it will have integration responsibility. + var firstObservedBundle = Avx2.AndNot(mergeBundle, batchBundle); + horizontalAvxMerge = Avx2.Or(firstObservedBundle, horizontalAvxMerge); + ((Vector256*)firstObservedInBatch.Flags.Memory)[avxBundleIndex] = firstObservedBundle; + } + var notEqual = Avx2.CompareNotEqual(horizontalAvxMerge.AsDouble(), Vector256.Zero); + ulong horizontalMerge = (ulong)Avx.MoveMask(notEqual); + + //Cleanup loop. + for (int flagBundleIndex = avxBundleCount * 4; flagBundleIndex < flagBundleCount; ++flagBundleIndex) + { + var mergeBundle = merged.Flags[flagBundleIndex]; + var batchBundle = batchHandles.Flags[flagBundleIndex]; + merged.Flags[flagBundleIndex] = mergeBundle | batchBundle; + //If this batch contains a body, and the merged set does not, then it's the first batch that sees a body and it will have integration responsibility. + var firstObservedBundle = ~mergeBundle & batchBundle; + horizontalMerge |= firstObservedBundle; + firstObservedInBatch.Flags[flagBundleIndex] = firstObservedBundle; + } + batchHasAnyIntegrationResponsibilities[batchIndex] = horizontalMerge != 0; + } + else + { + ulong horizontalMerge = 0; + for (int flagBundleIndex = 0; flagBundleIndex < flagBundleCount; ++flagBundleIndex) + { + var mergeBundle = merged.Flags[flagBundleIndex]; + var batchBundle = batchHandles.Flags[flagBundleIndex]; + merged.Flags[flagBundleIndex] = mergeBundle | batchBundle; + //If this batch contains a body, and the merged set does not, then it's the first batch that sees a body and it will have integration responsibility. + var firstObservedBundle = ~mergeBundle & batchBundle; + horizontalMerge |= firstObservedBundle; + firstObservedInBatch.Flags[flagBundleIndex] = firstObservedBundle; + } + batchHasAnyIntegrationResponsibilities[batchIndex] = horizontalMerge != 0; } - batchHasAnyIntegrationResponsibilities[batchIndex] = horizontalMerge != 0; } var start = Stopwatch.GetTimestamp(); //We now have index sets representing the first time each body handle is observed in a batch. diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index 248ff5466..920b2adb7 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -838,6 +838,8 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //CreateDeformable(Simulation, new Vector3(i * 3, 5 + i * 1.5f, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), MathF.PI * (i * 0.55f)), 1f, cellSize, weldSpringiness, volumeSpringiness, i, filters, ref vertices, ref vertexSpatialIndices, ref cellVertexIndices, ref tetrahedraVertexIndices); CreateDeformable(Simulation, new Vector3(i * 3, cellSize * 2f + i * 0f, 0), Quaternion.Identity, 1f, cellSize, weldSpringiness, volumeSpringiness, i, filters, ref vertices, ref vertexSpatialIndices, ref cellVertexIndices, ref tetrahedraVertexIndices); } + //Console.WriteLine($"body count: {Simulation.Bodies.ActiveSet.Count}"); + //Console.WriteLine($"constraint count: {Simulation.Solver.CountConstraints()}"); BufferPool.Return(ref vertices); vertexSpatialIndices.Dispose(BufferPool); From 24c66508a93c3ba668526044bab4846acd321f2c Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 14 Jul 2021 16:07:57 -0500 Subject: [PATCH 107/947] Multithreaded integration responsibility prepass. --- .../EmbeddedSubsteppingTimestepper2.cs | 2 +- BepuPhysics/Solver_SubsteppingSolve2.cs | 266 ++++++++++++++---- 2 files changed, 205 insertions(+), 63 deletions(-) diff --git a/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs b/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs index 57a8539dc..7a6506987 100644 --- a/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs +++ b/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs @@ -78,7 +78,7 @@ public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDi Debug.Assert(SubstepCount >= 0, "Substep count should be positive."); var substepDt = dt / SubstepCount; - simulation.Solver.PrepareConstraintIntegrationResponsibilities(); + simulation.Solver.PrepareConstraintIntegrationResponsibilities(threadDispatcher); for (int substepIndex = 0; substepIndex < SubstepCount; ++substepIndex) { SubstepStarted?.Invoke(substepIndex, dt, threadDispatcher); diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index 728034110..798159f2a 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -17,7 +17,7 @@ namespace BepuPhysics { public partial class Solver { - public virtual void PrepareConstraintIntegrationResponsibilities() + public virtual void PrepareConstraintIntegrationResponsibilities(IThreadDispatcher threadDispatcher = null) { } public virtual void DisposeConstraintIntegrationResponsibilities() @@ -39,6 +39,7 @@ public Solver(Bodies bodies, BufferPool pool, int iterationCount, int fallbackBa { PoseIntegrator = poseIntegrator; solveStep2Worker = SolveStep2Worker; + constraintIntegrationResponsibilitiesWorker = ConstraintIntegrationResponsibilitiesWorker; } public PoseIntegrator PoseIntegrator { get; private set; } @@ -184,14 +185,77 @@ void SolveStep2Worker(int workerIndex) unclaimedState ^= 1; } + + unsafe bool ComputeIntegrationResponsibilitiesForConstraintRegion(int batchIndex, int typeBatchIndex, int constraintStart, int exclusiveConstraintEnd) + { + ref var firstObservedForBatch = ref bodiesFirstObservedInBatches[batchIndex]; + ref var integrationFlagsForTypeBatch = ref integrationFlags[batchIndex][typeBatchIndex]; + ref var typeBatch = ref ActiveSet.Batches[batchIndex].TypeBatches[typeBatchIndex]; + var typeBatchBodyReferences = typeBatch.BodyReferences.As(); + var bodiesPerConstraintInTypeBatch = TypeProcessors[typeBatch.TypeId].BodiesPerConstraint; + var intsPerBundle = Vector.Count * bodiesPerConstraintInTypeBatch; + var bundleStartIndex = constraintStart / Vector.Count; + var bundleEndIndex = (exclusiveConstraintEnd + Vector.Count - 1) / Vector.Count; + Debug.Assert(bundleStartIndex >= 0 && bundleEndIndex <= typeBatch.BundleCount); + + for (int bundleIndex = bundleStartIndex; bundleIndex < bundleEndIndex; ++bundleIndex) + { + int bundleStartIndexInConstraints = bundleIndex * Vector.Count; + int countInBundle = Math.Min(Vector.Count, typeBatch.ConstraintCount - bundleStartIndexInConstraints); + //Body references are stored in AOSOA layout. + var bundleBodyReferencesStart = typeBatchBodyReferences.Memory + bundleIndex * intsPerBundle; + for (int bodyIndexInConstraint = 0; bodyIndexInConstraint < bodiesPerConstraintInTypeBatch; ++bodyIndexInConstraint) + { + ref var integrationFlagsForBodyInConstraint = ref integrationFlagsForTypeBatch[bodyIndexInConstraint]; + var bundleStart = bundleBodyReferencesStart + bodyIndexInConstraint * Vector.Count; + for (int bundleInnerIndex = 0; bundleInnerIndex < countInBundle; ++bundleInnerIndex) + { + //Constraints refer to bodies by index when they're in the active set, so we need to transform to handle to look up our merged batch results. + var bodyHandle = bodies.ActiveSet.IndexToHandle[bundleStart[bundleInnerIndex]].Value; + if (firstObservedForBatch.Contains(bodyHandle)) + { + integrationFlagsForBodyInConstraint.AddUnsafely(bundleStartIndexInConstraints + bundleInnerIndex); + } + } + } + } + //Precompute which type batches have *any* integration responsibilities, allowing us to use a all-or-nothing test before dispatching a workblock. + var flagBundleCount = IndexSet.GetBundleCapacity(typeBatch.ConstraintCount); + ulong mergedFlagBundles = 0; + for (int bodyIndexInConstraint = 0; bodyIndexInConstraint < bodiesPerConstraintInTypeBatch; ++bodyIndexInConstraint) + { + ref var integrationFlagsForBodyInTypeBatch = ref integrationFlagsForTypeBatch[bodyIndexInConstraint]; + for (int i = 0; i < flagBundleCount; ++i) + { + mergedFlagBundles |= integrationFlagsForBodyInTypeBatch.Flags[i]; + } + } + return mergedFlagBundles != 0; + } + + void ConstraintIntegrationResponsibilitiesWorker(int workerIndex) + { + int jobIndex; + while ((jobIndex = Interlocked.Increment(ref nextConstraintIntegrationResponsibilityJobIndex) - 1) < integrationResponsibilityPrepassJobs.Count) + { + ref var job = ref integrationResponsibilityPrepassJobs[jobIndex]; + jobAlignedIntegrationResponsibilities[jobIndex] = ComputeIntegrationResponsibilitiesForConstraintRegion(job.batch, job.typeBatch, job.start, job.end); + } + } + + int nextConstraintIntegrationResponsibilityJobIndex; + QuickList<(int batch, int typeBatch, int start, int end)> integrationResponsibilityPrepassJobs; + Buffer jobAlignedIntegrationResponsibilities; + Buffer bodiesFirstObservedInBatches; Buffer>> integrationFlags; /// /// Caches a single bool for whether type batches within batches have constraints with any integration responsibilities. /// Type batches with no integration responsibilities can use a codepath with no integration checks at all. /// Buffer> coarseBatchIntegrationResponsibilities; + Action constraintIntegrationResponsibilitiesWorker; - public override unsafe void PrepareConstraintIntegrationResponsibilities() + public override unsafe void PrepareConstraintIntegrationResponsibilities(IThreadDispatcher threadDispatcher = null) { //var start = Stopwatch.GetTimestamp(); pool.Take(ActiveSet.Batches.Count, out integrationFlags); @@ -245,7 +309,7 @@ public override unsafe void PrepareConstraintIntegrationResponsibilities() //var end = Stopwatch.GetTimestamp(); //Console.WriteLine($"Brute force time (ms): {(end - start) * 1e3 / Stopwatch.Frequency}"); - pool.Take(batchReferencedHandles.Count, out var bodiesFirstObservedInBatches); + pool.Take(batchReferencedHandles.Count, out bodiesFirstObservedInBatches); IndexSet merged; //We don't have to consider the first batch, since we know ahead of time that the first batch will be the first time we see any bodies in it. //Just copy directly from the first batch into the merged to initialize it. @@ -264,6 +328,8 @@ public override unsafe void PrepareConstraintIntegrationResponsibilities() //Note that we bypass the constructor to avoid zeroing unnecessarily. Every bundle will be fully assigned. pool.Take(bundleCount, out bodiesFirstObservedInBatches[batchIndex].Flags); } + //Note that we are not multithreading the batch merging phase. This typically takes a handful of microseconds. + //You'd likely need millions of bodies before you'd see any substantial benefit from multithreading this. for (int batchIndex = 1; batchIndex < ActiveSet.Batches.Count; ++batchIndex) { ref var batchHandles = ref batchReferencedHandles[batchIndex]; @@ -317,77 +383,153 @@ public override unsafe void PrepareConstraintIntegrationResponsibilities() } var start = Stopwatch.GetTimestamp(); //We now have index sets representing the first time each body handle is observed in a batch. - for (int batchIndex = 1; batchIndex < bodiesFirstObservedInBatches.Length; ++batchIndex) - { - if (!batchHasAnyIntegrationResponsibilities[batchIndex]) - continue; - ref var integrationFlagsForBatch = ref integrationFlags[batchIndex]; - ref var firstObservedForBatch = ref bodiesFirstObservedInBatches[batchIndex]; - ref var batch = ref ActiveSet.Batches[batchIndex]; - - //ulong totalConstraintCount = 0; - //ulong integratingConstraintCount = 0; - //ulong totalLanes = 0; - //ulong integratingLanes = 0; + //This process is significantly more expensive than the batch merging phase and can benefit from multithreading. + //It is still fairly cheap, though- we can't use really fine grained jobs or the cost of swapping jobs will exceed productive work. - for (int typeBatchIndex = 0; typeBatchIndex < batch.TypeBatches.Count; ++typeBatchIndex) + //Note that we arbitrarily use single threaded execution if the job is small enough. Dispatching isn't free. + bool useSingleThreadedPath = true; + if (threadDispatcher != null && threadDispatcher.ThreadCount > 1) + { + integrationResponsibilityPrepassJobs = new(128, pool); + int constraintCount = 0; + const int targetJobSize = 2048; + Debug.Assert(targetJobSize % 64 == 0, "Target job size must be a multiple of the index set bundles to avoid threads working on the same flag bundle."); + for (int batchIndex = 1; batchIndex < bodiesFirstObservedInBatches.Length; ++batchIndex) { - ref var integrationFlagsForTypeBatch = ref integrationFlagsForBatch[typeBatchIndex]; - ref var typeBatch = ref batch.TypeBatches[typeBatchIndex]; - var typeBatchBodyReferences = typeBatch.BodyReferences.As(); - var bodiesPerConstraintInTypeBatch = TypeProcessors[typeBatch.TypeId].BodiesPerConstraint; - var intsPerBundle = Vector.Count * bodiesPerConstraintInTypeBatch; - for (int bundleIndex = 0; bundleIndex < typeBatch.BundleCount; ++bundleIndex) + if (!batchHasAnyIntegrationResponsibilities[batchIndex]) + continue; + ref var batch = ref ActiveSet.Batches[batchIndex]; + for (int typeBatchIndex = 0; typeBatchIndex < batch.TypeBatches.Count; ++typeBatchIndex) { - int bundleStartIndexInConstraints = bundleIndex * Vector.Count; - int countInBundle = Math.Min(Vector.Count, typeBatch.ConstraintCount - bundleStartIndexInConstraints); - //Body references are stored in AOSOA layout. - var bundleBodyReferencesStart = typeBatchBodyReferences.Memory + bundleIndex * intsPerBundle; - for (int bodyIndexInConstraint = 0; bodyIndexInConstraint < bodiesPerConstraintInTypeBatch; ++bodyIndexInConstraint) + ref var typeBatch = ref batch.TypeBatches[typeBatchIndex]; + constraintCount += typeBatch.ConstraintCount; + int jobCountForTypeBatch = (typeBatch.ConstraintCount + targetJobSize - 1) / targetJobSize; + for (int i = 0; i < jobCountForTypeBatch; ++i) { - ref var integrationFlagsForBodyInConstraint = ref integrationFlagsForTypeBatch[bodyIndexInConstraint]; - var bundleStart = bundleBodyReferencesStart + bodyIndexInConstraint * Vector.Count; - for (int bundleInnerIndex = 0; bundleInnerIndex < countInBundle; ++bundleInnerIndex) - { - //Constraints refer to bodies by index when they're in the active set, so we need to transform to handle to look up our merged batch results. - var bodyHandle = bodies.ActiveSet.IndexToHandle[bundleStart[bundleInnerIndex]].Value; - if (firstObservedForBatch.Contains(bodyHandle)) - { - integrationFlagsForBodyInConstraint.AddUnsafely(bundleStartIndexInConstraints + bundleInnerIndex); - } - } + var jobStart = i * targetJobSize; + var jobEnd = Math.Min(jobStart + targetJobSize, typeBatch.ConstraintCount); + integrationResponsibilityPrepassJobs.Allocate(pool) = (batchIndex, typeBatchIndex, jobStart, jobEnd); } } - //Precompute which type batches have *any* integration responsibilities, allowing us to use a all-or-nothing test before dispatching a workblock. - var flagBundleCount = IndexSet.GetBundleCapacity(typeBatch.ConstraintCount); - ulong mergedFlagBundles = 0; - for (int bodyIndexInConstraint = 0; bodyIndexInConstraint < bodiesPerConstraintInTypeBatch; ++bodyIndexInConstraint) + } + if (constraintCount > 4096 + threadDispatcher.ThreadCount * 1024) + { + nextConstraintIntegrationResponsibilityJobIndex = 0; + useSingleThreadedPath = false; + pool.Take(integrationResponsibilityPrepassJobs.Count, out jobAlignedIntegrationResponsibilities); + //for (int i = 0; i < integrationResponsibilityPrepassJobs.Count; ++i) + //{ + // ref var job = ref integrationResponsibilityPrepassJobs[i]; + // jobAlignedIntegrationResponsibilities[i] = ComputeIntegrationResponsibilitiesForConstraintRegion(job.batch, job.typeBatch, job.start, job.end); + //} + threadDispatcher.DispatchWorkers(constraintIntegrationResponsibilitiesWorker); + + //Coarse batch integration responsibilities start uninitialized. Possible to have multiple jobs per type batch in multithreaded case, so we need to init to merge. + for (int i = 1; i < ActiveSet.Batches.Count; ++i) { - ref var integrationFlagsForBodyInTypeBatch = ref integrationFlagsForTypeBatch[bodyIndexInConstraint]; - for (int i = 0; i < flagBundleCount; ++i) + ref var batch = ref ActiveSet.Batches[i]; + for (int j = 0; j < batch.TypeBatches.Count; ++j) { - mergedFlagBundles |= integrationFlagsForBodyInTypeBatch.Flags[i]; + coarseBatchIntegrationResponsibilities[i][j] = false; } } - coarseBatchIntegrationResponsibilities[batchIndex][typeBatchIndex] = mergedFlagBundles != 0; - - //for (int i = 0; i < flagBundleCount; ++i) - //{ - // ulong countMerge = 0; - // for (int bodyIndexInConstraint = 0; bodyIndexInConstraint < bodiesPerConstraintInTypeBatch; ++bodyIndexInConstraint) - // { - // var flagsForBody = integrationFlagsForTypeBatch[bodyIndexInConstraint].Flags[i]; - // countMerge |= flagsForBody; - // integratingLanes += Popcnt.X64.PopCount(flagsForBody); - // totalLanes += (ulong)Math.Min(64, typeBatch.ConstraintCount - i * 64); - // } - // integratingConstraintCount += Popcnt.X64.PopCount(countMerge); - // totalConstraintCount += (ulong)Math.Min(64, typeBatch.ConstraintCount - i * 64); - //} + for (int i = 0; i < integrationResponsibilityPrepassJobs.Count; ++i) + { + ref var job = ref integrationResponsibilityPrepassJobs[i]; + coarseBatchIntegrationResponsibilities[job.batch][job.typeBatch] |= jobAlignedIntegrationResponsibilities[i]; + } + pool.Return(ref jobAlignedIntegrationResponsibilities); + } + integrationResponsibilityPrepassJobs.Dispose(pool); + } + if (useSingleThreadedPath) + { + for (int i = 1; i < ActiveSet.Batches.Count; ++i) + { + if (!batchHasAnyIntegrationResponsibilities[i]) + continue; + ref var batch = ref ActiveSet.Batches[i]; + for (int j = 0; j < batch.TypeBatches.Count; ++j) + { + ref var typeBatch = ref batch.TypeBatches[j]; + coarseBatchIntegrationResponsibilities[i][j] = ComputeIntegrationResponsibilitiesForConstraintRegion(i, j, 0, typeBatch.ConstraintCount); + } } - //Console.WriteLine($"Batch {batchIndex} integrating constraints: {integratingConstraintCount} over {totalConstraintCount}, {integratingConstraintCount / (double)totalConstraintCount}"); - //Console.WriteLine($"Batch {batchIndex} integrating lanes: {integratingLanes} over {totalLanes}, {integratingLanes / (double)totalLanes}"); + + } + + //for (int batchIndex = 1; batchIndex < bodiesFirstObservedInBatches.Length; ++batchIndex) + //{ + // if (!batchHasAnyIntegrationResponsibilities[batchIndex]) + // continue; + // ref var integrationFlagsForBatch = ref integrationFlags[batchIndex]; + // ref var firstObservedForBatch = ref bodiesFirstObservedInBatches[batchIndex]; + // ref var batch = ref ActiveSet.Batches[batchIndex]; + + // //ulong totalConstraintCount = 0; + // //ulong integratingConstraintCount = 0; + // //ulong totalLanes = 0; + // //ulong integratingLanes = 0; + + // for (int typeBatchIndex = 0; typeBatchIndex < batch.TypeBatches.Count; ++typeBatchIndex) + // { + // ref var integrationFlagsForTypeBatch = ref integrationFlagsForBatch[typeBatchIndex]; + // ref var typeBatch = ref batch.TypeBatches[typeBatchIndex]; + // var typeBatchBodyReferences = typeBatch.BodyReferences.As(); + // var bodiesPerConstraintInTypeBatch = TypeProcessors[typeBatch.TypeId].BodiesPerConstraint; + // var intsPerBundle = Vector.Count * bodiesPerConstraintInTypeBatch; + // for (int bundleIndex = 0; bundleIndex < typeBatch.BundleCount; ++bundleIndex) + // { + // int bundleStartIndexInConstraints = bundleIndex * Vector.Count; + // int countInBundle = Math.Min(Vector.Count, typeBatch.ConstraintCount - bundleStartIndexInConstraints); + // //Body references are stored in AOSOA layout. + // var bundleBodyReferencesStart = typeBatchBodyReferences.Memory + bundleIndex * intsPerBundle; + // for (int bodyIndexInConstraint = 0; bodyIndexInConstraint < bodiesPerConstraintInTypeBatch; ++bodyIndexInConstraint) + // { + // ref var integrationFlagsForBodyInConstraint = ref integrationFlagsForTypeBatch[bodyIndexInConstraint]; + // var bundleStart = bundleBodyReferencesStart + bodyIndexInConstraint * Vector.Count; + // for (int bundleInnerIndex = 0; bundleInnerIndex < countInBundle; ++bundleInnerIndex) + // { + // //Constraints refer to bodies by index when they're in the active set, so we need to transform to handle to look up our merged batch results. + // var bodyHandle = bodies.ActiveSet.IndexToHandle[bundleStart[bundleInnerIndex]].Value; + // if (firstObservedForBatch.Contains(bodyHandle)) + // { + // integrationFlagsForBodyInConstraint.AddUnsafely(bundleStartIndexInConstraints + bundleInnerIndex); + // } + // } + // } + // } + // //Precompute which type batches have *any* integration responsibilities, allowing us to use a all-or-nothing test before dispatching a workblock. + // var flagBundleCount = IndexSet.GetBundleCapacity(typeBatch.ConstraintCount); + // ulong mergedFlagBundles = 0; + // for (int bodyIndexInConstraint = 0; bodyIndexInConstraint < bodiesPerConstraintInTypeBatch; ++bodyIndexInConstraint) + // { + // ref var integrationFlagsForBodyInTypeBatch = ref integrationFlagsForTypeBatch[bodyIndexInConstraint]; + // for (int i = 0; i < flagBundleCount; ++i) + // { + // mergedFlagBundles |= integrationFlagsForBodyInTypeBatch.Flags[i]; + // } + // } + // coarseBatchIntegrationResponsibilities[batchIndex][typeBatchIndex] = mergedFlagBundles != 0; + + // //for (int i = 0; i < flagBundleCount; ++i) + // //{ + // // ulong countMerge = 0; + // // for (int bodyIndexInConstraint = 0; bodyIndexInConstraint < bodiesPerConstraintInTypeBatch; ++bodyIndexInConstraint) + // // { + // // var flagsForBody = integrationFlagsForTypeBatch[bodyIndexInConstraint].Flags[i]; + // // countMerge |= flagsForBody; + // // integratingLanes += Popcnt.X64.PopCount(flagsForBody); + // // totalLanes += (ulong)Math.Min(64, typeBatch.ConstraintCount - i * 64); + // // } + // // integratingConstraintCount += Popcnt.X64.PopCount(countMerge); + // // totalConstraintCount += (ulong)Math.Min(64, typeBatch.ConstraintCount - i * 64); + // //} + // } + // //Console.WriteLine($"Batch {batchIndex} integrating constraints: {integratingConstraintCount} over {totalConstraintCount}, {integratingConstraintCount / (double)totalConstraintCount}"); + // //Console.WriteLine($"Batch {batchIndex} integrating lanes: {integratingLanes} over {totalLanes}, {integratingLanes / (double)totalLanes}"); + //} pool.Return(ref batchHasAnyIntegrationResponsibilities); //for (int batchIndex = 1; batchIndex < bodiesFirstObservedInBatches.Length; ++batchIndex) //{ From 8887a7775e682a84da8b2a67630bf7d328d19d97 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 14 Jul 2021 16:11:56 -0500 Subject: [PATCH 108/947] Refactor/cleanup. --- BepuPhysics/Solver_SubsteppingSolve2.cs | 120 ++---------------------- 1 file changed, 6 insertions(+), 114 deletions(-) diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index 798159f2a..42821ac37 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -220,6 +220,8 @@ unsafe bool ComputeIntegrationResponsibilitiesForConstraintRegion(int batchIndex } } //Precompute which type batches have *any* integration responsibilities, allowing us to use a all-or-nothing test before dispatching a workblock. + //Note that this could be vectorized the same way we did in the batch merging, but... less likely to be useful. Less constraints in sequence in type batches, + //and we're already within a multithreaded context. Saving 1 microsecond not terribly meaningful. var flagBundleCount = IndexSet.GetBundleCapacity(typeBatch.ConstraintCount); ulong mergedFlagBundles = 0; for (int bodyIndexInConstraint = 0; bodyIndexInConstraint < bodiesPerConstraintInTypeBatch; ++bodyIndexInConstraint) @@ -279,6 +281,8 @@ public override unsafe void PrepareConstraintIntegrationResponsibilities(IThread } } } + + //Brute force fallback for debugging: //for (int i = 0; i < bodies.ActiveSet.Count; ++i) //{ // ref var constraints = ref bodies.ActiveSet.Constraints[i]; @@ -305,9 +309,6 @@ public override unsafe void PrepareConstraintIntegrationResponsibilities(IThread // indexSet.AddUnsafely(location.IndexInTypeBatch); // } //} - //Console.WriteLine($"body count: {bodies.ActiveSet.Count}, constraint count: {CountConstraints()}"); - //var end = Stopwatch.GetTimestamp(); - //Console.WriteLine($"Brute force time (ms): {(end - start) * 1e3 / Stopwatch.Frequency}"); pool.Take(batchReferencedHandles.Count, out bodiesFirstObservedInBatches); IndexSet merged; @@ -455,120 +456,11 @@ public override unsafe void PrepareConstraintIntegrationResponsibilities(IThread coarseBatchIntegrationResponsibilities[i][j] = ComputeIntegrationResponsibilitiesForConstraintRegion(i, j, 0, typeBatch.ConstraintCount); } } - - } - //for (int batchIndex = 1; batchIndex < bodiesFirstObservedInBatches.Length; ++batchIndex) - //{ - // if (!batchHasAnyIntegrationResponsibilities[batchIndex]) - // continue; - // ref var integrationFlagsForBatch = ref integrationFlags[batchIndex]; - // ref var firstObservedForBatch = ref bodiesFirstObservedInBatches[batchIndex]; - // ref var batch = ref ActiveSet.Batches[batchIndex]; - - // //ulong totalConstraintCount = 0; - // //ulong integratingConstraintCount = 0; - // //ulong totalLanes = 0; - // //ulong integratingLanes = 0; - - // for (int typeBatchIndex = 0; typeBatchIndex < batch.TypeBatches.Count; ++typeBatchIndex) - // { - // ref var integrationFlagsForTypeBatch = ref integrationFlagsForBatch[typeBatchIndex]; - // ref var typeBatch = ref batch.TypeBatches[typeBatchIndex]; - // var typeBatchBodyReferences = typeBatch.BodyReferences.As(); - // var bodiesPerConstraintInTypeBatch = TypeProcessors[typeBatch.TypeId].BodiesPerConstraint; - // var intsPerBundle = Vector.Count * bodiesPerConstraintInTypeBatch; - // for (int bundleIndex = 0; bundleIndex < typeBatch.BundleCount; ++bundleIndex) - // { - // int bundleStartIndexInConstraints = bundleIndex * Vector.Count; - // int countInBundle = Math.Min(Vector.Count, typeBatch.ConstraintCount - bundleStartIndexInConstraints); - // //Body references are stored in AOSOA layout. - // var bundleBodyReferencesStart = typeBatchBodyReferences.Memory + bundleIndex * intsPerBundle; - // for (int bodyIndexInConstraint = 0; bodyIndexInConstraint < bodiesPerConstraintInTypeBatch; ++bodyIndexInConstraint) - // { - // ref var integrationFlagsForBodyInConstraint = ref integrationFlagsForTypeBatch[bodyIndexInConstraint]; - // var bundleStart = bundleBodyReferencesStart + bodyIndexInConstraint * Vector.Count; - // for (int bundleInnerIndex = 0; bundleInnerIndex < countInBundle; ++bundleInnerIndex) - // { - // //Constraints refer to bodies by index when they're in the active set, so we need to transform to handle to look up our merged batch results. - // var bodyHandle = bodies.ActiveSet.IndexToHandle[bundleStart[bundleInnerIndex]].Value; - // if (firstObservedForBatch.Contains(bodyHandle)) - // { - // integrationFlagsForBodyInConstraint.AddUnsafely(bundleStartIndexInConstraints + bundleInnerIndex); - // } - // } - // } - // } - // //Precompute which type batches have *any* integration responsibilities, allowing us to use a all-or-nothing test before dispatching a workblock. - // var flagBundleCount = IndexSet.GetBundleCapacity(typeBatch.ConstraintCount); - // ulong mergedFlagBundles = 0; - // for (int bodyIndexInConstraint = 0; bodyIndexInConstraint < bodiesPerConstraintInTypeBatch; ++bodyIndexInConstraint) - // { - // ref var integrationFlagsForBodyInTypeBatch = ref integrationFlagsForTypeBatch[bodyIndexInConstraint]; - // for (int i = 0; i < flagBundleCount; ++i) - // { - // mergedFlagBundles |= integrationFlagsForBodyInTypeBatch.Flags[i]; - // } - // } - // coarseBatchIntegrationResponsibilities[batchIndex][typeBatchIndex] = mergedFlagBundles != 0; - - // //for (int i = 0; i < flagBundleCount; ++i) - // //{ - // // ulong countMerge = 0; - // // for (int bodyIndexInConstraint = 0; bodyIndexInConstraint < bodiesPerConstraintInTypeBatch; ++bodyIndexInConstraint) - // // { - // // var flagsForBody = integrationFlagsForTypeBatch[bodyIndexInConstraint].Flags[i]; - // // countMerge |= flagsForBody; - // // integratingLanes += Popcnt.X64.PopCount(flagsForBody); - // // totalLanes += (ulong)Math.Min(64, typeBatch.ConstraintCount - i * 64); - // // } - // // integratingConstraintCount += Popcnt.X64.PopCount(countMerge); - // // totalConstraintCount += (ulong)Math.Min(64, typeBatch.ConstraintCount - i * 64); - // //} - // } - // //Console.WriteLine($"Batch {batchIndex} integrating constraints: {integratingConstraintCount} over {totalConstraintCount}, {integratingConstraintCount / (double)totalConstraintCount}"); - // //Console.WriteLine($"Batch {batchIndex} integrating lanes: {integratingLanes} over {totalLanes}, {integratingLanes / (double)totalLanes}"); - //} pool.Return(ref batchHasAnyIntegrationResponsibilities); - //for (int batchIndex = 1; batchIndex < bodiesFirstObservedInBatches.Length; ++batchIndex) - //{ - // ref var integrationFlagsForBatch = ref integrationFlags[batchIndex]; - // ref var firstObservedForBatch = ref bodiesFirstObservedInBatches[batchIndex]; - // ref var batch = ref ActiveSet.Batches[batchIndex]; - // for (int typeBatchIndex = 0; typeBatchIndex < batch.TypeBatches.Count; ++typeBatchIndex) - // { - // ref var integrationFlagsForTypeBatch = ref integrationFlagsForBatch[typeBatchIndex]; - // ref var typeBatch = ref batch.TypeBatches[typeBatchIndex]; - // var typeBatchBodyReferences = typeBatch.BodyReferences.As(); - // var bodiesPerConstraintInTypeBatch = TypeProcessors[typeBatch.TypeId].BodiesPerConstraint; - // var intsPerBundle = Vector.Count * bodiesPerConstraintInTypeBatch; - // //We process over one strip of bodies in the constraint in each pass, constructing contiguous flag sequences as we go. - // for (int bodyIndexInConstraint = 0; bodyIndexInConstraint < bodiesPerConstraintInTypeBatch; ++bodyIndexInConstraint) - // { - // ref var integrationFlagsForBodyInConstraint = ref integrationFlagsForTypeBatch[bodyIndexInConstraint]; - // for (int flagBundleIndex = 0; flagBundleIndex < integrationFlagsForBodyInConstraint.Flags.Length; ++flagBundleIndex) - // { - // ulong flagBundle = 0; - // var constraintBundleStartIndexInConstraints = flagBundleIndex * 64; - // int countInFlagBundle = Math.Min(64, typeBatch.ConstraintCount - constraintBundleStartIndexInConstraints); - // for (int indexInFlagBundle = 0; indexInFlagBundle < countInFlagBundle; ++indexInFlagBundle) - // { - // var constraintIndex = constraintBundleStartIndexInConstraints + indexInFlagBundle; - // BundleIndexing.GetBundleIndices(constraintIndex, out var constraintBundleIndex, out var constraintIndexInBundle); - // var bodyHandle = bodies.ActiveSet.IndexToHandle[typeBatchBodyReferences.Memory[constraintBundleIndex * intsPerBundle + Vector.Count * bodyIndexInConstraint]].Value; - // if (firstObservedForBatch.Contains(bodyHandle)) - // { - // integrationFlagsForBodyInConstraint.AddUnsafely(constraintIndex); - // } - // } - // integrationFlagsForBodyInConstraint.Flags[flagBundleIndex] = flagBundle; - // } - // } - // } - //} - var end = Stopwatch.GetTimestamp(); - //Console.WriteLine($"Time (ms): {(end - start) * 1e3 / Stopwatch.Frequency}"); + + //Validation: //for (int i = 0; i < bodies.ActiveSet.Count; ++i) //{ // ref var constraints = ref bodies.ActiveSet.Constraints[i]; From 36e9e4037c179ec80435bac763794efa62611785 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 14 Jul 2021 19:44:10 -0500 Subject: [PATCH 109/947] Bundled substepping and iterations into embedded substepping solve. --- .../EmbeddedSubsteppingTimestepper2.cs | 29 +---- BepuPhysics/Solver_SubsteppingSolve2.cs | 117 ++++++++++-------- Demos/Demos/NewtDemo.cs | 4 +- 3 files changed, 68 insertions(+), 82 deletions(-) diff --git a/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs b/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs index 7a6506987..223571260 100644 --- a/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs +++ b/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs @@ -76,32 +76,11 @@ public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDi CollisionsDetected?.Invoke(dt, threadDispatcher); Debug.Assert(SubstepCount >= 0, "Substep count should be positive."); - var substepDt = dt / SubstepCount; - simulation.Solver.PrepareConstraintIntegrationResponsibilities(threadDispatcher); - for (int substepIndex = 0; substepIndex < SubstepCount; ++substepIndex) - { - SubstepStarted?.Invoke(substepIndex, dt, threadDispatcher); - if (substepIndex > 0) - { - //This takes the place of collision detection for the substeps. It uses the current velocity to update penetration depths. - //It's definitely an approximation, but it's important for avoiding some obviously weird behavior. - //Note that we do not run this on the first iteration- the actual collision detection above takes care of it. - simulation.IncrementallyUpdateContactConstraints(substepDt, threadDispatcher); - ContactConstraintsUpdatedForSubstep?.Invoke(substepIndex, substepDt, threadDispatcher); - } - //simulation.IntegrateVelocitiesAndUpdateInertias(substepDt, threadDispatcher); - //VelocitiesIntegrated?.Invoke(substepIndex, substepDt, threadDispatcher); - - simulation.Profiler.Start(simulation.Solver); - simulation.Solver.SolveStep2(substepDt, threadDispatcher); - simulation.Profiler.End(simulation.Solver); - ConstraintsSolved?.Invoke(substepIndex, substepDt, threadDispatcher); - - //simulation.IntegratePoses(substepDt, threadDispatcher); - //PosesIntegrated?.Invoke(substepIndex, substepDt, threadDispatcher); - SubstepEnded?.Invoke(substepIndex, substepDt, threadDispatcher); - } + simulation.Solver.PrepareConstraintIntegrationResponsibilities(SubstepCount, threadDispatcher); + simulation.Profiler.Start(simulation.Solver); + simulation.Solver.SolveStep2(dt, threadDispatcher); + simulation.Profiler.End(simulation.Solver); SubstepsComplete?.Invoke(dt, threadDispatcher); simulation.Solver.DisposeConstraintIntegrationResponsibilities(); diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index 42821ac37..3fb857790 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -17,7 +17,7 @@ namespace BepuPhysics { public partial class Solver { - public virtual void PrepareConstraintIntegrationResponsibilities(IThreadDispatcher threadDispatcher = null) + public virtual void PrepareConstraintIntegrationResponsibilities(int substepCount, IThreadDispatcher threadDispatcher = null) { } public virtual void DisposeConstraintIntegrationResponsibilities() @@ -151,38 +151,35 @@ void SolveStep2Worker(int workerIndex) InverseDt = 1f / context.Dt, solver = this }; - for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) - { - var batchOffset = batchIndex > 0 ? context.BatchBoundaries[batchIndex - 1] : 0; - ExecuteStage(ref warmstartStage, ref context.ConstraintBlocks, ref bounds, ref boundsBackBuffer, workerIndex, batchOffset, context.BatchBoundaries[batchIndex], - ref batchStarts[batchIndex], ref syncStage, claimedState, unclaimedState); - } - claimedState ^= 1; - unclaimedState ^= 1; var solveStage = new SolveStep2StageFunction { Dt = context.Dt, InverseDt = 1f / context.Dt, solver = this }; - for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) + + for (int i = 0; i < substepCount; ++i) { - var batchOffset = batchIndex > 0 ? context.BatchBoundaries[batchIndex - 1] : 0; - ExecuteStage(ref solveStage, ref context.ConstraintBlocks, ref bounds, ref boundsBackBuffer, workerIndex, batchOffset, context.BatchBoundaries[batchIndex], - ref batchStarts[batchIndex], ref syncStage, claimedState, unclaimedState); + for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) + { + var batchOffset = batchIndex > 0 ? context.BatchBoundaries[batchIndex - 1] : 0; + ExecuteStage(ref warmstartStage, ref context.ConstraintBlocks, ref bounds, ref boundsBackBuffer, workerIndex, batchOffset, context.BatchBoundaries[batchIndex], + ref batchStarts[batchIndex], ref syncStage, claimedState, unclaimedState); + } + claimedState ^= 1; + unclaimedState ^= 1; + for (int j = 0; j < IterationCount; ++j) + { + for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) + { + var batchOffset = batchIndex > 0 ? context.BatchBoundaries[batchIndex - 1] : 0; + ExecuteStage(ref solveStage, ref context.ConstraintBlocks, ref bounds, ref boundsBackBuffer, workerIndex, batchOffset, context.BatchBoundaries[batchIndex], + ref batchStarts[batchIndex], ref syncStage, claimedState, unclaimedState); + } + claimedState ^= 1; + unclaimedState ^= 1; + } } - //if (fallbackExists) - //{ - // var solveFallbackStage = new FallbackSolveStepStageFunction(); - // var fallbackScatterStage = new FallbackScatterStageFunction(); - // var batchOffset = FallbackBatchThreshold > 0 ? context.BatchBoundaries[FallbackBatchThreshold - 1] : 0; - // ExecuteStage(ref solveFallbackStage, ref context.ConstraintBlocks, ref bounds, ref boundsBackBuffer, workerIndex, batchOffset, context.BatchBoundaries[FallbackBatchThreshold], - // ref batchStarts[FallbackBatchThreshold], ref syncStage, claimedState, unclaimedState); - // ExecuteStage(ref fallbackScatterStage, ref context.FallbackBlocks, ref bounds, ref boundsBackBuffer, - // workerIndex, 0, context.FallbackBlocks.Blocks.Count, ref fallbackStart, ref syncStage, claimedState, unclaimedState); - //} - claimedState ^= 1; - unclaimedState ^= 1; } @@ -257,8 +254,11 @@ void ConstraintIntegrationResponsibilitiesWorker(int workerIndex) Buffer> coarseBatchIntegrationResponsibilities; Action constraintIntegrationResponsibilitiesWorker; - public override unsafe void PrepareConstraintIntegrationResponsibilities(IThreadDispatcher threadDispatcher = null) + int substepCount; + public override unsafe void PrepareConstraintIntegrationResponsibilities(int substepCount, IThreadDispatcher threadDispatcher = null) { + //TODO: we're caching it on a per call basis because we are still using the old substeppingtimestepper frame externally. Once we bite the bullet 100% on bundling, we can make it equivalent to IterationCount. + this.substepCount = substepCount; //var start = Stopwatch.GetTimestamp(); pool.Take(ActiveSet.Batches.Count, out integrationFlags); integrationFlags[0] = default; @@ -459,8 +459,8 @@ public override unsafe void PrepareConstraintIntegrationResponsibilities(IThread } pool.Return(ref batchHasAnyIntegrationResponsibilities); - - //Validation: + + ////Validation: //for (int i = 0; i < bodies.ActiveSet.Count; ++i) //{ // ref var constraints = ref bodies.ActiveSet.Constraints[i]; @@ -521,57 +521,64 @@ public override void DisposeConstraintIntegrationResponsibilities() pool.Return(ref coarseBatchIntegrationResponsibilities); } - public override void SolveStep2(float dt, IThreadDispatcher threadDispatcher = null) + public override void SolveStep2(float totalDt, IThreadDispatcher threadDispatcher = null) { + var substepDt = totalDt / substepCount; if (threadDispatcher == null) { - var inverseDt = 1f / dt; + var inverseDt = 1f / substepDt; ref var activeSet = ref ActiveSet; GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); Debug.Assert(!fallbackExists, "Not handling this yet."); - for (int i = 0; i < synchronizedBatchCount; ++i) + for (int substepIndex = 0; substepIndex < substepCount; ++substepIndex) { - ref var batch = ref activeSet.Batches[i]; - ref var integrationFlagsForBatch = ref integrationFlags[i]; - for (int j = 0; j < batch.TypeBatches.Count; ++j) + for (int i = 0; i < synchronizedBatchCount; ++i) { - ref var typeBatch = ref batch.TypeBatches[j]; - if (i == 0) - { - Buffer noFlagsRequired = default; - TypeProcessors[typeBatch.TypeId].WarmStart2(ref typeBatch, ref noFlagsRequired, bodies, ref PoseIntegrator.Callbacks, - dt, inverseDt, 0, typeBatch.BundleCount, 0); - } - else + ref var batch = ref activeSet.Batches[i]; + ref var integrationFlagsForBatch = ref integrationFlags[i]; + for (int j = 0; j < batch.TypeBatches.Count; ++j) { - if (coarseBatchIntegrationResponsibilities[i][j]) + ref var typeBatch = ref batch.TypeBatches[j]; + if (i == 0) { - TypeProcessors[typeBatch.TypeId].WarmStart2(ref typeBatch, ref integrationFlagsForBatch[j], bodies, ref PoseIntegrator.Callbacks, - dt, inverseDt, 0, typeBatch.BundleCount, 0); + Buffer noFlagsRequired = default; + TypeProcessors[typeBatch.TypeId].WarmStart2(ref typeBatch, ref noFlagsRequired, bodies, ref PoseIntegrator.Callbacks, + substepDt, inverseDt, 0, typeBatch.BundleCount, 0); } else { - TypeProcessors[typeBatch.TypeId].WarmStart2(ref typeBatch, ref integrationFlagsForBatch[j], bodies, ref PoseIntegrator.Callbacks, - dt, inverseDt, 0, typeBatch.BundleCount, 0); + if (coarseBatchIntegrationResponsibilities[i][j]) + { + TypeProcessors[typeBatch.TypeId].WarmStart2(ref typeBatch, ref integrationFlagsForBatch[j], bodies, ref PoseIntegrator.Callbacks, + substepDt, inverseDt, 0, typeBatch.BundleCount, 0); + } + else + { + TypeProcessors[typeBatch.TypeId].WarmStart2(ref typeBatch, ref integrationFlagsForBatch[j], bodies, ref PoseIntegrator.Callbacks, + substepDt, inverseDt, 0, typeBatch.BundleCount, 0); + } } } } - } - for (int i = 0; i < synchronizedBatchCount; ++i) - { - ref var batch = ref activeSet.Batches[i]; - for (int j = 0; j < batch.TypeBatches.Count; ++j) + for (int iterationIndex = 0; iterationIndex < IterationCount; ++iterationIndex) { - ref var typeBatch = ref batch.TypeBatches[j]; - TypeProcessors[typeBatch.TypeId].SolveStep2(ref typeBatch, bodies, dt, inverseDt, 0, typeBatch.BundleCount); + for (int i = 0; i < synchronizedBatchCount; ++i) + { + ref var batch = ref activeSet.Batches[i]; + for (int j = 0; j < batch.TypeBatches.Count; ++j) + { + ref var typeBatch = ref batch.TypeBatches[j]; + TypeProcessors[typeBatch.TypeId].SolveStep2(ref typeBatch, bodies, substepDt, inverseDt, 0, typeBatch.BundleCount); + } + } } } } else { - ExecuteMultithreaded(dt, threadDispatcher, solveStep2Worker); + ExecuteMultithreaded(substepDt, threadDispatcher, solveStep2Worker); } } } diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index 920b2adb7..8d56a4978 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -823,9 +823,9 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper(1)); - Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)), new EmbeddedSubsteppingTimestepper2(3)); + Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)), new EmbeddedSubsteppingTimestepper2(3), solverIterationCount: 1); //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)), new PositionFirstTimestepper2(), solverIterationCount: 5); - //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper2(3), solverIterationCount: 1); + //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)), new SubsteppingTimestepper2(3), solverIterationCount: 1); var meshContent = content.Load("Content\\newt.obj"); float cellSize = 0.1f; From d7a5d04f32814f959f27606215a1aa2810c5fc0d Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 15 Jul 2021 11:57:48 -0500 Subject: [PATCH 110/947] Alignified a couple of loads. --- BepuPhysics/Bodies_GatherScatter.cs | 7 ++++--- BepuPhysics/Solver_SubsteppingSolve2.cs | 6 +++--- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/BepuPhysics/Bodies_GatherScatter.cs b/BepuPhysics/Bodies_GatherScatter.cs index f37dd036f..1f4725b72 100644 --- a/BepuPhysics/Bodies_GatherScatter.cs +++ b/BepuPhysics/Bodies_GatherScatter.cs @@ -134,6 +134,7 @@ unsafe static void FallbackGatherInertia(int count, BodyInertias* inertias, ref pMass[i] = inertiaValues[6]; } } + //[MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe void GatherMotionState(ref Vector bodyIndices, int count, out Vector3Wide position, out QuaternionWide orientation, out BodyVelocityWide velocity) { @@ -166,7 +167,7 @@ public unsafe void GatherMotionState(ref Vector bodyIndices, int count, out { //Load every body for the first half of the motion state. //Note that buffers are allocated on cache line boundaries, so we can use aligned loads for all that matters. - var m0 = Avx.LoadVector256(s0); + var m0 = Avx.LoadAlignedVector256(s0); var m1 = count > 1 ? Avx.LoadAlignedVector256(s1) : Vector256.Zero; var m2 = count > 2 ? Avx.LoadAlignedVector256(s2) : Vector256.Zero; var m3 = count > 3 ? Avx.LoadAlignedVector256(s3) : Vector256.Zero; @@ -204,7 +205,7 @@ public unsafe void GatherMotionState(ref Vector bodyIndices, int count, out { //Second half. - var m0 = Avx.LoadVector256(s0 + 8); + var m0 = Avx.LoadAlignedVector256(s0 + 8); var m1 = count > 1 ? Avx.LoadAlignedVector256(s1 + 8) : Vector256.Zero; var m2 = count > 2 ? Avx.LoadAlignedVector256(s2 + 8) : Vector256.Zero; var m3 = count > 3 ? Avx.LoadAlignedVector256(s3 + 8) : Vector256.Zero; @@ -268,7 +269,7 @@ unsafe void GatherInertia(ref Vector bodyIndices, int count, int offsetInFl var s7 = (float*)(inertias + indices[7]) + offsetInFloats; //Load every inertia vector. - var m0 = Avx.LoadVector256(s0); + var m0 = Avx.LoadAlignedVector256(s0); var m1 = count > 1 ? Avx.LoadAlignedVector256(s1) : Vector256.Zero; var m2 = count > 2 ? Avx.LoadAlignedVector256(s2) : Vector256.Zero; var m3 = count > 3 ? Avx.LoadAlignedVector256(s3) : Vector256.Zero; diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index 3fb857790..27873fbdf 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -259,7 +259,6 @@ public override unsafe void PrepareConstraintIntegrationResponsibilities(int sub { //TODO: we're caching it on a per call basis because we are still using the old substeppingtimestepper frame externally. Once we bite the bullet 100% on bundling, we can make it equivalent to IterationCount. this.substepCount = substepCount; - //var start = Stopwatch.GetTimestamp(); pool.Take(ActiveSet.Batches.Count, out integrationFlags); integrationFlags[0] = default; pool.Take(ActiveSet.Batches.Count, out coarseBatchIntegrationResponsibilities); @@ -382,7 +381,7 @@ public override unsafe void PrepareConstraintIntegrationResponsibilities(int sub batchHasAnyIntegrationResponsibilities[batchIndex] = horizontalMerge != 0; } } - var start = Stopwatch.GetTimestamp(); + //var start = Stopwatch.GetTimestamp(); //We now have index sets representing the first time each body handle is observed in a batch. //This process is significantly more expensive than the batch merging phase and can benefit from multithreading. //It is still fairly cheap, though- we can't use really fine grained jobs or the cost of swapping jobs will exceed productive work. @@ -457,7 +456,8 @@ public override unsafe void PrepareConstraintIntegrationResponsibilities(int sub } } } - + //var end = Stopwatch.GetTimestamp(); + //Console.WriteLine($"time (ms): {(end - start) * 1e3 / Stopwatch.Frequency}"); pool.Return(ref batchHasAnyIntegrationResponsibilities); ////Validation: From 372c4f76293b7e15070f7b290c8f646a36eca9c6 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 15 Jul 2021 17:02:10 -0500 Subject: [PATCH 111/947] Trying and failing to get skylake to not barf. --- BepuPhysics/Bodies_GatherScatter.cs | 17 +++---- BepuPhysics/Constraints/Weld.cs | 77 ++++++++++++++++++++--------- BepuUtilities/Symmetric6x6Wide.cs | 6 +-- 3 files changed, 65 insertions(+), 35 deletions(-) diff --git a/BepuPhysics/Bodies_GatherScatter.cs b/BepuPhysics/Bodies_GatherScatter.cs index 1f4725b72..3970fab0e 100644 --- a/BepuPhysics/Bodies_GatherScatter.cs +++ b/BepuPhysics/Bodies_GatherScatter.cs @@ -251,7 +251,7 @@ public unsafe void GatherMotionState(ref Vector bodyIndices, int count, out } - [MethodImpl(MethodImplOptions.AggressiveInlining)] + //[MethodImpl(MethodImplOptions.AggressiveInlining)] unsafe void GatherInertia(ref Vector bodyIndices, int count, int offsetInFloats, out BodyInertiaWide inertia) { var inertias = ActiveSet.Inertias.Memory; @@ -312,12 +312,12 @@ unsafe void GatherInertia(ref Vector bodyIndices, int count, int offsetInFl } } - //[MethodImpl(MethodImplOptions.AggressiveInlining)] + [MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe void GatherWorldInertia(ref Vector bodyIndices, int count, out BodyInertiaWide inertia) { GatherInertia(ref bodyIndices, count, 8, out inertia); } - //[MethodImpl(MethodImplOptions.AggressiveInlining)] + [MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe void GatherLocalInertia(ref Vector bodyIndices, int count, out BodyInertiaWide inertia) { GatherInertia(ref bodyIndices, count, 0, out inertia); @@ -520,7 +520,7 @@ public void GatherState(ref FourBodyReferences references, int count, } - [MethodImpl(MethodImplOptions.AggressiveInlining)] + //[MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe void ScatterPoseAndInertia( ref Vector3Wide position, ref QuaternionWide orientation, ref BodyInertiaWide inertia, ref Vector references, int count, ref Vector mask) where TBatchIntegrationMode : struct, IBatchIntegrationMode @@ -644,7 +644,7 @@ public unsafe void ScatterPoseAndInertia( - [MethodImpl(MethodImplOptions.AggressiveInlining)] + //[MethodImpl(MethodImplOptions.AggressiveInlining)] private unsafe void ScatterVelocities(ref BodyVelocityWide sourceVelocities, ref int baseIndex, int innerIndex) { //TODO: How much value would there be in branching on kinematic state and avoiding a write? Depends a lot on the number of kinematics. @@ -654,7 +654,7 @@ private unsafe void ScatterVelocities(ref BodyVelocityWide sourceVelocities, ref target.Angular = new Vector3(sourceSlot.Angular.X[0], sourceSlot.Angular.Y[0], sourceSlot.Angular.Z[0]); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] + //[MethodImpl(MethodImplOptions.AggressiveInlining)] private unsafe void TransposeScatterVelocities(ref BodyVelocityWide sourceVelocities, MotionState* motionStates, ref Vector references, int count) { if (Avx.IsSupported && Vector.Count == 8) @@ -718,7 +718,7 @@ private unsafe void TransposeScatterVelocities(ref BodyVelocityWide sourceVeloci /// Velocities of body bundle A to scatter. /// Active set indices of the bodies to scatter velocity data to. /// Number of body pairs in the bundle. - [MethodImpl(MethodImplOptions.AggressiveInlining)] + //[MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe void ScatterVelocities(ref BodyVelocityWide sourceVelocities, ref Vector references, int count) { Debug.Assert(count >= 0 && count <= Vector.Count); @@ -738,7 +738,6 @@ public unsafe void ScatterVelocities(ref BodyVelocityWide sourceVelocities, ref /// Active set indices of the bodies to scatter velocity data to. /// Number of body pairs in the bundle. //[MethodImpl(MethodImplOptions.AggressiveInlining)] - [MethodImpl(MethodImplOptions.NoInlining)] public unsafe void ScatterVelocities(ref BodyVelocityWide sourceVelocitiesA, ref BodyVelocityWide sourceVelocitiesB, ref TwoBodyReferences references, int count) { @@ -764,7 +763,7 @@ public unsafe void ScatterVelocities(ref BodyVelocityWide sourceVelocitiesA, ref /// Velocities of body bundle C to scatter. /// Active set indices of the bodies to scatter velocity data to. /// Number of body pairs in the bundle. - [MethodImpl(MethodImplOptions.AggressiveInlining)] + //[MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe void ScatterVelocities( ref BodyVelocityWide sourceVelocitiesA, ref BodyVelocityWide sourceVelocitiesB, ref BodyVelocityWide sourceVelocitiesC, ref ThreeBodyReferences references, int count) diff --git a/BepuPhysics/Constraints/Weld.cs b/BepuPhysics/Constraints/Weld.cs index 024a7dde3..c108b8f29 100644 --- a/BepuPhysics/Constraints/Weld.cs +++ b/BepuPhysics/Constraints/Weld.cs @@ -206,6 +206,29 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB } + ////[MethodImpl(MethodImplOptions.AggressiveInlining)] + //private static void ApplyImpulse(in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, in Vector3Wide offset, in Vector3Wide orientationCSI, in Vector3Wide offsetCSI, ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB) + //{ + // //Recall the jacobians: + // //J = [ 0, I, 0, -I ] + // // [ I, skewSymmetric(localOffset * orientationA), -I, 0 ] + // //The velocity changes are: + // // csi * J * I^-1 + // //linearImpulseA = offsetCSI + // //angularImpulseA = orientationCSI + worldOffset x offsetCSI + // //linearImpulseB = -offsetCSI + // //angularImpulseB = -orientationCSI + // velocityA.Linear += offsetCSI * inertiaA.InverseMass; + + // //Note order of cross relative to the SolveIteration. + // //SolveIteration transforms velocity into constraint space velocity using JT, while this converts constraint space to world space using J. + // //The elements are transposed, and transposed skew symmetric matrices are negated. Flipping the cross product is equivalent to a negation. + // velocityA.Angular += (Cross(offset, offsetCSI) + orientationCSI) * inertiaA.InverseInertiaTensor; + + // velocityB.Linear -= offsetCSI * inertiaB.InverseMass;//note subtraction; the jacobian is -I + // velocityB.Angular -= orientationCSI * inertiaB.InverseInertiaTensor;//note subtraction; the jacobian is -I + //} + //[MethodImpl(MethodImplOptions.AggressiveInlining)] private static void ApplyImpulse(in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, in Vector3Wide offset, in Vector3Wide orientationCSI, in Vector3Wide offsetCSI, ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB) { @@ -218,15 +241,22 @@ private static void ApplyImpulse(in BodyInertiaWide inertiaA, in BodyInertiaWide //angularImpulseA = orientationCSI + worldOffset x offsetCSI //linearImpulseB = -offsetCSI //angularImpulseB = -orientationCSI - velocityA.Linear += offsetCSI * inertiaA.InverseMass; + Scale(offsetCSI, inertiaA.InverseMass, out var linearChangeA); + Add(velocityA.Linear, linearChangeA, out velocityA.Linear); //Note order of cross relative to the SolveIteration. //SolveIteration transforms velocity into constraint space velocity using JT, while this converts constraint space to world space using J. //The elements are transposed, and transposed skew symmetric matrices are negated. Flipping the cross product is equivalent to a negation. - velocityA.Angular += (Cross(offset, offsetCSI) + orientationCSI) * inertiaA.InverseInertiaTensor; + CrossWithoutOverlap(offset, offsetCSI, out var offsetWorldImpulse); + Add(offsetWorldImpulse, orientationCSI, out var angularImpulseA); + TransformWithoutOverlap(angularImpulseA, inertiaA.InverseInertiaTensor, out var angularChangeA); + Add(velocityA.Angular, angularChangeA, out velocityA.Angular); + + Scale(offsetCSI, inertiaB.InverseMass, out var negatedLinearChangeB); + Subtract(velocityB.Linear, negatedLinearChangeB, out velocityB.Linear); //note subtraction; the jacobian is -I - velocityB.Linear -= offsetCSI * inertiaB.InverseMass;//note subtraction; the jacobian is -I - velocityB.Angular -= orientationCSI * inertiaB.InverseInertiaTensor;//note subtraction; the jacobian is -I + TransformWithoutOverlap(orientationCSI, inertiaB.InverseInertiaTensor, out var negatedAngularChangeB); + Subtract(velocityB.Angular, negatedAngularChangeB, out velocityB.Angular); //note subtraction; the jacobian is -I } //[MethodImpl(MethodImplOptions.NoInlining)] @@ -257,7 +287,23 @@ public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. //var offset = prestep.LocalOffset * orientationA; - Transform(prestep.LocalOffset, orientationA, out var offset); + Transform(prestep.LocalOffset, orientationA, out var offset); + + //Effective mass = (J * M^-1 * JT)^-1, which is going to be a little tricky because J * M^-1 * JT is a 6x6 matrix: + //J * M^-1 * JT = [ Ia^-1 + Ib^-1, Ia^-1 * transpose(skewSymmetric(localOffset * orientationA)) ] + // [ skewSymmetric(localOffset * orientationA) * Ia^-1, Ma^-1 + Mb^-1 + skewSymmetric(localOffset * orientationA) * Ia^-1 * transpose(skewSymmetric(localOffset * orientationA)) ] + //where Ia^-1 and Ib^-1 are the inverse inertia tensors for a and b and Ma^-1 and Mb^-1 are the inverse masses of A and B expanded to 3x3 diagonal matrices. + //var jmjtA = inertiaA.InverseInertiaTensor + inertiaB.InverseInertiaTensor; + Add(inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, out var jmjtA); + CreateCrossProduct(offset, out var xAB); + Multiply(inertiaA.InverseInertiaTensor, xAB, out var jmjtB); + //var jmjtB = inertiaA.InverseInertiaTensor * xAB; + CompleteMatrixSandwichTranspose(xAB, jmjtB, out var jmjtD); + var diagonalAdd = inertiaA.InverseMass + inertiaB.InverseMass; + jmjtD.XX += diagonalAdd; + jmjtD.YY += diagonalAdd; + jmjtD.ZZ += diagonalAdd; + var positionError = ab - offset; var targetOrientationB = prestep.LocalOrientation * orientationA; //ConcatenateWithoutOverlap(prestep.LocalOrientation, orientationA, out var targetOrientationB); @@ -265,11 +311,8 @@ public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, GetApproximateAxisAngleFromQuaternion(rotationError, out var rotationErrorAxis, out var rotationErrorLength); SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); - var offsetBiasVelocity = positionError * positionErrorToVelocity; var orientationBiasVelocity = rotationErrorAxis * (rotationErrorLength * positionErrorToVelocity); - //offsetBiasVelocity = default; - //orientationBiasVelocity = default; - + var offsetBiasVelocity = positionError * positionErrorToVelocity; //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); //csi = -accumulatedImpulse * projection.SoftnessImpulseScale - (-biasVelocity + csvaLinear + csvaAngular + csvbLinear + csvbAngular) * effectiveMass; //csi = (biasVelocity - csvaLinear - csvaAngular - csvbLinear - csvbAngular) * effectiveMass - accumulatedImpulse * projection.SoftnessImpulseScale; @@ -287,23 +330,11 @@ public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, offsetCSV.Y = offsetBiasVelocity.Y - wsvA.Linear.Y + wsvB.Linear.Y - (wsvA.Angular.Z * offset.X - wsvA.Angular.X * offset.Z); offsetCSV.Z = offsetBiasVelocity.Z - wsvA.Linear.Z + wsvB.Linear.Z - (wsvA.Angular.X * offset.Y - wsvA.Angular.Y * offset.X); - //Effective mass = (J * M^-1 * JT)^-1, which is going to be a little tricky because J * M^-1 * JT is a 6x6 matrix: - //J * M^-1 * JT = [ Ia^-1 + Ib^-1, Ia^-1 * transpose(skewSymmetric(localOffset * orientationA)) ] - // [ skewSymmetric(localOffset * orientationA) * Ia^-1, Ma^-1 + Mb^-1 + skewSymmetric(localOffset * orientationA) * Ia^-1 * transpose(skewSymmetric(localOffset * orientationA)) ] - //where Ia^-1 and Ib^-1 are the inverse inertia tensors for a and b and Ma^-1 and Mb^-1 are the inverse masses of A and B expanded to 3x3 diagonal matrices. - //var jmjtA = inertiaA.InverseInertiaTensor + inertiaB.InverseInertiaTensor; - Symmetric3x3Wide.Add(inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, out var jmjtA); - CreateCrossProduct(offset, out var xAB); - Multiply(inertiaA.InverseInertiaTensor, xAB, out var jmjtB); - //var jmjtB = inertiaA.InverseInertiaTensor * xAB; - CompleteMatrixSandwichTranspose(xAB, jmjtB, out var jmjtD); - var diagonalAdd = inertiaA.InverseMass + inertiaB.InverseMass; - jmjtD.XX += diagonalAdd; - jmjtD.YY += diagonalAdd; - jmjtD.ZZ += diagonalAdd; //Note that there is no need to invert the 6x6 inverse effective mass matrix chonk. We want to convert a constraint space velocity into a constraint space impulse, csi = csv * effectiveMass. //This is equivalent to solving csi * effectiveMass^-1 = csv for csi, and since effectiveMass^-1 is symmetric positive semidefinite, we can use an LDLT decomposition to quickly solve it. Symmetric6x6Wide.LDLTSolve(orientationCSV, offsetCSV, jmjtA, jmjtB, jmjtD, out var orientationCSI, out var offsetCSI); + //Symmetric6x6Wide.Invert(jmjtA, jmjtB, jmjtD, out var inverse); + //Symmetric6x6Wide.TransformWithoutOverlap(orientationCSV, offsetCSV, inverse, out var orientationCSI, out var offsetCSI); //orientationCSI = orientationCSI * effectiveMassCFMScale - accumulatedImpulses.Orientation * softnessImpulseScale; //offsetCSI = offsetCSI * effectiveMassCFMScale - accumulatedImpulses.Offset * softnessImpulseScale; diff --git a/BepuUtilities/Symmetric6x6Wide.cs b/BepuUtilities/Symmetric6x6Wide.cs index deb81f7e4..4f307c9af 100644 --- a/BepuUtilities/Symmetric6x6Wide.cs +++ b/BepuUtilities/Symmetric6x6Wide.cs @@ -28,7 +28,7 @@ public static void Scale(in Symmetric6x6Wide m, in Vector scale, out Symm Symmetric3x3Wide.Scale(m.D, scale, out result.D); } - //[MethodImpl(MethodImplOptions.AggressiveInlining)] + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Invert(in Symmetric3x3Wide a, in Matrix3x3Wide b, in Symmetric3x3Wide d, out Symmetric6x6Wide result) { // [ A B ]^-1 = [ (A - B * D^-1 * BT)^-1, -(A - B * D^-1 * BT)^-1 * B * D^-1 ] @@ -62,7 +62,7 @@ public static void Invert(in Symmetric6x6Wide m, out Symmetric6x6Wide result) /// Matrix to transform with. /// First half of the result. /// Second half of the result. - [MethodImpl(MethodImplOptions.NoInlining)] + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void TransformWithoutOverlap(in Vector3Wide v0, in Vector3Wide v1, in Symmetric6x6Wide m, out Vector3Wide result0, out Vector3Wide result1) { result0.X = v0.X * m.A.XX + v0.Y * m.A.YX + v0.Z * m.A.ZX + v1.X * m.B.X.X + v1.Y * m.B.X.Y + v1.Z * m.B.X.Z; @@ -85,7 +85,7 @@ public static void TransformWithoutOverlap(in Vector3Wide v0, in Vector3Wide v1, /// Lower right 3x3 region of the matrix. /// First 3 values of the result vector. /// Second 3 values of the result vector. - [MethodImpl(MethodImplOptions.AggressiveInlining)] + //[MethodImpl(MethodImplOptions.AggressiveInlining)] public static void LDLTSolve( in Vector3Wide v0, in Vector3Wide v1, in Symmetric3x3Wide a, in Matrix3x3Wide b, in Symmetric3x3Wide d, out Vector3Wide result0, out Vector3Wide result1) { From 7b61ce9df09c7ebd9b3775b06d0b642291d23d3d Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 16 Jul 2021 14:33:48 -0500 Subject: [PATCH 112/947] Tiny attribute fiddling. --- BepuPhysics/Constraints/TwoBodyTypeProcessor.cs | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 2b4de587e..081ff58f5 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -358,7 +358,7 @@ public enum BundleIntegrationMode All = 2 } - //[MethodImpl(MethodImplOptions.AggressiveInlining)] + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static BundleIntegrationMode BundleShouldIntegrate(int bundleIndex, in IndexSet integrationFlags, out Vector integrationMask) { Debug.Assert(Vector.Count <= 32, "Wait, what? The integration mask isn't big enough to handle a vector this big."); @@ -407,7 +407,7 @@ public static BundleIntegrationMode BundleShouldIntegrate(int bundleIndex, in In integrationMask = default; return BundleIntegrationMode.None; } - + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static unsafe void IntegratePoseAndVelocity( ref TIntegratorCallbacks integratorCallbacks, ref Vector bodyIndices, int count, in BodyInertiaWide localInertia, float dt, Vector integrationMask, ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocityWide velocity, @@ -452,7 +452,7 @@ public static unsafe void IntegratePoseAndVelocity( Vector3Wide.ConditionalSelect(integrationMask, velocity.Linear, previousVelocity.Linear, out velocity.Linear); Vector3Wide.ConditionalSelect(integrationMask, velocity.Angular, previousVelocity.Angular, out velocity.Angular); } - + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static unsafe void IntegratePoseAndVelocity( ref TIntegratorCallbacks integratorCallbacks, ref Vector bodyIndices, int count, in BodyInertiaWide localInertia, float dt, ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocityWide velocity, @@ -493,6 +493,7 @@ private static unsafe void IntegrateUniformly( IntegratePoseAndVelocity(ref integratorCallbacks, ref bodyIndices, count, localInertia, dt, ref position, ref orientation, ref velocity, workerIndex, out inertia); bodies.ScatterPoseAndInertia(ref position, ref orientation, ref inertia, ref bodyIndices, count, ref integrationMask); } + //[MethodImpl(MethodImplOptions.AggressiveInlining)] public static unsafe void GatherAndIntegrate( Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, ref Buffer integrationFlags, int bodyIndexInConstraint, float dt, int workerIndex, int bundleIndex, ref Vector bodyIndices, int count, out Vector3Wide position, out QuaternionWide orientation, out BodyVelocityWide velocity, out BodyInertiaWide inertia) From 83a16494a24c586237f43b37f52f8677d15dfae1 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 16 Jul 2021 14:58:58 -0500 Subject: [PATCH 113/947] Rename for consistency with other types. --- BepuPhysics/BodyProperties.cs | 8 ++++---- BepuPhysics/BodySet.cs | 2 +- BepuPhysics/Collidables/BoundingBoxBatcher.cs | 2 +- BepuPhysics/Collidables/Box.cs | 2 +- BepuPhysics/Collidables/Capsule.cs | 2 +- BepuPhysics/Collidables/Compound.cs | 4 ++-- BepuPhysics/Collidables/ConvexHull.cs | 4 ++-- BepuPhysics/Collidables/Cylinder.cs | 2 +- BepuPhysics/Collidables/IShape.cs | 2 +- BepuPhysics/Collidables/Sphere.cs | 2 +- BepuPhysics/Collidables/Triangle.cs | 2 +- .../CollisionTasks/CompoundPairOverlapFinder.cs | 4 ++-- .../SweepTasks/ConvexSweepTaskCommon.cs | 4 ++-- BepuPhysics/CollisionDetection/WideRayTester.cs | 2 +- BepuPhysics/Statics.cs | 4 ++-- Demos/SpecializedTests/RayTesting.cs | 4 ++-- Demos/SpecializedTests/TriangleRayTestDemo.cs | 2 +- 17 files changed, 26 insertions(+), 26 deletions(-) diff --git a/BepuPhysics/BodyProperties.cs b/BepuPhysics/BodyProperties.cs index 3f993ae17..8ffb623a7 100644 --- a/BepuPhysics/BodyProperties.cs +++ b/BepuPhysics/BodyProperties.cs @@ -184,27 +184,27 @@ public struct BodyInertias public BodyInertia World; } - public struct RigidPoses + public struct RigidPoseWide { public Vector3Wide Position; public QuaternionWide Orientation; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Broadcast(in RigidPose pose, out RigidPoses poses) + public static void Broadcast(in RigidPose pose, out RigidPoseWide poses) { Vector3Wide.Broadcast(pose.Position, out poses.Position); QuaternionWide.Broadcast(pose.Orientation, out poses.Orientation); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void WriteFirst(in RigidPose pose, ref RigidPoses poses) + public static void WriteFirst(in RigidPose pose, ref RigidPoseWide poses) { Vector3Wide.WriteFirst(pose.Position, ref poses.Position); QuaternionWide.WriteFirst(pose.Orientation, ref poses.Orientation); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ReadFirst(in RigidPoses poses, out RigidPose pose) + public static void ReadFirst(in RigidPoseWide poses, out RigidPose pose) { Vector3Wide.ReadFirst(poses.Position, out pose.Position); QuaternionWide.ReadFirst(poses.Orientation, out pose.Orientation); diff --git a/BepuPhysics/BodySet.cs b/BepuPhysics/BodySet.cs index 38c933695..63ce7860b 100644 --- a/BepuPhysics/BodySet.cs +++ b/BepuPhysics/BodySet.cs @@ -236,7 +236,7 @@ internal unsafe void InternalResize(int targetBodyCapacity, BufferPool pool) //Note that we base the bundle capacities on post-resize capacity of the IndexToHandle array. This simplifies the conditions on allocation, but increases memory use. //You may want to change this in the future if memory use is concerning. targetBodyCapacity = BufferPool.GetCapacityForCount(targetBodyCapacity); - Debug.Assert(MotionStates.Length != BufferPool.GetCapacityForCount(targetBodyCapacity), "Should not try to use internal resize of the result won't change the size."); + Debug.Assert(MotionStates.Length != BufferPool.GetCapacityForCount(targetBodyCapacity), "Should not try to use internal resize of the result won't change the size."); pool.ResizeToAtLeast(ref MotionStates, targetBodyCapacity, Count); pool.ResizeToAtLeast(ref Inertias, targetBodyCapacity, Count); pool.ResizeToAtLeast(ref IndexToHandle, targetBodyCapacity, Count); diff --git a/BepuPhysics/Collidables/BoundingBoxBatcher.cs b/BepuPhysics/Collidables/BoundingBoxBatcher.cs index a6332ed81..d828331df 100644 --- a/BepuPhysics/Collidables/BoundingBoxBatcher.cs +++ b/BepuPhysics/Collidables/BoundingBoxBatcher.cs @@ -79,7 +79,7 @@ public struct BoundingBoxInstanceWide where TShape : unmanag { public TShapeWide Shape; public Vector MaximumExpansion; - public RigidPoses Pose; + public RigidPoseWide Pose; public BodyVelocityWide Velocities; } diff --git a/BepuPhysics/Collidables/Box.cs b/BepuPhysics/Collidables/Box.cs index ffbbca4f2..6fa573096 100644 --- a/BepuPhysics/Collidables/Box.cs +++ b/BepuPhysics/Collidables/Box.cs @@ -228,7 +228,7 @@ public int MinimumWideRayCount } } - public void RayTest(ref RigidPoses pose, ref RayWide ray, out Vector intersected, out Vector t, out Vector3Wide normal) + public void RayTest(ref RigidPoseWide pose, ref RayWide ray, out Vector intersected, out Vector t, out Vector3Wide normal) { Vector3Wide.Subtract(ray.Origin, pose.Position, out var offset); Matrix3x3Wide.CreateFromQuaternion(pose.Orientation, out var orientation); diff --git a/BepuPhysics/Collidables/Capsule.cs b/BepuPhysics/Collidables/Capsule.cs index 1eef29cde..62d17da46 100644 --- a/BepuPhysics/Collidables/Capsule.cs +++ b/BepuPhysics/Collidables/Capsule.cs @@ -243,7 +243,7 @@ public int MinimumWideRayCount } } - public void RayTest(ref RigidPoses pose, ref RayWide ray, out Vector intersected, out Vector t, out Vector3Wide normal) + public void RayTest(ref RigidPoseWide pose, ref RayWide ray, out Vector intersected, out Vector t, out Vector3Wide normal) { //It's convenient to work in local space, so pull the ray into the capsule's local space. Matrix3x3Wide.CreateFromQuaternion(pose.Orientation, out var orientation); diff --git a/BepuPhysics/Collidables/Compound.cs b/BepuPhysics/Collidables/Compound.cs index 3624a4b7e..8073e9473 100644 --- a/BepuPhysics/Collidables/Compound.cs +++ b/BepuPhysics/Collidables/Compound.cs @@ -108,13 +108,13 @@ public static void GetRotatedChildPose(in RigidPose localPose, in Quaternion ori QuaternionEx.Transform(localPose.Position, orientation, out rotatedChildPose.Position); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void GetRotatedChildPose(in RigidPoses localPose, in QuaternionWide orientation, out Vector3Wide childPosition, out QuaternionWide childOrientation) + public static void GetRotatedChildPose(in RigidPoseWide localPose, in QuaternionWide orientation, out Vector3Wide childPosition, out QuaternionWide childOrientation) { QuaternionWide.ConcatenateWithoutOverlap(localPose.Orientation, orientation, out childOrientation); QuaternionWide.TransformWithoutOverlap(localPose.Position, orientation, out childPosition); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void GetRotatedChildPose(in RigidPoses localPose, in QuaternionWide orientation, out RigidPoses rotatedChildPose) + public static void GetRotatedChildPose(in RigidPoseWide localPose, in QuaternionWide orientation, out RigidPoseWide rotatedChildPose) { GetRotatedChildPose(localPose, orientation, out rotatedChildPose.Position, out rotatedChildPose.Orientation); } diff --git a/BepuPhysics/Collidables/ConvexHull.cs b/BepuPhysics/Collidables/ConvexHull.cs index 74178b415..b16ef217b 100644 --- a/BepuPhysics/Collidables/ConvexHull.cs +++ b/BepuPhysics/Collidables/ConvexHull.cs @@ -358,7 +358,7 @@ public void GetBounds(ref QuaternionWide orientations, int countInBundle, out Ve maximumAngularExpansion = maximumRadius; } - public void RayTest(ref RigidPoses poses, ref RayWide rayWide, out Vector intersected, out Vector t, out Vector3Wide normal) + public void RayTest(ref RigidPoseWide poses, ref RayWide rayWide, out Vector intersected, out Vector t, out Vector3Wide normal) { Unsafe.SkipInit(out intersected); Unsafe.SkipInit(out t); @@ -366,7 +366,7 @@ public void RayTest(ref RigidPoses poses, ref RayWide rayWide, out Vector i Debug.Assert(Hulls.Length > 0 && Hulls.Length <= Vector.Count); for (int i = 0; i < Hulls.Length; ++i) { - RigidPoses.ReadFirst(GatherScatter.GetOffsetInstance(ref poses, i), out var pose); + RigidPoseWide.ReadFirst(GatherScatter.GetOffsetInstance(ref poses, i), out var pose); ref var offsetRay = ref GatherScatter.GetOffsetInstance(ref rayWide, i); Vector3Wide.ReadFirst(offsetRay.Origin, out var origin); Vector3Wide.ReadFirst(offsetRay.Direction, out var direction); diff --git a/BepuPhysics/Collidables/Cylinder.cs b/BepuPhysics/Collidables/Cylinder.cs index dbda4024f..91d2c256e 100644 --- a/BepuPhysics/Collidables/Cylinder.cs +++ b/BepuPhysics/Collidables/Cylinder.cs @@ -243,7 +243,7 @@ public int MinimumWideRayCount } } - public void RayTest(ref RigidPoses pose, ref RayWide ray, out Vector intersected, out Vector t, out Vector3Wide normal) + public void RayTest(ref RigidPoseWide pose, ref RayWide ray, out Vector intersected, out Vector t, out Vector3Wide normal) { //It's convenient to work in local space, so pull the ray into the capsule's local space. Matrix3x3Wide.CreateFromQuaternion(pose.Orientation, out var orientation); diff --git a/BepuPhysics/Collidables/IShape.cs b/BepuPhysics/Collidables/IShape.cs index 2f26b26a3..44a02a1bf 100644 --- a/BepuPhysics/Collidables/IShape.cs +++ b/BepuPhysics/Collidables/IShape.cs @@ -115,7 +115,7 @@ public interface IShapeWide where TShape : IShape /// Gets the lower bound on the number of rays to execute in a wide fashion. Ray bundles with fewer rays will fall back to the single ray code path. /// int MinimumWideRayCount { get; } - void RayTest(ref RigidPoses poses, ref RayWide rayWide, out Vector intersected, out Vector t, out Vector3Wide normal); + void RayTest(ref RigidPoseWide poses, ref RayWide rayWide, out Vector intersected, out Vector t, out Vector3Wide normal); } } diff --git a/BepuPhysics/Collidables/Sphere.cs b/BepuPhysics/Collidables/Sphere.cs index 0d893963d..a6349fcb9 100644 --- a/BepuPhysics/Collidables/Sphere.cs +++ b/BepuPhysics/Collidables/Sphere.cs @@ -170,7 +170,7 @@ public int MinimumWideRayCount } } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void RayTest(ref RigidPoses pose, ref RayWide rayWide, out Vector intersected, out Vector t, out Vector3Wide normal) + public void RayTest(ref RigidPoseWide pose, ref RayWide rayWide, out Vector intersected, out Vector t, out Vector3Wide normal) { //Normalize the direction. Sqrts aren't *that* bad, and it both simplifies things and helps avoid numerical problems. Vector3Wide.Length(rayWide.Direction, out var inverseDLength); diff --git a/BepuPhysics/Collidables/Triangle.cs b/BepuPhysics/Collidables/Triangle.cs index 2695353e3..eb02f10f4 100644 --- a/BepuPhysics/Collidables/Triangle.cs +++ b/BepuPhysics/Collidables/Triangle.cs @@ -253,7 +253,7 @@ public static void RayTest(ref Vector3Wide a, ref Vector3Wide b, ref Vector3Wide Vector.GreaterThanOrEqual(w, Vector.Zero)), Vector.LessThanOrEqual(v + w, dn))); } - public void RayTest(ref RigidPoses pose, ref RayWide ray, out Vector intersected, out Vector t, out Vector3Wide normal) + public void RayTest(ref RigidPoseWide pose, ref RayWide ray, out Vector intersected, out Vector t, out Vector3Wide normal) { Vector3Wide.Subtract(ray.Origin, pose.Position, out var offset); Matrix3x3Wide.CreateFromQuaternion(pose.Orientation, out var orientation); diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlapFinder.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlapFinder.cs index 55ffe5dcb..ab0cdd641 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlapFinder.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlapFinder.cs @@ -56,7 +56,7 @@ public unsafe void FindLocalOverlaps(ref Buffer pairs, int pai Unsafe.SkipInit(out Vector maximumAllowedExpansion); Unsafe.SkipInit(out Vector maximumRadius); Unsafe.SkipInit(out Vector maximumAngularExpansion); - Unsafe.SkipInit(out RigidPoses localPosesA); + Unsafe.SkipInit(out RigidPoseWide localPosesA); Unsafe.SkipInit(out Vector3Wide mins); Unsafe.SkipInit(out Vector3Wide maxes); for (int i = 0; i < totalCompoundChildCount; i += Vector.Count) @@ -79,7 +79,7 @@ public unsafe void FindLocalOverlaps(ref Buffer pairs, int pai Vector3Wide.WriteFirst(subpair.Pair->AngularVelocityB, ref GatherScatter.GetOffsetInstance(ref angularVelocityB, j)); Unsafe.Add(ref Unsafe.As, float>(ref maximumAllowedExpansion), j) = subpair.Pair->MaximumExpansion; - RigidPoses.WriteFirst(subpair.Child->LocalPose, ref GatherScatter.GetOffsetInstance(ref localPosesA, j)); + RigidPoseWide.WriteFirst(subpair.Child->LocalPose, ref GatherScatter.GetOffsetInstance(ref localPosesA, j)); } QuaternionWide.Conjugate(orientationB, out var toLocalB); diff --git a/BepuPhysics/CollisionDetection/SweepTasks/ConvexSweepTaskCommon.cs b/BepuPhysics/CollisionDetection/SweepTasks/ConvexSweepTaskCommon.cs index f1cfc95f3..c13c40fde 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/ConvexSweepTaskCommon.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/ConvexSweepTaskCommon.cs @@ -204,11 +204,11 @@ public void ConstructSamples(float t0, float t1, ref Vector3Wide linearB, ref Ve //Note that the initial orientations are properties of the owning body, not of the child. //The orientation of the child itself is the product of localOrientation * bodyOrientation. var halfSamples = samples * 0.5f; - RigidPoses.Broadcast(LocalPoseA, out var localPosesA); + RigidPoseWide.Broadcast(LocalPoseA, out var localPosesA); PoseIntegration.Integrate(initialOrientationA, angularA, halfSamples, out var integratedOrientationA); Compound.GetRotatedChildPose(localPosesA, integratedOrientationA, out var childPositionA, out sampleOrientationA); - RigidPoses.Broadcast(LocalPoseB, out var localPosesB); + RigidPoseWide.Broadcast(LocalPoseB, out var localPosesB); PoseIntegration.Integrate(initialOrientationB, angularB, halfSamples, out var integratedOrientationB); Compound.GetRotatedChildPose(localPosesB, integratedOrientationB, out var childPositionB, out sampleOrientationB); diff --git a/BepuPhysics/CollisionDetection/WideRayTester.cs b/BepuPhysics/CollisionDetection/WideRayTester.cs index 7d5718994..36aca4cbb 100644 --- a/BepuPhysics/CollisionDetection/WideRayTester.cs +++ b/BepuPhysics/CollisionDetection/WideRayTester.cs @@ -27,7 +27,7 @@ public unsafe static void Test(r wide.Initialize(new RawBuffer(memory, wide.InternalAllocationSize)); } wide.Broadcast(shape); - RigidPoses poses; + RigidPoseWide poses; Vector3Wide.Broadcast(pose.Position, out poses.Position); QuaternionWide.Broadcast(pose.Orientation, out poses.Orientation); for (int i = 0; i < raySource.RayCount; i += Vector.Count) diff --git a/BepuPhysics/Statics.cs b/BepuPhysics/Statics.cs index f2c40aa41..6e04cb8a8 100644 --- a/BepuPhysics/Statics.cs +++ b/BepuPhysics/Statics.cs @@ -93,7 +93,7 @@ unsafe void InternalResize(int targetCapacity) Debug.Assert(targetCapacity > 0, "Resize is not meant to be used as Dispose. If you want to return everything to the pool, use Dispose instead."); //Note that we base the bundle capacities on the static capacity. This simplifies the conditions on allocation targetCapacity = BufferPool.GetCapacityForCount(targetCapacity); - Debug.Assert(Poses.Length != BufferPool.GetCapacityForCount(targetCapacity), "Should not try to use internal resize of the result won't change the size."); + Debug.Assert(Poses.Length != BufferPool.GetCapacityForCount(targetCapacity), "Should not try to use internal resize of the result won't change the size."); pool.ResizeToAtLeast(ref Poses, targetCapacity, Count); pool.ResizeToAtLeast(ref IndexToHandle, targetCapacity, Count); pool.ResizeToAtLeast(ref HandleToIndex, targetCapacity, Count); @@ -505,7 +505,7 @@ private void GatherPose(ref float targetPositionBase, ref float targetOrientatio //This looks a little different because it's used by AABB calculation, not constraint pairs. [MethodImpl(MethodImplOptions.AggressiveInlining)] - internal void GatherDataForBounds(ref int start, int count, out RigidPoses poses, out Vector shapeIndices, out Vector maximumExpansion) + internal void GatherDataForBounds(ref int start, int count, out RigidPoseWide poses, out Vector shapeIndices, out Vector maximumExpansion) { Debug.Assert(count <= Vector.Count); Unsafe.SkipInit(out poses); diff --git a/Demos/SpecializedTests/RayTesting.cs b/Demos/SpecializedTests/RayTesting.cs index 6ac818fc8..687be0972 100644 --- a/Demos/SpecializedTests/RayTesting.cs +++ b/Demos/SpecializedTests/RayTesting.cs @@ -309,7 +309,7 @@ static void GetPointOnPlane(Random random, float centralExclusion, float span, r point = anchor + basisX * localPoint.X + basisZ * localPoint.Y; } - static void CheckWide(ref RigidPoses poses, ref TShapeWide shapeWide, ref Vector3 origin, ref Vector3 direction, bool intersected, float t, ref Vector3 normal) + static void CheckWide(ref RigidPoseWide poses, ref TShapeWide shapeWide, ref Vector3 origin, ref Vector3 direction, bool intersected, float t, ref Vector3 normal) where TShape : IConvexShape where TShapeWide : IShapeWide { RayWide rayWide; @@ -375,7 +375,7 @@ static void Test() where TShape : IConvexShape wher pose.Position = new Vector3(positionMin) + positionBoundsSpan * new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()); GetUnitQuaternion(random, out pose.Orientation); Matrix3x3.CreateFromQuaternion(pose.Orientation, out var orientation); - RigidPoses poses; + RigidPoseWide poses; Vector3Wide.Broadcast(pose.Position, out poses.Position); QuaternionWide.Broadcast(pose.Orientation, out poses.Orientation); for (int rayIndex = 0; rayIndex < outsideToInsideRays; ++rayIndex) diff --git a/Demos/SpecializedTests/TriangleRayTestDemo.cs b/Demos/SpecializedTests/TriangleRayTestDemo.cs index cb5cd56d5..c7c19fa37 100644 --- a/Demos/SpecializedTests/TriangleRayTestDemo.cs +++ b/Demos/SpecializedTests/TriangleRayTestDemo.cs @@ -69,7 +69,7 @@ void TestRay(in Triangle triangle, in RigidPose pose, in Vector3 rayOrigin, in V TriangleWide wide = default; wide.Broadcast(triangle); - RigidPoses.Broadcast(pose, out var poses); + RigidPoseWide.Broadcast(pose, out var poses); RayWide rayWide; Vector3Wide.Broadcast(rayOrigin, out rayWide.Origin); Vector3Wide.Broadcast(rayDirection, out rayWide.Direction); From 65a34975c24fc5feb6a4e5c3de50c9f95020dc63 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 16 Jul 2021 16:58:02 -0500 Subject: [PATCH 114/947] Combined inertia and motion state in memory. --- BepuPhysics/Bodies.cs | 28 +-- BepuPhysics/Bodies_GatherScatter.cs | 177 +++++++++--------- BepuPhysics/BodyProperties.cs | 17 ++ BepuPhysics/BodyReference.cs | 26 ++- BepuPhysics/BodySet.cs | 36 ++-- BepuPhysics/CollisionDetection/NarrowPhase.cs | 14 +- .../Constraints/TwoBodyTypeProcessor.cs | 35 ++-- BepuPhysics/FallbackBatch.cs | 2 +- BepuPhysics/IslandAwakener.cs | 11 +- BepuPhysics/IslandSleeper.cs | 3 +- BepuPhysics/PoseIntegrator.cs | 101 +++++----- BepuPhysics/Simulation_Queries.cs | 2 +- BepuUtilities/Memory/BufferPool.cs | 9 +- .../AngularSwivelHingeLineExtractor.cs | 4 +- .../Constraints/BallSocketLineExtractor.cs | 4 +- .../BallSocketMotorLineExtractor.cs | 4 +- .../BallSocketServoLineExtractor.cs | 4 +- .../CenterDistanceLineExtractor.cs | 4 +- .../Constraints/ContactLineExtractors.cs | 28 +-- .../Constraints/ContactLineExtractors.tt | 2 +- .../Constraints/DistanceLimitLineExtractor.cs | 4 +- .../Constraints/DistanceServoLineExtractor.cs | 4 +- .../Constraints/HingeLineExtractor.cs | 4 +- .../LinearAxisServoLineExtractor.cs | 4 +- .../OneBodyLinearServoLineExtractor.cs | 2 +- .../Constraints/PointOnLineLineExtractor.cs | 4 +- .../Constraints/SwivelHingeLineExtractor.cs | 4 +- DemoRenderer/Constraints/WeldLineExtractor.cs | 4 +- DemoRenderer/ShapeDrawing/ShapesExtractor.cs | 6 +- DemoTests/TestUtilities.cs | 2 +- Demos/Demo.cs | 2 +- .../Demos/Characters/CharacterControllers.cs | 14 +- Demos/Demos/FountainStressTestDemo.cs | 6 +- Demos/SpecializedTests/DeterminismTest.cs | 7 +- Demos/SpecializedTests/TestHelpers.cs | 5 +- 35 files changed, 293 insertions(+), 290 deletions(-) diff --git a/BepuPhysics/Bodies.cs b/BepuPhysics/Bodies.cs index d4ae8486c..f7cae99a3 100644 --- a/BepuPhysics/Bodies.cs +++ b/BepuPhysics/Bodies.cs @@ -107,7 +107,7 @@ public void UpdateBounds(BodyHandle bodyHandle) ref var collidable = ref set.Collidables[location.Index]; if (collidable.Shape.Exists) { - shapes.UpdateBounds(set.MotionStates[location.Index].Pose, ref collidable.Shape, out var bodyBounds); + shapes.UpdateBounds(set.SolverStates[location.Index].Motion.Pose, ref collidable.Shape, out var bodyBounds); if (location.SetIndex == 0) { broadPhase.UpdateActiveBounds(collidable.BroadPhaseIndex, bodyBounds.Min, bodyBounds.Max); @@ -300,7 +300,7 @@ public void LoopBody(int bodyIndex) { //The solver's connected bodies enumeration directly provides the constraint-stored reference, which is an index in the active set for active constraints and a handle for inactive constraints. //We forced the dynamic active at the beginning of BecomeKinematic, so we don't have to worry about the inactive side of things. - if (!IsKinematic(Bodies.ActiveSet.Inertias[bodyIndex].Local)) + if (!IsKinematic(Bodies.ActiveSet.SolverStates[bodyIndex].Inertia.Local)) ++DynamicCount; } } @@ -312,7 +312,7 @@ void UpdateForKinematicStateChange(BodyHandle handle, ref BodyMemoryLocation loc ref var collidable = ref set.Collidables[location.Index]; if (collidable.Shape.Exists) { - var mobility = IsKinematic(set.Inertias[location.Index].Local) ? CollidableMobility.Kinematic : CollidableMobility.Dynamic; + var mobility = IsKinematic(set.SolverStates[location.Index].Inertia.Local) ? CollidableMobility.Kinematic : CollidableMobility.Dynamic; if (location.SetIndex == 0) { broadPhase.activeLeaves[collidable.BroadPhaseIndex] = new CollidableReference(mobility, handle); @@ -362,8 +362,9 @@ public void SetLocalInertia(BodyHandle handle, in BodyInertia localInertia) } //Note that the HandleToLocation slot reference is still valid; it may have been updated, but handle slots don't move. ref var set = ref Sets[location.SetIndex]; - var newlyKinematic = IsKinematic(localInertia) && !IsKinematic(set.Inertias[location.Index].Local); - set.Inertias[location.Index].Local = localInertia; + ref var localInertiaReference = ref set.SolverStates[location.Index].Inertia.Local; + var newlyKinematic = IsKinematic(localInertia) && !IsKinematic(localInertiaReference); + localInertiaReference = localInertia; UpdateForKinematicStateChange(handle, ref location, ref set, newlyKinematic); } @@ -376,7 +377,8 @@ void UpdateForShapeChange(BodyHandle handle, int activeBodyIndex, TypedIndex old if (newShape.Exists) { //Add a collidable to the simulation for the new shape. - AddCollidableToBroadPhase(handle, set.MotionStates[activeBodyIndex].Pose, set.Inertias[activeBodyIndex].Local, ref set.Collidables[activeBodyIndex]); + ref var state = ref set.SolverStates[activeBodyIndex]; + AddCollidableToBroadPhase(handle, state.Motion.Pose, state.Inertia.Local, ref set.Collidables[activeBodyIndex]); } else { @@ -429,7 +431,7 @@ public void ApplyDescription(BodyHandle handle, in BodyDescription description) ref var set = ref Sets[location.SetIndex]; ref var collidable = ref set.Collidables[location.Index]; var oldShape = collidable.Shape; - var newlyKinematic = IsKinematic(description.LocalInertia) && !IsKinematic(set.Inertias[location.Index].Local); + var newlyKinematic = IsKinematic(description.LocalInertia) && !IsKinematic(set.SolverStates[location.Index].Inertia.Local); set.ApplyDescriptionByIndex(location.Index, description); UpdateForShapeChange(handle, location.Index, oldShape, description.Collidable.Shape); UpdateForKinematicStateChange(handle, ref location, ref set, newlyKinematic); @@ -498,17 +500,17 @@ internal void ValidateMotionStates() { for (int j = 0; j < set.Count; ++j) { - ref var state = ref set.MotionStates[j]; + ref var state = ref set.SolverStates[j]; try { - state.Pose.Position.Validate(); - state.Pose.Orientation.ValidateOrientation(); - state.Velocity.Linear.Validate(); - state.Velocity.Angular.Validate(); + state.Motion.Pose.Position.Validate(); + state.Motion.Pose.Orientation.ValidateOrientation(); + state.Motion.Velocity.Linear.Validate(); + state.Motion.Velocity.Angular.Validate(); } catch { - Console.WriteLine($"Validation failed on body {i} of set {j}. Position: {state.Pose.Position}, orientation: {state.Pose.Orientation}, linear: {state.Velocity.Linear}, angular: {state.Velocity.Angular}"); + Console.WriteLine($"Validation failed on body {i} of set {j}. Position: {state.Motion.Pose.Position}, orientation: {state.Motion.Pose.Orientation}, linear: {state.Motion.Velocity.Linear}, angular: {state.Motion.Velocity.Angular}"); throw; } diff --git a/BepuPhysics/Bodies_GatherScatter.cs b/BepuPhysics/Bodies_GatherScatter.cs index 3970fab0e..b6380b8dd 100644 --- a/BepuPhysics/Bodies_GatherScatter.cs +++ b/BepuPhysics/Bodies_GatherScatter.cs @@ -17,9 +17,9 @@ public partial class Bodies { [MethodImpl(MethodImplOptions.AggressiveInlining)] - private static void WriteGatherInertia(ref int bundleBaseBodyIndexInSet, int bodyIndexInBundle, ref Buffer inertias, ref BodyInertiaWide gatheredInertias) + private static void WriteGatherInertia(ref int bundleBaseBodyIndexInSet, int bodyIndexInBundle, ref Buffer states, ref BodyInertiaWide gatheredInertias) { - ref var source = ref inertias[Unsafe.Add(ref bundleBaseBodyIndexInSet, bodyIndexInBundle)].World; + ref var source = ref states[Unsafe.Add(ref bundleBaseBodyIndexInSet, bodyIndexInBundle)].Inertia.World; ref var targetSlot = ref GetOffsetInstance(ref gatheredInertias, bodyIndexInBundle); GetFirst(ref targetSlot.InverseInertiaTensor.XX) = source.InverseInertiaTensor.XX; GetFirst(ref targetSlot.InverseInertiaTensor.YX) = source.InverseInertiaTensor.YX; @@ -31,10 +31,10 @@ private static void WriteGatherInertia(ref int bundleBaseBodyIndexInSet, int bod } [MethodImpl(MethodImplOptions.AggressiveInlining)] - private static void WriteGatherState(ref int bundleBaseBodyIndexInSet, int bodyIndexInBundle, ref Buffer states, + private static void WriteGatherMotionState(ref int bundleBaseBodyIndexInSet, int bodyIndexInBundle, ref Buffer states, ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocityWide velocity) { - ref var state = ref states[Unsafe.Add(ref bundleBaseBodyIndexInSet, bodyIndexInBundle)]; + ref var state = ref states[Unsafe.Add(ref bundleBaseBodyIndexInSet, bodyIndexInBundle)].Motion; Vector3Wide.WriteFirst(state.Pose.Position, ref GetOffsetInstance(ref position, bodyIndexInBundle)); QuaternionWide.WriteFirst(state.Pose.Orientation, ref GetOffsetInstance(ref orientation, bodyIndexInBundle)); Vector3Wide.WriteFirst(state.Velocity.Linear, ref GetOffsetInstance(ref velocity.Linear, bodyIndexInBundle)); @@ -68,13 +68,13 @@ public void GatherState(ref Vector references, int count, for (int i = 0; i < count; ++i) { - WriteGatherState(ref baseIndex, i, ref ActiveSet.MotionStates, ref position, ref orientation, ref velocity); - WriteGatherInertia(ref baseIndex, i, ref ActiveSet.Inertias, ref inertia); + WriteGatherMotionState(ref baseIndex, i, ref ActiveSet.SolverStates, ref position, ref orientation, ref velocity); + WriteGatherInertia(ref baseIndex, i, ref ActiveSet.SolverStates, ref inertia); } } //[MethodImpl(MethodImplOptions.AggressiveInlining)] - unsafe static void FallbackGatherMotionState(int count, MotionState* motionStates, ref Vector baseIndex, ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocityWide velocity) + unsafe static void FallbackGatherMotionState(int count, SolverState* states, ref Vector baseIndex, ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocityWide velocity) { var indices = (int*)Unsafe.AsPointer(ref baseIndex); var pPositionX = (float*)Unsafe.AsPointer(ref position.X); @@ -94,7 +94,7 @@ unsafe static void FallbackGatherMotionState(int count, MotionState* motionState for (int i = 0; i < count; ++i) { var index = indices[i]; - var stateValues = (float*)(motionStates + index); + var stateValues = (float*)(states + index); pPositionX[i] = stateValues[MotionState.OffsetToPositionX]; pPositionY[i] = stateValues[MotionState.OffsetToPositionY]; pPositionZ[i] = stateValues[MotionState.OffsetToPositionZ]; @@ -110,7 +110,7 @@ unsafe static void FallbackGatherMotionState(int count, MotionState* motionState pAngularZ[i] = stateValues[MotionState.OffsetToAngularZ]; } } - unsafe static void FallbackGatherInertia(int count, BodyInertias* inertias, ref Vector baseIndex, ref BodyInertiaWide inertia, int offsetInFloats) + unsafe static void FallbackGatherInertia(int count, SolverState* states, ref Vector baseIndex, ref BodyInertiaWide inertia, int offsetInFloats) { var indices = (int*)Unsafe.AsPointer(ref baseIndex); var pMass = (float*)Unsafe.AsPointer(ref inertia.InverseMass); @@ -124,7 +124,7 @@ unsafe static void FallbackGatherInertia(int count, BodyInertias* inertias, ref for (int i = 0; i < count; ++i) { var index = indices[i]; - var inertiaValues = (float*)(inertias + index) + offsetInFloats; + var inertiaValues = (float*)(states + index) + offsetInFloats; pInertiaXX[i] = inertiaValues[0]; pInertiaYX[i] = inertiaValues[1]; pInertiaYY[i] = inertiaValues[2]; @@ -138,19 +138,19 @@ unsafe static void FallbackGatherInertia(int count, BodyInertias* inertias, ref //[MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe void GatherMotionState(ref Vector bodyIndices, int count, out Vector3Wide position, out QuaternionWide orientation, out BodyVelocityWide velocity) { - var motionStates = ActiveSet.MotionStates.Memory; + var solverStates = ActiveSet.SolverStates.Memory; if (Avx.IsSupported && Vector.Count == 8) { var indices = (int*)Unsafe.AsPointer(ref bodyIndices); - var s0 = (float*)(motionStates + indices[0]); - var s1 = (float*)(motionStates + indices[1]); - var s2 = (float*)(motionStates + indices[2]); - var s3 = (float*)(motionStates + indices[3]); - var s4 = (float*)(motionStates + indices[4]); - var s5 = (float*)(motionStates + indices[5]); - var s6 = (float*)(motionStates + indices[6]); - var s7 = (float*)(motionStates + indices[7]); + var s0 = (float*)(solverStates + indices[0]); + var s1 = (float*)(solverStates + indices[1]); + var s2 = (float*)(solverStates + indices[2]); + var s3 = (float*)(solverStates + indices[3]); + var s4 = (float*)(solverStates + indices[4]); + var s5 = (float*)(solverStates + indices[5]); + var s6 = (float*)(solverStates + indices[6]); + var s7 = (float*)(solverStates + indices[7]); //for (int i = 0; i < 8; ++i) //{ @@ -246,7 +246,7 @@ public unsafe void GatherMotionState(ref Vector bodyIndices, int count, out Unsafe.SkipInit(out position); Unsafe.SkipInit(out orientation); Unsafe.SkipInit(out velocity); - FallbackGatherMotionState(count, motionStates, ref bodyIndices, ref position, ref orientation, ref velocity); + FallbackGatherMotionState(count, solverStates, ref bodyIndices, ref position, ref orientation, ref velocity); } } @@ -254,7 +254,7 @@ public unsafe void GatherMotionState(ref Vector bodyIndices, int count, out //[MethodImpl(MethodImplOptions.AggressiveInlining)] unsafe void GatherInertia(ref Vector bodyIndices, int count, int offsetInFloats, out BodyInertiaWide inertia) { - var inertias = ActiveSet.Inertias.Memory; + var inertias = ActiveSet.SolverStates.Memory; if (Avx.IsSupported && Vector.Count == 8) { var indices = (int*)Unsafe.AsPointer(ref bodyIndices); @@ -308,19 +308,19 @@ unsafe void GatherInertia(ref Vector bodyIndices, int count, int offsetInFl else { Unsafe.SkipInit(out inertia); - FallbackGatherInertia(count, ActiveSet.Inertias.Memory, ref bodyIndices, ref inertia, offsetInFloats); + FallbackGatherInertia(count, ActiveSet.SolverStates.Memory, ref bodyIndices, ref inertia, offsetInFloats); } } [MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe void GatherWorldInertia(ref Vector bodyIndices, int count, out BodyInertiaWide inertia) { - GatherInertia(ref bodyIndices, count, 8, out inertia); + GatherInertia(ref bodyIndices, count, 24, out inertia); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe void GatherLocalInertia(ref Vector bodyIndices, int count, out BodyInertiaWide inertia) { - GatherInertia(ref bodyIndices, count, 0, out inertia); + GatherInertia(ref bodyIndices, count, 16, out inertia); } /// @@ -346,7 +346,7 @@ public unsafe void GatherState(ref TwoBodyReferences references, int count, ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); - ref var states = ref ActiveSet.MotionStates; + ref var states = ref ActiveSet.SolverStates; GatherMotionState(ref references.IndexA, count, out var positionA, out orientationA, out velocityA); GatherWorldInertia(ref references.IndexA, count, out inertiaA); @@ -417,16 +417,15 @@ public void GatherState(ref ThreeBodyReferences references, int count, ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); ref var baseIndexC = ref Unsafe.As, int>(ref references.IndexC); - ref var states = ref ActiveSet.MotionStates; - ref var inertias = ref ActiveSet.Inertias; + ref var states = ref ActiveSet.SolverStates; for (int i = 0; i < count; ++i) { - WriteGatherState(ref baseIndexA, i, ref states, ref positionA, ref orientationA, ref velocityA); - WriteGatherInertia(ref baseIndexA, i, ref inertias, ref inertiaA); - WriteGatherState(ref baseIndexB, i, ref states, ref positionB, ref orientationB, ref velocityB); - WriteGatherInertia(ref baseIndexB, i, ref inertias, ref inertiaB); - WriteGatherState(ref baseIndexC, i, ref states, ref positionC, ref orientationC, ref velocityC); - WriteGatherInertia(ref baseIndexC, i, ref inertias, ref inertiaC); + WriteGatherMotionState(ref baseIndexA, i, ref states, ref positionA, ref orientationA, ref velocityA); + WriteGatherInertia(ref baseIndexA, i, ref states, ref inertiaA); + WriteGatherMotionState(ref baseIndexB, i, ref states, ref positionB, ref orientationB, ref velocityB); + WriteGatherInertia(ref baseIndexB, i, ref states, ref inertiaB); + WriteGatherMotionState(ref baseIndexC, i, ref states, ref positionC, ref orientationC, ref velocityC); + WriteGatherInertia(ref baseIndexC, i, ref states, ref inertiaC); } //TODO: In future versions, we will likely store the body position in different forms to allow for extremely large worlds. //That will be an opt-in feature. The default implementation will use the FP32 representation, but the user could choose to swap it out for a fp64 or fixed64 representation. @@ -493,18 +492,17 @@ public void GatherState(ref FourBodyReferences references, int count, ref var baseIndexC = ref Unsafe.As, int>(ref references.IndexC); ref var baseIndexD = ref Unsafe.As, int>(ref references.IndexD); - ref var states = ref ActiveSet.MotionStates; - ref var inertias = ref ActiveSet.Inertias; + ref var states = ref ActiveSet.SolverStates; for (int i = 0; i < count; ++i) { - WriteGatherState(ref baseIndexA, i, ref states, ref positionA, ref orientationA, ref velocityA); - WriteGatherInertia(ref baseIndexA, i, ref inertias, ref inertiaA); - WriteGatherState(ref baseIndexB, i, ref states, ref positionB, ref orientationB, ref velocityB); - WriteGatherInertia(ref baseIndexB, i, ref inertias, ref inertiaB); - WriteGatherState(ref baseIndexC, i, ref states, ref positionC, ref orientationC, ref velocityC); - WriteGatherInertia(ref baseIndexC, i, ref inertias, ref inertiaC); - WriteGatherState(ref baseIndexD, i, ref states, ref positionD, ref orientationD, ref velocityD); - WriteGatherInertia(ref baseIndexD, i, ref inertias, ref inertiaD); + WriteGatherMotionState(ref baseIndexA, i, ref states, ref positionA, ref orientationA, ref velocityA); + WriteGatherInertia(ref baseIndexA, i, ref states, ref inertiaA); + WriteGatherMotionState(ref baseIndexB, i, ref states, ref positionB, ref orientationB, ref velocityB); + WriteGatherInertia(ref baseIndexB, i, ref states, ref inertiaB); + WriteGatherMotionState(ref baseIndexC, i, ref states, ref positionC, ref orientationC, ref velocityC); + WriteGatherInertia(ref baseIndexC, i, ref states, ref inertiaC); + WriteGatherMotionState(ref baseIndexD, i, ref states, ref positionD, ref orientationD, ref velocityD); + WriteGatherInertia(ref baseIndexD, i, ref states, ref inertiaD); } //TODO: In future versions, we will likely store the body position in different forms to allow for extremely large worlds. //That will be an opt-in feature. The default implementation will use the FP32 representation, but the user could choose to swap it out for a fp64 or fixed64 representation. @@ -527,6 +525,7 @@ public unsafe void ScatterPoseAndInertia( { if (Avx.IsSupported && Vector.Count == 8) { + var states = ActiveSet.SolverStates.Memory; { var m0 = orientation.X.AsVector256(); var m1 = orientation.Y.AsVector256(); @@ -556,29 +555,28 @@ public unsafe void ScatterPoseAndInertia( var maskPointer = (int*)Unsafe.AsPointer(ref mask); var indices = (int*)Unsafe.AsPointer(ref references); - var motionStates = ActiveSet.MotionStates.Memory; if (typeof(TBatchIntegrationMode) == typeof(BatchShouldAlwaysIntegrate)) { //If we are in an 'always integrate' batch, then the mask is not guaranteed to mask out out-of-range lanes, since we don't otherwise need the mask. - Avx.StoreAligned((float*)(motionStates + indices[0]), Avx.Permute2x128(o0, o1, 0 | (2 << 4))); - if (count > 1) Avx.StoreAligned((float*)(motionStates + indices[1]), Avx.Permute2x128(o4, o5, 0 | (2 << 4))); - if (count > 2) Avx.StoreAligned((float*)(motionStates + indices[2]), Avx.Permute2x128(o2, o3, 0 | (2 << 4))); - if (count > 3) Avx.StoreAligned((float*)(motionStates + indices[3]), Avx.Permute2x128(o6, o7, 0 | (2 << 4))); - if (count > 4) Avx.StoreAligned((float*)(motionStates + indices[4]), Avx.Permute2x128(o0, o1, 1 | (3 << 4))); - if (count > 5) Avx.StoreAligned((float*)(motionStates + indices[5]), Avx.Permute2x128(o4, o5, 1 | (3 << 4))); - if (count > 6) Avx.StoreAligned((float*)(motionStates + indices[6]), Avx.Permute2x128(o2, o3, 1 | (3 << 4))); - if (count > 7) Avx.StoreAligned((float*)(motionStates + indices[7]), Avx.Permute2x128(o6, o7, 1 | (3 << 4))); + Avx.StoreAligned((float*)(states + indices[0]), Avx.Permute2x128(o0, o1, 0 | (2 << 4))); + if (count > 1) Avx.StoreAligned((float*)(states + indices[1]), Avx.Permute2x128(o4, o5, 0 | (2 << 4))); + if (count > 2) Avx.StoreAligned((float*)(states + indices[2]), Avx.Permute2x128(o2, o3, 0 | (2 << 4))); + if (count > 3) Avx.StoreAligned((float*)(states + indices[3]), Avx.Permute2x128(o6, o7, 0 | (2 << 4))); + if (count > 4) Avx.StoreAligned((float*)(states + indices[4]), Avx.Permute2x128(o0, o1, 1 | (3 << 4))); + if (count > 5) Avx.StoreAligned((float*)(states + indices[5]), Avx.Permute2x128(o4, o5, 1 | (3 << 4))); + if (count > 6) Avx.StoreAligned((float*)(states + indices[6]), Avx.Permute2x128(o2, o3, 1 | (3 << 4))); + if (count > 7) Avx.StoreAligned((float*)(states + indices[7]), Avx.Permute2x128(o6, o7, 1 | (3 << 4))); } else { - if (maskPointer[0] != 0) Avx.StoreAligned((float*)(motionStates + indices[0]), Avx.Permute2x128(o0, o1, 0 | (2 << 4))); - if (maskPointer[1] != 0) Avx.StoreAligned((float*)(motionStates + indices[1]), Avx.Permute2x128(o4, o5, 0 | (2 << 4))); - if (maskPointer[2] != 0) Avx.StoreAligned((float*)(motionStates + indices[2]), Avx.Permute2x128(o2, o3, 0 | (2 << 4))); - if (maskPointer[3] != 0) Avx.StoreAligned((float*)(motionStates + indices[3]), Avx.Permute2x128(o6, o7, 0 | (2 << 4))); - if (maskPointer[4] != 0) Avx.StoreAligned((float*)(motionStates + indices[4]), Avx.Permute2x128(o0, o1, 1 | (3 << 4))); - if (maskPointer[5] != 0) Avx.StoreAligned((float*)(motionStates + indices[5]), Avx.Permute2x128(o4, o5, 1 | (3 << 4))); - if (maskPointer[6] != 0) Avx.StoreAligned((float*)(motionStates + indices[6]), Avx.Permute2x128(o2, o3, 1 | (3 << 4))); - if (maskPointer[7] != 0) Avx.StoreAligned((float*)(motionStates + indices[7]), Avx.Permute2x128(o6, o7, 1 | (3 << 4))); + if (maskPointer[0] != 0) Avx.StoreAligned((float*)(states + indices[0]), Avx.Permute2x128(o0, o1, 0 | (2 << 4))); + if (maskPointer[1] != 0) Avx.StoreAligned((float*)(states + indices[1]), Avx.Permute2x128(o4, o5, 0 | (2 << 4))); + if (maskPointer[2] != 0) Avx.StoreAligned((float*)(states + indices[2]), Avx.Permute2x128(o2, o3, 0 | (2 << 4))); + if (maskPointer[3] != 0) Avx.StoreAligned((float*)(states + indices[3]), Avx.Permute2x128(o6, o7, 0 | (2 << 4))); + if (maskPointer[4] != 0) Avx.StoreAligned((float*)(states + indices[4]), Avx.Permute2x128(o0, o1, 1 | (3 << 4))); + if (maskPointer[5] != 0) Avx.StoreAligned((float*)(states + indices[5]), Avx.Permute2x128(o4, o5, 1 | (3 << 4))); + if (maskPointer[6] != 0) Avx.StoreAligned((float*)(states + indices[6]), Avx.Permute2x128(o2, o3, 1 | (3 << 4))); + if (maskPointer[7] != 0) Avx.StoreAligned((float*)(states + indices[7]), Avx.Permute2x128(o6, o7, 1 | (3 << 4))); } } { @@ -611,29 +609,28 @@ public unsafe void ScatterPoseAndInertia( var maskPointer = (int*)Unsafe.AsPointer(ref mask); var indices = (int*)Unsafe.AsPointer(ref references); //Note the offset; we're scattering into the world inertias. - var inertias = ActiveSet.Inertias.Memory; if (typeof(TBatchIntegrationMode) == typeof(BatchShouldAlwaysIntegrate)) { //If we are in an 'always integrate' batch, then the mask is not guaranteed to mask out out-of-range lanes, since we don't otherwise need the mask. - Avx.StoreAligned((float*)(inertias + indices[0]) + 8, Avx.Permute2x128(o0, o1, 0 | (2 << 4))); - if (count > 1) Avx.StoreAligned((float*)(inertias + indices[1]) + 8, Avx.Permute2x128(o4, o5, 0 | (2 << 4))); - if (count > 2) Avx.StoreAligned((float*)(inertias + indices[2]) + 8, Avx.Permute2x128(o2, o3, 0 | (2 << 4))); - if (count > 3) Avx.StoreAligned((float*)(inertias + indices[3]) + 8, Avx.Permute2x128(o6, o7, 0 | (2 << 4))); - if (count > 4) Avx.StoreAligned((float*)(inertias + indices[4]) + 8, Avx.Permute2x128(o0, o1, 1 | (3 << 4))); - if (count > 5) Avx.StoreAligned((float*)(inertias + indices[5]) + 8, Avx.Permute2x128(o4, o5, 1 | (3 << 4))); - if (count > 6) Avx.StoreAligned((float*)(inertias + indices[6]) + 8, Avx.Permute2x128(o2, o3, 1 | (3 << 4))); - if (count > 7) Avx.StoreAligned((float*)(inertias + indices[7]) + 8, Avx.Permute2x128(o6, o7, 1 | (3 << 4))); + Avx.StoreAligned((float*)(states + indices[0]) + 24, Avx.Permute2x128(o0, o1, 0 | (2 << 4))); + if (count > 1) Avx.StoreAligned((float*)(states + indices[1]) + 24, Avx.Permute2x128(o4, o5, 0 | (2 << 4))); + if (count > 2) Avx.StoreAligned((float*)(states + indices[2]) + 24, Avx.Permute2x128(o2, o3, 0 | (2 << 4))); + if (count > 3) Avx.StoreAligned((float*)(states + indices[3]) + 24, Avx.Permute2x128(o6, o7, 0 | (2 << 4))); + if (count > 4) Avx.StoreAligned((float*)(states + indices[4]) + 24, Avx.Permute2x128(o0, o1, 1 | (3 << 4))); + if (count > 5) Avx.StoreAligned((float*)(states + indices[5]) + 24, Avx.Permute2x128(o4, o5, 1 | (3 << 4))); + if (count > 6) Avx.StoreAligned((float*)(states + indices[6]) + 24, Avx.Permute2x128(o2, o3, 1 | (3 << 4))); + if (count > 7) Avx.StoreAligned((float*)(states + indices[7]) + 24, Avx.Permute2x128(o6, o7, 1 | (3 << 4))); } else { - if (maskPointer[0] != 0) Avx.StoreAligned((float*)(inertias + indices[0]) + 8, Avx.Permute2x128(o0, o1, 0 | (2 << 4))); - if (maskPointer[1] != 0) Avx.StoreAligned((float*)(inertias + indices[1]) + 8, Avx.Permute2x128(o4, o5, 0 | (2 << 4))); - if (maskPointer[2] != 0) Avx.StoreAligned((float*)(inertias + indices[2]) + 8, Avx.Permute2x128(o2, o3, 0 | (2 << 4))); - if (maskPointer[3] != 0) Avx.StoreAligned((float*)(inertias + indices[3]) + 8, Avx.Permute2x128(o6, o7, 0 | (2 << 4))); - if (maskPointer[4] != 0) Avx.StoreAligned((float*)(inertias + indices[4]) + 8, Avx.Permute2x128(o0, o1, 1 | (3 << 4))); - if (maskPointer[5] != 0) Avx.StoreAligned((float*)(inertias + indices[5]) + 8, Avx.Permute2x128(o4, o5, 1 | (3 << 4))); - if (maskPointer[6] != 0) Avx.StoreAligned((float*)(inertias + indices[6]) + 8, Avx.Permute2x128(o2, o3, 1 | (3 << 4))); - if (maskPointer[7] != 0) Avx.StoreAligned((float*)(inertias + indices[7]) + 8, Avx.Permute2x128(o6, o7, 1 | (3 << 4))); + if (maskPointer[0] != 0) Avx.StoreAligned((float*)(states + indices[0]) + 24, Avx.Permute2x128(o0, o1, 0 | (2 << 4))); + if (maskPointer[1] != 0) Avx.StoreAligned((float*)(states + indices[1]) + 24, Avx.Permute2x128(o4, o5, 0 | (2 << 4))); + if (maskPointer[2] != 0) Avx.StoreAligned((float*)(states + indices[2]) + 24, Avx.Permute2x128(o2, o3, 0 | (2 << 4))); + if (maskPointer[3] != 0) Avx.StoreAligned((float*)(states + indices[3]) + 24, Avx.Permute2x128(o6, o7, 0 | (2 << 4))); + if (maskPointer[4] != 0) Avx.StoreAligned((float*)(states + indices[4]) + 24, Avx.Permute2x128(o0, o1, 1 | (3 << 4))); + if (maskPointer[5] != 0) Avx.StoreAligned((float*)(states + indices[5]) + 24, Avx.Permute2x128(o4, o5, 1 | (3 << 4))); + if (maskPointer[6] != 0) Avx.StoreAligned((float*)(states + indices[6]) + 24, Avx.Permute2x128(o2, o3, 1 | (3 << 4))); + if (maskPointer[7] != 0) Avx.StoreAligned((float*)(states + indices[7]) + 24, Avx.Permute2x128(o6, o7, 1 | (3 << 4))); } } } @@ -649,13 +646,13 @@ private unsafe void ScatterVelocities(ref BodyVelocityWide sourceVelocities, ref { //TODO: How much value would there be in branching on kinematic state and avoiding a write? Depends a lot on the number of kinematics. ref var sourceSlot = ref GetOffsetInstance(ref sourceVelocities, innerIndex); - ref var target = ref ActiveSet.MotionStates[Unsafe.Add(ref baseIndex, innerIndex)].Velocity; + ref var target = ref ActiveSet.SolverStates[Unsafe.Add(ref baseIndex, innerIndex)].Motion.Velocity; target.Linear = new Vector3(sourceSlot.Linear.X[0], sourceSlot.Linear.Y[0], sourceSlot.Linear.Z[0]); target.Angular = new Vector3(sourceSlot.Angular.X[0], sourceSlot.Angular.Y[0], sourceSlot.Angular.Z[0]); } //[MethodImpl(MethodImplOptions.AggressiveInlining)] - private unsafe void TransposeScatterVelocities(ref BodyVelocityWide sourceVelocities, MotionState* motionStates, ref Vector references, int count) + private unsafe void TransposeScatterVelocities(ref BodyVelocityWide sourceVelocities, SolverState* states, ref Vector references, int count) { if (Avx.IsSupported && Vector.Count == 8) { @@ -698,14 +695,14 @@ private unsafe void TransposeScatterVelocities(ref BodyVelocityWide sourceVeloci var o7 = Avx.Shuffle(n6, n7, 2 | (3 << 2) | (2 << 4) | (3 << 6)); var indices = (int*)Unsafe.AsPointer(ref references); - Avx.StoreAligned((float*)(motionStates + indices[0]) + 8, Avx.Permute2x128(o0, o1, 0 | (2 << 4))); - if (count > 1) Avx.StoreAligned((float*)(motionStates + indices[1]) + 8, Avx.Permute2x128(o4, o5, 0 | (2 << 4))); - if (count > 2) Avx.StoreAligned((float*)(motionStates + indices[2]) + 8, Avx.Permute2x128(o2, o3, 0 | (2 << 4))); - if (count > 3) Avx.StoreAligned((float*)(motionStates + indices[3]) + 8, Avx.Permute2x128(o6, o7, 0 | (2 << 4))); - if (count > 4) Avx.StoreAligned((float*)(motionStates + indices[4]) + 8, Avx.Permute2x128(o0, o1, 1 | (3 << 4))); - if (count > 5) Avx.StoreAligned((float*)(motionStates + indices[5]) + 8, Avx.Permute2x128(o4, o5, 1 | (3 << 4))); - if (count > 6) Avx.StoreAligned((float*)(motionStates + indices[6]) + 8, Avx.Permute2x128(o2, o3, 1 | (3 << 4))); - if (count > 7) Avx.StoreAligned((float*)(motionStates + indices[7]) + 8, Avx.Permute2x128(o6, o7, 1 | (3 << 4))); + Avx.StoreAligned((float*)(states + indices[0]) + 8, Avx.Permute2x128(o0, o1, 0 | (2 << 4))); + if (count > 1) Avx.StoreAligned((float*)(states + indices[1]) + 8, Avx.Permute2x128(o4, o5, 0 | (2 << 4))); + if (count > 2) Avx.StoreAligned((float*)(states + indices[2]) + 8, Avx.Permute2x128(o2, o3, 0 | (2 << 4))); + if (count > 3) Avx.StoreAligned((float*)(states + indices[3]) + 8, Avx.Permute2x128(o6, o7, 0 | (2 << 4))); + if (count > 4) Avx.StoreAligned((float*)(states + indices[4]) + 8, Avx.Permute2x128(o0, o1, 1 | (3 << 4))); + if (count > 5) Avx.StoreAligned((float*)(states + indices[5]) + 8, Avx.Permute2x128(o4, o5, 1 | (3 << 4))); + if (count > 6) Avx.StoreAligned((float*)(states + indices[6]) + 8, Avx.Permute2x128(o2, o3, 1 | (3 << 4))); + if (count > 7) Avx.StoreAligned((float*)(states + indices[7]) + 8, Avx.Permute2x128(o6, o7, 1 | (3 << 4))); } else { @@ -742,9 +739,9 @@ public unsafe void ScatterVelocities(ref BodyVelocityWide sourceVelocitiesA, ref ref TwoBodyReferences references, int count) { Debug.Assert(count >= 0 && count <= Vector.Count); - var motionStates = ActiveSet.MotionStates.Memory; - TransposeScatterVelocities(ref sourceVelocitiesA, motionStates, ref references.IndexA, count); - TransposeScatterVelocities(ref sourceVelocitiesB, motionStates, ref references.IndexB, count); + var states = ActiveSet.SolverStates.Memory; + TransposeScatterVelocities(ref sourceVelocitiesA, states, ref references.IndexA, count); + TransposeScatterVelocities(ref sourceVelocitiesB, states, ref references.IndexB, count); ////Grab the base references for the body indices. Note that we make use of the references memory layout again. //ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); //ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); diff --git a/BepuPhysics/BodyProperties.cs b/BepuPhysics/BodyProperties.cs index 8ffb623a7..245ba30d4 100644 --- a/BepuPhysics/BodyProperties.cs +++ b/BepuPhysics/BodyProperties.cs @@ -184,6 +184,23 @@ public struct BodyInertias public BodyInertia World; } + /// + /// Stores all body information needed by the solver together. + /// + /// + /// With 2.4's revamp of the solver, every solving stage loads pose, velocity, and inertia for every body in each constraint. + /// L2 prefetchers often fetch memory in even-odd pairs of cache lines (see https://www.intel.com/content/dam/www/public/us/en/documents/manuals/64-ia-32-architectures-optimization-manual.pdf#page=162). + /// Since L2 is likely pulling in adjacent cache lines when loading either motion state or inertias, they might as well live together in one block. + /// Note that this goes along with a change to the buffer pool's default alignment to 128 bytes. + /// + [StructLayout(LayoutKind.Sequential)] + public struct SolverState + { + public MotionState Motion; + public BodyInertias Inertia; + } + + public struct RigidPoseWide { public Vector3Wide Position; diff --git a/BepuPhysics/BodyReference.cs b/BepuPhysics/BodyReference.cs index 8ce00c2b4..6c311c79e 100644 --- a/BepuPhysics/BodyReference.cs +++ b/BepuPhysics/BodyReference.cs @@ -98,7 +98,7 @@ public ref BodyVelocity Velocity get { ref var location = ref MemoryLocation; - return ref Bodies.Sets[location.SetIndex].MotionStates[location.Index].Velocity; + return ref Bodies.Sets[location.SetIndex].SolverStates[location.Index].Motion.Velocity; } } @@ -111,7 +111,7 @@ public ref RigidPose Pose get { ref var location = ref MemoryLocation; - return ref Bodies.Sets[location.SetIndex].MotionStates[location.Index].Pose; + return ref Bodies.Sets[location.SetIndex].SolverStates[location.Index].Motion.Pose; } } @@ -137,7 +137,7 @@ public ref BodyInertia LocalInertia get { ref var location = ref MemoryLocation; - return ref Bodies.Sets[location.SetIndex].Inertias[location.Index].Local; + return ref Bodies.Sets[location.SetIndex].SolverStates[location.Index].Inertia.Local; } } @@ -221,9 +221,8 @@ public void ComputeInverseInertia(out Symmetric3x3 inverseInertia) ref var set = ref Bodies.Sets[MemoryLocation.SetIndex]; //Note that inertia.World is ephemeral data packed into the same cache line for the benefit of the solver. //It should not be assumed to contain up to date information outside of the velocity integration to pose integration interval, so this computes world inertia from scratch. - ref var inertia = ref set.Inertias[location.Index]; - ref var pose = ref set.MotionStates[location.Index].Pose; - PoseIntegration.RotateInverseInertia(inertia.Local.InverseInertiaTensor, pose.Orientation, out inverseInertia); + ref var state = ref set.SolverStates[location.Index]; + PoseIntegration.RotateInverseInertia(state.Inertia.Local.InverseInertiaTensor, state.Motion.Pose.Orientation, out inverseInertia); } /// @@ -343,9 +342,8 @@ public static void ApplyImpulse(in Vector3 impulse, in Vector3 impulseOffset, re [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ApplyImpulse(in BodySet set, int index, in Vector3 impulse, in Vector3 impulseOffset) { - ref var localInertia = ref set.Inertias[index].Local; - ref var state = ref set.MotionStates[index]; - ApplyImpulse(impulse, impulseOffset, ref localInertia, ref state.Pose, ref state.Velocity); + ref var state = ref set.SolverStates[index]; + ApplyImpulse(impulse, impulseOffset, ref state.Inertia.Local, ref state.Motion.Pose, ref state.Motion.Velocity); } /// @@ -395,7 +393,8 @@ public void ApplyLinearImpulse(in Vector3 impulse) { ref var location = ref MemoryLocation; ref var set = ref Bodies.Sets[location.SetIndex]; - ApplyLinearImpulse(impulse, set.Inertias[location.Index].Local.InverseMass, ref set.MotionStates[location.Index].Velocity.Linear); + ref var state = ref set.SolverStates[location.Index]; + ApplyLinearImpulse(impulse, state.Inertia.Local.InverseMass, ref state.Motion.Velocity.Linear); } /// @@ -418,12 +417,11 @@ public void ApplyAngularImpulse(in Vector3 angularImpulse) { ref var location = ref MemoryLocation; ref var set = ref Bodies.Sets[location.SetIndex]; - ref var inertia = ref set.Inertias[location.Index]; - ref var state = ref set.MotionStates[location.Index]; + ref var state = ref set.SolverStates[location.Index]; //Note that inertia.World is ephemeral data packed into the same cache line for the benefit of the solver. //It should not be assumed to contain up to date information outside of the velocity integration to pose integration interval, so this computes world inertia from scratch. - PoseIntegration.RotateInverseInertia(inertia.Local.InverseInertiaTensor, state.Pose.Orientation, out var inverseInertia); - ApplyAngularImpulse(angularImpulse, inverseInertia, ref state.Velocity.Angular); + PoseIntegration.RotateInverseInertia(state.Inertia.Local.InverseInertiaTensor, state.Motion.Pose.Orientation, out var inverseInertia); + ApplyAngularImpulse(angularImpulse, inverseInertia, ref state.Motion.Velocity.Angular); } } } diff --git a/BepuPhysics/BodySet.cs b/BepuPhysics/BodySet.cs index 63ce7860b..f3fd5c701 100644 --- a/BepuPhysics/BodySet.cs +++ b/BepuPhysics/BodySet.cs @@ -35,8 +35,10 @@ public struct BodySet /// public Buffer IndexToHandle; - public Buffer MotionStates; - public Buffer Inertias; + /// + /// Stores all data involved in solving constraints for a body, including pose, velocity, and inertia. + /// + public Buffer SolverStates; /// /// The collidables owned by each body in the set. Speculative margins, continuity settings, and shape indices can be changed directly. @@ -88,8 +90,7 @@ internal bool RemoveAt(int bodyIndex, out BodyHandle handle, out int movedBodyIn { movedBodyIndex = Count; //Copy the memory state of the last element down. - MotionStates[bodyIndex] = MotionStates[movedBodyIndex]; - Inertias[bodyIndex] = Inertias[movedBodyIndex]; + SolverStates[bodyIndex] = SolverStates[movedBodyIndex]; Activity[bodyIndex] = Activity[movedBodyIndex]; Collidables[bodyIndex] = Collidables[movedBodyIndex]; //Note that the constraint list is NOT disposed before being overwritten. @@ -124,11 +125,11 @@ internal void ApplyDescriptionByIndex(int index, in BodyDescription description) description.LocalInertia.InverseInertiaTensor.ZZ * description.LocalInertia.InverseInertiaTensor.ZZ), $"Invalid body inverse inertia tensor: {description.LocalInertia.InverseInertiaTensor}"); Debug.Assert(!MathChecker.IsInvalid(description.LocalInertia.InverseMass) && description.LocalInertia.InverseMass >= 0, $"Invalid body inverse mass: {description.LocalInertia.InverseMass}"); - ref var state = ref MotionStates[index]; - state.Pose = description.Pose; - state.Velocity = description.Velocity; + ref var state = ref SolverStates[index]; + state.Motion.Pose = description.Pose; + state.Motion.Velocity = description.Velocity; //Note that the world inertia is only valid in the velocity integration->pose integration interval, so we don't need to initialize it here. - Inertias[index].Local = description.LocalInertia; + state.Inertia.Local = description.LocalInertia; ref var collidable = ref Collidables[index]; collidable.Continuity = description.Collidable.Continuity; collidable.SpeculativeMargin = description.Collidable.SpeculativeMargin; @@ -144,10 +145,10 @@ internal void ApplyDescriptionByIndex(int index, in BodyDescription description) public void GetDescription(int index, out BodyDescription description) { - ref var state = ref MotionStates[index]; - description.Pose = state.Pose; - description.Velocity = state.Velocity; - description.LocalInertia = Inertias[index].Local; + ref var state = ref SolverStates[index]; + description.Pose = state.Motion.Pose; + description.Velocity = state.Motion.Velocity; + description.LocalInertia = state.Inertia.Local; ref var collidable = ref Collidables[index]; description.Collidable.Continuity = collidable.Continuity; description.Collidable.Shape = collidable.Shape; @@ -224,8 +225,7 @@ internal void Swap(int slotA, int slotB, ref Buffer handleTo handleToIndex[IndexToHandle[slotB].Value].Index = slotA; Helpers.Swap(ref IndexToHandle[slotA], ref IndexToHandle[slotB]); Helpers.Swap(ref Collidables[slotA], ref Collidables[slotB]); - Helpers.Swap(ref MotionStates[slotA], ref MotionStates[slotB]); - Helpers.Swap(ref Inertias[slotA], ref Inertias[slotB]); + Helpers.Swap(ref SolverStates[slotA], ref SolverStates[slotB]); Helpers.Swap(ref Activity[slotA], ref Activity[slotB]); Helpers.Swap(ref Constraints[slotA], ref Constraints[slotB]); } @@ -236,9 +236,8 @@ internal unsafe void InternalResize(int targetBodyCapacity, BufferPool pool) //Note that we base the bundle capacities on post-resize capacity of the IndexToHandle array. This simplifies the conditions on allocation, but increases memory use. //You may want to change this in the future if memory use is concerning. targetBodyCapacity = BufferPool.GetCapacityForCount(targetBodyCapacity); - Debug.Assert(MotionStates.Length != BufferPool.GetCapacityForCount(targetBodyCapacity), "Should not try to use internal resize of the result won't change the size."); - pool.ResizeToAtLeast(ref MotionStates, targetBodyCapacity, Count); - pool.ResizeToAtLeast(ref Inertias, targetBodyCapacity, Count); + Debug.Assert(SolverStates.Length != BufferPool.GetCapacityForCount(targetBodyCapacity), "Should not try to use internal resize of the result won't change the size."); + pool.ResizeToAtLeast(ref SolverStates, targetBodyCapacity, Count); pool.ResizeToAtLeast(ref IndexToHandle, targetBodyCapacity, Count); pool.ResizeToAtLeast(ref Collidables, targetBodyCapacity, Count); pool.ResizeToAtLeast(ref Activity, targetBodyCapacity, Count); @@ -260,8 +259,7 @@ public unsafe void Clear(BufferPool pool) /// Pool to return the set's top level buffers to. public void DisposeBuffers(BufferPool pool) { - pool.Return(ref MotionStates); - pool.Return(ref Inertias); + pool.Return(ref SolverStates); pool.Return(ref IndexToHandle); pool.Return(ref Collidables); pool.Return(ref Activity); diff --git a/BepuPhysics/CollisionDetection/NarrowPhase.cs b/BepuPhysics/CollisionDetection/NarrowPhase.cs index 9c5da2a9d..66463de17 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhase.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhase.cs @@ -416,12 +416,12 @@ public unsafe void HandleOverlap(int workerIndex, CollidableReference a, Collida Debug.Assert(bodyLocationA.SetIndex == 0 || bodyLocationB.SetIndex == 0, "One of the two bodies must be active. Otherwise, something is busted!"); ref var setA = ref Bodies.Sets[bodyLocationA.SetIndex]; ref var setB = ref Bodies.Sets[bodyLocationB.SetIndex]; - ref var stateA = ref setA.MotionStates[bodyLocationA.Index]; - ref var stateB = ref setB.MotionStates[bodyLocationB.Index]; + ref var stateA = ref setA.SolverStates[bodyLocationA.Index]; + ref var stateB = ref setB.SolverStates[bodyLocationB.Index]; AddBatchEntries(workerIndex, ref overlapWorker, ref pair, ref setA.Collidables[bodyLocationA.Index], ref setB.Collidables[bodyLocationB.Index], - ref stateA.Pose, ref stateB.Pose, - ref stateA.Velocity, ref stateB.Velocity); + ref stateA.Motion.Pose, ref stateB.Motion.Pose, + ref stateA.Motion.Velocity, ref stateB.Motion.Velocity); } else { @@ -436,11 +436,11 @@ public unsafe void HandleOverlap(int workerIndex, CollidableReference a, Collida //TODO: Ideally, the compiler would see this and optimize away the relevant math in AddBatchEntries. That's a longshot, though. May want to abuse some generics to force it. var zeroVelocity = default(BodyVelocity); ref var bodySet = ref Bodies.ActiveSet; - ref var bodyState = ref bodySet.MotionStates[bodyLocation.Index]; + ref var bodyState = ref bodySet.SolverStates[bodyLocation.Index]; AddBatchEntries(workerIndex, ref overlapWorker, ref pair, ref bodySet.Collidables[bodyLocation.Index], ref Statics.Collidables[staticIndex], - ref bodyState.Pose, ref Statics.Poses[staticIndex], - ref bodyState.Velocity, ref zeroVelocity); + ref bodyState.Motion.Pose, ref Statics.Poses[staticIndex], + ref bodyState.Motion.Velocity, ref zeroVelocity); } } diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 081ff58f5..4d21d997e 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -274,7 +274,7 @@ public unsafe override void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodi ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); - ref var motionStates = ref bodies.ActiveSet.MotionStates; + ref var motionStates = ref bodies.ActiveSet.SolverStates; var function = default(TConstraintFunctions); ref var jacobiResultsBundlesA = ref jacobiResults.GetVelocitiesForBody(0); ref var jacobiResultsBundlesB = ref jacobiResults.GetVelocitiesForBody(1); @@ -307,11 +307,12 @@ static unsafe void Prefetch(void* address) if (Sse.IsSupported) { Sse.Prefetch0(address); + //TODO: prefetch should grab cache line pair anyway, right? not much reason to explicitly do more? } //TODO: ARM? } [MethodImpl(MethodImplOptions.AggressiveInlining)] - static unsafe void PrefetchBundle(MotionState* motionBase, BodyInertias* inertiaBase, ref TwoBodyReferences references, int countInBundle) + static unsafe void PrefetchBundle(SolverState* stateBase, ref TwoBodyReferences references, int countInBundle) { var indicesA = (int*)Unsafe.AsPointer(ref references.IndexA); var indicesB = (int*)Unsafe.AsPointer(ref references.IndexB); @@ -319,35 +320,33 @@ static unsafe void PrefetchBundle(MotionState* motionBase, BodyInertias* inertia { var indexA = indicesA[i]; var indexB = indicesA[i]; - Prefetch(motionBase + indexA); - Prefetch(inertiaBase + indexA); - Prefetch(motionBase + indexB); - Prefetch(inertiaBase + indexB); + Prefetch(stateBase + indexA); + Prefetch(stateBase + indexB); } } const int warmStartPrefetchDistance = 8; const int solvePrefetchDistance = 4; [MethodImpl(MethodImplOptions.AggressiveInlining)] - unsafe static void EarlyPrefetch(int prefetchDistance, ref TypeBatch typeBatch, ref Buffer references, ref Buffer states, ref Buffer inertias, int startBundleIndex, int exclusiveEndBundleIndex) + unsafe static void EarlyPrefetch(int prefetchDistance, ref TypeBatch typeBatch, ref Buffer references, ref Buffer states, int startBundleIndex, int exclusiveEndBundleIndex) { exclusiveEndBundleIndex = Math.Min(exclusiveEndBundleIndex, startBundleIndex + prefetchDistance); var lastBundleIndex = exclusiveEndBundleIndex - 1; for (int i = startBundleIndex; i < lastBundleIndex; ++i) { - PrefetchBundle(states.Memory, inertias.Memory, ref references[i], Vector.Count); + PrefetchBundle(states.Memory, ref references[i], Vector.Count); } var countInBundle = GetCountInBundle(ref typeBatch, lastBundleIndex); - PrefetchBundle(states.Memory, inertias.Memory, ref references[lastBundleIndex], countInBundle); + PrefetchBundle(states.Memory, ref references[lastBundleIndex], countInBundle); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - unsafe static void Prefetch(int prefetchDistance, ref TypeBatch typeBatch, ref Buffer references, ref Buffer states, ref Buffer inertias, int bundleIndex, int exclusiveEndBundleIndex) + unsafe static void Prefetch(int prefetchDistance, ref TypeBatch typeBatch, ref Buffer references, ref Buffer states, int bundleIndex, int exclusiveEndBundleIndex) { var targetIndex = bundleIndex + prefetchDistance; if (targetIndex < exclusiveEndBundleIndex) { - PrefetchBundle(states.Memory, inertias.Memory, ref references[targetIndex], GetCountInBundle(ref typeBatch, targetIndex)); + PrefetchBundle(states.Memory, ref references[targetIndex], GetCountInBundle(ref typeBatch, targetIndex)); } } @@ -551,16 +550,15 @@ public unsafe override void WarmStart2(); var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); var function = default(TConstraintFunctions); - ref var motionStates = ref bodies.ActiveSet.MotionStates; - ref var inertias = ref bodies.ActiveSet.Inertias; - EarlyPrefetch(warmStartPrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, ref inertias, startBundle, exclusiveEndBundle); + ref var states = ref bodies.ActiveSet.SolverStates; + EarlyPrefetch(warmStartPrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref states, startBundle, exclusiveEndBundle); for (int i = startBundle; i < exclusiveEndBundle; ++i) { ref var prestep = ref prestepBundles[i]; ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; ref var references = ref bodyReferencesBundles[i]; var count = GetCountInBundle(ref typeBatch, i); - Prefetch(warmStartPrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, ref inertias, i, exclusiveEndBundle); + Prefetch(warmStartPrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref states, i, exclusiveEndBundle); GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 0, dt, workerIndex, i, ref references.IndexA, count, out var positionA, out var orientationA, out var wsvA, out var inertiaA); GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 1, dt, workerIndex, i, ref references.IndexB, count, @@ -592,16 +590,15 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f var bodyReferencesBundles = typeBatch.BodyReferences.As(); var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); var function = default(TConstraintFunctions); - ref var motionStates = ref bodies.ActiveSet.MotionStates; - ref var inertias = ref bodies.ActiveSet.Inertias; - EarlyPrefetch(solvePrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, ref inertias, startBundle, exclusiveEndBundle); + ref var motionStates = ref bodies.ActiveSet.SolverStates; + EarlyPrefetch(solvePrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, startBundle, exclusiveEndBundle); for (int i = startBundle; i < exclusiveEndBundle; ++i) { ref var prestep = ref prestepBundles[i]; ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; ref var references = ref bodyReferencesBundles[i]; var count = GetCountInBundle(ref typeBatch, i); - Prefetch(solvePrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, ref inertias, i, exclusiveEndBundle); + Prefetch(solvePrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, i, exclusiveEndBundle); bodies.GatherState(ref references, count, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB); if (typeof(TConstraintFunctions) == typeof(WeldFunctions)) { diff --git a/BepuPhysics/FallbackBatch.cs b/BepuPhysics/FallbackBatch.cs index e4cb95bde..c66e53af4 100644 --- a/BepuPhysics/FallbackBatch.cs +++ b/BepuPhysics/FallbackBatch.cs @@ -443,7 +443,7 @@ public void ScatterVelocities(Bodies bodies, Solver solver, ref BufferPose; + pose = &(set.SolverStates.Memory + location.Index)->Motion.Pose; shape = set.Collidables[location.Index].Shape; } } diff --git a/BepuUtilities/Memory/BufferPool.cs b/BepuUtilities/Memory/BufferPool.cs index 771a98307..72a165fd1 100644 --- a/BepuUtilities/Memory/BufferPool.cs +++ b/BepuUtilities/Memory/BufferPool.cs @@ -29,13 +29,14 @@ public Block(int blockSize) //Suballocations from the block will always occur on pow2 boundaries, so the only way for a suballocation to violate this alignment is if an individual //suballocation is smaller than the alignment- in which case it doesn't require the alignment to be that wide. Also, since the alignment and //suballocations are both pow2 sized, they won't drift out of sync. - const int cacheLineSize = 64; //Making an assumption here! But it's a reasonably good one. - Array = new byte[blockSize + cacheLineSize]; + //We pick 128 bytes to allow alignment with cache line pairs: https://www.intel.com/content/dam/www/public/us/en/documents/manuals/64-ia-32-architectures-optimization-manual.pdf#page=162 + const int allocationAlignment = 128; + Array = new byte[blockSize + allocationAlignment]; Handle = GCHandle.Alloc(Array, GCHandleType.Pinned); Pointer = (byte*)Handle.AddrOfPinnedObject(); - var mask = cacheLineSize - 1; + var mask = allocationAlignment - 1; var offset = (uint)Pointer & mask; - Pointer += cacheLineSize - offset; + Pointer += allocationAlignment - offset; } diff --git a/DemoRenderer/Constraints/AngularSwivelHingeLineExtractor.cs b/DemoRenderer/Constraints/AngularSwivelHingeLineExtractor.cs index 2f84a3bf6..2e46849dd 100644 --- a/DemoRenderer/Constraints/AngularSwivelHingeLineExtractor.cs +++ b/DemoRenderer/Constraints/AngularSwivelHingeLineExtractor.cs @@ -14,8 +14,8 @@ public unsafe void ExtractLines(ref AngularSwivelHingePrestepData prestepBundle, Bodies bodies, ref Vector3 tint, ref QuickList lines) { //Could do bundles of constraints at a time, but eh. - ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; - ref var poseB = ref bodies.Sets[setIndex].MotionStates[bodyIndices[1]].Pose; + ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; + ref var poseB = ref bodies.Sets[setIndex].SolverStates[bodyIndices[1]].Motion.Pose; Vector3Wide.ReadFirst(prestepBundle.LocalSwivelAxisA, out var localSwivelAxisA); Vector3Wide.ReadFirst(prestepBundle.LocalHingeAxisB, out var localHingeAxisB); QuaternionEx.Transform(localSwivelAxisA, poseA.Orientation, out var swivelAxis); diff --git a/DemoRenderer/Constraints/BallSocketLineExtractor.cs b/DemoRenderer/Constraints/BallSocketLineExtractor.cs index ecef23075..331eaae3b 100644 --- a/DemoRenderer/Constraints/BallSocketLineExtractor.cs +++ b/DemoRenderer/Constraints/BallSocketLineExtractor.cs @@ -14,8 +14,8 @@ public unsafe void ExtractLines(ref BallSocketPrestepData prestepBundle, int set Bodies bodies, ref Vector3 tint, ref QuickList lines) { //Could do bundles of constraints at a time, but eh. - ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; - ref var poseB = ref bodies.Sets[setIndex].MotionStates[bodyIndices[1]].Pose; + ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; + ref var poseB = ref bodies.Sets[setIndex].SolverStates[bodyIndices[1]].Motion.Pose; Vector3Wide.ReadFirst(prestepBundle.LocalOffsetA, out var localOffsetA); Vector3Wide.ReadFirst(prestepBundle.LocalOffsetB, out var localOffsetB); QuaternionEx.Transform(localOffsetA, poseA.Orientation, out var worldOffsetA); diff --git a/DemoRenderer/Constraints/BallSocketMotorLineExtractor.cs b/DemoRenderer/Constraints/BallSocketMotorLineExtractor.cs index 65b6d47b7..2d1172e43 100644 --- a/DemoRenderer/Constraints/BallSocketMotorLineExtractor.cs +++ b/DemoRenderer/Constraints/BallSocketMotorLineExtractor.cs @@ -14,8 +14,8 @@ public unsafe void ExtractLines(ref BallSocketMotorPrestepData prestepBundle, in Bodies bodies, ref Vector3 tint, ref QuickList lines) { //Could do bundles of constraints at a time, but eh. - ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; - ref var poseB = ref bodies.Sets[setIndex].MotionStates[bodyIndices[1]].Pose; + ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; + ref var poseB = ref bodies.Sets[setIndex].SolverStates[bodyIndices[1]].Motion.Pose; Vector3Wide.ReadFirst(prestepBundle.LocalOffsetB, out var localOffsetB); QuaternionEx.Transform(localOffsetB, poseB.Orientation, out var worldOffsetB); var anchor = poseB.Position + worldOffsetB; diff --git a/DemoRenderer/Constraints/BallSocketServoLineExtractor.cs b/DemoRenderer/Constraints/BallSocketServoLineExtractor.cs index 9f04324fd..c73eb8bce 100644 --- a/DemoRenderer/Constraints/BallSocketServoLineExtractor.cs +++ b/DemoRenderer/Constraints/BallSocketServoLineExtractor.cs @@ -14,8 +14,8 @@ public unsafe void ExtractLines(ref BallSocketServoPrestepData prestepBundle, in Bodies bodies, ref Vector3 tint, ref QuickList lines) { //Could do bundles of constraints at a time, but eh. - ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; - ref var poseB = ref bodies.Sets[setIndex].MotionStates[bodyIndices[1]].Pose; + ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; + ref var poseB = ref bodies.Sets[setIndex].SolverStates[bodyIndices[1]].Motion.Pose; Vector3Wide.ReadFirst(prestepBundle.LocalOffsetA, out var localOffsetA); Vector3Wide.ReadFirst(prestepBundle.LocalOffsetB, out var localOffsetB); QuaternionEx.Transform(localOffsetA, poseA.Orientation, out var worldOffsetA); diff --git a/DemoRenderer/Constraints/CenterDistanceLineExtractor.cs b/DemoRenderer/Constraints/CenterDistanceLineExtractor.cs index 99f0f155f..c219cf0a0 100644 --- a/DemoRenderer/Constraints/CenterDistanceLineExtractor.cs +++ b/DemoRenderer/Constraints/CenterDistanceLineExtractor.cs @@ -15,8 +15,8 @@ public unsafe void ExtractLines(ref CenterDistancePrestepData prestepBundle, int Bodies bodies, ref Vector3 tint, ref QuickList lines) { //Could do bundles of constraints at a time, but eh. - ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; - ref var poseB = ref bodies.Sets[setIndex].MotionStates[bodyIndices[1]].Pose; + ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; + ref var poseB = ref bodies.Sets[setIndex].SolverStates[bodyIndices[1]].Motion.Pose; var targetDistance = GatherScatter.GetFirst(ref prestepBundle.TargetDistance); var color = new Vector3(0.2f, 0.2f, 1f) * tint; var packedColor = Helpers.PackColor(color); diff --git a/DemoRenderer/Constraints/ContactLineExtractors.cs b/DemoRenderer/Constraints/ContactLineExtractors.cs index f8c127440..a6cc9cf77 100644 --- a/DemoRenderer/Constraints/ContactLineExtractors.cs +++ b/DemoRenderer/Constraints/ContactLineExtractors.cs @@ -14,7 +14,7 @@ struct Contact1OneBodyLineExtractor : IConstraintLineExtractor lines) { - ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; + ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; ContactLines.Add(poseA, ref prestepBundle.Contact0.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact0.Depth, tint, ref lines); } } @@ -25,7 +25,7 @@ struct Contact2OneBodyLineExtractor : IConstraintLineExtractor lines) { - ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; + ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; ContactLines.Add(poseA, ref prestepBundle.Contact0.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact0.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact1.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact1.Depth, tint, ref lines); } @@ -37,7 +37,7 @@ struct Contact3OneBodyLineExtractor : IConstraintLineExtractor lines) { - ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; + ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; ContactLines.Add(poseA, ref prestepBundle.Contact0.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact0.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact1.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact1.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact2.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact2.Depth, tint, ref lines); @@ -50,7 +50,7 @@ struct Contact4OneBodyLineExtractor : IConstraintLineExtractor lines) { - ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; + ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; ContactLines.Add(poseA, ref prestepBundle.Contact0.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact0.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact1.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact1.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact2.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact2.Depth, tint, ref lines); @@ -64,7 +64,7 @@ struct Contact1LineExtractor : IConstraintLineExtractor public unsafe void ExtractLines(ref Contact1PrestepData prestepBundle, int setIndex, int* bodyIndices, Bodies bodies, ref Vector3 tint, ref QuickList lines) { - ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; + ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; ContactLines.Add(poseA, ref prestepBundle.Contact0.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact0.Depth, tint, ref lines); } } @@ -75,7 +75,7 @@ struct Contact2LineExtractor : IConstraintLineExtractor public unsafe void ExtractLines(ref Contact2PrestepData prestepBundle, int setIndex, int* bodyIndices, Bodies bodies, ref Vector3 tint, ref QuickList lines) { - ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; + ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; ContactLines.Add(poseA, ref prestepBundle.Contact0.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact0.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact1.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact1.Depth, tint, ref lines); } @@ -87,7 +87,7 @@ struct Contact3LineExtractor : IConstraintLineExtractor public unsafe void ExtractLines(ref Contact3PrestepData prestepBundle, int setIndex, int* bodyIndices, Bodies bodies, ref Vector3 tint, ref QuickList lines) { - ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; + ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; ContactLines.Add(poseA, ref prestepBundle.Contact0.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact0.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact1.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact1.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact2.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact2.Depth, tint, ref lines); @@ -100,7 +100,7 @@ struct Contact4LineExtractor : IConstraintLineExtractor public unsafe void ExtractLines(ref Contact4PrestepData prestepBundle, int setIndex, int* bodyIndices, Bodies bodies, ref Vector3 tint, ref QuickList lines) { - ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; + ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; ContactLines.Add(poseA, ref prestepBundle.Contact0.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact0.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact1.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact1.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact2.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact2.Depth, tint, ref lines); @@ -114,7 +114,7 @@ struct Contact2NonconvexOneBodyLineExtractor : IConstraintLineExtractor lines) { - ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; + ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; ContactLines.Add(poseA, ref prestepBundle.Contact0.Offset, ref prestepBundle.Contact0.Normal, ref prestepBundle.Contact0.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact1.Offset, ref prestepBundle.Contact1.Normal, ref prestepBundle.Contact1.Depth, tint, ref lines); } @@ -126,7 +126,7 @@ struct Contact3NonconvexOneBodyLineExtractor : IConstraintLineExtractor lines) { - ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; + ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; ContactLines.Add(poseA, ref prestepBundle.Contact0.Offset, ref prestepBundle.Contact0.Normal, ref prestepBundle.Contact0.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact1.Offset, ref prestepBundle.Contact1.Normal, ref prestepBundle.Contact1.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact2.Offset, ref prestepBundle.Contact2.Normal, ref prestepBundle.Contact2.Depth, tint, ref lines); @@ -139,7 +139,7 @@ struct Contact4NonconvexOneBodyLineExtractor : IConstraintLineExtractor lines) { - ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; + ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; ContactLines.Add(poseA, ref prestepBundle.Contact0.Offset, ref prestepBundle.Contact0.Normal, ref prestepBundle.Contact0.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact1.Offset, ref prestepBundle.Contact1.Normal, ref prestepBundle.Contact1.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact2.Offset, ref prestepBundle.Contact2.Normal, ref prestepBundle.Contact2.Depth, tint, ref lines); @@ -153,7 +153,7 @@ struct Contact2NonconvexLineExtractor : IConstraintLineExtractor lines) { - ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; + ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; ContactLines.Add(poseA, ref prestepBundle.Contact0.Offset, ref prestepBundle.Contact0.Normal, ref prestepBundle.Contact0.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact1.Offset, ref prestepBundle.Contact1.Normal, ref prestepBundle.Contact1.Depth, tint, ref lines); } @@ -165,7 +165,7 @@ struct Contact3NonconvexLineExtractor : IConstraintLineExtractor lines) { - ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; + ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; ContactLines.Add(poseA, ref prestepBundle.Contact0.Offset, ref prestepBundle.Contact0.Normal, ref prestepBundle.Contact0.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact1.Offset, ref prestepBundle.Contact1.Normal, ref prestepBundle.Contact1.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact2.Offset, ref prestepBundle.Contact2.Normal, ref prestepBundle.Contact2.Depth, tint, ref lines); @@ -178,7 +178,7 @@ struct Contact4NonconvexLineExtractor : IConstraintLineExtractor lines) { - ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; + ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; ContactLines.Add(poseA, ref prestepBundle.Contact0.Offset, ref prestepBundle.Contact0.Normal, ref prestepBundle.Contact0.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact1.Offset, ref prestepBundle.Contact1.Normal, ref prestepBundle.Contact1.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact2.Offset, ref prestepBundle.Contact2.Normal, ref prestepBundle.Contact2.Depth, tint, ref lines); diff --git a/DemoRenderer/Constraints/ContactLineExtractors.tt b/DemoRenderer/Constraints/ContactLineExtractors.tt index 189f67641..3af241e3f 100644 --- a/DemoRenderer/Constraints/ContactLineExtractors.tt +++ b/DemoRenderer/Constraints/ContactLineExtractors.tt @@ -31,7 +31,7 @@ for (int convexity = 0; convexity <= 1; ++convexity) public unsafe void ExtractLines(ref Contact<#=contactCount#><#=convexitySuffix#><#=bodySuffix#>PrestepData prestepBundle, int setIndex, int* bodyIndices, Bodies bodies, ref Vector3 tint, ref QuickList lines) { - ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; + ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; <#for (int i = 0; i < contactCount; ++i) { if(convex) {#> ContactLines.Add(poseA, ref prestepBundle.Contact<#=i#>.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact<#=i#>.Depth, tint, ref lines); <#} else {#> diff --git a/DemoRenderer/Constraints/DistanceLimitLineExtractor.cs b/DemoRenderer/Constraints/DistanceLimitLineExtractor.cs index 7e85b96ca..1a29f6556 100644 --- a/DemoRenderer/Constraints/DistanceLimitLineExtractor.cs +++ b/DemoRenderer/Constraints/DistanceLimitLineExtractor.cs @@ -14,8 +14,8 @@ public unsafe void ExtractLines(ref DistanceLimitPrestepData prestepBundle, int Bodies bodies, ref Vector3 tint, ref QuickList lines) { //Could do bundles of constraints at a time, but eh. - ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; - ref var poseB = ref bodies.Sets[setIndex].MotionStates[bodyIndices[1]].Pose; + ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; + ref var poseB = ref bodies.Sets[setIndex].SolverStates[bodyIndices[1]].Motion.Pose; Vector3Wide.ReadFirst(prestepBundle.LocalOffsetA, out var localOffsetA); Vector3Wide.ReadFirst(prestepBundle.LocalOffsetB, out var localOffsetB); var minimumDistance = GatherScatter.GetFirst(ref prestepBundle.MinimumDistance); diff --git a/DemoRenderer/Constraints/DistanceServoLineExtractor.cs b/DemoRenderer/Constraints/DistanceServoLineExtractor.cs index 4eb058ef8..f8451107d 100644 --- a/DemoRenderer/Constraints/DistanceServoLineExtractor.cs +++ b/DemoRenderer/Constraints/DistanceServoLineExtractor.cs @@ -14,8 +14,8 @@ public unsafe void ExtractLines(ref DistanceServoPrestepData prestepBundle, int Bodies bodies, ref Vector3 tint, ref QuickList lines) { //Could do bundles of constraints at a time, but eh. - ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; - ref var poseB = ref bodies.Sets[setIndex].MotionStates[bodyIndices[1]].Pose; + ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; + ref var poseB = ref bodies.Sets[setIndex].SolverStates[bodyIndices[1]].Motion.Pose; Vector3Wide.ReadFirst(prestepBundle.LocalOffsetA, out var localOffsetA); Vector3Wide.ReadFirst(prestepBundle.LocalOffsetB, out var localOffsetB); var targetDistance = GatherScatter.GetFirst(ref prestepBundle.TargetDistance); diff --git a/DemoRenderer/Constraints/HingeLineExtractor.cs b/DemoRenderer/Constraints/HingeLineExtractor.cs index 7aa8163ac..5e8fa50eb 100644 --- a/DemoRenderer/Constraints/HingeLineExtractor.cs +++ b/DemoRenderer/Constraints/HingeLineExtractor.cs @@ -14,8 +14,8 @@ public unsafe void ExtractLines(ref HingePrestepData prestepBundle, int setIndex Bodies bodies, ref Vector3 tint, ref QuickList lines) { //Could do bundles of constraints at a time, but eh. - ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; - ref var poseB = ref bodies.Sets[setIndex].MotionStates[bodyIndices[1]].Pose; + ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; + ref var poseB = ref bodies.Sets[setIndex].SolverStates[bodyIndices[1]].Motion.Pose; Vector3Wide.ReadFirst(prestepBundle.LocalHingeAxisA, out var localHingeAxisA); Vector3Wide.ReadFirst(prestepBundle.LocalOffsetA, out var localOffsetA); Vector3Wide.ReadFirst(prestepBundle.LocalHingeAxisB, out var localHingeAxisB); diff --git a/DemoRenderer/Constraints/LinearAxisServoLineExtractor.cs b/DemoRenderer/Constraints/LinearAxisServoLineExtractor.cs index b5fe69780..7b330c3dd 100644 --- a/DemoRenderer/Constraints/LinearAxisServoLineExtractor.cs +++ b/DemoRenderer/Constraints/LinearAxisServoLineExtractor.cs @@ -14,8 +14,8 @@ public unsafe void ExtractLines(ref LinearAxisServoPrestepData prestepBundle, in Bodies bodies, ref Vector3 tint, ref QuickList lines) { //Could do bundles of constraints at a time, but eh. - ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; - ref var poseB = ref bodies.Sets[setIndex].MotionStates[bodyIndices[1]].Pose; + ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; + ref var poseB = ref bodies.Sets[setIndex].SolverStates[bodyIndices[1]].Motion.Pose; Vector3Wide.ReadFirst(prestepBundle.LocalOffsetA, out var localOffsetA); Vector3Wide.ReadFirst(prestepBundle.LocalOffsetB, out var localOffsetB); Vector3Wide.ReadFirst(prestepBundle.LocalPlaneNormal, out var localPlaneNormal); diff --git a/DemoRenderer/Constraints/OneBodyLinearServoLineExtractor.cs b/DemoRenderer/Constraints/OneBodyLinearServoLineExtractor.cs index 84c66e3ba..b60c469e7 100644 --- a/DemoRenderer/Constraints/OneBodyLinearServoLineExtractor.cs +++ b/DemoRenderer/Constraints/OneBodyLinearServoLineExtractor.cs @@ -14,7 +14,7 @@ public unsafe void ExtractLines(ref OneBodyLinearServoPrestepData prestepBundle, Bodies bodies, ref Vector3 tint, ref QuickList lines) { //Could do bundles of constraints at a time, but eh. - ref var pose = ref bodies.Sets[setIndex].MotionStates[*bodyIndices].Pose; + ref var pose = ref bodies.Sets[setIndex].SolverStates[*bodyIndices].Motion.Pose; Vector3Wide.ReadFirst(prestepBundle.LocalOffset, out var localOffset); Vector3Wide.ReadFirst(prestepBundle.Target, out var target); QuaternionEx.Transform(localOffset, pose.Orientation, out var worldOffset); diff --git a/DemoRenderer/Constraints/PointOnLineLineExtractor.cs b/DemoRenderer/Constraints/PointOnLineLineExtractor.cs index 6bbd59988..f69e6835b 100644 --- a/DemoRenderer/Constraints/PointOnLineLineExtractor.cs +++ b/DemoRenderer/Constraints/PointOnLineLineExtractor.cs @@ -14,8 +14,8 @@ public unsafe void ExtractLines(ref PointOnLineServoPrestepData prestepBundle, i Bodies bodies, ref Vector3 tint, ref QuickList lines) { //Could do bundles of constraints at a time, but eh. - ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; - ref var poseB = ref bodies.Sets[setIndex].MotionStates[bodyIndices[1]].Pose; + ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; + ref var poseB = ref bodies.Sets[setIndex].SolverStates[bodyIndices[1]].Motion.Pose; Vector3Wide.ReadFirst(prestepBundle.LocalOffsetA, out var localOffsetA); Vector3Wide.ReadFirst(prestepBundle.LocalOffsetB, out var localOffsetB); Vector3Wide.ReadFirst(prestepBundle.LocalDirection, out var localDirection); diff --git a/DemoRenderer/Constraints/SwivelHingeLineExtractor.cs b/DemoRenderer/Constraints/SwivelHingeLineExtractor.cs index f880afa27..c5c5ba0e4 100644 --- a/DemoRenderer/Constraints/SwivelHingeLineExtractor.cs +++ b/DemoRenderer/Constraints/SwivelHingeLineExtractor.cs @@ -14,8 +14,8 @@ public unsafe void ExtractLines(ref SwivelHingePrestepData prestepBundle, int se Bodies bodies, ref Vector3 tint, ref QuickList lines) { //Could do bundles of constraints at a time, but eh. - ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; - ref var poseB = ref bodies.Sets[setIndex].MotionStates[bodyIndices[1]].Pose; + ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; + ref var poseB = ref bodies.Sets[setIndex].SolverStates[bodyIndices[1]].Motion.Pose; Vector3Wide.ReadFirst(prestepBundle.LocalSwivelAxisA, out var localSwivelAxisA); Vector3Wide.ReadFirst(prestepBundle.LocalOffsetA, out var localOffsetA); Vector3Wide.ReadFirst(prestepBundle.LocalHingeAxisB, out var localHingeAxisB); diff --git a/DemoRenderer/Constraints/WeldLineExtractor.cs b/DemoRenderer/Constraints/WeldLineExtractor.cs index 36d76f577..730620cd0 100644 --- a/DemoRenderer/Constraints/WeldLineExtractor.cs +++ b/DemoRenderer/Constraints/WeldLineExtractor.cs @@ -14,8 +14,8 @@ public unsafe void ExtractLines(ref WeldPrestepData prestepBundle, int setIndex, Bodies bodies, ref Vector3 tint, ref QuickList lines) { //Could do bundles of constraints at a time, but eh. - ref var poseA = ref bodies.Sets[setIndex].MotionStates[bodyIndices[0]].Pose; - ref var poseB = ref bodies.Sets[setIndex].MotionStates[bodyIndices[1]].Pose; + ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; + ref var poseB = ref bodies.Sets[setIndex].SolverStates[bodyIndices[1]].Motion.Pose; Vector3Wide.ReadFirst(prestepBundle.LocalOffset, out var localOffset); QuaternionEx.Transform(localOffset, poseA.Orientation, out var worldOffset); var bTarget = poseA.Position + worldOffset; diff --git a/DemoRenderer/ShapeDrawing/ShapesExtractor.cs b/DemoRenderer/ShapeDrawing/ShapesExtractor.cs index 76b98bea0..e52f8dc56 100644 --- a/DemoRenderer/ShapeDrawing/ShapesExtractor.cs +++ b/DemoRenderer/ShapeDrawing/ShapesExtractor.cs @@ -236,10 +236,10 @@ void AddBodyShape(Shapes shapes, Bodies bodies, int setIndex, int indexInSet) //3) Activity state //The handle is hashed to get variation. ref var activity = ref set.Activity[indexInSet]; - ref var inertia = ref set.Inertias[indexInSet].Local; Vector3 color; Helpers.UnpackColor((uint)HashHelper.Rehash(handle.Value), out Vector3 colorVariation); - if (Bodies.IsKinematic(inertia)) + ref var state = ref set.SolverStates[indexInSet]; + if (Bodies.IsKinematic(state.Inertia.Local)) { var kinematicBase = new Vector3(0, 0.609f, 0.37f); var kinematicVariationSpan = new Vector3(0.1f, 0.1f, 0.1f); @@ -266,7 +266,7 @@ void AddBodyShape(Shapes shapes, Bodies bodies, int setIndex, int indexInSet) color *= sleepTint; } - AddShape(shapes, set.Collidables[indexInSet].Shape, ref set.MotionStates[indexInSet].Pose, color); + AddShape(shapes, set.Collidables[indexInSet].Shape, ref state.Motion.Pose, color); } [MethodImpl(MethodImplOptions.AggressiveInlining)] diff --git a/DemoTests/TestUtilities.cs b/DemoTests/TestUtilities.cs index 8ad454fa6..88308c4ca 100644 --- a/DemoTests/TestUtilities.cs +++ b/DemoTests/TestUtilities.cs @@ -45,7 +45,7 @@ public static long ComputeHash(ref Vector3 v, long constant) { for (int bodyIndex = 0; bodyIndex < set.Count; ++bodyIndex) { - ref var state = ref set.MotionStates[bodyIndex]; + ref var state = ref set.SolverStates[bodyIndex].Motion; ref var pose = ref state.Pose; ref var velocity = ref state.Velocity; var poseHash = ComputeHash(ref pose.Position, 89) + ComputeHash(ref pose.Orientation.X, 107) + ComputeHash(ref pose.Orientation.Y, 113) + ComputeHash(ref pose.Orientation.Z, 131) + ComputeHash(ref pose.Orientation.W, 149); diff --git a/Demos/Demo.cs b/Demos/Demo.cs index f00656836..37ebe5190 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -43,7 +43,7 @@ protected Demo() //It may be worth using something like hwloc to extract extra information to reason about. //var targetThreadCount = Math.Max(1, Environment.ProcessorCount > 4 ? Environment.ProcessorCount - 2 : Environment.ProcessorCount - 1); - var targetThreadCount = 32; + var targetThreadCount = 30; ThreadDispatcher = new SimpleThreadDispatcher(targetThreadCount); } diff --git a/Demos/Demos/Characters/CharacterControllers.cs b/Demos/Demos/Characters/CharacterControllers.cs index e3a32e97f..954df3c17 100644 --- a/Demos/Demos/Characters/CharacterControllers.cs +++ b/Demos/Demos/Characters/CharacterControllers.cs @@ -279,7 +279,7 @@ bool TryReportContacts(CollidableReference characterCollidable, Colli //Have to take into account the current potentially inactive location. ref var bodyLocation = ref Simulation.Bodies.HandleToLocation[character.BodyHandle.Value]; ref var set = ref Simulation.Bodies.Sets[bodyLocation.SetIndex]; - ref var pose = ref set.MotionStates[bodyLocation.Index].Pose; + ref var pose = ref set.SolverStates[bodyLocation.Index].Motion.Pose; QuaternionEx.Transform(character.LocalUp, pose.Orientation, out var up); //Note that this branch is compiled out- the generic constraints force type specialization. if (manifold.Convex) @@ -594,16 +594,16 @@ void AnalyzeContactsForCharacterRegion(int start, int exclusiveEnd, int workerIn //If the character is jumping, don't create a constraint. if (supportCandidate.Depth > float.MinValue && character.TryJump) { - QuaternionEx.Transform(character.LocalUp, Simulation.Bodies.ActiveSet.MotionStates[bodyLocation.Index].Pose.Orientation, out var characterUp); + QuaternionEx.Transform(character.LocalUp, Simulation.Bodies.ActiveSet.SolverStates[bodyLocation.Index].Motion.Pose.Orientation, out var characterUp); //Note that we assume that character orientations are constant. This isn't necessarily the case in all uses, but it's a decent approximation. - var characterUpVelocity = Vector3.Dot(Simulation.Bodies.ActiveSet.MotionStates[bodyLocation.Index].Velocity.Linear, characterUp); + var characterUpVelocity = Vector3.Dot(Simulation.Bodies.ActiveSet.SolverStates[bodyLocation.Index].Motion.Velocity.Linear, characterUp); //We don't want the character to be able to 'superboost' by simply adding jump speed on top of horizontal motion. //Instead, jumping targets a velocity change necessary to reach character.JumpVelocity along the up axis. if (character.Support.Mobility != CollidableMobility.Static) { ref var supportingBodyLocation = ref Simulation.Bodies.HandleToLocation[character.Support.BodyHandle.Value]; Debug.Assert(supportingBodyLocation.SetIndex == 0, "If the character is active, any support should be too."); - ref var supportVelocity = ref Simulation.Bodies.ActiveSet.MotionStates[supportingBodyLocation.Index].Velocity; + ref var supportVelocity = ref Simulation.Bodies.ActiveSet.SolverStates[supportingBodyLocation.Index].Motion.Velocity; var wxr = Vector3.Cross(supportVelocity.Angular, supportCandidate.OffsetFromSupport); var supportContactVelocity = supportVelocity.Linear + wxr; var supportUpVelocity = Vector3.Dot(supportContactVelocity, characterUp); @@ -644,7 +644,7 @@ void AnalyzeContactsForCharacterRegion(int start, int exclusiveEnd, int workerIn Matrix3x3 surfaceBasis; surfaceBasis.Y = supportCandidate.Normal; //Note negation: we're using a right handed basis where -Z is forward, +Z is backward. - QuaternionEx.Transform(character.LocalUp, Simulation.Bodies.ActiveSet.MotionStates[bodyLocation.Index].Pose.Orientation, out var up); + QuaternionEx.Transform(character.LocalUp, Simulation.Bodies.ActiveSet.SolverStates[bodyLocation.Index].Motion.Pose.Orientation, out var up); var rayDistance = Vector3.Dot(character.ViewDirection, surfaceBasis.Y); var rayVelocity = Vector3.Dot(up, surfaceBasis.Y); Debug.Assert(rayVelocity > 0, @@ -828,10 +828,10 @@ void AnalyzeContacts(float dt, IThreadDispatcher threadDispatcher) for (int i = 0; i < workerCache.Jumps.Count; ++i) { ref var jump = ref workerCache.Jumps[i]; - activeSet.MotionStates[jump.CharacterBodyIndex].Velocity.Linear += jump.CharacterVelocityChange; + activeSet.SolverStates[jump.CharacterBodyIndex].Motion.Velocity.Linear += jump.CharacterVelocityChange; if (jump.SupportBodyIndex >= 0) { - BodyReference.ApplyImpulse(Simulation.Bodies.ActiveSet, jump.SupportBodyIndex, jump.CharacterVelocityChange / -activeSet.Inertias[jump.CharacterBodyIndex].Local.InverseMass, jump.SupportImpulseOffset); + BodyReference.ApplyImpulse(Simulation.Bodies.ActiveSet, jump.SupportBodyIndex, jump.CharacterVelocityChange / -activeSet.SolverStates[jump.CharacterBodyIndex].Inertia.Local.InverseMass, jump.SupportImpulseOffset); } } workerCache.Dispose(pool); diff --git a/Demos/Demos/FountainStressTestDemo.cs b/Demos/Demos/FountainStressTestDemo.cs index 1d12db084..394df2e95 100644 --- a/Demos/Demos/FountainStressTestDemo.cs +++ b/Demos/Demos/FountainStressTestDemo.cs @@ -260,7 +260,7 @@ public override void Update(Window window, Camera camera, Input input, float dt) 16 + 16 * (float)Math.Cos(4 * (angle + t * 0.5)), radius * (float)Math.Sin(positionAngle)); - var correction = targetLocation - set.MotionStates[bodyLocation.Index].Pose.Position; + var correction = targetLocation - set.SolverStates[bodyLocation.Index].Motion.Pose.Position; var distance = correction.Length(); if (distance > 1e-4) { @@ -274,13 +274,13 @@ public override void Update(Window window, Camera camera, Input input, float dt) correction *= maxDisplacement / distance; } Debug.Assert(bodyLocation.SetIndex == 0); - Simulation.Bodies.ActiveSet.MotionStates[bodyLocation.Index].Velocity.Linear = correction * inverseDt; + Simulation.Bodies.ActiveSet.SolverStates[bodyLocation.Index].Motion.Velocity.Linear = correction * inverseDt; } else { if (bodyLocation.SetIndex == 0) { - Simulation.Bodies.ActiveSet.MotionStates[bodyLocation.Index].Velocity.Linear = new Vector3(); + Simulation.Bodies.ActiveSet.SolverStates[bodyLocation.Index].Motion.Velocity.Linear = new Vector3(); } } } diff --git a/Demos/SpecializedTests/DeterminismTest.cs b/Demos/SpecializedTests/DeterminismTest.cs index 4df939ef8..e9bf2a7a2 100644 --- a/Demos/SpecializedTests/DeterminismTest.cs +++ b/Demos/SpecializedTests/DeterminismTest.cs @@ -12,11 +12,6 @@ namespace Demos.SpecializedTests { public static class DeterminismTest where T : Demo, new() { - struct MotionState - { - public RigidPose Pose; - public BodyVelocity Velocity; - } static Dictionary ExecuteSimulation(ContentArchive content, int frameCount) { var demo = new T(); @@ -36,7 +31,7 @@ static Dictionary ExecuteSimulation(ContentArchive content, in { for (int bodyIndex = 0; bodyIndex < set.Count; ++bodyIndex) { - motionStates.Add(set.IndexToHandle[bodyIndex].Value, new MotionState { Pose = set.MotionStates[bodyIndex].Pose, Velocity = set.MotionStates[bodyIndex].Velocity }); + motionStates.Add(set.IndexToHandle[bodyIndex].Value, set.SolverStates[bodyIndex].Motion); } } } diff --git a/Demos/SpecializedTests/TestHelpers.cs b/Demos/SpecializedTests/TestHelpers.cs index e674d0e0b..622b5fc9e 100644 --- a/Demos/SpecializedTests/TestHelpers.cs +++ b/Demos/SpecializedTests/TestHelpers.cs @@ -17,8 +17,9 @@ public static float GetBodyEnergyHeuristic(Bodies bodies) float accumulated = 0; for (int index = 0; index < bodies.ActiveSet.Count; ++index) { - accumulated += Vector3.Dot(bodies.ActiveSet.MotionStates[index].Velocity.Linear, bodies.ActiveSet.MotionStates[index].Velocity.Linear); - accumulated += Vector3.Dot(bodies.ActiveSet.MotionStates[index].Velocity.Angular, bodies.ActiveSet.MotionStates[index].Velocity.Angular); + ref var velocity = ref bodies.ActiveSet.SolverStates[index].Motion.Velocity; + accumulated += Vector3.Dot(velocity.Linear, velocity.Linear); + accumulated += Vector3.Dot(velocity.Angular, velocity.Angular); } return accumulated; } From a81b3c19c3c3dc6a279742ed1397fc8909c88cc7 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 16 Jul 2021 19:41:13 -0500 Subject: [PATCH 115/947] Rename for consistency. --- BepuPhysics/Constraints/AngularAxisGearMotor.cs | 2 +- BepuPhysics/Constraints/AngularAxisMotor.cs | 2 +- BepuPhysics/Constraints/AngularHinge.cs | 2 +- BepuPhysics/Constraints/AngularMotor.cs | 2 +- BepuPhysics/Constraints/AngularServo.cs | 2 +- BepuPhysics/Constraints/AngularSwivelHinge.cs | 2 +- BepuPhysics/Constraints/BallSocket.cs | 2 +- BepuPhysics/Constraints/BallSocketMotor.cs | 2 +- BepuPhysics/Constraints/BallSocketServo.cs | 2 +- BepuPhysics/Constraints/CenterDistanceConstraint.cs | 2 +- BepuPhysics/Constraints/DistanceLimit.cs | 2 +- BepuPhysics/Constraints/DistanceServo.cs | 2 +- BepuPhysics/Constraints/Hinge.cs | 2 +- BepuPhysics/Constraints/LinearAxisLimit.cs | 2 +- BepuPhysics/Constraints/LinearAxisMotor.cs | 2 +- BepuPhysics/Constraints/LinearAxisServo.cs | 2 +- BepuPhysics/Constraints/PointOnLineServo.cs | 2 +- BepuPhysics/Constraints/SwingLimit.cs | 2 +- BepuPhysics/Constraints/SwivelHinge.cs | 2 +- BepuPhysics/Constraints/TwistLimit.cs | 2 +- BepuPhysics/Constraints/TwistMotor.cs | 2 +- BepuPhysics/Constraints/TwistServo.cs | 2 +- BepuPhysics/Constraints/TwoBodyTypeProcessor.cs | 6 +++--- BepuPhysics/Constraints/Weld.cs | 2 +- Demos/Demos/Characters/CharacterMotionConstraint.cs | 2 +- 25 files changed, 27 insertions(+), 27 deletions(-) diff --git a/BepuPhysics/Constraints/AngularAxisGearMotor.cs b/BepuPhysics/Constraints/AngularAxisGearMotor.cs index 757618ff3..b160abb62 100644 --- a/BepuPhysics/Constraints/AngularAxisGearMotor.cs +++ b/BepuPhysics/Constraints/AngularAxisGearMotor.cs @@ -78,7 +78,7 @@ public struct AngularAxisGearMotorProjection } - public struct AngularAxisGearMotorFunctions : IConstraintFunctions> + public struct AngularAxisGearMotorFunctions : ITwoBodyConstraintFunctions> { [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, diff --git a/BepuPhysics/Constraints/AngularAxisMotor.cs b/BepuPhysics/Constraints/AngularAxisMotor.cs index 45f8d9007..a1ab24c3f 100644 --- a/BepuPhysics/Constraints/AngularAxisMotor.cs +++ b/BepuPhysics/Constraints/AngularAxisMotor.cs @@ -77,7 +77,7 @@ public struct AngularAxisMotorProjection } - public struct AngularAxisMotorFunctions : IConstraintFunctions> + public struct AngularAxisMotorFunctions : ITwoBodyConstraintFunctions> { [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, diff --git a/BepuPhysics/Constraints/AngularHinge.cs b/BepuPhysics/Constraints/AngularHinge.cs index af39f15d1..524bee043 100644 --- a/BepuPhysics/Constraints/AngularHinge.cs +++ b/BepuPhysics/Constraints/AngularHinge.cs @@ -79,7 +79,7 @@ public struct AngularHingeProjection } - public struct AngularHingeFunctions : IConstraintFunctions + public struct AngularHingeFunctions : ITwoBodyConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void GetErrorAngles(in Vector3Wide hingeAxisA, in Vector3Wide hingeAxisB, in Matrix2x3Wide jacobianA, out Vector2Wide errorAngles) diff --git a/BepuPhysics/Constraints/AngularMotor.cs b/BepuPhysics/Constraints/AngularMotor.cs index e4947682f..59affbe2d 100644 --- a/BepuPhysics/Constraints/AngularMotor.cs +++ b/BepuPhysics/Constraints/AngularMotor.cs @@ -68,7 +68,7 @@ public struct AngularMotorProjection } - public struct AngularMotorFunctions : IConstraintFunctions + public struct AngularMotorFunctions : ITwoBodyConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, diff --git a/BepuPhysics/Constraints/AngularServo.cs b/BepuPhysics/Constraints/AngularServo.cs index b9afba394..208833bf7 100644 --- a/BepuPhysics/Constraints/AngularServo.cs +++ b/BepuPhysics/Constraints/AngularServo.cs @@ -76,7 +76,7 @@ public struct AngularServoProjection } - public struct AngularServoFunctions : IConstraintFunctions + public struct AngularServoFunctions : ITwoBodyConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, diff --git a/BepuPhysics/Constraints/AngularSwivelHinge.cs b/BepuPhysics/Constraints/AngularSwivelHinge.cs index ee7f80f73..7e9557dc8 100644 --- a/BepuPhysics/Constraints/AngularSwivelHinge.cs +++ b/BepuPhysics/Constraints/AngularSwivelHinge.cs @@ -78,7 +78,7 @@ public struct AngularSwivelHingeProjection public Vector3Wide NegatedImpulseToVelocityB; } - public struct AngularSwivelHingeFunctions : IConstraintFunctions> + public struct AngularSwivelHingeFunctions : ITwoBodyConstraintFunctions> { [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, diff --git a/BepuPhysics/Constraints/BallSocket.cs b/BepuPhysics/Constraints/BallSocket.cs index 260402fb7..30c1e14f3 100644 --- a/BepuPhysics/Constraints/BallSocket.cs +++ b/BepuPhysics/Constraints/BallSocket.cs @@ -75,7 +75,7 @@ public struct BallSocketProjection public BodyInertiaWide InertiaB; } - public struct BallSocketFunctions : IConstraintFunctions + public struct BallSocketFunctions : ITwoBodyConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, diff --git a/BepuPhysics/Constraints/BallSocketMotor.cs b/BepuPhysics/Constraints/BallSocketMotor.cs index eaee44460..cb17fe5c4 100644 --- a/BepuPhysics/Constraints/BallSocketMotor.cs +++ b/BepuPhysics/Constraints/BallSocketMotor.cs @@ -80,7 +80,7 @@ public struct BallSocketMotorProjection public BodyInertiaWide InertiaB; } - public struct BallSocketMotorFunctions : IConstraintFunctions + public struct BallSocketMotorFunctions : ITwoBodyConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, diff --git a/BepuPhysics/Constraints/BallSocketServo.cs b/BepuPhysics/Constraints/BallSocketServo.cs index 108a2eef8..422f8fc8d 100644 --- a/BepuPhysics/Constraints/BallSocketServo.cs +++ b/BepuPhysics/Constraints/BallSocketServo.cs @@ -84,7 +84,7 @@ public struct BallSocketServoProjection public BodyInertiaWide InertiaB; } - public struct BallSocketServoFunctions : IConstraintFunctions + public struct BallSocketServoFunctions : ITwoBodyConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, diff --git a/BepuPhysics/Constraints/CenterDistanceConstraint.cs b/BepuPhysics/Constraints/CenterDistanceConstraint.cs index fe211ffac..f44652bf3 100644 --- a/BepuPhysics/Constraints/CenterDistanceConstraint.cs +++ b/BepuPhysics/Constraints/CenterDistanceConstraint.cs @@ -76,7 +76,7 @@ public struct CenterDistanceProjection public Vector InverseMassB; } - public struct CenterDistanceConstraintFunctions : IConstraintFunctions> + public struct CenterDistanceConstraintFunctions : ITwoBodyConstraintFunctions> { [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, diff --git a/BepuPhysics/Constraints/DistanceLimit.cs b/BepuPhysics/Constraints/DistanceLimit.cs index 545800eb0..9983f9367 100644 --- a/BepuPhysics/Constraints/DistanceLimit.cs +++ b/BepuPhysics/Constraints/DistanceLimit.cs @@ -113,7 +113,7 @@ public struct DistanceLimitProjection public Vector3Wide AngularImpulseToVelocityB; } - public struct DistanceLimitFunctions : IConstraintFunctions> + public struct DistanceLimitFunctions : ITwoBodyConstraintFunctions> { [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, diff --git a/BepuPhysics/Constraints/DistanceServo.cs b/BepuPhysics/Constraints/DistanceServo.cs index e9e49e67d..ebfa3394b 100644 --- a/BepuPhysics/Constraints/DistanceServo.cs +++ b/BepuPhysics/Constraints/DistanceServo.cs @@ -118,7 +118,7 @@ public struct DistanceServoProjection public Vector3Wide AngularImpulseToVelocityB; } - public struct DistanceServoFunctions : IConstraintFunctions> + public struct DistanceServoFunctions : ITwoBodyConstraintFunctions> { public static void GetDistance(in QuaternionWide orientationA, in Vector3Wide ab, in QuaternionWide orientationB, in Vector3Wide localOffsetA, in Vector3Wide localOffsetB, out Vector3Wide anchorOffsetA, out Vector3Wide anchorOffsetB, out Vector3Wide anchorOffset, out Vector distance) diff --git a/BepuPhysics/Constraints/Hinge.cs b/BepuPhysics/Constraints/Hinge.cs index 16516ec81..e27fdd1a8 100644 --- a/BepuPhysics/Constraints/Hinge.cs +++ b/BepuPhysics/Constraints/Hinge.cs @@ -99,7 +99,7 @@ public struct HingeAccumulatedImpulses public Vector2Wide Hinge; } - public struct HingeFunctions : IConstraintFunctions + public struct HingeFunctions : ITwoBodyConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, diff --git a/BepuPhysics/Constraints/LinearAxisLimit.cs b/BepuPhysics/Constraints/LinearAxisLimit.cs index 995830ba4..84b2fb60b 100644 --- a/BepuPhysics/Constraints/LinearAxisLimit.cs +++ b/BepuPhysics/Constraints/LinearAxisLimit.cs @@ -87,7 +87,7 @@ public struct LinearAxisLimitPrestepData public SpringSettingsWide SpringSettings; } - public struct LinearAxisLimitFunctions : IConstraintFunctions> + public struct LinearAxisLimitFunctions : ITwoBodyConstraintFunctions> { public struct LimitJacobianModifier : LinearAxisServoFunctions.IJacobianModifier { diff --git a/BepuPhysics/Constraints/LinearAxisMotor.cs b/BepuPhysics/Constraints/LinearAxisMotor.cs index b3d9ce437..3ebbafd13 100644 --- a/BepuPhysics/Constraints/LinearAxisMotor.cs +++ b/BepuPhysics/Constraints/LinearAxisMotor.cs @@ -79,7 +79,7 @@ public struct LinearAxisMotorPrestepData public MotorSettingsWide Settings; } - public struct LinearAxisMotorFunctions : IConstraintFunctions> + public struct LinearAxisMotorFunctions : ITwoBodyConstraintFunctions> { [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, diff --git a/BepuPhysics/Constraints/LinearAxisServo.cs b/BepuPhysics/Constraints/LinearAxisServo.cs index 4ffaa4c45..2dc8c6470 100644 --- a/BepuPhysics/Constraints/LinearAxisServo.cs +++ b/BepuPhysics/Constraints/LinearAxisServo.cs @@ -100,7 +100,7 @@ public struct LinearAxisServoProjection public Vector3Wide AngularImpulseToVelocityB; } - public struct LinearAxisServoFunctions : IConstraintFunctions> + public struct LinearAxisServoFunctions : ITwoBodyConstraintFunctions> { public interface IJacobianModifier { diff --git a/BepuPhysics/Constraints/PointOnLineServo.cs b/BepuPhysics/Constraints/PointOnLineServo.cs index 214977b8f..5f1803008 100644 --- a/BepuPhysics/Constraints/PointOnLineServo.cs +++ b/BepuPhysics/Constraints/PointOnLineServo.cs @@ -92,7 +92,7 @@ public struct PointOnLineServoProjection public BodyInertiaWide InertiaB; } - public struct PointOnLineServoFunctions : IConstraintFunctions + public struct PointOnLineServoFunctions : ITwoBodyConstraintFunctions { static void GetAngularJacobians(in Matrix2x3Wide linearJacobians, in Vector3Wide offsetA, in Vector3Wide offsetB, out Matrix2x3Wide angularJacobianA, out Matrix2x3Wide angularJacobianB) { diff --git a/BepuPhysics/Constraints/SwingLimit.cs b/BepuPhysics/Constraints/SwingLimit.cs index 3ecf6f08b..872357211 100644 --- a/BepuPhysics/Constraints/SwingLimit.cs +++ b/BepuPhysics/Constraints/SwingLimit.cs @@ -99,7 +99,7 @@ public struct SwingLimitProjection public Vector3Wide NegatedImpulseToVelocityB; } - public struct SwingLimitFunctions : IConstraintFunctions> + public struct SwingLimitFunctions : ITwoBodyConstraintFunctions> { [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, diff --git a/BepuPhysics/Constraints/SwivelHinge.cs b/BepuPhysics/Constraints/SwivelHinge.cs index 9a99c4cf4..6a327f8cb 100644 --- a/BepuPhysics/Constraints/SwivelHinge.cs +++ b/BepuPhysics/Constraints/SwivelHinge.cs @@ -92,7 +92,7 @@ public struct SwivelHingeProjection public BodyInertiaWide InertiaB; } - public struct SwivelHingeFunctions : IConstraintFunctions + public struct SwivelHingeFunctions : ITwoBodyConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, diff --git a/BepuPhysics/Constraints/TwistLimit.cs b/BepuPhysics/Constraints/TwistLimit.cs index dc7811d3b..759e5dd95 100644 --- a/BepuPhysics/Constraints/TwistLimit.cs +++ b/BepuPhysics/Constraints/TwistLimit.cs @@ -92,7 +92,7 @@ public struct TwistLimitProjection } - public struct TwistLimitFunctions : IConstraintFunctions> + public struct TwistLimitFunctions : ITwoBodyConstraintFunctions> { [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, diff --git a/BepuPhysics/Constraints/TwistMotor.cs b/BepuPhysics/Constraints/TwistMotor.cs index b4240ce84..c96c7bd7d 100644 --- a/BepuPhysics/Constraints/TwistMotor.cs +++ b/BepuPhysics/Constraints/TwistMotor.cs @@ -84,7 +84,7 @@ public struct TwistMotorProjection } - public struct TwistMotorFunctions : IConstraintFunctions> + public struct TwistMotorFunctions : ITwoBodyConstraintFunctions> { [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, diff --git a/BepuPhysics/Constraints/TwistServo.cs b/BepuPhysics/Constraints/TwistServo.cs index 20461b123..4a7324db9 100644 --- a/BepuPhysics/Constraints/TwistServo.cs +++ b/BepuPhysics/Constraints/TwistServo.cs @@ -93,7 +93,7 @@ public struct TwistServoProjection } - public struct TwistServoFunctions : IConstraintFunctions> + public struct TwistServoFunctions : ITwoBodyConstraintFunctions> { [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ComputeJacobian(in QuaternionWide orientationA, in QuaternionWide orientationB, in QuaternionWide localBasisA, in QuaternionWide localBasisB, diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 4d21d997e..0799fc636 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -27,7 +27,7 @@ public struct TwoBodyReferences /// Type of the prestep data used by the constraint. /// Type of the accumulated impulses used by the constraint. /// Type of the projection to input. - public interface IConstraintFunctions + public interface ITwoBodyConstraintFunctions { void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref TPrestepData prestepData, out TProjection projection); void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref TProjection projection, ref TAccumulatedImpulse accumulatedImpulse); @@ -40,7 +40,7 @@ public interface IConstraintFunctionsType of the prestep data used by the constraint. /// Type of the accumulated impulses used by the constraint. /// Type of the projection to input. - public interface IContactConstraintFunctions : IConstraintFunctions + public interface IContactConstraintFunctions : ITwoBodyConstraintFunctions { void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityWide velocityA, in BodyVelocityWide velocityB, ref TPrestepData prestepData); } @@ -53,7 +53,7 @@ public interface IContactConstraintFunctions : TypeProcessor where TPrestepData : unmanaged where TProjection : unmanaged where TAccumulatedImpulse : unmanaged - where TConstraintFunctions : unmanaged, IConstraintFunctions + where TConstraintFunctions : unmanaged, ITwoBodyConstraintFunctions { protected sealed override int InternalBodiesPerConstraint => 2; diff --git a/BepuPhysics/Constraints/Weld.cs b/BepuPhysics/Constraints/Weld.cs index c108b8f29..a614cbbc0 100644 --- a/BepuPhysics/Constraints/Weld.cs +++ b/BepuPhysics/Constraints/Weld.cs @@ -91,7 +91,7 @@ public struct WeldAccumulatedImpulses public Vector3Wide Offset; } - public struct WeldFunctions : IConstraintFunctions + public struct WeldFunctions : ITwoBodyConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, diff --git a/Demos/Demos/Characters/CharacterMotionConstraint.cs b/Demos/Demos/Characters/CharacterMotionConstraint.cs index 4525cc950..5cd66ef65 100644 --- a/Demos/Demos/Characters/CharacterMotionConstraint.cs +++ b/Demos/Demos/Characters/CharacterMotionConstraint.cs @@ -439,7 +439,7 @@ public struct DynamicCharacterMotionProjection } - public struct DynamicCharacterMotionFunctions : IConstraintFunctions + public struct DynamicCharacterMotionFunctions : ITwoBodyConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] static void ComputeJacobians(in Vector3Wide offsetA, in Vector3Wide offsetB, in QuaternionWide basisQuaternion, From d7f93413011be6d153c936099a96dd835c831f5f Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 16 Jul 2021 20:46:56 -0500 Subject: [PATCH 116/947] First pass of access filters. Not yet doing anything with them. --- .../Constraints/AngularAxisGearMotor.cs | 2 +- BepuPhysics/Constraints/AngularAxisMotor.cs | 2 +- BepuPhysics/Constraints/AngularHinge.cs | 2 +- BepuPhysics/Constraints/AngularMotor.cs | 2 +- BepuPhysics/Constraints/AngularServo.cs | 2 +- BepuPhysics/Constraints/AngularSwivelHinge.cs | 2 +- BepuPhysics/Constraints/BallSocket.cs | 2 +- BepuPhysics/Constraints/BallSocketMotor.cs | 2 +- BepuPhysics/Constraints/BallSocketServo.cs | 2 +- .../Constraints/CenterDistanceConstraint.cs | 2 +- BepuPhysics/Constraints/DistanceLimit.cs | 2 +- BepuPhysics/Constraints/DistanceServo.cs | 2 +- BepuPhysics/Constraints/Hinge.cs | 2 +- BepuPhysics/Constraints/IBodyAccessFilter.cs | 67 +++++++++++++++++++ BepuPhysics/Constraints/LinearAxisLimit.cs | 2 +- BepuPhysics/Constraints/LinearAxisMotor.cs | 2 +- BepuPhysics/Constraints/LinearAxisServo.cs | 2 +- BepuPhysics/Constraints/PointOnLineServo.cs | 2 +- BepuPhysics/Constraints/SwingLimit.cs | 2 +- BepuPhysics/Constraints/SwivelHinge.cs | 2 +- BepuPhysics/Constraints/TwistLimit.cs | 2 +- BepuPhysics/Constraints/TwistMotor.cs | 2 +- BepuPhysics/Constraints/TwistServo.cs | 2 +- .../Constraints/TwoBodyTypeProcessor.cs | 16 +++-- BepuPhysics/Constraints/Weld.cs | 2 +- .../Characters/CharacterMotionConstraint.cs | 2 +- .../Characters/CharacterMotionConstraint.tt | 22 +++--- 27 files changed, 114 insertions(+), 39 deletions(-) create mode 100644 BepuPhysics/Constraints/IBodyAccessFilter.cs diff --git a/BepuPhysics/Constraints/AngularAxisGearMotor.cs b/BepuPhysics/Constraints/AngularAxisGearMotor.cs index b160abb62..af242e0e1 100644 --- a/BepuPhysics/Constraints/AngularAxisGearMotor.cs +++ b/BepuPhysics/Constraints/AngularAxisGearMotor.cs @@ -131,7 +131,7 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB } - public class AngularAxisGearMotorTypeProcessor : TwoBodyTypeProcessor, AngularAxisGearMotorFunctions> + public class AngularAxisGearMotorTypeProcessor : TwoBodyTypeProcessor, AngularAxisGearMotorFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular> { public const int BatchTypeId = 54; } diff --git a/BepuPhysics/Constraints/AngularAxisMotor.cs b/BepuPhysics/Constraints/AngularAxisMotor.cs index a1ab24c3f..8849bf6f1 100644 --- a/BepuPhysics/Constraints/AngularAxisMotor.cs +++ b/BepuPhysics/Constraints/AngularAxisMotor.cs @@ -127,7 +127,7 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB } - public class AngularAxisMotorTypeProcessor : TwoBodyTypeProcessor, AngularAxisMotorFunctions> + public class AngularAxisMotorTypeProcessor : TwoBodyTypeProcessor, AngularAxisMotorFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular> { public const int BatchTypeId = 41; } diff --git a/BepuPhysics/Constraints/AngularHinge.cs b/BepuPhysics/Constraints/AngularHinge.cs index 524bee043..4b8edacee 100644 --- a/BepuPhysics/Constraints/AngularHinge.cs +++ b/BepuPhysics/Constraints/AngularHinge.cs @@ -228,7 +228,7 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB } - public class AngularHingeTypeProcessor : TwoBodyTypeProcessor + public class AngularHingeTypeProcessor : TwoBodyTypeProcessor { public const int BatchTypeId = 23; } diff --git a/BepuPhysics/Constraints/AngularMotor.cs b/BepuPhysics/Constraints/AngularMotor.cs index 59affbe2d..45bd3eb2a 100644 --- a/BepuPhysics/Constraints/AngularMotor.cs +++ b/BepuPhysics/Constraints/AngularMotor.cs @@ -103,7 +103,7 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB } - public class AngularMotorTypeProcessor : TwoBodyTypeProcessor + public class AngularMotorTypeProcessor : TwoBodyTypeProcessor { public const int BatchTypeId = 30; } diff --git a/BepuPhysics/Constraints/AngularServo.cs b/BepuPhysics/Constraints/AngularServo.cs index 208833bf7..2c8e63693 100644 --- a/BepuPhysics/Constraints/AngularServo.cs +++ b/BepuPhysics/Constraints/AngularServo.cs @@ -144,7 +144,7 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB } - public class AngularServoTypeProcessor : TwoBodyTypeProcessor + public class AngularServoTypeProcessor : TwoBodyTypeProcessor { public const int BatchTypeId = 29; } diff --git a/BepuPhysics/Constraints/AngularSwivelHinge.cs b/BepuPhysics/Constraints/AngularSwivelHinge.cs index 7e9557dc8..4314934fb 100644 --- a/BepuPhysics/Constraints/AngularSwivelHinge.cs +++ b/BepuPhysics/Constraints/AngularSwivelHinge.cs @@ -156,7 +156,7 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB } - public class AngularSwivelHingeTypeProcessor : TwoBodyTypeProcessor, AngularSwivelHingeFunctions> + public class AngularSwivelHingeTypeProcessor : TwoBodyTypeProcessor, AngularSwivelHingeFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular> { public const int BatchTypeId = 24; } diff --git a/BepuPhysics/Constraints/BallSocket.cs b/BepuPhysics/Constraints/BallSocket.cs index 30c1e14f3..0b42e8cc7 100644 --- a/BepuPhysics/Constraints/BallSocket.cs +++ b/BepuPhysics/Constraints/BallSocket.cs @@ -141,7 +141,7 @@ public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, /// /// Handles the solve iterations of a bunch of ball socket constraints. /// - public class BallSocketTypeProcessor : TwoBodyTypeProcessor + public class BallSocketTypeProcessor : TwoBodyTypeProcessor { public const int BatchTypeId = 22; } diff --git a/BepuPhysics/Constraints/BallSocketMotor.cs b/BepuPhysics/Constraints/BallSocketMotor.cs index cb17fe5c4..7fd0e477b 100644 --- a/BepuPhysics/Constraints/BallSocketMotor.cs +++ b/BepuPhysics/Constraints/BallSocketMotor.cs @@ -117,7 +117,7 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB /// /// Handles the solve iterations of a bunch of ball socket motor constraints. /// - public class BallSocketMotorTypeProcessor : TwoBodyTypeProcessor + public class BallSocketMotorTypeProcessor : TwoBodyTypeProcessor { public const int BatchTypeId = 52; } diff --git a/BepuPhysics/Constraints/BallSocketServo.cs b/BepuPhysics/Constraints/BallSocketServo.cs index 422f8fc8d..536eb4602 100644 --- a/BepuPhysics/Constraints/BallSocketServo.cs +++ b/BepuPhysics/Constraints/BallSocketServo.cs @@ -123,7 +123,7 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB /// /// Handles the solve iterations of a bunch of ball socket servo constraints. /// - public class BallSocketServoTypeProcessor : TwoBodyTypeProcessor + public class BallSocketServoTypeProcessor : TwoBodyTypeProcessor { public const int BatchTypeId = 53; } diff --git a/BepuPhysics/Constraints/CenterDistanceConstraint.cs b/BepuPhysics/Constraints/CenterDistanceConstraint.cs index f44652bf3..1897afaab 100644 --- a/BepuPhysics/Constraints/CenterDistanceConstraint.cs +++ b/BepuPhysics/Constraints/CenterDistanceConstraint.cs @@ -133,7 +133,7 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB /// /// Handles the solve iterations of a bunch of distance servos. /// - public class CenterDistanceTypeProcessor : TwoBodyTypeProcessor, CenterDistanceConstraintFunctions> + public class CenterDistanceTypeProcessor : TwoBodyTypeProcessor, CenterDistanceConstraintFunctions, AccessOnlyLinear, AccessOnlyLinear, AccessOnlyLinear, AccessOnlyLinear> { public const int BatchTypeId = 35; } diff --git a/BepuPhysics/Constraints/DistanceLimit.cs b/BepuPhysics/Constraints/DistanceLimit.cs index 9983f9367..86f7d8965 100644 --- a/BepuPhysics/Constraints/DistanceLimit.cs +++ b/BepuPhysics/Constraints/DistanceLimit.cs @@ -165,7 +165,7 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB /// /// Handles the solve iterations of a bunch of distance servos. /// - public class DistanceLimitTypeProcessor : TwoBodyTypeProcessor, DistanceLimitFunctions> + public class DistanceLimitTypeProcessor : TwoBodyTypeProcessor, DistanceLimitFunctions, AccessAll, AccessAll, AccessAll, AccessAll> { public const int BatchTypeId = 34; } diff --git a/BepuPhysics/Constraints/DistanceServo.cs b/BepuPhysics/Constraints/DistanceServo.cs index ebfa3394b..1a1ca089f 100644 --- a/BepuPhysics/Constraints/DistanceServo.cs +++ b/BepuPhysics/Constraints/DistanceServo.cs @@ -247,7 +247,7 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB /// /// Handles the solve iterations of a bunch of distance servos. /// - public class DistanceServoTypeProcessor : TwoBodyTypeProcessor, DistanceServoFunctions> + public class DistanceServoTypeProcessor : TwoBodyTypeProcessor, DistanceServoFunctions, AccessAll, AccessAll, AccessAll, AccessAll> { public const int BatchTypeId = 33; } diff --git a/BepuPhysics/Constraints/Hinge.cs b/BepuPhysics/Constraints/Hinge.cs index e27fdd1a8..8dc37a01f 100644 --- a/BepuPhysics/Constraints/Hinge.cs +++ b/BepuPhysics/Constraints/Hinge.cs @@ -226,7 +226,7 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB } - public class HingeTypeProcessor : TwoBodyTypeProcessor + public class HingeTypeProcessor : TwoBodyTypeProcessor { public const int BatchTypeId = 47; } diff --git a/BepuPhysics/Constraints/IBodyAccessFilter.cs b/BepuPhysics/Constraints/IBodyAccessFilter.cs new file mode 100644 index 000000000..0ed33da11 --- /dev/null +++ b/BepuPhysics/Constraints/IBodyAccessFilter.cs @@ -0,0 +1,67 @@ +namespace BepuPhysics.Constraints +{ + /// + /// Constrains which body properties should be accessed in a body during constraint data gathering/scattering. + /// + public interface IBodyAccessFilter + { + /// + /// Gets whether position is loaded by the constraint. + /// + public bool GatherPosition { get; } + /// + /// Gets whether orientation is loaded by the constraint. + /// + public bool GatherOrientation { get; } + /// + /// Gets whether body mass is loaded by this constraint. + /// + public bool GatherMass { get; } + /// + /// Gets whether body inertia tensor is loaded by this constraint. + /// + public bool GatherInertiaTensor { get; } + /// + /// Gets whether to load or store body linear velocity in this constraint. + /// + public bool AccessLinearVelocity { get; } + /// + /// Gets whether to load or store body linear velocity in this constraint. + /// + public bool AccessAngularVelocity { get; } + } + + + /// + /// Marks all body properties as necessary for gather/scatter. + /// + public struct AccessAll : IBodyAccessFilter + { + public bool GatherPosition => true; + public bool GatherOrientation => true; + public bool GatherMass => true; + public bool GatherInertiaTensor => true; + public bool AccessLinearVelocity => true; + public bool AccessAngularVelocity => true; + } + + public struct AccessOnlyAngular : IBodyAccessFilter + { + public bool GatherPosition => false; + public bool GatherOrientation => true; + public bool GatherMass => false; + public bool GatherInertiaTensor => true; + public bool AccessLinearVelocity => false; + public bool AccessAngularVelocity => true; + } + + public struct AccessOnlyLinear : IBodyAccessFilter + { + public bool GatherPosition => true; + public bool GatherOrientation => false; + public bool GatherMass => true; + public bool GatherInertiaTensor => false; + public bool AccessLinearVelocity => true; + public bool AccessAngularVelocity => false; + } +} diff --git a/BepuPhysics/Constraints/LinearAxisLimit.cs b/BepuPhysics/Constraints/LinearAxisLimit.cs index 84b2fb60b..1510b0d6c 100644 --- a/BepuPhysics/Constraints/LinearAxisLimit.cs +++ b/BepuPhysics/Constraints/LinearAxisLimit.cs @@ -151,7 +151,7 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB } - public class LinearAxisLimitTypeProcessor : TwoBodyTypeProcessor, LinearAxisLimitFunctions> + public class LinearAxisLimitTypeProcessor : TwoBodyTypeProcessor, LinearAxisLimitFunctions, AccessAll, AccessAll, AccessAll, AccessAll> { public const int BatchTypeId = 40; } diff --git a/BepuPhysics/Constraints/LinearAxisMotor.cs b/BepuPhysics/Constraints/LinearAxisMotor.cs index 3ebbafd13..db5f7d5b0 100644 --- a/BepuPhysics/Constraints/LinearAxisMotor.cs +++ b/BepuPhysics/Constraints/LinearAxisMotor.cs @@ -116,7 +116,7 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB } - public class LinearAxisMotorTypeProcessor : TwoBodyTypeProcessor, LinearAxisMotorFunctions> + public class LinearAxisMotorTypeProcessor : TwoBodyTypeProcessor, LinearAxisMotorFunctions, AccessAll, AccessAll, AccessAll, AccessAll> { public const int BatchTypeId = 39; } diff --git a/BepuPhysics/Constraints/LinearAxisServo.cs b/BepuPhysics/Constraints/LinearAxisServo.cs index 2dc8c6470..e1d34513c 100644 --- a/BepuPhysics/Constraints/LinearAxisServo.cs +++ b/BepuPhysics/Constraints/LinearAxisServo.cs @@ -229,7 +229,7 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB } - public class LinearAxisServoTypeProcessor : TwoBodyTypeProcessor, LinearAxisServoFunctions> + public class LinearAxisServoTypeProcessor : TwoBodyTypeProcessor, LinearAxisServoFunctions, AccessAll, AccessAll, AccessAll, AccessAll> { public const int BatchTypeId = 38; } diff --git a/BepuPhysics/Constraints/PointOnLineServo.cs b/BepuPhysics/Constraints/PointOnLineServo.cs index 5f1803008..cf6ec6dff 100644 --- a/BepuPhysics/Constraints/PointOnLineServo.cs +++ b/BepuPhysics/Constraints/PointOnLineServo.cs @@ -223,7 +223,7 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB } - public class PointOnLineServoTypeProcessor : TwoBodyTypeProcessor + public class PointOnLineServoTypeProcessor : TwoBodyTypeProcessor { public const int BatchTypeId = 37; } diff --git a/BepuPhysics/Constraints/SwingLimit.cs b/BepuPhysics/Constraints/SwingLimit.cs index 872357211..7f491daf9 100644 --- a/BepuPhysics/Constraints/SwingLimit.cs +++ b/BepuPhysics/Constraints/SwingLimit.cs @@ -181,7 +181,7 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB } - public class SwingLimitTypeProcessor : TwoBodyTypeProcessor, SwingLimitFunctions> + public class SwingLimitTypeProcessor : TwoBodyTypeProcessor, SwingLimitFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular> { public const int BatchTypeId = 25; } diff --git a/BepuPhysics/Constraints/SwivelHinge.cs b/BepuPhysics/Constraints/SwivelHinge.cs index 6a327f8cb..da106d2a6 100644 --- a/BepuPhysics/Constraints/SwivelHinge.cs +++ b/BepuPhysics/Constraints/SwivelHinge.cs @@ -213,7 +213,7 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB } - public class SwivelHingeTypeProcessor : TwoBodyTypeProcessor + public class SwivelHingeTypeProcessor : TwoBodyTypeProcessor { public const int BatchTypeId = 46; } diff --git a/BepuPhysics/Constraints/TwistLimit.cs b/BepuPhysics/Constraints/TwistLimit.cs index 759e5dd95..21e8219c8 100644 --- a/BepuPhysics/Constraints/TwistLimit.cs +++ b/BepuPhysics/Constraints/TwistLimit.cs @@ -147,7 +147,7 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB } - public class TwistLimitTypeProcessor : TwoBodyTypeProcessor, TwistLimitFunctions> + public class TwistLimitTypeProcessor : TwoBodyTypeProcessor, TwistLimitFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular> { public const int BatchTypeId = 27; } diff --git a/BepuPhysics/Constraints/TwistMotor.cs b/BepuPhysics/Constraints/TwistMotor.cs index c96c7bd7d..30e1b5a05 100644 --- a/BepuPhysics/Constraints/TwistMotor.cs +++ b/BepuPhysics/Constraints/TwistMotor.cs @@ -132,7 +132,7 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB } - public class TwistMotorTypeProcessor : TwoBodyTypeProcessor, TwistMotorFunctions> + public class TwistMotorTypeProcessor : TwoBodyTypeProcessor, TwistMotorFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular> { public const int BatchTypeId = 28; } diff --git a/BepuPhysics/Constraints/TwistServo.cs b/BepuPhysics/Constraints/TwistServo.cs index 4a7324db9..f093b4033 100644 --- a/BepuPhysics/Constraints/TwistServo.cs +++ b/BepuPhysics/Constraints/TwistServo.cs @@ -217,7 +217,7 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB } - public class TwistServoTypeProcessor : TwoBodyTypeProcessor, TwistServoFunctions> + public class TwistServoTypeProcessor : TwoBodyTypeProcessor, TwistServoFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular> { public const int BatchTypeId = 26; } diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 0799fc636..d46e04a48 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -50,10 +50,15 @@ public interface IContactConstraintFunctions /// Shared implementation across all two body constraints. /// - public abstract class TwoBodyTypeProcessor + public abstract class TwoBodyTypeProcessor : TypeProcessor where TPrestepData : unmanaged where TProjection : unmanaged where TAccumulatedImpulse : unmanaged where TConstraintFunctions : unmanaged, ITwoBodyConstraintFunctions + where TWarmStartAccessFilterA : unmanaged, IBodyAccessFilter + where TWarmStartAccessFilterB : unmanaged, IBodyAccessFilter + where TSolveAccessFilterA : unmanaged, IBodyAccessFilter + where TSolveAccessFilterB : unmanaged, IBodyAccessFilter { protected sealed override int InternalBodiesPerConstraint => 2; @@ -301,12 +306,15 @@ public unsafe override void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodi } + const int warmStartPrefetchDistance = 8; + const int solvePrefetchDistance = 4; [MethodImpl(MethodImplOptions.AggressiveInlining)] static unsafe void Prefetch(void* address) { if (Sse.IsSupported) { Sse.Prefetch0(address); + //Sse.Prefetch0((byte*)address + 64); //TODO: prefetch should grab cache line pair anyway, right? not much reason to explicitly do more? } //TODO: ARM? @@ -325,9 +333,8 @@ static unsafe void PrefetchBundle(SolverState* stateBase, ref TwoBodyReferences } } - const int warmStartPrefetchDistance = 8; - const int solvePrefetchDistance = 4; [MethodImpl(MethodImplOptions.AggressiveInlining)] + [Conditional("PREFETCH")] unsafe static void EarlyPrefetch(int prefetchDistance, ref TypeBatch typeBatch, ref Buffer references, ref Buffer states, int startBundleIndex, int exclusiveEndBundleIndex) { exclusiveEndBundleIndex = Math.Min(exclusiveEndBundleIndex, startBundleIndex + prefetchDistance); @@ -341,6 +348,7 @@ unsafe static void EarlyPrefetch(int prefetchDistance, ref TypeBatch typeBatch, } [MethodImpl(MethodImplOptions.AggressiveInlining)] + [Conditional("PREFETCH")] unsafe static void Prefetch(int prefetchDistance, ref TypeBatch typeBatch, ref Buffer references, ref Buffer states, int bundleIndex, int exclusiveEndBundleIndex) { var targetIndex = bundleIndex + prefetchDistance; @@ -646,7 +654,7 @@ public override void JacobiPrestep2(ref TypeBatch typeBatch, Bodies bodies, ref } public abstract class TwoBodyContactTypeProcessor - : TwoBodyTypeProcessor + : TwoBodyTypeProcessor where TPrestepData : unmanaged where TProjection : unmanaged where TAccumulatedImpulse : unmanaged where TConstraintFunctions : unmanaged, IContactConstraintFunctions { diff --git a/BepuPhysics/Constraints/Weld.cs b/BepuPhysics/Constraints/Weld.cs index a614cbbc0..bccc85fbf 100644 --- a/BepuPhysics/Constraints/Weld.cs +++ b/BepuPhysics/Constraints/Weld.cs @@ -366,7 +366,7 @@ public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, /// /// Handles the solve iterations of a bunch of ball socket constraints. /// - public class WeldTypeProcessor : TwoBodyTypeProcessor + public class WeldTypeProcessor : TwoBodyTypeProcessor { public const int BatchTypeId = 31; } diff --git a/Demos/Demos/Characters/CharacterMotionConstraint.cs b/Demos/Demos/Characters/CharacterMotionConstraint.cs index 5cd66ef65..d2cda1838 100644 --- a/Demos/Demos/Characters/CharacterMotionConstraint.cs +++ b/Demos/Demos/Characters/CharacterMotionConstraint.cs @@ -642,7 +642,7 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB //Each constraint type has its own 'type processor'- it acts as the outer loop that handles all the common logic across batches of constraints and invokes //the per-constraint logic as needed. The CharacterMotionFunctions type provides the actual implementation. - public class DynamicCharacterMotionTypeProcessor : TwoBodyTypeProcessor + public class DynamicCharacterMotionTypeProcessor : TwoBodyTypeProcessor { /// /// Simulation-wide unique id for the character motion constraint. Every type has needs a unique compile time id; this is a little bit annoying to guarantee given that there is no central diff --git a/Demos/Demos/Characters/CharacterMotionConstraint.tt b/Demos/Demos/Characters/CharacterMotionConstraint.tt index c724350e2..8335557e8 100644 --- a/Demos/Demos/Characters/CharacterMotionConstraint.tt +++ b/Demos/Demos/Characters/CharacterMotionConstraint.tt @@ -156,9 +156,9 @@ namespace Demos.Demos.Characters public Vector2Wide TargetVelocity; public Symmetric2x2Wide HorizontalEffectiveMass; public Vector MaximumHorizontalImpulse; - public BodyInertias InertiaA; + public BodyInertiaWide InertiaA; <#if(dynamic) {#> - public BodyInertias InertiaB; + public BodyInertiaWide InertiaB; <#}#> public Vector VerticalBiasVelocity; public Vector VerticalEffectiveMass; @@ -166,7 +166,7 @@ namespace Demos.Demos.Characters } - public struct <#=prefix#>CharacterMotionFunctions : I<#if(!dynamic) { Write("OneBody"); }#>ConstraintFunctions<<#=prefix#>CharacterMotionPrestep, <#=prefix#>CharacterMotionProjection, CharacterMotionAccumulatedImpulse> + public struct <#=prefix#>CharacterMotionFunctions : I<#if(dynamic) { Write("TwoBody"); } else { Write("OneBody"); }#>ConstraintFunctions<<#=prefix#>CharacterMotionPrestep, <#=prefix#>CharacterMotionProjection, CharacterMotionAccumulatedImpulse> { [MethodImpl(MethodImplOptions.AggressiveInlining)] static void ComputeJacobians(in Vector3Wide offsetA, <#if (dynamic) {#>in Vector3Wide offsetB, <#}#>in QuaternionWide basisQuaternion, @@ -205,7 +205,7 @@ namespace Demos.Demos.Characters [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Prestep( - <#if(!dynamic) {#>in Vector3Wide positionA,<#}#> in QuaternionWide orientationA, in BodyInertias inertiaA, <#if(dynamic) {#>in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, <#}#>float dt, float inverseDt, ref <#=prefix#>CharacterMotionPrestep prestepData, out <#=prefix#>CharacterMotionProjection projection) + <#if(!dynamic) {#>in Vector3Wide positionA,<#}#> in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(dynamic) {#>in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>float dt, float inverseDt, ref <#=prefix#>CharacterMotionPrestep prestepData, out <#=prefix#>CharacterMotionProjection projection) { //The motion constraint is split into two parts: the horizontal constraint, and the vertical constraint. //The horizontal constraint acts almost exactly like the TangentFriction, but we'll duplicate some of the logic to keep this implementation self-contained. @@ -279,9 +279,9 @@ namespace Demos.Demos.Characters [MethodImpl(MethodImplOptions.AggressiveInlining)] private static void ApplyHorizontalImpulse(in Matrix3x3Wide basis, in Matrix2x3Wide angularJacobianA, <#if (dynamic) {#>in Matrix2x3Wide angularJacobianB, <#}#>in Vector2Wide constraintSpaceImpulse, - in BodyInertias inertiaA, <#if (dynamic) {#>in BodyInertias inertiaB,<#}#> + in BodyInertiaWide inertiaA, <#if (dynamic) {#>in BodyInertiaWide inertiaB,<#}#> - ref BodyVelocities velocityA<#if (dynamic) {#>, ref BodyVelocities velocityB<#}#>) + ref BodyVelocityWide velocityA<#if (dynamic) {#>, ref BodyVelocityWide velocityB<#}#>) { //Transform the constraint space impulse into world space by using the jacobian and then apply each body's inverse inertia to get the velocity change. Vector3Wide.Scale(basis.X, constraintSpaceImpulse.X, out var linearImpulseAX); @@ -307,9 +307,9 @@ namespace Demos.Demos.Characters [MethodImpl(MethodImplOptions.AggressiveInlining)] private static void ApplyVerticalImpulse(in Matrix3x3Wide basis, in Vector3Wide angularJacobianA, <#if (dynamic) {#>in Vector3Wide angularJacobianB, <#}#>in Vector constraintSpaceImpulse, - in BodyInertias inertiaA, <#if (dynamic) {#>in BodyInertias inertiaB,<#}#> + in BodyInertiaWide inertiaA, <#if (dynamic) {#>in BodyInertiaWide inertiaB,<#}#> - ref BodyVelocities velocityA<#if (dynamic) {#>, ref BodyVelocities velocityB<#}#>) + ref BodyVelocityWide velocityA<#if (dynamic) {#>, ref BodyVelocityWide velocityB<#}#>) { Vector3Wide.Scale(basis.Y, constraintSpaceImpulse, out var linearImpulseA); Vector3Wide.Scale(linearImpulseA, inertiaA.InverseMass, out var linearChangeA); @@ -330,7 +330,7 @@ namespace Demos.Demos.Characters } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocities velocityA, <#if (dynamic) {#>ref BodyVelocities velocityB, <#}#>ref <#=prefix#>CharacterMotionProjection projection, ref CharacterMotionAccumulatedImpulse accumulatedImpulse) + public void WarmStart(ref BodyVelocityWide velocityA, <#if (dynamic) {#>ref BodyVelocityWide velocityB, <#}#>ref <#=prefix#>CharacterMotionProjection projection, ref CharacterMotionAccumulatedImpulse accumulatedImpulse) { ComputeJacobians(projection.OffsetFromCharacter, <#if (dynamic) {#>projection.OffsetFromSupport, <#}#>projection.SurfaceBasis, out var basis, out var horizontalAngularJacobianA, <#if (dynamic) {#>out var horizontalAngularJacobianB, <#}#>out var verticalAngularJacobianA<#if (dynamic) {#>, out var verticalAngularJacobianB<#}#>); @@ -339,7 +339,7 @@ namespace Demos.Demos.Characters } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocities velocityA, <#if (dynamic) {#>ref BodyVelocities velocityB, <#}#>ref <#=prefix#>CharacterMotionProjection projection, ref CharacterMotionAccumulatedImpulse accumulatedImpulse) + public void Solve(ref BodyVelocityWide velocityA, <#if (dynamic) {#>ref BodyVelocityWide velocityB, <#}#>ref <#=prefix#>CharacterMotionProjection projection, ref CharacterMotionAccumulatedImpulse accumulatedImpulse) { ComputeJacobians(projection.OffsetFromCharacter, <#if (dynamic) {#>projection.OffsetFromSupport, <#}#>projection.SurfaceBasis, out var basis, out var horizontalAngularJacobianA, <#if (dynamic) {#>out var horizontalAngularJacobianB, <#}#>out var verticalAngularJacobianA<#if (dynamic) {#>, out var verticalAngularJacobianB<#}#>); @@ -397,7 +397,7 @@ namespace Demos.Demos.Characters //Each constraint type has its own 'type processor'- it acts as the outer loop that handles all the common logic across batches of constraints and invokes //the per-constraint logic as needed. The CharacterMotionFunctions type provides the actual implementation. - public class <#=prefix#>CharacterMotionTypeProcessor : <#=dynamic ? "Two" : "One"#>BodyTypeProcessor<<#=prefix#>CharacterMotionPrestep, <#=prefix#>CharacterMotionProjection, CharacterMotionAccumulatedImpulse, <#=prefix#>CharacterMotionFunctions> + public class <#=prefix#>CharacterMotionTypeProcessor : <#=dynamic ? "Two" : "One"#>BodyTypeProcessor<<#=prefix#>CharacterMotionPrestep, <#=prefix#>CharacterMotionProjection, CharacterMotionAccumulatedImpulse, <#=prefix#>CharacterMotionFunctions<#=dynamic ? (", AccessAll, AccessAll, AccessAll, AccessAll") : ""#>> { /// /// Simulation-wide unique id for the character motion constraint. Every type has needs a unique compile time id; this is a little bit annoying to guarantee given that there is no central From 91e914c6f41bc1c0ddf50a88ce369477d89c1984 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 17 Jul 2021 19:50:12 -0500 Subject: [PATCH 117/947] Added gather/scatter access filtering. Added warmstart2/solve2 paths for centerdistanceconstraint. --- BepuPhysics/Bodies_GatherScatter.cs | 306 +++++++++++++----- .../Constraints/CenterDistanceConstraint.cs | 51 +++ .../Constraints/TwoBodyTypeProcessor.cs | 44 ++- BepuPhysics/Constraints/TypeProcessor.cs | 2 +- BepuPhysics/Constraints/Weld.cs | 23 +- BepuUtilities/MathHelper.cs | 62 +++- 6 files changed, 374 insertions(+), 114 deletions(-) diff --git a/BepuPhysics/Bodies_GatherScatter.cs b/BepuPhysics/Bodies_GatherScatter.cs index b6380b8dd..64907e55a 100644 --- a/BepuPhysics/Bodies_GatherScatter.cs +++ b/BepuPhysics/Bodies_GatherScatter.cs @@ -136,9 +136,11 @@ unsafe static void FallbackGatherInertia(int count, SolverState* states, ref Vec } //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void GatherMotionState(ref Vector bodyIndices, int count, out Vector3Wide position, out QuaternionWide orientation, out BodyVelocityWide velocity) + public unsafe void GatherState(ref Vector bodyIndices, int count, bool worldInertia, out Vector3Wide position, out QuaternionWide orientation, out BodyVelocityWide velocity, out BodyInertiaWide inertia) + where TAccessFilter : unmanaged, IBodyAccessFilter { var solverStates = ActiveSet.SolverStates.Memory; + Unsafe.SkipInit(out TAccessFilter filter); if (Avx.IsSupported && Vector.Count == 8) { var indices = (int*)Unsafe.AsPointer(ref bodyIndices); @@ -194,13 +196,27 @@ public unsafe void GatherMotionState(ref Vector bodyIndices, int count, out var o6 = Avx.Shuffle(n4, n5, 2 | (3 << 2) | (2 << 4) | (3 << 6)); var o7 = Avx.Shuffle(n6, n7, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - orientation.X = Avx.Permute2x128(o0, o1, 0 | (2 << 4)).AsVector(); - orientation.Y = Avx.Permute2x128(o4, o5, 0 | (2 << 4)).AsVector(); - orientation.Z = Avx.Permute2x128(o2, o3, 0 | (2 << 4)).AsVector(); - orientation.W = Avx.Permute2x128(o6, o7, 0 | (2 << 4)).AsVector(); - position.X = Avx.Permute2x128(o0, o1, 1 | (3 << 4)).AsVector(); - position.Y = Avx.Permute2x128(o4, o5, 1 | (3 << 4)).AsVector(); - position.Z = Avx.Permute2x128(o2, o3, 1 | (3 << 4)).AsVector(); + if (filter.GatherOrientation) + { + orientation.X = Avx.Permute2x128(o0, o1, 0 | (2 << 4)).AsVector(); + orientation.Y = Avx.Permute2x128(o4, o5, 0 | (2 << 4)).AsVector(); + orientation.Z = Avx.Permute2x128(o2, o3, 0 | (2 << 4)).AsVector(); + orientation.W = Avx.Permute2x128(o6, o7, 0 | (2 << 4)).AsVector(); + } + else + { + Unsafe.SkipInit(out orientation); + } + if (filter.GatherPosition) + { + position.X = Avx.Permute2x128(o0, o1, 1 | (3 << 4)).AsVector(); + position.Y = Avx.Permute2x128(o4, o5, 1 | (3 << 4)).AsVector(); + position.Z = Avx.Permute2x128(o2, o3, 1 | (3 << 4)).AsVector(); + } + else + { + Unsafe.SkipInit(out position); + } } { @@ -232,51 +248,40 @@ public unsafe void GatherMotionState(ref Vector bodyIndices, int count, out var o6 = Avx.Shuffle(n4, n5, 2 | (3 << 2) | (2 << 4) | (3 << 6)); var o7 = Avx.Shuffle(n6, n7, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - velocity.Linear.X = Avx.Permute2x128(o0, o1, 0 | (2 << 4)).AsVector(); - velocity.Linear.Y = Avx.Permute2x128(o4, o5, 0 | (2 << 4)).AsVector(); - velocity.Linear.Z = Avx.Permute2x128(o2, o3, 0 | (2 << 4)).AsVector(); - velocity.Angular.X = Avx.Permute2x128(o0, o1, 1 | (3 << 4)).AsVector(); - velocity.Angular.Y = Avx.Permute2x128(o4, o5, 1 | (3 << 4)).AsVector(); - velocity.Angular.Z = Avx.Permute2x128(o2, o3, 1 | (3 << 4)).AsVector(); + if (filter.AccessLinearVelocity) + { + velocity.Linear.X = Avx.Permute2x128(o0, o1, 0 | (2 << 4)).AsVector(); + velocity.Linear.Y = Avx.Permute2x128(o4, o5, 0 | (2 << 4)).AsVector(); + velocity.Linear.Z = Avx.Permute2x128(o2, o3, 0 | (2 << 4)).AsVector(); + } + else + { + Unsafe.SkipInit(out velocity.Linear); + } + if (filter.AccessAngularVelocity) + { + velocity.Angular.X = Avx.Permute2x128(o0, o1, 1 | (3 << 4)).AsVector(); + velocity.Angular.Y = Avx.Permute2x128(o4, o5, 1 | (3 << 4)).AsVector(); + velocity.Angular.Z = Avx.Permute2x128(o2, o3, 1 | (3 << 4)).AsVector(); + } + else + { + Unsafe.SkipInit(out velocity.Angular); + } } - } - else - { - Unsafe.SkipInit(out position); - Unsafe.SkipInit(out orientation); - Unsafe.SkipInit(out velocity); - FallbackGatherMotionState(count, solverStates, ref bodyIndices, ref position, ref orientation, ref velocity); - } - } - - - //[MethodImpl(MethodImplOptions.AggressiveInlining)] - unsafe void GatherInertia(ref Vector bodyIndices, int count, int offsetInFloats, out BodyInertiaWide inertia) - { - var inertias = ActiveSet.SolverStates.Memory; - if (Avx.IsSupported && Vector.Count == 8) - { - var indices = (int*)Unsafe.AsPointer(ref bodyIndices); { - var s0 = (float*)(inertias + indices[0]) + offsetInFloats; - var s1 = (float*)(inertias + indices[1]) + offsetInFloats; - var s2 = (float*)(inertias + indices[2]) + offsetInFloats; - var s3 = (float*)(inertias + indices[3]) + offsetInFloats; - var s4 = (float*)(inertias + indices[4]) + offsetInFloats; - var s5 = (float*)(inertias + indices[5]) + offsetInFloats; - var s6 = (float*)(inertias + indices[6]) + offsetInFloats; - var s7 = (float*)(inertias + indices[7]) + offsetInFloats; + var offsetInFloats = worldInertia ? 24 : 16; //Load every inertia vector. - var m0 = Avx.LoadAlignedVector256(s0); - var m1 = count > 1 ? Avx.LoadAlignedVector256(s1) : Vector256.Zero; - var m2 = count > 2 ? Avx.LoadAlignedVector256(s2) : Vector256.Zero; - var m3 = count > 3 ? Avx.LoadAlignedVector256(s3) : Vector256.Zero; - var m4 = count > 4 ? Avx.LoadAlignedVector256(s4) : Vector256.Zero; - var m5 = count > 5 ? Avx.LoadAlignedVector256(s5) : Vector256.Zero; - var m6 = count > 6 ? Avx.LoadAlignedVector256(s6) : Vector256.Zero; - var m7 = count > 7 ? Avx.LoadAlignedVector256(s7) : Vector256.Zero; + var m0 = Avx.LoadAlignedVector256(s0 + offsetInFloats); + var m1 = count > 1 ? Avx.LoadAlignedVector256(s1 + offsetInFloats) : Vector256.Zero; + var m2 = count > 2 ? Avx.LoadAlignedVector256(s2 + offsetInFloats) : Vector256.Zero; + var m3 = count > 3 ? Avx.LoadAlignedVector256(s3 + offsetInFloats) : Vector256.Zero; + var m4 = count > 4 ? Avx.LoadAlignedVector256(s4 + offsetInFloats) : Vector256.Zero; + var m5 = count > 5 ? Avx.LoadAlignedVector256(s5 + offsetInFloats) : Vector256.Zero; + var m6 = count > 6 ? Avx.LoadAlignedVector256(s6 + offsetInFloats) : Vector256.Zero; + var m7 = count > 7 ? Avx.LoadAlignedVector256(s7 + offsetInFloats) : Vector256.Zero; var n0 = Avx.UnpackLow(m0, m1); var n1 = Avx.UnpackLow(m2, m3); @@ -296,32 +301,128 @@ unsafe void GatherInertia(ref Vector bodyIndices, int count, int offsetInFl var o6 = Avx.Shuffle(n4, n5, 2 | (3 << 2) | (2 << 4) | (3 << 6)); var o7 = Avx.Shuffle(n6, n7, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - inertia.InverseInertiaTensor.XX = Avx.Permute2x128(o0, o1, 0 | (2 << 4)).AsVector(); - inertia.InverseInertiaTensor.YX = Avx.Permute2x128(o4, o5, 0 | (2 << 4)).AsVector(); - inertia.InverseInertiaTensor.YY = Avx.Permute2x128(o2, o3, 0 | (2 << 4)).AsVector(); - inertia.InverseInertiaTensor.ZX = Avx.Permute2x128(o6, o7, 0 | (2 << 4)).AsVector(); - inertia.InverseInertiaTensor.ZY = Avx.Permute2x128(o0, o1, 1 | (3 << 4)).AsVector(); - inertia.InverseInertiaTensor.ZZ = Avx.Permute2x128(o4, o5, 1 | (3 << 4)).AsVector(); - inertia.InverseMass = Avx.Permute2x128(o2, o3, 1 | (3 << 4)).AsVector(); + if (filter.GatherInertiaTensor) + { + inertia.InverseInertiaTensor.XX = Avx.Permute2x128(o0, o1, 0 | (2 << 4)).AsVector(); + inertia.InverseInertiaTensor.YX = Avx.Permute2x128(o4, o5, 0 | (2 << 4)).AsVector(); + inertia.InverseInertiaTensor.YY = Avx.Permute2x128(o2, o3, 0 | (2 << 4)).AsVector(); + inertia.InverseInertiaTensor.ZX = Avx.Permute2x128(o6, o7, 0 | (2 << 4)).AsVector(); + inertia.InverseInertiaTensor.ZY = Avx.Permute2x128(o0, o1, 1 | (3 << 4)).AsVector(); + inertia.InverseInertiaTensor.ZZ = Avx.Permute2x128(o4, o5, 1 | (3 << 4)).AsVector(); + } + else + { + Unsafe.SkipInit(out inertia.InverseInertiaTensor); + } + if (filter.GatherMass) + { + inertia.InverseMass = Avx.Permute2x128(o2, o3, 1 | (3 << 4)).AsVector(); + } + else + { + Unsafe.SkipInit(out inertia.InverseMass); + } } + } else { + Unsafe.SkipInit(out position); + Unsafe.SkipInit(out orientation); + Unsafe.SkipInit(out velocity); Unsafe.SkipInit(out inertia); - FallbackGatherInertia(count, ActiveSet.SolverStates.Memory, ref bodyIndices, ref inertia, offsetInFloats); + FallbackGatherMotionState(count, solverStates, ref bodyIndices, ref position, ref orientation, ref velocity); + FallbackGatherInertia(count, solverStates, ref bodyIndices, ref inertia, worldInertia ? 24 : 16); } } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void GatherWorldInertia(ref Vector bodyIndices, int count, out BodyInertiaWide inertia) - { - GatherInertia(ref bodyIndices, count, 24, out inertia); - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void GatherLocalInertia(ref Vector bodyIndices, int count, out BodyInertiaWide inertia) - { - GatherInertia(ref bodyIndices, count, 16, out inertia); - } + + ////[MethodImpl(MethodImplOptions.AggressiveInlining)] + //unsafe void GatherInertia(ref Vector bodyIndices, int count, int offsetInFloats, out BodyInertiaWide inertia) where TAccessFilter : unmanaged, IBodyAccessFilter + //{ + // var inertias = ActiveSet.SolverStates.Memory; + // if (Avx.IsSupported && Vector.Count == 8) + // { + // Unsafe.SkipInit(out TAccessFilter filter); + // var indices = (int*)Unsafe.AsPointer(ref bodyIndices); + // { + // var s0 = (float*)(inertias + indices[0]) + offsetInFloats; + // var s1 = (float*)(inertias + indices[1]) + offsetInFloats; + // var s2 = (float*)(inertias + indices[2]) + offsetInFloats; + // var s3 = (float*)(inertias + indices[3]) + offsetInFloats; + // var s4 = (float*)(inertias + indices[4]) + offsetInFloats; + // var s5 = (float*)(inertias + indices[5]) + offsetInFloats; + // var s6 = (float*)(inertias + indices[6]) + offsetInFloats; + // var s7 = (float*)(inertias + indices[7]) + offsetInFloats; + + // //Load every inertia vector. + // var m0 = Avx.LoadAlignedVector256(s0); + // var m1 = count > 1 ? Avx.LoadAlignedVector256(s1) : Vector256.Zero; + // var m2 = count > 2 ? Avx.LoadAlignedVector256(s2) : Vector256.Zero; + // var m3 = count > 3 ? Avx.LoadAlignedVector256(s3) : Vector256.Zero; + // var m4 = count > 4 ? Avx.LoadAlignedVector256(s4) : Vector256.Zero; + // var m5 = count > 5 ? Avx.LoadAlignedVector256(s5) : Vector256.Zero; + // var m6 = count > 6 ? Avx.LoadAlignedVector256(s6) : Vector256.Zero; + // var m7 = count > 7 ? Avx.LoadAlignedVector256(s7) : Vector256.Zero; + + // var n0 = Avx.UnpackLow(m0, m1); + // var n1 = Avx.UnpackLow(m2, m3); + // var n2 = Avx.UnpackLow(m4, m5); + // var n3 = Avx.UnpackLow(m6, m7); + // var n4 = Avx.UnpackHigh(m0, m1); + // var n5 = Avx.UnpackHigh(m2, m3); + // var n6 = Avx.UnpackHigh(m4, m5); + // var n7 = Avx.UnpackHigh(m6, m7); + + // var o0 = Avx.Shuffle(n0, n1, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + // var o1 = Avx.Shuffle(n2, n3, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + // var o2 = Avx.Shuffle(n4, n5, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + // var o3 = Avx.Shuffle(n6, n7, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + // var o4 = Avx.Shuffle(n0, n1, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + // var o5 = Avx.Shuffle(n2, n3, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + // var o6 = Avx.Shuffle(n4, n5, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + // var o7 = Avx.Shuffle(n6, n7, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + + // if (filter.GatherInertiaTensor) + // { + // inertia.InverseInertiaTensor.XX = Avx.Permute2x128(o0, o1, 0 | (2 << 4)).AsVector(); + // inertia.InverseInertiaTensor.YX = Avx.Permute2x128(o4, o5, 0 | (2 << 4)).AsVector(); + // inertia.InverseInertiaTensor.YY = Avx.Permute2x128(o2, o3, 0 | (2 << 4)).AsVector(); + // inertia.InverseInertiaTensor.ZX = Avx.Permute2x128(o6, o7, 0 | (2 << 4)).AsVector(); + // inertia.InverseInertiaTensor.ZY = Avx.Permute2x128(o0, o1, 1 | (3 << 4)).AsVector(); + // inertia.InverseInertiaTensor.ZZ = Avx.Permute2x128(o4, o5, 1 | (3 << 4)).AsVector(); + // } + // else + // { + // Unsafe.SkipInit(out inertia.InverseInertiaTensor); + // } + // if (filter.GatherMass) + // { + // inertia.InverseMass = Avx.Permute2x128(o2, o3, 1 | (3 << 4)).AsVector(); + // } + // else + // { + // Unsafe.SkipInit(out inertia.InverseMass); + // } + // } + // } + // else + // { + // Unsafe.SkipInit(out inertia); + // FallbackGatherInertia(count, ActiveSet.SolverStates.Memory, ref bodyIndices, ref inertia, offsetInFloats); + // } + //} + + //[MethodImpl(MethodImplOptions.AggressiveInlining)] + //public unsafe void GatherWorldInertia(ref Vector bodyIndices, int count, out BodyInertiaWide inertia) where TAccessFilter : unmanaged, IBodyAccessFilter + //{ + // GatherInertia(ref bodyIndices, count, 24, out inertia); + //} + //[MethodImpl(MethodImplOptions.AggressiveInlining)] + //public unsafe void GatherLocalInertia(ref Vector bodyIndices, int count, out BodyInertiaWide inertia) where TAccessFilter : unmanaged, IBodyAccessFilter + //{ + // GatherInertia(ref bodyIndices, count, 16, out inertia); + //} /// /// Gathers orientations and relative positions for a two body bundle into an AOSOA bundle. @@ -348,10 +449,8 @@ public unsafe void GatherState(ref TwoBodyReferences references, int count, ref var states = ref ActiveSet.SolverStates; - GatherMotionState(ref references.IndexA, count, out var positionA, out orientationA, out velocityA); - GatherWorldInertia(ref references.IndexA, count, out inertiaA); - GatherMotionState(ref references.IndexB, count, out var positionB, out orientationB, out velocityB); - GatherWorldInertia(ref references.IndexB, count, out inertiaB); + GatherState(ref references.IndexA, count, true, out var positionA, out orientationA, out velocityA, out inertiaA); + GatherState(ref references.IndexB, count, true, out var positionB, out orientationB, out velocityB, out inertiaB); //for (int i = 0; i < count; ++i) //{ @@ -641,21 +740,14 @@ public unsafe void ScatterPoseAndInertia( - //[MethodImpl(MethodImplOptions.AggressiveInlining)] - private unsafe void ScatterVelocities(ref BodyVelocityWide sourceVelocities, ref int baseIndex, int innerIndex) - { - //TODO: How much value would there be in branching on kinematic state and avoiding a write? Depends a lot on the number of kinematics. - ref var sourceSlot = ref GetOffsetInstance(ref sourceVelocities, innerIndex); - ref var target = ref ActiveSet.SolverStates[Unsafe.Add(ref baseIndex, innerIndex)].Motion.Velocity; - target.Linear = new Vector3(sourceSlot.Linear.X[0], sourceSlot.Linear.Y[0], sourceSlot.Linear.Z[0]); - target.Angular = new Vector3(sourceSlot.Angular.X[0], sourceSlot.Angular.Y[0], sourceSlot.Angular.Z[0]); - } //[MethodImpl(MethodImplOptions.AggressiveInlining)] - private unsafe void TransposeScatterVelocities(ref BodyVelocityWide sourceVelocities, SolverState* states, ref Vector references, int count) + public unsafe void ScatterVelocities(ref BodyVelocityWide sourceVelocities, ref Vector references, int count) + where TAccessFilter : unmanaged, IBodyAccessFilter { if (Avx.IsSupported && Vector.Count == 8) { + Unsafe.SkipInit(out TAccessFilter filter); //for (int i = 0; i < 8; ++i) //{ // Get(ref sourceVelocities.Linear.X, i) = i + 100; @@ -668,12 +760,31 @@ private unsafe void TransposeScatterVelocities(ref BodyVelocityWide sourceVeloci //Just like the gather but... transposed, we transpose from the wide representation into per-body velocities. //The motion state struct puts the velocities into the second 32 byte chunk, so we can do a single write per body. - var m0 = sourceVelocities.Linear.X.AsVector256(); - var m1 = sourceVelocities.Linear.Y.AsVector256(); - var m2 = sourceVelocities.Linear.Z.AsVector256(); - var m4 = sourceVelocities.Angular.X.AsVector256(); - var m5 = sourceVelocities.Angular.Y.AsVector256(); - var m6 = sourceVelocities.Angular.Z.AsVector256(); + Vector256 m0, m1, m2, m4, m5, m6; + if (filter.AccessLinearVelocity) + { + m0 = sourceVelocities.Linear.X.AsVector256(); + m1 = sourceVelocities.Linear.Y.AsVector256(); + m2 = sourceVelocities.Linear.Z.AsVector256(); + } + else + { + m0 = Vector256.Zero; + m1 = Vector256.Zero; + m2 = Vector256.Zero; + } + if (filter.AccessAngularVelocity) + { + m4 = sourceVelocities.Angular.X.AsVector256(); + m5 = sourceVelocities.Angular.Y.AsVector256(); + m6 = sourceVelocities.Angular.Z.AsVector256(); + } + else + { + m4 = Vector256.Zero; + m5 = Vector256.Zero; + m6 = Vector256.Zero; + } //We're being a bit lazy here- you could reduce the instructions a bit more given the two empty source lanes. var n0 = Avx.UnpackLow(m0, m1); @@ -695,6 +806,7 @@ private unsafe void TransposeScatterVelocities(ref BodyVelocityWide sourceVeloci var o7 = Avx.Shuffle(n6, n7, 2 | (3 << 2) | (2 << 4) | (3 << 6)); var indices = (int*)Unsafe.AsPointer(ref references); + var states = ActiveSet.SolverStates.Memory; Avx.StoreAligned((float*)(states + indices[0]) + 8, Avx.Permute2x128(o0, o1, 0 | (2 << 4))); if (count > 1) Avx.StoreAligned((float*)(states + indices[1]) + 8, Avx.Permute2x128(o4, o5, 0 | (2 << 4))); if (count > 2) Avx.StoreAligned((float*)(states + indices[2]) + 8, Avx.Permute2x128(o2, o3, 0 | (2 << 4))); @@ -706,8 +818,25 @@ private unsafe void TransposeScatterVelocities(ref BodyVelocityWide sourceVeloci } else { + for (int innerIndex = 0; innerIndex < count; ++innerIndex) + { + ref var sourceSlot = ref GetOffsetInstance(ref sourceVelocities, innerIndex); + var indices = (int*)Unsafe.AsPointer(ref references); + ref var target = ref ActiveSet.SolverStates[indices[innerIndex]].Motion.Velocity; + target.Linear = new Vector3(sourceSlot.Linear.X[0], sourceSlot.Linear.Y[0], sourceSlot.Linear.Z[0]); + target.Angular = new Vector3(sourceSlot.Angular.X[0], sourceSlot.Angular.Y[0], sourceSlot.Angular.Z[0]); + } } } + //[MethodImpl(MethodImplOptions.AggressiveInlining)] + private unsafe void ScatterVelocities(ref BodyVelocityWide sourceVelocities, ref int baseIndex, int innerIndex) + { + //TODO: How much value would there be in branching on kinematic state and avoiding a write? Depends a lot on the number of kinematics. + ref var sourceSlot = ref GetOffsetInstance(ref sourceVelocities, innerIndex); + ref var target = ref ActiveSet.SolverStates[Unsafe.Add(ref baseIndex, innerIndex)].Motion.Velocity; + target.Linear = new Vector3(sourceSlot.Linear.X[0], sourceSlot.Linear.Y[0], sourceSlot.Linear.Z[0]); + target.Angular = new Vector3(sourceSlot.Angular.X[0], sourceSlot.Angular.Y[0], sourceSlot.Angular.Z[0]); + } /// /// Scatters velocities for one body bundle into the active body set. @@ -739,9 +868,8 @@ public unsafe void ScatterVelocities(ref BodyVelocityWide sourceVelocitiesA, ref ref TwoBodyReferences references, int count) { Debug.Assert(count >= 0 && count <= Vector.Count); - var states = ActiveSet.SolverStates.Memory; - TransposeScatterVelocities(ref sourceVelocitiesA, states, ref references.IndexA, count); - TransposeScatterVelocities(ref sourceVelocitiesB, states, ref references.IndexB, count); + ScatterVelocities(ref sourceVelocitiesA, ref references.IndexA, count); + ScatterVelocities(ref sourceVelocitiesB, ref references.IndexB, count); ////Grab the base references for the body indices. Note that we make use of the references memory layout again. //ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); //ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); diff --git a/BepuPhysics/Constraints/CenterDistanceConstraint.cs b/BepuPhysics/Constraints/CenterDistanceConstraint.cs index 1897afaab..eed750ef6 100644 --- a/BepuPhysics/Constraints/CenterDistanceConstraint.cs +++ b/BepuPhysics/Constraints/CenterDistanceConstraint.cs @@ -127,6 +127,57 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ApplyImpulse(ref velocityA, ref velocityB, ref projection, ref csi); } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + static void ApplyImpulse(in Vector3Wide jacobianA, in Vector inverseMassA, in Vector inverseMassB, in Vector impulse, ref BodyVelocityWide a, ref BodyVelocityWide b) + { + Vector3Wide.Scale(jacobianA, impulse * inverseMassA, out var changeA); + Vector3Wide.Scale(jacobianA, impulse * inverseMassB, out var negatedChangeB); + Vector3Wide.Add(a.Linear, changeA, out a.Linear); + Vector3Wide.Subtract(b.Linear, negatedChangeB, out b.Linear); + } + + //[MethodImpl(MethodImplOptions.NoInlining)] + public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, + in CenterDistancePrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + var lengthSquared = ab.LengthSquared(); + var inverseDistance = MathHelper.FastReciprocalSquareRoot(lengthSquared); + var useFallback = Vector.LessThan(lengthSquared, new Vector(1e-10f)); + Vector3Wide.Scale(ab, inverseDistance, out var jacobianA); + jacobianA.X = Vector.ConditionalSelect(useFallback, Vector.One, jacobianA.X); + jacobianA.Y = Vector.ConditionalSelect(useFallback, Vector.Zero, jacobianA.Y); + jacobianA.Z = Vector.ConditionalSelect(useFallback, Vector.Zero, jacobianA.Z); + + ApplyImpulse(jacobianA, inertiaA.InverseMass, inertiaB.InverseMass, accumulatedImpulses, ref wsvA, ref wsvB); + } + //[MethodImpl(MethodImplOptions.AggressiveInlining)] + public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, + in CenterDistancePrestepData prestep, ref Vector accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + //Note that we need the actual length for error calculation. + var distance = ab.Length(); + var inverseDistance = MathHelper.FastReciprocal(distance); + var useFallback = Vector.LessThan(distance, new Vector(1e-5f)); + Vector3Wide.Scale(ab, inverseDistance, out var jacobianA); + jacobianA.X = Vector.ConditionalSelect(useFallback, Vector.One, jacobianA.X); + jacobianA.Y = Vector.ConditionalSelect(useFallback, Vector.Zero, jacobianA.Y); + jacobianA.Z = Vector.ConditionalSelect(useFallback, Vector.Zero, jacobianA.Z); + + SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); + //Jacobian is just the unit length direction, so the effective mass is simple: + var effectiveMass = effectiveMassCFMScale / (inertiaA.InverseMass + inertiaB.InverseMass); + + //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. + var biasVelocity = (distance - prestep.TargetDistance) * positionErrorToVelocity; + + //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); + Vector3Wide.Dot(wsvA.Linear, jacobianA, out var linearCSVA); + Vector3Wide.Dot(wsvB.Linear, jacobianA, out var negatedCSVB); + var csi = (biasVelocity - (linearCSVA - negatedCSVB)) * effectiveMass - accumulatedImpulse * softnessImpulseScale; + accumulatedImpulse += csi; + ApplyImpulse(jacobianA, inertiaA.InverseMass, inertiaB.InverseMass, csi, ref wsvA, ref wsvB); + } + } diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index d46e04a48..5a1138457 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -494,29 +494,29 @@ public static unsafe void IntegratePoseAndVelocity( [MethodImpl(MethodImplOptions.AggressiveInlining)] private static unsafe void IntegrateUniformly( Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, float dt, int workerIndex, ref Vector bodyIndices, int count, ref Vector integrationMask, - ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocityWide velocity, out BodyInertiaWide inertia) where TIntegratorCallbacks : struct, IPoseIntegratorCallbacks + out Vector3Wide position, out QuaternionWide orientation, out BodyVelocityWide velocity, out BodyInertiaWide inertia) where TIntegratorCallbacks : struct, IPoseIntegratorCallbacks { - bodies.GatherLocalInertia(ref bodyIndices, count, out var localInertia); + bodies.GatherState(ref bodyIndices, count, false, out position, out orientation, out velocity, out var localInertia); IntegratePoseAndVelocity(ref integratorCallbacks, ref bodyIndices, count, localInertia, dt, ref position, ref orientation, ref velocity, workerIndex, out inertia); bodies.ScatterPoseAndInertia(ref position, ref orientation, ref inertia, ref bodyIndices, count, ref integrationMask); } //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public static unsafe void GatherAndIntegrate( + public static unsafe void GatherAndIntegrate( Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, ref Buffer integrationFlags, int bodyIndexInConstraint, float dt, int workerIndex, int bundleIndex, ref Vector bodyIndices, int count, out Vector3Wide position, out QuaternionWide orientation, out BodyVelocityWide velocity, out BodyInertiaWide inertia) where TIntegratorCallbacks : struct, IPoseIntegratorCallbacks - where TBatchIntegrationMode : struct, IBatchIntegrationMode + where TBatchIntegrationMode : unmanaged, IBatchIntegrationMode + where TAccessFilter : unmanaged, IBodyAccessFilter { - bodies.GatherMotionState(ref bodyIndices, count, out position, out orientation, out velocity); //These type tests are compile time constants and will be specialized. if (typeof(TBatchIntegrationMode) == typeof(BatchShouldAlwaysIntegrate)) { var integrationMask = new Vector(-1); - IntegrateUniformly(bodies, ref integratorCallbacks, dt, workerIndex, ref bodyIndices, count, ref integrationMask, ref position, ref orientation, ref velocity, out inertia); + IntegrateUniformly(bodies, ref integratorCallbacks, dt, workerIndex, ref bodyIndices, count, ref integrationMask, out position, out orientation, out velocity, out inertia); } else if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) { - bodies.GatherWorldInertia(ref bodyIndices, count, out inertia); + bodies.GatherState(ref bodyIndices, count, true, out position, out orientation, out velocity, out inertia); } else { @@ -527,7 +527,7 @@ public static unsafe void GatherAndIntegrate(ref bodyIndices, count, false, out position, out orientation, out velocity, out var localInertia); IntegratePoseAndVelocity(ref integratorCallbacks, ref bodyIndices, count, localInertia, dt, integrationMask, ref position, ref orientation, ref velocity, workerIndex, out inertia); //The scatter will be able to ignore any lanes which have a zeroed integration mask. bodies.ScatterPoseAndInertia(ref position, ref orientation, ref inertia, ref bodyIndices, count, ref integrationMask); @@ -543,7 +543,7 @@ public static unsafe void GatherAndIntegrate(ref bodyIndices, count, true, out position, out orientation, out velocity, out inertia); } break; } @@ -567,9 +567,9 @@ public unsafe override void WarmStart2(bodies, ref integratorCallbacks, ref integrationFlags, 0, dt, workerIndex, i, ref references.IndexA, count, + GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 0, dt, workerIndex, i, ref references.IndexA, count, out var positionA, out var orientationA, out var wsvA, out var inertiaA); - GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 1, dt, workerIndex, i, ref references.IndexB, count, + GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 1, dt, workerIndex, i, ref references.IndexB, count, out var positionB, out var orientationB, out var wsvB, out var inertiaB); var ab = positionB - positionA; if (typeof(TConstraintFunctions) == typeof(WeldFunctions)) @@ -582,12 +582,18 @@ public unsafe override void WarmStart2(ref prestep), Unsafe.As(ref accumulatedImpulses), ref wsvA, ref wsvB); } + else if (typeof(TConstraintFunctions) == typeof(CenterDistanceConstraint)) + { + default(CenterDistanceConstraintFunctions).WarmStart2(orientationA, inertiaA, ab, orientationB, inertiaB, + Unsafe.As(ref prestep), Unsafe.As>(ref accumulatedImpulses), ref wsvA, ref wsvB); + } else { function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, dt, inverseDt, ref prestep, out var projection); function.WarmStart(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); } - bodies.ScatterVelocities(ref wsvA, ref wsvB, ref references, count); + bodies.ScatterVelocities(ref wsvA, ref references.IndexA, count); + bodies.ScatterVelocities(ref wsvB, ref references.IndexB, count); } } @@ -607,7 +613,9 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f ref var references = ref bodyReferencesBundles[i]; var count = GetCountInBundle(ref typeBatch, i); Prefetch(solvePrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, i, exclusiveEndBundle); - bodies.GatherState(ref references, count, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB); + bodies.GatherState(ref references.IndexA, count, true, out var positionA, out var orientationA, out var wsvA, out var inertiaA); + bodies.GatherState(ref references.IndexB, count, true, out var positionB, out var orientationB, out var wsvB, out var inertiaB); + var ab = positionB - positionA; if (typeof(TConstraintFunctions) == typeof(WeldFunctions)) { default(WeldFunctions).Solve2(orientationA, inertiaA, ab, orientationB, inertiaB, dt, inverseDt, @@ -618,12 +626,18 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f default(BallSocketFunctions).Solve2(orientationA, inertiaA, ab, orientationB, inertiaB, dt, inverseDt, Unsafe.As(ref prestep), ref Unsafe.As(ref accumulatedImpulses), ref wsvA, ref wsvB); } + else if (typeof(TConstraintFunctions) == typeof(CenterDistanceConstraint)) + { + default(CenterDistanceConstraintFunctions).Solve2(orientationA, inertiaA, ab, orientationB, inertiaB, dt, inverseDt, + Unsafe.As(ref prestep), ref Unsafe.As>(ref accumulatedImpulses), ref wsvA, ref wsvB); + } else { function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, dt, inverseDt, ref prestep, out var projection); function.Solve(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); } - bodies.ScatterVelocities(ref wsvA, ref wsvB, ref references, count); + bodies.ScatterVelocities(ref wsvA, ref references.IndexA, count); + bodies.ScatterVelocities(ref wsvB, ref references.IndexB, count); } } diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index 5ce59279e..2bdd1436a 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -141,7 +141,7 @@ internal unsafe abstract void CopySleepingToActive( public abstract void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks poseIntegratorCallbacks, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) where TIntegratorCallbacks : struct, IPoseIntegratorCallbacks - where TBatchIntegrationMode : struct, IBatchIntegrationMode; + where TBatchIntegrationMode : unmanaged, IBatchIntegrationMode; public abstract void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); diff --git a/BepuPhysics/Constraints/Weld.cs b/BepuPhysics/Constraints/Weld.cs index bccc85fbf..9d91e61a3 100644 --- a/BepuPhysics/Constraints/Weld.cs +++ b/BepuPhysics/Constraints/Weld.cs @@ -287,7 +287,7 @@ public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. //var offset = prestep.LocalOffset * orientationA; - Transform(prestep.LocalOffset, orientationA, out var offset); + Transform(prestep.LocalOffset, orientationA, out var offset); //Effective mass = (J * M^-1 * JT)^-1, which is going to be a little tricky because J * M^-1 * JT is a 6x6 matrix: //J * M^-1 * JT = [ Ia^-1 + Ib^-1, Ia^-1 * transpose(skewSymmetric(localOffset * orientationA)) ] @@ -362,11 +362,30 @@ public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, } + public struct WeldWarmStartAccessFilterA : IBodyAccessFilter + { + public bool GatherPosition => false; + public bool GatherOrientation => true; + public bool GatherMass => true; + public bool GatherInertiaTensor => true; + public bool AccessLinearVelocity => true; + public bool AccessAngularVelocity => true; + } + public struct WeldWarmStartAccessFilterB : IBodyAccessFilter + { + public bool GatherPosition => false; + public bool GatherOrientation => false; + public bool GatherMass => true; + public bool GatherInertiaTensor => true; + public bool AccessLinearVelocity => true; + public bool AccessAngularVelocity => true; + } + /// /// Handles the solve iterations of a bunch of ball socket constraints. /// - public class WeldTypeProcessor : TwoBodyTypeProcessor + public class WeldTypeProcessor : TwoBodyTypeProcessor { public const int BatchTypeId = 31; } diff --git a/BepuUtilities/MathHelper.cs b/BepuUtilities/MathHelper.cs index e8c588fe8..22fcc425d 100644 --- a/BepuUtilities/MathHelper.cs +++ b/BepuUtilities/MathHelper.cs @@ -1,6 +1,9 @@ using System; using System.Numerics; using System.Runtime.CompilerServices; +using System.Runtime.Intrinsics; +using System.Runtime.Intrinsics.Arm; +using System.Runtime.Intrinsics.X86; namespace BepuUtilities { @@ -328,11 +331,22 @@ public static void ApproximateAcos(Vector x, out Vector acos) [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Floor(in Vector x, out Vector result) + public static Vector Floor(Vector v) { - //This is far from ideal. You could probably do better- especially with platform intrinsics. - var intX = Vector.ConvertToInt32(x); - result = Vector.ConvertToSingle(Vector.ConditionalSelect(Vector.LessThan(x, Vector.Zero), intX - Vector.One, intX)); + if (Avx.IsSupported && Vector.Count == 8) + { + return Avx.Floor(v.AsVector256()).AsVector(); + } + else if (Sse41.IsSupported && Vector.Count == 4) + { + return Sse41.Floor(v.AsVector128()).AsVector(); + } + else + { + var intX = Vector.ConvertToInt32(v); + return Vector.ConvertToSingle(Vector.ConditionalSelect(Vector.LessThan(v, Vector.Zero), intX - Vector.One, intX)); + } + //TODO: Arm! } /// @@ -344,11 +358,45 @@ public static void Floor(in Vector x, out Vector result) [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void GetSignedAngleDifference(in Vector a, in Vector b, out Vector difference) { - var pi = new Vector(Pi); var half = new Vector(0.5f); var x = (b - a) * new Vector(1f / TwoPi) + half; - Floor(x, out var flooredX); - difference = (x - flooredX - half) * TwoPi; + difference = (x - Floor(x) - half) * new Vector(TwoPi); + } + + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector FastReciprocal(Vector v) + { + if (Avx.IsSupported && Vector.Count == 8) + { + return Avx.Reciprocal(v.AsVector256()).AsVector(); + } + else if (Sse.IsSupported && Vector.Count == 4) + { + return Sse.Reciprocal(v.AsVector128()).AsVector(); + } + else + { + return Vector.One / v; + } + //TODO: Arm! + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector FastReciprocalSquareRoot(Vector v) + { + if (Avx.IsSupported && Vector.Count == 8) + { + return Avx.ReciprocalSqrt(v.AsVector256()).AsVector(); + } + else if (Sse.IsSupported && Vector.Count == 4) + { + return Sse.ReciprocalSqrt(v.AsVector128()).AsVector(); + } + else + { + return Vector.One / Vector.SquareRoot(v); + } + //TODO: Arm! } } } From acad41f679f59062590ab867e109dcd4360b900f Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 18 Jul 2021 18:13:46 -0500 Subject: [PATCH 118/947] Fixed scatter velocity/body access filter inconsistency during warmstart. --- BepuPhysics/Bodies_GatherScatter.cs | 207 +++++++++--------- .../Constraints/FourBodyTypeProcessor.cs | 17 +- .../Constraints/OneBodyTypeProcessor.cs | 12 +- .../Constraints/ThreeBodyTypeProcessor.cs | 12 +- .../Constraints/TwoBodyTypeProcessor.cs | 82 ++++--- Demos/Demos/NewtDemo.cs | 16 +- Demos/Program.cs | 2 +- 7 files changed, 187 insertions(+), 161 deletions(-) diff --git a/BepuPhysics/Bodies_GatherScatter.cs b/BepuPhysics/Bodies_GatherScatter.cs index 64907e55a..cbf9ec51a 100644 --- a/BepuPhysics/Bodies_GatherScatter.cs +++ b/BepuPhysics/Bodies_GatherScatter.cs @@ -742,8 +742,7 @@ public unsafe void ScatterPoseAndInertia( //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void ScatterVelocities(ref BodyVelocityWide sourceVelocities, ref Vector references, int count) - where TAccessFilter : unmanaged, IBodyAccessFilter + public unsafe void ScatterVelocities(ref BodyVelocityWide sourceVelocities, ref Vector references, int count) where TAccessFilter : unmanaged, IBodyAccessFilter { if (Avx.IsSupported && Vector.Count == 8) { @@ -828,111 +827,111 @@ public unsafe void ScatterVelocities(ref BodyVelocityWide sourceV } } } - //[MethodImpl(MethodImplOptions.AggressiveInlining)] - private unsafe void ScatterVelocities(ref BodyVelocityWide sourceVelocities, ref int baseIndex, int innerIndex) - { - //TODO: How much value would there be in branching on kinematic state and avoiding a write? Depends a lot on the number of kinematics. - ref var sourceSlot = ref GetOffsetInstance(ref sourceVelocities, innerIndex); - ref var target = ref ActiveSet.SolverStates[Unsafe.Add(ref baseIndex, innerIndex)].Motion.Velocity; - target.Linear = new Vector3(sourceSlot.Linear.X[0], sourceSlot.Linear.Y[0], sourceSlot.Linear.Z[0]); - target.Angular = new Vector3(sourceSlot.Angular.X[0], sourceSlot.Angular.Y[0], sourceSlot.Angular.Z[0]); - } + ////[MethodImpl(MethodImplOptions.AggressiveInlining)] + //private unsafe void ScatterVelocities(ref BodyVelocityWide sourceVelocities, ref int baseIndex, int innerIndex) + //{ + // //TODO: How much value would there be in branching on kinematic state and avoiding a write? Depends a lot on the number of kinematics. + // ref var sourceSlot = ref GetOffsetInstance(ref sourceVelocities, innerIndex); + // ref var target = ref ActiveSet.SolverStates[Unsafe.Add(ref baseIndex, innerIndex)].Motion.Velocity; + // target.Linear = new Vector3(sourceSlot.Linear.X[0], sourceSlot.Linear.Y[0], sourceSlot.Linear.Z[0]); + // target.Angular = new Vector3(sourceSlot.Angular.X[0], sourceSlot.Angular.Y[0], sourceSlot.Angular.Z[0]); + //} - /// - /// Scatters velocities for one body bundle into the active body set. - /// - /// Velocities of body bundle A to scatter. - /// Active set indices of the bodies to scatter velocity data to. - /// Number of body pairs in the bundle. - //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void ScatterVelocities(ref BodyVelocityWide sourceVelocities, ref Vector references, int count) - { - Debug.Assert(count >= 0 && count <= Vector.Count); - //Grab the base references for the body indices. Note that we make use of the references memory layout again. - ref var baseIndex = ref Unsafe.As, int>(ref references); - for (int i = 0; i < count; ++i) - { - ScatterVelocities(ref sourceVelocities, ref baseIndex, i); - } - } + ///// + ///// Scatters velocities for one body bundle into the active body set. + ///// + ///// Velocities of body bundle A to scatter. + ///// Active set indices of the bodies to scatter velocity data to. + ///// Number of body pairs in the bundle. + ////[MethodImpl(MethodImplOptions.AggressiveInlining)] + //public unsafe void ScatterVelocities(ref BodyVelocityWide sourceVelocities, ref Vector references, int count) + //{ + // Debug.Assert(count >= 0 && count <= Vector.Count); + // //Grab the base references for the body indices. Note that we make use of the references memory layout again. + // ref var baseIndex = ref Unsafe.As, int>(ref references); + // for (int i = 0; i < count; ++i) + // { + // ScatterVelocities(ref sourceVelocities, ref baseIndex, i); + // } + //} - /// - /// Scatters velocities for two body bundles into the active body set. - /// - /// Velocities of body bundle A to scatter. - /// Velocities of body bundle B to scatter. - /// Active set indices of the bodies to scatter velocity data to. - /// Number of body pairs in the bundle. - //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void ScatterVelocities(ref BodyVelocityWide sourceVelocitiesA, ref BodyVelocityWide sourceVelocitiesB, - ref TwoBodyReferences references, int count) - { - Debug.Assert(count >= 0 && count <= Vector.Count); - ScatterVelocities(ref sourceVelocitiesA, ref references.IndexA, count); - ScatterVelocities(ref sourceVelocitiesB, ref references.IndexB, count); - ////Grab the base references for the body indices. Note that we make use of the references memory layout again. - //ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); - //ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); - //for (int i = 0; i < count; ++i) - //{ - // ScatterVelocities(ref sourceVelocitiesA, ref baseIndexA, i); - // ScatterVelocities(ref sourceVelocitiesB, ref baseIndexB, i); - //} - } + ///// + ///// Scatters velocities for two body bundles into the active body set. + ///// + ///// Velocities of body bundle A to scatter. + ///// Velocities of body bundle B to scatter. + ///// Active set indices of the bodies to scatter velocity data to. + ///// Number of body pairs in the bundle. + ////[MethodImpl(MethodImplOptions.AggressiveInlining)] + //public unsafe void ScatterVelocities(ref BodyVelocityWide sourceVelocitiesA, ref BodyVelocityWide sourceVelocitiesB, + // ref TwoBodyReferences references, int count) + //{ + // Debug.Assert(count >= 0 && count <= Vector.Count); + // ScatterVelocities(ref sourceVelocitiesA, ref references.IndexA, count); + // ScatterVelocities(ref sourceVelocitiesB, ref references.IndexB, count); + // ////Grab the base references for the body indices. Note that we make use of the references memory layout again. + // //ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); + // //ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); + // //for (int i = 0; i < count; ++i) + // //{ + // // ScatterVelocities(ref sourceVelocitiesA, ref baseIndexA, i); + // // ScatterVelocities(ref sourceVelocitiesB, ref baseIndexB, i); + // //} + //} - /// - /// Scatters velocities for three body bundles into the active body set. - /// - /// Velocities of body bundle A to scatter. - /// Velocities of body bundle B to scatter. - /// Velocities of body bundle C to scatter. - /// Active set indices of the bodies to scatter velocity data to. - /// Number of body pairs in the bundle. - //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void ScatterVelocities( - ref BodyVelocityWide sourceVelocitiesA, ref BodyVelocityWide sourceVelocitiesB, ref BodyVelocityWide sourceVelocitiesC, - ref ThreeBodyReferences references, int count) - { - Debug.Assert(count >= 0 && count <= Vector.Count); - //Grab the base references for the body indices. Note that we make use of the references memory layout again. - ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); - ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); - ref var baseIndexC = ref Unsafe.As, int>(ref references.IndexC); - for (int i = 0; i < count; ++i) - { - ScatterVelocities(ref sourceVelocitiesA, ref baseIndexA, i); - ScatterVelocities(ref sourceVelocitiesB, ref baseIndexB, i); - ScatterVelocities(ref sourceVelocitiesC, ref baseIndexC, i); - } - } + ///// + ///// Scatters velocities for three body bundles into the active body set. + ///// + ///// Velocities of body bundle A to scatter. + ///// Velocities of body bundle B to scatter. + ///// Velocities of body bundle C to scatter. + ///// Active set indices of the bodies to scatter velocity data to. + ///// Number of body pairs in the bundle. + ////[MethodImpl(MethodImplOptions.AggressiveInlining)] + //public unsafe void ScatterVelocities( + // ref BodyVelocityWide sourceVelocitiesA, ref BodyVelocityWide sourceVelocitiesB, ref BodyVelocityWide sourceVelocitiesC, + // ref ThreeBodyReferences references, int count) + //{ + // Debug.Assert(count >= 0 && count <= Vector.Count); + // //Grab the base references for the body indices. Note that we make use of the references memory layout again. + // ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); + // ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); + // ref var baseIndexC = ref Unsafe.As, int>(ref references.IndexC); + // for (int i = 0; i < count; ++i) + // { + // ScatterVelocities(ref sourceVelocitiesA, ref baseIndexA, i); + // ScatterVelocities(ref sourceVelocitiesB, ref baseIndexB, i); + // ScatterVelocities(ref sourceVelocitiesC, ref baseIndexC, i); + // } + //} - /// - /// Scatters velocities for four body bundles into the active body set. - /// - /// Velocities of body bundle A to scatter. - /// Velocities of body bundle B to scatter. - /// Velocities of body bundle C to scatter. - /// Velocities of body bundle D to scatter. - /// Active set indices of the bodies to scatter velocity data to. - /// Number of body pairs in the bundle. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void ScatterVelocities( - ref BodyVelocityWide sourceVelocitiesA, ref BodyVelocityWide sourceVelocitiesB, ref BodyVelocityWide sourceVelocitiesC, ref BodyVelocityWide sourceVelocitiesD, - ref FourBodyReferences references, int count) - { - Debug.Assert(count >= 0 && count <= Vector.Count); - //Grab the base references for the body indices. Note that we make use of the references memory layout again. - ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); - ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); - ref var baseIndexC = ref Unsafe.As, int>(ref references.IndexC); - ref var baseIndexD = ref Unsafe.As, int>(ref references.IndexD); - for (int i = 0; i < count; ++i) - { - ScatterVelocities(ref sourceVelocitiesA, ref baseIndexA, i); - ScatterVelocities(ref sourceVelocitiesB, ref baseIndexB, i); - ScatterVelocities(ref sourceVelocitiesC, ref baseIndexC, i); - ScatterVelocities(ref sourceVelocitiesD, ref baseIndexD, i); - } - } + ///// + ///// Scatters velocities for four body bundles into the active body set. + ///// + ///// Velocities of body bundle A to scatter. + ///// Velocities of body bundle B to scatter. + ///// Velocities of body bundle C to scatter. + ///// Velocities of body bundle D to scatter. + ///// Active set indices of the bodies to scatter velocity data to. + ///// Number of body pairs in the bundle. + //[MethodImpl(MethodImplOptions.AggressiveInlining)] + //public unsafe void ScatterVelocities( + // ref BodyVelocityWide sourceVelocitiesA, ref BodyVelocityWide sourceVelocitiesB, ref BodyVelocityWide sourceVelocitiesC, ref BodyVelocityWide sourceVelocitiesD, + // ref FourBodyReferences references, int count) + //{ + // Debug.Assert(count >= 0 && count <= Vector.Count); + // //Grab the base references for the body indices. Note that we make use of the references memory layout again. + // ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); + // ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); + // ref var baseIndexC = ref Unsafe.As, int>(ref references.IndexC); + // ref var baseIndexD = ref Unsafe.As, int>(ref references.IndexD); + // for (int i = 0; i < count; ++i) + // { + // ScatterVelocities(ref sourceVelocitiesA, ref baseIndexA, i); + // ScatterVelocities(ref sourceVelocitiesB, ref baseIndexB, i); + // ScatterVelocities(ref sourceVelocitiesC, ref baseIndexC, i); + // ScatterVelocities(ref sourceVelocitiesD, ref baseIndexD, i); + // } + //} } } diff --git a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs index c72077b6d..33d88de5c 100644 --- a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs @@ -134,8 +134,11 @@ public unsafe override void WarmStart(ref TypeBatch typeBatch, Bodies bodies, in out var ab, out var orientationB, out var wsvB, out var inertiaB, out var ac, out var orientationC, out var wsvC, out var inertiaC, out var ad, out var orientationD, out var wsvD, out var inertiaD); - function.WarmStart(ref wsvA, ref wsvB, ref wsvC, ref wsvD, ref projection, ref accumulatedImpulses); - bodies.ScatterVelocities(ref wsvA, ref wsvB, ref wsvC, ref wsvD, ref bodyReferences, count); + function.WarmStart(ref wsvA, ref wsvB, ref wsvC, ref wsvD, ref projection, ref accumulatedImpulses); + bodies.ScatterVelocities(ref wsvA, ref bodyReferences.IndexA, count); + bodies.ScatterVelocities(ref wsvB, ref bodyReferences.IndexB, count); + bodies.ScatterVelocities(ref wsvC, ref bodyReferences.IndexC, count); + bodies.ScatterVelocities(ref wsvD, ref bodyReferences.IndexD, count); } } @@ -158,7 +161,10 @@ public unsafe override void SolveIteration(ref TypeBatch typeBatch, Bodies bodie out var ac, out var orientationC, out var wsvC, out var inertiaC, out var ad, out var orientationD, out var wsvD, out var inertiaD); function.Solve(ref wsvA, ref wsvB, ref wsvC, ref wsvD, ref projection, ref accumulatedImpulses); - bodies.ScatterVelocities(ref wsvA, ref wsvB, ref wsvC, ref wsvD, ref bodyReferences, count); + bodies.ScatterVelocities(ref wsvA, ref bodyReferences.IndexA, count); + bodies.ScatterVelocities(ref wsvB, ref bodyReferences.IndexB, count); + bodies.ScatterVelocities(ref wsvC, ref bodyReferences.IndexC, count); + bodies.ScatterVelocities(ref wsvD, ref bodyReferences.IndexD, count); } } @@ -271,7 +277,10 @@ public unsafe override void SolveStep(ref TypeBatch typeBatch, Bodies bodies, fl function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, ac, orientationC, inertiaC, ad, orientationD, inertiaD, dt, inverseDt, ref prestep, out var projection); function.WarmStart(ref wsvA, ref wsvB, ref wsvC, ref wsvD, ref projection, ref accumulatedImpulses); function.Solve(ref wsvA, ref wsvB, ref wsvC, ref wsvD, ref projection, ref accumulatedImpulses); - bodies.ScatterVelocities(ref wsvA, ref wsvB, ref wsvC, ref wsvD, ref bodyReferences, count); + bodies.ScatterVelocities(ref wsvA, ref bodyReferences.IndexA, count); + bodies.ScatterVelocities(ref wsvB, ref bodyReferences.IndexB, count); + bodies.ScatterVelocities(ref wsvC, ref bodyReferences.IndexC, count); + bodies.ScatterVelocities(ref wsvD, ref bodyReferences.IndexD, count); } } diff --git a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs index 47f55c8eb..211d16578 100644 --- a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs @@ -120,7 +120,7 @@ public unsafe override void WarmStart(ref TypeBatch typeBatch, Bodies bodies, in int count = GetCountInBundle(ref typeBatch, i); bodies.GatherState(ref bodyReferences, count, out _, out _, out var wsvA, out _); function.WarmStart(ref wsvA, ref projection, ref accumulatedImpulses); - bodies.ScatterVelocities(ref wsvA, ref bodyReferences, count); + bodies.ScatterVelocities(ref wsvA, ref bodyReferences, count); } } @@ -138,7 +138,7 @@ public unsafe override void SolveIteration(ref TypeBatch typeBatch, Bodies bodie int count = GetCountInBundle(ref typeBatch, i); bodies.GatherState(ref bodyReferences, count, out _, out _, out var wsvA, out _); function.Solve(ref wsvA, ref projection, ref accumulatedImpulses); - bodies.ScatterVelocities(ref wsvA, ref bodyReferences, count); + bodies.ScatterVelocities(ref wsvA, ref bodyReferences, count); } } @@ -217,7 +217,7 @@ public unsafe override void SolveStep(ref TypeBatch typeBatch, Bodies bodies, fl function.Prestep(position, orientation, inertiaA, dt, inverseDt, ref prestep, out var projection); function.WarmStart(ref wsvA, ref projection, ref accumulatedImpulses); function.Solve(ref wsvA, ref projection, ref accumulatedImpulses); - bodies.ScatterVelocities(ref wsvA, ref bodyReferences, count); + bodies.ScatterVelocities(ref wsvA, ref bodyReferences, count); } } @@ -267,7 +267,7 @@ public unsafe override void WarmStart2(ref wsvA, ref bodyReferences, count); } } public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) @@ -286,7 +286,7 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f bodies.GatherState(ref bodyReferences, count, out var position, out var orientation, out var wsvA, out var inertiaA); function.Prestep(position, orientation, inertiaA, dt, inverseDt, ref prestep, out var projection); function.Solve(ref wsvA, ref projection, ref accumulatedImpulses); - bodies.ScatterVelocities(ref wsvA, ref bodyReferences, count); + bodies.ScatterVelocities(ref wsvA, ref bodyReferences, count); } } @@ -307,7 +307,7 @@ public unsafe override void Prestep2(ref TypeBatch typeBatch, Bodies bodies, flo bodies.GatherState(ref references, count, out var position, out var orientation, out var wsvA, out var inertiaA); function.Prestep(position, orientation, inertiaA, dt, inverseDt, ref prestep, out projection); function.WarmStart(ref wsvA, ref projection, ref accumulatedImpulses); - bodies.ScatterVelocities(ref wsvA, ref references, count); + bodies.ScatterVelocities(ref wsvA, ref references, count); } } public override void JacobiPrestep2(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) diff --git a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs index 5fe12b6ae..d93abc1b7 100644 --- a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs @@ -127,7 +127,9 @@ public unsafe override void WarmStart(ref TypeBatch typeBatch, Bodies bodies, in out var ab, out var orientationB, out var wsvB, out var inertiaB, out var ac, out var orientationC, out var wsvC, out var inertiaC); function.WarmStart(ref wsvA, ref wsvB, ref wsvC, ref projection, ref accumulatedImpulses); - bodies.ScatterVelocities(ref wsvA, ref wsvB, ref wsvC, ref bodyReferences, count); + bodies.ScatterVelocities(ref wsvA, ref bodyReferences.IndexA, count); + bodies.ScatterVelocities(ref wsvB, ref bodyReferences.IndexB, count); + bodies.ScatterVelocities(ref wsvC, ref bodyReferences.IndexC, count); } } @@ -149,7 +151,9 @@ public unsafe override void SolveIteration(ref TypeBatch typeBatch, Bodies bodie out var ab, out var orientationB, out var wsvB, out var inertiaB, out var ac, out var orientationC, out var wsvC, out var inertiaC); function.Solve(ref wsvA, ref wsvB, ref wsvC, ref projection, ref accumulatedImpulses); - bodies.ScatterVelocities(ref wsvA, ref wsvB, ref wsvC, ref bodyReferences, count); + bodies.ScatterVelocities(ref wsvA, ref bodyReferences.IndexA, count); + bodies.ScatterVelocities(ref wsvB, ref bodyReferences.IndexB, count); + bodies.ScatterVelocities(ref wsvC, ref bodyReferences.IndexC, count); } } @@ -253,7 +257,9 @@ public unsafe override void SolveStep(ref TypeBatch typeBatch, Bodies bodies, fl function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, ac, orientationC, inertiaC, dt, inverseDt, ref prestep, out var projection); function.WarmStart(ref wsvA, ref wsvB, ref wsvC, ref projection, ref accumulatedImpulses); function.Solve(ref wsvA, ref wsvB, ref wsvC, ref projection, ref accumulatedImpulses); - bodies.ScatterVelocities(ref wsvA, ref wsvB, ref wsvC, ref bodyReferences, count); + bodies.ScatterVelocities(ref wsvA, ref bodyReferences.IndexA, count); + bodies.ScatterVelocities(ref wsvB, ref bodyReferences.IndexB, count); + bodies.ScatterVelocities(ref wsvC, ref bodyReferences.IndexC, count); } } diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 5a1138457..971a38903 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -161,11 +161,12 @@ public unsafe override void WarmStart(ref TypeBatch typeBatch, Bodies bodies, in { ref var projection = ref Unsafe.Add(ref projectionBase, i); ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); - ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); + ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); int count = GetCountInBundle(ref typeBatch, i); - bodies.GatherState(ref bodyReferences, count, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB); + bodies.GatherState(ref references, count, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB); function.WarmStart(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); - bodies.ScatterVelocities(ref wsvA, ref wsvB, ref bodyReferences, count); + bodies.ScatterVelocities(ref wsvA, ref references.IndexA, count); + bodies.ScatterVelocities(ref wsvB, ref references.IndexB, count); } } @@ -180,11 +181,12 @@ public unsafe override void SolveIteration(ref TypeBatch typeBatch, Bodies bodie { ref var projection = ref Unsafe.Add(ref projectionBase, i); ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); - ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); + ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); int count = GetCountInBundle(ref typeBatch, i); - bodies.GatherState(ref bodyReferences, count, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB); + bodies.GatherState(ref references, count, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB); function.Solve(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); - bodies.ScatterVelocities(ref wsvA, ref wsvB, ref bodyReferences, count); + bodies.ScatterVelocities(ref wsvA, ref references.IndexA, count); + bodies.ScatterVelocities(ref wsvB, ref references.IndexB, count); } } @@ -242,11 +244,11 @@ public unsafe override void JacobiSolveIteration(ref TypeBatch typeBatch, Bodies { ref var projection = ref Unsafe.Add(ref projectionBase, i); ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); - ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); + ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); int count = GetCountInBundle(ref typeBatch, i); ref var wsvA = ref jacobiResultsBundlesA[i]; ref var wsvB = ref jacobiResultsBundlesB[i]; - bodies.GatherState(ref bodyReferences, count, out var orientationA, out wsvA, out var inertiaA, out var ab, out var orientationB, out wsvB, out var inertiaB); + bodies.GatherState(ref references, count, out var orientationA, out wsvA, out var inertiaA, out var ab, out var orientationB, out wsvB, out var inertiaB); function.Solve(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); } } @@ -262,14 +264,14 @@ public unsafe override void SolveStep(ref TypeBatch typeBatch, Bodies bodies, fl { ref var prestep = ref Unsafe.Add(ref prestepBase, i); ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); - ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); var count = GetCountInBundle(ref typeBatch, i); bodies.GatherState(ref references, count, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB); function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, dt, inverseDt, ref prestep, out var projection); function.WarmStart(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); function.Solve(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); - bodies.ScatterVelocities(ref wsvA, ref wsvB, ref bodyReferences, count); + bodies.ScatterVelocities(ref wsvA, ref references.IndexA, count); + bodies.ScatterVelocities(ref wsvB, ref references.IndexB, count); } } @@ -523,29 +525,27 @@ public static unsafe void GatherAndIntegrate(ref bodyIndices, count, bundleIntegrationMode == BundleIntegrationMode.None, out position, out orientation, out velocity, out var gatheredInertia); + if (bundleIntegrationMode != BundleIntegrationMode.None) { - case BundleIntegrationMode.All: - { - IntegrateUniformly(bodies, ref integratorCallbacks, dt, workerIndex, ref bodyIndices, count, ref integrationMask, out position, out orientation, out velocity, out inertia); - } - break; - case BundleIntegrationMode.Partial: - { - //Note that if we take this codepath, the integration routine will reconstruct the world inertias from local inertia given the current pose. - //The changes to pose and velocity for integration inactive lanes will be masked out, so it'll just be identical to the world inertia if we had gathered it. - //Given that we're running the instructions in a bundle to build it, there's no reason to go out of our way to gather the world inertia. - bodies.GatherState(ref bodyIndices, count, false, out position, out orientation, out velocity, out var localInertia); - IntegratePoseAndVelocity(ref integratorCallbacks, ref bodyIndices, count, localInertia, dt, integrationMask, ref position, ref orientation, ref velocity, workerIndex, out inertia); - //The scatter will be able to ignore any lanes which have a zeroed integration mask. - bodies.ScatterPoseAndInertia(ref position, ref orientation, ref inertia, ref bodyIndices, count, ref integrationMask); - } - break; - default: - { - bodies.GatherState(ref bodyIndices, count, true, out position, out orientation, out velocity, out inertia); - } - break; + //Note that if we take this codepath, the integration routine will reconstruct the world inertias from local inertia given the current pose. + //The changes to pose and velocity for integration inactive lanes will be masked out, so it'll just be identical to the world inertia if we had gathered it. + //Given that we're running the instructions in a bundle to build it, there's no reason to go out of our way to gather the world inertia. + IntegratePoseAndVelocity(ref integratorCallbacks, ref bodyIndices, count, gatheredInertia, dt, ref position, ref orientation, ref velocity, workerIndex, out inertia); + //TODO: Worth checking if scatterposeandinertia benefits at all from type specialization. might very well not; it still has to do count masking... + if (bundleIntegrationMode == BundleIntegrationMode.All) + bodies.ScatterPoseAndInertia(ref position, ref orientation, ref inertia, ref bodyIndices, count, ref integrationMask); + else + bodies.ScatterPoseAndInertia(ref position, ref orientation, ref inertia, ref bodyIndices, count, ref integrationMask); + } + else + { + inertia = gatheredInertia; } } } @@ -592,8 +592,19 @@ public unsafe override void WarmStart2(ref wsvA, ref references.IndexA, count); - bodies.ScatterVelocities(ref wsvB, ref references.IndexB, count); + if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) + { + bodies.ScatterVelocities(ref wsvA, ref references.IndexA, count); + bodies.ScatterVelocities(ref wsvB, ref references.IndexB, count); + } + else + { + //This batch has some integrators, which means that every bundle is going to gather all velocities. + //(We don't make per-bundle determinations about this to avoid an extra branch and instruction complexity, and the difference is very small.) + bodies.ScatterVelocities(ref wsvA, ref references.IndexA, count); + bodies.ScatterVelocities(ref wsvB, ref references.IndexB, count); + } + } } @@ -658,7 +669,8 @@ public unsafe override void Prestep2(ref TypeBatch typeBatch, Bodies bodies, flo bodies.GatherState(ref references, count, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB); function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, dt, inverseDt, ref prestep, out projection); function.WarmStart(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); - bodies.ScatterVelocities(ref wsvA, ref wsvB, ref references, count); + bodies.ScatterVelocities(ref wsvA, ref references.IndexA, count); + bodies.ScatterVelocities(ref wsvB, ref references.IndexB, count); } } public override void JacobiPrestep2(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index 8d56a4978..4f6d7bd0a 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -676,15 +676,15 @@ internal unsafe static void CreateDeformable(Simulation simulation, in Vector3 p { ref var edge = ref edges[i]; var offset = vertices[edge.B] - vertices[edge.A]; - simulation.Solver.Add(vertexHandles[edge.A], vertexHandles[edge.B], - new Weld - { - LocalOffset = offset, - LocalOrientation = Quaternion.Identity, - SpringSettings = weldSpringiness - }); //simulation.Solver.Add(vertexHandles[edge.A], vertexHandles[edge.B], - // new CenterDistanceConstraint(offset.Length(), weldSpringiness)); + // new Weld + // { + // LocalOffset = offset, + // LocalOrientation = Quaternion.Identity, + // SpringSettings = weldSpringiness + // }); + simulation.Solver.Add(vertexHandles[edge.A], vertexHandles[edge.B], + new CenterDistanceConstraint(offset.Length(), weldSpringiness)); //simulation.Solver.Add(vertexHandles[edge.A], vertexHandles[edge.B], // new BallSocket { LocalOffsetA = offset * 0.5f, LocalOffsetB = offset * -0.5f, SpringSettings = weldSpringiness }); } diff --git a/Demos/Program.cs b/Demos/Program.cs index 6c769ece5..962deb171 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -19,7 +19,7 @@ static void Main(string[] args) { content = ContentArchive.Load(stream); } - HeadlessTest.Test(content, 4, 32, 512); + HeadlessTest.Test(content, 16, 32, 512); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); From 509b4c2da2f6fbc698b9a6e2b2deb91f204b1014 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 18 Jul 2021 18:26:14 -0500 Subject: [PATCH 119/947] Simplified pose scattering. Type specialization wasn't doing anything useful for it. --- BepuPhysics/Bodies_GatherScatter.cs | 65 +++++-------------- .../Constraints/TwoBodyTypeProcessor.cs | 8 +-- Demos/Demos/NewtDemo.cs | 16 ++--- 3 files changed, 27 insertions(+), 62 deletions(-) diff --git a/BepuPhysics/Bodies_GatherScatter.cs b/BepuPhysics/Bodies_GatherScatter.cs index cbf9ec51a..5056f128e 100644 --- a/BepuPhysics/Bodies_GatherScatter.cs +++ b/BepuPhysics/Bodies_GatherScatter.cs @@ -618,9 +618,8 @@ public void GatherState(ref FourBodyReferences references, int count, //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void ScatterPoseAndInertia( + public unsafe void ScatterPoseAndInertia( ref Vector3Wide position, ref QuaternionWide orientation, ref BodyInertiaWide inertia, ref Vector references, int count, ref Vector mask) - where TBatchIntegrationMode : struct, IBatchIntegrationMode { if (Avx.IsSupported && Vector.Count == 8) { @@ -654,29 +653,14 @@ public unsafe void ScatterPoseAndInertia( var maskPointer = (int*)Unsafe.AsPointer(ref mask); var indices = (int*)Unsafe.AsPointer(ref references); - if (typeof(TBatchIntegrationMode) == typeof(BatchShouldAlwaysIntegrate)) - { - //If we are in an 'always integrate' batch, then the mask is not guaranteed to mask out out-of-range lanes, since we don't otherwise need the mask. - Avx.StoreAligned((float*)(states + indices[0]), Avx.Permute2x128(o0, o1, 0 | (2 << 4))); - if (count > 1) Avx.StoreAligned((float*)(states + indices[1]), Avx.Permute2x128(o4, o5, 0 | (2 << 4))); - if (count > 2) Avx.StoreAligned((float*)(states + indices[2]), Avx.Permute2x128(o2, o3, 0 | (2 << 4))); - if (count > 3) Avx.StoreAligned((float*)(states + indices[3]), Avx.Permute2x128(o6, o7, 0 | (2 << 4))); - if (count > 4) Avx.StoreAligned((float*)(states + indices[4]), Avx.Permute2x128(o0, o1, 1 | (3 << 4))); - if (count > 5) Avx.StoreAligned((float*)(states + indices[5]), Avx.Permute2x128(o4, o5, 1 | (3 << 4))); - if (count > 6) Avx.StoreAligned((float*)(states + indices[6]), Avx.Permute2x128(o2, o3, 1 | (3 << 4))); - if (count > 7) Avx.StoreAligned((float*)(states + indices[7]), Avx.Permute2x128(o6, o7, 1 | (3 << 4))); - } - else - { - if (maskPointer[0] != 0) Avx.StoreAligned((float*)(states + indices[0]), Avx.Permute2x128(o0, o1, 0 | (2 << 4))); - if (maskPointer[1] != 0) Avx.StoreAligned((float*)(states + indices[1]), Avx.Permute2x128(o4, o5, 0 | (2 << 4))); - if (maskPointer[2] != 0) Avx.StoreAligned((float*)(states + indices[2]), Avx.Permute2x128(o2, o3, 0 | (2 << 4))); - if (maskPointer[3] != 0) Avx.StoreAligned((float*)(states + indices[3]), Avx.Permute2x128(o6, o7, 0 | (2 << 4))); - if (maskPointer[4] != 0) Avx.StoreAligned((float*)(states + indices[4]), Avx.Permute2x128(o0, o1, 1 | (3 << 4))); - if (maskPointer[5] != 0) Avx.StoreAligned((float*)(states + indices[5]), Avx.Permute2x128(o4, o5, 1 | (3 << 4))); - if (maskPointer[6] != 0) Avx.StoreAligned((float*)(states + indices[6]), Avx.Permute2x128(o2, o3, 1 | (3 << 4))); - if (maskPointer[7] != 0) Avx.StoreAligned((float*)(states + indices[7]), Avx.Permute2x128(o6, o7, 1 | (3 << 4))); - } + if (maskPointer[0] != 0) Avx.StoreAligned((float*)(states + indices[0]), Avx.Permute2x128(o0, o1, 0 | (2 << 4))); + if (maskPointer[1] != 0) Avx.StoreAligned((float*)(states + indices[1]), Avx.Permute2x128(o4, o5, 0 | (2 << 4))); + if (maskPointer[2] != 0) Avx.StoreAligned((float*)(states + indices[2]), Avx.Permute2x128(o2, o3, 0 | (2 << 4))); + if (maskPointer[3] != 0) Avx.StoreAligned((float*)(states + indices[3]), Avx.Permute2x128(o6, o7, 0 | (2 << 4))); + if (maskPointer[4] != 0) Avx.StoreAligned((float*)(states + indices[4]), Avx.Permute2x128(o0, o1, 1 | (3 << 4))); + if (maskPointer[5] != 0) Avx.StoreAligned((float*)(states + indices[5]), Avx.Permute2x128(o4, o5, 1 | (3 << 4))); + if (maskPointer[6] != 0) Avx.StoreAligned((float*)(states + indices[6]), Avx.Permute2x128(o2, o3, 1 | (3 << 4))); + if (maskPointer[7] != 0) Avx.StoreAligned((float*)(states + indices[7]), Avx.Permute2x128(o6, o7, 1 | (3 << 4))); } { var m0 = inertia.InverseInertiaTensor.XX.AsVector256(); @@ -708,29 +692,14 @@ public unsafe void ScatterPoseAndInertia( var maskPointer = (int*)Unsafe.AsPointer(ref mask); var indices = (int*)Unsafe.AsPointer(ref references); //Note the offset; we're scattering into the world inertias. - if (typeof(TBatchIntegrationMode) == typeof(BatchShouldAlwaysIntegrate)) - { - //If we are in an 'always integrate' batch, then the mask is not guaranteed to mask out out-of-range lanes, since we don't otherwise need the mask. - Avx.StoreAligned((float*)(states + indices[0]) + 24, Avx.Permute2x128(o0, o1, 0 | (2 << 4))); - if (count > 1) Avx.StoreAligned((float*)(states + indices[1]) + 24, Avx.Permute2x128(o4, o5, 0 | (2 << 4))); - if (count > 2) Avx.StoreAligned((float*)(states + indices[2]) + 24, Avx.Permute2x128(o2, o3, 0 | (2 << 4))); - if (count > 3) Avx.StoreAligned((float*)(states + indices[3]) + 24, Avx.Permute2x128(o6, o7, 0 | (2 << 4))); - if (count > 4) Avx.StoreAligned((float*)(states + indices[4]) + 24, Avx.Permute2x128(o0, o1, 1 | (3 << 4))); - if (count > 5) Avx.StoreAligned((float*)(states + indices[5]) + 24, Avx.Permute2x128(o4, o5, 1 | (3 << 4))); - if (count > 6) Avx.StoreAligned((float*)(states + indices[6]) + 24, Avx.Permute2x128(o2, o3, 1 | (3 << 4))); - if (count > 7) Avx.StoreAligned((float*)(states + indices[7]) + 24, Avx.Permute2x128(o6, o7, 1 | (3 << 4))); - } - else - { - if (maskPointer[0] != 0) Avx.StoreAligned((float*)(states + indices[0]) + 24, Avx.Permute2x128(o0, o1, 0 | (2 << 4))); - if (maskPointer[1] != 0) Avx.StoreAligned((float*)(states + indices[1]) + 24, Avx.Permute2x128(o4, o5, 0 | (2 << 4))); - if (maskPointer[2] != 0) Avx.StoreAligned((float*)(states + indices[2]) + 24, Avx.Permute2x128(o2, o3, 0 | (2 << 4))); - if (maskPointer[3] != 0) Avx.StoreAligned((float*)(states + indices[3]) + 24, Avx.Permute2x128(o6, o7, 0 | (2 << 4))); - if (maskPointer[4] != 0) Avx.StoreAligned((float*)(states + indices[4]) + 24, Avx.Permute2x128(o0, o1, 1 | (3 << 4))); - if (maskPointer[5] != 0) Avx.StoreAligned((float*)(states + indices[5]) + 24, Avx.Permute2x128(o4, o5, 1 | (3 << 4))); - if (maskPointer[6] != 0) Avx.StoreAligned((float*)(states + indices[6]) + 24, Avx.Permute2x128(o2, o3, 1 | (3 << 4))); - if (maskPointer[7] != 0) Avx.StoreAligned((float*)(states + indices[7]) + 24, Avx.Permute2x128(o6, o7, 1 | (3 << 4))); - } + if (maskPointer[0] != 0) Avx.StoreAligned((float*)(states + indices[0]) + 24, Avx.Permute2x128(o0, o1, 0 | (2 << 4))); + if (maskPointer[1] != 0) Avx.StoreAligned((float*)(states + indices[1]) + 24, Avx.Permute2x128(o4, o5, 0 | (2 << 4))); + if (maskPointer[2] != 0) Avx.StoreAligned((float*)(states + indices[2]) + 24, Avx.Permute2x128(o2, o3, 0 | (2 << 4))); + if (maskPointer[3] != 0) Avx.StoreAligned((float*)(states + indices[3]) + 24, Avx.Permute2x128(o6, o7, 0 | (2 << 4))); + if (maskPointer[4] != 0) Avx.StoreAligned((float*)(states + indices[4]) + 24, Avx.Permute2x128(o0, o1, 1 | (3 << 4))); + if (maskPointer[5] != 0) Avx.StoreAligned((float*)(states + indices[5]) + 24, Avx.Permute2x128(o4, o5, 1 | (3 << 4))); + if (maskPointer[6] != 0) Avx.StoreAligned((float*)(states + indices[6]) + 24, Avx.Permute2x128(o2, o3, 1 | (3 << 4))); + if (maskPointer[7] != 0) Avx.StoreAligned((float*)(states + indices[7]) + 24, Avx.Permute2x128(o6, o7, 1 | (3 << 4))); } } else diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 971a38903..5f488b9d0 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -500,7 +500,7 @@ private static unsafe void IntegrateUniformly( { bodies.GatherState(ref bodyIndices, count, false, out position, out orientation, out velocity, out var localInertia); IntegratePoseAndVelocity(ref integratorCallbacks, ref bodyIndices, count, localInertia, dt, ref position, ref orientation, ref velocity, workerIndex, out inertia); - bodies.ScatterPoseAndInertia(ref position, ref orientation, ref inertia, ref bodyIndices, count, ref integrationMask); + bodies.ScatterPoseAndInertia(ref position, ref orientation, ref inertia, ref bodyIndices, count, ref integrationMask); } //[MethodImpl(MethodImplOptions.AggressiveInlining)] public static unsafe void GatherAndIntegrate( @@ -537,11 +537,7 @@ public static unsafe void GatherAndIntegrate(ref position, ref orientation, ref inertia, ref bodyIndices, count, ref integrationMask); - else - bodies.ScatterPoseAndInertia(ref position, ref orientation, ref inertia, ref bodyIndices, count, ref integrationMask); + bodies.ScatterPoseAndInertia(ref position, ref orientation, ref inertia, ref bodyIndices, count, ref integrationMask); } else { diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index 4f6d7bd0a..8d56a4978 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -676,15 +676,15 @@ internal unsafe static void CreateDeformable(Simulation simulation, in Vector3 p { ref var edge = ref edges[i]; var offset = vertices[edge.B] - vertices[edge.A]; - //simulation.Solver.Add(vertexHandles[edge.A], vertexHandles[edge.B], - // new Weld - // { - // LocalOffset = offset, - // LocalOrientation = Quaternion.Identity, - // SpringSettings = weldSpringiness - // }); simulation.Solver.Add(vertexHandles[edge.A], vertexHandles[edge.B], - new CenterDistanceConstraint(offset.Length(), weldSpringiness)); + new Weld + { + LocalOffset = offset, + LocalOrientation = Quaternion.Identity, + SpringSettings = weldSpringiness + }); + //simulation.Solver.Add(vertexHandles[edge.A], vertexHandles[edge.B], + // new CenterDistanceConstraint(offset.Length(), weldSpringiness)); //simulation.Solver.Add(vertexHandles[edge.A], vertexHandles[edge.B], // new BallSocket { LocalOffsetA = offset * 0.5f, LocalOffsetB = offset * -0.5f, SpringSettings = weldSpringiness }); } From 8867cd9fc029e5f7e002fe2ff12a95d30f0c1f0e Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 19 Jul 2021 16:53:41 -0500 Subject: [PATCH 120/947] Tried scalar callbacks for API simplicity; alas, still measurably slower. --- .../Constraints/CenterDistanceConstraint.cs | 2 +- .../Constraints/TwoBodyTypeProcessor.cs | 31 +++++++++++++------ 2 files changed, 22 insertions(+), 11 deletions(-) diff --git a/BepuPhysics/Constraints/CenterDistanceConstraint.cs b/BepuPhysics/Constraints/CenterDistanceConstraint.cs index eed750ef6..65ed77ef8 100644 --- a/BepuPhysics/Constraints/CenterDistanceConstraint.cs +++ b/BepuPhysics/Constraints/CenterDistanceConstraint.cs @@ -136,7 +136,7 @@ static void ApplyImpulse(in Vector3Wide jacobianA, in Vector inverseMassA Vector3Wide.Subtract(b.Linear, negatedChangeB, out b.Linear); } - //[MethodImpl(MethodImplOptions.NoInlining)] + //[MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in CenterDistancePrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 5f488b9d0..362106ece 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -493,15 +493,6 @@ public static unsafe void IntegratePoseAndVelocity( integratorCallbacks.IntegrateVelocity(new ReadOnlySpan(Unsafe.AsPointer(ref bodyIndices), count), position, orientation, localInertia, new Vector(-1), workerIndex, new Vector(dt), ref velocity); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - private static unsafe void IntegrateUniformly( - Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, float dt, int workerIndex, ref Vector bodyIndices, int count, ref Vector integrationMask, - out Vector3Wide position, out QuaternionWide orientation, out BodyVelocityWide velocity, out BodyInertiaWide inertia) where TIntegratorCallbacks : struct, IPoseIntegratorCallbacks - { - bodies.GatherState(ref bodyIndices, count, false, out position, out orientation, out velocity, out var localInertia); - IntegratePoseAndVelocity(ref integratorCallbacks, ref bodyIndices, count, localInertia, dt, ref position, ref orientation, ref velocity, workerIndex, out inertia); - bodies.ScatterPoseAndInertia(ref position, ref orientation, ref inertia, ref bodyIndices, count, ref integrationMask); - } //[MethodImpl(MethodImplOptions.AggressiveInlining)] public static unsafe void GatherAndIntegrate( Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, ref Buffer integrationFlags, int bodyIndexInConstraint, float dt, int workerIndex, int bundleIndex, @@ -514,7 +505,16 @@ public static unsafe void GatherAndIntegrate(-1); - IntegrateUniformly(bodies, ref integratorCallbacks, dt, workerIndex, ref bodyIndices, count, ref integrationMask, out position, out orientation, out velocity, out inertia); + //var bodySpan = new ReadOnlySpan(Unsafe.AsPointer(ref bodyIndices), count); + //for (int i = 0; i < bodySpan.Length; ++i) + //{ + // var bodyIndex = bodySpan[i]; + // ref var state = ref bodies.ActiveSet.SolverStates[bodyIndex]; + // integratorCallbacks.IntegrateVelocity(bodyIndex, state.Motion.Pose, state.Inertia.Local, workerIndex, ref state.Motion.Velocity); + //} + bodies.GatherState(ref bodyIndices, count, false, out position, out orientation, out velocity, out var localInertia); + IntegratePoseAndVelocity(ref integratorCallbacks, ref bodyIndices, count, localInertia, dt, ref position, ref orientation, ref velocity, workerIndex, out inertia); + bodies.ScatterPoseAndInertia(ref position, ref orientation, ref inertia, ref bodyIndices, count, ref integrationMask); } else if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) { @@ -526,6 +526,17 @@ public static unsafe void GatherAndIntegrate(Unsafe.AsPointer(ref bodyIndices), count); + //var integrationMaskPointer = (int*)&integrationMask; + //for (int i = 0; i < bodySpan.Length; ++i) + //{ + // if (integrationMaskPointer[i] != 0) + // { + // var bodyIndex = bodySpan[i]; + // ref var state = ref bodies.ActiveSet.SolverStates[bodyIndex]; + // integratorCallbacks.IntegrateVelocity(bodyIndex, state.Motion.Pose, state.Inertia.Local, workerIndex, ref state.Motion.Velocity); + // } + //} //Note that this will gather world inertia if there is no integration in the bundle, but that it is guaranteed to load all motion state information. //This avoids complexity around later velocity scattering- we don't have to condition on whether the bundle is integrating. //In practice, since the access filters are only reducing instruction counts and not memory bandwidth, From a364479736b2f2c54aae7c4248261bbd265cb075 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 20 Jul 2021 15:35:24 -0500 Subject: [PATCH 121/947] First step of unconstrained integration post process. --- BepuPhysics/Bodies_GatherScatter.cs | 22 ++- .../Constraints/TwoBodyTypeProcessor.cs | 32 +--- BepuPhysics/PoseIntegrator.cs | 17 ++- BepuPhysics/Solver_SubsteppingSolve2.cs | 138 ++++++++++++++++-- BepuUtilities/BundleIndexing.cs | 27 +++- Demos/DemoCallbacks.cs | 10 ++ Demos/Demos/PlanetDemo.cs | 4 +- Demos/Demos/SimpleSelfContainedDemo.cs | 9 +- Demos/SpecializedTests/GyroscopeTestDemo.cs | 2 + 9 files changed, 211 insertions(+), 50 deletions(-) diff --git a/BepuPhysics/Bodies_GatherScatter.cs b/BepuPhysics/Bodies_GatherScatter.cs index 5056f128e..b933c4162 100644 --- a/BepuPhysics/Bodies_GatherScatter.cs +++ b/BepuPhysics/Bodies_GatherScatter.cs @@ -618,8 +618,8 @@ public void GatherState(ref FourBodyReferences references, int count, //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void ScatterPoseAndInertia( - ref Vector3Wide position, ref QuaternionWide orientation, ref BodyInertiaWide inertia, ref Vector references, int count, ref Vector mask) + public unsafe void ScatterPose( + ref Vector3Wide position, ref QuaternionWide orientation, ref Vector references, ref Vector mask) { if (Avx.IsSupported && Vector.Count == 8) { @@ -661,7 +661,21 @@ public unsafe void ScatterPoseAndInertia( if (maskPointer[5] != 0) Avx.StoreAligned((float*)(states + indices[5]), Avx.Permute2x128(o4, o5, 1 | (3 << 4))); if (maskPointer[6] != 0) Avx.StoreAligned((float*)(states + indices[6]), Avx.Permute2x128(o2, o3, 1 | (3 << 4))); if (maskPointer[7] != 0) Avx.StoreAligned((float*)(states + indices[7]), Avx.Permute2x128(o6, o7, 1 | (3 << 4))); - } + } + } + else + { + } + } + + + //[MethodImpl(MethodImplOptions.AggressiveInlining)] + public unsafe void ScatterInertia( + ref BodyInertiaWide inertia, ref Vector references, ref Vector mask) + { + if (Avx.IsSupported && Vector.Count == 8) + { + var states = ActiveSet.SolverStates.Memory; { var m0 = inertia.InverseInertiaTensor.XX.AsVector256(); var m1 = inertia.InverseInertiaTensor.YX.AsVector256(); @@ -708,8 +722,6 @@ public unsafe void ScatterPoseAndInertia( } - - //[MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe void ScatterVelocities(ref BodyVelocityWide sourceVelocities, ref Vector references, int count) where TAccessFilter : unmanaged, IBodyAccessFilter { diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 362106ece..e0d254b97 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -441,14 +441,14 @@ public static unsafe void IntegratePoseAndVelocity( PoseIntegration.Integrate(orientation, velocity.Angular, new Vector(dt * 0.5f), out newOrientation); QuaternionWide.ConditionalSelect(integrationMask, newOrientation, orientation, out orientation); PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, orientation, out inertia.InverseInertiaTensor); - PoseIntegration.IntegrateAngularVelocityConserveMomentum(previousOrientation, localInertia, inertia, ref velocity.Angular); + PoseIntegration.IntegrateAngularVelocityConserveMomentum(previousOrientation, localInertia.InverseInertiaTensor, inertia.InverseInertiaTensor, ref velocity.Angular); } else if (integratorCallbacks.AngularIntegrationMode == AngularIntegrationMode.ConserveMomentumWithGyroscopicTorque) { PoseIntegration.Integrate(orientation, velocity.Angular, new Vector(dt * 0.5f), out newOrientation); QuaternionWide.ConditionalSelect(integrationMask, newOrientation, orientation, out orientation); PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, orientation, out inertia.InverseInertiaTensor); - PoseIntegration.IntegrateAngularVelocityConserveMomentumWithGyroscopicTorque(orientation, localInertia, inertia, ref velocity.Angular, dt); + PoseIntegration.IntegrateAngularVelocityConserveMomentumWithGyroscopicTorque(orientation, localInertia.InverseInertiaTensor, ref velocity.Angular, dt); } else { @@ -477,13 +477,13 @@ public static unsafe void IntegratePoseAndVelocity( var previousOrientation = orientation; PoseIntegration.Integrate(orientation, velocity.Angular, new Vector(dt * 0.5f), out orientation); PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, orientation, out inertia.InverseInertiaTensor); - PoseIntegration.IntegrateAngularVelocityConserveMomentum(previousOrientation, localInertia, inertia, ref velocity.Angular); + PoseIntegration.IntegrateAngularVelocityConserveMomentum(previousOrientation, localInertia.InverseInertiaTensor, inertia.InverseInertiaTensor, ref velocity.Angular); } else if (integratorCallbacks.AngularIntegrationMode == AngularIntegrationMode.ConserveMomentumWithGyroscopicTorque) { PoseIntegration.Integrate(orientation, velocity.Angular, new Vector(dt * 0.5f), out orientation); PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, orientation, out inertia.InverseInertiaTensor); - PoseIntegration.IntegrateAngularVelocityConserveMomentumWithGyroscopicTorque(orientation, localInertia, inertia, ref velocity.Angular, dt); + PoseIntegration.IntegrateAngularVelocityConserveMomentumWithGyroscopicTorque(orientation, localInertia.InverseInertiaTensor, ref velocity.Angular, dt); } else { @@ -505,16 +505,10 @@ public static unsafe void GatherAndIntegrate(-1); - //var bodySpan = new ReadOnlySpan(Unsafe.AsPointer(ref bodyIndices), count); - //for (int i = 0; i < bodySpan.Length; ++i) - //{ - // var bodyIndex = bodySpan[i]; - // ref var state = ref bodies.ActiveSet.SolverStates[bodyIndex]; - // integratorCallbacks.IntegrateVelocity(bodyIndex, state.Motion.Pose, state.Inertia.Local, workerIndex, ref state.Motion.Velocity); - //} bodies.GatherState(ref bodyIndices, count, false, out position, out orientation, out velocity, out var localInertia); IntegratePoseAndVelocity(ref integratorCallbacks, ref bodyIndices, count, localInertia, dt, ref position, ref orientation, ref velocity, workerIndex, out inertia); - bodies.ScatterPoseAndInertia(ref position, ref orientation, ref inertia, ref bodyIndices, count, ref integrationMask); + bodies.ScatterPose(ref position, ref orientation, ref bodyIndices, ref integrationMask); + bodies.ScatterInertia(ref inertia, ref bodyIndices, ref integrationMask); } else if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) { @@ -526,17 +520,6 @@ public static unsafe void GatherAndIntegrate(Unsafe.AsPointer(ref bodyIndices), count); - //var integrationMaskPointer = (int*)&integrationMask; - //for (int i = 0; i < bodySpan.Length; ++i) - //{ - // if (integrationMaskPointer[i] != 0) - // { - // var bodyIndex = bodySpan[i]; - // ref var state = ref bodies.ActiveSet.SolverStates[bodyIndex]; - // integratorCallbacks.IntegrateVelocity(bodyIndex, state.Motion.Pose, state.Inertia.Local, workerIndex, ref state.Motion.Velocity); - // } - //} //Note that this will gather world inertia if there is no integration in the bundle, but that it is guaranteed to load all motion state information. //This avoids complexity around later velocity scattering- we don't have to condition on whether the bundle is integrating. //In practice, since the access filters are only reducing instruction counts and not memory bandwidth, @@ -548,7 +531,8 @@ public static unsafe void GatherAndIntegrate AngularIntegrationMode AngularIntegrationMode { get; } + /// + /// Gets whether the integrator should use only one step for unconstrained bodies when using a substepping solver. + /// If true, unconstrained bodies use a single step of length equal to the dt provided to Simulation.Timestep. + /// If false, unconstrained bodies will be integrated with the same number of substeps as the constrained bodies in the solver. + /// + bool AllowSubstepsForUnconstrainedBodies { get; } + /// /// Performs any required initialization logic after the Simulation instance has been constructed. /// @@ -169,22 +176,22 @@ public static void RotateInverseInertia(in Symmetric3x3Wide localInverseInertiaT } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void IntegrateAngularVelocityConserveMomentum(in QuaternionWide previousOrientation, in BodyInertiaWide localInertia, in BodyInertiaWide inertia, ref Vector3Wide angularVelocity) + public static void IntegrateAngularVelocityConserveMomentum(in QuaternionWide previousOrientation, in Symmetric3x3Wide localInverseInertia, in Symmetric3x3Wide worldInverseInertia, ref Vector3Wide angularVelocity) { //Note that this effectively recomputes the previous frame's inertia. There may not have been a previous inertia stored in the inertias buffer. //This just avoids the need for quite a bit of complexity around keeping the world inertias buffer updated with adds/removes/moves and other state changes that we can't easily track. //Also, even if it were cached, the memory bandwidth requirements of loading another inertia tensor would hurt multithreaded scaling enough to eliminate any performance advantage. Matrix3x3Wide.CreateFromQuaternion(previousOrientation, out var previousOrientationMatrix); Matrix3x3Wide.TransformByTransposedWithoutOverlap(angularVelocity, previousOrientationMatrix, out var localPreviousAngularVelocity); - Symmetric3x3Wide.Invert(localInertia.InverseInertiaTensor, out var localInertiaTensor); + Symmetric3x3Wide.Invert(localInverseInertia, out var localInertiaTensor); Symmetric3x3Wide.TransformWithoutOverlap(localPreviousAngularVelocity, localInertiaTensor, out var localAngularMomentum); Matrix3x3Wide.Transform(localAngularMomentum, previousOrientationMatrix, out var angularMomentum); - Symmetric3x3Wide.TransformWithoutOverlap(angularMomentum, inertia.InverseInertiaTensor, out angularVelocity); + Symmetric3x3Wide.TransformWithoutOverlap(angularMomentum, worldInverseInertia, out angularVelocity); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void IntegrateAngularVelocityConserveMomentumWithGyroscopicTorque( - in QuaternionWide orientation, in BodyInertiaWide localInertia, in BodyInertiaWide inertia, ref Vector3Wide angularVelocity, float dt) + in QuaternionWide orientation, in Symmetric3x3Wide localInverseInertia, ref Vector3Wide angularVelocity, float dt) { //Integrating the gyroscopic force explicitly can result in some instability, so we'll use an approximate implicit approach. //angularVelocity1 * inertia1 = angularVelocity0 * inertia1 + dt * ((angularVelocity1 * inertia1) x angularVelocity1) @@ -208,7 +215,7 @@ public static void IntegrateAngularVelocityConserveMomentumWithGyroscopicTorque( Matrix3x3Wide.CreateFromQuaternion(orientation, out var orientationMatrix); //Using localAngularVelocity0 as the first guess for localAngularVelocity1. Matrix3x3Wide.TransformByTransposedWithoutOverlap(angularVelocity, orientationMatrix, out var localAngularVelocity); - Symmetric3x3Wide.Invert(localInertia.InverseInertiaTensor, out var localInertiaTensor); + Symmetric3x3Wide.Invert(localInverseInertia, out var localInertiaTensor); Symmetric3x3Wide.TransformWithoutOverlap(localAngularVelocity, localInertiaTensor, out var localAngularMomentum); var dtWide = new Vector(dt); diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index 27873fbdf..210d706fb 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -253,6 +253,7 @@ void ConstraintIntegrationResponsibilitiesWorker(int workerIndex) /// Buffer> coarseBatchIntegrationResponsibilities; Action constraintIntegrationResponsibilitiesWorker; + IndexSet mergedConstrainedBodyHandles; int substepCount; public override unsafe void PrepareConstraintIntegrationResponsibilities(int substepCount, IThreadDispatcher threadDispatcher = null) @@ -310,12 +311,11 @@ public override unsafe void PrepareConstraintIntegrationResponsibilities(int sub //} pool.Take(batchReferencedHandles.Count, out bodiesFirstObservedInBatches); - IndexSet merged; //We don't have to consider the first batch, since we know ahead of time that the first batch will be the first time we see any bodies in it. //Just copy directly from the first batch into the merged to initialize it. - pool.Take((bodies.HandlePool.HighestPossiblyClaimedId + 63) / 64, out merged.Flags); - var copyLength = Math.Min(merged.Flags.Length, batchReferencedHandles[0].Flags.Length); - batchReferencedHandles[0].Flags.CopyTo(0, merged.Flags, 0, copyLength); + pool.Take((bodies.HandlePool.HighestPossiblyClaimedId + 63) / 64, out mergedConstrainedBodyHandles.Flags); + var copyLength = Math.Min(mergedConstrainedBodyHandles.Flags.Length, batchReferencedHandles[0].Flags.Length); + batchReferencedHandles[0].Flags.CopyTo(0, mergedConstrainedBodyHandles.Flags, 0, copyLength); batchReferencedHandles[0].Flags.Clear(copyLength, batchReferencedHandles[0].Flags.Length - copyLength); //Yup, we're just leaving the first slot unallocated to avoid having to offset indices all over the place. Slight wonk, but not a big deal. @@ -324,7 +324,7 @@ public override unsafe void PrepareConstraintIntegrationResponsibilities(int sub for (int batchIndex = 1; batchIndex < bodiesFirstObservedInBatches.Length; ++batchIndex) { ref var batchHandles = ref batchReferencedHandles[batchIndex]; - var bundleCount = Math.Min(merged.Flags.Length, batchHandles.Flags.Length); + var bundleCount = Math.Min(mergedConstrainedBodyHandles.Flags.Length, batchHandles.Flags.Length); //Note that we bypass the constructor to avoid zeroing unnecessarily. Every bundle will be fully assigned. pool.Take(bundleCount, out bodiesFirstObservedInBatches[batchIndex].Flags); } @@ -334,16 +334,16 @@ public override unsafe void PrepareConstraintIntegrationResponsibilities(int sub { ref var batchHandles = ref batchReferencedHandles[batchIndex]; ref var firstObservedInBatch = ref bodiesFirstObservedInBatches[batchIndex]; - var flagBundleCount = Math.Min(merged.Flags.Length, batchHandles.Flags.Length); + var flagBundleCount = Math.Min(mergedConstrainedBodyHandles.Flags.Length, batchHandles.Flags.Length); if (Avx2.IsSupported) { var avxBundleCount = flagBundleCount / 4; var horizontalAvxMerge = Vector256.Zero; for (int avxBundleIndex = 0; avxBundleIndex < avxBundleCount; ++avxBundleIndex) { - var mergeBundle = ((Vector256*)merged.Flags.Memory)[avxBundleIndex]; + var mergeBundle = ((Vector256*)mergedConstrainedBodyHandles.Flags.Memory)[avxBundleIndex]; var batchBundle = ((Vector256*)batchHandles.Flags.Memory)[avxBundleIndex]; - ((Vector256*)merged.Flags.Memory)[avxBundleIndex] = Avx2.Or(mergeBundle, batchBundle); + ((Vector256*)mergedConstrainedBodyHandles.Flags.Memory)[avxBundleIndex] = Avx2.Or(mergeBundle, batchBundle); //If this batch contains a body, and the merged set does not, then it's the first batch that sees a body and it will have integration responsibility. var firstObservedBundle = Avx2.AndNot(mergeBundle, batchBundle); horizontalAvxMerge = Avx2.Or(firstObservedBundle, horizontalAvxMerge); @@ -355,9 +355,9 @@ public override unsafe void PrepareConstraintIntegrationResponsibilities(int sub //Cleanup loop. for (int flagBundleIndex = avxBundleCount * 4; flagBundleIndex < flagBundleCount; ++flagBundleIndex) { - var mergeBundle = merged.Flags[flagBundleIndex]; + var mergeBundle = mergedConstrainedBodyHandles.Flags[flagBundleIndex]; var batchBundle = batchHandles.Flags[flagBundleIndex]; - merged.Flags[flagBundleIndex] = mergeBundle | batchBundle; + mergedConstrainedBodyHandles.Flags[flagBundleIndex] = mergeBundle | batchBundle; //If this batch contains a body, and the merged set does not, then it's the first batch that sees a body and it will have integration responsibility. var firstObservedBundle = ~mergeBundle & batchBundle; horizontalMerge |= firstObservedBundle; @@ -370,9 +370,9 @@ public override unsafe void PrepareConstraintIntegrationResponsibilities(int sub ulong horizontalMerge = 0; for (int flagBundleIndex = 0; flagBundleIndex < flagBundleCount; ++flagBundleIndex) { - var mergeBundle = merged.Flags[flagBundleIndex]; + var mergeBundle = mergedConstrainedBodyHandles.Flags[flagBundleIndex]; var batchBundle = batchHandles.Flags[flagBundleIndex]; - merged.Flags[flagBundleIndex] = mergeBundle | batchBundle; + mergedConstrainedBodyHandles.Flags[flagBundleIndex] = mergeBundle | batchBundle; //If this batch contains a body, and the merged set does not, then it's the first batch that sees a body and it will have integration responsibility. var firstObservedBundle = ~mergeBundle & batchBundle; horizontalMerge |= firstObservedBundle; @@ -491,7 +491,6 @@ public override unsafe void PrepareConstraintIntegrationResponsibilities(int sub // } //} - merged.Dispose(pool); Debug.Assert(!bodiesFirstObservedInBatches[0].Flags.Allocated, "Remember, we're assuming we're just leaving the first batch's slot empty to avoid indexing complexity."); for (int batchIndex = 1; batchIndex < bodiesFirstObservedInBatches.Length; ++batchIndex) { @@ -519,6 +518,119 @@ public override void DisposeConstraintIntegrationResponsibilities() } pool.Return(ref integrationFlags); pool.Return(ref coarseBatchIntegrationResponsibilities); + mergedConstrainedBodyHandles.Dispose(pool); + + } + + unsafe void IntegratePoses(int bundleStartIndex, int bundleEndIndex, float dt, float substepDt, int substepCount, int workerIndex) + { + var bodyCount = bodies.ActiveSet.Count; + var bundleCount = BundleIndexing.GetBundleCount(bodyCount); + var bundleDt = new Vector(dt); + var bundleSubstepDt = new Vector(substepDt); + int* unconstrainedMaskPointer = stackalloc int[Vector.Count]; + int* bodyIndicesPointer = stackalloc int[Vector.Count]; + for (int i = bundleStartIndex; i < bundleEndIndex; ++i) + { + var bundleBaseIndex = i * Vector.Count; + var countInBundle = Math.Min(bodyCount - bundleBaseIndex, Vector.Count); + //This is executed at the end of the frame, after all constraints are complete. + //It covers both constrained and unconstrained bodies. + //There is no need to write world inertia, since the solver is done. + //Bodies that are unconstrained should undergo velocity callbacks, velocity integration, and pose integration. + //Unconstrained bodies can optionally perform a single step for the whole timestep, or do multiple steps to match the integration behavior of constrained bodies. + //Bodies that are constrained should only undergo one substep of pose integration. + bool anyBodyInBundleIsUnconstrained = false; + for (int innerIndex = 0; innerIndex < countInBundle; ++innerIndex) + { + bodyIndicesPointer[innerIndex] = bundleBaseIndex + innerIndex; + var bodyHandle = bodies.ActiveSet.IndexToHandle[bundleBaseIndex + innerIndex].Value; + //Note the use of the solver-merged body handles set. In principle, you could check the body constraints list- if it's empty, then you know all you need to know. + //The merged set is preferred here just for the sake of less memory bandwidth. + if (mergedConstrainedBodyHandles.Contains(bodyHandle)) + { + unconstrainedMaskPointer[innerIndex] = 0; + } + else + { + unconstrainedMaskPointer[innerIndex] = -1; + anyBodyInBundleIsUnconstrained = true; + } + } + + var unconstrainedMask = Unsafe.AsRef>(unconstrainedMaskPointer); + var bundleEffectiveDt = Vector.ConditionalSelect(unconstrainedMask, bundleDt, bundleSubstepDt); + var halfDt = bundleEffectiveDt * new Vector(0.5f); + + var bodyIndices = Unsafe.AsRef>(bodyIndicesPointer); + bodies.GatherState(ref bodyIndices, countInBundle, false, out var position, out var orientation, out var velocity, out var localInertia); + if (anyBodyInBundleIsUnconstrained) + { + int integrationStepCount; + if (PoseIntegrator.Callbacks.AllowSubstepsForUnconstrainedBodies) + { + integrationStepCount = substepCount; + } + else + { + integrationStepCount = 1; + } + for (int stepIndex = 0; stepIndex < integrationStepCount; ++stepIndex) + { + //Note that the following integrates velocities, then poses. + var previousVelocity = velocity; + PoseIntegrator.Callbacks.IntegrateVelocity(new ReadOnlySpan(Unsafe.AsPointer(ref bodyIndices), countInBundle), position, orientation, localInertia, unconstrainedMask, workerIndex, bundleEffectiveDt, ref velocity); + //It would be annoying to make the user handle masking velocity writes to inactive lanes, so we handle it internally. + Vector3Wide.ConditionalSelect(unconstrainedMask, velocity.Linear, previousVelocity.Linear, out velocity.Linear); + Vector3Wide.ConditionalSelect(unconstrainedMask, velocity.Angular, previousVelocity.Angular, out velocity.Angular); + + position += velocity.Linear * bundleEffectiveDt; + + //(Note that the constraints in the embedded substepper integrate pose, then velocity- this is because the first substep only integrates velocity, + //so in reality, the full loop for constrained bodies with 3 substeps looks like: + //(velocity -> solve) -> (pose -> velocity -> solve) -> (pose -> velocity -> solve) -> pose + //For unconstrained bodies, it's a tight loop of just: + //(velocity -> pose) -> (velocity -> pose) -> (velocity -> pose) + if (PoseIntegrator.Callbacks.AngularIntegrationMode == AngularIntegrationMode.ConserveMomentum) + { + var previousOrientation = orientation; + PoseIntegration.Integrate(orientation, velocity.Angular, halfDt, out orientation); + PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, orientation, out var inverseInertiaTensor); + PoseIntegration.IntegrateAngularVelocityConserveMomentum(previousOrientation, localInertia.InverseInertiaTensor, inverseInertiaTensor, ref velocity.Angular); + } + else if (PoseIntegrator.Callbacks.AngularIntegrationMode == AngularIntegrationMode.ConserveMomentumWithGyroscopicTorque) + { + PoseIntegration.Integrate(orientation, velocity.Angular, halfDt, out orientation); + PoseIntegration.IntegrateAngularVelocityConserveMomentumWithGyroscopicTorque(orientation, localInertia.InverseInertiaTensor, ref velocity.Angular, dt); + } + else + { + PoseIntegration.Integrate(orientation, velocity.Angular, halfDt, out orientation); + } + var integratePoseMask = BundleIndexing.CreateMaskForCountInBundle(countInBundle); + if (PoseIntegrator.Callbacks.AllowSubstepsForUnconstrainedBodies) + { + if (stepIndex > 0) + { + //Only the first substep should integrate poses for the constrained bodies, so mask them out for later substeps. + integratePoseMask = Vector.BitwiseAnd(integratePoseMask, unconstrainedMask); + } + } + bodies.ScatterPose(ref position, ref orientation, ref bodyIndices, ref integratePoseMask); + //We already masked the velocities above, so scattering them doesn't need its own mask. + bodies.ScatterVelocities(ref velocity, ref bodyIndices, countInBundle); + } + } + else + { + //All bodies are constrained, so we do not need to do any kind of velocity integration. + //TODO: while vector gather for multiple substeps can make some sense, it seems unlikely that doing a full gather + scatter is justified for single step pose integration. + PoseIntegration.Integrate(orientation, velocity.Angular, halfDt, out orientation); + position += velocity.Linear * bundleEffectiveDt; + var integratePoseMask = BundleIndexing.CreateMaskForCountInBundle(countInBundle); + bodies.ScatterPose(ref position, ref orientation, ref bodyIndices, ref integratePoseMask); + } + } } public override void SolveStep2(float totalDt, IThreadDispatcher threadDispatcher = null) diff --git a/BepuUtilities/BundleIndexing.cs b/BepuUtilities/BundleIndexing.cs index 3ae5b0341..4122ae9c4 100644 --- a/BepuUtilities/BundleIndexing.cs +++ b/BepuUtilities/BundleIndexing.cs @@ -2,9 +2,11 @@ using System.Numerics; using System.Runtime.CompilerServices; using System.Runtime.InteropServices; +using System.Runtime.Intrinsics; +using System.Runtime.Intrinsics.X86; namespace BepuUtilities -{ +{ /// /// Some helpers for indexing into vector bundles. /// @@ -57,5 +59,28 @@ public static int GetBundleCount(int elementCount) { return (elementCount + VectorMask) >> VectorShift; } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static unsafe Vector CreateMaskForCountInBundle(int countInBundle) + { + if (Avx.IsSupported && Vector.Count == 8) + { + return Avx.CompareGreaterThan(Vector256.Create((float)countInBundle), Vector256.Create(0f, 1f, 2f, 3f, 4f, 5f, 6f, 7f)).AsInt32().AsVector(); + } + else if (Sse.IsSupported && Vector.Count == 4) + { + return Sse.CompareGreaterThan(Vector128.Create((float)countInBundle), Vector128.Create(0f, 1f, 2f, 3f)).AsInt32().AsVector(); + } + else + { + Vector mask; + var toReturnPointer = (int*)&mask; + for (int i = 0; i < Vector.Count; ++i) + { + toReturnPointer[i] = i < countInBundle ? -1 : 0; + } + return mask; + } + } } } diff --git a/Demos/DemoCallbacks.cs b/Demos/DemoCallbacks.cs index 40d3d2d1f..0d744b440 100644 --- a/Demos/DemoCallbacks.cs +++ b/Demos/DemoCallbacks.cs @@ -29,8 +29,18 @@ public struct DemoPoseIntegratorCallbacks : IPoseIntegratorCallbacks float linearDampingDt; float angularDampingDt; + /// + /// Gets how the pose integrator should handle angular velocity integration. + /// public readonly AngularIntegrationMode AngularIntegrationMode => AngularIntegrationMode.Nonconserving; + /// + /// Gets whether the integrator should use substepping for unconstrained bodies when using a substepping solver. + /// If true, unconstrained bodies will be integrated with the same number of substeps as the constrained bodies in the solver. + /// If false, unconstrained bodies use a single step of length equal to the dt provided to Simulation.Timestep. + /// + public readonly bool AllowSubstepsForUnconstrainedBodies => false; + public void Initialize(Simulation simulation) { //In this demo, we don't need to initialize anything. diff --git a/Demos/Demos/PlanetDemo.cs b/Demos/Demos/PlanetDemo.cs index 12685f5d1..d6724c509 100644 --- a/Demos/Demos/PlanetDemo.cs +++ b/Demos/Demos/PlanetDemo.cs @@ -27,7 +27,9 @@ struct PlanetaryGravityCallbacks : IPoseIntegratorCallbacks public float Gravity; float gravityDt; - public AngularIntegrationMode AngularIntegrationMode => AngularIntegrationMode.Nonconserving; + public readonly AngularIntegrationMode AngularIntegrationMode => AngularIntegrationMode.Nonconserving; + + public readonly bool AllowSubstepsForUnconstrainedBodies => false; public void Initialize(Simulation simulation) { diff --git a/Demos/Demos/SimpleSelfContainedDemo.cs b/Demos/Demos/SimpleSelfContainedDemo.cs index 59e53db22..635b038c1 100644 --- a/Demos/Demos/SimpleSelfContainedDemo.cs +++ b/Demos/Demos/SimpleSelfContainedDemo.cs @@ -140,7 +140,14 @@ public void Initialize(Simulation simulation) /// /// Gets how the pose integrator should handle angular velocity integration. /// - public AngularIntegrationMode AngularIntegrationMode => AngularIntegrationMode.Nonconserving; //Don't care about fidelity in this demo! + public readonly AngularIntegrationMode AngularIntegrationMode => AngularIntegrationMode.Nonconserving; + + /// + /// Gets whether the integrator should use substepping for unconstrained bodies when using a substepping solver. + /// If true, unconstrained bodies will be integrated with the same number of substeps as the constrained bodies in the solver. + /// If false, unconstrained bodies use a single step of length equal to the dt provided to Simulation.Timestep. + /// + public readonly bool AllowSubstepsForUnconstrainedBodies => false; public PoseIntegratorCallbacks(Vector3 gravity) : this() { diff --git a/Demos/SpecializedTests/GyroscopeTestDemo.cs b/Demos/SpecializedTests/GyroscopeTestDemo.cs index 95ed8f4a7..3309fe9f3 100644 --- a/Demos/SpecializedTests/GyroscopeTestDemo.cs +++ b/Demos/SpecializedTests/GyroscopeTestDemo.cs @@ -19,6 +19,8 @@ struct GyroscopicIntegratorCallbacks : IPoseIntegratorCallbacks //Unless your simulation requires the extra fidelity, there's not much reason to spend the extra time on it. DemoPoseIntegratorCallbacks innerCallbacks; public readonly AngularIntegrationMode AngularIntegrationMode => AngularIntegrationMode.ConserveMomentumWithGyroscopicTorque; + //For this demo, we'll allow substepping for unconstrained bodies. + public readonly bool AllowSubstepsForUnconstrainedBodies => true; public void Initialize(Simulation simulation) { From 42bd3732b01c27bf5f9a42b3dcf3044caa4cc900 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 20 Jul 2021 18:32:55 -0500 Subject: [PATCH 122/947] Final integration pass mostly implemented. Technically still redundant with constrained bodies first pose integration step. --- .../EmbeddedSubsteppingTimestepper2.cs | 32 +-- BepuPhysics/PoseIntegrator.cs | 205 ++++++++++++++++-- BepuPhysics/Solver_SubsteppingSolve2.cs | 120 +--------- Demos/Demos/NewtDemo.cs | 2 + Demos/Program.cs | 2 +- 5 files changed, 202 insertions(+), 159 deletions(-) diff --git a/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs b/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs index 223571260..8277eedab 100644 --- a/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs +++ b/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs @@ -31,29 +31,9 @@ public class EmbeddedSubsteppingTimestepper2 : ITimestepper /// public event TimestepperStageHandler CollisionsDetected; /// - /// Fires at the beginning of a substep. + /// Fires after the solver executes and before the final integration step. /// - public event TimestepperSubstepStageHandler SubstepStarted; - /// - /// Fires after contact constraints are incrementally updated at the beginning of substeps after the first and before velocities are integrated. - /// - public event TimestepperSubstepStageHandler ContactConstraintsUpdatedForSubstep; - /// - /// Fires after bodies have their velocities integrated and before the solver executes. - /// - public event TimestepperSubstepStageHandler VelocitiesIntegrated; - /// - /// Fires after the solver executes and before body poses are integrated. - /// - public event TimestepperSubstepStageHandler ConstraintsSolved; - /// - /// Fires after bodies have their poses integrated and before the substep ends. - /// - public event TimestepperSubstepStageHandler PosesIntegrated; - /// - /// Fires at the end of a substep. - /// - public event TimestepperSubstepStageHandler SubstepEnded; + public event TimestepperStageHandler ConstraintsSolved; /// /// Fires after all substeps are finished executing and before data structures are incrementally optimized. /// @@ -77,12 +57,16 @@ public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDi Debug.Assert(SubstepCount >= 0, "Substep count should be positive."); - simulation.Solver.PrepareConstraintIntegrationResponsibilities(SubstepCount, threadDispatcher); + var constrainedBodySet = simulation.Solver.PrepareConstraintIntegrationResponsibilities(SubstepCount, threadDispatcher); simulation.Profiler.Start(simulation.Solver); simulation.Solver.SolveStep2(dt, threadDispatcher); simulation.Profiler.End(simulation.Solver); - SubstepsComplete?.Invoke(dt, threadDispatcher); + ConstraintsSolved?.Invoke(dt, threadDispatcher); + simulation.Profiler.Start(simulation.PoseIntegrator); + simulation.PoseIntegrator.IntegrateAfterSubstepping(constrainedBodySet, dt, SubstepCount, threadDispatcher); + simulation.Profiler.End(simulation.PoseIntegrator); simulation.Solver.DisposeConstraintIntegrationResponsibilities(); + SubstepsComplete?.Invoke(dt, threadDispatcher); simulation.IncrementallyOptimizeDataStructures(threadDispatcher); } diff --git a/BepuPhysics/PoseIntegrator.cs b/BepuPhysics/PoseIntegrator.cs index 729be4f36..c3ee605e0 100644 --- a/BepuPhysics/PoseIntegrator.cs +++ b/BepuPhysics/PoseIntegrator.cs @@ -7,6 +7,8 @@ using System.Numerics; using System.Runtime.CompilerServices; using System.Threading; +using BepuUtilities.Collections; +using BepuPhysics.Constraints; namespace BepuPhysics { @@ -17,6 +19,7 @@ public interface IPoseIntegrator void IntegrateVelocitiesBoundsAndInertias(float dt, BufferPool pool, IThreadDispatcher threadDispatcher = null); void IntegrateVelocitiesAndUpdateInertias(float dt, BufferPool pool, IThreadDispatcher threadDispatcher = null); void IntegratePoses(float dt, BufferPool pool, IThreadDispatcher threadDispatcher = null); + void IntegrateAfterSubstepping(IndexSet constrainedBodies, float dt, int substepCount, IThreadDispatcher threadDispatcher = null); } /// @@ -90,7 +93,7 @@ public interface IPoseIntegratorCallbacks /// Durations to integrate the velocity over. Can vary over lanes. /// Velocity of bodies in the bundle. Any changes to lanes which are not active by the integrationMask will be discarded. void IntegrateVelocity( - ReadOnlySpan bodyIndices, in Vector3Wide position, in QuaternionWide orientation, in BodyInertiaWide localInertia, + ReadOnlySpan bodyIndices, in Vector3Wide position, in QuaternionWide orientation, in BodyInertiaWide localInertia, in Vector integrationMask, int workerIndex, in Vector dt, ref BodyVelocityWide velocity); } @@ -143,7 +146,7 @@ public static void Integrate(in Quaternion orientation, in Vector3 angularVeloci } } - [MethodImpl(MethodImplOptions.AggressiveInlining)] + //[MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Integrate(in QuaternionWide start, in Vector3Wide angularVelocity, in Vector halfDt, out QuaternionWide integrated) { Vector3Wide.Length(angularVelocity, out var speed); @@ -277,6 +280,7 @@ public PoseIntegrator(Bodies bodies, Shapes shapes, BroadPhase broadPhase, TCall integrateVelocitiesBoundsAndInertiasWorker = IntegrateVelocitiesBoundsAndInertiasWorker; integrateVelocitiesWorker = IntegrateVelocitiesWorker; integratePosesWorker = IntegratePosesWorker; + integrateAfterSubsteppingWorker = IntegrateAfterSubsteppingWorker; } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -541,7 +545,8 @@ unsafe void IntegratePoses(int startIndex, int endIndex, float dt, int workerInd float cachedDt; - int bodiesPerJob; + int jobSize; + int substepCount; IThreadDispatcher threadDispatcher; //Note that we aren't using a very cache-friendly work distribution here. @@ -549,7 +554,7 @@ unsafe void IntegratePoses(int startIndex, int endIndex, float dt, int workerInd //If this turns out to be false, this could be swapped over to a system similar to the solver- //preschedule offset regions for each worker to allow each one to consume a contiguous region before workstealing. int availableJobCount; - bool TryGetJob(int bodyCount, out int start, out int exclusiveEnd) + bool TryGetJob(int maximumJobInterval, out int start, out int exclusiveEnd) { var jobIndex = Interlocked.Decrement(ref availableJobCount); if (jobIndex < 0) @@ -558,10 +563,10 @@ bool TryGetJob(int bodyCount, out int start, out int exclusiveEnd) exclusiveEnd = 0; return false; } - start = jobIndex * bodiesPerJob; - exclusiveEnd = start + bodiesPerJob; - if (exclusiveEnd > bodyCount) - exclusiveEnd = bodyCount; + start = jobIndex * jobSize; + exclusiveEnd = start + jobSize; + if (exclusiveEnd > maximumJobInterval) + exclusiveEnd = maximumJobInterval; Debug.Assert(exclusiveEnd > start, "Jobs that would involve bundles beyond the body count should not be created."); return true; } @@ -618,16 +623,17 @@ void IntegratePosesWorker(int workerIndex) } } - void PrepareForMultithreadedExecution(float dt, int workerCount) + void PrepareForMultithreadedExecution(int loopIterationCount, float dt, int workerCount, int substepCount = 1) { cachedDt = dt; - const int jobsPerWorker = 4; + this.substepCount = substepCount; + const int jobsPerWorker = 2; var targetJobCount = workerCount * jobsPerWorker; - bodiesPerJob = bodies.ActiveSet.Count / targetJobCount; - if (bodiesPerJob == 0) - bodiesPerJob = 1; - availableJobCount = bodies.ActiveSet.Count / bodiesPerJob; - if (bodiesPerJob * availableJobCount < bodies.ActiveSet.Count) + jobSize = loopIterationCount / targetJobCount; + if (jobSize == 0) + jobSize = 1; + availableJobCount = loopIterationCount / jobSize; + if (jobSize * availableJobCount < loopIterationCount) ++availableJobCount; } @@ -648,7 +654,7 @@ public void IntegrateBodiesAndUpdateBoundingBoxes(float dt, BufferPool pool, ITh //Note that this bottleneck means the fact that we're working through bodies in a nonvectorized fashion (in favor of optimizing storage for solver access) is not a problem. - PrepareForMultithreadedExecution(dt, threadDispatcher.ThreadCount); + PrepareForMultithreadedExecution(bodies.ActiveSet.Count, dt, threadDispatcher.ThreadCount); this.threadDispatcher = threadDispatcher; threadDispatcher.DispatchWorkers(integrateBodiesAndUpdateBoundingBoxesWorker); this.threadDispatcher = null; @@ -669,7 +675,7 @@ public void PredictBoundingBoxes(float dt, BufferPool pool, IThreadDispatcher th Callbacks.PrepareForIntegration(dt); if (threadDispatcher != null) { - PrepareForMultithreadedExecution(dt, threadDispatcher.ThreadCount); + PrepareForMultithreadedExecution(bodies.ActiveSet.Count, dt, threadDispatcher.ThreadCount); this.threadDispatcher = threadDispatcher; threadDispatcher.DispatchWorkers(predictBoundingBoxesWorker); this.threadDispatcher = null; @@ -690,7 +696,7 @@ public void IntegrateVelocitiesBoundsAndInertias(float dt, BufferPool pool, IThr Callbacks.PrepareForIntegration(dt); if (threadDispatcher != null) { - PrepareForMultithreadedExecution(dt, threadDispatcher.ThreadCount); + PrepareForMultithreadedExecution(bodies.ActiveSet.Count, dt, threadDispatcher.ThreadCount); this.threadDispatcher = threadDispatcher; threadDispatcher.DispatchWorkers(integrateVelocitiesBoundsAndInertiasWorker); this.threadDispatcher = null; @@ -710,7 +716,7 @@ public void IntegrateVelocitiesAndUpdateInertias(float dt, BufferPool pool, IThr Callbacks.PrepareForIntegration(dt); if (threadDispatcher != null) { - PrepareForMultithreadedExecution(dt, threadDispatcher.ThreadCount); + PrepareForMultithreadedExecution(bodies.ActiveSet.Count, dt, threadDispatcher.ThreadCount); this.threadDispatcher = threadDispatcher; threadDispatcher.DispatchWorkers(integrateVelocitiesWorker); this.threadDispatcher = null; @@ -729,7 +735,7 @@ public void IntegratePoses(float dt, BufferPool pool, IThreadDispatcher threadDi Callbacks.PrepareForIntegration(dt); if (threadDispatcher != null) { - PrepareForMultithreadedExecution(dt, threadDispatcher.ThreadCount); + PrepareForMultithreadedExecution(bodies.ActiveSet.Count, dt, threadDispatcher.ThreadCount); this.threadDispatcher = threadDispatcher; threadDispatcher.DispatchWorkers(integratePosesWorker); this.threadDispatcher = null; @@ -739,5 +745,164 @@ public void IntegratePoses(float dt, BufferPool pool, IThreadDispatcher threadDi IntegratePoses(0, bodies.ActiveSet.Count, dt, 0); } } + + unsafe void IntegrateBundlesAfterSubstepping(ref IndexSet mergedConstrainedBodyHandles, int bundleStartIndex, int bundleEndIndex, float dt, float substepDt, int substepCount, int workerIndex) + { + var bodyCount = bodies.ActiveSet.Count; + var bundleCount = BundleIndexing.GetBundleCount(bodyCount); + var bundleDt = new Vector(dt); + var bundleSubstepDt = new Vector(substepDt); + + Vector unconstrainedMask; + Vector bodyIndices; + int* unconstrainedMaskPointer = (int*)&unconstrainedMask; + int* bodyIndicesPointer = (int*)&bodyIndices; + ref var callbacks = ref Callbacks; + ref var indexToHandle = ref bodies.ActiveSet.IndexToHandle; + + for (int i = bundleStartIndex; i < bundleEndIndex; ++i) + { + var bundleBaseIndex = i * Vector.Count; + var countInBundle = Math.Min(bodyCount - bundleBaseIndex, Vector.Count); + //This is executed at the end of the frame, after all constraints are complete. + //It covers both constrained and unconstrained bodies. + //There is no need to write world inertia, since the solver is done. + //Bodies that are unconstrained should undergo velocity callbacks, velocity integration, and pose integration. + //Unconstrained bodies can optionally perform a single step for the whole timestep, or do multiple steps to match the integration behavior of constrained bodies. + //Bodies that are constrained should only undergo one substep of pose integration. + bool anyBodyInBundleIsUnconstrained = false; + for (int innerIndex = 0; innerIndex < countInBundle; ++innerIndex) + { + var bodyIndex = bundleBaseIndex + innerIndex; + bodyIndicesPointer[innerIndex] = bodyIndex; + var bodyHandle = indexToHandle[bodyIndex].Value; + //Note the use of the solver-merged body handles set. In principle, you could check the body constraints list- if it's empty, then you know all you need to know. + //The merged set is preferred here just for the sake of less memory bandwidth. + if (mergedConstrainedBodyHandles.Contains(bodyHandle)) + { + unconstrainedMaskPointer[innerIndex] = 0; + } + else + { + unconstrainedMaskPointer[innerIndex] = -1; + anyBodyInBundleIsUnconstrained = true; + } + } + + + Vector bundleEffectiveDt; + if (callbacks.AllowSubstepsForUnconstrainedBodies) + { + bundleEffectiveDt = bundleSubstepDt; + } + else + { + bundleEffectiveDt = Vector.ConditionalSelect(unconstrainedMask, bundleDt, bundleSubstepDt); + } + var halfDt = bundleEffectiveDt * new Vector(0.5f); + bodies.GatherState(ref bodyIndices, countInBundle, false, out var position, out var orientation, out var velocity, out var localInertia); + if (anyBodyInBundleIsUnconstrained) + { + int integrationStepCount; + if (callbacks.AllowSubstepsForUnconstrainedBodies) + { + integrationStepCount = substepCount; + } + else + { + integrationStepCount = 1; + } + for (int stepIndex = 0; stepIndex < integrationStepCount; ++stepIndex) + { + //Note that the following integrates velocities, then poses. + var previousVelocity = velocity; + callbacks.IntegrateVelocity(new ReadOnlySpan(Unsafe.AsPointer(ref bodyIndices), countInBundle), position, orientation, localInertia, unconstrainedMask, workerIndex, bundleEffectiveDt, ref velocity); + //It would be annoying to make the user handle masking velocity writes to inactive lanes, so we handle it internally. + Vector3Wide.ConditionalSelect(unconstrainedMask, velocity.Linear, previousVelocity.Linear, out velocity.Linear); + Vector3Wide.ConditionalSelect(unconstrainedMask, velocity.Angular, previousVelocity.Angular, out velocity.Angular); + + position += velocity.Linear * bundleEffectiveDt; + + //(Note that the constraints in the embedded substepper integrate pose, then velocity- this is because the first substep only integrates velocity, + //so in reality, the full loop for constrained bodies with 3 substeps looks like: + //(velocity -> solve) -> (pose -> velocity -> solve) -> (pose -> velocity -> solve) -> pose + //For unconstrained bodies, it's a tight loop of just: + //(velocity -> pose) -> (velocity -> pose) -> (velocity -> pose) + if (callbacks.AngularIntegrationMode == AngularIntegrationMode.ConserveMomentum) + { + var previousOrientation = orientation; + PoseIntegration.Integrate(orientation, velocity.Angular, halfDt, out orientation); + PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, orientation, out var inverseInertiaTensor); + PoseIntegration.IntegrateAngularVelocityConserveMomentum(previousOrientation, localInertia.InverseInertiaTensor, inverseInertiaTensor, ref velocity.Angular); + } + else if (callbacks.AngularIntegrationMode == AngularIntegrationMode.ConserveMomentumWithGyroscopicTorque) + { + PoseIntegration.Integrate(orientation, velocity.Angular, halfDt, out orientation); + PoseIntegration.IntegrateAngularVelocityConserveMomentumWithGyroscopicTorque(orientation, localInertia.InverseInertiaTensor, ref velocity.Angular, dt); + } + else + { + PoseIntegration.Integrate(orientation, velocity.Angular, halfDt, out orientation); + } + var integratePoseMask = BundleIndexing.CreateMaskForCountInBundle(countInBundle); + if (callbacks.AllowSubstepsForUnconstrainedBodies) + { + if (stepIndex > 0) + { + //Only the first substep should integrate poses for the constrained bodies, so mask them out for later substeps. + integratePoseMask = Vector.BitwiseAnd(integratePoseMask, unconstrainedMask); + } + } + bodies.ScatterPose(ref position, ref orientation, ref bodyIndices, ref integratePoseMask); + //We already masked the velocities above, so scattering them doesn't need its own mask. + bodies.ScatterVelocities(ref velocity, ref bodyIndices, countInBundle); + } + } + else + { + //var halfDt = substepDt * 0.5f; + //for (int innerIndex = 0; innerIndex < countInBundle; ++innerIndex) + //{ + // ref var state = ref bodies.ActiveSet.SolverStates[bundleBaseIndex + innerIndex]; + // PoseIntegration.Integrate(state.Motion.Pose.Orientation, state.Motion.Velocity.Angular, halfDt, out state.Motion.Pose.Orientation); + // state.Motion.Pose.Position += substepDt * state.Motion.Velocity.Linear; + //} + //All bodies in the bundle are constrained, so we do not need to do any kind of velocity integration. + PoseIntegration.Integrate(orientation, velocity.Angular, halfDt, out orientation); + position += velocity.Linear * bundleEffectiveDt; + var integratePoseMask = BundleIndexing.CreateMaskForCountInBundle(countInBundle); + bodies.ScatterPose(ref position, ref orientation, ref bodyIndices, ref integratePoseMask); + } + } + } + + Action integrateAfterSubsteppingWorker; + IndexSet constrainedBodies; + private void IntegrateAfterSubsteppingWorker(int workerIndex) + { + var bundleCount = BundleIndexing.GetBundleCount(bodies.ActiveSet.Count); + var substepDt = cachedDt / substepCount; + while (TryGetJob(bundleCount, out var start, out var exclusiveEnd)) + { + IntegrateBundlesAfterSubstepping(ref constrainedBodies, start, exclusiveEnd, cachedDt, substepDt, substepCount, workerIndex); + } + } + + public void IntegrateAfterSubstepping(IndexSet constrainedBodies, float dt, int substepCount, IThreadDispatcher threadDispatcher) + { + if (threadDispatcher != null && threadDispatcher.ThreadCount > 1) + { + PrepareForMultithreadedExecution(BundleIndexing.GetBundleCount(bodies.ActiveSet.Count), dt, threadDispatcher.ThreadCount, substepCount); + this.constrainedBodies = constrainedBodies; + this.threadDispatcher = threadDispatcher; + threadDispatcher.DispatchWorkers(integrateAfterSubsteppingWorker); + this.threadDispatcher = null; + this.constrainedBodies = default; + } + else + { + IntegrateBundlesAfterSubstepping(ref constrainedBodies, 0, BundleIndexing.GetBundleCount(bodies.ActiveSet.Count), dt, dt / substepCount, substepCount, 0); + } + } } } diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index 210d706fb..63539a7c9 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -17,16 +17,18 @@ namespace BepuPhysics { public partial class Solver { - public virtual void PrepareConstraintIntegrationResponsibilities(int substepCount, IThreadDispatcher threadDispatcher = null) + public virtual IndexSet PrepareConstraintIntegrationResponsibilities(int substepCount, IThreadDispatcher threadDispatcher = null) { + throw new NotImplementedException(); } public virtual void DisposeConstraintIntegrationResponsibilities() { + throw new NotImplementedException(); } public virtual void SolveStep2(float dt, IThreadDispatcher threadDispatcher = null) { - + throw new NotImplementedException(); } } public class Solver : Solver where TIntegrationCallbacks : struct, IPoseIntegratorCallbacks @@ -256,7 +258,7 @@ void ConstraintIntegrationResponsibilitiesWorker(int workerIndex) IndexSet mergedConstrainedBodyHandles; int substepCount; - public override unsafe void PrepareConstraintIntegrationResponsibilities(int substepCount, IThreadDispatcher threadDispatcher = null) + public override unsafe IndexSet PrepareConstraintIntegrationResponsibilities(int substepCount, IThreadDispatcher threadDispatcher = null) { //TODO: we're caching it on a per call basis because we are still using the old substeppingtimestepper frame externally. Once we bite the bullet 100% on bundling, we can make it equivalent to IterationCount. this.substepCount = substepCount; @@ -497,6 +499,7 @@ public override unsafe void PrepareConstraintIntegrationResponsibilities(int sub bodiesFirstObservedInBatches[batchIndex].Dispose(pool); } pool.Return(ref bodiesFirstObservedInBatches); + return mergedConstrainedBodyHandles; } public override void DisposeConstraintIntegrationResponsibilities() { @@ -522,117 +525,6 @@ public override void DisposeConstraintIntegrationResponsibilities() } - unsafe void IntegratePoses(int bundleStartIndex, int bundleEndIndex, float dt, float substepDt, int substepCount, int workerIndex) - { - var bodyCount = bodies.ActiveSet.Count; - var bundleCount = BundleIndexing.GetBundleCount(bodyCount); - var bundleDt = new Vector(dt); - var bundleSubstepDt = new Vector(substepDt); - int* unconstrainedMaskPointer = stackalloc int[Vector.Count]; - int* bodyIndicesPointer = stackalloc int[Vector.Count]; - for (int i = bundleStartIndex; i < bundleEndIndex; ++i) - { - var bundleBaseIndex = i * Vector.Count; - var countInBundle = Math.Min(bodyCount - bundleBaseIndex, Vector.Count); - //This is executed at the end of the frame, after all constraints are complete. - //It covers both constrained and unconstrained bodies. - //There is no need to write world inertia, since the solver is done. - //Bodies that are unconstrained should undergo velocity callbacks, velocity integration, and pose integration. - //Unconstrained bodies can optionally perform a single step for the whole timestep, or do multiple steps to match the integration behavior of constrained bodies. - //Bodies that are constrained should only undergo one substep of pose integration. - bool anyBodyInBundleIsUnconstrained = false; - for (int innerIndex = 0; innerIndex < countInBundle; ++innerIndex) - { - bodyIndicesPointer[innerIndex] = bundleBaseIndex + innerIndex; - var bodyHandle = bodies.ActiveSet.IndexToHandle[bundleBaseIndex + innerIndex].Value; - //Note the use of the solver-merged body handles set. In principle, you could check the body constraints list- if it's empty, then you know all you need to know. - //The merged set is preferred here just for the sake of less memory bandwidth. - if (mergedConstrainedBodyHandles.Contains(bodyHandle)) - { - unconstrainedMaskPointer[innerIndex] = 0; - } - else - { - unconstrainedMaskPointer[innerIndex] = -1; - anyBodyInBundleIsUnconstrained = true; - } - } - - var unconstrainedMask = Unsafe.AsRef>(unconstrainedMaskPointer); - var bundleEffectiveDt = Vector.ConditionalSelect(unconstrainedMask, bundleDt, bundleSubstepDt); - var halfDt = bundleEffectiveDt * new Vector(0.5f); - - var bodyIndices = Unsafe.AsRef>(bodyIndicesPointer); - bodies.GatherState(ref bodyIndices, countInBundle, false, out var position, out var orientation, out var velocity, out var localInertia); - if (anyBodyInBundleIsUnconstrained) - { - int integrationStepCount; - if (PoseIntegrator.Callbacks.AllowSubstepsForUnconstrainedBodies) - { - integrationStepCount = substepCount; - } - else - { - integrationStepCount = 1; - } - for (int stepIndex = 0; stepIndex < integrationStepCount; ++stepIndex) - { - //Note that the following integrates velocities, then poses. - var previousVelocity = velocity; - PoseIntegrator.Callbacks.IntegrateVelocity(new ReadOnlySpan(Unsafe.AsPointer(ref bodyIndices), countInBundle), position, orientation, localInertia, unconstrainedMask, workerIndex, bundleEffectiveDt, ref velocity); - //It would be annoying to make the user handle masking velocity writes to inactive lanes, so we handle it internally. - Vector3Wide.ConditionalSelect(unconstrainedMask, velocity.Linear, previousVelocity.Linear, out velocity.Linear); - Vector3Wide.ConditionalSelect(unconstrainedMask, velocity.Angular, previousVelocity.Angular, out velocity.Angular); - - position += velocity.Linear * bundleEffectiveDt; - - //(Note that the constraints in the embedded substepper integrate pose, then velocity- this is because the first substep only integrates velocity, - //so in reality, the full loop for constrained bodies with 3 substeps looks like: - //(velocity -> solve) -> (pose -> velocity -> solve) -> (pose -> velocity -> solve) -> pose - //For unconstrained bodies, it's a tight loop of just: - //(velocity -> pose) -> (velocity -> pose) -> (velocity -> pose) - if (PoseIntegrator.Callbacks.AngularIntegrationMode == AngularIntegrationMode.ConserveMomentum) - { - var previousOrientation = orientation; - PoseIntegration.Integrate(orientation, velocity.Angular, halfDt, out orientation); - PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, orientation, out var inverseInertiaTensor); - PoseIntegration.IntegrateAngularVelocityConserveMomentum(previousOrientation, localInertia.InverseInertiaTensor, inverseInertiaTensor, ref velocity.Angular); - } - else if (PoseIntegrator.Callbacks.AngularIntegrationMode == AngularIntegrationMode.ConserveMomentumWithGyroscopicTorque) - { - PoseIntegration.Integrate(orientation, velocity.Angular, halfDt, out orientation); - PoseIntegration.IntegrateAngularVelocityConserveMomentumWithGyroscopicTorque(orientation, localInertia.InverseInertiaTensor, ref velocity.Angular, dt); - } - else - { - PoseIntegration.Integrate(orientation, velocity.Angular, halfDt, out orientation); - } - var integratePoseMask = BundleIndexing.CreateMaskForCountInBundle(countInBundle); - if (PoseIntegrator.Callbacks.AllowSubstepsForUnconstrainedBodies) - { - if (stepIndex > 0) - { - //Only the first substep should integrate poses for the constrained bodies, so mask them out for later substeps. - integratePoseMask = Vector.BitwiseAnd(integratePoseMask, unconstrainedMask); - } - } - bodies.ScatterPose(ref position, ref orientation, ref bodyIndices, ref integratePoseMask); - //We already masked the velocities above, so scattering them doesn't need its own mask. - bodies.ScatterVelocities(ref velocity, ref bodyIndices, countInBundle); - } - } - else - { - //All bodies are constrained, so we do not need to do any kind of velocity integration. - //TODO: while vector gather for multiple substeps can make some sense, it seems unlikely that doing a full gather + scatter is justified for single step pose integration. - PoseIntegration.Integrate(orientation, velocity.Angular, halfDt, out orientation); - position += velocity.Linear * bundleEffectiveDt; - var integratePoseMask = BundleIndexing.CreateMaskForCountInBundle(countInBundle); - bodies.ScatterPose(ref position, ref orientation, ref bodyIndices, ref integratePoseMask); - } - } - } - public override void SolveStep2(float totalDt, IThreadDispatcher threadDispatcher = null) { var substepDt = totalDt / substepCount; diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index 8d56a4978..d84e1c68d 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -846,6 +846,8 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) BufferPool.Return(ref cellVertexIndices); BufferPool.Return(ref tetrahedraVertexIndices); + //Simulation.Bodies.Add(BodyDescription.CreateDynamic(new RigidPose(new Vector3(0, 10, 0)), new BodyVelocity(default, new Vector3(1,2,3)), default, new CollidableDescription(Simulation.Shapes.Add(new Box(1, 2, 3)), 0.1f), new BodyActivityDescription(-1))); + //Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 100, -.5f), 10, Simulation.Shapes, new Sphere(5))); Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(1500, 1, 1500)), 0.1f))); diff --git a/Demos/Program.cs b/Demos/Program.cs index 962deb171..957d455c6 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -19,7 +19,7 @@ static void Main(string[] args) { content = ContentArchive.Load(stream); } - HeadlessTest.Test(content, 16, 32, 512); + //HeadlessTest.Test(content, 16, 32, 512); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); From 9eaabd64ca4fa107c2e409e59659e325f3384ebc Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 20 Jul 2021 19:29:43 -0500 Subject: [PATCH 123/947] Warm starts now avoid integrating pose on the first substep. --- .../Constraints/FourBodyTypeProcessor.cs | 2 +- .../Constraints/IBatchIntegrationMode.cs | 21 +++ .../Constraints/OneBodyTypeProcessor.cs | 2 +- .../Constraints/ThreeBodyTypeProcessor.cs | 2 +- .../Constraints/TwoBodyTypeProcessor.cs | 139 ++++++++++++++---- BepuPhysics/Constraints/TypeProcessor.cs | 5 +- BepuPhysics/Solver_SubsteppingSolve2.cs | 70 +++++---- Demos/Program.cs | 2 +- 8 files changed, 175 insertions(+), 68 deletions(-) diff --git a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs index 33d88de5c..9bff60817 100644 --- a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs @@ -328,7 +328,7 @@ public unsafe override void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodi } - public override void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks poseIntegratorCallbacks, + public override void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks poseIntegratorCallbacks, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { throw new NotImplementedException(); diff --git a/BepuPhysics/Constraints/IBatchIntegrationMode.cs b/BepuPhysics/Constraints/IBatchIntegrationMode.cs index e37c0844e..c06436ce0 100644 --- a/BepuPhysics/Constraints/IBatchIntegrationMode.cs +++ b/BepuPhysics/Constraints/IBatchIntegrationMode.cs @@ -34,4 +34,25 @@ public struct BatchShouldConditionallyIntegrate : IBatchIntegrationMode } + /// + /// Marks a type as determining whether pose integration should be performed on bodies within the constraint batch. + /// + public interface IBatchPoseIntegrationAllowed + { + + } + + /// + /// Marks a batch as integrating poses for any bodies with integration responsibility within the constraint batch. + /// + public struct BatchShouldIntegratePoses : IBatchPoseIntegrationAllowed + { + } + /// + /// Marks a batch as not integrating poses for any bodies within the constraint batch. + /// + public struct BatchShouldNotIntegratePoses : IBatchPoseIntegrationAllowed + { + } + } diff --git a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs index 211d16578..30da0859c 100644 --- a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs @@ -250,7 +250,7 @@ public unsafe override void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodi - public unsafe override void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks poseIntegratorCallbacks, + public unsafe override void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks poseIntegratorCallbacks, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); diff --git a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs index d93abc1b7..be37abd65 100644 --- a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs @@ -301,7 +301,7 @@ public unsafe override void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodi } } - public override void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks poseIntegratorCallbacks, + public override void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks poseIntegratorCallbacks, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { throw new NotImplementedException(); diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index e0d254b97..4a586b2ec 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -418,7 +418,7 @@ public static BundleIntegrationMode BundleShouldIntegrate(int bundleIndex, in In } [MethodImpl(MethodImplOptions.AggressiveInlining)] public static unsafe void IntegratePoseAndVelocity( - ref TIntegratorCallbacks integratorCallbacks, ref Vector bodyIndices, int count, in BodyInertiaWide localInertia, float dt, Vector integrationMask, + ref TIntegratorCallbacks integratorCallbacks, ref Vector bodyIndices, int count, in BodyInertiaWide localInertia, float dt, in Vector integrationMask, ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocityWide velocity, int workerIndex, out BodyInertiaWide inertia) @@ -493,56 +493,133 @@ public static unsafe void IntegratePoseAndVelocity( integratorCallbacks.IntegrateVelocity(new ReadOnlySpan(Unsafe.AsPointer(ref bodyIndices), count), position, orientation, localInertia, new Vector(-1), workerIndex, new Vector(dt), ref velocity); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static unsafe void IntegrateVelocity( + ref TIntegratorCallbacks integratorCallbacks, ref Vector bodyIndices, int count, in BodyInertiaWide localInertia, float dt, in Vector integrationMask, + in Vector3Wide position, in QuaternionWide orientation, ref BodyVelocityWide velocity, + int workerIndex, + out BodyInertiaWide inertia) + where TIntegratorCallbacks : struct, IPoseIntegratorCallbacks + where TBatchIntegrationMode : unmanaged, IBatchIntegrationMode + { + inertia.InverseMass = localInertia.InverseMass; + PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, orientation, out inertia.InverseInertiaTensor); + if (integratorCallbacks.AngularIntegrationMode == AngularIntegrationMode.ConserveMomentum) + { + //Yes, that's integrating backwards to get a previous orientation to convert to momentum. Yup, that's a bit janky. + PoseIntegration.Integrate(orientation, velocity.Angular, new Vector(dt * -0.5f), out var previousOrientation); + PoseIntegration.IntegrateAngularVelocityConserveMomentum(previousOrientation, localInertia.InverseInertiaTensor, inertia.InverseInertiaTensor, ref velocity.Angular); + } + else if (integratorCallbacks.AngularIntegrationMode == AngularIntegrationMode.ConserveMomentumWithGyroscopicTorque) + { + PoseIntegration.IntegrateAngularVelocityConserveMomentumWithGyroscopicTorque(orientation, localInertia.InverseInertiaTensor, ref velocity.Angular, dt); + } + if (typeof(TBatchIntegrationMode) == typeof(BatchShouldConditionallyIntegrate)) + { + var previousVelocity = velocity; + integratorCallbacks.IntegrateVelocity(new ReadOnlySpan(Unsafe.AsPointer(ref bodyIndices), count), position, orientation, localInertia, integrationMask, workerIndex, new Vector(dt), ref velocity); + //It would be annoying to make the user handle masking velocity writes to inactive lanes, so we handle it internally. + Vector3Wide.ConditionalSelect(integrationMask, velocity.Linear, previousVelocity.Linear, out velocity.Linear); + Vector3Wide.ConditionalSelect(integrationMask, velocity.Angular, previousVelocity.Angular, out velocity.Angular); + } + else + { + integratorCallbacks.IntegrateVelocity(new ReadOnlySpan(Unsafe.AsPointer(ref bodyIndices), count), position, orientation, localInertia, integrationMask, workerIndex, new Vector(dt), ref velocity); + } + } + //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public static unsafe void GatherAndIntegrate( + public static unsafe void GatherAndIntegrate( Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, ref Buffer integrationFlags, int bodyIndexInConstraint, float dt, int workerIndex, int bundleIndex, ref Vector bodyIndices, int count, out Vector3Wide position, out QuaternionWide orientation, out BodyVelocityWide velocity, out BodyInertiaWide inertia) where TIntegratorCallbacks : struct, IPoseIntegratorCallbacks where TBatchIntegrationMode : unmanaged, IBatchIntegrationMode where TAccessFilter : unmanaged, IBodyAccessFilter + where TShouldIntegratePoses : unmanaged, IBatchPoseIntegrationAllowed { //These type tests are compile time constants and will be specialized. - if (typeof(TBatchIntegrationMode) == typeof(BatchShouldAlwaysIntegrate)) + if (typeof(TShouldIntegratePoses) == typeof(BatchShouldIntegratePoses)) { - var integrationMask = new Vector(-1); - bodies.GatherState(ref bodyIndices, count, false, out position, out orientation, out velocity, out var localInertia); - IntegratePoseAndVelocity(ref integratorCallbacks, ref bodyIndices, count, localInertia, dt, ref position, ref orientation, ref velocity, workerIndex, out inertia); - bodies.ScatterPose(ref position, ref orientation, ref bodyIndices, ref integrationMask); - bodies.ScatterInertia(ref inertia, ref bodyIndices, ref integrationMask); - } - else if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) - { - bodies.GatherState(ref bodyIndices, count, true, out position, out orientation, out velocity, out inertia); + if (typeof(TBatchIntegrationMode) == typeof(BatchShouldAlwaysIntegrate)) + { + var integrationMask = new Vector(-1); + bodies.GatherState(ref bodyIndices, count, false, out position, out orientation, out velocity, out var localInertia); + IntegratePoseAndVelocity(ref integratorCallbacks, ref bodyIndices, count, localInertia, dt, ref position, ref orientation, ref velocity, workerIndex, out inertia); + bodies.ScatterPose(ref position, ref orientation, ref bodyIndices, ref integrationMask); + bodies.ScatterInertia(ref inertia, ref bodyIndices, ref integrationMask); + } + else if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) + { + bodies.GatherState(ref bodyIndices, count, true, out position, out orientation, out velocity, out inertia); + } + else + { + Debug.Assert(typeof(TBatchIntegrationMode) == typeof(BatchShouldConditionallyIntegrate)); + //This executes in warmstart, and warmstarts are typically quite simple from an instruction stream perspective. + //Having a dynamically chosen codepath is unlikely to cause instruction fetching issues. + var bundleIntegrationMode = BundleShouldIntegrate(bundleIndex, integrationFlags[bodyIndexInConstraint], out var integrationMask); + //Note that this will gather world inertia if there is no integration in the bundle, but that it is guaranteed to load all motion state information. + //This avoids complexity around later velocity scattering- we don't have to condition on whether the bundle is integrating. + //In practice, since the access filters are only reducing instruction counts and not memory bandwidth, + //the slightly increased unnecessary gathering is no worse than the more complex scatter condition in performance, and remains simpler. + bodies.GatherState(ref bodyIndices, count, bundleIntegrationMode == BundleIntegrationMode.None, out position, out orientation, out velocity, out var gatheredInertia); + if (bundleIntegrationMode != BundleIntegrationMode.None) + { + //Note that if we take this codepath, the integration routine will reconstruct the world inertias from local inertia given the current pose. + //The changes to pose and velocity for integration inactive lanes will be masked out, so it'll just be identical to the world inertia if we had gathered it. + //Given that we're running the instructions in a bundle to build it, there's no reason to go out of our way to gather the world inertia. + IntegratePoseAndVelocity(ref integratorCallbacks, ref bodyIndices, count, gatheredInertia, dt, ref position, ref orientation, ref velocity, workerIndex, out inertia); + bodies.ScatterPose(ref position, ref orientation, ref bodyIndices, ref integrationMask); + bodies.ScatterInertia(ref inertia, ref bodyIndices, ref integrationMask); + } + else + { + inertia = gatheredInertia; + } + } } else { - Debug.Assert(typeof(TBatchIntegrationMode) == typeof(BatchShouldConditionallyIntegrate)); - //This executes in warmstart, and warmstarts are typically quite simple from an instruction stream perspective. - //Having a dynamically chosen codepath is unlikely to cause instruction fetching issues. - var bundleIntegrationMode = BundleShouldIntegrate(bundleIndex, integrationFlags[bodyIndexInConstraint], out var integrationMask); - //Note that this will gather world inertia if there is no integration in the bundle, but that it is guaranteed to load all motion state information. - //This avoids complexity around later velocity scattering- we don't have to condition on whether the bundle is integrating. - //In practice, since the access filters are only reducing instruction counts and not memory bandwidth, - //the slightly increased unnecessary gathering is no worse than the more complex scatter condition in performance, and remains simpler. - bodies.GatherState(ref bodyIndices, count, bundleIntegrationMode == BundleIntegrationMode.None, out position, out orientation, out velocity, out var gatheredInertia); - if (bundleIntegrationMode != BundleIntegrationMode.None) + Debug.Assert(typeof(TShouldIntegratePoses) == typeof(BatchShouldNotIntegratePoses)); + //There is no need to integrate poses; this is the first substep. + //Note that the full loop for constrained bodies with 3 substeps looks like: + //(velocity -> solve) -> (pose -> velocity -> solve) -> (pose -> velocity -> solve) -> pose + //For unconstrained bodies, it's a tight loop of just: + //(velocity -> pose) -> (velocity -> pose) -> (velocity -> pose) + //So we're maintaining the same order. + //Note that world inertia is still scattered as a part of velocity integration; we need the updated value since we can't trust the cached value across frames. + if (typeof(TBatchIntegrationMode) == typeof(BatchShouldAlwaysIntegrate)) { - //Note that if we take this codepath, the integration routine will reconstruct the world inertias from local inertia given the current pose. - //The changes to pose and velocity for integration inactive lanes will be masked out, so it'll just be identical to the world inertia if we had gathered it. - //Given that we're running the instructions in a bundle to build it, there's no reason to go out of our way to gather the world inertia. - IntegratePoseAndVelocity(ref integratorCallbacks, ref bodyIndices, count, gatheredInertia, dt, ref position, ref orientation, ref velocity, workerIndex, out inertia); - bodies.ScatterPose(ref position, ref orientation, ref bodyIndices, ref integrationMask); + var integrationMask = new Vector(-1); + bodies.GatherState(ref bodyIndices, count, false, out position, out orientation, out velocity, out var localInertia); + IntegrateVelocity(ref integratorCallbacks, ref bodyIndices, count, localInertia, dt, integrationMask, position, orientation, ref velocity, workerIndex, out inertia); bodies.ScatterInertia(ref inertia, ref bodyIndices, ref integrationMask); } + else if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) + { + bodies.GatherState(ref bodyIndices, count, true, out position, out orientation, out velocity, out inertia); + } else { - inertia = gatheredInertia; + Debug.Assert(typeof(TBatchIntegrationMode) == typeof(BatchShouldConditionallyIntegrate)); + var bundleIntegrationMode = BundleShouldIntegrate(bundleIndex, integrationFlags[bodyIndexInConstraint], out var integrationMask); + bodies.GatherState(ref bodyIndices, count, bundleIntegrationMode == BundleIntegrationMode.None, out position, out orientation, out velocity, out var gatheredInertia); + if (bundleIntegrationMode != BundleIntegrationMode.None) + { + IntegrateVelocity(ref integratorCallbacks, ref bodyIndices, count, gatheredInertia, dt, integrationMask, position, orientation, ref velocity, workerIndex, out inertia); + bodies.ScatterInertia(ref inertia, ref bodyIndices, ref integrationMask); + } + else + { + inertia = gatheredInertia; + } } } } - public unsafe override void WarmStart2( + public unsafe override void WarmStart2( ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { var prestepBundles = typeBatch.PrestepData.As(); @@ -558,9 +635,9 @@ public unsafe override void WarmStart2(bodies, ref integratorCallbacks, ref integrationFlags, 0, dt, workerIndex, i, ref references.IndexA, count, + GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 0, dt, workerIndex, i, ref references.IndexA, count, out var positionA, out var orientationA, out var wsvA, out var inertiaA); - GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 1, dt, workerIndex, i, ref references.IndexB, count, + GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 1, dt, workerIndex, i, ref references.IndexB, count, out var positionB, out var orientationB, out var wsvB, out var inertiaB); var ab = positionB - positionA; if (typeof(TConstraintFunctions) == typeof(WeldFunctions)) diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index 2bdd1436a..f0fffe42d 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -138,10 +138,11 @@ internal unsafe abstract void CopySleepingToActive( public abstract void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); - public abstract void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks poseIntegratorCallbacks, + public abstract void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks poseIntegratorCallbacks, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) where TIntegratorCallbacks : struct, IPoseIntegratorCallbacks - where TBatchIntegrationMode : unmanaged, IBatchIntegrationMode; + where TBatchIntegrationMode : unmanaged, IBatchIntegrationMode + where TAllowPoseIntegration : unmanaged, IBatchPoseIntegrationAllowed; public abstract void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index 63539a7c9..5e0253c87 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -46,6 +46,36 @@ public Solver(Bodies bodies, BufferPool pool, int iterationCount, int fallbackBa public PoseIntegrator PoseIntegrator { get; private set; } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + private void WarmStartBlock(int workerIndex, int batchIndex, int typeBatchIndex, int startBundle, int endBundle, ref TypeBatch typeBatch, TypeProcessor typeProcessor, float dt, float inverseDt) + where TBatchShouldIntegratePoses : unmanaged, IBatchPoseIntegrationAllowed + { + if (batchIndex == 0) + { + Buffer noFlagsRequired = default; + typeProcessor.WarmStart2( + ref typeBatch, ref noFlagsRequired, bodies, ref PoseIntegrator.Callbacks, + dt, inverseDt, startBundle, endBundle, workerIndex); + } + else + { + if (coarseBatchIntegrationResponsibilities[batchIndex][typeBatchIndex]) + { + typeProcessor.WarmStart2( + ref typeBatch, ref integrationFlags[batchIndex][typeBatchIndex], bodies, ref PoseIntegrator.Callbacks, + dt, inverseDt, startBundle, endBundle, workerIndex); + } + else + { + typeProcessor.WarmStart2( + ref typeBatch, ref integrationFlags[batchIndex][typeBatchIndex], bodies, ref PoseIntegrator.Callbacks, + dt, inverseDt, startBundle, endBundle, workerIndex); + } + } + } + + //Split the solve process into a warmstart and solve, where warmstart doesn't try to store out anything. It just computes jacobians and modifies velocities according to the accumulated impulse. //The solve step then *recomputes* jacobians from prestep data and pose information. //Why? Memory bandwidth. Redoing the calculation is cheaper than storing it out. @@ -53,6 +83,7 @@ struct WarmStartStep2StageFunction : IStageFunction { public float Dt; public float InverseDt; + public int SubstepIndex; public Solver solver; [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -61,28 +92,15 @@ public void Execute(Solver solver, int blockIndex, int workerIndex) ref var block = ref this.solver.context.ConstraintBlocks.Blocks[blockIndex]; ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; - if (block.BatchIndex == 0) + if (SubstepIndex == 0) { - Buffer noFlagsRequired = default; - typeProcessor.WarmStart2( - ref typeBatch, ref noFlagsRequired, this.solver.bodies, ref this.solver.PoseIntegrator.Callbacks, - Dt, InverseDt, block.StartBundle, block.End, workerIndex); + this.solver.WarmStartBlock(workerIndex, block.BatchIndex, block.TypeBatchIndex, block.StartBundle, block.End, ref typeBatch, typeProcessor, Dt, InverseDt); } else { - if (this.solver.coarseBatchIntegrationResponsibilities[block.BatchIndex][block.TypeBatchIndex]) - { - typeProcessor.WarmStart2( - ref typeBatch, ref this.solver.integrationFlags[block.BatchIndex][block.TypeBatchIndex], this.solver.bodies, ref this.solver.PoseIntegrator.Callbacks, - Dt, InverseDt, block.StartBundle, block.End, workerIndex); - } - else - { - typeProcessor.WarmStart2( - ref typeBatch, ref this.solver.integrationFlags[block.BatchIndex][block.TypeBatchIndex], this.solver.bodies, ref this.solver.PoseIntegrator.Callbacks, - Dt, InverseDt, block.StartBundle, block.End, workerIndex); - } + this.solver.WarmStartBlock(workerIndex, block.BatchIndex, block.TypeBatchIndex, block.StartBundle, block.End, ref typeBatch, typeProcessor, Dt, InverseDt); } + } } //no fallback warmstart; the last constraint batch is always handled by the solve instead, and if the fallback batch exists, it's guaranteed to be the last batch. @@ -162,6 +180,7 @@ void SolveStep2Worker(int workerIndex) for (int i = 0; i < substepCount; ++i) { + warmstartStage.SubstepIndex = i; for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) { var batchOffset = batchIndex > 0 ? context.BatchBoundaries[batchIndex - 1] : 0; @@ -544,24 +563,13 @@ public override void SolveStep2(float totalDt, IThreadDispatcher threadDispatche for (int j = 0; j < batch.TypeBatches.Count; ++j) { ref var typeBatch = ref batch.TypeBatches[j]; - if (i == 0) + if (substepIndex == 0) { - Buffer noFlagsRequired = default; - TypeProcessors[typeBatch.TypeId].WarmStart2(ref typeBatch, ref noFlagsRequired, bodies, ref PoseIntegrator.Callbacks, - substepDt, inverseDt, 0, typeBatch.BundleCount, 0); + WarmStartBlock(0, i, j, 0, typeBatch.BundleCount, ref typeBatch, TypeProcessors[typeBatch.TypeId], substepDt, inverseDt); } else { - if (coarseBatchIntegrationResponsibilities[i][j]) - { - TypeProcessors[typeBatch.TypeId].WarmStart2(ref typeBatch, ref integrationFlagsForBatch[j], bodies, ref PoseIntegrator.Callbacks, - substepDt, inverseDt, 0, typeBatch.BundleCount, 0); - } - else - { - TypeProcessors[typeBatch.TypeId].WarmStart2(ref typeBatch, ref integrationFlagsForBatch[j], bodies, ref PoseIntegrator.Callbacks, - substepDt, inverseDt, 0, typeBatch.BundleCount, 0); - } + WarmStartBlock(0, i, j, 0, typeBatch.BundleCount, ref typeBatch, TypeProcessors[typeBatch.TypeId], substepDt, inverseDt); } } } diff --git a/Demos/Program.cs b/Demos/Program.cs index 957d455c6..962deb171 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -19,7 +19,7 @@ static void Main(string[] args) { content = ContentArchive.Load(stream); } - //HeadlessTest.Test(content, 16, 32, 512); + HeadlessTest.Test(content, 16, 32, 512); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); From b8261bdc2d60d29fe8f043b13b824e567c41632d Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 20 Jul 2021 19:35:01 -0500 Subject: [PATCH 124/947] Noting a peculiar broad phase failure. --- Demos/Demos/NewtDemo.cs | 8 ++++++-- Demos/Program.cs | 2 +- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index d84e1c68d..d773256d4 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -846,8 +846,12 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) BufferPool.Return(ref cellVertexIndices); BufferPool.Return(ref tetrahedraVertexIndices); - //Simulation.Bodies.Add(BodyDescription.CreateDynamic(new RigidPose(new Vector3(0, 10, 0)), new BodyVelocity(default, new Vector3(1,2,3)), default, new CollidableDescription(Simulation.Shapes.Add(new Box(1, 2, 3)), 0.1f), new BodyActivityDescription(-1))); - + var bodyDescription = BodyDescription.CreateDynamic(new RigidPose(new Vector3(0, 10, 0)), new BodyVelocity(default, new Vector3(1, 2, 3)), default, new CollidableDescription(Simulation.Shapes.Add(new Box(1, 2, 3)), 0.1f), new BodyActivityDescription(-1)); + for (int i = 0; i < 10000; ++i) + { + bodyDescription.Pose.Position = new Vector3(i * 6, 10, 3); + Simulation.Bodies.Add(bodyDescription); + } //Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 100, -.5f), 10, Simulation.Shapes, new Sphere(5))); Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(1500, 1, 1500)), 0.1f))); diff --git a/Demos/Program.cs b/Demos/Program.cs index 962deb171..957d455c6 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -19,7 +19,7 @@ static void Main(string[] args) { content = ContentArchive.Load(stream); } - HeadlessTest.Test(content, 16, 32, 512); + //HeadlessTest.Test(content, 16, 32, 512); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); From 32692728fcf04a58ea41cba8d0e3aec42efc563c Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 21 Jul 2021 16:06:13 -0500 Subject: [PATCH 125/947] Fixed two bad integration masks. --- BepuPhysics/Bodies_GatherScatter.cs | 4 ++-- .../Constraints/TwoBodyTypeProcessor.cs | 4 ++-- BepuUtilities/BundleIndexing.cs | 1 + Demos/Demo.cs | 2 +- Demos/Demos/NewtDemo.cs | 21 ++++++++++++------- 5 files changed, 19 insertions(+), 13 deletions(-) diff --git a/BepuPhysics/Bodies_GatherScatter.cs b/BepuPhysics/Bodies_GatherScatter.cs index b933c4162..3d400f09e 100644 --- a/BepuPhysics/Bodies_GatherScatter.cs +++ b/BepuPhysics/Bodies_GatherScatter.cs @@ -661,7 +661,7 @@ public unsafe void ScatterPose( if (maskPointer[5] != 0) Avx.StoreAligned((float*)(states + indices[5]), Avx.Permute2x128(o4, o5, 1 | (3 << 4))); if (maskPointer[6] != 0) Avx.StoreAligned((float*)(states + indices[6]), Avx.Permute2x128(o2, o3, 1 | (3 << 4))); if (maskPointer[7] != 0) Avx.StoreAligned((float*)(states + indices[7]), Avx.Permute2x128(o6, o7, 1 | (3 << 4))); - } + } } else { @@ -675,7 +675,7 @@ public unsafe void ScatterInertia( { if (Avx.IsSupported && Vector.Count == 8) { - var states = ActiveSet.SolverStates.Memory; + var states = ActiveSet.SolverStates.Memory; { var m0 = inertia.InverseInertiaTensor.XX.AsVector256(); var m1 = inertia.InverseInertiaTensor.YX.AsVector256(); diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 4a586b2ec..5f3dcec6d 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -543,7 +543,7 @@ public static unsafe void GatherAndIntegrate(-1); + var integrationMask = BundleIndexing.CreateMaskForCountInBundle(count); bodies.GatherState(ref bodyIndices, count, false, out position, out orientation, out velocity, out var localInertia); IntegratePoseAndVelocity(ref integratorCallbacks, ref bodyIndices, count, localInertia, dt, ref position, ref orientation, ref velocity, workerIndex, out inertia); bodies.ScatterPose(ref position, ref orientation, ref bodyIndices, ref integrationMask); @@ -591,7 +591,7 @@ public static unsafe void GatherAndIntegrate(-1); + var integrationMask = BundleIndexing.CreateMaskForCountInBundle(count); bodies.GatherState(ref bodyIndices, count, false, out position, out orientation, out velocity, out var localInertia); IntegrateVelocity(ref integratorCallbacks, ref bodyIndices, count, localInertia, dt, integrationMask, position, orientation, ref velocity, workerIndex, out inertia); bodies.ScatterInertia(ref inertia, ref bodyIndices, ref integrationMask); diff --git a/BepuUtilities/BundleIndexing.cs b/BepuUtilities/BundleIndexing.cs index 4122ae9c4..e9354d24a 100644 --- a/BepuUtilities/BundleIndexing.cs +++ b/BepuUtilities/BundleIndexing.cs @@ -81,6 +81,7 @@ public static unsafe Vector CreateMaskForCountInBundle(int countInBundle) } return mask; } + //TODO: ARM } } } diff --git a/Demos/Demo.cs b/Demos/Demo.cs index 37ebe5190..5f3790da3 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -62,7 +62,7 @@ public virtual void Update(Window window, Camera camera, Input input, float dt) //fully decouple simulation and rendering rates across different threads. //(In either case, you'd also want to interpolate or extrapolate simulation results during rendering for smoothness.) //Note that taking steps of variable length can reduce stability. Gradual or one-off changes can work reasonably well. - Simulation.Timestep(1 / 60f, ThreadDispatcher); + Simulation.Timestep(1 / 60f); ////Here's an example of how it would look to use more frequent updates, but still with a fixed amount of time simulated per update call: //const float timeToSimulate = 1 / 60f; diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index d773256d4..9f238e0c3 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -833,25 +833,30 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) out var vertices, out var vertexSpatialIndices, out var cellVertexIndices, out var tetrahedraVertexIndices); var weldSpringiness = new SpringSettings(30f, 1f); var volumeSpringiness = new SpringSettings(30f, 1); + int terboTotal = 0; + var terboShape = new Box(0.5f, 0.5f, 3); + terboShape.ComputeInertia(1, out var terboInertia); + var bodyDescription = BodyDescription.CreateDynamic(new RigidPose(new Vector3(0, 10, 0)), new BodyVelocity(default, new Vector3(1, 2, 3)), terboInertia, new CollidableDescription(Simulation.Shapes.Add(terboShape), 0.1f), new BodyActivityDescription(-1)); + for (int i = 0; i < 40; ++i) { //CreateDeformable(Simulation, new Vector3(i * 3, 5 + i * 1.5f, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), MathF.PI * (i * 0.55f)), 1f, cellSize, weldSpringiness, volumeSpringiness, i, filters, ref vertices, ref vertexSpatialIndices, ref cellVertexIndices, ref tetrahedraVertexIndices); CreateDeformable(Simulation, new Vector3(i * 3, cellSize * 2f + i * 0f, 0), Quaternion.Identity, 1f, cellSize, weldSpringiness, volumeSpringiness, i, filters, ref vertices, ref vertexSpatialIndices, ref cellVertexIndices, ref tetrahedraVertexIndices); + + for (int terbo = 0; terbo < 3; ++terbo) + { + bodyDescription.Pose.Position = new Vector3((terboTotal++) * 6, 10, 3); + Simulation.Bodies.Add(bodyDescription); + } } - //Console.WriteLine($"body count: {Simulation.Bodies.ActiveSet.Count}"); - //Console.WriteLine($"constraint count: {Simulation.Solver.CountConstraints()}"); + Console.WriteLine($"body count: {Simulation.Bodies.ActiveSet.Count}"); + Console.WriteLine($"constraint count: {Simulation.Solver.CountConstraints()}"); BufferPool.Return(ref vertices); vertexSpatialIndices.Dispose(BufferPool); BufferPool.Return(ref cellVertexIndices); BufferPool.Return(ref tetrahedraVertexIndices); - var bodyDescription = BodyDescription.CreateDynamic(new RigidPose(new Vector3(0, 10, 0)), new BodyVelocity(default, new Vector3(1, 2, 3)), default, new CollidableDescription(Simulation.Shapes.Add(new Box(1, 2, 3)), 0.1f), new BodyActivityDescription(-1)); - for (int i = 0; i < 10000; ++i) - { - bodyDescription.Pose.Position = new Vector3(i * 6, 10, 3); - Simulation.Bodies.Add(bodyDescription); - } //Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 100, -.5f), 10, Simulation.Shapes, new Sphere(5))); Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(1500, 1, 1500)), 0.1f))); From f5cabc3d016664bad5638f995c20d98eac558053 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 21 Jul 2021 16:35:40 -0500 Subject: [PATCH 126/947] Fixed a post-substepping integrator bug with gyroscopic conservation. --- .../Constraints/TwoBodyTypeProcessor.cs | 24 ++++++++++--------- BepuPhysics/PoseIntegrator.cs | 9 ++++--- Demos/Demo.cs | 2 +- Demos/Demos/NewtDemo.cs | 22 ++++++++--------- Demos/Program.cs | 2 +- 5 files changed, 30 insertions(+), 29 deletions(-) diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 5f3dcec6d..ba80a9ba3 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -429,7 +429,8 @@ public static unsafe void IntegratePoseAndVelocity( //This ensures that velocities set externally are still solved before being integrated. //So, the solver runs velocity integration alone on the first substep. All later substeps then run pose + velocity, and then after the last substep, a final pose integration. //This is equivalent in ordering to running each substep as velocity, warmstart, solve, pose integration, but just shifting the execution context. - var newPosition = position + velocity.Linear * new Vector(dt); + var dtWide = new Vector(dt); + var newPosition = position + velocity.Linear * dtWide; //Note that we only take results for slots which actually need integration. Reintegration would be an error. Vector3Wide.ConditionalSelect(integrationMask, newPosition, position, out position); QuaternionWide newOrientation; @@ -438,21 +439,21 @@ public static unsafe void IntegratePoseAndVelocity( if (integratorCallbacks.AngularIntegrationMode == AngularIntegrationMode.ConserveMomentum) { var previousOrientation = orientation; - PoseIntegration.Integrate(orientation, velocity.Angular, new Vector(dt * 0.5f), out newOrientation); + PoseIntegration.Integrate(orientation, velocity.Angular, dtWide * new Vector(0.5f), out newOrientation); QuaternionWide.ConditionalSelect(integrationMask, newOrientation, orientation, out orientation); PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, orientation, out inertia.InverseInertiaTensor); PoseIntegration.IntegrateAngularVelocityConserveMomentum(previousOrientation, localInertia.InverseInertiaTensor, inertia.InverseInertiaTensor, ref velocity.Angular); } else if (integratorCallbacks.AngularIntegrationMode == AngularIntegrationMode.ConserveMomentumWithGyroscopicTorque) { - PoseIntegration.Integrate(orientation, velocity.Angular, new Vector(dt * 0.5f), out newOrientation); + PoseIntegration.Integrate(orientation, velocity.Angular, dtWide * new Vector(0.5f), out newOrientation); QuaternionWide.ConditionalSelect(integrationMask, newOrientation, orientation, out orientation); PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, orientation, out inertia.InverseInertiaTensor); - PoseIntegration.IntegrateAngularVelocityConserveMomentumWithGyroscopicTorque(orientation, localInertia.InverseInertiaTensor, ref velocity.Angular, dt); + PoseIntegration.IntegrateAngularVelocityConserveMomentumWithGyroscopicTorque(orientation, localInertia.InverseInertiaTensor, ref velocity.Angular, dtWide); } else { - PoseIntegration.Integrate(orientation, velocity.Angular, new Vector(dt * 0.5f), out newOrientation); + PoseIntegration.Integrate(orientation, velocity.Angular, dtWide * new Vector(0.5f), out newOrientation); QuaternionWide.ConditionalSelect(integrationMask, newOrientation, orientation, out orientation); PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, orientation, out inertia.InverseInertiaTensor); } @@ -470,24 +471,25 @@ public static unsafe void IntegratePoseAndVelocity( where TIntegratorCallbacks : struct, IPoseIntegratorCallbacks { //This is identical to the other IntegratePoseAndVelocity, but it avoids any masking because we know ahead of time that the entire bundle is integrating. - position += velocity.Linear * new Vector(dt); + var dtWide = new Vector(dt); + position += velocity.Linear * dtWide; inertia.InverseMass = localInertia.InverseMass; if (integratorCallbacks.AngularIntegrationMode == AngularIntegrationMode.ConserveMomentum) { var previousOrientation = orientation; - PoseIntegration.Integrate(orientation, velocity.Angular, new Vector(dt * 0.5f), out orientation); + PoseIntegration.Integrate(orientation, velocity.Angular, dtWide * new Vector(0.5f), out orientation); PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, orientation, out inertia.InverseInertiaTensor); PoseIntegration.IntegrateAngularVelocityConserveMomentum(previousOrientation, localInertia.InverseInertiaTensor, inertia.InverseInertiaTensor, ref velocity.Angular); } else if (integratorCallbacks.AngularIntegrationMode == AngularIntegrationMode.ConserveMomentumWithGyroscopicTorque) { - PoseIntegration.Integrate(orientation, velocity.Angular, new Vector(dt * 0.5f), out orientation); + PoseIntegration.Integrate(orientation, velocity.Angular, dtWide * new Vector(0.5f), out orientation); PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, orientation, out inertia.InverseInertiaTensor); - PoseIntegration.IntegrateAngularVelocityConserveMomentumWithGyroscopicTorque(orientation, localInertia.InverseInertiaTensor, ref velocity.Angular, dt); + PoseIntegration.IntegrateAngularVelocityConserveMomentumWithGyroscopicTorque(orientation, localInertia.InverseInertiaTensor, ref velocity.Angular, dtWide); } else { - PoseIntegration.Integrate(orientation, velocity.Angular, new Vector(dt * 0.5f), out orientation); + PoseIntegration.Integrate(orientation, velocity.Angular, dtWide * new Vector(0.5f), out orientation); PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, orientation, out inertia.InverseInertiaTensor); } integratorCallbacks.IntegrateVelocity(new ReadOnlySpan(Unsafe.AsPointer(ref bodyIndices), count), position, orientation, localInertia, new Vector(-1), workerIndex, new Vector(dt), ref velocity); @@ -513,7 +515,7 @@ public static unsafe void IntegrateVelocity(dt)); } if (typeof(TBatchIntegrationMode) == typeof(BatchShouldConditionallyIntegrate)) { diff --git a/BepuPhysics/PoseIntegrator.cs b/BepuPhysics/PoseIntegrator.cs index c3ee605e0..3e197dbd9 100644 --- a/BepuPhysics/PoseIntegrator.cs +++ b/BepuPhysics/PoseIntegrator.cs @@ -194,7 +194,7 @@ public static void IntegrateAngularVelocityConserveMomentum(in QuaternionWide pr [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void IntegrateAngularVelocityConserveMomentumWithGyroscopicTorque( - in QuaternionWide orientation, in Symmetric3x3Wide localInverseInertia, ref Vector3Wide angularVelocity, float dt) + in QuaternionWide orientation, in Symmetric3x3Wide localInverseInertia, ref Vector3Wide angularVelocity, in Vector dt) { //Integrating the gyroscopic force explicitly can result in some instability, so we'll use an approximate implicit approach. //angularVelocity1 * inertia1 = angularVelocity0 * inertia1 + dt * ((angularVelocity1 * inertia1) x angularVelocity1) @@ -221,14 +221,13 @@ public static void IntegrateAngularVelocityConserveMomentumWithGyroscopicTorque( Symmetric3x3Wide.Invert(localInverseInertia, out var localInertiaTensor); Symmetric3x3Wide.TransformWithoutOverlap(localAngularVelocity, localInertiaTensor, out var localAngularMomentum); - var dtWide = new Vector(dt); - var residual = dtWide * Vector3Wide.Cross(localAngularMomentum, localAngularVelocity); + var residual = dt * Vector3Wide.Cross(localAngularMomentum, localAngularVelocity); Matrix3x3Wide.CreateCrossProduct(localAngularMomentum, out var skewMomentum); Matrix3x3Wide.CreateCrossProduct(localAngularVelocity, out var skewVelocity); var transformedSkewVelocity = skewVelocity * localInertiaTensor; Matrix3x3Wide.Subtract(transformedSkewVelocity, skewMomentum, out var changeOverDt); - Matrix3x3Wide.Scale(changeOverDt, dtWide, out var change); + Matrix3x3Wide.Scale(changeOverDt, dt, out var change); var jacobian = localInertiaTensor + change; Matrix3x3Wide.Invert(jacobian, out var inverseJacobian); @@ -838,7 +837,7 @@ unsafe void IntegrateBundlesAfterSubstepping(ref IndexSet mergedConstrainedBodyH else if (callbacks.AngularIntegrationMode == AngularIntegrationMode.ConserveMomentumWithGyroscopicTorque) { PoseIntegration.Integrate(orientation, velocity.Angular, halfDt, out orientation); - PoseIntegration.IntegrateAngularVelocityConserveMomentumWithGyroscopicTorque(orientation, localInertia.InverseInertiaTensor, ref velocity.Angular, dt); + PoseIntegration.IntegrateAngularVelocityConserveMomentumWithGyroscopicTorque(orientation, localInertia.InverseInertiaTensor, ref velocity.Angular, bundleEffectiveDt); } else { diff --git a/Demos/Demo.cs b/Demos/Demo.cs index 5f3790da3..37ebe5190 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -62,7 +62,7 @@ public virtual void Update(Window window, Camera camera, Input input, float dt) //fully decouple simulation and rendering rates across different threads. //(In either case, you'd also want to interpolate or extrapolate simulation results during rendering for smoothness.) //Note that taking steps of variable length can reduce stability. Gradual or one-off changes can work reasonably well. - Simulation.Timestep(1 / 60f); + Simulation.Timestep(1 / 60f, ThreadDispatcher); ////Here's an example of how it would look to use more frequent updates, but still with a fixed amount of time simulated per update call: //const float timeToSimulate = 1 / 60f; diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index 9f238e0c3..c02967ff6 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -833,24 +833,24 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) out var vertices, out var vertexSpatialIndices, out var cellVertexIndices, out var tetrahedraVertexIndices); var weldSpringiness = new SpringSettings(30f, 1f); var volumeSpringiness = new SpringSettings(30f, 1); - int terboTotal = 0; - var terboShape = new Box(0.5f, 0.5f, 3); - terboShape.ComputeInertia(1, out var terboInertia); - var bodyDescription = BodyDescription.CreateDynamic(new RigidPose(new Vector3(0, 10, 0)), new BodyVelocity(default, new Vector3(1, 2, 3)), terboInertia, new CollidableDescription(Simulation.Shapes.Add(terboShape), 0.1f), new BodyActivityDescription(-1)); + //int terboTotal = 0; + //var terboShape = new Box(0.5f, 0.5f, 3); + //terboShape.ComputeInertia(1, out var terboInertia); + //var bodyDescription = BodyDescription.CreateDynamic(new RigidPose(new Vector3(0, 10, 0)), new BodyVelocity(default, new Vector3(1, 2, 3)), terboInertia, new CollidableDescription(Simulation.Shapes.Add(terboShape), 0.1f), new BodyActivityDescription(-1)); for (int i = 0; i < 40; ++i) { //CreateDeformable(Simulation, new Vector3(i * 3, 5 + i * 1.5f, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), MathF.PI * (i * 0.55f)), 1f, cellSize, weldSpringiness, volumeSpringiness, i, filters, ref vertices, ref vertexSpatialIndices, ref cellVertexIndices, ref tetrahedraVertexIndices); CreateDeformable(Simulation, new Vector3(i * 3, cellSize * 2f + i * 0f, 0), Quaternion.Identity, 1f, cellSize, weldSpringiness, volumeSpringiness, i, filters, ref vertices, ref vertexSpatialIndices, ref cellVertexIndices, ref tetrahedraVertexIndices); - for (int terbo = 0; terbo < 3; ++terbo) - { - bodyDescription.Pose.Position = new Vector3((terboTotal++) * 6, 10, 3); - Simulation.Bodies.Add(bodyDescription); - } + //for (int terbo = 0; terbo < 17; ++terbo) + //{ + // bodyDescription.Pose.Position = new Vector3((terboTotal++) * 6, 10, 3); + // Simulation.Bodies.Add(bodyDescription); + //} } - Console.WriteLine($"body count: {Simulation.Bodies.ActiveSet.Count}"); - Console.WriteLine($"constraint count: {Simulation.Solver.CountConstraints()}"); + //Console.WriteLine($"body count: {Simulation.Bodies.ActiveSet.Count}"); + //Console.WriteLine($"constraint count: {Simulation.Solver.CountConstraints()}"); BufferPool.Return(ref vertices); vertexSpatialIndices.Dispose(BufferPool); diff --git a/Demos/Program.cs b/Demos/Program.cs index 957d455c6..962deb171 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -19,7 +19,7 @@ static void Main(string[] args) { content = ContentArchive.Load(stream); } - //HeadlessTest.Test(content, 16, 32, 512); + HeadlessTest.Test(content, 16, 32, 512); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); From 452432acda46f1adc4a3463e99a98d962bec13c7 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 21 Jul 2021 18:27:24 -0500 Subject: [PATCH 127/947] Pushing through new functional requirements on interfaces. Fixed contact convex types refactor miss. AngularAxisGearMotor updated for numode. --- .../Constraints/AngularAxisGearMotor.cs | 32 ++ BepuPhysics/Constraints/AngularAxisMotor.cs | 9 + BepuPhysics/Constraints/AngularHinge.cs | 9 + BepuPhysics/Constraints/AngularMotor.cs | 9 + BepuPhysics/Constraints/AngularServo.cs | 9 + BepuPhysics/Constraints/AngularSwivelHinge.cs | 9 + BepuPhysics/Constraints/BallSocketMotor.cs | 9 + BepuPhysics/Constraints/BallSocketServo.cs | 10 + .../Constraints/Contact/ContactConvexTypes.cs | 96 ++++++ .../Constraints/Contact/ContactConvexTypes.tt | 24 +- .../Contact/ContactNonconvexCommon.cs | 10 + BepuPhysics/Constraints/DistanceLimit.cs | 9 + BepuPhysics/Constraints/DistanceServo.cs | 9 + BepuPhysics/Constraints/Hinge.cs | 9 + BepuPhysics/Constraints/LinearAxisLimit.cs | 9 + BepuPhysics/Constraints/LinearAxisMotor.cs | 9 + BepuPhysics/Constraints/LinearAxisServo.cs | 9 + BepuPhysics/Constraints/MotorSettings.cs | 5 +- BepuPhysics/Constraints/PointOnLineServo.cs | 9 + BepuPhysics/Constraints/SwingLimit.cs | 9 + BepuPhysics/Constraints/SwivelHinge.cs | 9 + BepuPhysics/Constraints/TwistLimit.cs | 9 + BepuPhysics/Constraints/TwistMotor.cs | 9 + BepuPhysics/Constraints/TwistServo.cs | 9 + .../Constraints/TwoBodyTypeProcessor.cs | 325 +----------------- BepuPhysics/Constraints/TypeProcessor.cs | 318 ++++++++++++++++- .../Characters/CharacterMotionConstraint.cs | 18 + .../Characters/CharacterMotionConstraint.tt | 9 + 28 files changed, 684 insertions(+), 325 deletions(-) diff --git a/BepuPhysics/Constraints/AngularAxisGearMotor.cs b/BepuPhysics/Constraints/AngularAxisGearMotor.cs index af242e0e1..4f939455b 100644 --- a/BepuPhysics/Constraints/AngularAxisGearMotor.cs +++ b/BepuPhysics/Constraints/AngularAxisGearMotor.cs @@ -129,6 +129,38 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void ApplyImpulse(in Vector3Wide jA, in Vector3Wide negatedJB, in Vector csi, in Symmetric3x3Wide inertiaA, in Symmetric3x3Wide inertiaB, ref Vector3Wide angularVelocityA, ref Vector3Wide angularVelocityB) + { + Symmetric3x3Wide.TransformWithoutOverlap(jA * csi, inertiaA, out var changeA); + angularVelocityA += changeA; + Symmetric3x3Wide.TransformWithoutOverlap(negatedJB * csi, inertiaB, out var negatedChangeB); + angularVelocityA -= negatedChangeB; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in AngularAxisGearMotorPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + QuaternionWide.TransformWithoutOverlap(prestep.LocalAxisA, orientationA, out var axis); + ApplyImpulse(axis * prestep.VelocityScale, axis, accumulatedImpulses, inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, ref wsvA.Angular, ref wsvB.Angular); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in AngularAxisGearMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + QuaternionWide.TransformWithoutOverlap(prestep.LocalAxisA, orientationA, out var axis); + Vector3Wide.Scale(axis, prestep.VelocityScale, out var jA); + Symmetric3x3Wide.TransformWithoutOverlap(jA, inertiaA.InverseInertiaTensor, out var jIA); + Vector3Wide.Dot(jA, jIA, out var contributionA); + Symmetric3x3Wide.TransformWithoutOverlap(axis, inertiaB.InverseInertiaTensor, out var jIB); + Vector3Wide.Dot(axis, jIB, out var contributionB); + MotorSettingsWide.ComputeSoftness(prestep.Settings, dt, out var effectiveMassCFMScale, out var softnessImpulseScale, out var maximumImpulse); + + //csi = projection.BiasImpulse - accumulatedImpulse * softnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); + var csi = effectiveMassCFMScale * (Vector3Wide.Dot(wsvB.Angular, axis) - Vector3Wide.Dot(wsvA.Angular, axis)) / (contributionA + contributionB) - accumulatedImpulses * softnessImpulseScale; + ServoSettingsWide.ClampImpulse(maximumImpulse, ref accumulatedImpulses, ref csi); + ApplyImpulse(jA, axis, accumulatedImpulses, inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, ref wsvA.Angular, ref wsvB.Angular); + } } public class AngularAxisGearMotorTypeProcessor : TwoBodyTypeProcessor, AngularAxisGearMotorFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular> diff --git a/BepuPhysics/Constraints/AngularAxisMotor.cs b/BepuPhysics/Constraints/AngularAxisMotor.cs index 8849bf6f1..b032152d6 100644 --- a/BepuPhysics/Constraints/AngularAxisMotor.cs +++ b/BepuPhysics/Constraints/AngularAxisMotor.cs @@ -125,6 +125,15 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB } + public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in AngularAxisMotorPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } + + public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in AngularAxisMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } } public class AngularAxisMotorTypeProcessor : TwoBodyTypeProcessor, AngularAxisMotorFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular> diff --git a/BepuPhysics/Constraints/AngularHinge.cs b/BepuPhysics/Constraints/AngularHinge.cs index 4b8edacee..8e85c4ccb 100644 --- a/BepuPhysics/Constraints/AngularHinge.cs +++ b/BepuPhysics/Constraints/AngularHinge.cs @@ -226,6 +226,15 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, ref projection, ref csi); } + public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in AngularHingePrestepData prestep, in Vector2Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } + + public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in AngularHingePrestepData prestep, ref Vector2Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } } public class AngularHingeTypeProcessor : TwoBodyTypeProcessor diff --git a/BepuPhysics/Constraints/AngularMotor.cs b/BepuPhysics/Constraints/AngularMotor.cs index 45bd3eb2a..84010a369 100644 --- a/BepuPhysics/Constraints/AngularMotor.cs +++ b/BepuPhysics/Constraints/AngularMotor.cs @@ -101,6 +101,15 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB projection.MaximumImpulse, projection.ImpulseToVelocityA, projection.NegatedImpulseToVelocityB, ref accumulatedImpulse); } + public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in AngularMotorPrestepData prestep, in Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } + + public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in AngularMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } } public class AngularMotorTypeProcessor : TwoBodyTypeProcessor diff --git a/BepuPhysics/Constraints/AngularServo.cs b/BepuPhysics/Constraints/AngularServo.cs index 2c8e63693..9e8876dea 100644 --- a/BepuPhysics/Constraints/AngularServo.cs +++ b/BepuPhysics/Constraints/AngularServo.cs @@ -142,6 +142,15 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB projection.MaximumImpulse, projection.ImpulseToVelocityA, projection.NegatedImpulseToVelocityB, ref accumulatedImpulse); } + public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in AngularServoPrestepData prestep, in Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } + + public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in AngularServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } } public class AngularServoTypeProcessor : TwoBodyTypeProcessor diff --git a/BepuPhysics/Constraints/AngularSwivelHinge.cs b/BepuPhysics/Constraints/AngularSwivelHinge.cs index 4314934fb..33be3c7ff 100644 --- a/BepuPhysics/Constraints/AngularSwivelHinge.cs +++ b/BepuPhysics/Constraints/AngularSwivelHinge.cs @@ -154,6 +154,15 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, ref projection, ref csi); } + public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in AngularSwivelHingePrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } + + public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in AngularSwivelHingePrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } } public class AngularSwivelHingeTypeProcessor : TwoBodyTypeProcessor, AngularSwivelHingeFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular> diff --git a/BepuPhysics/Constraints/BallSocketMotor.cs b/BepuPhysics/Constraints/BallSocketMotor.cs index 7fd0e477b..eff9a298f 100644 --- a/BepuPhysics/Constraints/BallSocketMotor.cs +++ b/BepuPhysics/Constraints/BallSocketMotor.cs @@ -111,6 +111,15 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB BallSocketShared.Solve(ref velocityA, ref velocityB, ref projection.OffsetA, ref projection.OffsetB, ref projection.BiasVelocity, ref projection.EffectiveMass, ref projection.SoftnessImpulseScale, ref projection.MaximumImpulse, ref accumulatedImpulse, ref projection.InertiaA, ref projection.InertiaB); } + public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BallSocketMotorPrestepData prestep, in Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } + + public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in BallSocketMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } } diff --git a/BepuPhysics/Constraints/BallSocketServo.cs b/BepuPhysics/Constraints/BallSocketServo.cs index 536eb4602..513d5ce13 100644 --- a/BepuPhysics/Constraints/BallSocketServo.cs +++ b/BepuPhysics/Constraints/BallSocketServo.cs @@ -117,6 +117,16 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB { BallSocketShared.Solve(ref velocityA, ref velocityB, ref projection.OffsetA, ref projection.OffsetB, ref projection.BiasVelocity, ref projection.EffectiveMass, ref projection.SoftnessImpulseScale, ref projection.MaximumImpulse, ref accumulatedImpulse, ref projection.InertiaA, ref projection.InertiaB); } + + public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BallSocketServoPrestepData prestep, in Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } + + public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in BallSocketServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } } diff --git a/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs b/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs index d0a0a3ad6..649cb46f5 100644 --- a/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs +++ b/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs @@ -350,6 +350,18 @@ public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityW { PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, velocityA, ref prestep.Contact0.Depth); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Contact1OneBodyPrestepData prestep, in Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, in Contact1OneBodyPrestepData prestep, ref Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } } /// @@ -530,6 +542,18 @@ public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityW PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, velocityA, ref prestep.Contact0.Depth); PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.Normal, velocityA, ref prestep.Contact1.Depth); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Contact2OneBodyPrestepData prestep, in Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, in Contact2OneBodyPrestepData prestep, ref Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } } /// @@ -724,6 +748,18 @@ public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityW PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.Normal, velocityA, ref prestep.Contact1.Depth); PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact2.OffsetA, prestep.Normal, velocityA, ref prestep.Contact2.Depth); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Contact3OneBodyPrestepData prestep, in Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, in Contact3OneBodyPrestepData prestep, ref Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } } /// @@ -932,6 +968,18 @@ public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityW PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact2.OffsetA, prestep.Normal, velocityA, ref prestep.Contact2.Depth); PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact3.OffsetA, prestep.Normal, velocityA, ref prestep.Contact3.Depth); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Contact4OneBodyPrestepData prestep, in Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, in Contact4OneBodyPrestepData prestep, ref Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } } /// @@ -1115,6 +1163,18 @@ public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityW { PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact0.Depth); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Contact1PrestepData prestep, in Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in Contact1PrestepData prestep, ref Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } } /// @@ -1312,6 +1372,18 @@ public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityW PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact0.Depth); PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact1.Depth); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Contact2PrestepData prestep, in Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in Contact2PrestepData prestep, ref Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } } /// @@ -1524,6 +1596,18 @@ public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityW PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact1.Depth); PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact2.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact2.Depth); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Contact3PrestepData prestep, in Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in Contact3PrestepData prestep, ref Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } } /// @@ -1751,6 +1835,18 @@ public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityW PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact2.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact2.Depth); PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact3.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact3.Depth); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Contact4PrestepData prestep, in Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in Contact4PrestepData prestep, ref Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } } /// diff --git a/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt b/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt index 833d55bde..e22b047c0 100644 --- a/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt +++ b/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt @@ -217,9 +217,9 @@ for (int i = 0; i < contactCount; ++i) public unsafe struct Contact<#= contactCount #><#=suffix#>Projection { - public BodyInertias InertiaA; + public BodyInertiaWide InertiaA; <# if (bodyCount == 2) { #> - public BodyInertias InertiaB; + public BodyInertiaWide InertiaB; <#}#> public Vector PremultipliedFrictionCoefficient; public Vector3Wide Normal; @@ -239,7 +239,7 @@ for (int i = 0; i < contactCount; ++i) { [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Prestep( - <#if(bodyCount == 1) {#>in Vector3Wide positionA,<#}#> in QuaternionWide orientationA, in BodyInertias inertiaA, <#if(bodyCount == 2) {#>in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertias inertiaB, <#}#>float dt, float inverseDt, ref Contact<#=contactCount#><#=suffix#>PrestepData prestep, out Contact<#=contactCount#><#=suffix#>Projection projection) + <#if(bodyCount == 1) {#>in Vector3Wide positionA,<#}#> in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(bodyCount == 2) {#>in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>float dt, float inverseDt, ref Contact<#=contactCount#><#=suffix#>PrestepData prestep, out Contact<#=contactCount#><#=suffix#>Projection projection) { //Be careful about the execution order here. It should be aligned with the prestep data layout to ensure prefetching works well. projection.InertiaA = inertiaA; @@ -281,7 +281,7 @@ for (int i = 0; i < contactCount; ++i) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocities wsvA, <#if(bodyCount == 2) {#>ref BodyVelocities wsvB, <#}#>ref Contact<#=contactCount#><#=suffix#>Projection projection, ref Contact<#=contactCount#>AccumulatedImpulses accumulatedImpulses) + public void WarmStart(ref BodyVelocityWide wsvA, <#if(bodyCount == 2) {#>ref BodyVelocityWide wsvB, <#}#>ref Contact<#=contactCount#><#=suffix#>Projection projection, ref Contact<#=contactCount#>AccumulatedImpulses accumulatedImpulses) { Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); TangentFriction<#=suffix#>.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, <#if (bodyCount == 2) {#>ref projection.InertiaB, <#}#>ref accumulatedImpulses.Tangent, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); @@ -292,7 +292,7 @@ for (int i = 0; i < contactCount; ++i) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocities wsvA, <#if(bodyCount == 2) {#>ref BodyVelocities wsvB, <#}#>ref Contact<#=contactCount#><#=suffix#>Projection projection, ref Contact<#=contactCount#>AccumulatedImpulses accumulatedImpulses) + public void Solve(ref BodyVelocityWide wsvA, <#if(bodyCount == 2) {#>ref BodyVelocityWide wsvB, <#}#>ref Contact<#=contactCount#><#=suffix#>Projection projection, ref Contact<#=contactCount#>AccumulatedImpulses accumulatedImpulses) { Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * @@ -313,12 +313,24 @@ for (int i = 0; i < contactCount; ++i) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocities velocityA,<#if(bodyCount == 2) {#> in BodyVelocities velocityB,<#}#> ref Contact<#=contactCount#><#=suffix#>PrestepData prestep) + public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityWide velocityA,<#if(bodyCount == 2) {#> in BodyVelocityWide velocityB,<#}#> ref Contact<#=contactCount#><#=suffix#>PrestepData prestep) { <#for (int i = 0; i < contactCount; ++i) {#> PenetrationLimit<#=suffix#>.UpdatePenetrationDepth(dt, prestep.Contact<#=i#>.OffsetA, <#if(bodyCount == 2) {#>prestep.OffsetB, <#}#>prestep.Normal, velocityA, <#if (bodyCount == 2) {#>velocityB, <#}#>ref prestep.Contact<#=i#>.Depth); <#}#> } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void WarmStart2(<#if(bodyCount == 1) {#>in Vector3Wide positionA, <#}#>in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(bodyCount == 2) {#>in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>in Contact<#=contactCount#><#=suffix#>PrestepData prestep, in Contact<#=contactCount#>AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void Solve2(<#if(bodyCount == 1) {#>in Vector3Wide positionA, <#}#>in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(bodyCount == 2) {#>in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>float dt, float inverseDt, in Contact<#=contactCount#><#=suffix#>PrestepData prestep, ref Contact<#=contactCount#>AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } } /// diff --git a/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs b/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs index 27b05d31b..aa439c2e6 100644 --- a/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs +++ b/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs @@ -381,5 +381,15 @@ public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityW PenetrationLimit.UpdatePenetrationDepth(dt, prestepContact.Offset, prestepOffsetB, prestepContact.Normal, velocityA, velocityB, ref prestepContact.Depth); } } + + public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in TPrestep prestep, in TAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new System.NotImplementedException(); + } + + public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in TPrestep prestep, ref TAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new System.NotImplementedException(); + } } } diff --git a/BepuPhysics/Constraints/DistanceLimit.cs b/BepuPhysics/Constraints/DistanceLimit.cs index 86f7d8965..ea56fe6be 100644 --- a/BepuPhysics/Constraints/DistanceLimit.cs +++ b/BepuPhysics/Constraints/DistanceLimit.cs @@ -159,6 +159,15 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB } + public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in DistanceLimitPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } + + public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in DistanceLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } } diff --git a/BepuPhysics/Constraints/DistanceServo.cs b/BepuPhysics/Constraints/DistanceServo.cs index 1a1ca089f..f18859251 100644 --- a/BepuPhysics/Constraints/DistanceServo.cs +++ b/BepuPhysics/Constraints/DistanceServo.cs @@ -241,6 +241,15 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB projection.LinearImpulseToVelocityA, projection.AngularImpulseToVelocityA, projection.LinearImpulseToVelocityB, projection.AngularImpulseToVelocityB, ref csi); } + public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in DistanceServoPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } + + public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in DistanceServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } } diff --git a/BepuPhysics/Constraints/Hinge.cs b/BepuPhysics/Constraints/Hinge.cs index 8dc37a01f..3f336238b 100644 --- a/BepuPhysics/Constraints/Hinge.cs +++ b/BepuPhysics/Constraints/Hinge.cs @@ -224,6 +224,15 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ApplyImpulse(ref velocityA, ref velocityB, ref projection, ref csi); } + public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in HingePrestepData prestep, in HingeAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } + + public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in HingePrestepData prestep, ref HingeAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } } public class HingeTypeProcessor : TwoBodyTypeProcessor diff --git a/BepuPhysics/Constraints/LinearAxisLimit.cs b/BepuPhysics/Constraints/LinearAxisLimit.cs index 1510b0d6c..e2440583d 100644 --- a/BepuPhysics/Constraints/LinearAxisLimit.cs +++ b/BepuPhysics/Constraints/LinearAxisLimit.cs @@ -149,6 +149,15 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ref csi); } + public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in LinearAxisLimitPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } + + public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in LinearAxisLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } } public class LinearAxisLimitTypeProcessor : TwoBodyTypeProcessor, LinearAxisLimitFunctions, AccessAll, AccessAll, AccessAll, AccessAll> diff --git a/BepuPhysics/Constraints/LinearAxisMotor.cs b/BepuPhysics/Constraints/LinearAxisMotor.cs index db5f7d5b0..41a47d449 100644 --- a/BepuPhysics/Constraints/LinearAxisMotor.cs +++ b/BepuPhysics/Constraints/LinearAxisMotor.cs @@ -114,6 +114,15 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ref csi); } + public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in LinearAxisMotorPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } + + public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in LinearAxisMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } } public class LinearAxisMotorTypeProcessor : TwoBodyTypeProcessor, LinearAxisMotorFunctions, AccessAll, AccessAll, AccessAll, AccessAll> diff --git a/BepuPhysics/Constraints/LinearAxisServo.cs b/BepuPhysics/Constraints/LinearAxisServo.cs index e1d34513c..e269fdf82 100644 --- a/BepuPhysics/Constraints/LinearAxisServo.cs +++ b/BepuPhysics/Constraints/LinearAxisServo.cs @@ -227,6 +227,15 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ref csi); } + public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in LinearAxisServoPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } + + public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in LinearAxisServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } } public class LinearAxisServoTypeProcessor : TwoBodyTypeProcessor, LinearAxisServoFunctions, AccessAll, AccessAll, AccessAll, AccessAll> diff --git a/BepuPhysics/Constraints/MotorSettings.cs b/BepuPhysics/Constraints/MotorSettings.cs index 561998ca1..07079df2a 100644 --- a/BepuPhysics/Constraints/MotorSettings.cs +++ b/BepuPhysics/Constraints/MotorSettings.cs @@ -93,10 +93,11 @@ public static void ComputeSoftness(in MotorSettingsWide settings, float dt, out //CFM/dt * softenedEffectiveMass = 1 / (d * dt + 1) //(For more, see the Inequality1DOF example constraint.) - var dtd = dt * settings.Damping; + var dtWide = new Vector(dt); + var dtd = dtWide * settings.Damping; + maximumImpulse = settings.MaximumForce * dtWide; softnessImpulseScale = Vector.One / (dtd + Vector.One); effectiveMassCFMScale = dtd * softnessImpulseScale; - maximumImpulse = settings.MaximumForce * dt; } } } diff --git a/BepuPhysics/Constraints/PointOnLineServo.cs b/BepuPhysics/Constraints/PointOnLineServo.cs index cf6ec6dff..bf2315a2d 100644 --- a/BepuPhysics/Constraints/PointOnLineServo.cs +++ b/BepuPhysics/Constraints/PointOnLineServo.cs @@ -221,6 +221,15 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ApplyImpulse(ref velocityA, ref velocityB, projection.LinearJacobian, angularA, angularB, projection.InertiaA, projection.InertiaB, ref csi); } + public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in PointOnLineServoPrestepData prestep, in Vector2Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } + + public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in PointOnLineServoPrestepData prestep, ref Vector2Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } } public class PointOnLineServoTypeProcessor : TwoBodyTypeProcessor diff --git a/BepuPhysics/Constraints/SwingLimit.cs b/BepuPhysics/Constraints/SwingLimit.cs index 7f491daf9..3ec5ce40d 100644 --- a/BepuPhysics/Constraints/SwingLimit.cs +++ b/BepuPhysics/Constraints/SwingLimit.cs @@ -179,6 +179,15 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, ref projection, ref csi); } + public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in SwingLimitPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } + + public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in SwingLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } } public class SwingLimitTypeProcessor : TwoBodyTypeProcessor, SwingLimitFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular> diff --git a/BepuPhysics/Constraints/SwivelHinge.cs b/BepuPhysics/Constraints/SwivelHinge.cs index da106d2a6..4716a37f3 100644 --- a/BepuPhysics/Constraints/SwivelHinge.cs +++ b/BepuPhysics/Constraints/SwivelHinge.cs @@ -211,6 +211,15 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ApplyImpulse(ref velocityA, ref velocityB, ref projection, ref csi); } + public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in SwivelHingePrestepData prestep, in Vector4Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } + + public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in SwivelHingePrestepData prestep, ref Vector4Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } } public class SwivelHingeTypeProcessor : TwoBodyTypeProcessor diff --git a/BepuPhysics/Constraints/TwistLimit.cs b/BepuPhysics/Constraints/TwistLimit.cs index 21e8219c8..0080b5aae 100644 --- a/BepuPhysics/Constraints/TwistLimit.cs +++ b/BepuPhysics/Constraints/TwistLimit.cs @@ -145,6 +145,15 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB TwistServoFunctions.ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, projection.ImpulseToVelocityA, projection.NegatedImpulseToVelocityB, csi); } + public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in TwistLimitPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } + + public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in TwistLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } } public class TwistLimitTypeProcessor : TwoBodyTypeProcessor, TwistLimitFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular> diff --git a/BepuPhysics/Constraints/TwistMotor.cs b/BepuPhysics/Constraints/TwistMotor.cs index 30e1b5a05..7c3955360 100644 --- a/BepuPhysics/Constraints/TwistMotor.cs +++ b/BepuPhysics/Constraints/TwistMotor.cs @@ -130,6 +130,15 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB TwistServoFunctions.ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, projection.ImpulseToVelocityA, projection.NegatedImpulseToVelocityB, csi); } + public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in TwistMotorPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } + + public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in TwistMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } } public class TwistMotorTypeProcessor : TwoBodyTypeProcessor, TwistMotorFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular> diff --git a/BepuPhysics/Constraints/TwistServo.cs b/BepuPhysics/Constraints/TwistServo.cs index f093b4033..5aad15eb6 100644 --- a/BepuPhysics/Constraints/TwistServo.cs +++ b/BepuPhysics/Constraints/TwistServo.cs @@ -215,6 +215,15 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, projection.ImpulseToVelocityA, projection.NegatedImpulseToVelocityB, csi); } + public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in TwistServoPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } + + public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in TwistServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } } public class TwistServoTypeProcessor : TwoBodyTypeProcessor, TwistServoFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular> diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index ba80a9ba3..42f55e777 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -32,6 +32,11 @@ public interface ITwoBodyConstraintFunctions @@ -308,318 +313,6 @@ public unsafe override void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodi } - const int warmStartPrefetchDistance = 8; - const int solvePrefetchDistance = 4; - [MethodImpl(MethodImplOptions.AggressiveInlining)] - static unsafe void Prefetch(void* address) - { - if (Sse.IsSupported) - { - Sse.Prefetch0(address); - //Sse.Prefetch0((byte*)address + 64); - //TODO: prefetch should grab cache line pair anyway, right? not much reason to explicitly do more? - } - //TODO: ARM? - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - static unsafe void PrefetchBundle(SolverState* stateBase, ref TwoBodyReferences references, int countInBundle) - { - var indicesA = (int*)Unsafe.AsPointer(ref references.IndexA); - var indicesB = (int*)Unsafe.AsPointer(ref references.IndexB); - for (int i = 0; i < countInBundle; ++i) - { - var indexA = indicesA[i]; - var indexB = indicesA[i]; - Prefetch(stateBase + indexA); - Prefetch(stateBase + indexB); - } - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - [Conditional("PREFETCH")] - unsafe static void EarlyPrefetch(int prefetchDistance, ref TypeBatch typeBatch, ref Buffer references, ref Buffer states, int startBundleIndex, int exclusiveEndBundleIndex) - { - exclusiveEndBundleIndex = Math.Min(exclusiveEndBundleIndex, startBundleIndex + prefetchDistance); - var lastBundleIndex = exclusiveEndBundleIndex - 1; - for (int i = startBundleIndex; i < lastBundleIndex; ++i) - { - PrefetchBundle(states.Memory, ref references[i], Vector.Count); - } - var countInBundle = GetCountInBundle(ref typeBatch, lastBundleIndex); - PrefetchBundle(states.Memory, ref references[lastBundleIndex], countInBundle); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - [Conditional("PREFETCH")] - unsafe static void Prefetch(int prefetchDistance, ref TypeBatch typeBatch, ref Buffer references, ref Buffer states, int bundleIndex, int exclusiveEndBundleIndex) - { - var targetIndex = bundleIndex + prefetchDistance; - if (targetIndex < exclusiveEndBundleIndex) - { - PrefetchBundle(states.Memory, ref references[targetIndex], GetCountInBundle(ref typeBatch, targetIndex)); - } - } - - public enum BundleIntegrationMode - { - None = 0, - Partial = 1, - All = 2 - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static BundleIntegrationMode BundleShouldIntegrate(int bundleIndex, in IndexSet integrationFlags, out Vector integrationMask) - { - Debug.Assert(Vector.Count <= 32, "Wait, what? The integration mask isn't big enough to handle a vector this big."); - var constraintStartIndex = bundleIndex * Vector.Count; - var flagBundleIndex = constraintStartIndex >> 6; - var flagInnerIndex = constraintStartIndex - (flagBundleIndex << 6); - var flagMask = (1 << Vector.Count) - 1; - var scalarIntegrationMask = ((int)(integrationFlags.Flags[flagBundleIndex] >> flagInnerIndex)) & flagMask; - if (scalarIntegrationMask == flagMask) - { - //No need to carefully expand a bitstring into a vector mask if we know that a single broadcast will suffice. - integrationMask = new Vector(-1); - return BundleIntegrationMode.All; - } - else if (scalarIntegrationMask > 0) - { - if (Vector.Count == 4 || Vector.Count == 8) - { - Vector selectors; - if (Vector.Count == 8) - { - selectors = Vector256.Create(1, 2, 4, 8, 16, 32, 64, 128).AsVector(); - } - else - { - selectors = Vector128.Create(1, 2, 4, 8).AsVector(); - } - var scalarBroadcast = new Vector(scalarIntegrationMask); - var selected = Vector.BitwiseAnd(selectors, scalarBroadcast); - integrationMask = Vector.Equals(selected, selectors); - } - else - { - //This is not a good implementation, but I don't know of any target platforms that will hit this. - //TODO: AVX512 being enabled by the runtime could force this path to be taken; it'll require an update! - Debug.Assert(Vector.Count <= 8, "The vector path assumes that AVX512 is not supported, so this is hitting a fallback path."); - Span mask = stackalloc int[Vector.Count]; - for (int i = 0; i < Vector.Count; ++i) - { - mask[i] = (scalarIntegrationMask & (1 << i)) > 0 ? -1 : 0; - } - integrationMask = new Vector(mask); - } - return BundleIntegrationMode.Partial; - } - integrationMask = default; - return BundleIntegrationMode.None; - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static unsafe void IntegratePoseAndVelocity( - ref TIntegratorCallbacks integratorCallbacks, ref Vector bodyIndices, int count, in BodyInertiaWide localInertia, float dt, in Vector integrationMask, - ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocityWide velocity, - int workerIndex, - out BodyInertiaWide inertia) - where TIntegratorCallbacks : struct, IPoseIntegratorCallbacks - { - //Note that we integrate pose, then velocity. - //We only use this function where we can guarantee that the external-to-timestep view of velocities and poses looks like the frame starts on a velocity integration and ends on a pose integration. - //This ensures that velocities set externally are still solved before being integrated. - //So, the solver runs velocity integration alone on the first substep. All later substeps then run pose + velocity, and then after the last substep, a final pose integration. - //This is equivalent in ordering to running each substep as velocity, warmstart, solve, pose integration, but just shifting the execution context. - var dtWide = new Vector(dt); - var newPosition = position + velocity.Linear * dtWide; - //Note that we only take results for slots which actually need integration. Reintegration would be an error. - Vector3Wide.ConditionalSelect(integrationMask, newPosition, position, out position); - QuaternionWide newOrientation; - inertia.InverseMass = localInertia.InverseMass; - var previousVelocity = velocity; - if (integratorCallbacks.AngularIntegrationMode == AngularIntegrationMode.ConserveMomentum) - { - var previousOrientation = orientation; - PoseIntegration.Integrate(orientation, velocity.Angular, dtWide * new Vector(0.5f), out newOrientation); - QuaternionWide.ConditionalSelect(integrationMask, newOrientation, orientation, out orientation); - PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, orientation, out inertia.InverseInertiaTensor); - PoseIntegration.IntegrateAngularVelocityConserveMomentum(previousOrientation, localInertia.InverseInertiaTensor, inertia.InverseInertiaTensor, ref velocity.Angular); - } - else if (integratorCallbacks.AngularIntegrationMode == AngularIntegrationMode.ConserveMomentumWithGyroscopicTorque) - { - PoseIntegration.Integrate(orientation, velocity.Angular, dtWide * new Vector(0.5f), out newOrientation); - QuaternionWide.ConditionalSelect(integrationMask, newOrientation, orientation, out orientation); - PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, orientation, out inertia.InverseInertiaTensor); - PoseIntegration.IntegrateAngularVelocityConserveMomentumWithGyroscopicTorque(orientation, localInertia.InverseInertiaTensor, ref velocity.Angular, dtWide); - } - else - { - PoseIntegration.Integrate(orientation, velocity.Angular, dtWide * new Vector(0.5f), out newOrientation); - QuaternionWide.ConditionalSelect(integrationMask, newOrientation, orientation, out orientation); - PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, orientation, out inertia.InverseInertiaTensor); - } - integratorCallbacks.IntegrateVelocity(new ReadOnlySpan(Unsafe.AsPointer(ref bodyIndices), count), position, orientation, localInertia, integrationMask, workerIndex, new Vector(dt), ref velocity); - //It would be annoying to make the user handle masking velocity writes to inactive lanes, so we handle it internally. - Vector3Wide.ConditionalSelect(integrationMask, velocity.Linear, previousVelocity.Linear, out velocity.Linear); - Vector3Wide.ConditionalSelect(integrationMask, velocity.Angular, previousVelocity.Angular, out velocity.Angular); - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static unsafe void IntegratePoseAndVelocity( - ref TIntegratorCallbacks integratorCallbacks, ref Vector bodyIndices, int count, in BodyInertiaWide localInertia, float dt, - ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocityWide velocity, - int workerIndex, - out BodyInertiaWide inertia) - where TIntegratorCallbacks : struct, IPoseIntegratorCallbacks - { - //This is identical to the other IntegratePoseAndVelocity, but it avoids any masking because we know ahead of time that the entire bundle is integrating. - var dtWide = new Vector(dt); - position += velocity.Linear * dtWide; - inertia.InverseMass = localInertia.InverseMass; - if (integratorCallbacks.AngularIntegrationMode == AngularIntegrationMode.ConserveMomentum) - { - var previousOrientation = orientation; - PoseIntegration.Integrate(orientation, velocity.Angular, dtWide * new Vector(0.5f), out orientation); - PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, orientation, out inertia.InverseInertiaTensor); - PoseIntegration.IntegrateAngularVelocityConserveMomentum(previousOrientation, localInertia.InverseInertiaTensor, inertia.InverseInertiaTensor, ref velocity.Angular); - } - else if (integratorCallbacks.AngularIntegrationMode == AngularIntegrationMode.ConserveMomentumWithGyroscopicTorque) - { - PoseIntegration.Integrate(orientation, velocity.Angular, dtWide * new Vector(0.5f), out orientation); - PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, orientation, out inertia.InverseInertiaTensor); - PoseIntegration.IntegrateAngularVelocityConserveMomentumWithGyroscopicTorque(orientation, localInertia.InverseInertiaTensor, ref velocity.Angular, dtWide); - } - else - { - PoseIntegration.Integrate(orientation, velocity.Angular, dtWide * new Vector(0.5f), out orientation); - PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, orientation, out inertia.InverseInertiaTensor); - } - integratorCallbacks.IntegrateVelocity(new ReadOnlySpan(Unsafe.AsPointer(ref bodyIndices), count), position, orientation, localInertia, new Vector(-1), workerIndex, new Vector(dt), ref velocity); - } - - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static unsafe void IntegrateVelocity( - ref TIntegratorCallbacks integratorCallbacks, ref Vector bodyIndices, int count, in BodyInertiaWide localInertia, float dt, in Vector integrationMask, - in Vector3Wide position, in QuaternionWide orientation, ref BodyVelocityWide velocity, - int workerIndex, - out BodyInertiaWide inertia) - where TIntegratorCallbacks : struct, IPoseIntegratorCallbacks - where TBatchIntegrationMode : unmanaged, IBatchIntegrationMode - { - inertia.InverseMass = localInertia.InverseMass; - PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, orientation, out inertia.InverseInertiaTensor); - if (integratorCallbacks.AngularIntegrationMode == AngularIntegrationMode.ConserveMomentum) - { - //Yes, that's integrating backwards to get a previous orientation to convert to momentum. Yup, that's a bit janky. - PoseIntegration.Integrate(orientation, velocity.Angular, new Vector(dt * -0.5f), out var previousOrientation); - PoseIntegration.IntegrateAngularVelocityConserveMomentum(previousOrientation, localInertia.InverseInertiaTensor, inertia.InverseInertiaTensor, ref velocity.Angular); - } - else if (integratorCallbacks.AngularIntegrationMode == AngularIntegrationMode.ConserveMomentumWithGyroscopicTorque) - { - PoseIntegration.IntegrateAngularVelocityConserveMomentumWithGyroscopicTorque(orientation, localInertia.InverseInertiaTensor, ref velocity.Angular, new Vector(dt)); - } - if (typeof(TBatchIntegrationMode) == typeof(BatchShouldConditionallyIntegrate)) - { - var previousVelocity = velocity; - integratorCallbacks.IntegrateVelocity(new ReadOnlySpan(Unsafe.AsPointer(ref bodyIndices), count), position, orientation, localInertia, integrationMask, workerIndex, new Vector(dt), ref velocity); - //It would be annoying to make the user handle masking velocity writes to inactive lanes, so we handle it internally. - Vector3Wide.ConditionalSelect(integrationMask, velocity.Linear, previousVelocity.Linear, out velocity.Linear); - Vector3Wide.ConditionalSelect(integrationMask, velocity.Angular, previousVelocity.Angular, out velocity.Angular); - } - else - { - integratorCallbacks.IntegrateVelocity(new ReadOnlySpan(Unsafe.AsPointer(ref bodyIndices), count), position, orientation, localInertia, integrationMask, workerIndex, new Vector(dt), ref velocity); - } - } - - //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public static unsafe void GatherAndIntegrate( - Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, ref Buffer integrationFlags, int bodyIndexInConstraint, float dt, int workerIndex, int bundleIndex, - ref Vector bodyIndices, int count, out Vector3Wide position, out QuaternionWide orientation, out BodyVelocityWide velocity, out BodyInertiaWide inertia) - where TIntegratorCallbacks : struct, IPoseIntegratorCallbacks - where TBatchIntegrationMode : unmanaged, IBatchIntegrationMode - where TAccessFilter : unmanaged, IBodyAccessFilter - where TShouldIntegratePoses : unmanaged, IBatchPoseIntegrationAllowed - { - //These type tests are compile time constants and will be specialized. - if (typeof(TShouldIntegratePoses) == typeof(BatchShouldIntegratePoses)) - { - if (typeof(TBatchIntegrationMode) == typeof(BatchShouldAlwaysIntegrate)) - { - var integrationMask = BundleIndexing.CreateMaskForCountInBundle(count); - bodies.GatherState(ref bodyIndices, count, false, out position, out orientation, out velocity, out var localInertia); - IntegratePoseAndVelocity(ref integratorCallbacks, ref bodyIndices, count, localInertia, dt, ref position, ref orientation, ref velocity, workerIndex, out inertia); - bodies.ScatterPose(ref position, ref orientation, ref bodyIndices, ref integrationMask); - bodies.ScatterInertia(ref inertia, ref bodyIndices, ref integrationMask); - } - else if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) - { - bodies.GatherState(ref bodyIndices, count, true, out position, out orientation, out velocity, out inertia); - } - else - { - Debug.Assert(typeof(TBatchIntegrationMode) == typeof(BatchShouldConditionallyIntegrate)); - //This executes in warmstart, and warmstarts are typically quite simple from an instruction stream perspective. - //Having a dynamically chosen codepath is unlikely to cause instruction fetching issues. - var bundleIntegrationMode = BundleShouldIntegrate(bundleIndex, integrationFlags[bodyIndexInConstraint], out var integrationMask); - //Note that this will gather world inertia if there is no integration in the bundle, but that it is guaranteed to load all motion state information. - //This avoids complexity around later velocity scattering- we don't have to condition on whether the bundle is integrating. - //In practice, since the access filters are only reducing instruction counts and not memory bandwidth, - //the slightly increased unnecessary gathering is no worse than the more complex scatter condition in performance, and remains simpler. - bodies.GatherState(ref bodyIndices, count, bundleIntegrationMode == BundleIntegrationMode.None, out position, out orientation, out velocity, out var gatheredInertia); - if (bundleIntegrationMode != BundleIntegrationMode.None) - { - //Note that if we take this codepath, the integration routine will reconstruct the world inertias from local inertia given the current pose. - //The changes to pose and velocity for integration inactive lanes will be masked out, so it'll just be identical to the world inertia if we had gathered it. - //Given that we're running the instructions in a bundle to build it, there's no reason to go out of our way to gather the world inertia. - IntegratePoseAndVelocity(ref integratorCallbacks, ref bodyIndices, count, gatheredInertia, dt, ref position, ref orientation, ref velocity, workerIndex, out inertia); - bodies.ScatterPose(ref position, ref orientation, ref bodyIndices, ref integrationMask); - bodies.ScatterInertia(ref inertia, ref bodyIndices, ref integrationMask); - } - else - { - inertia = gatheredInertia; - } - } - } - else - { - Debug.Assert(typeof(TShouldIntegratePoses) == typeof(BatchShouldNotIntegratePoses)); - //There is no need to integrate poses; this is the first substep. - //Note that the full loop for constrained bodies with 3 substeps looks like: - //(velocity -> solve) -> (pose -> velocity -> solve) -> (pose -> velocity -> solve) -> pose - //For unconstrained bodies, it's a tight loop of just: - //(velocity -> pose) -> (velocity -> pose) -> (velocity -> pose) - //So we're maintaining the same order. - //Note that world inertia is still scattered as a part of velocity integration; we need the updated value since we can't trust the cached value across frames. - if (typeof(TBatchIntegrationMode) == typeof(BatchShouldAlwaysIntegrate)) - { - var integrationMask = BundleIndexing.CreateMaskForCountInBundle(count); - bodies.GatherState(ref bodyIndices, count, false, out position, out orientation, out velocity, out var localInertia); - IntegrateVelocity(ref integratorCallbacks, ref bodyIndices, count, localInertia, dt, integrationMask, position, orientation, ref velocity, workerIndex, out inertia); - bodies.ScatterInertia(ref inertia, ref bodyIndices, ref integrationMask); - } - else if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) - { - bodies.GatherState(ref bodyIndices, count, true, out position, out orientation, out velocity, out inertia); - } - else - { - Debug.Assert(typeof(TBatchIntegrationMode) == typeof(BatchShouldConditionallyIntegrate)); - var bundleIntegrationMode = BundleShouldIntegrate(bundleIndex, integrationFlags[bodyIndexInConstraint], out var integrationMask); - bodies.GatherState(ref bodyIndices, count, bundleIntegrationMode == BundleIntegrationMode.None, out position, out orientation, out velocity, out var gatheredInertia); - if (bundleIntegrationMode != BundleIntegrationMode.None) - { - IntegrateVelocity(ref integratorCallbacks, ref bodyIndices, count, gatheredInertia, dt, integrationMask, position, orientation, ref velocity, workerIndex, out inertia); - bodies.ScatterInertia(ref inertia, ref bodyIndices, ref integrationMask); - } - else - { - inertia = gatheredInertia; - } - } - } - } - public unsafe override void WarmStart2( ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) @@ -629,14 +322,14 @@ public unsafe override void WarmStart2(); var function = default(TConstraintFunctions); ref var states = ref bodies.ActiveSet.SolverStates; - EarlyPrefetch(warmStartPrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref states, startBundle, exclusiveEndBundle); + EarlyPrefetch(WarmStartPrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref states, startBundle, exclusiveEndBundle); for (int i = startBundle; i < exclusiveEndBundle; ++i) { ref var prestep = ref prestepBundles[i]; ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; ref var references = ref bodyReferencesBundles[i]; var count = GetCountInBundle(ref typeBatch, i); - Prefetch(warmStartPrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref states, i, exclusiveEndBundle); + Prefetch(WarmStartPrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref states, i, exclusiveEndBundle); GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 0, dt, workerIndex, i, ref references.IndexA, count, out var positionA, out var orientationA, out var wsvA, out var inertiaA); GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 1, dt, workerIndex, i, ref references.IndexB, count, @@ -686,14 +379,14 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); var function = default(TConstraintFunctions); ref var motionStates = ref bodies.ActiveSet.SolverStates; - EarlyPrefetch(solvePrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, startBundle, exclusiveEndBundle); + EarlyPrefetch(SolvePrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, startBundle, exclusiveEndBundle); for (int i = startBundle; i < exclusiveEndBundle; ++i) { ref var prestep = ref prestepBundles[i]; ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; ref var references = ref bodyReferencesBundles[i]; var count = GetCountInBundle(ref typeBatch, i); - Prefetch(solvePrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, i, exclusiveEndBundle); + Prefetch(SolvePrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, i, exclusiveEndBundle); bodies.GatherState(ref references.IndexA, count, true, out var positionA, out var orientationA, out var wsvA, out var inertiaA); bodies.GatherState(ref references.IndexB, count, true, out var positionB, out var orientationB, out var wsvB, out var inertiaB); var ab = positionB - positionA; diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index f0fffe42d..7857a447c 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -5,6 +5,8 @@ using System.Runtime.CompilerServices; using BepuUtilities.Collections; using BepuUtilities; +using System.Runtime.Intrinsics; +using System.Runtime.Intrinsics.X86; namespace BepuPhysics.Constraints { @@ -692,7 +694,321 @@ internal override int GetBodyReferenceCount(ref TypeBatch typeBatch, int bodyToF } return count; } + + + public const int WarmStartPrefetchDistance = 8; + public const int SolvePrefetchDistance = 4; + [MethodImpl(MethodImplOptions.AggressiveInlining)] + static unsafe void Prefetch(void* address) + { + if (Sse.IsSupported) + { + Sse.Prefetch0(address); + //Sse.Prefetch0((byte*)address + 64); + //TODO: prefetch should grab cache line pair anyway, right? not much reason to explicitly do more? + } + //TODO: ARM? + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + static unsafe void PrefetchBundle(SolverState* stateBase, ref TwoBodyReferences references, int countInBundle) + { + var indicesA = (int*)Unsafe.AsPointer(ref references.IndexA); + var indicesB = (int*)Unsafe.AsPointer(ref references.IndexB); + for (int i = 0; i < countInBundle; ++i) + { + var indexA = indicesA[i]; + var indexB = indicesA[i]; + Prefetch(stateBase + indexA); + Prefetch(stateBase + indexB); + } + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + [Conditional("PREFETCH")] + public unsafe static void EarlyPrefetch(int prefetchDistance, ref TypeBatch typeBatch, ref Buffer references, ref Buffer states, int startBundleIndex, int exclusiveEndBundleIndex) + { + exclusiveEndBundleIndex = Math.Min(exclusiveEndBundleIndex, startBundleIndex + prefetchDistance); + var lastBundleIndex = exclusiveEndBundleIndex - 1; + for (int i = startBundleIndex; i < lastBundleIndex; ++i) + { + PrefetchBundle(states.Memory, ref references[i], Vector.Count); + } + var countInBundle = GetCountInBundle(ref typeBatch, lastBundleIndex); + PrefetchBundle(states.Memory, ref references[lastBundleIndex], countInBundle); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + [Conditional("PREFETCH")] + public unsafe static void Prefetch(int prefetchDistance, ref TypeBatch typeBatch, ref Buffer references, ref Buffer states, int bundleIndex, int exclusiveEndBundleIndex) + { + var targetIndex = bundleIndex + prefetchDistance; + if (targetIndex < exclusiveEndBundleIndex) + { + PrefetchBundle(states.Memory, ref references[targetIndex], GetCountInBundle(ref typeBatch, targetIndex)); + } + } + + public enum BundleIntegrationMode + { + None = 0, + Partial = 1, + All = 2 + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static BundleIntegrationMode BundleShouldIntegrate(int bundleIndex, in IndexSet integrationFlags, out Vector integrationMask) + { + Debug.Assert(Vector.Count <= 32, "Wait, what? The integration mask isn't big enough to handle a vector this big."); + var constraintStartIndex = bundleIndex * Vector.Count; + var flagBundleIndex = constraintStartIndex >> 6; + var flagInnerIndex = constraintStartIndex - (flagBundleIndex << 6); + var flagMask = (1 << Vector.Count) - 1; + var scalarIntegrationMask = ((int)(integrationFlags.Flags[flagBundleIndex] >> flagInnerIndex)) & flagMask; + if (scalarIntegrationMask == flagMask) + { + //No need to carefully expand a bitstring into a vector mask if we know that a single broadcast will suffice. + integrationMask = new Vector(-1); + return BundleIntegrationMode.All; + } + else if (scalarIntegrationMask > 0) + { + if (Vector.Count == 4 || Vector.Count == 8) + { + Vector selectors; + if (Vector.Count == 8) + { + selectors = Vector256.Create(1, 2, 4, 8, 16, 32, 64, 128).AsVector(); + } + else + { + selectors = Vector128.Create(1, 2, 4, 8).AsVector(); + } + var scalarBroadcast = new Vector(scalarIntegrationMask); + var selected = Vector.BitwiseAnd(selectors, scalarBroadcast); + integrationMask = Vector.Equals(selected, selectors); + } + else + { + //This is not a good implementation, but I don't know of any target platforms that will hit this. + //TODO: AVX512 being enabled by the runtime could force this path to be taken; it'll require an update! + Debug.Assert(Vector.Count <= 8, "The vector path assumes that AVX512 is not supported, so this is hitting a fallback path."); + Span mask = stackalloc int[Vector.Count]; + for (int i = 0; i < Vector.Count; ++i) + { + mask[i] = (scalarIntegrationMask & (1 << i)) > 0 ? -1 : 0; + } + integrationMask = new Vector(mask); + } + return BundleIntegrationMode.Partial; + } + integrationMask = default; + return BundleIntegrationMode.None; + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static unsafe void IntegratePoseAndVelocity( + ref TIntegratorCallbacks integratorCallbacks, ref Vector bodyIndices, int count, in BodyInertiaWide localInertia, float dt, in Vector integrationMask, + ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocityWide velocity, + int workerIndex, + out BodyInertiaWide inertia) + where TIntegratorCallbacks : struct, IPoseIntegratorCallbacks + { + //Note that we integrate pose, then velocity. + //We only use this function where we can guarantee that the external-to-timestep view of velocities and poses looks like the frame starts on a velocity integration and ends on a pose integration. + //This ensures that velocities set externally are still solved before being integrated. + //So, the solver runs velocity integration alone on the first substep. All later substeps then run pose + velocity, and then after the last substep, a final pose integration. + //This is equivalent in ordering to running each substep as velocity, warmstart, solve, pose integration, but just shifting the execution context. + var dtWide = new Vector(dt); + var newPosition = position + velocity.Linear * dtWide; + //Note that we only take results for slots which actually need integration. Reintegration would be an error. + Vector3Wide.ConditionalSelect(integrationMask, newPosition, position, out position); + QuaternionWide newOrientation; + inertia.InverseMass = localInertia.InverseMass; + var previousVelocity = velocity; + if (integratorCallbacks.AngularIntegrationMode == AngularIntegrationMode.ConserveMomentum) + { + var previousOrientation = orientation; + PoseIntegration.Integrate(orientation, velocity.Angular, dtWide * new Vector(0.5f), out newOrientation); + QuaternionWide.ConditionalSelect(integrationMask, newOrientation, orientation, out orientation); + PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, orientation, out inertia.InverseInertiaTensor); + PoseIntegration.IntegrateAngularVelocityConserveMomentum(previousOrientation, localInertia.InverseInertiaTensor, inertia.InverseInertiaTensor, ref velocity.Angular); + } + else if (integratorCallbacks.AngularIntegrationMode == AngularIntegrationMode.ConserveMomentumWithGyroscopicTorque) + { + PoseIntegration.Integrate(orientation, velocity.Angular, dtWide * new Vector(0.5f), out newOrientation); + QuaternionWide.ConditionalSelect(integrationMask, newOrientation, orientation, out orientation); + PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, orientation, out inertia.InverseInertiaTensor); + PoseIntegration.IntegrateAngularVelocityConserveMomentumWithGyroscopicTorque(orientation, localInertia.InverseInertiaTensor, ref velocity.Angular, dtWide); + } + else + { + PoseIntegration.Integrate(orientation, velocity.Angular, dtWide * new Vector(0.5f), out newOrientation); + QuaternionWide.ConditionalSelect(integrationMask, newOrientation, orientation, out orientation); + PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, orientation, out inertia.InverseInertiaTensor); + } + integratorCallbacks.IntegrateVelocity(new ReadOnlySpan(Unsafe.AsPointer(ref bodyIndices), count), position, orientation, localInertia, integrationMask, workerIndex, new Vector(dt), ref velocity); + //It would be annoying to make the user handle masking velocity writes to inactive lanes, so we handle it internally. + Vector3Wide.ConditionalSelect(integrationMask, velocity.Linear, previousVelocity.Linear, out velocity.Linear); + Vector3Wide.ConditionalSelect(integrationMask, velocity.Angular, previousVelocity.Angular, out velocity.Angular); + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static unsafe void IntegratePoseAndVelocity( + ref TIntegratorCallbacks integratorCallbacks, ref Vector bodyIndices, int count, in BodyInertiaWide localInertia, float dt, + ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocityWide velocity, + int workerIndex, + out BodyInertiaWide inertia) + where TIntegratorCallbacks : struct, IPoseIntegratorCallbacks + { + //This is identical to the other IntegratePoseAndVelocity, but it avoids any masking because we know ahead of time that the entire bundle is integrating. + var dtWide = new Vector(dt); + position += velocity.Linear * dtWide; + inertia.InverseMass = localInertia.InverseMass; + if (integratorCallbacks.AngularIntegrationMode == AngularIntegrationMode.ConserveMomentum) + { + var previousOrientation = orientation; + PoseIntegration.Integrate(orientation, velocity.Angular, dtWide * new Vector(0.5f), out orientation); + PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, orientation, out inertia.InverseInertiaTensor); + PoseIntegration.IntegrateAngularVelocityConserveMomentum(previousOrientation, localInertia.InverseInertiaTensor, inertia.InverseInertiaTensor, ref velocity.Angular); + } + else if (integratorCallbacks.AngularIntegrationMode == AngularIntegrationMode.ConserveMomentumWithGyroscopicTorque) + { + PoseIntegration.Integrate(orientation, velocity.Angular, dtWide * new Vector(0.5f), out orientation); + PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, orientation, out inertia.InverseInertiaTensor); + PoseIntegration.IntegrateAngularVelocityConserveMomentumWithGyroscopicTorque(orientation, localInertia.InverseInertiaTensor, ref velocity.Angular, dtWide); + } + else + { + PoseIntegration.Integrate(orientation, velocity.Angular, dtWide * new Vector(0.5f), out orientation); + PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, orientation, out inertia.InverseInertiaTensor); + } + integratorCallbacks.IntegrateVelocity(new ReadOnlySpan(Unsafe.AsPointer(ref bodyIndices), count), position, orientation, localInertia, new Vector(-1), workerIndex, new Vector(dt), ref velocity); + } + + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static unsafe void IntegrateVelocity( + ref TIntegratorCallbacks integratorCallbacks, ref Vector bodyIndices, int count, in BodyInertiaWide localInertia, float dt, in Vector integrationMask, + in Vector3Wide position, in QuaternionWide orientation, ref BodyVelocityWide velocity, + int workerIndex, + out BodyInertiaWide inertia) + where TIntegratorCallbacks : struct, IPoseIntegratorCallbacks + where TBatchIntegrationMode : unmanaged, IBatchIntegrationMode + { + inertia.InverseMass = localInertia.InverseMass; + PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, orientation, out inertia.InverseInertiaTensor); + if (integratorCallbacks.AngularIntegrationMode == AngularIntegrationMode.ConserveMomentum) + { + //Yes, that's integrating backwards to get a previous orientation to convert to momentum. Yup, that's a bit janky. + PoseIntegration.Integrate(orientation, velocity.Angular, new Vector(dt * -0.5f), out var previousOrientation); + PoseIntegration.IntegrateAngularVelocityConserveMomentum(previousOrientation, localInertia.InverseInertiaTensor, inertia.InverseInertiaTensor, ref velocity.Angular); + } + else if (integratorCallbacks.AngularIntegrationMode == AngularIntegrationMode.ConserveMomentumWithGyroscopicTorque) + { + PoseIntegration.IntegrateAngularVelocityConserveMomentumWithGyroscopicTorque(orientation, localInertia.InverseInertiaTensor, ref velocity.Angular, new Vector(dt)); + } + if (typeof(TBatchIntegrationMode) == typeof(BatchShouldConditionallyIntegrate)) + { + var previousVelocity = velocity; + integratorCallbacks.IntegrateVelocity(new ReadOnlySpan(Unsafe.AsPointer(ref bodyIndices), count), position, orientation, localInertia, integrationMask, workerIndex, new Vector(dt), ref velocity); + //It would be annoying to make the user handle masking velocity writes to inactive lanes, so we handle it internally. + Vector3Wide.ConditionalSelect(integrationMask, velocity.Linear, previousVelocity.Linear, out velocity.Linear); + Vector3Wide.ConditionalSelect(integrationMask, velocity.Angular, previousVelocity.Angular, out velocity.Angular); + } + else + { + integratorCallbacks.IntegrateVelocity(new ReadOnlySpan(Unsafe.AsPointer(ref bodyIndices), count), position, orientation, localInertia, integrationMask, workerIndex, new Vector(dt), ref velocity); + } + } + + //[MethodImpl(MethodImplOptions.AggressiveInlining)] + public static unsafe void GatherAndIntegrate( + Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, ref Buffer integrationFlags, int bodyIndexInConstraint, float dt, int workerIndex, int bundleIndex, + ref Vector bodyIndices, int count, out Vector3Wide position, out QuaternionWide orientation, out BodyVelocityWide velocity, out BodyInertiaWide inertia) + where TIntegratorCallbacks : struct, IPoseIntegratorCallbacks + where TBatchIntegrationMode : unmanaged, IBatchIntegrationMode + where TAccessFilter : unmanaged, IBodyAccessFilter + where TShouldIntegratePoses : unmanaged, IBatchPoseIntegrationAllowed + { + //These type tests are compile time constants and will be specialized. + if (typeof(TShouldIntegratePoses) == typeof(BatchShouldIntegratePoses)) + { + if (typeof(TBatchIntegrationMode) == typeof(BatchShouldAlwaysIntegrate)) + { + var integrationMask = BundleIndexing.CreateMaskForCountInBundle(count); + bodies.GatherState(ref bodyIndices, count, false, out position, out orientation, out velocity, out var localInertia); + IntegratePoseAndVelocity(ref integratorCallbacks, ref bodyIndices, count, localInertia, dt, ref position, ref orientation, ref velocity, workerIndex, out inertia); + bodies.ScatterPose(ref position, ref orientation, ref bodyIndices, ref integrationMask); + bodies.ScatterInertia(ref inertia, ref bodyIndices, ref integrationMask); + } + else if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) + { + bodies.GatherState(ref bodyIndices, count, true, out position, out orientation, out velocity, out inertia); + } + else + { + Debug.Assert(typeof(TBatchIntegrationMode) == typeof(BatchShouldConditionallyIntegrate)); + //This executes in warmstart, and warmstarts are typically quite simple from an instruction stream perspective. + //Having a dynamically chosen codepath is unlikely to cause instruction fetching issues. + var bundleIntegrationMode = BundleShouldIntegrate(bundleIndex, integrationFlags[bodyIndexInConstraint], out var integrationMask); + //Note that this will gather world inertia if there is no integration in the bundle, but that it is guaranteed to load all motion state information. + //This avoids complexity around later velocity scattering- we don't have to condition on whether the bundle is integrating. + //In practice, since the access filters are only reducing instruction counts and not memory bandwidth, + //the slightly increased unnecessary gathering is no worse than the more complex scatter condition in performance, and remains simpler. + bodies.GatherState(ref bodyIndices, count, bundleIntegrationMode == BundleIntegrationMode.None, out position, out orientation, out velocity, out var gatheredInertia); + if (bundleIntegrationMode != BundleIntegrationMode.None) + { + //Note that if we take this codepath, the integration routine will reconstruct the world inertias from local inertia given the current pose. + //The changes to pose and velocity for integration inactive lanes will be masked out, so it'll just be identical to the world inertia if we had gathered it. + //Given that we're running the instructions in a bundle to build it, there's no reason to go out of our way to gather the world inertia. + IntegratePoseAndVelocity(ref integratorCallbacks, ref bodyIndices, count, gatheredInertia, dt, ref position, ref orientation, ref velocity, workerIndex, out inertia); + bodies.ScatterPose(ref position, ref orientation, ref bodyIndices, ref integrationMask); + bodies.ScatterInertia(ref inertia, ref bodyIndices, ref integrationMask); + } + else + { + inertia = gatheredInertia; + } + } + } + else + { + Debug.Assert(typeof(TShouldIntegratePoses) == typeof(BatchShouldNotIntegratePoses)); + //There is no need to integrate poses; this is the first substep. + //Note that the full loop for constrained bodies with 3 substeps looks like: + //(velocity -> solve) -> (pose -> velocity -> solve) -> (pose -> velocity -> solve) -> pose + //For unconstrained bodies, it's a tight loop of just: + //(velocity -> pose) -> (velocity -> pose) -> (velocity -> pose) + //So we're maintaining the same order. + //Note that world inertia is still scattered as a part of velocity integration; we need the updated value since we can't trust the cached value across frames. + if (typeof(TBatchIntegrationMode) == typeof(BatchShouldAlwaysIntegrate)) + { + var integrationMask = BundleIndexing.CreateMaskForCountInBundle(count); + bodies.GatherState(ref bodyIndices, count, false, out position, out orientation, out velocity, out var localInertia); + IntegrateVelocity(ref integratorCallbacks, ref bodyIndices, count, localInertia, dt, integrationMask, position, orientation, ref velocity, workerIndex, out inertia); + bodies.ScatterInertia(ref inertia, ref bodyIndices, ref integrationMask); + } + else if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) + { + bodies.GatherState(ref bodyIndices, count, true, out position, out orientation, out velocity, out inertia); + } + else + { + Debug.Assert(typeof(TBatchIntegrationMode) == typeof(BatchShouldConditionallyIntegrate)); + var bundleIntegrationMode = BundleShouldIntegrate(bundleIndex, integrationFlags[bodyIndexInConstraint], out var integrationMask); + bodies.GatherState(ref bodyIndices, count, bundleIntegrationMode == BundleIntegrationMode.None, out position, out orientation, out velocity, out var gatheredInertia); + if (bundleIntegrationMode != BundleIntegrationMode.None) + { + IntegrateVelocity(ref integratorCallbacks, ref bodyIndices, count, gatheredInertia, dt, integrationMask, position, orientation, ref velocity, workerIndex, out inertia); + bodies.ScatterInertia(ref inertia, ref bodyIndices, ref integrationMask); + } + else + { + inertia = gatheredInertia; + } + } + } + } + } -} +} \ No newline at end of file diff --git a/Demos/Demos/Characters/CharacterMotionConstraint.cs b/Demos/Demos/Characters/CharacterMotionConstraint.cs index d2cda1838..361055488 100644 --- a/Demos/Demos/Characters/CharacterMotionConstraint.cs +++ b/Demos/Demos/Characters/CharacterMotionConstraint.cs @@ -306,6 +306,15 @@ public void Solve(ref BodyVelocityWide velocityA, ref StaticCharacterMotionProje ApplyVerticalImpulse(basis, verticalAngularJacobianA, verticalCorrectiveImpulse, projection.InertiaA, ref velocityA); } + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in DynamicCharacterMotionPrestep prestep, in CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } + + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, in DynamicCharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } } //Each constraint type has its own 'type processor'- it acts as the outer loop that handles all the common logic across batches of constraints and invokes @@ -638,6 +647,15 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ApplyVerticalImpulse(basis, verticalAngularJacobianA, verticalAngularJacobianB, verticalCorrectiveImpulse, projection.InertiaA, projection.InertiaB, ref velocityA, ref velocityB); } + public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in DynamicCharacterMotionPrestep prestep, in CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } + + public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in DynamicCharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } } //Each constraint type has its own 'type processor'- it acts as the outer loop that handles all the common logic across batches of constraints and invokes diff --git a/Demos/Demos/Characters/CharacterMotionConstraint.tt b/Demos/Demos/Characters/CharacterMotionConstraint.tt index 8335557e8..9a4353705 100644 --- a/Demos/Demos/Characters/CharacterMotionConstraint.tt +++ b/Demos/Demos/Characters/CharacterMotionConstraint.tt @@ -393,6 +393,15 @@ namespace Demos.Demos.Characters ApplyVerticalImpulse(basis, verticalAngularJacobianA, <#if (dynamic) {#>verticalAngularJacobianB, <#}#>verticalCorrectiveImpulse, projection.InertiaA, <#if (dynamic) {#>projection.InertiaB, <#}#>ref velocityA<#if (dynamic) {#>, ref velocityB<#}#>); } + public void WarmStart2(<#if(!dynamic) {#>in Vector3Wide positionA, <#}#>in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(dynamic) {#>in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>in DynamicCharacterMotionPrestep prestep, in CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } + + public void Solve2(<#if(!dynamic) {#>in Vector3Wide positionA, <#}#>in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(dynamic) {#>in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>float dt, float inverseDt, in DynamicCharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + throw new NotImplementedException(); + } } //Each constraint type has its own 'type processor'- it acts as the outer loop that handles all the common logic across batches of constraints and invokes From 7f3a9875e64146d1f489ae8c52024b1fe4cf7810 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 21 Jul 2021 18:36:09 -0500 Subject: [PATCH 128/947] Interface for one, three, and four body, plus fixes. --- BepuPhysics/Constraints/AreaConstraint.cs | 10 ++++++++++ .../Constraints/Contact/ContactConvexTypes.cs | 16 ++++++++-------- .../Constraints/Contact/ContactConvexTypes.tt | 4 ++-- .../Contact/ContactNonconvexCommon.cs | 10 ++++++++++ BepuPhysics/Constraints/FourBodyTypeProcessor.cs | 12 ++++++++++++ BepuPhysics/Constraints/OneBodyAngularMotor.cs | 10 ++++++++++ BepuPhysics/Constraints/OneBodyAngularServo.cs | 10 ++++++++++ BepuPhysics/Constraints/OneBodyLinearMotor.cs | 9 +++++++++ BepuPhysics/Constraints/OneBodyLinearServo.cs | 9 +++++++++ BepuPhysics/Constraints/OneBodyTypeProcessor.cs | 5 +++++ .../Constraints/ThreeBodyTypeProcessor.cs | 13 ++++++++++++- BepuPhysics/Constraints/VolumeConstraint.cs | 9 +++++++++ .../Characters/CharacterMotionConstraint.cs | 4 ++-- .../Characters/CharacterMotionConstraint.tt | 4 ++-- 14 files changed, 110 insertions(+), 15 deletions(-) diff --git a/BepuPhysics/Constraints/AreaConstraint.cs b/BepuPhysics/Constraints/AreaConstraint.cs index 577379737..905819f3c 100644 --- a/BepuPhysics/Constraints/AreaConstraint.cs +++ b/BepuPhysics/Constraints/AreaConstraint.cs @@ -184,6 +184,16 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ApplyImpulse(ref velocityA, ref velocityB, ref velocityC, ref projection, ref negatedJacobianA, ref csi); } + + public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide ac, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in AreaConstraintPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC) + { + throw new NotImplementedException(); + } + + public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide ac, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, float dt, float inverseDt, in AreaConstraintPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC) + { + throw new NotImplementedException(); + } } diff --git a/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs b/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs index 649cb46f5..373400a1a 100644 --- a/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs +++ b/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs @@ -352,13 +352,13 @@ public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityW } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Contact1OneBodyPrestepData prestep, in Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Contact1OneBodyPrestepData prestep, in Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { throw new NotImplementedException(); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, in Contact1OneBodyPrestepData prestep, ref Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, in Contact1OneBodyPrestepData prestep, ref Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { throw new NotImplementedException(); } @@ -544,13 +544,13 @@ public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityW } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Contact2OneBodyPrestepData prestep, in Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Contact2OneBodyPrestepData prestep, in Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { throw new NotImplementedException(); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, in Contact2OneBodyPrestepData prestep, ref Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, in Contact2OneBodyPrestepData prestep, ref Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { throw new NotImplementedException(); } @@ -750,13 +750,13 @@ public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityW } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Contact3OneBodyPrestepData prestep, in Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Contact3OneBodyPrestepData prestep, in Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { throw new NotImplementedException(); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, in Contact3OneBodyPrestepData prestep, ref Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, in Contact3OneBodyPrestepData prestep, ref Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { throw new NotImplementedException(); } @@ -970,13 +970,13 @@ public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityW } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Contact4OneBodyPrestepData prestep, in Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Contact4OneBodyPrestepData prestep, in Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { throw new NotImplementedException(); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, in Contact4OneBodyPrestepData prestep, ref Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, in Contact4OneBodyPrestepData prestep, ref Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt b/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt index e22b047c0..c23e10985 100644 --- a/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt +++ b/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt @@ -321,13 +321,13 @@ for (int i = 0; i < contactCount; ++i) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart2(<#if(bodyCount == 1) {#>in Vector3Wide positionA, <#}#>in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(bodyCount == 2) {#>in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>in Contact<#=contactCount#><#=suffix#>PrestepData prestep, in Contact<#=contactCount#>AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(<#if(bodyCount == 1) {#>in Vector3Wide positionA, <#}#>in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(bodyCount == 2) {#>in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>in Contact<#=contactCount#><#=suffix#>PrestepData prestep, in Contact<#=contactCount#>AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA<#if(bodyCount == 2) {#>, ref BodyVelocityWide wsvB<#}#>) { throw new NotImplementedException(); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve2(<#if(bodyCount == 1) {#>in Vector3Wide positionA, <#}#>in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(bodyCount == 2) {#>in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>float dt, float inverseDt, in Contact<#=contactCount#><#=suffix#>PrestepData prestep, ref Contact<#=contactCount#>AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(<#if(bodyCount == 1) {#>in Vector3Wide positionA, <#}#>in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(bodyCount == 2) {#>in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>float dt, float inverseDt, in Contact<#=contactCount#><#=suffix#>PrestepData prestep, ref Contact<#=contactCount#>AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA<#if(bodyCount == 2) {#>, ref BodyVelocityWide wsvB<#}#>) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs b/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs index aa439c2e6..8a4298981 100644 --- a/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs +++ b/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs @@ -293,6 +293,16 @@ public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityW PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestepContact.Offset, prestepContact.Normal, velocity, ref prestepContact.Depth); } } + + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in TPrestep prestep, in TAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) + { + throw new System.NotImplementedException(); + } + + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, in TPrestep prestep, ref TAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) + { + throw new System.NotImplementedException(); + } } public struct ContactNonconvexTwoBodyFunctions : diff --git a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs index 9bff60817..eb92feff8 100644 --- a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs @@ -35,6 +35,18 @@ void Prestep( float dt, float inverseDt, ref TPrestepData prestepData, out TProjection projection); void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref BodyVelocityWide velocityC, ref BodyVelocityWide velocityD, ref TProjection projection, ref TAccumulatedImpulse accumulatedImpulse); void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref BodyVelocityWide velocityC, ref BodyVelocityWide velocityD, ref TProjection projection, ref TAccumulatedImpulse accumulatedImpulse); + void WarmStart2( + in QuaternionWide orientationA, in BodyInertiaWide inertiaA, + in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, + in Vector3Wide ac, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, + in Vector3Wide ad, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, + in TPrestepData prestep, in TAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC, ref BodyVelocityWide wsvD); + void Solve2( + in QuaternionWide orientationA, in BodyInertiaWide inertiaA, + in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, + in Vector3Wide ac, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, + in Vector3Wide ad, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, float dt, float inverseDt, + in TPrestepData prestep, ref TAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC, ref BodyVelocityWide wsvD); } /// diff --git a/BepuPhysics/Constraints/OneBodyAngularMotor.cs b/BepuPhysics/Constraints/OneBodyAngularMotor.cs index ab86f60e5..ab112a565 100644 --- a/BepuPhysics/Constraints/OneBodyAngularMotor.cs +++ b/BepuPhysics/Constraints/OneBodyAngularMotor.cs @@ -109,6 +109,16 @@ public void Solve(ref BodyVelocityWide velocityA, ref OneBodyAngularServoProject Solve(ref velocityA, projection.VelocityToImpulse, projection.SoftnessImpulseScale, projection.BiasImpulse, projection.MaximumImpulse, projection.ImpulseToVelocity, ref accumulatedImpulse); } + + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in OneBodyAngularMotorPrestepData prestep, in Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) + { + throw new NotImplementedException(); + } + + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, in OneBodyAngularMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) + { + throw new NotImplementedException(); + } } public class OneBodyAngularMotorTypeProcessor : OneBodyTypeProcessor diff --git a/BepuPhysics/Constraints/OneBodyAngularServo.cs b/BepuPhysics/Constraints/OneBodyAngularServo.cs index 9694a7827..ee8b191a4 100644 --- a/BepuPhysics/Constraints/OneBodyAngularServo.cs +++ b/BepuPhysics/Constraints/OneBodyAngularServo.cs @@ -133,6 +133,16 @@ public void Solve(ref BodyVelocityWide velocityA, ref OneBodyAngularServoProject Solve(ref velocityA, projection.VelocityToImpulse, projection.SoftnessImpulseScale, projection.BiasImpulse, projection.MaximumImpulse, projection.ImpulseToVelocity, ref accumulatedImpulse); } + + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in OneBodyAngularServoPrestepData prestep, in Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) + { + throw new NotImplementedException(); + } + + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, in OneBodyAngularServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) + { + throw new NotImplementedException(); + } } public class OneBodyAngularServoTypeProcessor : OneBodyTypeProcessor diff --git a/BepuPhysics/Constraints/OneBodyLinearMotor.cs b/BepuPhysics/Constraints/OneBodyLinearMotor.cs index 15678996a..98793742c 100644 --- a/BepuPhysics/Constraints/OneBodyLinearMotor.cs +++ b/BepuPhysics/Constraints/OneBodyLinearMotor.cs @@ -92,6 +92,15 @@ public void Solve(ref BodyVelocityWide velocityA, ref OneBodyLinearServoProjecti OneBodyLinearServoFunctions.SharedSolve(ref velocityA, projection, ref accumulatedImpulse); } + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in OneBodyLinearMotorPrestepData prestep, in Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) + { + throw new NotImplementedException(); + } + + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, in OneBodyLinearMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) + { + throw new NotImplementedException(); + } } public class OneBodyLinearMotorTypeProcessor : OneBodyTypeProcessor diff --git a/BepuPhysics/Constraints/OneBodyLinearServo.cs b/BepuPhysics/Constraints/OneBodyLinearServo.cs index 106958395..167a41779 100644 --- a/BepuPhysics/Constraints/OneBodyLinearServo.cs +++ b/BepuPhysics/Constraints/OneBodyLinearServo.cs @@ -168,6 +168,15 @@ public void Solve(ref BodyVelocityWide velocityA, ref OneBodyLinearServoProjecti SharedSolve(ref velocityA, projection, ref accumulatedImpulse); } + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in OneBodyLinearServoPrestepData prestep, in Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) + { + throw new NotImplementedException(); + } + + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, in OneBodyLinearServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) + { + throw new NotImplementedException(); + } } public class OneBodyLinearServoTypeProcessor : OneBodyTypeProcessor diff --git a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs index 30da0859c..512a8cc33 100644 --- a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs @@ -19,6 +19,11 @@ public interface IOneBodyConstraintFunctions diff --git a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs index be37abd65..89522934f 100644 --- a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs @@ -30,6 +30,17 @@ void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vec float dt, float inverseDt, ref TPrestepData prestepData, out TProjection projection); void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref BodyVelocityWide velocityC, ref TProjection projection, ref TAccumulatedImpulse accumulatedImpulse); void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref BodyVelocityWide velocityC, ref TProjection projection, ref TAccumulatedImpulse accumulatedImpulse); + + void WarmStart2( + in QuaternionWide orientationA, in BodyInertiaWide inertiaA, + in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, + in Vector3Wide ac, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, + in TPrestepData prestep, in TAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC); + void Solve2( + in QuaternionWide orientationA, in BodyInertiaWide inertiaA, + in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, + in Vector3Wide ac, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, float dt, float inverseDt, + in TPrestepData prestep, ref TAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC); } /// @@ -301,7 +312,7 @@ public unsafe override void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodi } } - public override void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks poseIntegratorCallbacks, + public override void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks poseIntegratorCallbacks, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { throw new NotImplementedException(); diff --git a/BepuPhysics/Constraints/VolumeConstraint.cs b/BepuPhysics/Constraints/VolumeConstraint.cs index 124217cdc..fcdf83c32 100644 --- a/BepuPhysics/Constraints/VolumeConstraint.cs +++ b/BepuPhysics/Constraints/VolumeConstraint.cs @@ -206,6 +206,15 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ApplyImpulse(ref velocityA, ref velocityB, ref velocityC, ref velocityD, ref projection, ref negatedJacobianA, ref csi); } + public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide ac, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in Vector3Wide ad, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, in VolumeConstraintPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC, ref BodyVelocityWide wsvD) + { + throw new NotImplementedException(); + } + + public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide ac, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in Vector3Wide ad, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, float dt, float inverseDt, in VolumeConstraintPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC, ref BodyVelocityWide wsvD) + { + throw new NotImplementedException(); + } } diff --git a/Demos/Demos/Characters/CharacterMotionConstraint.cs b/Demos/Demos/Characters/CharacterMotionConstraint.cs index 361055488..4e93f1ccb 100644 --- a/Demos/Demos/Characters/CharacterMotionConstraint.cs +++ b/Demos/Demos/Characters/CharacterMotionConstraint.cs @@ -306,12 +306,12 @@ public void Solve(ref BodyVelocityWide velocityA, ref StaticCharacterMotionProje ApplyVerticalImpulse(basis, verticalAngularJacobianA, verticalCorrectiveImpulse, projection.InertiaA, ref velocityA); } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in DynamicCharacterMotionPrestep prestep, in CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in StaticCharacterMotionPrestep prestep, in CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA) { throw new NotImplementedException(); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, in DynamicCharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, in StaticCharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA) { throw new NotImplementedException(); } diff --git a/Demos/Demos/Characters/CharacterMotionConstraint.tt b/Demos/Demos/Characters/CharacterMotionConstraint.tt index 9a4353705..fc94a8951 100644 --- a/Demos/Demos/Characters/CharacterMotionConstraint.tt +++ b/Demos/Demos/Characters/CharacterMotionConstraint.tt @@ -393,12 +393,12 @@ namespace Demos.Demos.Characters ApplyVerticalImpulse(basis, verticalAngularJacobianA, <#if (dynamic) {#>verticalAngularJacobianB, <#}#>verticalCorrectiveImpulse, projection.InertiaA, <#if (dynamic) {#>projection.InertiaB, <#}#>ref velocityA<#if (dynamic) {#>, ref velocityB<#}#>); } - public void WarmStart2(<#if(!dynamic) {#>in Vector3Wide positionA, <#}#>in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(dynamic) {#>in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>in DynamicCharacterMotionPrestep prestep, in CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(<#if(!dynamic) {#>in Vector3Wide positionA, <#}#>in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(dynamic) {#>in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>in <#=prefix#>CharacterMotionPrestep prestep, in CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA<#if(dynamic) {#>, ref BodyVelocityWide wsvB<#}#>) { throw new NotImplementedException(); } - public void Solve2(<#if(!dynamic) {#>in Vector3Wide positionA, <#}#>in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(dynamic) {#>in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>float dt, float inverseDt, in DynamicCharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(<#if(!dynamic) {#>in Vector3Wide positionA, <#}#>in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(dynamic) {#>in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>float dt, float inverseDt, in <#=prefix#>CharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA<#if(dynamic) {#>, ref BodyVelocityWide wsvB<#}#>) { throw new NotImplementedException(); } From 7fee632c91fe66063e76ebc87cf2b2dca2af45fa Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 21 Jul 2021 19:35:20 -0500 Subject: [PATCH 129/947] One body type processor definitions and access controls. --- .../Constraints/OneBodyAngularMotor.cs | 2 +- .../Constraints/OneBodyAngularServo.cs | 2 +- BepuPhysics/Constraints/OneBodyLinearMotor.cs | 2 +- BepuPhysics/Constraints/OneBodyLinearServo.cs | 2 +- .../Constraints/OneBodyTypeProcessor.cs | 74 ++++++++++++------- .../Constraints/TwoBodyTypeProcessor.cs | 47 ++---------- .../Characters/CharacterMotionConstraint.cs | 2 +- .../Characters/CharacterMotionConstraint.tt | 2 +- 8 files changed, 59 insertions(+), 74 deletions(-) diff --git a/BepuPhysics/Constraints/OneBodyAngularMotor.cs b/BepuPhysics/Constraints/OneBodyAngularMotor.cs index ab112a565..0c749f5f8 100644 --- a/BepuPhysics/Constraints/OneBodyAngularMotor.cs +++ b/BepuPhysics/Constraints/OneBodyAngularMotor.cs @@ -121,7 +121,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in } } - public class OneBodyAngularMotorTypeProcessor : OneBodyTypeProcessor + public class OneBodyAngularMotorTypeProcessor : OneBodyTypeProcessor { public const int BatchTypeId = 43; } diff --git a/BepuPhysics/Constraints/OneBodyAngularServo.cs b/BepuPhysics/Constraints/OneBodyAngularServo.cs index ee8b191a4..38d4ef046 100644 --- a/BepuPhysics/Constraints/OneBodyAngularServo.cs +++ b/BepuPhysics/Constraints/OneBodyAngularServo.cs @@ -145,7 +145,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in } } - public class OneBodyAngularServoTypeProcessor : OneBodyTypeProcessor + public class OneBodyAngularServoTypeProcessor : OneBodyTypeProcessor { public const int BatchTypeId = 42; } diff --git a/BepuPhysics/Constraints/OneBodyLinearMotor.cs b/BepuPhysics/Constraints/OneBodyLinearMotor.cs index 98793742c..5d33d26cb 100644 --- a/BepuPhysics/Constraints/OneBodyLinearMotor.cs +++ b/BepuPhysics/Constraints/OneBodyLinearMotor.cs @@ -103,7 +103,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in } } - public class OneBodyLinearMotorTypeProcessor : OneBodyTypeProcessor + public class OneBodyLinearMotorTypeProcessor : OneBodyTypeProcessor { public const int BatchTypeId = 45; } diff --git a/BepuPhysics/Constraints/OneBodyLinearServo.cs b/BepuPhysics/Constraints/OneBodyLinearServo.cs index 167a41779..39f250be0 100644 --- a/BepuPhysics/Constraints/OneBodyLinearServo.cs +++ b/BepuPhysics/Constraints/OneBodyLinearServo.cs @@ -179,7 +179,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in } } - public class OneBodyLinearServoTypeProcessor : OneBodyTypeProcessor + public class OneBodyLinearServoTypeProcessor : OneBodyTypeProcessor { public const int BatchTypeId = 44; } diff --git a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs index 512a8cc33..751e74226 100644 --- a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs @@ -42,10 +42,13 @@ public interface IOneBodyContactConstraintFunctions /// Shared implementation across all one body constraints. /// - public abstract class OneBodyTypeProcessor + public abstract class OneBodyTypeProcessor : TypeProcessor, TPrestepData, TProjection, TAccumulatedImpulse> where TPrestepData : unmanaged where TProjection : unmanaged where TAccumulatedImpulse : unmanaged where TConstraintFunctions : unmanaged, IOneBodyConstraintFunctions + where TWarmStartAccessFilterA : unmanaged, IBodyAccessFilter + where TSolveAccessFilterA : unmanaged, IBodyAccessFilter { protected sealed override int InternalBodiesPerConstraint => 1; @@ -254,44 +257,61 @@ public unsafe override void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodi } - - public unsafe override void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks poseIntegratorCallbacks, - float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) + public unsafe override void WarmStart2( + ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { - ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); - ref var bodyReferencesBase = ref Unsafe.AsRef>(typeBatch.BodyReferences.Memory); - ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); + var prestepBundles = typeBatch.PrestepData.As(); + var bodyReferencesBundles = typeBatch.BodyReferences.As(); + var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); var function = default(TConstraintFunctions); + ref var states = ref bodies.ActiveSet.SolverStates; + EarlyPrefetch(WarmStartPrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref states, startBundle, exclusiveEndBundle); for (int i = startBundle; i < exclusiveEndBundle; ++i) { - ref var prestep = ref Unsafe.Add(ref prestepBase, i); - ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); - ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); - ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); + ref var prestep = ref prestepBundles[i]; + ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; + ref var references = ref bodyReferencesBundles[i]; var count = GetCountInBundle(ref typeBatch, i); - bodies.GatherState(ref bodyReferences, count, out var position, out var orientation, out var wsvA, out var inertiaA); - function.Prestep(position, orientation, inertiaA, dt, inverseDt, ref prestep, out var projection); - function.WarmStart(ref wsvA, ref projection, ref accumulatedImpulses); - bodies.ScatterVelocities(ref wsvA, ref bodyReferences, count); + Prefetch(WarmStartPrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref states, i, exclusiveEndBundle); + GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 0, dt, workerIndex, i, ref references.IndexA, count, + out var positionA, out var orientationA, out var wsvA, out var inertiaA); + + function.WarmStart2(positionA, orientationA, inertiaA, prestep, accumulatedImpulses, ref wsvA); + + if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) + { + bodies.ScatterVelocities(ref wsvA, ref references.IndexA, count); + } + else + { + //This batch has some integrators, which means that every bundle is going to gather all velocities. + //(We don't make per-bundle determinations about this to avoid an extra branch and instruction complexity, and the difference is very small.) + bodies.ScatterVelocities(ref wsvA, ref references.IndexA, count); + } + } } + public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { - ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); - ref var bodyReferencesBase = ref Unsafe.AsRef>(typeBatch.BodyReferences.Memory); - ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); + var prestepBundles = typeBatch.PrestepData.As(); + var bodyReferencesBundles = typeBatch.BodyReferences.As(); + var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); var function = default(TConstraintFunctions); + ref var motionStates = ref bodies.ActiveSet.SolverStates; + EarlyPrefetch(SolvePrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, startBundle, exclusiveEndBundle); for (int i = startBundle; i < exclusiveEndBundle; ++i) { - ref var prestep = ref Unsafe.Add(ref prestepBase, i); - ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); - ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); - ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); + ref var prestep = ref prestepBundles[i]; + ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; + ref var references = ref bodyReferencesBundles[i]; var count = GetCountInBundle(ref typeBatch, i); - bodies.GatherState(ref bodyReferences, count, out var position, out var orientation, out var wsvA, out var inertiaA); - function.Prestep(position, orientation, inertiaA, dt, inverseDt, ref prestep, out var projection); - function.Solve(ref wsvA, ref projection, ref accumulatedImpulses); - bodies.ScatterVelocities(ref wsvA, ref bodyReferences, count); + Prefetch(SolvePrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, i, exclusiveEndBundle); + bodies.GatherState(ref references.IndexA, count, true, out var positionA, out var orientationA, out var wsvA, out var inertiaA); + + function.Solve2(positionA, orientationA, inertiaA, dt, inverseDt, prestep, ref accumulatedImpulses, ref wsvA); + + bodies.ScatterVelocities(ref wsvA, ref references.IndexA, count); } } @@ -323,7 +343,7 @@ public override void JacobiPrestep2(ref TypeBatch typeBatch, Bodies bodies, ref } public abstract class OneBodyContactTypeProcessor - : OneBodyTypeProcessor + : OneBodyTypeProcessor where TPrestepData : unmanaged where TProjection : unmanaged where TAccumulatedImpulse : unmanaged where TConstraintFunctions : unmanaged, IOneBodyContactConstraintFunctions { diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 42f55e777..38e650ce2 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -335,26 +335,9 @@ public unsafe override void WarmStart2(bodies, ref integratorCallbacks, ref integrationFlags, 1, dt, workerIndex, i, ref references.IndexB, count, out var positionB, out var orientationB, out var wsvB, out var inertiaB); var ab = positionB - positionA; - if (typeof(TConstraintFunctions) == typeof(WeldFunctions)) - { - default(WeldFunctions).WarmStart2(orientationA, inertiaA, ab, orientationB, inertiaB, - Unsafe.As(ref prestep), Unsafe.As(ref accumulatedImpulses), ref wsvA, ref wsvB); - } - else if (typeof(TConstraintFunctions) == typeof(BallSocketFunctions)) - { - default(BallSocketFunctions).WarmStart2(orientationA, inertiaA, ab, orientationB, inertiaB, - Unsafe.As(ref prestep), Unsafe.As(ref accumulatedImpulses), ref wsvA, ref wsvB); - } - else if (typeof(TConstraintFunctions) == typeof(CenterDistanceConstraint)) - { - default(CenterDistanceConstraintFunctions).WarmStart2(orientationA, inertiaA, ab, orientationB, inertiaB, - Unsafe.As(ref prestep), Unsafe.As>(ref accumulatedImpulses), ref wsvA, ref wsvB); - } - else - { - function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, dt, inverseDt, ref prestep, out var projection); - function.WarmStart(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); - } + + function.WarmStart2(orientationA, inertiaA, ab, orientationB, inertiaB, prestep, accumulatedImpulses, ref wsvA, ref wsvB); + if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) { bodies.ScatterVelocities(ref wsvA, ref references.IndexA, count); @@ -371,7 +354,6 @@ public unsafe override void WarmStart2(); @@ -390,26 +372,9 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f bodies.GatherState(ref references.IndexA, count, true, out var positionA, out var orientationA, out var wsvA, out var inertiaA); bodies.GatherState(ref references.IndexB, count, true, out var positionB, out var orientationB, out var wsvB, out var inertiaB); var ab = positionB - positionA; - if (typeof(TConstraintFunctions) == typeof(WeldFunctions)) - { - default(WeldFunctions).Solve2(orientationA, inertiaA, ab, orientationB, inertiaB, dt, inverseDt, - Unsafe.As(ref prestep), ref Unsafe.As(ref accumulatedImpulses), ref wsvA, ref wsvB); - } - else if (typeof(TConstraintFunctions) == typeof(BallSocketFunctions)) - { - default(BallSocketFunctions).Solve2(orientationA, inertiaA, ab, orientationB, inertiaB, dt, inverseDt, - Unsafe.As(ref prestep), ref Unsafe.As(ref accumulatedImpulses), ref wsvA, ref wsvB); - } - else if (typeof(TConstraintFunctions) == typeof(CenterDistanceConstraint)) - { - default(CenterDistanceConstraintFunctions).Solve2(orientationA, inertiaA, ab, orientationB, inertiaB, dt, inverseDt, - Unsafe.As(ref prestep), ref Unsafe.As>(ref accumulatedImpulses), ref wsvA, ref wsvB); - } - else - { - function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, dt, inverseDt, ref prestep, out var projection); - function.Solve(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); - } + + function.Solve2(orientationA, inertiaA, ab, orientationB, inertiaB, dt, inverseDt, prestep, ref accumulatedImpulses, ref wsvA, ref wsvB); + bodies.ScatterVelocities(ref wsvA, ref references.IndexA, count); bodies.ScatterVelocities(ref wsvB, ref references.IndexB, count); } diff --git a/Demos/Demos/Characters/CharacterMotionConstraint.cs b/Demos/Demos/Characters/CharacterMotionConstraint.cs index 4e93f1ccb..66d85d5d0 100644 --- a/Demos/Demos/Characters/CharacterMotionConstraint.cs +++ b/Demos/Demos/Characters/CharacterMotionConstraint.cs @@ -319,7 +319,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in //Each constraint type has its own 'type processor'- it acts as the outer loop that handles all the common logic across batches of constraints and invokes //the per-constraint logic as needed. The CharacterMotionFunctions type provides the actual implementation. - public class StaticCharacterMotionTypeProcessor : OneBodyTypeProcessor + public class StaticCharacterMotionTypeProcessor : OneBodyTypeProcessor { /// /// Simulation-wide unique id for the character motion constraint. Every type has needs a unique compile time id; this is a little bit annoying to guarantee given that there is no central diff --git a/Demos/Demos/Characters/CharacterMotionConstraint.tt b/Demos/Demos/Characters/CharacterMotionConstraint.tt index fc94a8951..d2876c9ee 100644 --- a/Demos/Demos/Characters/CharacterMotionConstraint.tt +++ b/Demos/Demos/Characters/CharacterMotionConstraint.tt @@ -406,7 +406,7 @@ namespace Demos.Demos.Characters //Each constraint type has its own 'type processor'- it acts as the outer loop that handles all the common logic across batches of constraints and invokes //the per-constraint logic as needed. The CharacterMotionFunctions type provides the actual implementation. - public class <#=prefix#>CharacterMotionTypeProcessor : <#=dynamic ? "Two" : "One"#>BodyTypeProcessor<<#=prefix#>CharacterMotionPrestep, <#=prefix#>CharacterMotionProjection, CharacterMotionAccumulatedImpulse, <#=prefix#>CharacterMotionFunctions<#=dynamic ? (", AccessAll, AccessAll, AccessAll, AccessAll") : ""#>> + public class <#=prefix#>CharacterMotionTypeProcessor : <#=dynamic ? "Two" : "One"#>BodyTypeProcessor<<#=prefix#>CharacterMotionPrestep, <#=prefix#>CharacterMotionProjection, CharacterMotionAccumulatedImpulse, <#=prefix#>CharacterMotionFunctions, AccessAll, AccessAll<#=dynamic ? (", AccessAll, AccessAll") : ""#>> { /// /// Simulation-wide unique id for the character motion constraint. Every type has needs a unique compile time id; this is a little bit annoying to guarantee given that there is no central From 8e80a06d6949b514364c592fb54b3b8df0d9d857 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 21 Jul 2021 20:00:44 -0500 Subject: [PATCH 130/947] One, three, and four body constraint backbones. --- BepuPhysics/Constraints/AreaConstraint.cs | 4 +- .../Constraints/FourBodyTypeProcessor.cs | 94 +++++++++++++++++-- .../Constraints/OneBodyTypeProcessor.cs | 4 - .../Constraints/ThreeBodyTypeProcessor.cs | 82 ++++++++++++++-- .../Constraints/TwoBodyTypeProcessor.cs | 60 +++++++++++- BepuPhysics/Constraints/TypeProcessor.cs | 52 ---------- BepuPhysics/Constraints/VolumeConstraint.cs | 2 +- 7 files changed, 218 insertions(+), 80 deletions(-) diff --git a/BepuPhysics/Constraints/AreaConstraint.cs b/BepuPhysics/Constraints/AreaConstraint.cs index 905819f3c..fe817ef43 100644 --- a/BepuPhysics/Constraints/AreaConstraint.cs +++ b/BepuPhysics/Constraints/AreaConstraint.cs @@ -198,9 +198,9 @@ public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, /// - /// Handles the solve iterations of a bunch of ball socket constraints. + /// Handles the solve iterations of a bunch of area constraints. /// - public class AreaConstraintTypeProcessor : ThreeBodyTypeProcessor, AreaConstraintFunctions> + public class AreaConstraintTypeProcessor : ThreeBodyTypeProcessor, AreaConstraintFunctions, AccessOnlyLinear, AccessOnlyLinear, AccessOnlyLinear, AccessOnlyLinear, AccessOnlyLinear, AccessOnlyLinear> { public const int BatchTypeId = 36; } diff --git a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs index eb92feff8..2f6b2fb79 100644 --- a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs @@ -28,7 +28,7 @@ public struct FourBodyReferences public interface IFourBodyConstraintFunctions { void Prestep( - in QuaternionWide orientationA, in BodyInertiaWide inertiaA, + in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide ac, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in Vector3Wide ad, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, @@ -52,10 +52,19 @@ void Solve2( /// /// Shared implementation across all four body constraints. /// - public abstract class FourBodyTypeProcessor + public abstract class FourBodyTypeProcessor : TypeProcessor where TPrestepData : unmanaged where TProjection : unmanaged where TAccumulatedImpulse : unmanaged where TConstraintFunctions : unmanaged, IFourBodyConstraintFunctions + where TWarmStartAccessFilterA : unmanaged, IBodyAccessFilter + where TWarmStartAccessFilterB : unmanaged, IBodyAccessFilter + where TWarmStartAccessFilterC : unmanaged, IBodyAccessFilter + where TWarmStartAccessFilterD : unmanaged, IBodyAccessFilter + where TSolveAccessFilterA : unmanaged, IBodyAccessFilter + where TSolveAccessFilterB : unmanaged, IBodyAccessFilter + where TSolveAccessFilterC : unmanaged, IBodyAccessFilter + where TSolveAccessFilterD : unmanaged, IBodyAccessFilter { protected sealed override int InternalBodiesPerConstraint => 4; @@ -146,7 +155,7 @@ public unsafe override void WarmStart(ref TypeBatch typeBatch, Bodies bodies, in out var ab, out var orientationB, out var wsvB, out var inertiaB, out var ac, out var orientationC, out var wsvC, out var inertiaC, out var ad, out var orientationD, out var wsvD, out var inertiaD); - function.WarmStart(ref wsvA, ref wsvB, ref wsvC, ref wsvD, ref projection, ref accumulatedImpulses); + function.WarmStart(ref wsvA, ref wsvB, ref wsvC, ref wsvD, ref projection, ref accumulatedImpulses); bodies.ScatterVelocities(ref wsvA, ref bodyReferences.IndexA, count); bodies.ScatterVelocities(ref wsvB, ref bodyReferences.IndexB, count); bodies.ScatterVelocities(ref wsvC, ref bodyReferences.IndexC, count); @@ -340,16 +349,83 @@ public unsafe override void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodi } - public override void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks poseIntegratorCallbacks, - float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) + public unsafe override void WarmStart2( + ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { - throw new NotImplementedException(); + var prestepBundles = typeBatch.PrestepData.As(); + var bodyReferencesBundles = typeBatch.BodyReferences.As(); + var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); + var function = default(TConstraintFunctions); + ref var states = ref bodies.ActiveSet.SolverStates; + for (int i = startBundle; i < exclusiveEndBundle; ++i) + { + ref var prestep = ref prestepBundles[i]; + ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; + ref var references = ref bodyReferencesBundles[i]; + var count = GetCountInBundle(ref typeBatch, i); + GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 0, dt, workerIndex, i, ref references.IndexA, count, + out var positionA, out var orientationA, out var wsvA, out var inertiaA); + GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 1, dt, workerIndex, i, ref references.IndexB, count, + out var positionB, out var orientationB, out var wsvB, out var inertiaB); + GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 2, dt, workerIndex, i, ref references.IndexC, count, + out var positionC, out var orientationC, out var wsvC, out var inertiaC); + GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 3, dt, workerIndex, i, ref references.IndexD, count, + out var positionD, out var orientationD, out var wsvD, out var inertiaD); + var ab = positionB - positionA; + var ac = positionC - positionA; + var ad = positionD - positionA; + + function.WarmStart2(orientationA, inertiaA, ab, orientationB, inertiaB, ac, orientationC, inertiaC, ad, orientationD, inertiaD, prestep, accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC, ref wsvD); + + if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) + { + bodies.ScatterVelocities(ref wsvA, ref references.IndexA, count); + bodies.ScatterVelocities(ref wsvB, ref references.IndexB, count); + bodies.ScatterVelocities(ref wsvC, ref references.IndexC, count); + bodies.ScatterVelocities(ref wsvD, ref references.IndexD, count); + } + else + { + //This batch has some integrators, which means that every bundle is going to gather all velocities. + //(We don't make per-bundle determinations about this to avoid an extra branch and instruction complexity, and the difference is very small.) + bodies.ScatterVelocities(ref wsvA, ref references.IndexA, count); + bodies.ScatterVelocities(ref wsvB, ref references.IndexB, count); + bodies.ScatterVelocities(ref wsvC, ref references.IndexC, count); + bodies.ScatterVelocities(ref wsvD, ref references.IndexD, count); + } + + } } - public override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + + public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { - throw new NotImplementedException(); - } + var prestepBundles = typeBatch.PrestepData.As(); + var bodyReferencesBundles = typeBatch.BodyReferences.As(); + var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); + var function = default(TConstraintFunctions); + ref var motionStates = ref bodies.ActiveSet.SolverStates; + for (int i = startBundle; i < exclusiveEndBundle; ++i) + { + ref var prestep = ref prestepBundles[i]; + ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; + ref var references = ref bodyReferencesBundles[i]; + var count = GetCountInBundle(ref typeBatch, i); + bodies.GatherState(ref references.IndexA, count, true, out var positionA, out var orientationA, out var wsvA, out var inertiaA); + bodies.GatherState(ref references.IndexB, count, true, out var positionB, out var orientationB, out var wsvB, out var inertiaB); + bodies.GatherState(ref references.IndexC, count, true, out var positionC, out var orientationC, out var wsvC, out var inertiaC); + bodies.GatherState(ref references.IndexD, count, true, out var positionD, out var orientationD, out var wsvD, out var inertiaD); + var ab = positionB - positionA; + var ac = positionC - positionA; + var ad = positionD - positionA; + + function.Solve2(orientationA, inertiaA, ab, orientationB, inertiaB, ac, orientationC, inertiaC, ad, orientationD, inertiaD, dt, inverseDt, prestep, ref accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC, ref wsvD); + bodies.ScatterVelocities(ref wsvA, ref references.IndexA, count); + bodies.ScatterVelocities(ref wsvB, ref references.IndexB, count); + bodies.ScatterVelocities(ref wsvC, ref references.IndexC, count); + bodies.ScatterVelocities(ref wsvD, ref references.IndexD, count); + } + } public override void Prestep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { diff --git a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs index 751e74226..c12dd017c 100644 --- a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs @@ -265,14 +265,12 @@ public unsafe override void WarmStart2(); var function = default(TConstraintFunctions); ref var states = ref bodies.ActiveSet.SolverStates; - EarlyPrefetch(WarmStartPrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref states, startBundle, exclusiveEndBundle); for (int i = startBundle; i < exclusiveEndBundle; ++i) { ref var prestep = ref prestepBundles[i]; ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; ref var references = ref bodyReferencesBundles[i]; var count = GetCountInBundle(ref typeBatch, i); - Prefetch(WarmStartPrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref states, i, exclusiveEndBundle); GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 0, dt, workerIndex, i, ref references.IndexA, count, out var positionA, out var orientationA, out var wsvA, out var inertiaA); @@ -299,14 +297,12 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); var function = default(TConstraintFunctions); ref var motionStates = ref bodies.ActiveSet.SolverStates; - EarlyPrefetch(SolvePrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, startBundle, exclusiveEndBundle); for (int i = startBundle; i < exclusiveEndBundle; ++i) { ref var prestep = ref prestepBundles[i]; ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; ref var references = ref bodyReferencesBundles[i]; var count = GetCountInBundle(ref typeBatch, i); - Prefetch(SolvePrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, i, exclusiveEndBundle); bodies.GatherState(ref references.IndexA, count, true, out var positionA, out var orientationA, out var wsvA, out var inertiaA); function.Solve2(positionA, orientationA, inertiaA, dt, inverseDt, prestep, ref accumulatedImpulses, ref wsvA); diff --git a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs index 89522934f..5f3c45a62 100644 --- a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs @@ -37,7 +37,7 @@ void WarmStart2( in Vector3Wide ac, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in TPrestepData prestep, in TAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC); void Solve2( - in QuaternionWide orientationA, in BodyInertiaWide inertiaA, + in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide ac, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, float dt, float inverseDt, in TPrestepData prestep, ref TAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC); @@ -46,10 +46,17 @@ void Solve2( /// /// Shared implementation across all three body constraints. /// - public abstract class ThreeBodyTypeProcessor + public abstract class ThreeBodyTypeProcessor : TypeProcessor where TPrestepData : unmanaged where TProjection : unmanaged where TAccumulatedImpulse : unmanaged where TConstraintFunctions : unmanaged, IThreeBodyConstraintFunctions + where TWarmStartAccessFilterA : unmanaged, IBodyAccessFilter + where TWarmStartAccessFilterB : unmanaged, IBodyAccessFilter + where TWarmStartAccessFilterC : unmanaged, IBodyAccessFilter + where TSolveAccessFilterA : unmanaged, IBodyAccessFilter + where TSolveAccessFilterB : unmanaged, IBodyAccessFilter + where TSolveAccessFilterC : unmanaged, IBodyAccessFilter { protected sealed override int InternalBodiesPerConstraint => 3; @@ -312,14 +319,75 @@ public unsafe override void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodi } } - public override void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks poseIntegratorCallbacks, - float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) + + public unsafe override void WarmStart2( + ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { - throw new NotImplementedException(); + var prestepBundles = typeBatch.PrestepData.As(); + var bodyReferencesBundles = typeBatch.BodyReferences.As(); + var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); + var function = default(TConstraintFunctions); + ref var states = ref bodies.ActiveSet.SolverStates; + for (int i = startBundle; i < exclusiveEndBundle; ++i) + { + ref var prestep = ref prestepBundles[i]; + ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; + ref var references = ref bodyReferencesBundles[i]; + var count = GetCountInBundle(ref typeBatch, i); + GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 0, dt, workerIndex, i, ref references.IndexA, count, + out var positionA, out var orientationA, out var wsvA, out var inertiaA); + GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 1, dt, workerIndex, i, ref references.IndexB, count, + out var positionB, out var orientationB, out var wsvB, out var inertiaB); + GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 2, dt, workerIndex, i, ref references.IndexC, count, + out var positionC, out var orientationC, out var wsvC, out var inertiaC); + var ab = positionB - positionA; + var ac = positionC - positionA; + + function.WarmStart2(orientationA, inertiaA, ab, orientationB, inertiaB, ac, orientationC, inertiaC, prestep, accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC); + + if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) + { + bodies.ScatterVelocities(ref wsvA, ref references.IndexA, count); + bodies.ScatterVelocities(ref wsvB, ref references.IndexB, count); + bodies.ScatterVelocities(ref wsvC, ref references.IndexC, count); + } + else + { + //This batch has some integrators, which means that every bundle is going to gather all velocities. + //(We don't make per-bundle determinations about this to avoid an extra branch and instruction complexity, and the difference is very small.) + bodies.ScatterVelocities(ref wsvA, ref references.IndexA, count); + bodies.ScatterVelocities(ref wsvB, ref references.IndexB, count); + bodies.ScatterVelocities(ref wsvC, ref references.IndexC, count); + } + + } } - public override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + + public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { - throw new NotImplementedException(); + var prestepBundles = typeBatch.PrestepData.As(); + var bodyReferencesBundles = typeBatch.BodyReferences.As(); + var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); + var function = default(TConstraintFunctions); + ref var motionStates = ref bodies.ActiveSet.SolverStates; + for (int i = startBundle; i < exclusiveEndBundle; ++i) + { + ref var prestep = ref prestepBundles[i]; + ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; + ref var references = ref bodyReferencesBundles[i]; + var count = GetCountInBundle(ref typeBatch, i); + bodies.GatherState(ref references.IndexA, count, true, out var positionA, out var orientationA, out var wsvA, out var inertiaA); + bodies.GatherState(ref references.IndexB, count, true, out var positionB, out var orientationB, out var wsvB, out var inertiaB); + bodies.GatherState(ref references.IndexC, count, true, out var positionC, out var orientationC, out var wsvC, out var inertiaC); + var ab = positionB - positionA; + var ac = positionC - positionA; + + function.Solve2(orientationA, inertiaA, ab, orientationB, inertiaB, ac, orientationC, inertiaC, dt, inverseDt, prestep, ref accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC); + + bodies.ScatterVelocities(ref wsvA, ref references.IndexA, count); + bodies.ScatterVelocities(ref wsvB, ref references.IndexB, count); + bodies.ScatterVelocities(ref wsvC, ref references.IndexC, count); + } } public override void Prestep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 38e650ce2..16c86ce44 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -312,7 +312,57 @@ public unsafe override void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodi } } - + //public const int WarmStartPrefetchDistance = 8; + //public const int SolvePrefetchDistance = 4; + //[MethodImpl(MethodImplOptions.AggressiveInlining)] + //static unsafe void Prefetch(void* address) + //{ + // if (Sse.IsSupported) + // { + // Sse.Prefetch0(address); + // //Sse.Prefetch0((byte*)address + 64); + // //TODO: prefetch should grab cache line pair anyway, right? not much reason to explicitly do more? + // } + // //TODO: ARM? + //} + //[MethodImpl(MethodImplOptions.AggressiveInlining)] + //static unsafe void PrefetchBundle(SolverState* stateBase, ref TwoBodyReferences references, int countInBundle) + //{ + // var indicesA = (int*)Unsafe.AsPointer(ref references.IndexA); + // var indicesB = (int*)Unsafe.AsPointer(ref references.IndexB); + // for (int i = 0; i < countInBundle; ++i) + // { + // var indexA = indicesA[i]; + // var indexB = indicesA[i]; + // Prefetch(stateBase + indexA); + // Prefetch(stateBase + indexB); + // } + //} + + //[MethodImpl(MethodImplOptions.AggressiveInlining)] + //[Conditional("PREFETCH")] + //public unsafe static void EarlyPrefetch(int prefetchDistance, ref TypeBatch typeBatch, ref Buffer references, ref Buffer states, int startBundleIndex, int exclusiveEndBundleIndex) + //{ + // exclusiveEndBundleIndex = Math.Min(exclusiveEndBundleIndex, startBundleIndex + prefetchDistance); + // var lastBundleIndex = exclusiveEndBundleIndex - 1; + // for (int i = startBundleIndex; i < lastBundleIndex; ++i) + // { + // PrefetchBundle(states.Memory, ref references[i], Vector.Count); + // } + // var countInBundle = GetCountInBundle(ref typeBatch, lastBundleIndex); + // PrefetchBundle(states.Memory, ref references[lastBundleIndex], countInBundle); + //} + + //[MethodImpl(MethodImplOptions.AggressiveInlining)] + //[Conditional("PREFETCH")] + //public unsafe static void Prefetch(int prefetchDistance, ref TypeBatch typeBatch, ref Buffer references, ref Buffer states, int bundleIndex, int exclusiveEndBundleIndex) + //{ + // var targetIndex = bundleIndex + prefetchDistance; + // if (targetIndex < exclusiveEndBundleIndex) + // { + // PrefetchBundle(states.Memory, ref references[targetIndex], GetCountInBundle(ref typeBatch, targetIndex)); + // } + //} public unsafe override void WarmStart2( ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) @@ -322,14 +372,14 @@ public unsafe override void WarmStart2(); var function = default(TConstraintFunctions); ref var states = ref bodies.ActiveSet.SolverStates; - EarlyPrefetch(WarmStartPrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref states, startBundle, exclusiveEndBundle); + //EarlyPrefetch(WarmStartPrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref states, startBundle, exclusiveEndBundle); for (int i = startBundle; i < exclusiveEndBundle; ++i) { ref var prestep = ref prestepBundles[i]; ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; ref var references = ref bodyReferencesBundles[i]; var count = GetCountInBundle(ref typeBatch, i); - Prefetch(WarmStartPrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref states, i, exclusiveEndBundle); + //Prefetch(WarmStartPrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref states, i, exclusiveEndBundle); GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 0, dt, workerIndex, i, ref references.IndexA, count, out var positionA, out var orientationA, out var wsvA, out var inertiaA); GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 1, dt, workerIndex, i, ref references.IndexB, count, @@ -361,14 +411,14 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); var function = default(TConstraintFunctions); ref var motionStates = ref bodies.ActiveSet.SolverStates; - EarlyPrefetch(SolvePrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, startBundle, exclusiveEndBundle); + //EarlyPrefetch(SolvePrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, startBundle, exclusiveEndBundle); for (int i = startBundle; i < exclusiveEndBundle; ++i) { ref var prestep = ref prestepBundles[i]; ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; ref var references = ref bodyReferencesBundles[i]; var count = GetCountInBundle(ref typeBatch, i); - Prefetch(SolvePrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, i, exclusiveEndBundle); + //Prefetch(SolvePrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, i, exclusiveEndBundle); bodies.GatherState(ref references.IndexA, count, true, out var positionA, out var orientationA, out var wsvA, out var inertiaA); bodies.GatherState(ref references.IndexB, count, true, out var positionB, out var orientationB, out var wsvB, out var inertiaB); var ab = positionB - positionA; diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index 7857a447c..c303992f8 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -696,58 +696,6 @@ internal override int GetBodyReferenceCount(ref TypeBatch typeBatch, int bodyToF } - public const int WarmStartPrefetchDistance = 8; - public const int SolvePrefetchDistance = 4; - [MethodImpl(MethodImplOptions.AggressiveInlining)] - static unsafe void Prefetch(void* address) - { - if (Sse.IsSupported) - { - Sse.Prefetch0(address); - //Sse.Prefetch0((byte*)address + 64); - //TODO: prefetch should grab cache line pair anyway, right? not much reason to explicitly do more? - } - //TODO: ARM? - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - static unsafe void PrefetchBundle(SolverState* stateBase, ref TwoBodyReferences references, int countInBundle) - { - var indicesA = (int*)Unsafe.AsPointer(ref references.IndexA); - var indicesB = (int*)Unsafe.AsPointer(ref references.IndexB); - for (int i = 0; i < countInBundle; ++i) - { - var indexA = indicesA[i]; - var indexB = indicesA[i]; - Prefetch(stateBase + indexA); - Prefetch(stateBase + indexB); - } - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - [Conditional("PREFETCH")] - public unsafe static void EarlyPrefetch(int prefetchDistance, ref TypeBatch typeBatch, ref Buffer references, ref Buffer states, int startBundleIndex, int exclusiveEndBundleIndex) - { - exclusiveEndBundleIndex = Math.Min(exclusiveEndBundleIndex, startBundleIndex + prefetchDistance); - var lastBundleIndex = exclusiveEndBundleIndex - 1; - for (int i = startBundleIndex; i < lastBundleIndex; ++i) - { - PrefetchBundle(states.Memory, ref references[i], Vector.Count); - } - var countInBundle = GetCountInBundle(ref typeBatch, lastBundleIndex); - PrefetchBundle(states.Memory, ref references[lastBundleIndex], countInBundle); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - [Conditional("PREFETCH")] - public unsafe static void Prefetch(int prefetchDistance, ref TypeBatch typeBatch, ref Buffer references, ref Buffer states, int bundleIndex, int exclusiveEndBundleIndex) - { - var targetIndex = bundleIndex + prefetchDistance; - if (targetIndex < exclusiveEndBundleIndex) - { - PrefetchBundle(states.Memory, ref references[targetIndex], GetCountInBundle(ref typeBatch, targetIndex)); - } - } - public enum BundleIntegrationMode { None = 0, diff --git a/BepuPhysics/Constraints/VolumeConstraint.cs b/BepuPhysics/Constraints/VolumeConstraint.cs index fcdf83c32..c91221962 100644 --- a/BepuPhysics/Constraints/VolumeConstraint.cs +++ b/BepuPhysics/Constraints/VolumeConstraint.cs @@ -221,7 +221,7 @@ public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, /// /// Handles the solve iterations of a bunch of volume constraints. /// - public class VolumeConstraintTypeProcessor : FourBodyTypeProcessor, VolumeConstraintFunctions> + public class VolumeConstraintTypeProcessor : FourBodyTypeProcessor, VolumeConstraintFunctions, AccessOnlyLinear, AccessOnlyLinear, AccessOnlyLinear, AccessOnlyLinear, AccessOnlyLinear, AccessOnlyLinear, AccessOnlyLinear, AccessOnlyLinear> { public const int BatchTypeId = 32; } From 92a55446634590678ae3e11e0e178b2bb4548a56 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 21 Jul 2021 20:18:03 -0500 Subject: [PATCH 131/947] Typofix in gear motor. 2-ized angularaxismotor. --- .../Constraints/AngularAxisGearMotor.cs | 2 +- BepuPhysics/Constraints/AngularAxisMotor.cs | 27 +++++++++++++++++-- 2 files changed, 26 insertions(+), 3 deletions(-) diff --git a/BepuPhysics/Constraints/AngularAxisGearMotor.cs b/BepuPhysics/Constraints/AngularAxisGearMotor.cs index 4f939455b..0815555ad 100644 --- a/BepuPhysics/Constraints/AngularAxisGearMotor.cs +++ b/BepuPhysics/Constraints/AngularAxisGearMotor.cs @@ -135,7 +135,7 @@ public static void ApplyImpulse(in Vector3Wide jA, in Vector3Wide negatedJB, in Symmetric3x3Wide.TransformWithoutOverlap(jA * csi, inertiaA, out var changeA); angularVelocityA += changeA; Symmetric3x3Wide.TransformWithoutOverlap(negatedJB * csi, inertiaB, out var negatedChangeB); - angularVelocityA -= negatedChangeB; + angularVelocityB -= negatedChangeB; } [MethodImpl(MethodImplOptions.AggressiveInlining)] diff --git a/BepuPhysics/Constraints/AngularAxisMotor.cs b/BepuPhysics/Constraints/AngularAxisMotor.cs index b032152d6..4fb2f4e19 100644 --- a/BepuPhysics/Constraints/AngularAxisMotor.cs +++ b/BepuPhysics/Constraints/AngularAxisMotor.cs @@ -125,14 +125,37 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void ApplyImpulse(in Vector3Wide jA, in Vector csi, in Symmetric3x3Wide inertiaA, in Symmetric3x3Wide inertiaB, ref Vector3Wide angularVelocityA, ref Vector3Wide angularVelocityB) + { + var wsiA = jA * csi; + Symmetric3x3Wide.TransformWithoutOverlap(wsiA, inertiaA, out var changeA); + angularVelocityA += changeA; + Symmetric3x3Wide.TransformWithoutOverlap(wsiA, inertiaB, out var negatedChangeB); + angularVelocityB -= negatedChangeB; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in AngularAxisMotorPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - throw new NotImplementedException(); + QuaternionWide.TransformWithoutOverlap(prestep.LocalAxisA, orientationA, out var axis); + ApplyImpulse(axis, accumulatedImpulses, inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, ref wsvA.Angular, ref wsvB.Angular); } + [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in AngularAxisMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - throw new NotImplementedException(); + QuaternionWide.TransformWithoutOverlap(prestep.LocalAxisA, orientationA, out var jA); + Symmetric3x3Wide.TransformWithoutOverlap(jA, inertiaA.InverseInertiaTensor, out var jIA); + Vector3Wide.Dot(jA, jIA, out var contributionA); + Symmetric3x3Wide.TransformWithoutOverlap(jA, inertiaB.InverseInertiaTensor, out var jIB); + Vector3Wide.Dot(jA, jIB, out var contributionB); + MotorSettingsWide.ComputeSoftness(prestep.Settings, dt, out var effectiveMassCFMScale, out var softnessImpulseScale, out var maximumImpulse); + + //csi = projection.BiasImpulse - accumulatedImpulse * softnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); + var csi = effectiveMassCFMScale * (Vector3Wide.Dot(wsvB.Angular, jA) - Vector3Wide.Dot(wsvA.Angular, jA)) / (contributionA + contributionB) - accumulatedImpulses * softnessImpulseScale; + ServoSettingsWide.ClampImpulse(maximumImpulse, ref accumulatedImpulses, ref csi); + ApplyImpulse(jA, accumulatedImpulses, inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, ref wsvA.Angular, ref wsvB.Angular); } } From 5f3de352136225269c0669fb1ff6d0382fe383f3 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 22 Jul 2021 17:44:37 -0500 Subject: [PATCH 132/947] Positions now directly exposed to constraints in the 2-ized path. --- .../Constraints/AngularAxisGearMotor.cs | 4 +-- BepuPhysics/Constraints/AngularAxisMotor.cs | 4 +-- BepuPhysics/Constraints/AngularHinge.cs | 4 +-- BepuPhysics/Constraints/AngularMotor.cs | 4 +-- BepuPhysics/Constraints/AngularServo.cs | 4 +-- BepuPhysics/Constraints/AngularSwivelHinge.cs | 4 +-- BepuPhysics/Constraints/AreaConstraint.cs | 4 +-- BepuPhysics/Constraints/BallSocket.cs | 5 ++-- BepuPhysics/Constraints/BallSocketMotor.cs | 4 +-- BepuPhysics/Constraints/BallSocketServo.cs | 4 +-- .../Constraints/CenterDistanceConstraint.cs | 6 +++-- .../Constraints/Contact/ContactConvexTypes.cs | 16 ++++++------ .../Constraints/Contact/ContactConvexTypes.tt | 4 +-- .../Contact/ContactNonconvexCommon.cs | 4 +-- BepuPhysics/Constraints/DistanceLimit.cs | 4 +-- BepuPhysics/Constraints/DistanceServo.cs | 4 +-- .../Constraints/FourBodyTypeProcessor.cs | 26 +++++++------------ BepuPhysics/Constraints/Hinge.cs | 4 +-- BepuPhysics/Constraints/LinearAxisLimit.cs | 4 +-- BepuPhysics/Constraints/LinearAxisMotor.cs | 4 +-- BepuPhysics/Constraints/LinearAxisServo.cs | 4 +-- BepuPhysics/Constraints/PointOnLineServo.cs | 4 +-- BepuPhysics/Constraints/SwingLimit.cs | 4 +-- BepuPhysics/Constraints/SwivelHinge.cs | 4 +-- .../Constraints/ThreeBodyTypeProcessor.cs | 20 ++++++-------- BepuPhysics/Constraints/TwistLimit.cs | 4 +-- BepuPhysics/Constraints/TwistMotor.cs | 4 +-- BepuPhysics/Constraints/TwistServo.cs | 4 +-- .../Constraints/TwoBodyTypeProcessor.cs | 10 +++---- BepuPhysics/Constraints/VolumeConstraint.cs | 4 +-- BepuPhysics/Constraints/Weld.cs | 8 +++--- .../Characters/CharacterMotionConstraint.cs | 4 +-- .../Characters/CharacterMotionConstraint.tt | 4 +-- 33 files changed, 93 insertions(+), 102 deletions(-) diff --git a/BepuPhysics/Constraints/AngularAxisGearMotor.cs b/BepuPhysics/Constraints/AngularAxisGearMotor.cs index 0815555ad..cec00320f 100644 --- a/BepuPhysics/Constraints/AngularAxisGearMotor.cs +++ b/BepuPhysics/Constraints/AngularAxisGearMotor.cs @@ -139,14 +139,14 @@ public static void ApplyImpulse(in Vector3Wide jA, in Vector3Wide negatedJB, in } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in AngularAxisGearMotorPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in AngularAxisGearMotorPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { QuaternionWide.TransformWithoutOverlap(prestep.LocalAxisA, orientationA, out var axis); ApplyImpulse(axis * prestep.VelocityScale, axis, accumulatedImpulses, inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, ref wsvA.Angular, ref wsvB.Angular); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in AngularAxisGearMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in AngularAxisGearMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { QuaternionWide.TransformWithoutOverlap(prestep.LocalAxisA, orientationA, out var axis); Vector3Wide.Scale(axis, prestep.VelocityScale, out var jA); diff --git a/BepuPhysics/Constraints/AngularAxisMotor.cs b/BepuPhysics/Constraints/AngularAxisMotor.cs index 4fb2f4e19..068ba21bf 100644 --- a/BepuPhysics/Constraints/AngularAxisMotor.cs +++ b/BepuPhysics/Constraints/AngularAxisMotor.cs @@ -136,14 +136,14 @@ public static void ApplyImpulse(in Vector3Wide jA, in Vector csi, in Symm } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in AngularAxisMotorPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in AngularAxisMotorPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { QuaternionWide.TransformWithoutOverlap(prestep.LocalAxisA, orientationA, out var axis); ApplyImpulse(axis, accumulatedImpulses, inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, ref wsvA.Angular, ref wsvB.Angular); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in AngularAxisMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in AngularAxisMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { QuaternionWide.TransformWithoutOverlap(prestep.LocalAxisA, orientationA, out var jA); Symmetric3x3Wide.TransformWithoutOverlap(jA, inertiaA.InverseInertiaTensor, out var jIA); diff --git a/BepuPhysics/Constraints/AngularHinge.cs b/BepuPhysics/Constraints/AngularHinge.cs index 8e85c4ccb..c304edd46 100644 --- a/BepuPhysics/Constraints/AngularHinge.cs +++ b/BepuPhysics/Constraints/AngularHinge.cs @@ -226,12 +226,12 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, ref projection, ref csi); } - public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in AngularHingePrestepData prestep, in Vector2Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in AngularHingePrestepData prestep, in Vector2Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } - public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in AngularHingePrestepData prestep, ref Vector2Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in AngularHingePrestepData prestep, ref Vector2Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/AngularMotor.cs b/BepuPhysics/Constraints/AngularMotor.cs index 84010a369..a678951a6 100644 --- a/BepuPhysics/Constraints/AngularMotor.cs +++ b/BepuPhysics/Constraints/AngularMotor.cs @@ -101,12 +101,12 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB projection.MaximumImpulse, projection.ImpulseToVelocityA, projection.NegatedImpulseToVelocityB, ref accumulatedImpulse); } - public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in AngularMotorPrestepData prestep, in Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in AngularMotorPrestepData prestep, in Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } - public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in AngularMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in AngularMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/AngularServo.cs b/BepuPhysics/Constraints/AngularServo.cs index 9e8876dea..414beec8f 100644 --- a/BepuPhysics/Constraints/AngularServo.cs +++ b/BepuPhysics/Constraints/AngularServo.cs @@ -142,12 +142,12 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB projection.MaximumImpulse, projection.ImpulseToVelocityA, projection.NegatedImpulseToVelocityB, ref accumulatedImpulse); } - public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in AngularServoPrestepData prestep, in Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in AngularServoPrestepData prestep, in Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } - public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in AngularServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in AngularServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/AngularSwivelHinge.cs b/BepuPhysics/Constraints/AngularSwivelHinge.cs index 33be3c7ff..5b802b9c2 100644 --- a/BepuPhysics/Constraints/AngularSwivelHinge.cs +++ b/BepuPhysics/Constraints/AngularSwivelHinge.cs @@ -154,12 +154,12 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, ref projection, ref csi); } - public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in AngularSwivelHingePrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in AngularSwivelHingePrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } - public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in AngularSwivelHingePrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in AngularSwivelHingePrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/AreaConstraint.cs b/BepuPhysics/Constraints/AreaConstraint.cs index fe817ef43..d02d89903 100644 --- a/BepuPhysics/Constraints/AreaConstraint.cs +++ b/BepuPhysics/Constraints/AreaConstraint.cs @@ -185,12 +185,12 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ApplyImpulse(ref velocityA, ref velocityB, ref velocityC, ref projection, ref negatedJacobianA, ref csi); } - public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide ac, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in AreaConstraintPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide ac, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in AreaConstraintPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC) { throw new NotImplementedException(); } - public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide ac, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, float dt, float inverseDt, in AreaConstraintPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide ac, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, float dt, float inverseDt, in AreaConstraintPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/BallSocket.cs b/BepuPhysics/Constraints/BallSocket.cs index 0b42e8cc7..65f617f96 100644 --- a/BepuPhysics/Constraints/BallSocket.cs +++ b/BepuPhysics/Constraints/BallSocket.cs @@ -109,7 +109,7 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB BallSocketShared.Solve(ref velocityA, ref velocityB, projection.OffsetA, projection.OffsetB, projection.BiasVelocity, projection.EffectiveMass, projection.SoftnessImpulseScale, ref accumulatedImpulse, projection.InertiaA, projection.InertiaB); } - public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BallSocketPrestepData prestep, in Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { //Note that we must reconstruct the world offsets from the body orientations since we do not store world offsets. @@ -118,7 +118,7 @@ public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inerti BallSocketShared.ApplyImpulse(ref wsvA, ref wsvB, offsetA, offsetB, inertiaA, inertiaB, accumulatedImpulses); } - public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in BallSocketPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { //Note that we must reconstruct the world offsets from the body orientations since we do not store world offsets. @@ -128,6 +128,7 @@ public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, BallSocketShared.ComputeEffectiveMass(inertiaA, inertiaB, ref offsetA, ref offsetB, ref effectiveMassCFMScale, out var effectiveMass); //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. + var ab = positionB - positionA; Vector3Wide.Add(ab, offsetB, out var anchorB); Vector3Wide.Subtract(anchorB, offsetA, out var error); Vector3Wide.Scale(error, positionErrorToVelocity, out var biasVelocity); diff --git a/BepuPhysics/Constraints/BallSocketMotor.cs b/BepuPhysics/Constraints/BallSocketMotor.cs index eff9a298f..3cc9f76f6 100644 --- a/BepuPhysics/Constraints/BallSocketMotor.cs +++ b/BepuPhysics/Constraints/BallSocketMotor.cs @@ -111,12 +111,12 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB BallSocketShared.Solve(ref velocityA, ref velocityB, ref projection.OffsetA, ref projection.OffsetB, ref projection.BiasVelocity, ref projection.EffectiveMass, ref projection.SoftnessImpulseScale, ref projection.MaximumImpulse, ref accumulatedImpulse, ref projection.InertiaA, ref projection.InertiaB); } - public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BallSocketMotorPrestepData prestep, in Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BallSocketMotorPrestepData prestep, in Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } - public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in BallSocketMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in BallSocketMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/BallSocketServo.cs b/BepuPhysics/Constraints/BallSocketServo.cs index 513d5ce13..7f25d38c9 100644 --- a/BepuPhysics/Constraints/BallSocketServo.cs +++ b/BepuPhysics/Constraints/BallSocketServo.cs @@ -118,12 +118,12 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB BallSocketShared.Solve(ref velocityA, ref velocityB, ref projection.OffsetA, ref projection.OffsetB, ref projection.BiasVelocity, ref projection.EffectiveMass, ref projection.SoftnessImpulseScale, ref projection.MaximumImpulse, ref accumulatedImpulse, ref projection.InertiaA, ref projection.InertiaB); } - public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BallSocketServoPrestepData prestep, in Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BallSocketServoPrestepData prestep, in Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } - public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in BallSocketServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in BallSocketServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/CenterDistanceConstraint.cs b/BepuPhysics/Constraints/CenterDistanceConstraint.cs index 65ed77ef8..c2941f3fa 100644 --- a/BepuPhysics/Constraints/CenterDistanceConstraint.cs +++ b/BepuPhysics/Constraints/CenterDistanceConstraint.cs @@ -137,9 +137,10 @@ static void ApplyImpulse(in Vector3Wide jacobianA, in Vector inverseMassA } //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in CenterDistancePrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { + var ab = positionB - positionA; var lengthSquared = ab.LengthSquared(); var inverseDistance = MathHelper.FastReciprocalSquareRoot(lengthSquared); var useFallback = Vector.LessThan(lengthSquared, new Vector(1e-10f)); @@ -151,10 +152,11 @@ public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inerti ApplyImpulse(jacobianA, inertiaA.InverseMass, inertiaB.InverseMass, accumulatedImpulses, ref wsvA, ref wsvB); } //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in CenterDistancePrestepData prestep, ref Vector accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { //Note that we need the actual length for error calculation. + var ab = positionB - positionA; var distance = ab.Length(); var inverseDistance = MathHelper.FastReciprocal(distance); var useFallback = Vector.LessThan(distance, new Vector(1e-5f)); diff --git a/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs b/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs index 373400a1a..8e3dba8fd 100644 --- a/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs +++ b/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs @@ -1165,13 +1165,13 @@ public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityW } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Contact1PrestepData prestep, in Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Contact1PrestepData prestep, in Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in Contact1PrestepData prestep, ref Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in Contact1PrestepData prestep, ref Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } @@ -1374,13 +1374,13 @@ public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityW } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Contact2PrestepData prestep, in Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Contact2PrestepData prestep, in Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in Contact2PrestepData prestep, ref Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in Contact2PrestepData prestep, ref Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } @@ -1598,13 +1598,13 @@ public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityW } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Contact3PrestepData prestep, in Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Contact3PrestepData prestep, in Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in Contact3PrestepData prestep, ref Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in Contact3PrestepData prestep, ref Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } @@ -1837,13 +1837,13 @@ public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityW } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Contact4PrestepData prestep, in Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Contact4PrestepData prestep, in Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in Contact4PrestepData prestep, ref Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in Contact4PrestepData prestep, ref Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt b/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt index c23e10985..0f5c855f0 100644 --- a/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt +++ b/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt @@ -321,13 +321,13 @@ for (int i = 0; i < contactCount; ++i) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart2(<#if(bodyCount == 1) {#>in Vector3Wide positionA, <#}#>in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(bodyCount == 2) {#>in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>in Contact<#=contactCount#><#=suffix#>PrestepData prestep, in Contact<#=contactCount#>AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA<#if(bodyCount == 2) {#>, ref BodyVelocityWide wsvB<#}#>) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(bodyCount == 2) {#>in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>in Contact<#=contactCount#><#=suffix#>PrestepData prestep, in Contact<#=contactCount#>AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA<#if(bodyCount == 2) {#>, ref BodyVelocityWide wsvB<#}#>) { throw new NotImplementedException(); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve2(<#if(bodyCount == 1) {#>in Vector3Wide positionA, <#}#>in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(bodyCount == 2) {#>in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>float dt, float inverseDt, in Contact<#=contactCount#><#=suffix#>PrestepData prestep, ref Contact<#=contactCount#>AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA<#if(bodyCount == 2) {#>, ref BodyVelocityWide wsvB<#}#>) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(bodyCount == 2) {#>in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>float dt, float inverseDt, in Contact<#=contactCount#><#=suffix#>PrestepData prestep, ref Contact<#=contactCount#>AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA<#if(bodyCount == 2) {#>, ref BodyVelocityWide wsvB<#}#>) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs b/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs index 8a4298981..0a5d7b96a 100644 --- a/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs +++ b/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs @@ -392,12 +392,12 @@ public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityW } } - public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in TPrestep prestep, in TAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in TPrestep prestep, in TAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new System.NotImplementedException(); } - public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in TPrestep prestep, ref TAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in TPrestep prestep, ref TAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new System.NotImplementedException(); } diff --git a/BepuPhysics/Constraints/DistanceLimit.cs b/BepuPhysics/Constraints/DistanceLimit.cs index ea56fe6be..38d6b6149 100644 --- a/BepuPhysics/Constraints/DistanceLimit.cs +++ b/BepuPhysics/Constraints/DistanceLimit.cs @@ -159,12 +159,12 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB } - public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in DistanceLimitPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in DistanceLimitPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } - public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in DistanceLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in DistanceLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/DistanceServo.cs b/BepuPhysics/Constraints/DistanceServo.cs index f18859251..718f85a75 100644 --- a/BepuPhysics/Constraints/DistanceServo.cs +++ b/BepuPhysics/Constraints/DistanceServo.cs @@ -241,12 +241,12 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB projection.LinearImpulseToVelocityA, projection.AngularImpulseToVelocityA, projection.LinearImpulseToVelocityB, projection.AngularImpulseToVelocityB, ref csi); } - public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in DistanceServoPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in DistanceServoPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } - public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in DistanceServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in DistanceServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs index 2f6b2fb79..b8f37b86c 100644 --- a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs @@ -36,16 +36,16 @@ void Prestep( void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref BodyVelocityWide velocityC, ref BodyVelocityWide velocityD, ref TProjection projection, ref TAccumulatedImpulse accumulatedImpulse); void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref BodyVelocityWide velocityC, ref BodyVelocityWide velocityD, ref TProjection projection, ref TAccumulatedImpulse accumulatedImpulse); void WarmStart2( - in QuaternionWide orientationA, in BodyInertiaWide inertiaA, - in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, - in Vector3Wide ac, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, - in Vector3Wide ad, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, + in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, + in Vector3Wide positionD, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, in TPrestepData prestep, in TAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC, ref BodyVelocityWide wsvD); void Solve2( - in QuaternionWide orientationA, in BodyInertiaWide inertiaA, - in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, - in Vector3Wide ac, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, - in Vector3Wide ad, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, float dt, float inverseDt, + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, + in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, + in Vector3Wide positionD, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, float dt, float inverseDt, in TPrestepData prestep, ref TAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC, ref BodyVelocityWide wsvD); } @@ -371,11 +371,8 @@ public unsafe override void WarmStart2(bodies, ref integratorCallbacks, ref integrationFlags, 3, dt, workerIndex, i, ref references.IndexD, count, out var positionD, out var orientationD, out var wsvD, out var inertiaD); - var ab = positionB - positionA; - var ac = positionC - positionA; - var ad = positionD - positionA; - function.WarmStart2(orientationA, inertiaA, ab, orientationB, inertiaB, ac, orientationC, inertiaC, ad, orientationD, inertiaD, prestep, accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC, ref wsvD); + function.WarmStart2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, positionD, orientationD, inertiaD, prestep, accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC, ref wsvD); if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) { @@ -414,11 +411,8 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f bodies.GatherState(ref references.IndexB, count, true, out var positionB, out var orientationB, out var wsvB, out var inertiaB); bodies.GatherState(ref references.IndexC, count, true, out var positionC, out var orientationC, out var wsvC, out var inertiaC); bodies.GatherState(ref references.IndexD, count, true, out var positionD, out var orientationD, out var wsvD, out var inertiaD); - var ab = positionB - positionA; - var ac = positionC - positionA; - var ad = positionD - positionA; - function.Solve2(orientationA, inertiaA, ab, orientationB, inertiaB, ac, orientationC, inertiaC, ad, orientationD, inertiaD, dt, inverseDt, prestep, ref accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC, ref wsvD); + function.Solve2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, positionD, orientationD, inertiaD, dt, inverseDt, prestep, ref accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC, ref wsvD); bodies.ScatterVelocities(ref wsvA, ref references.IndexA, count); bodies.ScatterVelocities(ref wsvB, ref references.IndexB, count); diff --git a/BepuPhysics/Constraints/Hinge.cs b/BepuPhysics/Constraints/Hinge.cs index 3f336238b..a048c526e 100644 --- a/BepuPhysics/Constraints/Hinge.cs +++ b/BepuPhysics/Constraints/Hinge.cs @@ -224,12 +224,12 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ApplyImpulse(ref velocityA, ref velocityB, ref projection, ref csi); } - public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in HingePrestepData prestep, in HingeAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in HingePrestepData prestep, in HingeAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } - public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in HingePrestepData prestep, ref HingeAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in HingePrestepData prestep, ref HingeAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/LinearAxisLimit.cs b/BepuPhysics/Constraints/LinearAxisLimit.cs index e2440583d..9b84f5179 100644 --- a/BepuPhysics/Constraints/LinearAxisLimit.cs +++ b/BepuPhysics/Constraints/LinearAxisLimit.cs @@ -149,12 +149,12 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ref csi); } - public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in LinearAxisLimitPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in LinearAxisLimitPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } - public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in LinearAxisLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in LinearAxisLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/LinearAxisMotor.cs b/BepuPhysics/Constraints/LinearAxisMotor.cs index 41a47d449..4e989e655 100644 --- a/BepuPhysics/Constraints/LinearAxisMotor.cs +++ b/BepuPhysics/Constraints/LinearAxisMotor.cs @@ -114,12 +114,12 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ref csi); } - public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in LinearAxisMotorPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in LinearAxisMotorPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } - public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in LinearAxisMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in LinearAxisMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/LinearAxisServo.cs b/BepuPhysics/Constraints/LinearAxisServo.cs index e269fdf82..7793b796d 100644 --- a/BepuPhysics/Constraints/LinearAxisServo.cs +++ b/BepuPhysics/Constraints/LinearAxisServo.cs @@ -227,12 +227,12 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ref csi); } - public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in LinearAxisServoPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in LinearAxisServoPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } - public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in LinearAxisServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in LinearAxisServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/PointOnLineServo.cs b/BepuPhysics/Constraints/PointOnLineServo.cs index bf2315a2d..577932da3 100644 --- a/BepuPhysics/Constraints/PointOnLineServo.cs +++ b/BepuPhysics/Constraints/PointOnLineServo.cs @@ -221,12 +221,12 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ApplyImpulse(ref velocityA, ref velocityB, projection.LinearJacobian, angularA, angularB, projection.InertiaA, projection.InertiaB, ref csi); } - public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in PointOnLineServoPrestepData prestep, in Vector2Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in PointOnLineServoPrestepData prestep, in Vector2Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } - public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in PointOnLineServoPrestepData prestep, ref Vector2Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in PointOnLineServoPrestepData prestep, ref Vector2Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/SwingLimit.cs b/BepuPhysics/Constraints/SwingLimit.cs index 3ec5ce40d..e6c0f95a9 100644 --- a/BepuPhysics/Constraints/SwingLimit.cs +++ b/BepuPhysics/Constraints/SwingLimit.cs @@ -179,12 +179,12 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, ref projection, ref csi); } - public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in SwingLimitPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in SwingLimitPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } - public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in SwingLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in SwingLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/SwivelHinge.cs b/BepuPhysics/Constraints/SwivelHinge.cs index 4716a37f3..db9bce044 100644 --- a/BepuPhysics/Constraints/SwivelHinge.cs +++ b/BepuPhysics/Constraints/SwivelHinge.cs @@ -211,12 +211,12 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ApplyImpulse(ref velocityA, ref velocityB, ref projection, ref csi); } - public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in SwivelHingePrestepData prestep, in Vector4Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in SwivelHingePrestepData prestep, in Vector4Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } - public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in SwivelHingePrestepData prestep, ref Vector4Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in SwivelHingePrestepData prestep, ref Vector4Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs index 5f3c45a62..06f63633b 100644 --- a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs @@ -32,14 +32,14 @@ void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vec void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref BodyVelocityWide velocityC, ref TProjection projection, ref TAccumulatedImpulse accumulatedImpulse); void WarmStart2( - in QuaternionWide orientationA, in BodyInertiaWide inertiaA, - in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, - in Vector3Wide ac, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, + in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in TPrestepData prestep, in TAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC); void Solve2( - in QuaternionWide orientationA, in BodyInertiaWide inertiaA, - in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, - in Vector3Wide ac, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, float dt, float inverseDt, + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, + in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, float dt, float inverseDt, in TPrestepData prestep, ref TAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC); } @@ -340,10 +340,8 @@ public unsafe override void WarmStart2(bodies, ref integratorCallbacks, ref integrationFlags, 2, dt, workerIndex, i, ref references.IndexC, count, out var positionC, out var orientationC, out var wsvC, out var inertiaC); - var ab = positionB - positionA; - var ac = positionC - positionA; - function.WarmStart2(orientationA, inertiaA, ab, orientationB, inertiaB, ac, orientationC, inertiaC, prestep, accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC); + function.WarmStart2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, prestep, accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC); if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) { @@ -379,10 +377,8 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f bodies.GatherState(ref references.IndexA, count, true, out var positionA, out var orientationA, out var wsvA, out var inertiaA); bodies.GatherState(ref references.IndexB, count, true, out var positionB, out var orientationB, out var wsvB, out var inertiaB); bodies.GatherState(ref references.IndexC, count, true, out var positionC, out var orientationC, out var wsvC, out var inertiaC); - var ab = positionB - positionA; - var ac = positionC - positionA; - function.Solve2(orientationA, inertiaA, ab, orientationB, inertiaB, ac, orientationC, inertiaC, dt, inverseDt, prestep, ref accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC); + function.Solve2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, dt, inverseDt, prestep, ref accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC); bodies.ScatterVelocities(ref wsvA, ref references.IndexA, count); bodies.ScatterVelocities(ref wsvB, ref references.IndexB, count); diff --git a/BepuPhysics/Constraints/TwistLimit.cs b/BepuPhysics/Constraints/TwistLimit.cs index 0080b5aae..9fef5459b 100644 --- a/BepuPhysics/Constraints/TwistLimit.cs +++ b/BepuPhysics/Constraints/TwistLimit.cs @@ -145,12 +145,12 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB TwistServoFunctions.ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, projection.ImpulseToVelocityA, projection.NegatedImpulseToVelocityB, csi); } - public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in TwistLimitPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in TwistLimitPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } - public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in TwistLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in TwistLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/TwistMotor.cs b/BepuPhysics/Constraints/TwistMotor.cs index 7c3955360..4ddddcef2 100644 --- a/BepuPhysics/Constraints/TwistMotor.cs +++ b/BepuPhysics/Constraints/TwistMotor.cs @@ -130,12 +130,12 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB TwistServoFunctions.ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, projection.ImpulseToVelocityA, projection.NegatedImpulseToVelocityB, csi); } - public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in TwistMotorPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in TwistMotorPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } - public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in TwistMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in TwistMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/TwistServo.cs b/BepuPhysics/Constraints/TwistServo.cs index 5aad15eb6..037b06dc9 100644 --- a/BepuPhysics/Constraints/TwistServo.cs +++ b/BepuPhysics/Constraints/TwistServo.cs @@ -215,12 +215,12 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, projection.ImpulseToVelocityA, projection.NegatedImpulseToVelocityB, csi); } - public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in TwistServoPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in TwistServoPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } - public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in TwistServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in TwistServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 16c86ce44..c055df4f4 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -33,9 +33,9 @@ public interface ITwoBodyConstraintFunctions(bodies, ref integratorCallbacks, ref integrationFlags, 1, dt, workerIndex, i, ref references.IndexB, count, out var positionB, out var orientationB, out var wsvB, out var inertiaB); - var ab = positionB - positionA; - function.WarmStart2(orientationA, inertiaA, ab, orientationB, inertiaB, prestep, accumulatedImpulses, ref wsvA, ref wsvB); + function.WarmStart2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, prestep, accumulatedImpulses, ref wsvA, ref wsvB); if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) { @@ -421,9 +420,8 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f //Prefetch(SolvePrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, i, exclusiveEndBundle); bodies.GatherState(ref references.IndexA, count, true, out var positionA, out var orientationA, out var wsvA, out var inertiaA); bodies.GatherState(ref references.IndexB, count, true, out var positionB, out var orientationB, out var wsvB, out var inertiaB); - var ab = positionB - positionA; - function.Solve2(orientationA, inertiaA, ab, orientationB, inertiaB, dt, inverseDt, prestep, ref accumulatedImpulses, ref wsvA, ref wsvB); + function.Solve2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, dt, inverseDt, prestep, ref accumulatedImpulses, ref wsvA, ref wsvB); bodies.ScatterVelocities(ref wsvA, ref references.IndexA, count); bodies.ScatterVelocities(ref wsvB, ref references.IndexB, count); diff --git a/BepuPhysics/Constraints/VolumeConstraint.cs b/BepuPhysics/Constraints/VolumeConstraint.cs index c91221962..c1ae458d0 100644 --- a/BepuPhysics/Constraints/VolumeConstraint.cs +++ b/BepuPhysics/Constraints/VolumeConstraint.cs @@ -206,12 +206,12 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ApplyImpulse(ref velocityA, ref velocityB, ref velocityC, ref velocityD, ref projection, ref negatedJacobianA, ref csi); } - public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide ac, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in Vector3Wide ad, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, in VolumeConstraintPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC, ref BodyVelocityWide wsvD) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in Vector3Wide positionD, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, in VolumeConstraintPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC, ref BodyVelocityWide wsvD) { throw new NotImplementedException(); } - public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide ac, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in Vector3Wide ad, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, float dt, float inverseDt, in VolumeConstraintPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC, ref BodyVelocityWide wsvD) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in Vector3Wide positionD, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, float dt, float inverseDt, in VolumeConstraintPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC, ref BodyVelocityWide wsvD) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/Weld.cs b/BepuPhysics/Constraints/Weld.cs index 9d91e61a3..aa69e5376 100644 --- a/BepuPhysics/Constraints/Weld.cs +++ b/BepuPhysics/Constraints/Weld.cs @@ -260,7 +260,7 @@ private static void ApplyImpulse(in BodyInertiaWide inertiaA, in BodyInertiaWide } //[MethodImpl(MethodImplOptions.NoInlining)] - public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in WeldPrestepData prestep, in WeldAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { Transform(prestep.LocalOffset, orientationA, out var offset); @@ -269,7 +269,7 @@ public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inerti //ApplyImpulse(inertiaA, inertiaB, prestep.LocalOffset * orientationA, accumulatedImpulses.Orientation, accumulatedImpulses.Offset, ref wsvA, ref wsvB); } //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in WeldPrestepData prestep, ref WeldAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { //The weld constraint handles 6 degrees of freedom simultaneously. The constraints are: @@ -304,7 +304,7 @@ public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, jmjtD.YY += diagonalAdd; jmjtD.ZZ += diagonalAdd; - var positionError = ab - offset; + var positionError = positionB - positionA - offset; var targetOrientationB = prestep.LocalOrientation * orientationA; //ConcatenateWithoutOverlap(prestep.LocalOrientation, orientationA, out var targetOrientationB); ConcatenateWithoutOverlap(Conjugate(targetOrientationB), orientationB, out var rotationError); @@ -356,7 +356,7 @@ public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, accumulatedImpulses.Offset.Y += offsetCSI.Y; accumulatedImpulses.Offset.Z += offsetCSI.Z; - ApplyImpulse(inertiaA, inertiaB, ab, orientationCSI, offsetCSI, ref wsvA, ref wsvB); + ApplyImpulse(inertiaA, inertiaB, offset, orientationCSI, offsetCSI, ref wsvA, ref wsvB); } diff --git a/Demos/Demos/Characters/CharacterMotionConstraint.cs b/Demos/Demos/Characters/CharacterMotionConstraint.cs index 66d85d5d0..8774c54b1 100644 --- a/Demos/Demos/Characters/CharacterMotionConstraint.cs +++ b/Demos/Demos/Characters/CharacterMotionConstraint.cs @@ -647,12 +647,12 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ApplyVerticalImpulse(basis, verticalAngularJacobianA, verticalAngularJacobianB, verticalCorrectiveImpulse, projection.InertiaA, projection.InertiaB, ref velocityA, ref velocityB); } - public void WarmStart2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in DynamicCharacterMotionPrestep prestep, in CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in DynamicCharacterMotionPrestep prestep, in CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } - public void Solve2(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in DynamicCharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in DynamicCharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } diff --git a/Demos/Demos/Characters/CharacterMotionConstraint.tt b/Demos/Demos/Characters/CharacterMotionConstraint.tt index d2876c9ee..e11069635 100644 --- a/Demos/Demos/Characters/CharacterMotionConstraint.tt +++ b/Demos/Demos/Characters/CharacterMotionConstraint.tt @@ -393,12 +393,12 @@ namespace Demos.Demos.Characters ApplyVerticalImpulse(basis, verticalAngularJacobianA, <#if (dynamic) {#>verticalAngularJacobianB, <#}#>verticalCorrectiveImpulse, projection.InertiaA, <#if (dynamic) {#>projection.InertiaB, <#}#>ref velocityA<#if (dynamic) {#>, ref velocityB<#}#>); } - public void WarmStart2(<#if(!dynamic) {#>in Vector3Wide positionA, <#}#>in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(dynamic) {#>in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>in <#=prefix#>CharacterMotionPrestep prestep, in CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA<#if(dynamic) {#>, ref BodyVelocityWide wsvB<#}#>) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(dynamic) {#>in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>in <#=prefix#>CharacterMotionPrestep prestep, in CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA<#if(dynamic) {#>, ref BodyVelocityWide wsvB<#}#>) { throw new NotImplementedException(); } - public void Solve2(<#if(!dynamic) {#>in Vector3Wide positionA, <#}#>in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(dynamic) {#>in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>float dt, float inverseDt, in <#=prefix#>CharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA<#if(dynamic) {#>, ref BodyVelocityWide wsvB<#}#>) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(dynamic) {#>in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>float dt, float inverseDt, in <#=prefix#>CharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA<#if(dynamic) {#>, ref BodyVelocityWide wsvB<#}#>) { throw new NotImplementedException(); } From 948f086b7c15464450f0ab224e33dc27b69f46cf Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 22 Jul 2021 20:07:54 -0500 Subject: [PATCH 133/947] OneBodyAngular/LinearServo 2-ified. Scatter velocities for partial access filters currently busted. --- .../Constraints/OneBodyAngularServo.cs | 29 ++++++++++++- BepuPhysics/Constraints/OneBodyLinearServo.cs | 42 ++++++++++++++++++- Demos/Demo.cs | 2 +- Demos/Program.cs | 2 +- 4 files changed, 69 insertions(+), 6 deletions(-) diff --git a/BepuPhysics/Constraints/OneBodyAngularServo.cs b/BepuPhysics/Constraints/OneBodyAngularServo.cs index 38d4ef046..d60325d90 100644 --- a/BepuPhysics/Constraints/OneBodyAngularServo.cs +++ b/BepuPhysics/Constraints/OneBodyAngularServo.cs @@ -134,14 +134,39 @@ public void Solve(ref BodyVelocityWide velocityA, ref OneBodyAngularServoProject projection.MaximumImpulse, projection.ImpulseToVelocity, ref accumulatedImpulse); } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void ApplyImpulse(in Symmetric3x3Wide inverseInertia, in Vector3Wide csi, ref Vector3Wide angularVelocity) + { + Symmetric3x3Wide.TransformWithoutOverlap(csi, inverseInertia, out var velocityChange); + Vector3Wide.Add(angularVelocity, velocityChange, out angularVelocity); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in OneBodyAngularServoPrestepData prestep, in Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) { - throw new NotImplementedException(); + ApplyImpulse(inertiaA.InverseInertiaTensor, accumulatedImpulses, ref wsvA.Angular); } + [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, in OneBodyAngularServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) { - throw new NotImplementedException(); + //Jacobians are just the identity matrix. + QuaternionWide.Conjugate(orientationA, out var inverseOrientation); + QuaternionWide.ConcatenateWithoutOverlap(inverseOrientation, prestep.TargetOrientation, out var errorRotation); + QuaternionWide.GetApproximateAxisAngleFromQuaternion(errorRotation, out var errorAxis, out var errorLength); + + SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); + Symmetric3x3Wide.Invert(inertiaA.InverseInertiaTensor, out var effectiveMass); + + ServoSettingsWide.ComputeClampedBiasVelocity(errorAxis, errorLength, positionErrorToVelocity, prestep.ServoSettings, dt, inverseDt, out var clampedBiasVelocity, out var maximumImpulse); + + //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - csiaAngular; + var csv = clampedBiasVelocity - wsvA.Angular; + Symmetric3x3Wide.TransformWithoutOverlap(csv, effectiveMass, out var csi); + csi = csi * effectiveMassCFMScale - accumulatedImpulses * softnessImpulseScale; + + ServoSettingsWide.ClampImpulse(maximumImpulse, ref accumulatedImpulses, ref csi); + ApplyImpulse(inertiaA.InverseInertiaTensor, csi, ref wsvA.Angular); } } diff --git a/BepuPhysics/Constraints/OneBodyLinearServo.cs b/BepuPhysics/Constraints/OneBodyLinearServo.cs index 39f250be0..732e7745f 100644 --- a/BepuPhysics/Constraints/OneBodyLinearServo.cs +++ b/BepuPhysics/Constraints/OneBodyLinearServo.cs @@ -168,14 +168,52 @@ public void Solve(ref BodyVelocityWide velocityA, ref OneBodyLinearServoProjecti SharedSolve(ref velocityA, projection, ref accumulatedImpulse); } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void ApplyImpulse(in Vector3Wide offset, in BodyInertiaWide inertia, ref BodyVelocityWide velocityA, in Vector3Wide csi) + { + Vector3Wide.CrossWithoutOverlap(offset, csi, out var wsi); + Symmetric3x3Wide.TransformWithoutOverlap(wsi, inertia.InverseInertiaTensor, out var change); + Vector3Wide.Add(velocityA.Angular, change, out velocityA.Angular); + + Vector3Wide.Scale(csi, inertia.InverseMass, out change); + Vector3Wide.Add(velocityA.Linear, change, out velocityA.Linear); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in OneBodyLinearServoPrestepData prestep, in Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) { - throw new NotImplementedException(); + QuaternionWide.TransformWithoutOverlap(prestep.LocalOffset, orientationA, out var offset); + ApplyImpulse(offset, inertiaA, ref wsvA, accumulatedImpulses); } + [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, in OneBodyLinearServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) { - throw new NotImplementedException(); + QuaternionWide.TransformWithoutOverlap(prestep.LocalOffset, orientationA, out var offset); + SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); + + //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. + Vector3Wide.Add(offset, positionA, out var worldGrabPoint); + Vector3Wide.Subtract(prestep.Target, worldGrabPoint, out var error); + ServoSettingsWide.ComputeClampedBiasVelocity(error, positionErrorToVelocity, prestep.ServoSettings, dt, inverseDt, out var biasVelocity, out var maximumImpulse); + + //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular); + var csv = biasVelocity - Vector3Wide.Cross(wsvA.Angular, offset) - wsvA.Linear; + + //The grabber is roughly equivalent to a ball socket joint with a nonzero goal (and only one body). + Symmetric3x3Wide.SkewSandwichWithoutOverlap(offset, inertiaA.InverseInertiaTensor, out var inverseEffectiveMass); + + //Linear contributions are simply I * inverseMass * I, which is just boosting the diagonal. + inverseEffectiveMass.XX += inertiaA.InverseMass; + inverseEffectiveMass.YY += inertiaA.InverseMass; + inverseEffectiveMass.ZZ += inertiaA.InverseMass; + Symmetric3x3Wide.Invert(inverseEffectiveMass, out var effectiveMass); + Symmetric3x3Wide.TransformWithoutOverlap(csv, effectiveMass, out var csi); + csi = csi * effectiveMassCFMScale - accumulatedImpulses * softnessImpulseScale; + + //The motor has a limited maximum force, so clamp the accumulated impulse. Watch out for division by zero. + ServoSettingsWide.ClampImpulse(maximumImpulse, ref accumulatedImpulses, ref csi); + ApplyImpulse(offset, inertiaA, ref wsvA, accumulatedImpulses); } } diff --git a/Demos/Demo.cs b/Demos/Demo.cs index 37ebe5190..5f3790da3 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -62,7 +62,7 @@ public virtual void Update(Window window, Camera camera, Input input, float dt) //fully decouple simulation and rendering rates across different threads. //(In either case, you'd also want to interpolate or extrapolate simulation results during rendering for smoothness.) //Note that taking steps of variable length can reduce stability. Gradual or one-off changes can work reasonably well. - Simulation.Timestep(1 / 60f, ThreadDispatcher); + Simulation.Timestep(1 / 60f); ////Here's an example of how it would look to use more frequent updates, but still with a fixed amount of time simulated per update call: //const float timeToSimulate = 1 / 60f; diff --git a/Demos/Program.cs b/Demos/Program.cs index 962deb171..957d455c6 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -19,7 +19,7 @@ static void Main(string[] args) { content = ContentArchive.Load(stream); } - HeadlessTest.Test(content, 16, 32, 512); + //HeadlessTest.Test(content, 16, 32, 512); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); From 6e4d5a2727c30ca6155e058ca0c249251350667f Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 23 Jul 2021 15:40:19 -0500 Subject: [PATCH 134/947] Fixed scatter velocities for filtered linear/angular only constraints. --- BepuPhysics/Bodies_GatherScatter.cs | 145 +++++++++++++++++----------- Demos/Demo.cs | 2 +- Demos/Program.cs | 2 +- 3 files changed, 89 insertions(+), 60 deletions(-) diff --git a/BepuPhysics/Bodies_GatherScatter.cs b/BepuPhysics/Bodies_GatherScatter.cs index 3d400f09e..4087c5b07 100644 --- a/BepuPhysics/Bodies_GatherScatter.cs +++ b/BepuPhysics/Bodies_GatherScatter.cs @@ -727,74 +727,103 @@ public unsafe void ScatterVelocities(ref BodyVelocityWide sourceV { if (Avx.IsSupported && Vector.Count == 8) { + //TODO: High precision poses means we'll end up with 64 bits of the second lane containing either orientation components or a position component. + //That'll require a revamp of this approach to mask out any writes to the pose components, but it'll all be compile time conditional. Unsafe.SkipInit(out TAccessFilter filter); - //for (int i = 0; i < 8; ++i) - //{ - // Get(ref sourceVelocities.Linear.X, i) = i + 100; - // Get(ref sourceVelocities.Linear.Y, i) = i + 200; - // Get(ref sourceVelocities.Linear.Z, i) = i + 300; - // Get(ref sourceVelocities.Angular.X, i) = i + 500; - // Get(ref sourceVelocities.Angular.Y, i) = i + 600; - // Get(ref sourceVelocities.Angular.Z, i) = i + 700; - //} - //Just like the gather but... transposed, we transpose from the wide representation into per-body velocities. - //The motion state struct puts the velocities into the second 32 byte chunk, so we can do a single write per body. - Vector256 m0, m1, m2, m4, m5, m6; - if (filter.AccessLinearVelocity) + if (filter.AccessLinearVelocity ^ filter.AccessAngularVelocity) { - m0 = sourceVelocities.Linear.X.AsVector256(); - m1 = sourceVelocities.Linear.Y.AsVector256(); - m2 = sourceVelocities.Linear.Z.AsVector256(); + //for (int i = 0; i < 8; ++i) + //{ + // Get(ref sourceVelocities.Linear.X, i) = i + 100; + // Get(ref sourceVelocities.Linear.Y, i) = i + 200; + // Get(ref sourceVelocities.Linear.Z, i) = i + 300; + // Get(ref sourceVelocities.Angular.X, i) = i + 500; + // Get(ref sourceVelocities.Angular.Y, i) = i + 600; + // Get(ref sourceVelocities.Angular.Z, i) = i + 700; + //} + //We can't write the entire lane if we only want linear or angular velocity; that would overwrite existing values with invalid data. + Vector256 m0, m1, m2; + int targetOffset; + if (filter.AccessLinearVelocity) + { + targetOffset = 8; + m0 = sourceVelocities.Linear.X.AsVector256(); + m1 = sourceVelocities.Linear.Y.AsVector256(); + m2 = sourceVelocities.Linear.Z.AsVector256(); + } + else + { + targetOffset = 12; + m0 = sourceVelocities.Angular.X.AsVector256(); + m1 = sourceVelocities.Angular.Y.AsVector256(); + m2 = sourceVelocities.Angular.Z.AsVector256(); + } + //We're being a bit lazy here- you could reduce the instructions a bit more given the two empty source lanes. + var n0 = Avx.UnpackLow(m0, m1); + var n1 = Avx.UnpackLow(m2, m2); + var n4 = Avx.UnpackHigh(m0, m1); + var n5 = Avx.UnpackHigh(m2, m2); + + var o0 = Avx.Shuffle(n0, n1, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o2 = Avx.Shuffle(n4, n5, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o4 = Avx.Shuffle(n0, n1, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o6 = Avx.Shuffle(n4, n5, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + + var indices = (int*)Unsafe.AsPointer(ref references); + var states = ActiveSet.SolverStates.Memory; + Sse.StoreAligned((float*)(states + indices[0]) + targetOffset, o0.GetLower()); + if (count > 1) Sse.StoreAligned((float*)(states + indices[1]) + targetOffset, o4.GetLower()); + if (count > 2) Sse.StoreAligned((float*)(states + indices[2]) + targetOffset, o2.GetLower()); + if (count > 3) Sse.StoreAligned((float*)(states + indices[3]) + targetOffset, o6.GetLower()); + if (count > 4) Sse.StoreAligned((float*)(states + indices[4]) + targetOffset, o0.GetUpper()); + if (count > 5) Sse.StoreAligned((float*)(states + indices[5]) + targetOffset, o4.GetUpper()); + if (count > 6) Sse.StoreAligned((float*)(states + indices[6]) + targetOffset, o2.GetUpper()); + if (count > 7) Sse.StoreAligned((float*)(states + indices[7]) + targetOffset, o6.GetUpper()); + } else { - m0 = Vector256.Zero; - m1 = Vector256.Zero; - m2 = Vector256.Zero; - } - if (filter.AccessAngularVelocity) - { + //Just like the gather but... transposed, we transpose from the wide representation into per-body velocities. + //The motion state struct puts the velocities into the second 32 byte chunk, so we can do a single write per body. + Vector256 m0, m1, m2, m4, m5, m6; + m0 = sourceVelocities.Linear.X.AsVector256(); + m1 = sourceVelocities.Linear.Y.AsVector256(); + m2 = sourceVelocities.Linear.Z.AsVector256(); m4 = sourceVelocities.Angular.X.AsVector256(); m5 = sourceVelocities.Angular.Y.AsVector256(); m6 = sourceVelocities.Angular.Z.AsVector256(); - } - else - { - m4 = Vector256.Zero; - m5 = Vector256.Zero; - m6 = Vector256.Zero; - } - //We're being a bit lazy here- you could reduce the instructions a bit more given the two empty source lanes. - var n0 = Avx.UnpackLow(m0, m1); - var n1 = Avx.UnpackLow(m2, m2); - var n2 = Avx.UnpackLow(m4, m5); - var n3 = Avx.UnpackLow(m6, m6); - var n4 = Avx.UnpackHigh(m0, m1); - var n5 = Avx.UnpackHigh(m2, m2); - var n6 = Avx.UnpackHigh(m4, m5); - var n7 = Avx.UnpackHigh(m6, m6); - - var o0 = Avx.Shuffle(n0, n1, 0 | (1 << 2) | (0 << 4) | (1 << 6)); - var o1 = Avx.Shuffle(n2, n3, 0 | (1 << 2) | (0 << 4) | (1 << 6)); - var o2 = Avx.Shuffle(n4, n5, 0 | (1 << 2) | (0 << 4) | (1 << 6)); - var o3 = Avx.Shuffle(n6, n7, 0 | (1 << 2) | (0 << 4) | (1 << 6)); - var o4 = Avx.Shuffle(n0, n1, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - var o5 = Avx.Shuffle(n2, n3, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - var o6 = Avx.Shuffle(n4, n5, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - var o7 = Avx.Shuffle(n6, n7, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - - var indices = (int*)Unsafe.AsPointer(ref references); - var states = ActiveSet.SolverStates.Memory; - Avx.StoreAligned((float*)(states + indices[0]) + 8, Avx.Permute2x128(o0, o1, 0 | (2 << 4))); - if (count > 1) Avx.StoreAligned((float*)(states + indices[1]) + 8, Avx.Permute2x128(o4, o5, 0 | (2 << 4))); - if (count > 2) Avx.StoreAligned((float*)(states + indices[2]) + 8, Avx.Permute2x128(o2, o3, 0 | (2 << 4))); - if (count > 3) Avx.StoreAligned((float*)(states + indices[3]) + 8, Avx.Permute2x128(o6, o7, 0 | (2 << 4))); - if (count > 4) Avx.StoreAligned((float*)(states + indices[4]) + 8, Avx.Permute2x128(o0, o1, 1 | (3 << 4))); - if (count > 5) Avx.StoreAligned((float*)(states + indices[5]) + 8, Avx.Permute2x128(o4, o5, 1 | (3 << 4))); - if (count > 6) Avx.StoreAligned((float*)(states + indices[6]) + 8, Avx.Permute2x128(o2, o3, 1 | (3 << 4))); - if (count > 7) Avx.StoreAligned((float*)(states + indices[7]) + 8, Avx.Permute2x128(o6, o7, 1 | (3 << 4))); + //We're being a bit lazy here- you could reduce the instructions a bit more given the two empty source lanes. + var n0 = Avx.UnpackLow(m0, m1); + var n1 = Avx.UnpackLow(m2, m2); + var n2 = Avx.UnpackLow(m4, m5); + var n3 = Avx.UnpackLow(m6, m6); + var n4 = Avx.UnpackHigh(m0, m1); + var n5 = Avx.UnpackHigh(m2, m2); + var n6 = Avx.UnpackHigh(m4, m5); + var n7 = Avx.UnpackHigh(m6, m6); + + var o0 = Avx.Shuffle(n0, n1, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o1 = Avx.Shuffle(n2, n3, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o2 = Avx.Shuffle(n4, n5, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o3 = Avx.Shuffle(n6, n7, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o4 = Avx.Shuffle(n0, n1, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o5 = Avx.Shuffle(n2, n3, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o6 = Avx.Shuffle(n4, n5, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o7 = Avx.Shuffle(n6, n7, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + + var indices = (int*)Unsafe.AsPointer(ref references); + var states = ActiveSet.SolverStates.Memory; + Avx.StoreAligned((float*)(states + indices[0]) + 8, Avx.Permute2x128(o0, o1, 0 | (2 << 4))); + if (count > 1) Avx.StoreAligned((float*)(states + indices[1]) + 8, Avx.Permute2x128(o4, o5, 0 | (2 << 4))); + if (count > 2) Avx.StoreAligned((float*)(states + indices[2]) + 8, Avx.Permute2x128(o2, o3, 0 | (2 << 4))); + if (count > 3) Avx.StoreAligned((float*)(states + indices[3]) + 8, Avx.Permute2x128(o6, o7, 0 | (2 << 4))); + if (count > 4) Avx.StoreAligned((float*)(states + indices[4]) + 8, Avx.Permute2x128(o0, o1, 1 | (3 << 4))); + if (count > 5) Avx.StoreAligned((float*)(states + indices[5]) + 8, Avx.Permute2x128(o4, o5, 1 | (3 << 4))); + if (count > 6) Avx.StoreAligned((float*)(states + indices[6]) + 8, Avx.Permute2x128(o2, o3, 1 | (3 << 4))); + if (count > 7) Avx.StoreAligned((float*)(states + indices[7]) + 8, Avx.Permute2x128(o6, o7, 1 | (3 << 4))); + } } else { diff --git a/Demos/Demo.cs b/Demos/Demo.cs index 5f3790da3..37ebe5190 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -62,7 +62,7 @@ public virtual void Update(Window window, Camera camera, Input input, float dt) //fully decouple simulation and rendering rates across different threads. //(In either case, you'd also want to interpolate or extrapolate simulation results during rendering for smoothness.) //Note that taking steps of variable length can reduce stability. Gradual or one-off changes can work reasonably well. - Simulation.Timestep(1 / 60f); + Simulation.Timestep(1 / 60f, ThreadDispatcher); ////Here's an example of how it would look to use more frequent updates, but still with a fixed amount of time simulated per update call: //const float timeToSimulate = 1 / 60f; diff --git a/Demos/Program.cs b/Demos/Program.cs index 957d455c6..962deb171 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -19,7 +19,7 @@ static void Main(string[] args) { content = ContentArchive.Load(stream); } - //HeadlessTest.Test(content, 16, 32, 512); + HeadlessTest.Test(content, 16, 32, 512); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); From 4687fe74d6c752d8c3b6ea826980cad65cf57ecb Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 23 Jul 2021 17:32:53 -0500 Subject: [PATCH 135/947] Convex contact warmstart2ification. --- .../Constraints/Contact/ContactConvexTypes.cs | 62 ++++++++++++++++--- .../Constraints/Contact/ContactConvexTypes.tt | 15 ++++- .../Constraints/Contact/PenetrationLimit.cs | 31 ++++++++++ .../Contact/PenetrationLimitOneBody.cs | 20 ++++++ .../Constraints/Contact/TangentFriction.cs | 37 ++++++++--- .../Contact/TangentFrictionOneBody.cs | 27 +++++--- .../Constraints/Contact/TwistFriction.cs | 15 +++-- .../Contact/TwistFrictionOneBody.cs | 20 +++--- 8 files changed, 191 insertions(+), 36 deletions(-) diff --git a/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs b/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs index 8e3dba8fd..e13c40d4e 100644 --- a/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs +++ b/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs @@ -354,7 +354,10 @@ public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityW [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Contact1OneBodyPrestepData prestep, in Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { - throw new NotImplementedException(); + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + TangentFrictionOneBody.WarmStart2(x, z, prestep.Contact0.OffsetA, inertiaA, accumulatedImpulses.Tangent, ref wsvA); + PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, accumulatedImpulses.Penetration0, ref wsvA); + TwistFrictionOneBody.WarmStart2(prestep.Normal, inertiaA, accumulatedImpulses.Twist, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -546,7 +549,12 @@ public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityW [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Contact2OneBodyPrestepData prestep, in Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { - throw new NotImplementedException(); + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, out var offsetToManifoldCenterA); + TangentFrictionOneBody.WarmStart2(x, z, offsetToManifoldCenterA, inertiaA, accumulatedImpulses.Tangent, ref wsvA); + PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, accumulatedImpulses.Penetration0, ref wsvA); + PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact1.OffsetA, accumulatedImpulses.Penetration0, ref wsvA); + TwistFrictionOneBody.WarmStart2(prestep.Normal, inertiaA, accumulatedImpulses.Twist, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -752,7 +760,13 @@ public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityW [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Contact3OneBodyPrestepData prestep, in Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { - throw new NotImplementedException(); + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, out var offsetToManifoldCenterA); + TangentFrictionOneBody.WarmStart2(x, z, offsetToManifoldCenterA, inertiaA, accumulatedImpulses.Tangent, ref wsvA); + PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, accumulatedImpulses.Penetration0, ref wsvA); + PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact1.OffsetA, accumulatedImpulses.Penetration0, ref wsvA); + PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact2.OffsetA, accumulatedImpulses.Penetration0, ref wsvA); + TwistFrictionOneBody.WarmStart2(prestep.Normal, inertiaA, accumulatedImpulses.Twist, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -972,7 +986,14 @@ public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityW [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Contact4OneBodyPrestepData prestep, in Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { - throw new NotImplementedException(); + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact3.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, prestep.Contact3.Depth, out var offsetToManifoldCenterA); + TangentFrictionOneBody.WarmStart2(x, z, offsetToManifoldCenterA, inertiaA, accumulatedImpulses.Tangent, ref wsvA); + PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, accumulatedImpulses.Penetration0, ref wsvA); + PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact1.OffsetA, accumulatedImpulses.Penetration0, ref wsvA); + PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact2.OffsetA, accumulatedImpulses.Penetration0, ref wsvA); + PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact3.OffsetA, accumulatedImpulses.Penetration0, ref wsvA); + TwistFrictionOneBody.WarmStart2(prestep.Normal, inertiaA, accumulatedImpulses.Twist, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -1167,7 +1188,11 @@ public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityW [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Contact1PrestepData prestep, in Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - throw new NotImplementedException(); + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + Vector3Wide.Subtract(prestep.Contact0.OffsetA, prestep.OffsetB, out var offsetToManifoldCenterB); + TangentFriction.WarmStart2(x, z, prestep.Contact0.OffsetA, offsetToManifoldCenterB, inertiaA, inertiaB, accumulatedImpulses.Tangent, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + TwistFriction.WarmStart2(prestep.Normal, inertiaA, inertiaB, accumulatedImpulses.Twist, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -1376,7 +1401,13 @@ public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityW [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Contact2PrestepData prestep, in Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - throw new NotImplementedException(); + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, out var offsetToManifoldCenterA); + Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); + TangentFriction.WarmStart2(x, z, offsetToManifoldCenterA, offsetToManifoldCenterB, inertiaA, inertiaB, accumulatedImpulses.Tangent, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + TwistFriction.WarmStart2(prestep.Normal, inertiaA, inertiaB, accumulatedImpulses.Twist, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -1600,7 +1631,14 @@ public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityW [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Contact3PrestepData prestep, in Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - throw new NotImplementedException(); + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, out var offsetToManifoldCenterA); + Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); + TangentFriction.WarmStart2(x, z, offsetToManifoldCenterA, offsetToManifoldCenterB, inertiaA, inertiaB, accumulatedImpulses.Tangent, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact2.OffsetA, prestep.Contact2.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + TwistFriction.WarmStart2(prestep.Normal, inertiaA, inertiaB, accumulatedImpulses.Twist, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -1839,7 +1877,15 @@ public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityW [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Contact4PrestepData prestep, in Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - throw new NotImplementedException(); + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact3.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, prestep.Contact3.Depth, out var offsetToManifoldCenterA); + Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); + TangentFriction.WarmStart2(x, z, offsetToManifoldCenterA, offsetToManifoldCenterB, inertiaA, inertiaB, accumulatedImpulses.Tangent, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact2.OffsetA, prestep.Contact2.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact3.OffsetA, prestep.Contact3.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + TwistFriction.WarmStart2(prestep.Normal, inertiaA, inertiaB, accumulatedImpulses.Twist, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] diff --git a/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt b/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt index 0f5c855f0..b33c9e721 100644 --- a/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt +++ b/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt @@ -323,7 +323,20 @@ for (int i = 0; i < contactCount; ++i) [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(bodyCount == 2) {#>in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>in Contact<#=contactCount#><#=suffix#>PrestepData prestep, in Contact<#=contactCount#>AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA<#if(bodyCount == 2) {#>, ref BodyVelocityWide wsvB<#}#>) { - throw new NotImplementedException(); + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); +<#if (contactCount > 1) {#> + FrictionHelpers.ComputeFrictionCenter(<#for (int i = 0; i < contactCount; ++i) {#>prestep.Contact<#=i#>.OffsetA, <#}#><#for (int i = 0; i < contactCount; ++i) {#>prestep.Contact<#=i#>.Depth, <#}#>out var offsetToManifoldCenterA); +<#if (bodyCount == 2) {#> + Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); +<#}#> +<#} else if(bodyCount == 2) {#> + Vector3Wide.Subtract(prestep.Contact0.OffsetA, prestep.OffsetB, out var offsetToManifoldCenterB); +<#}#> + TangentFriction<#=suffix#>.WarmStart2(x, z, <#if(contactCount > 1) {#>offsetToManifoldCenterA<#} else {#>prestep.Contact0.OffsetA<#}#>, <#if(bodyCount == 2) {#>offsetToManifoldCenterB, <#}#>inertiaA, <#if(bodyCount == 2) {#>inertiaB, <#}#>accumulatedImpulses.Tangent, ref wsvA<#if(bodyCount == 2) {#>, ref wsvB<#}#>); +<#for (int i = 0; i < contactCount; ++i) {#> + PenetrationLimit<#=suffix#>.WarmStart2(inertiaA, <#if(bodyCount == 2) {#>inertiaB, <#}#>prestep.Normal, prestep.Contact<#=i#>.OffsetA, <#if(bodyCount == 2) {#>prestep.Contact<#=i#>.OffsetA - prestep.OffsetB, <#}#>accumulatedImpulses.Penetration0, ref wsvA<#if(bodyCount == 2) {#>, ref wsvB<#}#>); +<#}#> + TwistFriction<#=suffix#>.WarmStart2(prestep.Normal, inertiaA, <#if(bodyCount == 2) {#>inertiaB, <#}#>accumulatedImpulses.Twist, ref wsvA<#if(bodyCount == 2) {#>, ref wsvB<#}#>); } [MethodImpl(MethodImplOptions.AggressiveInlining)] diff --git a/BepuPhysics/Constraints/Contact/PenetrationLimit.cs b/BepuPhysics/Constraints/Contact/PenetrationLimit.cs index d108051bb..551c81d0c 100644 --- a/BepuPhysics/Constraints/Contact/PenetrationLimit.cs +++ b/BepuPhysics/Constraints/Contact/PenetrationLimit.cs @@ -150,5 +150,36 @@ public static void UpdatePenetrationDepth(in Vector dt, in Vector3Wide co Vector3Wide.Dot(normal, contactVelocityDifference, out var estimatedDepthChangeVelocity); penetrationDepth -= estimatedDepthChangeVelocity * dt; } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void ApplyImpulse(in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, in Vector3Wide normal, in Vector3Wide angularA, in Vector3Wide angularB, + in Vector correctiveImpulse, + ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + var linearVelocityChangeA = correctiveImpulse * inertiaA.InverseMass; + Vector3Wide.Scale(normal, linearVelocityChangeA, out var correctiveVelocityALinearVelocity); + Vector3Wide.Scale(angularA, correctiveImpulse, out var correctiveAngularImpulseA); + Symmetric3x3Wide.TransformWithoutOverlap(correctiveAngularImpulseA, inertiaA.InverseInertiaTensor, out var correctiveVelocityAAngularVelocity); + + var linearVelocityChangeB = correctiveImpulse * inertiaB.InverseMass; + Vector3Wide.Scale(normal, linearVelocityChangeB, out var correctiveVelocityBLinearVelocity); + Vector3Wide.Scale(angularB, correctiveImpulse, out var correctiveAngularImpulseB); + Symmetric3x3Wide.TransformWithoutOverlap(correctiveAngularImpulseB, inertiaB.InverseInertiaTensor, out var correctiveVelocityBAngularVelocity); + + Vector3Wide.Add(wsvA.Linear, correctiveVelocityALinearVelocity, out wsvA.Linear); + Vector3Wide.Add(wsvA.Angular, correctiveVelocityAAngularVelocity, out wsvA.Angular); + Vector3Wide.Subtract(wsvB.Linear, correctiveVelocityBLinearVelocity, out wsvB.Linear); //Note subtract; normal = -jacobianLinearB + Vector3Wide.Add(wsvB.Angular, correctiveVelocityBAngularVelocity, out wsvB.Angular); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void WarmStart2( + in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, in Vector3Wide normal, in Vector3Wide contactOffsetA, in Vector3Wide contactOffsetB, + in Vector accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + Vector3Wide.CrossWithoutOverlap(contactOffsetA, normal, out var angularA); + Vector3Wide.CrossWithoutOverlap(normal, contactOffsetB, out var angularB); + ApplyImpulse(inertiaA, inertiaB, normal, angularA, angularB, accumulatedImpulse, ref wsvA, ref wsvB); + } } } diff --git a/BepuPhysics/Constraints/Contact/PenetrationLimitOneBody.cs b/BepuPhysics/Constraints/Contact/PenetrationLimitOneBody.cs index 4b9ab5679..8fe1aa600 100644 --- a/BepuPhysics/Constraints/Contact/PenetrationLimitOneBody.cs +++ b/BepuPhysics/Constraints/Contact/PenetrationLimitOneBody.cs @@ -101,5 +101,25 @@ public static void UpdatePenetrationDepth(in Vector dt, in Vector3Wide co penetrationDepth -= estimatedDepthChange * dt; } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void ApplyImpulse(in BodyInertiaWide inertiaA, in Vector3Wide normal, in Vector3Wide angularA, in Vector correctiveImpulse, ref BodyVelocityWide wsvA) + { + var linearVelocityChangeA = correctiveImpulse * inertiaA.InverseMass; + Vector3Wide.Scale(normal, linearVelocityChangeA, out var correctiveVelocityALinearVelocity); + Vector3Wide.Scale(angularA, correctiveImpulse, out var correctiveAngularImpulseA); + Symmetric3x3Wide.TransformWithoutOverlap(correctiveAngularImpulseA, inertiaA.InverseInertiaTensor, out var correctiveVelocityAAngularVelocity); + + Vector3Wide.Add(wsvA.Linear, correctiveVelocityALinearVelocity, out wsvA.Linear); + Vector3Wide.Add(wsvA.Angular, correctiveVelocityAAngularVelocity, out wsvA.Angular); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void WarmStart2(in BodyInertiaWide inertiaA, in Vector3Wide normal, in Vector3Wide contactOffsetA, in Vector accumulatedImpulse, ref BodyVelocityWide wsvA) + { + Vector3Wide.CrossWithoutOverlap(contactOffsetA, normal, out var angularA); + ApplyImpulse(inertiaA, normal, angularA, accumulatedImpulse, ref wsvA); + } + } } diff --git a/BepuPhysics/Constraints/Contact/TangentFriction.cs b/BepuPhysics/Constraints/Contact/TangentFriction.cs index d48475247..3efc468ca 100644 --- a/BepuPhysics/Constraints/Contact/TangentFriction.cs +++ b/BepuPhysics/Constraints/Contact/TangentFriction.cs @@ -28,7 +28,7 @@ public struct Jacobians } //Since this is an unshared specialized implementation, the jacobian calculation is kept in here rather than in the batch. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ComputeJacobians(ref Vector3Wide tangentX, ref Vector3Wide tangentY, ref Vector3Wide offsetA, ref Vector3Wide offsetB, + public static void ComputeJacobians(in Vector3Wide tangentX, in Vector3Wide tangentY, in Vector3Wide offsetA, in Vector3Wide offsetB, out Jacobians jacobians) { //Two velocity constraints: @@ -68,7 +68,7 @@ public static void Prestep(ref Vector3Wide tangentX, ref Vector3Wide tangentY, r ref BodyInertiaWide inertiaA, ref BodyInertiaWide inertiaB, out Projection projection) { - ComputeJacobians(ref tangentX, ref tangentY, ref offsetA, ref offsetB, out var jacobians); + ComputeJacobians(tangentX, tangentY, offsetA, offsetB, out var jacobians); //Compute effective mass matrix contributions. Symmetric2x2Wide.SandwichScale(jacobians.LinearA, inertiaA.InverseMass, out var linearContributionA); Symmetric2x2Wide.SandwichScale(jacobians.LinearA, inertiaB.InverseMass, out var linearContributionB); @@ -91,8 +91,8 @@ public static void Prestep(ref Vector3Wide tangentX, ref Vector3Wide tangentY, r /// Transforms an impulse from constraint space to world space, uses it to modify the cached world space velocities of the bodies. /// [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ApplyImpulse(ref Jacobians jacobians, ref BodyInertiaWide inertiaA, ref BodyInertiaWide inertiaB, - ref Vector2Wide correctiveImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void ApplyImpulse(in Jacobians jacobians, in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, + in Vector2Wide correctiveImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { Matrix2x3Wide.Transform(correctiveImpulse, jacobians.LinearA, out var linearImpulseA); Matrix2x3Wide.Transform(correctiveImpulse, jacobians.AngularA, out var angularImpulseA); @@ -112,10 +112,10 @@ public static void ApplyImpulse(ref Jacobians jacobians, ref BodyInertiaWide ine public static void WarmStart(ref Vector3Wide tangentX, ref Vector3Wide tangentY, ref TangentFriction.Projection projection, ref BodyInertiaWide inertiaA, ref BodyInertiaWide inertiaB, ref Vector2Wide accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - ComputeJacobians(ref tangentX, ref tangentY, ref projection.OffsetA, ref projection.OffsetB, out var jacobians); + ComputeJacobians(tangentX, tangentY, projection.OffsetA, projection.OffsetB, out var jacobians); //TODO: If the previous frame and current frame are associated with different time steps, the previous frame's solution won't be a good solution anymore. //To compensate for this, the accumulated impulse should be scaled if dt changes. - ApplyImpulse(ref jacobians, ref inertiaA, ref inertiaB, ref accumulatedImpulse, ref wsvA, ref wsvB); + ApplyImpulse(jacobians, inertiaA, inertiaB, accumulatedImpulse, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -151,11 +151,32 @@ public static void ComputeCorrectiveImpulse(ref BodyVelocityWide wsvA, ref BodyV [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Solve(ref Vector3Wide tangentX, ref Vector3Wide tangentY, ref TangentFriction.Projection projection, ref BodyInertiaWide inertiaA, ref BodyInertiaWide inertiaB, ref Vector maximumImpulse, ref Vector2Wide accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - ComputeJacobians(ref tangentX, ref tangentY, ref projection.OffsetA, ref projection.OffsetB, out var jacobians); + ComputeJacobians(tangentX, tangentY, projection.OffsetA, projection.OffsetB, out var jacobians); ComputeCorrectiveImpulse(ref wsvA, ref wsvB, ref projection, ref jacobians, ref maximumImpulse, ref accumulatedImpulse, out var correctiveCSI); - ApplyImpulse(ref jacobians, ref inertiaA, ref inertiaB, ref correctiveCSI, ref wsvA, ref wsvB); + ApplyImpulse(jacobians, inertiaA, inertiaB, correctiveCSI, ref wsvA, ref wsvB); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void WarmStart2(in Vector3Wide tangentX, in Vector3Wide tangentY, in Vector3Wide offsetToManifoldCenterA, in Vector3Wide offsetToManifoldCenterB, in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, + in Vector2Wide accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + ComputeJacobians(tangentX, tangentY, offsetToManifoldCenterA, offsetToManifoldCenterB, out var jacobians); + //TODO: If the previous frame and current frame are associated with different time steps, the previous frame's solution won't be a good solution anymore. + //To compensate for this, the accumulated impulse should be scaled if dt changes. + ApplyImpulse(jacobians, inertiaA, inertiaB, accumulatedImpulse, ref wsvA, ref wsvB); + } + + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Solve2(in Vector3Wide tangentX, in Vector3Wide tangentY, in Vector3Wide offsetToManifoldCenterA, in Vector3Wide offsetToManifoldCenterB, ref BodyInertiaWide inertiaA, ref BodyInertiaWide inertiaB, + in Vector2Wide accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + ComputeJacobians(tangentX, tangentY, offsetToManifoldCenterA, offsetToManifoldCenterB, out var jacobians); + //TODO: If the previous frame and current frame are associated with different time steps, the previous frame's solution won't be a good solution anymore. + //To compensate for this, the accumulated impulse should be scaled if dt changes. + ApplyImpulse(jacobians, inertiaA, inertiaB, accumulatedImpulse, ref wsvA, ref wsvB); + } } } diff --git a/BepuPhysics/Constraints/Contact/TangentFrictionOneBody.cs b/BepuPhysics/Constraints/Contact/TangentFrictionOneBody.cs index f6d357b48..00ea22916 100644 --- a/BepuPhysics/Constraints/Contact/TangentFrictionOneBody.cs +++ b/BepuPhysics/Constraints/Contact/TangentFrictionOneBody.cs @@ -25,7 +25,7 @@ public struct Projection //Since this is an unshared specialized implementation, the jacobian calculation is kept in here rather than in the batch. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ComputeJacobians(ref Vector3Wide tangentX, ref Vector3Wide tangentY, ref Vector3Wide offsetA, out Jacobians jacobians) + public static void ComputeJacobians(in Vector3Wide tangentX, in Vector3Wide tangentY, in Vector3Wide offsetA, out Jacobians jacobians) { //TODO: there would be a minor benefit in eliminating this copy manually, since it's very likely that the compiler won't. And it's probably also introducing more locals init. jacobians.LinearA.X = tangentX; @@ -38,7 +38,7 @@ public static void ComputeJacobians(ref Vector3Wide tangentX, ref Vector3Wide ta public static void Prestep(ref Vector3Wide tangentX, ref Vector3Wide tangentY, ref Vector3Wide offsetA, ref BodyInertiaWide inertiaA, out Projection projection) { - ComputeJacobians(ref tangentX, ref tangentY, ref offsetA, out var jacobians); + ComputeJacobians(tangentX, tangentY, offsetA, out var jacobians); //Compute effective mass matrix contributions. Symmetric2x2Wide.SandwichScale(jacobians.LinearA, inertiaA.InverseMass, out var linearContributionA); Symmetric3x3Wide.MatrixSandwich(jacobians.AngularA, inertiaA.InverseInertiaTensor, out var angularContributionA); @@ -55,8 +55,8 @@ public static void Prestep(ref Vector3Wide tangentX, ref Vector3Wide tangentY, r /// Transforms an impulse from constraint space to world space, uses it to modify the cached world space velocities of the bodies. /// [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ApplyImpulse(ref Jacobians jacobians, ref BodyInertiaWide inertiaA, - ref Vector2Wide correctiveImpulse, ref BodyVelocityWide wsvA) + public static void ApplyImpulse(in Jacobians jacobians, in BodyInertiaWide inertiaA, + in Vector2Wide correctiveImpulse, ref BodyVelocityWide wsvA) { Matrix2x3Wide.Transform(correctiveImpulse, jacobians.LinearA, out var linearImpulseA); Matrix2x3Wide.Transform(correctiveImpulse, jacobians.AngularA, out var angularImpulseA); @@ -71,10 +71,10 @@ public static void ApplyImpulse(ref Jacobians jacobians, ref BodyInertiaWide ine public static void WarmStart(ref Vector3Wide tangentX, ref Vector3Wide tangentY, ref Projection projection, ref BodyInertiaWide inertiaA, ref Vector2Wide accumulatedImpulse, ref BodyVelocityWide wsvA) { - ComputeJacobians(ref tangentX, ref tangentY, ref projection.OffsetA, out var jacobians); + ComputeJacobians(tangentX, tangentY, projection.OffsetA, out var jacobians); //TODO: If the previous frame and current frame are associated with different time steps, the previous frame's solution won't be a good solution anymore. //To compensate for this, the accumulated impulse should be scaled if dt changes. - ApplyImpulse(ref jacobians, ref inertiaA, ref accumulatedImpulse, ref wsvA); + ApplyImpulse(jacobians, inertiaA, accumulatedImpulse, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -103,11 +103,22 @@ public static void ComputeCorrectiveImpulse(ref BodyVelocityWide wsvA, ref Proje public static void Solve(ref Vector3Wide tangentX, ref Vector3Wide tangentY, ref Projection projection, ref BodyInertiaWide inertiaA, ref Vector maximumImpulse, ref Vector2Wide accumulatedImpulse, ref BodyVelocityWide wsvA) { - ComputeJacobians(ref tangentX, ref tangentY, ref projection.OffsetA, out var jacobians); + ComputeJacobians(tangentX, tangentY, projection.OffsetA, out var jacobians); ComputeCorrectiveImpulse(ref wsvA, ref projection, ref jacobians, ref maximumImpulse, ref accumulatedImpulse, out var correctiveCSI); - ApplyImpulse(ref jacobians, ref inertiaA, ref correctiveCSI, ref wsvA); + ApplyImpulse(jacobians, inertiaA, correctiveCSI, ref wsvA); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void WarmStart2(in Vector3Wide tangentX, in Vector3Wide tangentY, in Vector3Wide offsetToManifoldCenterA, in BodyInertiaWide inertiaA, in Vector2Wide accumulatedImpulse, ref BodyVelocityWide wsvA) + { + ComputeJacobians(tangentX, tangentY, offsetToManifoldCenterA, out var jacobians); + //TODO: If the previous frame and current frame are associated with different time steps, the previous frame's solution won't be a good solution anymore. + //To compensate for this, the accumulated impulse should be scaled if dt changes. + ApplyImpulse(jacobians, inertiaA, accumulatedImpulse, ref wsvA); + } + + } } diff --git a/BepuPhysics/Constraints/Contact/TwistFriction.cs b/BepuPhysics/Constraints/Contact/TwistFriction.cs index 286ccab70..8eaf8866a 100644 --- a/BepuPhysics/Constraints/Contact/TwistFriction.cs +++ b/BepuPhysics/Constraints/Contact/TwistFriction.cs @@ -44,8 +44,8 @@ public static void Prestep(ref BodyInertiaWide inertiaA, ref BodyInertiaWide ine /// Transforms an impulse from constraint space to world space, uses it to modify the cached world space velocities of the bodies. /// [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ApplyImpulse(ref Vector3Wide angularJacobianA, ref BodyInertiaWide inertiaA, ref BodyInertiaWide inertiaB, - ref Vector correctiveImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void ApplyImpulse(in Vector3Wide angularJacobianA, in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, + in Vector correctiveImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { Vector3Wide.Scale(angularJacobianA, correctiveImpulse, out var worldCorrectiveImpulseA); Symmetric3x3Wide.TransformWithoutOverlap(worldCorrectiveImpulseA, inertiaA.InverseInertiaTensor, out var worldCorrectiveVelocityA); @@ -58,7 +58,7 @@ public static void ApplyImpulse(ref Vector3Wide angularJacobianA, ref BodyInerti public static void WarmStart(ref Vector3Wide angularJacobianA, ref BodyInertiaWide inertiaA, ref BodyInertiaWide inertiaB, ref Vector accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - ApplyImpulse(ref angularJacobianA, ref inertiaA, ref inertiaB, ref accumulatedImpulse, ref wsvA, ref wsvB); + ApplyImpulse(angularJacobianA, inertiaA, inertiaB, accumulatedImpulse, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -82,9 +82,16 @@ public static void Solve(ref Vector3Wide angularJacobianA, ref BodyInertiaWide i ref Vector maximumImpulse, ref Vector accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeCorrectiveImpulse(ref angularJacobianA, ref projection, ref wsvA, ref wsvB, ref maximumImpulse, ref accumulatedImpulse, out var correctiveCSI); - ApplyImpulse(ref angularJacobianA, ref inertiaA, ref inertiaB, ref correctiveCSI, ref wsvA, ref wsvB); + ApplyImpulse(angularJacobianA, inertiaA, inertiaB, correctiveCSI, ref wsvA, ref wsvB); } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void WarmStart2(in Vector3Wide angularJacobianA, in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, + in Vector accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + ApplyImpulse(angularJacobianA, inertiaA, inertiaB, accumulatedImpulse, ref wsvA, ref wsvB); + } + } } diff --git a/BepuPhysics/Constraints/Contact/TwistFrictionOneBody.cs b/BepuPhysics/Constraints/Contact/TwistFrictionOneBody.cs index 9d55168c8..02b307916 100644 --- a/BepuPhysics/Constraints/Contact/TwistFrictionOneBody.cs +++ b/BepuPhysics/Constraints/Contact/TwistFrictionOneBody.cs @@ -18,7 +18,7 @@ public static void Prestep(ref BodyInertiaWide inertiaA, ref Vector3Wide angular { //Compute effective mass matrix contributions. No linear contributions for the twist constraint. Symmetric3x3Wide.VectorSandwich(angularJacobianA, inertiaA.InverseInertiaTensor, out var inverseEffectiveMass); - + //No softening; this constraint is rigid by design. (It does support a maximum force, but that is distinct from a proper damping ratio/natural frequency.) //Note that we have to guard against two bodies with infinite inertias. This is a valid state! //(We do not have to do such guarding on constraints with linear jacobians; dynamic bodies cannot have zero *mass*.) @@ -36,8 +36,8 @@ public static void Prestep(ref BodyInertiaWide inertiaA, ref Vector3Wide angular /// Transforms an impulse from constraint space to world space, uses it to modify the cached world space velocities of the bodies. /// [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ApplyImpulse(ref Vector3Wide angularJacobianA, ref BodyInertiaWide inertiaA, - ref Vector correctiveImpulse, ref BodyVelocityWide wsvA) + public static void ApplyImpulse(in Vector3Wide angularJacobianA, in BodyInertiaWide inertiaA, + in Vector correctiveImpulse, ref BodyVelocityWide wsvA) { Vector3Wide.Scale(angularJacobianA, correctiveImpulse, out var worldCorrectiveImpulseA); Symmetric3x3Wide.TransformWithoutOverlap(worldCorrectiveImpulseA, inertiaA.InverseInertiaTensor, out var worldCorrectiveVelocityA); @@ -48,11 +48,11 @@ public static void ApplyImpulse(ref Vector3Wide angularJacobianA, ref BodyInerti public static void WarmStart(ref Vector3Wide angularJacobianA, ref BodyInertiaWide inertiaA, ref Vector accumulatedImpulse, ref BodyVelocityWide wsvA) { - ApplyImpulse(ref angularJacobianA, ref inertiaA, ref accumulatedImpulse, ref wsvA); + ApplyImpulse(angularJacobianA, inertiaA, accumulatedImpulse, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ComputeCorrectiveImpulse(ref Vector3Wide angularJacobianA, ref TwistFrictionProjection projection, + public static void ComputeCorrectiveImpulse(ref Vector3Wide angularJacobianA, ref TwistFrictionProjection projection, ref BodyVelocityWide wsvA, ref Vector maximumImpulse, ref Vector accumulatedImpulse, out Vector correctiveCSI) { @@ -68,13 +68,19 @@ public static void ComputeCorrectiveImpulse(ref Vector3Wide angularJacobianA, re } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Solve(ref Vector3Wide angularJacobianA, ref BodyInertiaWide inertiaA, ref TwistFrictionProjection projection, + public static void Solve(ref Vector3Wide angularJacobianA, ref BodyInertiaWide inertiaA, ref TwistFrictionProjection projection, ref Vector maximumImpulse, ref Vector accumulatedImpulse, ref BodyVelocityWide wsvA) { ComputeCorrectiveImpulse(ref angularJacobianA, ref projection, ref wsvA, ref maximumImpulse, ref accumulatedImpulse, out var correctiveCSI); - ApplyImpulse(ref angularJacobianA, ref inertiaA, ref correctiveCSI, ref wsvA); + ApplyImpulse(angularJacobianA, inertiaA, correctiveCSI, ref wsvA); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void WarmStart2(in Vector3Wide angularJacobianA, in BodyInertiaWide inertiaA, in Vector accumulatedImpulse, ref BodyVelocityWide wsvA) + { + ApplyImpulse(angularJacobianA, inertiaA, accumulatedImpulse, ref wsvA); + } } } From a4b3ff73ea37b8335b0ffee37bc8e5ad04ee0495 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 23 Jul 2021 19:30:04 -0500 Subject: [PATCH 136/947] Convex solves. --- .../Constraints/Contact/ContactConvexTypes.cs | 186 +++++++++++++++--- .../Constraints/Contact/ContactConvexTypes.tt | 38 +++- .../Constraints/Contact/PenetrationLimit.cs | 69 ++++++- .../Contact/PenetrationLimitOneBody.cs | 63 +++++- .../Constraints/Contact/TangentFriction.cs | 30 ++- .../Contact/TangentFrictionOneBody.cs | 25 ++- .../Constraints/Contact/TwistFriction.cs | 34 +++- .../Contact/TwistFrictionOneBody.cs | 29 ++- 8 files changed, 408 insertions(+), 66 deletions(-) diff --git a/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs b/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs index e13c40d4e..ce44f4525 100644 --- a/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs +++ b/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs @@ -362,8 +362,20 @@ public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, in Contact1OneBodyPrestepData prestep, ref Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) - { - throw new NotImplementedException(); + { + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + var maximumTangentImpulse = prestep.MaterialProperties.FrictionCoefficient * (accumulatedImpulses.Penetration0); + TangentFrictionOneBody.Solve2(x, z, prestep.Contact0.OffsetA, inertiaA, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); + //Note that we solve the penetration constraints after the friction constraints. + //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. + //It's a pretty minor effect either way. + SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); + var inverseDtWide = new Vector(inverseDt); + PenetrationLimitOneBody.Solve2(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); + //If there's only one contact, then the contact patch as determined by contact distance would be zero. + //That can cause some subtle behavioral issues sometimes, so we approximate lever arm with the contact depth, assuming that the contact surface area will increase as the depth increases. + var maximumTwistImpulse = prestep.MaterialProperties.FrictionCoefficient * accumulatedImpulses.Penetration0 * prestep.Contact0.Depth; + TwistFrictionOneBody.Solve2(prestep.Normal, inertiaA, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); } } @@ -553,14 +565,29 @@ public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, out var offsetToManifoldCenterA); TangentFrictionOneBody.WarmStart2(x, z, offsetToManifoldCenterA, inertiaA, accumulatedImpulses.Tangent, ref wsvA); PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, accumulatedImpulses.Penetration0, ref wsvA); - PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact1.OffsetA, accumulatedImpulses.Penetration0, ref wsvA); + PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact1.OffsetA, accumulatedImpulses.Penetration1, ref wsvA); TwistFrictionOneBody.WarmStart2(prestep.Normal, inertiaA, accumulatedImpulses.Twist, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, in Contact2OneBodyPrestepData prestep, ref Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) - { - throw new NotImplementedException(); + { + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + var premultipliedFrictionCoefficient = new Vector(1f / 2f) * prestep.MaterialProperties.FrictionCoefficient; + var maximumTangentImpulse = premultipliedFrictionCoefficient * (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1); + FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, out var offsetToManifoldCenterA); + TangentFrictionOneBody.Solve2(x, z, offsetToManifoldCenterA, inertiaA, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); + //Note that we solve the penetration constraints after the friction constraints. + //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. + //It's a pretty minor effect either way. + SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); + var inverseDtWide = new Vector(inverseDt); + PenetrationLimitOneBody.Solve2(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); + PenetrationLimitOneBody.Solve2(inertiaA, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA); + var maximumTwistImpulse = premultipliedFrictionCoefficient * ( + accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact0.OffsetA) + + accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact1.OffsetA)); + TwistFrictionOneBody.Solve2(prestep.Normal, inertiaA, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); } } @@ -764,15 +791,32 @@ public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, out var offsetToManifoldCenterA); TangentFrictionOneBody.WarmStart2(x, z, offsetToManifoldCenterA, inertiaA, accumulatedImpulses.Tangent, ref wsvA); PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, accumulatedImpulses.Penetration0, ref wsvA); - PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact1.OffsetA, accumulatedImpulses.Penetration0, ref wsvA); - PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact2.OffsetA, accumulatedImpulses.Penetration0, ref wsvA); + PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact1.OffsetA, accumulatedImpulses.Penetration1, ref wsvA); + PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact2.OffsetA, accumulatedImpulses.Penetration2, ref wsvA); TwistFrictionOneBody.WarmStart2(prestep.Normal, inertiaA, accumulatedImpulses.Twist, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, in Contact3OneBodyPrestepData prestep, ref Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) - { - throw new NotImplementedException(); + { + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + var premultipliedFrictionCoefficient = new Vector(1f / 3f) * prestep.MaterialProperties.FrictionCoefficient; + var maximumTangentImpulse = premultipliedFrictionCoefficient * (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2); + FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, out var offsetToManifoldCenterA); + TangentFrictionOneBody.Solve2(x, z, offsetToManifoldCenterA, inertiaA, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); + //Note that we solve the penetration constraints after the friction constraints. + //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. + //It's a pretty minor effect either way. + SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); + var inverseDtWide = new Vector(inverseDt); + PenetrationLimitOneBody.Solve2(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); + PenetrationLimitOneBody.Solve2(inertiaA, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA); + PenetrationLimitOneBody.Solve2(inertiaA, prestep.Normal, prestep.Contact2.OffsetA, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA); + var maximumTwistImpulse = premultipliedFrictionCoefficient * ( + accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact0.OffsetA) + + accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact1.OffsetA) + + accumulatedImpulses.Penetration2 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact2.OffsetA)); + TwistFrictionOneBody.Solve2(prestep.Normal, inertiaA, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); } } @@ -990,16 +1034,35 @@ public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact3.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, prestep.Contact3.Depth, out var offsetToManifoldCenterA); TangentFrictionOneBody.WarmStart2(x, z, offsetToManifoldCenterA, inertiaA, accumulatedImpulses.Tangent, ref wsvA); PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, accumulatedImpulses.Penetration0, ref wsvA); - PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact1.OffsetA, accumulatedImpulses.Penetration0, ref wsvA); - PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact2.OffsetA, accumulatedImpulses.Penetration0, ref wsvA); - PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact3.OffsetA, accumulatedImpulses.Penetration0, ref wsvA); + PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact1.OffsetA, accumulatedImpulses.Penetration1, ref wsvA); + PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact2.OffsetA, accumulatedImpulses.Penetration2, ref wsvA); + PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact3.OffsetA, accumulatedImpulses.Penetration3, ref wsvA); TwistFrictionOneBody.WarmStart2(prestep.Normal, inertiaA, accumulatedImpulses.Twist, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, in Contact4OneBodyPrestepData prestep, ref Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) - { - throw new NotImplementedException(); + { + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + var premultipliedFrictionCoefficient = new Vector(1f / 4f) * prestep.MaterialProperties.FrictionCoefficient; + var maximumTangentImpulse = premultipliedFrictionCoefficient * (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2 + accumulatedImpulses.Penetration3); + FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact3.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, prestep.Contact3.Depth, out var offsetToManifoldCenterA); + TangentFrictionOneBody.Solve2(x, z, offsetToManifoldCenterA, inertiaA, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); + //Note that we solve the penetration constraints after the friction constraints. + //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. + //It's a pretty minor effect either way. + SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); + var inverseDtWide = new Vector(inverseDt); + PenetrationLimitOneBody.Solve2(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); + PenetrationLimitOneBody.Solve2(inertiaA, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA); + PenetrationLimitOneBody.Solve2(inertiaA, prestep.Normal, prestep.Contact2.OffsetA, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA); + PenetrationLimitOneBody.Solve2(inertiaA, prestep.Normal, prestep.Contact3.OffsetA, prestep.Contact3.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration3, ref wsvA); + var maximumTwistImpulse = premultipliedFrictionCoefficient * ( + accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact0.OffsetA) + + accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact1.OffsetA) + + accumulatedImpulses.Penetration2 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact2.OffsetA) + + accumulatedImpulses.Penetration3 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact3.OffsetA)); + TwistFrictionOneBody.Solve2(prestep.Normal, inertiaA, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); } } @@ -1197,8 +1260,21 @@ public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in Contact1PrestepData prestep, ref Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) - { - throw new NotImplementedException(); + { + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + var maximumTangentImpulse = prestep.MaterialProperties.FrictionCoefficient * (accumulatedImpulses.Penetration0); + Vector3Wide.Subtract(prestep.Contact0.OffsetA, prestep.OffsetB, out var offsetToManifoldCenterB); + TangentFriction.Solve2(x, z, prestep.Contact0.OffsetA, offsetToManifoldCenterB, inertiaA, inertiaB, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); + //Note that we solve the penetration constraints after the friction constraints. + //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. + //It's a pretty minor effect either way. + SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); + var inverseDtWide = new Vector(inverseDt); + PenetrationLimit.Solve2(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + //If there's only one contact, then the contact patch as determined by contact distance would be zero. + //That can cause some subtle behavioral issues sometimes, so we approximate lever arm with the contact depth, assuming that the contact surface area will increase as the depth increases. + var maximumTwistImpulse = prestep.MaterialProperties.FrictionCoefficient * accumulatedImpulses.Penetration0 * prestep.Contact0.Depth; + TwistFriction.Solve2(prestep.Normal, inertiaA, inertiaB, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); } } @@ -1406,14 +1482,30 @@ public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); TangentFriction.WarmStart2(x, z, offsetToManifoldCenterA, offsetToManifoldCenterB, inertiaA, inertiaB, accumulatedImpulses.Tangent, ref wsvA, ref wsvB); PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); TwistFriction.WarmStart2(prestep.Normal, inertiaA, inertiaB, accumulatedImpulses.Twist, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in Contact2PrestepData prestep, ref Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) - { - throw new NotImplementedException(); + { + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + var premultipliedFrictionCoefficient = new Vector(1f / 2f) * prestep.MaterialProperties.FrictionCoefficient; + var maximumTangentImpulse = premultipliedFrictionCoefficient * (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1); + FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, out var offsetToManifoldCenterA); + Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); + TangentFriction.Solve2(x, z, offsetToManifoldCenterA, offsetToManifoldCenterB, inertiaA, inertiaB, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); + //Note that we solve the penetration constraints after the friction constraints. + //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. + //It's a pretty minor effect either way. + SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); + var inverseDtWide = new Vector(inverseDt); + PenetrationLimit.Solve2(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + PenetrationLimit.Solve2(inertiaA, inertiaB, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.OffsetA - prestep.OffsetB, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); + var maximumTwistImpulse = premultipliedFrictionCoefficient * ( + accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact0.OffsetA) + + accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact1.OffsetA)); + TwistFriction.Solve2(prestep.Normal, inertiaA, inertiaB, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); } } @@ -1636,15 +1728,33 @@ public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); TangentFriction.WarmStart2(x, z, offsetToManifoldCenterA, offsetToManifoldCenterB, inertiaA, inertiaB, accumulatedImpulses.Tangent, ref wsvA, ref wsvB); PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact2.OffsetA, prestep.Contact2.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact2.OffsetA, prestep.Contact2.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); TwistFriction.WarmStart2(prestep.Normal, inertiaA, inertiaB, accumulatedImpulses.Twist, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in Contact3PrestepData prestep, ref Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) - { - throw new NotImplementedException(); + { + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + var premultipliedFrictionCoefficient = new Vector(1f / 3f) * prestep.MaterialProperties.FrictionCoefficient; + var maximumTangentImpulse = premultipliedFrictionCoefficient * (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2); + FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, out var offsetToManifoldCenterA); + Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); + TangentFriction.Solve2(x, z, offsetToManifoldCenterA, offsetToManifoldCenterB, inertiaA, inertiaB, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); + //Note that we solve the penetration constraints after the friction constraints. + //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. + //It's a pretty minor effect either way. + SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); + var inverseDtWide = new Vector(inverseDt); + PenetrationLimit.Solve2(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + PenetrationLimit.Solve2(inertiaA, inertiaB, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.OffsetA - prestep.OffsetB, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); + PenetrationLimit.Solve2(inertiaA, inertiaB, prestep.Normal, prestep.Contact2.OffsetA, prestep.Contact2.OffsetA - prestep.OffsetB, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); + var maximumTwistImpulse = premultipliedFrictionCoefficient * ( + accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact0.OffsetA) + + accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact1.OffsetA) + + accumulatedImpulses.Penetration2 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact2.OffsetA)); + TwistFriction.Solve2(prestep.Normal, inertiaA, inertiaB, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); } } @@ -1882,16 +1992,36 @@ public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); TangentFriction.WarmStart2(x, z, offsetToManifoldCenterA, offsetToManifoldCenterB, inertiaA, inertiaB, accumulatedImpulses.Tangent, ref wsvA, ref wsvB); PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact2.OffsetA, prestep.Contact2.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact3.OffsetA, prestep.Contact3.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact2.OffsetA, prestep.Contact2.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact3.OffsetA, prestep.Contact3.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration3, ref wsvA, ref wsvB); TwistFriction.WarmStart2(prestep.Normal, inertiaA, inertiaB, accumulatedImpulses.Twist, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in Contact4PrestepData prestep, ref Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) - { - throw new NotImplementedException(); + { + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + var premultipliedFrictionCoefficient = new Vector(1f / 4f) * prestep.MaterialProperties.FrictionCoefficient; + var maximumTangentImpulse = premultipliedFrictionCoefficient * (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2 + accumulatedImpulses.Penetration3); + FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact3.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, prestep.Contact3.Depth, out var offsetToManifoldCenterA); + Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); + TangentFriction.Solve2(x, z, offsetToManifoldCenterA, offsetToManifoldCenterB, inertiaA, inertiaB, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); + //Note that we solve the penetration constraints after the friction constraints. + //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. + //It's a pretty minor effect either way. + SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); + var inverseDtWide = new Vector(inverseDt); + PenetrationLimit.Solve2(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + PenetrationLimit.Solve2(inertiaA, inertiaB, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.OffsetA - prestep.OffsetB, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); + PenetrationLimit.Solve2(inertiaA, inertiaB, prestep.Normal, prestep.Contact2.OffsetA, prestep.Contact2.OffsetA - prestep.OffsetB, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); + PenetrationLimit.Solve2(inertiaA, inertiaB, prestep.Normal, prestep.Contact3.OffsetA, prestep.Contact3.OffsetA - prestep.OffsetB, prestep.Contact3.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration3, ref wsvA, ref wsvB); + var maximumTwistImpulse = premultipliedFrictionCoefficient * ( + accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact0.OffsetA) + + accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact1.OffsetA) + + accumulatedImpulses.Penetration2 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact2.OffsetA) + + accumulatedImpulses.Penetration3 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact3.OffsetA)); + TwistFriction.Solve2(prestep.Normal, inertiaA, inertiaB, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); } } diff --git a/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt b/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt index b33c9e721..65014855f 100644 --- a/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt +++ b/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt @@ -334,15 +334,47 @@ for (int i = 0; i < contactCount; ++i) <#}#> TangentFriction<#=suffix#>.WarmStart2(x, z, <#if(contactCount > 1) {#>offsetToManifoldCenterA<#} else {#>prestep.Contact0.OffsetA<#}#>, <#if(bodyCount == 2) {#>offsetToManifoldCenterB, <#}#>inertiaA, <#if(bodyCount == 2) {#>inertiaB, <#}#>accumulatedImpulses.Tangent, ref wsvA<#if(bodyCount == 2) {#>, ref wsvB<#}#>); <#for (int i = 0; i < contactCount; ++i) {#> - PenetrationLimit<#=suffix#>.WarmStart2(inertiaA, <#if(bodyCount == 2) {#>inertiaB, <#}#>prestep.Normal, prestep.Contact<#=i#>.OffsetA, <#if(bodyCount == 2) {#>prestep.Contact<#=i#>.OffsetA - prestep.OffsetB, <#}#>accumulatedImpulses.Penetration0, ref wsvA<#if(bodyCount == 2) {#>, ref wsvB<#}#>); + PenetrationLimit<#=suffix#>.WarmStart2(inertiaA, <#if(bodyCount == 2) {#>inertiaB, <#}#>prestep.Normal, prestep.Contact<#=i#>.OffsetA, <#if(bodyCount == 2) {#>prestep.Contact<#=i#>.OffsetA - prestep.OffsetB, <#}#>accumulatedImpulses.Penetration<#=i#>, ref wsvA<#if(bodyCount == 2) {#>, ref wsvB<#}#>); <#}#> TwistFriction<#=suffix#>.WarmStart2(prestep.Normal, inertiaA, <#if(bodyCount == 2) {#>inertiaB, <#}#>accumulatedImpulses.Twist, ref wsvA<#if(bodyCount == 2) {#>, ref wsvB<#}#>); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(bodyCount == 2) {#>in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>float dt, float inverseDt, in Contact<#=contactCount#><#=suffix#>PrestepData prestep, ref Contact<#=contactCount#>AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA<#if(bodyCount == 2) {#>, ref BodyVelocityWide wsvB<#}#>) - { - throw new NotImplementedException(); + { + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); +<#if (contactCount > 1) {#> + var premultipliedFrictionCoefficient = new Vector(1f / <#=contactCount#>f) * prestep.MaterialProperties.FrictionCoefficient; +<#}#> + var maximumTangentImpulse = <#=contactCount > 1 ? "premultipliedFrictionCoefficient" : "prestep.MaterialProperties.FrictionCoefficient"#> * (<#for (int i = 0; i < contactCount; ++i) {#>accumulatedImpulses.Penetration<#=i#><#if(i < contactCount - 1){#> + <#}}#>); +<#if (contactCount > 1) {#> + FrictionHelpers.ComputeFrictionCenter(<#for (int i = 0; i < contactCount; ++i) {#>prestep.Contact<#=i#>.OffsetA, <#}#><#for (int i = 0; i < contactCount; ++i) {#>prestep.Contact<#=i#>.Depth, <#}#>out var offsetToManifoldCenterA); +<#if (bodyCount == 2) {#> + Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); +<#}#> +<#} else if(bodyCount == 2) {#> + Vector3Wide.Subtract(prestep.Contact0.OffsetA, prestep.OffsetB, out var offsetToManifoldCenterB); +<#}#> + TangentFriction<#=suffix#>.Solve2(x, z, <#=contactCount == 1 ? "prestep.Contact0.OffsetA" : "offsetToManifoldCenterA"#><#=bodyCount == 2 ? ", offsetToManifoldCenterB" : ""#>, inertiaA<#=bodyCount == 2 ? ", inertiaB" : ""#>, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); + //Note that we solve the penetration constraints after the friction constraints. + //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. + //It's a pretty minor effect either way. + SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); + var inverseDtWide = new Vector(inverseDt); +<#for (int i = 0; i < contactCount; ++i) {#> + PenetrationLimit<#=suffix#>.Solve2(inertiaA<#=bodyCount == 2 ? ", inertiaB" : ""#>, prestep.Normal, prestep.Contact<#=i#>.OffsetA, <#if(bodyCount == 2) {#>prestep.Contact<#=i#>.OffsetA - prestep.OffsetB, <#}#>prestep.Contact<#=i#>.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration<#=i#>, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); +<#}#> +<#if (contactCount == 1) {#> + //If there's only one contact, then the contact patch as determined by contact distance would be zero. + //That can cause some subtle behavioral issues sometimes, so we approximate lever arm with the contact depth, assuming that the contact surface area will increase as the depth increases. + var maximumTwistImpulse = <#=contactCount > 1 ? "premultipliedFrictionCoefficient" : "prestep.MaterialProperties.FrictionCoefficient"#> * accumulatedImpulses.Penetration0 * prestep.Contact0.Depth; +<#} else {#> + var maximumTwistImpulse = <#=contactCount > 1 ? "premultipliedFrictionCoefficient" : "prestep.MaterialProperties.FrictionCoefficient"#> * ( +<#for (int i = 0; i < contactCount; ++i) {#> + accumulatedImpulses.Penetration<#=i#> * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact<#=i#>.OffsetA)<#=i == contactCount - 1 ? ");" : " +"#> +<#}#> +<#}#> + TwistFriction<#=suffix#>.Solve2(prestep.Normal, inertiaA, <#if (bodyCount == 2) {#>inertiaB, <#}#>maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); } } diff --git a/BepuPhysics/Constraints/Contact/PenetrationLimit.cs b/BepuPhysics/Constraints/Contact/PenetrationLimit.cs index 551c81d0c..e0840b4ab 100644 --- a/BepuPhysics/Constraints/Contact/PenetrationLimit.cs +++ b/BepuPhysics/Constraints/Contact/PenetrationLimit.cs @@ -107,30 +107,28 @@ public static void WarmStart( [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ComputeCorrectiveImpulse(in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, - in PenetrationLimitProjection projection, - in Vector3Wide normal, in Vector softnessImpulseScale, + in Vector3Wide normal, in Vector3Wide angularA, in Vector3Wide angularB, in Vector biasVelocity, in Vector softnessImpulseScale, in Vector effectiveMass, ref Vector accumulatedImpulse, out Vector correctiveCSI) { //Note that we do NOT use pretransformed jacobians here; the linear jacobian sharing (normal) meant that we had the effective mass anyway. Vector3Wide.Dot(wsvA.Linear, normal, out var csvaLinear); - Vector3Wide.Dot(wsvA.Angular, projection.AngularA, out var csvaAngular); + Vector3Wide.Dot(wsvA.Angular, angularA, out var csvaAngular); Vector3Wide.Dot(wsvB.Linear, normal, out var negatedCSVBLinear); - Vector3Wide.Dot(wsvB.Angular, projection.AngularB, out var csvbAngular); + Vector3Wide.Dot(wsvB.Angular, angularB, out var csvbAngular); //Compute negated version to avoid the need for an explicit negate. - var negatedCSI = accumulatedImpulse * softnessImpulseScale + (csvaLinear - negatedCSVBLinear + csvaAngular + csvbAngular - projection.BiasVelocity) * projection.EffectiveMass; + var negatedCSI = accumulatedImpulse * softnessImpulseScale + (csvaLinear - negatedCSVBLinear + csvaAngular + csvbAngular - biasVelocity) * effectiveMass; var previousAccumulated = accumulatedImpulse; accumulatedImpulse = Vector.Max(Vector.Zero, accumulatedImpulse - negatedCSI); correctiveCSI = accumulatedImpulse - previousAccumulated; - } [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Solve(in PenetrationLimitProjection projection, in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, in Vector3Wide normal, in Vector softnessImpulseScale, ref Vector accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - ComputeCorrectiveImpulse(wsvA, wsvB, projection, normal, softnessImpulseScale, ref accumulatedImpulse, out var correctiveCSI); + ComputeCorrectiveImpulse(wsvA, wsvB, normal, projection.AngularA, projection.AngularB, projection.BiasVelocity, softnessImpulseScale, projection.EffectiveMass, ref accumulatedImpulse, out var correctiveCSI); ApplyImpulse(projection, inertiaA, inertiaB, normal, correctiveCSI, ref wsvA, ref wsvB); } @@ -181,5 +179,62 @@ public static void WarmStart2( Vector3Wide.CrossWithoutOverlap(normal, contactOffsetB, out var angularB); ApplyImpulse(inertiaA, inertiaB, normal, angularA, angularB, accumulatedImpulse, ref wsvA, ref wsvB); } + + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Solve2( + in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, in Vector3Wide normal, in Vector3Wide contactOffsetA, in Vector3Wide contactOffsetB, + in Vector depth, in Vector positionErrorToVelocity, in Vector effectiveMassCFMScale, in Vector maximumRecoveryVelocity, in Vector inverseDt, in Vector softnessImpulseScale, + ref Vector accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + //The contact penetration constraint takes the form: + //dot(positionA + offsetA, N) >= dot(positionB + offsetB, N) + //Or: + //dot(positionA + offsetA, N) - dot(positionB + offsetB, N) >= 0 + //dot(positionA + offsetA - positionB - offsetB, N) >= 0 + //where positionA and positionB are the center of mass positions of the bodies offsetA and offsetB are world space offsets from the center of mass to the contact, + //and N is a unit length vector calibrated to point from B to A. (The normal pointing direction is important; it changes the sign.) + //In practice, we'll use the collision detection system's penetration depth instead of trying to recompute the error here. + + //So, treating the normal as constant, the velocity constraint is: + //dot(d/dt(positionA + offsetA - positionB - offsetB), N) >= 0 + //dot(linearVelocityA + d/dt(offsetA) - linearVelocityB - d/dt(offsetB)), N) >= 0 + //The velocity of the offsets are defined by the angular velocity. + //dot(linearVelocityA + angularVelocityA x offsetA - linearVelocityB - angularVelocityB x offsetB), N) >= 0 + //dot(linearVelocityA, N) + dot(angularVelocityA x offsetA, N) - dot(linearVelocityB, N) - dot(angularVelocityB x offsetB), N) >= 0 + //Use the properties of the scalar triple product: + //dot(linearVelocityA, N) + dot(offsetA x N, angularVelocityA) - dot(linearVelocityB, N) - dot(offsetB x N, angularVelocityB) >= 0 + //Bake in the negations: + //dot(linearVelocityA, N) + dot(offsetA x N, angularVelocityA) + dot(linearVelocityB, -N) + dot(-offsetB x N, angularVelocityB) >= 0 + //A x B = -B x A: + //dot(linearVelocityA, N) + dot(offsetA x N, angularVelocityA) + dot(linearVelocityB, -N) + dot(N x offsetB, angularVelocityB) >= 0 + //And there you go, the jacobians! + //linearA: N + //angularA: offsetA x N + //linearB: -N + //angularB: N x offsetB + //Note that we leave the penetration depth as is, even when it's negative. Speculative contacts! + Vector3Wide.CrossWithoutOverlap(contactOffsetA, normal, out var angularA); + Vector3Wide.CrossWithoutOverlap(normal, contactOffsetB, out var angularB); + + //effective mass + Symmetric3x3Wide.VectorSandwich(angularA, inertiaA.InverseInertiaTensor, out var angularA0); + Symmetric3x3Wide.VectorSandwich(angularB, inertiaB.InverseInertiaTensor, out var angularB0); + + //Linear effective mass contribution notes: + //1) The J * M^-1 * JT can be reordered to J * JT * M^-1 for the linear components, since M^-1 is a scalar and dot(n * scalar, n) = dot(n, n) * scalar. + //2) dot(normal, normal) == 1, so the contribution from each body is just its inverse mass. + var linear = inertiaA.InverseMass + inertiaB.InverseMass; + //Note that we don't precompute the JT * effectiveMass term. Since the jacobians are shared, we have to do that multiply anyway. + var effectiveMass = effectiveMassCFMScale / (linear + angularA0 + angularB0); + + //If depth is negative, the bias velocity will permit motion up until the depth hits zero. This works because positionErrorToVelocity * dt will always be <=1. + var biasVelocity = Vector.Min( + depth * inverseDt, + Vector.Min(depth * positionErrorToVelocity, maximumRecoveryVelocity)); + + ComputeCorrectiveImpulse(wsvA, wsvB, normal, angularA, angularB, biasVelocity, softnessImpulseScale, effectiveMass, ref accumulatedImpulse, out var correctiveCSI); + ApplyImpulse(inertiaA, inertiaB, normal, angularA, angularB, correctiveCSI, ref wsvA, ref wsvB); + } } } diff --git a/BepuPhysics/Constraints/Contact/PenetrationLimitOneBody.cs b/BepuPhysics/Constraints/Contact/PenetrationLimitOneBody.cs index 8fe1aa600..883595026 100644 --- a/BepuPhysics/Constraints/Contact/PenetrationLimitOneBody.cs +++ b/BepuPhysics/Constraints/Contact/PenetrationLimitOneBody.cs @@ -64,28 +64,26 @@ public static void WarmStart( [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ComputeCorrectiveImpulse(in BodyVelocityWide wsvA, - in PenetrationLimitOneBodyProjection projection, - in Vector3Wide normal, in Vector softnessImpulseScale, + in Vector3Wide normal, in Vector3Wide angularA, in Vector biasVelocity, in Vector softnessImpulseScale, in Vector effectiveMass, ref Vector accumulatedImpulse, out Vector correctiveCSI) { //Note that we do NOT use pretransformed jacobians here; the linear jacobian sharing (normal) meant that we had the effective mass anyway. Vector3Wide.Dot(wsvA.Linear, normal, out var csvaLinear); - Vector3Wide.Dot(wsvA.Angular, projection.AngularA, out var csvaAngular); + Vector3Wide.Dot(wsvA.Angular, angularA, out var csvaAngular); //Compute negated version to avoid the need for an explicit negate. - var negatedCSI = accumulatedImpulse * softnessImpulseScale + (csvaLinear + csvaAngular - projection.BiasVelocity) * projection.EffectiveMass; + var negatedCSI = accumulatedImpulse * softnessImpulseScale + (csvaLinear + csvaAngular - biasVelocity) * effectiveMass; var previousAccumulated = accumulatedImpulse; accumulatedImpulse = Vector.Max(Vector.Zero, accumulatedImpulse - negatedCSI); correctiveCSI = accumulatedImpulse - previousAccumulated; - } [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Solve(in PenetrationLimitOneBodyProjection projection, in BodyInertiaWide inertiaA, in Vector3Wide normal, in Vector softnessImpulseScale, ref Vector accumulatedImpulse, ref BodyVelocityWide wsvA) { - ComputeCorrectiveImpulse(wsvA, projection, normal, softnessImpulseScale, ref accumulatedImpulse, out var correctiveCSI); + ComputeCorrectiveImpulse(wsvA, normal, projection.AngularA, projection.BiasVelocity, softnessImpulseScale, projection.EffectiveMass, ref accumulatedImpulse, out var correctiveCSI); ApplyImpulse(projection, inertiaA, normal, correctiveCSI, ref wsvA); } @@ -121,5 +119,58 @@ public static void WarmStart2(in BodyInertiaWide inertiaA, in Vector3Wide normal ApplyImpulse(inertiaA, normal, angularA, accumulatedImpulse, ref wsvA); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Solve2( + in BodyInertiaWide inertiaA, in Vector3Wide normal, in Vector3Wide contactOffsetA, + in Vector depth, in Vector positionErrorToVelocity, in Vector effectiveMassCFMScale, in Vector maximumRecoveryVelocity, in Vector inverseDt, in Vector softnessImpulseScale, + ref Vector accumulatedImpulse, ref BodyVelocityWide wsvA) + { + //The contact penetration constraint takes the form: + //dot(positionA + offsetA, N) >= dot(positionB + offsetB, N) + //Or: + //dot(positionA + offsetA, N) - dot(positionB + offsetB, N) >= 0 + //dot(positionA + offsetA - positionB - offsetB, N) >= 0 + //where positionA and positionB are the center of mass positions of the bodies offsetA and offsetB are world space offsets from the center of mass to the contact, + //and N is a unit length vector calibrated to point from B to A. (The normal pointing direction is important; it changes the sign.) + //In practice, we'll use the collision detection system's penetration depth instead of trying to recompute the error here. + + //So, treating the normal as constant, the velocity constraint is: + //dot(d/dt(positionA + offsetA - positionB - offsetB), N) >= 0 + //dot(linearVelocityA + d/dt(offsetA) - linearVelocityB - d/dt(offsetB)), N) >= 0 + //The velocity of the offsets are defined by the angular velocity. + //dot(linearVelocityA + angularVelocityA x offsetA - linearVelocityB - angularVelocityB x offsetB), N) >= 0 + //dot(linearVelocityA, N) + dot(angularVelocityA x offsetA, N) - dot(linearVelocityB, N) - dot(angularVelocityB x offsetB), N) >= 0 + //Use the properties of the scalar triple product: + //dot(linearVelocityA, N) + dot(offsetA x N, angularVelocityA) - dot(linearVelocityB, N) - dot(offsetB x N, angularVelocityB) >= 0 + //Bake in the negations: + //dot(linearVelocityA, N) + dot(offsetA x N, angularVelocityA) + dot(linearVelocityB, -N) + dot(-offsetB x N, angularVelocityB) >= 0 + //A x B = -B x A: + //dot(linearVelocityA, N) + dot(offsetA x N, angularVelocityA) + dot(linearVelocityB, -N) + dot(N x offsetB, angularVelocityB) >= 0 + //And there you go, the jacobians! + //linearA: N + //angularA: offsetA x N + //linearB: -N + //angularB: N x offsetB + //Note that we leave the penetration depth as is, even when it's negative. Speculative contacts! + Vector3Wide.CrossWithoutOverlap(contactOffsetA, normal, out var angularA); + + //effective mass + Symmetric3x3Wide.VectorSandwich(angularA, inertiaA.InverseInertiaTensor, out var angularA0); + + //Linear effective mass contribution notes: + //1) The J * M^-1 * JT can be reordered to J * JT * M^-1 for the linear components, since M^-1 is a scalar and dot(n * scalar, n) = dot(n, n) * scalar. + //2) dot(normal, normal) == 1, so the contribution from each body is just its inverse mass. + //Note that we don't precompute the JT * effectiveMass term. Since the jacobians are shared, we have to do that multiply anyway. + var effectiveMass = effectiveMassCFMScale / (inertiaA.InverseMass + angularA0); + + //If depth is negative, the bias velocity will permit motion up until the depth hits zero. This works because positionErrorToVelocity * dt will always be <=1. + var biasVelocity = Vector.Min( + depth * inverseDt, + Vector.Min(depth * positionErrorToVelocity, maximumRecoveryVelocity)); + + ComputeCorrectiveImpulse(wsvA, normal, angularA, biasVelocity, softnessImpulseScale, effectiveMass, ref accumulatedImpulse, out var correctiveCSI); + ApplyImpulse(inertiaA, normal, angularA, correctiveCSI, ref wsvA); + } } } diff --git a/BepuPhysics/Constraints/Contact/TangentFriction.cs b/BepuPhysics/Constraints/Contact/TangentFriction.cs index 3efc468ca..57ec56654 100644 --- a/BepuPhysics/Constraints/Contact/TangentFriction.cs +++ b/BepuPhysics/Constraints/Contact/TangentFriction.cs @@ -119,8 +119,8 @@ public static void WarmStart(ref Vector3Wide tangentX, ref Vector3Wide tangentY, } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ComputeCorrectiveImpulse(ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref TangentFriction.Projection data, ref Jacobians jacobians, - ref Vector maximumImpulse, ref Vector2Wide accumulatedImpulse, out Vector2Wide correctiveCSI) + public static void ComputeCorrectiveImpulse(in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, in Symmetric2x2Wide effectiveMass, in Jacobians jacobians, + in Vector maximumImpulse, ref Vector2Wide accumulatedImpulse, out Vector2Wide correctiveCSI) { Matrix2x3Wide.TransformByTransposeWithoutOverlap(wsvA.Linear, jacobians.LinearA, out var csvaLinear); Matrix2x3Wide.TransformByTransposeWithoutOverlap(wsvA.Angular, jacobians.AngularA, out var csvaAngular); @@ -134,7 +134,7 @@ public static void ComputeCorrectiveImpulse(ref BodyVelocityWide wsvA, ref BodyV Vector2Wide.Add(csvaAngular, csvbAngular, out var csvAngular); Vector2Wide.Subtract(csvLinear, csvAngular, out var csv); - Symmetric2x2Wide.TransformWithoutOverlap(csv, data.EffectiveMass, out var csi); + Symmetric2x2Wide.TransformWithoutOverlap(csv, effectiveMass, out var csi); var previousAccumulated = accumulatedImpulse; Vector2Wide.Add(accumulatedImpulse, csi, out accumulatedImpulse); @@ -152,7 +152,7 @@ public static void ComputeCorrectiveImpulse(ref BodyVelocityWide wsvA, ref BodyV public static void Solve(ref Vector3Wide tangentX, ref Vector3Wide tangentY, ref TangentFriction.Projection projection, ref BodyInertiaWide inertiaA, ref BodyInertiaWide inertiaB, ref Vector maximumImpulse, ref Vector2Wide accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeJacobians(tangentX, tangentY, projection.OffsetA, projection.OffsetB, out var jacobians); - ComputeCorrectiveImpulse(ref wsvA, ref wsvB, ref projection, ref jacobians, ref maximumImpulse, ref accumulatedImpulse, out var correctiveCSI); + ComputeCorrectiveImpulse(wsvA, wsvB, projection.EffectiveMass, jacobians, maximumImpulse, ref accumulatedImpulse, out var correctiveCSI); ApplyImpulse(jacobians, inertiaA, inertiaB, correctiveCSI, ref wsvA, ref wsvB); } @@ -170,13 +170,25 @@ public static void WarmStart2(in Vector3Wide tangentX, in Vector3Wide tangentY, [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Solve2(in Vector3Wide tangentX, in Vector3Wide tangentY, in Vector3Wide offsetToManifoldCenterA, in Vector3Wide offsetToManifoldCenterB, ref BodyInertiaWide inertiaA, ref BodyInertiaWide inertiaB, - in Vector2Wide accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void Solve2(in Vector3Wide tangentX, in Vector3Wide tangentY, in Vector3Wide offsetToManifoldCenterA, in Vector3Wide offsetToManifoldCenterB, in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, + in Vector maximumImpulse, ref Vector2Wide accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeJacobians(tangentX, tangentY, offsetToManifoldCenterA, offsetToManifoldCenterB, out var jacobians); - //TODO: If the previous frame and current frame are associated with different time steps, the previous frame's solution won't be a good solution anymore. - //To compensate for this, the accumulated impulse should be scaled if dt changes. - ApplyImpulse(jacobians, inertiaA, inertiaB, accumulatedImpulse, ref wsvA, ref wsvB); + //Compute effective mass matrix contributions. + Symmetric2x2Wide.SandwichScale(jacobians.LinearA, inertiaA.InverseMass, out var linearContributionA); + Symmetric2x2Wide.SandwichScale(jacobians.LinearA, inertiaB.InverseMass, out var linearContributionB); + + Symmetric3x3Wide.MatrixSandwich(jacobians.AngularA, inertiaA.InverseInertiaTensor, out var angularContributionA); + Symmetric3x3Wide.MatrixSandwich(jacobians.AngularB, inertiaB.InverseInertiaTensor, out var angularContributionB); + + //No softening; this constraint is rigid by design. (It does support a maximum force, but that is distinct from a proper damping ratio/natural frequency.) + Symmetric2x2Wide.Add(linearContributionA, linearContributionB, out var linear); + Symmetric2x2Wide.Add(angularContributionA, angularContributionB, out var angular); + Symmetric2x2Wide.Add(linear, angular, out var inverseEffectiveMass); + Symmetric2x2Wide.InvertWithoutOverlap(inverseEffectiveMass, out var effectiveMass); + + ComputeCorrectiveImpulse(wsvA, wsvB, effectiveMass, jacobians, maximumImpulse, ref accumulatedImpulse, out var correctiveCSI); + ApplyImpulse(jacobians, inertiaA, inertiaB, correctiveCSI, ref wsvA, ref wsvB); } } } diff --git a/BepuPhysics/Constraints/Contact/TangentFrictionOneBody.cs b/BepuPhysics/Constraints/Contact/TangentFrictionOneBody.cs index 00ea22916..4178f8d55 100644 --- a/BepuPhysics/Constraints/Contact/TangentFrictionOneBody.cs +++ b/BepuPhysics/Constraints/Contact/TangentFrictionOneBody.cs @@ -78,14 +78,14 @@ public static void WarmStart(ref Vector3Wide tangentX, ref Vector3Wide tangentY, } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ComputeCorrectiveImpulse(ref BodyVelocityWide wsvA, ref Projection data, ref Jacobians jacobians, - ref Vector maximumImpulse, ref Vector2Wide accumulatedImpulse, out Vector2Wide correctiveCSI) + public static void ComputeCorrectiveImpulse(in BodyVelocityWide wsvA, in Symmetric2x2Wide effectiveMass, in Jacobians jacobians, + in Vector maximumImpulse, ref Vector2Wide accumulatedImpulse, out Vector2Wide correctiveCSI) { Matrix2x3Wide.TransformByTransposeWithoutOverlap(wsvA.Linear, jacobians.LinearA, out var csvaLinear); Matrix2x3Wide.TransformByTransposeWithoutOverlap(wsvA.Angular, jacobians.AngularA, out var csvaAngular); Vector2Wide.Add(csvaLinear, csvaAngular, out var csv); //Required corrective velocity is the negation of the current constraint space velocity. - Symmetric2x2Wide.TransformWithoutOverlap(csv, data.EffectiveMass, out var negativeCSI); + Symmetric2x2Wide.TransformWithoutOverlap(csv, effectiveMass, out var negativeCSI); var previousAccumulated = accumulatedImpulse; Vector2Wide.Subtract(accumulatedImpulse, negativeCSI, out accumulatedImpulse); @@ -104,7 +104,7 @@ public static void Solve(ref Vector3Wide tangentX, ref Vector3Wide tangentY, ref Projection projection, ref BodyInertiaWide inertiaA, ref Vector maximumImpulse, ref Vector2Wide accumulatedImpulse, ref BodyVelocityWide wsvA) { ComputeJacobians(tangentX, tangentY, projection.OffsetA, out var jacobians); - ComputeCorrectiveImpulse(ref wsvA, ref projection, ref jacobians, ref maximumImpulse, ref accumulatedImpulse, out var correctiveCSI); + ComputeCorrectiveImpulse(wsvA, projection.EffectiveMass, jacobians, maximumImpulse, ref accumulatedImpulse, out var correctiveCSI); ApplyImpulse(jacobians, inertiaA, correctiveCSI, ref wsvA); } @@ -119,6 +119,23 @@ public static void WarmStart2(in Vector3Wide tangentX, in Vector3Wide tangentY, ApplyImpulse(jacobians, inertiaA, accumulatedImpulse, ref wsvA); } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Solve2(in Vector3Wide tangentX, in Vector3Wide tangentY, in Vector3Wide offsetToManifoldCenterA, in BodyInertiaWide inertiaA, + in Vector maximumImpulse, ref Vector2Wide accumulatedImpulse, ref BodyVelocityWide wsvA) + { + ComputeJacobians(tangentX, tangentY, offsetToManifoldCenterA, out var jacobians); + //Compute effective mass matrix contributions. + Symmetric2x2Wide.SandwichScale(jacobians.LinearA, inertiaA.InverseMass, out var linearContributionA); + + Symmetric3x3Wide.MatrixSandwich(jacobians.AngularA, inertiaA.InverseInertiaTensor, out var angularContributionA); + + //No softening; this constraint is rigid by design. (It does support a maximum force, but that is distinct from a proper damping ratio/natural frequency.) + Symmetric2x2Wide.Add(linearContributionA, angularContributionA, out var inverseEffectiveMass); + Symmetric2x2Wide.InvertWithoutOverlap(inverseEffectiveMass, out var effectiveMass); + + ComputeCorrectiveImpulse(wsvA, effectiveMass, jacobians, maximumImpulse, ref accumulatedImpulse, out var correctiveCSI); + ApplyImpulse(jacobians, inertiaA, correctiveCSI, ref wsvA); + } } } diff --git a/BepuPhysics/Constraints/Contact/TwistFriction.cs b/BepuPhysics/Constraints/Contact/TwistFriction.cs index 8eaf8866a..e39f180a2 100644 --- a/BepuPhysics/Constraints/Contact/TwistFriction.cs +++ b/BepuPhysics/Constraints/Contact/TwistFriction.cs @@ -62,14 +62,14 @@ public static void WarmStart(ref Vector3Wide angularJacobianA, ref BodyInertiaWi } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ComputeCorrectiveImpulse(ref Vector3Wide angularJacobianA, ref TwistFrictionProjection projection, - ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref Vector maximumImpulse, + public static void ComputeCorrectiveImpulse(in Vector3Wide angularJacobianA, in Vector effectiveMass, + in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, in Vector maximumImpulse, ref Vector accumulatedImpulse, out Vector correctiveCSI) { Vector3Wide.Dot(wsvA.Angular, angularJacobianA, out var csvA); Vector3Wide.Dot(wsvB.Angular, angularJacobianA, out var negatedCSVB); - var negatedCSI = (csvA - negatedCSVB) * projection.EffectiveMass; //Since there is no bias or softness to give us the negative, we just do it when we apply to the accumulated impulse. - + var negatedCSI = (csvA - negatedCSVB) * effectiveMass; //Since there is no bias or softness to give us the negative, we just do it when we apply to the accumulated impulse. + var previousAccumulated = accumulatedImpulse; accumulatedImpulse = Vector.Min(maximumImpulse, Vector.Max(-maximumImpulse, accumulatedImpulse - negatedCSI)); @@ -81,7 +81,7 @@ public static void ComputeCorrectiveImpulse(ref Vector3Wide angularJacobianA, re public static void Solve(ref Vector3Wide angularJacobianA, ref BodyInertiaWide inertiaA, ref BodyInertiaWide inertiaB, ref TwistFrictionProjection projection, ref Vector maximumImpulse, ref Vector accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - ComputeCorrectiveImpulse(ref angularJacobianA, ref projection, ref wsvA, ref wsvB, ref maximumImpulse, ref accumulatedImpulse, out var correctiveCSI); + ComputeCorrectiveImpulse(angularJacobianA, projection.EffectiveMass, wsvA, wsvB, maximumImpulse, ref accumulatedImpulse, out var correctiveCSI); ApplyImpulse(angularJacobianA, inertiaA, inertiaB, correctiveCSI, ref wsvA, ref wsvB); } @@ -93,5 +93,29 @@ public static void WarmStart2(in Vector3Wide angularJacobianA, in BodyInertiaWid ApplyImpulse(angularJacobianA, inertiaA, inertiaB, accumulatedImpulse, ref wsvA, ref wsvB); } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Solve2(in Vector3Wide angularJacobianA, in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, + in Vector maximumImpulse, ref Vector accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + //Compute effective mass matrix contributions. No linear contributions for the twist constraint. + //Note that we use the angularJacobianA (that is, the normal) for both, despite angularJacobianB = -angularJacobianA. That's fine- J * M * JT is going to be positive regardless. + Symmetric3x3Wide.VectorSandwich(angularJacobianA, inertiaA.InverseInertiaTensor, out var angularA); + Symmetric3x3Wide.VectorSandwich(angularJacobianA, inertiaB.InverseInertiaTensor, out var angularB); + + //No softening; this constraint is rigid by design. (It does support a maximum force, but that is distinct from a proper damping ratio/natural frequency.) + //Note that we have to guard against two bodies with infinite inertias. This is a valid state! + //(We do not have to do such guarding on constraints with linear jacobians; dynamic bodies cannot have zero *mass*.) + //(Also note that there's no need for epsilons here... users shouldn't be setting their inertias to the absurd values it would take to cause a problem. + //Invalid conditions can't arise dynamically.) + var inverseEffectiveMass = angularA + angularB; + var inverseIsZero = Vector.Equals(Vector.Zero, inverseEffectiveMass); + var effectiveMass = Vector.ConditionalSelect(inverseIsZero, Vector.Zero, Vector.One / inverseEffectiveMass); + + //Note that friction constraints have no bias velocity. They target zero velocity. + ComputeCorrectiveImpulse(angularJacobianA, effectiveMass, wsvA, wsvB, maximumImpulse, ref accumulatedImpulse, out var correctiveCSI); + ApplyImpulse(angularJacobianA, inertiaA, inertiaB, correctiveCSI, ref wsvA, ref wsvB); + + } + } } diff --git a/BepuPhysics/Constraints/Contact/TwistFrictionOneBody.cs b/BepuPhysics/Constraints/Contact/TwistFrictionOneBody.cs index 02b307916..b35322c30 100644 --- a/BepuPhysics/Constraints/Contact/TwistFrictionOneBody.cs +++ b/BepuPhysics/Constraints/Contact/TwistFrictionOneBody.cs @@ -52,12 +52,12 @@ public static void WarmStart(ref Vector3Wide angularJacobianA, ref BodyInertiaWi } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ComputeCorrectiveImpulse(ref Vector3Wide angularJacobianA, ref TwistFrictionProjection projection, - ref BodyVelocityWide wsvA, ref Vector maximumImpulse, + public static void ComputeCorrectiveImpulse(in Vector3Wide angularJacobianA, in Vector effectiveMass, + in BodyVelocityWide wsvA, in Vector maximumImpulse, ref Vector accumulatedImpulse, out Vector correctiveCSI) { Vector3Wide.Dot(wsvA.Angular, angularJacobianA, out var csvA); - var negativeCSI = csvA * projection.EffectiveMass; //Since there is no bias or softness to give us the negative, we just do it when we apply to the accumulated impulse. + var negativeCSI = csvA * effectiveMass; //Since there is no bias or softness to give us the negative, we just do it when we apply to the accumulated impulse. var previousAccumulated = accumulatedImpulse; //The maximum force of friction depends upon the normal impulse. @@ -71,7 +71,7 @@ public static void ComputeCorrectiveImpulse(ref Vector3Wide angularJacobianA, re public static void Solve(ref Vector3Wide angularJacobianA, ref BodyInertiaWide inertiaA, ref TwistFrictionProjection projection, ref Vector maximumImpulse, ref Vector accumulatedImpulse, ref BodyVelocityWide wsvA) { - ComputeCorrectiveImpulse(ref angularJacobianA, ref projection, ref wsvA, ref maximumImpulse, ref accumulatedImpulse, out var correctiveCSI); + ComputeCorrectiveImpulse(angularJacobianA, projection.EffectiveMass, wsvA, maximumImpulse, ref accumulatedImpulse, out var correctiveCSI); ApplyImpulse(angularJacobianA, inertiaA, correctiveCSI, ref wsvA); } @@ -82,5 +82,26 @@ public static void WarmStart2(in Vector3Wide angularJacobianA, in BodyInertiaWid { ApplyImpulse(angularJacobianA, inertiaA, accumulatedImpulse, ref wsvA); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Solve2(in Vector3Wide angularJacobianA, in BodyInertiaWide inertiaA, in Vector maximumImpulse, ref Vector accumulatedImpulse, ref BodyVelocityWide wsvA) + { + //Compute effective mass matrix contributions. No linear contributions for the twist constraint. + //Note that we use the angularJacobianA (that is, the normal) for both, despite angularJacobianB = -angularJacobianA. That's fine- J * M * JT is going to be positive regardless. + Symmetric3x3Wide.VectorSandwich(angularJacobianA, inertiaA.InverseInertiaTensor, out var angularA); + + //No softening; this constraint is rigid by design. (It does support a maximum force, but that is distinct from a proper damping ratio/natural frequency.) + //Note that we have to guard against two bodies with infinite inertias. This is a valid state! + //(We do not have to do such guarding on constraints with linear jacobians; dynamic bodies cannot have zero *mass*.) + //(Also note that there's no need for epsilons here... users shouldn't be setting their inertias to the absurd values it would take to cause a problem. + //Invalid conditions can't arise dynamically.) + var inverseIsZero = Vector.Equals(Vector.Zero, angularA); + var effectiveMass = Vector.ConditionalSelect(inverseIsZero, Vector.Zero, Vector.One / angularA); + + //Note that friction constraints have no bias velocity. They target zero velocity. + ComputeCorrectiveImpulse(angularJacobianA, effectiveMass, wsvA, maximumImpulse, ref accumulatedImpulse, out var correctiveCSI); + ApplyImpulse(angularJacobianA, inertiaA, correctiveCSI, ref wsvA); + + } } } From cf6b67e0b9f2d5b6a27df04a44ba18de88d55ef8 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 23 Jul 2021 20:25:22 -0500 Subject: [PATCH 137/947] One body warmstart2/solve2 now have proper body references type. --- BepuPhysics/Bodies_GatherScatter.cs | 13 ++++++------- BepuPhysics/Constraints/OneBodyTypeProcessor.cs | 17 ++++++++--------- 2 files changed, 14 insertions(+), 16 deletions(-) diff --git a/BepuPhysics/Bodies_GatherScatter.cs b/BepuPhysics/Bodies_GatherScatter.cs index 4087c5b07..2e8a12728 100644 --- a/BepuPhysics/Bodies_GatherScatter.cs +++ b/BepuPhysics/Bodies_GatherScatter.cs @@ -786,13 +786,12 @@ public unsafe void ScatterVelocities(ref BodyVelocityWide sourceV { //Just like the gather but... transposed, we transpose from the wide representation into per-body velocities. //The motion state struct puts the velocities into the second 32 byte chunk, so we can do a single write per body. - Vector256 m0, m1, m2, m4, m5, m6; - m0 = sourceVelocities.Linear.X.AsVector256(); - m1 = sourceVelocities.Linear.Y.AsVector256(); - m2 = sourceVelocities.Linear.Z.AsVector256(); - m4 = sourceVelocities.Angular.X.AsVector256(); - m5 = sourceVelocities.Angular.Y.AsVector256(); - m6 = sourceVelocities.Angular.Z.AsVector256(); + var m0 = sourceVelocities.Linear.X.AsVector256(); + var m1 = sourceVelocities.Linear.Y.AsVector256(); + var m2 = sourceVelocities.Linear.Z.AsVector256(); + var m4 = sourceVelocities.Angular.X.AsVector256(); + var m5 = sourceVelocities.Angular.Y.AsVector256(); + var m6 = sourceVelocities.Angular.Z.AsVector256(); //We're being a bit lazy here- you could reduce the instructions a bit more given the two empty source lanes. var n0 = Avx.UnpackLow(m0, m1); diff --git a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs index c12dd017c..05b0e699a 100644 --- a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs @@ -261,7 +261,7 @@ public unsafe override void WarmStart2 integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { var prestepBundles = typeBatch.PrestepData.As(); - var bodyReferencesBundles = typeBatch.BodyReferences.As(); + var bodyReferencesBundles = typeBatch.BodyReferences.As>(); var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); var function = default(TConstraintFunctions); ref var states = ref bodies.ActiveSet.SolverStates; @@ -271,29 +271,28 @@ public unsafe override void WarmStart2(bodies, ref integratorCallbacks, ref integrationFlags, 0, dt, workerIndex, i, ref references.IndexA, count, + GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 0, dt, workerIndex, i, ref references, count, out var positionA, out var orientationA, out var wsvA, out var inertiaA); - +; function.WarmStart2(positionA, orientationA, inertiaA, prestep, accumulatedImpulses, ref wsvA); if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) { - bodies.ScatterVelocities(ref wsvA, ref references.IndexA, count); + bodies.ScatterVelocities(ref wsvA, ref references, count); } else { //This batch has some integrators, which means that every bundle is going to gather all velocities. //(We don't make per-bundle determinations about this to avoid an extra branch and instruction complexity, and the difference is very small.) - bodies.ScatterVelocities(ref wsvA, ref references.IndexA, count); + bodies.ScatterVelocities(ref wsvA, ref references, count); } - } } public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { var prestepBundles = typeBatch.PrestepData.As(); - var bodyReferencesBundles = typeBatch.BodyReferences.As(); + var bodyReferencesBundles = typeBatch.BodyReferences.As>(); var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); var function = default(TConstraintFunctions); ref var motionStates = ref bodies.ActiveSet.SolverStates; @@ -303,11 +302,11 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; ref var references = ref bodyReferencesBundles[i]; var count = GetCountInBundle(ref typeBatch, i); - bodies.GatherState(ref references.IndexA, count, true, out var positionA, out var orientationA, out var wsvA, out var inertiaA); + bodies.GatherState(ref references, count, true, out var positionA, out var orientationA, out var wsvA, out var inertiaA); function.Solve2(positionA, orientationA, inertiaA, dt, inverseDt, prestep, ref accumulatedImpulses, ref wsvA); - bodies.ScatterVelocities(ref wsvA, ref references.IndexA, count); + bodies.ScatterVelocities(ref wsvA, ref references, count); } } From 200ef61aabca6eac674fb620c87619534dbfe0c7 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 24 Jul 2021 15:11:34 -0500 Subject: [PATCH 138/947] Tightened access restrictions on type processors. --- BepuPhysics/Constraints/IBodyAccessFilter.cs | 10 ++++++++++ BepuPhysics/Constraints/OneBodyTypeProcessor.cs | 2 +- BepuPhysics/Constraints/TwoBodyTypeProcessor.cs | 2 +- Demos/DemoCallbacks.cs | 3 +++ Demos/Demos/NewtDemo.cs | 2 +- Demos/Program.cs | 2 +- 6 files changed, 17 insertions(+), 4 deletions(-) diff --git a/BepuPhysics/Constraints/IBodyAccessFilter.cs b/BepuPhysics/Constraints/IBodyAccessFilter.cs index 0ed33da11..f2a35589a 100644 --- a/BepuPhysics/Constraints/IBodyAccessFilter.cs +++ b/BepuPhysics/Constraints/IBodyAccessFilter.cs @@ -45,6 +45,16 @@ public struct AccessAll : IBodyAccessFilter public bool AccessAngularVelocity => true; } + public struct AccessNoPoses : IBodyAccessFilter + { + public bool GatherPosition => false; + public bool GatherOrientation => false; + public bool GatherMass => true; + public bool GatherInertiaTensor => true; + public bool AccessLinearVelocity => true; + public bool AccessAngularVelocity => true; + } + public struct AccessOnlyAngular : IBodyAccessFilter { public bool GatherPosition => false; diff --git a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs index 05b0e699a..79ee5eba7 100644 --- a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs @@ -338,7 +338,7 @@ public override void JacobiPrestep2(ref TypeBatch typeBatch, Bodies bodies, ref } public abstract class OneBodyContactTypeProcessor - : OneBodyTypeProcessor + : OneBodyTypeProcessor where TPrestepData : unmanaged where TProjection : unmanaged where TAccumulatedImpulse : unmanaged where TConstraintFunctions : unmanaged, IOneBodyContactConstraintFunctions { diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index c055df4f4..3d65abe7d 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -456,7 +456,7 @@ public override void JacobiPrestep2(ref TypeBatch typeBatch, Bodies bodies, ref } public abstract class TwoBodyContactTypeProcessor - : TwoBodyTypeProcessor + : TwoBodyTypeProcessor where TPrestepData : unmanaged where TProjection : unmanaged where TAccumulatedImpulse : unmanaged where TConstraintFunctions : unmanaged, IContactConstraintFunctions { diff --git a/Demos/DemoCallbacks.cs b/Demos/DemoCallbacks.cs index 0d744b440..73cf1e5ea 100644 --- a/Demos/DemoCallbacks.cs +++ b/Demos/DemoCallbacks.cs @@ -67,7 +67,9 @@ public void PrepareForIntegration(float dt) //Since these callbacks don't use per-body damping values, we can precalculate everything. linearDampingDt = MathF.Pow(MathHelper.Clamp(1 - LinearDamping, 0, 1), dt); angularDampingDt = MathF.Pow(MathHelper.Clamp(1 - AngularDamping, 0, 1), dt); + gravityWide = Vector3Wide.Broadcast(Gravity); } + Vector3Wide gravityWide; [MethodImpl(MethodImplOptions.AggressiveInlining)] public void IntegrateVelocity(int bodyIndex, in RigidPose pose, in BodyInertia localInertia, int workerIndex, ref BodyVelocity velocity) { @@ -93,6 +95,7 @@ public void IntegrateVelocity(int bodyIndex, in RigidPose pose, in BodyInertia l [MethodImpl(MethodImplOptions.AggressiveInlining)] public void IntegrateVelocity(ReadOnlySpan bodyIndices, in Vector3Wide position, in QuaternionWide orientation, in BodyInertiaWide localInertia, in Vector integrationMask, int workerIndex, in Vector dt, ref BodyVelocityWide velocity) { + velocity.Linear += gravityWide * dt; } } public unsafe struct DemoNarrowPhaseCallbacks : INarrowPhaseCallbacks diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index c02967ff6..2bcec515d 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -823,7 +823,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper(1)); - Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)), new EmbeddedSubsteppingTimestepper2(3), solverIterationCount: 1); + Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), solverIterationCount: 1); //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)), new PositionFirstTimestepper2(), solverIterationCount: 5); //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)), new SubsteppingTimestepper2(3), solverIterationCount: 1); diff --git a/Demos/Program.cs b/Demos/Program.cs index 962deb171..6c769ece5 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -19,7 +19,7 @@ static void Main(string[] args) { content = ContentArchive.Load(stream); } - HeadlessTest.Test(content, 16, 32, 512); + HeadlessTest.Test(content, 4, 32, 512); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); From a284a470afe74ff3c01a7406370e8664bd1a82a8 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 24 Jul 2021 17:03:37 -0500 Subject: [PATCH 139/947] OneBodyLinearServo solve bug fixed. --- BepuPhysics/Constraints/OneBodyLinearServo.cs | 2 +- BepuPhysics/Constraints/ServoSettings.cs | 12 ++++++------ BepuPhysics/Constraints/Weld.cs | 3 --- Demos/Program.cs | 2 +- 4 files changed, 8 insertions(+), 11 deletions(-) diff --git a/BepuPhysics/Constraints/OneBodyLinearServo.cs b/BepuPhysics/Constraints/OneBodyLinearServo.cs index 732e7745f..b6d4d53b2 100644 --- a/BepuPhysics/Constraints/OneBodyLinearServo.cs +++ b/BepuPhysics/Constraints/OneBodyLinearServo.cs @@ -213,7 +213,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in //The motor has a limited maximum force, so clamp the accumulated impulse. Watch out for division by zero. ServoSettingsWide.ClampImpulse(maximumImpulse, ref accumulatedImpulses, ref csi); - ApplyImpulse(offset, inertiaA, ref wsvA, accumulatedImpulses); + ApplyImpulse(offset, inertiaA, ref wsvA, csi); } } diff --git a/BepuPhysics/Constraints/ServoSettings.cs b/BepuPhysics/Constraints/ServoSettings.cs index 36137656a..f485298d5 100644 --- a/BepuPhysics/Constraints/ServoSettings.cs +++ b/BepuPhysics/Constraints/ServoSettings.cs @@ -49,12 +49,12 @@ public static void ComputeClampedBiasVelocity(in Vector error, in Vector< out Vector clampedBiasVelocity, out Vector maximumImpulse) { //Can't request speed that would cause an overshoot. - var baseSpeed = Vector.Min(servoSettings.BaseSpeed, Vector.Abs(error) * inverseDt); + var baseSpeed = Vector.Min(servoSettings.BaseSpeed, Vector.Abs(error) * new Vector(inverseDt)); var biasVelocity = error * positionErrorToVelocity; clampedBiasVelocity = Vector.ConditionalSelect(Vector.LessThan(biasVelocity, Vector.Zero), Vector.Max(-servoSettings.MaximumSpeed, Vector.Min(-baseSpeed, biasVelocity)), Vector.Min(servoSettings.MaximumSpeed, Vector.Max(baseSpeed, biasVelocity))); - maximumImpulse = servoSettings.MaximumForce * dt; + maximumImpulse = servoSettings.MaximumForce * new Vector(dt); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -62,7 +62,7 @@ public static void ComputeClampedBiasVelocity(in Vector2Wide errorAxis, in Vecto float dt, float inverseDt, out Vector2Wide clampedBiasVelocity, out Vector maximumImpulse) { //Can't request speed that would cause an overshoot. - var baseSpeed = Vector.Min(servoSettings.BaseSpeed, errorLength * inverseDt); + var baseSpeed = Vector.Min(servoSettings.BaseSpeed, errorLength * new Vector(inverseDt)); var unclampedBiasSpeed = errorLength * positionErrorToBiasVelocity; var targetSpeed = Vector.Max(baseSpeed, unclampedBiasSpeed); var scale = Vector.Min(Vector.One, servoSettings.MaximumSpeed / targetSpeed); @@ -70,7 +70,7 @@ public static void ComputeClampedBiasVelocity(in Vector2Wide errorAxis, in Vecto var useFallback = Vector.LessThan(targetSpeed, new Vector(1e-10f)); scale = Vector.ConditionalSelect(useFallback, Vector.One, scale); Vector2Wide.Scale(errorAxis, scale * unclampedBiasSpeed, out clampedBiasVelocity); - maximumImpulse = servoSettings.MaximumForce * dt; + maximumImpulse = servoSettings.MaximumForce * new Vector(dt); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -90,7 +90,7 @@ public static void ComputeClampedBiasVelocity(in Vector3Wide errorAxis, in Vecto float dt, float inverseDt, out Vector3Wide clampedBiasVelocity, out Vector maximumImpulse) { //Can't request speed that would cause an overshoot. - var baseSpeed = Vector.Min(servoSettings.BaseSpeed, errorLength * inverseDt); + var baseSpeed = Vector.Min(servoSettings.BaseSpeed, errorLength * new Vector(inverseDt)); var unclampedBiasSpeed = errorLength * positionErrorToBiasVelocity; var targetSpeed = Vector.Max(baseSpeed, unclampedBiasSpeed); var scale = Vector.Min(Vector.One, servoSettings.MaximumSpeed / targetSpeed); @@ -98,7 +98,7 @@ public static void ComputeClampedBiasVelocity(in Vector3Wide errorAxis, in Vecto var useFallback = Vector.LessThan(targetSpeed, new Vector(1e-10f)); scale = Vector.ConditionalSelect(useFallback, Vector.One, scale); Vector3Wide.Scale(errorAxis, scale * unclampedBiasSpeed, out clampedBiasVelocity); - maximumImpulse = servoSettings.MaximumForce * dt; + maximumImpulse = servoSettings.MaximumForce * new Vector(dt); } [MethodImpl(MethodImplOptions.AggressiveInlining)] diff --git a/BepuPhysics/Constraints/Weld.cs b/BepuPhysics/Constraints/Weld.cs index aa69e5376..0352e58f6 100644 --- a/BepuPhysics/Constraints/Weld.cs +++ b/BepuPhysics/Constraints/Weld.cs @@ -265,8 +265,6 @@ public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, { Transform(prestep.LocalOffset, orientationA, out var offset); ApplyImpulse(inertiaA, inertiaB, offset, accumulatedImpulses.Orientation, accumulatedImpulses.Offset, ref wsvA, ref wsvB); - //var offset = prestep.LocalOffset * orientationA; - //ApplyImpulse(inertiaA, inertiaB, prestep.LocalOffset * orientationA, accumulatedImpulses.Orientation, accumulatedImpulses.Offset, ref wsvA, ref wsvB); } //[MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, @@ -286,7 +284,6 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in //where I is the 3x3 identity matrix. //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. - //var offset = prestep.LocalOffset * orientationA; Transform(prestep.LocalOffset, orientationA, out var offset); //Effective mass = (J * M^-1 * JT)^-1, which is going to be a little tricky because J * M^-1 * JT is a 6x6 matrix: diff --git a/Demos/Program.cs b/Demos/Program.cs index 6c769ece5..7118826dd 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -19,7 +19,7 @@ static void Main(string[] args) { content = ContentArchive.Load(stream); } - HeadlessTest.Test(content, 4, 32, 512); + //HeadlessTest.Test(content, 4, 32, 512); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); From a1631f6896cda6b3f664a5d790a841beecb64e9d Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 24 Jul 2021 17:35:09 -0500 Subject: [PATCH 140/947] Pushing through incremental updating callback for constraints. --- .../Constraints/AngularAxisGearMotor.cs | 5 +++++ BepuPhysics/Constraints/AngularAxisMotor.cs | 5 +++++ BepuPhysics/Constraints/AngularHinge.cs | 5 +++++ BepuPhysics/Constraints/AngularMotor.cs | 5 +++++ BepuPhysics/Constraints/AngularServo.cs | 5 +++++ BepuPhysics/Constraints/AngularSwivelHinge.cs | 5 +++++ BepuPhysics/Constraints/AreaConstraint.cs | 9 +++++++++ BepuPhysics/Constraints/BallSocket.cs | 4 ++++ BepuPhysics/Constraints/BallSocketMotor.cs | 5 +++++ BepuPhysics/Constraints/BallSocketServo.cs | 5 +++++ .../Constraints/CenterDistanceConstraint.cs | 6 ++++++ BepuPhysics/Constraints/DistanceLimit.cs | 6 ++++++ BepuPhysics/Constraints/DistanceServo.cs | 5 +++++ .../Constraints/FourBodyTypeProcessor.cs | 20 ++++++++++++++----- BepuPhysics/Constraints/Hinge.cs | 5 +++++ .../Constraints/IBatchIntegrationMode.cs | 6 ++++-- BepuPhysics/Constraints/LinearAxisLimit.cs | 5 +++++ BepuPhysics/Constraints/LinearAxisMotor.cs | 5 +++++ BepuPhysics/Constraints/LinearAxisServo.cs | 5 +++++ .../Constraints/OneBodyAngularMotor.cs | 5 +++++ .../Constraints/OneBodyAngularServo.cs | 5 +++++ BepuPhysics/Constraints/OneBodyLinearMotor.cs | 5 +++++ BepuPhysics/Constraints/OneBodyLinearServo.cs | 5 +++++ .../Constraints/OneBodyTypeProcessor.cs | 9 ++++++++- BepuPhysics/Constraints/PointOnLineServo.cs | 5 +++++ BepuPhysics/Constraints/SwingLimit.cs | 5 +++++ BepuPhysics/Constraints/SwivelHinge.cs | 5 +++++ .../Constraints/ThreeBodyTypeProcessor.cs | 9 +++++++++ BepuPhysics/Constraints/TwistLimit.cs | 5 +++++ BepuPhysics/Constraints/TwistMotor.cs | 5 +++++ BepuPhysics/Constraints/TwistServo.cs | 5 +++++ .../Constraints/TwoBodyTypeProcessor.cs | 8 ++++++++ BepuPhysics/Constraints/TypeProcessor.cs | 4 ++-- BepuPhysics/Constraints/VolumeConstraint.cs | 10 ++++++++++ BepuPhysics/Constraints/Weld.cs | 4 ++++ BepuPhysics/Solver_SubsteppingSolve2.cs | 8 ++++---- 36 files changed, 204 insertions(+), 14 deletions(-) diff --git a/BepuPhysics/Constraints/AngularAxisGearMotor.cs b/BepuPhysics/Constraints/AngularAxisGearMotor.cs index cec00320f..9f809b0d5 100644 --- a/BepuPhysics/Constraints/AngularAxisGearMotor.cs +++ b/BepuPhysics/Constraints/AngularAxisGearMotor.cs @@ -161,6 +161,11 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in ServoSettingsWide.ClampImpulse(maximumImpulse, ref accumulatedImpulses, ref csi); ApplyImpulse(jA, axis, accumulatedImpulses, inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, ref wsvA.Angular, ref wsvB.Angular); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector accumulatedImpulses, ref AngularAxisGearMotorPrestepData prestep) + { + } } public class AngularAxisGearMotorTypeProcessor : TwoBodyTypeProcessor, AngularAxisGearMotorFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular> diff --git a/BepuPhysics/Constraints/AngularAxisMotor.cs b/BepuPhysics/Constraints/AngularAxisMotor.cs index 068ba21bf..68ebcbaa4 100644 --- a/BepuPhysics/Constraints/AngularAxisMotor.cs +++ b/BepuPhysics/Constraints/AngularAxisMotor.cs @@ -157,6 +157,11 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in ServoSettingsWide.ClampImpulse(maximumImpulse, ref accumulatedImpulses, ref csi); ApplyImpulse(jA, accumulatedImpulses, inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, ref wsvA.Angular, ref wsvB.Angular); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector accumulatedImpulses, ref AngularAxisMotorPrestepData prestep) + { + } } public class AngularAxisMotorTypeProcessor : TwoBodyTypeProcessor, AngularAxisMotorFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular> diff --git a/BepuPhysics/Constraints/AngularHinge.cs b/BepuPhysics/Constraints/AngularHinge.cs index c304edd46..c7b729d64 100644 --- a/BepuPhysics/Constraints/AngularHinge.cs +++ b/BepuPhysics/Constraints/AngularHinge.cs @@ -235,6 +235,11 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in { throw new NotImplementedException(); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector2Wide accumulatedImpulses, ref AngularHingePrestepData prestep) + { + } } public class AngularHingeTypeProcessor : TwoBodyTypeProcessor diff --git a/BepuPhysics/Constraints/AngularMotor.cs b/BepuPhysics/Constraints/AngularMotor.cs index a678951a6..af48e319e 100644 --- a/BepuPhysics/Constraints/AngularMotor.cs +++ b/BepuPhysics/Constraints/AngularMotor.cs @@ -110,6 +110,11 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in { throw new NotImplementedException(); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector3Wide accumulatedImpulses, ref AngularMotorPrestepData prestep) + { + } } public class AngularMotorTypeProcessor : TwoBodyTypeProcessor diff --git a/BepuPhysics/Constraints/AngularServo.cs b/BepuPhysics/Constraints/AngularServo.cs index 414beec8f..9ed82fca5 100644 --- a/BepuPhysics/Constraints/AngularServo.cs +++ b/BepuPhysics/Constraints/AngularServo.cs @@ -151,6 +151,11 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in { throw new NotImplementedException(); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector3Wide accumulatedImpulses, ref AngularServoPrestepData prestep) + { + } } public class AngularServoTypeProcessor : TwoBodyTypeProcessor diff --git a/BepuPhysics/Constraints/AngularSwivelHinge.cs b/BepuPhysics/Constraints/AngularSwivelHinge.cs index 5b802b9c2..c4d01b5c8 100644 --- a/BepuPhysics/Constraints/AngularSwivelHinge.cs +++ b/BepuPhysics/Constraints/AngularSwivelHinge.cs @@ -163,6 +163,11 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in { throw new NotImplementedException(); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector accumulatedImpulses, ref AngularSwivelHingePrestepData prestep) + { + } } public class AngularSwivelHingeTypeProcessor : TwoBodyTypeProcessor, AngularSwivelHingeFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular> diff --git a/BepuPhysics/Constraints/AreaConstraint.cs b/BepuPhysics/Constraints/AreaConstraint.cs index d02d89903..e95c9ef00 100644 --- a/BepuPhysics/Constraints/AreaConstraint.cs +++ b/BepuPhysics/Constraints/AreaConstraint.cs @@ -194,6 +194,15 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in { throw new NotImplementedException(); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void UpdateForNewPose( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, + in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in BodyVelocityWide wsvC, + in Vector accumulatedImpulses, ref AreaConstraintPrestepData prestep) + { + } } diff --git a/BepuPhysics/Constraints/BallSocket.cs b/BepuPhysics/Constraints/BallSocket.cs index 65f617f96..1b677b158 100644 --- a/BepuPhysics/Constraints/BallSocket.cs +++ b/BepuPhysics/Constraints/BallSocket.cs @@ -134,7 +134,11 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in Vector3Wide.Scale(error, positionErrorToVelocity, out var biasVelocity); BallSocketShared.Solve(ref wsvA, ref wsvB, offsetA, offsetB, biasVelocity, effectiveMass, softnessImpulseScale, ref accumulatedImpulses, inertiaA, inertiaB); + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector3Wide accumulatedImpulses, ref BallSocketPrestepData prestep) + { } } diff --git a/BepuPhysics/Constraints/BallSocketMotor.cs b/BepuPhysics/Constraints/BallSocketMotor.cs index 3cc9f76f6..613353639 100644 --- a/BepuPhysics/Constraints/BallSocketMotor.cs +++ b/BepuPhysics/Constraints/BallSocketMotor.cs @@ -120,6 +120,11 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in { throw new NotImplementedException(); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector3Wide accumulatedImpulses, ref BallSocketMotorPrestepData prestep) + { + } } diff --git a/BepuPhysics/Constraints/BallSocketServo.cs b/BepuPhysics/Constraints/BallSocketServo.cs index 7f25d38c9..77760aae3 100644 --- a/BepuPhysics/Constraints/BallSocketServo.cs +++ b/BepuPhysics/Constraints/BallSocketServo.cs @@ -127,6 +127,11 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in { throw new NotImplementedException(); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector3Wide accumulatedImpulses, ref BallSocketServoPrestepData prestep) + { + } } diff --git a/BepuPhysics/Constraints/CenterDistanceConstraint.cs b/BepuPhysics/Constraints/CenterDistanceConstraint.cs index c2941f3fa..7bde7a066 100644 --- a/BepuPhysics/Constraints/CenterDistanceConstraint.cs +++ b/BepuPhysics/Constraints/CenterDistanceConstraint.cs @@ -180,6 +180,12 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in ApplyImpulse(jacobianA, inertiaA.InverseMass, inertiaB.InverseMass, csi, ref wsvA, ref wsvB); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector accumulatedImpulses, ref CenterDistancePrestepData prestep) + { + } + } diff --git a/BepuPhysics/Constraints/DistanceLimit.cs b/BepuPhysics/Constraints/DistanceLimit.cs index 38d6b6149..057971133 100644 --- a/BepuPhysics/Constraints/DistanceLimit.cs +++ b/BepuPhysics/Constraints/DistanceLimit.cs @@ -168,6 +168,12 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in { throw new NotImplementedException(); } + + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector accumulatedImpulses, ref DistanceLimitPrestepData prestep) + { + } } diff --git a/BepuPhysics/Constraints/DistanceServo.cs b/BepuPhysics/Constraints/DistanceServo.cs index 718f85a75..e64432173 100644 --- a/BepuPhysics/Constraints/DistanceServo.cs +++ b/BepuPhysics/Constraints/DistanceServo.cs @@ -250,6 +250,11 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in { throw new NotImplementedException(); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector accumulatedImpulses, ref DistanceServoPrestepData prestep) + { + } } diff --git a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs index b8f37b86c..7ae9b118c 100644 --- a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs @@ -36,17 +36,24 @@ void Prestep( void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref BodyVelocityWide velocityC, ref BodyVelocityWide velocityD, ref TProjection projection, ref TAccumulatedImpulse accumulatedImpulse); void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref BodyVelocityWide velocityC, ref BodyVelocityWide velocityD, ref TProjection projection, ref TAccumulatedImpulse accumulatedImpulse); void WarmStart2( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, - in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, - in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, - in Vector3Wide positionD, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, - in TPrestepData prestep, in TAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC, ref BodyVelocityWide wsvD); + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, + in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, + in Vector3Wide positionD, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, + in TPrestepData prestep, in TAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC, ref BodyVelocityWide wsvD); void Solve2( in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in Vector3Wide positionD, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, float dt, float inverseDt, in TPrestepData prestep, ref TAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC, ref BodyVelocityWide wsvD); + + void UpdateForNewPose( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, + in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in BodyVelocityWide wsvC, + in Vector3Wide positionD, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, in BodyVelocityWide wsvD, + in TAccumulatedImpulse accumulatedImpulses, ref TPrestepData prestep); } /// @@ -372,6 +379,9 @@ public unsafe override void WarmStart2(bodies, ref integratorCallbacks, ref integrationFlags, 3, dt, workerIndex, i, ref references.IndexD, count, out var positionD, out var orientationD, out var wsvD, out var inertiaD); + if (typeof(TAllowPoseIntegration) == typeof(AllowPoseIntegration)) + function.UpdateForNewPose(positionA, orientationA, inertiaA, wsvA, positionB, orientationB, inertiaB, wsvB, positionC, orientationC, inertiaC, wsvC, positionD, orientationD, inertiaD, wsvD, accumulatedImpulses, ref prestep); + function.WarmStart2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, positionD, orientationD, inertiaD, prestep, accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC, ref wsvD); if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) diff --git a/BepuPhysics/Constraints/Hinge.cs b/BepuPhysics/Constraints/Hinge.cs index a048c526e..f052a042f 100644 --- a/BepuPhysics/Constraints/Hinge.cs +++ b/BepuPhysics/Constraints/Hinge.cs @@ -233,6 +233,11 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in { throw new NotImplementedException(); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in HingeAccumulatedImpulses accumulatedImpulses, ref HingePrestepData prestep) + { + } } public class HingeTypeProcessor : TwoBodyTypeProcessor diff --git a/BepuPhysics/Constraints/IBatchIntegrationMode.cs b/BepuPhysics/Constraints/IBatchIntegrationMode.cs index c06436ce0..7a20b6f25 100644 --- a/BepuPhysics/Constraints/IBatchIntegrationMode.cs +++ b/BepuPhysics/Constraints/IBatchIntegrationMode.cs @@ -44,14 +44,16 @@ public interface IBatchPoseIntegrationAllowed /// /// Marks a batch as integrating poses for any bodies with integration responsibility within the constraint batch. + /// Constraints which need to be updated in response to pose integration will also have their UpdateForNewPose function called. /// - public struct BatchShouldIntegratePoses : IBatchPoseIntegrationAllowed + public struct AllowPoseIntegration : IBatchPoseIntegrationAllowed { } + /// /// Marks a batch as not integrating poses for any bodies within the constraint batch. /// - public struct BatchShouldNotIntegratePoses : IBatchPoseIntegrationAllowed + public struct DisallowPoseIntegration : IBatchPoseIntegrationAllowed { } diff --git a/BepuPhysics/Constraints/LinearAxisLimit.cs b/BepuPhysics/Constraints/LinearAxisLimit.cs index 9b84f5179..12e15d814 100644 --- a/BepuPhysics/Constraints/LinearAxisLimit.cs +++ b/BepuPhysics/Constraints/LinearAxisLimit.cs @@ -158,6 +158,11 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in { throw new NotImplementedException(); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector accumulatedImpulses, ref LinearAxisLimitPrestepData prestep) + { + } } public class LinearAxisLimitTypeProcessor : TwoBodyTypeProcessor, LinearAxisLimitFunctions, AccessAll, AccessAll, AccessAll, AccessAll> diff --git a/BepuPhysics/Constraints/LinearAxisMotor.cs b/BepuPhysics/Constraints/LinearAxisMotor.cs index 4e989e655..6e01ba7b5 100644 --- a/BepuPhysics/Constraints/LinearAxisMotor.cs +++ b/BepuPhysics/Constraints/LinearAxisMotor.cs @@ -123,6 +123,11 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in { throw new NotImplementedException(); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector accumulatedImpulses, ref LinearAxisMotorPrestepData prestep) + { + } } public class LinearAxisMotorTypeProcessor : TwoBodyTypeProcessor, LinearAxisMotorFunctions, AccessAll, AccessAll, AccessAll, AccessAll> diff --git a/BepuPhysics/Constraints/LinearAxisServo.cs b/BepuPhysics/Constraints/LinearAxisServo.cs index 7793b796d..760cc2c0b 100644 --- a/BepuPhysics/Constraints/LinearAxisServo.cs +++ b/BepuPhysics/Constraints/LinearAxisServo.cs @@ -236,6 +236,11 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in { throw new NotImplementedException(); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector accumulatedImpulses, ref LinearAxisServoPrestepData prestep) + { + } } public class LinearAxisServoTypeProcessor : TwoBodyTypeProcessor, LinearAxisServoFunctions, AccessAll, AccessAll, AccessAll, AccessAll> diff --git a/BepuPhysics/Constraints/OneBodyAngularMotor.cs b/BepuPhysics/Constraints/OneBodyAngularMotor.cs index 0c749f5f8..6ab5f233f 100644 --- a/BepuPhysics/Constraints/OneBodyAngularMotor.cs +++ b/BepuPhysics/Constraints/OneBodyAngularMotor.cs @@ -119,6 +119,11 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in { throw new NotImplementedException(); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide accumulatedImpulses, ref OneBodyAngularMotorPrestepData prestep) + { + } } public class OneBodyAngularMotorTypeProcessor : OneBodyTypeProcessor diff --git a/BepuPhysics/Constraints/OneBodyAngularServo.cs b/BepuPhysics/Constraints/OneBodyAngularServo.cs index d60325d90..6c3dc3714 100644 --- a/BepuPhysics/Constraints/OneBodyAngularServo.cs +++ b/BepuPhysics/Constraints/OneBodyAngularServo.cs @@ -168,6 +168,11 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in ServoSettingsWide.ClampImpulse(maximumImpulse, ref accumulatedImpulses, ref csi); ApplyImpulse(inertiaA.InverseInertiaTensor, csi, ref wsvA.Angular); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide accumulatedImpulses, ref OneBodyAngularServoPrestepData prestep) + { + } } public class OneBodyAngularServoTypeProcessor : OneBodyTypeProcessor diff --git a/BepuPhysics/Constraints/OneBodyLinearMotor.cs b/BepuPhysics/Constraints/OneBodyLinearMotor.cs index 5d33d26cb..84a151f7f 100644 --- a/BepuPhysics/Constraints/OneBodyLinearMotor.cs +++ b/BepuPhysics/Constraints/OneBodyLinearMotor.cs @@ -101,6 +101,11 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in { throw new NotImplementedException(); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide accumulatedImpulses, ref OneBodyLinearMotorPrestepData prestep) + { + } } public class OneBodyLinearMotorTypeProcessor : OneBodyTypeProcessor diff --git a/BepuPhysics/Constraints/OneBodyLinearServo.cs b/BepuPhysics/Constraints/OneBodyLinearServo.cs index b6d4d53b2..c46fa6ca3 100644 --- a/BepuPhysics/Constraints/OneBodyLinearServo.cs +++ b/BepuPhysics/Constraints/OneBodyLinearServo.cs @@ -215,6 +215,11 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in ServoSettingsWide.ClampImpulse(maximumImpulse, ref accumulatedImpulses, ref csi); ApplyImpulse(offset, inertiaA, ref wsvA, csi); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide accumulatedImpulses, ref OneBodyLinearServoPrestepData prestep) + { + } } public class OneBodyLinearServoTypeProcessor : OneBodyTypeProcessor diff --git a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs index 79ee5eba7..524b25841 100644 --- a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs @@ -24,6 +24,10 @@ void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in Bod in TPrestepData prestep, in TAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA); void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, in TPrestepData prestep, ref TAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA); + + void UpdateForNewPose( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in TAccumulatedImpulse accumulatedImpulses, ref TPrestepData prestep); } /// @@ -273,7 +277,10 @@ public unsafe override void WarmStart2(bodies, ref integratorCallbacks, ref integrationFlags, 0, dt, workerIndex, i, ref references, count, out var positionA, out var orientationA, out var wsvA, out var inertiaA); -; + + if (typeof(TAllowPoseIntegration) == typeof(AllowPoseIntegration)) + function.UpdateForNewPose(positionA, orientationA, inertiaA, wsvA, accumulatedImpulses, ref prestep); + ; function.WarmStart2(positionA, orientationA, inertiaA, prestep, accumulatedImpulses, ref wsvA); if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) diff --git a/BepuPhysics/Constraints/PointOnLineServo.cs b/BepuPhysics/Constraints/PointOnLineServo.cs index 577932da3..b10508c25 100644 --- a/BepuPhysics/Constraints/PointOnLineServo.cs +++ b/BepuPhysics/Constraints/PointOnLineServo.cs @@ -230,6 +230,11 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in { throw new NotImplementedException(); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector2Wide accumulatedImpulses, ref PointOnLineServoPrestepData prestep) + { + } } public class PointOnLineServoTypeProcessor : TwoBodyTypeProcessor diff --git a/BepuPhysics/Constraints/SwingLimit.cs b/BepuPhysics/Constraints/SwingLimit.cs index e6c0f95a9..e05d95b91 100644 --- a/BepuPhysics/Constraints/SwingLimit.cs +++ b/BepuPhysics/Constraints/SwingLimit.cs @@ -188,6 +188,11 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in { throw new NotImplementedException(); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector accumulatedImpulses, ref SwingLimitPrestepData prestep) + { + } } public class SwingLimitTypeProcessor : TwoBodyTypeProcessor, SwingLimitFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular> diff --git a/BepuPhysics/Constraints/SwivelHinge.cs b/BepuPhysics/Constraints/SwivelHinge.cs index db9bce044..b8fa2e07c 100644 --- a/BepuPhysics/Constraints/SwivelHinge.cs +++ b/BepuPhysics/Constraints/SwivelHinge.cs @@ -220,6 +220,11 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in { throw new NotImplementedException(); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector4Wide accumulatedImpulses, ref SwivelHingePrestepData prestep) + { + } } public class SwivelHingeTypeProcessor : TwoBodyTypeProcessor diff --git a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs index 06f63633b..5a34fe3f3 100644 --- a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs @@ -41,6 +41,12 @@ void Solve2( in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, float dt, float inverseDt, in TPrestepData prestep, ref TAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC); + + void UpdateForNewPose( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, + in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in BodyVelocityWide wsvC, + in TAccumulatedImpulse accumulatedImpulses, ref TPrestepData prestep); } /// @@ -341,6 +347,9 @@ public unsafe override void WarmStart2(bodies, ref integratorCallbacks, ref integrationFlags, 2, dt, workerIndex, i, ref references.IndexC, count, out var positionC, out var orientationC, out var wsvC, out var inertiaC); + if (typeof(TAllowPoseIntegration) == typeof(AllowPoseIntegration)) + function.UpdateForNewPose(positionA, orientationA, inertiaA, wsvA, positionB, orientationB, inertiaB, wsvB, positionC, orientationC, inertiaC, wsvC, accumulatedImpulses, ref prestep); + function.WarmStart2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, prestep, accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC); if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) diff --git a/BepuPhysics/Constraints/TwistLimit.cs b/BepuPhysics/Constraints/TwistLimit.cs index 9fef5459b..e7314da87 100644 --- a/BepuPhysics/Constraints/TwistLimit.cs +++ b/BepuPhysics/Constraints/TwistLimit.cs @@ -154,6 +154,11 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in { throw new NotImplementedException(); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector accumulatedImpulses, ref TwistLimitPrestepData prestep) + { + } } public class TwistLimitTypeProcessor : TwoBodyTypeProcessor, TwistLimitFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular> diff --git a/BepuPhysics/Constraints/TwistMotor.cs b/BepuPhysics/Constraints/TwistMotor.cs index 4ddddcef2..85b34882c 100644 --- a/BepuPhysics/Constraints/TwistMotor.cs +++ b/BepuPhysics/Constraints/TwistMotor.cs @@ -139,6 +139,11 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in { throw new NotImplementedException(); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector accumulatedImpulses, ref TwistMotorPrestepData prestep) + { + } } public class TwistMotorTypeProcessor : TwoBodyTypeProcessor, TwistMotorFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular> diff --git a/BepuPhysics/Constraints/TwistServo.cs b/BepuPhysics/Constraints/TwistServo.cs index 037b06dc9..4a65983c2 100644 --- a/BepuPhysics/Constraints/TwistServo.cs +++ b/BepuPhysics/Constraints/TwistServo.cs @@ -224,6 +224,11 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in { throw new NotImplementedException(); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector accumulatedImpulses, ref TwistServoPrestepData prestep) + { + } } public class TwistServoTypeProcessor : TwoBodyTypeProcessor, TwistServoFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular> diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 3d65abe7d..d76b6dafa 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -37,6 +37,11 @@ void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in Bod in TPrestepData prestep, in TAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB); void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in TPrestepData prestep, ref TAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB); + + void UpdateForNewPose( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, + in TAccumulatedImpulse accumulatedImpulses, ref TPrestepData prestep); } /// @@ -385,6 +390,9 @@ public unsafe override void WarmStart2(bodies, ref integratorCallbacks, ref integrationFlags, 1, dt, workerIndex, i, ref references.IndexB, count, out var positionB, out var orientationB, out var wsvB, out var inertiaB); + if (typeof(TAllowPoseIntegration) == typeof(AllowPoseIntegration)) + function.UpdateForNewPose(positionA, orientationA, inertiaA, wsvA, positionB, orientationB, inertiaB, wsvB, accumulatedImpulses, ref prestep); + function.WarmStart2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, prestep, accumulatedImpulses, ref wsvA, ref wsvB); if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index c303992f8..2fda76812 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -877,7 +877,7 @@ public static unsafe void GatherAndIntegrate solve) -> (pose -> velocity -> solve) -> (pose -> velocity -> solve) -> pose diff --git a/BepuPhysics/Constraints/VolumeConstraint.cs b/BepuPhysics/Constraints/VolumeConstraint.cs index c1ae458d0..848b96168 100644 --- a/BepuPhysics/Constraints/VolumeConstraint.cs +++ b/BepuPhysics/Constraints/VolumeConstraint.cs @@ -215,6 +215,16 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in { throw new NotImplementedException(); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void UpdateForNewPose( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, + in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in BodyVelocityWide wsvC, + in Vector3Wide positionD, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, in BodyVelocityWide wsvD, + in Vector accumulatedImpulses, ref VolumeConstraintPrestepData prestep) + { + } } diff --git a/BepuPhysics/Constraints/Weld.cs b/BepuPhysics/Constraints/Weld.cs index 0352e58f6..6e606dd81 100644 --- a/BepuPhysics/Constraints/Weld.cs +++ b/BepuPhysics/Constraints/Weld.cs @@ -356,6 +356,10 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in ApplyImpulse(inertiaA, inertiaB, offset, orientationCSI, offsetCSI, ref wsvA, ref wsvB); } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in WeldAccumulatedImpulses accumulatedImpulses, ref WeldPrestepData prestep) + { + } } diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index 5e0253c87..55c691c47 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -94,11 +94,11 @@ public void Execute(Solver solver, int blockIndex, int workerIndex) var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; if (SubstepIndex == 0) { - this.solver.WarmStartBlock(workerIndex, block.BatchIndex, block.TypeBatchIndex, block.StartBundle, block.End, ref typeBatch, typeProcessor, Dt, InverseDt); + this.solver.WarmStartBlock(workerIndex, block.BatchIndex, block.TypeBatchIndex, block.StartBundle, block.End, ref typeBatch, typeProcessor, Dt, InverseDt); } else { - this.solver.WarmStartBlock(workerIndex, block.BatchIndex, block.TypeBatchIndex, block.StartBundle, block.End, ref typeBatch, typeProcessor, Dt, InverseDt); + this.solver.WarmStartBlock(workerIndex, block.BatchIndex, block.TypeBatchIndex, block.StartBundle, block.End, ref typeBatch, typeProcessor, Dt, InverseDt); } } @@ -565,11 +565,11 @@ public override void SolveStep2(float totalDt, IThreadDispatcher threadDispatche ref var typeBatch = ref batch.TypeBatches[j]; if (substepIndex == 0) { - WarmStartBlock(0, i, j, 0, typeBatch.BundleCount, ref typeBatch, TypeProcessors[typeBatch.TypeId], substepDt, inverseDt); + WarmStartBlock(0, i, j, 0, typeBatch.BundleCount, ref typeBatch, TypeProcessors[typeBatch.TypeId], substepDt, inverseDt); } else { - WarmStartBlock(0, i, j, 0, typeBatch.BundleCount, ref typeBatch, TypeProcessors[typeBatch.TypeId], substepDt, inverseDt); + WarmStartBlock(0, i, j, 0, typeBatch.BundleCount, ref typeBatch, TypeProcessors[typeBatch.TypeId], substepDt, inverseDt); } } } From 3ce7779bb1d2e26b6270d4483ab0067d4a419105 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 24 Jul 2021 17:49:31 -0500 Subject: [PATCH 141/947] Refactor, cleanup. --- .../Constraints/AngularAxisGearMotor.cs | 5 +- BepuPhysics/Constraints/AngularAxisMotor.cs | 5 +- BepuPhysics/Constraints/AngularHinge.cs | 5 +- BepuPhysics/Constraints/AngularMotor.cs | 5 +- BepuPhysics/Constraints/AngularServo.cs | 5 +- BepuPhysics/Constraints/AngularSwivelHinge.cs | 5 +- BepuPhysics/Constraints/AreaConstraint.cs | 4 +- BepuPhysics/Constraints/BallSocket.cs | 5 +- BepuPhysics/Constraints/BallSocketMotor.cs | 5 +- BepuPhysics/Constraints/BallSocketServo.cs | 5 +- .../Constraints/CenterDistanceConstraint.cs | 5 +- .../Constraints/Contact/ContactConvexTypes.cs | 60 +++++++++++++++++++ .../Constraints/Contact/ContactConvexTypes.tt | 10 ++++ .../Contact/ContactNonconvexCommon.cs | 17 ++++++ BepuPhysics/Constraints/DistanceLimit.cs | 5 +- BepuPhysics/Constraints/DistanceServo.cs | 5 +- .../Constraints/FourBodyTypeProcessor.cs | 4 +- BepuPhysics/Constraints/Hinge.cs | 5 +- BepuPhysics/Constraints/LinearAxisLimit.cs | 5 +- BepuPhysics/Constraints/LinearAxisMotor.cs | 5 +- BepuPhysics/Constraints/LinearAxisServo.cs | 5 +- .../Constraints/OneBodyAngularMotor.cs | 4 +- .../Constraints/OneBodyAngularServo.cs | 4 +- BepuPhysics/Constraints/OneBodyLinearMotor.cs | 4 +- BepuPhysics/Constraints/OneBodyLinearServo.cs | 4 +- .../Constraints/OneBodyTypeProcessor.cs | 4 +- BepuPhysics/Constraints/PointOnLineServo.cs | 5 +- BepuPhysics/Constraints/SwingLimit.cs | 5 +- BepuPhysics/Constraints/SwivelHinge.cs | 5 +- .../Constraints/ThreeBodyTypeProcessor.cs | 4 +- BepuPhysics/Constraints/TwistLimit.cs | 5 +- BepuPhysics/Constraints/TwistMotor.cs | 5 +- BepuPhysics/Constraints/TwistServo.cs | 5 +- .../Constraints/TwoBodyTypeProcessor.cs | 4 +- BepuPhysics/Constraints/VolumeConstraint.cs | 2 +- BepuPhysics/Constraints/Weld.cs | 5 +- .../Characters/CharacterMotionConstraint.cs | 19 +++++- .../Characters/CharacterMotionConstraint.tt | 12 +++- 38 files changed, 230 insertions(+), 41 deletions(-) diff --git a/BepuPhysics/Constraints/AngularAxisGearMotor.cs b/BepuPhysics/Constraints/AngularAxisGearMotor.cs index 9f809b0d5..712de5683 100644 --- a/BepuPhysics/Constraints/AngularAxisGearMotor.cs +++ b/BepuPhysics/Constraints/AngularAxisGearMotor.cs @@ -163,7 +163,10 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector accumulatedImpulses, ref AngularAxisGearMotorPrestepData prestep) + public void UpdateForNewPose( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, + in Vector dt, in Vector accumulatedImpulses, ref AngularAxisGearMotorPrestepData prestep) { } } diff --git a/BepuPhysics/Constraints/AngularAxisMotor.cs b/BepuPhysics/Constraints/AngularAxisMotor.cs index 68ebcbaa4..775e8c56f 100644 --- a/BepuPhysics/Constraints/AngularAxisMotor.cs +++ b/BepuPhysics/Constraints/AngularAxisMotor.cs @@ -159,7 +159,10 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector accumulatedImpulses, ref AngularAxisMotorPrestepData prestep) + public void UpdateForNewPose( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, + in Vector dt, in Vector accumulatedImpulses, ref AngularAxisMotorPrestepData prestep) { } } diff --git a/BepuPhysics/Constraints/AngularHinge.cs b/BepuPhysics/Constraints/AngularHinge.cs index c7b729d64..482736a40 100644 --- a/BepuPhysics/Constraints/AngularHinge.cs +++ b/BepuPhysics/Constraints/AngularHinge.cs @@ -237,7 +237,10 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector2Wide accumulatedImpulses, ref AngularHingePrestepData prestep) + public void UpdateForNewPose( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, + in Vector dt, in Vector2Wide accumulatedImpulses, ref AngularHingePrestepData prestep) { } } diff --git a/BepuPhysics/Constraints/AngularMotor.cs b/BepuPhysics/Constraints/AngularMotor.cs index af48e319e..aac11cdb6 100644 --- a/BepuPhysics/Constraints/AngularMotor.cs +++ b/BepuPhysics/Constraints/AngularMotor.cs @@ -112,7 +112,10 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector3Wide accumulatedImpulses, ref AngularMotorPrestepData prestep) + public void UpdateForNewPose( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, + in Vector dt, in Vector3Wide accumulatedImpulses, ref AngularMotorPrestepData prestep) { } } diff --git a/BepuPhysics/Constraints/AngularServo.cs b/BepuPhysics/Constraints/AngularServo.cs index 9ed82fca5..b9d4dbdf9 100644 --- a/BepuPhysics/Constraints/AngularServo.cs +++ b/BepuPhysics/Constraints/AngularServo.cs @@ -153,7 +153,10 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector3Wide accumulatedImpulses, ref AngularServoPrestepData prestep) + public void UpdateForNewPose( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, + in Vector dt, in Vector3Wide accumulatedImpulses, ref AngularServoPrestepData prestep) { } } diff --git a/BepuPhysics/Constraints/AngularSwivelHinge.cs b/BepuPhysics/Constraints/AngularSwivelHinge.cs index c4d01b5c8..0a33eda6e 100644 --- a/BepuPhysics/Constraints/AngularSwivelHinge.cs +++ b/BepuPhysics/Constraints/AngularSwivelHinge.cs @@ -165,7 +165,10 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector accumulatedImpulses, ref AngularSwivelHingePrestepData prestep) + public void UpdateForNewPose( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, + in Vector dt, in Vector accumulatedImpulses, ref AngularSwivelHingePrestepData prestep) { } } diff --git a/BepuPhysics/Constraints/AreaConstraint.cs b/BepuPhysics/Constraints/AreaConstraint.cs index e95c9ef00..18397c332 100644 --- a/BepuPhysics/Constraints/AreaConstraint.cs +++ b/BepuPhysics/Constraints/AreaConstraint.cs @@ -199,8 +199,8 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in public void UpdateForNewPose( in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, - in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in BodyVelocityWide wsvC, - in Vector accumulatedImpulses, ref AreaConstraintPrestepData prestep) + in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in BodyVelocityWide wsvC, + in Vector dt, in Vector accumulatedImpulses, ref AreaConstraintPrestepData prestep) { } } diff --git a/BepuPhysics/Constraints/BallSocket.cs b/BepuPhysics/Constraints/BallSocket.cs index 1b677b158..0d6491c7d 100644 --- a/BepuPhysics/Constraints/BallSocket.cs +++ b/BepuPhysics/Constraints/BallSocket.cs @@ -137,7 +137,10 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector3Wide accumulatedImpulses, ref BallSocketPrestepData prestep) + public void UpdateForNewPose( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, + in Vector dt, in Vector3Wide accumulatedImpulses, ref BallSocketPrestepData prestep) { } } diff --git a/BepuPhysics/Constraints/BallSocketMotor.cs b/BepuPhysics/Constraints/BallSocketMotor.cs index 613353639..daed5ac6b 100644 --- a/BepuPhysics/Constraints/BallSocketMotor.cs +++ b/BepuPhysics/Constraints/BallSocketMotor.cs @@ -122,7 +122,10 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector3Wide accumulatedImpulses, ref BallSocketMotorPrestepData prestep) + public void UpdateForNewPose( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, + in Vector dt, in Vector3Wide accumulatedImpulses, ref BallSocketMotorPrestepData prestep) { } } diff --git a/BepuPhysics/Constraints/BallSocketServo.cs b/BepuPhysics/Constraints/BallSocketServo.cs index 77760aae3..0bf2041e1 100644 --- a/BepuPhysics/Constraints/BallSocketServo.cs +++ b/BepuPhysics/Constraints/BallSocketServo.cs @@ -129,7 +129,10 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector3Wide accumulatedImpulses, ref BallSocketServoPrestepData prestep) + public void UpdateForNewPose( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, + in Vector dt, in Vector3Wide accumulatedImpulses, ref BallSocketServoPrestepData prestep) { } } diff --git a/BepuPhysics/Constraints/CenterDistanceConstraint.cs b/BepuPhysics/Constraints/CenterDistanceConstraint.cs index 7bde7a066..e50e73d48 100644 --- a/BepuPhysics/Constraints/CenterDistanceConstraint.cs +++ b/BepuPhysics/Constraints/CenterDistanceConstraint.cs @@ -182,7 +182,10 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector accumulatedImpulses, ref CenterDistancePrestepData prestep) + public void UpdateForNewPose( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, + in Vector dt, in Vector accumulatedImpulses, ref CenterDistancePrestepData prestep) { } diff --git a/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs b/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs index ce44f4525..8a69fb67c 100644 --- a/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs +++ b/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs @@ -377,6 +377,13 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in var maximumTwistImpulse = prestep.MaterialProperties.FrictionCoefficient * accumulatedImpulses.Penetration0 * prestep.Contact0.Depth; TwistFrictionOneBody.Solve2(prestep.Normal, inertiaA, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void UpdateForNewPose( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector dt, in Contact1AccumulatedImpulses accumulatedImpulses, ref Contact1OneBodyPrestepData prestep) + { + } } /// @@ -589,6 +596,13 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact1.OffsetA)); TwistFrictionOneBody.Solve2(prestep.Normal, inertiaA, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void UpdateForNewPose( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector dt, in Contact2AccumulatedImpulses accumulatedImpulses, ref Contact2OneBodyPrestepData prestep) + { + } } /// @@ -818,6 +832,13 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in accumulatedImpulses.Penetration2 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact2.OffsetA)); TwistFrictionOneBody.Solve2(prestep.Normal, inertiaA, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void UpdateForNewPose( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector dt, in Contact3AccumulatedImpulses accumulatedImpulses, ref Contact3OneBodyPrestepData prestep) + { + } } /// @@ -1064,6 +1085,13 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in accumulatedImpulses.Penetration3 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact3.OffsetA)); TwistFrictionOneBody.Solve2(prestep.Normal, inertiaA, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void UpdateForNewPose( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector dt, in Contact4AccumulatedImpulses accumulatedImpulses, ref Contact4OneBodyPrestepData prestep) + { + } } /// @@ -1276,6 +1304,14 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in var maximumTwistImpulse = prestep.MaterialProperties.FrictionCoefficient * accumulatedImpulses.Penetration0 * prestep.Contact0.Depth; TwistFriction.Solve2(prestep.Normal, inertiaA, inertiaB, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void UpdateForNewPose( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, + in Vector dt, in Contact1AccumulatedImpulses accumulatedImpulses, ref Contact1PrestepData prestep) + { + } } /// @@ -1507,6 +1543,14 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact1.OffsetA)); TwistFriction.Solve2(prestep.Normal, inertiaA, inertiaB, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void UpdateForNewPose( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, + in Vector dt, in Contact2AccumulatedImpulses accumulatedImpulses, ref Contact2PrestepData prestep) + { + } } /// @@ -1756,6 +1800,14 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in accumulatedImpulses.Penetration2 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact2.OffsetA)); TwistFriction.Solve2(prestep.Normal, inertiaA, inertiaB, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void UpdateForNewPose( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, + in Vector dt, in Contact3AccumulatedImpulses accumulatedImpulses, ref Contact3PrestepData prestep) + { + } } /// @@ -2023,6 +2075,14 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in accumulatedImpulses.Penetration3 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact3.OffsetA)); TwistFriction.Solve2(prestep.Normal, inertiaA, inertiaB, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void UpdateForNewPose( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, + in Vector dt, in Contact4AccumulatedImpulses accumulatedImpulses, ref Contact4PrestepData prestep) + { + } } /// diff --git a/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt b/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt index 65014855f..920955117 100644 --- a/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt +++ b/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt @@ -376,6 +376,16 @@ for (int i = 0; i < contactCount; ++i) <#}#> TwistFriction<#=suffix#>.Solve2(prestep.Normal, inertiaA, <#if (bodyCount == 2) {#>inertiaB, <#}#>maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void UpdateForNewPose( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, +<# if(bodyCount == 2) {#> + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, +<#}#> + in Vector dt, in Contact<#= contactCount #>AccumulatedImpulses accumulatedImpulses, ref Contact<#= contactCount #><#=suffix#>PrestepData prestep) + { + } } /// diff --git a/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs b/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs index 0a5d7b96a..9bfbbca06 100644 --- a/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs +++ b/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs @@ -303,6 +303,14 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in { throw new System.NotImplementedException(); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void UpdateForNewPose( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector dt, in TAccumulatedImpulses accumulatedImpulses, ref TPrestep prestep) + { + throw new System.NotImplementedException(); + } } public struct ContactNonconvexTwoBodyFunctions : @@ -401,5 +409,14 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in { throw new System.NotImplementedException(); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void UpdateForNewPose( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, + in Vector dt, in TAccumulatedImpulses accumulatedImpulses, ref TPrestep prestep) + { + throw new System.NotImplementedException(); + } } } diff --git a/BepuPhysics/Constraints/DistanceLimit.cs b/BepuPhysics/Constraints/DistanceLimit.cs index 057971133..3bfafb3fc 100644 --- a/BepuPhysics/Constraints/DistanceLimit.cs +++ b/BepuPhysics/Constraints/DistanceLimit.cs @@ -171,7 +171,10 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector accumulatedImpulses, ref DistanceLimitPrestepData prestep) + public void UpdateForNewPose( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, + in Vector dt, in Vector accumulatedImpulses, ref DistanceLimitPrestepData prestep) { } } diff --git a/BepuPhysics/Constraints/DistanceServo.cs b/BepuPhysics/Constraints/DistanceServo.cs index e64432173..d3b23679d 100644 --- a/BepuPhysics/Constraints/DistanceServo.cs +++ b/BepuPhysics/Constraints/DistanceServo.cs @@ -252,7 +252,10 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector accumulatedImpulses, ref DistanceServoPrestepData prestep) + public void UpdateForNewPose( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, + in Vector dt, in Vector accumulatedImpulses, ref DistanceServoPrestepData prestep) { } } diff --git a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs index 7ae9b118c..f34f0f92b 100644 --- a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs @@ -53,7 +53,7 @@ void UpdateForNewPose( in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in BodyVelocityWide wsvC, in Vector3Wide positionD, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, in BodyVelocityWide wsvD, - in TAccumulatedImpulse accumulatedImpulses, ref TPrestepData prestep); + in Vector dt, in TAccumulatedImpulse accumulatedImpulses, ref TPrestepData prestep); } /// @@ -380,7 +380,7 @@ public unsafe override void WarmStart2(dt), accumulatedImpulses, ref prestep); function.WarmStart2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, positionD, orientationD, inertiaD, prestep, accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC, ref wsvD); diff --git a/BepuPhysics/Constraints/Hinge.cs b/BepuPhysics/Constraints/Hinge.cs index f052a042f..6e0c4fe8e 100644 --- a/BepuPhysics/Constraints/Hinge.cs +++ b/BepuPhysics/Constraints/Hinge.cs @@ -235,7 +235,10 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in HingeAccumulatedImpulses accumulatedImpulses, ref HingePrestepData prestep) + public void UpdateForNewPose( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, + in Vector dt, in HingeAccumulatedImpulses accumulatedImpulses, ref HingePrestepData prestep) { } } diff --git a/BepuPhysics/Constraints/LinearAxisLimit.cs b/BepuPhysics/Constraints/LinearAxisLimit.cs index 12e15d814..9afc7e360 100644 --- a/BepuPhysics/Constraints/LinearAxisLimit.cs +++ b/BepuPhysics/Constraints/LinearAxisLimit.cs @@ -160,7 +160,10 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector accumulatedImpulses, ref LinearAxisLimitPrestepData prestep) + public void UpdateForNewPose( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, + in Vector dt, in Vector accumulatedImpulses, ref LinearAxisLimitPrestepData prestep) { } } diff --git a/BepuPhysics/Constraints/LinearAxisMotor.cs b/BepuPhysics/Constraints/LinearAxisMotor.cs index 6e01ba7b5..68c95b97b 100644 --- a/BepuPhysics/Constraints/LinearAxisMotor.cs +++ b/BepuPhysics/Constraints/LinearAxisMotor.cs @@ -125,7 +125,10 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector accumulatedImpulses, ref LinearAxisMotorPrestepData prestep) + public void UpdateForNewPose( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, + in Vector dt, in Vector accumulatedImpulses, ref LinearAxisMotorPrestepData prestep) { } } diff --git a/BepuPhysics/Constraints/LinearAxisServo.cs b/BepuPhysics/Constraints/LinearAxisServo.cs index 760cc2c0b..655a1bd5f 100644 --- a/BepuPhysics/Constraints/LinearAxisServo.cs +++ b/BepuPhysics/Constraints/LinearAxisServo.cs @@ -238,7 +238,10 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector accumulatedImpulses, ref LinearAxisServoPrestepData prestep) + public void UpdateForNewPose( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, + in Vector dt, in Vector accumulatedImpulses, ref LinearAxisServoPrestepData prestep) { } } diff --git a/BepuPhysics/Constraints/OneBodyAngularMotor.cs b/BepuPhysics/Constraints/OneBodyAngularMotor.cs index 6ab5f233f..d21e72b63 100644 --- a/BepuPhysics/Constraints/OneBodyAngularMotor.cs +++ b/BepuPhysics/Constraints/OneBodyAngularMotor.cs @@ -121,7 +121,9 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide accumulatedImpulses, ref OneBodyAngularMotorPrestepData prestep) + public void UpdateForNewPose( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector dt, in Vector3Wide accumulatedImpulses, ref OneBodyAngularMotorPrestepData prestep) { } } diff --git a/BepuPhysics/Constraints/OneBodyAngularServo.cs b/BepuPhysics/Constraints/OneBodyAngularServo.cs index 6c3dc3714..729beb7f5 100644 --- a/BepuPhysics/Constraints/OneBodyAngularServo.cs +++ b/BepuPhysics/Constraints/OneBodyAngularServo.cs @@ -170,7 +170,9 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide accumulatedImpulses, ref OneBodyAngularServoPrestepData prestep) + public void UpdateForNewPose( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector dt, in Vector3Wide accumulatedImpulses, ref OneBodyAngularServoPrestepData prestep) { } } diff --git a/BepuPhysics/Constraints/OneBodyLinearMotor.cs b/BepuPhysics/Constraints/OneBodyLinearMotor.cs index 84a151f7f..da925c0a9 100644 --- a/BepuPhysics/Constraints/OneBodyLinearMotor.cs +++ b/BepuPhysics/Constraints/OneBodyLinearMotor.cs @@ -103,7 +103,9 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide accumulatedImpulses, ref OneBodyLinearMotorPrestepData prestep) + public void UpdateForNewPose( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector dt, in Vector3Wide accumulatedImpulses, ref OneBodyLinearMotorPrestepData prestep) { } } diff --git a/BepuPhysics/Constraints/OneBodyLinearServo.cs b/BepuPhysics/Constraints/OneBodyLinearServo.cs index c46fa6ca3..98aa67d1f 100644 --- a/BepuPhysics/Constraints/OneBodyLinearServo.cs +++ b/BepuPhysics/Constraints/OneBodyLinearServo.cs @@ -217,7 +217,9 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide accumulatedImpulses, ref OneBodyLinearServoPrestepData prestep) + public void UpdateForNewPose( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector dt, in Vector3Wide accumulatedImpulses, ref OneBodyLinearServoPrestepData prestep) { } } diff --git a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs index 524b25841..6f378d20d 100644 --- a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs @@ -27,7 +27,7 @@ void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyIne void UpdateForNewPose( in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in TAccumulatedImpulse accumulatedImpulses, ref TPrestepData prestep); + in Vector dt, in TAccumulatedImpulse accumulatedImpulses, ref TPrestepData prestep); } /// @@ -279,7 +279,7 @@ public unsafe override void WarmStart2(dt), accumulatedImpulses, ref prestep); ; function.WarmStart2(positionA, orientationA, inertiaA, prestep, accumulatedImpulses, ref wsvA); diff --git a/BepuPhysics/Constraints/PointOnLineServo.cs b/BepuPhysics/Constraints/PointOnLineServo.cs index b10508c25..b17ff5832 100644 --- a/BepuPhysics/Constraints/PointOnLineServo.cs +++ b/BepuPhysics/Constraints/PointOnLineServo.cs @@ -232,7 +232,10 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector2Wide accumulatedImpulses, ref PointOnLineServoPrestepData prestep) + public void UpdateForNewPose( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, + in Vector dt, in Vector2Wide accumulatedImpulses, ref PointOnLineServoPrestepData prestep) { } } diff --git a/BepuPhysics/Constraints/SwingLimit.cs b/BepuPhysics/Constraints/SwingLimit.cs index e05d95b91..c759c080a 100644 --- a/BepuPhysics/Constraints/SwingLimit.cs +++ b/BepuPhysics/Constraints/SwingLimit.cs @@ -190,7 +190,10 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector accumulatedImpulses, ref SwingLimitPrestepData prestep) + public void UpdateForNewPose( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, + in Vector dt, in Vector accumulatedImpulses, ref SwingLimitPrestepData prestep) { } } diff --git a/BepuPhysics/Constraints/SwivelHinge.cs b/BepuPhysics/Constraints/SwivelHinge.cs index b8fa2e07c..1925702d7 100644 --- a/BepuPhysics/Constraints/SwivelHinge.cs +++ b/BepuPhysics/Constraints/SwivelHinge.cs @@ -222,7 +222,10 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector4Wide accumulatedImpulses, ref SwivelHingePrestepData prestep) + public void UpdateForNewPose( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, + in Vector dt, in Vector4Wide accumulatedImpulses, ref SwivelHingePrestepData prestep) { } } diff --git a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs index 5a34fe3f3..1b7f9c341 100644 --- a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs @@ -46,7 +46,7 @@ void UpdateForNewPose( in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in BodyVelocityWide wsvC, - in TAccumulatedImpulse accumulatedImpulses, ref TPrestepData prestep); + in Vector dt, in TAccumulatedImpulse accumulatedImpulses, ref TPrestepData prestep); } /// @@ -348,7 +348,7 @@ public unsafe override void WarmStart2(dt), accumulatedImpulses, ref prestep); function.WarmStart2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, prestep, accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC); diff --git a/BepuPhysics/Constraints/TwistLimit.cs b/BepuPhysics/Constraints/TwistLimit.cs index e7314da87..8aa64b2f5 100644 --- a/BepuPhysics/Constraints/TwistLimit.cs +++ b/BepuPhysics/Constraints/TwistLimit.cs @@ -156,7 +156,10 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector accumulatedImpulses, ref TwistLimitPrestepData prestep) + public void UpdateForNewPose( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, + in Vector dt, in Vector accumulatedImpulses, ref TwistLimitPrestepData prestep) { } } diff --git a/BepuPhysics/Constraints/TwistMotor.cs b/BepuPhysics/Constraints/TwistMotor.cs index 85b34882c..16ae337df 100644 --- a/BepuPhysics/Constraints/TwistMotor.cs +++ b/BepuPhysics/Constraints/TwistMotor.cs @@ -141,7 +141,10 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector accumulatedImpulses, ref TwistMotorPrestepData prestep) + public void UpdateForNewPose( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, + in Vector dt, in Vector accumulatedImpulses, ref TwistMotorPrestepData prestep) { } } diff --git a/BepuPhysics/Constraints/TwistServo.cs b/BepuPhysics/Constraints/TwistServo.cs index 4a65983c2..40a8f4f42 100644 --- a/BepuPhysics/Constraints/TwistServo.cs +++ b/BepuPhysics/Constraints/TwistServo.cs @@ -226,7 +226,10 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector accumulatedImpulses, ref TwistServoPrestepData prestep) + public void UpdateForNewPose( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, + in Vector dt, in Vector accumulatedImpulses, ref TwistServoPrestepData prestep) { } } diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index d76b6dafa..bfa641192 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -41,7 +41,7 @@ void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyIne void UpdateForNewPose( in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, - in TAccumulatedImpulse accumulatedImpulses, ref TPrestepData prestep); + in Vector dt, in TAccumulatedImpulse accumulatedImpulses, ref TPrestepData prestep); } /// @@ -391,7 +391,7 @@ public unsafe override void WarmStart2(dt), accumulatedImpulses, ref prestep); function.WarmStart2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, prestep, accumulatedImpulses, ref wsvA, ref wsvB); diff --git a/BepuPhysics/Constraints/VolumeConstraint.cs b/BepuPhysics/Constraints/VolumeConstraint.cs index 848b96168..617f798f3 100644 --- a/BepuPhysics/Constraints/VolumeConstraint.cs +++ b/BepuPhysics/Constraints/VolumeConstraint.cs @@ -222,7 +222,7 @@ public void UpdateForNewPose( in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in BodyVelocityWide wsvC, in Vector3Wide positionD, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, in BodyVelocityWide wsvD, - in Vector accumulatedImpulses, ref VolumeConstraintPrestepData prestep) + in Vector dt, in Vector accumulatedImpulses, ref VolumeConstraintPrestepData prestep) { } } diff --git a/BepuPhysics/Constraints/Weld.cs b/BepuPhysics/Constraints/Weld.cs index 6e606dd81..e918d148a 100644 --- a/BepuPhysics/Constraints/Weld.cs +++ b/BepuPhysics/Constraints/Weld.cs @@ -357,7 +357,10 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in WeldAccumulatedImpulses accumulatedImpulses, ref WeldPrestepData prestep) + public void UpdateForNewPose( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, + in Vector dt, in WeldAccumulatedImpulses accumulatedImpulses, ref WeldPrestepData prestep) { } diff --git a/Demos/Demos/Characters/CharacterMotionConstraint.cs b/Demos/Demos/Characters/CharacterMotionConstraint.cs index 8774c54b1..811b51ea8 100644 --- a/Demos/Demos/Characters/CharacterMotionConstraint.cs +++ b/Demos/Demos/Characters/CharacterMotionConstraint.cs @@ -310,11 +310,18 @@ public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, { throw new NotImplementedException(); } - + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, in StaticCharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA) { throw new NotImplementedException(); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void UpdateForNewPose( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector dt, in CharacterMotionAccumulatedImpulse accumulatedImpulses, ref StaticCharacterMotionPrestep prestep) + { + } } //Each constraint type has its own 'type processor'- it acts as the outer loop that handles all the common logic across batches of constraints and invokes @@ -651,11 +658,19 @@ public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, { throw new NotImplementedException(); } - + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in DynamicCharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void UpdateForNewPose( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, + in Vector dt, in CharacterMotionAccumulatedImpulse accumulatedImpulses, ref DynamicCharacterMotionPrestep prestep) + { + } } //Each constraint type has its own 'type processor'- it acts as the outer loop that handles all the common logic across batches of constraints and invokes diff --git a/Demos/Demos/Characters/CharacterMotionConstraint.tt b/Demos/Demos/Characters/CharacterMotionConstraint.tt index e11069635..640fb8a4d 100644 --- a/Demos/Demos/Characters/CharacterMotionConstraint.tt +++ b/Demos/Demos/Characters/CharacterMotionConstraint.tt @@ -397,11 +397,21 @@ namespace Demos.Demos.Characters { throw new NotImplementedException(); } - + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(dynamic) {#>in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>float dt, float inverseDt, in <#=prefix#>CharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA<#if(dynamic) {#>, ref BodyVelocityWide wsvB<#}#>) { throw new NotImplementedException(); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void UpdateForNewPose( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, +<#if(dynamic){#> + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, +<#}#> + in Vector dt, in CharacterMotionAccumulatedImpulse accumulatedImpulses, ref <#=prefix#>CharacterMotionPrestep prestep) + { + } } //Each constraint type has its own 'type processor'- it acts as the outer loop that handles all the common logic across batches of constraints and invokes From 2ac95849b0aa812c587cba3438c45eaa5519598f Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 24 Jul 2021 20:02:05 -0500 Subject: [PATCH 142/947] Incremental contact updates pushed through, but temporarily disabled. Old model requires incremental updates after pose integration and before next velocity integration, which isn't feasible in the current model, so we'll need to reconstruct from local. --- .../Constraints/Contact/ContactConvexTypes.cs | 24 + .../Constraints/Contact/ContactConvexTypes.tt | 6 + BepuPhysics/Constraints/IBodyAccessFilter.cs | 11 +- .../Constraints/OneBodyTypeProcessor.cs | 8 +- .../Constraints/TwoBodyTypeProcessor.cs | 6 +- BepuPhysics/Solver_SubsteppingSolve2.cs | 445 +++++++++--------- Demos/Demos/NewtDemo.cs | 2 +- Demos/Demos/PyramidDemo.cs | 4 +- 8 files changed, 278 insertions(+), 228 deletions(-) diff --git a/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs b/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs index 8a69fb67c..f269444fb 100644 --- a/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs +++ b/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs @@ -383,6 +383,7 @@ public void UpdateForNewPose( in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector dt, in Contact1AccumulatedImpulses accumulatedImpulses, ref Contact1OneBodyPrestepData prestep) { + PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, wsvA, ref prestep.Contact0.Depth); } } @@ -602,6 +603,8 @@ public void UpdateForNewPose( in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector dt, in Contact2AccumulatedImpulses accumulatedImpulses, ref Contact2OneBodyPrestepData prestep) { + PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, wsvA, ref prestep.Contact0.Depth); + PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.Normal, wsvA, ref prestep.Contact1.Depth); } } @@ -838,6 +841,9 @@ public void UpdateForNewPose( in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector dt, in Contact3AccumulatedImpulses accumulatedImpulses, ref Contact3OneBodyPrestepData prestep) { + PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, wsvA, ref prestep.Contact0.Depth); + PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.Normal, wsvA, ref prestep.Contact1.Depth); + PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact2.OffsetA, prestep.Normal, wsvA, ref prestep.Contact2.Depth); } } @@ -1091,6 +1097,10 @@ public void UpdateForNewPose( in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector dt, in Contact4AccumulatedImpulses accumulatedImpulses, ref Contact4OneBodyPrestepData prestep) { + PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, wsvA, ref prestep.Contact0.Depth); + PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.Normal, wsvA, ref prestep.Contact1.Depth); + PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact2.OffsetA, prestep.Normal, wsvA, ref prestep.Contact2.Depth); + PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact3.OffsetA, prestep.Normal, wsvA, ref prestep.Contact3.Depth); } } @@ -1311,6 +1321,8 @@ public void UpdateForNewPose( in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector dt, in Contact1AccumulatedImpulses accumulatedImpulses, ref Contact1PrestepData prestep) { + prestep.OffsetB = positionB - positionA; + PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact0.Depth); } } @@ -1550,6 +1562,9 @@ public void UpdateForNewPose( in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector dt, in Contact2AccumulatedImpulses accumulatedImpulses, ref Contact2PrestepData prestep) { + prestep.OffsetB = positionB - positionA; + PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact0.Depth); + PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact1.Depth); } } @@ -1807,6 +1822,10 @@ public void UpdateForNewPose( in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector dt, in Contact3AccumulatedImpulses accumulatedImpulses, ref Contact3PrestepData prestep) { + prestep.OffsetB = positionB - positionA; + PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact0.Depth); + PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact1.Depth); + PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact2.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact2.Depth); } } @@ -2082,6 +2101,11 @@ public void UpdateForNewPose( in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector dt, in Contact4AccumulatedImpulses accumulatedImpulses, ref Contact4PrestepData prestep) { + prestep.OffsetB = positionB - positionA; + PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact0.Depth); + PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact1.Depth); + PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact2.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact2.Depth); + PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact3.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact3.Depth); } } diff --git a/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt b/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt index 920955117..c6234671e 100644 --- a/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt +++ b/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt @@ -385,6 +385,12 @@ for (int i = 0; i < contactCount; ++i) <#}#> in Vector dt, in Contact<#= contactCount #>AccumulatedImpulses accumulatedImpulses, ref Contact<#= contactCount #><#=suffix#>PrestepData prestep) { +<#if (bodyCount == 2) {#> + prestep.OffsetB = positionB - positionA; +<#}#> +<#for (int i = 0; i < contactCount; ++i) {#> + PenetrationLimit<#=suffix#>.UpdatePenetrationDepth(dt, prestep.Contact<#=i#>.OffsetA, <#if(bodyCount == 2) {#>prestep.OffsetB, <#}#>prestep.Normal, wsvA, <#if (bodyCount == 2) {#>wsvB, <#}#>ref prestep.Contact<#=i#>.Depth); +<#}#> } } diff --git a/BepuPhysics/Constraints/IBodyAccessFilter.cs b/BepuPhysics/Constraints/IBodyAccessFilter.cs index f2a35589a..8b70ff663 100644 --- a/BepuPhysics/Constraints/IBodyAccessFilter.cs +++ b/BepuPhysics/Constraints/IBodyAccessFilter.cs @@ -45,7 +45,7 @@ public struct AccessAll : IBodyAccessFilter public bool AccessAngularVelocity => true; } - public struct AccessNoPoses : IBodyAccessFilter + public struct AccessNoPose : IBodyAccessFilter { public bool GatherPosition => false; public bool GatherOrientation => false; @@ -54,6 +54,15 @@ public struct AccessNoPoses : IBodyAccessFilter public bool AccessLinearVelocity => true; public bool AccessAngularVelocity => true; } + public struct AccessNoOrientation : IBodyAccessFilter + { + public bool GatherPosition => true; + public bool GatherOrientation => false; + public bool GatherMass => true; + public bool GatherInertiaTensor => true; + public bool AccessLinearVelocity => true; + public bool AccessAngularVelocity => true; + } public struct AccessOnlyAngular : IBodyAccessFilter { diff --git a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs index 6f378d20d..9701262e7 100644 --- a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs @@ -278,9 +278,9 @@ public unsafe override void WarmStart2(bodies, ref integratorCallbacks, ref integrationFlags, 0, dt, workerIndex, i, ref references, count, out var positionA, out var orientationA, out var wsvA, out var inertiaA); - if (typeof(TAllowPoseIntegration) == typeof(AllowPoseIntegration)) - function.UpdateForNewPose(positionA, orientationA, inertiaA, wsvA, new Vector(dt), accumulatedImpulses, ref prestep); - ; + //if (typeof(TAllowPoseIntegration) == typeof(AllowPoseIntegration)) + // function.UpdateForNewPose(positionA, orientationA, inertiaA, wsvA, new Vector(dt), accumulatedImpulses, ref prestep); + function.WarmStart2(positionA, orientationA, inertiaA, prestep, accumulatedImpulses, ref wsvA); if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) @@ -345,7 +345,7 @@ public override void JacobiPrestep2(ref TypeBatch typeBatch, Bodies bodies, ref } public abstract class OneBodyContactTypeProcessor - : OneBodyTypeProcessor + : OneBodyTypeProcessor where TPrestepData : unmanaged where TProjection : unmanaged where TAccumulatedImpulse : unmanaged where TConstraintFunctions : unmanaged, IOneBodyContactConstraintFunctions { diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index bfa641192..ef60f84df 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -390,8 +390,8 @@ public unsafe override void WarmStart2(bodies, ref integratorCallbacks, ref integrationFlags, 1, dt, workerIndex, i, ref references.IndexB, count, out var positionB, out var orientationB, out var wsvB, out var inertiaB); - if (typeof(TAllowPoseIntegration) == typeof(AllowPoseIntegration)) - function.UpdateForNewPose(positionA, orientationA, inertiaA, wsvA, positionB, orientationB, inertiaB, wsvB, new Vector(dt), accumulatedImpulses, ref prestep); + //if (typeof(TAllowPoseIntegration) == typeof(AllowPoseIntegration)) + // function.UpdateForNewPose(positionA, orientationA, inertiaA, wsvA, positionB, orientationB, inertiaB, wsvB, new Vector(dt), accumulatedImpulses, ref prestep); function.WarmStart2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, prestep, accumulatedImpulses, ref wsvA, ref wsvB); @@ -464,7 +464,7 @@ public override void JacobiPrestep2(ref TypeBatch typeBatch, Bodies bodies, ref } public abstract class TwoBodyContactTypeProcessor - : TwoBodyTypeProcessor + : TwoBodyTypeProcessor where TPrestepData : unmanaged where TProjection : unmanaged where TAccumulatedImpulse : unmanaged where TConstraintFunctions : unmanaged, IContactConstraintFunctions { diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index 55c691c47..f9348cafe 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -281,267 +281,276 @@ public override unsafe IndexSet PrepareConstraintIntegrationResponsibilities(int { //TODO: we're caching it on a per call basis because we are still using the old substeppingtimestepper frame externally. Once we bite the bullet 100% on bundling, we can make it equivalent to IterationCount. this.substepCount = substepCount; - pool.Take(ActiveSet.Batches.Count, out integrationFlags); - integrationFlags[0] = default; - pool.Take(ActiveSet.Batches.Count, out coarseBatchIntegrationResponsibilities); - for (int batchIndex = 1; batchIndex < integrationFlags.Length; ++batchIndex) + if (ActiveSet.Batches.Count > 0) { - ref var batch = ref ActiveSet.Batches[batchIndex]; - ref var flagsForBatch = ref integrationFlags[batchIndex]; - pool.Take(batch.TypeBatches.Count, out flagsForBatch); - pool.Take(batch.TypeBatches.Count, out coarseBatchIntegrationResponsibilities[batchIndex]); - for (int typeBatchIndex = 0; typeBatchIndex < flagsForBatch.Length; ++typeBatchIndex) + pool.Take(ActiveSet.Batches.Count, out integrationFlags); + integrationFlags[0] = default; + pool.Take(ActiveSet.Batches.Count, out coarseBatchIntegrationResponsibilities); + for (int batchIndex = 1; batchIndex < integrationFlags.Length; ++batchIndex) { - ref var flagsForTypeBatch = ref flagsForBatch[typeBatchIndex]; - ref var typeBatch = ref batch.TypeBatches[typeBatchIndex]; - var bodiesPerConstraint = TypeProcessors[typeBatch.TypeId].BodiesPerConstraint; - pool.Take(bodiesPerConstraint, out flagsForTypeBatch); - for (int bodyIndexInConstraint = 0; bodyIndexInConstraint < bodiesPerConstraint; ++bodyIndexInConstraint) + ref var batch = ref ActiveSet.Batches[batchIndex]; + ref var flagsForBatch = ref integrationFlags[batchIndex]; + pool.Take(batch.TypeBatches.Count, out flagsForBatch); + pool.Take(batch.TypeBatches.Count, out coarseBatchIntegrationResponsibilities[batchIndex]); + for (int typeBatchIndex = 0; typeBatchIndex < flagsForBatch.Length; ++typeBatchIndex) { - flagsForTypeBatch[bodyIndexInConstraint] = new IndexSet(pool, typeBatch.ConstraintCount); + ref var flagsForTypeBatch = ref flagsForBatch[typeBatchIndex]; + ref var typeBatch = ref batch.TypeBatches[typeBatchIndex]; + var bodiesPerConstraint = TypeProcessors[typeBatch.TypeId].BodiesPerConstraint; + pool.Take(bodiesPerConstraint, out flagsForTypeBatch); + for (int bodyIndexInConstraint = 0; bodyIndexInConstraint < bodiesPerConstraint; ++bodyIndexInConstraint) + { + flagsForTypeBatch[bodyIndexInConstraint] = new IndexSet(pool, typeBatch.ConstraintCount); + } } } - } - //Brute force fallback for debugging: - //for (int i = 0; i < bodies.ActiveSet.Count; ++i) - //{ - // ref var constraints = ref bodies.ActiveSet.Constraints[i]; - // ConstraintHandle minimumConstraint; - // minimumConstraint.Value = -1; - // int minimumBatchIndex = int.MaxValue; - // int minimumIndexInConstraint = -1; - // for (int j = 0; j < constraints.Count; ++j) - // { - // ref var constraint = ref constraints[j]; - // var batchIndex = HandleToConstraint[constraint.ConnectingConstraintHandle.Value].BatchIndex; - // if (batchIndex < minimumBatchIndex) - // { - // minimumBatchIndex = batchIndex; - // minimumIndexInConstraint = constraint.BodyIndexInConstraint; - // minimumConstraint = constraint.ConnectingConstraintHandle; - // } - // } - // if (minimumConstraint.Value >= 0) - // { - // ref var location = ref HandleToConstraint[minimumConstraint.Value]; - // var typeBatchIndex = ActiveSet.Batches[location.BatchIndex].TypeIndexToTypeBatchIndex[location.TypeId]; - // ref var indexSet = ref integrationFlags[location.BatchIndex][typeBatchIndex][minimumIndexInConstraint]; - // indexSet.AddUnsafely(location.IndexInTypeBatch); - // } - //} - - pool.Take(batchReferencedHandles.Count, out bodiesFirstObservedInBatches); - //We don't have to consider the first batch, since we know ahead of time that the first batch will be the first time we see any bodies in it. - //Just copy directly from the first batch into the merged to initialize it. - pool.Take((bodies.HandlePool.HighestPossiblyClaimedId + 63) / 64, out mergedConstrainedBodyHandles.Flags); - var copyLength = Math.Min(mergedConstrainedBodyHandles.Flags.Length, batchReferencedHandles[0].Flags.Length); - batchReferencedHandles[0].Flags.CopyTo(0, mergedConstrainedBodyHandles.Flags, 0, copyLength); - batchReferencedHandles[0].Flags.Clear(copyLength, batchReferencedHandles[0].Flags.Length - copyLength); - - //Yup, we're just leaving the first slot unallocated to avoid having to offset indices all over the place. Slight wonk, but not a big deal. - bodiesFirstObservedInBatches[0] = default; - pool.Take(batchReferencedHandles.Count, out var batchHasAnyIntegrationResponsibilities); - for (int batchIndex = 1; batchIndex < bodiesFirstObservedInBatches.Length; ++batchIndex) - { - ref var batchHandles = ref batchReferencedHandles[batchIndex]; - var bundleCount = Math.Min(mergedConstrainedBodyHandles.Flags.Length, batchHandles.Flags.Length); - //Note that we bypass the constructor to avoid zeroing unnecessarily. Every bundle will be fully assigned. - pool.Take(bundleCount, out bodiesFirstObservedInBatches[batchIndex].Flags); - } - //Note that we are not multithreading the batch merging phase. This typically takes a handful of microseconds. - //You'd likely need millions of bodies before you'd see any substantial benefit from multithreading this. - for (int batchIndex = 1; batchIndex < ActiveSet.Batches.Count; ++batchIndex) - { - ref var batchHandles = ref batchReferencedHandles[batchIndex]; - ref var firstObservedInBatch = ref bodiesFirstObservedInBatches[batchIndex]; - var flagBundleCount = Math.Min(mergedConstrainedBodyHandles.Flags.Length, batchHandles.Flags.Length); - if (Avx2.IsSupported) + //Brute force fallback for debugging: + //for (int i = 0; i < bodies.ActiveSet.Count; ++i) + //{ + // ref var constraints = ref bodies.ActiveSet.Constraints[i]; + // ConstraintHandle minimumConstraint; + // minimumConstraint.Value = -1; + // int minimumBatchIndex = int.MaxValue; + // int minimumIndexInConstraint = -1; + // for (int j = 0; j < constraints.Count; ++j) + // { + // ref var constraint = ref constraints[j]; + // var batchIndex = HandleToConstraint[constraint.ConnectingConstraintHandle.Value].BatchIndex; + // if (batchIndex < minimumBatchIndex) + // { + // minimumBatchIndex = batchIndex; + // minimumIndexInConstraint = constraint.BodyIndexInConstraint; + // minimumConstraint = constraint.ConnectingConstraintHandle; + // } + // } + // if (minimumConstraint.Value >= 0) + // { + // ref var location = ref HandleToConstraint[minimumConstraint.Value]; + // var typeBatchIndex = ActiveSet.Batches[location.BatchIndex].TypeIndexToTypeBatchIndex[location.TypeId]; + // ref var indexSet = ref integrationFlags[location.BatchIndex][typeBatchIndex][minimumIndexInConstraint]; + // indexSet.AddUnsafely(location.IndexInTypeBatch); + // } + //} + + pool.Take(batchReferencedHandles.Count, out bodiesFirstObservedInBatches); + //We don't have to consider the first batch, since we know ahead of time that the first batch will be the first time we see any bodies in it. + //Just copy directly from the first batch into the merged to initialize it. + pool.Take((bodies.HandlePool.HighestPossiblyClaimedId + 63) / 64, out mergedConstrainedBodyHandles.Flags); + var copyLength = Math.Min(mergedConstrainedBodyHandles.Flags.Length, batchReferencedHandles[0].Flags.Length); + batchReferencedHandles[0].Flags.CopyTo(0, mergedConstrainedBodyHandles.Flags, 0, copyLength); + batchReferencedHandles[0].Flags.Clear(copyLength, batchReferencedHandles[0].Flags.Length - copyLength); + + //Yup, we're just leaving the first slot unallocated to avoid having to offset indices all over the place. Slight wonk, but not a big deal. + bodiesFirstObservedInBatches[0] = default; + pool.Take(batchReferencedHandles.Count, out var batchHasAnyIntegrationResponsibilities); + for (int batchIndex = 1; batchIndex < bodiesFirstObservedInBatches.Length; ++batchIndex) { - var avxBundleCount = flagBundleCount / 4; - var horizontalAvxMerge = Vector256.Zero; - for (int avxBundleIndex = 0; avxBundleIndex < avxBundleCount; ++avxBundleIndex) + ref var batchHandles = ref batchReferencedHandles[batchIndex]; + var bundleCount = Math.Min(mergedConstrainedBodyHandles.Flags.Length, batchHandles.Flags.Length); + //Note that we bypass the constructor to avoid zeroing unnecessarily. Every bundle will be fully assigned. + pool.Take(bundleCount, out bodiesFirstObservedInBatches[batchIndex].Flags); + } + //Note that we are not multithreading the batch merging phase. This typically takes a handful of microseconds. + //You'd likely need millions of bodies before you'd see any substantial benefit from multithreading this. + for (int batchIndex = 1; batchIndex < ActiveSet.Batches.Count; ++batchIndex) + { + ref var batchHandles = ref batchReferencedHandles[batchIndex]; + ref var firstObservedInBatch = ref bodiesFirstObservedInBatches[batchIndex]; + var flagBundleCount = Math.Min(mergedConstrainedBodyHandles.Flags.Length, batchHandles.Flags.Length); + if (Avx2.IsSupported) { - var mergeBundle = ((Vector256*)mergedConstrainedBodyHandles.Flags.Memory)[avxBundleIndex]; - var batchBundle = ((Vector256*)batchHandles.Flags.Memory)[avxBundleIndex]; - ((Vector256*)mergedConstrainedBodyHandles.Flags.Memory)[avxBundleIndex] = Avx2.Or(mergeBundle, batchBundle); - //If this batch contains a body, and the merged set does not, then it's the first batch that sees a body and it will have integration responsibility. - var firstObservedBundle = Avx2.AndNot(mergeBundle, batchBundle); - horizontalAvxMerge = Avx2.Or(firstObservedBundle, horizontalAvxMerge); - ((Vector256*)firstObservedInBatch.Flags.Memory)[avxBundleIndex] = firstObservedBundle; - } - var notEqual = Avx2.CompareNotEqual(horizontalAvxMerge.AsDouble(), Vector256.Zero); - ulong horizontalMerge = (ulong)Avx.MoveMask(notEqual); + var avxBundleCount = flagBundleCount / 4; + var horizontalAvxMerge = Vector256.Zero; + for (int avxBundleIndex = 0; avxBundleIndex < avxBundleCount; ++avxBundleIndex) + { + var mergeBundle = ((Vector256*)mergedConstrainedBodyHandles.Flags.Memory)[avxBundleIndex]; + var batchBundle = ((Vector256*)batchHandles.Flags.Memory)[avxBundleIndex]; + ((Vector256*)mergedConstrainedBodyHandles.Flags.Memory)[avxBundleIndex] = Avx2.Or(mergeBundle, batchBundle); + //If this batch contains a body, and the merged set does not, then it's the first batch that sees a body and it will have integration responsibility. + var firstObservedBundle = Avx2.AndNot(mergeBundle, batchBundle); + horizontalAvxMerge = Avx2.Or(firstObservedBundle, horizontalAvxMerge); + ((Vector256*)firstObservedInBatch.Flags.Memory)[avxBundleIndex] = firstObservedBundle; + } + var notEqual = Avx2.CompareNotEqual(horizontalAvxMerge.AsDouble(), Vector256.Zero); + ulong horizontalMerge = (ulong)Avx.MoveMask(notEqual); - //Cleanup loop. - for (int flagBundleIndex = avxBundleCount * 4; flagBundleIndex < flagBundleCount; ++flagBundleIndex) + //Cleanup loop. + for (int flagBundleIndex = avxBundleCount * 4; flagBundleIndex < flagBundleCount; ++flagBundleIndex) + { + var mergeBundle = mergedConstrainedBodyHandles.Flags[flagBundleIndex]; + var batchBundle = batchHandles.Flags[flagBundleIndex]; + mergedConstrainedBodyHandles.Flags[flagBundleIndex] = mergeBundle | batchBundle; + //If this batch contains a body, and the merged set does not, then it's the first batch that sees a body and it will have integration responsibility. + var firstObservedBundle = ~mergeBundle & batchBundle; + horizontalMerge |= firstObservedBundle; + firstObservedInBatch.Flags[flagBundleIndex] = firstObservedBundle; + } + batchHasAnyIntegrationResponsibilities[batchIndex] = horizontalMerge != 0; + } + else { - var mergeBundle = mergedConstrainedBodyHandles.Flags[flagBundleIndex]; - var batchBundle = batchHandles.Flags[flagBundleIndex]; - mergedConstrainedBodyHandles.Flags[flagBundleIndex] = mergeBundle | batchBundle; - //If this batch contains a body, and the merged set does not, then it's the first batch that sees a body and it will have integration responsibility. - var firstObservedBundle = ~mergeBundle & batchBundle; - horizontalMerge |= firstObservedBundle; - firstObservedInBatch.Flags[flagBundleIndex] = firstObservedBundle; + ulong horizontalMerge = 0; + for (int flagBundleIndex = 0; flagBundleIndex < flagBundleCount; ++flagBundleIndex) + { + var mergeBundle = mergedConstrainedBodyHandles.Flags[flagBundleIndex]; + var batchBundle = batchHandles.Flags[flagBundleIndex]; + mergedConstrainedBodyHandles.Flags[flagBundleIndex] = mergeBundle | batchBundle; + //If this batch contains a body, and the merged set does not, then it's the first batch that sees a body and it will have integration responsibility. + var firstObservedBundle = ~mergeBundle & batchBundle; + horizontalMerge |= firstObservedBundle; + firstObservedInBatch.Flags[flagBundleIndex] = firstObservedBundle; + } + batchHasAnyIntegrationResponsibilities[batchIndex] = horizontalMerge != 0; } - batchHasAnyIntegrationResponsibilities[batchIndex] = horizontalMerge != 0; } - else + //var start = Stopwatch.GetTimestamp(); + //We now have index sets representing the first time each body handle is observed in a batch. + //This process is significantly more expensive than the batch merging phase and can benefit from multithreading. + //It is still fairly cheap, though- we can't use really fine grained jobs or the cost of swapping jobs will exceed productive work. + + //Note that we arbitrarily use single threaded execution if the job is small enough. Dispatching isn't free. + bool useSingleThreadedPath = true; + if (threadDispatcher != null && threadDispatcher.ThreadCount > 1) { - ulong horizontalMerge = 0; - for (int flagBundleIndex = 0; flagBundleIndex < flagBundleCount; ++flagBundleIndex) + integrationResponsibilityPrepassJobs = new(128, pool); + int constraintCount = 0; + const int targetJobSize = 2048; + Debug.Assert(targetJobSize % 64 == 0, "Target job size must be a multiple of the index set bundles to avoid threads working on the same flag bundle."); + for (int batchIndex = 1; batchIndex < bodiesFirstObservedInBatches.Length; ++batchIndex) { - var mergeBundle = mergedConstrainedBodyHandles.Flags[flagBundleIndex]; - var batchBundle = batchHandles.Flags[flagBundleIndex]; - mergedConstrainedBodyHandles.Flags[flagBundleIndex] = mergeBundle | batchBundle; - //If this batch contains a body, and the merged set does not, then it's the first batch that sees a body and it will have integration responsibility. - var firstObservedBundle = ~mergeBundle & batchBundle; - horizontalMerge |= firstObservedBundle; - firstObservedInBatch.Flags[flagBundleIndex] = firstObservedBundle; + if (!batchHasAnyIntegrationResponsibilities[batchIndex]) + continue; + ref var batch = ref ActiveSet.Batches[batchIndex]; + for (int typeBatchIndex = 0; typeBatchIndex < batch.TypeBatches.Count; ++typeBatchIndex) + { + ref var typeBatch = ref batch.TypeBatches[typeBatchIndex]; + constraintCount += typeBatch.ConstraintCount; + int jobCountForTypeBatch = (typeBatch.ConstraintCount + targetJobSize - 1) / targetJobSize; + for (int i = 0; i < jobCountForTypeBatch; ++i) + { + var jobStart = i * targetJobSize; + var jobEnd = Math.Min(jobStart + targetJobSize, typeBatch.ConstraintCount); + integrationResponsibilityPrepassJobs.Allocate(pool) = (batchIndex, typeBatchIndex, jobStart, jobEnd); + } + } } - batchHasAnyIntegrationResponsibilities[batchIndex] = horizontalMerge != 0; - } - } - //var start = Stopwatch.GetTimestamp(); - //We now have index sets representing the first time each body handle is observed in a batch. - //This process is significantly more expensive than the batch merging phase and can benefit from multithreading. - //It is still fairly cheap, though- we can't use really fine grained jobs or the cost of swapping jobs will exceed productive work. - - //Note that we arbitrarily use single threaded execution if the job is small enough. Dispatching isn't free. - bool useSingleThreadedPath = true; - if (threadDispatcher != null && threadDispatcher.ThreadCount > 1) - { - integrationResponsibilityPrepassJobs = new(128, pool); - int constraintCount = 0; - const int targetJobSize = 2048; - Debug.Assert(targetJobSize % 64 == 0, "Target job size must be a multiple of the index set bundles to avoid threads working on the same flag bundle."); - for (int batchIndex = 1; batchIndex < bodiesFirstObservedInBatches.Length; ++batchIndex) - { - if (!batchHasAnyIntegrationResponsibilities[batchIndex]) - continue; - ref var batch = ref ActiveSet.Batches[batchIndex]; - for (int typeBatchIndex = 0; typeBatchIndex < batch.TypeBatches.Count; ++typeBatchIndex) + if (constraintCount > 4096 + threadDispatcher.ThreadCount * 1024) { - ref var typeBatch = ref batch.TypeBatches[typeBatchIndex]; - constraintCount += typeBatch.ConstraintCount; - int jobCountForTypeBatch = (typeBatch.ConstraintCount + targetJobSize - 1) / targetJobSize; - for (int i = 0; i < jobCountForTypeBatch; ++i) + nextConstraintIntegrationResponsibilityJobIndex = 0; + useSingleThreadedPath = false; + pool.Take(integrationResponsibilityPrepassJobs.Count, out jobAlignedIntegrationResponsibilities); + //for (int i = 0; i < integrationResponsibilityPrepassJobs.Count; ++i) + //{ + // ref var job = ref integrationResponsibilityPrepassJobs[i]; + // jobAlignedIntegrationResponsibilities[i] = ComputeIntegrationResponsibilitiesForConstraintRegion(job.batch, job.typeBatch, job.start, job.end); + //} + threadDispatcher.DispatchWorkers(constraintIntegrationResponsibilitiesWorker); + + //Coarse batch integration responsibilities start uninitialized. Possible to have multiple jobs per type batch in multithreaded case, so we need to init to merge. + for (int i = 1; i < ActiveSet.Batches.Count; ++i) + { + ref var batch = ref ActiveSet.Batches[i]; + for (int j = 0; j < batch.TypeBatches.Count; ++j) + { + coarseBatchIntegrationResponsibilities[i][j] = false; + } + } + for (int i = 0; i < integrationResponsibilityPrepassJobs.Count; ++i) { - var jobStart = i * targetJobSize; - var jobEnd = Math.Min(jobStart + targetJobSize, typeBatch.ConstraintCount); - integrationResponsibilityPrepassJobs.Allocate(pool) = (batchIndex, typeBatchIndex, jobStart, jobEnd); + ref var job = ref integrationResponsibilityPrepassJobs[i]; + coarseBatchIntegrationResponsibilities[job.batch][job.typeBatch] |= jobAlignedIntegrationResponsibilities[i]; } + pool.Return(ref jobAlignedIntegrationResponsibilities); } + integrationResponsibilityPrepassJobs.Dispose(pool); } - if (constraintCount > 4096 + threadDispatcher.ThreadCount * 1024) + if (useSingleThreadedPath) { - nextConstraintIntegrationResponsibilityJobIndex = 0; - useSingleThreadedPath = false; - pool.Take(integrationResponsibilityPrepassJobs.Count, out jobAlignedIntegrationResponsibilities); - //for (int i = 0; i < integrationResponsibilityPrepassJobs.Count; ++i) - //{ - // ref var job = ref integrationResponsibilityPrepassJobs[i]; - // jobAlignedIntegrationResponsibilities[i] = ComputeIntegrationResponsibilitiesForConstraintRegion(job.batch, job.typeBatch, job.start, job.end); - //} - threadDispatcher.DispatchWorkers(constraintIntegrationResponsibilitiesWorker); - - //Coarse batch integration responsibilities start uninitialized. Possible to have multiple jobs per type batch in multithreaded case, so we need to init to merge. for (int i = 1; i < ActiveSet.Batches.Count; ++i) { + if (!batchHasAnyIntegrationResponsibilities[i]) + continue; ref var batch = ref ActiveSet.Batches[i]; for (int j = 0; j < batch.TypeBatches.Count; ++j) { - coarseBatchIntegrationResponsibilities[i][j] = false; + ref var typeBatch = ref batch.TypeBatches[j]; + coarseBatchIntegrationResponsibilities[i][j] = ComputeIntegrationResponsibilitiesForConstraintRegion(i, j, 0, typeBatch.ConstraintCount); } } - for (int i = 0; i < integrationResponsibilityPrepassJobs.Count; ++i) - { - ref var job = ref integrationResponsibilityPrepassJobs[i]; - coarseBatchIntegrationResponsibilities[job.batch][job.typeBatch] |= jobAlignedIntegrationResponsibilities[i]; - } - pool.Return(ref jobAlignedIntegrationResponsibilities); } - integrationResponsibilityPrepassJobs.Dispose(pool); - } - if (useSingleThreadedPath) - { - for (int i = 1; i < ActiveSet.Batches.Count; ++i) + //var end = Stopwatch.GetTimestamp(); + //Console.WriteLine($"time (ms): {(end - start) * 1e3 / Stopwatch.Frequency}"); + pool.Return(ref batchHasAnyIntegrationResponsibilities); + + ////Validation: + //for (int i = 0; i < bodies.ActiveSet.Count; ++i) + //{ + // ref var constraints = ref bodies.ActiveSet.Constraints[i]; + // ConstraintHandle minimumConstraint; + // minimumConstraint.Value = -1; + // int minimumBatchIndex = int.MaxValue; + // int minimumIndexInConstraint = -1; + // for (int j = 0; j < constraints.Count; ++j) + // { + // ref var constraint = ref constraints[j]; + // var batchIndex = HandleToConstraint[constraint.ConnectingConstraintHandle.Value].BatchIndex; + // if (batchIndex < minimumBatchIndex) + // { + // minimumBatchIndex = batchIndex; + // minimumIndexInConstraint = constraint.BodyIndexInConstraint; + // minimumConstraint = constraint.ConnectingConstraintHandle; + // } + // } + // if (minimumConstraint.Value >= 0) + // { + // ref var location = ref HandleToConstraint[minimumConstraint.Value]; + // var typeBatchIndex = ActiveSet.Batches[location.BatchIndex].TypeIndexToTypeBatchIndex[location.TypeId]; + // if (location.BatchIndex > 0) + // { + // ref var indexSet = ref integrationFlags[location.BatchIndex][typeBatchIndex][minimumIndexInConstraint]; + // Debug.Assert(indexSet.Contains(location.IndexInTypeBatch)); + // } + // } + //} + + Debug.Assert(!bodiesFirstObservedInBatches[0].Flags.Allocated, "Remember, we're assuming we're just leaving the first batch's slot empty to avoid indexing complexity."); + for (int batchIndex = 1; batchIndex < bodiesFirstObservedInBatches.Length; ++batchIndex) { - if (!batchHasAnyIntegrationResponsibilities[i]) - continue; - ref var batch = ref ActiveSet.Batches[i]; - for (int j = 0; j < batch.TypeBatches.Count; ++j) - { - ref var typeBatch = ref batch.TypeBatches[j]; - coarseBatchIntegrationResponsibilities[i][j] = ComputeIntegrationResponsibilitiesForConstraintRegion(i, j, 0, typeBatch.ConstraintCount); - } + bodiesFirstObservedInBatches[batchIndex].Dispose(pool); } + pool.Return(ref bodiesFirstObservedInBatches); + return mergedConstrainedBodyHandles; } - //var end = Stopwatch.GetTimestamp(); - //Console.WriteLine($"time (ms): {(end - start) * 1e3 / Stopwatch.Frequency}"); - pool.Return(ref batchHasAnyIntegrationResponsibilities); - - ////Validation: - //for (int i = 0; i < bodies.ActiveSet.Count; ++i) - //{ - // ref var constraints = ref bodies.ActiveSet.Constraints[i]; - // ConstraintHandle minimumConstraint; - // minimumConstraint.Value = -1; - // int minimumBatchIndex = int.MaxValue; - // int minimumIndexInConstraint = -1; - // for (int j = 0; j < constraints.Count; ++j) - // { - // ref var constraint = ref constraints[j]; - // var batchIndex = HandleToConstraint[constraint.ConnectingConstraintHandle.Value].BatchIndex; - // if (batchIndex < minimumBatchIndex) - // { - // minimumBatchIndex = batchIndex; - // minimumIndexInConstraint = constraint.BodyIndexInConstraint; - // minimumConstraint = constraint.ConnectingConstraintHandle; - // } - // } - // if (minimumConstraint.Value >= 0) - // { - // ref var location = ref HandleToConstraint[minimumConstraint.Value]; - // var typeBatchIndex = ActiveSet.Batches[location.BatchIndex].TypeIndexToTypeBatchIndex[location.TypeId]; - // if (location.BatchIndex > 0) - // { - // ref var indexSet = ref integrationFlags[location.BatchIndex][typeBatchIndex][minimumIndexInConstraint]; - // Debug.Assert(indexSet.Contains(location.IndexInTypeBatch)); - // } - // } - //} - - Debug.Assert(!bodiesFirstObservedInBatches[0].Flags.Allocated, "Remember, we're assuming we're just leaving the first batch's slot empty to avoid indexing complexity."); - for (int batchIndex = 1; batchIndex < bodiesFirstObservedInBatches.Length; ++batchIndex) + else { - bodiesFirstObservedInBatches[batchIndex].Dispose(pool); + return new IndexSet(); } - pool.Return(ref bodiesFirstObservedInBatches); - return mergedConstrainedBodyHandles; } public override void DisposeConstraintIntegrationResponsibilities() { - Debug.Assert(!integrationFlags[0].Allocated, "Remember, we're assuming we're just leaving the first batch's slot empty to avoid indexing complexity."); - for (int batchIndex = 1; batchIndex < integrationFlags.Length; ++batchIndex) + if (ActiveSet.Batches.Count > 0) { - ref var flagsForBatch = ref integrationFlags[batchIndex]; - for (int typeBatchIndex = 0; typeBatchIndex < flagsForBatch.Length; ++typeBatchIndex) + Debug.Assert(!integrationFlags[0].Allocated, "Remember, we're assuming we're just leaving the first batch's slot empty to avoid indexing complexity."); + for (int batchIndex = 1; batchIndex < integrationFlags.Length; ++batchIndex) { - ref var flagsForTypeBatch = ref flagsForBatch[typeBatchIndex]; - for (int bodyIndexInConstraint = 0; bodyIndexInConstraint < flagsForTypeBatch.Length; ++bodyIndexInConstraint) + ref var flagsForBatch = ref integrationFlags[batchIndex]; + for (int typeBatchIndex = 0; typeBatchIndex < flagsForBatch.Length; ++typeBatchIndex) { - flagsForTypeBatch[bodyIndexInConstraint].Dispose(pool); + ref var flagsForTypeBatch = ref flagsForBatch[typeBatchIndex]; + for (int bodyIndexInConstraint = 0; bodyIndexInConstraint < flagsForTypeBatch.Length; ++bodyIndexInConstraint) + { + flagsForTypeBatch[bodyIndexInConstraint].Dispose(pool); + } + pool.Return(ref flagsForTypeBatch); } - pool.Return(ref flagsForTypeBatch); + pool.Return(ref flagsForBatch); + pool.Return(ref coarseBatchIntegrationResponsibilities[batchIndex]); } - pool.Return(ref flagsForBatch); - pool.Return(ref coarseBatchIntegrationResponsibilities[batchIndex]); + pool.Return(ref integrationFlags); + pool.Return(ref coarseBatchIntegrationResponsibilities); + mergedConstrainedBodyHandles.Dispose(pool); } - pool.Return(ref integrationFlags); - pool.Return(ref coarseBatchIntegrationResponsibilities); - mergedConstrainedBodyHandles.Dispose(pool); - } public override void SolveStep2(float totalDt, IThreadDispatcher threadDispatcher = null) diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index 2bcec515d..49a666741 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -838,7 +838,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //terboShape.ComputeInertia(1, out var terboInertia); //var bodyDescription = BodyDescription.CreateDynamic(new RigidPose(new Vector3(0, 10, 0)), new BodyVelocity(default, new Vector3(1, 2, 3)), terboInertia, new CollidableDescription(Simulation.Shapes.Add(terboShape), 0.1f), new BodyActivityDescription(-1)); - for (int i = 0; i < 40; ++i) + for (int i = 0; i < 1; ++i) { //CreateDeformable(Simulation, new Vector3(i * 3, 5 + i * 1.5f, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), MathF.PI * (i * 0.55f)), 1f, cellSize, weldSpringiness, volumeSpringiness, i, filters, ref vertices, ref vertexSpatialIndices, ref cellVertexIndices, ref tetrahedraVertexIndices); CreateDeformable(Simulation, new Vector3(i * 3, cellSize * 2f + i * 0f, 0), Quaternion.Identity, 1f, cellSize, weldSpringiness, volumeSpringiness, i, filters, ref vertices, ref vertexSpatialIndices, ref cellVertexIndices, ref tetrahedraVertexIndices); diff --git a/Demos/Demos/PyramidDemo.cs b/Demos/Demos/PyramidDemo.cs index f060be8e3..8430e1ee6 100644 --- a/Demos/Demos/PyramidDemo.cs +++ b/Demos/Demos/PyramidDemo.cs @@ -25,7 +25,9 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //The PositionFirstTimestepper is the simplest timestepping mode, but since it integrates velocity into position at the start of the frame, directly modified velocities outside of the timestep //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + //Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), solverIterationCount: 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper2(3), solverIterationCount: 1); + //Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); var boxShape = new Box(1, 1, 1); boxShape.ComputeInertia(1, out var boxInertia); From c94a068148f80a4241f722c8be827bd93bb242d6 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 24 Jul 2021 21:57:55 -0500 Subject: [PATCH 143/947] Checkpoint! --- Demos/Demo.cs | 4 ++-- Demos/Demos/NewtDemo.cs | 7 ++++--- Demos/Program.cs | 2 +- 3 files changed, 7 insertions(+), 6 deletions(-) diff --git a/Demos/Demo.cs b/Demos/Demo.cs index 37ebe5190..88b3e17f6 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -43,7 +43,7 @@ protected Demo() //It may be worth using something like hwloc to extract extra information to reason about. //var targetThreadCount = Math.Max(1, Environment.ProcessorCount > 4 ? Environment.ProcessorCount - 2 : Environment.ProcessorCount - 1); - var targetThreadCount = 30; + var targetThreadCount = 3; ThreadDispatcher = new SimpleThreadDispatcher(targetThreadCount); } @@ -62,7 +62,7 @@ public virtual void Update(Window window, Camera camera, Input input, float dt) //fully decouple simulation and rendering rates across different threads. //(In either case, you'd also want to interpolate or extrapolate simulation results during rendering for smoothness.) //Note that taking steps of variable length can reduce stability. Gradual or one-off changes can work reasonably well. - Simulation.Timestep(1 / 60f, ThreadDispatcher); + Simulation.Timestep(1 / 60f); ////Here's an example of how it would look to use more frequent updates, but still with a fixed amount of time simulated per update call: //const float timeToSimulate = 1 / 60f; diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index 49a666741..6b9eeba54 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -7,6 +7,7 @@ using BepuUtilities.Memory; using DemoContentLoader; using DemoRenderer; +using DemoUtilities; using System; using System.Diagnostics; using System.Numerics; @@ -824,8 +825,8 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper(1)); Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), solverIterationCount: 1); - //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)), new PositionFirstTimestepper2(), solverIterationCount: 5); - //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)), new SubsteppingTimestepper2(3), solverIterationCount: 1); + //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper2(), solverIterationCount: 60); + //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)), new SubsteppingTimestepper2(10), solverIterationCount: 1); var meshContent = content.Load("Content\\newt.obj"); float cellSize = 0.1f; @@ -838,7 +839,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //terboShape.ComputeInertia(1, out var terboInertia); //var bodyDescription = BodyDescription.CreateDynamic(new RigidPose(new Vector3(0, 10, 0)), new BodyVelocity(default, new Vector3(1, 2, 3)), terboInertia, new CollidableDescription(Simulation.Shapes.Add(terboShape), 0.1f), new BodyActivityDescription(-1)); - for (int i = 0; i < 1; ++i) + for (int i = 0; i < 40; ++i) { //CreateDeformable(Simulation, new Vector3(i * 3, 5 + i * 1.5f, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), MathF.PI * (i * 0.55f)), 1f, cellSize, weldSpringiness, volumeSpringiness, i, filters, ref vertices, ref vertexSpatialIndices, ref cellVertexIndices, ref tetrahedraVertexIndices); CreateDeformable(Simulation, new Vector3(i * 3, cellSize * 2f + i * 0f, 0), Quaternion.Identity, 1f, cellSize, weldSpringiness, volumeSpringiness, i, filters, ref vertices, ref vertexSpatialIndices, ref cellVertexIndices, ref tetrahedraVertexIndices); diff --git a/Demos/Program.cs b/Demos/Program.cs index 7118826dd..6c769ece5 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -19,7 +19,7 @@ static void Main(string[] args) { content = ContentArchive.Load(stream); } - //HeadlessTest.Test(content, 4, 32, 512); + HeadlessTest.Test(content, 4, 32, 512); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); From cc8046e62ad51e2d35df7b09a7d3975209bebd7b Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 3 Aug 2021 18:37:19 -0500 Subject: [PATCH 144/947] Does not build. Halfway through an attempt at dealing with contacts in substepping. --- .../Contact/ContactConvexCommon.cs | 7 +- .../Constraints/Contact/ContactConvexTypes.cs | 1242 +++++++++-------- .../Constraints/Contact/ContactConvexTypes.tt | 171 ++- .../Contact/IContactConstraintDescription.cs | 7 +- Demos/Demo.cs | 4 +- 5 files changed, 759 insertions(+), 672 deletions(-) diff --git a/BepuPhysics/Constraints/Contact/ContactConvexCommon.cs b/BepuPhysics/Constraints/Contact/ContactConvexCommon.cs index bb6776a28..3706f526a 100644 --- a/BepuPhysics/Constraints/Contact/ContactConvexCommon.cs +++ b/BepuPhysics/Constraints/Contact/ContactConvexCommon.cs @@ -9,8 +9,9 @@ namespace BepuPhysics.Constraints.Contact { public struct ConvexContactWide { - public Vector3Wide OffsetA; + public Vector3Wide LocalOffsetA; public Vector Depth; + public Vector PlaneOffsetB; } public struct MaterialPropertiesWide @@ -30,14 +31,14 @@ public interface IContactPrestep where TPrestep : struct, IContactPres public interface IConvexContactPrestep : IContactPrestep where TPrestep : struct, IConvexContactPrestep { - ref Vector3Wide GetNormal(ref TPrestep prestep); + ref Vector3Wide GetLocalNormalB(ref TPrestep prestep); ref ConvexContactWide GetContact(ref TPrestep prestep, int index); } public interface ITwoBodyConvexContactPrestep : IConvexContactPrestep where TPrestep : struct, ITwoBodyConvexContactPrestep { - ref Vector3Wide GetOffsetB(ref TPrestep prestep); + //TODO: Purge this! } public interface IContactAccumulatedImpulses where TAccumulatedImpulses : struct, IContactAccumulatedImpulses diff --git a/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs b/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs index f269444fb..966ceff4e 100644 --- a/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs +++ b/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs @@ -199,7 +199,7 @@ public static void ComputeFrictionCenter( public struct Contact1OneBody : IConvexOneBodyContactConstraintDescription { public ConstraintContactData Contact0; - public Vector3 Normal; + public Vector3 LocalNormalB; public float FrictionCoefficient; public SpringSettings SpringSettings; public float MaximumRecoveryVelocity; @@ -208,10 +208,11 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int { Debug.Assert(batch.TypeId == ConstraintTypeId, "The type batch passed to the description must match the description's expected type."); ref var target = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); - Vector3Wide.WriteFirst(Contact0.OffsetA, ref target.Contact0.OffsetA); + Vector3Wide.WriteFirst(Contact0.LocalOffsetA, ref target.Contact0.LocalOffsetA); GetFirst(ref target.Contact0.Depth) = Contact0.PenetrationDepth; - - Vector3Wide.WriteFirst(Normal, ref target.Normal); + GetFirst(ref target.Contact0.PlaneOffsetB) = Contact0.PlaneOffsetB; + + Vector3Wide.WriteFirst(LocalNormalB, ref target.LocalNormalB); GetFirst(ref target.MaterialProperties.FrictionCoefficient) = FrictionCoefficient; SpringSettingsWide.WriteFirst(SpringSettings, ref target.MaterialProperties.SpringSettings); GetFirst(ref target.MaterialProperties.MaximumRecoveryVelocity) = MaximumRecoveryVelocity; @@ -221,19 +222,20 @@ public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int { Debug.Assert(batch.TypeId == ConstraintTypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); - Vector3Wide.ReadFirst(source.Contact0.OffsetA, out description.Contact0.OffsetA); + Vector3Wide.ReadFirst(source.Contact0.LocalOffsetA, out description.Contact0.LocalOffsetA); description.Contact0.PenetrationDepth = GetFirst(ref source.Contact0.Depth); - Vector3Wide.ReadFirst(source.Normal, out description.Normal); + description.Contact0.PlaneOffsetB = GetFirst(ref source.Contact0.PlaneOffsetB); + Vector3Wide.ReadFirst(source.LocalNormalB, out description.LocalNormalB); description.FrictionCoefficient = GetFirst(ref source.MaterialProperties.FrictionCoefficient); SpringSettingsWide.ReadFirst(source.MaterialProperties.SpringSettings, out description.SpringSettings); description.MaximumRecoveryVelocity = GetFirst(ref source.MaterialProperties.MaximumRecoveryVelocity); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void CopyManifoldWideProperties(ref Vector3 normal, ref PairMaterialProperties material) + public void CopyManifoldWideProperties(ref Vector3 localNormalB, ref PairMaterialProperties material) { FrictionCoefficient = material.FrictionCoefficient; - Normal = normal; + LocalNormalB = localNormalB; SpringSettings = material.SpringSettings; MaximumRecoveryVelocity = material.MaximumRecoveryVelocity; } @@ -261,16 +263,16 @@ public struct Contact1OneBodyPrestepData : IConvexContactPrestep 1; public readonly int ContactCount => 1; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3Wide GetNormal(ref Contact1OneBodyPrestepData prestep) + public ref Vector3Wide GetLocalNormalB(ref Contact1OneBodyPrestepData prestep) { - return ref prestep.Normal; + return ref prestep.LocalNormalB; } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -284,7 +286,6 @@ public ref MaterialPropertiesWide GetMaterialProperties(ref Contact1OneBodyPrest { return ref prestep.MaterialProperties; } - } public unsafe struct Contact1OneBodyProjection @@ -307,75 +308,81 @@ public void Prestep( in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref Contact1OneBodyPrestepData prestep, out Contact1OneBodyProjection projection) { //Be careful about the execution order here. It should be aligned with the prestep data layout to ensure prefetching works well. - projection.InertiaA = inertiaA; - projection.PremultipliedFrictionCoefficient = prestep.MaterialProperties.FrictionCoefficient; - projection.Normal = prestep.Normal; - Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); - TangentFrictionOneBody.Prestep(ref x, ref z, ref prestep.Contact0.OffsetA, ref projection.InertiaA, out projection.Tangent); - SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); - PenetrationLimitOneBody.Prestep(projection.InertiaA, prestep.Contact0.OffsetA, prestep.Normal, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration0); + //projection.InertiaA = inertiaA; + //QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); + //projection.PremultipliedFrictionCoefficient = prestep.MaterialProperties.FrictionCoefficient; + //projection.Normal = prestep.Normal; + //Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + //TangentFrictionOneBody.Prestep(ref x, ref z, ref worldOffsetA0, ref projection.InertiaA, out projection.Tangent); + //SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); + //PenetrationLimitOneBody.Prestep(projection.InertiaA, worldOffsetA0, prestep.Normal, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration0); //If there's only one contact, then the contact patch as determined by contact distance would be zero. //That can cause some subtle behavioral issues sometimes, so we approximate lever arm with the contact depth, assuming that the contact surface area will increase as the depth increases. - projection.LeverArm0 = Vector.Max(Vector.Zero, prestep.Contact0.Depth); - TwistFrictionOneBody.Prestep(ref projection.InertiaA, ref prestep.Normal, out projection.Twist); + //projection.LeverArm0 = Vector.Max(Vector.Zero, prestep.Contact0.Depth); + //TwistFrictionOneBody.Prestep(ref projection.InertiaA, ref prestep.Normal, out projection.Twist); + + projection = default; } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart(ref BodyVelocityWide wsvA, ref Contact1OneBodyProjection projection, ref Contact1AccumulatedImpulses accumulatedImpulses) { - Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - TangentFrictionOneBody.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref accumulatedImpulses.Tangent, ref wsvA); - PenetrationLimitOneBody.WarmStart(projection.Penetration0, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration0, ref wsvA); - TwistFrictionOneBody.WarmStart(ref projection.Normal, ref projection.InertiaA, ref accumulatedImpulses.Twist, ref wsvA); + //Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); + //TangentFrictionOneBody.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref accumulatedImpulses.Tangent, ref wsvA); + //PenetrationLimitOneBody.WarmStart(projection.Penetration0, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration0, ref wsvA); + //TwistFrictionOneBody.WarmStart(ref projection.Normal, ref projection.InertiaA, ref accumulatedImpulses.Twist, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve(ref BodyVelocityWide wsvA, ref Contact1OneBodyProjection projection, ref Contact1AccumulatedImpulses accumulatedImpulses) { - Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * - (accumulatedImpulses.Penetration0); - TangentFrictionOneBody.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); + //Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); + //var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * + // (accumulatedImpulses.Penetration0); + //TangentFrictionOneBody.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); //Note that we solve the penetration constraints after the friction constraints. //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. //It's a pretty minor effect either way. - PenetrationLimitOneBody.Solve(projection.Penetration0, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); - var maximumTwistImpulse = projection.PremultipliedFrictionCoefficient * ( - accumulatedImpulses.Penetration0 * projection.LeverArm0); - TwistFrictionOneBody.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); + //PenetrationLimitOneBody.Solve(projection.Penetration0, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); + //var maximumTwistImpulse = projection.PremultipliedFrictionCoefficient * ( + // accumulatedImpulses.Penetration0 * projection.LeverArm0); + //TwistFrictionOneBody.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityWide velocityA, ref Contact1OneBodyPrestepData prestep) { - PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, velocityA, ref prestep.Contact0.Depth); + //QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); + //PenetrationLimitOneBody.UpdatePenetrationDepth(dt, worldOffsetA, prestep.Normal, velocityA, ref prestep.Contact0.Depth); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Contact1OneBodyPrestepData prestep, in Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { - Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); - TangentFrictionOneBody.WarmStart2(x, z, prestep.Contact0.OffsetA, inertiaA, accumulatedImpulses.Tangent, ref wsvA); - PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, accumulatedImpulses.Penetration0, ref wsvA); - TwistFrictionOneBody.WarmStart2(prestep.Normal, inertiaA, accumulatedImpulses.Twist, ref wsvA); + Helpers.BuildOrthonormalBasis(prestep.LocalNormalB, out var x, out var z); + QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); + TangentFrictionOneBody.WarmStart2(x, z, worldOffsetA0, inertiaA, accumulatedImpulses.Tangent, ref wsvA); + PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.LocalNormalB, worldOffsetA0, accumulatedImpulses.Penetration0, ref wsvA); + TwistFrictionOneBody.WarmStart2(prestep.LocalNormalB, inertiaA, accumulatedImpulses.Twist, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, in Contact1OneBodyPrestepData prestep, ref Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { - Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + Helpers.BuildOrthonormalBasis(prestep.LocalNormalB, out var x, out var z); + QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); var maximumTangentImpulse = prestep.MaterialProperties.FrictionCoefficient * (accumulatedImpulses.Penetration0); - TangentFrictionOneBody.Solve2(x, z, prestep.Contact0.OffsetA, inertiaA, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); + TangentFrictionOneBody.Solve2(x, z, worldOffsetA0, inertiaA, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); //Note that we solve the penetration constraints after the friction constraints. //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. //It's a pretty minor effect either way. SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); var inverseDtWide = new Vector(inverseDt); - PenetrationLimitOneBody.Solve2(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); + PenetrationLimitOneBody.Solve2(inertiaA, prestep.LocalNormalB, worldOffsetA0, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); //If there's only one contact, then the contact patch as determined by contact distance would be zero. //That can cause some subtle behavioral issues sometimes, so we approximate lever arm with the contact depth, assuming that the contact surface area will increase as the depth increases. var maximumTwistImpulse = prestep.MaterialProperties.FrictionCoefficient * accumulatedImpulses.Penetration0 * prestep.Contact0.Depth; - TwistFrictionOneBody.Solve2(prestep.Normal, inertiaA, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); + TwistFrictionOneBody.Solve2(prestep.LocalNormalB, inertiaA, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -383,7 +390,7 @@ public void UpdateForNewPose( in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector dt, in Contact1AccumulatedImpulses accumulatedImpulses, ref Contact1OneBodyPrestepData prestep) { - PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, wsvA, ref prestep.Contact0.Depth); + //PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, wsvA, ref prestep.Contact0.Depth); } } @@ -402,7 +409,7 @@ public struct Contact2OneBody : IConvexOneBodyContactConstraintDescription.Get(ref batch.PrestepData, bundleIndex), innerIndex); - Vector3Wide.WriteFirst(Contact0.OffsetA, ref target.Contact0.OffsetA); + Vector3Wide.WriteFirst(Contact0.LocalOffsetA, ref target.Contact0.LocalOffsetA); GetFirst(ref target.Contact0.Depth) = Contact0.PenetrationDepth; - Vector3Wide.WriteFirst(Contact1.OffsetA, ref target.Contact1.OffsetA); + GetFirst(ref target.Contact0.PlaneOffsetB) = Contact0.PlaneOffsetB; + Vector3Wide.WriteFirst(Contact1.LocalOffsetA, ref target.Contact1.LocalOffsetA); GetFirst(ref target.Contact1.Depth) = Contact1.PenetrationDepth; - - Vector3Wide.WriteFirst(Normal, ref target.Normal); + GetFirst(ref target.Contact1.PlaneOffsetB) = Contact1.PlaneOffsetB; + + Vector3Wide.WriteFirst(LocalNormalB, ref target.LocalNormalB); GetFirst(ref target.MaterialProperties.FrictionCoefficient) = FrictionCoefficient; SpringSettingsWide.WriteFirst(SpringSettings, ref target.MaterialProperties.SpringSettings); GetFirst(ref target.MaterialProperties.MaximumRecoveryVelocity) = MaximumRecoveryVelocity; @@ -426,21 +435,23 @@ public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int { Debug.Assert(batch.TypeId == ConstraintTypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); - Vector3Wide.ReadFirst(source.Contact0.OffsetA, out description.Contact0.OffsetA); + Vector3Wide.ReadFirst(source.Contact0.LocalOffsetA, out description.Contact0.LocalOffsetA); description.Contact0.PenetrationDepth = GetFirst(ref source.Contact0.Depth); - Vector3Wide.ReadFirst(source.Contact1.OffsetA, out description.Contact1.OffsetA); + description.Contact0.PlaneOffsetB = GetFirst(ref source.Contact0.PlaneOffsetB); + Vector3Wide.ReadFirst(source.Contact1.LocalOffsetA, out description.Contact1.LocalOffsetA); description.Contact1.PenetrationDepth = GetFirst(ref source.Contact1.Depth); - Vector3Wide.ReadFirst(source.Normal, out description.Normal); + description.Contact1.PlaneOffsetB = GetFirst(ref source.Contact1.PlaneOffsetB); + Vector3Wide.ReadFirst(source.LocalNormalB, out description.LocalNormalB); description.FrictionCoefficient = GetFirst(ref source.MaterialProperties.FrictionCoefficient); SpringSettingsWide.ReadFirst(source.MaterialProperties.SpringSettings, out description.SpringSettings); description.MaximumRecoveryVelocity = GetFirst(ref source.MaterialProperties.MaximumRecoveryVelocity); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void CopyManifoldWideProperties(ref Vector3 normal, ref PairMaterialProperties material) + public void CopyManifoldWideProperties(ref Vector3 localNormalB, ref PairMaterialProperties material) { FrictionCoefficient = material.FrictionCoefficient; - Normal = normal; + LocalNormalB = localNormalB; SpringSettings = material.SpringSettings; MaximumRecoveryVelocity = material.MaximumRecoveryVelocity; } @@ -469,16 +480,16 @@ public struct Contact2OneBodyPrestepData : IConvexContactPrestep 1; public readonly int ContactCount => 2; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3Wide GetNormal(ref Contact2OneBodyPrestepData prestep) + public ref Vector3Wide GetLocalNormalB(ref Contact2OneBodyPrestepData prestep) { - return ref prestep.Normal; + return ref prestep.LocalNormalB; } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -492,7 +503,6 @@ public ref MaterialPropertiesWide GetMaterialProperties(ref Contact2OneBodyPrest { return ref prestep.MaterialProperties; } - } public unsafe struct Contact2OneBodyProjection @@ -517,85 +527,95 @@ public void Prestep( in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref Contact2OneBodyPrestepData prestep, out Contact2OneBodyProjection projection) { //Be careful about the execution order here. It should be aligned with the prestep data layout to ensure prefetching works well. - projection.InertiaA = inertiaA; - FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, out var offsetToManifoldCenterA); - projection.PremultipliedFrictionCoefficient = (1f / 2f) * prestep.MaterialProperties.FrictionCoefficient; - projection.Normal = prestep.Normal; - Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); - TangentFrictionOneBody.Prestep(ref x, ref z, ref offsetToManifoldCenterA, ref projection.InertiaA, out projection.Tangent); - SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); - PenetrationLimitOneBody.Prestep(projection.InertiaA, prestep.Contact0.OffsetA, prestep.Normal, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration0); - PenetrationLimitOneBody.Prestep(projection.InertiaA, prestep.Contact1.OffsetA, prestep.Normal, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration1); - Vector3Wide.Distance(prestep.Contact0.OffsetA, offsetToManifoldCenterA, out projection.LeverArm0); - Vector3Wide.Distance(prestep.Contact1.OffsetA, offsetToManifoldCenterA, out projection.LeverArm1); - TwistFrictionOneBody.Prestep(ref projection.InertiaA, ref prestep.Normal, out projection.Twist); + //projection.InertiaA = inertiaA; + //QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); + //QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); + //FrictionHelpers.ComputeFrictionCenter(worldOffsetA0, worldOffsetA1, prestep.Contact0.Depth, prestep.Contact1.Depth, out var offsetToManifoldCenterA); + //projection.PremultipliedFrictionCoefficient = (1f / 2f) * prestep.MaterialProperties.FrictionCoefficient; + //projection.Normal = prestep.Normal; + //Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + //TangentFrictionOneBody.Prestep(ref x, ref z, ref offsetToManifoldCenterA, ref projection.InertiaA, out projection.Tangent); + //SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); + //PenetrationLimitOneBody.Prestep(projection.InertiaA, worldOffsetA0, prestep.Normal, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration0); + //PenetrationLimitOneBody.Prestep(projection.InertiaA, worldOffsetA1, prestep.Normal, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration1); + //Vector3Wide.Distance(worldOffsetA0, offsetToManifoldCenterA, out projection.LeverArm0); + //Vector3Wide.Distance(worldOffsetA1, offsetToManifoldCenterA, out projection.LeverArm1); + //TwistFrictionOneBody.Prestep(ref projection.InertiaA, ref prestep.Normal, out projection.Twist); + + projection = default; } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart(ref BodyVelocityWide wsvA, ref Contact2OneBodyProjection projection, ref Contact2AccumulatedImpulses accumulatedImpulses) { - Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - TangentFrictionOneBody.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref accumulatedImpulses.Tangent, ref wsvA); - PenetrationLimitOneBody.WarmStart(projection.Penetration0, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration0, ref wsvA); - PenetrationLimitOneBody.WarmStart(projection.Penetration1, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration1, ref wsvA); - TwistFrictionOneBody.WarmStart(ref projection.Normal, ref projection.InertiaA, ref accumulatedImpulses.Twist, ref wsvA); + //Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); + //TangentFrictionOneBody.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref accumulatedImpulses.Tangent, ref wsvA); + //PenetrationLimitOneBody.WarmStart(projection.Penetration0, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration0, ref wsvA); + //PenetrationLimitOneBody.WarmStart(projection.Penetration1, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration1, ref wsvA); + //TwistFrictionOneBody.WarmStart(ref projection.Normal, ref projection.InertiaA, ref accumulatedImpulses.Twist, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve(ref BodyVelocityWide wsvA, ref Contact2OneBodyProjection projection, ref Contact2AccumulatedImpulses accumulatedImpulses) { - Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * - (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1); - TangentFrictionOneBody.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); + //Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); + //var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * + // (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1); + //TangentFrictionOneBody.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); //Note that we solve the penetration constraints after the friction constraints. //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. //It's a pretty minor effect either way. - PenetrationLimitOneBody.Solve(projection.Penetration0, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); - PenetrationLimitOneBody.Solve(projection.Penetration1, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA); - var maximumTwistImpulse = projection.PremultipliedFrictionCoefficient * ( - accumulatedImpulses.Penetration0 * projection.LeverArm0 + - accumulatedImpulses.Penetration1 * projection.LeverArm1); - TwistFrictionOneBody.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); + //PenetrationLimitOneBody.Solve(projection.Penetration0, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); + //PenetrationLimitOneBody.Solve(projection.Penetration1, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA); + //var maximumTwistImpulse = projection.PremultipliedFrictionCoefficient * ( + // accumulatedImpulses.Penetration0 * projection.LeverArm0 + + // accumulatedImpulses.Penetration1 * projection.LeverArm1); + //TwistFrictionOneBody.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityWide velocityA, ref Contact2OneBodyPrestepData prestep) { - PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, velocityA, ref prestep.Contact0.Depth); - PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.Normal, velocityA, ref prestep.Contact1.Depth); + //QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); + //PenetrationLimitOneBody.UpdatePenetrationDepth(dt, worldOffsetA, prestep.Normal, velocityA, ref prestep.Contact0.Depth); + //QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); + //PenetrationLimitOneBody.UpdatePenetrationDepth(dt, worldOffsetA, prestep.Normal, velocityA, ref prestep.Contact1.Depth); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Contact2OneBodyPrestepData prestep, in Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { - Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); - FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, out var offsetToManifoldCenterA); + Helpers.BuildOrthonormalBasis(prestep.LocalNormalB, out var x, out var z); + QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); + QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); + FrictionHelpers.ComputeFrictionCenter(worldOffsetA0, worldOffsetA1, prestep.Contact0.Depth, prestep.Contact1.Depth, out var offsetToManifoldCenterA); TangentFrictionOneBody.WarmStart2(x, z, offsetToManifoldCenterA, inertiaA, accumulatedImpulses.Tangent, ref wsvA); - PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, accumulatedImpulses.Penetration0, ref wsvA); - PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact1.OffsetA, accumulatedImpulses.Penetration1, ref wsvA); - TwistFrictionOneBody.WarmStart2(prestep.Normal, inertiaA, accumulatedImpulses.Twist, ref wsvA); + PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.LocalNormalB, worldOffsetA0, accumulatedImpulses.Penetration0, ref wsvA); + PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.LocalNormalB, worldOffsetA1, accumulatedImpulses.Penetration1, ref wsvA); + TwistFrictionOneBody.WarmStart2(prestep.LocalNormalB, inertiaA, accumulatedImpulses.Twist, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, in Contact2OneBodyPrestepData prestep, ref Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { - Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + Helpers.BuildOrthonormalBasis(prestep.LocalNormalB, out var x, out var z); + QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); + QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); var premultipliedFrictionCoefficient = new Vector(1f / 2f) * prestep.MaterialProperties.FrictionCoefficient; var maximumTangentImpulse = premultipliedFrictionCoefficient * (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1); - FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, out var offsetToManifoldCenterA); + FrictionHelpers.ComputeFrictionCenter(worldOffsetA0, worldOffsetA1, prestep.Contact0.Depth, prestep.Contact1.Depth, out var offsetToManifoldCenterA); TangentFrictionOneBody.Solve2(x, z, offsetToManifoldCenterA, inertiaA, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); //Note that we solve the penetration constraints after the friction constraints. //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. //It's a pretty minor effect either way. SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); var inverseDtWide = new Vector(inverseDt); - PenetrationLimitOneBody.Solve2(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); - PenetrationLimitOneBody.Solve2(inertiaA, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA); + PenetrationLimitOneBody.Solve2(inertiaA, prestep.LocalNormalB, worldOffsetA0, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); + PenetrationLimitOneBody.Solve2(inertiaA, prestep.LocalNormalB, worldOffsetA0, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA); var maximumTwistImpulse = premultipliedFrictionCoefficient * ( - accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact0.OffsetA) + - accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact1.OffsetA)); - TwistFrictionOneBody.Solve2(prestep.Normal, inertiaA, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); + accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, worldOffsetA0) + + accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, worldOffsetA1)); + TwistFrictionOneBody.Solve2(prestep.LocalNormalB, inertiaA, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -603,8 +623,8 @@ public void UpdateForNewPose( in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector dt, in Contact2AccumulatedImpulses accumulatedImpulses, ref Contact2OneBodyPrestepData prestep) { - PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, wsvA, ref prestep.Contact0.Depth); - PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.Normal, wsvA, ref prestep.Contact1.Depth); + //PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, wsvA, ref prestep.Contact0.Depth); + //PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.Normal, wsvA, ref prestep.Contact1.Depth); } } @@ -624,7 +644,7 @@ public struct Contact3OneBody : IConvexOneBodyContactConstraintDescription.Get(ref batch.PrestepData, bundleIndex), innerIndex); - Vector3Wide.WriteFirst(Contact0.OffsetA, ref target.Contact0.OffsetA); + Vector3Wide.WriteFirst(Contact0.LocalOffsetA, ref target.Contact0.LocalOffsetA); GetFirst(ref target.Contact0.Depth) = Contact0.PenetrationDepth; - Vector3Wide.WriteFirst(Contact1.OffsetA, ref target.Contact1.OffsetA); + GetFirst(ref target.Contact0.PlaneOffsetB) = Contact0.PlaneOffsetB; + Vector3Wide.WriteFirst(Contact1.LocalOffsetA, ref target.Contact1.LocalOffsetA); GetFirst(ref target.Contact1.Depth) = Contact1.PenetrationDepth; - Vector3Wide.WriteFirst(Contact2.OffsetA, ref target.Contact2.OffsetA); + GetFirst(ref target.Contact1.PlaneOffsetB) = Contact1.PlaneOffsetB; + Vector3Wide.WriteFirst(Contact2.LocalOffsetA, ref target.Contact2.LocalOffsetA); GetFirst(ref target.Contact2.Depth) = Contact2.PenetrationDepth; - - Vector3Wide.WriteFirst(Normal, ref target.Normal); + GetFirst(ref target.Contact2.PlaneOffsetB) = Contact2.PlaneOffsetB; + + Vector3Wide.WriteFirst(LocalNormalB, ref target.LocalNormalB); GetFirst(ref target.MaterialProperties.FrictionCoefficient) = FrictionCoefficient; SpringSettingsWide.WriteFirst(SpringSettings, ref target.MaterialProperties.SpringSettings); GetFirst(ref target.MaterialProperties.MaximumRecoveryVelocity) = MaximumRecoveryVelocity; @@ -650,23 +673,26 @@ public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int { Debug.Assert(batch.TypeId == ConstraintTypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); - Vector3Wide.ReadFirst(source.Contact0.OffsetA, out description.Contact0.OffsetA); + Vector3Wide.ReadFirst(source.Contact0.LocalOffsetA, out description.Contact0.LocalOffsetA); description.Contact0.PenetrationDepth = GetFirst(ref source.Contact0.Depth); - Vector3Wide.ReadFirst(source.Contact1.OffsetA, out description.Contact1.OffsetA); + description.Contact0.PlaneOffsetB = GetFirst(ref source.Contact0.PlaneOffsetB); + Vector3Wide.ReadFirst(source.Contact1.LocalOffsetA, out description.Contact1.LocalOffsetA); description.Contact1.PenetrationDepth = GetFirst(ref source.Contact1.Depth); - Vector3Wide.ReadFirst(source.Contact2.OffsetA, out description.Contact2.OffsetA); + description.Contact1.PlaneOffsetB = GetFirst(ref source.Contact1.PlaneOffsetB); + Vector3Wide.ReadFirst(source.Contact2.LocalOffsetA, out description.Contact2.LocalOffsetA); description.Contact2.PenetrationDepth = GetFirst(ref source.Contact2.Depth); - Vector3Wide.ReadFirst(source.Normal, out description.Normal); + description.Contact2.PlaneOffsetB = GetFirst(ref source.Contact2.PlaneOffsetB); + Vector3Wide.ReadFirst(source.LocalNormalB, out description.LocalNormalB); description.FrictionCoefficient = GetFirst(ref source.MaterialProperties.FrictionCoefficient); SpringSettingsWide.ReadFirst(source.MaterialProperties.SpringSettings, out description.SpringSettings); description.MaximumRecoveryVelocity = GetFirst(ref source.MaterialProperties.MaximumRecoveryVelocity); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void CopyManifoldWideProperties(ref Vector3 normal, ref PairMaterialProperties material) + public void CopyManifoldWideProperties(ref Vector3 localNormalB, ref PairMaterialProperties material) { FrictionCoefficient = material.FrictionCoefficient; - Normal = normal; + LocalNormalB = localNormalB; SpringSettings = material.SpringSettings; MaximumRecoveryVelocity = material.MaximumRecoveryVelocity; } @@ -696,16 +722,16 @@ public struct Contact3OneBodyPrestepData : IConvexContactPrestep 1; public readonly int ContactCount => 3; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3Wide GetNormal(ref Contact3OneBodyPrestepData prestep) + public ref Vector3Wide GetLocalNormalB(ref Contact3OneBodyPrestepData prestep) { - return ref prestep.Normal; + return ref prestep.LocalNormalB; } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -719,7 +745,6 @@ public ref MaterialPropertiesWide GetMaterialProperties(ref Contact3OneBodyPrest { return ref prestep.MaterialProperties; } - } public unsafe struct Contact3OneBodyProjection @@ -746,94 +771,108 @@ public void Prestep( in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref Contact3OneBodyPrestepData prestep, out Contact3OneBodyProjection projection) { //Be careful about the execution order here. It should be aligned with the prestep data layout to ensure prefetching works well. - projection.InertiaA = inertiaA; - FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, out var offsetToManifoldCenterA); - projection.PremultipliedFrictionCoefficient = (1f / 3f) * prestep.MaterialProperties.FrictionCoefficient; - projection.Normal = prestep.Normal; - Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); - TangentFrictionOneBody.Prestep(ref x, ref z, ref offsetToManifoldCenterA, ref projection.InertiaA, out projection.Tangent); - SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); - PenetrationLimitOneBody.Prestep(projection.InertiaA, prestep.Contact0.OffsetA, prestep.Normal, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration0); - PenetrationLimitOneBody.Prestep(projection.InertiaA, prestep.Contact1.OffsetA, prestep.Normal, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration1); - PenetrationLimitOneBody.Prestep(projection.InertiaA, prestep.Contact2.OffsetA, prestep.Normal, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration2); - Vector3Wide.Distance(prestep.Contact0.OffsetA, offsetToManifoldCenterA, out projection.LeverArm0); - Vector3Wide.Distance(prestep.Contact1.OffsetA, offsetToManifoldCenterA, out projection.LeverArm1); - Vector3Wide.Distance(prestep.Contact2.OffsetA, offsetToManifoldCenterA, out projection.LeverArm2); - TwistFrictionOneBody.Prestep(ref projection.InertiaA, ref prestep.Normal, out projection.Twist); + //projection.InertiaA = inertiaA; + //QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); + //QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); + //QuaternionWide.TransformWithoutOverlap(prestep.Contact2.LocalOffsetA, orientationA, out var worldOffsetA2); + //FrictionHelpers.ComputeFrictionCenter(worldOffsetA0, worldOffsetA1, worldOffsetA2, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, out var offsetToManifoldCenterA); + //projection.PremultipliedFrictionCoefficient = (1f / 3f) * prestep.MaterialProperties.FrictionCoefficient; + //projection.Normal = prestep.Normal; + //Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + //TangentFrictionOneBody.Prestep(ref x, ref z, ref offsetToManifoldCenterA, ref projection.InertiaA, out projection.Tangent); + //SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); + //PenetrationLimitOneBody.Prestep(projection.InertiaA, worldOffsetA0, prestep.Normal, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration0); + //PenetrationLimitOneBody.Prestep(projection.InertiaA, worldOffsetA1, prestep.Normal, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration1); + //PenetrationLimitOneBody.Prestep(projection.InertiaA, worldOffsetA2, prestep.Normal, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration2); + //Vector3Wide.Distance(worldOffsetA0, offsetToManifoldCenterA, out projection.LeverArm0); + //Vector3Wide.Distance(worldOffsetA1, offsetToManifoldCenterA, out projection.LeverArm1); + //Vector3Wide.Distance(worldOffsetA2, offsetToManifoldCenterA, out projection.LeverArm2); + //TwistFrictionOneBody.Prestep(ref projection.InertiaA, ref prestep.Normal, out projection.Twist); + + projection = default; } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart(ref BodyVelocityWide wsvA, ref Contact3OneBodyProjection projection, ref Contact3AccumulatedImpulses accumulatedImpulses) { - Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - TangentFrictionOneBody.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref accumulatedImpulses.Tangent, ref wsvA); - PenetrationLimitOneBody.WarmStart(projection.Penetration0, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration0, ref wsvA); - PenetrationLimitOneBody.WarmStart(projection.Penetration1, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration1, ref wsvA); - PenetrationLimitOneBody.WarmStart(projection.Penetration2, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration2, ref wsvA); - TwistFrictionOneBody.WarmStart(ref projection.Normal, ref projection.InertiaA, ref accumulatedImpulses.Twist, ref wsvA); + //Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); + //TangentFrictionOneBody.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref accumulatedImpulses.Tangent, ref wsvA); + //PenetrationLimitOneBody.WarmStart(projection.Penetration0, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration0, ref wsvA); + //PenetrationLimitOneBody.WarmStart(projection.Penetration1, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration1, ref wsvA); + //PenetrationLimitOneBody.WarmStart(projection.Penetration2, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration2, ref wsvA); + //TwistFrictionOneBody.WarmStart(ref projection.Normal, ref projection.InertiaA, ref accumulatedImpulses.Twist, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve(ref BodyVelocityWide wsvA, ref Contact3OneBodyProjection projection, ref Contact3AccumulatedImpulses accumulatedImpulses) { - Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * - (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2); - TangentFrictionOneBody.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); + //Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); + //var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * + // (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2); + //TangentFrictionOneBody.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); //Note that we solve the penetration constraints after the friction constraints. //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. //It's a pretty minor effect either way. - PenetrationLimitOneBody.Solve(projection.Penetration0, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); - PenetrationLimitOneBody.Solve(projection.Penetration1, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA); - PenetrationLimitOneBody.Solve(projection.Penetration2, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA); - var maximumTwistImpulse = projection.PremultipliedFrictionCoefficient * ( - accumulatedImpulses.Penetration0 * projection.LeverArm0 + - accumulatedImpulses.Penetration1 * projection.LeverArm1 + - accumulatedImpulses.Penetration2 * projection.LeverArm2); - TwistFrictionOneBody.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); + //PenetrationLimitOneBody.Solve(projection.Penetration0, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); + //PenetrationLimitOneBody.Solve(projection.Penetration1, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA); + //PenetrationLimitOneBody.Solve(projection.Penetration2, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA); + //var maximumTwistImpulse = projection.PremultipliedFrictionCoefficient * ( + // accumulatedImpulses.Penetration0 * projection.LeverArm0 + + // accumulatedImpulses.Penetration1 * projection.LeverArm1 + + // accumulatedImpulses.Penetration2 * projection.LeverArm2); + //TwistFrictionOneBody.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityWide velocityA, ref Contact3OneBodyPrestepData prestep) { - PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, velocityA, ref prestep.Contact0.Depth); - PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.Normal, velocityA, ref prestep.Contact1.Depth); - PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact2.OffsetA, prestep.Normal, velocityA, ref prestep.Contact2.Depth); + //QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); + //PenetrationLimitOneBody.UpdatePenetrationDepth(dt, worldOffsetA, prestep.Normal, velocityA, ref prestep.Contact0.Depth); + //QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); + //PenetrationLimitOneBody.UpdatePenetrationDepth(dt, worldOffsetA, prestep.Normal, velocityA, ref prestep.Contact1.Depth); + //QuaternionWide.TransformWithoutOverlap(prestep.Contact2.LocalOffsetA, orientationA, out var worldOffsetA2); + //PenetrationLimitOneBody.UpdatePenetrationDepth(dt, worldOffsetA, prestep.Normal, velocityA, ref prestep.Contact2.Depth); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Contact3OneBodyPrestepData prestep, in Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { - Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); - FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, out var offsetToManifoldCenterA); + Helpers.BuildOrthonormalBasis(prestep.LocalNormalB, out var x, out var z); + QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); + QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); + QuaternionWide.TransformWithoutOverlap(prestep.Contact2.LocalOffsetA, orientationA, out var worldOffsetA2); + FrictionHelpers.ComputeFrictionCenter(worldOffsetA0, worldOffsetA1, worldOffsetA2, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, out var offsetToManifoldCenterA); TangentFrictionOneBody.WarmStart2(x, z, offsetToManifoldCenterA, inertiaA, accumulatedImpulses.Tangent, ref wsvA); - PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, accumulatedImpulses.Penetration0, ref wsvA); - PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact1.OffsetA, accumulatedImpulses.Penetration1, ref wsvA); - PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact2.OffsetA, accumulatedImpulses.Penetration2, ref wsvA); - TwistFrictionOneBody.WarmStart2(prestep.Normal, inertiaA, accumulatedImpulses.Twist, ref wsvA); + PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.LocalNormalB, worldOffsetA0, accumulatedImpulses.Penetration0, ref wsvA); + PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.LocalNormalB, worldOffsetA1, accumulatedImpulses.Penetration1, ref wsvA); + PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.LocalNormalB, worldOffsetA2, accumulatedImpulses.Penetration2, ref wsvA); + TwistFrictionOneBody.WarmStart2(prestep.LocalNormalB, inertiaA, accumulatedImpulses.Twist, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, in Contact3OneBodyPrestepData prestep, ref Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { - Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + Helpers.BuildOrthonormalBasis(prestep.LocalNormalB, out var x, out var z); + QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); + QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); + QuaternionWide.TransformWithoutOverlap(prestep.Contact2.LocalOffsetA, orientationA, out var worldOffsetA2); var premultipliedFrictionCoefficient = new Vector(1f / 3f) * prestep.MaterialProperties.FrictionCoefficient; var maximumTangentImpulse = premultipliedFrictionCoefficient * (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2); - FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, out var offsetToManifoldCenterA); + FrictionHelpers.ComputeFrictionCenter(worldOffsetA0, worldOffsetA1, worldOffsetA2, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, out var offsetToManifoldCenterA); TangentFrictionOneBody.Solve2(x, z, offsetToManifoldCenterA, inertiaA, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); //Note that we solve the penetration constraints after the friction constraints. //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. //It's a pretty minor effect either way. SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); var inverseDtWide = new Vector(inverseDt); - PenetrationLimitOneBody.Solve2(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); - PenetrationLimitOneBody.Solve2(inertiaA, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA); - PenetrationLimitOneBody.Solve2(inertiaA, prestep.Normal, prestep.Contact2.OffsetA, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA); + PenetrationLimitOneBody.Solve2(inertiaA, prestep.LocalNormalB, worldOffsetA0, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); + PenetrationLimitOneBody.Solve2(inertiaA, prestep.LocalNormalB, worldOffsetA0, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA); + PenetrationLimitOneBody.Solve2(inertiaA, prestep.LocalNormalB, worldOffsetA0, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA); var maximumTwistImpulse = premultipliedFrictionCoefficient * ( - accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact0.OffsetA) + - accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact1.OffsetA) + - accumulatedImpulses.Penetration2 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact2.OffsetA)); - TwistFrictionOneBody.Solve2(prestep.Normal, inertiaA, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); + accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, worldOffsetA0) + + accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, worldOffsetA1) + + accumulatedImpulses.Penetration2 * Vector3Wide.Distance(offsetToManifoldCenterA, worldOffsetA2)); + TwistFrictionOneBody.Solve2(prestep.LocalNormalB, inertiaA, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -841,9 +880,9 @@ public void UpdateForNewPose( in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector dt, in Contact3AccumulatedImpulses accumulatedImpulses, ref Contact3OneBodyPrestepData prestep) { - PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, wsvA, ref prestep.Contact0.Depth); - PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.Normal, wsvA, ref prestep.Contact1.Depth); - PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact2.OffsetA, prestep.Normal, wsvA, ref prestep.Contact2.Depth); + //PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, wsvA, ref prestep.Contact0.Depth); + //PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.Normal, wsvA, ref prestep.Contact1.Depth); + //PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact2.OffsetA, prestep.Normal, wsvA, ref prestep.Contact2.Depth); } } @@ -864,7 +903,7 @@ public struct Contact4OneBody : IConvexOneBodyContactConstraintDescription.Get(ref batch.PrestepData, bundleIndex), innerIndex); - Vector3Wide.WriteFirst(Contact0.OffsetA, ref target.Contact0.OffsetA); + Vector3Wide.WriteFirst(Contact0.LocalOffsetA, ref target.Contact0.LocalOffsetA); GetFirst(ref target.Contact0.Depth) = Contact0.PenetrationDepth; - Vector3Wide.WriteFirst(Contact1.OffsetA, ref target.Contact1.OffsetA); + GetFirst(ref target.Contact0.PlaneOffsetB) = Contact0.PlaneOffsetB; + Vector3Wide.WriteFirst(Contact1.LocalOffsetA, ref target.Contact1.LocalOffsetA); GetFirst(ref target.Contact1.Depth) = Contact1.PenetrationDepth; - Vector3Wide.WriteFirst(Contact2.OffsetA, ref target.Contact2.OffsetA); + GetFirst(ref target.Contact1.PlaneOffsetB) = Contact1.PlaneOffsetB; + Vector3Wide.WriteFirst(Contact2.LocalOffsetA, ref target.Contact2.LocalOffsetA); GetFirst(ref target.Contact2.Depth) = Contact2.PenetrationDepth; - Vector3Wide.WriteFirst(Contact3.OffsetA, ref target.Contact3.OffsetA); + GetFirst(ref target.Contact2.PlaneOffsetB) = Contact2.PlaneOffsetB; + Vector3Wide.WriteFirst(Contact3.LocalOffsetA, ref target.Contact3.LocalOffsetA); GetFirst(ref target.Contact3.Depth) = Contact3.PenetrationDepth; - - Vector3Wide.WriteFirst(Normal, ref target.Normal); + GetFirst(ref target.Contact3.PlaneOffsetB) = Contact3.PlaneOffsetB; + + Vector3Wide.WriteFirst(LocalNormalB, ref target.LocalNormalB); GetFirst(ref target.MaterialProperties.FrictionCoefficient) = FrictionCoefficient; SpringSettingsWide.WriteFirst(SpringSettings, ref target.MaterialProperties.SpringSettings); GetFirst(ref target.MaterialProperties.MaximumRecoveryVelocity) = MaximumRecoveryVelocity; @@ -892,25 +935,29 @@ public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int { Debug.Assert(batch.TypeId == ConstraintTypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); - Vector3Wide.ReadFirst(source.Contact0.OffsetA, out description.Contact0.OffsetA); + Vector3Wide.ReadFirst(source.Contact0.LocalOffsetA, out description.Contact0.LocalOffsetA); description.Contact0.PenetrationDepth = GetFirst(ref source.Contact0.Depth); - Vector3Wide.ReadFirst(source.Contact1.OffsetA, out description.Contact1.OffsetA); + description.Contact0.PlaneOffsetB = GetFirst(ref source.Contact0.PlaneOffsetB); + Vector3Wide.ReadFirst(source.Contact1.LocalOffsetA, out description.Contact1.LocalOffsetA); description.Contact1.PenetrationDepth = GetFirst(ref source.Contact1.Depth); - Vector3Wide.ReadFirst(source.Contact2.OffsetA, out description.Contact2.OffsetA); + description.Contact1.PlaneOffsetB = GetFirst(ref source.Contact1.PlaneOffsetB); + Vector3Wide.ReadFirst(source.Contact2.LocalOffsetA, out description.Contact2.LocalOffsetA); description.Contact2.PenetrationDepth = GetFirst(ref source.Contact2.Depth); - Vector3Wide.ReadFirst(source.Contact3.OffsetA, out description.Contact3.OffsetA); + description.Contact2.PlaneOffsetB = GetFirst(ref source.Contact2.PlaneOffsetB); + Vector3Wide.ReadFirst(source.Contact3.LocalOffsetA, out description.Contact3.LocalOffsetA); description.Contact3.PenetrationDepth = GetFirst(ref source.Contact3.Depth); - Vector3Wide.ReadFirst(source.Normal, out description.Normal); + description.Contact3.PlaneOffsetB = GetFirst(ref source.Contact3.PlaneOffsetB); + Vector3Wide.ReadFirst(source.LocalNormalB, out description.LocalNormalB); description.FrictionCoefficient = GetFirst(ref source.MaterialProperties.FrictionCoefficient); SpringSettingsWide.ReadFirst(source.MaterialProperties.SpringSettings, out description.SpringSettings); description.MaximumRecoveryVelocity = GetFirst(ref source.MaterialProperties.MaximumRecoveryVelocity); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void CopyManifoldWideProperties(ref Vector3 normal, ref PairMaterialProperties material) + public void CopyManifoldWideProperties(ref Vector3 localNormalB, ref PairMaterialProperties material) { FrictionCoefficient = material.FrictionCoefficient; - Normal = normal; + LocalNormalB = localNormalB; SpringSettings = material.SpringSettings; MaximumRecoveryVelocity = material.MaximumRecoveryVelocity; } @@ -941,16 +988,16 @@ public struct Contact4OneBodyPrestepData : IConvexContactPrestep 1; public readonly int ContactCount => 4; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3Wide GetNormal(ref Contact4OneBodyPrestepData prestep) + public ref Vector3Wide GetLocalNormalB(ref Contact4OneBodyPrestepData prestep) { - return ref prestep.Normal; + return ref prestep.LocalNormalB; } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -964,7 +1011,6 @@ public ref MaterialPropertiesWide GetMaterialProperties(ref Contact4OneBodyPrest { return ref prestep.MaterialProperties; } - } public unsafe struct Contact4OneBodyProjection @@ -993,103 +1039,121 @@ public void Prestep( in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref Contact4OneBodyPrestepData prestep, out Contact4OneBodyProjection projection) { //Be careful about the execution order here. It should be aligned with the prestep data layout to ensure prefetching works well. - projection.InertiaA = inertiaA; - FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact3.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, prestep.Contact3.Depth, out var offsetToManifoldCenterA); - projection.PremultipliedFrictionCoefficient = (1f / 4f) * prestep.MaterialProperties.FrictionCoefficient; - projection.Normal = prestep.Normal; - Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); - TangentFrictionOneBody.Prestep(ref x, ref z, ref offsetToManifoldCenterA, ref projection.InertiaA, out projection.Tangent); - SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); - PenetrationLimitOneBody.Prestep(projection.InertiaA, prestep.Contact0.OffsetA, prestep.Normal, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration0); - PenetrationLimitOneBody.Prestep(projection.InertiaA, prestep.Contact1.OffsetA, prestep.Normal, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration1); - PenetrationLimitOneBody.Prestep(projection.InertiaA, prestep.Contact2.OffsetA, prestep.Normal, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration2); - PenetrationLimitOneBody.Prestep(projection.InertiaA, prestep.Contact3.OffsetA, prestep.Normal, prestep.Contact3.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration3); - Vector3Wide.Distance(prestep.Contact0.OffsetA, offsetToManifoldCenterA, out projection.LeverArm0); - Vector3Wide.Distance(prestep.Contact1.OffsetA, offsetToManifoldCenterA, out projection.LeverArm1); - Vector3Wide.Distance(prestep.Contact2.OffsetA, offsetToManifoldCenterA, out projection.LeverArm2); - Vector3Wide.Distance(prestep.Contact3.OffsetA, offsetToManifoldCenterA, out projection.LeverArm3); - TwistFrictionOneBody.Prestep(ref projection.InertiaA, ref prestep.Normal, out projection.Twist); + //projection.InertiaA = inertiaA; + //QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); + //QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); + //QuaternionWide.TransformWithoutOverlap(prestep.Contact2.LocalOffsetA, orientationA, out var worldOffsetA2); + //QuaternionWide.TransformWithoutOverlap(prestep.Contact3.LocalOffsetA, orientationA, out var worldOffsetA3); + //FrictionHelpers.ComputeFrictionCenter(worldOffsetA0, worldOffsetA1, worldOffsetA2, worldOffsetA3, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, prestep.Contact3.Depth, out var offsetToManifoldCenterA); + //projection.PremultipliedFrictionCoefficient = (1f / 4f) * prestep.MaterialProperties.FrictionCoefficient; + //projection.Normal = prestep.Normal; + //Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + //TangentFrictionOneBody.Prestep(ref x, ref z, ref offsetToManifoldCenterA, ref projection.InertiaA, out projection.Tangent); + //SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); + //PenetrationLimitOneBody.Prestep(projection.InertiaA, worldOffsetA0, prestep.Normal, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration0); + //PenetrationLimitOneBody.Prestep(projection.InertiaA, worldOffsetA1, prestep.Normal, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration1); + //PenetrationLimitOneBody.Prestep(projection.InertiaA, worldOffsetA2, prestep.Normal, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration2); + //PenetrationLimitOneBody.Prestep(projection.InertiaA, worldOffsetA3, prestep.Normal, prestep.Contact3.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration3); + //Vector3Wide.Distance(worldOffsetA0, offsetToManifoldCenterA, out projection.LeverArm0); + //Vector3Wide.Distance(worldOffsetA1, offsetToManifoldCenterA, out projection.LeverArm1); + //Vector3Wide.Distance(worldOffsetA2, offsetToManifoldCenterA, out projection.LeverArm2); + //Vector3Wide.Distance(worldOffsetA3, offsetToManifoldCenterA, out projection.LeverArm3); + //TwistFrictionOneBody.Prestep(ref projection.InertiaA, ref prestep.Normal, out projection.Twist); + + projection = default; } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart(ref BodyVelocityWide wsvA, ref Contact4OneBodyProjection projection, ref Contact4AccumulatedImpulses accumulatedImpulses) { - Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - TangentFrictionOneBody.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref accumulatedImpulses.Tangent, ref wsvA); - PenetrationLimitOneBody.WarmStart(projection.Penetration0, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration0, ref wsvA); - PenetrationLimitOneBody.WarmStart(projection.Penetration1, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration1, ref wsvA); - PenetrationLimitOneBody.WarmStart(projection.Penetration2, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration2, ref wsvA); - PenetrationLimitOneBody.WarmStart(projection.Penetration3, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration3, ref wsvA); - TwistFrictionOneBody.WarmStart(ref projection.Normal, ref projection.InertiaA, ref accumulatedImpulses.Twist, ref wsvA); + //Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); + //TangentFrictionOneBody.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref accumulatedImpulses.Tangent, ref wsvA); + //PenetrationLimitOneBody.WarmStart(projection.Penetration0, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration0, ref wsvA); + //PenetrationLimitOneBody.WarmStart(projection.Penetration1, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration1, ref wsvA); + //PenetrationLimitOneBody.WarmStart(projection.Penetration2, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration2, ref wsvA); + //PenetrationLimitOneBody.WarmStart(projection.Penetration3, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration3, ref wsvA); + //TwistFrictionOneBody.WarmStart(ref projection.Normal, ref projection.InertiaA, ref accumulatedImpulses.Twist, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve(ref BodyVelocityWide wsvA, ref Contact4OneBodyProjection projection, ref Contact4AccumulatedImpulses accumulatedImpulses) { - Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * - (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2 + accumulatedImpulses.Penetration3); - TangentFrictionOneBody.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); + //Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); + //var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * + // (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2 + accumulatedImpulses.Penetration3); + //TangentFrictionOneBody.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); //Note that we solve the penetration constraints after the friction constraints. //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. //It's a pretty minor effect either way. - PenetrationLimitOneBody.Solve(projection.Penetration0, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); - PenetrationLimitOneBody.Solve(projection.Penetration1, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA); - PenetrationLimitOneBody.Solve(projection.Penetration2, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA); - PenetrationLimitOneBody.Solve(projection.Penetration3, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration3, ref wsvA); - var maximumTwistImpulse = projection.PremultipliedFrictionCoefficient * ( - accumulatedImpulses.Penetration0 * projection.LeverArm0 + - accumulatedImpulses.Penetration1 * projection.LeverArm1 + - accumulatedImpulses.Penetration2 * projection.LeverArm2 + - accumulatedImpulses.Penetration3 * projection.LeverArm3); - TwistFrictionOneBody.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); + //PenetrationLimitOneBody.Solve(projection.Penetration0, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); + //PenetrationLimitOneBody.Solve(projection.Penetration1, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA); + //PenetrationLimitOneBody.Solve(projection.Penetration2, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA); + //PenetrationLimitOneBody.Solve(projection.Penetration3, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration3, ref wsvA); + //var maximumTwistImpulse = projection.PremultipliedFrictionCoefficient * ( + // accumulatedImpulses.Penetration0 * projection.LeverArm0 + + // accumulatedImpulses.Penetration1 * projection.LeverArm1 + + // accumulatedImpulses.Penetration2 * projection.LeverArm2 + + // accumulatedImpulses.Penetration3 * projection.LeverArm3); + //TwistFrictionOneBody.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityWide velocityA, ref Contact4OneBodyPrestepData prestep) { - PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, velocityA, ref prestep.Contact0.Depth); - PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.Normal, velocityA, ref prestep.Contact1.Depth); - PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact2.OffsetA, prestep.Normal, velocityA, ref prestep.Contact2.Depth); - PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact3.OffsetA, prestep.Normal, velocityA, ref prestep.Contact3.Depth); + //QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); + //PenetrationLimitOneBody.UpdatePenetrationDepth(dt, worldOffsetA, prestep.Normal, velocityA, ref prestep.Contact0.Depth); + //QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); + //PenetrationLimitOneBody.UpdatePenetrationDepth(dt, worldOffsetA, prestep.Normal, velocityA, ref prestep.Contact1.Depth); + //QuaternionWide.TransformWithoutOverlap(prestep.Contact2.LocalOffsetA, orientationA, out var worldOffsetA2); + //PenetrationLimitOneBody.UpdatePenetrationDepth(dt, worldOffsetA, prestep.Normal, velocityA, ref prestep.Contact2.Depth); + //QuaternionWide.TransformWithoutOverlap(prestep.Contact3.LocalOffsetA, orientationA, out var worldOffsetA3); + //PenetrationLimitOneBody.UpdatePenetrationDepth(dt, worldOffsetA, prestep.Normal, velocityA, ref prestep.Contact3.Depth); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Contact4OneBodyPrestepData prestep, in Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { - Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); - FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact3.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, prestep.Contact3.Depth, out var offsetToManifoldCenterA); + Helpers.BuildOrthonormalBasis(prestep.LocalNormalB, out var x, out var z); + QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); + QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); + QuaternionWide.TransformWithoutOverlap(prestep.Contact2.LocalOffsetA, orientationA, out var worldOffsetA2); + QuaternionWide.TransformWithoutOverlap(prestep.Contact3.LocalOffsetA, orientationA, out var worldOffsetA3); + FrictionHelpers.ComputeFrictionCenter(worldOffsetA0, worldOffsetA1, worldOffsetA2, worldOffsetA3, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, prestep.Contact3.Depth, out var offsetToManifoldCenterA); TangentFrictionOneBody.WarmStart2(x, z, offsetToManifoldCenterA, inertiaA, accumulatedImpulses.Tangent, ref wsvA); - PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, accumulatedImpulses.Penetration0, ref wsvA); - PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact1.OffsetA, accumulatedImpulses.Penetration1, ref wsvA); - PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact2.OffsetA, accumulatedImpulses.Penetration2, ref wsvA); - PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact3.OffsetA, accumulatedImpulses.Penetration3, ref wsvA); - TwistFrictionOneBody.WarmStart2(prestep.Normal, inertiaA, accumulatedImpulses.Twist, ref wsvA); + PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.LocalNormalB, worldOffsetA0, accumulatedImpulses.Penetration0, ref wsvA); + PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.LocalNormalB, worldOffsetA1, accumulatedImpulses.Penetration1, ref wsvA); + PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.LocalNormalB, worldOffsetA2, accumulatedImpulses.Penetration2, ref wsvA); + PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.LocalNormalB, worldOffsetA3, accumulatedImpulses.Penetration3, ref wsvA); + TwistFrictionOneBody.WarmStart2(prestep.LocalNormalB, inertiaA, accumulatedImpulses.Twist, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, in Contact4OneBodyPrestepData prestep, ref Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { - Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + Helpers.BuildOrthonormalBasis(prestep.LocalNormalB, out var x, out var z); + QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); + QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); + QuaternionWide.TransformWithoutOverlap(prestep.Contact2.LocalOffsetA, orientationA, out var worldOffsetA2); + QuaternionWide.TransformWithoutOverlap(prestep.Contact3.LocalOffsetA, orientationA, out var worldOffsetA3); var premultipliedFrictionCoefficient = new Vector(1f / 4f) * prestep.MaterialProperties.FrictionCoefficient; var maximumTangentImpulse = premultipliedFrictionCoefficient * (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2 + accumulatedImpulses.Penetration3); - FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact3.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, prestep.Contact3.Depth, out var offsetToManifoldCenterA); + FrictionHelpers.ComputeFrictionCenter(worldOffsetA0, worldOffsetA1, worldOffsetA2, worldOffsetA3, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, prestep.Contact3.Depth, out var offsetToManifoldCenterA); TangentFrictionOneBody.Solve2(x, z, offsetToManifoldCenterA, inertiaA, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); //Note that we solve the penetration constraints after the friction constraints. //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. //It's a pretty minor effect either way. SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); var inverseDtWide = new Vector(inverseDt); - PenetrationLimitOneBody.Solve2(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); - PenetrationLimitOneBody.Solve2(inertiaA, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA); - PenetrationLimitOneBody.Solve2(inertiaA, prestep.Normal, prestep.Contact2.OffsetA, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA); - PenetrationLimitOneBody.Solve2(inertiaA, prestep.Normal, prestep.Contact3.OffsetA, prestep.Contact3.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration3, ref wsvA); + PenetrationLimitOneBody.Solve2(inertiaA, prestep.LocalNormalB, worldOffsetA0, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); + PenetrationLimitOneBody.Solve2(inertiaA, prestep.LocalNormalB, worldOffsetA0, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA); + PenetrationLimitOneBody.Solve2(inertiaA, prestep.LocalNormalB, worldOffsetA0, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA); + PenetrationLimitOneBody.Solve2(inertiaA, prestep.LocalNormalB, worldOffsetA0, prestep.Contact3.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration3, ref wsvA); var maximumTwistImpulse = premultipliedFrictionCoefficient * ( - accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact0.OffsetA) + - accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact1.OffsetA) + - accumulatedImpulses.Penetration2 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact2.OffsetA) + - accumulatedImpulses.Penetration3 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact3.OffsetA)); - TwistFrictionOneBody.Solve2(prestep.Normal, inertiaA, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); + accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, worldOffsetA0) + + accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, worldOffsetA1) + + accumulatedImpulses.Penetration2 * Vector3Wide.Distance(offsetToManifoldCenterA, worldOffsetA2) + + accumulatedImpulses.Penetration3 * Vector3Wide.Distance(offsetToManifoldCenterA, worldOffsetA3)); + TwistFrictionOneBody.Solve2(prestep.LocalNormalB, inertiaA, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -1097,10 +1161,10 @@ public void UpdateForNewPose( in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector dt, in Contact4AccumulatedImpulses accumulatedImpulses, ref Contact4OneBodyPrestepData prestep) { - PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, wsvA, ref prestep.Contact0.Depth); - PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.Normal, wsvA, ref prestep.Contact1.Depth); - PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact2.OffsetA, prestep.Normal, wsvA, ref prestep.Contact2.Depth); - PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact3.OffsetA, prestep.Normal, wsvA, ref prestep.Contact3.Depth); + //PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, wsvA, ref prestep.Contact0.Depth); + //PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.Normal, wsvA, ref prestep.Contact1.Depth); + //PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact2.OffsetA, prestep.Normal, wsvA, ref prestep.Contact2.Depth); + //PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact3.OffsetA, prestep.Normal, wsvA, ref prestep.Contact3.Depth); } } @@ -1118,8 +1182,7 @@ public class Contact4OneBodyTypeProcessor : public struct Contact1 : IConvexTwoBodyContactConstraintDescription { public ConstraintContactData Contact0; - public Vector3 OffsetB; - public Vector3 Normal; + public Vector3 LocalNormalB; public float FrictionCoefficient; public SpringSettings SpringSettings; public float MaximumRecoveryVelocity; @@ -1128,11 +1191,11 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int { Debug.Assert(batch.TypeId == ConstraintTypeId, "The type batch passed to the description must match the description's expected type."); ref var target = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); - Vector3Wide.WriteFirst(Contact0.OffsetA, ref target.Contact0.OffsetA); + Vector3Wide.WriteFirst(Contact0.LocalOffsetA, ref target.Contact0.LocalOffsetA); GetFirst(ref target.Contact0.Depth) = Contact0.PenetrationDepth; - - Vector3Wide.WriteFirst(OffsetB, ref target.OffsetB); - Vector3Wide.WriteFirst(Normal, ref target.Normal); + GetFirst(ref target.Contact0.PlaneOffsetB) = Contact0.PlaneOffsetB; + + Vector3Wide.WriteFirst(LocalNormalB, ref target.LocalNormalB); GetFirst(ref target.MaterialProperties.FrictionCoefficient) = FrictionCoefficient; SpringSettingsWide.WriteFirst(SpringSettings, ref target.MaterialProperties.SpringSettings); GetFirst(ref target.MaterialProperties.MaximumRecoveryVelocity) = MaximumRecoveryVelocity; @@ -1142,22 +1205,20 @@ public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int { Debug.Assert(batch.TypeId == ConstraintTypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); - Vector3Wide.ReadFirst(source.Contact0.OffsetA, out description.Contact0.OffsetA); + Vector3Wide.ReadFirst(source.Contact0.LocalOffsetA, out description.Contact0.LocalOffsetA); description.Contact0.PenetrationDepth = GetFirst(ref source.Contact0.Depth); - - Vector3Wide.ReadFirst(source.OffsetB, out description.OffsetB); - Vector3Wide.ReadFirst(source.Normal, out description.Normal); + description.Contact0.PlaneOffsetB = GetFirst(ref source.Contact0.PlaneOffsetB); + Vector3Wide.ReadFirst(source.LocalNormalB, out description.LocalNormalB); description.FrictionCoefficient = GetFirst(ref source.MaterialProperties.FrictionCoefficient); SpringSettingsWide.ReadFirst(source.MaterialProperties.SpringSettings, out description.SpringSettings); description.MaximumRecoveryVelocity = GetFirst(ref source.MaterialProperties.MaximumRecoveryVelocity); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void CopyManifoldWideProperties(ref Vector3 offsetB, ref Vector3 normal, ref PairMaterialProperties material) + public void CopyManifoldWideProperties(ref Vector3 localNormalB, ref PairMaterialProperties material) { - OffsetB = offsetB; FrictionCoefficient = material.FrictionCoefficient; - Normal = normal; + LocalNormalB = localNormalB; SpringSettings = material.SpringSettings; MaximumRecoveryVelocity = material.MaximumRecoveryVelocity; } @@ -1184,18 +1245,17 @@ public struct Contact1PrestepData : ITwoBodyConvexContactPrestep 2; public readonly int ContactCount => 1; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3Wide GetNormal(ref Contact1PrestepData prestep) + public ref Vector3Wide GetLocalNormalB(ref Contact1PrestepData prestep) { - return ref prestep.Normal; + return ref prestep.LocalNormalB; } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -1209,12 +1269,6 @@ public ref MaterialPropertiesWide GetMaterialProperties(ref Contact1PrestepData { return ref prestep.MaterialProperties; } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3Wide GetOffsetB(ref Contact1PrestepData prestep) - { - return ref prestep.OffsetB; - } } public unsafe struct Contact1Projection @@ -1238,81 +1292,89 @@ public void Prestep( in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref Contact1PrestepData prestep, out Contact1Projection projection) { //Be careful about the execution order here. It should be aligned with the prestep data layout to ensure prefetching works well. - projection.InertiaA = inertiaA; - projection.InertiaB = inertiaB; - Vector3Wide.Subtract(prestep.Contact0.OffsetA, prestep.OffsetB, out var offsetToManifoldCenterB); - projection.PremultipliedFrictionCoefficient = prestep.MaterialProperties.FrictionCoefficient; - projection.Normal = prestep.Normal; - Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); - TangentFriction.Prestep(ref x, ref z, ref prestep.Contact0.OffsetA, ref offsetToManifoldCenterB, ref projection.InertiaA, ref projection.InertiaB, out projection.Tangent); - SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); - Vector3Wide contactOffsetB; - Vector3Wide.Subtract(prestep.Contact0.OffsetA, prestep.OffsetB, out contactOffsetB); - PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, prestep.Contact0.OffsetA, contactOffsetB, prestep.Normal, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration0); + //projection.InertiaA = inertiaA; + //projection.InertiaB = inertiaB; + //QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); + //Vector3Wide.Subtract(worldOffsetA0, ab, out var offsetToManifoldCenterB); + //projection.PremultipliedFrictionCoefficient = prestep.MaterialProperties.FrictionCoefficient; + //projection.Normal = prestep.Normal; + //Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + //TangentFriction.Prestep(ref x, ref z, ref worldOffsetA0, ref offsetToManifoldCenterB, ref projection.InertiaA, ref projection.InertiaB, out projection.Tangent); + //SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); + //PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, worldOffsetA0, worldOffsetA0 - ab, prestep.Normal, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration0); //If there's only one contact, then the contact patch as determined by contact distance would be zero. //That can cause some subtle behavioral issues sometimes, so we approximate lever arm with the contact depth, assuming that the contact surface area will increase as the depth increases. - projection.LeverArm0 = Vector.Max(Vector.Zero, prestep.Contact0.Depth); - TwistFriction.Prestep(ref projection.InertiaA, ref projection.InertiaB, ref prestep.Normal, out projection.Twist); + //projection.LeverArm0 = Vector.Max(Vector.Zero, prestep.Contact0.Depth); + //TwistFriction.Prestep(ref projection.InertiaA, ref projection.InertiaB, ref prestep.Normal, out projection.Twist); + + projection = default; } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart(ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref Contact1Projection projection, ref Contact1AccumulatedImpulses accumulatedImpulses) { - Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - TangentFriction.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart(projection.Penetration0, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - TwistFriction.WarmStart(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); + //Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); + //TangentFriction.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); + //PenetrationLimit.WarmStart(projection.Penetration0, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + //TwistFriction.WarmStart(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve(ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref Contact1Projection projection, ref Contact1AccumulatedImpulses accumulatedImpulses) { - Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * - (accumulatedImpulses.Penetration0); - TangentFriction.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref projection.InertiaB, ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); + //Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); + //var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * + // (accumulatedImpulses.Penetration0); + //TangentFriction.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref projection.InertiaB, ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); //Note that we solve the penetration constraints after the friction constraints. //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. //It's a pretty minor effect either way. - PenetrationLimit.Solve(projection.Penetration0, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - var maximumTwistImpulse = projection.PremultipliedFrictionCoefficient * ( - accumulatedImpulses.Penetration0 * projection.LeverArm0); - TwistFriction.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); + //PenetrationLimit.Solve(projection.Penetration0, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + //var maximumTwistImpulse = projection.PremultipliedFrictionCoefficient * ( + // accumulatedImpulses.Penetration0 * projection.LeverArm0); + //TwistFriction.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityWide velocityA, in BodyVelocityWide velocityB, ref Contact1PrestepData prestep) { - PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact0.Depth); + //QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); + //PenetrationLimit.UpdatePenetrationDepth(dt, worldOffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact0.Depth); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Contact1PrestepData prestep, in Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); - Vector3Wide.Subtract(prestep.Contact0.OffsetA, prestep.OffsetB, out var offsetToManifoldCenterB); - TangentFriction.WarmStart2(x, z, prestep.Contact0.OffsetA, offsetToManifoldCenterB, inertiaA, inertiaB, accumulatedImpulses.Tangent, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - TwistFriction.WarmStart2(prestep.Normal, inertiaA, inertiaB, accumulatedImpulses.Twist, ref wsvA, ref wsvB); + QuaternionWide.TransformWithoutOverlap(prestep.LocalNormalB, orientationB, out var normal); + Helpers.BuildOrthonormalBasis(normal, out var x, out var z); + QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); + var ab = positionB - positionA; + Vector3Wide.Subtract(worldOffsetA0, ab, out var offsetToManifoldCenterB); + TangentFriction.WarmStart2(x, z, worldOffsetA0, offsetToManifoldCenterB, inertiaA, inertiaB, accumulatedImpulses.Tangent, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart2(inertiaA, inertiaB, normal, worldOffsetA0, worldOffsetA0 - ab, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + TwistFriction.WarmStart2(normal, inertiaA, inertiaB, accumulatedImpulses.Twist, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in Contact1PrestepData prestep, ref Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + QuaternionWide.TransformWithoutOverlap(prestep.LocalNormalB, orientationB, out var normal); + Helpers.BuildOrthonormalBasis(normal, out var x, out var z); + QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); var maximumTangentImpulse = prestep.MaterialProperties.FrictionCoefficient * (accumulatedImpulses.Penetration0); - Vector3Wide.Subtract(prestep.Contact0.OffsetA, prestep.OffsetB, out var offsetToManifoldCenterB); - TangentFriction.Solve2(x, z, prestep.Contact0.OffsetA, offsetToManifoldCenterB, inertiaA, inertiaB, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); + var ab = positionB - positionA; + Vector3Wide.Subtract(worldOffsetA0, ab, out var offsetToManifoldCenterB); + TangentFriction.Solve2(x, z, worldOffsetA0, offsetToManifoldCenterB, inertiaA, inertiaB, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); //Note that we solve the penetration constraints after the friction constraints. //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. //It's a pretty minor effect either way. SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); var inverseDtWide = new Vector(inverseDt); - PenetrationLimit.Solve2(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + PenetrationLimit.Solve2(inertiaA, inertiaB, normal, worldOffsetA0, worldOffsetA0 - ab, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); //If there's only one contact, then the contact patch as determined by contact distance would be zero. //That can cause some subtle behavioral issues sometimes, so we approximate lever arm with the contact depth, assuming that the contact surface area will increase as the depth increases. var maximumTwistImpulse = prestep.MaterialProperties.FrictionCoefficient * accumulatedImpulses.Penetration0 * prestep.Contact0.Depth; - TwistFriction.Solve2(prestep.Normal, inertiaA, inertiaB, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); + TwistFriction.Solve2(normal, inertiaA, inertiaB, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -1321,8 +1383,8 @@ public void UpdateForNewPose( in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector dt, in Contact1AccumulatedImpulses accumulatedImpulses, ref Contact1PrestepData prestep) { - prestep.OffsetB = positionB - positionA; - PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact0.Depth); + //prestep.OffsetB = positionB - positionA; + //PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact0.Depth); } } @@ -1341,8 +1403,7 @@ public struct Contact2 : IConvexTwoBodyContactConstraintDescription { public ConstraintContactData Contact0; public ConstraintContactData Contact1; - public Vector3 OffsetB; - public Vector3 Normal; + public Vector3 LocalNormalB; public float FrictionCoefficient; public SpringSettings SpringSettings; public float MaximumRecoveryVelocity; @@ -1351,13 +1412,14 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int { Debug.Assert(batch.TypeId == ConstraintTypeId, "The type batch passed to the description must match the description's expected type."); ref var target = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); - Vector3Wide.WriteFirst(Contact0.OffsetA, ref target.Contact0.OffsetA); + Vector3Wide.WriteFirst(Contact0.LocalOffsetA, ref target.Contact0.LocalOffsetA); GetFirst(ref target.Contact0.Depth) = Contact0.PenetrationDepth; - Vector3Wide.WriteFirst(Contact1.OffsetA, ref target.Contact1.OffsetA); + GetFirst(ref target.Contact0.PlaneOffsetB) = Contact0.PlaneOffsetB; + Vector3Wide.WriteFirst(Contact1.LocalOffsetA, ref target.Contact1.LocalOffsetA); GetFirst(ref target.Contact1.Depth) = Contact1.PenetrationDepth; - - Vector3Wide.WriteFirst(OffsetB, ref target.OffsetB); - Vector3Wide.WriteFirst(Normal, ref target.Normal); + GetFirst(ref target.Contact1.PlaneOffsetB) = Contact1.PlaneOffsetB; + + Vector3Wide.WriteFirst(LocalNormalB, ref target.LocalNormalB); GetFirst(ref target.MaterialProperties.FrictionCoefficient) = FrictionCoefficient; SpringSettingsWide.WriteFirst(SpringSettings, ref target.MaterialProperties.SpringSettings); GetFirst(ref target.MaterialProperties.MaximumRecoveryVelocity) = MaximumRecoveryVelocity; @@ -1367,24 +1429,23 @@ public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int { Debug.Assert(batch.TypeId == ConstraintTypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); - Vector3Wide.ReadFirst(source.Contact0.OffsetA, out description.Contact0.OffsetA); + Vector3Wide.ReadFirst(source.Contact0.LocalOffsetA, out description.Contact0.LocalOffsetA); description.Contact0.PenetrationDepth = GetFirst(ref source.Contact0.Depth); - Vector3Wide.ReadFirst(source.Contact1.OffsetA, out description.Contact1.OffsetA); + description.Contact0.PlaneOffsetB = GetFirst(ref source.Contact0.PlaneOffsetB); + Vector3Wide.ReadFirst(source.Contact1.LocalOffsetA, out description.Contact1.LocalOffsetA); description.Contact1.PenetrationDepth = GetFirst(ref source.Contact1.Depth); - - Vector3Wide.ReadFirst(source.OffsetB, out description.OffsetB); - Vector3Wide.ReadFirst(source.Normal, out description.Normal); + description.Contact1.PlaneOffsetB = GetFirst(ref source.Contact1.PlaneOffsetB); + Vector3Wide.ReadFirst(source.LocalNormalB, out description.LocalNormalB); description.FrictionCoefficient = GetFirst(ref source.MaterialProperties.FrictionCoefficient); SpringSettingsWide.ReadFirst(source.MaterialProperties.SpringSettings, out description.SpringSettings); description.MaximumRecoveryVelocity = GetFirst(ref source.MaterialProperties.MaximumRecoveryVelocity); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void CopyManifoldWideProperties(ref Vector3 offsetB, ref Vector3 normal, ref PairMaterialProperties material) + public void CopyManifoldWideProperties(ref Vector3 localNormalB, ref PairMaterialProperties material) { - OffsetB = offsetB; FrictionCoefficient = material.FrictionCoefficient; - Normal = normal; + LocalNormalB = localNormalB; SpringSettings = material.SpringSettings; MaximumRecoveryVelocity = material.MaximumRecoveryVelocity; } @@ -1412,18 +1473,17 @@ public struct Contact2PrestepData : ITwoBodyConvexContactPrestep 2; public readonly int ContactCount => 2; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3Wide GetNormal(ref Contact2PrestepData prestep) + public ref Vector3Wide GetLocalNormalB(ref Contact2PrestepData prestep) { - return ref prestep.Normal; + return ref prestep.LocalNormalB; } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -1437,12 +1497,6 @@ public ref MaterialPropertiesWide GetMaterialProperties(ref Contact2PrestepData { return ref prestep.MaterialProperties; } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3Wide GetOffsetB(ref Contact2PrestepData prestep) - { - return ref prestep.OffsetB; - } } public unsafe struct Contact2Projection @@ -1468,92 +1522,103 @@ public void Prestep( in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref Contact2PrestepData prestep, out Contact2Projection projection) { //Be careful about the execution order here. It should be aligned with the prestep data layout to ensure prefetching works well. - projection.InertiaA = inertiaA; - projection.InertiaB = inertiaB; - FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, out var offsetToManifoldCenterA); - Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); - projection.PremultipliedFrictionCoefficient = (1f / 2f) * prestep.MaterialProperties.FrictionCoefficient; - projection.Normal = prestep.Normal; - Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); - TangentFriction.Prestep(ref x, ref z, ref offsetToManifoldCenterA, ref offsetToManifoldCenterB, ref projection.InertiaA, ref projection.InertiaB, out projection.Tangent); - SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); - Vector3Wide contactOffsetB; - Vector3Wide.Subtract(prestep.Contact0.OffsetA, prestep.OffsetB, out contactOffsetB); - PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, prestep.Contact0.OffsetA, contactOffsetB, prestep.Normal, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration0); - Vector3Wide.Subtract(prestep.Contact1.OffsetA, prestep.OffsetB, out contactOffsetB); - PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, prestep.Contact1.OffsetA, contactOffsetB, prestep.Normal, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration1); - Vector3Wide.Distance(prestep.Contact0.OffsetA, offsetToManifoldCenterA, out projection.LeverArm0); - Vector3Wide.Distance(prestep.Contact1.OffsetA, offsetToManifoldCenterA, out projection.LeverArm1); - TwistFriction.Prestep(ref projection.InertiaA, ref projection.InertiaB, ref prestep.Normal, out projection.Twist); + //projection.InertiaA = inertiaA; + //projection.InertiaB = inertiaB; + //QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); + //QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); + //FrictionHelpers.ComputeFrictionCenter(worldOffsetA0, worldOffsetA1, prestep.Contact0.Depth, prestep.Contact1.Depth, out var offsetToManifoldCenterA); + //Vector3Wide.Subtract(offsetToManifoldCenterA, ab, out var offsetToManifoldCenterB); + //projection.PremultipliedFrictionCoefficient = (1f / 2f) * prestep.MaterialProperties.FrictionCoefficient; + //projection.Normal = prestep.Normal; + //Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + //TangentFriction.Prestep(ref x, ref z, ref offsetToManifoldCenterA, ref offsetToManifoldCenterB, ref projection.InertiaA, ref projection.InertiaB, out projection.Tangent); + //SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); + //PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, worldOffsetA0, worldOffsetA0 - ab, prestep.Normal, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration0); + //PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, worldOffsetA1, worldOffsetA1 - ab, prestep.Normal, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration1); + //Vector3Wide.Distance(worldOffsetA0, offsetToManifoldCenterA, out projection.LeverArm0); + //Vector3Wide.Distance(worldOffsetA1, offsetToManifoldCenterA, out projection.LeverArm1); + //TwistFriction.Prestep(ref projection.InertiaA, ref projection.InertiaB, ref prestep.Normal, out projection.Twist); + + projection = default; } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart(ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref Contact2Projection projection, ref Contact2AccumulatedImpulses accumulatedImpulses) { - Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - TangentFriction.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart(projection.Penetration0, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart(projection.Penetration1, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); - TwistFriction.WarmStart(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); + //Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); + //TangentFriction.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); + //PenetrationLimit.WarmStart(projection.Penetration0, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + //PenetrationLimit.WarmStart(projection.Penetration1, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); + //TwistFriction.WarmStart(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve(ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref Contact2Projection projection, ref Contact2AccumulatedImpulses accumulatedImpulses) { - Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * - (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1); - TangentFriction.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref projection.InertiaB, ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); + //Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); + //var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * + // (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1); + //TangentFriction.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref projection.InertiaB, ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); //Note that we solve the penetration constraints after the friction constraints. //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. //It's a pretty minor effect either way. - PenetrationLimit.Solve(projection.Penetration0, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - PenetrationLimit.Solve(projection.Penetration1, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); - var maximumTwistImpulse = projection.PremultipliedFrictionCoefficient * ( - accumulatedImpulses.Penetration0 * projection.LeverArm0 + - accumulatedImpulses.Penetration1 * projection.LeverArm1); - TwistFriction.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); + //PenetrationLimit.Solve(projection.Penetration0, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + //PenetrationLimit.Solve(projection.Penetration1, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); + //var maximumTwistImpulse = projection.PremultipliedFrictionCoefficient * ( + // accumulatedImpulses.Penetration0 * projection.LeverArm0 + + // accumulatedImpulses.Penetration1 * projection.LeverArm1); + //TwistFriction.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityWide velocityA, in BodyVelocityWide velocityB, ref Contact2PrestepData prestep) { - PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact0.Depth); - PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact1.Depth); + //QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); + //PenetrationLimit.UpdatePenetrationDepth(dt, worldOffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact0.Depth); + //QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); + //PenetrationLimit.UpdatePenetrationDepth(dt, worldOffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact1.Depth); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Contact2PrestepData prestep, in Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); - FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, out var offsetToManifoldCenterA); - Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); + QuaternionWide.TransformWithoutOverlap(prestep.LocalNormalB, orientationB, out var normal); + Helpers.BuildOrthonormalBasis(normal, out var x, out var z); + QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); + QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); + FrictionHelpers.ComputeFrictionCenter(worldOffsetA0, worldOffsetA1, prestep.Contact0.Depth, prestep.Contact1.Depth, out var offsetToManifoldCenterA); + var ab = positionB - positionA; + Vector3Wide.Subtract(offsetToManifoldCenterA, ab, out var offsetToManifoldCenterB); TangentFriction.WarmStart2(x, z, offsetToManifoldCenterA, offsetToManifoldCenterB, inertiaA, inertiaB, accumulatedImpulses.Tangent, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); - TwistFriction.WarmStart2(prestep.Normal, inertiaA, inertiaB, accumulatedImpulses.Twist, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart2(inertiaA, inertiaB, normal, worldOffsetA0, worldOffsetA0 - ab, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart2(inertiaA, inertiaB, normal, worldOffsetA1, worldOffsetA1 - ab, accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); + TwistFriction.WarmStart2(normal, inertiaA, inertiaB, accumulatedImpulses.Twist, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in Contact2PrestepData prestep, ref Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + QuaternionWide.TransformWithoutOverlap(prestep.LocalNormalB, orientationB, out var normal); + Helpers.BuildOrthonormalBasis(normal, out var x, out var z); + QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); + QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); var premultipliedFrictionCoefficient = new Vector(1f / 2f) * prestep.MaterialProperties.FrictionCoefficient; var maximumTangentImpulse = premultipliedFrictionCoefficient * (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1); - FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, out var offsetToManifoldCenterA); - Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); + FrictionHelpers.ComputeFrictionCenter(worldOffsetA0, worldOffsetA1, prestep.Contact0.Depth, prestep.Contact1.Depth, out var offsetToManifoldCenterA); + var ab = positionB - positionA; + Vector3Wide.Subtract(offsetToManifoldCenterA, ab, out var offsetToManifoldCenterB); TangentFriction.Solve2(x, z, offsetToManifoldCenterA, offsetToManifoldCenterB, inertiaA, inertiaB, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); //Note that we solve the penetration constraints after the friction constraints. //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. //It's a pretty minor effect either way. SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); var inverseDtWide = new Vector(inverseDt); - PenetrationLimit.Solve2(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - PenetrationLimit.Solve2(inertiaA, inertiaB, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.OffsetA - prestep.OffsetB, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); + PenetrationLimit.Solve2(inertiaA, inertiaB, normal, worldOffsetA0, worldOffsetA0 - ab, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + PenetrationLimit.Solve2(inertiaA, inertiaB, normal, worldOffsetA0, worldOffsetA1 - ab, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); var maximumTwistImpulse = premultipliedFrictionCoefficient * ( - accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact0.OffsetA) + - accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact1.OffsetA)); - TwistFriction.Solve2(prestep.Normal, inertiaA, inertiaB, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); + accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, worldOffsetA0) + + accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, worldOffsetA1)); + TwistFriction.Solve2(normal, inertiaA, inertiaB, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -1562,9 +1627,9 @@ public void UpdateForNewPose( in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector dt, in Contact2AccumulatedImpulses accumulatedImpulses, ref Contact2PrestepData prestep) { - prestep.OffsetB = positionB - positionA; - PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact0.Depth); - PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact1.Depth); + //prestep.OffsetB = positionB - positionA; + //PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact0.Depth); + //PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact1.Depth); } } @@ -1584,8 +1649,7 @@ public struct Contact3 : IConvexTwoBodyContactConstraintDescription public ConstraintContactData Contact0; public ConstraintContactData Contact1; public ConstraintContactData Contact2; - public Vector3 OffsetB; - public Vector3 Normal; + public Vector3 LocalNormalB; public float FrictionCoefficient; public SpringSettings SpringSettings; public float MaximumRecoveryVelocity; @@ -1594,15 +1658,17 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int { Debug.Assert(batch.TypeId == ConstraintTypeId, "The type batch passed to the description must match the description's expected type."); ref var target = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); - Vector3Wide.WriteFirst(Contact0.OffsetA, ref target.Contact0.OffsetA); + Vector3Wide.WriteFirst(Contact0.LocalOffsetA, ref target.Contact0.LocalOffsetA); GetFirst(ref target.Contact0.Depth) = Contact0.PenetrationDepth; - Vector3Wide.WriteFirst(Contact1.OffsetA, ref target.Contact1.OffsetA); + GetFirst(ref target.Contact0.PlaneOffsetB) = Contact0.PlaneOffsetB; + Vector3Wide.WriteFirst(Contact1.LocalOffsetA, ref target.Contact1.LocalOffsetA); GetFirst(ref target.Contact1.Depth) = Contact1.PenetrationDepth; - Vector3Wide.WriteFirst(Contact2.OffsetA, ref target.Contact2.OffsetA); + GetFirst(ref target.Contact1.PlaneOffsetB) = Contact1.PlaneOffsetB; + Vector3Wide.WriteFirst(Contact2.LocalOffsetA, ref target.Contact2.LocalOffsetA); GetFirst(ref target.Contact2.Depth) = Contact2.PenetrationDepth; - - Vector3Wide.WriteFirst(OffsetB, ref target.OffsetB); - Vector3Wide.WriteFirst(Normal, ref target.Normal); + GetFirst(ref target.Contact2.PlaneOffsetB) = Contact2.PlaneOffsetB; + + Vector3Wide.WriteFirst(LocalNormalB, ref target.LocalNormalB); GetFirst(ref target.MaterialProperties.FrictionCoefficient) = FrictionCoefficient; SpringSettingsWide.WriteFirst(SpringSettings, ref target.MaterialProperties.SpringSettings); GetFirst(ref target.MaterialProperties.MaximumRecoveryVelocity) = MaximumRecoveryVelocity; @@ -1612,26 +1678,26 @@ public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int { Debug.Assert(batch.TypeId == ConstraintTypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); - Vector3Wide.ReadFirst(source.Contact0.OffsetA, out description.Contact0.OffsetA); + Vector3Wide.ReadFirst(source.Contact0.LocalOffsetA, out description.Contact0.LocalOffsetA); description.Contact0.PenetrationDepth = GetFirst(ref source.Contact0.Depth); - Vector3Wide.ReadFirst(source.Contact1.OffsetA, out description.Contact1.OffsetA); + description.Contact0.PlaneOffsetB = GetFirst(ref source.Contact0.PlaneOffsetB); + Vector3Wide.ReadFirst(source.Contact1.LocalOffsetA, out description.Contact1.LocalOffsetA); description.Contact1.PenetrationDepth = GetFirst(ref source.Contact1.Depth); - Vector3Wide.ReadFirst(source.Contact2.OffsetA, out description.Contact2.OffsetA); + description.Contact1.PlaneOffsetB = GetFirst(ref source.Contact1.PlaneOffsetB); + Vector3Wide.ReadFirst(source.Contact2.LocalOffsetA, out description.Contact2.LocalOffsetA); description.Contact2.PenetrationDepth = GetFirst(ref source.Contact2.Depth); - - Vector3Wide.ReadFirst(source.OffsetB, out description.OffsetB); - Vector3Wide.ReadFirst(source.Normal, out description.Normal); + description.Contact2.PlaneOffsetB = GetFirst(ref source.Contact2.PlaneOffsetB); + Vector3Wide.ReadFirst(source.LocalNormalB, out description.LocalNormalB); description.FrictionCoefficient = GetFirst(ref source.MaterialProperties.FrictionCoefficient); SpringSettingsWide.ReadFirst(source.MaterialProperties.SpringSettings, out description.SpringSettings); description.MaximumRecoveryVelocity = GetFirst(ref source.MaterialProperties.MaximumRecoveryVelocity); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void CopyManifoldWideProperties(ref Vector3 offsetB, ref Vector3 normal, ref PairMaterialProperties material) + public void CopyManifoldWideProperties(ref Vector3 localNormalB, ref PairMaterialProperties material) { - OffsetB = offsetB; FrictionCoefficient = material.FrictionCoefficient; - Normal = normal; + LocalNormalB = localNormalB; SpringSettings = material.SpringSettings; MaximumRecoveryVelocity = material.MaximumRecoveryVelocity; } @@ -1660,18 +1726,17 @@ public struct Contact3PrestepData : ITwoBodyConvexContactPrestep 2; public readonly int ContactCount => 3; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3Wide GetNormal(ref Contact3PrestepData prestep) + public ref Vector3Wide GetLocalNormalB(ref Contact3PrestepData prestep) { - return ref prestep.Normal; + return ref prestep.LocalNormalB; } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -1685,12 +1750,6 @@ public ref MaterialPropertiesWide GetMaterialProperties(ref Contact3PrestepData { return ref prestep.MaterialProperties; } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3Wide GetOffsetB(ref Contact3PrestepData prestep) - { - return ref prestep.OffsetB; - } } public unsafe struct Contact3Projection @@ -1718,102 +1777,116 @@ public void Prestep( in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref Contact3PrestepData prestep, out Contact3Projection projection) { //Be careful about the execution order here. It should be aligned with the prestep data layout to ensure prefetching works well. - projection.InertiaA = inertiaA; - projection.InertiaB = inertiaB; - FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, out var offsetToManifoldCenterA); - Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); - projection.PremultipliedFrictionCoefficient = (1f / 3f) * prestep.MaterialProperties.FrictionCoefficient; - projection.Normal = prestep.Normal; - Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); - TangentFriction.Prestep(ref x, ref z, ref offsetToManifoldCenterA, ref offsetToManifoldCenterB, ref projection.InertiaA, ref projection.InertiaB, out projection.Tangent); - SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); - Vector3Wide contactOffsetB; - Vector3Wide.Subtract(prestep.Contact0.OffsetA, prestep.OffsetB, out contactOffsetB); - PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, prestep.Contact0.OffsetA, contactOffsetB, prestep.Normal, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration0); - Vector3Wide.Subtract(prestep.Contact1.OffsetA, prestep.OffsetB, out contactOffsetB); - PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, prestep.Contact1.OffsetA, contactOffsetB, prestep.Normal, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration1); - Vector3Wide.Subtract(prestep.Contact2.OffsetA, prestep.OffsetB, out contactOffsetB); - PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, prestep.Contact2.OffsetA, contactOffsetB, prestep.Normal, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration2); - Vector3Wide.Distance(prestep.Contact0.OffsetA, offsetToManifoldCenterA, out projection.LeverArm0); - Vector3Wide.Distance(prestep.Contact1.OffsetA, offsetToManifoldCenterA, out projection.LeverArm1); - Vector3Wide.Distance(prestep.Contact2.OffsetA, offsetToManifoldCenterA, out projection.LeverArm2); - TwistFriction.Prestep(ref projection.InertiaA, ref projection.InertiaB, ref prestep.Normal, out projection.Twist); + //projection.InertiaA = inertiaA; + //projection.InertiaB = inertiaB; + //QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); + //QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); + //QuaternionWide.TransformWithoutOverlap(prestep.Contact2.LocalOffsetA, orientationA, out var worldOffsetA2); + //FrictionHelpers.ComputeFrictionCenter(worldOffsetA0, worldOffsetA1, worldOffsetA2, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, out var offsetToManifoldCenterA); + //Vector3Wide.Subtract(offsetToManifoldCenterA, ab, out var offsetToManifoldCenterB); + //projection.PremultipliedFrictionCoefficient = (1f / 3f) * prestep.MaterialProperties.FrictionCoefficient; + //projection.Normal = prestep.Normal; + //Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + //TangentFriction.Prestep(ref x, ref z, ref offsetToManifoldCenterA, ref offsetToManifoldCenterB, ref projection.InertiaA, ref projection.InertiaB, out projection.Tangent); + //SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); + //PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, worldOffsetA0, worldOffsetA0 - ab, prestep.Normal, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration0); + //PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, worldOffsetA1, worldOffsetA1 - ab, prestep.Normal, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration1); + //PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, worldOffsetA2, worldOffsetA2 - ab, prestep.Normal, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration2); + //Vector3Wide.Distance(worldOffsetA0, offsetToManifoldCenterA, out projection.LeverArm0); + //Vector3Wide.Distance(worldOffsetA1, offsetToManifoldCenterA, out projection.LeverArm1); + //Vector3Wide.Distance(worldOffsetA2, offsetToManifoldCenterA, out projection.LeverArm2); + //TwistFriction.Prestep(ref projection.InertiaA, ref projection.InertiaB, ref prestep.Normal, out projection.Twist); + + projection = default; } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart(ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref Contact3Projection projection, ref Contact3AccumulatedImpulses accumulatedImpulses) { - Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - TangentFriction.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart(projection.Penetration0, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart(projection.Penetration1, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart(projection.Penetration2, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); - TwistFriction.WarmStart(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); + //Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); + //TangentFriction.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); + //PenetrationLimit.WarmStart(projection.Penetration0, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + //PenetrationLimit.WarmStart(projection.Penetration1, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); + //PenetrationLimit.WarmStart(projection.Penetration2, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); + //TwistFriction.WarmStart(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve(ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref Contact3Projection projection, ref Contact3AccumulatedImpulses accumulatedImpulses) { - Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * - (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2); - TangentFriction.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref projection.InertiaB, ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); + //Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); + //var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * + // (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2); + //TangentFriction.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref projection.InertiaB, ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); //Note that we solve the penetration constraints after the friction constraints. //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. //It's a pretty minor effect either way. - PenetrationLimit.Solve(projection.Penetration0, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - PenetrationLimit.Solve(projection.Penetration1, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); - PenetrationLimit.Solve(projection.Penetration2, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); - var maximumTwistImpulse = projection.PremultipliedFrictionCoefficient * ( - accumulatedImpulses.Penetration0 * projection.LeverArm0 + - accumulatedImpulses.Penetration1 * projection.LeverArm1 + - accumulatedImpulses.Penetration2 * projection.LeverArm2); - TwistFriction.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); + //PenetrationLimit.Solve(projection.Penetration0, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + //PenetrationLimit.Solve(projection.Penetration1, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); + //PenetrationLimit.Solve(projection.Penetration2, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); + //var maximumTwistImpulse = projection.PremultipliedFrictionCoefficient * ( + // accumulatedImpulses.Penetration0 * projection.LeverArm0 + + // accumulatedImpulses.Penetration1 * projection.LeverArm1 + + // accumulatedImpulses.Penetration2 * projection.LeverArm2); + //TwistFriction.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityWide velocityA, in BodyVelocityWide velocityB, ref Contact3PrestepData prestep) { - PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact0.Depth); - PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact1.Depth); - PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact2.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact2.Depth); + //QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); + //PenetrationLimit.UpdatePenetrationDepth(dt, worldOffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact0.Depth); + //QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); + //PenetrationLimit.UpdatePenetrationDepth(dt, worldOffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact1.Depth); + //QuaternionWide.TransformWithoutOverlap(prestep.Contact2.LocalOffsetA, orientationA, out var worldOffsetA2); + //PenetrationLimit.UpdatePenetrationDepth(dt, worldOffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact2.Depth); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Contact3PrestepData prestep, in Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); - FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, out var offsetToManifoldCenterA); - Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); + QuaternionWide.TransformWithoutOverlap(prestep.LocalNormalB, orientationB, out var normal); + Helpers.BuildOrthonormalBasis(normal, out var x, out var z); + QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); + QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); + QuaternionWide.TransformWithoutOverlap(prestep.Contact2.LocalOffsetA, orientationA, out var worldOffsetA2); + FrictionHelpers.ComputeFrictionCenter(worldOffsetA0, worldOffsetA1, worldOffsetA2, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, out var offsetToManifoldCenterA); + var ab = positionB - positionA; + Vector3Wide.Subtract(offsetToManifoldCenterA, ab, out var offsetToManifoldCenterB); TangentFriction.WarmStart2(x, z, offsetToManifoldCenterA, offsetToManifoldCenterB, inertiaA, inertiaB, accumulatedImpulses.Tangent, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact2.OffsetA, prestep.Contact2.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); - TwistFriction.WarmStart2(prestep.Normal, inertiaA, inertiaB, accumulatedImpulses.Twist, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart2(inertiaA, inertiaB, normal, worldOffsetA0, worldOffsetA0 - ab, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart2(inertiaA, inertiaB, normal, worldOffsetA1, worldOffsetA1 - ab, accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart2(inertiaA, inertiaB, normal, worldOffsetA2, worldOffsetA2 - ab, accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); + TwistFriction.WarmStart2(normal, inertiaA, inertiaB, accumulatedImpulses.Twist, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in Contact3PrestepData prestep, ref Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + QuaternionWide.TransformWithoutOverlap(prestep.LocalNormalB, orientationB, out var normal); + Helpers.BuildOrthonormalBasis(normal, out var x, out var z); + QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); + QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); + QuaternionWide.TransformWithoutOverlap(prestep.Contact2.LocalOffsetA, orientationA, out var worldOffsetA2); var premultipliedFrictionCoefficient = new Vector(1f / 3f) * prestep.MaterialProperties.FrictionCoefficient; var maximumTangentImpulse = premultipliedFrictionCoefficient * (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2); - FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, out var offsetToManifoldCenterA); - Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); + FrictionHelpers.ComputeFrictionCenter(worldOffsetA0, worldOffsetA1, worldOffsetA2, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, out var offsetToManifoldCenterA); + var ab = positionB - positionA; + Vector3Wide.Subtract(offsetToManifoldCenterA, ab, out var offsetToManifoldCenterB); TangentFriction.Solve2(x, z, offsetToManifoldCenterA, offsetToManifoldCenterB, inertiaA, inertiaB, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); //Note that we solve the penetration constraints after the friction constraints. //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. //It's a pretty minor effect either way. SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); var inverseDtWide = new Vector(inverseDt); - PenetrationLimit.Solve2(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - PenetrationLimit.Solve2(inertiaA, inertiaB, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.OffsetA - prestep.OffsetB, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); - PenetrationLimit.Solve2(inertiaA, inertiaB, prestep.Normal, prestep.Contact2.OffsetA, prestep.Contact2.OffsetA - prestep.OffsetB, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); + PenetrationLimit.Solve2(inertiaA, inertiaB, normal, worldOffsetA0, worldOffsetA0 - ab, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + PenetrationLimit.Solve2(inertiaA, inertiaB, normal, worldOffsetA0, worldOffsetA1 - ab, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); + PenetrationLimit.Solve2(inertiaA, inertiaB, normal, worldOffsetA0, worldOffsetA2 - ab, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); var maximumTwistImpulse = premultipliedFrictionCoefficient * ( - accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact0.OffsetA) + - accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact1.OffsetA) + - accumulatedImpulses.Penetration2 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact2.OffsetA)); - TwistFriction.Solve2(prestep.Normal, inertiaA, inertiaB, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); + accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, worldOffsetA0) + + accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, worldOffsetA1) + + accumulatedImpulses.Penetration2 * Vector3Wide.Distance(offsetToManifoldCenterA, worldOffsetA2)); + TwistFriction.Solve2(normal, inertiaA, inertiaB, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -1822,10 +1895,10 @@ public void UpdateForNewPose( in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector dt, in Contact3AccumulatedImpulses accumulatedImpulses, ref Contact3PrestepData prestep) { - prestep.OffsetB = positionB - positionA; - PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact0.Depth); - PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact1.Depth); - PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact2.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact2.Depth); + //prestep.OffsetB = positionB - positionA; + //PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact0.Depth); + //PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact1.Depth); + //PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact2.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact2.Depth); } } @@ -1846,8 +1919,7 @@ public struct Contact4 : IConvexTwoBodyContactConstraintDescription public ConstraintContactData Contact1; public ConstraintContactData Contact2; public ConstraintContactData Contact3; - public Vector3 OffsetB; - public Vector3 Normal; + public Vector3 LocalNormalB; public float FrictionCoefficient; public SpringSettings SpringSettings; public float MaximumRecoveryVelocity; @@ -1856,17 +1928,20 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int { Debug.Assert(batch.TypeId == ConstraintTypeId, "The type batch passed to the description must match the description's expected type."); ref var target = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); - Vector3Wide.WriteFirst(Contact0.OffsetA, ref target.Contact0.OffsetA); + Vector3Wide.WriteFirst(Contact0.LocalOffsetA, ref target.Contact0.LocalOffsetA); GetFirst(ref target.Contact0.Depth) = Contact0.PenetrationDepth; - Vector3Wide.WriteFirst(Contact1.OffsetA, ref target.Contact1.OffsetA); + GetFirst(ref target.Contact0.PlaneOffsetB) = Contact0.PlaneOffsetB; + Vector3Wide.WriteFirst(Contact1.LocalOffsetA, ref target.Contact1.LocalOffsetA); GetFirst(ref target.Contact1.Depth) = Contact1.PenetrationDepth; - Vector3Wide.WriteFirst(Contact2.OffsetA, ref target.Contact2.OffsetA); + GetFirst(ref target.Contact1.PlaneOffsetB) = Contact1.PlaneOffsetB; + Vector3Wide.WriteFirst(Contact2.LocalOffsetA, ref target.Contact2.LocalOffsetA); GetFirst(ref target.Contact2.Depth) = Contact2.PenetrationDepth; - Vector3Wide.WriteFirst(Contact3.OffsetA, ref target.Contact3.OffsetA); + GetFirst(ref target.Contact2.PlaneOffsetB) = Contact2.PlaneOffsetB; + Vector3Wide.WriteFirst(Contact3.LocalOffsetA, ref target.Contact3.LocalOffsetA); GetFirst(ref target.Contact3.Depth) = Contact3.PenetrationDepth; - - Vector3Wide.WriteFirst(OffsetB, ref target.OffsetB); - Vector3Wide.WriteFirst(Normal, ref target.Normal); + GetFirst(ref target.Contact3.PlaneOffsetB) = Contact3.PlaneOffsetB; + + Vector3Wide.WriteFirst(LocalNormalB, ref target.LocalNormalB); GetFirst(ref target.MaterialProperties.FrictionCoefficient) = FrictionCoefficient; SpringSettingsWide.WriteFirst(SpringSettings, ref target.MaterialProperties.SpringSettings); GetFirst(ref target.MaterialProperties.MaximumRecoveryVelocity) = MaximumRecoveryVelocity; @@ -1876,28 +1951,29 @@ public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int { Debug.Assert(batch.TypeId == ConstraintTypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); - Vector3Wide.ReadFirst(source.Contact0.OffsetA, out description.Contact0.OffsetA); + Vector3Wide.ReadFirst(source.Contact0.LocalOffsetA, out description.Contact0.LocalOffsetA); description.Contact0.PenetrationDepth = GetFirst(ref source.Contact0.Depth); - Vector3Wide.ReadFirst(source.Contact1.OffsetA, out description.Contact1.OffsetA); + description.Contact0.PlaneOffsetB = GetFirst(ref source.Contact0.PlaneOffsetB); + Vector3Wide.ReadFirst(source.Contact1.LocalOffsetA, out description.Contact1.LocalOffsetA); description.Contact1.PenetrationDepth = GetFirst(ref source.Contact1.Depth); - Vector3Wide.ReadFirst(source.Contact2.OffsetA, out description.Contact2.OffsetA); + description.Contact1.PlaneOffsetB = GetFirst(ref source.Contact1.PlaneOffsetB); + Vector3Wide.ReadFirst(source.Contact2.LocalOffsetA, out description.Contact2.LocalOffsetA); description.Contact2.PenetrationDepth = GetFirst(ref source.Contact2.Depth); - Vector3Wide.ReadFirst(source.Contact3.OffsetA, out description.Contact3.OffsetA); + description.Contact2.PlaneOffsetB = GetFirst(ref source.Contact2.PlaneOffsetB); + Vector3Wide.ReadFirst(source.Contact3.LocalOffsetA, out description.Contact3.LocalOffsetA); description.Contact3.PenetrationDepth = GetFirst(ref source.Contact3.Depth); - - Vector3Wide.ReadFirst(source.OffsetB, out description.OffsetB); - Vector3Wide.ReadFirst(source.Normal, out description.Normal); + description.Contact3.PlaneOffsetB = GetFirst(ref source.Contact3.PlaneOffsetB); + Vector3Wide.ReadFirst(source.LocalNormalB, out description.LocalNormalB); description.FrictionCoefficient = GetFirst(ref source.MaterialProperties.FrictionCoefficient); SpringSettingsWide.ReadFirst(source.MaterialProperties.SpringSettings, out description.SpringSettings); description.MaximumRecoveryVelocity = GetFirst(ref source.MaterialProperties.MaximumRecoveryVelocity); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void CopyManifoldWideProperties(ref Vector3 offsetB, ref Vector3 normal, ref PairMaterialProperties material) + public void CopyManifoldWideProperties(ref Vector3 localNormalB, ref PairMaterialProperties material) { - OffsetB = offsetB; FrictionCoefficient = material.FrictionCoefficient; - Normal = normal; + LocalNormalB = localNormalB; SpringSettings = material.SpringSettings; MaximumRecoveryVelocity = material.MaximumRecoveryVelocity; } @@ -1927,18 +2003,17 @@ public struct Contact4PrestepData : ITwoBodyConvexContactPrestep 2; public readonly int ContactCount => 4; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3Wide GetNormal(ref Contact4PrestepData prestep) + public ref Vector3Wide GetLocalNormalB(ref Contact4PrestepData prestep) { - return ref prestep.Normal; + return ref prestep.LocalNormalB; } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -1952,12 +2027,6 @@ public ref MaterialPropertiesWide GetMaterialProperties(ref Contact4PrestepData { return ref prestep.MaterialProperties; } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3Wide GetOffsetB(ref Contact4PrestepData prestep) - { - return ref prestep.OffsetB; - } } public unsafe struct Contact4Projection @@ -1987,112 +2056,129 @@ public void Prestep( in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref Contact4PrestepData prestep, out Contact4Projection projection) { //Be careful about the execution order here. It should be aligned with the prestep data layout to ensure prefetching works well. - projection.InertiaA = inertiaA; - projection.InertiaB = inertiaB; - FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact3.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, prestep.Contact3.Depth, out var offsetToManifoldCenterA); - Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); - projection.PremultipliedFrictionCoefficient = (1f / 4f) * prestep.MaterialProperties.FrictionCoefficient; - projection.Normal = prestep.Normal; - Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); - TangentFriction.Prestep(ref x, ref z, ref offsetToManifoldCenterA, ref offsetToManifoldCenterB, ref projection.InertiaA, ref projection.InertiaB, out projection.Tangent); - SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); - Vector3Wide contactOffsetB; - Vector3Wide.Subtract(prestep.Contact0.OffsetA, prestep.OffsetB, out contactOffsetB); - PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, prestep.Contact0.OffsetA, contactOffsetB, prestep.Normal, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration0); - Vector3Wide.Subtract(prestep.Contact1.OffsetA, prestep.OffsetB, out contactOffsetB); - PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, prestep.Contact1.OffsetA, contactOffsetB, prestep.Normal, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration1); - Vector3Wide.Subtract(prestep.Contact2.OffsetA, prestep.OffsetB, out contactOffsetB); - PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, prestep.Contact2.OffsetA, contactOffsetB, prestep.Normal, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration2); - Vector3Wide.Subtract(prestep.Contact3.OffsetA, prestep.OffsetB, out contactOffsetB); - PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, prestep.Contact3.OffsetA, contactOffsetB, prestep.Normal, prestep.Contact3.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration3); - Vector3Wide.Distance(prestep.Contact0.OffsetA, offsetToManifoldCenterA, out projection.LeverArm0); - Vector3Wide.Distance(prestep.Contact1.OffsetA, offsetToManifoldCenterA, out projection.LeverArm1); - Vector3Wide.Distance(prestep.Contact2.OffsetA, offsetToManifoldCenterA, out projection.LeverArm2); - Vector3Wide.Distance(prestep.Contact3.OffsetA, offsetToManifoldCenterA, out projection.LeverArm3); - TwistFriction.Prestep(ref projection.InertiaA, ref projection.InertiaB, ref prestep.Normal, out projection.Twist); + //projection.InertiaA = inertiaA; + //projection.InertiaB = inertiaB; + //QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); + //QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); + //QuaternionWide.TransformWithoutOverlap(prestep.Contact2.LocalOffsetA, orientationA, out var worldOffsetA2); + //QuaternionWide.TransformWithoutOverlap(prestep.Contact3.LocalOffsetA, orientationA, out var worldOffsetA3); + //FrictionHelpers.ComputeFrictionCenter(worldOffsetA0, worldOffsetA1, worldOffsetA2, worldOffsetA3, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, prestep.Contact3.Depth, out var offsetToManifoldCenterA); + //Vector3Wide.Subtract(offsetToManifoldCenterA, ab, out var offsetToManifoldCenterB); + //projection.PremultipliedFrictionCoefficient = (1f / 4f) * prestep.MaterialProperties.FrictionCoefficient; + //projection.Normal = prestep.Normal; + //Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + //TangentFriction.Prestep(ref x, ref z, ref offsetToManifoldCenterA, ref offsetToManifoldCenterB, ref projection.InertiaA, ref projection.InertiaB, out projection.Tangent); + //SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); + //PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, worldOffsetA0, worldOffsetA0 - ab, prestep.Normal, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration0); + //PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, worldOffsetA1, worldOffsetA1 - ab, prestep.Normal, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration1); + //PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, worldOffsetA2, worldOffsetA2 - ab, prestep.Normal, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration2); + //PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, worldOffsetA3, worldOffsetA3 - ab, prestep.Normal, prestep.Contact3.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration3); + //Vector3Wide.Distance(worldOffsetA0, offsetToManifoldCenterA, out projection.LeverArm0); + //Vector3Wide.Distance(worldOffsetA1, offsetToManifoldCenterA, out projection.LeverArm1); + //Vector3Wide.Distance(worldOffsetA2, offsetToManifoldCenterA, out projection.LeverArm2); + //Vector3Wide.Distance(worldOffsetA3, offsetToManifoldCenterA, out projection.LeverArm3); + //TwistFriction.Prestep(ref projection.InertiaA, ref projection.InertiaB, ref prestep.Normal, out projection.Twist); + + projection = default; } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart(ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref Contact4Projection projection, ref Contact4AccumulatedImpulses accumulatedImpulses) { - Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - TangentFriction.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart(projection.Penetration0, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart(projection.Penetration1, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart(projection.Penetration2, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart(projection.Penetration3, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration3, ref wsvA, ref wsvB); - TwistFriction.WarmStart(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); + //Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); + //TangentFriction.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); + //PenetrationLimit.WarmStart(projection.Penetration0, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + //PenetrationLimit.WarmStart(projection.Penetration1, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); + //PenetrationLimit.WarmStart(projection.Penetration2, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); + //PenetrationLimit.WarmStart(projection.Penetration3, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration3, ref wsvA, ref wsvB); + //TwistFriction.WarmStart(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve(ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref Contact4Projection projection, ref Contact4AccumulatedImpulses accumulatedImpulses) { - Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * - (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2 + accumulatedImpulses.Penetration3); - TangentFriction.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref projection.InertiaB, ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); + //Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); + //var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * + // (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2 + accumulatedImpulses.Penetration3); + //TangentFriction.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref projection.InertiaB, ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); //Note that we solve the penetration constraints after the friction constraints. //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. //It's a pretty minor effect either way. - PenetrationLimit.Solve(projection.Penetration0, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - PenetrationLimit.Solve(projection.Penetration1, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); - PenetrationLimit.Solve(projection.Penetration2, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); - PenetrationLimit.Solve(projection.Penetration3, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration3, ref wsvA, ref wsvB); - var maximumTwistImpulse = projection.PremultipliedFrictionCoefficient * ( - accumulatedImpulses.Penetration0 * projection.LeverArm0 + - accumulatedImpulses.Penetration1 * projection.LeverArm1 + - accumulatedImpulses.Penetration2 * projection.LeverArm2 + - accumulatedImpulses.Penetration3 * projection.LeverArm3); - TwistFriction.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); + //PenetrationLimit.Solve(projection.Penetration0, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + //PenetrationLimit.Solve(projection.Penetration1, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); + //PenetrationLimit.Solve(projection.Penetration2, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); + //PenetrationLimit.Solve(projection.Penetration3, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration3, ref wsvA, ref wsvB); + //var maximumTwistImpulse = projection.PremultipliedFrictionCoefficient * ( + // accumulatedImpulses.Penetration0 * projection.LeverArm0 + + // accumulatedImpulses.Penetration1 * projection.LeverArm1 + + // accumulatedImpulses.Penetration2 * projection.LeverArm2 + + // accumulatedImpulses.Penetration3 * projection.LeverArm3); + //TwistFriction.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityWide velocityA, in BodyVelocityWide velocityB, ref Contact4PrestepData prestep) { - PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact0.Depth); - PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact1.Depth); - PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact2.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact2.Depth); - PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact3.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact3.Depth); + //QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); + //PenetrationLimit.UpdatePenetrationDepth(dt, worldOffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact0.Depth); + //QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); + //PenetrationLimit.UpdatePenetrationDepth(dt, worldOffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact1.Depth); + //QuaternionWide.TransformWithoutOverlap(prestep.Contact2.LocalOffsetA, orientationA, out var worldOffsetA2); + //PenetrationLimit.UpdatePenetrationDepth(dt, worldOffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact2.Depth); + //QuaternionWide.TransformWithoutOverlap(prestep.Contact3.LocalOffsetA, orientationA, out var worldOffsetA3); + //PenetrationLimit.UpdatePenetrationDepth(dt, worldOffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact3.Depth); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Contact4PrestepData prestep, in Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); - FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact3.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, prestep.Contact3.Depth, out var offsetToManifoldCenterA); - Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); + QuaternionWide.TransformWithoutOverlap(prestep.LocalNormalB, orientationB, out var normal); + Helpers.BuildOrthonormalBasis(normal, out var x, out var z); + QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); + QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); + QuaternionWide.TransformWithoutOverlap(prestep.Contact2.LocalOffsetA, orientationA, out var worldOffsetA2); + QuaternionWide.TransformWithoutOverlap(prestep.Contact3.LocalOffsetA, orientationA, out var worldOffsetA3); + FrictionHelpers.ComputeFrictionCenter(worldOffsetA0, worldOffsetA1, worldOffsetA2, worldOffsetA3, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, prestep.Contact3.Depth, out var offsetToManifoldCenterA); + var ab = positionB - positionA; + Vector3Wide.Subtract(offsetToManifoldCenterA, ab, out var offsetToManifoldCenterB); TangentFriction.WarmStart2(x, z, offsetToManifoldCenterA, offsetToManifoldCenterB, inertiaA, inertiaB, accumulatedImpulses.Tangent, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact2.OffsetA, prestep.Contact2.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact3.OffsetA, prestep.Contact3.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration3, ref wsvA, ref wsvB); - TwistFriction.WarmStart2(prestep.Normal, inertiaA, inertiaB, accumulatedImpulses.Twist, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart2(inertiaA, inertiaB, normal, worldOffsetA0, worldOffsetA0 - ab, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart2(inertiaA, inertiaB, normal, worldOffsetA1, worldOffsetA1 - ab, accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart2(inertiaA, inertiaB, normal, worldOffsetA2, worldOffsetA2 - ab, accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart2(inertiaA, inertiaB, normal, worldOffsetA3, worldOffsetA3 - ab, accumulatedImpulses.Penetration3, ref wsvA, ref wsvB); + TwistFriction.WarmStart2(normal, inertiaA, inertiaB, accumulatedImpulses.Twist, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in Contact4PrestepData prestep, ref Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + QuaternionWide.TransformWithoutOverlap(prestep.LocalNormalB, orientationB, out var normal); + Helpers.BuildOrthonormalBasis(normal, out var x, out var z); + QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); + QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); + QuaternionWide.TransformWithoutOverlap(prestep.Contact2.LocalOffsetA, orientationA, out var worldOffsetA2); + QuaternionWide.TransformWithoutOverlap(prestep.Contact3.LocalOffsetA, orientationA, out var worldOffsetA3); var premultipliedFrictionCoefficient = new Vector(1f / 4f) * prestep.MaterialProperties.FrictionCoefficient; var maximumTangentImpulse = premultipliedFrictionCoefficient * (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2 + accumulatedImpulses.Penetration3); - FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact3.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, prestep.Contact3.Depth, out var offsetToManifoldCenterA); - Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); + FrictionHelpers.ComputeFrictionCenter(worldOffsetA0, worldOffsetA1, worldOffsetA2, worldOffsetA3, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, prestep.Contact3.Depth, out var offsetToManifoldCenterA); + var ab = positionB - positionA; + Vector3Wide.Subtract(offsetToManifoldCenterA, ab, out var offsetToManifoldCenterB); TangentFriction.Solve2(x, z, offsetToManifoldCenterA, offsetToManifoldCenterB, inertiaA, inertiaB, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); //Note that we solve the penetration constraints after the friction constraints. //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. //It's a pretty minor effect either way. SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); var inverseDtWide = new Vector(inverseDt); - PenetrationLimit.Solve2(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - PenetrationLimit.Solve2(inertiaA, inertiaB, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.OffsetA - prestep.OffsetB, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); - PenetrationLimit.Solve2(inertiaA, inertiaB, prestep.Normal, prestep.Contact2.OffsetA, prestep.Contact2.OffsetA - prestep.OffsetB, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); - PenetrationLimit.Solve2(inertiaA, inertiaB, prestep.Normal, prestep.Contact3.OffsetA, prestep.Contact3.OffsetA - prestep.OffsetB, prestep.Contact3.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration3, ref wsvA, ref wsvB); + PenetrationLimit.Solve2(inertiaA, inertiaB, normal, worldOffsetA0, worldOffsetA0 - ab, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + PenetrationLimit.Solve2(inertiaA, inertiaB, normal, worldOffsetA0, worldOffsetA1 - ab, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); + PenetrationLimit.Solve2(inertiaA, inertiaB, normal, worldOffsetA0, worldOffsetA2 - ab, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); + PenetrationLimit.Solve2(inertiaA, inertiaB, normal, worldOffsetA0, worldOffsetA3 - ab, prestep.Contact3.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration3, ref wsvA, ref wsvB); var maximumTwistImpulse = premultipliedFrictionCoefficient * ( - accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact0.OffsetA) + - accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact1.OffsetA) + - accumulatedImpulses.Penetration2 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact2.OffsetA) + - accumulatedImpulses.Penetration3 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact3.OffsetA)); - TwistFriction.Solve2(prestep.Normal, inertiaA, inertiaB, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); + accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, worldOffsetA0) + + accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, worldOffsetA1) + + accumulatedImpulses.Penetration2 * Vector3Wide.Distance(offsetToManifoldCenterA, worldOffsetA2) + + accumulatedImpulses.Penetration3 * Vector3Wide.Distance(offsetToManifoldCenterA, worldOffsetA3)); + TwistFriction.Solve2(normal, inertiaA, inertiaB, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -2101,11 +2187,11 @@ public void UpdateForNewPose( in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector dt, in Contact4AccumulatedImpulses accumulatedImpulses, ref Contact4PrestepData prestep) { - prestep.OffsetB = positionB - positionA; - PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact0.Depth); - PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact1.Depth); - PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact2.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact2.Depth); - PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact3.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact3.Depth); + //prestep.OffsetB = positionB - positionA; + //PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact0.Depth); + //PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact1.Depth); + //PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact2.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact2.Depth); + //PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact3.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact3.Depth); } } diff --git a/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt b/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt index c6234671e..db9800956 100644 --- a/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt +++ b/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt @@ -100,10 +100,7 @@ for (int i = 0; i < contactCount; ++i) {#> public ConstraintContactData Contact<#=i#>; <#}#> -<#if (bodyCount == 2) {#> - public Vector3 OffsetB; -<#}#> - public Vector3 Normal; + public Vector3 LocalNormalB; public float FrictionCoefficient; public SpringSettings SpringSettings; public float MaximumRecoveryVelocity; @@ -113,13 +110,11 @@ for (int i = 0; i < contactCount; ++i) Debug.Assert(batch.TypeId == ConstraintTypeId, "The type batch passed to the description must match the description's expected type."); ref var target = ref GetOffsetInstance(ref Buffer<#=suffix#>PrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex); <# for (int i = 0; i < contactCount; ++i) {#> - Vector3Wide.WriteFirst(Contact<#=i#>.OffsetA, ref target.Contact<#=i#>.OffsetA); + Vector3Wide.WriteFirst(Contact<#=i#>.LocalOffsetA, ref target.Contact<#=i#>.LocalOffsetA); GetFirst(ref target.Contact<#=i#>.Depth) = Contact<#=i#>.PenetrationDepth; -<#}#> -<#if (bodyCount == 2) {#> - Vector3Wide.WriteFirst(OffsetB, ref target.OffsetB); -<#}#> - Vector3Wide.WriteFirst(Normal, ref target.Normal); + GetFirst(ref target.Contact<#=i#>.PlaneOffsetB) = Contact<#=i#>.PlaneOffsetB; +<#}#> + Vector3Wide.WriteFirst(LocalNormalB, ref target.LocalNormalB); GetFirst(ref target.MaterialProperties.FrictionCoefficient) = FrictionCoefficient; SpringSettingsWide.WriteFirst(SpringSettings, ref target.MaterialProperties.SpringSettings); GetFirst(ref target.MaterialProperties.MaximumRecoveryVelocity) = MaximumRecoveryVelocity; @@ -130,26 +125,21 @@ for (int i = 0; i < contactCount; ++i) Debug.Assert(batch.TypeId == ConstraintTypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer<#=suffix#>PrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex); <#for (int i = 0; i < contactCount; ++i) {#> - Vector3Wide.ReadFirst(source.Contact<#=i#>.OffsetA, out description.Contact<#=i#>.OffsetA); + Vector3Wide.ReadFirst(source.Contact<#=i#>.LocalOffsetA, out description.Contact<#=i#>.LocalOffsetA); description.Contact<#=i#>.PenetrationDepth = GetFirst(ref source.Contact<#=i#>.Depth); + description.Contact<#=i#>.PlaneOffsetB = GetFirst(ref source.Contact<#=i#>.PlaneOffsetB); <#}#> -<#if (bodyCount == 2) {#> - Vector3Wide.ReadFirst(source.OffsetB, out description.OffsetB); -<#}#> - Vector3Wide.ReadFirst(source.Normal, out description.Normal); + Vector3Wide.ReadFirst(source.LocalNormalB, out description.LocalNormalB); description.FrictionCoefficient = GetFirst(ref source.MaterialProperties.FrictionCoefficient); SpringSettingsWide.ReadFirst(source.MaterialProperties.SpringSettings, out description.SpringSettings); description.MaximumRecoveryVelocity = GetFirst(ref source.MaterialProperties.MaximumRecoveryVelocity); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void CopyManifoldWideProperties(<#if (bodyCount == 2) {#>ref Vector3 offsetB, <#}#>ref Vector3 normal, ref PairMaterialProperties material) + public void CopyManifoldWideProperties(ref Vector3 localNormalB, ref PairMaterialProperties material) { -<#if (bodyCount == 2) {#> - OffsetB = offsetB; -<#}#> FrictionCoefficient = material.FrictionCoefficient; - Normal = normal; + LocalNormalB = localNormalB; SpringSettings = material.SpringSettings; MaximumRecoveryVelocity = material.MaximumRecoveryVelocity; } @@ -177,21 +167,18 @@ for (int i = 0; i < contactCount; ++i) //Note that this layout is defined by the execution order in the prestep. The function accesses it sequentially to ensure the prefetcher can do its job. <#for (int i = 0; i < contactCount; ++i) {#> public ConvexContactWide Contact<#=i#>; -<#}#> -<#if (bodyCount == 2) {#> - public Vector3Wide OffsetB; <#}#> //In a convex manifold, all contacts share the same normal and tangents. - public Vector3Wide Normal; + public Vector3Wide LocalNormalB; public MaterialPropertiesWide MaterialProperties; public readonly int BodyCount => <#=bodyCount#>; public readonly int ContactCount => <#=contactCount#>; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3Wide GetNormal(ref Contact<#= contactCount #><#=suffix#>PrestepData prestep) + public ref Vector3Wide GetLocalNormalB(ref Contact<#= contactCount #><#=suffix#>PrestepData prestep) { - return ref prestep.Normal; + return ref prestep.LocalNormalB; } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -205,14 +192,6 @@ for (int i = 0; i < contactCount; ++i) { return ref prestep.MaterialProperties; } - -<#if (bodyCount == 2){#> - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3Wide GetOffsetB(ref Contact<#= contactCount #><#=suffix#>PrestepData prestep) - { - return ref prestep.OffsetB; - } -<#}#> } public unsafe struct Contact<#= contactCount #><#=suffix#>Projection @@ -242,127 +221,147 @@ for (int i = 0; i < contactCount; ++i) <#if(bodyCount == 1) {#>in Vector3Wide positionA,<#}#> in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(bodyCount == 2) {#>in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>float dt, float inverseDt, ref Contact<#=contactCount#><#=suffix#>PrestepData prestep, out Contact<#=contactCount#><#=suffix#>Projection projection) { //Be careful about the execution order here. It should be aligned with the prestep data layout to ensure prefetching works well. - projection.InertiaA = inertiaA; + //projection.InertiaA = inertiaA; <#if (bodyCount == 2) {#> - projection.InertiaB = inertiaB; + //projection.InertiaB = inertiaB; +<#}#> +<#for (int i = 0; i < contactCount; ++i) {#> + //QuaternionWide.TransformWithoutOverlap(prestep.Contact<#=i#>.LocalOffsetA, orientationA, out var worldOffsetA<#=i#>); <#}#> <#if (contactCount > 1) {#> - FrictionHelpers.ComputeFrictionCenter(<#for (int i = 0; i < contactCount; ++i) {#>prestep.Contact<#=i#>.OffsetA, <#}#><#for (int i = 0; i < contactCount; ++i) {#>prestep.Contact<#=i#>.Depth, <#}#>out var offsetToManifoldCenterA); + //FrictionHelpers.ComputeFrictionCenter(<#for (int i = 0; i < contactCount; ++i) {#>worldOffsetA<#=i#>, <#}#><#for (int i = 0; i < contactCount; ++i) {#>prestep.Contact<#=i#>.Depth, <#}#>out var offsetToManifoldCenterA); <#if (bodyCount == 2) {#> - Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); + //Vector3Wide.Subtract(offsetToManifoldCenterA, ab, out var offsetToManifoldCenterB); <#}#> <#} else if(bodyCount == 2) {#> - Vector3Wide.Subtract(prestep.Contact0.OffsetA, prestep.OffsetB, out var offsetToManifoldCenterB); -<#}#> - projection.PremultipliedFrictionCoefficient = <#if (contactCount > 1) {#>(1f / <#=contactCount#>f) * <#}#>prestep.MaterialProperties.FrictionCoefficient; - projection.Normal = prestep.Normal; - Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); -<#var offsetName = contactCount == 1 ? "prestep.Contact0.OffsetA" : "offsetToManifoldCenterA";#> - TangentFriction<#=suffix#>.Prestep(ref x, ref z, ref <#=offsetName#>, <#if (bodyCount == 2) {#>ref offsetToManifoldCenterB, <#}#>ref projection.InertiaA, <#if (bodyCount == 2) {#>ref projection.InertiaB, <#}#>out projection.Tangent); - SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); -<#if (bodyCount == 2) {#> - Vector3Wide contactOffsetB; -<#}#> + //Vector3Wide.Subtract(worldOffsetA0, ab, out var offsetToManifoldCenterB); +<#}#> + //projection.PremultipliedFrictionCoefficient = <#if (contactCount > 1) {#>(1f / <#=contactCount#>f) * <#}#>prestep.MaterialProperties.FrictionCoefficient; + //projection.Normal = prestep.Normal; + //Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); +<#var offsetName = contactCount == 1 ? "worldOffsetA0" : "offsetToManifoldCenterA";#> + //TangentFriction<#=suffix#>.Prestep(ref x, ref z, ref <#=offsetName#>, <#if (bodyCount == 2) {#>ref offsetToManifoldCenterB, <#}#>ref projection.InertiaA, <#if (bodyCount == 2) {#>ref projection.InertiaB, <#}#>out projection.Tangent); + //SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); <#for (int i = 0; i < contactCount; ++i) {#> -<#if (bodyCount == 2) {#> - Vector3Wide.Subtract(prestep.Contact<#=i#>.OffsetA, prestep.OffsetB, out contactOffsetB); -<#}#> - PenetrationLimit<#=suffix#>.Prestep(projection.InertiaA, <#if (bodyCount == 2) {#>projection.InertiaB, <#}#>prestep.Contact<#=i#>.OffsetA, <#if (bodyCount == 2) {#>contactOffsetB, <#}#>prestep.Normal, prestep.Contact<#=i#>.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration<#=i#>); + //PenetrationLimit<#=suffix#>.Prestep(projection.InertiaA, <#if (bodyCount == 2) {#>projection.InertiaB, <#}#>worldOffsetA<#=i#>, <#if (bodyCount == 2) {#>worldOffsetA<#=i#> - ab, <#}#>prestep.Normal, prestep.Contact<#=i#>.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration<#=i#>); <#}#> <#if (contactCount == 1) {#> //If there's only one contact, then the contact patch as determined by contact distance would be zero. //That can cause some subtle behavioral issues sometimes, so we approximate lever arm with the contact depth, assuming that the contact surface area will increase as the depth increases. - projection.LeverArm0 = Vector.Max(Vector.Zero, prestep.Contact0.Depth); + //projection.LeverArm0 = Vector.Max(Vector.Zero, prestep.Contact0.Depth); <#} else {#> <#for (int i = 0; i < contactCount; ++i) {#> - Vector3Wide.Distance(prestep.Contact<#=i#>.OffsetA, <#=offsetName#>, out projection.LeverArm<#=i#>); + //Vector3Wide.Distance(worldOffsetA<#=i#>, <#=offsetName#>, out projection.LeverArm<#=i#>); <#}}#> - TwistFriction<#=suffix#>.Prestep(ref projection.InertiaA, <#if (bodyCount == 2) {#>ref projection.InertiaB, <#}#>ref prestep.Normal, out projection.Twist); + //TwistFriction<#=suffix#>.Prestep(ref projection.InertiaA, <#if (bodyCount == 2) {#>ref projection.InertiaB, <#}#>ref prestep.Normal, out projection.Twist); + + projection = default; } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart(ref BodyVelocityWide wsvA, <#if(bodyCount == 2) {#>ref BodyVelocityWide wsvB, <#}#>ref Contact<#=contactCount#><#=suffix#>Projection projection, ref Contact<#=contactCount#>AccumulatedImpulses accumulatedImpulses) { - Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - TangentFriction<#=suffix#>.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, <#if (bodyCount == 2) {#>ref projection.InertiaB, <#}#>ref accumulatedImpulses.Tangent, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); + //Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); + //TangentFriction<#=suffix#>.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, <#if (bodyCount == 2) {#>ref projection.InertiaB, <#}#>ref accumulatedImpulses.Tangent, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); <#for (int i = 0; i < contactCount; ++i) {#> - PenetrationLimit<#=suffix#>.WarmStart(projection.Penetration<#=i#>, projection.InertiaA, <#if (bodyCount == 2) {#>projection.InertiaB, <#}#>projection.Normal, accumulatedImpulses.Penetration<#=i#>, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); + //PenetrationLimit<#=suffix#>.WarmStart(projection.Penetration<#=i#>, projection.InertiaA, <#if (bodyCount == 2) {#>projection.InertiaB, <#}#>projection.Normal, accumulatedImpulses.Penetration<#=i#>, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); <#}#> - TwistFriction<#=suffix#>.WarmStart(ref projection.Normal, ref projection.InertiaA, <#if (bodyCount == 2) {#>ref projection.InertiaB, <#}#>ref accumulatedImpulses.Twist, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); + //TwistFriction<#=suffix#>.WarmStart(ref projection.Normal, ref projection.InertiaA, <#if (bodyCount == 2) {#>ref projection.InertiaB, <#}#>ref accumulatedImpulses.Twist, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve(ref BodyVelocityWide wsvA, <#if(bodyCount == 2) {#>ref BodyVelocityWide wsvB, <#}#>ref Contact<#=contactCount#><#=suffix#>Projection projection, ref Contact<#=contactCount#>AccumulatedImpulses accumulatedImpulses) { - Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * - (<#for (int i = 0; i < contactCount; ++i) {#>accumulatedImpulses.Penetration<#=i#><#if(i < contactCount - 1){#> + <#}}#>); - TangentFriction<#=suffix#>.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, <#if (bodyCount == 2) {#>ref projection.InertiaB, <#}#>ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); + //Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); + //var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * + // (<#for (int i = 0; i < contactCount; ++i) {#>accumulatedImpulses.Penetration<#=i#><#if(i < contactCount - 1){#> + <#}}#>); + //TangentFriction<#=suffix#>.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, <#if (bodyCount == 2) {#>ref projection.InertiaB, <#}#>ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); //Note that we solve the penetration constraints after the friction constraints. //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. //It's a pretty minor effect either way. <#for (int i = 0; i < contactCount; ++i) {#> - PenetrationLimit<#=suffix#>.Solve(projection.Penetration<#=i#>, projection.InertiaA, <#if (bodyCount == 2) {#>projection.InertiaB, <#}#>projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration<#=i#>, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); + //PenetrationLimit<#=suffix#>.Solve(projection.Penetration<#=i#>, projection.InertiaA, <#if (bodyCount == 2) {#>projection.InertiaB, <#}#>projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration<#=i#>, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); <#}#> - var maximumTwistImpulse = projection.PremultipliedFrictionCoefficient * ( + //var maximumTwistImpulse = projection.PremultipliedFrictionCoefficient * ( <#for (int i = 0; i < contactCount; ++i) {#> - accumulatedImpulses.Penetration<#=i#> * projection.LeverArm<#=i#><#if (i < contactCount - 1){#> +<#} else{#>);<#}#> + // accumulatedImpulses.Penetration<#=i#> * projection.LeverArm<#=i#><#if (i < contactCount - 1){#> +<#} else{#>);<#}#> <#}#> - TwistFriction<#=suffix#>.Solve(ref projection.Normal, ref projection.InertiaA, <#if (bodyCount == 2) {#>ref projection.InertiaB, <#}#>ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); + //TwistFriction<#=suffix#>.Solve(ref projection.Normal, ref projection.InertiaA, <#if (bodyCount == 2) {#>ref projection.InertiaB, <#}#>ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityWide velocityA,<#if(bodyCount == 2) {#> in BodyVelocityWide velocityB,<#}#> ref Contact<#=contactCount#><#=suffix#>PrestepData prestep) { <#for (int i = 0; i < contactCount; ++i) {#> - PenetrationLimit<#=suffix#>.UpdatePenetrationDepth(dt, prestep.Contact<#=i#>.OffsetA, <#if(bodyCount == 2) {#>prestep.OffsetB, <#}#>prestep.Normal, velocityA, <#if (bodyCount == 2) {#>velocityB, <#}#>ref prestep.Contact<#=i#>.Depth); + //QuaternionWide.TransformWithoutOverlap(prestep.Contact<#=i#>.LocalOffsetA, orientationA, out var worldOffsetA<#=i#>); + //PenetrationLimit<#=suffix#>.UpdatePenetrationDepth(dt, worldOffsetA, <#if(bodyCount == 2) {#>prestep.OffsetB, <#}#>prestep.Normal, velocityA, <#if (bodyCount == 2) {#>velocityB, <#}#>ref prestep.Contact<#=i#>.Depth); <#}#> } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(bodyCount == 2) {#>in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>in Contact<#=contactCount#><#=suffix#>PrestepData prestep, in Contact<#=contactCount#>AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA<#if(bodyCount == 2) {#>, ref BodyVelocityWide wsvB<#}#>) { - Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); +<#if (bodyCount == 1) {#> + Helpers.BuildOrthonormalBasis(prestep.LocalNormalB, out var x, out var z); +<#} else {#> + QuaternionWide.TransformWithoutOverlap(prestep.LocalNormalB, orientationB, out var normal); + Helpers.BuildOrthonormalBasis(normal, out var x, out var z); +<#}#> +<#for (int i = 0; i < contactCount; ++i) {#> + QuaternionWide.TransformWithoutOverlap(prestep.Contact<#=i#>.LocalOffsetA, orientationA, out var worldOffsetA<#=i#>); +<#}#> <#if (contactCount > 1) {#> - FrictionHelpers.ComputeFrictionCenter(<#for (int i = 0; i < contactCount; ++i) {#>prestep.Contact<#=i#>.OffsetA, <#}#><#for (int i = 0; i < contactCount; ++i) {#>prestep.Contact<#=i#>.Depth, <#}#>out var offsetToManifoldCenterA); + FrictionHelpers.ComputeFrictionCenter(<#for (int i = 0; i < contactCount; ++i) {#>worldOffsetA<#=i#>, <#}#><#for (int i = 0; i < contactCount; ++i) {#>prestep.Contact<#=i#>.Depth, <#}#>out var offsetToManifoldCenterA); <#if (bodyCount == 2) {#> - Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); + var ab = positionB - positionA; + Vector3Wide.Subtract(offsetToManifoldCenterA, ab, out var offsetToManifoldCenterB); <#}#> <#} else if(bodyCount == 2) {#> - Vector3Wide.Subtract(prestep.Contact0.OffsetA, prestep.OffsetB, out var offsetToManifoldCenterB); + var ab = positionB - positionA; + Vector3Wide.Subtract(worldOffsetA0, ab, out var offsetToManifoldCenterB); <#}#> - TangentFriction<#=suffix#>.WarmStart2(x, z, <#if(contactCount > 1) {#>offsetToManifoldCenterA<#} else {#>prestep.Contact0.OffsetA<#}#>, <#if(bodyCount == 2) {#>offsetToManifoldCenterB, <#}#>inertiaA, <#if(bodyCount == 2) {#>inertiaB, <#}#>accumulatedImpulses.Tangent, ref wsvA<#if(bodyCount == 2) {#>, ref wsvB<#}#>); + TangentFriction<#=suffix#>.WarmStart2(x, z, <#if(contactCount > 1) {#>offsetToManifoldCenterA<#} else {#>worldOffsetA0<#}#>, <#if(bodyCount == 2) {#>offsetToManifoldCenterB, <#}#>inertiaA, <#if(bodyCount == 2) {#>inertiaB, <#}#>accumulatedImpulses.Tangent, ref wsvA<#if(bodyCount == 2) {#>, ref wsvB<#}#>); <#for (int i = 0; i < contactCount; ++i) {#> - PenetrationLimit<#=suffix#>.WarmStart2(inertiaA, <#if(bodyCount == 2) {#>inertiaB, <#}#>prestep.Normal, prestep.Contact<#=i#>.OffsetA, <#if(bodyCount == 2) {#>prestep.Contact<#=i#>.OffsetA - prestep.OffsetB, <#}#>accumulatedImpulses.Penetration<#=i#>, ref wsvA<#if(bodyCount == 2) {#>, ref wsvB<#}#>); + PenetrationLimit<#=suffix#>.WarmStart2(inertiaA, <#if(bodyCount == 2) {#>inertiaB, <#}#><#=bodyCount == 2 ? "normal" : "prestep.LocalNormalB"#>, worldOffsetA<#=i#>, <#if(bodyCount == 2) {#>worldOffsetA<#=i#> - ab, <#}#>accumulatedImpulses.Penetration<#=i#>, ref wsvA<#if(bodyCount == 2) {#>, ref wsvB<#}#>); <#}#> - TwistFriction<#=suffix#>.WarmStart2(prestep.Normal, inertiaA, <#if(bodyCount == 2) {#>inertiaB, <#}#>accumulatedImpulses.Twist, ref wsvA<#if(bodyCount == 2) {#>, ref wsvB<#}#>); + TwistFriction<#=suffix#>.WarmStart2(<#=bodyCount == 2 ? "normal" : "prestep.LocalNormalB"#>, inertiaA, <#if(bodyCount == 2) {#>inertiaB, <#}#>accumulatedImpulses.Twist, ref wsvA<#if(bodyCount == 2) {#>, ref wsvB<#}#>); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(bodyCount == 2) {#>in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>float dt, float inverseDt, in Contact<#=contactCount#><#=suffix#>PrestepData prestep, ref Contact<#=contactCount#>AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA<#if(bodyCount == 2) {#>, ref BodyVelocityWide wsvB<#}#>) { - Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); +<#if (bodyCount == 1) {#> + Helpers.BuildOrthonormalBasis(prestep.LocalNormalB, out var x, out var z); +<#} else {#> + QuaternionWide.TransformWithoutOverlap(prestep.LocalNormalB, orientationB, out var normal); + Helpers.BuildOrthonormalBasis(normal, out var x, out var z); +<#}#> +<#for (int i = 0; i < contactCount; ++i) {#> + QuaternionWide.TransformWithoutOverlap(prestep.Contact<#=i#>.LocalOffsetA, orientationA, out var worldOffsetA<#=i#>); +<#}#> <#if (contactCount > 1) {#> var premultipliedFrictionCoefficient = new Vector(1f / <#=contactCount#>f) * prestep.MaterialProperties.FrictionCoefficient; <#}#> var maximumTangentImpulse = <#=contactCount > 1 ? "premultipliedFrictionCoefficient" : "prestep.MaterialProperties.FrictionCoefficient"#> * (<#for (int i = 0; i < contactCount; ++i) {#>accumulatedImpulses.Penetration<#=i#><#if(i < contactCount - 1){#> + <#}}#>); <#if (contactCount > 1) {#> - FrictionHelpers.ComputeFrictionCenter(<#for (int i = 0; i < contactCount; ++i) {#>prestep.Contact<#=i#>.OffsetA, <#}#><#for (int i = 0; i < contactCount; ++i) {#>prestep.Contact<#=i#>.Depth, <#}#>out var offsetToManifoldCenterA); + FrictionHelpers.ComputeFrictionCenter(<#for (int i = 0; i < contactCount; ++i) {#>worldOffsetA<#=i#>, <#}#><#for (int i = 0; i < contactCount; ++i) {#>prestep.Contact<#=i#>.Depth, <#}#>out var offsetToManifoldCenterA); <#if (bodyCount == 2) {#> - Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); + var ab = positionB - positionA; + Vector3Wide.Subtract(offsetToManifoldCenterA, ab, out var offsetToManifoldCenterB); <#}#> <#} else if(bodyCount == 2) {#> - Vector3Wide.Subtract(prestep.Contact0.OffsetA, prestep.OffsetB, out var offsetToManifoldCenterB); + var ab = positionB - positionA; + Vector3Wide.Subtract(worldOffsetA0, ab, out var offsetToManifoldCenterB); <#}#> - TangentFriction<#=suffix#>.Solve2(x, z, <#=contactCount == 1 ? "prestep.Contact0.OffsetA" : "offsetToManifoldCenterA"#><#=bodyCount == 2 ? ", offsetToManifoldCenterB" : ""#>, inertiaA<#=bodyCount == 2 ? ", inertiaB" : ""#>, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); + TangentFriction<#=suffix#>.Solve2(x, z, <#=contactCount == 1 ? "worldOffsetA0" : "offsetToManifoldCenterA"#><#=bodyCount == 2 ? ", offsetToManifoldCenterB" : ""#>, inertiaA<#=bodyCount == 2 ? ", inertiaB" : ""#>, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); //Note that we solve the penetration constraints after the friction constraints. //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. //It's a pretty minor effect either way. SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); var inverseDtWide = new Vector(inverseDt); <#for (int i = 0; i < contactCount; ++i) {#> - PenetrationLimit<#=suffix#>.Solve2(inertiaA<#=bodyCount == 2 ? ", inertiaB" : ""#>, prestep.Normal, prestep.Contact<#=i#>.OffsetA, <#if(bodyCount == 2) {#>prestep.Contact<#=i#>.OffsetA - prestep.OffsetB, <#}#>prestep.Contact<#=i#>.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration<#=i#>, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); + PenetrationLimit<#=suffix#>.Solve2(inertiaA<#=bodyCount == 2 ? ", inertiaB" : ""#>, <#=bodyCount == 2 ? "normal" : "prestep.LocalNormalB"#>, worldOffsetA0, <#if(bodyCount == 2) {#>worldOffsetA<#=i#> - ab, <#}#>prestep.Contact<#=i#>.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration<#=i#>, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); <#}#> <#if (contactCount == 1) {#> //If there's only one contact, then the contact patch as determined by contact distance would be zero. @@ -371,10 +370,10 @@ for (int i = 0; i < contactCount; ++i) <#} else {#> var maximumTwistImpulse = <#=contactCount > 1 ? "premultipliedFrictionCoefficient" : "prestep.MaterialProperties.FrictionCoefficient"#> * ( <#for (int i = 0; i < contactCount; ++i) {#> - accumulatedImpulses.Penetration<#=i#> * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact<#=i#>.OffsetA)<#=i == contactCount - 1 ? ");" : " +"#> + accumulatedImpulses.Penetration<#=i#> * Vector3Wide.Distance(offsetToManifoldCenterA, worldOffsetA<#=i#>)<#=i == contactCount - 1 ? ");" : " +"#> <#}#> <#}#> - TwistFriction<#=suffix#>.Solve2(prestep.Normal, inertiaA, <#if (bodyCount == 2) {#>inertiaB, <#}#>maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); + TwistFriction<#=suffix#>.Solve2(<#=bodyCount == 2 ? "normal" : "prestep.LocalNormalB"#>, inertiaA, <#if (bodyCount == 2) {#>inertiaB, <#}#>maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -386,10 +385,10 @@ for (int i = 0; i < contactCount; ++i) in Vector dt, in Contact<#= contactCount #>AccumulatedImpulses accumulatedImpulses, ref Contact<#= contactCount #><#=suffix#>PrestepData prestep) { <#if (bodyCount == 2) {#> - prestep.OffsetB = positionB - positionA; + //prestep.OffsetB = positionB - positionA; <#}#> <#for (int i = 0; i < contactCount; ++i) {#> - PenetrationLimit<#=suffix#>.UpdatePenetrationDepth(dt, prestep.Contact<#=i#>.OffsetA, <#if(bodyCount == 2) {#>prestep.OffsetB, <#}#>prestep.Normal, wsvA, <#if (bodyCount == 2) {#>wsvB, <#}#>ref prestep.Contact<#=i#>.Depth); + //PenetrationLimit<#=suffix#>.UpdatePenetrationDepth(dt, prestep.Contact<#=i#>.OffsetA, <#if(bodyCount == 2) {#>prestep.OffsetB, <#}#>prestep.Normal, wsvA, <#if (bodyCount == 2) {#>wsvB, <#}#>ref prestep.Contact<#=i#>.Depth); <#}#> } } diff --git a/BepuPhysics/Constraints/Contact/IContactConstraintDescription.cs b/BepuPhysics/Constraints/Contact/IContactConstraintDescription.cs index b1a90df04..d3b5e5eeb 100644 --- a/BepuPhysics/Constraints/Contact/IContactConstraintDescription.cs +++ b/BepuPhysics/Constraints/Contact/IContactConstraintDescription.cs @@ -8,19 +8,20 @@ namespace BepuPhysics.Constraints.Contact { public struct ConstraintContactData { - public Vector3 OffsetA; + public Vector3 LocalOffsetA; public float PenetrationDepth; + public float PlaneOffsetB; } public interface IConvexOneBodyContactConstraintDescription : IOneBodyConstraintDescription where TDescription : unmanaged, IConvexOneBodyContactConstraintDescription { - void CopyManifoldWideProperties(ref Vector3 normal, ref PairMaterialProperties material); + void CopyManifoldWideProperties(ref Vector3 localNormalB, ref PairMaterialProperties material); ref ConstraintContactData GetFirstContact(ref TDescription description); } public interface IConvexTwoBodyContactConstraintDescription : ITwoBodyConstraintDescription where TDescription : unmanaged, IConvexTwoBodyContactConstraintDescription { - void CopyManifoldWideProperties(ref Vector3 offsetB, ref Vector3 normal, ref PairMaterialProperties material); + void CopyManifoldWideProperties(ref Vector3 localNormalB, ref PairMaterialProperties material); ref ConstraintContactData GetFirstContact(ref TDescription description); } diff --git a/Demos/Demo.cs b/Demos/Demo.cs index 88b3e17f6..37ebe5190 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -43,7 +43,7 @@ protected Demo() //It may be worth using something like hwloc to extract extra information to reason about. //var targetThreadCount = Math.Max(1, Environment.ProcessorCount > 4 ? Environment.ProcessorCount - 2 : Environment.ProcessorCount - 1); - var targetThreadCount = 3; + var targetThreadCount = 30; ThreadDispatcher = new SimpleThreadDispatcher(targetThreadCount); } @@ -62,7 +62,7 @@ public virtual void Update(Window window, Camera camera, Input input, float dt) //fully decouple simulation and rendering rates across different threads. //(In either case, you'd also want to interpolate or extrapolate simulation results during rendering for smoothness.) //Note that taking steps of variable length can reduce stability. Gradual or one-off changes can work reasonably well. - Simulation.Timestep(1 / 60f); + Simulation.Timestep(1 / 60f, ThreadDispatcher); ////Here's an example of how it would look to use more frequent updates, but still with a fixed amount of time simulated per update call: //const float timeToSimulate = 1 / 60f; From e7c7eb6d1bf8903b0c1781ed5d9c936307e026ad Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 5 Aug 2021 18:50:15 -0500 Subject: [PATCH 145/947] This path feels like a mistake. Going to cache changes thus far, revert, introduce incremental updates old style. They at least worked acceptably, most of the time. --- BepuPhysics/BodyReference.cs | 26 +++++ .../ContactConstraintAccessor.cs | 4 +- .../CollisionDetection/ContactManifold.cs | 95 +++++++++---------- .../NarrowPhaseCCDContinuations.cs | 32 ++++--- .../NarrowPhaseConstraintUpdate.cs | 2 +- 5 files changed, 95 insertions(+), 64 deletions(-) diff --git a/BepuPhysics/BodyReference.cs b/BepuPhysics/BodyReference.cs index 6c311c79e..5bc27a918 100644 --- a/BepuPhysics/BodyReference.cs +++ b/BepuPhysics/BodyReference.cs @@ -115,6 +115,32 @@ public ref RigidPose Pose } } + /// + /// Gets a reference to the body's motion state, including both pose and velocity. + /// + public ref MotionState MotionState + { + [MethodImpl(MethodImplOptions.NoInlining)] + get + { + ref var location = ref MemoryLocation; + return ref Bodies.Sets[location.SetIndex].SolverStates[location.Index].Motion; + } + } + + /// + /// Gets a reference to the body's solver-relevant state, including both pose, velocity, and inertia. + /// + public ref SolverState SolverState + { + [MethodImpl(MethodImplOptions.NoInlining)] + get + { + ref var location = ref MemoryLocation; + return ref Bodies.Sets[location.SetIndex].SolverStates[location.Index]; + } + } + /// /// Gets a reference to the body's collidable. /// diff --git a/BepuPhysics/CollisionDetection/ContactConstraintAccessor.cs b/BepuPhysics/CollisionDetection/ContactConstraintAccessor.cs index 3b4a45e23..47ea8ca59 100644 --- a/BepuPhysics/CollisionDetection/ContactConstraintAccessor.cs +++ b/BepuPhysics/CollisionDetection/ContactConstraintAccessor.cs @@ -349,7 +349,7 @@ public override void UpdateConstraintForManifold(ref manifoldPointer); CopyContactData(ref manifold, out var constraintCache, out var description); - description.CopyManifoldWideProperties(ref manifold.OffsetB, ref manifold.Normal, ref material); + description.CopyManifoldWideProperties(ref manifold.Normal, ref material); UpdateConstraint(narrowPhase, manifoldTypeAsConstraintType, workerIndex, ref pair, ref constraintCache, ref collisionCache, ref description, bodyHandles); } else @@ -360,7 +360,7 @@ public override void UpdateConstraintForManifold - /// Offset from the position of collidable A to the contact position. + /// Offset from the position of collidable A to the contact position in collidable A's local space.. /// [FieldOffset(0)] - public Vector3 Offset; + public Vector3 LocalOffsetA; /// /// Penetration depth between the two collidables at this contact. Negative values represent separation. /// [FieldOffset(12)] public float Depth; /// - /// Surface basis of the contact. If transformed into a rotation matrix, X and Z represent tangent directions and Y represents the contact normal. Points from collidable B to collidable A. + /// Normal of the surface at the contact point in collidable B's local space, or world space if B is a static. /// [FieldOffset(16)] - public Vector3 Normal; + public Vector3 LocalNormalB; /// /// Id of the features involved in the collision that generated this contact. If a contact has the same feature id as in a previous frame, it is an indication that the /// same parts of the shape contributed to its creation. This is useful for carrying information from frame to frame. @@ -42,10 +42,10 @@ public struct NonconvexContact public struct ConvexContact { /// - /// Offset from the position of collidable A to the contact position. + /// Offset from the position of collidable A to the contact position in collidable A's local space.. /// [FieldOffset(0)] - public Vector3 Offset; + public Vector3 LocalOffsetA; /// /// Penetration depth between the two collidables at this contact. Negative values represent separation. /// @@ -82,12 +82,12 @@ public interface IContactManifold where TManifold : struct, IContactM /// Retrieves a copy of a contact's data. /// /// Index of the contact to copy data from. - /// Offset from the first collidable's position to the contact position. - /// Normal of the contact surface at the requested contact. Points from collidable B to collidable A. + /// Offset from collidable A's position to the contact position in collidable A's local space. + /// Normal of the contact surface at the requested contact in collidable B's local space, or world space if B is a static. Points from collidable B to collidable A. /// Penetration depth at the requested contact. /// Feature id of the requested contact. /// Feature ids represent which parts of the collidables formed the contact and can be used to track unique contacts across frames. - void GetContact(int contactIndex, out Vector3 offset, out Vector3 normal, out float depth, out int featureId); + void GetContact(int contactIndex, out Vector3 localOffsetA, out Vector3 localNormalB, out float depth, out int featureId); //Can't return refs to the this instance, but it's convenient to have ref returns for parameters and interfaces can't require static functions, so... /// @@ -99,20 +99,20 @@ public interface IContactManifold where TManifold : struct, IContactM ref float GetDepth(ref TManifold manifold, int contactIndex); /// - /// Pulls a reference to a contact's normal. Points from collidable B to collidable A. For convex manifolds that share a normal, all contact indices will simply return a reference to the manifold-wide normal. + /// Pulls a reference to a contact's normal in collidable B's local space, or world space if B is a static. Points from collidable B to collidable A. For convex manifolds that share a normal, all contact indices will simply return a reference to the manifold-wide normal. /// /// Manifold to pull a reference from. /// Contact to pull data from. /// Reference to a contact's normal (or the manifold-wide normal in a convex manifold). - ref Vector3 GetNormal(ref TManifold manifold, int contactIndex); + ref Vector3 GetLocalNormalB(ref TManifold manifold, int contactIndex); /// - /// Pulls a reference to a contact's offset. + /// Pulls a reference to a contact's offset in collidable A's local space. /// /// Manifold to pull a reference from. /// Contact to pull data from. /// Reference to a contact's offset. - ref Vector3 GetOffset(ref TManifold manifold, int contactIndex); + ref Vector3 GetLocalOffsetA(ref TManifold manifold, int contactIndex); /// /// Pulls a reference to a contact's feature id. @@ -168,18 +168,18 @@ private readonly void ValidateIndex(int contactIndex) /// Retrieves a copy of a contact's data. /// /// Index of the contact to copy data from. - /// Offset from the first collidable's position to the contact position. - /// Normal of the contact surface at the requested contact. Points from collidable B to collidable A. + /// Offset from collidable A's position to the contact position in collidable A's local space.. + /// Normal of the contact surface at the requested contact in collidable B's local space, or world space if B is a static. Points from collidable B to collidable A. /// Penetration depth at the requested contact. /// Feature id of the requested contact. /// Feature ids represent which parts of the collidables formed the contact and can be used to track unique contacts across frames. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void GetContact(int contactIndex, out Vector3 offset, out Vector3 normal, out float depth, out int featureId) + public void GetContact(int contactIndex, out Vector3 localOffsetA, out Vector3 localNormalB, out float depth, out int featureId) { ValidateIndex(contactIndex); ref var contact = ref Unsafe.Add(ref Contact0, contactIndex); - offset = contact.Offset; - normal = contact.Normal; + localOffsetA = contact.LocalOffsetA; + localNormalB = contact.LocalNormalB; depth = contact.Depth; featureId = contact.FeatureId; } @@ -207,13 +207,13 @@ public static void FastRemoveAt(NonconvexContactManifold* manifold, int index) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Add(NonconvexContactManifold* manifold, ref Vector3 normal, ref ConvexContact convexContact) + public static void Add(NonconvexContactManifold* manifold, ref Vector3 localNormalB, ref ConvexContact convexContact) { Debug.Assert(manifold->Count < MaximumContactCount); ref var targetContact = ref (&manifold->Contact0)[manifold->Count++]; targetContact.Depth = convexContact.Depth; - targetContact.Offset = convexContact.Offset; - targetContact.Normal = normal; + targetContact.LocalOffsetA = convexContact.LocalOffsetA; + targetContact.LocalNormalB = localNormalB; targetContact.FeatureId = convexContact.FeatureId; } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -236,28 +236,28 @@ public ref float GetDepth(ref NonconvexContactManifold manifold, int contactInde } /// - /// Pulls a reference to a contact's normal. Points from collidable B to collidable A. + /// Pulls a reference to a contact's normal in collidable B's local space, or world space if B is a static. Points from collidable B to collidable A. /// /// Manifold to pull a reference from. /// Contact to pull data from. /// Reference to a contact's normal. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3 GetNormal(ref NonconvexContactManifold manifold, int contactIndex) + public ref Vector3 GetLocalNormalB(ref NonconvexContactManifold manifold, int contactIndex) { - return ref Unsafe.Add(ref manifold.Contact0, contactIndex).Normal; + return ref Unsafe.Add(ref manifold.Contact0, contactIndex).LocalNormalB; } /// - /// Pulls a reference to a contact's offset. + /// Pulls a reference to a contact's offset in collidable A's local space. /// /// Manifold to pull a reference from. /// Contact to pull data from. /// Reference to a contact's offset. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3 GetOffset(ref NonconvexContactManifold manifold, int contactIndex) + public ref Vector3 GetLocalOffsetA(ref NonconvexContactManifold manifold, int contactIndex) { - return ref Unsafe.Add(ref manifold.Contact0, contactIndex).Offset; + return ref Unsafe.Add(ref manifold.Contact0, contactIndex).LocalOffsetA; } /// @@ -276,30 +276,25 @@ public ref int GetFeatureId(ref NonconvexContactManifold manifold, int contactIn /// /// Contains the data associated with a convex contact manifold. /// - [StructLayout(LayoutKind.Explicit, Size = 108)] + [StructLayout(LayoutKind.Explicit, Size = 96)] public unsafe struct ConvexContactManifold : IContactManifold { /// - /// Offset from collidable A to collidable B. + /// Surface normal shared by all contacts, stored in collidable B's local space if it is a body, or world space if it is a static. Points from collidable B to collidable A. /// [FieldOffset(0)] - public Vector3 OffsetB; + public Vector3 LocalNormalB; + [FieldOffset(12)] public int Count; - /// - /// Surface normal shared by all contacts. Points from collidable B to collidable A. - /// [FieldOffset(16)] - public Vector3 Normal; - - [FieldOffset(28)] public ConvexContact Contact0; - [FieldOffset(48)] + [FieldOffset(36)] public ConvexContact Contact1; - [FieldOffset(68)] + [FieldOffset(56)] public ConvexContact Contact2; - [FieldOffset(88)] + [FieldOffset(76)] public ConvexContact Contact3; readonly int IContactManifold.Count => Count; @@ -328,18 +323,18 @@ public int GetFeatureId(int contactIndex) /// Retrieves a copy of a contact's data. /// /// Index of the contact to copy data from. - /// Offset from the first collidable's position to the contact position. - /// Normal of the contact surface at the requested contact. Points from collidable B to collidable A. + /// Offset from collidable A's position to the contact position in collidable A's local space. + /// Normal of the contact surface at the requested contact in collidable B's local space, or world space if B is a static. Points from collidable B to collidable A. /// Penetration depth at the requested contact. /// Feature id of the requested contact. /// Feature ids represent which parts of the collidables formed the contact and can be used to track unique contacts across frames. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void GetContact(int contactIndex, out Vector3 offset, out Vector3 normal, out float depth, out int featureId) + public void GetContact(int contactIndex, out Vector3 localOffsetA, out Vector3 localNormalB, out float depth, out int featureId) { ValidateIndex(contactIndex); ref var contact = ref Unsafe.Add(ref Contact0, contactIndex); - offset = contact.Offset; - normal = Normal; + localOffsetA = contact.LocalOffsetA; + localNormalB = LocalNormalB; depth = contact.Depth; featureId = contact.FeatureId; } @@ -367,28 +362,28 @@ public ref float GetDepth(ref ConvexContactManifold manifold, int contactIndex) } /// - /// Pulls a reference to a contact manifold's normal. Points from collidable B to collidable A. Convex manifolds share a single normal across all contacts. + /// Pulls a reference to a contact manifold's normal in collidable B's local space, or world space if B is a static. Points from collidable B to collidable A. Convex manifolds share a single normal across all contacts. /// /// Manifold to pull a reference from. /// Contact to pull data from. /// Reference to the contact manifold's normal. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3 GetNormal(ref ConvexContactManifold manifold, int contactIndex) + public ref Vector3 GetLocalNormalB(ref ConvexContactManifold manifold, int contactIndex) { - return ref manifold.Normal; + return ref manifold.LocalNormalB; } /// - /// Pulls a reference to a contact's offset. + /// Pulls a reference to a contact's offset in collidable A's local space. /// /// Manifold to pull a reference from. /// Contact to pull data from. /// Reference to a contact's offset. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3 GetOffset(ref ConvexContactManifold manifold, int contactIndex) + public ref Vector3 GetLocalOffsetA(ref ConvexContactManifold manifold, int contactIndex) { - return ref Unsafe.Add(ref manifold.Contact0, contactIndex).Offset; + return ref Unsafe.Add(ref manifold.Contact0, contactIndex).LocalOffsetA; } /// diff --git a/BepuPhysics/CollisionDetection/NarrowPhaseCCDContinuations.cs b/BepuPhysics/CollisionDetection/NarrowPhaseCCDContinuations.cs index 54663ebdc..79cb13fb0 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhaseCCDContinuations.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhaseCCDContinuations.cs @@ -48,18 +48,12 @@ public void Initialize(ref CollidablePair pair) struct ContinuousPair { public CollidablePair Pair; - public Vector3 RelativeLinearVelocity; - public Vector3 AngularA; - public Vector3 AngularB; public float T; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Initialize(ref CollidablePair pair, in Vector3 relativeLinearVelocity, in Vector3 angularVelocityA, in Vector3 angularVelocityB, float t) + public void Initialize(ref CollidablePair pair, float t) { Pair = pair; - AngularA = angularVelocityA; - AngularB = angularVelocityB; - RelativeLinearVelocity = relativeLinearVelocity; T = t; } } @@ -122,7 +116,7 @@ public CCDContinuationIndex AddDiscrete(ref CollidablePair pair) [MethodImpl(MethodImplOptions.AggressiveInlining)] public CCDContinuationIndex AddContinuous(ref CollidablePair pair, in Vector3 relativeLinearVelocity, in Vector3 angularVelocityA, in Vector3 angularVelocityB, float t) { - continuous.Allocate(pool, out var index).Initialize(ref pair, relativeLinearVelocity, angularVelocityA, angularVelocityB, t); + continuous.Allocate(pool, out var index).Initialize(ref pair, t); return new CCDContinuationIndex((int)ConstraintGeneratorType.Continuous, index); } @@ -165,9 +159,25 @@ public unsafe void OnPairCompleted(int pairId, ref TManifold manifold for (int i = 0; i < manifold.Count; ++i) { ref var contact = ref Unsafe.Add(ref manifold.Contact0, i); - var angularContributionA = Vector3.Cross(continuation.AngularA, contact.Offset); - var angularContributionB = Vector3.Cross(continuation.AngularB, contact.Offset - manifold.OffsetB); - var velocityAtContact = Vector3.Dot(angularContributionB - angularContributionA + continuation.RelativeLinearVelocity, manifold.Normal); + var a = narrowPhase.Bodies.GetBodyReference(continuation.Pair.A.BodyHandle); + ref var motionA = ref a.MotionState; + QuaternionEx.TransformWithoutOverlap(contact.LocalOffsetA, motionA.Pose.Orientation, out var contactOffsetA); + var angularContributionA = Vector3.Cross(motionA.Velocity.Angular, contactOffsetA); + float velocityAtContact; + if (continuation.Pair.B.Mobility != CollidableMobility.Static) + { + var b = narrowPhase.Bodies.GetBodyReference(continuation.Pair.B.BodyHandle); + ref var motionB = ref b.MotionState; + var offsetB = motionB.Pose.Position - motionA.Pose.Position; + var angularContributionB = Vector3.Cross(motionB.Velocity.Angular, contactOffsetA - offsetB); + QuaternionEx.TransformWithoutOverlap(manifold.LocalNormalB, motionB.Pose.Orientation, out var normal); + velocityAtContact = Vector3.Dot(angularContributionB - angularContributionA + motionB.Velocity.Linear - motionA.Velocity.Linear, normal); + } + else + { + //For statics, 'local' normal B is just world space since statics do not rotate over substeps. + velocityAtContact = -Vector3.Dot(motionA.Velocity.Linear + angularContributionA, manifold.LocalNormalB); + } contact.Depth -= velocityAtContact * continuation.T; } } diff --git a/BepuPhysics/CollisionDetection/NarrowPhaseConstraintUpdate.cs b/BepuPhysics/CollisionDetection/NarrowPhaseConstraintUpdate.cs index 079fd6cf4..671c11d9c 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhaseConstraintUpdate.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhaseConstraintUpdate.cs @@ -309,7 +309,7 @@ unsafe void UpdateConstraintForManifold(int workerIndex, ref CollidablePair pair, ref TContactManifold manifold, ref TCollisionCache collisionCache) + public unsafe void UpdateConstraintsForPair(int workerIndex, ref CollidablePair pair, ref TContactManifold manifold, ref TCollisionCache collisionCache) where TCollisionCache : unmanaged, IPairCacheEntry where TContactManifold : unmanaged, IContactManifold { From 7bc509c4f00114c319b73c96dc7cb34e4e5f0018 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 5 Aug 2021 18:53:03 -0500 Subject: [PATCH 146/947] Revert! --- BepuPhysics/BodyReference.cs | 26 - .../ContactConstraintAccessor.cs | 4 +- .../CollisionDetection/ContactManifold.cs | 95 +- .../NarrowPhaseCCDContinuations.cs | 32 +- .../NarrowPhaseConstraintUpdate.cs | 2 +- .../Contact/ContactConvexCommon.cs | 7 +- .../Constraints/Contact/ContactConvexTypes.cs | 1242 ++++++++--------- .../Constraints/Contact/ContactConvexTypes.tt | 171 +-- .../Contact/IContactConstraintDescription.cs | 7 +- Demos/Demo.cs | 4 +- 10 files changed, 736 insertions(+), 854 deletions(-) diff --git a/BepuPhysics/BodyReference.cs b/BepuPhysics/BodyReference.cs index 5bc27a918..6c311c79e 100644 --- a/BepuPhysics/BodyReference.cs +++ b/BepuPhysics/BodyReference.cs @@ -115,32 +115,6 @@ public ref RigidPose Pose } } - /// - /// Gets a reference to the body's motion state, including both pose and velocity. - /// - public ref MotionState MotionState - { - [MethodImpl(MethodImplOptions.NoInlining)] - get - { - ref var location = ref MemoryLocation; - return ref Bodies.Sets[location.SetIndex].SolverStates[location.Index].Motion; - } - } - - /// - /// Gets a reference to the body's solver-relevant state, including both pose, velocity, and inertia. - /// - public ref SolverState SolverState - { - [MethodImpl(MethodImplOptions.NoInlining)] - get - { - ref var location = ref MemoryLocation; - return ref Bodies.Sets[location.SetIndex].SolverStates[location.Index]; - } - } - /// /// Gets a reference to the body's collidable. /// diff --git a/BepuPhysics/CollisionDetection/ContactConstraintAccessor.cs b/BepuPhysics/CollisionDetection/ContactConstraintAccessor.cs index 47ea8ca59..3b4a45e23 100644 --- a/BepuPhysics/CollisionDetection/ContactConstraintAccessor.cs +++ b/BepuPhysics/CollisionDetection/ContactConstraintAccessor.cs @@ -349,7 +349,7 @@ public override void UpdateConstraintForManifold(ref manifoldPointer); CopyContactData(ref manifold, out var constraintCache, out var description); - description.CopyManifoldWideProperties(ref manifold.Normal, ref material); + description.CopyManifoldWideProperties(ref manifold.OffsetB, ref manifold.Normal, ref material); UpdateConstraint(narrowPhase, manifoldTypeAsConstraintType, workerIndex, ref pair, ref constraintCache, ref collisionCache, ref description, bodyHandles); } else @@ -360,7 +360,7 @@ public override void UpdateConstraintForManifold - /// Offset from the position of collidable A to the contact position in collidable A's local space.. + /// Offset from the position of collidable A to the contact position. /// [FieldOffset(0)] - public Vector3 LocalOffsetA; + public Vector3 Offset; /// /// Penetration depth between the two collidables at this contact. Negative values represent separation. /// [FieldOffset(12)] public float Depth; /// - /// Normal of the surface at the contact point in collidable B's local space, or world space if B is a static. + /// Surface basis of the contact. If transformed into a rotation matrix, X and Z represent tangent directions and Y represents the contact normal. Points from collidable B to collidable A. /// [FieldOffset(16)] - public Vector3 LocalNormalB; + public Vector3 Normal; /// /// Id of the features involved in the collision that generated this contact. If a contact has the same feature id as in a previous frame, it is an indication that the /// same parts of the shape contributed to its creation. This is useful for carrying information from frame to frame. @@ -42,10 +42,10 @@ public struct NonconvexContact public struct ConvexContact { /// - /// Offset from the position of collidable A to the contact position in collidable A's local space.. + /// Offset from the position of collidable A to the contact position. /// [FieldOffset(0)] - public Vector3 LocalOffsetA; + public Vector3 Offset; /// /// Penetration depth between the two collidables at this contact. Negative values represent separation. /// @@ -82,12 +82,12 @@ public interface IContactManifold where TManifold : struct, IContactM /// Retrieves a copy of a contact's data. /// /// Index of the contact to copy data from. - /// Offset from collidable A's position to the contact position in collidable A's local space. - /// Normal of the contact surface at the requested contact in collidable B's local space, or world space if B is a static. Points from collidable B to collidable A. + /// Offset from the first collidable's position to the contact position. + /// Normal of the contact surface at the requested contact. Points from collidable B to collidable A. /// Penetration depth at the requested contact. /// Feature id of the requested contact. /// Feature ids represent which parts of the collidables formed the contact and can be used to track unique contacts across frames. - void GetContact(int contactIndex, out Vector3 localOffsetA, out Vector3 localNormalB, out float depth, out int featureId); + void GetContact(int contactIndex, out Vector3 offset, out Vector3 normal, out float depth, out int featureId); //Can't return refs to the this instance, but it's convenient to have ref returns for parameters and interfaces can't require static functions, so... /// @@ -99,20 +99,20 @@ public interface IContactManifold where TManifold : struct, IContactM ref float GetDepth(ref TManifold manifold, int contactIndex); /// - /// Pulls a reference to a contact's normal in collidable B's local space, or world space if B is a static. Points from collidable B to collidable A. For convex manifolds that share a normal, all contact indices will simply return a reference to the manifold-wide normal. + /// Pulls a reference to a contact's normal. Points from collidable B to collidable A. For convex manifolds that share a normal, all contact indices will simply return a reference to the manifold-wide normal. /// /// Manifold to pull a reference from. /// Contact to pull data from. /// Reference to a contact's normal (or the manifold-wide normal in a convex manifold). - ref Vector3 GetLocalNormalB(ref TManifold manifold, int contactIndex); + ref Vector3 GetNormal(ref TManifold manifold, int contactIndex); /// - /// Pulls a reference to a contact's offset in collidable A's local space. + /// Pulls a reference to a contact's offset. /// /// Manifold to pull a reference from. /// Contact to pull data from. /// Reference to a contact's offset. - ref Vector3 GetLocalOffsetA(ref TManifold manifold, int contactIndex); + ref Vector3 GetOffset(ref TManifold manifold, int contactIndex); /// /// Pulls a reference to a contact's feature id. @@ -168,18 +168,18 @@ private readonly void ValidateIndex(int contactIndex) /// Retrieves a copy of a contact's data. /// /// Index of the contact to copy data from. - /// Offset from collidable A's position to the contact position in collidable A's local space.. - /// Normal of the contact surface at the requested contact in collidable B's local space, or world space if B is a static. Points from collidable B to collidable A. + /// Offset from the first collidable's position to the contact position. + /// Normal of the contact surface at the requested contact. Points from collidable B to collidable A. /// Penetration depth at the requested contact. /// Feature id of the requested contact. /// Feature ids represent which parts of the collidables formed the contact and can be used to track unique contacts across frames. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void GetContact(int contactIndex, out Vector3 localOffsetA, out Vector3 localNormalB, out float depth, out int featureId) + public void GetContact(int contactIndex, out Vector3 offset, out Vector3 normal, out float depth, out int featureId) { ValidateIndex(contactIndex); ref var contact = ref Unsafe.Add(ref Contact0, contactIndex); - localOffsetA = contact.LocalOffsetA; - localNormalB = contact.LocalNormalB; + offset = contact.Offset; + normal = contact.Normal; depth = contact.Depth; featureId = contact.FeatureId; } @@ -207,13 +207,13 @@ public static void FastRemoveAt(NonconvexContactManifold* manifold, int index) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Add(NonconvexContactManifold* manifold, ref Vector3 localNormalB, ref ConvexContact convexContact) + public static void Add(NonconvexContactManifold* manifold, ref Vector3 normal, ref ConvexContact convexContact) { Debug.Assert(manifold->Count < MaximumContactCount); ref var targetContact = ref (&manifold->Contact0)[manifold->Count++]; targetContact.Depth = convexContact.Depth; - targetContact.LocalOffsetA = convexContact.LocalOffsetA; - targetContact.LocalNormalB = localNormalB; + targetContact.Offset = convexContact.Offset; + targetContact.Normal = normal; targetContact.FeatureId = convexContact.FeatureId; } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -236,28 +236,28 @@ public ref float GetDepth(ref NonconvexContactManifold manifold, int contactInde } /// - /// Pulls a reference to a contact's normal in collidable B's local space, or world space if B is a static. Points from collidable B to collidable A. + /// Pulls a reference to a contact's normal. Points from collidable B to collidable A. /// /// Manifold to pull a reference from. /// Contact to pull data from. /// Reference to a contact's normal. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3 GetLocalNormalB(ref NonconvexContactManifold manifold, int contactIndex) + public ref Vector3 GetNormal(ref NonconvexContactManifold manifold, int contactIndex) { - return ref Unsafe.Add(ref manifold.Contact0, contactIndex).LocalNormalB; + return ref Unsafe.Add(ref manifold.Contact0, contactIndex).Normal; } /// - /// Pulls a reference to a contact's offset in collidable A's local space. + /// Pulls a reference to a contact's offset. /// /// Manifold to pull a reference from. /// Contact to pull data from. /// Reference to a contact's offset. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3 GetLocalOffsetA(ref NonconvexContactManifold manifold, int contactIndex) + public ref Vector3 GetOffset(ref NonconvexContactManifold manifold, int contactIndex) { - return ref Unsafe.Add(ref manifold.Contact0, contactIndex).LocalOffsetA; + return ref Unsafe.Add(ref manifold.Contact0, contactIndex).Offset; } /// @@ -276,25 +276,30 @@ public ref int GetFeatureId(ref NonconvexContactManifold manifold, int contactIn /// /// Contains the data associated with a convex contact manifold. /// - [StructLayout(LayoutKind.Explicit, Size = 96)] + [StructLayout(LayoutKind.Explicit, Size = 108)] public unsafe struct ConvexContactManifold : IContactManifold { /// - /// Surface normal shared by all contacts, stored in collidable B's local space if it is a body, or world space if it is a static. Points from collidable B to collidable A. + /// Offset from collidable A to collidable B. /// [FieldOffset(0)] - public Vector3 LocalNormalB; - + public Vector3 OffsetB; [FieldOffset(12)] public int Count; + /// + /// Surface normal shared by all contacts. Points from collidable B to collidable A. + /// [FieldOffset(16)] + public Vector3 Normal; + + [FieldOffset(28)] public ConvexContact Contact0; - [FieldOffset(36)] + [FieldOffset(48)] public ConvexContact Contact1; - [FieldOffset(56)] + [FieldOffset(68)] public ConvexContact Contact2; - [FieldOffset(76)] + [FieldOffset(88)] public ConvexContact Contact3; readonly int IContactManifold.Count => Count; @@ -323,18 +328,18 @@ public int GetFeatureId(int contactIndex) /// Retrieves a copy of a contact's data. /// /// Index of the contact to copy data from. - /// Offset from collidable A's position to the contact position in collidable A's local space. - /// Normal of the contact surface at the requested contact in collidable B's local space, or world space if B is a static. Points from collidable B to collidable A. + /// Offset from the first collidable's position to the contact position. + /// Normal of the contact surface at the requested contact. Points from collidable B to collidable A. /// Penetration depth at the requested contact. /// Feature id of the requested contact. /// Feature ids represent which parts of the collidables formed the contact and can be used to track unique contacts across frames. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void GetContact(int contactIndex, out Vector3 localOffsetA, out Vector3 localNormalB, out float depth, out int featureId) + public void GetContact(int contactIndex, out Vector3 offset, out Vector3 normal, out float depth, out int featureId) { ValidateIndex(contactIndex); ref var contact = ref Unsafe.Add(ref Contact0, contactIndex); - localOffsetA = contact.LocalOffsetA; - localNormalB = LocalNormalB; + offset = contact.Offset; + normal = Normal; depth = contact.Depth; featureId = contact.FeatureId; } @@ -362,28 +367,28 @@ public ref float GetDepth(ref ConvexContactManifold manifold, int contactIndex) } /// - /// Pulls a reference to a contact manifold's normal in collidable B's local space, or world space if B is a static. Points from collidable B to collidable A. Convex manifolds share a single normal across all contacts. + /// Pulls a reference to a contact manifold's normal. Points from collidable B to collidable A. Convex manifolds share a single normal across all contacts. /// /// Manifold to pull a reference from. /// Contact to pull data from. /// Reference to the contact manifold's normal. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3 GetLocalNormalB(ref ConvexContactManifold manifold, int contactIndex) + public ref Vector3 GetNormal(ref ConvexContactManifold manifold, int contactIndex) { - return ref manifold.LocalNormalB; + return ref manifold.Normal; } /// - /// Pulls a reference to a contact's offset in collidable A's local space. + /// Pulls a reference to a contact's offset. /// /// Manifold to pull a reference from. /// Contact to pull data from. /// Reference to a contact's offset. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3 GetLocalOffsetA(ref ConvexContactManifold manifold, int contactIndex) + public ref Vector3 GetOffset(ref ConvexContactManifold manifold, int contactIndex) { - return ref Unsafe.Add(ref manifold.Contact0, contactIndex).LocalOffsetA; + return ref Unsafe.Add(ref manifold.Contact0, contactIndex).Offset; } /// diff --git a/BepuPhysics/CollisionDetection/NarrowPhaseCCDContinuations.cs b/BepuPhysics/CollisionDetection/NarrowPhaseCCDContinuations.cs index 79cb13fb0..54663ebdc 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhaseCCDContinuations.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhaseCCDContinuations.cs @@ -48,12 +48,18 @@ public void Initialize(ref CollidablePair pair) struct ContinuousPair { public CollidablePair Pair; + public Vector3 RelativeLinearVelocity; + public Vector3 AngularA; + public Vector3 AngularB; public float T; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Initialize(ref CollidablePair pair, float t) + public void Initialize(ref CollidablePair pair, in Vector3 relativeLinearVelocity, in Vector3 angularVelocityA, in Vector3 angularVelocityB, float t) { Pair = pair; + AngularA = angularVelocityA; + AngularB = angularVelocityB; + RelativeLinearVelocity = relativeLinearVelocity; T = t; } } @@ -116,7 +122,7 @@ public CCDContinuationIndex AddDiscrete(ref CollidablePair pair) [MethodImpl(MethodImplOptions.AggressiveInlining)] public CCDContinuationIndex AddContinuous(ref CollidablePair pair, in Vector3 relativeLinearVelocity, in Vector3 angularVelocityA, in Vector3 angularVelocityB, float t) { - continuous.Allocate(pool, out var index).Initialize(ref pair, t); + continuous.Allocate(pool, out var index).Initialize(ref pair, relativeLinearVelocity, angularVelocityA, angularVelocityB, t); return new CCDContinuationIndex((int)ConstraintGeneratorType.Continuous, index); } @@ -159,25 +165,9 @@ public unsafe void OnPairCompleted(int pairId, ref TManifold manifold for (int i = 0; i < manifold.Count; ++i) { ref var contact = ref Unsafe.Add(ref manifold.Contact0, i); - var a = narrowPhase.Bodies.GetBodyReference(continuation.Pair.A.BodyHandle); - ref var motionA = ref a.MotionState; - QuaternionEx.TransformWithoutOverlap(contact.LocalOffsetA, motionA.Pose.Orientation, out var contactOffsetA); - var angularContributionA = Vector3.Cross(motionA.Velocity.Angular, contactOffsetA); - float velocityAtContact; - if (continuation.Pair.B.Mobility != CollidableMobility.Static) - { - var b = narrowPhase.Bodies.GetBodyReference(continuation.Pair.B.BodyHandle); - ref var motionB = ref b.MotionState; - var offsetB = motionB.Pose.Position - motionA.Pose.Position; - var angularContributionB = Vector3.Cross(motionB.Velocity.Angular, contactOffsetA - offsetB); - QuaternionEx.TransformWithoutOverlap(manifold.LocalNormalB, motionB.Pose.Orientation, out var normal); - velocityAtContact = Vector3.Dot(angularContributionB - angularContributionA + motionB.Velocity.Linear - motionA.Velocity.Linear, normal); - } - else - { - //For statics, 'local' normal B is just world space since statics do not rotate over substeps. - velocityAtContact = -Vector3.Dot(motionA.Velocity.Linear + angularContributionA, manifold.LocalNormalB); - } + var angularContributionA = Vector3.Cross(continuation.AngularA, contact.Offset); + var angularContributionB = Vector3.Cross(continuation.AngularB, contact.Offset - manifold.OffsetB); + var velocityAtContact = Vector3.Dot(angularContributionB - angularContributionA + continuation.RelativeLinearVelocity, manifold.Normal); contact.Depth -= velocityAtContact * continuation.T; } } diff --git a/BepuPhysics/CollisionDetection/NarrowPhaseConstraintUpdate.cs b/BepuPhysics/CollisionDetection/NarrowPhaseConstraintUpdate.cs index 671c11d9c..079fd6cf4 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhaseConstraintUpdate.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhaseConstraintUpdate.cs @@ -309,7 +309,7 @@ unsafe void UpdateConstraintForManifold(int workerIndex, ref CollidablePair pair, ref TContactManifold manifold, ref TCollisionCache collisionCache) + public unsafe void UpdateConstraintsForPair(int workerIndex, ref CollidablePair pair, ref TContactManifold manifold, ref TCollisionCache collisionCache) where TCollisionCache : unmanaged, IPairCacheEntry where TContactManifold : unmanaged, IContactManifold { diff --git a/BepuPhysics/Constraints/Contact/ContactConvexCommon.cs b/BepuPhysics/Constraints/Contact/ContactConvexCommon.cs index 3706f526a..bb6776a28 100644 --- a/BepuPhysics/Constraints/Contact/ContactConvexCommon.cs +++ b/BepuPhysics/Constraints/Contact/ContactConvexCommon.cs @@ -9,9 +9,8 @@ namespace BepuPhysics.Constraints.Contact { public struct ConvexContactWide { - public Vector3Wide LocalOffsetA; + public Vector3Wide OffsetA; public Vector Depth; - public Vector PlaneOffsetB; } public struct MaterialPropertiesWide @@ -31,14 +30,14 @@ public interface IContactPrestep where TPrestep : struct, IContactPres public interface IConvexContactPrestep : IContactPrestep where TPrestep : struct, IConvexContactPrestep { - ref Vector3Wide GetLocalNormalB(ref TPrestep prestep); + ref Vector3Wide GetNormal(ref TPrestep prestep); ref ConvexContactWide GetContact(ref TPrestep prestep, int index); } public interface ITwoBodyConvexContactPrestep : IConvexContactPrestep where TPrestep : struct, ITwoBodyConvexContactPrestep { - //TODO: Purge this! + ref Vector3Wide GetOffsetB(ref TPrestep prestep); } public interface IContactAccumulatedImpulses where TAccumulatedImpulses : struct, IContactAccumulatedImpulses diff --git a/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs b/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs index 966ceff4e..f269444fb 100644 --- a/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs +++ b/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs @@ -199,7 +199,7 @@ public static void ComputeFrictionCenter( public struct Contact1OneBody : IConvexOneBodyContactConstraintDescription { public ConstraintContactData Contact0; - public Vector3 LocalNormalB; + public Vector3 Normal; public float FrictionCoefficient; public SpringSettings SpringSettings; public float MaximumRecoveryVelocity; @@ -208,11 +208,10 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int { Debug.Assert(batch.TypeId == ConstraintTypeId, "The type batch passed to the description must match the description's expected type."); ref var target = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); - Vector3Wide.WriteFirst(Contact0.LocalOffsetA, ref target.Contact0.LocalOffsetA); + Vector3Wide.WriteFirst(Contact0.OffsetA, ref target.Contact0.OffsetA); GetFirst(ref target.Contact0.Depth) = Contact0.PenetrationDepth; - GetFirst(ref target.Contact0.PlaneOffsetB) = Contact0.PlaneOffsetB; - - Vector3Wide.WriteFirst(LocalNormalB, ref target.LocalNormalB); + + Vector3Wide.WriteFirst(Normal, ref target.Normal); GetFirst(ref target.MaterialProperties.FrictionCoefficient) = FrictionCoefficient; SpringSettingsWide.WriteFirst(SpringSettings, ref target.MaterialProperties.SpringSettings); GetFirst(ref target.MaterialProperties.MaximumRecoveryVelocity) = MaximumRecoveryVelocity; @@ -222,20 +221,19 @@ public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int { Debug.Assert(batch.TypeId == ConstraintTypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); - Vector3Wide.ReadFirst(source.Contact0.LocalOffsetA, out description.Contact0.LocalOffsetA); + Vector3Wide.ReadFirst(source.Contact0.OffsetA, out description.Contact0.OffsetA); description.Contact0.PenetrationDepth = GetFirst(ref source.Contact0.Depth); - description.Contact0.PlaneOffsetB = GetFirst(ref source.Contact0.PlaneOffsetB); - Vector3Wide.ReadFirst(source.LocalNormalB, out description.LocalNormalB); + Vector3Wide.ReadFirst(source.Normal, out description.Normal); description.FrictionCoefficient = GetFirst(ref source.MaterialProperties.FrictionCoefficient); SpringSettingsWide.ReadFirst(source.MaterialProperties.SpringSettings, out description.SpringSettings); description.MaximumRecoveryVelocity = GetFirst(ref source.MaterialProperties.MaximumRecoveryVelocity); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void CopyManifoldWideProperties(ref Vector3 localNormalB, ref PairMaterialProperties material) + public void CopyManifoldWideProperties(ref Vector3 normal, ref PairMaterialProperties material) { FrictionCoefficient = material.FrictionCoefficient; - LocalNormalB = localNormalB; + Normal = normal; SpringSettings = material.SpringSettings; MaximumRecoveryVelocity = material.MaximumRecoveryVelocity; } @@ -263,16 +261,16 @@ public struct Contact1OneBodyPrestepData : IConvexContactPrestep 1; public readonly int ContactCount => 1; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3Wide GetLocalNormalB(ref Contact1OneBodyPrestepData prestep) + public ref Vector3Wide GetNormal(ref Contact1OneBodyPrestepData prestep) { - return ref prestep.LocalNormalB; + return ref prestep.Normal; } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -286,6 +284,7 @@ public ref MaterialPropertiesWide GetMaterialProperties(ref Contact1OneBodyPrest { return ref prestep.MaterialProperties; } + } public unsafe struct Contact1OneBodyProjection @@ -308,81 +307,75 @@ public void Prestep( in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref Contact1OneBodyPrestepData prestep, out Contact1OneBodyProjection projection) { //Be careful about the execution order here. It should be aligned with the prestep data layout to ensure prefetching works well. - //projection.InertiaA = inertiaA; - //QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); - //projection.PremultipliedFrictionCoefficient = prestep.MaterialProperties.FrictionCoefficient; - //projection.Normal = prestep.Normal; - //Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); - //TangentFrictionOneBody.Prestep(ref x, ref z, ref worldOffsetA0, ref projection.InertiaA, out projection.Tangent); - //SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); - //PenetrationLimitOneBody.Prestep(projection.InertiaA, worldOffsetA0, prestep.Normal, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration0); + projection.InertiaA = inertiaA; + projection.PremultipliedFrictionCoefficient = prestep.MaterialProperties.FrictionCoefficient; + projection.Normal = prestep.Normal; + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + TangentFrictionOneBody.Prestep(ref x, ref z, ref prestep.Contact0.OffsetA, ref projection.InertiaA, out projection.Tangent); + SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); + PenetrationLimitOneBody.Prestep(projection.InertiaA, prestep.Contact0.OffsetA, prestep.Normal, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration0); //If there's only one contact, then the contact patch as determined by contact distance would be zero. //That can cause some subtle behavioral issues sometimes, so we approximate lever arm with the contact depth, assuming that the contact surface area will increase as the depth increases. - //projection.LeverArm0 = Vector.Max(Vector.Zero, prestep.Contact0.Depth); - //TwistFrictionOneBody.Prestep(ref projection.InertiaA, ref prestep.Normal, out projection.Twist); - - projection = default; + projection.LeverArm0 = Vector.Max(Vector.Zero, prestep.Contact0.Depth); + TwistFrictionOneBody.Prestep(ref projection.InertiaA, ref prestep.Normal, out projection.Twist); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart(ref BodyVelocityWide wsvA, ref Contact1OneBodyProjection projection, ref Contact1AccumulatedImpulses accumulatedImpulses) { - //Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - //TangentFrictionOneBody.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref accumulatedImpulses.Tangent, ref wsvA); - //PenetrationLimitOneBody.WarmStart(projection.Penetration0, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration0, ref wsvA); - //TwistFrictionOneBody.WarmStart(ref projection.Normal, ref projection.InertiaA, ref accumulatedImpulses.Twist, ref wsvA); + Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); + TangentFrictionOneBody.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref accumulatedImpulses.Tangent, ref wsvA); + PenetrationLimitOneBody.WarmStart(projection.Penetration0, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration0, ref wsvA); + TwistFrictionOneBody.WarmStart(ref projection.Normal, ref projection.InertiaA, ref accumulatedImpulses.Twist, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve(ref BodyVelocityWide wsvA, ref Contact1OneBodyProjection projection, ref Contact1AccumulatedImpulses accumulatedImpulses) { - //Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - //var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * - // (accumulatedImpulses.Penetration0); - //TangentFrictionOneBody.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); + Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); + var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * + (accumulatedImpulses.Penetration0); + TangentFrictionOneBody.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); //Note that we solve the penetration constraints after the friction constraints. //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. //It's a pretty minor effect either way. - //PenetrationLimitOneBody.Solve(projection.Penetration0, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); - //var maximumTwistImpulse = projection.PremultipliedFrictionCoefficient * ( - // accumulatedImpulses.Penetration0 * projection.LeverArm0); - //TwistFrictionOneBody.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); + PenetrationLimitOneBody.Solve(projection.Penetration0, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); + var maximumTwistImpulse = projection.PremultipliedFrictionCoefficient * ( + accumulatedImpulses.Penetration0 * projection.LeverArm0); + TwistFrictionOneBody.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityWide velocityA, ref Contact1OneBodyPrestepData prestep) { - //QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); - //PenetrationLimitOneBody.UpdatePenetrationDepth(dt, worldOffsetA, prestep.Normal, velocityA, ref prestep.Contact0.Depth); + PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, velocityA, ref prestep.Contact0.Depth); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Contact1OneBodyPrestepData prestep, in Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { - Helpers.BuildOrthonormalBasis(prestep.LocalNormalB, out var x, out var z); - QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); - TangentFrictionOneBody.WarmStart2(x, z, worldOffsetA0, inertiaA, accumulatedImpulses.Tangent, ref wsvA); - PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.LocalNormalB, worldOffsetA0, accumulatedImpulses.Penetration0, ref wsvA); - TwistFrictionOneBody.WarmStart2(prestep.LocalNormalB, inertiaA, accumulatedImpulses.Twist, ref wsvA); + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + TangentFrictionOneBody.WarmStart2(x, z, prestep.Contact0.OffsetA, inertiaA, accumulatedImpulses.Tangent, ref wsvA); + PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, accumulatedImpulses.Penetration0, ref wsvA); + TwistFrictionOneBody.WarmStart2(prestep.Normal, inertiaA, accumulatedImpulses.Twist, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, in Contact1OneBodyPrestepData prestep, ref Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { - Helpers.BuildOrthonormalBasis(prestep.LocalNormalB, out var x, out var z); - QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); var maximumTangentImpulse = prestep.MaterialProperties.FrictionCoefficient * (accumulatedImpulses.Penetration0); - TangentFrictionOneBody.Solve2(x, z, worldOffsetA0, inertiaA, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); + TangentFrictionOneBody.Solve2(x, z, prestep.Contact0.OffsetA, inertiaA, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); //Note that we solve the penetration constraints after the friction constraints. //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. //It's a pretty minor effect either way. SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); var inverseDtWide = new Vector(inverseDt); - PenetrationLimitOneBody.Solve2(inertiaA, prestep.LocalNormalB, worldOffsetA0, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); + PenetrationLimitOneBody.Solve2(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); //If there's only one contact, then the contact patch as determined by contact distance would be zero. //That can cause some subtle behavioral issues sometimes, so we approximate lever arm with the contact depth, assuming that the contact surface area will increase as the depth increases. var maximumTwistImpulse = prestep.MaterialProperties.FrictionCoefficient * accumulatedImpulses.Penetration0 * prestep.Contact0.Depth; - TwistFrictionOneBody.Solve2(prestep.LocalNormalB, inertiaA, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); + TwistFrictionOneBody.Solve2(prestep.Normal, inertiaA, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -390,7 +383,7 @@ public void UpdateForNewPose( in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector dt, in Contact1AccumulatedImpulses accumulatedImpulses, ref Contact1OneBodyPrestepData prestep) { - //PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, wsvA, ref prestep.Contact0.Depth); + PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, wsvA, ref prestep.Contact0.Depth); } } @@ -409,7 +402,7 @@ public struct Contact2OneBody : IConvexOneBodyContactConstraintDescription.Get(ref batch.PrestepData, bundleIndex), innerIndex); - Vector3Wide.WriteFirst(Contact0.LocalOffsetA, ref target.Contact0.LocalOffsetA); + Vector3Wide.WriteFirst(Contact0.OffsetA, ref target.Contact0.OffsetA); GetFirst(ref target.Contact0.Depth) = Contact0.PenetrationDepth; - GetFirst(ref target.Contact0.PlaneOffsetB) = Contact0.PlaneOffsetB; - Vector3Wide.WriteFirst(Contact1.LocalOffsetA, ref target.Contact1.LocalOffsetA); + Vector3Wide.WriteFirst(Contact1.OffsetA, ref target.Contact1.OffsetA); GetFirst(ref target.Contact1.Depth) = Contact1.PenetrationDepth; - GetFirst(ref target.Contact1.PlaneOffsetB) = Contact1.PlaneOffsetB; - - Vector3Wide.WriteFirst(LocalNormalB, ref target.LocalNormalB); + + Vector3Wide.WriteFirst(Normal, ref target.Normal); GetFirst(ref target.MaterialProperties.FrictionCoefficient) = FrictionCoefficient; SpringSettingsWide.WriteFirst(SpringSettings, ref target.MaterialProperties.SpringSettings); GetFirst(ref target.MaterialProperties.MaximumRecoveryVelocity) = MaximumRecoveryVelocity; @@ -435,23 +426,21 @@ public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int { Debug.Assert(batch.TypeId == ConstraintTypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); - Vector3Wide.ReadFirst(source.Contact0.LocalOffsetA, out description.Contact0.LocalOffsetA); + Vector3Wide.ReadFirst(source.Contact0.OffsetA, out description.Contact0.OffsetA); description.Contact0.PenetrationDepth = GetFirst(ref source.Contact0.Depth); - description.Contact0.PlaneOffsetB = GetFirst(ref source.Contact0.PlaneOffsetB); - Vector3Wide.ReadFirst(source.Contact1.LocalOffsetA, out description.Contact1.LocalOffsetA); + Vector3Wide.ReadFirst(source.Contact1.OffsetA, out description.Contact1.OffsetA); description.Contact1.PenetrationDepth = GetFirst(ref source.Contact1.Depth); - description.Contact1.PlaneOffsetB = GetFirst(ref source.Contact1.PlaneOffsetB); - Vector3Wide.ReadFirst(source.LocalNormalB, out description.LocalNormalB); + Vector3Wide.ReadFirst(source.Normal, out description.Normal); description.FrictionCoefficient = GetFirst(ref source.MaterialProperties.FrictionCoefficient); SpringSettingsWide.ReadFirst(source.MaterialProperties.SpringSettings, out description.SpringSettings); description.MaximumRecoveryVelocity = GetFirst(ref source.MaterialProperties.MaximumRecoveryVelocity); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void CopyManifoldWideProperties(ref Vector3 localNormalB, ref PairMaterialProperties material) + public void CopyManifoldWideProperties(ref Vector3 normal, ref PairMaterialProperties material) { FrictionCoefficient = material.FrictionCoefficient; - LocalNormalB = localNormalB; + Normal = normal; SpringSettings = material.SpringSettings; MaximumRecoveryVelocity = material.MaximumRecoveryVelocity; } @@ -480,16 +469,16 @@ public struct Contact2OneBodyPrestepData : IConvexContactPrestep 1; public readonly int ContactCount => 2; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3Wide GetLocalNormalB(ref Contact2OneBodyPrestepData prestep) + public ref Vector3Wide GetNormal(ref Contact2OneBodyPrestepData prestep) { - return ref prestep.LocalNormalB; + return ref prestep.Normal; } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -503,6 +492,7 @@ public ref MaterialPropertiesWide GetMaterialProperties(ref Contact2OneBodyPrest { return ref prestep.MaterialProperties; } + } public unsafe struct Contact2OneBodyProjection @@ -527,95 +517,85 @@ public void Prestep( in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref Contact2OneBodyPrestepData prestep, out Contact2OneBodyProjection projection) { //Be careful about the execution order here. It should be aligned with the prestep data layout to ensure prefetching works well. - //projection.InertiaA = inertiaA; - //QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); - //QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); - //FrictionHelpers.ComputeFrictionCenter(worldOffsetA0, worldOffsetA1, prestep.Contact0.Depth, prestep.Contact1.Depth, out var offsetToManifoldCenterA); - //projection.PremultipliedFrictionCoefficient = (1f / 2f) * prestep.MaterialProperties.FrictionCoefficient; - //projection.Normal = prestep.Normal; - //Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); - //TangentFrictionOneBody.Prestep(ref x, ref z, ref offsetToManifoldCenterA, ref projection.InertiaA, out projection.Tangent); - //SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); - //PenetrationLimitOneBody.Prestep(projection.InertiaA, worldOffsetA0, prestep.Normal, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration0); - //PenetrationLimitOneBody.Prestep(projection.InertiaA, worldOffsetA1, prestep.Normal, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration1); - //Vector3Wide.Distance(worldOffsetA0, offsetToManifoldCenterA, out projection.LeverArm0); - //Vector3Wide.Distance(worldOffsetA1, offsetToManifoldCenterA, out projection.LeverArm1); - //TwistFrictionOneBody.Prestep(ref projection.InertiaA, ref prestep.Normal, out projection.Twist); - - projection = default; + projection.InertiaA = inertiaA; + FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, out var offsetToManifoldCenterA); + projection.PremultipliedFrictionCoefficient = (1f / 2f) * prestep.MaterialProperties.FrictionCoefficient; + projection.Normal = prestep.Normal; + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + TangentFrictionOneBody.Prestep(ref x, ref z, ref offsetToManifoldCenterA, ref projection.InertiaA, out projection.Tangent); + SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); + PenetrationLimitOneBody.Prestep(projection.InertiaA, prestep.Contact0.OffsetA, prestep.Normal, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration0); + PenetrationLimitOneBody.Prestep(projection.InertiaA, prestep.Contact1.OffsetA, prestep.Normal, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration1); + Vector3Wide.Distance(prestep.Contact0.OffsetA, offsetToManifoldCenterA, out projection.LeverArm0); + Vector3Wide.Distance(prestep.Contact1.OffsetA, offsetToManifoldCenterA, out projection.LeverArm1); + TwistFrictionOneBody.Prestep(ref projection.InertiaA, ref prestep.Normal, out projection.Twist); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart(ref BodyVelocityWide wsvA, ref Contact2OneBodyProjection projection, ref Contact2AccumulatedImpulses accumulatedImpulses) { - //Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - //TangentFrictionOneBody.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref accumulatedImpulses.Tangent, ref wsvA); - //PenetrationLimitOneBody.WarmStart(projection.Penetration0, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration0, ref wsvA); - //PenetrationLimitOneBody.WarmStart(projection.Penetration1, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration1, ref wsvA); - //TwistFrictionOneBody.WarmStart(ref projection.Normal, ref projection.InertiaA, ref accumulatedImpulses.Twist, ref wsvA); + Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); + TangentFrictionOneBody.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref accumulatedImpulses.Tangent, ref wsvA); + PenetrationLimitOneBody.WarmStart(projection.Penetration0, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration0, ref wsvA); + PenetrationLimitOneBody.WarmStart(projection.Penetration1, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration1, ref wsvA); + TwistFrictionOneBody.WarmStart(ref projection.Normal, ref projection.InertiaA, ref accumulatedImpulses.Twist, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve(ref BodyVelocityWide wsvA, ref Contact2OneBodyProjection projection, ref Contact2AccumulatedImpulses accumulatedImpulses) { - //Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - //var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * - // (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1); - //TangentFrictionOneBody.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); + Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); + var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * + (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1); + TangentFrictionOneBody.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); //Note that we solve the penetration constraints after the friction constraints. //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. //It's a pretty minor effect either way. - //PenetrationLimitOneBody.Solve(projection.Penetration0, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); - //PenetrationLimitOneBody.Solve(projection.Penetration1, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA); - //var maximumTwistImpulse = projection.PremultipliedFrictionCoefficient * ( - // accumulatedImpulses.Penetration0 * projection.LeverArm0 + - // accumulatedImpulses.Penetration1 * projection.LeverArm1); - //TwistFrictionOneBody.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); + PenetrationLimitOneBody.Solve(projection.Penetration0, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); + PenetrationLimitOneBody.Solve(projection.Penetration1, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA); + var maximumTwistImpulse = projection.PremultipliedFrictionCoefficient * ( + accumulatedImpulses.Penetration0 * projection.LeverArm0 + + accumulatedImpulses.Penetration1 * projection.LeverArm1); + TwistFrictionOneBody.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityWide velocityA, ref Contact2OneBodyPrestepData prestep) { - //QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); - //PenetrationLimitOneBody.UpdatePenetrationDepth(dt, worldOffsetA, prestep.Normal, velocityA, ref prestep.Contact0.Depth); - //QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); - //PenetrationLimitOneBody.UpdatePenetrationDepth(dt, worldOffsetA, prestep.Normal, velocityA, ref prestep.Contact1.Depth); + PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, velocityA, ref prestep.Contact0.Depth); + PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.Normal, velocityA, ref prestep.Contact1.Depth); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Contact2OneBodyPrestepData prestep, in Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { - Helpers.BuildOrthonormalBasis(prestep.LocalNormalB, out var x, out var z); - QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); - QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); - FrictionHelpers.ComputeFrictionCenter(worldOffsetA0, worldOffsetA1, prestep.Contact0.Depth, prestep.Contact1.Depth, out var offsetToManifoldCenterA); + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, out var offsetToManifoldCenterA); TangentFrictionOneBody.WarmStart2(x, z, offsetToManifoldCenterA, inertiaA, accumulatedImpulses.Tangent, ref wsvA); - PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.LocalNormalB, worldOffsetA0, accumulatedImpulses.Penetration0, ref wsvA); - PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.LocalNormalB, worldOffsetA1, accumulatedImpulses.Penetration1, ref wsvA); - TwistFrictionOneBody.WarmStart2(prestep.LocalNormalB, inertiaA, accumulatedImpulses.Twist, ref wsvA); + PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, accumulatedImpulses.Penetration0, ref wsvA); + PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact1.OffsetA, accumulatedImpulses.Penetration1, ref wsvA); + TwistFrictionOneBody.WarmStart2(prestep.Normal, inertiaA, accumulatedImpulses.Twist, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, in Contact2OneBodyPrestepData prestep, ref Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { - Helpers.BuildOrthonormalBasis(prestep.LocalNormalB, out var x, out var z); - QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); - QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); var premultipliedFrictionCoefficient = new Vector(1f / 2f) * prestep.MaterialProperties.FrictionCoefficient; var maximumTangentImpulse = premultipliedFrictionCoefficient * (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1); - FrictionHelpers.ComputeFrictionCenter(worldOffsetA0, worldOffsetA1, prestep.Contact0.Depth, prestep.Contact1.Depth, out var offsetToManifoldCenterA); + FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, out var offsetToManifoldCenterA); TangentFrictionOneBody.Solve2(x, z, offsetToManifoldCenterA, inertiaA, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); //Note that we solve the penetration constraints after the friction constraints. //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. //It's a pretty minor effect either way. SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); var inverseDtWide = new Vector(inverseDt); - PenetrationLimitOneBody.Solve2(inertiaA, prestep.LocalNormalB, worldOffsetA0, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); - PenetrationLimitOneBody.Solve2(inertiaA, prestep.LocalNormalB, worldOffsetA0, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA); + PenetrationLimitOneBody.Solve2(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); + PenetrationLimitOneBody.Solve2(inertiaA, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA); var maximumTwistImpulse = premultipliedFrictionCoefficient * ( - accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, worldOffsetA0) + - accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, worldOffsetA1)); - TwistFrictionOneBody.Solve2(prestep.LocalNormalB, inertiaA, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); + accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact0.OffsetA) + + accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact1.OffsetA)); + TwistFrictionOneBody.Solve2(prestep.Normal, inertiaA, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -623,8 +603,8 @@ public void UpdateForNewPose( in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector dt, in Contact2AccumulatedImpulses accumulatedImpulses, ref Contact2OneBodyPrestepData prestep) { - //PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, wsvA, ref prestep.Contact0.Depth); - //PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.Normal, wsvA, ref prestep.Contact1.Depth); + PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, wsvA, ref prestep.Contact0.Depth); + PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.Normal, wsvA, ref prestep.Contact1.Depth); } } @@ -644,7 +624,7 @@ public struct Contact3OneBody : IConvexOneBodyContactConstraintDescription.Get(ref batch.PrestepData, bundleIndex), innerIndex); - Vector3Wide.WriteFirst(Contact0.LocalOffsetA, ref target.Contact0.LocalOffsetA); + Vector3Wide.WriteFirst(Contact0.OffsetA, ref target.Contact0.OffsetA); GetFirst(ref target.Contact0.Depth) = Contact0.PenetrationDepth; - GetFirst(ref target.Contact0.PlaneOffsetB) = Contact0.PlaneOffsetB; - Vector3Wide.WriteFirst(Contact1.LocalOffsetA, ref target.Contact1.LocalOffsetA); + Vector3Wide.WriteFirst(Contact1.OffsetA, ref target.Contact1.OffsetA); GetFirst(ref target.Contact1.Depth) = Contact1.PenetrationDepth; - GetFirst(ref target.Contact1.PlaneOffsetB) = Contact1.PlaneOffsetB; - Vector3Wide.WriteFirst(Contact2.LocalOffsetA, ref target.Contact2.LocalOffsetA); + Vector3Wide.WriteFirst(Contact2.OffsetA, ref target.Contact2.OffsetA); GetFirst(ref target.Contact2.Depth) = Contact2.PenetrationDepth; - GetFirst(ref target.Contact2.PlaneOffsetB) = Contact2.PlaneOffsetB; - - Vector3Wide.WriteFirst(LocalNormalB, ref target.LocalNormalB); + + Vector3Wide.WriteFirst(Normal, ref target.Normal); GetFirst(ref target.MaterialProperties.FrictionCoefficient) = FrictionCoefficient; SpringSettingsWide.WriteFirst(SpringSettings, ref target.MaterialProperties.SpringSettings); GetFirst(ref target.MaterialProperties.MaximumRecoveryVelocity) = MaximumRecoveryVelocity; @@ -673,26 +650,23 @@ public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int { Debug.Assert(batch.TypeId == ConstraintTypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); - Vector3Wide.ReadFirst(source.Contact0.LocalOffsetA, out description.Contact0.LocalOffsetA); + Vector3Wide.ReadFirst(source.Contact0.OffsetA, out description.Contact0.OffsetA); description.Contact0.PenetrationDepth = GetFirst(ref source.Contact0.Depth); - description.Contact0.PlaneOffsetB = GetFirst(ref source.Contact0.PlaneOffsetB); - Vector3Wide.ReadFirst(source.Contact1.LocalOffsetA, out description.Contact1.LocalOffsetA); + Vector3Wide.ReadFirst(source.Contact1.OffsetA, out description.Contact1.OffsetA); description.Contact1.PenetrationDepth = GetFirst(ref source.Contact1.Depth); - description.Contact1.PlaneOffsetB = GetFirst(ref source.Contact1.PlaneOffsetB); - Vector3Wide.ReadFirst(source.Contact2.LocalOffsetA, out description.Contact2.LocalOffsetA); + Vector3Wide.ReadFirst(source.Contact2.OffsetA, out description.Contact2.OffsetA); description.Contact2.PenetrationDepth = GetFirst(ref source.Contact2.Depth); - description.Contact2.PlaneOffsetB = GetFirst(ref source.Contact2.PlaneOffsetB); - Vector3Wide.ReadFirst(source.LocalNormalB, out description.LocalNormalB); + Vector3Wide.ReadFirst(source.Normal, out description.Normal); description.FrictionCoefficient = GetFirst(ref source.MaterialProperties.FrictionCoefficient); SpringSettingsWide.ReadFirst(source.MaterialProperties.SpringSettings, out description.SpringSettings); description.MaximumRecoveryVelocity = GetFirst(ref source.MaterialProperties.MaximumRecoveryVelocity); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void CopyManifoldWideProperties(ref Vector3 localNormalB, ref PairMaterialProperties material) + public void CopyManifoldWideProperties(ref Vector3 normal, ref PairMaterialProperties material) { FrictionCoefficient = material.FrictionCoefficient; - LocalNormalB = localNormalB; + Normal = normal; SpringSettings = material.SpringSettings; MaximumRecoveryVelocity = material.MaximumRecoveryVelocity; } @@ -722,16 +696,16 @@ public struct Contact3OneBodyPrestepData : IConvexContactPrestep 1; public readonly int ContactCount => 3; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3Wide GetLocalNormalB(ref Contact3OneBodyPrestepData prestep) + public ref Vector3Wide GetNormal(ref Contact3OneBodyPrestepData prestep) { - return ref prestep.LocalNormalB; + return ref prestep.Normal; } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -745,6 +719,7 @@ public ref MaterialPropertiesWide GetMaterialProperties(ref Contact3OneBodyPrest { return ref prestep.MaterialProperties; } + } public unsafe struct Contact3OneBodyProjection @@ -771,108 +746,94 @@ public void Prestep( in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref Contact3OneBodyPrestepData prestep, out Contact3OneBodyProjection projection) { //Be careful about the execution order here. It should be aligned with the prestep data layout to ensure prefetching works well. - //projection.InertiaA = inertiaA; - //QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); - //QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); - //QuaternionWide.TransformWithoutOverlap(prestep.Contact2.LocalOffsetA, orientationA, out var worldOffsetA2); - //FrictionHelpers.ComputeFrictionCenter(worldOffsetA0, worldOffsetA1, worldOffsetA2, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, out var offsetToManifoldCenterA); - //projection.PremultipliedFrictionCoefficient = (1f / 3f) * prestep.MaterialProperties.FrictionCoefficient; - //projection.Normal = prestep.Normal; - //Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); - //TangentFrictionOneBody.Prestep(ref x, ref z, ref offsetToManifoldCenterA, ref projection.InertiaA, out projection.Tangent); - //SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); - //PenetrationLimitOneBody.Prestep(projection.InertiaA, worldOffsetA0, prestep.Normal, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration0); - //PenetrationLimitOneBody.Prestep(projection.InertiaA, worldOffsetA1, prestep.Normal, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration1); - //PenetrationLimitOneBody.Prestep(projection.InertiaA, worldOffsetA2, prestep.Normal, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration2); - //Vector3Wide.Distance(worldOffsetA0, offsetToManifoldCenterA, out projection.LeverArm0); - //Vector3Wide.Distance(worldOffsetA1, offsetToManifoldCenterA, out projection.LeverArm1); - //Vector3Wide.Distance(worldOffsetA2, offsetToManifoldCenterA, out projection.LeverArm2); - //TwistFrictionOneBody.Prestep(ref projection.InertiaA, ref prestep.Normal, out projection.Twist); - - projection = default; + projection.InertiaA = inertiaA; + FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, out var offsetToManifoldCenterA); + projection.PremultipliedFrictionCoefficient = (1f / 3f) * prestep.MaterialProperties.FrictionCoefficient; + projection.Normal = prestep.Normal; + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + TangentFrictionOneBody.Prestep(ref x, ref z, ref offsetToManifoldCenterA, ref projection.InertiaA, out projection.Tangent); + SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); + PenetrationLimitOneBody.Prestep(projection.InertiaA, prestep.Contact0.OffsetA, prestep.Normal, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration0); + PenetrationLimitOneBody.Prestep(projection.InertiaA, prestep.Contact1.OffsetA, prestep.Normal, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration1); + PenetrationLimitOneBody.Prestep(projection.InertiaA, prestep.Contact2.OffsetA, prestep.Normal, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration2); + Vector3Wide.Distance(prestep.Contact0.OffsetA, offsetToManifoldCenterA, out projection.LeverArm0); + Vector3Wide.Distance(prestep.Contact1.OffsetA, offsetToManifoldCenterA, out projection.LeverArm1); + Vector3Wide.Distance(prestep.Contact2.OffsetA, offsetToManifoldCenterA, out projection.LeverArm2); + TwistFrictionOneBody.Prestep(ref projection.InertiaA, ref prestep.Normal, out projection.Twist); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart(ref BodyVelocityWide wsvA, ref Contact3OneBodyProjection projection, ref Contact3AccumulatedImpulses accumulatedImpulses) { - //Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - //TangentFrictionOneBody.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref accumulatedImpulses.Tangent, ref wsvA); - //PenetrationLimitOneBody.WarmStart(projection.Penetration0, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration0, ref wsvA); - //PenetrationLimitOneBody.WarmStart(projection.Penetration1, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration1, ref wsvA); - //PenetrationLimitOneBody.WarmStart(projection.Penetration2, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration2, ref wsvA); - //TwistFrictionOneBody.WarmStart(ref projection.Normal, ref projection.InertiaA, ref accumulatedImpulses.Twist, ref wsvA); + Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); + TangentFrictionOneBody.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref accumulatedImpulses.Tangent, ref wsvA); + PenetrationLimitOneBody.WarmStart(projection.Penetration0, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration0, ref wsvA); + PenetrationLimitOneBody.WarmStart(projection.Penetration1, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration1, ref wsvA); + PenetrationLimitOneBody.WarmStart(projection.Penetration2, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration2, ref wsvA); + TwistFrictionOneBody.WarmStart(ref projection.Normal, ref projection.InertiaA, ref accumulatedImpulses.Twist, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve(ref BodyVelocityWide wsvA, ref Contact3OneBodyProjection projection, ref Contact3AccumulatedImpulses accumulatedImpulses) { - //Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - //var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * - // (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2); - //TangentFrictionOneBody.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); + Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); + var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * + (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2); + TangentFrictionOneBody.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); //Note that we solve the penetration constraints after the friction constraints. //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. //It's a pretty minor effect either way. - //PenetrationLimitOneBody.Solve(projection.Penetration0, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); - //PenetrationLimitOneBody.Solve(projection.Penetration1, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA); - //PenetrationLimitOneBody.Solve(projection.Penetration2, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA); - //var maximumTwistImpulse = projection.PremultipliedFrictionCoefficient * ( - // accumulatedImpulses.Penetration0 * projection.LeverArm0 + - // accumulatedImpulses.Penetration1 * projection.LeverArm1 + - // accumulatedImpulses.Penetration2 * projection.LeverArm2); - //TwistFrictionOneBody.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); + PenetrationLimitOneBody.Solve(projection.Penetration0, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); + PenetrationLimitOneBody.Solve(projection.Penetration1, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA); + PenetrationLimitOneBody.Solve(projection.Penetration2, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA); + var maximumTwistImpulse = projection.PremultipliedFrictionCoefficient * ( + accumulatedImpulses.Penetration0 * projection.LeverArm0 + + accumulatedImpulses.Penetration1 * projection.LeverArm1 + + accumulatedImpulses.Penetration2 * projection.LeverArm2); + TwistFrictionOneBody.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityWide velocityA, ref Contact3OneBodyPrestepData prestep) { - //QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); - //PenetrationLimitOneBody.UpdatePenetrationDepth(dt, worldOffsetA, prestep.Normal, velocityA, ref prestep.Contact0.Depth); - //QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); - //PenetrationLimitOneBody.UpdatePenetrationDepth(dt, worldOffsetA, prestep.Normal, velocityA, ref prestep.Contact1.Depth); - //QuaternionWide.TransformWithoutOverlap(prestep.Contact2.LocalOffsetA, orientationA, out var worldOffsetA2); - //PenetrationLimitOneBody.UpdatePenetrationDepth(dt, worldOffsetA, prestep.Normal, velocityA, ref prestep.Contact2.Depth); + PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, velocityA, ref prestep.Contact0.Depth); + PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.Normal, velocityA, ref prestep.Contact1.Depth); + PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact2.OffsetA, prestep.Normal, velocityA, ref prestep.Contact2.Depth); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Contact3OneBodyPrestepData prestep, in Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { - Helpers.BuildOrthonormalBasis(prestep.LocalNormalB, out var x, out var z); - QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); - QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); - QuaternionWide.TransformWithoutOverlap(prestep.Contact2.LocalOffsetA, orientationA, out var worldOffsetA2); - FrictionHelpers.ComputeFrictionCenter(worldOffsetA0, worldOffsetA1, worldOffsetA2, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, out var offsetToManifoldCenterA); + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, out var offsetToManifoldCenterA); TangentFrictionOneBody.WarmStart2(x, z, offsetToManifoldCenterA, inertiaA, accumulatedImpulses.Tangent, ref wsvA); - PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.LocalNormalB, worldOffsetA0, accumulatedImpulses.Penetration0, ref wsvA); - PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.LocalNormalB, worldOffsetA1, accumulatedImpulses.Penetration1, ref wsvA); - PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.LocalNormalB, worldOffsetA2, accumulatedImpulses.Penetration2, ref wsvA); - TwistFrictionOneBody.WarmStart2(prestep.LocalNormalB, inertiaA, accumulatedImpulses.Twist, ref wsvA); + PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, accumulatedImpulses.Penetration0, ref wsvA); + PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact1.OffsetA, accumulatedImpulses.Penetration1, ref wsvA); + PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact2.OffsetA, accumulatedImpulses.Penetration2, ref wsvA); + TwistFrictionOneBody.WarmStart2(prestep.Normal, inertiaA, accumulatedImpulses.Twist, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, in Contact3OneBodyPrestepData prestep, ref Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { - Helpers.BuildOrthonormalBasis(prestep.LocalNormalB, out var x, out var z); - QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); - QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); - QuaternionWide.TransformWithoutOverlap(prestep.Contact2.LocalOffsetA, orientationA, out var worldOffsetA2); + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); var premultipliedFrictionCoefficient = new Vector(1f / 3f) * prestep.MaterialProperties.FrictionCoefficient; var maximumTangentImpulse = premultipliedFrictionCoefficient * (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2); - FrictionHelpers.ComputeFrictionCenter(worldOffsetA0, worldOffsetA1, worldOffsetA2, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, out var offsetToManifoldCenterA); + FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, out var offsetToManifoldCenterA); TangentFrictionOneBody.Solve2(x, z, offsetToManifoldCenterA, inertiaA, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); //Note that we solve the penetration constraints after the friction constraints. //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. //It's a pretty minor effect either way. SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); var inverseDtWide = new Vector(inverseDt); - PenetrationLimitOneBody.Solve2(inertiaA, prestep.LocalNormalB, worldOffsetA0, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); - PenetrationLimitOneBody.Solve2(inertiaA, prestep.LocalNormalB, worldOffsetA0, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA); - PenetrationLimitOneBody.Solve2(inertiaA, prestep.LocalNormalB, worldOffsetA0, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA); + PenetrationLimitOneBody.Solve2(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); + PenetrationLimitOneBody.Solve2(inertiaA, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA); + PenetrationLimitOneBody.Solve2(inertiaA, prestep.Normal, prestep.Contact2.OffsetA, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA); var maximumTwistImpulse = premultipliedFrictionCoefficient * ( - accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, worldOffsetA0) + - accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, worldOffsetA1) + - accumulatedImpulses.Penetration2 * Vector3Wide.Distance(offsetToManifoldCenterA, worldOffsetA2)); - TwistFrictionOneBody.Solve2(prestep.LocalNormalB, inertiaA, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); + accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact0.OffsetA) + + accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact1.OffsetA) + + accumulatedImpulses.Penetration2 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact2.OffsetA)); + TwistFrictionOneBody.Solve2(prestep.Normal, inertiaA, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -880,9 +841,9 @@ public void UpdateForNewPose( in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector dt, in Contact3AccumulatedImpulses accumulatedImpulses, ref Contact3OneBodyPrestepData prestep) { - //PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, wsvA, ref prestep.Contact0.Depth); - //PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.Normal, wsvA, ref prestep.Contact1.Depth); - //PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact2.OffsetA, prestep.Normal, wsvA, ref prestep.Contact2.Depth); + PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, wsvA, ref prestep.Contact0.Depth); + PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.Normal, wsvA, ref prestep.Contact1.Depth); + PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact2.OffsetA, prestep.Normal, wsvA, ref prestep.Contact2.Depth); } } @@ -903,7 +864,7 @@ public struct Contact4OneBody : IConvexOneBodyContactConstraintDescription.Get(ref batch.PrestepData, bundleIndex), innerIndex); - Vector3Wide.WriteFirst(Contact0.LocalOffsetA, ref target.Contact0.LocalOffsetA); + Vector3Wide.WriteFirst(Contact0.OffsetA, ref target.Contact0.OffsetA); GetFirst(ref target.Contact0.Depth) = Contact0.PenetrationDepth; - GetFirst(ref target.Contact0.PlaneOffsetB) = Contact0.PlaneOffsetB; - Vector3Wide.WriteFirst(Contact1.LocalOffsetA, ref target.Contact1.LocalOffsetA); + Vector3Wide.WriteFirst(Contact1.OffsetA, ref target.Contact1.OffsetA); GetFirst(ref target.Contact1.Depth) = Contact1.PenetrationDepth; - GetFirst(ref target.Contact1.PlaneOffsetB) = Contact1.PlaneOffsetB; - Vector3Wide.WriteFirst(Contact2.LocalOffsetA, ref target.Contact2.LocalOffsetA); + Vector3Wide.WriteFirst(Contact2.OffsetA, ref target.Contact2.OffsetA); GetFirst(ref target.Contact2.Depth) = Contact2.PenetrationDepth; - GetFirst(ref target.Contact2.PlaneOffsetB) = Contact2.PlaneOffsetB; - Vector3Wide.WriteFirst(Contact3.LocalOffsetA, ref target.Contact3.LocalOffsetA); + Vector3Wide.WriteFirst(Contact3.OffsetA, ref target.Contact3.OffsetA); GetFirst(ref target.Contact3.Depth) = Contact3.PenetrationDepth; - GetFirst(ref target.Contact3.PlaneOffsetB) = Contact3.PlaneOffsetB; - - Vector3Wide.WriteFirst(LocalNormalB, ref target.LocalNormalB); + + Vector3Wide.WriteFirst(Normal, ref target.Normal); GetFirst(ref target.MaterialProperties.FrictionCoefficient) = FrictionCoefficient; SpringSettingsWide.WriteFirst(SpringSettings, ref target.MaterialProperties.SpringSettings); GetFirst(ref target.MaterialProperties.MaximumRecoveryVelocity) = MaximumRecoveryVelocity; @@ -935,29 +892,25 @@ public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int { Debug.Assert(batch.TypeId == ConstraintTypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); - Vector3Wide.ReadFirst(source.Contact0.LocalOffsetA, out description.Contact0.LocalOffsetA); + Vector3Wide.ReadFirst(source.Contact0.OffsetA, out description.Contact0.OffsetA); description.Contact0.PenetrationDepth = GetFirst(ref source.Contact0.Depth); - description.Contact0.PlaneOffsetB = GetFirst(ref source.Contact0.PlaneOffsetB); - Vector3Wide.ReadFirst(source.Contact1.LocalOffsetA, out description.Contact1.LocalOffsetA); + Vector3Wide.ReadFirst(source.Contact1.OffsetA, out description.Contact1.OffsetA); description.Contact1.PenetrationDepth = GetFirst(ref source.Contact1.Depth); - description.Contact1.PlaneOffsetB = GetFirst(ref source.Contact1.PlaneOffsetB); - Vector3Wide.ReadFirst(source.Contact2.LocalOffsetA, out description.Contact2.LocalOffsetA); + Vector3Wide.ReadFirst(source.Contact2.OffsetA, out description.Contact2.OffsetA); description.Contact2.PenetrationDepth = GetFirst(ref source.Contact2.Depth); - description.Contact2.PlaneOffsetB = GetFirst(ref source.Contact2.PlaneOffsetB); - Vector3Wide.ReadFirst(source.Contact3.LocalOffsetA, out description.Contact3.LocalOffsetA); + Vector3Wide.ReadFirst(source.Contact3.OffsetA, out description.Contact3.OffsetA); description.Contact3.PenetrationDepth = GetFirst(ref source.Contact3.Depth); - description.Contact3.PlaneOffsetB = GetFirst(ref source.Contact3.PlaneOffsetB); - Vector3Wide.ReadFirst(source.LocalNormalB, out description.LocalNormalB); + Vector3Wide.ReadFirst(source.Normal, out description.Normal); description.FrictionCoefficient = GetFirst(ref source.MaterialProperties.FrictionCoefficient); SpringSettingsWide.ReadFirst(source.MaterialProperties.SpringSettings, out description.SpringSettings); description.MaximumRecoveryVelocity = GetFirst(ref source.MaterialProperties.MaximumRecoveryVelocity); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void CopyManifoldWideProperties(ref Vector3 localNormalB, ref PairMaterialProperties material) + public void CopyManifoldWideProperties(ref Vector3 normal, ref PairMaterialProperties material) { FrictionCoefficient = material.FrictionCoefficient; - LocalNormalB = localNormalB; + Normal = normal; SpringSettings = material.SpringSettings; MaximumRecoveryVelocity = material.MaximumRecoveryVelocity; } @@ -988,16 +941,16 @@ public struct Contact4OneBodyPrestepData : IConvexContactPrestep 1; public readonly int ContactCount => 4; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3Wide GetLocalNormalB(ref Contact4OneBodyPrestepData prestep) + public ref Vector3Wide GetNormal(ref Contact4OneBodyPrestepData prestep) { - return ref prestep.LocalNormalB; + return ref prestep.Normal; } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -1011,6 +964,7 @@ public ref MaterialPropertiesWide GetMaterialProperties(ref Contact4OneBodyPrest { return ref prestep.MaterialProperties; } + } public unsafe struct Contact4OneBodyProjection @@ -1039,121 +993,103 @@ public void Prestep( in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref Contact4OneBodyPrestepData prestep, out Contact4OneBodyProjection projection) { //Be careful about the execution order here. It should be aligned with the prestep data layout to ensure prefetching works well. - //projection.InertiaA = inertiaA; - //QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); - //QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); - //QuaternionWide.TransformWithoutOverlap(prestep.Contact2.LocalOffsetA, orientationA, out var worldOffsetA2); - //QuaternionWide.TransformWithoutOverlap(prestep.Contact3.LocalOffsetA, orientationA, out var worldOffsetA3); - //FrictionHelpers.ComputeFrictionCenter(worldOffsetA0, worldOffsetA1, worldOffsetA2, worldOffsetA3, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, prestep.Contact3.Depth, out var offsetToManifoldCenterA); - //projection.PremultipliedFrictionCoefficient = (1f / 4f) * prestep.MaterialProperties.FrictionCoefficient; - //projection.Normal = prestep.Normal; - //Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); - //TangentFrictionOneBody.Prestep(ref x, ref z, ref offsetToManifoldCenterA, ref projection.InertiaA, out projection.Tangent); - //SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); - //PenetrationLimitOneBody.Prestep(projection.InertiaA, worldOffsetA0, prestep.Normal, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration0); - //PenetrationLimitOneBody.Prestep(projection.InertiaA, worldOffsetA1, prestep.Normal, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration1); - //PenetrationLimitOneBody.Prestep(projection.InertiaA, worldOffsetA2, prestep.Normal, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration2); - //PenetrationLimitOneBody.Prestep(projection.InertiaA, worldOffsetA3, prestep.Normal, prestep.Contact3.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration3); - //Vector3Wide.Distance(worldOffsetA0, offsetToManifoldCenterA, out projection.LeverArm0); - //Vector3Wide.Distance(worldOffsetA1, offsetToManifoldCenterA, out projection.LeverArm1); - //Vector3Wide.Distance(worldOffsetA2, offsetToManifoldCenterA, out projection.LeverArm2); - //Vector3Wide.Distance(worldOffsetA3, offsetToManifoldCenterA, out projection.LeverArm3); - //TwistFrictionOneBody.Prestep(ref projection.InertiaA, ref prestep.Normal, out projection.Twist); - - projection = default; + projection.InertiaA = inertiaA; + FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact3.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, prestep.Contact3.Depth, out var offsetToManifoldCenterA); + projection.PremultipliedFrictionCoefficient = (1f / 4f) * prestep.MaterialProperties.FrictionCoefficient; + projection.Normal = prestep.Normal; + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + TangentFrictionOneBody.Prestep(ref x, ref z, ref offsetToManifoldCenterA, ref projection.InertiaA, out projection.Tangent); + SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); + PenetrationLimitOneBody.Prestep(projection.InertiaA, prestep.Contact0.OffsetA, prestep.Normal, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration0); + PenetrationLimitOneBody.Prestep(projection.InertiaA, prestep.Contact1.OffsetA, prestep.Normal, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration1); + PenetrationLimitOneBody.Prestep(projection.InertiaA, prestep.Contact2.OffsetA, prestep.Normal, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration2); + PenetrationLimitOneBody.Prestep(projection.InertiaA, prestep.Contact3.OffsetA, prestep.Normal, prestep.Contact3.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration3); + Vector3Wide.Distance(prestep.Contact0.OffsetA, offsetToManifoldCenterA, out projection.LeverArm0); + Vector3Wide.Distance(prestep.Contact1.OffsetA, offsetToManifoldCenterA, out projection.LeverArm1); + Vector3Wide.Distance(prestep.Contact2.OffsetA, offsetToManifoldCenterA, out projection.LeverArm2); + Vector3Wide.Distance(prestep.Contact3.OffsetA, offsetToManifoldCenterA, out projection.LeverArm3); + TwistFrictionOneBody.Prestep(ref projection.InertiaA, ref prestep.Normal, out projection.Twist); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart(ref BodyVelocityWide wsvA, ref Contact4OneBodyProjection projection, ref Contact4AccumulatedImpulses accumulatedImpulses) { - //Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - //TangentFrictionOneBody.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref accumulatedImpulses.Tangent, ref wsvA); - //PenetrationLimitOneBody.WarmStart(projection.Penetration0, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration0, ref wsvA); - //PenetrationLimitOneBody.WarmStart(projection.Penetration1, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration1, ref wsvA); - //PenetrationLimitOneBody.WarmStart(projection.Penetration2, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration2, ref wsvA); - //PenetrationLimitOneBody.WarmStart(projection.Penetration3, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration3, ref wsvA); - //TwistFrictionOneBody.WarmStart(ref projection.Normal, ref projection.InertiaA, ref accumulatedImpulses.Twist, ref wsvA); + Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); + TangentFrictionOneBody.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref accumulatedImpulses.Tangent, ref wsvA); + PenetrationLimitOneBody.WarmStart(projection.Penetration0, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration0, ref wsvA); + PenetrationLimitOneBody.WarmStart(projection.Penetration1, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration1, ref wsvA); + PenetrationLimitOneBody.WarmStart(projection.Penetration2, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration2, ref wsvA); + PenetrationLimitOneBody.WarmStart(projection.Penetration3, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration3, ref wsvA); + TwistFrictionOneBody.WarmStart(ref projection.Normal, ref projection.InertiaA, ref accumulatedImpulses.Twist, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve(ref BodyVelocityWide wsvA, ref Contact4OneBodyProjection projection, ref Contact4AccumulatedImpulses accumulatedImpulses) { - //Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - //var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * - // (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2 + accumulatedImpulses.Penetration3); - //TangentFrictionOneBody.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); + Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); + var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * + (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2 + accumulatedImpulses.Penetration3); + TangentFrictionOneBody.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); //Note that we solve the penetration constraints after the friction constraints. //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. //It's a pretty minor effect either way. - //PenetrationLimitOneBody.Solve(projection.Penetration0, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); - //PenetrationLimitOneBody.Solve(projection.Penetration1, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA); - //PenetrationLimitOneBody.Solve(projection.Penetration2, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA); - //PenetrationLimitOneBody.Solve(projection.Penetration3, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration3, ref wsvA); - //var maximumTwistImpulse = projection.PremultipliedFrictionCoefficient * ( - // accumulatedImpulses.Penetration0 * projection.LeverArm0 + - // accumulatedImpulses.Penetration1 * projection.LeverArm1 + - // accumulatedImpulses.Penetration2 * projection.LeverArm2 + - // accumulatedImpulses.Penetration3 * projection.LeverArm3); - //TwistFrictionOneBody.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); + PenetrationLimitOneBody.Solve(projection.Penetration0, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); + PenetrationLimitOneBody.Solve(projection.Penetration1, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA); + PenetrationLimitOneBody.Solve(projection.Penetration2, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA); + PenetrationLimitOneBody.Solve(projection.Penetration3, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration3, ref wsvA); + var maximumTwistImpulse = projection.PremultipliedFrictionCoefficient * ( + accumulatedImpulses.Penetration0 * projection.LeverArm0 + + accumulatedImpulses.Penetration1 * projection.LeverArm1 + + accumulatedImpulses.Penetration2 * projection.LeverArm2 + + accumulatedImpulses.Penetration3 * projection.LeverArm3); + TwistFrictionOneBody.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityWide velocityA, ref Contact4OneBodyPrestepData prestep) { - //QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); - //PenetrationLimitOneBody.UpdatePenetrationDepth(dt, worldOffsetA, prestep.Normal, velocityA, ref prestep.Contact0.Depth); - //QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); - //PenetrationLimitOneBody.UpdatePenetrationDepth(dt, worldOffsetA, prestep.Normal, velocityA, ref prestep.Contact1.Depth); - //QuaternionWide.TransformWithoutOverlap(prestep.Contact2.LocalOffsetA, orientationA, out var worldOffsetA2); - //PenetrationLimitOneBody.UpdatePenetrationDepth(dt, worldOffsetA, prestep.Normal, velocityA, ref prestep.Contact2.Depth); - //QuaternionWide.TransformWithoutOverlap(prestep.Contact3.LocalOffsetA, orientationA, out var worldOffsetA3); - //PenetrationLimitOneBody.UpdatePenetrationDepth(dt, worldOffsetA, prestep.Normal, velocityA, ref prestep.Contact3.Depth); + PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, velocityA, ref prestep.Contact0.Depth); + PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.Normal, velocityA, ref prestep.Contact1.Depth); + PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact2.OffsetA, prestep.Normal, velocityA, ref prestep.Contact2.Depth); + PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact3.OffsetA, prestep.Normal, velocityA, ref prestep.Contact3.Depth); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Contact4OneBodyPrestepData prestep, in Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { - Helpers.BuildOrthonormalBasis(prestep.LocalNormalB, out var x, out var z); - QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); - QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); - QuaternionWide.TransformWithoutOverlap(prestep.Contact2.LocalOffsetA, orientationA, out var worldOffsetA2); - QuaternionWide.TransformWithoutOverlap(prestep.Contact3.LocalOffsetA, orientationA, out var worldOffsetA3); - FrictionHelpers.ComputeFrictionCenter(worldOffsetA0, worldOffsetA1, worldOffsetA2, worldOffsetA3, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, prestep.Contact3.Depth, out var offsetToManifoldCenterA); + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact3.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, prestep.Contact3.Depth, out var offsetToManifoldCenterA); TangentFrictionOneBody.WarmStart2(x, z, offsetToManifoldCenterA, inertiaA, accumulatedImpulses.Tangent, ref wsvA); - PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.LocalNormalB, worldOffsetA0, accumulatedImpulses.Penetration0, ref wsvA); - PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.LocalNormalB, worldOffsetA1, accumulatedImpulses.Penetration1, ref wsvA); - PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.LocalNormalB, worldOffsetA2, accumulatedImpulses.Penetration2, ref wsvA); - PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.LocalNormalB, worldOffsetA3, accumulatedImpulses.Penetration3, ref wsvA); - TwistFrictionOneBody.WarmStart2(prestep.LocalNormalB, inertiaA, accumulatedImpulses.Twist, ref wsvA); + PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, accumulatedImpulses.Penetration0, ref wsvA); + PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact1.OffsetA, accumulatedImpulses.Penetration1, ref wsvA); + PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact2.OffsetA, accumulatedImpulses.Penetration2, ref wsvA); + PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact3.OffsetA, accumulatedImpulses.Penetration3, ref wsvA); + TwistFrictionOneBody.WarmStart2(prestep.Normal, inertiaA, accumulatedImpulses.Twist, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, in Contact4OneBodyPrestepData prestep, ref Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { - Helpers.BuildOrthonormalBasis(prestep.LocalNormalB, out var x, out var z); - QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); - QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); - QuaternionWide.TransformWithoutOverlap(prestep.Contact2.LocalOffsetA, orientationA, out var worldOffsetA2); - QuaternionWide.TransformWithoutOverlap(prestep.Contact3.LocalOffsetA, orientationA, out var worldOffsetA3); + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); var premultipliedFrictionCoefficient = new Vector(1f / 4f) * prestep.MaterialProperties.FrictionCoefficient; var maximumTangentImpulse = premultipliedFrictionCoefficient * (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2 + accumulatedImpulses.Penetration3); - FrictionHelpers.ComputeFrictionCenter(worldOffsetA0, worldOffsetA1, worldOffsetA2, worldOffsetA3, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, prestep.Contact3.Depth, out var offsetToManifoldCenterA); + FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact3.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, prestep.Contact3.Depth, out var offsetToManifoldCenterA); TangentFrictionOneBody.Solve2(x, z, offsetToManifoldCenterA, inertiaA, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); //Note that we solve the penetration constraints after the friction constraints. //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. //It's a pretty minor effect either way. SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); var inverseDtWide = new Vector(inverseDt); - PenetrationLimitOneBody.Solve2(inertiaA, prestep.LocalNormalB, worldOffsetA0, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); - PenetrationLimitOneBody.Solve2(inertiaA, prestep.LocalNormalB, worldOffsetA0, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA); - PenetrationLimitOneBody.Solve2(inertiaA, prestep.LocalNormalB, worldOffsetA0, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA); - PenetrationLimitOneBody.Solve2(inertiaA, prestep.LocalNormalB, worldOffsetA0, prestep.Contact3.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration3, ref wsvA); + PenetrationLimitOneBody.Solve2(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); + PenetrationLimitOneBody.Solve2(inertiaA, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA); + PenetrationLimitOneBody.Solve2(inertiaA, prestep.Normal, prestep.Contact2.OffsetA, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA); + PenetrationLimitOneBody.Solve2(inertiaA, prestep.Normal, prestep.Contact3.OffsetA, prestep.Contact3.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration3, ref wsvA); var maximumTwistImpulse = premultipliedFrictionCoefficient * ( - accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, worldOffsetA0) + - accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, worldOffsetA1) + - accumulatedImpulses.Penetration2 * Vector3Wide.Distance(offsetToManifoldCenterA, worldOffsetA2) + - accumulatedImpulses.Penetration3 * Vector3Wide.Distance(offsetToManifoldCenterA, worldOffsetA3)); - TwistFrictionOneBody.Solve2(prestep.LocalNormalB, inertiaA, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); + accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact0.OffsetA) + + accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact1.OffsetA) + + accumulatedImpulses.Penetration2 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact2.OffsetA) + + accumulatedImpulses.Penetration3 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact3.OffsetA)); + TwistFrictionOneBody.Solve2(prestep.Normal, inertiaA, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -1161,10 +1097,10 @@ public void UpdateForNewPose( in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector dt, in Contact4AccumulatedImpulses accumulatedImpulses, ref Contact4OneBodyPrestepData prestep) { - //PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, wsvA, ref prestep.Contact0.Depth); - //PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.Normal, wsvA, ref prestep.Contact1.Depth); - //PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact2.OffsetA, prestep.Normal, wsvA, ref prestep.Contact2.Depth); - //PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact3.OffsetA, prestep.Normal, wsvA, ref prestep.Contact3.Depth); + PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, wsvA, ref prestep.Contact0.Depth); + PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.Normal, wsvA, ref prestep.Contact1.Depth); + PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact2.OffsetA, prestep.Normal, wsvA, ref prestep.Contact2.Depth); + PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact3.OffsetA, prestep.Normal, wsvA, ref prestep.Contact3.Depth); } } @@ -1182,7 +1118,8 @@ public class Contact4OneBodyTypeProcessor : public struct Contact1 : IConvexTwoBodyContactConstraintDescription { public ConstraintContactData Contact0; - public Vector3 LocalNormalB; + public Vector3 OffsetB; + public Vector3 Normal; public float FrictionCoefficient; public SpringSettings SpringSettings; public float MaximumRecoveryVelocity; @@ -1191,11 +1128,11 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int { Debug.Assert(batch.TypeId == ConstraintTypeId, "The type batch passed to the description must match the description's expected type."); ref var target = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); - Vector3Wide.WriteFirst(Contact0.LocalOffsetA, ref target.Contact0.LocalOffsetA); + Vector3Wide.WriteFirst(Contact0.OffsetA, ref target.Contact0.OffsetA); GetFirst(ref target.Contact0.Depth) = Contact0.PenetrationDepth; - GetFirst(ref target.Contact0.PlaneOffsetB) = Contact0.PlaneOffsetB; - - Vector3Wide.WriteFirst(LocalNormalB, ref target.LocalNormalB); + + Vector3Wide.WriteFirst(OffsetB, ref target.OffsetB); + Vector3Wide.WriteFirst(Normal, ref target.Normal); GetFirst(ref target.MaterialProperties.FrictionCoefficient) = FrictionCoefficient; SpringSettingsWide.WriteFirst(SpringSettings, ref target.MaterialProperties.SpringSettings); GetFirst(ref target.MaterialProperties.MaximumRecoveryVelocity) = MaximumRecoveryVelocity; @@ -1205,20 +1142,22 @@ public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int { Debug.Assert(batch.TypeId == ConstraintTypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); - Vector3Wide.ReadFirst(source.Contact0.LocalOffsetA, out description.Contact0.LocalOffsetA); + Vector3Wide.ReadFirst(source.Contact0.OffsetA, out description.Contact0.OffsetA); description.Contact0.PenetrationDepth = GetFirst(ref source.Contact0.Depth); - description.Contact0.PlaneOffsetB = GetFirst(ref source.Contact0.PlaneOffsetB); - Vector3Wide.ReadFirst(source.LocalNormalB, out description.LocalNormalB); + + Vector3Wide.ReadFirst(source.OffsetB, out description.OffsetB); + Vector3Wide.ReadFirst(source.Normal, out description.Normal); description.FrictionCoefficient = GetFirst(ref source.MaterialProperties.FrictionCoefficient); SpringSettingsWide.ReadFirst(source.MaterialProperties.SpringSettings, out description.SpringSettings); description.MaximumRecoveryVelocity = GetFirst(ref source.MaterialProperties.MaximumRecoveryVelocity); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void CopyManifoldWideProperties(ref Vector3 localNormalB, ref PairMaterialProperties material) + public void CopyManifoldWideProperties(ref Vector3 offsetB, ref Vector3 normal, ref PairMaterialProperties material) { + OffsetB = offsetB; FrictionCoefficient = material.FrictionCoefficient; - LocalNormalB = localNormalB; + Normal = normal; SpringSettings = material.SpringSettings; MaximumRecoveryVelocity = material.MaximumRecoveryVelocity; } @@ -1245,17 +1184,18 @@ public struct Contact1PrestepData : ITwoBodyConvexContactPrestep 2; public readonly int ContactCount => 1; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3Wide GetLocalNormalB(ref Contact1PrestepData prestep) + public ref Vector3Wide GetNormal(ref Contact1PrestepData prestep) { - return ref prestep.LocalNormalB; + return ref prestep.Normal; } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -1269,6 +1209,12 @@ public ref MaterialPropertiesWide GetMaterialProperties(ref Contact1PrestepData { return ref prestep.MaterialProperties; } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public ref Vector3Wide GetOffsetB(ref Contact1PrestepData prestep) + { + return ref prestep.OffsetB; + } } public unsafe struct Contact1Projection @@ -1292,89 +1238,81 @@ public void Prestep( in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref Contact1PrestepData prestep, out Contact1Projection projection) { //Be careful about the execution order here. It should be aligned with the prestep data layout to ensure prefetching works well. - //projection.InertiaA = inertiaA; - //projection.InertiaB = inertiaB; - //QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); - //Vector3Wide.Subtract(worldOffsetA0, ab, out var offsetToManifoldCenterB); - //projection.PremultipliedFrictionCoefficient = prestep.MaterialProperties.FrictionCoefficient; - //projection.Normal = prestep.Normal; - //Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); - //TangentFriction.Prestep(ref x, ref z, ref worldOffsetA0, ref offsetToManifoldCenterB, ref projection.InertiaA, ref projection.InertiaB, out projection.Tangent); - //SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); - //PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, worldOffsetA0, worldOffsetA0 - ab, prestep.Normal, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration0); + projection.InertiaA = inertiaA; + projection.InertiaB = inertiaB; + Vector3Wide.Subtract(prestep.Contact0.OffsetA, prestep.OffsetB, out var offsetToManifoldCenterB); + projection.PremultipliedFrictionCoefficient = prestep.MaterialProperties.FrictionCoefficient; + projection.Normal = prestep.Normal; + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + TangentFriction.Prestep(ref x, ref z, ref prestep.Contact0.OffsetA, ref offsetToManifoldCenterB, ref projection.InertiaA, ref projection.InertiaB, out projection.Tangent); + SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); + Vector3Wide contactOffsetB; + Vector3Wide.Subtract(prestep.Contact0.OffsetA, prestep.OffsetB, out contactOffsetB); + PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, prestep.Contact0.OffsetA, contactOffsetB, prestep.Normal, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration0); //If there's only one contact, then the contact patch as determined by contact distance would be zero. //That can cause some subtle behavioral issues sometimes, so we approximate lever arm with the contact depth, assuming that the contact surface area will increase as the depth increases. - //projection.LeverArm0 = Vector.Max(Vector.Zero, prestep.Contact0.Depth); - //TwistFriction.Prestep(ref projection.InertiaA, ref projection.InertiaB, ref prestep.Normal, out projection.Twist); - - projection = default; + projection.LeverArm0 = Vector.Max(Vector.Zero, prestep.Contact0.Depth); + TwistFriction.Prestep(ref projection.InertiaA, ref projection.InertiaB, ref prestep.Normal, out projection.Twist); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart(ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref Contact1Projection projection, ref Contact1AccumulatedImpulses accumulatedImpulses) { - //Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - //TangentFriction.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); - //PenetrationLimit.WarmStart(projection.Penetration0, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - //TwistFriction.WarmStart(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); + Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); + TangentFriction.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart(projection.Penetration0, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + TwistFriction.WarmStart(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve(ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref Contact1Projection projection, ref Contact1AccumulatedImpulses accumulatedImpulses) { - //Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - //var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * - // (accumulatedImpulses.Penetration0); - //TangentFriction.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref projection.InertiaB, ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); + Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); + var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * + (accumulatedImpulses.Penetration0); + TangentFriction.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref projection.InertiaB, ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); //Note that we solve the penetration constraints after the friction constraints. //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. //It's a pretty minor effect either way. - //PenetrationLimit.Solve(projection.Penetration0, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - //var maximumTwistImpulse = projection.PremultipliedFrictionCoefficient * ( - // accumulatedImpulses.Penetration0 * projection.LeverArm0); - //TwistFriction.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); + PenetrationLimit.Solve(projection.Penetration0, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + var maximumTwistImpulse = projection.PremultipliedFrictionCoefficient * ( + accumulatedImpulses.Penetration0 * projection.LeverArm0); + TwistFriction.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityWide velocityA, in BodyVelocityWide velocityB, ref Contact1PrestepData prestep) { - //QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); - //PenetrationLimit.UpdatePenetrationDepth(dt, worldOffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact0.Depth); + PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact0.Depth); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Contact1PrestepData prestep, in Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - QuaternionWide.TransformWithoutOverlap(prestep.LocalNormalB, orientationB, out var normal); - Helpers.BuildOrthonormalBasis(normal, out var x, out var z); - QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); - var ab = positionB - positionA; - Vector3Wide.Subtract(worldOffsetA0, ab, out var offsetToManifoldCenterB); - TangentFriction.WarmStart2(x, z, worldOffsetA0, offsetToManifoldCenterB, inertiaA, inertiaB, accumulatedImpulses.Tangent, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart2(inertiaA, inertiaB, normal, worldOffsetA0, worldOffsetA0 - ab, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - TwistFriction.WarmStart2(normal, inertiaA, inertiaB, accumulatedImpulses.Twist, ref wsvA, ref wsvB); + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + Vector3Wide.Subtract(prestep.Contact0.OffsetA, prestep.OffsetB, out var offsetToManifoldCenterB); + TangentFriction.WarmStart2(x, z, prestep.Contact0.OffsetA, offsetToManifoldCenterB, inertiaA, inertiaB, accumulatedImpulses.Tangent, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + TwistFriction.WarmStart2(prestep.Normal, inertiaA, inertiaB, accumulatedImpulses.Twist, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in Contact1PrestepData prestep, ref Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - QuaternionWide.TransformWithoutOverlap(prestep.LocalNormalB, orientationB, out var normal); - Helpers.BuildOrthonormalBasis(normal, out var x, out var z); - QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); var maximumTangentImpulse = prestep.MaterialProperties.FrictionCoefficient * (accumulatedImpulses.Penetration0); - var ab = positionB - positionA; - Vector3Wide.Subtract(worldOffsetA0, ab, out var offsetToManifoldCenterB); - TangentFriction.Solve2(x, z, worldOffsetA0, offsetToManifoldCenterB, inertiaA, inertiaB, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); + Vector3Wide.Subtract(prestep.Contact0.OffsetA, prestep.OffsetB, out var offsetToManifoldCenterB); + TangentFriction.Solve2(x, z, prestep.Contact0.OffsetA, offsetToManifoldCenterB, inertiaA, inertiaB, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); //Note that we solve the penetration constraints after the friction constraints. //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. //It's a pretty minor effect either way. SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); var inverseDtWide = new Vector(inverseDt); - PenetrationLimit.Solve2(inertiaA, inertiaB, normal, worldOffsetA0, worldOffsetA0 - ab, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + PenetrationLimit.Solve2(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); //If there's only one contact, then the contact patch as determined by contact distance would be zero. //That can cause some subtle behavioral issues sometimes, so we approximate lever arm with the contact depth, assuming that the contact surface area will increase as the depth increases. var maximumTwistImpulse = prestep.MaterialProperties.FrictionCoefficient * accumulatedImpulses.Penetration0 * prestep.Contact0.Depth; - TwistFriction.Solve2(normal, inertiaA, inertiaB, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); + TwistFriction.Solve2(prestep.Normal, inertiaA, inertiaB, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -1383,8 +1321,8 @@ public void UpdateForNewPose( in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector dt, in Contact1AccumulatedImpulses accumulatedImpulses, ref Contact1PrestepData prestep) { - //prestep.OffsetB = positionB - positionA; - //PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact0.Depth); + prestep.OffsetB = positionB - positionA; + PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact0.Depth); } } @@ -1403,7 +1341,8 @@ public struct Contact2 : IConvexTwoBodyContactConstraintDescription { public ConstraintContactData Contact0; public ConstraintContactData Contact1; - public Vector3 LocalNormalB; + public Vector3 OffsetB; + public Vector3 Normal; public float FrictionCoefficient; public SpringSettings SpringSettings; public float MaximumRecoveryVelocity; @@ -1412,14 +1351,13 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int { Debug.Assert(batch.TypeId == ConstraintTypeId, "The type batch passed to the description must match the description's expected type."); ref var target = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); - Vector3Wide.WriteFirst(Contact0.LocalOffsetA, ref target.Contact0.LocalOffsetA); + Vector3Wide.WriteFirst(Contact0.OffsetA, ref target.Contact0.OffsetA); GetFirst(ref target.Contact0.Depth) = Contact0.PenetrationDepth; - GetFirst(ref target.Contact0.PlaneOffsetB) = Contact0.PlaneOffsetB; - Vector3Wide.WriteFirst(Contact1.LocalOffsetA, ref target.Contact1.LocalOffsetA); + Vector3Wide.WriteFirst(Contact1.OffsetA, ref target.Contact1.OffsetA); GetFirst(ref target.Contact1.Depth) = Contact1.PenetrationDepth; - GetFirst(ref target.Contact1.PlaneOffsetB) = Contact1.PlaneOffsetB; - - Vector3Wide.WriteFirst(LocalNormalB, ref target.LocalNormalB); + + Vector3Wide.WriteFirst(OffsetB, ref target.OffsetB); + Vector3Wide.WriteFirst(Normal, ref target.Normal); GetFirst(ref target.MaterialProperties.FrictionCoefficient) = FrictionCoefficient; SpringSettingsWide.WriteFirst(SpringSettings, ref target.MaterialProperties.SpringSettings); GetFirst(ref target.MaterialProperties.MaximumRecoveryVelocity) = MaximumRecoveryVelocity; @@ -1429,23 +1367,24 @@ public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int { Debug.Assert(batch.TypeId == ConstraintTypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); - Vector3Wide.ReadFirst(source.Contact0.LocalOffsetA, out description.Contact0.LocalOffsetA); + Vector3Wide.ReadFirst(source.Contact0.OffsetA, out description.Contact0.OffsetA); description.Contact0.PenetrationDepth = GetFirst(ref source.Contact0.Depth); - description.Contact0.PlaneOffsetB = GetFirst(ref source.Contact0.PlaneOffsetB); - Vector3Wide.ReadFirst(source.Contact1.LocalOffsetA, out description.Contact1.LocalOffsetA); + Vector3Wide.ReadFirst(source.Contact1.OffsetA, out description.Contact1.OffsetA); description.Contact1.PenetrationDepth = GetFirst(ref source.Contact1.Depth); - description.Contact1.PlaneOffsetB = GetFirst(ref source.Contact1.PlaneOffsetB); - Vector3Wide.ReadFirst(source.LocalNormalB, out description.LocalNormalB); + + Vector3Wide.ReadFirst(source.OffsetB, out description.OffsetB); + Vector3Wide.ReadFirst(source.Normal, out description.Normal); description.FrictionCoefficient = GetFirst(ref source.MaterialProperties.FrictionCoefficient); SpringSettingsWide.ReadFirst(source.MaterialProperties.SpringSettings, out description.SpringSettings); description.MaximumRecoveryVelocity = GetFirst(ref source.MaterialProperties.MaximumRecoveryVelocity); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void CopyManifoldWideProperties(ref Vector3 localNormalB, ref PairMaterialProperties material) + public void CopyManifoldWideProperties(ref Vector3 offsetB, ref Vector3 normal, ref PairMaterialProperties material) { + OffsetB = offsetB; FrictionCoefficient = material.FrictionCoefficient; - LocalNormalB = localNormalB; + Normal = normal; SpringSettings = material.SpringSettings; MaximumRecoveryVelocity = material.MaximumRecoveryVelocity; } @@ -1473,17 +1412,18 @@ public struct Contact2PrestepData : ITwoBodyConvexContactPrestep 2; public readonly int ContactCount => 2; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3Wide GetLocalNormalB(ref Contact2PrestepData prestep) + public ref Vector3Wide GetNormal(ref Contact2PrestepData prestep) { - return ref prestep.LocalNormalB; + return ref prestep.Normal; } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -1497,6 +1437,12 @@ public ref MaterialPropertiesWide GetMaterialProperties(ref Contact2PrestepData { return ref prestep.MaterialProperties; } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public ref Vector3Wide GetOffsetB(ref Contact2PrestepData prestep) + { + return ref prestep.OffsetB; + } } public unsafe struct Contact2Projection @@ -1522,103 +1468,92 @@ public void Prestep( in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref Contact2PrestepData prestep, out Contact2Projection projection) { //Be careful about the execution order here. It should be aligned with the prestep data layout to ensure prefetching works well. - //projection.InertiaA = inertiaA; - //projection.InertiaB = inertiaB; - //QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); - //QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); - //FrictionHelpers.ComputeFrictionCenter(worldOffsetA0, worldOffsetA1, prestep.Contact0.Depth, prestep.Contact1.Depth, out var offsetToManifoldCenterA); - //Vector3Wide.Subtract(offsetToManifoldCenterA, ab, out var offsetToManifoldCenterB); - //projection.PremultipliedFrictionCoefficient = (1f / 2f) * prestep.MaterialProperties.FrictionCoefficient; - //projection.Normal = prestep.Normal; - //Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); - //TangentFriction.Prestep(ref x, ref z, ref offsetToManifoldCenterA, ref offsetToManifoldCenterB, ref projection.InertiaA, ref projection.InertiaB, out projection.Tangent); - //SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); - //PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, worldOffsetA0, worldOffsetA0 - ab, prestep.Normal, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration0); - //PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, worldOffsetA1, worldOffsetA1 - ab, prestep.Normal, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration1); - //Vector3Wide.Distance(worldOffsetA0, offsetToManifoldCenterA, out projection.LeverArm0); - //Vector3Wide.Distance(worldOffsetA1, offsetToManifoldCenterA, out projection.LeverArm1); - //TwistFriction.Prestep(ref projection.InertiaA, ref projection.InertiaB, ref prestep.Normal, out projection.Twist); - - projection = default; + projection.InertiaA = inertiaA; + projection.InertiaB = inertiaB; + FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, out var offsetToManifoldCenterA); + Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); + projection.PremultipliedFrictionCoefficient = (1f / 2f) * prestep.MaterialProperties.FrictionCoefficient; + projection.Normal = prestep.Normal; + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + TangentFriction.Prestep(ref x, ref z, ref offsetToManifoldCenterA, ref offsetToManifoldCenterB, ref projection.InertiaA, ref projection.InertiaB, out projection.Tangent); + SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); + Vector3Wide contactOffsetB; + Vector3Wide.Subtract(prestep.Contact0.OffsetA, prestep.OffsetB, out contactOffsetB); + PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, prestep.Contact0.OffsetA, contactOffsetB, prestep.Normal, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration0); + Vector3Wide.Subtract(prestep.Contact1.OffsetA, prestep.OffsetB, out contactOffsetB); + PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, prestep.Contact1.OffsetA, contactOffsetB, prestep.Normal, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration1); + Vector3Wide.Distance(prestep.Contact0.OffsetA, offsetToManifoldCenterA, out projection.LeverArm0); + Vector3Wide.Distance(prestep.Contact1.OffsetA, offsetToManifoldCenterA, out projection.LeverArm1); + TwistFriction.Prestep(ref projection.InertiaA, ref projection.InertiaB, ref prestep.Normal, out projection.Twist); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart(ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref Contact2Projection projection, ref Contact2AccumulatedImpulses accumulatedImpulses) { - //Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - //TangentFriction.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); - //PenetrationLimit.WarmStart(projection.Penetration0, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - //PenetrationLimit.WarmStart(projection.Penetration1, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); - //TwistFriction.WarmStart(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); + Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); + TangentFriction.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart(projection.Penetration0, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart(projection.Penetration1, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); + TwistFriction.WarmStart(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve(ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref Contact2Projection projection, ref Contact2AccumulatedImpulses accumulatedImpulses) { - //Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - //var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * - // (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1); - //TangentFriction.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref projection.InertiaB, ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); + Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); + var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * + (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1); + TangentFriction.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref projection.InertiaB, ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); //Note that we solve the penetration constraints after the friction constraints. //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. //It's a pretty minor effect either way. - //PenetrationLimit.Solve(projection.Penetration0, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - //PenetrationLimit.Solve(projection.Penetration1, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); - //var maximumTwistImpulse = projection.PremultipliedFrictionCoefficient * ( - // accumulatedImpulses.Penetration0 * projection.LeverArm0 + - // accumulatedImpulses.Penetration1 * projection.LeverArm1); - //TwistFriction.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); + PenetrationLimit.Solve(projection.Penetration0, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + PenetrationLimit.Solve(projection.Penetration1, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); + var maximumTwistImpulse = projection.PremultipliedFrictionCoefficient * ( + accumulatedImpulses.Penetration0 * projection.LeverArm0 + + accumulatedImpulses.Penetration1 * projection.LeverArm1); + TwistFriction.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityWide velocityA, in BodyVelocityWide velocityB, ref Contact2PrestepData prestep) { - //QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); - //PenetrationLimit.UpdatePenetrationDepth(dt, worldOffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact0.Depth); - //QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); - //PenetrationLimit.UpdatePenetrationDepth(dt, worldOffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact1.Depth); + PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact0.Depth); + PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact1.Depth); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Contact2PrestepData prestep, in Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - QuaternionWide.TransformWithoutOverlap(prestep.LocalNormalB, orientationB, out var normal); - Helpers.BuildOrthonormalBasis(normal, out var x, out var z); - QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); - QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); - FrictionHelpers.ComputeFrictionCenter(worldOffsetA0, worldOffsetA1, prestep.Contact0.Depth, prestep.Contact1.Depth, out var offsetToManifoldCenterA); - var ab = positionB - positionA; - Vector3Wide.Subtract(offsetToManifoldCenterA, ab, out var offsetToManifoldCenterB); + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, out var offsetToManifoldCenterA); + Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); TangentFriction.WarmStart2(x, z, offsetToManifoldCenterA, offsetToManifoldCenterB, inertiaA, inertiaB, accumulatedImpulses.Tangent, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart2(inertiaA, inertiaB, normal, worldOffsetA0, worldOffsetA0 - ab, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart2(inertiaA, inertiaB, normal, worldOffsetA1, worldOffsetA1 - ab, accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); - TwistFriction.WarmStart2(normal, inertiaA, inertiaB, accumulatedImpulses.Twist, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); + TwistFriction.WarmStart2(prestep.Normal, inertiaA, inertiaB, accumulatedImpulses.Twist, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in Contact2PrestepData prestep, ref Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - QuaternionWide.TransformWithoutOverlap(prestep.LocalNormalB, orientationB, out var normal); - Helpers.BuildOrthonormalBasis(normal, out var x, out var z); - QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); - QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); var premultipliedFrictionCoefficient = new Vector(1f / 2f) * prestep.MaterialProperties.FrictionCoefficient; var maximumTangentImpulse = premultipliedFrictionCoefficient * (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1); - FrictionHelpers.ComputeFrictionCenter(worldOffsetA0, worldOffsetA1, prestep.Contact0.Depth, prestep.Contact1.Depth, out var offsetToManifoldCenterA); - var ab = positionB - positionA; - Vector3Wide.Subtract(offsetToManifoldCenterA, ab, out var offsetToManifoldCenterB); + FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, out var offsetToManifoldCenterA); + Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); TangentFriction.Solve2(x, z, offsetToManifoldCenterA, offsetToManifoldCenterB, inertiaA, inertiaB, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); //Note that we solve the penetration constraints after the friction constraints. //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. //It's a pretty minor effect either way. SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); var inverseDtWide = new Vector(inverseDt); - PenetrationLimit.Solve2(inertiaA, inertiaB, normal, worldOffsetA0, worldOffsetA0 - ab, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - PenetrationLimit.Solve2(inertiaA, inertiaB, normal, worldOffsetA0, worldOffsetA1 - ab, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); + PenetrationLimit.Solve2(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + PenetrationLimit.Solve2(inertiaA, inertiaB, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.OffsetA - prestep.OffsetB, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); var maximumTwistImpulse = premultipliedFrictionCoefficient * ( - accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, worldOffsetA0) + - accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, worldOffsetA1)); - TwistFriction.Solve2(normal, inertiaA, inertiaB, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); + accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact0.OffsetA) + + accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact1.OffsetA)); + TwistFriction.Solve2(prestep.Normal, inertiaA, inertiaB, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -1627,9 +1562,9 @@ public void UpdateForNewPose( in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector dt, in Contact2AccumulatedImpulses accumulatedImpulses, ref Contact2PrestepData prestep) { - //prestep.OffsetB = positionB - positionA; - //PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact0.Depth); - //PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact1.Depth); + prestep.OffsetB = positionB - positionA; + PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact0.Depth); + PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact1.Depth); } } @@ -1649,7 +1584,8 @@ public struct Contact3 : IConvexTwoBodyContactConstraintDescription public ConstraintContactData Contact0; public ConstraintContactData Contact1; public ConstraintContactData Contact2; - public Vector3 LocalNormalB; + public Vector3 OffsetB; + public Vector3 Normal; public float FrictionCoefficient; public SpringSettings SpringSettings; public float MaximumRecoveryVelocity; @@ -1658,17 +1594,15 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int { Debug.Assert(batch.TypeId == ConstraintTypeId, "The type batch passed to the description must match the description's expected type."); ref var target = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); - Vector3Wide.WriteFirst(Contact0.LocalOffsetA, ref target.Contact0.LocalOffsetA); + Vector3Wide.WriteFirst(Contact0.OffsetA, ref target.Contact0.OffsetA); GetFirst(ref target.Contact0.Depth) = Contact0.PenetrationDepth; - GetFirst(ref target.Contact0.PlaneOffsetB) = Contact0.PlaneOffsetB; - Vector3Wide.WriteFirst(Contact1.LocalOffsetA, ref target.Contact1.LocalOffsetA); + Vector3Wide.WriteFirst(Contact1.OffsetA, ref target.Contact1.OffsetA); GetFirst(ref target.Contact1.Depth) = Contact1.PenetrationDepth; - GetFirst(ref target.Contact1.PlaneOffsetB) = Contact1.PlaneOffsetB; - Vector3Wide.WriteFirst(Contact2.LocalOffsetA, ref target.Contact2.LocalOffsetA); + Vector3Wide.WriteFirst(Contact2.OffsetA, ref target.Contact2.OffsetA); GetFirst(ref target.Contact2.Depth) = Contact2.PenetrationDepth; - GetFirst(ref target.Contact2.PlaneOffsetB) = Contact2.PlaneOffsetB; - - Vector3Wide.WriteFirst(LocalNormalB, ref target.LocalNormalB); + + Vector3Wide.WriteFirst(OffsetB, ref target.OffsetB); + Vector3Wide.WriteFirst(Normal, ref target.Normal); GetFirst(ref target.MaterialProperties.FrictionCoefficient) = FrictionCoefficient; SpringSettingsWide.WriteFirst(SpringSettings, ref target.MaterialProperties.SpringSettings); GetFirst(ref target.MaterialProperties.MaximumRecoveryVelocity) = MaximumRecoveryVelocity; @@ -1678,26 +1612,26 @@ public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int { Debug.Assert(batch.TypeId == ConstraintTypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); - Vector3Wide.ReadFirst(source.Contact0.LocalOffsetA, out description.Contact0.LocalOffsetA); + Vector3Wide.ReadFirst(source.Contact0.OffsetA, out description.Contact0.OffsetA); description.Contact0.PenetrationDepth = GetFirst(ref source.Contact0.Depth); - description.Contact0.PlaneOffsetB = GetFirst(ref source.Contact0.PlaneOffsetB); - Vector3Wide.ReadFirst(source.Contact1.LocalOffsetA, out description.Contact1.LocalOffsetA); + Vector3Wide.ReadFirst(source.Contact1.OffsetA, out description.Contact1.OffsetA); description.Contact1.PenetrationDepth = GetFirst(ref source.Contact1.Depth); - description.Contact1.PlaneOffsetB = GetFirst(ref source.Contact1.PlaneOffsetB); - Vector3Wide.ReadFirst(source.Contact2.LocalOffsetA, out description.Contact2.LocalOffsetA); + Vector3Wide.ReadFirst(source.Contact2.OffsetA, out description.Contact2.OffsetA); description.Contact2.PenetrationDepth = GetFirst(ref source.Contact2.Depth); - description.Contact2.PlaneOffsetB = GetFirst(ref source.Contact2.PlaneOffsetB); - Vector3Wide.ReadFirst(source.LocalNormalB, out description.LocalNormalB); + + Vector3Wide.ReadFirst(source.OffsetB, out description.OffsetB); + Vector3Wide.ReadFirst(source.Normal, out description.Normal); description.FrictionCoefficient = GetFirst(ref source.MaterialProperties.FrictionCoefficient); SpringSettingsWide.ReadFirst(source.MaterialProperties.SpringSettings, out description.SpringSettings); description.MaximumRecoveryVelocity = GetFirst(ref source.MaterialProperties.MaximumRecoveryVelocity); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void CopyManifoldWideProperties(ref Vector3 localNormalB, ref PairMaterialProperties material) + public void CopyManifoldWideProperties(ref Vector3 offsetB, ref Vector3 normal, ref PairMaterialProperties material) { + OffsetB = offsetB; FrictionCoefficient = material.FrictionCoefficient; - LocalNormalB = localNormalB; + Normal = normal; SpringSettings = material.SpringSettings; MaximumRecoveryVelocity = material.MaximumRecoveryVelocity; } @@ -1726,17 +1660,18 @@ public struct Contact3PrestepData : ITwoBodyConvexContactPrestep 2; public readonly int ContactCount => 3; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3Wide GetLocalNormalB(ref Contact3PrestepData prestep) + public ref Vector3Wide GetNormal(ref Contact3PrestepData prestep) { - return ref prestep.LocalNormalB; + return ref prestep.Normal; } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -1750,6 +1685,12 @@ public ref MaterialPropertiesWide GetMaterialProperties(ref Contact3PrestepData { return ref prestep.MaterialProperties; } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public ref Vector3Wide GetOffsetB(ref Contact3PrestepData prestep) + { + return ref prestep.OffsetB; + } } public unsafe struct Contact3Projection @@ -1777,116 +1718,102 @@ public void Prestep( in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref Contact3PrestepData prestep, out Contact3Projection projection) { //Be careful about the execution order here. It should be aligned with the prestep data layout to ensure prefetching works well. - //projection.InertiaA = inertiaA; - //projection.InertiaB = inertiaB; - //QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); - //QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); - //QuaternionWide.TransformWithoutOverlap(prestep.Contact2.LocalOffsetA, orientationA, out var worldOffsetA2); - //FrictionHelpers.ComputeFrictionCenter(worldOffsetA0, worldOffsetA1, worldOffsetA2, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, out var offsetToManifoldCenterA); - //Vector3Wide.Subtract(offsetToManifoldCenterA, ab, out var offsetToManifoldCenterB); - //projection.PremultipliedFrictionCoefficient = (1f / 3f) * prestep.MaterialProperties.FrictionCoefficient; - //projection.Normal = prestep.Normal; - //Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); - //TangentFriction.Prestep(ref x, ref z, ref offsetToManifoldCenterA, ref offsetToManifoldCenterB, ref projection.InertiaA, ref projection.InertiaB, out projection.Tangent); - //SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); - //PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, worldOffsetA0, worldOffsetA0 - ab, prestep.Normal, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration0); - //PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, worldOffsetA1, worldOffsetA1 - ab, prestep.Normal, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration1); - //PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, worldOffsetA2, worldOffsetA2 - ab, prestep.Normal, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration2); - //Vector3Wide.Distance(worldOffsetA0, offsetToManifoldCenterA, out projection.LeverArm0); - //Vector3Wide.Distance(worldOffsetA1, offsetToManifoldCenterA, out projection.LeverArm1); - //Vector3Wide.Distance(worldOffsetA2, offsetToManifoldCenterA, out projection.LeverArm2); - //TwistFriction.Prestep(ref projection.InertiaA, ref projection.InertiaB, ref prestep.Normal, out projection.Twist); - - projection = default; + projection.InertiaA = inertiaA; + projection.InertiaB = inertiaB; + FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, out var offsetToManifoldCenterA); + Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); + projection.PremultipliedFrictionCoefficient = (1f / 3f) * prestep.MaterialProperties.FrictionCoefficient; + projection.Normal = prestep.Normal; + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + TangentFriction.Prestep(ref x, ref z, ref offsetToManifoldCenterA, ref offsetToManifoldCenterB, ref projection.InertiaA, ref projection.InertiaB, out projection.Tangent); + SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); + Vector3Wide contactOffsetB; + Vector3Wide.Subtract(prestep.Contact0.OffsetA, prestep.OffsetB, out contactOffsetB); + PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, prestep.Contact0.OffsetA, contactOffsetB, prestep.Normal, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration0); + Vector3Wide.Subtract(prestep.Contact1.OffsetA, prestep.OffsetB, out contactOffsetB); + PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, prestep.Contact1.OffsetA, contactOffsetB, prestep.Normal, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration1); + Vector3Wide.Subtract(prestep.Contact2.OffsetA, prestep.OffsetB, out contactOffsetB); + PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, prestep.Contact2.OffsetA, contactOffsetB, prestep.Normal, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration2); + Vector3Wide.Distance(prestep.Contact0.OffsetA, offsetToManifoldCenterA, out projection.LeverArm0); + Vector3Wide.Distance(prestep.Contact1.OffsetA, offsetToManifoldCenterA, out projection.LeverArm1); + Vector3Wide.Distance(prestep.Contact2.OffsetA, offsetToManifoldCenterA, out projection.LeverArm2); + TwistFriction.Prestep(ref projection.InertiaA, ref projection.InertiaB, ref prestep.Normal, out projection.Twist); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart(ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref Contact3Projection projection, ref Contact3AccumulatedImpulses accumulatedImpulses) { - //Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - //TangentFriction.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); - //PenetrationLimit.WarmStart(projection.Penetration0, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - //PenetrationLimit.WarmStart(projection.Penetration1, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); - //PenetrationLimit.WarmStart(projection.Penetration2, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); - //TwistFriction.WarmStart(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); + Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); + TangentFriction.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart(projection.Penetration0, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart(projection.Penetration1, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart(projection.Penetration2, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); + TwistFriction.WarmStart(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve(ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref Contact3Projection projection, ref Contact3AccumulatedImpulses accumulatedImpulses) { - //Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - //var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * - // (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2); - //TangentFriction.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref projection.InertiaB, ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); + Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); + var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * + (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2); + TangentFriction.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref projection.InertiaB, ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); //Note that we solve the penetration constraints after the friction constraints. //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. //It's a pretty minor effect either way. - //PenetrationLimit.Solve(projection.Penetration0, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - //PenetrationLimit.Solve(projection.Penetration1, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); - //PenetrationLimit.Solve(projection.Penetration2, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); - //var maximumTwistImpulse = projection.PremultipliedFrictionCoefficient * ( - // accumulatedImpulses.Penetration0 * projection.LeverArm0 + - // accumulatedImpulses.Penetration1 * projection.LeverArm1 + - // accumulatedImpulses.Penetration2 * projection.LeverArm2); - //TwistFriction.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); + PenetrationLimit.Solve(projection.Penetration0, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + PenetrationLimit.Solve(projection.Penetration1, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); + PenetrationLimit.Solve(projection.Penetration2, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); + var maximumTwistImpulse = projection.PremultipliedFrictionCoefficient * ( + accumulatedImpulses.Penetration0 * projection.LeverArm0 + + accumulatedImpulses.Penetration1 * projection.LeverArm1 + + accumulatedImpulses.Penetration2 * projection.LeverArm2); + TwistFriction.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityWide velocityA, in BodyVelocityWide velocityB, ref Contact3PrestepData prestep) { - //QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); - //PenetrationLimit.UpdatePenetrationDepth(dt, worldOffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact0.Depth); - //QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); - //PenetrationLimit.UpdatePenetrationDepth(dt, worldOffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact1.Depth); - //QuaternionWide.TransformWithoutOverlap(prestep.Contact2.LocalOffsetA, orientationA, out var worldOffsetA2); - //PenetrationLimit.UpdatePenetrationDepth(dt, worldOffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact2.Depth); + PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact0.Depth); + PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact1.Depth); + PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact2.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact2.Depth); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Contact3PrestepData prestep, in Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - QuaternionWide.TransformWithoutOverlap(prestep.LocalNormalB, orientationB, out var normal); - Helpers.BuildOrthonormalBasis(normal, out var x, out var z); - QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); - QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); - QuaternionWide.TransformWithoutOverlap(prestep.Contact2.LocalOffsetA, orientationA, out var worldOffsetA2); - FrictionHelpers.ComputeFrictionCenter(worldOffsetA0, worldOffsetA1, worldOffsetA2, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, out var offsetToManifoldCenterA); - var ab = positionB - positionA; - Vector3Wide.Subtract(offsetToManifoldCenterA, ab, out var offsetToManifoldCenterB); + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, out var offsetToManifoldCenterA); + Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); TangentFriction.WarmStart2(x, z, offsetToManifoldCenterA, offsetToManifoldCenterB, inertiaA, inertiaB, accumulatedImpulses.Tangent, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart2(inertiaA, inertiaB, normal, worldOffsetA0, worldOffsetA0 - ab, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart2(inertiaA, inertiaB, normal, worldOffsetA1, worldOffsetA1 - ab, accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart2(inertiaA, inertiaB, normal, worldOffsetA2, worldOffsetA2 - ab, accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); - TwistFriction.WarmStart2(normal, inertiaA, inertiaB, accumulatedImpulses.Twist, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact2.OffsetA, prestep.Contact2.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); + TwistFriction.WarmStart2(prestep.Normal, inertiaA, inertiaB, accumulatedImpulses.Twist, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in Contact3PrestepData prestep, ref Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - QuaternionWide.TransformWithoutOverlap(prestep.LocalNormalB, orientationB, out var normal); - Helpers.BuildOrthonormalBasis(normal, out var x, out var z); - QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); - QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); - QuaternionWide.TransformWithoutOverlap(prestep.Contact2.LocalOffsetA, orientationA, out var worldOffsetA2); + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); var premultipliedFrictionCoefficient = new Vector(1f / 3f) * prestep.MaterialProperties.FrictionCoefficient; var maximumTangentImpulse = premultipliedFrictionCoefficient * (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2); - FrictionHelpers.ComputeFrictionCenter(worldOffsetA0, worldOffsetA1, worldOffsetA2, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, out var offsetToManifoldCenterA); - var ab = positionB - positionA; - Vector3Wide.Subtract(offsetToManifoldCenterA, ab, out var offsetToManifoldCenterB); + FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, out var offsetToManifoldCenterA); + Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); TangentFriction.Solve2(x, z, offsetToManifoldCenterA, offsetToManifoldCenterB, inertiaA, inertiaB, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); //Note that we solve the penetration constraints after the friction constraints. //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. //It's a pretty minor effect either way. SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); var inverseDtWide = new Vector(inverseDt); - PenetrationLimit.Solve2(inertiaA, inertiaB, normal, worldOffsetA0, worldOffsetA0 - ab, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - PenetrationLimit.Solve2(inertiaA, inertiaB, normal, worldOffsetA0, worldOffsetA1 - ab, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); - PenetrationLimit.Solve2(inertiaA, inertiaB, normal, worldOffsetA0, worldOffsetA2 - ab, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); + PenetrationLimit.Solve2(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + PenetrationLimit.Solve2(inertiaA, inertiaB, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.OffsetA - prestep.OffsetB, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); + PenetrationLimit.Solve2(inertiaA, inertiaB, prestep.Normal, prestep.Contact2.OffsetA, prestep.Contact2.OffsetA - prestep.OffsetB, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); var maximumTwistImpulse = premultipliedFrictionCoefficient * ( - accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, worldOffsetA0) + - accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, worldOffsetA1) + - accumulatedImpulses.Penetration2 * Vector3Wide.Distance(offsetToManifoldCenterA, worldOffsetA2)); - TwistFriction.Solve2(normal, inertiaA, inertiaB, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); + accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact0.OffsetA) + + accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact1.OffsetA) + + accumulatedImpulses.Penetration2 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact2.OffsetA)); + TwistFriction.Solve2(prestep.Normal, inertiaA, inertiaB, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -1895,10 +1822,10 @@ public void UpdateForNewPose( in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector dt, in Contact3AccumulatedImpulses accumulatedImpulses, ref Contact3PrestepData prestep) { - //prestep.OffsetB = positionB - positionA; - //PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact0.Depth); - //PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact1.Depth); - //PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact2.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact2.Depth); + prestep.OffsetB = positionB - positionA; + PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact0.Depth); + PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact1.Depth); + PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact2.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact2.Depth); } } @@ -1919,7 +1846,8 @@ public struct Contact4 : IConvexTwoBodyContactConstraintDescription public ConstraintContactData Contact1; public ConstraintContactData Contact2; public ConstraintContactData Contact3; - public Vector3 LocalNormalB; + public Vector3 OffsetB; + public Vector3 Normal; public float FrictionCoefficient; public SpringSettings SpringSettings; public float MaximumRecoveryVelocity; @@ -1928,20 +1856,17 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int { Debug.Assert(batch.TypeId == ConstraintTypeId, "The type batch passed to the description must match the description's expected type."); ref var target = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); - Vector3Wide.WriteFirst(Contact0.LocalOffsetA, ref target.Contact0.LocalOffsetA); + Vector3Wide.WriteFirst(Contact0.OffsetA, ref target.Contact0.OffsetA); GetFirst(ref target.Contact0.Depth) = Contact0.PenetrationDepth; - GetFirst(ref target.Contact0.PlaneOffsetB) = Contact0.PlaneOffsetB; - Vector3Wide.WriteFirst(Contact1.LocalOffsetA, ref target.Contact1.LocalOffsetA); + Vector3Wide.WriteFirst(Contact1.OffsetA, ref target.Contact1.OffsetA); GetFirst(ref target.Contact1.Depth) = Contact1.PenetrationDepth; - GetFirst(ref target.Contact1.PlaneOffsetB) = Contact1.PlaneOffsetB; - Vector3Wide.WriteFirst(Contact2.LocalOffsetA, ref target.Contact2.LocalOffsetA); + Vector3Wide.WriteFirst(Contact2.OffsetA, ref target.Contact2.OffsetA); GetFirst(ref target.Contact2.Depth) = Contact2.PenetrationDepth; - GetFirst(ref target.Contact2.PlaneOffsetB) = Contact2.PlaneOffsetB; - Vector3Wide.WriteFirst(Contact3.LocalOffsetA, ref target.Contact3.LocalOffsetA); + Vector3Wide.WriteFirst(Contact3.OffsetA, ref target.Contact3.OffsetA); GetFirst(ref target.Contact3.Depth) = Contact3.PenetrationDepth; - GetFirst(ref target.Contact3.PlaneOffsetB) = Contact3.PlaneOffsetB; - - Vector3Wide.WriteFirst(LocalNormalB, ref target.LocalNormalB); + + Vector3Wide.WriteFirst(OffsetB, ref target.OffsetB); + Vector3Wide.WriteFirst(Normal, ref target.Normal); GetFirst(ref target.MaterialProperties.FrictionCoefficient) = FrictionCoefficient; SpringSettingsWide.WriteFirst(SpringSettings, ref target.MaterialProperties.SpringSettings); GetFirst(ref target.MaterialProperties.MaximumRecoveryVelocity) = MaximumRecoveryVelocity; @@ -1951,29 +1876,28 @@ public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int { Debug.Assert(batch.TypeId == ConstraintTypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); - Vector3Wide.ReadFirst(source.Contact0.LocalOffsetA, out description.Contact0.LocalOffsetA); + Vector3Wide.ReadFirst(source.Contact0.OffsetA, out description.Contact0.OffsetA); description.Contact0.PenetrationDepth = GetFirst(ref source.Contact0.Depth); - description.Contact0.PlaneOffsetB = GetFirst(ref source.Contact0.PlaneOffsetB); - Vector3Wide.ReadFirst(source.Contact1.LocalOffsetA, out description.Contact1.LocalOffsetA); + Vector3Wide.ReadFirst(source.Contact1.OffsetA, out description.Contact1.OffsetA); description.Contact1.PenetrationDepth = GetFirst(ref source.Contact1.Depth); - description.Contact1.PlaneOffsetB = GetFirst(ref source.Contact1.PlaneOffsetB); - Vector3Wide.ReadFirst(source.Contact2.LocalOffsetA, out description.Contact2.LocalOffsetA); + Vector3Wide.ReadFirst(source.Contact2.OffsetA, out description.Contact2.OffsetA); description.Contact2.PenetrationDepth = GetFirst(ref source.Contact2.Depth); - description.Contact2.PlaneOffsetB = GetFirst(ref source.Contact2.PlaneOffsetB); - Vector3Wide.ReadFirst(source.Contact3.LocalOffsetA, out description.Contact3.LocalOffsetA); + Vector3Wide.ReadFirst(source.Contact3.OffsetA, out description.Contact3.OffsetA); description.Contact3.PenetrationDepth = GetFirst(ref source.Contact3.Depth); - description.Contact3.PlaneOffsetB = GetFirst(ref source.Contact3.PlaneOffsetB); - Vector3Wide.ReadFirst(source.LocalNormalB, out description.LocalNormalB); + + Vector3Wide.ReadFirst(source.OffsetB, out description.OffsetB); + Vector3Wide.ReadFirst(source.Normal, out description.Normal); description.FrictionCoefficient = GetFirst(ref source.MaterialProperties.FrictionCoefficient); SpringSettingsWide.ReadFirst(source.MaterialProperties.SpringSettings, out description.SpringSettings); description.MaximumRecoveryVelocity = GetFirst(ref source.MaterialProperties.MaximumRecoveryVelocity); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void CopyManifoldWideProperties(ref Vector3 localNormalB, ref PairMaterialProperties material) + public void CopyManifoldWideProperties(ref Vector3 offsetB, ref Vector3 normal, ref PairMaterialProperties material) { + OffsetB = offsetB; FrictionCoefficient = material.FrictionCoefficient; - LocalNormalB = localNormalB; + Normal = normal; SpringSettings = material.SpringSettings; MaximumRecoveryVelocity = material.MaximumRecoveryVelocity; } @@ -2003,17 +1927,18 @@ public struct Contact4PrestepData : ITwoBodyConvexContactPrestep 2; public readonly int ContactCount => 4; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3Wide GetLocalNormalB(ref Contact4PrestepData prestep) + public ref Vector3Wide GetNormal(ref Contact4PrestepData prestep) { - return ref prestep.LocalNormalB; + return ref prestep.Normal; } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -2027,6 +1952,12 @@ public ref MaterialPropertiesWide GetMaterialProperties(ref Contact4PrestepData { return ref prestep.MaterialProperties; } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public ref Vector3Wide GetOffsetB(ref Contact4PrestepData prestep) + { + return ref prestep.OffsetB; + } } public unsafe struct Contact4Projection @@ -2056,129 +1987,112 @@ public void Prestep( in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref Contact4PrestepData prestep, out Contact4Projection projection) { //Be careful about the execution order here. It should be aligned with the prestep data layout to ensure prefetching works well. - //projection.InertiaA = inertiaA; - //projection.InertiaB = inertiaB; - //QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); - //QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); - //QuaternionWide.TransformWithoutOverlap(prestep.Contact2.LocalOffsetA, orientationA, out var worldOffsetA2); - //QuaternionWide.TransformWithoutOverlap(prestep.Contact3.LocalOffsetA, orientationA, out var worldOffsetA3); - //FrictionHelpers.ComputeFrictionCenter(worldOffsetA0, worldOffsetA1, worldOffsetA2, worldOffsetA3, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, prestep.Contact3.Depth, out var offsetToManifoldCenterA); - //Vector3Wide.Subtract(offsetToManifoldCenterA, ab, out var offsetToManifoldCenterB); - //projection.PremultipliedFrictionCoefficient = (1f / 4f) * prestep.MaterialProperties.FrictionCoefficient; - //projection.Normal = prestep.Normal; - //Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); - //TangentFriction.Prestep(ref x, ref z, ref offsetToManifoldCenterA, ref offsetToManifoldCenterB, ref projection.InertiaA, ref projection.InertiaB, out projection.Tangent); - //SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); - //PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, worldOffsetA0, worldOffsetA0 - ab, prestep.Normal, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration0); - //PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, worldOffsetA1, worldOffsetA1 - ab, prestep.Normal, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration1); - //PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, worldOffsetA2, worldOffsetA2 - ab, prestep.Normal, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration2); - //PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, worldOffsetA3, worldOffsetA3 - ab, prestep.Normal, prestep.Contact3.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration3); - //Vector3Wide.Distance(worldOffsetA0, offsetToManifoldCenterA, out projection.LeverArm0); - //Vector3Wide.Distance(worldOffsetA1, offsetToManifoldCenterA, out projection.LeverArm1); - //Vector3Wide.Distance(worldOffsetA2, offsetToManifoldCenterA, out projection.LeverArm2); - //Vector3Wide.Distance(worldOffsetA3, offsetToManifoldCenterA, out projection.LeverArm3); - //TwistFriction.Prestep(ref projection.InertiaA, ref projection.InertiaB, ref prestep.Normal, out projection.Twist); - - projection = default; + projection.InertiaA = inertiaA; + projection.InertiaB = inertiaB; + FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact3.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, prestep.Contact3.Depth, out var offsetToManifoldCenterA); + Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); + projection.PremultipliedFrictionCoefficient = (1f / 4f) * prestep.MaterialProperties.FrictionCoefficient; + projection.Normal = prestep.Normal; + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + TangentFriction.Prestep(ref x, ref z, ref offsetToManifoldCenterA, ref offsetToManifoldCenterB, ref projection.InertiaA, ref projection.InertiaB, out projection.Tangent); + SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); + Vector3Wide contactOffsetB; + Vector3Wide.Subtract(prestep.Contact0.OffsetA, prestep.OffsetB, out contactOffsetB); + PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, prestep.Contact0.OffsetA, contactOffsetB, prestep.Normal, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration0); + Vector3Wide.Subtract(prestep.Contact1.OffsetA, prestep.OffsetB, out contactOffsetB); + PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, prestep.Contact1.OffsetA, contactOffsetB, prestep.Normal, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration1); + Vector3Wide.Subtract(prestep.Contact2.OffsetA, prestep.OffsetB, out contactOffsetB); + PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, prestep.Contact2.OffsetA, contactOffsetB, prestep.Normal, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration2); + Vector3Wide.Subtract(prestep.Contact3.OffsetA, prestep.OffsetB, out contactOffsetB); + PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, prestep.Contact3.OffsetA, contactOffsetB, prestep.Normal, prestep.Contact3.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration3); + Vector3Wide.Distance(prestep.Contact0.OffsetA, offsetToManifoldCenterA, out projection.LeverArm0); + Vector3Wide.Distance(prestep.Contact1.OffsetA, offsetToManifoldCenterA, out projection.LeverArm1); + Vector3Wide.Distance(prestep.Contact2.OffsetA, offsetToManifoldCenterA, out projection.LeverArm2); + Vector3Wide.Distance(prestep.Contact3.OffsetA, offsetToManifoldCenterA, out projection.LeverArm3); + TwistFriction.Prestep(ref projection.InertiaA, ref projection.InertiaB, ref prestep.Normal, out projection.Twist); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart(ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref Contact4Projection projection, ref Contact4AccumulatedImpulses accumulatedImpulses) { - //Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - //TangentFriction.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); - //PenetrationLimit.WarmStart(projection.Penetration0, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - //PenetrationLimit.WarmStart(projection.Penetration1, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); - //PenetrationLimit.WarmStart(projection.Penetration2, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); - //PenetrationLimit.WarmStart(projection.Penetration3, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration3, ref wsvA, ref wsvB); - //TwistFriction.WarmStart(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); + Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); + TangentFriction.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart(projection.Penetration0, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart(projection.Penetration1, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart(projection.Penetration2, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart(projection.Penetration3, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration3, ref wsvA, ref wsvB); + TwistFriction.WarmStart(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve(ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref Contact4Projection projection, ref Contact4AccumulatedImpulses accumulatedImpulses) { - //Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - //var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * - // (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2 + accumulatedImpulses.Penetration3); - //TangentFriction.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref projection.InertiaB, ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); + Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); + var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * + (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2 + accumulatedImpulses.Penetration3); + TangentFriction.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref projection.InertiaB, ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); //Note that we solve the penetration constraints after the friction constraints. //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. //It's a pretty minor effect either way. - //PenetrationLimit.Solve(projection.Penetration0, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - //PenetrationLimit.Solve(projection.Penetration1, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); - //PenetrationLimit.Solve(projection.Penetration2, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); - //PenetrationLimit.Solve(projection.Penetration3, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration3, ref wsvA, ref wsvB); - //var maximumTwistImpulse = projection.PremultipliedFrictionCoefficient * ( - // accumulatedImpulses.Penetration0 * projection.LeverArm0 + - // accumulatedImpulses.Penetration1 * projection.LeverArm1 + - // accumulatedImpulses.Penetration2 * projection.LeverArm2 + - // accumulatedImpulses.Penetration3 * projection.LeverArm3); - //TwistFriction.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); + PenetrationLimit.Solve(projection.Penetration0, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + PenetrationLimit.Solve(projection.Penetration1, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); + PenetrationLimit.Solve(projection.Penetration2, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); + PenetrationLimit.Solve(projection.Penetration3, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration3, ref wsvA, ref wsvB); + var maximumTwistImpulse = projection.PremultipliedFrictionCoefficient * ( + accumulatedImpulses.Penetration0 * projection.LeverArm0 + + accumulatedImpulses.Penetration1 * projection.LeverArm1 + + accumulatedImpulses.Penetration2 * projection.LeverArm2 + + accumulatedImpulses.Penetration3 * projection.LeverArm3); + TwistFriction.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityWide velocityA, in BodyVelocityWide velocityB, ref Contact4PrestepData prestep) { - //QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); - //PenetrationLimit.UpdatePenetrationDepth(dt, worldOffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact0.Depth); - //QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); - //PenetrationLimit.UpdatePenetrationDepth(dt, worldOffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact1.Depth); - //QuaternionWide.TransformWithoutOverlap(prestep.Contact2.LocalOffsetA, orientationA, out var worldOffsetA2); - //PenetrationLimit.UpdatePenetrationDepth(dt, worldOffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact2.Depth); - //QuaternionWide.TransformWithoutOverlap(prestep.Contact3.LocalOffsetA, orientationA, out var worldOffsetA3); - //PenetrationLimit.UpdatePenetrationDepth(dt, worldOffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact3.Depth); + PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact0.Depth); + PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact1.Depth); + PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact2.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact2.Depth); + PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact3.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact3.Depth); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Contact4PrestepData prestep, in Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - QuaternionWide.TransformWithoutOverlap(prestep.LocalNormalB, orientationB, out var normal); - Helpers.BuildOrthonormalBasis(normal, out var x, out var z); - QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); - QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); - QuaternionWide.TransformWithoutOverlap(prestep.Contact2.LocalOffsetA, orientationA, out var worldOffsetA2); - QuaternionWide.TransformWithoutOverlap(prestep.Contact3.LocalOffsetA, orientationA, out var worldOffsetA3); - FrictionHelpers.ComputeFrictionCenter(worldOffsetA0, worldOffsetA1, worldOffsetA2, worldOffsetA3, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, prestep.Contact3.Depth, out var offsetToManifoldCenterA); - var ab = positionB - positionA; - Vector3Wide.Subtract(offsetToManifoldCenterA, ab, out var offsetToManifoldCenterB); + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact3.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, prestep.Contact3.Depth, out var offsetToManifoldCenterA); + Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); TangentFriction.WarmStart2(x, z, offsetToManifoldCenterA, offsetToManifoldCenterB, inertiaA, inertiaB, accumulatedImpulses.Tangent, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart2(inertiaA, inertiaB, normal, worldOffsetA0, worldOffsetA0 - ab, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart2(inertiaA, inertiaB, normal, worldOffsetA1, worldOffsetA1 - ab, accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart2(inertiaA, inertiaB, normal, worldOffsetA2, worldOffsetA2 - ab, accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart2(inertiaA, inertiaB, normal, worldOffsetA3, worldOffsetA3 - ab, accumulatedImpulses.Penetration3, ref wsvA, ref wsvB); - TwistFriction.WarmStart2(normal, inertiaA, inertiaB, accumulatedImpulses.Twist, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact2.OffsetA, prestep.Contact2.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact3.OffsetA, prestep.Contact3.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration3, ref wsvA, ref wsvB); + TwistFriction.WarmStart2(prestep.Normal, inertiaA, inertiaB, accumulatedImpulses.Twist, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in Contact4PrestepData prestep, ref Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - QuaternionWide.TransformWithoutOverlap(prestep.LocalNormalB, orientationB, out var normal); - Helpers.BuildOrthonormalBasis(normal, out var x, out var z); - QuaternionWide.TransformWithoutOverlap(prestep.Contact0.LocalOffsetA, orientationA, out var worldOffsetA0); - QuaternionWide.TransformWithoutOverlap(prestep.Contact1.LocalOffsetA, orientationA, out var worldOffsetA1); - QuaternionWide.TransformWithoutOverlap(prestep.Contact2.LocalOffsetA, orientationA, out var worldOffsetA2); - QuaternionWide.TransformWithoutOverlap(prestep.Contact3.LocalOffsetA, orientationA, out var worldOffsetA3); + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); var premultipliedFrictionCoefficient = new Vector(1f / 4f) * prestep.MaterialProperties.FrictionCoefficient; var maximumTangentImpulse = premultipliedFrictionCoefficient * (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2 + accumulatedImpulses.Penetration3); - FrictionHelpers.ComputeFrictionCenter(worldOffsetA0, worldOffsetA1, worldOffsetA2, worldOffsetA3, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, prestep.Contact3.Depth, out var offsetToManifoldCenterA); - var ab = positionB - positionA; - Vector3Wide.Subtract(offsetToManifoldCenterA, ab, out var offsetToManifoldCenterB); + FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact3.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, prestep.Contact3.Depth, out var offsetToManifoldCenterA); + Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); TangentFriction.Solve2(x, z, offsetToManifoldCenterA, offsetToManifoldCenterB, inertiaA, inertiaB, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); //Note that we solve the penetration constraints after the friction constraints. //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. //It's a pretty minor effect either way. SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); var inverseDtWide = new Vector(inverseDt); - PenetrationLimit.Solve2(inertiaA, inertiaB, normal, worldOffsetA0, worldOffsetA0 - ab, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - PenetrationLimit.Solve2(inertiaA, inertiaB, normal, worldOffsetA0, worldOffsetA1 - ab, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); - PenetrationLimit.Solve2(inertiaA, inertiaB, normal, worldOffsetA0, worldOffsetA2 - ab, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); - PenetrationLimit.Solve2(inertiaA, inertiaB, normal, worldOffsetA0, worldOffsetA3 - ab, prestep.Contact3.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration3, ref wsvA, ref wsvB); + PenetrationLimit.Solve2(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + PenetrationLimit.Solve2(inertiaA, inertiaB, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.OffsetA - prestep.OffsetB, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); + PenetrationLimit.Solve2(inertiaA, inertiaB, prestep.Normal, prestep.Contact2.OffsetA, prestep.Contact2.OffsetA - prestep.OffsetB, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); + PenetrationLimit.Solve2(inertiaA, inertiaB, prestep.Normal, prestep.Contact3.OffsetA, prestep.Contact3.OffsetA - prestep.OffsetB, prestep.Contact3.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration3, ref wsvA, ref wsvB); var maximumTwistImpulse = premultipliedFrictionCoefficient * ( - accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, worldOffsetA0) + - accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, worldOffsetA1) + - accumulatedImpulses.Penetration2 * Vector3Wide.Distance(offsetToManifoldCenterA, worldOffsetA2) + - accumulatedImpulses.Penetration3 * Vector3Wide.Distance(offsetToManifoldCenterA, worldOffsetA3)); - TwistFriction.Solve2(normal, inertiaA, inertiaB, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); + accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact0.OffsetA) + + accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact1.OffsetA) + + accumulatedImpulses.Penetration2 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact2.OffsetA) + + accumulatedImpulses.Penetration3 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact3.OffsetA)); + TwistFriction.Solve2(prestep.Normal, inertiaA, inertiaB, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -2187,11 +2101,11 @@ public void UpdateForNewPose( in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector dt, in Contact4AccumulatedImpulses accumulatedImpulses, ref Contact4PrestepData prestep) { - //prestep.OffsetB = positionB - positionA; - //PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact0.Depth); - //PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact1.Depth); - //PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact2.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact2.Depth); - //PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact3.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact3.Depth); + prestep.OffsetB = positionB - positionA; + PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact0.Depth); + PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact1.Depth); + PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact2.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact2.Depth); + PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact3.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact3.Depth); } } diff --git a/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt b/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt index db9800956..c6234671e 100644 --- a/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt +++ b/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt @@ -100,7 +100,10 @@ for (int i = 0; i < contactCount; ++i) {#> public ConstraintContactData Contact<#=i#>; <#}#> - public Vector3 LocalNormalB; +<#if (bodyCount == 2) {#> + public Vector3 OffsetB; +<#}#> + public Vector3 Normal; public float FrictionCoefficient; public SpringSettings SpringSettings; public float MaximumRecoveryVelocity; @@ -110,11 +113,13 @@ for (int i = 0; i < contactCount; ++i) Debug.Assert(batch.TypeId == ConstraintTypeId, "The type batch passed to the description must match the description's expected type."); ref var target = ref GetOffsetInstance(ref Buffer<#=suffix#>PrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex); <# for (int i = 0; i < contactCount; ++i) {#> - Vector3Wide.WriteFirst(Contact<#=i#>.LocalOffsetA, ref target.Contact<#=i#>.LocalOffsetA); + Vector3Wide.WriteFirst(Contact<#=i#>.OffsetA, ref target.Contact<#=i#>.OffsetA); GetFirst(ref target.Contact<#=i#>.Depth) = Contact<#=i#>.PenetrationDepth; - GetFirst(ref target.Contact<#=i#>.PlaneOffsetB) = Contact<#=i#>.PlaneOffsetB; -<#}#> - Vector3Wide.WriteFirst(LocalNormalB, ref target.LocalNormalB); +<#}#> +<#if (bodyCount == 2) {#> + Vector3Wide.WriteFirst(OffsetB, ref target.OffsetB); +<#}#> + Vector3Wide.WriteFirst(Normal, ref target.Normal); GetFirst(ref target.MaterialProperties.FrictionCoefficient) = FrictionCoefficient; SpringSettingsWide.WriteFirst(SpringSettings, ref target.MaterialProperties.SpringSettings); GetFirst(ref target.MaterialProperties.MaximumRecoveryVelocity) = MaximumRecoveryVelocity; @@ -125,21 +130,26 @@ for (int i = 0; i < contactCount; ++i) Debug.Assert(batch.TypeId == ConstraintTypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer<#=suffix#>PrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex); <#for (int i = 0; i < contactCount; ++i) {#> - Vector3Wide.ReadFirst(source.Contact<#=i#>.LocalOffsetA, out description.Contact<#=i#>.LocalOffsetA); + Vector3Wide.ReadFirst(source.Contact<#=i#>.OffsetA, out description.Contact<#=i#>.OffsetA); description.Contact<#=i#>.PenetrationDepth = GetFirst(ref source.Contact<#=i#>.Depth); - description.Contact<#=i#>.PlaneOffsetB = GetFirst(ref source.Contact<#=i#>.PlaneOffsetB); <#}#> - Vector3Wide.ReadFirst(source.LocalNormalB, out description.LocalNormalB); +<#if (bodyCount == 2) {#> + Vector3Wide.ReadFirst(source.OffsetB, out description.OffsetB); +<#}#> + Vector3Wide.ReadFirst(source.Normal, out description.Normal); description.FrictionCoefficient = GetFirst(ref source.MaterialProperties.FrictionCoefficient); SpringSettingsWide.ReadFirst(source.MaterialProperties.SpringSettings, out description.SpringSettings); description.MaximumRecoveryVelocity = GetFirst(ref source.MaterialProperties.MaximumRecoveryVelocity); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void CopyManifoldWideProperties(ref Vector3 localNormalB, ref PairMaterialProperties material) + public void CopyManifoldWideProperties(<#if (bodyCount == 2) {#>ref Vector3 offsetB, <#}#>ref Vector3 normal, ref PairMaterialProperties material) { +<#if (bodyCount == 2) {#> + OffsetB = offsetB; +<#}#> FrictionCoefficient = material.FrictionCoefficient; - LocalNormalB = localNormalB; + Normal = normal; SpringSettings = material.SpringSettings; MaximumRecoveryVelocity = material.MaximumRecoveryVelocity; } @@ -167,18 +177,21 @@ for (int i = 0; i < contactCount; ++i) //Note that this layout is defined by the execution order in the prestep. The function accesses it sequentially to ensure the prefetcher can do its job. <#for (int i = 0; i < contactCount; ++i) {#> public ConvexContactWide Contact<#=i#>; +<#}#> +<#if (bodyCount == 2) {#> + public Vector3Wide OffsetB; <#}#> //In a convex manifold, all contacts share the same normal and tangents. - public Vector3Wide LocalNormalB; + public Vector3Wide Normal; public MaterialPropertiesWide MaterialProperties; public readonly int BodyCount => <#=bodyCount#>; public readonly int ContactCount => <#=contactCount#>; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3Wide GetLocalNormalB(ref Contact<#= contactCount #><#=suffix#>PrestepData prestep) + public ref Vector3Wide GetNormal(ref Contact<#= contactCount #><#=suffix#>PrestepData prestep) { - return ref prestep.LocalNormalB; + return ref prestep.Normal; } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -192,6 +205,14 @@ for (int i = 0; i < contactCount; ++i) { return ref prestep.MaterialProperties; } + +<#if (bodyCount == 2){#> + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public ref Vector3Wide GetOffsetB(ref Contact<#= contactCount #><#=suffix#>PrestepData prestep) + { + return ref prestep.OffsetB; + } +<#}#> } public unsafe struct Contact<#= contactCount #><#=suffix#>Projection @@ -221,147 +242,127 @@ for (int i = 0; i < contactCount; ++i) <#if(bodyCount == 1) {#>in Vector3Wide positionA,<#}#> in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(bodyCount == 2) {#>in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>float dt, float inverseDt, ref Contact<#=contactCount#><#=suffix#>PrestepData prestep, out Contact<#=contactCount#><#=suffix#>Projection projection) { //Be careful about the execution order here. It should be aligned with the prestep data layout to ensure prefetching works well. - //projection.InertiaA = inertiaA; + projection.InertiaA = inertiaA; <#if (bodyCount == 2) {#> - //projection.InertiaB = inertiaB; -<#}#> -<#for (int i = 0; i < contactCount; ++i) {#> - //QuaternionWide.TransformWithoutOverlap(prestep.Contact<#=i#>.LocalOffsetA, orientationA, out var worldOffsetA<#=i#>); + projection.InertiaB = inertiaB; <#}#> <#if (contactCount > 1) {#> - //FrictionHelpers.ComputeFrictionCenter(<#for (int i = 0; i < contactCount; ++i) {#>worldOffsetA<#=i#>, <#}#><#for (int i = 0; i < contactCount; ++i) {#>prestep.Contact<#=i#>.Depth, <#}#>out var offsetToManifoldCenterA); + FrictionHelpers.ComputeFrictionCenter(<#for (int i = 0; i < contactCount; ++i) {#>prestep.Contact<#=i#>.OffsetA, <#}#><#for (int i = 0; i < contactCount; ++i) {#>prestep.Contact<#=i#>.Depth, <#}#>out var offsetToManifoldCenterA); <#if (bodyCount == 2) {#> - //Vector3Wide.Subtract(offsetToManifoldCenterA, ab, out var offsetToManifoldCenterB); + Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); <#}#> <#} else if(bodyCount == 2) {#> - //Vector3Wide.Subtract(worldOffsetA0, ab, out var offsetToManifoldCenterB); -<#}#> - //projection.PremultipliedFrictionCoefficient = <#if (contactCount > 1) {#>(1f / <#=contactCount#>f) * <#}#>prestep.MaterialProperties.FrictionCoefficient; - //projection.Normal = prestep.Normal; - //Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); -<#var offsetName = contactCount == 1 ? "worldOffsetA0" : "offsetToManifoldCenterA";#> - //TangentFriction<#=suffix#>.Prestep(ref x, ref z, ref <#=offsetName#>, <#if (bodyCount == 2) {#>ref offsetToManifoldCenterB, <#}#>ref projection.InertiaA, <#if (bodyCount == 2) {#>ref projection.InertiaB, <#}#>out projection.Tangent); - //SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); + Vector3Wide.Subtract(prestep.Contact0.OffsetA, prestep.OffsetB, out var offsetToManifoldCenterB); +<#}#> + projection.PremultipliedFrictionCoefficient = <#if (contactCount > 1) {#>(1f / <#=contactCount#>f) * <#}#>prestep.MaterialProperties.FrictionCoefficient; + projection.Normal = prestep.Normal; + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); +<#var offsetName = contactCount == 1 ? "prestep.Contact0.OffsetA" : "offsetToManifoldCenterA";#> + TangentFriction<#=suffix#>.Prestep(ref x, ref z, ref <#=offsetName#>, <#if (bodyCount == 2) {#>ref offsetToManifoldCenterB, <#}#>ref projection.InertiaA, <#if (bodyCount == 2) {#>ref projection.InertiaB, <#}#>out projection.Tangent); + SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); +<#if (bodyCount == 2) {#> + Vector3Wide contactOffsetB; +<#}#> <#for (int i = 0; i < contactCount; ++i) {#> - //PenetrationLimit<#=suffix#>.Prestep(projection.InertiaA, <#if (bodyCount == 2) {#>projection.InertiaB, <#}#>worldOffsetA<#=i#>, <#if (bodyCount == 2) {#>worldOffsetA<#=i#> - ab, <#}#>prestep.Normal, prestep.Contact<#=i#>.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration<#=i#>); +<#if (bodyCount == 2) {#> + Vector3Wide.Subtract(prestep.Contact<#=i#>.OffsetA, prestep.OffsetB, out contactOffsetB); +<#}#> + PenetrationLimit<#=suffix#>.Prestep(projection.InertiaA, <#if (bodyCount == 2) {#>projection.InertiaB, <#}#>prestep.Contact<#=i#>.OffsetA, <#if (bodyCount == 2) {#>contactOffsetB, <#}#>prestep.Normal, prestep.Contact<#=i#>.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration<#=i#>); <#}#> <#if (contactCount == 1) {#> //If there's only one contact, then the contact patch as determined by contact distance would be zero. //That can cause some subtle behavioral issues sometimes, so we approximate lever arm with the contact depth, assuming that the contact surface area will increase as the depth increases. - //projection.LeverArm0 = Vector.Max(Vector.Zero, prestep.Contact0.Depth); + projection.LeverArm0 = Vector.Max(Vector.Zero, prestep.Contact0.Depth); <#} else {#> <#for (int i = 0; i < contactCount; ++i) {#> - //Vector3Wide.Distance(worldOffsetA<#=i#>, <#=offsetName#>, out projection.LeverArm<#=i#>); + Vector3Wide.Distance(prestep.Contact<#=i#>.OffsetA, <#=offsetName#>, out projection.LeverArm<#=i#>); <#}}#> - //TwistFriction<#=suffix#>.Prestep(ref projection.InertiaA, <#if (bodyCount == 2) {#>ref projection.InertiaB, <#}#>ref prestep.Normal, out projection.Twist); - - projection = default; + TwistFriction<#=suffix#>.Prestep(ref projection.InertiaA, <#if (bodyCount == 2) {#>ref projection.InertiaB, <#}#>ref prestep.Normal, out projection.Twist); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart(ref BodyVelocityWide wsvA, <#if(bodyCount == 2) {#>ref BodyVelocityWide wsvB, <#}#>ref Contact<#=contactCount#><#=suffix#>Projection projection, ref Contact<#=contactCount#>AccumulatedImpulses accumulatedImpulses) { - //Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - //TangentFriction<#=suffix#>.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, <#if (bodyCount == 2) {#>ref projection.InertiaB, <#}#>ref accumulatedImpulses.Tangent, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); + Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); + TangentFriction<#=suffix#>.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, <#if (bodyCount == 2) {#>ref projection.InertiaB, <#}#>ref accumulatedImpulses.Tangent, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); <#for (int i = 0; i < contactCount; ++i) {#> - //PenetrationLimit<#=suffix#>.WarmStart(projection.Penetration<#=i#>, projection.InertiaA, <#if (bodyCount == 2) {#>projection.InertiaB, <#}#>projection.Normal, accumulatedImpulses.Penetration<#=i#>, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); + PenetrationLimit<#=suffix#>.WarmStart(projection.Penetration<#=i#>, projection.InertiaA, <#if (bodyCount == 2) {#>projection.InertiaB, <#}#>projection.Normal, accumulatedImpulses.Penetration<#=i#>, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); <#}#> - //TwistFriction<#=suffix#>.WarmStart(ref projection.Normal, ref projection.InertiaA, <#if (bodyCount == 2) {#>ref projection.InertiaB, <#}#>ref accumulatedImpulses.Twist, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); + TwistFriction<#=suffix#>.WarmStart(ref projection.Normal, ref projection.InertiaA, <#if (bodyCount == 2) {#>ref projection.InertiaB, <#}#>ref accumulatedImpulses.Twist, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve(ref BodyVelocityWide wsvA, <#if(bodyCount == 2) {#>ref BodyVelocityWide wsvB, <#}#>ref Contact<#=contactCount#><#=suffix#>Projection projection, ref Contact<#=contactCount#>AccumulatedImpulses accumulatedImpulses) { - //Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - //var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * - // (<#for (int i = 0; i < contactCount; ++i) {#>accumulatedImpulses.Penetration<#=i#><#if(i < contactCount - 1){#> + <#}}#>); - //TangentFriction<#=suffix#>.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, <#if (bodyCount == 2) {#>ref projection.InertiaB, <#}#>ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); + Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); + var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * + (<#for (int i = 0; i < contactCount; ++i) {#>accumulatedImpulses.Penetration<#=i#><#if(i < contactCount - 1){#> + <#}}#>); + TangentFriction<#=suffix#>.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, <#if (bodyCount == 2) {#>ref projection.InertiaB, <#}#>ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); //Note that we solve the penetration constraints after the friction constraints. //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. //It's a pretty minor effect either way. <#for (int i = 0; i < contactCount; ++i) {#> - //PenetrationLimit<#=suffix#>.Solve(projection.Penetration<#=i#>, projection.InertiaA, <#if (bodyCount == 2) {#>projection.InertiaB, <#}#>projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration<#=i#>, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); + PenetrationLimit<#=suffix#>.Solve(projection.Penetration<#=i#>, projection.InertiaA, <#if (bodyCount == 2) {#>projection.InertiaB, <#}#>projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration<#=i#>, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); <#}#> - //var maximumTwistImpulse = projection.PremultipliedFrictionCoefficient * ( + var maximumTwistImpulse = projection.PremultipliedFrictionCoefficient * ( <#for (int i = 0; i < contactCount; ++i) {#> - // accumulatedImpulses.Penetration<#=i#> * projection.LeverArm<#=i#><#if (i < contactCount - 1){#> +<#} else{#>);<#}#> + accumulatedImpulses.Penetration<#=i#> * projection.LeverArm<#=i#><#if (i < contactCount - 1){#> +<#} else{#>);<#}#> <#}#> - //TwistFriction<#=suffix#>.Solve(ref projection.Normal, ref projection.InertiaA, <#if (bodyCount == 2) {#>ref projection.InertiaB, <#}#>ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); + TwistFriction<#=suffix#>.Solve(ref projection.Normal, ref projection.InertiaA, <#if (bodyCount == 2) {#>ref projection.InertiaB, <#}#>ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityWide velocityA,<#if(bodyCount == 2) {#> in BodyVelocityWide velocityB,<#}#> ref Contact<#=contactCount#><#=suffix#>PrestepData prestep) { <#for (int i = 0; i < contactCount; ++i) {#> - //QuaternionWide.TransformWithoutOverlap(prestep.Contact<#=i#>.LocalOffsetA, orientationA, out var worldOffsetA<#=i#>); - //PenetrationLimit<#=suffix#>.UpdatePenetrationDepth(dt, worldOffsetA, <#if(bodyCount == 2) {#>prestep.OffsetB, <#}#>prestep.Normal, velocityA, <#if (bodyCount == 2) {#>velocityB, <#}#>ref prestep.Contact<#=i#>.Depth); + PenetrationLimit<#=suffix#>.UpdatePenetrationDepth(dt, prestep.Contact<#=i#>.OffsetA, <#if(bodyCount == 2) {#>prestep.OffsetB, <#}#>prestep.Normal, velocityA, <#if (bodyCount == 2) {#>velocityB, <#}#>ref prestep.Contact<#=i#>.Depth); <#}#> } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(bodyCount == 2) {#>in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>in Contact<#=contactCount#><#=suffix#>PrestepData prestep, in Contact<#=contactCount#>AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA<#if(bodyCount == 2) {#>, ref BodyVelocityWide wsvB<#}#>) { -<#if (bodyCount == 1) {#> - Helpers.BuildOrthonormalBasis(prestep.LocalNormalB, out var x, out var z); -<#} else {#> - QuaternionWide.TransformWithoutOverlap(prestep.LocalNormalB, orientationB, out var normal); - Helpers.BuildOrthonormalBasis(normal, out var x, out var z); -<#}#> -<#for (int i = 0; i < contactCount; ++i) {#> - QuaternionWide.TransformWithoutOverlap(prestep.Contact<#=i#>.LocalOffsetA, orientationA, out var worldOffsetA<#=i#>); -<#}#> + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); <#if (contactCount > 1) {#> - FrictionHelpers.ComputeFrictionCenter(<#for (int i = 0; i < contactCount; ++i) {#>worldOffsetA<#=i#>, <#}#><#for (int i = 0; i < contactCount; ++i) {#>prestep.Contact<#=i#>.Depth, <#}#>out var offsetToManifoldCenterA); + FrictionHelpers.ComputeFrictionCenter(<#for (int i = 0; i < contactCount; ++i) {#>prestep.Contact<#=i#>.OffsetA, <#}#><#for (int i = 0; i < contactCount; ++i) {#>prestep.Contact<#=i#>.Depth, <#}#>out var offsetToManifoldCenterA); <#if (bodyCount == 2) {#> - var ab = positionB - positionA; - Vector3Wide.Subtract(offsetToManifoldCenterA, ab, out var offsetToManifoldCenterB); + Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); <#}#> <#} else if(bodyCount == 2) {#> - var ab = positionB - positionA; - Vector3Wide.Subtract(worldOffsetA0, ab, out var offsetToManifoldCenterB); + Vector3Wide.Subtract(prestep.Contact0.OffsetA, prestep.OffsetB, out var offsetToManifoldCenterB); <#}#> - TangentFriction<#=suffix#>.WarmStart2(x, z, <#if(contactCount > 1) {#>offsetToManifoldCenterA<#} else {#>worldOffsetA0<#}#>, <#if(bodyCount == 2) {#>offsetToManifoldCenterB, <#}#>inertiaA, <#if(bodyCount == 2) {#>inertiaB, <#}#>accumulatedImpulses.Tangent, ref wsvA<#if(bodyCount == 2) {#>, ref wsvB<#}#>); + TangentFriction<#=suffix#>.WarmStart2(x, z, <#if(contactCount > 1) {#>offsetToManifoldCenterA<#} else {#>prestep.Contact0.OffsetA<#}#>, <#if(bodyCount == 2) {#>offsetToManifoldCenterB, <#}#>inertiaA, <#if(bodyCount == 2) {#>inertiaB, <#}#>accumulatedImpulses.Tangent, ref wsvA<#if(bodyCount == 2) {#>, ref wsvB<#}#>); <#for (int i = 0; i < contactCount; ++i) {#> - PenetrationLimit<#=suffix#>.WarmStart2(inertiaA, <#if(bodyCount == 2) {#>inertiaB, <#}#><#=bodyCount == 2 ? "normal" : "prestep.LocalNormalB"#>, worldOffsetA<#=i#>, <#if(bodyCount == 2) {#>worldOffsetA<#=i#> - ab, <#}#>accumulatedImpulses.Penetration<#=i#>, ref wsvA<#if(bodyCount == 2) {#>, ref wsvB<#}#>); + PenetrationLimit<#=suffix#>.WarmStart2(inertiaA, <#if(bodyCount == 2) {#>inertiaB, <#}#>prestep.Normal, prestep.Contact<#=i#>.OffsetA, <#if(bodyCount == 2) {#>prestep.Contact<#=i#>.OffsetA - prestep.OffsetB, <#}#>accumulatedImpulses.Penetration<#=i#>, ref wsvA<#if(bodyCount == 2) {#>, ref wsvB<#}#>); <#}#> - TwistFriction<#=suffix#>.WarmStart2(<#=bodyCount == 2 ? "normal" : "prestep.LocalNormalB"#>, inertiaA, <#if(bodyCount == 2) {#>inertiaB, <#}#>accumulatedImpulses.Twist, ref wsvA<#if(bodyCount == 2) {#>, ref wsvB<#}#>); + TwistFriction<#=suffix#>.WarmStart2(prestep.Normal, inertiaA, <#if(bodyCount == 2) {#>inertiaB, <#}#>accumulatedImpulses.Twist, ref wsvA<#if(bodyCount == 2) {#>, ref wsvB<#}#>); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(bodyCount == 2) {#>in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>float dt, float inverseDt, in Contact<#=contactCount#><#=suffix#>PrestepData prestep, ref Contact<#=contactCount#>AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA<#if(bodyCount == 2) {#>, ref BodyVelocityWide wsvB<#}#>) { -<#if (bodyCount == 1) {#> - Helpers.BuildOrthonormalBasis(prestep.LocalNormalB, out var x, out var z); -<#} else {#> - QuaternionWide.TransformWithoutOverlap(prestep.LocalNormalB, orientationB, out var normal); - Helpers.BuildOrthonormalBasis(normal, out var x, out var z); -<#}#> -<#for (int i = 0; i < contactCount; ++i) {#> - QuaternionWide.TransformWithoutOverlap(prestep.Contact<#=i#>.LocalOffsetA, orientationA, out var worldOffsetA<#=i#>); -<#}#> + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); <#if (contactCount > 1) {#> var premultipliedFrictionCoefficient = new Vector(1f / <#=contactCount#>f) * prestep.MaterialProperties.FrictionCoefficient; <#}#> var maximumTangentImpulse = <#=contactCount > 1 ? "premultipliedFrictionCoefficient" : "prestep.MaterialProperties.FrictionCoefficient"#> * (<#for (int i = 0; i < contactCount; ++i) {#>accumulatedImpulses.Penetration<#=i#><#if(i < contactCount - 1){#> + <#}}#>); <#if (contactCount > 1) {#> - FrictionHelpers.ComputeFrictionCenter(<#for (int i = 0; i < contactCount; ++i) {#>worldOffsetA<#=i#>, <#}#><#for (int i = 0; i < contactCount; ++i) {#>prestep.Contact<#=i#>.Depth, <#}#>out var offsetToManifoldCenterA); + FrictionHelpers.ComputeFrictionCenter(<#for (int i = 0; i < contactCount; ++i) {#>prestep.Contact<#=i#>.OffsetA, <#}#><#for (int i = 0; i < contactCount; ++i) {#>prestep.Contact<#=i#>.Depth, <#}#>out var offsetToManifoldCenterA); <#if (bodyCount == 2) {#> - var ab = positionB - positionA; - Vector3Wide.Subtract(offsetToManifoldCenterA, ab, out var offsetToManifoldCenterB); + Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); <#}#> <#} else if(bodyCount == 2) {#> - var ab = positionB - positionA; - Vector3Wide.Subtract(worldOffsetA0, ab, out var offsetToManifoldCenterB); + Vector3Wide.Subtract(prestep.Contact0.OffsetA, prestep.OffsetB, out var offsetToManifoldCenterB); <#}#> - TangentFriction<#=suffix#>.Solve2(x, z, <#=contactCount == 1 ? "worldOffsetA0" : "offsetToManifoldCenterA"#><#=bodyCount == 2 ? ", offsetToManifoldCenterB" : ""#>, inertiaA<#=bodyCount == 2 ? ", inertiaB" : ""#>, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); + TangentFriction<#=suffix#>.Solve2(x, z, <#=contactCount == 1 ? "prestep.Contact0.OffsetA" : "offsetToManifoldCenterA"#><#=bodyCount == 2 ? ", offsetToManifoldCenterB" : ""#>, inertiaA<#=bodyCount == 2 ? ", inertiaB" : ""#>, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); //Note that we solve the penetration constraints after the friction constraints. //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. //It's a pretty minor effect either way. SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); var inverseDtWide = new Vector(inverseDt); <#for (int i = 0; i < contactCount; ++i) {#> - PenetrationLimit<#=suffix#>.Solve2(inertiaA<#=bodyCount == 2 ? ", inertiaB" : ""#>, <#=bodyCount == 2 ? "normal" : "prestep.LocalNormalB"#>, worldOffsetA0, <#if(bodyCount == 2) {#>worldOffsetA<#=i#> - ab, <#}#>prestep.Contact<#=i#>.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration<#=i#>, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); + PenetrationLimit<#=suffix#>.Solve2(inertiaA<#=bodyCount == 2 ? ", inertiaB" : ""#>, prestep.Normal, prestep.Contact<#=i#>.OffsetA, <#if(bodyCount == 2) {#>prestep.Contact<#=i#>.OffsetA - prestep.OffsetB, <#}#>prestep.Contact<#=i#>.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration<#=i#>, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); <#}#> <#if (contactCount == 1) {#> //If there's only one contact, then the contact patch as determined by contact distance would be zero. @@ -370,10 +371,10 @@ for (int i = 0; i < contactCount; ++i) <#} else {#> var maximumTwistImpulse = <#=contactCount > 1 ? "premultipliedFrictionCoefficient" : "prestep.MaterialProperties.FrictionCoefficient"#> * ( <#for (int i = 0; i < contactCount; ++i) {#> - accumulatedImpulses.Penetration<#=i#> * Vector3Wide.Distance(offsetToManifoldCenterA, worldOffsetA<#=i#>)<#=i == contactCount - 1 ? ");" : " +"#> + accumulatedImpulses.Penetration<#=i#> * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact<#=i#>.OffsetA)<#=i == contactCount - 1 ? ");" : " +"#> <#}#> <#}#> - TwistFriction<#=suffix#>.Solve2(<#=bodyCount == 2 ? "normal" : "prestep.LocalNormalB"#>, inertiaA, <#if (bodyCount == 2) {#>inertiaB, <#}#>maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); + TwistFriction<#=suffix#>.Solve2(prestep.Normal, inertiaA, <#if (bodyCount == 2) {#>inertiaB, <#}#>maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -385,10 +386,10 @@ for (int i = 0; i < contactCount; ++i) in Vector dt, in Contact<#= contactCount #>AccumulatedImpulses accumulatedImpulses, ref Contact<#= contactCount #><#=suffix#>PrestepData prestep) { <#if (bodyCount == 2) {#> - //prestep.OffsetB = positionB - positionA; + prestep.OffsetB = positionB - positionA; <#}#> <#for (int i = 0; i < contactCount; ++i) {#> - //PenetrationLimit<#=suffix#>.UpdatePenetrationDepth(dt, prestep.Contact<#=i#>.OffsetA, <#if(bodyCount == 2) {#>prestep.OffsetB, <#}#>prestep.Normal, wsvA, <#if (bodyCount == 2) {#>wsvB, <#}#>ref prestep.Contact<#=i#>.Depth); + PenetrationLimit<#=suffix#>.UpdatePenetrationDepth(dt, prestep.Contact<#=i#>.OffsetA, <#if(bodyCount == 2) {#>prestep.OffsetB, <#}#>prestep.Normal, wsvA, <#if (bodyCount == 2) {#>wsvB, <#}#>ref prestep.Contact<#=i#>.Depth); <#}#> } } diff --git a/BepuPhysics/Constraints/Contact/IContactConstraintDescription.cs b/BepuPhysics/Constraints/Contact/IContactConstraintDescription.cs index d3b5e5eeb..b1a90df04 100644 --- a/BepuPhysics/Constraints/Contact/IContactConstraintDescription.cs +++ b/BepuPhysics/Constraints/Contact/IContactConstraintDescription.cs @@ -8,20 +8,19 @@ namespace BepuPhysics.Constraints.Contact { public struct ConstraintContactData { - public Vector3 LocalOffsetA; + public Vector3 OffsetA; public float PenetrationDepth; - public float PlaneOffsetB; } public interface IConvexOneBodyContactConstraintDescription : IOneBodyConstraintDescription where TDescription : unmanaged, IConvexOneBodyContactConstraintDescription { - void CopyManifoldWideProperties(ref Vector3 localNormalB, ref PairMaterialProperties material); + void CopyManifoldWideProperties(ref Vector3 normal, ref PairMaterialProperties material); ref ConstraintContactData GetFirstContact(ref TDescription description); } public interface IConvexTwoBodyContactConstraintDescription : ITwoBodyConstraintDescription where TDescription : unmanaged, IConvexTwoBodyContactConstraintDescription { - void CopyManifoldWideProperties(ref Vector3 localNormalB, ref PairMaterialProperties material); + void CopyManifoldWideProperties(ref Vector3 offsetB, ref Vector3 normal, ref PairMaterialProperties material); ref ConstraintContactData GetFirstContact(ref TDescription description); } diff --git a/Demos/Demo.cs b/Demos/Demo.cs index 37ebe5190..88b3e17f6 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -43,7 +43,7 @@ protected Demo() //It may be worth using something like hwloc to extract extra information to reason about. //var targetThreadCount = Math.Max(1, Environment.ProcessorCount > 4 ? Environment.ProcessorCount - 2 : Environment.ProcessorCount - 1); - var targetThreadCount = 30; + var targetThreadCount = 3; ThreadDispatcher = new SimpleThreadDispatcher(targetThreadCount); } @@ -62,7 +62,7 @@ public virtual void Update(Window window, Camera camera, Input input, float dt) //fully decouple simulation and rendering rates across different threads. //(In either case, you'd also want to interpolate or extrapolate simulation results during rendering for smoothness.) //Note that taking steps of variable length can reduce stability. Gradual or one-off changes can work reasonably well. - Simulation.Timestep(1 / 60f, ThreadDispatcher); + Simulation.Timestep(1 / 60f); ////Here's an example of how it would look to use more frequent updates, but still with a fixed amount of time simulated per update call: //const float timeToSimulate = 1 / 60f; From 767b425573ac8189f721b77698ed9f5fc35ee1e8 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 5 Aug 2021 18:53:18 -0500 Subject: [PATCH 147/947] That part's okay, though. --- BepuPhysics/BodyReference.cs | 26 ++++++++++++++++++++++++++ 1 file changed, 26 insertions(+) diff --git a/BepuPhysics/BodyReference.cs b/BepuPhysics/BodyReference.cs index 6c311c79e..5bc27a918 100644 --- a/BepuPhysics/BodyReference.cs +++ b/BepuPhysics/BodyReference.cs @@ -115,6 +115,32 @@ public ref RigidPose Pose } } + /// + /// Gets a reference to the body's motion state, including both pose and velocity. + /// + public ref MotionState MotionState + { + [MethodImpl(MethodImplOptions.NoInlining)] + get + { + ref var location = ref MemoryLocation; + return ref Bodies.Sets[location.SetIndex].SolverStates[location.Index].Motion; + } + } + + /// + /// Gets a reference to the body's solver-relevant state, including both pose, velocity, and inertia. + /// + public ref SolverState SolverState + { + [MethodImpl(MethodImplOptions.NoInlining)] + get + { + ref var location = ref MemoryLocation; + return ref Bodies.Sets[location.SetIndex].SolverStates[location.Index]; + } + } + /// /// Gets a reference to the body's collidable. /// From f800b2521d799f3752ac535e5fb5a4da116471d2 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 8 Aug 2021 18:24:05 -0500 Subject: [PATCH 148/947] Pushing through contact incremental updates for embedded substepping. --- BepuPhysics/Constraints/IBodyAccessFilter.cs | 10 +++ .../Constraints/OneBodyTypeProcessor.cs | 2 +- .../Constraints/TwoBodyTypeProcessor.cs | 3 +- .../Solver_IncrementalContactUpdate.cs | 2 +- BepuPhysics/Solver_Solve.cs | 67 +++++++++++++------ BepuPhysics/Solver_SubsteppingSolve2.cs | 50 +++++++++++++- Demos/Demo.cs | 2 +- Demos/Demos/ColosseumDemo.cs | 6 +- Demos/Program.cs | 3 +- 9 files changed, 116 insertions(+), 29 deletions(-) diff --git a/BepuPhysics/Constraints/IBodyAccessFilter.cs b/BepuPhysics/Constraints/IBodyAccessFilter.cs index 8b70ff663..d569287f4 100644 --- a/BepuPhysics/Constraints/IBodyAccessFilter.cs +++ b/BepuPhysics/Constraints/IBodyAccessFilter.cs @@ -54,6 +54,7 @@ public struct AccessNoPose : IBodyAccessFilter public bool AccessLinearVelocity => true; public bool AccessAngularVelocity => true; } + public struct AccessNoOrientation : IBodyAccessFilter { public bool GatherPosition => true; @@ -63,6 +64,15 @@ public struct AccessNoOrientation : IBodyAccessFilter public bool AccessLinearVelocity => true; public bool AccessAngularVelocity => true; } + public struct AccessOnlyVelocity: IBodyAccessFilter + { + public bool GatherPosition => false; + public bool GatherOrientation => false; + public bool GatherMass => false; + public bool GatherInertiaTensor => false; + public bool AccessLinearVelocity => true; + public bool AccessAngularVelocity => true; + } public struct AccessOnlyAngular : IBodyAccessFilter { diff --git a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs index 9701262e7..2148bded4 100644 --- a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs @@ -360,7 +360,7 @@ public unsafe override void IncrementallyUpdateContactData(ref TypeBatch typeBat ref var prestep = ref Unsafe.Add(ref prestepBase, i); ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); var count = GetCountInBundle(ref typeBatch, i); - bodies.GatherState(ref references, count, out _, out _, out var wsvA, out _); + bodies.GatherState(ref references, count, true, out _, out _, out var wsvA, out _); function.IncrementallyUpdateContactData(dtWide, wsvA, ref prestep); } } diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index ef60f84df..167e73706 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -479,7 +479,8 @@ public unsafe override void IncrementallyUpdateContactData(ref TypeBatch typeBat ref var prestep = ref Unsafe.Add(ref prestepBase, i); ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); var count = GetCountInBundle(ref typeBatch, i); - bodies.GatherState(ref references, count, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB); + bodies.GatherState(ref references.IndexA, count, true, out _, out _, out var wsvA, out _); + bodies.GatherState(ref references.IndexB, count, true, out _, out _, out var wsvB, out _); function.IncrementallyUpdateContactData(dtWide, wsvA, wsvB, ref prestep); } } diff --git a/BepuPhysics/Solver_IncrementalContactUpdate.cs b/BepuPhysics/Solver_IncrementalContactUpdate.cs index 118d4a4d9..d2f8708d4 100644 --- a/BepuPhysics/Solver_IncrementalContactUpdate.cs +++ b/BepuPhysics/Solver_IncrementalContactUpdate.cs @@ -11,7 +11,7 @@ namespace BepuPhysics { public partial class Solver { - struct IncrementalContactDataUpdateFilter : ITypeBatchSolveFilter + protected struct IncrementalContactDataUpdateFilter : ITypeBatchSolveFilter { public bool AllowFallback { get { return false; } } diff --git a/BepuPhysics/Solver_Solve.cs b/BepuPhysics/Solver_Solve.cs index 78cd1f93c..fa5ca1def 100644 --- a/BepuPhysics/Solver_Solve.cs +++ b/BepuPhysics/Solver_Solve.cs @@ -118,11 +118,32 @@ public bool AllowType(int typeId) } } - protected unsafe void BuildWorkBlocks(BufferPool pool, int minimumBlockSizeInBundles, int maximumBlockSizeInBundles, int targetBlocksPerBatch, ref TTypeBatchFilter typeBatchFilter) where TTypeBatchFilter : ITypeBatchSolveFilter + void BuildFallbackScatterWorkBlocks(int targetBlocksPerBatch) { ref var activeSet = ref ActiveSet; - context.ConstraintBlocks.Blocks = new QuickList(targetBlocksPerBatch * activeSet.Batches.Count, pool); - pool.Take(activeSet.Batches.Count, out context.BatchBoundaries); + if (activeSet.Batches.Count > FallbackBatchThreshold) + { + //There is a fallback batch, so we need to create fallback work blocks for it. + var blockCount = Math.Min(targetBlocksPerBatch, ActiveSet.Fallback.BodyCount); + context.FallbackBlocks.Blocks = new QuickList(blockCount, pool); + var baseBodiesPerBlock = activeSet.Fallback.BodyCount / blockCount; + var remainder = activeSet.Fallback.BodyCount - baseBodiesPerBlock * blockCount; + int previousEnd = 0; + for (int i = 0; i < blockCount; ++i) + { + var bodiesInBlock = i < remainder ? baseBodiesPerBlock + 1 : baseBodiesPerBlock; + context.FallbackBlocks.Blocks.AllocateUnsafely() = new FallbackScatterWorkBlock { Start = previousEnd, End = previousEnd += bodiesInBlock }; + } + } + } + + protected unsafe void BuildWorkBlocks( + BufferPool pool, int minimumBlockSizeInBundles, int maximumBlockSizeInBundles, int targetBlocksPerBatch, ref TTypeBatchFilter typeBatchFilter, + out QuickList workBlocks, out Buffer batchBoundaries) where TTypeBatchFilter : ITypeBatchSolveFilter + { + ref var activeSet = ref ActiveSet; + workBlocks = new QuickList(targetBlocksPerBatch * activeSet.Batches.Count, pool); + pool.Take(activeSet.Batches.Count, out batchBoundaries); var inverseMinimumBlockSizeInBundles = 1f / minimumBlockSizeInBundles; var inverseMaximumBlockSizeInBundles = 1f / maximumBlockSizeInBundles; for (int batchIndex = 0; batchIndex < activeSet.Batches.Count; ++batchIndex) @@ -152,7 +173,7 @@ protected unsafe void BuildWorkBlocks(BufferPool pool, int min var remainder = typeBatch.BundleCount - baseBlockSizeInBundles * typeBatchBlockCount; for (int newBlockIndex = 0; newBlockIndex < typeBatchBlockCount; ++newBlockIndex) { - ref var block = ref context.ConstraintBlocks.Blocks.Allocate(pool); + ref var block = ref workBlocks.Allocate(pool); var blockBundleCount = newBlockIndex < remainder ? baseBlockSizeInBundles + 1 : baseBlockSizeInBundles; block.BatchIndex = batchIndex; block.TypeBatchIndex = typeBatchIndex; @@ -163,21 +184,7 @@ protected unsafe void BuildWorkBlocks(BufferPool pool, int min Debug.Assert(block.End >= block.StartBundle + Math.Min(minimumBlockSizeInBundles, typeBatch.BundleCount) && block.End <= typeBatch.BundleCount); } } - context.BatchBoundaries[batchIndex] = context.ConstraintBlocks.Blocks.Count; - } - if (typeBatchFilter.AllowFallback && activeSet.Batches.Count > FallbackBatchThreshold) - { - //There is a fallback batch, so we need to create fallback work blocks for it. - var blockCount = Math.Min(targetBlocksPerBatch, ActiveSet.Fallback.BodyCount); - context.FallbackBlocks.Blocks = new QuickList(blockCount, pool); - var baseBodiesPerBlock = activeSet.Fallback.BodyCount / blockCount; - var remainder = activeSet.Fallback.BodyCount - baseBodiesPerBlock * blockCount; - int previousEnd = 0; - for (int i = 0; i < blockCount; ++i) - { - var bodiesInBlock = i < remainder ? baseBodiesPerBlock + 1 : baseBodiesPerBlock; - context.FallbackBlocks.Blocks.AllocateUnsafely() = new FallbackScatterWorkBlock { Start = previousEnd, End = previousEnd += bodiesInBlock }; - } + batchBoundaries[batchIndex] = workBlocks.Count; } } @@ -244,9 +251,14 @@ protected struct MultithreadingParameters public int WorkerCount; public Buffer FallbackResults; + public WorkBlocks IncrementalUpdateBlocks; + public Buffer IncrementalUpdateBatchBoundaries; + public Buffer WorkerBoundsA; public Buffer WorkerBoundsB; + + } protected MultithreadingParameters context; @@ -713,7 +725,7 @@ void ValidateWorkBlocks(ref TTypeBatchSolveFilter filter) } - protected void ExecuteMultithreaded(float dt, IThreadDispatcher threadDispatcher, Action workDelegate) where TTypeBatchSolveFilter : struct, ITypeBatchSolveFilter + protected void ExecuteMultithreaded(float dt, IThreadDispatcher threadDispatcher, Action workDelegate, bool includeIncrementalUpdate = false) where TTypeBatchSolveFilter : struct, ITypeBatchSolveFilter { var filter = default(TTypeBatchSolveFilter); var workerCount = context.WorkerCount = threadDispatcher.ThreadCount; @@ -729,11 +741,22 @@ protected void ExecuteMultithreaded(float dt, IThreadDisp const int maximumBlockSizeInBundles = 1024; var targetBlocksPerBatch = workerCount * targetBlocksPerBatchPerWorker; - BuildWorkBlocks(pool, minimumBlockSizeInBundles, maximumBlockSizeInBundles, targetBlocksPerBatch, ref filter); + BuildWorkBlocks(pool, minimumBlockSizeInBundles, maximumBlockSizeInBundles, targetBlocksPerBatch, ref filter, out context.ConstraintBlocks.Blocks, out context.BatchBoundaries); + if (includeIncrementalUpdate) + { + //This looks a little weird- having a filter, which is sometimes the incremental filter, as the main filter- then having this internal filter that's conditionally used... + //It's just a hack to deal with the fact that we're in the middle of a fairly major restructuring in the solver. Once the old style incremental update is gone, the weirdness can be purged. + var incrementalFilter = new IncrementalContactDataUpdateFilter(); + BuildWorkBlocks(pool, minimumBlockSizeInBundles, maximumBlockSizeInBundles, targetBlocksPerBatch, ref incrementalFilter, out context.IncrementalUpdateBlocks.Blocks, out context.IncrementalUpdateBatchBoundaries); + } + if (filter.AllowFallback) + BuildFallbackScatterWorkBlocks(targetBlocksPerBatch); ValidateWorkBlocks(ref filter); //Note the clear; the block claims must be initialized to 0 so that the first worker stage knows that the data is available to claim. context.ConstraintBlocks.CreateClaims(pool); + if (includeIncrementalUpdate) + context.IncrementalUpdateBlocks.CreateClaims(pool); if (filter.AllowFallback && ActiveSet.Batches.Count > FallbackBatchThreshold) { Debug.Assert(context.FallbackBlocks.Blocks.Count > 0); @@ -754,6 +777,8 @@ protected void ExecuteMultithreaded(float dt, IThreadDisp threadDispatcher.DispatchWorkers(workDelegate); context.ConstraintBlocks.Dispose(pool); + if (includeIncrementalUpdate) + context.IncrementalUpdateBlocks.Dispose(pool); if (filter.AllowFallback && ActiveSet.Batches.Count > FallbackBatchThreshold) { FallbackBatch.DisposeResults(this, pool, ref ActiveSet.Batches[FallbackBatchThreshold], ref context.FallbackResults); diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index f9348cafe..14995666c 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -136,6 +136,21 @@ public void Execute(Solver solver, int blockIndex, int workerIndex) // } //} + struct IncrementalUpdateStageFunction : IStageFunction + { + public float Dt; + public float InverseDt; + public Solver solver; + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void Execute(Solver solver, int blockIndex, int workerIndex) + { + ref var block = ref this.solver.context.IncrementalUpdateBlocks.Blocks[blockIndex]; + ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; + solver.TypeProcessors[typeBatch.TypeId].IncrementallyUpdateContactData(ref typeBatch, solver.bodies, Dt, InverseDt, block.StartBundle, block.End); + } + } + Action solveStep2Worker; void SolveStep2Worker(int workerIndex) { @@ -177,9 +192,28 @@ void SolveStep2Worker(int workerIndex) InverseDt = 1f / context.Dt, solver = this }; + var incrementalUpdateStage = new IncrementalUpdateStageFunction + { + Dt = context.Dt, + InverseDt = 1f / context.Dt, + solver = this + }; + //We have a different set of work blocks for incremental updates, so they used a different set of claimed/unclaimed state ping ponging locals. + var incrementalClaimedState = 1; + int incrementalUnclaimedState = 0; + + var incrementalUpdateWorkerStart = GetUniformlyDistributedStart(workerIndex, context.IncrementalUpdateBlocks.Blocks.Count, context.WorkerCount, 0); for (int i = 0; i < substepCount; ++i) { + if (i > 0) + { + ExecuteStage( + ref incrementalUpdateStage, ref context.IncrementalUpdateBlocks, ref bounds, ref boundsBackBuffer, workerIndex, 0, context.IncrementalUpdateBlocks.Blocks.Count, + ref incrementalUpdateWorkerStart, ref syncStage, incrementalClaimedState, incrementalUnclaimedState); + incrementalClaimedState ^= 1; + incrementalUnclaimedState ^= 1; + } warmstartStage.SubstepIndex = i; for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) { @@ -563,8 +597,22 @@ public override void SolveStep2(float totalDt, IThreadDispatcher threadDispatche GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); Debug.Assert(!fallbackExists, "Not handling this yet."); + var incrementalUpdateFilter = default(IncrementalContactDataUpdateFilter); for (int substepIndex = 0; substepIndex < substepCount; ++substepIndex) { + if (substepIndex > 0) + { + for (int i = 0; i < ActiveSet.Batches.Count; ++i) + { + ref var batch = ref activeSet.Batches[i]; + for (int j = 0; j < batch.TypeBatches.Count; ++j) + { + ref var typeBatch = ref batch.TypeBatches[j]; + if (incrementalUpdateFilter.AllowType(typeBatch.TypeId)) + TypeProcessors[typeBatch.TypeId].IncrementallyUpdateContactData(ref typeBatch, bodies, substepDt, inverseDt, 0, typeBatch.BundleCount); + } + } + } for (int i = 0; i < synchronizedBatchCount; ++i) { ref var batch = ref activeSet.Batches[i]; @@ -599,7 +647,7 @@ public override void SolveStep2(float totalDt, IThreadDispatcher threadDispatche } else { - ExecuteMultithreaded(substepDt, threadDispatcher, solveStep2Worker); + ExecuteMultithreaded(substepDt, threadDispatcher, solveStep2Worker, includeIncrementalUpdate: true); } } } diff --git a/Demos/Demo.cs b/Demos/Demo.cs index 88b3e17f6..fcc959c56 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -43,7 +43,7 @@ protected Demo() //It may be worth using something like hwloc to extract extra information to reason about. //var targetThreadCount = Math.Max(1, Environment.ProcessorCount > 4 ? Environment.ProcessorCount - 2 : Environment.ProcessorCount - 1); - var targetThreadCount = 3; + var targetThreadCount = 29; ThreadDispatcher = new SimpleThreadDispatcher(targetThreadCount); } diff --git a/Demos/Demos/ColosseumDemo.cs b/Demos/Demos/ColosseumDemo.cs index f0d207d78..7aacfb20a 100644 --- a/Demos/Demos/ColosseumDemo.cs +++ b/Demos/Demos/ColosseumDemo.cs @@ -70,13 +70,15 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //The PositionFirstTimestepper is the simplest timestepping mode, but since it integrates velocity into position at the start of the frame, directly modified velocities outside of the timestep //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + //Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + //Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper2(3), solverIterationCount: 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), solverIterationCount: 1); var ringBoxShape = new Box(0.5f, 1, 3); ringBoxShape.ComputeInertia(1, out var ringBoxInertia); var boxDescription = BodyDescription.CreateDynamic(new Vector3(), ringBoxInertia, new CollidableDescription(Simulation.Shapes.Add(ringBoxShape), 0.1f), - new BodyActivityDescription(0.01f)); + new BodyActivityDescription(0.0001f)); var layerPosition = new Vector3(); const int layerCount = 6; diff --git a/Demos/Program.cs b/Demos/Program.cs index 6c769ece5..2c1aded45 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -19,7 +19,8 @@ static void Main(string[] args) { content = ContentArchive.Load(stream); } - HeadlessTest.Test(content, 4, 32, 512); + //HeadlessTest.Test(content, 4, 32, 512); + //HeadlessTest.Test(content, 4, 32, 512); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); From 1dab604f9bd7f82c35b43f43013910d9900f55a8 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 10 Aug 2021 18:58:10 -0500 Subject: [PATCH 149/947] Fixed lack of masking in velocity integration during pose+velocity constraint integration. --- BepuPhysics/Constraints/OneBodyTypeProcessor.cs | 4 ++-- BepuPhysics/Constraints/TwoBodyTypeProcessor.cs | 2 +- BepuPhysics/Constraints/TypeProcessor.cs | 2 +- Demos/Demo.cs | 2 +- Demos/Demos/NewtDemo.cs | 4 ++-- 5 files changed, 7 insertions(+), 7 deletions(-) diff --git a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs index 2148bded4..cce25949d 100644 --- a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs @@ -280,7 +280,7 @@ public unsafe override void WarmStart2(dt), accumulatedImpulses, ref prestep); - + function.WarmStart2(positionA, orientationA, inertiaA, prestep, accumulatedImpulses, ref wsvA); if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) @@ -313,7 +313,7 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f function.Solve2(positionA, orientationA, inertiaA, dt, inverseDt, prestep, ref accumulatedImpulses, ref wsvA); - bodies.ScatterVelocities(ref wsvA, ref references, count); + bodies.ScatterVelocities(ref wsvA, ref references, count); } } diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 167e73706..1c3be470f 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -428,7 +428,7 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f //Prefetch(SolvePrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, i, exclusiveEndBundle); bodies.GatherState(ref references.IndexA, count, true, out var positionA, out var orientationA, out var wsvA, out var inertiaA); bodies.GatherState(ref references.IndexB, count, true, out var positionB, out var orientationB, out var wsvB, out var inertiaB); - + function.Solve2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, dt, inverseDt, prestep, ref accumulatedImpulses, ref wsvA, ref wsvB); bodies.ScatterVelocities(ref wsvA, ref references.IndexA, count); diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index 2fda76812..0ab2fd829 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -907,7 +907,7 @@ public static unsafe void GatherAndIntegrate("Content\\newt.obj"); float cellSize = 0.1f; From 1e6bf81ed8e96e1b7f2bf34eb96904e59536e5d4 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 12 Aug 2021 21:04:02 -0500 Subject: [PATCH 150/947] Added RopeTwistDemo and uncovered an odd bug in embedded substepping. --- BepuPhysics/Constraints/DistanceLimit.cs | 70 ++++++- BepuPhysics/Constraints/DistanceServo.cs | 2 +- .../Constraints/TwoBodyTypeProcessor.cs | 2 +- Demos/Demo.cs | 2 +- Demos/DemoCallbacks.cs | 2 +- Demos/DemoSet.cs | 1 + Demos/Demos/RopeStabilityDemo.cs | 4 +- Demos/Demos/RopeTwistDemo.cs | 174 ++++++++++++++++++ 8 files changed, 246 insertions(+), 11 deletions(-) create mode 100644 Demos/Demos/RopeTwistDemo.cs diff --git a/BepuPhysics/Constraints/DistanceLimit.cs b/BepuPhysics/Constraints/DistanceLimit.cs index 3bfafb3fc..71f8af4bc 100644 --- a/BepuPhysics/Constraints/DistanceLimit.cs +++ b/BepuPhysics/Constraints/DistanceLimit.cs @@ -52,7 +52,7 @@ public DistanceLimit(in Vector3 localOffsetA, in Vector3 localOffsetB, float min MaximumDistance = maximumDistance; SpringSettings = springSettings; } - + public readonly int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -140,7 +140,7 @@ public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref DistanceLimitProjection projection, ref Vector accumulatedImpulse) { - DistanceServoFunctions.ApplyImpulse(ref velocityA, ref velocityB, + DistanceServoFunctions.ApplyImpulse(ref velocityA, ref velocityB, projection.LinearImpulseToVelocityA, projection.AngularImpulseToVelocityA, projection.LinearImpulseToVelocityB, projection.AngularImpulseToVelocityB, ref accumulatedImpulse); } @@ -159,20 +159,80 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void ApplyImpulse(in Vector3Wide linearJacobianA, in Vector3Wide angularJacobianA, in Vector3Wide angularJacobianB, in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, + in Vector csi, ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB) + { + //TODO: Examine codegen quality for operators before generalizing. + var impulseScaledLinearJacobian = linearJacobianA * csi; + velocityA.Linear += impulseScaledLinearJacobian * inertiaA.InverseMass; + velocityB.Linear -= impulseScaledLinearJacobian * inertiaB.InverseMass; + velocityA.Angular += (angularJacobianA * csi) * inertiaA.InverseInertiaTensor; + velocityB.Angular += (angularJacobianB * csi) * inertiaB.InverseInertiaTensor; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void ComputeJacobians( + in Vector3Wide localOffsetA, in Vector3Wide positionA, in QuaternionWide orientationA, in Vector3Wide localOffsetB, in Vector3Wide positionB, in QuaternionWide orientationB, + in Vector minimumDistance, in Vector maximumDistance, out Vector useMinimum, out Vector distance, out Vector3Wide direction, out Vector3Wide angularJA, out Vector3Wide angularJB) + { + QuaternionWide.TransformWithoutOverlap(localOffsetA, orientationA, out var offsetA); + QuaternionWide.TransformWithoutOverlap(localOffsetB, orientationB, out var offsetB); + var anchorOffset = (offsetB - offsetA) + (positionB - positionA); + Vector3Wide.Length(anchorOffset, out distance); + //If the current distance is closer to the minimum, calibrate for the minimum. Otherwise, calibrate for the maximum. + useMinimum = Vector.LessThan(Vector.Abs(distance - minimumDistance), Vector.Abs(distance - maximumDistance)); + var sign = Vector.ConditionalSelect(useMinimum, new Vector(-1f), Vector.One); + Vector3Wide.Scale(anchorOffset, sign / distance, out direction); + //If the distance is too short to extract a direction, use an arbitrary fallback. + var needFallback = Vector.LessThan(distance, new Vector(1e-9f)); + direction.X = Vector.ConditionalSelect(needFallback, Vector.One, direction.X); + direction.Y = Vector.ConditionalSelect(needFallback, Vector.Zero, direction.Y); + direction.Z = Vector.ConditionalSelect(needFallback, Vector.Zero, direction.Z); + + Vector3Wide.CrossWithoutOverlap(offsetA, direction, out angularJA); + Vector3Wide.CrossWithoutOverlap(direction, offsetB, out angularJB); //Note flip negation. + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in DistanceLimitPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - throw new NotImplementedException(); + ComputeJacobians(prestep.LocalOffsetA, positionA, orientationA, prestep.LocalOffsetB, positionB, orientationB, prestep.MinimumDistance, prestep.MaximumDistance, out _, out _, out var direction, out var angularJA, out var angularJB); + ApplyImpulse(direction, angularJA, angularJB, inertiaA, inertiaB, accumulatedImpulses, ref wsvA, ref wsvB); } + [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in DistanceLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - throw new NotImplementedException(); + ComputeJacobians(prestep.LocalOffsetA, positionA, orientationA, prestep.LocalOffsetB, positionB, orientationB, prestep.MinimumDistance, prestep.MaximumDistance, out var useMinimum, out var distance, out var direction, out var angularJA, out var angularJB); + + //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); + Vector3Wide.Dot(wsvA.Linear, direction, out var linearCSVA); + Vector3Wide.Dot(wsvB.Linear, direction, out var negatedLinearCSVB); + Vector3Wide.Dot(wsvA.Angular, angularJA, out var angularCSVA); + Vector3Wide.Dot(wsvB.Angular, angularJB, out var angularCSVB); + var csv = linearCSVA - negatedLinearCSVB + angularCSVA + angularCSVB; + + //The linear jacobian contributions are just a scalar multiplication by 1 since it's a unit length vector. + Symmetric3x3Wide.VectorSandwich(angularJA, inertiaA.InverseInertiaTensor, out var angularContributionA); + Symmetric3x3Wide.VectorSandwich(angularJB, inertiaB.InverseInertiaTensor, out var angularContributionB); + var inverseEffectiveMass = inertiaA.InverseMass + inertiaB.InverseMass + angularContributionA + angularContributionB; + + SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); + var effectiveMass = effectiveMassCFMScale / inverseEffectiveMass; + var error = Vector.ConditionalSelect(useMinimum, prestep.MinimumDistance - distance, distance - prestep.MaximumDistance); + InequalityHelpers.ComputeBiasVelocity(error, positionErrorToVelocity, inverseDt, out var biasVelocity); + var csi = -accumulatedImpulses * softnessImpulseScale - effectiveMass * (csv - biasVelocity); + InequalityHelpers.ClampPositive(ref accumulatedImpulses, ref csi); + + ApplyImpulse(direction, angularJA, angularJB, inertiaA, inertiaB, csi, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector dt, in Vector accumulatedImpulses, ref DistanceLimitPrestepData prestep) { diff --git a/BepuPhysics/Constraints/DistanceServo.cs b/BepuPhysics/Constraints/DistanceServo.cs index d3b23679d..d84d2cbb6 100644 --- a/BepuPhysics/Constraints/DistanceServo.cs +++ b/BepuPhysics/Constraints/DistanceServo.cs @@ -253,7 +253,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in [MethodImpl(MethodImplOptions.AggressiveInlining)] public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector dt, in Vector accumulatedImpulses, ref DistanceServoPrestepData prestep) { diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 1c3be470f..167e73706 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -428,7 +428,7 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f //Prefetch(SolvePrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, i, exclusiveEndBundle); bodies.GatherState(ref references.IndexA, count, true, out var positionA, out var orientationA, out var wsvA, out var inertiaA); bodies.GatherState(ref references.IndexB, count, true, out var positionB, out var orientationB, out var wsvB, out var inertiaB); - + function.Solve2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, dt, inverseDt, prestep, ref accumulatedImpulses, ref wsvA, ref wsvB); bodies.ScatterVelocities(ref wsvA, ref references.IndexA, count); diff --git a/Demos/Demo.cs b/Demos/Demo.cs index 9189f4ba5..fcc959c56 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -62,7 +62,7 @@ public virtual void Update(Window window, Camera camera, Input input, float dt) //fully decouple simulation and rendering rates across different threads. //(In either case, you'd also want to interpolate or extrapolate simulation results during rendering for smoothness.) //Note that taking steps of variable length can reduce stability. Gradual or one-off changes can work reasonably well. - Simulation.Timestep(1 / 60f, ThreadDispatcher); + Simulation.Timestep(1 / 60f); ////Here's an example of how it would look to use more frequent updates, but still with a fixed amount of time simulated per update call: //const float timeToSimulate = 1 / 60f; diff --git a/Demos/DemoCallbacks.cs b/Demos/DemoCallbacks.cs index 73cf1e5ea..226d73d0d 100644 --- a/Demos/DemoCallbacks.cs +++ b/Demos/DemoCallbacks.cs @@ -95,7 +95,7 @@ public void IntegrateVelocity(int bodyIndex, in RigidPose pose, in BodyInertia l [MethodImpl(MethodImplOptions.AggressiveInlining)] public void IntegrateVelocity(ReadOnlySpan bodyIndices, in Vector3Wide position, in QuaternionWide orientation, in BodyInertiaWide localInertia, in Vector integrationMask, int workerIndex, in Vector dt, ref BodyVelocityWide velocity) { - velocity.Linear += gravityWide * dt; + velocity.Linear = Vector3Wide.ConditionalSelect(Vector.Equals(localInertia.InverseMass, Vector.Zero), velocity.Linear, velocity.Linear + gravityWide * dt); } } public unsafe struct DemoNarrowPhaseCallbacks : INarrowPhaseCallbacks diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index f90f14bc8..06b69ca1c 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -45,6 +45,7 @@ struct Option public DemoSet() { + AddOption(); AddOption(); AddOption(); AddOption(); diff --git a/Demos/Demos/RopeStabilityDemo.cs b/Demos/Demos/RopeStabilityDemo.cs index f04a174f8..d5dd2ada1 100644 --- a/Demos/Demos/RopeStabilityDemo.cs +++ b/Demos/Demos/RopeStabilityDemo.cs @@ -19,7 +19,7 @@ namespace Demos.Demos /// public class RopeStabilityDemo : Demo { - static BodyHandle[] BuildRopeBodies(Simulation simulation, in Vector3 start, int bodyCount, float bodySize, float bodySpacing, float massPerBody, float inverseInertiaScale) + public static BodyHandle[] BuildRopeBodies(Simulation simulation, in Vector3 start, int bodyCount, float bodySize, float bodySpacing, float massPerBody, float inverseInertiaScale, float speculativeMargin = 0.1f) { BodyHandle[] handles = new BodyHandle[bodyCount + 1]; var ropeShape = new Sphere(bodySize); @@ -31,7 +31,7 @@ static BodyHandle[] BuildRopeBodies(Simulation simulation, in Vector3 start, int { //Make the uppermost block kinematic to hold up the rest of the chain. Activity = new BodyActivityDescription(.01f), - Collidable = new CollidableDescription(ropeShapeIndex, 0.1f), + Collidable = new CollidableDescription(ropeShapeIndex, speculativeMargin), }; for (int linkIndex = 0; linkIndex < bodyCount + 1; ++linkIndex) { diff --git a/Demos/Demos/RopeTwistDemo.cs b/Demos/Demos/RopeTwistDemo.cs new file mode 100644 index 000000000..bcd2d4c80 --- /dev/null +++ b/Demos/Demos/RopeTwistDemo.cs @@ -0,0 +1,174 @@ +using BepuPhysics; +using BepuPhysics.Collidables; +using BepuPhysics.CollisionDetection; +using BepuPhysics.Constraints; +using BepuUtilities; +using DemoContentLoader; +using DemoRenderer; +using DemoRenderer.UI; +using DemoUtilities; +using SharpDX.Direct3D11; +using System; +using System.Collections.Generic; +using System.Numerics; +using System.Runtime.CompilerServices; +using System.Text; + +namespace Demos.Demos +{ + /// + /// Shows a bundle of ropes being tangled up by spinning weights. + /// + public class RopeTwistDemo : Demo + { + struct Filter + { + public short RopeIndex; + public short IndexInRope; + } + + unsafe struct RopeNarrowPhaseCallbacks : INarrowPhaseCallbacks + { + public CollidableProperty Filters; + public SpringSettings ContactSpringiness; + + public void Initialize(Simulation simulation) + { + Filters.Initialize(simulation); + //Use a default if the springiness value wasn't initialized. + if (ContactSpringiness.AngularFrequency == 0 && ContactSpringiness.TwiceDampingRatio == 0) + ContactSpringiness = new SpringSettings(30, 1); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b) + { + var aFilter = Filters[a]; + var bFilter = Filters[b]; + return (aFilter.RopeIndex != bFilter.RopeIndex || Math.Abs(aFilter.IndexInRope - bFilter.IndexInRope) > 3) && (a.Mobility == CollidableMobility.Dynamic || b.Mobility == CollidableMobility.Dynamic); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB) + { + return true; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public unsafe bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold + { + pairMaterial.FrictionCoefficient = 0f; + pairMaterial.MaximumRecoveryVelocity = 200f; + pairMaterial.SpringSettings = ContactSpringiness; + return true; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold) + { + return true; + } + + public void Dispose() + { + } + } + + public unsafe override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(0, 25, 45); + camera.Yaw = 0; + camera.Pitch = 0; + + var filters = new CollidableProperty(); + Simulation = Simulation.Create(BufferPool, + new RopeNarrowPhaseCallbacks { ContactSpringiness = new SpringSettings(1200, 1), Filters = filters }, + new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)), new EmbeddedSubsteppingTimestepper2(30), solverIterationCount: 1); + //Simulation = Simulation.Create(BufferPool, + // new RopeNarrowPhaseCallbacks { ContactSpringiness = new SpringSettings(1200, 1), Filters = filters }, + // new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper2(30), solverIterationCount: 1); + + + for (int twistIndex = 0; twistIndex < 5; ++twistIndex) + { + const int ropeCount = 4; + var startLocation = new Vector3(-30 + twistIndex * 15, 50, 0); + + var bigWreckingBall = new Sphere(3); + //This wrecking ball is much, much heavier. + bigWreckingBall.ComputeInertia(10000, out var bigWreckingBallInertia); + var bigWreckingBallIndex = Simulation.Shapes.Add(bigWreckingBall); + const float ropeBodySpacing = -0.1f; + const float ropeBodyRadius = 0.2f; + const int ropeBodyCount = 130; + var wreckingBallPosition = startLocation - new Vector3(0, ropeBodyRadius + (ropeBodyRadius * 2 + ropeBodySpacing) * ropeBodyCount + bigWreckingBall.Radius, 0); + var description = BodyDescription.CreateDynamic(wreckingBallPosition, bigWreckingBallInertia, new CollidableDescription(bigWreckingBallIndex, 25f), new BodyActivityDescription(-0.01f)); + //Give it a little bump. + //description.Velocity = new BodyVelocity(new Vector3(-10, 0, 0), default); + var wreckingBallBodyHandle = Simulation.Bodies.Add(description); + var wreckingBallBody = Simulation.Bodies.GetBodyReference(wreckingBallBodyHandle); + wreckingBallBody.Velocity.Angular = new Vector3(0, 0, 0); + filters.Allocate(wreckingBallBodyHandle) = new Filter { RopeIndex = (short)(16384 + twistIndex), IndexInRope = ropeBodyCount }; + + for (int ropeIndex = 0; ropeIndex < ropeCount; ++ropeIndex) + { + var angle = ropeIndex * MathF.PI * 2 / ropeCount; + const float ropeDistributionRadius = 1f; + var horizontalOffset = ropeDistributionRadius * new Vector3(MathF.Sin(angle), 0, MathF.Cos(angle)); + var ropeStartLocation = startLocation + horizontalOffset; + + var springSettings = new SpringSettings(300, 1); + var bodyHandles = RopeStabilityDemo.BuildRopeBodies(Simulation, ropeStartLocation, ropeBodyCount, ropeBodyRadius, ropeBodySpacing, 1f, 0, 25f); + for (int i = 0; i < bodyHandles.Length; ++i) + { + filters.Allocate(bodyHandles[i]) = new Filter { RopeIndex = (short)ropeIndex, IndexInRope = (short)i }; + } + + bool TryCreateConstraint(int handleIndexA, int handleIndexB) + { + if (handleIndexA >= bodyHandles.Length || handleIndexB >= bodyHandles.Length) + return false; + var maximumDistance = Vector3.Distance( + new BodyReference(bodyHandles[handleIndexA], Simulation.Bodies).Pose.Position, + new BodyReference(bodyHandles[handleIndexB], Simulation.Bodies).Pose.Position); + Simulation.Solver.Add(bodyHandles[handleIndexA], bodyHandles[handleIndexB], new DistanceLimit(default, default, .01f, maximumDistance, springSettings)); + return true; + } + const int constraintsPerBody = 1; + for (int i = 0; i < bodyHandles.Length - 1; ++i) + { + //Note that you could also create constraints which span even more links. For example, connect i and i+1, i+2, i+4, i+8 and i+16 rather than just the nearest bodies. + //That would make it behave a bit more like the previous cheat constraint, but it can be useful. + for (int j = 1; j <= constraintsPerBody; ++j) + { + if (!TryCreateConstraint(i, i + j)) + break; + } + } + + var wreckingBallConnectionOffset = horizontalOffset + new Vector3(0, bigWreckingBall.Radius, 0); + var ropeConnectionToBall = wreckingBallBody.Pose.Position + wreckingBallConnectionOffset; + for (int i = 1; i <= constraintsPerBody; ++i) + { + var targetBodyHandleIndex = bodyHandles.Length - i; + if (targetBodyHandleIndex < 0) + break; + var maximumDistance = Vector3.Distance( + new BodyReference(bodyHandles[targetBodyHandleIndex], Simulation.Bodies).Pose.Position, + ropeConnectionToBall); + Simulation.Solver.Add(bodyHandles[targetBodyHandleIndex], wreckingBallBodyHandle, new DistanceLimit(default, wreckingBallConnectionOffset, 0.01f, maximumDistance, springSettings)); + } + + } + } + + + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(200, 1, 200)), 0.1f))); + //Simulation.Statics.Add(new StaticDescription( + // new Vector3(100, 70, 0), BepuUtilities.QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), MathF.PI * 0.5f), + // new CollidableDescription(Simulation.Shapes.Add(new Capsule(8, 64)), 0.1f))); + + } + + } +} From 3f13149754a0175dc43659d60a569557e4502469 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 13 Aug 2021 18:22:24 -0500 Subject: [PATCH 151/947] Fixed integration responsibilities bug, tuned rope twist demo. --- BepuPhysics/Solver_SubsteppingSolve2.cs | 3 ++- Demos/Demos/RopeTwistDemo.cs | 30 +++++++++++++++---------- Demos/Program.cs | 1 + 3 files changed, 21 insertions(+), 13 deletions(-) diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index 14995666c..6aecbe926 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -370,7 +370,8 @@ public override unsafe IndexSet PrepareConstraintIntegrationResponsibilities(int pool.Take(batchReferencedHandles.Count, out bodiesFirstObservedInBatches); //We don't have to consider the first batch, since we know ahead of time that the first batch will be the first time we see any bodies in it. //Just copy directly from the first batch into the merged to initialize it. - pool.Take((bodies.HandlePool.HighestPossiblyClaimedId + 63) / 64, out mergedConstrainedBodyHandles.Flags); + //Note "+ 64" instead of "+ 63": the highest possibly claimed id is inclusive! + pool.Take((bodies.HandlePool.HighestPossiblyClaimedId + 64) / 64, out mergedConstrainedBodyHandles.Flags); var copyLength = Math.Min(mergedConstrainedBodyHandles.Flags.Length, batchReferencedHandles[0].Flags.Length); batchReferencedHandles[0].Flags.CopyTo(0, mergedConstrainedBodyHandles.Flags, 0, copyLength); batchReferencedHandles[0].Flags.Clear(copyLength, batchReferencedHandles[0].Flags.Length - copyLength); diff --git a/Demos/Demos/RopeTwistDemo.cs b/Demos/Demos/RopeTwistDemo.cs index bcd2d4c80..7068757be 100644 --- a/Demos/Demos/RopeTwistDemo.cs +++ b/Demos/Demos/RopeTwistDemo.cs @@ -76,38 +76,36 @@ public void Dispose() public unsafe override void Initialize(ContentArchive content, Camera camera) { - camera.Position = new Vector3(0, 25, 45); + camera.Position = new Vector3(0, 20, 20); camera.Yaw = 0; camera.Pitch = 0; var filters = new CollidableProperty(); Simulation = Simulation.Create(BufferPool, - new RopeNarrowPhaseCallbacks { ContactSpringiness = new SpringSettings(1200, 1), Filters = filters }, - new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)), new EmbeddedSubsteppingTimestepper2(30), solverIterationCount: 1); + new RopeNarrowPhaseCallbacks { ContactSpringiness = new SpringSettings(2000, 1), Filters = filters }, + new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(60), solverIterationCount: 1, solverFallbackBatchThreshold: 128); //Simulation = Simulation.Create(BufferPool, - // new RopeNarrowPhaseCallbacks { ContactSpringiness = new SpringSettings(1200, 1), Filters = filters }, - // new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper2(30), solverIterationCount: 1); + // new RopeNarrowPhaseCallbacks { ContactSpringiness = new SpringSettings(2000, 1), Filters = filters }, + // new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper2(60), solverIterationCount: 1, solverFallbackBatchThreshold: 128); - for (int twistIndex = 0; twistIndex < 5; ++twistIndex) + for (int twistIndex = 0; twistIndex < 1; ++twistIndex) { const int ropeCount = 4; - var startLocation = new Vector3(-30 + twistIndex * 15, 50, 0); + var startLocation = new Vector3(0 + twistIndex * 15, 30, 0); var bigWreckingBall = new Sphere(3); //This wrecking ball is much, much heavier. bigWreckingBall.ComputeInertia(10000, out var bigWreckingBallInertia); var bigWreckingBallIndex = Simulation.Shapes.Add(bigWreckingBall); const float ropeBodySpacing = -0.1f; - const float ropeBodyRadius = 0.2f; + const float ropeBodyRadius = 0.1f; const int ropeBodyCount = 130; var wreckingBallPosition = startLocation - new Vector3(0, ropeBodyRadius + (ropeBodyRadius * 2 + ropeBodySpacing) * ropeBodyCount + bigWreckingBall.Radius, 0); var description = BodyDescription.CreateDynamic(wreckingBallPosition, bigWreckingBallInertia, new CollidableDescription(bigWreckingBallIndex, 25f), new BodyActivityDescription(-0.01f)); - //Give it a little bump. - //description.Velocity = new BodyVelocity(new Vector3(-10, 0, 0), default); var wreckingBallBodyHandle = Simulation.Bodies.Add(description); var wreckingBallBody = Simulation.Bodies.GetBodyReference(wreckingBallBodyHandle); - wreckingBallBody.Velocity.Angular = new Vector3(0, 0, 0); + wreckingBallBody.Velocity.Angular = new Vector3(0, 20, 0); filters.Allocate(wreckingBallBodyHandle) = new Filter { RopeIndex = (short)(16384 + twistIndex), IndexInRope = ropeBodyCount }; for (int ropeIndex = 0; ropeIndex < ropeCount; ++ropeIndex) @@ -117,7 +115,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var horizontalOffset = ropeDistributionRadius * new Vector3(MathF.Sin(angle), 0, MathF.Cos(angle)); var ropeStartLocation = startLocation + horizontalOffset; - var springSettings = new SpringSettings(300, 1); + var springSettings = new SpringSettings(600, 1); var bodyHandles = RopeStabilityDemo.BuildRopeBodies(Simulation, ropeStartLocation, ropeBodyCount, ropeBodyRadius, ropeBodySpacing, 1f, 0, 25f); for (int i = 0; i < bodyHandles.Length; ++i) { @@ -169,6 +167,14 @@ bool TryCreateConstraint(int handleIndexA, int handleIndexB) // new CollidableDescription(Simulation.Shapes.Add(new Capsule(8, 64)), 0.1f))); } + public override void Update(Window window, Camera camera, Input input, float dt) + { + //This demo is an extreme case. + //As of this writing, extremely high substep counts and extremely high batch counts with a relatively small simulations (like in this demo) + //actually run faster on a single thread. The overhead of synchronizing the thread group thousands of times overwhelms the actual work being done. + //On the upside, this means that adding more bodies uninvolved to this simulation is almost free if using multithreading. + Simulation.Timestep(1 / 60f); + } } } diff --git a/Demos/Program.cs b/Demos/Program.cs index 2c1aded45..668a4ab43 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -21,6 +21,7 @@ static void Main(string[] args) } //HeadlessTest.Test(content, 4, 32, 512); //HeadlessTest.Test(content, 4, 32, 512); + //HeadlessTest.Test(content, 4, 32, 512); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); From f59dec993b568644495fa32fc533faca415e2dfa Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 14 Aug 2021 19:27:15 -0500 Subject: [PATCH 152/947] Fiddling with alternative work block scheduling to reduce overhead. Naive increment workstealer massively reduces overhead, but not by 100%, and it ruins memory stickiness. --- BepuPhysics/BepuPhysics.csproj | 4 ++- BepuPhysics/Solver_Solve.cs | 7 +++- BepuPhysics/Solver_SubsteppingSolve2.cs | 46 +++++++++++++++++++++---- BepuUtilities/BepuUtilities.csproj | 2 +- Demos/Demo.cs | 2 +- Demos/Demos.csproj | 2 +- Demos/Demos/RopeTwistDemo.cs | 2 +- Demos/Program.cs | 2 +- 8 files changed, 53 insertions(+), 14 deletions(-) diff --git a/BepuPhysics/BepuPhysics.csproj b/BepuPhysics/BepuPhysics.csproj index bb1b09cdc..4b5f96148 100644 --- a/BepuPhysics/BepuPhysics.csproj +++ b/BepuPhysics/BepuPhysics.csproj @@ -28,12 +28,14 @@ true - embedded + full true TRACE;RELEASE;PROFILE + true + full true TRACE;RELEASE diff --git a/BepuPhysics/Solver_Solve.cs b/BepuPhysics/Solver_Solve.cs index fa5ca1def..a4684d10d 100644 --- a/BepuPhysics/Solver_Solve.cs +++ b/BepuPhysics/Solver_Solve.cs @@ -257,7 +257,7 @@ protected struct MultithreadingParameters public Buffer WorkerBoundsA; public Buffer WorkerBoundsB; - + public Buffer SyncStageWorkCounters; } protected MultithreadingParameters context; @@ -772,6 +772,10 @@ protected void ExecuteMultithreaded(float dt, IThreadDisp context.WorkerBoundsA[i] = new WorkerBounds { Min = int.MaxValue, Max = int.MinValue }; } + pool.Take(2, out context.SyncStageWorkCounters); + context.SyncStageWorkCounters[0] = -1; + context.SyncStageWorkCounters[1] = -1; + //While we could be a little more aggressive about culling work with this condition, it doesn't matter much. Have to do it for correctness; worker relies on it. if (ActiveSet.Batches.Count > 0) threadDispatcher.DispatchWorkers(workDelegate); @@ -787,6 +791,7 @@ protected void ExecuteMultithreaded(float dt, IThreadDisp pool.Return(ref context.BatchBoundaries); pool.Return(ref context.WorkerBoundsA); pool.Return(ref context.WorkerBoundsB); + pool.Return(ref context.SyncStageWorkCounters); } diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index 6aecbe926..06d3d3ce9 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -33,6 +33,35 @@ public virtual void SolveStep2(float dt, IThreadDispatcher threadDispatcher = nu } public class Solver : Solver where TIntegrationCallbacks : struct, IPoseIntegratorCallbacks { + void ExecuteStage(ref TStageFunction stageFunction, int workerIndex, int availableBlocksStartIndex, int availableBlocksCount, ref int syncStage) where TStageFunction : IStageFunction + { + var workCounterIndex = syncStage & 1; + while (true) + { + var workBlockIndexOffset = Interlocked.Increment(ref context.SyncStageWorkCounters[workCounterIndex]); //note counters are initialized to -1. + Debug.Assert(workBlockIndexOffset >= 0, "A stage cannot try to execute a work block that came before the current stage!"); + if (workBlockIndexOffset < availableBlocksCount) + { + stageFunction.Execute(this, availableBlocksStartIndex + workBlockIndexOffset, workerIndex); + } + else + { + //No more work available. + break; + } + } + + if (workerIndex == 0) + { + //Clear the work counter for the next sync stage. + context.SyncStageWorkCounters[workCounterIndex ^ 1] = -1; + } + InterstageSync(ref syncStage); + + + + } + public Solver(Bodies bodies, BufferPool pool, int iterationCount, int fallbackBatchThreshold, int initialCapacity, int initialIslandCapacity, @@ -208,9 +237,10 @@ void SolveStep2Worker(int workerIndex) { if (i > 0) { - ExecuteStage( - ref incrementalUpdateStage, ref context.IncrementalUpdateBlocks, ref bounds, ref boundsBackBuffer, workerIndex, 0, context.IncrementalUpdateBlocks.Blocks.Count, - ref incrementalUpdateWorkerStart, ref syncStage, incrementalClaimedState, incrementalUnclaimedState); + ExecuteStage(ref incrementalUpdateStage, workerIndex, 0, context.IncrementalUpdateBlocks.Blocks.Count, ref syncStage); + //ExecuteStage( + // ref incrementalUpdateStage, ref context.IncrementalUpdateBlocks, ref bounds, ref boundsBackBuffer, workerIndex, 0, context.IncrementalUpdateBlocks.Blocks.Count, + // ref incrementalUpdateWorkerStart, ref syncStage, incrementalClaimedState, incrementalUnclaimedState); incrementalClaimedState ^= 1; incrementalUnclaimedState ^= 1; } @@ -218,8 +248,9 @@ void SolveStep2Worker(int workerIndex) for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) { var batchOffset = batchIndex > 0 ? context.BatchBoundaries[batchIndex - 1] : 0; - ExecuteStage(ref warmstartStage, ref context.ConstraintBlocks, ref bounds, ref boundsBackBuffer, workerIndex, batchOffset, context.BatchBoundaries[batchIndex], - ref batchStarts[batchIndex], ref syncStage, claimedState, unclaimedState); + ExecuteStage(ref warmstartStage, workerIndex, batchOffset, context.BatchBoundaries[batchIndex] - batchOffset, ref syncStage); + //ExecuteStage(ref warmstartStage, ref context.ConstraintBlocks, ref bounds, ref boundsBackBuffer, workerIndex, batchOffset, context.BatchBoundaries[batchIndex], + // ref batchStarts[batchIndex], ref syncStage, claimedState, unclaimedState); } claimedState ^= 1; unclaimedState ^= 1; @@ -228,8 +259,9 @@ void SolveStep2Worker(int workerIndex) for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) { var batchOffset = batchIndex > 0 ? context.BatchBoundaries[batchIndex - 1] : 0; - ExecuteStage(ref solveStage, ref context.ConstraintBlocks, ref bounds, ref boundsBackBuffer, workerIndex, batchOffset, context.BatchBoundaries[batchIndex], - ref batchStarts[batchIndex], ref syncStage, claimedState, unclaimedState); + ExecuteStage(ref solveStage, workerIndex, batchOffset, context.BatchBoundaries[batchIndex] - batchOffset, ref syncStage); + //ExecuteStage(ref solveStage, ref context.ConstraintBlocks, ref bounds, ref boundsBackBuffer, workerIndex, batchOffset, context.BatchBoundaries[batchIndex], + // ref batchStarts[batchIndex], ref syncStage, claimedState, unclaimedState); } claimedState ^= 1; unclaimedState ^= 1; diff --git a/BepuUtilities/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj index ff2af19d1..6769b7236 100644 --- a/BepuUtilities/BepuUtilities.csproj +++ b/BepuUtilities/BepuUtilities.csproj @@ -31,7 +31,7 @@ true TRACE;RELEASE true - embedded + full diff --git a/Demos/Demo.cs b/Demos/Demo.cs index fcc959c56..9189f4ba5 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -62,7 +62,7 @@ public virtual void Update(Window window, Camera camera, Input input, float dt) //fully decouple simulation and rendering rates across different threads. //(In either case, you'd also want to interpolate or extrapolate simulation results during rendering for smoothness.) //Note that taking steps of variable length can reduce stability. Gradual or one-off changes can work reasonably well. - Simulation.Timestep(1 / 60f); + Simulation.Timestep(1 / 60f, ThreadDispatcher); ////Here's an example of how it would look to use more frequent updates, but still with a fixed amount of time simulated per update call: //const float timeToSimulate = 1 / 60f; diff --git a/Demos/Demos.csproj b/Demos/Demos.csproj index 4c7e0b045..229b40de0 100644 --- a/Demos/Demos.csproj +++ b/Demos/Demos.csproj @@ -16,8 +16,8 @@ true TRACE;RELEASE - embedded true + full diff --git a/Demos/Demos/RopeTwistDemo.cs b/Demos/Demos/RopeTwistDemo.cs index 7068757be..c717f478c 100644 --- a/Demos/Demos/RopeTwistDemo.cs +++ b/Demos/Demos/RopeTwistDemo.cs @@ -173,7 +173,7 @@ public override void Update(Window window, Camera camera, Input input, float dt) //As of this writing, extremely high substep counts and extremely high batch counts with a relatively small simulations (like in this demo) //actually run faster on a single thread. The overhead of synchronizing the thread group thousands of times overwhelms the actual work being done. //On the upside, this means that adding more bodies uninvolved to this simulation is almost free if using multithreading. - Simulation.Timestep(1 / 60f); + Simulation.Timestep(1 / 60f, ThreadDispatcher); } } diff --git a/Demos/Program.cs b/Demos/Program.cs index 668a4ab43..d1cf79f64 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -21,7 +21,7 @@ static void Main(string[] args) } //HeadlessTest.Test(content, 4, 32, 512); //HeadlessTest.Test(content, 4, 32, 512); - //HeadlessTest.Test(content, 4, 32, 512); + HeadlessTest.Test(content, 4, 32, 512); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); From ca9f10ffd399657e8ebba9cca59a2a389f1195b7 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 14 Aug 2021 19:59:19 -0500 Subject: [PATCH 153/947] Sticky scheduling, but no workstealing. --- BepuPhysics/Solver_SubsteppingSolve2.cs | 31 ++++++++++--------------- Demos/Demos/NewtDemo.cs | 2 +- Demos/Program.cs | 2 +- 3 files changed, 14 insertions(+), 21 deletions(-) diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index 06d3d3ce9..bb2428695 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -33,22 +33,14 @@ public virtual void SolveStep2(float dt, IThreadDispatcher threadDispatcher = nu } public class Solver : Solver where TIntegrationCallbacks : struct, IPoseIntegratorCallbacks { - void ExecuteStage(ref TStageFunction stageFunction, int workerIndex, int availableBlocksStartIndex, int availableBlocksCount, ref int syncStage) where TStageFunction : IStageFunction + void ExecuteStage(ref TStageFunction stageFunction, int workerIndex, int workerCount, int availableBlocksStartIndex, int availableBlocksCount, ref int syncStage) where TStageFunction : IStageFunction { var workCounterIndex = syncStage & 1; - while (true) + int workBlockIndexOffset = workerIndex; + while (workBlockIndexOffset < availableBlocksCount) { - var workBlockIndexOffset = Interlocked.Increment(ref context.SyncStageWorkCounters[workCounterIndex]); //note counters are initialized to -1. - Debug.Assert(workBlockIndexOffset >= 0, "A stage cannot try to execute a work block that came before the current stage!"); - if (workBlockIndexOffset < availableBlocksCount) - { - stageFunction.Execute(this, availableBlocksStartIndex + workBlockIndexOffset, workerIndex); - } - else - { - //No more work available. - break; - } + stageFunction.Execute(this, availableBlocksStartIndex + workBlockIndexOffset, workerIndex); + workBlockIndexOffset += workerCount; } if (workerIndex == 0) @@ -183,7 +175,8 @@ public void Execute(Solver solver, int blockIndex, int workerIndex) Action solveStep2Worker; void SolveStep2Worker(int workerIndex) { - int fallbackStart = GetUniformlyDistributedStart(workerIndex, context.FallbackBlocks.Blocks.Count, context.WorkerCount, 0); + int workerCount = context.WorkerCount; + int fallbackStart = GetUniformlyDistributedStart(workerIndex, context.FallbackBlocks.Blocks.Count, workerCount, 0); Buffer batchStarts; ref var activeSet = ref ActiveSet; unsafe @@ -195,7 +188,7 @@ void SolveStep2Worker(int workerIndex) { var batchOffset = batchIndex > 0 ? context.BatchBoundaries[batchIndex - 1] : 0; var batchCount = context.BatchBoundaries[batchIndex] - batchOffset; - batchStarts[batchIndex] = GetUniformlyDistributedStart(workerIndex, batchCount, context.WorkerCount, batchOffset); + batchStarts[batchIndex] = GetUniformlyDistributedStart(workerIndex, batchCount, workerCount, batchOffset); } int syncStage = 0; @@ -232,12 +225,12 @@ void SolveStep2Worker(int workerIndex) var incrementalClaimedState = 1; int incrementalUnclaimedState = 0; - var incrementalUpdateWorkerStart = GetUniformlyDistributedStart(workerIndex, context.IncrementalUpdateBlocks.Blocks.Count, context.WorkerCount, 0); + var incrementalUpdateWorkerStart = GetUniformlyDistributedStart(workerIndex, context.IncrementalUpdateBlocks.Blocks.Count, workerCount, 0); for (int i = 0; i < substepCount; ++i) { if (i > 0) { - ExecuteStage(ref incrementalUpdateStage, workerIndex, 0, context.IncrementalUpdateBlocks.Blocks.Count, ref syncStage); + ExecuteStage(ref incrementalUpdateStage, workerIndex, workerCount, 0, context.IncrementalUpdateBlocks.Blocks.Count, ref syncStage); //ExecuteStage( // ref incrementalUpdateStage, ref context.IncrementalUpdateBlocks, ref bounds, ref boundsBackBuffer, workerIndex, 0, context.IncrementalUpdateBlocks.Blocks.Count, // ref incrementalUpdateWorkerStart, ref syncStage, incrementalClaimedState, incrementalUnclaimedState); @@ -248,7 +241,7 @@ void SolveStep2Worker(int workerIndex) for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) { var batchOffset = batchIndex > 0 ? context.BatchBoundaries[batchIndex - 1] : 0; - ExecuteStage(ref warmstartStage, workerIndex, batchOffset, context.BatchBoundaries[batchIndex] - batchOffset, ref syncStage); + ExecuteStage(ref warmstartStage, workerIndex, workerCount, batchOffset, context.BatchBoundaries[batchIndex] - batchOffset, ref syncStage); //ExecuteStage(ref warmstartStage, ref context.ConstraintBlocks, ref bounds, ref boundsBackBuffer, workerIndex, batchOffset, context.BatchBoundaries[batchIndex], // ref batchStarts[batchIndex], ref syncStage, claimedState, unclaimedState); } @@ -259,7 +252,7 @@ void SolveStep2Worker(int workerIndex) for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) { var batchOffset = batchIndex > 0 ? context.BatchBoundaries[batchIndex - 1] : 0; - ExecuteStage(ref solveStage, workerIndex, batchOffset, context.BatchBoundaries[batchIndex] - batchOffset, ref syncStage); + ExecuteStage(ref solveStage, workerIndex, workerCount, batchOffset, context.BatchBoundaries[batchIndex] - batchOffset, ref syncStage); //ExecuteStage(ref solveStage, ref context.ConstraintBlocks, ref bounds, ref boundsBackBuffer, workerIndex, batchOffset, context.BatchBoundaries[batchIndex], // ref batchStarts[batchIndex], ref syncStage, claimedState, unclaimedState); } diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index 45d4f49b1..c5eb915ae 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -824,7 +824,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper(1)); - Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0), 0, 0), new EmbeddedSubsteppingTimestepper2(3), solverIterationCount: 1); + Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0), 0, 0), new EmbeddedSubsteppingTimestepper2(3), solverIterationCount: 1); //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper2(), solverIterationCount: 60); //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)), new SubsteppingTimestepper2(3), solverIterationCount: 1); diff --git a/Demos/Program.cs b/Demos/Program.cs index d1cf79f64..668a4ab43 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -21,7 +21,7 @@ static void Main(string[] args) } //HeadlessTest.Test(content, 4, 32, 512); //HeadlessTest.Test(content, 4, 32, 512); - HeadlessTest.Test(content, 4, 32, 512); + //HeadlessTest.Test(content, 4, 32, 512); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); From 865d3080715361e4ff12774172aeb73c389dc7e6 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 16 Aug 2021 16:51:32 -0500 Subject: [PATCH 154/947] Does not run. Working towards lower overhead solver work scheduling. --- BepuPhysics/Solver_Solve.cs | 8 +- BepuPhysics/Solver_SubsteppingSolve2.cs | 282 +++++++++++++++++++++++- Demos/Program.cs | 2 +- 3 files changed, 277 insertions(+), 15 deletions(-) diff --git a/BepuPhysics/Solver_Solve.cs b/BepuPhysics/Solver_Solve.cs index a4684d10d..f99f80e4b 100644 --- a/BepuPhysics/Solver_Solve.cs +++ b/BepuPhysics/Solver_Solve.cs @@ -420,11 +420,10 @@ public void Execute(Solver solver, int blockIndex, int workerIndex) } - //TODO: It's very likely that this spin wait isn't ideal for some newer systems like threadripper. /// /// Behaves like a framework SpinWait, but never voluntarily relinquishes the timeslice to off-core threads. /// - /// There are three big reasons for using this over the regular framework SpinWait: + /// There are two big reasons for using this over the regular framework SpinWait: /// 1) The framework spinwait relies on spins for quite a while before resorting to any form of timeslice surrender. /// Empirically, this is not ideal for the solver- if the sync condition isn't met within several nanoseconds, it will tend to be some microseconds away. /// This spinwait is much more aggressive about moving to yields. @@ -432,11 +431,10 @@ public void Execute(Solver solver, int blockIndex, int workerIndex) /// This widens the potential set of schedulable threads to those not native to the current core. If we permit that transition, it is likely to evict cached solver data. /// (For very large simulations, the use of Sleep(0) isn't that concerning- every iteration can be large enough to evict all of cache- /// but there still isn't much benefit to using it over yields in context.) - /// 3) After a particularly long wait, the framework SpinWait resorts to Sleep(1). This is catastrophic for the solver- worse than merely interfering with cached data, - /// it also simply prevents the thread from being rescheduled for an extremely long period of time (potentially most of a frame!) under the default clock resolution. + /// SpinWait will also fall back to Sleep(1) by default which obliterates performance, but that behavior can be disabled. /// Note that this isn't an indication that the framework SpinWait should be changed, but rather that the solver's requirements are extremely specific and don't match /// a general purpose solution very well. - struct LocalSpinWait + protected struct LocalSpinWait { public int WaitCount; diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index bb2428695..3c5a94fd6 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -35,7 +35,6 @@ public class Solver : Solver where TIntegrationCallbacks { void ExecuteStage(ref TStageFunction stageFunction, int workerIndex, int workerCount, int availableBlocksStartIndex, int availableBlocksCount, ref int syncStage) where TStageFunction : IStageFunction { - var workCounterIndex = syncStage & 1; int workBlockIndexOffset = workerIndex; while (workBlockIndexOffset < availableBlocksCount) { @@ -43,15 +42,7 @@ void ExecuteStage(ref TStageFunction stageFunction, int workerIn workBlockIndexOffset += workerCount; } - if (workerIndex == 0) - { - //Clear the work counter for the next sync stage. - context.SyncStageWorkCounters[workCounterIndex ^ 1] = -1; - } InterstageSync(ref syncStage); - - - } public Solver(Bodies bodies, BufferPool pool, int iterationCount, int fallbackBatchThreshold, @@ -262,6 +253,279 @@ void SolveStep2Worker(int workerIndex) } } + void ExecuteWorkerStage(ref TStageFunction stageFunction, int workerIndex, int workerStart, int availableBlocksStartIndex, int availableBlocksCount, ref Buffer claims, ref int completedWorkBlocks) where TStageFunction : IStageFunction + { + if (workerStart == -1) + { + //Thread count exceeds work block count; nothing for this worker to do. + //(Technically, there's a possibility that an earlier thread would fail to wake and allowing this thread to steal that block COULD help, + //but on average it's better to keep jobs scheduled to the same core to avoid excess memory traffic, and we can rely on some other active worker to take care of it.) + return; + } + int workBlockIndex = workerStart; + var availableBlocksEndIndex = availableBlocksStartIndex + availableBlocksCount; + int locallyCompletedCount = 0; + //Try to claim blocks by traversing forward until we're blocked by another claim. + while (Interlocked.CompareExchange(ref claims[workBlockIndex], -1, 0) == 0) + { + //Successfully claimed a work block. + stageFunction.Execute(this, workBlockIndex, workerIndex); + ++locallyCompletedCount; + workBlockIndex++; + if (workBlockIndex >= availableBlocksEndIndex) + { + //Wrap around. + workBlockIndex = 0; + } + } + //TODO: Technically, given wrap around of forward traversal, backwards looping seems.. questionable. Verify. + //Try to claim work blocks going backward. + workBlockIndex = workerStart - 1; + while (true) + { + if (workBlockIndex < 0) + { + //Wrap around. + workBlockIndex = claims.Length - 1; + } + if (Interlocked.CompareExchange(ref claims[workBlockIndex], -1, 0) != 0) + { + break; + } + //Successfully claimed a work block. + stageFunction.Execute(this, workBlockIndex, workerIndex); + ++locallyCompletedCount; + workBlockIndex--; + } + //No more adjacent work blocks are available. This thread is done! + Interlocked.Add(ref completedWorkBlocks, locallyCompletedCount); + } + void ExecuteMainStage(ref TStageFunction stageFunction, int workerIndex, int workerStart, int availableBlocksStartIndex, int availableBlocksCount, ref int stageIndex) where TStageFunction : IStageFunction + { + Debug.Assert(availableBlocksCount > 0, "We assume any zero count batches are not included in the stages available to execute."); + if (availableBlocksCount == 1) + { + //There is only one work block available. There's no reason to notify other threads about it or do any claims management; just execute it sequentially. + stageFunction.Execute(this, availableBlocksStartIndex, workerIndex); + } + else + { + //Write the new stage index so other spinning threads will begin work on it. + Volatile.Write(ref substepContext.StageIndex, stageIndex); + ref var stage = ref substepContext.Stages[stageIndex]; + ExecuteWorkerStage(ref stageFunction, workerIndex, workerStart, availableBlocksStartIndex, availableBlocksCount, ref stage.Claims, ref stage.CompletedWorkBlockCount); + + //Since we asked other threads to do work, we must wait until the requested work is done before proceeding. + //Note that we DO NOT yield on the main thread! + //This significantly increases the chance *some* progress will be made on the available work, even if all other workers are stuck unscheduled. + //The reasoning here is that the OS is not likely to unschedule an active thread, but will be far less aggressive about scheduling a *currently unscheduled* thread. + //Critically, yielding threads are not in any kind of execution queue- from the OS's perspective, they aren't asking to be woken up. + //If another thread comes in with significant work, they could be stalled for (from the solver's perspective) an arbitrarily long time. + //By having the main thread never yield, the only way for all progress to halt is for the OS to aggressively unschedule the main thread. + //That is very rare when dealing with CPUs with plenty of cores to go around relative to the scheduled work. + //(Why not notify the OS that waiting threads actually want to be executed? Just overhead. Feel free to experiment with different approaches, but so far this has won empirically.) + while (Volatile.Read(ref stage.CompletedWorkBlockCount) != availableBlocksCount) + { + Thread.SpinWait(3); + } + } + ++stageIndex; + } + enum JobType + { + IncrementalUpdate, + WarmStart, + WarmStartFallback, + Solve, + SolveFallback, + } + + [StructLayout(LayoutKind.Explicit)] + struct SyncStage + { + [FieldOffset(0)] + public Buffer Claims; + + [FieldOffset(20)] + public JobType JobType; + + [FieldOffset(24)] + public int BatchIndex; + + [FieldOffset(128)] + public int CompletedWorkBlockCount; + + public SyncStage(BufferPool pool, int claimCount, JobType jobType, int batchIndex = 0) + { + CompletedWorkBlockCount = 0; + JobType = jobType; + BatchIndex = batchIndex; + pool.Take(claimCount, out Claims); + } + public void Dispose(BufferPool pool) + { + pool.Return(ref Claims); + } + } + + [StructLayout(LayoutKind.Explicit)] + struct SubstepMultithreadingContext + { + [FieldOffset(0)] + public Buffer Stages; + [FieldOffset(16)] + public WorkBlocks IncrementalUpdateBlocks; + [FieldOffset(64)] + public WorkBlocks ConstraintBlocks; + [FieldOffset(112)] + public WorkBlocks FallbackBlocks; + [FieldOffset(160)] + public Buffer BatchBoundaries; + [FieldOffset(176)] + public float Dt; + [FieldOffset(180)] + public float InverseDt; + + //Stage index is written; don't want to infest any of the more frequently read properties, so it's shoved out of any dangerous cache line. + [FieldOffset(256)] + public int StageIndex; + } + + SubstepMultithreadingContext substepContext; + + + void SolveStep2Worker2(int workerIndex) + { + //The solver has two codepaths: one thread, acting as an orchestrator, and the others, just waiting to be used. + //There is no requirement that a worker thread above index 0 actually runs at all for a given dispatch. + //If a worker fails to schedule for a long time because the OS went with a different thread, that's perfectly fine- + //another thread will consume the work that would have otherwise been handled by it, and the execution as a whole + //will continue on unimpeded. + //There's still nothing done if the OS unschedules an active worker that claimed work, but that's a far, far rarer concern. + //Note that this attempts to maintain a given worker's relationship to a set of work blocks. This increases the probability that + //data will remain in some cache that's reasonably close to the core. + int workerCount = context.WorkerCount; + var incrementalUpdateWorkerStart = GetUniformlyDistributedStart(workerIndex, substepContext.IncrementalUpdateBlocks.Blocks.Count, workerCount, 0); + int fallbackStart = GetUniformlyDistributedStart(workerIndex, substepContext.FallbackBlocks.Blocks.Count, workerCount, 0); + Buffer batchStarts; + ref var activeSet = ref ActiveSet; + unsafe + { + var batchStartsData = stackalloc int[activeSet.Batches.Count]; + batchStarts = new Buffer(batchStartsData, activeSet.Batches.Count); + } + for (int batchIndex = 0; batchIndex < activeSet.Batches.Count; ++batchIndex) + { + var batchOffset = batchIndex > 0 ? context.BatchBoundaries[batchIndex - 1] : 0; + var batchCount = substepContext.BatchBoundaries[batchIndex] - batchOffset; + batchStarts[batchIndex] = GetUniformlyDistributedStart(workerIndex, batchCount, workerCount, batchOffset); + } + + Debug.Assert(activeSet.Batches.Count > 0, "Don't dispatch if there are no constraints."); + GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); + + var warmstartStage = new WarmStartStep2StageFunction + { + Dt = substepContext.Dt, + InverseDt = substepContext.InverseDt, + solver = this + }; + var solveStage = new SolveStep2StageFunction + { + Dt = substepContext.Dt, + InverseDt = substepContext.InverseDt, + solver = this + }; + var incrementalUpdateStage = new IncrementalUpdateStageFunction + { + Dt = substepContext.Dt, + InverseDt = substepContext.InverseDt, + solver = this + }; + + if (workerIndex == 0) + { + int stageIndex = 0; + //This is the main 'orchestrator' thread. It tracks execution progress and notifies other threads that's it's time to work. + for (int i = 0; i < substepCount; ++i) + { + if (i > 0) + { + ExecuteMainStage(ref incrementalUpdateStage, workerIndex, workerCount, 0, substepContext.IncrementalUpdateBlocks.Blocks.Count, ref stageIndex); + } + warmstartStage.SubstepIndex = i; + for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) + { + var batchOffset = batchIndex > 0 ? context.BatchBoundaries[batchIndex - 1] : 0; + ExecuteMainStage(ref warmstartStage, workerIndex, workerCount, batchOffset, substepContext.BatchBoundaries[batchIndex] - batchOffset, ref stageIndex); + } + for (int j = 0; j < IterationCount; ++j) + { + for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) + { + var batchOffset = batchIndex > 0 ? context.BatchBoundaries[batchIndex - 1] : 0; + ExecuteMainStage(ref solveStage, workerIndex, workerCount, batchOffset, substepContext.BatchBoundaries[batchIndex] - batchOffset, ref stageIndex); + } + } + } + } + else + { + //This is a worker thread. It does not need to track execution progress; it only checks to see if there's any work that needs to be done, and if there is, does it, then goes back into a wait. + int latestCompletedStageIndex = -1; + while (true) + { + var spinWait = new LocalSpinWait(); + int stageIndex; + while (latestCompletedStageIndex == (stageIndex = Volatile.Read(ref substepContext.StageIndex))) + { + //No work yet available. + spinWait.SpinOnce(); + } + //Stages were set up prior to execution. Note that we don't attempt to ping pong buffers or anything; there are unique entries for every single stage. + //This guarantees that a worker thread can go idle and miss an arbitrary number of stages without blocking any progress. + if (stageIndex == substepContext.Stages.Length) + { + //No more stages; exit the work loop. + break; + } + var stage = substepContext.Stages[stageIndex]; + switch (stage.JobType) + { + case JobType.IncrementalUpdate: + { + ExecuteWorkerStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStart, 0, substepContext.IncrementalUpdateBlocks.Blocks.Count, ref stage.Claims, ref stage.CompletedWorkBlockCount); + } + break; + case JobType.WarmStart: + { + var batchStart = stage.BatchIndex > 0 ? context.BatchBoundaries[stage.BatchIndex - 1] : 0; + ExecuteWorkerStage(ref warmstartStage, workerIndex, batchStarts[stage.BatchIndex], batchStart, substepContext.BatchBoundaries[stage.BatchIndex] - batchStart, ref stage.Claims, ref stage.CompletedWorkBlockCount); + } + break; + case JobType.WarmStartFallback: + { + Debug.Fail("Not yet supported."); + } + break; + case JobType.Solve: + { + var batchStart = stage.BatchIndex > 0 ? context.BatchBoundaries[stage.BatchIndex - 1] : 0; + ExecuteWorkerStage(ref solveStage, workerIndex, batchStarts[stage.BatchIndex], batchStart, substepContext.BatchBoundaries[stage.BatchIndex] - batchStart, ref stage.Claims, ref stage.CompletedWorkBlockCount); + } + break; + case JobType.SolveFallback: + { + Debug.Fail("Not yet supported."); + } + break; + } + + } + } + + } + unsafe bool ComputeIntegrationResponsibilitiesForConstraintRegion(int batchIndex, int typeBatchIndex, int constraintStart, int exclusiveConstraintEnd) { diff --git a/Demos/Program.cs b/Demos/Program.cs index 668a4ab43..d1cf79f64 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -21,7 +21,7 @@ static void Main(string[] args) } //HeadlessTest.Test(content, 4, 32, 512); //HeadlessTest.Test(content, 4, 32, 512); - //HeadlessTest.Test(content, 4, 32, 512); + HeadlessTest.Test(content, 4, 32, 512); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); From 285e72cd188b858df0b4d8d3725a49e9a1f51fd8 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 16 Aug 2021 19:20:37 -0500 Subject: [PATCH 155/947] New workblock scheduling running. Not yet fine tuned, but obviously better. --- BepuPhysics/Solver_Solve.cs | 4 +- BepuPhysics/Solver_SubsteppingSolve2.cs | 349 ++++++++++++++++-------- Demos/Demos/RopeTwistDemo.cs | 2 +- Demos/Program.cs | 2 +- 4 files changed, 241 insertions(+), 116 deletions(-) diff --git a/BepuPhysics/Solver_Solve.cs b/BepuPhysics/Solver_Solve.cs index f99f80e4b..e91fb7f04 100644 --- a/BepuPhysics/Solver_Solve.cs +++ b/BepuPhysics/Solver_Solve.cs @@ -74,7 +74,7 @@ public partial class Solver //Without sticky scheduling, memory bandwidth use could skyrocket during iterations as the L3 gets missed over and over. - protected struct WorkBlock + protected internal struct WorkBlock { public int BatchIndex; public int TypeBatchIndex; @@ -88,7 +88,7 @@ protected struct WorkBlock public int End; } - protected struct FallbackScatterWorkBlock + protected internal struct FallbackScatterWorkBlock { public int Start; public int End; diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index 3c5a94fd6..83ac0f822 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -12,9 +12,63 @@ using System.Runtime.Intrinsics.X86; using System.Numerics; using System.Runtime.Intrinsics; +using static BepuPhysics.Solver; namespace BepuPhysics { + internal enum SolverJobType + { + IncrementalUpdate, + WarmStart, + WarmStartFallback, + Solve, + SolveFallback, + } + + [StructLayout(LayoutKind.Explicit)] + internal struct SolverSyncStage + { + [FieldOffset(0)] + public Buffer Claims; + [FieldOffset(20)] + public SolverJobType JobType; + [FieldOffset(24)] + public int BatchIndex; + [FieldOffset(28)] + public int WorkBlockStartIndex; + [FieldOffset(32)] + public int SubstepIndex; + + [FieldOffset(128)] + public int CompletedWorkBlockCount; + } + + [StructLayout(LayoutKind.Explicit)] + internal struct SubstepMultithreadingContext + { + [FieldOffset(0)] + public Buffer Stages; + [FieldOffset(16)] + public Buffer IncrementalUpdateBlocks; + [FieldOffset(32)] + public Buffer ConstraintBlocks; + [FieldOffset(48)] + public Buffer FallbackBlocks; + [FieldOffset(64)] + public Buffer ConstraintBatchBoundaries; + [FieldOffset(80)] + public float Dt; + [FieldOffset(84)] + public float InverseDt; + [FieldOffset(88)] + public int WorkerCount; + + //Stage index is written during multithreaded execution; don't want to infest any of the more frequently read properties, so it's shoved out of any dangerous cache line. + [FieldOffset(256)] + public int StageIndex; + } + + public partial class Solver { public virtual IndexSet PrepareConstraintIntegrationResponsibilities(int substepCount, IThreadDispatcher threadDispatcher = null) @@ -53,6 +107,7 @@ public Solver(Bodies bodies, BufferPool pool, int iterationCount, int fallbackBa { PoseIntegrator = poseIntegrator; solveStep2Worker = SolveStep2Worker; + solveStep2Worker2 = SolveStep2Worker2; constraintIntegrationResponsibilitiesWorker = ConstraintIntegrationResponsibilitiesWorker; } @@ -101,7 +156,7 @@ struct WarmStartStep2StageFunction : IStageFunction [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Execute(Solver solver, int blockIndex, int workerIndex) { - ref var block = ref this.solver.context.ConstraintBlocks.Blocks[blockIndex]; + ref var block = ref this.solver.substepContext.ConstraintBlocks[blockIndex]; ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; if (SubstepIndex == 0) @@ -115,7 +170,6 @@ public void Execute(Solver solver, int blockIndex, int workerIndex) } } - //no fallback warmstart; the last constraint batch is always handled by the solve instead, and if the fallback batch exists, it's guaranteed to be the last batch. struct SolveStep2StageFunction : IStageFunction { @@ -126,7 +180,7 @@ struct SolveStep2StageFunction : IStageFunction [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Execute(Solver solver, int blockIndex, int workerIndex) { - ref var block = ref this.solver.context.ConstraintBlocks.Blocks[blockIndex]; + ref var block = ref this.solver.substepContext.ConstraintBlocks[blockIndex]; ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; typeProcessor.SolveStep2(ref typeBatch, solver.bodies, Dt, InverseDt, block.StartBundle, block.End); @@ -157,7 +211,7 @@ struct IncrementalUpdateStageFunction : IStageFunction [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Execute(Solver solver, int blockIndex, int workerIndex) { - ref var block = ref this.solver.context.IncrementalUpdateBlocks.Blocks[blockIndex]; + ref var block = ref this.solver.substepContext.IncrementalUpdateBlocks[blockIndex]; ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; solver.TypeProcessors[typeBatch.TypeId].IncrementallyUpdateContactData(ref typeBatch, solver.bodies, Dt, InverseDt, block.StartBundle, block.End); } @@ -217,9 +271,9 @@ void SolveStep2Worker(int workerIndex) int incrementalUnclaimedState = 0; var incrementalUpdateWorkerStart = GetUniformlyDistributedStart(workerIndex, context.IncrementalUpdateBlocks.Blocks.Count, workerCount, 0); - for (int i = 0; i < substepCount; ++i) + for (int substepIndex = 0; substepIndex < substepCount; ++substepIndex) { - if (i > 0) + if (substepIndex > 0) { ExecuteStage(ref incrementalUpdateStage, workerIndex, workerCount, 0, context.IncrementalUpdateBlocks.Blocks.Count, ref syncStage); //ExecuteStage( @@ -228,7 +282,7 @@ void SolveStep2Worker(int workerIndex) incrementalClaimedState ^= 1; incrementalUnclaimedState ^= 1; } - warmstartStage.SubstepIndex = i; + warmstartStage.SubstepIndex = substepIndex; for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) { var batchOffset = batchIndex > 0 ? context.BatchBoundaries[batchIndex - 1] : 0; @@ -253,7 +307,7 @@ void SolveStep2Worker(int workerIndex) } } - void ExecuteWorkerStage(ref TStageFunction stageFunction, int workerIndex, int workerStart, int availableBlocksStartIndex, int availableBlocksCount, ref Buffer claims, ref int completedWorkBlocks) where TStageFunction : IStageFunction + void ExecuteWorkerStage(ref TStageFunction stageFunction, int workerIndex, int workerStart, int availableBlocksStartIndex, ref Buffer claims, ref int completedWorkBlocks) where TStageFunction : IStageFunction { if (workerStart == -1) { @@ -263,16 +317,15 @@ void ExecuteWorkerStage(ref TStageFunction stageFunction, int wo return; } int workBlockIndex = workerStart; - var availableBlocksEndIndex = availableBlocksStartIndex + availableBlocksCount; int locallyCompletedCount = 0; //Try to claim blocks by traversing forward until we're blocked by another claim. while (Interlocked.CompareExchange(ref claims[workBlockIndex], -1, 0) == 0) { //Successfully claimed a work block. - stageFunction.Execute(this, workBlockIndex, workerIndex); + stageFunction.Execute(this, availableBlocksStartIndex + workBlockIndex, workerIndex); ++locallyCompletedCount; workBlockIndex++; - if (workBlockIndex >= availableBlocksEndIndex) + if (workBlockIndex >= claims.Length) { //Wrap around. workBlockIndex = 0; @@ -293,16 +346,24 @@ void ExecuteWorkerStage(ref TStageFunction stageFunction, int wo break; } //Successfully claimed a work block. - stageFunction.Execute(this, workBlockIndex, workerIndex); + stageFunction.Execute(this, availableBlocksStartIndex + workBlockIndex, workerIndex); ++locallyCompletedCount; workBlockIndex--; } //No more adjacent work blocks are available. This thread is done! Interlocked.Add(ref completedWorkBlocks, locallyCompletedCount); } - void ExecuteMainStage(ref TStageFunction stageFunction, int workerIndex, int workerStart, int availableBlocksStartIndex, int availableBlocksCount, ref int stageIndex) where TStageFunction : IStageFunction + void ExecuteMainStage(ref TStageFunction stageFunction, int workerIndex, int workerStart, int availableBlocksStartIndex, ref SolverSyncStage stage, int stageIndex) where TStageFunction : IStageFunction { + var availableBlocksCount = stage.Claims.Length; Debug.Assert(availableBlocksCount > 0, "We assume any zero count batches are not included in the stages available to execute."); + + //for (int i = 0; i < availableBlocksCount; ++i) + //{ + // stageFunction.Execute(this, availableBlocksStartIndex + i, workerIndex); + //} + //return; + if (availableBlocksCount == 1) { //There is only one work block available. There's no reason to notify other threads about it or do any claims management; just execute it sequentially. @@ -312,8 +373,7 @@ void ExecuteMainStage(ref TStageFunction stageFunction, int work { //Write the new stage index so other spinning threads will begin work on it. Volatile.Write(ref substepContext.StageIndex, stageIndex); - ref var stage = ref substepContext.Stages[stageIndex]; - ExecuteWorkerStage(ref stageFunction, workerIndex, workerStart, availableBlocksStartIndex, availableBlocksCount, ref stage.Claims, ref stage.CompletedWorkBlockCount); + ExecuteWorkerStage(ref stageFunction, workerIndex, workerStart, availableBlocksStartIndex, ref stage.Claims, ref stage.CompletedWorkBlockCount); //Since we asked other threads to do work, we must wait until the requested work is done before proceeding. //Note that we DO NOT yield on the main thread! @@ -329,71 +389,11 @@ void ExecuteMainStage(ref TStageFunction stageFunction, int work Thread.SpinWait(3); } } - ++stageIndex; - } - enum JobType - { - IncrementalUpdate, - WarmStart, - WarmStartFallback, - Solve, - SolveFallback, } - - [StructLayout(LayoutKind.Explicit)] - struct SyncStage - { - [FieldOffset(0)] - public Buffer Claims; - - [FieldOffset(20)] - public JobType JobType; - - [FieldOffset(24)] - public int BatchIndex; - - [FieldOffset(128)] - public int CompletedWorkBlockCount; - - public SyncStage(BufferPool pool, int claimCount, JobType jobType, int batchIndex = 0) - { - CompletedWorkBlockCount = 0; - JobType = jobType; - BatchIndex = batchIndex; - pool.Take(claimCount, out Claims); - } - public void Dispose(BufferPool pool) - { - pool.Return(ref Claims); - } - } - - [StructLayout(LayoutKind.Explicit)] - struct SubstepMultithreadingContext - { - [FieldOffset(0)] - public Buffer Stages; - [FieldOffset(16)] - public WorkBlocks IncrementalUpdateBlocks; - [FieldOffset(64)] - public WorkBlocks ConstraintBlocks; - [FieldOffset(112)] - public WorkBlocks FallbackBlocks; - [FieldOffset(160)] - public Buffer BatchBoundaries; - [FieldOffset(176)] - public float Dt; - [FieldOffset(180)] - public float InverseDt; - - //Stage index is written; don't want to infest any of the more frequently read properties, so it's shoved out of any dangerous cache line. - [FieldOffset(256)] - public int StageIndex; - } - SubstepMultithreadingContext substepContext; + Action solveStep2Worker2; void SolveStep2Worker2(int workerIndex) { //The solver has two codepaths: one thread, acting as an orchestrator, and the others, just waiting to be used. @@ -404,9 +404,9 @@ void SolveStep2Worker2(int workerIndex) //There's still nothing done if the OS unschedules an active worker that claimed work, but that's a far, far rarer concern. //Note that this attempts to maintain a given worker's relationship to a set of work blocks. This increases the probability that //data will remain in some cache that's reasonably close to the core. - int workerCount = context.WorkerCount; - var incrementalUpdateWorkerStart = GetUniformlyDistributedStart(workerIndex, substepContext.IncrementalUpdateBlocks.Blocks.Count, workerCount, 0); - int fallbackStart = GetUniformlyDistributedStart(workerIndex, substepContext.FallbackBlocks.Blocks.Count, workerCount, 0); + int workerCount = substepContext.WorkerCount; + var incrementalUpdateWorkerStart = GetUniformlyDistributedStart(workerIndex, substepContext.IncrementalUpdateBlocks.Length, workerCount, 0); + int fallbackStart = GetUniformlyDistributedStart(workerIndex, substepContext.FallbackBlocks.Length, workerCount, 0); Buffer batchStarts; ref var activeSet = ref ActiveSet; unsafe @@ -416,9 +416,9 @@ void SolveStep2Worker2(int workerIndex) } for (int batchIndex = 0; batchIndex < activeSet.Batches.Count; ++batchIndex) { - var batchOffset = batchIndex > 0 ? context.BatchBoundaries[batchIndex - 1] : 0; - var batchCount = substepContext.BatchBoundaries[batchIndex] - batchOffset; - batchStarts[batchIndex] = GetUniformlyDistributedStart(workerIndex, batchCount, workerCount, batchOffset); + var batchOffset = batchIndex > 0 ? substepContext.ConstraintBatchBoundaries[batchIndex - 1] : 0; + var batchCount = substepContext.ConstraintBatchBoundaries[batchIndex] - batchOffset; + batchStarts[batchIndex] = GetUniformlyDistributedStart(workerIndex, batchCount, workerCount, 0); } Debug.Assert(activeSet.Batches.Count > 0, "Don't dispatch if there are no constraints."); @@ -445,29 +445,42 @@ void SolveStep2Worker2(int workerIndex) if (workerIndex == 0) { - int stageIndex = 0; //This is the main 'orchestrator' thread. It tracks execution progress and notifies other threads that's it's time to work. - for (int i = 0; i < substepCount; ++i) + for (int stageIndex = 0; stageIndex < substepContext.Stages.Length; ++stageIndex) { - if (i > 0) - { - ExecuteMainStage(ref incrementalUpdateStage, workerIndex, workerCount, 0, substepContext.IncrementalUpdateBlocks.Blocks.Count, ref stageIndex); - } - warmstartStage.SubstepIndex = i; - for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) - { - var batchOffset = batchIndex > 0 ? context.BatchBoundaries[batchIndex - 1] : 0; - ExecuteMainStage(ref warmstartStage, workerIndex, workerCount, batchOffset, substepContext.BatchBoundaries[batchIndex] - batchOffset, ref stageIndex); - } - for (int j = 0; j < IterationCount; ++j) + ref var stage = ref substepContext.Stages[stageIndex]; + switch (stage.JobType) { - for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) - { - var batchOffset = batchIndex > 0 ? context.BatchBoundaries[batchIndex - 1] : 0; - ExecuteMainStage(ref solveStage, workerIndex, workerCount, batchOffset, substepContext.BatchBoundaries[batchIndex] - batchOffset, ref stageIndex); - } + case SolverJobType.IncrementalUpdate: + { + ExecuteMainStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStart, 0, ref stage, stageIndex); + } + break; + case SolverJobType.WarmStart: + { + warmstartStage.SubstepIndex = stage.SubstepIndex; + ExecuteMainStage(ref warmstartStage, workerIndex, batchStarts[stage.BatchIndex], stage.WorkBlockStartIndex, ref stage, stageIndex); + } + break; + case SolverJobType.WarmStartFallback: + { + Debug.Fail("Not yet supported."); + } + break; + case SolverJobType.Solve: + { + ExecuteMainStage(ref solveStage, workerIndex, batchStarts[stage.BatchIndex], stage.WorkBlockStartIndex, ref stage, stageIndex); + } + break; + case SolverJobType.SolveFallback: + { + Debug.Fail("Not yet supported."); + } + break; } } + //All done; notify waiting threads to join. + Volatile.Write(ref substepContext.StageIndex, substepContext.Stages.Length); } else { @@ -489,40 +502,151 @@ void SolveStep2Worker2(int workerIndex) //No more stages; exit the work loop. break; } - var stage = substepContext.Stages[stageIndex]; + ref var stage = ref substepContext.Stages[stageIndex]; switch (stage.JobType) { - case JobType.IncrementalUpdate: + case SolverJobType.IncrementalUpdate: { - ExecuteWorkerStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStart, 0, substepContext.IncrementalUpdateBlocks.Blocks.Count, ref stage.Claims, ref stage.CompletedWorkBlockCount); + ExecuteWorkerStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStart, 0, ref stage.Claims, ref stage.CompletedWorkBlockCount); } break; - case JobType.WarmStart: + case SolverJobType.WarmStart: { - var batchStart = stage.BatchIndex > 0 ? context.BatchBoundaries[stage.BatchIndex - 1] : 0; - ExecuteWorkerStage(ref warmstartStage, workerIndex, batchStarts[stage.BatchIndex], batchStart, substepContext.BatchBoundaries[stage.BatchIndex] - batchStart, ref stage.Claims, ref stage.CompletedWorkBlockCount); + warmstartStage.SubstepIndex = stage.SubstepIndex; + ExecuteWorkerStage(ref warmstartStage, workerIndex, batchStarts[stage.BatchIndex], stage.WorkBlockStartIndex, ref stage.Claims, ref stage.CompletedWorkBlockCount); } break; - case JobType.WarmStartFallback: + case SolverJobType.WarmStartFallback: { Debug.Fail("Not yet supported."); } break; - case JobType.Solve: + case SolverJobType.Solve: { - var batchStart = stage.BatchIndex > 0 ? context.BatchBoundaries[stage.BatchIndex - 1] : 0; - ExecuteWorkerStage(ref solveStage, workerIndex, batchStarts[stage.BatchIndex], batchStart, substepContext.BatchBoundaries[stage.BatchIndex] - batchStart, ref stage.Claims, ref stage.CompletedWorkBlockCount); + ExecuteWorkerStage(ref solveStage, workerIndex, batchStarts[stage.BatchIndex], stage.WorkBlockStartIndex, ref stage.Claims, ref stage.CompletedWorkBlockCount); } break; - case JobType.SolveFallback: + case SolverJobType.SolveFallback: { Debug.Fail("Not yet supported."); } break; } + latestCompletedStageIndex = stageIndex; + + } + } + + } + + protected void ExecuteMultithreaded2(float dt, IThreadDispatcher threadDispatcher, Action workDelegate) + { + var workerCount = substepContext.WorkerCount = threadDispatcher.ThreadCount; + substepContext.Dt = dt; + substepContext.InverseDt = 1f / dt; + //First build a set of work blocks. + //The block size should be relatively small to give the workstealer something to do, but we don't want to go crazy with the number of blocks. + //These values are found by empirical tuning. The optimal values may vary by architecture. + //The goal here is to have just enough blocks that, in the event that we end up some underpowered threads (due to competition or hyperthreading), + //there are enough blocks that workstealing will still generally allow the extra threads to be useful. + const int targetBlocksPerBatchPerWorker = 1; + const int minimumBlockSizeInBundles = 1; + const int maximumBlockSizeInBundles = 1024; + var targetBlocksPerBatch = workerCount * targetBlocksPerBatchPerWorker; + var mainFilter = new MainSolveFilter(); + var incrementalFilter = new IncrementalContactDataUpdateFilter(); + BuildWorkBlocks(pool, minimumBlockSizeInBundles, maximumBlockSizeInBundles, targetBlocksPerBatch, ref mainFilter, out var constraintBlocks, out substepContext.ConstraintBatchBoundaries); + BuildWorkBlocks(pool, minimumBlockSizeInBundles, maximumBlockSizeInBundles, targetBlocksPerBatch, ref incrementalFilter, out var incrementalBlocks, out var incrementalUpdateBatchBoundaries); + pool.Return(ref incrementalUpdateBatchBoundaries); //TODO: No need to create this in the first place. Doesn't really cost anything, but... + substepContext.ConstraintBlocks = constraintBlocks.Span.Slice(constraintBlocks.Count); + substepContext.IncrementalUpdateBlocks = incrementalBlocks.Span.Slice(incrementalBlocks.Count); + + //Not every batch will actually have work blocks associated with it; the batch compressor could be falling behind, which means older constraints could be at higher batches than they need to be, leaving gaps. + //We don't want to include those empty batches as sync points in the solver. + var batchSyncPointCount = ActiveSet.Batches.Count; + for (int i = 0; i < ActiveSet.Batches.Count; ++i) + { + var batchStart = i == 0 ? 0 : substepContext.ConstraintBatchBoundaries[i - 1]; + var workBlocksInBatch = substepContext.ConstraintBatchBoundaries[i] - batchStart; + if (workBlocksInBatch == 0) + --batchSyncPointCount; + } + var syncStageCount = substepCount * (1 + batchSyncPointCount * (1 + IterationCount)) - 1; + pool.Take(syncStageCount, out substepContext.Stages); + substepContext.StageIndex = -1; + + int syncStageIndex = 0; + var totalConstraintBatchWorkBlockCount = substepContext.ConstraintBatchBoundaries[^1]; + var totalClaimCount = substepCount * (incrementalBlocks.Count + (totalConstraintBatchWorkBlockCount * (1 + IterationCount))) - incrementalBlocks.Count; + //var timeStart = Stopwatch.GetTimestamp(); + pool.Take(totalClaimCount, out var claims); + claims.Clear(0, claims.Length); + int claimStart = 0; + for (int substepIndex = 0; substepIndex < substepCount; ++substepIndex) + { + if (substepIndex > 0) + { + //Incremental update. + if (incrementalBlocks.Count > 0) + { + ref var stage = ref substepContext.Stages[syncStageIndex]; + stage.Claims = claims.Slice(claimStart, incrementalBlocks.Count); + claimStart += incrementalBlocks.Count; + stage.JobType = SolverJobType.IncrementalUpdate; + stage.WorkBlockStartIndex = 0; + stage.CompletedWorkBlockCount = 0; + ++syncStageIndex; + } + } + //Warm start. + for (int batchIndex = 0; batchIndex < ActiveSet.Batches.Count; ++batchIndex) + { + var batchStart = batchIndex == 0 ? 0 : substepContext.ConstraintBatchBoundaries[batchIndex - 1]; + var workBlocksInBatch = substepContext.ConstraintBatchBoundaries[batchIndex] - batchStart; + if (workBlocksInBatch > 0) + { + ref var stage = ref substepContext.Stages[syncStageIndex]; + stage.Claims = claims.Slice(claimStart, workBlocksInBatch); + claimStart += workBlocksInBatch; + stage.JobType = SolverJobType.WarmStart; + stage.BatchIndex = batchIndex; + stage.WorkBlockStartIndex = batchStart; + stage.SubstepIndex = substepIndex; + stage.CompletedWorkBlockCount = 0; + ++syncStageIndex; + } + } + //Solve. + for (int batchIndex = 0; batchIndex < ActiveSet.Batches.Count; ++batchIndex) + { + var batchStart = batchIndex == 0 ? 0 : substepContext.ConstraintBatchBoundaries[batchIndex - 1]; + var workBlocksInBatch = substepContext.ConstraintBatchBoundaries[batchIndex] - batchStart; + if (workBlocksInBatch > 0) + { + ref var stage = ref substepContext.Stages[syncStageIndex]; + stage.Claims = claims.Slice(claimStart, workBlocksInBatch); + claimStart += workBlocksInBatch; + stage.JobType = SolverJobType.Solve; + stage.BatchIndex = batchIndex; + stage.WorkBlockStartIndex = batchStart; + stage.CompletedWorkBlockCount = 0; + ++syncStageIndex; + } } } + //var timeEnd = Stopwatch.GetTimestamp(); + //Console.WriteLine($"Time (ms): {(timeEnd - timeStart) * 1e3 / Stopwatch.Frequency}"); + + //While we could be a little more aggressive about culling work with this condition, it doesn't matter much. Have to do it for correctness; worker relies on it. + if (ActiveSet.Batches.Count > 0) + threadDispatcher.DispatchWorkers(workDelegate); + + pool.Return(ref claims); + pool.Return(ref substepContext.Stages); + pool.Return(ref substepContext.ConstraintBatchBoundaries); + pool.Return(ref substepContext.IncrementalUpdateBlocks); + pool.Return(ref substepContext.ConstraintBlocks); } @@ -937,7 +1061,8 @@ public override void SolveStep2(float totalDt, IThreadDispatcher threadDispatche } else { - ExecuteMultithreaded(substepDt, threadDispatcher, solveStep2Worker, includeIncrementalUpdate: true); + //ExecuteMultithreaded(substepDt, threadDispatcher, solveStep2Worker, includeIncrementalUpdate: true); + ExecuteMultithreaded2(substepDt, threadDispatcher, solveStep2Worker2); } } } diff --git a/Demos/Demos/RopeTwistDemo.cs b/Demos/Demos/RopeTwistDemo.cs index c717f478c..5f8531b09 100644 --- a/Demos/Demos/RopeTwistDemo.cs +++ b/Demos/Demos/RopeTwistDemo.cs @@ -83,7 +83,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var filters = new CollidableProperty(); Simulation = Simulation.Create(BufferPool, new RopeNarrowPhaseCallbacks { ContactSpringiness = new SpringSettings(2000, 1), Filters = filters }, - new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(60), solverIterationCount: 1, solverFallbackBatchThreshold: 128); + new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(60), solverIterationCount: 1, solverFallbackBatchThreshold: 512); //Simulation = Simulation.Create(BufferPool, // new RopeNarrowPhaseCallbacks { ContactSpringiness = new SpringSettings(2000, 1), Filters = filters }, // new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper2(60), solverIterationCount: 1, solverFallbackBatchThreshold: 128); diff --git a/Demos/Program.cs b/Demos/Program.cs index d1cf79f64..668a4ab43 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -21,7 +21,7 @@ static void Main(string[] args) } //HeadlessTest.Test(content, 4, 32, 512); //HeadlessTest.Test(content, 4, 32, 512); - HeadlessTest.Test(content, 4, 32, 512); + //HeadlessTest.Test(content, 4, 32, 512); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); From fcb577e7666a1f2566051ce2c2cf9f1434a92ec3 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 16 Aug 2021 22:30:55 -0500 Subject: [PATCH 156/947] Got rid of expensive preamble, introduced deadlocks. Workers are claiming things they should not. --- BepuPhysics/Solver_SubsteppingSolve2.cs | 404 ++++++++---------------- Demos/Demo.cs | 2 +- Demos/Demos/RopeTwistDemo.cs | 14 +- 3 files changed, 134 insertions(+), 286 deletions(-) diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index 83ac0f822..574c28265 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -25,22 +25,29 @@ internal enum SolverJobType SolveFallback, } - [StructLayout(LayoutKind.Explicit)] + [StructLayout(LayoutKind.Explicit, Size = 256)] internal struct SolverSyncStage { [FieldOffset(0)] public Buffer Claims; - [FieldOffset(20)] - public SolverJobType JobType; - [FieldOffset(24)] + [FieldOffset(16)] public int BatchIndex; - [FieldOffset(28)] + [FieldOffset(20)] public int WorkBlockStartIndex; - [FieldOffset(32)] - public int SubstepIndex; + [FieldOffset(24)] + public int PreviousSyncIndex; [FieldOffset(128)] public int CompletedWorkBlockCount; + + public SolverSyncStage(Buffer claims, int workBlockStartIndex, int batchIndex = -1) + { + Claims = claims; + BatchIndex = batchIndex; + WorkBlockStartIndex = workBlockStartIndex; + CompletedWorkBlockCount = 0; + PreviousSyncIndex = 0; + } } [StructLayout(LayoutKind.Explicit)] @@ -63,9 +70,14 @@ internal struct SubstepMultithreadingContext [FieldOffset(88)] public int WorkerCount; - //Stage index is written during multithreaded execution; don't want to infest any of the more frequently read properties, so it's shoved out of any dangerous cache line. + + //This index is written during multithreaded execution; don't want to infest any of the more frequently read properties, so it's shoved out of any dangerous cache line. + /// + /// Monotonically increasing index of executed stages during a frame. + /// [FieldOffset(256)] - public int StageIndex; + public int SyncIndex; + } @@ -87,18 +99,6 @@ public virtual void SolveStep2(float dt, IThreadDispatcher threadDispatcher = nu } public class Solver : Solver where TIntegrationCallbacks : struct, IPoseIntegratorCallbacks { - void ExecuteStage(ref TStageFunction stageFunction, int workerIndex, int workerCount, int availableBlocksStartIndex, int availableBlocksCount, ref int syncStage) where TStageFunction : IStageFunction - { - int workBlockIndexOffset = workerIndex; - while (workBlockIndexOffset < availableBlocksCount) - { - stageFunction.Execute(this, availableBlocksStartIndex + workBlockIndexOffset, workerIndex); - workBlockIndexOffset += workerCount; - } - - InterstageSync(ref syncStage); - } - public Solver(Bodies bodies, BufferPool pool, int iterationCount, int fallbackBatchThreshold, int initialCapacity, int initialIslandCapacity, @@ -106,7 +106,6 @@ public Solver(Bodies bodies, BufferPool pool, int iterationCount, int fallbackBa : base(bodies, pool, iterationCount, fallbackBatchThreshold, initialCapacity, initialIslandCapacity, minimumCapacityPerTypeBatch) { PoseIntegrator = poseIntegrator; - solveStep2Worker = SolveStep2Worker; solveStep2Worker2 = SolveStep2Worker2; constraintIntegrationResponsibilitiesWorker = ConstraintIntegrationResponsibilitiesWorker; } @@ -217,97 +216,7 @@ public void Execute(Solver solver, int blockIndex, int workerIndex) } } - Action solveStep2Worker; - void SolveStep2Worker(int workerIndex) - { - int workerCount = context.WorkerCount; - int fallbackStart = GetUniformlyDistributedStart(workerIndex, context.FallbackBlocks.Blocks.Count, workerCount, 0); - Buffer batchStarts; - ref var activeSet = ref ActiveSet; - unsafe - { - var batchStartsData = stackalloc int[activeSet.Batches.Count]; - batchStarts = new Buffer(batchStartsData, activeSet.Batches.Count); - } - for (int batchIndex = 0; batchIndex < activeSet.Batches.Count; ++batchIndex) - { - var batchOffset = batchIndex > 0 ? context.BatchBoundaries[batchIndex - 1] : 0; - var batchCount = context.BatchBoundaries[batchIndex] - batchOffset; - batchStarts[batchIndex] = GetUniformlyDistributedStart(workerIndex, batchCount, workerCount, batchOffset); - } - - int syncStage = 0; - //The claimed and unclaimed state swap after every usage of both pingpong claims buffers. - int claimedState = 1; - int unclaimedState = 0; - var bounds = context.WorkerBoundsA; - var boundsBackBuffer = context.WorkerBoundsB; - //Note that every batch has a different start position. Each covers a different subset of constraints, so they require different start locations. - //The same concept applies to the prestep- the prestep covers all constraints at once, rather than batch by batch. - Debug.Assert(activeSet.Batches.Count > 0, "Don't dispatch if there are no constraints."); - GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); - - var warmstartStage = new WarmStartStep2StageFunction - { - Dt = context.Dt, - InverseDt = 1f / context.Dt, - solver = this - }; - var solveStage = new SolveStep2StageFunction - { - Dt = context.Dt, - InverseDt = 1f / context.Dt, - solver = this - }; - var incrementalUpdateStage = new IncrementalUpdateStageFunction - { - Dt = context.Dt, - InverseDt = 1f / context.Dt, - solver = this - }; - - //We have a different set of work blocks for incremental updates, so they used a different set of claimed/unclaimed state ping ponging locals. - var incrementalClaimedState = 1; - int incrementalUnclaimedState = 0; - - var incrementalUpdateWorkerStart = GetUniformlyDistributedStart(workerIndex, context.IncrementalUpdateBlocks.Blocks.Count, workerCount, 0); - for (int substepIndex = 0; substepIndex < substepCount; ++substepIndex) - { - if (substepIndex > 0) - { - ExecuteStage(ref incrementalUpdateStage, workerIndex, workerCount, 0, context.IncrementalUpdateBlocks.Blocks.Count, ref syncStage); - //ExecuteStage( - // ref incrementalUpdateStage, ref context.IncrementalUpdateBlocks, ref bounds, ref boundsBackBuffer, workerIndex, 0, context.IncrementalUpdateBlocks.Blocks.Count, - // ref incrementalUpdateWorkerStart, ref syncStage, incrementalClaimedState, incrementalUnclaimedState); - incrementalClaimedState ^= 1; - incrementalUnclaimedState ^= 1; - } - warmstartStage.SubstepIndex = substepIndex; - for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) - { - var batchOffset = batchIndex > 0 ? context.BatchBoundaries[batchIndex - 1] : 0; - ExecuteStage(ref warmstartStage, workerIndex, workerCount, batchOffset, context.BatchBoundaries[batchIndex] - batchOffset, ref syncStage); - //ExecuteStage(ref warmstartStage, ref context.ConstraintBlocks, ref bounds, ref boundsBackBuffer, workerIndex, batchOffset, context.BatchBoundaries[batchIndex], - // ref batchStarts[batchIndex], ref syncStage, claimedState, unclaimedState); - } - claimedState ^= 1; - unclaimedState ^= 1; - for (int j = 0; j < IterationCount; ++j) - { - for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) - { - var batchOffset = batchIndex > 0 ? context.BatchBoundaries[batchIndex - 1] : 0; - ExecuteStage(ref solveStage, workerIndex, workerCount, batchOffset, context.BatchBoundaries[batchIndex] - batchOffset, ref syncStage); - //ExecuteStage(ref solveStage, ref context.ConstraintBlocks, ref bounds, ref boundsBackBuffer, workerIndex, batchOffset, context.BatchBoundaries[batchIndex], - // ref batchStarts[batchIndex], ref syncStage, claimedState, unclaimedState); - } - claimedState ^= 1; - unclaimedState ^= 1; - } - } - } - - void ExecuteWorkerStage(ref TStageFunction stageFunction, int workerIndex, int workerStart, int availableBlocksStartIndex, ref Buffer claims, ref int completedWorkBlocks) where TStageFunction : IStageFunction + void ExecuteWorkerStage(ref TStageFunction stageFunction, int workerIndex, int workerStart, int availableBlocksStartIndex, ref Buffer claims, int previousSyncIndex, int syncIndex, ref int completedWorkBlocks) where TStageFunction : IStageFunction { if (workerStart == -1) { @@ -319,7 +228,7 @@ void ExecuteWorkerStage(ref TStageFunction stageFunction, int wo int workBlockIndex = workerStart; int locallyCompletedCount = 0; //Try to claim blocks by traversing forward until we're blocked by another claim. - while (Interlocked.CompareExchange(ref claims[workBlockIndex], -1, 0) == 0) + while (Interlocked.CompareExchange(ref claims[workBlockIndex], syncIndex, previousSyncIndex) == previousSyncIndex) { //Successfully claimed a work block. stageFunction.Execute(this, availableBlocksStartIndex + workBlockIndex, workerIndex); @@ -331,49 +240,53 @@ void ExecuteWorkerStage(ref TStageFunction stageFunction, int wo workBlockIndex = 0; } } - //TODO: Technically, given wrap around of forward traversal, backwards looping seems.. questionable. Verify. - //Try to claim work blocks going backward. - workBlockIndex = workerStart - 1; - while (true) - { - if (workBlockIndex < 0) - { - //Wrap around. - workBlockIndex = claims.Length - 1; - } - if (Interlocked.CompareExchange(ref claims[workBlockIndex], -1, 0) != 0) - { - break; - } - //Successfully claimed a work block. - stageFunction.Execute(this, availableBlocksStartIndex + workBlockIndex, workerIndex); - ++locallyCompletedCount; - workBlockIndex--; - } + ////TODO: Technically, given wrap around of forward traversal, backwards looping seems.. questionable. Verify. + ////Try to claim work blocks going backward. + //workBlockIndex = workerStart - 1; + //while (true) + //{ + // if (workBlockIndex < 0) + // { + // //Wrap around. + // workBlockIndex = claims.Length - 1; + // } + // if (Interlocked.CompareExchange(ref claims[workBlockIndex], syncIndex, previousSyncIndex) == previousSyncIndex) + // { + // break; + // } + // //Successfully claimed a work block. + // stageFunction.Execute(this, availableBlocksStartIndex + workBlockIndex, workerIndex); + // ++locallyCompletedCount; + // workBlockIndex--; + //} //No more adjacent work blocks are available. This thread is done! Interlocked.Add(ref completedWorkBlocks, locallyCompletedCount); } - void ExecuteMainStage(ref TStageFunction stageFunction, int workerIndex, int workerStart, int availableBlocksStartIndex, ref SolverSyncStage stage, int stageIndex) where TStageFunction : IStageFunction + void ExecuteMainStage(ref TStageFunction stageFunction, int workerIndex, int workerStart, ref SolverSyncStage stage, ref int syncIndex) where TStageFunction : IStageFunction { + //Note that the main thread's view of the sync index increments every single time, even if there is no work. + //This ensures that the workers are able to advance to the appropriate stage by examining the sync index snapshot. + ++syncIndex; var availableBlocksCount = stage.Claims.Length; - Debug.Assert(availableBlocksCount > 0, "We assume any zero count batches are not included in the stages available to execute."); + if (availableBlocksCount == 0) + return; //for (int i = 0; i < availableBlocksCount; ++i) //{ - // stageFunction.Execute(this, availableBlocksStartIndex + i, workerIndex); + // stageFunction.Execute(this, stage.WorkBlockStartIndex + i, workerIndex); //} //return; if (availableBlocksCount == 1) { //There is only one work block available. There's no reason to notify other threads about it or do any claims management; just execute it sequentially. - stageFunction.Execute(this, availableBlocksStartIndex, workerIndex); + stageFunction.Execute(this, stage.WorkBlockStartIndex, workerIndex); } else { //Write the new stage index so other spinning threads will begin work on it. - Volatile.Write(ref substepContext.StageIndex, stageIndex); - ExecuteWorkerStage(ref stageFunction, workerIndex, workerStart, availableBlocksStartIndex, ref stage.Claims, ref stage.CompletedWorkBlockCount); + Volatile.Write(ref substepContext.SyncIndex, syncIndex); + ExecuteWorkerStage(ref stageFunction, workerIndex, workerStart, stage.WorkBlockStartIndex, ref stage.Claims, stage.PreviousSyncIndex, syncIndex, ref stage.CompletedWorkBlockCount); //Since we asked other threads to do work, we must wait until the requested work is done before proceeding. //Note that we DO NOT yield on the main thread! @@ -388,6 +301,9 @@ void ExecuteMainStage(ref TStageFunction stageFunction, int work { Thread.SpinWait(3); } + //All workers are done. We can safely reset the counter for the next time this stage is used. + stage.CompletedWorkBlockCount = 0; + stage.PreviousSyncIndex = syncIndex; } } SubstepMultithreadingContext substepContext; @@ -424,19 +340,19 @@ void SolveStep2Worker2(int workerIndex) Debug.Assert(activeSet.Batches.Count > 0, "Don't dispatch if there are no constraints."); GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); - var warmstartStage = new WarmStartStep2StageFunction + var incrementalUpdateStage = new IncrementalUpdateStageFunction { Dt = substepContext.Dt, InverseDt = substepContext.InverseDt, solver = this }; - var solveStage = new SolveStep2StageFunction + var warmstartStage = new WarmStartStep2StageFunction { Dt = substepContext.Dt, InverseDt = substepContext.InverseDt, solver = this }; - var incrementalUpdateStage = new IncrementalUpdateStageFunction + var solveStage = new SolveStep2StageFunction { Dt = substepContext.Dt, InverseDt = substepContext.InverseDt, @@ -446,93 +362,90 @@ void SolveStep2Worker2(int workerIndex) if (workerIndex == 0) { //This is the main 'orchestrator' thread. It tracks execution progress and notifies other threads that's it's time to work. - for (int stageIndex = 0; stageIndex < substepContext.Stages.Length; ++stageIndex) + int syncIndex = 0; + for (int substepIndex = 0; substepIndex < substepCount; ++substepIndex) { - ref var stage = ref substepContext.Stages[stageIndex]; - switch (stage.JobType) + if (substepIndex > 0) { - case SolverJobType.IncrementalUpdate: - { - ExecuteMainStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStart, 0, ref stage, stageIndex); - } - break; - case SolverJobType.WarmStart: - { - warmstartStage.SubstepIndex = stage.SubstepIndex; - ExecuteMainStage(ref warmstartStage, workerIndex, batchStarts[stage.BatchIndex], stage.WorkBlockStartIndex, ref stage, stageIndex); - } - break; - case SolverJobType.WarmStartFallback: - { - Debug.Fail("Not yet supported."); - } - break; - case SolverJobType.Solve: - { - ExecuteMainStage(ref solveStage, workerIndex, batchStarts[stage.BatchIndex], stage.WorkBlockStartIndex, ref stage, stageIndex); - } - break; - case SolverJobType.SolveFallback: - { - Debug.Fail("Not yet supported."); - } - break; + ExecuteMainStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStart, ref substepContext.Stages[0], ref syncIndex); + } + for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) + { + warmstartStage.SubstepIndex = substepIndex; + ExecuteMainStage(ref warmstartStage, workerIndex, batchStarts[batchIndex], ref substepContext.Stages[batchIndex + 1], ref syncIndex); + } + for (int iterationIndex = 0; iterationIndex < IterationCount; ++iterationIndex) + { + for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) + { + ExecuteMainStage(ref solveStage, workerIndex, batchStarts[batchIndex], ref substepContext.Stages[batchIndex + 1], ref syncIndex); + } } } //All done; notify waiting threads to join. - Volatile.Write(ref substepContext.StageIndex, substepContext.Stages.Length); + Volatile.Write(ref substepContext.SyncIndex, int.MinValue); } else { //This is a worker thread. It does not need to track execution progress; it only checks to see if there's any work that needs to be done, and if there is, does it, then goes back into a wait. - int latestCompletedStageIndex = -1; + int latestCompletedSyncIndex = 0; + int syncIndexInSubstep = -1; + int substepIndex = 0; + var baseStageCountInSubstep = synchronizedBatchCount * (1 + IterationCount); while (true) { var spinWait = new LocalSpinWait(); - int stageIndex; - while (latestCompletedStageIndex == (stageIndex = Volatile.Read(ref substepContext.StageIndex))) + int syncIndex; + while (latestCompletedSyncIndex == (syncIndex = Volatile.Read(ref substepContext.SyncIndex))) { //No work yet available. spinWait.SpinOnce(); } //Stages were set up prior to execution. Note that we don't attempt to ping pong buffers or anything; there are unique entries for every single stage. //This guarantees that a worker thread can go idle and miss an arbitrary number of stages without blocking any progress. - if (stageIndex == substepContext.Stages.Length) + if (syncIndex == int.MinValue) { //No more stages; exit the work loop. break; } - ref var stage = ref substepContext.Stages[stageIndex]; - switch (stage.JobType) + //Extract the job type, stage index, and substep index from the sync index. + var syncStepsSinceLast = syncIndex - latestCompletedSyncIndex; + syncIndexInSubstep += syncStepsSinceLast; + while (true) { - case SolverJobType.IncrementalUpdate: - { - ExecuteWorkerStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStart, 0, ref stage.Claims, ref stage.CompletedWorkBlockCount); - } - break; - case SolverJobType.WarmStart: - { - warmstartStage.SubstepIndex = stage.SubstepIndex; - ExecuteWorkerStage(ref warmstartStage, workerIndex, batchStarts[stage.BatchIndex], stage.WorkBlockStartIndex, ref stage.Claims, ref stage.CompletedWorkBlockCount); - } - break; - case SolverJobType.WarmStartFallback: - { - Debug.Fail("Not yet supported."); - } - break; - case SolverJobType.Solve: - { - ExecuteWorkerStage(ref solveStage, workerIndex, batchStarts[stage.BatchIndex], stage.WorkBlockStartIndex, ref stage.Claims, ref stage.CompletedWorkBlockCount); - } - break; - case SolverJobType.SolveFallback: - { - Debug.Fail("Not yet supported."); - } + var stageCountInSubstep = substepIndex > 0 ? baseStageCountInSubstep + 1 : baseStageCountInSubstep; + if (syncIndexInSubstep >= stageCountInSubstep) + { + syncIndexInSubstep -= stageCountInSubstep; + ++substepIndex; + } + else + { break; + } + } + var stageIndex = substepIndex == 0 ? syncIndexInSubstep + 1 : syncIndexInSubstep; + if (stageIndex == 0) + { + //Incremental update. + ref var stage = ref substepContext.Stages[stageIndex]; + ExecuteWorkerStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStart, 0, ref stage.Claims, stage.PreviousSyncIndex, syncIndex, ref stage.CompletedWorkBlockCount); } - latestCompletedStageIndex = stageIndex; + else if (stageIndex < 1 + synchronizedBatchCount) + { + //Warm start. + ref var stage = ref substepContext.Stages[stageIndex]; + warmstartStage.SubstepIndex = substepIndex; + ExecuteWorkerStage(ref warmstartStage, workerIndex, batchStarts[stage.BatchIndex], stage.WorkBlockStartIndex, ref stage.Claims, stage.PreviousSyncIndex, syncIndex, ref stage.CompletedWorkBlockCount); + } + else + { + //Solve. + stageIndex -= synchronizedBatchCount; + ref var stage = ref substepContext.Stages[stageIndex]; + ExecuteWorkerStage(ref solveStage, workerIndex, batchStarts[stage.BatchIndex], stage.WorkBlockStartIndex, ref stage.Claims, stage.PreviousSyncIndex, syncIndex, ref stage.CompletedWorkBlockCount); + } + latestCompletedSyncIndex = syncIndex; } } @@ -564,79 +477,26 @@ protected void ExecuteMultithreaded2(float dt, IThreadDispatcher threadDispatche //Not every batch will actually have work blocks associated with it; the batch compressor could be falling behind, which means older constraints could be at higher batches than they need to be, leaving gaps. //We don't want to include those empty batches as sync points in the solver. - var batchSyncPointCount = ActiveSet.Batches.Count; - for (int i = 0; i < ActiveSet.Batches.Count; ++i) - { - var batchStart = i == 0 ? 0 : substepContext.ConstraintBatchBoundaries[i - 1]; - var workBlocksInBatch = substepContext.ConstraintBatchBoundaries[i] - batchStart; - if (workBlocksInBatch == 0) - --batchSyncPointCount; - } - var syncStageCount = substepCount * (1 + batchSyncPointCount * (1 + IterationCount)) - 1; - pool.Take(syncStageCount, out substepContext.Stages); - substepContext.StageIndex = -1; + GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); + pool.Take(1 + synchronizedBatchCount, out substepContext.Stages); + substepContext.SyncIndex = 0; - int syncStageIndex = 0; var totalConstraintBatchWorkBlockCount = substepContext.ConstraintBatchBoundaries[^1]; - var totalClaimCount = substepCount * (incrementalBlocks.Count + (totalConstraintBatchWorkBlockCount * (1 + IterationCount))) - incrementalBlocks.Count; - //var timeStart = Stopwatch.GetTimestamp(); + var totalClaimCount = incrementalBlocks.Count + totalConstraintBatchWorkBlockCount; + //Claims will be monotonically increasing throughout execution. All should start at zero to match with the initial sync index. pool.Take(totalClaimCount, out var claims); claims.Clear(0, claims.Length); - int claimStart = 0; - for (int substepIndex = 0; substepIndex < substepCount; ++substepIndex) - { - if (substepIndex > 0) - { - //Incremental update. - if (incrementalBlocks.Count > 0) - { - ref var stage = ref substepContext.Stages[syncStageIndex]; - stage.Claims = claims.Slice(claimStart, incrementalBlocks.Count); - claimStart += incrementalBlocks.Count; - stage.JobType = SolverJobType.IncrementalUpdate; - stage.WorkBlockStartIndex = 0; - stage.CompletedWorkBlockCount = 0; - ++syncStageIndex; - } - } - //Warm start. - for (int batchIndex = 0; batchIndex < ActiveSet.Batches.Count; ++batchIndex) - { - var batchStart = batchIndex == 0 ? 0 : substepContext.ConstraintBatchBoundaries[batchIndex - 1]; - var workBlocksInBatch = substepContext.ConstraintBatchBoundaries[batchIndex] - batchStart; - if (workBlocksInBatch > 0) - { - ref var stage = ref substepContext.Stages[syncStageIndex]; - stage.Claims = claims.Slice(claimStart, workBlocksInBatch); - claimStart += workBlocksInBatch; - stage.JobType = SolverJobType.WarmStart; - stage.BatchIndex = batchIndex; - stage.WorkBlockStartIndex = batchStart; - stage.SubstepIndex = substepIndex; - stage.CompletedWorkBlockCount = 0; - ++syncStageIndex; - } - } - //Solve. - for (int batchIndex = 0; batchIndex < ActiveSet.Batches.Count; ++batchIndex) - { - var batchStart = batchIndex == 0 ? 0 : substepContext.ConstraintBatchBoundaries[batchIndex - 1]; - var workBlocksInBatch = substepContext.ConstraintBatchBoundaries[batchIndex] - batchStart; - if (workBlocksInBatch > 0) - { - ref var stage = ref substepContext.Stages[syncStageIndex]; - stage.Claims = claims.Slice(claimStart, workBlocksInBatch); - claimStart += workBlocksInBatch; - stage.JobType = SolverJobType.Solve; - stage.BatchIndex = batchIndex; - stage.WorkBlockStartIndex = batchStart; - stage.CompletedWorkBlockCount = 0; - ++syncStageIndex; - } - } + substepContext.Stages[0] = new SolverSyncStage(claims.Slice(incrementalBlocks.Count), 0); + int claimStart = incrementalBlocks.Count; + for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) + { + var stageIndex = batchIndex + 1; + var batchStart = batchIndex == 0 ? 0 : substepContext.ConstraintBatchBoundaries[batchIndex - 1]; + var workBlocksInBatch = substepContext.ConstraintBatchBoundaries[batchIndex] - batchStart; + substepContext.Stages[stageIndex] = new(claims.Slice(claimStart, workBlocksInBatch), batchStart, batchIndex); + claimStart += workBlocksInBatch; } - //var timeEnd = Stopwatch.GetTimestamp(); - //Console.WriteLine($"Time (ms): {(timeEnd - timeStart) * 1e3 / Stopwatch.Frequency}"); + //While we could be a little more aggressive about culling work with this condition, it doesn't matter much. Have to do it for correctness; worker relies on it. if (ActiveSet.Batches.Count > 0) diff --git a/Demos/Demo.cs b/Demos/Demo.cs index 9189f4ba5..37ebe5190 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -43,7 +43,7 @@ protected Demo() //It may be worth using something like hwloc to extract extra information to reason about. //var targetThreadCount = Math.Max(1, Environment.ProcessorCount > 4 ? Environment.ProcessorCount - 2 : Environment.ProcessorCount - 1); - var targetThreadCount = 29; + var targetThreadCount = 30; ThreadDispatcher = new SimpleThreadDispatcher(targetThreadCount); } diff --git a/Demos/Demos/RopeTwistDemo.cs b/Demos/Demos/RopeTwistDemo.cs index 5f8531b09..8c95b908b 100644 --- a/Demos/Demos/RopeTwistDemo.cs +++ b/Demos/Demos/RopeTwistDemo.cs @@ -86,7 +86,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(60), solverIterationCount: 1, solverFallbackBatchThreshold: 512); //Simulation = Simulation.Create(BufferPool, // new RopeNarrowPhaseCallbacks { ContactSpringiness = new SpringSettings(2000, 1), Filters = filters }, - // new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper2(60), solverIterationCount: 1, solverFallbackBatchThreshold: 128); + // new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper2(60), solverIterationCount: 1, solverFallbackBatchThreshold: 512); for (int twistIndex = 0; twistIndex < 1; ++twistIndex) @@ -162,19 +162,7 @@ bool TryCreateConstraint(int handleIndexA, int handleIndexB) Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(200, 1, 200)), 0.1f))); - //Simulation.Statics.Add(new StaticDescription( - // new Vector3(100, 70, 0), BepuUtilities.QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), MathF.PI * 0.5f), - // new CollidableDescription(Simulation.Shapes.Add(new Capsule(8, 64)), 0.1f))); } - public override void Update(Window window, Camera camera, Input input, float dt) - { - //This demo is an extreme case. - //As of this writing, extremely high substep counts and extremely high batch counts with a relatively small simulations (like in this demo) - //actually run faster on a single thread. The overhead of synchronizing the thread group thousands of times overwhelms the actual work being done. - //On the upside, this means that adding more bodies uninvolved to this simulation is almost free if using multithreading. - Simulation.Timestep(1 / 60f, ThreadDispatcher); - } - } } From 9996f4b1f307bb7fbd00aceb76ed335c0bdde94a Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 17 Aug 2021 18:36:05 -0500 Subject: [PATCH 157/947] Work scheduler now working. --- BepuPhysics/BepuPhysics.csproj | 10 ++- BepuPhysics/Solver_SubsteppingSolve2.cs | 94 +++++++++++++------------ BepuUtilities/BepuUtilities.csproj | 6 +- Demos/Program.cs | 2 +- 4 files changed, 56 insertions(+), 56 deletions(-) diff --git a/BepuPhysics/BepuPhysics.csproj b/BepuPhysics/BepuPhysics.csproj index 4b5f96148..396beef48 100644 --- a/BepuPhysics/BepuPhysics.csproj +++ b/BepuPhysics/BepuPhysics.csproj @@ -13,29 +13,27 @@ Debug;Release;ReleaseNoProfiling latest physics;3d;rigid body;real time;simulation - True + True + true + full true false key.snk true false - + false TRACE;DEBUG;CHECKMATH;PROFILE - true - full true TRACE;RELEASE;PROFILE - true - full true TRACE;RELEASE diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index 574c28265..370a05ae6 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -25,28 +25,17 @@ internal enum SolverJobType SolveFallback, } - [StructLayout(LayoutKind.Explicit, Size = 256)] internal struct SolverSyncStage { - [FieldOffset(0)] public Buffer Claims; - [FieldOffset(16)] public int BatchIndex; - [FieldOffset(20)] public int WorkBlockStartIndex; - [FieldOffset(24)] - public int PreviousSyncIndex; - - [FieldOffset(128)] - public int CompletedWorkBlockCount; public SolverSyncStage(Buffer claims, int workBlockStartIndex, int batchIndex = -1) { Claims = claims; BatchIndex = batchIndex; WorkBlockStartIndex = workBlockStartIndex; - CompletedWorkBlockCount = 0; - PreviousSyncIndex = 0; } } @@ -78,6 +67,12 @@ internal struct SubstepMultithreadingContext [FieldOffset(256)] public int SyncIndex; + /// + /// Counter of work completed for the current stage. + /// + [FieldOffset(384)] + public int CompletedWorkBlockCount; + } @@ -216,7 +211,7 @@ public void Execute(Solver solver, int blockIndex, int workerIndex) } } - void ExecuteWorkerStage(ref TStageFunction stageFunction, int workerIndex, int workerStart, int availableBlocksStartIndex, ref Buffer claims, int previousSyncIndex, int syncIndex, ref int completedWorkBlocks) where TStageFunction : IStageFunction + void ExecuteWorkerStage(ref TStageFunction stageFunction, int workerIndex, int workerStart, int availableBlocksStartIndex, ref Buffer claims, int previousSyncIndexOffset, int syncIndex, ref int completedWorkBlocks) where TStageFunction : IStageFunction { if (workerStart == -1) { @@ -227,42 +222,48 @@ void ExecuteWorkerStage(ref TStageFunction stageFunction, int wo } int workBlockIndex = workerStart; int locallyCompletedCount = 0; + var previousSyncIndex = Math.Max(0, syncIndex - previousSyncIndexOffset); //Try to claim blocks by traversing forward until we're blocked by another claim. while (Interlocked.CompareExchange(ref claims[workBlockIndex], syncIndex, previousSyncIndex) == previousSyncIndex) { //Successfully claimed a work block. stageFunction.Execute(this, availableBlocksStartIndex + workBlockIndex, workerIndex); ++locallyCompletedCount; - workBlockIndex++; + ++workBlockIndex; if (workBlockIndex >= claims.Length) { //Wrap around. workBlockIndex = 0; } } - ////TODO: Technically, given wrap around of forward traversal, backwards looping seems.. questionable. Verify. - ////Try to claim work blocks going backward. - //workBlockIndex = workerStart - 1; - //while (true) - //{ - // if (workBlockIndex < 0) - // { - // //Wrap around. - // workBlockIndex = claims.Length - 1; - // } - // if (Interlocked.CompareExchange(ref claims[workBlockIndex], syncIndex, previousSyncIndex) == previousSyncIndex) - // { - // break; - // } - // //Successfully claimed a work block. - // stageFunction.Execute(this, availableBlocksStartIndex + workBlockIndex, workerIndex); - // ++locallyCompletedCount; - // workBlockIndex--; - //} + //TODO: Technically, given wrap around of forward traversal, backwards looping seems.. questionable. Verify. + //Try to claim work blocks going backward. + workBlockIndex = workerStart - 1; + while (true) + { + if (workBlockIndex < 0) + { + //Wrap around. + workBlockIndex = claims.Length - 1; + } + if (Interlocked.CompareExchange(ref claims[workBlockIndex], syncIndex, previousSyncIndex) != previousSyncIndex) + { + break; + } + //Successfully claimed a work block. + stageFunction.Execute(this, availableBlocksStartIndex + workBlockIndex, workerIndex); + ++locallyCompletedCount; + workBlockIndex--; + } //No more adjacent work blocks are available. This thread is done! Interlocked.Add(ref completedWorkBlocks, locallyCompletedCount); + //if (workerIndex == 3) + //{ + // Console.WriteLine($"Worker {workerIndex} completed {locallyCompletedCount / (double)claims.Length:G2} ({locallyCompletedCount} of {claims.Length})."); + //} + } - void ExecuteMainStage(ref TStageFunction stageFunction, int workerIndex, int workerStart, ref SolverSyncStage stage, ref int syncIndex) where TStageFunction : IStageFunction + void ExecuteMainStage(ref TStageFunction stageFunction, int workerIndex, int workerStart, ref SolverSyncStage stage, int previousSyncIndexOffset, ref int syncIndex) where TStageFunction : IStageFunction { //Note that the main thread's view of the sync index increments every single time, even if there is no work. //This ensures that the workers are able to advance to the appropriate stage by examining the sync index snapshot. @@ -286,7 +287,7 @@ void ExecuteMainStage(ref TStageFunction stageFunction, int work { //Write the new stage index so other spinning threads will begin work on it. Volatile.Write(ref substepContext.SyncIndex, syncIndex); - ExecuteWorkerStage(ref stageFunction, workerIndex, workerStart, stage.WorkBlockStartIndex, ref stage.Claims, stage.PreviousSyncIndex, syncIndex, ref stage.CompletedWorkBlockCount); + ExecuteWorkerStage(ref stageFunction, workerIndex, workerStart, stage.WorkBlockStartIndex, ref stage.Claims, previousSyncIndexOffset, syncIndex, ref substepContext.CompletedWorkBlockCount); //Since we asked other threads to do work, we must wait until the requested work is done before proceeding. //Note that we DO NOT yield on the main thread! @@ -297,13 +298,12 @@ void ExecuteMainStage(ref TStageFunction stageFunction, int work //By having the main thread never yield, the only way for all progress to halt is for the OS to aggressively unschedule the main thread. //That is very rare when dealing with CPUs with plenty of cores to go around relative to the scheduled work. //(Why not notify the OS that waiting threads actually want to be executed? Just overhead. Feel free to experiment with different approaches, but so far this has won empirically.) - while (Volatile.Read(ref stage.CompletedWorkBlockCount) != availableBlocksCount) + while (Volatile.Read(ref substepContext.CompletedWorkBlockCount) != availableBlocksCount) { Thread.SpinWait(3); } //All workers are done. We can safely reset the counter for the next time this stage is used. - stage.CompletedWorkBlockCount = 0; - stage.PreviousSyncIndex = syncIndex; + substepContext.CompletedWorkBlockCount = 0; } } SubstepMultithreadingContext substepContext; @@ -359,6 +359,7 @@ void SolveStep2Worker2(int workerIndex) solver = this }; + var baseStageCountInSubstep = synchronizedBatchCount * (1 + IterationCount); if (workerIndex == 0) { //This is the main 'orchestrator' thread. It tracks execution progress and notifies other threads that's it's time to work. @@ -367,18 +368,18 @@ void SolveStep2Worker2(int workerIndex) { if (substepIndex > 0) { - ExecuteMainStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStart, ref substepContext.Stages[0], ref syncIndex); + ExecuteMainStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStart, ref substepContext.Stages[0], 1 + baseStageCountInSubstep, ref syncIndex); } for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) { warmstartStage.SubstepIndex = substepIndex; - ExecuteMainStage(ref warmstartStage, workerIndex, batchStarts[batchIndex], ref substepContext.Stages[batchIndex + 1], ref syncIndex); + ExecuteMainStage(ref warmstartStage, workerIndex, batchStarts[batchIndex], ref substepContext.Stages[batchIndex + 1], 1 + synchronizedBatchCount, ref syncIndex); } for (int iterationIndex = 0; iterationIndex < IterationCount; ++iterationIndex) { for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) { - ExecuteMainStage(ref solveStage, workerIndex, batchStarts[batchIndex], ref substepContext.Stages[batchIndex + 1], ref syncIndex); + ExecuteMainStage(ref solveStage, workerIndex, batchStarts[batchIndex], ref substepContext.Stages[batchIndex + 1], synchronizedBatchCount, ref syncIndex); } } } @@ -391,7 +392,6 @@ void SolveStep2Worker2(int workerIndex) int latestCompletedSyncIndex = 0; int syncIndexInSubstep = -1; int substepIndex = 0; - var baseStageCountInSubstep = synchronizedBatchCount * (1 + IterationCount); while (true) { var spinWait = new LocalSpinWait(); @@ -425,25 +425,27 @@ void SolveStep2Worker2(int workerIndex) } } var stageIndex = substepIndex == 0 ? syncIndexInSubstep + 1 : syncIndexInSubstep; + //Note that we're going to do a compare exchange that prevents any claim on work blocks that *arent* of the previous sync index, which means we need the previous sync index. + //Storing that in a reliable way is annoying, so we derive it from syncIndex. if (stageIndex == 0) { //Incremental update. ref var stage = ref substepContext.Stages[stageIndex]; - ExecuteWorkerStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStart, 0, ref stage.Claims, stage.PreviousSyncIndex, syncIndex, ref stage.CompletedWorkBlockCount); + ExecuteWorkerStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStart, 0, ref stage.Claims, 1 + baseStageCountInSubstep, syncIndex, ref substepContext.CompletedWorkBlockCount); } else if (stageIndex < 1 + synchronizedBatchCount) { //Warm start. ref var stage = ref substepContext.Stages[stageIndex]; warmstartStage.SubstepIndex = substepIndex; - ExecuteWorkerStage(ref warmstartStage, workerIndex, batchStarts[stage.BatchIndex], stage.WorkBlockStartIndex, ref stage.Claims, stage.PreviousSyncIndex, syncIndex, ref stage.CompletedWorkBlockCount); + ExecuteWorkerStage(ref warmstartStage, workerIndex, batchStarts[stage.BatchIndex], stage.WorkBlockStartIndex, ref stage.Claims, 1 + synchronizedBatchCount, syncIndex, ref substepContext.CompletedWorkBlockCount); } else { //Solve. stageIndex -= synchronizedBatchCount; ref var stage = ref substepContext.Stages[stageIndex]; - ExecuteWorkerStage(ref solveStage, workerIndex, batchStarts[stage.BatchIndex], stage.WorkBlockStartIndex, ref stage.Claims, stage.PreviousSyncIndex, syncIndex, ref stage.CompletedWorkBlockCount); + ExecuteWorkerStage(ref solveStage, workerIndex, batchStarts[stage.BatchIndex], stage.WorkBlockStartIndex, ref stage.Claims, synchronizedBatchCount, syncIndex, ref substepContext.CompletedWorkBlockCount); } latestCompletedSyncIndex = syncIndex; @@ -462,7 +464,7 @@ protected void ExecuteMultithreaded2(float dt, IThreadDispatcher threadDispatche //These values are found by empirical tuning. The optimal values may vary by architecture. //The goal here is to have just enough blocks that, in the event that we end up some underpowered threads (due to competition or hyperthreading), //there are enough blocks that workstealing will still generally allow the extra threads to be useful. - const int targetBlocksPerBatchPerWorker = 1; + const int targetBlocksPerBatchPerWorker = 32; const int minimumBlockSizeInBundles = 1; const int maximumBlockSizeInBundles = 1024; @@ -486,7 +488,7 @@ protected void ExecuteMultithreaded2(float dt, IThreadDispatcher threadDispatche //Claims will be monotonically increasing throughout execution. All should start at zero to match with the initial sync index. pool.Take(totalClaimCount, out var claims); claims.Clear(0, claims.Length); - substepContext.Stages[0] = new SolverSyncStage(claims.Slice(incrementalBlocks.Count), 0); + substepContext.Stages[0] = new(claims.Slice(incrementalBlocks.Count), 0); int claimStart = incrementalBlocks.Count; for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) { diff --git a/BepuUtilities/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj index 6769b7236..fd1cefeaa 100644 --- a/BepuUtilities/BepuUtilities.csproj +++ b/BepuUtilities/BepuUtilities.csproj @@ -14,7 +14,9 @@ bepuphysicslogo256.png Debug;Release latest - True + True + true + full true false key.snk @@ -30,8 +32,6 @@ true TRACE;RELEASE - true - full diff --git a/Demos/Program.cs b/Demos/Program.cs index 668a4ab43..d1cf79f64 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -21,7 +21,7 @@ static void Main(string[] args) } //HeadlessTest.Test(content, 4, 32, 512); //HeadlessTest.Test(content, 4, 32, 512); - //HeadlessTest.Test(content, 4, 32, 512); + HeadlessTest.Test(content, 4, 32, 512); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); From fbb2be6d4ebf784d2c516f2938e9a9772ecfda5b Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 18 Aug 2021 14:45:45 -0500 Subject: [PATCH 158/947] Some debug stuff. --- BepuPhysics/Solver_SubsteppingSolve2.cs | 77 ++++++++++++++++++++++++- BepuUtilities/Memory/Buffer.cs | 1 - Demos/Program.cs | 4 +- 3 files changed, 78 insertions(+), 4 deletions(-) diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index 370a05ae6..95da750e7 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -236,7 +236,6 @@ void ExecuteWorkerStage(ref TStageFunction stageFunction, int wo workBlockIndex = 0; } } - //TODO: Technically, given wrap around of forward traversal, backwards looping seems.. questionable. Verify. //Try to claim work blocks going backward. workBlockIndex = workerStart - 1; while (true) @@ -257,6 +256,7 @@ void ExecuteWorkerStage(ref TStageFunction stageFunction, int wo } //No more adjacent work blocks are available. This thread is done! Interlocked.Add(ref completedWorkBlocks, locallyCompletedCount); + //debugStageWorkBlocksCompleted[syncIndex - 1][workerIndex] = locallyCompletedCount; //if (workerIndex == 3) //{ // Console.WriteLine($"Worker {workerIndex} completed {locallyCompletedCount / (double)claims.Length:G2} ({locallyCompletedCount} of {claims.Length})."); @@ -309,6 +309,7 @@ void ExecuteMainStage(ref TStageFunction stageFunction, int work SubstepMultithreadingContext substepContext; + Action solveStep2Worker2; void SolveStep2Worker2(int workerIndex) { @@ -454,6 +455,7 @@ void SolveStep2Worker2(int workerIndex) } + Buffer> debugStageWorkBlocksCompleted; protected void ExecuteMultithreaded2(float dt, IThreadDispatcher threadDispatcher, Action workDelegate) { var workerCount = substepContext.WorkerCount = threadDispatcher.ThreadCount; @@ -499,17 +501,90 @@ protected void ExecuteMultithreaded2(float dt, IThreadDispatcher threadDispatche claimStart += workBlocksInBatch; } + //var syncCount = substepCount * (1 + synchronizedBatchCount * (1 + IterationCount)) - 1; + //pool.Take(syncCount, out debugStageWorkBlocksCompleted); + //pool.Take(syncCount * workerCount, out var workBlocksCompleted); + //workBlocksCompleted.Clear(0, workBlocksCompleted.Length); + //for (int i = 0; i < syncCount; ++i) + //{ + // debugStageWorkBlocksCompleted[i] = workBlocksCompleted.Slice(i * workerCount, workerCount); + //} //While we could be a little more aggressive about culling work with this condition, it doesn't matter much. Have to do it for correctness; worker relies on it. if (ActiveSet.Batches.Count > 0) threadDispatcher.DispatchWorkers(workDelegate); + //pool.Take(syncCount, out var availableCountPerSync); + //var syncIndex = 0; + //for (int substepIndex = 0; substepIndex < substepCount; ++substepIndex) + //{ + // if (substepIndex > 0) + // { + // availableCountPerSync[syncIndex] = incrementalBlocks.Count; + // ++syncIndex; + // } + // for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) + // { + // var batchStart = batchIndex == 0 ? 0 : substepContext.ConstraintBatchBoundaries[batchIndex - 1]; + // var workBlocksInBatch = substepContext.ConstraintBatchBoundaries[batchIndex] - batchStart; + // availableCountPerSync[syncIndex] = workBlocksInBatch; + // ++syncIndex; + // } + // for (int i = 0; i < IterationCount; ++i) + // { + // for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) + // { + // var batchStart = batchIndex == 0 ? 0 : substepContext.ConstraintBatchBoundaries[batchIndex - 1]; + // var workBlocksInBatch = substepContext.ConstraintBatchBoundaries[batchIndex] - batchStart; + // availableCountPerSync[syncIndex] = workBlocksInBatch; + // ++syncIndex; + // } + // } + //} + + //pool.Take(workerCount, out var workerBlocksCompletedSums); + //pool.Take(workerCount, out var workerFractionSum); + //workerBlocksCompletedSums.Clear(0, workerBlocksCompletedSums.Length); + //var availableCountSum = 0; + //for (int i = 0; i < syncCount; ++i) + //{ + // if (availableCountPerSync[i] <= 1) + // continue; + // Console.WriteLine($"Sync {i}, available {availableCountPerSync[i]}, ideal {(availableCountPerSync[i] / (double)workerCount):G2}:"); + // var stageWorkerBlockCounts = debugStageWorkBlocksCompleted[i]; + // for (int j = 0; j < workerCount; ++j) + // { + // workerBlocksCompletedSums[j] += stageWorkerBlockCounts[j]; + // var expectedCount = availableCountPerSync[i] / (double)workerCount; + // if (j >= availableCountPerSync[i]) + // workerFractionSum[j] += 1; + // else + // workerFractionSum[j] += stageWorkerBlockCounts[j] / expectedCount; + // Console.WriteLine($"{j}: {stageWorkerBlockCounts[j]}"); + // } + // availableCountSum += availableCountPerSync[i]; + //} + ////var idealOccupancy = 1.0 / workerCount; + ////Console.WriteLine($"Worker occupancy (ideal {idealOccupancy}):"); + ////for (int i = 0; i < workerCount; ++i) + ////{ + //// //Console.WriteLine($"{i}: {(workerBlocksCompletedSums[i] / (double)availableCountSum):G3}"); + //// Console.WriteLine($"{i}: {workerFractionSum[i] / syncCount:G3}"); + ////} + + //pool.Return(ref workerBlocksCompletedSums); + //pool.Return(ref workBlocksCompleted); + //pool.Return(ref debugStageWorkBlocksCompleted); + //pool.Return(ref availableCountPerSync); + pool.Return(ref claims); pool.Return(ref substepContext.Stages); pool.Return(ref substepContext.ConstraintBatchBoundaries); pool.Return(ref substepContext.IncrementalUpdateBlocks); pool.Return(ref substepContext.ConstraintBlocks); + + } diff --git a/BepuUtilities/Memory/Buffer.cs b/BepuUtilities/Memory/Buffer.cs index 801b14227..b0b0a9b0f 100644 --- a/BepuUtilities/Memory/Buffer.cs +++ b/BepuUtilities/Memory/Buffer.cs @@ -304,5 +304,4 @@ public static implicit operator ReadOnlySpan(in Buffer buffer) return new ReadOnlySpan(buffer.Memory, buffer.Length); } } - } \ No newline at end of file diff --git a/Demos/Program.cs b/Demos/Program.cs index d1cf79f64..de93fb6fd 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -19,9 +19,9 @@ static void Main(string[] args) { content = ContentArchive.Load(stream); } - //HeadlessTest.Test(content, 4, 32, 512); + HeadlessTest.Test(content, 4, 32, 512); //HeadlessTest.Test(content, 4, 32, 512); - HeadlessTest.Test(content, 4, 32, 512); + //HeadlessTest.Test(content, 4, 32, 512); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); From cdd632677a7cb4c6e3bd47e79e62ee99f0eba474 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 18 Aug 2021 20:52:39 -0500 Subject: [PATCH 159/947] Trying out a different stealing idea. Seems notgreat. --- BepuPhysics/Solver_SubsteppingSolve2.cs | 156 ++++++++++++++++-------- Demos/Program.cs | 4 +- 2 files changed, 109 insertions(+), 51 deletions(-) diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index 95da750e7..b20954c8f 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -211,59 +211,65 @@ public void Execute(Solver solver, int blockIndex, int workerIndex) } } - void ExecuteWorkerStage(ref TStageFunction stageFunction, int workerIndex, int workerStart, int availableBlocksStartIndex, ref Buffer claims, int previousSyncIndexOffset, int syncIndex, ref int completedWorkBlocks) where TStageFunction : IStageFunction + void ExecuteWorkerStage(ref TStageFunction stageFunction, int workerIndex, Buffer workerStarts, int availableBlocksStartIndex, ref Buffer claims, int previousSyncIndexOffset, int syncIndex, ref int completedWorkBlocks) where TStageFunction : IStageFunction { - if (workerStart == -1) + if (workerStarts.Length == 0) { //Thread count exceeds work block count; nothing for this worker to do. //(Technically, there's a possibility that an earlier thread would fail to wake and allowing this thread to steal that block COULD help, //but on average it's better to keep jobs scheduled to the same core to avoid excess memory traffic, and we can rely on some other active worker to take care of it.) return; } - int workBlockIndex = workerStart; int locallyCompletedCount = 0; - var previousSyncIndex = Math.Max(0, syncIndex - previousSyncIndexOffset); - //Try to claim blocks by traversing forward until we're blocked by another claim. - while (Interlocked.CompareExchange(ref claims[workBlockIndex], syncIndex, previousSyncIndex) == previousSyncIndex) + for (int indexInWorkerStarts = 0; indexInWorkerStarts < workerStarts.Length; ++indexInWorkerStarts) { - //Successfully claimed a work block. - stageFunction.Execute(this, availableBlocksStartIndex + workBlockIndex, workerIndex); - ++locallyCompletedCount; - ++workBlockIndex; - if (workBlockIndex >= claims.Length) + var workerStart = workerStarts[indexInWorkerStarts]; + int workBlockIndex = workerStart; + var previousSyncIndex = Math.Max(0, syncIndex - previousSyncIndexOffset); + //Try to claim blocks by traversing forward until we're blocked by another claim. + while (Interlocked.CompareExchange(ref claims[workBlockIndex], syncIndex, previousSyncIndex) == previousSyncIndex) { - //Wrap around. - workBlockIndex = 0; - } - } - //Try to claim work blocks going backward. - workBlockIndex = workerStart - 1; - while (true) - { - if (workBlockIndex < 0) - { - //Wrap around. - workBlockIndex = claims.Length - 1; + //Successfully claimed a work block. + stageFunction.Execute(this, availableBlocksStartIndex + workBlockIndex, workerIndex); + ++locallyCompletedCount; + ++workBlockIndex; + if (workBlockIndex >= claims.Length) + { + //Wrap around. + workBlockIndex = 0; + } } - if (Interlocked.CompareExchange(ref claims[workBlockIndex], syncIndex, previousSyncIndex) != previousSyncIndex) + //Try to claim work blocks going backward. + workBlockIndex = workerStart - 1; + while (true) { - break; + if (workBlockIndex < 0) + { + //Wrap around. + workBlockIndex = claims.Length - 1; + } + if (Interlocked.CompareExchange(ref claims[workBlockIndex], syncIndex, previousSyncIndex) != previousSyncIndex) + { + break; + } + //Successfully claimed a work block. + stageFunction.Execute(this, availableBlocksStartIndex + workBlockIndex, workerIndex); + ++locallyCompletedCount; + workBlockIndex--; } - //Successfully claimed a work block. - stageFunction.Execute(this, availableBlocksStartIndex + workBlockIndex, workerIndex); - ++locallyCompletedCount; - workBlockIndex--; } - //No more adjacent work blocks are available. This thread is done! + //No more work blocks are available. This thread is done! Interlocked.Add(ref completedWorkBlocks, locallyCompletedCount); + //debugStageWorkBlocksCompleted[syncIndex - 1][workerIndex] = locallyCompletedCount; //if (workerIndex == 3) //{ // Console.WriteLine($"Worker {workerIndex} completed {locallyCompletedCount / (double)claims.Length:G2} ({locallyCompletedCount} of {claims.Length})."); //} + } - void ExecuteMainStage(ref TStageFunction stageFunction, int workerIndex, int workerStart, ref SolverSyncStage stage, int previousSyncIndexOffset, ref int syncIndex) where TStageFunction : IStageFunction + void ExecuteMainStage(ref TStageFunction stageFunction, int workerIndex, Buffer workerStarts, ref SolverSyncStage stage, int previousSyncIndexOffset, ref int syncIndex) where TStageFunction : IStageFunction { //Note that the main thread's view of the sync index increments every single time, even if there is no work. //This ensures that the workers are able to advance to the appropriate stage by examining the sync index snapshot. @@ -287,7 +293,7 @@ void ExecuteMainStage(ref TStageFunction stageFunction, int work { //Write the new stage index so other spinning threads will begin work on it. Volatile.Write(ref substepContext.SyncIndex, syncIndex); - ExecuteWorkerStage(ref stageFunction, workerIndex, workerStart, stage.WorkBlockStartIndex, ref stage.Claims, previousSyncIndexOffset, syncIndex, ref substepContext.CompletedWorkBlockCount); + ExecuteWorkerStage(ref stageFunction, workerIndex, workerStarts, stage.WorkBlockStartIndex, ref stage.Claims, previousSyncIndexOffset, syncIndex, ref substepContext.CompletedWorkBlockCount); //Since we asked other threads to do work, we must wait until the requested work is done before proceeding. //Note that we DO NOT yield on the main thread! @@ -309,6 +315,47 @@ void ExecuteMainStage(ref TStageFunction stageFunction, int work SubstepMultithreadingContext substepContext; + const int targetStartsPerWorker = 2; + protected static Buffer GetUniformlyDistributedStarts(int workerIndex, int blockCount, int workerCount, ref int startsCounter, Buffer startsAllocation) + { + var startCount = startsCounter; + if (blockCount <= workerCount) + { + //Too few blocks to give every worker a job; give the jobs to the first context.WorkBlocks.Count workers. + if (workerIndex >= blockCount) + { + return default; + } + else + { + startsAllocation[startsCounter++] = workerIndex; + } + return startsAllocation.Slice(startCount, 1); + } + var blocksPerWorker = blockCount / workerCount; + var remainder = blockCount - blocksPerWorker * workerCount; + var previousStart = blocksPerWorker * workerIndex + Math.Min(remainder, workerIndex); + startsAllocation[startsCounter++] = previousStart; + previousStart += blocksPerWorker / 2; + if (blocksPerWorker >= 2) + { + for (int i = 1; i < targetStartsPerWorker; ++i) + { + var nextStart = previousStart + blocksPerWorker; + if (nextStart >= blockCount) + nextStart -= blockCount; + startsAllocation[startsCounter++] = nextStart; + previousStart = nextStart; + } + } + var toReturn = startsAllocation.Slice(startCount, startsCounter - startCount); + //for (int i = 0; i < toReturn.Length; ++i) + //{ + // if (toReturn[i] < 0 || toReturn[i] >= blockCount) + // Console.WriteLine("sugh"); + //} + return toReturn; + } Action solveStep2Worker2; void SolveStep2Worker2(int workerIndex) @@ -321,21 +368,32 @@ void SolveStep2Worker2(int workerIndex) //There's still nothing done if the OS unschedules an active worker that claimed work, but that's a far, far rarer concern. //Note that this attempts to maintain a given worker's relationship to a set of work blocks. This increases the probability that //data will remain in some cache that's reasonably close to the core. - int workerCount = substepContext.WorkerCount; - var incrementalUpdateWorkerStart = GetUniformlyDistributedStart(workerIndex, substepContext.IncrementalUpdateBlocks.Length, workerCount, 0); - int fallbackStart = GetUniformlyDistributedStart(workerIndex, substepContext.FallbackBlocks.Length, workerCount, 0); - Buffer batchStarts; + + var totalStartCount = 0; ref var activeSet = ref ActiveSet; + totalStartCount = targetStartsPerWorker * (ActiveSet.Batches.Count + 2); + + Buffer startsAllocation; unsafe { - var batchStartsData = stackalloc int[activeSet.Batches.Count]; - batchStarts = new Buffer(batchStartsData, activeSet.Batches.Count); + var rawWorkerStarts = stackalloc int[totalStartCount]; + startsAllocation = new Buffer(rawWorkerStarts, totalStartCount); + } + var startsCounter = 0; + int workerCount = substepContext.WorkerCount; + var incrementalUpdateWorkerStarts = GetUniformlyDistributedStarts(workerIndex, substepContext.IncrementalUpdateBlocks.Length, workerCount, ref startsCounter, startsAllocation); + var fallbackStarts = GetUniformlyDistributedStarts(workerIndex, substepContext.FallbackBlocks.Length, workerCount, ref startsCounter, startsAllocation); + Buffer> batchStarts; + unsafe + { + var batchStartsData = stackalloc Buffer[activeSet.Batches.Count]; + batchStarts = new Buffer>(batchStartsData, activeSet.Batches.Count); } for (int batchIndex = 0; batchIndex < activeSet.Batches.Count; ++batchIndex) { var batchOffset = batchIndex > 0 ? substepContext.ConstraintBatchBoundaries[batchIndex - 1] : 0; var batchCount = substepContext.ConstraintBatchBoundaries[batchIndex] - batchOffset; - batchStarts[batchIndex] = GetUniformlyDistributedStart(workerIndex, batchCount, workerCount, 0); + batchStarts[batchIndex] = GetUniformlyDistributedStarts(workerIndex, batchCount, workerCount, ref startsCounter, startsAllocation); } Debug.Assert(activeSet.Batches.Count > 0, "Don't dispatch if there are no constraints."); @@ -369,7 +427,7 @@ void SolveStep2Worker2(int workerIndex) { if (substepIndex > 0) { - ExecuteMainStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStart, ref substepContext.Stages[0], 1 + baseStageCountInSubstep, ref syncIndex); + ExecuteMainStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStarts, ref substepContext.Stages[0], 1 + baseStageCountInSubstep, ref syncIndex); } for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) { @@ -432,7 +490,7 @@ void SolveStep2Worker2(int workerIndex) { //Incremental update. ref var stage = ref substepContext.Stages[stageIndex]; - ExecuteWorkerStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStart, 0, ref stage.Claims, 1 + baseStageCountInSubstep, syncIndex, ref substepContext.CompletedWorkBlockCount); + ExecuteWorkerStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStarts, 0, ref stage.Claims, 1 + baseStageCountInSubstep, syncIndex, ref substepContext.CompletedWorkBlockCount); } else if (stageIndex < 1 + synchronizedBatchCount) { @@ -466,7 +524,7 @@ protected void ExecuteMultithreaded2(float dt, IThreadDispatcher threadDispatche //These values are found by empirical tuning. The optimal values may vary by architecture. //The goal here is to have just enough blocks that, in the event that we end up some underpowered threads (due to competition or hyperthreading), //there are enough blocks that workstealing will still generally allow the extra threads to be useful. - const int targetBlocksPerBatchPerWorker = 32; + const int targetBlocksPerBatchPerWorker = 4; const int minimumBlockSizeInBundles = 1; const int maximumBlockSizeInBundles = 1024; @@ -564,13 +622,13 @@ protected void ExecuteMultithreaded2(float dt, IThreadDispatcher threadDispatche // } // availableCountSum += availableCountPerSync[i]; //} - ////var idealOccupancy = 1.0 / workerCount; - ////Console.WriteLine($"Worker occupancy (ideal {idealOccupancy}):"); - ////for (int i = 0; i < workerCount; ++i) - ////{ - //// //Console.WriteLine($"{i}: {(workerBlocksCompletedSums[i] / (double)availableCountSum):G3}"); - //// Console.WriteLine($"{i}: {workerFractionSum[i] / syncCount:G3}"); - ////} + //var idealOccupancy = 1.0 / workerCount; + //Console.WriteLine($"Worker occupancy (ideal {idealOccupancy}):"); + //for (int i = 0; i < workerCount; ++i) + //{ + // //Console.WriteLine($"{i}: {(workerBlocksCompletedSums[i] / (double)availableCountSum):G3}"); + // Console.WriteLine($"{i}: {workerFractionSum[i] / syncCount:G3}"); + //} //pool.Return(ref workerBlocksCompletedSums); //pool.Return(ref workBlocksCompleted); diff --git a/Demos/Program.cs b/Demos/Program.cs index de93fb6fd..0b1b9abba 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -19,9 +19,9 @@ static void Main(string[] args) { content = ContentArchive.Load(stream); } - HeadlessTest.Test(content, 4, 32, 512); + HeadlessTest.Test(content, 64, 32, 512); //HeadlessTest.Test(content, 4, 32, 512); - //HeadlessTest.Test(content, 4, 32, 512); + //HeadlessTest.Test(content, 64, 32, 512); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); From 79a584bf1b3598edad493cd29b522cc93d1c79f1 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 18 Aug 2021 21:10:01 -0500 Subject: [PATCH 160/947] This'll do. --- BepuPhysics/Solver_SubsteppingSolve2.cs | 156 ++++++++---------------- Demos/Program.cs | 4 +- 2 files changed, 51 insertions(+), 109 deletions(-) diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index b20954c8f..5ae210d3e 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -211,65 +211,59 @@ public void Execute(Solver solver, int blockIndex, int workerIndex) } } - void ExecuteWorkerStage(ref TStageFunction stageFunction, int workerIndex, Buffer workerStarts, int availableBlocksStartIndex, ref Buffer claims, int previousSyncIndexOffset, int syncIndex, ref int completedWorkBlocks) where TStageFunction : IStageFunction + void ExecuteWorkerStage(ref TStageFunction stageFunction, int workerIndex, int workerStart, int availableBlocksStartIndex, ref Buffer claims, int previousSyncIndexOffset, int syncIndex, ref int completedWorkBlocks) where TStageFunction : IStageFunction { - if (workerStarts.Length == 0) + if (workerStart == -1) { //Thread count exceeds work block count; nothing for this worker to do. //(Technically, there's a possibility that an earlier thread would fail to wake and allowing this thread to steal that block COULD help, //but on average it's better to keep jobs scheduled to the same core to avoid excess memory traffic, and we can rely on some other active worker to take care of it.) return; } + int workBlockIndex = workerStart; int locallyCompletedCount = 0; - for (int indexInWorkerStarts = 0; indexInWorkerStarts < workerStarts.Length; ++indexInWorkerStarts) + var previousSyncIndex = Math.Max(0, syncIndex - previousSyncIndexOffset); + //Try to claim blocks by traversing forward until we're blocked by another claim. + while (Interlocked.CompareExchange(ref claims[workBlockIndex], syncIndex, previousSyncIndex) == previousSyncIndex) { - var workerStart = workerStarts[indexInWorkerStarts]; - int workBlockIndex = workerStart; - var previousSyncIndex = Math.Max(0, syncIndex - previousSyncIndexOffset); - //Try to claim blocks by traversing forward until we're blocked by another claim. - while (Interlocked.CompareExchange(ref claims[workBlockIndex], syncIndex, previousSyncIndex) == previousSyncIndex) + //Successfully claimed a work block. + stageFunction.Execute(this, availableBlocksStartIndex + workBlockIndex, workerIndex); + ++locallyCompletedCount; + ++workBlockIndex; + if (workBlockIndex >= claims.Length) { - //Successfully claimed a work block. - stageFunction.Execute(this, availableBlocksStartIndex + workBlockIndex, workerIndex); - ++locallyCompletedCount; - ++workBlockIndex; - if (workBlockIndex >= claims.Length) - { - //Wrap around. - workBlockIndex = 0; - } + //Wrap around. + workBlockIndex = 0; } - //Try to claim work blocks going backward. - workBlockIndex = workerStart - 1; - while (true) + } + //Try to claim work blocks going backward. + workBlockIndex = workerStart - 1; + while (true) + { + if (workBlockIndex < 0) { - if (workBlockIndex < 0) - { - //Wrap around. - workBlockIndex = claims.Length - 1; - } - if (Interlocked.CompareExchange(ref claims[workBlockIndex], syncIndex, previousSyncIndex) != previousSyncIndex) - { - break; - } - //Successfully claimed a work block. - stageFunction.Execute(this, availableBlocksStartIndex + workBlockIndex, workerIndex); - ++locallyCompletedCount; - workBlockIndex--; + //Wrap around. + workBlockIndex = claims.Length - 1; } + if (Interlocked.CompareExchange(ref claims[workBlockIndex], syncIndex, previousSyncIndex) != previousSyncIndex) + { + break; + } + //Successfully claimed a work block. + stageFunction.Execute(this, availableBlocksStartIndex + workBlockIndex, workerIndex); + ++locallyCompletedCount; + workBlockIndex--; } - //No more work blocks are available. This thread is done! + //No more adjacent work blocks are available. This thread is done! Interlocked.Add(ref completedWorkBlocks, locallyCompletedCount); - //debugStageWorkBlocksCompleted[syncIndex - 1][workerIndex] = locallyCompletedCount; //if (workerIndex == 3) //{ // Console.WriteLine($"Worker {workerIndex} completed {locallyCompletedCount / (double)claims.Length:G2} ({locallyCompletedCount} of {claims.Length})."); //} - } - void ExecuteMainStage(ref TStageFunction stageFunction, int workerIndex, Buffer workerStarts, ref SolverSyncStage stage, int previousSyncIndexOffset, ref int syncIndex) where TStageFunction : IStageFunction + void ExecuteMainStage(ref TStageFunction stageFunction, int workerIndex, int workerStart, ref SolverSyncStage stage, int previousSyncIndexOffset, ref int syncIndex) where TStageFunction : IStageFunction { //Note that the main thread's view of the sync index increments every single time, even if there is no work. //This ensures that the workers are able to advance to the appropriate stage by examining the sync index snapshot. @@ -293,7 +287,7 @@ void ExecuteMainStage(ref TStageFunction stageFunction, int work { //Write the new stage index so other spinning threads will begin work on it. Volatile.Write(ref substepContext.SyncIndex, syncIndex); - ExecuteWorkerStage(ref stageFunction, workerIndex, workerStarts, stage.WorkBlockStartIndex, ref stage.Claims, previousSyncIndexOffset, syncIndex, ref substepContext.CompletedWorkBlockCount); + ExecuteWorkerStage(ref stageFunction, workerIndex, workerStart, stage.WorkBlockStartIndex, ref stage.Claims, previousSyncIndexOffset, syncIndex, ref substepContext.CompletedWorkBlockCount); //Since we asked other threads to do work, we must wait until the requested work is done before proceeding. //Note that we DO NOT yield on the main thread! @@ -315,47 +309,6 @@ void ExecuteMainStage(ref TStageFunction stageFunction, int work SubstepMultithreadingContext substepContext; - const int targetStartsPerWorker = 2; - protected static Buffer GetUniformlyDistributedStarts(int workerIndex, int blockCount, int workerCount, ref int startsCounter, Buffer startsAllocation) - { - var startCount = startsCounter; - if (blockCount <= workerCount) - { - //Too few blocks to give every worker a job; give the jobs to the first context.WorkBlocks.Count workers. - if (workerIndex >= blockCount) - { - return default; - } - else - { - startsAllocation[startsCounter++] = workerIndex; - } - return startsAllocation.Slice(startCount, 1); - } - var blocksPerWorker = blockCount / workerCount; - var remainder = blockCount - blocksPerWorker * workerCount; - var previousStart = blocksPerWorker * workerIndex + Math.Min(remainder, workerIndex); - startsAllocation[startsCounter++] = previousStart; - previousStart += blocksPerWorker / 2; - if (blocksPerWorker >= 2) - { - for (int i = 1; i < targetStartsPerWorker; ++i) - { - var nextStart = previousStart + blocksPerWorker; - if (nextStart >= blockCount) - nextStart -= blockCount; - startsAllocation[startsCounter++] = nextStart; - previousStart = nextStart; - } - } - var toReturn = startsAllocation.Slice(startCount, startsCounter - startCount); - //for (int i = 0; i < toReturn.Length; ++i) - //{ - // if (toReturn[i] < 0 || toReturn[i] >= blockCount) - // Console.WriteLine("sugh"); - //} - return toReturn; - } Action solveStep2Worker2; void SolveStep2Worker2(int workerIndex) @@ -368,32 +321,21 @@ void SolveStep2Worker2(int workerIndex) //There's still nothing done if the OS unschedules an active worker that claimed work, but that's a far, far rarer concern. //Note that this attempts to maintain a given worker's relationship to a set of work blocks. This increases the probability that //data will remain in some cache that's reasonably close to the core. - - var totalStartCount = 0; - ref var activeSet = ref ActiveSet; - totalStartCount = targetStartsPerWorker * (ActiveSet.Batches.Count + 2); - - Buffer startsAllocation; - unsafe - { - var rawWorkerStarts = stackalloc int[totalStartCount]; - startsAllocation = new Buffer(rawWorkerStarts, totalStartCount); - } - var startsCounter = 0; int workerCount = substepContext.WorkerCount; - var incrementalUpdateWorkerStarts = GetUniformlyDistributedStarts(workerIndex, substepContext.IncrementalUpdateBlocks.Length, workerCount, ref startsCounter, startsAllocation); - var fallbackStarts = GetUniformlyDistributedStarts(workerIndex, substepContext.FallbackBlocks.Length, workerCount, ref startsCounter, startsAllocation); - Buffer> batchStarts; + var incrementalUpdateWorkerStart = GetUniformlyDistributedStart(workerIndex, substepContext.IncrementalUpdateBlocks.Length, workerCount, 0); + int fallbackStart = GetUniformlyDistributedStart(workerIndex, substepContext.FallbackBlocks.Length, workerCount, 0); + Buffer batchStarts; + ref var activeSet = ref ActiveSet; unsafe { - var batchStartsData = stackalloc Buffer[activeSet.Batches.Count]; - batchStarts = new Buffer>(batchStartsData, activeSet.Batches.Count); + var batchStartsData = stackalloc int[activeSet.Batches.Count]; + batchStarts = new Buffer(batchStartsData, activeSet.Batches.Count); } for (int batchIndex = 0; batchIndex < activeSet.Batches.Count; ++batchIndex) { var batchOffset = batchIndex > 0 ? substepContext.ConstraintBatchBoundaries[batchIndex - 1] : 0; var batchCount = substepContext.ConstraintBatchBoundaries[batchIndex] - batchOffset; - batchStarts[batchIndex] = GetUniformlyDistributedStarts(workerIndex, batchCount, workerCount, ref startsCounter, startsAllocation); + batchStarts[batchIndex] = GetUniformlyDistributedStart(workerIndex, batchCount, workerCount, 0); } Debug.Assert(activeSet.Batches.Count > 0, "Don't dispatch if there are no constraints."); @@ -427,7 +369,7 @@ void SolveStep2Worker2(int workerIndex) { if (substepIndex > 0) { - ExecuteMainStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStarts, ref substepContext.Stages[0], 1 + baseStageCountInSubstep, ref syncIndex); + ExecuteMainStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStart, ref substepContext.Stages[0], 1 + baseStageCountInSubstep, ref syncIndex); } for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) { @@ -490,7 +432,7 @@ void SolveStep2Worker2(int workerIndex) { //Incremental update. ref var stage = ref substepContext.Stages[stageIndex]; - ExecuteWorkerStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStarts, 0, ref stage.Claims, 1 + baseStageCountInSubstep, syncIndex, ref substepContext.CompletedWorkBlockCount); + ExecuteWorkerStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStart, 0, ref stage.Claims, 1 + baseStageCountInSubstep, syncIndex, ref substepContext.CompletedWorkBlockCount); } else if (stageIndex < 1 + synchronizedBatchCount) { @@ -513,7 +455,7 @@ void SolveStep2Worker2(int workerIndex) } - Buffer> debugStageWorkBlocksCompleted; + //Buffer> debugStageWorkBlocksCompleted; protected void ExecuteMultithreaded2(float dt, IThreadDispatcher threadDispatcher, Action workDelegate) { var workerCount = substepContext.WorkerCount = threadDispatcher.ThreadCount; @@ -622,13 +564,13 @@ protected void ExecuteMultithreaded2(float dt, IThreadDispatcher threadDispatche // } // availableCountSum += availableCountPerSync[i]; //} - //var idealOccupancy = 1.0 / workerCount; - //Console.WriteLine($"Worker occupancy (ideal {idealOccupancy}):"); - //for (int i = 0; i < workerCount; ++i) - //{ - // //Console.WriteLine($"{i}: {(workerBlocksCompletedSums[i] / (double)availableCountSum):G3}"); - // Console.WriteLine($"{i}: {workerFractionSum[i] / syncCount:G3}"); - //} + ////var idealOccupancy = 1.0 / workerCount; + ////Console.WriteLine($"Worker occupancy (ideal {idealOccupancy}):"); + ////for (int i = 0; i < workerCount; ++i) + ////{ + //// //Console.WriteLine($"{i}: {(workerBlocksCompletedSums[i] / (double)availableCountSum):G3}"); + //// Console.WriteLine($"{i}: {workerFractionSum[i] / syncCount:G3}"); + ////} //pool.Return(ref workerBlocksCompletedSums); //pool.Return(ref workBlocksCompleted); diff --git a/Demos/Program.cs b/Demos/Program.cs index 0b1b9abba..de93fb6fd 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -19,9 +19,9 @@ static void Main(string[] args) { content = ContentArchive.Load(stream); } - HeadlessTest.Test(content, 64, 32, 512); + HeadlessTest.Test(content, 4, 32, 512); //HeadlessTest.Test(content, 4, 32, 512); - //HeadlessTest.Test(content, 64, 32, 512); + //HeadlessTest.Test(content, 4, 32, 512); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); From bdc9c257236d228c7e56b8242c7b3f91462df31d Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 25 Sep 2021 15:04:54 -0500 Subject: [PATCH 161/947] Nonconvex contacts 2-ified. Prestep/impulse ref-ified in signatures to meet nonconvex requirements. Bumped demos to .net 6, at least for now. --- .../Constraints/AngularAxisGearMotor.cs | 4 +- BepuPhysics/Constraints/AngularAxisMotor.cs | 4 +- BepuPhysics/Constraints/AngularHinge.cs | 4 +- BepuPhysics/Constraints/AngularMotor.cs | 4 +- BepuPhysics/Constraints/AngularServo.cs | 4 +- BepuPhysics/Constraints/AngularSwivelHinge.cs | 4 +- BepuPhysics/Constraints/AreaConstraint.cs | 4 +- BepuPhysics/Constraints/BallSocket.cs | 4 +- BepuPhysics/Constraints/BallSocketMotor.cs | 4 +- BepuPhysics/Constraints/BallSocketServo.cs | 4 +- .../Constraints/CenterDistanceConstraint.cs | 4 +- .../Constraints/Contact/ContactConvexTypes.cs | 32 ++++---- .../Constraints/Contact/ContactConvexTypes.tt | 4 +- .../Contact/ContactNonconvexCommon.cs | 82 ++++++++++++++++--- BepuPhysics/Constraints/DistanceLimit.cs | 4 +- BepuPhysics/Constraints/DistanceServo.cs | 4 +- .../Constraints/FourBodyTypeProcessor.cs | 8 +- BepuPhysics/Constraints/Hinge.cs | 4 +- BepuPhysics/Constraints/LinearAxisLimit.cs | 4 +- BepuPhysics/Constraints/LinearAxisMotor.cs | 4 +- BepuPhysics/Constraints/LinearAxisServo.cs | 4 +- .../Constraints/OneBodyAngularMotor.cs | 4 +- .../Constraints/OneBodyAngularServo.cs | 4 +- BepuPhysics/Constraints/OneBodyLinearMotor.cs | 4 +- BepuPhysics/Constraints/OneBodyLinearServo.cs | 4 +- .../Constraints/OneBodyTypeProcessor.cs | 8 +- BepuPhysics/Constraints/PointOnLineServo.cs | 4 +- BepuPhysics/Constraints/SwingLimit.cs | 4 +- BepuPhysics/Constraints/SwivelHinge.cs | 4 +- .../Constraints/ThreeBodyTypeProcessor.cs | 8 +- BepuPhysics/Constraints/TwistLimit.cs | 4 +- BepuPhysics/Constraints/TwistMotor.cs | 4 +- BepuPhysics/Constraints/TwistServo.cs | 4 +- .../Constraints/TwoBodyTypeProcessor.cs | 8 +- BepuPhysics/Constraints/VolumeConstraint.cs | 4 +- BepuPhysics/Constraints/Weld.cs | 4 +- DemoTests/DemoTests.csproj | 2 +- Demos/Demos.csproj | 2 +- .../Characters/CharacterMotionConstraint.cs | 8 +- .../Characters/CharacterMotionConstraint.tt | 4 +- Demos/Program.cs | 2 +- 41 files changed, 170 insertions(+), 114 deletions(-) diff --git a/BepuPhysics/Constraints/AngularAxisGearMotor.cs b/BepuPhysics/Constraints/AngularAxisGearMotor.cs index 712de5683..29d777f9e 100644 --- a/BepuPhysics/Constraints/AngularAxisGearMotor.cs +++ b/BepuPhysics/Constraints/AngularAxisGearMotor.cs @@ -139,14 +139,14 @@ public static void ApplyImpulse(in Vector3Wide jA, in Vector3Wide negatedJB, in } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in AngularAxisGearMotorPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref AngularAxisGearMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { QuaternionWide.TransformWithoutOverlap(prestep.LocalAxisA, orientationA, out var axis); ApplyImpulse(axis * prestep.VelocityScale, axis, accumulatedImpulses, inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, ref wsvA.Angular, ref wsvB.Angular); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in AngularAxisGearMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref AngularAxisGearMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { QuaternionWide.TransformWithoutOverlap(prestep.LocalAxisA, orientationA, out var axis); Vector3Wide.Scale(axis, prestep.VelocityScale, out var jA); diff --git a/BepuPhysics/Constraints/AngularAxisMotor.cs b/BepuPhysics/Constraints/AngularAxisMotor.cs index 775e8c56f..104d2cb0f 100644 --- a/BepuPhysics/Constraints/AngularAxisMotor.cs +++ b/BepuPhysics/Constraints/AngularAxisMotor.cs @@ -136,14 +136,14 @@ public static void ApplyImpulse(in Vector3Wide jA, in Vector csi, in Symm } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in AngularAxisMotorPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref AngularAxisMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { QuaternionWide.TransformWithoutOverlap(prestep.LocalAxisA, orientationA, out var axis); ApplyImpulse(axis, accumulatedImpulses, inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, ref wsvA.Angular, ref wsvB.Angular); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in AngularAxisMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref AngularAxisMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { QuaternionWide.TransformWithoutOverlap(prestep.LocalAxisA, orientationA, out var jA); Symmetric3x3Wide.TransformWithoutOverlap(jA, inertiaA.InverseInertiaTensor, out var jIA); diff --git a/BepuPhysics/Constraints/AngularHinge.cs b/BepuPhysics/Constraints/AngularHinge.cs index 482736a40..f00c95c3c 100644 --- a/BepuPhysics/Constraints/AngularHinge.cs +++ b/BepuPhysics/Constraints/AngularHinge.cs @@ -226,12 +226,12 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, ref projection, ref csi); } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in AngularHingePrestepData prestep, in Vector2Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref AngularHingePrestepData prestep, ref Vector2Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in AngularHingePrestepData prestep, ref Vector2Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref AngularHingePrestepData prestep, ref Vector2Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/AngularMotor.cs b/BepuPhysics/Constraints/AngularMotor.cs index aac11cdb6..88b2d4312 100644 --- a/BepuPhysics/Constraints/AngularMotor.cs +++ b/BepuPhysics/Constraints/AngularMotor.cs @@ -101,12 +101,12 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB projection.MaximumImpulse, projection.ImpulseToVelocityA, projection.NegatedImpulseToVelocityB, ref accumulatedImpulse); } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in AngularMotorPrestepData prestep, in Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref AngularMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in AngularMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref AngularMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/AngularServo.cs b/BepuPhysics/Constraints/AngularServo.cs index b9d4dbdf9..79112e8d4 100644 --- a/BepuPhysics/Constraints/AngularServo.cs +++ b/BepuPhysics/Constraints/AngularServo.cs @@ -142,12 +142,12 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB projection.MaximumImpulse, projection.ImpulseToVelocityA, projection.NegatedImpulseToVelocityB, ref accumulatedImpulse); } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in AngularServoPrestepData prestep, in Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref AngularServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in AngularServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref AngularServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/AngularSwivelHinge.cs b/BepuPhysics/Constraints/AngularSwivelHinge.cs index 0a33eda6e..a59fd6b5d 100644 --- a/BepuPhysics/Constraints/AngularSwivelHinge.cs +++ b/BepuPhysics/Constraints/AngularSwivelHinge.cs @@ -154,12 +154,12 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, ref projection, ref csi); } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in AngularSwivelHingePrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref AngularSwivelHingePrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in AngularSwivelHingePrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref AngularSwivelHingePrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/AreaConstraint.cs b/BepuPhysics/Constraints/AreaConstraint.cs index 18397c332..58834847b 100644 --- a/BepuPhysics/Constraints/AreaConstraint.cs +++ b/BepuPhysics/Constraints/AreaConstraint.cs @@ -185,12 +185,12 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ApplyImpulse(ref velocityA, ref velocityB, ref velocityC, ref projection, ref negatedJacobianA, ref csi); } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide ac, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in AreaConstraintPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide ac, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, ref AreaConstraintPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC) { throw new NotImplementedException(); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide ac, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, float dt, float inverseDt, in AreaConstraintPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide ac, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, float dt, float inverseDt, ref AreaConstraintPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/BallSocket.cs b/BepuPhysics/Constraints/BallSocket.cs index 0d6491c7d..e0a9d9a73 100644 --- a/BepuPhysics/Constraints/BallSocket.cs +++ b/BepuPhysics/Constraints/BallSocket.cs @@ -110,7 +110,7 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB } public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, - in BallSocketPrestepData prestep, in Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + ref BallSocketPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { //Note that we must reconstruct the world offsets from the body orientations since we do not store world offsets. QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetA, orientationA, out var offsetA); @@ -119,7 +119,7 @@ public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, } public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, - in BallSocketPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + ref BallSocketPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { //Note that we must reconstruct the world offsets from the body orientations since we do not store world offsets. QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetA, orientationA, out var offsetA); diff --git a/BepuPhysics/Constraints/BallSocketMotor.cs b/BepuPhysics/Constraints/BallSocketMotor.cs index daed5ac6b..dea4ab0ed 100644 --- a/BepuPhysics/Constraints/BallSocketMotor.cs +++ b/BepuPhysics/Constraints/BallSocketMotor.cs @@ -111,12 +111,12 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB BallSocketShared.Solve(ref velocityA, ref velocityB, ref projection.OffsetA, ref projection.OffsetB, ref projection.BiasVelocity, ref projection.EffectiveMass, ref projection.SoftnessImpulseScale, ref projection.MaximumImpulse, ref accumulatedImpulse, ref projection.InertiaA, ref projection.InertiaB); } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BallSocketMotorPrestepData prestep, in Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref BallSocketMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in BallSocketMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref BallSocketMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/BallSocketServo.cs b/BepuPhysics/Constraints/BallSocketServo.cs index 0bf2041e1..6359b509f 100644 --- a/BepuPhysics/Constraints/BallSocketServo.cs +++ b/BepuPhysics/Constraints/BallSocketServo.cs @@ -118,12 +118,12 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB BallSocketShared.Solve(ref velocityA, ref velocityB, ref projection.OffsetA, ref projection.OffsetB, ref projection.BiasVelocity, ref projection.EffectiveMass, ref projection.SoftnessImpulseScale, ref projection.MaximumImpulse, ref accumulatedImpulse, ref projection.InertiaA, ref projection.InertiaB); } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BallSocketServoPrestepData prestep, in Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref BallSocketServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in BallSocketServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref BallSocketServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/CenterDistanceConstraint.cs b/BepuPhysics/Constraints/CenterDistanceConstraint.cs index e50e73d48..c3839e3f2 100644 --- a/BepuPhysics/Constraints/CenterDistanceConstraint.cs +++ b/BepuPhysics/Constraints/CenterDistanceConstraint.cs @@ -138,7 +138,7 @@ static void ApplyImpulse(in Vector3Wide jacobianA, in Vector inverseMassA //[MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, - in CenterDistancePrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + ref CenterDistancePrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { var ab = positionB - positionA; var lengthSquared = ab.LengthSquared(); @@ -153,7 +153,7 @@ public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, } //[MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, - in CenterDistancePrestepData prestep, ref Vector accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + ref CenterDistancePrestepData prestep, ref Vector accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { //Note that we need the actual length for error calculation. var ab = positionB - positionA; diff --git a/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs b/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs index f269444fb..bd07c3164 100644 --- a/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs +++ b/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs @@ -352,7 +352,7 @@ public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityW } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Contact1OneBodyPrestepData prestep, in Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref Contact1OneBodyPrestepData prestep, ref Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); TangentFrictionOneBody.WarmStart2(x, z, prestep.Contact0.OffsetA, inertiaA, accumulatedImpulses.Tangent, ref wsvA); @@ -361,7 +361,7 @@ public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, in Contact1OneBodyPrestepData prestep, ref Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref Contact1OneBodyPrestepData prestep, ref Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); var maximumTangentImpulse = prestep.MaterialProperties.FrictionCoefficient * (accumulatedImpulses.Penetration0); @@ -567,7 +567,7 @@ public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityW } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Contact2OneBodyPrestepData prestep, in Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref Contact2OneBodyPrestepData prestep, ref Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, out var offsetToManifoldCenterA); @@ -578,7 +578,7 @@ public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, in Contact2OneBodyPrestepData prestep, ref Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref Contact2OneBodyPrestepData prestep, ref Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); var premultipliedFrictionCoefficient = new Vector(1f / 2f) * prestep.MaterialProperties.FrictionCoefficient; @@ -802,7 +802,7 @@ public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityW } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Contact3OneBodyPrestepData prestep, in Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref Contact3OneBodyPrestepData prestep, ref Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, out var offsetToManifoldCenterA); @@ -814,7 +814,7 @@ public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, in Contact3OneBodyPrestepData prestep, ref Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref Contact3OneBodyPrestepData prestep, ref Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); var premultipliedFrictionCoefficient = new Vector(1f / 3f) * prestep.MaterialProperties.FrictionCoefficient; @@ -1055,7 +1055,7 @@ public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityW } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Contact4OneBodyPrestepData prestep, in Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref Contact4OneBodyPrestepData prestep, ref Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact3.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, prestep.Contact3.Depth, out var offsetToManifoldCenterA); @@ -1068,7 +1068,7 @@ public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, in Contact4OneBodyPrestepData prestep, ref Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref Contact4OneBodyPrestepData prestep, ref Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); var premultipliedFrictionCoefficient = new Vector(1f / 4f) * prestep.MaterialProperties.FrictionCoefficient; @@ -1287,7 +1287,7 @@ public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityW } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Contact1PrestepData prestep, in Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref Contact1PrestepData prestep, ref Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); Vector3Wide.Subtract(prestep.Contact0.OffsetA, prestep.OffsetB, out var offsetToManifoldCenterB); @@ -1297,7 +1297,7 @@ public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in Contact1PrestepData prestep, ref Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref Contact1PrestepData prestep, ref Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); var maximumTangentImpulse = prestep.MaterialProperties.FrictionCoefficient * (accumulatedImpulses.Penetration0); @@ -1523,7 +1523,7 @@ public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityW } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Contact2PrestepData prestep, in Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref Contact2PrestepData prestep, ref Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, out var offsetToManifoldCenterA); @@ -1535,7 +1535,7 @@ public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in Contact2PrestepData prestep, ref Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref Contact2PrestepData prestep, ref Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); var premultipliedFrictionCoefficient = new Vector(1f / 2f) * prestep.MaterialProperties.FrictionCoefficient; @@ -1780,7 +1780,7 @@ public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityW } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Contact3PrestepData prestep, in Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref Contact3PrestepData prestep, ref Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, out var offsetToManifoldCenterA); @@ -1793,7 +1793,7 @@ public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in Contact3PrestepData prestep, ref Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref Contact3PrestepData prestep, ref Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); var premultipliedFrictionCoefficient = new Vector(1f / 3f) * prestep.MaterialProperties.FrictionCoefficient; @@ -2056,7 +2056,7 @@ public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityW } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Contact4PrestepData prestep, in Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref Contact4PrestepData prestep, ref Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact3.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, prestep.Contact3.Depth, out var offsetToManifoldCenterA); @@ -2070,7 +2070,7 @@ public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in Contact4PrestepData prestep, ref Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref Contact4PrestepData prestep, ref Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); var premultipliedFrictionCoefficient = new Vector(1f / 4f) * prestep.MaterialProperties.FrictionCoefficient; diff --git a/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt b/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt index c6234671e..a2892ec98 100644 --- a/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt +++ b/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt @@ -321,7 +321,7 @@ for (int i = 0; i < contactCount; ++i) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(bodyCount == 2) {#>in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>in Contact<#=contactCount#><#=suffix#>PrestepData prestep, in Contact<#=contactCount#>AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA<#if(bodyCount == 2) {#>, ref BodyVelocityWide wsvB<#}#>) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(bodyCount == 2) {#>in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>ref Contact<#=contactCount#><#=suffix#>PrestepData prestep, ref Contact<#=contactCount#>AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA<#if(bodyCount == 2) {#>, ref BodyVelocityWide wsvB<#}#>) { Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); <#if (contactCount > 1) {#> @@ -340,7 +340,7 @@ for (int i = 0; i < contactCount; ++i) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(bodyCount == 2) {#>in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>float dt, float inverseDt, in Contact<#=contactCount#><#=suffix#>PrestepData prestep, ref Contact<#=contactCount#>AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA<#if(bodyCount == 2) {#>, ref BodyVelocityWide wsvB<#}#>) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(bodyCount == 2) {#>in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>float dt, float inverseDt, ref Contact<#=contactCount#><#=suffix#>PrestepData prestep, ref Contact<#=contactCount#>AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA<#if(bodyCount == 2) {#>, ref BodyVelocityWide wsvB<#}#>) { Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); <#if (contactCount > 1) {#> diff --git a/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs b/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs index 9bfbbca06..c158cdeb3 100644 --- a/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs +++ b/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs @@ -294,19 +294,45 @@ public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityW } } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in TPrestep prestep, in TAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref TPrestep prestep, ref TAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { - throw new System.NotImplementedException(); + ref var prestepMaterial = ref prestep.GetMaterialProperties(ref prestep); + ref var prestepContactStart = ref prestep.GetContact(ref prestep, 0); + ref var accumulatedImpulsesStart = ref Unsafe.As(ref accumulatedImpulses); + for (int i = 0; i < prestep.ContactCount; ++i) + { + ref var prestepContact = ref Unsafe.Add(ref prestepContactStart, i); + Helpers.BuildOrthonormalBasis(prestepContact.Normal, out var x, out var z); + ref var contactImpulse = ref Unsafe.Add(ref accumulatedImpulsesStart, i); + TangentFrictionOneBody.WarmStart2(x, z, prestepContact.Offset, inertiaA, contactImpulse.Tangent, ref wsvA); + PenetrationLimitOneBody.WarmStart2(inertiaA, prestepContact.Normal, prestepContact.Offset, contactImpulse.Penetration, ref wsvA); + } } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, in TPrestep prestep, ref TAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) - { - throw new System.NotImplementedException(); + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref TPrestep prestep, ref TAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) + { + //Note that, unlike convex manifolds, we simply solve every contact in sequence rather than tangent->penetration. + //This is not for any principled reason- only simplicity. May want to reconsider later, but remember the significant change in access pattern. + ref var prestepMaterial = ref prestep.GetMaterialProperties(ref prestep); + ref var accumulatedImpulsesStart = ref Unsafe.As(ref accumulatedImpulses); + ref var prestepContactStart = ref prestep.GetContact(ref prestep, 0); + SpringSettingsWide.ComputeSpringiness(prestepMaterial.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); + var inverseDtWide = new Vector(inverseDt); + for (int i = 0; i < prestep.ContactCount; ++i) + { + ref var contact = ref Unsafe.Add(ref prestepContactStart, i); + ref var contactImpulse = ref Unsafe.Add(ref accumulatedImpulsesStart, i); + Helpers.BuildOrthonormalBasis(contact.Normal, out var x, out var z); + var maximumTangentImpulse = prestepMaterial.FrictionCoefficient * contactImpulse.Penetration; + TangentFrictionOneBody.Solve2(x, z, contact.Offset, inertiaA, maximumTangentImpulse, ref contactImpulse.Tangent, ref wsvA); + PenetrationLimitOneBody.Solve2(inertiaA, contact.Normal, contact.Offset, contact.Depth, + positionErrorToVelocity, effectiveMassCFMScale, prestepMaterial.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref contactImpulse.Penetration, ref wsvA); + } } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector dt, in TAccumulatedImpulses accumulatedImpulses, ref TPrestep prestep) { throw new System.NotImplementedException(); @@ -345,7 +371,7 @@ public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, Vector3Wide.Subtract(prestepContact.Offset, prestepOffsetB, out var contactOffsetB); TangentFriction.Prestep(ref x, ref z, ref prestepContact.Offset, ref contactOffsetB, ref projectionCommon.InertiaA, ref projectionCommon.InertiaB, out projectionContact.Tangent); PenetrationLimit.Prestep(projectionCommon.InertiaA, projectionCommon.InertiaB, - prestepContact.Offset, contactOffsetB, prestepContact.Normal, prestepContact.Depth, + prestepContact.Offset, contactOffsetB, prestepContact.Normal, prestepContact.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestepMaterial.MaximumRecoveryVelocity, inverseDt, out projectionContact.Penetration); } @@ -400,20 +426,50 @@ public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityW } } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in TPrestep prestep, in TAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref TPrestep prestep, ref TAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - throw new System.NotImplementedException(); + ref var prestepMaterial = ref prestep.GetMaterialProperties(ref prestep); + ref var prestepOffsetB = ref prestep.GetOffsetB(ref prestep); + ref var prestepContactStart = ref prestep.GetContact(ref prestep, 0); + ref var accumulatedImpulsesStart = ref Unsafe.As(ref accumulatedImpulses); + for (int i = 0; i < prestep.ContactCount; ++i) + { + ref var prestepContact = ref Unsafe.Add(ref prestepContactStart, i); + Helpers.BuildOrthonormalBasis(prestepContact.Normal, out var x, out var z); + Vector3Wide.Subtract(prestepContact.Offset, prestepOffsetB, out var contactOffsetB); + ref var contactImpulse = ref Unsafe.Add(ref accumulatedImpulsesStart, i); + TangentFriction.WarmStart2(x, z, prestepContact.Offset, contactOffsetB, inertiaA, inertiaB, contactImpulse.Tangent, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestepContact.Normal, prestepContact.Offset, contactOffsetB, contactImpulse.Penetration, ref wsvA, ref wsvB); + } } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in TPrestep prestep, ref TAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref TPrestep prestep, ref TAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - throw new System.NotImplementedException(); + //Note that, unlike convex manifolds, we simply solve every contact in sequence rather than tangent->penetration. + //This is not for any principled reason- only simplicity. May want to reconsider later, but remember the significant change in access pattern. + ref var prestepOffsetB = ref prestep.GetOffsetB(ref prestep); + ref var prestepMaterial = ref prestep.GetMaterialProperties(ref prestep); + ref var accumulatedImpulsesStart = ref Unsafe.As(ref accumulatedImpulses); + ref var prestepContactStart = ref prestep.GetContact(ref prestep, 0); + SpringSettingsWide.ComputeSpringiness(prestepMaterial.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); + var inverseDtWide = new Vector(inverseDt); + for (int i = 0; i < prestep.ContactCount; ++i) + { + ref var contact = ref Unsafe.Add(ref prestepContactStart, i); + ref var contactImpulse = ref Unsafe.Add(ref accumulatedImpulsesStart, i); + Vector3Wide.Subtract(contact.Offset, prestepOffsetB, out var contactOffsetB); + Helpers.BuildOrthonormalBasis(contact.Normal, out var x, out var z); + var maximumTangentImpulse = prestepMaterial.FrictionCoefficient * contactImpulse.Penetration; + TangentFriction.Solve2(x, z, contact.Offset, contactOffsetB, inertiaA, inertiaB, maximumTangentImpulse, ref contactImpulse.Tangent, ref wsvA, ref wsvB); + PenetrationLimit.Solve2(inertiaA, inertiaB, contact.Normal, contact.Offset, contactOffsetB, contact.Depth, + positionErrorToVelocity, effectiveMassCFMScale, prestepMaterial.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref contactImpulse.Penetration, ref wsvA, ref wsvB); + } } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector dt, in TAccumulatedImpulses accumulatedImpulses, ref TPrestep prestep) { throw new System.NotImplementedException(); diff --git a/BepuPhysics/Constraints/DistanceLimit.cs b/BepuPhysics/Constraints/DistanceLimit.cs index 71f8af4bc..d76be3ed6 100644 --- a/BepuPhysics/Constraints/DistanceLimit.cs +++ b/BepuPhysics/Constraints/DistanceLimit.cs @@ -196,14 +196,14 @@ public void ComputeJacobians( } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in DistanceLimitPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref DistanceLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeJacobians(prestep.LocalOffsetA, positionA, orientationA, prestep.LocalOffsetB, positionB, orientationB, prestep.MinimumDistance, prestep.MaximumDistance, out _, out _, out var direction, out var angularJA, out var angularJB); ApplyImpulse(direction, angularJA, angularJB, inertiaA, inertiaB, accumulatedImpulses, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in DistanceLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref DistanceLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeJacobians(prestep.LocalOffsetA, positionA, orientationA, prestep.LocalOffsetB, positionB, orientationB, prestep.MinimumDistance, prestep.MaximumDistance, out var useMinimum, out var distance, out var direction, out var angularJA, out var angularJB); diff --git a/BepuPhysics/Constraints/DistanceServo.cs b/BepuPhysics/Constraints/DistanceServo.cs index d84d2cbb6..633110805 100644 --- a/BepuPhysics/Constraints/DistanceServo.cs +++ b/BepuPhysics/Constraints/DistanceServo.cs @@ -241,12 +241,12 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB projection.LinearImpulseToVelocityA, projection.AngularImpulseToVelocityA, projection.LinearImpulseToVelocityB, projection.AngularImpulseToVelocityB, ref csi); } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in DistanceServoPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref DistanceServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in DistanceServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref DistanceServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs index f34f0f92b..7155059d9 100644 --- a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs @@ -40,13 +40,13 @@ void WarmStart2( in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in Vector3Wide positionD, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, - in TPrestepData prestep, in TAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC, ref BodyVelocityWide wsvD); + ref TPrestepData prestep, ref TAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC, ref BodyVelocityWide wsvD); void Solve2( in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in Vector3Wide positionD, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, float dt, float inverseDt, - in TPrestepData prestep, ref TAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC, ref BodyVelocityWide wsvD); + ref TPrestepData prestep, ref TAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC, ref BodyVelocityWide wsvD); void UpdateForNewPose( in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, @@ -382,7 +382,7 @@ public unsafe override void WarmStart2(dt), accumulatedImpulses, ref prestep); - function.WarmStart2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, positionD, orientationD, inertiaD, prestep, accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC, ref wsvD); + function.WarmStart2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, positionD, orientationD, inertiaD, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC, ref wsvD); if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) { @@ -422,7 +422,7 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f bodies.GatherState(ref references.IndexC, count, true, out var positionC, out var orientationC, out var wsvC, out var inertiaC); bodies.GatherState(ref references.IndexD, count, true, out var positionD, out var orientationD, out var wsvD, out var inertiaD); - function.Solve2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, positionD, orientationD, inertiaD, dt, inverseDt, prestep, ref accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC, ref wsvD); + function.Solve2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, positionD, orientationD, inertiaD, dt, inverseDt, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC, ref wsvD); bodies.ScatterVelocities(ref wsvA, ref references.IndexA, count); bodies.ScatterVelocities(ref wsvB, ref references.IndexB, count); diff --git a/BepuPhysics/Constraints/Hinge.cs b/BepuPhysics/Constraints/Hinge.cs index 6e0c4fe8e..b7b6d5109 100644 --- a/BepuPhysics/Constraints/Hinge.cs +++ b/BepuPhysics/Constraints/Hinge.cs @@ -224,12 +224,12 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ApplyImpulse(ref velocityA, ref velocityB, ref projection, ref csi); } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in HingePrestepData prestep, in HingeAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref HingePrestepData prestep, ref HingeAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in HingePrestepData prestep, ref HingeAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref HingePrestepData prestep, ref HingeAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/LinearAxisLimit.cs b/BepuPhysics/Constraints/LinearAxisLimit.cs index 9afc7e360..989cfe889 100644 --- a/BepuPhysics/Constraints/LinearAxisLimit.cs +++ b/BepuPhysics/Constraints/LinearAxisLimit.cs @@ -149,12 +149,12 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ref csi); } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in LinearAxisLimitPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref LinearAxisLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in LinearAxisLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref LinearAxisLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/LinearAxisMotor.cs b/BepuPhysics/Constraints/LinearAxisMotor.cs index 68c95b97b..619ca4fbd 100644 --- a/BepuPhysics/Constraints/LinearAxisMotor.cs +++ b/BepuPhysics/Constraints/LinearAxisMotor.cs @@ -114,12 +114,12 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ref csi); } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in LinearAxisMotorPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref LinearAxisMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in LinearAxisMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref LinearAxisMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/LinearAxisServo.cs b/BepuPhysics/Constraints/LinearAxisServo.cs index 655a1bd5f..941843352 100644 --- a/BepuPhysics/Constraints/LinearAxisServo.cs +++ b/BepuPhysics/Constraints/LinearAxisServo.cs @@ -227,12 +227,12 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ref csi); } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in LinearAxisServoPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref LinearAxisServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in LinearAxisServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref LinearAxisServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/OneBodyAngularMotor.cs b/BepuPhysics/Constraints/OneBodyAngularMotor.cs index d21e72b63..d1e1f8f94 100644 --- a/BepuPhysics/Constraints/OneBodyAngularMotor.cs +++ b/BepuPhysics/Constraints/OneBodyAngularMotor.cs @@ -110,12 +110,12 @@ public void Solve(ref BodyVelocityWide velocityA, ref OneBodyAngularServoProject projection.MaximumImpulse, projection.ImpulseToVelocity, ref accumulatedImpulse); } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in OneBodyAngularMotorPrestepData prestep, in Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref OneBodyAngularMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) { throw new NotImplementedException(); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, in OneBodyAngularMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref OneBodyAngularMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/OneBodyAngularServo.cs b/BepuPhysics/Constraints/OneBodyAngularServo.cs index 729beb7f5..b8b52856f 100644 --- a/BepuPhysics/Constraints/OneBodyAngularServo.cs +++ b/BepuPhysics/Constraints/OneBodyAngularServo.cs @@ -142,13 +142,13 @@ public static void ApplyImpulse(in Symmetric3x3Wide inverseInertia, in Vector3Wi } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in OneBodyAngularServoPrestepData prestep, in Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref OneBodyAngularServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) { ApplyImpulse(inertiaA.InverseInertiaTensor, accumulatedImpulses, ref wsvA.Angular); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, in OneBodyAngularServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref OneBodyAngularServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) { //Jacobians are just the identity matrix. QuaternionWide.Conjugate(orientationA, out var inverseOrientation); diff --git a/BepuPhysics/Constraints/OneBodyLinearMotor.cs b/BepuPhysics/Constraints/OneBodyLinearMotor.cs index da925c0a9..fa1fb3293 100644 --- a/BepuPhysics/Constraints/OneBodyLinearMotor.cs +++ b/BepuPhysics/Constraints/OneBodyLinearMotor.cs @@ -92,12 +92,12 @@ public void Solve(ref BodyVelocityWide velocityA, ref OneBodyLinearServoProjecti OneBodyLinearServoFunctions.SharedSolve(ref velocityA, projection, ref accumulatedImpulse); } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in OneBodyLinearMotorPrestepData prestep, in Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref OneBodyLinearMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) { throw new NotImplementedException(); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, in OneBodyLinearMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref OneBodyLinearMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/OneBodyLinearServo.cs b/BepuPhysics/Constraints/OneBodyLinearServo.cs index 98aa67d1f..190d946f3 100644 --- a/BepuPhysics/Constraints/OneBodyLinearServo.cs +++ b/BepuPhysics/Constraints/OneBodyLinearServo.cs @@ -180,14 +180,14 @@ public static void ApplyImpulse(in Vector3Wide offset, in BodyInertiaWide inerti } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in OneBodyLinearServoPrestepData prestep, in Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref OneBodyLinearServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) { QuaternionWide.TransformWithoutOverlap(prestep.LocalOffset, orientationA, out var offset); ApplyImpulse(offset, inertiaA, ref wsvA, accumulatedImpulses); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, in OneBodyLinearServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref OneBodyLinearServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) { QuaternionWide.TransformWithoutOverlap(prestep.LocalOffset, orientationA, out var offset); SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); diff --git a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs index cce25949d..8fcd684b2 100644 --- a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs @@ -21,9 +21,9 @@ public interface IOneBodyConstraintFunctions(dt), accumulatedImpulses, ref prestep); - function.WarmStart2(positionA, orientationA, inertiaA, prestep, accumulatedImpulses, ref wsvA); + function.WarmStart2(positionA, orientationA, inertiaA, ref prestep, ref accumulatedImpulses, ref wsvA); if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) { @@ -311,7 +311,7 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f var count = GetCountInBundle(ref typeBatch, i); bodies.GatherState(ref references, count, true, out var positionA, out var orientationA, out var wsvA, out var inertiaA); - function.Solve2(positionA, orientationA, inertiaA, dt, inverseDt, prestep, ref accumulatedImpulses, ref wsvA); + function.Solve2(positionA, orientationA, inertiaA, dt, inverseDt, ref prestep, ref accumulatedImpulses, ref wsvA); bodies.ScatterVelocities(ref wsvA, ref references, count); } diff --git a/BepuPhysics/Constraints/PointOnLineServo.cs b/BepuPhysics/Constraints/PointOnLineServo.cs index b17ff5832..46a2056c9 100644 --- a/BepuPhysics/Constraints/PointOnLineServo.cs +++ b/BepuPhysics/Constraints/PointOnLineServo.cs @@ -221,12 +221,12 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ApplyImpulse(ref velocityA, ref velocityB, projection.LinearJacobian, angularA, angularB, projection.InertiaA, projection.InertiaB, ref csi); } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in PointOnLineServoPrestepData prestep, in Vector2Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref PointOnLineServoPrestepData prestep, ref Vector2Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in PointOnLineServoPrestepData prestep, ref Vector2Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref PointOnLineServoPrestepData prestep, ref Vector2Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/SwingLimit.cs b/BepuPhysics/Constraints/SwingLimit.cs index c759c080a..42bd4c786 100644 --- a/BepuPhysics/Constraints/SwingLimit.cs +++ b/BepuPhysics/Constraints/SwingLimit.cs @@ -179,12 +179,12 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, ref projection, ref csi); } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in SwingLimitPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref SwingLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in SwingLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref SwingLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/SwivelHinge.cs b/BepuPhysics/Constraints/SwivelHinge.cs index 1925702d7..6d7645753 100644 --- a/BepuPhysics/Constraints/SwivelHinge.cs +++ b/BepuPhysics/Constraints/SwivelHinge.cs @@ -211,12 +211,12 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ApplyImpulse(ref velocityA, ref velocityB, ref projection, ref csi); } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in SwivelHingePrestepData prestep, in Vector4Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref SwivelHingePrestepData prestep, ref Vector4Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in SwivelHingePrestepData prestep, ref Vector4Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref SwivelHingePrestepData prestep, ref Vector4Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs index 1b7f9c341..037525f7d 100644 --- a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs @@ -35,12 +35,12 @@ void WarmStart2( in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, - in TPrestepData prestep, in TAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC); + ref TPrestepData prestep, ref TAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC); void Solve2( in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, float dt, float inverseDt, - in TPrestepData prestep, ref TAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC); + ref TPrestepData prestep, ref TAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC); void UpdateForNewPose( in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, @@ -350,7 +350,7 @@ public unsafe override void WarmStart2(dt), accumulatedImpulses, ref prestep); - function.WarmStart2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, prestep, accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC); + function.WarmStart2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC); if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) { @@ -387,7 +387,7 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f bodies.GatherState(ref references.IndexB, count, true, out var positionB, out var orientationB, out var wsvB, out var inertiaB); bodies.GatherState(ref references.IndexC, count, true, out var positionC, out var orientationC, out var wsvC, out var inertiaC); - function.Solve2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, dt, inverseDt, prestep, ref accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC); + function.Solve2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, dt, inverseDt, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC); bodies.ScatterVelocities(ref wsvA, ref references.IndexA, count); bodies.ScatterVelocities(ref wsvB, ref references.IndexB, count); diff --git a/BepuPhysics/Constraints/TwistLimit.cs b/BepuPhysics/Constraints/TwistLimit.cs index 8aa64b2f5..9040ba730 100644 --- a/BepuPhysics/Constraints/TwistLimit.cs +++ b/BepuPhysics/Constraints/TwistLimit.cs @@ -145,12 +145,12 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB TwistServoFunctions.ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, projection.ImpulseToVelocityA, projection.NegatedImpulseToVelocityB, csi); } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in TwistLimitPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref TwistLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in TwistLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref TwistLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/TwistMotor.cs b/BepuPhysics/Constraints/TwistMotor.cs index 16ae337df..5ca358f01 100644 --- a/BepuPhysics/Constraints/TwistMotor.cs +++ b/BepuPhysics/Constraints/TwistMotor.cs @@ -130,12 +130,12 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB TwistServoFunctions.ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, projection.ImpulseToVelocityA, projection.NegatedImpulseToVelocityB, csi); } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in TwistMotorPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref TwistMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in TwistMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref TwistMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/TwistServo.cs b/BepuPhysics/Constraints/TwistServo.cs index 40a8f4f42..788d9f414 100644 --- a/BepuPhysics/Constraints/TwistServo.cs +++ b/BepuPhysics/Constraints/TwistServo.cs @@ -215,12 +215,12 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, projection.ImpulseToVelocityA, projection.NegatedImpulseToVelocityB, csi); } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in TwistServoPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref TwistServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in TwistServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref TwistServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 167e73706..fc4bc6057 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -34,9 +34,9 @@ public interface ITwoBodyConstraintFunctions(dt), accumulatedImpulses, ref prestep); - function.WarmStart2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, prestep, accumulatedImpulses, ref wsvA, ref wsvB); + function.WarmStart2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB); if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) { @@ -429,7 +429,7 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f bodies.GatherState(ref references.IndexA, count, true, out var positionA, out var orientationA, out var wsvA, out var inertiaA); bodies.GatherState(ref references.IndexB, count, true, out var positionB, out var orientationB, out var wsvB, out var inertiaB); - function.Solve2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, dt, inverseDt, prestep, ref accumulatedImpulses, ref wsvA, ref wsvB); + function.Solve2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, dt, inverseDt, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB); bodies.ScatterVelocities(ref wsvA, ref references.IndexA, count); bodies.ScatterVelocities(ref wsvB, ref references.IndexB, count); diff --git a/BepuPhysics/Constraints/VolumeConstraint.cs b/BepuPhysics/Constraints/VolumeConstraint.cs index 617f798f3..e97e34e00 100644 --- a/BepuPhysics/Constraints/VolumeConstraint.cs +++ b/BepuPhysics/Constraints/VolumeConstraint.cs @@ -206,12 +206,12 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ApplyImpulse(ref velocityA, ref velocityB, ref velocityC, ref velocityD, ref projection, ref negatedJacobianA, ref csi); } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in Vector3Wide positionD, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, in VolumeConstraintPrestepData prestep, in Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC, ref BodyVelocityWide wsvD) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in Vector3Wide positionD, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, ref VolumeConstraintPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC, ref BodyVelocityWide wsvD) { throw new NotImplementedException(); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in Vector3Wide positionD, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, float dt, float inverseDt, in VolumeConstraintPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC, ref BodyVelocityWide wsvD) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in Vector3Wide positionD, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, float dt, float inverseDt, ref VolumeConstraintPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC, ref BodyVelocityWide wsvD) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/Weld.cs b/BepuPhysics/Constraints/Weld.cs index e918d148a..7aa741ef2 100644 --- a/BepuPhysics/Constraints/Weld.cs +++ b/BepuPhysics/Constraints/Weld.cs @@ -261,14 +261,14 @@ private static void ApplyImpulse(in BodyInertiaWide inertiaA, in BodyInertiaWide //[MethodImpl(MethodImplOptions.NoInlining)] public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, - in WeldPrestepData prestep, in WeldAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + ref WeldPrestepData prestep, ref WeldAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { Transform(prestep.LocalOffset, orientationA, out var offset); ApplyImpulse(inertiaA, inertiaB, offset, accumulatedImpulses.Orientation, accumulatedImpulses.Offset, ref wsvA, ref wsvB); } //[MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, - in WeldPrestepData prestep, ref WeldAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + ref WeldPrestepData prestep, ref WeldAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { //The weld constraint handles 6 degrees of freedom simultaneously. The constraints are: //localOrientation * orientationA = orientationB diff --git a/DemoTests/DemoTests.csproj b/DemoTests/DemoTests.csproj index 72cf83db0..cc62ea6e8 100644 --- a/DemoTests/DemoTests.csproj +++ b/DemoTests/DemoTests.csproj @@ -1,7 +1,7 @@ - net5.0 + net6.0 true false latest diff --git a/Demos/Demos.csproj b/Demos/Demos.csproj index 229b40de0..6752aa2a3 100644 --- a/Demos/Demos.csproj +++ b/Demos/Demos.csproj @@ -1,7 +1,7 @@  Exe - net5.0 + net6.0 True Debug;Release latest diff --git a/Demos/Demos/Characters/CharacterMotionConstraint.cs b/Demos/Demos/Characters/CharacterMotionConstraint.cs index 811b51ea8..b1d272891 100644 --- a/Demos/Demos/Characters/CharacterMotionConstraint.cs +++ b/Demos/Demos/Characters/CharacterMotionConstraint.cs @@ -306,12 +306,12 @@ public void Solve(ref BodyVelocityWide velocityA, ref StaticCharacterMotionProje ApplyVerticalImpulse(basis, verticalAngularJacobianA, verticalCorrectiveImpulse, projection.InertiaA, ref velocityA); } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in StaticCharacterMotionPrestep prestep, in CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref StaticCharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA) { throw new NotImplementedException(); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, in StaticCharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref StaticCharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA) { throw new NotImplementedException(); } @@ -654,12 +654,12 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ApplyVerticalImpulse(basis, verticalAngularJacobianA, verticalAngularJacobianB, verticalCorrectiveImpulse, projection.InertiaA, projection.InertiaB, ref velocityA, ref velocityB); } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in DynamicCharacterMotionPrestep prestep, in CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref DynamicCharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, in DynamicCharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref DynamicCharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { throw new NotImplementedException(); } diff --git a/Demos/Demos/Characters/CharacterMotionConstraint.tt b/Demos/Demos/Characters/CharacterMotionConstraint.tt index 640fb8a4d..a4e79219d 100644 --- a/Demos/Demos/Characters/CharacterMotionConstraint.tt +++ b/Demos/Demos/Characters/CharacterMotionConstraint.tt @@ -393,12 +393,12 @@ namespace Demos.Demos.Characters ApplyVerticalImpulse(basis, verticalAngularJacobianA, <#if (dynamic) {#>verticalAngularJacobianB, <#}#>verticalCorrectiveImpulse, projection.InertiaA, <#if (dynamic) {#>projection.InertiaB, <#}#>ref velocityA<#if (dynamic) {#>, ref velocityB<#}#>); } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(dynamic) {#>in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>in <#=prefix#>CharacterMotionPrestep prestep, in CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA<#if(dynamic) {#>, ref BodyVelocityWide wsvB<#}#>) + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(dynamic) {#>in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>ref <#=prefix#>CharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA<#if(dynamic) {#>, ref BodyVelocityWide wsvB<#}#>) { throw new NotImplementedException(); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(dynamic) {#>in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>float dt, float inverseDt, in <#=prefix#>CharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA<#if(dynamic) {#>, ref BodyVelocityWide wsvB<#}#>) + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(dynamic) {#>in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>float dt, float inverseDt, ref <#=prefix#>CharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA<#if(dynamic) {#>, ref BodyVelocityWide wsvB<#}#>) { throw new NotImplementedException(); } diff --git a/Demos/Program.cs b/Demos/Program.cs index de93fb6fd..668a4ab43 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -19,7 +19,7 @@ static void Main(string[] args) { content = ContentArchive.Load(stream); } - HeadlessTest.Test(content, 4, 32, 512); + //HeadlessTest.Test(content, 4, 32, 512); //HeadlessTest.Test(content, 4, 32, 512); //HeadlessTest.Test(content, 4, 32, 512); var demo = new DemoHarness(loop, content); From f8e4b6f323d945b7bee148d9d6d8384111ecff91 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 27 Sep 2021 14:33:39 -0500 Subject: [PATCH 162/947] LinearAxisServo 2ified. --- BepuPhysics/Constraints/LinearAxisServo.cs | 65 ++++++++++++++++++++-- 1 file changed, 61 insertions(+), 4 deletions(-) diff --git a/BepuPhysics/Constraints/LinearAxisServo.cs b/BepuPhysics/Constraints/LinearAxisServo.cs index 941843352..d89fb8763 100644 --- a/BepuPhysics/Constraints/LinearAxisServo.cs +++ b/BepuPhysics/Constraints/LinearAxisServo.cs @@ -116,7 +116,7 @@ public void Modify(in Vector3Wide anchorA, in Vector3Wide anchorB, ref Vector3Wi } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ComputeTransforms(ref TJacobianModifier jacobianModifier, + public static void ComputeTransforms(ref TJacobianModifier jacobianModifier, in Vector3Wide localOffsetA, in Vector3Wide localOffsetB, in Vector3Wide localPlaneNormal, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector effectiveMassCFMScale, @@ -227,19 +227,76 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ref csi); } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void ApplyImpulse(in Vector3Wide linearJA, in Vector3Wide angularImpulseToVelocityA, in Vector3Wide angularImpulseToVelocityB, in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, + in Vector csi, ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB) + { + velocityA.Linear += linearJA * (csi * inertiaA.InverseMass); + velocityB.Linear -= linearJA * (csi * inertiaB.InverseMass); + velocityA.Angular += angularImpulseToVelocityA * csi; + velocityB.Angular += angularImpulseToVelocityB * csi; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void ComputeJacobians(in Vector3Wide ab, in QuaternionWide orientationA, in QuaternionWide orientationB, in Vector3Wide localPlaneNormalA, in Vector3Wide localOffsetB, + out Vector3Wide anchorB, out Vector3Wide normal, out Vector3Wide angularJA, out Vector3Wide angularJB) + { + //Linear jacobians are just normal and -normal. Angular jacobians are offsetA x normal and offsetB x normal. + QuaternionWide.TransformWithoutOverlap(localPlaneNormalA, orientationA, out normal); + QuaternionWide.TransformWithoutOverlap(localOffsetB, orientationB, out var offsetB); + //Note that the angular jacobian for A uses the offset from A to the attachment point on B. + anchorB = ab + offsetB; + Vector3Wide.CrossWithoutOverlap(anchorB, normal, out angularJA); + Vector3Wide.CrossWithoutOverlap(normal, offsetB, out angularJB); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void ComputeEffectiveMass(in Vector3Wide angularJA, in Vector3Wide angularJB, + in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, float dt, + in SpringSettingsWide springSettings, + out Vector positionErrorToVelocity, out Vector softnessImpulseScale, out Vector3Wide angularImpulseToVelocityA, out Vector3Wide angularImpulseToVelocityB, out Vector effectiveMass) + { + Symmetric3x3Wide.TransformWithoutOverlap(angularJA, inertiaA.InverseInertiaTensor, out angularImpulseToVelocityA); + Symmetric3x3Wide.TransformWithoutOverlap(angularJB, inertiaB.InverseInertiaTensor, out angularImpulseToVelocityB); + Vector3Wide.Dot(angularJA, angularImpulseToVelocityA, out var angularContributionA); + Vector3Wide.Dot(angularJB, angularImpulseToVelocityB, out var angularContributionB); + SpringSettingsWide.ComputeSpringiness(springSettings, dt, out positionErrorToVelocity, out var effectiveMassCFMScale, out softnessImpulseScale); + effectiveMass = effectiveMassCFMScale / (inertiaA.InverseMass + inertiaB.InverseMass + angularContributionA + angularContributionB); + } + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref LinearAxisServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - throw new NotImplementedException(); + ComputeJacobians(positionB - positionA, orientationA, orientationB, prestep.LocalPlaneNormal, prestep.LocalOffsetB, out _, out var normal, out var angularJA, out var angularJB); + Symmetric3x3Wide.TransformWithoutOverlap(angularJA, inertiaA.InverseInertiaTensor, out var angularImpulseToVelocityA); + Symmetric3x3Wide.TransformWithoutOverlap(angularJB, inertiaB.InverseInertiaTensor, out var angularImpulseToVelocityB); + ApplyImpulse(normal, angularImpulseToVelocityA, angularImpulseToVelocityB, inertiaA, inertiaB, accumulatedImpulses, ref wsvA, ref wsvB); } public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref LinearAxisServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - throw new NotImplementedException(); + ComputeJacobians(positionB - positionA, orientationA, orientationB, prestep.LocalPlaneNormal, prestep.LocalOffsetB, out var anchorB, out var normal, out var angularJA, out var angularJB); + ComputeEffectiveMass(angularJA, angularJB, inertiaA, inertiaB, dt, prestep.SpringSettings, + out var positionErrorToVelocity, out var softnessImpulseScale, + out var angularImpulseToVelocityA, out var angularImpulseToVelocityB, out var effectiveMass); + + QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetA, orientationA, out var anchorA); + Vector3Wide.Dot(anchorB - anchorA, normal, out var planeNormalDot); + + //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. + ServoSettingsWide.ComputeClampedBiasVelocity(planeNormalDot - prestep.TargetOffset, positionErrorToVelocity, prestep.ServoSettings, dt, inverseDt, out var biasVelocity, out var maximumImpulse); + + //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); + var csv = Vector3Wide.Dot(wsvA.Linear - wsvB.Linear, normal) + Vector3Wide.Dot(wsvA.Angular, angularJA) + Vector3Wide.Dot(wsvB.Angular, angularJB); + + var csi = effectiveMass * (biasVelocity - csv) - accumulatedImpulses * softnessImpulseScale; + + ServoSettingsWide.ClampImpulse(maximumImpulse, ref accumulatedImpulses, ref csi); + ApplyImpulse(normal, angularImpulseToVelocityA, angularImpulseToVelocityB, inertiaA, inertiaB, csi, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector dt, in Vector accumulatedImpulses, ref LinearAxisServoPrestepData prestep) { From 2cc0d97ecc073824e60fd28f3890fe61bfb2c381 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 27 Sep 2021 15:51:28 -0500 Subject: [PATCH 163/947] LinearAxisLimit and PointOnLineServo twoified. --- BepuPhysics/Constraints/LinearAxisLimit.cs | 52 ++++++++++++- BepuPhysics/Constraints/PointOnLineServo.cs | 83 ++++++++++++++++++++- 2 files changed, 129 insertions(+), 6 deletions(-) diff --git a/BepuPhysics/Constraints/LinearAxisLimit.cs b/BepuPhysics/Constraints/LinearAxisLimit.cs index 989cfe889..119356214 100644 --- a/BepuPhysics/Constraints/LinearAxisLimit.cs +++ b/BepuPhysics/Constraints/LinearAxisLimit.cs @@ -149,19 +149,65 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ref csi); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + static void ComputeJacobians( + in Vector3Wide ab, in QuaternionWide orientationA, in QuaternionWide orientationB, in Vector3Wide localPlaneNormal, in Vector3Wide localOffsetA, in Vector3Wide localOffsetB, in Vector minimumOffset, in Vector maximumOffset, + out Vector error, out Vector3Wide normal, out Vector3Wide angularJA, out Vector3Wide angularJB) + { + //Linear jacobians are just normal and -normal. Angular jacobians are offsetA x normal and offsetB x normal. + Matrix3x3Wide.CreateFromQuaternion(orientationA, out var orientationMatrixA); + Matrix3x3Wide.TransformWithoutOverlap(localPlaneNormal, orientationMatrixA, out normal); + QuaternionWide.TransformWithoutOverlap(localOffsetB, orientationB, out var offsetB); + + //The limit chooses the normal's sign depending on which limit is closer. + Matrix3x3Wide.TransformWithoutOverlap(localOffsetA, orientationMatrixA, out var anchorA); + var anchorB = ab + offsetB; + Vector3Wide.Subtract(anchorB, anchorA, out var anchorOffset); + Vector3Wide.Dot(anchorOffset, normal, out var planeNormalDot); + var minimumError = minimumOffset - planeNormalDot; + var maximumError = planeNormalDot - maximumOffset; + var useMin = Vector.LessThan(Vector.Abs(minimumError), Vector.Abs(maximumError)); + error = Vector.ConditionalSelect(useMin, minimumError, maximumError); + normal.X = Vector.ConditionalSelect(useMin, -normal.X, normal.X); + normal.Y = Vector.ConditionalSelect(useMin, -normal.Y, normal.Y); + normal.Z = Vector.ConditionalSelect(useMin, -normal.Z, normal.Z); + + //Note that the angular jacobian for A uses the offset from A to the attachment point on B. + Vector3Wide.CrossWithoutOverlap(anchorB, normal, out angularJA); + Vector3Wide.CrossWithoutOverlap(normal, offsetB, out angularJB); + } + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref LinearAxisLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - throw new NotImplementedException(); + ComputeJacobians(positionB - positionA, orientationA, orientationB, prestep.LocalPlaneNormal, prestep.LocalOffsetA, prestep.LocalOffsetB, prestep.MinimumOffset, prestep.MaximumOffset, out _, out var normal, out var angularJA, out var angularJB); + Symmetric3x3Wide.TransformWithoutOverlap(angularJA, inertiaA.InverseInertiaTensor, out var angularImpulseToVelocityA); + Symmetric3x3Wide.TransformWithoutOverlap(angularJB, inertiaB.InverseInertiaTensor, out var angularImpulseToVelocityB); + LinearAxisServoFunctions.ApplyImpulse(normal, angularImpulseToVelocityA, angularImpulseToVelocityB, inertiaA, inertiaB, accumulatedImpulses, ref wsvA, ref wsvB); } public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref LinearAxisLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - throw new NotImplementedException(); + ComputeJacobians(positionB - positionA, orientationA, orientationB, prestep.LocalPlaneNormal, prestep.LocalOffsetA, prestep.LocalOffsetB, prestep.MinimumOffset, prestep.MaximumOffset, out var error, out var normal, out var angularJA, out var angularJB); + + LinearAxisServoFunctions.ComputeEffectiveMass(angularJA, angularJB, inertiaA, inertiaB, dt, prestep.SpringSettings, + out var positionErrorToVelocity, out var softnessImpulseScale, + out var angularImpulseToVelocityA, out var angularImpulseToVelocityB, out var effectiveMass); + + InequalityHelpers.ComputeBiasVelocity(error, positionErrorToVelocity, inverseDt, out var biasVelocity); + + //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); + var csv = Vector3Wide.Dot(wsvA.Linear - wsvB.Linear, normal) + Vector3Wide.Dot(wsvA.Angular, angularJA) + Vector3Wide.Dot(wsvB.Angular, angularJB); + + var csi = effectiveMass * (biasVelocity - csv) - accumulatedImpulses * softnessImpulseScale; + + InequalityHelpers.ClampPositive(ref accumulatedImpulses, ref csi); + LinearAxisServoFunctions.ApplyImpulse(normal, angularImpulseToVelocityA, angularImpulseToVelocityB, inertiaA, inertiaB, csi, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector dt, in Vector accumulatedImpulses, ref LinearAxisLimitPrestepData prestep) { diff --git a/BepuPhysics/Constraints/PointOnLineServo.cs b/BepuPhysics/Constraints/PointOnLineServo.cs index 46a2056c9..02af61e62 100644 --- a/BepuPhysics/Constraints/PointOnLineServo.cs +++ b/BepuPhysics/Constraints/PointOnLineServo.cs @@ -221,19 +221,96 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ApplyImpulse(ref velocityA, ref velocityB, projection.LinearJacobian, angularA, angularB, projection.InertiaA, projection.InertiaB, ref csi); } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void ComputeJacobians(in Vector3Wide ab, in QuaternionWide orientationA, in QuaternionWide orientationB, in Vector3Wide localDirection, in Vector3Wide localOffsetA, in Vector3Wide localOffsetB, + out Vector3Wide anchorOffset, out Matrix2x3Wide linearJacobian, out Matrix2x3Wide angularJA, out Matrix2x3Wide angularJB) + { + Helpers.BuildOrthonormalBasis(localDirection, out var localTangentX, out var localTangentY); + Matrix3x3Wide.CreateFromQuaternion(orientationA, out var orientationMatrixA); + Matrix3x3Wide.TransformWithoutOverlap(localOffsetA, orientationMatrixA, out var anchorA); + QuaternionWide.TransformWithoutOverlap(localOffsetB, orientationB, out var offsetB); + + //Find offsetA by computing the closest point on the line to anchorB. + Matrix3x3Wide.TransformWithoutOverlap(localDirection, orientationMatrixA, out var direction); + Vector3Wide.Add(offsetB, ab, out var anchorB); + Vector3Wide.Subtract(anchorB, anchorA, out anchorOffset); + Vector3Wide.Dot(anchorOffset, direction, out var d); + Vector3Wide.Scale(direction, d, out var lineStartToClosestPointOnLine); + Vector3Wide.Add(lineStartToClosestPointOnLine, anchorA, out var offsetA); + + Matrix3x3Wide.TransformWithoutOverlap(localTangentX, orientationMatrixA, out linearJacobian.X); + Matrix3x3Wide.TransformWithoutOverlap(localTangentY, orientationMatrixA, out linearJacobian.Y); + + Vector3Wide.CrossWithoutOverlap(offsetA, linearJacobian.X, out angularJA.X); + Vector3Wide.CrossWithoutOverlap(offsetA, linearJacobian.Y, out angularJA.Y); + Vector3Wide.CrossWithoutOverlap(linearJacobian.X, offsetB, out angularJB.X); + Vector3Wide.CrossWithoutOverlap(linearJacobian.Y, offsetB, out angularJB.Y); + } public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref PointOnLineServoPrestepData prestep, ref Vector2Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - throw new NotImplementedException(); + ComputeJacobians(positionB - positionA, orientationA, orientationB, prestep.LocalDirection, prestep.LocalOffsetA, prestep.LocalOffsetB, out _, out var linearJacobian, out var angularJA, out var angularJB); + ApplyImpulse(ref wsvA, ref wsvB, linearJacobian, angularJA, angularJB, inertiaA, inertiaB, ref accumulatedImpulses); } public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref PointOnLineServoPrestepData prestep, ref Vector2Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - throw new NotImplementedException(); + //This constrains a point on B to a line attached to A. It works on two degrees of freedom at the same time; those are the tangent axes to the line direction. + //The error is measured as closest offset from the line. In other words: + //dot(closestPointOnLineToAnchorB - anchorB, t1) = 0 + //dot(closestPointOnLineToAnchorB - anchorB, t2) = 0 + //where closestPointOnLineToAnchorB = dot(anchorB - anchorA, lineDirection) * lineDirection + anchorA + //For the purposes of this derivation, we'll treat t1, t2, and lineDirection as constant with respect to time. + //In the following, offsetA from the center of A to the closestPointOnLineToAnchorB, and offsetB refers to the LocalOffsetB * orientationB. + //dot(positionA + offsetA - (positionB + offsetB), t1) = 0 + //dot(positionA + offsetA - (positionB + offsetB), t2) = 0 + //Velocity constraint for t1: + //dot(d/dt(positionA + offsetA - (positionB + offsetB), t1) + dot(positionA + offsetA - (positionB + offsetB), d/dt(t1)) = 0 + //Treat d/dt(t1) as constant: + //dot(d/dt(positionA + offsetA - (positionB + offsetB), t1) = 0 + //dot(linearA + angularA x offsetA - linearB - angularB x offsetB, t1) = 0 + //dot(linearA, t1) + dot(angularA x offsetA, t1) + dot(linearB, -t1) + dot(offsetB x angularB, t1) = 0 + //dot(linearA, t1) + dot(offsetA x t1, angularA) + dot(linearB, -t1) + dot(t1 x offsetB, angularB) = 0 + //Following the same pattern for the second degree of freedom, the jacobians are: + //linearA: t1, t2 + //angularA: offsetA x t1, offsetA x t2 + //linearB: -t1, -t2 + //angularB: t1 x offsetB, t2 x offsetB + ComputeJacobians(positionB - positionA, orientationA, orientationB, prestep.LocalDirection, prestep.LocalOffsetA, prestep.LocalOffsetB, out var anchorOffset, out var linearJacobian, out var angularJA, out var angularJB); + Symmetric2x2Wide.SandwichScale(linearJacobian, inertiaA.InverseMass + inertiaB.InverseMass, out var linearContribution); + Symmetric3x3Wide.MatrixSandwich(angularJA, inertiaA.InverseInertiaTensor, out var angularContributionA); + Symmetric3x3Wide.MatrixSandwich(angularJB, inertiaB.InverseInertiaTensor, out var angularContributionB); + Symmetric2x2Wide.Add(angularContributionA, angularContributionB, out var inverseEffectiveMass); + Symmetric2x2Wide.Add(inverseEffectiveMass, linearContribution, out inverseEffectiveMass); + + Symmetric2x2Wide.InvertWithoutOverlap(inverseEffectiveMass, out var effectiveMass); + + SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); + Symmetric2x2Wide.Scale(effectiveMass, effectiveMassCFMScale, out effectiveMass); + + //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); + Matrix2x3Wide.TransformByTransposeWithoutOverlap(wsvA.Linear, linearJacobian, out var linearCSVA); + Matrix2x3Wide.TransformByTransposeWithoutOverlap(wsvB.Linear, linearJacobian, out var negatedLinearCSVB); + Matrix2x3Wide.TransformByTransposeWithoutOverlap(wsvA.Angular, angularJA, out var angularCSVA); + Matrix2x3Wide.TransformByTransposeWithoutOverlap(wsvB.Angular, angularJB, out var angularCSVB); + Vector2Wide.Subtract(linearCSVA, negatedLinearCSVB, out var linearCSV); + Vector2Wide.Add(angularCSVA, angularCSVB, out var angularCSV); + Vector2Wide.Add(linearCSV, angularCSV, out var csv); + //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. + Vector2Wide error; + Vector3Wide.Dot(anchorOffset, linearJacobian.X, out error.X); + Vector3Wide.Dot(anchorOffset, linearJacobian.Y, out error.Y); + ServoSettingsWide.ComputeClampedBiasVelocity(error, positionErrorToVelocity, prestep.ServoSettings, dt, inverseDt, out var biasVelocity, out var maximumImpulse); + Vector2Wide.Subtract(biasVelocity, csv, out csv); + Symmetric2x2Wide.TransformWithoutOverlap(csv, effectiveMass, out var csi); + Vector2Wide.Scale(accumulatedImpulses, softnessImpulseScale, out var softnessContribution); + Vector2Wide.Subtract(csi, softnessContribution, out csi); + ServoSettingsWide.ClampImpulse(maximumImpulse, ref accumulatedImpulses, ref csi); + ApplyImpulse(ref wsvA, ref wsvB, linearJacobian, angularJA, angularJB, inertiaA, inertiaB, ref csi); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector dt, in Vector2Wide accumulatedImpulses, ref PointOnLineServoPrestepData prestep) { From 14032d838dfb5bd7e5f1916a007f41eb6932be92 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 27 Sep 2021 16:05:40 -0500 Subject: [PATCH 164/947] LinearAxisMotor twoified. --- BepuPhysics/Constraints/LinearAxisLimit.cs | 4 ++-- BepuPhysics/Constraints/LinearAxisMotor.cs | 27 ++++++++++++++++------ BepuPhysics/Constraints/LinearAxisServo.cs | 11 ++++----- 3 files changed, 26 insertions(+), 16 deletions(-) diff --git a/BepuPhysics/Constraints/LinearAxisLimit.cs b/BepuPhysics/Constraints/LinearAxisLimit.cs index 119356214..5265ddd64 100644 --- a/BepuPhysics/Constraints/LinearAxisLimit.cs +++ b/BepuPhysics/Constraints/LinearAxisLimit.cs @@ -190,8 +190,8 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in { ComputeJacobians(positionB - positionA, orientationA, orientationB, prestep.LocalPlaneNormal, prestep.LocalOffsetA, prestep.LocalOffsetB, prestep.MinimumOffset, prestep.MaximumOffset, out var error, out var normal, out var angularJA, out var angularJB); - LinearAxisServoFunctions.ComputeEffectiveMass(angularJA, angularJB, inertiaA, inertiaB, dt, prestep.SpringSettings, - out var positionErrorToVelocity, out var softnessImpulseScale, + SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); + LinearAxisServoFunctions.ComputeEffectiveMass(angularJA, angularJB, inertiaA, inertiaB, effectiveMassCFMScale, out var angularImpulseToVelocityA, out var angularImpulseToVelocityB, out var effectiveMass); InequalityHelpers.ComputeBiasVelocity(error, positionErrorToVelocity, inverseDt, out var biasVelocity); diff --git a/BepuPhysics/Constraints/LinearAxisMotor.cs b/BepuPhysics/Constraints/LinearAxisMotor.cs index 619ca4fbd..87e0167c8 100644 --- a/BepuPhysics/Constraints/LinearAxisMotor.cs +++ b/BepuPhysics/Constraints/LinearAxisMotor.cs @@ -78,7 +78,7 @@ public struct LinearAxisMotorPrestepData public Vector TargetVelocity; public MotorSettingsWide Settings; } - + public struct LinearAxisMotorFunctions : ITwoBodyConstraintFunctions> { [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -91,10 +91,10 @@ public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, out _, out _, out _, out var effectiveMass, out projection.LinearVelocityToImpulseA, out projection.AngularVelocityToImpulseA, out projection.AngularVelocityToImpulseB, out projection.LinearImpulseToVelocityA, out projection.AngularImpulseToVelocityA, out projection.NegatedLinearImpulseToVelocityB, out projection.AngularImpulseToVelocityB); - + projection.BiasImpulse = -prestep.TargetVelocity * effectiveMass; } - + [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref LinearAxisServoProjection projection, ref Vector accumulatedImpulse) { @@ -102,7 +102,7 @@ public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide veloc projection.LinearImpulseToVelocityA, projection.AngularImpulseToVelocityA, projection.NegatedLinearImpulseToVelocityB, projection.AngularImpulseToVelocityB, ref accumulatedImpulse); } - + [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref LinearAxisServoProjection projection, ref Vector accumulatedImpulse) { @@ -116,17 +116,30 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref LinearAxisMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - throw new NotImplementedException(); + LinearAxisServoFunctions.ComputeJacobians(positionB - positionA, orientationA, orientationB, prestep.LocalPlaneNormal, prestep.LocalOffsetB, out _, out var normal, out var angularJA, out var angularJB); + Symmetric3x3Wide.TransformWithoutOverlap(angularJA, inertiaA.InverseInertiaTensor, out var angularImpulseToVelocityA); + Symmetric3x3Wide.TransformWithoutOverlap(angularJB, inertiaB.InverseInertiaTensor, out var angularImpulseToVelocityB); + LinearAxisServoFunctions.ApplyImpulse(normal, angularImpulseToVelocityA, angularImpulseToVelocityB, inertiaA, inertiaB, accumulatedImpulses, ref wsvA, ref wsvB); } public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref LinearAxisMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - throw new NotImplementedException(); + LinearAxisServoFunctions.ComputeJacobians(positionB - positionA, orientationA, orientationB, prestep.LocalPlaneNormal, prestep.LocalOffsetB, out _, out var normal, out var angularJA, out var angularJB); + MotorSettingsWide.ComputeSoftness(prestep.Settings, dt, out var effectiveMassCFMScale, out var softnessImpulseScale, out var maximumImpulse); + LinearAxisServoFunctions.ComputeEffectiveMass(angularJA, angularJB, inertiaA, inertiaB, effectiveMassCFMScale, out var angularImpulseToVelocityA, out var angularImpulseToVelocityB, out var effectiveMass); + + //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); + var csv = Vector3Wide.Dot(wsvA.Linear - wsvB.Linear, normal) + Vector3Wide.Dot(wsvA.Angular, angularJA) + Vector3Wide.Dot(wsvB.Angular, angularJB); + + var csi = effectiveMass * (-prestep.TargetVelocity - csv) - accumulatedImpulses * softnessImpulseScale; + + ServoSettingsWide.ClampImpulse(maximumImpulse, ref accumulatedImpulses, ref csi); + LinearAxisServoFunctions.ApplyImpulse(normal, angularImpulseToVelocityA, angularImpulseToVelocityB, inertiaA, inertiaB, csi, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector dt, in Vector accumulatedImpulses, ref LinearAxisMotorPrestepData prestep) { diff --git a/BepuPhysics/Constraints/LinearAxisServo.cs b/BepuPhysics/Constraints/LinearAxisServo.cs index d89fb8763..aa44099dc 100644 --- a/BepuPhysics/Constraints/LinearAxisServo.cs +++ b/BepuPhysics/Constraints/LinearAxisServo.cs @@ -252,15 +252,13 @@ public static void ComputeJacobians(in Vector3Wide ab, in QuaternionWide orienta [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ComputeEffectiveMass(in Vector3Wide angularJA, in Vector3Wide angularJB, - in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, float dt, - in SpringSettingsWide springSettings, - out Vector positionErrorToVelocity, out Vector softnessImpulseScale, out Vector3Wide angularImpulseToVelocityA, out Vector3Wide angularImpulseToVelocityB, out Vector effectiveMass) + in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, + in Vector effectiveMassCFMScale, out Vector3Wide angularImpulseToVelocityA, out Vector3Wide angularImpulseToVelocityB, out Vector effectiveMass) { Symmetric3x3Wide.TransformWithoutOverlap(angularJA, inertiaA.InverseInertiaTensor, out angularImpulseToVelocityA); Symmetric3x3Wide.TransformWithoutOverlap(angularJB, inertiaB.InverseInertiaTensor, out angularImpulseToVelocityB); Vector3Wide.Dot(angularJA, angularImpulseToVelocityA, out var angularContributionA); Vector3Wide.Dot(angularJB, angularImpulseToVelocityB, out var angularContributionB); - SpringSettingsWide.ComputeSpringiness(springSettings, dt, out positionErrorToVelocity, out var effectiveMassCFMScale, out softnessImpulseScale); effectiveMass = effectiveMassCFMScale / (inertiaA.InverseMass + inertiaB.InverseMass + angularContributionA + angularContributionB); } @@ -275,9 +273,8 @@ public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref LinearAxisServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeJacobians(positionB - positionA, orientationA, orientationB, prestep.LocalPlaneNormal, prestep.LocalOffsetB, out var anchorB, out var normal, out var angularJA, out var angularJB); - ComputeEffectiveMass(angularJA, angularJB, inertiaA, inertiaB, dt, prestep.SpringSettings, - out var positionErrorToVelocity, out var softnessImpulseScale, - out var angularImpulseToVelocityA, out var angularImpulseToVelocityB, out var effectiveMass); + SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); + ComputeEffectiveMass(angularJA, angularJB, inertiaA, inertiaB, effectiveMassCFMScale, out var angularImpulseToVelocityA, out var angularImpulseToVelocityB, out var effectiveMass); QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetA, orientationA, out var anchorA); Vector3Wide.Dot(anchorB - anchorA, normal, out var planeNormalDot); From 6a744ee8e49bd02a2070fb5424ccca4c5cc00478 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 27 Sep 2021 16:11:38 -0500 Subject: [PATCH 165/947] Fixed twoified angular axis motor. --- BepuPhysics/Constraints/AngularAxisMotor.cs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/BepuPhysics/Constraints/AngularAxisMotor.cs b/BepuPhysics/Constraints/AngularAxisMotor.cs index 104d2cb0f..970d35d6c 100644 --- a/BepuPhysics/Constraints/AngularAxisMotor.cs +++ b/BepuPhysics/Constraints/AngularAxisMotor.cs @@ -153,14 +153,14 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in MotorSettingsWide.ComputeSoftness(prestep.Settings, dt, out var effectiveMassCFMScale, out var softnessImpulseScale, out var maximumImpulse); //csi = projection.BiasImpulse - accumulatedImpulse * softnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); - var csi = effectiveMassCFMScale * (Vector3Wide.Dot(wsvB.Angular, jA) - Vector3Wide.Dot(wsvA.Angular, jA)) / (contributionA + contributionB) - accumulatedImpulses * softnessImpulseScale; + var csi = effectiveMassCFMScale * (prestep.TargetVelocity + Vector3Wide.Dot(wsvB.Angular, jA) - Vector3Wide.Dot(wsvA.Angular, jA)) / (contributionA + contributionB) - accumulatedImpulses * softnessImpulseScale; ServoSettingsWide.ClampImpulse(maximumImpulse, ref accumulatedImpulses, ref csi); ApplyImpulse(jA, accumulatedImpulses, inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, ref wsvA.Angular, ref wsvB.Angular); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector dt, in Vector accumulatedImpulses, ref AngularAxisMotorPrestepData prestep) { From 0d8616de4231914f1f0ff08c0a98c989f5e47e4d Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 27 Sep 2021 16:22:16 -0500 Subject: [PATCH 166/947] AngularHinge twoified. --- BepuPhysics/Constraints/AngularHinge.cs | 102 +++++++++++++++++++++++- 1 file changed, 99 insertions(+), 3 deletions(-) diff --git a/BepuPhysics/Constraints/AngularHinge.cs b/BepuPhysics/Constraints/AngularHinge.cs index f00c95c3c..427f56e06 100644 --- a/BepuPhysics/Constraints/AngularHinge.cs +++ b/BepuPhysics/Constraints/AngularHinge.cs @@ -226,19 +226,115 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, ref projection, ref csi); } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + private static void ApplyImpulse(in Matrix2x3Wide impulseToVelocityA, in Matrix2x3Wide negatedImpulseToVelocityB, in Vector2Wide csi, ref Vector3Wide angularVelocityA, ref Vector3Wide angularVelocityB) + { + Matrix2x3Wide.Transform(csi, impulseToVelocityA, out var velocityChangeA); + Vector3Wide.Add(angularVelocityA, velocityChangeA, out angularVelocityA); + Matrix2x3Wide.Transform(csi, negatedImpulseToVelocityB, out var negatedVelocityChangeB); + Vector3Wide.Subtract(angularVelocityB, negatedVelocityChangeB, out angularVelocityB); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + private static void ComputeJacobians(in Vector3Wide localHingeAxisA, in QuaternionWide orientationA, out Vector3Wide hingeAxisA, out Matrix2x3Wide jacobianA) + { + //Note that we build the tangents in local space first to avoid inconsistencies. + Helpers.BuildOrthonormalBasis(localHingeAxisA, out var localAX, out var localAY); + Matrix3x3Wide.CreateFromQuaternion(orientationA, out var orientationMatrixA); + Matrix3x3Wide.TransformWithoutOverlap(localHingeAxisA, orientationMatrixA, out hingeAxisA); + Matrix3x3Wide.TransformWithoutOverlap(localAX, orientationMatrixA, out jacobianA.X); + Matrix3x3Wide.TransformWithoutOverlap(localAY, orientationMatrixA, out jacobianA.Y); + } + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref AngularHingePrestepData prestep, ref Vector2Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - throw new NotImplementedException(); + ComputeJacobians(prestep.LocalHingeAxisA, orientationA, out _, out var jacobianA); + Symmetric3x3Wide.MultiplyWithoutOverlap(jacobianA, inertiaA.InverseInertiaTensor, out var impulseToVelocityA); + Symmetric3x3Wide.MultiplyWithoutOverlap(jacobianA, inertiaB.InverseInertiaTensor, out var negatedImpulseToVelocityB); + ApplyImpulse(impulseToVelocityA, negatedImpulseToVelocityB, accumulatedImpulses, ref wsvA.Angular, ref wsvB.Angular); } public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref AngularHingePrestepData prestep, ref Vector2Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - throw new NotImplementedException(); + //Note that we build the tangents in local space first to avoid inconsistencies. + ComputeJacobians(prestep.LocalHingeAxisA, orientationA, out var hingeAxisA, out var jacobianA); + QuaternionWide.TransformWithoutOverlap(prestep.LocalHingeAxisB, orientationB, out var hingeAxisB); + + //We project hingeAxisB onto the planes defined by A's axis X and and axis Y, and treat them as constant with respect to A's velocity. + //This hand waves away a bit of complexity related to the fact that A's axes have velocity too, but it works out pretty nicely in the end. + //hingeAxisBOnPlaneX = hingeAxisB - dot(constraintAxisX, hingeAxisB) * constraintAxisX + //hingeAxisBOnPlaneY = hingeAxisB - dot(constraintAxisY, hingeAxisB) * constraintAxisY + //Note that we actually make use of inverse trig here. This is largely for the sake of the formulation, and the derivative will end up collapsing nicely. + //C = [atan(dot(hingeAxisBOnPlaneX, hingeAxisA), dot(hingeAxisBOnPlaneX, constraintAxisAY))] = [0] + // [atan(dot(hingeAxisBOnPlaneY, hingeAxisA), dot(hingeAxisBOnPlaneY, constraintAxisAX))] [0] + //Focusing on the hingeAxisOnPlaneX jacobian: + //C' = (dot(hingeAxisBOnPlaneX, hingeAxisA) * d/dt(dot(hingeAxisBOnPlaneX, constraintAxisAY)) - + // d/dt(dot(hingeAxisBOnPlaneX, hingeAxisA)) * dot(hingeAxisBOnPlaneX, constraintAxisAY)) * denom + //where denom = 1f / (dot(hingeAxisBOnPlaneX, hingeAxisA)^2 + dot(hingeAxisBOnPlaneX, constraintAxisAY)^2) + //C' = (dot(hingeAxisBOnPlaneX, hingeAxisA) * (dot(d/dt(hingeAxisBOnPlaneX), constraintAxisAY) + dot(hingeAxisBOnPlaneX, d/dt(constraintAxisAY))) - + // (dot(d/dt(hingeAxisBOnPlaneX), hingeAxisA) + dot(hingeAxisBOnPlaneX, d/dt(hingeAxisA))) * dot(hingeAxisBOnPlaneX, constraintAxisAY)) * denom + //C' = (dot(hingeAxisBOnPlaneX, hingeAxisA) * (dot(wB x hingeAxisBOnPlaneX, constraintAxisAY) + dot(hingeAxisBOnPlaneX, wA x constraintAxisAY)) - + // (dot(wB x hingeAxisBOnPlaneX, hingeAxisA) + dot(hingeAxisBOnPlaneX, wA x hingeAxisA)) * dot(hingeAxisBOnPlaneX, constraintAxisAY)) * denom + //C' = (dot(hingeAxisBOnPlaneX, hingeAxisA) * (dot(hingeAxisBOnPlaneX x constraintAxisAY, wB) + dot(wA, constraintAxisAY x hingeAxisBOnPlaneX)) - + // (dot(hingeAxisBOnPlaneX x hingeAxisA, wB) + dot(wA, hingeAxisA x hingeAxisBOnPlaneX)) * dot(hingeAxisBOnPlaneX, constraintAxisAY)) * denom + //C' = ((dot(dot(hingeAxisBOnPlaneX, hingeAxisA) * (hingeAxisBOnPlaneX x constraintAxisAY), wB) + dot(wA, dot(hingeAxisBOnPlaneX, hingeAxisA) * (constraintAxisAY x hingeAxisBOnPlaneX))) - + // (dot((hingeAxisBOnPlaneX x hingeAxisA) * dot(hingeAxisBOnPlaneX, constraintAxisAY), wB) + dot(wA, (hingeAxisA x hingeAxisBOnPlaneX) * dot(hingeAxisBOnPlaneX, constraintAxisAY)))) * denom + + //C' = ((dot(dot(hingeAxisBOnPlaneX, hingeAxisA) * (hingeAxisBOnPlaneX x constraintAxisAY) - (hingeAxisBOnPlaneX x hingeAxisA) * dot(hingeAxisBOnPlaneX, constraintAxisAY), wB) + + // dot(wA, dot(hingeAxisBOnPlaneX, hingeAxisA) * (constraintAxisAY x hingeAxisBOnPlaneX) - (hingeAxisA x hingeAxisBOnPlaneX) * dot(hingeAxisBOnPlaneX, constraintAxisAY)))) * denom + //C' = (dot(wB, dot(hingeAxisBOnPlaneX, hingeAxisA) * (hingeAxisBOnPlaneX x constraintAxisAY) - + // dot(hingeAxisBOnPlaneX, constraintAxisAY) * (hingeAxisBOnPlaneX x hingeAxisA) + + // dot(wA, dot(hingeAxisBOnPlaneX, hingeAxisA) * (constraintAxisAY x hingeAxisBOnPlaneX) - + // dot(hingeAxisBOnPlaneX, constraintAxisAY) * (hingeAxisA x hingeAxisBOnPlaneX)) * denom + //Note that both contributing vectors of jacobian A, constraintAxisAY x hingeAxisBOnPlaneX and hingeAxisA x hingeAxisBOnPlaneX, are aligned with constraintAxisAX. + //The only remaining question is the scale. Measure it by dotting with the constraintAxisAX. + //(Switching notation for conciseness here: a = hingeAxisA, b = hingeAxisBOnPlaneX, x = constraintAxisAX, y = constraintAxisAY) + //(dot(b, a) * cross(y, b) - dot(b, y) * cross(a, b)) / (dot(b, a)^2 + dot(b,y)^2) + //scale = dot((dot(b, a) * cross(y, b) - dot(b, y) * cross(a, b)) / (dot(b, a)^2 + dot(b,y)^2), x) + //scale = (dot(b, a) * dot(x, cross(y, b)) - dot(b, y) * dot(x, cross(a, b))) / (dot(b, a)^2 + dot(b,y)^2) + //scale = (dot(b, a) * dot(b, cross(x, y)) - dot(b, y) * dot(b, cross(x, a))) / (dot(b, a)^2 + dot(b,y)^2) + //scale = (dot(b, a) * dot(b, a) - dot(b, y) * dot(b, -y)) / (dot(b, a)^2 + dot(b,y)^2) + //scale = (dot(b, a) * dot(b, a) + dot(b, y) * dot(b, y)) / (dot(b, a)^2 + dot(b,y)^2) + //scale = 1 + //How convenient! + //jacobianA = [constraintAxisAX] + // [constraintAxisAY] + //jacobianB = -jacobianA + + //Note that JA = -JB, but for the purposes of calculating the effective mass the sign is irrelevant. + //This computes the effective mass using the usual (J * M^-1 * JT)^-1 formulation, but we actually make use of the intermediate result J * M^-1 so we compute it directly. + Symmetric3x3Wide.MultiplyWithoutOverlap(jacobianA, inertiaA.InverseInertiaTensor, out var impulseToVelocityA); + //Note that we don't use -jacobianA here, so we're actually storing out the negated version of the transform. That's fine; we'll simply subtract in the iteration. + Symmetric3x3Wide.MultiplyWithoutOverlap(jacobianA, inertiaB.InverseInertiaTensor, out var negatedImpulseToVelocityB); + Symmetric2x2Wide.CompleteMatrixSandwich(impulseToVelocityA, jacobianA, out var angularA); + Symmetric2x2Wide.CompleteMatrixSandwich(negatedImpulseToVelocityB, jacobianA, out var angularB); + Symmetric2x2Wide.Add(angularA, angularB, out var inverseEffectiveMass); + Symmetric2x2Wide.InvertWithoutOverlap(inverseEffectiveMass, out var effectiveMass); + + SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); + Symmetric2x2Wide.Scale(effectiveMass, effectiveMassCFMScale, out effectiveMass); + Symmetric2x2Wide.MultiplyTransposed(jacobianA, effectiveMass, out var velocityToImpulseA); + GetErrorAngles(hingeAxisA, hingeAxisB, jacobianA, out var errorAngle); + //Note the negation: we want to oppose the separation. TODO: arguably, should bake the negation into positionErrorToVelocity, given its name. + Vector2Wide.Scale(errorAngle, -positionErrorToVelocity, out var biasVelocity); + Symmetric2x2Wide.TransformWithoutOverlap(biasVelocity, effectiveMass, out var biasImpulse); + + //JB = -JA. This is (angularVelocityA * JA + angularVelocityB * JB) * effectiveMass => (angularVelocityA - angularVelocityB) * (JA * effectiveMass) + Vector3Wide.Subtract(wsvA.Angular, wsvB.Angular, out var difference); + Matrix2x3Wide.TransformByTransposeWithoutOverlap(difference, velocityToImpulseA, out var csi); + //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); + Vector2Wide.Scale(accumulatedImpulses, softnessImpulseScale, out var softnessContribution); + Vector2Wide.Add(softnessContribution, csi, out csi); + Vector2Wide.Subtract(biasImpulse, csi, out csi); + + Vector2Wide.Add(accumulatedImpulses, csi, out accumulatedImpulses); + + ApplyImpulse(impulseToVelocityA, negatedImpulseToVelocityB, csi, ref wsvA.Angular, ref wsvB.Angular); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector dt, in Vector2Wide accumulatedImpulses, ref AngularHingePrestepData prestep) { From 0c76b60146f19fb45480997669c981d9045b6923 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 27 Sep 2021 16:31:10 -0500 Subject: [PATCH 167/947] Constraint test demo updating incrementally as twoification proceeds. --- Demos/DemoSet.cs | 1 + Demos/SpecializedTests/ConstraintTestDemo.cs | 477 +++++++++---------- 2 files changed, 239 insertions(+), 239 deletions(-) diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index 06b69ca1c..4e5de6939 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -45,6 +45,7 @@ struct Option public DemoSet() { + AddOption(); AddOption(); AddOption(); AddOption(); diff --git a/Demos/SpecializedTests/ConstraintTestDemo.cs b/Demos/SpecializedTests/ConstraintTestDemo.cs index ab7ffca24..5909a8548 100644 --- a/Demos/SpecializedTests/ConstraintTestDemo.cs +++ b/Demos/SpecializedTests/ConstraintTestDemo.cs @@ -5,8 +5,6 @@ using System.Numerics; using DemoContentLoader; using BepuPhysics.Constraints; -using Demos.Demos; -using System; namespace Demos.SpecializedTests { @@ -24,7 +22,8 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(25, 4, 40); camera.Yaw = 0; Simulation = Simulation.Create(BufferPool, - new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); + //new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); var shapeA = new Box(.75f, 1, .5f); var shapeIndexA = Simulation.Shapes.Add(shapeA); @@ -49,103 +48,103 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); Simulation.Solver.Add(a, b, new AngularHinge { LocalHingeAxisA = new Vector3(0, 1, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); } - { - var x = GetNextPosition(ref nextX); - var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); - var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - Simulation.Solver.Add(a, b, new Hinge - { - LocalOffsetA = new Vector3(0, 1, 0), - LocalHingeAxisA = new Vector3(0, 1, 0), - LocalOffsetB = new Vector3(0, -1, 0), - LocalHingeAxisB = new Vector3(0, 1, 0), - SpringSettings = new SpringSettings(30, 1) - }); - } - { - var x = GetNextPosition(ref nextX); - var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); - var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - Simulation.Solver.Add(a, b, new AngularSwivelHinge { LocalSwivelAxisA = new Vector3(1, 0, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); - Simulation.Solver.Add(a, b, new SwingLimit { AxisLocalA = new Vector3(0, 1, 0), AxisLocalB = new Vector3(0, 1, 0), MaximumSwingAngle = MathHelper.PiOver2, SpringSettings = new SpringSettings(30, 1) }); - } - { - var x = GetNextPosition(ref nextX); - var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); - var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - Simulation.Solver.Add(a, b, new SwivelHinge - { - LocalOffsetA = new Vector3(0, 1, 0), - LocalSwivelAxisA = new Vector3(1, 0, 0), - LocalOffsetB = new Vector3(0, -1, 0), - LocalHingeAxisB = new Vector3(0, 1, 0), - SpringSettings = new SpringSettings(30, 1) - }); - Simulation.Solver.Add(a, b, new SwingLimit { AxisLocalA = new Vector3(0, 1, 0), AxisLocalB = new Vector3(0, 1, 0), MaximumSwingAngle = MathHelper.PiOver2, SpringSettings = new SpringSettings(30, 1) }); - } - { - var x = GetNextPosition(ref nextX); - var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); - var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - Simulation.Solver.Add(a, b, new TwistServo - { - LocalBasisA = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)), - LocalBasisB = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)), - TargetAngle = MathHelper.PiOver4, - SpringSettings = new SpringSettings(30, 1), - ServoSettings = new ServoSettings(float.MaxValue, 0, float.MaxValue) - }); - } - { - var x = GetNextPosition(ref nextX); - var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); - var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - Simulation.Solver.Add(a, b, new TwistLimit - { - LocalBasisA = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)), - LocalBasisB = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)), - MinimumAngle = MathHelper.Pi * -0.5f, - MaximumAngle = MathHelper.Pi * 0.95f, - SpringSettings = new SpringSettings(30, 1), - }); - Simulation.Solver.Add(a, b, new AngularHinge { LocalHingeAxisA = new Vector3(0, 1, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); - } - { - var x = GetNextPosition(ref nextX); - var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); - var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - Simulation.Solver.Add(a, b, new TwistMotor - { - LocalAxisA = new Vector3(0, 1, 0), - LocalAxisB = new Vector3(0, 1, 0), - TargetVelocity = MathHelper.Pi * 2, - Settings = new MotorSettings(float.MaxValue, 0.1f) - }); - Simulation.Solver.Add(a, b, new AngularHinge { LocalHingeAxisA = new Vector3(0, 1, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); - } - { - var x = GetNextPosition(ref nextX); - var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); - var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - Simulation.Solver.Add(a, b, new AngularServo - { - TargetRelativeRotationLocalA = QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), MathHelper.PiOver2), - ServoSettings = new ServoSettings(float.MaxValue, 0, 12f), - SpringSettings = new SpringSettings(30, 1) - }); - } - { - var x = GetNextPosition(ref nextX); - var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); - var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - Simulation.Solver.Add(a, b, new AngularMotor { TargetVelocityLocalA = new Vector3(0, 1, 0), Settings = new MotorSettings(15, 0.0001f) }); - } + //{ + // var x = GetNextPosition(ref nextX); + // var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); + // var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + // Simulation.Solver.Add(a, b, new Hinge + // { + // LocalOffsetA = new Vector3(0, 1, 0), + // LocalHingeAxisA = new Vector3(0, 1, 0), + // LocalOffsetB = new Vector3(0, -1, 0), + // LocalHingeAxisB = new Vector3(0, 1, 0), + // SpringSettings = new SpringSettings(30, 1) + // }); + //} + //{ + // var x = GetNextPosition(ref nextX); + // var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); + // var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + // Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + // Simulation.Solver.Add(a, b, new AngularSwivelHinge { LocalSwivelAxisA = new Vector3(1, 0, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); + // Simulation.Solver.Add(a, b, new SwingLimit { AxisLocalA = new Vector3(0, 1, 0), AxisLocalB = new Vector3(0, 1, 0), MaximumSwingAngle = MathHelper.PiOver2, SpringSettings = new SpringSettings(30, 1) }); + //} + //{ + // var x = GetNextPosition(ref nextX); + // var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); + // var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + // Simulation.Solver.Add(a, b, new SwivelHinge + // { + // LocalOffsetA = new Vector3(0, 1, 0), + // LocalSwivelAxisA = new Vector3(1, 0, 0), + // LocalOffsetB = new Vector3(0, -1, 0), + // LocalHingeAxisB = new Vector3(0, 1, 0), + // SpringSettings = new SpringSettings(30, 1) + // }); + // Simulation.Solver.Add(a, b, new SwingLimit { AxisLocalA = new Vector3(0, 1, 0), AxisLocalB = new Vector3(0, 1, 0), MaximumSwingAngle = MathHelper.PiOver2, SpringSettings = new SpringSettings(30, 1) }); + //} + //{ + // var x = GetNextPosition(ref nextX); + // var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); + // var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + // Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + // Simulation.Solver.Add(a, b, new TwistServo + // { + // LocalBasisA = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)), + // LocalBasisB = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)), + // TargetAngle = MathHelper.PiOver4, + // SpringSettings = new SpringSettings(30, 1), + // ServoSettings = new ServoSettings(float.MaxValue, 0, float.MaxValue) + // }); + //} + //{ + // var x = GetNextPosition(ref nextX); + // var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); + // var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + // Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + // Simulation.Solver.Add(a, b, new TwistLimit + // { + // LocalBasisA = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)), + // LocalBasisB = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)), + // MinimumAngle = MathHelper.Pi * -0.5f, + // MaximumAngle = MathHelper.Pi * 0.95f, + // SpringSettings = new SpringSettings(30, 1), + // }); + // Simulation.Solver.Add(a, b, new AngularHinge { LocalHingeAxisA = new Vector3(0, 1, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); + //} + //{ + // var x = GetNextPosition(ref nextX); + // var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); + // var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + // Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + // Simulation.Solver.Add(a, b, new TwistMotor + // { + // LocalAxisA = new Vector3(0, 1, 0), + // LocalAxisB = new Vector3(0, 1, 0), + // TargetVelocity = MathHelper.Pi * 2, + // Settings = new MotorSettings(float.MaxValue, 0.1f) + // }); + // Simulation.Solver.Add(a, b, new AngularHinge { LocalHingeAxisA = new Vector3(0, 1, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); + //} + //{ + // var x = GetNextPosition(ref nextX); + // var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); + // var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + // Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + // Simulation.Solver.Add(a, b, new AngularServo + // { + // TargetRelativeRotationLocalA = QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), MathHelper.PiOver2), + // ServoSettings = new ServoSettings(float.MaxValue, 0, 12f), + // SpringSettings = new SpringSettings(30, 1) + // }); + //} + //{ + // var x = GetNextPosition(ref nextX); + // var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); + // var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + // Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + // Simulation.Solver.Add(a, b, new AngularMotor { TargetVelocityLocalA = new Vector3(0, 1, 0), Settings = new MotorSettings(15, 0.0001f) }); + //} { var x = GetNextPosition(ref nextX); var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); @@ -155,41 +154,41 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var b = Simulation.Bodies.Add(bDescription); Simulation.Solver.Add(a, b, new Weld { LocalOffset = new Vector3(0, 2, 0), LocalOrientation = Quaternion.Identity, SpringSettings = new SpringSettings(30, 1) }); } - { - var x = GetNextPosition(ref nextX); - var sphere = new Sphere(0.125f); - //Treat each vertex as a point mass that cannot rotate. - var sphereInertia = new BodyInertia { InverseMass = 1 }; - var sphereCollidable = new CollidableDescription(Simulation.Shapes.Add(sphere), 0.1f); - var a = new Vector3(x, 3, 0); - var b = new Vector3(x, 4, 0); - var c = new Vector3(x, 3, 1); - var d = new Vector3(x + 1, 3, 0); - var aDescription = BodyDescription.CreateDynamic(a, sphereInertia, sphereCollidable, activity); - var bDescription = BodyDescription.CreateDynamic(b, sphereInertia, sphereCollidable, activity); - var cDescription = BodyDescription.CreateDynamic(c, sphereInertia, sphereCollidable, activity); - var dDescription = BodyDescription.CreateDynamic(d, sphereInertia, sphereCollidable, activity); - var aHandle = Simulation.Bodies.Add(aDescription); - var bHandle = Simulation.Bodies.Add(bDescription); - var cHandle = Simulation.Bodies.Add(cDescription); - var dHandle = Simulation.Bodies.Add(dDescription); - var distanceSpringiness = new SpringSettings(3f, 1); - Simulation.Solver.Add(aHandle, bHandle, new CenterDistanceConstraint(Vector3.Distance(a, b), distanceSpringiness)); - Simulation.Solver.Add(aHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(a, c), distanceSpringiness)); - Simulation.Solver.Add(aHandle, dHandle, new CenterDistanceConstraint(Vector3.Distance(a, d), distanceSpringiness)); - Simulation.Solver.Add(bHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(b, c), distanceSpringiness)); - Simulation.Solver.Add(bHandle, dHandle, new CenterDistanceConstraint(Vector3.Distance(b, d), distanceSpringiness)); - Simulation.Solver.Add(cHandle, dHandle, new CenterDistanceConstraint(Vector3.Distance(c, d), distanceSpringiness)); - Simulation.Solver.Add(aHandle, bHandle, cHandle, dHandle, new VolumeConstraint(a, b, c, d, new SpringSettings(30, 1))); - } - { - var x = GetNextPosition(ref nextX); - var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); - var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); - var a = Simulation.Bodies.Add(aDescription); - var b = Simulation.Bodies.Add(bDescription); - Simulation.Solver.Add(a, b, new DistanceServo(new Vector3(0, 0.55f, 0), new Vector3(0, -0.55f, 0), 1.9f, new SpringSettings(30, 1), ServoSettings.Default)); - } + //{ + // var x = GetNextPosition(ref nextX); + // var sphere = new Sphere(0.125f); + // //Treat each vertex as a point mass that cannot rotate. + // var sphereInertia = new BodyInertia { InverseMass = 1 }; + // var sphereCollidable = new CollidableDescription(Simulation.Shapes.Add(sphere), 0.1f); + // var a = new Vector3(x, 3, 0); + // var b = new Vector3(x, 4, 0); + // var c = new Vector3(x, 3, 1); + // var d = new Vector3(x + 1, 3, 0); + // var aDescription = BodyDescription.CreateDynamic(a, sphereInertia, sphereCollidable, activity); + // var bDescription = BodyDescription.CreateDynamic(b, sphereInertia, sphereCollidable, activity); + // var cDescription = BodyDescription.CreateDynamic(c, sphereInertia, sphereCollidable, activity); + // var dDescription = BodyDescription.CreateDynamic(d, sphereInertia, sphereCollidable, activity); + // var aHandle = Simulation.Bodies.Add(aDescription); + // var bHandle = Simulation.Bodies.Add(bDescription); + // var cHandle = Simulation.Bodies.Add(cDescription); + // var dHandle = Simulation.Bodies.Add(dDescription); + // var distanceSpringiness = new SpringSettings(3f, 1); + // Simulation.Solver.Add(aHandle, bHandle, new CenterDistanceConstraint(Vector3.Distance(a, b), distanceSpringiness)); + // Simulation.Solver.Add(aHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(a, c), distanceSpringiness)); + // Simulation.Solver.Add(aHandle, dHandle, new CenterDistanceConstraint(Vector3.Distance(a, d), distanceSpringiness)); + // Simulation.Solver.Add(bHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(b, c), distanceSpringiness)); + // Simulation.Solver.Add(bHandle, dHandle, new CenterDistanceConstraint(Vector3.Distance(b, d), distanceSpringiness)); + // Simulation.Solver.Add(cHandle, dHandle, new CenterDistanceConstraint(Vector3.Distance(c, d), distanceSpringiness)); + // Simulation.Solver.Add(aHandle, bHandle, cHandle, dHandle, new VolumeConstraint(a, b, c, d, new SpringSettings(30, 1))); + //} + //{ + // var x = GetNextPosition(ref nextX); + // var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); + // var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); + // var a = Simulation.Bodies.Add(aDescription); + // var b = Simulation.Bodies.Add(bDescription); + // Simulation.Solver.Add(a, b, new DistanceServo(new Vector3(0, 0.55f, 0), new Vector3(0, -0.55f, 0), 1.9f, new SpringSettings(30, 1), ServoSettings.Default)); + //} { var x = GetNextPosition(ref nextX); var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); @@ -198,27 +197,27 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var b = Simulation.Bodies.Add(bDescription); Simulation.Solver.Add(a, b, new DistanceLimit(new Vector3(0, 0.55f, 0), new Vector3(0, -0.55f, 0), 1f, 3, new SpringSettings(30, 1))); } - { - var x = GetNextPosition(ref nextX); - var sphere = new Sphere(0.125f); - //Treat each vertex as a point mass that cannot rotate. - var sphereInertia = new BodyInertia { InverseMass = 1 }; - var sphereCollidable = new CollidableDescription(Simulation.Shapes.Add(sphere), 0.1f); - var a = new Vector3(x, 3, 0); - var b = new Vector3(x, 4, 0); - var c = new Vector3(x + 1, 3, 0); - var aDescription = BodyDescription.CreateDynamic(a, sphereInertia, sphereCollidable, activity); - var bDescription = BodyDescription.CreateDynamic(b, sphereInertia, sphereCollidable, activity); - var cDescription = BodyDescription.CreateDynamic(c, sphereInertia, sphereCollidable, activity); - var aHandle = Simulation.Bodies.Add(aDescription); - var bHandle = Simulation.Bodies.Add(bDescription); - var cHandle = Simulation.Bodies.Add(cDescription); - var distanceSpringiness = new SpringSettings(3f, 1); - Simulation.Solver.Add(aHandle, bHandle, new CenterDistanceConstraint(Vector3.Distance(a, b), distanceSpringiness)); - Simulation.Solver.Add(aHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(a, c), distanceSpringiness)); - Simulation.Solver.Add(bHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(b, c), distanceSpringiness)); - Simulation.Solver.Add(aHandle, bHandle, cHandle, new AreaConstraint(a, b, c, new SpringSettings(30, 1))); - } + //{ + // var x = GetNextPosition(ref nextX); + // var sphere = new Sphere(0.125f); + // //Treat each vertex as a point mass that cannot rotate. + // var sphereInertia = new BodyInertia { InverseMass = 1 }; + // var sphereCollidable = new CollidableDescription(Simulation.Shapes.Add(sphere), 0.1f); + // var a = new Vector3(x, 3, 0); + // var b = new Vector3(x, 4, 0); + // var c = new Vector3(x + 1, 3, 0); + // var aDescription = BodyDescription.CreateDynamic(a, sphereInertia, sphereCollidable, activity); + // var bDescription = BodyDescription.CreateDynamic(b, sphereInertia, sphereCollidable, activity); + // var cDescription = BodyDescription.CreateDynamic(c, sphereInertia, sphereCollidable, activity); + // var aHandle = Simulation.Bodies.Add(aDescription); + // var bHandle = Simulation.Bodies.Add(bDescription); + // var cHandle = Simulation.Bodies.Add(cDescription); + // var distanceSpringiness = new SpringSettings(3f, 1); + // Simulation.Solver.Add(aHandle, bHandle, new CenterDistanceConstraint(Vector3.Distance(a, b), distanceSpringiness)); + // Simulation.Solver.Add(aHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(a, c), distanceSpringiness)); + // Simulation.Solver.Add(bHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(b, c), distanceSpringiness)); + // Simulation.Solver.Add(aHandle, bHandle, cHandle, new AreaConstraint(a, b, c, new SpringSettings(30, 1))); + //} { var x = GetNextPosition(ref nextX); var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), default, collidableA, activity); @@ -320,18 +319,18 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) SpringSettings = new SpringSettings(5, 1) }); } - { - var x = GetNextPosition(ref nextX); - var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); - var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - Simulation.Solver.Add(a, new OneBodyLinearMotor - { - LocalOffset = new Vector3(0, 1, 0), - TargetVelocity = new Vector3(0, -1, 0), - Settings = new MotorSettings(float.MaxValue, 0.01f), - }); - } + //{ + // var x = GetNextPosition(ref nextX); + // var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); + // var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + // Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + // Simulation.Solver.Add(a, new OneBodyLinearMotor + // { + // LocalOffset = new Vector3(0, 1, 0), + // TargetVelocity = new Vector3(0, -1, 0), + // Settings = new MotorSettings(float.MaxValue, 0.01f), + // }); + //} { var x = GetNextPosition(ref nextX); var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); @@ -344,77 +343,77 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) SpringSettings = new SpringSettings(30f, 1f) }); } - { - var x = GetNextPosition(ref nextX); - var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); - var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - Simulation.Solver.Add(a, new OneBodyAngularMotor - { - TargetVelocity = new Vector3(1, 0, 0), - Settings = new MotorSettings(float.MaxValue, 0.001f), - }); - } - { - var x = GetNextPosition(ref nextX); - var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); - var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); - var a = Simulation.Bodies.Add(aDescription); - var b = Simulation.Bodies.Add(bDescription); - Simulation.Solver.Add(a, b, new BallSocketMotor - { - LocalOffsetB = new Vector3(0, -1, 0), - TargetVelocityLocalA = new Vector3(0, -0.25f, 0), - Settings = new MotorSettings(10, 1e-4f) - }); - } - { - var x = GetNextPosition(ref nextX); - var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); - var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); - var a = Simulation.Bodies.Add(aDescription); - var b = Simulation.Bodies.Add(bDescription); - Simulation.Solver.Add(a, b, new BallSocketServo - { - LocalOffsetA = new Vector3(0, 1, 0), - LocalOffsetB = new Vector3(0, -1, 0), - SpringSettings = new SpringSettings(30, 1), - ServoSettings = new ServoSettings(100, 1, 100) - }); - } - { - var x = GetNextPosition(ref nextX); - var wheelShape = new CollidableDescription(Simulation.Shapes.Add(new Cylinder(1, 0.1f)), 0.1f); - var wheelOrientation = Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), MathF.PI * 0.5f); - var aDescription = BodyDescription.CreateDynamic(new RigidPose(new Vector3(x, 3, 0), wheelOrientation), inertiaA, wheelShape, activity); - var bDescription = BodyDescription.CreateDynamic(new RigidPose(new Vector3(x, 6, 0), wheelOrientation), inertiaB, wheelShape, activity); - var cDescription = BodyDescription.CreateKinematic(new Vector3(x, 4.5f, -1), new CollidableDescription(Simulation.Shapes.Add(new Box(3, 6, 1)), 0.1f), activity); - var a = Simulation.Bodies.Add(aDescription); - var b = Simulation.Bodies.Add(bDescription); - var c = Simulation.Bodies.Add(cDescription); - Simulation.Solver.Add(a, b, new AngularAxisGearMotor - { - LocalAxisA = new Vector3(0, 1, 0), - VelocityScale = -4, - Settings = new MotorSettings(float.MaxValue, 0.0001f) - }); - Simulation.Solver.Add(c, a, new Hinge - { - LocalOffsetA = new Vector3(0, -1.5f, 1), - LocalHingeAxisA = new Vector3(0, 0, 1), - LocalOffsetB = new Vector3(0, 0, 0), - LocalHingeAxisB = new Vector3(0, 1, 0), - SpringSettings = new SpringSettings(30, 1) - }); - Simulation.Solver.Add(c, b, new Hinge - { - LocalOffsetA = new Vector3(0, 1.5f, 1), - LocalHingeAxisA = new Vector3(0, 0, 1), - LocalOffsetB = new Vector3(0, 0, 0), - LocalHingeAxisB = new Vector3(0, 1, 0), - SpringSettings = new SpringSettings(30, 1) - }); - } + //{ + // var x = GetNextPosition(ref nextX); + // var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); + // var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + // Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + // Simulation.Solver.Add(a, new OneBodyAngularMotor + // { + // TargetVelocity = new Vector3(1, 0, 0), + // Settings = new MotorSettings(float.MaxValue, 0.001f), + // }); + //} + //{ + // var x = GetNextPosition(ref nextX); + // var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); + // var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); + // var a = Simulation.Bodies.Add(aDescription); + // var b = Simulation.Bodies.Add(bDescription); + // Simulation.Solver.Add(a, b, new BallSocketMotor + // { + // LocalOffsetB = new Vector3(0, -1, 0), + // TargetVelocityLocalA = new Vector3(0, -0.25f, 0), + // Settings = new MotorSettings(10, 1e-4f) + // }); + //} + //{ + // var x = GetNextPosition(ref nextX); + // var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); + // var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); + // var a = Simulation.Bodies.Add(aDescription); + // var b = Simulation.Bodies.Add(bDescription); + // Simulation.Solver.Add(a, b, new BallSocketServo + // { + // LocalOffsetA = new Vector3(0, 1, 0), + // LocalOffsetB = new Vector3(0, -1, 0), + // SpringSettings = new SpringSettings(30, 1), + // ServoSettings = new ServoSettings(100, 1, 100) + // }); + //} + //{ + // var x = GetNextPosition(ref nextX); + // var wheelShape = new CollidableDescription(Simulation.Shapes.Add(new Cylinder(1, 0.1f)), 0.1f); + // var wheelOrientation = Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), MathF.PI * 0.5f); + // var aDescription = BodyDescription.CreateDynamic(new RigidPose(new Vector3(x, 3, 0), wheelOrientation), inertiaA, wheelShape, activity); + // var bDescription = BodyDescription.CreateDynamic(new RigidPose(new Vector3(x, 6, 0), wheelOrientation), inertiaB, wheelShape, activity); + // var cDescription = BodyDescription.CreateKinematic(new Vector3(x, 4.5f, -1), new CollidableDescription(Simulation.Shapes.Add(new Box(3, 6, 1)), 0.1f), activity); + // var a = Simulation.Bodies.Add(aDescription); + // var b = Simulation.Bodies.Add(bDescription); + // var c = Simulation.Bodies.Add(cDescription); + // Simulation.Solver.Add(a, b, new AngularAxisGearMotor + // { + // LocalAxisA = new Vector3(0, 1, 0), + // VelocityScale = -4, + // Settings = new MotorSettings(float.MaxValue, 0.0001f) + // }); + // Simulation.Solver.Add(c, a, new Hinge + // { + // LocalOffsetA = new Vector3(0, -1.5f, 1), + // LocalHingeAxisA = new Vector3(0, 0, 1), + // LocalOffsetB = new Vector3(0, 0, 0), + // LocalHingeAxisB = new Vector3(0, 1, 0), + // SpringSettings = new SpringSettings(30, 1) + // }); + // Simulation.Solver.Add(c, b, new Hinge + // { + // LocalOffsetA = new Vector3(0, 1.5f, 1), + // LocalHingeAxisA = new Vector3(0, 0, 1), + // LocalOffsetB = new Vector3(0, 0, 0), + // LocalHingeAxisB = new Vector3(0, 1, 0), + // SpringSettings = new SpringSettings(30, 1) + // }); + //} Simulation.Statics.Add(new StaticDescription(new Vector3(), new CollidableDescription(Simulation.Shapes.Add(new Box(256, 1, 256)), 0.1f))); } From 3451778f260143455668706ce9bb44a49d93cd3a Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 27 Sep 2021 19:47:30 -0500 Subject: [PATCH 168/947] Fixed bug with multiple velocity iterations in new work scheduler. --- BepuPhysics/Solver_SubsteppingSolve2.cs | 26 ++++++++++++++++--------- 1 file changed, 17 insertions(+), 9 deletions(-) diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index 5ae210d3e..be3688214 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -380,6 +380,8 @@ void SolveStep2Worker2(int workerIndex) { for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) { + //Note that this is using a 'different' stage by index than the worker thread if the iteration index > 1. + //That's totally fine- the warmstart/iteration stages share the same claims buffers per batch. They're redundant for the sake of easier indexing. ExecuteMainStage(ref solveStage, workerIndex, batchStarts[batchIndex], ref substepContext.Stages[batchIndex + 1], synchronizedBatchCount, ref syncIndex); } } @@ -444,7 +446,6 @@ void SolveStep2Worker2(int workerIndex) else { //Solve. - stageIndex -= synchronizedBatchCount; ref var stage = ref substepContext.Stages[stageIndex]; ExecuteWorkerStage(ref solveStage, workerIndex, batchStarts[stage.BatchIndex], stage.WorkBlockStartIndex, ref stage.Claims, synchronizedBatchCount, syncIndex, ref substepContext.CompletedWorkBlockCount); } @@ -482,7 +483,8 @@ protected void ExecuteMultithreaded2(float dt, IThreadDispatcher threadDispatche //Not every batch will actually have work blocks associated with it; the batch compressor could be falling behind, which means older constraints could be at higher batches than they need to be, leaving gaps. //We don't want to include those empty batches as sync points in the solver. GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); - pool.Take(1 + synchronizedBatchCount, out substepContext.Stages); + var iterationCountPlusOne = 1 + IterationCount; + pool.Take(1 + synchronizedBatchCount * iterationCountPlusOne, out substepContext.Stages); substepContext.SyncIndex = 0; var totalConstraintBatchWorkBlockCount = substepContext.ConstraintBatchBoundaries[^1]; @@ -491,14 +493,20 @@ protected void ExecuteMultithreaded2(float dt, IThreadDispatcher threadDispatche pool.Take(totalClaimCount, out var claims); claims.Clear(0, claims.Length); substepContext.Stages[0] = new(claims.Slice(incrementalBlocks.Count), 0); - int claimStart = incrementalBlocks.Count; - for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) + //Note that we create redundant stages that share the same workblock targets and claims buffers. + //This is just to make indexing a little simpler during the multithreaded work. + int targetStageIndex = 1; + for (int i = 0; i < iterationCountPlusOne; ++i) { - var stageIndex = batchIndex + 1; - var batchStart = batchIndex == 0 ? 0 : substepContext.ConstraintBatchBoundaries[batchIndex - 1]; - var workBlocksInBatch = substepContext.ConstraintBatchBoundaries[batchIndex] - batchStart; - substepContext.Stages[stageIndex] = new(claims.Slice(claimStart, workBlocksInBatch), batchStart, batchIndex); - claimStart += workBlocksInBatch; + int claimStart = incrementalBlocks.Count; + for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) + { + var stageIndex = targetStageIndex++; + var batchStart = batchIndex == 0 ? 0 : substepContext.ConstraintBatchBoundaries[batchIndex - 1]; + var workBlocksInBatch = substepContext.ConstraintBatchBoundaries[batchIndex] - batchStart; + substepContext.Stages[stageIndex] = new(claims.Slice(claimStart, workBlocksInBatch), batchStart, batchIndex); + claimStart += workBlocksInBatch; + } } //var syncCount = substepCount * (1 + synchronizedBatchCount * (1 + IterationCount)) - 1; From d38ee6cb5aca73b9abbd5841bc9ee4b80ce6f9fd Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 27 Sep 2021 20:03:40 -0500 Subject: [PATCH 169/947] Weld now uses current state for jacobian. Bit of a tradeoff- more accurate behavior, but lever arms are now state-defined and can explode more easily if the solver isn't given enough time. --- BepuPhysics/Constraints/Weld.cs | 31 +++++++++++++++---------------- 1 file changed, 15 insertions(+), 16 deletions(-) diff --git a/BepuPhysics/Constraints/Weld.cs b/BepuPhysics/Constraints/Weld.cs index 7aa741ef2..f47c7567c 100644 --- a/BepuPhysics/Constraints/Weld.cs +++ b/BepuPhysics/Constraints/Weld.cs @@ -244,7 +244,7 @@ private static void ApplyImpulse(in BodyInertiaWide inertiaA, in BodyInertiaWide Scale(offsetCSI, inertiaA.InverseMass, out var linearChangeA); Add(velocityA.Linear, linearChangeA, out velocityA.Linear); - //Note order of cross relative to the SolveIteration. + //Note order of cross relative to the Solve. //SolveIteration transforms velocity into constraint space velocity using JT, while this converts constraint space to world space using J. //The elements are transposed, and transposed skew symmetric matrices are negated. Flipping the cross product is equivalent to a negation. CrossWithoutOverlap(offset, offsetCSI, out var offsetWorldImpulse); @@ -263,10 +263,9 @@ private static void ApplyImpulse(in BodyInertiaWide inertiaA, in BodyInertiaWide public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref WeldPrestepData prestep, ref WeldAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - Transform(prestep.LocalOffset, orientationA, out var offset); - ApplyImpulse(inertiaA, inertiaB, offset, accumulatedImpulses.Orientation, accumulatedImpulses.Offset, ref wsvA, ref wsvB); + ApplyImpulse(inertiaA, inertiaB, positionB - positionA, accumulatedImpulses.Orientation, accumulatedImpulses.Offset, ref wsvA, ref wsvB); } - //[MethodImpl(MethodImplOptions.AggressiveInlining)] + //[MethodImpl(MethodImplOptions.NoInlining)] public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref WeldPrestepData prestep, ref WeldAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { @@ -283,16 +282,14 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in // [ I, skewSymmetric(localOffset * orientationA), -I, 0 ] //where I is the 3x3 identity matrix. - //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. - Transform(prestep.LocalOffset, orientationA, out var offset); - //Effective mass = (J * M^-1 * JT)^-1, which is going to be a little tricky because J * M^-1 * JT is a 6x6 matrix: //J * M^-1 * JT = [ Ia^-1 + Ib^-1, Ia^-1 * transpose(skewSymmetric(localOffset * orientationA)) ] // [ skewSymmetric(localOffset * orientationA) * Ia^-1, Ma^-1 + Mb^-1 + skewSymmetric(localOffset * orientationA) * Ia^-1 * transpose(skewSymmetric(localOffset * orientationA)) ] //where Ia^-1 and Ib^-1 are the inverse inertia tensors for a and b and Ma^-1 and Mb^-1 are the inverse masses of A and B expanded to 3x3 diagonal matrices. //var jmjtA = inertiaA.InverseInertiaTensor + inertiaB.InverseInertiaTensor; Add(inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, out var jmjtA); - CreateCrossProduct(offset, out var xAB); + var ab = positionB - positionA; + CreateCrossProduct(ab, out var xAB); Multiply(inertiaA.InverseInertiaTensor, xAB, out var jmjtB); //var jmjtB = inertiaA.InverseInertiaTensor * xAB; CompleteMatrixSandwichTranspose(xAB, jmjtB, out var jmjtD); @@ -301,7 +298,9 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in jmjtD.YY += diagonalAdd; jmjtD.ZZ += diagonalAdd; - var positionError = positionB - positionA - offset; + //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. + Transform(prestep.LocalOffset, orientationA, out var targetOffset); + var positionError = ab - targetOffset; var targetOrientationB = prestep.LocalOrientation * orientationA; //ConcatenateWithoutOverlap(prestep.LocalOrientation, orientationA, out var targetOrientationB); ConcatenateWithoutOverlap(Conjugate(targetOrientationB), orientationB, out var rotationError); @@ -323,9 +322,9 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in orientationCSV.Y = orientationBiasVelocity.Y - wsvA.Angular.Y + wsvB.Angular.Y; orientationCSV.Z = orientationBiasVelocity.Z - wsvA.Angular.Z + wsvB.Angular.Z; - offsetCSV.X = offsetBiasVelocity.X - wsvA.Linear.X + wsvB.Linear.X - (wsvA.Angular.Y * offset.Z - wsvA.Angular.Z * offset.Y); - offsetCSV.Y = offsetBiasVelocity.Y - wsvA.Linear.Y + wsvB.Linear.Y - (wsvA.Angular.Z * offset.X - wsvA.Angular.X * offset.Z); - offsetCSV.Z = offsetBiasVelocity.Z - wsvA.Linear.Z + wsvB.Linear.Z - (wsvA.Angular.X * offset.Y - wsvA.Angular.Y * offset.X); + offsetCSV.X = offsetBiasVelocity.X - wsvA.Linear.X + wsvB.Linear.X - (wsvA.Angular.Y * ab.Z - wsvA.Angular.Z * ab.Y); + offsetCSV.Y = offsetBiasVelocity.Y - wsvA.Linear.Y + wsvB.Linear.Y - (wsvA.Angular.Z * ab.X - wsvA.Angular.X * ab.Z); + offsetCSV.Z = offsetBiasVelocity.Z - wsvA.Linear.Z + wsvB.Linear.Z - (wsvA.Angular.X * ab.Y - wsvA.Angular.Y * ab.X); //Note that there is no need to invert the 6x6 inverse effective mass matrix chonk. We want to convert a constraint space velocity into a constraint space impulse, csi = csv * effectiveMass. //This is equivalent to solving csi * effectiveMass^-1 = csv for csi, and since effectiveMass^-1 is symmetric positive semidefinite, we can use an LDLT decomposition to quickly solve it. @@ -353,7 +352,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in accumulatedImpulses.Offset.Y += offsetCSI.Y; accumulatedImpulses.Offset.Z += offsetCSI.Z; - ApplyImpulse(inertiaA, inertiaB, offset, orientationCSI, offsetCSI, ref wsvA, ref wsvB); + ApplyImpulse(inertiaA, inertiaB, ab, orientationCSI, offsetCSI, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -368,8 +367,8 @@ public void UpdateForNewPose( public struct WeldWarmStartAccessFilterA : IBodyAccessFilter { - public bool GatherPosition => false; - public bool GatherOrientation => true; + public bool GatherPosition => true; + public bool GatherOrientation => false; public bool GatherMass => true; public bool GatherInertiaTensor => true; public bool AccessLinearVelocity => true; @@ -377,7 +376,7 @@ public struct WeldWarmStartAccessFilterA : IBodyAccessFilter } public struct WeldWarmStartAccessFilterB : IBodyAccessFilter { - public bool GatherPosition => false; + public bool GatherPosition => true; public bool GatherOrientation => false; public bool GatherMass => true; public bool GatherInertiaTensor => true; From add17dfded67e9147b9089be5617acd911896cad Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 28 Sep 2021 13:02:38 -0500 Subject: [PATCH 170/947] BallSocket variants twoified. --- BepuPhysics/Constraints/BallSocket.cs | 18 ++++++------- BepuPhysics/Constraints/BallSocketMotor.cs | 25 ++++++++++++------ BepuPhysics/Constraints/BallSocketServo.cs | 27 +++++++++++++++----- BepuPhysics/Constraints/BallSocketShared.cs | 6 ++--- BepuPhysics/Constraints/IBodyAccessFilter.cs | 10 +++++++- 5 files changed, 57 insertions(+), 29 deletions(-) diff --git a/BepuPhysics/Constraints/BallSocket.cs b/BepuPhysics/Constraints/BallSocket.cs index e0a9d9a73..8ed618dc2 100644 --- a/BepuPhysics/Constraints/BallSocket.cs +++ b/BepuPhysics/Constraints/BallSocket.cs @@ -14,11 +14,11 @@ namespace BepuPhysics.Constraints public struct BallSocket : ITwoBodyConstraintDescription { /// - /// Local offset from the center of body A to its attachment point. + /// Offset from the center of body A to its attachment in A's local space. /// public Vector3 LocalOffsetA; /// - /// Local offset from the center of body B to its attachment point. + /// Offset from the center of body B to its attachment in B's local space. /// public Vector3 LocalOffsetB; /// @@ -88,7 +88,7 @@ public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetA, orientationA, out projection.OffsetA); QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetB, orientationB, out projection.OffsetB); SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); - BallSocketShared.ComputeEffectiveMass(inertiaA, inertiaB, ref projection.OffsetA, ref projection.OffsetB, ref effectiveMassCFMScale, out projection.EffectiveMass); + BallSocketShared.ComputeEffectiveMass(inertiaA, inertiaB, projection.OffsetA, projection.OffsetB, effectiveMassCFMScale, out projection.EffectiveMass); //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. Vector3Wide.Add(ab, projection.OffsetB, out var anchorB); @@ -112,20 +112,18 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref BallSocketPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - //Note that we must reconstruct the world offsets from the body orientations since we do not store world offsets. QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetA, orientationA, out var offsetA); QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetB, orientationB, out var offsetB); BallSocketShared.ApplyImpulse(ref wsvA, ref wsvB, offsetA, offsetB, inertiaA, inertiaB, accumulatedImpulses); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref BallSocketPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) - { - //Note that we must reconstruct the world offsets from the body orientations since we do not store world offsets. + { QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetA, orientationA, out var offsetA); QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetB, orientationB, out var offsetB); SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); - BallSocketShared.ComputeEffectiveMass(inertiaA, inertiaB, ref offsetA, ref offsetB, ref effectiveMassCFMScale, out var effectiveMass); + BallSocketShared.ComputeEffectiveMass(inertiaA, inertiaB, offsetA, offsetB, effectiveMassCFMScale, out var effectiveMass); //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. var ab = positionB - positionA; @@ -138,7 +136,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in [MethodImpl(MethodImplOptions.AggressiveInlining)] public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector dt, in Vector3Wide accumulatedImpulses, ref BallSocketPrestepData prestep) { @@ -149,7 +147,7 @@ public void UpdateForNewPose( /// /// Handles the solve iterations of a bunch of ball socket constraints. /// - public class BallSocketTypeProcessor : TwoBodyTypeProcessor + public class BallSocketTypeProcessor : TwoBodyTypeProcessor { public const int BatchTypeId = 22; } diff --git a/BepuPhysics/Constraints/BallSocketMotor.cs b/BepuPhysics/Constraints/BallSocketMotor.cs index dea4ab0ed..5f2e4d722 100644 --- a/BepuPhysics/Constraints/BallSocketMotor.cs +++ b/BepuPhysics/Constraints/BallSocketMotor.cs @@ -12,13 +12,12 @@ namespace BepuPhysics.Constraints { /// - /// Constrains the relative linear velocity between two bodies to a target. - /// Conceptually, controls the relative velocity by a virtual lever arm attached to the center of A and leading to the anchor of B. + /// Controls the relative linear velocity from the center of body A to an attachment point on body B. /// public struct BallSocketMotor : ITwoBodyConstraintDescription { /// - /// Offset from body B to its anchor. + /// Offset from body B to its attachment in B's local space. /// public Vector3 LocalOffsetB; /// @@ -93,7 +92,7 @@ public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetB, orientationB, out projection.OffsetB); Vector3Wide.Add(ab, projection.OffsetB, out projection.OffsetA); MotorSettingsWide.ComputeSoftness(prestep.Settings, dt, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale, out projection.MaximumImpulse); - BallSocketShared.ComputeEffectiveMass(inertiaA, inertiaB, ref projection.OffsetA, ref projection.OffsetB, ref effectiveMassCFMScale, out projection.EffectiveMass); + BallSocketShared.ComputeEffectiveMass(inertiaA, inertiaB, projection.OffsetA, projection.OffsetB, effectiveMassCFMScale, out projection.EffectiveMass); QuaternionWide.Transform(prestep.TargetVelocityLocalA, orientationA, out projection.BiasVelocity); Vector3Wide.Negate(projection.BiasVelocity, out projection.BiasVelocity); @@ -108,17 +107,27 @@ public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide veloc [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref BallSocketMotorProjection projection, ref Vector3Wide accumulatedImpulse) { - BallSocketShared.Solve(ref velocityA, ref velocityB, ref projection.OffsetA, ref projection.OffsetB, ref projection.BiasVelocity, ref projection.EffectiveMass, ref projection.SoftnessImpulseScale, ref projection.MaximumImpulse, ref accumulatedImpulse, ref projection.InertiaA, ref projection.InertiaB); + BallSocketShared.Solve(ref velocityA, ref velocityB, projection.OffsetA, projection.OffsetB, projection.BiasVelocity, projection.EffectiveMass, projection.SoftnessImpulseScale, projection.MaximumImpulse, ref accumulatedImpulse, projection.InertiaA, projection.InertiaB); } public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref BallSocketMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - throw new NotImplementedException(); + QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetB, orientationB, out var targetOffsetB); + BallSocketShared.ApplyImpulse(ref wsvA, ref wsvB, (positionB - positionA) + targetOffsetB, targetOffsetB, inertiaA, inertiaB, accumulatedImpulses); } public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref BallSocketMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - throw new NotImplementedException(); + QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetB, orientationB, out var targetOffsetB); + var offsetA = (positionB - positionA) + targetOffsetB; + + MotorSettingsWide.ComputeSoftness(prestep.Settings, dt, out var effectiveMassCFMScale, out var softnessImpulseScale, out var maximumImpulse); + BallSocketShared.ComputeEffectiveMass(inertiaA, inertiaB, offsetA, targetOffsetB, effectiveMassCFMScale, out var effectiveMass); + + QuaternionWide.Transform(prestep.TargetVelocityLocalA, orientationA, out var biasVelocity); + Vector3Wide.Negate(biasVelocity, out biasVelocity); + + BallSocketShared.Solve(ref wsvA, ref wsvB, offsetA, targetOffsetB, biasVelocity, effectiveMass, softnessImpulseScale, maximumImpulse, ref accumulatedImpulses, inertiaA, inertiaB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -134,7 +143,7 @@ public void UpdateForNewPose( /// /// Handles the solve iterations of a bunch of ball socket motor constraints. /// - public class BallSocketMotorTypeProcessor : TwoBodyTypeProcessor + public class BallSocketMotorTypeProcessor : TwoBodyTypeProcessor { public const int BatchTypeId = 52; } diff --git a/BepuPhysics/Constraints/BallSocketServo.cs b/BepuPhysics/Constraints/BallSocketServo.cs index 6359b509f..0daa35bbf 100644 --- a/BepuPhysics/Constraints/BallSocketServo.cs +++ b/BepuPhysics/Constraints/BallSocketServo.cs @@ -15,11 +15,11 @@ namespace BepuPhysics.Constraints public struct BallSocketServo : ITwoBodyConstraintDescription { /// - /// Local offset from the center of body A to its attachment point. + /// Offset from the center of body A to its attachment in A's local space. /// public Vector3 LocalOffsetA; /// - /// Local offset from the center of body B to its attachment point. + /// Offset from the center of body B to its attachment in B's local space. /// public Vector3 LocalOffsetB; /// @@ -97,7 +97,7 @@ public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetA, orientationA, out projection.OffsetA); QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetB, orientationB, out projection.OffsetB); SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); - BallSocketShared.ComputeEffectiveMass(inertiaA, inertiaB, ref projection.OffsetA, ref projection.OffsetB, ref effectiveMassCFMScale, out projection.EffectiveMass); + BallSocketShared.ComputeEffectiveMass(inertiaA, inertiaB, projection.OffsetA, projection.OffsetB, effectiveMassCFMScale, out projection.EffectiveMass); //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. Vector3Wide.Add(ab, projection.OffsetB, out var anchorB); @@ -115,17 +115,30 @@ public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide veloc [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref BallSocketServoProjection projection, ref Vector3Wide accumulatedImpulse) { - BallSocketShared.Solve(ref velocityA, ref velocityB, ref projection.OffsetA, ref projection.OffsetB, ref projection.BiasVelocity, ref projection.EffectiveMass, ref projection.SoftnessImpulseScale, ref projection.MaximumImpulse, ref accumulatedImpulse, ref projection.InertiaA, ref projection.InertiaB); + BallSocketShared.Solve(ref velocityA, ref velocityB, projection.OffsetA, projection.OffsetB, projection.BiasVelocity, projection.EffectiveMass, projection.SoftnessImpulseScale, projection.MaximumImpulse, ref accumulatedImpulse, projection.InertiaA, projection.InertiaB); } public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref BallSocketServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - throw new NotImplementedException(); + QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetA, orientationA, out var offsetA); + QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetB, orientationB, out var offsetB); + BallSocketShared.ApplyImpulse(ref wsvA, ref wsvB, offsetA, offsetB, inertiaA, inertiaB, accumulatedImpulses); } public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref BallSocketServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - throw new NotImplementedException(); + QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetA, orientationA, out var offsetA); + QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetB, orientationB, out var offsetB); + SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); + BallSocketShared.ComputeEffectiveMass(inertiaA, inertiaB, offsetA, offsetB, effectiveMassCFMScale, out var effectiveMass); + + //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. + var ab = positionB - positionA; + Vector3Wide.Add(ab, offsetB, out var anchorB); + Vector3Wide.Subtract(anchorB, offsetA, out var error); + ServoSettingsWide.ComputeClampedBiasVelocity(error, positionErrorToVelocity, prestep.ServoSettings, dt, inverseDt, out var biasVelocity, out var maximumImpulse); + + BallSocketShared.Solve(ref wsvA, ref wsvB, offsetA, offsetB, biasVelocity, effectiveMass, softnessImpulseScale, maximumImpulse, ref accumulatedImpulses, inertiaA, inertiaB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -141,7 +154,7 @@ public void UpdateForNewPose( /// /// Handles the solve iterations of a bunch of ball socket servo constraints. /// - public class BallSocketServoTypeProcessor : TwoBodyTypeProcessor + public class BallSocketServoTypeProcessor : TwoBodyTypeProcessor { public const int BatchTypeId = 53; } diff --git a/BepuPhysics/Constraints/BallSocketShared.cs b/BepuPhysics/Constraints/BallSocketShared.cs index 309451a44..b985731c4 100644 --- a/BepuPhysics/Constraints/BallSocketShared.cs +++ b/BepuPhysics/Constraints/BallSocketShared.cs @@ -17,7 +17,7 @@ public static class BallSocketShared //The only reason not to do that is codegen concerns. But we may want to stop holding back just because of some hopefully-not-permanent quirks in the JIT. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ComputeEffectiveMass(in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, - ref Vector3Wide offsetA, ref Vector3Wide offsetB, ref Vector effectiveMassCFMScale, out Symmetric3x3Wide effectiveMass) + in Vector3Wide offsetA, in Vector3Wide offsetB, in Vector effectiveMassCFMScale, out Symmetric3x3Wide effectiveMass) { //Anchor points attached to each body are constrained to stay in the same position, yielding a position constraint of: //C = positionA + anchorOffsetA - (positionB + anchorOffsetB) = 0 @@ -123,8 +123,8 @@ public static void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide ve } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref Vector3Wide offsetA, ref Vector3Wide offsetB, - ref Vector3Wide biasVelocity, ref Symmetric3x3Wide effectiveMass, ref Vector softnessImpulseScale, ref Vector maximumImpulse, ref Vector3Wide accumulatedImpulse, ref BodyInertiaWide inertiaA, ref BodyInertiaWide inertiaB) + public static void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, in Vector3Wide offsetA, in Vector3Wide offsetB, + in Vector3Wide biasVelocity, in Symmetric3x3Wide effectiveMass, in Vector softnessImpulseScale, in Vector maximumImpulse, ref Vector3Wide accumulatedImpulse, in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB) { ComputeCorrectiveImpulse(ref velocityA, ref velocityB, offsetA, offsetB, biasVelocity, effectiveMass, softnessImpulseScale, accumulatedImpulse, out var correctiveImpulse); //This function DOES have a maximum impulse limit. diff --git a/BepuPhysics/Constraints/IBodyAccessFilter.cs b/BepuPhysics/Constraints/IBodyAccessFilter.cs index d569287f4..858e342f1 100644 --- a/BepuPhysics/Constraints/IBodyAccessFilter.cs +++ b/BepuPhysics/Constraints/IBodyAccessFilter.cs @@ -54,7 +54,15 @@ public struct AccessNoPose : IBodyAccessFilter public bool AccessLinearVelocity => true; public bool AccessAngularVelocity => true; } - + public struct AccessNoPosition : IBodyAccessFilter + { + public bool GatherPosition => false; + public bool GatherOrientation => true; + public bool GatherMass => true; + public bool GatherInertiaTensor => true; + public bool AccessLinearVelocity => true; + public bool AccessAngularVelocity => true; + } public struct AccessNoOrientation : IBodyAccessFilter { public bool GatherPosition => true; From 434d32c6fe6d17aa2c91ac66e82c0e5ab251bfb5 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 28 Sep 2021 13:07:21 -0500 Subject: [PATCH 171/947] Backed out the weld changes; weird lever arms relative to error can lead to wonky behavior. --- BepuPhysics/Constraints/Weld.cs | 46 ++++++++++----------------------- 1 file changed, 14 insertions(+), 32 deletions(-) diff --git a/BepuPhysics/Constraints/Weld.cs b/BepuPhysics/Constraints/Weld.cs index f47c7567c..84bb45e20 100644 --- a/BepuPhysics/Constraints/Weld.cs +++ b/BepuPhysics/Constraints/Weld.cs @@ -263,9 +263,10 @@ private static void ApplyImpulse(in BodyInertiaWide inertiaA, in BodyInertiaWide public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref WeldPrestepData prestep, ref WeldAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - ApplyImpulse(inertiaA, inertiaB, positionB - positionA, accumulatedImpulses.Orientation, accumulatedImpulses.Offset, ref wsvA, ref wsvB); + Transform(prestep.LocalOffset, orientationA, out var offset); + ApplyImpulse(inertiaA, inertiaB, offset, accumulatedImpulses.Orientation, accumulatedImpulses.Offset, ref wsvA, ref wsvB); } - //[MethodImpl(MethodImplOptions.NoInlining)] + //[MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref WeldPrestepData prestep, ref WeldAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { @@ -282,14 +283,16 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in // [ I, skewSymmetric(localOffset * orientationA), -I, 0 ] //where I is the 3x3 identity matrix. + //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. + Transform(prestep.LocalOffset, orientationA, out var offset); + //Effective mass = (J * M^-1 * JT)^-1, which is going to be a little tricky because J * M^-1 * JT is a 6x6 matrix: //J * M^-1 * JT = [ Ia^-1 + Ib^-1, Ia^-1 * transpose(skewSymmetric(localOffset * orientationA)) ] // [ skewSymmetric(localOffset * orientationA) * Ia^-1, Ma^-1 + Mb^-1 + skewSymmetric(localOffset * orientationA) * Ia^-1 * transpose(skewSymmetric(localOffset * orientationA)) ] //where Ia^-1 and Ib^-1 are the inverse inertia tensors for a and b and Ma^-1 and Mb^-1 are the inverse masses of A and B expanded to 3x3 diagonal matrices. //var jmjtA = inertiaA.InverseInertiaTensor + inertiaB.InverseInertiaTensor; Add(inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, out var jmjtA); - var ab = positionB - positionA; - CreateCrossProduct(ab, out var xAB); + CreateCrossProduct(offset, out var xAB); Multiply(inertiaA.InverseInertiaTensor, xAB, out var jmjtB); //var jmjtB = inertiaA.InverseInertiaTensor * xAB; CompleteMatrixSandwichTranspose(xAB, jmjtB, out var jmjtD); @@ -298,9 +301,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in jmjtD.YY += diagonalAdd; jmjtD.ZZ += diagonalAdd; - //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. - Transform(prestep.LocalOffset, orientationA, out var targetOffset); - var positionError = ab - targetOffset; + var positionError = positionB - positionA - offset; var targetOrientationB = prestep.LocalOrientation * orientationA; //ConcatenateWithoutOverlap(prestep.LocalOrientation, orientationA, out var targetOrientationB); ConcatenateWithoutOverlap(Conjugate(targetOrientationB), orientationB, out var rotationError); @@ -322,9 +323,9 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in orientationCSV.Y = orientationBiasVelocity.Y - wsvA.Angular.Y + wsvB.Angular.Y; orientationCSV.Z = orientationBiasVelocity.Z - wsvA.Angular.Z + wsvB.Angular.Z; - offsetCSV.X = offsetBiasVelocity.X - wsvA.Linear.X + wsvB.Linear.X - (wsvA.Angular.Y * ab.Z - wsvA.Angular.Z * ab.Y); - offsetCSV.Y = offsetBiasVelocity.Y - wsvA.Linear.Y + wsvB.Linear.Y - (wsvA.Angular.Z * ab.X - wsvA.Angular.X * ab.Z); - offsetCSV.Z = offsetBiasVelocity.Z - wsvA.Linear.Z + wsvB.Linear.Z - (wsvA.Angular.X * ab.Y - wsvA.Angular.Y * ab.X); + offsetCSV.X = offsetBiasVelocity.X - wsvA.Linear.X + wsvB.Linear.X - (wsvA.Angular.Y * offset.Z - wsvA.Angular.Z * offset.Y); + offsetCSV.Y = offsetBiasVelocity.Y - wsvA.Linear.Y + wsvB.Linear.Y - (wsvA.Angular.Z * offset.X - wsvA.Angular.X * offset.Z); + offsetCSV.Z = offsetBiasVelocity.Z - wsvA.Linear.Z + wsvB.Linear.Z - (wsvA.Angular.X * offset.Y - wsvA.Angular.Y * offset.X); //Note that there is no need to invert the 6x6 inverse effective mass matrix chonk. We want to convert a constraint space velocity into a constraint space impulse, csi = csv * effectiveMass. //This is equivalent to solving csi * effectiveMass^-1 = csv for csi, and since effectiveMass^-1 is symmetric positive semidefinite, we can use an LDLT decomposition to quickly solve it. @@ -352,12 +353,12 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in accumulatedImpulses.Offset.Y += offsetCSI.Y; accumulatedImpulses.Offset.Z += offsetCSI.Z; - ApplyImpulse(inertiaA, inertiaB, ab, orientationCSI, offsetCSI, ref wsvA, ref wsvB); + ApplyImpulse(inertiaA, inertiaB, offset, orientationCSI, offsetCSI, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector dt, in WeldAccumulatedImpulses accumulatedImpulses, ref WeldPrestepData prestep) { @@ -365,30 +366,11 @@ public void UpdateForNewPose( } - public struct WeldWarmStartAccessFilterA : IBodyAccessFilter - { - public bool GatherPosition => true; - public bool GatherOrientation => false; - public bool GatherMass => true; - public bool GatherInertiaTensor => true; - public bool AccessLinearVelocity => true; - public bool AccessAngularVelocity => true; - } - public struct WeldWarmStartAccessFilterB : IBodyAccessFilter - { - public bool GatherPosition => true; - public bool GatherOrientation => false; - public bool GatherMass => true; - public bool GatherInertiaTensor => true; - public bool AccessLinearVelocity => true; - public bool AccessAngularVelocity => true; - } - /// /// Handles the solve iterations of a bunch of ball socket constraints. /// - public class WeldTypeProcessor : TwoBodyTypeProcessor + public class WeldTypeProcessor : TwoBodyTypeProcessor { public const int BatchTypeId = 31; } From ceec5861165a4859f9761cd25a3c1fa52a27f49a Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 28 Sep 2021 14:38:20 -0500 Subject: [PATCH 172/947] LInearAxis* constraints now use angular jacobians that are more consistent with error calculations. --- BepuPhysics/Constraints/LinearAxisLimit.cs | 11 ++++++----- BepuPhysics/Constraints/LinearAxisMotor.cs | 4 ++-- BepuPhysics/Constraints/LinearAxisServo.cs | 20 +++++++++++--------- 3 files changed, 19 insertions(+), 16 deletions(-) diff --git a/BepuPhysics/Constraints/LinearAxisLimit.cs b/BepuPhysics/Constraints/LinearAxisLimit.cs index 5265ddd64..a7dd9d349 100644 --- a/BepuPhysics/Constraints/LinearAxisLimit.cs +++ b/BepuPhysics/Constraints/LinearAxisLimit.cs @@ -158,13 +158,13 @@ static void ComputeJacobians( //Linear jacobians are just normal and -normal. Angular jacobians are offsetA x normal and offsetB x normal. Matrix3x3Wide.CreateFromQuaternion(orientationA, out var orientationMatrixA); Matrix3x3Wide.TransformWithoutOverlap(localPlaneNormal, orientationMatrixA, out normal); + Matrix3x3Wide.TransformWithoutOverlap(localOffsetA, orientationMatrixA, out var anchorA); QuaternionWide.TransformWithoutOverlap(localOffsetB, orientationB, out var offsetB); + //Note that the angular jacobian for A uses the offset from A to the attachment point on B. + var anchorB = ab + offsetB; + Vector3Wide.Dot(anchorB - anchorA, normal, out var planeNormalDot); //The limit chooses the normal's sign depending on which limit is closer. - Matrix3x3Wide.TransformWithoutOverlap(localOffsetA, orientationMatrixA, out var anchorA); - var anchorB = ab + offsetB; - Vector3Wide.Subtract(anchorB, anchorA, out var anchorOffset); - Vector3Wide.Dot(anchorOffset, normal, out var planeNormalDot); var minimumError = minimumOffset - planeNormalDot; var maximumError = planeNormalDot - maximumOffset; var useMin = Vector.LessThan(Vector.Abs(minimumError), Vector.Abs(maximumError)); @@ -174,7 +174,8 @@ static void ComputeJacobians( normal.Z = Vector.ConditionalSelect(useMin, -normal.Z, normal.Z); //Note that the angular jacobian for A uses the offset from A to the attachment point on B. - Vector3Wide.CrossWithoutOverlap(anchorB, normal, out angularJA); + var offsetFromAToClosetPointOnPlaneToB = anchorB - planeNormalDot * normal; + Vector3Wide.CrossWithoutOverlap(offsetFromAToClosetPointOnPlaneToB, normal, out angularJA); Vector3Wide.CrossWithoutOverlap(normal, offsetB, out angularJB); } diff --git a/BepuPhysics/Constraints/LinearAxisMotor.cs b/BepuPhysics/Constraints/LinearAxisMotor.cs index 87e0167c8..3251cdd1f 100644 --- a/BepuPhysics/Constraints/LinearAxisMotor.cs +++ b/BepuPhysics/Constraints/LinearAxisMotor.cs @@ -116,7 +116,7 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref LinearAxisMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - LinearAxisServoFunctions.ComputeJacobians(positionB - positionA, orientationA, orientationB, prestep.LocalPlaneNormal, prestep.LocalOffsetB, out _, out var normal, out var angularJA, out var angularJB); + LinearAxisServoFunctions.ComputeJacobians(positionB - positionA, orientationA, orientationB, prestep.LocalPlaneNormal, prestep.LocalOffsetA, prestep.LocalOffsetB, out _, out var normal, out var angularJA, out var angularJB); Symmetric3x3Wide.TransformWithoutOverlap(angularJA, inertiaA.InverseInertiaTensor, out var angularImpulseToVelocityA); Symmetric3x3Wide.TransformWithoutOverlap(angularJB, inertiaB.InverseInertiaTensor, out var angularImpulseToVelocityB); LinearAxisServoFunctions.ApplyImpulse(normal, angularImpulseToVelocityA, angularImpulseToVelocityB, inertiaA, inertiaB, accumulatedImpulses, ref wsvA, ref wsvB); @@ -124,7 +124,7 @@ public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref LinearAxisMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - LinearAxisServoFunctions.ComputeJacobians(positionB - positionA, orientationA, orientationB, prestep.LocalPlaneNormal, prestep.LocalOffsetB, out _, out var normal, out var angularJA, out var angularJB); + LinearAxisServoFunctions.ComputeJacobians(positionB - positionA, orientationA, orientationB, prestep.LocalPlaneNormal, prestep.LocalOffsetA, prestep.LocalOffsetB, out _, out var normal, out var angularJA, out var angularJB); MotorSettingsWide.ComputeSoftness(prestep.Settings, dt, out var effectiveMassCFMScale, out var softnessImpulseScale, out var maximumImpulse); LinearAxisServoFunctions.ComputeEffectiveMass(angularJA, angularJB, inertiaA, inertiaB, effectiveMassCFMScale, out var angularImpulseToVelocityA, out var angularImpulseToVelocityB, out var effectiveMass); diff --git a/BepuPhysics/Constraints/LinearAxisServo.cs b/BepuPhysics/Constraints/LinearAxisServo.cs index aa44099dc..3cdadecff 100644 --- a/BepuPhysics/Constraints/LinearAxisServo.cs +++ b/BepuPhysics/Constraints/LinearAxisServo.cs @@ -238,15 +238,19 @@ public static void ApplyImpulse(in Vector3Wide linearJA, in Vector3Wide angularI } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ComputeJacobians(in Vector3Wide ab, in QuaternionWide orientationA, in QuaternionWide orientationB, in Vector3Wide localPlaneNormalA, in Vector3Wide localOffsetB, - out Vector3Wide anchorB, out Vector3Wide normal, out Vector3Wide angularJA, out Vector3Wide angularJB) + public static void ComputeJacobians(in Vector3Wide ab, in QuaternionWide orientationA, in QuaternionWide orientationB, in Vector3Wide localPlaneNormalA, in Vector3Wide localOffsetA, in Vector3Wide localOffsetB, + out Vector planeNormalDot, out Vector3Wide normal, out Vector3Wide angularJA, out Vector3Wide angularJB) { //Linear jacobians are just normal and -normal. Angular jacobians are offsetA x normal and offsetB x normal. - QuaternionWide.TransformWithoutOverlap(localPlaneNormalA, orientationA, out normal); + Matrix3x3Wide.CreateFromQuaternion(orientationA, out var orientationMatrixA); + Matrix3x3Wide.TransformWithoutOverlap(localPlaneNormalA, orientationMatrixA, out normal); + Matrix3x3Wide.TransformWithoutOverlap(localOffsetA, orientationMatrixA, out var anchorA); QuaternionWide.TransformWithoutOverlap(localOffsetB, orientationB, out var offsetB); //Note that the angular jacobian for A uses the offset from A to the attachment point on B. - anchorB = ab + offsetB; - Vector3Wide.CrossWithoutOverlap(anchorB, normal, out angularJA); + var anchorB = ab + offsetB; + Vector3Wide.Dot(anchorB - anchorA, normal, out planeNormalDot); + var offsetFromAToClosetPointOnPlaneToB = anchorB - planeNormalDot * normal; + Vector3Wide.CrossWithoutOverlap(offsetFromAToClosetPointOnPlaneToB, normal, out angularJA); Vector3Wide.CrossWithoutOverlap(normal, offsetB, out angularJB); } @@ -264,7 +268,7 @@ public static void ComputeEffectiveMass(in Vector3Wide angularJA, in Vector3Wide public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref LinearAxisServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - ComputeJacobians(positionB - positionA, orientationA, orientationB, prestep.LocalPlaneNormal, prestep.LocalOffsetB, out _, out var normal, out var angularJA, out var angularJB); + ComputeJacobians(positionB - positionA, orientationA, orientationB, prestep.LocalPlaneNormal, prestep.LocalOffsetA, prestep.LocalOffsetB, out _, out var normal, out var angularJA, out var angularJB); Symmetric3x3Wide.TransformWithoutOverlap(angularJA, inertiaA.InverseInertiaTensor, out var angularImpulseToVelocityA); Symmetric3x3Wide.TransformWithoutOverlap(angularJB, inertiaB.InverseInertiaTensor, out var angularImpulseToVelocityB); ApplyImpulse(normal, angularImpulseToVelocityA, angularImpulseToVelocityB, inertiaA, inertiaB, accumulatedImpulses, ref wsvA, ref wsvB); @@ -272,12 +276,10 @@ public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref LinearAxisServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - ComputeJacobians(positionB - positionA, orientationA, orientationB, prestep.LocalPlaneNormal, prestep.LocalOffsetB, out var anchorB, out var normal, out var angularJA, out var angularJB); + ComputeJacobians(positionB - positionA, orientationA, orientationB, prestep.LocalPlaneNormal, prestep.LocalOffsetA, prestep.LocalOffsetB, out var planeNormalDot, out var normal, out var angularJA, out var angularJB); SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); ComputeEffectiveMass(angularJA, angularJB, inertiaA, inertiaB, effectiveMassCFMScale, out var angularImpulseToVelocityA, out var angularImpulseToVelocityB, out var effectiveMass); - QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetA, orientationA, out var anchorA); - Vector3Wide.Dot(anchorB - anchorA, normal, out var planeNormalDot); //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. ServoSettingsWide.ComputeClampedBiasVelocity(planeNormalDot - prestep.TargetOffset, positionErrorToVelocity, prestep.ServoSettings, dt, inverseDt, out var biasVelocity, out var maximumImpulse); From 78d15fdf55392f2dba4606148fb89e2668be03b8 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 28 Sep 2021 14:43:16 -0500 Subject: [PATCH 173/947] Tightened AngularHinge accesses. --- BepuPhysics/Constraints/AngularHinge.cs | 2 +- BepuPhysics/Constraints/IBodyAccessFilter.cs | 10 ++++++++++ 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/BepuPhysics/Constraints/AngularHinge.cs b/BepuPhysics/Constraints/AngularHinge.cs index 427f56e06..f481aad15 100644 --- a/BepuPhysics/Constraints/AngularHinge.cs +++ b/BepuPhysics/Constraints/AngularHinge.cs @@ -341,7 +341,7 @@ public void UpdateForNewPose( } } - public class AngularHingeTypeProcessor : TwoBodyTypeProcessor + public class AngularHingeTypeProcessor : TwoBodyTypeProcessor { public const int BatchTypeId = 23; } diff --git a/BepuPhysics/Constraints/IBodyAccessFilter.cs b/BepuPhysics/Constraints/IBodyAccessFilter.cs index 858e342f1..d8c98b5e6 100644 --- a/BepuPhysics/Constraints/IBodyAccessFilter.cs +++ b/BepuPhysics/Constraints/IBodyAccessFilter.cs @@ -92,6 +92,16 @@ public struct AccessOnlyAngular : IBodyAccessFilter public bool AccessAngularVelocity => true; } + public struct AccessOnlyAngularWithoutPose : IBodyAccessFilter + { + public bool GatherPosition => false; + public bool GatherOrientation => false; + public bool GatherMass => false; + public bool GatherInertiaTensor => true; + public bool AccessLinearVelocity => false; + public bool AccessAngularVelocity => true; + } + public struct AccessOnlyLinear : IBodyAccessFilter { public bool GatherPosition => true; From 8f6f572bff9b60d0a82b4134dff3e7f32b9ce8be Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 28 Sep 2021 14:47:49 -0500 Subject: [PATCH 174/947] Tightened AngularAxisMotor accesses. --- BepuPhysics/Constraints/AngularAxisMotor.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/BepuPhysics/Constraints/AngularAxisMotor.cs b/BepuPhysics/Constraints/AngularAxisMotor.cs index 970d35d6c..59fe035a1 100644 --- a/BepuPhysics/Constraints/AngularAxisMotor.cs +++ b/BepuPhysics/Constraints/AngularAxisMotor.cs @@ -167,7 +167,7 @@ public void UpdateForNewPose( } } - public class AngularAxisMotorTypeProcessor : TwoBodyTypeProcessor, AngularAxisMotorFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular> + public class AngularAxisMotorTypeProcessor : TwoBodyTypeProcessor, AngularAxisMotorFunctions, AccessOnlyAngular, AccessOnlyAngularWithoutPose, AccessOnlyAngular, AccessOnlyAngular> { public const int BatchTypeId = 41; } From ec1bafc01cf88a4717a2113d68e18fd4bb9106f5 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 28 Sep 2021 15:03:07 -0500 Subject: [PATCH 175/947] Twoified Hinge. --- BepuPhysics/Constraints/Hinge.cs | 125 +++++++++++++++++++++++++++++-- BepuUtilities/Vector2Wide.cs | 21 +++++- 2 files changed, 139 insertions(+), 7 deletions(-) diff --git a/BepuPhysics/Constraints/Hinge.cs b/BepuPhysics/Constraints/Hinge.cs index b7b6d5109..3d25e91cf 100644 --- a/BepuPhysics/Constraints/Hinge.cs +++ b/BepuPhysics/Constraints/Hinge.cs @@ -164,7 +164,7 @@ public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, //Note the negation: we want to oppose the separation. TODO: arguably, should bake the negation into positionErrorToVelocity, given its name. Vector2Wide.Scale(errorAngles, -positionErrorToVelocity, out projection.HingeBiasVelocity); } - + [MethodImpl(MethodImplOptions.AggressiveInlining)] private static void ApplyImpulse(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref HingeProjection projection, ref HingeAccumulatedImpulses csi) { @@ -224,26 +224,141 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ApplyImpulse(ref velocityA, ref velocityB, ref projection, ref csi); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + private static void ApplyImpulse(in Vector3Wide offsetA, in Vector3Wide offsetB, in Matrix2x3Wide hingeJacobian, in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, in HingeAccumulatedImpulses csi, + ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB) + { + //[ csi ] * [ I, skew(offsetA), -I, -skew(offsetB) ] + // [ 0, constraintAxisAX, 0, -constraintAxisAX ] + // [ 0, constraintAxisAY, 0, -constraintAxisAY ] + Vector3Wide.Scale(csi.BallSocket, inertiaA.InverseMass, out var linearChangeA); + Vector3Wide.Add(velocityA.Linear, linearChangeA, out velocityA.Linear); + + Vector3Wide.CrossWithoutOverlap(offsetA, csi.BallSocket, out var ballSocketAngularImpulseA); + Matrix2x3Wide.Transform(csi.Hinge, hingeJacobian, out var hingeAngularImpulseA); + Vector3Wide.Add(ballSocketAngularImpulseA, hingeAngularImpulseA, out var angularImpulseA); + Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseA, inertiaA.InverseInertiaTensor, out var angularChangeA); + Vector3Wide.Add(velocityA.Angular, angularChangeA, out velocityA.Angular); + + //Note cross order flip for negation. + Vector3Wide.Scale(csi.BallSocket, inertiaB.InverseMass, out var negatedLinearChangeB); + Vector3Wide.Subtract(velocityB.Linear, negatedLinearChangeB, out velocityB.Linear); + Vector3Wide.CrossWithoutOverlap(csi.BallSocket, offsetB, out var ballSocketAngularImpulseB); + Vector3Wide.Subtract(ballSocketAngularImpulseB, hingeAngularImpulseA, out var angularImpulseB); + Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseB, inertiaB.InverseInertiaTensor, out var angularChangeB); + Vector3Wide.Add(velocityB.Angular, angularChangeB, out velocityB.Angular); + } + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref HingePrestepData prestep, ref HingeAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - throw new NotImplementedException(); + Matrix3x3Wide.CreateFromQuaternion(orientationA, out var orientationMatrixA); + Matrix3x3Wide.TransformWithoutOverlap(prestep.LocalOffsetA, orientationMatrixA, out var offsetA); + QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetB, orientationB, out var offsetB); + Helpers.BuildOrthonormalBasis(prestep.LocalHingeAxisA, out var localAX, out var localAY); + Matrix2x3Wide hingeJacobian; + Matrix3x3Wide.TransformWithoutOverlap(localAX, orientationMatrixA, out hingeJacobian.X); + Matrix3x3Wide.TransformWithoutOverlap(localAY, orientationMatrixA, out hingeJacobian.Y); + ApplyImpulse(offsetA, offsetB, hingeJacobian, inertiaA, inertiaB, accumulatedImpulses, ref wsvA, ref wsvB); } public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref HingePrestepData prestep, ref HingeAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - throw new NotImplementedException(); + //5x12 jacobians, from BallSocket and AngularHinge: + //[ I, skew(offsetA), -I, -skew(offsetB) ] + //[ 0, constraintAxisAX, 0, -constraintAxisAX ] + //[ 0, constraintAxisAY, 0, -constraintAxisAY ] + + Matrix3x3Wide.CreateFromQuaternion(orientationA, out var orientationMatrixA); + Matrix3x3Wide.CreateFromQuaternion(orientationB, out var orientationMatrixB); + Matrix3x3Wide.TransformWithoutOverlap(prestep.LocalOffsetA, orientationMatrixA, out var offsetA); + Matrix3x3Wide.TransformWithoutOverlap(prestep.LocalHingeAxisA, orientationMatrixA, out var hingeAxisA); + Matrix3x3Wide.TransformWithoutOverlap(prestep.LocalOffsetB, orientationMatrixB, out var offsetB); + Matrix3x3Wide.TransformWithoutOverlap(prestep.LocalHingeAxisB, orientationMatrixB, out var hingeAxisB); + Helpers.BuildOrthonormalBasis(prestep.LocalHingeAxisA, out var localAX, out var localAY); + Matrix2x3Wide hingeJacobian; + Matrix3x3Wide.TransformWithoutOverlap(localAX, orientationMatrixA, out hingeJacobian.X); + Matrix3x3Wide.TransformWithoutOverlap(localAY, orientationMatrixA, out hingeJacobian.Y); + + //The upper left 3x3 block is just the ball socket. + Symmetric3x3Wide.SkewSandwichWithoutOverlap(offsetA, inertiaA.InverseInertiaTensor, out var ballSocketContributionAngularA); + Symmetric3x3Wide.SkewSandwichWithoutOverlap(offsetB, inertiaB.InverseInertiaTensor, out var ballSocketContributionAngularB); + Symmetric5x5Wide inverseEffectiveMass; + Symmetric3x3Wide.Add(ballSocketContributionAngularA, ballSocketContributionAngularB, out inverseEffectiveMass.A); + var linearContribution = inertiaA.InverseMass + inertiaB.InverseMass; + inverseEffectiveMass.A.XX += linearContribution; + inverseEffectiveMass.A.YY += linearContribution; + inverseEffectiveMass.A.ZZ += linearContribution; + + //The lower right 2x2 block is the AngularHinge. + Symmetric3x3Wide.MultiplyWithoutOverlap(hingeJacobian, inertiaA.InverseInertiaTensor, out var hingeInertiaA); + Symmetric3x3Wide.MultiplyWithoutOverlap(hingeJacobian, inertiaB.InverseInertiaTensor, out var hingeInertiaB); + Symmetric2x2Wide.CompleteMatrixSandwich(hingeInertiaA, hingeJacobian, out var hingeContributionAngularA); + Symmetric2x2Wide.CompleteMatrixSandwich(hingeInertiaB, hingeJacobian, out var hingeContributionAngularB); + Symmetric2x2Wide.Add(hingeContributionAngularA, hingeContributionAngularB, out inverseEffectiveMass.D); + + //The remaining off-diagonal region is skew(offsetA) * Ia^-1 * hingeJacobianV + skew(offsetB) * Ib^-1 * hingeJacobianA + //skew(offsetA) * (Ia^-1 * hingeJacobianA) = [ (Ia^-1 * hingeJacobianA.X) x offsetA ] + // [ (Ia^-1 * hingeJacobianA.Y) x offsetA ] + //Careful with cross order/signs! + Vector3Wide.CrossWithoutOverlap(hingeInertiaA.X, offsetA, out var offDiagonalContributionAX); + Vector3Wide.CrossWithoutOverlap(hingeInertiaA.Y, offsetA, out var offDiagonalContributionAY); + Vector3Wide.CrossWithoutOverlap(hingeInertiaB.X, offsetB, out var offDiagonalContributionBX); + Vector3Wide.CrossWithoutOverlap(hingeInertiaB.Y, offsetB, out var offDiagonalContributionBY); + Vector3Wide.Add(offDiagonalContributionAX, offDiagonalContributionBX, out inverseEffectiveMass.B.X); + Vector3Wide.Add(offDiagonalContributionAY, offDiagonalContributionBY, out inverseEffectiveMass.B.Y); + + Symmetric5x5Wide.InvertWithoutOverlap(inverseEffectiveMass, out var effectiveMass); + SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); + //Note that the effective mass is *not* scaled by the effectiveMassCFMScale here; instead, we scale the impulse later. + + //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. + Vector3Wide.Add(positionB - positionA, offsetB, out var anchorB); + Vector3Wide.Subtract(anchorB, offsetA, out var ballSocketError); + Vector3Wide.Scale(ballSocketError, positionErrorToVelocity, out var ballSocketBiasVelocity); + + AngularHingeFunctions.GetErrorAngles(hingeAxisA, hingeAxisB, hingeJacobian, out var errorAngles); + //Note the negation: we want to oppose the separation. TODO: arguably, should bake the negation into positionErrorToVelocity, given its name. + Vector2Wide.Scale(errorAngles, -positionErrorToVelocity, out var hingeBiasVelocity); + + //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); + // [ I, skew(offsetA), -I, -skew(offsetB) ] + //J = [ 0, constraintAxisAX, 0, -constraintAxisAX ] + // [ 0, constraintAxisAY, 0, -constraintAxisAY ] + Vector3Wide.CrossWithoutOverlap(wsvA.Angular, offsetA, out var ballSocketAngularCSVA); + Matrix2x3Wide.TransformByTransposeWithoutOverlap(wsvA.Angular, hingeJacobian, out var hingeCSVA); + Vector3Wide.CrossWithoutOverlap(offsetB, wsvB.Angular, out var ballSocketAngularCSVB); + Matrix2x3Wide.TransformByTransposeWithoutOverlap(wsvB.Angular, hingeJacobian, out var negatedHingeCSVB); + + Vector3Wide.Add(ballSocketAngularCSVA, ballSocketAngularCSVB, out var ballSocketAngularCSV); + Vector3Wide.Subtract(wsvA.Linear, wsvB.Linear, out var ballSocketLinearCSV); + Vector3Wide.Add(ballSocketAngularCSV, ballSocketLinearCSV, out var ballSocketCSV); + Vector3Wide.Subtract(ballSocketBiasVelocity, ballSocketCSV, out ballSocketCSV); + Vector2Wide.Subtract(hingeCSVA, negatedHingeCSVB, out var hingeCSV); + Vector2Wide.Subtract(hingeBiasVelocity, hingeCSV, out hingeCSV); + + HingeAccumulatedImpulses csi; + Symmetric5x5Wide.TransformWithoutOverlap(ballSocketCSV, hingeCSV, effectiveMass, out csi.BallSocket, out csi.Hinge); + csi.BallSocket *= effectiveMassCFMScale; + csi.Hinge *= effectiveMassCFMScale; + Vector3Wide.Scale(accumulatedImpulses.BallSocket, softnessImpulseScale, out var ballSocketSoftnessContribution); + Vector3Wide.Subtract(csi.BallSocket, ballSocketSoftnessContribution, out csi.BallSocket); + Vector2Wide.Scale(accumulatedImpulses.Hinge, softnessImpulseScale, out var hingeSoftnessContribution); + Vector2Wide.Subtract(csi.Hinge, hingeSoftnessContribution, out csi.Hinge); + + ApplyImpulse(offsetA, offsetB, hingeJacobian, inertiaA, inertiaB, csi, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector dt, in HingeAccumulatedImpulses accumulatedImpulses, ref HingePrestepData prestep) { } } - public class HingeTypeProcessor : TwoBodyTypeProcessor + public class HingeTypeProcessor : TwoBodyTypeProcessor { public const int BatchTypeId = 47; } diff --git a/BepuUtilities/Vector2Wide.cs b/BepuUtilities/Vector2Wide.cs index a0de2b2c4..40d44128a 100644 --- a/BepuUtilities/Vector2Wide.cs +++ b/BepuUtilities/Vector2Wide.cs @@ -21,7 +21,7 @@ public static void Subtract(in Vector2Wide a, in Vector2Wide b, out Vector2Wide { result.X = a.X - b.X; result.Y = a.Y - b.Y; - } + } [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Dot(in Vector2Wide a, in Vector2Wide b, out Vector result) @@ -36,6 +36,23 @@ public static void Scale(in Vector2Wide vector, in Vector scalar, out Vec result.Y = vector.Y * scalar; } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector2Wide operator *(Vector2Wide vector, Vector scalar) + { + Vector2Wide result; + result.X = vector.X * scalar; + result.Y = vector.Y * scalar; + return result; + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector2Wide operator *(Vector scalar, Vector2Wide vector) + { + Vector2Wide result; + result.X = vector.X * scalar; + result.Y = vector.Y * scalar; + return result; + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Negate(in Vector2Wide v, out Vector2Wide result) { @@ -142,6 +159,6 @@ public override string ToString() { return $"<{X}, {Y}>"; } - + } } From a3f5d8616f117a686295521d01e127c87bdda998 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 28 Sep 2021 15:45:20 -0500 Subject: [PATCH 176/947] AngularHinge fiddle. --- BepuPhysics/Constraints/AngularHinge.cs | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/BepuPhysics/Constraints/AngularHinge.cs b/BepuPhysics/Constraints/AngularHinge.cs index f481aad15..6978d6cdb 100644 --- a/BepuPhysics/Constraints/AngularHinge.cs +++ b/BepuPhysics/Constraints/AngularHinge.cs @@ -312,16 +312,17 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in Symmetric2x2Wide.InvertWithoutOverlap(inverseEffectiveMass, out var effectiveMass); SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); - Symmetric2x2Wide.Scale(effectiveMass, effectiveMassCFMScale, out effectiveMass); - Symmetric2x2Wide.MultiplyTransposed(jacobianA, effectiveMass, out var velocityToImpulseA); + //Note the effective mass is not scaled directly. Instead, we scale the csi to save a multiply. GetErrorAngles(hingeAxisA, hingeAxisB, jacobianA, out var errorAngle); //Note the negation: we want to oppose the separation. TODO: arguably, should bake the negation into positionErrorToVelocity, given its name. Vector2Wide.Scale(errorAngle, -positionErrorToVelocity, out var biasVelocity); Symmetric2x2Wide.TransformWithoutOverlap(biasVelocity, effectiveMass, out var biasImpulse); - //JB = -JA. This is (angularVelocityA * JA + angularVelocityB * JB) * effectiveMass => (angularVelocityA - angularVelocityB) * (JA * effectiveMass) + //JB = -JA. This is (angularVelocityA * JA + angularVelocityB * JB) * effectiveMass Vector3Wide.Subtract(wsvA.Angular, wsvB.Angular, out var difference); - Matrix2x3Wide.TransformByTransposeWithoutOverlap(difference, velocityToImpulseA, out var csi); + Matrix2x3Wide.TransformByTransposeWithoutOverlap(difference, jacobianA, out var csv); + Symmetric2x2Wide.TransformWithoutOverlap(csv, effectiveMass, out var csi); + Vector2Wide.Scale(csi, effectiveMassCFMScale, out csi); //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); Vector2Wide.Scale(accumulatedImpulses, softnessImpulseScale, out var softnessContribution); Vector2Wide.Add(softnessContribution, csi, out csi); From fccf86b66b376fe8dadcd66675f426abdf3baf39 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 28 Sep 2021 16:08:05 -0500 Subject: [PATCH 177/947] AngularSwivelHinge and SwingLimit twoified. --- BepuPhysics/Constraints/AngularSwivelHinge.cs | 68 +++++++++++++++++- BepuPhysics/Constraints/Hinge.cs | 1 + BepuPhysics/Constraints/SwingLimit.cs | 69 +++++++++++++++++- Demos/SpecializedTests/ConstraintTestDemo.cs | 72 +++++++++---------- 4 files changed, 168 insertions(+), 42 deletions(-) diff --git a/BepuPhysics/Constraints/AngularSwivelHinge.cs b/BepuPhysics/Constraints/AngularSwivelHinge.cs index a59fd6b5d..ee1492257 100644 --- a/BepuPhysics/Constraints/AngularSwivelHinge.cs +++ b/BepuPhysics/Constraints/AngularSwivelHinge.cs @@ -154,19 +154,81 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, ref projection, ref csi); } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + private static void ApplyImpulse(in Vector3Wide impulseToVelocityA, in Vector3Wide negatedImpulseToVelocityB, in Vector csi, ref Vector3Wide angularVelocityA, ref Vector3Wide angularVelocityB) + { + Vector3Wide.Scale(impulseToVelocityA, csi, out var velocityChangeA); + Vector3Wide.Add(angularVelocityA, velocityChangeA, out angularVelocityA); + Vector3Wide.Scale(negatedImpulseToVelocityB, csi, out var negatedVelocityChangeB); + Vector3Wide.Subtract(angularVelocityB, negatedVelocityChangeB, out angularVelocityB); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + static void ComputeJacobian(in Vector3Wide localSwivelAxisA, in Vector3Wide localHingeAxisB, in QuaternionWide orientationA, in QuaternionWide orientationB, out Vector3Wide swivelAxis, out Vector3Wide hingeAxis, out Vector3Wide jacobianA) + { + QuaternionWide.TransformWithoutOverlap(localSwivelAxisA, orientationA, out swivelAxis); + QuaternionWide.TransformWithoutOverlap(localHingeAxisB, orientationB, out hingeAxis); + Vector3Wide.CrossWithoutOverlap(swivelAxis, hingeAxis, out jacobianA); + //In the event that the axes are parallel, there is no unique jacobian. Arbitrarily pick one. + //Note that this causes a discontinuity in jacobian length at the poles. We just don't worry about it. + Helpers.FindPerpendicular(swivelAxis, out var fallbackJacobian); + Vector3Wide.Dot(jacobianA, jacobianA, out var jacobianLengthSquared); + var useFallback = Vector.LessThan(jacobianLengthSquared, new Vector(1e-7f)); + Vector3Wide.ConditionalSelect(useFallback, fallbackJacobian, jacobianA, out jacobianA); + } + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref AngularSwivelHingePrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - throw new NotImplementedException(); + ComputeJacobian(prestep.LocalSwivelAxisA, prestep.LocalHingeAxisB, orientationA, orientationB, out _, out _, out var jacobianA); + Symmetric3x3Wide.TransformWithoutOverlap(jacobianA, inertiaA.InverseInertiaTensor, out var impulseToVelocityA); + Symmetric3x3Wide.TransformWithoutOverlap(jacobianA, inertiaB.InverseInertiaTensor, out var negatedImpulseToVelocityB); + ApplyImpulse(impulseToVelocityA, negatedImpulseToVelocityB, accumulatedImpulses, ref wsvA.Angular, ref wsvB.Angular); } public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref AngularSwivelHingePrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - throw new NotImplementedException(); + //The swivel hinge attempts to keep an axis on body A separated 90 degrees from an axis on body B. In other words, this is the same as a hinge joint, but with one fewer DOF. + //C = dot(swivelA, hingeB) = 0 + //C' = dot(d/dt(swivelA), hingeB) + dot(swivelA, d/dt(hingeB)) = 0 + //C' = dot(angularVelocityB x hingeB, swivelA) + dot(hingeB, angularVelocityA x swivelA) = 0 + //C' = dot(hingeB x swivelA, angularVelocityB) + dot(angularVelocityA, swivelA x hingeB) = 0 + //Providing jacobians of: + //JA = swivelA x hingeB + //JB = hingeB x swivelA + //a x b == -b x a, so JB == -JA. + + ComputeJacobian(prestep.LocalSwivelAxisA, prestep.LocalHingeAxisB, orientationA, orientationB, out var swivelAxis, out var hingeAxis, out var jacobianA); + + //Note that JA = -JB, but for the purposes of calculating the effective mass the sign is irrelevant. + + //This computes the effective mass using the usual (J * M^-1 * JT)^-1 formulation, but we actually make use of the intermediate result J * M^-1 so we compute it directly. + Symmetric3x3Wide.TransformWithoutOverlap(jacobianA, inertiaA.InverseInertiaTensor, out var impulseToVelocityA); + //Note that we don't use -jacobianA here, so we're actually storing out the negated version of the transform. That's fine; we'll simply subtract in the iteration. + Symmetric3x3Wide.TransformWithoutOverlap(jacobianA, inertiaB.InverseInertiaTensor, out var negatedImpulseToVelocityB); + Vector3Wide.Dot(impulseToVelocityA, jacobianA, out var angularA); + Vector3Wide.Dot(negatedImpulseToVelocityB, jacobianA, out var angularB); + + SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); + var effectiveMass = effectiveMassCFMScale / (angularA + angularB); + + Vector3Wide.Dot(hingeAxis, swivelAxis, out var error); + //Note the negation: we want to oppose the separation. + var biasVelocity = -(positionErrorToVelocity * error); + + //JB = -JA. This is (angularVelocityA * JA + angularVelocityB * JB) * effectiveMass => (angularVelocityA - angularVelocityB) * (JA * effectiveMass) + Vector3Wide.Subtract(wsvA.Angular, wsvB.Angular, out var difference); + Vector3Wide.Dot(difference, jacobianA, out var csv); + //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); + var csi = effectiveMass * (biasVelocity - csv) - accumulatedImpulses * softnessImpulseScale; + + accumulatedImpulses += csi; + ApplyImpulse(impulseToVelocityA, negatedImpulseToVelocityB, csi, ref wsvA.Angular, ref wsvB.Angular); + } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector dt, in Vector accumulatedImpulses, ref AngularSwivelHingePrestepData prestep) { diff --git a/BepuPhysics/Constraints/Hinge.cs b/BepuPhysics/Constraints/Hinge.cs index 3d25e91cf..1ccfb2f28 100644 --- a/BepuPhysics/Constraints/Hinge.cs +++ b/BepuPhysics/Constraints/Hinge.cs @@ -308,6 +308,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in Vector3Wide.Add(offDiagonalContributionAX, offDiagonalContributionBX, out inverseEffectiveMass.B.X); Vector3Wide.Add(offDiagonalContributionAY, offDiagonalContributionBY, out inverseEffectiveMass.B.Y); + //TODO: Could consider an LDLT solve here. Helped a little bit in Weld; probably would still be worth it for a 5x5. Symmetric5x5Wide.InvertWithoutOverlap(inverseEffectiveMass, out var effectiveMass); SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); //Note that the effective mass is *not* scaled by the effectiveMassCFMScale here; instead, we scale the impulse later. diff --git a/BepuPhysics/Constraints/SwingLimit.cs b/BepuPhysics/Constraints/SwingLimit.cs index 42bd4c786..ec9f5fd1a 100644 --- a/BepuPhysics/Constraints/SwingLimit.cs +++ b/BepuPhysics/Constraints/SwingLimit.cs @@ -179,19 +179,82 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, ref projection, ref csi); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + private static void ApplyImpulse(in Vector3Wide impulseToVelocityA, in Vector3Wide negatedImpulseToVelocityB, in Vector csi, ref Vector3Wide angularVelocityA, ref Vector3Wide angularVelocityB) + { + Vector3Wide.Scale(impulseToVelocityA, csi, out var velocityChangeA); + Vector3Wide.Add(angularVelocityA, velocityChangeA, out angularVelocityA); + Vector3Wide.Scale(negatedImpulseToVelocityB, csi, out var negatedVelocityChangeB); + Vector3Wide.Subtract(angularVelocityB, negatedVelocityChangeB, out angularVelocityB); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + static void ComputeJacobian(in Vector3Wide axisLocalA, in Vector3Wide axisLocalB, in QuaternionWide orientationA, in QuaternionWide orientationB, out Vector3Wide axisA, out Vector3Wide axisB, out Vector3Wide jacobianA) + { + QuaternionWide.TransformWithoutOverlap(axisLocalA, orientationA, out axisA); + QuaternionWide.TransformWithoutOverlap(axisLocalB, orientationB, out axisB); + Vector3Wide.CrossWithoutOverlap(axisA, axisB, out jacobianA); + //In the event that the axes are parallel, there is no unique jacobian. Arbitrarily pick one. + //Note that this causes a discontinuity in jacobian length at the poles. We just don't worry about it. + Helpers.FindPerpendicular(axisA, out var fallbackJacobian); + Vector3Wide.Dot(jacobianA, jacobianA, out var jacobianLengthSquared); + var useFallback = Vector.LessThan(jacobianLengthSquared, new Vector(1e-7f)); + Vector3Wide.ConditionalSelect(useFallback, fallbackJacobian, jacobianA, out jacobianA); + } public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref SwingLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - throw new NotImplementedException(); + ComputeJacobian(prestep.AxisLocalA, prestep.AxisLocalB, orientationA, orientationB, out _, out _, out var jacobianA); + Symmetric3x3Wide.TransformWithoutOverlap(jacobianA, inertiaA.InverseInertiaTensor, out var impulseToVelocityA); + Symmetric3x3Wide.TransformWithoutOverlap(jacobianA, inertiaB.InverseInertiaTensor, out var negatedImpulseToVelocityB); + ApplyImpulse(impulseToVelocityA, negatedImpulseToVelocityB, accumulatedImpulses, ref wsvA.Angular, ref wsvB.Angular); } public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref SwingLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - throw new NotImplementedException(); + //The swing limit attempts to keep an axis on body A within from an axis on body B. In other words, this is the same as a hinge joint, but with one fewer DOF. + //(Note that the jacobians are extremely similar to the AngularSwivelHinge; the difference is that this is a speculative inequality constraint.) + //C = dot(axisA, axisB) >= MinimumDot + //C' = dot(d/dt(axisA), axisB) + dot(axisA, d/dt(axisB)) >= 0 + //C' = dot(angularVelocityA x axisA, axisB) + dot(axisA, angularVelocityB x axisB) >= 0 + //C' = dot(axisA x axisB, angularVelocityA) + dot(angularVelocityB, axisB x axisA) >= 0 + //Providing jacobians of: + //JA = axisA x axisB + //JB = axisB x axisA + //a x b == -b x a, so JB == -JA. + + ComputeJacobian(prestep.AxisLocalA, prestep.AxisLocalB, orientationA, orientationB, out var axisA, out var axisB, out var jacobianA); + + //Note that JA = -JB, but for the purposes of calculating the effective mass the sign is irrelevant. + + //This computes the effective mass using the usual (J * M^-1 * JT)^-1 formulation, but we actually make use of the intermediate result J * M^-1 so we compute it directly. + Symmetric3x3Wide.TransformWithoutOverlap(jacobianA, inertiaA.InverseInertiaTensor, out var impulseToVelocityA); + //Note that we don't use -jacobianA here, so we're actually storing out the negated version of the transform. That's fine; we'll simply subtract in the iteration. + Symmetric3x3Wide.TransformWithoutOverlap(jacobianA, inertiaB.InverseInertiaTensor, out var negatedImpulseToVelocityB); + Vector3Wide.Dot(impulseToVelocityA, jacobianA, out var angularContributionA); + Vector3Wide.Dot(negatedImpulseToVelocityB, jacobianA, out var angularContributionB); + + SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); + var effectiveMass = effectiveMassCFMScale / (angularContributionA + angularContributionB); + + Vector3Wide.Dot(axisA, axisB, out var axisDot); + var error = axisDot - prestep.MinimumDot; + //Note the negation: we want to oppose the separation. + var biasVelocity = -Vector.Min(error * new Vector(inverseDt), error * positionErrorToVelocity); + + //JB = -JA. This is (angularVelocityA * JA + angularVelocityB * JB) * effectiveMass => (angularVelocityA - angularVelocityB) * (JA * effectiveMass) + Vector3Wide.Subtract(wsvA.Angular, wsvB.Angular, out var difference); + Vector3Wide.Dot(difference, jacobianA, out var csv); + //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); + var csi = effectiveMass * (biasVelocity - csv) - accumulatedImpulses * softnessImpulseScale; + + InequalityHelpers.ClampPositive(ref accumulatedImpulses, ref csi); + ApplyImpulse(impulseToVelocityA, negatedImpulseToVelocityB, csi, ref wsvA.Angular, ref wsvB.Angular); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector dt, in Vector accumulatedImpulses, ref SwingLimitPrestepData prestep) { diff --git a/Demos/SpecializedTests/ConstraintTestDemo.cs b/Demos/SpecializedTests/ConstraintTestDemo.cs index 5909a8548..8a3ddbf9e 100644 --- a/Demos/SpecializedTests/ConstraintTestDemo.cs +++ b/Demos/SpecializedTests/ConstraintTestDemo.cs @@ -22,7 +22,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(25, 4, 40); camera.Yaw = 0; Simulation = Simulation.Create(BufferPool, - new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); + new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 2); //new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); var shapeA = new Box(.75f, 1, .5f); @@ -39,7 +39,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var x = GetNextPosition(ref nextX); var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(1, 1) }); } { var x = GetNextPosition(ref nextX); @@ -48,27 +48,27 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); Simulation.Solver.Add(a, b, new AngularHinge { LocalHingeAxisA = new Vector3(0, 1, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); } - //{ - // var x = GetNextPosition(ref nextX); - // var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); - // var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - // Simulation.Solver.Add(a, b, new Hinge - // { - // LocalOffsetA = new Vector3(0, 1, 0), - // LocalHingeAxisA = new Vector3(0, 1, 0), - // LocalOffsetB = new Vector3(0, -1, 0), - // LocalHingeAxisB = new Vector3(0, 1, 0), - // SpringSettings = new SpringSettings(30, 1) - // }); - //} - //{ - // var x = GetNextPosition(ref nextX); - // var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); - // var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - // Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - // Simulation.Solver.Add(a, b, new AngularSwivelHinge { LocalSwivelAxisA = new Vector3(1, 0, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); - // Simulation.Solver.Add(a, b, new SwingLimit { AxisLocalA = new Vector3(0, 1, 0), AxisLocalB = new Vector3(0, 1, 0), MaximumSwingAngle = MathHelper.PiOver2, SpringSettings = new SpringSettings(30, 1) }); - //} + { + var x = GetNextPosition(ref nextX); + var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); + var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + Simulation.Solver.Add(a, b, new Hinge + { + LocalOffsetA = new Vector3(0, 1, 0), + LocalHingeAxisA = new Vector3(0, 1, 0), + LocalOffsetB = new Vector3(0, -1, 0), + LocalHingeAxisB = new Vector3(0, 1, 0), + SpringSettings = new SpringSettings(30, 1) + }); + } + { + var x = GetNextPosition(ref nextX); + var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); + var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + Simulation.Solver.Add(a, b, new AngularSwivelHinge { LocalSwivelAxisA = new Vector3(1, 0, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); + Simulation.Solver.Add(a, b, new SwingLimit { AxisLocalA = new Vector3(0, 1, 0), AxisLocalB = new Vector3(0, 1, 0), MaximumSwingAngle = MathHelper.PiOver2, SpringSettings = new SpringSettings(30, 1) }); + } //{ // var x = GetNextPosition(ref nextX); // var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); @@ -354,19 +354,19 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) // Settings = new MotorSettings(float.MaxValue, 0.001f), // }); //} - //{ - // var x = GetNextPosition(ref nextX); - // var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); - // var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); - // var a = Simulation.Bodies.Add(aDescription); - // var b = Simulation.Bodies.Add(bDescription); - // Simulation.Solver.Add(a, b, new BallSocketMotor - // { - // LocalOffsetB = new Vector3(0, -1, 0), - // TargetVelocityLocalA = new Vector3(0, -0.25f, 0), - // Settings = new MotorSettings(10, 1e-4f) - // }); - //} + { + var x = GetNextPosition(ref nextX); + var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); + var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); + var a = Simulation.Bodies.Add(aDescription); + var b = Simulation.Bodies.Add(bDescription); + Simulation.Solver.Add(a, b, new BallSocketMotor + { + LocalOffsetB = new Vector3(0, -1, 0), + TargetVelocityLocalA = new Vector3(0, -0.25f, 0), + Settings = new MotorSettings(10, 1e-4f) + }); + } //{ // var x = GetNextPosition(ref nextX); // var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); From 5c7a5b99d99efe787cd8beade5c75e25252ee74a Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 28 Sep 2021 16:29:25 -0500 Subject: [PATCH 178/947] SwivelHinge twoified. --- BepuPhysics/Constraints/SwivelHinge.cs | 120 +++++++++++++++++++++++-- 1 file changed, 115 insertions(+), 5 deletions(-) diff --git a/BepuPhysics/Constraints/SwivelHinge.cs b/BepuPhysics/Constraints/SwivelHinge.cs index 6d7645753..ba84ddd94 100644 --- a/BepuPhysics/Constraints/SwivelHinge.cs +++ b/BepuPhysics/Constraints/SwivelHinge.cs @@ -211,26 +211,136 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ApplyImpulse(ref velocityA, ref velocityB, ref projection, ref csi); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + private static void ApplyImpulse(in Vector3Wide offsetA, in Vector3Wide offsetB, in Vector3Wide swivelHingeJacobian, in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, ref Vector4Wide csi, ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB) + { + //[ csi ] * [ I, skew(offsetA), -I, -skew(offsetB) ] + // [ 0, swivelA x hingeB, 0, -swivelA x hingeB ] + ref var ballSocketCSI = ref Unsafe.As, Vector3Wide>(ref csi.X); + Vector3Wide.Scale(ballSocketCSI, inertiaA.InverseMass, out var linearChangeA); + Vector3Wide.Add(velocityA.Linear, linearChangeA, out velocityA.Linear); + + Vector3Wide.CrossWithoutOverlap(offsetA, ballSocketCSI, out var ballSocketAngularImpulseA); + Vector3Wide.Scale(swivelHingeJacobian, csi.W, out var swivelHingeAngularImpulseA); + Vector3Wide.Add(ballSocketAngularImpulseA, swivelHingeAngularImpulseA, out var angularImpulseA); + Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseA, inertiaA.InverseInertiaTensor, out var angularChangeA); + Vector3Wide.Add(velocityA.Angular, angularChangeA, out velocityA.Angular); + + //Note cross order flip for negation. + Vector3Wide.Scale(ballSocketCSI, inertiaB.InverseMass, out var negatedLinearChangeB); + Vector3Wide.Subtract(velocityB.Linear, negatedLinearChangeB, out velocityB.Linear); + Vector3Wide.CrossWithoutOverlap(ballSocketCSI, offsetB, out var ballSocketAngularImpulseB); + Vector3Wide.Subtract(ballSocketAngularImpulseB, swivelHingeAngularImpulseA, out var angularImpulseB); + Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseB, inertiaB.InverseInertiaTensor, out var angularChangeB); + Vector3Wide.Add(velocityB.Angular, angularChangeB, out velocityB.Angular); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + private static void ComputeJacobian(in Vector3Wide localOffsetA, in Vector3Wide localSwivelAxisA, in Vector3Wide localOffsetB, in Vector3Wide localHingeAxisB, in QuaternionWide orientationA, in QuaternionWide orientationB, + out Vector3Wide swivelAxis, out Vector3Wide hingeAxis, out Vector3Wide offsetA, out Vector3Wide offsetB, out Vector3Wide swivelHingeJacobian) + { + Matrix3x3Wide.CreateFromQuaternion(orientationA, out var orientationMatrixA); + Matrix3x3Wide.CreateFromQuaternion(orientationB, out var orientationMatrixB); + Matrix3x3Wide.TransformWithoutOverlap(localOffsetA, orientationMatrixA, out offsetA); + Matrix3x3Wide.TransformWithoutOverlap(localSwivelAxisA, orientationMatrixA, out swivelAxis); + Matrix3x3Wide.TransformWithoutOverlap(localOffsetB, orientationMatrixB, out offsetB); + Matrix3x3Wide.TransformWithoutOverlap(localHingeAxisB, orientationMatrixB, out hingeAxis); + Vector3Wide.CrossWithoutOverlap(swivelAxis, hingeAxis, out swivelHingeJacobian); + } + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref SwivelHingePrestepData prestep, ref Vector4Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - throw new NotImplementedException(); + ComputeJacobian(prestep.LocalOffsetA, prestep.LocalSwivelAxisA, prestep.LocalOffsetB, prestep.LocalHingeAxisB, orientationA, orientationB, + out _, out _, out var offsetA, out var offsetB, out var swivelHingeJacobian); + ApplyImpulse(offsetA, offsetB, swivelHingeJacobian, inertiaA, inertiaB, ref accumulatedImpulses, ref wsvA, ref wsvB); } public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref SwivelHingePrestepData prestep, ref Vector4Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - throw new NotImplementedException(); + //4x12 jacobians, from BallSocket and AngularSwivelHinge: + //[ I, skew(offsetA), -I, -skew(offsetB) ] + //[ 0, swivelA x hingeB, 0, -swivelA x hingeB ] + + ComputeJacobian(prestep.LocalOffsetA, prestep.LocalSwivelAxisA, prestep.LocalOffsetB, prestep.LocalHingeAxisB, orientationA, orientationB, + out var swivelAxis, out var hingeAxis, out var offsetA, out var offsetB, out var swivelHingeJacobian); + + //The upper left 3x3 block is just the ball socket. + Symmetric3x3Wide.SkewSandwichWithoutOverlap(offsetA, inertiaA.InverseInertiaTensor, out var ballSocketContributionAngularA); + Symmetric3x3Wide.SkewSandwichWithoutOverlap(offsetB, inertiaB.InverseInertiaTensor, out var ballSocketContributionAngularB); + Unsafe.SkipInit(out Symmetric4x4Wide inverseEffectiveMass); + ref var upperLeft = ref Symmetric4x4Wide.GetUpperLeft3x3Block(ref inverseEffectiveMass); + Symmetric3x3Wide.Add(ballSocketContributionAngularA, ballSocketContributionAngularB, out upperLeft); + var linearContribution = inertiaA.InverseMass + inertiaB.InverseMass; + upperLeft.XX += linearContribution; + upperLeft.YY += linearContribution; + upperLeft.ZZ += linearContribution; + + //The lower right 1x1 block is the AngularSwivelHinge. + Symmetric3x3Wide.TransformWithoutOverlap(swivelHingeJacobian, inertiaA.InverseInertiaTensor, out var swivelHingeInertiaA); + Symmetric3x3Wide.TransformWithoutOverlap(swivelHingeJacobian, inertiaB.InverseInertiaTensor, out var swivelHingeInertiaB); + Vector3Wide.Dot(swivelHingeInertiaA, swivelHingeJacobian, out var swivelHingeContributionAngularA); + Vector3Wide.Dot(swivelHingeInertiaB, swivelHingeJacobian, out var swivelHingeContributionAngularB); + inverseEffectiveMass.WW = swivelHingeContributionAngularA + swivelHingeContributionAngularB; + + //The remaining off-diagonal region is skew(offsetA) * Ia^-1 * (swivelAxis x hingeAxis) + skew(offsetB) * Ib^-1 * (swivelAxis x hingeAxis) + //skew(offsetA) * (Ia^-1 * (swivelAxis x hingeAxis) = (Ia^-1 * (swivelAxis x hingeAxis)) x offsetA + //Careful with cross order/signs! + Vector3Wide.CrossWithoutOverlap(swivelHingeInertiaA, offsetA, out var offDiagonalContributionA); + Vector3Wide.CrossWithoutOverlap(swivelHingeInertiaB, offsetB, out var offDiagonalContributionB); + Vector3Wide.Add(offDiagonalContributionA, offDiagonalContributionB, out Symmetric4x4Wide.GetUpperRight3x1Block(ref inverseEffectiveMass)); + + //TODO: May benefit from LDLT. Weld (6x6) does. + Symmetric4x4Wide.InvertWithoutOverlap(inverseEffectiveMass, out var effectiveMass); + SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); + //Note that we do not directly scale the effective mass; instead we just scale the CSI; it's smaller. + + //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. + Vector3Wide.Add(positionB - positionA, offsetB, out var anchorB); + Vector3Wide.Subtract(anchorB, offsetA, out var ballSocketError); + Vector4Wide biasVelocity; + biasVelocity.X = ballSocketError.X * positionErrorToVelocity; + biasVelocity.Y = ballSocketError.Y * positionErrorToVelocity; + biasVelocity.Z = ballSocketError.Z * positionErrorToVelocity; + + Vector3Wide.Dot(hingeAxis, swivelAxis, out var error); + //Note the negation: we want to oppose the separation. TODO: arguably, should bake the negation into positionErrorToVelocity, given its name. + biasVelocity.W = positionErrorToVelocity * -error; + //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); + //[ csi ] * [ I, skew(offsetA), -I, -skew(offsetB) ] + // [ 0, swivelA x hingeB, 0, -swivelA x hingeB ] + Vector3Wide.CrossWithoutOverlap(wsvA.Angular, offsetA, out var ballSocketAngularCSVA); + Vector3Wide.Dot(swivelHingeJacobian, wsvA.Angular, out var swivelHingeCSVA); + Vector3Wide.CrossWithoutOverlap(offsetB, wsvB.Angular, out var ballSocketAngularCSVB); + Vector3Wide.Dot(swivelHingeJacobian, wsvB.Angular, out var negatedSwivelHingeCSVB); + + Vector3Wide.Add(ballSocketAngularCSVA, ballSocketAngularCSVB, out var ballSocketAngularCSV); + Vector3Wide.Subtract(wsvA.Linear, wsvB.Linear, out var ballSocketLinearCSV); + Vector4Wide csv; + csv.X = ballSocketAngularCSV.X + ballSocketLinearCSV.X; + csv.Y = ballSocketAngularCSV.Y + ballSocketLinearCSV.Y; + csv.Z = ballSocketAngularCSV.Z + ballSocketLinearCSV.Z; + csv.W = swivelHingeCSVA - negatedSwivelHingeCSVB; + Vector4Wide.Subtract(biasVelocity, csv, out csv); + + Symmetric4x4Wide.TransformWithoutOverlap(csv, effectiveMass, out var csi); + Vector4Wide.Scale(csi, effectiveMassCFMScale, out csi); + Vector4Wide.Scale(accumulatedImpulses, softnessImpulseScale, out var softnessContribution); + Vector4Wide.Subtract(csi, softnessContribution, out csi); + + ApplyImpulse(offsetA, offsetB, swivelHingeJacobian, inertiaA, inertiaB, ref csi, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector dt, in Vector4Wide accumulatedImpulses, ref SwivelHingePrestepData prestep) { } } - public class SwivelHingeTypeProcessor : TwoBodyTypeProcessor + public class SwivelHingeTypeProcessor : TwoBodyTypeProcessor { public const int BatchTypeId = 46; } From d3e781b6e42a89d76b4040483c9f0942211467a9 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 28 Sep 2021 17:21:37 -0500 Subject: [PATCH 179/947] TwistLimit, TwistMotor, and TwistServo twoified. --- BepuPhysics/Constraints/TwistLimit.cs | 46 +++++- BepuPhysics/Constraints/TwistMotor.cs | 45 +++++- BepuPhysics/Constraints/TwistServo.cs | 55 ++++++- Demos/SpecializedTests/ConstraintTestDemo.cs | 145 ++++++++++--------- 4 files changed, 202 insertions(+), 89 deletions(-) diff --git a/BepuPhysics/Constraints/TwistLimit.cs b/BepuPhysics/Constraints/TwistLimit.cs index 9040ba730..e7a41fcec 100644 --- a/BepuPhysics/Constraints/TwistLimit.cs +++ b/BepuPhysics/Constraints/TwistLimit.cs @@ -116,7 +116,7 @@ public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, Vector3Wide.ConditionalSelect(useMin, negatedJacobianA, jacobianA, out jacobianA); TwistServoFunctions.ComputeEffectiveMass(dt, prestep.SpringSettings, inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, jacobianA, - ref projection.ImpulseToVelocityA, ref projection.NegatedImpulseToVelocityB, + out projection.ImpulseToVelocityA, out projection.NegatedImpulseToVelocityB, out var positionErrorToVelocity, out projection.SoftnessImpulseScale, out var effectiveMass, out projection.VelocityToImpulseA); //In the speculative case, allow the limit to be approached. @@ -145,20 +145,56 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB TwistServoFunctions.ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, projection.ImpulseToVelocityA, projection.NegatedImpulseToVelocityB, csi); } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + static void ComputeJacobian(in QuaternionWide orientationA, in QuaternionWide orientationB, + in QuaternionWide localBasisA, in QuaternionWide localBasisB, in Vector minimumAngle, in Vector maximumAngle, out Vector error, out Vector3Wide jacobianA) + { + TwistServoFunctions.ComputeJacobian(orientationA, orientationB, localBasisA, localBasisB, out var basisBX, out var basisBZ, out var basisA, out jacobianA); + TwistServoFunctions.ComputeCurrentAngle(basisBX, basisBZ, basisA, out var angle); + //For simplicity, the solve iterations can only apply a positive impulse. So, the jacobians get flipped when necessary to make that consistent. + //To figure out which way to flip, take the angular distance from minimum to current angle, and maximum to current angle. + MathHelper.GetSignedAngleDifference(minimumAngle, angle, out var minError); + MathHelper.GetSignedAngleDifference(maximumAngle, angle, out var maxError); + var useMin = Vector.LessThan(Vector.Abs(minError), Vector.Abs(maxError)); + + //If we use the maximum bound, flip the jacobian. + error = Vector.ConditionalSelect(useMin, -minError, maxError); + Vector3Wide.Negate(jacobianA, out var negatedJacobianA); + Vector3Wide.ConditionalSelect(useMin, negatedJacobianA, jacobianA, out jacobianA); + } public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref TwistLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - throw new NotImplementedException(); + ComputeJacobian(orientationA, orientationB, prestep.LocalBasisA, prestep.LocalBasisB, prestep.MinimumAngle, prestep.MaximumAngle, out _, out var jacobianA); + Symmetric3x3Wide.TransformWithoutOverlap(jacobianA, inertiaA.InverseInertiaTensor, out var impulseToVelocityA); + Symmetric3x3Wide.TransformWithoutOverlap(jacobianA, inertiaB.InverseInertiaTensor, out var negatedImpulseToVelocityB); + TwistServoFunctions.ApplyImpulse(ref wsvA.Angular, ref wsvB.Angular, impulseToVelocityA, negatedImpulseToVelocityB, accumulatedImpulses); } public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref TwistLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - throw new NotImplementedException(); + ComputeJacobian(orientationA, orientationB, prestep.LocalBasisA, prestep.LocalBasisB, prestep.MinimumAngle, prestep.MaximumAngle, out var error, out var jacobianA); + + TwistServoFunctions.ComputeEffectiveMass(dt, prestep.SpringSettings, inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, jacobianA, + out var impulseToVelocityA, out var negatedImpulseToVelocityB, + out var positionErrorToVelocity, out var softnessImpulseScale, out var effectiveMass, out var velocityToImpulseA); + + //In the speculative case, allow the limit to be approached. + var biasVelocity = Vector.ConditionalSelect(Vector.LessThan(error, Vector.Zero), error * inverseDt, error * positionErrorToVelocity); + var biasImpulse = biasVelocity * effectiveMass; + + Vector3Wide.Subtract(wsvA.Angular, wsvB.Angular, out var netVelocity); + Vector3Wide.Dot(netVelocity, velocityToImpulseA, out var csiVelocityComponent); + //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); + var csi = biasImpulse - accumulatedImpulses * softnessImpulseScale - csiVelocityComponent; + InequalityHelpers.ClampPositive(ref accumulatedImpulses, ref csi); + + TwistServoFunctions.ApplyImpulse(ref wsvA.Angular, ref wsvB.Angular, impulseToVelocityA, negatedImpulseToVelocityB, csi); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector dt, in Vector accumulatedImpulses, ref TwistLimitPrestepData prestep) { } diff --git a/BepuPhysics/Constraints/TwistMotor.cs b/BepuPhysics/Constraints/TwistMotor.cs index 5ca358f01..3e68e2274 100644 --- a/BepuPhysics/Constraints/TwistMotor.cs +++ b/BepuPhysics/Constraints/TwistMotor.cs @@ -100,12 +100,12 @@ public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, Vector3Wide.ConditionalSelect(Vector.LessThan(length, new Vector(1e-10f)), axisA, jacobianA, out jacobianA); TwistServoFunctions.ComputeEffectiveMassContributions(inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, jacobianA, - ref projection.ImpulseToVelocityA, ref projection.NegatedImpulseToVelocityB, out var unsoftenedInverseEffectiveMass); + out projection.ImpulseToVelocityA, out projection.NegatedImpulseToVelocityB, out var unsoftenedInverseEffectiveMass); MotorSettingsWide.ComputeSoftness(prestep.Settings, dt, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale, out projection.MaximumImpulse); var effectiveMass = effectiveMassCFMScale / unsoftenedInverseEffectiveMass; Vector3Wide.Scale(jacobianA, effectiveMass, out projection.VelocityToImpulseA); - + projection.BiasImpulse = prestep.TargetVelocity * effectiveMass; } @@ -130,19 +130,54 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB TwistServoFunctions.ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, projection.ImpulseToVelocityA, projection.NegatedImpulseToVelocityB, csi); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void ComputeJacobian(in QuaternionWide orientationA, in QuaternionWide orientationB, in Vector3Wide localAxisA, in Vector3Wide localAxisB, out Vector3Wide jacobianA) + { + //We don't need any measurement basis in a velocity motor, so the prestep data needs only the axes. + QuaternionWide.TransformWithoutOverlap(localAxisA, orientationA, out var axisA); + QuaternionWide.TransformWithoutOverlap(localAxisB, orientationB, out var axisB); + Vector3Wide.Add(axisA, axisB, out jacobianA); + Vector3Wide.Length(jacobianA, out var length); + Vector3Wide.Scale(jacobianA, Vector.One / length, out jacobianA); + Vector3Wide.ConditionalSelect(Vector.LessThan(length, new Vector(1e-10f)), axisA, jacobianA, out jacobianA); + } + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref TwistMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - throw new NotImplementedException(); + ComputeJacobian(orientationA, orientationB, prestep.LocalAxisA, prestep.LocalAxisB, out var jacobianA); + Symmetric3x3Wide.TransformWithoutOverlap(jacobianA, inertiaA.InverseInertiaTensor, out var impulseToVelocityA); + Symmetric3x3Wide.TransformWithoutOverlap(jacobianA, inertiaB.InverseInertiaTensor, out var negatedImpulseToVelocityB); + TwistServoFunctions.ApplyImpulse(ref wsvA.Angular, ref wsvB.Angular, impulseToVelocityA, negatedImpulseToVelocityB, accumulatedImpulses); } public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref TwistMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - throw new NotImplementedException(); + ComputeJacobian(orientationA, orientationB, prestep.LocalAxisA, prestep.LocalAxisB, out var jacobianA); + + TwistServoFunctions.ComputeEffectiveMassContributions(inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, jacobianA, + out var impulseToVelocityA, out var negatedImpulseToVelocityB, out var unsoftenedInverseEffectiveMass); + + MotorSettingsWide.ComputeSoftness(prestep.Settings, dt, out var effectiveMassCFMScale, out var softnessImpulseScale, out var maximumImpulse); + var effectiveMass = effectiveMassCFMScale / unsoftenedInverseEffectiveMass; + Vector3Wide.Scale(jacobianA, effectiveMass, out var velocityToImpulseA); + + var biasImpulse = prestep.TargetVelocity * effectiveMass; + + Vector3Wide.Subtract(wsvA.Angular, wsvB.Angular, out var netVelocity); + Vector3Wide.Dot(netVelocity, velocityToImpulseA, out var csiVelocityComponent); + //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); + var csi = biasImpulse - accumulatedImpulses * softnessImpulseScale - csiVelocityComponent; + var previousAccumulatedImpulse = accumulatedImpulses; + accumulatedImpulses = Vector.Max(Vector.Min(accumulatedImpulses + csi, maximumImpulse), -maximumImpulse); + csi = accumulatedImpulses - previousAccumulatedImpulse; + + TwistServoFunctions.ApplyImpulse(ref wsvA.Angular, ref wsvB.Angular, impulseToVelocityA, negatedImpulseToVelocityB, csi); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector dt, in Vector accumulatedImpulses, ref TwistMotorPrestepData prestep) { diff --git a/BepuPhysics/Constraints/TwistServo.cs b/BepuPhysics/Constraints/TwistServo.cs index 788d9f414..8869e0feb 100644 --- a/BepuPhysics/Constraints/TwistServo.cs +++ b/BepuPhysics/Constraints/TwistServo.cs @@ -142,7 +142,7 @@ public static void ComputeCurrentAngle(in Vector3Wide basisBX, in Vector3Wide ba [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ComputeEffectiveMassContributions( in Symmetric3x3Wide inverseInertiaA, in Symmetric3x3Wide inverseInertiaB, in Vector3Wide jacobianA, - ref Vector3Wide impulseToVelocityA, ref Vector3Wide negatedImpulseToVelocityB, out Vector unsoftenedInverseEffectiveMass) + out Vector3Wide impulseToVelocityA, out Vector3Wide negatedImpulseToVelocityB, out Vector unsoftenedInverseEffectiveMass) { //Note that JA = -JB, but for the purposes of calculating the effective mass the sign is irrelevant. //This computes the effective mass using the usual (J * M^-1 * JT)^-1 formulation, but we actually make use of the intermediate result J * M^-1 so we compute it directly. @@ -156,10 +156,10 @@ public static void ComputeEffectiveMassContributions( [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ComputeEffectiveMass(float dt, in SpringSettingsWide springSettings, in Symmetric3x3Wide inverseInertiaA, in Symmetric3x3Wide inverseInertiaB, in Vector3Wide jacobianA, - ref Vector3Wide impulseToVelocityA, ref Vector3Wide negatedImpulseToVelocityB, out Vector positionErrorToVelocity, out Vector softnessImpulseScale, + out Vector3Wide impulseToVelocityA, out Vector3Wide negatedImpulseToVelocityB, out Vector positionErrorToVelocity, out Vector softnessImpulseScale, out Vector effectiveMass, out Vector3Wide velocityToImpulseA) { - ComputeEffectiveMassContributions(inverseInertiaA, inverseInertiaB, jacobianA, ref impulseToVelocityA, ref negatedImpulseToVelocityB, out var unsoftenedInverseEffectiveMass); + ComputeEffectiveMassContributions(inverseInertiaA, inverseInertiaB, jacobianA, out impulseToVelocityA, out negatedImpulseToVelocityB, out var unsoftenedInverseEffectiveMass); SpringSettingsWide.ComputeSpringiness(springSettings, dt, out positionErrorToVelocity, out var effectiveMassCFMScale, out softnessImpulseScale); effectiveMass = effectiveMassCFMScale / unsoftenedInverseEffectiveMass; @@ -175,7 +175,7 @@ public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, out var basisBX, out var basisBZ, out var basisA, out var jacobianA); ComputeEffectiveMass(dt, prestep.SpringSettings, inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, jacobianA, - ref projection.ImpulseToVelocityA, ref projection.NegatedImpulseToVelocityB, + out projection.ImpulseToVelocityA, out projection.NegatedImpulseToVelocityB, out var positionErrorToVelocity, out projection.SoftnessImpulseScale, out var effectiveMass, out projection.VelocityToImpulseA); ComputeCurrentAngle(basisBX, basisBZ, basisA, out var angle); @@ -215,19 +215,60 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, projection.ImpulseToVelocityA, projection.NegatedImpulseToVelocityB, csi); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void ComputeJacobian(in QuaternionWide orientationA, in QuaternionWide orientationB, in QuaternionWide localBasisA, in QuaternionWide localBasisB, out Vector3Wide jacobianA) + { + QuaternionWide.ConcatenateWithoutOverlap(localBasisA, orientationA, out var basisQuaternionA); + QuaternionWide.ConcatenateWithoutOverlap(localBasisB, orientationB, out var basisQuaternionB); + + var basisAZ = QuaternionWide.TransformUnitZ(basisQuaternionA); + var basisBZ = QuaternionWide.TransformUnitZ(basisQuaternionB); + //Protect against singularity when the axes point at each other. + Vector3Wide.Add(basisAZ, basisBZ, out jacobianA); + Vector3Wide.Length(jacobianA, out var length); + Vector3Wide.Scale(jacobianA, Vector.One / length, out jacobianA); + Vector3Wide.ConditionalSelect(Vector.LessThan(length, new Vector(1e-10f)), basisAZ, jacobianA, out jacobianA); + } + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref TwistServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - throw new NotImplementedException(); + ComputeJacobian(orientationA, orientationB, prestep.LocalBasisA, prestep.LocalBasisB, out var jacobianA); + Symmetric3x3Wide.TransformWithoutOverlap(jacobianA, inertiaA.InverseInertiaTensor, out var impulseToVelocityA); + Symmetric3x3Wide.TransformWithoutOverlap(jacobianA, inertiaB.InverseInertiaTensor, out var negatedImpulseToVelocityB); + ApplyImpulse(ref wsvA.Angular, ref wsvB.Angular, impulseToVelocityA, negatedImpulseToVelocityB, accumulatedImpulses); } public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref TwistServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - throw new NotImplementedException(); + ComputeJacobian(orientationA, orientationB, prestep.LocalBasisA, prestep.LocalBasisB, + out var basisBX, out var basisBZ, out var basisA, out var jacobianA); + + ComputeEffectiveMass(dt, prestep.SpringSettings, inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, jacobianA, + out var impulseToVelocityA, out var negatedImpulseToVelocityB, + out var positionErrorToVelocity, out var softnessImpulseScale, out var effectiveMass, out var velocityToImpulseA); + + ComputeCurrentAngle(basisBX, basisBZ, basisA, out var angle); + + MathHelper.GetSignedAngleDifference(prestep.TargetAngle, angle, out var error); + + ServoSettingsWide.ComputeClampedBiasVelocity(error, positionErrorToVelocity, prestep.ServoSettings, dt, inverseDt, out var clampedBiasVelocity, out var maximumImpulse); + var biasImpulse = clampedBiasVelocity * effectiveMass; + + Vector3Wide.Subtract(wsvA.Angular, wsvB.Angular, out var netVelocity); + Vector3Wide.Dot(netVelocity, velocityToImpulseA, out var csiVelocityComponent); + //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); + var csi = biasImpulse - accumulatedImpulses * softnessImpulseScale - csiVelocityComponent; + var previousAccumulatedImpulse = accumulatedImpulses; + accumulatedImpulses = Vector.Min(Vector.Max(accumulatedImpulses + csi, -maximumImpulse), maximumImpulse); + csi = accumulatedImpulses - previousAccumulatedImpulse; + + ApplyImpulse(ref wsvA.Angular, ref wsvB.Angular, impulseToVelocityA, negatedImpulseToVelocityB, csi); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector dt, in Vector accumulatedImpulses, ref TwistServoPrestepData prestep) { diff --git a/Demos/SpecializedTests/ConstraintTestDemo.cs b/Demos/SpecializedTests/ConstraintTestDemo.cs index 8a3ddbf9e..de4fade1a 100644 --- a/Demos/SpecializedTests/ConstraintTestDemo.cs +++ b/Demos/SpecializedTests/ConstraintTestDemo.cs @@ -5,6 +5,7 @@ using System.Numerics; using DemoContentLoader; using BepuPhysics.Constraints; +using Demos.Demos; namespace Demos.SpecializedTests { @@ -39,7 +40,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var x = GetNextPosition(ref nextX); var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(1, 1) }); + Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); } { var x = GetNextPosition(ref nextX); @@ -69,63 +70,63 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Simulation.Solver.Add(a, b, new AngularSwivelHinge { LocalSwivelAxisA = new Vector3(1, 0, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); Simulation.Solver.Add(a, b, new SwingLimit { AxisLocalA = new Vector3(0, 1, 0), AxisLocalB = new Vector3(0, 1, 0), MaximumSwingAngle = MathHelper.PiOver2, SpringSettings = new SpringSettings(30, 1) }); } - //{ - // var x = GetNextPosition(ref nextX); - // var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); - // var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - // Simulation.Solver.Add(a, b, new SwivelHinge - // { - // LocalOffsetA = new Vector3(0, 1, 0), - // LocalSwivelAxisA = new Vector3(1, 0, 0), - // LocalOffsetB = new Vector3(0, -1, 0), - // LocalHingeAxisB = new Vector3(0, 1, 0), - // SpringSettings = new SpringSettings(30, 1) - // }); - // Simulation.Solver.Add(a, b, new SwingLimit { AxisLocalA = new Vector3(0, 1, 0), AxisLocalB = new Vector3(0, 1, 0), MaximumSwingAngle = MathHelper.PiOver2, SpringSettings = new SpringSettings(30, 1) }); - //} - //{ - // var x = GetNextPosition(ref nextX); - // var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); - // var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - // Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - // Simulation.Solver.Add(a, b, new TwistServo - // { - // LocalBasisA = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)), - // LocalBasisB = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)), - // TargetAngle = MathHelper.PiOver4, - // SpringSettings = new SpringSettings(30, 1), - // ServoSettings = new ServoSettings(float.MaxValue, 0, float.MaxValue) - // }); - //} - //{ - // var x = GetNextPosition(ref nextX); - // var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); - // var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - // Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - // Simulation.Solver.Add(a, b, new TwistLimit - // { - // LocalBasisA = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)), - // LocalBasisB = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)), - // MinimumAngle = MathHelper.Pi * -0.5f, - // MaximumAngle = MathHelper.Pi * 0.95f, - // SpringSettings = new SpringSettings(30, 1), - // }); - // Simulation.Solver.Add(a, b, new AngularHinge { LocalHingeAxisA = new Vector3(0, 1, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); - //} - //{ - // var x = GetNextPosition(ref nextX); - // var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); - // var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - // Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - // Simulation.Solver.Add(a, b, new TwistMotor - // { - // LocalAxisA = new Vector3(0, 1, 0), - // LocalAxisB = new Vector3(0, 1, 0), - // TargetVelocity = MathHelper.Pi * 2, - // Settings = new MotorSettings(float.MaxValue, 0.1f) - // }); - // Simulation.Solver.Add(a, b, new AngularHinge { LocalHingeAxisA = new Vector3(0, 1, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); - //} + { + var x = GetNextPosition(ref nextX); + var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); + var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + Simulation.Solver.Add(a, b, new SwivelHinge + { + LocalOffsetA = new Vector3(0, 1, 0), + LocalSwivelAxisA = new Vector3(1, 0, 0), + LocalOffsetB = new Vector3(0, -1, 0), + LocalHingeAxisB = new Vector3(0, 1, 0), + SpringSettings = new SpringSettings(30, 1) + }); + Simulation.Solver.Add(a, b, new SwingLimit { AxisLocalA = new Vector3(0, 1, 0), AxisLocalB = new Vector3(0, 1, 0), MaximumSwingAngle = MathHelper.PiOver2, SpringSettings = new SpringSettings(30, 1) }); + } + { + var x = GetNextPosition(ref nextX); + var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); + var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + Simulation.Solver.Add(a, b, new TwistServo + { + LocalBasisA = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)), + LocalBasisB = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)), + TargetAngle = MathHelper.PiOver4, + SpringSettings = new SpringSettings(30, 1), + ServoSettings = new ServoSettings(float.MaxValue, 0, float.MaxValue) + }); + } + { + var x = GetNextPosition(ref nextX); + var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); + var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + Simulation.Solver.Add(a, b, new TwistLimit + { + LocalBasisA = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)), + LocalBasisB = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)), + MinimumAngle = MathHelper.Pi * -0.5f, + MaximumAngle = MathHelper.Pi * 0.95f, + SpringSettings = new SpringSettings(30, 1), + }); + Simulation.Solver.Add(a, b, new AngularHinge { LocalHingeAxisA = new Vector3(0, 1, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); + } + { + var x = GetNextPosition(ref nextX); + var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); + var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + Simulation.Solver.Add(a, b, new TwistMotor + { + LocalAxisA = new Vector3(0, 1, 0), + LocalAxisB = new Vector3(0, 1, 0), + TargetVelocity = MathHelper.Pi * 2, + Settings = new MotorSettings(float.MaxValue, 0.1f) + }); + Simulation.Solver.Add(a, b, new AngularHinge { LocalHingeAxisA = new Vector3(0, 1, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); + } //{ // var x = GetNextPosition(ref nextX); // var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); @@ -367,20 +368,20 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Settings = new MotorSettings(10, 1e-4f) }); } - //{ - // var x = GetNextPosition(ref nextX); - // var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); - // var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); - // var a = Simulation.Bodies.Add(aDescription); - // var b = Simulation.Bodies.Add(bDescription); - // Simulation.Solver.Add(a, b, new BallSocketServo - // { - // LocalOffsetA = new Vector3(0, 1, 0), - // LocalOffsetB = new Vector3(0, -1, 0), - // SpringSettings = new SpringSettings(30, 1), - // ServoSettings = new ServoSettings(100, 1, 100) - // }); - //} + { + var x = GetNextPosition(ref nextX); + var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); + var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); + var a = Simulation.Bodies.Add(aDescription); + var b = Simulation.Bodies.Add(bDescription); + Simulation.Solver.Add(a, b, new BallSocketServo + { + LocalOffsetA = new Vector3(0, 1, 0), + LocalOffsetB = new Vector3(0, -1, 0), + SpringSettings = new SpringSettings(30, 1), + ServoSettings = new ServoSettings(100, 1, 100) + }); + } //{ // var x = GetNextPosition(ref nextX); // var wheelShape = new CollidableDescription(Simulation.Shapes.Add(new Cylinder(1, 0.1f)), 0.1f); From a3952ac009eaef0ec5765743b9fa6725ed58c8a8 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 28 Sep 2021 18:20:46 -0500 Subject: [PATCH 180/947] AngularMotor and AngularServo twoified. --- BepuPhysics/Constraints/AngularMotor.cs | 24 +++++++++++-- BepuPhysics/Constraints/AngularServo.cs | 32 ++++++++++++++--- Demos/SpecializedTests/ConstraintTestDemo.cs | 38 ++++++++++---------- 3 files changed, 68 insertions(+), 26 deletions(-) diff --git a/BepuPhysics/Constraints/AngularMotor.cs b/BepuPhysics/Constraints/AngularMotor.cs index 88b2d4312..b28b8074a 100644 --- a/BepuPhysics/Constraints/AngularMotor.cs +++ b/BepuPhysics/Constraints/AngularMotor.cs @@ -103,12 +103,30 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref AngularMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - throw new NotImplementedException(); + AngularServoFunctions.ApplyImpulse(ref wsvA.Angular, ref wsvB.Angular, inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, accumulatedImpulses); } public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref AngularMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - throw new NotImplementedException(); + //Jacobians are just the identity matrix. + MotorSettingsWide.ComputeSoftness(prestep.Settings, dt, out var effectiveMassCFMScale, out var softnessImpulseScale, out var maximumImpulse); + + Symmetric3x3Wide.Add(inertiaB.InverseInertiaTensor, inertiaB.InverseInertiaTensor, out var unsoftenedInverseEffectiveMass); + Symmetric3x3Wide.Invert(unsoftenedInverseEffectiveMass, out var unsoftenedEffectiveMass); + //Note that we don't scale the effective mass directly; instead scale CSI. + + QuaternionWide.TransformWithoutOverlap(prestep.TargetVelocityLocalA, orientationA, out var biasVelocity); + + //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); + Vector3Wide.Subtract(wsvA.Angular, wsvB.Angular, out var csv); + Vector3Wide.Subtract(biasVelocity, csv, out csv); + Symmetric3x3Wide.TransformWithoutOverlap(csv, unsoftenedEffectiveMass, out var csi); + csi *= effectiveMassCFMScale; + Vector3Wide.Scale(accumulatedImpulses, softnessImpulseScale, out var softnessComponent); + Vector3Wide.Subtract(csi, softnessComponent, out csi); + + ServoSettingsWide.ClampImpulse(maximumImpulse, ref accumulatedImpulses, ref csi); + AngularServoFunctions.ApplyImpulse(ref wsvA.Angular, ref wsvB.Angular, inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, csi); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -120,7 +138,7 @@ public void UpdateForNewPose( } } - public class AngularMotorTypeProcessor : TwoBodyTypeProcessor + public class AngularMotorTypeProcessor : TwoBodyTypeProcessor { public const int BatchTypeId = 30; } diff --git a/BepuPhysics/Constraints/AngularServo.cs b/BepuPhysics/Constraints/AngularServo.cs index 79112e8d4..de2860fd0 100644 --- a/BepuPhysics/Constraints/AngularServo.cs +++ b/BepuPhysics/Constraints/AngularServo.cs @@ -144,24 +144,48 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref AngularServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - throw new NotImplementedException(); + ApplyImpulse(ref wsvA.Angular, ref wsvB.Angular, inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, accumulatedImpulses); } public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref AngularServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - throw new NotImplementedException(); + //Jacobians are just I and -I. + QuaternionWide.ConcatenateWithoutOverlap(prestep.TargetRelativeRotationLocalA, orientationA, out var targetOrientationB); + QuaternionWide.Conjugate(targetOrientationB, out var inverseTarget); + QuaternionWide.ConcatenateWithoutOverlap(inverseTarget, orientationB, out var errorRotation); + + QuaternionWide.GetApproximateAxisAngleFromQuaternion(errorRotation, out var errorAxis, out var errorLength); + + SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); + Symmetric3x3Wide.Add(inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, out var unsoftenedInverseEffectiveMass); + Symmetric3x3Wide.Invert(unsoftenedInverseEffectiveMass, out var unsoftenedEffectiveMass); + //Note effective mass is not directly scaled by CFM scale; instead scale the CSI. + + ServoSettingsWide.ComputeClampedBiasVelocity(errorAxis, errorLength, positionErrorToVelocity, prestep.ServoSettings, dt, inverseDt, out var clampedBiasVelocity, out var maximumImpulse); + + //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); + Vector3Wide.Subtract(wsvA.Angular, wsvB.Angular, out var csv); + Vector3Wide.Subtract(clampedBiasVelocity, csv, out csv); + Symmetric3x3Wide.TransformWithoutOverlap(csv, unsoftenedEffectiveMass, out var csi); + csi *= effectiveMassCFMScale; + Vector3Wide.Scale(accumulatedImpulses, softnessImpulseScale, out var softnessComponent); + Vector3Wide.Subtract(csi, softnessComponent, out csi); + + ServoSettingsWide.ClampImpulse(maximumImpulse, ref accumulatedImpulses, ref csi); + + ApplyImpulse(ref wsvA.Angular, ref wsvB.Angular, inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, csi); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector dt, in Vector3Wide accumulatedImpulses, ref AngularServoPrestepData prestep) { } } - public class AngularServoTypeProcessor : TwoBodyTypeProcessor + public class AngularServoTypeProcessor : TwoBodyTypeProcessor { public const int BatchTypeId = 29; } diff --git a/Demos/SpecializedTests/ConstraintTestDemo.cs b/Demos/SpecializedTests/ConstraintTestDemo.cs index de4fade1a..68384bfb0 100644 --- a/Demos/SpecializedTests/ConstraintTestDemo.cs +++ b/Demos/SpecializedTests/ConstraintTestDemo.cs @@ -127,25 +127,25 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) }); Simulation.Solver.Add(a, b, new AngularHinge { LocalHingeAxisA = new Vector3(0, 1, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); } - //{ - // var x = GetNextPosition(ref nextX); - // var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); - // var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - // Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - // Simulation.Solver.Add(a, b, new AngularServo - // { - // TargetRelativeRotationLocalA = QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), MathHelper.PiOver2), - // ServoSettings = new ServoSettings(float.MaxValue, 0, 12f), - // SpringSettings = new SpringSettings(30, 1) - // }); - //} - //{ - // var x = GetNextPosition(ref nextX); - // var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); - // var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - // Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - // Simulation.Solver.Add(a, b, new AngularMotor { TargetVelocityLocalA = new Vector3(0, 1, 0), Settings = new MotorSettings(15, 0.0001f) }); - //} + { + var x = GetNextPosition(ref nextX); + var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); + var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + Simulation.Solver.Add(a, b, new AngularServo + { + TargetRelativeRotationLocalA = QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), MathHelper.PiOver2), + ServoSettings = new ServoSettings(float.MaxValue, 0, 12f), + SpringSettings = new SpringSettings(30, 1) + }); + } + { + var x = GetNextPosition(ref nextX); + var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); + var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + Simulation.Solver.Add(a, b, new AngularMotor { TargetVelocityLocalA = new Vector3(0, 1, 0), Settings = new MotorSettings(15, 0.0001f) }); + } { var x = GetNextPosition(ref nextX); var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); From 486f86156db7622cfaf572ffcec8cf1c7be88b26 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 28 Sep 2021 19:56:37 -0500 Subject: [PATCH 181/947] VolumeConstraint twoified. --- .../Constraints/FourBodyTypeProcessor.cs | 4 +- BepuPhysics/Constraints/VolumeConstraint.cs | 92 ++++++++++++++++++- Demos/Demo.cs | 2 +- Demos/Demos/Cars/CarDemo.cs | 6 +- Demos/Demos/NewtDemo.cs | 5 +- Demos/SpecializedTests/ConstraintTestDemo.cs | 58 ++++++------ 6 files changed, 129 insertions(+), 38 deletions(-) diff --git a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs index 7155059d9..693237bef 100644 --- a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs @@ -379,8 +379,8 @@ public unsafe override void WarmStart2(bodies, ref integratorCallbacks, ref integrationFlags, 3, dt, workerIndex, i, ref references.IndexD, count, out var positionD, out var orientationD, out var wsvD, out var inertiaD); - if (typeof(TAllowPoseIntegration) == typeof(AllowPoseIntegration)) - function.UpdateForNewPose(positionA, orientationA, inertiaA, wsvA, positionB, orientationB, inertiaB, wsvB, positionC, orientationC, inertiaC, wsvC, positionD, orientationD, inertiaD, wsvD, new Vector(dt), accumulatedImpulses, ref prestep); + //if (typeof(TAllowPoseIntegration) == typeof(AllowPoseIntegration)) + // function.UpdateForNewPose(positionA, orientationA, inertiaA, wsvA, positionB, orientationB, inertiaB, wsvB, positionC, orientationC, inertiaC, wsvC, positionD, orientationD, inertiaD, wsvD, new Vector(dt), accumulatedImpulses, ref prestep); function.WarmStart2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, positionD, orientationD, inertiaD, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC, ref wsvD); diff --git a/BepuPhysics/Constraints/VolumeConstraint.cs b/BepuPhysics/Constraints/VolumeConstraint.cs index e97e34e00..484a73a17 100644 --- a/BepuPhysics/Constraints/VolumeConstraint.cs +++ b/BepuPhysics/Constraints/VolumeConstraint.cs @@ -206,14 +206,102 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ApplyImpulse(ref velocityA, ref velocityB, ref velocityC, ref velocityD, ref projection, ref negatedJacobianA, ref csi); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + private static void ApplyImpulse( + in Vector inverseMassA, in Vector inverseMassB, in Vector inverseMassC, in Vector inverseMassD, + in Vector3Wide negatedJacobianA, in Vector3Wide jacobianB, in Vector3Wide jacobianC, in Vector3Wide jacobianD, in Vector impulse, + ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref BodyVelocityWide velocityC, ref BodyVelocityWide velocityD) + { + Vector3Wide.Scale(negatedJacobianA, inverseMassA * impulse, out var negativeVelocityChangeA); + Vector3Wide.Scale(jacobianB, inverseMassB * impulse, out var velocityChangeB); + Vector3Wide.Scale(jacobianC, inverseMassC * impulse, out var velocityChangeC); + Vector3Wide.Scale(jacobianD, inverseMassD * impulse, out var velocityChangeD); + Vector3Wide.Subtract(velocityA.Linear, negativeVelocityChangeA, out velocityA.Linear); + Vector3Wide.Add(velocityB.Linear, velocityChangeB, out velocityB.Linear); + Vector3Wide.Add(velocityC.Linear, velocityChangeC, out velocityC.Linear); + Vector3Wide.Add(velocityD.Linear, velocityChangeD, out velocityD.Linear); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + static void ComputeJacobian(in Vector3Wide positionA, in Vector3Wide positionB, in Vector3Wide positionC, in Vector3Wide positionD, + out Vector3Wide ad, + out Vector3Wide negatedJA, out Vector3Wide jacobianB, out Vector3Wide jacobianC, out Vector3Wide jacobianD) + { + var ab = positionB - positionA; + var ac = positionC - positionA; + ad = positionD - positionA; + Vector3Wide.CrossWithoutOverlap(ac, ad, out jacobianB); + Vector3Wide.CrossWithoutOverlap(ad, ab, out jacobianC); + Vector3Wide.CrossWithoutOverlap(ab, ac, out jacobianD); + Vector3Wide.Add(jacobianB, jacobianC, out negatedJA); + Vector3Wide.Add(jacobianD, negatedJA, out negatedJA); + } public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in Vector3Wide positionD, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, ref VolumeConstraintPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC, ref BodyVelocityWide wsvD) { - throw new NotImplementedException(); + ComputeJacobian(positionA, positionB, positionC, positionD, out var ad, out var negatedJA, out var jacobianB, out var jacobianC, out var jacobianD); + //Vector3Wide.Dot(jacobianD, ad, out var unscaledVolume); + ApplyImpulse(inertiaA.InverseMass, inertiaB.InverseMass, inertiaC.InverseMass, inertiaD.InverseMass, negatedJA, jacobianB, jacobianC, jacobianD, accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC, ref wsvD); } public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in Vector3Wide positionD, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, float dt, float inverseDt, ref VolumeConstraintPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC, ref BodyVelocityWide wsvD) { - throw new NotImplementedException(); + //Volume of parallelepiped with vertices a, b, c, d is: + //(ab x ac) * ad + //A tetrahedron with the same edges will have one sixth of this volume. As a constant factor, it's not relevant. So the constraint is just: + //OriginalVolume * 6 = (ab x ac) * ad + //Taking the derivative to get the velocity constraint: + //0 = d/dt(ab x ac) * ad + (ab x ac) * d/dt(ad) + //0 = d/dt(ab x ac) * ad + (ab x ac) * (d/dt(d) - d/dt(a)) + //0 = (d/dt(ab) x ac + ab x d/dt(ac)) * ad + (ab x ac) * (d/dt(d) - d/dt(a)) + //0 = ((d/dt(ab) x ac) * ad + (ab x d/dt(ac)) * ad + (ab x ac) * (d/dt(d) - d/dt(a)) + //0 = (ac x ad) * (d/dt(b) - d/dt(a)) + (ad x ab) * (d/dt(c) - d/dt(a)) + (ab x ac) * (d/dt(d) - d/dt(a)) + //Giving the linear jacobians: + //JA: -ac x ad - ad x ab - ab x ac == bd x bc + //JB: ac x ad + //JC: ad x ab + //JD: ab x ac + //We're not blending the jacobians into the effective mass or inverse mass either- even though that would save ALU time, the goal here is to minimize memory bandwidth since that + //tends to be the bottleneck for any multithreaded simulation. (Despite being a 1DOF constraint, this doesn't need to output inverse inertia tensors, so premultiplying isn't a win.) + ComputeJacobian(positionA, positionB, positionC, positionD, out var ad, out var negatedJA, out var jacobianB, out var jacobianC, out var jacobianD); + + Vector3Wide.Dot(negatedJA, negatedJA, out var contributionA); + Vector3Wide.Dot(jacobianB, jacobianB, out var contributionB); + Vector3Wide.Dot(jacobianC, jacobianC, out var contributionC); + Vector3Wide.Dot(jacobianD, jacobianD, out var contributionD); + + //Protect against singularity by padding the jacobian contributions. This is very much a hack, but it's a pretty simple hack. + //Less sensitive to tuning than attempting to guard the inverseEffectiveMass itself, since that is sensitive to both scale AND mass. + + //Choose an epsilon based on the target volume. Note that volume ~= width^3, whereas our jacobian contributions are things like (ac x ad) * (ac x ad), which is proportional + //to the area of the triangle acd squared. In other words, the contribution is ~ width^4. + //Scaling the volume by a constant factor will not match the growth rate of the jacobian contributions. + //We're going to ignore this until proven to be a noticeable problem because Vector does not expose exp or pow and this is cheap. + //Could still implement it, but it's not super high value. + var epsilon = 5e-4f * prestep.TargetScaledVolume; + contributionA = Vector.Max(epsilon, contributionA); + contributionB = Vector.Max(epsilon, contributionB); + contributionC = Vector.Max(epsilon, contributionC); + contributionD = Vector.Max(epsilon, contributionD); + var inverseEffectiveMass = contributionA * inertiaA.InverseMass + contributionB * inertiaB.InverseMass + contributionC * inertiaC.InverseMass + contributionD * inertiaD.InverseMass; + + SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); + + var effectiveMass = effectiveMassCFMScale / inverseEffectiveMass; + //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. + Vector3Wide.Dot(jacobianD, ad, out var unscaledVolume); + var biasVelocity = (prestep.TargetScaledVolume - unscaledVolume) * positionErrorToVelocity; + + //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); + Vector3Wide.Dot(negatedJA, wsvA.Linear, out var negatedVelocityContributionA); + Vector3Wide.Dot(jacobianB, wsvB.Linear, out var velocityContributionB); + Vector3Wide.Dot(jacobianC, wsvC.Linear, out var velocityContributionC); + Vector3Wide.Dot(jacobianD, wsvD.Linear, out var velocityContributionD); + var csv = velocityContributionB + velocityContributionC + velocityContributionD - negatedVelocityContributionA; + var csi = (biasVelocity - csv) * effectiveMass - accumulatedImpulses * softnessImpulseScale; + accumulatedImpulses += csi; + + ApplyImpulse(inertiaA.InverseMass, inertiaB.InverseMass, inertiaC.InverseMass, inertiaD.InverseMass, negatedJA, jacobianB, jacobianC, jacobianD, csi, ref wsvA, ref wsvB, ref wsvC, ref wsvD); } [MethodImpl(MethodImplOptions.AggressiveInlining)] diff --git a/Demos/Demo.cs b/Demos/Demo.cs index 37ebe5190..5f3790da3 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -62,7 +62,7 @@ public virtual void Update(Window window, Camera camera, Input input, float dt) //fully decouple simulation and rendering rates across different threads. //(In either case, you'd also want to interpolate or extrapolate simulation results during rendering for smoothness.) //Note that taking steps of variable length can reduce stability. Gradual or one-off changes can work reasonably well. - Simulation.Timestep(1 / 60f, ThreadDispatcher); + Simulation.Timestep(1 / 60f); ////Here's an example of how it would look to use more frequent updates, but still with a fixed amount of time simulated per update call: //const float timeToSimulate = 1 / 60f; diff --git a/Demos/Demos/Cars/CarDemo.cs b/Demos/Demos/Cars/CarDemo.cs index b0bbe4ded..5bd6512b5 100644 --- a/Demos/Demos/Cars/CarDemo.cs +++ b/Demos/Demos/Cars/CarDemo.cs @@ -43,8 +43,10 @@ public override void Initialize(ContentArchive content, Camera camera) var properties = new CollidableProperty(); //The PositionFirstTimestepper is the simplest timestepping mode, but since it integrates velocity into position at the start of the frame, directly modified velocities outside of the timestep //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. - //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. - Simulation = Simulation.Create(BufferPool, new CarCallbacks() { Properties = properties }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. + //Simulation = Simulation.Create(BufferPool, new CarCallbacks() { Properties = properties }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + Simulation = Simulation.Create(BufferPool, new CarCallbacks() { Properties = properties }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(8), 1); + var builder = new CompoundBuilder(BufferPool, Simulation.Shapes, 2); builder.Add(new Box(1.85f, 0.7f, 4.73f), RigidPose.Identity, 10); diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index c5eb915ae..37b200f8c 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -824,7 +824,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper(1)); - Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0), 0, 0), new EmbeddedSubsteppingTimestepper2(3), solverIterationCount: 1); + Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0), 0, 0), new EmbeddedSubsteppingTimestepper2(9), solverIterationCount: 1); //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper2(), solverIterationCount: 60); //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)), new SubsteppingTimestepper2(3), solverIterationCount: 1); @@ -832,7 +832,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) float cellSize = 0.1f; DumbTetrahedralizer.Tetrahedralize(meshContent.Triangles, cellSize, BufferPool, out var vertices, out var vertexSpatialIndices, out var cellVertexIndices, out var tetrahedraVertexIndices); - var weldSpringiness = new SpringSettings(30f, 1f); + var weldSpringiness = new SpringSettings(100f, 1f); var volumeSpringiness = new SpringSettings(30f, 1); //int terboTotal = 0; //var terboShape = new Box(0.5f, 0.5f, 3); @@ -852,6 +852,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) } //Console.WriteLine($"body count: {Simulation.Bodies.ActiveSet.Count}"); //Console.WriteLine($"constraint count: {Simulation.Solver.CountConstraints()}"); + Simulation.Bodies.GetBodyReference(new BodyHandle(55)).Pose.Position += new Vector3(10, 5, 0); BufferPool.Return(ref vertices); vertexSpatialIndices.Dispose(BufferPool); diff --git a/Demos/SpecializedTests/ConstraintTestDemo.cs b/Demos/SpecializedTests/ConstraintTestDemo.cs index 68384bfb0..c179871c3 100644 --- a/Demos/SpecializedTests/ConstraintTestDemo.cs +++ b/Demos/SpecializedTests/ConstraintTestDemo.cs @@ -23,8 +23,8 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(25, 4, 40); camera.Yaw = 0; Simulation = Simulation.Create(BufferPool, - new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 2); - //new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(30, 1), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 2f }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 2); + //new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(30, 1), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 2f }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper(), 8); var shapeA = new Box(.75f, 1, .5f); var shapeIndexA = Simulation.Shapes.Add(shapeA); @@ -155,33 +155,33 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var b = Simulation.Bodies.Add(bDescription); Simulation.Solver.Add(a, b, new Weld { LocalOffset = new Vector3(0, 2, 0), LocalOrientation = Quaternion.Identity, SpringSettings = new SpringSettings(30, 1) }); } - //{ - // var x = GetNextPosition(ref nextX); - // var sphere = new Sphere(0.125f); - // //Treat each vertex as a point mass that cannot rotate. - // var sphereInertia = new BodyInertia { InverseMass = 1 }; - // var sphereCollidable = new CollidableDescription(Simulation.Shapes.Add(sphere), 0.1f); - // var a = new Vector3(x, 3, 0); - // var b = new Vector3(x, 4, 0); - // var c = new Vector3(x, 3, 1); - // var d = new Vector3(x + 1, 3, 0); - // var aDescription = BodyDescription.CreateDynamic(a, sphereInertia, sphereCollidable, activity); - // var bDescription = BodyDescription.CreateDynamic(b, sphereInertia, sphereCollidable, activity); - // var cDescription = BodyDescription.CreateDynamic(c, sphereInertia, sphereCollidable, activity); - // var dDescription = BodyDescription.CreateDynamic(d, sphereInertia, sphereCollidable, activity); - // var aHandle = Simulation.Bodies.Add(aDescription); - // var bHandle = Simulation.Bodies.Add(bDescription); - // var cHandle = Simulation.Bodies.Add(cDescription); - // var dHandle = Simulation.Bodies.Add(dDescription); - // var distanceSpringiness = new SpringSettings(3f, 1); - // Simulation.Solver.Add(aHandle, bHandle, new CenterDistanceConstraint(Vector3.Distance(a, b), distanceSpringiness)); - // Simulation.Solver.Add(aHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(a, c), distanceSpringiness)); - // Simulation.Solver.Add(aHandle, dHandle, new CenterDistanceConstraint(Vector3.Distance(a, d), distanceSpringiness)); - // Simulation.Solver.Add(bHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(b, c), distanceSpringiness)); - // Simulation.Solver.Add(bHandle, dHandle, new CenterDistanceConstraint(Vector3.Distance(b, d), distanceSpringiness)); - // Simulation.Solver.Add(cHandle, dHandle, new CenterDistanceConstraint(Vector3.Distance(c, d), distanceSpringiness)); - // Simulation.Solver.Add(aHandle, bHandle, cHandle, dHandle, new VolumeConstraint(a, b, c, d, new SpringSettings(30, 1))); - //} + { + var x = GetNextPosition(ref nextX); + var sphere = new Sphere(0.125f); + //Treat each vertex as a point mass that cannot rotate. + var sphereInertia = new BodyInertia { InverseMass = 1 }; + var sphereCollidable = new CollidableDescription(Simulation.Shapes.Add(sphere), 0.1f); + var a = new Vector3(x, 3, 0); + var b = new Vector3(x, 4, 0); + var c = new Vector3(x, 3, 1); + var d = new Vector3(x + 1, 3, 0); + var aDescription = BodyDescription.CreateDynamic(a, sphereInertia, sphereCollidable, activity); + var bDescription = BodyDescription.CreateDynamic(b, sphereInertia, sphereCollidable, activity); + var cDescription = BodyDescription.CreateDynamic(c, sphereInertia, sphereCollidable, activity); + var dDescription = BodyDescription.CreateDynamic(d, sphereInertia, sphereCollidable, activity); + var aHandle = Simulation.Bodies.Add(aDescription); + var bHandle = Simulation.Bodies.Add(bDescription); + var cHandle = Simulation.Bodies.Add(cDescription); + var dHandle = Simulation.Bodies.Add(dDescription); + var distanceSpringiness = new SpringSettings(3f, 1); + Simulation.Solver.Add(aHandle, bHandle, new CenterDistanceConstraint(Vector3.Distance(a, b), distanceSpringiness)); + Simulation.Solver.Add(aHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(a, c), distanceSpringiness)); + Simulation.Solver.Add(aHandle, dHandle, new CenterDistanceConstraint(Vector3.Distance(a, d), distanceSpringiness)); + Simulation.Solver.Add(bHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(b, c), distanceSpringiness)); + Simulation.Solver.Add(bHandle, dHandle, new CenterDistanceConstraint(Vector3.Distance(b, d), distanceSpringiness)); + Simulation.Solver.Add(cHandle, dHandle, new CenterDistanceConstraint(Vector3.Distance(c, d), distanceSpringiness)); + Simulation.Solver.Add(aHandle, bHandle, cHandle, dHandle, new VolumeConstraint(a, b, c, d, new SpringSettings(30, 1))); + } //{ // var x = GetNextPosition(ref nextX); // var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); From c72423f5bba390c86254501a052e8d5cf71bab13 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 28 Sep 2021 21:00:53 -0500 Subject: [PATCH 182/947] DistanceServo twoified. --- BepuPhysics/Constraints/DistanceLimit.cs | 4 +- BepuPhysics/Constraints/DistanceServo.cs | 99 +++++++++++++++++++- Demos/SpecializedTests/ConstraintTestDemo.cs | 16 ++-- 3 files changed, 104 insertions(+), 15 deletions(-) diff --git a/BepuPhysics/Constraints/DistanceLimit.cs b/BepuPhysics/Constraints/DistanceLimit.cs index d76be3ed6..ff6178df6 100644 --- a/BepuPhysics/Constraints/DistanceLimit.cs +++ b/BepuPhysics/Constraints/DistanceLimit.cs @@ -141,7 +141,7 @@ public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref DistanceLimitProjection projection, ref Vector accumulatedImpulse) { DistanceServoFunctions.ApplyImpulse(ref velocityA, ref velocityB, - projection.LinearImpulseToVelocityA, projection.AngularImpulseToVelocityA, projection.LinearImpulseToVelocityB, projection.AngularImpulseToVelocityB, ref accumulatedImpulse); + projection.LinearImpulseToVelocityA, projection.AngularImpulseToVelocityA, projection.LinearImpulseToVelocityB, projection.AngularImpulseToVelocityB, accumulatedImpulse); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -155,7 +155,7 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB var csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (linearCSIA + angularCSIA - negatedLinearCSIB + angularCSIB); InequalityHelpers.ClampPositive(ref accumulatedImpulse, ref csi); DistanceServoFunctions.ApplyImpulse(ref velocityA, ref velocityB, - projection.LinearImpulseToVelocityA, projection.AngularImpulseToVelocityA, projection.LinearImpulseToVelocityB, projection.AngularImpulseToVelocityB, ref csi); + projection.LinearImpulseToVelocityA, projection.AngularImpulseToVelocityA, projection.LinearImpulseToVelocityB, projection.AngularImpulseToVelocityB, csi); } diff --git a/BepuPhysics/Constraints/DistanceServo.cs b/BepuPhysics/Constraints/DistanceServo.cs index 633110805..4ceea8383 100644 --- a/BepuPhysics/Constraints/DistanceServo.cs +++ b/BepuPhysics/Constraints/DistanceServo.cs @@ -207,7 +207,7 @@ public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ApplyImpulse(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, in Vector3Wide linearImpulseToVelocityA, in Vector3Wide angularImpulseToVelocityA, in Vector3Wide linearImpulseToVelocityB, in Vector3Wide angularImpulseToVelocityB, - ref Vector csi) + in Vector csi) { Vector3Wide.Scale(linearImpulseToVelocityA, csi, out var linearVelocityChangeA); Vector3Wide.Scale(angularImpulseToVelocityA, csi, out var angularVelocityChangeA); @@ -223,7 +223,7 @@ public static void ApplyImpulse(ref BodyVelocityWide velocityA, ref BodyVelocity public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref DistanceServoProjection projection, ref Vector accumulatedImpulse) { ApplyImpulse(ref velocityA, ref velocityB, - projection.LinearImpulseToVelocityA, projection.AngularImpulseToVelocityA, projection.LinearImpulseToVelocityB, projection.AngularImpulseToVelocityB, ref accumulatedImpulse); + projection.LinearImpulseToVelocityA, projection.AngularImpulseToVelocityA, projection.LinearImpulseToVelocityB, projection.AngularImpulseToVelocityB, accumulatedImpulse); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -238,17 +238,106 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ServoSettingsWide.ClampImpulse(projection.MaximumImpulse, ref accumulatedImpulse, ref csi); ApplyImpulse(ref velocityA, ref velocityB, - projection.LinearImpulseToVelocityA, projection.AngularImpulseToVelocityA, projection.LinearImpulseToVelocityB, projection.AngularImpulseToVelocityB, ref csi); + projection.LinearImpulseToVelocityA, projection.AngularImpulseToVelocityA, projection.LinearImpulseToVelocityB, projection.AngularImpulseToVelocityB, csi); } + public static void ComputeJacobian(in Vector distance, in Vector3Wide anchorOffsetA, in Vector3Wide anchorOffsetB, ref Vector3Wide direction, out Vector3Wide angularJA, out Vector3Wide angularJB) + { + //If the distance is zero, there is no valid offset direction. Pick one arbitrarily. + var needFallback = Vector.LessThan(distance, new Vector(1e-9f)); + direction.X = Vector.ConditionalSelect(needFallback, Vector.One, direction.X); + direction.Y = Vector.ConditionalSelect(needFallback, Vector.Zero, direction.Y); + direction.Z = Vector.ConditionalSelect(needFallback, Vector.Zero, direction.Z); + + Vector3Wide.CrossWithoutOverlap(anchorOffsetA, direction, out angularJA); + Vector3Wide.CrossWithoutOverlap(direction, anchorOffsetB, out angularJB); //Note flip negation. + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void ComputeTransforms( + in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, in Vector3Wide anchorOffsetA, in Vector3Wide anchorOffsetB, in Vector distance, ref Vector3Wide direction, + float dt, in SpringSettingsWide springSettings, + out Vector positionErrorToVelocity, out Vector softnessImpulseScale, out Vector effectiveMass, + out Vector3Wide angularJA, out Vector3Wide angularJB, out Vector3Wide angularImpulseToVelocityA, out Vector3Wide angularImpulseToVelocityB) + { + //Position constraint: + //||positionA + localOffsetA * orientationA - positionB - localOffsetB * orientationB|| = distance + //Skipping a bunch of algebra, the velocity constraint applies to the change in velocity along the separating axis. + //dot(linearA + angularA x (localOffsetA * orientationA) - linearB - angularA x (localOffsetB * orientationB), normalize(positionA + localOffsetA * orientationA - positionB - localOffsetB * orientationB)) = 0 + //dot(linearA, direction) + dot(angularA x offsetA, direction) + dot(linearB, -direction) + dot(angularB x offsetB, -direction) = 0 + //dot(linearA, direction) + dot(offsetA x direction, angularA) + dot(linearB, -direction) + dot(offsetB x -direction, angularB) = 0 + //dot(linearA, direction) + dot(offsetA x direction, angularA) - dot(linearB, direction) + dot(direction x offsetB, angularB) = 0 + //Jacobians are direction, -direction, offsetA x direction, and direction x offsetB. + //That's 9 unique scalars. + //We can either store those 9 plus 14 for the inverse masses, or we can premultiply. + //V * JT * Me * J * I^-1 + //If you premultiply JT * Me and J * I^-1, you get 9 scalars for velocity->impulse and 12 for impulse->velocity. + //If you don't premultiply, it takes 9 for jacobians, 14 for inverse inertia, and then 1 for effective mass. + //That's 21 versus 24. On top of that, premultiplying saves some ALU work. + + //Note that we're working with the distance instead of distance squared. That makes it easier to use and reason about at the cost of a square root in the prestep. + //That really, really doesn't matter. + ComputeJacobian(distance, anchorOffsetA, anchorOffsetB, ref direction, out angularJA, out angularJB); + + //The linear jacobian contributions are just a scalar multiplication by 1 since it's a unit length vector. + Symmetric3x3Wide.TransformWithoutOverlap(angularJA, inertiaA.InverseInertiaTensor, out angularImpulseToVelocityA); + Symmetric3x3Wide.TransformWithoutOverlap(angularJB, inertiaB.InverseInertiaTensor, out angularImpulseToVelocityB); + Vector3Wide.Dot(angularJA, angularImpulseToVelocityA, out var angularContributionA); + Vector3Wide.Dot(angularJB, angularImpulseToVelocityB, out var angularContributionB); + var inverseEffectiveMass = inertiaA.InverseMass + inertiaB.InverseMass + angularContributionA + angularContributionB; + + SpringSettingsWide.ComputeSpringiness(springSettings, dt, out positionErrorToVelocity, out var effectiveMassCFMScale, out softnessImpulseScale); + effectiveMass = effectiveMassCFMScale / inverseEffectiveMass; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void ApplyImpulse( + in Vector inverseMassA, in Vector inverseMassB, in Vector3Wide direction, in Vector3Wide angularImpulseToVelocityA, in Vector3Wide angularImpulseToVelocityB, + in Vector csi, ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB) + { + Vector3Wide.Scale(direction, csi * inverseMassA, out var linearVelocityChangeA); + Vector3Wide.Scale(angularImpulseToVelocityA, csi, out var angularVelocityChangeA); + Vector3Wide.Add(linearVelocityChangeA, velocityA.Linear, out velocityA.Linear); + Vector3Wide.Add(angularVelocityChangeA, velocityA.Angular, out velocityA.Angular); + Vector3Wide.Scale(direction, csi * inverseMassB, out var negatedLinearVelocityChangeB); + Vector3Wide.Scale(angularImpulseToVelocityB, csi, out var angularVelocityChangeB); + Vector3Wide.Subtract(velocityB.Linear, negatedLinearVelocityChangeB, out velocityB.Linear); + Vector3Wide.Add(angularVelocityChangeB, velocityB.Angular, out velocityB.Angular); + } + + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref DistanceServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - throw new NotImplementedException(); + GetDistance(orientationA, positionB - positionA, orientationB, prestep.LocalOffsetA, prestep.LocalOffsetB, out var anchorOffsetA, out var anchorOffsetB, out var anchorOffset, out var distance); + Vector3Wide.Scale(anchorOffset, Vector.One / distance, out var direction); + ComputeJacobian(distance, anchorOffsetA, anchorOffsetB, ref direction, out var angularJA, out var angularJB); + Symmetric3x3Wide.TransformWithoutOverlap(angularJA, inertiaA.InverseInertiaTensor, out var angularImpulseToVelocityA); + Symmetric3x3Wide.TransformWithoutOverlap(angularJB, inertiaB.InverseInertiaTensor, out var angularImpulseToVelocityB); + ApplyImpulse(inertiaA.InverseMass, inertiaB.InverseMass, direction, angularImpulseToVelocityA, angularImpulseToVelocityB, accumulatedImpulses, ref wsvA, ref wsvB); } public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref DistanceServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - throw new NotImplementedException(); + GetDistance(orientationA, positionB - positionA, orientationB, prestep.LocalOffsetA, prestep.LocalOffsetB, out var anchorOffsetA, out var anchorOffsetB, out var anchorOffset, out var distance); + + Vector3Wide.Scale(anchorOffset, Vector.One / distance, out var direction); + + ComputeTransforms(inertiaA, inertiaB, anchorOffsetA, anchorOffsetB, distance, ref direction, dt, prestep.SpringSettings, + out var positionErrorToVelocity, out var softnessImpulseScale, out var effectiveMass, out var angularJA, out var angularJB, out var angularImpulseToVelocityA, out var angularImpulseToVelocityB); + + //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. + var error = distance - prestep.TargetDistance; + ServoSettingsWide.ComputeClampedBiasVelocity(error, positionErrorToVelocity, prestep.ServoSettings, dt, inverseDt, out var clampedBiasVelocity, out var maximumImpulse); + + //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); + Vector3Wide.Dot(wsvA.Linear, direction, out var linearCSVA); + Vector3Wide.Dot(wsvB.Linear, direction, out var negatedLinearCSVB); + Vector3Wide.Dot(wsvA.Angular, angularJA, out var angularCSVA); + Vector3Wide.Dot(wsvB.Angular, angularJB, out var angularCSVB); + var csi = (clampedBiasVelocity - linearCSVA - angularCSVA + negatedLinearCSVB - angularCSVB) * effectiveMass - accumulatedImpulses * softnessImpulseScale; + ServoSettingsWide.ClampImpulse(maximumImpulse, ref accumulatedImpulses, ref csi); + + ApplyImpulse(inertiaA.InverseMass, inertiaB.InverseMass, direction, angularImpulseToVelocityA, angularImpulseToVelocityB, csi, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] diff --git a/Demos/SpecializedTests/ConstraintTestDemo.cs b/Demos/SpecializedTests/ConstraintTestDemo.cs index c179871c3..ad317558a 100644 --- a/Demos/SpecializedTests/ConstraintTestDemo.cs +++ b/Demos/SpecializedTests/ConstraintTestDemo.cs @@ -182,14 +182,14 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Simulation.Solver.Add(cHandle, dHandle, new CenterDistanceConstraint(Vector3.Distance(c, d), distanceSpringiness)); Simulation.Solver.Add(aHandle, bHandle, cHandle, dHandle, new VolumeConstraint(a, b, c, d, new SpringSettings(30, 1))); } - //{ - // var x = GetNextPosition(ref nextX); - // var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); - // var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); - // var a = Simulation.Bodies.Add(aDescription); - // var b = Simulation.Bodies.Add(bDescription); - // Simulation.Solver.Add(a, b, new DistanceServo(new Vector3(0, 0.55f, 0), new Vector3(0, -0.55f, 0), 1.9f, new SpringSettings(30, 1), ServoSettings.Default)); - //} + { + var x = GetNextPosition(ref nextX); + var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); + var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); + var a = Simulation.Bodies.Add(aDescription); + var b = Simulation.Bodies.Add(bDescription); + Simulation.Solver.Add(a, b, new DistanceServo(new Vector3(0, 0.55f, 0), new Vector3(0, -0.55f, 0), 1.9f, new SpringSettings(30, 1), ServoSettings.Default)); + } { var x = GetNextPosition(ref nextX); var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); From ec9ba86a5326a5969198e650fa62685adc5f5485 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 28 Sep 2021 21:17:22 -0500 Subject: [PATCH 183/947] AreaConstraint twoified. --- BepuPhysics/Constraints/AreaConstraint.cs | 97 +++++++++++++++++-- .../Constraints/ThreeBodyTypeProcessor.cs | 4 +- Demos/SpecializedTests/ConstraintTestDemo.cs | 42 ++++---- 3 files changed, 112 insertions(+), 31 deletions(-) diff --git a/BepuPhysics/Constraints/AreaConstraint.cs b/BepuPhysics/Constraints/AreaConstraint.cs index 58834847b..02011c75f 100644 --- a/BepuPhysics/Constraints/AreaConstraint.cs +++ b/BepuPhysics/Constraints/AreaConstraint.cs @@ -89,8 +89,8 @@ public struct AreaConstraintFunctions : IThreeBodyConstraintFunctions accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC) + [MethodImpl(MethodImplOptions.AggressiveInlining)] + private static void ApplyImpulse(in Vector inverseMassA, in Vector inverseMassB, in Vector inverseMassC, + in Vector3Wide negatedJacobianA, in Vector3Wide jacobianB, in Vector3Wide jacobianC, in Vector impulse, + ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref BodyVelocityWide velocityC) { - throw new NotImplementedException(); + Vector3Wide.Scale(negatedJacobianA, inverseMassA * impulse, out var negativeVelocityChangeA); + Vector3Wide.Scale(jacobianB, inverseMassB * impulse, out var velocityChangeB); + Vector3Wide.Scale(jacobianC, inverseMassC * impulse, out var velocityChangeC); + Vector3Wide.Subtract(velocityA.Linear, negativeVelocityChangeA, out velocityA.Linear); + Vector3Wide.Add(velocityB.Linear, velocityChangeB, out velocityB.Linear); + Vector3Wide.Add(velocityC.Linear, velocityChangeC, out velocityC.Linear); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide ac, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, float dt, float inverseDt, ref AreaConstraintPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC) + [MethodImpl(MethodImplOptions.AggressiveInlining)] + static void ComputeJacobian(in Vector3Wide positionA, in Vector3Wide positionB, in Vector3Wide positionC, out Vector normalLength, out Vector3Wide negatedJacobianA, out Vector3Wide jacobianB, out Vector3Wide jacobianC) { - throw new NotImplementedException(); + //Area of a triangle with vertices a, b, and c is: + //||ab x ac|| * 0.5 + //So the constraint is: + //OriginalArea * 2 = ||ab x ac|| + //Leading to a velocity constraint: + //d/dt(OriginalArea * 2) = d/dt(||ab x ac||) + //0 = d/dt(dot(ab x ac, ab x ac)^0.5) + //0 = d/dt(dot(ab x ac, ab x ac)) * 0.5 * dot(ab x ac, ab x ac)^-0.5) + //0 = (dot(d/dt(ab x ac), ab x ac) + dot(ab x ac, d/dt(ab x ac))) * 0.5 * dot(ab x ac, ab x ac)^-0.5) + //0 = (2 * dot(d/dt(ab x ac), ab x ac)) * 0.5 * dot(ab x ac, ab x ac)^-0.5) + //0 = (2 * dot(d/dt(ab) x ac + ab x d/dt(ac), ab x ac)) * 0.5 * dot(ab x ac, ab x ac)^-0.5) + //0 = dot(d/dt(ab) x ac + ab x d/dt(ac), ab x ac) * dot(ab x ac, ab x ac)^-0.5) + //0 = dot(d/dt(ab) x ac + ab x d/dt(ac), ab x ac) / ||ab x ac|| + //0 = (dot(d/dt(ab) x ac, ab x ac) + dot(ab x d/dt(ac), ab x ac)) / ||ab x ac|| + //0 = (dot(ac x (ab x ac), d/dt(ab)) + dot((ab x ac) x ab, d/dt(ac))) / ||ab x ac|| + //0 = dot(ac x ((ab x ac) / ||ab x ac||), d/dt(ab)) + dot(((ab x ac) / ||ab x ac||) x ab, d/dt(ac)) + var ab = positionB - positionA; + var ac = positionC - positionA; + Vector3Wide.CrossWithoutOverlap(ab, ac, out var abxac); + Vector3Wide.Length(abxac, out normalLength); + //The triangle normal length can be zero if the edges are parallel or antiparallel. Protect against the potential division by zero. + Vector3Wide.Scale(abxac, Vector.ConditionalSelect(Vector.GreaterThan(normalLength, new Vector(1e-10f)), Vector.One / normalLength, Vector.Zero), out var normal); + + Vector3Wide.CrossWithoutOverlap(ac, normal, out jacobianB); + Vector3Wide.CrossWithoutOverlap(normal, ab, out jacobianC); + //Similar to the volume constraint, we could create a similar expression for jacobianA, but it's cheap to just do a couple of adds. + Vector3Wide.Add(jacobianB, jacobianC, out negatedJacobianA); + } + + public void WarmStart2( + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, + in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, + ref AreaConstraintPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC) + { + ComputeJacobian(positionA, positionB, positionC, out _, out var negatedJacobianA, out var jacobianB, out var jacobianC); + ApplyImpulse(inertiaA.InverseMass, inertiaB.InverseMass, inertiaC.InverseMass, negatedJacobianA, jacobianB, jacobianC, accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC); + } + + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, float dt, float inverseDt, ref AreaConstraintPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC) + { + ComputeJacobian(positionA, positionB, positionC, out var normalLength, out var negatedJacobianA, out var jacobianB, out var jacobianC); + + Vector3Wide.Dot(negatedJacobianA, negatedJacobianA, out var contributionA); + Vector3Wide.Dot(jacobianB, jacobianB, out var contributionB); + Vector3Wide.Dot(jacobianC, jacobianC, out var contributionC); + + //Protect against singularity by padding the jacobian contributions. This is very much a hack, but it's a pretty simple hack. + //Less sensitive to tuning than attempting to guard the inverseEffectiveMass itself, since that is sensitive to both scale AND mass. + + //Choose an epsilon based on the target area. Note that area ~= width^2 and our jacobian contributions are things like (ac x N) * (ac x N). + //Given that N is perpendicular to AC, ||(ac x N)|| == ||ac||, so the contribution is just ||ac||^2. Given the square, it's proportional to area and the area is a decent epsilon source. + var epsilon = 5e-4f * prestep.TargetScaledArea; + contributionA = Vector.Max(epsilon, contributionA); + contributionB = Vector.Max(epsilon, contributionB); + contributionC = Vector.Max(epsilon, contributionC); + var inverseEffectiveMass = contributionA * inertiaA.InverseMass + contributionB * inertiaB.InverseMass + contributionC * inertiaC.InverseMass; + + SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); + + var effectiveMass = effectiveMassCFMScale / inverseEffectiveMass; + //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. + var biasVelocity = (prestep.TargetScaledArea - normalLength) * positionErrorToVelocity; + + //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); + Vector3Wide.Dot(negatedJacobianA, wsvA.Linear, out var negatedVelocityContributionA); + Vector3Wide.Dot(jacobianB, wsvB.Linear, out var velocityContributionB); + Vector3Wide.Dot(jacobianC, wsvC.Linear, out var velocityContributionC); + var csv = velocityContributionB + velocityContributionC - negatedVelocityContributionA; + var csi = (biasVelocity - csv) * effectiveMass - accumulatedImpulses * softnessImpulseScale; + accumulatedImpulses += csi; + + ApplyImpulse(inertiaA.InverseMass, inertiaB.InverseMass, inertiaC.InverseMass, negatedJacobianA, jacobianB, jacobianC, csi, ref wsvA, ref wsvB, ref wsvC); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in BodyVelocityWide wsvC, in Vector dt, in Vector accumulatedImpulses, ref AreaConstraintPrestepData prestep) { diff --git a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs index 037525f7d..243e917c4 100644 --- a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs @@ -347,8 +347,8 @@ public unsafe override void WarmStart2(bodies, ref integratorCallbacks, ref integrationFlags, 2, dt, workerIndex, i, ref references.IndexC, count, out var positionC, out var orientationC, out var wsvC, out var inertiaC); - if (typeof(TAllowPoseIntegration) == typeof(AllowPoseIntegration)) - function.UpdateForNewPose(positionA, orientationA, inertiaA, wsvA, positionB, orientationB, inertiaB, wsvB, positionC, orientationC, inertiaC, wsvC, new Vector(dt), accumulatedImpulses, ref prestep); + //if (typeof(TAllowPoseIntegration) == typeof(AllowPoseIntegration)) + // function.UpdateForNewPose(positionA, orientationA, inertiaA, wsvA, positionB, orientationB, inertiaB, wsvB, positionC, orientationC, inertiaC, wsvC, new Vector(dt), accumulatedImpulses, ref prestep); function.WarmStart2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC); diff --git a/Demos/SpecializedTests/ConstraintTestDemo.cs b/Demos/SpecializedTests/ConstraintTestDemo.cs index ad317558a..c61929b50 100644 --- a/Demos/SpecializedTests/ConstraintTestDemo.cs +++ b/Demos/SpecializedTests/ConstraintTestDemo.cs @@ -198,27 +198,27 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var b = Simulation.Bodies.Add(bDescription); Simulation.Solver.Add(a, b, new DistanceLimit(new Vector3(0, 0.55f, 0), new Vector3(0, -0.55f, 0), 1f, 3, new SpringSettings(30, 1))); } - //{ - // var x = GetNextPosition(ref nextX); - // var sphere = new Sphere(0.125f); - // //Treat each vertex as a point mass that cannot rotate. - // var sphereInertia = new BodyInertia { InverseMass = 1 }; - // var sphereCollidable = new CollidableDescription(Simulation.Shapes.Add(sphere), 0.1f); - // var a = new Vector3(x, 3, 0); - // var b = new Vector3(x, 4, 0); - // var c = new Vector3(x + 1, 3, 0); - // var aDescription = BodyDescription.CreateDynamic(a, sphereInertia, sphereCollidable, activity); - // var bDescription = BodyDescription.CreateDynamic(b, sphereInertia, sphereCollidable, activity); - // var cDescription = BodyDescription.CreateDynamic(c, sphereInertia, sphereCollidable, activity); - // var aHandle = Simulation.Bodies.Add(aDescription); - // var bHandle = Simulation.Bodies.Add(bDescription); - // var cHandle = Simulation.Bodies.Add(cDescription); - // var distanceSpringiness = new SpringSettings(3f, 1); - // Simulation.Solver.Add(aHandle, bHandle, new CenterDistanceConstraint(Vector3.Distance(a, b), distanceSpringiness)); - // Simulation.Solver.Add(aHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(a, c), distanceSpringiness)); - // Simulation.Solver.Add(bHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(b, c), distanceSpringiness)); - // Simulation.Solver.Add(aHandle, bHandle, cHandle, new AreaConstraint(a, b, c, new SpringSettings(30, 1))); - //} + { + var x = GetNextPosition(ref nextX); + var sphere = new Sphere(0.125f); + //Treat each vertex as a point mass that cannot rotate. + var sphereInertia = new BodyInertia { InverseMass = 1 }; + var sphereCollidable = new CollidableDescription(Simulation.Shapes.Add(sphere), 0.1f); + var a = new Vector3(x, 3, 0); + var b = new Vector3(x, 4, 0); + var c = new Vector3(x + 1, 3, 0); + var aDescription = BodyDescription.CreateDynamic(a, sphereInertia, sphereCollidable, activity); + var bDescription = BodyDescription.CreateDynamic(b, sphereInertia, sphereCollidable, activity); + var cDescription = BodyDescription.CreateDynamic(c, sphereInertia, sphereCollidable, activity); + var aHandle = Simulation.Bodies.Add(aDescription); + var bHandle = Simulation.Bodies.Add(bDescription); + var cHandle = Simulation.Bodies.Add(cDescription); + var distanceSpringiness = new SpringSettings(3f, 1); + Simulation.Solver.Add(aHandle, bHandle, new CenterDistanceConstraint(Vector3.Distance(a, b), distanceSpringiness)); + Simulation.Solver.Add(aHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(a, c), distanceSpringiness)); + Simulation.Solver.Add(bHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(b, c), distanceSpringiness)); + Simulation.Solver.Add(aHandle, bHandle, cHandle, new AreaConstraint(a, b, c, new SpringSettings(30, 1))); + } { var x = GetNextPosition(ref nextX); var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), default, collidableA, activity); From 4d3db2cb54ba25ca583c54f85f1d86fd90fad32b Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 28 Sep 2021 21:28:48 -0500 Subject: [PATCH 184/947] OneBodyLinearMotor twoified. --- BepuPhysics/Constraints/OneBodyLinearMotor.cs | 27 ++++++++++++++++--- Demos/SpecializedTests/ConstraintTestDemo.cs | 24 ++++++++--------- 2 files changed, 35 insertions(+), 16 deletions(-) diff --git a/BepuPhysics/Constraints/OneBodyLinearMotor.cs b/BepuPhysics/Constraints/OneBodyLinearMotor.cs index fa1fb3293..f76acdb09 100644 --- a/BepuPhysics/Constraints/OneBodyLinearMotor.cs +++ b/BepuPhysics/Constraints/OneBodyLinearMotor.cs @@ -94,12 +94,32 @@ public void Solve(ref BodyVelocityWide velocityA, ref OneBodyLinearServoProjecti public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref OneBodyLinearMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) { - throw new NotImplementedException(); + QuaternionWide.TransformWithoutOverlap(prestep.LocalOffset, orientationA, out var offset); + OneBodyLinearServoFunctions.ApplyImpulse(offset, inertiaA, ref wsvA, accumulatedImpulses); } public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref OneBodyLinearMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) { - throw new NotImplementedException(); + QuaternionWide.TransformWithoutOverlap(prestep.LocalOffset, orientationA, out var offset); + MotorSettingsWide.ComputeSoftness(prestep.Settings, dt, out var effectiveMassCFMScale, out var softnessImpulseScale, out var maximumImpulse); + + //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular); + var csv = prestep.TargetVelocity - Vector3Wide.Cross(wsvA.Angular, offset) - wsvA.Linear; + + //The grabber is roughly equivalent to a ball socket joint with a nonzero goal (and only one body). + Symmetric3x3Wide.SkewSandwichWithoutOverlap(offset, inertiaA.InverseInertiaTensor, out var inverseEffectiveMass); + + //Linear contributions are simply I * inverseMass * I, which is just boosting the diagonal. + inverseEffectiveMass.XX += inertiaA.InverseMass; + inverseEffectiveMass.YY += inertiaA.InverseMass; + inverseEffectiveMass.ZZ += inertiaA.InverseMass; + Symmetric3x3Wide.Invert(inverseEffectiveMass, out var effectiveMass); + Symmetric3x3Wide.TransformWithoutOverlap(csv, effectiveMass, out var csi); + csi = csi * effectiveMassCFMScale - accumulatedImpulses * softnessImpulseScale; + + //The motor has a limited maximum force, so clamp the accumulated impulse. Watch out for division by zero. + ServoSettingsWide.ClampImpulse(maximumImpulse, ref accumulatedImpulses, ref csi); + OneBodyLinearServoFunctions.ApplyImpulse(offset, inertiaA, ref wsvA, csi); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -109,8 +129,7 @@ public void UpdateForNewPose( { } } - - public class OneBodyLinearMotorTypeProcessor : OneBodyTypeProcessor + public class OneBodyLinearMotorTypeProcessor : OneBodyTypeProcessor { public const int BatchTypeId = 45; } diff --git a/Demos/SpecializedTests/ConstraintTestDemo.cs b/Demos/SpecializedTests/ConstraintTestDemo.cs index c61929b50..e34cf386b 100644 --- a/Demos/SpecializedTests/ConstraintTestDemo.cs +++ b/Demos/SpecializedTests/ConstraintTestDemo.cs @@ -320,18 +320,18 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) SpringSettings = new SpringSettings(5, 1) }); } - //{ - // var x = GetNextPosition(ref nextX); - // var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); - // var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - // Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - // Simulation.Solver.Add(a, new OneBodyLinearMotor - // { - // LocalOffset = new Vector3(0, 1, 0), - // TargetVelocity = new Vector3(0, -1, 0), - // Settings = new MotorSettings(float.MaxValue, 0.01f), - // }); - //} + { + var x = GetNextPosition(ref nextX); + var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); + var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + Simulation.Solver.Add(a, new OneBodyLinearMotor + { + LocalOffset = new Vector3(0, 1, 0), + TargetVelocity = new Vector3(0, -1, 0), + Settings = new MotorSettings(float.MaxValue, 1e-2f), + }); + } { var x = GetNextPosition(ref nextX); var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); From 62befaa39fef23684bb148f12b6bd0a599c2d25b Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 29 Sep 2021 13:06:51 -0500 Subject: [PATCH 185/947] OneBodyAngularMotor twoified. --- .../Constraints/OneBodyAngularMotor.cs | 16 +++++++++++--- BepuPhysics/Constraints/OneBodyLinearMotor.cs | 1 - Demos/SpecializedTests/ConstraintTestDemo.cs | 22 +++++++++---------- 3 files changed, 24 insertions(+), 15 deletions(-) diff --git a/BepuPhysics/Constraints/OneBodyAngularMotor.cs b/BepuPhysics/Constraints/OneBodyAngularMotor.cs index d1e1f8f94..e9c91e4fa 100644 --- a/BepuPhysics/Constraints/OneBodyAngularMotor.cs +++ b/BepuPhysics/Constraints/OneBodyAngularMotor.cs @@ -112,12 +112,22 @@ public void Solve(ref BodyVelocityWide velocityA, ref OneBodyAngularServoProject public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref OneBodyAngularMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) { - throw new NotImplementedException(); + ApplyImpulse(ref wsvA.Angular, inertiaA.InverseInertiaTensor, accumulatedImpulses); } public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref OneBodyAngularMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) { - throw new NotImplementedException(); + //Jacobians are just the identity matrix. + MotorSettingsWide.ComputeSoftness(prestep.Settings, dt, out var effectiveMassCFMScale, out var softnessImpulseScale, out var maximumImpulse); + Symmetric3x3Wide.Invert(inertiaA.InverseInertiaTensor, out var unsoftenedEffectiveMass); + + //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - csiaAngular; + Symmetric3x3Wide.TransformWithoutOverlap(prestep.TargetVelocity - wsvA.Angular, unsoftenedEffectiveMass, out var csi); + csi = csi * effectiveMassCFMScale - accumulatedImpulses * softnessImpulseScale; + + ServoSettingsWide.ClampImpulse(maximumImpulse, ref accumulatedImpulses, ref csi); + + ApplyImpulse(ref wsvA.Angular, inertiaA.InverseInertiaTensor, csi); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -128,7 +138,7 @@ public void UpdateForNewPose( } } - public class OneBodyAngularMotorTypeProcessor : OneBodyTypeProcessor + public class OneBodyAngularMotorTypeProcessor : OneBodyTypeProcessor { public const int BatchTypeId = 43; } diff --git a/BepuPhysics/Constraints/OneBodyLinearMotor.cs b/BepuPhysics/Constraints/OneBodyLinearMotor.cs index f76acdb09..387112a70 100644 --- a/BepuPhysics/Constraints/OneBodyLinearMotor.cs +++ b/BepuPhysics/Constraints/OneBodyLinearMotor.cs @@ -117,7 +117,6 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in Symmetric3x3Wide.TransformWithoutOverlap(csv, effectiveMass, out var csi); csi = csi * effectiveMassCFMScale - accumulatedImpulses * softnessImpulseScale; - //The motor has a limited maximum force, so clamp the accumulated impulse. Watch out for division by zero. ServoSettingsWide.ClampImpulse(maximumImpulse, ref accumulatedImpulses, ref csi); OneBodyLinearServoFunctions.ApplyImpulse(offset, inertiaA, ref wsvA, csi); } diff --git a/Demos/SpecializedTests/ConstraintTestDemo.cs b/Demos/SpecializedTests/ConstraintTestDemo.cs index e34cf386b..148918fcc 100644 --- a/Demos/SpecializedTests/ConstraintTestDemo.cs +++ b/Demos/SpecializedTests/ConstraintTestDemo.cs @@ -344,17 +344,17 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) SpringSettings = new SpringSettings(30f, 1f) }); } - //{ - // var x = GetNextPosition(ref nextX); - // var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); - // var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - // Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - // Simulation.Solver.Add(a, new OneBodyAngularMotor - // { - // TargetVelocity = new Vector3(1, 0, 0), - // Settings = new MotorSettings(float.MaxValue, 0.001f), - // }); - //} + { + var x = GetNextPosition(ref nextX); + var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); + var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + Simulation.Solver.Add(a, new OneBodyAngularMotor + { + TargetVelocity = new Vector3(1, 0, 0), + Settings = new MotorSettings(float.MaxValue, 0.001f), + }); + } { var x = GetNextPosition(ref nextX); var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); From a32ab116b4149a218e71b8d375725bc48f87dc84 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 29 Sep 2021 13:23:40 -0500 Subject: [PATCH 186/947] AngularAxisGearMotor twoified. All 2.3-era constraints now twoified. --- .../Constraints/AngularAxisGearMotor.cs | 36 +++++----- Demos/SpecializedTests/ConstraintTestDemo.cs | 67 ++++++++++--------- 2 files changed, 55 insertions(+), 48 deletions(-) diff --git a/BepuPhysics/Constraints/AngularAxisGearMotor.cs b/BepuPhysics/Constraints/AngularAxisGearMotor.cs index 29d777f9e..005a7c412 100644 --- a/BepuPhysics/Constraints/AngularAxisGearMotor.cs +++ b/BepuPhysics/Constraints/AngularAxisGearMotor.cs @@ -130,48 +130,54 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ApplyImpulse(in Vector3Wide jA, in Vector3Wide negatedJB, in Vector csi, in Symmetric3x3Wide inertiaA, in Symmetric3x3Wide inertiaB, ref Vector3Wide angularVelocityA, ref Vector3Wide angularVelocityB) + public static void ApplyImpulse(in Vector3Wide impulseToVelocityA, in Vector3Wide negatedImpulseToVelocityB, in Vector csi, ref Vector3Wide angularVelocityA, ref Vector3Wide angularVelocityB) { - Symmetric3x3Wide.TransformWithoutOverlap(jA * csi, inertiaA, out var changeA); - angularVelocityA += changeA; - Symmetric3x3Wide.TransformWithoutOverlap(negatedJB * csi, inertiaB, out var negatedChangeB); - angularVelocityB -= negatedChangeB; + angularVelocityA += impulseToVelocityA * csi; + angularVelocityB -= negatedImpulseToVelocityB * csi; } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref AngularAxisGearMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { QuaternionWide.TransformWithoutOverlap(prestep.LocalAxisA, orientationA, out var axis); - ApplyImpulse(axis * prestep.VelocityScale, axis, accumulatedImpulses, inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, ref wsvA.Angular, ref wsvB.Angular); + Vector3Wide.Scale(axis, prestep.VelocityScale, out var jA); + Symmetric3x3Wide.TransformWithoutOverlap(jA, inertiaA.InverseInertiaTensor, out var impulseToVelocityA); + Symmetric3x3Wide.TransformWithoutOverlap(axis, inertiaB.InverseInertiaTensor, out var negatedImpulseToVelocityB); + ApplyImpulse(impulseToVelocityA, negatedImpulseToVelocityB, accumulatedImpulses, ref wsvA.Angular, ref wsvB.Angular); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref AngularAxisGearMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { + //This is mildly more complex than the AngularAxisMotor: + //dot(wa, axis) * velocityScale - dot(wb, axis) = 0, so jacobianA is actually axis * velocityScale, not just -axis. QuaternionWide.TransformWithoutOverlap(prestep.LocalAxisA, orientationA, out var axis); Vector3Wide.Scale(axis, prestep.VelocityScale, out var jA); - Symmetric3x3Wide.TransformWithoutOverlap(jA, inertiaA.InverseInertiaTensor, out var jIA); - Vector3Wide.Dot(jA, jIA, out var contributionA); - Symmetric3x3Wide.TransformWithoutOverlap(axis, inertiaB.InverseInertiaTensor, out var jIB); - Vector3Wide.Dot(axis, jIB, out var contributionB); + Symmetric3x3Wide.TransformWithoutOverlap(jA, inertiaA.InverseInertiaTensor, out var impulseToVelocityA); + Vector3Wide.Dot(jA, impulseToVelocityA, out var contributionA); + Symmetric3x3Wide.TransformWithoutOverlap(axis, inertiaB.InverseInertiaTensor, out var negatedImpulseToVelocityB); + Vector3Wide.Dot(axis, negatedImpulseToVelocityB, out var contributionB); MotorSettingsWide.ComputeSoftness(prestep.Settings, dt, out var effectiveMassCFMScale, out var softnessImpulseScale, out var maximumImpulse); + var effectiveMass = effectiveMassCFMScale / (contributionA + contributionB); - //csi = projection.BiasImpulse - accumulatedImpulse * softnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); - var csi = effectiveMassCFMScale * (Vector3Wide.Dot(wsvB.Angular, axis) - Vector3Wide.Dot(wsvA.Angular, axis)) / (contributionA + contributionB) - accumulatedImpulses * softnessImpulseScale; + //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); + Vector3Wide.Dot(wsvA.Angular, jA, out var unscaledCSVA); + Vector3Wide.Dot(wsvB.Angular, axis, out var negatedCSVB); + var csi = (negatedCSVB - unscaledCSVA) * effectiveMass - accumulatedImpulses * softnessImpulseScale; ServoSettingsWide.ClampImpulse(maximumImpulse, ref accumulatedImpulses, ref csi); - ApplyImpulse(jA, axis, accumulatedImpulses, inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, ref wsvA.Angular, ref wsvB.Angular); + ApplyImpulse(impulseToVelocityA, negatedImpulseToVelocityB, accumulatedImpulses, ref wsvA.Angular, ref wsvB.Angular); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, in Vector dt, in Vector accumulatedImpulses, ref AngularAxisGearMotorPrestepData prestep) { } } - public class AngularAxisGearMotorTypeProcessor : TwoBodyTypeProcessor, AngularAxisGearMotorFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular> + public class AngularAxisGearMotorTypeProcessor : TwoBodyTypeProcessor, AngularAxisGearMotorFunctions, AccessOnlyAngular, AccessOnlyAngularWithoutPose, AccessOnlyAngular, AccessOnlyAngularWithoutPose> { public const int BatchTypeId = 54; } diff --git a/Demos/SpecializedTests/ConstraintTestDemo.cs b/Demos/SpecializedTests/ConstraintTestDemo.cs index 148918fcc..85eecea8c 100644 --- a/Demos/SpecializedTests/ConstraintTestDemo.cs +++ b/Demos/SpecializedTests/ConstraintTestDemo.cs @@ -6,6 +6,7 @@ using DemoContentLoader; using BepuPhysics.Constraints; using Demos.Demos; +using System; namespace Demos.SpecializedTests { @@ -382,39 +383,39 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) ServoSettings = new ServoSettings(100, 1, 100) }); } - //{ - // var x = GetNextPosition(ref nextX); - // var wheelShape = new CollidableDescription(Simulation.Shapes.Add(new Cylinder(1, 0.1f)), 0.1f); - // var wheelOrientation = Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), MathF.PI * 0.5f); - // var aDescription = BodyDescription.CreateDynamic(new RigidPose(new Vector3(x, 3, 0), wheelOrientation), inertiaA, wheelShape, activity); - // var bDescription = BodyDescription.CreateDynamic(new RigidPose(new Vector3(x, 6, 0), wheelOrientation), inertiaB, wheelShape, activity); - // var cDescription = BodyDescription.CreateKinematic(new Vector3(x, 4.5f, -1), new CollidableDescription(Simulation.Shapes.Add(new Box(3, 6, 1)), 0.1f), activity); - // var a = Simulation.Bodies.Add(aDescription); - // var b = Simulation.Bodies.Add(bDescription); - // var c = Simulation.Bodies.Add(cDescription); - // Simulation.Solver.Add(a, b, new AngularAxisGearMotor - // { - // LocalAxisA = new Vector3(0, 1, 0), - // VelocityScale = -4, - // Settings = new MotorSettings(float.MaxValue, 0.0001f) - // }); - // Simulation.Solver.Add(c, a, new Hinge - // { - // LocalOffsetA = new Vector3(0, -1.5f, 1), - // LocalHingeAxisA = new Vector3(0, 0, 1), - // LocalOffsetB = new Vector3(0, 0, 0), - // LocalHingeAxisB = new Vector3(0, 1, 0), - // SpringSettings = new SpringSettings(30, 1) - // }); - // Simulation.Solver.Add(c, b, new Hinge - // { - // LocalOffsetA = new Vector3(0, 1.5f, 1), - // LocalHingeAxisA = new Vector3(0, 0, 1), - // LocalOffsetB = new Vector3(0, 0, 0), - // LocalHingeAxisB = new Vector3(0, 1, 0), - // SpringSettings = new SpringSettings(30, 1) - // }); - //} + { + var x = GetNextPosition(ref nextX); + var wheelShape = new CollidableDescription(Simulation.Shapes.Add(new Cylinder(1, 0.1f)), 0.1f); + var wheelOrientation = Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), MathF.PI * 0.5f); + var aDescription = BodyDescription.CreateDynamic(new RigidPose(new Vector3(x, 3, 0), wheelOrientation), inertiaA, wheelShape, activity); + var bDescription = BodyDescription.CreateDynamic(new RigidPose(new Vector3(x, 6, 0), wheelOrientation), inertiaB, wheelShape, activity); + var cDescription = BodyDescription.CreateKinematic(new Vector3(x, 4.5f, -1), new CollidableDescription(Simulation.Shapes.Add(new Box(3, 6, 1)), 0.1f), activity); + var a = Simulation.Bodies.Add(aDescription); + var b = Simulation.Bodies.Add(bDescription); + var c = Simulation.Bodies.Add(cDescription); + Simulation.Solver.Add(a, b, new AngularAxisGearMotor + { + LocalAxisA = new Vector3(0, 1, 0), + VelocityScale = -4, + Settings = new MotorSettings(float.MaxValue, 0.0001f) + }); + Simulation.Solver.Add(c, a, new Hinge + { + LocalOffsetA = new Vector3(0, -1.5f, 1), + LocalHingeAxisA = new Vector3(0, 0, 1), + LocalOffsetB = new Vector3(0, 0, 0), + LocalHingeAxisB = new Vector3(0, 1, 0), + SpringSettings = new SpringSettings(30, 1) + }); + Simulation.Solver.Add(c, b, new Hinge + { + LocalOffsetA = new Vector3(0, 1.5f, 1), + LocalHingeAxisA = new Vector3(0, 0, 1), + LocalOffsetB = new Vector3(0, 0, 0), + LocalHingeAxisB = new Vector3(0, 1, 0), + SpringSettings = new SpringSettings(30, 1) + }); + } Simulation.Statics.Add(new StaticDescription(new Vector3(), new CollidableDescription(Simulation.Shapes.Add(new Box(256, 1, 256)), 0.1f))); } From 533dfc90ccc2285f57966f19915bc63affe75150 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 29 Sep 2021 16:45:27 -0500 Subject: [PATCH 187/947] AngularAxisMotor fixed. --- BepuPhysics/Constraints/AngularAxisMotor.cs | 17 ++++++++--------- Demos/Demo.cs | 2 +- Demos/Demos/BlockChainDemo.cs | 2 +- Demos/Demos/Cars/CarDemo.cs | 2 +- Demos/Demos/RagdollDemo.cs | 2 +- Demos/Demos/Tanks/TankDemo.cs | 2 +- 6 files changed, 13 insertions(+), 14 deletions(-) diff --git a/BepuPhysics/Constraints/AngularAxisMotor.cs b/BepuPhysics/Constraints/AngularAxisMotor.cs index 59fe035a1..cb314005d 100644 --- a/BepuPhysics/Constraints/AngularAxisMotor.cs +++ b/BepuPhysics/Constraints/AngularAxisMotor.cs @@ -126,20 +126,19 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ApplyImpulse(in Vector3Wide jA, in Vector csi, in Symmetric3x3Wide inertiaA, in Symmetric3x3Wide inertiaB, ref Vector3Wide angularVelocityA, ref Vector3Wide angularVelocityB) + public static void ApplyImpulse(in Vector3Wide impulseToVelocityA, in Vector3Wide negatedImpulseToVelocityB, in Vector csi, ref Vector3Wide angularVelocityA, ref Vector3Wide angularVelocityB) { - var wsiA = jA * csi; - Symmetric3x3Wide.TransformWithoutOverlap(wsiA, inertiaA, out var changeA); - angularVelocityA += changeA; - Symmetric3x3Wide.TransformWithoutOverlap(wsiA, inertiaB, out var negatedChangeB); - angularVelocityB -= negatedChangeB; + angularVelocityA += impulseToVelocityA * csi; + angularVelocityB -= negatedImpulseToVelocityB * csi; } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref AngularAxisMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { QuaternionWide.TransformWithoutOverlap(prestep.LocalAxisA, orientationA, out var axis); - ApplyImpulse(axis, accumulatedImpulses, inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, ref wsvA.Angular, ref wsvB.Angular); + Symmetric3x3Wide.TransformWithoutOverlap(axis, inertiaA.InverseInertiaTensor, out var jIA); + Symmetric3x3Wide.TransformWithoutOverlap(axis, inertiaB.InverseInertiaTensor, out var jIB); + ApplyImpulse(jIA, jIB, accumulatedImpulses, ref wsvA.Angular, ref wsvB.Angular); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -153,9 +152,9 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in MotorSettingsWide.ComputeSoftness(prestep.Settings, dt, out var effectiveMassCFMScale, out var softnessImpulseScale, out var maximumImpulse); //csi = projection.BiasImpulse - accumulatedImpulse * softnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); - var csi = effectiveMassCFMScale * (prestep.TargetVelocity + Vector3Wide.Dot(wsvB.Angular, jA) - Vector3Wide.Dot(wsvA.Angular, jA)) / (contributionA + contributionB) - accumulatedImpulses * softnessImpulseScale; + var csi = (prestep.TargetVelocity + Vector3Wide.Dot(wsvB.Angular, jA) - Vector3Wide.Dot(wsvA.Angular, jA)) * effectiveMassCFMScale / (contributionA + contributionB) - accumulatedImpulses * softnessImpulseScale; ServoSettingsWide.ClampImpulse(maximumImpulse, ref accumulatedImpulses, ref csi); - ApplyImpulse(jA, accumulatedImpulses, inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, ref wsvA.Angular, ref wsvB.Angular); + ApplyImpulse(jIA, jIB, csi, ref wsvA.Angular, ref wsvB.Angular); } [MethodImpl(MethodImplOptions.AggressiveInlining)] diff --git a/Demos/Demo.cs b/Demos/Demo.cs index 5f3790da3..37ebe5190 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -62,7 +62,7 @@ public virtual void Update(Window window, Camera camera, Input input, float dt) //fully decouple simulation and rendering rates across different threads. //(In either case, you'd also want to interpolate or extrapolate simulation results during rendering for smoothness.) //Note that taking steps of variable length can reduce stability. Gradual or one-off changes can work reasonably well. - Simulation.Timestep(1 / 60f); + Simulation.Timestep(1 / 60f, ThreadDispatcher); ////Here's an example of how it would look to use more frequent updates, but still with a fixed amount of time simulated per update call: //const float timeToSimulate = 1 / 60f; diff --git a/Demos/Demos/BlockChainDemo.cs b/Demos/Demos/BlockChainDemo.cs index d6aa97b02..c52928e7e 100644 --- a/Demos/Demos/BlockChainDemo.cs +++ b/Demos/Demos/BlockChainDemo.cs @@ -28,7 +28,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //The PositionFirstTimestepper is the simplest timestepping mode, but since it integrates velocity into position at the start of the frame, directly modified velocities outside of the timestep //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), 1); var boxShape = new Box(1, 1, 1); boxShape.ComputeInertia(1, out var boxInertia); diff --git a/Demos/Demos/Cars/CarDemo.cs b/Demos/Demos/Cars/CarDemo.cs index 5bd6512b5..ff1848620 100644 --- a/Demos/Demos/Cars/CarDemo.cs +++ b/Demos/Demos/Cars/CarDemo.cs @@ -45,7 +45,7 @@ public override void Initialize(ContentArchive content, Camera camera) //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. //Simulation = Simulation.Create(BufferPool, new CarCallbacks() { Properties = properties }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); - Simulation = Simulation.Create(BufferPool, new CarCallbacks() { Properties = properties }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(8), 1); + Simulation = Simulation.Create(BufferPool, new CarCallbacks() { Properties = properties }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), 1); var builder = new CompoundBuilder(BufferPool, Simulation.Shapes, 2); diff --git a/Demos/Demos/RagdollDemo.cs b/Demos/Demos/RagdollDemo.cs index 21deb7a01..91a71e186 100644 --- a/Demos/Demos/RagdollDemo.cs +++ b/Demos/Demos/RagdollDemo.cs @@ -537,7 +537,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //The PositionFirstTimestepper is the simplest timestepping mode, but since it integrates velocity into position at the start of the frame, directly modified velocities outside of the timestep //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. - Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks { CollisionFilters = collisionFilters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks { CollisionFilters = collisionFilters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), 1); int ragdollIndex = 0; var spacing = new Vector3(2f, 3, 1); diff --git a/Demos/Demos/Tanks/TankDemo.cs b/Demos/Demos/Tanks/TankDemo.cs index 5b5655a0d..374dd3a75 100644 --- a/Demos/Demos/Tanks/TankDemo.cs +++ b/Demos/Demos/Tanks/TankDemo.cs @@ -51,7 +51,7 @@ public override void Initialize(ContentArchive content, Camera camera) bodyProperties = new CollidableProperty(); //We assign velocities outside of the timestep to fire bullets, so using the PositionLastTimestepper avoids integrating those velocities into positions before the solver has a chance to intervene. //We could have also modified velocities in the PositionFirstTimestepper's BeforeCollisionDetection callback, but it's just a little simpler to do this with very little cost. - Simulation = Simulation.Create(BufferPool, new TankCallbacks() { Properties = bodyProperties }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionLastTimestepper()); + Simulation = Simulation.Create(BufferPool, new TankCallbacks() { Properties = bodyProperties }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), 1); var builder = new CompoundBuilder(BufferPool, Simulation.Shapes, 2); builder.Add(new Box(1.85f, 0.7f, 4.73f), RigidPose.Identity, 10); From 6aaf8f063c25f7fd2e12af7b8a63f84b87b3b31e Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 29 Sep 2021 17:18:05 -0500 Subject: [PATCH 188/947] Purged the unworthy; preparing for embedded substepper fallback batches. --- .../Constraints/FourBodyTypeProcessor.cs | 78 +------ .../Constraints/OneBodyTypeProcessor.cs | 73 +------ .../Constraints/ThreeBodyTypeProcessor.cs | 72 +------ .../Constraints/TwoBodyTypeProcessor.cs | 104 ++++------ BepuPhysics/Constraints/TypeProcessor.cs | 13 +- BepuPhysics/EmbeddedSubsteppingTimestepper.cs | 109 ---------- BepuPhysics/PositionFirstTimestepper2.cs | 74 ------- BepuPhysics/Solver.cs | 5 +- BepuPhysics/Solver_Solve2.cs | 193 ------------------ BepuPhysics/Solver_SubsteppingSolve.cs | 142 ------------- BepuPhysics/SubsteppingTimestepper2.cs | 109 ---------- Demos/DemoSet.cs | 1 + Demos/Demos/PyramidDemo.cs | 2 +- Demos/SpecializedTests/RagdollTubeDemo.cs | 2 +- 14 files changed, 55 insertions(+), 922 deletions(-) delete mode 100644 BepuPhysics/EmbeddedSubsteppingTimestepper.cs delete mode 100644 BepuPhysics/PositionFirstTimestepper2.cs delete mode 100644 BepuPhysics/Solver_Solve2.cs delete mode 100644 BepuPhysics/Solver_SubsteppingSolve.cs delete mode 100644 BepuPhysics/SubsteppingTimestepper2.cs diff --git a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs index 693237bef..3d07a3695 100644 --- a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs @@ -283,79 +283,6 @@ public unsafe override void JacobiSolveIteration(ref TypeBatch typeBatch, Bodies } } - public unsafe override void SolveStep(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) - { - ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); - ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); - ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); - ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); - var function = default(TConstraintFunctions); - for (int i = startBundle; i < exclusiveEndBundle; ++i) - { - ref var prestep = ref Unsafe.Add(ref prestepBase, i); - ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); - ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); - ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); - var count = GetCountInBundle(ref typeBatch, i); - bodies.GatherState(ref references, count, - out var orientationA, out var wsvA, out var inertiaA, - out var ab, out var orientationB, out var wsvB, out var inertiaB, - out var ac, out var orientationC, out var wsvC, out var inertiaC, - out var ad, out var orientationD, out var wsvD, out var inertiaD); - function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, ac, orientationC, inertiaC, ad, orientationD, inertiaD, dt, inverseDt, ref prestep, out var projection); - function.WarmStart(ref wsvA, ref wsvB, ref wsvC, ref wsvD, ref projection, ref accumulatedImpulses); - function.Solve(ref wsvA, ref wsvB, ref wsvC, ref wsvD, ref projection, ref accumulatedImpulses); - bodies.ScatterVelocities(ref wsvA, ref bodyReferences.IndexA, count); - bodies.ScatterVelocities(ref wsvB, ref bodyReferences.IndexB, count); - bodies.ScatterVelocities(ref wsvC, ref bodyReferences.IndexC, count); - bodies.ScatterVelocities(ref wsvD, ref bodyReferences.IndexD, count); - } - } - - public unsafe override void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) - { - ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); - ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); - ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); - ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); - var function = default(TConstraintFunctions); - ref var jacobiResultsBundlesA = ref jacobiResults.GetVelocitiesForBody(0); - ref var jacobiResultsBundlesB = ref jacobiResults.GetVelocitiesForBody(1); - ref var jacobiResultsBundlesC = ref jacobiResults.GetVelocitiesForBody(2); - ref var jacobiResultsBundlesD = ref jacobiResults.GetVelocitiesForBody(3); - for (int i = startBundle; i < exclusiveEndBundle; ++i) - { - ref var prestep = ref Unsafe.Add(ref prestepBase, i); - ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); - ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); - ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); - var count = GetCountInBundle(ref typeBatch, i); - ref var wsvA = ref jacobiResultsBundlesA[i]; - ref var wsvB = ref jacobiResultsBundlesB[i]; - ref var wsvC = ref jacobiResultsBundlesC[i]; - ref var wsvD = ref jacobiResultsBundlesD[i]; - bodies.GatherState(ref references, count, - out var orientationA, out wsvA, out var inertiaA, - out var ab, out var orientationB, out wsvB, out var inertiaB, - out var ac, out var orientationC, out wsvC, out var inertiaC, - out var ad, out var orientationD, out wsvC, out var inertiaD); - //Jacobi batches split affected bodies into multiple pieces to guarantee convergence. - jacobiBatch.GetJacobiScaleForBodies(ref references, count, out var jacobiScaleA, out var jacobiScaleB, out var jacobiScaleC, out var jacobiScaleD); - Symmetric3x3Wide.Scale(inertiaA.InverseInertiaTensor, jacobiScaleA, out inertiaA.InverseInertiaTensor); - inertiaA.InverseMass *= jacobiScaleA; - Symmetric3x3Wide.Scale(inertiaB.InverseInertiaTensor, jacobiScaleB, out inertiaB.InverseInertiaTensor); - inertiaB.InverseMass *= jacobiScaleB; - Symmetric3x3Wide.Scale(inertiaC.InverseInertiaTensor, jacobiScaleC, out inertiaC.InverseInertiaTensor); - inertiaC.InverseMass *= jacobiScaleC; - Symmetric3x3Wide.Scale(inertiaD.InverseInertiaTensor, jacobiScaleD, out inertiaD.InverseInertiaTensor); - inertiaD.InverseMass *= jacobiScaleD; - function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, ac, orientationC, inertiaC, ad, orientationD, inertiaD, dt, inverseDt, ref prestep, out var projection); - function.WarmStart(ref wsvA, ref wsvB, ref wsvC, ref wsvD, ref projection, ref accumulatedImpulses); - function.Solve(ref wsvA, ref wsvB, ref wsvC, ref wsvD, ref projection, ref accumulatedImpulses); - } - } - - public unsafe override void WarmStart2( ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { @@ -431,11 +358,12 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f } } - public override void Prestep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + public override void JacobiWarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks poseIntegratorCallbacks, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { throw new NotImplementedException(); } - public override void JacobiPrestep2(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + + public override void JacobiSolveStep2(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs index 8fcd684b2..7821b26d2 100644 --- a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs @@ -211,56 +211,6 @@ public unsafe override void JacobiSolveIteration(ref TypeBatch typeBatch, Bodies } } - public unsafe override void SolveStep(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) - { - ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); - ref var bodyReferencesBase = ref Unsafe.AsRef>(typeBatch.BodyReferences.Memory); - ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); - ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); - var function = default(TConstraintFunctions); - for (int i = startBundle; i < exclusiveEndBundle; ++i) - { - ref var prestep = ref Unsafe.Add(ref prestepBase, i); - ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); - ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); - ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); - var count = GetCountInBundle(ref typeBatch, i); - bodies.GatherState(ref bodyReferences, count, out var position, out var orientation, out var wsvA, out var inertiaA); - function.Prestep(position, orientation, inertiaA, dt, inverseDt, ref prestep, out var projection); - function.WarmStart(ref wsvA, ref projection, ref accumulatedImpulses); - function.Solve(ref wsvA, ref projection, ref accumulatedImpulses); - bodies.ScatterVelocities(ref wsvA, ref bodyReferences, count); - } - } - - public unsafe override void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) - { - ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); - ref var bodyReferencesBase = ref Unsafe.AsRef>(typeBatch.BodyReferences.Memory); - ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); - ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); - var function = default(TConstraintFunctions); - ref var jacobiResultsBundlesA = ref jacobiResults.GetVelocitiesForBody(0); - for (int i = startBundle; i < exclusiveEndBundle; ++i) - { - ref var prestep = ref Unsafe.Add(ref prestepBase, i); - ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); - ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); - ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); - var count = GetCountInBundle(ref typeBatch, i); - ref var wsvA = ref jacobiResultsBundlesA[i]; - bodies.GatherState(ref bodyReferences, count, out var position, out var orientation, out wsvA, out var inertiaA); - //Jacobi batches split affected bodies into multiple pieces to guarantee convergence. - jacobiBatch.GetJacobiScaleForBodies(ref references, count, out var jacobiScale); - Symmetric3x3Wide.Scale(inertiaA.InverseInertiaTensor, jacobiScale, out inertiaA.InverseInertiaTensor); - inertiaA.InverseMass *= jacobiScale; - function.Prestep(position, orientation, inertiaA, dt, inverseDt, ref prestep, out var projection); - function.WarmStart(ref wsvA, ref projection, ref accumulatedImpulses); - function.Solve(ref wsvA, ref projection, ref accumulatedImpulses); - } - } - - public unsafe override void WarmStart2( ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { @@ -317,31 +267,14 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f } } - public unsafe override void Prestep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + public override void JacobiWarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks poseIntegratorCallbacks, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { - ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); - ref var bodyReferencesBase = ref Unsafe.AsRef>(typeBatch.BodyReferences.Memory); - ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); - ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); - var function = default(TConstraintFunctions); - for (int i = startBundle; i < exclusiveEndBundle; ++i) - { - ref var prestep = ref Unsafe.Add(ref prestepBase, i); - ref var projection = ref Unsafe.Add(ref projectionBase, i); - ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); - ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); - var count = GetCountInBundle(ref typeBatch, i); - bodies.GatherState(ref references, count, out var position, out var orientation, out var wsvA, out var inertiaA); - function.Prestep(position, orientation, inertiaA, dt, inverseDt, ref prestep, out projection); - function.WarmStart(ref wsvA, ref projection, ref accumulatedImpulses); - bodies.ScatterVelocities(ref wsvA, ref references, count); - } + throw new NotImplementedException(); } - public override void JacobiPrestep2(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + public override void JacobiSolveStep2(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { throw new NotImplementedException(); } - } public abstract class OneBodyContactTypeProcessor diff --git a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs index 243e917c4..94d4d331e 100644 --- a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs @@ -259,73 +259,6 @@ public unsafe override void JacobiSolveIteration(ref TypeBatch typeBatch, Bodies } } - - public unsafe override void SolveStep(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) - { - ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); - ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); - ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); - ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); - var function = default(TConstraintFunctions); - for (int i = startBundle; i < exclusiveEndBundle; ++i) - { - ref var prestep = ref Unsafe.Add(ref prestepBase, i); - ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); - ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); - ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); - var count = GetCountInBundle(ref typeBatch, i); - bodies.GatherState(ref references, count, - out var orientationA, out var wsvA, out var inertiaA, - out var ab, out var orientationB, out var wsvB, out var inertiaB, - out var ac, out var orientationC, out var wsvC, out var inertiaC); - function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, ac, orientationC, inertiaC, dt, inverseDt, ref prestep, out var projection); - function.WarmStart(ref wsvA, ref wsvB, ref wsvC, ref projection, ref accumulatedImpulses); - function.Solve(ref wsvA, ref wsvB, ref wsvC, ref projection, ref accumulatedImpulses); - bodies.ScatterVelocities(ref wsvA, ref bodyReferences.IndexA, count); - bodies.ScatterVelocities(ref wsvB, ref bodyReferences.IndexB, count); - bodies.ScatterVelocities(ref wsvC, ref bodyReferences.IndexC, count); - } - } - - public unsafe override void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) - { - ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); - ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); - ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); - ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); - var function = default(TConstraintFunctions); - ref var jacobiResultsBundlesA = ref jacobiResults.GetVelocitiesForBody(0); - ref var jacobiResultsBundlesB = ref jacobiResults.GetVelocitiesForBody(1); - ref var jacobiResultsBundlesC = ref jacobiResults.GetVelocitiesForBody(2); - for (int i = startBundle; i < exclusiveEndBundle; ++i) - { - ref var prestep = ref Unsafe.Add(ref prestepBase, i); - ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); - ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); - ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); - var count = GetCountInBundle(ref typeBatch, i); - ref var wsvA = ref jacobiResultsBundlesA[i]; - ref var wsvB = ref jacobiResultsBundlesB[i]; - ref var wsvC = ref jacobiResultsBundlesC[i]; - bodies.GatherState(ref references, count, - out var orientationA, out wsvA, out var inertiaA, - out var ab, out var orientationB, out wsvB, out var inertiaB, - out var ac, out var orientationC, out wsvC, out var inertiaC); - //Jacobi batches split affected bodies into multiple pieces to guarantee convergence. - jacobiBatch.GetJacobiScaleForBodies(ref references, count, out var jacobiScaleA, out var jacobiScaleB, out var jacobiScaleC); - Symmetric3x3Wide.Scale(inertiaA.InverseInertiaTensor, jacobiScaleA, out inertiaA.InverseInertiaTensor); - inertiaA.InverseMass *= jacobiScaleA; - Symmetric3x3Wide.Scale(inertiaB.InverseInertiaTensor, jacobiScaleB, out inertiaB.InverseInertiaTensor); - inertiaB.InverseMass *= jacobiScaleB; - Symmetric3x3Wide.Scale(inertiaC.InverseInertiaTensor, jacobiScaleC, out inertiaC.InverseInertiaTensor); - inertiaC.InverseMass *= jacobiScaleC; - function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, ac, orientationC, inertiaC, dt, inverseDt, ref prestep, out var projection); - function.WarmStart(ref wsvA, ref wsvB, ref wsvC, ref projection, ref accumulatedImpulses); - function.Solve(ref wsvA, ref wsvB, ref wsvC, ref projection, ref accumulatedImpulses); - } - } - - public unsafe override void WarmStart2( ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { @@ -395,11 +328,12 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f } } - public override void Prestep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + public override void JacobiWarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks poseIntegratorCallbacks, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { throw new NotImplementedException(); } - public override void JacobiPrestep2(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + + public override void JacobiSolveStep2(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index fc4bc6057..03967e6c5 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -263,60 +263,6 @@ public unsafe override void JacobiSolveIteration(ref TypeBatch typeBatch, Bodies } } - public unsafe override void SolveStep(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) - { - ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); - ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); - ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); - ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); - var function = default(TConstraintFunctions); - for (int i = startBundle; i < exclusiveEndBundle; ++i) - { - ref var prestep = ref Unsafe.Add(ref prestepBase, i); - ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); - ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); - var count = GetCountInBundle(ref typeBatch, i); - bodies.GatherState(ref references, count, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB); - function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, dt, inverseDt, ref prestep, out var projection); - function.WarmStart(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); - function.Solve(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); - bodies.ScatterVelocities(ref wsvA, ref references.IndexA, count); - bodies.ScatterVelocities(ref wsvB, ref references.IndexB, count); - } - } - - public unsafe override void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) - { - ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); - ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); - ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); - ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); - ref var motionStates = ref bodies.ActiveSet.SolverStates; - var function = default(TConstraintFunctions); - ref var jacobiResultsBundlesA = ref jacobiResults.GetVelocitiesForBody(0); - ref var jacobiResultsBundlesB = ref jacobiResults.GetVelocitiesForBody(1); - for (int i = startBundle; i < exclusiveEndBundle; ++i) - { - ref var prestep = ref Unsafe.Add(ref prestepBase, i); - ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); - ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); - ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); - var count = GetCountInBundle(ref typeBatch, i); - ref var wsvA = ref jacobiResultsBundlesA[i]; - ref var wsvB = ref jacobiResultsBundlesB[i]; - bodies.GatherState(ref references, count, out var orientationA, out wsvA, out var inertiaA, out var ab, out var orientationB, out wsvB, out var inertiaB); - //Jacobi batches split affected bodies into multiple pieces to guarantee convergence. - jacobiBatch.GetJacobiScaleForBodies(ref references, count, out var jacobiScaleA, out var jacobiScaleB); - Symmetric3x3Wide.Scale(inertiaA.InverseInertiaTensor, jacobiScaleA, out inertiaA.InverseInertiaTensor); - inertiaA.InverseMass *= jacobiScaleA; - Symmetric3x3Wide.Scale(inertiaB.InverseInertiaTensor, jacobiScaleB, out inertiaB.InverseInertiaTensor); - inertiaB.InverseMass *= jacobiScaleB; - function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, dt, inverseDt, ref prestep, out var projection); - function.WarmStart(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); - function.Solve(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); - } - } - //public const int WarmStartPrefetchDistance = 8; //public const int SolvePrefetchDistance = 4; //[MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -436,28 +382,48 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f } } - public unsafe override void Prestep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + public override void JacobiWarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { - ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); - ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); - ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); - ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); + var prestepBundles = typeBatch.PrestepData.As(); + var bodyReferencesBundles = typeBatch.BodyReferences.As(); + var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); var function = default(TConstraintFunctions); + ref var states = ref bodies.ActiveSet.SolverStates; + //EarlyPrefetch(WarmStartPrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref states, startBundle, exclusiveEndBundle); for (int i = startBundle; i < exclusiveEndBundle; ++i) { - ref var prestep = ref Unsafe.Add(ref prestepBase, i); - ref var projection = ref Unsafe.Add(ref projectionBase, i); - ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); - ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); + ref var prestep = ref prestepBundles[i]; + ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; + ref var references = ref bodyReferencesBundles[i]; var count = GetCountInBundle(ref typeBatch, i); - bodies.GatherState(ref references, count, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB); - function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, dt, inverseDt, ref prestep, out projection); - function.WarmStart(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); - bodies.ScatterVelocities(ref wsvA, ref references.IndexA, count); - bodies.ScatterVelocities(ref wsvB, ref references.IndexB, count); + //Prefetch(WarmStartPrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref states, i, exclusiveEndBundle); + GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 0, dt, workerIndex, i, ref references.IndexA, count, + out var positionA, out var orientationA, out var wsvA, out var inertiaA); + GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 1, dt, workerIndex, i, ref references.IndexB, count, + out var positionB, out var orientationB, out var wsvB, out var inertiaB); + + //if (typeof(TAllowPoseIntegration) == typeof(AllowPoseIntegration)) + // function.UpdateForNewPose(positionA, orientationA, inertiaA, wsvA, positionB, orientationB, inertiaB, wsvB, new Vector(dt), accumulatedImpulses, ref prestep); + + function.WarmStart2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB); + + if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) + { + bodies.ScatterVelocities(ref wsvA, ref references.IndexA, count); + bodies.ScatterVelocities(ref wsvB, ref references.IndexB, count); + } + else + { + //This batch has some integrators, which means that every bundle is going to gather all velocities. + //(We don't make per-bundle determinations about this to avoid an extra branch and instruction complexity, and the difference is very small.) + bodies.ScatterVelocities(ref wsvA, ref references.IndexA, count); + bodies.ScatterVelocities(ref wsvB, ref references.IndexB, count); + } + } } - public override void JacobiPrestep2(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + + public override void JacobiSolveStep2(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { throw new NotImplementedException(); } diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index 0ab2fd829..26833acb0 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -136,10 +136,6 @@ internal unsafe abstract void CopySleepingToActive( public abstract void JacobiWarmStart(ref TypeBatch typeBatch, Bodies bodies, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle); public abstract void JacobiSolveIteration(ref TypeBatch typeBatch, Bodies bodies, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle); - public abstract void SolveStep(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); - public abstract void JacobiSolveStep(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); - - public abstract void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks poseIntegratorCallbacks, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) where TIntegratorCallbacks : struct, IPoseIntegratorCallbacks @@ -147,9 +143,14 @@ public abstract void WarmStart2( + ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks poseIntegratorCallbacks, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, + float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) + where TIntegratorCallbacks : struct, IPoseIntegratorCallbacks + where TBatchIntegrationMode : unmanaged, IBatchIntegrationMode + where TAllowPoseIntegration : unmanaged, IBatchPoseIntegrationAllowed; - public abstract void Prestep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); - public abstract void JacobiPrestep2(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); + public abstract void JacobiSolveStep2(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); public virtual void IncrementallyUpdateContactData(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int end) { diff --git a/BepuPhysics/EmbeddedSubsteppingTimestepper.cs b/BepuPhysics/EmbeddedSubsteppingTimestepper.cs deleted file mode 100644 index c60f9e9e7..000000000 --- a/BepuPhysics/EmbeddedSubsteppingTimestepper.cs +++ /dev/null @@ -1,109 +0,0 @@ -using System; -using System.Collections.Generic; -using System.Diagnostics; -using System.Text; -using BepuUtilities; - -namespace BepuPhysics -{ - /// - /// Updates the simulation in the order of: sleeper -> predict body bounding boxes -> collision detection -> LOOP { contact data update (if on iteration > 0) -> integrate body velocities -> solver -> integrate body poses } -> data structure optimization. - /// Each inner loop execution simulates a sub-timestep of length dt/substepCount. - /// Useful for simulations with difficult to solve constraint systems that need shorter timestep durations but which don't require high frequency collision detection. - /// - public class EmbeddedSubsteppingTimestepper : ITimestepper - { - /// - /// Gets or sets the number of substeps to execute during each timestep. - /// - public int SubstepCount { get; set; } - - /// - /// Fires after the sleeper completes and before bodies are integrated. - /// - public event TimestepperStageHandler Slept; - /// - /// Fires after bodies have their bounding boxes updated for the frame's predicted motion and before collision detection. - /// - public event TimestepperStageHandler BeforeCollisionDetection; - /// - /// Fires after all collisions have been identified, but before the substep loop begins. - /// - public event TimestepperStageHandler CollisionsDetected; - /// - /// Fires at the beginning of a substep. - /// - public event TimestepperSubstepStageHandler SubstepStarted; - /// - /// Fires after contact constraints are incrementally updated at the beginning of substeps after the first and before velocities are integrated. - /// - public event TimestepperSubstepStageHandler ContactConstraintsUpdatedForSubstep; - /// - /// Fires after bodies have their velocities integrated and before the solver executes. - /// - public event TimestepperSubstepStageHandler VelocitiesIntegrated; - /// - /// Fires after the solver executes and before body poses are integrated. - /// - public event TimestepperSubstepStageHandler ConstraintsSolved; - /// - /// Fires after bodies have their poses integrated and before the substep ends. - /// - public event TimestepperSubstepStageHandler PosesIntegrated; - /// - /// Fires at the end of a substep. - /// - public event TimestepperSubstepStageHandler SubstepEnded; - /// - /// Fires after all substeps are finished executing and before data structures are incrementally optimized. - /// - public event TimestepperStageHandler SubstepsComplete; - - public EmbeddedSubsteppingTimestepper(int substepCount) - { - SubstepCount = substepCount; - } - - public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDispatcher = null) - { - simulation.Sleep(threadDispatcher); - Slept?.Invoke(dt, threadDispatcher); - - simulation.PredictBoundingBoxes(dt, threadDispatcher); - BeforeCollisionDetection?.Invoke(dt, threadDispatcher); - - simulation.CollisionDetection(dt, threadDispatcher); - CollisionsDetected?.Invoke(dt, threadDispatcher); - - Debug.Assert(SubstepCount >= 0, "Substep count should be positive."); - var substepDt = dt / SubstepCount; - - for (int substepIndex = 0; substepIndex < SubstepCount; ++substepIndex) - { - SubstepStarted?.Invoke(substepIndex, dt, threadDispatcher); - if (substepIndex > 0) - { - //This takes the place of collision detection for the substeps. It uses the current velocity to update penetration depths. - //It's definitely an approximation, but it's important for avoiding some obviously weird behavior. - //Note that we do not run this on the first iteration- the actual collision detection above takes care of it. - simulation.IncrementallyUpdateContactConstraints(substepDt, threadDispatcher); - ContactConstraintsUpdatedForSubstep?.Invoke(substepIndex, substepDt, threadDispatcher); - } - simulation.IntegrateVelocitiesAndUpdateInertias(substepDt, threadDispatcher); - VelocitiesIntegrated?.Invoke(substepIndex, substepDt, threadDispatcher); - - simulation.Profiler.Start(simulation.Solver); - simulation.Solver.SolveStep(substepDt, threadDispatcher); - simulation.Profiler.End(simulation.Solver); - ConstraintsSolved?.Invoke(substepIndex, substepDt, threadDispatcher); - - simulation.IntegratePoses(substepDt, threadDispatcher); - PosesIntegrated?.Invoke(substepIndex, substepDt, threadDispatcher); - SubstepEnded?.Invoke(substepIndex, substepDt, threadDispatcher); - } - SubstepsComplete?.Invoke(dt, threadDispatcher); - - simulation.IncrementallyOptimizeDataStructures(threadDispatcher); - } - } -} diff --git a/BepuPhysics/PositionFirstTimestepper2.cs b/BepuPhysics/PositionFirstTimestepper2.cs deleted file mode 100644 index 6b358a272..000000000 --- a/BepuPhysics/PositionFirstTimestepper2.cs +++ /dev/null @@ -1,74 +0,0 @@ -using System; -using System.Collections.Generic; -using System.Text; -using BepuUtilities; - -namespace BepuPhysics -{ - /// - /// Updates the simulation in the order of: sleeper -> integrate body poses, velocity and bounding boxes -> collision detection -> solver -> data structure optimization. - /// - public class PositionFirstTimestepper2 : ITimestepper - { - /// - /// Fires after the sleeper completes and before bodies are integrated. - /// - public event TimestepperStageHandler Slept; - /// - /// Fires after bodies have had their position, velocity, and bounding boxes updated, but before collision detection begins. - /// - public event TimestepperStageHandler BeforeCollisionDetection; - /// - /// Fires after all collisions have been identified, but before constraints are solved. - /// - public event TimestepperStageHandler CollisionsDetected; - /// - /// Fires after the solver executes and before data structures are incrementally optimized. - /// - public event TimestepperStageHandler ConstraintsSolved; - - public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDispatcher = null) - { - //Note that there is a reason to put the sleep *after* velocity integration. That sounds a little weird, but there's a good reason: - //When the narrow phase activates a bunch of objects in a pile, their accumulated impulses will represent all forces acting on them at the time of sleep. - //That includes gravity. If we sleep objects *before* gravity is applied in a given frame, then when those bodies are awakened, the accumulated impulses - //will be less accurate because they assume that gravity has already been applied. This can cause a small bump. - //So, velocity integration (and deactivation candidacy management) could come before sleep. - - //Sleep at the start, on the other hand, stops some forms of unintuitive behavior when using direct awakenings. Just a matter of preference. - simulation.Sleep(threadDispatcher); - Slept?.Invoke(dt, threadDispatcher); - - //Note that pose integrator comes before collision detection and solving. This is a shift from v1, where collision detection went first. - //This is a tradeoff: - //1) Any externally set velocities will be integrated without input from the solver. The v1-style external velocity control won't work as well- - //the user would instead have to change velocities after the pose integrator runs. This isn't perfect either, since the pose integrator is also responsible - //for updating the bounding boxes used for collision detection. - //2) By bundling bounding box calculation with pose integration, you avoid redundant pose and velocity memory accesses. - //3) Generated contact positions are in sync with the integrated poses. - //That's often helpful for gameplay purposes- you don't have to reinterpret contact data when creating graphical effects or positioning sound sources. - - //#1 is a difficult problem, though. There is no fully 'correct' place to change velocities. We might just have to bite the bullet and create a - //inertia tensor/bounding box update separate from pose integration. If the cache gets evicted in between (virtually guaranteed unless no stages run), - //this basically means an extra 100-200 microseconds per frame on a processor with ~20GBps bandwidth simulating 32768 bodies. - - //Note that the reason why the pose integrator comes first instead of, say, the solver, is that the solver relies on world space inertias calculated by the pose integration. - //If the pose integrator doesn't run first, we either need - //1) complicated on demand updates of world inertia when objects are added or local inertias are changed or - //2) local->world inertia calculation before the solver. - simulation.IntegrateBodiesAndUpdateBoundingBoxes(dt, threadDispatcher); - BeforeCollisionDetection?.Invoke(dt, threadDispatcher); - - simulation.CollisionDetection(dt, threadDispatcher); - CollisionsDetected?.Invoke(dt, threadDispatcher); - - //simulation.Solve(dt, threadDispatcher); - simulation.Profiler.Start(simulation.Solver); - simulation.Solver.Solve2(dt, threadDispatcher); - simulation.Profiler.End(simulation.Solver); - ConstraintsSolved?.Invoke(dt, threadDispatcher); - - simulation.IncrementallyOptimizeDataStructures(threadDispatcher); - } - } -} diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index fbf8f7fc9..fcef7a219 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -179,7 +179,6 @@ public int CountConstraints() Action solveWorker; - Action solve2Worker; Action incrementalContactUpdateWorker; public Solver(Bodies bodies, BufferPool pool, int iterationCount, int fallbackBatchThreshold, int initialCapacity, @@ -196,9 +195,7 @@ public Solver(Bodies bodies, BufferPool pool, int iterationCount, int fallbackBa ActiveSet = new ConstraintSet(pool, fallbackBatchThreshold + 1); batchReferencedHandles = new QuickList(fallbackBatchThreshold + 1, pool); ResizeHandleCapacity(initialCapacity); - solveWorker = SolveWorker; - solve2Worker = Solve2Worker; - solveStepWorker = SolveStepWorker; + solveWorker = SolveWorker;; incrementalContactUpdateWorker = IncrementalContactUpdateWorker; } diff --git a/BepuPhysics/Solver_Solve2.cs b/BepuPhysics/Solver_Solve2.cs deleted file mode 100644 index b6e9aed94..000000000 --- a/BepuPhysics/Solver_Solve2.cs +++ /dev/null @@ -1,193 +0,0 @@ -using BepuUtilities; -using BepuUtilities.Collections; -using BepuUtilities.Memory; -using BepuPhysics.Constraints; -using System; -using System.Collections.Generic; -using System.Diagnostics; -using System.Runtime.CompilerServices; -using System.Runtime.InteropServices; -using System.Text; -using System.Threading; - -namespace BepuPhysics -{ - public partial class Solver - { - - - struct Prestep2StageFunction : IStageFunction - { - public float Dt; - public float InverseDt; - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Execute(Solver solver, int blockIndex, int workerIndex) - { - ref var block = ref solver.context.ConstraintBlocks.Blocks[blockIndex]; - ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; - var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; - typeProcessor.Prestep2(ref typeBatch, solver.bodies, Dt, InverseDt, block.StartBundle, block.End); - } - } - - struct Prestep2FallbackStageFunction : IStageFunction - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Execute(Solver solver, int blockIndex, int workerIndex) - { - ref var block = ref solver.context.ConstraintBlocks.Blocks[blockIndex]; - ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; - var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; - //typeProcessor.JacobiPrestep2(ref typeBatch, ref solver.bodies.ActiveSet.Velocities, solver ref solver.context.FallbackResults[block.TypeBatchIndex], block.StartBundle, block.End); - - } - } - - void Solve2Worker(int workerIndex) - { - int prestepStart = GetUniformlyDistributedStart(workerIndex, context.ConstraintBlocks.Blocks.Count, context.WorkerCount, 0); - int fallbackStart = GetUniformlyDistributedStart(workerIndex, context.FallbackBlocks.Blocks.Count, context.WorkerCount, 0); - Buffer batchStarts; - ref var activeSet = ref ActiveSet; - unsafe - { - //stackalloc is actually a little bit slow since the localsinit behavior forces a zeroing. - //Fortunately, this executes once per thread per frame. With 32 batches, it would add... a few nanoseconds per frame. We can accept that overhead. - //This is preferred over preallocating on the heap- we might write to these values and we don't want to risk false sharing for no reason. - //A single instance of false sharing would cost far more than the overhead of zeroing out the array. - var batchStartsData = stackalloc int[activeSet.Batches.Count]; - batchStarts = new Buffer(batchStartsData, activeSet.Batches.Count); - } - for (int batchIndex = 0; batchIndex < activeSet.Batches.Count; ++batchIndex) - { - var batchOffset = batchIndex > 0 ? context.BatchBoundaries[batchIndex - 1] : 0; - var batchCount = context.BatchBoundaries[batchIndex] - batchOffset; - batchStarts[batchIndex] = GetUniformlyDistributedStart(workerIndex, batchCount, context.WorkerCount, batchOffset); - } - - - int syncStage = 0; - //The claimed and unclaimed state swap after every usage of both pingpong claims buffers. - int claimedState = 1; - int unclaimedState = 0; - var bounds = context.WorkerBoundsA; - var boundsBackBuffer = context.WorkerBoundsB; - Debug.Assert(activeSet.Batches.Count > 0, "Don't dispatch if there are no constraints."); - - //Note that every batch has a different start position. Each covers a different subset of constraints, so they require different start locations. - GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); - //claimedState ^= 1; - //unclaimedState ^= 1; - var prestepStage = new Prestep2StageFunction { Dt = context.Dt, InverseDt = 1f / context.Dt }; - for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) - { - var batchOffset = batchIndex > 0 ? context.BatchBoundaries[batchIndex - 1] : 0; - //Don't use the warm start to guess at the solve iteration work distribution. - var workerBatchStartCopy = batchStarts[batchIndex]; - ExecuteStage(ref prestepStage, ref context.ConstraintBlocks, ref bounds, ref boundsBackBuffer, workerIndex, batchOffset, context.BatchBoundaries[batchIndex], - ref workerBatchStartCopy, ref syncStage, claimedState, unclaimedState); - } - var fallbackScatterStage = new FallbackScatterStageFunction(); - if (fallbackExists) - { - var prestepFallbackStage = new Prestep2FallbackStageFunction(); - var batchStart = FallbackBatchThreshold > 0 ? context.BatchBoundaries[FallbackBatchThreshold - 1] : 0; - //Don't use the warm start to guess at the solve iteration work distribution. - var workerBatchStartCopy = batchStarts[FallbackBatchThreshold]; - ExecuteStage(ref prestepFallbackStage, ref context.ConstraintBlocks, ref bounds, ref boundsBackBuffer, workerIndex, batchStart, context.BatchBoundaries[FallbackBatchThreshold], - ref workerBatchStartCopy, ref syncStage, claimedState, unclaimedState); - ExecuteStage(ref fallbackScatterStage, ref context.FallbackBlocks, ref bounds, ref boundsBackBuffer, - workerIndex, 0, context.FallbackBlocks.Blocks.Count, ref fallbackStart, ref syncStage, unclaimedState, claimedState); //note claim state swap: fallback scatter claims have no prestep, so it's off by one cycle - } - claimedState ^= 1; - unclaimedState ^= 1; - - var solveStage = new SolveStageFunction(); - var solveFallbackStage = new SolveFallbackStageFunction(); - for (int iterationIndex = 0; iterationIndex < iterationCount; ++iterationIndex) - { - for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) - { - var batchOffset = batchIndex > 0 ? context.BatchBoundaries[batchIndex - 1] : 0; - ExecuteStage(ref solveStage, ref context.ConstraintBlocks, ref bounds, ref boundsBackBuffer, workerIndex, batchOffset, context.BatchBoundaries[batchIndex], - ref batchStarts[batchIndex], ref syncStage, claimedState, unclaimedState); - } - if (fallbackExists) - { - var batchOffset = FallbackBatchThreshold > 0 ? context.BatchBoundaries[FallbackBatchThreshold - 1] : 0; - ExecuteStage(ref solveFallbackStage, ref context.ConstraintBlocks, ref bounds, ref boundsBackBuffer, workerIndex, batchOffset, context.BatchBoundaries[FallbackBatchThreshold], - ref batchStarts[FallbackBatchThreshold], ref syncStage, claimedState, unclaimedState); - ExecuteStage(ref fallbackScatterStage, ref context.FallbackBlocks, ref bounds, ref boundsBackBuffer, - workerIndex, 0, context.FallbackBlocks.Blocks.Count, ref fallbackStart, ref syncStage, unclaimedState, claimedState); //note claim state swap: fallback scatter claims have no prestep, so it's off by one cycle - } - claimedState ^= 1; - unclaimedState ^= 1; - } - } - - public void Solve2(float dt, IThreadDispatcher threadDispatcher = null) - { - if (threadDispatcher == null) - { - var inverseDt = 1f / dt; - ref var activeSet = ref ActiveSet; - GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); - //TODO: May want to consider executing warmstart immediately following the prestep. Multithreading can't do that, so there could be some bitwise differences introduced. - //On the upside, it would make use of cached data. - for (int i = 0; i < synchronizedBatchCount; ++i) - { - ref var batch = ref activeSet.Batches[i]; - for (int j = 0; j < batch.TypeBatches.Count; ++j) - { - ref var typeBatch = ref batch.TypeBatches[j]; - TypeProcessors[typeBatch.TypeId].Prestep2(ref typeBatch, bodies, dt, inverseDt, 0, typeBatch.BundleCount); - } - } - Buffer fallbackResults = default; - if (fallbackExists) - { - ref var batch = ref activeSet.Batches[FallbackBatchThreshold]; - FallbackBatch.AllocateResults(this, pool, ref batch, out fallbackResults); - for (int j = 0; j < batch.TypeBatches.Count; ++j) - { - ref var typeBatch = ref batch.TypeBatches[j]; - TypeProcessors[typeBatch.TypeId].JacobiPrestep2(ref typeBatch, bodies, ref activeSet.Fallback, ref fallbackResults[j], dt, inverseDt, 0, typeBatch.BundleCount); - } - activeSet.Fallback.ScatterVelocities(bodies, this, ref fallbackResults, 0, activeSet.Fallback.BodyCount); - } - for (int iterationIndex = 0; iterationIndex < iterationCount; ++iterationIndex) - { - for (int i = 0; i < synchronizedBatchCount; ++i) - { - ref var batch = ref activeSet.Batches[i]; - for (int j = 0; j < batch.TypeBatches.Count; ++j) - { - ref var typeBatch = ref batch.TypeBatches[j]; - TypeProcessors[typeBatch.TypeId].SolveIteration(ref typeBatch, bodies, 0, typeBatch.BundleCount); - } - } - if (fallbackExists) - { - ref var batch = ref activeSet.Batches[FallbackBatchThreshold]; - for (int j = 0; j < batch.TypeBatches.Count; ++j) - { - ref var typeBatch = ref batch.TypeBatches[j]; - TypeProcessors[typeBatch.TypeId].JacobiSolveIteration(ref typeBatch, bodies, ref fallbackResults[j], 0, typeBatch.BundleCount); - } - activeSet.Fallback.ScatterVelocities(bodies, this, ref fallbackResults, 0, activeSet.Fallback.BodyCount); - } - } - if (fallbackExists) - { - FallbackBatch.DisposeResults(this, pool, ref activeSet.Batches[FallbackBatchThreshold], ref fallbackResults); - } - } - else - { - ExecuteMultithreaded(dt, threadDispatcher, solve2Worker); - } - } - - } -} diff --git a/BepuPhysics/Solver_SubsteppingSolve.cs b/BepuPhysics/Solver_SubsteppingSolve.cs deleted file mode 100644 index 196675beb..000000000 --- a/BepuPhysics/Solver_SubsteppingSolve.cs +++ /dev/null @@ -1,142 +0,0 @@ -using BepuUtilities; -using BepuUtilities.Collections; -using BepuUtilities.Memory; -using BepuPhysics.Constraints; -using System; -using System.Collections.Generic; -using System.Diagnostics; -using System.Runtime.CompilerServices; -using System.Runtime.InteropServices; -using System.Text; -using System.Threading; - -namespace BepuPhysics -{ - public partial class Solver - { - - struct SolveStepStageFunction : IStageFunction - { - public float Dt; - public float InverseDt; - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Execute(Solver solver, int blockIndex, int workerIndex) - { - ref var block = ref solver.context.ConstraintBlocks.Blocks[blockIndex]; - ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; - var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; - typeProcessor.SolveStep(ref typeBatch, solver.bodies, Dt, InverseDt, block.StartBundle, block.End); - } - } - - struct FallbackSolveStepStageFunction : IStageFunction - { - public float Dt; - public float InverseDt; - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Execute(Solver solver, int blockIndex, int workerIndex) - { - ref var block = ref solver.context.ConstraintBlocks.Blocks[blockIndex]; - ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; - var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; - typeProcessor.JacobiSolveStep(ref typeBatch, solver.bodies, ref solver.ActiveSet.Fallback, ref solver.context.FallbackResults[block.TypeBatchIndex], Dt, InverseDt, block.StartBundle, block.End); - } - } - - Action solveStepWorker; - - - void SolveStepWorker(int workerIndex) - { - int prestepStart = GetUniformlyDistributedStart(workerIndex, context.ConstraintBlocks.Blocks.Count, context.WorkerCount, 0); - int fallbackStart = GetUniformlyDistributedStart(workerIndex, context.FallbackBlocks.Blocks.Count, context.WorkerCount, 0); - Buffer batchStarts; - ref var activeSet = ref ActiveSet; - unsafe - { - //stackalloc is actually a little bit slow since the localsinit behavior forces a zeroing. - //Fortunately, this executes once per thread per frame. With 32 batches, it would add... a few nanoseconds per frame. We can accept that overhead. - //This is preferred over preallocating on the heap- we might write to these values and we don't want to risk false sharing for no reason. - //A single instance of false sharing would cost far more than the overhead of zeroing out the array. - var batchStartsData = stackalloc int[activeSet.Batches.Count]; - batchStarts = new Buffer(batchStartsData, activeSet.Batches.Count); - } - for (int batchIndex = 0; batchIndex < activeSet.Batches.Count; ++batchIndex) - { - var batchOffset = batchIndex > 0 ? context.BatchBoundaries[batchIndex - 1] : 0; - var batchCount = context.BatchBoundaries[batchIndex] - batchOffset; - batchStarts[batchIndex] = GetUniformlyDistributedStart(workerIndex, batchCount, context.WorkerCount, batchOffset); - } - - int syncStage = 0; - //The claimed and unclaimed state swap after every usage of both pingpong claims buffers. - int claimedState = 1; - int unclaimedState = 0; - var bounds = context.WorkerBoundsA; - var boundsBackBuffer = context.WorkerBoundsB; - //Note that every batch has a different start position. Each covers a different subset of constraints, so they require different start locations. - //The same concept applies to the prestep- the prestep covers all constraints at once, rather than batch by batch. - Debug.Assert(activeSet.Batches.Count > 0, "Don't dispatch if there are no constraints."); - GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); - - var solveStage = new SolveStepStageFunction { Dt = context.Dt, InverseDt = 1f / context.Dt }; - for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) - { - var batchOffset = batchIndex > 0 ? context.BatchBoundaries[batchIndex - 1] : 0; - ExecuteStage(ref solveStage, ref context.ConstraintBlocks, ref bounds, ref boundsBackBuffer, workerIndex, batchOffset, context.BatchBoundaries[batchIndex], - ref batchStarts[batchIndex], ref syncStage, claimedState, unclaimedState); - } - if (fallbackExists) - { - var solveFallbackStage = new FallbackSolveStepStageFunction { Dt = context.Dt, InverseDt = 1f / context.Dt }; - var fallbackScatterStage = new FallbackScatterStageFunction(); - var batchOffset = FallbackBatchThreshold > 0 ? context.BatchBoundaries[FallbackBatchThreshold - 1] : 0; - ExecuteStage(ref solveFallbackStage, ref context.ConstraintBlocks, ref bounds, ref boundsBackBuffer, workerIndex, batchOffset, context.BatchBoundaries[FallbackBatchThreshold], - ref batchStarts[FallbackBatchThreshold], ref syncStage, claimedState, unclaimedState); - ExecuteStage(ref fallbackScatterStage, ref context.FallbackBlocks, ref bounds, ref boundsBackBuffer, - workerIndex, 0, context.FallbackBlocks.Blocks.Count, ref fallbackStart, ref syncStage, claimedState, unclaimedState); - } - claimedState ^= 1; - unclaimedState ^= 1; - } - - public void SolveStep(float dt, IThreadDispatcher threadDispatcher = null) - { - if (threadDispatcher == null) - { - var inverseDt = 1f / dt; - ref var activeSet = ref ActiveSet; - GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); - - for (int i = 0; i < synchronizedBatchCount; ++i) - { - ref var batch = ref activeSet.Batches[i]; - for (int j = 0; j < batch.TypeBatches.Count; ++j) - { - ref var typeBatch = ref batch.TypeBatches[j]; - TypeProcessors[typeBatch.TypeId].SolveStep(ref typeBatch, bodies, dt, inverseDt, 0, typeBatch.BundleCount); - } - } - if (fallbackExists) - { - ref var batch = ref activeSet.Batches[FallbackBatchThreshold]; - FallbackBatch.AllocateResults(this, pool, ref batch, out var fallbackResults); - for (int j = 0; j < batch.TypeBatches.Count; ++j) - { - ref var typeBatch = ref batch.TypeBatches[j]; - TypeProcessors[typeBatch.TypeId].JacobiSolveStep(ref typeBatch, bodies, ref activeSet.Fallback, ref fallbackResults[j], dt, inverseDt, 0, typeBatch.BundleCount); - } - activeSet.Fallback.ScatterVelocities(bodies, this, ref fallbackResults, 0, activeSet.Fallback.BodyCount); - FallbackBatch.DisposeResults(this, pool, ref activeSet.Batches[FallbackBatchThreshold], ref fallbackResults); - } - } - else - { - ExecuteMultithreaded(dt, threadDispatcher, solveStepWorker); - } - } - - } -} diff --git a/BepuPhysics/SubsteppingTimestepper2.cs b/BepuPhysics/SubsteppingTimestepper2.cs deleted file mode 100644 index 6854077df..000000000 --- a/BepuPhysics/SubsteppingTimestepper2.cs +++ /dev/null @@ -1,109 +0,0 @@ -using System; -using System.Collections.Generic; -using System.Diagnostics; -using System.Text; -using BepuUtilities; - -namespace BepuPhysics -{ - /// - /// Updates the simulation in the order of: sleeper -> predict body bounding boxes -> collision detection -> LOOP { contact data update (if on iteration > 0) -> integrate body velocities -> solver -> integrate body poses } -> data structure optimization. - /// Each inner loop execution simulates a sub-timestep of length dt/substepCount. - /// Useful for simulations with difficult to solve constraint systems that need shorter timestep durations but which don't require high frequency collision detection. - /// - public class SubsteppingTimestepper2 : ITimestepper - { - /// - /// Gets or sets the number of substeps to execute during each timestep. - /// - public int SubstepCount { get; set; } - - /// - /// Fires after the sleeper completes and before bodies are integrated. - /// - public event TimestepperStageHandler Slept; - /// - /// Fires after bodies have their bounding boxes updated for the frame's predicted motion and before collision detection. - /// - public event TimestepperStageHandler BeforeCollisionDetection; - /// - /// Fires after all collisions have been identified, but before the substep loop begins. - /// - public event TimestepperStageHandler CollisionsDetected; - /// - /// Fires at the beginning of a substep. - /// - public event TimestepperSubstepStageHandler SubstepStarted; - /// - /// Fires after contact constraints are incrementally updated at the beginning of substeps after the first and before velocities are integrated. - /// - public event TimestepperSubstepStageHandler ContactConstraintsUpdatedForSubstep; - /// - /// Fires after bodies have their velocities integrated and before the solver executes. - /// - public event TimestepperSubstepStageHandler VelocitiesIntegrated; - /// - /// Fires after the solver executes and before body poses are integrated. - /// - public event TimestepperSubstepStageHandler ConstraintsSolved; - /// - /// Fires after bodies have their poses integrated and before the substep ends. - /// - public event TimestepperSubstepStageHandler PosesIntegrated; - /// - /// Fires at the end of a substep. - /// - public event TimestepperSubstepStageHandler SubstepEnded; - /// - /// Fires after all substeps are finished executing and before data structures are incrementally optimized. - /// - public event TimestepperStageHandler SubstepsComplete; - - public SubsteppingTimestepper2(int substepCount) - { - SubstepCount = substepCount; - } - - public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDispatcher = null) - { - simulation.Sleep(threadDispatcher); - Slept?.Invoke(dt, threadDispatcher); - - simulation.PredictBoundingBoxes(dt, threadDispatcher); - BeforeCollisionDetection?.Invoke(dt, threadDispatcher); - - simulation.CollisionDetection(dt, threadDispatcher); - CollisionsDetected?.Invoke(dt, threadDispatcher); - - Debug.Assert(SubstepCount >= 0, "Substep count should be positive."); - var substepDt = dt / SubstepCount; - - for (int substepIndex = 0; substepIndex < SubstepCount; ++substepIndex) - { - SubstepStarted?.Invoke(substepIndex, dt, threadDispatcher); - if (substepIndex > 0) - { - //This takes the place of collision detection for the substeps. It uses the current velocity to update penetration depths. - //It's definitely an approximation, but it's important for avoiding some obviously weird behavior. - //Note that we do not run this on the first iteration- the actual collision detection above takes care of it. - simulation.IncrementallyUpdateContactConstraints(substepDt, threadDispatcher); - ContactConstraintsUpdatedForSubstep?.Invoke(substepIndex, dt, threadDispatcher); - } - simulation.IntegrateVelocitiesAndUpdateInertias(substepDt, threadDispatcher); - VelocitiesIntegrated?.Invoke(substepIndex, dt, threadDispatcher); - - simulation.Profiler.Start(simulation.Solver); - simulation.Solver.Solve2(substepDt, threadDispatcher); - simulation.Profiler.End(simulation.Solver); - ConstraintsSolved?.Invoke(substepIndex, dt, threadDispatcher); - - simulation.IntegratePoses(substepDt, threadDispatcher); - PosesIntegrated?.Invoke(substepIndex, dt, threadDispatcher); - SubstepEnded?.Invoke(substepIndex, dt, threadDispatcher); - } - SubstepsComplete?.Invoke(dt, threadDispatcher); - - simulation.IncrementallyOptimizeDataStructures(threadDispatcher); - } - } -} diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index 4e5de6939..19aaf7760 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -45,6 +45,7 @@ struct Option public DemoSet() { + AddOption(); AddOption(); AddOption(); AddOption(); diff --git a/Demos/Demos/PyramidDemo.cs b/Demos/Demos/PyramidDemo.cs index 8430e1ee6..821bff209 100644 --- a/Demos/Demos/PyramidDemo.cs +++ b/Demos/Demos/PyramidDemo.cs @@ -26,7 +26,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. //Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), solverIterationCount: 1); - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper2(3), solverIterationCount: 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), solverIterationCount: 1); //Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); var boxShape = new Box(1, 1, 1); diff --git a/Demos/SpecializedTests/RagdollTubeDemo.cs b/Demos/SpecializedTests/RagdollTubeDemo.cs index 2b51d1325..3b9cfdbd4 100644 --- a/Demos/SpecializedTests/RagdollTubeDemo.cs +++ b/Demos/SpecializedTests/RagdollTubeDemo.cs @@ -20,7 +20,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi; camera.Pitch = 0; var filters = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks { CollisionFilters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks { CollisionFilters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), 1); int ragdollIndex = 0; var spacing = new Vector3(1.7f, 1.8f, 0.5f); From ee744d3dfd4b0eb0d03c9a2a2e8b5f9820cfea2e Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 29 Sep 2021 18:47:36 -0500 Subject: [PATCH 189/947] TypeProcessor jacobi solves implemented. Probably. --- .../Constraints/FourBodyTypeProcessor.cs | 89 ++++++++++++++++++- .../Constraints/OneBodyTypeProcessor.cs | 52 ++++++++++- .../Constraints/ThreeBodyTypeProcessor.cs | 76 +++++++++++++++- .../Constraints/TwoBodyTypeProcessor.cs | 72 +++++++++------ Demos/Demos/PyramidDemo.cs | 1 - 5 files changed, 255 insertions(+), 35 deletions(-) diff --git a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs index 3d07a3695..3f1378ee6 100644 --- a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs @@ -358,14 +358,97 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f } } - public override void JacobiWarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks poseIntegratorCallbacks, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) + + public override void JacobiWarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { - throw new NotImplementedException(); + var prestepBundles = typeBatch.PrestepData.As(); + var bodyReferencesBundles = typeBatch.BodyReferences.As(); + var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); + Unsafe.SkipInit(out TConstraintFunctions function); + ref var states = ref bodies.ActiveSet.SolverStates; + //EarlyPrefetch(WarmStartPrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref states, startBundle, exclusiveEndBundle); + ref var jacobiResultsBundlesA = ref jacobiResults.GetVelocitiesForBody(0); + ref var jacobiResultsBundlesB = ref jacobiResults.GetVelocitiesForBody(1); + ref var jacobiResultsBundlesC = ref jacobiResults.GetVelocitiesForBody(2); + ref var jacobiResultsBundlesD = ref jacobiResults.GetVelocitiesForBody(3); + for (int i = startBundle; i < exclusiveEndBundle; ++i) + { + ref var prestep = ref prestepBundles[i]; + ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; + ref var references = ref bodyReferencesBundles[i]; + ref var wsvA = ref jacobiResultsBundlesA[i]; + ref var wsvB = ref jacobiResultsBundlesB[i]; + ref var wsvC = ref jacobiResultsBundlesC[i]; + ref var wsvD = ref jacobiResultsBundlesD[i]; + var count = GetCountInBundle(ref typeBatch, i); + //Prefetch(WarmStartPrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref states, i, exclusiveEndBundle); + //Note jacobi batches do not do access filtering at the moment. The fallback accumulation expects all velocities to be present. + GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 0, dt, workerIndex, i, ref references.IndexA, count, + out var positionA, out var orientationA, out wsvA, out var inertiaA); + GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 1, dt, workerIndex, i, ref references.IndexB, count, + out var positionB, out var orientationB, out wsvB, out var inertiaB); + GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 2, dt, workerIndex, i, ref references.IndexC, count, + out var positionC, out var orientationC, out wsvC, out var inertiaC); + GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 3, dt, workerIndex, i, ref references.IndexD, count, + out var positionD, out var orientationD, out wsvC, out var inertiaD); + jacobiBatch.GetJacobiScaleForBodies(ref references, count, out var jacobiScaleA, out var jacobiScaleB, out var jacobiScaleC, out var jacobiScaleD); + Symmetric3x3Wide.Scale(inertiaA.InverseInertiaTensor, jacobiScaleA, out inertiaA.InverseInertiaTensor); + inertiaA.InverseMass *= jacobiScaleA; + Symmetric3x3Wide.Scale(inertiaB.InverseInertiaTensor, jacobiScaleB, out inertiaB.InverseInertiaTensor); + inertiaB.InverseMass *= jacobiScaleB; + Symmetric3x3Wide.Scale(inertiaC.InverseInertiaTensor, jacobiScaleC, out inertiaC.InverseInertiaTensor); + inertiaC.InverseMass *= jacobiScaleC; + Symmetric3x3Wide.Scale(inertiaD.InverseInertiaTensor, jacobiScaleD, out inertiaD.InverseInertiaTensor); + inertiaD.InverseMass *= jacobiScaleD; + + function.WarmStart2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, positionD, orientationD, inertiaD, + ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC, ref wsvD); + //Jacobi batches do not scatter velocities directly; they are handled in a postpass. + } } public override void JacobiSolveStep2(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { - throw new NotImplementedException(); + var prestepBundles = typeBatch.PrestepData.As(); + var bodyReferencesBundles = typeBatch.BodyReferences.As(); + var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); + Unsafe.SkipInit(out TConstraintFunctions function); + ref var motionStates = ref bodies.ActiveSet.SolverStates; + //EarlyPrefetch(SolvePrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, startBundle, exclusiveEndBundle); + ref var jacobiResultsBundlesA = ref jacobiResults.GetVelocitiesForBody(0); + ref var jacobiResultsBundlesB = ref jacobiResults.GetVelocitiesForBody(1); + ref var jacobiResultsBundlesC = ref jacobiResults.GetVelocitiesForBody(2); + ref var jacobiResultsBundlesD = ref jacobiResults.GetVelocitiesForBody(3); + for (int i = startBundle; i < exclusiveEndBundle; ++i) + { + ref var prestep = ref prestepBundles[i]; + ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; + ref var references = ref bodyReferencesBundles[i]; + var count = GetCountInBundle(ref typeBatch, i); + ref var wsvA = ref jacobiResultsBundlesA[i]; + ref var wsvB = ref jacobiResultsBundlesB[i]; + ref var wsvC = ref jacobiResultsBundlesC[i]; + ref var wsvD = ref jacobiResultsBundlesD[i]; + //Prefetch(SolvePrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, i, exclusiveEndBundle); + //Note jacobi batches do not do access filtering at the moment. The fallback accumulation expects all velocities to be present. + bodies.GatherState(ref references.IndexA, count, true, out var positionA, out var orientationA, out wsvA, out var inertiaA); + bodies.GatherState(ref references.IndexB, count, true, out var positionB, out var orientationB, out wsvB, out var inertiaB); + bodies.GatherState(ref references.IndexC, count, true, out var positionC, out var orientationC, out wsvC, out var inertiaC); + bodies.GatherState(ref references.IndexD, count, true, out var positionD, out var orientationD, out wsvD, out var inertiaD); + jacobiBatch.GetJacobiScaleForBodies(ref references, count, out var jacobiScaleA, out var jacobiScaleB, out var jacobiScaleC, out var jacobiScaleD); + Symmetric3x3Wide.Scale(inertiaA.InverseInertiaTensor, jacobiScaleA, out inertiaA.InverseInertiaTensor); + inertiaA.InverseMass *= jacobiScaleA; + Symmetric3x3Wide.Scale(inertiaB.InverseInertiaTensor, jacobiScaleB, out inertiaB.InverseInertiaTensor); + inertiaB.InverseMass *= jacobiScaleB; + Symmetric3x3Wide.Scale(inertiaC.InverseInertiaTensor, jacobiScaleC, out inertiaC.InverseInertiaTensor); + inertiaC.InverseMass *= jacobiScaleC; + Symmetric3x3Wide.Scale(inertiaD.InverseInertiaTensor, jacobiScaleD, out inertiaD.InverseInertiaTensor); + inertiaD.InverseMass *= jacobiScaleD; + + function.Solve2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, positionD, orientationD, inertiaD, + dt, inverseDt, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC, ref wsvD); + //Jacobi batches do not scatter velocities directly; they are handled in a postpass. + } } } } diff --git a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs index 7821b26d2..9ab4761a0 100644 --- a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs @@ -267,13 +267,59 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f } } - public override void JacobiWarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks poseIntegratorCallbacks, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) + public override void JacobiWarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { - throw new NotImplementedException(); + var prestepBundles = typeBatch.PrestepData.As(); + var bodyReferencesBundles = typeBatch.BodyReferences.As>(); + var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); + Unsafe.SkipInit(out TConstraintFunctions function); + ref var states = ref bodies.ActiveSet.SolverStates; + ref var jacobiResultsBundlesA = ref jacobiResults.GetVelocitiesForBody(0); + for (int i = startBundle; i < exclusiveEndBundle; ++i) + { + ref var prestep = ref prestepBundles[i]; + ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; + ref var references = ref bodyReferencesBundles[i]; + ref var wsvA = ref jacobiResultsBundlesA[i]; + var count = GetCountInBundle(ref typeBatch, i); + //Prefetch(WarmStartPrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref states, i, exclusiveEndBundle); + //Note jacobi batches do not do access filtering at the moment. The fallback accumulation expects all velocities to be present. + GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 0, dt, workerIndex, i, ref references, count, + out var positionA, out var orientationA, out wsvA, out var inertiaA); + jacobiBatch.GetJacobiScaleForBodies(ref references, count, out var jacobiScaleA); + Symmetric3x3Wide.Scale(inertiaA.InverseInertiaTensor, jacobiScaleA, out inertiaA.InverseInertiaTensor); + inertiaA.InverseMass *= jacobiScaleA; + function.WarmStart2(positionA, orientationA, inertiaA, ref prestep, ref accumulatedImpulses, ref wsvA); + //Jacobi batches do not scatter velocities directly; they are handled in a postpass. + } } + public override void JacobiSolveStep2(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { - throw new NotImplementedException(); + var prestepBundles = typeBatch.PrestepData.As(); + var bodyReferencesBundles = typeBatch.BodyReferences.As>(); + var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); + Unsafe.SkipInit(out TConstraintFunctions function); + ref var motionStates = ref bodies.ActiveSet.SolverStates; + //EarlyPrefetch(SolvePrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, startBundle, exclusiveEndBundle); + ref var jacobiResultsBundlesA = ref jacobiResults.GetVelocitiesForBody(0); + for (int i = startBundle; i < exclusiveEndBundle; ++i) + { + ref var prestep = ref prestepBundles[i]; + ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; + ref var references = ref bodyReferencesBundles[i]; + var count = GetCountInBundle(ref typeBatch, i); + ref var wsvA = ref jacobiResultsBundlesA[i]; + //Prefetch(SolvePrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, i, exclusiveEndBundle); + //Note jacobi batches do not do access filtering at the moment. The fallback accumulation expects all velocities to be present. + bodies.GatherState(ref references, count, true, out var positionA, out var orientationA, out wsvA, out var inertiaA); + jacobiBatch.GetJacobiScaleForBodies(ref references, count, out var jacobiScaleA); + Symmetric3x3Wide.Scale(inertiaA.InverseInertiaTensor, jacobiScaleA, out inertiaA.InverseInertiaTensor); + inertiaA.InverseMass *= jacobiScaleA; + + function.Solve2(positionA, orientationA, inertiaA, dt, inverseDt, ref prestep, ref accumulatedImpulses, ref wsvA); + //Jacobi batches do not scatter velocities directly; they are handled in a postpass. + } } } diff --git a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs index 94d4d331e..5fba5207e 100644 --- a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs @@ -328,14 +328,84 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f } } - public override void JacobiWarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks poseIntegratorCallbacks, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) + + public override void JacobiWarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { - throw new NotImplementedException(); + var prestepBundles = typeBatch.PrestepData.As(); + var bodyReferencesBundles = typeBatch.BodyReferences.As(); + var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); + Unsafe.SkipInit(out TConstraintFunctions function); + ref var states = ref bodies.ActiveSet.SolverStates; + //EarlyPrefetch(WarmStartPrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref states, startBundle, exclusiveEndBundle); + ref var jacobiResultsBundlesA = ref jacobiResults.GetVelocitiesForBody(0); + ref var jacobiResultsBundlesB = ref jacobiResults.GetVelocitiesForBody(1); + ref var jacobiResultsBundlesC = ref jacobiResults.GetVelocitiesForBody(2); + for (int i = startBundle; i < exclusiveEndBundle; ++i) + { + ref var prestep = ref prestepBundles[i]; + ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; + ref var references = ref bodyReferencesBundles[i]; + ref var wsvA = ref jacobiResultsBundlesA[i]; + ref var wsvB = ref jacobiResultsBundlesB[i]; + ref var wsvC = ref jacobiResultsBundlesC[i]; + var count = GetCountInBundle(ref typeBatch, i); + //Prefetch(WarmStartPrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref states, i, exclusiveEndBundle); + //Note jacobi batches do not do access filtering at the moment. The fallback accumulation expects all velocities to be present. + GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 0, dt, workerIndex, i, ref references.IndexA, count, + out var positionA, out var orientationA, out wsvA, out var inertiaA); + GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 1, dt, workerIndex, i, ref references.IndexB, count, + out var positionB, out var orientationB, out wsvB, out var inertiaB); + GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 2, dt, workerIndex, i, ref references.IndexC, count, + out var positionC, out var orientationC, out wsvC, out var inertiaC); + jacobiBatch.GetJacobiScaleForBodies(ref references, count, out var jacobiScaleA, out var jacobiScaleB, out var jacobiScaleC); + Symmetric3x3Wide.Scale(inertiaA.InverseInertiaTensor, jacobiScaleA, out inertiaA.InverseInertiaTensor); + inertiaA.InverseMass *= jacobiScaleA; + Symmetric3x3Wide.Scale(inertiaB.InverseInertiaTensor, jacobiScaleB, out inertiaB.InverseInertiaTensor); + inertiaB.InverseMass *= jacobiScaleB; + Symmetric3x3Wide.Scale(inertiaC.InverseInertiaTensor, jacobiScaleC, out inertiaC.InverseInertiaTensor); + inertiaC.InverseMass *= jacobiScaleC; + + function.WarmStart2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC); + //Jacobi batches do not scatter velocities directly; they are handled in a postpass. + } } public override void JacobiSolveStep2(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { - throw new NotImplementedException(); + var prestepBundles = typeBatch.PrestepData.As(); + var bodyReferencesBundles = typeBatch.BodyReferences.As(); + var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); + Unsafe.SkipInit(out TConstraintFunctions function); + ref var motionStates = ref bodies.ActiveSet.SolverStates; + //EarlyPrefetch(SolvePrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, startBundle, exclusiveEndBundle); + ref var jacobiResultsBundlesA = ref jacobiResults.GetVelocitiesForBody(0); + ref var jacobiResultsBundlesB = ref jacobiResults.GetVelocitiesForBody(1); + ref var jacobiResultsBundlesC = ref jacobiResults.GetVelocitiesForBody(2); + for (int i = startBundle; i < exclusiveEndBundle; ++i) + { + ref var prestep = ref prestepBundles[i]; + ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; + ref var references = ref bodyReferencesBundles[i]; + var count = GetCountInBundle(ref typeBatch, i); + ref var wsvA = ref jacobiResultsBundlesA[i]; + ref var wsvB = ref jacobiResultsBundlesB[i]; + ref var wsvC = ref jacobiResultsBundlesC[i]; + //Prefetch(SolvePrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, i, exclusiveEndBundle); + //Note jacobi batches do not do access filtering at the moment. The fallback accumulation expects all velocities to be present. + bodies.GatherState(ref references.IndexA, count, true, out var positionA, out var orientationA, out wsvA, out var inertiaA); + bodies.GatherState(ref references.IndexB, count, true, out var positionB, out var orientationB, out wsvB, out var inertiaB); + bodies.GatherState(ref references.IndexC, count, true, out var positionC, out var orientationC, out wsvC, out var inertiaC); + jacobiBatch.GetJacobiScaleForBodies(ref references, count, out var jacobiScaleA, out var jacobiScaleB, out var jacobiScaleC); + Symmetric3x3Wide.Scale(inertiaA.InverseInertiaTensor, jacobiScaleA, out inertiaA.InverseInertiaTensor); + inertiaA.InverseMass *= jacobiScaleA; + Symmetric3x3Wide.Scale(inertiaB.InverseInertiaTensor, jacobiScaleB, out inertiaB.InverseInertiaTensor); + inertiaB.InverseMass *= jacobiScaleB; + Symmetric3x3Wide.Scale(inertiaC.InverseInertiaTensor, jacobiScaleC, out inertiaC.InverseInertiaTensor); + inertiaC.InverseMass *= jacobiScaleC; + + function.Solve2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, dt, inverseDt, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC); + //Jacobi batches do not scatter velocities directly; they are handled in a postpass. + } } } } diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 03967e6c5..866bc18b2 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -321,7 +321,7 @@ public unsafe override void WarmStart2(); var bodyReferencesBundles = typeBatch.BodyReferences.As(); var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); - var function = default(TConstraintFunctions); + Unsafe.SkipInit(out TConstraintFunctions function); ref var states = ref bodies.ActiveSet.SolverStates; //EarlyPrefetch(WarmStartPrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref states, startBundle, exclusiveEndBundle); for (int i = startBundle; i < exclusiveEndBundle; ++i) @@ -362,7 +362,7 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f var prestepBundles = typeBatch.PrestepData.As(); var bodyReferencesBundles = typeBatch.BodyReferences.As(); var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); - var function = default(TConstraintFunctions); + Unsafe.SkipInit(out TConstraintFunctions function); ref var motionStates = ref bodies.ActiveSet.SolverStates; //EarlyPrefetch(SolvePrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, startBundle, exclusiveEndBundle); for (int i = startBundle; i < exclusiveEndBundle; ++i) @@ -387,45 +387,67 @@ public override void JacobiWarmStart2(); var bodyReferencesBundles = typeBatch.BodyReferences.As(); var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); - var function = default(TConstraintFunctions); + Unsafe.SkipInit(out TConstraintFunctions function); ref var states = ref bodies.ActiveSet.SolverStates; //EarlyPrefetch(WarmStartPrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref states, startBundle, exclusiveEndBundle); + ref var jacobiResultsBundlesA = ref jacobiResults.GetVelocitiesForBody(0); + ref var jacobiResultsBundlesB = ref jacobiResults.GetVelocitiesForBody(1); for (int i = startBundle; i < exclusiveEndBundle; ++i) { ref var prestep = ref prestepBundles[i]; ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; ref var references = ref bodyReferencesBundles[i]; + ref var wsvA = ref jacobiResultsBundlesA[i]; + ref var wsvB = ref jacobiResultsBundlesB[i]; var count = GetCountInBundle(ref typeBatch, i); //Prefetch(WarmStartPrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref states, i, exclusiveEndBundle); - GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 0, dt, workerIndex, i, ref references.IndexA, count, - out var positionA, out var orientationA, out var wsvA, out var inertiaA); - GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 1, dt, workerIndex, i, ref references.IndexB, count, - out var positionB, out var orientationB, out var wsvB, out var inertiaB); - - //if (typeof(TAllowPoseIntegration) == typeof(AllowPoseIntegration)) - // function.UpdateForNewPose(positionA, orientationA, inertiaA, wsvA, positionB, orientationB, inertiaB, wsvB, new Vector(dt), accumulatedImpulses, ref prestep); + //Note jacobi batches do not do access filtering at the moment. The fallback accumulation expects all velocities to be present. + GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 0, dt, workerIndex, i, ref references.IndexA, count, + out var positionA, out var orientationA, out wsvA, out var inertiaA); + GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 1, dt, workerIndex, i, ref references.IndexB, count, + out var positionB, out var orientationB, out wsvB, out var inertiaB); + jacobiBatch.GetJacobiScaleForBodies(ref references, count, out var jacobiScaleA, out var jacobiScaleB); + Symmetric3x3Wide.Scale(inertiaA.InverseInertiaTensor, jacobiScaleA, out inertiaA.InverseInertiaTensor); + inertiaA.InverseMass *= jacobiScaleA; + Symmetric3x3Wide.Scale(inertiaB.InverseInertiaTensor, jacobiScaleB, out inertiaB.InverseInertiaTensor); + inertiaB.InverseMass *= jacobiScaleB; function.WarmStart2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB); - - if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) - { - bodies.ScatterVelocities(ref wsvA, ref references.IndexA, count); - bodies.ScatterVelocities(ref wsvB, ref references.IndexB, count); - } - else - { - //This batch has some integrators, which means that every bundle is going to gather all velocities. - //(We don't make per-bundle determinations about this to avoid an extra branch and instruction complexity, and the difference is very small.) - bodies.ScatterVelocities(ref wsvA, ref references.IndexA, count); - bodies.ScatterVelocities(ref wsvB, ref references.IndexB, count); - } - + //Jacobi batches do not scatter velocities directly; they are handled in a postpass. } } public override void JacobiSolveStep2(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { - throw new NotImplementedException(); + var prestepBundles = typeBatch.PrestepData.As(); + var bodyReferencesBundles = typeBatch.BodyReferences.As(); + var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); + Unsafe.SkipInit(out TConstraintFunctions function); + ref var motionStates = ref bodies.ActiveSet.SolverStates; + //EarlyPrefetch(SolvePrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, startBundle, exclusiveEndBundle); + ref var jacobiResultsBundlesA = ref jacobiResults.GetVelocitiesForBody(0); + ref var jacobiResultsBundlesB = ref jacobiResults.GetVelocitiesForBody(1); + for (int i = startBundle; i < exclusiveEndBundle; ++i) + { + ref var prestep = ref prestepBundles[i]; + ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; + ref var references = ref bodyReferencesBundles[i]; + var count = GetCountInBundle(ref typeBatch, i); + ref var wsvA = ref jacobiResultsBundlesA[i]; + ref var wsvB = ref jacobiResultsBundlesB[i]; + //Prefetch(SolvePrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, i, exclusiveEndBundle); + //Note jacobi batches do not do access filtering at the moment. The fallback accumulation expects all velocities to be present. + bodies.GatherState(ref references.IndexA, count, true, out var positionA, out var orientationA, out wsvA, out var inertiaA); + bodies.GatherState(ref references.IndexB, count, true, out var positionB, out var orientationB, out wsvB, out var inertiaB); + jacobiBatch.GetJacobiScaleForBodies(ref references, count, out var jacobiScaleA, out var jacobiScaleB); + Symmetric3x3Wide.Scale(inertiaA.InverseInertiaTensor, jacobiScaleA, out inertiaA.InverseInertiaTensor); + inertiaA.InverseMass *= jacobiScaleA; + Symmetric3x3Wide.Scale(inertiaB.InverseInertiaTensor, jacobiScaleB, out inertiaB.InverseInertiaTensor); + inertiaB.InverseMass *= jacobiScaleB; + + function.Solve2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, dt, inverseDt, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB); + //Jacobi batches do not scatter velocities directly; they are handled in a postpass. + } } } diff --git a/Demos/Demos/PyramidDemo.cs b/Demos/Demos/PyramidDemo.cs index 821bff209..69bd395fa 100644 --- a/Demos/Demos/PyramidDemo.cs +++ b/Demos/Demos/PyramidDemo.cs @@ -25,7 +25,6 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //The PositionFirstTimestepper is the simplest timestepping mode, but since it integrates velocity into position at the start of the frame, directly modified velocities outside of the timestep //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. - //Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), solverIterationCount: 1); Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), solverIterationCount: 1); //Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); From 63dc38f7af2585519226705381b44a38da45c02d Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 29 Sep 2021 19:25:43 -0500 Subject: [PATCH 190/947] Single threaded fallback batches up and running for embedded substepper. Ignoring integration responsibilities, anyway. --- BepuPhysics/Solver_SubsteppingSolve2.cs | 69 ++++++++++++++++++++++- Demos/Demo.cs | 2 +- Demos/SpecializedTests/RagdollTubeDemo.cs | 2 +- 3 files changed, 68 insertions(+), 5 deletions(-) diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index be3688214..3b5a2a07e 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -136,6 +136,35 @@ private void WarmStartBlock(int workerIndex, int bat } } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + private void JacobianWarmStartBlock( + int workerIndex, int batchIndex, int typeBatchIndex, int startBundle, int endBundle, ref TypeBatch typeBatch, TypeProcessor typeProcessor, float dt, float inverseDt, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults) + where TBatchShouldIntegratePoses : unmanaged, IBatchPoseIntegrationAllowed + { + if (batchIndex == 0) //technically not disallowed... + { + Buffer noFlagsRequired = default; + typeProcessor.JacobiWarmStart2( + ref typeBatch, ref noFlagsRequired, bodies, ref PoseIntegrator.Callbacks, ref jacobiBatch, ref jacobiResults, + dt, inverseDt, startBundle, endBundle, workerIndex); + } + else + { + if (coarseBatchIntegrationResponsibilities[batchIndex][typeBatchIndex]) + { + typeProcessor.JacobiWarmStart2( + ref typeBatch, ref integrationFlags[batchIndex][typeBatchIndex], bodies, ref PoseIntegrator.Callbacks, ref jacobiBatch, ref jacobiResults, + dt, inverseDt, startBundle, endBundle, workerIndex); + } + else + { + typeProcessor.JacobiWarmStart2( + ref typeBatch, ref integrationFlags[batchIndex][typeBatchIndex], bodies, ref PoseIntegrator.Callbacks, ref jacobiBatch, ref jacobiResults, + dt, inverseDt, startBundle, endBundle, workerIndex); + } + } + } + //Split the solve process into a warmstart and solve, where warmstart doesn't try to store out anything. It just computes jacobians and modifies velocities according to the accumulated impulse. //The solve step then *recomputes* jacobians from prestep data and pose information. @@ -744,9 +773,10 @@ public override unsafe IndexSet PrepareConstraintIntegrationResponsibilities(int //Note that we bypass the constructor to avoid zeroing unnecessarily. Every bundle will be fully assigned. pool.Take(bundleCount, out bodiesFirstObservedInBatches[batchIndex].Flags); } + GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); //Note that we are not multithreading the batch merging phase. This typically takes a handful of microseconds. //You'd likely need millions of bodies before you'd see any substantial benefit from multithreading this. - for (int batchIndex = 1; batchIndex < ActiveSet.Batches.Count; ++batchIndex) + for (int batchIndex = 1; batchIndex < synchronizedBatchCount; ++batchIndex) { ref var batchHandles = ref batchReferencedHandles[batchIndex]; ref var firstObservedInBatch = ref bodiesFirstObservedInBatches[batchIndex]; @@ -860,7 +890,7 @@ public override unsafe IndexSet PrepareConstraintIntegrationResponsibilities(int } if (useSingleThreadedPath) { - for (int i = 1; i < ActiveSet.Batches.Count; ++i) + for (int i = 1; i < synchronizedBatchCount; ++i) { if (!batchHasAnyIntegrationResponsibilities[i]) continue; @@ -954,7 +984,6 @@ public override void SolveStep2(float totalDt, IThreadDispatcher threadDispatche var inverseDt = 1f / substepDt; ref var activeSet = ref ActiveSet; GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); - Debug.Assert(!fallbackExists, "Not handling this yet."); var incrementalUpdateFilter = default(IncrementalContactDataUpdateFilter); for (int substepIndex = 0; substepIndex < substepCount; ++substepIndex) @@ -989,6 +1018,26 @@ public override void SolveStep2(float totalDt, IThreadDispatcher threadDispatche } } } + Buffer fallbackResults = default; + if (fallbackExists) + { + ref var batch = ref activeSet.Batches[FallbackBatchThreshold]; + FallbackBatch.AllocateResults(this, pool, ref batch, out fallbackResults); + + for (int j = 0; j < batch.TypeBatches.Count; ++j) + { + ref var typeBatch = ref batch.TypeBatches[j]; + if (substepIndex == 0) + { + JacobianWarmStartBlock(0, FallbackBatchThreshold, j, 0, typeBatch.BundleCount, ref typeBatch, TypeProcessors[typeBatch.TypeId], substepDt, inverseDt, ref ActiveSet.Fallback, ref fallbackResults[j]); + } + else + { + JacobianWarmStartBlock(0, FallbackBatchThreshold, j, 0, typeBatch.BundleCount, ref typeBatch, TypeProcessors[typeBatch.TypeId], substepDt, inverseDt, ref ActiveSet.Fallback, ref fallbackResults[j]); + } + } + activeSet.Fallback.ScatterVelocities(bodies, this, ref fallbackResults, 0, activeSet.Fallback.BodyCount); + } for (int iterationIndex = 0; iterationIndex < IterationCount; ++iterationIndex) { @@ -1001,6 +1050,20 @@ public override void SolveStep2(float totalDt, IThreadDispatcher threadDispatche TypeProcessors[typeBatch.TypeId].SolveStep2(ref typeBatch, bodies, substepDt, inverseDt, 0, typeBatch.BundleCount); } } + if (fallbackExists) + { + ref var batch = ref activeSet.Batches[FallbackBatchThreshold]; + for (int j = 0; j < batch.TypeBatches.Count; ++j) + { + ref var typeBatch = ref batch.TypeBatches[j]; + TypeProcessors[typeBatch.TypeId].JacobiSolveStep2(ref typeBatch, bodies, ref ActiveSet.Fallback, ref fallbackResults[j], substepDt, inverseDt, 0, typeBatch.BundleCount); + } + activeSet.Fallback.ScatterVelocities(bodies, this, ref fallbackResults, 0, activeSet.Fallback.BodyCount); + } + } + if (fallbackExists) + { + FallbackBatch.DisposeResults(this, pool, ref activeSet.Batches[FallbackBatchThreshold], ref fallbackResults); } } } diff --git a/Demos/Demo.cs b/Demos/Demo.cs index 37ebe5190..5f3790da3 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -62,7 +62,7 @@ public virtual void Update(Window window, Camera camera, Input input, float dt) //fully decouple simulation and rendering rates across different threads. //(In either case, you'd also want to interpolate or extrapolate simulation results during rendering for smoothness.) //Note that taking steps of variable length can reduce stability. Gradual or one-off changes can work reasonably well. - Simulation.Timestep(1 / 60f, ThreadDispatcher); + Simulation.Timestep(1 / 60f); ////Here's an example of how it would look to use more frequent updates, but still with a fixed amount of time simulated per update call: //const float timeToSimulate = 1 / 60f; diff --git a/Demos/SpecializedTests/RagdollTubeDemo.cs b/Demos/SpecializedTests/RagdollTubeDemo.cs index 3b9cfdbd4..62c912c32 100644 --- a/Demos/SpecializedTests/RagdollTubeDemo.cs +++ b/Demos/SpecializedTests/RagdollTubeDemo.cs @@ -20,7 +20,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi; camera.Pitch = 0; var filters = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks { CollisionFilters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), 1); + Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks { CollisionFilters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), 1, 64); int ragdollIndex = 0; var spacing = new Vector3(1.7f, 1.8f, 0.5f); From 5d84f5dad011a369312da7a2a5140bf89e80d3ad Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 30 Sep 2021 14:44:20 -0500 Subject: [PATCH 191/947] Removed expectation of integration within jacobi batch. That's invalid; have to handle it as a prepass if you go that route. Added part of multithreaded jacobi solver. --- .../Constraints/FourBodyTypeProcessor.cs | 15 +- .../Constraints/OneBodyTypeProcessor.cs | 6 +- .../Constraints/ThreeBodyTypeProcessor.cs | 12 +- .../Constraints/TwoBodyTypeProcessor.cs | 9 +- BepuPhysics/Constraints/TypeProcessor.cs | 9 +- BepuPhysics/FallbackBatch.cs | 1 - BepuPhysics/Solver_SubsteppingSolve2.cs | 176 ++++++++++++------ 7 files changed, 135 insertions(+), 93 deletions(-) diff --git a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs index 3f1378ee6..da0b957ef 100644 --- a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs @@ -359,7 +359,7 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f } - public override void JacobiWarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) + public override void JacobiWarmStart2(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { var prestepBundles = typeBatch.PrestepData.As(); var bodyReferencesBundles = typeBatch.BodyReferences.As(); @@ -383,14 +383,11 @@ public override void JacobiWarmStart2(bodies, ref integratorCallbacks, ref integrationFlags, 0, dt, workerIndex, i, ref references.IndexA, count, - out var positionA, out var orientationA, out wsvA, out var inertiaA); - GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 1, dt, workerIndex, i, ref references.IndexB, count, - out var positionB, out var orientationB, out wsvB, out var inertiaB); - GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 2, dt, workerIndex, i, ref references.IndexC, count, - out var positionC, out var orientationC, out wsvC, out var inertiaC); - GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 3, dt, workerIndex, i, ref references.IndexD, count, - out var positionD, out var orientationD, out wsvC, out var inertiaD); + //Also, jacobi batches cannot integrate. + bodies.GatherState(ref references.IndexA, count, true, out var positionA, out var orientationA, out wsvA, out var inertiaA); + bodies.GatherState(ref references.IndexB, count, true, out var positionB, out var orientationB, out wsvB, out var inertiaB); + bodies.GatherState(ref references.IndexC, count, true, out var positionC, out var orientationC, out wsvC, out var inertiaC); + bodies.GatherState(ref references.IndexD, count, true, out var positionD, out var orientationD, out wsvD, out var inertiaD); jacobiBatch.GetJacobiScaleForBodies(ref references, count, out var jacobiScaleA, out var jacobiScaleB, out var jacobiScaleC, out var jacobiScaleD); Symmetric3x3Wide.Scale(inertiaA.InverseInertiaTensor, jacobiScaleA, out inertiaA.InverseInertiaTensor); inertiaA.InverseMass *= jacobiScaleA; diff --git a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs index 9ab4761a0..8c6515c42 100644 --- a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs @@ -267,7 +267,7 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f } } - public override void JacobiWarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) + public override void JacobiWarmStart2(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { var prestepBundles = typeBatch.PrestepData.As(); var bodyReferencesBundles = typeBatch.BodyReferences.As>(); @@ -284,8 +284,8 @@ public override void JacobiWarmStart2(bodies, ref integratorCallbacks, ref integrationFlags, 0, dt, workerIndex, i, ref references, count, - out var positionA, out var orientationA, out wsvA, out var inertiaA); + //Also, jacobi batches cannot integrate. + bodies.GatherState(ref references, count, true, out var positionA, out var orientationA, out wsvA, out var inertiaA); jacobiBatch.GetJacobiScaleForBodies(ref references, count, out var jacobiScaleA); Symmetric3x3Wide.Scale(inertiaA.InverseInertiaTensor, jacobiScaleA, out inertiaA.InverseInertiaTensor); inertiaA.InverseMass *= jacobiScaleA; diff --git a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs index 5fba5207e..100b922bf 100644 --- a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs @@ -329,7 +329,7 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f } - public override void JacobiWarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) + public override void JacobiWarmStart2(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { var prestepBundles = typeBatch.PrestepData.As(); var bodyReferencesBundles = typeBatch.BodyReferences.As(); @@ -351,12 +351,10 @@ public override void JacobiWarmStart2(bodies, ref integratorCallbacks, ref integrationFlags, 0, dt, workerIndex, i, ref references.IndexA, count, - out var positionA, out var orientationA, out wsvA, out var inertiaA); - GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 1, dt, workerIndex, i, ref references.IndexB, count, - out var positionB, out var orientationB, out wsvB, out var inertiaB); - GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 2, dt, workerIndex, i, ref references.IndexC, count, - out var positionC, out var orientationC, out wsvC, out var inertiaC); + //Also, jacobi batches cannot integrate. + bodies.GatherState(ref references.IndexA, count, true, out var positionA, out var orientationA, out wsvA, out var inertiaA); + bodies.GatherState(ref references.IndexB, count, true, out var positionB, out var orientationB, out wsvB, out var inertiaB); + bodies.GatherState(ref references.IndexC, count, true, out var positionC, out var orientationC, out wsvC, out var inertiaC); jacobiBatch.GetJacobiScaleForBodies(ref references, count, out var jacobiScaleA, out var jacobiScaleB, out var jacobiScaleC); Symmetric3x3Wide.Scale(inertiaA.InverseInertiaTensor, jacobiScaleA, out inertiaA.InverseInertiaTensor); inertiaA.InverseMass *= jacobiScaleA; diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 866bc18b2..436eff16c 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -382,7 +382,7 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f } } - public override void JacobiWarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) + public override void JacobiWarmStart2(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { var prestepBundles = typeBatch.PrestepData.As(); var bodyReferencesBundles = typeBatch.BodyReferences.As(); @@ -402,10 +402,9 @@ public override void JacobiWarmStart2(bodies, ref integratorCallbacks, ref integrationFlags, 0, dt, workerIndex, i, ref references.IndexA, count, - out var positionA, out var orientationA, out wsvA, out var inertiaA); - GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 1, dt, workerIndex, i, ref references.IndexB, count, - out var positionB, out var orientationB, out wsvB, out var inertiaB); + //Also, jacobi batches cannot integrate. + bodies.GatherState(ref references.IndexA, count, true, out var positionA, out var orientationA, out wsvA, out var inertiaA); + bodies.GatherState(ref references.IndexB, count, true, out var positionB, out var orientationB, out wsvB, out var inertiaB); jacobiBatch.GetJacobiScaleForBodies(ref references, count, out var jacobiScaleA, out var jacobiScaleB); Symmetric3x3Wide.Scale(inertiaA.InverseInertiaTensor, jacobiScaleA, out inertiaA.InverseInertiaTensor); inertiaA.InverseMass *= jacobiScaleA; diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index 26833acb0..188ad543e 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -143,12 +143,9 @@ public abstract void WarmStart2( - ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks poseIntegratorCallbacks, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, - float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) - where TIntegratorCallbacks : struct, IPoseIntegratorCallbacks - where TBatchIntegrationMode : unmanaged, IBatchIntegrationMode - where TAllowPoseIntegration : unmanaged, IBatchPoseIntegrationAllowed; + public abstract void JacobiWarmStart2( + ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, + float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex); public abstract void JacobiSolveStep2(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); diff --git a/BepuPhysics/FallbackBatch.cs b/BepuPhysics/FallbackBatch.cs index c66e53af4..f3be5593f 100644 --- a/BepuPhysics/FallbackBatch.cs +++ b/BepuPhysics/FallbackBatch.cs @@ -525,6 +525,5 @@ public void Dispose(BufferPool pool) bodyConstraintReferences.Dispose(pool); } } - } } diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index 3b5a2a07e..689fc144c 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -51,12 +51,14 @@ internal struct SubstepMultithreadingContext [FieldOffset(48)] public Buffer FallbackBlocks; [FieldOffset(64)] - public Buffer ConstraintBatchBoundaries; + public Buffer FallbackResults; [FieldOffset(80)] + public Buffer ConstraintBatchBoundaries; + [FieldOffset(96)] public float Dt; - [FieldOffset(84)] + [FieldOffset(100)] public float InverseDt; - [FieldOffset(88)] + [FieldOffset(104)] public int WorkerCount; @@ -72,7 +74,6 @@ internal struct SubstepMultithreadingContext /// [FieldOffset(384)] public int CompletedWorkBlockCount; - } @@ -136,36 +137,6 @@ private void WarmStartBlock(int workerIndex, int bat } } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - private void JacobianWarmStartBlock( - int workerIndex, int batchIndex, int typeBatchIndex, int startBundle, int endBundle, ref TypeBatch typeBatch, TypeProcessor typeProcessor, float dt, float inverseDt, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults) - where TBatchShouldIntegratePoses : unmanaged, IBatchPoseIntegrationAllowed - { - if (batchIndex == 0) //technically not disallowed... - { - Buffer noFlagsRequired = default; - typeProcessor.JacobiWarmStart2( - ref typeBatch, ref noFlagsRequired, bodies, ref PoseIntegrator.Callbacks, ref jacobiBatch, ref jacobiResults, - dt, inverseDt, startBundle, endBundle, workerIndex); - } - else - { - if (coarseBatchIntegrationResponsibilities[batchIndex][typeBatchIndex]) - { - typeProcessor.JacobiWarmStart2( - ref typeBatch, ref integrationFlags[batchIndex][typeBatchIndex], bodies, ref PoseIntegrator.Callbacks, ref jacobiBatch, ref jacobiResults, - dt, inverseDt, startBundle, endBundle, workerIndex); - } - else - { - typeProcessor.JacobiWarmStart2( - ref typeBatch, ref integrationFlags[batchIndex][typeBatchIndex], bodies, ref PoseIntegrator.Callbacks, ref jacobiBatch, ref jacobiResults, - dt, inverseDt, startBundle, endBundle, workerIndex); - } - } - } - - //Split the solve process into a warmstart and solve, where warmstart doesn't try to store out anything. It just computes jacobians and modifies velocities according to the accumulated impulse. //The solve step then *recomputes* jacobians from prestep data and pose information. //Why? Memory bandwidth. Redoing the calculation is cheaper than storing it out. @@ -193,6 +164,22 @@ public void Execute(Solver solver, int blockIndex, int workerIndex) } } + struct JacobiWarmStartStep2StageFunction : IStageFunction + { + public float Dt; + public float InverseDt; + public int SubstepIndex; + public Solver solver; + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void Execute(Solver solver, int blockIndex, int workerIndex) + { + ref var block = ref this.solver.substepContext.ConstraintBlocks[blockIndex]; + ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; + var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; + typeProcessor.JacobiWarmStart2(ref typeBatch, this.solver.bodies, ref this.solver.ActiveSet.Fallback, ref this.solver.substepContext.FallbackResults[block.TypeBatchIndex], Dt, InverseDt, block.StartBundle, block.End, workerIndex); + } + } struct SolveStep2StageFunction : IStageFunction { @@ -210,20 +197,21 @@ public void Execute(Solver solver, int blockIndex, int workerIndex) } } - //struct FallbackSolveStep2StageFunction : IStageFunction - //{ - // public float Dt; - // public float InverseDt; - - // [MethodImpl(MethodImplOptions.AggressiveInlining)] - // public void Execute(Solver solver, int blockIndex) - // { - // ref var block = ref solver.context.ConstraintBlocks.Blocks[blockIndex]; - // ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; - // var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; - // typeProcessor.JacobiSolveStep2(ref typeBatch, solver.bodies, ref solver.ActiveSet.Fallback, ref solver.context.FallbackResults[block.TypeBatchIndex], Dt, InverseDt, block.StartBundle, block.End); - // } - //} + struct JacobiSolveStep2StageFunction : IStageFunction + { + public float Dt; + public float InverseDt; + public Solver solver; + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void Execute(Solver solver, int blockIndex, int workerIndex) + { + ref var block = ref this.solver.substepContext.ConstraintBlocks[blockIndex]; + ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; + var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; + typeProcessor.JacobiSolveStep2(ref typeBatch, solver.bodies, ref this.solver.ActiveSet.Fallback, ref this.solver.substepContext.FallbackResults[block.TypeBatchIndex], Dt, InverseDt, block.StartBundle, block.End); + } + } struct IncrementalUpdateStageFunction : IStageFunction { @@ -335,7 +323,7 @@ void ExecuteMainStage(ref TStageFunction stageFunction, int work substepContext.CompletedWorkBlockCount = 0; } } - SubstepMultithreadingContext substepContext; + internal SubstepMultithreadingContext substepContext; @@ -370,6 +358,7 @@ void SolveStep2Worker2(int workerIndex) Debug.Assert(activeSet.Batches.Count > 0, "Don't dispatch if there are no constraints."); GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); + //TODO: Every single one of these offers up the same parameters. Could avoid the need to initialize any of them. var incrementalUpdateStage = new IncrementalUpdateStageFunction { Dt = substepContext.Dt, @@ -382,14 +371,37 @@ void SolveStep2Worker2(int workerIndex) InverseDt = substepContext.InverseDt, solver = this }; + var jacobiWarmstartStage = new JacobiWarmStartStep2StageFunction + { + Dt = substepContext.Dt, + InverseDt = substepContext.InverseDt, + solver = this + }; var solveStage = new SolveStep2StageFunction { Dt = substepContext.Dt, InverseDt = substepContext.InverseDt, solver = this }; + var jacobiSolveStage = new JacobiSolveStep2StageFunction + { + Dt = substepContext.Dt, + InverseDt = substepContext.InverseDt, + solver = this + }; - var baseStageCountInSubstep = synchronizedBatchCount * (1 + IterationCount); + //A thread is only allowed to claim a workblock if the claim index for that workblock matches the expected value- which is the claim index it would have from the last time it was executed. + //Each thread calculates what that claim index would have been based on the current sync index by subtracting the expected number of sync indices elapsed since last execution. + var syncStagesPerWarmStartOrSolve = synchronizedBatchCount; + //If a fallback exists, then each warm start/solve will have a final fallback batch stage and then a scatter stage. + if (fallbackExists) syncStagesPerWarmStartOrSolve += 2; + var baseStageCountInSubstep = syncStagesPerWarmStartOrSolve * (1 + IterationCount) + 1; + //All warmstarts and solves, plus an incremental contact update. First substep doesn't do an incremental contact update, but that's fine, it'll end up expecting 0. + var syncOffsetToPreviousSubstep = baseStageCountInSubstep + 1; + //To find the previous execution sync index of a constraint batch, we have to scan through all the constraint batches, but ALSO skip over the incremental contact update stage, hence + 1. + var syncOffsetToPreviousClaimOnBatchForWarmStart = syncStagesPerWarmStartOrSolve + 1; + //For solves, there is no incremental update in the way. + var syncOffsetToPreviousClaimOnBatchForSolve = syncStagesPerWarmStartOrSolve; if (workerIndex == 0) { //This is the main 'orchestrator' thread. It tracks execution progress and notifies other threads that's it's time to work. @@ -398,12 +410,17 @@ void SolveStep2Worker2(int workerIndex) { if (substepIndex > 0) { - ExecuteMainStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStart, ref substepContext.Stages[0], 1 + baseStageCountInSubstep, ref syncIndex); + ExecuteMainStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStart, ref substepContext.Stages[0], syncOffsetToPreviousSubstep, ref syncIndex); } for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) { warmstartStage.SubstepIndex = substepIndex; - ExecuteMainStage(ref warmstartStage, workerIndex, batchStarts[batchIndex], ref substepContext.Stages[batchIndex + 1], 1 + synchronizedBatchCount, ref syncIndex); + ExecuteMainStage(ref warmstartStage, workerIndex, batchStarts[batchIndex], ref substepContext.Stages[batchIndex + 1], syncOffsetToPreviousClaimOnBatchForWarmStart, ref syncIndex); + } + if (fallbackExists) + { + ExecuteMainStage(ref jacobiWarmstartStage, workerIndex, batchStarts[FallbackBatchThreshold], ref substepContext.Stages[FallbackBatchThreshold + 1], syncOffsetToPreviousClaimOnBatchForWarmStart, ref syncIndex); + //TODO: FALLBACK SCATTER } for (int iterationIndex = 0; iterationIndex < IterationCount; ++iterationIndex) { @@ -411,7 +428,12 @@ void SolveStep2Worker2(int workerIndex) { //Note that this is using a 'different' stage by index than the worker thread if the iteration index > 1. //That's totally fine- the warmstart/iteration stages share the same claims buffers per batch. They're redundant for the sake of easier indexing. - ExecuteMainStage(ref solveStage, workerIndex, batchStarts[batchIndex], ref substepContext.Stages[batchIndex + 1], synchronizedBatchCount, ref syncIndex); + ExecuteMainStage(ref solveStage, workerIndex, batchStarts[batchIndex], ref substepContext.Stages[batchIndex + 1], syncOffsetToPreviousClaimOnBatchForSolve, ref syncIndex); + } + if (fallbackExists) + { + ExecuteMainStage(ref jacobiSolveStage, workerIndex, batchStarts[FallbackBatchThreshold], ref substepContext.Stages[FallbackBatchThreshold + 1], syncOffsetToPreviousClaimOnBatchForSolve, ref syncIndex); + //TODO: FALLBACK SCATTER } } } @@ -485,6 +507,32 @@ void SolveStep2Worker2(int workerIndex) } + void BuildFallbackScatterWorkBlocks(int targetBlocksPerBatch, out Buffer workBlocks) + { + ref var activeSet = ref ActiveSet; + if (activeSet.Batches.Count > FallbackBatchThreshold) + { + //There is a fallback batch, so we need to create fallback work blocks for it. + var blockCount = Math.Min(targetBlocksPerBatch, ActiveSet.Fallback.BodyCount); + pool.Take(blockCount, out workBlocks); + var blocksList = new QuickList(workBlocks); + context.FallbackBlocks.Blocks = new QuickList(blockCount, pool); + var baseBodiesPerBlock = activeSet.Fallback.BodyCount / blockCount; + var remainder = activeSet.Fallback.BodyCount - baseBodiesPerBlock * blockCount; + int previousEnd = 0; + for (int i = 0; i < blockCount; ++i) + { + var bodiesInBlock = i < remainder ? baseBodiesPerBlock + 1 : baseBodiesPerBlock; + blocksList.AllocateUnsafely() = new FallbackScatterWorkBlock { Start = previousEnd, End = previousEnd += bodiesInBlock }; + } + workBlocks = workBlocks.Slice(blocksList.Count); + } + else + { + workBlocks = default; + } + } + //Buffer> debugStageWorkBlocksCompleted; protected void ExecuteMultithreaded2(float dt, IThreadDispatcher threadDispatcher, Action workDelegate) { @@ -508,6 +556,7 @@ protected void ExecuteMultithreaded2(float dt, IThreadDispatcher threadDispatche pool.Return(ref incrementalUpdateBatchBoundaries); //TODO: No need to create this in the first place. Doesn't really cost anything, but... substepContext.ConstraintBlocks = constraintBlocks.Span.Slice(constraintBlocks.Count); substepContext.IncrementalUpdateBlocks = incrementalBlocks.Span.Slice(incrementalBlocks.Count); + BuildFallbackScatterWorkBlocks(targetBlocksPerBatch, out substepContext.FallbackBlocks); //Not every batch will actually have work blocks associated with it; the batch compressor could be falling behind, which means older constraints could be at higher batches than they need to be, leaving gaps. //We don't want to include those empty batches as sync points in the solver. @@ -518,6 +567,11 @@ protected void ExecuteMultithreaded2(float dt, IThreadDispatcher threadDispatche var totalConstraintBatchWorkBlockCount = substepContext.ConstraintBatchBoundaries[^1]; var totalClaimCount = incrementalBlocks.Count + totalConstraintBatchWorkBlockCount; + if (ActiveSet.Batches.Count > FallbackBatchThreshold) + { + Debug.Assert(substepContext.FallbackBlocks.Length > 0); + FallbackBatch.AllocateResults(this, pool, ref ActiveSet.Batches[FallbackBatchThreshold], out substepContext.FallbackResults); + } //Claims will be monotonically increasing throughout execution. All should start at zero to match with the initial sync index. pool.Take(totalClaimCount, out var claims); claims.Clear(0, claims.Length); @@ -614,6 +668,11 @@ protected void ExecuteMultithreaded2(float dt, IThreadDispatcher threadDispatche //pool.Return(ref debugStageWorkBlocksCompleted); //pool.Return(ref availableCountPerSync); + if (ActiveSet.Batches.Count > FallbackBatchThreshold) + { + FallbackBatch.DisposeResults(this, pool, ref ActiveSet.Batches[FallbackBatchThreshold], ref substepContext.FallbackResults); + pool.Return(ref substepContext.FallbackBlocks); + } pool.Return(ref claims); pool.Return(ref substepContext.Stages); pool.Return(ref substepContext.ConstraintBatchBoundaries); @@ -1023,18 +1082,11 @@ public override void SolveStep2(float totalDt, IThreadDispatcher threadDispatche { ref var batch = ref activeSet.Batches[FallbackBatchThreshold]; FallbackBatch.AllocateResults(this, pool, ref batch, out fallbackResults); - + for (int j = 0; j < batch.TypeBatches.Count; ++j) { ref var typeBatch = ref batch.TypeBatches[j]; - if (substepIndex == 0) - { - JacobianWarmStartBlock(0, FallbackBatchThreshold, j, 0, typeBatch.BundleCount, ref typeBatch, TypeProcessors[typeBatch.TypeId], substepDt, inverseDt, ref ActiveSet.Fallback, ref fallbackResults[j]); - } - else - { - JacobianWarmStartBlock(0, FallbackBatchThreshold, j, 0, typeBatch.BundleCount, ref typeBatch, TypeProcessors[typeBatch.TypeId], substepDt, inverseDt, ref ActiveSet.Fallback, ref fallbackResults[j]); - } + TypeProcessors[typeBatch.TypeId].JacobiWarmStart2(ref typeBatch, bodies, ref ActiveSet.Fallback, ref fallbackResults[j], substepDt, inverseDt, 0, typeBatch.BundleCount, 0); } activeSet.Fallback.ScatterVelocities(bodies, this, ref fallbackResults, 0, activeSet.Fallback.BodyCount); } From 110c819cac55697705cafb5ee029bc8d143cf283 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 30 Sep 2021 17:47:27 -0500 Subject: [PATCH 192/947] Multithreaded execution runs again, but not when there's a fallback batch active. --- .../Constraints/TwoBodyTypeProcessor.cs | 20 +++ BepuPhysics/FallbackBatch.cs | 12 +- BepuPhysics/Solver_SubsteppingSolve2.cs | 158 +++++++++++++----- Demos/Demo.cs | 2 +- 4 files changed, 149 insertions(+), 43 deletions(-) diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 436eff16c..1a304d85c 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -411,7 +411,17 @@ public override void JacobiWarmStart2(ref TypeBatch typeBatch, Bodies bodies, re Symmetric3x3Wide.Scale(inertiaB.InverseInertiaTensor, jacobiScaleB, out inertiaB.InverseInertiaTensor); inertiaB.InverseMass *= jacobiScaleB; + + wsvA.Linear.Validate(count); + wsvA.Angular.Validate(count); + wsvB.Linear.Validate(count); + wsvB.Angular.Validate(count); function.WarmStart2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB); + + wsvA.Linear.Validate(count); + wsvA.Angular.Validate(count); + wsvB.Linear.Validate(count); + wsvB.Angular.Validate(count); //Jacobi batches do not scatter velocities directly; they are handled in a postpass. } } @@ -444,7 +454,17 @@ public override void JacobiSolveStep2(ref TypeBatch typeBatch, Bodies bodies, re Symmetric3x3Wide.Scale(inertiaB.InverseInertiaTensor, jacobiScaleB, out inertiaB.InverseInertiaTensor); inertiaB.InverseMass *= jacobiScaleB; + wsvA.Linear.Validate(count); + wsvA.Angular.Validate(count); + wsvB.Linear.Validate(count); + wsvB.Angular.Validate(count); + function.Solve2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, dt, inverseDt, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB); + + wsvA.Linear.Validate(count); + wsvA.Angular.Validate(count); + wsvB.Linear.Validate(count); + wsvB.Angular.Validate(count); //Jacobi batches do not scatter velocities directly; they are handled in a postpass. } } diff --git a/BepuPhysics/FallbackBatch.cs b/BepuPhysics/FallbackBatch.cs index f3be5593f..f2f130741 100644 --- a/BepuPhysics/FallbackBatch.cs +++ b/BepuPhysics/FallbackBatch.cs @@ -422,12 +422,12 @@ public void ScatterVelocities(Bodies bodies, Solver solver, ref Buffer Claims; public int BatchIndex; public int WorkBlockStartIndex; + public SolverStageType StageType; - public SolverSyncStage(Buffer claims, int workBlockStartIndex, int batchIndex = -1) + public SolverSyncStage(Buffer claims, int workBlockStartIndex, SolverStageType type, int batchIndex = -1) { Claims = claims; BatchIndex = batchIndex; WorkBlockStartIndex = workBlockStartIndex; + StageType = type; } } @@ -168,7 +172,6 @@ struct JacobiWarmStartStep2StageFunction : IStageFunction { public float Dt; public float InverseDt; - public int SubstepIndex; public Solver solver; [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -213,6 +216,20 @@ public void Execute(Solver solver, int blockIndex, int workerIndex) } } + struct JacobiScatterStageFunction : IStageFunction + { + public float Dt; + public float InverseDt; + public Solver solver; + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void Execute(Solver solver, int blockIndex, int workerIndex) + { + ref var block = ref this.solver.substepContext.FallbackBlocks[blockIndex]; + solver.ActiveSet.Fallback.ScatterVelocities(solver.bodies, solver, ref this.solver.substepContext.FallbackResults, block.Start, block.End); + } + } + struct IncrementalUpdateStageFunction : IStageFunction { public float Dt; @@ -340,7 +357,7 @@ void SolveStep2Worker2(int workerIndex) //data will remain in some cache that's reasonably close to the core. int workerCount = substepContext.WorkerCount; var incrementalUpdateWorkerStart = GetUniformlyDistributedStart(workerIndex, substepContext.IncrementalUpdateBlocks.Length, workerCount, 0); - int fallbackStart = GetUniformlyDistributedStart(workerIndex, substepContext.FallbackBlocks.Length, workerCount, 0); + int jacobiScatterStart = GetUniformlyDistributedStart(workerIndex, substepContext.FallbackBlocks.Length, workerCount, 0); Buffer batchStarts; ref var activeSet = ref ActiveSet; unsafe @@ -389,13 +406,19 @@ void SolveStep2Worker2(int workerIndex) InverseDt = substepContext.InverseDt, solver = this }; + var jacobiScatterStage = new JacobiScatterStageFunction + { + Dt = substepContext.Dt, + InverseDt = substepContext.InverseDt, + solver = this + }; //A thread is only allowed to claim a workblock if the claim index for that workblock matches the expected value- which is the claim index it would have from the last time it was executed. //Each thread calculates what that claim index would have been based on the current sync index by subtracting the expected number of sync indices elapsed since last execution. var syncStagesPerWarmStartOrSolve = synchronizedBatchCount; //If a fallback exists, then each warm start/solve will have a final fallback batch stage and then a scatter stage. if (fallbackExists) syncStagesPerWarmStartOrSolve += 2; - var baseStageCountInSubstep = syncStagesPerWarmStartOrSolve * (1 + IterationCount) + 1; + var baseStageCountInSubstep = syncStagesPerWarmStartOrSolve * (1 + IterationCount); //All warmstarts and solves, plus an incremental contact update. First substep doesn't do an incremental contact update, but that's fine, it'll end up expecting 0. var syncOffsetToPreviousSubstep = baseStageCountInSubstep + 1; //To find the previous execution sync index of a constraint batch, we have to scan through all the constraint batches, but ALSO skip over the incremental contact update stage, hence + 1. @@ -412,15 +435,15 @@ void SolveStep2Worker2(int workerIndex) { ExecuteMainStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStart, ref substepContext.Stages[0], syncOffsetToPreviousSubstep, ref syncIndex); } + warmstartStage.SubstepIndex = substepIndex; for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) { - warmstartStage.SubstepIndex = substepIndex; ExecuteMainStage(ref warmstartStage, workerIndex, batchStarts[batchIndex], ref substepContext.Stages[batchIndex + 1], syncOffsetToPreviousClaimOnBatchForWarmStart, ref syncIndex); } if (fallbackExists) { ExecuteMainStage(ref jacobiWarmstartStage, workerIndex, batchStarts[FallbackBatchThreshold], ref substepContext.Stages[FallbackBatchThreshold + 1], syncOffsetToPreviousClaimOnBatchForWarmStart, ref syncIndex); - //TODO: FALLBACK SCATTER + ExecuteMainStage(ref jacobiScatterStage, workerIndex, jacobiScatterStart, ref substepContext.Stages[FallbackBatchThreshold + 2], syncOffsetToPreviousClaimOnBatchForWarmStart, ref syncIndex); } for (int iterationIndex = 0; iterationIndex < IterationCount; ++iterationIndex) { @@ -433,7 +456,7 @@ void SolveStep2Worker2(int workerIndex) if (fallbackExists) { ExecuteMainStage(ref jacobiSolveStage, workerIndex, batchStarts[FallbackBatchThreshold], ref substepContext.Stages[FallbackBatchThreshold + 1], syncOffsetToPreviousClaimOnBatchForSolve, ref syncIndex); - //TODO: FALLBACK SCATTER + ExecuteMainStage(ref jacobiScatterStage, workerIndex, jacobiScatterStart, ref substepContext.Stages[FallbackBatchThreshold + 2], syncOffsetToPreviousClaimOnBatchForSolve, ref syncIndex); } } } @@ -455,7 +478,7 @@ void SolveStep2Worker2(int workerIndex) //No work yet available. spinWait.SpinOnce(); } - //Stages were set up prior to execution. Note that we don't attempt to ping pong buffers or anything; there are unique entries for every single stage. + //Stages were set up prior to execution. Note that we don't attempt to ping pong buffers or anything; workblock claim indices monotonically increase across the execution of the solver. //This guarantees that a worker thread can go idle and miss an arbitrary number of stages without blocking any progress. if (syncIndex == int.MinValue) { @@ -467,7 +490,7 @@ void SolveStep2Worker2(int workerIndex) syncIndexInSubstep += syncStepsSinceLast; while (true) { - var stageCountInSubstep = substepIndex > 0 ? baseStageCountInSubstep + 1 : baseStageCountInSubstep; + var stageCountInSubstep = substepIndex > 0 ? syncOffsetToPreviousSubstep : baseStageCountInSubstep; if (syncIndexInSubstep >= stageCountInSubstep) { syncIndexInSubstep -= stageCountInSubstep; @@ -478,27 +501,35 @@ void SolveStep2Worker2(int workerIndex) break; } } + //If it's the first substep index, there's no incremental update, so we jump straight into it. var stageIndex = substepIndex == 0 ? syncIndexInSubstep + 1 : syncIndexInSubstep; //Note that we're going to do a compare exchange that prevents any claim on work blocks that *arent* of the previous sync index, which means we need the previous sync index. //Storing that in a reliable way is annoying, so we derive it from syncIndex. - if (stageIndex == 0) - { - //Incremental update. - ref var stage = ref substepContext.Stages[stageIndex]; - ExecuteWorkerStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStart, 0, ref stage.Claims, 1 + baseStageCountInSubstep, syncIndex, ref substepContext.CompletedWorkBlockCount); - } - else if (stageIndex < 1 + synchronizedBatchCount) - { - //Warm start. - ref var stage = ref substepContext.Stages[stageIndex]; - warmstartStage.SubstepIndex = substepIndex; - ExecuteWorkerStage(ref warmstartStage, workerIndex, batchStarts[stage.BatchIndex], stage.WorkBlockStartIndex, ref stage.Claims, 1 + synchronizedBatchCount, syncIndex, ref substepContext.CompletedWorkBlockCount); - } - else + ref var stage = ref substepContext.Stages[stageIndex]; + switch (stage.StageType) { - //Solve. - ref var stage = ref substepContext.Stages[stageIndex]; - ExecuteWorkerStage(ref solveStage, workerIndex, batchStarts[stage.BatchIndex], stage.WorkBlockStartIndex, ref stage.Claims, synchronizedBatchCount, syncIndex, ref substepContext.CompletedWorkBlockCount); + case SolverStageType.IncrementalUpdate: + ExecuteWorkerStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStart, 0, ref stage.Claims, syncOffsetToPreviousSubstep, syncIndex, ref substepContext.CompletedWorkBlockCount); + break; + case SolverStageType.WarmStart: + warmstartStage.SubstepIndex = substepIndex; + ExecuteWorkerStage(ref warmstartStage, workerIndex, batchStarts[stage.BatchIndex], stage.WorkBlockStartIndex, ref stage.Claims, syncOffsetToPreviousClaimOnBatchForWarmStart, syncIndex, ref substepContext.CompletedWorkBlockCount); + break; + case SolverStageType.JacobiWarmStart: + ExecuteWorkerStage(ref jacobiWarmstartStage, workerIndex, batchStarts[stage.BatchIndex], stage.WorkBlockStartIndex, ref stage.Claims, syncOffsetToPreviousClaimOnBatchForWarmStart, syncIndex, ref substepContext.CompletedWorkBlockCount); + break; + case SolverStageType.JacobiWarmStartScatter: + ExecuteWorkerStage(ref jacobiScatterStage, workerIndex, jacobiScatterStart, stage.WorkBlockStartIndex, ref stage.Claims, syncOffsetToPreviousClaimOnBatchForWarmStart, syncIndex, ref substepContext.CompletedWorkBlockCount); + break; + case SolverStageType.Solve: + ExecuteWorkerStage(ref solveStage, workerIndex, batchStarts[stage.BatchIndex], stage.WorkBlockStartIndex, ref stage.Claims, syncOffsetToPreviousClaimOnBatchForSolve, syncIndex, ref substepContext.CompletedWorkBlockCount); + break; + case SolverStageType.JacobiSolve: + ExecuteWorkerStage(ref jacobiSolveStage, workerIndex, batchStarts[stage.BatchIndex], stage.WorkBlockStartIndex, ref stage.Claims, syncOffsetToPreviousClaimOnBatchForSolve, syncIndex, ref substepContext.CompletedWorkBlockCount); + break; + case SolverStageType.JacobiSolveScatter: + ExecuteWorkerStage(ref jacobiScatterStage, workerIndex, jacobiScatterStart, stage.WorkBlockStartIndex, ref stage.Claims, syncOffsetToPreviousClaimOnBatchForSolve, syncIndex, ref substepContext.CompletedWorkBlockCount); + break; } latestCompletedSyncIndex = syncIndex; @@ -561,37 +592,88 @@ protected void ExecuteMultithreaded2(float dt, IThreadDispatcher threadDispatche //Not every batch will actually have work blocks associated with it; the batch compressor could be falling behind, which means older constraints could be at higher batches than they need to be, leaving gaps. //We don't want to include those empty batches as sync points in the solver. GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); - var iterationCountPlusOne = 1 + IterationCount; - pool.Take(1 + synchronizedBatchCount * iterationCountPlusOne, out substepContext.Stages); substepContext.SyncIndex = 0; - var totalConstraintBatchWorkBlockCount = substepContext.ConstraintBatchBoundaries[^1]; var totalClaimCount = incrementalBlocks.Count + totalConstraintBatchWorkBlockCount; - if (ActiveSet.Batches.Count > FallbackBatchThreshold) + var stagesPerIteration = synchronizedBatchCount; + if (fallbackExists) { Debug.Assert(substepContext.FallbackBlocks.Length > 0); FallbackBatch.AllocateResults(this, pool, ref ActiveSet.Batches[FallbackBatchThreshold], out substepContext.FallbackResults); + totalClaimCount += substepContext.FallbackBlocks.Length; + stagesPerIteration += 2; //jacobi batch, plus scatter. } + pool.Take(1 + stagesPerIteration * (1 + IterationCount), out substepContext.Stages); //Claims will be monotonically increasing throughout execution. All should start at zero to match with the initial sync index. pool.Take(totalClaimCount, out var claims); claims.Clear(0, claims.Length); - substepContext.Stages[0] = new(claims.Slice(incrementalBlocks.Count), 0); + substepContext.Stages[0] = new(claims.Slice(incrementalBlocks.Count), 0, SolverStageType.IncrementalUpdate); //Note that we create redundant stages that share the same workblock targets and claims buffers. //This is just to make indexing a little simpler during the multithreaded work. int targetStageIndex = 1; - for (int i = 0; i < iterationCountPlusOne; ++i) + //Warm start. + int claimStart = incrementalBlocks.Count; + for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) { - int claimStart = incrementalBlocks.Count; + var stageIndex = targetStageIndex++; + var batchStart = batchIndex == 0 ? 0 : substepContext.ConstraintBatchBoundaries[batchIndex - 1]; + var workBlocksInBatch = substepContext.ConstraintBatchBoundaries[batchIndex] - batchStart; + substepContext.Stages[stageIndex] = new(claims.Slice(claimStart, workBlocksInBatch), batchStart, SolverStageType.WarmStart, batchIndex); + claimStart += workBlocksInBatch; + } + if (fallbackExists) + { + //Jacobi warm start. + var stageIndex = targetStageIndex++; + var batchStart = FallbackBatchThreshold == 0 ? 0 : substepContext.ConstraintBatchBoundaries[FallbackBatchThreshold]; + var workBlocksInBatch = substepContext.ConstraintBatchBoundaries[FallbackBatchThreshold] - batchStart; + substepContext.Stages[stageIndex] = new(claims.Slice(claimStart, workBlocksInBatch), batchStart, SolverStageType.JacobiWarmStart); + claimStart += workBlocksInBatch; + + //Jacobi warm start scatter. + stageIndex = targetStageIndex++; + substepContext.Stages[stageIndex] = new(claims.Slice(claimStart, substepContext.FallbackBlocks.Length), 0, SolverStageType.JacobiWarmStartScatter); + } + for (int iterationIndex = 0; iterationIndex < IterationCount; ++iterationIndex) + { + //Solve. Note that we're reusing the same claims as were used in the warm start for these stages; the stages just tell the workers what kind of work to do. + claimStart = incrementalBlocks.Count; for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) { var stageIndex = targetStageIndex++; var batchStart = batchIndex == 0 ? 0 : substepContext.ConstraintBatchBoundaries[batchIndex - 1]; var workBlocksInBatch = substepContext.ConstraintBatchBoundaries[batchIndex] - batchStart; - substepContext.Stages[stageIndex] = new(claims.Slice(claimStart, workBlocksInBatch), batchStart, batchIndex); + substepContext.Stages[stageIndex] = new(claims.Slice(claimStart, workBlocksInBatch), batchStart, SolverStageType.Solve, batchIndex); claimStart += workBlocksInBatch; } + if (fallbackExists) + { + //Jacobi solve. + var stageIndex = targetStageIndex++; + var batchStart = FallbackBatchThreshold == 0 ? 0 : substepContext.ConstraintBatchBoundaries[FallbackBatchThreshold]; + var workBlocksInBatch = substepContext.ConstraintBatchBoundaries[FallbackBatchThreshold] - batchStart; + substepContext.Stages[stageIndex] = new(claims.Slice(claimStart, workBlocksInBatch), batchStart, SolverStageType.JacobiSolve); + claimStart += workBlocksInBatch; + + //Jacobi solve scatter. + stageIndex = targetStageIndex++; + substepContext.Stages[stageIndex] = new(claims.Slice(claimStart, substepContext.FallbackBlocks.Length), 0, SolverStageType.JacobiSolveScatter); + } } + //for (int i = 0; i < iterationCountPlusOne; ++i) + //{ + // int claimStart = incrementalBlocks.Count; + // for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) + // { + // var stageIndex = targetStageIndex++; + // var batchStart = batchIndex == 0 ? 0 : substepContext.ConstraintBatchBoundaries[batchIndex - 1]; + // var workBlocksInBatch = substepContext.ConstraintBatchBoundaries[batchIndex] - batchStart; + // substepContext.Stages[stageIndex] = new(claims.Slice(claimStart, workBlocksInBatch), batchStart, batchIndex); + // claimStart += workBlocksInBatch; + // } + //} + //var syncCount = substepCount * (1 + synchronizedBatchCount * (1 + IterationCount)) - 1; //pool.Take(syncCount, out debugStageWorkBlocksCompleted); //pool.Take(syncCount * workerCount, out var workBlocksCompleted); @@ -668,7 +750,7 @@ protected void ExecuteMultithreaded2(float dt, IThreadDispatcher threadDispatche //pool.Return(ref debugStageWorkBlocksCompleted); //pool.Return(ref availableCountPerSync); - if (ActiveSet.Batches.Count > FallbackBatchThreshold) + if (fallbackExists) { FallbackBatch.DisposeResults(this, pool, ref ActiveSet.Batches[FallbackBatchThreshold], ref substepContext.FallbackResults); pool.Return(ref substepContext.FallbackBlocks); diff --git a/Demos/Demo.cs b/Demos/Demo.cs index 5f3790da3..37ebe5190 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -62,7 +62,7 @@ public virtual void Update(Window window, Camera camera, Input input, float dt) //fully decouple simulation and rendering rates across different threads. //(In either case, you'd also want to interpolate or extrapolate simulation results during rendering for smoothness.) //Note that taking steps of variable length can reduce stability. Gradual or one-off changes can work reasonably well. - Simulation.Timestep(1 / 60f); + Simulation.Timestep(1 / 60f, ThreadDispatcher); ////Here's an example of how it would look to use more frequent updates, but still with a fixed amount of time simulated per update call: //const float timeToSimulate = 1 / 60f; From abc93f12d24e9d776a878663c70959434f327b11 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 30 Sep 2021 18:09:49 -0500 Subject: [PATCH 193/947] Missing filter in ragdoll tube fixed. --- Demos/SpecializedTests/RagdollTubeDemo.cs | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/Demos/SpecializedTests/RagdollTubeDemo.cs b/Demos/SpecializedTests/RagdollTubeDemo.cs index 62c912c32..fa1bcca0e 100644 --- a/Demos/SpecializedTests/RagdollTubeDemo.cs +++ b/Demos/SpecializedTests/RagdollTubeDemo.cs @@ -20,7 +20,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi; camera.Pitch = 0; var filters = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks { CollisionFilters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), 1, 64); + Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks { CollisionFilters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), 1, 8); int ragdollIndex = 0; var spacing = new Vector3(1.7f, 1.8f, 0.5f); @@ -55,7 +55,8 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) builder.AddForKinematic(Simulation.Shapes.Add(new Box(1, 2, panelShape.Length)), new RigidPose(new Vector3(0, tubeRadius - 1, 0)), 0); builder.BuildKinematicCompound(out var children); var compound = new BigCompound(children, Simulation.Shapes, BufferPool); - Simulation.Bodies.Add(BodyDescription.CreateKinematic(tubeCenter, new BodyVelocity(default, new Vector3(0, 0, .25f)), new CollidableDescription(Simulation.Shapes.Add(compound), 0.1f), new BodyActivityDescription())); + var tubeHandle = Simulation.Bodies.Add(BodyDescription.CreateKinematic(tubeCenter, new BodyVelocity(default, new Vector3(0, 0, .25f)), new CollidableDescription(Simulation.Shapes.Add(compound), 0.1f), new BodyActivityDescription())); + filters[tubeHandle] = new SubgroupCollisionFilter(int.MaxValue); builder.Dispose(); var staticShape = new Box(300, 1, 300); From 1ce57ad3d028368d8b057758ae042936f36e82ce Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 30 Sep 2021 18:58:41 -0500 Subject: [PATCH 194/947] Multithreaded jacobi fallback back up and running. Fallback batch still does not handle pose integration. --- .../Constraints/TwoBodyTypeProcessor.cs | 22 +------------------ BepuPhysics/FallbackBatch.cs | 16 +++++++------- BepuPhysics/Solver_SubsteppingSolve2.cs | 11 +++++----- Demos/Program.cs | 1 + Demos/SpecializedTests/RagdollTubeDemo.cs | 2 +- 5 files changed, 17 insertions(+), 35 deletions(-) diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 1a304d85c..019195093 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -410,18 +410,8 @@ public override void JacobiWarmStart2(ref TypeBatch typeBatch, Bodies bodies, re inertiaA.InverseMass *= jacobiScaleA; Symmetric3x3Wide.Scale(inertiaB.InverseInertiaTensor, jacobiScaleB, out inertiaB.InverseInertiaTensor); inertiaB.InverseMass *= jacobiScaleB; - - - wsvA.Linear.Validate(count); - wsvA.Angular.Validate(count); - wsvB.Linear.Validate(count); - wsvB.Angular.Validate(count); + function.WarmStart2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB); - - wsvA.Linear.Validate(count); - wsvA.Angular.Validate(count); - wsvB.Linear.Validate(count); - wsvB.Angular.Validate(count); //Jacobi batches do not scatter velocities directly; they are handled in a postpass. } } @@ -454,17 +444,7 @@ public override void JacobiSolveStep2(ref TypeBatch typeBatch, Bodies bodies, re Symmetric3x3Wide.Scale(inertiaB.InverseInertiaTensor, jacobiScaleB, out inertiaB.InverseInertiaTensor); inertiaB.InverseMass *= jacobiScaleB; - wsvA.Linear.Validate(count); - wsvA.Angular.Validate(count); - wsvB.Linear.Validate(count); - wsvB.Angular.Validate(count); - function.Solve2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, dt, inverseDt, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB); - - wsvA.Linear.Validate(count); - wsvA.Angular.Validate(count); - wsvB.Linear.Validate(count); - wsvB.Angular.Validate(count); //Jacobi batches do not scatter velocities directly; they are handled in a postpass. } } diff --git a/BepuPhysics/FallbackBatch.cs b/BepuPhysics/FallbackBatch.cs index f2f130741..edc918d54 100644 --- a/BepuPhysics/FallbackBatch.cs +++ b/BepuPhysics/FallbackBatch.cs @@ -422,12 +422,12 @@ public void ScatterVelocities(Bodies bodies, Solver solver, ref Buffer(ref TStageFunction stageFunction, int work { Thread.SpinWait(3); } + //Console.WriteLine($"Completed blocks count: {substepContext.CompletedWorkBlockCount}."); //All workers are done. We can safely reset the counter for the next time this stage is used. substepContext.CompletedWorkBlockCount = 0; } @@ -593,7 +594,7 @@ protected void ExecuteMultithreaded2(float dt, IThreadDispatcher threadDispatche //We don't want to include those empty batches as sync points in the solver. GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); substepContext.SyncIndex = 0; - var totalConstraintBatchWorkBlockCount = substepContext.ConstraintBatchBoundaries[^1]; + var totalConstraintBatchWorkBlockCount = substepContext.ConstraintBatchBoundaries.Length == 0 ? 0 : substepContext.ConstraintBatchBoundaries[^1]; var totalClaimCount = incrementalBlocks.Count + totalConstraintBatchWorkBlockCount; var stagesPerIteration = synchronizedBatchCount; if (fallbackExists) @@ -625,9 +626,9 @@ protected void ExecuteMultithreaded2(float dt, IThreadDispatcher threadDispatche { //Jacobi warm start. var stageIndex = targetStageIndex++; - var batchStart = FallbackBatchThreshold == 0 ? 0 : substepContext.ConstraintBatchBoundaries[FallbackBatchThreshold]; + var batchStart = FallbackBatchThreshold == 0 ? 0 : substepContext.ConstraintBatchBoundaries[FallbackBatchThreshold - 1]; var workBlocksInBatch = substepContext.ConstraintBatchBoundaries[FallbackBatchThreshold] - batchStart; - substepContext.Stages[stageIndex] = new(claims.Slice(claimStart, workBlocksInBatch), batchStart, SolverStageType.JacobiWarmStart); + substepContext.Stages[stageIndex] = new(claims.Slice(claimStart, workBlocksInBatch), batchStart, SolverStageType.JacobiWarmStart, FallbackBatchThreshold); claimStart += workBlocksInBatch; //Jacobi warm start scatter. @@ -650,9 +651,9 @@ protected void ExecuteMultithreaded2(float dt, IThreadDispatcher threadDispatche { //Jacobi solve. var stageIndex = targetStageIndex++; - var batchStart = FallbackBatchThreshold == 0 ? 0 : substepContext.ConstraintBatchBoundaries[FallbackBatchThreshold]; + var batchStart = FallbackBatchThreshold == 0 ? 0 : substepContext.ConstraintBatchBoundaries[FallbackBatchThreshold - 1]; var workBlocksInBatch = substepContext.ConstraintBatchBoundaries[FallbackBatchThreshold] - batchStart; - substepContext.Stages[stageIndex] = new(claims.Slice(claimStart, workBlocksInBatch), batchStart, SolverStageType.JacobiSolve); + substepContext.Stages[stageIndex] = new(claims.Slice(claimStart, workBlocksInBatch), batchStart, SolverStageType.JacobiSolve, FallbackBatchThreshold); claimStart += workBlocksInBatch; //Jacobi solve scatter. diff --git a/Demos/Program.cs b/Demos/Program.cs index 668a4ab43..62f8240e6 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -22,6 +22,7 @@ static void Main(string[] args) //HeadlessTest.Test(content, 4, 32, 512); //HeadlessTest.Test(content, 4, 32, 512); //HeadlessTest.Test(content, 4, 32, 512); + //HeadlessTest.Test(content, 4, 256, 512); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); diff --git a/Demos/SpecializedTests/RagdollTubeDemo.cs b/Demos/SpecializedTests/RagdollTubeDemo.cs index fa1bcca0e..8313eb8a6 100644 --- a/Demos/SpecializedTests/RagdollTubeDemo.cs +++ b/Demos/SpecializedTests/RagdollTubeDemo.cs @@ -20,7 +20,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi; camera.Pitch = 0; var filters = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks { CollisionFilters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), 1, 8); + Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks { CollisionFilters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), 1, 64); int ragdollIndex = 0; var spacing = new Vector3(1.7f, 1.8f, 0.5f); From 7ba9e3688676c76e5bab1e7669a8c0dd2db8d2e6 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 1 Oct 2021 15:57:29 -0500 Subject: [PATCH 195/947] IndexSet now makes a distinction between adding and setting. --- BepuUtilities/Collections/IndexSet.cs | 38 +++++++++++++++++++++------ 1 file changed, 30 insertions(+), 8 deletions(-) diff --git a/BepuUtilities/Collections/IndexSet.cs b/BepuUtilities/Collections/IndexSet.cs index dadb1da0e..150c00960 100644 --- a/BepuUtilities/Collections/IndexSet.cs +++ b/BepuUtilities/Collections/IndexSet.cs @@ -79,14 +79,40 @@ public unsafe bool CanFit(Span indexList) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - void AddUnsafely(int index, int bundleIndex) + void SetUnsafely(int index, int bundleIndex) { ref var bundle = ref Flags[bundleIndex]; var slot = 1ul << (index & mask); - Debug.Assert((bundle & slot) == 0, "Cannot add if it's already present!"); //Not much point in branching to stop a single instruction that doesn't change the result. bundle |= slot; } + + /// + /// Sets an index in the set to contained without checking capacity or whether it is already set. + /// + /// Index to add. + /// This is functionally identical to the Add method, but it doesn't include the same debug assertions. Just a way to make intent clear so that the assert can catch errors. + public void SetUnsafely(int index) + { + SetUnsafely(index, index >> shift); + } + + /// + /// Adds an index to the set without checking capacity. + /// + /// Index to add. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void AddUnsafely(int index) + { + Debug.Assert((Flags[index >> shift] & (1ul << (index & mask))) == 0, "Cannot add if it's already present!"); + SetUnsafely(index, index >> shift); + } + + /// + /// Adds an index to the set. + /// + /// Index to add. + /// Pool to use to resize the set if necessary. [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Add(int index, BufferPool pool) { @@ -96,14 +122,10 @@ public void Add(int index, BufferPool pool) //Note that the bundle index may be larger than two times the current capacity, since indices are not guaranteed to be appended. InternalResizeForBundleCount(pool, 1 << SpanHelper.GetContainingPowerOf2(bundleIndex + 1)); } - AddUnsafely(index, bundleIndex); + Debug.Assert((Flags[index >> shift] & (1ul << (index & mask))) == 0, "Cannot add if it's already present!"); + SetUnsafely(index, bundleIndex); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void AddUnsafely(int index) - { - AddUnsafely(index, index >> shift); - } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Remove(int index) From d85c0058c1228ec1be37397c5d35c8157d663695 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 1 Oct 2021 17:40:37 -0500 Subject: [PATCH 196/947] IndexSet now has a capacity-testing Set. --- BepuUtilities/Collections/IndexSet.cs | 23 ++++++++++++++++++++++- 1 file changed, 22 insertions(+), 1 deletion(-) diff --git a/BepuUtilities/Collections/IndexSet.cs b/BepuUtilities/Collections/IndexSet.cs index 150c00960..909298514 100644 --- a/BepuUtilities/Collections/IndexSet.cs +++ b/BepuUtilities/Collections/IndexSet.cs @@ -91,12 +91,33 @@ void SetUnsafely(int index, int bundleIndex) /// Sets an index in the set to contained without checking capacity or whether it is already set. /// /// Index to add. - /// This is functionally identical to the Add method, but it doesn't include the same debug assertions. Just a way to make intent clear so that the assert can catch errors. + /// This is functionally identical to the AddUnsafely method, but it doesn't include the same debug assertions. Just a way to make intent clear so that the assert can catch errors. + [MethodImpl(MethodImplOptions.AggressiveInlining)] public void SetUnsafely(int index) { SetUnsafely(index, index >> shift); } + /// + /// Sets an index in the set to contained without checking whether it is already set. + /// + /// Index to add. + /// Pool to reuse the set if necessary. + /// This is functionally identical to the Add method, but it doesn't include the same debug assertions. Just a way to make intent clear so that the assert can catch errors. + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void Set(int index, BufferPool pool) + { + var bundleIndex = index >> shift; + if (bundleIndex >= Flags.Length) + { + //Note that the bundle index may be larger than two times the current capacity, since indices are not guaranteed to be appended. + InternalResizeForBundleCount(pool, 1 << SpanHelper.GetContainingPowerOf2(bundleIndex + 1)); + } + SetUnsafely(index, index >> shift); + } + + /// /// Adds an index to the set without checking capacity. /// From 276a548276bfc04574cce50bd00abcdc9505ebe4 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 1 Oct 2021 19:57:24 -0500 Subject: [PATCH 197/947] Fallback batch tracking batch handles for the purposes of computing integration responsibilities. Debugging some corner cases. --- BepuPhysics/BatchCompressor.cs | 2 +- .../CollisionDetection/ConstraintRemover.cs | 10 ++- BepuPhysics/ConstraintBatch.cs | 29 +++--- BepuPhysics/Constraints/TypeProcessor.cs | 8 +- BepuPhysics/FallbackBatch.cs | 18 +++- BepuPhysics/IslandAwakener.cs | 19 ++-- BepuPhysics/Solver.cs | 89 +++++++++---------- BepuPhysics/Solver_SubsteppingSolve2.cs | 60 +++++++++++-- Demos/DemoSet.cs | 1 + Demos/Demos/Characters/CharacterDemo.cs | 2 +- Demos/Demos/CompoundTestDemo.cs | 2 +- Demos/Demos/FountainStressTestDemo.cs | 3 +- Demos/Demos/NewtDemo.cs | 10 +-- Demos/Demos/RagdollDemo.cs | 2 +- Demos/Demos/SubsteppingDemo.cs | 4 +- Demos/SpecializedTests/RagdollTubeDemo.cs | 14 ++- 16 files changed, 162 insertions(+), 111 deletions(-) diff --git a/BepuPhysics/BatchCompressor.cs b/BepuPhysics/BatchCompressor.cs index e0b78e558..523430da7 100644 --- a/BepuPhysics/BatchCompressor.cs +++ b/BepuPhysics/BatchCompressor.cs @@ -146,7 +146,7 @@ unsafe void DoJob(ref AnalysisRegion region, int workerIndex, BufferPool pool) typeProcessor.EnumerateConnectedBodyIndices(ref typeBatch, i, ref handleAccumulator); for (int batchIndex = 0; batchIndex < nextBatchIndex; ++batchIndex) { - //The batch index will never be the fallback batch, since the fallback batch is the very last batch (if it exists at all). So uses batch referenced handles is safe. + //The batch index will never be the fallback batch, since the fallback batch is the very last batch (if it exists at all). So uses of batch referenced handles is safe. if (Solver.batchReferencedHandles[batchIndex].CanFit(bodyHandlesSpan)) { compressions.Add(new Compression { ConstraintHandle = typeBatch.IndexToHandle[i], TargetBatch = batchIndex }, pool); diff --git a/BepuPhysics/CollisionDetection/ConstraintRemover.cs b/BepuPhysics/CollisionDetection/ConstraintRemover.cs index 51b994ccf..e85b49d88 100644 --- a/BepuPhysics/CollisionDetection/ConstraintRemover.cs +++ b/BepuPhysics/CollisionDetection/ConstraintRemover.cs @@ -247,7 +247,7 @@ public int Compare(ref TypeBatchIndex a, ref TypeBatchIndex b) //in sequence at that point- sequential removes would cost around 5us in that case, so any kind of multithreaded overhead can overwhelm the work being done. //Doubling the cost of the best case, resulting in handfuls of wasted microseconds, isn't concerning (and we could special case it if we really wanted to). //Cutting the cost of the worst case when thousands of constraints get removed by a factor of ~ThreadCount is worth this complexity. Frame spikes are evil! - + RemovalCache batches; /// /// Processes enqueued constraint removals and prepares removal jobs. @@ -352,7 +352,7 @@ public void RemoveConstraintsFromBatchReferencedHandles() { if (batches.TypeBatches[i].Batch == solver.FallbackBatchThreshold) { - //Batch referenced handles do not exist for the fallback batch. + //Batch referenced handles for the fallback are handled in RemoveConstraintsFromFallbackBatch. continue; } ref var removals = ref batches.RemovalsForTypeBatches[i].PerBodyRemovalTargets; @@ -376,7 +376,11 @@ public void RemoveConstraintsFromFallbackBatch() for (int j = 0; j < removals.Count; ++j) { ref var target = ref removals[j]; - solver.ActiveSet.Fallback.Remove(target.BodyIndex, target.ConstraintHandle, ref allocationIdsToFree); + if (solver.ActiveSet.Fallback.Remove(target.BodyIndex, target.ConstraintHandle, ref allocationIdsToFree)) + { + //No more constraints for this body in the fallback set; it should not exist in the fallback batch's referenced handles anymore. + solver.batchReferencedHandles[target.BatchIndex].Remove(target.BodyHandle.Value); + } } } } diff --git a/BepuPhysics/ConstraintBatch.cs b/BepuPhysics/ConstraintBatch.cs index 033a193f5..c5a9c81cc 100644 --- a/BepuPhysics/ConstraintBatch.cs +++ b/BepuPhysics/ConstraintBatch.cs @@ -206,33 +206,24 @@ public void RemoveTypeBatchIfEmpty(ref TypeBatch typeBatch, int typeBatchIndexTo } ValidateTypeBatchMappings(); } - - public unsafe void RemoveWithHandles(int constraintTypeId, int indexInTypeBatch, IndexSet* handles, Solver solver) + public unsafe void RemoveBodyHandlesFromBatchForConstraint(int constraintTypeId, int indexInTypeBatch, int batchIndex, Solver solver) { - Debug.Assert(TypeIndexToTypeBatchIndex[constraintTypeId] >= 0, "Type index must actually exist within this batch."); - + Debug.Assert(batchIndex <= solver.FallbackBatchThreshold, "This should only be used for non-fallback batches. The body handles set for a fallback batch should be handled by the fallback batch's remove call."); + var indexSet = solver.batchReferencedHandles.GetPointer(batchIndex); + var handleRemover = new ActiveBodyHandleRemover(solver.bodies, indexSet); var typeBatchIndex = TypeIndexToTypeBatchIndex[constraintTypeId]; - var handleRemover = new ActiveBodyHandleRemover(solver.bodies, handles); - ref var typeBatch = ref TypeBatches[typeBatchIndex]; - Debug.Assert(typeBatch.ConstraintCount > indexInTypeBatch); - solver.TypeProcessors[constraintTypeId].EnumerateConnectedBodyIndices(ref typeBatch, indexInTypeBatch, ref handleRemover); - Remove(ref typeBatch, typeBatchIndex, indexInTypeBatch, solver.TypeProcessors[constraintTypeId], ref solver.HandleToConstraint, solver.pool); - + solver.TypeProcessors[constraintTypeId].EnumerateConnectedBodyIndices(ref TypeBatches[typeBatchIndex], indexInTypeBatch, ref handleRemover); } public unsafe void Remove(int constraintTypeId, int indexInTypeBatch, Solver solver) { - Debug.Assert(TypeIndexToTypeBatchIndex[constraintTypeId] >= 0, "Type index must actually exist within this batch."); - var typeBatchIndex = TypeIndexToTypeBatchIndex[constraintTypeId]; ref var typeBatch = ref TypeBatches[typeBatchIndex]; - Remove(ref TypeBatches[typeBatchIndex], typeBatchIndex, indexInTypeBatch, solver.TypeProcessors[constraintTypeId], ref solver.HandleToConstraint, solver.pool); - } - - unsafe void Remove(ref TypeBatch typeBatch, int typeBatchIndex, int indexInTypeBatch, TypeProcessor typeProcessor, ref Buffer handleToConstraint, BufferPool pool) - { - typeProcessor.Remove(ref typeBatch, indexInTypeBatch, ref handleToConstraint); - RemoveTypeBatchIfEmpty(ref typeBatch, typeBatchIndex, pool); + Debug.Assert(TypeIndexToTypeBatchIndex[constraintTypeId] >= 0, "Type index must actually exist within this batch."); + Debug.Assert(typeBatch.ConstraintCount > indexInTypeBatch); + var typeProcessor = solver.TypeProcessors[constraintTypeId]; + typeProcessor.Remove(ref typeBatch, indexInTypeBatch, ref solver.HandleToConstraint); + RemoveTypeBatchIfEmpty(ref typeBatch, typeBatchIndex, solver.pool); } public void Clear(BufferPool pool) diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index 188ad543e..345221599 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -351,7 +351,7 @@ public unsafe override void TransferConstraint(ref TypeBatch typeBatch, int sour var bodyHandleCollector = new ActiveConstraintBodyHandleCollector(bodies, bodyHandles); EnumerateConnectedBodyIndices(ref typeBatch, indexInTypeBatch, ref bodyHandleCollector); Debug.Assert(targetBatchIndex <= solver.FallbackBatchThreshold, - "Constraint transfers should never target the fallback batch. It doesn't have any body handles so attempting to allocate in the same way wouldn't turn out well."); + "Constraint transfers should never target the fallback batch. Its registered body handles don't block new constraints so attempting to allocate in the same way wouldn't turn out well."); //Allocate a spot in the new batch. Note that it does not change the Handle->Constraint mapping in the Solver; that's important when we call Solver.Remove below. var constraintHandle = typeBatch.IndexToHandle[indexInTypeBatch]; solver.AllocateInBatch(targetBatchIndex, constraintHandle, new Span(bodyHandles, bodiesPerConstraint), typeId, out var targetReference); @@ -656,11 +656,10 @@ internal unsafe sealed override void AddWakingBodyHandlesToBatchReferences(ref T for (int j = 0; j < bodiesPerConstraint; ++j) { var bodyHandle = Unsafe.Add(ref sourceHandlesStart, offset); - Debug.Assert(!targetBatchReferencedHandles.Contains(bodyHandle), - "It should be impossible for a batch in the active set to already contain a reference to a body that is being woken up."); //Given that we're only adding references to bodies that already exist, and therefore were at some point in the active set, it should never be necessary //to resize the batch referenced handles structure. - targetBatchReferencedHandles.AddUnsafely(bodyHandle); + //Note that this will happily set an existing bit if the target batch is the fallback batch. + targetBatchReferencedHandles.SetUnsafely(bodyHandle); offset += Vector.Count; } } @@ -686,7 +685,6 @@ internal override int GetBodyReferenceCount(ref TypeBatch typeBatch, int bodyToF { if (Unsafe.Add(ref bodyVectorBase, innerIndex) == bodyToFind) ++count; - Debug.Assert(count <= 1); } } } diff --git a/BepuPhysics/FallbackBatch.cs b/BepuPhysics/FallbackBatch.cs index edc918d54..4af3d7a50 100644 --- a/BepuPhysics/FallbackBatch.cs +++ b/BepuPhysics/FallbackBatch.cs @@ -132,7 +132,14 @@ internal void AllocateForInactive(ConstraintHandle handle, Span cons } - internal unsafe void Remove(int bodyReference, ConstraintHandle constraintHandle, ref QuickList allocationIdsToFree) + /// + /// Removes a constraint from a body in the fallback batch. + /// + /// Body associated with a constraint in the fallback batch. + /// Constraint associated with the body being removed. + /// Allocations that should be freed once execution is back in a safe context. + /// True if the body no longer has any constraints associated with it, false otherwise. + internal unsafe bool Remove(int bodyReference, ConstraintHandle constraintHandle, ref QuickList allocationIdsToFree) { var bodyPresent = bodyConstraintReferences.GetTableIndices(ref bodyReference, out var tableIndex, out var bodyReferencesIndex); Debug.Assert(bodyPresent, "If we've been asked to remove a constraint associated with a body, that body must be in this batch."); @@ -155,12 +162,14 @@ internal unsafe void Remove(int bodyReference, ConstraintHandle constraintHandle allocationIdsToFree.AllocateUnsafely() = bodyConstraintReferences.Values.Id; allocationIdsToFree.AllocateUnsafely() = bodyConstraintReferences.Table.Id; bodyConstraintReferences = default; + return true; } } + return false; } - internal unsafe void Remove(Solver solver, BufferPool bufferPool, ref ConstraintBatch batch, ConstraintHandle constraintHandle, int typeId, int indexInTypeBatch) + internal unsafe void Remove(Solver solver, BufferPool bufferPool, ref ConstraintBatch batch, ConstraintHandle constraintHandle, ref IndexSet fallbackBatchHandles, int typeId, int indexInTypeBatch) { var typeProcessor = solver.TypeProcessors[typeId]; var bodyCount = typeProcessor.BodiesPerConstraint; @@ -174,7 +183,10 @@ internal unsafe void Remove(Solver solver, BufferPool bufferPool, ref Constraint typeProcessor.EnumerateConnectedBodyIndices(ref batch.TypeBatches[batch.TypeIndexToTypeBatchIndex[typeId]], indexInTypeBatch, ref enumerator); for (int i = 0; i < bodyCount; ++i) { - Remove(bodyIndices[i], constraintHandle, ref allocationIdsToFree); + if (Remove(bodyIndices[i], constraintHandle, ref allocationIdsToFree)) + { + fallbackBatchHandles.Remove(solver.bodies.ActiveSet.IndexToHandle[bodyIndices[i]].Value); + } } for (int i = 0; i < allocationIdsToFree.Count; ++i) { diff --git a/BepuPhysics/IslandAwakener.cs b/BepuPhysics/IslandAwakener.cs index 51d356339..00c65508a 100644 --- a/BepuPhysics/IslandAwakener.cs +++ b/BepuPhysics/IslandAwakener.cs @@ -223,7 +223,6 @@ internal unsafe void ExecutePhaseOneJob(int index) //But the speculative process is *speculative*; it is fine for it to be wrong, so long as it isn't wrong in a way that makes it choose a higher batch index. //Note that this is parallel over different batches, regardless of which source set they're from. - Debug.Assert(job.BatchIndex < solver.FallbackBatchThreshold, "The fallback batch doesn't have any referenced handles to update!"); ref var targetBatchReferencedHandles = ref solver.batchReferencedHandles[job.BatchIndex]; for (int i = 0; i < uniqueSetIndices.Count; ++i) { @@ -560,26 +559,19 @@ void AccumulatePairCacheTypeCounts(ref Buffer sourceTypeCaches, r solver.ActiveSet.Batches.EnsureCapacity(highestNewBatchCount, pool); if (additionalRequiredFallbackCapacity > 0) solver.ActiveSet.Fallback.EnsureCapacity(solver.ActiveSet.Fallback.BodyCount + additionalRequiredFallbackCapacity, pool); - solver.batchReferencedHandles.EnsureCapacity(Math.Min(solver.FallbackBatchThreshold, highestNewBatchCount), pool); + Debug.Assert(highestNewBatchCount <= solver.FallbackBatchThreshold + 1, "Shouldn't have any batches beyond the fallback batch."); + solver.batchReferencedHandles.EnsureCapacity(highestNewBatchCount, pool); for (int batchIndex = solver.ActiveSet.Batches.Count; batchIndex < highestNewBatchCount; ++batchIndex) { solver.ActiveSet.Batches.AllocateUnsafely() = new ConstraintBatch(pool); - //The fallback batch has no batch referenced handles. - if (batchIndex < solver.FallbackBatchThreshold) - { - solver.batchReferencedHandles.AllocateUnsafely() = new IndexSet(pool, bodies.HandlePool.HighestPossiblyClaimedId + 1); - } + solver.batchReferencedHandles.AllocateUnsafely() = new IndexSet(pool, bodies.HandlePool.HighestPossiblyClaimedId + 1); } for (int batchIndex = 0; batchIndex < highestNewBatchCount; ++batchIndex) { ref var constraintCountPerType = ref constraintCountPerTypePerBatch[batchIndex]; ref var batch = ref solver.ActiveSet.Batches[batchIndex]; batch.EnsureTypeMapSize(pool, constraintCountPerType.HighestOccupiedTypeIndex); - //The fallback batch has no batch referenced handles. - if (batchIndex < solver.FallbackBatchThreshold) - { - solver.batchReferencedHandles[batchIndex].EnsureCapacity(bodies.HandlePool.HighestPossiblyClaimedId + 1, pool); - } + solver.batchReferencedHandles[batchIndex].EnsureCapacity(bodies.HandlePool.HighestPossiblyClaimedId + 1, pool); for (int typeId = 0; typeId <= constraintCountPerType.HighestOccupiedTypeIndex; ++typeId) { var countForType = constraintCountPerType.TypeCounts[typeId].Count; @@ -625,8 +617,7 @@ void EnsurePairCacheTypeCapacities(ref TypeAllocationSizes cache phaseOneJobs.AllocateUnsafely() = new PhaseOneJob { Type = PhaseOneJobType.PairCache }; phaseOneJobs.AllocateUnsafely() = new PhaseOneJob { Type = PhaseOneJobType.MoveFallbackBatchBodies }; //Don't create batch referenced handles update jobs for the fallback batch; it has no referenced handles! - var highestSynchronizedBatchCount = Math.Min(solver.FallbackBatchThreshold, highestNewBatchCount); - for (int batchIndex = 0; batchIndex < highestSynchronizedBatchCount; ++batchIndex) + for (int batchIndex = 0; batchIndex < highestNewBatchCount; ++batchIndex) { phaseOneJobs.AllocateUnsafely() = new PhaseOneJob { Type = PhaseOneJobType.UpdateBatchReferencedHandles, BatchIndex = batchIndex }; } diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index fcef7a219..7eb6e374e 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -195,7 +195,7 @@ public Solver(Bodies bodies, BufferPool pool, int iterationCount, int fallbackBa ActiveSet = new ConstraintSet(pool, fallbackBatchThreshold + 1); batchReferencedHandles = new QuickList(fallbackBatchThreshold + 1, pool); ResizeHandleCapacity(initialCapacity); - solveWorker = SolveWorker;; + solveWorker = SolveWorker; ; incrementalContactUpdateWorker = IncrementalContactUpdateWorker; } @@ -268,7 +268,7 @@ private void ValidateBodyReference(int body, int expectedCount, ref ConstraintBa } [Conditional("DEBUG")] - internal unsafe void ValidateExistingHandles(bool activeOnly = false) + public unsafe void ValidateExistingHandles(bool activeOnly = false) { var maxBodySet = activeOnly ? 1 : bodies.Sets.Length; for (int i = 0; i < maxBodySet; ++i) @@ -283,18 +283,35 @@ internal unsafe void ValidateExistingHandles(bool activeOnly = false) } } //Validate the bodies referenced in the active batchReferencedHandles collections. - //Note that this only applies to the active set synchronized batches; inactive sets and the fallback batch do not explicitly track referenced handles. - for (int batchIndex = 0; batchIndex < Math.Min(ActiveSet.Batches.Count, FallbackBatchThreshold); ++batchIndex) + //Note that this only applies to the active set batches; inactive sets do not explicitly track referenced handles. + for (int batchIndex = 0; batchIndex < ActiveSet.Batches.Count; ++batchIndex) { ref var handles = ref batchReferencedHandles[batchIndex]; ref var batch = ref ActiveSet.Batches[batchIndex]; for (int i = 0; i < bodies.ActiveSet.Count; ++i) { var handle = bodies.ActiveSet.IndexToHandle[i]; + int expectedCount; if (handles.Contains(handle.Value)) - ValidateBodyReference(i, 1, ref batch); + { + if (batchIndex < FallbackBatchThreshold) + { + //A body can only appear in a non-fallback batch at most once. + expectedCount = 1; + } + else + { + //If this is the fallback batch, then the expected count may be more than 1. + var foundBody = ActiveSet.Fallback.bodyConstraintReferences.TryGetValue(i, out var referencesForBody); + Debug.Assert(foundBody, "A body was in the fallback batch's referenced handles, so the fallback batch should have a reference for that body."); + expectedCount = foundBody ? referencesForBody.Count : 0; + } + } else - ValidateBodyReference(i, 0, ref batch); + { + expectedCount = 0; + } + ValidateBodyReference(i, expectedCount, ref batch); } //No inactive bodies should be present in the active set solver batch referenced handles. for (int inactiveBodySetIndex = 1; inactiveBodySetIndex < bodies.Sets.Length; ++inactiveBodySetIndex) @@ -457,17 +474,14 @@ internal unsafe void AllocateInBatch(int targetBatchIndex, ConstraintHandle cons { ref var batch = ref ActiveSet.Batches[targetBatchIndex]; batch.Allocate(constraintHandle, bodyHandles, bodies, typeId, TypeProcessors[typeId], GetMinimumCapacityForType(typeId), pool, out reference); - if (targetBatchIndex < FallbackBatchThreshold) + ref var handlesSet = ref batchReferencedHandles[targetBatchIndex]; + for (int i = 0; i < bodyHandles.Length; ++i) { - ref var handlesSet = ref batchReferencedHandles[targetBatchIndex]; - for (int j = 0; j < bodyHandles.Length; ++j) - { - handlesSet.Add(bodyHandles[j].Value, pool); - } + Debug.Assert(targetBatchIndex == FallbackBatchThreshold || !handlesSet.Contains(bodyHandles[i].Value), "Non-fallback batches should not come to include references to the same body more than once."); + handlesSet.Set(bodyHandles[i].Value, pool); } - else + if (targetBatchIndex == FallbackBatchThreshold) { - Debug.Assert(targetBatchIndex == FallbackBatchThreshold); ActiveSet.Fallback.AllocateForActive(constraintHandle, bodyHandles, bodies, typeId, pool); } } @@ -480,17 +494,14 @@ internal unsafe bool TryAllocateInBatch(int typeId, int targetBatchIndex, Span public void Dispose() { - //Note that the fallback batch does not have a batch referenced handle. - GetSynchronizedBatchCount(out var synchronizedBatchCount, out _); - for (int i = 0; i < synchronizedBatchCount; ++i) + for (int i = 0; i < ActiveSet.Batches.Count; ++i) { batchReferencedHandles[i].Dispose(pool); } diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index 593e6ace6..adb4b9c1b 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -766,8 +766,9 @@ protected void ExecuteMultithreaded2(float dt, IThreadDispatcher threadDispatche } - - unsafe bool ComputeIntegrationResponsibilitiesForConstraintRegion(int batchIndex, int typeBatchIndex, int constraintStart, int exclusiveConstraintEnd) + struct IsFallbackBatch { } + struct IsNotFallbackBatch { } + unsafe bool ComputeIntegrationResponsibilitiesForConstraintRegion(int batchIndex, int typeBatchIndex, int constraintStart, int exclusiveConstraintEnd) where TFallbackness : unmanaged { ref var firstObservedForBatch = ref bodiesFirstObservedInBatches[batchIndex]; ref var integrationFlagsForTypeBatch = ref integrationFlags[batchIndex][typeBatchIndex]; @@ -778,6 +779,7 @@ unsafe bool ComputeIntegrationResponsibilitiesForConstraintRegion(int batchIndex var bundleStartIndex = constraintStart / Vector.Count; var bundleEndIndex = (exclusiveConstraintEnd + Vector.Count - 1) / Vector.Count; Debug.Assert(bundleStartIndex >= 0 && bundleEndIndex <= typeBatch.BundleCount); + ref var activeSet = ref bodies.ActiveSet; for (int bundleIndex = bundleStartIndex; bundleIndex < bundleEndIndex; ++bundleIndex) { @@ -792,10 +794,40 @@ unsafe bool ComputeIntegrationResponsibilitiesForConstraintRegion(int batchIndex for (int bundleInnerIndex = 0; bundleInnerIndex < countInBundle; ++bundleInnerIndex) { //Constraints refer to bodies by index when they're in the active set, so we need to transform to handle to look up our merged batch results. - var bodyHandle = bodies.ActiveSet.IndexToHandle[bundleStart[bundleInnerIndex]].Value; + var bodyIndex = bundleStart[bundleInnerIndex]; + var bodyHandle = activeSet.IndexToHandle[bodyIndex].Value; if (firstObservedForBatch.Contains(bodyHandle)) { - integrationFlagsForBodyInConstraint.AddUnsafely(bundleStartIndexInConstraints + bundleInnerIndex); + if (typeof(TFallbackness) == typeof(IsFallbackBatch)) + { + //This is a fallback. Being contained is not sufficient to require integration; it must also be the *first* constraint that will be executed. + //This is guaranteed by the index of the constraint in the type batch, and the type batch's index. + //Note that, since the fallback batch was the first time this body was seen, we know that *all* constraints associated with this body must be in the fallback batch. + //This could be significantly optimized by not recalculating the earliest candidate every single time a body is encountered in the fallback batch, but + //the fallback batch should effectively *never* contain any integration responsibilities. + ulong earliestIndex = ulong.MaxValue; + ref var constraintsForBody = ref activeSet.Constraints[bodyIndex]; + for (int constraintIndexInBody = 0; constraintIndexInBody < constraintsForBody.Count; ++constraintIndexInBody) + { + ref var fallbackBatch = ref ActiveSet.Batches[FallbackBatchThreshold]; + ref var location = ref HandleToConstraint[constraintsForBody[constraintIndexInBody].ConnectingConstraintHandle.Value]; + var typeBatchIndexForCandidate = fallbackBatch.TypeIndexToTypeBatchIndex[location.TypeId]; + var candidate = ((ulong)typeBatchIndexForCandidate << 32) | (uint)location.IndexInTypeBatch; + if (candidate < earliestIndex) + earliestIndex = candidate; + } + var indexInTypeBatch = bundleStartIndexInConstraints + bundleInnerIndex; + var currentSlot = ((ulong)typeBatchIndex << 32) | (uint)indexInTypeBatch; + if (currentSlot == earliestIndex) + { + integrationFlagsForBodyInConstraint.AddUnsafely(indexInTypeBatch); + } + } + else + { + //Not a fallback; being contained in the observed set is sufficient. + integrationFlagsForBodyInConstraint.AddUnsafely(bundleStartIndexInConstraints + bundleInnerIndex); + } } } } @@ -822,7 +854,10 @@ void ConstraintIntegrationResponsibilitiesWorker(int workerIndex) while ((jobIndex = Interlocked.Increment(ref nextConstraintIntegrationResponsibilityJobIndex) - 1) < integrationResponsibilityPrepassJobs.Count) { ref var job = ref integrationResponsibilityPrepassJobs[jobIndex]; - jobAlignedIntegrationResponsibilities[jobIndex] = ComputeIntegrationResponsibilitiesForConstraintRegion(job.batch, job.typeBatch, job.start, job.end); + if (job.batch == FallbackBatchThreshold) + jobAlignedIntegrationResponsibilities[jobIndex] = ComputeIntegrationResponsibilitiesForConstraintRegion(job.batch, job.typeBatch, job.start, job.end); + else + jobAlignedIntegrationResponsibilities[jobIndex] = ComputeIntegrationResponsibilitiesForConstraintRegion(job.batch, job.typeBatch, job.start, job.end); } } @@ -918,7 +953,8 @@ public override unsafe IndexSet PrepareConstraintIntegrationResponsibilities(int GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); //Note that we are not multithreading the batch merging phase. This typically takes a handful of microseconds. //You'd likely need millions of bodies before you'd see any substantial benefit from multithreading this. - for (int batchIndex = 1; batchIndex < synchronizedBatchCount; ++batchIndex) + var batchCount = ActiveSet.Batches.Count; + for (int batchIndex = 1; batchIndex < batchCount; ++batchIndex) { ref var batchHandles = ref batchReferencedHandles[batchIndex]; ref var firstObservedInBatch = ref bodiesFirstObservedInBatches[batchIndex]; @@ -969,6 +1005,7 @@ public override unsafe IndexSet PrepareConstraintIntegrationResponsibilities(int batchHasAnyIntegrationResponsibilities[batchIndex] = horizontalMerge != 0; } } + //var start = Stopwatch.GetTimestamp(); //We now have index sets representing the first time each body handle is observed in a batch. //This process is significantly more expensive than the batch merging phase and can benefit from multithreading. @@ -1040,7 +1077,16 @@ public override unsafe IndexSet PrepareConstraintIntegrationResponsibilities(int for (int j = 0; j < batch.TypeBatches.Count; ++j) { ref var typeBatch = ref batch.TypeBatches[j]; - coarseBatchIntegrationResponsibilities[i][j] = ComputeIntegrationResponsibilitiesForConstraintRegion(i, j, 0, typeBatch.ConstraintCount); + coarseBatchIntegrationResponsibilities[i][j] = ComputeIntegrationResponsibilitiesForConstraintRegion(i, j, 0, typeBatch.ConstraintCount); + } + } + if (fallbackExists && batchHasAnyIntegrationResponsibilities[FallbackBatchThreshold]) + { + ref var batch = ref ActiveSet.Batches[FallbackBatchThreshold]; + for (int j = 0; j < batch.TypeBatches.Count; ++j) + { + ref var typeBatch = ref batch.TypeBatches[j]; + coarseBatchIntegrationResponsibilities[FallbackBatchThreshold][j] = ComputeIntegrationResponsibilitiesForConstraintRegion(FallbackBatchThreshold, j, 0, typeBatch.ConstraintCount); } } } diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index 19aaf7760..7b6619490 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -45,6 +45,7 @@ struct Option public DemoSet() { + AddOption(); AddOption(); AddOption(); AddOption(); diff --git a/Demos/Demos/Characters/CharacterDemo.cs b/Demos/Demos/Characters/CharacterDemo.cs index 497b2a3b7..30ea3c322 100644 --- a/Demos/Demos/Characters/CharacterDemo.cs +++ b/Demos/Demos/Characters/CharacterDemo.cs @@ -28,7 +28,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //The PositionFirstTimestepper is the simplest timestepping mode, but since it integrates velocity into position at the start of the frame, directly modified velocities outside of the timestep //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. - Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), 1); CreateCharacter(new Vector3(0, 2, -4)); diff --git a/Demos/Demos/CompoundTestDemo.cs b/Demos/Demos/CompoundTestDemo.cs index a619ac8c9..add663ebe 100644 --- a/Demos/Demos/CompoundTestDemo.cs +++ b/Demos/Demos/CompoundTestDemo.cs @@ -19,7 +19,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //The PositionFirstTimestepper is the simplest timestepping mode, but since it integrates velocity into position at the start of the frame, directly modified velocities outside of the timestep //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10f, 0)), new PositionFirstTimestepper()); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10f, 0)), new EmbeddedSubsteppingTimestepper2(3), 1); using (var compoundBuilder = new CompoundBuilder(BufferPool, Simulation.Shapes, 8)) { diff --git a/Demos/Demos/FountainStressTestDemo.cs b/Demos/Demos/FountainStressTestDemo.cs index 394df2e95..fc0ac627c 100644 --- a/Demos/Demos/FountainStressTestDemo.cs +++ b/Demos/Demos/FountainStressTestDemo.cs @@ -23,7 +23,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = MathHelper.Pi * 0.1f; //Using minimum sized allocations forces as many resizes as possible. - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper(), initialAllocationSizes: + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), 1, solverFallbackBatchThreshold: 8, initialAllocationSizes: new SimulationAllocationSizes { Bodies = 1, @@ -233,6 +233,7 @@ public void CreateBodyDescription(Random random, in RigidPose pose, in BodyVeloc public override void Update(Window window, Camera camera, Input input, float dt) { + Simulation.Solver.ValidateExistingHandles(); var timestepDuration = 1f / 60f; time += timestepDuration; diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index 37b200f8c..d1e43dbd3 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -824,7 +824,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper(1)); - Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0), 0, 0), new EmbeddedSubsteppingTimestepper2(9), solverIterationCount: 1); + Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0), 0, 0), new EmbeddedSubsteppingTimestepper2(3), solverIterationCount: 1, 512); //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper2(), solverIterationCount: 60); //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)), new SubsteppingTimestepper2(3), solverIterationCount: 1); @@ -832,7 +832,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) float cellSize = 0.1f; DumbTetrahedralizer.Tetrahedralize(meshContent.Triangles, cellSize, BufferPool, out var vertices, out var vertexSpatialIndices, out var cellVertexIndices, out var tetrahedraVertexIndices); - var weldSpringiness = new SpringSettings(100f, 1f); + var weldSpringiness = new SpringSettings(30f, 1f); var volumeSpringiness = new SpringSettings(30f, 1); //int terboTotal = 0; //var terboShape = new Box(0.5f, 0.5f, 3); @@ -842,7 +842,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) for (int i = 0; i < 40; ++i) { //CreateDeformable(Simulation, new Vector3(i * 3, 5 + i * 1.5f, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), MathF.PI * (i * 0.55f)), 1f, cellSize, weldSpringiness, volumeSpringiness, i, filters, ref vertices, ref vertexSpatialIndices, ref cellVertexIndices, ref tetrahedraVertexIndices); - CreateDeformable(Simulation, new Vector3(i * 3, cellSize * 2f + i * 0f, 0), Quaternion.Identity, 1f, cellSize, weldSpringiness, volumeSpringiness, i, filters, ref vertices, ref vertexSpatialIndices, ref cellVertexIndices, ref tetrahedraVertexIndices); + CreateDeformable(Simulation, new Vector3(i * 3, 5 + cellSize * 2f + i * 0f, 0), Quaternion.Identity, 1f, cellSize, weldSpringiness, volumeSpringiness, i, filters, ref vertices, ref vertexSpatialIndices, ref cellVertexIndices, ref tetrahedraVertexIndices); //for (int terbo = 0; terbo < 17; ++terbo) //{ @@ -859,10 +859,10 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) BufferPool.Return(ref cellVertexIndices); BufferPool.Return(ref tetrahedraVertexIndices); - //Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 100, -.5f), 10, Simulation.Shapes, new Sphere(5))); + Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 100, -.5f), 10, Simulation.Shapes, new Sphere(5))); Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(1500, 1, 1500)), 0.1f))); - //Simulation.Statics.Add(new StaticDescription(new Vector3(0, -1.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Sphere(3)), 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -1.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Sphere(3)), 0.1f))); } diff --git a/Demos/Demos/RagdollDemo.cs b/Demos/Demos/RagdollDemo.cs index 91a71e186..98e34199d 100644 --- a/Demos/Demos/RagdollDemo.cs +++ b/Demos/Demos/RagdollDemo.cs @@ -146,7 +146,7 @@ static BodyHandle AddBody(TShape shape, float mass, in RigidPose pose, S //Also, the cost of registering different shapes isn't that high for tiny implicit shapes. var shapeIndex = simulation.Shapes.Add(shape); shape.ComputeInertia(mass, out var inertia); - var description = BodyDescription.CreateDynamic(pose, inertia, new CollidableDescription(shapeIndex, 0.1f), new BodyActivityDescription(0.01f)); + var description = BodyDescription.CreateDynamic(pose, inertia, new CollidableDescription(shapeIndex, 0.1f), new BodyActivityDescription(-0.01f)); return simulation.Bodies.Add(description); } diff --git a/Demos/Demos/SubsteppingDemo.cs b/Demos/Demos/SubsteppingDemo.cs index 53e7185d8..35a02e730 100644 --- a/Demos/Demos/SubsteppingDemo.cs +++ b/Demos/Demos/SubsteppingDemo.cs @@ -17,13 +17,13 @@ public class SubsteppingDemo : Demo { RolloverInfo rolloverInfo; - SubsteppingTimestepper timestepper; + EmbeddedSubsteppingTimestepper2 timestepper; public unsafe override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(0, 25, 80); camera.Yaw = 0; camera.Pitch = 0; - timestepper = new SubsteppingTimestepper(8); + timestepper = new EmbeddedSubsteppingTimestepper2(8); Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(120, 120), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 2f }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), timestepper, 8); diff --git a/Demos/SpecializedTests/RagdollTubeDemo.cs b/Demos/SpecializedTests/RagdollTubeDemo.cs index 8313eb8a6..a38ea3704 100644 --- a/Demos/SpecializedTests/RagdollTubeDemo.cs +++ b/Demos/SpecializedTests/RagdollTubeDemo.cs @@ -6,6 +6,7 @@ using System; using DemoContentLoader; using Demos.Demos; +using DemoUtilities; namespace Demos.SpecializedTests { @@ -55,7 +56,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) builder.AddForKinematic(Simulation.Shapes.Add(new Box(1, 2, panelShape.Length)), new RigidPose(new Vector3(0, tubeRadius - 1, 0)), 0); builder.BuildKinematicCompound(out var children); var compound = new BigCompound(children, Simulation.Shapes, BufferPool); - var tubeHandle = Simulation.Bodies.Add(BodyDescription.CreateKinematic(tubeCenter, new BodyVelocity(default, new Vector3(0, 0, .25f)), new CollidableDescription(Simulation.Shapes.Add(compound), 0.1f), new BodyActivityDescription())); + tubeHandle = Simulation.Bodies.Add(BodyDescription.CreateKinematic(tubeCenter, new BodyVelocity(default, new Vector3(0, 0, .25f)), new CollidableDescription(Simulation.Shapes.Add(compound), 0.1f), new BodyActivityDescription())); filters[tubeHandle] = new SubgroupCollisionFilter(int.MaxValue); builder.Dispose(); @@ -78,6 +79,17 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Simulation.Statics.Add(staticDescription); } + BodyHandle tubeHandle; + + //public override void Update(Window window, Camera camera, Input input, float dt) + //{ + // base.Update(window, camera, input, dt); + + // Console.WriteLine($"Constraint count: {Simulation.Solver.CountConstraints()}"); + + // Console.WriteLine($"Constraints affecting tube: {Simulation.Bodies.GetBodyReference(tubeHandle).Constraints.Count}"); + //} + } } From 696320a24d1c6f8086b30eefcf5b4c1826a36c99 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 1 Oct 2021 20:32:59 -0500 Subject: [PATCH 198/947] Fixed a failure to remove fallback handle references due to a bad condition. --- BepuPhysics/FallbackBatch.cs | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/BepuPhysics/FallbackBatch.cs b/BepuPhysics/FallbackBatch.cs index 4af3d7a50..749a469b0 100644 --- a/BepuPhysics/FallbackBatch.cs +++ b/BepuPhysics/FallbackBatch.cs @@ -138,15 +138,13 @@ internal void AllocateForInactive(ConstraintHandle handle, Span cons /// Body associated with a constraint in the fallback batch. /// Constraint associated with the body being removed. /// Allocations that should be freed once execution is back in a safe context. - /// True if the body no longer has any constraints associated with it, false otherwise. + /// True if the body no longer has any constraints associated with it in the fallback batch, false otherwise. internal unsafe bool Remove(int bodyReference, ConstraintHandle constraintHandle, ref QuickList allocationIdsToFree) { var bodyPresent = bodyConstraintReferences.GetTableIndices(ref bodyReference, out var tableIndex, out var bodyReferencesIndex); Debug.Assert(bodyPresent, "If we've been asked to remove a constraint associated with a body, that body must be in this batch."); ref var constraintReferences = ref bodyConstraintReferences.Values[bodyReferencesIndex]; - //TODO: Should really just be using a dictionary here. - var dummy = new FallbackReference { ConstraintHandle = constraintHandle }; - var removed = constraintReferences.FastRemove(ref dummy); + var removed = constraintReferences.FastRemove(new FallbackReference { ConstraintHandle = constraintHandle }); Debug.Assert(removed, "If a constraint removal was requested, it must exist within the referenced body's constraint set."); if (constraintReferences.Count == 0) { @@ -162,8 +160,8 @@ internal unsafe bool Remove(int bodyReference, ConstraintHandle constraintHandle allocationIdsToFree.AllocateUnsafely() = bodyConstraintReferences.Values.Id; allocationIdsToFree.AllocateUnsafely() = bodyConstraintReferences.Table.Id; bodyConstraintReferences = default; - return true; } + return true; } return false; } From a94d3ff19d11b3c90cf18f599231872a8465ce28 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 1 Oct 2021 21:28:14 -0500 Subject: [PATCH 199/947] Fixed missing fallback handle management in sleeper. --- .../CollisionDetection/ConstraintRemover.cs | 7 ++- BepuPhysics/FallbackBatch.cs | 47 +++++++++++-------- BepuPhysics/IslandSleeper.cs | 9 ++-- 3 files changed, 38 insertions(+), 25 deletions(-) diff --git a/BepuPhysics/CollisionDetection/ConstraintRemover.cs b/BepuPhysics/CollisionDetection/ConstraintRemover.cs index e85b49d88..d1bd35f98 100644 --- a/BepuPhysics/CollisionDetection/ConstraintRemover.cs +++ b/BepuPhysics/CollisionDetection/ConstraintRemover.cs @@ -385,9 +385,12 @@ public void RemoveConstraintsFromFallbackBatch() } } } - public void TryRemoveAllConstraintsForBodyFromFallbackBatch(int bodyIndex) + public void TryRemoveAllConstraintsForBodyFromFallbackBatch(BodyHandle bodyHandle, int bodyIndex) { - solver.ActiveSet.Fallback.TryRemove(bodyIndex, ref allocationIdsToFree); + if (solver.ActiveSet.Fallback.TryRemove(bodyIndex, ref allocationIdsToFree)) + { + solver.batchReferencedHandles[solver.FallbackBatchThreshold].Remove(bodyHandle.Value); + } } QuickList removedTypeBatches; diff --git a/BepuPhysics/FallbackBatch.cs b/BepuPhysics/FallbackBatch.cs index 749a469b0..423cadd22 100644 --- a/BepuPhysics/FallbackBatch.cs +++ b/BepuPhysics/FallbackBatch.cs @@ -166,6 +166,34 @@ internal unsafe bool Remove(int bodyReference, ConstraintHandle constraintHandle return false; } + /// + /// Removes a body from the fallback batch if it is present. + /// + /// Reference to the body to remove from the fallback batch. + /// Allocations that should be freed once execution is back in a safe context. + /// True if the body was present in the fallback batch and was removed, false otherwise. + internal unsafe bool TryRemove(int bodyReference, ref QuickList allocationIdsToFree) + { + if (bodyConstraintReferences.Keys.Allocated && bodyConstraintReferences.GetTableIndices(ref bodyReference, out var tableIndex, out var bodyReferencesIndex)) + { + ref var constraintReferences = ref bodyConstraintReferences.Values[bodyReferencesIndex]; + //If there are no more constraints associated with this body, get rid of the body list. + allocationIdsToFree.AllocateUnsafely() = constraintReferences.Span.Id; + allocationIdsToFree.AllocateUnsafely() = constraintReferences.Table.Id; + bodyConstraintReferences.FastRemove(tableIndex, bodyReferencesIndex); + if (bodyConstraintReferences.Count == 0) + { + //No constraints remain in the fallback batch. Drop the dictionary. + allocationIdsToFree.AllocateUnsafely() = bodyConstraintReferences.Keys.Id; + allocationIdsToFree.AllocateUnsafely() = bodyConstraintReferences.Values.Id; + allocationIdsToFree.AllocateUnsafely() = bodyConstraintReferences.Table.Id; + bodyConstraintReferences = default; + } + return true; + } + return false; + } + internal unsafe void Remove(Solver solver, BufferPool bufferPool, ref ConstraintBatch batch, ConstraintHandle constraintHandle, ref IndexSet fallbackBatchHandles, int typeId, int indexInTypeBatch) { @@ -192,25 +220,6 @@ internal unsafe void Remove(Solver solver, BufferPool bufferPool, ref Constraint } } - internal unsafe void TryRemove(int bodyReference, ref QuickList allocationIdsToFree) - { - if (bodyConstraintReferences.Keys.Allocated && bodyConstraintReferences.GetTableIndices(ref bodyReference, out var tableIndex, out var bodyReferencesIndex)) - { - ref var constraintReferences = ref bodyConstraintReferences.Values[bodyReferencesIndex]; - //If there are no more constraints associated with this body, get rid of the body list. - allocationIdsToFree.AllocateUnsafely() = constraintReferences.Span.Id; - allocationIdsToFree.AllocateUnsafely() = constraintReferences.Table.Id; - bodyConstraintReferences.FastRemove(tableIndex, bodyReferencesIndex); - if (bodyConstraintReferences.Count == 0) - { - //No constraints remain in the fallback batch. Drop the dictionary. - allocationIdsToFree.AllocateUnsafely() = bodyConstraintReferences.Keys.Id; - allocationIdsToFree.AllocateUnsafely() = bodyConstraintReferences.Values.Id; - allocationIdsToFree.AllocateUnsafely() = bodyConstraintReferences.Table.Id; - bodyConstraintReferences = default; - } - } - } public static void AllocateResults(Solver solver, BufferPool pool, ref ConstraintBatch batch, out Buffer results) { diff --git a/BepuPhysics/IslandSleeper.cs b/BepuPhysics/IslandSleeper.cs index 9b24ae7ee..8f5bab043 100644 --- a/BepuPhysics/IslandSleeper.cs +++ b/BepuPhysics/IslandSleeper.cs @@ -446,15 +446,16 @@ void ExecuteRemoval(ref RemovalJob job) var setIndex = newInactiveSets[setReferenceIndex].Index; ref var inactiveBodySet = ref bodies.Sets[setIndex]; ref var inactiveConstraintSet = ref solver.Sets[setIndex]; - for (int bodyIndex = 0; bodyIndex < inactiveBodySet.Count; ++bodyIndex) + for (int bodyIndexInInactiveSet = 0; bodyIndexInInactiveSet < inactiveBodySet.Count; ++bodyIndexInInactiveSet) { - ref var location = ref bodies.HandleToLocation[inactiveBodySet.IndexToHandle[bodyIndex].Value]; + var bodyHandle = inactiveBodySet.IndexToHandle[bodyIndexInInactiveSet]; + ref var location = ref bodies.HandleToLocation[bodyHandle.Value]; Debug.Assert(location.SetIndex == 0, "At this point, the sleep hasn't gone through so the set should still be 0."); - constraintRemover.TryRemoveAllConstraintsForBodyFromFallbackBatch(location.Index); + constraintRemover.TryRemoveAllConstraintsForBodyFromFallbackBatch(bodyHandle, location.Index); bodies.RemoveFromActiveSet(location.Index); //And now we can actually update the handle->body mapping. location.SetIndex = setIndex; - location.Index = bodyIndex; + location.Index = bodyIndexInInactiveSet; } } } From 2eabd1f41e9bf96232d94cef65e029eb065e22ae Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 3 Oct 2021 19:42:24 -0500 Subject: [PATCH 200/947] Heavy validation enabled; tracking down integration responsibility bug. --- BepuPhysics/BatchCompressor.cs | 3 +- BepuPhysics/Simulation.cs | 2 +- BepuPhysics/Solver_SubsteppingSolve2.cs | 80 +++++++++++++++---------- Demos/Demo.cs | 2 +- Demos/Demos/FountainStressTestDemo.cs | 2 +- 5 files changed, 53 insertions(+), 36 deletions(-) diff --git a/BepuPhysics/BatchCompressor.cs b/BepuPhysics/BatchCompressor.cs index 523430da7..679026911 100644 --- a/BepuPhysics/BatchCompressor.cs +++ b/BepuPhysics/BatchCompressor.cs @@ -174,8 +174,7 @@ public int Compare(ref CompressionTarget a, ref CompressionTarget b) [MethodImpl(MethodImplOptions.AggressiveInlining)] private unsafe void ApplyCompression(int sourceBatchIndex, ref ConstraintBatch sourceBatch, ref Compression compression) { - //Careful here: this is a reference for the sake of not doing pointless copies, but you cannot rely on it having the same values after the completion of the transfer. - ref var constraintLocation = ref Solver.HandleToConstraint[compression.ConstraintHandle.Value]; + var constraintLocation = Solver.HandleToConstraint[compression.ConstraintHandle.Value]; var typeProcessor = Solver.TypeProcessors[constraintLocation.TypeId]; if (sourceBatchIndex == Solver.FallbackBatchThreshold) { diff --git a/BepuPhysics/Simulation.cs b/BepuPhysics/Simulation.cs index 22d2bc18b..9890508b8 100644 --- a/BepuPhysics/Simulation.cs +++ b/BepuPhysics/Simulation.cs @@ -330,7 +330,7 @@ public void IncrementallyOptimizeDataStructures(IThreadDispatcher threadDispatch profiler.End(ConstraintLayoutOptimizer); profiler.Start(SolverBatchCompressor); - SolverBatchCompressor.Compress(BufferPool, threadDispatcher, threadDispatcher != null && Deterministic); + //SolverBatchCompressor.Compress(BufferPool, threadDispatcher, threadDispatcher != null && Deterministic); profiler.End(SolverBatchCompressor); } diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index adb4b9c1b..230ae028d 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -1092,38 +1092,56 @@ public override unsafe IndexSet PrepareConstraintIntegrationResponsibilities(int } //var end = Stopwatch.GetTimestamp(); //Console.WriteLine($"time (ms): {(end - start) * 1e3 / Stopwatch.Frequency}"); - pool.Return(ref batchHasAnyIntegrationResponsibilities); - ////Validation: - //for (int i = 0; i < bodies.ActiveSet.Count; ++i) - //{ - // ref var constraints = ref bodies.ActiveSet.Constraints[i]; - // ConstraintHandle minimumConstraint; - // minimumConstraint.Value = -1; - // int minimumBatchIndex = int.MaxValue; - // int minimumIndexInConstraint = -1; - // for (int j = 0; j < constraints.Count; ++j) - // { - // ref var constraint = ref constraints[j]; - // var batchIndex = HandleToConstraint[constraint.ConnectingConstraintHandle.Value].BatchIndex; - // if (batchIndex < minimumBatchIndex) - // { - // minimumBatchIndex = batchIndex; - // minimumIndexInConstraint = constraint.BodyIndexInConstraint; - // minimumConstraint = constraint.ConnectingConstraintHandle; - // } - // } - // if (minimumConstraint.Value >= 0) - // { - // ref var location = ref HandleToConstraint[minimumConstraint.Value]; - // var typeBatchIndex = ActiveSet.Batches[location.BatchIndex].TypeIndexToTypeBatchIndex[location.TypeId]; - // if (location.BatchIndex > 0) - // { - // ref var indexSet = ref integrationFlags[location.BatchIndex][typeBatchIndex][minimumIndexInConstraint]; - // Debug.Assert(indexSet.Contains(location.IndexInTypeBatch)); - // } - // } - //} + //Validation: + for (int i = 0; i < bodies.ActiveSet.Count; ++i) + { + ref var constraints = ref bodies.ActiveSet.Constraints[i]; + ConstraintHandle minimumConstraint; + minimumConstraint.Value = -1; + int minimumBatchIndex = int.MaxValue; + int minimumBodyIndexInConstraint = -1; + ulong earliestSlotInFallback = ulong.MaxValue; + for (int j = 0; j < constraints.Count; ++j) + { + ref var constraint = ref constraints[j]; + ref var location = ref HandleToConstraint[constraint.ConnectingConstraintHandle.Value]; + + if (location.BatchIndex <= minimumBatchIndex) //Note that the only time it can be equal is if this is in the fallback batch. + { + if (location.BatchIndex == FallbackBatchThreshold) + { + var typeBatchIndex = ActiveSet.Batches[location.BatchIndex].TypeIndexToTypeBatchIndex[location.TypeId]; + var encodedSlotCandidate = ((ulong)typeBatchIndex << 32) | (uint)location.IndexInTypeBatch; + if (encodedSlotCandidate < earliestSlotInFallback) + { + earliestSlotInFallback = encodedSlotCandidate; + //We should only accept another fallback constraint as minimal if it has an earlier typebatch index/index in type batch. + minimumBatchIndex = location.BatchIndex; + minimumBodyIndexInConstraint = constraint.BodyIndexInConstraint; + minimumConstraint = constraint.ConnectingConstraintHandle; + } + } + else + { + minimumBatchIndex = location.BatchIndex; + minimumBodyIndexInConstraint = constraint.BodyIndexInConstraint; + minimumConstraint = constraint.ConnectingConstraintHandle; + } + } + } + if (minimumConstraint.Value >= 0) + { + ref var location = ref HandleToConstraint[minimumConstraint.Value]; + var typeBatchIndex = ActiveSet.Batches[location.BatchIndex].TypeIndexToTypeBatchIndex[location.TypeId]; + if (location.BatchIndex > 0) + { + ref var indexSet = ref integrationFlags[location.BatchIndex][typeBatchIndex][minimumBodyIndexInConstraint]; + Debug.Assert(indexSet.Contains(location.IndexInTypeBatch)); + } + } + } + pool.Return(ref batchHasAnyIntegrationResponsibilities); Debug.Assert(!bodiesFirstObservedInBatches[0].Flags.Allocated, "Remember, we're assuming we're just leaving the first batch's slot empty to avoid indexing complexity."); for (int batchIndex = 1; batchIndex < bodiesFirstObservedInBatches.Length; ++batchIndex) diff --git a/Demos/Demo.cs b/Demos/Demo.cs index 37ebe5190..5f3790da3 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -62,7 +62,7 @@ public virtual void Update(Window window, Camera camera, Input input, float dt) //fully decouple simulation and rendering rates across different threads. //(In either case, you'd also want to interpolate or extrapolate simulation results during rendering for smoothness.) //Note that taking steps of variable length can reduce stability. Gradual or one-off changes can work reasonably well. - Simulation.Timestep(1 / 60f, ThreadDispatcher); + Simulation.Timestep(1 / 60f); ////Here's an example of how it would look to use more frequent updates, but still with a fixed amount of time simulated per update call: //const float timeToSimulate = 1 / 60f; diff --git a/Demos/Demos/FountainStressTestDemo.cs b/Demos/Demos/FountainStressTestDemo.cs index fc0ac627c..43135574a 100644 --- a/Demos/Demos/FountainStressTestDemo.cs +++ b/Demos/Demos/FountainStressTestDemo.cs @@ -23,7 +23,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = MathHelper.Pi * 0.1f; //Using minimum sized allocations forces as many resizes as possible. - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), 1, solverFallbackBatchThreshold: 8, initialAllocationSizes: + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), 1, solverFallbackBatchThreshold: 1024, initialAllocationSizes: new SimulationAllocationSizes { Bodies = 1, From 39b2f7edc1db7c1709bc6ff9e9e298ee6e3558b0 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 11 Oct 2021 13:16:51 -0500 Subject: [PATCH 201/947] Added more validation and fixed an error in pose integration computation. --- BepuPhysics/Simulation.cs | 2 +- BepuPhysics/Solver_SubsteppingSolve2.cs | 65 ++++++++++++++++++++++--- Demos/Demo.cs | 2 +- Demos/Demos/FountainStressTestDemo.cs | 3 +- 4 files changed, 61 insertions(+), 11 deletions(-) diff --git a/BepuPhysics/Simulation.cs b/BepuPhysics/Simulation.cs index 9890508b8..22d2bc18b 100644 --- a/BepuPhysics/Simulation.cs +++ b/BepuPhysics/Simulation.cs @@ -330,7 +330,7 @@ public void IncrementallyOptimizeDataStructures(IThreadDispatcher threadDispatch profiler.End(ConstraintLayoutOptimizer); profiler.Start(SolverBatchCompressor); - //SolverBatchCompressor.Compress(BufferPool, threadDispatcher, threadDispatcher != null && Deterministic); + SolverBatchCompressor.Compress(BufferPool, threadDispatcher, threadDispatcher != null && Deterministic); profiler.End(SolverBatchCompressor); } diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index 230ae028d..de5c8f741 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -932,6 +932,7 @@ public override unsafe IndexSet PrepareConstraintIntegrationResponsibilities(int //} pool.Take(batchReferencedHandles.Count, out bodiesFirstObservedInBatches); + pool.Take(batchReferencedHandles.Count, out Buffer debugBodiesFirstObservedInBatches); //We don't have to consider the first batch, since we know ahead of time that the first batch will be the first time we see any bodies in it. //Just copy directly from the first batch into the merged to initialize it. //Note "+ 64" instead of "+ 63": the highest possibly claimed id is inclusive! @@ -949,6 +950,7 @@ public override unsafe IndexSet PrepareConstraintIntegrationResponsibilities(int var bundleCount = Math.Min(mergedConstrainedBodyHandles.Flags.Length, batchHandles.Flags.Length); //Note that we bypass the constructor to avoid zeroing unnecessarily. Every bundle will be fully assigned. pool.Take(bundleCount, out bodiesFirstObservedInBatches[batchIndex].Flags); + pool.Take(bundleCount, out debugBodiesFirstObservedInBatches[batchIndex].Flags); } GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); //Note that we are not multithreading the batch merging phase. This typically takes a handful of microseconds. @@ -959,22 +961,49 @@ public override unsafe IndexSet PrepareConstraintIntegrationResponsibilities(int ref var batchHandles = ref batchReferencedHandles[batchIndex]; ref var firstObservedInBatch = ref bodiesFirstObservedInBatches[batchIndex]; var flagBundleCount = Math.Min(mergedConstrainedBodyHandles.Flags.Length, batchHandles.Flags.Length); + ref var debugFirstObservedInBatch = ref debugBodiesFirstObservedInBatches[batchIndex]; + + ulong horizontalMergeDebug = 0; + for (int flagBundleIndex = 0; flagBundleIndex < flagBundleCount; ++flagBundleIndex) + { + var mergeBundle = mergedConstrainedBodyHandles.Flags[flagBundleIndex]; + var batchBundle = batchHandles.Flags[flagBundleIndex]; + //If this batch contains a body, and the merged set does not, then it's the first batch that sees a body and it will have integration responsibility. + var firstObservedBundle = ~mergeBundle & batchBundle; + horizontalMergeDebug |= firstObservedBundle; + debugFirstObservedInBatch.Flags[flagBundleIndex] = firstObservedBundle; + } + if (Avx2.IsSupported) { var avxBundleCount = flagBundleCount / 4; var horizontalAvxMerge = Vector256.Zero; for (int avxBundleIndex = 0; avxBundleIndex < avxBundleCount; ++avxBundleIndex) { - var mergeBundle = ((Vector256*)mergedConstrainedBodyHandles.Flags.Memory)[avxBundleIndex]; - var batchBundle = ((Vector256*)batchHandles.Flags.Memory)[avxBundleIndex]; - ((Vector256*)mergedConstrainedBodyHandles.Flags.Memory)[avxBundleIndex] = Avx2.Or(mergeBundle, batchBundle); + //These will *almost* always be aligned, but guaranteeing it is not worth the complexity. + var mergeBundle = Avx2.LoadVector256((ulong*)((Vector256*)mergedConstrainedBodyHandles.Flags.Memory + avxBundleIndex)); + var batchBundle = Avx2.LoadVector256((ulong*)((Vector256*)batchHandles.Flags.Memory + avxBundleIndex)); + Avx.Store((ulong*)((Vector256*)mergedConstrainedBodyHandles.Flags.Memory + avxBundleIndex), Avx2.Or(mergeBundle, batchBundle)); //If this batch contains a body, and the merged set does not, then it's the first batch that sees a body and it will have integration responsibility. var firstObservedBundle = Avx2.AndNot(mergeBundle, batchBundle); + var expected0 = ~mergeBundle.GetElement(0) & batchBundle.GetElement(0); + var expected1 = ~mergeBundle.GetElement(1) & batchBundle.GetElement(1); + var expected2 = ~mergeBundle.GetElement(2) & batchBundle.GetElement(2); + var expected3 = ~mergeBundle.GetElement(3) & batchBundle.GetElement(3); + Debug.Assert(expected0 == firstObservedBundle.GetElement(0)); + Debug.Assert(expected1 == firstObservedBundle.GetElement(1)); + Debug.Assert(expected2 == firstObservedBundle.GetElement(2)); + Debug.Assert(expected3 == firstObservedBundle.GetElement(3)); horizontalAvxMerge = Avx2.Or(firstObservedBundle, horizontalAvxMerge); - ((Vector256*)firstObservedInBatch.Flags.Memory)[avxBundleIndex] = firstObservedBundle; + Avx.Store((ulong*)((Vector256*)firstObservedInBatch.Flags.Memory + avxBundleIndex), firstObservedBundle); + } + var notEqual = Avx2.Xor(Avx2.CompareEqual(horizontalAvxMerge, Vector256.Zero), Vector256.AllBitsSet); + ulong horizontalMerge = (ulong)Avx2.MoveMask(notEqual.AsDouble()); + + for (int flagBundleIndex = 0; flagBundleIndex < avxBundleCount * 4; ++flagBundleIndex) + { + Debug.Assert(debugFirstObservedInBatch.Flags[flagBundleIndex] == firstObservedInBatch.Flags[flagBundleIndex]); } - var notEqual = Avx2.CompareNotEqual(horizontalAvxMerge.AsDouble(), Vector256.Zero); - ulong horizontalMerge = (ulong)Avx.MoveMask(notEqual); //Cleanup loop. for (int flagBundleIndex = avxBundleCount * 4; flagBundleIndex < flagBundleCount; ++flagBundleIndex) @@ -988,6 +1017,12 @@ public override unsafe IndexSet PrepareConstraintIntegrationResponsibilities(int firstObservedInBatch.Flags[flagBundleIndex] = firstObservedBundle; } batchHasAnyIntegrationResponsibilities[batchIndex] = horizontalMerge != 0; + + for (int flagBundleIndex = avxBundleCount * 4; flagBundleIndex < flagBundleCount; ++flagBundleIndex) + { + Debug.Assert(debugFirstObservedInBatch.Flags[flagBundleIndex] == firstObservedInBatch.Flags[flagBundleIndex]); + } + Debug.Assert((horizontalMerge != 0) == (horizontalMergeDebug != 0)); } else { @@ -1102,16 +1137,18 @@ public override unsafe IndexSet PrepareConstraintIntegrationResponsibilities(int int minimumBatchIndex = int.MaxValue; int minimumBodyIndexInConstraint = -1; ulong earliestSlotInFallback = ulong.MaxValue; + ConstraintHandle detectedConstraint; + detectedConstraint.Value = -1; for (int j = 0; j < constraints.Count; ++j) { ref var constraint = ref constraints[j]; ref var location = ref HandleToConstraint[constraint.ConnectingConstraintHandle.Value]; + var typeBatchIndex = ActiveSet.Batches[location.BatchIndex].TypeIndexToTypeBatchIndex[location.TypeId]; if (location.BatchIndex <= minimumBatchIndex) //Note that the only time it can be equal is if this is in the fallback batch. { if (location.BatchIndex == FallbackBatchThreshold) { - var typeBatchIndex = ActiveSet.Batches[location.BatchIndex].TypeIndexToTypeBatchIndex[location.TypeId]; var encodedSlotCandidate = ((ulong)typeBatchIndex << 32) | (uint)location.IndexInTypeBatch; if (encodedSlotCandidate < earliestSlotInFallback) { @@ -1129,9 +1166,23 @@ public override unsafe IndexSet PrepareConstraintIntegrationResponsibilities(int minimumConstraint = constraint.ConnectingConstraintHandle; } } + + if (location.BatchIndex > 0) + { + ref var indexSet = ref integrationFlags[location.BatchIndex][typeBatchIndex][constraint.BodyIndexInConstraint]; + if (indexSet.Contains(location.IndexInTypeBatch)) + { + //This constraint has integration responsibility for this body. + Debug.Assert(detectedConstraint.Value == -1, "Only one constraint should have integration responsibility for a given body."); + detectedConstraint = constraint.ConnectingConstraintHandle; + } + } } + if (constraints.Count > 0 && minimumBatchIndex > 0) + Debug.Assert(detectedConstraint.Value >= 0, "At least one constraint must have integration responsibility for a body if it has a constraint."); if (minimumConstraint.Value >= 0) { + Debug.Assert(minimumBatchIndex == 0 || detectedConstraint.Value == minimumConstraint.Value); ref var location = ref HandleToConstraint[minimumConstraint.Value]; var typeBatchIndex = ActiveSet.Batches[location.BatchIndex].TypeIndexToTypeBatchIndex[location.TypeId]; if (location.BatchIndex > 0) diff --git a/Demos/Demo.cs b/Demos/Demo.cs index 5f3790da3..37ebe5190 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -62,7 +62,7 @@ public virtual void Update(Window window, Camera camera, Input input, float dt) //fully decouple simulation and rendering rates across different threads. //(In either case, you'd also want to interpolate or extrapolate simulation results during rendering for smoothness.) //Note that taking steps of variable length can reduce stability. Gradual or one-off changes can work reasonably well. - Simulation.Timestep(1 / 60f); + Simulation.Timestep(1 / 60f, ThreadDispatcher); ////Here's an example of how it would look to use more frequent updates, but still with a fixed amount of time simulated per update call: //const float timeToSimulate = 1 / 60f; diff --git a/Demos/Demos/FountainStressTestDemo.cs b/Demos/Demos/FountainStressTestDemo.cs index 43135574a..5eb142898 100644 --- a/Demos/Demos/FountainStressTestDemo.cs +++ b/Demos/Demos/FountainStressTestDemo.cs @@ -23,7 +23,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = MathHelper.Pi * 0.1f; //Using minimum sized allocations forces as many resizes as possible. - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), 1, solverFallbackBatchThreshold: 1024, initialAllocationSizes: + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), 1, initialAllocationSizes: new SimulationAllocationSizes { Bodies = 1, @@ -233,7 +233,6 @@ public void CreateBodyDescription(Random random, in RigidPose pose, in BodyVeloc public override void Update(Window window, Camera camera, Input input, float dt) { - Simulation.Solver.ValidateExistingHandles(); var timestepDuration = 1f / 60f; time += timestepDuration; From a7efd4e0a650e75ebb00b7ef371c8e615cf70fe2 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 11 Oct 2021 13:25:24 -0500 Subject: [PATCH 202/947] Disabled some of the bonus validation. --- BepuPhysics/Solver_SubsteppingSolve2.cs | 158 ++++++++++-------------- 1 file changed, 63 insertions(+), 95 deletions(-) diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index de5c8f741..f0cb58a8f 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -932,7 +932,6 @@ public override unsafe IndexSet PrepareConstraintIntegrationResponsibilities(int //} pool.Take(batchReferencedHandles.Count, out bodiesFirstObservedInBatches); - pool.Take(batchReferencedHandles.Count, out Buffer debugBodiesFirstObservedInBatches); //We don't have to consider the first batch, since we know ahead of time that the first batch will be the first time we see any bodies in it. //Just copy directly from the first batch into the merged to initialize it. //Note "+ 64" instead of "+ 63": the highest possibly claimed id is inclusive! @@ -950,7 +949,6 @@ public override unsafe IndexSet PrepareConstraintIntegrationResponsibilities(int var bundleCount = Math.Min(mergedConstrainedBodyHandles.Flags.Length, batchHandles.Flags.Length); //Note that we bypass the constructor to avoid zeroing unnecessarily. Every bundle will be fully assigned. pool.Take(bundleCount, out bodiesFirstObservedInBatches[batchIndex].Flags); - pool.Take(bundleCount, out debugBodiesFirstObservedInBatches[batchIndex].Flags); } GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); //Note that we are not multithreading the batch merging phase. This typically takes a handful of microseconds. @@ -961,18 +959,6 @@ public override unsafe IndexSet PrepareConstraintIntegrationResponsibilities(int ref var batchHandles = ref batchReferencedHandles[batchIndex]; ref var firstObservedInBatch = ref bodiesFirstObservedInBatches[batchIndex]; var flagBundleCount = Math.Min(mergedConstrainedBodyHandles.Flags.Length, batchHandles.Flags.Length); - ref var debugFirstObservedInBatch = ref debugBodiesFirstObservedInBatches[batchIndex]; - - ulong horizontalMergeDebug = 0; - for (int flagBundleIndex = 0; flagBundleIndex < flagBundleCount; ++flagBundleIndex) - { - var mergeBundle = mergedConstrainedBodyHandles.Flags[flagBundleIndex]; - var batchBundle = batchHandles.Flags[flagBundleIndex]; - //If this batch contains a body, and the merged set does not, then it's the first batch that sees a body and it will have integration responsibility. - var firstObservedBundle = ~mergeBundle & batchBundle; - horizontalMergeDebug |= firstObservedBundle; - debugFirstObservedInBatch.Flags[flagBundleIndex] = firstObservedBundle; - } if (Avx2.IsSupported) { @@ -986,25 +972,12 @@ public override unsafe IndexSet PrepareConstraintIntegrationResponsibilities(int Avx.Store((ulong*)((Vector256*)mergedConstrainedBodyHandles.Flags.Memory + avxBundleIndex), Avx2.Or(mergeBundle, batchBundle)); //If this batch contains a body, and the merged set does not, then it's the first batch that sees a body and it will have integration responsibility. var firstObservedBundle = Avx2.AndNot(mergeBundle, batchBundle); - var expected0 = ~mergeBundle.GetElement(0) & batchBundle.GetElement(0); - var expected1 = ~mergeBundle.GetElement(1) & batchBundle.GetElement(1); - var expected2 = ~mergeBundle.GetElement(2) & batchBundle.GetElement(2); - var expected3 = ~mergeBundle.GetElement(3) & batchBundle.GetElement(3); - Debug.Assert(expected0 == firstObservedBundle.GetElement(0)); - Debug.Assert(expected1 == firstObservedBundle.GetElement(1)); - Debug.Assert(expected2 == firstObservedBundle.GetElement(2)); - Debug.Assert(expected3 == firstObservedBundle.GetElement(3)); horizontalAvxMerge = Avx2.Or(firstObservedBundle, horizontalAvxMerge); Avx.Store((ulong*)((Vector256*)firstObservedInBatch.Flags.Memory + avxBundleIndex), firstObservedBundle); } var notEqual = Avx2.Xor(Avx2.CompareEqual(horizontalAvxMerge, Vector256.Zero), Vector256.AllBitsSet); ulong horizontalMerge = (ulong)Avx2.MoveMask(notEqual.AsDouble()); - for (int flagBundleIndex = 0; flagBundleIndex < avxBundleCount * 4; ++flagBundleIndex) - { - Debug.Assert(debugFirstObservedInBatch.Flags[flagBundleIndex] == firstObservedInBatch.Flags[flagBundleIndex]); - } - //Cleanup loop. for (int flagBundleIndex = avxBundleCount * 4; flagBundleIndex < flagBundleCount; ++flagBundleIndex) { @@ -1017,12 +990,6 @@ public override unsafe IndexSet PrepareConstraintIntegrationResponsibilities(int firstObservedInBatch.Flags[flagBundleIndex] = firstObservedBundle; } batchHasAnyIntegrationResponsibilities[batchIndex] = horizontalMerge != 0; - - for (int flagBundleIndex = avxBundleCount * 4; flagBundleIndex < flagBundleCount; ++flagBundleIndex) - { - Debug.Assert(debugFirstObservedInBatch.Flags[flagBundleIndex] == firstObservedInBatch.Flags[flagBundleIndex]); - } - Debug.Assert((horizontalMerge != 0) == (horizontalMergeDebug != 0)); } else { @@ -1128,70 +1095,71 @@ public override unsafe IndexSet PrepareConstraintIntegrationResponsibilities(int //var end = Stopwatch.GetTimestamp(); //Console.WriteLine($"time (ms): {(end - start) * 1e3 / Stopwatch.Frequency}"); - //Validation: - for (int i = 0; i < bodies.ActiveSet.Count; ++i) - { - ref var constraints = ref bodies.ActiveSet.Constraints[i]; - ConstraintHandle minimumConstraint; - minimumConstraint.Value = -1; - int minimumBatchIndex = int.MaxValue; - int minimumBodyIndexInConstraint = -1; - ulong earliestSlotInFallback = ulong.MaxValue; - ConstraintHandle detectedConstraint; - detectedConstraint.Value = -1; - for (int j = 0; j < constraints.Count; ++j) - { - ref var constraint = ref constraints[j]; - ref var location = ref HandleToConstraint[constraint.ConnectingConstraintHandle.Value]; - var typeBatchIndex = ActiveSet.Batches[location.BatchIndex].TypeIndexToTypeBatchIndex[location.TypeId]; + ////Validation: + //for (int i = 0; i < bodies.ActiveSet.Count; ++i) + //{ + // ref var constraints = ref bodies.ActiveSet.Constraints[i]; + // ConstraintHandle minimumConstraint; + // minimumConstraint.Value = -1; + // int minimumBatchIndex = int.MaxValue; + // int minimumBodyIndexInConstraint = -1; + // ulong earliestSlotInFallback = ulong.MaxValue; + // ConstraintHandle detectedConstraint; + // detectedConstraint.Value = -1; + // for (int j = 0; j < constraints.Count; ++j) + // { + // ref var constraint = ref constraints[j]; + // ref var location = ref HandleToConstraint[constraint.ConnectingConstraintHandle.Value]; + // var typeBatchIndex = ActiveSet.Batches[location.BatchIndex].TypeIndexToTypeBatchIndex[location.TypeId]; - if (location.BatchIndex <= minimumBatchIndex) //Note that the only time it can be equal is if this is in the fallback batch. - { - if (location.BatchIndex == FallbackBatchThreshold) - { - var encodedSlotCandidate = ((ulong)typeBatchIndex << 32) | (uint)location.IndexInTypeBatch; - if (encodedSlotCandidate < earliestSlotInFallback) - { - earliestSlotInFallback = encodedSlotCandidate; - //We should only accept another fallback constraint as minimal if it has an earlier typebatch index/index in type batch. - minimumBatchIndex = location.BatchIndex; - minimumBodyIndexInConstraint = constraint.BodyIndexInConstraint; - minimumConstraint = constraint.ConnectingConstraintHandle; - } - } - else - { - minimumBatchIndex = location.BatchIndex; - minimumBodyIndexInConstraint = constraint.BodyIndexInConstraint; - minimumConstraint = constraint.ConnectingConstraintHandle; - } - } + // if (location.BatchIndex <= minimumBatchIndex) //Note that the only time it can be equal is if this is in the fallback batch. + // { + // if (location.BatchIndex == FallbackBatchThreshold) + // { + // var encodedSlotCandidate = ((ulong)typeBatchIndex << 32) | (uint)location.IndexInTypeBatch; + // if (encodedSlotCandidate < earliestSlotInFallback) + // { + // earliestSlotInFallback = encodedSlotCandidate; + // //We should only accept another fallback constraint as minimal if it has an earlier typebatch index/index in type batch. + // minimumBatchIndex = location.BatchIndex; + // minimumBodyIndexInConstraint = constraint.BodyIndexInConstraint; + // minimumConstraint = constraint.ConnectingConstraintHandle; + // } + // } + // else + // { + // minimumBatchIndex = location.BatchIndex; + // minimumBodyIndexInConstraint = constraint.BodyIndexInConstraint; + // minimumConstraint = constraint.ConnectingConstraintHandle; + // } + // } + + // if (location.BatchIndex > 0) + // { + // ref var indexSet = ref integrationFlags[location.BatchIndex][typeBatchIndex][constraint.BodyIndexInConstraint]; + // if (indexSet.Contains(location.IndexInTypeBatch)) + // { + // //This constraint has integration responsibility for this body. + // Debug.Assert(detectedConstraint.Value == -1, "Only one constraint should have integration responsibility for a given body."); + // detectedConstraint = constraint.ConnectingConstraintHandle; + // } + // } + // } + // if (constraints.Count > 0 && minimumBatchIndex > 0) + // Debug.Assert(detectedConstraint.Value >= 0, "At least one constraint must have integration responsibility for a body if it has a constraint."); + // if (minimumConstraint.Value >= 0) + // { + // Debug.Assert(minimumBatchIndex == 0 || detectedConstraint.Value == minimumConstraint.Value); + // ref var location = ref HandleToConstraint[minimumConstraint.Value]; + // var typeBatchIndex = ActiveSet.Batches[location.BatchIndex].TypeIndexToTypeBatchIndex[location.TypeId]; + // if (location.BatchIndex > 0) + // { + // ref var indexSet = ref integrationFlags[location.BatchIndex][typeBatchIndex][minimumBodyIndexInConstraint]; + // Debug.Assert(indexSet.Contains(location.IndexInTypeBatch)); + // } + // } + //} - if (location.BatchIndex > 0) - { - ref var indexSet = ref integrationFlags[location.BatchIndex][typeBatchIndex][constraint.BodyIndexInConstraint]; - if (indexSet.Contains(location.IndexInTypeBatch)) - { - //This constraint has integration responsibility for this body. - Debug.Assert(detectedConstraint.Value == -1, "Only one constraint should have integration responsibility for a given body."); - detectedConstraint = constraint.ConnectingConstraintHandle; - } - } - } - if (constraints.Count > 0 && minimumBatchIndex > 0) - Debug.Assert(detectedConstraint.Value >= 0, "At least one constraint must have integration responsibility for a body if it has a constraint."); - if (minimumConstraint.Value >= 0) - { - Debug.Assert(minimumBatchIndex == 0 || detectedConstraint.Value == minimumConstraint.Value); - ref var location = ref HandleToConstraint[minimumConstraint.Value]; - var typeBatchIndex = ActiveSet.Batches[location.BatchIndex].TypeIndexToTypeBatchIndex[location.TypeId]; - if (location.BatchIndex > 0) - { - ref var indexSet = ref integrationFlags[location.BatchIndex][typeBatchIndex][minimumBodyIndexInConstraint]; - Debug.Assert(indexSet.Contains(location.IndexInTypeBatch)); - } - } - } pool.Return(ref batchHasAnyIntegrationResponsibilities); Debug.Assert(!bodiesFirstObservedInBatches[0].Flags.Allocated, "Remember, we're assuming we're just leaving the first batch's slot empty to avoid indexing complexity."); From 2021158136cf3511e1a3a8a7eb8e811429bda951 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 11 Oct 2021 15:08:08 -0500 Subject: [PATCH 203/947] Preparing for sequential fallback option. --- .../Constraints/FourBodyTypeProcessor.cs | 12 ++--- .../Constraints/OneBodyTypeProcessor.cs | 12 ++--- .../Constraints/ThreeBodyTypeProcessor.cs | 12 ++--- .../Constraints/TwoBodyTypeProcessor.cs | 15 ++++--- BepuPhysics/Constraints/TypeProcessor.cs | 45 ++++++++++++++----- BepuPhysics/Solver_SubsteppingSolve2.cs | 16 +++---- 6 files changed, 70 insertions(+), 42 deletions(-) diff --git a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs index da0b957ef..249123c63 100644 --- a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs @@ -282,9 +282,9 @@ public unsafe override void JacobiSolveIteration(ref TypeBatch typeBatch, Bodies function.Solve(ref wsvA, ref wsvB, ref wsvC, ref wsvD, ref projection, ref accumulatedImpulses); } } - - public unsafe override void WarmStart2( - ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) + public unsafe override void WarmStart2( + ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, in TBundleCountCalculator bundleCountCalculator, + float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { var prestepBundles = typeBatch.PrestepData.As(); var bodyReferencesBundles = typeBatch.BodyReferences.As(); @@ -296,7 +296,7 @@ public unsafe override void WarmStart2(bodies, ref integratorCallbacks, ref integrationFlags, 0, dt, workerIndex, i, ref references.IndexA, count, out var positionA, out var orientationA, out var wsvA, out var inertiaA); GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 1, dt, workerIndex, i, ref references.IndexB, count, @@ -331,7 +331,7 @@ public unsafe override void WarmStart2(ref TypeBatch typeBatch, Bodies bodies, in TBundleCountCalculator bundleCountCalculator, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { var prestepBundles = typeBatch.PrestepData.As(); var bodyReferencesBundles = typeBatch.BodyReferences.As(); @@ -343,7 +343,7 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f ref var prestep = ref prestepBundles[i]; ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; ref var references = ref bodyReferencesBundles[i]; - var count = GetCountInBundle(ref typeBatch, i); + var count = bundleCountCalculator.GetCountInBundle(ref typeBatch, i); bodies.GatherState(ref references.IndexA, count, true, out var positionA, out var orientationA, out var wsvA, out var inertiaA); bodies.GatherState(ref references.IndexB, count, true, out var positionB, out var orientationB, out var wsvB, out var inertiaB); bodies.GatherState(ref references.IndexC, count, true, out var positionC, out var orientationC, out var wsvC, out var inertiaC); diff --git a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs index 8c6515c42..34a045d25 100644 --- a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs @@ -210,9 +210,9 @@ public unsafe override void JacobiSolveIteration(ref TypeBatch typeBatch, Bodies function.Solve(ref wsvA, ref projection, ref accumulatedImpulses); } } - - public unsafe override void WarmStart2( - ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) + public unsafe override void WarmStart2( + ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, in TBundleCountCalculator bundleCountCalculator, + float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { var prestepBundles = typeBatch.PrestepData.As(); var bodyReferencesBundles = typeBatch.BodyReferences.As>(); @@ -224,7 +224,7 @@ public unsafe override void WarmStart2(bodies, ref integratorCallbacks, ref integrationFlags, 0, dt, workerIndex, i, ref references, count, out var positionA, out var orientationA, out var wsvA, out var inertiaA); @@ -246,7 +246,7 @@ public unsafe override void WarmStart2(ref TypeBatch typeBatch, Bodies bodies, in TBundleCountCalculator bundleCountCalculator, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { var prestepBundles = typeBatch.PrestepData.As(); var bodyReferencesBundles = typeBatch.BodyReferences.As>(); @@ -258,7 +258,7 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f ref var prestep = ref prestepBundles[i]; ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; ref var references = ref bodyReferencesBundles[i]; - var count = GetCountInBundle(ref typeBatch, i); + var count = bundleCountCalculator.GetCountInBundle(ref typeBatch, i); bodies.GatherState(ref references, count, true, out var positionA, out var orientationA, out var wsvA, out var inertiaA); function.Solve2(positionA, orientationA, inertiaA, dt, inverseDt, ref prestep, ref accumulatedImpulses, ref wsvA); diff --git a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs index 100b922bf..61c598c4b 100644 --- a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs @@ -258,9 +258,9 @@ public unsafe override void JacobiSolveIteration(ref TypeBatch typeBatch, Bodies function.Solve(ref wsvA, ref wsvB, ref wsvC, ref projection, ref accumulatedImpulses); } } - - public unsafe override void WarmStart2( - ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) + public unsafe override void WarmStart2( + ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, in TBundleCountCalculator bundleCountCalculator, + float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { var prestepBundles = typeBatch.PrestepData.As(); var bodyReferencesBundles = typeBatch.BodyReferences.As(); @@ -272,7 +272,7 @@ public unsafe override void WarmStart2(bodies, ref integratorCallbacks, ref integrationFlags, 0, dt, workerIndex, i, ref references.IndexA, count, out var positionA, out var orientationA, out var wsvA, out var inertiaA); GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 1, dt, workerIndex, i, ref references.IndexB, count, @@ -303,7 +303,7 @@ public unsafe override void WarmStart2(ref TypeBatch typeBatch, Bodies bodies, in TBundleCountCalculator bundleCountCalculator, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { var prestepBundles = typeBatch.PrestepData.As(); var bodyReferencesBundles = typeBatch.BodyReferences.As(); @@ -315,7 +315,7 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f ref var prestep = ref prestepBundles[i]; ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; ref var references = ref bodyReferencesBundles[i]; - var count = GetCountInBundle(ref typeBatch, i); + var count = bundleCountCalculator.GetCountInBundle(ref typeBatch, i); bodies.GatherState(ref references.IndexA, count, true, out var positionA, out var orientationA, out var wsvA, out var inertiaA); bodies.GatherState(ref references.IndexB, count, true, out var positionB, out var orientationB, out var wsvB, out var inertiaB); bodies.GatherState(ref references.IndexC, count, true, out var positionC, out var orientationC, out var wsvC, out var inertiaC); diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 019195093..37543ce01 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -315,8 +315,11 @@ public unsafe override void JacobiSolveIteration(ref TypeBatch typeBatch, Bodies // } //} - public unsafe override void WarmStart2( - ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) + + + public unsafe override void WarmStart2( + ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, in TBundleCountCalculator bundleCountCalculator, + float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { var prestepBundles = typeBatch.PrestepData.As(); var bodyReferencesBundles = typeBatch.BodyReferences.As(); @@ -329,7 +332,7 @@ public unsafe override void WarmStart2(bodies, ref integratorCallbacks, ref integrationFlags, 0, dt, workerIndex, i, ref references.IndexA, count, out var positionA, out var orientationA, out var wsvA, out var inertiaA); @@ -357,7 +360,7 @@ public unsafe override void WarmStart2(ref TypeBatch typeBatch, Bodies bodies, in TBundleCountCalculator bundleCountCalculator, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { var prestepBundles = typeBatch.PrestepData.As(); var bodyReferencesBundles = typeBatch.BodyReferences.As(); @@ -370,7 +373,7 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f ref var prestep = ref prestepBundles[i]; ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; ref var references = ref bodyReferencesBundles[i]; - var count = GetCountInBundle(ref typeBatch, i); + var count = bundleCountCalculator.GetCountInBundle(ref typeBatch, i); //Prefetch(SolvePrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, i, exclusiveEndBundle); bodies.GatherState(ref references.IndexA, count, true, out var positionA, out var orientationA, out var wsvA, out var inertiaA); bodies.GatherState(ref references.IndexB, count, true, out var positionB, out var orientationB, out var wsvB, out var inertiaB); @@ -410,7 +413,7 @@ public override void JacobiWarmStart2(ref TypeBatch typeBatch, Bodies bodies, re inertiaA.InverseMass *= jacobiScaleA; Symmetric3x3Wide.Scale(inertiaB.InverseInertiaTensor, jacobiScaleB, out inertiaB.InverseInertiaTensor); inertiaB.InverseMass *= jacobiScaleB; - + function.WarmStart2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB); //Jacobi batches do not scatter velocities directly; they are handled in a postpass. } diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index 345221599..a2da34692 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -10,7 +10,28 @@ namespace BepuPhysics.Constraints { + public interface IConstraintBundleCountCalculator + { + int GetCountInBundle(ref TypeBatch typeBatch, int bundleIndex); + } + public struct DenseBundleCountCalculator : IConstraintBundleCountCalculator + { + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public readonly int GetCountInBundle(ref TypeBatch typeBatch, int bundleIndex) + { + return TypeProcessor.GetCountInBundle(ref typeBatch, bundleIndex); + } + } + + public struct SequentialFallbackBundleCountCalculator : IConstraintBundleCountCalculator + { + Buffer countsInBundles; + public readonly int GetCountInBundle(ref TypeBatch typeBatch, int bundleIndex) + { + return countsInBundles[bundleIndex]; + } + } /// /// Superclass of constraint type batch processors. Responsible for interpreting raw type batches for the purposes of bookkeeping and solving. @@ -136,12 +157,15 @@ internal unsafe abstract void CopySleepingToActive( public abstract void JacobiWarmStart(ref TypeBatch typeBatch, Bodies bodies, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle); public abstract void JacobiSolveIteration(ref TypeBatch typeBatch, Bodies bodies, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle); - public abstract void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks poseIntegratorCallbacks, + public abstract void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, + ref TIntegratorCallbacks poseIntegratorCallbacks, in TCountInBundleCalculator countInBundleCalculator, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) where TIntegratorCallbacks : struct, IPoseIntegratorCallbacks where TBatchIntegrationMode : unmanaged, IBatchIntegrationMode - where TAllowPoseIntegration : unmanaged, IBatchPoseIntegrationAllowed; - public abstract void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); + where TAllowPoseIntegration : unmanaged, IBatchPoseIntegrationAllowed + where TCountInBundleCalculator : unmanaged, IConstraintBundleCountCalculator; + public abstract void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, in TCountInBundleCalculator countInBundleCalculator, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + where TCountInBundleCalculator : unmanaged, IConstraintBundleCountCalculator; public abstract void JacobiWarmStart2( ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, @@ -153,6 +177,14 @@ public virtual void IncrementallyUpdateContactData(ref TypeBatch typeBatch, Bodi { Debug.Fail("A contact data update was scheduled for a type batch that does not have a contact data update implementation."); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + internal static int GetCountInBundle(ref TypeBatch typeBatch, int bundleStartIndex) + { + //TODO: May want to check codegen on this. Min vs explicit branch. Theoretically, it could do this branchlessly... + return Math.Min(Vector.Count, typeBatch.ConstraintCount - (bundleStartIndex << BundleIndexing.VectorShift)); + } + } /// @@ -212,13 +244,6 @@ public unsafe static void AddBodyReferencesLane(ref TBodyReferences bundle, int } } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - protected static int GetCountInBundle(ref TypeBatch typeBatch, int bundleStartIndex) - { - //TODO: May want to check codegen on this. Min vs explicit branch. Theoretically, it could do this branchlessly... - return Math.Min(Vector.Count, typeBatch.ConstraintCount - (bundleStartIndex << BundleIndexing.VectorShift)); - } - public unsafe sealed override int Allocate(ref TypeBatch typeBatch, ConstraintHandle handle, int* bodyIndices, BufferPool pool) { diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index f0cb58a8f..d7a0b03f9 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -120,22 +120,22 @@ private void WarmStartBlock(int workerIndex, int bat if (batchIndex == 0) { Buffer noFlagsRequired = default; - typeProcessor.WarmStart2( - ref typeBatch, ref noFlagsRequired, bodies, ref PoseIntegrator.Callbacks, + typeProcessor.WarmStart2( + ref typeBatch, ref noFlagsRequired, bodies, ref PoseIntegrator.Callbacks, new DenseBundleCountCalculator(), dt, inverseDt, startBundle, endBundle, workerIndex); } else { if (coarseBatchIntegrationResponsibilities[batchIndex][typeBatchIndex]) { - typeProcessor.WarmStart2( - ref typeBatch, ref integrationFlags[batchIndex][typeBatchIndex], bodies, ref PoseIntegrator.Callbacks, + typeProcessor.WarmStart2( + ref typeBatch, ref integrationFlags[batchIndex][typeBatchIndex], bodies, ref PoseIntegrator.Callbacks, new DenseBundleCountCalculator(), dt, inverseDt, startBundle, endBundle, workerIndex); } else { - typeProcessor.WarmStart2( - ref typeBatch, ref integrationFlags[batchIndex][typeBatchIndex], bodies, ref PoseIntegrator.Callbacks, + typeProcessor.WarmStart2( + ref typeBatch, ref integrationFlags[batchIndex][typeBatchIndex], bodies, ref PoseIntegrator.Callbacks, new DenseBundleCountCalculator(), dt, inverseDt, startBundle, endBundle, workerIndex); } } @@ -196,7 +196,7 @@ public void Execute(Solver solver, int blockIndex, int workerIndex) ref var block = ref this.solver.substepContext.ConstraintBlocks[blockIndex]; ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; - typeProcessor.SolveStep2(ref typeBatch, solver.bodies, Dt, InverseDt, block.StartBundle, block.End); + typeProcessor.SolveStep2(ref typeBatch, solver.bodies, new DenseBundleCountCalculator(), Dt, InverseDt, block.StartBundle, block.End); } } @@ -1265,7 +1265,7 @@ public override void SolveStep2(float totalDt, IThreadDispatcher threadDispatche for (int j = 0; j < batch.TypeBatches.Count; ++j) { ref var typeBatch = ref batch.TypeBatches[j]; - TypeProcessors[typeBatch.TypeId].SolveStep2(ref typeBatch, bodies, substepDt, inverseDt, 0, typeBatch.BundleCount); + TypeProcessors[typeBatch.TypeId].SolveStep2(ref typeBatch, bodies, new DenseBundleCountCalculator(), substepDt, inverseDt, 0, typeBatch.BundleCount); } } if (fallbackExists) From 04b8047781cc07558c1c993071a77d0455cb2cff Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 11 Oct 2021 16:54:56 -0500 Subject: [PATCH 204/947] Fiddling around with the idea of sequential fallback batch. --- .../CollisionDetection/ConstraintRemover.cs | 6 +- BepuPhysics/ConstraintSet.cs | 12 +- .../Constraints/FourBodyTypeProcessor.cs | 10 +- .../Constraints/OneBodyTypeProcessor.cs | 10 +- .../Constraints/ThreeBodyTypeProcessor.cs | 10 +- .../Constraints/TwoBodyTypeProcessor.cs | 10 +- BepuPhysics/Constraints/TypeProcessor.cs | 10 +- BepuPhysics/IslandAwakener.cs | 8 +- BepuPhysics/IslandScaffold.cs | 4 +- BepuPhysics/IslandSleeper.cs | 2 +- ...allbackBatch.cs => JacobiFallbackBatch.cs} | 40 +-- BepuPhysics/SequentialFallbackBatch.cs | 320 ++++++++++++++++++ BepuPhysics/Solver.cs | 14 +- BepuPhysics/Solver_Solve.cs | 28 +- BepuPhysics/Solver_SubsteppingSolve2.cs | 32 +- 15 files changed, 408 insertions(+), 108 deletions(-) rename BepuPhysics/{FallbackBatch.cs => JacobiFallbackBatch.cs} (95%) create mode 100644 BepuPhysics/SequentialFallbackBatch.cs diff --git a/BepuPhysics/CollisionDetection/ConstraintRemover.cs b/BepuPhysics/CollisionDetection/ConstraintRemover.cs index d1bd35f98..fe950faf1 100644 --- a/BepuPhysics/CollisionDetection/ConstraintRemover.cs +++ b/BepuPhysics/CollisionDetection/ConstraintRemover.cs @@ -212,7 +212,7 @@ public void Prepare(IThreadDispatcher dispatcher) { //Ensure that the fallback deallocation list is also large enough. The fallback batch may result in 3 returned buffers for the primary dictionary, plus another two for each potentially //removed body constraint references subset. - allocationIdsToFree = new QuickList(3 + solver.ActiveSet.Fallback.BodyCount * 2, pool); + allocationIdsToFree = new QuickList(3 + solver.ActiveSet.JacobiFallback.BodyCount * 2, pool); } } @@ -376,7 +376,7 @@ public void RemoveConstraintsFromFallbackBatch() for (int j = 0; j < removals.Count; ++j) { ref var target = ref removals[j]; - if (solver.ActiveSet.Fallback.Remove(target.BodyIndex, target.ConstraintHandle, ref allocationIdsToFree)) + if (solver.ActiveSet.JacobiFallback.Remove(target.BodyIndex, target.ConstraintHandle, ref allocationIdsToFree)) { //No more constraints for this body in the fallback set; it should not exist in the fallback batch's referenced handles anymore. solver.batchReferencedHandles[target.BatchIndex].Remove(target.BodyHandle.Value); @@ -387,7 +387,7 @@ public void RemoveConstraintsFromFallbackBatch() } public void TryRemoveAllConstraintsForBodyFromFallbackBatch(BodyHandle bodyHandle, int bodyIndex) { - if (solver.ActiveSet.Fallback.TryRemove(bodyIndex, ref allocationIdsToFree)) + if (solver.ActiveSet.JacobiFallback.TryRemove(bodyIndex, ref allocationIdsToFree)) { solver.batchReferencedHandles[solver.FallbackBatchThreshold].Remove(bodyHandle.Value); } diff --git a/BepuPhysics/ConstraintSet.cs b/BepuPhysics/ConstraintSet.cs index f7cc631f6..6616a6bae 100644 --- a/BepuPhysics/ConstraintSet.cs +++ b/BepuPhysics/ConstraintSet.cs @@ -7,12 +7,14 @@ namespace BepuPhysics public struct ConstraintSet { public QuickList Batches; - public FallbackBatch Fallback; + public JacobiFallbackBatch JacobiFallback; + public SequentialFallbackBatch SequentialFallback; public ConstraintSet(BufferPool pool, int initialBatchCapacity) { Batches = new QuickList(initialBatchCapacity, pool); - Fallback = default; + JacobiFallback = default; + SequentialFallback = default; } /// @@ -70,7 +72,8 @@ public void Clear(BufferPool pool) { Batches[i].Dispose(pool); } - Fallback.Dispose(pool); + JacobiFallback.Dispose(pool); + SequentialFallback.Dispose(pool); Batches.Count = 0; } @@ -80,7 +83,8 @@ public void Dispose(BufferPool pool) { Batches[i].Dispose(pool); } - Fallback.Dispose(pool); + JacobiFallback.Dispose(pool); + SequentialFallback.Dispose(pool); Batches.Dispose(pool); this = new ConstraintSet(); } diff --git a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs index 249123c63..ba7ea1c49 100644 --- a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs @@ -196,7 +196,7 @@ public unsafe override void SolveIteration(ref TypeBatch typeBatch, Bodies bodie } } - public unsafe override void JacobiPrestep(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + public unsafe override void JacobiPrestep(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackBatch jacobiBatch, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); @@ -226,7 +226,7 @@ public unsafe override void JacobiPrestep(ref TypeBatch typeBatch, Bodies bodies function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, ac, orientationC, inertiaC, ad, orientationD, inertiaD, dt, inverseDt, ref prestep, out projection); } } - public unsafe override void JacobiWarmStart(ref TypeBatch typeBatch, Bodies bodies, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle) + public unsafe override void JacobiWarmStart(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle) { ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); @@ -254,7 +254,7 @@ public unsafe override void JacobiWarmStart(ref TypeBatch typeBatch, Bodies bodi function.WarmStart(ref wsvA, ref wsvB, ref wsvC, ref wsvD, ref projection, ref accumulatedImpulses); } } - public unsafe override void JacobiSolveIteration(ref TypeBatch typeBatch, Bodies bodies, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle) + public unsafe override void JacobiSolveIteration(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle) { ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); @@ -359,7 +359,7 @@ public unsafe override void SolveStep2(ref TypeBatch typ } - public override void JacobiWarmStart2(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) + public override void JacobiWarmStart2(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackBatch jacobiBatch, ref JacobiFallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { var prestepBundles = typeBatch.PrestepData.As(); var bodyReferencesBundles = typeBatch.BodyReferences.As(); @@ -404,7 +404,7 @@ public override void JacobiWarmStart2(ref TypeBatch typeBatch, Bodies bodies, re } } - public override void JacobiSolveStep2(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + public override void JacobiSolveStep2(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackBatch jacobiBatch, ref JacobiFallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { var prestepBundles = typeBatch.PrestepData.As(); var bodyReferencesBundles = typeBatch.BodyReferences.As(); diff --git a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs index 34a045d25..c3f209294 100644 --- a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs @@ -154,7 +154,7 @@ public unsafe override void SolveIteration(ref TypeBatch typeBatch, Bodies bodie } } - public unsafe override void JacobiPrestep(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + public unsafe override void JacobiPrestep(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackBatch jacobiBatch, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); ref var bodyReferencesBase = ref Unsafe.AsRef>(typeBatch.BodyReferences.Memory); @@ -174,7 +174,7 @@ public unsafe override void JacobiPrestep(ref TypeBatch typeBatch, Bodies bodies function.Prestep(position, orientation, inertia, dt, inverseDt, ref prestep, out projection); } } - public unsafe override void JacobiWarmStart(ref TypeBatch typeBatch, Bodies bodies, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle) + public unsafe override void JacobiWarmStart(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle) { ref var bodyReferencesBase = ref Unsafe.AsRef>(typeBatch.BodyReferences.Memory); ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); @@ -192,7 +192,7 @@ public unsafe override void JacobiWarmStart(ref TypeBatch typeBatch, Bodies bodi function.WarmStart(ref wsvA, ref projection, ref accumulatedImpulses); } } - public unsafe override void JacobiSolveIteration(ref TypeBatch typeBatch, Bodies bodies, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle) + public unsafe override void JacobiSolveIteration(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle) { ref var bodyReferencesBase = ref Unsafe.AsRef>(typeBatch.BodyReferences.Memory); ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); @@ -267,7 +267,7 @@ public unsafe override void SolveStep2(ref TypeBatch typ } } - public override void JacobiWarmStart2(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) + public override void JacobiWarmStart2(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackBatch jacobiBatch, ref JacobiFallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { var prestepBundles = typeBatch.PrestepData.As(); var bodyReferencesBundles = typeBatch.BodyReferences.As>(); @@ -294,7 +294,7 @@ public override void JacobiWarmStart2(ref TypeBatch typeBatch, Bodies bodies, re } } - public override void JacobiSolveStep2(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + public override void JacobiSolveStep2(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackBatch jacobiBatch, ref JacobiFallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { var prestepBundles = typeBatch.PrestepData.As(); var bodyReferencesBundles = typeBatch.BodyReferences.As>(); diff --git a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs index 61c598c4b..3e8b36311 100644 --- a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs @@ -181,7 +181,7 @@ public unsafe override void SolveIteration(ref TypeBatch typeBatch, Bodies bodie } } - public unsafe override void JacobiPrestep(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + public unsafe override void JacobiPrestep(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackBatch jacobiBatch, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); @@ -208,7 +208,7 @@ public unsafe override void JacobiPrestep(ref TypeBatch typeBatch, Bodies bodies function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, ac, orientationC, inertiaC, dt, inverseDt, ref prestep, out projection); } } - public unsafe override void JacobiWarmStart(ref TypeBatch typeBatch, Bodies bodies, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle) + public unsafe override void JacobiWarmStart(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle) { ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); @@ -233,7 +233,7 @@ public unsafe override void JacobiWarmStart(ref TypeBatch typeBatch, Bodies bodi function.WarmStart(ref wsvA, ref wsvB, ref wsvC, ref projection, ref accumulatedImpulses); } } - public unsafe override void JacobiSolveIteration(ref TypeBatch typeBatch, Bodies bodies, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle) + public unsafe override void JacobiSolveIteration(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle) { ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); @@ -329,7 +329,7 @@ public unsafe override void SolveStep2(ref TypeBatch typ } - public override void JacobiWarmStart2(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) + public override void JacobiWarmStart2(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackBatch jacobiBatch, ref JacobiFallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { var prestepBundles = typeBatch.PrestepData.As(); var bodyReferencesBundles = typeBatch.BodyReferences.As(); @@ -368,7 +368,7 @@ public override void JacobiWarmStart2(ref TypeBatch typeBatch, Bodies bodies, re } } - public override void JacobiSolveStep2(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + public override void JacobiSolveStep2(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackBatch jacobiBatch, ref JacobiFallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { var prestepBundles = typeBatch.PrestepData.As(); var bodyReferencesBundles = typeBatch.BodyReferences.As(); diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 37543ce01..d2260d5de 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -200,7 +200,7 @@ public unsafe override void SolveIteration(ref TypeBatch typeBatch, Bodies bodie } } - public unsafe override void JacobiPrestep(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + public unsafe override void JacobiPrestep(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackBatch jacobiBatch, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); @@ -222,7 +222,7 @@ public unsafe override void JacobiPrestep(ref TypeBatch typeBatch, Bodies bodies function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, dt, inverseDt, ref prestep, out projection); } } - public unsafe override void JacobiWarmStart(ref TypeBatch typeBatch, Bodies bodies, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle) + public unsafe override void JacobiWarmStart(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle) { ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); @@ -242,7 +242,7 @@ public unsafe override void JacobiWarmStart(ref TypeBatch typeBatch, Bodies bodi function.WarmStart(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); } } - public unsafe override void JacobiSolveIteration(ref TypeBatch typeBatch, Bodies bodies, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle) + public unsafe override void JacobiSolveIteration(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle) { ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); @@ -385,7 +385,7 @@ public unsafe override void SolveStep2(ref TypeBatch typ } } - public override void JacobiWarmStart2(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) + public override void JacobiWarmStart2(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackBatch jacobiBatch, ref JacobiFallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { var prestepBundles = typeBatch.PrestepData.As(); var bodyReferencesBundles = typeBatch.BodyReferences.As(); @@ -419,7 +419,7 @@ public override void JacobiWarmStart2(ref TypeBatch typeBatch, Bodies bodies, re } } - public override void JacobiSolveStep2(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + public override void JacobiSolveStep2(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackBatch jacobiBatch, ref JacobiFallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { var prestepBundles = typeBatch.PrestepData.As(); var bodyReferencesBundles = typeBatch.BodyReferences.As(); diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index a2da34692..8325ced1b 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -153,9 +153,9 @@ internal unsafe abstract void CopySleepingToActive( public abstract void WarmStart(ref TypeBatch typeBatch, Bodies bodies, int startBundle, int exclusiveEndBundle); public abstract void SolveIteration(ref TypeBatch typeBatch, Bodies bodies, int startBundle, int exclusiveEndBundle); - public abstract void JacobiPrestep(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); - public abstract void JacobiWarmStart(ref TypeBatch typeBatch, Bodies bodies, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle); - public abstract void JacobiSolveIteration(ref TypeBatch typeBatch, Bodies bodies, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle); + public abstract void JacobiPrestep(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackBatch jacobiBatch, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); + public abstract void JacobiWarmStart(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle); + public abstract void JacobiSolveIteration(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle); public abstract void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks poseIntegratorCallbacks, in TCountInBundleCalculator countInBundleCalculator, @@ -168,10 +168,10 @@ public abstract void SolveStep2(ref TypeBatch typeBatc where TCountInBundleCalculator : unmanaged, IConstraintBundleCountCalculator; public abstract void JacobiWarmStart2( - ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, + ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackBatch jacobiBatch, ref JacobiFallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex); - public abstract void JacobiSolveStep2(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, ref FallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); + public abstract void JacobiSolveStep2(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackBatch jacobiBatch, ref JacobiFallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); public virtual void IncrementallyUpdateContactData(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int end) { diff --git a/BepuPhysics/IslandAwakener.cs b/BepuPhysics/IslandAwakener.cs index 00c65508a..851dbb7e0 100644 --- a/BepuPhysics/IslandAwakener.cs +++ b/BepuPhysics/IslandAwakener.cs @@ -245,8 +245,8 @@ internal unsafe void ExecutePhaseOneJob(int index) for (int i = 0; i < uniqueSetIndices.Count; ++i) { Debug.Assert(uniqueSetIndices[i] > 0); - ref var source = ref solver.Sets[uniqueSetIndices[i]].Fallback; - ref var target = ref solver.ActiveSet.Fallback; + ref var source = ref solver.Sets[uniqueSetIndices[i]].JacobiFallback; + ref var target = ref solver.ActiveSet.JacobiFallback; if (source.bodyConstraintReferences.Count > 0) { for (int j = 0; j < source.bodyConstraintReferences.Count; ++j) @@ -493,7 +493,7 @@ unsafe internal (int phaseOneJobCount, int phaseTwoJobCount) PrepareJobs(ref Qui if (highestNewBatchCount < setBatchCount) highestNewBatchCount = setBatchCount; ref var constraintSet = ref solver.Sets[setIndex]; - additionalRequiredFallbackCapacity += constraintSet.Fallback.BodyCount; + additionalRequiredFallbackCapacity += constraintSet.JacobiFallback.BodyCount; for (int batchIndex = 0; batchIndex < constraintSet.Batches.Count; ++batchIndex) { ref var batch = ref constraintSet.Batches[batchIndex]; @@ -558,7 +558,7 @@ void AccumulatePairCacheTypeCounts(ref Buffer sourceTypeCaches, r //constraints, solver.ActiveSet.Batches.EnsureCapacity(highestNewBatchCount, pool); if (additionalRequiredFallbackCapacity > 0) - solver.ActiveSet.Fallback.EnsureCapacity(solver.ActiveSet.Fallback.BodyCount + additionalRequiredFallbackCapacity, pool); + solver.ActiveSet.JacobiFallback.EnsureCapacity(solver.ActiveSet.JacobiFallback.BodyCount + additionalRequiredFallbackCapacity, pool); Debug.Assert(highestNewBatchCount <= solver.FallbackBatchThreshold + 1, "Shouldn't have any batches beyond the fallback batch."); solver.batchReferencedHandles.EnsureCapacity(highestNewBatchCount, pool); for (int batchIndex = solver.ActiveSet.Batches.Count; batchIndex < highestNewBatchCount; ++batchIndex) diff --git a/BepuPhysics/IslandScaffold.cs b/BepuPhysics/IslandScaffold.cs index e20c16240..3caeffa6e 100644 --- a/BepuPhysics/IslandScaffold.cs +++ b/BepuPhysics/IslandScaffold.cs @@ -76,7 +76,7 @@ internal void Validate(Solver solver) } } - public unsafe bool TryAdd(ConstraintHandle constraintHandle, int batchIndex, Solver solver, BufferPool pool, ref FallbackBatch fallbackBatch) + public unsafe bool TryAdd(ConstraintHandle constraintHandle, int batchIndex, Solver solver, BufferPool pool, ref JacobiFallbackBatch fallbackBatch) { ref var constraintLocation = ref solver.HandleToConstraint[constraintHandle.Value]; var typeProcessor = solver.TypeProcessors[constraintLocation.TypeId]; @@ -137,7 +137,7 @@ internal struct IslandScaffold { public QuickList BodyIndices; public QuickList Protobatches; - public FallbackBatch FallbackBatch; + public JacobiFallbackBatch FallbackBatch; public IslandScaffold(ref QuickList bodyIndices, ref QuickList constraintHandles, Solver solver, BufferPool pool) : this() { diff --git a/BepuPhysics/IslandSleeper.cs b/BepuPhysics/IslandSleeper.cs index 8f5bab043..2596f3daa 100644 --- a/BepuPhysics/IslandSleeper.cs +++ b/BepuPhysics/IslandSleeper.cs @@ -779,7 +779,7 @@ void DisposeWorkerTraversalResults() //Pull the fallback batch data into the new fallback batch. Note that this isn't just a shallow copy; we're pushing all the allocations into the main pool. //They were previously on a thread-specific pool. (This isn't technically required right now, but it's cheap and convenient if we change the per thread pools //to make use of the usually-ephemeral nature of their allocations.) - FallbackBatch.CreateFrom(ref island.FallbackBatch, pool, out constraintSet.Fallback); + JacobiFallbackBatch.CreateFrom(ref island.FallbackBatch, pool, out constraintSet.JacobiFallback); } } } diff --git a/BepuPhysics/FallbackBatch.cs b/BepuPhysics/JacobiFallbackBatch.cs similarity index 95% rename from BepuPhysics/FallbackBatch.cs rename to BepuPhysics/JacobiFallbackBatch.cs index 423cadd22..bdf77eda8 100644 --- a/BepuPhysics/FallbackBatch.cs +++ b/BepuPhysics/JacobiFallbackBatch.cs @@ -11,7 +11,7 @@ namespace BepuPhysics { - public struct FallbackTypeBatchResults + public struct JacobiFallbackTypeBatchResults { public Buffer> BodyVelocities; @@ -26,7 +26,7 @@ public ref Buffer GetVelocitiesForBody(int slotIndex) /// Contains constraints that could not belong to any lower constraint batch due to their involved bodies. All of the contained constraints will be solved using a fallback solver that /// trades rigidity for parallelism. /// - public struct FallbackBatch + public struct JacobiFallbackBatch { public struct FallbackReference { @@ -92,30 +92,6 @@ unsafe void Allocate(ConstraintHandle constraintHandle, Sp } } - interface IBodyReferenceGetter - { - int GetBodyReference(Bodies bodies, BodyHandle handle); - } - - struct ActiveSetGetter : IBodyReferenceGetter - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public int GetBodyReference(Bodies bodies, BodyHandle bodyHandle) - { - ref var bodyLocation = ref bodies.HandleToLocation[bodyHandle.Value]; - Debug.Assert(bodyLocation.SetIndex == 0, "When creating a fallback batch for the active set, all bodies associated with it must be active."); - return bodyLocation.Index; - } - } - struct InactiveSetGetter : IBodyReferenceGetter - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public int GetBodyReference(Bodies bodies, BodyHandle bodyHandle) - { - return bodyHandle.Value; - } - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] internal unsafe void AllocateForActive(ConstraintHandle handle, Span constraintBodyHandles, Bodies bodies, @@ -221,7 +197,7 @@ internal unsafe void Remove(Solver solver, BufferPool bufferPool, ref Constraint } - public static void AllocateResults(Solver solver, BufferPool pool, ref ConstraintBatch batch, out Buffer results) + public static void AllocateResults(Solver solver, BufferPool pool, ref ConstraintBatch batch, out Buffer results) { pool.TakeAtLeast(batch.TypeBatches.Count, out results); for (int i = 0; i < batch.TypeBatches.Count; ++i) @@ -237,7 +213,7 @@ public static void AllocateResults(Solver solver, BufferPool pool, ref Constrain } } - public static void DisposeResults(Solver solver, BufferPool pool, ref ConstraintBatch batch, ref Buffer results) + public static void DisposeResults(Solver solver, BufferPool pool, ref ConstraintBatch batch, ref Buffer results) { for (int i = 0; i < batch.TypeBatches.Count; ++i) { @@ -368,8 +344,8 @@ public static unsafe void ValidateSetReferences(Solver solver, int setIndex) Debug.Assert(set.Allocated); if (set.Batches.Count > solver.FallbackBatchThreshold) { - Debug.Assert(set.Fallback.bodyConstraintReferences.Keys.Allocated); - ref var bodyConstraintReferences = ref set.Fallback.bodyConstraintReferences; + Debug.Assert(set.JacobiFallback.bodyConstraintReferences.Keys.Allocated); + ref var bodyConstraintReferences = ref set.JacobiFallback.bodyConstraintReferences; for (int i = 0; i < bodyConstraintReferences.Count; ++i) { //This is a handle on inactive sets, and an index for active sets. @@ -417,7 +393,7 @@ public static unsafe void ValidateReferences(Solver solver) } } - public void ScatterVelocities(Bodies bodies, Solver solver, ref Buffer velocities, int start, int exclusiveEnd) + public void ScatterVelocities(Bodies bodies, Solver solver, ref Buffer velocities, int start, int exclusiveEnd) { ref var fallbackBatch = ref solver.ActiveSet.Batches[solver.FallbackBatchThreshold]; for (int i = start; i < exclusiveEnd; ++i) @@ -487,7 +463,7 @@ internal void UpdateForBodyMemorySwap(int a, int b) Helpers.Swap(ref bodyConstraintReferences.Values[indexA], ref bodyConstraintReferences.Values[indexB]); } - internal static void CreateFrom(ref FallbackBatch sourceBatch, BufferPool pool, out FallbackBatch targetBatch) + internal static void CreateFrom(ref JacobiFallbackBatch sourceBatch, BufferPool pool, out JacobiFallbackBatch targetBatch) { //Copy over non-buffer state. This copies buffer references pointlessly, but that doesn't matter. targetBatch.bodyConstraintReferences = sourceBatch.bodyConstraintReferences; diff --git a/BepuPhysics/SequentialFallbackBatch.cs b/BepuPhysics/SequentialFallbackBatch.cs new file mode 100644 index 000000000..8452ade57 --- /dev/null +++ b/BepuPhysics/SequentialFallbackBatch.cs @@ -0,0 +1,320 @@ +using BepuPhysics.Constraints; +using BepuUtilities; +using BepuUtilities.Collections; +using BepuUtilities.Memory; +using System; +using System.Collections.Generic; +using System.Diagnostics; +using System.Numerics; +using System.Runtime.CompilerServices; +using System.Text; + +namespace BepuPhysics +{ + interface IBodyReferenceGetter + { + int GetBodyReference(Bodies bodies, BodyHandle handle); + } + + struct ActiveSetGetter : IBodyReferenceGetter + { + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public int GetBodyReference(Bodies bodies, BodyHandle bodyHandle) + { + ref var bodyLocation = ref bodies.HandleToLocation[bodyHandle.Value]; + Debug.Assert(bodyLocation.SetIndex == 0, "When creating a fallback batch for the active set, all bodies associated with it must be active."); + return bodyLocation.Index; + } + } + struct InactiveSetGetter : IBodyReferenceGetter + { + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public int GetBodyReference(Bodies bodies, BodyHandle bodyHandle) + { + return bodyHandle.Value; + } + } + + /// + /// Contains constraints that could not belong to any lower constraint batch due to their involved bodies. All of the contained constraints will be solved using a fallback solver that + /// trades rigidity for parallelism. + /// + public struct SequentialFallbackBatch + { + /// + /// Gets the number of bodies in the fallback batch. + /// + public readonly int BodyCount { get { return bodyConstraintCounts.Count; } } + + //In order to maintain the batch referenced handles for the fallback batch (which can have the same body appear more than once), + //every body must maintain a count of fallback constraints associated with it. + //Note that this dictionary uses active set body *indices* while active, but body *handles* when associated with an inactive set. + //This is consistent with the body references stored by active/inactive constraints. + internal QuickDictionary> bodyConstraintCounts; + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + unsafe void Allocate(Span constraintBodyHandles, Bodies bodies, + BufferPool pool, TBodyReferenceGetter bodyReferenceGetter, int minimumBodyCapacity) + where TBodyReferenceGetter : struct, IBodyReferenceGetter + { + EnsureCapacity(Math.Max(bodyConstraintCounts.Count + constraintBodyHandles.Length, minimumBodyCapacity), pool); + for (int i = 0; i < constraintBodyHandles.Length; ++i) + { + var bodyReference = bodyReferenceGetter.GetBodyReference(bodies, constraintBodyHandles[i]); + + if (bodyConstraintCounts.FindOrAllocateSlotUnsafely(bodyReference, out var slotIndex)) + { + ++bodyConstraintCounts.Values[slotIndex]; + } + else + { + bodyConstraintCounts.Values[slotIndex] = 1; + } + } + } + + + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + internal unsafe void AllocateForActive(Span constraintBodyHandles, Bodies bodies, + BufferPool pool, int minimumBodyCapacity = 8) + { + Allocate(constraintBodyHandles, bodies, pool, new ActiveSetGetter(), minimumBodyCapacity); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + internal void AllocateForInactive(Span constraintBodyHandles, Bodies bodies, + BufferPool pool, int minimumBodyCapacity = 8) + { + Allocate(constraintBodyHandles, bodies, pool, new InactiveSetGetter(), minimumBodyCapacity); + } + + + /// + /// Removes a constraint from a body in the fallback batch. + /// + /// Body associated with a constraint in the fallback batch. + /// Allocations that should be freed once execution is back in a safe context. + /// True if the body no longer has any constraints associated with it in the fallback batch, false otherwise. + internal unsafe bool Remove(int bodyReference, ref QuickList allocationIdsToFree) + { + var bodyPresent = bodyConstraintCounts.GetTableIndices(ref bodyReference, out var tableIndex, out var bodyReferencesIndex); + Debug.Assert(bodyPresent, "If we've been asked to remove a constraint associated with a body, that body must be in this batch."); + ref var constraintCount = ref bodyConstraintCounts.Values[bodyReferencesIndex]; + --constraintCount; + if (constraintCount == 0) + { + //If there are no more constraints associated with this body, get rid of the body list. + constraintCount = default; + bodyConstraintCounts.FastRemove(tableIndex, bodyReferencesIndex); + if (bodyConstraintCounts.Count == 0) + { + //No constraints remain in the fallback batch. Drop the dictionary. + allocationIdsToFree.AllocateUnsafely() = bodyConstraintCounts.Keys.Id; + allocationIdsToFree.AllocateUnsafely() = bodyConstraintCounts.Values.Id; + allocationIdsToFree.AllocateUnsafely() = bodyConstraintCounts.Table.Id; + bodyConstraintCounts = default; + } + return true; + } + return false; + } + + /// + /// Removes a body from the fallback batch if it is present. + /// + /// Reference to the body to remove from the fallback batch. + /// Allocations that should be freed once execution is back in a safe context. + /// True if the body was present in the fallback batch and was removed, false otherwise. + internal unsafe bool TryRemove(int bodyReference, ref QuickList allocationIdsToFree) + { + if (bodyConstraintCounts.Keys.Allocated && bodyConstraintCounts.GetTableIndices(ref bodyReference, out var tableIndex, out var bodyReferencesIndex)) + { + ref var constraintReferences = ref bodyConstraintCounts.Values[bodyReferencesIndex]; + //If there are no more constraints associated with this body, get rid of the body list. + bodyConstraintCounts.FastRemove(tableIndex, bodyReferencesIndex); + if (bodyConstraintCounts.Count == 0) + { + //No constraints remain in the fallback batch. Drop the dictionary. + allocationIdsToFree.AllocateUnsafely() = bodyConstraintCounts.Keys.Id; + allocationIdsToFree.AllocateUnsafely() = bodyConstraintCounts.Values.Id; + allocationIdsToFree.AllocateUnsafely() = bodyConstraintCounts.Table.Id; + bodyConstraintCounts = default; + } + return true; + } + return false; + } + + + internal unsafe void Remove(Solver solver, BufferPool bufferPool, ref ConstraintBatch batch, ConstraintHandle constraintHandle, ref IndexSet fallbackBatchHandles, int typeId, int indexInTypeBatch) + { + var typeProcessor = solver.TypeProcessors[typeId]; + var bodyCount = typeProcessor.BodiesPerConstraint; + var bodyIndices = stackalloc int[bodyCount]; + var enumerator = new ReferenceCollector(bodyIndices); + solver.EnumerateConnectedBodies(constraintHandle, ref enumerator); + var maximumAllocationIdsToFree = 3 + bodyCount * 2; + var allocationIdsToRemoveMemory = stackalloc int[maximumAllocationIdsToFree]; + var initialSpan = new Buffer(allocationIdsToRemoveMemory, maximumAllocationIdsToFree); + var allocationIdsToFree = new QuickList(initialSpan); + typeProcessor.EnumerateConnectedBodyIndices(ref batch.TypeBatches[batch.TypeIndexToTypeBatchIndex[typeId]], indexInTypeBatch, ref enumerator); + for (int i = 0; i < bodyCount; ++i) + { + if (Remove(bodyIndices[i], ref allocationIdsToFree)) + { + fallbackBatchHandles.Remove(solver.bodies.ActiveSet.IndexToHandle[bodyIndices[i]].Value); + } + } + for (int i = 0; i < allocationIdsToFree.Count; ++i) + { + bufferPool.ReturnUnsafely(allocationIdsToFree[i]); + } + } + + + public static void AllocateResults(Solver solver, BufferPool pool, ref ConstraintBatch batch, out Buffer results) + { + pool.TakeAtLeast(batch.TypeBatches.Count, out results); + for (int i = 0; i < batch.TypeBatches.Count; ++i) + { + ref var typeBatch = ref batch.TypeBatches[i]; + var bodiesPerConstraint = solver.TypeProcessors[typeBatch.TypeId].BodiesPerConstraint; + ref var typeBatchResults = ref results[i]; + pool.TakeAtLeast(bodiesPerConstraint, out typeBatchResults.BodyVelocities); + for (int j = 0; j < bodiesPerConstraint; ++j) + { + pool.TakeAtLeast(typeBatch.BundleCount, out typeBatchResults.GetVelocitiesForBody(j)); + } + } + } + + [Conditional("DEBUG")] + unsafe static void ValidateBodyConstraintReference(Solver solver, int setIndex, int bodyReference, ConstraintHandle constraintHandle, int expectedIndexInConstraint) + { + ref var constraintLocation = ref solver.HandleToConstraint[constraintHandle.Value]; + Debug.Assert(constraintLocation.SetIndex == setIndex); + Debug.Assert(constraintLocation.BatchIndex == solver.FallbackBatchThreshold, "Should only be working on constraints which are members of the active fallback batch."); + var debugReferences = stackalloc int[solver.TypeProcessors[constraintLocation.TypeId].BodiesPerConstraint]; + var debugBodyReferenceCollector = new ReferenceCollector(debugReferences); + solver.EnumerateConnectedBodies(constraintHandle, ref debugBodyReferenceCollector); + Debug.Assert(debugReferences[expectedIndexInConstraint] == bodyReference, "The constraint's true body references must agree with the fallback batch."); + } + [Conditional("DEBUG")] + public static unsafe void ValidateSetReferences(Solver solver, int setIndex) + { + ref var set = ref solver.Sets[setIndex]; + Debug.Assert(set.Allocated); + if (set.Batches.Count > solver.FallbackBatchThreshold) + { + Debug.Assert(set.SequentialFallback.bodyConstraintCounts.Keys.Allocated); + ref var bodyConstraintCounts = ref set.SequentialFallback.bodyConstraintCounts; + for (int i = 0; i < bodyConstraintCounts.Count; ++i) + { + //This is a handle on inactive sets, and an index for active sets. + var bodyReference = bodyConstraintCounts.Keys[i]; + var count = bodyConstraintCounts.Values[i]; + Debug.Assert(count > 0, "If there exists a body reference set, it should be populated."); + } + ref var batch = ref set.Batches[solver.FallbackBatchThreshold]; + for (int typeBatchIndex = 0; typeBatchIndex < batch.TypeBatches.Count; ++typeBatchIndex) + { + ref var typeBatch = ref batch.TypeBatches[typeBatchIndex]; + var bodiesPerConstraint = solver.TypeProcessors[typeBatch.TypeId].BodiesPerConstraint; + var connectedBodies = stackalloc int[bodiesPerConstraint]; + for (int constraintIndex = 0; constraintIndex < typeBatch.ConstraintCount; ++constraintIndex) + { + var constraintHandle = typeBatch.IndexToHandle[constraintIndex]; + var collector = new ReferenceCollector(connectedBodies); + solver.EnumerateConnectedBodies(constraintHandle, ref collector); + for (int i = 0; i < bodiesPerConstraint; ++i) + { + var localBodyIndex = bodyConstraintCounts.IndexOf(connectedBodies[i]); + Debug.Assert(localBodyIndex >= 0, "Any body referenced by a constraint in the fallback batch should exist within the fallback batch's body listing."); + var count = bodyConstraintCounts.Values[localBodyIndex]; + } + } + } + } + } + [Conditional("DEBUG")] + public static unsafe void ValidateReferences(Solver solver) + { + for (int i = 0; i < solver.Sets.Length; ++i) + { + if (solver.Sets[i].Allocated) + ValidateSetReferences(solver, i); + } + } + internal void UpdateForBodyMemoryMove(int originalBodyIndex, int newBodyLocation) + { + Debug.Assert(bodyConstraintCounts.Keys.Allocated && !bodyConstraintCounts.ContainsKey(newBodyLocation), "If a body is being moved, as opposed to swapped, then the target index should not be present."); + bodyConstraintCounts.GetTableIndices(ref originalBodyIndex, out var tableIndex, out var elementIndex); + var references = bodyConstraintCounts.Values[elementIndex]; + bodyConstraintCounts.FastRemove(tableIndex, elementIndex); + bodyConstraintCounts.AddUnsafely(ref newBodyLocation, references); + } + + internal void UpdateForBodyMemorySwap(int a, int b) + { + var indexA = bodyConstraintCounts.IndexOf(a); + var indexB = bodyConstraintCounts.IndexOf(b); + Debug.Assert(indexA >= 0 && indexB >= 0, "A swap requires that both indices are already present."); + Helpers.Swap(ref bodyConstraintCounts.Values[indexA], ref bodyConstraintCounts.Values[indexB]); + } + + internal static void CreateFrom(ref JacobiFallbackBatch sourceBatch, BufferPool pool, out JacobiFallbackBatch targetBatch) + { + //Copy over non-buffer state. This copies buffer references pointlessly, but that doesn't matter. + targetBatch.bodyConstraintReferences = sourceBatch.bodyConstraintReferences; + pool.TakeAtLeast(sourceBatch.bodyConstraintReferences.Count, out targetBatch.bodyConstraintReferences.Keys); + pool.TakeAtLeast(targetBatch.bodyConstraintReferences.Keys.Length, out targetBatch.bodyConstraintReferences.Values); + pool.TakeAtLeast(sourceBatch.bodyConstraintReferences.TableMask + 1, out targetBatch.bodyConstraintReferences.Table); + sourceBatch.bodyConstraintReferences.Keys.CopyTo(0, targetBatch.bodyConstraintReferences.Keys, 0, sourceBatch.bodyConstraintReferences.Count); + sourceBatch.bodyConstraintReferences.Values.CopyTo(0, targetBatch.bodyConstraintReferences.Values, 0, sourceBatch.bodyConstraintReferences.Count); + sourceBatch.bodyConstraintReferences.Table.CopyTo(0, targetBatch.bodyConstraintReferences.Table, 0, sourceBatch.bodyConstraintReferences.TableMask + 1); + + for (int i = 0; i < sourceBatch.bodyConstraintReferences.Count; ++i) + { + ref var source = ref sourceBatch.bodyConstraintReferences.Values[i]; + ref var target = ref targetBatch.bodyConstraintReferences.Values[i]; + target = source; + pool.TakeAtLeast(source.Count, out target.Span); + pool.TakeAtLeast(source.TableMask + 1, out target.Table); + source.Span.CopyTo(0, target.Span, 0, source.Count); + source.Table.CopyTo(0, target.Table, 0, source.TableMask + 1); + } + } + + internal void EnsureCapacity(int bodyCapacity, BufferPool pool) + { + if (bodyConstraintCounts.Keys.Allocated) + { + //This is conservative since there's no guarantee that we'll actually need to resize at all if these bodies are already present, but that's fine. + bodyConstraintCounts.EnsureCapacity(bodyCapacity, pool); + } + else + { + bodyConstraintCounts = new QuickDictionary>(bodyCapacity, pool); + } + + } + + public void Compact(BufferPool pool) + { + if (bodyConstraintCounts.Keys.Allocated) + { + bodyConstraintCounts.Compact(pool); + } + } + + + public void Dispose(BufferPool pool) + { + if (bodyConstraintCounts.Keys.Allocated) + { + bodyConstraintCounts.Dispose(pool); + } + } + } +} diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index 7eb6e374e..ad2872f45 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -302,7 +302,7 @@ public unsafe void ValidateExistingHandles(bool activeOnly = false) else { //If this is the fallback batch, then the expected count may be more than 1. - var foundBody = ActiveSet.Fallback.bodyConstraintReferences.TryGetValue(i, out var referencesForBody); + var foundBody = ActiveSet.JacobiFallback.bodyConstraintReferences.TryGetValue(i, out var referencesForBody); Debug.Assert(foundBody, "A body was in the fallback batch's referenced handles, so the fallback batch should have a reference for that body."); expectedCount = foundBody ? referencesForBody.Count : 0; } @@ -482,7 +482,7 @@ internal unsafe void AllocateInBatch(int targetBatchIndex, ConstraintHandle cons } if (targetBatchIndex == FallbackBatchThreshold) { - ActiveSet.Fallback.AllocateForActive(constraintHandle, bodyHandles, bodies, typeId, pool); + ActiveSet.JacobiFallback.AllocateForActive(constraintHandle, bodyHandles, bodies, typeId, pool); } } @@ -839,7 +839,7 @@ internal unsafe void RemoveFromBatch(ConstraintHandle constraintHandle, int batc if (batchIndex == FallbackBatchThreshold) { //Note that we have to remove from fallback first because it accesses the batch's information. - ActiveSet.Fallback.Remove(this, pool, ref batch, constraintHandle, ref batchReferencedHandles[batchIndex], typeId, indexInTypeBatch); + ActiveSet.JacobiFallback.Remove(this, pool, ref batch, constraintHandle, ref batchReferencedHandles[batchIndex], typeId, indexInTypeBatch); } else { @@ -936,7 +936,7 @@ internal void UpdateForBodyMemoryMove(int originalBodyIndex, int newBodyLocation if (UpdateConstraintsForBodyMemoryMove(originalBodyIndex, newBodyLocation)) { //One of the moved constraints involved the fallback batch, so we need to update the fallback batch's body indices. - ActiveSet.Fallback.UpdateForBodyMemoryMove(originalBodyIndex, newBodyLocation); + ActiveSet.JacobiFallback.UpdateForBodyMemoryMove(originalBodyIndex, newBodyLocation); } } @@ -951,15 +951,15 @@ internal void UpdateForBodyMemorySwap(int a, int b) var bInFallback = UpdateConstraintsForBodyMemoryMove(b, a); if (aInFallback && bInFallback) { - ActiveSet.Fallback.UpdateForBodyMemorySwap(a, b); + ActiveSet.JacobiFallback.UpdateForBodyMemorySwap(a, b); } else if (aInFallback) { - ActiveSet.Fallback.UpdateForBodyMemoryMove(a, b); + ActiveSet.JacobiFallback.UpdateForBodyMemoryMove(a, b); } else if (bInFallback) { - ActiveSet.Fallback.UpdateForBodyMemoryMove(b, a); + ActiveSet.JacobiFallback.UpdateForBodyMemoryMove(b, a); } } diff --git a/BepuPhysics/Solver_Solve.cs b/BepuPhysics/Solver_Solve.cs index e91fb7f04..a905b179e 100644 --- a/BepuPhysics/Solver_Solve.cs +++ b/BepuPhysics/Solver_Solve.cs @@ -124,10 +124,10 @@ void BuildFallbackScatterWorkBlocks(int targetBlocksPerBatch) if (activeSet.Batches.Count > FallbackBatchThreshold) { //There is a fallback batch, so we need to create fallback work blocks for it. - var blockCount = Math.Min(targetBlocksPerBatch, ActiveSet.Fallback.BodyCount); + var blockCount = Math.Min(targetBlocksPerBatch, ActiveSet.JacobiFallback.BodyCount); context.FallbackBlocks.Blocks = new QuickList(blockCount, pool); - var baseBodiesPerBlock = activeSet.Fallback.BodyCount / blockCount; - var remainder = activeSet.Fallback.BodyCount - baseBodiesPerBlock * blockCount; + var baseBodiesPerBlock = activeSet.JacobiFallback.BodyCount / blockCount; + var remainder = activeSet.JacobiFallback.BodyCount - baseBodiesPerBlock * blockCount; int previousEnd = 0; for (int i = 0; i < blockCount; ++i) { @@ -249,7 +249,7 @@ protected struct MultithreadingParameters public WorkBlocks FallbackBlocks; public int WorkerCompletedCount; public int WorkerCount; - public Buffer FallbackResults; + public Buffer FallbackResults; public WorkBlocks IncrementalUpdateBlocks; public Buffer IncrementalUpdateBatchBoundaries; @@ -358,7 +358,7 @@ public void Execute(Solver solver, int blockIndex, int workerIndex) if (block.BatchIndex < solver.FallbackBatchThreshold) typeProcessor.Prestep(ref typeBatch, solver.bodies, Dt, InverseDt, block.StartBundle, block.End); else - typeProcessor.JacobiPrestep(ref typeBatch, solver.bodies, ref solver.ActiveSet.Fallback, Dt, InverseDt, block.StartBundle, block.End); + typeProcessor.JacobiPrestep(ref typeBatch, solver.bodies, ref solver.ActiveSet.JacobiFallback, Dt, InverseDt, block.StartBundle, block.End); } } struct WarmStartStageFunction : IStageFunction @@ -415,7 +415,7 @@ struct FallbackScatterStageFunction : IStageFunction public void Execute(Solver solver, int blockIndex, int workerIndex) { ref var block = ref solver.context.FallbackBlocks.Blocks[blockIndex]; - solver.ActiveSet.Fallback.ScatterVelocities(solver.bodies, solver, ref solver.context.FallbackResults, block.Start, block.End); + solver.ActiveSet.JacobiFallback.ScatterVelocities(solver.bodies, solver, ref solver.context.FallbackResults, block.Start, block.End); } } @@ -758,7 +758,7 @@ protected void ExecuteMultithreaded(float dt, IThreadDisp if (filter.AllowFallback && ActiveSet.Batches.Count > FallbackBatchThreshold) { Debug.Assert(context.FallbackBlocks.Blocks.Count > 0); - FallbackBatch.AllocateResults(this, pool, ref ActiveSet.Batches[FallbackBatchThreshold], out context.FallbackResults); + JacobiFallbackBatch.AllocateResults(this, pool, ref ActiveSet.Batches[FallbackBatchThreshold], out context.FallbackResults); context.FallbackBlocks.CreateClaims(pool); } pool.Take(workerCount, out context.WorkerBoundsA); @@ -783,7 +783,7 @@ protected void ExecuteMultithreaded(float dt, IThreadDisp context.IncrementalUpdateBlocks.Dispose(pool); if (filter.AllowFallback && ActiveSet.Batches.Count > FallbackBatchThreshold) { - FallbackBatch.DisposeResults(this, pool, ref ActiveSet.Batches[FallbackBatchThreshold], ref context.FallbackResults); + JacobiFallbackBatch.DisposeResults(this, pool, ref ActiveSet.Batches[FallbackBatchThreshold], ref context.FallbackResults); context.FallbackBlocks.Dispose(pool); } pool.Return(ref context.BatchBoundaries); @@ -815,7 +815,7 @@ public void Solve(float dt, IThreadDispatcher threadDispatcher = null) for (int j = 0; j < batch.TypeBatches.Count; ++j) { ref var typeBatch = ref batch.TypeBatches[j]; - TypeProcessors[typeBatch.TypeId].JacobiPrestep(ref typeBatch, bodies, ref activeSet.Fallback, dt, inverseDt, 0, typeBatch.BundleCount); + TypeProcessors[typeBatch.TypeId].JacobiPrestep(ref typeBatch, bodies, ref activeSet.JacobiFallback, dt, inverseDt, 0, typeBatch.BundleCount); } } //TODO: May want to consider executing warmstart immediately following the prestep. Multithreading can't do that, so there could be some bitwise differences introduced. @@ -829,17 +829,17 @@ public void Solve(float dt, IThreadDispatcher threadDispatcher = null) TypeProcessors[typeBatch.TypeId].WarmStart(ref typeBatch, bodies, 0, typeBatch.BundleCount); } } - Buffer fallbackResults = default; + Buffer fallbackResults = default; if (fallbackExists) { ref var batch = ref activeSet.Batches[FallbackBatchThreshold]; - FallbackBatch.AllocateResults(this, pool, ref batch, out fallbackResults); + JacobiFallbackBatch.AllocateResults(this, pool, ref batch, out fallbackResults); for (int j = 0; j < batch.TypeBatches.Count; ++j) { ref var typeBatch = ref batch.TypeBatches[j]; TypeProcessors[typeBatch.TypeId].JacobiWarmStart(ref typeBatch, bodies, ref fallbackResults[j], 0, typeBatch.BundleCount); } - activeSet.Fallback.ScatterVelocities(bodies, this, ref fallbackResults, 0, activeSet.Fallback.BodyCount); + activeSet.JacobiFallback.ScatterVelocities(bodies, this, ref fallbackResults, 0, activeSet.JacobiFallback.BodyCount); } for (int iterationIndex = 0; iterationIndex < iterationCount; ++iterationIndex) { @@ -860,12 +860,12 @@ public void Solve(float dt, IThreadDispatcher threadDispatcher = null) ref var typeBatch = ref batch.TypeBatches[j]; TypeProcessors[typeBatch.TypeId].JacobiSolveIteration(ref typeBatch, bodies, ref fallbackResults[j], 0, typeBatch.BundleCount); } - activeSet.Fallback.ScatterVelocities(bodies, this, ref fallbackResults, 0, activeSet.Fallback.BodyCount); + activeSet.JacobiFallback.ScatterVelocities(bodies, this, ref fallbackResults, 0, activeSet.JacobiFallback.BodyCount); } } if (fallbackExists) { - FallbackBatch.DisposeResults(this, pool, ref activeSet.Batches[FallbackBatchThreshold], ref fallbackResults); + JacobiFallbackBatch.DisposeResults(this, pool, ref activeSet.Batches[FallbackBatchThreshold], ref fallbackResults); } } else diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index d7a0b03f9..537cdbdd1 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -55,7 +55,7 @@ internal struct SubstepMultithreadingContext [FieldOffset(48)] public Buffer FallbackBlocks; [FieldOffset(64)] - public Buffer FallbackResults; + public Buffer FallbackResults; [FieldOffset(80)] public Buffer ConstraintBatchBoundaries; [FieldOffset(96)] @@ -180,7 +180,7 @@ public void Execute(Solver solver, int blockIndex, int workerIndex) ref var block = ref this.solver.substepContext.ConstraintBlocks[blockIndex]; ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; - typeProcessor.JacobiWarmStart2(ref typeBatch, this.solver.bodies, ref this.solver.ActiveSet.Fallback, ref this.solver.substepContext.FallbackResults[block.TypeBatchIndex], Dt, InverseDt, block.StartBundle, block.End, workerIndex); + typeProcessor.JacobiWarmStart2(ref typeBatch, this.solver.bodies, ref this.solver.ActiveSet.JacobiFallback, ref this.solver.substepContext.FallbackResults[block.TypeBatchIndex], Dt, InverseDt, block.StartBundle, block.End, workerIndex); } } @@ -212,7 +212,7 @@ public void Execute(Solver solver, int blockIndex, int workerIndex) ref var block = ref this.solver.substepContext.ConstraintBlocks[blockIndex]; ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; - typeProcessor.JacobiSolveStep2(ref typeBatch, solver.bodies, ref this.solver.ActiveSet.Fallback, ref this.solver.substepContext.FallbackResults[block.TypeBatchIndex], Dt, InverseDt, block.StartBundle, block.End); + typeProcessor.JacobiSolveStep2(ref typeBatch, solver.bodies, ref this.solver.ActiveSet.JacobiFallback, ref this.solver.substepContext.FallbackResults[block.TypeBatchIndex], Dt, InverseDt, block.StartBundle, block.End); } } @@ -226,7 +226,7 @@ struct JacobiScatterStageFunction : IStageFunction public void Execute(Solver solver, int blockIndex, int workerIndex) { ref var block = ref this.solver.substepContext.FallbackBlocks[blockIndex]; - solver.ActiveSet.Fallback.ScatterVelocities(solver.bodies, solver, ref this.solver.substepContext.FallbackResults, block.Start, block.End); + solver.ActiveSet.JacobiFallback.ScatterVelocities(solver.bodies, solver, ref this.solver.substepContext.FallbackResults, block.Start, block.End); } } @@ -545,12 +545,12 @@ void BuildFallbackScatterWorkBlocks(int targetBlocksPerBatch, out Buffer FallbackBatchThreshold) { //There is a fallback batch, so we need to create fallback work blocks for it. - var blockCount = Math.Min(targetBlocksPerBatch, ActiveSet.Fallback.BodyCount); + var blockCount = Math.Min(targetBlocksPerBatch, ActiveSet.JacobiFallback.BodyCount); pool.Take(blockCount, out workBlocks); var blocksList = new QuickList(workBlocks); context.FallbackBlocks.Blocks = new QuickList(blockCount, pool); - var baseBodiesPerBlock = activeSet.Fallback.BodyCount / blockCount; - var remainder = activeSet.Fallback.BodyCount - baseBodiesPerBlock * blockCount; + var baseBodiesPerBlock = activeSet.JacobiFallback.BodyCount / blockCount; + var remainder = activeSet.JacobiFallback.BodyCount - baseBodiesPerBlock * blockCount; int previousEnd = 0; for (int i = 0; i < blockCount; ++i) { @@ -600,7 +600,7 @@ protected void ExecuteMultithreaded2(float dt, IThreadDispatcher threadDispatche if (fallbackExists) { Debug.Assert(substepContext.FallbackBlocks.Length > 0); - FallbackBatch.AllocateResults(this, pool, ref ActiveSet.Batches[FallbackBatchThreshold], out substepContext.FallbackResults); + JacobiFallbackBatch.AllocateResults(this, pool, ref ActiveSet.Batches[FallbackBatchThreshold], out substepContext.FallbackResults); totalClaimCount += substepContext.FallbackBlocks.Length; stagesPerIteration += 2; //jacobi batch, plus scatter. } @@ -753,7 +753,7 @@ protected void ExecuteMultithreaded2(float dt, IThreadDispatcher threadDispatche if (fallbackExists) { - FallbackBatch.DisposeResults(this, pool, ref ActiveSet.Batches[FallbackBatchThreshold], ref substepContext.FallbackResults); + JacobiFallbackBatch.DisposeResults(this, pool, ref ActiveSet.Batches[FallbackBatchThreshold], ref substepContext.FallbackResults); pool.Return(ref substepContext.FallbackBlocks); } pool.Return(ref claims); @@ -1243,18 +1243,18 @@ public override void SolveStep2(float totalDt, IThreadDispatcher threadDispatche } } } - Buffer fallbackResults = default; + Buffer fallbackResults = default; if (fallbackExists) { ref var batch = ref activeSet.Batches[FallbackBatchThreshold]; - FallbackBatch.AllocateResults(this, pool, ref batch, out fallbackResults); + JacobiFallbackBatch.AllocateResults(this, pool, ref batch, out fallbackResults); for (int j = 0; j < batch.TypeBatches.Count; ++j) { ref var typeBatch = ref batch.TypeBatches[j]; - TypeProcessors[typeBatch.TypeId].JacobiWarmStart2(ref typeBatch, bodies, ref ActiveSet.Fallback, ref fallbackResults[j], substepDt, inverseDt, 0, typeBatch.BundleCount, 0); + TypeProcessors[typeBatch.TypeId].JacobiWarmStart2(ref typeBatch, bodies, ref ActiveSet.JacobiFallback, ref fallbackResults[j], substepDt, inverseDt, 0, typeBatch.BundleCount, 0); } - activeSet.Fallback.ScatterVelocities(bodies, this, ref fallbackResults, 0, activeSet.Fallback.BodyCount); + activeSet.JacobiFallback.ScatterVelocities(bodies, this, ref fallbackResults, 0, activeSet.JacobiFallback.BodyCount); } for (int iterationIndex = 0; iterationIndex < IterationCount; ++iterationIndex) @@ -1274,14 +1274,14 @@ public override void SolveStep2(float totalDt, IThreadDispatcher threadDispatche for (int j = 0; j < batch.TypeBatches.Count; ++j) { ref var typeBatch = ref batch.TypeBatches[j]; - TypeProcessors[typeBatch.TypeId].JacobiSolveStep2(ref typeBatch, bodies, ref ActiveSet.Fallback, ref fallbackResults[j], substepDt, inverseDt, 0, typeBatch.BundleCount); + TypeProcessors[typeBatch.TypeId].JacobiSolveStep2(ref typeBatch, bodies, ref ActiveSet.JacobiFallback, ref fallbackResults[j], substepDt, inverseDt, 0, typeBatch.BundleCount); } - activeSet.Fallback.ScatterVelocities(bodies, this, ref fallbackResults, 0, activeSet.Fallback.BodyCount); + activeSet.JacobiFallback.ScatterVelocities(bodies, this, ref fallbackResults, 0, activeSet.JacobiFallback.BodyCount); } } if (fallbackExists) { - FallbackBatch.DisposeResults(this, pool, ref activeSet.Batches[FallbackBatchThreshold], ref fallbackResults); + JacobiFallbackBatch.DisposeResults(this, pool, ref activeSet.Batches[FallbackBatchThreshold], ref fallbackResults); } } } From 4d445523ecafb4ed4e8f94e747b6a56a22b64cb3 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 14 Oct 2021 15:29:38 -0500 Subject: [PATCH 205/947] Purged jacobi fallback due to structural incompatibility with sequential fallback prototype. Builds, but fallback batch not yet functional. --- .../CollisionDetection/ConstraintRemover.cs | 10 +- BepuPhysics/ConstraintSet.cs | 4 - .../Constraints/FourBodyTypeProcessor.cs | 175 ------ .../Constraints/OneBodyTypeProcessor.cs | 111 ---- .../Constraints/ThreeBodyTypeProcessor.cs | 156 ------ .../Constraints/TwoBodyTypeProcessor.cs | 130 ----- BepuPhysics/Constraints/TypeProcessor.cs | 10 - BepuPhysics/IslandAwakener.cs | 18 +- BepuPhysics/IslandScaffold.cs | 6 +- BepuPhysics/IslandSleeper.cs | 2 +- BepuPhysics/JacobiFallbackBatch.cs | 528 ------------------ BepuPhysics/SequentialFallbackBatch.cs | 44 +- BepuPhysics/Solver.cs | 16 +- BepuPhysics/Solver_Solve.cs | 147 +---- BepuPhysics/Solver_SubsteppingSolve2.cs | 217 +------ 15 files changed, 52 insertions(+), 1522 deletions(-) delete mode 100644 BepuPhysics/JacobiFallbackBatch.cs diff --git a/BepuPhysics/CollisionDetection/ConstraintRemover.cs b/BepuPhysics/CollisionDetection/ConstraintRemover.cs index fe950faf1..a289c7222 100644 --- a/BepuPhysics/CollisionDetection/ConstraintRemover.cs +++ b/BepuPhysics/CollisionDetection/ConstraintRemover.cs @@ -210,9 +210,9 @@ public void Prepare(IThreadDispatcher dispatcher) //The island sleeper job order requires this allocation to be done in the Prepare instead of CreateFlushJobs. if (solver.ActiveSet.Batches.Count > solver.FallbackBatchThreshold) { - //Ensure that the fallback deallocation list is also large enough. The fallback batch may result in 3 returned buffers for the primary dictionary, plus another two for each potentially - //removed body constraint references subset. - allocationIdsToFree = new QuickList(3 + solver.ActiveSet.JacobiFallback.BodyCount * 2, pool); + //Ensure that the fallback deallocation list is also large enough. The fallback batch may result in 3 returned buffers for the primary dictionary. + //TODO: Since this is no longer a variable count, there's no reason to allocate a list like this. + allocationIdsToFree = new QuickList(3, pool); } } @@ -376,7 +376,7 @@ public void RemoveConstraintsFromFallbackBatch() for (int j = 0; j < removals.Count; ++j) { ref var target = ref removals[j]; - if (solver.ActiveSet.JacobiFallback.Remove(target.BodyIndex, target.ConstraintHandle, ref allocationIdsToFree)) + if (solver.ActiveSet.SequentialFallback.Remove(target.BodyIndex, ref allocationIdsToFree)) { //No more constraints for this body in the fallback set; it should not exist in the fallback batch's referenced handles anymore. solver.batchReferencedHandles[target.BatchIndex].Remove(target.BodyHandle.Value); @@ -387,7 +387,7 @@ public void RemoveConstraintsFromFallbackBatch() } public void TryRemoveAllConstraintsForBodyFromFallbackBatch(BodyHandle bodyHandle, int bodyIndex) { - if (solver.ActiveSet.JacobiFallback.TryRemove(bodyIndex, ref allocationIdsToFree)) + if (solver.ActiveSet.SequentialFallback.TryRemove(bodyIndex, ref allocationIdsToFree)) { solver.batchReferencedHandles[solver.FallbackBatchThreshold].Remove(bodyHandle.Value); } diff --git a/BepuPhysics/ConstraintSet.cs b/BepuPhysics/ConstraintSet.cs index 6616a6bae..8aa1a6dae 100644 --- a/BepuPhysics/ConstraintSet.cs +++ b/BepuPhysics/ConstraintSet.cs @@ -7,13 +7,11 @@ namespace BepuPhysics public struct ConstraintSet { public QuickList Batches; - public JacobiFallbackBatch JacobiFallback; public SequentialFallbackBatch SequentialFallback; public ConstraintSet(BufferPool pool, int initialBatchCapacity) { Batches = new QuickList(initialBatchCapacity, pool); - JacobiFallback = default; SequentialFallback = default; } @@ -72,7 +70,6 @@ public void Clear(BufferPool pool) { Batches[i].Dispose(pool); } - JacobiFallback.Dispose(pool); SequentialFallback.Dispose(pool); Batches.Count = 0; } @@ -83,7 +80,6 @@ public void Dispose(BufferPool pool) { Batches[i].Dispose(pool); } - JacobiFallback.Dispose(pool); SequentialFallback.Dispose(pool); Batches.Dispose(pool); this = new ConstraintSet(); diff --git a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs index ba7ea1c49..71fcdc0fe 100644 --- a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs @@ -196,92 +196,6 @@ public unsafe override void SolveIteration(ref TypeBatch typeBatch, Bodies bodie } } - public unsafe override void JacobiPrestep(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackBatch jacobiBatch, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) - { - ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); - ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); - ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); - var function = default(TConstraintFunctions); - for (int i = startBundle; i < exclusiveEndBundle; ++i) - { - ref var prestep = ref Unsafe.Add(ref prestepBase, i); - ref var projection = ref Unsafe.Add(ref projectionBase, i); - ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); - var count = GetCountInBundle(ref typeBatch, i); - bodies.GatherState(ref references, count, - out var orientationA, out var wsvA, out var inertiaA, - out var ab, out var orientationB, out var wsvB, out var inertiaB, - out var ac, out var orientationC, out var wsvC, out var inertiaC, - out var ad, out var orientationD, out var wsvD, out var inertiaD); - //Jacobi batches split affected bodies into multiple pieces to guarantee convergence. - jacobiBatch.GetJacobiScaleForBodies(ref references, count, out var jacobiScaleA, out var jacobiScaleB, out var jacobiScaleC, out var jacobiScaleD); - Symmetric3x3Wide.Scale(inertiaA.InverseInertiaTensor, jacobiScaleA, out inertiaA.InverseInertiaTensor); - inertiaA.InverseMass *= jacobiScaleA; - Symmetric3x3Wide.Scale(inertiaB.InverseInertiaTensor, jacobiScaleB, out inertiaB.InverseInertiaTensor); - inertiaB.InverseMass *= jacobiScaleB; - Symmetric3x3Wide.Scale(inertiaC.InverseInertiaTensor, jacobiScaleC, out inertiaC.InverseInertiaTensor); - inertiaC.InverseMass *= jacobiScaleC; - Symmetric3x3Wide.Scale(inertiaD.InverseInertiaTensor, jacobiScaleD, out inertiaD.InverseInertiaTensor); - inertiaD.InverseMass *= jacobiScaleD; - function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, ac, orientationC, inertiaC, ad, orientationD, inertiaD, dt, inverseDt, ref prestep, out projection); - } - } - public unsafe override void JacobiWarmStart(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle) - { - ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); - ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); - ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); - var function = default(TConstraintFunctions); - ref var jacobiResultsBundlesA = ref jacobiResults.GetVelocitiesForBody(0); - ref var jacobiResultsBundlesB = ref jacobiResults.GetVelocitiesForBody(1); - ref var jacobiResultsBundlesC = ref jacobiResults.GetVelocitiesForBody(2); - ref var jacobiResultsBundlesD = ref jacobiResults.GetVelocitiesForBody(3); - for (int i = startBundle; i < exclusiveEndBundle; ++i) - { - ref var projection = ref Unsafe.Add(ref projectionBase, i); - ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); - ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); - int count = GetCountInBundle(ref typeBatch, i); - ref var wsvA = ref jacobiResultsBundlesA[i]; - ref var wsvB = ref jacobiResultsBundlesB[i]; - ref var wsvC = ref jacobiResultsBundlesC[i]; - ref var wsvD = ref jacobiResultsBundlesD[i]; - bodies.GatherState(ref bodyReferences, count, - out var orientationA, out wsvA, out var inertiaA, - out var ab, out var orientationB, out wsvB, out var inertiaB, - out var ac, out var orientationC, out wsvC, out var inertiaC, - out var ad, out var orientationD, out wsvD, out var inertiaD); - function.WarmStart(ref wsvA, ref wsvB, ref wsvC, ref wsvD, ref projection, ref accumulatedImpulses); - } - } - public unsafe override void JacobiSolveIteration(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle) - { - ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); - ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); - ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); - var function = default(TConstraintFunctions); - ref var jacobiResultsBundlesA = ref jacobiResults.GetVelocitiesForBody(0); - ref var jacobiResultsBundlesB = ref jacobiResults.GetVelocitiesForBody(1); - ref var jacobiResultsBundlesC = ref jacobiResults.GetVelocitiesForBody(2); - ref var jacobiResultsBundlesD = ref jacobiResults.GetVelocitiesForBody(3); - for (int i = startBundle; i < exclusiveEndBundle; ++i) - { - ref var projection = ref Unsafe.Add(ref projectionBase, i); - ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); - ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); - int count = GetCountInBundle(ref typeBatch, i); - ref var wsvA = ref jacobiResultsBundlesA[i]; - ref var wsvB = ref jacobiResultsBundlesB[i]; - ref var wsvC = ref jacobiResultsBundlesC[i]; - ref var wsvD = ref jacobiResultsBundlesD[i]; - bodies.GatherState(ref bodyReferences, count, - out var orientationA, out wsvA, out var inertiaA, - out var ab, out var orientationB, out wsvB, out var inertiaB, - out var ac, out var orientationC, out wsvC, out var inertiaC, - out var ad, out var orientationD, out wsvC, out var inertiaD); - function.Solve(ref wsvA, ref wsvB, ref wsvC, ref wsvD, ref projection, ref accumulatedImpulses); - } - } public unsafe override void WarmStart2( ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, in TBundleCountCalculator bundleCountCalculator, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) @@ -358,94 +272,5 @@ public unsafe override void SolveStep2(ref TypeBatch typ } } - - public override void JacobiWarmStart2(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackBatch jacobiBatch, ref JacobiFallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) - { - var prestepBundles = typeBatch.PrestepData.As(); - var bodyReferencesBundles = typeBatch.BodyReferences.As(); - var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); - Unsafe.SkipInit(out TConstraintFunctions function); - ref var states = ref bodies.ActiveSet.SolverStates; - //EarlyPrefetch(WarmStartPrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref states, startBundle, exclusiveEndBundle); - ref var jacobiResultsBundlesA = ref jacobiResults.GetVelocitiesForBody(0); - ref var jacobiResultsBundlesB = ref jacobiResults.GetVelocitiesForBody(1); - ref var jacobiResultsBundlesC = ref jacobiResults.GetVelocitiesForBody(2); - ref var jacobiResultsBundlesD = ref jacobiResults.GetVelocitiesForBody(3); - for (int i = startBundle; i < exclusiveEndBundle; ++i) - { - ref var prestep = ref prestepBundles[i]; - ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; - ref var references = ref bodyReferencesBundles[i]; - ref var wsvA = ref jacobiResultsBundlesA[i]; - ref var wsvB = ref jacobiResultsBundlesB[i]; - ref var wsvC = ref jacobiResultsBundlesC[i]; - ref var wsvD = ref jacobiResultsBundlesD[i]; - var count = GetCountInBundle(ref typeBatch, i); - //Prefetch(WarmStartPrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref states, i, exclusiveEndBundle); - //Note jacobi batches do not do access filtering at the moment. The fallback accumulation expects all velocities to be present. - //Also, jacobi batches cannot integrate. - bodies.GatherState(ref references.IndexA, count, true, out var positionA, out var orientationA, out wsvA, out var inertiaA); - bodies.GatherState(ref references.IndexB, count, true, out var positionB, out var orientationB, out wsvB, out var inertiaB); - bodies.GatherState(ref references.IndexC, count, true, out var positionC, out var orientationC, out wsvC, out var inertiaC); - bodies.GatherState(ref references.IndexD, count, true, out var positionD, out var orientationD, out wsvD, out var inertiaD); - jacobiBatch.GetJacobiScaleForBodies(ref references, count, out var jacobiScaleA, out var jacobiScaleB, out var jacobiScaleC, out var jacobiScaleD); - Symmetric3x3Wide.Scale(inertiaA.InverseInertiaTensor, jacobiScaleA, out inertiaA.InverseInertiaTensor); - inertiaA.InverseMass *= jacobiScaleA; - Symmetric3x3Wide.Scale(inertiaB.InverseInertiaTensor, jacobiScaleB, out inertiaB.InverseInertiaTensor); - inertiaB.InverseMass *= jacobiScaleB; - Symmetric3x3Wide.Scale(inertiaC.InverseInertiaTensor, jacobiScaleC, out inertiaC.InverseInertiaTensor); - inertiaC.InverseMass *= jacobiScaleC; - Symmetric3x3Wide.Scale(inertiaD.InverseInertiaTensor, jacobiScaleD, out inertiaD.InverseInertiaTensor); - inertiaD.InverseMass *= jacobiScaleD; - - function.WarmStart2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, positionD, orientationD, inertiaD, - ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC, ref wsvD); - //Jacobi batches do not scatter velocities directly; they are handled in a postpass. - } - } - - public override void JacobiSolveStep2(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackBatch jacobiBatch, ref JacobiFallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) - { - var prestepBundles = typeBatch.PrestepData.As(); - var bodyReferencesBundles = typeBatch.BodyReferences.As(); - var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); - Unsafe.SkipInit(out TConstraintFunctions function); - ref var motionStates = ref bodies.ActiveSet.SolverStates; - //EarlyPrefetch(SolvePrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, startBundle, exclusiveEndBundle); - ref var jacobiResultsBundlesA = ref jacobiResults.GetVelocitiesForBody(0); - ref var jacobiResultsBundlesB = ref jacobiResults.GetVelocitiesForBody(1); - ref var jacobiResultsBundlesC = ref jacobiResults.GetVelocitiesForBody(2); - ref var jacobiResultsBundlesD = ref jacobiResults.GetVelocitiesForBody(3); - for (int i = startBundle; i < exclusiveEndBundle; ++i) - { - ref var prestep = ref prestepBundles[i]; - ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; - ref var references = ref bodyReferencesBundles[i]; - var count = GetCountInBundle(ref typeBatch, i); - ref var wsvA = ref jacobiResultsBundlesA[i]; - ref var wsvB = ref jacobiResultsBundlesB[i]; - ref var wsvC = ref jacobiResultsBundlesC[i]; - ref var wsvD = ref jacobiResultsBundlesD[i]; - //Prefetch(SolvePrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, i, exclusiveEndBundle); - //Note jacobi batches do not do access filtering at the moment. The fallback accumulation expects all velocities to be present. - bodies.GatherState(ref references.IndexA, count, true, out var positionA, out var orientationA, out wsvA, out var inertiaA); - bodies.GatherState(ref references.IndexB, count, true, out var positionB, out var orientationB, out wsvB, out var inertiaB); - bodies.GatherState(ref references.IndexC, count, true, out var positionC, out var orientationC, out wsvC, out var inertiaC); - bodies.GatherState(ref references.IndexD, count, true, out var positionD, out var orientationD, out wsvD, out var inertiaD); - jacobiBatch.GetJacobiScaleForBodies(ref references, count, out var jacobiScaleA, out var jacobiScaleB, out var jacobiScaleC, out var jacobiScaleD); - Symmetric3x3Wide.Scale(inertiaA.InverseInertiaTensor, jacobiScaleA, out inertiaA.InverseInertiaTensor); - inertiaA.InverseMass *= jacobiScaleA; - Symmetric3x3Wide.Scale(inertiaB.InverseInertiaTensor, jacobiScaleB, out inertiaB.InverseInertiaTensor); - inertiaB.InverseMass *= jacobiScaleB; - Symmetric3x3Wide.Scale(inertiaC.InverseInertiaTensor, jacobiScaleC, out inertiaC.InverseInertiaTensor); - inertiaC.InverseMass *= jacobiScaleC; - Symmetric3x3Wide.Scale(inertiaD.InverseInertiaTensor, jacobiScaleD, out inertiaD.InverseInertiaTensor); - inertiaD.InverseMass *= jacobiScaleD; - - function.Solve2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, positionD, orientationD, inertiaD, - dt, inverseDt, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC, ref wsvD); - //Jacobi batches do not scatter velocities directly; they are handled in a postpass. - } - } } } diff --git a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs index c3f209294..8d103b767 100644 --- a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs @@ -154,62 +154,6 @@ public unsafe override void SolveIteration(ref TypeBatch typeBatch, Bodies bodie } } - public unsafe override void JacobiPrestep(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackBatch jacobiBatch, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) - { - ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); - ref var bodyReferencesBase = ref Unsafe.AsRef>(typeBatch.BodyReferences.Memory); - ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); - var function = default(TConstraintFunctions); - for (int i = startBundle; i < exclusiveEndBundle; ++i) - { - ref var prestep = ref Unsafe.Add(ref prestepBase, i); - ref var projection = ref Unsafe.Add(ref projectionBase, i); - ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); - var count = GetCountInBundle(ref typeBatch, i); - bodies.GatherState(ref references, count, out var position, out var orientation, out _, out var inertia); - //Jacobi batches split affected bodies into multiple pieces to guarantee convergence. - jacobiBatch.GetJacobiScaleForBodies(ref references, count, out var jacobiScale); - Symmetric3x3Wide.Scale(inertia.InverseInertiaTensor, jacobiScale, out inertia.InverseInertiaTensor); - inertia.InverseMass *= jacobiScale; - function.Prestep(position, orientation, inertia, dt, inverseDt, ref prestep, out projection); - } - } - public unsafe override void JacobiWarmStart(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle) - { - ref var bodyReferencesBase = ref Unsafe.AsRef>(typeBatch.BodyReferences.Memory); - ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); - ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); - var function = default(TConstraintFunctions); - ref var jacobiResultsBundlesA = ref jacobiResults.GetVelocitiesForBody(0); - for (int i = startBundle; i < exclusiveEndBundle; ++i) - { - ref var projection = ref Unsafe.Add(ref projectionBase, i); - ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); - ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); - int count = GetCountInBundle(ref typeBatch, i); - ref var wsvA = ref jacobiResultsBundlesA[i]; - bodies.GatherState(ref bodyReferences, count, out _, out _, out wsvA, out _); - function.WarmStart(ref wsvA, ref projection, ref accumulatedImpulses); - } - } - public unsafe override void JacobiSolveIteration(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle) - { - ref var bodyReferencesBase = ref Unsafe.AsRef>(typeBatch.BodyReferences.Memory); - ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); - ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); - var function = default(TConstraintFunctions); - ref var jacobiResultsBundlesA = ref jacobiResults.GetVelocitiesForBody(0); - for (int i = startBundle; i < exclusiveEndBundle; ++i) - { - ref var projection = ref Unsafe.Add(ref projectionBase, i); - ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); - ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); - int count = GetCountInBundle(ref typeBatch, i); - ref var wsvA = ref jacobiResultsBundlesA[i]; - bodies.GatherState(ref bodyReferences, count, out _, out _, out wsvA, out _); - function.Solve(ref wsvA, ref projection, ref accumulatedImpulses); - } - } public unsafe override void WarmStart2( ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, in TBundleCountCalculator bundleCountCalculator, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) @@ -266,61 +210,6 @@ public unsafe override void SolveStep2(ref TypeBatch typ bodies.ScatterVelocities(ref wsvA, ref references, count); } } - - public override void JacobiWarmStart2(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackBatch jacobiBatch, ref JacobiFallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) - { - var prestepBundles = typeBatch.PrestepData.As(); - var bodyReferencesBundles = typeBatch.BodyReferences.As>(); - var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); - Unsafe.SkipInit(out TConstraintFunctions function); - ref var states = ref bodies.ActiveSet.SolverStates; - ref var jacobiResultsBundlesA = ref jacobiResults.GetVelocitiesForBody(0); - for (int i = startBundle; i < exclusiveEndBundle; ++i) - { - ref var prestep = ref prestepBundles[i]; - ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; - ref var references = ref bodyReferencesBundles[i]; - ref var wsvA = ref jacobiResultsBundlesA[i]; - var count = GetCountInBundle(ref typeBatch, i); - //Prefetch(WarmStartPrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref states, i, exclusiveEndBundle); - //Note jacobi batches do not do access filtering at the moment. The fallback accumulation expects all velocities to be present. - //Also, jacobi batches cannot integrate. - bodies.GatherState(ref references, count, true, out var positionA, out var orientationA, out wsvA, out var inertiaA); - jacobiBatch.GetJacobiScaleForBodies(ref references, count, out var jacobiScaleA); - Symmetric3x3Wide.Scale(inertiaA.InverseInertiaTensor, jacobiScaleA, out inertiaA.InverseInertiaTensor); - inertiaA.InverseMass *= jacobiScaleA; - function.WarmStart2(positionA, orientationA, inertiaA, ref prestep, ref accumulatedImpulses, ref wsvA); - //Jacobi batches do not scatter velocities directly; they are handled in a postpass. - } - } - - public override void JacobiSolveStep2(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackBatch jacobiBatch, ref JacobiFallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) - { - var prestepBundles = typeBatch.PrestepData.As(); - var bodyReferencesBundles = typeBatch.BodyReferences.As>(); - var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); - Unsafe.SkipInit(out TConstraintFunctions function); - ref var motionStates = ref bodies.ActiveSet.SolverStates; - //EarlyPrefetch(SolvePrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, startBundle, exclusiveEndBundle); - ref var jacobiResultsBundlesA = ref jacobiResults.GetVelocitiesForBody(0); - for (int i = startBundle; i < exclusiveEndBundle; ++i) - { - ref var prestep = ref prestepBundles[i]; - ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; - ref var references = ref bodyReferencesBundles[i]; - var count = GetCountInBundle(ref typeBatch, i); - ref var wsvA = ref jacobiResultsBundlesA[i]; - //Prefetch(SolvePrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, i, exclusiveEndBundle); - //Note jacobi batches do not do access filtering at the moment. The fallback accumulation expects all velocities to be present. - bodies.GatherState(ref references, count, true, out var positionA, out var orientationA, out wsvA, out var inertiaA); - jacobiBatch.GetJacobiScaleForBodies(ref references, count, out var jacobiScaleA); - Symmetric3x3Wide.Scale(inertiaA.InverseInertiaTensor, jacobiScaleA, out inertiaA.InverseInertiaTensor); - inertiaA.InverseMass *= jacobiScaleA; - - function.Solve2(positionA, orientationA, inertiaA, dt, inverseDt, ref prestep, ref accumulatedImpulses, ref wsvA); - //Jacobi batches do not scatter velocities directly; they are handled in a postpass. - } - } } public abstract class OneBodyContactTypeProcessor diff --git a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs index 3e8b36311..bf333e7bd 100644 --- a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs @@ -180,84 +180,6 @@ public unsafe override void SolveIteration(ref TypeBatch typeBatch, Bodies bodie bodies.ScatterVelocities(ref wsvC, ref bodyReferences.IndexC, count); } } - - public unsafe override void JacobiPrestep(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackBatch jacobiBatch, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) - { - ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); - ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); - ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); - var function = default(TConstraintFunctions); - for (int i = startBundle; i < exclusiveEndBundle; ++i) - { - ref var prestep = ref Unsafe.Add(ref prestepBase, i); - ref var projection = ref Unsafe.Add(ref projectionBase, i); - ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); - var count = GetCountInBundle(ref typeBatch, i); - bodies.GatherState(ref references, count, - out var orientationA, out var wsvA, out var inertiaA, - out var ab, out var orientationB, out var wsvB, out var inertiaB, - out var ac, out var orientationC, out var wsvC, out var inertiaC); - //Jacobi batches split affected bodies into multiple pieces to guarantee convergence. - jacobiBatch.GetJacobiScaleForBodies(ref references, count, out var jacobiScaleA, out var jacobiScaleB, out var jacobiScaleC); - Symmetric3x3Wide.Scale(inertiaA.InverseInertiaTensor, jacobiScaleA, out inertiaA.InverseInertiaTensor); - inertiaA.InverseMass *= jacobiScaleA; - Symmetric3x3Wide.Scale(inertiaB.InverseInertiaTensor, jacobiScaleB, out inertiaB.InverseInertiaTensor); - inertiaB.InverseMass *= jacobiScaleB; - Symmetric3x3Wide.Scale(inertiaC.InverseInertiaTensor, jacobiScaleC, out inertiaC.InverseInertiaTensor); - inertiaC.InverseMass *= jacobiScaleC; - function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, ac, orientationC, inertiaC, dt, inverseDt, ref prestep, out projection); - } - } - public unsafe override void JacobiWarmStart(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle) - { - ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); - ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); - ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); - var function = default(TConstraintFunctions); - ref var jacobiResultsBundlesA = ref jacobiResults.GetVelocitiesForBody(0); - ref var jacobiResultsBundlesB = ref jacobiResults.GetVelocitiesForBody(1); - ref var jacobiResultsBundlesC = ref jacobiResults.GetVelocitiesForBody(2); - for (int i = startBundle; i < exclusiveEndBundle; ++i) - { - ref var projection = ref Unsafe.Add(ref projectionBase, i); - ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); - ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); - int count = GetCountInBundle(ref typeBatch, i); - ref var wsvA = ref jacobiResultsBundlesA[i]; - ref var wsvB = ref jacobiResultsBundlesB[i]; - ref var wsvC = ref jacobiResultsBundlesC[i]; - bodies.GatherState(ref bodyReferences, count, - out var orientationA, out wsvA, out var inertiaA, - out var ab, out var orientationB, out wsvB, out var inertiaB, - out var ac, out var orientationC, out wsvC, out var inertiaC); - function.WarmStart(ref wsvA, ref wsvB, ref wsvC, ref projection, ref accumulatedImpulses); - } - } - public unsafe override void JacobiSolveIteration(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle) - { - ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); - ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); - ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); - var function = default(TConstraintFunctions); - ref var jacobiResultsBundlesA = ref jacobiResults.GetVelocitiesForBody(0); - ref var jacobiResultsBundlesB = ref jacobiResults.GetVelocitiesForBody(1); - ref var jacobiResultsBundlesC = ref jacobiResults.GetVelocitiesForBody(2); - for (int i = startBundle; i < exclusiveEndBundle; ++i) - { - ref var projection = ref Unsafe.Add(ref projectionBase, i); - ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); - ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); - int count = GetCountInBundle(ref typeBatch, i); - ref var wsvA = ref jacobiResultsBundlesA[i]; - ref var wsvB = ref jacobiResultsBundlesB[i]; - ref var wsvC = ref jacobiResultsBundlesC[i]; - bodies.GatherState(ref bodyReferences, count, - out var orientationA, out wsvA, out var inertiaA, - out var ab, out var orientationB, out wsvB, out var inertiaB, - out var ac, out var orientationC, out wsvC, out var inertiaC); - function.Solve(ref wsvA, ref wsvB, ref wsvC, ref projection, ref accumulatedImpulses); - } - } public unsafe override void WarmStart2( ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, in TBundleCountCalculator bundleCountCalculator, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) @@ -327,83 +249,5 @@ public unsafe override void SolveStep2(ref TypeBatch typ bodies.ScatterVelocities(ref wsvC, ref references.IndexC, count); } } - - - public override void JacobiWarmStart2(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackBatch jacobiBatch, ref JacobiFallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) - { - var prestepBundles = typeBatch.PrestepData.As(); - var bodyReferencesBundles = typeBatch.BodyReferences.As(); - var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); - Unsafe.SkipInit(out TConstraintFunctions function); - ref var states = ref bodies.ActiveSet.SolverStates; - //EarlyPrefetch(WarmStartPrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref states, startBundle, exclusiveEndBundle); - ref var jacobiResultsBundlesA = ref jacobiResults.GetVelocitiesForBody(0); - ref var jacobiResultsBundlesB = ref jacobiResults.GetVelocitiesForBody(1); - ref var jacobiResultsBundlesC = ref jacobiResults.GetVelocitiesForBody(2); - for (int i = startBundle; i < exclusiveEndBundle; ++i) - { - ref var prestep = ref prestepBundles[i]; - ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; - ref var references = ref bodyReferencesBundles[i]; - ref var wsvA = ref jacobiResultsBundlesA[i]; - ref var wsvB = ref jacobiResultsBundlesB[i]; - ref var wsvC = ref jacobiResultsBundlesC[i]; - var count = GetCountInBundle(ref typeBatch, i); - //Prefetch(WarmStartPrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref states, i, exclusiveEndBundle); - //Note jacobi batches do not do access filtering at the moment. The fallback accumulation expects all velocities to be present. - //Also, jacobi batches cannot integrate. - bodies.GatherState(ref references.IndexA, count, true, out var positionA, out var orientationA, out wsvA, out var inertiaA); - bodies.GatherState(ref references.IndexB, count, true, out var positionB, out var orientationB, out wsvB, out var inertiaB); - bodies.GatherState(ref references.IndexC, count, true, out var positionC, out var orientationC, out wsvC, out var inertiaC); - jacobiBatch.GetJacobiScaleForBodies(ref references, count, out var jacobiScaleA, out var jacobiScaleB, out var jacobiScaleC); - Symmetric3x3Wide.Scale(inertiaA.InverseInertiaTensor, jacobiScaleA, out inertiaA.InverseInertiaTensor); - inertiaA.InverseMass *= jacobiScaleA; - Symmetric3x3Wide.Scale(inertiaB.InverseInertiaTensor, jacobiScaleB, out inertiaB.InverseInertiaTensor); - inertiaB.InverseMass *= jacobiScaleB; - Symmetric3x3Wide.Scale(inertiaC.InverseInertiaTensor, jacobiScaleC, out inertiaC.InverseInertiaTensor); - inertiaC.InverseMass *= jacobiScaleC; - - function.WarmStart2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC); - //Jacobi batches do not scatter velocities directly; they are handled in a postpass. - } - } - - public override void JacobiSolveStep2(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackBatch jacobiBatch, ref JacobiFallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) - { - var prestepBundles = typeBatch.PrestepData.As(); - var bodyReferencesBundles = typeBatch.BodyReferences.As(); - var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); - Unsafe.SkipInit(out TConstraintFunctions function); - ref var motionStates = ref bodies.ActiveSet.SolverStates; - //EarlyPrefetch(SolvePrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, startBundle, exclusiveEndBundle); - ref var jacobiResultsBundlesA = ref jacobiResults.GetVelocitiesForBody(0); - ref var jacobiResultsBundlesB = ref jacobiResults.GetVelocitiesForBody(1); - ref var jacobiResultsBundlesC = ref jacobiResults.GetVelocitiesForBody(2); - for (int i = startBundle; i < exclusiveEndBundle; ++i) - { - ref var prestep = ref prestepBundles[i]; - ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; - ref var references = ref bodyReferencesBundles[i]; - var count = GetCountInBundle(ref typeBatch, i); - ref var wsvA = ref jacobiResultsBundlesA[i]; - ref var wsvB = ref jacobiResultsBundlesB[i]; - ref var wsvC = ref jacobiResultsBundlesC[i]; - //Prefetch(SolvePrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, i, exclusiveEndBundle); - //Note jacobi batches do not do access filtering at the moment. The fallback accumulation expects all velocities to be present. - bodies.GatherState(ref references.IndexA, count, true, out var positionA, out var orientationA, out wsvA, out var inertiaA); - bodies.GatherState(ref references.IndexB, count, true, out var positionB, out var orientationB, out wsvB, out var inertiaB); - bodies.GatherState(ref references.IndexC, count, true, out var positionC, out var orientationC, out wsvC, out var inertiaC); - jacobiBatch.GetJacobiScaleForBodies(ref references, count, out var jacobiScaleA, out var jacobiScaleB, out var jacobiScaleC); - Symmetric3x3Wide.Scale(inertiaA.InverseInertiaTensor, jacobiScaleA, out inertiaA.InverseInertiaTensor); - inertiaA.InverseMass *= jacobiScaleA; - Symmetric3x3Wide.Scale(inertiaB.InverseInertiaTensor, jacobiScaleB, out inertiaB.InverseInertiaTensor); - inertiaB.InverseMass *= jacobiScaleB; - Symmetric3x3Wide.Scale(inertiaC.InverseInertiaTensor, jacobiScaleC, out inertiaC.InverseInertiaTensor); - inertiaC.InverseMass *= jacobiScaleC; - - function.Solve2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, dt, inverseDt, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC); - //Jacobi batches do not scatter velocities directly; they are handled in a postpass. - } - } } } diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index d2260d5de..cd886dce8 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -200,69 +200,6 @@ public unsafe override void SolveIteration(ref TypeBatch typeBatch, Bodies bodie } } - public unsafe override void JacobiPrestep(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackBatch jacobiBatch, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) - { - ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); - ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); - ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); - var function = default(TConstraintFunctions); - for (int i = startBundle; i < exclusiveEndBundle; ++i) - { - ref var prestep = ref Unsafe.Add(ref prestepBase, i); - ref var projection = ref Unsafe.Add(ref projectionBase, i); - ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); - var count = GetCountInBundle(ref typeBatch, i); - bodies.GatherState(ref references, count, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB); - //Jacobi batches split affected bodies into multiple pieces to guarantee convergence. - jacobiBatch.GetJacobiScaleForBodies(ref references, count, out var jacobiScaleA, out var jacobiScaleB); - Symmetric3x3Wide.Scale(inertiaA.InverseInertiaTensor, jacobiScaleA, out inertiaA.InverseInertiaTensor); - inertiaA.InverseMass *= jacobiScaleA; - Symmetric3x3Wide.Scale(inertiaB.InverseInertiaTensor, jacobiScaleB, out inertiaB.InverseInertiaTensor); - inertiaB.InverseMass *= jacobiScaleB; - function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, dt, inverseDt, ref prestep, out projection); - } - } - public unsafe override void JacobiWarmStart(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle) - { - ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); - ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); - ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); - var function = default(TConstraintFunctions); - ref var jacobiResultsBundlesA = ref jacobiResults.GetVelocitiesForBody(0); - ref var jacobiResultsBundlesB = ref jacobiResults.GetVelocitiesForBody(1); - for (int i = startBundle; i < exclusiveEndBundle; ++i) - { - ref var projection = ref Unsafe.Add(ref projectionBase, i); - ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); - ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); - int count = GetCountInBundle(ref typeBatch, i); - ref var wsvA = ref jacobiResultsBundlesA[i]; - ref var wsvB = ref jacobiResultsBundlesB[i]; - bodies.GatherState(ref bodyReferences, count, out var orientationA, out wsvA, out var inertiaA, out var ab, out var orientationB, out wsvB, out var inertiaB); - function.WarmStart(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); - } - } - public unsafe override void JacobiSolveIteration(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle) - { - ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); - ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); - ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); - var function = default(TConstraintFunctions); - ref var jacobiResultsBundlesA = ref jacobiResults.GetVelocitiesForBody(0); - ref var jacobiResultsBundlesB = ref jacobiResults.GetVelocitiesForBody(1); - for (int i = startBundle; i < exclusiveEndBundle; ++i) - { - ref var projection = ref Unsafe.Add(ref projectionBase, i); - ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); - ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); - int count = GetCountInBundle(ref typeBatch, i); - ref var wsvA = ref jacobiResultsBundlesA[i]; - ref var wsvB = ref jacobiResultsBundlesB[i]; - bodies.GatherState(ref references, count, out var orientationA, out wsvA, out var inertiaA, out var ab, out var orientationB, out wsvB, out var inertiaB); - function.Solve(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); - } - } - //public const int WarmStartPrefetchDistance = 8; //public const int SolvePrefetchDistance = 4; //[MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -384,73 +321,6 @@ public unsafe override void SolveStep2(ref TypeBatch typ bodies.ScatterVelocities(ref wsvB, ref references.IndexB, count); } } - - public override void JacobiWarmStart2(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackBatch jacobiBatch, ref JacobiFallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) - { - var prestepBundles = typeBatch.PrestepData.As(); - var bodyReferencesBundles = typeBatch.BodyReferences.As(); - var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); - Unsafe.SkipInit(out TConstraintFunctions function); - ref var states = ref bodies.ActiveSet.SolverStates; - //EarlyPrefetch(WarmStartPrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref states, startBundle, exclusiveEndBundle); - ref var jacobiResultsBundlesA = ref jacobiResults.GetVelocitiesForBody(0); - ref var jacobiResultsBundlesB = ref jacobiResults.GetVelocitiesForBody(1); - for (int i = startBundle; i < exclusiveEndBundle; ++i) - { - ref var prestep = ref prestepBundles[i]; - ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; - ref var references = ref bodyReferencesBundles[i]; - ref var wsvA = ref jacobiResultsBundlesA[i]; - ref var wsvB = ref jacobiResultsBundlesB[i]; - var count = GetCountInBundle(ref typeBatch, i); - //Prefetch(WarmStartPrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref states, i, exclusiveEndBundle); - //Note jacobi batches do not do access filtering at the moment. The fallback accumulation expects all velocities to be present. - //Also, jacobi batches cannot integrate. - bodies.GatherState(ref references.IndexA, count, true, out var positionA, out var orientationA, out wsvA, out var inertiaA); - bodies.GatherState(ref references.IndexB, count, true, out var positionB, out var orientationB, out wsvB, out var inertiaB); - jacobiBatch.GetJacobiScaleForBodies(ref references, count, out var jacobiScaleA, out var jacobiScaleB); - Symmetric3x3Wide.Scale(inertiaA.InverseInertiaTensor, jacobiScaleA, out inertiaA.InverseInertiaTensor); - inertiaA.InverseMass *= jacobiScaleA; - Symmetric3x3Wide.Scale(inertiaB.InverseInertiaTensor, jacobiScaleB, out inertiaB.InverseInertiaTensor); - inertiaB.InverseMass *= jacobiScaleB; - - function.WarmStart2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB); - //Jacobi batches do not scatter velocities directly; they are handled in a postpass. - } - } - - public override void JacobiSolveStep2(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackBatch jacobiBatch, ref JacobiFallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) - { - var prestepBundles = typeBatch.PrestepData.As(); - var bodyReferencesBundles = typeBatch.BodyReferences.As(); - var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); - Unsafe.SkipInit(out TConstraintFunctions function); - ref var motionStates = ref bodies.ActiveSet.SolverStates; - //EarlyPrefetch(SolvePrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, startBundle, exclusiveEndBundle); - ref var jacobiResultsBundlesA = ref jacobiResults.GetVelocitiesForBody(0); - ref var jacobiResultsBundlesB = ref jacobiResults.GetVelocitiesForBody(1); - for (int i = startBundle; i < exclusiveEndBundle; ++i) - { - ref var prestep = ref prestepBundles[i]; - ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; - ref var references = ref bodyReferencesBundles[i]; - var count = GetCountInBundle(ref typeBatch, i); - ref var wsvA = ref jacobiResultsBundlesA[i]; - ref var wsvB = ref jacobiResultsBundlesB[i]; - //Prefetch(SolvePrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, i, exclusiveEndBundle); - //Note jacobi batches do not do access filtering at the moment. The fallback accumulation expects all velocities to be present. - bodies.GatherState(ref references.IndexA, count, true, out var positionA, out var orientationA, out wsvA, out var inertiaA); - bodies.GatherState(ref references.IndexB, count, true, out var positionB, out var orientationB, out wsvB, out var inertiaB); - jacobiBatch.GetJacobiScaleForBodies(ref references, count, out var jacobiScaleA, out var jacobiScaleB); - Symmetric3x3Wide.Scale(inertiaA.InverseInertiaTensor, jacobiScaleA, out inertiaA.InverseInertiaTensor); - inertiaA.InverseMass *= jacobiScaleA; - Symmetric3x3Wide.Scale(inertiaB.InverseInertiaTensor, jacobiScaleB, out inertiaB.InverseInertiaTensor); - inertiaB.InverseMass *= jacobiScaleB; - - function.Solve2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, dt, inverseDt, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB); - //Jacobi batches do not scatter velocities directly; they are handled in a postpass. - } - } } public abstract class TwoBodyContactTypeProcessor diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index 8325ced1b..1eca54345 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -153,10 +153,6 @@ internal unsafe abstract void CopySleepingToActive( public abstract void WarmStart(ref TypeBatch typeBatch, Bodies bodies, int startBundle, int exclusiveEndBundle); public abstract void SolveIteration(ref TypeBatch typeBatch, Bodies bodies, int startBundle, int exclusiveEndBundle); - public abstract void JacobiPrestep(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackBatch jacobiBatch, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); - public abstract void JacobiWarmStart(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle); - public abstract void JacobiSolveIteration(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle); - public abstract void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks poseIntegratorCallbacks, in TCountInBundleCalculator countInBundleCalculator, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) @@ -167,12 +163,6 @@ public abstract void WarmStart2(ref TypeBatch typeBatch, Bodies bodies, in TCountInBundleCalculator countInBundleCalculator, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) where TCountInBundleCalculator : unmanaged, IConstraintBundleCountCalculator; - public abstract void JacobiWarmStart2( - ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackBatch jacobiBatch, ref JacobiFallbackTypeBatchResults jacobiResults, - float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex); - - public abstract void JacobiSolveStep2(ref TypeBatch typeBatch, Bodies bodies, ref JacobiFallbackBatch jacobiBatch, ref JacobiFallbackTypeBatchResults jacobiResults, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); - public virtual void IncrementallyUpdateContactData(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int end) { Debug.Fail("A contact data update was scheduled for a type batch that does not have a contact data update implementation."); diff --git a/BepuPhysics/IslandAwakener.cs b/BepuPhysics/IslandAwakener.cs index 851dbb7e0..534beaeb2 100644 --- a/BepuPhysics/IslandAwakener.cs +++ b/BepuPhysics/IslandAwakener.cs @@ -245,21 +245,19 @@ internal unsafe void ExecutePhaseOneJob(int index) for (int i = 0; i < uniqueSetIndices.Count; ++i) { Debug.Assert(uniqueSetIndices[i] > 0); - ref var source = ref solver.Sets[uniqueSetIndices[i]].JacobiFallback; - ref var target = ref solver.ActiveSet.JacobiFallback; - if (source.bodyConstraintReferences.Count > 0) + ref var source = ref solver.Sets[uniqueSetIndices[i]].SequentialFallback; + ref var target = ref solver.ActiveSet.SequentialFallback; + if (source.bodyConstraintCounts.Count > 0) { - for (int j = 0; j < source.bodyConstraintReferences.Count; ++j) + for (int j = 0; j < source.bodyConstraintCounts.Count; ++j) { //Inactive sets refer to body handles. Active set refers to body indices. Make the transition. //The HandleToLocation was updated during job setup, so we can use it. - ref var bodyLocation = ref bodies.HandleToLocation[source.bodyConstraintReferences.Keys[j]]; + ref var bodyLocation = ref bodies.HandleToLocation[source.bodyConstraintCounts.Keys[j]]; Debug.Assert(bodyLocation.SetIndex == 0, "Any batch moved into the active set should be dealing with bodies which have already been moved into the active set."); - var added = target.bodyConstraintReferences.AddUnsafely(ref bodyLocation.Index, source.bodyConstraintReferences.Values[j]); + var added = target.bodyConstraintCounts.AddUnsafely(bodyLocation.Index, source.bodyConstraintCounts.Values[j]); Debug.Assert(added, "Any body moving from an inactive set to the active set should not already be present in the active set's fallback batch."); } - //We've reused the lists. Set the count to zero so they don't get disposed later. - source.bodyConstraintReferences.Count = 0; } } } @@ -493,7 +491,7 @@ unsafe internal (int phaseOneJobCount, int phaseTwoJobCount) PrepareJobs(ref Qui if (highestNewBatchCount < setBatchCount) highestNewBatchCount = setBatchCount; ref var constraintSet = ref solver.Sets[setIndex]; - additionalRequiredFallbackCapacity += constraintSet.JacobiFallback.BodyCount; + additionalRequiredFallbackCapacity += constraintSet.SequentialFallback.BodyCount; for (int batchIndex = 0; batchIndex < constraintSet.Batches.Count; ++batchIndex) { ref var batch = ref constraintSet.Batches[batchIndex]; @@ -558,7 +556,7 @@ void AccumulatePairCacheTypeCounts(ref Buffer sourceTypeCaches, r //constraints, solver.ActiveSet.Batches.EnsureCapacity(highestNewBatchCount, pool); if (additionalRequiredFallbackCapacity > 0) - solver.ActiveSet.JacobiFallback.EnsureCapacity(solver.ActiveSet.JacobiFallback.BodyCount + additionalRequiredFallbackCapacity, pool); + solver.ActiveSet.SequentialFallback.EnsureCapacity(solver.ActiveSet.SequentialFallback.BodyCount + additionalRequiredFallbackCapacity, pool); Debug.Assert(highestNewBatchCount <= solver.FallbackBatchThreshold + 1, "Shouldn't have any batches beyond the fallback batch."); solver.batchReferencedHandles.EnsureCapacity(highestNewBatchCount, pool); for (int batchIndex = solver.ActiveSet.Batches.Count; batchIndex < highestNewBatchCount; ++batchIndex) diff --git a/BepuPhysics/IslandScaffold.cs b/BepuPhysics/IslandScaffold.cs index 3caeffa6e..ca4e642ef 100644 --- a/BepuPhysics/IslandScaffold.cs +++ b/BepuPhysics/IslandScaffold.cs @@ -76,7 +76,7 @@ internal void Validate(Solver solver) } } - public unsafe bool TryAdd(ConstraintHandle constraintHandle, int batchIndex, Solver solver, BufferPool pool, ref JacobiFallbackBatch fallbackBatch) + public unsafe bool TryAdd(ConstraintHandle constraintHandle, int batchIndex, Solver solver, BufferPool pool, ref SequentialFallbackBatch fallbackBatch) { ref var constraintLocation = ref solver.HandleToConstraint[constraintHandle.Value]; var typeProcessor = solver.TypeProcessors[constraintLocation.TypeId]; @@ -109,7 +109,7 @@ ref solver.ActiveSet.Batches[constraintLocation.BatchIndex].GetTypeBatch(constra { bodyHandles[i] = solver.bodies.ActiveSet.IndexToHandle[bodyIndices[i]]; } - fallbackBatch.AllocateForInactive(constraintHandle, bodyHandles, solver.bodies, constraintLocation.TypeId, pool); + fallbackBatch.AllocateForInactive(bodyHandles, solver.bodies, pool); } return true; } @@ -137,7 +137,7 @@ internal struct IslandScaffold { public QuickList BodyIndices; public QuickList Protobatches; - public JacobiFallbackBatch FallbackBatch; + public SequentialFallbackBatch FallbackBatch; public IslandScaffold(ref QuickList bodyIndices, ref QuickList constraintHandles, Solver solver, BufferPool pool) : this() { diff --git a/BepuPhysics/IslandSleeper.cs b/BepuPhysics/IslandSleeper.cs index 2596f3daa..dc08a7d53 100644 --- a/BepuPhysics/IslandSleeper.cs +++ b/BepuPhysics/IslandSleeper.cs @@ -779,7 +779,7 @@ void DisposeWorkerTraversalResults() //Pull the fallback batch data into the new fallback batch. Note that this isn't just a shallow copy; we're pushing all the allocations into the main pool. //They were previously on a thread-specific pool. (This isn't technically required right now, but it's cheap and convenient if we change the per thread pools //to make use of the usually-ephemeral nature of their allocations.) - JacobiFallbackBatch.CreateFrom(ref island.FallbackBatch, pool, out constraintSet.JacobiFallback); + SequentialFallbackBatch.CreateFrom(ref island.FallbackBatch, pool, out constraintSet.SequentialFallback); } } } diff --git a/BepuPhysics/JacobiFallbackBatch.cs b/BepuPhysics/JacobiFallbackBatch.cs deleted file mode 100644 index bdf77eda8..000000000 --- a/BepuPhysics/JacobiFallbackBatch.cs +++ /dev/null @@ -1,528 +0,0 @@ -using BepuPhysics.Constraints; -using BepuUtilities; -using BepuUtilities.Collections; -using BepuUtilities.Memory; -using System; -using System.Collections.Generic; -using System.Diagnostics; -using System.Numerics; -using System.Runtime.CompilerServices; -using System.Text; - -namespace BepuPhysics -{ - public struct JacobiFallbackTypeBatchResults - { - public Buffer> BodyVelocities; - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Buffer GetVelocitiesForBody(int slotIndex) - { - return ref BodyVelocities[slotIndex]; - } - } - - /// - /// Contains constraints that could not belong to any lower constraint batch due to their involved bodies. All of the contained constraints will be solved using a fallback solver that - /// trades rigidity for parallelism. - /// - public struct JacobiFallbackBatch - { - public struct FallbackReference - { - public ConstraintHandle ConstraintHandle; - public int IndexInConstraint; - public override string ToString() - { - return $"{ConstraintHandle}, {IndexInConstraint}"; - } - } - /// - /// Gets the number of bodies in the fallback batch. - /// - public readonly int BodyCount { get { return bodyConstraintReferences.Count; } } - - //Every body in the fallback batch must track what constraints are associated with it. These tables must be maintained as constraints are added and removed. - //Note that this dictionary contains active set body *indices* while active, but body *handles* when associated with an inactive set. - //This is consistent with the body references stored by active/inactive constraints. - //Note that this is a dictionary of *sets*. This is because fallback batches are expected to be used in pathological cases where there are many constraints associated with - //a single body. There are likely to be too many constraints for list-based containment/removal to be faster than the set implementation. - internal QuickDictionary, PrimitiveComparer> bodyConstraintReferences; - - internal struct FallbackReferenceComparer : IEqualityComparerRef - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool Equals(ref FallbackReference a, ref FallbackReference b) - { - return a.ConstraintHandle.Value == b.ConstraintHandle.Value; - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public int Hash(ref FallbackReference item) - { - return item.ConstraintHandle.Value; - } - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - unsafe void Allocate(ConstraintHandle constraintHandle, Span constraintBodyHandles, Bodies bodies, - int typeId, BufferPool pool, TBodyReferenceGetter bodyReferenceGetter, int minimumBodyCapacity, int minimumReferenceCapacity) - where TBodyReferenceGetter : struct, IBodyReferenceGetter - { - EnsureCapacity(Math.Max(bodyConstraintReferences.Count + constraintBodyHandles.Length, minimumBodyCapacity), pool); - for (int i = 0; i < constraintBodyHandles.Length; ++i) - { - var bodyReference = bodyReferenceGetter.GetBodyReference(bodies, constraintBodyHandles[i]); - - var bodyAlreadyListed = bodyConstraintReferences.GetTableIndices(ref bodyReference, out var tableIndex, out var elementIndex); - //If an entry for this body does not yet exist, we'll create one. - if (!bodyAlreadyListed) - elementIndex = bodyConstraintReferences.Count; - ref var constraintReferences = ref bodyConstraintReferences.Values[elementIndex]; - - if (!bodyAlreadyListed) - { - //The body is not already contained. Create a list for it. - constraintReferences = new QuickSet(minimumReferenceCapacity, pool); - bodyConstraintReferences.Keys[elementIndex] = bodyReference; - bodyConstraintReferences.Table[tableIndex] = elementIndex + 1; - ++bodyConstraintReferences.Count; - } - var fallbackReference = new FallbackReference { ConstraintHandle = constraintHandle, IndexInConstraint = i }; - constraintReferences.Add(ref fallbackReference, pool); - } - } - - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - internal unsafe void AllocateForActive(ConstraintHandle handle, Span constraintBodyHandles, Bodies bodies, - int typeId, BufferPool pool, int minimumBodyCapacity = 8, int minimumReferenceCapacity = 8) - { - Allocate(handle, constraintBodyHandles, bodies, typeId, pool, new ActiveSetGetter(), minimumBodyCapacity, minimumReferenceCapacity); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - internal void AllocateForInactive(ConstraintHandle handle, Span constraintBodyHandles, Bodies bodies, - int typeId, BufferPool pool, int minimumBodyCapacity = 8, int minimumReferenceCapacity = 8) - { - Allocate(handle, constraintBodyHandles, bodies, typeId, pool, new InactiveSetGetter(), minimumBodyCapacity, minimumReferenceCapacity); - } - - - /// - /// Removes a constraint from a body in the fallback batch. - /// - /// Body associated with a constraint in the fallback batch. - /// Constraint associated with the body being removed. - /// Allocations that should be freed once execution is back in a safe context. - /// True if the body no longer has any constraints associated with it in the fallback batch, false otherwise. - internal unsafe bool Remove(int bodyReference, ConstraintHandle constraintHandle, ref QuickList allocationIdsToFree) - { - var bodyPresent = bodyConstraintReferences.GetTableIndices(ref bodyReference, out var tableIndex, out var bodyReferencesIndex); - Debug.Assert(bodyPresent, "If we've been asked to remove a constraint associated with a body, that body must be in this batch."); - ref var constraintReferences = ref bodyConstraintReferences.Values[bodyReferencesIndex]; - var removed = constraintReferences.FastRemove(new FallbackReference { ConstraintHandle = constraintHandle }); - Debug.Assert(removed, "If a constraint removal was requested, it must exist within the referenced body's constraint set."); - if (constraintReferences.Count == 0) - { - //If there are no more constraints associated with this body, get rid of the body list. - allocationIdsToFree.AllocateUnsafely() = constraintReferences.Span.Id; - allocationIdsToFree.AllocateUnsafely() = constraintReferences.Table.Id; - constraintReferences = default; - bodyConstraintReferences.FastRemove(tableIndex, bodyReferencesIndex); - if (bodyConstraintReferences.Count == 0) - { - //No constraints remain in the fallback batch. Drop the dictionary. - allocationIdsToFree.AllocateUnsafely() = bodyConstraintReferences.Keys.Id; - allocationIdsToFree.AllocateUnsafely() = bodyConstraintReferences.Values.Id; - allocationIdsToFree.AllocateUnsafely() = bodyConstraintReferences.Table.Id; - bodyConstraintReferences = default; - } - return true; - } - return false; - } - - /// - /// Removes a body from the fallback batch if it is present. - /// - /// Reference to the body to remove from the fallback batch. - /// Allocations that should be freed once execution is back in a safe context. - /// True if the body was present in the fallback batch and was removed, false otherwise. - internal unsafe bool TryRemove(int bodyReference, ref QuickList allocationIdsToFree) - { - if (bodyConstraintReferences.Keys.Allocated && bodyConstraintReferences.GetTableIndices(ref bodyReference, out var tableIndex, out var bodyReferencesIndex)) - { - ref var constraintReferences = ref bodyConstraintReferences.Values[bodyReferencesIndex]; - //If there are no more constraints associated with this body, get rid of the body list. - allocationIdsToFree.AllocateUnsafely() = constraintReferences.Span.Id; - allocationIdsToFree.AllocateUnsafely() = constraintReferences.Table.Id; - bodyConstraintReferences.FastRemove(tableIndex, bodyReferencesIndex); - if (bodyConstraintReferences.Count == 0) - { - //No constraints remain in the fallback batch. Drop the dictionary. - allocationIdsToFree.AllocateUnsafely() = bodyConstraintReferences.Keys.Id; - allocationIdsToFree.AllocateUnsafely() = bodyConstraintReferences.Values.Id; - allocationIdsToFree.AllocateUnsafely() = bodyConstraintReferences.Table.Id; - bodyConstraintReferences = default; - } - return true; - } - return false; - } - - - internal unsafe void Remove(Solver solver, BufferPool bufferPool, ref ConstraintBatch batch, ConstraintHandle constraintHandle, ref IndexSet fallbackBatchHandles, int typeId, int indexInTypeBatch) - { - var typeProcessor = solver.TypeProcessors[typeId]; - var bodyCount = typeProcessor.BodiesPerConstraint; - var bodyIndices = stackalloc int[bodyCount]; - var enumerator = new ReferenceCollector(bodyIndices); - solver.EnumerateConnectedBodies(constraintHandle, ref enumerator); - var maximumAllocationIdsToFree = 3 + bodyCount * 2; - var allocationIdsToRemoveMemory = stackalloc int[maximumAllocationIdsToFree]; - var initialSpan = new Buffer(allocationIdsToRemoveMemory, maximumAllocationIdsToFree); - var allocationIdsToFree = new QuickList(initialSpan); - typeProcessor.EnumerateConnectedBodyIndices(ref batch.TypeBatches[batch.TypeIndexToTypeBatchIndex[typeId]], indexInTypeBatch, ref enumerator); - for (int i = 0; i < bodyCount; ++i) - { - if (Remove(bodyIndices[i], constraintHandle, ref allocationIdsToFree)) - { - fallbackBatchHandles.Remove(solver.bodies.ActiveSet.IndexToHandle[bodyIndices[i]].Value); - } - } - for (int i = 0; i < allocationIdsToFree.Count; ++i) - { - bufferPool.ReturnUnsafely(allocationIdsToFree[i]); - } - } - - - public static void AllocateResults(Solver solver, BufferPool pool, ref ConstraintBatch batch, out Buffer results) - { - pool.TakeAtLeast(batch.TypeBatches.Count, out results); - for (int i = 0; i < batch.TypeBatches.Count; ++i) - { - ref var typeBatch = ref batch.TypeBatches[i]; - var bodiesPerConstraint = solver.TypeProcessors[typeBatch.TypeId].BodiesPerConstraint; - ref var typeBatchResults = ref results[i]; - pool.TakeAtLeast(bodiesPerConstraint, out typeBatchResults.BodyVelocities); - for (int j = 0; j < bodiesPerConstraint; ++j) - { - pool.TakeAtLeast(typeBatch.BundleCount, out typeBatchResults.GetVelocitiesForBody(j)); - } - } - } - - public static void DisposeResults(Solver solver, BufferPool pool, ref ConstraintBatch batch, ref Buffer results) - { - for (int i = 0; i < batch.TypeBatches.Count; ++i) - { - var bodiesPerConstraint = solver.TypeProcessors[batch.TypeBatches[i].TypeId].BodiesPerConstraint; - ref var typeBatchResults = ref results[i]; - for (int j = 0; j < bodiesPerConstraint; ++j) - { - pool.ReturnUnsafely(typeBatchResults.GetVelocitiesForBody(j).Id); - } - pool.ReturnUnsafely(typeBatchResults.BodyVelocities.Id); - } - pool.Return(ref results); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void GetJacobiScaleForBodies(ref Vector references, int count, out Vector jacobiScale) - { - ref var start = ref Unsafe.As, int>(ref references); - Unsafe.SkipInit(out Vector counts); - ref var countsStart = ref Unsafe.As, int>(ref counts); - for (int i = 0; i < count; ++i) - { - var index = bodyConstraintReferences.IndexOf(ref Unsafe.Add(ref start, i)); - Debug.Assert(index >= 0, "If a prestep is looking up constraint counts associated with a body, it better be in the jacobi batch!"); - Unsafe.Add(ref countsStart, i) = bodyConstraintReferences.Values[index].Count; - } - jacobiScale = Vector.ConvertToSingle(counts); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void GetJacobiScaleForBodies(ref TwoBodyReferences references, int count, out Vector jacobiScaleA, out Vector jacobiScaleB) - { - ref var startA = ref Unsafe.As, int>(ref references.IndexA); - ref var startB = ref Unsafe.As, int>(ref references.IndexB); - Unsafe.SkipInit(out Vector countsA); - Unsafe.SkipInit(out Vector countsB); - ref var countsAStart = ref Unsafe.As, int>(ref countsA); - ref var countsBStart = ref Unsafe.As, int>(ref countsB); - for (int i = 0; i < count; ++i) - { - var indexA = bodyConstraintReferences.IndexOf(ref Unsafe.Add(ref startA, i)); - var indexB = bodyConstraintReferences.IndexOf(ref Unsafe.Add(ref startB, i)); - Debug.Assert(indexA >= 0 && indexB >= 0, "If a prestep is looking up constraint counts associated with a body, it better be in the jacobi batch!"); - Unsafe.Add(ref countsAStart, i) = bodyConstraintReferences.Values[indexA].Count; - Unsafe.Add(ref countsBStart, i) = bodyConstraintReferences.Values[indexB].Count; - } - jacobiScaleA = Vector.ConvertToSingle(countsA); - jacobiScaleB = Vector.ConvertToSingle(countsB); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void GetJacobiScaleForBodies(ref ThreeBodyReferences references, int count, - out Vector jacobiScaleA, out Vector jacobiScaleB, out Vector jacobiScaleC) - { - ref var startA = ref Unsafe.As, int>(ref references.IndexA); - ref var startB = ref Unsafe.As, int>(ref references.IndexB); - ref var startC = ref Unsafe.As, int>(ref references.IndexC); - Unsafe.SkipInit(out Vector countsA); - Unsafe.SkipInit(out Vector countsB); - Unsafe.SkipInit(out Vector countsC); - ref var countsAStart = ref Unsafe.As, int>(ref countsA); - ref var countsBStart = ref Unsafe.As, int>(ref countsB); - ref var countsCStart = ref Unsafe.As, int>(ref countsC); - for (int i = 0; i < count; ++i) - { - var indexA = bodyConstraintReferences.IndexOf(ref Unsafe.Add(ref startA, i)); - var indexB = bodyConstraintReferences.IndexOf(ref Unsafe.Add(ref startB, i)); - var indexC = bodyConstraintReferences.IndexOf(ref Unsafe.Add(ref startC, i)); - Debug.Assert(indexA >= 0 && indexB >= 0, "If a prestep is looking up constraint counts associated with a body, it better be in the jacobi batch!"); - Unsafe.Add(ref countsAStart, i) = bodyConstraintReferences.Values[indexA].Count; - Unsafe.Add(ref countsBStart, i) = bodyConstraintReferences.Values[indexB].Count; - Unsafe.Add(ref countsCStart, i) = bodyConstraintReferences.Values[indexC].Count; - } - jacobiScaleA = Vector.ConvertToSingle(countsA); - jacobiScaleB = Vector.ConvertToSingle(countsB); - jacobiScaleC = Vector.ConvertToSingle(countsC); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void GetJacobiScaleForBodies(ref FourBodyReferences references, int count, - out Vector jacobiScaleA, out Vector jacobiScaleB, out Vector jacobiScaleC, out Vector jacobiScaleD) - { - ref var startA = ref Unsafe.As, int>(ref references.IndexA); - ref var startB = ref Unsafe.As, int>(ref references.IndexB); - ref var startC = ref Unsafe.As, int>(ref references.IndexC); - ref var startD = ref Unsafe.As, int>(ref references.IndexD); - Unsafe.SkipInit(out Vector countsA); - Unsafe.SkipInit(out Vector countsB); - Unsafe.SkipInit(out Vector countsC); - Unsafe.SkipInit(out Vector countsD); - ref var countsAStart = ref Unsafe.As, int>(ref countsA); - ref var countsBStart = ref Unsafe.As, int>(ref countsB); - ref var countsCStart = ref Unsafe.As, int>(ref countsC); - ref var countsDStart = ref Unsafe.As, int>(ref countsD); - for (int i = 0; i < count; ++i) - { - var indexA = bodyConstraintReferences.IndexOf(ref Unsafe.Add(ref startA, i)); - var indexB = bodyConstraintReferences.IndexOf(ref Unsafe.Add(ref startB, i)); - var indexC = bodyConstraintReferences.IndexOf(ref Unsafe.Add(ref startC, i)); - var indexD = bodyConstraintReferences.IndexOf(ref Unsafe.Add(ref startD, i)); - Debug.Assert(indexA >= 0 && indexB >= 0, "If a prestep is looking up constraint counts associated with a body, it better be in the jacobi batch!"); - Unsafe.Add(ref countsAStart, i) = bodyConstraintReferences.Values[indexA].Count; - Unsafe.Add(ref countsBStart, i) = bodyConstraintReferences.Values[indexB].Count; - Unsafe.Add(ref countsCStart, i) = bodyConstraintReferences.Values[indexC].Count; - Unsafe.Add(ref countsDStart, i) = bodyConstraintReferences.Values[indexD].Count; - } - jacobiScaleA = Vector.ConvertToSingle(countsA); - jacobiScaleB = Vector.ConvertToSingle(countsB); - jacobiScaleC = Vector.ConvertToSingle(countsC); - jacobiScaleD = Vector.ConvertToSingle(countsD); - } - - [Conditional("DEBUG")] - unsafe static void ValidateBodyConstraintReference(Solver solver, int setIndex, int bodyReference, ConstraintHandle constraintHandle, int expectedIndexInConstraint) - { - ref var constraintLocation = ref solver.HandleToConstraint[constraintHandle.Value]; - Debug.Assert(constraintLocation.SetIndex == setIndex); - Debug.Assert(constraintLocation.BatchIndex == solver.FallbackBatchThreshold, "Should only be working on constraints which are members of the active fallback batch."); - var debugReferences = stackalloc int[solver.TypeProcessors[constraintLocation.TypeId].BodiesPerConstraint]; - var debugBodyReferenceCollector = new ReferenceCollector(debugReferences); - solver.EnumerateConnectedBodies(constraintHandle, ref debugBodyReferenceCollector); - Debug.Assert(debugReferences[expectedIndexInConstraint] == bodyReference, "The constraint's true body references must agree with the fallback batch."); - } - [Conditional("DEBUG")] - public static unsafe void ValidateSetReferences(Solver solver, int setIndex) - { - ref var set = ref solver.Sets[setIndex]; - Debug.Assert(set.Allocated); - if (set.Batches.Count > solver.FallbackBatchThreshold) - { - Debug.Assert(set.JacobiFallback.bodyConstraintReferences.Keys.Allocated); - ref var bodyConstraintReferences = ref set.JacobiFallback.bodyConstraintReferences; - for (int i = 0; i < bodyConstraintReferences.Count; ++i) - { - //This is a handle on inactive sets, and an index for active sets. - var bodyReference = bodyConstraintReferences.Keys[i]; - ref var references = ref bodyConstraintReferences.Values[i]; - Debug.Assert(references.Count > 0, "If there exists a body reference set, it should be populated."); - for (int j = 0; j < references.Count; ++j) - { - ref var reference = ref references.Span[j]; - ValidateBodyConstraintReference(solver, setIndex, bodyReference, reference.ConstraintHandle, reference.IndexInConstraint); - } - } - ref var batch = ref set.Batches[solver.FallbackBatchThreshold]; - for (int typeBatchIndex = 0; typeBatchIndex < batch.TypeBatches.Count; ++typeBatchIndex) - { - ref var typeBatch = ref batch.TypeBatches[typeBatchIndex]; - var bodiesPerConstraint = solver.TypeProcessors[typeBatch.TypeId].BodiesPerConstraint; - var connectedBodies = stackalloc int[bodiesPerConstraint]; - for (int constraintIndex = 0; constraintIndex < typeBatch.ConstraintCount; ++constraintIndex) - { - var constraintHandle = typeBatch.IndexToHandle[constraintIndex]; - var collector = new ReferenceCollector(connectedBodies); - solver.EnumerateConnectedBodies(constraintHandle, ref collector); - for (int i = 0; i < bodiesPerConstraint; ++i) - { - var localBodyIndex = bodyConstraintReferences.IndexOf(connectedBodies[i]); - Debug.Assert(localBodyIndex >= 0, "Any body referenced by a constraint in the fallback batch should exist within the fallback batch's body listing."); - ref var references = ref bodyConstraintReferences.Values[localBodyIndex]; - var constraintIndexInBodySet = references.IndexOf(new FallbackReference { ConstraintHandle = constraintHandle }); - Debug.Assert(constraintIndexInBodySet >= 0, "Any constraint in the fallback batch should be in all connected bodies' constraint handle listings."); - ref var reference = ref references[constraintIndexInBodySet]; - Debug.Assert(reference.ConstraintHandle.Value == constraintHandle.Value && reference.IndexInConstraint == i); - } - } - } - } - } - [Conditional("DEBUG")] - public static unsafe void ValidateReferences(Solver solver) - { - for (int i = 0; i < solver.Sets.Length; ++i) - { - if (solver.Sets[i].Allocated) - ValidateSetReferences(solver, i); - } - } - - public void ScatterVelocities(Bodies bodies, Solver solver, ref Buffer velocities, int start, int exclusiveEnd) - { - ref var fallbackBatch = ref solver.ActiveSet.Batches[solver.FallbackBatchThreshold]; - for (int i = start; i < exclusiveEnd; ++i) - { - //Velocity scattering is only ever executed on the active set, so the body reference is always an index. - var bodyIndex = bodyConstraintReferences.Keys[i]; - BodyVelocity bodyVelocity = default; - ref var constraintReferences = ref bodyConstraintReferences.Values[i]; - for (int j = 0; j < constraintReferences.Count; ++j) - { - //TODO: This can't be optimally vectorized due to the inherent gathers involved, but you may be able to do much better in terms of wasted instructions - //using platform intrinsics (like many other places). The benefit of true gather instructions here is more than some other places since it's likely all in L3 cache - //(if it's a shared L3, anyway). - ref var reference = ref constraintReferences[j]; - ref var constraintLocation = ref solver.HandleToConstraint[reference.ConstraintHandle.Value]; - //ValidateReferences(solver, bodyIndex, reference.ConstraintHandle, reference.IndexInConstraint); - var typeBatchIndex = fallbackBatch.TypeIndexToTypeBatchIndex[constraintLocation.TypeId]; - ref var typeBatchVelocities = ref velocities[typeBatchIndex]; - BundleIndexing.GetBundleIndices(constraintLocation.IndexInTypeBatch, out var bundleIndex, out var innerIndex); - ref var bundle = ref typeBatchVelocities.BodyVelocities[reference.IndexInConstraint][bundleIndex]; - ref var offsetBundle = ref GatherScatter.GetOffsetInstance(ref bundle, innerIndex); - Vector3Wide.ReadFirst(offsetBundle.Linear, out var linear); - Vector3Wide.ReadFirst(offsetBundle.Angular, out var angular); - //bodyVelocity.Linear.Validate(); - //bodyVelocity.Angular.Validate(); - bodyVelocity.Linear += linear; - bodyVelocity.Angular += angular; - //bodyVelocity.Linear.Validate(); - //bodyVelocity.Angular.Validate(); - } - //This simply averages all velocity results from the iteration for the body. This is equivalent to PGS/SI in terms of convergence because it is mathematically equivalent - //to having a linear/angular 'weld' constraint between N separate bodies that happen to all be in the same spot, except each of them has 1/N as much mass as the original. - //In other words, each jacobi batch constraint computed: - //newVelocity = oldVelocity + impulse * (1 / (inertia / N)) = oldVelocity + impulse * N / inertia - //All constraints together give a sum: - //summedVelocity = (oldVelocity + impulse0 * N / inertia) + (oldVelocity + impulse1 * N / inertia) + (oldVelocity + impulse2 * N / inertia) + ... - //averageVelocity = summedVelocity / N = (oldVelocity + impulse0 * N / inertia) / N + (oldVelocity + impulse0 * N / inertia) / N + ... - //averageVelocity = (oldVelocity / N + impulse0 / inertia) + (oldVelocity / N + impulse0 / inertia) + ... - //averageVelocity = (oldVelocity / N + oldVelocity / N + ...) + impulse0 / inertia + impulse1 / inertia + ... - //averageVelocity = oldVelocity + (impulse0 + impulse1 + ... ) / inertia - //Which is exactly what we want. - var inverseCount = 1f / constraintReferences.Count; - //bodyVelocity.Linear.Validate(); - //bodyVelocity.Angular.Validate(); - bodyVelocity.Linear *= inverseCount; - bodyVelocity.Angular *= inverseCount; - //bodyVelocity.Linear.Validate(); - //bodyVelocity.Angular.Validate(); - bodies.ActiveSet.SolverStates[bodyIndex].Motion.Velocity = bodyVelocity; - } - } - - internal void UpdateForBodyMemoryMove(int originalBodyIndex, int newBodyLocation) - { - Debug.Assert(bodyConstraintReferences.Keys.Allocated && !bodyConstraintReferences.ContainsKey(newBodyLocation), "If a body is being moved, as opposed to swapped, then the target index should not be present."); - bodyConstraintReferences.GetTableIndices(ref originalBodyIndex, out var tableIndex, out var elementIndex); - var references = bodyConstraintReferences.Values[elementIndex]; - bodyConstraintReferences.FastRemove(tableIndex, elementIndex); - bodyConstraintReferences.AddUnsafely(ref newBodyLocation, references); - } - - internal void UpdateForBodyMemorySwap(int a, int b) - { - var indexA = bodyConstraintReferences.IndexOf(a); - var indexB = bodyConstraintReferences.IndexOf(b); - Debug.Assert(indexA >= 0 && indexB >= 0, "A swap requires that both indices are already present."); - Helpers.Swap(ref bodyConstraintReferences.Values[indexA], ref bodyConstraintReferences.Values[indexB]); - } - - internal static void CreateFrom(ref JacobiFallbackBatch sourceBatch, BufferPool pool, out JacobiFallbackBatch targetBatch) - { - //Copy over non-buffer state. This copies buffer references pointlessly, but that doesn't matter. - targetBatch.bodyConstraintReferences = sourceBatch.bodyConstraintReferences; - pool.TakeAtLeast(sourceBatch.bodyConstraintReferences.Count, out targetBatch.bodyConstraintReferences.Keys); - pool.TakeAtLeast(targetBatch.bodyConstraintReferences.Keys.Length, out targetBatch.bodyConstraintReferences.Values); - pool.TakeAtLeast(sourceBatch.bodyConstraintReferences.TableMask + 1, out targetBatch.bodyConstraintReferences.Table); - sourceBatch.bodyConstraintReferences.Keys.CopyTo(0, targetBatch.bodyConstraintReferences.Keys, 0, sourceBatch.bodyConstraintReferences.Count); - sourceBatch.bodyConstraintReferences.Values.CopyTo(0, targetBatch.bodyConstraintReferences.Values, 0, sourceBatch.bodyConstraintReferences.Count); - sourceBatch.bodyConstraintReferences.Table.CopyTo(0, targetBatch.bodyConstraintReferences.Table, 0, sourceBatch.bodyConstraintReferences.TableMask + 1); - - for (int i = 0; i < sourceBatch.bodyConstraintReferences.Count; ++i) - { - ref var source = ref sourceBatch.bodyConstraintReferences.Values[i]; - ref var target = ref targetBatch.bodyConstraintReferences.Values[i]; - target = source; - pool.TakeAtLeast(source.Count, out target.Span); - pool.TakeAtLeast(source.TableMask + 1, out target.Table); - source.Span.CopyTo(0, target.Span, 0, source.Count); - source.Table.CopyTo(0, target.Table, 0, source.TableMask + 1); - } - } - - internal void EnsureCapacity(int bodyCapacity, BufferPool pool) - { - if (bodyConstraintReferences.Keys.Allocated) - { - //This is conservative since there's no guarantee that we'll actually need to resize at all if these bodies are already present, but that's fine. - bodyConstraintReferences.EnsureCapacity(bodyCapacity, pool); - } - else - { - bodyConstraintReferences = new QuickDictionary, PrimitiveComparer>(bodyCapacity, pool); - } - - } - - public void Compact(BufferPool pool) - { - if (bodyConstraintReferences.Keys.Allocated) - { - bodyConstraintReferences.Compact(pool); - for (int i = 0; i < bodyConstraintReferences.Count; ++i) - { - bodyConstraintReferences.Values[i].Compact(pool); - } - } - } - - - public void Dispose(BufferPool pool) - { - if (bodyConstraintReferences.Keys.Allocated) - { - for (int i = 0; i < bodyConstraintReferences.Count; ++i) - { - bodyConstraintReferences.Values[i].Dispose(pool); - } - bodyConstraintReferences.Dispose(pool); - } - } - } -} diff --git a/BepuPhysics/SequentialFallbackBatch.cs b/BepuPhysics/SequentialFallbackBatch.cs index 8452ade57..a7f7ebb53 100644 --- a/BepuPhysics/SequentialFallbackBatch.cs +++ b/BepuPhysics/SequentialFallbackBatch.cs @@ -172,23 +172,6 @@ internal unsafe void Remove(Solver solver, BufferPool bufferPool, ref Constraint } } - - public static void AllocateResults(Solver solver, BufferPool pool, ref ConstraintBatch batch, out Buffer results) - { - pool.TakeAtLeast(batch.TypeBatches.Count, out results); - for (int i = 0; i < batch.TypeBatches.Count; ++i) - { - ref var typeBatch = ref batch.TypeBatches[i]; - var bodiesPerConstraint = solver.TypeProcessors[typeBatch.TypeId].BodiesPerConstraint; - ref var typeBatchResults = ref results[i]; - pool.TakeAtLeast(bodiesPerConstraint, out typeBatchResults.BodyVelocities); - for (int j = 0; j < bodiesPerConstraint; ++j) - { - pool.TakeAtLeast(typeBatch.BundleCount, out typeBatchResults.GetVelocitiesForBody(j)); - } - } - } - [Conditional("DEBUG")] unsafe static void ValidateBodyConstraintReference(Solver solver, int setIndex, int bodyReference, ConstraintHandle constraintHandle, int expectedIndexInConstraint) { @@ -263,27 +246,16 @@ internal void UpdateForBodyMemorySwap(int a, int b) Helpers.Swap(ref bodyConstraintCounts.Values[indexA], ref bodyConstraintCounts.Values[indexB]); } - internal static void CreateFrom(ref JacobiFallbackBatch sourceBatch, BufferPool pool, out JacobiFallbackBatch targetBatch) + internal static void CreateFrom(ref SequentialFallbackBatch sourceBatch, BufferPool pool, out SequentialFallbackBatch targetBatch) { //Copy over non-buffer state. This copies buffer references pointlessly, but that doesn't matter. - targetBatch.bodyConstraintReferences = sourceBatch.bodyConstraintReferences; - pool.TakeAtLeast(sourceBatch.bodyConstraintReferences.Count, out targetBatch.bodyConstraintReferences.Keys); - pool.TakeAtLeast(targetBatch.bodyConstraintReferences.Keys.Length, out targetBatch.bodyConstraintReferences.Values); - pool.TakeAtLeast(sourceBatch.bodyConstraintReferences.TableMask + 1, out targetBatch.bodyConstraintReferences.Table); - sourceBatch.bodyConstraintReferences.Keys.CopyTo(0, targetBatch.bodyConstraintReferences.Keys, 0, sourceBatch.bodyConstraintReferences.Count); - sourceBatch.bodyConstraintReferences.Values.CopyTo(0, targetBatch.bodyConstraintReferences.Values, 0, sourceBatch.bodyConstraintReferences.Count); - sourceBatch.bodyConstraintReferences.Table.CopyTo(0, targetBatch.bodyConstraintReferences.Table, 0, sourceBatch.bodyConstraintReferences.TableMask + 1); - - for (int i = 0; i < sourceBatch.bodyConstraintReferences.Count; ++i) - { - ref var source = ref sourceBatch.bodyConstraintReferences.Values[i]; - ref var target = ref targetBatch.bodyConstraintReferences.Values[i]; - target = source; - pool.TakeAtLeast(source.Count, out target.Span); - pool.TakeAtLeast(source.TableMask + 1, out target.Table); - source.Span.CopyTo(0, target.Span, 0, source.Count); - source.Table.CopyTo(0, target.Table, 0, source.TableMask + 1); - } + targetBatch.bodyConstraintCounts = sourceBatch.bodyConstraintCounts; + pool.TakeAtLeast(sourceBatch.bodyConstraintCounts.Count, out targetBatch.bodyConstraintCounts.Keys); + pool.TakeAtLeast(targetBatch.bodyConstraintCounts.Keys.Length, out targetBatch.bodyConstraintCounts.Values); + pool.TakeAtLeast(sourceBatch.bodyConstraintCounts.TableMask + 1, out targetBatch.bodyConstraintCounts.Table); + sourceBatch.bodyConstraintCounts.Keys.CopyTo(0, targetBatch.bodyConstraintCounts.Keys, 0, sourceBatch.bodyConstraintCounts.Count); + sourceBatch.bodyConstraintCounts.Values.CopyTo(0, targetBatch.bodyConstraintCounts.Values, 0, sourceBatch.bodyConstraintCounts.Count); + sourceBatch.bodyConstraintCounts.Table.CopyTo(0, targetBatch.bodyConstraintCounts.Table, 0, sourceBatch.bodyConstraintCounts.TableMask + 1); } internal void EnsureCapacity(int bodyCapacity, BufferPool pool) diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index ad2872f45..7ec462343 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -302,9 +302,9 @@ public unsafe void ValidateExistingHandles(bool activeOnly = false) else { //If this is the fallback batch, then the expected count may be more than 1. - var foundBody = ActiveSet.JacobiFallback.bodyConstraintReferences.TryGetValue(i, out var referencesForBody); + var foundBody = ActiveSet.SequentialFallback.bodyConstraintCounts.TryGetValue(i, out var constraintCountInFallbackBatchForBody); Debug.Assert(foundBody, "A body was in the fallback batch's referenced handles, so the fallback batch should have a reference for that body."); - expectedCount = foundBody ? referencesForBody.Count : 0; + expectedCount = foundBody ? constraintCountInFallbackBatchForBody : 0; } } else @@ -482,7 +482,7 @@ internal unsafe void AllocateInBatch(int targetBatchIndex, ConstraintHandle cons } if (targetBatchIndex == FallbackBatchThreshold) { - ActiveSet.JacobiFallback.AllocateForActive(constraintHandle, bodyHandles, bodies, typeId, pool); + ActiveSet.SequentialFallback.AllocateForActive(bodyHandles, bodies, pool); } } @@ -839,7 +839,7 @@ internal unsafe void RemoveFromBatch(ConstraintHandle constraintHandle, int batc if (batchIndex == FallbackBatchThreshold) { //Note that we have to remove from fallback first because it accesses the batch's information. - ActiveSet.JacobiFallback.Remove(this, pool, ref batch, constraintHandle, ref batchReferencedHandles[batchIndex], typeId, indexInTypeBatch); + ActiveSet.SequentialFallback.Remove(this, pool, ref batch, constraintHandle, ref batchReferencedHandles[batchIndex], typeId, indexInTypeBatch); } else { @@ -936,7 +936,7 @@ internal void UpdateForBodyMemoryMove(int originalBodyIndex, int newBodyLocation if (UpdateConstraintsForBodyMemoryMove(originalBodyIndex, newBodyLocation)) { //One of the moved constraints involved the fallback batch, so we need to update the fallback batch's body indices. - ActiveSet.JacobiFallback.UpdateForBodyMemoryMove(originalBodyIndex, newBodyLocation); + ActiveSet.SequentialFallback.UpdateForBodyMemoryMove(originalBodyIndex, newBodyLocation); } } @@ -951,15 +951,15 @@ internal void UpdateForBodyMemorySwap(int a, int b) var bInFallback = UpdateConstraintsForBodyMemoryMove(b, a); if (aInFallback && bInFallback) { - ActiveSet.JacobiFallback.UpdateForBodyMemorySwap(a, b); + ActiveSet.SequentialFallback.UpdateForBodyMemorySwap(a, b); } else if (aInFallback) { - ActiveSet.JacobiFallback.UpdateForBodyMemoryMove(a, b); + ActiveSet.SequentialFallback.UpdateForBodyMemoryMove(a, b); } else if (bInFallback) { - ActiveSet.JacobiFallback.UpdateForBodyMemoryMove(b, a); + ActiveSet.SequentialFallback.UpdateForBodyMemoryMove(b, a); } } diff --git a/BepuPhysics/Solver_Solve.cs b/BepuPhysics/Solver_Solve.cs index a905b179e..8c066f0bc 100644 --- a/BepuPhysics/Solver_Solve.cs +++ b/BepuPhysics/Solver_Solve.cs @@ -88,12 +88,6 @@ protected internal struct WorkBlock public int End; } - protected internal struct FallbackScatterWorkBlock - { - public int Start; - public int End; - } - protected interface ITypeBatchSolveFilter { bool AllowFallback { get; } @@ -118,25 +112,6 @@ public bool AllowType(int typeId) } } - void BuildFallbackScatterWorkBlocks(int targetBlocksPerBatch) - { - ref var activeSet = ref ActiveSet; - if (activeSet.Batches.Count > FallbackBatchThreshold) - { - //There is a fallback batch, so we need to create fallback work blocks for it. - var blockCount = Math.Min(targetBlocksPerBatch, ActiveSet.JacobiFallback.BodyCount); - context.FallbackBlocks.Blocks = new QuickList(blockCount, pool); - var baseBodiesPerBlock = activeSet.JacobiFallback.BodyCount / blockCount; - var remainder = activeSet.JacobiFallback.BodyCount - baseBodiesPerBlock * blockCount; - int previousEnd = 0; - for (int i = 0; i < blockCount; ++i) - { - var bodiesInBlock = i < remainder ? baseBodiesPerBlock + 1 : baseBodiesPerBlock; - context.FallbackBlocks.Blocks.AllocateUnsafely() = new FallbackScatterWorkBlock { Start = previousEnd, End = previousEnd += bodiesInBlock }; - } - } - } - protected unsafe void BuildWorkBlocks( BufferPool pool, int minimumBlockSizeInBundles, int maximumBlockSizeInBundles, int targetBlocksPerBatch, ref TTypeBatchFilter typeBatchFilter, out QuickList workBlocks, out Buffer batchBoundaries) where TTypeBatchFilter : ITypeBatchSolveFilter @@ -246,10 +221,8 @@ protected struct MultithreadingParameters public float Dt; public WorkBlocks ConstraintBlocks; public Buffer BatchBoundaries; - public WorkBlocks FallbackBlocks; public int WorkerCompletedCount; public int WorkerCount; - public Buffer FallbackResults; public WorkBlocks IncrementalUpdateBlocks; public Buffer IncrementalUpdateBatchBoundaries; @@ -353,12 +326,7 @@ public void Execute(Solver solver, int blockIndex, int workerIndex) ref var block = ref solver.context.ConstraintBlocks.Blocks[blockIndex]; ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; - //Prestep dynamically picks the path since it executes in parallel across all batches. - //WarmStart /Solve, in contrast, have to dispatch once per batch, so we can choose the codepath at the entrypoint. - if (block.BatchIndex < solver.FallbackBatchThreshold) - typeProcessor.Prestep(ref typeBatch, solver.bodies, Dt, InverseDt, block.StartBundle, block.End); - else - typeProcessor.JacobiPrestep(ref typeBatch, solver.bodies, ref solver.ActiveSet.JacobiFallback, Dt, InverseDt, block.StartBundle, block.End); + typeProcessor.Prestep(ref typeBatch, solver.bodies, Dt, InverseDt, block.StartBundle, block.End); } } struct WarmStartStageFunction : IStageFunction @@ -385,39 +353,6 @@ public void Execute(Solver solver, int blockIndex, int workerIndex) } } - struct WarmStartFallbackStageFunction : IStageFunction - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Execute(Solver solver, int blockIndex, int workerIndex) - { - ref var block = ref solver.context.ConstraintBlocks.Blocks[blockIndex]; - ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; - var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; - typeProcessor.JacobiWarmStart(ref typeBatch, solver.bodies, ref solver.context.FallbackResults[block.TypeBatchIndex], block.StartBundle, block.End); - - } - } - struct SolveFallbackStageFunction : IStageFunction - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Execute(Solver solver, int blockIndex, int workerIndex) - { - ref var block = ref solver.context.ConstraintBlocks.Blocks[blockIndex]; - ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; - var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; - typeProcessor.JacobiSolveIteration(ref typeBatch, solver.bodies, ref solver.context.FallbackResults[block.TypeBatchIndex], block.StartBundle, block.End); - } - } - - struct FallbackScatterStageFunction : IStageFunction - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Execute(Solver solver, int blockIndex, int workerIndex) - { - ref var block = ref solver.context.FallbackBlocks.Blocks[blockIndex]; - solver.ActiveSet.JacobiFallback.ScatterVelocities(solver.bodies, solver, ref solver.context.FallbackResults, block.Start, block.End); - } - } /// @@ -597,7 +532,6 @@ protected static int GetUniformlyDistributedStart(int workerIndex, int blockCoun void SolveWorker(int workerIndex) { int prestepStart = GetUniformlyDistributedStart(workerIndex, context.ConstraintBlocks.Blocks.Count, context.WorkerCount, 0); - int fallbackStart = GetUniformlyDistributedStart(workerIndex, context.FallbackBlocks.Blocks.Count, context.WorkerCount, 0); Buffer batchStarts; ref var activeSet = ref ActiveSet; unsafe @@ -643,23 +577,10 @@ void SolveWorker(int workerIndex) ExecuteStage(ref warmStartStage, ref context.ConstraintBlocks, ref bounds, ref boundsBackBuffer, workerIndex, batchOffset, context.BatchBoundaries[batchIndex], ref workerBatchStartCopy, ref syncStage, claimedState, unclaimedState); } - var fallbackScatterStage = new FallbackScatterStageFunction(); - if (fallbackExists) - { - var warmStartFallbackStage = new WarmStartFallbackStageFunction(); - var batchStart = FallbackBatchThreshold > 0 ? context.BatchBoundaries[FallbackBatchThreshold - 1] : 0; - //Don't use the warm start to guess at the solve iteration work distribution. - var workerBatchStartCopy = batchStarts[FallbackBatchThreshold]; - ExecuteStage(ref warmStartFallbackStage, ref context.ConstraintBlocks, ref bounds, ref boundsBackBuffer, workerIndex, batchStart, context.BatchBoundaries[FallbackBatchThreshold], - ref workerBatchStartCopy, ref syncStage, claimedState, unclaimedState); - ExecuteStage(ref fallbackScatterStage, ref context.FallbackBlocks, ref bounds, ref boundsBackBuffer, - workerIndex, 0, context.FallbackBlocks.Blocks.Count, ref fallbackStart, ref syncStage, unclaimedState, claimedState); //note claim state swap: fallback scatter claims have no prestep, so it's off by one cycle - } claimedState ^= 1; unclaimedState ^= 1; var solveStage = new SolveStageFunction(); - var solveFallbackStage = new SolveFallbackStageFunction(); for (int iterationIndex = 0; iterationIndex < iterationCount; ++iterationIndex) { for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) @@ -668,14 +589,6 @@ void SolveWorker(int workerIndex) ExecuteStage(ref solveStage, ref context.ConstraintBlocks, ref bounds, ref boundsBackBuffer, workerIndex, batchOffset, context.BatchBoundaries[batchIndex], ref batchStarts[batchIndex], ref syncStage, claimedState, unclaimedState); } - if (fallbackExists) - { - var batchOffset = FallbackBatchThreshold > 0 ? context.BatchBoundaries[FallbackBatchThreshold - 1] : 0; - ExecuteStage(ref solveFallbackStage, ref context.ConstraintBlocks, ref bounds, ref boundsBackBuffer, workerIndex, batchOffset, context.BatchBoundaries[FallbackBatchThreshold], - ref batchStarts[FallbackBatchThreshold], ref syncStage, claimedState, unclaimedState); - ExecuteStage(ref fallbackScatterStage, ref context.FallbackBlocks, ref bounds, ref boundsBackBuffer, - workerIndex, 0, context.FallbackBlocks.Blocks.Count, ref fallbackStart, ref syncStage, unclaimedState, claimedState); //note claim state swap: fallback scatter claims have no prestep, so it's off by one cycle - } claimedState ^= 1; unclaimedState ^= 1; } @@ -747,20 +660,12 @@ protected void ExecuteMultithreaded(float dt, IThreadDisp var incrementalFilter = new IncrementalContactDataUpdateFilter(); BuildWorkBlocks(pool, minimumBlockSizeInBundles, maximumBlockSizeInBundles, targetBlocksPerBatch, ref incrementalFilter, out context.IncrementalUpdateBlocks.Blocks, out context.IncrementalUpdateBatchBoundaries); } - if (filter.AllowFallback) - BuildFallbackScatterWorkBlocks(targetBlocksPerBatch); ValidateWorkBlocks(ref filter); //Note the clear; the block claims must be initialized to 0 so that the first worker stage knows that the data is available to claim. context.ConstraintBlocks.CreateClaims(pool); if (includeIncrementalUpdate) context.IncrementalUpdateBlocks.CreateClaims(pool); - if (filter.AllowFallback && ActiveSet.Batches.Count > FallbackBatchThreshold) - { - Debug.Assert(context.FallbackBlocks.Blocks.Count > 0); - JacobiFallbackBatch.AllocateResults(this, pool, ref ActiveSet.Batches[FallbackBatchThreshold], out context.FallbackResults); - context.FallbackBlocks.CreateClaims(pool); - } pool.Take(workerCount, out context.WorkerBoundsA); pool.Take(workerCount, out context.WorkerBoundsB); //The worker bounds front buffer should be initialized to avoid trash interval data from messing up the workstealing. @@ -781,11 +686,6 @@ protected void ExecuteMultithreaded(float dt, IThreadDisp context.ConstraintBlocks.Dispose(pool); if (includeIncrementalUpdate) context.IncrementalUpdateBlocks.Dispose(pool); - if (filter.AllowFallback && ActiveSet.Batches.Count > FallbackBatchThreshold) - { - JacobiFallbackBatch.DisposeResults(this, pool, ref ActiveSet.Batches[FallbackBatchThreshold], ref context.FallbackResults); - context.FallbackBlocks.Dispose(pool); - } pool.Return(ref context.BatchBoundaries); pool.Return(ref context.WorkerBoundsA); pool.Return(ref context.WorkerBoundsB); @@ -799,8 +699,8 @@ public void Solve(float dt, IThreadDispatcher threadDispatcher = null) { var inverseDt = 1f / dt; ref var activeSet = ref ActiveSet; - GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); - for (int i = 0; i < synchronizedBatchCount; ++i) + var batchCount = activeSet.Batches.Count; + for (int i = 0; i < batchCount; ++i) { ref var batch = ref activeSet.Batches[i]; for (int j = 0; j < batch.TypeBatches.Count; ++j) @@ -809,18 +709,7 @@ public void Solve(float dt, IThreadDispatcher threadDispatcher = null) TypeProcessors[typeBatch.TypeId].Prestep(ref typeBatch, bodies, dt, inverseDt, 0, typeBatch.BundleCount); } } - if (fallbackExists) - { - ref var batch = ref activeSet.Batches[FallbackBatchThreshold]; - for (int j = 0; j < batch.TypeBatches.Count; ++j) - { - ref var typeBatch = ref batch.TypeBatches[j]; - TypeProcessors[typeBatch.TypeId].JacobiPrestep(ref typeBatch, bodies, ref activeSet.JacobiFallback, dt, inverseDt, 0, typeBatch.BundleCount); - } - } - //TODO: May want to consider executing warmstart immediately following the prestep. Multithreading can't do that, so there could be some bitwise differences introduced. - //On the upside, it would make use of cached data. - for (int i = 0; i < synchronizedBatchCount; ++i) + for (int i = 0; i < batchCount; ++i) { ref var batch = ref activeSet.Batches[i]; for (int j = 0; j < batch.TypeBatches.Count; ++j) @@ -829,21 +718,9 @@ public void Solve(float dt, IThreadDispatcher threadDispatcher = null) TypeProcessors[typeBatch.TypeId].WarmStart(ref typeBatch, bodies, 0, typeBatch.BundleCount); } } - Buffer fallbackResults = default; - if (fallbackExists) - { - ref var batch = ref activeSet.Batches[FallbackBatchThreshold]; - JacobiFallbackBatch.AllocateResults(this, pool, ref batch, out fallbackResults); - for (int j = 0; j < batch.TypeBatches.Count; ++j) - { - ref var typeBatch = ref batch.TypeBatches[j]; - TypeProcessors[typeBatch.TypeId].JacobiWarmStart(ref typeBatch, bodies, ref fallbackResults[j], 0, typeBatch.BundleCount); - } - activeSet.JacobiFallback.ScatterVelocities(bodies, this, ref fallbackResults, 0, activeSet.JacobiFallback.BodyCount); - } for (int iterationIndex = 0; iterationIndex < iterationCount; ++iterationIndex) { - for (int i = 0; i < synchronizedBatchCount; ++i) + for (int i = 0; i < batchCount; ++i) { ref var batch = ref activeSet.Batches[i]; for (int j = 0; j < batch.TypeBatches.Count; ++j) @@ -852,20 +729,6 @@ public void Solve(float dt, IThreadDispatcher threadDispatcher = null) TypeProcessors[typeBatch.TypeId].SolveIteration(ref typeBatch, bodies, 0, typeBatch.BundleCount); } } - if (fallbackExists) - { - ref var batch = ref activeSet.Batches[FallbackBatchThreshold]; - for (int j = 0; j < batch.TypeBatches.Count; ++j) - { - ref var typeBatch = ref batch.TypeBatches[j]; - TypeProcessors[typeBatch.TypeId].JacobiSolveIteration(ref typeBatch, bodies, ref fallbackResults[j], 0, typeBatch.BundleCount); - } - activeSet.JacobiFallback.ScatterVelocities(bodies, this, ref fallbackResults, 0, activeSet.JacobiFallback.BodyCount); - } - } - if (fallbackExists) - { - JacobiFallbackBatch.DisposeResults(this, pool, ref activeSet.Batches[FallbackBatchThreshold], ref fallbackResults); } } else diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index 537cdbdd1..0f26d3e44 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -20,11 +20,7 @@ internal enum SolverStageType { IncrementalUpdate, WarmStart, - JacobiWarmStart, - JacobiWarmStartScatter, Solve, - JacobiSolve, - JacobiSolveScatter, } internal struct SolverSyncStage @@ -53,16 +49,12 @@ internal struct SubstepMultithreadingContext [FieldOffset(32)] public Buffer ConstraintBlocks; [FieldOffset(48)] - public Buffer FallbackBlocks; - [FieldOffset(64)] - public Buffer FallbackResults; - [FieldOffset(80)] public Buffer ConstraintBatchBoundaries; - [FieldOffset(96)] + [FieldOffset(64)] public float Dt; - [FieldOffset(100)] + [FieldOffset(68)] public float InverseDt; - [FieldOffset(104)] + [FieldOffset(72)] public int WorkerCount; @@ -168,21 +160,6 @@ public void Execute(Solver solver, int blockIndex, int workerIndex) } } - struct JacobiWarmStartStep2StageFunction : IStageFunction - { - public float Dt; - public float InverseDt; - public Solver solver; - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Execute(Solver solver, int blockIndex, int workerIndex) - { - ref var block = ref this.solver.substepContext.ConstraintBlocks[blockIndex]; - ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; - var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; - typeProcessor.JacobiWarmStart2(ref typeBatch, this.solver.bodies, ref this.solver.ActiveSet.JacobiFallback, ref this.solver.substepContext.FallbackResults[block.TypeBatchIndex], Dt, InverseDt, block.StartBundle, block.End, workerIndex); - } - } struct SolveStep2StageFunction : IStageFunction { @@ -200,36 +177,6 @@ public void Execute(Solver solver, int blockIndex, int workerIndex) } } - struct JacobiSolveStep2StageFunction : IStageFunction - { - public float Dt; - public float InverseDt; - public Solver solver; - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Execute(Solver solver, int blockIndex, int workerIndex) - { - ref var block = ref this.solver.substepContext.ConstraintBlocks[blockIndex]; - ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; - var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; - typeProcessor.JacobiSolveStep2(ref typeBatch, solver.bodies, ref this.solver.ActiveSet.JacobiFallback, ref this.solver.substepContext.FallbackResults[block.TypeBatchIndex], Dt, InverseDt, block.StartBundle, block.End); - } - } - - struct JacobiScatterStageFunction : IStageFunction - { - public float Dt; - public float InverseDt; - public Solver solver; - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Execute(Solver solver, int blockIndex, int workerIndex) - { - ref var block = ref this.solver.substepContext.FallbackBlocks[blockIndex]; - solver.ActiveSet.JacobiFallback.ScatterVelocities(solver.bodies, solver, ref this.solver.substepContext.FallbackResults, block.Start, block.End); - } - } - struct IncrementalUpdateStageFunction : IStageFunction { public float Dt; @@ -358,7 +305,6 @@ void SolveStep2Worker2(int workerIndex) //data will remain in some cache that's reasonably close to the core. int workerCount = substepContext.WorkerCount; var incrementalUpdateWorkerStart = GetUniformlyDistributedStart(workerIndex, substepContext.IncrementalUpdateBlocks.Length, workerCount, 0); - int jacobiScatterStart = GetUniformlyDistributedStart(workerIndex, substepContext.FallbackBlocks.Length, workerCount, 0); Buffer batchStarts; ref var activeSet = ref ActiveSet; unsafe @@ -389,36 +335,16 @@ void SolveStep2Worker2(int workerIndex) InverseDt = substepContext.InverseDt, solver = this }; - var jacobiWarmstartStage = new JacobiWarmStartStep2StageFunction - { - Dt = substepContext.Dt, - InverseDt = substepContext.InverseDt, - solver = this - }; var solveStage = new SolveStep2StageFunction { Dt = substepContext.Dt, InverseDt = substepContext.InverseDt, solver = this }; - var jacobiSolveStage = new JacobiSolveStep2StageFunction - { - Dt = substepContext.Dt, - InverseDt = substepContext.InverseDt, - solver = this - }; - var jacobiScatterStage = new JacobiScatterStageFunction - { - Dt = substepContext.Dt, - InverseDt = substepContext.InverseDt, - solver = this - }; //A thread is only allowed to claim a workblock if the claim index for that workblock matches the expected value- which is the claim index it would have from the last time it was executed. //Each thread calculates what that claim index would have been based on the current sync index by subtracting the expected number of sync indices elapsed since last execution. - var syncStagesPerWarmStartOrSolve = synchronizedBatchCount; - //If a fallback exists, then each warm start/solve will have a final fallback batch stage and then a scatter stage. - if (fallbackExists) syncStagesPerWarmStartOrSolve += 2; + var syncStagesPerWarmStartOrSolve = ActiveSet.Batches.Count; var baseStageCountInSubstep = syncStagesPerWarmStartOrSolve * (1 + IterationCount); //All warmstarts and solves, plus an incremental contact update. First substep doesn't do an incremental contact update, but that's fine, it'll end up expecting 0. var syncOffsetToPreviousSubstep = baseStageCountInSubstep + 1; @@ -437,28 +363,18 @@ void SolveStep2Worker2(int workerIndex) ExecuteMainStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStart, ref substepContext.Stages[0], syncOffsetToPreviousSubstep, ref syncIndex); } warmstartStage.SubstepIndex = substepIndex; - for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) + for (int batchIndex = 0; batchIndex < syncStagesPerWarmStartOrSolve; ++batchIndex) { ExecuteMainStage(ref warmstartStage, workerIndex, batchStarts[batchIndex], ref substepContext.Stages[batchIndex + 1], syncOffsetToPreviousClaimOnBatchForWarmStart, ref syncIndex); } - if (fallbackExists) - { - ExecuteMainStage(ref jacobiWarmstartStage, workerIndex, batchStarts[FallbackBatchThreshold], ref substepContext.Stages[FallbackBatchThreshold + 1], syncOffsetToPreviousClaimOnBatchForWarmStart, ref syncIndex); - ExecuteMainStage(ref jacobiScatterStage, workerIndex, jacobiScatterStart, ref substepContext.Stages[FallbackBatchThreshold + 2], syncOffsetToPreviousClaimOnBatchForWarmStart, ref syncIndex); - } for (int iterationIndex = 0; iterationIndex < IterationCount; ++iterationIndex) { - for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) + for (int batchIndex = 0; batchIndex < syncStagesPerWarmStartOrSolve; ++batchIndex) { //Note that this is using a 'different' stage by index than the worker thread if the iteration index > 1. //That's totally fine- the warmstart/iteration stages share the same claims buffers per batch. They're redundant for the sake of easier indexing. ExecuteMainStage(ref solveStage, workerIndex, batchStarts[batchIndex], ref substepContext.Stages[batchIndex + 1], syncOffsetToPreviousClaimOnBatchForSolve, ref syncIndex); } - if (fallbackExists) - { - ExecuteMainStage(ref jacobiSolveStage, workerIndex, batchStarts[FallbackBatchThreshold], ref substepContext.Stages[FallbackBatchThreshold + 1], syncOffsetToPreviousClaimOnBatchForSolve, ref syncIndex); - ExecuteMainStage(ref jacobiScatterStage, workerIndex, jacobiScatterStart, ref substepContext.Stages[FallbackBatchThreshold + 2], syncOffsetToPreviousClaimOnBatchForSolve, ref syncIndex); - } } } //All done; notify waiting threads to join. @@ -516,21 +432,9 @@ void SolveStep2Worker2(int workerIndex) warmstartStage.SubstepIndex = substepIndex; ExecuteWorkerStage(ref warmstartStage, workerIndex, batchStarts[stage.BatchIndex], stage.WorkBlockStartIndex, ref stage.Claims, syncOffsetToPreviousClaimOnBatchForWarmStart, syncIndex, ref substepContext.CompletedWorkBlockCount); break; - case SolverStageType.JacobiWarmStart: - ExecuteWorkerStage(ref jacobiWarmstartStage, workerIndex, batchStarts[stage.BatchIndex], stage.WorkBlockStartIndex, ref stage.Claims, syncOffsetToPreviousClaimOnBatchForWarmStart, syncIndex, ref substepContext.CompletedWorkBlockCount); - break; - case SolverStageType.JacobiWarmStartScatter: - ExecuteWorkerStage(ref jacobiScatterStage, workerIndex, jacobiScatterStart, stage.WorkBlockStartIndex, ref stage.Claims, syncOffsetToPreviousClaimOnBatchForWarmStart, syncIndex, ref substepContext.CompletedWorkBlockCount); - break; case SolverStageType.Solve: ExecuteWorkerStage(ref solveStage, workerIndex, batchStarts[stage.BatchIndex], stage.WorkBlockStartIndex, ref stage.Claims, syncOffsetToPreviousClaimOnBatchForSolve, syncIndex, ref substepContext.CompletedWorkBlockCount); break; - case SolverStageType.JacobiSolve: - ExecuteWorkerStage(ref jacobiSolveStage, workerIndex, batchStarts[stage.BatchIndex], stage.WorkBlockStartIndex, ref stage.Claims, syncOffsetToPreviousClaimOnBatchForSolve, syncIndex, ref substepContext.CompletedWorkBlockCount); - break; - case SolverStageType.JacobiSolveScatter: - ExecuteWorkerStage(ref jacobiScatterStage, workerIndex, jacobiScatterStart, stage.WorkBlockStartIndex, ref stage.Claims, syncOffsetToPreviousClaimOnBatchForSolve, syncIndex, ref substepContext.CompletedWorkBlockCount); - break; } latestCompletedSyncIndex = syncIndex; @@ -539,31 +443,6 @@ void SolveStep2Worker2(int workerIndex) } - void BuildFallbackScatterWorkBlocks(int targetBlocksPerBatch, out Buffer workBlocks) - { - ref var activeSet = ref ActiveSet; - if (activeSet.Batches.Count > FallbackBatchThreshold) - { - //There is a fallback batch, so we need to create fallback work blocks for it. - var blockCount = Math.Min(targetBlocksPerBatch, ActiveSet.JacobiFallback.BodyCount); - pool.Take(blockCount, out workBlocks); - var blocksList = new QuickList(workBlocks); - context.FallbackBlocks.Blocks = new QuickList(blockCount, pool); - var baseBodiesPerBlock = activeSet.JacobiFallback.BodyCount / blockCount; - var remainder = activeSet.JacobiFallback.BodyCount - baseBodiesPerBlock * blockCount; - int previousEnd = 0; - for (int i = 0; i < blockCount; ++i) - { - var bodiesInBlock = i < remainder ? baseBodiesPerBlock + 1 : baseBodiesPerBlock; - blocksList.AllocateUnsafely() = new FallbackScatterWorkBlock { Start = previousEnd, End = previousEnd += bodiesInBlock }; - } - workBlocks = workBlocks.Slice(blocksList.Count); - } - else - { - workBlocks = default; - } - } //Buffer> debugStageWorkBlocksCompleted; protected void ExecuteMultithreaded2(float dt, IThreadDispatcher threadDispatcher, Action workDelegate) @@ -588,22 +467,14 @@ protected void ExecuteMultithreaded2(float dt, IThreadDispatcher threadDispatche pool.Return(ref incrementalUpdateBatchBoundaries); //TODO: No need to create this in the first place. Doesn't really cost anything, but... substepContext.ConstraintBlocks = constraintBlocks.Span.Slice(constraintBlocks.Count); substepContext.IncrementalUpdateBlocks = incrementalBlocks.Span.Slice(incrementalBlocks.Count); - BuildFallbackScatterWorkBlocks(targetBlocksPerBatch, out substepContext.FallbackBlocks); //Not every batch will actually have work blocks associated with it; the batch compressor could be falling behind, which means older constraints could be at higher batches than they need to be, leaving gaps. //We don't want to include those empty batches as sync points in the solver. - GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); + var batchCount = ActiveSet.Batches.Count; substepContext.SyncIndex = 0; var totalConstraintBatchWorkBlockCount = substepContext.ConstraintBatchBoundaries.Length == 0 ? 0 : substepContext.ConstraintBatchBoundaries[^1]; var totalClaimCount = incrementalBlocks.Count + totalConstraintBatchWorkBlockCount; - var stagesPerIteration = synchronizedBatchCount; - if (fallbackExists) - { - Debug.Assert(substepContext.FallbackBlocks.Length > 0); - JacobiFallbackBatch.AllocateResults(this, pool, ref ActiveSet.Batches[FallbackBatchThreshold], out substepContext.FallbackResults); - totalClaimCount += substepContext.FallbackBlocks.Length; - stagesPerIteration += 2; //jacobi batch, plus scatter. - } + var stagesPerIteration = batchCount; pool.Take(1 + stagesPerIteration * (1 + IterationCount), out substepContext.Stages); //Claims will be monotonically increasing throughout execution. All should start at zero to match with the initial sync index. pool.Take(totalClaimCount, out var claims); @@ -614,7 +485,7 @@ protected void ExecuteMultithreaded2(float dt, IThreadDispatcher threadDispatche int targetStageIndex = 1; //Warm start. int claimStart = incrementalBlocks.Count; - for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) + for (int batchIndex = 0; batchIndex < batchCount; ++batchIndex) { var stageIndex = targetStageIndex++; var batchStart = batchIndex == 0 ? 0 : substepContext.ConstraintBatchBoundaries[batchIndex - 1]; @@ -622,24 +493,11 @@ protected void ExecuteMultithreaded2(float dt, IThreadDispatcher threadDispatche substepContext.Stages[stageIndex] = new(claims.Slice(claimStart, workBlocksInBatch), batchStart, SolverStageType.WarmStart, batchIndex); claimStart += workBlocksInBatch; } - if (fallbackExists) - { - //Jacobi warm start. - var stageIndex = targetStageIndex++; - var batchStart = FallbackBatchThreshold == 0 ? 0 : substepContext.ConstraintBatchBoundaries[FallbackBatchThreshold - 1]; - var workBlocksInBatch = substepContext.ConstraintBatchBoundaries[FallbackBatchThreshold] - batchStart; - substepContext.Stages[stageIndex] = new(claims.Slice(claimStart, workBlocksInBatch), batchStart, SolverStageType.JacobiWarmStart, FallbackBatchThreshold); - claimStart += workBlocksInBatch; - - //Jacobi warm start scatter. - stageIndex = targetStageIndex++; - substepContext.Stages[stageIndex] = new(claims.Slice(claimStart, substepContext.FallbackBlocks.Length), 0, SolverStageType.JacobiWarmStartScatter); - } for (int iterationIndex = 0; iterationIndex < IterationCount; ++iterationIndex) { //Solve. Note that we're reusing the same claims as were used in the warm start for these stages; the stages just tell the workers what kind of work to do. claimStart = incrementalBlocks.Count; - for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) + for (int batchIndex = 0; batchIndex < batchCount; ++batchIndex) { var stageIndex = targetStageIndex++; var batchStart = batchIndex == 0 ? 0 : substepContext.ConstraintBatchBoundaries[batchIndex - 1]; @@ -647,19 +505,6 @@ protected void ExecuteMultithreaded2(float dt, IThreadDispatcher threadDispatche substepContext.Stages[stageIndex] = new(claims.Slice(claimStart, workBlocksInBatch), batchStart, SolverStageType.Solve, batchIndex); claimStart += workBlocksInBatch; } - if (fallbackExists) - { - //Jacobi solve. - var stageIndex = targetStageIndex++; - var batchStart = FallbackBatchThreshold == 0 ? 0 : substepContext.ConstraintBatchBoundaries[FallbackBatchThreshold - 1]; - var workBlocksInBatch = substepContext.ConstraintBatchBoundaries[FallbackBatchThreshold] - batchStart; - substepContext.Stages[stageIndex] = new(claims.Slice(claimStart, workBlocksInBatch), batchStart, SolverStageType.JacobiSolve, FallbackBatchThreshold); - claimStart += workBlocksInBatch; - - //Jacobi solve scatter. - stageIndex = targetStageIndex++; - substepContext.Stages[stageIndex] = new(claims.Slice(claimStart, substepContext.FallbackBlocks.Length), 0, SolverStageType.JacobiSolveScatter); - } } //for (int i = 0; i < iterationCountPlusOne; ++i) @@ -751,11 +596,6 @@ protected void ExecuteMultithreaded2(float dt, IThreadDispatcher threadDispatche //pool.Return(ref debugStageWorkBlocksCompleted); //pool.Return(ref availableCountPerSync); - if (fallbackExists) - { - JacobiFallbackBatch.DisposeResults(this, pool, ref ActiveSet.Batches[FallbackBatchThreshold], ref substepContext.FallbackResults); - pool.Return(ref substepContext.FallbackBlocks); - } pool.Return(ref claims); pool.Return(ref substepContext.Stages); pool.Return(ref substepContext.ConstraintBatchBoundaries); @@ -1208,14 +1048,13 @@ public override void SolveStep2(float totalDt, IThreadDispatcher threadDispatche { var inverseDt = 1f / substepDt; ref var activeSet = ref ActiveSet; - GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); - + var batchCount = activeSet.Batches.Count; var incrementalUpdateFilter = default(IncrementalContactDataUpdateFilter); for (int substepIndex = 0; substepIndex < substepCount; ++substepIndex) { if (substepIndex > 0) { - for (int i = 0; i < ActiveSet.Batches.Count; ++i) + for (int i = 0; i < batchCount; ++i) { ref var batch = ref activeSet.Batches[i]; for (int j = 0; j < batch.TypeBatches.Count; ++j) @@ -1226,7 +1065,7 @@ public override void SolveStep2(float totalDt, IThreadDispatcher threadDispatche } } } - for (int i = 0; i < synchronizedBatchCount; ++i) + for (int i = 0; i < batchCount; ++i) { ref var batch = ref activeSet.Batches[i]; ref var integrationFlagsForBatch = ref integrationFlags[i]; @@ -1243,23 +1082,9 @@ public override void SolveStep2(float totalDt, IThreadDispatcher threadDispatche } } } - Buffer fallbackResults = default; - if (fallbackExists) - { - ref var batch = ref activeSet.Batches[FallbackBatchThreshold]; - JacobiFallbackBatch.AllocateResults(this, pool, ref batch, out fallbackResults); - - for (int j = 0; j < batch.TypeBatches.Count; ++j) - { - ref var typeBatch = ref batch.TypeBatches[j]; - TypeProcessors[typeBatch.TypeId].JacobiWarmStart2(ref typeBatch, bodies, ref ActiveSet.JacobiFallback, ref fallbackResults[j], substepDt, inverseDt, 0, typeBatch.BundleCount, 0); - } - activeSet.JacobiFallback.ScatterVelocities(bodies, this, ref fallbackResults, 0, activeSet.JacobiFallback.BodyCount); - } - for (int iterationIndex = 0; iterationIndex < IterationCount; ++iterationIndex) { - for (int i = 0; i < synchronizedBatchCount; ++i) + for (int i = 0; i < batchCount; ++i) { ref var batch = ref activeSet.Batches[i]; for (int j = 0; j < batch.TypeBatches.Count; ++j) @@ -1268,20 +1093,6 @@ public override void SolveStep2(float totalDt, IThreadDispatcher threadDispatche TypeProcessors[typeBatch.TypeId].SolveStep2(ref typeBatch, bodies, new DenseBundleCountCalculator(), substepDt, inverseDt, 0, typeBatch.BundleCount); } } - if (fallbackExists) - { - ref var batch = ref activeSet.Batches[FallbackBatchThreshold]; - for (int j = 0; j < batch.TypeBatches.Count; ++j) - { - ref var typeBatch = ref batch.TypeBatches[j]; - TypeProcessors[typeBatch.TypeId].JacobiSolveStep2(ref typeBatch, bodies, ref ActiveSet.JacobiFallback, ref fallbackResults[j], substepDt, inverseDt, 0, typeBatch.BundleCount); - } - activeSet.JacobiFallback.ScatterVelocities(bodies, this, ref fallbackResults, 0, activeSet.JacobiFallback.BodyCount); - } - } - if (fallbackExists) - { - JacobiFallbackBatch.DisposeResults(this, pool, ref activeSet.Batches[FallbackBatchThreshold], ref fallbackResults); } } } From eb0e047f05e88e6443a12ebc2f4ee86555d45d22 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 14 Oct 2021 18:34:31 -0500 Subject: [PATCH 206/947] Allocating a lane in a type batch now conditionally fills other slots with -1. --- BepuPhysics/Constraints/TypeProcessor.cs | 20 +++++++++++++++----- 1 file changed, 15 insertions(+), 5 deletions(-) diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index 1eca54345..8d7c284cb 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -220,17 +220,27 @@ public override unsafe void ScaleAccumulatedImpulses(ref TypeBatch typeBatch, fl [MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe static void AddBodyReferencesLane(ref TBodyReferences bundle, int innerIndex, int* bodyIndices) { + var strideInInts = Vector.Count; + //We assume that the body references struct is organized in memory like Bundle0, Inner0, ... BundleN, InnerN, Count + //Assuming contiguous storage, Count is then located at start + stride * BodyCount. + var bodyCount = Unsafe.SizeOf() / (strideInInts * sizeof(int)); + if (innerIndex == 0) + { + //This constraint is the first one in a new bundle; set all body references in the constraint to -1 to mean 'no constraint allocated'. + var negativeOne = new Vector(-1); + ref var bodyReferenceBundle = ref Unsafe.As>(ref bundle); + for (int i = 0; i < bodyCount; ++i) + { + Unsafe.Add(ref bodyReferenceBundle, i) = negativeOne; + } + } //The jit should be able to fold almost all of the size-related calculations and address fiddling. ref var start = ref Unsafe.As(ref bundle); ref var targetLane = ref Unsafe.Add(ref start, innerIndex); - var stride = Vector.Count; - //We assume that the body references struct is organized in memory like Bundle0, Inner0, ... BundleN, InnerN, Count - //Assuming contiguous storage, Count is then located at start + stride * BodyCount. - var bodyCount = Unsafe.SizeOf() / (stride * sizeof(int)); targetLane = *bodyIndices; for (int i = 1; i < bodyCount; ++i) { - Unsafe.Add(ref targetLane, i * stride) = bodyIndices[i]; + Unsafe.Add(ref targetLane, i * strideInInts) = bodyIndices[i]; } } From f8ae88dac8c953ffe80f050f1d3521c7eb5758d8 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 14 Oct 2021 18:47:18 -0500 Subject: [PATCH 207/947] Removing from a type batch now clears the last slot's reference indices to -1. --- BepuPhysics/Constraints/TypeProcessor.cs | 26 ++++++++++++++++++------ 1 file changed, 20 insertions(+), 6 deletions(-) diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index 8d7c284cb..1ac09734b 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -220,10 +220,8 @@ public override unsafe void ScaleAccumulatedImpulses(ref TypeBatch typeBatch, fl [MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe static void AddBodyReferencesLane(ref TBodyReferences bundle, int innerIndex, int* bodyIndices) { - var strideInInts = Vector.Count; - //We assume that the body references struct is organized in memory like Bundle0, Inner0, ... BundleN, InnerN, Count - //Assuming contiguous storage, Count is then located at start + stride * BodyCount. - var bodyCount = Unsafe.SizeOf() / (strideInInts * sizeof(int)); + //The jit should be able to fold almost all of the size-related calculations and address fiddling. + var bodyCount = Unsafe.SizeOf() / (Vector.Count * sizeof(int)); if (innerIndex == 0) { //This constraint is the first one in a new bundle; set all body references in the constraint to -1 to mean 'no constraint allocated'. @@ -234,13 +232,27 @@ public unsafe static void AddBodyReferencesLane(ref TBodyReferences bundle, int Unsafe.Add(ref bodyReferenceBundle, i) = negativeOne; } } - //The jit should be able to fold almost all of the size-related calculations and address fiddling. + //We assume that the body references struct is organized in memory like Bundle0, Inner0, ... BundleN, InnerN, Count + //Assuming contiguous storage, Count is then located at start + stride * BodyCount. ref var start = ref Unsafe.As(ref bundle); ref var targetLane = ref Unsafe.Add(ref start, innerIndex); targetLane = *bodyIndices; for (int i = 1; i < bodyCount; ++i) { - Unsafe.Add(ref targetLane, i * strideInInts) = bodyIndices[i]; + Unsafe.Add(ref targetLane, i * Vector.Count) = bodyIndices[i]; + } + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public unsafe static void RemoveBodyReferencesLane(ref TBodyReferences bundle, int innerIndex) + { + var bodyCount = Unsafe.SizeOf() / (Vector.Count * sizeof(int)); + ref var start = ref Unsafe.As(ref bundle); + ref var targetLane = ref Unsafe.Add(ref start, innerIndex); + targetLane = -1; + for (int i = 1; i < bodyCount; ++i) + { + Unsafe.Add(ref targetLane, i * Vector.Count) = -1; } } @@ -356,6 +368,8 @@ ref Unsafe.Add(ref bodyReferences, targetBundleIndex), ref Unsafe.Add(ref preste ref typeBatch.IndexToHandle[index], targetInnerIndex, index, ref handlesToConstraints); } + //Clear the now-empty last slot of the body references bundle. + RemoveBodyReferencesLane(ref Unsafe.Add(ref bodyReferences, sourceBundleIndex), sourceInnerIndex); } /// From 1368cfd265256e2c759f52525fcbefa99d86e419 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 14 Oct 2021 19:49:07 -0500 Subject: [PATCH 208/947] Trailing body reference validation. --- .../EmbeddedSubsteppingTimestepper2.cs | 1 + BepuPhysics/Solver.cs | 28 +++++++++++++++++++ 2 files changed, 29 insertions(+) diff --git a/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs b/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs index 8277eedab..c9838e2a3 100644 --- a/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs +++ b/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs @@ -57,6 +57,7 @@ public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDi Debug.Assert(SubstepCount >= 0, "Substep count should be positive."); + simulation.Solver.ValidateTrailingTypeBatchBodyReferences(); var constrainedBodySet = simulation.Solver.PrepareConstraintIntegrationResponsibilities(SubstepCount, threadDispatcher); simulation.Profiler.Start(simulation.Solver); simulation.Solver.SolveStep2(dt, threadDispatcher); diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index 7ec462343..5bd3100d4 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -7,6 +7,7 @@ using BepuPhysics.CollisionDetection; using BepuUtilities; using System.Runtime.InteropServices; +using System.Numerics; namespace BepuPhysics { @@ -267,6 +268,33 @@ private void ValidateBodyReference(int body, int expectedCount, ref ConstraintBa Debug.Assert(referencesToBody == expectedCount); } + [Conditional("DEBUG")] + public unsafe void ValidateTrailingTypeBatchBodyReferences() + { + ref var set = ref ActiveSet; + for (int batchIndex = 0; batchIndex < set.Batches.Count; ++batchIndex) + { + ref var batch = ref set.Batches[batchIndex]; + for (int typeBatchIndex = 0; typeBatchIndex < batch.TypeBatches.Count; ++typeBatchIndex) + { + ref var typeBatch = ref batch.TypeBatches[typeBatchIndex]; + var bodiesPerConstraint = TypeProcessors[typeBatch.TypeId].BodiesPerConstraint; + var expectedEmptyLanesInLastBundle = typeBatch.BundleCount * Vector.Count - typeBatch.ConstraintCount; + var firstEmptySlotIndex = Vector.Count - expectedEmptyLanesInLastBundle; + ref var lastBodyReferencesBundle = ref typeBatch.BodyReferences[(typeBatch.BundleCount - 1) * bodiesPerConstraint * Unsafe.SizeOf>()]; + for (int bodyIndexInConstraint = 0; bodyIndexInConstraint < bodiesPerConstraint; ++bodyIndexInConstraint) + { + ref var bodyBundleInConstraint = ref Unsafe.Add(ref Unsafe.As>(ref lastBodyReferencesBundle), bodyIndexInConstraint); + ref var bodiesInBundle = ref Unsafe.As, int>(ref bodyBundleInConstraint); + for (int i = firstEmptySlotIndex; i < Vector.Count; ++i) + { + Debug.Assert(Unsafe.Add(ref bodiesInBundle, i) == -1, "Any awake incomplete bundle should have its trailing values initialized to -1."); + } + } + } + } + } + [Conditional("DEBUG")] public unsafe void ValidateExistingHandles(bool activeOnly = false) { From 6f2185b690405a60276a2b88863a406edc50d834 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 15 Oct 2021 12:44:23 -0500 Subject: [PATCH 209/947] IslandAwakener now initializes new trailing body reference bundles to -1. --- BepuPhysics/IslandAwakener.cs | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/BepuPhysics/IslandAwakener.cs b/BepuPhysics/IslandAwakener.cs index 534beaeb2..740beea66 100644 --- a/BepuPhysics/IslandAwakener.cs +++ b/BepuPhysics/IslandAwakener.cs @@ -5,6 +5,7 @@ using System; using System.Collections.Generic; using System.Diagnostics; +using System.Numerics; using System.Runtime.CompilerServices; using System.Text; using System.Threading; @@ -611,10 +612,8 @@ void EnsurePairCacheTypeCapacities(ref TypeAllocationSizes cache phaseTwoJobs = new QuickList(32, pool); //Finally, create actual jobs. Note that this involves actually allocating space in the bodies set and in type batches for the workers to fill in. //(Pair caches are currently handled in a locally sequential way and do not require preallocation.) - phaseOneJobs.AllocateUnsafely() = new PhaseOneJob { Type = PhaseOneJobType.PairCache }; phaseOneJobs.AllocateUnsafely() = new PhaseOneJob { Type = PhaseOneJobType.MoveFallbackBatchBodies }; - //Don't create batch referenced handles update jobs for the fallback batch; it has no referenced handles! for (int batchIndex = 0; batchIndex < highestNewBatchCount; ++batchIndex) { phaseOneJobs.AllocateUnsafely() = new PhaseOneJob { Type = PhaseOneJobType.UpdateBatchReferencedHandles, BatchIndex = batchIndex }; @@ -693,7 +692,20 @@ void EnsurePairCacheTypeCapacities(ref TypeAllocationSizes cache job.SourceStart = previousSourceEnd; job.TargetStart = targetTypeBatch.ConstraintCount; previousSourceEnd += job.Count; + var oldBundleCount = targetTypeBatch.BundleCount; targetTypeBatch.ConstraintCount += job.Count; + if (targetTypeBatch.BundleCount != oldBundleCount) + { + //A new bundle was created; guarantee any trailing slots are set to -1. + //Since it's a whole new bundle that has not yet had any data set to it, we can safely just initialize the whole bundle's body references to -1. + var vectorCount = solver.TypeProcessors[job.TypeId].BodiesPerConstraint; + var bundleStart = (Vector*)(targetTypeBatch.BodyReferences.Memory + (targetTypeBatch.BundleCount - 1) * vectorCount * Unsafe.SizeOf>()); + var negativeOne = new Vector(-1); + for (int vectorIndex = 0; vectorIndex < vectorCount; ++vectorIndex) + { + bundleStart[vectorIndex] = negativeOne; + } + } } Debug.Assert(previousSourceEnd == sourceTypeBatch.ConstraintCount); Debug.Assert(targetTypeBatch.ConstraintCount <= targetTypeBatch.IndexToHandle.Length); From 6a0a0135e121dd036f47155d41d570bcad9388ca Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 15 Oct 2021 13:16:53 -0500 Subject: [PATCH 210/947] Bundle counts removed from solver execution time in favor of masking. --- BepuPhysics/Bodies_GatherScatter.cs | 214 +++++++----------- .../Constraints/FourBodyTypeProcessor.cs | 67 +++--- .../Constraints/OneBodyTypeProcessor.cs | 28 +-- .../Constraints/ThreeBodyTypeProcessor.cs | 53 ++--- .../Constraints/TwoBodyTypeProcessor.cs | 44 ++-- BepuPhysics/Constraints/TypeProcessor.cs | 40 ++-- BepuPhysics/PoseIntegrator.cs | 10 +- Demos/DemoCallbacks.cs | 2 +- Demos/Demos/PlanetDemo.cs | 2 +- Demos/Demos/SimpleSelfContainedDemo.cs | 2 +- Demos/SpecializedTests/GyroscopeTestDemo.cs | 2 +- 11 files changed, 198 insertions(+), 266 deletions(-) diff --git a/BepuPhysics/Bodies_GatherScatter.cs b/BepuPhysics/Bodies_GatherScatter.cs index 2e8a12728..05858d7d3 100644 --- a/BepuPhysics/Bodies_GatherScatter.cs +++ b/BepuPhysics/Bodies_GatherScatter.cs @@ -17,9 +17,9 @@ public partial class Bodies { [MethodImpl(MethodImplOptions.AggressiveInlining)] - private static void WriteGatherInertia(ref int bundleBaseBodyIndexInSet, int bodyIndexInBundle, ref Buffer states, ref BodyInertiaWide gatheredInertias) + private static void WriteGatherInertia(int index, int bodyIndexInBundle, ref Buffer states, ref BodyInertiaWide gatheredInertias) { - ref var source = ref states[Unsafe.Add(ref bundleBaseBodyIndexInSet, bodyIndexInBundle)].Inertia.World; + ref var source = ref states[index].Inertia.World; ref var targetSlot = ref GetOffsetInstance(ref gatheredInertias, bodyIndexInBundle); GetFirst(ref targetSlot.InverseInertiaTensor.XX) = source.InverseInertiaTensor.XX; GetFirst(ref targetSlot.InverseInertiaTensor.YX) = source.InverseInertiaTensor.YX; @@ -31,10 +31,10 @@ private static void WriteGatherInertia(ref int bundleBaseBodyIndexInSet, int bod } [MethodImpl(MethodImplOptions.AggressiveInlining)] - private static void WriteGatherMotionState(ref int bundleBaseBodyIndexInSet, int bodyIndexInBundle, ref Buffer states, + private static void WriteGatherMotionState(int index, int bodyIndexInBundle, ref Buffer states, ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocityWide velocity) { - ref var state = ref states[Unsafe.Add(ref bundleBaseBodyIndexInSet, bodyIndexInBundle)].Motion; + ref var state = ref states[index].Motion; Vector3Wide.WriteFirst(state.Pose.Position, ref GetOffsetInstance(ref position, bodyIndexInBundle)); QuaternionWide.WriteFirst(state.Pose.Orientation, ref GetOffsetInstance(ref orientation, bodyIndexInBundle)); Vector3Wide.WriteFirst(state.Velocity.Linear, ref GetOffsetInstance(ref velocity.Linear, bodyIndexInBundle)); @@ -45,13 +45,12 @@ private static void WriteGatherMotionState(ref int bundleBaseBodyIndexInSet, int /// Gathers motion state information for a body bundle into an AOSOA bundle. /// /// Active body indices being gathered. - /// Number of body pairs in the bundle. /// Gathered absolute position of the body. /// Gathered orientation of the body. /// Gathered velocity of the body. /// Gathered inertia of the body. //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public void GatherState(ref Vector references, int count, + public void GatherState(ref Vector references, out Vector3Wide position, out QuaternionWide orientation, out BodyVelocityWide velocity, out BodyInertiaWide inertia) { Unsafe.SkipInit(out position); @@ -62,19 +61,21 @@ public void GatherState(ref Vector references, int count, //It directly reports the position, thereby infecting vectorized logic with the high precision representation. //You might be able to redesign the users of this function to not need it, but that comes with its own difficulties //(for example, making the grab motor rely on having its goal offset updated every frame by the user). - Debug.Assert(count >= 0 && count <= Vector.Count); //Grab the base references for the body indices. Note that we make use of the references memory layout again. ref var baseIndex = ref Unsafe.As, int>(ref references); - for (int i = 0; i < count; ++i) + for (int i = 0; i < Vector.Count; ++i) { - WriteGatherMotionState(ref baseIndex, i, ref ActiveSet.SolverStates, ref position, ref orientation, ref velocity); - WriteGatherInertia(ref baseIndex, i, ref ActiveSet.SolverStates, ref inertia); + var index = Unsafe.Add(ref baseIndex, i); + if (index < 0) + continue; + WriteGatherMotionState(index, i, ref ActiveSet.SolverStates, ref position, ref orientation, ref velocity); + WriteGatherInertia(index, i, ref ActiveSet.SolverStates, ref inertia); } } //[MethodImpl(MethodImplOptions.AggressiveInlining)] - unsafe static void FallbackGatherMotionState(int count, SolverState* states, ref Vector baseIndex, ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocityWide velocity) + unsafe static void FallbackGatherMotionState(SolverState* states, ref Vector baseIndex, ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocityWide velocity) { var indices = (int*)Unsafe.AsPointer(ref baseIndex); var pPositionX = (float*)Unsafe.AsPointer(ref position.X); @@ -91,9 +92,11 @@ unsafe static void FallbackGatherMotionState(int count, SolverState* states, ref var pAngularY = (float*)Unsafe.AsPointer(ref velocity.Angular.Y); var pAngularZ = (float*)Unsafe.AsPointer(ref velocity.Angular.Z); - for (int i = 0; i < count; ++i) + for (int i = 0; i < Vector.Count; ++i) { var index = indices[i]; + if (index < 0) + continue; var stateValues = (float*)(states + index); pPositionX[i] = stateValues[MotionState.OffsetToPositionX]; pPositionY[i] = stateValues[MotionState.OffsetToPositionY]; @@ -110,7 +113,7 @@ unsafe static void FallbackGatherMotionState(int count, SolverState* states, ref pAngularZ[i] = stateValues[MotionState.OffsetToAngularZ]; } } - unsafe static void FallbackGatherInertia(int count, SolverState* states, ref Vector baseIndex, ref BodyInertiaWide inertia, int offsetInFloats) + unsafe static void FallbackGatherInertia(SolverState* states, ref Vector baseIndex, ref BodyInertiaWide inertia, int offsetInFloats) { var indices = (int*)Unsafe.AsPointer(ref baseIndex); var pMass = (float*)Unsafe.AsPointer(ref inertia.InverseMass); @@ -121,9 +124,11 @@ unsafe static void FallbackGatherInertia(int count, SolverState* states, ref Vec var pInertiaZY = (float*)Unsafe.AsPointer(ref inertia.InverseInertiaTensor.ZY); var pInertiaZZ = (float*)Unsafe.AsPointer(ref inertia.InverseInertiaTensor.ZZ); - for (int i = 0; i < count; ++i) + for (int i = 0; i < Vector.Count; ++i) { var index = indices[i]; + if (index < 0) + continue; var inertiaValues = (float*)(states + index) + offsetInFloats; pInertiaXX[i] = inertiaValues[0]; pInertiaYX[i] = inertiaValues[1]; @@ -136,7 +141,7 @@ unsafe static void FallbackGatherInertia(int count, SolverState* states, ref Vec } //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void GatherState(ref Vector bodyIndices, int count, bool worldInertia, out Vector3Wide position, out QuaternionWide orientation, out BodyVelocityWide velocity, out BodyInertiaWide inertia) + public unsafe void GatherState(ref Vector bodyIndices, bool worldInertia, out Vector3Wide position, out QuaternionWide orientation, out BodyVelocityWide velocity, out BodyInertiaWide inertia) where TAccessFilter : unmanaged, IBodyAccessFilter { var solverStates = ActiveSet.SolverStates.Memory; @@ -169,14 +174,14 @@ public unsafe void GatherState(ref Vector bodyIndices, int c { //Load every body for the first half of the motion state. //Note that buffers are allocated on cache line boundaries, so we can use aligned loads for all that matters. - var m0 = Avx.LoadAlignedVector256(s0); - var m1 = count > 1 ? Avx.LoadAlignedVector256(s1) : Vector256.Zero; - var m2 = count > 2 ? Avx.LoadAlignedVector256(s2) : Vector256.Zero; - var m3 = count > 3 ? Avx.LoadAlignedVector256(s3) : Vector256.Zero; - var m4 = count > 4 ? Avx.LoadAlignedVector256(s4) : Vector256.Zero; - var m5 = count > 5 ? Avx.LoadAlignedVector256(s5) : Vector256.Zero; - var m6 = count > 6 ? Avx.LoadAlignedVector256(s6) : Vector256.Zero; - var m7 = count > 7 ? Avx.LoadAlignedVector256(s7) : Vector256.Zero; + var m0 = indices[0] >= 0 ? Avx.LoadAlignedVector256(s0) : Vector256.Zero; + var m1 = indices[1] >= 0 ? Avx.LoadAlignedVector256(s1) : Vector256.Zero; + var m2 = indices[2] >= 0 ? Avx.LoadAlignedVector256(s2) : Vector256.Zero; + var m3 = indices[3] >= 0 ? Avx.LoadAlignedVector256(s3) : Vector256.Zero; + var m4 = indices[4] >= 0 ? Avx.LoadAlignedVector256(s4) : Vector256.Zero; + var m5 = indices[5] >= 0 ? Avx.LoadAlignedVector256(s5) : Vector256.Zero; + var m6 = indices[6] >= 0 ? Avx.LoadAlignedVector256(s6) : Vector256.Zero; + var m7 = indices[7] >= 0 ? Avx.LoadAlignedVector256(s7) : Vector256.Zero; var n0 = Avx.UnpackLow(m0, m1); var n1 = Avx.UnpackLow(m2, m3); @@ -221,14 +226,14 @@ public unsafe void GatherState(ref Vector bodyIndices, int c { //Second half. - var m0 = Avx.LoadAlignedVector256(s0 + 8); - var m1 = count > 1 ? Avx.LoadAlignedVector256(s1 + 8) : Vector256.Zero; - var m2 = count > 2 ? Avx.LoadAlignedVector256(s2 + 8) : Vector256.Zero; - var m3 = count > 3 ? Avx.LoadAlignedVector256(s3 + 8) : Vector256.Zero; - var m4 = count > 4 ? Avx.LoadAlignedVector256(s4 + 8) : Vector256.Zero; - var m5 = count > 5 ? Avx.LoadAlignedVector256(s5 + 8) : Vector256.Zero; - var m6 = count > 6 ? Avx.LoadAlignedVector256(s6 + 8) : Vector256.Zero; - var m7 = count > 7 ? Avx.LoadAlignedVector256(s7 + 8) : Vector256.Zero; + var m0 = indices[0] >= 0 ? Avx.LoadAlignedVector256(s0 + 8) : Vector256.Zero; + var m1 = indices[1] >= 0 ? Avx.LoadAlignedVector256(s1 + 8) : Vector256.Zero; + var m2 = indices[2] >= 0 ? Avx.LoadAlignedVector256(s2 + 8) : Vector256.Zero; + var m3 = indices[3] >= 0 ? Avx.LoadAlignedVector256(s3 + 8) : Vector256.Zero; + var m4 = indices[4] >= 0 ? Avx.LoadAlignedVector256(s4 + 8) : Vector256.Zero; + var m5 = indices[5] >= 0 ? Avx.LoadAlignedVector256(s5 + 8) : Vector256.Zero; + var m6 = indices[6] >= 0 ? Avx.LoadAlignedVector256(s6 + 8) : Vector256.Zero; + var m7 = indices[7] >= 0 ? Avx.LoadAlignedVector256(s7 + 8) : Vector256.Zero; var n0 = Avx.UnpackLow(m0, m1); var n1 = Avx.UnpackLow(m2, m3); @@ -274,14 +279,14 @@ public unsafe void GatherState(ref Vector bodyIndices, int c var offsetInFloats = worldInertia ? 24 : 16; //Load every inertia vector. - var m0 = Avx.LoadAlignedVector256(s0 + offsetInFloats); - var m1 = count > 1 ? Avx.LoadAlignedVector256(s1 + offsetInFloats) : Vector256.Zero; - var m2 = count > 2 ? Avx.LoadAlignedVector256(s2 + offsetInFloats) : Vector256.Zero; - var m3 = count > 3 ? Avx.LoadAlignedVector256(s3 + offsetInFloats) : Vector256.Zero; - var m4 = count > 4 ? Avx.LoadAlignedVector256(s4 + offsetInFloats) : Vector256.Zero; - var m5 = count > 5 ? Avx.LoadAlignedVector256(s5 + offsetInFloats) : Vector256.Zero; - var m6 = count > 6 ? Avx.LoadAlignedVector256(s6 + offsetInFloats) : Vector256.Zero; - var m7 = count > 7 ? Avx.LoadAlignedVector256(s7 + offsetInFloats) : Vector256.Zero; + var m0 = indices[0] >= 0 ? Avx.LoadAlignedVector256(s0 + offsetInFloats) : Vector256.Zero; + var m1 = indices[1] >= 0 ? Avx.LoadAlignedVector256(s1 + offsetInFloats) : Vector256.Zero; + var m2 = indices[2] >= 0 ? Avx.LoadAlignedVector256(s2 + offsetInFloats) : Vector256.Zero; + var m3 = indices[3] >= 0 ? Avx.LoadAlignedVector256(s3 + offsetInFloats) : Vector256.Zero; + var m4 = indices[4] >= 0 ? Avx.LoadAlignedVector256(s4 + offsetInFloats) : Vector256.Zero; + var m5 = indices[5] >= 0 ? Avx.LoadAlignedVector256(s5 + offsetInFloats) : Vector256.Zero; + var m6 = indices[6] >= 0 ? Avx.LoadAlignedVector256(s6 + offsetInFloats) : Vector256.Zero; + var m7 = indices[7] >= 0 ? Avx.LoadAlignedVector256(s7 + offsetInFloats) : Vector256.Zero; var n0 = Avx.UnpackLow(m0, m1); var n1 = Avx.UnpackLow(m2, m3); @@ -331,8 +336,8 @@ public unsafe void GatherState(ref Vector bodyIndices, int c Unsafe.SkipInit(out orientation); Unsafe.SkipInit(out velocity); Unsafe.SkipInit(out inertia); - FallbackGatherMotionState(count, solverStates, ref bodyIndices, ref position, ref orientation, ref velocity); - FallbackGatherInertia(count, solverStates, ref bodyIndices, ref inertia, worldInertia ? 24 : 16); + FallbackGatherMotionState(solverStates, ref bodyIndices, ref position, ref orientation, ref velocity); + FallbackGatherInertia(solverStates, ref bodyIndices, ref inertia, worldInertia ? 24 : 16); } } @@ -437,21 +442,19 @@ public unsafe void GatherState(ref Vector bodyIndices, int c /// Gathered inertia of body A. /// Gathered inertia of body B. //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void GatherState(ref TwoBodyReferences references, int count, + public unsafe void GatherState(ref TwoBodyReferences references, out QuaternionWide orientationA, out BodyVelocityWide velocityA, out BodyInertiaWide inertiaA, out Vector3Wide ab, out QuaternionWide orientationB, out BodyVelocityWide velocityB, out BodyInertiaWide inertiaB) { - Debug.Assert(count >= 0 && count <= Vector.Count); - //Grab the base references for the body indices. Note that we make use of the references memory layout again. - ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); - ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); - ref var states = ref ActiveSet.SolverStates; - GatherState(ref references.IndexA, count, true, out var positionA, out orientationA, out velocityA, out inertiaA); - GatherState(ref references.IndexB, count, true, out var positionB, out orientationB, out velocityB, out inertiaB); + GatherState(ref references.IndexA, true, out var positionA, out orientationA, out velocityA, out inertiaA); + GatherState(ref references.IndexB, true, out var positionB, out orientationB, out velocityB, out inertiaB); + ////Grab the base references for the body indices. Note that we make use of the references memory layout again. + //ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); + //ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); //for (int i = 0; i < count; ++i) //{ // //WriteGatherState(ref baseIndexA, i, ref states, ref positionA, ref orientationA, ref velocityA); @@ -491,41 +494,18 @@ public unsafe void GatherState(ref TwoBodyReferences references, int count, /// Gathered inertia of body B. /// Gathered inertia of body C. //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public void GatherState(ref ThreeBodyReferences references, int count, + public void GatherState(ref ThreeBodyReferences references, out QuaternionWide orientationA, out BodyVelocityWide velocityA, out BodyInertiaWide inertiaA, out Vector3Wide ab, out QuaternionWide orientationB, out BodyVelocityWide velocityB, out BodyInertiaWide inertiaB, out Vector3Wide ac, out QuaternionWide orientationC, out BodyVelocityWide velocityC, out BodyInertiaWide inertiaC) { - Unsafe.SkipInit(out Vector3Wide positionA); - Unsafe.SkipInit(out Vector3Wide positionB); - Unsafe.SkipInit(out Vector3Wide positionC); - Unsafe.SkipInit(out orientationA); - Unsafe.SkipInit(out orientationB); - Unsafe.SkipInit(out orientationC); - Unsafe.SkipInit(out velocityA); - Unsafe.SkipInit(out velocityB); - Unsafe.SkipInit(out velocityC); - Unsafe.SkipInit(out inertiaA); - Unsafe.SkipInit(out inertiaB); - Unsafe.SkipInit(out inertiaC); - Debug.Assert(count >= 0 && count <= Vector.Count); - //Grab the base references for the body indices. Note that we make use of the references memory layout again. - ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); - ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); - ref var baseIndexC = ref Unsafe.As, int>(ref references.IndexC); - ref var states = ref ActiveSet.SolverStates; - for (int i = 0; i < count; ++i) - { - WriteGatherMotionState(ref baseIndexA, i, ref states, ref positionA, ref orientationA, ref velocityA); - WriteGatherInertia(ref baseIndexA, i, ref states, ref inertiaA); - WriteGatherMotionState(ref baseIndexB, i, ref states, ref positionB, ref orientationB, ref velocityB); - WriteGatherInertia(ref baseIndexB, i, ref states, ref inertiaB); - WriteGatherMotionState(ref baseIndexC, i, ref states, ref positionC, ref orientationC, ref velocityC); - WriteGatherInertia(ref baseIndexC, i, ref states, ref inertiaC); - } + GatherState(ref references.IndexA, true, out var positionA, out orientationA, out velocityA, out inertiaA); + GatherState(ref references.IndexB, true, out var positionB, out orientationB, out velocityB, out inertiaB); + GatherState(ref references.IndexC, true, out var positionC, out orientationC, out velocityC, out inertiaC); + //TODO: In future versions, we will likely store the body position in different forms to allow for extremely large worlds. //That will be an opt-in feature. The default implementation will use the FP32 representation, but the user could choose to swap it out for a fp64 or fixed64 representation. //This affects other systems- AABB calculation, pose integration, solving, and in extreme (64 bit) cases, the broadphase. @@ -559,7 +539,7 @@ public void GatherState(ref ThreeBodyReferences references, int count, /// Gathered inertia of body C. /// Gathered inertia of body C. //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public void GatherState(ref FourBodyReferences references, int count, + public void GatherState(ref FourBodyReferences references, out QuaternionWide orientationA, out BodyVelocityWide velocityA, out BodyInertiaWide inertiaA, out Vector3Wide ab, out QuaternionWide orientationB, out BodyVelocityWide velocityB, out BodyInertiaWide inertiaB, @@ -568,41 +548,11 @@ public void GatherState(ref FourBodyReferences references, int count, out Vector3Wide ad, out QuaternionWide orientationD, out BodyVelocityWide velocityD, out BodyInertiaWide inertiaD) { - Unsafe.SkipInit(out Vector3Wide positionA); - Unsafe.SkipInit(out Vector3Wide positionB); - Unsafe.SkipInit(out Vector3Wide positionC); - Unsafe.SkipInit(out Vector3Wide positionD); - Unsafe.SkipInit(out orientationA); - Unsafe.SkipInit(out orientationB); - Unsafe.SkipInit(out orientationC); - Unsafe.SkipInit(out orientationD); - Unsafe.SkipInit(out velocityA); - Unsafe.SkipInit(out velocityB); - Unsafe.SkipInit(out velocityC); - Unsafe.SkipInit(out velocityD); - Unsafe.SkipInit(out inertiaA); - Unsafe.SkipInit(out inertiaB); - Unsafe.SkipInit(out inertiaC); - Unsafe.SkipInit(out inertiaD); - Debug.Assert(count >= 0 && count <= Vector.Count); - //Grab the base references for the body indices. Note that we make use of the references memory layout again. - ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); - ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); - ref var baseIndexC = ref Unsafe.As, int>(ref references.IndexC); - ref var baseIndexD = ref Unsafe.As, int>(ref references.IndexD); - ref var states = ref ActiveSet.SolverStates; - for (int i = 0; i < count; ++i) - { - WriteGatherMotionState(ref baseIndexA, i, ref states, ref positionA, ref orientationA, ref velocityA); - WriteGatherInertia(ref baseIndexA, i, ref states, ref inertiaA); - WriteGatherMotionState(ref baseIndexB, i, ref states, ref positionB, ref orientationB, ref velocityB); - WriteGatherInertia(ref baseIndexB, i, ref states, ref inertiaB); - WriteGatherMotionState(ref baseIndexC, i, ref states, ref positionC, ref orientationC, ref velocityC); - WriteGatherInertia(ref baseIndexC, i, ref states, ref inertiaC); - WriteGatherMotionState(ref baseIndexD, i, ref states, ref positionD, ref orientationD, ref velocityD); - WriteGatherInertia(ref baseIndexD, i, ref states, ref inertiaD); - } + GatherState(ref references.IndexA, true, out var positionA, out orientationA, out velocityA, out inertiaA); + GatherState(ref references.IndexB, true, out var positionB, out orientationB, out velocityB, out inertiaB); + GatherState(ref references.IndexC, true, out var positionC, out orientationC, out velocityC, out inertiaC); + GatherState(ref references.IndexD, true, out var positionD, out orientationD, out velocityD, out inertiaD); //TODO: In future versions, we will likely store the body position in different forms to allow for extremely large worlds. //That will be an opt-in feature. The default implementation will use the FP32 representation, but the user could choose to swap it out for a fp64 or fixed64 representation. //This affects other systems- AABB calculation, pose integration, solving, and in extreme (64 bit) cases, the broadphase. @@ -723,7 +673,7 @@ public unsafe void ScatterInertia( //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void ScatterVelocities(ref BodyVelocityWide sourceVelocities, ref Vector references, int count) where TAccessFilter : unmanaged, IBodyAccessFilter + public unsafe void ScatterVelocities(ref BodyVelocityWide sourceVelocities, ref Vector references) where TAccessFilter : unmanaged, IBodyAccessFilter { if (Avx.IsSupported && Vector.Count == 8) { @@ -772,14 +722,14 @@ public unsafe void ScatterVelocities(ref BodyVelocityWide sourceV var indices = (int*)Unsafe.AsPointer(ref references); var states = ActiveSet.SolverStates.Memory; - Sse.StoreAligned((float*)(states + indices[0]) + targetOffset, o0.GetLower()); - if (count > 1) Sse.StoreAligned((float*)(states + indices[1]) + targetOffset, o4.GetLower()); - if (count > 2) Sse.StoreAligned((float*)(states + indices[2]) + targetOffset, o2.GetLower()); - if (count > 3) Sse.StoreAligned((float*)(states + indices[3]) + targetOffset, o6.GetLower()); - if (count > 4) Sse.StoreAligned((float*)(states + indices[4]) + targetOffset, o0.GetUpper()); - if (count > 5) Sse.StoreAligned((float*)(states + indices[5]) + targetOffset, o4.GetUpper()); - if (count > 6) Sse.StoreAligned((float*)(states + indices[6]) + targetOffset, o2.GetUpper()); - if (count > 7) Sse.StoreAligned((float*)(states + indices[7]) + targetOffset, o6.GetUpper()); + if (indices[0] >= 0) Sse.StoreAligned((float*)(states + indices[0]) + targetOffset, o0.GetLower()); + if (indices[1] >= 0) Sse.StoreAligned((float*)(states + indices[1]) + targetOffset, o4.GetLower()); + if (indices[2] >= 0) Sse.StoreAligned((float*)(states + indices[2]) + targetOffset, o2.GetLower()); + if (indices[3] >= 0) Sse.StoreAligned((float*)(states + indices[3]) + targetOffset, o6.GetLower()); + if (indices[4] >= 0) Sse.StoreAligned((float*)(states + indices[4]) + targetOffset, o0.GetUpper()); + if (indices[5] >= 0) Sse.StoreAligned((float*)(states + indices[5]) + targetOffset, o4.GetUpper()); + if (indices[6] >= 0) Sse.StoreAligned((float*)(states + indices[6]) + targetOffset, o2.GetUpper()); + if (indices[7] >= 0) Sse.StoreAligned((float*)(states + indices[7]) + targetOffset, o6.GetUpper()); } else @@ -814,22 +764,24 @@ public unsafe void ScatterVelocities(ref BodyVelocityWide sourceV var indices = (int*)Unsafe.AsPointer(ref references); var states = ActiveSet.SolverStates.Memory; - Avx.StoreAligned((float*)(states + indices[0]) + 8, Avx.Permute2x128(o0, o1, 0 | (2 << 4))); - if (count > 1) Avx.StoreAligned((float*)(states + indices[1]) + 8, Avx.Permute2x128(o4, o5, 0 | (2 << 4))); - if (count > 2) Avx.StoreAligned((float*)(states + indices[2]) + 8, Avx.Permute2x128(o2, o3, 0 | (2 << 4))); - if (count > 3) Avx.StoreAligned((float*)(states + indices[3]) + 8, Avx.Permute2x128(o6, o7, 0 | (2 << 4))); - if (count > 4) Avx.StoreAligned((float*)(states + indices[4]) + 8, Avx.Permute2x128(o0, o1, 1 | (3 << 4))); - if (count > 5) Avx.StoreAligned((float*)(states + indices[5]) + 8, Avx.Permute2x128(o4, o5, 1 | (3 << 4))); - if (count > 6) Avx.StoreAligned((float*)(states + indices[6]) + 8, Avx.Permute2x128(o2, o3, 1 | (3 << 4))); - if (count > 7) Avx.StoreAligned((float*)(states + indices[7]) + 8, Avx.Permute2x128(o6, o7, 1 | (3 << 4))); + if (indices[0] >= 0) Avx.StoreAligned((float*)(states + indices[0]) + 8, Avx.Permute2x128(o0, o1, 0 | (2 << 4))); + if (indices[1] >= 0) Avx.StoreAligned((float*)(states + indices[1]) + 8, Avx.Permute2x128(o4, o5, 0 | (2 << 4))); + if (indices[2] >= 0) Avx.StoreAligned((float*)(states + indices[2]) + 8, Avx.Permute2x128(o2, o3, 0 | (2 << 4))); + if (indices[3] >= 0) Avx.StoreAligned((float*)(states + indices[3]) + 8, Avx.Permute2x128(o6, o7, 0 | (2 << 4))); + if (indices[4] >= 0) Avx.StoreAligned((float*)(states + indices[4]) + 8, Avx.Permute2x128(o0, o1, 1 | (3 << 4))); + if (indices[5] >= 0) Avx.StoreAligned((float*)(states + indices[5]) + 8, Avx.Permute2x128(o4, o5, 1 | (3 << 4))); + if (indices[6] >= 0) Avx.StoreAligned((float*)(states + indices[6]) + 8, Avx.Permute2x128(o2, o3, 1 | (3 << 4))); + if (indices[7] >= 0) Avx.StoreAligned((float*)(states + indices[7]) + 8, Avx.Permute2x128(o6, o7, 1 | (3 << 4))); } } else { - for (int innerIndex = 0; innerIndex < count; ++innerIndex) + var indices = (int*)Unsafe.AsPointer(ref references); + for (int innerIndex = 0; innerIndex < Vector.Count; ++innerIndex) { + if (indices[innerIndex] < 0) + continue; ref var sourceSlot = ref GetOffsetInstance(ref sourceVelocities, innerIndex); - var indices = (int*)Unsafe.AsPointer(ref references); ref var target = ref ActiveSet.SolverStates[indices[innerIndex]].Motion.Velocity; target.Linear = new Vector3(sourceSlot.Linear.X[0], sourceSlot.Linear.Y[0], sourceSlot.Linear.Z[0]); target.Angular = new Vector3(sourceSlot.Angular.X[0], sourceSlot.Angular.Y[0], sourceSlot.Angular.Z[0]); diff --git a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs index 71fcdc0fe..1d4e6916a 100644 --- a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs @@ -135,8 +135,7 @@ public unsafe override void Prestep(ref TypeBatch typeBatch, Bodies bodies, floa ref var prestep = ref Unsafe.Add(ref prestepBase, i); ref var projection = ref Unsafe.Add(ref projectionBase, i); ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); - var count = GetCountInBundle(ref typeBatch, i); - bodies.GatherState(ref references, count, + bodies.GatherState(ref references, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB, out var ac, out var orientationC, out var wsvC, out var inertiaC, @@ -156,17 +155,16 @@ public unsafe override void WarmStart(ref TypeBatch typeBatch, Bodies bodies, in ref var projection = ref Unsafe.Add(ref projectionBase, i); ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); - int count = GetCountInBundle(ref typeBatch, i); - bodies.GatherState(ref bodyReferences, count, + bodies.GatherState(ref bodyReferences, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB, out var ac, out var orientationC, out var wsvC, out var inertiaC, out var ad, out var orientationD, out var wsvD, out var inertiaD); function.WarmStart(ref wsvA, ref wsvB, ref wsvC, ref wsvD, ref projection, ref accumulatedImpulses); - bodies.ScatterVelocities(ref wsvA, ref bodyReferences.IndexA, count); - bodies.ScatterVelocities(ref wsvB, ref bodyReferences.IndexB, count); - bodies.ScatterVelocities(ref wsvC, ref bodyReferences.IndexC, count); - bodies.ScatterVelocities(ref wsvD, ref bodyReferences.IndexD, count); + bodies.ScatterVelocities(ref wsvA, ref bodyReferences.IndexA); + bodies.ScatterVelocities(ref wsvB, ref bodyReferences.IndexB); + bodies.ScatterVelocities(ref wsvC, ref bodyReferences.IndexC); + bodies.ScatterVelocities(ref wsvD, ref bodyReferences.IndexD); } } @@ -182,17 +180,16 @@ public unsafe override void SolveIteration(ref TypeBatch typeBatch, Bodies bodie ref var projection = ref Unsafe.Add(ref projectionBase, i); ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); - int count = GetCountInBundle(ref typeBatch, i); - bodies.GatherState(ref bodyReferences, count, + bodies.GatherState(ref bodyReferences, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB, out var ac, out var orientationC, out var wsvC, out var inertiaC, out var ad, out var orientationD, out var wsvD, out var inertiaD); function.Solve(ref wsvA, ref wsvB, ref wsvC, ref wsvD, ref projection, ref accumulatedImpulses); - bodies.ScatterVelocities(ref wsvA, ref bodyReferences.IndexA, count); - bodies.ScatterVelocities(ref wsvB, ref bodyReferences.IndexB, count); - bodies.ScatterVelocities(ref wsvC, ref bodyReferences.IndexC, count); - bodies.ScatterVelocities(ref wsvD, ref bodyReferences.IndexD, count); + bodies.ScatterVelocities(ref wsvA, ref bodyReferences.IndexA); + bodies.ScatterVelocities(ref wsvB, ref bodyReferences.IndexB); + bodies.ScatterVelocities(ref wsvC, ref bodyReferences.IndexC); + bodies.ScatterVelocities(ref wsvD, ref bodyReferences.IndexD); } } @@ -210,14 +207,13 @@ public unsafe override void WarmStart2(bodies, ref integratorCallbacks, ref integrationFlags, 0, dt, workerIndex, i, ref references.IndexA, count, + GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 0, dt, workerIndex, i, ref references.IndexA, out var positionA, out var orientationA, out var wsvA, out var inertiaA); - GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 1, dt, workerIndex, i, ref references.IndexB, count, + GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 1, dt, workerIndex, i, ref references.IndexB, out var positionB, out var orientationB, out var wsvB, out var inertiaB); - GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 2, dt, workerIndex, i, ref references.IndexC, count, + GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 2, dt, workerIndex, i, ref references.IndexC, out var positionC, out var orientationC, out var wsvC, out var inertiaC); - GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 3, dt, workerIndex, i, ref references.IndexD, count, + GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 3, dt, workerIndex, i, ref references.IndexD, out var positionD, out var orientationD, out var wsvD, out var inertiaD); //if (typeof(TAllowPoseIntegration) == typeof(AllowPoseIntegration)) @@ -227,19 +223,19 @@ public unsafe override void WarmStart2(ref wsvA, ref references.IndexA, count); - bodies.ScatterVelocities(ref wsvB, ref references.IndexB, count); - bodies.ScatterVelocities(ref wsvC, ref references.IndexC, count); - bodies.ScatterVelocities(ref wsvD, ref references.IndexD, count); + bodies.ScatterVelocities(ref wsvA, ref references.IndexA); + bodies.ScatterVelocities(ref wsvB, ref references.IndexB); + bodies.ScatterVelocities(ref wsvC, ref references.IndexC); + bodies.ScatterVelocities(ref wsvD, ref references.IndexD); } else { //This batch has some integrators, which means that every bundle is going to gather all velocities. //(We don't make per-bundle determinations about this to avoid an extra branch and instruction complexity, and the difference is very small.) - bodies.ScatterVelocities(ref wsvA, ref references.IndexA, count); - bodies.ScatterVelocities(ref wsvB, ref references.IndexB, count); - bodies.ScatterVelocities(ref wsvC, ref references.IndexC, count); - bodies.ScatterVelocities(ref wsvD, ref references.IndexD, count); + bodies.ScatterVelocities(ref wsvA, ref references.IndexA); + bodies.ScatterVelocities(ref wsvB, ref references.IndexB); + bodies.ScatterVelocities(ref wsvC, ref references.IndexC); + bodies.ScatterVelocities(ref wsvD, ref references.IndexD); } } @@ -257,18 +253,17 @@ public unsafe override void SolveStep2(ref TypeBatch typ ref var prestep = ref prestepBundles[i]; ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; ref var references = ref bodyReferencesBundles[i]; - var count = bundleCountCalculator.GetCountInBundle(ref typeBatch, i); - bodies.GatherState(ref references.IndexA, count, true, out var positionA, out var orientationA, out var wsvA, out var inertiaA); - bodies.GatherState(ref references.IndexB, count, true, out var positionB, out var orientationB, out var wsvB, out var inertiaB); - bodies.GatherState(ref references.IndexC, count, true, out var positionC, out var orientationC, out var wsvC, out var inertiaC); - bodies.GatherState(ref references.IndexD, count, true, out var positionD, out var orientationD, out var wsvD, out var inertiaD); + bodies.GatherState(ref references.IndexA, true, out var positionA, out var orientationA, out var wsvA, out var inertiaA); + bodies.GatherState(ref references.IndexB, true, out var positionB, out var orientationB, out var wsvB, out var inertiaB); + bodies.GatherState(ref references.IndexC, true, out var positionC, out var orientationC, out var wsvC, out var inertiaC); + bodies.GatherState(ref references.IndexD, true, out var positionD, out var orientationD, out var wsvD, out var inertiaD); function.Solve2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, positionD, orientationD, inertiaD, dt, inverseDt, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC, ref wsvD); - bodies.ScatterVelocities(ref wsvA, ref references.IndexA, count); - bodies.ScatterVelocities(ref wsvB, ref references.IndexB, count); - bodies.ScatterVelocities(ref wsvC, ref references.IndexC, count); - bodies.ScatterVelocities(ref wsvD, ref references.IndexD, count); + bodies.ScatterVelocities(ref wsvA, ref references.IndexA); + bodies.ScatterVelocities(ref wsvB, ref references.IndexB); + bodies.ScatterVelocities(ref wsvC, ref references.IndexC); + bodies.ScatterVelocities(ref wsvD, ref references.IndexD); } } diff --git a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs index 8d103b767..ba1f73865 100644 --- a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs @@ -112,8 +112,7 @@ public unsafe override void Prestep(ref TypeBatch typeBatch, Bodies bodies, floa ref var prestep = ref Unsafe.Add(ref prestepBase, i); ref var projection = ref Unsafe.Add(ref projectionBase, i); ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); - var count = GetCountInBundle(ref typeBatch, i); - bodies.GatherState(ref references, count, out var position, out var orientation, out _, out var inertia); + bodies.GatherState(ref references, out var position, out var orientation, out _, out var inertia); function.Prestep(position, orientation, inertia, dt, inverseDt, ref prestep, out projection); } } @@ -129,10 +128,9 @@ public unsafe override void WarmStart(ref TypeBatch typeBatch, Bodies bodies, in ref var projection = ref Unsafe.Add(ref projectionBase, i); ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); - int count = GetCountInBundle(ref typeBatch, i); - bodies.GatherState(ref bodyReferences, count, out _, out _, out var wsvA, out _); + bodies.GatherState(ref bodyReferences, out _, out _, out var wsvA, out _); function.WarmStart(ref wsvA, ref projection, ref accumulatedImpulses); - bodies.ScatterVelocities(ref wsvA, ref bodyReferences, count); + bodies.ScatterVelocities(ref wsvA, ref bodyReferences); } } @@ -147,10 +145,9 @@ public unsafe override void SolveIteration(ref TypeBatch typeBatch, Bodies bodie ref var projection = ref Unsafe.Add(ref projectionBase, i); ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); - int count = GetCountInBundle(ref typeBatch, i); - bodies.GatherState(ref bodyReferences, count, out _, out _, out var wsvA, out _); + bodies.GatherState(ref bodyReferences, out _, out _, out var wsvA, out _); function.Solve(ref wsvA, ref projection, ref accumulatedImpulses); - bodies.ScatterVelocities(ref wsvA, ref bodyReferences, count); + bodies.ScatterVelocities(ref wsvA, ref bodyReferences); } } @@ -168,8 +165,7 @@ public unsafe override void WarmStart2(bodies, ref integratorCallbacks, ref integrationFlags, 0, dt, workerIndex, i, ref references, count, + GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 0, dt, workerIndex, i, ref references, out var positionA, out var orientationA, out var wsvA, out var inertiaA); //if (typeof(TAllowPoseIntegration) == typeof(AllowPoseIntegration)) @@ -179,13 +175,13 @@ public unsafe override void WarmStart2(ref wsvA, ref references, count); + bodies.ScatterVelocities(ref wsvA, ref references); } else { //This batch has some integrators, which means that every bundle is going to gather all velocities. //(We don't make per-bundle determinations about this to avoid an extra branch and instruction complexity, and the difference is very small.) - bodies.ScatterVelocities(ref wsvA, ref references, count); + bodies.ScatterVelocities(ref wsvA, ref references); } } } @@ -202,12 +198,11 @@ public unsafe override void SolveStep2(ref TypeBatch typ ref var prestep = ref prestepBundles[i]; ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; ref var references = ref bodyReferencesBundles[i]; - var count = bundleCountCalculator.GetCountInBundle(ref typeBatch, i); - bodies.GatherState(ref references, count, true, out var positionA, out var orientationA, out var wsvA, out var inertiaA); + bodies.GatherState(ref references, true, out var positionA, out var orientationA, out var wsvA, out var inertiaA); function.Solve2(positionA, orientationA, inertiaA, dt, inverseDt, ref prestep, ref accumulatedImpulses, ref wsvA); - bodies.ScatterVelocities(ref wsvA, ref references, count); + bodies.ScatterVelocities(ref wsvA, ref references); } } } @@ -227,8 +222,7 @@ public unsafe override void IncrementallyUpdateContactData(ref TypeBatch typeBat { ref var prestep = ref Unsafe.Add(ref prestepBase, i); ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); - var count = GetCountInBundle(ref typeBatch, i); - bodies.GatherState(ref references, count, true, out _, out _, out var wsvA, out _); + bodies.GatherState(ref references, true, out _, out _, out var wsvA, out _); function.IncrementallyUpdateContactData(dtWide, wsvA, ref prestep); } } diff --git a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs index bf333e7bd..f3bec1722 100644 --- a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs @@ -125,8 +125,7 @@ public unsafe override void Prestep(ref TypeBatch typeBatch, Bodies bodies, floa ref var prestep = ref Unsafe.Add(ref prestepBase, i); ref var projection = ref Unsafe.Add(ref projectionBase, i); ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); - var count = GetCountInBundle(ref typeBatch, i); - bodies.GatherState(ref references, count, + bodies.GatherState(ref references, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB, out var ac, out var orientationC, out var wsvC, out var inertiaC); @@ -145,15 +144,14 @@ public unsafe override void WarmStart(ref TypeBatch typeBatch, Bodies bodies, in ref var projection = ref Unsafe.Add(ref projectionBase, i); ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); - int count = GetCountInBundle(ref typeBatch, i); - bodies.GatherState(ref bodyReferences, count, + bodies.GatherState(ref bodyReferences, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB, out var ac, out var orientationC, out var wsvC, out var inertiaC); function.WarmStart(ref wsvA, ref wsvB, ref wsvC, ref projection, ref accumulatedImpulses); - bodies.ScatterVelocities(ref wsvA, ref bodyReferences.IndexA, count); - bodies.ScatterVelocities(ref wsvB, ref bodyReferences.IndexB, count); - bodies.ScatterVelocities(ref wsvC, ref bodyReferences.IndexC, count); + bodies.ScatterVelocities(ref wsvA, ref bodyReferences.IndexA); + bodies.ScatterVelocities(ref wsvB, ref bodyReferences.IndexB); + bodies.ScatterVelocities(ref wsvC, ref bodyReferences.IndexC); } } @@ -169,15 +167,14 @@ public unsafe override void SolveIteration(ref TypeBatch typeBatch, Bodies bodie ref var projection = ref Unsafe.Add(ref projectionBase, i); ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); - int count = GetCountInBundle(ref typeBatch, i); - bodies.GatherState(ref bodyReferences, count, + bodies.GatherState(ref bodyReferences, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB, out var ac, out var orientationC, out var wsvC, out var inertiaC); function.Solve(ref wsvA, ref wsvB, ref wsvC, ref projection, ref accumulatedImpulses); - bodies.ScatterVelocities(ref wsvA, ref bodyReferences.IndexA, count); - bodies.ScatterVelocities(ref wsvB, ref bodyReferences.IndexB, count); - bodies.ScatterVelocities(ref wsvC, ref bodyReferences.IndexC, count); + bodies.ScatterVelocities(ref wsvA, ref bodyReferences.IndexA); + bodies.ScatterVelocities(ref wsvB, ref bodyReferences.IndexB); + bodies.ScatterVelocities(ref wsvC, ref bodyReferences.IndexC); } } public unsafe override void WarmStart2( @@ -194,12 +191,11 @@ public unsafe override void WarmStart2(bodies, ref integratorCallbacks, ref integrationFlags, 0, dt, workerIndex, i, ref references.IndexA, count, + GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 0, dt, workerIndex, i, ref references.IndexA, out var positionA, out var orientationA, out var wsvA, out var inertiaA); - GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 1, dt, workerIndex, i, ref references.IndexB, count, + GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 1, dt, workerIndex, i, ref references.IndexB, out var positionB, out var orientationB, out var wsvB, out var inertiaB); - GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 2, dt, workerIndex, i, ref references.IndexC, count, + GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 2, dt, workerIndex, i, ref references.IndexC, out var positionC, out var orientationC, out var wsvC, out var inertiaC); //if (typeof(TAllowPoseIntegration) == typeof(AllowPoseIntegration)) @@ -209,17 +205,17 @@ public unsafe override void WarmStart2(ref wsvA, ref references.IndexA, count); - bodies.ScatterVelocities(ref wsvB, ref references.IndexB, count); - bodies.ScatterVelocities(ref wsvC, ref references.IndexC, count); + bodies.ScatterVelocities(ref wsvA, ref references.IndexA); + bodies.ScatterVelocities(ref wsvB, ref references.IndexB); + bodies.ScatterVelocities(ref wsvC, ref references.IndexC); } else { //This batch has some integrators, which means that every bundle is going to gather all velocities. //(We don't make per-bundle determinations about this to avoid an extra branch and instruction complexity, and the difference is very small.) - bodies.ScatterVelocities(ref wsvA, ref references.IndexA, count); - bodies.ScatterVelocities(ref wsvB, ref references.IndexB, count); - bodies.ScatterVelocities(ref wsvC, ref references.IndexC, count); + bodies.ScatterVelocities(ref wsvA, ref references.IndexA); + bodies.ScatterVelocities(ref wsvB, ref references.IndexB); + bodies.ScatterVelocities(ref wsvC, ref references.IndexC); } } @@ -237,16 +233,15 @@ public unsafe override void SolveStep2(ref TypeBatch typ ref var prestep = ref prestepBundles[i]; ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; ref var references = ref bodyReferencesBundles[i]; - var count = bundleCountCalculator.GetCountInBundle(ref typeBatch, i); - bodies.GatherState(ref references.IndexA, count, true, out var positionA, out var orientationA, out var wsvA, out var inertiaA); - bodies.GatherState(ref references.IndexB, count, true, out var positionB, out var orientationB, out var wsvB, out var inertiaB); - bodies.GatherState(ref references.IndexC, count, true, out var positionC, out var orientationC, out var wsvC, out var inertiaC); + bodies.GatherState(ref references.IndexA, true, out var positionA, out var orientationA, out var wsvA, out var inertiaA); + bodies.GatherState(ref references.IndexB, true, out var positionB, out var orientationB, out var wsvB, out var inertiaB); + bodies.GatherState(ref references.IndexC, true, out var positionC, out var orientationC, out var wsvC, out var inertiaC); function.Solve2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, dt, inverseDt, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC); - bodies.ScatterVelocities(ref wsvA, ref references.IndexA, count); - bodies.ScatterVelocities(ref wsvB, ref references.IndexB, count); - bodies.ScatterVelocities(ref wsvC, ref references.IndexC, count); + bodies.ScatterVelocities(ref wsvA, ref references.IndexA); + bodies.ScatterVelocities(ref wsvB, ref references.IndexB); + bodies.ScatterVelocities(ref wsvC, ref references.IndexC); } } } diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index cd886dce8..22393f239 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -155,8 +155,7 @@ public unsafe override void Prestep(ref TypeBatch typeBatch, Bodies bodies, floa ref var prestep = ref Unsafe.Add(ref prestepBase, i); ref var projection = ref Unsafe.Add(ref projectionBase, i); ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); - var count = GetCountInBundle(ref typeBatch, i); - bodies.GatherState(ref references, count, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB); + bodies.GatherState(ref references, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB); function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, dt, inverseDt, ref prestep, out projection); } } @@ -172,11 +171,10 @@ public unsafe override void WarmStart(ref TypeBatch typeBatch, Bodies bodies, in ref var projection = ref Unsafe.Add(ref projectionBase, i); ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); - int count = GetCountInBundle(ref typeBatch, i); - bodies.GatherState(ref references, count, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB); + bodies.GatherState(ref references, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB); function.WarmStart(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); - bodies.ScatterVelocities(ref wsvA, ref references.IndexA, count); - bodies.ScatterVelocities(ref wsvB, ref references.IndexB, count); + bodies.ScatterVelocities(ref wsvA, ref references.IndexA); + bodies.ScatterVelocities(ref wsvB, ref references.IndexB); } } @@ -192,11 +190,10 @@ public unsafe override void SolveIteration(ref TypeBatch typeBatch, Bodies bodie ref var projection = ref Unsafe.Add(ref projectionBase, i); ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); - int count = GetCountInBundle(ref typeBatch, i); - bodies.GatherState(ref references, count, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB); + bodies.GatherState(ref references, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB); function.Solve(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); - bodies.ScatterVelocities(ref wsvA, ref references.IndexA, count); - bodies.ScatterVelocities(ref wsvB, ref references.IndexB, count); + bodies.ScatterVelocities(ref wsvA, ref references.IndexA); + bodies.ScatterVelocities(ref wsvB, ref references.IndexB); } } @@ -269,11 +266,10 @@ public unsafe override void WarmStart2(bodies, ref integratorCallbacks, ref integrationFlags, 0, dt, workerIndex, i, ref references.IndexA, count, + GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 0, dt, workerIndex, i, ref references.IndexA, out var positionA, out var orientationA, out var wsvA, out var inertiaA); - GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 1, dt, workerIndex, i, ref references.IndexB, count, + GatherAndIntegrate(bodies, ref integratorCallbacks, ref integrationFlags, 1, dt, workerIndex, i, ref references.IndexB, out var positionB, out var orientationB, out var wsvB, out var inertiaB); //if (typeof(TAllowPoseIntegration) == typeof(AllowPoseIntegration)) @@ -283,15 +279,15 @@ public unsafe override void WarmStart2(ref wsvA, ref references.IndexA, count); - bodies.ScatterVelocities(ref wsvB, ref references.IndexB, count); + bodies.ScatterVelocities(ref wsvA, ref references.IndexA); + bodies.ScatterVelocities(ref wsvB, ref references.IndexB); } else { //This batch has some integrators, which means that every bundle is going to gather all velocities. //(We don't make per-bundle determinations about this to avoid an extra branch and instruction complexity, and the difference is very small.) - bodies.ScatterVelocities(ref wsvA, ref references.IndexA, count); - bodies.ScatterVelocities(ref wsvB, ref references.IndexB, count); + bodies.ScatterVelocities(ref wsvA, ref references.IndexA); + bodies.ScatterVelocities(ref wsvB, ref references.IndexB); } } @@ -310,15 +306,14 @@ public unsafe override void SolveStep2(ref TypeBatch typ ref var prestep = ref prestepBundles[i]; ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; ref var references = ref bodyReferencesBundles[i]; - var count = bundleCountCalculator.GetCountInBundle(ref typeBatch, i); //Prefetch(SolvePrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, i, exclusiveEndBundle); - bodies.GatherState(ref references.IndexA, count, true, out var positionA, out var orientationA, out var wsvA, out var inertiaA); - bodies.GatherState(ref references.IndexB, count, true, out var positionB, out var orientationB, out var wsvB, out var inertiaB); + bodies.GatherState(ref references.IndexA, true, out var positionA, out var orientationA, out var wsvA, out var inertiaA); + bodies.GatherState(ref references.IndexB, true, out var positionB, out var orientationB, out var wsvB, out var inertiaB); function.Solve2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, dt, inverseDt, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB); - bodies.ScatterVelocities(ref wsvA, ref references.IndexA, count); - bodies.ScatterVelocities(ref wsvB, ref references.IndexB, count); + bodies.ScatterVelocities(ref wsvA, ref references.IndexA); + bodies.ScatterVelocities(ref wsvB, ref references.IndexB); } } } @@ -338,9 +333,8 @@ public unsafe override void IncrementallyUpdateContactData(ref TypeBatch typeBat { ref var prestep = ref Unsafe.Add(ref prestepBase, i); ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); - var count = GetCountInBundle(ref typeBatch, i); - bodies.GatherState(ref references.IndexA, count, true, out _, out _, out var wsvA, out _); - bodies.GatherState(ref references.IndexB, count, true, out _, out _, out var wsvB, out _); + bodies.GatherState(ref references.IndexA, true, out _, out _, out var wsvA, out _); + bodies.GatherState(ref references.IndexB, true, out _, out _, out var wsvB, out _); function.IncrementallyUpdateContactData(dtWide, wsvA, wsvB, ref prestep); } } diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index 1ac09734b..630ccc6d9 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -789,7 +789,7 @@ public static BundleIntegrationMode BundleShouldIntegrate(int bundleIndex, in In } [MethodImpl(MethodImplOptions.AggressiveInlining)] public static unsafe void IntegratePoseAndVelocity( - ref TIntegratorCallbacks integratorCallbacks, ref Vector bodyIndices, int count, in BodyInertiaWide localInertia, float dt, in Vector integrationMask, + ref TIntegratorCallbacks integratorCallbacks, ref Vector bodyIndices, in BodyInertiaWide localInertia, float dt, in Vector integrationMask, ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocityWide velocity, int workerIndex, out BodyInertiaWide inertia) @@ -828,14 +828,14 @@ public static unsafe void IntegratePoseAndVelocity( QuaternionWide.ConditionalSelect(integrationMask, newOrientation, orientation, out orientation); PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, orientation, out inertia.InverseInertiaTensor); } - integratorCallbacks.IntegrateVelocity(new ReadOnlySpan(Unsafe.AsPointer(ref bodyIndices), count), position, orientation, localInertia, integrationMask, workerIndex, new Vector(dt), ref velocity); + integratorCallbacks.IntegrateVelocity(bodyIndices, position, orientation, localInertia, integrationMask, workerIndex, new Vector(dt), ref velocity); //It would be annoying to make the user handle masking velocity writes to inactive lanes, so we handle it internally. Vector3Wide.ConditionalSelect(integrationMask, velocity.Linear, previousVelocity.Linear, out velocity.Linear); Vector3Wide.ConditionalSelect(integrationMask, velocity.Angular, previousVelocity.Angular, out velocity.Angular); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public static unsafe void IntegratePoseAndVelocity( - ref TIntegratorCallbacks integratorCallbacks, ref Vector bodyIndices, int count, in BodyInertiaWide localInertia, float dt, + ref TIntegratorCallbacks integratorCallbacks, ref Vector bodyIndices, in BodyInertiaWide localInertia, float dt, ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocityWide velocity, int workerIndex, out BodyInertiaWide inertia) @@ -863,13 +863,13 @@ public static unsafe void IntegratePoseAndVelocity( PoseIntegration.Integrate(orientation, velocity.Angular, dtWide * new Vector(0.5f), out orientation); PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, orientation, out inertia.InverseInertiaTensor); } - integratorCallbacks.IntegrateVelocity(new ReadOnlySpan(Unsafe.AsPointer(ref bodyIndices), count), position, orientation, localInertia, new Vector(-1), workerIndex, new Vector(dt), ref velocity); + integratorCallbacks.IntegrateVelocity(bodyIndices, position, orientation, localInertia, new Vector(-1), workerIndex, new Vector(dt), ref velocity); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public static unsafe void IntegrateVelocity( - ref TIntegratorCallbacks integratorCallbacks, ref Vector bodyIndices, int count, in BodyInertiaWide localInertia, float dt, in Vector integrationMask, + ref TIntegratorCallbacks integratorCallbacks, ref Vector bodyIndices, in BodyInertiaWide localInertia, float dt, in Vector integrationMask, in Vector3Wide position, in QuaternionWide orientation, ref BodyVelocityWide velocity, int workerIndex, out BodyInertiaWide inertia) @@ -891,21 +891,21 @@ public static unsafe void IntegrateVelocity(Unsafe.AsPointer(ref bodyIndices), count), position, orientation, localInertia, integrationMask, workerIndex, new Vector(dt), ref velocity); + integratorCallbacks.IntegrateVelocity(bodyIndices, position, orientation, localInertia, integrationMask, workerIndex, new Vector(dt), ref velocity); //It would be annoying to make the user handle masking velocity writes to inactive lanes, so we handle it internally. Vector3Wide.ConditionalSelect(integrationMask, velocity.Linear, previousVelocity.Linear, out velocity.Linear); Vector3Wide.ConditionalSelect(integrationMask, velocity.Angular, previousVelocity.Angular, out velocity.Angular); } else { - integratorCallbacks.IntegrateVelocity(new ReadOnlySpan(Unsafe.AsPointer(ref bodyIndices), count), position, orientation, localInertia, integrationMask, workerIndex, new Vector(dt), ref velocity); + integratorCallbacks.IntegrateVelocity(bodyIndices, position, orientation, localInertia, integrationMask, workerIndex, new Vector(dt), ref velocity); } } //[MethodImpl(MethodImplOptions.AggressiveInlining)] public static unsafe void GatherAndIntegrate( Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, ref Buffer integrationFlags, int bodyIndexInConstraint, float dt, int workerIndex, int bundleIndex, - ref Vector bodyIndices, int count, out Vector3Wide position, out QuaternionWide orientation, out BodyVelocityWide velocity, out BodyInertiaWide inertia) + ref Vector bodyIndices, out Vector3Wide position, out QuaternionWide orientation, out BodyVelocityWide velocity, out BodyInertiaWide inertia) where TIntegratorCallbacks : struct, IPoseIntegratorCallbacks where TBatchIntegrationMode : unmanaged, IBatchIntegrationMode where TAccessFilter : unmanaged, IBodyAccessFilter @@ -916,15 +916,15 @@ public static unsafe void GatherAndIntegrate(ref bodyIndices, count, false, out position, out orientation, out velocity, out var localInertia); - IntegratePoseAndVelocity(ref integratorCallbacks, ref bodyIndices, count, localInertia, dt, ref position, ref orientation, ref velocity, workerIndex, out inertia); + var integrationMask = Vector.GreaterThanOrEqual(bodyIndices, Vector.Zero); + bodies.GatherState(ref bodyIndices, false, out position, out orientation, out velocity, out var localInertia); + IntegratePoseAndVelocity(ref integratorCallbacks, ref bodyIndices, localInertia, dt, ref position, ref orientation, ref velocity, workerIndex, out inertia); bodies.ScatterPose(ref position, ref orientation, ref bodyIndices, ref integrationMask); bodies.ScatterInertia(ref inertia, ref bodyIndices, ref integrationMask); } else if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) { - bodies.GatherState(ref bodyIndices, count, true, out position, out orientation, out velocity, out inertia); + bodies.GatherState(ref bodyIndices, true, out position, out orientation, out velocity, out inertia); } else { @@ -936,13 +936,13 @@ public static unsafe void GatherAndIntegrate(ref bodyIndices, count, bundleIntegrationMode == BundleIntegrationMode.None, out position, out orientation, out velocity, out var gatheredInertia); + bodies.GatherState(ref bodyIndices, bundleIntegrationMode == BundleIntegrationMode.None, out position, out orientation, out velocity, out var gatheredInertia); if (bundleIntegrationMode != BundleIntegrationMode.None) { //Note that if we take this codepath, the integration routine will reconstruct the world inertias from local inertia given the current pose. //The changes to pose and velocity for integration inactive lanes will be masked out, so it'll just be identical to the world inertia if we had gathered it. //Given that we're running the instructions in a bundle to build it, there's no reason to go out of our way to gather the world inertia. - IntegratePoseAndVelocity(ref integratorCallbacks, ref bodyIndices, count, gatheredInertia, dt, integrationMask, ref position, ref orientation, ref velocity, workerIndex, out inertia); + IntegratePoseAndVelocity(ref integratorCallbacks, ref bodyIndices, gatheredInertia, dt, integrationMask, ref position, ref orientation, ref velocity, workerIndex, out inertia); bodies.ScatterPose(ref position, ref orientation, ref bodyIndices, ref integrationMask); bodies.ScatterInertia(ref inertia, ref bodyIndices, ref integrationMask); } @@ -964,23 +964,23 @@ public static unsafe void GatherAndIntegrate(ref bodyIndices, count, false, out position, out orientation, out velocity, out var localInertia); - IntegrateVelocity(ref integratorCallbacks, ref bodyIndices, count, localInertia, dt, integrationMask, position, orientation, ref velocity, workerIndex, out inertia); + var integrationMask = Vector.GreaterThanOrEqual(bodyIndices, Vector.Zero); + bodies.GatherState(ref bodyIndices, false, out position, out orientation, out velocity, out var localInertia); + IntegrateVelocity(ref integratorCallbacks, ref bodyIndices, localInertia, dt, integrationMask, position, orientation, ref velocity, workerIndex, out inertia); bodies.ScatterInertia(ref inertia, ref bodyIndices, ref integrationMask); } else if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) { - bodies.GatherState(ref bodyIndices, count, true, out position, out orientation, out velocity, out inertia); + bodies.GatherState(ref bodyIndices, true, out position, out orientation, out velocity, out inertia); } else { Debug.Assert(typeof(TBatchIntegrationMode) == typeof(BatchShouldConditionallyIntegrate)); var bundleIntegrationMode = BundleShouldIntegrate(bundleIndex, integrationFlags[bodyIndexInConstraint], out var integrationMask); - bodies.GatherState(ref bodyIndices, count, bundleIntegrationMode == BundleIntegrationMode.None, out position, out orientation, out velocity, out var gatheredInertia); + bodies.GatherState(ref bodyIndices, bundleIntegrationMode == BundleIntegrationMode.None, out position, out orientation, out velocity, out var gatheredInertia); if (bundleIntegrationMode != BundleIntegrationMode.None) { - IntegrateVelocity(ref integratorCallbacks, ref bodyIndices, count, gatheredInertia, dt, integrationMask, position, orientation, ref velocity, workerIndex, out inertia); + IntegrateVelocity(ref integratorCallbacks, ref bodyIndices, gatheredInertia, dt, integrationMask, position, orientation, ref velocity, workerIndex, out inertia); bodies.ScatterInertia(ref inertia, ref bodyIndices, ref integrationMask); } else diff --git a/BepuPhysics/PoseIntegrator.cs b/BepuPhysics/PoseIntegrator.cs index 3e197dbd9..7bdc3b47d 100644 --- a/BepuPhysics/PoseIntegrator.cs +++ b/BepuPhysics/PoseIntegrator.cs @@ -93,7 +93,7 @@ public interface IPoseIntegratorCallbacks /// Durations to integrate the velocity over. Can vary over lanes. /// Velocity of bodies in the bundle. Any changes to lanes which are not active by the integrationMask will be discarded. void IntegrateVelocity( - ReadOnlySpan bodyIndices, in Vector3Wide position, in QuaternionWide orientation, in BodyInertiaWide localInertia, + in Vector bodyIndices, in Vector3Wide position, in QuaternionWide orientation, in BodyInertiaWide localInertia, in Vector integrationMask, int workerIndex, in Vector dt, ref BodyVelocityWide velocity); } @@ -756,6 +756,7 @@ unsafe void IntegrateBundlesAfterSubstepping(ref IndexSet mergedConstrainedBodyH Vector bodyIndices; int* unconstrainedMaskPointer = (int*)&unconstrainedMask; int* bodyIndicesPointer = (int*)&bodyIndices; + var negativeOne = new Vector(-1); ref var callbacks = ref Callbacks; ref var indexToHandle = ref bodies.ActiveSet.IndexToHandle; @@ -770,6 +771,7 @@ unsafe void IntegrateBundlesAfterSubstepping(ref IndexSet mergedConstrainedBodyH //Unconstrained bodies can optionally perform a single step for the whole timestep, or do multiple steps to match the integration behavior of constrained bodies. //Bodies that are constrained should only undergo one substep of pose integration. bool anyBodyInBundleIsUnconstrained = false; + bodyIndices = negativeOne; //Initialize bundles to -1 so that inactive lanes are consistent with the active set's storage of body references (empty lanes are -1) for (int innerIndex = 0; innerIndex < countInBundle; ++innerIndex) { var bodyIndex = bundleBaseIndex + innerIndex; @@ -799,7 +801,7 @@ unsafe void IntegrateBundlesAfterSubstepping(ref IndexSet mergedConstrainedBodyH bundleEffectiveDt = Vector.ConditionalSelect(unconstrainedMask, bundleDt, bundleSubstepDt); } var halfDt = bundleEffectiveDt * new Vector(0.5f); - bodies.GatherState(ref bodyIndices, countInBundle, false, out var position, out var orientation, out var velocity, out var localInertia); + bodies.GatherState(ref bodyIndices, false, out var position, out var orientation, out var velocity, out var localInertia); if (anyBodyInBundleIsUnconstrained) { int integrationStepCount; @@ -815,7 +817,7 @@ unsafe void IntegrateBundlesAfterSubstepping(ref IndexSet mergedConstrainedBodyH { //Note that the following integrates velocities, then poses. var previousVelocity = velocity; - callbacks.IntegrateVelocity(new ReadOnlySpan(Unsafe.AsPointer(ref bodyIndices), countInBundle), position, orientation, localInertia, unconstrainedMask, workerIndex, bundleEffectiveDt, ref velocity); + callbacks.IntegrateVelocity(bodyIndices, position, orientation, localInertia, unconstrainedMask, workerIndex, bundleEffectiveDt, ref velocity); //It would be annoying to make the user handle masking velocity writes to inactive lanes, so we handle it internally. Vector3Wide.ConditionalSelect(unconstrainedMask, velocity.Linear, previousVelocity.Linear, out velocity.Linear); Vector3Wide.ConditionalSelect(unconstrainedMask, velocity.Angular, previousVelocity.Angular, out velocity.Angular); @@ -854,7 +856,7 @@ unsafe void IntegrateBundlesAfterSubstepping(ref IndexSet mergedConstrainedBodyH } bodies.ScatterPose(ref position, ref orientation, ref bodyIndices, ref integratePoseMask); //We already masked the velocities above, so scattering them doesn't need its own mask. - bodies.ScatterVelocities(ref velocity, ref bodyIndices, countInBundle); + bodies.ScatterVelocities(ref velocity, ref bodyIndices); } } else diff --git a/Demos/DemoCallbacks.cs b/Demos/DemoCallbacks.cs index 226d73d0d..6427d5ba3 100644 --- a/Demos/DemoCallbacks.cs +++ b/Demos/DemoCallbacks.cs @@ -93,7 +93,7 @@ public void IntegrateVelocity(int bodyIndex, in RigidPose pose, in BodyInertia l } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IntegrateVelocity(ReadOnlySpan bodyIndices, in Vector3Wide position, in QuaternionWide orientation, in BodyInertiaWide localInertia, in Vector integrationMask, int workerIndex, in Vector dt, ref BodyVelocityWide velocity) + public void IntegrateVelocity(in Vector bodyIndices, in Vector3Wide position, in QuaternionWide orientation, in BodyInertiaWide localInertia, in Vector integrationMask, int workerIndex, in Vector dt, ref BodyVelocityWide velocity) { velocity.Linear = Vector3Wide.ConditionalSelect(Vector.Equals(localInertia.InverseMass, Vector.Zero), velocity.Linear, velocity.Linear + gravityWide * dt); } diff --git a/Demos/Demos/PlanetDemo.cs b/Demos/Demos/PlanetDemo.cs index d6724c509..3603aa146 100644 --- a/Demos/Demos/PlanetDemo.cs +++ b/Demos/Demos/PlanetDemo.cs @@ -52,7 +52,7 @@ public void IntegrateVelocity(int bodyIndex, in RigidPose pose, in BodyInertia l } } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IntegrateVelocity(ReadOnlySpan bodyIndices, in Vector3Wide position, in QuaternionWide orientation, in BodyInertiaWide localInertia, in Vector integrationMask, int workerIndex, in Vector dt, ref BodyVelocityWide velocity) + public void IntegrateVelocity(in Vector bodyIndices, in Vector3Wide position, in QuaternionWide orientation, in BodyInertiaWide localInertia, in Vector integrationMask, int workerIndex, in Vector dt, ref BodyVelocityWide velocity) { } } diff --git a/Demos/Demos/SimpleSelfContainedDemo.cs b/Demos/Demos/SimpleSelfContainedDemo.cs index 635b038c1..228e342e0 100644 --- a/Demos/Demos/SimpleSelfContainedDemo.cs +++ b/Demos/Demos/SimpleSelfContainedDemo.cs @@ -182,7 +182,7 @@ public void IntegrateVelocity(int bodyIndex, in RigidPose pose, in BodyInertia l } } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IntegrateVelocity(ReadOnlySpan bodyIndices, in Vector3Wide position, in QuaternionWide orientation, in BodyInertiaWide localInertia, in Vector integrationMask, int workerIndex, in Vector dt, ref BodyVelocityWide velocity) + public void IntegrateVelocity(in Vector bodyIndices, in Vector3Wide position, in QuaternionWide orientation, in BodyInertiaWide localInertia, in Vector integrationMask, int workerIndex, in Vector dt, ref BodyVelocityWide velocity) { } diff --git a/Demos/SpecializedTests/GyroscopeTestDemo.cs b/Demos/SpecializedTests/GyroscopeTestDemo.cs index 3309fe9f3..ea03a8a99 100644 --- a/Demos/SpecializedTests/GyroscopeTestDemo.cs +++ b/Demos/SpecializedTests/GyroscopeTestDemo.cs @@ -44,7 +44,7 @@ public void IntegrateVelocity(int bodyIndex, in RigidPose pose, in BodyInertia l innerCallbacks.IntegrateVelocity(bodyIndex, pose, localInertia, workerIndex, ref velocity); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IntegrateVelocity(ReadOnlySpan bodyIndices, in Vector3Wide position, in QuaternionWide orientation, in BodyInertiaWide localInertia, in Vector integrationMask, int workerIndex, in Vector dt, ref BodyVelocityWide velocity) + public void IntegrateVelocity(in Vector bodyIndices, in Vector3Wide position, in QuaternionWide orientation, in BodyInertiaWide localInertia, in Vector integrationMask, int workerIndex, in Vector dt, ref BodyVelocityWide velocity) { } From f141c6844644e77529505f0f0d5e72461039f560 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 15 Oct 2021 13:39:44 -0500 Subject: [PATCH 211/947] Purged now unnecessary count calculations. --- .../Constraints/FourBodyTypeProcessor.cs | 6 ++-- .../Constraints/OneBodyTypeProcessor.cs | 6 ++-- .../Constraints/ThreeBodyTypeProcessor.cs | 6 ++-- .../Constraints/TwoBodyTypeProcessor.cs | 6 ++-- BepuPhysics/Constraints/TypeProcessor.cs | 33 +++---------------- BepuPhysics/Solver_SubsteppingSolve2.cs | 16 ++++----- 6 files changed, 24 insertions(+), 49 deletions(-) diff --git a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs index 1d4e6916a..e08fa06ff 100644 --- a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs @@ -193,8 +193,8 @@ public unsafe override void SolveIteration(ref TypeBatch typeBatch, Bodies bodie } } - public unsafe override void WarmStart2( - ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, in TBundleCountCalculator bundleCountCalculator, + public unsafe override void WarmStart2( + ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { var prestepBundles = typeBatch.PrestepData.As(); @@ -241,7 +241,7 @@ public unsafe override void WarmStart2(ref TypeBatch typeBatch, Bodies bodies, in TBundleCountCalculator bundleCountCalculator, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { var prestepBundles = typeBatch.PrestepData.As(); var bodyReferencesBundles = typeBatch.BodyReferences.As(); diff --git a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs index ba1f73865..5a469d5d0 100644 --- a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs @@ -151,8 +151,8 @@ public unsafe override void SolveIteration(ref TypeBatch typeBatch, Bodies bodie } } - public unsafe override void WarmStart2( - ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, in TBundleCountCalculator bundleCountCalculator, + public unsafe override void WarmStart2( + ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { var prestepBundles = typeBatch.PrestepData.As(); @@ -186,7 +186,7 @@ public unsafe override void WarmStart2(ref TypeBatch typeBatch, Bodies bodies, in TBundleCountCalculator bundleCountCalculator, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { var prestepBundles = typeBatch.PrestepData.As(); var bodyReferencesBundles = typeBatch.BodyReferences.As>(); diff --git a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs index f3bec1722..aded4a47c 100644 --- a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs @@ -177,8 +177,8 @@ public unsafe override void SolveIteration(ref TypeBatch typeBatch, Bodies bodie bodies.ScatterVelocities(ref wsvC, ref bodyReferences.IndexC); } } - public unsafe override void WarmStart2( - ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, in TBundleCountCalculator bundleCountCalculator, + public unsafe override void WarmStart2( + ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { var prestepBundles = typeBatch.PrestepData.As(); @@ -221,7 +221,7 @@ public unsafe override void WarmStart2(ref TypeBatch typeBatch, Bodies bodies, in TBundleCountCalculator bundleCountCalculator, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { var prestepBundles = typeBatch.PrestepData.As(); var bodyReferencesBundles = typeBatch.BodyReferences.As(); diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 22393f239..93387939a 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -251,8 +251,8 @@ public unsafe override void SolveIteration(ref TypeBatch typeBatch, Bodies bodie - public unsafe override void WarmStart2( - ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, in TBundleCountCalculator bundleCountCalculator, + public unsafe override void WarmStart2( + ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { var prestepBundles = typeBatch.PrestepData.As(); @@ -293,7 +293,7 @@ public unsafe override void WarmStart2(ref TypeBatch typeBatch, Bodies bodies, in TBundleCountCalculator bundleCountCalculator, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { var prestepBundles = typeBatch.PrestepData.As(); var bodyReferencesBundles = typeBatch.BodyReferences.As(); diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index 630ccc6d9..4ae9a20a8 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -10,29 +10,6 @@ namespace BepuPhysics.Constraints { - public interface IConstraintBundleCountCalculator - { - int GetCountInBundle(ref TypeBatch typeBatch, int bundleIndex); - } - - public struct DenseBundleCountCalculator : IConstraintBundleCountCalculator - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public readonly int GetCountInBundle(ref TypeBatch typeBatch, int bundleIndex) - { - return TypeProcessor.GetCountInBundle(ref typeBatch, bundleIndex); - } - } - - public struct SequentialFallbackBundleCountCalculator : IConstraintBundleCountCalculator - { - Buffer countsInBundles; - public readonly int GetCountInBundle(ref TypeBatch typeBatch, int bundleIndex) - { - return countsInBundles[bundleIndex]; - } - } - /// /// Superclass of constraint type batch processors. Responsible for interpreting raw type batches for the purposes of bookkeeping and solving. /// @@ -153,15 +130,13 @@ internal unsafe abstract void CopySleepingToActive( public abstract void WarmStart(ref TypeBatch typeBatch, Bodies bodies, int startBundle, int exclusiveEndBundle); public abstract void SolveIteration(ref TypeBatch typeBatch, Bodies bodies, int startBundle, int exclusiveEndBundle); - public abstract void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, - ref TIntegratorCallbacks poseIntegratorCallbacks, in TCountInBundleCalculator countInBundleCalculator, + public abstract void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, + ref TIntegratorCallbacks poseIntegratorCallbacks, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) where TIntegratorCallbacks : struct, IPoseIntegratorCallbacks where TBatchIntegrationMode : unmanaged, IBatchIntegrationMode - where TAllowPoseIntegration : unmanaged, IBatchPoseIntegrationAllowed - where TCountInBundleCalculator : unmanaged, IConstraintBundleCountCalculator; - public abstract void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, in TCountInBundleCalculator countInBundleCalculator, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) - where TCountInBundleCalculator : unmanaged, IConstraintBundleCountCalculator; + where TAllowPoseIntegration : unmanaged, IBatchPoseIntegrationAllowed; + public abstract void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); public virtual void IncrementallyUpdateContactData(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int end) { diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index 0f26d3e44..d0642b52c 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -112,22 +112,22 @@ private void WarmStartBlock(int workerIndex, int bat if (batchIndex == 0) { Buffer noFlagsRequired = default; - typeProcessor.WarmStart2( - ref typeBatch, ref noFlagsRequired, bodies, ref PoseIntegrator.Callbacks, new DenseBundleCountCalculator(), + typeProcessor.WarmStart2( + ref typeBatch, ref noFlagsRequired, bodies, ref PoseIntegrator.Callbacks, dt, inverseDt, startBundle, endBundle, workerIndex); } else { if (coarseBatchIntegrationResponsibilities[batchIndex][typeBatchIndex]) { - typeProcessor.WarmStart2( - ref typeBatch, ref integrationFlags[batchIndex][typeBatchIndex], bodies, ref PoseIntegrator.Callbacks, new DenseBundleCountCalculator(), + typeProcessor.WarmStart2( + ref typeBatch, ref integrationFlags[batchIndex][typeBatchIndex], bodies, ref PoseIntegrator.Callbacks, dt, inverseDt, startBundle, endBundle, workerIndex); } else { - typeProcessor.WarmStart2( - ref typeBatch, ref integrationFlags[batchIndex][typeBatchIndex], bodies, ref PoseIntegrator.Callbacks, new DenseBundleCountCalculator(), + typeProcessor.WarmStart2( + ref typeBatch, ref integrationFlags[batchIndex][typeBatchIndex], bodies, ref PoseIntegrator.Callbacks, dt, inverseDt, startBundle, endBundle, workerIndex); } } @@ -173,7 +173,7 @@ public void Execute(Solver solver, int blockIndex, int workerIndex) ref var block = ref this.solver.substepContext.ConstraintBlocks[blockIndex]; ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; - typeProcessor.SolveStep2(ref typeBatch, solver.bodies, new DenseBundleCountCalculator(), Dt, InverseDt, block.StartBundle, block.End); + typeProcessor.SolveStep2(ref typeBatch, solver.bodies, Dt, InverseDt, block.StartBundle, block.End); } } @@ -1090,7 +1090,7 @@ public override void SolveStep2(float totalDt, IThreadDispatcher threadDispatche for (int j = 0; j < batch.TypeBatches.Count; ++j) { ref var typeBatch = ref batch.TypeBatches[j]; - TypeProcessors[typeBatch.TypeId].SolveStep2(ref typeBatch, bodies, new DenseBundleCountCalculator(), substepDt, inverseDt, 0, typeBatch.BundleCount); + TypeProcessors[typeBatch.TypeId].SolveStep2(ref typeBatch, bodies, substepDt, inverseDt, 0, typeBatch.BundleCount); } } } From 3c87cfb22849824f1885acf632a78583f19fd5e0 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 15 Oct 2021 16:15:02 -0500 Subject: [PATCH 212/947] Added fallback type batch allocation. --- BepuPhysics/ConstraintBatch.cs | 26 --- BepuPhysics/Constraints/TypeProcessor.cs | 194 +++++++++++++++++++++-- BepuPhysics/Solver.cs | 28 +++- 3 files changed, 209 insertions(+), 39 deletions(-) diff --git a/BepuPhysics/ConstraintBatch.cs b/BepuPhysics/ConstraintBatch.cs index c5a9c81cc..6be71844e 100644 --- a/BepuPhysics/ConstraintBatch.cs +++ b/BepuPhysics/ConstraintBatch.cs @@ -143,32 +143,6 @@ public unsafe ref TypeBatch GetTypeBatch(int typeId) } } - public unsafe void Allocate(ConstraintHandle handle, Span constraintBodyHandles, Bodies bodies, - int typeId, TypeProcessor typeProcessor, int initialCapacity, BufferPool pool, out ConstraintReference reference) - { - //Add all the constraint's body handles to the batch we found (or created) to block future references to the same bodies. - //Also, convert the handle into a memory index. Constraints store a direct memory reference for performance reasons. - var bodyIndices = stackalloc int[constraintBodyHandles.Length]; - for (int j = 0; j < constraintBodyHandles.Length; ++j) - { - var bodyHandle = constraintBodyHandles[j]; - ref var location = ref bodies.HandleToLocation[bodyHandle.Value]; - Debug.Assert(location.SetIndex == 0, "Creating a new constraint should have forced the connected bodies awake."); - bodyIndices[j] = location.Index; - } - var typeBatch = GetOrCreateTypeBatch(typeId, typeProcessor, initialCapacity, pool); - reference = new ConstraintReference(typeBatch, typeProcessor.Allocate(ref *typeBatch, handle, bodyIndices, pool)); - //TODO: We could adjust the typeBatchAllocation capacities in response to the allocated index. - //If it exceeds the current capacity, we could ensure the new size is still included. - //The idea here would be to avoid resizes later by ensuring that the historically encountered size is always used to initialize. - //This isn't necessarily beneficial, though- often, higher indexed batches will contain smaller numbers of constraints, so allocating a huge number - //of constraints into them is very low value. You may want to be a little more clever about the heuristic. Either way, only bother with this once there is - //evidence that typebatch resizes are ever a concern. This will require frame spike analysis, not merely average timings. - //(While resizes will definitely occur, remember that it only really matters for *new* type batches- - //and it is rare that a new type batch will be created that actually needs to be enormous.) - } - - unsafe struct ActiveBodyHandleRemover : IForEach { public Bodies Bodies; diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index 4ae9a20a8..cf2173d83 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -49,14 +49,24 @@ public void Initialize(int typeId) } /// - /// Allocates a slot in the batch. + /// Allocates a slot in the batch, assuming the batch is not a fallback batch. /// /// Type batch to allocate in. /// Handle of the constraint to allocate. Establishes a link from the allocated constraint to its handle. /// Pointer to a list of body indices (not handles!) with count equal to the type batch's expected number of involved bodies. /// Allocation provider to use if the type batch has to be resized. /// Index of the slot in the batch. - public unsafe abstract int Allocate(ref TypeBatch typeBatch, ConstraintHandle handle, int* bodyIndices, BufferPool pool); + public unsafe abstract int AllocateInTypeBatch(ref TypeBatch typeBatch, ConstraintHandle handle, int* bodyIndices, BufferPool pool); + + /// + /// Allocates a slot in the batch, assuming the batch is a fallback batch. + /// + /// Type batch to allocate in. + /// Handle of the constraint to allocate. Establishes a link from the allocated constraint to its handle. + /// Pointer to a list of body indices (not handles!) with count equal to the type batch's expected number of involved bodies. + /// Allocation provider to use if the type batch has to be resized. + /// Index of the slot in the batch. + public unsafe abstract int AllocateInTypeBatchForFallback(ref TypeBatch typeBatch, ConstraintHandle handle, int* bodyIndices, BufferPool pool); public abstract void Remove(ref TypeBatch typeBatch, int index, ref Buffer handlesToConstraints); /// @@ -191,6 +201,21 @@ public override unsafe void ScaleAccumulatedImpulses(ref TypeBatch typeBatch, fl } } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public unsafe static void SetBodyReferencesLane(ref TBodyReferences bundle, int innerIndex, int* bodyIndices) + { + //The jit should be able to fold almost all of the size-related calculations and address fiddling. + var bodyCount = Unsafe.SizeOf() / (Vector.Count * sizeof(int)); + //We assume that the body references struct is organized in memory like Bundle0, Inner0, ... BundleN, InnerN, Count + //Assuming contiguous storage, Count is then located at start + stride * BodyCount. + ref var start = ref Unsafe.As(ref bundle); + ref var targetLane = ref Unsafe.Add(ref start, innerIndex); + targetLane = *bodyIndices; + for (int i = 1; i < bodyCount; ++i) + { + Unsafe.Add(ref targetLane, i * Vector.Count) = bodyIndices[i]; + } + } [MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe static void AddBodyReferencesLane(ref TBodyReferences bundle, int innerIndex, int* bodyIndices) @@ -207,15 +232,7 @@ public unsafe static void AddBodyReferencesLane(ref TBodyReferences bundle, int Unsafe.Add(ref bodyReferenceBundle, i) = negativeOne; } } - //We assume that the body references struct is organized in memory like Bundle0, Inner0, ... BundleN, InnerN, Count - //Assuming contiguous storage, Count is then located at start + stride * BodyCount. - ref var start = ref Unsafe.As(ref bundle); - ref var targetLane = ref Unsafe.Add(ref start, innerIndex); - targetLane = *bodyIndices; - for (int i = 1; i < bodyCount; ++i) - { - Unsafe.Add(ref targetLane, i * Vector.Count) = bodyIndices[i]; - } + SetBodyReferencesLane(ref bundle, innerIndex, bodyIndices); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -232,7 +249,7 @@ public unsafe static void RemoveBodyReferencesLane(ref TBodyReferences bundle, i } - public unsafe sealed override int Allocate(ref TypeBatch typeBatch, ConstraintHandle handle, int* bodyIndices, BufferPool pool) + public unsafe sealed override int AllocateInTypeBatch(ref TypeBatch typeBatch, ConstraintHandle handle, int* bodyIndices, BufferPool pool) { Debug.Assert(typeBatch.BodyReferences.Allocated, "Should initialize the batch before allocating anything from it."); if (typeBatch.ConstraintCount == typeBatch.IndexToHandle.Length) @@ -255,6 +272,159 @@ public unsafe sealed override int Allocate(ref TypeBatch typeBatch, ConstraintHa return index; } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + private unsafe static bool AllowFallbackBundleAllocation(ref TBodyReferences bundle, Vector* broadcastedBodyIndices) + { + //TODO: depending on codegen, there's a chance that doing special cases for the 1, 2, 3, and 4 body cases would be worth it. No need for loop jumps and such. + //The type batches are always held in pinned memory, this is not a GC hole. + var bundleBodyIndices = (Vector*)Unsafe.AsPointer(ref bundle); + var bodiesPerConstraint = Unsafe.SizeOf() / Vector.Count; //redundant, but folds. + for (int broadcastedBundleBodyInConstraint = 0; broadcastedBundleBodyInConstraint < bodiesPerConstraint; ++broadcastedBundleBodyInConstraint) + { + var broadcastedBodies = broadcastedBodyIndices[broadcastedBundleBodyInConstraint]; + for (int bundleBodyIndexInConstraint = 0; bundleBodyIndexInConstraint < bodiesPerConstraint; ++bundleBodyIndexInConstraint) + { + if (Vector.EqualsAny(bundleBodyIndices[bundleBodyIndexInConstraint], broadcastedBodies)) + { + return false; + } + } + } + //The new allocation is not blocked by matching indices, but is there room? At least one slot would have to have -1s in it. + return Vector.LessThanAny(*bundleBodyIndices, Vector.Zero); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + private unsafe static int GetInnerIndexForFallbackAllocation(ref TBodyReferences bundle) + { + //The type batches are always held in pinned memory, this is not a GC hole. + var bundleBodyIndices = Unsafe.As>(ref bundle); + //Choose the first empty slot as the allocation target. This requires picking the lowest index lane that contains -1. + var mask = Vector.LessThan(bundleBodyIndices, Vector.Zero); + if (Avx.IsSupported && Vector.Count == 8 && Bmi1.IsSupported) + { + var scalarMask = Avx.MoveMask(mask.AsVector256().As()); + return (int)Bmi1.TrailingZeroCount((uint)scalarMask); + } + else if (Sse.IsSupported && Vector.Count == 4 && Bmi1.IsSupported) + { + var scalarMask = Sse.MoveMask(mask.AsVector128().As()); + return (int)Bmi1.TrailingZeroCount((uint)scalarMask); + } + else + { + Debug.Assert(Vector.Count <= 8, "We made an assumption that AVX512 and similar widths aren't available, this should be updated if vectors get wider!"); + for (int i = 0; i < Vector.Count; ++i) + { + if (mask[i] == -1) + return i; + } + } + Debug.Fail("This should not be possible; this function should only be called if at least one element of the mask was -1."); + return -1; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + private unsafe static bool ProbeBundleForFallback(Buffer typeBatchBodyIndices, Vector* broadcastedBodyIndices, int* bodyIndices, int bundleIndex, ref int targetBundleIndex, ref int targetInnerIndex) + { + ref var bundle = ref typeBatchBodyIndices[bundleIndex]; + if (AllowFallbackBundleAllocation(ref bundle, broadcastedBodyIndices)) + { + //We've found a place to put the allocation. + targetBundleIndex = bundleIndex; + targetInnerIndex = GetInnerIndexForFallbackAllocation(ref bundle); + SetBodyReferencesLane(ref bundle, targetInnerIndex, bodyIndices); + return true; + } + return false; + } + + public unsafe sealed override int AllocateInTypeBatchForFallback(ref TypeBatch typeBatch, ConstraintHandle handle, int* bodyIndices, BufferPool pool) + { + Debug.Assert(typeBatch.BodyReferences.Allocated, "Should initialize the batch before allocating anything from it."); + if (typeBatch.ConstraintCount == typeBatch.IndexToHandle.Length) + { + InternalResize(ref typeBatch, pool, typeBatch.ConstraintCount * 2); + } + //The sequential fallback batch has different allocation rules. + //Allocation must guarantee that a constraint does not fall into a bundle which shares any of the same body references. + //(That responsibility usually falls on batch referenced handles blocking new constraints, + //but the fallback exists precisely because the simulation is asking for pathological numbers of constraints affecting the same body.) + const int probeLocationCount = 8; + //Note that this only ever executes for the active set, so body references are indices. + var typeBatchBodyIndices = typeBatch.BodyReferences.As(); + int targetBundleIndex = -1; + int targetInnerIndex = -1; + //This folds. + var bodiesPerConstraint = Unsafe.SizeOf() / Vector.Count; + var broadcastedBodyIndices = stackalloc Vector[bodiesPerConstraint]; + for (int bodyIndexInConstraint = 0; bodyIndexInConstraint < bodiesPerConstraint; ++bodyIndexInConstraint) + { + broadcastedBodyIndices[bodyIndexInConstraint] = new Vector(bodyIndices[bodyIndexInConstraint]); + } + var bundleCount = typeBatch.BundleCount; + if (bundleCount <= probeLocationCount) + { + //The fallback batch is small; there's no need to do a stochastic insertion. Just enumerate all bundles. + for (int bundleIndexInTypeBatch = 0; bundleIndexInTypeBatch < bundleCount; ++bundleIndexInTypeBatch) + { + if (ProbeBundleForFallback(typeBatchBodyIndices, broadcastedBodyIndices, bodyIndices, bundleIndexInTypeBatch, ref targetBundleIndex, ref targetInnerIndex)) + break; + } + } + else + { + //The fallback batch is large enough to warrant stochastic probing. + //The idea here is that fallback batches very often involve a single body over and over and over. + //Scanning every single bundle to find a spot would be needlessly expensive given the most common use case. + //Instead, we probe a few locations and then give up. + //First, probe the final bundle just in case it's nice and simple, then do stochastic probes. + //Stochastic probing works by pseudorandomly choosing a starting point, then picking probe locations based on that starting point. + var lastBundleIndex = bundleCount - 1; + if (!ProbeBundleForFallback(typeBatchBodyIndices, broadcastedBodyIndices, bodyIndices, lastBundleIndex, ref targetBundleIndex, ref targetInnerIndex)) + { + //No room in the final bundle; keep looking with stochastic probes. + var nextProbeIndex = HashHelper.Rehash(handle.Value) % lastBundleIndex; + var bundleJump = bundleCount / probeLocationCount; + var remainder = bundleCount - bundleJump * probeLocationCount; + for (int probeIndex = 0; probeIndex < probeLocationCount; ++probeIndex) + { + if (ProbeBundleForFallback(typeBatchBodyIndices, broadcastedBodyIndices, bodyIndices, nextProbeIndex, ref targetBundleIndex, ref targetInnerIndex)) + break; + nextProbeIndex += bundleJump; + if (probeIndex < remainder) + ++nextProbeIndex; + if (nextProbeIndex >= bundleCount) + nextProbeIndex -= bundleCount; + } + } + } + if (targetBundleIndex == -1) + { + //None of the existing bundles can hold the constraint; we need a new one. + //Since a new bundle is guaranteed to not contain any other constraints, we can reuse the non-fallback logic. + return AllocateInTypeBatch(ref typeBatch, handle, bodyIndices, pool); + } + else + { + //Clear the slot's accumulated impulse. The backing memory could be initialized to any value. + GatherScatter.ClearLane(ref Buffer.Get(ref typeBatch.AccumulatedImpulses, targetBundleIndex), targetInnerIndex); + var indexInTypeBatch = targetBundleIndex * Vector.Count + targetInnerIndex; + //If the constraint was added after the highest index currently existing constraint, the constraint count needs to be boosted. + typeBatch.ConstraintCount = Math.Max(indexInTypeBatch + 1, typeBatch.ConstraintCount); + Debug.Assert(typeBatch.IndexToHandle.Length >= typeBatch.ConstraintCount); + typeBatch.IndexToHandle[indexInTypeBatch] = handle; + Debug.Assert(typeBatch.ConstraintCount <= typeBatch.IndexToHandle.Length); + Debug.Assert(typeBatch.PrestepData.Length >= bundleCount * Unsafe.SizeOf()); + Debug.Assert(typeBatch.Projection.Length >= bundleCount * Unsafe.SizeOf()); + Debug.Assert(typeBatch.BodyReferences.Length >= bundleCount * Unsafe.SizeOf()); + Debug.Assert(typeBatch.AccumulatedImpulses.Length >= bundleCount * Unsafe.SizeOf()); + return indexInTypeBatch; + } + + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] protected static void CopyConstraintData( ref TBodyReferences sourceReferencesBundle, ref TPrestepData sourcePrestepBundle, ref TAccumulatedImpulse sourceAccumulatedBundle, int sourceInner, diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index 5bd3100d4..35eb8a40f 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -501,7 +501,33 @@ internal unsafe int FindCandidateBatch(Span bodyHandles) internal unsafe void AllocateInBatch(int targetBatchIndex, ConstraintHandle constraintHandle, Span bodyHandles, int typeId, out ConstraintReference reference) { ref var batch = ref ActiveSet.Batches[targetBatchIndex]; - batch.Allocate(constraintHandle, bodyHandles, bodies, typeId, TypeProcessors[typeId], GetMinimumCapacityForType(typeId), pool, out reference); + //Add all the constraint's body handles to the batch we found (or created) to block future references to the same bodies. + //Also, convert the handle into a memory index. Constraints store a direct memory reference for performance reasons. + var bodyIndices = stackalloc int[bodyHandles.Length]; + for (int j = 0; j < bodyHandles.Length; ++j) + { + var bodyHandle = bodyHandles[j]; + ref var location = ref bodies.HandleToLocation[bodyHandle.Value]; + Debug.Assert(location.SetIndex == 0, "Creating a new constraint should have forced the connected bodies awake."); + bodyIndices[j] = location.Index; + } + var typeProcessor = TypeProcessors[typeId]; + var typeBatch = batch.GetOrCreateTypeBatch(typeId, typeProcessor, GetMinimumCapacityForType(typeId), pool); + int indexInTypeBatch; + if (targetBatchIndex == FallbackBatchThreshold) + indexInTypeBatch = typeProcessor.AllocateInTypeBatchForFallback(ref *typeBatch, constraintHandle, bodyIndices, pool); + else + indexInTypeBatch = typeProcessor.AllocateInTypeBatch(ref *typeBatch, constraintHandle, bodyIndices, pool); + reference = new ConstraintReference(typeBatch, indexInTypeBatch); + //TODO: We could adjust the typeBatchAllocation capacities in response to the allocated index. + //If it exceeds the current capacity, we could ensure the new size is still included. + //The idea here would be to avoid resizes later by ensuring that the historically encountered size is always used to initialize. + //This isn't necessarily beneficial, though- often, higher indexed batches will contain smaller numbers of constraints, so allocating a huge number + //of constraints into them is very low value. You may want to be a little more clever about the heuristic. Either way, only bother with this once there is + //evidence that typebatch resizes are ever a concern. This will require frame spike analysis, not merely average timings. + //(While resizes will definitely occur, remember that it only really matters for *new* type batches- + //and it is rare that a new type batch will be created that actually needs to be enormous.) + ref var handlesSet = ref batchReferencedHandles[targetBatchIndex]; for (int i = 0; i < bodyHandles.Length; ++i) { From cbf0c66d680a65f18426df57f4fc2d80dfa0b5ea Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 15 Oct 2021 17:39:47 -0500 Subject: [PATCH 213/947] Fixed a fallback allocation bug; added the meat of fallback removal. --- BepuPhysics/Constraints/TypeProcessor.cs | 89 ++++++++++++++++++------ BepuUtilities/BundleIndexing.cs | 52 ++++++++++++++ 2 files changed, 118 insertions(+), 23 deletions(-) diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index cf2173d83..22df0a5c6 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -301,28 +301,7 @@ private unsafe static int GetInnerIndexForFallbackAllocation(ref TBodyReferences //The type batches are always held in pinned memory, this is not a GC hole. var bundleBodyIndices = Unsafe.As>(ref bundle); //Choose the first empty slot as the allocation target. This requires picking the lowest index lane that contains -1. - var mask = Vector.LessThan(bundleBodyIndices, Vector.Zero); - if (Avx.IsSupported && Vector.Count == 8 && Bmi1.IsSupported) - { - var scalarMask = Avx.MoveMask(mask.AsVector256().As()); - return (int)Bmi1.TrailingZeroCount((uint)scalarMask); - } - else if (Sse.IsSupported && Vector.Count == 4 && Bmi1.IsSupported) - { - var scalarMask = Sse.MoveMask(mask.AsVector128().As()); - return (int)Bmi1.TrailingZeroCount((uint)scalarMask); - } - else - { - Debug.Assert(Vector.Count <= 8, "We made an assumption that AVX512 and similar widths aren't available, this should be updated if vectors get wider!"); - for (int i = 0; i < Vector.Count; ++i) - { - if (mask[i] == -1) - return i; - } - } - Debug.Fail("This should not be possible; this function should only be called if at least one element of the mask was -1."); - return -1; + return BundleIndexing.GetFirstSetLaneIndex(Vector.LessThan(bundleBodyIndices, Vector.Zero)); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -385,7 +364,7 @@ public unsafe sealed override int AllocateInTypeBatchForFallback(ref TypeBatch t if (!ProbeBundleForFallback(typeBatchBodyIndices, broadcastedBodyIndices, bodyIndices, lastBundleIndex, ref targetBundleIndex, ref targetInnerIndex)) { //No room in the final bundle; keep looking with stochastic probes. - var nextProbeIndex = HashHelper.Rehash(handle.Value) % lastBundleIndex; + var nextProbeIndex = (HashHelper.Rehash(handle.Value) & 0x7FFF_FFFF) % lastBundleIndex; var bundleJump = bundleCount / probeLocationCount; var remainder = bundleCount - bundleJump * probeLocationCount; for (int probeIndex = 0; probeIndex < probeLocationCount; ++probeIndex) @@ -517,6 +496,70 @@ ref Unsafe.Add(ref bodyReferences, targetBundleIndex), ref Unsafe.Add(ref preste RemoveBodyReferencesLane(ref Unsafe.Add(ref bodyReferences, sourceBundleIndex), sourceInnerIndex); } + /// + /// Removes a constraint from the batch, assuming it belongs to a fallback batch. + /// + /// Index of the constraint to remove. + /// The handle to constraint mapping used by the solver that could be modified by a swap on removal. + public unsafe void RemoveForFallback(ref TypeBatch typeBatch, int index, ref Buffer handlesToConstraints) + { + Debug.Assert(index >= 0 && index < typeBatch.ConstraintCount, "Can only remove elements that are actually in the batch!"); + //The fallback batch does not guarantee contiguity of constraints, only contiguity of *bundles*. + //Bundles may be incomplete. + //We must guarantee that a bundle never contains references to the same body more than once. + //So, it's not safe to simply pull the last constraint into the removed slot. + //Instead, remove the constraint from whatever bundle it's in, and if it is empty afterwards, pull the whole last bundle into its position. + BundleIndexing.GetBundleIndices(index, out var removedBundleIndex, out var removedInnerIndex); + var bodyReferences = typeBatch.BodyReferences.As(); + ref var removedBundleSlot = ref bodyReferences[removedBundleIndex]; + RemoveBodyReferencesLane(ref removedBundleSlot, removedInnerIndex); + var firstBodyReferences = Unsafe.As>(ref removedBundleSlot); + if (Vector.LessThanAll(firstBodyReferences, Vector.Zero)) + { + //All slots in the bundle are now empty; this bundle should be removed. + var lastBundleIndex = typeBatch.BundleCount - 1; + if (removedBundleIndex != lastBundleIndex) + { + //There is a bundle to move into the now-dead bundle slot. + var prestepData = typeBatch.PrestepData.As(); + var accumulatedImpulses = typeBatch.PrestepData.As(); + prestepData[removedBundleIndex] = prestepData[lastBundleIndex]; + accumulatedImpulses[removedBundleIndex] = accumulatedImpulses[lastBundleIndex]; + bodyReferences[removedBundleIndex] = bodyReferences[lastBundleIndex]; + var firstBodyLaneForMovedBundle = (int*)Unsafe.AsPointer(ref bodyReferences[lastBundleIndex]); + //Update all constraint locations for the move. + var constraintIndexShift = (lastBundleIndex - removedBundleIndex) * Vector.Count; + var bundleStartIndexInConstraints = lastBundleIndex * Vector.Count; + for (int i = 0; i < Vector.Count; ++i) + { + if (firstBodyLaneForMovedBundle[i] >= 0) + { + //This constraint actually exists. + var constraintIndex = bundleStartIndexInConstraints + i; + var newConstraintIndex = constraintIndex - constraintIndexShift; + var handle = typeBatch.IndexToHandle[constraintIndex]; + typeBatch.IndexToHandle[newConstraintIndex] = handle; + handlesToConstraints[handle.Value].IndexInTypeBatch = newConstraintIndex; + } + } + //Removed the last bundle index, so drop back by one. + --lastBundleIndex; + } + if (lastBundleIndex >= 0) + { + //Calculate the new constraint count by getting the highest index in the new last bundle. + var lastInnerIndex = BundleIndexing.GetLastSetLaneIndex(Vector.LessThan(Unsafe.As>(ref bodyReferences[lastBundleIndex]), Vector.Zero)); + typeBatch.ConstraintCount = lastBundleIndex * Vector.Count + lastInnerIndex; + + } + else + { + //The last bundle was removed; there are no more constraints. + typeBatch.ConstraintCount = 0; + } + } + } + /// /// Moves a constraint from one ConstraintBatch's TypeBatch to another ConstraintBatch's TypeBatch of the same type. /// diff --git a/BepuUtilities/BundleIndexing.cs b/BepuUtilities/BundleIndexing.cs index e9354d24a..74adc2e3f 100644 --- a/BepuUtilities/BundleIndexing.cs +++ b/BepuUtilities/BundleIndexing.cs @@ -1,4 +1,5 @@ using System; +using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; using System.Runtime.InteropServices; @@ -83,5 +84,56 @@ public static unsafe Vector CreateMaskForCountInBundle(int countInBundle) } //TODO: ARM } + + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static int GetFirstSetLaneIndex(Vector v) + { + if (Avx.IsSupported && Vector.Count == 8) + { + var scalarMask = Avx.MoveMask(v.AsVector256().As()); + return BitOperations.TrailingZeroCount(scalarMask); + } + else if (Sse.IsSupported && Vector.Count == 4) + { + var scalarMask = Sse.MoveMask(v.AsVector128().As()); + return BitOperations.TrailingZeroCount(scalarMask); + } + else + { + Debug.Assert(Vector.Count <= 8, "We made an assumption that AVX512 and similar widths aren't available, this should be updated if vectors get wider!"); + for (int i = 0; i < Vector.Count; ++i) + { + if (v[i] == -1) + return i; + } + } + return -1; + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static int GetLastSetLaneIndex(Vector v) + { + if (Avx.IsSupported && Vector.Count == 8) + { + var scalarMask = Avx.MoveMask(v.AsVector256().As()); + return BitOperations.LeadingZeroCount((uint)scalarMask); + } + else if (Sse.IsSupported && Vector.Count == 4) + { + var scalarMask = Sse.MoveMask(v.AsVector128().As()); + return BitOperations.LeadingZeroCount((uint)scalarMask); + } + else + { + Debug.Assert(Vector.Count <= 8, "We made an assumption that AVX512 and similar widths aren't available, this should be updated if vectors get wider!"); + for (int i = Vector.Count - 1; i >= 0; --i) + { + if (v[i] == -1) + return i; + } + } + return -1; + } + } } From 6e34c55a98e73d0235d3855f65a896cb85436483 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 15 Oct 2021 18:00:01 -0500 Subject: [PATCH 214/947] Hooked up fallback removal, fixed bad final count. --- .../CollisionDetection/ConstraintRemover.cs | 3 +- BepuPhysics/ConstraintBatch.cs | 4 +- BepuPhysics/Constraints/TypeProcessor.cs | 150 +++++++++--------- BepuPhysics/Solver.cs | 2 +- BepuUtilities/BundleIndexing.cs | 15 +- Demos/Demo.cs | 2 +- 6 files changed, 91 insertions(+), 85 deletions(-) diff --git a/BepuPhysics/CollisionDetection/ConstraintRemover.cs b/BepuPhysics/CollisionDetection/ConstraintRemover.cs index a289c7222..c69fdbb3b 100644 --- a/BepuPhysics/CollisionDetection/ConstraintRemover.cs +++ b/BepuPhysics/CollisionDetection/ConstraintRemover.cs @@ -410,7 +410,8 @@ public void RemoveConstraintsFromTypeBatch(int index) //That's because removals can change the index, so caching indices would require sorting the indices for each type batch before removing. //That's very much doable, but not doing it is simpler, and the performance difference is likely trivial. //TODO: Likely worth testing. - typeProcessor.Remove(ref typeBatch, solver.HandleToConstraint[handle.Value].IndexInTypeBatch, ref solver.HandleToConstraint); + ref var location = ref solver.HandleToConstraint[handle.Value]; + typeProcessor.Remove(ref typeBatch, location.IndexInTypeBatch, ref solver.HandleToConstraint, location.BatchIndex == solver.FallbackBatchThreshold); if (typeBatch.ConstraintCount == 0) { //This batch-typebatch needs to be removed. diff --git a/BepuPhysics/ConstraintBatch.cs b/BepuPhysics/ConstraintBatch.cs index 6be71844e..05b5b0302 100644 --- a/BepuPhysics/ConstraintBatch.cs +++ b/BepuPhysics/ConstraintBatch.cs @@ -189,14 +189,14 @@ public unsafe void RemoveBodyHandlesFromBatchForConstraint(int constraintTypeId, solver.TypeProcessors[constraintTypeId].EnumerateConnectedBodyIndices(ref TypeBatches[typeBatchIndex], indexInTypeBatch, ref handleRemover); } - public unsafe void Remove(int constraintTypeId, int indexInTypeBatch, Solver solver) + public unsafe void Remove(int constraintTypeId, int indexInTypeBatch, bool isFallback, Solver solver) { var typeBatchIndex = TypeIndexToTypeBatchIndex[constraintTypeId]; ref var typeBatch = ref TypeBatches[typeBatchIndex]; Debug.Assert(TypeIndexToTypeBatchIndex[constraintTypeId] >= 0, "Type index must actually exist within this batch."); Debug.Assert(typeBatch.ConstraintCount > indexInTypeBatch); var typeProcessor = solver.TypeProcessors[constraintTypeId]; - typeProcessor.Remove(ref typeBatch, indexInTypeBatch, ref solver.HandleToConstraint); + typeProcessor.Remove(ref typeBatch, indexInTypeBatch, ref solver.HandleToConstraint, isFallback); RemoveTypeBatchIfEmpty(ref typeBatch, typeBatchIndex, solver.pool); } diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index 22df0a5c6..5a79b0287 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -67,7 +67,7 @@ public void Initialize(int typeId) /// Allocation provider to use if the type batch has to be resized. /// Index of the slot in the batch. public unsafe abstract int AllocateInTypeBatchForFallback(ref TypeBatch typeBatch, ConstraintHandle handle, int* bodyIndices, BufferPool pool); - public abstract void Remove(ref TypeBatch typeBatch, int index, ref Buffer handlesToConstraints); + public abstract void Remove(ref TypeBatch typeBatch, int index, ref Buffer handlesToConstraints, bool isFallback); /// /// Moves a constraint from one ConstraintBatch's TypeBatch to another ConstraintBatch's TypeBatch of the same type. @@ -469,97 +469,97 @@ public sealed override unsafe void Scramble(ref TypeBatch typeBatch, Random rand /// /// Removes a constraint from the batch. /// + /// Type batch to remove a constraint from. /// Index of the constraint to remove. /// The handle to constraint mapping used by the solver that could be modified by a swap on removal. - public override unsafe void Remove(ref TypeBatch typeBatch, int index, ref Buffer handlesToConstraints) + /// True if the type batch being removed from belongs to the fallback batch, false otherwise. + public override unsafe void Remove(ref TypeBatch typeBatch, int index, ref Buffer handlesToConstraints, bool isFallback) { Debug.Assert(index >= 0 && index < typeBatch.ConstraintCount, "Can only remove elements that are actually in the batch!"); - var lastIndex = typeBatch.ConstraintCount - 1; - typeBatch.ConstraintCount = lastIndex; - BundleIndexing.GetBundleIndices(lastIndex, out var sourceBundleIndex, out var sourceInnerIndex); - - ref var bodyReferences = ref Unsafe.As(ref *typeBatch.BodyReferences.Memory); - if (index < lastIndex) - { - //Need to swap. - ref var prestepData = ref Unsafe.As(ref *typeBatch.PrestepData.Memory); - ref var accumulatedImpulses = ref Unsafe.As(ref *typeBatch.AccumulatedImpulses.Memory); - BundleIndexing.GetBundleIndices(index, out var targetBundleIndex, out var targetInnerIndex); - Move( - ref Unsafe.Add(ref bodyReferences, sourceBundleIndex), ref Unsafe.Add(ref prestepData, sourceBundleIndex), ref Unsafe.Add(ref accumulatedImpulses, sourceBundleIndex), - typeBatch.IndexToHandle[lastIndex], sourceInnerIndex, - ref Unsafe.Add(ref bodyReferences, targetBundleIndex), ref Unsafe.Add(ref prestepData, targetBundleIndex), ref Unsafe.Add(ref accumulatedImpulses, targetBundleIndex), - ref typeBatch.IndexToHandle[index], targetInnerIndex, index, - ref handlesToConstraints); - } - //Clear the now-empty last slot of the body references bundle. - RemoveBodyReferencesLane(ref Unsafe.Add(ref bodyReferences, sourceBundleIndex), sourceInnerIndex); - } - - /// - /// Removes a constraint from the batch, assuming it belongs to a fallback batch. - /// - /// Index of the constraint to remove. - /// The handle to constraint mapping used by the solver that could be modified by a swap on removal. - public unsafe void RemoveForFallback(ref TypeBatch typeBatch, int index, ref Buffer handlesToConstraints) - { - Debug.Assert(index >= 0 && index < typeBatch.ConstraintCount, "Can only remove elements that are actually in the batch!"); - //The fallback batch does not guarantee contiguity of constraints, only contiguity of *bundles*. - //Bundles may be incomplete. - //We must guarantee that a bundle never contains references to the same body more than once. - //So, it's not safe to simply pull the last constraint into the removed slot. - //Instead, remove the constraint from whatever bundle it's in, and if it is empty afterwards, pull the whole last bundle into its position. - BundleIndexing.GetBundleIndices(index, out var removedBundleIndex, out var removedInnerIndex); - var bodyReferences = typeBatch.BodyReferences.As(); - ref var removedBundleSlot = ref bodyReferences[removedBundleIndex]; - RemoveBodyReferencesLane(ref removedBundleSlot, removedInnerIndex); - var firstBodyReferences = Unsafe.As>(ref removedBundleSlot); - if (Vector.LessThanAll(firstBodyReferences, Vector.Zero)) + if (isFallback) { - //All slots in the bundle are now empty; this bundle should be removed. - var lastBundleIndex = typeBatch.BundleCount - 1; - if (removedBundleIndex != lastBundleIndex) + //The fallback batch does not guarantee contiguity of constraints, only contiguity of *bundles*. + //Bundles may be incomplete. + //We must guarantee that a bundle never contains references to the same body more than once. + //So, it's not safe to simply pull the last constraint into the removed slot. + //Instead, remove the constraint from whatever bundle it's in, and if it is empty afterwards, pull the whole last bundle into its position. + BundleIndexing.GetBundleIndices(index, out var removedBundleIndex, out var removedInnerIndex); + var bodyReferences = typeBatch.BodyReferences.As(); + ref var removedBundleSlot = ref bodyReferences[removedBundleIndex]; + RemoveBodyReferencesLane(ref removedBundleSlot, removedInnerIndex); + var firstBodyReferences = Unsafe.As>(ref removedBundleSlot); + if (Vector.LessThanAll(firstBodyReferences, Vector.Zero)) { - //There is a bundle to move into the now-dead bundle slot. - var prestepData = typeBatch.PrestepData.As(); - var accumulatedImpulses = typeBatch.PrestepData.As(); - prestepData[removedBundleIndex] = prestepData[lastBundleIndex]; - accumulatedImpulses[removedBundleIndex] = accumulatedImpulses[lastBundleIndex]; - bodyReferences[removedBundleIndex] = bodyReferences[lastBundleIndex]; - var firstBodyLaneForMovedBundle = (int*)Unsafe.AsPointer(ref bodyReferences[lastBundleIndex]); - //Update all constraint locations for the move. - var constraintIndexShift = (lastBundleIndex - removedBundleIndex) * Vector.Count; - var bundleStartIndexInConstraints = lastBundleIndex * Vector.Count; - for (int i = 0; i < Vector.Count; ++i) + //All slots in the bundle are now empty; this bundle should be removed. + var lastBundleIndex = typeBatch.BundleCount - 1; + if (removedBundleIndex != lastBundleIndex) { - if (firstBodyLaneForMovedBundle[i] >= 0) + //There is a bundle to move into the now-dead bundle slot. + var prestepData = typeBatch.PrestepData.As(); + var accumulatedImpulses = typeBatch.PrestepData.As(); + prestepData[removedBundleIndex] = prestepData[lastBundleIndex]; + accumulatedImpulses[removedBundleIndex] = accumulatedImpulses[lastBundleIndex]; + bodyReferences[removedBundleIndex] = bodyReferences[lastBundleIndex]; + var firstBodyLaneForMovedBundle = (int*)Unsafe.AsPointer(ref bodyReferences[lastBundleIndex]); + //Update all constraint locations for the move. + var constraintIndexShift = (lastBundleIndex - removedBundleIndex) * Vector.Count; + var bundleStartIndexInConstraints = lastBundleIndex * Vector.Count; + for (int i = 0; i < Vector.Count; ++i) { - //This constraint actually exists. - var constraintIndex = bundleStartIndexInConstraints + i; - var newConstraintIndex = constraintIndex - constraintIndexShift; - var handle = typeBatch.IndexToHandle[constraintIndex]; - typeBatch.IndexToHandle[newConstraintIndex] = handle; - handlesToConstraints[handle.Value].IndexInTypeBatch = newConstraintIndex; + if (firstBodyLaneForMovedBundle[i] >= 0) + { + //This constraint actually exists. + var constraintIndex = bundleStartIndexInConstraints + i; + var newConstraintIndex = constraintIndex - constraintIndexShift; + var handle = typeBatch.IndexToHandle[constraintIndex]; + typeBatch.IndexToHandle[newConstraintIndex] = handle; + handlesToConstraints[handle.Value].IndexInTypeBatch = newConstraintIndex; + } } + //Removed the last bundle index, so drop back by one. + --lastBundleIndex; } - //Removed the last bundle index, so drop back by one. - --lastBundleIndex; - } - if (lastBundleIndex >= 0) - { - //Calculate the new constraint count by getting the highest index in the new last bundle. - var lastInnerIndex = BundleIndexing.GetLastSetLaneIndex(Vector.LessThan(Unsafe.As>(ref bodyReferences[lastBundleIndex]), Vector.Zero)); - typeBatch.ConstraintCount = lastBundleIndex * Vector.Count + lastInnerIndex; + if (lastBundleIndex >= 0) + { + //Calculate the new constraint count by getting the highest index in the new last bundle. + var innerLaneCount = BundleIndexing.GetLastSetLaneCount(Vector.LessThan(Unsafe.As>(ref bodyReferences[lastBundleIndex]), Vector.Zero)); + typeBatch.ConstraintCount = lastBundleIndex * Vector.Count + innerLaneCount; + } + else + { + //The last bundle was removed; there are no more constraints. + typeBatch.ConstraintCount = 0; + } } - else + } + else + { + var lastIndex = typeBatch.ConstraintCount - 1; + typeBatch.ConstraintCount = lastIndex; + BundleIndexing.GetBundleIndices(lastIndex, out var sourceBundleIndex, out var sourceInnerIndex); + + ref var bodyReferences = ref Unsafe.As(ref *typeBatch.BodyReferences.Memory); + if (index < lastIndex) { - //The last bundle was removed; there are no more constraints. - typeBatch.ConstraintCount = 0; + //Need to swap. + ref var prestepData = ref Unsafe.As(ref *typeBatch.PrestepData.Memory); + ref var accumulatedImpulses = ref Unsafe.As(ref *typeBatch.AccumulatedImpulses.Memory); + BundleIndexing.GetBundleIndices(index, out var targetBundleIndex, out var targetInnerIndex); + Move( + ref Unsafe.Add(ref bodyReferences, sourceBundleIndex), ref Unsafe.Add(ref prestepData, sourceBundleIndex), ref Unsafe.Add(ref accumulatedImpulses, sourceBundleIndex), + typeBatch.IndexToHandle[lastIndex], sourceInnerIndex, + ref Unsafe.Add(ref bodyReferences, targetBundleIndex), ref Unsafe.Add(ref prestepData, targetBundleIndex), ref Unsafe.Add(ref accumulatedImpulses, targetBundleIndex), + ref typeBatch.IndexToHandle[index], targetInnerIndex, index, + ref handlesToConstraints); } + //Clear the now-empty last slot of the body references bundle. + RemoveBodyReferencesLane(ref Unsafe.Add(ref bodyReferences, sourceBundleIndex), sourceInnerIndex); } + } + /// /// Moves a constraint from one ConstraintBatch's TypeBatch to another ConstraintBatch's TypeBatch of the same type. /// diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index 35eb8a40f..6d5906d8e 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -899,7 +899,7 @@ internal unsafe void RemoveFromBatch(ConstraintHandle constraintHandle, int batc { batch.RemoveBodyHandlesFromBatchForConstraint(typeId, indexInTypeBatch, batchIndex, this); } - batch.Remove(typeId, indexInTypeBatch, this); + batch.Remove(typeId, indexInTypeBatch, batchIndex == FallbackBatchThreshold, this); RemoveBatchIfEmpty(ref batch, batchIndex); } diff --git a/BepuUtilities/BundleIndexing.cs b/BepuUtilities/BundleIndexing.cs index 74adc2e3f..2bf5d948b 100644 --- a/BepuUtilities/BundleIndexing.cs +++ b/BepuUtilities/BundleIndexing.cs @@ -110,18 +110,23 @@ public static int GetFirstSetLaneIndex(Vector v) } return -1; } + /// + /// Gets the number of lanes that occur at or before the last set lane. In other words, if the lanes in the vector are (-1, 0, -1, 0), then this will return 3. + /// + /// Vector to examine. + /// Number of lanes that occur at or before the last set lane. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static int GetLastSetLaneIndex(Vector v) + public static int GetLastSetLaneCount(Vector v) { if (Avx.IsSupported && Vector.Count == 8) { var scalarMask = Avx.MoveMask(v.AsVector256().As()); - return BitOperations.LeadingZeroCount((uint)scalarMask); + return 32 - BitOperations.LeadingZeroCount((uint)scalarMask); } else if (Sse.IsSupported && Vector.Count == 4) { var scalarMask = Sse.MoveMask(v.AsVector128().As()); - return BitOperations.LeadingZeroCount((uint)scalarMask); + return 32 - BitOperations.LeadingZeroCount((uint)scalarMask); } else { @@ -129,10 +134,10 @@ public static int GetLastSetLaneIndex(Vector v) for (int i = Vector.Count - 1; i >= 0; --i) { if (v[i] == -1) - return i; + return i + 1; } } - return -1; + return 0; } } diff --git a/Demos/Demo.cs b/Demos/Demo.cs index 37ebe5190..5f3790da3 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -62,7 +62,7 @@ public virtual void Update(Window window, Camera camera, Input input, float dt) //fully decouple simulation and rendering rates across different threads. //(In either case, you'd also want to interpolate or extrapolate simulation results during rendering for smoothness.) //Note that taking steps of variable length can reduce stability. Gradual or one-off changes can work reasonably well. - Simulation.Timestep(1 / 60f, ThreadDispatcher); + Simulation.Timestep(1 / 60f); ////Here's an example of how it would look to use more frequent updates, but still with a fixed amount of time simulated per update call: //const float timeToSimulate = 1 / 60f; From 7449353c4f79a449e1a2e0ae7c04b19157ff69d0 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 15 Oct 2021 18:50:04 -0500 Subject: [PATCH 215/947] Added validation around empty slots in fallback batch; filled empty slot handle values with -1; fixed another slot picking bug in remove. Batch compressor can now work with the new fallback batch. --- BepuPhysics/BatchCompressor.cs | 52 ++++++++++++++----- BepuPhysics/Constraints/TypeProcessor.cs | 42 ++++++++++----- .../EmbeddedSubsteppingTimestepper2.cs | 1 + BepuPhysics/Solver.cs | 22 ++++++++ 4 files changed, 92 insertions(+), 25 deletions(-) diff --git a/BepuPhysics/BatchCompressor.cs b/BepuPhysics/BatchCompressor.cs index 679026911..1c0767a34 100644 --- a/BepuPhysics/BatchCompressor.cs +++ b/BepuPhysics/BatchCompressor.cs @@ -1,4 +1,5 @@ -using BepuUtilities; +using BepuPhysics.Constraints; +using BepuUtilities; using BepuUtilities.Collections; using BepuUtilities.Memory; using System; @@ -124,6 +125,26 @@ void AnalysisWorker(int workerIndex) //It'll just require some testing. //(The broad phase is a pretty likely candidate for this overlay- it both causes no changes in constraints and is very stally compared to most other phases.) + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + private unsafe void TryToFindBetterBatchForConstraint( + BufferPool pool, ref QuickList compressions, ref TypeBatch typeBatch, Span bodyHandlesSpan, ref ActiveConstraintBodyHandleCollector handleAccumulator, TypeProcessor typeProcessor, int constraintIndex) + { + handleAccumulator.Index = 0; + typeProcessor.EnumerateConnectedBodyIndices(ref typeBatch, constraintIndex, ref handleAccumulator); + for (int batchIndex = nextBatchIndex - 1; batchIndex >= 0; --batchIndex) + { + //The batch index will never be the fallback batch, since the fallback batch is the very last batch (if it exists at all). So uses of batch referenced handles is safe. + if (Solver.batchReferencedHandles[batchIndex].CanFit(bodyHandlesSpan)) + { + compressions.Add(new Compression { ConstraintHandle = typeBatch.IndexToHandle[constraintIndex], TargetBatch = batchIndex }, pool); + return; + } + } + + } + + unsafe void DoJob(ref AnalysisRegion region, int workerIndex, BufferPool pool) { ref var compressions = ref this.workerCompressions[workerIndex]; @@ -135,27 +156,32 @@ unsafe void DoJob(ref AnalysisRegion region, int workerIndex, BufferPool pool) //Each job only works on a subset of a single type batch. var bodiesPerConstraint = typeProcessor.BodiesPerConstraint; var bodyHandles = stackalloc int[bodiesPerConstraint]; + var bodyHandlesSpan = new Span(bodyHandles, bodiesPerConstraint); ActiveConstraintBodyHandleCollector handleAccumulator; handleAccumulator.Bodies = Bodies; handleAccumulator.Handles = bodyHandles; - var bodyHandlesSpan = new Span(bodyHandles, bodiesPerConstraint); - for (int i = region.StartIndexInTypeBatch; i < region.EndIndexInTypeBatch; ++i) + handleAccumulator.Index = 0; + if (nextBatchIndex == Solver.FallbackBatchThreshold) { - //Check if this constraint can be removed. - handleAccumulator.Index = 0; - typeProcessor.EnumerateConnectedBodyIndices(ref typeBatch, i, ref handleAccumulator); - for (int batchIndex = 0; batchIndex < nextBatchIndex; ++batchIndex) + for (int i = region.StartIndexInTypeBatch; i < region.EndIndexInTypeBatch; ++i) { - //The batch index will never be the fallback batch, since the fallback batch is the very last batch (if it exists at all). So uses of batch referenced handles is safe. - if (Solver.batchReferencedHandles[batchIndex].CanFit(bodyHandlesSpan)) - { - compressions.Add(new Compression { ConstraintHandle = typeBatch.IndexToHandle[i], TargetBatch = batchIndex }, pool); - break; - } + //This is a fallback batch; the rules are a little different. + //Not all constraint slots up to the typeBatch.ConstraintCount are guaranteed to actually exist. It's potentially sparse. + //Just skip them. + if (typeBatch.IndexToHandle[i].Value >= 0) + TryToFindBetterBatchForConstraint(pool, ref compressions, ref typeBatch, bodyHandlesSpan, ref handleAccumulator, typeProcessor, i); + } + } + else + { + for (int i = region.StartIndexInTypeBatch; i < region.EndIndexInTypeBatch; ++i) + { + TryToFindBetterBatchForConstraint(pool, ref compressions, ref typeBatch, bodyHandlesSpan, ref handleAccumulator, typeProcessor, i); } } } + struct CompressionTarget { public ushort WorkerIndex; diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index 5a79b0287..9fa8b9c76 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -319,6 +319,17 @@ private unsafe static bool ProbeBundleForFallback(Buffer typeBa return false; } + [Conditional("DEBUG")] + void ValidateEmptyFallbackSlots(ref TypeBatch typeBatch) + { + for (int i = 0; i < typeBatch.ConstraintCount; ++i) + { + BundleIndexing.GetBundleIndices(i, out var bundleIndex, out var innerIndex); + var laneIsEmpty = Unsafe.As>(ref typeBatch.BodyReferences.As()[bundleIndex])[innerIndex] == -1; + Debug.Assert((typeBatch.IndexToHandle[i].Value == -1) == laneIsEmpty); + } + } + public unsafe sealed override int AllocateInTypeBatchForFallback(ref TypeBatch typeBatch, ConstraintHandle handle, int* bodyIndices, BufferPool pool) { Debug.Assert(typeBatch.BodyReferences.Allocated, "Should initialize the batch before allocating anything from it."); @@ -383,7 +394,17 @@ public unsafe sealed override int AllocateInTypeBatchForFallback(ref TypeBatch t { //None of the existing bundles can hold the constraint; we need a new one. //Since a new bundle is guaranteed to not contain any other constraints, we can reuse the non-fallback logic. - return AllocateInTypeBatch(ref typeBatch, handle, bodyIndices, pool); + var oldCount = typeBatch.ConstraintCount; + var indexInTypeBatch = AllocateInTypeBatch(ref typeBatch, handle, bodyIndices, pool); + //Batch compression relies on all unoccupied slots having a IndexToHandle of -1. + //We've created a new bundle, which means we're responsible for setting all the slots from the previous count to the new count (excluding our just-added constraint!) to -1. + Debug.Assert(indexInTypeBatch == typeBatch.ConstraintCount - 1); + for (int i = oldCount; i < indexInTypeBatch; ++i) + { + typeBatch.IndexToHandle[i].Value = -1; + } + //ValidateEmptyFallbackSlots(ref typeBatch); + return indexInTypeBatch; } else { @@ -399,6 +420,7 @@ public unsafe sealed override int AllocateInTypeBatchForFallback(ref TypeBatch t Debug.Assert(typeBatch.Projection.Length >= bundleCount * Unsafe.SizeOf()); Debug.Assert(typeBatch.BodyReferences.Length >= bundleCount * Unsafe.SizeOf()); Debug.Assert(typeBatch.AccumulatedImpulses.Length >= bundleCount * Unsafe.SizeOf()); + //ValidateEmptyFallbackSlots(ref typeBatch); return indexInTypeBatch; } @@ -486,6 +508,8 @@ public override unsafe void Remove(ref TypeBatch typeBatch, int index, ref Buffe BundleIndexing.GetBundleIndices(index, out var removedBundleIndex, out var removedInnerIndex); var bodyReferences = typeBatch.BodyReferences.As(); ref var removedBundleSlot = ref bodyReferences[removedBundleIndex]; + //Batch compression relies on unused constraints in the [0, ConstraintCount) interval having their handles pointing to -1. + typeBatch.IndexToHandle[index].Value = -1; RemoveBodyReferencesLane(ref removedBundleSlot, removedInnerIndex); var firstBodyReferences = Unsafe.As>(ref removedBundleSlot); if (Vector.LessThanAll(firstBodyReferences, Vector.Zero)) @@ -512,6 +536,7 @@ public override unsafe void Remove(ref TypeBatch typeBatch, int index, ref Buffe var constraintIndex = bundleStartIndexInConstraints + i; var newConstraintIndex = constraintIndex - constraintIndexShift; var handle = typeBatch.IndexToHandle[constraintIndex]; + typeBatch.IndexToHandle[constraintIndex].Value = -1; //Removed handles should be set to -1. typeBatch.IndexToHandle[newConstraintIndex] = handle; handlesToConstraints[handle.Value].IndexInTypeBatch = newConstraintIndex; } @@ -519,18 +544,11 @@ public override unsafe void Remove(ref TypeBatch typeBatch, int index, ref Buffe //Removed the last bundle index, so drop back by one. --lastBundleIndex; } - if (lastBundleIndex >= 0) - { - //Calculate the new constraint count by getting the highest index in the new last bundle. - var innerLaneCount = BundleIndexing.GetLastSetLaneCount(Vector.LessThan(Unsafe.As>(ref bodyReferences[lastBundleIndex]), Vector.Zero)); - typeBatch.ConstraintCount = lastBundleIndex * Vector.Count + innerLaneCount; + //Calculate the new constraint count by getting the highest index in the new last bundle. + var innerLaneCount = BundleIndexing.GetLastSetLaneCount(Vector.GreaterThanOrEqual(Unsafe.As>(ref bodyReferences[lastBundleIndex]), Vector.Zero)); + typeBatch.ConstraintCount = lastBundleIndex * Vector.Count + innerLaneCount; - } - else - { - //The last bundle was removed; there are no more constraints. - typeBatch.ConstraintCount = 0; - } + //ValidateEmptyFallbackSlots(ref typeBatch); } } else diff --git a/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs b/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs index c9838e2a3..f37ffac31 100644 --- a/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs +++ b/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs @@ -58,6 +58,7 @@ public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDi Debug.Assert(SubstepCount >= 0, "Substep count should be positive."); simulation.Solver.ValidateTrailingTypeBatchBodyReferences(); + simulation.Solver.ValidateFallbackBatchEmptySlotReferences(); var constrainedBodySet = simulation.Solver.PrepareConstraintIntegrationResponsibilities(SubstepCount, threadDispatcher); simulation.Profiler.Start(simulation.Solver); simulation.Solver.SolveStep2(dt, threadDispatcher); diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index 6d5906d8e..dc3e9c700 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -294,6 +294,28 @@ public unsafe void ValidateTrailingTypeBatchBodyReferences() } } } + [Conditional("DEBUG")] + public unsafe void ValidateFallbackBatchEmptySlotReferences() + { + ref var set = ref ActiveSet; + if (set.Batches.Count > FallbackBatchThreshold) + { + ref var batch = ref set.Batches[FallbackBatchThreshold]; + for (int typeBatchIndex = 0; typeBatchIndex < batch.TypeBatches.Count; ++typeBatchIndex) + { + ref var typeBatch = ref batch.TypeBatches[typeBatchIndex]; + var bodiesPerConstraint = TypeProcessors[typeBatch.TypeId].BodiesPerConstraint; + var bodyReferencesBundleSize = Unsafe.SizeOf>() * bodiesPerConstraint; + for (int i = 0; i < typeBatch.ConstraintCount; ++i) + { + var expectDeadSlot = typeBatch.IndexToHandle[i].Value == -1; + BundleIndexing.GetBundleIndices(i, out var bundleIndex, out var innerIndex); + var bodyReferenceForFirstBody = Unsafe.As(ref typeBatch.BodyReferences[bundleIndex * bodyReferencesBundleSize + 4 * innerIndex]); + Debug.Assert(expectDeadSlot == (bodyReferenceForFirstBody == -1), "For fallback batches, the IndexToHandle should be -1 when the body lanes are -1, corresponding to empty lanes in the sparse batch."); + } + } + } + } [Conditional("DEBUG")] public unsafe void ValidateExistingHandles(bool activeOnly = false) From 1ee6267ae36ede0753fd48324989ab75ce96365c Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 15 Oct 2021 19:22:20 -0500 Subject: [PATCH 216/947] Off by one semierror. --- BepuPhysics/Constraints/TypeProcessor.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index 9fa8b9c76..cd144cf8d 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -354,7 +354,7 @@ public unsafe sealed override int AllocateInTypeBatchForFallback(ref TypeBatch t broadcastedBodyIndices[bodyIndexInConstraint] = new Vector(bodyIndices[bodyIndexInConstraint]); } var bundleCount = typeBatch.BundleCount; - if (bundleCount <= probeLocationCount) + if (bundleCount <= probeLocationCount + 1) //(we always probe the last bundle) { //The fallback batch is small; there's no need to do a stochastic insertion. Just enumerate all bundles. for (int bundleIndexInTypeBatch = 0; bundleIndexInTypeBatch < bundleCount; ++bundleIndexInTypeBatch) From cc3f507d8b8ba89e48c20fb52f1ae3e011ea747e Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 15 Oct 2021 20:36:24 -0500 Subject: [PATCH 217/947] Added more validation, fixed relevant bugs. --- BepuPhysics/Constraints/TypeProcessor.cs | 58 +++++++++++++++++-- .../EmbeddedSubsteppingTimestepper2.cs | 1 + BepuPhysics/Solver.cs | 42 +++++++++++++- 3 files changed, 96 insertions(+), 5 deletions(-) diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index cd144cf8d..be9343386 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -279,7 +279,7 @@ private unsafe static bool AllowFallbackBundleAllocation(ref TBodyReferences bun //TODO: depending on codegen, there's a chance that doing special cases for the 1, 2, 3, and 4 body cases would be worth it. No need for loop jumps and such. //The type batches are always held in pinned memory, this is not a GC hole. var bundleBodyIndices = (Vector*)Unsafe.AsPointer(ref bundle); - var bodiesPerConstraint = Unsafe.SizeOf() / Vector.Count; //redundant, but folds. + var bodiesPerConstraint = Unsafe.SizeOf() / Unsafe.SizeOf>(); //redundant, but folds. for (int broadcastedBundleBodyInConstraint = 0; broadcastedBundleBodyInConstraint < bodiesPerConstraint; ++broadcastedBundleBodyInConstraint) { var broadcastedBodies = broadcastedBodyIndices[broadcastedBundleBodyInConstraint]; @@ -330,11 +330,42 @@ void ValidateEmptyFallbackSlots(ref TypeBatch typeBatch) } } + [Conditional("DEBUG")] + void ValidateFallbackAccessSafety(ref TypeBatch typeBatch, int bodiesPerConstraint) + { + var bodyReferencesBundleSize = Unsafe.SizeOf>() * bodiesPerConstraint; + for (int bundleIndex = 0; bundleIndex < typeBatch.BundleCount; ++bundleIndex) + { + ref var bodyReferenceForFirstBody = ref Unsafe.As>(ref typeBatch.BodyReferences[bundleIndex * bodyReferencesBundleSize]); + for (int sourceBodyIndexInConstraint = 0; sourceBodyIndexInConstraint < bodiesPerConstraint; ++sourceBodyIndexInConstraint) + { + var bodyReferencesForSource = Unsafe.Add(ref bodyReferenceForFirstBody, sourceBodyIndexInConstraint); + for (int innerIndex = 0; innerIndex < Vector.Count; ++innerIndex) + { + var index = bodyReferencesForSource[innerIndex]; + if (index >= 0) + { + var broadcasted = new Vector(bodyReferencesForSource[innerIndex]); + int matchesTotal = 0; + for (int targetBodyIndexInConstraint = 0; targetBodyIndexInConstraint < bodiesPerConstraint; ++targetBodyIndexInConstraint) + { + var bodyReferencesForTarget = Unsafe.Add(ref bodyReferenceForFirstBody, targetBodyIndexInConstraint); + var matchesInLane = -Vector.Dot(Vector.Equals(broadcasted, bodyReferencesForTarget), Vector.One); + matchesTotal += matchesInLane; + } + Debug.Assert(matchesTotal == 1, "A body reference should occur no more than once in any constraint bundle."); + } + } + } + } + } + public unsafe sealed override int AllocateInTypeBatchForFallback(ref TypeBatch typeBatch, ConstraintHandle handle, int* bodyIndices, BufferPool pool) { Debug.Assert(typeBatch.BodyReferences.Allocated, "Should initialize the batch before allocating anything from it."); if (typeBatch.ConstraintCount == typeBatch.IndexToHandle.Length) { + //This isn't technically required (since probing might find an earlier slot), but it makes things simpler and rarely allocates more than necessary. InternalResize(ref typeBatch, pool, typeBatch.ConstraintCount * 2); } //The sequential fallback batch has different allocation rules. @@ -347,7 +378,7 @@ public unsafe sealed override int AllocateInTypeBatchForFallback(ref TypeBatch t int targetBundleIndex = -1; int targetInnerIndex = -1; //This folds. - var bodiesPerConstraint = Unsafe.SizeOf() / Vector.Count; + var bodiesPerConstraint = Unsafe.SizeOf() / Unsafe.SizeOf>(); var broadcastedBodyIndices = stackalloc Vector[bodiesPerConstraint]; for (int bodyIndexInConstraint = 0; bodyIndexInConstraint < bodiesPerConstraint; ++bodyIndexInConstraint) { @@ -393,9 +424,25 @@ public unsafe sealed override int AllocateInTypeBatchForFallback(ref TypeBatch t if (targetBundleIndex == -1) { //None of the existing bundles can hold the constraint; we need a new one. - //Since a new bundle is guaranteed to not contain any other constraints, we can reuse the non-fallback logic. var oldCount = typeBatch.ConstraintCount; - var indexInTypeBatch = AllocateInTypeBatch(ref typeBatch, handle, bodyIndices, pool); + + var indexInTypeBatch = bundleCount * Vector.Count; + typeBatch.ConstraintCount = indexInTypeBatch + 1; + if (typeBatch.ConstraintCount >= typeBatch.IndexToHandle.Length) + { + InternalResize(ref typeBatch, pool, typeBatch.ConstraintCount * 2); + } + typeBatch.IndexToHandle[indexInTypeBatch] = handle; + ref var bundle = ref Buffer.Get(ref typeBatch.BodyReferences, bundleCount); + AddBodyReferencesLane(ref bundle, 0, bodyIndices); + //Clear the slot's accumulated impulse. The backing memory could be initialized to any value. + GatherScatter.ClearLane(ref Buffer.Get(ref typeBatch.AccumulatedImpulses, bundleCount), 0); + Debug.Assert(typeBatch.PrestepData.Length >= typeBatch.BundleCount * Unsafe.SizeOf()); + Debug.Assert(typeBatch.Projection.Length >= typeBatch.BundleCount * Unsafe.SizeOf()); + Debug.Assert(typeBatch.BodyReferences.Length >= typeBatch.BundleCount * Unsafe.SizeOf()); + Debug.Assert(typeBatch.AccumulatedImpulses.Length >= typeBatch.BundleCount * Unsafe.SizeOf()); + Debug.Assert(typeBatch.IndexToHandle.Length >= typeBatch.ConstraintCount); + //Batch compression relies on all unoccupied slots having a IndexToHandle of -1. //We've created a new bundle, which means we're responsible for setting all the slots from the previous count to the new count (excluding our just-added constraint!) to -1. Debug.Assert(indexInTypeBatch == typeBatch.ConstraintCount - 1); @@ -404,6 +451,7 @@ public unsafe sealed override int AllocateInTypeBatchForFallback(ref TypeBatch t typeBatch.IndexToHandle[i].Value = -1; } //ValidateEmptyFallbackSlots(ref typeBatch); + ValidateFallbackAccessSafety(ref typeBatch, bodiesPerConstraint); return indexInTypeBatch; } else @@ -421,6 +469,7 @@ public unsafe sealed override int AllocateInTypeBatchForFallback(ref TypeBatch t Debug.Assert(typeBatch.BodyReferences.Length >= bundleCount * Unsafe.SizeOf()); Debug.Assert(typeBatch.AccumulatedImpulses.Length >= bundleCount * Unsafe.SizeOf()); //ValidateEmptyFallbackSlots(ref typeBatch); + ValidateFallbackAccessSafety(ref typeBatch, bodiesPerConstraint); return indexInTypeBatch; } @@ -549,6 +598,7 @@ public override unsafe void Remove(ref TypeBatch typeBatch, int index, ref Buffe typeBatch.ConstraintCount = lastBundleIndex * Vector.Count + innerLaneCount; //ValidateEmptyFallbackSlots(ref typeBatch); + ValidateFallbackAccessSafety(ref typeBatch, bodiesPerConstraint); } } else diff --git a/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs b/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs index f37ffac31..4dc427c85 100644 --- a/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs +++ b/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs @@ -59,6 +59,7 @@ public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDi simulation.Solver.ValidateTrailingTypeBatchBodyReferences(); simulation.Solver.ValidateFallbackBatchEmptySlotReferences(); + simulation.Solver.ValidateFallbackBatchAccessSafety(); var constrainedBodySet = simulation.Solver.PrepareConstraintIntegrationResponsibilities(SubstepCount, threadDispatcher); simulation.Profiler.Start(simulation.Solver); simulation.Solver.SolveStep2(dt, threadDispatcher); diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index dc3e9c700..206221912 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -311,7 +311,47 @@ public unsafe void ValidateFallbackBatchEmptySlotReferences() var expectDeadSlot = typeBatch.IndexToHandle[i].Value == -1; BundleIndexing.GetBundleIndices(i, out var bundleIndex, out var innerIndex); var bodyReferenceForFirstBody = Unsafe.As(ref typeBatch.BodyReferences[bundleIndex * bodyReferencesBundleSize + 4 * innerIndex]); - Debug.Assert(expectDeadSlot == (bodyReferenceForFirstBody == -1), "For fallback batches, the IndexToHandle should be -1 when the body lanes are -1, corresponding to empty lanes in the sparse batch."); + Debug.Assert(expectDeadSlot == (bodyReferenceForFirstBody == -1), "For fallback batches, the IndexToHandle should be -1 when the body lanes are -1, corresponding to empty lanes in the sparse batch."); + } + } + } + } + + [Conditional("DEBUG")] + public unsafe void ValidateFallbackBatchAccessSafety() + { + ref var set = ref ActiveSet; + if (set.Batches.Count > FallbackBatchThreshold) + { + ref var batch = ref set.Batches[FallbackBatchThreshold]; + for (int typeBatchIndex = 0; typeBatchIndex < batch.TypeBatches.Count; ++typeBatchIndex) + { + ref var typeBatch = ref batch.TypeBatches[typeBatchIndex]; + var bodiesPerConstraint = TypeProcessors[typeBatch.TypeId].BodiesPerConstraint; + var bodyReferencesBundleSize = Unsafe.SizeOf>() * bodiesPerConstraint; + for (int bundleIndex = 0; bundleIndex < typeBatch.BundleCount; ++bundleIndex) + { + ref var bodyReferenceForFirstBody = ref Unsafe.As>(ref typeBatch.BodyReferences[bundleIndex * bodyReferencesBundleSize]); + for (int sourceBodyIndexInConstraint = 0; sourceBodyIndexInConstraint < bodiesPerConstraint; ++sourceBodyIndexInConstraint) + { + var bodyReferencesForSource = Unsafe.Add(ref bodyReferenceForFirstBody, sourceBodyIndexInConstraint); + for (int innerIndex = 0; innerIndex < Vector.Count; ++innerIndex) + { + var index = bodyReferencesForSource[innerIndex]; + if (index >= 0) + { + var broadcasted = new Vector(bodyReferencesForSource[innerIndex]); + int matchesTotal = 0; + for (int targetBodyIndexInConstraint = 0; targetBodyIndexInConstraint < bodiesPerConstraint; ++targetBodyIndexInConstraint) + { + var bodyReferencesForTarget = Unsafe.Add(ref bodyReferenceForFirstBody, targetBodyIndexInConstraint); + var matchesInLane = -Vector.Dot(Vector.Equals(broadcasted, bodyReferencesForTarget), Vector.One); + matchesTotal += matchesInLane; + } + Debug.Assert(matchesTotal == 1, "A body reference should occur no more than once in any constraint bundle."); + } + } + } } } } From 5c23585a0ca361b068d657668767f6b41892a47c Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 16 Oct 2021 16:11:53 -0500 Subject: [PATCH 218/947] Fixed resize bug on fallback allocation and a constraint integration responsibilities calculation bug with fallback batches. --- BepuPhysics/Constraints/TypeProcessor.cs | 7 ++++--- BepuPhysics/Solver_SubsteppingSolve2.cs | 9 +++++++++ 2 files changed, 13 insertions(+), 3 deletions(-) diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index be9343386..0668658dc 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -427,11 +427,12 @@ public unsafe sealed override int AllocateInTypeBatchForFallback(ref TypeBatch t var oldCount = typeBatch.ConstraintCount; var indexInTypeBatch = bundleCount * Vector.Count; - typeBatch.ConstraintCount = indexInTypeBatch + 1; - if (typeBatch.ConstraintCount >= typeBatch.IndexToHandle.Length) + var newConstraintCount = indexInTypeBatch + 1; + if (newConstraintCount >= typeBatch.IndexToHandle.Length) { - InternalResize(ref typeBatch, pool, typeBatch.ConstraintCount * 2); + InternalResize(ref typeBatch, pool, newConstraintCount * 2); } + typeBatch.ConstraintCount = newConstraintCount; typeBatch.IndexToHandle[indexInTypeBatch] = handle; ref var bundle = ref Buffer.Get(ref typeBatch.BodyReferences, bundleCount); AddBodyReferencesLane(ref bundle, 0, bodyIndices); diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index d0642b52c..45a1f8a1f 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -635,6 +635,15 @@ unsafe bool ComputeIntegrationResponsibilitiesForConstraintRegion { //Constraints refer to bodies by index when they're in the active set, so we need to transform to handle to look up our merged batch results. var bodyIndex = bundleStart[bundleInnerIndex]; + if (typeof(TFallbackness) == typeof(IsFallbackBatch)) + { + //Fallback batches can contain empty lanes; there's no guarantee of constraint contiguity. Such lanes are marked with -1 in the body references. + //Just skip over them. + if (bodyIndex == -1) + { + continue; + } + } var bodyHandle = activeSet.IndexToHandle[bodyIndex].Value; if (firstObservedForBatch.Contains(bodyHandle)) { From c3a63e1ac48daad48e97aece2b1242226116a7c5 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 16 Oct 2021 21:00:54 -0500 Subject: [PATCH 219/947] Contact type processors no longer pointlessly load position. Fixed remainder bug in fallback probing. Updated workblock creation (or lack thereof) for fallbacks. Added more validation. --- BepuPhysics/Bodies_GatherScatter.cs | 32 +++++++++++++++ .../NarrowPhaseConstraintUpdate.cs | 1 + .../CollisionDetection/NarrowPhasePreflush.cs | 1 - .../Constraints/OneBodyTypeProcessor.cs | 2 +- .../Constraints/TwoBodyTypeProcessor.cs | 2 +- BepuPhysics/Constraints/TypeProcessor.cs | 37 ++++++++++++++--- .../EmbeddedSubsteppingTimestepper2.cs | 1 + BepuPhysics/Solver.cs | 40 +++++++++++++++++++ .../Solver_IncrementalContactUpdate.cs | 2 +- BepuPhysics/Solver_Solve.cs | 21 +++++++--- BepuPhysics/Solver_SubsteppingSolve2.cs | 32 ++++++++++++++- Demos/Demos/FountainStressTestDemo.cs | 2 +- 12 files changed, 154 insertions(+), 19 deletions(-) diff --git a/BepuPhysics/Bodies_GatherScatter.cs b/BepuPhysics/Bodies_GatherScatter.cs index 05858d7d3..0bda3d7fb 100644 --- a/BepuPhysics/Bodies_GatherScatter.cs +++ b/BepuPhysics/Bodies_GatherScatter.cs @@ -611,11 +611,21 @@ public unsafe void ScatterPose( if (maskPointer[5] != 0) Avx.StoreAligned((float*)(states + indices[5]), Avx.Permute2x128(o4, o5, 1 | (3 << 4))); if (maskPointer[6] != 0) Avx.StoreAligned((float*)(states + indices[6]), Avx.Permute2x128(o2, o3, 1 | (3 << 4))); if (maskPointer[7] != 0) Avx.StoreAligned((float*)(states + indices[7]), Avx.Permute2x128(o6, o7, 1 | (3 << 4))); + + //if (maskPointer[0] != 0) { states[indices[0]].Motion.Pose.Position.Validate(); states[indices[0]].Motion.Pose.Orientation.Validate(); } + //if (maskPointer[1] != 0) { states[indices[1]].Motion.Pose.Position.Validate(); states[indices[1]].Motion.Pose.Orientation.Validate(); } + //if (maskPointer[2] != 0) { states[indices[2]].Motion.Pose.Position.Validate(); states[indices[2]].Motion.Pose.Orientation.Validate(); } + //if (maskPointer[3] != 0) { states[indices[3]].Motion.Pose.Position.Validate(); states[indices[3]].Motion.Pose.Orientation.Validate(); } + //if (maskPointer[4] != 0) { states[indices[4]].Motion.Pose.Position.Validate(); states[indices[4]].Motion.Pose.Orientation.Validate(); } + //if (maskPointer[5] != 0) { states[indices[5]].Motion.Pose.Position.Validate(); states[indices[5]].Motion.Pose.Orientation.Validate(); } + //if (maskPointer[6] != 0) { states[indices[6]].Motion.Pose.Position.Validate(); states[indices[6]].Motion.Pose.Orientation.Validate(); } + //if (maskPointer[7] != 0) { states[indices[7]].Motion.Pose.Position.Validate(); states[indices[7]].Motion.Pose.Orientation.Validate(); } } } else { } + } @@ -664,6 +674,15 @@ public unsafe void ScatterInertia( if (maskPointer[5] != 0) Avx.StoreAligned((float*)(states + indices[5]) + 24, Avx.Permute2x128(o4, o5, 1 | (3 << 4))); if (maskPointer[6] != 0) Avx.StoreAligned((float*)(states + indices[6]) + 24, Avx.Permute2x128(o2, o3, 1 | (3 << 4))); if (maskPointer[7] != 0) Avx.StoreAligned((float*)(states + indices[7]) + 24, Avx.Permute2x128(o6, o7, 1 | (3 << 4))); + + //if (maskPointer[0] != 0) { states[indices[0]].Inertia.Local.InverseInertiaTensor.Validate(); states[indices[0]].Inertia.Local.InverseMass.Validate(); } + //if (maskPointer[1] != 0) { states[indices[1]].Inertia.Local.InverseInertiaTensor.Validate(); states[indices[1]].Inertia.Local.InverseMass.Validate(); } + //if (maskPointer[2] != 0) { states[indices[2]].Inertia.Local.InverseInertiaTensor.Validate(); states[indices[2]].Inertia.Local.InverseMass.Validate(); } + //if (maskPointer[3] != 0) { states[indices[3]].Inertia.Local.InverseInertiaTensor.Validate(); states[indices[3]].Inertia.Local.InverseMass.Validate(); } + //if (maskPointer[4] != 0) { states[indices[4]].Inertia.Local.InverseInertiaTensor.Validate(); states[indices[4]].Inertia.Local.InverseMass.Validate(); } + //if (maskPointer[5] != 0) { states[indices[5]].Inertia.Local.InverseInertiaTensor.Validate(); states[indices[5]].Inertia.Local.InverseMass.Validate(); } + //if (maskPointer[6] != 0) { states[indices[6]].Inertia.Local.InverseInertiaTensor.Validate(); states[indices[6]].Inertia.Local.InverseMass.Validate(); } + //if (maskPointer[7] != 0) { states[indices[7]].Inertia.Local.InverseInertiaTensor.Validate(); states[indices[7]].Inertia.Local.InverseMass.Validate(); } } } else @@ -773,6 +792,19 @@ public unsafe void ScatterVelocities(ref BodyVelocityWide sourceV if (indices[6] >= 0) Avx.StoreAligned((float*)(states + indices[6]) + 8, Avx.Permute2x128(o2, o3, 1 | (3 << 4))); if (indices[7] >= 0) Avx.StoreAligned((float*)(states + indices[7]) + 8, Avx.Permute2x128(o6, o7, 1 | (3 << 4))); } + + //{ + // var indices = (int*)Unsafe.AsPointer(ref references); + // var states = ActiveSet.SolverStates.Memory; + // if (indices[0] >= 0) { states[indices[0]].Motion.Velocity.Linear.Validate(); states[indices[0]].Motion.Velocity.Angular.Validate(); } + // if (indices[1] >= 0) { states[indices[1]].Motion.Velocity.Linear.Validate(); states[indices[1]].Motion.Velocity.Angular.Validate(); } + // if (indices[2] >= 0) { states[indices[2]].Motion.Velocity.Linear.Validate(); states[indices[2]].Motion.Velocity.Angular.Validate(); } + // if (indices[3] >= 0) { states[indices[3]].Motion.Velocity.Linear.Validate(); states[indices[3]].Motion.Velocity.Angular.Validate(); } + // if (indices[4] >= 0) { states[indices[4]].Motion.Velocity.Linear.Validate(); states[indices[4]].Motion.Velocity.Angular.Validate(); } + // if (indices[5] >= 0) { states[indices[5]].Motion.Velocity.Linear.Validate(); states[indices[5]].Motion.Velocity.Angular.Validate(); } + // if (indices[6] >= 0) { states[indices[6]].Motion.Velocity.Linear.Validate(); states[indices[6]].Motion.Velocity.Angular.Validate(); } + // if (indices[7] >= 0) { states[indices[7]].Motion.Velocity.Linear.Validate(); states[indices[7]].Motion.Velocity.Angular.Validate(); } + //} } else { diff --git a/BepuPhysics/CollisionDetection/NarrowPhaseConstraintUpdate.cs b/BepuPhysics/CollisionDetection/NarrowPhaseConstraintUpdate.cs index 079fd6cf4..e3e81cf53 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhaseConstraintUpdate.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhaseConstraintUpdate.cs @@ -7,6 +7,7 @@ using BepuPhysics.Collidables; using System; using BepuPhysics.Constraints.Contact; +using BepuUtilities; namespace BepuPhysics.CollisionDetection { diff --git a/BepuPhysics/CollisionDetection/NarrowPhasePreflush.cs b/BepuPhysics/CollisionDetection/NarrowPhasePreflush.cs index 7c468a930..56faff3c5 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhasePreflush.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhasePreflush.cs @@ -417,7 +417,6 @@ protected override void OnPreflush(IThreadDispatcher threadDispatcher, bool dete Simulation.Awakener.DisposeForCompletedAwakenings(ref setsToAwaken); } setsToAwaken.Dispose(Pool); - } } } diff --git a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs index 5a469d5d0..e8c9d99f5 100644 --- a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs @@ -208,7 +208,7 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f } public abstract class OneBodyContactTypeProcessor - : OneBodyTypeProcessor + : OneBodyTypeProcessor where TPrestepData : unmanaged where TProjection : unmanaged where TAccumulatedImpulse : unmanaged where TConstraintFunctions : unmanaged, IOneBodyContactConstraintFunctions { diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 93387939a..a8bb9b673 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -319,7 +319,7 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f } public abstract class TwoBodyContactTypeProcessor - : TwoBodyTypeProcessor + : TwoBodyTypeProcessor where TPrestepData : unmanaged where TProjection : unmanaged where TAccumulatedImpulse : unmanaged where TConstraintFunctions : unmanaged, IContactConstraintFunctions { diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index 0668658dc..80e2bd004 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -360,6 +360,23 @@ void ValidateFallbackAccessSafety(ref TypeBatch typeBatch, int bodiesPerConstrai } } + [Conditional("DEBUG")] + void ValidateAccumulatedImpulses(ref TypeBatch typeBatch) + { + var dofCount = Unsafe.SizeOf() / Unsafe.SizeOf>(); + for (int i = 0; i < typeBatch.BundleCount; ++i) + { + var impulseBundle = typeBatch.AccumulatedImpulses.As()[i]; + ref var impulses = ref Unsafe.As>(ref impulseBundle); + var mask = Vector.GreaterThanOrEqual(Unsafe.As>(ref typeBatch.BodyReferences.As()[i]), Vector.Zero); + for (int dofIndex = 0; dofIndex < dofCount; ++dofIndex) + { + var impulsesForDOF = Unsafe.Add(ref impulses, dofIndex); + impulsesForDOF.Validate(mask); + } + } + } + public unsafe sealed override int AllocateInTypeBatchForFallback(ref TypeBatch typeBatch, ConstraintHandle handle, int* bodyIndices, BufferPool pool) { Debug.Assert(typeBatch.BodyReferences.Allocated, "Should initialize the batch before allocating anything from it."); @@ -408,7 +425,7 @@ public unsafe sealed override int AllocateInTypeBatchForFallback(ref TypeBatch t //No room in the final bundle; keep looking with stochastic probes. var nextProbeIndex = (HashHelper.Rehash(handle.Value) & 0x7FFF_FFFF) % lastBundleIndex; var bundleJump = bundleCount / probeLocationCount; - var remainder = bundleCount - bundleJump * probeLocationCount; + var remainder = lastBundleIndex - bundleJump * probeLocationCount; for (int probeIndex = 0; probeIndex < probeLocationCount; ++probeIndex) { if (ProbeBundleForFallback(typeBatchBodyIndices, broadcastedBodyIndices, bodyIndices, nextProbeIndex, ref targetBundleIndex, ref targetInnerIndex)) @@ -452,7 +469,8 @@ public unsafe sealed override int AllocateInTypeBatchForFallback(ref TypeBatch t typeBatch.IndexToHandle[i].Value = -1; } //ValidateEmptyFallbackSlots(ref typeBatch); - ValidateFallbackAccessSafety(ref typeBatch, bodiesPerConstraint); + //ValidateFallbackAccessSafety(ref typeBatch, bodiesPerConstraint); + //ValidateAccumulatedImpulses(ref typeBatch); return indexInTypeBatch; } else @@ -470,7 +488,8 @@ public unsafe sealed override int AllocateInTypeBatchForFallback(ref TypeBatch t Debug.Assert(typeBatch.BodyReferences.Length >= bundleCount * Unsafe.SizeOf()); Debug.Assert(typeBatch.AccumulatedImpulses.Length >= bundleCount * Unsafe.SizeOf()); //ValidateEmptyFallbackSlots(ref typeBatch); - ValidateFallbackAccessSafety(ref typeBatch, bodiesPerConstraint); + //ValidateFallbackAccessSafety(ref typeBatch, bodiesPerConstraint); + //ValidateAccumulatedImpulses(ref typeBatch); return indexInTypeBatch; } @@ -570,7 +589,7 @@ public override unsafe void Remove(ref TypeBatch typeBatch, int index, ref Buffe { //There is a bundle to move into the now-dead bundle slot. var prestepData = typeBatch.PrestepData.As(); - var accumulatedImpulses = typeBatch.PrestepData.As(); + var accumulatedImpulses = typeBatch.AccumulatedImpulses.As(); prestepData[removedBundleIndex] = prestepData[lastBundleIndex]; accumulatedImpulses[removedBundleIndex] = accumulatedImpulses[lastBundleIndex]; bodyReferences[removedBundleIndex] = bodyReferences[lastBundleIndex]; @@ -599,7 +618,8 @@ public override unsafe void Remove(ref TypeBatch typeBatch, int index, ref Buffe typeBatch.ConstraintCount = lastBundleIndex * Vector.Count + innerLaneCount; //ValidateEmptyFallbackSlots(ref typeBatch); - ValidateFallbackAccessSafety(ref typeBatch, bodiesPerConstraint); + //ValidateFallbackAccessSafety(ref typeBatch, bodiesPerConstraint); + //ValidateAccumulatedImpulses(ref typeBatch); } } else @@ -625,7 +645,6 @@ ref Unsafe.Add(ref bodyReferences, targetBundleIndex), ref Unsafe.Add(ref preste //Clear the now-empty last slot of the body references bundle. RemoveBodyReferencesLane(ref Unsafe.Add(ref bodyReferences, sourceBundleIndex), sourceInnerIndex); } - } @@ -1246,6 +1265,12 @@ public static unsafe void GatherAndIntegrate.Zero); + orientation.Validate(validationMask); + position.Validate(validationMask); + velocity.Linear.Validate(validationMask); + velocity.Angular.Validate(validationMask); } } diff --git a/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs b/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs index 4dc427c85..2744eb020 100644 --- a/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs +++ b/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs @@ -60,6 +60,7 @@ public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDi simulation.Solver.ValidateTrailingTypeBatchBodyReferences(); simulation.Solver.ValidateFallbackBatchEmptySlotReferences(); simulation.Solver.ValidateFallbackBatchAccessSafety(); + simulation.Solver.ValidateFallbackBatchAccumulatedImpulses(); var constrainedBodySet = simulation.Solver.PrepareConstraintIntegrationResponsibilities(SubstepCount, threadDispatcher); simulation.Profiler.Start(simulation.Solver); simulation.Solver.SolveStep2(dt, threadDispatcher); diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index 206221912..eab3ff5f4 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -357,6 +357,46 @@ public unsafe void ValidateFallbackBatchAccessSafety() } } + unsafe struct ValidateAccumulatedImpulsesEnumerator : IForEach + { + public int Index = 0; + public float* AccumulatedImpulses; + public void LoopBody(float impulse) + { + AccumulatedImpulses[Index] = impulse; + } + } + + [Conditional("DEBUG")] + public unsafe void ValidateFallbackBatchAccumulatedImpulses() + { + ref var set = ref ActiveSet; + if (set.Batches.Count > FallbackBatchThreshold) + { + ref var batch = ref set.Batches[FallbackBatchThreshold]; + var impulseMemory = stackalloc float[8]; + var impulsesEnumerator = new ValidateAccumulatedImpulsesEnumerator { AccumulatedImpulses = impulseMemory }; + for (int typeBatchIndex = 0; typeBatchIndex < batch.TypeBatches.Count; ++typeBatchIndex) + { + ref var typeBatch = ref batch.TypeBatches[typeBatchIndex]; + var dofCount = TypeProcessors[typeBatch.TypeId].ConstrainedDegreesOfFreedom; + for (int constraintIndex = 0; constraintIndex < typeBatch.ConstraintCount; ++constraintIndex) + { + if (typeBatch.IndexToHandle[constraintIndex].Value >= 0) + { + impulsesEnumerator.Index = 0; + TypeProcessors[typeBatch.TypeId].EnumerateAccumulatedImpulses(ref typeBatch, constraintIndex, ref impulsesEnumerator); + for (int dofIndex = 0; dofIndex < dofCount; ++dofIndex) + { + impulseMemory[dofIndex].Validate(); + } + } + } + + } + } + } + [Conditional("DEBUG")] public unsafe void ValidateExistingHandles(bool activeOnly = false) { diff --git a/BepuPhysics/Solver_IncrementalContactUpdate.cs b/BepuPhysics/Solver_IncrementalContactUpdate.cs index d2f8708d4..36683cf56 100644 --- a/BepuPhysics/Solver_IncrementalContactUpdate.cs +++ b/BepuPhysics/Solver_IncrementalContactUpdate.cs @@ -13,7 +13,7 @@ public partial class Solver { protected struct IncrementalContactDataUpdateFilter : ITypeBatchSolveFilter { - public bool AllowFallback { get { return false; } } + public bool IncludeFallbackBatchForWorkBlocks { get { return true; } } [MethodImpl(MethodImplOptions.AggressiveInlining)] public bool AllowType(int typeId) diff --git a/BepuPhysics/Solver_Solve.cs b/BepuPhysics/Solver_Solve.cs index 8c066f0bc..478267adb 100644 --- a/BepuPhysics/Solver_Solve.cs +++ b/BepuPhysics/Solver_Solve.cs @@ -90,18 +90,18 @@ protected internal struct WorkBlock protected interface ITypeBatchSolveFilter { - bool AllowFallback { get; } + bool IncludeFallbackBatchForWorkBlocks { get; } bool AllowType(int typeId); } protected struct MainSolveFilter : ITypeBatchSolveFilter { - public bool AllowFallback + public bool IncludeFallbackBatchForWorkBlocks { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { - return true; + return false; } } @@ -117,11 +117,20 @@ protected unsafe void BuildWorkBlocks( out QuickList workBlocks, out Buffer batchBoundaries) where TTypeBatchFilter : ITypeBatchSolveFilter { ref var activeSet = ref ActiveSet; - workBlocks = new QuickList(targetBlocksPerBatch * activeSet.Batches.Count, pool); - pool.Take(activeSet.Batches.Count, out batchBoundaries); + int batchCount; + if (typeBatchFilter.IncludeFallbackBatchForWorkBlocks) + { + batchCount = activeSet.Batches.Count; + } + else + { + GetSynchronizedBatchCount(out batchCount, out _); + } + workBlocks = new QuickList(targetBlocksPerBatch * batchCount, pool); + pool.Take(batchCount, out batchBoundaries); var inverseMinimumBlockSizeInBundles = 1f / minimumBlockSizeInBundles; var inverseMaximumBlockSizeInBundles = 1f / maximumBlockSizeInBundles; - for (int batchIndex = 0; batchIndex < activeSet.Batches.Count; ++batchIndex) + for (int batchIndex = 0; batchIndex < batchCount; ++batchIndex) { ref var typeBatches = ref activeSet.Batches[batchIndex].TypeBatches; var bundleCount = 0; diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index 45a1f8a1f..567096b8a 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -344,7 +344,7 @@ void SolveStep2Worker2(int workerIndex) //A thread is only allowed to claim a workblock if the claim index for that workblock matches the expected value- which is the claim index it would have from the last time it was executed. //Each thread calculates what that claim index would have been based on the current sync index by subtracting the expected number of sync indices elapsed since last execution. - var syncStagesPerWarmStartOrSolve = ActiveSet.Batches.Count; + var syncStagesPerWarmStartOrSolve = synchronizedBatchCount; var baseStageCountInSubstep = syncStagesPerWarmStartOrSolve * (1 + IterationCount); //All warmstarts and solves, plus an incremental contact update. First substep doesn't do an incremental contact update, but that's fine, it'll end up expecting 0. var syncOffsetToPreviousSubstep = baseStageCountInSubstep + 1; @@ -367,6 +367,24 @@ void SolveStep2Worker2(int workerIndex) { ExecuteMainStage(ref warmstartStage, workerIndex, batchStarts[batchIndex], ref substepContext.Stages[batchIndex + 1], syncOffsetToPreviousClaimOnBatchForWarmStart, ref syncIndex); } + if (fallbackExists) + { + //The fallback runs only on the main thread. + ref var batch = ref activeSet.Batches[FallbackBatchThreshold]; + ref var integrationFlagsForBatch = ref integrationFlags[FallbackBatchThreshold]; + for (int j = 0; j < batch.TypeBatches.Count; ++j) + { + ref var typeBatch = ref batch.TypeBatches[j]; + if (substepIndex == 0) + { + WarmStartBlock(0, FallbackBatchThreshold, j, 0, typeBatch.BundleCount, ref typeBatch, TypeProcessors[typeBatch.TypeId], substepContext.Dt, substepContext.InverseDt); + } + else + { + WarmStartBlock(0, FallbackBatchThreshold, j, 0, typeBatch.BundleCount, ref typeBatch, TypeProcessors[typeBatch.TypeId], substepContext.Dt, substepContext.InverseDt); + } + } + } for (int iterationIndex = 0; iterationIndex < IterationCount; ++iterationIndex) { for (int batchIndex = 0; batchIndex < syncStagesPerWarmStartOrSolve; ++batchIndex) @@ -375,6 +393,16 @@ void SolveStep2Worker2(int workerIndex) //That's totally fine- the warmstart/iteration stages share the same claims buffers per batch. They're redundant for the sake of easier indexing. ExecuteMainStage(ref solveStage, workerIndex, batchStarts[batchIndex], ref substepContext.Stages[batchIndex + 1], syncOffsetToPreviousClaimOnBatchForSolve, ref syncIndex); } + if (fallbackExists) + { + //The fallback runs only on the main thread. + ref var batch = ref activeSet.Batches[FallbackBatchThreshold]; + for (int j = 0; j < batch.TypeBatches.Count; ++j) + { + ref var typeBatch = ref batch.TypeBatches[j]; + TypeProcessors[typeBatch.TypeId].SolveStep2(ref typeBatch, bodies, substepContext.Dt, substepContext.InverseDt, 0, typeBatch.BundleCount); + } + } } } //All done; notify waiting threads to join. @@ -474,7 +502,7 @@ protected void ExecuteMultithreaded2(float dt, IThreadDispatcher threadDispatche substepContext.SyncIndex = 0; var totalConstraintBatchWorkBlockCount = substepContext.ConstraintBatchBoundaries.Length == 0 ? 0 : substepContext.ConstraintBatchBoundaries[^1]; var totalClaimCount = incrementalBlocks.Count + totalConstraintBatchWorkBlockCount; - var stagesPerIteration = batchCount; + GetSynchronizedBatchCount(out var stagesPerIteration, out var fallbackExists); pool.Take(1 + stagesPerIteration * (1 + IterationCount), out substepContext.Stages); //Claims will be monotonically increasing throughout execution. All should start at zero to match with the initial sync index. pool.Take(totalClaimCount, out var claims); diff --git a/Demos/Demos/FountainStressTestDemo.cs b/Demos/Demos/FountainStressTestDemo.cs index 5eb142898..4634d34dd 100644 --- a/Demos/Demos/FountainStressTestDemo.cs +++ b/Demos/Demos/FountainStressTestDemo.cs @@ -23,7 +23,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = MathHelper.Pi * 0.1f; //Using minimum sized allocations forces as many resizes as possible. - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), 1, initialAllocationSizes: + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), 1, solverFallbackBatchThreshold: 24, initialAllocationSizes: new SimulationAllocationSizes { Bodies = 1, From 7d33b03cd9ba2735b85ea654db6b2825f71453ad Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 16 Oct 2021 23:02:49 -0500 Subject: [PATCH 220/947] Multithreaded fallback execution fixed. --- BepuPhysics/Solver_SubsteppingSolve2.cs | 8 ++++---- Demos/Demo.cs | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index 567096b8a..0a9433084 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -312,7 +312,8 @@ void SolveStep2Worker2(int workerIndex) var batchStartsData = stackalloc int[activeSet.Batches.Count]; batchStarts = new Buffer(batchStartsData, activeSet.Batches.Count); } - for (int batchIndex = 0; batchIndex < activeSet.Batches.Count; ++batchIndex) + GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); + for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) { var batchOffset = batchIndex > 0 ? substepContext.ConstraintBatchBoundaries[batchIndex - 1] : 0; var batchCount = substepContext.ConstraintBatchBoundaries[batchIndex] - batchOffset; @@ -320,7 +321,6 @@ void SolveStep2Worker2(int workerIndex) } Debug.Assert(activeSet.Batches.Count > 0, "Don't dispatch if there are no constraints."); - GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); //TODO: Every single one of these offers up the same parameters. Could avoid the need to initialize any of them. var incrementalUpdateStage = new IncrementalUpdateStageFunction @@ -513,7 +513,7 @@ protected void ExecuteMultithreaded2(float dt, IThreadDispatcher threadDispatche int targetStageIndex = 1; //Warm start. int claimStart = incrementalBlocks.Count; - for (int batchIndex = 0; batchIndex < batchCount; ++batchIndex) + for (int batchIndex = 0; batchIndex < stagesPerIteration; ++batchIndex) { var stageIndex = targetStageIndex++; var batchStart = batchIndex == 0 ? 0 : substepContext.ConstraintBatchBoundaries[batchIndex - 1]; @@ -525,7 +525,7 @@ protected void ExecuteMultithreaded2(float dt, IThreadDispatcher threadDispatche { //Solve. Note that we're reusing the same claims as were used in the warm start for these stages; the stages just tell the workers what kind of work to do. claimStart = incrementalBlocks.Count; - for (int batchIndex = 0; batchIndex < batchCount; ++batchIndex) + for (int batchIndex = 0; batchIndex < stagesPerIteration; ++batchIndex) { var stageIndex = targetStageIndex++; var batchStart = batchIndex == 0 ? 0 : substepContext.ConstraintBatchBoundaries[batchIndex - 1]; diff --git a/Demos/Demo.cs b/Demos/Demo.cs index 5f3790da3..37ebe5190 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -62,7 +62,7 @@ public virtual void Update(Window window, Camera camera, Input input, float dt) //fully decouple simulation and rendering rates across different threads. //(In either case, you'd also want to interpolate or extrapolate simulation results during rendering for smoothness.) //Note that taking steps of variable length can reduce stability. Gradual or one-off changes can work reasonably well. - Simulation.Timestep(1 / 60f); + Simulation.Timestep(1 / 60f, ThreadDispatcher); ////Here's an example of how it would look to use more frequent updates, but still with a fixed amount of time simulated per update call: //const float timeToSimulate = 1 / 60f; From c73d24cf45e14038b1c3aa252c2dd9fcf0e4154f Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 17 Oct 2021 11:41:05 -0500 Subject: [PATCH 221/947] Fixed accumulated impulse validation bug. Added more fallback occupancy validation. Trying to catch an extremely rare failure... --- .../CollisionDetection/CollisionBatcher.cs | 15 +++++++++++---- BepuPhysics/Constraints/TypeProcessor.cs | 2 +- BepuPhysics/Solver.cs | 15 ++++++++++++++- Demos/Demos/FountainStressTestDemo.cs | 2 +- 4 files changed, 27 insertions(+), 7 deletions(-) diff --git a/BepuPhysics/CollisionDetection/CollisionBatcher.cs b/BepuPhysics/CollisionDetection/CollisionBatcher.cs index 8067d4c34..e26515649 100644 --- a/BepuPhysics/CollisionDetection/CollisionBatcher.cs +++ b/BepuPhysics/CollisionDetection/CollisionBatcher.cs @@ -5,6 +5,7 @@ using BepuPhysics.CollisionDetection.CollisionTasks; using System.Numerics; using System; +using BepuUtilities; namespace BepuPhysics.CollisionDetection { @@ -210,10 +211,10 @@ public unsafe void AddDirectly(int shapeTypeA, int shapeTypeB, void* shapeA, voi AddDirectly(shapeTypeA, shapeTypeB, shapeA, shapeB, offsetB, orientationA, orientationB, default, default, speculativeMargin, default, pairContinuation); } - + [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void Add(TypedIndex shapeIndexA, TypedIndex shapeIndexB, - in Vector3 offsetB, in Quaternion orientationA, in Quaternion orientationB, in BodyVelocity velocityA, in BodyVelocity velocityB, + public unsafe void Add(TypedIndex shapeIndexA, TypedIndex shapeIndexB, + in Vector3 offsetB, in Quaternion orientationA, in Quaternion orientationB, in BodyVelocity velocityA, in BodyVelocity velocityB, float speculativeMargin, float maximumExpansion, in PairContinuation continuation) { @@ -223,7 +224,7 @@ public unsafe void Add(TypedIndex shapeIndexA, TypedIndex shapeIndexB, Shapes[shapeIndexB.Type].GetShapeData(shapeIndexB.Index, out var shapeB, out var shapeSizeB); AddDirectly(shapeTypeA, shapeTypeB, shapeA, shapeB, offsetB, orientationA, orientationB, velocityA, velocityB, speculativeMargin, maximumExpansion, continuation); } - + [MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe void Add(TypedIndex shapeIndexA, TypedIndex shapeIndexB, in Vector3 offsetB, in Quaternion orientationA, in Quaternion orientationB, float speculativeMargin, in PairContinuation continuation) @@ -314,6 +315,12 @@ public void Flush() public unsafe void ProcessConvexResult(ref ConvexContactManifold manifold, ref PairContinuation continuation) { +#if DEBUG + if (manifold.Count > 0) + { + manifold.Normal.Validate(); + } +#endif if (continuation.Type == CollisionContinuationType.Direct) { //This result concerns a pair which had no higher level owner. Directly report the manifold result. diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index 80e2bd004..bfd5d017d 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -389,7 +389,7 @@ public unsafe sealed override int AllocateInTypeBatchForFallback(ref TypeBatch t //Allocation must guarantee that a constraint does not fall into a bundle which shares any of the same body references. //(That responsibility usually falls on batch referenced handles blocking new constraints, //but the fallback exists precisely because the simulation is asking for pathological numbers of constraints affecting the same body.) - const int probeLocationCount = 8; + const int probeLocationCount = 16; //Note that this only ever executes for the active set, so body references are indices. var typeBatchBodyIndices = typeBatch.BodyReferences.As(); int targetBundleIndex = -1; diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index eab3ff5f4..88222a2db 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -324,14 +324,26 @@ public unsafe void ValidateFallbackBatchAccessSafety() if (set.Batches.Count > FallbackBatchThreshold) { ref var batch = ref set.Batches[FallbackBatchThreshold]; + int occupiedLaneCountAcrossBatch = 0; + int totalBundleCount = 0; for (int typeBatchIndex = 0; typeBatchIndex < batch.TypeBatches.Count; ++typeBatchIndex) { ref var typeBatch = ref batch.TypeBatches[typeBatchIndex]; + totalBundleCount += typeBatch.BundleCount; var bodiesPerConstraint = TypeProcessors[typeBatch.TypeId].BodiesPerConstraint; var bodyReferencesBundleSize = Unsafe.SizeOf>() * bodiesPerConstraint; for (int bundleIndex = 0; bundleIndex < typeBatch.BundleCount; ++bundleIndex) { ref var bodyReferenceForFirstBody = ref Unsafe.As>(ref typeBatch.BodyReferences[bundleIndex * bodyReferencesBundleSize]); + var occupiedLaneMask = Vector.GreaterThanOrEqual(bodyReferenceForFirstBody, Vector.Zero); + var occupiedLaneCountInBundle = 0; + for (int i = 0; i < Vector.Count; ++i) + { + if (occupiedLaneMask[i] < 0) + ++occupiedLaneCountInBundle; + } + occupiedLaneCountAcrossBatch += occupiedLaneCountInBundle; + Debug.Assert(occupiedLaneCountInBundle > 0, "For any bundle in the [0, BundleCount) interval, there must be at least one occupied lane."); for (int sourceBodyIndexInConstraint = 0; sourceBodyIndexInConstraint < bodiesPerConstraint; ++sourceBodyIndexInConstraint) { var bodyReferencesForSource = Unsafe.Add(ref bodyReferenceForFirstBody, sourceBodyIndexInConstraint); @@ -354,6 +366,7 @@ public unsafe void ValidateFallbackBatchAccessSafety() } } } + Console.WriteLine($"Average fallback occupancy: {Vector.Count * occupiedLaneCountAcrossBatch / (double)(totalBundleCount * Vector.Count):G3} / {Vector.Count}, total bundle count: {totalBundleCount}"); } } @@ -374,7 +387,7 @@ public unsafe void ValidateFallbackBatchAccumulatedImpulses() if (set.Batches.Count > FallbackBatchThreshold) { ref var batch = ref set.Batches[FallbackBatchThreshold]; - var impulseMemory = stackalloc float[8]; + var impulseMemory = stackalloc float[16]; var impulsesEnumerator = new ValidateAccumulatedImpulsesEnumerator { AccumulatedImpulses = impulseMemory }; for (int typeBatchIndex = 0; typeBatchIndex < batch.TypeBatches.Count; ++typeBatchIndex) { diff --git a/Demos/Demos/FountainStressTestDemo.cs b/Demos/Demos/FountainStressTestDemo.cs index 4634d34dd..07ab0da60 100644 --- a/Demos/Demos/FountainStressTestDemo.cs +++ b/Demos/Demos/FountainStressTestDemo.cs @@ -23,7 +23,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = MathHelper.Pi * 0.1f; //Using minimum sized allocations forces as many resizes as possible. - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), 1, solverFallbackBatchThreshold: 24, initialAllocationSizes: + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), 1, solverFallbackBatchThreshold: 2, initialAllocationSizes: new SimulationAllocationSizes { Bodies = 1, From b270b2102792929bd01b3d8a9539b6607b3de08f Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 17 Oct 2021 21:17:14 -0500 Subject: [PATCH 222/947] Fallback waking now has an actual implementation. Lots of validation added. There's a very silly bug in AddSleepingToActiveForFallback; three points if you can spot it. --- BepuPhysics/Constraints/TypeProcessor.cs | 148 ++++++++++++++---- .../EmbeddedSubsteppingTimestepper2.cs | 1 + BepuPhysics/IslandAwakener.cs | 53 +++++-- BepuPhysics/Solver.cs | 84 ++++++++-- 4 files changed, 224 insertions(+), 62 deletions(-) diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index bfd5d017d..605244b07 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -122,8 +122,11 @@ internal abstract void Regather( internal unsafe abstract void GatherActiveConstraints(Bodies bodies, Solver solver, ref QuickList sourceHandles, int startIndex, int endIndex, ref TypeBatch targetTypeBatch); + internal unsafe abstract void AddSleepingToActiveForFallback( + int sourceSet, int sourceTypeBatchIndex, int targetTypeBatchIndex, Bodies bodies, Solver solver); + internal unsafe abstract void CopySleepingToActive( - int sourceSet, int sourceBatchIndex, int sourceTypeBatchIndex, int targetBatchIndex, int targetTypeBatchIndex, + int sourceSet, int sourceBatchIndex, int sourceTypeBatchIndex, int targetTypeBatchIndex, int sourceStart, int targetStart, int count, Bodies bodies, Solver solver); @@ -339,6 +342,14 @@ void ValidateFallbackAccessSafety(ref TypeBatch typeBatch, int bodiesPerConstrai ref var bodyReferenceForFirstBody = ref Unsafe.As>(ref typeBatch.BodyReferences[bundleIndex * bodyReferencesBundleSize]); for (int sourceBodyIndexInConstraint = 0; sourceBodyIndexInConstraint < bodiesPerConstraint; ++sourceBodyIndexInConstraint) { + var occupiedLaneMask = Vector.GreaterThanOrEqual(bodyReferenceForFirstBody, Vector.Zero); + var occupiedLaneCountInBundle = 0; + for (int i = 0; i < Vector.Count; ++i) + { + if (occupiedLaneMask[i] < 0) + ++occupiedLaneCountInBundle; + } + Debug.Assert(occupiedLaneCountInBundle > 0, "For any bundle in the [0, BundleCount) interval, there must be at least one occupied lane."); var bodyReferencesForSource = Unsafe.Add(ref bodyReferenceForFirstBody, sourceBodyIndexInConstraint); for (int innerIndex = 0; innerIndex < Vector.Count; ++innerIndex) { @@ -379,9 +390,15 @@ void ValidateAccumulatedImpulses(ref TypeBatch typeBatch) public unsafe sealed override int AllocateInTypeBatchForFallback(ref TypeBatch typeBatch, ConstraintHandle handle, int* bodyIndices, BufferPool pool) { + //This folds. + var bodiesPerConstraint = Unsafe.SizeOf() / Unsafe.SizeOf>(); + //ValidateEmptyFallbackSlots(ref typeBatch); + //ValidateFallbackAccessSafety(ref typeBatch, bodiesPerConstraint); + //ValidateAccumulatedImpulses(ref typeBatch); Debug.Assert(typeBatch.BodyReferences.Allocated, "Should initialize the batch before allocating anything from it."); if (typeBatch.ConstraintCount == typeBatch.IndexToHandle.Length) { + Debug.Assert(pool != null, "Looks like a user that doesn't have access to a pool (the awakener, probably?) tried to add a constraint without preallocating enough room."); //This isn't technically required (since probing might find an earlier slot), but it makes things simpler and rarely allocates more than necessary. InternalResize(ref typeBatch, pool, typeBatch.ConstraintCount * 2); } @@ -394,8 +411,6 @@ public unsafe sealed override int AllocateInTypeBatchForFallback(ref TypeBatch t var typeBatchBodyIndices = typeBatch.BodyReferences.As(); int targetBundleIndex = -1; int targetInnerIndex = -1; - //This folds. - var bodiesPerConstraint = Unsafe.SizeOf() / Unsafe.SizeOf>(); var broadcastedBodyIndices = stackalloc Vector[bodiesPerConstraint]; for (int bodyIndexInConstraint = 0; bodyIndexInConstraint < bodiesPerConstraint; ++bodyIndexInConstraint) { @@ -468,9 +483,9 @@ public unsafe sealed override int AllocateInTypeBatchForFallback(ref TypeBatch t { typeBatch.IndexToHandle[i].Value = -1; } - //ValidateEmptyFallbackSlots(ref typeBatch); - //ValidateFallbackAccessSafety(ref typeBatch, bodiesPerConstraint); - //ValidateAccumulatedImpulses(ref typeBatch); + ValidateEmptyFallbackSlots(ref typeBatch); + ValidateFallbackAccessSafety(ref typeBatch, bodiesPerConstraint); + ValidateAccumulatedImpulses(ref typeBatch); return indexInTypeBatch; } else @@ -487,9 +502,9 @@ public unsafe sealed override int AllocateInTypeBatchForFallback(ref TypeBatch t Debug.Assert(typeBatch.Projection.Length >= bundleCount * Unsafe.SizeOf()); Debug.Assert(typeBatch.BodyReferences.Length >= bundleCount * Unsafe.SizeOf()); Debug.Assert(typeBatch.AccumulatedImpulses.Length >= bundleCount * Unsafe.SizeOf()); - //ValidateEmptyFallbackSlots(ref typeBatch); - //ValidateFallbackAccessSafety(ref typeBatch, bodiesPerConstraint); - //ValidateAccumulatedImpulses(ref typeBatch); + ValidateEmptyFallbackSlots(ref typeBatch); + ValidateFallbackAccessSafety(ref typeBatch, bodiesPerConstraint); + ValidateAccumulatedImpulses(ref typeBatch); return indexInTypeBatch; } @@ -569,6 +584,9 @@ public override unsafe void Remove(ref TypeBatch typeBatch, int index, ref Buffe Debug.Assert(index >= 0 && index < typeBatch.ConstraintCount, "Can only remove elements that are actually in the batch!"); if (isFallback) { + //ValidateEmptyFallbackSlots(ref typeBatch); + //ValidateFallbackAccessSafety(ref typeBatch, bodiesPerConstraint); + //ValidateAccumulatedImpulses(ref typeBatch); //The fallback batch does not guarantee contiguity of constraints, only contiguity of *bundles*. //Bundles may be incomplete. //We must guarantee that a bundle never contains references to the same body more than once. @@ -617,9 +635,9 @@ public override unsafe void Remove(ref TypeBatch typeBatch, int index, ref Buffe var innerLaneCount = BundleIndexing.GetLastSetLaneCount(Vector.GreaterThanOrEqual(Unsafe.As>(ref bodyReferences[lastBundleIndex]), Vector.Zero)); typeBatch.ConstraintCount = lastBundleIndex * Vector.Count + innerLaneCount; - //ValidateEmptyFallbackSlots(ref typeBatch); - //ValidateFallbackAccessSafety(ref typeBatch, bodiesPerConstraint); - //ValidateAccumulatedImpulses(ref typeBatch); + ValidateEmptyFallbackSlots(ref typeBatch); + ValidateFallbackAccessSafety(ref typeBatch, bodiesPerConstraint); + ValidateAccumulatedImpulses(ref typeBatch); } } else @@ -878,33 +896,83 @@ ref Buffer.Get(ref sourceTypeBatch.AccumulatedImpulses, sou } } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - void CopyIncompleteBundle(int sourceStart, int targetStart, int count, ref TypeBatch sourceTypeBatch, ref TypeBatch targetTypeBatch) + + internal unsafe sealed override void AddSleepingToActiveForFallback(int sourceSet, int sourceTypeBatchIndex, int targetTypeBatchIndex, Bodies bodies, Solver solver) { - for (int i = 0; i < count; ++i) + //Unlike the bulk copies that the awakener can do for non-fallback batches, we have to do the heavyweight allocations for fallbacks. + //This arises because sleeping constraint sets do not maintain the 'no constraints refer to the same bodies in a given bundle' rule; everything just got packed together. + var batchIndex = solver.FallbackBatchThreshold; + ref var sourceTypeBatch = ref solver.Sets[sourceSet].Batches[batchIndex].TypeBatches[sourceTypeBatchIndex]; + solver.ValidateSetOwnership(ref sourceTypeBatch, sourceSet); + ref var targetTypeBatch = ref solver.ActiveSet.Batches[batchIndex].TypeBatches[targetTypeBatchIndex]; + Debug.Assert(sourceTypeBatch.TypeId == targetTypeBatch.TypeId); + var bodyCount = Unsafe.SizeOf() / Unsafe.SizeOf>(); + //ValidateAccumulatedImpulses(ref targetTypeBatch); + //ValidateEmptyFallbackSlots(ref targetTypeBatch); + //ValidateFallbackAccessSafety(ref targetTypeBatch, bodyCount); + solver.ValidateConstraintMaps(0, batchIndex, targetTypeBatchIndex); + solver.ValidateConstraintMaps(sourceSet, batchIndex, sourceTypeBatchIndex); + int* bodyIndices = stackalloc int[bodyCount]; + var sourceBundleCount = sourceTypeBatch.BundleCount; + var sourceBodyReferences = sourceTypeBatch.BodyReferences.As(); + var sourcePrestepData = sourceTypeBatch.PrestepData.As(); + var sourceAccumulatedImpulses = sourceTypeBatch.AccumulatedImpulses.As(); + var targetPrestepData = targetTypeBatch.PrestepData.As(); + var targetAccumulatedImpulses = targetTypeBatch.AccumulatedImpulses.As(); + var bodyHandleToLocation = bodies.HandleToLocation; + var constraintHandleToLocation = solver.HandleToConstraint; + for (int bundleIndexInSource = 0; bundleIndexInSource < sourceBundleCount; ++bundleIndexInSource) { - //Note that this implementation allows two threads to access a single bundle. That would be a pretty bad case of false sharing if it happens, but - //it won't cause correctness problems. - var sourceIndex = sourceStart + i; - var targetIndex = targetStart + i; - BundleIndexing.GetBundleIndices(sourceIndex, out var sourceBundle, out var sourceInner); - BundleIndexing.GetBundleIndices(targetIndex, out var targetBundle, out var targetInner); - GatherScatter.CopyLane( - ref Buffer.Get(ref sourceTypeBatch.PrestepData, sourceBundle), sourceInner, - ref Buffer.Get(ref targetTypeBatch.PrestepData, targetBundle), targetInner); - GatherScatter.CopyLane( - ref Buffer.Get(ref sourceTypeBatch.AccumulatedImpulses, sourceBundle), sourceInner, - ref Buffer.Get(ref targetTypeBatch.AccumulatedImpulses, targetBundle), targetInner); + //It's possible that the sleeping fallback entries do not have any repeat entries. In that case, we could bulk copy the bundle into the active set. + //It's unclear if that's the best option- consider that it would always add a new bundle, but the members of the bundle might be able to be inserted in previous bundles. + var bundleStartConstraintIndex = bundleIndexInSource * Vector.Count; + var countInBundle = sourceTypeBatch.ConstraintCount - bundleStartConstraintIndex; + ref var sourceBodyReferencesBundle = ref sourceBodyReferences[bundleIndexInSource]; + ref var sourceAccumulatedImpulsesBundle = ref sourceAccumulatedImpulses[bundleIndexInSource]; + ref var sourcePrestepBundle = ref sourcePrestepData[bundleIndexInSource]; + for (int sourceInnerIndex = 0; sourceInnerIndex < countInBundle; ++sourceInnerIndex) + { + var sourceIndex = bundleStartConstraintIndex + sourceInnerIndex; + ref var bodyReferencesLane = ref Unsafe.As(ref GatherScatter.GetOffsetInstance(ref sourceBodyReferencesBundle, sourceInnerIndex)); + //Note that the sleeping set stores body references as handles, while the active set uses indices. We have to translate here. + for (int i = 0; i < bodyCount; ++i) + { + //Bodies have already been moved into the active set, so we can use the mapping. + var bodyHandleValue = Unsafe.Add(ref bodyReferencesLane, Vector.Count * i); + Debug.Assert(bodyHandleToLocation[bodyHandleValue].SetIndex == 0); + bodyIndices[i] = bodyHandleToLocation[bodyHandleValue].Index; + } + var handle = sourceTypeBatch.IndexToHandle[sourceIndex]; + Debug.Assert(constraintHandleToLocation[handle.Value].SetIndex == sourceSet); + Debug.Assert(constraintHandleToLocation[handle.Value].IndexInTypeBatch == sourceIndex); + Debug.Assert(constraintHandleToLocation[handle.Value].TypeId == sourceTypeBatch.TypeId); + Debug.Assert(constraintHandleToLocation[handle.Value].BatchIndex == batchIndex); + //Note that we pass null for the buffer pool. The user (awakener) must preallocate worst case room in the type batches ahead of time so that multiple threads can proceed at the same time. + var targetIndex = AllocateInTypeBatchForFallback(ref targetTypeBatch, handle, bodyIndices, null); + BundleIndexing.GetBundleIndices(targetIndex, out var targetBundle, out var targetInner); + + GatherScatter.CopyLane(ref sourceAccumulatedImpulsesBundle, sourceInnerIndex, ref targetAccumulatedImpulses[targetBundle], targetInner); + GatherScatter.CopyLane(ref sourcePrestepBundle, sourceInnerIndex, ref targetPrestepData[targetBundle], targetInner); + ref var location = ref constraintHandleToLocation[handle.Value]; + Debug.Assert(location.SetIndex == sourceSet); + location.SetIndex = 0; + location.BatchIndex = batchIndex; + Debug.Assert(sourceTypeBatch.TypeId == location.TypeId); + location.IndexInTypeBatch = targetIndex; + } } + ValidateAccumulatedImpulses(ref targetTypeBatch); + ValidateEmptyFallbackSlots(ref targetTypeBatch); + ValidateFallbackAccessSafety(ref targetTypeBatch, bodyCount); + solver.ValidateConstraintMaps(0, batchIndex, targetTypeBatchIndex); } - internal unsafe sealed override void CopySleepingToActive( - int sourceSet, int sourceBatchIndex, int sourceTypeBatchIndex, int targetBatchIndex, int targetTypeBatchIndex, + int sourceSet, int batchIndex, int sourceTypeBatchIndex, int targetTypeBatchIndex, int sourceStart, int targetStart, int count, Bodies bodies, Solver solver) { - ref var sourceTypeBatch = ref solver.Sets[sourceSet].Batches[sourceBatchIndex].TypeBatches[sourceTypeBatchIndex]; - ref var targetTypeBatch = ref solver.ActiveSet.Batches[targetBatchIndex].TypeBatches[targetTypeBatchIndex]; + ref var sourceTypeBatch = ref solver.Sets[sourceSet].Batches[batchIndex].TypeBatches[sourceTypeBatchIndex]; + ref var targetTypeBatch = ref solver.ActiveSet.Batches[batchIndex].TypeBatches[targetTypeBatchIndex]; Debug.Assert(sourceStart >= 0 && sourceStart + count <= sourceTypeBatch.ConstraintCount); Debug.Assert(targetStart >= 0 && targetStart + count <= targetTypeBatch.ConstraintCount, "This function should only be used when a region has been preallocated within the type batch."); @@ -930,7 +998,21 @@ internal unsafe sealed override void CopySleepingToActive( } else { - CopyIncompleteBundle(sourceStart, targetStart, count, ref sourceTypeBatch, ref targetTypeBatch); + for (int i = 0; i < count; ++i) + { + //Note that this implementation allows two threads to access a single bundle. That would be a pretty bad case of false sharing if it happens, but + //it won't cause correctness problems. + var sourceIndex = sourceStart + i; + var targetIndex = targetStart + i; + BundleIndexing.GetBundleIndices(sourceIndex, out var sourceBundle, out var sourceInner); + BundleIndexing.GetBundleIndices(targetIndex, out var targetBundle, out var targetInner); + GatherScatter.CopyLane( + ref Buffer.Get(ref sourceTypeBatch.PrestepData, sourceBundle), sourceInner, + ref Buffer.Get(ref targetTypeBatch.PrestepData, targetBundle), targetInner); + GatherScatter.CopyLane( + ref Buffer.Get(ref sourceTypeBatch.AccumulatedImpulses, sourceBundle), sourceInner, + ref Buffer.Get(ref targetTypeBatch.AccumulatedImpulses, targetBundle), targetInner); + } } //Note that body reference copies cannot be done in bulk because inactive constraints refer to body handles while active constraints refer to body indices. for (int i = 0; i < count; ++i) @@ -952,7 +1034,7 @@ internal unsafe sealed override void CopySleepingToActive( ref var location = ref solver.HandleToConstraint[constraintHandle.Value]; Debug.Assert(location.SetIndex == sourceSet); location.SetIndex = 0; - location.BatchIndex = targetBatchIndex; + location.BatchIndex = batchIndex; Debug.Assert(sourceTypeBatch.TypeId == location.TypeId); location.IndexInTypeBatch = targetIndex; //This could be done with a bulk copy, but eh! We already touched the memory. diff --git a/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs b/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs index 2744eb020..6c2b17e8b 100644 --- a/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs +++ b/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs @@ -61,6 +61,7 @@ public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDi simulation.Solver.ValidateFallbackBatchEmptySlotReferences(); simulation.Solver.ValidateFallbackBatchAccessSafety(); simulation.Solver.ValidateFallbackBatchAccumulatedImpulses(); + simulation.Solver.ValidateConstraintMaps(); var constrainedBodySet = simulation.Solver.PrepareConstraintIntegrationResponsibilities(SubstepCount, threadDispatcher); simulation.Profiler.Start(simulation.Solver); simulation.Solver.SolveStep2(dt, threadDispatcher); diff --git a/BepuPhysics/IslandAwakener.cs b/BepuPhysics/IslandAwakener.cs index 740beea66..578b17bde 100644 --- a/BepuPhysics/IslandAwakener.cs +++ b/BepuPhysics/IslandAwakener.cs @@ -179,6 +179,7 @@ enum PhaseTwoJobType { BroadPhase, CopyConstraintRegion, + AddFallbackTypeBatchConstraints } struct PhaseTwoJob { @@ -363,14 +364,19 @@ internal unsafe void ExecutePhaseTwoJob(int index) //2) Sleeping constraints store their body references as body *handles* rather than body indices. //Pulling the type batches back into the active set requires translating those body handles to body indices. //3) The translation from body handle to body index requires that the bodies already have an active set identity, which is why the constraints wait until the second phase. - ref var sourceTypeBatch = ref solver.Sets[job.SourceSet].Batches[job.Batch].TypeBatches[job.SourceTypeBatch]; - ref var targetTypeBatch = ref solver.ActiveSet.Batches[job.Batch].TypeBatches[job.TargetTypeBatch]; - Debug.Assert(targetTypeBatch.TypeId == sourceTypeBatch.TypeId); + Debug.Assert(solver.ActiveSet.Batches[job.Batch].TypeBatches[job.TargetTypeBatch].TypeId == solver.Sets[job.SourceSet].Batches[job.Batch].TypeBatches[job.SourceTypeBatch].TypeId); + Debug.Assert(job.Batch != solver.FallbackBatchThreshold, "Fallback batches must only be handled by the fallback-specific job."); solver.TypeProcessors[job.TypeId].CopySleepingToActive( - job.SourceSet, job.Batch, job.SourceTypeBatch, job.Batch, job.TargetTypeBatch, + job.SourceSet, job.Batch, job.SourceTypeBatch, job.TargetTypeBatch, job.SourceStart, job.TargetStart, job.Count, bodies, solver); } break; + case PhaseTwoJobType.AddFallbackTypeBatchConstraints: + { + Debug.Assert(solver.ActiveSet.Batches[job.Batch].TypeBatches[job.TargetTypeBatch].TypeId == solver.Sets[job.SourceSet].Batches[job.Batch].TypeBatches[job.SourceTypeBatch].TypeId); + solver.TypeProcessors[job.TypeId].AddSleepingToActiveForFallback(job.SourceSet, job.SourceTypeBatch, job.TargetTypeBatch, bodies, solver); + } + break; } } @@ -574,6 +580,11 @@ void AccumulatePairCacheTypeCounts(ref Buffer sourceTypeCaches, r for (int typeId = 0; typeId <= constraintCountPerType.HighestOccupiedTypeIndex; ++typeId) { var countForType = constraintCountPerType.TypeCounts[typeId].Count; + //The fallback batch must allocate a worst case scenario assuming that every new constraint needs its own bundle. + //It's difficult to be more conservative ahead of time; we don't know which existing partial bundles will be able to accept the new constraints. + //Fallback batches should tend to be rarely used and relatively small, and the extra memory won't be touched, so this isn't a major concern. + if (batchIndex == solver.FallbackBatchThreshold) + countForType *= Vector.Count; if (countForType > 0) { var typeProcessor = solver.TypeProcessors[typeId]; @@ -663,8 +674,10 @@ void EnsurePairCacheTypeCapacities(ref TypeAllocationSizes cache { const int constraintJobSize = 32; ref var sourceSet = ref solver.Sets[sourceSetIndex]; + var fallbackIndex = solver.FallbackBatchThreshold; for (int batchIndex = 0; batchIndex < sourceSet.Batches.Count; ++batchIndex) { + var jobType = batchIndex == fallbackIndex ? PhaseTwoJobType.AddFallbackTypeBatchConstraints : PhaseTwoJobType.CopyConstraintRegion; ref var sourceBatch = ref sourceSet.Batches[batchIndex]; ref var targetBatch = ref activeSolverSet.Batches[batchIndex]; for (int sourceTypeBatchIndex = 0; sourceTypeBatchIndex < sourceBatch.TypeBatches.Count; ++sourceTypeBatchIndex) @@ -673,16 +686,19 @@ void EnsurePairCacheTypeCapacities(ref TypeAllocationSizes cache var targetTypeBatchIndex = targetBatch.TypeIndexToTypeBatchIndex[sourceTypeBatch.TypeId]; ref var targetTypeBatch = ref targetBatch.TypeBatches[targetTypeBatchIndex]; //TODO: It would be nice to be a little more clever about scheduling start and end points for the sake of avoiding partial bundles. - var jobCount = Math.Max(1, sourceTypeBatch.ConstraintCount / constraintJobSize); + var jobCount = batchIndex == fallbackIndex ? 1 : Math.Max(1, sourceTypeBatch.ConstraintCount / constraintJobSize); var baseConstraintsPerJob = sourceTypeBatch.ConstraintCount / jobCount; var remainder = sourceTypeBatch.ConstraintCount - baseConstraintsPerJob * jobCount; phaseTwoJobs.EnsureCapacity(phaseTwoJobs.Count + jobCount, pool); - + if(batchIndex == fallbackIndex) + { + Console.WriteLine("Asdf"); + } var previousSourceEnd = 0; for (int jobIndex = 0; jobIndex < jobCount; ++jobIndex) { ref var job = ref phaseTwoJobs.AllocateUnsafely(); - job.Type = PhaseTwoJobType.CopyConstraintRegion; + job.Type = jobType; job.TypeId = sourceTypeBatch.TypeId; job.Batch = batchIndex; job.SourceSet = sourceSetIndex; @@ -692,18 +708,21 @@ void EnsurePairCacheTypeCapacities(ref TypeAllocationSizes cache job.SourceStart = previousSourceEnd; job.TargetStart = targetTypeBatch.ConstraintCount; previousSourceEnd += job.Count; - var oldBundleCount = targetTypeBatch.BundleCount; - targetTypeBatch.ConstraintCount += job.Count; - if (targetTypeBatch.BundleCount != oldBundleCount) + if (batchIndex < fallbackIndex) //The fallback batch add isn't a bulk copy; it'll handle incrementing count as necessary. { - //A new bundle was created; guarantee any trailing slots are set to -1. - //Since it's a whole new bundle that has not yet had any data set to it, we can safely just initialize the whole bundle's body references to -1. - var vectorCount = solver.TypeProcessors[job.TypeId].BodiesPerConstraint; - var bundleStart = (Vector*)(targetTypeBatch.BodyReferences.Memory + (targetTypeBatch.BundleCount - 1) * vectorCount * Unsafe.SizeOf>()); - var negativeOne = new Vector(-1); - for (int vectorIndex = 0; vectorIndex < vectorCount; ++vectorIndex) + var oldBundleCount = targetTypeBatch.BundleCount; + targetTypeBatch.ConstraintCount += job.Count; + if (targetTypeBatch.BundleCount != oldBundleCount) { - bundleStart[vectorIndex] = negativeOne; + //A new bundle was created; guarantee any trailing slots are set to -1. + //Since it's a whole new bundle that has not yet had any data set to it, we can safely just initialize the whole bundle's body references to -1. + var vectorCount = solver.TypeProcessors[job.TypeId].BodiesPerConstraint; + var bundleStart = (Vector*)(targetTypeBatch.BodyReferences.Memory + (targetTypeBatch.BundleCount - 1) * vectorCount * Unsafe.SizeOf>()); + var negativeOne = new Vector(-1); + for (int vectorIndex = 0; vectorIndex < vectorCount; ++vectorIndex) + { + bundleStart[vectorIndex] = negativeOne; + } } } } diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index 88222a2db..5537313b7 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -366,7 +366,38 @@ public unsafe void ValidateFallbackBatchAccessSafety() } } } - Console.WriteLine($"Average fallback occupancy: {Vector.Count * occupiedLaneCountAcrossBatch / (double)(totalBundleCount * Vector.Count):G3} / {Vector.Count}, total bundle count: {totalBundleCount}"); + //Console.WriteLine($"Average fallback occupancy: {Vector.Count * occupiedLaneCountAcrossBatch / (double)(totalBundleCount * Vector.Count):G3} / {Vector.Count}, total bundle count: {totalBundleCount}"); + } + } + [Conditional("DEBUG")] + internal void ValidateSetOwnership(ref TypeBatch typeBatch, int expectedSetIndex) + { + for (int i = 0; i < typeBatch.ConstraintCount; ++i) + { + var handle = typeBatch.IndexToHandle[i]; + if (handle.Value >= 0) + { + Debug.Assert(HandleToConstraint[handle.Value].SetIndex == expectedSetIndex); + } + } + } + [Conditional("DEBUG")] + internal void ValidateSetOwnership() + { + for (int setIndex = 0; setIndex < Sets.Length; ++setIndex) + { + ref var set = ref Sets[setIndex]; + if (!set.Allocated) + continue; + for (int batchIndex = 0; batchIndex < set.Batches.Count; ++batchIndex) + { + ref var batch = ref set.Batches[batchIndex]; + for (int typeBatchIndex = 0; typeBatchIndex < batch.TypeBatches.Count; ++typeBatchIndex) + { + ref var typeBatch = ref batch.TypeBatches[typeBatchIndex]; + ValidateSetOwnership(ref typeBatch, setIndex); + } + } } } @@ -547,7 +578,45 @@ public unsafe void ValidateExistingHandles(bool activeOnly = false) } [Conditional("DEBUG")] - internal void ValidateConstraintMaps(bool activeOnly = false) + internal void ValidateConstraintMaps(int setIndex, int batchIndex, int typeBatchIndex) + { + ref var set = ref Sets[setIndex]; + ref var batch = ref set.Batches[batchIndex]; + ref var typeBatch = ref batch.TypeBatches[typeBatchIndex]; + if (batchIndex == FallbackBatchThreshold) + { + for (int indexInTypeBatch = 0; indexInTypeBatch < typeBatch.ConstraintCount; ++indexInTypeBatch) + { + //Fallback batches can have empty slots, marked with a -1 in the handle slot. + var handle = typeBatch.IndexToHandle[indexInTypeBatch]; + if (handle.Value >= 0) + { + ref var constraintLocation = ref HandleToConstraint[handle.Value]; + Debug.Assert(constraintLocation.SetIndex == setIndex); + Debug.Assert(constraintLocation.BatchIndex == batchIndex); + Debug.Assert(constraintLocation.IndexInTypeBatch == indexInTypeBatch); + Debug.Assert(constraintLocation.TypeId == typeBatch.TypeId); + Debug.Assert(batch.TypeIndexToTypeBatchIndex[constraintLocation.TypeId] == typeBatchIndex); + } + } + } + else + { + for (int indexInTypeBatch = 0; indexInTypeBatch < typeBatch.ConstraintCount; ++indexInTypeBatch) + { + var handle = typeBatch.IndexToHandle[indexInTypeBatch]; + ref var constraintLocation = ref HandleToConstraint[handle.Value]; + Debug.Assert(constraintLocation.SetIndex == setIndex); + Debug.Assert(constraintLocation.BatchIndex == batchIndex); + Debug.Assert(constraintLocation.IndexInTypeBatch == indexInTypeBatch); + Debug.Assert(constraintLocation.TypeId == typeBatch.TypeId); + Debug.Assert(batch.TypeIndexToTypeBatchIndex[constraintLocation.TypeId] == typeBatchIndex); + } + } + } + + [Conditional("DEBUG")] + public void ValidateConstraintMaps(bool activeOnly = false) { var setCount = activeOnly ? 1 : Sets.Length; for (int setIndex = 0; setIndex < setCount; ++setIndex) @@ -560,16 +629,7 @@ internal void ValidateConstraintMaps(bool activeOnly = false) ref var batch = ref set.Batches[batchIndex]; for (int typeBatchIndex = 0; typeBatchIndex < batch.TypeBatches.Count; ++typeBatchIndex) { - ref var typeBatch = ref batch.TypeBatches[typeBatchIndex]; - for (int indexInTypeBatch = 0; indexInTypeBatch < typeBatch.ConstraintCount; ++indexInTypeBatch) - { - var handle = typeBatch.IndexToHandle[indexInTypeBatch]; - ref var constraintLocation = ref HandleToConstraint[handle.Value]; - Debug.Assert(constraintLocation.BatchIndex == batchIndex); - Debug.Assert(constraintLocation.IndexInTypeBatch == indexInTypeBatch); - Debug.Assert(constraintLocation.TypeId == typeBatch.TypeId); - Debug.Assert(batch.TypeIndexToTypeBatchIndex[constraintLocation.TypeId] == typeBatchIndex); - } + ValidateConstraintMaps(setIndex, batchIndex, typeBatchIndex); } } } From 5567ab8f06d2ddd49cb82912c16f1e1724408f55 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 18 Oct 2021 14:02:04 -0500 Subject: [PATCH 223/947] Fixed the goof. Validation juggling. Multithreading still has some cornerbugs. --- BepuPhysics/Constraints/TypeProcessor.cs | 44 +++++++++++++----------- BepuPhysics/IslandAwakener.cs | 4 --- BepuPhysics/Solver.cs | 19 ++++++++++ 3 files changed, 42 insertions(+), 25 deletions(-) diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index 605244b07..eee2eadc2 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -483,9 +483,9 @@ public unsafe sealed override int AllocateInTypeBatchForFallback(ref TypeBatch t { typeBatch.IndexToHandle[i].Value = -1; } - ValidateEmptyFallbackSlots(ref typeBatch); - ValidateFallbackAccessSafety(ref typeBatch, bodiesPerConstraint); - ValidateAccumulatedImpulses(ref typeBatch); + //ValidateEmptyFallbackSlots(ref typeBatch); + //ValidateFallbackAccessSafety(ref typeBatch, bodiesPerConstraint); + //ValidateAccumulatedImpulses(ref typeBatch); return indexInTypeBatch; } else @@ -502,9 +502,9 @@ public unsafe sealed override int AllocateInTypeBatchForFallback(ref TypeBatch t Debug.Assert(typeBatch.Projection.Length >= bundleCount * Unsafe.SizeOf()); Debug.Assert(typeBatch.BodyReferences.Length >= bundleCount * Unsafe.SizeOf()); Debug.Assert(typeBatch.AccumulatedImpulses.Length >= bundleCount * Unsafe.SizeOf()); - ValidateEmptyFallbackSlots(ref typeBatch); - ValidateFallbackAccessSafety(ref typeBatch, bodiesPerConstraint); - ValidateAccumulatedImpulses(ref typeBatch); + //ValidateEmptyFallbackSlots(ref typeBatch); + //ValidateFallbackAccessSafety(ref typeBatch, bodiesPerConstraint); + //ValidateAccumulatedImpulses(ref typeBatch); return indexInTypeBatch; } @@ -635,9 +635,9 @@ public override unsafe void Remove(ref TypeBatch typeBatch, int index, ref Buffe var innerLaneCount = BundleIndexing.GetLastSetLaneCount(Vector.GreaterThanOrEqual(Unsafe.As>(ref bodyReferences[lastBundleIndex]), Vector.Zero)); typeBatch.ConstraintCount = lastBundleIndex * Vector.Count + innerLaneCount; - ValidateEmptyFallbackSlots(ref typeBatch); - ValidateFallbackAccessSafety(ref typeBatch, bodiesPerConstraint); - ValidateAccumulatedImpulses(ref typeBatch); + //ValidateEmptyFallbackSlots(ref typeBatch); + //ValidateFallbackAccessSafety(ref typeBatch, bodiesPerConstraint); + //ValidateAccumulatedImpulses(ref typeBatch); } } else @@ -903,15 +903,15 @@ internal unsafe sealed override void AddSleepingToActiveForFallback(int sourceSe //This arises because sleeping constraint sets do not maintain the 'no constraints refer to the same bodies in a given bundle' rule; everything just got packed together. var batchIndex = solver.FallbackBatchThreshold; ref var sourceTypeBatch = ref solver.Sets[sourceSet].Batches[batchIndex].TypeBatches[sourceTypeBatchIndex]; - solver.ValidateSetOwnership(ref sourceTypeBatch, sourceSet); + //solver.ValidateSetOwnership(ref sourceTypeBatch, sourceSet); ref var targetTypeBatch = ref solver.ActiveSet.Batches[batchIndex].TypeBatches[targetTypeBatchIndex]; Debug.Assert(sourceTypeBatch.TypeId == targetTypeBatch.TypeId); var bodyCount = Unsafe.SizeOf() / Unsafe.SizeOf>(); //ValidateAccumulatedImpulses(ref targetTypeBatch); //ValidateEmptyFallbackSlots(ref targetTypeBatch); //ValidateFallbackAccessSafety(ref targetTypeBatch, bodyCount); - solver.ValidateConstraintMaps(0, batchIndex, targetTypeBatchIndex); - solver.ValidateConstraintMaps(sourceSet, batchIndex, sourceTypeBatchIndex); + //solver.ValidateConstraintMaps(0, batchIndex, targetTypeBatchIndex); + //solver.ValidateConstraintMaps(sourceSet, batchIndex, sourceTypeBatchIndex); int* bodyIndices = stackalloc int[bodyCount]; var sourceBundleCount = sourceTypeBatch.BundleCount; var sourceBodyReferences = sourceTypeBatch.BodyReferences.As(); @@ -927,6 +927,8 @@ internal unsafe sealed override void AddSleepingToActiveForFallback(int sourceSe //It's unclear if that's the best option- consider that it would always add a new bundle, but the members of the bundle might be able to be inserted in previous bundles. var bundleStartConstraintIndex = bundleIndexInSource * Vector.Count; var countInBundle = sourceTypeBatch.ConstraintCount - bundleStartConstraintIndex; + if (countInBundle > Vector.Count) + countInBundle = Vector.Count; ref var sourceBodyReferencesBundle = ref sourceBodyReferences[bundleIndexInSource]; ref var sourceAccumulatedImpulsesBundle = ref sourceAccumulatedImpulses[bundleIndexInSource]; ref var sourcePrestepBundle = ref sourcePrestepData[bundleIndexInSource]; @@ -961,10 +963,10 @@ internal unsafe sealed override void AddSleepingToActiveForFallback(int sourceSe location.IndexInTypeBatch = targetIndex; } } - ValidateAccumulatedImpulses(ref targetTypeBatch); - ValidateEmptyFallbackSlots(ref targetTypeBatch); - ValidateFallbackAccessSafety(ref targetTypeBatch, bodyCount); - solver.ValidateConstraintMaps(0, batchIndex, targetTypeBatchIndex); + //ValidateAccumulatedImpulses(ref targetTypeBatch); + //ValidateEmptyFallbackSlots(ref targetTypeBatch); + //ValidateFallbackAccessSafety(ref targetTypeBatch, bodyCount); + //solver.ValidateConstraintMaps(0, batchIndex, targetTypeBatchIndex); } internal unsafe sealed override void CopySleepingToActive( @@ -1348,11 +1350,11 @@ public static unsafe void GatherAndIntegrate.Zero); - orientation.Validate(validationMask); - position.Validate(validationMask); - velocity.Linear.Validate(validationMask); - velocity.Angular.Validate(validationMask); + //var validationMask = Vector.GreaterThanOrEqual(bodyIndices, Vector.Zero); + //orientation.Validate(validationMask); + //position.Validate(validationMask); + //velocity.Linear.Validate(validationMask); + //velocity.Angular.Validate(validationMask); } } diff --git a/BepuPhysics/IslandAwakener.cs b/BepuPhysics/IslandAwakener.cs index 578b17bde..0b33ec428 100644 --- a/BepuPhysics/IslandAwakener.cs +++ b/BepuPhysics/IslandAwakener.cs @@ -690,10 +690,6 @@ void EnsurePairCacheTypeCapacities(ref TypeAllocationSizes cache var baseConstraintsPerJob = sourceTypeBatch.ConstraintCount / jobCount; var remainder = sourceTypeBatch.ConstraintCount - baseConstraintsPerJob * jobCount; phaseTwoJobs.EnsureCapacity(phaseTwoJobs.Count + jobCount, pool); - if(batchIndex == fallbackIndex) - { - Console.WriteLine("Asdf"); - } var previousSourceEnd = 0; for (int jobIndex = 0; jobIndex < jobCount; ++jobIndex) { diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index 5537313b7..c65e8a0bc 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -591,6 +591,7 @@ internal void ValidateConstraintMaps(int setIndex, int batchIndex, int typeBatch var handle = typeBatch.IndexToHandle[indexInTypeBatch]; if (handle.Value >= 0) { + AssertConstraintHandleExists(handle); ref var constraintLocation = ref HandleToConstraint[handle.Value]; Debug.Assert(constraintLocation.SetIndex == setIndex); Debug.Assert(constraintLocation.BatchIndex == batchIndex); @@ -605,6 +606,7 @@ internal void ValidateConstraintMaps(int setIndex, int batchIndex, int typeBatch for (int indexInTypeBatch = 0; indexInTypeBatch < typeBatch.ConstraintCount; ++indexInTypeBatch) { var handle = typeBatch.IndexToHandle[indexInTypeBatch]; + AssertConstraintHandleExists(handle); ref var constraintLocation = ref HandleToConstraint[handle.Value]; Debug.Assert(constraintLocation.SetIndex == setIndex); Debug.Assert(constraintLocation.BatchIndex == batchIndex); @@ -615,6 +617,23 @@ internal void ValidateConstraintMaps(int setIndex, int batchIndex, int typeBatch } } + [Conditional("DEBUG")] + public void ValidateActiveFallbackConstraintMaps() + { + ref var set = ref Sets[0]; + if (set.Allocated) + { + if (set.Batches.Count > FallbackBatchThreshold) + { + ref var batch = ref set.Batches[FallbackBatchThreshold]; + for (int typeBatchIndex = 0; typeBatchIndex < batch.TypeBatches.Count; ++typeBatchIndex) + { + ValidateConstraintMaps(0, FallbackBatchThreshold, typeBatchIndex); + } + } + } + } + [Conditional("DEBUG")] public void ValidateConstraintMaps(bool activeOnly = false) { From f140b4d882485392c4e76280fc7bc9d15c392ac9 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 19 Oct 2021 15:30:18 -0500 Subject: [PATCH 224/947] Multithreaded with sequential fallback now passes validation testing. --- BepuPhysics/Constraints/TypeProcessor.cs | 3 +- .../EmbeddedSubsteppingTimestepper2.cs | 1 - BepuPhysics/IslandAwakener.cs | 135 +++++++++++++----- BepuPhysics/Solver.cs | 22 ++- 4 files changed, 122 insertions(+), 39 deletions(-) diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index eee2eadc2..5e39581f3 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -903,10 +903,10 @@ internal unsafe sealed override void AddSleepingToActiveForFallback(int sourceSe //This arises because sleeping constraint sets do not maintain the 'no constraints refer to the same bodies in a given bundle' rule; everything just got packed together. var batchIndex = solver.FallbackBatchThreshold; ref var sourceTypeBatch = ref solver.Sets[sourceSet].Batches[batchIndex].TypeBatches[sourceTypeBatchIndex]; - //solver.ValidateSetOwnership(ref sourceTypeBatch, sourceSet); ref var targetTypeBatch = ref solver.ActiveSet.Batches[batchIndex].TypeBatches[targetTypeBatchIndex]; Debug.Assert(sourceTypeBatch.TypeId == targetTypeBatch.TypeId); var bodyCount = Unsafe.SizeOf() / Unsafe.SizeOf>(); + //solver.ValidateSetOwnership(ref sourceTypeBatch, sourceSet); //ValidateAccumulatedImpulses(ref targetTypeBatch); //ValidateEmptyFallbackSlots(ref targetTypeBatch); //ValidateFallbackAccessSafety(ref targetTypeBatch, bodyCount); @@ -956,7 +956,6 @@ internal unsafe sealed override void AddSleepingToActiveForFallback(int sourceSe GatherScatter.CopyLane(ref sourceAccumulatedImpulsesBundle, sourceInnerIndex, ref targetAccumulatedImpulses[targetBundle], targetInner); GatherScatter.CopyLane(ref sourcePrestepBundle, sourceInnerIndex, ref targetPrestepData[targetBundle], targetInner); ref var location = ref constraintHandleToLocation[handle.Value]; - Debug.Assert(location.SetIndex == sourceSet); location.SetIndex = 0; location.BatchIndex = batchIndex; Debug.Assert(sourceTypeBatch.TypeId == location.TypeId); diff --git a/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs b/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs index 6c2b17e8b..60976c3a6 100644 --- a/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs +++ b/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs @@ -54,7 +54,6 @@ public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDi simulation.CollisionDetection(dt, threadDispatcher); CollisionsDetected?.Invoke(dt, threadDispatcher); - Debug.Assert(SubstepCount >= 0, "Substep count should be positive."); simulation.Solver.ValidateTrailingTypeBatchBodyReferences(); diff --git a/BepuPhysics/IslandAwakener.cs b/BepuPhysics/IslandAwakener.cs index 0b33ec428..b0aab79a6 100644 --- a/BepuPhysics/IslandAwakener.cs +++ b/BepuPhysics/IslandAwakener.cs @@ -7,6 +7,7 @@ using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; using System.Text; using System.Threading; @@ -181,9 +182,8 @@ enum PhaseTwoJobType CopyConstraintRegion, AddFallbackTypeBatchConstraints } - struct PhaseTwoJob + struct CopyConstraintRegionJob { - public PhaseTwoJobType Type; public int SourceStart; public int TargetStart; public int Count; @@ -194,6 +194,29 @@ struct PhaseTwoJob public int TargetTypeBatch; } + struct FallbackAddSource + { + public int SourceSet; + public int SourceTypeBatchIndex; + } + struct AddFallbackTypeBatchConstraintsJob + { + public Buffer Sources; + public int TypeId; + public int TargetTypeBatch; + } + + [StructLayout(LayoutKind.Explicit)] + struct PhaseTwoJob + { + [FieldOffset(0)] + public PhaseTwoJobType Type; + [FieldOffset(4)] + public CopyConstraintRegionJob CopyConstraintRegion; + [FieldOffset(8)] + public AddFallbackTypeBatchConstraintsJob AddFallbackTypeBatchConstraints; + } + bool resetActivityStates; QuickList uniqueSetIndices; QuickList phaseOneJobs; @@ -307,8 +330,8 @@ internal unsafe void ExecutePhaseOneJob(int index) internal unsafe void ExecutePhaseTwoJob(int index) { - ref var job = ref phaseTwoJobs[index]; - switch (job.Type) + ref var phaseTwoJob = ref phaseTwoJobs[index]; + switch (phaseTwoJob.Type) { case PhaseTwoJobType.BroadPhase: { @@ -364,17 +387,23 @@ internal unsafe void ExecutePhaseTwoJob(int index) //2) Sleeping constraints store their body references as body *handles* rather than body indices. //Pulling the type batches back into the active set requires translating those body handles to body indices. //3) The translation from body handle to body index requires that the bodies already have an active set identity, which is why the constraints wait until the second phase. + ref var job = ref phaseTwoJob.CopyConstraintRegion; Debug.Assert(solver.ActiveSet.Batches[job.Batch].TypeBatches[job.TargetTypeBatch].TypeId == solver.Sets[job.SourceSet].Batches[job.Batch].TypeBatches[job.SourceTypeBatch].TypeId); Debug.Assert(job.Batch != solver.FallbackBatchThreshold, "Fallback batches must only be handled by the fallback-specific job."); solver.TypeProcessors[job.TypeId].CopySleepingToActive( job.SourceSet, job.Batch, job.SourceTypeBatch, job.TargetTypeBatch, job.SourceStart, job.TargetStart, job.Count, bodies, solver); + solver.ValidateConstraintMaps(0, job.Batch, job.TargetTypeBatch, job.TargetStart, job.Count); } break; case PhaseTwoJobType.AddFallbackTypeBatchConstraints: { - Debug.Assert(solver.ActiveSet.Batches[job.Batch].TypeBatches[job.TargetTypeBatch].TypeId == solver.Sets[job.SourceSet].Batches[job.Batch].TypeBatches[job.SourceTypeBatch].TypeId); - solver.TypeProcessors[job.TypeId].AddSleepingToActiveForFallback(job.SourceSet, job.SourceTypeBatch, job.TargetTypeBatch, bodies, solver); + ref var job = ref phaseTwoJob.AddFallbackTypeBatchConstraints; + for (int i = 0; i < job.Sources.Length; ++i) + { + var source = job.Sources[i]; + solver.TypeProcessors[job.TypeId].AddSleepingToActiveForFallback(source.SourceSet, source.SourceTypeBatchIndex, job.TargetTypeBatch, bodies, solver); + } } break; } @@ -634,6 +663,9 @@ void EnsurePairCacheTypeCapacities(ref TypeAllocationSizes cache ref var activeBodySet = ref bodies.ActiveSet; ref var activeSolverSet = ref solver.ActiveSet; //TODO: The job sizes are a little goofy for single threaded execution. Easy enough to resolve with special case or dynamic size. + + //Multiple source sets can contribute to the same target type batch. Track those as we enumerate sets so we can create a single job for each target after the loop. + QuickDictionary, PrimitiveComparer> targetFallbackTypeBatchesToSources = highestNewBatchCount > solver.FallbackBatchThreshold ? targetFallbackTypeBatchesToSources = new(8, pool) : default; for (int i = 0; i < uniqueSetIndices.Count; ++i) { var sourceSetIndex = uniqueSetIndices[i]; @@ -675,9 +707,10 @@ void EnsurePairCacheTypeCapacities(ref TypeAllocationSizes cache const int constraintJobSize = 32; ref var sourceSet = ref solver.Sets[sourceSetIndex]; var fallbackIndex = solver.FallbackBatchThreshold; - for (int batchIndex = 0; batchIndex < sourceSet.Batches.Count; ++batchIndex) + + int synchronizedBatchCountInSource = sourceSet.Batches.Count > solver.FallbackBatchThreshold ? solver.FallbackBatchThreshold : sourceSet.Batches.Count; + for (int batchIndex = 0; batchIndex < synchronizedBatchCountInSource; ++batchIndex) { - var jobType = batchIndex == fallbackIndex ? PhaseTwoJobType.AddFallbackTypeBatchConstraints : PhaseTwoJobType.CopyConstraintRegion; ref var sourceBatch = ref sourceSet.Batches[batchIndex]; ref var targetBatch = ref activeSolverSet.Batches[batchIndex]; for (int sourceTypeBatchIndex = 0; sourceTypeBatchIndex < sourceBatch.TypeBatches.Count; ++sourceTypeBatchIndex) @@ -686,7 +719,7 @@ void EnsurePairCacheTypeCapacities(ref TypeAllocationSizes cache var targetTypeBatchIndex = targetBatch.TypeIndexToTypeBatchIndex[sourceTypeBatch.TypeId]; ref var targetTypeBatch = ref targetBatch.TypeBatches[targetTypeBatchIndex]; //TODO: It would be nice to be a little more clever about scheduling start and end points for the sake of avoiding partial bundles. - var jobCount = batchIndex == fallbackIndex ? 1 : Math.Max(1, sourceTypeBatch.ConstraintCount / constraintJobSize); + var jobCount = Math.Max(1, sourceTypeBatch.ConstraintCount / constraintJobSize); var baseConstraintsPerJob = sourceTypeBatch.ConstraintCount / jobCount; var remainder = sourceTypeBatch.ConstraintCount - baseConstraintsPerJob * jobCount; phaseTwoJobs.EnsureCapacity(phaseTwoJobs.Count + jobCount, pool); @@ -694,31 +727,31 @@ void EnsurePairCacheTypeCapacities(ref TypeAllocationSizes cache for (int jobIndex = 0; jobIndex < jobCount; ++jobIndex) { ref var job = ref phaseTwoJobs.AllocateUnsafely(); - job.Type = jobType; - job.TypeId = sourceTypeBatch.TypeId; - job.Batch = batchIndex; - job.SourceSet = sourceSetIndex; - job.SourceTypeBatch = sourceTypeBatchIndex; - job.TargetTypeBatch = targetTypeBatchIndex; - job.Count = jobIndex >= remainder ? baseConstraintsPerJob : baseConstraintsPerJob + 1; - job.SourceStart = previousSourceEnd; - job.TargetStart = targetTypeBatch.ConstraintCount; - previousSourceEnd += job.Count; - if (batchIndex < fallbackIndex) //The fallback batch add isn't a bulk copy; it'll handle incrementing count as necessary. + job.Type = PhaseTwoJobType.CopyConstraintRegion; + job.CopyConstraintRegion = new CopyConstraintRegionJob { - var oldBundleCount = targetTypeBatch.BundleCount; - targetTypeBatch.ConstraintCount += job.Count; - if (targetTypeBatch.BundleCount != oldBundleCount) + TypeId = sourceTypeBatch.TypeId, + Batch = batchIndex, + SourceSet = sourceSetIndex, + SourceTypeBatch = sourceTypeBatchIndex, + TargetTypeBatch = targetTypeBatchIndex, + Count = jobIndex >= remainder ? baseConstraintsPerJob : baseConstraintsPerJob + 1, + SourceStart = previousSourceEnd, + TargetStart = targetTypeBatch.ConstraintCount + }; + previousSourceEnd += job.CopyConstraintRegion.Count; + var oldBundleCount = targetTypeBatch.BundleCount; + targetTypeBatch.ConstraintCount += job.CopyConstraintRegion.Count; + if (targetTypeBatch.BundleCount != oldBundleCount) + { + //A new bundle was created; guarantee any trailing slots are set to -1. + //Since it's a whole new bundle that has not yet had any data set to it, we can safely just initialize the whole bundle's body references to -1. + var vectorCount = solver.TypeProcessors[job.CopyConstraintRegion.TypeId].BodiesPerConstraint; + var bundleStart = (Vector*)(targetTypeBatch.BodyReferences.Memory + (targetTypeBatch.BundleCount - 1) * vectorCount * Unsafe.SizeOf>()); + var negativeOne = new Vector(-1); + for (int vectorIndex = 0; vectorIndex < vectorCount; ++vectorIndex) { - //A new bundle was created; guarantee any trailing slots are set to -1. - //Since it's a whole new bundle that has not yet had any data set to it, we can safely just initialize the whole bundle's body references to -1. - var vectorCount = solver.TypeProcessors[job.TypeId].BodiesPerConstraint; - var bundleStart = (Vector*)(targetTypeBatch.BodyReferences.Memory + (targetTypeBatch.BundleCount - 1) * vectorCount * Unsafe.SizeOf>()); - var negativeOne = new Vector(-1); - for (int vectorIndex = 0; vectorIndex < vectorCount; ++vectorIndex) - { - bundleStart[vectorIndex] = negativeOne; - } + bundleStart[vectorIndex] = negativeOne; } } } @@ -726,7 +759,37 @@ void EnsurePairCacheTypeCapacities(ref TypeAllocationSizes cache Debug.Assert(targetTypeBatch.ConstraintCount <= targetTypeBatch.IndexToHandle.Length); } } + if (sourceSet.Batches.Count > fallbackIndex) + { + ref var sourceBatch = ref sourceSet.Batches[fallbackIndex]; + ref var targetBatch = ref activeSolverSet.Batches[fallbackIndex]; + for (int sourceTypeBatchIndex = 0; sourceTypeBatchIndex < sourceBatch.TypeBatches.Count; ++sourceTypeBatchIndex) + { + ref var sourceTypeBatch = ref sourceBatch.TypeBatches[sourceTypeBatchIndex]; + var targetTypeBatchIndex = targetBatch.TypeIndexToTypeBatchIndex[sourceTypeBatch.TypeId]; + if (!targetFallbackTypeBatchesToSources.FindOrAllocateSlot(targetTypeBatchIndex, pool, out var slotIndex)) + { + targetFallbackTypeBatchesToSources.Values[slotIndex] = new QuickList(8, pool); + } + targetFallbackTypeBatchesToSources.Values[slotIndex].Allocate(pool) = new FallbackAddSource { SourceSet = sourceSetIndex, SourceTypeBatchIndex = sourceTypeBatchIndex }; + } + } + } + } + if (targetFallbackTypeBatchesToSources.Keys.Allocated) + { + phaseTwoJobs.EnsureCapacity(phaseTwoJobs.Count + targetFallbackTypeBatchesToSources.Count, pool); + for (int i = 0; i < targetFallbackTypeBatchesToSources.Count; ++i) + { + ref var job = ref phaseTwoJobs.AllocateUnsafely(); + job.Type = PhaseTwoJobType.AddFallbackTypeBatchConstraints; + ref var list = ref targetFallbackTypeBatchesToSources.Values[i]; + job.AddFallbackTypeBatchConstraints.Sources = list.Span.Slice(list.Count); + job.AddFallbackTypeBatchConstraints.TargetTypeBatch = targetFallbackTypeBatchesToSources.Keys[i]; + job.AddFallbackTypeBatchConstraints.TypeId = solver.ActiveSet.Batches[solver.FallbackBatchThreshold].TypeBatches[job.AddFallbackTypeBatchConstraints.TargetTypeBatch].TypeId; } + //Note that the per target lists will be disposed in the DisposeForCompletedAwakenings, since the spans created for the per-target lists are used in the PhaseTwoJobs. + targetFallbackTypeBatchesToSources.Dispose(pool); } return (phaseOneJobs.Count, phaseTwoJobs.Count); } @@ -754,6 +817,14 @@ internal void DisposeForCompletedAwakenings(ref QuickList setIndices) } phaseOneJobs.Dispose(pool); + for (int i = 0; i < phaseTwoJobs.Count; ++i) + { + ref var job = ref phaseTwoJobs[i]; + if (job.Type == PhaseTwoJobType.AddFallbackTypeBatchConstraints) + { + pool.Return(ref job.AddFallbackTypeBatchConstraints.Sources); + } + } phaseTwoJobs.Dispose(pool); } } diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index c65e8a0bc..c842d6deb 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -235,7 +235,7 @@ public void Register() where TDescription : unmanaged, IConstraint public bool ConstraintExists(ConstraintHandle constraintHandle) { //A constraint location with a negative set index marks a mapping slot as unused. - return constraintHandle.Value >= 0 && constraintHandle.Value < HandleToConstraint.Length && HandleToConstraint[constraintHandle.Value].SetIndex >= 0; + return constraintHandle.Value >= 0 && constraintHandle.Value <= HandlePool.HighestPossiblyClaimedId && HandleToConstraint[constraintHandle.Value].SetIndex >= 0; } /// @@ -578,14 +578,15 @@ public unsafe void ValidateExistingHandles(bool activeOnly = false) } [Conditional("DEBUG")] - internal void ValidateConstraintMaps(int setIndex, int batchIndex, int typeBatchIndex) + internal void ValidateConstraintMaps(int setIndex, int batchIndex, int typeBatchIndex, int constraintStart, int constraintCount) { ref var set = ref Sets[setIndex]; ref var batch = ref set.Batches[batchIndex]; ref var typeBatch = ref batch.TypeBatches[typeBatchIndex]; + var end = constraintStart + constraintCount; if (batchIndex == FallbackBatchThreshold) { - for (int indexInTypeBatch = 0; indexInTypeBatch < typeBatch.ConstraintCount; ++indexInTypeBatch) + for (int indexInTypeBatch = constraintStart; indexInTypeBatch < end; ++indexInTypeBatch) { //Fallback batches can have empty slots, marked with a -1 in the handle slot. var handle = typeBatch.IndexToHandle[indexInTypeBatch]; @@ -603,7 +604,7 @@ internal void ValidateConstraintMaps(int setIndex, int batchIndex, int typeBatch } else { - for (int indexInTypeBatch = 0; indexInTypeBatch < typeBatch.ConstraintCount; ++indexInTypeBatch) + for (int indexInTypeBatch = constraintStart; indexInTypeBatch < end; ++indexInTypeBatch) { var handle = typeBatch.IndexToHandle[indexInTypeBatch]; AssertConstraintHandleExists(handle); @@ -616,6 +617,11 @@ internal void ValidateConstraintMaps(int setIndex, int batchIndex, int typeBatch } } } + [Conditional("DEBUG")] + internal void ValidateConstraintMaps(int setIndex, int batchIndex, int typeBatchIndex) + { + ValidateConstraintMaps(setIndex, batchIndex, typeBatchIndex, 0, Sets[setIndex].Batches[batchIndex].TypeBatches[typeBatchIndex].ConstraintCount); + } [Conditional("DEBUG")] public void ValidateActiveFallbackConstraintMaps() @@ -637,6 +643,14 @@ public void ValidateActiveFallbackConstraintMaps() [Conditional("DEBUG")] public void ValidateConstraintMaps(bool activeOnly = false) { + for (int i = 0; i < HandleToConstraint.Length; ++i) + { + var handle = new ConstraintHandle { Value = i }; + if (ConstraintExists(handle)) + { + AssertConstraintHandleExists(handle); + } + } var setCount = activeOnly ? 1 : Sets.Length; for (int setIndex = 0; setIndex < setCount; ++setIndex) { From f84b2930ccb7dffc467a60775f054f121fe4b222 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 19 Oct 2021 16:28:33 -0500 Subject: [PATCH 225/947] Sequential fallback pretty much done. --- BepuPhysics/Solver.cs | 16 +++---- .../Constraints/ConstraintLineExtractor.cs | 42 +++++++++++-------- Demos/Demos/NewtDemo.cs | 1 - Demos/Demos/RopeTwistDemo.cs | 2 +- Demos/Program.cs | 2 +- 5 files changed, 34 insertions(+), 29 deletions(-) diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index c842d6deb..075c1e1a7 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -269,7 +269,7 @@ private void ValidateBodyReference(int body, int expectedCount, ref ConstraintBa } [Conditional("DEBUG")] - public unsafe void ValidateTrailingTypeBatchBodyReferences() + internal unsafe void ValidateTrailingTypeBatchBodyReferences() { ref var set = ref ActiveSet; for (int batchIndex = 0; batchIndex < set.Batches.Count; ++batchIndex) @@ -295,7 +295,7 @@ public unsafe void ValidateTrailingTypeBatchBodyReferences() } } [Conditional("DEBUG")] - public unsafe void ValidateFallbackBatchEmptySlotReferences() + internal unsafe void ValidateFallbackBatchEmptySlotReferences() { ref var set = ref ActiveSet; if (set.Batches.Count > FallbackBatchThreshold) @@ -318,7 +318,7 @@ public unsafe void ValidateFallbackBatchEmptySlotReferences() } [Conditional("DEBUG")] - public unsafe void ValidateFallbackBatchAccessSafety() + internal unsafe void ValidateFallbackBatchAccessSafety() { ref var set = ref ActiveSet; if (set.Batches.Count > FallbackBatchThreshold) @@ -366,7 +366,7 @@ public unsafe void ValidateFallbackBatchAccessSafety() } } } - //Console.WriteLine($"Average fallback occupancy: {Vector.Count * occupiedLaneCountAcrossBatch / (double)(totalBundleCount * Vector.Count):G3} / {Vector.Count}, total bundle count: {totalBundleCount}"); + Console.WriteLine($"Average fallback occupancy: {Vector.Count * occupiedLaneCountAcrossBatch / (double)(totalBundleCount * Vector.Count):G3} / {Vector.Count}, total bundle count: {totalBundleCount}"); } } [Conditional("DEBUG")] @@ -412,7 +412,7 @@ public void LoopBody(float impulse) } [Conditional("DEBUG")] - public unsafe void ValidateFallbackBatchAccumulatedImpulses() + internal unsafe void ValidateFallbackBatchAccumulatedImpulses() { ref var set = ref ActiveSet; if (set.Batches.Count > FallbackBatchThreshold) @@ -442,7 +442,7 @@ public unsafe void ValidateFallbackBatchAccumulatedImpulses() } [Conditional("DEBUG")] - public unsafe void ValidateExistingHandles(bool activeOnly = false) + internal unsafe void ValidateExistingHandles(bool activeOnly = false) { var maxBodySet = activeOnly ? 1 : bodies.Sets.Length; for (int i = 0; i < maxBodySet; ++i) @@ -624,7 +624,7 @@ internal void ValidateConstraintMaps(int setIndex, int batchIndex, int typeBatch } [Conditional("DEBUG")] - public void ValidateActiveFallbackConstraintMaps() + internal void ValidateActiveFallbackConstraintMaps() { ref var set = ref Sets[0]; if (set.Allocated) @@ -641,7 +641,7 @@ public void ValidateActiveFallbackConstraintMaps() } [Conditional("DEBUG")] - public void ValidateConstraintMaps(bool activeOnly = false) + internal void ValidateConstraintMaps(bool activeOnly = false) { for (int i = 0; i < HandleToConstraint.Length; ++i) { diff --git a/DemoRenderer/Constraints/ConstraintLineExtractor.cs b/DemoRenderer/Constraints/ConstraintLineExtractor.cs index af8b8af62..a076fa50f 100644 --- a/DemoRenderer/Constraints/ConstraintLineExtractor.cs +++ b/DemoRenderer/Constraints/ConstraintLineExtractor.cs @@ -49,16 +49,19 @@ public unsafe override void ExtractLines(Bodies bodies, int setIndex, ref TypeBa var tint = new Vector3(1, 1, 1); for (int i = constraintStart; i < constraintEnd; ++i) { - BundleIndexing.GetBundleIndices(i, out var bundleIndex, out var innerIndex); - ref var prestepBundle = ref Unsafe.Add(ref prestepStart, bundleIndex); - ref var referencesBundle = ref Unsafe.Add(ref referencesStart, bundleIndex); - ref var firstReference = ref Unsafe.As>(ref referencesBundle); - for (int j = 0; j < bodyCount; ++j) + if (typeBatch.IndexToHandle[i].Value >= 0) { - //Active set constraint body references refer directly to the body index. - bodyIndices[j] = GatherScatter.Get(ref Unsafe.Add(ref firstReference, j), innerIndex); + BundleIndexing.GetBundleIndices(i, out var bundleIndex, out var innerIndex); + ref var prestepBundle = ref Unsafe.Add(ref prestepStart, bundleIndex); + ref var referencesBundle = ref Unsafe.Add(ref referencesStart, bundleIndex); + ref var firstReference = ref Unsafe.As>(ref referencesBundle); + for (int j = 0; j < bodyCount; ++j) + { + //Active set constraint body references refer directly to the body index. + bodyIndices[j] = GatherScatter.Get(ref Unsafe.Add(ref firstReference, j), innerIndex); + } + extractor.ExtractLines(ref GatherScatter.GetOffsetInstance(ref prestepBundle, innerIndex), setIndex, bodyIndices, bodies, ref tint, ref lines); } - extractor.ExtractLines(ref GatherScatter.GetOffsetInstance(ref prestepBundle, innerIndex), setIndex, bodyIndices, bodies, ref tint, ref lines); } } else @@ -66,18 +69,21 @@ public unsafe override void ExtractLines(Bodies bodies, int setIndex, ref TypeBa var tint = new Vector3(0.4f, 0.4f, 0.8f); for (int i = constraintStart; i < constraintEnd; ++i) { - BundleIndexing.GetBundleIndices(i, out var bundleIndex, out var innerIndex); - ref var prestepBundle = ref Unsafe.Add(ref prestepStart, bundleIndex); - ref var referencesBundle = ref Unsafe.Add(ref referencesStart, bundleIndex); - ref var firstReference = ref Unsafe.As>(ref referencesBundle); - for (int j = 0; j < bodyCount; ++j) + if (typeBatch.IndexToHandle[i].Value >= 0) { - //Inactive constraints store body references in the form of handles, so we have to follow the indirection. - var bodyHandle = GatherScatter.Get(ref Unsafe.Add(ref firstReference, j), innerIndex); - Debug.Assert(bodies.HandleToLocation[bodyHandle].SetIndex == setIndex); - bodyIndices[j] = bodies.HandleToLocation[bodyHandle].Index; + BundleIndexing.GetBundleIndices(i, out var bundleIndex, out var innerIndex); + ref var prestepBundle = ref Unsafe.Add(ref prestepStart, bundleIndex); + ref var referencesBundle = ref Unsafe.Add(ref referencesStart, bundleIndex); + ref var firstReference = ref Unsafe.As>(ref referencesBundle); + for (int j = 0; j < bodyCount; ++j) + { + //Inactive constraints store body references in the form of handles, so we have to follow the indirection. + var bodyHandle = GatherScatter.Get(ref Unsafe.Add(ref firstReference, j), innerIndex); + Debug.Assert(bodies.HandleToLocation[bodyHandle].SetIndex == setIndex); + bodyIndices[j] = bodies.HandleToLocation[bodyHandle].Index; + } + extractor.ExtractLines(ref GatherScatter.GetOffsetInstance(ref prestepBundle, innerIndex), setIndex, bodyIndices, bodies, ref tint, ref lines); } - extractor.ExtractLines(ref GatherScatter.GetOffsetInstance(ref prestepBundle, innerIndex), setIndex, bodyIndices, bodies, ref tint, ref lines); } } } diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index d1e43dbd3..31f4466f2 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -866,6 +866,5 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) } - } } diff --git a/Demos/Demos/RopeTwistDemo.cs b/Demos/Demos/RopeTwistDemo.cs index 8c95b908b..df089db99 100644 --- a/Demos/Demos/RopeTwistDemo.cs +++ b/Demos/Demos/RopeTwistDemo.cs @@ -83,7 +83,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var filters = new CollidableProperty(); Simulation = Simulation.Create(BufferPool, new RopeNarrowPhaseCallbacks { ContactSpringiness = new SpringSettings(2000, 1), Filters = filters }, - new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(60), solverIterationCount: 1, solverFallbackBatchThreshold: 512); + new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(60), solverIterationCount: 1, solverFallbackBatchThreshold: 64); //Simulation = Simulation.Create(BufferPool, // new RopeNarrowPhaseCallbacks { ContactSpringiness = new SpringSettings(2000, 1), Filters = filters }, // new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper2(60), solverIterationCount: 1, solverFallbackBatchThreshold: 512); diff --git a/Demos/Program.cs b/Demos/Program.cs index 62f8240e6..ecdd929c9 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -22,7 +22,7 @@ static void Main(string[] args) //HeadlessTest.Test(content, 4, 32, 512); //HeadlessTest.Test(content, 4, 32, 512); //HeadlessTest.Test(content, 4, 32, 512); - //HeadlessTest.Test(content, 4, 256, 512); + //HeadlessTest.Test(content, 4, 128, 512); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); From 7ffb3cd7d857f4c4c9c5973b8efea44d9840edd2 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 23 Oct 2021 21:42:01 -0500 Subject: [PATCH 226/947] Fixed a missing sort for sets being awakened (newly required by sequential fallback). Added InvasiveHashDiagnostics to help find goofs in the future. Seems to pass determinism tests now. --- .../CollidableOverlapFinder.cs | 1 + .../CollisionDetection/ConstraintRemover.cs | 2 +- .../ContactConstraintAccessor.cs | 1 + BepuPhysics/CollisionDetection/NarrowPhase.cs | 3 +- .../NarrowPhaseConstraintUpdate.cs | 3 +- .../NarrowPhasePendingConstraintAdds.cs | 14 +- .../CollisionDetection/NarrowPhasePreflush.cs | 19 +-- BepuPhysics/Constraints/TypeProcessor.cs | 1 + .../EmbeddedSubsteppingTimestepper2.cs | 12 +- BepuPhysics/InvasiveHashDiagnostics.cs | 137 ++++++++++++++++++ BepuPhysics/IslandAwakener.cs | 4 + BepuPhysics/IslandSleeper.cs | 1 + BepuPhysics/Solver.cs | 44 +++++- 13 files changed, 210 insertions(+), 32 deletions(-) create mode 100644 BepuPhysics/InvasiveHashDiagnostics.cs diff --git a/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs b/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs index a2f0fb56f..7aede53cc 100644 --- a/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs +++ b/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs @@ -128,6 +128,7 @@ public override void DispatchOverlaps(float dt, IThreadDispatcher threadDispatch intertreeTestContext.PrepareJobs(ref broadPhase.ActiveTree, ref broadPhase.StaticTree, intertreeHandlers, threadDispatcher.ThreadCount); nextJobIndex = -1; threadDispatcher.DispatchWorkers(workerAction); + //workerAction(0); selfTestContext.CompleteSelfTest(); intertreeTestContext.CompleteTest(); } diff --git a/BepuPhysics/CollisionDetection/ConstraintRemover.cs b/BepuPhysics/CollisionDetection/ConstraintRemover.cs index c69fdbb3b..30cd39839 100644 --- a/BepuPhysics/CollisionDetection/ConstraintRemover.cs +++ b/BepuPhysics/CollisionDetection/ConstraintRemover.cs @@ -365,7 +365,7 @@ public void RemoveConstraintsFromBatchReferencedHandles() } QuickList allocationIdsToFree; - public void RemoveConstraintsFromFallbackBatch() + public void RemoveConstraintsFromFallbackBatchReferencedHandles() { Debug.Assert(solver.ActiveSet.Batches.Count > solver.FallbackBatchThreshold); for (int i = 0; i < batches.BatchCount; ++i) diff --git a/BepuPhysics/CollisionDetection/ContactConstraintAccessor.cs b/BepuPhysics/CollisionDetection/ContactConstraintAccessor.cs index 3b4a45e23..7e1b9c85e 100644 --- a/BepuPhysics/CollisionDetection/ContactConstraintAccessor.cs +++ b/BepuPhysics/CollisionDetection/ContactConstraintAccessor.cs @@ -145,6 +145,7 @@ public void ExtractContactPrestepAndImpulses(ConstraintHandle constr //Note that the vast majority of the 'work' done by these accessor implementations is just type definitions used to call back into some other functions that need that type knowledge. public abstract class ContactConstraintAccessor : ContactConstraintAccessor + where TBodyHandles : unmanaged where TConstraintDescription : unmanaged, IConstraintDescription where TContactImpulses : unmanaged where TConstraintCache : unmanaged, IPairCacheEntry diff --git a/BepuPhysics/CollisionDetection/NarrowPhase.cs b/BepuPhysics/CollisionDetection/NarrowPhase.cs index 66463de17..7d637f52d 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhase.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhase.cs @@ -222,7 +222,7 @@ void ExecuteFlushJob(ref NarrowPhaseFlushJob job, BufferPool threadPool) ConstraintRemover.RemoveConstraintsFromBatchReferencedHandles(); break; case NarrowPhaseFlushJobType.RemoveConstraintsFromFallbackBatch: - ConstraintRemover.RemoveConstraintsFromFallbackBatch(); + ConstraintRemover.RemoveConstraintsFromFallbackBatchReferencedHandles(); break; case NarrowPhaseFlushJobType.RemoveConstraintFromTypeBatch: ConstraintRemover.RemoveConstraintsFromTypeBatch(job.Index); @@ -271,6 +271,7 @@ public void Flush(IThreadDispatcher threadDispatcher = null) flushJobIndex = -1; this.threadDispatcher = threadDispatcher; threadDispatcher.DispatchWorkers(flushWorkerLoop); + //flushWorkerLoop(0); this.threadDispatcher = null; } //var end = Stopwatch.GetTimestamp(); diff --git a/BepuPhysics/CollisionDetection/NarrowPhaseConstraintUpdate.cs b/BepuPhysics/CollisionDetection/NarrowPhaseConstraintUpdate.cs index e3e81cf53..c83f82ea7 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhaseConstraintUpdate.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhaseConstraintUpdate.cs @@ -147,7 +147,7 @@ private unsafe void RedistributeImpulses( [MethodImpl(MethodImplOptions.AggressiveInlining)] unsafe void RequestAddConstraint(int workerIndex, int manifoldConstraintType, ref CollidablePair pair, PairCacheIndex constraintCacheIndex, ref TContactImpulses newImpulses, - ref TDescription description, TBodyHandles bodyHandles) where TDescription : unmanaged, IConstraintDescription + ref TDescription description, TBodyHandles bodyHandles) where TBodyHandles : unmanaged where TDescription : unmanaged, IConstraintDescription { //Note that this branch is (was?) JIT constant. if (typeof(TBodyHandles) != typeof(TwoBodyHandles) && typeof(TBodyHandles) != typeof(int)) @@ -160,6 +160,7 @@ unsafe void RequestAddConstraint(i public unsafe void UpdateConstraint(int workerIndex, ref CollidablePair pair, int manifoldTypeAsConstraintType, ref TConstraintCache newConstraintCache, ref TCollisionCache collisionCache, ref TDescription description, TBodyHandles bodyHandles) + where TBodyHandles : unmanaged where TConstraintCache : unmanaged, IPairCacheEntry where TCollisionCache : unmanaged, IPairCacheEntry where TDescription : unmanaged, IConstraintDescription diff --git a/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs b/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs index 2bee68413..d90f383df 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs @@ -22,7 +22,7 @@ public partial class NarrowPhase public struct PendingConstraintAddCache { BufferPool pool; - struct PendingConstraint where TDescription : unmanaged, IConstraintDescription + struct PendingConstraint where TBodyHandles : unmanaged where TDescription : unmanaged, IConstraintDescription { //Note the memory ordering. Collidable pair comes first; deterministic flushes rely the memory layout to sort pending constraints. public CollidablePair Pair; @@ -48,7 +48,7 @@ public PendingConstraintAddCache(BufferPool pool, int minimumConstraintCountPerC public unsafe void AddConstraint(int manifoldConstraintType, ref CollidablePair pair, PairCacheIndex constraintCacheIndex, TBodyHandles bodyHandles, ref TDescription constraintDescription, ref TContactImpulses impulses) - where TDescription : unmanaged, IConstraintDescription + where TBodyHandles : unmanaged where TDescription : unmanaged, IConstraintDescription { ref var cache = ref pendingConstraintsByType[manifoldConstraintType]; var byteIndex = cache.Allocate>(minimumConstraintCountPerCache, pool); @@ -62,7 +62,7 @@ public unsafe void AddConstraint(i [MethodImpl(MethodImplOptions.AggressiveInlining)] public static unsafe void SequentialAddToSimulation(ref UntypedList list, int narrowPhaseConstraintTypeId, Simulation simulation, PairCache pairCache) - where TDescription : unmanaged, IConstraintDescription + where TBodyHandles : unmanaged where TDescription : unmanaged, IConstraintDescription { if (list.Buffer.Allocated) { @@ -96,7 +96,7 @@ internal void FlushSequentially(Simulation simulation, PairCache pairCache) [MethodImpl(MethodImplOptions.AggressiveInlining)] static unsafe void AddToSimulationSpeculative( ref PendingConstraint constraint, int batchIndex, Simulation simulation, PairCache pairCache) - where TDescription : unmanaged, IConstraintDescription + where TBodyHandles : unmanaged where TDescription : unmanaged, IConstraintDescription { //This function takes full responsibility for what a Simulation.Add would do, plus the need to complete the constraint add in the pair cache. //1) Allocate in solver batch and type batch. @@ -145,7 +145,7 @@ static unsafe void AddToSimulationSpeculative( ref UntypedList list, int narrowPhaseConstraintTypeId, ref Buffer> speculativeBatchIndices, Simulation simulation, PairCache pairCache) - where TDescription : unmanaged, IConstraintDescription + where TBodyHandles : unmanaged where TDescription : unmanaged, IConstraintDescription { if (list.Buffer.Allocated) { @@ -177,7 +177,7 @@ internal void FlushWithSpeculativeBatches(Simulation simulation, ref PairCache p [MethodImpl(MethodImplOptions.AggressiveInlining)] public static unsafe void DeterministicAdd( int typeIndex, ref SortConstraintTarget target, OverlapWorker[] overlapWorkers, Simulation simulation, ref PairCache pairCache) - where TDescription : unmanaged, IConstraintDescription + where TBodyHandles : unmanaged where TDescription : unmanaged, IConstraintDescription { ref var cache = ref overlapWorkers[target.WorkerIndex].PendingConstraints; ref var constraint = ref Unsafe.As>( @@ -261,7 +261,7 @@ internal int CountConstraints() [MethodImpl(MethodImplOptions.AggressiveInlining)] void AddConstraint(int workerIndex, int manifoldConstraintType, ref CollidablePair pair, PairCacheIndex constraintCacheIndex, ref TContactImpulses impulses, TBodyHandles bodyHandles, ref TDescription constraintDescription) - where TDescription : unmanaged, IConstraintDescription + where TBodyHandles : unmanaged where TDescription : unmanaged, IConstraintDescription { overlapWorkers[workerIndex].PendingConstraints.AddConstraint(manifoldConstraintType, ref pair, constraintCacheIndex, bodyHandles, ref constraintDescription, ref impulses); } diff --git a/BepuPhysics/CollisionDetection/NarrowPhasePreflush.cs b/BepuPhysics/CollisionDetection/NarrowPhasePreflush.cs index 56faff3c5..2ff6fabc6 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhasePreflush.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhasePreflush.cs @@ -205,7 +205,7 @@ unsafe void ExecutePreflushJob(int workerIndex, ref PreflushJob job) break; } } - + protected override void OnPreflush(IThreadDispatcher threadDispatcher, bool deterministic) { var threadCount = threadDispatcher == null ? 1 : threadDispatcher.ThreadCount; @@ -323,10 +323,7 @@ protected override void OnPreflush(IThreadDispatcher threadDispatcher, bool dete //var start = Stopwatch.GetTimestamp(); preflushJobIndex = -1; threadDispatcher.DispatchWorkers(preflushWorkerLoop); - //for (int i = 0; i < preflushJobs.Count; ++i) - //{ - // ExecutePreflushJob(0, ref preflushJobs[i]); - //} + //preflushWorkerLoop(0); //var end = Stopwatch.GetTimestamp(); //Console.WriteLine($"Preflush phase 1 time (us): {1e6 * (end - start) / Stopwatch.Frequency}"); @@ -338,14 +335,10 @@ protected override void OnPreflush(IThreadDispatcher threadDispatcher, bool dete { preflushJobs.Add(new PreflushJob { Type = PreflushJobType.AwakenerPhaseTwo, JobIndex = i }, Pool); } - //start = Stopwatch.GetTimestamp(); preflushJobIndex = -1; threadDispatcher.DispatchWorkers(preflushWorkerLoop); - //for (int i = 0; i < preflushJobs.Count; ++i) - //{ - // ExecutePreflushJob(0, ref preflushJobs[i]); - //} + //preflushWorkerLoop(0); //end = Stopwatch.GetTimestamp(); //Console.WriteLine($"Preflush phase 2 time (us): {1e6 * (end - start) / Stopwatch.Frequency}"); @@ -366,14 +359,10 @@ protected override void OnPreflush(IThreadDispatcher threadDispatcher, bool dete //ExecutePreflushJob(0, ref job); } FreshnessChecker.CreateJobs(threadCount, ref preflushJobs, Pool, originalPairCacheMappingCount); - //start = Stopwatch.GetTimestamp(); preflushJobIndex = -1; threadDispatcher.DispatchWorkers(preflushWorkerLoop); - //for (int i = 0; i < preflushJobs.Count; ++i) - //{ - // ExecutePreflushJob(0, ref preflushJobs[i]); - //} + //preflushWorkerLoop(0); //end = Stopwatch.GetTimestamp(); //Console.WriteLine($"Preflush phase 3 time (us): {1e6 * (end - start) / Stopwatch.Frequency}"); diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index 5e39581f3..20046c482 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -462,6 +462,7 @@ public unsafe sealed override int AllocateInTypeBatchForFallback(ref TypeBatch t var newConstraintCount = indexInTypeBatch + 1; if (newConstraintCount >= typeBatch.IndexToHandle.Length) { + Debug.Assert(pool != null, "Looks like a user that doesn't have access to a pool (the awakener, probably?) tried to add a constraint without preallocating enough room."); InternalResize(ref typeBatch, pool, newConstraintCount * 2); } typeBatch.ConstraintCount = newConstraintCount; diff --git a/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs b/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs index 60976c3a6..82b583bc5 100644 --- a/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs +++ b/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs @@ -56,11 +56,11 @@ public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDi CollisionsDetected?.Invoke(dt, threadDispatcher); Debug.Assert(SubstepCount >= 0, "Substep count should be positive."); - simulation.Solver.ValidateTrailingTypeBatchBodyReferences(); - simulation.Solver.ValidateFallbackBatchEmptySlotReferences(); - simulation.Solver.ValidateFallbackBatchAccessSafety(); - simulation.Solver.ValidateFallbackBatchAccumulatedImpulses(); - simulation.Solver.ValidateConstraintMaps(); + //simulation.Solver.ValidateTrailingTypeBatchBodyReferences(); + //simulation.Solver.ValidateFallbackBatchEmptySlotReferences(); + //simulation.Solver.ValidateFallbackBatchAccessSafety(); + //simulation.Solver.ValidateFallbackBatchAccumulatedImpulses(); + //simulation.Solver.ValidateConstraintMaps(); var constrainedBodySet = simulation.Solver.PrepareConstraintIntegrationResponsibilities(SubstepCount, threadDispatcher); simulation.Profiler.Start(simulation.Solver); simulation.Solver.SolveStep2(dt, threadDispatcher); @@ -71,7 +71,7 @@ public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDi simulation.Profiler.End(simulation.PoseIntegrator); simulation.Solver.DisposeConstraintIntegrationResponsibilities(); SubstepsComplete?.Invoke(dt, threadDispatcher); - + simulation.IncrementallyOptimizeDataStructures(threadDispatcher); } } diff --git a/BepuPhysics/InvasiveHashDiagnostics.cs b/BepuPhysics/InvasiveHashDiagnostics.cs new file mode 100644 index 000000000..589198d08 --- /dev/null +++ b/BepuPhysics/InvasiveHashDiagnostics.cs @@ -0,0 +1,137 @@ +using BepuUtilities.Collections; +using System; +using System.Collections.Generic; +using System.Linq; +using System.Runtime.CompilerServices; +using System.Text; +using System.Threading.Tasks; + +namespace BepuPhysics +{ + /// + /// Hardcoded hash types used by invasive hash diagnostics. + /// + public enum HashDiagnosticType + { + AddSleepingToActiveForFallback, + SolverBodyReferenceBeforeCollisionDetection, + SolverBodyReferenceBeforePreflush, + SolverBodyReferenceAfterPreflushPhase1, + SolverBodyReferenceAfterPreflushPhase2, + SolverBodyReferenceAfterPreflushPhase3, + SolverBodyReferenceAfterPreflush, + SolverBodyReferenceBeforeSolver, + SolverBodyReferenceAfterSolver, + SolverBodyReferenceAtEnd, + DeterministicConstraintAdd, + AddToSimulationSpeculative, + AddToSimulationSpeculativeFallbackSolverReferences, + EnqueueStaleRemoval, + RemoveConstraintsFromFallbackBatchReferencedHandles, + RemoveConstraintsFromBatchReferencedHandles, + RemoveConstraintsFromBodyLists, + RemoveConstraintsFromTypeBatch, + ReturnConstraintHandles, + PreflushJobs, + AllocateInTypeBatchForFallback, + AllocateInTypeBatchForFallbackProbes, + AllocateInBatch, + TypeProcessorRemove + } + /// + /// Helper diagnostics class for monitoring internal state determinism across runs. + /// Typically used by inserting tests into engine internals. + /// + public class InvasiveHashDiagnostics + { + /// + /// This is meant as an internal diagnostic utility, so hardcoding some things is totally fine. + /// + const int HashTypeCount = 24; + public static InvasiveHashDiagnostics Instance; + public static void Initialize(int runCount, int hashCapacityPerType) + { + var instance = new InvasiveHashDiagnostics(); + instance.Hashes = new int[runCount][][]; + for (int runIndex = 0; runIndex < runCount; ++runIndex) + { + instance.Hashes[runIndex] = new int[HashTypeCount][]; + for (int hashTypeIndex = 0; hashTypeIndex < HashTypeCount; ++hashTypeIndex) + { + instance.Hashes[runIndex][hashTypeIndex] = new int[hashCapacityPerType]; + } + } + Instance = instance; + } + + public int CurrentRunIndex; + public int CurrentHashIndex; + public int[][][] Hashes; + + public bool TypeIsActive(HashDiagnosticType hashType) + { + return CurrentRunIndex >= 0 && CurrentRunIndex < Hashes.Length && (int)hashType >= 0 && (int)hashType < Hashes[CurrentRunIndex].Length && CurrentHashIndex >= 0 && CurrentHashIndex < Hashes[CurrentRunIndex][(int)hashType].Length; + } + + public void MoveToNextRun() + { + ++CurrentRunIndex; + CurrentHashIndex = 0; + } + + public void MoveToNextHashFrame() + { + if (CurrentRunIndex > Hashes.Length) + return; + if (CurrentHashIndex < 0) + throw new ArgumentException($"Invalid hash index: {CurrentHashIndex}"); + for (int hashTypeIndex = 0; hashTypeIndex < Hashes[CurrentRunIndex].Length; ++hashTypeIndex) + { + if (CurrentHashIndex >= Hashes[CurrentRunIndex][hashTypeIndex].Length) + continue; + for (int previousRunIndex = 0; previousRunIndex < CurrentRunIndex; ++previousRunIndex) + { + if (Hashes[CurrentRunIndex][hashTypeIndex][CurrentHashIndex] != Hashes[previousRunIndex][hashTypeIndex][CurrentHashIndex]) + { + Console.WriteLine($"Hash failure on {(HashDiagnosticType)hashTypeIndex} frame {CurrentHashIndex}, current run {CurrentRunIndex} vs previous {previousRunIndex}: {Hashes[CurrentRunIndex][hashTypeIndex][CurrentHashIndex] } vs {Hashes[previousRunIndex][hashTypeIndex][CurrentHashIndex]}."); + } + } + } + ++CurrentHashIndex; + } + + public ref int GetHashForType(HashDiagnosticType hashType) + { + return ref Hashes[CurrentRunIndex][(int)hashType][CurrentHashIndex]; + } + + public void ContributeToHash(ref int hash, int value) + { + hash = HashHelper.Rehash(hash ^ value); + } + public void ContributeToHash(ref int hash, T value) where T : unmanaged + { + var intCount = Unsafe.SizeOf() / 4; + ref var intBase = ref Unsafe.As(ref value); + for (int i = 0; i < intCount; ++i) + { + ContributeToHash(ref hash, Unsafe.Add(ref intBase, i)); + } + ref var byteBase = ref Unsafe.As(ref Unsafe.Add(ref intBase, intCount)); + var byteRemainder = Unsafe.SizeOf() - intCount * 4; + for (int i = 0; i < byteRemainder; ++i) + { + ContributeToHash(ref hash, Unsafe.Add(ref byteBase, i)); + } + } + + public void ContributeToHash(HashDiagnosticType hashType, int value) + { + ContributeToHash(ref GetHashForType(hashType), value); + } + public void ContributeToHash(HashDiagnosticType hashType, T value) where T : unmanaged + { + ContributeToHash(ref GetHashForType(hashType), value); + } + } +} diff --git a/BepuPhysics/IslandAwakener.cs b/BepuPhysics/IslandAwakener.cs index b0aab79a6..24e7cf907 100644 --- a/BepuPhysics/IslandAwakener.cs +++ b/BepuPhysics/IslandAwakener.cs @@ -421,6 +421,10 @@ internal void AccumulateUniqueIndices(ref QuickList candidateSetIndices, re uniqueSetIndices.AllocateUnsafely() = candidateSetIndex; } } + //The number of unique set indices being awakened is typically very small (<4), so sorting even when the simulation is running nondeterministically really isn't a concern. + //Determinism requires source sets are awakened in order when the fallback batch may be involved, since constraint order and typebatch order within the sequential fallback matter. + var comparer = new PrimitiveComparer(); + QuickSort.Sort(ref uniqueSetIndices[0], 0, uniqueSetIndices.Count - 1, ref comparer); } [Conditional("DEBUG")] void ValidateSleepingSetIndex(int setIndex) diff --git a/BepuPhysics/IslandSleeper.cs b/BepuPhysics/IslandSleeper.cs index dc08a7d53..fcd687174 100644 --- a/BepuPhysics/IslandSleeper.cs +++ b/BepuPhysics/IslandSleeper.cs @@ -842,6 +842,7 @@ void DisposeWorkerTraversalResults() if (threadCount > 1) { threadDispatcher.DispatchWorkers(typeBatchConstraintRemovalDelegate); + //typeBatchConstraintRemovalDelegate(0); } else { diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index 075c1e1a7..51da94847 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -366,7 +366,7 @@ internal unsafe void ValidateFallbackBatchAccessSafety() } } } - Console.WriteLine($"Average fallback occupancy: {Vector.Count * occupiedLaneCountAcrossBatch / (double)(totalBundleCount * Vector.Count):G3} / {Vector.Count}, total bundle count: {totalBundleCount}"); + //Console.WriteLine($"Average fallback occupancy: {Vector.Count * occupiedLaneCountAcrossBatch / (double)(totalBundleCount * Vector.Count):G3} / {Vector.Count}, total bundle count: {totalBundleCount}"); } } [Conditional("DEBUG")] @@ -411,6 +411,48 @@ public void LoopBody(float impulse) } } + + internal unsafe void ValidateFallbackBodyReferencesByHash(HashDiagnosticType hashDiagnosticType) + { + var hashes = InvasiveHashDiagnostics.Instance; + ref var hash = ref hashes.GetHashForType(hashDiagnosticType); + if (ActiveSet.Batches.Count > FallbackBatchThreshold) + { + ref var batch = ref ActiveSet.Batches[FallbackBatchThreshold]; + for (int i = 0; i < batch.TypeBatches.Count; ++i) + { + ref var typeBatch = ref batch.TypeBatches[i]; + hashes.ContributeToHash(ref hash, typeBatch.TypeId); + hashes.ContributeToHash(ref hash, typeBatch.ConstraintCount); + var bodiesPerConstraint = TypeProcessors[typeBatch.TypeId].BodiesPerConstraint; + var bytesPerBodyReferencesBundle = bodiesPerConstraint * Unsafe.SizeOf>(); + for (int bundleIndex = 0; bundleIndex < typeBatch.BundleCount; ++bundleIndex) + { + var countInBundle = typeBatch.ConstraintCount - bundleIndex * Vector.Count; + if (countInBundle > Vector.Count) + countInBundle = Vector.Count; + ref var bundleStart = ref Unsafe.As>(ref typeBatch.BodyReferences[bytesPerBodyReferencesBundle * bundleIndex]); + for (int bodyIndexInConstraint = 0; bodyIndexInConstraint < bodiesPerConstraint; ++bodyIndexInConstraint) + { + var bodyVector = Unsafe.Add(ref bundleStart, bodyIndexInConstraint); + for (int innerIndex = 0; innerIndex < countInBundle; ++innerIndex) + { + var bodyIndex = bodyVector[innerIndex]; + if (bodyIndex >= 0) + hashes.ContributeToHash(ref hash, bodies.ActiveSet.IndexToHandle[bodyIndex]); + else + hashes.ContributeToHash(ref hash, bodyIndex); + } + } + } + for (int constraintIndex = 0; constraintIndex < typeBatch.ConstraintCount; ++constraintIndex) + { + hashes.ContributeToHash(ref hash, typeBatch.IndexToHandle[constraintIndex].Value); + } + } + } + } + [Conditional("DEBUG")] internal unsafe void ValidateFallbackBatchAccumulatedImpulses() { From 6df17948a924485398abd4bfc2041475673b12e8 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 24 Oct 2021 13:15:39 -0500 Subject: [PATCH 227/947] Fixed an oops in the new awakener sort. Updated CountConstraints for sequential fallback noncontiguity. --- BepuPhysics/IslandAwakener.cs | 11 +++++++---- BepuPhysics/Solver.cs | 22 +++++++++++++++++++++- Demos/Demos/FountainStressTestDemo.cs | 3 ++- Demos/Program.cs | 4 +++- Demos/SpecializedTests/DeterminismTest.cs | 5 +++++ 5 files changed, 38 insertions(+), 7 deletions(-) diff --git a/BepuPhysics/IslandAwakener.cs b/BepuPhysics/IslandAwakener.cs index 24e7cf907..0c890c9c4 100644 --- a/BepuPhysics/IslandAwakener.cs +++ b/BepuPhysics/IslandAwakener.cs @@ -421,10 +421,13 @@ internal void AccumulateUniqueIndices(ref QuickList candidateSetIndices, re uniqueSetIndices.AllocateUnsafely() = candidateSetIndex; } } - //The number of unique set indices being awakened is typically very small (<4), so sorting even when the simulation is running nondeterministically really isn't a concern. - //Determinism requires source sets are awakened in order when the fallback batch may be involved, since constraint order and typebatch order within the sequential fallback matter. - var comparer = new PrimitiveComparer(); - QuickSort.Sort(ref uniqueSetIndices[0], 0, uniqueSetIndices.Count - 1, ref comparer); + if (uniqueSetIndices.Count > 0) + { + //The number of unique set indices being awakened is typically very small (<4), so sorting even when the simulation is running nondeterministically really isn't a concern. + //Determinism requires source sets are awakened in order when the fallback batch may be involved, since constraint order and typebatch order within the sequential fallback matter. + var comparer = new PrimitiveComparer(); + QuickSort.Sort(ref uniqueSetIndices[0], 0, uniqueSetIndices.Count - 1, ref comparer); + } } [Conditional("DEBUG")] void ValidateSleepingSetIndex(int setIndex) diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index 51da94847..a15e86526 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -165,7 +165,9 @@ public int CountConstraints() ref var set = ref Sets[setIndex]; if (set.Allocated) { - for (int batchIndex = 0; batchIndex < set.Batches.Count; ++batchIndex) + var setIsActiveAndFallbackExists = setIndex == 0 && set.Batches.Count > FallbackBatchThreshold; + var contiguousBatchCount = setIsActiveAndFallbackExists ? FallbackBatchThreshold : set.Batches.Count; + for (int batchIndex = 0; batchIndex < contiguousBatchCount; ++batchIndex) { ref var batch = ref set.Batches[batchIndex]; for (int typeBatchIndex = 0; typeBatchIndex < batch.TypeBatches.Count; ++typeBatchIndex) @@ -173,6 +175,24 @@ public int CountConstraints() count += batch.TypeBatches[typeBatchIndex].ConstraintCount; } } + if (setIsActiveAndFallbackExists) + { + //Sequential fallback batches in the active set currently store constraints noncontiguously to guarantee that each bundle does not share any body references. + //Counting constraints requires skipping over any empty slots. + ref var batch = ref set.Batches[FallbackBatchThreshold]; + for (int typeBatchIndex = 0; typeBatchIndex < batch.TypeBatches.Count; ++typeBatchIndex) + { + var constraintCount = batch.TypeBatches[typeBatchIndex].ConstraintCount; + var indexToHandle = batch.TypeBatches[typeBatchIndex].IndexToHandle; + for (int i = 0; i < constraintCount; ++i) + { + //Empty slots are marked with a -1 in the index to handles mapping (and in body references). + if (indexToHandle[i].Value >= 0) + ++count; + } + } + } + } } return count; diff --git a/Demos/Demos/FountainStressTestDemo.cs b/Demos/Demos/FountainStressTestDemo.cs index 07ab0da60..a7a57e94c 100644 --- a/Demos/Demos/FountainStressTestDemo.cs +++ b/Demos/Demos/FountainStressTestDemo.cs @@ -23,7 +23,8 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = MathHelper.Pi * 0.1f; //Using minimum sized allocations forces as many resizes as possible. - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), 1, solverFallbackBatchThreshold: 2, initialAllocationSizes: + //Note the low solverFallbackBatchThreshold- we want the fallback batches to get tested thoroughly. + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), 1, solverFallbackBatchThreshold: 4, initialAllocationSizes: new SimulationAllocationSizes { Bodies = 1, diff --git a/Demos/Program.cs b/Demos/Program.cs index ecdd929c9..1550acdd6 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -1,4 +1,5 @@ -using BepuUtilities; +using BepuPhysics; +using BepuUtilities; using DemoContentLoader; using Demos.Demos; using Demos.SpecializedTests; @@ -23,6 +24,7 @@ static void Main(string[] args) //HeadlessTest.Test(content, 4, 32, 512); //HeadlessTest.Test(content, 4, 32, 512); //HeadlessTest.Test(content, 4, 128, 512); + //DeterminismTest.Test(content, 1, 16384); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); diff --git a/Demos/SpecializedTests/DeterminismTest.cs b/Demos/SpecializedTests/DeterminismTest.cs index e9bf2a7a2..5f1bcc684 100644 --- a/Demos/SpecializedTests/DeterminismTest.cs +++ b/Demos/SpecializedTests/DeterminismTest.cs @@ -20,6 +20,7 @@ static Dictionary ExecuteSimulation(ContentArchive content, in for (int i = 0; i < frameCount; ++i) { demo.Update(null, null, null, 1 / 60f); + //InvasiveHashDiagnostics.Instance.MoveToNextHashFrame(); if ((i + 1) % 32 == 0) Console.Write($"{i + 1}, "); } @@ -42,11 +43,15 @@ static Dictionary ExecuteSimulation(ContentArchive content, in public static void Test(ContentArchive archive, int runCount, int frameCount) { + //InvasiveHashDiagnostics.Initialize(1 + runCount, frameCount); + //var hashInstance = InvasiveHashDiagnostics.Instance; var initialStates = ExecuteSimulation(archive, frameCount); + //hashInstance.MoveToNextRun(); Console.WriteLine($"Completed initial run."); for (int i = 0; i < runCount; ++i) { var states = ExecuteSimulation(archive, frameCount); + //hashInstance.MoveToNextRun(); Console.Write($"Completed iteration {i}; checking... "); if (states.Count != initialStates.Count) Console.WriteLine("DETERMINISM FAILURE: Differing body count."); From f8d338470c1cb1a3c510658186e0e7a0b6b99cf3 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 25 Oct 2021 17:29:45 -0500 Subject: [PATCH 228/947] Some gather cleanup and preparing for index encoded kinematics. --- BepuPhysics/Bodies_GatherScatter.cs | 317 +++++++----------- .../Constraints/FourBodyTypeProcessor.cs | 8 +- .../Constraints/OneBodyTypeProcessor.cs | 4 +- .../Constraints/ThreeBodyTypeProcessor.cs | 6 +- .../Constraints/TwoBodyTypeProcessor.cs | 8 +- BepuPhysics/Constraints/TypeProcessor.cs | 24 +- BepuPhysics/PoseIntegrator.cs | 6 +- BepuUtilities/Memory/Buffer.cs | 27 ++ Demos/Demos/NewtDemo.cs | 6 +- Demos/Program.cs | 2 +- 10 files changed, 175 insertions(+), 233 deletions(-) diff --git a/BepuPhysics/Bodies_GatherScatter.cs b/BepuPhysics/Bodies_GatherScatter.cs index 0bda3d7fb..9a6184225 100644 --- a/BepuPhysics/Bodies_GatherScatter.cs +++ b/BepuPhysics/Bodies_GatherScatter.cs @@ -74,8 +74,8 @@ public void GatherState(ref Vector references, } } - //[MethodImpl(MethodImplOptions.AggressiveInlining)] - unsafe static void FallbackGatherMotionState(SolverState* states, ref Vector baseIndex, ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocityWide velocity) + [MethodImpl(MethodImplOptions.AggressiveInlining)] + unsafe static void FallbackGatherMotionState(SolverState* states, Vector baseIndex, ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocityWide velocity) { var indices = (int*)Unsafe.AsPointer(ref baseIndex); var pPositionX = (float*)Unsafe.AsPointer(ref position.X); @@ -113,7 +113,8 @@ unsafe static void FallbackGatherMotionState(SolverState* states, ref Vector baseIndex, ref BodyInertiaWide inertia, int offsetInFloats) + [MethodImpl(MethodImplOptions.AggressiveInlining)] + unsafe static void FallbackGatherInertia(SolverState* states, Vector baseIndex, ref BodyInertiaWide inertia, int offsetInFloats) { var indices = (int*)Unsafe.AsPointer(ref baseIndex); var pMass = (float*)Unsafe.AsPointer(ref inertia.InverseMass); @@ -140,24 +141,47 @@ unsafe static void FallbackGatherInertia(SolverState* states, ref Vector ba } } + const int DoesntExistFlagIndex = 31; + const int KinematicFlagIndex = 30; + /// + /// Constraint body index references greater than a given unsigned value are either kinematic (1<<30 set) or correspond to an empty lane (1<<31 set). + /// + const uint DynamicLimit = 1 << KinematicFlagIndex; + const uint BodyIndexMetadataMask = (1u << DoesntExistFlagIndex) | (1u << KinematicFlagIndex); + const int BodyIndexMask = (int)~BodyIndexMetadataMask; + //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void GatherState(ref Vector bodyIndices, bool worldInertia, out Vector3Wide position, out QuaternionWide orientation, out BodyVelocityWide velocity, out BodyInertiaWide inertia) + public unsafe void GatherState(Vector encodedBodyIndices, bool worldInertia, out Vector3Wide position, out QuaternionWide orientation, out BodyVelocityWide velocity, out BodyInertiaWide inertia) where TAccessFilter : unmanaged, IBodyAccessFilter { var solverStates = ActiveSet.SolverStates.Memory; Unsafe.SkipInit(out TAccessFilter filter); if (Avx.IsSupported && Vector.Count == 8) { - var indices = (int*)Unsafe.AsPointer(ref bodyIndices); - - var s0 = (float*)(solverStates + indices[0]); - var s1 = (float*)(solverStates + indices[1]); - var s2 = (float*)(solverStates + indices[2]); - var s3 = (float*)(solverStates + indices[3]); - var s4 = (float*)(solverStates + indices[4]); - var s5 = (float*)(solverStates + indices[5]); - var s6 = (float*)(solverStates + indices[6]); - var s7 = (float*)(solverStates + indices[7]); + var bodyIndices0 = encodedBodyIndices[0]; + var empty0 = bodyIndices0 < 0; + var s0 = (float*)(solverStates + (bodyIndices0 & BodyIndexMask)); + var bodyIndices1 = encodedBodyIndices[1]; + var empty1 = bodyIndices1 < 0; + var s1 = (float*)(solverStates + (bodyIndices1 & BodyIndexMask)); + var bodyIndices2 = encodedBodyIndices[2]; + var empty2 = bodyIndices2 < 0; + var s2 = (float*)(solverStates + (bodyIndices2 & BodyIndexMask)); + var bodyIndices3 = encodedBodyIndices[3]; + var empty3 = bodyIndices3 < 0; + var s3 = (float*)(solverStates + (bodyIndices3 & BodyIndexMask)); + var bodyIndices4 = encodedBodyIndices[4]; + var empty4 = bodyIndices4 < 0; + var s4 = (float*)(solverStates + (bodyIndices4 & BodyIndexMask)); + var bodyIndices5 = encodedBodyIndices[5]; + var empty5 = bodyIndices5 < 0; + var s5 = (float*)(solverStates + (bodyIndices5 & BodyIndexMask)); + var bodyIndices6 = encodedBodyIndices[6]; + var empty6 = bodyIndices6 < 0; + var s6 = (float*)(solverStates + (bodyIndices6 & BodyIndexMask)); + var bodyIndices7 = encodedBodyIndices[7]; + var empty7 = bodyIndices7 < 0; + var s7 = (float*)(solverStates + (bodyIndices7 & BodyIndexMask)); //for (int i = 0; i < 8; ++i) //{ @@ -174,14 +198,14 @@ public unsafe void GatherState(ref Vector bodyIndices, bool { //Load every body for the first half of the motion state. //Note that buffers are allocated on cache line boundaries, so we can use aligned loads for all that matters. - var m0 = indices[0] >= 0 ? Avx.LoadAlignedVector256(s0) : Vector256.Zero; - var m1 = indices[1] >= 0 ? Avx.LoadAlignedVector256(s1) : Vector256.Zero; - var m2 = indices[2] >= 0 ? Avx.LoadAlignedVector256(s2) : Vector256.Zero; - var m3 = indices[3] >= 0 ? Avx.LoadAlignedVector256(s3) : Vector256.Zero; - var m4 = indices[4] >= 0 ? Avx.LoadAlignedVector256(s4) : Vector256.Zero; - var m5 = indices[5] >= 0 ? Avx.LoadAlignedVector256(s5) : Vector256.Zero; - var m6 = indices[6] >= 0 ? Avx.LoadAlignedVector256(s6) : Vector256.Zero; - var m7 = indices[7] >= 0 ? Avx.LoadAlignedVector256(s7) : Vector256.Zero; + var m0 = empty0 ? Vector256.Zero : Avx.LoadAlignedVector256(s0); + var m1 = empty1 ? Vector256.Zero : Avx.LoadAlignedVector256(s1); + var m2 = empty2 ? Vector256.Zero : Avx.LoadAlignedVector256(s2); + var m3 = empty3 ? Vector256.Zero : Avx.LoadAlignedVector256(s3); + var m4 = empty4 ? Vector256.Zero : Avx.LoadAlignedVector256(s4); + var m5 = empty5 ? Vector256.Zero : Avx.LoadAlignedVector256(s5); + var m6 = empty6 ? Vector256.Zero : Avx.LoadAlignedVector256(s6); + var m7 = empty7 ? Vector256.Zero : Avx.LoadAlignedVector256(s7); var n0 = Avx.UnpackLow(m0, m1); var n1 = Avx.UnpackLow(m2, m3); @@ -226,14 +250,14 @@ public unsafe void GatherState(ref Vector bodyIndices, bool { //Second half. - var m0 = indices[0] >= 0 ? Avx.LoadAlignedVector256(s0 + 8) : Vector256.Zero; - var m1 = indices[1] >= 0 ? Avx.LoadAlignedVector256(s1 + 8) : Vector256.Zero; - var m2 = indices[2] >= 0 ? Avx.LoadAlignedVector256(s2 + 8) : Vector256.Zero; - var m3 = indices[3] >= 0 ? Avx.LoadAlignedVector256(s3 + 8) : Vector256.Zero; - var m4 = indices[4] >= 0 ? Avx.LoadAlignedVector256(s4 + 8) : Vector256.Zero; - var m5 = indices[5] >= 0 ? Avx.LoadAlignedVector256(s5 + 8) : Vector256.Zero; - var m6 = indices[6] >= 0 ? Avx.LoadAlignedVector256(s6 + 8) : Vector256.Zero; - var m7 = indices[7] >= 0 ? Avx.LoadAlignedVector256(s7 + 8) : Vector256.Zero; + var m0 = empty0 ? Vector256.Zero : Avx.LoadAlignedVector256(s0 + 8); + var m1 = empty1 ? Vector256.Zero : Avx.LoadAlignedVector256(s1 + 8); + var m2 = empty2 ? Vector256.Zero : Avx.LoadAlignedVector256(s2 + 8); + var m3 = empty3 ? Vector256.Zero : Avx.LoadAlignedVector256(s3 + 8); + var m4 = empty4 ? Vector256.Zero : Avx.LoadAlignedVector256(s4 + 8); + var m5 = empty5 ? Vector256.Zero : Avx.LoadAlignedVector256(s5 + 8); + var m6 = empty6 ? Vector256.Zero : Avx.LoadAlignedVector256(s6 + 8); + var m7 = empty7 ? Vector256.Zero : Avx.LoadAlignedVector256(s7 + 8); var n0 = Avx.UnpackLow(m0, m1); var n1 = Avx.UnpackLow(m2, m3); @@ -250,8 +274,6 @@ public unsafe void GatherState(ref Vector bodyIndices, bool var o3 = Avx.Shuffle(n6, n7, 0 | (1 << 2) | (0 << 4) | (1 << 6)); var o4 = Avx.Shuffle(n0, n1, 2 | (3 << 2) | (2 << 4) | (3 << 6)); var o5 = Avx.Shuffle(n2, n3, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - var o6 = Avx.Shuffle(n4, n5, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - var o7 = Avx.Shuffle(n6, n7, 2 | (3 << 2) | (2 << 4) | (3 << 6)); if (filter.AccessLinearVelocity) { @@ -279,14 +301,15 @@ public unsafe void GatherState(ref Vector bodyIndices, bool var offsetInFloats = worldInertia ? 24 : 16; //Load every inertia vector. - var m0 = indices[0] >= 0 ? Avx.LoadAlignedVector256(s0 + offsetInFloats) : Vector256.Zero; - var m1 = indices[1] >= 0 ? Avx.LoadAlignedVector256(s1 + offsetInFloats) : Vector256.Zero; - var m2 = indices[2] >= 0 ? Avx.LoadAlignedVector256(s2 + offsetInFloats) : Vector256.Zero; - var m3 = indices[3] >= 0 ? Avx.LoadAlignedVector256(s3 + offsetInFloats) : Vector256.Zero; - var m4 = indices[4] >= 0 ? Avx.LoadAlignedVector256(s4 + offsetInFloats) : Vector256.Zero; - var m5 = indices[5] >= 0 ? Avx.LoadAlignedVector256(s5 + offsetInFloats) : Vector256.Zero; - var m6 = indices[6] >= 0 ? Avx.LoadAlignedVector256(s6 + offsetInFloats) : Vector256.Zero; - var m7 = indices[7] >= 0 ? Avx.LoadAlignedVector256(s7 + offsetInFloats) : Vector256.Zero; + //Assuming here that most bodies are dynamic, so it's not worth the extra work extracting a dynamic-only mask to avoid loading some kinematic zeroes. + var m0 = empty0 ? Vector256.Zero : Avx.LoadAlignedVector256(s0 + offsetInFloats); + var m1 = empty1 ? Vector256.Zero : Avx.LoadAlignedVector256(s1 + offsetInFloats); + var m2 = empty2 ? Vector256.Zero : Avx.LoadAlignedVector256(s2 + offsetInFloats); + var m3 = empty3 ? Vector256.Zero : Avx.LoadAlignedVector256(s3 + offsetInFloats); + var m4 = empty4 ? Vector256.Zero : Avx.LoadAlignedVector256(s4 + offsetInFloats); + var m5 = empty5 ? Vector256.Zero : Avx.LoadAlignedVector256(s5 + offsetInFloats); + var m6 = empty6 ? Vector256.Zero : Avx.LoadAlignedVector256(s6 + offsetInFloats); + var m7 = empty7 ? Vector256.Zero : Avx.LoadAlignedVector256(s7 + offsetInFloats); var n0 = Avx.UnpackLow(m0, m1); var n1 = Avx.UnpackLow(m2, m3); @@ -336,8 +359,8 @@ public unsafe void GatherState(ref Vector bodyIndices, bool Unsafe.SkipInit(out orientation); Unsafe.SkipInit(out velocity); Unsafe.SkipInit(out inertia); - FallbackGatherMotionState(solverStates, ref bodyIndices, ref position, ref orientation, ref velocity); - FallbackGatherInertia(solverStates, ref bodyIndices, ref inertia, worldInertia ? 24 : 16); + FallbackGatherMotionState(solverStates, encodedBodyIndices, ref position, ref orientation, ref velocity); + FallbackGatherInertia(solverStates, encodedBodyIndices, ref inertia, worldInertia ? 24 : 16); } } @@ -449,8 +472,8 @@ public unsafe void GatherState(ref TwoBodyReferences references, { ref var states = ref ActiveSet.SolverStates; - GatherState(ref references.IndexA, true, out var positionA, out orientationA, out velocityA, out inertiaA); - GatherState(ref references.IndexB, true, out var positionB, out orientationB, out velocityB, out inertiaB); + GatherState(references.IndexA, true, out var positionA, out orientationA, out velocityA, out inertiaA); + GatherState(references.IndexB, true, out var positionB, out orientationB, out velocityB, out inertiaB); ////Grab the base references for the body indices. Note that we make use of the references memory layout again. //ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); @@ -502,9 +525,9 @@ public void GatherState(ref ThreeBodyReferences references, out QuaternionWide orientationC, out BodyVelocityWide velocityC, out BodyInertiaWide inertiaC) { - GatherState(ref references.IndexA, true, out var positionA, out orientationA, out velocityA, out inertiaA); - GatherState(ref references.IndexB, true, out var positionB, out orientationB, out velocityB, out inertiaB); - GatherState(ref references.IndexC, true, out var positionC, out orientationC, out velocityC, out inertiaC); + GatherState(references.IndexA, true, out var positionA, out orientationA, out velocityA, out inertiaA); + GatherState(references.IndexB, true, out var positionB, out orientationB, out velocityB, out inertiaB); + GatherState(references.IndexC, true, out var positionC, out orientationC, out velocityC, out inertiaC); //TODO: In future versions, we will likely store the body position in different forms to allow for extremely large worlds. //That will be an opt-in feature. The default implementation will use the FP32 representation, but the user could choose to swap it out for a fp64 or fixed64 representation. @@ -549,10 +572,10 @@ public void GatherState(ref FourBodyReferences references, out QuaternionWide orientationD, out BodyVelocityWide velocityD, out BodyInertiaWide inertiaD) { - GatherState(ref references.IndexA, true, out var positionA, out orientationA, out velocityA, out inertiaA); - GatherState(ref references.IndexB, true, out var positionB, out orientationB, out velocityB, out inertiaB); - GatherState(ref references.IndexC, true, out var positionC, out orientationC, out velocityC, out inertiaC); - GatherState(ref references.IndexD, true, out var positionD, out orientationD, out velocityD, out inertiaD); + GatherState(references.IndexA, true, out var positionA, out orientationA, out velocityA, out inertiaA); + GatherState(references.IndexB, true, out var positionB, out orientationB, out velocityB, out inertiaB); + GatherState(references.IndexC, true, out var positionC, out orientationC, out velocityC, out inertiaC); + GatherState(references.IndexD, true, out var positionD, out orientationD, out velocityD, out inertiaD); //TODO: In future versions, we will likely store the body position in different forms to allow for extremely large worlds. //That will be an opt-in feature. The default implementation will use the FP32 representation, but the user could choose to swap it out for a fp64 or fixed64 representation. //This affects other systems- AABB calculation, pose integration, solving, and in extreme (64 bit) cases, the broadphase. @@ -563,13 +586,15 @@ public void GatherState(ref FourBodyReferences references, //Given the current limits of C# and the compiler, the best option seems to be conditional compilation. Vector3Wide.Subtract(positionB, positionA, out ab); Vector3Wide.Subtract(positionC, positionA, out ac); - Vector3Wide.Subtract(positionC, positionA, out ad); + Vector3Wide.Subtract(positionD, positionA, out ad); } + //Note that ScatterPose and ScatterInertia do not need to check body references for empty lanes or for kinematicity. + //The mask is only set for lanes which were subject to constraint integration responsibility; empty lanes and kinematics cannot be integrated by constraints. //[MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe void ScatterPose( - ref Vector3Wide position, ref QuaternionWide orientation, ref Vector references, ref Vector mask) + ref Vector3Wide position, ref QuaternionWide orientation, Vector encodedBodyIndices, Vector mask) { if (Avx.IsSupported && Vector.Count == 8) { @@ -601,16 +626,14 @@ public unsafe void ScatterPose( var o6 = Avx.Shuffle(n4, n5, 2 | (3 << 2) | (2 << 4) | (3 << 6)); var o7 = Avx.Shuffle(n6, n7, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - var maskPointer = (int*)Unsafe.AsPointer(ref mask); - var indices = (int*)Unsafe.AsPointer(ref references); - if (maskPointer[0] != 0) Avx.StoreAligned((float*)(states + indices[0]), Avx.Permute2x128(o0, o1, 0 | (2 << 4))); - if (maskPointer[1] != 0) Avx.StoreAligned((float*)(states + indices[1]), Avx.Permute2x128(o4, o5, 0 | (2 << 4))); - if (maskPointer[2] != 0) Avx.StoreAligned((float*)(states + indices[2]), Avx.Permute2x128(o2, o3, 0 | (2 << 4))); - if (maskPointer[3] != 0) Avx.StoreAligned((float*)(states + indices[3]), Avx.Permute2x128(o6, o7, 0 | (2 << 4))); - if (maskPointer[4] != 0) Avx.StoreAligned((float*)(states + indices[4]), Avx.Permute2x128(o0, o1, 1 | (3 << 4))); - if (maskPointer[5] != 0) Avx.StoreAligned((float*)(states + indices[5]), Avx.Permute2x128(o4, o5, 1 | (3 << 4))); - if (maskPointer[6] != 0) Avx.StoreAligned((float*)(states + indices[6]), Avx.Permute2x128(o2, o3, 1 | (3 << 4))); - if (maskPointer[7] != 0) Avx.StoreAligned((float*)(states + indices[7]), Avx.Permute2x128(o6, o7, 1 | (3 << 4))); + if (mask[0] != 0) Avx.StoreAligned((float*)(states + encodedBodyIndices[0]), Avx.Permute2x128(o0, o1, 0 | (2 << 4))); + if (mask[1] != 0) Avx.StoreAligned((float*)(states + encodedBodyIndices[1]), Avx.Permute2x128(o4, o5, 0 | (2 << 4))); + if (mask[2] != 0) Avx.StoreAligned((float*)(states + encodedBodyIndices[2]), Avx.Permute2x128(o2, o3, 0 | (2 << 4))); + if (mask[3] != 0) Avx.StoreAligned((float*)(states + encodedBodyIndices[3]), Avx.Permute2x128(o6, o7, 0 | (2 << 4))); + if (mask[4] != 0) Avx.StoreAligned((float*)(states + encodedBodyIndices[4]), Avx.Permute2x128(o0, o1, 1 | (3 << 4))); + if (mask[5] != 0) Avx.StoreAligned((float*)(states + encodedBodyIndices[5]), Avx.Permute2x128(o4, o5, 1 | (3 << 4))); + if (mask[6] != 0) Avx.StoreAligned((float*)(states + encodedBodyIndices[6]), Avx.Permute2x128(o2, o3, 1 | (3 << 4))); + if (mask[7] != 0) Avx.StoreAligned((float*)(states + encodedBodyIndices[7]), Avx.Permute2x128(o6, o7, 1 | (3 << 4))); //if (maskPointer[0] != 0) { states[indices[0]].Motion.Pose.Position.Validate(); states[indices[0]].Motion.Pose.Orientation.Validate(); } //if (maskPointer[1] != 0) { states[indices[1]].Motion.Pose.Position.Validate(); states[indices[1]].Motion.Pose.Orientation.Validate(); } @@ -631,7 +654,7 @@ public unsafe void ScatterPose( //[MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe void ScatterInertia( - ref BodyInertiaWide inertia, ref Vector references, ref Vector mask) + ref BodyInertiaWide inertia, Vector encodedBodyIndices, Vector mask) { if (Avx.IsSupported && Vector.Count == 8) { @@ -663,17 +686,15 @@ public unsafe void ScatterInertia( var o6 = Avx.Shuffle(n4, n5, 2 | (3 << 2) | (2 << 4) | (3 << 6)); var o7 = Avx.Shuffle(n6, n7, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - var maskPointer = (int*)Unsafe.AsPointer(ref mask); - var indices = (int*)Unsafe.AsPointer(ref references); //Note the offset; we're scattering into the world inertias. - if (maskPointer[0] != 0) Avx.StoreAligned((float*)(states + indices[0]) + 24, Avx.Permute2x128(o0, o1, 0 | (2 << 4))); - if (maskPointer[1] != 0) Avx.StoreAligned((float*)(states + indices[1]) + 24, Avx.Permute2x128(o4, o5, 0 | (2 << 4))); - if (maskPointer[2] != 0) Avx.StoreAligned((float*)(states + indices[2]) + 24, Avx.Permute2x128(o2, o3, 0 | (2 << 4))); - if (maskPointer[3] != 0) Avx.StoreAligned((float*)(states + indices[3]) + 24, Avx.Permute2x128(o6, o7, 0 | (2 << 4))); - if (maskPointer[4] != 0) Avx.StoreAligned((float*)(states + indices[4]) + 24, Avx.Permute2x128(o0, o1, 1 | (3 << 4))); - if (maskPointer[5] != 0) Avx.StoreAligned((float*)(states + indices[5]) + 24, Avx.Permute2x128(o4, o5, 1 | (3 << 4))); - if (maskPointer[6] != 0) Avx.StoreAligned((float*)(states + indices[6]) + 24, Avx.Permute2x128(o2, o3, 1 | (3 << 4))); - if (maskPointer[7] != 0) Avx.StoreAligned((float*)(states + indices[7]) + 24, Avx.Permute2x128(o6, o7, 1 | (3 << 4))); + if (mask[0] != 0) Avx.StoreAligned((float*)(states + encodedBodyIndices[0]) + 24, Avx.Permute2x128(o0, o1, 0 | (2 << 4))); + if (mask[1] != 0) Avx.StoreAligned((float*)(states + encodedBodyIndices[1]) + 24, Avx.Permute2x128(o4, o5, 0 | (2 << 4))); + if (mask[2] != 0) Avx.StoreAligned((float*)(states + encodedBodyIndices[2]) + 24, Avx.Permute2x128(o2, o3, 0 | (2 << 4))); + if (mask[3] != 0) Avx.StoreAligned((float*)(states + encodedBodyIndices[3]) + 24, Avx.Permute2x128(o6, o7, 0 | (2 << 4))); + if (mask[4] != 0) Avx.StoreAligned((float*)(states + encodedBodyIndices[4]) + 24, Avx.Permute2x128(o0, o1, 1 | (3 << 4))); + if (mask[5] != 0) Avx.StoreAligned((float*)(states + encodedBodyIndices[5]) + 24, Avx.Permute2x128(o4, o5, 1 | (3 << 4))); + if (mask[6] != 0) Avx.StoreAligned((float*)(states + encodedBodyIndices[6]) + 24, Avx.Permute2x128(o2, o3, 1 | (3 << 4))); + if (mask[7] != 0) Avx.StoreAligned((float*)(states + encodedBodyIndices[7]) + 24, Avx.Permute2x128(o6, o7, 1 | (3 << 4))); //if (maskPointer[0] != 0) { states[indices[0]].Inertia.Local.InverseInertiaTensor.Validate(); states[indices[0]].Inertia.Local.InverseMass.Validate(); } //if (maskPointer[1] != 0) { states[indices[1]].Inertia.Local.InverseInertiaTensor.Validate(); states[indices[1]].Inertia.Local.InverseMass.Validate(); } @@ -692,7 +713,7 @@ public unsafe void ScatterInertia( //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void ScatterVelocities(ref BodyVelocityWide sourceVelocities, ref Vector references) where TAccessFilter : unmanaged, IBodyAccessFilter + public unsafe void ScatterVelocities(ref BodyVelocityWide sourceVelocities, ref Vector encodedBodyIndices) where TAccessFilter : unmanaged, IBodyAccessFilter { if (Avx.IsSupported && Vector.Count == 8) { @@ -739,16 +760,16 @@ public unsafe void ScatterVelocities(ref BodyVelocityWide sourceV var o4 = Avx.Shuffle(n0, n1, 2 | (3 << 2) | (2 << 4) | (3 << 6)); var o6 = Avx.Shuffle(n4, n5, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - var indices = (int*)Unsafe.AsPointer(ref references); + var indices = (uint*)Unsafe.AsPointer(ref encodedBodyIndices); var states = ActiveSet.SolverStates.Memory; - if (indices[0] >= 0) Sse.StoreAligned((float*)(states + indices[0]) + targetOffset, o0.GetLower()); - if (indices[1] >= 0) Sse.StoreAligned((float*)(states + indices[1]) + targetOffset, o4.GetLower()); - if (indices[2] >= 0) Sse.StoreAligned((float*)(states + indices[2]) + targetOffset, o2.GetLower()); - if (indices[3] >= 0) Sse.StoreAligned((float*)(states + indices[3]) + targetOffset, o6.GetLower()); - if (indices[4] >= 0) Sse.StoreAligned((float*)(states + indices[4]) + targetOffset, o0.GetUpper()); - if (indices[5] >= 0) Sse.StoreAligned((float*)(states + indices[5]) + targetOffset, o4.GetUpper()); - if (indices[6] >= 0) Sse.StoreAligned((float*)(states + indices[6]) + targetOffset, o2.GetUpper()); - if (indices[7] >= 0) Sse.StoreAligned((float*)(states + indices[7]) + targetOffset, o6.GetUpper()); + if (indices[0] < DynamicLimit) Sse.StoreAligned((float*)(states + indices[0]) + targetOffset, o0.GetLower()); + if (indices[1] < DynamicLimit) Sse.StoreAligned((float*)(states + indices[1]) + targetOffset, o4.GetLower()); + if (indices[2] < DynamicLimit) Sse.StoreAligned((float*)(states + indices[2]) + targetOffset, o2.GetLower()); + if (indices[3] < DynamicLimit) Sse.StoreAligned((float*)(states + indices[3]) + targetOffset, o6.GetLower()); + if (indices[4] < DynamicLimit) Sse.StoreAligned((float*)(states + indices[4]) + targetOffset, o0.GetUpper()); + if (indices[5] < DynamicLimit) Sse.StoreAligned((float*)(states + indices[5]) + targetOffset, o4.GetUpper()); + if (indices[6] < DynamicLimit) Sse.StoreAligned((float*)(states + indices[6]) + targetOffset, o2.GetUpper()); + if (indices[7] < DynamicLimit) Sse.StoreAligned((float*)(states + indices[7]) + targetOffset, o6.GetUpper()); } else @@ -781,16 +802,16 @@ public unsafe void ScatterVelocities(ref BodyVelocityWide sourceV var o6 = Avx.Shuffle(n4, n5, 2 | (3 << 2) | (2 << 4) | (3 << 6)); var o7 = Avx.Shuffle(n6, n7, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - var indices = (int*)Unsafe.AsPointer(ref references); + var indices = (uint*)Unsafe.AsPointer(ref encodedBodyIndices); var states = ActiveSet.SolverStates.Memory; - if (indices[0] >= 0) Avx.StoreAligned((float*)(states + indices[0]) + 8, Avx.Permute2x128(o0, o1, 0 | (2 << 4))); - if (indices[1] >= 0) Avx.StoreAligned((float*)(states + indices[1]) + 8, Avx.Permute2x128(o4, o5, 0 | (2 << 4))); - if (indices[2] >= 0) Avx.StoreAligned((float*)(states + indices[2]) + 8, Avx.Permute2x128(o2, o3, 0 | (2 << 4))); - if (indices[3] >= 0) Avx.StoreAligned((float*)(states + indices[3]) + 8, Avx.Permute2x128(o6, o7, 0 | (2 << 4))); - if (indices[4] >= 0) Avx.StoreAligned((float*)(states + indices[4]) + 8, Avx.Permute2x128(o0, o1, 1 | (3 << 4))); - if (indices[5] >= 0) Avx.StoreAligned((float*)(states + indices[5]) + 8, Avx.Permute2x128(o4, o5, 1 | (3 << 4))); - if (indices[6] >= 0) Avx.StoreAligned((float*)(states + indices[6]) + 8, Avx.Permute2x128(o2, o3, 1 | (3 << 4))); - if (indices[7] >= 0) Avx.StoreAligned((float*)(states + indices[7]) + 8, Avx.Permute2x128(o6, o7, 1 | (3 << 4))); + if (indices[0] < DynamicLimit) Avx.StoreAligned((float*)(states + indices[0]) + 8, Avx.Permute2x128(o0, o1, 0 | (2 << 4))); + if (indices[1] < DynamicLimit) Avx.StoreAligned((float*)(states + indices[1]) + 8, Avx.Permute2x128(o4, o5, 0 | (2 << 4))); + if (indices[2] < DynamicLimit) Avx.StoreAligned((float*)(states + indices[2]) + 8, Avx.Permute2x128(o2, o3, 0 | (2 << 4))); + if (indices[3] < DynamicLimit) Avx.StoreAligned((float*)(states + indices[3]) + 8, Avx.Permute2x128(o6, o7, 0 | (2 << 4))); + if (indices[4] < DynamicLimit) Avx.StoreAligned((float*)(states + indices[4]) + 8, Avx.Permute2x128(o0, o1, 1 | (3 << 4))); + if (indices[5] < DynamicLimit) Avx.StoreAligned((float*)(states + indices[5]) + 8, Avx.Permute2x128(o4, o5, 1 | (3 << 4))); + if (indices[6] < DynamicLimit) Avx.StoreAligned((float*)(states + indices[6]) + 8, Avx.Permute2x128(o2, o3, 1 | (3 << 4))); + if (indices[7] < DynamicLimit) Avx.StoreAligned((float*)(states + indices[7]) + 8, Avx.Permute2x128(o6, o7, 1 | (3 << 4))); } //{ @@ -808,10 +829,10 @@ public unsafe void ScatterVelocities(ref BodyVelocityWide sourceV } else { - var indices = (int*)Unsafe.AsPointer(ref references); + var indices = (uint*)Unsafe.AsPointer(ref encodedBodyIndices); for (int innerIndex = 0; innerIndex < Vector.Count; ++innerIndex) { - if (indices[innerIndex] < 0) + if (indices[innerIndex] >= DynamicLimit) continue; ref var sourceSlot = ref GetOffsetInstance(ref sourceVelocities, innerIndex); ref var target = ref ActiveSet.SolverStates[indices[innerIndex]].Motion.Velocity; @@ -820,111 +841,5 @@ public unsafe void ScatterVelocities(ref BodyVelocityWide sourceV } } } - ////[MethodImpl(MethodImplOptions.AggressiveInlining)] - //private unsafe void ScatterVelocities(ref BodyVelocityWide sourceVelocities, ref int baseIndex, int innerIndex) - //{ - // //TODO: How much value would there be in branching on kinematic state and avoiding a write? Depends a lot on the number of kinematics. - // ref var sourceSlot = ref GetOffsetInstance(ref sourceVelocities, innerIndex); - // ref var target = ref ActiveSet.SolverStates[Unsafe.Add(ref baseIndex, innerIndex)].Motion.Velocity; - // target.Linear = new Vector3(sourceSlot.Linear.X[0], sourceSlot.Linear.Y[0], sourceSlot.Linear.Z[0]); - // target.Angular = new Vector3(sourceSlot.Angular.X[0], sourceSlot.Angular.Y[0], sourceSlot.Angular.Z[0]); - //} - - ///// - ///// Scatters velocities for one body bundle into the active body set. - ///// - ///// Velocities of body bundle A to scatter. - ///// Active set indices of the bodies to scatter velocity data to. - ///// Number of body pairs in the bundle. - ////[MethodImpl(MethodImplOptions.AggressiveInlining)] - //public unsafe void ScatterVelocities(ref BodyVelocityWide sourceVelocities, ref Vector references, int count) - //{ - // Debug.Assert(count >= 0 && count <= Vector.Count); - // //Grab the base references for the body indices. Note that we make use of the references memory layout again. - // ref var baseIndex = ref Unsafe.As, int>(ref references); - // for (int i = 0; i < count; ++i) - // { - // ScatterVelocities(ref sourceVelocities, ref baseIndex, i); - // } - //} - - ///// - ///// Scatters velocities for two body bundles into the active body set. - ///// - ///// Velocities of body bundle A to scatter. - ///// Velocities of body bundle B to scatter. - ///// Active set indices of the bodies to scatter velocity data to. - ///// Number of body pairs in the bundle. - ////[MethodImpl(MethodImplOptions.AggressiveInlining)] - //public unsafe void ScatterVelocities(ref BodyVelocityWide sourceVelocitiesA, ref BodyVelocityWide sourceVelocitiesB, - // ref TwoBodyReferences references, int count) - //{ - // Debug.Assert(count >= 0 && count <= Vector.Count); - // ScatterVelocities(ref sourceVelocitiesA, ref references.IndexA, count); - // ScatterVelocities(ref sourceVelocitiesB, ref references.IndexB, count); - // ////Grab the base references for the body indices. Note that we make use of the references memory layout again. - // //ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); - // //ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); - // //for (int i = 0; i < count; ++i) - // //{ - // // ScatterVelocities(ref sourceVelocitiesA, ref baseIndexA, i); - // // ScatterVelocities(ref sourceVelocitiesB, ref baseIndexB, i); - // //} - //} - - ///// - ///// Scatters velocities for three body bundles into the active body set. - ///// - ///// Velocities of body bundle A to scatter. - ///// Velocities of body bundle B to scatter. - ///// Velocities of body bundle C to scatter. - ///// Active set indices of the bodies to scatter velocity data to. - ///// Number of body pairs in the bundle. - ////[MethodImpl(MethodImplOptions.AggressiveInlining)] - //public unsafe void ScatterVelocities( - // ref BodyVelocityWide sourceVelocitiesA, ref BodyVelocityWide sourceVelocitiesB, ref BodyVelocityWide sourceVelocitiesC, - // ref ThreeBodyReferences references, int count) - //{ - // Debug.Assert(count >= 0 && count <= Vector.Count); - // //Grab the base references for the body indices. Note that we make use of the references memory layout again. - // ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); - // ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); - // ref var baseIndexC = ref Unsafe.As, int>(ref references.IndexC); - // for (int i = 0; i < count; ++i) - // { - // ScatterVelocities(ref sourceVelocitiesA, ref baseIndexA, i); - // ScatterVelocities(ref sourceVelocitiesB, ref baseIndexB, i); - // ScatterVelocities(ref sourceVelocitiesC, ref baseIndexC, i); - // } - //} - - ///// - ///// Scatters velocities for four body bundles into the active body set. - ///// - ///// Velocities of body bundle A to scatter. - ///// Velocities of body bundle B to scatter. - ///// Velocities of body bundle C to scatter. - ///// Velocities of body bundle D to scatter. - ///// Active set indices of the bodies to scatter velocity data to. - ///// Number of body pairs in the bundle. - //[MethodImpl(MethodImplOptions.AggressiveInlining)] - //public unsafe void ScatterVelocities( - // ref BodyVelocityWide sourceVelocitiesA, ref BodyVelocityWide sourceVelocitiesB, ref BodyVelocityWide sourceVelocitiesC, ref BodyVelocityWide sourceVelocitiesD, - // ref FourBodyReferences references, int count) - //{ - // Debug.Assert(count >= 0 && count <= Vector.Count); - // //Grab the base references for the body indices. Note that we make use of the references memory layout again. - // ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); - // ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); - // ref var baseIndexC = ref Unsafe.As, int>(ref references.IndexC); - // ref var baseIndexD = ref Unsafe.As, int>(ref references.IndexD); - // for (int i = 0; i < count; ++i) - // { - // ScatterVelocities(ref sourceVelocitiesA, ref baseIndexA, i); - // ScatterVelocities(ref sourceVelocitiesB, ref baseIndexB, i); - // ScatterVelocities(ref sourceVelocitiesC, ref baseIndexC, i); - // ScatterVelocities(ref sourceVelocitiesD, ref baseIndexD, i); - // } - //} } } diff --git a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs index e08fa06ff..cb802bdea 100644 --- a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs @@ -253,10 +253,10 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f ref var prestep = ref prestepBundles[i]; ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; ref var references = ref bodyReferencesBundles[i]; - bodies.GatherState(ref references.IndexA, true, out var positionA, out var orientationA, out var wsvA, out var inertiaA); - bodies.GatherState(ref references.IndexB, true, out var positionB, out var orientationB, out var wsvB, out var inertiaB); - bodies.GatherState(ref references.IndexC, true, out var positionC, out var orientationC, out var wsvC, out var inertiaC); - bodies.GatherState(ref references.IndexD, true, out var positionD, out var orientationD, out var wsvD, out var inertiaD); + bodies.GatherState(references.IndexA, true, out var positionA, out var orientationA, out var wsvA, out var inertiaA); + bodies.GatherState(references.IndexB, true, out var positionB, out var orientationB, out var wsvB, out var inertiaB); + bodies.GatherState(references.IndexC, true, out var positionC, out var orientationC, out var wsvC, out var inertiaC); + bodies.GatherState(references.IndexD, true, out var positionD, out var orientationD, out var wsvD, out var inertiaD); function.Solve2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, positionD, orientationD, inertiaD, dt, inverseDt, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC, ref wsvD); diff --git a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs index e8c9d99f5..21b3f7799 100644 --- a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs @@ -198,7 +198,7 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f ref var prestep = ref prestepBundles[i]; ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; ref var references = ref bodyReferencesBundles[i]; - bodies.GatherState(ref references, true, out var positionA, out var orientationA, out var wsvA, out var inertiaA); + bodies.GatherState(references, true, out var positionA, out var orientationA, out var wsvA, out var inertiaA); function.Solve2(positionA, orientationA, inertiaA, dt, inverseDt, ref prestep, ref accumulatedImpulses, ref wsvA); @@ -222,7 +222,7 @@ public unsafe override void IncrementallyUpdateContactData(ref TypeBatch typeBat { ref var prestep = ref Unsafe.Add(ref prestepBase, i); ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); - bodies.GatherState(ref references, true, out _, out _, out var wsvA, out _); + bodies.GatherState(references, true, out _, out _, out var wsvA, out _); function.IncrementallyUpdateContactData(dtWide, wsvA, ref prestep); } } diff --git a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs index aded4a47c..2a47c9dcb 100644 --- a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs @@ -233,9 +233,9 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f ref var prestep = ref prestepBundles[i]; ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; ref var references = ref bodyReferencesBundles[i]; - bodies.GatherState(ref references.IndexA, true, out var positionA, out var orientationA, out var wsvA, out var inertiaA); - bodies.GatherState(ref references.IndexB, true, out var positionB, out var orientationB, out var wsvB, out var inertiaB); - bodies.GatherState(ref references.IndexC, true, out var positionC, out var orientationC, out var wsvC, out var inertiaC); + bodies.GatherState(references.IndexA, true, out var positionA, out var orientationA, out var wsvA, out var inertiaA); + bodies.GatherState(references.IndexB, true, out var positionB, out var orientationB, out var wsvB, out var inertiaB); + bodies.GatherState(references.IndexC, true, out var positionC, out var orientationC, out var wsvC, out var inertiaC); function.Solve2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, dt, inverseDt, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC); diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index a8bb9b673..bdfe35d8b 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -307,8 +307,8 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f ref var accumulatedImpulses = ref accumulatedImpulsesBundles[i]; ref var references = ref bodyReferencesBundles[i]; //Prefetch(SolvePrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, i, exclusiveEndBundle); - bodies.GatherState(ref references.IndexA, true, out var positionA, out var orientationA, out var wsvA, out var inertiaA); - bodies.GatherState(ref references.IndexB, true, out var positionB, out var orientationB, out var wsvB, out var inertiaB); + bodies.GatherState(references.IndexA, true, out var positionA, out var orientationA, out var wsvA, out var inertiaA); + bodies.GatherState(references.IndexB, true, out var positionB, out var orientationB, out var wsvB, out var inertiaB); function.Solve2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, dt, inverseDt, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB); @@ -333,8 +333,8 @@ public unsafe override void IncrementallyUpdateContactData(ref TypeBatch typeBat { ref var prestep = ref Unsafe.Add(ref prestepBase, i); ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); - bodies.GatherState(ref references.IndexA, true, out _, out _, out var wsvA, out _); - bodies.GatherState(ref references.IndexB, true, out _, out _, out var wsvB, out _); + bodies.GatherState(references.IndexA, true, out _, out _, out var wsvA, out _); + bodies.GatherState(references.IndexB, true, out _, out _, out var wsvB, out _); function.IncrementallyUpdateContactData(dtWide, wsvA, wsvB, ref prestep); } } diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index 20046c482..a5a2ca2a4 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -1277,14 +1277,14 @@ public static unsafe void GatherAndIntegrate.Zero); - bodies.GatherState(ref bodyIndices, false, out position, out orientation, out velocity, out var localInertia); + bodies.GatherState(bodyIndices, false, out position, out orientation, out velocity, out var localInertia); IntegratePoseAndVelocity(ref integratorCallbacks, ref bodyIndices, localInertia, dt, ref position, ref orientation, ref velocity, workerIndex, out inertia); - bodies.ScatterPose(ref position, ref orientation, ref bodyIndices, ref integrationMask); - bodies.ScatterInertia(ref inertia, ref bodyIndices, ref integrationMask); + bodies.ScatterPose(ref position, ref orientation, bodyIndices, integrationMask); + bodies.ScatterInertia(ref inertia, bodyIndices, integrationMask); } else if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) { - bodies.GatherState(ref bodyIndices, true, out position, out orientation, out velocity, out inertia); + bodies.GatherState(bodyIndices, true, out position, out orientation, out velocity, out inertia); } else { @@ -1296,15 +1296,15 @@ public static unsafe void GatherAndIntegrate(ref bodyIndices, bundleIntegrationMode == BundleIntegrationMode.None, out position, out orientation, out velocity, out var gatheredInertia); + bodies.GatherState(bodyIndices, bundleIntegrationMode == BundleIntegrationMode.None, out position, out orientation, out velocity, out var gatheredInertia); if (bundleIntegrationMode != BundleIntegrationMode.None) { //Note that if we take this codepath, the integration routine will reconstruct the world inertias from local inertia given the current pose. //The changes to pose and velocity for integration inactive lanes will be masked out, so it'll just be identical to the world inertia if we had gathered it. //Given that we're running the instructions in a bundle to build it, there's no reason to go out of our way to gather the world inertia. IntegratePoseAndVelocity(ref integratorCallbacks, ref bodyIndices, gatheredInertia, dt, integrationMask, ref position, ref orientation, ref velocity, workerIndex, out inertia); - bodies.ScatterPose(ref position, ref orientation, ref bodyIndices, ref integrationMask); - bodies.ScatterInertia(ref inertia, ref bodyIndices, ref integrationMask); + bodies.ScatterPose(ref position, ref orientation, bodyIndices, integrationMask); + bodies.ScatterInertia(ref inertia, bodyIndices, integrationMask); } else { @@ -1325,23 +1325,23 @@ public static unsafe void GatherAndIntegrate.Zero); - bodies.GatherState(ref bodyIndices, false, out position, out orientation, out velocity, out var localInertia); + bodies.GatherState(bodyIndices, false, out position, out orientation, out velocity, out var localInertia); IntegrateVelocity(ref integratorCallbacks, ref bodyIndices, localInertia, dt, integrationMask, position, orientation, ref velocity, workerIndex, out inertia); - bodies.ScatterInertia(ref inertia, ref bodyIndices, ref integrationMask); + bodies.ScatterInertia(ref inertia, bodyIndices, integrationMask); } else if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) { - bodies.GatherState(ref bodyIndices, true, out position, out orientation, out velocity, out inertia); + bodies.GatherState(bodyIndices, true, out position, out orientation, out velocity, out inertia); } else { Debug.Assert(typeof(TBatchIntegrationMode) == typeof(BatchShouldConditionallyIntegrate)); var bundleIntegrationMode = BundleShouldIntegrate(bundleIndex, integrationFlags[bodyIndexInConstraint], out var integrationMask); - bodies.GatherState(ref bodyIndices, bundleIntegrationMode == BundleIntegrationMode.None, out position, out orientation, out velocity, out var gatheredInertia); + bodies.GatherState(bodyIndices, bundleIntegrationMode == BundleIntegrationMode.None, out position, out orientation, out velocity, out var gatheredInertia); if (bundleIntegrationMode != BundleIntegrationMode.None) { IntegrateVelocity(ref integratorCallbacks, ref bodyIndices, gatheredInertia, dt, integrationMask, position, orientation, ref velocity, workerIndex, out inertia); - bodies.ScatterInertia(ref inertia, ref bodyIndices, ref integrationMask); + bodies.ScatterInertia(ref inertia, bodyIndices, integrationMask); } else { diff --git a/BepuPhysics/PoseIntegrator.cs b/BepuPhysics/PoseIntegrator.cs index 7bdc3b47d..f52f9b042 100644 --- a/BepuPhysics/PoseIntegrator.cs +++ b/BepuPhysics/PoseIntegrator.cs @@ -801,7 +801,7 @@ unsafe void IntegrateBundlesAfterSubstepping(ref IndexSet mergedConstrainedBodyH bundleEffectiveDt = Vector.ConditionalSelect(unconstrainedMask, bundleDt, bundleSubstepDt); } var halfDt = bundleEffectiveDt * new Vector(0.5f); - bodies.GatherState(ref bodyIndices, false, out var position, out var orientation, out var velocity, out var localInertia); + bodies.GatherState(bodyIndices, false, out var position, out var orientation, out var velocity, out var localInertia); if (anyBodyInBundleIsUnconstrained) { int integrationStepCount; @@ -854,7 +854,7 @@ unsafe void IntegrateBundlesAfterSubstepping(ref IndexSet mergedConstrainedBodyH integratePoseMask = Vector.BitwiseAnd(integratePoseMask, unconstrainedMask); } } - bodies.ScatterPose(ref position, ref orientation, ref bodyIndices, ref integratePoseMask); + bodies.ScatterPose(ref position, ref orientation, bodyIndices, integratePoseMask); //We already masked the velocities above, so scattering them doesn't need its own mask. bodies.ScatterVelocities(ref velocity, ref bodyIndices); } @@ -872,7 +872,7 @@ unsafe void IntegrateBundlesAfterSubstepping(ref IndexSet mergedConstrainedBodyH PoseIntegration.Integrate(orientation, velocity.Angular, halfDt, out orientation); position += velocity.Linear * bundleEffectiveDt; var integratePoseMask = BundleIndexing.CreateMaskForCountInBundle(countInBundle); - bodies.ScatterPose(ref position, ref orientation, ref bodyIndices, ref integratePoseMask); + bodies.ScatterPose(ref position, ref orientation, bodyIndices, integratePoseMask); } } } diff --git a/BepuUtilities/Memory/Buffer.cs b/BepuUtilities/Memory/Buffer.cs index b0b0a9b0f..72deefd5d 100644 --- a/BepuUtilities/Memory/Buffer.cs +++ b/BepuUtilities/Memory/Buffer.cs @@ -62,6 +62,21 @@ public ref T this[int index] } } + /// + /// Gets a reference to the element at the given index. + /// + /// Index of the element to grab a reference of. + /// Reference to the element at the given index. + public ref T this[uint index] + { + [MethodImpl(MethodImplOptions.AggressiveInlining)] + get + { + Debug.Assert(index >= 0 && index < length, "Index out of range."); + return ref Memory[index]; + } + } + /// /// Gets a pointer to the element at the given index. /// @@ -74,6 +89,18 @@ public ref T this[int index] return Memory + index; } + /// + /// Gets a pointer to the element at the given index. + /// + /// Index of the element to retrieve a pointer for. + /// Pointer to the element at the given index. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public T* GetPointer(uint index) + { + Debug.Assert(index >= 0 && index < Length, "Index out of range."); + return Memory + index; + } + /// /// Creates a view of a subset of the buffer's memory. /// diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index 31f4466f2..c2239f40f 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -852,17 +852,17 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) } //Console.WriteLine($"body count: {Simulation.Bodies.ActiveSet.Count}"); //Console.WriteLine($"constraint count: {Simulation.Solver.CountConstraints()}"); - Simulation.Bodies.GetBodyReference(new BodyHandle(55)).Pose.Position += new Vector3(10, 5, 0); + //Simulation.Bodies.GetBodyReference(new BodyHandle(55)).Pose.Position += new Vector3(10, 5, 0); BufferPool.Return(ref vertices); vertexSpatialIndices.Dispose(BufferPool); BufferPool.Return(ref cellVertexIndices); BufferPool.Return(ref tetrahedraVertexIndices); - Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 100, -.5f), 10, Simulation.Shapes, new Sphere(5))); + //Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 100, -.5f), 10, Simulation.Shapes, new Sphere(5))); Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(1500, 1, 1500)), 0.1f))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -1.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Sphere(3)), 0.1f))); + //Simulation.Statics.Add(new StaticDescription(new Vector3(0, -1.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Sphere(3)), 0.1f))); } diff --git a/Demos/Program.cs b/Demos/Program.cs index 1550acdd6..35e88cdf6 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -20,7 +20,7 @@ static void Main(string[] args) { content = ContentArchive.Load(stream); } - //HeadlessTest.Test(content, 4, 32, 512); + HeadlessTest.Test(content, 4, 32, 512); //HeadlessTest.Test(content, 4, 32, 512); //HeadlessTest.Test(content, 4, 32, 512); //HeadlessTest.Test(content, 4, 128, 512); From c4095606a6f51a1de057ea6dda3e6709c5db05aa Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 25 Oct 2021 18:13:39 -0500 Subject: [PATCH 229/947] Kinematic testing intrinsified a little. --- BepuPhysics/Bodies.cs | 62 +++++++++++++++++++++++++++++------ BepuPhysics/BodyProperties.cs | 2 +- BepuPhysics/BodyReference.cs | 4 +-- Demos/DemoSet.cs | 2 +- Demos/Program.cs | 2 +- 5 files changed, 57 insertions(+), 15 deletions(-) diff --git a/BepuPhysics/Bodies.cs b/BepuPhysics/Bodies.cs index f7cae99a3..bae84a495 100644 --- a/BepuPhysics/Bodies.cs +++ b/BepuPhysics/Bodies.cs @@ -8,6 +8,8 @@ using BepuPhysics.CollisionDetection; using BepuUtilities; using static BepuUtilities.GatherScatter; +using System.Runtime.Intrinsics; +using System.Runtime.Intrinsics.X86; namespace BepuPhysics { @@ -270,9 +272,29 @@ internal void RemoveConstraintReference(int bodyIndex, ConstraintHandle constrai /// Body inertia to analyze. /// True if all components of inverse mass and inertia are zero, false otherwise. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static bool IsKinematic(in BodyInertia inertia) + public unsafe static bool IsKinematic(BodyInertia inertia) { - return inertia.InverseMass == 0 && HasLockedInertia(inertia.InverseInertiaTensor); + return IsKinematic(&inertia); + } + + /// + /// Gets whether the inertia matches that of a kinematic body (that is, all inverse mass and inertia components are zero). + /// + /// Body inertia to analyze. + /// True if all components of inverse mass and inertia are zero, false otherwise. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public unsafe static bool IsKinematic(BodyInertia* inertia) + { + if (Avx.IsSupported) + { + var inertiaVector = Avx.LoadVector256((float*)inertia); + var masked = Avx.CompareEqual(inertiaVector, Vector256.Zero); + return (Avx.MoveMask(masked) & 0x7F) == 0x7F; + } + else + { + return inertia->InverseMass == 0 && HasLockedInertia(&inertia->InverseInertiaTensor); + } } /// @@ -281,14 +303,34 @@ public static bool IsKinematic(in BodyInertia inertia) /// Body inertia to analyze. /// True if all components of inverse mass and inertia are zero, false otherwise. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static bool HasLockedInertia(in Symmetric3x3 inertia) - { - return inertia.XX == 0 && - inertia.YX == 0 && - inertia.YY == 0 && - inertia.ZX == 0 && - inertia.ZY == 0 && - inertia.ZZ == 0; + public unsafe static bool HasLockedInertia(Symmetric3x3 inertia) + { + return HasLockedInertia(&inertia); + } + + /// + /// Gets whether the angular inertia matches that of a kinematic body (that is, all inverse inertia tensor components are zero). + /// + /// Body inertia to analyze. + /// True if all components of inverse mass and inertia are zero, false otherwise. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public unsafe static bool HasLockedInertia(Symmetric3x3* inertia) + { + if (Avx.IsSupported) + { + var inertiaVector = Avx.LoadVector256((float*)inertia); + var masked = Avx.CompareEqual(inertiaVector, Vector256.Zero); + return (Avx.MoveMask(masked) & 0x3F) == 0x3F; + } + else + { + return inertia->XX == 0 && + inertia->YX == 0 && + inertia->YY == 0 && + inertia->ZX == 0 && + inertia->ZY == 0 && + inertia->ZZ == 0; + } } private struct ConnectedDynamicCounter : IForEach diff --git a/BepuPhysics/BodyProperties.cs b/BepuPhysics/BodyProperties.cs index 245ba30d4..f08ab4cc4 100644 --- a/BepuPhysics/BodyProperties.cs +++ b/BepuPhysics/BodyProperties.cs @@ -160,7 +160,7 @@ public BodyVelocity(in Vector3 linear, in Vector3 angular) } } - [StructLayout(LayoutKind.Sequential, Size = 32)] + [StructLayout(LayoutKind.Sequential, Size = 32, Pack = 4)] public struct BodyInertia { public Symmetric3x3 InverseInertiaTensor; diff --git a/BepuPhysics/BodyReference.cs b/BepuPhysics/BodyReference.cs index 5bc27a918..4ad7fea95 100644 --- a/BepuPhysics/BodyReference.cs +++ b/BepuPhysics/BodyReference.cs @@ -208,12 +208,12 @@ public CollidableReference CollidableReference /// /// Gets whether the body is kinematic, meaning its inverse inertia and mass are all zero. /// - public bool Kinematic { get { return Bodies.IsKinematic(LocalInertia); } } + public unsafe bool Kinematic { get { return Bodies.IsKinematic((BodyInertia*)Unsafe.AsPointer(ref LocalInertia)); } } /// /// Gets whether the body has locked inertia, meaning its inverse inertia tensor is zero. /// - public bool HasLockedInertia { get { return Bodies.HasLockedInertia(LocalInertia.InverseInertiaTensor); } } + public unsafe bool HasLockedInertia { get { return Bodies.HasLockedInertia((Symmetric3x3*)Unsafe.AsPointer(ref LocalInertia.InverseInertiaTensor)); } } /// /// If the body is dynamic, turns the body kinematic by setting all inverse inertia and mass values to zero and activates it. diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index 7b6619490..669d8f52a 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -45,8 +45,8 @@ struct Option public DemoSet() { - AddOption(); AddOption(); + AddOption(); AddOption(); AddOption(); AddOption(); diff --git a/Demos/Program.cs b/Demos/Program.cs index 35e88cdf6..1550acdd6 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -20,7 +20,7 @@ static void Main(string[] args) { content = ContentArchive.Load(stream); } - HeadlessTest.Test(content, 4, 32, 512); + //HeadlessTest.Test(content, 4, 32, 512); //HeadlessTest.Test(content, 4, 32, 512); //HeadlessTest.Test(content, 4, 32, 512); //HeadlessTest.Test(content, 4, 128, 512); From 6cd2c38f8632e2accc67b94a94db10d41750ddf2 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 26 Oct 2021 16:39:11 -0500 Subject: [PATCH 230/947] In the middle of marking constraint reference indices as kinematic. --- BepuPhysics/Bodies_GatherScatter.cs | 10 ++--- .../NarrowPhasePendingConstraintAdds.cs | 4 +- BepuPhysics/Solver.cs | 38 ++++++++++++++++--- 3 files changed, 40 insertions(+), 12 deletions(-) diff --git a/BepuPhysics/Bodies_GatherScatter.cs b/BepuPhysics/Bodies_GatherScatter.cs index 9a6184225..f6d0dc76d 100644 --- a/BepuPhysics/Bodies_GatherScatter.cs +++ b/BepuPhysics/Bodies_GatherScatter.cs @@ -141,14 +141,14 @@ unsafe static void FallbackGatherInertia(SolverState* states, Vector baseIn } } - const int DoesntExistFlagIndex = 31; - const int KinematicFlagIndex = 30; + public const int DoesntExistFlagIndex = 31; + public const int KinematicFlagIndex = 30; /// /// Constraint body index references greater than a given unsigned value are either kinematic (1<<30 set) or correspond to an empty lane (1<<31 set). /// - const uint DynamicLimit = 1 << KinematicFlagIndex; - const uint BodyIndexMetadataMask = (1u << DoesntExistFlagIndex) | (1u << KinematicFlagIndex); - const int BodyIndexMask = (int)~BodyIndexMetadataMask; + public const uint DynamicLimit = 1 << KinematicFlagIndex; + public const uint BodyIndexMetadataMask = (1u << DoesntExistFlagIndex) | (1u << KinematicFlagIndex); + public const int BodyIndexMask = (int)~BodyIndexMetadataMask; //[MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe void GatherState(Vector encodedBodyIndices, bool worldInertia, out Vector3Wide position, out QuaternionWide orientation, out BodyVelocityWide velocity, out BodyInertiaWide inertia) diff --git a/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs b/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs index d90f383df..ff3216cf5 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs @@ -120,9 +120,11 @@ static unsafe void AddToSimulationSpeculative(Unsafe.AsPointer(ref constraint.BodyHandles), typeof(TBodyHandles) == typeof(TwoBodyHandles) ? 2 : 1); + Span blockingBodyHandles = stackalloc BodyHandle[handles.Length]; + blockingBodyHandles = simulation.Solver.GetBlockingBodyHandles(handles, blockingBodyHandles); while (!simulation.Solver.TryAllocateInBatch( default(TDescription).ConstraintTypeId, batchIndex, - handles, out constraintHandle, out reference)) + blockingBodyHandles, handles, out constraintHandle, out reference)) { //If a batch index failed, just try the next one. This is guaranteed to eventually work. ++batchIndex; diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index a15e86526..d50732220 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -758,10 +758,11 @@ internal unsafe int FindCandidateBatch(Span bodyHandles) { ref var set = ref ActiveSet; GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); - var bodyHandlesAsIntegers = MemoryMarshal.Cast(bodyHandles); + Span blockingBodyHandlesAllocation = stackalloc BodyHandle[bodyHandles.Length]; + var blockingBodyHandles = MemoryMarshal.Cast(GetBlockingBodyHandles(bodyHandles, blockingBodyHandlesAllocation)); for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) { - if (batchReferencedHandles[batchIndex].CanFit(bodyHandlesAsIntegers)) + if (batchReferencedHandles[batchIndex].CanFit(blockingBodyHandles)) return batchIndex; } //No synchronized batch worked. Either there's a fallback batch or there aren't yet enough batches to warrant a fallback batch and none of the existing batches could fit the handles. @@ -779,7 +780,13 @@ internal unsafe void AllocateInBatch(int targetBatchIndex, ConstraintHandle cons var bodyHandle = bodyHandles[j]; ref var location = ref bodies.HandleToLocation[bodyHandle.Value]; Debug.Assert(location.SetIndex == 0, "Creating a new constraint should have forced the connected bodies awake."); - bodyIndices[j] = location.Index; + var index = location.Index; + //Include the flag in the body index if it's kinematic. + if (Bodies.IsKinematic(bodies.ActiveSet.SolverStates[index].Inertia.Local)) + { + index |= 1 << Bodies.KinematicFlagIndex; + } + bodyIndices[j] = index; } var typeProcessor = TypeProcessors[typeId]; var typeBatch = batch.GetOrCreateTypeBatch(typeId, typeProcessor, GetMinimumCapacityForType(typeId), pool); @@ -810,7 +817,24 @@ internal unsafe void AllocateInBatch(int targetBatchIndex, ConstraintHandle cons } } - internal unsafe bool TryAllocateInBatch(int typeId, int targetBatchIndex, Span bodyHandles, out ConstraintHandle constraintHandle, out ConstraintReference reference) + internal Span GetBlockingBodyHandles(Span bodyHandles, Span allocation) + { + //Kinematics do not block allocation in a batch; they are treated as read only by the solver. + int blockingCount = 0; + var solverStates = bodies.ActiveSet.SolverStates; + for (int i = 0; i < bodyHandles.Length; ++i) + { + var location = bodies.HandleToLocation[bodyHandles[i].Value]; + Debug.Assert(location.SetIndex == 0); + if (Bodies.IsKinematic(solverStates[location.Index].Inertia.Local)) + { + allocation[blockingCount++] = bodyHandles[i]; + } + } + return allocation.Slice(0, blockingCount); + } + + internal unsafe bool TryAllocateInBatch(int typeId, int targetBatchIndex, Span blockingBodyHandles, Span bodyHandles, out ConstraintHandle constraintHandle, out ConstraintReference reference) { ref var set = ref ActiveSet; Debug.Assert(targetBatchIndex <= set.Batches.Count, @@ -833,7 +857,7 @@ internal unsafe bool TryAllocateInBatch(int typeId, int targetBatchIndex, Span(bodyHandles))) + if (!batchReferencedHandles[targetBatchIndex].CanFit(MemoryMarshal.Cast(blockingBodyHandles))) { //This batch cannot hold the constraint. constraintHandle = new ConstraintHandle(-1); @@ -939,9 +963,11 @@ void Add(Span bodyHandles, ref TDescription descriptio where TDescription : unmanaged, IConstraintDescription { ref var set = ref ActiveSet; + Span blockingBodyHandles = stackalloc BodyHandle[bodyHandles.Length]; + blockingBodyHandles = GetBlockingBodyHandles(bodyHandles, blockingBodyHandles); for (int i = 0; i <= set.Batches.Count; ++i) { - if (TryAllocateInBatch(description.ConstraintTypeId, i, bodyHandles, out handle, out var reference)) + if (TryAllocateInBatch(description.ConstraintTypeId, i, blockingBodyHandles, bodyHandles, out handle, out var reference)) { ApplyDescriptionWithoutWaking(ref reference, ref description); return; From 6240ee0a18348de6b26645d6600b8549d3ecca0e Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 26 Oct 2021 16:48:06 -0500 Subject: [PATCH 231/947] Purged a number of now-unnecessary ref overloads. --- .../NarrowPhaseConstraintUpdate.cs | 2 +- .../NarrowPhasePendingConstraintAdds.cs | 4 +- BepuPhysics/Solver.cs | 134 +++--------------- .../ConstraintDescriptionMappingTests.cs | 2 +- Demos/Demos/BlockChainDemo.cs | 2 +- Demos/Demos/Cars/SimpleCar.cs | 2 +- .../Demos/Characters/CharacterControllers.cs | 8 +- Demos/Demos/Tanks/Tank.cs | 10 +- Demos/Grabber.cs | 8 +- Demos/SpecializedTests/ClothLatticeDemo.cs | 8 +- .../SpecializedTests/SimulationScrambling.cs | 2 +- Demos/SpecializedTests/SolverBatchTestDemo.cs | 8 +- 12 files changed, 46 insertions(+), 144 deletions(-) diff --git a/BepuPhysics/CollisionDetection/NarrowPhaseConstraintUpdate.cs b/BepuPhysics/CollisionDetection/NarrowPhaseConstraintUpdate.cs index c83f82ea7..722ae15f8 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhaseConstraintUpdate.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhaseConstraintUpdate.cs @@ -206,7 +206,7 @@ public unsafe void UpdateConstraint(ref newConstraintCache) = constraintHandle; PairCache.Update(workerIndex, index, ref pointers, ref collisionCache, ref newConstraintCache); //There exists a constraint and it has the same type as the manifold. Directly apply the new description and impulses. - Solver.ApplyDescriptionWithoutWaking(ref constraintReference, ref description); + Solver.ApplyDescriptionWithoutWaking(constraintReference, description); accessor.ScatterNewImpulses(ref constraintReference, ref newImpulses); } else diff --git a/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs b/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs index ff3216cf5..e086b2b6f 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs @@ -73,7 +73,7 @@ public static unsafe void SequentialAddToSimulation(Unsafe.AsPointer(ref add.BodyHandles), typeof(TBodyHandles) == typeof(TwoBodyHandles) ? 2 : 1), ref add.ConstraintDescription); + var handle = simulation.Solver.Add(new Span(Unsafe.AsPointer(ref add.BodyHandles), typeof(TBodyHandles) == typeof(TwoBodyHandles) ? 2 : 1), add.ConstraintDescription); pairCache.CompleteConstraintAdd(simulation.NarrowPhase, simulation.Solver, ref add.Impulses, add.ConstraintCacheIndex, handle, ref add.Pair); } } @@ -129,7 +129,7 @@ static unsafe void AddToSimulationSpeculativeType of the description to apply. /// Reference of the constraint being updated. /// Description to apply to the slot. - [MethodImpl(MethodImplOptions.NoInlining)] - public void ApplyDescriptionWithoutWaking(ref ConstraintReference constraintReference, ref TDescription description) + public unsafe void ApplyDescriptionWithoutWaking(in ConstraintReference constraintReference, in TDescription description) where TDescription : unmanaged, IConstraintDescription { BundleIndexing.GetBundleIndices(constraintReference.IndexInTypeBatch, out var bundleIndex, out var innerIndex); - //TODO: Note that it would be pretty nice to allow in parameters to avoid the need for the inefficient value type convenience overloads. - //The reason why we use ref is that the JIT does not recognize that this instance call is not mutating the instance. - //It emits a localsinit AND a copy. - //An ideal solution here (other than raw optimizer improvements) would be some language feature that permits the expression of functions-that-work-on-data - //in a generic fashion without indirection, and without introducing syntax pain. - //(If you accept syntax pain, it is possible already- pass a struct type that exposes interface implementations that process descriptions, but contains no data of its own. - //That 'executor' type has trivial clearing cost which should go away entirely with inlining even with the current optimizer. Compare that level of added complexity - //with IConstraintDescription simply carrying a requirement to implement a static function. Future versions of C# should make this sort of construct easier to deal with.) - description.ApplyDescription(ref constraintReference.TypeBatch, bundleIndex, innerIndex); + description.ApplyDescription(ref *constraintReference.typeBatchPointer, bundleIndex, innerIndex); } /// @@ -917,22 +908,11 @@ public void ApplyDescriptionWithoutWaking(ref ConstraintReference /// Type of the description to apply. /// Handle of the constraint being updated. /// Description to apply to the slot. - public void ApplyDescriptionWithoutWaking(ConstraintHandle constraintHandle, ref TDescription description) + public void ApplyDescriptionWithoutWaking(ConstraintHandle constraintHandle, in TDescription description) where TDescription : unmanaged, IConstraintDescription { GetConstraintReference(constraintHandle, out var constraintReference); - ApplyDescriptionWithoutWaking(ref constraintReference, ref description); - } - /// - /// Applies a description to a constraint slot without waking up the associated island. - /// - /// Type of the description to apply. - /// Handle of the constraint being updated. - /// Description to apply to the slot. - public void ApplyDescriptionWithoutWaking(ConstraintHandle constraintHandle, TDescription description) - where TDescription : unmanaged, IConstraintDescription - { - ApplyDescriptionWithoutWaking(constraintHandle, ref description); + ApplyDescriptionWithoutWaking(constraintReference, description); } /// @@ -941,25 +921,14 @@ public void ApplyDescriptionWithoutWaking(ConstraintHandle constra /// Type of the description to apply. /// Handle of the constraint being updated. /// Description to apply to the slot. - public void ApplyDescription(ConstraintHandle constraintHandle, ref TDescription description) + public void ApplyDescription(ConstraintHandle constraintHandle, in TDescription description) where TDescription : unmanaged, IConstraintDescription { awakener.AwakenConstraint(constraintHandle); - ApplyDescriptionWithoutWaking(constraintHandle, ref description); - } - /// - /// Applies a description to a constraint slot, waking up the connected bodies if necessary. - /// - /// Type of the description to apply. - /// Handle of the constraint being updated. - /// Description to apply to the slot. - public void ApplyDescription(ConstraintHandle constraintHandle, TDescription description) - where TDescription : unmanaged, IConstraintDescription - { - ApplyDescription(constraintHandle, ref description); + ApplyDescriptionWithoutWaking(constraintHandle, description); } - void Add(Span bodyHandles, ref TDescription description, out ConstraintHandle handle) + void Add(Span bodyHandles, in TDescription description, out ConstraintHandle handle) where TDescription : unmanaged, IConstraintDescription { ref var set = ref ActiveSet; @@ -969,7 +938,7 @@ void Add(Span bodyHandles, ref TDescription descriptio { if (TryAllocateInBatch(description.ConstraintTypeId, i, blockingBodyHandles, bodyHandles, out handle, out var reference)) { - ApplyDescriptionWithoutWaking(ref reference, ref description); + ApplyDescriptionWithoutWaking(reference, description); return; } } @@ -983,7 +952,7 @@ void Add(Span bodyHandles, ref TDescription descriptio /// Type of the constraint description to add. /// Body handles used by the constraint. /// Allocated constraint handle. - public ConstraintHandle Add(Span bodyHandles, ref TDescription description) + public ConstraintHandle Add(Span bodyHandles, in TDescription description) where TDescription : unmanaged, IConstraintDescription { Debug.Assert(description.ConstraintTypeId >= 0 && description.ConstraintTypeId < TypeProcessors.Length && @@ -996,7 +965,7 @@ public ConstraintHandle Add(Span bodyHandles, ref TDes { awakener.AwakenBody(bodyHandles[i]); } - Add(bodyHandles, ref description, out var constraintHandle); + Add(bodyHandles, description, out var constraintHandle); for (int i = 0; i < bodyHandles.Length; ++i) { var bodyHandle = bodyHandles[i]; @@ -1006,42 +975,17 @@ public ConstraintHandle Add(Span bodyHandles, ref TDes return constraintHandle; } - /// - /// Allocates a constraint slot and sets up a constraint with the specified description. - /// - /// Type of the constraint description to add. - /// Body handles referenced by the constraint. - /// Allocated constraint handle. - public ConstraintHandle Add(Span bodyHandles, TDescription description) - where TDescription : unmanaged, IConstraintDescription - { - return Add(bodyHandles, ref description); - } - /// /// Allocates a one-body constraint slot and sets up a constraint with the specified description. /// /// Type of the constraint description to add. /// Body connected to the constraint. /// Allocated constraint handle. - public unsafe ConstraintHandle Add(BodyHandle bodyHandle, ref TDescription description) - where TDescription : unmanaged, IOneBodyConstraintDescription - { - Span bodyHandles = stackalloc BodyHandle[] { bodyHandle }; - return Add(bodyHandles, ref description); - } - - /// - /// Allocates a one-body constraint slot and sets up a constraint with the specified description. - /// - /// Type of the constraint description to add. - /// First body of the constraint. - /// Allocated constraint handle. - public unsafe ConstraintHandle Add(BodyHandle bodyHandle, TDescription description) + public unsafe ConstraintHandle Add(BodyHandle bodyHandle, in TDescription description) where TDescription : unmanaged, IOneBodyConstraintDescription { Span bodyHandles = stackalloc BodyHandle[] { bodyHandle }; - return Add(bodyHandles, ref description); + return Add(bodyHandles, description); } /// @@ -1051,24 +995,11 @@ public unsafe ConstraintHandle Add(BodyHandle bodyHandle, TDescrip /// First body of the constraint. /// Second body of the constraint. /// Allocated constraint handle. - public unsafe ConstraintHandle Add(BodyHandle bodyHandleA, BodyHandle bodyHandleB, ref TDescription description) + public unsafe ConstraintHandle Add(BodyHandle bodyHandleA, BodyHandle bodyHandleB, in TDescription description) where TDescription : unmanaged, ITwoBodyConstraintDescription { Span bodyHandles = stackalloc BodyHandle[] { bodyHandleA, bodyHandleB }; - return Add(bodyHandles, ref description); - } - - /// - /// Allocates a two-body constraint slot and sets up a constraint with the specified description. - /// - /// Type of the constraint description to add. - /// First body of the constraint. - /// Second body of the constraint. - /// Allocated constraint handle. - public unsafe ConstraintHandle Add(BodyHandle bodyHandleA, BodyHandle bodyHandleB, TDescription description) - where TDescription : unmanaged, ITwoBodyConstraintDescription - { - return Add(bodyHandleA, bodyHandleB, ref description); + return Add(bodyHandles, description); } /// @@ -1079,25 +1010,11 @@ public unsafe ConstraintHandle Add(BodyHandle bodyHandleA, BodyHan /// Second body of the constraint. /// Third body of the constraint. /// Allocated constraint handle. - public unsafe ConstraintHandle Add(BodyHandle bodyHandleA, BodyHandle bodyHandleB, BodyHandle bodyHandleC, ref TDescription description) + public unsafe ConstraintHandle Add(BodyHandle bodyHandleA, BodyHandle bodyHandleB, BodyHandle bodyHandleC, in TDescription description) where TDescription : unmanaged, IThreeBodyConstraintDescription { Span bodyHandles = stackalloc BodyHandle[] { bodyHandleA, bodyHandleB, bodyHandleC }; - return Add(bodyHandles, ref description); - } - - /// - /// Allocates a three-body constraint slot and sets up a constraint with the specified description. - /// - /// Type of the constraint description to add. - /// First body of the constraint. - /// Second body of the constraint. - /// Third body of the constraint. - /// Allocated constraint handle. - public unsafe ConstraintHandle Add(BodyHandle bodyHandleA, BodyHandle bodyHandleB, BodyHandle bodyHandleC, TDescription description) - where TDescription : unmanaged, IThreeBodyConstraintDescription - { - return Add(bodyHandleA, bodyHandleB, bodyHandleC, ref description); + return Add(bodyHandles, description); } /// @@ -1109,26 +1026,11 @@ public unsafe ConstraintHandle Add(BodyHandle bodyHandleA, BodyHan /// Third body of the constraint. /// Fourth body of the constraint. /// Allocated constraint handle. - public unsafe ConstraintHandle Add(BodyHandle bodyHandleA, BodyHandle bodyHandleB, BodyHandle bodyHandleC, BodyHandle bodyHandleD, ref TDescription description) + public unsafe ConstraintHandle Add(BodyHandle bodyHandleA, BodyHandle bodyHandleB, BodyHandle bodyHandleC, BodyHandle bodyHandleD, in TDescription description) where TDescription : unmanaged, IFourBodyConstraintDescription { Span bodyHandles = stackalloc BodyHandle[] { bodyHandleA, bodyHandleB, bodyHandleC, bodyHandleD }; - return Add(bodyHandles, ref description); - } - - /// - /// Allocates a four-body constraint slot and sets up a constraint with the specified description. - /// - /// Type of the constraint description to add. - /// First body of the constraint. - /// Second body of the constraint. - /// Third body of the constraint. - /// Fourth body of the constraint. - /// Allocated constraint handle. - public unsafe ConstraintHandle Add(BodyHandle bodyHandleA, BodyHandle bodyHandleB, BodyHandle bodyHandleC, BodyHandle bodyHandleD, TDescription description) - where TDescription : unmanaged, IFourBodyConstraintDescription - { - return Add(bodyHandleA, bodyHandleB, bodyHandleC, bodyHandleD, ref description); + return Add(bodyHandles, description); } //This is split out for use by the multithreaded constraint remover. diff --git a/DemoTests/ConstraintDescriptionMappingTests.cs b/DemoTests/ConstraintDescriptionMappingTests.cs index 7687662b5..f0d8e4ae1 100644 --- a/DemoTests/ConstraintDescriptionMappingTests.cs +++ b/DemoTests/ConstraintDescriptionMappingTests.cs @@ -57,7 +57,7 @@ static void Test(BufferPool pool, Random random, int constraintTypeBodyCount) ref var source = ref sources[constraintTestIndex]; FillWithRandomBytes(ref source, random); - constraintHandles[constraintTestIndex] = simulation.Solver.Add(constraintBodyHandles, ref source); + constraintHandles[constraintTestIndex] = simulation.Solver.Add(constraintBodyHandles, source); } for (int constraintTestIndex = 0; constraintTestIndex < constraintTestCount; ++constraintTestIndex) diff --git a/Demos/Demos/BlockChainDemo.cs b/Demos/Demos/BlockChainDemo.cs index c52928e7e..a2010246f 100644 --- a/Demos/Demos/BlockChainDemo.cs +++ b/Demos/Demos/BlockChainDemo.cs @@ -58,7 +58,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) LocalOffsetB = new Vector3(0, -1f, 0), SpringSettings = new SpringSettings(30, 5) }; - Simulation.Solver.Add(blockHandles[i - 1], blockHandles[i], ref ballSocket); + Simulation.Solver.Add(blockHandles[i - 1], blockHandles[i], ballSocket); } } diff --git a/Demos/Demos/Cars/SimpleCar.cs b/Demos/Demos/Cars/SimpleCar.cs index 97ebb74ce..e585be0bf 100644 --- a/Demos/Demos/Cars/SimpleCar.cs +++ b/Demos/Demos/Cars/SimpleCar.cs @@ -22,7 +22,7 @@ public void Steer(Simulation simulation, in WheelHandles wheel, float angle) var steeredHinge = hingeDescription; Matrix3x3.CreateFromAxisAngle(suspensionDirection, -angle, out var rotation); Matrix3x3.Transform(hingeDescription.LocalHingeAxisA, rotation, out steeredHinge.LocalHingeAxisA); - simulation.Solver.ApplyDescription(wheel.Hinge, ref steeredHinge); + simulation.Solver.ApplyDescription(wheel.Hinge, steeredHinge); } public void SetSpeed(Simulation simulation, in WheelHandles wheel, float speed, float maximumForce) diff --git a/Demos/Demos/Characters/CharacterControllers.cs b/Demos/Demos/Characters/CharacterControllers.cs index 954df3c17..ebb921d35 100644 --- a/Demos/Demos/Characters/CharacterControllers.cs +++ b/Demos/Demos/Characters/CharacterControllers.cs @@ -678,7 +678,7 @@ void AnalyzeContactsForCharacterRegion(int start, int exclusiveEnd, int workerIn if (character.Supported && !shouldRemove) { //Already exists, update it. - Simulation.Solver.ApplyDescriptionWithoutWaking(character.MotionConstraintHandle, ref motionConstraint); + Simulation.Solver.ApplyDescriptionWithoutWaking(character.MotionConstraintHandle, motionConstraint); } else { @@ -703,7 +703,7 @@ void AnalyzeContactsForCharacterRegion(int start, int exclusiveEnd, int workerIn if (character.Supported && !shouldRemove) { //Already exists, update it. - Simulation.Solver.ApplyDescriptionWithoutWaking(character.MotionConstraintHandle, ref motionConstraint); + Simulation.Solver.ApplyDescriptionWithoutWaking(character.MotionConstraintHandle, motionConstraint); } else { @@ -815,14 +815,14 @@ void AnalyzeContacts(float dt, IThreadDispatcher threadDispatcher) ref var pendingConstraint = ref workerCache.StaticConstraintsToAdd[i]; ref var character = ref characters[pendingConstraint.CharacterIndex]; Debug.Assert(character.Support.Mobility == CollidableMobility.Static); - character.MotionConstraintHandle = Simulation.Solver.Add(character.BodyHandle, ref pendingConstraint.Description); + character.MotionConstraintHandle = Simulation.Solver.Add(character.BodyHandle, pendingConstraint.Description); } for (int i = 0; i < workerCache.DynamicConstraintsToAdd.Count; ++i) { ref var pendingConstraint = ref workerCache.DynamicConstraintsToAdd[i]; ref var character = ref characters[pendingConstraint.CharacterIndex]; Debug.Assert(character.Support.Mobility != CollidableMobility.Static); - character.MotionConstraintHandle = Simulation.Solver.Add(character.BodyHandle, character.Support.BodyHandle, ref pendingConstraint.Description); + character.MotionConstraintHandle = Simulation.Solver.Add(character.BodyHandle, character.Support.BodyHandle, pendingConstraint.Description); } ref var activeSet = ref Simulation.Bodies.ActiveSet; for (int i = 0; i < workerCache.Jumps.Count; ++i) diff --git a/Demos/Demos/Tanks/Tank.cs b/Demos/Demos/Tanks/Tank.cs index 984d0d024..390104566 100644 --- a/Demos/Demos/Tanks/Tank.cs +++ b/Demos/Demos/Tanks/Tank.cs @@ -97,7 +97,7 @@ public void SetSpeed(Simulation simulation, Buffer motors, flo }; for (int i = 0; i < motors.Length; ++i) { - simulation.Solver.ApplyDescription(motors[i], ref motorDescription); + simulation.Solver.ApplyDescription(motors[i], motorDescription); } } @@ -136,10 +136,10 @@ public void SetAim(Simulation simulation, float targetSwivelAngle, float targetP { var turretDescription = TurretServoDescription; turretDescription.TargetAngle = targetSwivelAngle; - simulation.Solver.ApplyDescription(TurretServo, ref turretDescription); + simulation.Solver.ApplyDescription(TurretServo, turretDescription); var barrelDescription = BarrelServoDescription; barrelDescription.TargetAngle = targetPitchAngle; - simulation.Solver.ApplyDescription(BarrelServo, ref barrelDescription); + simulation.Solver.ApplyDescription(BarrelServo, barrelDescription); } @@ -370,8 +370,8 @@ public static Tank Create(Simulation simulation, CollidableProperty= 1) - Simulation.Solver.Add(nodeHandles[i][j], nodeHandles[i - 1][j], ref left); + Simulation.Solver.Add(nodeHandles[i][j], nodeHandles[i - 1][j], left); if (j >= 1) - Simulation.Solver.Add(nodeHandles[i][j], nodeHandles[i][j - 1], ref up); + Simulation.Solver.Add(nodeHandles[i][j], nodeHandles[i][j - 1], up); if (i >= 1 && j >= 1) - Simulation.Solver.Add(nodeHandles[i][j], nodeHandles[i - 1][j - 1], ref leftUp); + Simulation.Solver.Add(nodeHandles[i][j], nodeHandles[i - 1][j - 1], leftUp); if (i < width - 1 && j >= 1) - Simulation.Solver.Add(nodeHandles[i][j], nodeHandles[i + 1][j - 1], ref rightUp); + Simulation.Solver.Add(nodeHandles[i][j], nodeHandles[i + 1][j - 1], rightUp); } } var bigBallShape = new Sphere(25); diff --git a/Demos/SpecializedTests/SimulationScrambling.cs b/Demos/SpecializedTests/SimulationScrambling.cs index 0daee0dfd..f6ac91c1b 100644 --- a/Demos/SpecializedTests/SimulationScrambling.cs +++ b/Demos/SpecializedTests/SimulationScrambling.cs @@ -243,7 +243,7 @@ private static void ChurnAddConstraint(Simulation simulation, BodyHandle[] bo if (handleA.Value >= 0 && handleB.Value >= 0) { //The constraint is addable. - var constraintHandle = simulation.Solver.Add(handleA, handleB, ref constraint.Description); + var constraintHandle = simulation.Solver.Add(handleA, handleB, constraint.Description); constraintHandles[constraintIdentity] = constraintHandle; constraintHandlesToIdentity[constraintHandle.Value] = constraintIdentity; WriteLine($"Added constraint, handle: {constraintHandle}"); diff --git a/Demos/SpecializedTests/SolverBatchTestDemo.cs b/Demos/SpecializedTests/SolverBatchTestDemo.cs index 8dd332938..773c3f232 100644 --- a/Demos/SpecializedTests/SolverBatchTestDemo.cs +++ b/Demos/SpecializedTests/SolverBatchTestDemo.cs @@ -87,13 +87,13 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) for (int j = 0; j < length; ++j) { if (i >= 1) - Simulation.Solver.Add(nodeHandles[i][j], nodeHandles[i - 1][j], ref left); + Simulation.Solver.Add(nodeHandles[i][j], nodeHandles[i - 1][j], left); if (j >= 1) - Simulation.Solver.Add(nodeHandles[i][j], nodeHandles[i][j - 1], ref up); + Simulation.Solver.Add(nodeHandles[i][j], nodeHandles[i][j - 1], up); if (i >= 1 && j >= 1) - Simulation.Solver.Add(nodeHandles[i][j], nodeHandles[i - 1][j - 1], ref leftUp); + Simulation.Solver.Add(nodeHandles[i][j], nodeHandles[i - 1][j - 1], leftUp); if (i < width - 1 && j >= 1) - Simulation.Solver.Add(nodeHandles[i][j], nodeHandles[i + 1][j - 1], ref rightUp); + Simulation.Solver.Add(nodeHandles[i][j], nodeHandles[i + 1][j - 1], rightUp); } } var bigBallShape = new Sphere(45); From 4aea4ea405e6c1f8ccb78c5109795dca1ce9425b Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 26 Oct 2021 17:43:31 -0500 Subject: [PATCH 232/947] Added validation for reference kinematicity. --- BepuPhysics/Bodies.cs | 17 ++++++++++++-- BepuPhysics/Bodies_GatherScatter.cs | 3 ++- BepuPhysics/BodyReference.cs | 2 +- BepuPhysics/Solver.cs | 36 +++++++++++++++++++++++++++-- 4 files changed, 52 insertions(+), 6 deletions(-) diff --git a/BepuPhysics/Bodies.cs b/BepuPhysics/Bodies.cs index bae84a495..301818eea 100644 --- a/BepuPhysics/Bodies.cs +++ b/BepuPhysics/Bodies.cs @@ -277,6 +277,19 @@ public unsafe static bool IsKinematic(BodyInertia inertia) return IsKinematic(&inertia); } + /// + /// Gets whether the inertia matches that of a kinematic body (that is, all inverse mass and inertia components are zero). + /// + /// Body inertia to analyze. Must be a reference to fixed data; a pointer will be taken. + /// True if all components of inverse mass and inertia are zero, false otherwise. + /// This is not exposed by default because of the risk of a non-obvious GC hole. + /// It exists because it's a mildly more convenient form than the pointer overload, and every use within the engine references only pinned data. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + internal unsafe static bool IsKinematicUnsafe(ref BodyInertia inertia) + { + return IsKinematic((BodyInertia*)Unsafe.AsPointer(ref inertia)); + } + /// /// Gets whether the inertia matches that of a kinematic body (that is, all inverse mass and inertia components are zero). /// @@ -338,11 +351,11 @@ private struct ConnectedDynamicCounter : IForEach public Bodies Bodies; public int DynamicCount; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void LoopBody(int bodyIndex) + public unsafe void LoopBody(int bodyIndex) { //The solver's connected bodies enumeration directly provides the constraint-stored reference, which is an index in the active set for active constraints and a handle for inactive constraints. //We forced the dynamic active at the beginning of BecomeKinematic, so we don't have to worry about the inactive side of things. - if (!IsKinematic(Bodies.ActiveSet.SolverStates[bodyIndex].Inertia.Local)) + if (!IsKinematicUnsafe(ref Bodies.ActiveSet.SolverStates[bodyIndex].Inertia.Local)) ++DynamicCount; } } diff --git a/BepuPhysics/Bodies_GatherScatter.cs b/BepuPhysics/Bodies_GatherScatter.cs index f6d0dc76d..c2b78d7e4 100644 --- a/BepuPhysics/Bodies_GatherScatter.cs +++ b/BepuPhysics/Bodies_GatherScatter.cs @@ -143,10 +143,11 @@ unsafe static void FallbackGatherInertia(SolverState* states, Vector baseIn public const int DoesntExistFlagIndex = 31; public const int KinematicFlagIndex = 30; + public const uint KinematicMask = 1u << KinematicFlagIndex; /// /// Constraint body index references greater than a given unsigned value are either kinematic (1<<30 set) or correspond to an empty lane (1<<31 set). /// - public const uint DynamicLimit = 1 << KinematicFlagIndex; + public const uint DynamicLimit = KinematicMask; public const uint BodyIndexMetadataMask = (1u << DoesntExistFlagIndex) | (1u << KinematicFlagIndex); public const int BodyIndexMask = (int)~BodyIndexMetadataMask; diff --git a/BepuPhysics/BodyReference.cs b/BepuPhysics/BodyReference.cs index 4ad7fea95..3ea8619fe 100644 --- a/BepuPhysics/BodyReference.cs +++ b/BepuPhysics/BodyReference.cs @@ -208,7 +208,7 @@ public CollidableReference CollidableReference /// /// Gets whether the body is kinematic, meaning its inverse inertia and mass are all zero. /// - public unsafe bool Kinematic { get { return Bodies.IsKinematic((BodyInertia*)Unsafe.AsPointer(ref LocalInertia)); } } + public unsafe bool Kinematic { get { return Bodies.IsKinematicUnsafe(ref LocalInertia); } } /// /// Gets whether the body has locked inertia, meaning its inverse inertia tensor is zero. diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index 8051b5726..78f43f348 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -274,6 +274,38 @@ public unsafe void GetConstraintReference(ConstraintHandle handle, out Constrain reference = new ConstraintReference(Sets[constraintLocation.SetIndex].Batches[constraintLocation.BatchIndex].GetTypeBatchPointer(constraintLocation.TypeId), constraintLocation.IndexInTypeBatch); } + [Conditional("DEBUG")] + unsafe internal void ValidateConstraintReferenceKinematicity() + { + //Only the active set's body indices are flagged for kinematicity; the inactive sets store body handles. + ref var activeSet = ref ActiveSet; + for (int batchIndex = 0; batchIndex < activeSet.Batches.Count; ++batchIndex) + { + ref var batch = ref activeSet.Batches[batchIndex]; + for (int typeBatchIndex = 0; typeBatchIndex < batch.TypeBatches.Count; ++typeBatchIndex) + { + ref var typeBatch = ref batch.TypeBatches[typeBatchIndex]; + var bodiesPerConstraint = TypeProcessors[typeBatch.TypeId].BodiesPerConstraint; + for (int i = 0; i < typeBatch.ConstraintCount; ++i) + { + BundleIndexing.GetBundleIndices(i, out var bundleIndex, out var innerIndex); + ref var bodyReferencesBundle = ref typeBatch.BodyReferences[bundleIndex * bodiesPerConstraint * Unsafe.SizeOf>()]; + for (int bodyIndexInConstraint = 0; bodyIndexInConstraint < bodiesPerConstraint; ++bodyIndexInConstraint) + { + var referencesForBodyIndexInConstraint = Unsafe.Add(ref Unsafe.As>(ref bodyReferencesBundle), bodyIndexInConstraint); + var encodedBodyIndex = referencesForBodyIndexInConstraint[innerIndex]; + if (encodedBodyIndex > 0) + { + var kinematicByEncodedIndex = (encodedBodyIndex & Bodies.KinematicMask) != 0; + var kinematicByInertia = Bodies.IsKinematicUnsafe(ref bodies.ActiveSet.SolverStates[encodedBodyIndex & Bodies.BodyIndexMask].Inertia.Local); + Debug.Assert(kinematicByEncodedIndex == kinematicByInertia, "Constraint reference encoded kinematicity must match actual kinematicity by inertia."); + } + } + } + } + } + } + [Conditional("DEBUG")] private void ValidateBodyReference(int body, int expectedCount, ref ConstraintBatch batch) { @@ -817,7 +849,7 @@ internal unsafe void AllocateInBatch(int targetBatchIndex, ConstraintHandle cons } } - internal Span GetBlockingBodyHandles(Span bodyHandles, Span allocation) + unsafe internal Span GetBlockingBodyHandles(Span bodyHandles, Span allocation) { //Kinematics do not block allocation in a batch; they are treated as read only by the solver. int blockingCount = 0; @@ -826,7 +858,7 @@ internal Span GetBlockingBodyHandles(Span bodyHandles, S { var location = bodies.HandleToLocation[bodyHandles[i].Value]; Debug.Assert(location.SetIndex == 0); - if (Bodies.IsKinematic(solverStates[location.Index].Inertia.Local)) + if (Bodies.IsKinematicUnsafe(ref solverStates[location.Index].Inertia.Local)) { allocation[blockingCount++] = bodyHandles[i]; } From fceeee3570d8a6b3a9437dbf5c80ef76807d980f Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 26 Oct 2021 19:57:23 -0500 Subject: [PATCH 233/947] Does not build. Filling out solver kinematic integration prepass. --- .../EmbeddedSubsteppingTimestepper2.cs | 11 +- BepuPhysics/Solver.cs | 21 +++- BepuPhysics/Solver_SubsteppingSolve2.cs | 108 +++++++++++++++--- 3 files changed, 118 insertions(+), 22 deletions(-) diff --git a/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs b/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs index 82b583bc5..9473bda7a 100644 --- a/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs +++ b/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs @@ -56,11 +56,12 @@ public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDi CollisionsDetected?.Invoke(dt, threadDispatcher); Debug.Assert(SubstepCount >= 0, "Substep count should be positive."); - //simulation.Solver.ValidateTrailingTypeBatchBodyReferences(); - //simulation.Solver.ValidateFallbackBatchEmptySlotReferences(); - //simulation.Solver.ValidateFallbackBatchAccessSafety(); - //simulation.Solver.ValidateFallbackBatchAccumulatedImpulses(); - //simulation.Solver.ValidateConstraintMaps(); + simulation.Solver.ValidateTrailingTypeBatchBodyReferences(); + simulation.Solver.ValidateFallbackBatchEmptySlotReferences(); + simulation.Solver.ValidateFallbackBatchAccessSafety(); + simulation.Solver.ValidateFallbackBatchAccumulatedImpulses(); + simulation.Solver.ValidateConstraintMaps(); + simulation.Solver.ValidateConstraintReferenceKinematicity(); var constrainedBodySet = simulation.Solver.PrepareConstraintIntegrationResponsibilities(SubstepCount, threadDispatcher); simulation.Profiler.Start(simulation.Solver); simulation.Solver.SolveStep2(dt, threadDispatcher); diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index 78f43f348..459b3b89e 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -82,6 +82,10 @@ public partial class Solver /// public int FallbackBatchThreshold { get; private set; } + /// + /// Set of body handles associated with constrained kinematic bodies. These will be integrated during substepping. + /// + public QuickSet> ConstrainedKinematicHandles; int iterationCount; /// @@ -204,6 +208,7 @@ public int CountConstraints() public Solver(Bodies bodies, BufferPool pool, int iterationCount, int fallbackBatchThreshold, int initialCapacity, int initialIslandCapacity, + int initialConstrainedKinematicCapacity, int minimumCapacityPerTypeBatch) { this.iterationCount = iterationCount; @@ -216,8 +221,9 @@ public Solver(Bodies bodies, BufferPool pool, int iterationCount, int fallbackBa ActiveSet = new ConstraintSet(pool, fallbackBatchThreshold + 1); batchReferencedHandles = new QuickList(fallbackBatchThreshold + 1, pool); ResizeHandleCapacity(initialCapacity); - solveWorker = SolveWorker; ; + solveWorker = SolveWorker; incrementalContactUpdateWorker = IncrementalContactUpdateWorker; + ConstrainedKinematicHandles = new QuickSet>(initialConstrainedKinematicCapacity, pool); } public void Register() where TDescription : unmanaged, IConstraintDescription @@ -1377,6 +1383,7 @@ public void Clear() { batchReferencedHandles[batchIndex].Dispose(pool); } + ConstrainedKinematicHandles.Clear(); batchReferencedHandles.Clear(); ActiveSet.Clear(pool); //All inactive sets are returned to the pool. @@ -1396,7 +1403,8 @@ public void Clear() /// /// Size of the span of body handles to allocate space for. Applies to batch referenced handle sets. /// Number of constraint handles to allocate space for. Applies to the handle->constraint mapping table. - public void EnsureSolverCapacities(int bodyHandleCapacity, int constraintHandleCapacity) + /// Number of constrained kinematic body handles to allocate space for. + public void EnsureSolverCapacities(int bodyHandleCapacity, int constraintHandleCapacity, int constrainedKinematicCapacity) { if (HandleToConstraint.Length < constraintHandleCapacity) { @@ -1408,6 +1416,8 @@ public void EnsureSolverCapacities(int bodyHandleCapacity, int constraintHandleC { batchReferencedHandles[i].EnsureCapacity(targetBatchReferencedHandleSize, pool); } + + ConstrainedKinematicHandles.EnsureCapacity(constrainedKinematicCapacity, pool); } void ResizeHandleCapacity(int constraintHandleCapacity) @@ -1426,7 +1436,8 @@ void ResizeHandleCapacity(int constraintHandleCapacity) /// /// Size of the span of body handles to allocate space for. Applies to batch referenced handle sets. /// Number of constraint handles to allocate space for. Applies to the handle->constraint mapping table. - public void ResizeSolverCapacities(int bodyHandleCapacity, int constraintHandleCapacity) + /// Number of constrained kinematic body handles to allocate space for. + public void ResizeSolverCapacities(int bodyHandleCapacity, int constraintHandleCapacity, int constrainedKinematicCapacity) { var targetConstraintCount = BufferPool.GetCapacityForCount(Math.Max(constraintHandleCapacity, HandlePool.HighestPossiblyClaimedId + 1)); if (HandleToConstraint.Length != targetConstraintCount) @@ -1439,6 +1450,9 @@ public void ResizeSolverCapacities(int bodyHandleCapacity, int constraintHandleC { batchReferencedHandles[i].Resize(targetBatchReferencedHandleSize, pool); } + + var targetConstrainedKinematicsCapacity = Math.Max(ConstrainedKinematicHandles.Count, constrainedKinematicCapacity); + ConstrainedKinematicHandles.Resize(targetConstrainedKinematicsCapacity, pool); } internal void ResizeSetsCapacity(int setsCapacity, int potentiallyAllocatedCount) @@ -1493,6 +1507,7 @@ public void Dispose() batchReferencedHandles[i].Dispose(pool); } batchReferencedHandles.Dispose(pool); + ConstrainedKinematicHandles.Dispose(pool); for (int i = 0; i < Sets.Length; ++i) { if (Sets[i].Allocated) diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index 0a9433084..998126f23 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -19,6 +19,7 @@ namespace BepuPhysics internal enum SolverStageType { IncrementalUpdate, + IntegrateConstrainedKinematics, WarmStart, Solve, } @@ -39,6 +40,12 @@ public SolverSyncStage(Buffer claims, int workBlockStartIndex, SolverStageT } } + internal struct IntegrationWorkBlock + { + public int StartBundleIndex; + public int EndBundleIndex; + } + [StructLayout(LayoutKind.Explicit)] internal struct SubstepMultithreadingContext { @@ -47,14 +54,16 @@ internal struct SubstepMultithreadingContext [FieldOffset(16)] public Buffer IncrementalUpdateBlocks; [FieldOffset(32)] - public Buffer ConstraintBlocks; + public Buffer KinematicIntegrationBlocks; [FieldOffset(48)] - public Buffer ConstraintBatchBoundaries; + public Buffer ConstraintBlocks; [FieldOffset(64)] + public Buffer ConstraintBatchBoundaries; + [FieldOffset(80)] public float Dt; - [FieldOffset(68)] + [FieldOffset(84)] public float InverseDt; - [FieldOffset(72)] + [FieldOffset(88)] public int WorkerCount; @@ -94,8 +103,9 @@ public class Solver : Solver where TIntegrationCallbacks public Solver(Bodies bodies, BufferPool pool, int iterationCount, int fallbackBatchThreshold, int initialCapacity, int initialIslandCapacity, + int initialConstrainedKinematicCapacity, int minimumCapacityPerTypeBatch, PoseIntegrator poseIntegrator) - : base(bodies, pool, iterationCount, fallbackBatchThreshold, initialCapacity, initialIslandCapacity, minimumCapacityPerTypeBatch) + : base(bodies, pool, iterationCount, fallbackBatchThreshold, initialCapacity, initialIslandCapacity, initialConstrainedKinematicCapacity, minimumCapacityPerTypeBatch) { PoseIntegrator = poseIntegrator; solveStep2Worker2 = SolveStep2Worker2; @@ -192,6 +202,28 @@ public void Execute(Solver solver, int blockIndex, int workerIndex) } } + struct IntegrateConstrainedKinematicsStageFunction : IStageFunction + { + public float Dt; + public float InverseDt; + public int SubstepIndex; + public Solver solver; + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void Execute(Solver solver, int blockIndex, int workerIndex) + { + ref var block = ref this.solver.substepContext.KinematicIntegrationBlocks[blockIndex]; + if (SubstepIndex == 0) + { + this.solver.PoseIntegrator.IntegrateVelocities(solver.ConstrainedKinematicHandles.Span.Slice(solver.ConstrainedKinematicHandles.Count), block.StartBundleIndex, block.EndBundleIndex, Dt, workerIndex); + } + else + { + this.solver.PoseIntegrator.IntegratePosesAndVelocities(solver.ConstrainedKinematicHandles.Span.Slice(solver.ConstrainedKinematicHandles.Count), block.StartBundleIndex, block.EndBundleIndex, Dt, workerIndex); + } + } + } + void ExecuteWorkerStage(ref TStageFunction stageFunction, int workerIndex, int workerStart, int availableBlocksStartIndex, ref Buffer claims, int previousSyncIndexOffset, int syncIndex, ref int completedWorkBlocks) where TStageFunction : IStageFunction { if (workerStart == -1) @@ -305,6 +337,7 @@ void SolveStep2Worker2(int workerIndex) //data will remain in some cache that's reasonably close to the core. int workerCount = substepContext.WorkerCount; var incrementalUpdateWorkerStart = GetUniformlyDistributedStart(workerIndex, substepContext.IncrementalUpdateBlocks.Length, workerCount, 0); + var kinematicIntegrationWorkerStart = GetUniformlyDistributedStart(workerIndex, substepContext.KinematicIntegrationBlocks.Length, workerCount, 0); Buffer batchStarts; ref var activeSet = ref ActiveSet; unsafe @@ -329,6 +362,12 @@ void SolveStep2Worker2(int workerIndex) InverseDt = substepContext.InverseDt, solver = this }; + var integrateConstrainedKinematicsStage = new IntegrateConstrainedKinematicsStageFunction + { + Dt = substepContext.Dt, + InverseDt = substepContext.InverseDt, + solver = this + }; var warmstartStage = new WarmStartStep2StageFunction { Dt = substepContext.Dt, @@ -347,9 +386,9 @@ void SolveStep2Worker2(int workerIndex) var syncStagesPerWarmStartOrSolve = synchronizedBatchCount; var baseStageCountInSubstep = syncStagesPerWarmStartOrSolve * (1 + IterationCount); //All warmstarts and solves, plus an incremental contact update. First substep doesn't do an incremental contact update, but that's fine, it'll end up expecting 0. - var syncOffsetToPreviousSubstep = baseStageCountInSubstep + 1; - //To find the previous execution sync index of a constraint batch, we have to scan through all the constraint batches, but ALSO skip over the incremental contact update stage, hence + 1. - var syncOffsetToPreviousClaimOnBatchForWarmStart = syncStagesPerWarmStartOrSolve + 1; + var syncOffsetToPreviousSubstep = baseStageCountInSubstep + 2; + //To find the previous execution sync index of a constraint batch, we have to scan through all the constraint batches, but ALSO skip over the incremental contact update stage and kinematic integration, hence + 2. + var syncOffsetToPreviousClaimOnBatchForWarmStart = syncStagesPerWarmStartOrSolve + 2; //For solves, there is no incremental update in the way. var syncOffsetToPreviousClaimOnBatchForSolve = syncStagesPerWarmStartOrSolve; if (workerIndex == 0) @@ -362,10 +401,12 @@ void SolveStep2Worker2(int workerIndex) { ExecuteMainStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStart, ref substepContext.Stages[0], syncOffsetToPreviousSubstep, ref syncIndex); } + integrateConstrainedKinematicsStage.SubstepIndex = substepIndex; + ExecuteMainStage(ref integrateConstrainedKinematicsStage, workerIndex, kinematicIntegrationWorkerStart, ref substepContext.Stages[1], syncOffsetToPreviousSubstep, ref syncIndex); warmstartStage.SubstepIndex = substepIndex; for (int batchIndex = 0; batchIndex < syncStagesPerWarmStartOrSolve; ++batchIndex) { - ExecuteMainStage(ref warmstartStage, workerIndex, batchStarts[batchIndex], ref substepContext.Stages[batchIndex + 1], syncOffsetToPreviousClaimOnBatchForWarmStart, ref syncIndex); + ExecuteMainStage(ref warmstartStage, workerIndex, batchStarts[batchIndex], ref substepContext.Stages[batchIndex + 2], syncOffsetToPreviousClaimOnBatchForWarmStart, ref syncIndex); } if (fallbackExists) { @@ -391,7 +432,7 @@ void SolveStep2Worker2(int workerIndex) { //Note that this is using a 'different' stage by index than the worker thread if the iteration index > 1. //That's totally fine- the warmstart/iteration stages share the same claims buffers per batch. They're redundant for the sake of easier indexing. - ExecuteMainStage(ref solveStage, workerIndex, batchStarts[batchIndex], ref substepContext.Stages[batchIndex + 1], syncOffsetToPreviousClaimOnBatchForSolve, ref syncIndex); + ExecuteMainStage(ref solveStage, workerIndex, batchStarts[batchIndex], ref substepContext.Stages[batchIndex + 2], syncOffsetToPreviousClaimOnBatchForSolve, ref syncIndex); } if (fallbackExists) { @@ -456,6 +497,10 @@ void SolveStep2Worker2(int workerIndex) case SolverStageType.IncrementalUpdate: ExecuteWorkerStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStart, 0, ref stage.Claims, syncOffsetToPreviousSubstep, syncIndex, ref substepContext.CompletedWorkBlockCount); break; + case SolverStageType.IntegrateConstrainedKinematics: + integrateConstrainedKinematicsStage.SubstepIndex = substepIndex; + ExecuteWorkerStage(ref integrateConstrainedKinematicsStage, workerIndex, kinematicIntegrationWorkerStart, 0, ref stage.Claims, syncOffsetToPreviousSubstep, syncIndex, ref substepContext.CompletedWorkBlockCount); + break; case SolverStageType.WarmStart: warmstartStage.SubstepIndex = substepIndex; ExecuteWorkerStage(ref warmstartStage, workerIndex, batchStarts[stage.BatchIndex], stage.WorkBlockStartIndex, ref stage.Claims, syncOffsetToPreviousClaimOnBatchForWarmStart, syncIndex, ref substepContext.CompletedWorkBlockCount); @@ -471,6 +516,33 @@ void SolveStep2Worker2(int workerIndex) } + Buffer BuildIntegrationWorkBlocks(int minimumBlockSizeInBundles, int maximumBlockSizeInBundles, int targetBlockCount) + { + var bundleCount = BundleIndexing.GetBundleCount(ConstrainedKinematicHandles.Count); + var targetBundlesPerBlock = bundleCount / targetBlockCount; + if (targetBundlesPerBlock < minimumBlockSizeInBundles) + targetBundlesPerBlock = minimumBlockSizeInBundles; + if (targetBundlesPerBlock > maximumBlockSizeInBundles) + targetBundlesPerBlock = maximumBlockSizeInBundles; + var blockCount = (bundleCount + targetBundlesPerBlock - 1) / targetBundlesPerBlock; + var bundlesPerBlock = bundleCount / blockCount; + var remainder = bundleCount - bundlesPerBlock * blockCount; + var previousEnd = 0; + if (blockCount > 0) + { + pool.Take(blockCount, out Buffer workBlocks); + for (int i = 0; i < blockCount; ++i) + { + var bundleCountForBlock = bundlesPerBlock; + if (i < remainder) + ++bundleCountForBlock; + workBlocks[i] = new IntegrationWorkBlock { StartBundleIndex = previousEnd, EndBundleIndex = previousEnd += bundleCountForBlock }; + } + return workBlocks; + + } + return default; + } //Buffer> debugStageWorkBlocksCompleted; protected void ExecuteMultithreaded2(float dt, IThreadDispatcher threadDispatcher, Action workDelegate) @@ -495,22 +567,24 @@ protected void ExecuteMultithreaded2(float dt, IThreadDispatcher threadDispatche pool.Return(ref incrementalUpdateBatchBoundaries); //TODO: No need to create this in the first place. Doesn't really cost anything, but... substepContext.ConstraintBlocks = constraintBlocks.Span.Slice(constraintBlocks.Count); substepContext.IncrementalUpdateBlocks = incrementalBlocks.Span.Slice(incrementalBlocks.Count); + substepContext.KinematicIntegrationBlocks = BuildIntegrationWorkBlocks(minimumBlockSizeInBundles, maximumBlockSizeInBundles, targetBlocksPerBatch); //Not every batch will actually have work blocks associated with it; the batch compressor could be falling behind, which means older constraints could be at higher batches than they need to be, leaving gaps. //We don't want to include those empty batches as sync points in the solver. var batchCount = ActiveSet.Batches.Count; substepContext.SyncIndex = 0; var totalConstraintBatchWorkBlockCount = substepContext.ConstraintBatchBoundaries.Length == 0 ? 0 : substepContext.ConstraintBatchBoundaries[^1]; - var totalClaimCount = incrementalBlocks.Count + totalConstraintBatchWorkBlockCount; + var totalClaimCount = incrementalBlocks.Count + substepContext.KinematicIntegrationBlocks.Length + totalConstraintBatchWorkBlockCount; GetSynchronizedBatchCount(out var stagesPerIteration, out var fallbackExists); - pool.Take(1 + stagesPerIteration * (1 + IterationCount), out substepContext.Stages); + pool.Take(2 + stagesPerIteration * (1 + IterationCount), out substepContext.Stages); //Claims will be monotonically increasing throughout execution. All should start at zero to match with the initial sync index. pool.Take(totalClaimCount, out var claims); claims.Clear(0, claims.Length); substepContext.Stages[0] = new(claims.Slice(incrementalBlocks.Count), 0, SolverStageType.IncrementalUpdate); + substepContext.Stages[1] = new(claims.Slice(substepContext.KinematicIntegrationBlocks.Length), 0, SolverStageType.IntegrateConstrainedKinematics); //Note that we create redundant stages that share the same workblock targets and claims buffers. //This is just to make indexing a little simpler during the multithreaded work. - int targetStageIndex = 1; + int targetStageIndex = 2; //Warm start. int claimStart = incrementalBlocks.Count; for (int batchIndex = 0; batchIndex < stagesPerIteration; ++batchIndex) @@ -628,6 +702,8 @@ protected void ExecuteMultithreaded2(float dt, IThreadDispatcher threadDispatche pool.Return(ref substepContext.Stages); pool.Return(ref substepContext.ConstraintBatchBoundaries); pool.Return(ref substepContext.IncrementalUpdateBlocks); + if (substepContext.KinematicIntegrationBlocks.Allocated) + pool.Return(ref substepContext.KinematicIntegrationBlocks); pool.Return(ref substepContext.ConstraintBlocks); @@ -1101,6 +1177,11 @@ public override void SolveStep2(float totalDt, IThreadDispatcher threadDispatche TypeProcessors[typeBatch.TypeId].IncrementallyUpdateContactData(ref typeBatch, bodies, substepDt, inverseDt, 0, typeBatch.BundleCount); } } + PoseIntegrator.IntegratePosesAndVelocities(ConstrainedKinematicHandles.Span.Slice(ConstrainedKinematicHandles.Count), 0, ConstrainedKinematicHandles.Count, substepDt, 0); + } + else + { + PoseIntegrator.IntegrateVelocities(ConstrainedKinematicHandles.Span.Slice(ConstrainedKinematicHandles.Count), 0, ConstrainedKinematicHandles, substepDt, 0); } for (int i = 0; i < batchCount; ++i) { @@ -1135,7 +1216,6 @@ public override void SolveStep2(float totalDt, IThreadDispatcher threadDispatche } else { - //ExecuteMultithreaded(substepDt, threadDispatcher, solveStep2Worker, includeIncrementalUpdate: true); ExecuteMultithreaded2(substepDt, threadDispatcher, solveStep2Worker2); } } From 588d71e5972a92a03f326d8f49621ff5f6802e4e Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 26 Oct 2021 20:04:24 -0500 Subject: [PATCH 234/947] Still doesn't build. Added more validation. --- BepuPhysics/EmbeddedSubsteppingTimestepper2.cs | 11 ++++++----- BepuPhysics/Solver.cs | 18 ++++++++++++++++++ 2 files changed, 24 insertions(+), 5 deletions(-) diff --git a/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs b/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs index 9473bda7a..f08b3c548 100644 --- a/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs +++ b/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs @@ -56,12 +56,13 @@ public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDi CollisionsDetected?.Invoke(dt, threadDispatcher); Debug.Assert(SubstepCount >= 0, "Substep count should be positive."); - simulation.Solver.ValidateTrailingTypeBatchBodyReferences(); - simulation.Solver.ValidateFallbackBatchEmptySlotReferences(); - simulation.Solver.ValidateFallbackBatchAccessSafety(); - simulation.Solver.ValidateFallbackBatchAccumulatedImpulses(); - simulation.Solver.ValidateConstraintMaps(); + //simulation.Solver.ValidateTrailingTypeBatchBodyReferences(); + //simulation.Solver.ValidateFallbackBatchEmptySlotReferences(); + //simulation.Solver.ValidateFallbackBatchAccessSafety(); + //simulation.Solver.ValidateFallbackBatchAccumulatedImpulses(); + //simulation.Solver.ValidateConstraintMaps(); simulation.Solver.ValidateConstraintReferenceKinematicity(); + simulation.Solver.ValidateConstrainedKinematicsSet(); var constrainedBodySet = simulation.Solver.PrepareConstraintIntegrationResponsibilities(SubstepCount, threadDispatcher); simulation.Profiler.Start(simulation.Solver); simulation.Solver.SolveStep2(dt, threadDispatcher); diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index 459b3b89e..67c7d10bb 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -312,6 +312,24 @@ unsafe internal void ValidateConstraintReferenceKinematicity() } } + [Conditional("DEBUG")] + unsafe internal void ValidateConstrainedKinematicsSet() + { + ref var set = ref bodies.ActiveSet; + for (int i = 0; i < set.Count; ++i) + { + if (Bodies.IsKinematicUnsafe(ref set.SolverStates[i].Inertia.Local) && set.Constraints[i].Count > 0) + { + Debug.Assert(ConstrainedKinematicHandles.Contains(set.IndexToHandle[i].Value), "Any active kinematic with constraints must appear in the constrained kinematic set."); + } + } + for (int i = 0; i < ConstrainedKinematicHandles.Count; ++i) + { + var bodyReference = bodies.GetBodyReference(new BodyHandle(ConstrainedKinematicHandles[i])); + Debug.Assert(bodyReference.Kinematic && bodyReference.Constraints.Count > 0, "Any body listed in the constrained kinematics set must be kinematic and constrained."); + } + } + [Conditional("DEBUG")] private void ValidateBodyReference(int body, int expectedCount, ref ConstraintBatch batch) { From 4051e5fca03926e7026d978ba6342609dd1748c0 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 26 Oct 2021 20:27:30 -0500 Subject: [PATCH 235/947] Does not build. Fallback batch now avoids storing kinematics in its constraint count lists. AllocateInBatch pushes kinematics into the constrained handles set. --- .../EmbeddedSubsteppingTimestepper2.cs | 10 +- BepuPhysics/IslandAwakener.cs | 8 +- BepuPhysics/SequentialFallbackBatch.cs | 106 +++++++++--------- BepuPhysics/Solver.cs | 18 ++- 4 files changed, 75 insertions(+), 67 deletions(-) diff --git a/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs b/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs index f08b3c548..711f9dbca 100644 --- a/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs +++ b/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs @@ -56,11 +56,11 @@ public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDi CollisionsDetected?.Invoke(dt, threadDispatcher); Debug.Assert(SubstepCount >= 0, "Substep count should be positive."); - //simulation.Solver.ValidateTrailingTypeBatchBodyReferences(); - //simulation.Solver.ValidateFallbackBatchEmptySlotReferences(); - //simulation.Solver.ValidateFallbackBatchAccessSafety(); - //simulation.Solver.ValidateFallbackBatchAccumulatedImpulses(); - //simulation.Solver.ValidateConstraintMaps(); + simulation.Solver.ValidateTrailingTypeBatchBodyReferences(); + simulation.Solver.ValidateFallbackBatchEmptySlotReferences(); + simulation.Solver.ValidateFallbackBatchAccessSafety(); + simulation.Solver.ValidateFallbackBatchAccumulatedImpulses(); + simulation.Solver.ValidateConstraintMaps(); simulation.Solver.ValidateConstraintReferenceKinematicity(); simulation.Solver.ValidateConstrainedKinematicsSet(); var constrainedBodySet = simulation.Solver.PrepareConstraintIntegrationResponsibilities(SubstepCount, threadDispatcher); diff --git a/BepuPhysics/IslandAwakener.cs b/BepuPhysics/IslandAwakener.cs index 0c890c9c4..b2677401a 100644 --- a/BepuPhysics/IslandAwakener.cs +++ b/BepuPhysics/IslandAwakener.cs @@ -272,15 +272,15 @@ internal unsafe void ExecutePhaseOneJob(int index) Debug.Assert(uniqueSetIndices[i] > 0); ref var source = ref solver.Sets[uniqueSetIndices[i]].SequentialFallback; ref var target = ref solver.ActiveSet.SequentialFallback; - if (source.bodyConstraintCounts.Count > 0) + if (source.dynamicBodyConstraintCounts.Count > 0) { - for (int j = 0; j < source.bodyConstraintCounts.Count; ++j) + for (int j = 0; j < source.dynamicBodyConstraintCounts.Count; ++j) { //Inactive sets refer to body handles. Active set refers to body indices. Make the transition. //The HandleToLocation was updated during job setup, so we can use it. - ref var bodyLocation = ref bodies.HandleToLocation[source.bodyConstraintCounts.Keys[j]]; + ref var bodyLocation = ref bodies.HandleToLocation[source.dynamicBodyConstraintCounts.Keys[j]]; Debug.Assert(bodyLocation.SetIndex == 0, "Any batch moved into the active set should be dealing with bodies which have already been moved into the active set."); - var added = target.bodyConstraintCounts.AddUnsafely(bodyLocation.Index, source.bodyConstraintCounts.Values[j]); + var added = target.dynamicBodyConstraintCounts.AddUnsafely(bodyLocation.Index, source.dynamicBodyConstraintCounts.Values[j]); Debug.Assert(added, "Any body moving from an inactive set to the active set should not already be present in the active set's fallback batch."); } } diff --git a/BepuPhysics/SequentialFallbackBatch.cs b/BepuPhysics/SequentialFallbackBatch.cs index a7f7ebb53..af5fb7b80 100644 --- a/BepuPhysics/SequentialFallbackBatch.cs +++ b/BepuPhysics/SequentialFallbackBatch.cs @@ -44,31 +44,31 @@ public struct SequentialFallbackBatch /// /// Gets the number of bodies in the fallback batch. /// - public readonly int BodyCount { get { return bodyConstraintCounts.Count; } } + public readonly int BodyCount { get { return dynamicBodyConstraintCounts.Count; } } //In order to maintain the batch referenced handles for the fallback batch (which can have the same body appear more than once), //every body must maintain a count of fallback constraints associated with it. //Note that this dictionary uses active set body *indices* while active, but body *handles* when associated with an inactive set. //This is consistent with the body references stored by active/inactive constraints. - internal QuickDictionary> bodyConstraintCounts; + internal QuickDictionary> dynamicBodyConstraintCounts; [MethodImpl(MethodImplOptions.AggressiveInlining)] - unsafe void Allocate(Span constraintBodyHandles, Bodies bodies, + unsafe void Allocate(Span dynamicBodyHandles, Bodies bodies, BufferPool pool, TBodyReferenceGetter bodyReferenceGetter, int minimumBodyCapacity) where TBodyReferenceGetter : struct, IBodyReferenceGetter { - EnsureCapacity(Math.Max(bodyConstraintCounts.Count + constraintBodyHandles.Length, minimumBodyCapacity), pool); - for (int i = 0; i < constraintBodyHandles.Length; ++i) + EnsureCapacity(Math.Max(dynamicBodyConstraintCounts.Count + dynamicBodyHandles.Length, minimumBodyCapacity), pool); + for (int i = 0; i < dynamicBodyHandles.Length; ++i) { - var bodyReference = bodyReferenceGetter.GetBodyReference(bodies, constraintBodyHandles[i]); + var bodyReference = bodyReferenceGetter.GetBodyReference(bodies, dynamicBodyHandles[i]); - if (bodyConstraintCounts.FindOrAllocateSlotUnsafely(bodyReference, out var slotIndex)) + if (dynamicBodyConstraintCounts.FindOrAllocateSlotUnsafely(bodyReference, out var slotIndex)) { - ++bodyConstraintCounts.Values[slotIndex]; + ++dynamicBodyConstraintCounts.Values[slotIndex]; } else { - bodyConstraintCounts.Values[slotIndex] = 1; + dynamicBodyConstraintCounts.Values[slotIndex] = 1; } } } @@ -76,17 +76,17 @@ unsafe void Allocate(Span constraintBodyHandle [MethodImpl(MethodImplOptions.AggressiveInlining)] - internal unsafe void AllocateForActive(Span constraintBodyHandles, Bodies bodies, + internal unsafe void AllocateForActive(Span dynamicBodyHandles, Bodies bodies, BufferPool pool, int minimumBodyCapacity = 8) { - Allocate(constraintBodyHandles, bodies, pool, new ActiveSetGetter(), minimumBodyCapacity); + Allocate(dynamicBodyHandles, bodies, pool, new ActiveSetGetter(), minimumBodyCapacity); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - internal void AllocateForInactive(Span constraintBodyHandles, Bodies bodies, + internal void AllocateForInactive(Span dynamicBodyHandles, Bodies bodies, BufferPool pool, int minimumBodyCapacity = 8) { - Allocate(constraintBodyHandles, bodies, pool, new InactiveSetGetter(), minimumBodyCapacity); + Allocate(dynamicBodyHandles, bodies, pool, new InactiveSetGetter(), minimumBodyCapacity); } @@ -98,22 +98,22 @@ internal void AllocateForInactive(Span constraintBodyHandles, Bodies /// True if the body no longer has any constraints associated with it in the fallback batch, false otherwise. internal unsafe bool Remove(int bodyReference, ref QuickList allocationIdsToFree) { - var bodyPresent = bodyConstraintCounts.GetTableIndices(ref bodyReference, out var tableIndex, out var bodyReferencesIndex); + var bodyPresent = dynamicBodyConstraintCounts.GetTableIndices(ref bodyReference, out var tableIndex, out var bodyReferencesIndex); Debug.Assert(bodyPresent, "If we've been asked to remove a constraint associated with a body, that body must be in this batch."); - ref var constraintCount = ref bodyConstraintCounts.Values[bodyReferencesIndex]; + ref var constraintCount = ref dynamicBodyConstraintCounts.Values[bodyReferencesIndex]; --constraintCount; if (constraintCount == 0) { //If there are no more constraints associated with this body, get rid of the body list. constraintCount = default; - bodyConstraintCounts.FastRemove(tableIndex, bodyReferencesIndex); - if (bodyConstraintCounts.Count == 0) + dynamicBodyConstraintCounts.FastRemove(tableIndex, bodyReferencesIndex); + if (dynamicBodyConstraintCounts.Count == 0) { //No constraints remain in the fallback batch. Drop the dictionary. - allocationIdsToFree.AllocateUnsafely() = bodyConstraintCounts.Keys.Id; - allocationIdsToFree.AllocateUnsafely() = bodyConstraintCounts.Values.Id; - allocationIdsToFree.AllocateUnsafely() = bodyConstraintCounts.Table.Id; - bodyConstraintCounts = default; + allocationIdsToFree.AllocateUnsafely() = dynamicBodyConstraintCounts.Keys.Id; + allocationIdsToFree.AllocateUnsafely() = dynamicBodyConstraintCounts.Values.Id; + allocationIdsToFree.AllocateUnsafely() = dynamicBodyConstraintCounts.Table.Id; + dynamicBodyConstraintCounts = default; } return true; } @@ -128,18 +128,18 @@ internal unsafe bool Remove(int bodyReference, ref QuickList allocationIdsT /// True if the body was present in the fallback batch and was removed, false otherwise. internal unsafe bool TryRemove(int bodyReference, ref QuickList allocationIdsToFree) { - if (bodyConstraintCounts.Keys.Allocated && bodyConstraintCounts.GetTableIndices(ref bodyReference, out var tableIndex, out var bodyReferencesIndex)) + if (dynamicBodyConstraintCounts.Keys.Allocated && dynamicBodyConstraintCounts.GetTableIndices(ref bodyReference, out var tableIndex, out var bodyReferencesIndex)) { - ref var constraintReferences = ref bodyConstraintCounts.Values[bodyReferencesIndex]; + ref var constraintReferences = ref dynamicBodyConstraintCounts.Values[bodyReferencesIndex]; //If there are no more constraints associated with this body, get rid of the body list. - bodyConstraintCounts.FastRemove(tableIndex, bodyReferencesIndex); - if (bodyConstraintCounts.Count == 0) + dynamicBodyConstraintCounts.FastRemove(tableIndex, bodyReferencesIndex); + if (dynamicBodyConstraintCounts.Count == 0) { //No constraints remain in the fallback batch. Drop the dictionary. - allocationIdsToFree.AllocateUnsafely() = bodyConstraintCounts.Keys.Id; - allocationIdsToFree.AllocateUnsafely() = bodyConstraintCounts.Values.Id; - allocationIdsToFree.AllocateUnsafely() = bodyConstraintCounts.Table.Id; - bodyConstraintCounts = default; + allocationIdsToFree.AllocateUnsafely() = dynamicBodyConstraintCounts.Keys.Id; + allocationIdsToFree.AllocateUnsafely() = dynamicBodyConstraintCounts.Values.Id; + allocationIdsToFree.AllocateUnsafely() = dynamicBodyConstraintCounts.Table.Id; + dynamicBodyConstraintCounts = default; } return true; } @@ -190,8 +190,8 @@ public static unsafe void ValidateSetReferences(Solver solver, int setIndex) Debug.Assert(set.Allocated); if (set.Batches.Count > solver.FallbackBatchThreshold) { - Debug.Assert(set.SequentialFallback.bodyConstraintCounts.Keys.Allocated); - ref var bodyConstraintCounts = ref set.SequentialFallback.bodyConstraintCounts; + Debug.Assert(set.SequentialFallback.dynamicBodyConstraintCounts.Keys.Allocated); + ref var bodyConstraintCounts = ref set.SequentialFallback.dynamicBodyConstraintCounts; for (int i = 0; i < bodyConstraintCounts.Count; ++i) { //This is a handle on inactive sets, and an index for active sets. @@ -231,61 +231,61 @@ public static unsafe void ValidateReferences(Solver solver) } internal void UpdateForBodyMemoryMove(int originalBodyIndex, int newBodyLocation) { - Debug.Assert(bodyConstraintCounts.Keys.Allocated && !bodyConstraintCounts.ContainsKey(newBodyLocation), "If a body is being moved, as opposed to swapped, then the target index should not be present."); - bodyConstraintCounts.GetTableIndices(ref originalBodyIndex, out var tableIndex, out var elementIndex); - var references = bodyConstraintCounts.Values[elementIndex]; - bodyConstraintCounts.FastRemove(tableIndex, elementIndex); - bodyConstraintCounts.AddUnsafely(ref newBodyLocation, references); + Debug.Assert(dynamicBodyConstraintCounts.Keys.Allocated && !dynamicBodyConstraintCounts.ContainsKey(newBodyLocation), "If a body is being moved, as opposed to swapped, then the target index should not be present."); + dynamicBodyConstraintCounts.GetTableIndices(ref originalBodyIndex, out var tableIndex, out var elementIndex); + var references = dynamicBodyConstraintCounts.Values[elementIndex]; + dynamicBodyConstraintCounts.FastRemove(tableIndex, elementIndex); + dynamicBodyConstraintCounts.AddUnsafely(ref newBodyLocation, references); } internal void UpdateForBodyMemorySwap(int a, int b) { - var indexA = bodyConstraintCounts.IndexOf(a); - var indexB = bodyConstraintCounts.IndexOf(b); + var indexA = dynamicBodyConstraintCounts.IndexOf(a); + var indexB = dynamicBodyConstraintCounts.IndexOf(b); Debug.Assert(indexA >= 0 && indexB >= 0, "A swap requires that both indices are already present."); - Helpers.Swap(ref bodyConstraintCounts.Values[indexA], ref bodyConstraintCounts.Values[indexB]); + Helpers.Swap(ref dynamicBodyConstraintCounts.Values[indexA], ref dynamicBodyConstraintCounts.Values[indexB]); } internal static void CreateFrom(ref SequentialFallbackBatch sourceBatch, BufferPool pool, out SequentialFallbackBatch targetBatch) { //Copy over non-buffer state. This copies buffer references pointlessly, but that doesn't matter. - targetBatch.bodyConstraintCounts = sourceBatch.bodyConstraintCounts; - pool.TakeAtLeast(sourceBatch.bodyConstraintCounts.Count, out targetBatch.bodyConstraintCounts.Keys); - pool.TakeAtLeast(targetBatch.bodyConstraintCounts.Keys.Length, out targetBatch.bodyConstraintCounts.Values); - pool.TakeAtLeast(sourceBatch.bodyConstraintCounts.TableMask + 1, out targetBatch.bodyConstraintCounts.Table); - sourceBatch.bodyConstraintCounts.Keys.CopyTo(0, targetBatch.bodyConstraintCounts.Keys, 0, sourceBatch.bodyConstraintCounts.Count); - sourceBatch.bodyConstraintCounts.Values.CopyTo(0, targetBatch.bodyConstraintCounts.Values, 0, sourceBatch.bodyConstraintCounts.Count); - sourceBatch.bodyConstraintCounts.Table.CopyTo(0, targetBatch.bodyConstraintCounts.Table, 0, sourceBatch.bodyConstraintCounts.TableMask + 1); + targetBatch.dynamicBodyConstraintCounts = sourceBatch.dynamicBodyConstraintCounts; + pool.TakeAtLeast(sourceBatch.dynamicBodyConstraintCounts.Count, out targetBatch.dynamicBodyConstraintCounts.Keys); + pool.TakeAtLeast(targetBatch.dynamicBodyConstraintCounts.Keys.Length, out targetBatch.dynamicBodyConstraintCounts.Values); + pool.TakeAtLeast(sourceBatch.dynamicBodyConstraintCounts.TableMask + 1, out targetBatch.dynamicBodyConstraintCounts.Table); + sourceBatch.dynamicBodyConstraintCounts.Keys.CopyTo(0, targetBatch.dynamicBodyConstraintCounts.Keys, 0, sourceBatch.dynamicBodyConstraintCounts.Count); + sourceBatch.dynamicBodyConstraintCounts.Values.CopyTo(0, targetBatch.dynamicBodyConstraintCounts.Values, 0, sourceBatch.dynamicBodyConstraintCounts.Count); + sourceBatch.dynamicBodyConstraintCounts.Table.CopyTo(0, targetBatch.dynamicBodyConstraintCounts.Table, 0, sourceBatch.dynamicBodyConstraintCounts.TableMask + 1); } internal void EnsureCapacity(int bodyCapacity, BufferPool pool) { - if (bodyConstraintCounts.Keys.Allocated) + if (dynamicBodyConstraintCounts.Keys.Allocated) { //This is conservative since there's no guarantee that we'll actually need to resize at all if these bodies are already present, but that's fine. - bodyConstraintCounts.EnsureCapacity(bodyCapacity, pool); + dynamicBodyConstraintCounts.EnsureCapacity(bodyCapacity, pool); } else { - bodyConstraintCounts = new QuickDictionary>(bodyCapacity, pool); + dynamicBodyConstraintCounts = new QuickDictionary>(bodyCapacity, pool); } } public void Compact(BufferPool pool) { - if (bodyConstraintCounts.Keys.Allocated) + if (dynamicBodyConstraintCounts.Keys.Allocated) { - bodyConstraintCounts.Compact(pool); + dynamicBodyConstraintCounts.Compact(pool); } } public void Dispose(BufferPool pool) { - if (bodyConstraintCounts.Keys.Allocated) + if (dynamicBodyConstraintCounts.Keys.Allocated) { - bodyConstraintCounts.Dispose(pool); + dynamicBodyConstraintCounts.Dispose(pool); } } } diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index 67c7d10bb..c3df77215 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -594,7 +594,7 @@ internal unsafe void ValidateExistingHandles(bool activeOnly = false) else { //If this is the fallback batch, then the expected count may be more than 1. - var foundBody = ActiveSet.SequentialFallback.bodyConstraintCounts.TryGetValue(i, out var constraintCountInFallbackBatchForBody); + var foundBody = ActiveSet.SequentialFallback.dynamicBodyConstraintCounts.TryGetValue(i, out var constraintCountInFallbackBatchForBody); Debug.Assert(foundBody, "A body was in the fallback batch's referenced handles, so the fallback batch should have a reference for that body."); expectedCount = foundBody ? constraintCountInFallbackBatchForBody : 0; } @@ -831,6 +831,8 @@ internal unsafe void AllocateInBatch(int targetBatchIndex, ConstraintHandle cons //Add all the constraint's body handles to the batch we found (or created) to block future references to the same bodies. //Also, convert the handle into a memory index. Constraints store a direct memory reference for performance reasons. var bodyIndices = stackalloc int[bodyHandles.Length]; + Span dynamicBodyHandles = stackalloc BodyHandle[bodyHandles.Length]; + int dynamicBodyCount = 0; for (int j = 0; j < bodyHandles.Length; ++j) { var bodyHandle = bodyHandles[j]; @@ -841,9 +843,15 @@ internal unsafe void AllocateInBatch(int targetBatchIndex, ConstraintHandle cons if (Bodies.IsKinematic(bodies.ActiveSet.SolverStates[index].Inertia.Local)) { index |= 1 << Bodies.KinematicFlagIndex; + ConstrainedKinematicHandles.Add(bodyHandle.Value, pool); + } + else + { + dynamicBodyHandles[dynamicBodyCount++] = bodyHandles[j]; } bodyIndices[j] = index; } + dynamicBodyHandles = dynamicBodyHandles.Slice(0, dynamicBodyCount); var typeProcessor = TypeProcessors[typeId]; var typeBatch = batch.GetOrCreateTypeBatch(typeId, typeProcessor, GetMinimumCapacityForType(typeId), pool); int indexInTypeBatch; @@ -862,14 +870,14 @@ internal unsafe void AllocateInBatch(int targetBatchIndex, ConstraintHandle cons //and it is rare that a new type batch will be created that actually needs to be enormous.) ref var handlesSet = ref batchReferencedHandles[targetBatchIndex]; - for (int i = 0; i < bodyHandles.Length; ++i) + for (int i = 0; i < dynamicBodyHandles.Length; ++i) { - Debug.Assert(targetBatchIndex == FallbackBatchThreshold || !handlesSet.Contains(bodyHandles[i].Value), "Non-fallback batches should not come to include references to the same body more than once."); - handlesSet.Set(bodyHandles[i].Value, pool); + Debug.Assert(targetBatchIndex == FallbackBatchThreshold || !handlesSet.Contains(dynamicBodyHandles[i].Value), "Non-fallback batches should not come to include references to the same body more than once."); + handlesSet.Set(dynamicBodyHandles[i].Value, pool); } if (targetBatchIndex == FallbackBatchThreshold) { - ActiveSet.SequentialFallback.AllocateForActive(bodyHandles, bodies, pool); + ActiveSet.SequentialFallback.AllocateForActive(dynamicBodyHandles, bodies, pool); } } From 7471c1ef3a4bc5f2f8b54e7703c6e3489a222f4b Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 26 Oct 2021 20:45:53 -0500 Subject: [PATCH 236/947] Does not build. Dynamic only fallback removal. --- BepuPhysics/SequentialFallbackBatch.cs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/BepuPhysics/SequentialFallbackBatch.cs b/BepuPhysics/SequentialFallbackBatch.cs index af5fb7b80..24875324a 100644 --- a/BepuPhysics/SequentialFallbackBatch.cs +++ b/BepuPhysics/SequentialFallbackBatch.cs @@ -95,11 +95,11 @@ internal void AllocateForInactive(Span dynamicBodyHandles, Bodies bo /// /// Body associated with a constraint in the fallback batch. /// Allocations that should be freed once execution is back in a safe context. - /// True if the body no longer has any constraints associated with it in the fallback batch, false otherwise. + /// True if the body was dynamic and no longer has any constraints associated with it in the fallback batch, false otherwise. internal unsafe bool Remove(int bodyReference, ref QuickList allocationIdsToFree) { - var bodyPresent = dynamicBodyConstraintCounts.GetTableIndices(ref bodyReference, out var tableIndex, out var bodyReferencesIndex); - Debug.Assert(bodyPresent, "If we've been asked to remove a constraint associated with a body, that body must be in this batch."); + if (!dynamicBodyConstraintCounts.GetTableIndices(ref bodyReference, out var tableIndex, out var bodyReferencesIndex)) + return false; ref var constraintCount = ref dynamicBodyConstraintCounts.Values[bodyReferencesIndex]; --constraintCount; if (constraintCount == 0) From 2d2f8c6ff683f02215e534c12b20f92d4cd91990 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 27 Oct 2021 17:28:46 -0500 Subject: [PATCH 237/947] Does not buildl. Implemented poseintegrator side of kinematics integration prepass. --- BepuPhysics/Constraints/IBodyAccessFilter.cs | 13 +++ BepuPhysics/PoseIntegrator.cs | 95 ++++++++++++++++++++ BepuPhysics/Simulation.cs | 2 +- BepuPhysics/Solver_SubsteppingSolve2.cs | 14 +-- BepuUtilities/BundleIndexing.cs | 27 +++++- Demos/DemoCallbacks.cs | 11 ++- Demos/Demos/PlanetDemo.cs | 2 + Demos/Demos/SimpleSelfContainedDemo.cs | 8 ++ Demos/SpecializedTests/GyroscopeTestDemo.cs | 2 + 9 files changed, 166 insertions(+), 8 deletions(-) diff --git a/BepuPhysics/Constraints/IBodyAccessFilter.cs b/BepuPhysics/Constraints/IBodyAccessFilter.cs index d8c98b5e6..4a6abb168 100644 --- a/BepuPhysics/Constraints/IBodyAccessFilter.cs +++ b/BepuPhysics/Constraints/IBodyAccessFilter.cs @@ -45,6 +45,19 @@ public struct AccessAll : IBodyAccessFilter public bool AccessAngularVelocity => true; } + /// + /// Used for kinematic integration; the inertias are known ahead of time and there's no reason to gather them. + /// + public struct AccessNoInertia : IBodyAccessFilter + { + public bool GatherPosition => true; + public bool GatherOrientation => true; + public bool GatherMass => false; + public bool GatherInertiaTensor => false; + public bool AccessLinearVelocity => true; + public bool AccessAngularVelocity => true; + } + public struct AccessNoPose : IBodyAccessFilter { public bool GatherPosition => false; diff --git a/BepuPhysics/PoseIntegrator.cs b/BepuPhysics/PoseIntegrator.cs index f52f9b042..636a6bd23 100644 --- a/BepuPhysics/PoseIntegrator.cs +++ b/BepuPhysics/PoseIntegrator.cs @@ -58,6 +58,15 @@ public interface IPoseIntegratorCallbacks /// bool AllowSubstepsForUnconstrainedBodies { get; } + /// + /// Gets whether the velocity integration callback should be called for kinematic bodies. + /// If true, IntegrateVelocity will be called for bundles including kinematic bodies. + /// If false, kinematic bodies will just continue using whatever velocity they have set. + /// Most use cases should set this to false. + /// + bool IntegrateVelocityForKinematics { get; } + + /// /// Performs any required initialization logic after the Simulation instance has been constructed. /// @@ -745,6 +754,92 @@ public void IntegratePoses(float dt, BufferPool pool, IThreadDispatcher threadDi } } + + /// + /// Integrates the velocities of kinematic bodies as a prepass to the first substep during solving. + /// Kinematics have to be integrated ahead of time since they don't block constraint batch membership; the same kinematic could appear in the batch multiple times. + /// + internal unsafe void IntegrateKinematicVelocities(Buffer bodyHandles, int bundleStartIndex, int bundleEndIndex, float substepDt, int workerIndex) + { + var bodyCount = bodyHandles.Length; + var bundleCount = BundleIndexing.GetBundleCount(bodyCount); + var bundleDt = new Vector(substepDt); + var halfDt = bundleDt * new Vector(0.5f); + + int* bodyIndices = stackalloc int[Vector.Count]; + ref var callbacks = ref Callbacks; + var handleToLocation = bodies.HandleToLocation; + BodyInertiaWide zeroInertia = default; + + for (int bundleIndex = bundleStartIndex; bundleIndex < bundleEndIndex; ++bundleIndex) + { + var bundleBaseIndex = bundleIndex * Vector.Count; + var countInBundle = Math.Min(bodyCount - bundleBaseIndex, Vector.Count); + for (int i = 0; i < countInBundle; ++i) + { + bodyIndices[i] = handleToLocation[bodyHandles[i]].Index; + } + + var existingMask = BundleIndexing.CreateMaskForCountInBundle(countInBundle); + var trailingMask = Vector.OnesComplement(existingMask); + var bodyIndicesVector = new Vector(new Span(bodyIndices, Vector.Count)); + bodyIndicesVector = Vector.BitwiseOr(trailingMask, bodyIndicesVector); + //Slightly unfortunate sacrifice to API simplicity: + //We're doing a full gather so we can use the vectorized IntegrateVelocity callback even though the amount of work we're doing is absolutely trivial. + //With luck, the user sets the appropriate flag on the callbacks so this is never called in the first place. (Kinematics are generally not subject to user velocity integration!) + bodies.GatherState(bodyIndicesVector, false, out var position, out var orientation, out var velocity, out _); + callbacks.IntegrateVelocity(bodyIndicesVector, position, orientation, zeroInertia, existingMask, workerIndex, bundleDt, ref velocity); + //Writes to the empty lanes won't matter (scatter is masked), so we don't need to clean them up. + //Kinematic bodies have infinite inertia, so using the momentum conserving codepaths would hit a singularity. + bodies.ScatterVelocities(ref velocity, ref bodyIndicesVector); + + } + } + + /// + /// Integrates the poses *then* velocities of kinematic bodies as a prepass to the second or later substeps during solving. + /// Kinematics have to be integrated ahead of time since they don't block constraint batch membership; the same kinematic could appear in the batch multiple times. + /// + internal unsafe void IntegrateKinematicPosesAndVelocities(Buffer bodyHandles, int bundleStartIndex, int bundleEndIndex, float substepDt, int workerIndex) + { + var bodyCount = bodyHandles.Length; + var bundleCount = BundleIndexing.GetBundleCount(bodyCount); + var bundleDt = new Vector(substepDt); + var halfDt = bundleDt * new Vector(0.5f); + + int* bodyIndices = stackalloc int[Vector.Count]; + ref var callbacks = ref Callbacks; + var handleToLocation = bodies.HandleToLocation; + BodyInertiaWide zeroInertia = default; + + for (int bundleIndex = bundleStartIndex; bundleIndex < bundleEndIndex; ++bundleIndex) + { + var bundleBaseIndex = bundleIndex * Vector.Count; + var countInBundle = Math.Min(bodyCount - bundleBaseIndex, Vector.Count); + for (int i = 0; i < countInBundle; ++i) + { + bodyIndices[i] = handleToLocation[bodyHandles[i]].Index; + } + + var existingMask = BundleIndexing.CreateMaskForCountInBundle(countInBundle); + var trailingMask = Vector.OnesComplement(existingMask); + var bodyIndicesVector = new Vector(new Span(bodyIndices, Vector.Count)); + bodyIndicesVector = Vector.BitwiseOr(trailingMask, bodyIndicesVector); + bodies.GatherState(bodyIndicesVector, false, out var position, out var orientation, out var velocity, out _); + //Note that we integrate pose, THEN velocity. This is executing in the context of the second (or beyond) substep, which are effectively completing the previous substep's frame. + //In other words, the pose integration completes the last substep, and then velocity integration prepares for the current substep. + //The last substep's pose integration is handled in the IntegrateBundlesAfterSubstepping. + position += velocity.Linear * bundleDt; + //Kinematic bodies have infinite inertia, so using the angular momentum conserving codepaths would hit a singularity. + PoseIntegration.Integrate(orientation, velocity.Angular, halfDt, out orientation); + if (callbacks.IntegrateVelocityForKinematics) + callbacks.IntegrateVelocity(bodyIndicesVector, position, orientation, zeroInertia, existingMask, workerIndex, bundleDt, ref velocity); + //Writes to the empty lanes won't matter (scatter is masked), so we don't need to clean them up. + bodies.ScatterVelocities(ref velocity, ref bodyIndicesVector); + + } + } + unsafe void IntegrateBundlesAfterSubstepping(ref IndexSet mergedConstrainedBodyHandles, int bundleStartIndex, int bundleEndIndex, float dt, float substepDt, int substepCount, int workerIndex) { var bodyCount = bodies.ActiveSet.Count; diff --git a/BepuPhysics/Simulation.cs b/BepuPhysics/Simulation.cs index 22d2bc18b..8d0395075 100644 --- a/BepuPhysics/Simulation.cs +++ b/BepuPhysics/Simulation.cs @@ -415,7 +415,7 @@ public void EnsureCapacity(SimulationAllocationSizes allocationTarget) /// Allocation sizes to guarantee sufficient size for. public void Resize(SimulationAllocationSizes allocationTarget) { - Solver.ResizeSolverCapacities(allocationTarget.Bodies, allocationTarget.Constraints); + Solver.ResizeSolverCapacities(allocationTarget.Bodies, allocationTarget.Constraints, allocationTarget.ConstrainedKinematics); Solver.MinimumCapacityPerTypeBatch = allocationTarget.ConstraintsPerTypeBatch; Solver.ResizeTypeBatchCapacities(); NarrowPhase.PairCache.ResizeConstraintToPairMappingCapacity(Solver, allocationTarget.Constraints); diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index 998126f23..db94fbbea 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -215,11 +215,11 @@ public void Execute(Solver solver, int blockIndex, int workerIndex) ref var block = ref this.solver.substepContext.KinematicIntegrationBlocks[blockIndex]; if (SubstepIndex == 0) { - this.solver.PoseIntegrator.IntegrateVelocities(solver.ConstrainedKinematicHandles.Span.Slice(solver.ConstrainedKinematicHandles.Count), block.StartBundleIndex, block.EndBundleIndex, Dt, workerIndex); + this.solver.PoseIntegrator.IntegrateKinematicVelocities(solver.ConstrainedKinematicHandles.Span.Slice(solver.ConstrainedKinematicHandles.Count), block.StartBundleIndex, block.EndBundleIndex, Dt, workerIndex); } else { - this.solver.PoseIntegrator.IntegratePosesAndVelocities(solver.ConstrainedKinematicHandles.Span.Slice(solver.ConstrainedKinematicHandles.Count), block.StartBundleIndex, block.EndBundleIndex, Dt, workerIndex); + this.solver.PoseIntegrator.IntegrateKinematicPosesAndVelocities(solver.ConstrainedKinematicHandles.Span.Slice(solver.ConstrainedKinematicHandles.Count), block.StartBundleIndex, block.EndBundleIndex, Dt, workerIndex); } } } @@ -402,7 +402,11 @@ void SolveStep2Worker2(int workerIndex) ExecuteMainStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStart, ref substepContext.Stages[0], syncOffsetToPreviousSubstep, ref syncIndex); } integrateConstrainedKinematicsStage.SubstepIndex = substepIndex; - ExecuteMainStage(ref integrateConstrainedKinematicsStage, workerIndex, kinematicIntegrationWorkerStart, ref substepContext.Stages[1], syncOffsetToPreviousSubstep, ref syncIndex); + //Note that we do not invoke velocity integration on the first substep if kinematics do not need velocity integration. + if (substepIndex > 1 || PoseIntegrator.Callbacks.IntegrateVelocityForKinematics) + ExecuteMainStage(ref integrateConstrainedKinematicsStage, workerIndex, kinematicIntegrationWorkerStart, ref substepContext.Stages[1], syncOffsetToPreviousSubstep, ref syncIndex); + else + ++syncIndex; warmstartStage.SubstepIndex = substepIndex; for (int batchIndex = 0; batchIndex < syncStagesPerWarmStartOrSolve; ++batchIndex) { @@ -1177,11 +1181,11 @@ public override void SolveStep2(float totalDt, IThreadDispatcher threadDispatche TypeProcessors[typeBatch.TypeId].IncrementallyUpdateContactData(ref typeBatch, bodies, substepDt, inverseDt, 0, typeBatch.BundleCount); } } - PoseIntegrator.IntegratePosesAndVelocities(ConstrainedKinematicHandles.Span.Slice(ConstrainedKinematicHandles.Count), 0, ConstrainedKinematicHandles.Count, substepDt, 0); + PoseIntegrator.IntegrateKinematicPosesAndVelocities(ConstrainedKinematicHandles.Span.Slice(ConstrainedKinematicHandles.Count), 0, ConstrainedKinematicHandles.Count, substepDt, 0); } else { - PoseIntegrator.IntegrateVelocities(ConstrainedKinematicHandles.Span.Slice(ConstrainedKinematicHandles.Count), 0, ConstrainedKinematicHandles, substepDt, 0); + PoseIntegrator.IntegrateKinematicVelocities(ConstrainedKinematicHandles.Span.Slice(ConstrainedKinematicHandles.Count), 0, ConstrainedKinematicHandles, substepDt, 0); } for (int i = 0; i < batchCount; ++i) { diff --git a/BepuUtilities/BundleIndexing.cs b/BepuUtilities/BundleIndexing.cs index 2bf5d948b..8e3d442f4 100644 --- a/BepuUtilities/BundleIndexing.cs +++ b/BepuUtilities/BundleIndexing.cs @@ -61,6 +61,31 @@ public static int GetBundleCount(int elementCount) return (elementCount + VectorMask) >> VectorShift; } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static unsafe Vector CreateTrailingMaskForCountInBundle(int countInBundle) + { + if (Avx.IsSupported && Vector.Count == 8) + { + return Avx.CompareLessThanOrEqual(Vector256.Create((float)countInBundle), Vector256.Create(0f, 1f, 2f, 3f, 4f, 5f, 6f, 7f)).AsInt32().AsVector(); + } + else if (Sse.IsSupported && Vector.Count == 4) + { + return Sse.CompareLessThanOrEqual(Vector128.Create((float)countInBundle), Vector128.Create(0f, 1f, 2f, 3f)).AsInt32().AsVector(); + } + else + { + Vector mask; + var toReturnPointer = (int*)&mask; + for (int i = 0; i < Vector.Count; ++i) + { + toReturnPointer[i] = countInBundle <= i ? -1 : 0; + } + return mask; + } + //TODO: ARM + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static unsafe Vector CreateMaskForCountInBundle(int countInBundle) { @@ -78,7 +103,7 @@ public static unsafe Vector CreateMaskForCountInBundle(int countInBundle) var toReturnPointer = (int*)&mask; for (int i = 0; i < Vector.Count; ++i) { - toReturnPointer[i] = i < countInBundle ? -1 : 0; + toReturnPointer[i] = countInBundle > i ? -1 : 0; } return mask; } diff --git a/Demos/DemoCallbacks.cs b/Demos/DemoCallbacks.cs index 6427d5ba3..a730b41ae 100644 --- a/Demos/DemoCallbacks.cs +++ b/Demos/DemoCallbacks.cs @@ -41,6 +41,14 @@ public struct DemoPoseIntegratorCallbacks : IPoseIntegratorCallbacks /// public readonly bool AllowSubstepsForUnconstrainedBodies => false; + /// + /// Gets whether the velocity integration callback should be called for kinematic bodies. + /// If true, IntegrateVelocity will be called for bundles including kinematic bodies. + /// If false, kinematic bodies will just continue using whatever velocity they have set. + /// Most use cases should set this to false. + /// + public readonly bool IntegrateVelocityForKinematics => false; + public void Initialize(Simulation simulation) { //In this demo, we don't need to initialize anything. @@ -95,7 +103,8 @@ public void IntegrateVelocity(int bodyIndex, in RigidPose pose, in BodyInertia l [MethodImpl(MethodImplOptions.AggressiveInlining)] public void IntegrateVelocity(in Vector bodyIndices, in Vector3Wide position, in QuaternionWide orientation, in BodyInertiaWide localInertia, in Vector integrationMask, int workerIndex, in Vector dt, ref BodyVelocityWide velocity) { - velocity.Linear = Vector3Wide.ConditionalSelect(Vector.Equals(localInertia.InverseMass, Vector.Zero), velocity.Linear, velocity.Linear + gravityWide * dt); + //Note that we don't have to check for kinematics; IntegrateVelocityForKinematics returns false, so we'll never see them in this callback. + velocity.Linear += gravityWide * dt; } } public unsafe struct DemoNarrowPhaseCallbacks : INarrowPhaseCallbacks diff --git a/Demos/Demos/PlanetDemo.cs b/Demos/Demos/PlanetDemo.cs index 3603aa146..0f139417f 100644 --- a/Demos/Demos/PlanetDemo.cs +++ b/Demos/Demos/PlanetDemo.cs @@ -31,6 +31,8 @@ struct PlanetaryGravityCallbacks : IPoseIntegratorCallbacks public readonly bool AllowSubstepsForUnconstrainedBodies => false; + public readonly bool IntegrateVelocityForKinematics => false; + public void Initialize(Simulation simulation) { } diff --git a/Demos/Demos/SimpleSelfContainedDemo.cs b/Demos/Demos/SimpleSelfContainedDemo.cs index 228e342e0..2050cb5e4 100644 --- a/Demos/Demos/SimpleSelfContainedDemo.cs +++ b/Demos/Demos/SimpleSelfContainedDemo.cs @@ -149,6 +149,14 @@ public void Initialize(Simulation simulation) /// public readonly bool AllowSubstepsForUnconstrainedBodies => false; + /// + /// Gets whether the velocity integration callback should be called for kinematic bodies. + /// If true, IntegrateVelocity will be called for bundles including kinematic bodies. + /// If false, kinematic bodies will just continue using whatever velocity they have set. + /// Most use cases should set this to false. + /// + public readonly bool IntegrateVelocityForKinematics => false; + public PoseIntegratorCallbacks(Vector3 gravity) : this() { Gravity = gravity; diff --git a/Demos/SpecializedTests/GyroscopeTestDemo.cs b/Demos/SpecializedTests/GyroscopeTestDemo.cs index ea03a8a99..b546bcaa5 100644 --- a/Demos/SpecializedTests/GyroscopeTestDemo.cs +++ b/Demos/SpecializedTests/GyroscopeTestDemo.cs @@ -22,6 +22,8 @@ struct GyroscopicIntegratorCallbacks : IPoseIntegratorCallbacks //For this demo, we'll allow substepping for unconstrained bodies. public readonly bool AllowSubstepsForUnconstrainedBodies => true; + public readonly bool IntegrateVelocityForKinematics => false; + public void Initialize(Simulation simulation) { innerCallbacks.Initialize(simulation); From 60eef5577e276fa9bf412eb2be442ec7670df8b4 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 27 Oct 2021 20:04:41 -0500 Subject: [PATCH 238/947] Conditionally avoided integrating velocity of kinematics in post-solver pass. --- BepuPhysics/PoseIntegrator.cs | 31 +++++++++++++++++++++---- BepuPhysics/Solver_SubsteppingSolve2.cs | 13 ++++++++++- 2 files changed, 39 insertions(+), 5 deletions(-) diff --git a/BepuPhysics/PoseIntegrator.cs b/BepuPhysics/PoseIntegrator.cs index 636a6bd23..87b329d5d 100644 --- a/BepuPhysics/PoseIntegrator.cs +++ b/BepuPhysics/PoseIntegrator.cs @@ -897,6 +897,25 @@ unsafe void IntegrateBundlesAfterSubstepping(ref IndexSet mergedConstrainedBodyH } var halfDt = bundleEffectiveDt * new Vector(0.5f); bodies.GatherState(bodyIndices, false, out var position, out var orientation, out var velocity, out var localInertia); + + + Vector unconstrainedVelocityIntegrationMask; + bool anyBodyInBundleNeedsVelocityIntegration; + if (callbacks.IntegrateVelocityForKinematics) + { + unconstrainedVelocityIntegrationMask = unconstrainedMask; + anyBodyInBundleNeedsVelocityIntegration = anyBodyInBundleIsUnconstrained; + } + else + { + var isKinematic = + Vector.Equals(Vector.BitwiseOr( + Vector.BitwiseOr(Vector.BitwiseOr(localInertia.InverseMass, localInertia.InverseInertiaTensor.XX), Vector.BitwiseOr(localInertia.InverseInertiaTensor.YX, localInertia.InverseInertiaTensor.YY)), + Vector.BitwiseOr(Vector.BitwiseOr(localInertia.InverseInertiaTensor.ZX, localInertia.InverseInertiaTensor.ZY), localInertia.InverseInertiaTensor.ZZ)), Vector.Zero); + unconstrainedVelocityIntegrationMask = Vector.AndNot(unconstrainedMask, isKinematic); + anyBodyInBundleNeedsVelocityIntegration = Vector.LessThanAny(unconstrainedVelocityIntegrationMask, Vector.Zero); + } + if (anyBodyInBundleIsUnconstrained) { int integrationStepCount; @@ -912,10 +931,14 @@ unsafe void IntegrateBundlesAfterSubstepping(ref IndexSet mergedConstrainedBodyH { //Note that the following integrates velocities, then poses. var previousVelocity = velocity; - callbacks.IntegrateVelocity(bodyIndices, position, orientation, localInertia, unconstrainedMask, workerIndex, bundleEffectiveDt, ref velocity); - //It would be annoying to make the user handle masking velocity writes to inactive lanes, so we handle it internally. - Vector3Wide.ConditionalSelect(unconstrainedMask, velocity.Linear, previousVelocity.Linear, out velocity.Linear); - Vector3Wide.ConditionalSelect(unconstrainedMask, velocity.Angular, previousVelocity.Angular, out velocity.Angular); + + if (anyBodyInBundleNeedsVelocityIntegration) + { + callbacks.IntegrateVelocity(bodyIndices, position, orientation, localInertia, unconstrainedVelocityIntegrationMask, workerIndex, bundleEffectiveDt, ref velocity); + //It would be annoying to make the user handle masking velocity writes to inactive lanes, so we handle it internally. + Vector3Wide.ConditionalSelect(unconstrainedVelocityIntegrationMask, velocity.Linear, previousVelocity.Linear, out velocity.Linear); + Vector3Wide.ConditionalSelect(unconstrainedVelocityIntegrationMask, velocity.Angular, previousVelocity.Angular, out velocity.Angular); + } position += velocity.Linear * bundleEffectiveDt; diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index db94fbbea..36ecb1ec7 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -406,7 +406,7 @@ void SolveStep2Worker2(int workerIndex) if (substepIndex > 1 || PoseIntegrator.Callbacks.IntegrateVelocityForKinematics) ExecuteMainStage(ref integrateConstrainedKinematicsStage, workerIndex, kinematicIntegrationWorkerStart, ref substepContext.Stages[1], syncOffsetToPreviousSubstep, ref syncIndex); else - ++syncIndex; + ++syncIndex; //Don't want to desync sync indices just because we aren't running a stage. warmstartStage.SubstepIndex = substepIndex; for (int batchIndex = 0; batchIndex < syncStagesPerWarmStartOrSolve; ++batchIndex) { @@ -1125,6 +1125,17 @@ public override unsafe IndexSet PrepareConstraintIntegrationResponsibilities(int bodiesFirstObservedInBatches[batchIndex].Dispose(pool); } pool.Return(ref bodiesFirstObservedInBatches); + + //Add the constrained kinematics to the constrained body handles. The kinematics were absent from batch referenced handles. + //TODO: This assumes the number of kinematics is low relative to the number of bodies and does not need to be multithreaded. + //This assumption is *usually* fine, but we should probably have a fallback that is more efficient if this assumption is wrong. + //Could maintain an indexset parallel to the ConstrainedKinematicHandles- same set, just different format. + //If we detect a lot of constrained kinematics, just do an indexset merge. + //That would be fast enough even if all bodies were kinematic. + for (int i = 0; i < ConstrainedKinematicHandles.Count; ++i) + { + mergedConstrainedBodyHandles.AddUnsafely(ConstrainedKinematicHandles[i]); + } return mergedConstrainedBodyHandles; } else From 6498610d08aa63832ec0618a05efd0e964c0627d Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 27 Oct 2021 20:06:49 -0500 Subject: [PATCH 239/947] Does not build. Fixed single threaded integration prepass work bounds, and conditionally avoided kinematic velocity integration. --- BepuPhysics/Solver_SubsteppingSolve2.cs | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index 36ecb1ec7..db141c1d3 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -520,7 +520,7 @@ void SolveStep2Worker2(int workerIndex) } - Buffer BuildIntegrationWorkBlocks(int minimumBlockSizeInBundles, int maximumBlockSizeInBundles, int targetBlockCount) + Buffer BuildKinematicIntegrationWorkBlocks(int minimumBlockSizeInBundles, int maximumBlockSizeInBundles, int targetBlockCount) { var bundleCount = BundleIndexing.GetBundleCount(ConstrainedKinematicHandles.Count); var targetBundlesPerBlock = bundleCount / targetBlockCount; @@ -571,7 +571,7 @@ protected void ExecuteMultithreaded2(float dt, IThreadDispatcher threadDispatche pool.Return(ref incrementalUpdateBatchBoundaries); //TODO: No need to create this in the first place. Doesn't really cost anything, but... substepContext.ConstraintBlocks = constraintBlocks.Span.Slice(constraintBlocks.Count); substepContext.IncrementalUpdateBlocks = incrementalBlocks.Span.Slice(incrementalBlocks.Count); - substepContext.KinematicIntegrationBlocks = BuildIntegrationWorkBlocks(minimumBlockSizeInBundles, maximumBlockSizeInBundles, targetBlocksPerBatch); + substepContext.KinematicIntegrationBlocks = BuildKinematicIntegrationWorkBlocks(minimumBlockSizeInBundles, maximumBlockSizeInBundles, targetBlocksPerBatch); //Not every batch will actually have work blocks associated with it; the batch compressor could be falling behind, which means older constraints could be at higher batches than they need to be, leaving gaps. //We don't want to include those empty batches as sync points in the solver. @@ -1192,11 +1192,12 @@ public override void SolveStep2(float totalDt, IThreadDispatcher threadDispatche TypeProcessors[typeBatch.TypeId].IncrementallyUpdateContactData(ref typeBatch, bodies, substepDt, inverseDt, 0, typeBatch.BundleCount); } } - PoseIntegrator.IntegrateKinematicPosesAndVelocities(ConstrainedKinematicHandles.Span.Slice(ConstrainedKinematicHandles.Count), 0, ConstrainedKinematicHandles.Count, substepDt, 0); + PoseIntegrator.IntegrateKinematicPosesAndVelocities(ConstrainedKinematicHandles.Span.Slice(ConstrainedKinematicHandles.Count), 0, BundleIndexing.GetBundleCount(ConstrainedKinematicHandles.Count), substepDt, 0); } else { - PoseIntegrator.IntegrateKinematicVelocities(ConstrainedKinematicHandles.Span.Slice(ConstrainedKinematicHandles.Count), 0, ConstrainedKinematicHandles, substepDt, 0); + if (PoseIntegrator.Callbacks.IntegrateVelocityForKinematics) + PoseIntegrator.IntegrateKinematicVelocities(ConstrainedKinematicHandles.Span.Slice(ConstrainedKinematicHandles.Count), 0, BundleIndexing.GetBundleCount(ConstrainedKinematicHandles.Count), substepDt, 0); } for (int i = 0; i < batchCount; ++i) { From 99d84dbeed17c5e303e8327849b818d164763927 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 28 Oct 2021 16:48:41 -0500 Subject: [PATCH 240/947] Doesn't run properly. ConstrainedKinematicHandles now allocates pretty conservatively. Not ideal, but okay for prototype. --- BepuPhysics/IslandAwakener.cs | 1 + BepuPhysics/Simulation.cs | 4 ++-- BepuPhysics/Solver.cs | 12 +++++------- BepuPhysics/Solver_SubsteppingSolve2.cs | 3 +-- 4 files changed, 9 insertions(+), 11 deletions(-) diff --git a/BepuPhysics/IslandAwakener.cs b/BepuPhysics/IslandAwakener.cs index b2677401a..d022595cc 100644 --- a/BepuPhysics/IslandAwakener.cs +++ b/BepuPhysics/IslandAwakener.cs @@ -594,6 +594,7 @@ void AccumulatePairCacheTypeCounts(ref Buffer sourceTypeCaches, r //Ensure capacities on all systems: //bodies, bodies.EnsureCapacity(bodies.ActiveSet.Count + newBodyCount); + solver.ConstrainedKinematicHandles.EnsureCapacity(solver.ConstrainedKinematicHandles.Count + newBodyCount, pool); //TODO: This could be FAR more conservative. Few bodies are typically kinematic. //broad phase, (technically overestimating, not every body has a collidable, but vast majority do and shrug) broadPhase.EnsureCapacity(broadPhase.ActiveTree.LeafCount + newBodyCount, broadPhase.StaticTree.LeafCount); //constraints, diff --git a/BepuPhysics/Simulation.cs b/BepuPhysics/Simulation.cs index 8d0395075..3a6c26ab9 100644 --- a/BepuPhysics/Simulation.cs +++ b/BepuPhysics/Simulation.cs @@ -105,7 +105,7 @@ public static Simulation Create simulation.Solver = new Solver(simulation.Bodies, simulation.BufferPool, solverIterationCount, solverFallbackBatchThreshold, initialCapacity: initialAllocationSizes.Value.Constraints, - initialIslandCapacity: initialAllocationSizes.Value.Islands, + initialIslandCapacity: initialAllocationSizes.Value.Islands, minimumCapacityPerTypeBatch: initialAllocationSizes.Value.ConstraintsPerTypeBatch, poseIntegrator); simulation.constraintRemover = new ConstraintRemover(simulation.BufferPool, simulation.Bodies, simulation.Solver); simulation.Sleeper = new IslandSleeper(simulation.Bodies, simulation.Solver, simulation.BroadPhase, simulation.constraintRemover, simulation.BufferPool); @@ -415,7 +415,7 @@ public void EnsureCapacity(SimulationAllocationSizes allocationTarget) /// Allocation sizes to guarantee sufficient size for. public void Resize(SimulationAllocationSizes allocationTarget) { - Solver.ResizeSolverCapacities(allocationTarget.Bodies, allocationTarget.Constraints, allocationTarget.ConstrainedKinematics); + Solver.ResizeSolverCapacities(allocationTarget.Bodies, allocationTarget.Constraints); Solver.MinimumCapacityPerTypeBatch = allocationTarget.ConstraintsPerTypeBatch; Solver.ResizeTypeBatchCapacities(); NarrowPhase.PairCache.ResizeConstraintToPairMappingCapacity(Solver, allocationTarget.Constraints); diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index c3df77215..6c2230b59 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -208,7 +208,6 @@ public int CountConstraints() public Solver(Bodies bodies, BufferPool pool, int iterationCount, int fallbackBatchThreshold, int initialCapacity, int initialIslandCapacity, - int initialConstrainedKinematicCapacity, int minimumCapacityPerTypeBatch) { this.iterationCount = iterationCount; @@ -223,7 +222,7 @@ public Solver(Bodies bodies, BufferPool pool, int iterationCount, int fallbackBa ResizeHandleCapacity(initialCapacity); solveWorker = SolveWorker; incrementalContactUpdateWorker = IncrementalContactUpdateWorker; - ConstrainedKinematicHandles = new QuickSet>(initialConstrainedKinematicCapacity, pool); + ConstrainedKinematicHandles = new QuickSet>(bodies.HandleToLocation.Length, pool); } public void Register() where TDescription : unmanaged, IConstraintDescription @@ -1430,7 +1429,7 @@ public void Clear() /// Size of the span of body handles to allocate space for. Applies to batch referenced handle sets. /// Number of constraint handles to allocate space for. Applies to the handle->constraint mapping table. /// Number of constrained kinematic body handles to allocate space for. - public void EnsureSolverCapacities(int bodyHandleCapacity, int constraintHandleCapacity, int constrainedKinematicCapacity) + public void EnsureSolverCapacities(int bodyHandleCapacity, int constraintHandleCapacity) { if (HandleToConstraint.Length < constraintHandleCapacity) { @@ -1443,7 +1442,7 @@ public void EnsureSolverCapacities(int bodyHandleCapacity, int constraintHandleC batchReferencedHandles[i].EnsureCapacity(targetBatchReferencedHandleSize, pool); } - ConstrainedKinematicHandles.EnsureCapacity(constrainedKinematicCapacity, pool); + ConstrainedKinematicHandles.EnsureCapacity(bodyHandleCapacity, pool); } void ResizeHandleCapacity(int constraintHandleCapacity) @@ -1462,8 +1461,7 @@ void ResizeHandleCapacity(int constraintHandleCapacity) /// /// Size of the span of body handles to allocate space for. Applies to batch referenced handle sets. /// Number of constraint handles to allocate space for. Applies to the handle->constraint mapping table. - /// Number of constrained kinematic body handles to allocate space for. - public void ResizeSolverCapacities(int bodyHandleCapacity, int constraintHandleCapacity, int constrainedKinematicCapacity) + public void ResizeSolverCapacities(int bodyHandleCapacity, int constraintHandleCapacity) { var targetConstraintCount = BufferPool.GetCapacityForCount(Math.Max(constraintHandleCapacity, HandlePool.HighestPossiblyClaimedId + 1)); if (HandleToConstraint.Length != targetConstraintCount) @@ -1477,7 +1475,7 @@ public void ResizeSolverCapacities(int bodyHandleCapacity, int constraintHandleC batchReferencedHandles[i].Resize(targetBatchReferencedHandleSize, pool); } - var targetConstrainedKinematicsCapacity = Math.Max(ConstrainedKinematicHandles.Count, constrainedKinematicCapacity); + var targetConstrainedKinematicsCapacity = Math.Max(ConstrainedKinematicHandles.Count, bodyHandleCapacity); ConstrainedKinematicHandles.Resize(targetConstrainedKinematicsCapacity, pool); } diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index db141c1d3..905c17ea9 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -103,9 +103,8 @@ public class Solver : Solver where TIntegrationCallbacks public Solver(Bodies bodies, BufferPool pool, int iterationCount, int fallbackBatchThreshold, int initialCapacity, int initialIslandCapacity, - int initialConstrainedKinematicCapacity, int minimumCapacityPerTypeBatch, PoseIntegrator poseIntegrator) - : base(bodies, pool, iterationCount, fallbackBatchThreshold, initialCapacity, initialIslandCapacity, initialConstrainedKinematicCapacity, minimumCapacityPerTypeBatch) + : base(bodies, pool, iterationCount, fallbackBatchThreshold, initialCapacity, initialIslandCapacity, minimumCapacityPerTypeBatch) { PoseIntegrator = poseIntegrator; solveStep2Worker2 = SolveStep2Worker2; From 8c074c216c684236b5e56cb5c116c87be2cb0b76 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 28 Oct 2021 16:50:42 -0500 Subject: [PATCH 241/947] Fixed missing pose scatter in kinematic pose integration. --- BepuPhysics/PoseIntegrator.cs | 1 + 1 file changed, 1 insertion(+) diff --git a/BepuPhysics/PoseIntegrator.cs b/BepuPhysics/PoseIntegrator.cs index 87b329d5d..cab00d58c 100644 --- a/BepuPhysics/PoseIntegrator.cs +++ b/BepuPhysics/PoseIntegrator.cs @@ -835,6 +835,7 @@ internal unsafe void IntegrateKinematicPosesAndVelocities(Buffer bodyHandle if (callbacks.IntegrateVelocityForKinematics) callbacks.IntegrateVelocity(bodyIndicesVector, position, orientation, zeroInertia, existingMask, workerIndex, bundleDt, ref velocity); //Writes to the empty lanes won't matter (scatter is masked), so we don't need to clean them up. + bodies.ScatterPose(ref position, ref orientation, bodyIndicesVector, existingMask); bodies.ScatterVelocities(ref velocity, ref bodyIndicesVector); } From d0d42ade92658eeab15fea0a25b7053c4897a974 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 28 Oct 2021 16:56:29 -0500 Subject: [PATCH 242/947] Added fallback for partially or fully inertia locked kinematic bodies when using angular momentum conserving integration. --- BepuPhysics/PoseIntegrator.cs | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/BepuPhysics/PoseIntegrator.cs b/BepuPhysics/PoseIntegrator.cs index cab00d58c..77b94518e 100644 --- a/BepuPhysics/PoseIntegrator.cs +++ b/BepuPhysics/PoseIntegrator.cs @@ -187,6 +187,18 @@ public static void RotateInverseInertia(in Symmetric3x3Wide localInverseInertiaT Symmetric3x3Wide.RotationSandwich(orientationMatrix, localInverseInertiaTensor, out rotatedInverseInertiaTensor); } + /// + /// Uses the previous angular velocity if attempting to conserve angular momentum introduced infinities or NaNs. Happens when attempting to conserve momentum with a kinematic or partially inertia locked body. + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + static void FallbackIfInertiaIncompatible(in Vector3Wide previousAngularVelocity, ref Vector3Wide angularVelocity) + { + var infinity = new Vector(float.PositiveInfinity); + angularVelocity.X = Vector.ConditionalSelect(Vector.LessThan(Vector.Abs(angularVelocity.X), infinity), angularVelocity.X, previousAngularVelocity.X); + angularVelocity.Y = Vector.ConditionalSelect(Vector.LessThan(Vector.Abs(angularVelocity.Y), infinity), angularVelocity.Y, previousAngularVelocity.Y); + angularVelocity.Z = Vector.ConditionalSelect(Vector.LessThan(Vector.Abs(angularVelocity.Z), infinity), angularVelocity.Z, previousAngularVelocity.Z); + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void IntegrateAngularVelocityConserveMomentum(in QuaternionWide previousOrientation, in Symmetric3x3Wide localInverseInertia, in Symmetric3x3Wide worldInverseInertia, ref Vector3Wide angularVelocity) { @@ -198,7 +210,9 @@ public static void IntegrateAngularVelocityConserveMomentum(in QuaternionWide pr Symmetric3x3Wide.Invert(localInverseInertia, out var localInertiaTensor); Symmetric3x3Wide.TransformWithoutOverlap(localPreviousAngularVelocity, localInertiaTensor, out var localAngularMomentum); Matrix3x3Wide.Transform(localAngularMomentum, previousOrientationMatrix, out var angularMomentum); + var previousVelocity = angularVelocity; Symmetric3x3Wide.TransformWithoutOverlap(angularMomentum, worldInverseInertia, out angularVelocity); + FallbackIfInertiaIncompatible(previousVelocity, ref angularVelocity); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -243,7 +257,9 @@ public static void IntegrateAngularVelocityConserveMomentumWithGyroscopicTorque( Matrix3x3Wide.Transform(residual, inverseJacobian, out var newtonStep); localAngularVelocity -= newtonStep; + var previousVelocity = angularVelocity; Matrix3x3Wide.Transform(localAngularVelocity, orientationMatrix, out angularVelocity); + FallbackIfInertiaIncompatible(previousVelocity, ref angularVelocity); } [MethodImpl(MethodImplOptions.AggressiveInlining)] From b6a3739a03788c78f092d5187c463956ba3d1209 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 28 Oct 2021 18:33:09 -0500 Subject: [PATCH 243/947] Does not run. Added more validation for fallback dynamic body references. --- .../EmbeddedSubsteppingTimestepper2.cs | 1 + BepuPhysics/Solver.cs | 37 +++++++++++++++++++ 2 files changed, 38 insertions(+) diff --git a/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs b/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs index 711f9dbca..7a64696f5 100644 --- a/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs +++ b/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs @@ -63,6 +63,7 @@ public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDi simulation.Solver.ValidateConstraintMaps(); simulation.Solver.ValidateConstraintReferenceKinematicity(); simulation.Solver.ValidateConstrainedKinematicsSet(); + simulation.Solver.ValidateFallbackBodiesAreDynamic(); var constrainedBodySet = simulation.Solver.PrepareConstraintIntegrationResponsibilities(SubstepCount, threadDispatcher); simulation.Profiler.Start(simulation.Solver); simulation.Solver.SolveStep2(dt, threadDispatcher); diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index 6c2230b59..ace451e43 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -392,6 +392,43 @@ internal unsafe void ValidateFallbackBatchEmptySlotReferences() } } + + [Conditional("DEBUG")] + internal unsafe void ValidateFallbackBodiesAreDynamic() + { + ref var set = ref ActiveSet; + if (set.Batches.Count > FallbackBatchThreshold) + { + for (int i = 0; i < set.SequentialFallback.dynamicBodyConstraintCounts.Count; ++i) + { + Debug.Assert(!Bodies.IsKinematicUnsafe(ref bodies.ActiveSet.SolverStates[set.SequentialFallback.dynamicBodyConstraintCounts.Keys[i]].Inertia.Local), + "All ostensibly dynamic bodies tracked by the fallback batch must actually be dynamic."); + } + for (int i = 0; i < bodies.ActiveSet.Count; ++i) + { + var constraints = bodies.ActiveSet.Constraints[i]; + var fallbackConstraintsForDynamicBody = 0; + for (int j = 0; j < constraints.Count; ++j) + { + if (HandleToConstraint[constraints[j].ConnectingConstraintHandle.Value].BatchIndex == FallbackBatchThreshold) + { + ++fallbackConstraintsForDynamicBody; + } + } + if (Bodies.IsKinematicUnsafe(ref bodies.ActiveSet.SolverStates[i].Inertia.Local)) + { + Debug.Assert(fallbackConstraintsForDynamicBody == 0, "Kinematics should not be present in the dynamic bodies referenced by the fallback batch."); + } + else + { + var succeeded = ActiveSet.SequentialFallback.dynamicBodyConstraintCounts.TryGetValue(i, out var countForBody); + Debug.Assert(succeeded == (fallbackConstraintsForDynamicBody > 0), "The fallback batch should contain a reference to the dynamic body if there are constraints associated with it in the fallback batch."); + Debug.Assert(fallbackConstraintsForDynamicBody == countForBody, "If the dynamic body is referenced in the fallback batch, the brute force count should match the cached count."); + } + } + } + } + [Conditional("DEBUG")] internal unsafe void ValidateFallbackBatchAccessSafety() { From 82d4bb51d911d93d564f709c46c98f0950748dd8 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 28 Oct 2021 18:51:08 -0500 Subject: [PATCH 244/947] Does not run. Hammering swarms of bugs. --- BepuPhysics/Constraints/FourBodyTypeProcessor.cs | 8 ++++---- BepuPhysics/Constraints/OneBodyTypeProcessor.cs | 2 +- BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs | 6 +++--- BepuPhysics/Constraints/TwoBodyTypeProcessor.cs | 4 ++-- BepuPhysics/Constraints/TypeProcessor.cs | 6 ++++-- BepuPhysics/Solver.cs | 2 +- BepuPhysics/Solver_SubsteppingSolve2.cs | 2 +- Demos/Demo.cs | 2 +- 8 files changed, 17 insertions(+), 15 deletions(-) diff --git a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs index cb802bdea..c14ec2891 100644 --- a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs @@ -80,10 +80,10 @@ public sealed unsafe override void EnumerateConnectedBodyIndices(re BundleIndexing.GetBundleIndices(indexInTypeBatch, out var constraintBundleIndex, out var constraintInnerIndex); ref var indices = ref GatherScatter.GetOffsetInstance(ref Buffer.Get(typeBatch.BodyReferences.Memory, constraintBundleIndex), constraintInnerIndex); //Note that we hold a reference to the indices. That's important if the loop body mutates indices. - enumerator.LoopBody(GatherScatter.GetFirst(ref indices.IndexA)); - enumerator.LoopBody(GatherScatter.GetFirst(ref indices.IndexB)); - enumerator.LoopBody(GatherScatter.GetFirst(ref indices.IndexC)); - enumerator.LoopBody(GatherScatter.GetFirst(ref indices.IndexD)); + enumerator.LoopBody(GatherScatter.GetFirst(ref indices.IndexA) & Bodies.BodyIndexMask); + enumerator.LoopBody(GatherScatter.GetFirst(ref indices.IndexB) & Bodies.BodyIndexMask); + enumerator.LoopBody(GatherScatter.GetFirst(ref indices.IndexC) & Bodies.BodyIndexMask); + enumerator.LoopBody(GatherScatter.GetFirst(ref indices.IndexD) & Bodies.BodyIndexMask); } struct FourBodySortKeyGenerator : ISortKeyGenerator { diff --git a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs index 21b3f7799..c5835f8e3 100644 --- a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs @@ -59,7 +59,7 @@ public abstract class OneBodyTypeProcessor(ref TypeBatch typeBatch, int indexInTypeBatch, ref TEnumerator enumerator) { BundleIndexing.GetBundleIndices(indexInTypeBatch, out var constraintBundleIndex, out var constraintInnerIndex); - enumerator.LoopBody(GatherScatter.Get(ref Buffer>.Get(ref typeBatch.BodyReferences, constraintBundleIndex), constraintInnerIndex)); + enumerator.LoopBody(GatherScatter.Get(ref Buffer>.Get(ref typeBatch.BodyReferences, constraintBundleIndex), constraintInnerIndex) & Bodies.BodyIndexMask); } diff --git a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs index 2a47c9dcb..7d616581a 100644 --- a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs @@ -71,9 +71,9 @@ public sealed unsafe override void EnumerateConnectedBodyIndices(re BundleIndexing.GetBundleIndices(indexInTypeBatch, out var constraintBundleIndex, out var constraintInnerIndex); ref var indices = ref GatherScatter.GetOffsetInstance(ref Buffer.Get(typeBatch.BodyReferences.Memory, constraintBundleIndex), constraintInnerIndex); //Note that we hold a reference to the indices. That's important if the loop body mutates indices. - enumerator.LoopBody(GatherScatter.GetFirst(ref indices.IndexA)); - enumerator.LoopBody(GatherScatter.GetFirst(ref indices.IndexB)); - enumerator.LoopBody(GatherScatter.GetFirst(ref indices.IndexC)); + enumerator.LoopBody(GatherScatter.GetFirst(ref indices.IndexA) & Bodies.BodyIndexMask); + enumerator.LoopBody(GatherScatter.GetFirst(ref indices.IndexB) & Bodies.BodyIndexMask); + enumerator.LoopBody(GatherScatter.GetFirst(ref indices.IndexC) & Bodies.BodyIndexMask); } struct ThreeBodySortKeyGenerator : ISortKeyGenerator { diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index bdfe35d8b..5375d73f8 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -79,8 +79,8 @@ public sealed override void EnumerateConnectedBodyIndices(ref TypeB ref var indexB = ref Unsafe.Add(ref indexA, Vector.Count); //Note that the variables are ref locals! This is important for correctness, because every execution of LoopBody could result in a swap. //Ref locals aren't the only solution, but if you ever change this, make sure you account for the potential mutation in the enumerator. - enumerator.LoopBody(indexA); - enumerator.LoopBody(indexB); + enumerator.LoopBody(indexA & Bodies.BodyIndexMask); + enumerator.LoopBody(indexB & Bodies.BodyIndexMask); } struct TwoBodySortKeyGenerator : ISortKeyGenerator { diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index a5a2ca2a4..d927ed413 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -1276,7 +1276,8 @@ public static unsafe void GatherAndIntegrate.Zero); + //Avoid slots that are empty (-1) or slots that are kinematic. Both can be tested by checking the unsigned magnitude against the flag lower limit. + var integrationMask = Vector.AsVectorInt32(Vector.LessThan(Vector.AsVectorUInt32(bodyIndices), new Vector(Bodies.DynamicLimit))); bodies.GatherState(bodyIndices, false, out position, out orientation, out velocity, out var localInertia); IntegratePoseAndVelocity(ref integratorCallbacks, ref bodyIndices, localInertia, dt, ref position, ref orientation, ref velocity, workerIndex, out inertia); bodies.ScatterPose(ref position, ref orientation, bodyIndices, integrationMask); @@ -1324,7 +1325,8 @@ public static unsafe void GatherAndIntegrate.Zero); + //Avoid slots that are empty (-1) or slots that are kinematic. Both can be tested by checking the unsigned magnitude against the flag lower limit. + var integrationMask = Vector.AsVectorInt32(Vector.LessThan(Vector.AsVectorUInt32(bodyIndices), new Vector(Bodies.DynamicLimit))); bodies.GatherState(bodyIndices, false, out position, out orientation, out velocity, out var localInertia); IntegrateVelocity(ref integratorCallbacks, ref bodyIndices, localInertia, dt, integrationMask, position, orientation, ref velocity, workerIndex, out inertia); bodies.ScatterInertia(ref inertia, bodyIndices, integrationMask); diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index ace451e43..19380a735 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -926,7 +926,7 @@ unsafe internal Span GetBlockingBodyHandles(Span bodyHan { var location = bodies.HandleToLocation[bodyHandles[i].Value]; Debug.Assert(location.SetIndex == 0); - if (Bodies.IsKinematicUnsafe(ref solverStates[location.Index].Inertia.Local)) + if (!Bodies.IsKinematicUnsafe(ref solverStates[location.Index].Inertia.Local)) { allocation[blockingCount++] = bodyHandles[i]; } diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index 905c17ea9..bffff3559 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -741,7 +741,7 @@ unsafe bool ComputeIntegrationResponsibilitiesForConstraintRegion for (int bundleInnerIndex = 0; bundleInnerIndex < countInBundle; ++bundleInnerIndex) { //Constraints refer to bodies by index when they're in the active set, so we need to transform to handle to look up our merged batch results. - var bodyIndex = bundleStart[bundleInnerIndex]; + var bodyIndex = bundleStart[bundleInnerIndex] & Bodies.BodyIndexMask; if (typeof(TFallbackness) == typeof(IsFallbackBatch)) { //Fallback batches can contain empty lanes; there's no guarantee of constraint contiguity. Such lanes are marked with -1 in the body references. diff --git a/Demos/Demo.cs b/Demos/Demo.cs index 37ebe5190..5f3790da3 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -62,7 +62,7 @@ public virtual void Update(Window window, Camera camera, Input input, float dt) //fully decouple simulation and rendering rates across different threads. //(In either case, you'd also want to interpolate or extrapolate simulation results during rendering for smoothness.) //Note that taking steps of variable length can reduce stability. Gradual or one-off changes can work reasonably well. - Simulation.Timestep(1 / 60f, ThreadDispatcher); + Simulation.Timestep(1 / 60f); ////Here's an example of how it would look to use more frequent updates, but still with a fixed amount of time simulated per update call: //const float timeToSimulate = 1 / 60f; From 447fd77e4839c3dd94f6f6c635e6b965f137432b Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 28 Oct 2021 19:07:47 -0500 Subject: [PATCH 245/947] Still doesn't run. Added IndexSet.Unset; ConstraintRemover no longer expects to see kinematics referenced in batchreferencedhandles. --- .../CollisionDetection/ConstraintRemover.cs | 12 +++++++++--- BepuUtilities/Collections/IndexSet.cs | 14 +++++++++++++- 2 files changed, 22 insertions(+), 4 deletions(-) diff --git a/BepuPhysics/CollisionDetection/ConstraintRemover.cs b/BepuPhysics/CollisionDetection/ConstraintRemover.cs index 30cd39839..ffa35d497 100644 --- a/BepuPhysics/CollisionDetection/ConstraintRemover.cs +++ b/BepuPhysics/CollisionDetection/ConstraintRemover.cs @@ -359,7 +359,9 @@ public void RemoveConstraintsFromBatchReferencedHandles() for (int j = 0; j < removals.Count; ++j) { ref var target = ref removals[j]; - solver.batchReferencedHandles[target.BatchIndex].Remove(target.BodyHandle.Value); + Debug.Assert(solver.batchReferencedHandles[target.BatchIndex].Contains(target.BodyHandle.Value) || bodies.GetBodyReference(target.BodyHandle).Kinematic, + "The batch referenced handles must include all constraint-involved dynamics, but will not include kinematics."); + solver.batchReferencedHandles[target.BatchIndex].Unset(target.BodyHandle.Value); } } } @@ -379,7 +381,9 @@ public void RemoveConstraintsFromFallbackBatchReferencedHandles() if (solver.ActiveSet.SequentialFallback.Remove(target.BodyIndex, ref allocationIdsToFree)) { //No more constraints for this body in the fallback set; it should not exist in the fallback batch's referenced handles anymore. - solver.batchReferencedHandles[target.BatchIndex].Remove(target.BodyHandle.Value); + Debug.Assert(solver.batchReferencedHandles[target.BatchIndex].Contains(target.BodyHandle.Value) || bodies.GetBodyReference(target.BodyHandle).Kinematic, + "The batch referenced handles must include all constraint-involved dynamics, but will not include kinematics."); + solver.batchReferencedHandles[target.BatchIndex].Unset(target.BodyHandle.Value); } } } @@ -389,7 +393,9 @@ public void TryRemoveAllConstraintsForBodyFromFallbackBatch(BodyHandle bodyHandl { if (solver.ActiveSet.SequentialFallback.TryRemove(bodyIndex, ref allocationIdsToFree)) { - solver.batchReferencedHandles[solver.FallbackBatchThreshold].Remove(bodyHandle.Value); + Debug.Assert(solver.batchReferencedHandles[solver.FallbackBatchThreshold].Contains(bodyHandle.Value) || bodies.GetBodyReference(bodyHandle).Kinematic, + "The batch referenced handles must include all constraint-involved dynamics, but will not include kinematics."); + solver.batchReferencedHandles[solver.FallbackBatchThreshold].Unset(bodyHandle.Value); } } diff --git a/BepuUtilities/Collections/IndexSet.cs b/BepuUtilities/Collections/IndexSet.cs index 909298514..2547b9321 100644 --- a/BepuUtilities/Collections/IndexSet.cs +++ b/BepuUtilities/Collections/IndexSet.cs @@ -117,6 +117,18 @@ public void Set(int index, BufferPool pool) SetUnsafely(index, index >> shift); } + /// + /// Marks an index in the set as uncontained without checking whether it is already set. + /// + /// Index to add. + /// This is functionally identical to the Remove method, but it doesn't include the same debug assertions. Just a way to make intent clear so that the assert can catch errors. + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void Unset(int index) + { + Flags[index >> shift] &= ~(1ul << (index & mask)); + } + /// /// Adds an index to the set without checking capacity. @@ -152,7 +164,7 @@ public void Add(int index, BufferPool pool) public void Remove(int index) { Debug.Assert((Flags[index >> shift] & (1ul << (index & mask))) > 0, "If you try to remove a index, it should be present."); - Flags[index >> shift] &= ~(1ul << (index & mask)); + Unset(index); } public void Clear() From e815050b14c6de5caba97d63244f9b08ab53b732 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 29 Oct 2021 17:22:18 -0500 Subject: [PATCH 246/947] Multiple bugs fixed, including one ancient one in the FindCandidateBatch that directly used collidable references rather than body handles. --- .../NarrowPhasePendingConstraintAdds.cs | 3 +- BepuPhysics/ConstraintBatch.cs | 4 ++- BepuPhysics/Solver.cs | 29 +++++++++++++++---- BepuPhysics/Solver_SubsteppingSolve2.cs | 6 ++-- Demos/Demo.cs | 2 +- 5 files changed, 31 insertions(+), 13 deletions(-) diff --git a/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs b/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs index e086b2b6f..9ab130733 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs @@ -199,11 +199,10 @@ internal unsafe void SpeculativeConstraintBatchSearch(Solver solver, int typeInd Debug.Assert(list.Buffer.Allocated, "The target region should be allocated, or else the job scheduler is broken."); Debug.Assert(list.Count > 0); int byteIndex = start * list.ElementSizeInBytes; - int bodyCount = ExtractContactConstraintBodyCount(typeIndex); ref var speculativeBatchIndicesForType = ref speculativeBatchIndices[typeIndex]; for (int i = start; i < end; ++i) { - speculativeBatchIndicesForType[i] = (ushort)solver.FindCandidateBatch(new Span(list.Buffer.Memory + byteIndex, bodyCount)); + speculativeBatchIndicesForType[i] = (ushort)solver.FindCandidateBatch(*(CollidablePair*)(list.Buffer.Memory + byteIndex)); byteIndex += list.ElementSizeInBytes; } } diff --git a/BepuPhysics/ConstraintBatch.cs b/BepuPhysics/ConstraintBatch.cs index 05b5b0302..8db78f710 100644 --- a/BepuPhysics/ConstraintBatch.cs +++ b/BepuPhysics/ConstraintBatch.cs @@ -158,7 +158,9 @@ public ActiveBodyHandleRemover(Bodies bodies, IndexSet* handles) [MethodImpl(MethodImplOptions.AggressiveInlining)] public void LoopBody(int bodyIndex) { - Handles->Remove(Bodies.ActiveSet.IndexToHandle[bodyIndex].Value); + Debug.Assert(Bodies.IsKinematicUnsafe(ref Bodies.ActiveSet.SolverStates[bodyIndex].Inertia.Local) || Handles->Contains(Bodies.ActiveSet.IndexToHandle[bodyIndex].Value), + "Batch referenced handles will not include kinematics, but all referenced dynamics must be present."); + Handles->Unset(Bodies.ActiveSet.IndexToHandle[bodyIndex].Value); } } diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index 19380a735..a0af4b45f 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -846,16 +846,33 @@ internal void AssertConstraintHandleExists(ConstraintHandle handle) /// Index of the batch that the constraint would fit in. /// This is used by the narrowphase's multithreaded constraint adders to locate a spot for a new constraint without requiring a lock. Only after a candidate is located /// do those systems attempt an actual claim, limiting the duration of locks and increasing potential parallelism. - internal unsafe int FindCandidateBatch(Span bodyHandles) + internal unsafe int FindCandidateBatch(CollidablePair collidablePair) { ref var set = ref ActiveSet; GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); - Span blockingBodyHandlesAllocation = stackalloc BodyHandle[bodyHandles.Length]; - var blockingBodyHandles = MemoryMarshal.Cast(GetBlockingBodyHandles(bodyHandles, blockingBodyHandlesAllocation)); - for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) + var aIsDynamic = collidablePair.A.Mobility == Collidables.CollidableMobility.Dynamic; + if (aIsDynamic && collidablePair.B.Mobility == Collidables.CollidableMobility.Dynamic) { - if (batchReferencedHandles[batchIndex].CanFit(blockingBodyHandles)) - return batchIndex; + //Both collidables are dynamic. + var a = collidablePair.A.BodyHandle.Value; + var b = collidablePair.B.BodyHandle.Value; + for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) + { + if (!batchReferencedHandles[batchIndex].Contains(a) && !batchReferencedHandles[batchIndex].Contains(b)) + return batchIndex; + } + } + else + { + //Only one collidable is dynamic. Statics and kinematics will not block batch containment. + Debug.Assert(aIsDynamic || collidablePair.B.Mobility == Collidables.CollidableMobility.Dynamic, + "Constraints can only be created when at least one body in the pair is dynamic."); + var dynamicHandle = (aIsDynamic ? collidablePair.A.BodyHandle : collidablePair.B.BodyHandle).Value; + for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) + { + if (!batchReferencedHandles[batchIndex].Contains(dynamicHandle)) + return batchIndex; + } } //No synchronized batch worked. Either there's a fallback batch or there aren't yet enough batches to warrant a fallback batch and none of the existing batches could fit the handles. return synchronizedBatchCount; diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index bffff3559..9d914271a 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -402,10 +402,10 @@ void SolveStep2Worker2(int workerIndex) } integrateConstrainedKinematicsStage.SubstepIndex = substepIndex; //Note that we do not invoke velocity integration on the first substep if kinematics do not need velocity integration. - if (substepIndex > 1 || PoseIntegrator.Callbacks.IntegrateVelocityForKinematics) + if (substepIndex > 0 || PoseIntegrator.Callbacks.IntegrateVelocityForKinematics) + { ExecuteMainStage(ref integrateConstrainedKinematicsStage, workerIndex, kinematicIntegrationWorkerStart, ref substepContext.Stages[1], syncOffsetToPreviousSubstep, ref syncIndex); - else - ++syncIndex; //Don't want to desync sync indices just because we aren't running a stage. + } warmstartStage.SubstepIndex = substepIndex; for (int batchIndex = 0; batchIndex < syncStagesPerWarmStartOrSolve; ++batchIndex) { diff --git a/Demos/Demo.cs b/Demos/Demo.cs index 5f3790da3..37ebe5190 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -62,7 +62,7 @@ public virtual void Update(Window window, Camera camera, Input input, float dt) //fully decouple simulation and rendering rates across different threads. //(In either case, you'd also want to interpolate or extrapolate simulation results during rendering for smoothness.) //Note that taking steps of variable length can reduce stability. Gradual or one-off changes can work reasonably well. - Simulation.Timestep(1 / 60f); + Simulation.Timestep(1 / 60f, ThreadDispatcher); ////Here's an example of how it would look to use more frequent updates, but still with a fixed amount of time simulated per update call: //const float timeToSimulate = 1 / 60f; From e3ef50743b93092b5fec118795e42162188b9614 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 29 Oct 2021 17:33:40 -0500 Subject: [PATCH 247/947] Fixed zero kinematic divby0. --- BepuPhysics/Solver_SubsteppingSolve2.cs | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index 9d914271a..32c94fe55 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -522,17 +522,17 @@ void SolveStep2Worker2(int workerIndex) Buffer BuildKinematicIntegrationWorkBlocks(int minimumBlockSizeInBundles, int maximumBlockSizeInBundles, int targetBlockCount) { var bundleCount = BundleIndexing.GetBundleCount(ConstrainedKinematicHandles.Count); - var targetBundlesPerBlock = bundleCount / targetBlockCount; - if (targetBundlesPerBlock < minimumBlockSizeInBundles) - targetBundlesPerBlock = minimumBlockSizeInBundles; - if (targetBundlesPerBlock > maximumBlockSizeInBundles) - targetBundlesPerBlock = maximumBlockSizeInBundles; - var blockCount = (bundleCount + targetBundlesPerBlock - 1) / targetBundlesPerBlock; - var bundlesPerBlock = bundleCount / blockCount; - var remainder = bundleCount - bundlesPerBlock * blockCount; - var previousEnd = 0; - if (blockCount > 0) + if (bundleCount > 0) { + var targetBundlesPerBlock = bundleCount / targetBlockCount; + if (targetBundlesPerBlock < minimumBlockSizeInBundles) + targetBundlesPerBlock = minimumBlockSizeInBundles; + if (targetBundlesPerBlock > maximumBlockSizeInBundles) + targetBundlesPerBlock = maximumBlockSizeInBundles; + var blockCount = (bundleCount + targetBundlesPerBlock - 1) / targetBundlesPerBlock; + var bundlesPerBlock = bundleCount / blockCount; + var remainder = bundleCount - bundlesPerBlock * blockCount; + var previousEnd = 0; pool.Take(blockCount, out Buffer workBlocks); for (int i = 0; i < blockCount; ++i) { From 775c25e440a3940ac7f3720baa4b83445006e34b Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 29 Oct 2021 20:23:26 -0500 Subject: [PATCH 248/947] Runs incompletely. Fixed solver work scheduling for new stage; always active simulations now work as expected. Any sleeping instantly crashes. --- BepuPhysics/Solver_SubsteppingSolve2.cs | 33 +++++++++++++++++-------- 1 file changed, 23 insertions(+), 10 deletions(-) diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index 32c94fe55..f7f286e95 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -271,7 +271,7 @@ void ExecuteWorkerStage(ref TStageFunction stageFunction, int wo //debugStageWorkBlocksCompleted[syncIndex - 1][workerIndex] = locallyCompletedCount; //if (workerIndex == 3) //{ - // Console.WriteLine($"Worker {workerIndex} completed {locallyCompletedCount / (double)claims.Length:G2} ({locallyCompletedCount} of {claims.Length})."); + //Console.WriteLine($"Worker {workerIndex}, sync index {syncIndex} completed {locallyCompletedCount / (double)claims.Length:G2} ({locallyCompletedCount} of {claims.Length})."); //} } @@ -292,11 +292,13 @@ void ExecuteMainStage(ref TStageFunction stageFunction, int work if (availableBlocksCount == 1) { + //Console.WriteLine($"Main thread is executing {syncIndex} by itself; stage function: {stageFunction.GetType().Name}"); //There is only one work block available. There's no reason to notify other threads about it or do any claims management; just execute it sequentially. stageFunction.Execute(this, stage.WorkBlockStartIndex, workerIndex); } else { + //Console.WriteLine($"Main thread is requesting workers begin for sync index {syncIndex}; stage function: {stageFunction.GetType().Name}"); //Write the new stage index so other spinning threads will begin work on it. Volatile.Write(ref substepContext.SyncIndex, syncIndex); ExecuteWorkerStage(ref stageFunction, workerIndex, workerStart, stage.WorkBlockStartIndex, ref stage.Claims, previousSyncIndexOffset, syncIndex, ref substepContext.CompletedWorkBlockCount); @@ -400,12 +402,20 @@ void SolveStep2Worker2(int workerIndex) { ExecuteMainStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStart, ref substepContext.Stages[0], syncOffsetToPreviousSubstep, ref syncIndex); } - integrateConstrainedKinematicsStage.SubstepIndex = substepIndex; + else + { + syncIndex++; + } //Note that we do not invoke velocity integration on the first substep if kinematics do not need velocity integration. if (substepIndex > 0 || PoseIntegrator.Callbacks.IntegrateVelocityForKinematics) { + integrateConstrainedKinematicsStage.SubstepIndex = substepIndex; ExecuteMainStage(ref integrateConstrainedKinematicsStage, workerIndex, kinematicIntegrationWorkerStart, ref substepContext.Stages[1], syncOffsetToPreviousSubstep, ref syncIndex); } + else + { + syncIndex++; + } warmstartStage.SubstepIndex = substepIndex; for (int batchIndex = 0; batchIndex < syncStagesPerWarmStartOrSolve; ++batchIndex) { @@ -458,6 +468,11 @@ void SolveStep2Worker2(int workerIndex) int latestCompletedSyncIndex = 0; int syncIndexInSubstep = -1; int substepIndex = 0; + + //Inactive stages in the first substep (incremental contact updates and kinematic velocity integration) will have 0 in their claims buffer. + //Using a longer offset puts them at 0 like they're supposed to be. + var syncOffsetToPreviousSubstepForSecondSubstep = syncOffsetToPreviousSubstep + (PoseIntegrator.Callbacks.IntegrateVelocityForKinematics ? 1 : 2); + while (true) { var spinWait = new LocalSpinWait(); @@ -479,10 +494,9 @@ void SolveStep2Worker2(int workerIndex) syncIndexInSubstep += syncStepsSinceLast; while (true) { - var stageCountInSubstep = substepIndex > 0 ? syncOffsetToPreviousSubstep : baseStageCountInSubstep; - if (syncIndexInSubstep >= stageCountInSubstep) + if (syncIndexInSubstep >= syncOffsetToPreviousSubstep) { - syncIndexInSubstep -= stageCountInSubstep; + syncIndexInSubstep -= syncOffsetToPreviousSubstep; ++substepIndex; } else @@ -490,19 +504,18 @@ void SolveStep2Worker2(int workerIndex) break; } } - //If it's the first substep index, there's no incremental update, so we jump straight into it. - var stageIndex = substepIndex == 0 ? syncIndexInSubstep + 1 : syncIndexInSubstep; + //Console.WriteLine($"Worker working on sync index {syncIndex}, sync index in substep: {syncIndexInSubstep}"); //Note that we're going to do a compare exchange that prevents any claim on work blocks that *arent* of the previous sync index, which means we need the previous sync index. //Storing that in a reliable way is annoying, so we derive it from syncIndex. - ref var stage = ref substepContext.Stages[stageIndex]; + ref var stage = ref substepContext.Stages[syncIndexInSubstep]; switch (stage.StageType) { case SolverStageType.IncrementalUpdate: - ExecuteWorkerStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStart, 0, ref stage.Claims, syncOffsetToPreviousSubstep, syncIndex, ref substepContext.CompletedWorkBlockCount); + ExecuteWorkerStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStart, 0, ref stage.Claims, substepIndex == 1 ? syncOffsetToPreviousSubstepForSecondSubstep : syncOffsetToPreviousSubstep, syncIndex, ref substepContext.CompletedWorkBlockCount); break; case SolverStageType.IntegrateConstrainedKinematics: integrateConstrainedKinematicsStage.SubstepIndex = substepIndex; - ExecuteWorkerStage(ref integrateConstrainedKinematicsStage, workerIndex, kinematicIntegrationWorkerStart, 0, ref stage.Claims, syncOffsetToPreviousSubstep, syncIndex, ref substepContext.CompletedWorkBlockCount); + ExecuteWorkerStage(ref integrateConstrainedKinematicsStage, workerIndex, kinematicIntegrationWorkerStart, 0, ref stage.Claims, substepIndex == 1 ? syncOffsetToPreviousSubstepForSecondSubstep : syncOffsetToPreviousSubstep, syncIndex, ref substepContext.CompletedWorkBlockCount); break; case SolverStageType.WarmStart: warmstartStage.SubstepIndex = substepIndex; From ce6e9eab6a7c0537c40d9d110f0cb31aa5c10597 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 30 Oct 2021 14:06:33 -0500 Subject: [PATCH 249/947] Fixed bad stage claim slicing. --- BepuPhysics/Solver_SubsteppingSolve2.cs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index f7f286e95..9d451ce48 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -597,12 +597,12 @@ protected void ExecuteMultithreaded2(float dt, IThreadDispatcher threadDispatche pool.Take(totalClaimCount, out var claims); claims.Clear(0, claims.Length); substepContext.Stages[0] = new(claims.Slice(incrementalBlocks.Count), 0, SolverStageType.IncrementalUpdate); - substepContext.Stages[1] = new(claims.Slice(substepContext.KinematicIntegrationBlocks.Length), 0, SolverStageType.IntegrateConstrainedKinematics); + substepContext.Stages[1] = new(claims.Slice(incrementalBlocks.Count, substepContext.KinematicIntegrationBlocks.Length), 0, SolverStageType.IntegrateConstrainedKinematics); //Note that we create redundant stages that share the same workblock targets and claims buffers. //This is just to make indexing a little simpler during the multithreaded work. int targetStageIndex = 2; //Warm start. - int claimStart = incrementalBlocks.Count; + int claimStart = incrementalBlocks.Count + substepContext.KinematicIntegrationBlocks.Length; for (int batchIndex = 0; batchIndex < stagesPerIteration; ++batchIndex) { var stageIndex = targetStageIndex++; From 46d5111f5bac59cf732aea99df2c47196bd6fea9 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 30 Oct 2021 14:06:50 -0500 Subject: [PATCH 250/947] Fixed unmasked constraint index accesses. --- BepuPhysics/Constraints/TypeProcessor.cs | 2 +- DemoRenderer/Constraints/ConstraintLineExtractor.cs | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index d927ed413..4961ad5ab 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -891,7 +891,7 @@ ref Buffer.Get(ref sourceTypeBatch.AccumulatedImpulses, sou var offset = 0; for (int j = 0; j < bodiesPerConstraint; ++j) { - Unsafe.Add(ref targetReferencesLaneStart, offset) = activeBodySet.IndexToHandle[Unsafe.Add(ref sourceReferencesLaneStart, offset)].Value; + Unsafe.Add(ref targetReferencesLaneStart, offset) = activeBodySet.IndexToHandle[Unsafe.Add(ref sourceReferencesLaneStart, offset) & Bodies.BodyIndexMask].Value; offset += Vector.Count; } } diff --git a/DemoRenderer/Constraints/ConstraintLineExtractor.cs b/DemoRenderer/Constraints/ConstraintLineExtractor.cs index a076fa50f..2da80bcae 100644 --- a/DemoRenderer/Constraints/ConstraintLineExtractor.cs +++ b/DemoRenderer/Constraints/ConstraintLineExtractor.cs @@ -58,7 +58,7 @@ public unsafe override void ExtractLines(Bodies bodies, int setIndex, ref TypeBa for (int j = 0; j < bodyCount; ++j) { //Active set constraint body references refer directly to the body index. - bodyIndices[j] = GatherScatter.Get(ref Unsafe.Add(ref firstReference, j), innerIndex); + bodyIndices[j] = GatherScatter.Get(ref Unsafe.Add(ref firstReference, j), innerIndex) & Bodies.BodyIndexMask; } extractor.ExtractLines(ref GatherScatter.GetOffsetInstance(ref prestepBundle, innerIndex), setIndex, bodyIndices, bodies, ref tint, ref lines); } @@ -80,7 +80,7 @@ public unsafe override void ExtractLines(Bodies bodies, int setIndex, ref TypeBa //Inactive constraints store body references in the form of handles, so we have to follow the indirection. var bodyHandle = GatherScatter.Get(ref Unsafe.Add(ref firstReference, j), innerIndex); Debug.Assert(bodies.HandleToLocation[bodyHandle].SetIndex == setIndex); - bodyIndices[j] = bodies.HandleToLocation[bodyHandle].Index; + bodyIndices[j] = bodies.HandleToLocation[bodyHandle].Index & Bodies.BodyIndexMask; } extractor.ExtractLines(ref GatherScatter.GetOffsetInstance(ref prestepBundle, innerIndex), setIndex, bodyIndices, bodies, ref tint, ref lines); } From 436514957a7a918fe9191a439b9e20141eed1180 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 30 Oct 2021 19:13:18 -0500 Subject: [PATCH 251/947] Main thread now handles second substep claim offsetting properly. --- BepuPhysics/Solver_SubsteppingSolve2.cs | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index 9d451ce48..39bf0693c 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -392,6 +392,11 @@ void SolveStep2Worker2(int workerIndex) var syncOffsetToPreviousClaimOnBatchForWarmStart = syncStagesPerWarmStartOrSolve + 2; //For solves, there is no incremental update in the way. var syncOffsetToPreviousClaimOnBatchForSolve = syncStagesPerWarmStartOrSolve; + + //Inactive stages in the first substep (incremental contact updates and kinematic velocity integration) will have 0 in their claims buffer. + //Using a longer offset puts them at 0 like they're supposed to be. + var syncOffsetToPreviousSubstepForSecondSubstep = syncOffsetToPreviousSubstep + (PoseIntegrator.Callbacks.IntegrateVelocityForKinematics ? 1 : 2); + if (workerIndex == 0) { //This is the main 'orchestrator' thread. It tracks execution progress and notifies other threads that's it's time to work. @@ -400,7 +405,7 @@ void SolveStep2Worker2(int workerIndex) { if (substepIndex > 0) { - ExecuteMainStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStart, ref substepContext.Stages[0], syncOffsetToPreviousSubstep, ref syncIndex); + ExecuteMainStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStart, ref substepContext.Stages[0], substepIndex == 1 ? syncOffsetToPreviousSubstepForSecondSubstep : syncOffsetToPreviousSubstep, ref syncIndex); } else { @@ -410,7 +415,7 @@ void SolveStep2Worker2(int workerIndex) if (substepIndex > 0 || PoseIntegrator.Callbacks.IntegrateVelocityForKinematics) { integrateConstrainedKinematicsStage.SubstepIndex = substepIndex; - ExecuteMainStage(ref integrateConstrainedKinematicsStage, workerIndex, kinematicIntegrationWorkerStart, ref substepContext.Stages[1], syncOffsetToPreviousSubstep, ref syncIndex); + ExecuteMainStage(ref integrateConstrainedKinematicsStage, workerIndex, kinematicIntegrationWorkerStart, ref substepContext.Stages[1], substepIndex == 1 ? syncOffsetToPreviousSubstepForSecondSubstep : syncOffsetToPreviousSubstep, ref syncIndex); } else { @@ -469,10 +474,6 @@ void SolveStep2Worker2(int workerIndex) int syncIndexInSubstep = -1; int substepIndex = 0; - //Inactive stages in the first substep (incremental contact updates and kinematic velocity integration) will have 0 in their claims buffer. - //Using a longer offset puts them at 0 like they're supposed to be. - var syncOffsetToPreviousSubstepForSecondSubstep = syncOffsetToPreviousSubstep + (PoseIntegrator.Callbacks.IntegrateVelocityForKinematics ? 1 : 2); - while (true) { var spinWait = new LocalSpinWait(); @@ -649,7 +650,10 @@ protected void ExecuteMultithreaded2(float dt, IThreadDispatcher threadDispatche //While we could be a little more aggressive about culling work with this condition, it doesn't matter much. Have to do it for correctness; worker relies on it. if (ActiveSet.Batches.Count > 0) + { + //workDelegate(0); threadDispatcher.DispatchWorkers(workDelegate); + } //pool.Take(syncCount, out var availableCountPerSync); //var syncIndex = 0; From 5ad8991bcc3367e87f4f7823159f0822008b7143 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 31 Oct 2021 19:46:13 -0500 Subject: [PATCH 252/947] Runs, but sleeping will still break stuff. Got rid of bad asserts in constraintremover, update EnumerateConnectedBodyIndices APIs to be more explicit about what data is being reported (raw vs processed index), fixed some batch compressor bugs with kinematics, fixed some incorrect usages of kinematic flags, fixed ValidateExistingHandles for kinematics, added some more handyenumerators, fixed some body index enumeration in islandscaffold/islandsleeper, fixed a long-standing stack corruption bug in fallback batch removal (double enumeration!) and updated it for kinematics change, fixed solver claim indexing bug, and made a very large commit. --- BepuPhysics/BatchCompressor.cs | 23 ++- BepuPhysics/Bodies.cs | 25 ++-- BepuPhysics/Bodies_GatherScatter.cs | 2 +- BepuPhysics/BodyReference.cs | 2 +- .../CollisionDetection/ConstraintRemover.cs | 12 +- .../CollisionDetection/FreshnessChecker.cs | 6 +- BepuPhysics/ConstraintBatch.cs | 13 +- .../Constraints/FourBodyTypeProcessor.cs | 10 +- .../Constraints/OneBodyTypeProcessor.cs | 4 +- .../Constraints/ThreeBodyTypeProcessor.cs | 8 +- .../Constraints/TwoBodyTypeProcessor.cs | 10 +- BepuPhysics/Constraints/TypeProcessor.cs | 14 +- .../EmbeddedSubsteppingTimestepper2.cs | 3 +- BepuPhysics/HandyEnumerators.cs | 66 ++++++++- BepuPhysics/IslandScaffold.cs | 26 ++-- BepuPhysics/IslandSleeper.cs | 15 +- BepuPhysics/SequentialFallbackBatch.cs | 39 ++--- BepuPhysics/Solver.cs | 140 ++++++++++++++---- BepuPhysics/Solver_SubsteppingSolve2.cs | 20 ++- .../SpecializedTests/SimulationScrambling.cs | 12 +- 20 files changed, 300 insertions(+), 150 deletions(-) diff --git a/BepuPhysics/BatchCompressor.cs b/BepuPhysics/BatchCompressor.cs index 1c0767a34..2cc9f7d3a 100644 --- a/BepuPhysics/BatchCompressor.cs +++ b/BepuPhysics/BatchCompressor.cs @@ -128,14 +128,14 @@ void AnalysisWorker(int workerIndex) [MethodImpl(MethodImplOptions.AggressiveInlining)] private unsafe void TryToFindBetterBatchForConstraint( - BufferPool pool, ref QuickList compressions, ref TypeBatch typeBatch, Span bodyHandlesSpan, ref ActiveConstraintBodyHandleCollector handleAccumulator, TypeProcessor typeProcessor, int constraintIndex) + BufferPool pool, ref QuickList compressions, ref TypeBatch typeBatch, int* bodyHandles, ref ActiveConstraintDynamicBodyHandleCollector handleAccumulator, TypeProcessor typeProcessor, int constraintIndex) { - handleAccumulator.Index = 0; - typeProcessor.EnumerateConnectedBodyIndices(ref typeBatch, constraintIndex, ref handleAccumulator); + handleAccumulator.Count = 0; + typeProcessor.EnumerateConnectedRawBodyReferences(ref typeBatch, constraintIndex, ref handleAccumulator); + var dynamicBodyHandles = new Span(bodyHandles, handleAccumulator.Count); for (int batchIndex = nextBatchIndex - 1; batchIndex >= 0; --batchIndex) { - //The batch index will never be the fallback batch, since the fallback batch is the very last batch (if it exists at all). So uses of batch referenced handles is safe. - if (Solver.batchReferencedHandles[batchIndex].CanFit(bodyHandlesSpan)) + if (Solver.batchReferencedHandles[batchIndex].CanFit(dynamicBodyHandles)) { compressions.Add(new Compression { ConstraintHandle = typeBatch.IndexToHandle[constraintIndex], TargetBatch = batchIndex }, pool); return; @@ -156,11 +156,10 @@ unsafe void DoJob(ref AnalysisRegion region, int workerIndex, BufferPool pool) //Each job only works on a subset of a single type batch. var bodiesPerConstraint = typeProcessor.BodiesPerConstraint; var bodyHandles = stackalloc int[bodiesPerConstraint]; - var bodyHandlesSpan = new Span(bodyHandles, bodiesPerConstraint); - ActiveConstraintBodyHandleCollector handleAccumulator; + ActiveConstraintDynamicBodyHandleCollector handleAccumulator; handleAccumulator.Bodies = Bodies; handleAccumulator.Handles = bodyHandles; - handleAccumulator.Index = 0; + handleAccumulator.Count = 0; if (nextBatchIndex == Solver.FallbackBatchThreshold) { for (int i = region.StartIndexInTypeBatch; i < region.EndIndexInTypeBatch; ++i) @@ -169,14 +168,14 @@ unsafe void DoJob(ref AnalysisRegion region, int workerIndex, BufferPool pool) //Not all constraint slots up to the typeBatch.ConstraintCount are guaranteed to actually exist. It's potentially sparse. //Just skip them. if (typeBatch.IndexToHandle[i].Value >= 0) - TryToFindBetterBatchForConstraint(pool, ref compressions, ref typeBatch, bodyHandlesSpan, ref handleAccumulator, typeProcessor, i); + TryToFindBetterBatchForConstraint(pool, ref compressions, ref typeBatch, bodyHandles, ref handleAccumulator, typeProcessor, i); } } else { for (int i = region.StartIndexInTypeBatch; i < region.EndIndexInTypeBatch; ++i) { - TryToFindBetterBatchForConstraint(pool, ref compressions, ref typeBatch, bodyHandlesSpan, ref handleAccumulator, typeProcessor, i); + TryToFindBetterBatchForConstraint(pool, ref compressions, ref typeBatch, bodyHandles, ref handleAccumulator, typeProcessor, i); } } } @@ -211,8 +210,8 @@ private unsafe void ApplyCompression(int sourceBatchIndex, ref ConstraintBatch s ActiveConstraintBodyHandleCollector handleAccumulator; handleAccumulator.Bodies = Bodies; handleAccumulator.Handles = bodyHandles; - handleAccumulator.Index = 0; - Solver.EnumerateConnectedBodies(compression.ConstraintHandle, ref handleAccumulator); + handleAccumulator.Count = 0; + Solver.EnumerateActiveDynamicConnectedBodyIndices(compression.ConstraintHandle, ref handleAccumulator); if (!Solver.batchReferencedHandles[compression.TargetBatch].CanFit(new Span(bodyHandles, typeProcessor.BodiesPerConstraint))) { //Another compression from the fallback batch has blocked this compression. diff --git a/BepuPhysics/Bodies.cs b/BepuPhysics/Bodies.cs index 301818eea..4b4c2a68b 100644 --- a/BepuPhysics/Bodies.cs +++ b/BepuPhysics/Bodies.cs @@ -285,7 +285,7 @@ public unsafe static bool IsKinematic(BodyInertia inertia) /// This is not exposed by default because of the risk of a non-obvious GC hole. /// It exists because it's a mildly more convenient form than the pointer overload, and every use within the engine references only pinned data. [MethodImpl(MethodImplOptions.AggressiveInlining)] - internal unsafe static bool IsKinematicUnsafe(ref BodyInertia inertia) + internal unsafe static bool IsKinematicUnsafeGCHole(ref BodyInertia inertia) { return IsKinematic((BodyInertia*)Unsafe.AsPointer(ref inertia)); } @@ -348,15 +348,12 @@ public unsafe static bool HasLockedInertia(Symmetric3x3* inertia) private struct ConnectedDynamicCounter : IForEach { - public Bodies Bodies; public int DynamicCount; [MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe void LoopBody(int bodyIndex) { - //The solver's connected bodies enumeration directly provides the constraint-stored reference, which is an index in the active set for active constraints and a handle for inactive constraints. - //We forced the dynamic active at the beginning of BecomeKinematic, so we don't have to worry about the inactive side of things. - if (!IsKinematicUnsafe(ref Bodies.ActiveSet.SolverStates[bodyIndex].Inertia.Local)) - ++DynamicCount; + //The enumerator is only called if the body is dynamic, so just increment. + ++DynamicCount; } } @@ -381,12 +378,11 @@ void UpdateForKinematicStateChange(BodyHandle handle, ref BodyMemoryLocation loc { ref var constraints = ref set.Constraints[location.Index]; ConnectedDynamicCounter enumerator; - enumerator.Bodies = this; for (int i = 0; i < constraints.Count; ++i) { ref var constraint = ref constraints[i]; enumerator.DynamicCount = 0; - solver.EnumerateConnectedBodies(constraint.ConnectingConstraintHandle, ref enumerator); + solver.EnumerateActiveDynamicConnectedBodyIndices(constraint.ConnectingConstraintHandle, ref enumerator); if (enumerator.DynamicCount == 0) { //This constraint connects only kinematic bodies; keeping it in the solver would cause a singularity. @@ -610,13 +606,14 @@ struct ActiveConstraintBodyHandleEnumerator : IForEach wh public int SourceBodyIndex; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void LoopBody(int connectedBodyIndex) + public void LoopBody(int encodedBodyIndex) { - if (SourceBodyIndex != connectedBodyIndex) + var bodyIndex = encodedBodyIndex & Bodies.BodyIndexMask; + if (SourceBodyIndex != bodyIndex) { //This enumerator is associated with the public connected bodies enumerator function. The user supplies a handle and expects handles in return, so we //must convert the solver-provided indices to handles. - InnerEnumerator.LoopBody(bodies.ActiveSet.IndexToHandle[connectedBodyIndex]); + InnerEnumerator.LoopBody(bodies.ActiveSet.IndexToHandle[bodyIndex]); } } @@ -661,7 +658,7 @@ internal void EnumerateConnectedBodyIndices(int activeBodyIndex, re //Non-reversed iteration would result in skipped elements if the loop body removed anything. This relies on convention; any remover should be aware of this order. for (int i = list.Count - 1; i >= 0; --i) { - solver.EnumerateConnectedBodies(list[i].ConnectingConstraintHandle, ref constraintBodiesEnumerator); + solver.EnumerateActiveConnectedBodyIndices(list[i].ConnectingConstraintHandle, ref constraintBodiesEnumerator); } //Note that we have to assume the enumerator contains state mutated by the internal loop bodies. //If it's a value type, those mutations won't be reflected in the original reference. @@ -694,7 +691,7 @@ public void EnumerateConnectedBodies(BodyHandle bodyHandle, ref TEn for (int i = list.Count - 1; i >= 0; --i) { - solver.EnumerateConnectedBodies(list[i].ConnectingConstraintHandle, ref constraintBodiesEnumerator); + solver.EnumerateConnectedRawBodyReferences(list[i].ConnectingConstraintHandle, ref constraintBodiesEnumerator); } enumerator = constraintBodiesEnumerator.InnerEnumerator; } @@ -707,7 +704,7 @@ public void EnumerateConnectedBodies(BodyHandle bodyHandle, ref TEn for (int i = list.Count - 1; i >= 0; --i) { - solver.EnumerateConnectedBodies(list[i].ConnectingConstraintHandle, ref constraintBodiesEnumerator); + solver.EnumerateConnectedRawBodyReferences(list[i].ConnectingConstraintHandle, ref constraintBodiesEnumerator); } enumerator = constraintBodiesEnumerator.InnerEnumerator; } diff --git a/BepuPhysics/Bodies_GatherScatter.cs b/BepuPhysics/Bodies_GatherScatter.cs index c2b78d7e4..c3dfa8295 100644 --- a/BepuPhysics/Bodies_GatherScatter.cs +++ b/BepuPhysics/Bodies_GatherScatter.cs @@ -148,7 +148,7 @@ unsafe static void FallbackGatherInertia(SolverState* states, Vector baseIn /// Constraint body index references greater than a given unsigned value are either kinematic (1<<30 set) or correspond to an empty lane (1<<31 set). /// public const uint DynamicLimit = KinematicMask; - public const uint BodyIndexMetadataMask = (1u << DoesntExistFlagIndex) | (1u << KinematicFlagIndex); + public const uint BodyIndexMetadataMask = (1u << DoesntExistFlagIndex) | KinematicMask; public const int BodyIndexMask = (int)~BodyIndexMetadataMask; //[MethodImpl(MethodImplOptions.AggressiveInlining)] diff --git a/BepuPhysics/BodyReference.cs b/BepuPhysics/BodyReference.cs index 3ea8619fe..70b748f99 100644 --- a/BepuPhysics/BodyReference.cs +++ b/BepuPhysics/BodyReference.cs @@ -208,7 +208,7 @@ public CollidableReference CollidableReference /// /// Gets whether the body is kinematic, meaning its inverse inertia and mass are all zero. /// - public unsafe bool Kinematic { get { return Bodies.IsKinematicUnsafe(ref LocalInertia); } } + public unsafe bool Kinematic { get { return Bodies.IsKinematicUnsafeGCHole(ref LocalInertia); } } /// /// Gets whether the body has locked inertia, meaning its inverse inertia tensor is zero. diff --git a/BepuPhysics/CollisionDetection/ConstraintRemover.cs b/BepuPhysics/CollisionDetection/ConstraintRemover.cs index ffa35d497..1c049ac7b 100644 --- a/BepuPhysics/CollisionDetection/ConstraintRemover.cs +++ b/BepuPhysics/CollisionDetection/ConstraintRemover.cs @@ -146,8 +146,8 @@ public unsafe void EnqueueForRemoval(ConstraintHandle constraintHandle, Solver s //We have to perform the enumeration here rather than in the later flush. Removals from type batches make enumerating connected body indices a race condition there. ref var typeBatch = ref constraintBatch.TypeBatches[typeBatchIndex.TypeBatch]; var bodyIndices = stackalloc int[bodiesPerConstraint]; - var enumerator = new ReferenceCollector(bodyIndices); - typeProcessor.EnumerateConnectedBodyIndices(ref typeBatch, constraint.IndexInTypeBatch, ref enumerator); + var enumerator = new ActiveConstraintBodyIndexCollector(bodyIndices); + typeProcessor.EnumerateConnectedRawBodyReferences(ref typeBatch, constraint.IndexInTypeBatch, ref enumerator); for (int i = 0; i < bodiesPerConstraint; ++i) { @@ -359,8 +359,8 @@ public void RemoveConstraintsFromBatchReferencedHandles() for (int j = 0; j < removals.Count; ++j) { ref var target = ref removals[j]; - Debug.Assert(solver.batchReferencedHandles[target.BatchIndex].Contains(target.BodyHandle.Value) || bodies.GetBodyReference(target.BodyHandle).Kinematic, - "The batch referenced handles must include all constraint-involved dynamics, but will not include kinematics."); + //Debug.Assert(solver.batchReferencedHandles[target.BatchIndex].Contains(target.BodyHandle.Value) || bodies.GetBodyReference(target.BodyHandle).Kinematic, + // "The batch referenced handles must include all constraint-involved dynamics, but will not include kinematics."); solver.batchReferencedHandles[target.BatchIndex].Unset(target.BodyHandle.Value); } } @@ -381,8 +381,8 @@ public void RemoveConstraintsFromFallbackBatchReferencedHandles() if (solver.ActiveSet.SequentialFallback.Remove(target.BodyIndex, ref allocationIdsToFree)) { //No more constraints for this body in the fallback set; it should not exist in the fallback batch's referenced handles anymore. - Debug.Assert(solver.batchReferencedHandles[target.BatchIndex].Contains(target.BodyHandle.Value) || bodies.GetBodyReference(target.BodyHandle).Kinematic, - "The batch referenced handles must include all constraint-involved dynamics, but will not include kinematics."); + //Debug.Assert(solver.batchReferencedHandles[target.BatchIndex].Contains(target.BodyHandle.Value) || bodies.GetBodyReference(target.BodyHandle).Kinematic, + // "The batch referenced handles must include all constraint-involved dynamics, but will not include kinematics."); solver.batchReferencedHandles[target.BatchIndex].Unset(target.BodyHandle.Value); } } diff --git a/BepuPhysics/CollisionDetection/FreshnessChecker.cs b/BepuPhysics/CollisionDetection/FreshnessChecker.cs index 903d22175..a0a1e037b 100644 --- a/BepuPhysics/CollisionDetection/FreshnessChecker.cs +++ b/BepuPhysics/CollisionDetection/FreshnessChecker.cs @@ -140,12 +140,10 @@ unsafe void PrintRemovalInformation(ConstraintHandle constraintHandle) ref var batch = ref constraintRemover.solver.ActiveSet.Batches[location.BatchIndex]; ref var typeBatch = ref batch.TypeBatches[batch.TypeIndexToTypeBatchIndex[location.TypeId]]; Debug.Assert(typeBatch.TypeId == location.TypeId); - ReferenceCollector enumerator; - enumerator.Index = 0; var typeProcessor = constraintRemover.solver.TypeProcessors[location.TypeId]; var references = stackalloc int[typeProcessor.BodiesPerConstraint]; - enumerator.References = references; - typeProcessor.EnumerateConnectedBodyIndices(ref typeBatch, location.IndexInTypeBatch, ref enumerator); + var enumerator = new ActiveConstraintBodyIndexCollector(references); + typeProcessor.EnumerateConnectedRawBodyReferences(ref typeBatch, location.IndexInTypeBatch, ref enumerator); for (int i = 0; i < typeProcessor.BodiesPerConstraint; ++i) { var bodyHandle = constraintRemover.bodies.ActiveSet.IndexToHandle[references[i]]; diff --git a/BepuPhysics/ConstraintBatch.cs b/BepuPhysics/ConstraintBatch.cs index 8db78f710..ffc96da07 100644 --- a/BepuPhysics/ConstraintBatch.cs +++ b/BepuPhysics/ConstraintBatch.cs @@ -156,11 +156,12 @@ public ActiveBodyHandleRemover(Bodies bodies, IndexSet* handles) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void LoopBody(int bodyIndex) + public void LoopBody(int encodedBodyIndex) { - Debug.Assert(Bodies.IsKinematicUnsafe(ref Bodies.ActiveSet.SolverStates[bodyIndex].Inertia.Local) || Handles->Contains(Bodies.ActiveSet.IndexToHandle[bodyIndex].Value), - "Batch referenced handles will not include kinematics, but all referenced dynamics must be present."); - Handles->Unset(Bodies.ActiveSet.IndexToHandle[bodyIndex].Value); + if (encodedBodyIndex < Bodies.DynamicLimit) + { + Handles->Remove(Bodies.ActiveSet.IndexToHandle[encodedBodyIndex & Bodies.BodyIndexMask].Value); + } } } @@ -186,9 +187,9 @@ public unsafe void RemoveBodyHandlesFromBatchForConstraint(int constraintTypeId, { Debug.Assert(batchIndex <= solver.FallbackBatchThreshold, "This should only be used for non-fallback batches. The body handles set for a fallback batch should be handled by the fallback batch's remove call."); var indexSet = solver.batchReferencedHandles.GetPointer(batchIndex); - var handleRemover = new ActiveBodyHandleRemover(solver.bodies, indexSet); + var handleRemover = new ActiveBodyHandleRemover(solver.bodies, indexSet); var typeBatchIndex = TypeIndexToTypeBatchIndex[constraintTypeId]; - solver.TypeProcessors[constraintTypeId].EnumerateConnectedBodyIndices(ref TypeBatches[typeBatchIndex], indexInTypeBatch, ref handleRemover); + solver.TypeProcessors[constraintTypeId].EnumerateConnectedRawBodyReferences(ref TypeBatches[typeBatchIndex], indexInTypeBatch, ref handleRemover); } public unsafe void Remove(int constraintTypeId, int indexInTypeBatch, bool isFallback, Solver solver) diff --git a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs index c14ec2891..563251e89 100644 --- a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs @@ -75,15 +75,15 @@ public abstract class FourBodyTypeProcessor 4; - public sealed unsafe override void EnumerateConnectedBodyIndices(ref TypeBatch typeBatch, int indexInTypeBatch, ref TEnumerator enumerator) + public sealed unsafe override void EnumerateConnectedRawBodyReferences(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.Get(typeBatch.BodyReferences.Memory, constraintBundleIndex), constraintInnerIndex); //Note that we hold a reference to the indices. That's important if the loop body mutates indices. - enumerator.LoopBody(GatherScatter.GetFirst(ref indices.IndexA) & Bodies.BodyIndexMask); - enumerator.LoopBody(GatherScatter.GetFirst(ref indices.IndexB) & Bodies.BodyIndexMask); - enumerator.LoopBody(GatherScatter.GetFirst(ref indices.IndexC) & Bodies.BodyIndexMask); - enumerator.LoopBody(GatherScatter.GetFirst(ref indices.IndexD) & Bodies.BodyIndexMask); + enumerator.LoopBody(GatherScatter.GetFirst(ref indices.IndexA)); + enumerator.LoopBody(GatherScatter.GetFirst(ref indices.IndexB)); + enumerator.LoopBody(GatherScatter.GetFirst(ref indices.IndexC)); + enumerator.LoopBody(GatherScatter.GetFirst(ref indices.IndexD)); } struct FourBodySortKeyGenerator : ISortKeyGenerator { diff --git a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs index c5835f8e3..a57b8badb 100644 --- a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs @@ -56,10 +56,10 @@ public abstract class OneBodyTypeProcessor 1; - public sealed override void EnumerateConnectedBodyIndices(ref TypeBatch typeBatch, int indexInTypeBatch, ref TEnumerator enumerator) + public sealed unsafe override void EnumerateConnectedRawBodyReferences(ref TypeBatch typeBatch, int indexInTypeBatch, ref TEnumerator enumerator) { BundleIndexing.GetBundleIndices(indexInTypeBatch, out var constraintBundleIndex, out var constraintInnerIndex); - enumerator.LoopBody(GatherScatter.Get(ref Buffer>.Get(ref typeBatch.BodyReferences, constraintBundleIndex), constraintInnerIndex) & Bodies.BodyIndexMask); + enumerator.LoopBody(GatherScatter.Get(ref Buffer>.Get(ref typeBatch.BodyReferences, constraintBundleIndex), constraintInnerIndex)); } diff --git a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs index 7d616581a..6604de5df 100644 --- a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs @@ -66,14 +66,14 @@ public abstract class ThreeBodyTypeProcessor 3; - public sealed unsafe override void EnumerateConnectedBodyIndices(ref TypeBatch typeBatch, int indexInTypeBatch, ref TEnumerator enumerator) + public sealed unsafe override void EnumerateConnectedRawBodyReferences(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.Get(typeBatch.BodyReferences.Memory, constraintBundleIndex), constraintInnerIndex); //Note that we hold a reference to the indices. That's important if the loop body mutates indices. - enumerator.LoopBody(GatherScatter.GetFirst(ref indices.IndexA) & Bodies.BodyIndexMask); - enumerator.LoopBody(GatherScatter.GetFirst(ref indices.IndexB) & Bodies.BodyIndexMask); - enumerator.LoopBody(GatherScatter.GetFirst(ref indices.IndexC) & Bodies.BodyIndexMask); + enumerator.LoopBody(GatherScatter.GetFirst(ref indices.IndexA)); + enumerator.LoopBody(GatherScatter.GetFirst(ref indices.IndexB)); + enumerator.LoopBody(GatherScatter.GetFirst(ref indices.IndexC)); } struct ThreeBodySortKeyGenerator : ISortKeyGenerator { diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 5375d73f8..576af1940 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -72,15 +72,15 @@ public abstract class TwoBodyTypeProcessor 2; - public sealed override void EnumerateConnectedBodyIndices(ref TypeBatch typeBatch, int indexInTypeBatch, ref TEnumerator enumerator) + public sealed override void EnumerateConnectedRawBodyReferences(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.Get(ref typeBatch.BodyReferences, constraintBundleIndex).IndexA, constraintInnerIndex); - ref var indexB = ref Unsafe.Add(ref indexA, Vector.Count); + ref var referenceA = ref GatherScatter.Get(ref Buffer.Get(ref typeBatch.BodyReferences, constraintBundleIndex).IndexA, constraintInnerIndex); + ref var referenceB = ref Unsafe.Add(ref referenceA, Vector.Count); //Note that the variables are ref locals! This is important for correctness, because every execution of LoopBody could result in a swap. //Ref locals aren't the only solution, but if you ever change this, make sure you account for the potential mutation in the enumerator. - enumerator.LoopBody(indexA & Bodies.BodyIndexMask); - enumerator.LoopBody(indexB & Bodies.BodyIndexMask); + enumerator.LoopBody(referenceA); + enumerator.LoopBody(referenceB); } struct TwoBodySortKeyGenerator : ISortKeyGenerator { diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index 4961ad5ab..aa096cccc 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -79,7 +79,17 @@ public void Initialize(int typeId) /// Index of the ConstraintBatch in the solver to copy the constraint into. public unsafe abstract void TransferConstraint(ref TypeBatch typeBatch, int sourceBatchIndex, int indexInTypeBatch, Solver solver, Bodies bodies, int targetBatchIndex); - public abstract void EnumerateConnectedBodyIndices(ref TypeBatch typeBatch, int indexInTypeBatch, ref TEnumerator enumerator) where TEnumerator : IForEach; + /// + /// Enumerates body references in the constraint. + /// For waking constraints, this enumerates body indices including any encoded metadata like whether the body is kinematic. + /// For sleeping constraints, this enumerates body handles. + /// In other words, this reports whatever is stored in the constraint. + /// + /// Type of the enumerator called for each body index in the constraint. + /// Type batch containing the constraint to enumerate. + /// Index of the constraint to enumerate in the type batch. + /// Enumerator called for each body index in the constraint. + public abstract void EnumerateConnectedRawBodyReferences(ref TypeBatch typeBatch, int indexInTypeBatch, ref TEnumerator enumerator) where TEnumerator : IForEach; [Conditional("DEBUG")] protected abstract void ValidateAccumulatedImpulsesSizeInBytes(int sizeInBytes); public unsafe void EnumerateAccumulatedImpulses(ref TypeBatch typeBatch, int indexInTypeBatch, ref TEnumerator enumerator) where TEnumerator : IForEach @@ -683,7 +693,7 @@ public unsafe override void TransferConstraint(ref TypeBatch typeBatch, int sour int bodiesPerConstraint = InternalBodiesPerConstraint; var bodyHandles = stackalloc int[bodiesPerConstraint]; var bodyHandleCollector = new ActiveConstraintBodyHandleCollector(bodies, bodyHandles); - EnumerateConnectedBodyIndices(ref typeBatch, indexInTypeBatch, ref bodyHandleCollector); + EnumerateConnectedRawBodyReferences(ref typeBatch, indexInTypeBatch, ref bodyHandleCollector); Debug.Assert(targetBatchIndex <= solver.FallbackBatchThreshold, "Constraint transfers should never target the fallback batch. Its registered body handles don't block new constraints so attempting to allocate in the same way wouldn't turn out well."); //Allocate a spot in the new batch. Note that it does not change the Handle->Constraint mapping in the Solver; that's important when we call Solver.Remove below. diff --git a/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs b/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs index 7a64696f5..2cc7d138b 100644 --- a/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs +++ b/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs @@ -64,6 +64,7 @@ public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDi simulation.Solver.ValidateConstraintReferenceKinematicity(); simulation.Solver.ValidateConstrainedKinematicsSet(); simulation.Solver.ValidateFallbackBodiesAreDynamic(); + //simulation.Solver.ValidateExistingHandles(); var constrainedBodySet = simulation.Solver.PrepareConstraintIntegrationResponsibilities(SubstepCount, threadDispatcher); simulation.Profiler.Start(simulation.Solver); simulation.Solver.SolveStep2(dt, threadDispatcher); @@ -74,7 +75,7 @@ public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDi simulation.Profiler.End(simulation.PoseIntegrator); simulation.Solver.DisposeConstraintIntegrationResponsibilities(); SubstepsComplete?.Invoke(dt, threadDispatcher); - + simulation.IncrementallyOptimizeDataStructures(threadDispatcher); } } diff --git a/BepuPhysics/HandyEnumerators.cs b/BepuPhysics/HandyEnumerators.cs index 6b05aa334..7df4e3560 100644 --- a/BepuPhysics/HandyEnumerators.cs +++ b/BepuPhysics/HandyEnumerators.cs @@ -11,31 +11,83 @@ namespace BepuPhysics /// /// Collects body handles associated with an active constraint as integers. /// - public unsafe struct ActiveConstraintBodyHandleCollector : IForEach + public unsafe struct ActiveConstraintBodyHandleCollector : IForEach { public Bodies Bodies; public int* Handles; - public int Index; + public int Count; public ActiveConstraintBodyHandleCollector(Bodies bodies, int* handles) { Bodies = bodies; Handles = handles; - Index = 0; + Count = 0; } - public void LoopBody(int bodyIndex) + public void LoopBody(int encodedBodyIndex) { - Handles[Index++] = Bodies.ActiveSet.IndexToHandle[bodyIndex].Value; + //Note that this enumerator is used with prefiltered body indices and with raw body indices. A redundant & isn't much of a concern; lets us share more frequently. + Handles[Count++] = Bodies.ActiveSet.IndexToHandle[encodedBodyIndex & Bodies.BodyIndexMask].Value; } } - public unsafe struct ReferenceCollector : IForEach + /// + /// Collects body handles associated with an active constraint as integers. + /// + public unsafe struct ActiveConstraintDynamicBodyHandleCollector : IForEach + { + public Bodies Bodies; + public int* Handles; + public int Count; + + public ActiveConstraintDynamicBodyHandleCollector(Bodies bodies, int* handles) + { + Bodies = bodies; + Handles = handles; + Count = 0; + } + + public void LoopBody(int encodedBodyIndex) + { + if (encodedBodyIndex < Bodies.DynamicLimit) + { + //Note that this enumerator is used with prefiltered body indices and with raw body indices. A redundant & isn't much of a concern; lets us share more frequently. + Handles[Count++] = Bodies.ActiveSet.IndexToHandle[encodedBodyIndex & Bodies.BodyIndexMask].Value; + } + } + } + + /// + /// Collects body indices associated with an active constraint. Encoded metadata is stripped. + /// + public unsafe struct ActiveConstraintBodyIndexCollector : IForEach + { + public int* Indices; + public int Count; + + public ActiveConstraintBodyIndexCollector(int* indices) + { + Indices = indices; + Count = 0; + } + + public void LoopBody(int encodedBodyIndex) + { + Indices[Count++] = encodedBodyIndex & Bodies.BodyIndexMask; + } + } + + /// + /// Directly reports references as provided by whatever is being enumerated. + /// For example, when used directly with the TypeProcessor's EnumerateConnectedRawBodyReferences, if the constraint is active, this will report encoded body indices. If the constraint is sleeping, this will report body handles. + /// If used with an enumerator that does filtering, the filtered results will be reported unmodified. + /// + public unsafe struct PassthroughReferenceCollector : IForEach { public int* References; public int Index; - public ReferenceCollector(int* references) + public PassthroughReferenceCollector(int* references) { References = references; Index = 0; diff --git a/BepuPhysics/IslandScaffold.cs b/BepuPhysics/IslandScaffold.cs index ca4e642ef..ede51923c 100644 --- a/BepuPhysics/IslandScaffold.cs +++ b/BepuPhysics/IslandScaffold.cs @@ -10,10 +10,10 @@ namespace BepuPhysics unsafe struct ConstraintHandleEnumerator : IForEach { public int* BodyIndices; - public int IndexInConstraint; + public int Count; public void LoopBody(int i) { - BodyIndices[IndexInConstraint++] = i; + BodyIndices[Count++] = i; } } @@ -84,32 +84,30 @@ public unsafe bool TryAdd(ConstraintHandle constraintHandle, int batchIndex, Sol var bodyIndices = stackalloc int[bodiesPerConstraint]; ConstraintHandleEnumerator enumerator; enumerator.BodyIndices = bodyIndices; - enumerator.IndexInConstraint = 0; - typeProcessor.EnumerateConnectedBodyIndices( - ref solver.ActiveSet.Batches[constraintLocation.BatchIndex].GetTypeBatch(constraintLocation.TypeId), - constraintLocation.IndexInTypeBatch, - ref enumerator); - if (batchIndex == solver.FallbackBatchThreshold || ReferencedBodyIndices.CanFit(new Span(enumerator.BodyIndices, bodiesPerConstraint))) + enumerator.Count = 0; + solver.EnumerateActiveDynamicConnectedBodyIndices(constraintHandle, ref enumerator); + var dynamicBodyIndices = new Span(enumerator.BodyIndices, enumerator.Count); + if (batchIndex == solver.FallbackBatchThreshold || ReferencedBodyIndices.CanFit(dynamicBodyIndices)) { ref var typeBatch = ref GetOrCreateTypeBatch(constraintLocation.TypeId, solver, pool); Debug.Assert(typeBatch.TypeId == constraintLocation.TypeId); typeBatch.Handles.Add(constraintHandle.Value, pool); if (batchIndex < solver.FallbackBatchThreshold) { - for (int i = 0; i < bodiesPerConstraint; ++i) + for (int i = 0; i < dynamicBodyIndices.Length; ++i) { - ReferencedBodyIndices.AddUnsafely(enumerator.BodyIndices[i]); + ReferencedBodyIndices.AddUnsafely(dynamicBodyIndices[i]); } } else { //This is the fallback batch, so we need to fill the fallback batch with relevant information. - Span bodyHandles = stackalloc BodyHandle[bodiesPerConstraint]; - for (int i = 0; i < bodyHandles.Length; ++i) + Span dynamicBodyHandles = stackalloc BodyHandle[dynamicBodyIndices.Length]; + for (int i = 0; i < dynamicBodyIndices.Length; ++i) { - bodyHandles[i] = solver.bodies.ActiveSet.IndexToHandle[bodyIndices[i]]; + dynamicBodyHandles[i] = solver.bodies.ActiveSet.IndexToHandle[dynamicBodyIndices[i]]; } - fallbackBatch.AllocateForInactive(bodyHandles, solver.bodies, pool); + fallbackBatch.AllocateForInactive(dynamicBodyHandles, solver.bodies, pool); } return true; } diff --git a/BepuPhysics/IslandSleeper.cs b/BepuPhysics/IslandSleeper.cs index fcd687174..23e277306 100644 --- a/BepuPhysics/IslandSleeper.cs +++ b/BepuPhysics/IslandSleeper.cs @@ -150,7 +150,7 @@ bool EnqueueUnvisitedNeighbors(int bodyIndex, constraintHandles.Add(entry.ConnectingConstraintHandle, pool); consideredConstraints.AddUnsafely(entry.ConnectingConstraintHandle.Value); bodyEnumerator.ConstraintBodyIndices.Count = 0; - solver.EnumerateConnectedBodies(entry.ConnectingConstraintHandle, ref bodyEnumerator); + solver.EnumerateActiveConnectedBodyIndices(entry.ConnectingConstraintHandle, ref bodyEnumerator); for (int j = 0; j < bodyEnumerator.ConstraintBodyIndices.Count; ++j) { var connectedBodyIndex = bodyEnumerator.ConstraintBodyIndices[j]; @@ -549,7 +549,8 @@ unsafe void PrintIsland(ref IslandScaffold island) } } Console.Write($"{constraintCount} constraint handles: "); - ReferenceCollector bodyIndexEnumerator; + PassthroughReferenceCollector bodyIndexEnumerator; + var rawBodyIndices = stackalloc int[4]; var constraintReferencedBodyHandles = new QuickSet>(8, pool); for (int batchIndex = 0; batchIndex < island.Protobatches.Count; ++batchIndex) { @@ -558,21 +559,23 @@ unsafe void PrintIsland(ref IslandScaffold island) { ref var typeBatch = ref batch.TypeBatches[typeBatchIndex]; var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; - var references = stackalloc int[typeProcessor.BodiesPerConstraint]; - bodyIndexEnumerator.References = references; + bodyIndexEnumerator.References = rawBodyIndices; for (int indexInTypeBatch = 0; indexInTypeBatch < typeBatch.Handles.Count; ++indexInTypeBatch) { + Debug.Assert(typeProcessor.BodiesPerConstraint <= 4, + "We assumed a maximum of 4 bodies per constraint when allocating the body indices buffer earlier. " + + "This validation must be updated if that assumption is no longer valid."); var handle = typeBatch.Handles[indexInTypeBatch]; ref var location = ref solver.HandleToConstraint[handle]; Debug.Assert(location.SetIndex == 0); Debug.Assert(location.TypeId == typeBatch.TypeId); ref var solverBatch = ref solver.Sets[0].Batches[location.BatchIndex]; bodyIndexEnumerator.Index = 0; - typeProcessor.EnumerateConnectedBodyIndices( + typeProcessor.EnumerateConnectedRawBodyReferences( ref solverBatch.TypeBatches[solverBatch.TypeIndexToTypeBatchIndex[location.TypeId]], location.IndexInTypeBatch, ref bodyIndexEnumerator); for (int i = 0; i < typeProcessor.BodiesPerConstraint; ++i) { - constraintReferencedBodyHandles.Add(ref bodies.ActiveSet.IndexToHandle[references[i]].Value, pool); + constraintReferencedBodyHandles.Add(ref bodies.ActiveSet.IndexToHandle[rawBodyIndices[i] & Bodies.BodyIndexMask].Value, pool); } Console.Write($"{handle}, "); } diff --git a/BepuPhysics/SequentialFallbackBatch.cs b/BepuPhysics/SequentialFallbackBatch.cs index 24875324a..69ae5dfde 100644 --- a/BepuPhysics/SequentialFallbackBatch.cs +++ b/BepuPhysics/SequentialFallbackBatch.cs @@ -152,18 +152,23 @@ internal unsafe void Remove(Solver solver, BufferPool bufferPool, ref Constraint var typeProcessor = solver.TypeProcessors[typeId]; var bodyCount = typeProcessor.BodiesPerConstraint; var bodyIndices = stackalloc int[bodyCount]; - var enumerator = new ReferenceCollector(bodyIndices); - solver.EnumerateConnectedBodies(constraintHandle, ref enumerator); + var enumerator = new PassthroughReferenceCollector(bodyIndices); var maximumAllocationIdsToFree = 3 + bodyCount * 2; var allocationIdsToRemoveMemory = stackalloc int[maximumAllocationIdsToFree]; var initialSpan = new Buffer(allocationIdsToRemoveMemory, maximumAllocationIdsToFree); var allocationIdsToFree = new QuickList(initialSpan); - typeProcessor.EnumerateConnectedBodyIndices(ref batch.TypeBatches[batch.TypeIndexToTypeBatchIndex[typeId]], indexInTypeBatch, ref enumerator); + typeProcessor.EnumerateConnectedRawBodyReferences(ref batch.TypeBatches[batch.TypeIndexToTypeBatchIndex[typeId]], indexInTypeBatch, ref enumerator); for (int i = 0; i < bodyCount; ++i) { - if (Remove(bodyIndices[i], ref allocationIdsToFree)) + var rawBodyIndex = bodyIndices[i]; + var isDynamic = (rawBodyIndex & Bodies.KinematicMask) == 0; + if (isDynamic) { - fallbackBatchHandles.Remove(solver.bodies.ActiveSet.IndexToHandle[bodyIndices[i]].Value); + var bodyIndex = rawBodyIndex & Bodies.BodyIndexMask; + if (Remove(bodyIndex, ref allocationIdsToFree)) + { + fallbackBatchHandles.Remove(solver.bodies.ActiveSet.IndexToHandle[bodyIndex].Value); + } } } for (int i = 0; i < allocationIdsToFree.Count; ++i) @@ -172,17 +177,6 @@ internal unsafe void Remove(Solver solver, BufferPool bufferPool, ref Constraint } } - [Conditional("DEBUG")] - unsafe static void ValidateBodyConstraintReference(Solver solver, int setIndex, int bodyReference, ConstraintHandle constraintHandle, int expectedIndexInConstraint) - { - ref var constraintLocation = ref solver.HandleToConstraint[constraintHandle.Value]; - Debug.Assert(constraintLocation.SetIndex == setIndex); - Debug.Assert(constraintLocation.BatchIndex == solver.FallbackBatchThreshold, "Should only be working on constraints which are members of the active fallback batch."); - var debugReferences = stackalloc int[solver.TypeProcessors[constraintLocation.TypeId].BodiesPerConstraint]; - var debugBodyReferenceCollector = new ReferenceCollector(debugReferences); - solver.EnumerateConnectedBodies(constraintHandle, ref debugBodyReferenceCollector); - Debug.Assert(debugReferences[expectedIndexInConstraint] == bodyReference, "The constraint's true body references must agree with the fallback batch."); - } [Conditional("DEBUG")] public static unsafe void ValidateSetReferences(Solver solver, int setIndex) { @@ -208,12 +202,19 @@ public static unsafe void ValidateSetReferences(Solver solver, int setIndex) for (int constraintIndex = 0; constraintIndex < typeBatch.ConstraintCount; ++constraintIndex) { var constraintHandle = typeBatch.IndexToHandle[constraintIndex]; - var collector = new ReferenceCollector(connectedBodies); - solver.EnumerateConnectedBodies(constraintHandle, ref collector); + var collector = new PassthroughReferenceCollector(connectedBodies); + if (setIndex == 0) + { + solver.EnumerateActiveDynamicConnectedBodyIndices(constraintHandle, ref collector); + } + else + { + solver.EnumerateConnectedRawBodyReferences(constraintHandle, ref collector); + } for (int i = 0; i < bodiesPerConstraint; ++i) { var localBodyIndex = bodyConstraintCounts.IndexOf(connectedBodies[i]); - Debug.Assert(localBodyIndex >= 0, "Any body referenced by a constraint in the fallback batch should exist within the fallback batch's body listing."); + Debug.Assert(localBodyIndex >= 0, "Any dynamic body referenced by a constraint in the fallback batch should exist within the fallback batch's dynamic body listing."); var count = bodyConstraintCounts.Values[localBodyIndex]; } } diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index a0af4b45f..651882b77 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -302,7 +302,7 @@ unsafe internal void ValidateConstraintReferenceKinematicity() if (encodedBodyIndex > 0) { var kinematicByEncodedIndex = (encodedBodyIndex & Bodies.KinematicMask) != 0; - var kinematicByInertia = Bodies.IsKinematicUnsafe(ref bodies.ActiveSet.SolverStates[encodedBodyIndex & Bodies.BodyIndexMask].Inertia.Local); + var kinematicByInertia = Bodies.IsKinematicUnsafeGCHole(ref bodies.ActiveSet.SolverStates[encodedBodyIndex & Bodies.BodyIndexMask].Inertia.Local); Debug.Assert(kinematicByEncodedIndex == kinematicByInertia, "Constraint reference encoded kinematicity must match actual kinematicity by inertia."); } } @@ -317,7 +317,7 @@ unsafe internal void ValidateConstrainedKinematicsSet() ref var set = ref bodies.ActiveSet; for (int i = 0; i < set.Count; ++i) { - if (Bodies.IsKinematicUnsafe(ref set.SolverStates[i].Inertia.Local) && set.Constraints[i].Count > 0) + if (Bodies.IsKinematicUnsafeGCHole(ref set.SolverStates[i].Inertia.Local) && set.Constraints[i].Count > 0) { Debug.Assert(ConstrainedKinematicHandles.Contains(set.IndexToHandle[i].Value), "Any active kinematic with constraints must appear in the constrained kinematic set."); } @@ -401,7 +401,7 @@ internal unsafe void ValidateFallbackBodiesAreDynamic() { for (int i = 0; i < set.SequentialFallback.dynamicBodyConstraintCounts.Count; ++i) { - Debug.Assert(!Bodies.IsKinematicUnsafe(ref bodies.ActiveSet.SolverStates[set.SequentialFallback.dynamicBodyConstraintCounts.Keys[i]].Inertia.Local), + Debug.Assert(!Bodies.IsKinematicUnsafeGCHole(ref bodies.ActiveSet.SolverStates[set.SequentialFallback.dynamicBodyConstraintCounts.Keys[i]].Inertia.Local), "All ostensibly dynamic bodies tracked by the fallback batch must actually be dynamic."); } for (int i = 0; i < bodies.ActiveSet.Count; ++i) @@ -415,7 +415,7 @@ internal unsafe void ValidateFallbackBodiesAreDynamic() ++fallbackConstraintsForDynamicBody; } } - if (Bodies.IsKinematicUnsafe(ref bodies.ActiveSet.SolverStates[i].Inertia.Local)) + if (Bodies.IsKinematicUnsafeGCHole(ref bodies.ActiveSet.SolverStates[i].Inertia.Local)) { Debug.Assert(fallbackConstraintsForDynamicBody == 0, "Kinematics should not be present in the dynamic bodies referenced by the fallback batch."); } @@ -619,27 +619,42 @@ internal unsafe void ValidateExistingHandles(bool activeOnly = false) for (int i = 0; i < bodies.ActiveSet.Count; ++i) { var handle = bodies.ActiveSet.IndexToHandle[i]; - int expectedCount; - if (handles.Contains(handle.Value)) + int expectedCount = 0; + int bodyReference = i; + if (Bodies.IsKinematicUnsafeGCHole(ref bodies.ActiveSet.SolverStates[i].Inertia.Local)) { - if (batchIndex < FallbackBatchThreshold) + //Kinematic bodies may appear more than once in non-fallback batches, so we have to count how many references to expect. + var constraints = bodies.ActiveSet.Constraints[i]; + for (int constraintForBodyIndex = 0; constraintForBodyIndex < constraints.Count; ++constraintForBodyIndex) { - //A body can only appear in a non-fallback batch at most once. - expectedCount = 1; - } - else - { - //If this is the fallback batch, then the expected count may be more than 1. - var foundBody = ActiveSet.SequentialFallback.dynamicBodyConstraintCounts.TryGetValue(i, out var constraintCountInFallbackBatchForBody); - Debug.Assert(foundBody, "A body was in the fallback batch's referenced handles, so the fallback batch should have a reference for that body."); - expectedCount = foundBody ? constraintCountInFallbackBatchForBody : 0; + if (HandleToConstraint[constraints[constraintForBodyIndex].ConnectingConstraintHandle.Value].BatchIndex == batchIndex) + ++expectedCount; } + bodyReference |= (int)Bodies.KinematicMask; } else { - expectedCount = 0; + if (handles.Contains(handle.Value)) + { + if (batchIndex < FallbackBatchThreshold) + { + //A dynamic body can only appear in a non-fallback batch at most once. + expectedCount = 1; + } + else + { + //If this is the fallback batch, then the expected count may be more than 1. + var foundBody = ActiveSet.SequentialFallback.dynamicBodyConstraintCounts.TryGetValue(i, out var constraintCountInFallbackBatchForBody); + Debug.Assert(foundBody, "A body was in the fallback batch's referenced handles, so the fallback batch should have a reference for that body."); + expectedCount = foundBody ? constraintCountInFallbackBatchForBody : 0; + } + } + else + { + expectedCount = 0; + } } - ValidateBodyReference(i, expectedCount, ref batch); + ValidateBodyReference(bodyReference, expectedCount, ref batch); } //No inactive bodies should be present in the active set solver batch referenced handles. for (int inactiveBodySetIndex = 1; inactiveBodySetIndex < bodies.Sets.Length; ++inactiveBodySetIndex) @@ -655,7 +670,7 @@ internal unsafe void ValidateExistingHandles(bool activeOnly = false) } } //Now, for all sets, validate that constraint and body references to each other are consistent and complete. - ReferenceCollector enumerator; + PassthroughReferenceCollector enumerator; int maximumBodiesPerConstraint = 0; for (int i = 0; i < TypeProcessors.Length; ++i) { @@ -686,7 +701,7 @@ internal unsafe void ValidateExistingHandles(bool activeOnly = false) for (int indexInTypeBatch = 0; indexInTypeBatch < typeBatch.ConstraintCount; ++indexInTypeBatch) { enumerator.Index = 0; - processor.EnumerateConnectedBodyIndices(ref typeBatch, indexInTypeBatch, ref enumerator); + processor.EnumerateConnectedRawBodyReferences(ref typeBatch, indexInTypeBatch, ref enumerator); for (int i = 0; i < processor.BodiesPerConstraint; ++i) { @@ -694,7 +709,7 @@ internal unsafe void ValidateExistingHandles(bool activeOnly = false) int bodyIndex; if (setIndex == 0) { - bodyIndex = constraintBodyReferences[i]; + bodyIndex = constraintBodyReferences[i] & Bodies.BodyIndexMask; } else { @@ -720,10 +735,13 @@ internal unsafe void ValidateExistingHandles(bool activeOnly = false) ref var typeBatch = ref batch.TypeBatches[batch.TypeIndexToTypeBatchIndex[constraintLocation.TypeId]]; var processor = TypeProcessors[typeBatch.TypeId]; enumerator.Index = 0; - processor.EnumerateConnectedBodyIndices(ref typeBatch, constraintLocation.IndexInTypeBatch, ref enumerator); + processor.EnumerateConnectedRawBodyReferences(ref typeBatch, constraintLocation.IndexInTypeBatch, ref enumerator); //Active constraints refer to bodies by index; inactive constraints use handles. int bodyReference = setIndex == 0 ? bodyIndex : bodySet.IndexToHandle[bodyIndex].Value; - Debug.Assert(constraintBodyReferences[constraintList[constraintIndex].BodyIndexInConstraint] == bodyReference, + var bodyReferenceInConstraint = constraintBodyReferences[constraintList[constraintIndex].BodyIndexInConstraint]; + if (setIndex == 0) + bodyReferenceInConstraint &= Bodies.BodyIndexMask; + Debug.Assert(bodyReferenceInConstraint == bodyReference, "If a body refers to a constraint, the constraint should refer to the body."); } } @@ -865,7 +883,7 @@ internal unsafe int FindCandidateBatch(CollidablePair collidablePair) else { //Only one collidable is dynamic. Statics and kinematics will not block batch containment. - Debug.Assert(aIsDynamic || collidablePair.B.Mobility == Collidables.CollidableMobility.Dynamic, + Debug.Assert(aIsDynamic || collidablePair.B.Mobility == Collidables.CollidableMobility.Dynamic, "Constraints can only be created when at least one body in the pair is dynamic."); var dynamicHandle = (aIsDynamic ? collidablePair.A.BodyHandle : collidablePair.B.BodyHandle).Value; for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) @@ -895,7 +913,7 @@ internal unsafe void AllocateInBatch(int targetBatchIndex, ConstraintHandle cons //Include the flag in the body index if it's kinematic. if (Bodies.IsKinematic(bodies.ActiveSet.SolverStates[index].Inertia.Local)) { - index |= 1 << Bodies.KinematicFlagIndex; + index |= (int)Bodies.KinematicMask; ConstrainedKinematicHandles.Add(bodyHandle.Value, pool); } else @@ -943,7 +961,7 @@ unsafe internal Span GetBlockingBodyHandles(Span bodyHan { var location = bodies.HandleToLocation[bodyHandles[i].Value]; Debug.Assert(location.SetIndex == 0); - if (!Bodies.IsKinematicUnsafe(ref solverStates[location.Index].Inertia.Local)) + if (!Bodies.IsKinematicUnsafeGCHole(ref solverStates[location.Index].Inertia.Local)) { allocation[blockingCount++] = bodyHandles[i]; } @@ -1236,7 +1254,7 @@ public void Remove(ConstraintHandle handle) ConstraintGraphRemovalEnumerator enumerator; enumerator.bodies = bodies; enumerator.constraintHandle = handle; - EnumerateConnectedBodies(handle, ref enumerator); + EnumerateActiveConnectedBodyIndices(handle, ref enumerator); pairCache.RemoveReferenceIfContactConstraint(handle, constraintLocation.TypeId); RemoveFromBatch(handle, constraintLocation.BatchIndex, constraintLocation.TypeId, constraintLocation.IndexInTypeBatch); @@ -1425,16 +1443,78 @@ public unsafe float GetAccumulatedImpulseMagnitude(ConstraintHandle constraintHa } /// - /// Enumerates the set of bodies associated with a constraint in order of their references within the constraint. + /// Enumerates the set of body references associated with a constraint in order of their references within the constraint. + /// If the constraint is awake, this will report the raw body index and any encoded metadata, like whether the body is kinematic. + /// If the constraint is asleep, this will report the body handle. + /// + /// Constraint to enumerate. + /// Enumerator to use. + internal void EnumerateConnectedRawBodyReferences(ConstraintHandle constraintHandle, ref TEnumerator enumerator) where TEnumerator : IForEach + { + ref var constraintLocation = ref HandleToConstraint[constraintHandle.Value]; + Debug.Assert(constraintLocation.SetIndex > 0, "This function is intended to be used only with sleeping constraints."); + ref var typeBatch = ref Sets[constraintLocation.SetIndex].Batches[constraintLocation.BatchIndex].GetTypeBatch(constraintLocation.TypeId); + Debug.Assert(constraintLocation.IndexInTypeBatch >= 0 && constraintLocation.IndexInTypeBatch < typeBatch.ConstraintCount, "Bad constraint location; likely some add/remove bug."); + TypeProcessors[constraintLocation.TypeId].EnumerateConnectedRawBodyReferences(ref typeBatch, constraintLocation.IndexInTypeBatch, ref enumerator); + } + struct ActiveBodyIndexEnumerator : IForEach where TWrapped : IForEach + { + public TWrapped Wrapped; + public void LoopBody(int encodedBodyIndex) + { + //This is only used on the active set, so we know we're getting body indices. They include encoded metadata that we can check to see whether the body is kinematic. + Wrapped.LoopBody(encodedBodyIndex & Bodies.BodyIndexMask); + + } + } + + /// + /// Enumerates the set of body indices associated with an active constraint in order of their references within the constraint. + /// + /// Constraint to enumerate. + /// Enumerator to use. + internal void EnumerateActiveConnectedBodyIndices(ConstraintHandle constraintHandle, ref TEnumerator enumerator) where TEnumerator : IForEach + { + ref var constraintLocation = ref HandleToConstraint[constraintHandle.Value]; + Debug.Assert(constraintLocation.SetIndex == 0, "This function is intended to be used only with waking constraints."); + ref var typeBatch = ref Sets[constraintLocation.SetIndex].Batches[constraintLocation.BatchIndex].GetTypeBatch(constraintLocation.TypeId); + Debug.Assert(constraintLocation.IndexInTypeBatch >= 0 && constraintLocation.IndexInTypeBatch < typeBatch.ConstraintCount, "Bad constraint location; likely some add/remove bug."); + ActiveBodyIndexEnumerator wrapper; + wrapper.Wrapped = enumerator; + TypeProcessors[constraintLocation.TypeId].EnumerateConnectedRawBodyReferences(ref typeBatch, constraintLocation.IndexInTypeBatch, ref wrapper); + //Enumeration might have mutated the enumerator; if it's a value type, we need to copy those mutations back into it. + enumerator = wrapper.Wrapped; + } + + struct ActiveDynamicEnumerator : IForEach where TWrapped : IForEach + { + public TWrapped Wrapped; + public void LoopBody(int encodedBodyIndex) + { + //This is only used on the active set, so we know we're getting body indices. They include encoded metadata that we can check to see whether the body is kinematic. + if (encodedBodyIndex < Bodies.DynamicLimit) + { + Wrapped.LoopBody(encodedBodyIndex & Bodies.BodyIndexMask); + } + } + } + + /// + /// Enumerates the set of dynamic body indices associated with an active constraint in order of their references within the constraint. /// /// Constraint to enumerate. /// Enumerator to use. - internal void EnumerateConnectedBodies(ConstraintHandle constraintHandle, ref TEnumerator enumerator) where TEnumerator : IForEach + internal void EnumerateActiveDynamicConnectedBodyIndices(ConstraintHandle constraintHandle, ref TEnumerator enumerator) where TEnumerator : IForEach { ref var constraintLocation = ref HandleToConstraint[constraintHandle.Value]; + Debug.Assert(constraintLocation.SetIndex == 0, "This function is intended to be used only with the active set."); ref var typeBatch = ref Sets[constraintLocation.SetIndex].Batches[constraintLocation.BatchIndex].GetTypeBatch(constraintLocation.TypeId); Debug.Assert(constraintLocation.IndexInTypeBatch >= 0 && constraintLocation.IndexInTypeBatch < typeBatch.ConstraintCount, "Bad constraint location; likely some add/remove bug."); - TypeProcessors[constraintLocation.TypeId].EnumerateConnectedBodyIndices(ref typeBatch, constraintLocation.IndexInTypeBatch, ref enumerator); + ActiveDynamicEnumerator wrapper; + wrapper.Wrapped = enumerator; + TypeProcessors[constraintLocation.TypeId].EnumerateConnectedRawBodyReferences(ref typeBatch, constraintLocation.IndexInTypeBatch, ref wrapper); + //Enumeration might have mutated the enumerator; if it's a value type, we need to copy those mutations back into it. + enumerator = wrapper.Wrapped; } internal void GetSynchronizedBatchCount(out int synchronizedBatchCount, out bool fallbackExists) diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index 39bf0693c..65c850753 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -271,7 +271,14 @@ void ExecuteWorkerStage(ref TStageFunction stageFunction, int wo //debugStageWorkBlocksCompleted[syncIndex - 1][workerIndex] = locallyCompletedCount; //if (workerIndex == 3) //{ - //Console.WriteLine($"Worker {workerIndex}, sync index {syncIndex} completed {locallyCompletedCount / (double)claims.Length:G2} ({locallyCompletedCount} of {claims.Length})."); + //Console.WriteLine($"Worker {workerIndex}, stage {typeof(TStageFunction).Name}, sync index {syncIndex} completed {locallyCompletedCount / (double)claims.Length:G2} ({locallyCompletedCount} of {claims.Length})."); + //} + //for (int i = 0; i < claims.Length; ++i) + //{ + // if (claims[i] != syncIndex) + // { + // Console.WriteLine($"Failed to claim index {i}, claim value is {claims[i]} instead of {syncIndex}, previous claim should have been {previousSyncIndex}, worker start {workerStart}"); + // } //} } @@ -289,7 +296,7 @@ void ExecuteMainStage(ref TStageFunction stageFunction, int work // stageFunction.Execute(this, stage.WorkBlockStartIndex + i, workerIndex); //} //return; - + //Console.WriteLine($"Main executing {typeof(TStageFunction).Name} for sync index {syncIndex}, expected claim {syncIndex - previousSyncIndexOffset}"); if (availableBlocksCount == 1) { //Console.WriteLine($"Main thread is executing {syncIndex} by itself; stage function: {stageFunction.GetType().Name}"); @@ -385,9 +392,8 @@ void SolveStep2Worker2(int workerIndex) //A thread is only allowed to claim a workblock if the claim index for that workblock matches the expected value- which is the claim index it would have from the last time it was executed. //Each thread calculates what that claim index would have been based on the current sync index by subtracting the expected number of sync indices elapsed since last execution. var syncStagesPerWarmStartOrSolve = synchronizedBatchCount; - var baseStageCountInSubstep = syncStagesPerWarmStartOrSolve * (1 + IterationCount); //All warmstarts and solves, plus an incremental contact update. First substep doesn't do an incremental contact update, but that's fine, it'll end up expecting 0. - var syncOffsetToPreviousSubstep = baseStageCountInSubstep + 2; + var syncOffsetToPreviousSubstep = 2 + syncStagesPerWarmStartOrSolve * (1 + IterationCount); //To find the previous execution sync index of a constraint batch, we have to scan through all the constraint batches, but ALSO skip over the incremental contact update stage and kinematic integration, hence + 2. var syncOffsetToPreviousClaimOnBatchForWarmStart = syncStagesPerWarmStartOrSolve + 2; //For solves, there is no incremental update in the way. @@ -509,6 +515,7 @@ void SolveStep2Worker2(int workerIndex) //Note that we're going to do a compare exchange that prevents any claim on work blocks that *arent* of the previous sync index, which means we need the previous sync index. //Storing that in a reliable way is annoying, so we derive it from syncIndex. ref var stage = ref substepContext.Stages[syncIndexInSubstep]; + //Console.WriteLine($"Worker {workerIndex} executing {stage.StageType} for sync index {syncIndex}, stage index {syncIndexInSubstep}"); switch (stage.StageType) { case SolverStageType.IncrementalUpdate: @@ -603,7 +610,8 @@ protected void ExecuteMultithreaded2(float dt, IThreadDispatcher threadDispatche //This is just to make indexing a little simpler during the multithreaded work. int targetStageIndex = 2; //Warm start. - int claimStart = incrementalBlocks.Count + substepContext.KinematicIntegrationBlocks.Length; + var preambleClaimCount = incrementalBlocks.Count + substepContext.KinematicIntegrationBlocks.Length; + int claimStart = preambleClaimCount; for (int batchIndex = 0; batchIndex < stagesPerIteration; ++batchIndex) { var stageIndex = targetStageIndex++; @@ -615,7 +623,7 @@ protected void ExecuteMultithreaded2(float dt, IThreadDispatcher threadDispatche for (int iterationIndex = 0; iterationIndex < IterationCount; ++iterationIndex) { //Solve. Note that we're reusing the same claims as were used in the warm start for these stages; the stages just tell the workers what kind of work to do. - claimStart = incrementalBlocks.Count; + claimStart = preambleClaimCount; for (int batchIndex = 0; batchIndex < stagesPerIteration; ++batchIndex) { var stageIndex = targetStageIndex++; diff --git a/Demos/SpecializedTests/SimulationScrambling.cs b/Demos/SpecializedTests/SimulationScrambling.cs index f6ac91c1b..52d29eeb2 100644 --- a/Demos/SpecializedTests/SimulationScrambling.cs +++ b/Demos/SpecializedTests/SimulationScrambling.cs @@ -87,9 +87,10 @@ public BodyEnumerator(Bodies bodies, int[] handleToEntryIndex) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void LoopBody(int connectedBodyIndex) + public void LoopBody(int encodedBodyIndex) { - var entryIndex = HandlesToIdentity[Bodies.ActiveSet.IndexToHandle[connectedBodyIndex].Value]; + var bodyIndex = encodedBodyIndex & Bodies.BodyIndexMask; + var entryIndex = HandlesToIdentity[Bodies.ActiveSet.IndexToHandle[bodyIndex].Value]; if (IndexInConstraint == 0) IdentityA = entryIndex; else @@ -112,11 +113,12 @@ struct ConstraintBodyValidationEnumerator : IForEach { public Simulation Simulation; public ConstraintHandle ConstraintHandle; - public void LoopBody(int bodyIndex) + public void LoopBody(int encodedBodyIndex) { //The body in this constraint should both: //1) have a handle associated with it, and //2) the constraint graph list for the body should include the constraint handle. + var bodyIndex = encodedBodyIndex & Bodies.BodyIndexMask; Debug.Assert(Simulation.Bodies.ActiveSet.IndexToHandle[bodyIndex].Value >= 0); Debug.Assert(Simulation.Bodies.ActiveSet.BodyIsConstrainedBy(bodyIndex, ConstraintHandle)); } @@ -157,7 +159,7 @@ static void Validate(Simulation simulation, List removedConstraints, List(Simulation simulation, int iterations, Bo simulation.Solver.GetConstraintReference(constraintHandle, out var reference); var bodyIdentityEnumerator = new BodyEnumerator(simulation.Bodies, bodyHandlesToIdentity); - simulation.Solver.TypeProcessors[reference.TypeBatch.TypeId].EnumerateConnectedBodyIndices(ref reference.TypeBatch, reference.IndexInTypeBatch, ref bodyIdentityEnumerator); + simulation.Solver.TypeProcessors[reference.TypeBatch.TypeId].EnumerateConnectedRawBodyReferences(ref reference.TypeBatch, reference.IndexInTypeBatch, ref bodyIdentityEnumerator); constraintDescriptions[i].BodyA = bodyIdentityEnumerator.IdentityA; constraintDescriptions[i].BodyB = bodyIdentityEnumerator.IdentityB; } From 42eaea80cb03fb0c70b46f4021404a2efd8e7166 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 1 Nov 2021 17:30:13 -0500 Subject: [PATCH 253/947] Refactored protobatch constraint adds. Less redundant enumeration. --- BepuPhysics/IslandScaffold.cs | 31 ++++++++++++++++--------------- 1 file changed, 16 insertions(+), 15 deletions(-) diff --git a/BepuPhysics/IslandScaffold.cs b/BepuPhysics/IslandScaffold.cs index ede51923c..63c708ef2 100644 --- a/BepuPhysics/IslandScaffold.cs +++ b/BepuPhysics/IslandScaffold.cs @@ -76,21 +76,12 @@ internal void Validate(Solver solver) } } - public unsafe bool TryAdd(ConstraintHandle constraintHandle, int batchIndex, Solver solver, BufferPool pool, ref SequentialFallbackBatch fallbackBatch) + public unsafe bool TryAdd(ConstraintHandle constraintHandle, Span dynamicBodyIndices, int typeId, int batchIndex, Solver solver, BufferPool pool, ref SequentialFallbackBatch fallbackBatch) { - ref var constraintLocation = ref solver.HandleToConstraint[constraintHandle.Value]; - var typeProcessor = solver.TypeProcessors[constraintLocation.TypeId]; - var bodiesPerConstraint = typeProcessor.BodiesPerConstraint; - var bodyIndices = stackalloc int[bodiesPerConstraint]; - ConstraintHandleEnumerator enumerator; - enumerator.BodyIndices = bodyIndices; - enumerator.Count = 0; - solver.EnumerateActiveDynamicConnectedBodyIndices(constraintHandle, ref enumerator); - var dynamicBodyIndices = new Span(enumerator.BodyIndices, enumerator.Count); if (batchIndex == solver.FallbackBatchThreshold || ReferencedBodyIndices.CanFit(dynamicBodyIndices)) { - ref var typeBatch = ref GetOrCreateTypeBatch(constraintLocation.TypeId, solver, pool); - Debug.Assert(typeBatch.TypeId == constraintLocation.TypeId); + ref var typeBatch = ref GetOrCreateTypeBatch(typeId, solver, pool); + Debug.Assert(typeBatch.TypeId == typeId); typeBatch.Handles.Add(constraintHandle.Value, pool); if (batchIndex < solver.FallbackBatchThreshold) { @@ -160,11 +151,20 @@ public void Validate(Solver solver) } } - void AddConstraint(ConstraintHandle constraintHandle, Solver solver, BufferPool pool) + unsafe void AddConstraint(ConstraintHandle constraintHandle, Solver solver, BufferPool pool) { + var typeId = solver.HandleToConstraint[constraintHandle.Value].TypeId; + var typeProcessor = solver.TypeProcessors[typeId]; + var bodiesPerConstraint = typeProcessor.BodiesPerConstraint; + var bodyIndices = stackalloc int[bodiesPerConstraint]; + ConstraintHandleEnumerator enumerator; + enumerator.BodyIndices = bodyIndices; + enumerator.Count = 0; + solver.EnumerateActiveDynamicConnectedBodyIndices(constraintHandle, ref enumerator); + var dynamicBodyIndices = new Span(enumerator.BodyIndices, enumerator.Count); for (int batchIndex = 0; batchIndex < Protobatches.Count; ++batchIndex) { - if (Protobatches[batchIndex].TryAdd(constraintHandle, batchIndex, solver, pool, ref FallbackBatch)) + if (Protobatches[batchIndex].TryAdd(constraintHandle, dynamicBodyIndices, typeId, batchIndex, solver, pool, ref FallbackBatch)) { return; } @@ -174,7 +174,8 @@ void AddConstraint(ConstraintHandle constraintHandle, Solver solver, BufferPool var newBatchIndex = Protobatches.Count; ref var newBatch = ref Protobatches.AllocateUnsafely(); newBatch = new IslandScaffoldConstraintBatch(solver, pool, newBatchIndex); - newBatch.TryAdd(constraintHandle, newBatchIndex, solver, pool, ref FallbackBatch); + var addedSuccessfully = newBatch.TryAdd(constraintHandle, dynamicBodyIndices, typeId, newBatchIndex, solver, pool, ref FallbackBatch); + Debug.Assert(addedSuccessfully, "If we created a new batch for a constraint, then it must successfully add."); } internal void Dispose(BufferPool pool) From be3039858b5cc4cee5d4bcd25bd8c1f8d795f68e Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 1 Nov 2021 21:00:12 -0500 Subject: [PATCH 254/947] Simplified constraint add kinematic/dynamic testing; moving toward sleeping constraints referencing bodies by encoded handle with kinematic flag. Non-sleepy simulations run, but sleep/wake cycles still blow stuff up. --- BepuPhysics/Bodies.cs | 2 +- BepuPhysics/Bodies_GatherScatter.cs | 48 +- .../NarrowPhasePendingConstraintAdds.cs | 5 +- BepuPhysics/ConstraintBatch.cs | 2 +- BepuPhysics/Constraints/TypeProcessor.cs | 109 ++- BepuPhysics/HandyEnumerators.cs | 6 +- BepuPhysics/IslandSleeper.cs | 2 +- BepuPhysics/SequentialFallbackBatch.cs | 2 +- BepuPhysics/Solver.cs | 79 +- BepuPhysics/Solver_SubsteppingSolve2.cs | 2 +- .../Constraints/ConstraintLineExtractor.cs | 6 +- Demos/DemoSet.cs | 2 +- Demos/SpecializedTests/ConstraintTestDemo.cs | 744 +++++++++--------- .../SpecializedTests/SimulationScrambling.cs | 4 +- 14 files changed, 536 insertions(+), 477 deletions(-) diff --git a/BepuPhysics/Bodies.cs b/BepuPhysics/Bodies.cs index 4b4c2a68b..f555c5f4b 100644 --- a/BepuPhysics/Bodies.cs +++ b/BepuPhysics/Bodies.cs @@ -608,7 +608,7 @@ struct ActiveConstraintBodyHandleEnumerator : IForEach wh [MethodImpl(MethodImplOptions.AggressiveInlining)] public void LoopBody(int encodedBodyIndex) { - var bodyIndex = encodedBodyIndex & Bodies.BodyIndexMask; + var bodyIndex = encodedBodyIndex & BodyReferenceMask; if (SourceBodyIndex != bodyIndex) { //This enumerator is associated with the public connected bodies enumerator function. The user supplies a handle and expects handles in return, so we diff --git a/BepuPhysics/Bodies_GatherScatter.cs b/BepuPhysics/Bodies_GatherScatter.cs index c3dfa8295..7023ee935 100644 --- a/BepuPhysics/Bodies_GatherScatter.cs +++ b/BepuPhysics/Bodies_GatherScatter.cs @@ -143,13 +143,37 @@ unsafe static void FallbackGatherInertia(SolverState* states, Vector baseIn public const int DoesntExistFlagIndex = 31; public const int KinematicFlagIndex = 30; - public const uint KinematicMask = 1u << KinematicFlagIndex; + public const int KinematicMask = 1 << KinematicFlagIndex; /// - /// Constraint body index references greater than a given unsigned value are either kinematic (1<<30 set) or correspond to an empty lane (1<<31 set). + /// Constraint body references greater than a given unsigned value are either kinematic (1<<30 set) or correspond to an empty lane (1<<31 set). /// public const uint DynamicLimit = KinematicMask; - public const uint BodyIndexMetadataMask = (1u << DoesntExistFlagIndex) | KinematicMask; - public const int BodyIndexMask = (int)~BodyIndexMetadataMask; + public const uint BodyReferenceMetadataMask = (1u << DoesntExistFlagIndex) | KinematicMask; + /// + /// Mask of bits containing the decoded body reference in a constraint body reference. For active constraints this would be the body index bits, for sleeping constraints this would be the body handle bits. + /// + public const int BodyReferenceMask = (int)~BodyReferenceMetadataMask; + + /// + /// Checks whether a constraint encoded body reference value refers to a dynamic body. + /// + /// Raw encoded value taken from a constraint. + /// True if the encoded body reference refers to a dynamic body, false otherwise. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static bool IsEncodedDynamicReference(int encodedBodyReferenceValue) + { + return (encodedBodyReferenceValue & KinematicMask) == 0; + } + /// + /// Checks whether a constraint encoded body reference value refers to a kinematic body. + /// + /// Raw encoded value taken from a constraint. + /// True if the encoded body reference refers to a kinematic body, false otherwise. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static bool IsEncodedKinematicReference(int encodedBodyReferenceValue) + { + return (encodedBodyReferenceValue & KinematicMask) > 0; + } //[MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe void GatherState(Vector encodedBodyIndices, bool worldInertia, out Vector3Wide position, out QuaternionWide orientation, out BodyVelocityWide velocity, out BodyInertiaWide inertia) @@ -161,28 +185,28 @@ public unsafe void GatherState(Vector encodedBodyIndices, bo { var bodyIndices0 = encodedBodyIndices[0]; var empty0 = bodyIndices0 < 0; - var s0 = (float*)(solverStates + (bodyIndices0 & BodyIndexMask)); + var s0 = (float*)(solverStates + (bodyIndices0 & BodyReferenceMask)); var bodyIndices1 = encodedBodyIndices[1]; var empty1 = bodyIndices1 < 0; - var s1 = (float*)(solverStates + (bodyIndices1 & BodyIndexMask)); + var s1 = (float*)(solverStates + (bodyIndices1 & BodyReferenceMask)); var bodyIndices2 = encodedBodyIndices[2]; var empty2 = bodyIndices2 < 0; - var s2 = (float*)(solverStates + (bodyIndices2 & BodyIndexMask)); + var s2 = (float*)(solverStates + (bodyIndices2 & BodyReferenceMask)); var bodyIndices3 = encodedBodyIndices[3]; var empty3 = bodyIndices3 < 0; - var s3 = (float*)(solverStates + (bodyIndices3 & BodyIndexMask)); + var s3 = (float*)(solverStates + (bodyIndices3 & BodyReferenceMask)); var bodyIndices4 = encodedBodyIndices[4]; var empty4 = bodyIndices4 < 0; - var s4 = (float*)(solverStates + (bodyIndices4 & BodyIndexMask)); + var s4 = (float*)(solverStates + (bodyIndices4 & BodyReferenceMask)); var bodyIndices5 = encodedBodyIndices[5]; var empty5 = bodyIndices5 < 0; - var s5 = (float*)(solverStates + (bodyIndices5 & BodyIndexMask)); + var s5 = (float*)(solverStates + (bodyIndices5 & BodyReferenceMask)); var bodyIndices6 = encodedBodyIndices[6]; var empty6 = bodyIndices6 < 0; - var s6 = (float*)(solverStates + (bodyIndices6 & BodyIndexMask)); + var s6 = (float*)(solverStates + (bodyIndices6 & BodyReferenceMask)); var bodyIndices7 = encodedBodyIndices[7]; var empty7 = bodyIndices7 < 0; - var s7 = (float*)(solverStates + (bodyIndices7 & BodyIndexMask)); + var s7 = (float*)(solverStates + (bodyIndices7 & BodyReferenceMask)); //for (int i = 0; i < 8; ++i) //{ diff --git a/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs b/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs index 9ab130733..5b0f9c4b4 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs @@ -121,10 +121,11 @@ static unsafe void AddToSimulationSpeculative(Unsafe.AsPointer(ref constraint.BodyHandles), typeof(TBodyHandles) == typeof(TwoBodyHandles) ? 2 : 1); Span blockingBodyHandles = stackalloc BodyHandle[handles.Length]; - blockingBodyHandles = simulation.Solver.GetBlockingBodyHandles(handles, blockingBodyHandles); + Span encodedBodyIndices = stackalloc int[handles.Length]; + simulation.Solver.GetBlockingBodyHandles(handles, ref blockingBodyHandles, encodedBodyIndices); while (!simulation.Solver.TryAllocateInBatch( default(TDescription).ConstraintTypeId, batchIndex, - blockingBodyHandles, handles, out constraintHandle, out reference)) + blockingBodyHandles, encodedBodyIndices, out constraintHandle, out reference)) { //If a batch index failed, just try the next one. This is guaranteed to eventually work. ++batchIndex; diff --git a/BepuPhysics/ConstraintBatch.cs b/BepuPhysics/ConstraintBatch.cs index ffc96da07..6ab24cba5 100644 --- a/BepuPhysics/ConstraintBatch.cs +++ b/BepuPhysics/ConstraintBatch.cs @@ -160,7 +160,7 @@ public void LoopBody(int encodedBodyIndex) { if (encodedBodyIndex < Bodies.DynamicLimit) { - Handles->Remove(Bodies.ActiveSet.IndexToHandle[encodedBodyIndex & Bodies.BodyIndexMask].Value); + Handles->Remove(Bodies.ActiveSet.IndexToHandle[encodedBodyIndex & Bodies.BodyReferenceMask].Value); } } } diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index aa096cccc..6e6ff785e 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -53,20 +53,20 @@ public void Initialize(int typeId) /// /// Type batch to allocate in. /// Handle of the constraint to allocate. Establishes a link from the allocated constraint to its handle. - /// Pointer to a list of body indices (not handles!) with count equal to the type batch's expected number of involved bodies. + /// List of body indices (not handles!) with count equal to the type batch's expected number of involved bodies. /// Allocation provider to use if the type batch has to be resized. /// Index of the slot in the batch. - public unsafe abstract int AllocateInTypeBatch(ref TypeBatch typeBatch, ConstraintHandle handle, int* bodyIndices, BufferPool pool); + public unsafe abstract int AllocateInTypeBatch(ref TypeBatch typeBatch, ConstraintHandle handle, Span encodedBodyIndices, BufferPool pool); /// /// Allocates a slot in the batch, assuming the batch is a fallback batch. /// /// Type batch to allocate in. /// Handle of the constraint to allocate. Establishes a link from the allocated constraint to its handle. - /// Pointer to a list of body indices (not handles!) with count equal to the type batch's expected number of involved bodies. + /// List of body indices (not handles!) with count equal to the type batch's expected number of involved bodies. /// Allocation provider to use if the type batch has to be resized. /// Index of the slot in the batch. - public unsafe abstract int AllocateInTypeBatchForFallback(ref TypeBatch typeBatch, ConstraintHandle handle, int* bodyIndices, BufferPool pool); + public unsafe abstract int AllocateInTypeBatchForFallback(ref TypeBatch typeBatch, ConstraintHandle handle, Span encodedBodyIndices, BufferPool pool); public abstract void Remove(ref TypeBatch typeBatch, int index, ref Buffer handlesToConstraints, bool isFallback); /// @@ -215,23 +215,21 @@ public override unsafe void ScaleAccumulatedImpulses(ref TypeBatch typeBatch, fl } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe static void SetBodyReferencesLane(ref TBodyReferences bundle, int innerIndex, int* bodyIndices) + public unsafe static void SetBodyReferencesLane(ref TBodyReferences bundle, int innerIndex, Span bodyIndices) { - //The jit should be able to fold almost all of the size-related calculations and address fiddling. - var bodyCount = Unsafe.SizeOf() / (Vector.Count * sizeof(int)); //We assume that the body references struct is organized in memory like Bundle0, Inner0, ... BundleN, InnerN, Count //Assuming contiguous storage, Count is then located at start + stride * BodyCount. ref var start = ref Unsafe.As(ref bundle); ref var targetLane = ref Unsafe.Add(ref start, innerIndex); - targetLane = *bodyIndices; - for (int i = 1; i < bodyCount; ++i) + targetLane = bodyIndices[0]; + for (int i = 0; i < bodyIndices.Length; ++i) { Unsafe.Add(ref targetLane, i * Vector.Count) = bodyIndices[i]; } } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe static void AddBodyReferencesLane(ref TBodyReferences bundle, int innerIndex, int* bodyIndices) + public unsafe static void AddBodyReferencesLane(ref TBodyReferences bundle, int innerIndex, Span bodyIndices) { //The jit should be able to fold almost all of the size-related calculations and address fiddling. var bodyCount = Unsafe.SizeOf() / (Vector.Count * sizeof(int)); @@ -262,7 +260,7 @@ public unsafe static void RemoveBodyReferencesLane(ref TBodyReferences bundle, i } - public unsafe sealed override int AllocateInTypeBatch(ref TypeBatch typeBatch, ConstraintHandle handle, int* bodyIndices, BufferPool pool) + public unsafe sealed override int AllocateInTypeBatch(ref TypeBatch typeBatch, ConstraintHandle handle, Span bodyIndices, BufferPool pool) { Debug.Assert(typeBatch.BodyReferences.Allocated, "Should initialize the batch before allocating anything from it."); if (typeBatch.ConstraintCount == typeBatch.IndexToHandle.Length) @@ -298,6 +296,8 @@ private unsafe static bool AllowFallbackBundleAllocation(ref TBodyReferences bun var broadcastedBodies = broadcastedBodyIndices[broadcastedBundleBodyInConstraint]; for (int bundleBodyIndexInConstraint = 0; bundleBodyIndexInConstraint < bodiesPerConstraint; ++bundleBodyIndexInConstraint) { + //Note that the broadcastedBodies were created with the kinematic flag stripped, so when comparing against the constraint-held references, they will never return true. + //This means that kinematics can appear more than once in a single bundle, which is what we want. Kinematics can appear multiple times in batches, too. if (Vector.EqualsAny(bundleBodyIndices[bundleBodyIndexInConstraint], broadcastedBodies)) { return false; @@ -318,7 +318,7 @@ private unsafe static int GetInnerIndexForFallbackAllocation(ref TBodyReferences } [MethodImpl(MethodImplOptions.AggressiveInlining)] - private unsafe static bool ProbeBundleForFallback(Buffer typeBatchBodyIndices, Vector* broadcastedBodyIndices, int* bodyIndices, int bundleIndex, ref int targetBundleIndex, ref int targetInnerIndex) + private unsafe static bool ProbeBundleForFallback(Buffer typeBatchBodyIndices, Vector* broadcastedBodyIndices, Span encodedBodyIndices, int bundleIndex, ref int targetBundleIndex, ref int targetInnerIndex) { ref var bundle = ref typeBatchBodyIndices[bundleIndex]; if (AllowFallbackBundleAllocation(ref bundle, broadcastedBodyIndices)) @@ -326,7 +326,7 @@ private unsafe static bool ProbeBundleForFallback(Buffer typeBa //We've found a place to put the allocation. targetBundleIndex = bundleIndex; targetInnerIndex = GetInnerIndexForFallbackAllocation(ref bundle); - SetBodyReferencesLane(ref bundle, targetInnerIndex, bodyIndices); + SetBodyReferencesLane(ref bundle, targetInnerIndex, encodedBodyIndices); return true; } return false; @@ -398,7 +398,7 @@ void ValidateAccumulatedImpulses(ref TypeBatch typeBatch) } } - public unsafe sealed override int AllocateInTypeBatchForFallback(ref TypeBatch typeBatch, ConstraintHandle handle, int* bodyIndices, BufferPool pool) + public unsafe sealed override int AllocateInTypeBatchForFallback(ref TypeBatch typeBatch, ConstraintHandle handle, Span encodedBodyIndices, BufferPool pool) { //This folds. var bodiesPerConstraint = Unsafe.SizeOf() / Unsafe.SizeOf>(); @@ -422,9 +422,11 @@ public unsafe sealed override int AllocateInTypeBatchForFallback(ref TypeBatch t int targetBundleIndex = -1; int targetInnerIndex = -1; var broadcastedBodyIndices = stackalloc Vector[bodiesPerConstraint]; - for (int bodyIndexInConstraint = 0; bodyIndexInConstraint < bodiesPerConstraint; ++bodyIndexInConstraint) + for (int bodyIndexInConstraint = 0; bodyIndexInConstraint < encodedBodyIndices.Length; ++bodyIndexInConstraint) { - broadcastedBodyIndices[bodyIndexInConstraint] = new Vector(bodyIndices[bodyIndexInConstraint]); + //Note that the broadcastedBodies are created with the kinematic flag stripped, so when comparing against the constraint-held references, they will never return true. + //This means that kinematics can appear more than once in a single bundle, which is what we want. Kinematics can appear multiple times in batches, too. + broadcastedBodyIndices[bodyIndexInConstraint] = new Vector(encodedBodyIndices[bodyIndexInConstraint] & Bodies.BodyReferenceMask); } var bundleCount = typeBatch.BundleCount; if (bundleCount <= probeLocationCount + 1) //(we always probe the last bundle) @@ -432,7 +434,7 @@ public unsafe sealed override int AllocateInTypeBatchForFallback(ref TypeBatch t //The fallback batch is small; there's no need to do a stochastic insertion. Just enumerate all bundles. for (int bundleIndexInTypeBatch = 0; bundleIndexInTypeBatch < bundleCount; ++bundleIndexInTypeBatch) { - if (ProbeBundleForFallback(typeBatchBodyIndices, broadcastedBodyIndices, bodyIndices, bundleIndexInTypeBatch, ref targetBundleIndex, ref targetInnerIndex)) + if (ProbeBundleForFallback(typeBatchBodyIndices, broadcastedBodyIndices, encodedBodyIndices, bundleIndexInTypeBatch, ref targetBundleIndex, ref targetInnerIndex)) break; } } @@ -445,7 +447,7 @@ public unsafe sealed override int AllocateInTypeBatchForFallback(ref TypeBatch t //First, probe the final bundle just in case it's nice and simple, then do stochastic probes. //Stochastic probing works by pseudorandomly choosing a starting point, then picking probe locations based on that starting point. var lastBundleIndex = bundleCount - 1; - if (!ProbeBundleForFallback(typeBatchBodyIndices, broadcastedBodyIndices, bodyIndices, lastBundleIndex, ref targetBundleIndex, ref targetInnerIndex)) + if (!ProbeBundleForFallback(typeBatchBodyIndices, broadcastedBodyIndices, encodedBodyIndices, lastBundleIndex, ref targetBundleIndex, ref targetInnerIndex)) { //No room in the final bundle; keep looking with stochastic probes. var nextProbeIndex = (HashHelper.Rehash(handle.Value) & 0x7FFF_FFFF) % lastBundleIndex; @@ -453,7 +455,7 @@ public unsafe sealed override int AllocateInTypeBatchForFallback(ref TypeBatch t var remainder = lastBundleIndex - bundleJump * probeLocationCount; for (int probeIndex = 0; probeIndex < probeLocationCount; ++probeIndex) { - if (ProbeBundleForFallback(typeBatchBodyIndices, broadcastedBodyIndices, bodyIndices, nextProbeIndex, ref targetBundleIndex, ref targetInnerIndex)) + if (ProbeBundleForFallback(typeBatchBodyIndices, broadcastedBodyIndices, encodedBodyIndices, nextProbeIndex, ref targetBundleIndex, ref targetInnerIndex)) break; nextProbeIndex += bundleJump; if (probeIndex < remainder) @@ -478,7 +480,7 @@ public unsafe sealed override int AllocateInTypeBatchForFallback(ref TypeBatch t typeBatch.ConstraintCount = newConstraintCount; typeBatch.IndexToHandle[indexInTypeBatch] = handle; ref var bundle = ref Buffer.Get(ref typeBatch.BodyReferences, bundleCount); - AddBodyReferencesLane(ref bundle, 0, bodyIndices); + AddBodyReferencesLane(ref bundle, 0, encodedBodyIndices); //Clear the slot's accumulated impulse. The backing memory could be initialized to any value. GatherScatter.ClearLane(ref Buffer.Get(ref typeBatch.AccumulatedImpulses, bundleCount), 0); Debug.Assert(typeBatch.PrestepData.Length >= typeBatch.BundleCount * Unsafe.SizeOf()); @@ -677,6 +679,38 @@ ref Unsafe.Add(ref bodyReferences, targetBundleIndex), ref Unsafe.Add(ref preste } + /// + /// Collects body references from active constraints and converts them into properly flagged constraint kinematic body handles. + /// + unsafe struct ActiveKinematicFlaggedBodyHandleCollector : IForEach + { + public Bodies Bodies; + public int* DynamicBodyHandles; + public int DynamicCount; + public int* EncodedBodyIndices; + public int IndexCount; + + + public ActiveKinematicFlaggedBodyHandleCollector(Bodies bodies, int* dynamicHandles, int* encodedBodyIndices) + { + Bodies = bodies; + DynamicBodyHandles = dynamicHandles; + DynamicCount = 0; + EncodedBodyIndices = encodedBodyIndices; + IndexCount = 0; + } + + public void LoopBody(int encodedBodyIndex) + { + if (Bodies.IsEncodedDynamicReference(encodedBodyIndex)) + { + DynamicBodyHandles[DynamicCount++] = Bodies.ActiveSet.IndexToHandle[encodedBodyIndex].Value; + } + EncodedBodyIndices[IndexCount++] = encodedBodyIndex; + } + } + + /// /// Moves a constraint from one ConstraintBatch's TypeBatch to another ConstraintBatch's TypeBatch of the same type. /// @@ -691,14 +725,15 @@ public unsafe override void TransferConstraint(ref TypeBatch typeBatch, int sour //It's not exactly trivial to keep everything straight, especially over time- it becomes a maintenance nightmare. //So instead, given that compressions should generally be extremely rare (relatively speaking) and highly deferrable, we'll accept some minor overhead. int bodiesPerConstraint = InternalBodiesPerConstraint; - var bodyHandles = stackalloc int[bodiesPerConstraint]; - var bodyHandleCollector = new ActiveConstraintBodyHandleCollector(bodies, bodyHandles); + var dynamicBodyHandles = stackalloc int[bodiesPerConstraint]; + var encodedBodyIndices = stackalloc int[bodiesPerConstraint]; + var bodyHandleCollector = new ActiveKinematicFlaggedBodyHandleCollector(bodies, dynamicBodyHandles, encodedBodyIndices); EnumerateConnectedRawBodyReferences(ref typeBatch, indexInTypeBatch, ref bodyHandleCollector); Debug.Assert(targetBatchIndex <= solver.FallbackBatchThreshold, "Constraint transfers should never target the fallback batch. Its registered body handles don't block new constraints so attempting to allocate in the same way wouldn't turn out well."); //Allocate a spot in the new batch. Note that it does not change the Handle->Constraint mapping in the Solver; that's important when we call Solver.Remove below. var constraintHandle = typeBatch.IndexToHandle[indexInTypeBatch]; - solver.AllocateInBatch(targetBatchIndex, constraintHandle, new Span(bodyHandles, bodiesPerConstraint), typeId, out var targetReference); + solver.AllocateInBatch(targetBatchIndex, constraintHandle, new Span((BodyHandle*)dynamicBodyHandles, bodyHandleCollector.DynamicCount), new Span(encodedBodyIndices, bodiesPerConstraint), typeId, out var targetReference); BundleIndexing.GetBundleIndices(targetReference.IndexInTypeBatch, out var targetBundle, out var targetInner); BundleIndexing.GetBundleIndices(indexInTypeBatch, out var sourceBundle, out var sourceInner); @@ -901,7 +936,9 @@ ref Buffer.Get(ref sourceTypeBatch.AccumulatedImpulses, sou var offset = 0; for (int j = 0; j < bodiesPerConstraint; ++j) { - Unsafe.Add(ref targetReferencesLaneStart, offset) = activeBodySet.IndexToHandle[Unsafe.Add(ref sourceReferencesLaneStart, offset) & Bodies.BodyIndexMask].Value; + var encodedBodyIndex = Unsafe.Add(ref sourceReferencesLaneStart, offset); + //Note that when we transfer the body reference into the sleeping batch, the body reference turns into a handle- but it preserves the kinematic flag. + Unsafe.Add(ref targetReferencesLaneStart, offset) = activeBodySet.IndexToHandle[encodedBodyIndex & Bodies.BodyReferenceMask].Value | (encodedBodyIndex & Bodies.KinematicMask); offset += Vector.Count; } } @@ -923,7 +960,7 @@ internal unsafe sealed override void AddSleepingToActiveForFallback(int sourceSe //ValidateFallbackAccessSafety(ref targetTypeBatch, bodyCount); //solver.ValidateConstraintMaps(0, batchIndex, targetTypeBatchIndex); //solver.ValidateConstraintMaps(sourceSet, batchIndex, sourceTypeBatchIndex); - int* bodyIndices = stackalloc int[bodyCount]; + Span bodyIndices = stackalloc int[bodyCount]; var sourceBundleCount = sourceTypeBatch.BundleCount; var sourceBodyReferences = sourceTypeBatch.BodyReferences.As(); var sourcePrestepData = sourceTypeBatch.PrestepData.As(); @@ -948,7 +985,7 @@ internal unsafe sealed override void AddSleepingToActiveForFallback(int sourceSe var sourceIndex = bundleStartConstraintIndex + sourceInnerIndex; ref var bodyReferencesLane = ref Unsafe.As(ref GatherScatter.GetOffsetInstance(ref sourceBodyReferencesBundle, sourceInnerIndex)); //Note that the sleeping set stores body references as handles, while the active set uses indices. We have to translate here. - for (int i = 0; i < bodyCount; ++i) + for (int i = 0; i < bodyIndices.Length; ++i) { //Bodies have already been moved into the active set, so we can use the mapping. var bodyHandleValue = Unsafe.Add(ref bodyReferencesLane, Vector.Count * i); @@ -1039,7 +1076,9 @@ ref Buffer.Get(ref sourceTypeBatch.AccumulatedImpulses, sou var offset = 0; for (int j = 0; j < bodiesPerConstraint; ++j) { - Unsafe.Add(ref targetReferencesLaneStart, offset) = bodies.HandleToLocation[Unsafe.Add(ref sourceReferencesLaneStart, offset)].Index; + var encodedBodyHandle = Unsafe.Add(ref sourceReferencesLaneStart, offset); + //Note that encoded kinematicity flags are carried over to the active index reference. + Unsafe.Add(ref targetReferencesLaneStart, offset) = bodies.HandleToLocation[encodedBodyHandle & Bodies.BodyReferenceMask].Index | (encodedBodyHandle & Bodies.KinematicMask); offset += Vector.Count; } var constraintHandle = sourceTypeBatch.IndexToHandle[sourceIndex]; @@ -1064,12 +1103,16 @@ internal unsafe sealed override void AddWakingBodyHandlesToBatchReferences(ref T var offset = 0; for (int j = 0; j < bodiesPerConstraint; ++j) { - var bodyHandle = Unsafe.Add(ref sourceHandlesStart, offset); - //Given that we're only adding references to bodies that already exist, and therefore were at some point in the active set, it should never be necessary - //to resize the batch referenced handles structure. - //Note that this will happily set an existing bit if the target batch is the fallback batch. - targetBatchReferencedHandles.SetUnsafely(bodyHandle); - offset += Vector.Count; + var encodedBodyHandle = Unsafe.Add(ref sourceHandlesStart, offset); + if (Bodies.IsEncodedDynamicReference(encodedBodyHandle)) + { + //Note that only dynamic bodies are added to the batch referenced handles. + //Given that we're only adding references to bodies that already exist, and therefore were at some point in the active set, it should never be necessary + //to resize the batch referenced handles structure. + //Note that this will happily set an existing bit if the target batch is the fallback batch. + targetBatchReferencedHandles.SetUnsafely(encodedBodyHandle); + offset += Vector.Count; + } } } } diff --git a/BepuPhysics/HandyEnumerators.cs b/BepuPhysics/HandyEnumerators.cs index 7df4e3560..853b832ce 100644 --- a/BepuPhysics/HandyEnumerators.cs +++ b/BepuPhysics/HandyEnumerators.cs @@ -27,7 +27,7 @@ public ActiveConstraintBodyHandleCollector(Bodies bodies, int* handles) public void LoopBody(int encodedBodyIndex) { //Note that this enumerator is used with prefiltered body indices and with raw body indices. A redundant & isn't much of a concern; lets us share more frequently. - Handles[Count++] = Bodies.ActiveSet.IndexToHandle[encodedBodyIndex & Bodies.BodyIndexMask].Value; + Handles[Count++] = Bodies.ActiveSet.IndexToHandle[encodedBodyIndex & Bodies.BodyReferenceMask].Value; } } @@ -52,7 +52,7 @@ public void LoopBody(int encodedBodyIndex) if (encodedBodyIndex < Bodies.DynamicLimit) { //Note that this enumerator is used with prefiltered body indices and with raw body indices. A redundant & isn't much of a concern; lets us share more frequently. - Handles[Count++] = Bodies.ActiveSet.IndexToHandle[encodedBodyIndex & Bodies.BodyIndexMask].Value; + Handles[Count++] = Bodies.ActiveSet.IndexToHandle[encodedBodyIndex & Bodies.BodyReferenceMask].Value; } } } @@ -73,7 +73,7 @@ public ActiveConstraintBodyIndexCollector(int* indices) public void LoopBody(int encodedBodyIndex) { - Indices[Count++] = encodedBodyIndex & Bodies.BodyIndexMask; + Indices[Count++] = encodedBodyIndex & Bodies.BodyReferenceMask; } } diff --git a/BepuPhysics/IslandSleeper.cs b/BepuPhysics/IslandSleeper.cs index 23e277306..62fcf90cd 100644 --- a/BepuPhysics/IslandSleeper.cs +++ b/BepuPhysics/IslandSleeper.cs @@ -575,7 +575,7 @@ unsafe void PrintIsland(ref IslandScaffold island) ref solverBatch.TypeBatches[solverBatch.TypeIndexToTypeBatchIndex[location.TypeId]], location.IndexInTypeBatch, ref bodyIndexEnumerator); for (int i = 0; i < typeProcessor.BodiesPerConstraint; ++i) { - constraintReferencedBodyHandles.Add(ref bodies.ActiveSet.IndexToHandle[rawBodyIndices[i] & Bodies.BodyIndexMask].Value, pool); + constraintReferencedBodyHandles.Add(ref bodies.ActiveSet.IndexToHandle[rawBodyIndices[i] & Bodies.BodyReferenceMask].Value, pool); } Console.Write($"{handle}, "); } diff --git a/BepuPhysics/SequentialFallbackBatch.cs b/BepuPhysics/SequentialFallbackBatch.cs index 69ae5dfde..3cef64546 100644 --- a/BepuPhysics/SequentialFallbackBatch.cs +++ b/BepuPhysics/SequentialFallbackBatch.cs @@ -164,7 +164,7 @@ internal unsafe void Remove(Solver solver, BufferPool bufferPool, ref Constraint var isDynamic = (rawBodyIndex & Bodies.KinematicMask) == 0; if (isDynamic) { - var bodyIndex = rawBodyIndex & Bodies.BodyIndexMask; + var bodyIndex = rawBodyIndex & Bodies.BodyReferenceMask; if (Remove(bodyIndex, ref allocationIdsToFree)) { fallbackBatchHandles.Remove(solver.bodies.ActiveSet.IndexToHandle[bodyIndex].Value); diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index 651882b77..91d5dce07 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -302,7 +302,7 @@ unsafe internal void ValidateConstraintReferenceKinematicity() if (encodedBodyIndex > 0) { var kinematicByEncodedIndex = (encodedBodyIndex & Bodies.KinematicMask) != 0; - var kinematicByInertia = Bodies.IsKinematicUnsafeGCHole(ref bodies.ActiveSet.SolverStates[encodedBodyIndex & Bodies.BodyIndexMask].Inertia.Local); + var kinematicByInertia = Bodies.IsKinematicUnsafeGCHole(ref bodies.ActiveSet.SolverStates[encodedBodyIndex & Bodies.BodyReferenceMask].Inertia.Local); Debug.Assert(kinematicByEncodedIndex == kinematicByInertia, "Constraint reference encoded kinematicity must match actual kinematicity by inertia."); } } @@ -630,7 +630,7 @@ internal unsafe void ValidateExistingHandles(bool activeOnly = false) if (HandleToConstraint[constraints[constraintForBodyIndex].ConnectingConstraintHandle.Value].BatchIndex == batchIndex) ++expectedCount; } - bodyReference |= (int)Bodies.KinematicMask; + bodyReference |= Bodies.KinematicMask; } else { @@ -709,7 +709,7 @@ internal unsafe void ValidateExistingHandles(bool activeOnly = false) int bodyIndex; if (setIndex == 0) { - bodyIndex = constraintBodyReferences[i] & Bodies.BodyIndexMask; + bodyIndex = constraintBodyReferences[i] & Bodies.BodyReferenceMask; } else { @@ -740,7 +740,7 @@ internal unsafe void ValidateExistingHandles(bool activeOnly = false) int bodyReference = setIndex == 0 ? bodyIndex : bodySet.IndexToHandle[bodyIndex].Value; var bodyReferenceInConstraint = constraintBodyReferences[constraintList[constraintIndex].BodyIndexInConstraint]; if (setIndex == 0) - bodyReferenceInConstraint &= Bodies.BodyIndexMask; + bodyReferenceInConstraint &= Bodies.BodyReferenceMask; Debug.Assert(bodyReferenceInConstraint == bodyReference, "If a body refers to a constraint, the constraint should refer to the body."); } @@ -896,40 +896,25 @@ internal unsafe int FindCandidateBatch(CollidablePair collidablePair) return synchronizedBatchCount; } - internal unsafe void AllocateInBatch(int targetBatchIndex, ConstraintHandle constraintHandle, Span bodyHandles, int typeId, out ConstraintReference reference) + internal unsafe void AllocateInBatch(int targetBatchIndex, ConstraintHandle constraintHandle, Span dynamicBodyHandles, Span encodedBodyIndices, int typeId, out ConstraintReference reference) { ref var batch = ref ActiveSet.Batches[targetBatchIndex]; - //Add all the constraint's body handles to the batch we found (or created) to block future references to the same bodies. - //Also, convert the handle into a memory index. Constraints store a direct memory reference for performance reasons. - var bodyIndices = stackalloc int[bodyHandles.Length]; - Span dynamicBodyHandles = stackalloc BodyHandle[bodyHandles.Length]; - int dynamicBodyCount = 0; - for (int j = 0; j < bodyHandles.Length; ++j) - { - var bodyHandle = bodyHandles[j]; - ref var location = ref bodies.HandleToLocation[bodyHandle.Value]; - Debug.Assert(location.SetIndex == 0, "Creating a new constraint should have forced the connected bodies awake."); - var index = location.Index; - //Include the flag in the body index if it's kinematic. - if (Bodies.IsKinematic(bodies.ActiveSet.SolverStates[index].Inertia.Local)) - { - index |= (int)Bodies.KinematicMask; - ConstrainedKinematicHandles.Add(bodyHandle.Value, pool); - } - else + for (int j = 0; j < encodedBodyIndices.Length; ++j) + { + //Include the body in the constrained kinematics set if necessary. + var encodedBodyIndex = encodedBodyIndices[j]; + if (Bodies.IsEncodedKinematicReference(encodedBodyIndex)) { - dynamicBodyHandles[dynamicBodyCount++] = bodyHandles[j]; + ConstrainedKinematicHandles.Add(bodies.ActiveSet.IndexToHandle[encodedBodyIndex & Bodies.BodyReferenceMask].Value, pool); } - bodyIndices[j] = index; } - dynamicBodyHandles = dynamicBodyHandles.Slice(0, dynamicBodyCount); var typeProcessor = TypeProcessors[typeId]; var typeBatch = batch.GetOrCreateTypeBatch(typeId, typeProcessor, GetMinimumCapacityForType(typeId), pool); int indexInTypeBatch; if (targetBatchIndex == FallbackBatchThreshold) - indexInTypeBatch = typeProcessor.AllocateInTypeBatchForFallback(ref *typeBatch, constraintHandle, bodyIndices, pool); + indexInTypeBatch = typeProcessor.AllocateInTypeBatchForFallback(ref *typeBatch, constraintHandle, encodedBodyIndices, pool); else - indexInTypeBatch = typeProcessor.AllocateInTypeBatch(ref *typeBatch, constraintHandle, bodyIndices, pool); + indexInTypeBatch = typeProcessor.AllocateInTypeBatch(ref *typeBatch, constraintHandle, encodedBodyIndices, pool); reference = new ConstraintReference(typeBatch, indexInTypeBatch); //TODO: We could adjust the typeBatchAllocation capacities in response to the allocated index. //If it exceeds the current capacity, we could ensure the new size is still included. @@ -952,7 +937,7 @@ internal unsafe void AllocateInBatch(int targetBatchIndex, ConstraintHandle cons } } - unsafe internal Span GetBlockingBodyHandles(Span bodyHandles, Span allocation) + unsafe internal void GetBlockingBodyHandles(Span bodyHandles, ref Span blockingBodyHandlesAllocation, Span encodedBodyIndices) { //Kinematics do not block allocation in a batch; they are treated as read only by the solver. int blockingCount = 0; @@ -961,15 +946,20 @@ unsafe internal Span GetBlockingBodyHandles(Span bodyHan { var location = bodies.HandleToLocation[bodyHandles[i].Value]; Debug.Assert(location.SetIndex == 0); - if (!Bodies.IsKinematicUnsafeGCHole(ref solverStates[location.Index].Inertia.Local)) + if (Bodies.IsKinematicUnsafeGCHole(ref solverStates[location.Index].Inertia.Local)) { - allocation[blockingCount++] = bodyHandles[i]; + encodedBodyIndices[i] = location.Index | Bodies.KinematicMask; + } + else + { + blockingBodyHandlesAllocation[blockingCount++] = bodyHandles[i]; + encodedBodyIndices[i] = location.Index; } } - return allocation.Slice(0, blockingCount); + blockingBodyHandlesAllocation = blockingBodyHandlesAllocation.Slice(0, blockingCount); } - internal unsafe bool TryAllocateInBatch(int typeId, int targetBatchIndex, Span blockingBodyHandles, Span bodyHandles, out ConstraintHandle constraintHandle, out ConstraintReference reference) + internal unsafe bool TryAllocateInBatch(int typeId, int targetBatchIndex, Span dynamicBodyHandles, Span encodedBodyIndices, out ConstraintHandle constraintHandle, out ConstraintReference reference) { ref var set = ref ActiveSet; Debug.Assert(targetBatchIndex <= set.Batches.Count, @@ -992,7 +982,7 @@ internal unsafe bool TryAllocateInBatch(int typeId, int targetBatchIndex, Span(blockingBodyHandles))) + if (!batchReferencedHandles[targetBatchIndex].CanFit(MemoryMarshal.Cast(dynamicBodyHandles))) { //This batch cannot hold the constraint. constraintHandle = new ConstraintHandle(-1); @@ -1002,7 +992,7 @@ internal unsafe bool TryAllocateInBatch(int typeId, int targetBatchIndex, Span= HandleToConstraint.Length) { @@ -1010,12 +1000,13 @@ internal unsafe bool TryAllocateInBatch(int typeId, int targetBatchIndex, Span(Span bodyHandles, in TDescription description { ref var set = ref ActiveSet; Span blockingBodyHandles = stackalloc BodyHandle[bodyHandles.Length]; - blockingBodyHandles = GetBlockingBodyHandles(bodyHandles, blockingBodyHandles); + Span encodedBodyIndices = stackalloc int[bodyHandles.Length]; + GetBlockingBodyHandles(bodyHandles, ref blockingBodyHandles, encodedBodyIndices); for (int i = 0; i <= set.Batches.Count; ++i) { - if (TryAllocateInBatch(description.ConstraintTypeId, i, blockingBodyHandles, bodyHandles, out handle, out var reference)) + if (TryAllocateInBatch(description.ConstraintTypeId, i, blockingBodyHandles, encodedBodyIndices, out handle, out var reference)) { ApplyDescriptionWithoutWaking(reference, description); return; @@ -1444,8 +1436,7 @@ public unsafe float GetAccumulatedImpulseMagnitude(ConstraintHandle constraintHa /// /// Enumerates the set of body references associated with a constraint in order of their references within the constraint. - /// If the constraint is awake, this will report the raw body index and any encoded metadata, like whether the body is kinematic. - /// If the constraint is asleep, this will report the body handle. + /// This will report the raw body reference (body index if awake, handle if asleep) and any encoded metadata, like whether the body is kinematic. /// /// Constraint to enumerate. /// Enumerator to use. @@ -1463,7 +1454,7 @@ struct ActiveBodyIndexEnumerator : IForEach where TWrapped : IFor public void LoopBody(int encodedBodyIndex) { //This is only used on the active set, so we know we're getting body indices. They include encoded metadata that we can check to see whether the body is kinematic. - Wrapped.LoopBody(encodedBodyIndex & Bodies.BodyIndexMask); + Wrapped.LoopBody(encodedBodyIndex & Bodies.BodyReferenceMask); } } @@ -1494,7 +1485,7 @@ public void LoopBody(int encodedBodyIndex) //This is only used on the active set, so we know we're getting body indices. They include encoded metadata that we can check to see whether the body is kinematic. if (encodedBodyIndex < Bodies.DynamicLimit) { - Wrapped.LoopBody(encodedBodyIndex & Bodies.BodyIndexMask); + Wrapped.LoopBody(encodedBodyIndex & Bodies.BodyReferenceMask); } } } diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index 65c850753..7bf6bf460 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -766,7 +766,7 @@ unsafe bool ComputeIntegrationResponsibilitiesForConstraintRegion for (int bundleInnerIndex = 0; bundleInnerIndex < countInBundle; ++bundleInnerIndex) { //Constraints refer to bodies by index when they're in the active set, so we need to transform to handle to look up our merged batch results. - var bodyIndex = bundleStart[bundleInnerIndex] & Bodies.BodyIndexMask; + var bodyIndex = bundleStart[bundleInnerIndex] & Bodies.BodyReferenceMask; if (typeof(TFallbackness) == typeof(IsFallbackBatch)) { //Fallback batches can contain empty lanes; there's no guarantee of constraint contiguity. Such lanes are marked with -1 in the body references. diff --git a/DemoRenderer/Constraints/ConstraintLineExtractor.cs b/DemoRenderer/Constraints/ConstraintLineExtractor.cs index 2da80bcae..99f01a50f 100644 --- a/DemoRenderer/Constraints/ConstraintLineExtractor.cs +++ b/DemoRenderer/Constraints/ConstraintLineExtractor.cs @@ -58,7 +58,7 @@ public unsafe override void ExtractLines(Bodies bodies, int setIndex, ref TypeBa for (int j = 0; j < bodyCount; ++j) { //Active set constraint body references refer directly to the body index. - bodyIndices[j] = GatherScatter.Get(ref Unsafe.Add(ref firstReference, j), innerIndex) & Bodies.BodyIndexMask; + bodyIndices[j] = GatherScatter.Get(ref Unsafe.Add(ref firstReference, j), innerIndex) & Bodies.BodyReferenceMask; } extractor.ExtractLines(ref GatherScatter.GetOffsetInstance(ref prestepBundle, innerIndex), setIndex, bodyIndices, bodies, ref tint, ref lines); } @@ -78,9 +78,9 @@ public unsafe override void ExtractLines(Bodies bodies, int setIndex, ref TypeBa for (int j = 0; j < bodyCount; ++j) { //Inactive constraints store body references in the form of handles, so we have to follow the indirection. - var bodyHandle = GatherScatter.Get(ref Unsafe.Add(ref firstReference, j), innerIndex); + var bodyHandle = GatherScatter.Get(ref Unsafe.Add(ref firstReference, j), innerIndex) & Bodies.BodyReferenceMask; Debug.Assert(bodies.HandleToLocation[bodyHandle].SetIndex == setIndex); - bodyIndices[j] = bodies.HandleToLocation[bodyHandle].Index & Bodies.BodyIndexMask; + bodyIndices[j] = bodies.HandleToLocation[bodyHandle].Index & Bodies.BodyReferenceMask; } extractor.ExtractLines(ref GatherScatter.GetOffsetInstance(ref prestepBundle, innerIndex), setIndex, bodyIndices, bodies, ref tint, ref lines); } diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index 669d8f52a..c710dcbb1 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -45,9 +45,9 @@ struct Option public DemoSet() { + AddOption(); AddOption(); AddOption(); - AddOption(); AddOption(); AddOption(); AddOption(); diff --git a/Demos/SpecializedTests/ConstraintTestDemo.cs b/Demos/SpecializedTests/ConstraintTestDemo.cs index 85eecea8c..919bc0ead 100644 --- a/Demos/SpecializedTests/ConstraintTestDemo.cs +++ b/Demos/SpecializedTests/ConstraintTestDemo.cs @@ -37,385 +37,385 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) shapeA.ComputeInertia(1, out var inertiaA); shapeA.ComputeInertia(1, out var inertiaB); var nextX = -10f; - { - var x = GetNextPosition(ref nextX); - var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); - var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - } - { - var x = GetNextPosition(ref nextX); - var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); - var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - Simulation.Solver.Add(a, b, new AngularHinge { LocalHingeAxisA = new Vector3(0, 1, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); - } - { - var x = GetNextPosition(ref nextX); - var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); - var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - Simulation.Solver.Add(a, b, new Hinge - { - LocalOffsetA = new Vector3(0, 1, 0), - LocalHingeAxisA = new Vector3(0, 1, 0), - LocalOffsetB = new Vector3(0, -1, 0), - LocalHingeAxisB = new Vector3(0, 1, 0), - SpringSettings = new SpringSettings(30, 1) - }); - } - { - var x = GetNextPosition(ref nextX); - var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); - var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - Simulation.Solver.Add(a, b, new AngularSwivelHinge { LocalSwivelAxisA = new Vector3(1, 0, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); - Simulation.Solver.Add(a, b, new SwingLimit { AxisLocalA = new Vector3(0, 1, 0), AxisLocalB = new Vector3(0, 1, 0), MaximumSwingAngle = MathHelper.PiOver2, SpringSettings = new SpringSettings(30, 1) }); - } - { - var x = GetNextPosition(ref nextX); - var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); - var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - Simulation.Solver.Add(a, b, new SwivelHinge - { - LocalOffsetA = new Vector3(0, 1, 0), - LocalSwivelAxisA = new Vector3(1, 0, 0), - LocalOffsetB = new Vector3(0, -1, 0), - LocalHingeAxisB = new Vector3(0, 1, 0), - SpringSettings = new SpringSettings(30, 1) - }); - Simulation.Solver.Add(a, b, new SwingLimit { AxisLocalA = new Vector3(0, 1, 0), AxisLocalB = new Vector3(0, 1, 0), MaximumSwingAngle = MathHelper.PiOver2, SpringSettings = new SpringSettings(30, 1) }); - } + //{ + // var x = GetNextPosition(ref nextX); + // var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); + // var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + // Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + //} { var x = GetNextPosition(ref nextX); var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - Simulation.Solver.Add(a, b, new TwistServo - { - LocalBasisA = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)), - LocalBasisB = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)), - TargetAngle = MathHelper.PiOver4, - SpringSettings = new SpringSettings(30, 1), - ServoSettings = new ServoSettings(float.MaxValue, 0, float.MaxValue) - }); - } - { - var x = GetNextPosition(ref nextX); - var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); - var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - Simulation.Solver.Add(a, b, new TwistLimit - { - LocalBasisA = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)), - LocalBasisB = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)), - MinimumAngle = MathHelper.Pi * -0.5f, - MaximumAngle = MathHelper.Pi * 0.95f, - SpringSettings = new SpringSettings(30, 1), - }); - Simulation.Solver.Add(a, b, new AngularHinge { LocalHingeAxisA = new Vector3(0, 1, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); - } - { - var x = GetNextPosition(ref nextX); - var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); - var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - Simulation.Solver.Add(a, b, new TwistMotor - { - LocalAxisA = new Vector3(0, 1, 0), - LocalAxisB = new Vector3(0, 1, 0), - TargetVelocity = MathHelper.Pi * 2, - Settings = new MotorSettings(float.MaxValue, 0.1f) - }); Simulation.Solver.Add(a, b, new AngularHinge { LocalHingeAxisA = new Vector3(0, 1, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); } - { - var x = GetNextPosition(ref nextX); - var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); - var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - Simulation.Solver.Add(a, b, new AngularServo - { - TargetRelativeRotationLocalA = QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), MathHelper.PiOver2), - ServoSettings = new ServoSettings(float.MaxValue, 0, 12f), - SpringSettings = new SpringSettings(30, 1) - }); - } - { - var x = GetNextPosition(ref nextX); - var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); - var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - Simulation.Solver.Add(a, b, new AngularMotor { TargetVelocityLocalA = new Vector3(0, 1, 0), Settings = new MotorSettings(15, 0.0001f) }); - } - { - var x = GetNextPosition(ref nextX); - var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); - var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity); - //aDescription.Velocity.Angular = new Vector3(0, 0, 5); - var a = Simulation.Bodies.Add(aDescription); - var b = Simulation.Bodies.Add(bDescription); - Simulation.Solver.Add(a, b, new Weld { LocalOffset = new Vector3(0, 2, 0), LocalOrientation = Quaternion.Identity, SpringSettings = new SpringSettings(30, 1) }); - } - { - var x = GetNextPosition(ref nextX); - var sphere = new Sphere(0.125f); - //Treat each vertex as a point mass that cannot rotate. - var sphereInertia = new BodyInertia { InverseMass = 1 }; - var sphereCollidable = new CollidableDescription(Simulation.Shapes.Add(sphere), 0.1f); - var a = new Vector3(x, 3, 0); - var b = new Vector3(x, 4, 0); - var c = new Vector3(x, 3, 1); - var d = new Vector3(x + 1, 3, 0); - var aDescription = BodyDescription.CreateDynamic(a, sphereInertia, sphereCollidable, activity); - var bDescription = BodyDescription.CreateDynamic(b, sphereInertia, sphereCollidable, activity); - var cDescription = BodyDescription.CreateDynamic(c, sphereInertia, sphereCollidable, activity); - var dDescription = BodyDescription.CreateDynamic(d, sphereInertia, sphereCollidable, activity); - var aHandle = Simulation.Bodies.Add(aDescription); - var bHandle = Simulation.Bodies.Add(bDescription); - var cHandle = Simulation.Bodies.Add(cDescription); - var dHandle = Simulation.Bodies.Add(dDescription); - var distanceSpringiness = new SpringSettings(3f, 1); - Simulation.Solver.Add(aHandle, bHandle, new CenterDistanceConstraint(Vector3.Distance(a, b), distanceSpringiness)); - Simulation.Solver.Add(aHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(a, c), distanceSpringiness)); - Simulation.Solver.Add(aHandle, dHandle, new CenterDistanceConstraint(Vector3.Distance(a, d), distanceSpringiness)); - Simulation.Solver.Add(bHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(b, c), distanceSpringiness)); - Simulation.Solver.Add(bHandle, dHandle, new CenterDistanceConstraint(Vector3.Distance(b, d), distanceSpringiness)); - Simulation.Solver.Add(cHandle, dHandle, new CenterDistanceConstraint(Vector3.Distance(c, d), distanceSpringiness)); - Simulation.Solver.Add(aHandle, bHandle, cHandle, dHandle, new VolumeConstraint(a, b, c, d, new SpringSettings(30, 1))); - } - { - var x = GetNextPosition(ref nextX); - var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); - var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); - var a = Simulation.Bodies.Add(aDescription); - var b = Simulation.Bodies.Add(bDescription); - Simulation.Solver.Add(a, b, new DistanceServo(new Vector3(0, 0.55f, 0), new Vector3(0, -0.55f, 0), 1.9f, new SpringSettings(30, 1), ServoSettings.Default)); - } - { - var x = GetNextPosition(ref nextX); - var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); - var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); - var a = Simulation.Bodies.Add(aDescription); - var b = Simulation.Bodies.Add(bDescription); - Simulation.Solver.Add(a, b, new DistanceLimit(new Vector3(0, 0.55f, 0), new Vector3(0, -0.55f, 0), 1f, 3, new SpringSettings(30, 1))); - } - { - var x = GetNextPosition(ref nextX); - var sphere = new Sphere(0.125f); - //Treat each vertex as a point mass that cannot rotate. - var sphereInertia = new BodyInertia { InverseMass = 1 }; - var sphereCollidable = new CollidableDescription(Simulation.Shapes.Add(sphere), 0.1f); - var a = new Vector3(x, 3, 0); - var b = new Vector3(x, 4, 0); - var c = new Vector3(x + 1, 3, 0); - var aDescription = BodyDescription.CreateDynamic(a, sphereInertia, sphereCollidable, activity); - var bDescription = BodyDescription.CreateDynamic(b, sphereInertia, sphereCollidable, activity); - var cDescription = BodyDescription.CreateDynamic(c, sphereInertia, sphereCollidable, activity); - var aHandle = Simulation.Bodies.Add(aDescription); - var bHandle = Simulation.Bodies.Add(bDescription); - var cHandle = Simulation.Bodies.Add(cDescription); - var distanceSpringiness = new SpringSettings(3f, 1); - Simulation.Solver.Add(aHandle, bHandle, new CenterDistanceConstraint(Vector3.Distance(a, b), distanceSpringiness)); - Simulation.Solver.Add(aHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(a, c), distanceSpringiness)); - Simulation.Solver.Add(bHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(b, c), distanceSpringiness)); - Simulation.Solver.Add(aHandle, bHandle, cHandle, new AreaConstraint(a, b, c, new SpringSettings(30, 1))); - } - { - var x = GetNextPosition(ref nextX); - var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), default, collidableA, activity); - var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); - var a = Simulation.Bodies.Add(aDescription); - var b = Simulation.Bodies.Add(bDescription); - Simulation.Solver.Add(a, b, new PointOnLineServo - { - LocalOffsetA = new Vector3(0, 0.5f, 0), - LocalOffsetB = new Vector3(0, -0.5f, 0), - LocalDirection = new Vector3(0, 1, 0), - SpringSettings = new SpringSettings(30, 1), - ServoSettings = ServoSettings.Default - }); - } - { - var x = GetNextPosition(ref nextX); - var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); - var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); - var a = Simulation.Bodies.Add(aDescription); - var b = Simulation.Bodies.Add(bDescription); - Simulation.Solver.Add(a, b, new LinearAxisServo - { - LocalOffsetA = new Vector3(0, 0.5f, 0), - LocalOffsetB = new Vector3(0, -0.5f, 0), - LocalPlaneNormal = new Vector3(0, 1, 0), - TargetOffset = 2, - SpringSettings = new SpringSettings(30, 1), - ServoSettings = ServoSettings.Default - }); - } - { - var x = GetNextPosition(ref nextX); - var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); - var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); - var a = Simulation.Bodies.Add(aDescription); - var b = Simulation.Bodies.Add(bDescription); - Simulation.Solver.Add(a, b, new PointOnLineServo - { - LocalOffsetA = new Vector3(0, 0.5f, 0), - LocalOffsetB = new Vector3(0, -0.5f, 0), - LocalDirection = new Vector3(0, 1, 0), - SpringSettings = new SpringSettings(30, 1), - ServoSettings = ServoSettings.Default - }); - Simulation.Solver.Add(a, b, new LinearAxisMotor - { - LocalOffsetA = new Vector3(0, 0.5f, 0), - LocalOffsetB = new Vector3(0, -0.5f, 0), - LocalAxis = new Vector3(0, 1, 0), - TargetVelocity = -2, - Settings = new MotorSettings(15, 0.01f) - }); - } - { - var x = GetNextPosition(ref nextX); - var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); - var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); - var a = Simulation.Bodies.Add(aDescription); - var b = Simulation.Bodies.Add(bDescription); - Simulation.Solver.Add(a, b, new PointOnLineServo - { - LocalOffsetA = new Vector3(0, 0.5f, 0), - LocalOffsetB = new Vector3(0, -0.5f, 0), - LocalDirection = new Vector3(0, 1, 0), - SpringSettings = new SpringSettings(30, 1), - ServoSettings = ServoSettings.Default - }); - Simulation.Solver.Add(a, b, new LinearAxisLimit - { - LocalOffsetA = new Vector3(0, 0.5f, 0), - LocalOffsetB = new Vector3(0, -0.5f, 0), - LocalAxis = new Vector3(0, 1, 0), - MinimumOffset = 1, - MaximumOffset = 2, - SpringSettings = new SpringSettings(30, 1) - }); - } - { - var x = GetNextPosition(ref nextX); - var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); - var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - Simulation.Solver.Add(b, a, new AngularAxisMotor - { - LocalAxisA = new Vector3(0, 1, 0), - TargetVelocity = MathHelper.Pi * 5, - Settings = new MotorSettings(float.MaxValue, 0.1f) - }); - } - { - var x = GetNextPosition(ref nextX); - var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); - Simulation.Solver.Add(a, new OneBodyLinearServo - { - LocalOffset = new Vector3(0, 1, 0), - Target = new Vector3(x, 3, 0), - ServoSettings = new ServoSettings(2, 0, float.MaxValue), - SpringSettings = new SpringSettings(5, 1) - }); - } - { - var x = GetNextPosition(ref nextX); - var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); - var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - Simulation.Solver.Add(a, new OneBodyLinearMotor - { - LocalOffset = new Vector3(0, 1, 0), - TargetVelocity = new Vector3(0, -1, 0), - Settings = new MotorSettings(float.MaxValue, 1e-2f), - }); - } - { - var x = GetNextPosition(ref nextX); - var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); - var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - Simulation.Solver.Add(a, new OneBodyAngularServo - { - TargetOrientation = Quaternion.Identity, - ServoSettings = ServoSettings.Default, - SpringSettings = new SpringSettings(30f, 1f) - }); - } - { - var x = GetNextPosition(ref nextX); - var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); - var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - Simulation.Solver.Add(a, new OneBodyAngularMotor - { - TargetVelocity = new Vector3(1, 0, 0), - Settings = new MotorSettings(float.MaxValue, 0.001f), - }); - } - { - var x = GetNextPosition(ref nextX); - var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); - var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); - var a = Simulation.Bodies.Add(aDescription); - var b = Simulation.Bodies.Add(bDescription); - Simulation.Solver.Add(a, b, new BallSocketMotor - { - LocalOffsetB = new Vector3(0, -1, 0), - TargetVelocityLocalA = new Vector3(0, -0.25f, 0), - Settings = new MotorSettings(10, 1e-4f) - }); - } - { - var x = GetNextPosition(ref nextX); - var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); - var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); - var a = Simulation.Bodies.Add(aDescription); - var b = Simulation.Bodies.Add(bDescription); - Simulation.Solver.Add(a, b, new BallSocketServo - { - LocalOffsetA = new Vector3(0, 1, 0), - LocalOffsetB = new Vector3(0, -1, 0), - SpringSettings = new SpringSettings(30, 1), - ServoSettings = new ServoSettings(100, 1, 100) - }); - } - { - var x = GetNextPosition(ref nextX); - var wheelShape = new CollidableDescription(Simulation.Shapes.Add(new Cylinder(1, 0.1f)), 0.1f); - var wheelOrientation = Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), MathF.PI * 0.5f); - var aDescription = BodyDescription.CreateDynamic(new RigidPose(new Vector3(x, 3, 0), wheelOrientation), inertiaA, wheelShape, activity); - var bDescription = BodyDescription.CreateDynamic(new RigidPose(new Vector3(x, 6, 0), wheelOrientation), inertiaB, wheelShape, activity); - var cDescription = BodyDescription.CreateKinematic(new Vector3(x, 4.5f, -1), new CollidableDescription(Simulation.Shapes.Add(new Box(3, 6, 1)), 0.1f), activity); - var a = Simulation.Bodies.Add(aDescription); - var b = Simulation.Bodies.Add(bDescription); - var c = Simulation.Bodies.Add(cDescription); - Simulation.Solver.Add(a, b, new AngularAxisGearMotor - { - LocalAxisA = new Vector3(0, 1, 0), - VelocityScale = -4, - Settings = new MotorSettings(float.MaxValue, 0.0001f) - }); - Simulation.Solver.Add(c, a, new Hinge - { - LocalOffsetA = new Vector3(0, -1.5f, 1), - LocalHingeAxisA = new Vector3(0, 0, 1), - LocalOffsetB = new Vector3(0, 0, 0), - LocalHingeAxisB = new Vector3(0, 1, 0), - SpringSettings = new SpringSettings(30, 1) - }); - Simulation.Solver.Add(c, b, new Hinge - { - LocalOffsetA = new Vector3(0, 1.5f, 1), - LocalHingeAxisA = new Vector3(0, 0, 1), - LocalOffsetB = new Vector3(0, 0, 0), - LocalHingeAxisB = new Vector3(0, 1, 0), - SpringSettings = new SpringSettings(30, 1) - }); - } + //{ + // var x = GetNextPosition(ref nextX); + // var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); + // var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + // Simulation.Solver.Add(a, b, new Hinge + // { + // LocalOffsetA = new Vector3(0, 1, 0), + // LocalHingeAxisA = new Vector3(0, 1, 0), + // LocalOffsetB = new Vector3(0, -1, 0), + // LocalHingeAxisB = new Vector3(0, 1, 0), + // SpringSettings = new SpringSettings(30, 1) + // }); + //} + //{ + // var x = GetNextPosition(ref nextX); + // var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); + // var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + // Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + // Simulation.Solver.Add(a, b, new AngularSwivelHinge { LocalSwivelAxisA = new Vector3(1, 0, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); + // Simulation.Solver.Add(a, b, new SwingLimit { AxisLocalA = new Vector3(0, 1, 0), AxisLocalB = new Vector3(0, 1, 0), MaximumSwingAngle = MathHelper.PiOver2, SpringSettings = new SpringSettings(30, 1) }); + //} + //{ + // var x = GetNextPosition(ref nextX); + // var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); + // var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + // Simulation.Solver.Add(a, b, new SwivelHinge + // { + // LocalOffsetA = new Vector3(0, 1, 0), + // LocalSwivelAxisA = new Vector3(1, 0, 0), + // LocalOffsetB = new Vector3(0, -1, 0), + // LocalHingeAxisB = new Vector3(0, 1, 0), + // SpringSettings = new SpringSettings(30, 1) + // }); + // Simulation.Solver.Add(a, b, new SwingLimit { AxisLocalA = new Vector3(0, 1, 0), AxisLocalB = new Vector3(0, 1, 0), MaximumSwingAngle = MathHelper.PiOver2, SpringSettings = new SpringSettings(30, 1) }); + //} + //{ + // var x = GetNextPosition(ref nextX); + // var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); + // var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + // Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + // Simulation.Solver.Add(a, b, new TwistServo + // { + // LocalBasisA = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)), + // LocalBasisB = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)), + // TargetAngle = MathHelper.PiOver4, + // SpringSettings = new SpringSettings(30, 1), + // ServoSettings = new ServoSettings(float.MaxValue, 0, float.MaxValue) + // }); + //} + //{ + // var x = GetNextPosition(ref nextX); + // var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); + // var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + // Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + // Simulation.Solver.Add(a, b, new TwistLimit + // { + // LocalBasisA = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)), + // LocalBasisB = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)), + // MinimumAngle = MathHelper.Pi * -0.5f, + // MaximumAngle = MathHelper.Pi * 0.95f, + // SpringSettings = new SpringSettings(30, 1), + // }); + // Simulation.Solver.Add(a, b, new AngularHinge { LocalHingeAxisA = new Vector3(0, 1, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); + //} + //{ + // var x = GetNextPosition(ref nextX); + // var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); + // var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + // Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + // Simulation.Solver.Add(a, b, new TwistMotor + // { + // LocalAxisA = new Vector3(0, 1, 0), + // LocalAxisB = new Vector3(0, 1, 0), + // TargetVelocity = MathHelper.Pi * 2, + // Settings = new MotorSettings(float.MaxValue, 0.1f) + // }); + // Simulation.Solver.Add(a, b, new AngularHinge { LocalHingeAxisA = new Vector3(0, 1, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); + //} + //{ + // var x = GetNextPosition(ref nextX); + // var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); + // var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + // Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + // Simulation.Solver.Add(a, b, new AngularServo + // { + // TargetRelativeRotationLocalA = QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), MathHelper.PiOver2), + // ServoSettings = new ServoSettings(float.MaxValue, 0, 12f), + // SpringSettings = new SpringSettings(30, 1) + // }); + //} + //{ + // var x = GetNextPosition(ref nextX); + // var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); + // var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + // Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + // Simulation.Solver.Add(a, b, new AngularMotor { TargetVelocityLocalA = new Vector3(0, 1, 0), Settings = new MotorSettings(15, 0.0001f) }); + //} + //{ + // var x = GetNextPosition(ref nextX); + // var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); + // var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity); + // //aDescription.Velocity.Angular = new Vector3(0, 0, 5); + // var a = Simulation.Bodies.Add(aDescription); + // var b = Simulation.Bodies.Add(bDescription); + // Simulation.Solver.Add(a, b, new Weld { LocalOffset = new Vector3(0, 2, 0), LocalOrientation = Quaternion.Identity, SpringSettings = new SpringSettings(30, 1) }); + //} + //{ + // var x = GetNextPosition(ref nextX); + // var sphere = new Sphere(0.125f); + // //Treat each vertex as a point mass that cannot rotate. + // var sphereInertia = new BodyInertia { InverseMass = 1 }; + // var sphereCollidable = new CollidableDescription(Simulation.Shapes.Add(sphere), 0.1f); + // var a = new Vector3(x, 3, 0); + // var b = new Vector3(x, 4, 0); + // var c = new Vector3(x, 3, 1); + // var d = new Vector3(x + 1, 3, 0); + // var aDescription = BodyDescription.CreateDynamic(a, sphereInertia, sphereCollidable, activity); + // var bDescription = BodyDescription.CreateDynamic(b, sphereInertia, sphereCollidable, activity); + // var cDescription = BodyDescription.CreateDynamic(c, sphereInertia, sphereCollidable, activity); + // var dDescription = BodyDescription.CreateDynamic(d, sphereInertia, sphereCollidable, activity); + // var aHandle = Simulation.Bodies.Add(aDescription); + // var bHandle = Simulation.Bodies.Add(bDescription); + // var cHandle = Simulation.Bodies.Add(cDescription); + // var dHandle = Simulation.Bodies.Add(dDescription); + // var distanceSpringiness = new SpringSettings(3f, 1); + // Simulation.Solver.Add(aHandle, bHandle, new CenterDistanceConstraint(Vector3.Distance(a, b), distanceSpringiness)); + // Simulation.Solver.Add(aHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(a, c), distanceSpringiness)); + // Simulation.Solver.Add(aHandle, dHandle, new CenterDistanceConstraint(Vector3.Distance(a, d), distanceSpringiness)); + // Simulation.Solver.Add(bHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(b, c), distanceSpringiness)); + // Simulation.Solver.Add(bHandle, dHandle, new CenterDistanceConstraint(Vector3.Distance(b, d), distanceSpringiness)); + // Simulation.Solver.Add(cHandle, dHandle, new CenterDistanceConstraint(Vector3.Distance(c, d), distanceSpringiness)); + // Simulation.Solver.Add(aHandle, bHandle, cHandle, dHandle, new VolumeConstraint(a, b, c, d, new SpringSettings(30, 1))); + //} + //{ + // var x = GetNextPosition(ref nextX); + // var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); + // var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); + // var a = Simulation.Bodies.Add(aDescription); + // var b = Simulation.Bodies.Add(bDescription); + // Simulation.Solver.Add(a, b, new DistanceServo(new Vector3(0, 0.55f, 0), new Vector3(0, -0.55f, 0), 1.9f, new SpringSettings(30, 1), ServoSettings.Default)); + //} + //{ + // var x = GetNextPosition(ref nextX); + // var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); + // var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); + // var a = Simulation.Bodies.Add(aDescription); + // var b = Simulation.Bodies.Add(bDescription); + // Simulation.Solver.Add(a, b, new DistanceLimit(new Vector3(0, 0.55f, 0), new Vector3(0, -0.55f, 0), 1f, 3, new SpringSettings(30, 1))); + //} + //{ + // var x = GetNextPosition(ref nextX); + // var sphere = new Sphere(0.125f); + // //Treat each vertex as a point mass that cannot rotate. + // var sphereInertia = new BodyInertia { InverseMass = 1 }; + // var sphereCollidable = new CollidableDescription(Simulation.Shapes.Add(sphere), 0.1f); + // var a = new Vector3(x, 3, 0); + // var b = new Vector3(x, 4, 0); + // var c = new Vector3(x + 1, 3, 0); + // var aDescription = BodyDescription.CreateDynamic(a, sphereInertia, sphereCollidable, activity); + // var bDescription = BodyDescription.CreateDynamic(b, sphereInertia, sphereCollidable, activity); + // var cDescription = BodyDescription.CreateDynamic(c, sphereInertia, sphereCollidable, activity); + // var aHandle = Simulation.Bodies.Add(aDescription); + // var bHandle = Simulation.Bodies.Add(bDescription); + // var cHandle = Simulation.Bodies.Add(cDescription); + // var distanceSpringiness = new SpringSettings(3f, 1); + // Simulation.Solver.Add(aHandle, bHandle, new CenterDistanceConstraint(Vector3.Distance(a, b), distanceSpringiness)); + // Simulation.Solver.Add(aHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(a, c), distanceSpringiness)); + // Simulation.Solver.Add(bHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(b, c), distanceSpringiness)); + // Simulation.Solver.Add(aHandle, bHandle, cHandle, new AreaConstraint(a, b, c, new SpringSettings(30, 1))); + //} + //{ + // var x = GetNextPosition(ref nextX); + // var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), default, collidableA, activity); + // var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); + // var a = Simulation.Bodies.Add(aDescription); + // var b = Simulation.Bodies.Add(bDescription); + // Simulation.Solver.Add(a, b, new PointOnLineServo + // { + // LocalOffsetA = new Vector3(0, 0.5f, 0), + // LocalOffsetB = new Vector3(0, -0.5f, 0), + // LocalDirection = new Vector3(0, 1, 0), + // SpringSettings = new SpringSettings(30, 1), + // ServoSettings = ServoSettings.Default + // }); + //} + //{ + // var x = GetNextPosition(ref nextX); + // var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); + // var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); + // var a = Simulation.Bodies.Add(aDescription); + // var b = Simulation.Bodies.Add(bDescription); + // Simulation.Solver.Add(a, b, new LinearAxisServo + // { + // LocalOffsetA = new Vector3(0, 0.5f, 0), + // LocalOffsetB = new Vector3(0, -0.5f, 0), + // LocalPlaneNormal = new Vector3(0, 1, 0), + // TargetOffset = 2, + // SpringSettings = new SpringSettings(30, 1), + // ServoSettings = ServoSettings.Default + // }); + //} + //{ + // var x = GetNextPosition(ref nextX); + // var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); + // var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); + // var a = Simulation.Bodies.Add(aDescription); + // var b = Simulation.Bodies.Add(bDescription); + // Simulation.Solver.Add(a, b, new PointOnLineServo + // { + // LocalOffsetA = new Vector3(0, 0.5f, 0), + // LocalOffsetB = new Vector3(0, -0.5f, 0), + // LocalDirection = new Vector3(0, 1, 0), + // SpringSettings = new SpringSettings(30, 1), + // ServoSettings = ServoSettings.Default + // }); + // Simulation.Solver.Add(a, b, new LinearAxisMotor + // { + // LocalOffsetA = new Vector3(0, 0.5f, 0), + // LocalOffsetB = new Vector3(0, -0.5f, 0), + // LocalAxis = new Vector3(0, 1, 0), + // TargetVelocity = -2, + // Settings = new MotorSettings(15, 0.01f) + // }); + //} + //{ + // var x = GetNextPosition(ref nextX); + // var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); + // var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); + // var a = Simulation.Bodies.Add(aDescription); + // var b = Simulation.Bodies.Add(bDescription); + // Simulation.Solver.Add(a, b, new PointOnLineServo + // { + // LocalOffsetA = new Vector3(0, 0.5f, 0), + // LocalOffsetB = new Vector3(0, -0.5f, 0), + // LocalDirection = new Vector3(0, 1, 0), + // SpringSettings = new SpringSettings(30, 1), + // ServoSettings = ServoSettings.Default + // }); + // Simulation.Solver.Add(a, b, new LinearAxisLimit + // { + // LocalOffsetA = new Vector3(0, 0.5f, 0), + // LocalOffsetB = new Vector3(0, -0.5f, 0), + // LocalAxis = new Vector3(0, 1, 0), + // MinimumOffset = 1, + // MaximumOffset = 2, + // SpringSettings = new SpringSettings(30, 1) + // }); + //} + //{ + // var x = GetNextPosition(ref nextX); + // var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); + // var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + // Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + // Simulation.Solver.Add(b, a, new AngularAxisMotor + // { + // LocalAxisA = new Vector3(0, 1, 0), + // TargetVelocity = MathHelper.Pi * 5, + // Settings = new MotorSettings(float.MaxValue, 0.1f) + // }); + //} + //{ + // var x = GetNextPosition(ref nextX); + // var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); + // Simulation.Solver.Add(a, new OneBodyLinearServo + // { + // LocalOffset = new Vector3(0, 1, 0), + // Target = new Vector3(x, 3, 0), + // ServoSettings = new ServoSettings(2, 0, float.MaxValue), + // SpringSettings = new SpringSettings(5, 1) + // }); + //} + //{ + // var x = GetNextPosition(ref nextX); + // var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); + // var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + // Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + // Simulation.Solver.Add(a, new OneBodyLinearMotor + // { + // LocalOffset = new Vector3(0, 1, 0), + // TargetVelocity = new Vector3(0, -1, 0), + // Settings = new MotorSettings(float.MaxValue, 1e-2f), + // }); + //} + //{ + // var x = GetNextPosition(ref nextX); + // var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); + // var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + // Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + // Simulation.Solver.Add(a, new OneBodyAngularServo + // { + // TargetOrientation = Quaternion.Identity, + // ServoSettings = ServoSettings.Default, + // SpringSettings = new SpringSettings(30f, 1f) + // }); + //} + //{ + // var x = GetNextPosition(ref nextX); + // var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); + // var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + // Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + // Simulation.Solver.Add(a, new OneBodyAngularMotor + // { + // TargetVelocity = new Vector3(1, 0, 0), + // Settings = new MotorSettings(float.MaxValue, 0.001f), + // }); + //} + //{ + // var x = GetNextPosition(ref nextX); + // var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); + // var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); + // var a = Simulation.Bodies.Add(aDescription); + // var b = Simulation.Bodies.Add(bDescription); + // Simulation.Solver.Add(a, b, new BallSocketMotor + // { + // LocalOffsetB = new Vector3(0, -1, 0), + // TargetVelocityLocalA = new Vector3(0, -0.25f, 0), + // Settings = new MotorSettings(10, 1e-4f) + // }); + //} + //{ + // var x = GetNextPosition(ref nextX); + // var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); + // var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); + // var a = Simulation.Bodies.Add(aDescription); + // var b = Simulation.Bodies.Add(bDescription); + // Simulation.Solver.Add(a, b, new BallSocketServo + // { + // LocalOffsetA = new Vector3(0, 1, 0), + // LocalOffsetB = new Vector3(0, -1, 0), + // SpringSettings = new SpringSettings(30, 1), + // ServoSettings = new ServoSettings(100, 1, 100) + // }); + //} + //{ + // var x = GetNextPosition(ref nextX); + // var wheelShape = new CollidableDescription(Simulation.Shapes.Add(new Cylinder(1, 0.1f)), 0.1f); + // var wheelOrientation = Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), MathF.PI * 0.5f); + // var aDescription = BodyDescription.CreateDynamic(new RigidPose(new Vector3(x, 3, 0), wheelOrientation), inertiaA, wheelShape, activity); + // var bDescription = BodyDescription.CreateDynamic(new RigidPose(new Vector3(x, 6, 0), wheelOrientation), inertiaB, wheelShape, activity); + // var cDescription = BodyDescription.CreateKinematic(new Vector3(x, 4.5f, -1), new CollidableDescription(Simulation.Shapes.Add(new Box(3, 6, 1)), 0.1f), activity); + // var a = Simulation.Bodies.Add(aDescription); + // var b = Simulation.Bodies.Add(bDescription); + // var c = Simulation.Bodies.Add(cDescription); + // Simulation.Solver.Add(a, b, new AngularAxisGearMotor + // { + // LocalAxisA = new Vector3(0, 1, 0), + // VelocityScale = -4, + // Settings = new MotorSettings(float.MaxValue, 0.0001f) + // }); + // Simulation.Solver.Add(c, a, new Hinge + // { + // LocalOffsetA = new Vector3(0, -1.5f, 1), + // LocalHingeAxisA = new Vector3(0, 0, 1), + // LocalOffsetB = new Vector3(0, 0, 0), + // LocalHingeAxisB = new Vector3(0, 1, 0), + // SpringSettings = new SpringSettings(30, 1) + // }); + // Simulation.Solver.Add(c, b, new Hinge + // { + // LocalOffsetA = new Vector3(0, 1.5f, 1), + // LocalHingeAxisA = new Vector3(0, 0, 1), + // LocalOffsetB = new Vector3(0, 0, 0), + // LocalHingeAxisB = new Vector3(0, 1, 0), + // SpringSettings = new SpringSettings(30, 1) + // }); + //} Simulation.Statics.Add(new StaticDescription(new Vector3(), new CollidableDescription(Simulation.Shapes.Add(new Box(256, 1, 256)), 0.1f))); } diff --git a/Demos/SpecializedTests/SimulationScrambling.cs b/Demos/SpecializedTests/SimulationScrambling.cs index 52d29eeb2..4f757a3b9 100644 --- a/Demos/SpecializedTests/SimulationScrambling.cs +++ b/Demos/SpecializedTests/SimulationScrambling.cs @@ -89,7 +89,7 @@ public BodyEnumerator(Bodies bodies, int[] handleToEntryIndex) [MethodImpl(MethodImplOptions.AggressiveInlining)] public void LoopBody(int encodedBodyIndex) { - var bodyIndex = encodedBodyIndex & Bodies.BodyIndexMask; + var bodyIndex = encodedBodyIndex & Bodies.BodyReferenceMask; var entryIndex = HandlesToIdentity[Bodies.ActiveSet.IndexToHandle[bodyIndex].Value]; if (IndexInConstraint == 0) IdentityA = entryIndex; @@ -118,7 +118,7 @@ public void LoopBody(int encodedBodyIndex) //The body in this constraint should both: //1) have a handle associated with it, and //2) the constraint graph list for the body should include the constraint handle. - var bodyIndex = encodedBodyIndex & Bodies.BodyIndexMask; + var bodyIndex = encodedBodyIndex & Bodies.BodyReferenceMask; Debug.Assert(Simulation.Bodies.ActiveSet.IndexToHandle[bodyIndex].Value >= 0); Debug.Assert(Simulation.Bodies.ActiveSet.BodyIsConstrainedBy(bodyIndex, ConstraintHandle)); } From d0274d099e2171f555d98cac5453ea15686680fe Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 1 Nov 2021 21:02:09 -0500 Subject: [PATCH 255/947] Comment update. --- BepuPhysics/Constraints/TypeProcessor.cs | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index 6e6ff785e..cb296edc3 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -80,10 +80,7 @@ public void Initialize(int typeId) public unsafe abstract void TransferConstraint(ref TypeBatch typeBatch, int sourceBatchIndex, int indexInTypeBatch, Solver solver, Bodies bodies, int targetBatchIndex); /// - /// Enumerates body references in the constraint. - /// For waking constraints, this enumerates body indices including any encoded metadata like whether the body is kinematic. - /// For sleeping constraints, this enumerates body handles. - /// In other words, this reports whatever is stored in the constraint. + /// Enumerates body references in the constraint. Reported body references (body index for an awake constraint, body handle for a sleeping constraint) include encoded metadata like whether the body is kinematic. /// /// Type of the enumerator called for each body index in the constraint. /// Type batch containing the constraint to enumerate. From b5f57de4eab0ebff7df5d873927acb1c6c780446 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 2 Nov 2021 18:26:14 -0500 Subject: [PATCH 256/947] Sleeping->active for fallbacks now preserves kinematic flag. BatchCompressor compression application now uses dynamic body handles correctly. Kinematic flag validation now checks all sets. --- BepuPhysics/BatchCompressor.cs | 10 ++--- BepuPhysics/Constraints/TypeProcessor.cs | 6 ++- BepuPhysics/Solver.cs | 49 ++++++++++++++++-------- 3 files changed, 41 insertions(+), 24 deletions(-) diff --git a/BepuPhysics/BatchCompressor.cs b/BepuPhysics/BatchCompressor.cs index 2cc9f7d3a..03ac0bb06 100644 --- a/BepuPhysics/BatchCompressor.cs +++ b/BepuPhysics/BatchCompressor.cs @@ -206,13 +206,13 @@ private unsafe void ApplyCompression(int sourceBatchIndex, ref ConstraintBatch s //We're optimizing the fallback batch, so we need to be careful about compressions interfering with each other. The parallel analysis assumed each batch //contained at most one instance of each body, which doesn't hold for the fallback batch. //Easy enough to address: check to see if the target batch can still hold the constraint. - var bodyHandles = stackalloc int[typeProcessor.BodiesPerConstraint]; - ActiveConstraintBodyHandleCollector handleAccumulator; + var dynamicBodyHandles = stackalloc int[typeProcessor.BodiesPerConstraint]; + ActiveConstraintDynamicBodyHandleCollector handleAccumulator; handleAccumulator.Bodies = Bodies; - handleAccumulator.Handles = bodyHandles; + handleAccumulator.Handles = dynamicBodyHandles; handleAccumulator.Count = 0; - Solver.EnumerateActiveDynamicConnectedBodyIndices(compression.ConstraintHandle, ref handleAccumulator); - if (!Solver.batchReferencedHandles[compression.TargetBatch].CanFit(new Span(bodyHandles, typeProcessor.BodiesPerConstraint))) + Solver.EnumerateConnectedRawBodyReferences(compression.ConstraintHandle, ref handleAccumulator); + if (!Solver.batchReferencedHandles[compression.TargetBatch].CanFit(new Span(dynamicBodyHandles, handleAccumulator.Count))) { //Another compression from the fallback batch has blocked this compression. //Note that this isn't really a problem- batch compression is an incremental process. If some other compression was possible, a future frame will find it pretty quickly. diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index cb296edc3..435a5b1a7 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -985,9 +985,11 @@ internal unsafe sealed override void AddSleepingToActiveForFallback(int sourceSe for (int i = 0; i < bodyIndices.Length; ++i) { //Bodies have already been moved into the active set, so we can use the mapping. - var bodyHandleValue = Unsafe.Add(ref bodyReferencesLane, Vector.Count * i); + var encodedBodyHandleValue = Unsafe.Add(ref bodyReferencesLane, Vector.Count * i); + var bodyHandleValue = encodedBodyHandleValue & Bodies.BodyReferenceMask; Debug.Assert(bodyHandleToLocation[bodyHandleValue].SetIndex == 0); - bodyIndices[i] = bodyHandleToLocation[bodyHandleValue].Index; + //Preserve the kinematic flag when converting from handle to index. + bodyIndices[i] = bodyHandleToLocation[bodyHandleValue].Index | (encodedBodyHandleValue & Bodies.KinematicMask); } var handle = sourceTypeBatch.IndexToHandle[sourceIndex]; Debug.Assert(constraintHandleToLocation[handle.Value].SetIndex == sourceSet); diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index 91d5dce07..c3d095d45 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -267,7 +267,6 @@ public bool ConstraintExists(ConstraintHandle constraintHandle) /// Gets a direct reference to the constraint associated with a handle. /// The reference is temporary; any constraint removals that affect the referenced type batch may invalidate the index. /// - /// Type of the type batch being referred to. /// Handle index of the constraint. /// Temporary direct reference to the type batch and index in the type batch associated with the constraint handle. /// May be invalidated by constraint removals. @@ -283,27 +282,43 @@ public unsafe void GetConstraintReference(ConstraintHandle handle, out Constrain unsafe internal void ValidateConstraintReferenceKinematicity() { //Only the active set's body indices are flagged for kinematicity; the inactive sets store body handles. - ref var activeSet = ref ActiveSet; - for (int batchIndex = 0; batchIndex < activeSet.Batches.Count; ++batchIndex) + for (int setIndex = 0; setIndex < Sets.Length; ++setIndex) { - ref var batch = ref activeSet.Batches[batchIndex]; - for (int typeBatchIndex = 0; typeBatchIndex < batch.TypeBatches.Count; ++typeBatchIndex) + ref var set = ref Sets[setIndex]; + if (set.Allocated) { - ref var typeBatch = ref batch.TypeBatches[typeBatchIndex]; - var bodiesPerConstraint = TypeProcessors[typeBatch.TypeId].BodiesPerConstraint; - for (int i = 0; i < typeBatch.ConstraintCount; ++i) + for (int batchIndex = 0; batchIndex < set.Batches.Count; ++batchIndex) { - BundleIndexing.GetBundleIndices(i, out var bundleIndex, out var innerIndex); - ref var bodyReferencesBundle = ref typeBatch.BodyReferences[bundleIndex * bodiesPerConstraint * Unsafe.SizeOf>()]; - for (int bodyIndexInConstraint = 0; bodyIndexInConstraint < bodiesPerConstraint; ++bodyIndexInConstraint) + ref var batch = ref set.Batches[batchIndex]; + for (int typeBatchIndex = 0; typeBatchIndex < batch.TypeBatches.Count; ++typeBatchIndex) { - var referencesForBodyIndexInConstraint = Unsafe.Add(ref Unsafe.As>(ref bodyReferencesBundle), bodyIndexInConstraint); - var encodedBodyIndex = referencesForBodyIndexInConstraint[innerIndex]; - if (encodedBodyIndex > 0) + ref var typeBatch = ref batch.TypeBatches[typeBatchIndex]; + var bodiesPerConstraint = TypeProcessors[typeBatch.TypeId].BodiesPerConstraint; + for (int i = 0; i < typeBatch.ConstraintCount; ++i) { - var kinematicByEncodedIndex = (encodedBodyIndex & Bodies.KinematicMask) != 0; - var kinematicByInertia = Bodies.IsKinematicUnsafeGCHole(ref bodies.ActiveSet.SolverStates[encodedBodyIndex & Bodies.BodyReferenceMask].Inertia.Local); - Debug.Assert(kinematicByEncodedIndex == kinematicByInertia, "Constraint reference encoded kinematicity must match actual kinematicity by inertia."); + BundleIndexing.GetBundleIndices(i, out var bundleIndex, out var innerIndex); + ref var bodyReferencesBundle = ref typeBatch.BodyReferences[bundleIndex * bodiesPerConstraint * Unsafe.SizeOf>()]; + for (int bodyIndexInConstraint = 0; bodyIndexInConstraint < bodiesPerConstraint; ++bodyIndexInConstraint) + { + var referencesForBodyIndexInConstraint = Unsafe.Add(ref Unsafe.As>(ref bodyReferencesBundle), bodyIndexInConstraint); + var encodedBodyReference = referencesForBodyIndexInConstraint[innerIndex]; + if (encodedBodyReference > 0) + { + var kinematicByEncodedReference = (encodedBodyReference & Bodies.KinematicMask) != 0; + bool kinematicByInertia; + if (setIndex == 0) + { + //Active set references are indices. + kinematicByInertia = Bodies.IsKinematicUnsafeGCHole(ref bodies.ActiveSet.SolverStates[encodedBodyReference & Bodies.BodyReferenceMask].Inertia.Local); + } + else + { + //Sleeping set references are handles. + kinematicByInertia = bodies.GetBodyReference(new BodyHandle { Value = encodedBodyReference & Bodies.BodyReferenceMask }).Kinematic; + } + Debug.Assert(kinematicByEncodedReference == kinematicByInertia, "Constraint reference encoded kinematicity must match actual kinematicity by inertia."); + } + } } } } From d7d1e6cf9cac645e44c7ea5ce016bf2b809e395e Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 2 Nov 2021 19:30:02 -0500 Subject: [PATCH 257/947] Fixed AddWakingBodyHandlesToBatchReferences; now correctly applies offset to addresses. Simplified awakener's CopyBodyRegion handling of inertia given substepping exclusivity. Update validation for kinematic handle flags. Added some missing XML docs. --- BepuPhysics/Constraints/TypeProcessor.cs | 4 ++-- BepuPhysics/EmbeddedSubsteppingTimestepper2.cs | 3 ++- BepuPhysics/IslandAwakener.cs | 17 +---------------- BepuPhysics/Solver.cs | 13 ++++++++++--- 4 files changed, 15 insertions(+), 22 deletions(-) diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index 435a5b1a7..7e2a05943 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -1075,7 +1075,7 @@ ref Buffer.Get(ref sourceTypeBatch.AccumulatedImpulses, sou var offset = 0; for (int j = 0; j < bodiesPerConstraint; ++j) { - var encodedBodyHandle = Unsafe.Add(ref sourceReferencesLaneStart, offset); + var encodedBodyHandle = Unsafe.Add(ref sourceReferencesLaneStart, offset); //Note that encoded kinematicity flags are carried over to the active index reference. Unsafe.Add(ref targetReferencesLaneStart, offset) = bodies.HandleToLocation[encodedBodyHandle & Bodies.BodyReferenceMask].Index | (encodedBodyHandle & Bodies.KinematicMask); offset += Vector.Count; @@ -1110,8 +1110,8 @@ internal unsafe sealed override void AddWakingBodyHandlesToBatchReferences(ref T //to resize the batch referenced handles structure. //Note that this will happily set an existing bit if the target batch is the fallback batch. targetBatchReferencedHandles.SetUnsafely(encodedBodyHandle); - offset += Vector.Count; } + offset += Vector.Count; } } } diff --git a/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs b/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs index 2cc7d138b..b546f9b20 100644 --- a/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs +++ b/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs @@ -64,7 +64,8 @@ public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDi simulation.Solver.ValidateConstraintReferenceKinematicity(); simulation.Solver.ValidateConstrainedKinematicsSet(); simulation.Solver.ValidateFallbackBodiesAreDynamic(); - //simulation.Solver.ValidateExistingHandles(); + simulation.Solver.ValidateExistingHandles(); + var constrainedBodySet = simulation.Solver.PrepareConstraintIntegrationResponsibilities(SubstepCount, threadDispatcher); simulation.Profiler.Start(simulation.Solver); simulation.Solver.SolveStep2(dt, threadDispatcher); diff --git a/BepuPhysics/IslandAwakener.cs b/BepuPhysics/IslandAwakener.cs index d022595cc..80c92a3c2 100644 --- a/BepuPhysics/IslandAwakener.cs +++ b/BepuPhysics/IslandAwakener.cs @@ -295,22 +295,7 @@ internal unsafe void ExecutePhaseOneJob(int index) ref var targetSet = ref bodies.ActiveSet; sourceSet.Collidables.CopyTo(job.SourceStart, targetSet.Collidables, job.TargetStart, job.Count); sourceSet.Constraints.CopyTo(job.SourceStart, targetSet.Constraints, job.TargetStart, job.Count); - //The world inertias must be updated as well. They are stored outside the sets. - //Note that we use a manual loop copy for the local inertias and motion state since we're accessing them during the world inertia calculation anyway. - //This can worsen the copy codegen a little, but it means we only have to scan the memory once. - //(Realistically, either option is fast- these regions won't tend to fill L1.) - for (int i = 0; i < job.Count; ++i) - { - var sourceIndex = job.SourceStart + i; - var targetIndex = job.TargetStart + i; - ref var sourceState = ref sourceSet.SolverStates[sourceIndex]; - ref var targetState = ref targetSet.SolverStates[targetIndex]; - targetState = sourceState; - //TODO: In principle, if velocity integration is always a part of the solver, then there is no need for this. That's not the case in 2.3.0, but embedded substepping should change that. - //Leaving this here for now so we don't break the other substeppers (and because it's a fairly tiny concern), but something to consider later. - PoseIntegration.RotateInverseInertia(sourceState.Inertia.Local.InverseInertiaTensor, sourceState.Motion.Pose.Orientation, out targetState.Inertia.World.InverseInertiaTensor); - targetState.Inertia.Local.InverseMass = sourceState.Inertia.Local.InverseMass; - } + sourceSet.SolverStates.CopyTo(job.SourceStart, targetSet.SolverStates, job.TargetStart, job.Count); sourceSet.Activity.CopyTo(job.SourceStart, targetSet.Activity, job.TargetStart, job.Count); if (resetActivityStates) { diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index c3d095d45..cbc669bc6 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -728,7 +728,7 @@ internal unsafe void ValidateExistingHandles(bool activeOnly = false) } else { - ref var referencedBodyLocation = ref bodies.HandleToLocation[constraintBodyReferences[i]]; + ref var referencedBodyLocation = ref bodies.HandleToLocation[constraintBodyReferences[i] & Bodies.BodyReferenceMask]; Debug.Assert(referencedBodyLocation.SetIndex == setIndex, "Any body involved with a constraint should be in the same set."); bodyIndex = referencedBodyLocation.Index; } @@ -754,8 +754,7 @@ internal unsafe void ValidateExistingHandles(bool activeOnly = false) //Active constraints refer to bodies by index; inactive constraints use handles. int bodyReference = setIndex == 0 ? bodyIndex : bodySet.IndexToHandle[bodyIndex].Value; var bodyReferenceInConstraint = constraintBodyReferences[constraintList[constraintIndex].BodyIndexInConstraint]; - if (setIndex == 0) - bodyReferenceInConstraint &= Bodies.BodyReferenceMask; + bodyReferenceInConstraint &= Bodies.BodyReferenceMask; Debug.Assert(bodyReferenceInConstraint == bodyReference, "If a body refers to a constraint, the constraint should refer to the body."); } @@ -1093,6 +1092,7 @@ void Add(Span bodyHandles, in TDescription description /// /// Type of the constraint description to add. /// Body handles used by the constraint. + /// Description of the constraint to add. /// Allocated constraint handle. public ConstraintHandle Add(Span bodyHandles, in TDescription description) where TDescription : unmanaged, IConstraintDescription @@ -1103,10 +1103,12 @@ public ConstraintHandle Add(Span bodyHandles, in TDesc Debug.Assert(bodyHandles.Length == TypeProcessors[description.ConstraintTypeId].BodiesPerConstraint, "The number of bodies supplied to a constraint add must match the expected number of bodies involved in that constraint type. Did you use the wrong Solver.Add overload?"); //Adding a constraint assumes that the involved bodies are active, so wake up anything that is sleeping. + ValidateExistingHandles(); for (int i = 0; i < bodyHandles.Length; ++i) { awakener.AwakenBody(bodyHandles[i]); } + ValidateExistingHandles(); Add(bodyHandles, description, out var constraintHandle); for (int i = 0; i < bodyHandles.Length; ++i) { @@ -1114,6 +1116,7 @@ public ConstraintHandle Add(Span bodyHandles, in TDesc bodies.ValidateExistingHandle(bodyHandle); bodies.AddConstraint(bodies.HandleToLocation[bodyHandle.Value].Index, constraintHandle, i); } + ValidateExistingHandles(); return constraintHandle; } @@ -1122,6 +1125,7 @@ public ConstraintHandle Add(Span bodyHandles, in TDesc /// /// Type of the constraint description to add. /// Body connected to the constraint. + /// Description of the constraint to add. /// Allocated constraint handle. public unsafe ConstraintHandle Add(BodyHandle bodyHandle, in TDescription description) where TDescription : unmanaged, IOneBodyConstraintDescription @@ -1136,6 +1140,7 @@ public unsafe ConstraintHandle Add(BodyHandle bodyHandle, in TDesc /// Type of the constraint description to add. /// First body of the constraint. /// Second body of the constraint. + /// Description of the constraint to add. /// Allocated constraint handle. public unsafe ConstraintHandle Add(BodyHandle bodyHandleA, BodyHandle bodyHandleB, in TDescription description) where TDescription : unmanaged, ITwoBodyConstraintDescription @@ -1151,6 +1156,7 @@ public unsafe ConstraintHandle Add(BodyHandle bodyHandleA, BodyHan /// First body of the constraint. /// Second body of the constraint. /// Third body of the constraint. + /// Description of the constraint to add. /// Allocated constraint handle. public unsafe ConstraintHandle Add(BodyHandle bodyHandleA, BodyHandle bodyHandleB, BodyHandle bodyHandleC, in TDescription description) where TDescription : unmanaged, IThreeBodyConstraintDescription @@ -1167,6 +1173,7 @@ public unsafe ConstraintHandle Add(BodyHandle bodyHandleA, BodyHan /// Second body of the constraint. /// Third body of the constraint. /// Fourth body of the constraint. + /// Description of the constraint to add. /// Allocated constraint handle. public unsafe ConstraintHandle Add(BodyHandle bodyHandleA, BodyHandle bodyHandleB, BodyHandle bodyHandleC, BodyHandle bodyHandleD, in TDescription description) where TDescription : unmanaged, IFourBodyConstraintDescription From 9c8816b8484f21b670be637702b67a25e90cbf5e Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 2 Nov 2021 20:14:29 -0500 Subject: [PATCH 258/947] Fixed body memory location move with respect to kinematic flags. Sleep/wake seems to work. --- BepuPhysics/Constraints/TypeProcessor.cs | 4 +- .../EmbeddedSubsteppingTimestepper2.cs | 20 +- BepuPhysics/Solver.cs | 3 - Demos/SpecializedTests/ConstraintTestDemo.cs | 746 +++++++++--------- 4 files changed, 395 insertions(+), 378 deletions(-) diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index 7e2a05943..8b016fd11 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -810,7 +810,9 @@ public sealed override void UpdateForBodyMemoryMove(ref TypeBatch typeBatch, int BundleIndexing.GetBundleIndices(indexInTypeBatch, out var constraintBundleIndex, out var constraintInnerIndex); //Note that this relies on the bodyreferences memory layout. It uses the stride of vectors to skip to the next body based on the bodyIndexInConstraint. ref var bundle = ref Unsafe.As>(ref Buffer.Get(ref typeBatch.BodyReferences, constraintBundleIndex)); - GatherScatter.Get(ref bundle, constraintInnerIndex + bodyIndexInConstraint * Vector.Count) = newBodyLocation; + ref var referenceLocation = ref GatherScatter.Get(ref bundle, constraintInnerIndex + bodyIndexInConstraint * Vector.Count); + //Note that the old kinematic mask is preserved so that the caller doesn't have to requery the object for its kinematicity. + referenceLocation = newBodyLocation | (referenceLocation & Bodies.KinematicMask); } //Note that these next two sort key users require a generic sort key implementation; this avoids virtual dispatch on a per-object level while still sharing the bulk of the logic. diff --git a/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs b/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs index b546f9b20..73f5c8a7e 100644 --- a/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs +++ b/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs @@ -46,8 +46,26 @@ public EmbeddedSubsteppingTimestepper2(int substepCount) public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDispatcher = null) { + simulation.Solver.ValidateTrailingTypeBatchBodyReferences(); + simulation.Solver.ValidateFallbackBatchEmptySlotReferences(); + simulation.Solver.ValidateFallbackBatchAccessSafety(); + simulation.Solver.ValidateFallbackBatchAccumulatedImpulses(); + simulation.Solver.ValidateConstraintMaps(); + simulation.Solver.ValidateConstraintReferenceKinematicity(); + simulation.Solver.ValidateConstrainedKinematicsSet(); + simulation.Solver.ValidateFallbackBodiesAreDynamic(); + //simulation.Solver.ValidateExistingHandles(); simulation.Sleep(threadDispatcher); Slept?.Invoke(dt, threadDispatcher); + simulation.Solver.ValidateTrailingTypeBatchBodyReferences(); + simulation.Solver.ValidateFallbackBatchEmptySlotReferences(); + simulation.Solver.ValidateFallbackBatchAccessSafety(); + simulation.Solver.ValidateFallbackBatchAccumulatedImpulses(); + simulation.Solver.ValidateConstraintMaps(); + simulation.Solver.ValidateConstraintReferenceKinematicity(); + simulation.Solver.ValidateConstrainedKinematicsSet(); + simulation.Solver.ValidateFallbackBodiesAreDynamic(); + //simulation.Solver.ValidateExistingHandles(); simulation.PredictBoundingBoxes(dt, threadDispatcher); BeforeCollisionDetection?.Invoke(dt, threadDispatcher); @@ -64,7 +82,7 @@ public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDi simulation.Solver.ValidateConstraintReferenceKinematicity(); simulation.Solver.ValidateConstrainedKinematicsSet(); simulation.Solver.ValidateFallbackBodiesAreDynamic(); - simulation.Solver.ValidateExistingHandles(); + //simulation.Solver.ValidateExistingHandles(); var constrainedBodySet = simulation.Solver.PrepareConstraintIntegrationResponsibilities(SubstepCount, threadDispatcher); simulation.Profiler.Start(simulation.Solver); diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index cbc669bc6..f1cd99478 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -1103,12 +1103,10 @@ public ConstraintHandle Add(Span bodyHandles, in TDesc Debug.Assert(bodyHandles.Length == TypeProcessors[description.ConstraintTypeId].BodiesPerConstraint, "The number of bodies supplied to a constraint add must match the expected number of bodies involved in that constraint type. Did you use the wrong Solver.Add overload?"); //Adding a constraint assumes that the involved bodies are active, so wake up anything that is sleeping. - ValidateExistingHandles(); for (int i = 0; i < bodyHandles.Length; ++i) { awakener.AwakenBody(bodyHandles[i]); } - ValidateExistingHandles(); Add(bodyHandles, description, out var constraintHandle); for (int i = 0; i < bodyHandles.Length; ++i) { @@ -1116,7 +1114,6 @@ public ConstraintHandle Add(Span bodyHandles, in TDesc bodies.ValidateExistingHandle(bodyHandle); bodies.AddConstraint(bodies.HandleToLocation[bodyHandle.Value].Index, constraintHandle, i); } - ValidateExistingHandles(); return constraintHandle; } diff --git a/Demos/SpecializedTests/ConstraintTestDemo.cs b/Demos/SpecializedTests/ConstraintTestDemo.cs index 919bc0ead..7a51fd724 100644 --- a/Demos/SpecializedTests/ConstraintTestDemo.cs +++ b/Demos/SpecializedTests/ConstraintTestDemo.cs @@ -24,7 +24,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(25, 4, 40); camera.Yaw = 0; Simulation = Simulation.Create(BufferPool, - new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(30, 1), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 2f }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 2); + new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(30, 1), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 2f }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), 1); //new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(30, 1), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 2f }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper(), 8); var shapeA = new Box(.75f, 1, .5f); @@ -37,385 +37,385 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) shapeA.ComputeInertia(1, out var inertiaA); shapeA.ComputeInertia(1, out var inertiaB); var nextX = -10f; - //{ - // var x = GetNextPosition(ref nextX); - // var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); - // var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - // Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - //} + { + var x = GetNextPosition(ref nextX); + var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); + var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + } + { + var x = GetNextPosition(ref nextX); + var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); + var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + Simulation.Solver.Add(a, b, new AngularHinge { LocalHingeAxisA = new Vector3(0, 1, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); + } + { + var x = GetNextPosition(ref nextX); + var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); + var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + Simulation.Solver.Add(a, b, new Hinge + { + LocalOffsetA = new Vector3(0, 1, 0), + LocalHingeAxisA = new Vector3(0, 1, 0), + LocalOffsetB = new Vector3(0, -1, 0), + LocalHingeAxisB = new Vector3(0, 1, 0), + SpringSettings = new SpringSettings(30, 1) + }); + } + { + var x = GetNextPosition(ref nextX); + var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); + var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + Simulation.Solver.Add(a, b, new AngularSwivelHinge { LocalSwivelAxisA = new Vector3(1, 0, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); + Simulation.Solver.Add(a, b, new SwingLimit { AxisLocalA = new Vector3(0, 1, 0), AxisLocalB = new Vector3(0, 1, 0), MaximumSwingAngle = MathHelper.PiOver2, SpringSettings = new SpringSettings(30, 1) }); + } + { + var x = GetNextPosition(ref nextX); + var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); + var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + Simulation.Solver.Add(a, b, new SwivelHinge + { + LocalOffsetA = new Vector3(0, 1, 0), + LocalSwivelAxisA = new Vector3(1, 0, 0), + LocalOffsetB = new Vector3(0, -1, 0), + LocalHingeAxisB = new Vector3(0, 1, 0), + SpringSettings = new SpringSettings(30, 1) + }); + Simulation.Solver.Add(a, b, new SwingLimit { AxisLocalA = new Vector3(0, 1, 0), AxisLocalB = new Vector3(0, 1, 0), MaximumSwingAngle = MathHelper.PiOver2, SpringSettings = new SpringSettings(30, 1) }); + } { var x = GetNextPosition(ref nextX); var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + Simulation.Solver.Add(a, b, new TwistServo + { + LocalBasisA = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)), + LocalBasisB = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)), + TargetAngle = MathHelper.PiOver4, + SpringSettings = new SpringSettings(30, 1), + ServoSettings = new ServoSettings(float.MaxValue, 0, float.MaxValue) + }); + } + { + var x = GetNextPosition(ref nextX); + var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); + var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + Simulation.Solver.Add(a, b, new TwistLimit + { + LocalBasisA = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)), + LocalBasisB = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)), + MinimumAngle = MathHelper.Pi * -0.5f, + MaximumAngle = MathHelper.Pi * 0.95f, + SpringSettings = new SpringSettings(30, 1), + }); + Simulation.Solver.Add(a, b, new AngularHinge { LocalHingeAxisA = new Vector3(0, 1, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); + } + { + var x = GetNextPosition(ref nextX); + var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); + var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + Simulation.Solver.Add(a, b, new TwistMotor + { + LocalAxisA = new Vector3(0, 1, 0), + LocalAxisB = new Vector3(0, 1, 0), + TargetVelocity = MathHelper.Pi * 2, + Settings = new MotorSettings(float.MaxValue, 0.1f) + }); Simulation.Solver.Add(a, b, new AngularHinge { LocalHingeAxisA = new Vector3(0, 1, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); } - //{ - // var x = GetNextPosition(ref nextX); - // var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); - // var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - // Simulation.Solver.Add(a, b, new Hinge - // { - // LocalOffsetA = new Vector3(0, 1, 0), - // LocalHingeAxisA = new Vector3(0, 1, 0), - // LocalOffsetB = new Vector3(0, -1, 0), - // LocalHingeAxisB = new Vector3(0, 1, 0), - // SpringSettings = new SpringSettings(30, 1) - // }); - //} - //{ - // var x = GetNextPosition(ref nextX); - // var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); - // var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - // Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - // Simulation.Solver.Add(a, b, new AngularSwivelHinge { LocalSwivelAxisA = new Vector3(1, 0, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); - // Simulation.Solver.Add(a, b, new SwingLimit { AxisLocalA = new Vector3(0, 1, 0), AxisLocalB = new Vector3(0, 1, 0), MaximumSwingAngle = MathHelper.PiOver2, SpringSettings = new SpringSettings(30, 1) }); - //} - //{ - // var x = GetNextPosition(ref nextX); - // var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); - // var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - // Simulation.Solver.Add(a, b, new SwivelHinge - // { - // LocalOffsetA = new Vector3(0, 1, 0), - // LocalSwivelAxisA = new Vector3(1, 0, 0), - // LocalOffsetB = new Vector3(0, -1, 0), - // LocalHingeAxisB = new Vector3(0, 1, 0), - // SpringSettings = new SpringSettings(30, 1) - // }); - // Simulation.Solver.Add(a, b, new SwingLimit { AxisLocalA = new Vector3(0, 1, 0), AxisLocalB = new Vector3(0, 1, 0), MaximumSwingAngle = MathHelper.PiOver2, SpringSettings = new SpringSettings(30, 1) }); - //} - //{ - // var x = GetNextPosition(ref nextX); - // var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); - // var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - // Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - // Simulation.Solver.Add(a, b, new TwistServo - // { - // LocalBasisA = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)), - // LocalBasisB = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)), - // TargetAngle = MathHelper.PiOver4, - // SpringSettings = new SpringSettings(30, 1), - // ServoSettings = new ServoSettings(float.MaxValue, 0, float.MaxValue) - // }); - //} - //{ - // var x = GetNextPosition(ref nextX); - // var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); - // var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - // Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - // Simulation.Solver.Add(a, b, new TwistLimit - // { - // LocalBasisA = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)), - // LocalBasisB = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)), - // MinimumAngle = MathHelper.Pi * -0.5f, - // MaximumAngle = MathHelper.Pi * 0.95f, - // SpringSettings = new SpringSettings(30, 1), - // }); - // Simulation.Solver.Add(a, b, new AngularHinge { LocalHingeAxisA = new Vector3(0, 1, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); - //} - //{ - // var x = GetNextPosition(ref nextX); - // var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); - // var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - // Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - // Simulation.Solver.Add(a, b, new TwistMotor - // { - // LocalAxisA = new Vector3(0, 1, 0), - // LocalAxisB = new Vector3(0, 1, 0), - // TargetVelocity = MathHelper.Pi * 2, - // Settings = new MotorSettings(float.MaxValue, 0.1f) - // }); - // Simulation.Solver.Add(a, b, new AngularHinge { LocalHingeAxisA = new Vector3(0, 1, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); - //} - //{ - // var x = GetNextPosition(ref nextX); - // var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); - // var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - // Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - // Simulation.Solver.Add(a, b, new AngularServo - // { - // TargetRelativeRotationLocalA = QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), MathHelper.PiOver2), - // ServoSettings = new ServoSettings(float.MaxValue, 0, 12f), - // SpringSettings = new SpringSettings(30, 1) - // }); - //} - //{ - // var x = GetNextPosition(ref nextX); - // var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); - // var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - // Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - // Simulation.Solver.Add(a, b, new AngularMotor { TargetVelocityLocalA = new Vector3(0, 1, 0), Settings = new MotorSettings(15, 0.0001f) }); - //} - //{ - // var x = GetNextPosition(ref nextX); - // var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); - // var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity); - // //aDescription.Velocity.Angular = new Vector3(0, 0, 5); - // var a = Simulation.Bodies.Add(aDescription); - // var b = Simulation.Bodies.Add(bDescription); - // Simulation.Solver.Add(a, b, new Weld { LocalOffset = new Vector3(0, 2, 0), LocalOrientation = Quaternion.Identity, SpringSettings = new SpringSettings(30, 1) }); - //} - //{ - // var x = GetNextPosition(ref nextX); - // var sphere = new Sphere(0.125f); - // //Treat each vertex as a point mass that cannot rotate. - // var sphereInertia = new BodyInertia { InverseMass = 1 }; - // var sphereCollidable = new CollidableDescription(Simulation.Shapes.Add(sphere), 0.1f); - // var a = new Vector3(x, 3, 0); - // var b = new Vector3(x, 4, 0); - // var c = new Vector3(x, 3, 1); - // var d = new Vector3(x + 1, 3, 0); - // var aDescription = BodyDescription.CreateDynamic(a, sphereInertia, sphereCollidable, activity); - // var bDescription = BodyDescription.CreateDynamic(b, sphereInertia, sphereCollidable, activity); - // var cDescription = BodyDescription.CreateDynamic(c, sphereInertia, sphereCollidable, activity); - // var dDescription = BodyDescription.CreateDynamic(d, sphereInertia, sphereCollidable, activity); - // var aHandle = Simulation.Bodies.Add(aDescription); - // var bHandle = Simulation.Bodies.Add(bDescription); - // var cHandle = Simulation.Bodies.Add(cDescription); - // var dHandle = Simulation.Bodies.Add(dDescription); - // var distanceSpringiness = new SpringSettings(3f, 1); - // Simulation.Solver.Add(aHandle, bHandle, new CenterDistanceConstraint(Vector3.Distance(a, b), distanceSpringiness)); - // Simulation.Solver.Add(aHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(a, c), distanceSpringiness)); - // Simulation.Solver.Add(aHandle, dHandle, new CenterDistanceConstraint(Vector3.Distance(a, d), distanceSpringiness)); - // Simulation.Solver.Add(bHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(b, c), distanceSpringiness)); - // Simulation.Solver.Add(bHandle, dHandle, new CenterDistanceConstraint(Vector3.Distance(b, d), distanceSpringiness)); - // Simulation.Solver.Add(cHandle, dHandle, new CenterDistanceConstraint(Vector3.Distance(c, d), distanceSpringiness)); - // Simulation.Solver.Add(aHandle, bHandle, cHandle, dHandle, new VolumeConstraint(a, b, c, d, new SpringSettings(30, 1))); - //} - //{ - // var x = GetNextPosition(ref nextX); - // var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); - // var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); - // var a = Simulation.Bodies.Add(aDescription); - // var b = Simulation.Bodies.Add(bDescription); - // Simulation.Solver.Add(a, b, new DistanceServo(new Vector3(0, 0.55f, 0), new Vector3(0, -0.55f, 0), 1.9f, new SpringSettings(30, 1), ServoSettings.Default)); - //} - //{ - // var x = GetNextPosition(ref nextX); - // var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); - // var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); - // var a = Simulation.Bodies.Add(aDescription); - // var b = Simulation.Bodies.Add(bDescription); - // Simulation.Solver.Add(a, b, new DistanceLimit(new Vector3(0, 0.55f, 0), new Vector3(0, -0.55f, 0), 1f, 3, new SpringSettings(30, 1))); - //} - //{ - // var x = GetNextPosition(ref nextX); - // var sphere = new Sphere(0.125f); - // //Treat each vertex as a point mass that cannot rotate. - // var sphereInertia = new BodyInertia { InverseMass = 1 }; - // var sphereCollidable = new CollidableDescription(Simulation.Shapes.Add(sphere), 0.1f); - // var a = new Vector3(x, 3, 0); - // var b = new Vector3(x, 4, 0); - // var c = new Vector3(x + 1, 3, 0); - // var aDescription = BodyDescription.CreateDynamic(a, sphereInertia, sphereCollidable, activity); - // var bDescription = BodyDescription.CreateDynamic(b, sphereInertia, sphereCollidable, activity); - // var cDescription = BodyDescription.CreateDynamic(c, sphereInertia, sphereCollidable, activity); - // var aHandle = Simulation.Bodies.Add(aDescription); - // var bHandle = Simulation.Bodies.Add(bDescription); - // var cHandle = Simulation.Bodies.Add(cDescription); - // var distanceSpringiness = new SpringSettings(3f, 1); - // Simulation.Solver.Add(aHandle, bHandle, new CenterDistanceConstraint(Vector3.Distance(a, b), distanceSpringiness)); - // Simulation.Solver.Add(aHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(a, c), distanceSpringiness)); - // Simulation.Solver.Add(bHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(b, c), distanceSpringiness)); - // Simulation.Solver.Add(aHandle, bHandle, cHandle, new AreaConstraint(a, b, c, new SpringSettings(30, 1))); - //} - //{ - // var x = GetNextPosition(ref nextX); - // var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), default, collidableA, activity); - // var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); - // var a = Simulation.Bodies.Add(aDescription); - // var b = Simulation.Bodies.Add(bDescription); - // Simulation.Solver.Add(a, b, new PointOnLineServo - // { - // LocalOffsetA = new Vector3(0, 0.5f, 0), - // LocalOffsetB = new Vector3(0, -0.5f, 0), - // LocalDirection = new Vector3(0, 1, 0), - // SpringSettings = new SpringSettings(30, 1), - // ServoSettings = ServoSettings.Default - // }); - //} - //{ - // var x = GetNextPosition(ref nextX); - // var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); - // var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); - // var a = Simulation.Bodies.Add(aDescription); - // var b = Simulation.Bodies.Add(bDescription); - // Simulation.Solver.Add(a, b, new LinearAxisServo - // { - // LocalOffsetA = new Vector3(0, 0.5f, 0), - // LocalOffsetB = new Vector3(0, -0.5f, 0), - // LocalPlaneNormal = new Vector3(0, 1, 0), - // TargetOffset = 2, - // SpringSettings = new SpringSettings(30, 1), - // ServoSettings = ServoSettings.Default - // }); - //} - //{ - // var x = GetNextPosition(ref nextX); - // var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); - // var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); - // var a = Simulation.Bodies.Add(aDescription); - // var b = Simulation.Bodies.Add(bDescription); - // Simulation.Solver.Add(a, b, new PointOnLineServo - // { - // LocalOffsetA = new Vector3(0, 0.5f, 0), - // LocalOffsetB = new Vector3(0, -0.5f, 0), - // LocalDirection = new Vector3(0, 1, 0), - // SpringSettings = new SpringSettings(30, 1), - // ServoSettings = ServoSettings.Default - // }); - // Simulation.Solver.Add(a, b, new LinearAxisMotor - // { - // LocalOffsetA = new Vector3(0, 0.5f, 0), - // LocalOffsetB = new Vector3(0, -0.5f, 0), - // LocalAxis = new Vector3(0, 1, 0), - // TargetVelocity = -2, - // Settings = new MotorSettings(15, 0.01f) - // }); - //} - //{ - // var x = GetNextPosition(ref nextX); - // var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); - // var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); - // var a = Simulation.Bodies.Add(aDescription); - // var b = Simulation.Bodies.Add(bDescription); - // Simulation.Solver.Add(a, b, new PointOnLineServo - // { - // LocalOffsetA = new Vector3(0, 0.5f, 0), - // LocalOffsetB = new Vector3(0, -0.5f, 0), - // LocalDirection = new Vector3(0, 1, 0), - // SpringSettings = new SpringSettings(30, 1), - // ServoSettings = ServoSettings.Default - // }); - // Simulation.Solver.Add(a, b, new LinearAxisLimit - // { - // LocalOffsetA = new Vector3(0, 0.5f, 0), - // LocalOffsetB = new Vector3(0, -0.5f, 0), - // LocalAxis = new Vector3(0, 1, 0), - // MinimumOffset = 1, - // MaximumOffset = 2, - // SpringSettings = new SpringSettings(30, 1) - // }); - //} - //{ - // var x = GetNextPosition(ref nextX); - // var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); - // var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - // Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - // Simulation.Solver.Add(b, a, new AngularAxisMotor - // { - // LocalAxisA = new Vector3(0, 1, 0), - // TargetVelocity = MathHelper.Pi * 5, - // Settings = new MotorSettings(float.MaxValue, 0.1f) - // }); - //} - //{ - // var x = GetNextPosition(ref nextX); - // var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); - // Simulation.Solver.Add(a, new OneBodyLinearServo - // { - // LocalOffset = new Vector3(0, 1, 0), - // Target = new Vector3(x, 3, 0), - // ServoSettings = new ServoSettings(2, 0, float.MaxValue), - // SpringSettings = new SpringSettings(5, 1) - // }); - //} - //{ - // var x = GetNextPosition(ref nextX); - // var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); - // var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - // Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - // Simulation.Solver.Add(a, new OneBodyLinearMotor - // { - // LocalOffset = new Vector3(0, 1, 0), - // TargetVelocity = new Vector3(0, -1, 0), - // Settings = new MotorSettings(float.MaxValue, 1e-2f), - // }); - //} - //{ - // var x = GetNextPosition(ref nextX); - // var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); - // var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - // Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - // Simulation.Solver.Add(a, new OneBodyAngularServo - // { - // TargetOrientation = Quaternion.Identity, - // ServoSettings = ServoSettings.Default, - // SpringSettings = new SpringSettings(30f, 1f) - // }); - //} - //{ - // var x = GetNextPosition(ref nextX); - // var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); - // var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - // Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - // Simulation.Solver.Add(a, new OneBodyAngularMotor - // { - // TargetVelocity = new Vector3(1, 0, 0), - // Settings = new MotorSettings(float.MaxValue, 0.001f), - // }); - //} - //{ - // var x = GetNextPosition(ref nextX); - // var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); - // var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); - // var a = Simulation.Bodies.Add(aDescription); - // var b = Simulation.Bodies.Add(bDescription); - // Simulation.Solver.Add(a, b, new BallSocketMotor - // { - // LocalOffsetB = new Vector3(0, -1, 0), - // TargetVelocityLocalA = new Vector3(0, -0.25f, 0), - // Settings = new MotorSettings(10, 1e-4f) - // }); - //} - //{ - // var x = GetNextPosition(ref nextX); - // var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); - // var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); - // var a = Simulation.Bodies.Add(aDescription); - // var b = Simulation.Bodies.Add(bDescription); - // Simulation.Solver.Add(a, b, new BallSocketServo - // { - // LocalOffsetA = new Vector3(0, 1, 0), - // LocalOffsetB = new Vector3(0, -1, 0), - // SpringSettings = new SpringSettings(30, 1), - // ServoSettings = new ServoSettings(100, 1, 100) - // }); - //} - //{ - // var x = GetNextPosition(ref nextX); - // var wheelShape = new CollidableDescription(Simulation.Shapes.Add(new Cylinder(1, 0.1f)), 0.1f); - // var wheelOrientation = Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), MathF.PI * 0.5f); - // var aDescription = BodyDescription.CreateDynamic(new RigidPose(new Vector3(x, 3, 0), wheelOrientation), inertiaA, wheelShape, activity); - // var bDescription = BodyDescription.CreateDynamic(new RigidPose(new Vector3(x, 6, 0), wheelOrientation), inertiaB, wheelShape, activity); - // var cDescription = BodyDescription.CreateKinematic(new Vector3(x, 4.5f, -1), new CollidableDescription(Simulation.Shapes.Add(new Box(3, 6, 1)), 0.1f), activity); - // var a = Simulation.Bodies.Add(aDescription); - // var b = Simulation.Bodies.Add(bDescription); - // var c = Simulation.Bodies.Add(cDescription); - // Simulation.Solver.Add(a, b, new AngularAxisGearMotor - // { - // LocalAxisA = new Vector3(0, 1, 0), - // VelocityScale = -4, - // Settings = new MotorSettings(float.MaxValue, 0.0001f) - // }); - // Simulation.Solver.Add(c, a, new Hinge - // { - // LocalOffsetA = new Vector3(0, -1.5f, 1), - // LocalHingeAxisA = new Vector3(0, 0, 1), - // LocalOffsetB = new Vector3(0, 0, 0), - // LocalHingeAxisB = new Vector3(0, 1, 0), - // SpringSettings = new SpringSettings(30, 1) - // }); - // Simulation.Solver.Add(c, b, new Hinge - // { - // LocalOffsetA = new Vector3(0, 1.5f, 1), - // LocalHingeAxisA = new Vector3(0, 0, 1), - // LocalOffsetB = new Vector3(0, 0, 0), - // LocalHingeAxisB = new Vector3(0, 1, 0), - // SpringSettings = new SpringSettings(30, 1) - // }); - //} + { + var x = GetNextPosition(ref nextX); + var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); + var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + Simulation.Solver.Add(a, b, new AngularServo + { + TargetRelativeRotationLocalA = QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), MathHelper.PiOver2), + ServoSettings = new ServoSettings(float.MaxValue, 0, 12f), + SpringSettings = new SpringSettings(30, 1) + }); + } + { + var x = GetNextPosition(ref nextX); + var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); + var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + Simulation.Solver.Add(a, b, new AngularMotor { TargetVelocityLocalA = new Vector3(0, 1, 0), Settings = new MotorSettings(15, 0.0001f) }); + } + { + var x = GetNextPosition(ref nextX); + var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); + var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity); + //aDescription.Velocity.Angular = new Vector3(0, 0, 5); + var a = Simulation.Bodies.Add(aDescription); + var b = Simulation.Bodies.Add(bDescription); + Simulation.Solver.Add(a, b, new Weld { LocalOffset = new Vector3(0, 2, 0), LocalOrientation = Quaternion.Identity, SpringSettings = new SpringSettings(30, 1) }); + } + { + var x = GetNextPosition(ref nextX); + var sphere = new Sphere(0.125f); + //Treat each vertex as a point mass that cannot rotate. + var sphereInertia = new BodyInertia { InverseMass = 1 }; + var sphereCollidable = new CollidableDescription(Simulation.Shapes.Add(sphere), 0.1f); + var a = new Vector3(x, 3, 0); + var b = new Vector3(x, 4, 0); + var c = new Vector3(x, 3, 1); + var d = new Vector3(x + 1, 3, 0); + var aDescription = BodyDescription.CreateDynamic(a, sphereInertia, sphereCollidable, activity); + var bDescription = BodyDescription.CreateDynamic(b, sphereInertia, sphereCollidable, activity); + var cDescription = BodyDescription.CreateDynamic(c, sphereInertia, sphereCollidable, activity); + var dDescription = BodyDescription.CreateDynamic(d, sphereInertia, sphereCollidable, activity); + var aHandle = Simulation.Bodies.Add(aDescription); + var bHandle = Simulation.Bodies.Add(bDescription); + var cHandle = Simulation.Bodies.Add(cDescription); + var dHandle = Simulation.Bodies.Add(dDescription); + var distanceSpringiness = new SpringSettings(3f, 1); + Simulation.Solver.Add(aHandle, bHandle, new CenterDistanceConstraint(Vector3.Distance(a, b), distanceSpringiness)); + Simulation.Solver.Add(aHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(a, c), distanceSpringiness)); + Simulation.Solver.Add(aHandle, dHandle, new CenterDistanceConstraint(Vector3.Distance(a, d), distanceSpringiness)); + Simulation.Solver.Add(bHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(b, c), distanceSpringiness)); + Simulation.Solver.Add(bHandle, dHandle, new CenterDistanceConstraint(Vector3.Distance(b, d), distanceSpringiness)); + Simulation.Solver.Add(cHandle, dHandle, new CenterDistanceConstraint(Vector3.Distance(c, d), distanceSpringiness)); + Simulation.Solver.Add(aHandle, bHandle, cHandle, dHandle, new VolumeConstraint(a, b, c, d, new SpringSettings(30, 1))); + } + { + var x = GetNextPosition(ref nextX); + var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); + var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); + var a = Simulation.Bodies.Add(aDescription); + var b = Simulation.Bodies.Add(bDescription); + Simulation.Solver.Add(a, b, new DistanceServo(new Vector3(0, 0.55f, 0), new Vector3(0, -0.55f, 0), 1.9f, new SpringSettings(30, 1), ServoSettings.Default)); + } + { + var x = GetNextPosition(ref nextX); + var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); + var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); + var a = Simulation.Bodies.Add(aDescription); + var b = Simulation.Bodies.Add(bDescription); + Simulation.Solver.Add(a, b, new DistanceLimit(new Vector3(0, 0.55f, 0), new Vector3(0, -0.55f, 0), 1f, 3, new SpringSettings(30, 1))); + } + { + var x = GetNextPosition(ref nextX); + var sphere = new Sphere(0.125f); + //Treat each vertex as a point mass that cannot rotate. + var sphereInertia = new BodyInertia { InverseMass = 1 }; + var sphereCollidable = new CollidableDescription(Simulation.Shapes.Add(sphere), 0.1f); + var a = new Vector3(x, 3, 0); + var b = new Vector3(x, 4, 0); + var c = new Vector3(x + 1, 3, 0); + var aDescription = BodyDescription.CreateDynamic(a, sphereInertia, sphereCollidable, activity); + var bDescription = BodyDescription.CreateDynamic(b, sphereInertia, sphereCollidable, activity); + var cDescription = BodyDescription.CreateDynamic(c, sphereInertia, sphereCollidable, activity); + var aHandle = Simulation.Bodies.Add(aDescription); + var bHandle = Simulation.Bodies.Add(bDescription); + var cHandle = Simulation.Bodies.Add(cDescription); + var distanceSpringiness = new SpringSettings(3f, 1); + Simulation.Solver.Add(aHandle, bHandle, new CenterDistanceConstraint(Vector3.Distance(a, b), distanceSpringiness)); + Simulation.Solver.Add(aHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(a, c), distanceSpringiness)); + Simulation.Solver.Add(bHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(b, c), distanceSpringiness)); + Simulation.Solver.Add(aHandle, bHandle, cHandle, new AreaConstraint(a, b, c, new SpringSettings(30, 1))); + } + { + var x = GetNextPosition(ref nextX); + var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), default, collidableA, activity); + var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); + var a = Simulation.Bodies.Add(aDescription); + var b = Simulation.Bodies.Add(bDescription); + Simulation.Solver.Add(a, b, new PointOnLineServo + { + LocalOffsetA = new Vector3(0, 0.5f, 0), + LocalOffsetB = new Vector3(0, -0.5f, 0), + LocalDirection = new Vector3(0, 1, 0), + SpringSettings = new SpringSettings(30, 1), + ServoSettings = ServoSettings.Default + }); + } + { + var x = GetNextPosition(ref nextX); + var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); + var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); + var a = Simulation.Bodies.Add(aDescription); + var b = Simulation.Bodies.Add(bDescription); + Simulation.Solver.Add(a, b, new LinearAxisServo + { + LocalOffsetA = new Vector3(0, 0.5f, 0), + LocalOffsetB = new Vector3(0, -0.5f, 0), + LocalPlaneNormal = new Vector3(0, 1, 0), + TargetOffset = 2, + SpringSettings = new SpringSettings(30, 1), + ServoSettings = ServoSettings.Default + }); + } + { + var x = GetNextPosition(ref nextX); + var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); + var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); + var a = Simulation.Bodies.Add(aDescription); + var b = Simulation.Bodies.Add(bDescription); + Simulation.Solver.Add(a, b, new PointOnLineServo + { + LocalOffsetA = new Vector3(0, 0.5f, 0), + LocalOffsetB = new Vector3(0, -0.5f, 0), + LocalDirection = new Vector3(0, 1, 0), + SpringSettings = new SpringSettings(30, 1), + ServoSettings = ServoSettings.Default + }); + Simulation.Solver.Add(a, b, new LinearAxisMotor + { + LocalOffsetA = new Vector3(0, 0.5f, 0), + LocalOffsetB = new Vector3(0, -0.5f, 0), + LocalAxis = new Vector3(0, 1, 0), + TargetVelocity = -2, + Settings = new MotorSettings(15, 0.01f) + }); + } + { + var x = GetNextPosition(ref nextX); + var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); + var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); + var a = Simulation.Bodies.Add(aDescription); + var b = Simulation.Bodies.Add(bDescription); + Simulation.Solver.Add(a, b, new PointOnLineServo + { + LocalOffsetA = new Vector3(0, 0.5f, 0), + LocalOffsetB = new Vector3(0, -0.5f, 0), + LocalDirection = new Vector3(0, 1, 0), + SpringSettings = new SpringSettings(30, 1), + ServoSettings = ServoSettings.Default + }); + Simulation.Solver.Add(a, b, new LinearAxisLimit + { + LocalOffsetA = new Vector3(0, 0.5f, 0), + LocalOffsetB = new Vector3(0, -0.5f, 0), + LocalAxis = new Vector3(0, 1, 0), + MinimumOffset = 1, + MaximumOffset = 2, + SpringSettings = new SpringSettings(30, 1) + }); + } + { + var x = GetNextPosition(ref nextX); + var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); + var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + Simulation.Solver.Add(b, a, new AngularAxisMotor + { + LocalAxisA = new Vector3(0, 1, 0), + TargetVelocity = MathHelper.Pi * 5, + Settings = new MotorSettings(float.MaxValue, 0.1f) + }); + } + { + var x = GetNextPosition(ref nextX); + var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); + Simulation.Solver.Add(a, new OneBodyLinearServo + { + LocalOffset = new Vector3(0, 1, 0), + Target = new Vector3(x, 3, 0), + ServoSettings = new ServoSettings(2, 0, float.MaxValue), + SpringSettings = new SpringSettings(5, 1) + }); + } + { + var x = GetNextPosition(ref nextX); + var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); + var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + Simulation.Solver.Add(a, new OneBodyLinearMotor + { + LocalOffset = new Vector3(0, 1, 0), + TargetVelocity = new Vector3(0, -1, 0), + Settings = new MotorSettings(float.MaxValue, 1e-2f), + }); + } + { + var x = GetNextPosition(ref nextX); + var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); + var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + Simulation.Solver.Add(a, new OneBodyAngularServo + { + TargetOrientation = Quaternion.Identity, + ServoSettings = ServoSettings.Default, + SpringSettings = new SpringSettings(30f, 1f) + }); + } + { + var x = GetNextPosition(ref nextX); + var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); + var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + Simulation.Solver.Add(a, new OneBodyAngularMotor + { + TargetVelocity = new Vector3(1, 0, 0), + Settings = new MotorSettings(float.MaxValue, 0.001f), + }); + } + { + var x = GetNextPosition(ref nextX); + var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); + var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); + var a = Simulation.Bodies.Add(aDescription); + var b = Simulation.Bodies.Add(bDescription); + Simulation.Solver.Add(a, b, new BallSocketMotor + { + LocalOffsetB = new Vector3(0, -1, 0), + TargetVelocityLocalA = new Vector3(0, -0.25f, 0), + Settings = new MotorSettings(10, 1e-4f) + }); + } + { + var x = GetNextPosition(ref nextX); + var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); + var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); + var a = Simulation.Bodies.Add(aDescription); + var b = Simulation.Bodies.Add(bDescription); + Simulation.Solver.Add(a, b, new BallSocketServo + { + LocalOffsetA = new Vector3(0, 1, 0), + LocalOffsetB = new Vector3(0, -1, 0), + SpringSettings = new SpringSettings(30, 1), + ServoSettings = new ServoSettings(100, 1, 100) + }); + } + { + var x = GetNextPosition(ref nextX); + var wheelShape = new CollidableDescription(Simulation.Shapes.Add(new Cylinder(1, 0.1f)), 0.1f); + var wheelOrientation = Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), MathF.PI * 0.5f); + var aDescription = BodyDescription.CreateDynamic(new RigidPose(new Vector3(x, 3, 0), wheelOrientation), inertiaA, wheelShape, activity); + var bDescription = BodyDescription.CreateDynamic(new RigidPose(new Vector3(x, 6, 0), wheelOrientation), inertiaB, wheelShape, activity); + var cDescription = BodyDescription.CreateKinematic(new Vector3(x, 4.5f, -1), new CollidableDescription(Simulation.Shapes.Add(new Box(3, 6, 1)), 0.1f), activity); + var a = Simulation.Bodies.Add(aDescription); + var b = Simulation.Bodies.Add(bDescription); + var c = Simulation.Bodies.Add(cDescription); + Simulation.Solver.Add(a, b, new AngularAxisGearMotor + { + LocalAxisA = new Vector3(0, 1, 0), + VelocityScale = -4, + Settings = new MotorSettings(float.MaxValue, 0.0001f) + }); + Simulation.Solver.Add(c, a, new Hinge + { + LocalOffsetA = new Vector3(0, -1.5f, 1), + LocalHingeAxisA = new Vector3(0, 0, 1), + LocalOffsetB = new Vector3(0, 0, 0), + LocalHingeAxisB = new Vector3(0, 1, 0), + SpringSettings = new SpringSettings(30, 1) + }); + Simulation.Solver.Add(c, b, new Hinge + { + LocalOffsetA = new Vector3(0, 1.5f, 1), + LocalHingeAxisA = new Vector3(0, 0, 1), + LocalOffsetB = new Vector3(0, 0, 0), + LocalHingeAxisB = new Vector3(0, 1, 0), + SpringSettings = new SpringSettings(30, 1) + }); + } Simulation.Statics.Add(new StaticDescription(new Vector3(), new CollidableDescription(Simulation.Shapes.Add(new Box(256, 1, 256)), 0.1f))); } From 5a036fda26c20bdc2a8d9d69d0a2731656fcd2c3 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 2 Nov 2021 20:18:57 -0500 Subject: [PATCH 259/947] Integration responsibilities flag bug fixed. Bad assert removed from EnumerateConnectedRawBodyReferences. --- BepuPhysics/Solver.cs | 3 +-- BepuPhysics/Solver_SubsteppingSolve2.cs | 10 ++++++++-- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index f1cd99478..400cfbba0 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -1462,7 +1462,6 @@ public unsafe float GetAccumulatedImpulseMagnitude(ConstraintHandle constraintHa internal void EnumerateConnectedRawBodyReferences(ConstraintHandle constraintHandle, ref TEnumerator enumerator) where TEnumerator : IForEach { ref var constraintLocation = ref HandleToConstraint[constraintHandle.Value]; - Debug.Assert(constraintLocation.SetIndex > 0, "This function is intended to be used only with sleeping constraints."); ref var typeBatch = ref Sets[constraintLocation.SetIndex].Batches[constraintLocation.BatchIndex].GetTypeBatch(constraintLocation.TypeId); Debug.Assert(constraintLocation.IndexInTypeBatch >= 0 && constraintLocation.IndexInTypeBatch < typeBatch.ConstraintCount, "Bad constraint location; likely some add/remove bug."); TypeProcessors[constraintLocation.TypeId].EnumerateConnectedRawBodyReferences(ref typeBatch, constraintLocation.IndexInTypeBatch, ref enumerator); @@ -1517,7 +1516,7 @@ public void LoopBody(int encodedBodyIndex) internal void EnumerateActiveDynamicConnectedBodyIndices(ConstraintHandle constraintHandle, ref TEnumerator enumerator) where TEnumerator : IForEach { ref var constraintLocation = ref HandleToConstraint[constraintHandle.Value]; - Debug.Assert(constraintLocation.SetIndex == 0, "This function is intended to be used only with the active set."); + Debug.Assert(constraintLocation.SetIndex == 0, "This function is intended to be used only with waking constraints."); ref var typeBatch = ref Sets[constraintLocation.SetIndex].Batches[constraintLocation.BatchIndex].GetTypeBatch(constraintLocation.TypeId); Debug.Assert(constraintLocation.IndexInTypeBatch >= 0 && constraintLocation.IndexInTypeBatch < typeBatch.ConstraintCount, "Bad constraint location; likely some add/remove bug."); ActiveDynamicEnumerator wrapper; diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index 7bf6bf460..a5b6651f5 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -766,15 +766,21 @@ unsafe bool ComputeIntegrationResponsibilitiesForConstraintRegion for (int bundleInnerIndex = 0; bundleInnerIndex < countInBundle; ++bundleInnerIndex) { //Constraints refer to bodies by index when they're in the active set, so we need to transform to handle to look up our merged batch results. - var bodyIndex = bundleStart[bundleInnerIndex] & Bodies.BodyReferenceMask; + int bodyIndex; if (typeof(TFallbackness) == typeof(IsFallbackBatch)) { //Fallback batches can contain empty lanes; there's no guarantee of constraint contiguity. Such lanes are marked with -1 in the body references. //Just skip over them. - if (bodyIndex == -1) + var rawBodyIndex = bundleStart[bundleInnerIndex]; + if (rawBodyIndex == -1) { continue; } + bodyIndex = rawBodyIndex & Bodies.BodyReferenceMask; + } + else + { + bodyIndex = bundleStart[bundleInnerIndex] & Bodies.BodyReferenceMask; } var bodyHandle = activeSet.IndexToHandle[bodyIndex].Value; if (firstObservedForBatch.Contains(bodyHandle)) From 77c7bf07b4f38e0460db6a922e282ca389a7330b Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 4 Nov 2021 17:55:48 -0500 Subject: [PATCH 260/947] Fixed some bad documentation gave RemoveConstraintReference a return. --- BepuPhysics/Bodies.cs | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/BepuPhysics/Bodies.cs b/BepuPhysics/Bodies.cs index f555c5f4b..d01cc2376 100644 --- a/BepuPhysics/Bodies.cs +++ b/BepuPhysics/Bodies.cs @@ -13,15 +13,17 @@ namespace BepuPhysics { + /// + /// Location of a body in memory. + /// public struct BodyMemoryLocation { /// - /// Index of the set owning the body reference. If the island index is 0, the body is active. + /// Index of the set owning the body reference. If the set index is 0, the body is awake. If the set index is greater than zero, the body is asleep. /// public int SetIndex; /// - /// Index of the body within its owning set. If the body is active (and so the Island index is -1), this is an index into the Bodies data arrays. - /// If it is nonnegative, it is an index into the inactive island + /// Index of the body within its owning set. /// public int Index; } @@ -36,6 +38,9 @@ public partial class Bodies /// The backing array index may change in response to cache optimization. /// public Buffer HandleToLocation; + /// + /// Pool from which handles are pulled for new bodies. + /// public IdPool HandlePool; /// /// The set of existing bodies. The slot at index 0 contains all active bodies. Later slots, if allocated, contain the bodies associated with inactive islands. @@ -261,9 +266,10 @@ internal void AddConstraint(int bodyIndex, ConstraintHandle constraintHandle, in /// /// Index of the active body. /// Handle of the constraint to remove. - internal void RemoveConstraintReference(int bodyIndex, ConstraintHandle constraintHandle) + /// True if the number of constraints remaining attached to the body is 0, false otherwise. + internal bool RemoveConstraintReference(int bodyIndex, ConstraintHandle constraintHandle) { - ActiveSet.RemoveConstraintReference(bodyIndex, constraintHandle, MinimumConstraintCapacityPerBody, Pool); + return ActiveSet.RemoveConstraintReference(bodyIndex, constraintHandle, MinimumConstraintCapacityPerBody, Pool); } /// From 9c9357c592992f82b2e25fc268952773ddaf512d Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 5 Nov 2021 17:18:33 -0500 Subject: [PATCH 261/947] ConstraintRemover and Solver.Remove now remove from the ConstrainedKinematicHandles. Awakener CopyBodyRegion now adds tot he ConstrainedKinematicHandles. --- BepuPhysics/BodySet.cs | 11 +++- .../CollisionDetection/ConstraintRemover.cs | 23 ++++--- .../ConstraintGraphRemovalEnumerator.cs | 24 ------- BepuPhysics/IslandAwakener.cs | 14 +++- BepuPhysics/IslandSleeper.cs | 2 +- BepuPhysics/Solver.cs | 66 +++++++++++++++++-- 6 files changed, 99 insertions(+), 41 deletions(-) delete mode 100644 BepuPhysics/ConstraintGraphRemovalEnumerator.cs diff --git a/BepuPhysics/BodySet.cs b/BepuPhysics/BodySet.cs index f3fd5c701..bb359854d 100644 --- a/BepuPhysics/BodySet.cs +++ b/BepuPhysics/BodySet.cs @@ -171,8 +171,16 @@ internal void AddConstraint(int bodyIndex, ConstraintHandle constraintHandle, in constraints.AllocateUnsafely() = constraint; } + /// + /// Removes a constraint from a body's constraint list. + /// + /// Index of the body to remove the constraint reference from. + /// Handle of the constraint to remove. + /// Minimum constraint capacity to maintain the body's constraint list. The list will automatically downsize as constraints are removed, but its capacity will not go below this threshold. + /// Pool to use to resize the constraint list. + /// True if the number of constraints remaining attached to the body is 0, false otherwise. [MethodImpl(MethodImplOptions.AggressiveInlining)] - internal void RemoveConstraintReference(int bodyIndex, ConstraintHandle constraintHandle, int minimumConstraintCapacityPerBody, BufferPool pool) + internal bool RemoveConstraintReference(int bodyIndex, ConstraintHandle constraintHandle, int minimumConstraintCapacityPerBody, BufferPool pool) { //This uses a linear search. That's fine; bodies will rarely have more than a handful of constraints associated with them. //Attempting to use something like a hash set for fast removes would just introduce more constant overhead and slow it down on average. @@ -197,6 +205,7 @@ internal void RemoveConstraintReference(int bodyIndex, ConstraintHandle constrai //The list can be trimmed down a bit while still holding all existing constraints and obeying the minimum capacity. list.Resize(targetCapacity, pool); } + return list.Count == 0; } public bool BodyIsConstrainedBy(int bodyIndex, ConstraintHandle constraintHandle) diff --git a/BepuPhysics/CollisionDetection/ConstraintRemover.cs b/BepuPhysics/CollisionDetection/ConstraintRemover.cs index 1c049ac7b..97532a38b 100644 --- a/BepuPhysics/CollisionDetection/ConstraintRemover.cs +++ b/BepuPhysics/CollisionDetection/ConstraintRemover.cs @@ -27,7 +27,7 @@ public class ConstraintRemover internal struct PerBodyRemovalTarget { - public int BodyIndex; + public int EncodedBodyIndex; public ConstraintHandle ConstraintHandle; public int BatchIndex; @@ -145,18 +145,18 @@ public unsafe void EnqueueForRemoval(ConstraintHandle constraintHandle, Solver s //Now extract and enqueue the body list constraint removal targets and the constraint batch body handle removal targets. //We have to perform the enumeration here rather than in the later flush. Removals from type batches make enumerating connected body indices a race condition there. ref var typeBatch = ref constraintBatch.TypeBatches[typeBatchIndex.TypeBatch]; - var bodyIndices = stackalloc int[bodiesPerConstraint]; - var enumerator = new ActiveConstraintBodyIndexCollector(bodyIndices); + var encodedBodyIndices = stackalloc int[bodiesPerConstraint]; + var enumerator = new PassthroughReferenceCollector(encodedBodyIndices); typeProcessor.EnumerateConnectedRawBodyReferences(ref typeBatch, constraint.IndexInTypeBatch, ref enumerator); for (int i = 0; i < bodiesPerConstraint; ++i) { ref var target = ref typeBatchRemovals.PerBodyRemovalTargets.AllocateUnsafely(); - target.BodyIndex = bodyIndices[i]; + target.EncodedBodyIndex = encodedBodyIndices[i]; target.ConstraintHandle = constraintHandle; target.BatchIndex = typeBatchIndex.Batch; - target.BodyHandle = bodies.ActiveSet.IndexToHandle[target.BodyIndex]; + target.BodyHandle = bodies.ActiveSet.IndexToHandle[target.EncodedBodyIndex & Bodies.BodyReferenceMask]; } } @@ -341,7 +341,12 @@ public void RemoveConstraintsFromBodyLists() for (int j = 0; j < removals.Count; ++j) { ref var target = ref removals[j]; - bodies.RemoveConstraintReference(target.BodyIndex, target.ConstraintHandle); + if (bodies.RemoveConstraintReference(target.EncodedBodyIndex & Bodies.BodyReferenceMask, target.ConstraintHandle) && (target.EncodedBodyIndex & Bodies.KinematicMask) != 0) + { + //This is a kinematic, and it has no remaining constraint connections. Remove it from the solver constrained kinematic set. + var removed = solver.ConstrainedKinematicHandles.FastRemove(target.BodyHandle.Value); + Debug.Assert(removed, "The last constraint removed from a kinematic should see the body removed from the constrained kinematic set."); + } } } } @@ -378,7 +383,7 @@ public void RemoveConstraintsFromFallbackBatchReferencedHandles() for (int j = 0; j < removals.Count; ++j) { ref var target = ref removals[j]; - if (solver.ActiveSet.SequentialFallback.Remove(target.BodyIndex, ref allocationIdsToFree)) + if (solver.ActiveSet.SequentialFallback.Remove(target.EncodedBodyIndex & Bodies.BodyReferenceMask, ref allocationIdsToFree)) { //No more constraints for this body in the fallback set; it should not exist in the fallback batch's referenced handles anymore. //Debug.Assert(solver.batchReferencedHandles[target.BatchIndex].Contains(target.BodyHandle.Value) || bodies.GetBodyReference(target.BodyHandle).Kinematic, @@ -389,7 +394,7 @@ public void RemoveConstraintsFromFallbackBatchReferencedHandles() } } } - public void TryRemoveAllConstraintsForBodyFromFallbackBatch(BodyHandle bodyHandle, int bodyIndex) + public void TryRemoveBodyFromConstrainedKinematicsAndRemoveAllConstraintsForBodyFromFallbackBatch(BodyHandle bodyHandle, int bodyIndex) { if (solver.ActiveSet.SequentialFallback.TryRemove(bodyIndex, ref allocationIdsToFree)) { @@ -397,6 +402,8 @@ public void TryRemoveAllConstraintsForBodyFromFallbackBatch(BodyHandle bodyHandl "The batch referenced handles must include all constraint-involved dynamics, but will not include kinematics."); solver.batchReferencedHandles[solver.FallbackBatchThreshold].Unset(bodyHandle.Value); } + //Note that we don't check kinematicity here. If it's dynamic, that's fine, this won't do anything. + solver.ConstrainedKinematicHandles.FastRemove(bodyHandle.Value); } QuickList removedTypeBatches; diff --git a/BepuPhysics/ConstraintGraphRemovalEnumerator.cs b/BepuPhysics/ConstraintGraphRemovalEnumerator.cs deleted file mode 100644 index 2ab3de17e..000000000 --- a/BepuPhysics/ConstraintGraphRemovalEnumerator.cs +++ /dev/null @@ -1,24 +0,0 @@ -using BepuUtilities; -using System; -using System.Collections.Generic; -using System.Runtime.CompilerServices; -using System.Text; - -namespace BepuPhysics -{ - /// - /// Enumerates the bodies attached to an active constraint and removes the constraint's handle from all of the connected body constraint reference lists. - /// - struct ConstraintGraphRemovalEnumerator : IForEach - { - internal Bodies bodies; - internal ConstraintHandle constraintHandle; - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void LoopBody(int bodyIndex) - { - //Note that this only looks in the active set. Directly removing inactive objects is unsupported- removals and adds activate all involved islands. - bodies.RemoveConstraintReference(bodyIndex, constraintHandle); - } - } - -} diff --git a/BepuPhysics/IslandAwakener.cs b/BepuPhysics/IslandAwakener.cs index 80c92a3c2..57aab0ceb 100644 --- a/BepuPhysics/IslandAwakener.cs +++ b/BepuPhysics/IslandAwakener.cs @@ -296,6 +296,18 @@ internal unsafe void ExecutePhaseOneJob(int index) sourceSet.Collidables.CopyTo(job.SourceStart, targetSet.Collidables, job.TargetStart, job.Count); sourceSet.Constraints.CopyTo(job.SourceStart, targetSet.Constraints, job.TargetStart, job.Count); sourceSet.SolverStates.CopyTo(job.SourceStart, targetSet.SolverStates, job.TargetStart, job.Count); + //This rescans the memory, but it should be still floating in cache ready to access. + for (int i = 0; i < sourceSet.Count; ++i) + { + var sourceBodyIndex = i + job.SourceStart; + if (Bodies.IsKinematicUnsafeGCHole(ref sourceSet.SolverStates[sourceBodyIndex].Inertia.Local) && sourceSet.Constraints[sourceBodyIndex].Count > 0) + { + bool taken = false; + solver.constrainedKinematicLock.Enter(ref taken); + solver.ConstrainedKinematicHandles.AddUnsafely(sourceSet.IndexToHandle[sourceBodyIndex].Value); + solver.constrainedKinematicLock.Exit(); + } + } sourceSet.Activity.CopyTo(job.SourceStart, targetSet.Activity, job.TargetStart, job.Count); if (resetActivityStates) { @@ -378,7 +390,7 @@ internal unsafe void ExecutePhaseTwoJob(int index) solver.TypeProcessors[job.TypeId].CopySleepingToActive( job.SourceSet, job.Batch, job.SourceTypeBatch, job.TargetTypeBatch, job.SourceStart, job.TargetStart, job.Count, bodies, solver); - solver.ValidateConstraintMaps(0, job.Batch, job.TargetTypeBatch, job.TargetStart, job.Count); + //solver.ValidateConstraintMaps(0, job.Batch, job.TargetTypeBatch, job.TargetStart, job.Count); } break; case PhaseTwoJobType.AddFallbackTypeBatchConstraints: diff --git a/BepuPhysics/IslandSleeper.cs b/BepuPhysics/IslandSleeper.cs index 62fcf90cd..963576ebe 100644 --- a/BepuPhysics/IslandSleeper.cs +++ b/BepuPhysics/IslandSleeper.cs @@ -451,7 +451,7 @@ void ExecuteRemoval(ref RemovalJob job) var bodyHandle = inactiveBodySet.IndexToHandle[bodyIndexInInactiveSet]; ref var location = ref bodies.HandleToLocation[bodyHandle.Value]; Debug.Assert(location.SetIndex == 0, "At this point, the sleep hasn't gone through so the set should still be 0."); - constraintRemover.TryRemoveAllConstraintsForBodyFromFallbackBatch(bodyHandle, location.Index); + constraintRemover.TryRemoveBodyFromConstrainedKinematicsAndRemoveAllConstraintsForBodyFromFallbackBatch(bodyHandle, location.Index); bodies.RemoveFromActiveSet(location.Index); //And now we can actually update the handle->body mapping. location.SetIndex = setIndex; diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index 400cfbba0..b0d5f94a6 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -8,6 +8,7 @@ using BepuUtilities; using System.Runtime.InteropServices; using System.Numerics; +using System.Threading; namespace BepuPhysics { @@ -32,6 +33,9 @@ public ConstraintReference(TypeBatch* typeBatchPointer, int indexInTypeBatch) } } + /// + /// Location in memory where a constraint is stored. + /// public struct ConstraintLocation { //Note that the type id is included, even though we can extract it from a type parameter. @@ -39,9 +43,21 @@ public struct ConstraintLocation //so instead we keep a type id cached. //(You could pack these a bit- it's pretty reasonable to say you can't have more than 2^24 constraints of a given type and 2^8 constraint types... //It's just not that valuable, unless proven otherwise.) + /// + /// Index of the constraint set that owns the constraint. If zero, the constraint is attached to bodies that are awake. + /// public int SetIndex; + /// + /// Index of the constraint batch the constraint belongs to. + /// public int BatchIndex; + /// + /// Type id of the constraint. Used to look up the type batch index in a constraint batch's type id to type batch index table. + /// public int TypeId; + /// + /// Index of the constraint in a type batch. + /// public int IndexInTypeBatch; } @@ -73,6 +89,9 @@ public partial class Solver /// public IdPool HandlePool; internal BufferPool pool; + /// + /// Mapping from constraint handle (via its internal integer value) to the location of a constraint in memory. + /// public Buffer HandleToConstraint; /// @@ -82,6 +101,10 @@ public partial class Solver /// public int FallbackBatchThreshold { get; private set; } + /// + /// Lock used to add to the constrained kinematic handles from multiple threads, if necessary. + /// + internal SpinLock constrainedKinematicLock; /// /// Set of body handles associated with constrained kinematic bodies. These will be integrated during substepping. /// @@ -327,14 +350,17 @@ unsafe internal void ValidateConstraintReferenceKinematicity() } [Conditional("DEBUG")] - unsafe internal void ValidateConstrainedKinematicsSet() + unsafe public void ValidateConstrainedKinematicsSet() { ref var set = ref bodies.ActiveSet; for (int i = 0; i < set.Count; ++i) { if (Bodies.IsKinematicUnsafeGCHole(ref set.SolverStates[i].Inertia.Local) && set.Constraints[i].Count > 0) { - Debug.Assert(ConstrainedKinematicHandles.Contains(set.IndexToHandle[i].Value), "Any active kinematic with constraints must appear in the constrained kinematic set."); + var contained = ConstrainedKinematicHandles.Contains(set.IndexToHandle[i].Value); + if (!contained) + ValidateExistingHandles(); + Debug.Assert(contained, "Any active kinematic with constraints must appear in the constrained kinematic set."); } } for (int i = 0; i < ConstrainedKinematicHandles.Count; ++i) @@ -923,6 +949,15 @@ internal unsafe void AllocateInBatch(int targetBatchIndex, ConstraintHandle cons } } var typeProcessor = TypeProcessors[typeId]; + Debug.Assert(typeProcessor.BodiesPerConstraint == encodedBodyIndices.Length); + for (int i = 0; i < encodedBodyIndices.Length; ++i) + { + Debug.Assert(Bodies.IsEncodedKinematicReference(encodedBodyIndices[i]) == Bodies.IsKinematicUnsafeGCHole(ref bodies.ActiveSet.SolverStates[encodedBodyIndices[i] & Bodies.BodyReferenceMask].Inertia.Local)); + if (Bodies.IsEncodedKinematicReference(encodedBodyIndices[i])) + { + Debug.Assert(ConstrainedKinematicHandles.Contains(bodies.ActiveSet.IndexToHandle[encodedBodyIndices[i] & Bodies.BodyReferenceMask].Value)); + } + } var typeBatch = batch.GetOrCreateTypeBatch(typeId, typeProcessor, GetMinimumCapacityForType(typeId), pool); int indexInTypeBatch; if (targetBatchIndex == FallbackBatchThreshold) @@ -1247,6 +1282,26 @@ internal unsafe void RemoveFromBatch(ConstraintHandle constraintHandle, int batc RemoveBatchIfEmpty(ref batch, batchIndex); } + /// + /// Enumerates the bodies attached to an active constraint and removes the constraint's handle from all of the connected body constraint reference lists. + /// + struct RemoveConstraintReferencesFromBodiesEnumerator : IForEach + { + internal Solver solver; + internal ConstraintHandle constraintHandle; + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void LoopBody(int encodedBodyIndex) + { + var bodyIndex = encodedBodyIndex & Bodies.BodyReferenceMask; + //Note that this only looks in the active set. Directly removing inactive objects is unsupported- removals and adds activate all involved islands. + if (solver.bodies.RemoveConstraintReference(bodyIndex, constraintHandle) && Bodies.IsEncodedKinematicReference(encodedBodyIndex)) + { + var removed = solver.ConstrainedKinematicHandles.FastRemove(solver.bodies.ActiveSet.IndexToHandle[bodyIndex].Value); + Debug.Assert(removed, "If we just removed the last constraint from a kinematic, then the constrained kinematics set must have contained the body handle so it can be removed."); + } + } + } + /// /// Removes the constraint associated with the given handle. Note that this may invalidate any outstanding direct constraint references /// by reordering the constraints within the TypeBatch subject to removal. @@ -1262,10 +1317,10 @@ public void Remove(ConstraintHandle handle) awakener.AwakenConstraint(handle); } Debug.Assert(constraintLocation.SetIndex == 0); - ConstraintGraphRemovalEnumerator enumerator; - enumerator.bodies = bodies; + RemoveConstraintReferencesFromBodiesEnumerator enumerator; + enumerator.solver = this; enumerator.constraintHandle = handle; - EnumerateActiveConnectedBodyIndices(handle, ref enumerator); + EnumerateConnectedRawBodyReferences(handle, ref enumerator); pairCache.RemoveReferenceIfContactConstraint(handle, constraintLocation.TypeId); RemoveFromBatch(handle, constraintLocation.BatchIndex, constraintLocation.TypeId, constraintLocation.IndexInTypeBatch); @@ -1571,7 +1626,6 @@ public void Clear() /// /// Size of the span of body handles to allocate space for. Applies to batch referenced handle sets. /// Number of constraint handles to allocate space for. Applies to the handle->constraint mapping table. - /// Number of constrained kinematic body handles to allocate space for. public void EnsureSolverCapacities(int bodyHandleCapacity, int constraintHandleCapacity) { if (HandleToConstraint.Length < constraintHandleCapacity) From f96f3714629c8aa4496573320b8bbcec7b305743 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 5 Nov 2021 18:35:24 -0500 Subject: [PATCH 262/947] Fixed a doofbug in constrained kinematic integration indexing. --- BepuPhysics/PoseIntegrator.cs | 33 +++++----- Demos/DemoSet.cs | 1 + .../ConstrainedKinematicIntegrationTest.cs | 61 +++++++++++++++++++ 3 files changed, 78 insertions(+), 17 deletions(-) create mode 100644 Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs diff --git a/BepuPhysics/PoseIntegrator.cs b/BepuPhysics/PoseIntegrator.cs index 77b94518e..7d6f0a802 100644 --- a/BepuPhysics/PoseIntegrator.cs +++ b/BepuPhysics/PoseIntegrator.cs @@ -793,7 +793,7 @@ internal unsafe void IntegrateKinematicVelocities(Buffer bodyHandles, int b var countInBundle = Math.Min(bodyCount - bundleBaseIndex, Vector.Count); for (int i = 0; i < countInBundle; ++i) { - bodyIndices[i] = handleToLocation[bodyHandles[i]].Index; + bodyIndices[i] = handleToLocation[bodyHandles[bundleBaseIndex + i]].Index; } var existingMask = BundleIndexing.CreateMaskForCountInBundle(countInBundle); @@ -834,7 +834,7 @@ internal unsafe void IntegrateKinematicPosesAndVelocities(Buffer bodyHandle var countInBundle = Math.Min(bodyCount - bundleBaseIndex, Vector.Count); for (int i = 0; i < countInBundle; ++i) { - bodyIndices[i] = handleToLocation[bodyHandles[i]].Index; + bodyIndices[i] = handleToLocation[bodyHandles[bundleBaseIndex + i]].Index; } var existingMask = BundleIndexing.CreateMaskForCountInBundle(countInBundle); @@ -848,11 +848,13 @@ internal unsafe void IntegrateKinematicPosesAndVelocities(Buffer bodyHandle position += velocity.Linear * bundleDt; //Kinematic bodies have infinite inertia, so using the angular momentum conserving codepaths would hit a singularity. PoseIntegration.Integrate(orientation, velocity.Angular, halfDt, out orientation); + bodies.ScatterPose(ref position, ref orientation, bodyIndicesVector, existingMask); if (callbacks.IntegrateVelocityForKinematics) + { callbacks.IntegrateVelocity(bodyIndicesVector, position, orientation, zeroInertia, existingMask, workerIndex, bundleDt, ref velocity); - //Writes to the empty lanes won't matter (scatter is masked), so we don't need to clean them up. - bodies.ScatterPose(ref position, ref orientation, bodyIndicesVector, existingMask); - bodies.ScatterVelocities(ref velocity, ref bodyIndicesVector); + //Writes to the empty lanes won't matter (scatter is masked), so we don't need to clean them up. + bodies.ScatterVelocities(ref velocity, ref bodyIndicesVector); + } } } @@ -926,12 +928,14 @@ unsafe void IntegrateBundlesAfterSubstepping(ref IndexSet mergedConstrainedBodyH else { var isKinematic = - Vector.Equals(Vector.BitwiseOr( - Vector.BitwiseOr(Vector.BitwiseOr(localInertia.InverseMass, localInertia.InverseInertiaTensor.XX), Vector.BitwiseOr(localInertia.InverseInertiaTensor.YX, localInertia.InverseInertiaTensor.YY)), - Vector.BitwiseOr(Vector.BitwiseOr(localInertia.InverseInertiaTensor.ZX, localInertia.InverseInertiaTensor.ZY), localInertia.InverseInertiaTensor.ZZ)), Vector.Zero); + Vector.Equals(Vector.BitwiseOr( + Vector.BitwiseOr(Vector.BitwiseOr(localInertia.InverseMass, localInertia.InverseInertiaTensor.XX), Vector.BitwiseOr(localInertia.InverseInertiaTensor.YX, localInertia.InverseInertiaTensor.YY)), + Vector.BitwiseOr(Vector.BitwiseOr(localInertia.InverseInertiaTensor.ZX, localInertia.InverseInertiaTensor.ZY), localInertia.InverseInertiaTensor.ZZ)), Vector.Zero); unconstrainedVelocityIntegrationMask = Vector.AndNot(unconstrainedMask, isKinematic); anyBodyInBundleNeedsVelocityIntegration = Vector.LessThanAny(unconstrainedVelocityIntegrationMask, Vector.Zero); } + //We don't want to scatter velocities into any slots that don't want velocity writes. By setting all the bits in such lanes, velocity scatter will skip them. + var velocityMaskedBodyIndices = Vector.BitwiseOr(bodyIndices, Vector.OnesComplement(unconstrainedVelocityIntegrationMask)); if (anyBodyInBundleIsUnconstrained) { @@ -990,19 +994,14 @@ unsafe void IntegrateBundlesAfterSubstepping(ref IndexSet mergedConstrainedBodyH } } bodies.ScatterPose(ref position, ref orientation, bodyIndices, integratePoseMask); - //We already masked the velocities above, so scattering them doesn't need its own mask. - bodies.ScatterVelocities(ref velocity, ref bodyIndices); + if (anyBodyInBundleNeedsVelocityIntegration) + { + bodies.ScatterVelocities(ref velocity, ref velocityMaskedBodyIndices); + } } } else { - //var halfDt = substepDt * 0.5f; - //for (int innerIndex = 0; innerIndex < countInBundle; ++innerIndex) - //{ - // ref var state = ref bodies.ActiveSet.SolverStates[bundleBaseIndex + innerIndex]; - // PoseIntegration.Integrate(state.Motion.Pose.Orientation, state.Motion.Velocity.Angular, halfDt, out state.Motion.Pose.Orientation); - // state.Motion.Pose.Position += substepDt * state.Motion.Velocity.Linear; - //} //All bodies in the bundle are constrained, so we do not need to do any kind of velocity integration. PoseIntegration.Integrate(orientation, velocity.Angular, halfDt, out orientation); position += velocity.Linear * bundleEffectiveDt; diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index c710dcbb1..5f2816774 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -45,6 +45,7 @@ struct Option public DemoSet() { + AddOption(); AddOption(); AddOption(); AddOption(); diff --git a/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs b/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs new file mode 100644 index 000000000..15476f625 --- /dev/null +++ b/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs @@ -0,0 +1,61 @@ +using BepuUtilities; +using DemoRenderer; +using BepuPhysics; +using BepuPhysics.Collidables; +using System.Numerics; +using DemoContentLoader; +using BepuPhysics.Constraints; +using Demos.Demos; +using System; + +namespace Demos.SpecializedTests +{ + public class ConstrainedKinematicIntegrationTest : Demo + { + + public unsafe override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(25, 4, 40); + camera.Yaw = 0; + Simulation = Simulation.Create(BufferPool, + new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(30, 1), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 2f }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), 1); + //new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(30, 1), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 2f }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper(), 8); + + var shapeA = new Box(.75f, 1, .5f); + var shapeIndexA = Simulation.Shapes.Add(shapeA); + var collidableA = new CollidableDescription(shapeIndexA, 0.1f); + var shapeB = new Box(.75f, 1, .5f); + var shapeIndexB = Simulation.Shapes.Add(shapeB); + var collidableB = new CollidableDescription(shapeIndexB, 0.1f); + var activity = new BodyActivityDescription(0.01f); + shapeA.ComputeInertia(1, out var inertiaA); + shapeA.ComputeInertia(1, out var inertiaB); + + for (int i = 0; i < 32; ++i) + { + var x = 0; + var z = i * 3; + var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, z), collidableA, activity)); + var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, z), inertiaB, collidableB, activity)); + Simulation.Bodies.GetBodyReference(a).Velocity.Linear = new Vector3(1, 0, 0); + Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + Simulation.Solver.Add(a, b, new AngularHinge { LocalHingeAxisA = new Vector3(0, 1, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); + } + + for (int i = 0; i < 32; ++i) + { + var x = 0; + var z = i * 3; + var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 8, z), collidableA, activity)); + //var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, z), inertiaB, collidableB, activity)); + Simulation.Bodies.GetBodyReference(a).Velocity.Linear = new Vector3(1, 0, 0); + //Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + //Simulation.Solver.Add(a, b, new AngularHinge { LocalHingeAxisA = new Vector3(0, 1, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); + } + + Simulation.Statics.Add(new StaticDescription(new Vector3(), new CollidableDescription(Simulation.Shapes.Add(new Box(256, 1, 256)), 0.1f))); + } + } +} + + From 5c01fc74bde5b0844c9180161e57aedb8195795a Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 5 Nov 2021 23:46:46 -0500 Subject: [PATCH 263/947] Body memory moves now properly consider kinematicity. Fixed validation for fallbacks and kinematics. --- BepuPhysics/Constraints/TypeProcessor.cs | 15 +++++++++++++-- BepuPhysics/SequentialFallbackBatch.cs | 2 +- BepuPhysics/Solver.cs | 23 ++++++++++++----------- Demos/Demos/FountainStressTestDemo.cs | 23 +++++++++++++++-------- 4 files changed, 41 insertions(+), 22 deletions(-) diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index 8b016fd11..54771a261 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -103,7 +103,16 @@ public unsafe void EnumerateAccumulatedImpulses(ref TypeBatch typeB } } public abstract void ScaleAccumulatedImpulses(ref TypeBatch typeBatch, float scale); - public abstract void UpdateForBodyMemoryMove(ref TypeBatch typeBatch, int indexInTypeBatch, int bodyIndexInConstraint, int newBodyLocation); + + /// + /// Updates a type batch's body index references for the movement of a body in memory. + /// + /// Type batch containing a constraint that references the body. + /// Index of the constraint in the type batch. + /// Index within the constraint of the body. + /// New index of the body in the bodies active set. + /// True if the body being moved was kinematic according to the constraint's reference. + public abstract bool UpdateForBodyMemoryMove(ref TypeBatch typeBatch, int indexInTypeBatch, int bodyIndexInConstraint, int newBodyLocation); public abstract void Scramble(ref TypeBatch typeBatch, Random random, ref Buffer handlesToConstraints); @@ -805,14 +814,16 @@ internal sealed override void GetBundleTypeSizes(out int bodyReferencesBundleSiz } - public sealed override void UpdateForBodyMemoryMove(ref TypeBatch typeBatch, int indexInTypeBatch, int bodyIndexInConstraint, int newBodyLocation) + public sealed override bool UpdateForBodyMemoryMove(ref TypeBatch typeBatch, int indexInTypeBatch, int bodyIndexInConstraint, int newBodyLocation) { BundleIndexing.GetBundleIndices(indexInTypeBatch, out var constraintBundleIndex, out var constraintInnerIndex); //Note that this relies on the bodyreferences memory layout. It uses the stride of vectors to skip to the next body based on the bodyIndexInConstraint. ref var bundle = ref Unsafe.As>(ref Buffer.Get(ref typeBatch.BodyReferences, constraintBundleIndex)); ref var referenceLocation = ref GatherScatter.Get(ref bundle, constraintInnerIndex + bodyIndexInConstraint * Vector.Count); //Note that the old kinematic mask is preserved so that the caller doesn't have to requery the object for its kinematicity. + var isKinematic = Bodies.IsEncodedKinematicReference(referenceLocation); referenceLocation = newBodyLocation | (referenceLocation & Bodies.KinematicMask); + return isKinematic; } //Note that these next two sort key users require a generic sort key implementation; this avoids virtual dispatch on a per-object level while still sharing the bulk of the logic. diff --git a/BepuPhysics/SequentialFallbackBatch.cs b/BepuPhysics/SequentialFallbackBatch.cs index 3cef64546..012e0ff1b 100644 --- a/BepuPhysics/SequentialFallbackBatch.cs +++ b/BepuPhysics/SequentialFallbackBatch.cs @@ -230,7 +230,7 @@ public static unsafe void ValidateReferences(Solver solver) ValidateSetReferences(solver, i); } } - internal void UpdateForBodyMemoryMove(int originalBodyIndex, int newBodyLocation) + internal void UpdateForDynamicBodyMemoryMove(int originalBodyIndex, int newBodyLocation) { Debug.Assert(dynamicBodyConstraintCounts.Keys.Allocated && !dynamicBodyConstraintCounts.ContainsKey(newBodyLocation), "If a body is being moved, as opposed to swapped, then the target index should not be present."); dynamicBodyConstraintCounts.GetTableIndices(ref originalBodyIndex, out var tableIndex, out var elementIndex); diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index b0d5f94a6..a36a5c666 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -456,14 +456,14 @@ internal unsafe void ValidateFallbackBodiesAreDynamic() ++fallbackConstraintsForDynamicBody; } } + var bodyIsInFallbackDynamicsSet = ActiveSet.SequentialFallback.dynamicBodyConstraintCounts.TryGetValue(i, out var countForBody); if (Bodies.IsKinematicUnsafeGCHole(ref bodies.ActiveSet.SolverStates[i].Inertia.Local)) { - Debug.Assert(fallbackConstraintsForDynamicBody == 0, "Kinematics should not be present in the dynamic bodies referenced by the fallback batch."); + Debug.Assert(!bodyIsInFallbackDynamicsSet, "Kinematics should not be present in the dynamic bodies referenced by the fallback batch."); } else { - var succeeded = ActiveSet.SequentialFallback.dynamicBodyConstraintCounts.TryGetValue(i, out var countForBody); - Debug.Assert(succeeded == (fallbackConstraintsForDynamicBody > 0), "The fallback batch should contain a reference to the dynamic body if there are constraints associated with it in the fallback batch."); + Debug.Assert(bodyIsInFallbackDynamicsSet == (fallbackConstraintsForDynamicBody > 0), "The fallback batch should contain a reference to the dynamic body if there are constraints associated with it in the fallback batch."); Debug.Assert(fallbackConstraintsForDynamicBody == countForBody, "If the dynamic body is referenced in the fallback batch, the brute force count should match the cached count."); } } @@ -503,7 +503,7 @@ internal unsafe void ValidateFallbackBatchAccessSafety() for (int innerIndex = 0; innerIndex < Vector.Count; ++innerIndex) { var index = bodyReferencesForSource[innerIndex]; - if (index >= 0) + if (index >= 0 && Bodies.IsEncodedDynamicReference(index)) { var broadcasted = new Vector(bodyReferencesForSource[innerIndex]); int matchesTotal = 0; @@ -513,7 +513,7 @@ internal unsafe void ValidateFallbackBatchAccessSafety() var matchesInLane = -Vector.Dot(Vector.Equals(broadcasted, bodyReferencesForTarget), Vector.One); matchesTotal += matchesInLane; } - Debug.Assert(matchesTotal == 1, "A body reference should occur no more than once in any constraint bundle."); + Debug.Assert(matchesTotal == 1, "A dynamic body reference should occur no more than once in any constraint bundle."); } } } @@ -1370,10 +1370,11 @@ private bool UpdateConstraintsForBodyMemoryMove(int originalIndex, int newIndex) //This does require a virtual call, but memory swaps should not be an ultra-frequent thing. //(A few hundred calls per frame in a simulation of 10000 active objects would probably be overkill.) //(Also, there's a sufficient number of cache-missy indirections here that a virtual call is pretty irrelevant.) - TypeProcessors[constraintLocation.TypeId].UpdateForBodyMemoryMove( + var bodyIsKinematic = TypeProcessors[constraintLocation.TypeId].UpdateForBodyMemoryMove( ref ActiveSet.Batches[constraintLocation.BatchIndex].GetTypeBatch(constraintLocation.TypeId), constraintLocation.IndexInTypeBatch, constraint.BodyIndexInConstraint, newIndex); - if (constraintLocation.BatchIndex == FallbackBatchThreshold) + //Note that only dynamic bodies + if (!bodyIsKinematic && constraintLocation.BatchIndex == FallbackBatchThreshold) bodyShouldBePresentInFallback = true; } return bodyShouldBePresentInFallback; @@ -1388,8 +1389,8 @@ internal void UpdateForBodyMemoryMove(int originalBodyIndex, int newBodyLocation { if (UpdateConstraintsForBodyMemoryMove(originalBodyIndex, newBodyLocation)) { - //One of the moved constraints involved the fallback batch, so we need to update the fallback batch's body indices. - ActiveSet.SequentialFallback.UpdateForBodyMemoryMove(originalBodyIndex, newBodyLocation); + //One of the moved constraints involved the fallback batch, and this body was dynamic, so we need to update the fallback batch's body indices. + ActiveSet.SequentialFallback.UpdateForDynamicBodyMemoryMove(originalBodyIndex, newBodyLocation); } } @@ -1408,11 +1409,11 @@ internal void UpdateForBodyMemorySwap(int a, int b) } else if (aInFallback) { - ActiveSet.SequentialFallback.UpdateForBodyMemoryMove(a, b); + ActiveSet.SequentialFallback.UpdateForDynamicBodyMemoryMove(a, b); } else if (bInFallback) { - ActiveSet.SequentialFallback.UpdateForBodyMemoryMove(b, a); + ActiveSet.SequentialFallback.UpdateForDynamicBodyMemoryMove(b, a); } } diff --git a/Demos/Demos/FountainStressTestDemo.cs b/Demos/Demos/FountainStressTestDemo.cs index a7a57e94c..1ab3fc6ad 100644 --- a/Demos/Demos/FountainStressTestDemo.cs +++ b/Demos/Demos/FountainStressTestDemo.cs @@ -24,7 +24,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = MathHelper.Pi * 0.1f; //Using minimum sized allocations forces as many resizes as possible. //Note the low solverFallbackBatchThreshold- we want the fallback batches to get tested thoroughly. - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), 1, solverFallbackBatchThreshold: 4, initialAllocationSizes: + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), 1, solverFallbackBatchThreshold: 2, initialAllocationSizes: new SimulationAllocationSizes { Bodies = 1, @@ -237,6 +237,7 @@ public override void Update(Window window, Camera camera, Input input, float dt) var timestepDuration = 1f / 60f; time += timestepDuration; + Simulation.Solver.ValidateConstrainedKinematicsSet(); //Occasionally, the animation stops completely. The resulting velocities will be zero, so the kinematics will have a chance to rest (testing kinematic rest states). var dip = 0.1; var progressionMultiplier = 0.5 - dip + (1 + dip) * 0.5 * Math.Cos(time * 0.25); @@ -285,6 +286,7 @@ public override void Update(Window window, Camera camera, Input input, float dt) } } } + Simulation.Solver.ValidateConstrainedKinematicsSet(); //Remove some statics from the simulation. var missingStaticsAsymptote = 512; @@ -296,6 +298,7 @@ public override void Update(Window window, Camera camera, Input input, float dt) Simulation.Statics.RemoveAt(indexToRemove); removedStatics.Enqueue(staticDescription, BufferPool); } + Simulation.Solver.ValidateConstrainedKinematicsSet(); var staticApplyDescriptionsPerFrame = 8; for (int i = 0; i < staticApplyDescriptionsPerFrame; ++i) @@ -311,6 +314,7 @@ public override void Update(Window window, Camera camera, Input input, float dt) Simulation.Statics.ApplyDescription(handleToReapply, staticDescription); } + Simulation.Solver.ValidateConstrainedKinematicsSet(); //Add some of the missing static bodies back into the simulation. var staticAddCount = removedStatics.Count * (staticRemovalsPerFrame / (float)missingStaticsAsymptote); @@ -322,12 +326,13 @@ public override void Update(Window window, Camera camera, Input input, float dt) } + Simulation.Solver.ValidateConstrainedKinematicsSet(); //Spray some shapes! - int newShapeCount = 8; + int newShapeCount = 1; var spawnPose = new RigidPose(new Vector3(0, 10, 0)); for (int i = 0; i < newShapeCount; ++i) { - CreateBodyDescription(random, spawnPose, new BodyVelocity(new Vector3(-30 + 60 * (float)random.NextDouble(), 75, -30 + 60 * (float)random.NextDouble()), default), out var bodyDescription); + CreateBodyDescription(random, spawnPose, new BodyVelocity(new Vector3(-60 + 120 * (float)random.NextDouble(), 0, -60 + 120 * (float)random.NextDouble()), default), out var bodyDescription); dynamicHandles.Enqueue(Simulation.Bodies.Add(bodyDescription), BufferPool); } int targetAsymptote = 65536; @@ -347,6 +352,7 @@ public override void Update(Window window, Camera camera, Input input, float dt) break; } } + Simulation.Solver.ValidateConstrainedKinematicsSet(); //Change some dynamic objects without adding/removing them to make sure all the state transition stuff works reasonably well. var dynamicApplyDescriptionsPerFrame = 8; @@ -356,14 +362,15 @@ public override void Update(Window window, Camera camera, Input input, float dt) Simulation.Bodies.GetDescription(handle, out var description); Simulation.Shapes.RecursivelyRemoveAndDispose(description.Collidable.Shape, BufferPool); CreateBodyDescription(random, description.Pose, description.Velocity, out var newDescription); - if (random.NextDouble() < 0.1f) - { - //Occasionally make a dynamic kinematic. - newDescription.LocalInertia = default; - } + //if (random.NextDouble() < 0.1f) + //{ + // //Occasionally make a dynamic kinematic. + // newDescription.LocalInertia = default; + //} Simulation.Bodies.ApplyDescription(handle, newDescription); } + Simulation.Solver.ValidateConstrainedKinematicsSet(); base.Update(window, camera, input, dt); if (input != null && input.WasPushed(OpenTK.Input.Key.P)) From 2cc9732caeba66a6e461a01abf2da9643f327b99 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 6 Nov 2021 11:56:38 -0500 Subject: [PATCH 264/947] Choice of angular conservation fallback is now all or nothing. --- BepuPhysics/PoseIntegrator.cs | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/BepuPhysics/PoseIntegrator.cs b/BepuPhysics/PoseIntegrator.cs index 7d6f0a802..522de218b 100644 --- a/BepuPhysics/PoseIntegrator.cs +++ b/BepuPhysics/PoseIntegrator.cs @@ -194,9 +194,12 @@ public static void RotateInverseInertia(in Symmetric3x3Wide localInverseInertiaT static void FallbackIfInertiaIncompatible(in Vector3Wide previousAngularVelocity, ref Vector3Wide angularVelocity) { var infinity = new Vector(float.PositiveInfinity); - angularVelocity.X = Vector.ConditionalSelect(Vector.LessThan(Vector.Abs(angularVelocity.X), infinity), angularVelocity.X, previousAngularVelocity.X); - angularVelocity.Y = Vector.ConditionalSelect(Vector.LessThan(Vector.Abs(angularVelocity.Y), infinity), angularVelocity.Y, previousAngularVelocity.Y); - angularVelocity.Z = Vector.ConditionalSelect(Vector.LessThan(Vector.Abs(angularVelocity.Z), infinity), angularVelocity.Z, previousAngularVelocity.Z); + var useNewVelocity = Vector.BitwiseAnd(Vector.LessThan(Vector.Abs(angularVelocity.X), infinity), Vector.BitwiseAnd( + Vector.LessThan(Vector.Abs(angularVelocity.Y), infinity), + Vector.LessThan(Vector.Abs(angularVelocity.Z), infinity))); + angularVelocity.X = Vector.ConditionalSelect(useNewVelocity, angularVelocity.X, previousAngularVelocity.X); + angularVelocity.Y = Vector.ConditionalSelect(useNewVelocity, angularVelocity.Y, previousAngularVelocity.Y); + angularVelocity.Z = Vector.ConditionalSelect(useNewVelocity, angularVelocity.Z, previousAngularVelocity.Z); } [MethodImpl(MethodImplOptions.AggressiveInlining)] From 251e5ef747fc70ed5a8586322f03fa3c40fc7354 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 6 Nov 2021 15:07:34 -0500 Subject: [PATCH 265/947] Simplified thread workblock claims (and in doing so, fixed a kinematic velocity integration bug). --- BepuPhysics/Solver_SubsteppingSolve2.cs | 56 ++++++++----------- .../ConstrainedKinematicIntegrationTest.cs | 5 +- 2 files changed, 26 insertions(+), 35 deletions(-) diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index a5b6651f5..b2dffdb00 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -223,7 +223,7 @@ public void Execute(Solver solver, int blockIndex, int workerIndex) } } - void ExecuteWorkerStage(ref TStageFunction stageFunction, int workerIndex, int workerStart, int availableBlocksStartIndex, ref Buffer claims, int previousSyncIndexOffset, int syncIndex, ref int completedWorkBlocks) where TStageFunction : IStageFunction + void ExecuteWorkerStage(ref TStageFunction stageFunction, int workerIndex, int workerStart, int availableBlocksStartIndex, ref Buffer claims, int syncIndex, ref int completedWorkBlocks) where TStageFunction : IStageFunction { if (workerStart == -1) { @@ -234,9 +234,8 @@ void ExecuteWorkerStage(ref TStageFunction stageFunction, int wo } int workBlockIndex = workerStart; int locallyCompletedCount = 0; - var previousSyncIndex = Math.Max(0, syncIndex - previousSyncIndexOffset); - //Try to claim blocks by traversing forward until we're blocked by another claim. - while (Interlocked.CompareExchange(ref claims[workBlockIndex], syncIndex, previousSyncIndex) == previousSyncIndex) + //Try to claim blocks by traversing forward until we're blocked by another claim. + while (Interlocked.Exchange(ref claims[workBlockIndex], syncIndex) < syncIndex) { //Successfully claimed a work block. stageFunction.Execute(this, availableBlocksStartIndex + workBlockIndex, workerIndex); @@ -257,7 +256,11 @@ void ExecuteWorkerStage(ref TStageFunction stageFunction, int wo //Wrap around. workBlockIndex = claims.Length - 1; } - if (Interlocked.CompareExchange(ref claims[workBlockIndex], syncIndex, previousSyncIndex) != previousSyncIndex) + //Note the comparison: equal *or greater* blocks. + //Consider what happens if this thread was heavily delayed and the stage it was dispatched for has already ended. + //Other threads could be working on the next sync index. A mere equality test could result in this thread thinking there's work to be done, so it starts claiming for an *earlier* stage. + //Then everything dies. + if (Interlocked.Exchange(ref claims[workBlockIndex], syncIndex) >= syncIndex) { break; } @@ -282,7 +285,7 @@ void ExecuteWorkerStage(ref TStageFunction stageFunction, int wo //} } - void ExecuteMainStage(ref TStageFunction stageFunction, int workerIndex, int workerStart, ref SolverSyncStage stage, int previousSyncIndexOffset, ref int syncIndex) where TStageFunction : IStageFunction + void ExecuteMainStage(ref TStageFunction stageFunction, int workerIndex, int workerStart, ref SolverSyncStage stage, ref int syncIndex) where TStageFunction : IStageFunction { //Note that the main thread's view of the sync index increments every single time, even if there is no work. //This ensures that the workers are able to advance to the appropriate stage by examining the sync index snapshot. @@ -308,7 +311,7 @@ void ExecuteMainStage(ref TStageFunction stageFunction, int work //Console.WriteLine($"Main thread is requesting workers begin for sync index {syncIndex}; stage function: {stageFunction.GetType().Name}"); //Write the new stage index so other spinning threads will begin work on it. Volatile.Write(ref substepContext.SyncIndex, syncIndex); - ExecuteWorkerStage(ref stageFunction, workerIndex, workerStart, stage.WorkBlockStartIndex, ref stage.Claims, previousSyncIndexOffset, syncIndex, ref substepContext.CompletedWorkBlockCount); + ExecuteWorkerStage(ref stageFunction, workerIndex, workerStart, stage.WorkBlockStartIndex, ref stage.Claims, syncIndex, ref substepContext.CompletedWorkBlockCount); //Since we asked other threads to do work, we must wait until the requested work is done before proceeding. //Note that we DO NOT yield on the main thread! @@ -389,20 +392,7 @@ void SolveStep2Worker2(int workerIndex) solver = this }; - //A thread is only allowed to claim a workblock if the claim index for that workblock matches the expected value- which is the claim index it would have from the last time it was executed. - //Each thread calculates what that claim index would have been based on the current sync index by subtracting the expected number of sync indices elapsed since last execution. - var syncStagesPerWarmStartOrSolve = synchronizedBatchCount; - //All warmstarts and solves, plus an incremental contact update. First substep doesn't do an incremental contact update, but that's fine, it'll end up expecting 0. - var syncOffsetToPreviousSubstep = 2 + syncStagesPerWarmStartOrSolve * (1 + IterationCount); - //To find the previous execution sync index of a constraint batch, we have to scan through all the constraint batches, but ALSO skip over the incremental contact update stage and kinematic integration, hence + 2. - var syncOffsetToPreviousClaimOnBatchForWarmStart = syncStagesPerWarmStartOrSolve + 2; - //For solves, there is no incremental update in the way. - var syncOffsetToPreviousClaimOnBatchForSolve = syncStagesPerWarmStartOrSolve; - - //Inactive stages in the first substep (incremental contact updates and kinematic velocity integration) will have 0 in their claims buffer. - //Using a longer offset puts them at 0 like they're supposed to be. - var syncOffsetToPreviousSubstepForSecondSubstep = syncOffsetToPreviousSubstep + (PoseIntegrator.Callbacks.IntegrateVelocityForKinematics ? 1 : 2); - + var syncStagesPerSubstep = 2 + synchronizedBatchCount * (1 + IterationCount); if (workerIndex == 0) { //This is the main 'orchestrator' thread. It tracks execution progress and notifies other threads that's it's time to work. @@ -411,7 +401,7 @@ void SolveStep2Worker2(int workerIndex) { if (substepIndex > 0) { - ExecuteMainStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStart, ref substepContext.Stages[0], substepIndex == 1 ? syncOffsetToPreviousSubstepForSecondSubstep : syncOffsetToPreviousSubstep, ref syncIndex); + ExecuteMainStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStart, ref substepContext.Stages[0], ref syncIndex); } else { @@ -421,16 +411,16 @@ void SolveStep2Worker2(int workerIndex) if (substepIndex > 0 || PoseIntegrator.Callbacks.IntegrateVelocityForKinematics) { integrateConstrainedKinematicsStage.SubstepIndex = substepIndex; - ExecuteMainStage(ref integrateConstrainedKinematicsStage, workerIndex, kinematicIntegrationWorkerStart, ref substepContext.Stages[1], substepIndex == 1 ? syncOffsetToPreviousSubstepForSecondSubstep : syncOffsetToPreviousSubstep, ref syncIndex); + ExecuteMainStage(ref integrateConstrainedKinematicsStage, workerIndex, kinematicIntegrationWorkerStart, ref substepContext.Stages[1], ref syncIndex); } else { syncIndex++; } warmstartStage.SubstepIndex = substepIndex; - for (int batchIndex = 0; batchIndex < syncStagesPerWarmStartOrSolve; ++batchIndex) + for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) { - ExecuteMainStage(ref warmstartStage, workerIndex, batchStarts[batchIndex], ref substepContext.Stages[batchIndex + 2], syncOffsetToPreviousClaimOnBatchForWarmStart, ref syncIndex); + ExecuteMainStage(ref warmstartStage, workerIndex, batchStarts[batchIndex], ref substepContext.Stages[batchIndex + 2], ref syncIndex); } if (fallbackExists) { @@ -452,11 +442,11 @@ void SolveStep2Worker2(int workerIndex) } for (int iterationIndex = 0; iterationIndex < IterationCount; ++iterationIndex) { - for (int batchIndex = 0; batchIndex < syncStagesPerWarmStartOrSolve; ++batchIndex) + for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) { //Note that this is using a 'different' stage by index than the worker thread if the iteration index > 1. //That's totally fine- the warmstart/iteration stages share the same claims buffers per batch. They're redundant for the sake of easier indexing. - ExecuteMainStage(ref solveStage, workerIndex, batchStarts[batchIndex], ref substepContext.Stages[batchIndex + 2], syncOffsetToPreviousClaimOnBatchForSolve, ref syncIndex); + ExecuteMainStage(ref solveStage, workerIndex, batchStarts[batchIndex], ref substepContext.Stages[batchIndex + 2], ref syncIndex); } if (fallbackExists) { @@ -501,9 +491,9 @@ void SolveStep2Worker2(int workerIndex) syncIndexInSubstep += syncStepsSinceLast; while (true) { - if (syncIndexInSubstep >= syncOffsetToPreviousSubstep) + if (syncIndexInSubstep >= syncStagesPerSubstep) { - syncIndexInSubstep -= syncOffsetToPreviousSubstep; + syncIndexInSubstep -= syncStagesPerSubstep; ++substepIndex; } else @@ -519,18 +509,18 @@ void SolveStep2Worker2(int workerIndex) switch (stage.StageType) { case SolverStageType.IncrementalUpdate: - ExecuteWorkerStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStart, 0, ref stage.Claims, substepIndex == 1 ? syncOffsetToPreviousSubstepForSecondSubstep : syncOffsetToPreviousSubstep, syncIndex, ref substepContext.CompletedWorkBlockCount); + ExecuteWorkerStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStart, 0, ref stage.Claims, syncIndex, ref substepContext.CompletedWorkBlockCount); break; case SolverStageType.IntegrateConstrainedKinematics: integrateConstrainedKinematicsStage.SubstepIndex = substepIndex; - ExecuteWorkerStage(ref integrateConstrainedKinematicsStage, workerIndex, kinematicIntegrationWorkerStart, 0, ref stage.Claims, substepIndex == 1 ? syncOffsetToPreviousSubstepForSecondSubstep : syncOffsetToPreviousSubstep, syncIndex, ref substepContext.CompletedWorkBlockCount); + ExecuteWorkerStage(ref integrateConstrainedKinematicsStage, workerIndex, kinematicIntegrationWorkerStart, 0, ref stage.Claims, syncIndex, ref substepContext.CompletedWorkBlockCount); break; case SolverStageType.WarmStart: warmstartStage.SubstepIndex = substepIndex; - ExecuteWorkerStage(ref warmstartStage, workerIndex, batchStarts[stage.BatchIndex], stage.WorkBlockStartIndex, ref stage.Claims, syncOffsetToPreviousClaimOnBatchForWarmStart, syncIndex, ref substepContext.CompletedWorkBlockCount); + ExecuteWorkerStage(ref warmstartStage, workerIndex, batchStarts[stage.BatchIndex], stage.WorkBlockStartIndex, ref stage.Claims, syncIndex, ref substepContext.CompletedWorkBlockCount); break; case SolverStageType.Solve: - ExecuteWorkerStage(ref solveStage, workerIndex, batchStarts[stage.BatchIndex], stage.WorkBlockStartIndex, ref stage.Claims, syncOffsetToPreviousClaimOnBatchForSolve, syncIndex, ref substepContext.CompletedWorkBlockCount); + ExecuteWorkerStage(ref solveStage, workerIndex, batchStarts[stage.BatchIndex], stage.WorkBlockStartIndex, ref stage.Claims, syncIndex, ref substepContext.CompletedWorkBlockCount); break; } latestCompletedSyncIndex = syncIndex; diff --git a/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs b/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs index 15476f625..17db92d83 100644 --- a/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs +++ b/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs @@ -18,7 +18,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(25, 4, 40); camera.Yaw = 0; Simulation = Simulation.Create(BufferPool, - new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(30, 1), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 2f }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), 1); + new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(30, 1), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 2f }, new DemoPoseIntegratorCallbacks(new Vector3(0, -0.1f, 0)), new EmbeddedSubsteppingTimestepper2(3), 1); //new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(30, 1), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 2f }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper(), 8); var shapeA = new Box(.75f, 1, .5f); @@ -47,8 +47,9 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var x = 0; var z = i * 3; var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 8, z), collidableA, activity)); - //var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, z), inertiaB, collidableB, activity)); + var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 8, z + 2), inertiaB, collidableB, activity)); Simulation.Bodies.GetBodyReference(a).Velocity.Linear = new Vector3(1, 0, 0); + Simulation.Bodies.GetBodyReference(b).Velocity.Linear = new Vector3(1, 0, 0); //Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); //Simulation.Solver.Add(a, b, new AngularHinge { LocalHingeAxisA = new Vector3(0, 1, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); } From 5f7797bd1e79a3f358e4747a09b22a2a980aefde Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 6 Nov 2021 18:01:45 -0500 Subject: [PATCH 266/947] That's the second time I tried to use Exchange even though I had previously known it's not sufficient dangit. --- BepuPhysics/Solver_SubsteppingSolve2.cs | 71 ++++++++++++------- .../ConstrainedKinematicIntegrationTest.cs | 2 +- 2 files changed, 47 insertions(+), 26 deletions(-) diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index b2dffdb00..88df92124 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -223,7 +223,7 @@ public void Execute(Solver solver, int blockIndex, int workerIndex) } } - void ExecuteWorkerStage(ref TStageFunction stageFunction, int workerIndex, int workerStart, int availableBlocksStartIndex, ref Buffer claims, int syncIndex, ref int completedWorkBlocks) where TStageFunction : IStageFunction + unsafe void ExecuteWorkerStage(ref TStageFunction stageFunction, int workerIndex, int workerStart, int availableBlocksStartIndex, ref Buffer claims, int previousSyncIndex, int syncIndex, ref int completedWorkBlocks) where TStageFunction : IStageFunction { if (workerStart == -1) { @@ -234,8 +234,8 @@ void ExecuteWorkerStage(ref TStageFunction stageFunction, int wo } int workBlockIndex = workerStart; int locallyCompletedCount = 0; - //Try to claim blocks by traversing forward until we're blocked by another claim. - while (Interlocked.Exchange(ref claims[workBlockIndex], syncIndex) < syncIndex) + //Try to claim blocks by traversing forward until we're blocked by another claim. + while (Interlocked.CompareExchange(ref claims[workBlockIndex], syncIndex, previousSyncIndex) == previousSyncIndex) { //Successfully claimed a work block. stageFunction.Execute(this, availableBlocksStartIndex + workBlockIndex, workerIndex); @@ -260,7 +260,7 @@ void ExecuteWorkerStage(ref TStageFunction stageFunction, int wo //Consider what happens if this thread was heavily delayed and the stage it was dispatched for has already ended. //Other threads could be working on the next sync index. A mere equality test could result in this thread thinking there's work to be done, so it starts claiming for an *earlier* stage. //Then everything dies. - if (Interlocked.Exchange(ref claims[workBlockIndex], syncIndex) >= syncIndex) + if (Interlocked.CompareExchange(ref claims[workBlockIndex], syncIndex, previousSyncIndex) != previousSyncIndex) { break; } @@ -271,6 +271,8 @@ void ExecuteWorkerStage(ref TStageFunction stageFunction, int wo } //No more adjacent work blocks are available. This thread is done! Interlocked.Add(ref completedWorkBlocks, locallyCompletedCount); + + //debugStageWorkBlocksCompleted[syncIndex - 1][workerIndex] = locallyCompletedCount; //if (workerIndex == 3) //{ @@ -285,11 +287,8 @@ void ExecuteWorkerStage(ref TStageFunction stageFunction, int wo //} } - void ExecuteMainStage(ref TStageFunction stageFunction, int workerIndex, int workerStart, ref SolverSyncStage stage, ref int syncIndex) where TStageFunction : IStageFunction + void ExecuteMainStage(ref TStageFunction stageFunction, int workerIndex, int workerStart, ref SolverSyncStage stage, int previousSyncIndex, int syncIndex) where TStageFunction : IStageFunction { - //Note that the main thread's view of the sync index increments every single time, even if there is no work. - //This ensures that the workers are able to advance to the appropriate stage by examining the sync index snapshot. - ++syncIndex; var availableBlocksCount = stage.Claims.Length; if (availableBlocksCount == 0) return; @@ -311,7 +310,7 @@ void ExecuteMainStage(ref TStageFunction stageFunction, int work //Console.WriteLine($"Main thread is requesting workers begin for sync index {syncIndex}; stage function: {stageFunction.GetType().Name}"); //Write the new stage index so other spinning threads will begin work on it. Volatile.Write(ref substepContext.SyncIndex, syncIndex); - ExecuteWorkerStage(ref stageFunction, workerIndex, workerStart, stage.WorkBlockStartIndex, ref stage.Claims, syncIndex, ref substepContext.CompletedWorkBlockCount); + ExecuteWorkerStage(ref stageFunction, workerIndex, workerStart, stage.WorkBlockStartIndex, ref stage.Claims, previousSyncIndex, syncIndex, ref substepContext.CompletedWorkBlockCount); //Since we asked other threads to do work, we must wait until the requested work is done before proceeding. //Note that we DO NOT yield on the main thread! @@ -335,6 +334,30 @@ void ExecuteMainStage(ref TStageFunction stageFunction, int work + [MethodImpl(MethodImplOptions.AggressiveInlining)] + int GetPreviousSyncIndexForIncrementalUpdate(int substepIndex, int syncIndex, int syncStagesPerSubstep) + { + return substepIndex == 1 ? 0 : Math.Max(0, syncIndex - syncStagesPerSubstep); + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + int GetPreviousSyncIndexForIntegrateConstrainedKinematics(int substepIndex, int syncIndex, int syncStagesPerSubstep) + { + //If kinematics have their velocities integrated, then the first substep will have executed and left the claims at 1. Otherwise, the first substep will leave them cleared at 0. + //The second substep and later will always run (since kinematics need their poses integrated regardless) so their sync index isn't weirdly conditional. + return substepIndex == 1 ? PoseIntegrator.Callbacks.IntegrateVelocityForKinematics ? 1 : 0 : Math.Max(0, syncIndex - syncStagesPerSubstep); + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + int GetPreviousSyncIndexForWarmStart(int syncIndex, int synchronizedBatchCount) + { + //The claims for warmstarts and solves are shared. So we want to look back to the last solve's claims, which would be beyond the incremental update and integrate constrained kinematics. + return Math.Max(0, syncIndex - synchronizedBatchCount - 2); + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + int GetPreviousSyncIndexForSolve(int syncIndex, int synchronizedBatchCount) + { + return Math.Max(0, syncIndex - synchronizedBatchCount); + } + Action solveStep2Worker2; void SolveStep2Worker2(int workerIndex) { @@ -399,28 +422,25 @@ void SolveStep2Worker2(int workerIndex) int syncIndex = 0; for (int substepIndex = 0; substepIndex < substepCount; ++substepIndex) { + //Note that the main thread's view of the sync index increments every single dispatch, even if there is no work. + //This ensures that the workers are able to advance to the appropriate stage by examining the sync index snapshot. + ++syncIndex; if (substepIndex > 0) { - ExecuteMainStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStart, ref substepContext.Stages[0], ref syncIndex); - } - else - { - syncIndex++; + ExecuteMainStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStart, ref substepContext.Stages[0], GetPreviousSyncIndexForIncrementalUpdate(substepIndex, syncIndex, syncStagesPerSubstep), syncIndex); } //Note that we do not invoke velocity integration on the first substep if kinematics do not need velocity integration. + ++syncIndex; if (substepIndex > 0 || PoseIntegrator.Callbacks.IntegrateVelocityForKinematics) { integrateConstrainedKinematicsStage.SubstepIndex = substepIndex; - ExecuteMainStage(ref integrateConstrainedKinematicsStage, workerIndex, kinematicIntegrationWorkerStart, ref substepContext.Stages[1], ref syncIndex); - } - else - { - syncIndex++; + ExecuteMainStage(ref integrateConstrainedKinematicsStage, workerIndex, kinematicIntegrationWorkerStart, ref substepContext.Stages[1], GetPreviousSyncIndexForIntegrateConstrainedKinematics(substepIndex, syncIndex, syncStagesPerSubstep), syncIndex); } warmstartStage.SubstepIndex = substepIndex; for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) { - ExecuteMainStage(ref warmstartStage, workerIndex, batchStarts[batchIndex], ref substepContext.Stages[batchIndex + 2], ref syncIndex); + ++syncIndex; + ExecuteMainStage(ref warmstartStage, workerIndex, batchStarts[batchIndex], ref substepContext.Stages[batchIndex + 2], GetPreviousSyncIndexForWarmStart(syncIndex, synchronizedBatchCount), syncIndex); } if (fallbackExists) { @@ -446,7 +466,8 @@ void SolveStep2Worker2(int workerIndex) { //Note that this is using a 'different' stage by index than the worker thread if the iteration index > 1. //That's totally fine- the warmstart/iteration stages share the same claims buffers per batch. They're redundant for the sake of easier indexing. - ExecuteMainStage(ref solveStage, workerIndex, batchStarts[batchIndex], ref substepContext.Stages[batchIndex + 2], ref syncIndex); + ++syncIndex; + ExecuteMainStage(ref solveStage, workerIndex, batchStarts[batchIndex], ref substepContext.Stages[batchIndex + 2], GetPreviousSyncIndexForSolve(syncIndex, synchronizedBatchCount), syncIndex); } if (fallbackExists) { @@ -509,18 +530,18 @@ void SolveStep2Worker2(int workerIndex) switch (stage.StageType) { case SolverStageType.IncrementalUpdate: - ExecuteWorkerStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStart, 0, ref stage.Claims, syncIndex, ref substepContext.CompletedWorkBlockCount); + ExecuteWorkerStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStart, 0, ref stage.Claims, GetPreviousSyncIndexForIncrementalUpdate(substepIndex, syncIndex, syncStagesPerSubstep), syncIndex, ref substepContext.CompletedWorkBlockCount); break; case SolverStageType.IntegrateConstrainedKinematics: integrateConstrainedKinematicsStage.SubstepIndex = substepIndex; - ExecuteWorkerStage(ref integrateConstrainedKinematicsStage, workerIndex, kinematicIntegrationWorkerStart, 0, ref stage.Claims, syncIndex, ref substepContext.CompletedWorkBlockCount); + ExecuteWorkerStage(ref integrateConstrainedKinematicsStage, workerIndex, kinematicIntegrationWorkerStart, 0, ref stage.Claims, GetPreviousSyncIndexForIntegrateConstrainedKinematics(substepIndex, syncIndex, syncStagesPerSubstep), syncIndex, ref substepContext.CompletedWorkBlockCount); break; case SolverStageType.WarmStart: warmstartStage.SubstepIndex = substepIndex; - ExecuteWorkerStage(ref warmstartStage, workerIndex, batchStarts[stage.BatchIndex], stage.WorkBlockStartIndex, ref stage.Claims, syncIndex, ref substepContext.CompletedWorkBlockCount); + ExecuteWorkerStage(ref warmstartStage, workerIndex, batchStarts[stage.BatchIndex], stage.WorkBlockStartIndex, ref stage.Claims, GetPreviousSyncIndexForWarmStart(syncIndex, synchronizedBatchCount), syncIndex, ref substepContext.CompletedWorkBlockCount); break; case SolverStageType.Solve: - ExecuteWorkerStage(ref solveStage, workerIndex, batchStarts[stage.BatchIndex], stage.WorkBlockStartIndex, ref stage.Claims, syncIndex, ref substepContext.CompletedWorkBlockCount); + ExecuteWorkerStage(ref solveStage, workerIndex, batchStarts[stage.BatchIndex], stage.WorkBlockStartIndex, ref stage.Claims, GetPreviousSyncIndexForSolve(syncIndex, synchronizedBatchCount), syncIndex, ref substepContext.CompletedWorkBlockCount); break; } latestCompletedSyncIndex = syncIndex; diff --git a/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs b/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs index 17db92d83..8f25bc28c 100644 --- a/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs +++ b/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs @@ -54,7 +54,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //Simulation.Solver.Add(a, b, new AngularHinge { LocalHingeAxisA = new Vector3(0, 1, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); } - Simulation.Statics.Add(new StaticDescription(new Vector3(), new CollidableDescription(Simulation.Shapes.Add(new Box(256, 1, 256)), 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(), new CollidableDescription(Simulation.Shapes.Add(new Box(8192, 1, 8192)), 0.1f))); } } } From 50283268b7fe0b4eb2bad3c84e004944e69fb34f Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 6 Nov 2021 18:11:00 -0500 Subject: [PATCH 267/947] Re-fixed the bug that motivated the changes in the first place. --- BepuPhysics/Solver_SubsteppingSolve2.cs | 2 +- Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index 88df92124..0b8e70a6d 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -344,7 +344,7 @@ int GetPreviousSyncIndexForIntegrateConstrainedKinematics(int substepIndex, int { //If kinematics have their velocities integrated, then the first substep will have executed and left the claims at 1. Otherwise, the first substep will leave them cleared at 0. //The second substep and later will always run (since kinematics need their poses integrated regardless) so their sync index isn't weirdly conditional. - return substepIndex == 1 ? PoseIntegrator.Callbacks.IntegrateVelocityForKinematics ? 1 : 0 : Math.Max(0, syncIndex - syncStagesPerSubstep); + return substepIndex == 1 ? PoseIntegrator.Callbacks.IntegrateVelocityForKinematics ? 2 : 0 : Math.Max(0, syncIndex - syncStagesPerSubstep); } [MethodImpl(MethodImplOptions.AggressiveInlining)] int GetPreviousSyncIndexForWarmStart(int syncIndex, int synchronizedBatchCount) diff --git a/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs b/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs index 8f25bc28c..7b4b6fd99 100644 --- a/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs +++ b/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs @@ -18,8 +18,8 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(25, 4, 40); camera.Yaw = 0; Simulation = Simulation.Create(BufferPool, - new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(30, 1), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 2f }, new DemoPoseIntegratorCallbacks(new Vector3(0, -0.1f, 0)), new EmbeddedSubsteppingTimestepper2(3), 1); - //new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(30, 1), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 2f }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper(), 8); + new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(30, 1), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 2f }, + new DemoPoseIntegratorCallbacks(new Vector3(0, -0.1f, 0)), new EmbeddedSubsteppingTimestepper2(3), 1); var shapeA = new Box(.75f, 1, .5f); var shapeIndexA = Simulation.Shapes.Add(shapeA); From 4f7df8cbfc92e1c6f35240ca33f492607630e043 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 7 Nov 2021 13:29:04 -0600 Subject: [PATCH 268/947] Got rid of TypeProcessor.Enumerate... in favor of a Solver side implementation with a bit more flexibility. In the middle of updating body kinematicity state change. --- BepuPhysics/BatchCompressor.cs | 2 +- BepuPhysics/Bodies.cs | 79 ++++++---- .../CollisionDetection/ConstraintRemover.cs | 2 +- .../CollisionDetection/FreshnessChecker.cs | 2 +- BepuPhysics/ConstraintBatch.cs | 2 +- .../Constraints/FourBodyTypeProcessor.cs | 10 -- .../Constraints/OneBodyTypeProcessor.cs | 7 - .../Constraints/ThreeBodyTypeProcessor.cs | 9 -- .../Constraints/TwoBodyTypeProcessor.cs | 10 -- BepuPhysics/Constraints/TypeProcessor.cs | 26 ++-- BepuPhysics/IslandScaffold.cs | 2 +- BepuPhysics/IslandSleeper.cs | 4 +- BepuPhysics/SequentialFallbackBatch.cs | 11 +- BepuPhysics/Solver.cs | 137 ++++++++++++------ .../SpecializedTests/SimulationScrambling.cs | 4 +- 15 files changed, 168 insertions(+), 139 deletions(-) diff --git a/BepuPhysics/BatchCompressor.cs b/BepuPhysics/BatchCompressor.cs index 03ac0bb06..e0d7c0ebd 100644 --- a/BepuPhysics/BatchCompressor.cs +++ b/BepuPhysics/BatchCompressor.cs @@ -131,7 +131,7 @@ private unsafe void TryToFindBetterBatchForConstraint( BufferPool pool, ref QuickList compressions, ref TypeBatch typeBatch, int* bodyHandles, ref ActiveConstraintDynamicBodyHandleCollector handleAccumulator, TypeProcessor typeProcessor, int constraintIndex) { handleAccumulator.Count = 0; - typeProcessor.EnumerateConnectedRawBodyReferences(ref typeBatch, constraintIndex, ref handleAccumulator); + Solver.EnumerateConnectedRawBodyReferences(ref typeBatch, constraintIndex, ref handleAccumulator); var dynamicBodyHandles = new Span(bodyHandles, handleAccumulator.Count); for (int batchIndex = nextBatchIndex - 1; batchIndex >= 0; --batchIndex) { diff --git a/BepuPhysics/Bodies.cs b/BepuPhysics/Bodies.cs index d01cc2376..a18d628ea 100644 --- a/BepuPhysics/Bodies.cs +++ b/BepuPhysics/Bodies.cs @@ -364,35 +364,61 @@ public unsafe void LoopBody(int bodyIndex) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - void UpdateForKinematicStateChange(BodyHandle handle, ref BodyMemoryLocation location, ref BodySet set, bool newlyKinematic) + unsafe void UpdateForKinematicStateChange(BodyHandle handle, ref BodyMemoryLocation location, ref BodySet set, bool previouslyKinematic, bool currentlyKinematic) { Debug.Assert(location.SetIndex == 0, "If we're changing kinematic state, we should have already awoken the body."); - ref var collidable = ref set.Collidables[location.Index]; - if (collidable.Shape.Exists) + if (previouslyKinematic != currentlyKinematic) { - var mobility = IsKinematic(set.SolverStates[location.Index].Inertia.Local) ? CollidableMobility.Kinematic : CollidableMobility.Dynamic; - if (location.SetIndex == 0) + ref var collidable = ref set.Collidables[location.Index]; + if (collidable.Shape.Exists) { - broadPhase.activeLeaves[collidable.BroadPhaseIndex] = new CollidableReference(mobility, handle); + //Any collidable references need their encoded mobility updated. + var mobility = currentlyKinematic ? CollidableMobility.Kinematic : CollidableMobility.Dynamic; + if (location.SetIndex == 0) + { + broadPhase.activeLeaves[collidable.BroadPhaseIndex] = new CollidableReference(mobility, handle); + } + else + { + broadPhase.staticLeaves[collidable.BroadPhaseIndex] = new CollidableReference(mobility, handle); + } } - else + ref var constraints = ref set.Constraints[location.Index]; + if (currentlyKinematic) { - broadPhase.staticLeaves[collidable.BroadPhaseIndex] = new CollidableReference(mobility, handle); + //Any constraints that connect only kinematic bodies together should be removed; they'll NaN out. + //Ideally, the user would handle this for all non-contact constraints, but it would be rather annoying to + //have to explicitly enumerate and remove all contact constraints any time you wanted to make a body kinematic. + ConnectedDynamicCounter enumerator; + for (int i = 0; i < constraints.Count; ++i) + { + ref var constraint = ref constraints[i]; + enumerator.DynamicCount = 0; + solver.EnumerateConnectedDynamicBodies(constraint.ConnectingConstraintHandle, ref enumerator); + if (enumerator.DynamicCount == 0) + { + //This constraint connects only kinematic bodies; keeping it in the solver would cause a singularity. + solver.Remove(constraint.ConnectingConstraintHandle); + } + } } - } - if (newlyKinematic) - { - ref var constraints = ref set.Constraints[location.Index]; - ConnectedDynamicCounter enumerator; - for (int i = 0; i < constraints.Count; ++i) + else { - ref var constraint = ref constraints[i]; - enumerator.DynamicCount = 0; - solver.EnumerateActiveDynamicConnectedBodyIndices(constraint.ConnectingConstraintHandle, ref enumerator); - if (enumerator.DynamicCount == 0) + //A kinematic body has become dynamic. Kinematic bodies do not block membership in constraint batches, dynamic bodies do. + //For any constraint connected to the new dynamic, ensure that it belongs to a constraint batch not shared by any other constraints connected to the same body. + const int maximumBodiesPerConstraint = 4; + int* handles = stackalloc int[maximumBodiesPerConstraint]; + ActiveConstraintDynamicBodyHandleCollector enumerator = new(this, handles); + for (int i = 0; i < constraints.Count; ++i) { - //This constraint connects only kinematic bodies; keeping it in the solver would cause a singularity. - solver.Remove(constraint.ConnectingConstraintHandle); + ref var constraint = ref constraints[i]; + enumerator.Count = 0; + solver.EnumerateConnectedDynamicBodies(constraint.ConnectingConstraintHandle, ref enumerator); + if (enumerator.Count == 0) + { + //This constraint connects only kinematic bodies; keeping it in the solver would cause a singularity. + solver.Remove(constraint.ConnectingConstraintHandle); + } } } } @@ -420,9 +446,10 @@ public void SetLocalInertia(BodyHandle handle, in BodyInertia localInertia) //Note that the HandleToLocation slot reference is still valid; it may have been updated, but handle slots don't move. ref var set = ref Sets[location.SetIndex]; ref var localInertiaReference = ref set.SolverStates[location.Index].Inertia.Local; - var newlyKinematic = IsKinematic(localInertia) && !IsKinematic(localInertiaReference); + var nowKinematic = IsKinematic(localInertia); + var previouslyKinematic = IsKinematicUnsafeGCHole(ref localInertiaReference); localInertiaReference = localInertia; - UpdateForKinematicStateChange(handle, ref location, ref set, newlyKinematic); + UpdateForKinematicStateChange(handle, ref location, ref set, previouslyKinematic, nowKinematic); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -488,10 +515,11 @@ public void ApplyDescription(BodyHandle handle, in BodyDescription description) ref var set = ref Sets[location.SetIndex]; ref var collidable = ref set.Collidables[location.Index]; var oldShape = collidable.Shape; - var newlyKinematic = IsKinematic(description.LocalInertia) && !IsKinematic(set.SolverStates[location.Index].Inertia.Local); + var nowKinematic = IsKinematic(description.LocalInertia); + var previouslyKinematic = IsKinematicUnsafeGCHole(ref set.SolverStates[location.Index].Inertia.Local); set.ApplyDescriptionByIndex(location.Index, description); UpdateForShapeChange(handle, location.Index, oldShape, description.Collidable.Shape); - UpdateForKinematicStateChange(handle, ref location, ref set, newlyKinematic); + UpdateForKinematicStateChange(handle, ref location, ref set, previouslyKinematic, nowKinematic); UpdateBounds(handle); } @@ -651,7 +679,6 @@ public void LoopBody(int connectedBodyHandle) /// Type of the enumerator to execute on each connected body. /// Index of the active body to enumerate the connections of. This body will not appear in the set of enumerated bodies, even if it is connected to itself somehow. /// Enumerator instance to run on each connected body. - /// Solver from which to pull constraint body references. [MethodImpl(MethodImplOptions.AggressiveInlining)] internal void EnumerateConnectedBodyIndices(int activeBodyIndex, ref TEnumerator enumerator) where TEnumerator : IForEach { @@ -664,7 +691,7 @@ internal void EnumerateConnectedBodyIndices(int activeBodyIndex, re //Non-reversed iteration would result in skipped elements if the loop body removed anything. This relies on convention; any remover should be aware of this order. for (int i = list.Count - 1; i >= 0; --i) { - solver.EnumerateActiveConnectedBodyIndices(list[i].ConnectingConstraintHandle, ref constraintBodiesEnumerator); + solver.EnumerateConnectedBodyReferences(list[i].ConnectingConstraintHandle, ref constraintBodiesEnumerator); } //Note that we have to assume the enumerator contains state mutated by the internal loop bodies. //If it's a value type, those mutations won't be reflected in the original reference. diff --git a/BepuPhysics/CollisionDetection/ConstraintRemover.cs b/BepuPhysics/CollisionDetection/ConstraintRemover.cs index 97532a38b..8e26b32af 100644 --- a/BepuPhysics/CollisionDetection/ConstraintRemover.cs +++ b/BepuPhysics/CollisionDetection/ConstraintRemover.cs @@ -147,7 +147,7 @@ public unsafe void EnqueueForRemoval(ConstraintHandle constraintHandle, Solver s ref var typeBatch = ref constraintBatch.TypeBatches[typeBatchIndex.TypeBatch]; var encodedBodyIndices = stackalloc int[bodiesPerConstraint]; var enumerator = new PassthroughReferenceCollector(encodedBodyIndices); - typeProcessor.EnumerateConnectedRawBodyReferences(ref typeBatch, constraint.IndexInTypeBatch, ref enumerator); + solver.EnumerateConnectedRawBodyReferences(ref typeBatch, constraint.IndexInTypeBatch, ref enumerator); for (int i = 0; i < bodiesPerConstraint; ++i) { diff --git a/BepuPhysics/CollisionDetection/FreshnessChecker.cs b/BepuPhysics/CollisionDetection/FreshnessChecker.cs index a0a1e037b..28e46c0c4 100644 --- a/BepuPhysics/CollisionDetection/FreshnessChecker.cs +++ b/BepuPhysics/CollisionDetection/FreshnessChecker.cs @@ -143,7 +143,7 @@ unsafe void PrintRemovalInformation(ConstraintHandle constraintHandle) var typeProcessor = constraintRemover.solver.TypeProcessors[location.TypeId]; var references = stackalloc int[typeProcessor.BodiesPerConstraint]; var enumerator = new ActiveConstraintBodyIndexCollector(references); - typeProcessor.EnumerateConnectedRawBodyReferences(ref typeBatch, location.IndexInTypeBatch, ref enumerator); + constraintRemover.solver.EnumerateConnectedRawBodyReferences(ref typeBatch, location.IndexInTypeBatch, ref enumerator); for (int i = 0; i < typeProcessor.BodiesPerConstraint; ++i) { var bodyHandle = constraintRemover.bodies.ActiveSet.IndexToHandle[references[i]]; diff --git a/BepuPhysics/ConstraintBatch.cs b/BepuPhysics/ConstraintBatch.cs index 6ab24cba5..2cba627c2 100644 --- a/BepuPhysics/ConstraintBatch.cs +++ b/BepuPhysics/ConstraintBatch.cs @@ -189,7 +189,7 @@ public unsafe void RemoveBodyHandlesFromBatchForConstraint(int constraintTypeId, var indexSet = solver.batchReferencedHandles.GetPointer(batchIndex); var handleRemover = new ActiveBodyHandleRemover(solver.bodies, indexSet); var typeBatchIndex = TypeIndexToTypeBatchIndex[constraintTypeId]; - solver.TypeProcessors[constraintTypeId].EnumerateConnectedRawBodyReferences(ref TypeBatches[typeBatchIndex], indexInTypeBatch, ref handleRemover); + solver.EnumerateConnectedRawBodyReferences(ref TypeBatches[typeBatchIndex], indexInTypeBatch, ref handleRemover); } public unsafe void Remove(int constraintTypeId, int indexInTypeBatch, bool isFallback, Solver solver) diff --git a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs index 563251e89..52c514e1f 100644 --- a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs @@ -75,16 +75,6 @@ public abstract class FourBodyTypeProcessor 4; - public sealed unsafe override void EnumerateConnectedRawBodyReferences(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.Get(typeBatch.BodyReferences.Memory, constraintBundleIndex), constraintInnerIndex); - //Note that we hold a reference to the indices. That's important if the loop body mutates indices. - enumerator.LoopBody(GatherScatter.GetFirst(ref indices.IndexA)); - enumerator.LoopBody(GatherScatter.GetFirst(ref indices.IndexB)); - enumerator.LoopBody(GatherScatter.GetFirst(ref indices.IndexC)); - enumerator.LoopBody(GatherScatter.GetFirst(ref indices.IndexD)); - } struct FourBodySortKeyGenerator : ISortKeyGenerator { [MethodImpl(MethodImplOptions.AggressiveInlining)] diff --git a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs index a57b8badb..75e41150f 100644 --- a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs @@ -56,13 +56,6 @@ public abstract class OneBodyTypeProcessor 1; - public sealed unsafe override void EnumerateConnectedRawBodyReferences(ref TypeBatch typeBatch, int indexInTypeBatch, ref TEnumerator enumerator) - { - BundleIndexing.GetBundleIndices(indexInTypeBatch, out var constraintBundleIndex, out var constraintInnerIndex); - enumerator.LoopBody(GatherScatter.Get(ref Buffer>.Get(ref typeBatch.BodyReferences, constraintBundleIndex), constraintInnerIndex)); - } - - struct OneBodySortKeyGenerator : ISortKeyGenerator> { [MethodImpl(MethodImplOptions.AggressiveInlining)] diff --git a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs index 6604de5df..c2312a3b3 100644 --- a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs @@ -66,15 +66,6 @@ public abstract class ThreeBodyTypeProcessor 3; - public sealed unsafe override void EnumerateConnectedRawBodyReferences(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.Get(typeBatch.BodyReferences.Memory, constraintBundleIndex), constraintInnerIndex); - //Note that we hold a reference to the indices. That's important if the loop body mutates indices. - enumerator.LoopBody(GatherScatter.GetFirst(ref indices.IndexA)); - enumerator.LoopBody(GatherScatter.GetFirst(ref indices.IndexB)); - enumerator.LoopBody(GatherScatter.GetFirst(ref indices.IndexC)); - } struct ThreeBodySortKeyGenerator : ISortKeyGenerator { [MethodImpl(MethodImplOptions.AggressiveInlining)] diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 576af1940..704252634 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -72,16 +72,6 @@ public abstract class TwoBodyTypeProcessor 2; - public sealed override void EnumerateConnectedRawBodyReferences(ref TypeBatch typeBatch, int indexInTypeBatch, ref TEnumerator enumerator) - { - BundleIndexing.GetBundleIndices(indexInTypeBatch, out var constraintBundleIndex, out var constraintInnerIndex); - ref var referenceA = ref GatherScatter.Get(ref Buffer.Get(ref typeBatch.BodyReferences, constraintBundleIndex).IndexA, constraintInnerIndex); - ref var referenceB = ref Unsafe.Add(ref referenceA, Vector.Count); - //Note that the variables are ref locals! This is important for correctness, because every execution of LoopBody could result in a swap. - //Ref locals aren't the only solution, but if you ever change this, make sure you account for the potential mutation in the enumerator. - enumerator.LoopBody(referenceA); - enumerator.LoopBody(referenceB); - } struct TwoBodySortKeyGenerator : ISortKeyGenerator { [MethodImpl(MethodImplOptions.AggressiveInlining)] diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index 54771a261..d196d1320 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -72,21 +72,14 @@ public void Initialize(int typeId) /// /// Moves a constraint from one ConstraintBatch's TypeBatch to another ConstraintBatch's TypeBatch of the same type. /// + /// Source type batch from which the constraint will be taken. /// Index of the batch that owns the type batch that is the source of the constraint transfer. /// Index of the constraint to move in the current type batch. /// Solver that owns the batches. /// Bodies set that owns all the constraint's bodies. /// Index of the ConstraintBatch in the solver to copy the constraint into. - public unsafe abstract void TransferConstraint(ref TypeBatch typeBatch, int sourceBatchIndex, int indexInTypeBatch, Solver solver, Bodies bodies, int targetBatchIndex); + public unsafe abstract void TransferConstraint(ref TypeBatch sourceTypeBatch, int sourceBatchIndex, int indexInTypeBatch, Solver solver, Bodies bodies, int targetBatchIndex); - /// - /// Enumerates body references in the constraint. Reported body references (body index for an awake constraint, body handle for a sleeping constraint) include encoded metadata like whether the body is kinematic. - /// - /// Type of the enumerator called for each body index in the constraint. - /// Type batch containing the constraint to enumerate. - /// Index of the constraint to enumerate in the type batch. - /// Enumerator called for each body index in the constraint. - public abstract void EnumerateConnectedRawBodyReferences(ref TypeBatch typeBatch, int indexInTypeBatch, ref TEnumerator enumerator) where TEnumerator : IForEach; [Conditional("DEBUG")] protected abstract void ValidateAccumulatedImpulsesSizeInBytes(int sizeInBytes); public unsafe void EnumerateAccumulatedImpulses(ref TypeBatch typeBatch, int indexInTypeBatch, ref TEnumerator enumerator) where TEnumerator : IForEach @@ -720,12 +713,13 @@ public void LoopBody(int encodedBodyIndex) /// /// Moves a constraint from one ConstraintBatch's TypeBatch to another ConstraintBatch's TypeBatch of the same type. /// + /// Source type batch to transfer the constraint out of. /// Index of the batch that owns the type batch that is the source of the constraint transfer. /// Index of the constraint to move in the current type batch. /// Solver that owns the batches. /// Bodies set that owns all the constraint's bodies. /// Index of the ConstraintBatch in the solver to copy the constraint into. - public unsafe override void TransferConstraint(ref TypeBatch typeBatch, int sourceBatchIndex, int indexInTypeBatch, Solver solver, Bodies bodies, int targetBatchIndex) + public unsafe override void TransferConstraint(ref TypeBatch sourceTypeBatch, int sourceBatchIndex, int indexInTypeBatch, Solver solver, Bodies bodies, int targetBatchIndex) { //Note that the following does some redundant work. It's technically possible to do better than this, but it requires bypassing a lot of bookkeeping. //It's not exactly trivial to keep everything straight, especially over time- it becomes a maintenance nightmare. @@ -734,11 +728,9 @@ public unsafe override void TransferConstraint(ref TypeBatch typeBatch, int sour var dynamicBodyHandles = stackalloc int[bodiesPerConstraint]; var encodedBodyIndices = stackalloc int[bodiesPerConstraint]; var bodyHandleCollector = new ActiveKinematicFlaggedBodyHandleCollector(bodies, dynamicBodyHandles, encodedBodyIndices); - EnumerateConnectedRawBodyReferences(ref typeBatch, indexInTypeBatch, ref bodyHandleCollector); - Debug.Assert(targetBatchIndex <= solver.FallbackBatchThreshold, - "Constraint transfers should never target the fallback batch. Its registered body handles don't block new constraints so attempting to allocate in the same way wouldn't turn out well."); + solver.EnumerateConnectedRawBodyReferences(ref sourceTypeBatch, indexInTypeBatch, ref bodyHandleCollector); //Allocate a spot in the new batch. Note that it does not change the Handle->Constraint mapping in the Solver; that's important when we call Solver.Remove below. - var constraintHandle = typeBatch.IndexToHandle[indexInTypeBatch]; + var constraintHandle = sourceTypeBatch.IndexToHandle[indexInTypeBatch]; solver.AllocateInBatch(targetBatchIndex, constraintHandle, new Span((BodyHandle*)dynamicBodyHandles, bodyHandleCollector.DynamicCount), new Span(encodedBodyIndices, bodiesPerConstraint), typeId, out var targetReference); BundleIndexing.GetBundleIndices(targetReference.IndexInTypeBatch, out var targetBundle, out var targetInner); @@ -747,9 +739,9 @@ public unsafe override void TransferConstraint(ref TypeBatch typeBatch, int sour //Instead, we just directly copy from lane to lane. //Note that we leave out the runtime generated bits- they'll just get regenerated. CopyConstraintData( - ref Buffer.Get(ref typeBatch.BodyReferences, sourceBundle), - ref Buffer.Get(ref typeBatch.PrestepData, sourceBundle), - ref Buffer.Get(ref typeBatch.AccumulatedImpulses, sourceBundle), + ref Buffer.Get(ref sourceTypeBatch.BodyReferences, sourceBundle), + ref Buffer.Get(ref sourceTypeBatch.PrestepData, sourceBundle), + ref Buffer.Get(ref sourceTypeBatch.AccumulatedImpulses, sourceBundle), sourceInner, ref Buffer.Get(ref targetReference.TypeBatch.BodyReferences, targetBundle), ref Buffer.Get(ref targetReference.TypeBatch.PrestepData, targetBundle), diff --git a/BepuPhysics/IslandScaffold.cs b/BepuPhysics/IslandScaffold.cs index 63c708ef2..a2e2a8ef7 100644 --- a/BepuPhysics/IslandScaffold.cs +++ b/BepuPhysics/IslandScaffold.cs @@ -160,7 +160,7 @@ unsafe void AddConstraint(ConstraintHandle constraintHandle, Solver solver, Buff ConstraintHandleEnumerator enumerator; enumerator.BodyIndices = bodyIndices; enumerator.Count = 0; - solver.EnumerateActiveDynamicConnectedBodyIndices(constraintHandle, ref enumerator); + solver.EnumerateConnectedDynamicBodies(constraintHandle, ref enumerator); var dynamicBodyIndices = new Span(enumerator.BodyIndices, enumerator.Count); for (int batchIndex = 0; batchIndex < Protobatches.Count; ++batchIndex) { diff --git a/BepuPhysics/IslandSleeper.cs b/BepuPhysics/IslandSleeper.cs index 963576ebe..b10502b10 100644 --- a/BepuPhysics/IslandSleeper.cs +++ b/BepuPhysics/IslandSleeper.cs @@ -150,7 +150,7 @@ bool EnqueueUnvisitedNeighbors(int bodyIndex, constraintHandles.Add(entry.ConnectingConstraintHandle, pool); consideredConstraints.AddUnsafely(entry.ConnectingConstraintHandle.Value); bodyEnumerator.ConstraintBodyIndices.Count = 0; - solver.EnumerateActiveConnectedBodyIndices(entry.ConnectingConstraintHandle, ref bodyEnumerator); + solver.EnumerateConnectedBodyReferences(entry.ConnectingConstraintHandle, ref bodyEnumerator); for (int j = 0; j < bodyEnumerator.ConstraintBodyIndices.Count; ++j) { var connectedBodyIndex = bodyEnumerator.ConstraintBodyIndices[j]; @@ -571,7 +571,7 @@ unsafe void PrintIsland(ref IslandScaffold island) Debug.Assert(location.TypeId == typeBatch.TypeId); ref var solverBatch = ref solver.Sets[0].Batches[location.BatchIndex]; bodyIndexEnumerator.Index = 0; - typeProcessor.EnumerateConnectedRawBodyReferences( + solver.EnumerateConnectedRawBodyReferences( ref solverBatch.TypeBatches[solverBatch.TypeIndexToTypeBatchIndex[location.TypeId]], location.IndexInTypeBatch, ref bodyIndexEnumerator); for (int i = 0; i < typeProcessor.BodiesPerConstraint; ++i) { diff --git a/BepuPhysics/SequentialFallbackBatch.cs b/BepuPhysics/SequentialFallbackBatch.cs index 012e0ff1b..13ab1455d 100644 --- a/BepuPhysics/SequentialFallbackBatch.cs +++ b/BepuPhysics/SequentialFallbackBatch.cs @@ -157,7 +157,7 @@ internal unsafe void Remove(Solver solver, BufferPool bufferPool, ref Constraint var allocationIdsToRemoveMemory = stackalloc int[maximumAllocationIdsToFree]; var initialSpan = new Buffer(allocationIdsToRemoveMemory, maximumAllocationIdsToFree); var allocationIdsToFree = new QuickList(initialSpan); - typeProcessor.EnumerateConnectedRawBodyReferences(ref batch.TypeBatches[batch.TypeIndexToTypeBatchIndex[typeId]], indexInTypeBatch, ref enumerator); + solver.EnumerateConnectedRawBodyReferences(ref batch.TypeBatches[batch.TypeIndexToTypeBatchIndex[typeId]], indexInTypeBatch, ref enumerator); for (int i = 0; i < bodyCount; ++i) { var rawBodyIndex = bodyIndices[i]; @@ -203,14 +203,7 @@ public static unsafe void ValidateSetReferences(Solver solver, int setIndex) { var constraintHandle = typeBatch.IndexToHandle[constraintIndex]; var collector = new PassthroughReferenceCollector(connectedBodies); - if (setIndex == 0) - { - solver.EnumerateActiveDynamicConnectedBodyIndices(constraintHandle, ref collector); - } - else - { - solver.EnumerateConnectedRawBodyReferences(constraintHandle, ref collector); - } + solver.EnumerateConnectedDynamicBodies(constraintHandle, ref collector); for (int i = 0; i < bodiesPerConstraint; ++i) { var localBodyIndex = bodyConstraintCounts.IndexOf(connectedBodies[i]); diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index a36a5c666..7610bfce8 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -742,7 +742,7 @@ internal unsafe void ValidateExistingHandles(bool activeOnly = false) for (int indexInTypeBatch = 0; indexInTypeBatch < typeBatch.ConstraintCount; ++indexInTypeBatch) { enumerator.Index = 0; - processor.EnumerateConnectedRawBodyReferences(ref typeBatch, indexInTypeBatch, ref enumerator); + EnumerateConnectedRawBodyReferences(ref typeBatch, indexInTypeBatch, ref enumerator); for (int i = 0; i < processor.BodiesPerConstraint; ++i) { @@ -776,7 +776,7 @@ internal unsafe void ValidateExistingHandles(bool activeOnly = false) ref var typeBatch = ref batch.TypeBatches[batch.TypeIndexToTypeBatchIndex[constraintLocation.TypeId]]; var processor = TypeProcessors[typeBatch.TypeId]; enumerator.Index = 0; - processor.EnumerateConnectedRawBodyReferences(ref typeBatch, constraintLocation.IndexInTypeBatch, ref enumerator); + EnumerateConnectedRawBodyReferences(ref typeBatch, constraintLocation.IndexInTypeBatch, ref enumerator); //Active constraints refer to bodies by index; inactive constraints use handles. int bodyReference = setIndex == 0 ? bodyIndex : bodySet.IndexToHandle[bodyIndex].Value; var bodyReferenceInConstraint = constraintBodyReferences[constraintList[constraintIndex].BodyIndexInConstraint]; @@ -977,7 +977,7 @@ internal unsafe void AllocateInBatch(int targetBatchIndex, ConstraintHandle cons ref var handlesSet = ref batchReferencedHandles[targetBatchIndex]; for (int i = 0; i < dynamicBodyHandles.Length; ++i) { - Debug.Assert(targetBatchIndex == FallbackBatchThreshold || !handlesSet.Contains(dynamicBodyHandles[i].Value), "Non-fallback batches should not come to include references to the same body more than once."); + Debug.Assert(targetBatchIndex == FallbackBatchThreshold || !handlesSet.Contains(dynamicBodyHandles[i].Value), "Non-fallback batches should not come to include references to the same dynamic body more than once."); handlesSet.Set(dynamicBodyHandles[i].Value, pool); } if (targetBatchIndex == FallbackBatchThreshold) @@ -1509,77 +1509,130 @@ public unsafe float GetAccumulatedImpulseMagnitude(ConstraintHandle constraintHa return (float)Math.Sqrt(GetAccumulatedImpulseMagnitudeSquared(constraintHandle)); } + internal interface IConstraintReferenceReportType { } + internal struct ReportEncodedReferences : IConstraintReferenceReportType { } + internal struct ReportDecodedReferences : IConstraintReferenceReportType { } + internal struct ReportDecodedDynamicReferences : IConstraintReferenceReportType { } + + /// + /// Enumerates body references in the constraint. Reports data according to the TReportType. + /// + /// Type of the enumerator called for each body index in the constraint. + /// Type of information to report to the enumerator. + /// Type batch containing the constraint to enumerate. + /// Index of the constraint to enumerate in the type batch. + /// Enumerator to call for each connected body reference. + internal unsafe void EnumerateConnectedBodyReferences(ref TypeBatch typeBatch, int indexInTypeBatch, ref TEnumerator enumerator) where TEnumerator : IForEach where TReportType : unmanaged, IConstraintReferenceReportType + { + var bodiesPerConstraint = TypeProcessors[typeBatch.TypeId].BodiesPerConstraint; + //Type batches store body references in AOSOA format, with one Vector for each constraint body reference in sequence, tightly packed. + //We can extract directly from memory. + var bytesPerBundle = bodiesPerConstraint * Unsafe.SizeOf>(); + BundleIndexing.GetBundleIndices(indexInTypeBatch, out var bundleIndex, out var innerIndex); + Debug.Assert(bytesPerBundle * typeBatch.BundleCount <= typeBatch.BodyReferences.Length, "Buffer must be large enough to hold the bundles of our assumed size. If this fails, an important assumption has been invalidated somewhere."); + var startByte = bundleIndex * bytesPerBundle + innerIndex * 4; + for (int i = 0; i < bodiesPerConstraint; ++i) + { + var raw = *(int*)(typeBatch.BodyReferences.Memory + startByte + i * Unsafe.SizeOf>()); + if (typeof(TReportType) == typeof(ReportEncodedReferences)) + { + enumerator.LoopBody(raw); + } + else if (typeof(TReportType) == typeof(ReportDecodedReferences)) + { + enumerator.LoopBody(raw & Bodies.BodyReferenceMask); + } + else if (typeof(TReportType) == typeof(ReportDecodedDynamicReferences)) + { + if ((raw & Bodies.KinematicMask) == 0) + enumerator.LoopBody(raw & Bodies.BodyReferenceMask); + } + } + } + /// + /// Enumerates the set of body references associated with a constraint in order of their references within the constraint. + /// This will report the raw body reference (body index if awake, handle if asleep) and any encoded metadata, like whether the body is kinematic. + /// + /// Type of the enumerator to call on each connected body reference. + /// Type batch containing the constraint to enumerate. + /// Index of the constraint to enumerate in the type batch. + /// Enumerator to call for each connected body reference. + public unsafe void EnumerateConnectedRawBodyReferences(ref TypeBatch typeBatch, int indexInTypeBatch, ref TEnumerator enumerator) where TEnumerator : IForEach + { + EnumerateConnectedBodyReferences(ref typeBatch, indexInTypeBatch, ref enumerator); + } + /// /// Enumerates the set of body references associated with a constraint in order of their references within the constraint. /// This will report the raw body reference (body index if awake, handle if asleep) and any encoded metadata, like whether the body is kinematic. /// + /// Type of the enumerator to call on each connected body reference. /// Constraint to enumerate. - /// Enumerator to use. - internal void EnumerateConnectedRawBodyReferences(ConstraintHandle constraintHandle, ref TEnumerator enumerator) where TEnumerator : IForEach + /// Enumerator to call for each connected body reference. + public unsafe void EnumerateConnectedRawBodyReferences(ConstraintHandle constraintHandle, ref TEnumerator enumerator) where TEnumerator : IForEach { ref var constraintLocation = ref HandleToConstraint[constraintHandle.Value]; ref var typeBatch = ref Sets[constraintLocation.SetIndex].Batches[constraintLocation.BatchIndex].GetTypeBatch(constraintLocation.TypeId); Debug.Assert(constraintLocation.IndexInTypeBatch >= 0 && constraintLocation.IndexInTypeBatch < typeBatch.ConstraintCount, "Bad constraint location; likely some add/remove bug."); - TypeProcessors[constraintLocation.TypeId].EnumerateConnectedRawBodyReferences(ref typeBatch, constraintLocation.IndexInTypeBatch, ref enumerator); + EnumerateConnectedBodyReferences(ref typeBatch, constraintLocation.IndexInTypeBatch, ref enumerator); } - struct ActiveBodyIndexEnumerator : IForEach where TWrapped : IForEach - { - public TWrapped Wrapped; - public void LoopBody(int encodedBodyIndex) - { - //This is only used on the active set, so we know we're getting body indices. They include encoded metadata that we can check to see whether the body is kinematic. - Wrapped.LoopBody(encodedBodyIndex & Bodies.BodyReferenceMask); - } + /// + /// Enumerates the set of body references associated with an active constraint in order of their references within the constraint. + /// This will report the body reference (body index if awake, handle if asleep) without any encoded kinematicity metadata. + /// + /// Type of the enumerator to call on each connected body reference. + /// Type batch containing the constraint to enumerate. + /// Index of the constraint to enumerate in the type batch. + /// Enumerator to call for each connected body reference. + public unsafe void EnumerateConnectedBodyReferences(ref TypeBatch typeBatch, int indexInTypeBatch, ref TEnumerator enumerator) where TEnumerator : IForEach + { + EnumerateConnectedBodyReferences(ref typeBatch, indexInTypeBatch, ref enumerator); } /// - /// Enumerates the set of body indices associated with an active constraint in order of their references within the constraint. + /// Enumerates the set of body references associated with an active constraint in order of their references within the constraint. + /// This will report the body reference (body index if awake, handle if asleep) without any encoded kinematicity metadata. /// + /// Type of the enumerator to call on each connected body reference. /// Constraint to enumerate. - /// Enumerator to use. - internal void EnumerateActiveConnectedBodyIndices(ConstraintHandle constraintHandle, ref TEnumerator enumerator) where TEnumerator : IForEach + /// Enumerator to call for each connected body reference. + public unsafe void EnumerateConnectedBodyReferences(ConstraintHandle constraintHandle, ref TEnumerator enumerator) where TEnumerator : IForEach { ref var constraintLocation = ref HandleToConstraint[constraintHandle.Value]; - Debug.Assert(constraintLocation.SetIndex == 0, "This function is intended to be used only with waking constraints."); ref var typeBatch = ref Sets[constraintLocation.SetIndex].Batches[constraintLocation.BatchIndex].GetTypeBatch(constraintLocation.TypeId); Debug.Assert(constraintLocation.IndexInTypeBatch >= 0 && constraintLocation.IndexInTypeBatch < typeBatch.ConstraintCount, "Bad constraint location; likely some add/remove bug."); - ActiveBodyIndexEnumerator wrapper; - wrapper.Wrapped = enumerator; - TypeProcessors[constraintLocation.TypeId].EnumerateConnectedRawBodyReferences(ref typeBatch, constraintLocation.IndexInTypeBatch, ref wrapper); - //Enumeration might have mutated the enumerator; if it's a value type, we need to copy those mutations back into it. - enumerator = wrapper.Wrapped; + EnumerateConnectedBodyReferences(ref typeBatch, constraintLocation.IndexInTypeBatch, ref enumerator); } - struct ActiveDynamicEnumerator : IForEach where TWrapped : IForEach + /// + /// Enumerates the set of dynamic body references associated with a constraint in order of their references within the constraint. + /// This will report the body reference (body index if awake, handle if asleep) without any encoded kinematicity metadata. + /// Kinematic references are skipped. + /// + /// Type of the enumerator to call on each connected dynamic body reference. + /// Type batch containing the constraint to enumerate. + /// Index of the constraint to enumerate in the type batch. + /// Enumerator to call for each connected dynamic body reference. + public unsafe void EnumerateConnectedDynamicBodies(ref TypeBatch typeBatch, int indexInTypeBatch, ref TEnumerator enumerator) where TEnumerator : IForEach { - public TWrapped Wrapped; - public void LoopBody(int encodedBodyIndex) - { - //This is only used on the active set, so we know we're getting body indices. They include encoded metadata that we can check to see whether the body is kinematic. - if (encodedBodyIndex < Bodies.DynamicLimit) - { - Wrapped.LoopBody(encodedBodyIndex & Bodies.BodyReferenceMask); - } - } + EnumerateConnectedBodyReferences(ref typeBatch, indexInTypeBatch, ref enumerator); } /// - /// Enumerates the set of dynamic body indices associated with an active constraint in order of their references within the constraint. + /// Enumerates the set of dynamic body references associated with a constraint in order of their references within the constraint. + /// This will report the body reference (body index if awake, handle if asleep) without any encoded kinematicity metadata. + /// Kinematic references are skipped. /// + /// Type of the enumerator to call on each connected dynamic body reference. /// Constraint to enumerate. - /// Enumerator to use. - internal void EnumerateActiveDynamicConnectedBodyIndices(ConstraintHandle constraintHandle, ref TEnumerator enumerator) where TEnumerator : IForEach + /// Enumerator to call for each connected dynamic body reference. + public unsafe void EnumerateConnectedDynamicBodies(ConstraintHandle constraintHandle, ref TEnumerator enumerator) where TEnumerator : IForEach { ref var constraintLocation = ref HandleToConstraint[constraintHandle.Value]; - Debug.Assert(constraintLocation.SetIndex == 0, "This function is intended to be used only with waking constraints."); ref var typeBatch = ref Sets[constraintLocation.SetIndex].Batches[constraintLocation.BatchIndex].GetTypeBatch(constraintLocation.TypeId); Debug.Assert(constraintLocation.IndexInTypeBatch >= 0 && constraintLocation.IndexInTypeBatch < typeBatch.ConstraintCount, "Bad constraint location; likely some add/remove bug."); - ActiveDynamicEnumerator wrapper; - wrapper.Wrapped = enumerator; - TypeProcessors[constraintLocation.TypeId].EnumerateConnectedRawBodyReferences(ref typeBatch, constraintLocation.IndexInTypeBatch, ref wrapper); - //Enumeration might have mutated the enumerator; if it's a value type, we need to copy those mutations back into it. - enumerator = wrapper.Wrapped; + EnumerateConnectedBodyReferences(ref typeBatch, constraintLocation.IndexInTypeBatch, ref enumerator); } internal void GetSynchronizedBatchCount(out int synchronizedBatchCount, out bool fallbackExists) diff --git a/Demos/SpecializedTests/SimulationScrambling.cs b/Demos/SpecializedTests/SimulationScrambling.cs index 4f757a3b9..6ed0f6f7b 100644 --- a/Demos/SpecializedTests/SimulationScrambling.cs +++ b/Demos/SpecializedTests/SimulationScrambling.cs @@ -159,7 +159,7 @@ static void Validate(Simulation simulation, List removedConstraints, List(Simulation simulation, int iterations, Bo simulation.Solver.GetConstraintReference(constraintHandle, out var reference); var bodyIdentityEnumerator = new BodyEnumerator(simulation.Bodies, bodyHandlesToIdentity); - simulation.Solver.TypeProcessors[reference.TypeBatch.TypeId].EnumerateConnectedRawBodyReferences(ref reference.TypeBatch, reference.IndexInTypeBatch, ref bodyIdentityEnumerator); + simulation.Solver.EnumerateConnectedRawBodyReferences(ref reference.TypeBatch, reference.IndexInTypeBatch, ref bodyIdentityEnumerator); constraintDescriptions[i].BodyA = bodyIdentityEnumerator.IdentityA; constraintDescriptions[i].BodyB = bodyIdentityEnumerator.IdentityB; } From 3de78560801e8002c1a3d4e6d4610aaef7dbffa0 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 7 Nov 2021 16:21:15 -0600 Subject: [PATCH 269/947] Fixed some potentially incorrect dynamic state tests. Made GetConstraintReference return instead of out. Kinematic state changing is moving forward, but still quite a few gaps around fallback batch management. --- BepuPhysics/Bodies.cs | 74 +++++++++++++++---- BepuPhysics/Bodies_GatherScatter.cs | 4 +- .../NarrowPhaseConstraintUpdate.cs | 2 +- BepuPhysics/CollisionDetection/PairCache.cs | 2 +- BepuPhysics/ConstraintBatch.cs | 2 +- BepuPhysics/HandyEnumerators.cs | 2 +- BepuPhysics/SequentialFallbackBatch.cs | 3 +- BepuPhysics/Solver.cs | 55 ++++++++++++-- .../Solver_IncrementalContactUpdate.cs | 1 - .../SpecializedTests/SimulationScrambling.cs | 2 +- 10 files changed, 117 insertions(+), 30 deletions(-) diff --git a/BepuPhysics/Bodies.cs b/BepuPhysics/Bodies.cs index a18d628ea..0c4153040 100644 --- a/BepuPhysics/Bodies.cs +++ b/BepuPhysics/Bodies.cs @@ -352,16 +352,27 @@ public unsafe static bool HasLockedInertia(Symmetric3x3* inertia) } } - private struct ConnectedDynamicCounter : IForEach + private struct DynamicToKinematicEnumerator : IForEach { public int DynamicCount; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void LoopBody(int bodyIndex) + public unsafe void LoopBody(int encodedBodyReference) { - //The enumerator is only called if the body is dynamic, so just increment. ++DynamicCount; } } + private unsafe struct KinematicToDynamicEnumerator : IForEach + { + public const int MaximumBodiesPerConstraint = 4; + public int* DynamicBodyIndices; + public int DynamicCount; + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public unsafe void LoopBody(int encodedBodyReference) + { + Debug.Assert(DynamicCount < MaximumBodiesPerConstraint, "We assumed that the number of bodies per constraint was limited; if this assumption fails, it could cause a stack overrun."); + DynamicBodyIndices[DynamicCount++] = encodedBodyReference; + } + } [MethodImpl(MethodImplOptions.AggressiveInlining)] unsafe void UpdateForKinematicStateChange(BodyHandle handle, ref BodyMemoryLocation location, ref BodySet set, bool previouslyKinematic, bool currentlyKinematic) @@ -389,10 +400,11 @@ unsafe void UpdateForKinematicStateChange(BodyHandle handle, ref BodyMemoryLocat //Any constraints that connect only kinematic bodies together should be removed; they'll NaN out. //Ideally, the user would handle this for all non-contact constraints, but it would be rather annoying to //have to explicitly enumerate and remove all contact constraints any time you wanted to make a body kinematic. - ConnectedDynamicCounter enumerator; + DynamicToKinematicEnumerator enumerator; for (int i = 0; i < constraints.Count; ++i) { ref var constraint = ref constraints[i]; + solver.UpdateReferenceForBodyBecomingKinematic(constraint.ConnectingConstraintHandle, constraint.BodyIndexInConstraint); enumerator.DynamicCount = 0; solver.EnumerateConnectedDynamicBodies(constraint.ConnectingConstraintHandle, ref enumerator); if (enumerator.DynamicCount == 0) @@ -406,19 +418,54 @@ unsafe void UpdateForKinematicStateChange(BodyHandle handle, ref BodyMemoryLocat { //A kinematic body has become dynamic. Kinematic bodies do not block membership in constraint batches, dynamic bodies do. //For any constraint connected to the new dynamic, ensure that it belongs to a constraint batch not shared by any other constraints connected to the same body. - const int maximumBodiesPerConstraint = 4; - int* handles = stackalloc int[maximumBodiesPerConstraint]; - ActiveConstraintDynamicBodyHandleCollector enumerator = new(this, handles); - for (int i = 0; i < constraints.Count; ++i) + int* dynamicBodyIndices = stackalloc int[KinematicToDynamicEnumerator.MaximumBodiesPerConstraint]; + int* dynamicBodyHandles = stackalloc int[KinematicToDynamicEnumerator.MaximumBodiesPerConstraint]; + KinematicToDynamicEnumerator enumerator; + enumerator.DynamicBodyIndices = dynamicBodyIndices; + enumerator.DynamicCount = 0; + var indexToHandle = ActiveSet.IndexToHandle; + var handleToConstraint = solver.HandleToConstraint; + for (int constraintIndex = 0; constraintIndex < constraints.Count; ++constraintIndex) { - ref var constraint = ref constraints[i]; - enumerator.Count = 0; + ref var constraint = ref constraints[constraintIndex]; + enumerator.DynamicCount = 0; solver.EnumerateConnectedDynamicBodies(constraint.ConnectingConstraintHandle, ref enumerator); - if (enumerator.Count == 0) + for (int indexInDynamics = 0; indexInDynamics < enumerator.DynamicCount; ++indexInDynamics) { - //This constraint connects only kinematic bodies; keeping it in the solver would cause a singularity. - solver.Remove(constraint.ConnectingConstraintHandle); + dynamicBodyHandles[indexInDynamics] = indexToHandle[dynamicBodyIndices[indexInDynamics]].Value; + } + //Since we haven't updated the constraint reference to this body's kinematicity yet, it was not included in the dynamicBodyHandles. + //Include it here. + dynamicBodyHandles[enumerator.DynamicCount++] = handle.Value; + var dynamicBodyHandlesSpan = new Span(enumerator.DynamicBodyIndices, enumerator.DynamicCount); + solver.GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); + var constraintLocation = handleToConstraint[constraint.ConnectingConstraintHandle.Value]; + bool foundBatch = false; + ref var batch = ref solver.ActiveSet.Batches[constraintLocation.BatchIndex]; + ref var typeBatch = ref batch.TypeBatches[batch.TypeIndexToTypeBatchIndex[constraintLocation.TypeId]]; + for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) + { + if (solver.batchReferencedHandles[batchIndex].CanFit(dynamicBodyHandlesSpan)) + { + //Because we haven't removed the constraint from the simulation, it's currently blocking the constraint batch it previously lived in. + //It must have had at least one other dynamic before (otherwise it would have violated the 'constraints must have at least one dynamic in them' rule), so that body will block it. + //This causes a little bit of batch inefficiency, but the batch compressor will take care of it eventually and this codepath is reasonably rare- the simplicity of reusing TransferConstraint wins. + Debug.Assert(batchIndex != constraintLocation.BatchIndex, "It should not be possible for a newly dynamic reference to insert itself into the same batch it was in while kinematic."); + solver.TypeProcessors[constraintLocation.TypeId].TransferConstraint(ref typeBatch, constraintLocation.BatchIndex, constraintLocation.IndexInTypeBatch, solver, this, batchIndex); + foundBatch = true; + break; + } + } + if (!foundBatch && fallbackExists && constraintLocation.BatchIndex != solver.FallbackBatchThreshold) + { + //The fallback batch does not block new constraints regardless of what constraints are connected, so it's safe to add. + solver.TypeProcessors[constraintLocation.TypeId].TransferConstraint(ref typeBatch, constraintLocation.BatchIndex, constraintLocation.IndexInTypeBatch, solver, this, solver.FallbackBatchThreshold); + foundBatch = true; } + //Note that we wait until the transfer completes to update the body reference in the constraint. + //This ensures the transfer process still views the constraint as kinematic and doesn't try to remove handles from batch referenced handles that aren't present. + solver.UpdateReferenceForBodyBecomingDynamic(constraint.ConnectingConstraintHandle, constraint.BodyIndexInConstraint); + } } } @@ -705,7 +752,6 @@ internal void EnumerateConnectedBodyIndices(int activeBodyIndex, re /// Type of the enumerator to execute on each connected body. /// Handle of the body to enumerate the connections of. This body will not appear in the set of enumerated bodies, even if it is connected to itself somehow. /// Enumerator instance to run on each connected body. - /// Solver from which to pull constraint body references. [MethodImpl(MethodImplOptions.AggressiveInlining)] public void EnumerateConnectedBodies(BodyHandle bodyHandle, ref TEnumerator enumerator) where TEnumerator : IForEach { diff --git a/BepuPhysics/Bodies_GatherScatter.cs b/BepuPhysics/Bodies_GatherScatter.cs index 7023ee935..056178bde 100644 --- a/BepuPhysics/Bodies_GatherScatter.cs +++ b/BepuPhysics/Bodies_GatherScatter.cs @@ -162,7 +162,7 @@ unsafe static void FallbackGatherInertia(SolverState* states, Vector baseIn [MethodImpl(MethodImplOptions.AggressiveInlining)] public static bool IsEncodedDynamicReference(int encodedBodyReferenceValue) { - return (encodedBodyReferenceValue & KinematicMask) == 0; + return (uint)encodedBodyReferenceValue < DynamicLimit; } /// /// Checks whether a constraint encoded body reference value refers to a kinematic body. @@ -172,7 +172,7 @@ public static bool IsEncodedDynamicReference(int encodedBodyReferenceValue) [MethodImpl(MethodImplOptions.AggressiveInlining)] public static bool IsEncodedKinematicReference(int encodedBodyReferenceValue) { - return (encodedBodyReferenceValue & KinematicMask) > 0; + return (uint)encodedBodyReferenceValue >= DynamicLimit; } //[MethodImpl(MethodImplOptions.AggressiveInlining)] diff --git a/BepuPhysics/CollisionDetection/NarrowPhaseConstraintUpdate.cs b/BepuPhysics/CollisionDetection/NarrowPhaseConstraintUpdate.cs index 722ae15f8..dd0b8ea48 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhaseConstraintUpdate.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhaseConstraintUpdate.cs @@ -176,7 +176,7 @@ public unsafe void UpdateConstraint= 0 && diff --git a/BepuPhysics/CollisionDetection/PairCache.cs b/BepuPhysics/CollisionDetection/PairCache.cs index d6d0f11ab..085d6dacc 100644 --- a/BepuPhysics/CollisionDetection/PairCache.cs +++ b/BepuPhysics/CollisionDetection/PairCache.cs @@ -416,7 +416,7 @@ internal unsafe void CompleteConstraintAdd(NarrowPhase narrowP //Note that the update is being directed to the *next* worker caches. We have not yet performed the flush that swaps references. //Note that this assumes that the constraint handle is stored in the first 4 bytes of the constraint cache. *(ConstraintHandle*)NextWorkerCaches[constraintCacheIndex.Cache].GetConstraintCachePointer(constraintCacheIndex) = constraintHandle; - solver.GetConstraintReference(constraintHandle, out var reference); + var reference = solver.GetConstraintReference(constraintHandle); Debug.Assert(reference.IndexInTypeBatch >= 0 && reference.IndexInTypeBatch < reference.TypeBatch.ConstraintCount); narrowPhase.contactConstraintAccessors[reference.TypeBatch.TypeId].ScatterNewImpulses(ref reference, ref impulses); //This mapping entry had to be deferred until now because no constraint handle was known until now. Now that we have it, diff --git a/BepuPhysics/ConstraintBatch.cs b/BepuPhysics/ConstraintBatch.cs index 2cba627c2..1cac82072 100644 --- a/BepuPhysics/ConstraintBatch.cs +++ b/BepuPhysics/ConstraintBatch.cs @@ -158,7 +158,7 @@ public ActiveBodyHandleRemover(Bodies bodies, IndexSet* handles) [MethodImpl(MethodImplOptions.AggressiveInlining)] public void LoopBody(int encodedBodyIndex) { - if (encodedBodyIndex < Bodies.DynamicLimit) + if (Bodies.IsEncodedDynamicReference(encodedBodyIndex)) { Handles->Remove(Bodies.ActiveSet.IndexToHandle[encodedBodyIndex & Bodies.BodyReferenceMask].Value); } diff --git a/BepuPhysics/HandyEnumerators.cs b/BepuPhysics/HandyEnumerators.cs index 853b832ce..72a71fa1d 100644 --- a/BepuPhysics/HandyEnumerators.cs +++ b/BepuPhysics/HandyEnumerators.cs @@ -49,7 +49,7 @@ public ActiveConstraintDynamicBodyHandleCollector(Bodies bodies, int* handles) public void LoopBody(int encodedBodyIndex) { - if (encodedBodyIndex < Bodies.DynamicLimit) + if (Bodies.IsEncodedDynamicReference(encodedBodyIndex)) { //Note that this enumerator is used with prefiltered body indices and with raw body indices. A redundant & isn't much of a concern; lets us share more frequently. Handles[Count++] = Bodies.ActiveSet.IndexToHandle[encodedBodyIndex & Bodies.BodyReferenceMask].Value; diff --git a/BepuPhysics/SequentialFallbackBatch.cs b/BepuPhysics/SequentialFallbackBatch.cs index 13ab1455d..04cf66259 100644 --- a/BepuPhysics/SequentialFallbackBatch.cs +++ b/BepuPhysics/SequentialFallbackBatch.cs @@ -161,8 +161,7 @@ internal unsafe void Remove(Solver solver, BufferPool bufferPool, ref Constraint for (int i = 0; i < bodyCount; ++i) { var rawBodyIndex = bodyIndices[i]; - var isDynamic = (rawBodyIndex & Bodies.KinematicMask) == 0; - if (isDynamic) + if (Bodies.IsEncodedDynamicReference(rawBodyIndex)) { var bodyIndex = rawBodyIndex & Bodies.BodyReferenceMask; if (Remove(bodyIndex, ref allocationIdsToFree)) diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index 7610bfce8..fe93c6bdc 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -12,9 +12,15 @@ namespace BepuPhysics { + /// + /// Reference to a constraint's memory location in the solver. + /// public unsafe struct ConstraintReference { internal TypeBatch* typeBatchPointer; + /// + /// Gets a reference to the type batch holding the constraint. + /// public ref TypeBatch TypeBatch { [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -23,8 +29,16 @@ public ref TypeBatch TypeBatch return ref *typeBatchPointer; } } + /// + /// Index in the type batch where the constraint is allocated. + /// public readonly int IndexInTypeBatch; + /// + /// Creates a new constraint reference from a constraint memory location. + /// + /// Pointer to the type batch where the constraint lives. + /// Index in the type batch where the constraint is allocated. [MethodImpl(MethodImplOptions.AggressiveInlining)] public ConstraintReference(TypeBatch* typeBatchPointer, int indexInTypeBatch) { @@ -291,14 +305,14 @@ public bool ConstraintExists(ConstraintHandle constraintHandle) /// The reference is temporary; any constraint removals that affect the referenced type batch may invalidate the index. /// /// Handle index of the constraint. - /// Temporary direct reference to the type batch and index in the type batch associated with the constraint handle. - /// May be invalidated by constraint removals. + /// Temporary direct reference to the type batch and index in the type batch associated with the constraint handle. + /// May be invalidated by constraint removals. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void GetConstraintReference(ConstraintHandle handle, out ConstraintReference reference) + public unsafe ConstraintReference GetConstraintReference(ConstraintHandle handle) { AssertConstraintHandleExists(handle); ref var constraintLocation = ref HandleToConstraint[handle.Value]; - reference = new ConstraintReference(Sets[constraintLocation.SetIndex].Batches[constraintLocation.BatchIndex].GetTypeBatchPointer(constraintLocation.TypeId), constraintLocation.IndexInTypeBatch); + return new ConstraintReference(Sets[constraintLocation.SetIndex].Batches[constraintLocation.BatchIndex].GetTypeBatchPointer(constraintLocation.TypeId), constraintLocation.IndexInTypeBatch); } [Conditional("DEBUG")] @@ -1086,7 +1100,7 @@ public unsafe void ApplyDescriptionWithoutWaking(in ConstraintRefe public void ApplyDescriptionWithoutWaking(ConstraintHandle constraintHandle, in TDescription description) where TDescription : unmanaged, IConstraintDescription { - GetConstraintReference(constraintHandle, out var constraintReference); + var constraintReference = GetConstraintReference(constraintHandle); ApplyDescriptionWithoutWaking(constraintReference, description); } @@ -1509,6 +1523,35 @@ public unsafe float GetAccumulatedImpulseMagnitude(ConstraintHandle constraintHa return (float)Math.Sqrt(GetAccumulatedImpulseMagnitudeSquared(constraintHandle)); } + internal interface IConstraintBodyReferenceMutationType { } + internal struct BecomingKinematic : IConstraintBodyReferenceMutationType { } + internal struct BecomingDynamic : IConstraintBodyReferenceMutationType { } + unsafe void UpdateReferenceForBodyKinematicChange(ConstraintHandle connectingConstraintHandle, int bodyIndexInConstraint) where TMutationType : unmanaged, IConstraintBodyReferenceMutationType + { + var reference = GetConstraintReference(connectingConstraintHandle); + var bodiesPerConstraint = TypeProcessors[reference.TypeBatch.TypeId].BodiesPerConstraint; + var intsPerBundle = Vector.Count * bodiesPerConstraint; + BundleIndexing.GetBundleIndices(reference.IndexInTypeBatch, out var bundleIndex, out var innerIndex); + var firstBodyReference = (uint*)reference.TypeBatch.BodyReferences.Memory + intsPerBundle * bundleIndex + innerIndex; + if (typeof(TMutationType) == typeof(BecomingKinematic)) + firstBodyReference[bodyIndexInConstraint * Vector.Count] |= Bodies.KinematicMask; + else if (typeof(TMutationType) == typeof(BecomingDynamic)) + firstBodyReference[bodyIndexInConstraint * Vector.Count] &= Bodies.BodyReferenceMask; + else + Debug.Fail("Hey, that's not a valid type!"); + + } + + internal unsafe void UpdateReferenceForBodyBecomingKinematic(ConstraintHandle connectingConstraintHandle, int bodyIndexInConstraint) + { + UpdateReferenceForBodyKinematicChange(connectingConstraintHandle, bodyIndexInConstraint); + } + + internal void UpdateReferenceForBodyBecomingDynamic(ConstraintHandle connectingConstraintHandle, int bodyIndexInConstraint) + { + UpdateReferenceForBodyKinematicChange(connectingConstraintHandle, bodyIndexInConstraint); + } + internal interface IConstraintReferenceReportType { } internal struct ReportEncodedReferences : IConstraintReferenceReportType { } internal struct ReportDecodedReferences : IConstraintReferenceReportType { } @@ -1544,7 +1587,7 @@ internal unsafe void EnumerateConnectedBodyReferences( } else if (typeof(TReportType) == typeof(ReportDecodedDynamicReferences)) { - if ((raw & Bodies.KinematicMask) == 0) + if (Bodies.IsEncodedDynamicReference(raw)) enumerator.LoopBody(raw & Bodies.BodyReferenceMask); } } diff --git a/BepuPhysics/Solver_IncrementalContactUpdate.cs b/BepuPhysics/Solver_IncrementalContactUpdate.cs index 36683cf56..df771c121 100644 --- a/BepuPhysics/Solver_IncrementalContactUpdate.cs +++ b/BepuPhysics/Solver_IncrementalContactUpdate.cs @@ -79,6 +79,5 @@ internal void IncrementallyUpdateContactConstraints(float dt, IThreadDispatcher ExecuteMultithreaded(dt, threadDispatcher, incrementalContactUpdateWorker); } } - } } diff --git a/Demos/SpecializedTests/SimulationScrambling.cs b/Demos/SpecializedTests/SimulationScrambling.cs index 6ed0f6f7b..b901c5b35 100644 --- a/Demos/SpecializedTests/SimulationScrambling.cs +++ b/Demos/SpecializedTests/SimulationScrambling.cs @@ -317,7 +317,7 @@ public static double AddRemoveChurn(Simulation simulation, int iterations, Bo var constraintHandle = constraintHandles[i]; constraintHandlesToIdentity[constraintHandle.Value] = i; simulation.Solver.GetDescription(constraintHandle, out constraintDescriptions[i].Description); - simulation.Solver.GetConstraintReference(constraintHandle, out var reference); + var reference = simulation.Solver.GetConstraintReference(constraintHandle); var bodyIdentityEnumerator = new BodyEnumerator(simulation.Bodies, bodyHandlesToIdentity); simulation.Solver.EnumerateConnectedRawBodyReferences(ref reference.TypeBatch, reference.IndexInTypeBatch, ref bodyIdentityEnumerator); From 5617c333537088c34a3966c9c675dec7c08aca37 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 7 Nov 2021 16:54:18 -0600 Subject: [PATCH 270/947] TransferConstraint now natively takes the dynamic handles and encoded body indices from the caller, letting them differ from what is stored. --- BepuPhysics/Bodies.cs | 38 ++++++--- BepuPhysics/Constraints/TypeProcessor.cs | 104 +++++++++++++---------- 2 files changed, 84 insertions(+), 58 deletions(-) diff --git a/BepuPhysics/Bodies.cs b/BepuPhysics/Bodies.cs index 0c4153040..cbee76f46 100644 --- a/BepuPhysics/Bodies.cs +++ b/BepuPhysics/Bodies.cs @@ -364,13 +364,22 @@ public unsafe void LoopBody(int encodedBodyReference) private unsafe struct KinematicToDynamicEnumerator : IForEach { public const int MaximumBodiesPerConstraint = 4; - public int* DynamicBodyIndices; + + public Buffer IndexToHandle; + public int* DynamicBodyHandles; public int DynamicCount; + public int* EncodedBodyIndices; + public int EncodedCount; + [MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe void LoopBody(int encodedBodyReference) { - Debug.Assert(DynamicCount < MaximumBodiesPerConstraint, "We assumed that the number of bodies per constraint was limited; if this assumption fails, it could cause a stack overrun."); - DynamicBodyIndices[DynamicCount++] = encodedBodyReference; + Debug.Assert(EncodedCount < MaximumBodiesPerConstraint, "We assumed that the number of bodies per constraint was limited; if this assumption fails, it could cause a stack overrun."); + if (IsEncodedDynamicReference(encodedBodyReference)) + { + DynamicBodyHandles[DynamicCount++] = IndexToHandle[encodedBodyReference].Value; + } + EncodedBodyIndices[EncodedCount++] = encodedBodyReference; } } @@ -418,26 +427,25 @@ unsafe void UpdateForKinematicStateChange(BodyHandle handle, ref BodyMemoryLocat { //A kinematic body has become dynamic. Kinematic bodies do not block membership in constraint batches, dynamic bodies do. //For any constraint connected to the new dynamic, ensure that it belongs to a constraint batch not shared by any other constraints connected to the same body. - int* dynamicBodyIndices = stackalloc int[KinematicToDynamicEnumerator.MaximumBodiesPerConstraint]; int* dynamicBodyHandles = stackalloc int[KinematicToDynamicEnumerator.MaximumBodiesPerConstraint]; + int* encodedBodyIndices = stackalloc int[KinematicToDynamicEnumerator.MaximumBodiesPerConstraint]; KinematicToDynamicEnumerator enumerator; - enumerator.DynamicBodyIndices = dynamicBodyIndices; - enumerator.DynamicCount = 0; + enumerator.IndexToHandle = ActiveSet.IndexToHandle; + enumerator.DynamicBodyHandles = dynamicBodyHandles; + enumerator.EncodedBodyIndices = dynamicBodyHandles; var indexToHandle = ActiveSet.IndexToHandle; var handleToConstraint = solver.HandleToConstraint; for (int constraintIndex = 0; constraintIndex < constraints.Count; ++constraintIndex) { ref var constraint = ref constraints[constraintIndex]; enumerator.DynamicCount = 0; - solver.EnumerateConnectedDynamicBodies(constraint.ConnectingConstraintHandle, ref enumerator); - for (int indexInDynamics = 0; indexInDynamics < enumerator.DynamicCount; ++indexInDynamics) - { - dynamicBodyHandles[indexInDynamics] = indexToHandle[dynamicBodyIndices[indexInDynamics]].Value; - } + enumerator.EncodedCount = 0; + solver.EnumerateConnectedRawBodyReferences(constraint.ConnectingConstraintHandle, ref enumerator); //Since we haven't updated the constraint reference to this body's kinematicity yet, it was not included in the dynamicBodyHandles. //Include it here. dynamicBodyHandles[enumerator.DynamicCount++] = handle.Value; - var dynamicBodyHandlesSpan = new Span(enumerator.DynamicBodyIndices, enumerator.DynamicCount); + var dynamicBodyHandlesSpan = new Span(dynamicBodyHandles, enumerator.DynamicCount); + var encodedBodyIndicesSpan = new Span(encodedBodyIndices, enumerator.DynamicCount); solver.GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); var constraintLocation = handleToConstraint[constraint.ConnectingConstraintHandle.Value]; bool foundBatch = false; @@ -451,7 +459,8 @@ unsafe void UpdateForKinematicStateChange(BodyHandle handle, ref BodyMemoryLocat //It must have had at least one other dynamic before (otherwise it would have violated the 'constraints must have at least one dynamic in them' rule), so that body will block it. //This causes a little bit of batch inefficiency, but the batch compressor will take care of it eventually and this codepath is reasonably rare- the simplicity of reusing TransferConstraint wins. Debug.Assert(batchIndex != constraintLocation.BatchIndex, "It should not be possible for a newly dynamic reference to insert itself into the same batch it was in while kinematic."); - solver.TypeProcessors[constraintLocation.TypeId].TransferConstraint(ref typeBatch, constraintLocation.BatchIndex, constraintLocation.IndexInTypeBatch, solver, this, batchIndex); + solver.TypeProcessors[constraintLocation.TypeId].TransferConstraint(ref typeBatch, constraintLocation.BatchIndex, constraintLocation.IndexInTypeBatch, solver, this, batchIndex, + new Span(dynamicBodyHandles, enumerator.DynamicCount), encodedBodyIndicesSpan); foundBatch = true; break; } @@ -459,7 +468,8 @@ unsafe void UpdateForKinematicStateChange(BodyHandle handle, ref BodyMemoryLocat if (!foundBatch && fallbackExists && constraintLocation.BatchIndex != solver.FallbackBatchThreshold) { //The fallback batch does not block new constraints regardless of what constraints are connected, so it's safe to add. - solver.TypeProcessors[constraintLocation.TypeId].TransferConstraint(ref typeBatch, constraintLocation.BatchIndex, constraintLocation.IndexInTypeBatch, solver, this, solver.FallbackBatchThreshold); + solver.TypeProcessors[constraintLocation.TypeId].TransferConstraint(ref typeBatch, constraintLocation.BatchIndex, constraintLocation.IndexInTypeBatch, solver, this, solver.FallbackBatchThreshold, + new Span(dynamicBodyHandles, enumerator.DynamicCount), encodedBodyIndicesSpan); foundBatch = true; } //Note that we wait until the transfer completes to update the body reference in the constraint. diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index d196d1320..a99ebd0d7 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -69,16 +69,69 @@ public void Initialize(int typeId) public unsafe abstract int AllocateInTypeBatchForFallback(ref TypeBatch typeBatch, ConstraintHandle handle, Span encodedBodyIndices, BufferPool pool); public abstract void Remove(ref TypeBatch typeBatch, int index, ref Buffer handlesToConstraints, bool isFallback); + + /// + /// Collects body references from active constraints and converts them into properly flagged constraint kinematic body handles. + /// + unsafe struct ActiveKinematicFlaggedBodyHandleCollector : IForEach + { + public Bodies Bodies; + public int* DynamicBodyHandles; + public int DynamicCount; + public int* EncodedBodyIndices; + public int IndexCount; + + + public ActiveKinematicFlaggedBodyHandleCollector(Bodies bodies, int* dynamicHandles, int* encodedBodyIndices) + { + Bodies = bodies; + DynamicBodyHandles = dynamicHandles; + DynamicCount = 0; + EncodedBodyIndices = encodedBodyIndices; + IndexCount = 0; + } + + public void LoopBody(int encodedBodyIndex) + { + if (Bodies.IsEncodedDynamicReference(encodedBodyIndex)) + { + DynamicBodyHandles[DynamicCount++] = Bodies.ActiveSet.IndexToHandle[encodedBodyIndex].Value; + } + EncodedBodyIndices[IndexCount++] = encodedBodyIndex; + } + } + /// + /// Moves a constraint from one ConstraintBatch's TypeBatch to another ConstraintBatch's TypeBatch of the same type. + /// + /// Source type batch to transfer the constraint out of. + /// Index of the batch that owns the type batch that is the source of the constraint transfer. + /// Index of the constraint to move in the current type batch. + /// Solver that owns the batches. + /// Bodies set that owns all the constraint's bodies. + /// Index of the ConstraintBatch in the solver to copy the constraint into. + public unsafe void TransferConstraint(ref TypeBatch sourceTypeBatch, int sourceBatchIndex, int indexInTypeBatch, Solver solver, Bodies bodies, int targetBatchIndex) + { + int bodiesPerConstraint = InternalBodiesPerConstraint; + var dynamicBodyHandles = stackalloc int[bodiesPerConstraint]; + var encodedBodyIndices = stackalloc int[bodiesPerConstraint]; + var bodyHandleCollector = new ActiveKinematicFlaggedBodyHandleCollector(bodies, dynamicBodyHandles, encodedBodyIndices); + solver.EnumerateConnectedRawBodyReferences(ref sourceTypeBatch, indexInTypeBatch, ref bodyHandleCollector); + var constraintHandle = sourceTypeBatch.IndexToHandle[indexInTypeBatch]; + TransferConstraint(ref sourceTypeBatch, sourceBatchIndex, indexInTypeBatch, solver, bodies, targetBatchIndex, new Span(dynamicBodyHandles, bodyHandleCollector.DynamicCount), new Span(encodedBodyIndices, bodiesPerConstraint)); + } + /// /// Moves a constraint from one ConstraintBatch's TypeBatch to another ConstraintBatch's TypeBatch of the same type. /// - /// Source type batch from which the constraint will be taken. + /// Source type batch to transfer the constraint out of. /// Index of the batch that owns the type batch that is the source of the constraint transfer. /// Index of the constraint to move in the current type batch. /// Solver that owns the batches. /// Bodies set that owns all the constraint's bodies. /// Index of the ConstraintBatch in the solver to copy the constraint into. - public unsafe abstract void TransferConstraint(ref TypeBatch sourceTypeBatch, int sourceBatchIndex, int indexInTypeBatch, Solver solver, Bodies bodies, int targetBatchIndex); + /// Set of body handles in the constraint referring to dynamic bodies. + /// Set of encoded body indices to use in the new constraint allocation. + public unsafe abstract void TransferConstraint(ref TypeBatch sourceTypeBatch, int sourceBatchIndex, int indexInTypeBatch, Solver solver, Bodies bodies, int targetBatchIndex, Span dynamicBodyHandles, Span encodedBodyIndices); [Conditional("DEBUG")] protected abstract void ValidateAccumulatedImpulsesSizeInBytes(int sizeInBytes); @@ -678,37 +731,6 @@ ref Unsafe.Add(ref bodyReferences, targetBundleIndex), ref Unsafe.Add(ref preste } - /// - /// Collects body references from active constraints and converts them into properly flagged constraint kinematic body handles. - /// - unsafe struct ActiveKinematicFlaggedBodyHandleCollector : IForEach - { - public Bodies Bodies; - public int* DynamicBodyHandles; - public int DynamicCount; - public int* EncodedBodyIndices; - public int IndexCount; - - - public ActiveKinematicFlaggedBodyHandleCollector(Bodies bodies, int* dynamicHandles, int* encodedBodyIndices) - { - Bodies = bodies; - DynamicBodyHandles = dynamicHandles; - DynamicCount = 0; - EncodedBodyIndices = encodedBodyIndices; - IndexCount = 0; - } - - public void LoopBody(int encodedBodyIndex) - { - if (Bodies.IsEncodedDynamicReference(encodedBodyIndex)) - { - DynamicBodyHandles[DynamicCount++] = Bodies.ActiveSet.IndexToHandle[encodedBodyIndex].Value; - } - EncodedBodyIndices[IndexCount++] = encodedBodyIndex; - } - } - /// /// Moves a constraint from one ConstraintBatch's TypeBatch to another ConstraintBatch's TypeBatch of the same type. @@ -719,19 +741,13 @@ public void LoopBody(int encodedBodyIndex) /// Solver that owns the batches. /// Bodies set that owns all the constraint's bodies. /// Index of the ConstraintBatch in the solver to copy the constraint into. - public unsafe override void TransferConstraint(ref TypeBatch sourceTypeBatch, int sourceBatchIndex, int indexInTypeBatch, Solver solver, Bodies bodies, int targetBatchIndex) + /// Set of body handles in the constraint referring to dynamic bodies. + /// Set of encoded body indices to use in the new constraint allocation. + public unsafe override sealed void TransferConstraint(ref TypeBatch sourceTypeBatch, int sourceBatchIndex, int indexInTypeBatch, Solver solver, Bodies bodies, int targetBatchIndex, Span dynamicBodyHandles, Span encodedBodyIndices) { - //Note that the following does some redundant work. It's technically possible to do better than this, but it requires bypassing a lot of bookkeeping. - //It's not exactly trivial to keep everything straight, especially over time- it becomes a maintenance nightmare. - //So instead, given that compressions should generally be extremely rare (relatively speaking) and highly deferrable, we'll accept some minor overhead. - int bodiesPerConstraint = InternalBodiesPerConstraint; - var dynamicBodyHandles = stackalloc int[bodiesPerConstraint]; - var encodedBodyIndices = stackalloc int[bodiesPerConstraint]; - var bodyHandleCollector = new ActiveKinematicFlaggedBodyHandleCollector(bodies, dynamicBodyHandles, encodedBodyIndices); - solver.EnumerateConnectedRawBodyReferences(ref sourceTypeBatch, indexInTypeBatch, ref bodyHandleCollector); - //Allocate a spot in the new batch. Note that it does not change the Handle->Constraint mapping in the Solver; that's important when we call Solver.Remove below. var constraintHandle = sourceTypeBatch.IndexToHandle[indexInTypeBatch]; - solver.AllocateInBatch(targetBatchIndex, constraintHandle, new Span((BodyHandle*)dynamicBodyHandles, bodyHandleCollector.DynamicCount), new Span(encodedBodyIndices, bodiesPerConstraint), typeId, out var targetReference); + //Allocate a spot in the new batch. Note that it does not change the Handle->Constraint mapping in the Solver; that's important when we call Solver.Remove below. + solver.AllocateInBatch(targetBatchIndex, constraintHandle, dynamicBodyHandles, encodedBodyIndices, typeId, out var targetReference); BundleIndexing.GetBundleIndices(targetReference.IndexInTypeBatch, out var targetBundle, out var targetInner); BundleIndexing.GetBundleIndices(indexInTypeBatch, out var sourceBundle, out var sourceInner); From edfdc675756b8d851d5aed66c631495b8ed22f26 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 7 Nov 2021 17:39:59 -0600 Subject: [PATCH 271/947] Only one hole remaining... --- BepuPhysics/Bodies.cs | 34 ++++++++++++++++++++++++-------- BepuPhysics/Solver.cs | 45 ++++++++++++++++--------------------------- 2 files changed, 43 insertions(+), 36 deletions(-) diff --git a/BepuPhysics/Bodies.cs b/BepuPhysics/Bodies.cs index cbee76f46..d458df4a0 100644 --- a/BepuPhysics/Bodies.cs +++ b/BepuPhysics/Bodies.cs @@ -444,8 +444,10 @@ unsafe void UpdateForKinematicStateChange(BodyHandle handle, ref BodyMemoryLocat //Since we haven't updated the constraint reference to this body's kinematicity yet, it was not included in the dynamicBodyHandles. //Include it here. dynamicBodyHandles[enumerator.DynamicCount++] = handle.Value; + //Remove the kinematic flag from the body's encoded index. Updating this before attempting to transfer the constraint ensures that the proper flags get stored in the new location. + encodedBodyIndices[constraint.BodyIndexInConstraint] &= BodyReferenceMask; var dynamicBodyHandlesSpan = new Span(dynamicBodyHandles, enumerator.DynamicCount); - var encodedBodyIndicesSpan = new Span(encodedBodyIndices, enumerator.DynamicCount); + var encodedBodyIndicesSpan = new Span(encodedBodyIndices, enumerator.EncodedCount); solver.GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); var constraintLocation = handleToConstraint[constraint.ConnectingConstraintHandle.Value]; bool foundBatch = false; @@ -465,16 +467,32 @@ unsafe void UpdateForKinematicStateChange(BodyHandle handle, ref BodyMemoryLocat break; } } - if (!foundBatch && fallbackExists && constraintLocation.BatchIndex != solver.FallbackBatchThreshold) + if (!foundBatch) { - //The fallback batch does not block new constraints regardless of what constraints are connected, so it's safe to add. - solver.TypeProcessors[constraintLocation.TypeId].TransferConstraint(ref typeBatch, constraintLocation.BatchIndex, constraintLocation.IndexInTypeBatch, solver, this, solver.FallbackBatchThreshold, + //Still need a batch. + if (fallbackExists && constraintLocation.BatchIndex != solver.FallbackBatchThreshold) + { + //There's a fallback, and the source *isn't* the fallback. We can safely transfer over. + solver.TypeProcessors[constraintLocation.TypeId].TransferConstraint(ref typeBatch, constraintLocation.BatchIndex, constraintLocation.IndexInTypeBatch, solver, this, solver.FallbackBatchThreshold, new Span(dynamicBodyHandles, enumerator.DynamicCount), encodedBodyIndicesSpan); - foundBatch = true; + foundBatch = true; + } + else if (fallbackExists) + { + //There's a fallback, but the constraint is already in the fallback batch. + //It's not safe to use TransferConstraint for fallback->fallback transfers; it assumes source and target differ. + //Use a special case fallback move. + + } + else + { + //No batch has been found that can hold the constraint, but there is room for additional constraint batches. + var targetBatchIndex = solver.AllocateNewConstraintBatch(); + solver.TypeProcessors[constraintLocation.TypeId].TransferConstraint(ref typeBatch, constraintLocation.BatchIndex, constraintLocation.IndexInTypeBatch, solver, this, targetBatchIndex, + new Span(dynamicBodyHandles, enumerator.DynamicCount), encodedBodyIndicesSpan); + } } - //Note that we wait until the transfer completes to update the body reference in the constraint. - //This ensures the transfer process still views the constraint as kinematic and doesn't try to remove handles from batch referenced handles that aren't present. - solver.UpdateReferenceForBodyBecomingDynamic(constraint.ConnectingConstraintHandle, constraint.BodyIndexInConstraint); + //Note that there's no need to strip kinematic flags- we stripped the flag appropriately when we created the encodedBodyIndices earlier, and those were the values that got stuck into the new allocation. } } diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index fe93c6bdc..d036bca4a 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -1022,6 +1022,19 @@ unsafe internal void GetBlockingBodyHandles(Span bodyHandles, ref Sp blockingBodyHandlesAllocation = blockingBodyHandlesAllocation.Slice(0, blockingCount); } + internal int AllocateNewConstraintBatch() + { + ref var set = ref ActiveSet; + if (set.Batches.Count == set.Batches.Span.Length) + set.Batches.Resize(set.Batches.Count + 1, pool); + set.Batches.AllocateUnsafely() = new ConstraintBatch(pool, TypeProcessors.Length); + //Create an index set for the new batch. + if (set.Batches.Count == batchReferencedHandles.Span.Length) + batchReferencedHandles.Resize(set.Batches.Count + 1, pool); + batchReferencedHandles.AllocateUnsafely() = new IndexSet(pool, bodies.ActiveSet.Count); + return set.Batches.Count - 1; + } + internal unsafe bool TryAllocateInBatch(int typeId, int targetBatchIndex, Span dynamicBodyHandles, Span encodedBodyIndices, out ConstraintHandle constraintHandle, out ConstraintReference reference) { ref var set = ref ActiveSet; @@ -1031,13 +1044,7 @@ internal unsafe bool TryAllocateInBatch(int typeId, int targetBatchIndex, Span(ConstraintHandle connectingConstraintHandle, int bodyIndexInConstraint) where TMutationType : unmanaged, IConstraintBodyReferenceMutationType + + internal unsafe void UpdateReferenceForBodyBecomingKinematic(ConstraintHandle connectingConstraintHandle, int bodyIndexInConstraint) { var reference = GetConstraintReference(connectingConstraintHandle); var bodiesPerConstraint = TypeProcessors[reference.TypeBatch.TypeId].BodiesPerConstraint; var intsPerBundle = Vector.Count * bodiesPerConstraint; BundleIndexing.GetBundleIndices(reference.IndexInTypeBatch, out var bundleIndex, out var innerIndex); var firstBodyReference = (uint*)reference.TypeBatch.BodyReferences.Memory + intsPerBundle * bundleIndex + innerIndex; - if (typeof(TMutationType) == typeof(BecomingKinematic)) - firstBodyReference[bodyIndexInConstraint * Vector.Count] |= Bodies.KinematicMask; - else if (typeof(TMutationType) == typeof(BecomingDynamic)) - firstBodyReference[bodyIndexInConstraint * Vector.Count] &= Bodies.BodyReferenceMask; - else - Debug.Fail("Hey, that's not a valid type!"); - - } - - internal unsafe void UpdateReferenceForBodyBecomingKinematic(ConstraintHandle connectingConstraintHandle, int bodyIndexInConstraint) - { - UpdateReferenceForBodyKinematicChange(connectingConstraintHandle, bodyIndexInConstraint); - } - - internal void UpdateReferenceForBodyBecomingDynamic(ConstraintHandle connectingConstraintHandle, int bodyIndexInConstraint) - { - UpdateReferenceForBodyKinematicChange(connectingConstraintHandle, bodyIndexInConstraint); + firstBodyReference[bodyIndexInConstraint * Vector.Count] |= Bodies.KinematicMask; } internal interface IConstraintReferenceReportType { } From ae209cefc2e4e6d05878cb8510a0edbe37370c93 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 7 Nov 2021 19:07:14 -0600 Subject: [PATCH 272/947] Removed redundant body references copy in TransferConstraints. Fixed some UpdateForKinematicStateChange bugs, but more remain. --- BepuPhysics/Bodies.cs | 41 ++++++++++-------------- BepuPhysics/Constraints/TypeProcessor.cs | 35 +++++++------------- BepuPhysics/Solver.cs | 10 +----- Demos/Demos/FountainStressTestDemo.cs | 13 ++++---- 4 files changed, 36 insertions(+), 63 deletions(-) diff --git a/BepuPhysics/Bodies.cs b/BepuPhysics/Bodies.cs index d458df4a0..17eb896ed 100644 --- a/BepuPhysics/Bodies.cs +++ b/BepuPhysics/Bodies.cs @@ -413,14 +413,18 @@ unsafe void UpdateForKinematicStateChange(BodyHandle handle, ref BodyMemoryLocat for (int i = 0; i < constraints.Count; ++i) { ref var constraint = ref constraints[i]; - solver.UpdateReferenceForBodyBecomingKinematic(constraint.ConnectingConstraintHandle, constraint.BodyIndexInConstraint); enumerator.DynamicCount = 0; solver.EnumerateConnectedDynamicBodies(constraint.ConnectingConstraintHandle, ref enumerator); - if (enumerator.DynamicCount == 0) + if (enumerator.DynamicCount == 1) { - //This constraint connects only kinematic bodies; keeping it in the solver would cause a singularity. + //Given that *this* body is becoming kinematic, this constraint now connects only kinematic bodies; keeping it in the solver would cause a singularity. solver.Remove(constraint.ConnectingConstraintHandle); } + else + { + //The constraint survived, so update its kinematicity flag for this body. + solver.UpdateReferenceForBodyBecomingKinematic(constraint.ConnectingConstraintHandle, constraint.BodyIndexInConstraint); + } } } else @@ -432,7 +436,7 @@ unsafe void UpdateForKinematicStateChange(BodyHandle handle, ref BodyMemoryLocat KinematicToDynamicEnumerator enumerator; enumerator.IndexToHandle = ActiveSet.IndexToHandle; enumerator.DynamicBodyHandles = dynamicBodyHandles; - enumerator.EncodedBodyIndices = dynamicBodyHandles; + enumerator.EncodedBodyIndices = encodedBodyIndices; var indexToHandle = ActiveSet.IndexToHandle; var handleToConstraint = solver.HandleToConstraint; for (int constraintIndex = 0; constraintIndex < constraints.Count; ++constraintIndex) @@ -450,9 +454,9 @@ unsafe void UpdateForKinematicStateChange(BodyHandle handle, ref BodyMemoryLocat var encodedBodyIndicesSpan = new Span(encodedBodyIndices, enumerator.EncodedCount); solver.GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); var constraintLocation = handleToConstraint[constraint.ConnectingConstraintHandle.Value]; - bool foundBatch = false; ref var batch = ref solver.ActiveSet.Batches[constraintLocation.BatchIndex]; ref var typeBatch = ref batch.TypeBatches[batch.TypeIndexToTypeBatchIndex[constraintLocation.TypeId]]; + int targetBatchIndex = -1; for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) { if (solver.batchReferencedHandles[batchIndex].CanFit(dynamicBodyHandlesSpan)) @@ -461,38 +465,27 @@ unsafe void UpdateForKinematicStateChange(BodyHandle handle, ref BodyMemoryLocat //It must have had at least one other dynamic before (otherwise it would have violated the 'constraints must have at least one dynamic in them' rule), so that body will block it. //This causes a little bit of batch inefficiency, but the batch compressor will take care of it eventually and this codepath is reasonably rare- the simplicity of reusing TransferConstraint wins. Debug.Assert(batchIndex != constraintLocation.BatchIndex, "It should not be possible for a newly dynamic reference to insert itself into the same batch it was in while kinematic."); - solver.TypeProcessors[constraintLocation.TypeId].TransferConstraint(ref typeBatch, constraintLocation.BatchIndex, constraintLocation.IndexInTypeBatch, solver, this, batchIndex, - new Span(dynamicBodyHandles, enumerator.DynamicCount), encodedBodyIndicesSpan); - foundBatch = true; + targetBatchIndex = batchIndex; break; } } - if (!foundBatch) + if (targetBatchIndex == -1) { //Still need a batch. - if (fallbackExists && constraintLocation.BatchIndex != solver.FallbackBatchThreshold) + if (fallbackExists) { - //There's a fallback, and the source *isn't* the fallback. We can safely transfer over. - solver.TypeProcessors[constraintLocation.TypeId].TransferConstraint(ref typeBatch, constraintLocation.BatchIndex, constraintLocation.IndexInTypeBatch, solver, this, solver.FallbackBatchThreshold, - new Span(dynamicBodyHandles, enumerator.DynamicCount), encodedBodyIndicesSpan); - foundBatch = true; - } - else if (fallbackExists) - { - //There's a fallback, but the constraint is already in the fallback batch. - //It's not safe to use TransferConstraint for fallback->fallback transfers; it assumes source and target differ. - //Use a special case fallback move. - + targetBatchIndex = solver.FallbackBatchThreshold; } else { //No batch has been found that can hold the constraint, but there is room for additional constraint batches. - var targetBatchIndex = solver.AllocateNewConstraintBatch(); - solver.TypeProcessors[constraintLocation.TypeId].TransferConstraint(ref typeBatch, constraintLocation.BatchIndex, constraintLocation.IndexInTypeBatch, solver, this, targetBatchIndex, - new Span(dynamicBodyHandles, enumerator.DynamicCount), encodedBodyIndicesSpan); + targetBatchIndex = solver.AllocateNewConstraintBatch(); } } + //Perform the transfer! //Note that there's no need to strip kinematic flags- we stripped the flag appropriately when we created the encodedBodyIndices earlier, and those were the values that got stuck into the new allocation. + solver.TypeProcessors[constraintLocation.TypeId].TransferConstraint(ref typeBatch, constraintLocation.BatchIndex, constraintLocation.IndexInTypeBatch, solver, this, targetBatchIndex, + new Span(dynamicBodyHandles, enumerator.DynamicCount), encodedBodyIndicesSpan); } } diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index a99ebd0d7..fd7ef6dd3 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -575,16 +575,6 @@ public unsafe sealed override int AllocateInTypeBatchForFallback(ref TypeBatch t } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - protected static void CopyConstraintData( - ref TBodyReferences sourceReferencesBundle, ref TPrestepData sourcePrestepBundle, ref TAccumulatedImpulse sourceAccumulatedBundle, int sourceInner, - ref TBodyReferences targetReferencesBundle, ref TPrestepData targetPrestepBundle, ref TAccumulatedImpulse targetAccumulatedBundle, int targetInner) - { - //Note that we do NOT copy the iteration data. It is regenerated each frame from scratch. - GatherScatter.CopyLane(ref sourceReferencesBundle, sourceInner, ref targetReferencesBundle, targetInner); - GatherScatter.CopyLane(ref sourcePrestepBundle, sourceInner, ref targetPrestepBundle, targetInner); - GatherScatter.CopyLane(ref sourceAccumulatedBundle, sourceInner, ref targetAccumulatedBundle, targetInner); - } /// /// Overwrites all the data in the target constraint slot with source data. /// @@ -595,9 +585,10 @@ protected static void Move( ref TBodyReferences targetReferencesBundle, ref TPrestepData targetPrestepBundle, ref TAccumulatedImpulse targetAccumulatedBundle, ref ConstraintHandle targetIndexToHandle, int targetInner, int targetIndex, ref Buffer handlesToConstraints) { - CopyConstraintData( - ref sourceReferencesBundle, ref sourcePrestepBundle, ref sourceAccumulatedBundle, sourceInner, - ref targetReferencesBundle, ref targetPrestepBundle, ref targetAccumulatedBundle, targetInner); + //Note that we do NOT copy the iteration data. It is regenerated each frame from scratch. + GatherScatter.CopyLane(ref sourceReferencesBundle, sourceInner, ref targetReferencesBundle, targetInner); + GatherScatter.CopyLane(ref sourcePrestepBundle, sourceInner, ref targetPrestepBundle, targetInner); + GatherScatter.CopyLane(ref sourceAccumulatedBundle, sourceInner, ref targetAccumulatedBundle, targetInner); targetIndexToHandle = sourceHandle; handlesToConstraints[sourceHandle.Value].IndexInTypeBatch = targetIndex; } @@ -752,17 +743,13 @@ public unsafe override sealed void TransferConstraint(ref TypeBatch sourceTypeBa BundleIndexing.GetBundleIndices(targetReference.IndexInTypeBatch, out var targetBundle, out var targetInner); BundleIndexing.GetBundleIndices(indexInTypeBatch, out var sourceBundle, out var sourceInner); //We don't pull a description or anything from the old constraint. That would require having a unique mapping from constraint to 'full description'. - //Instead, we just directly copy from lane to lane. - //Note that we leave out the runtime generated bits- they'll just get regenerated. - CopyConstraintData( - ref Buffer.Get(ref sourceTypeBatch.BodyReferences, sourceBundle), - ref Buffer.Get(ref sourceTypeBatch.PrestepData, sourceBundle), - ref Buffer.Get(ref sourceTypeBatch.AccumulatedImpulses, sourceBundle), - sourceInner, - ref Buffer.Get(ref targetReference.TypeBatch.BodyReferences, targetBundle), - ref Buffer.Get(ref targetReference.TypeBatch.PrestepData, targetBundle), - ref Buffer.Get(ref targetReference.TypeBatch.AccumulatedImpulses, targetBundle), - targetInner); + //Instead, we just directly copy from lane to lane. Note that body references are excluded; AllocateInBatch already took care of setting those values. + GatherScatter.CopyLane( + ref Buffer.Get(ref sourceTypeBatch.PrestepData, sourceBundle), sourceInner, + ref Buffer.Get(ref targetReference.TypeBatch.PrestepData, targetBundle), targetInner); + GatherScatter.CopyLane( + ref Buffer.Get(ref sourceTypeBatch.AccumulatedImpulses, sourceBundle), sourceInner, + ref Buffer.Get(ref targetReference.TypeBatch.AccumulatedImpulses, targetBundle), targetInner); //Now we can get rid of the old allocation. //Note the use of RemoveFromBatch instead of Remove. Solver.Remove returns the handle to the pool, which we do not want! diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index d036bca4a..76fcdfec0 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -316,7 +316,7 @@ public unsafe ConstraintReference GetConstraintReference(ConstraintHandle handle } [Conditional("DEBUG")] - unsafe internal void ValidateConstraintReferenceKinematicity() + public unsafe void ValidateConstraintReferenceKinematicity() { //Only the active set's body indices are flagged for kinematicity; the inactive sets store body handles. for (int setIndex = 0; setIndex < Sets.Length; ++setIndex) @@ -964,14 +964,6 @@ internal unsafe void AllocateInBatch(int targetBatchIndex, ConstraintHandle cons } var typeProcessor = TypeProcessors[typeId]; Debug.Assert(typeProcessor.BodiesPerConstraint == encodedBodyIndices.Length); - for (int i = 0; i < encodedBodyIndices.Length; ++i) - { - Debug.Assert(Bodies.IsEncodedKinematicReference(encodedBodyIndices[i]) == Bodies.IsKinematicUnsafeGCHole(ref bodies.ActiveSet.SolverStates[encodedBodyIndices[i] & Bodies.BodyReferenceMask].Inertia.Local)); - if (Bodies.IsEncodedKinematicReference(encodedBodyIndices[i])) - { - Debug.Assert(ConstrainedKinematicHandles.Contains(bodies.ActiveSet.IndexToHandle[encodedBodyIndices[i] & Bodies.BodyReferenceMask].Value)); - } - } var typeBatch = batch.GetOrCreateTypeBatch(typeId, typeProcessor, GetMinimumCapacityForType(typeId), pool); int indexInTypeBatch; if (targetBatchIndex == FallbackBatchThreshold) diff --git a/Demos/Demos/FountainStressTestDemo.cs b/Demos/Demos/FountainStressTestDemo.cs index 1ab3fc6ad..80926f217 100644 --- a/Demos/Demos/FountainStressTestDemo.cs +++ b/Demos/Demos/FountainStressTestDemo.cs @@ -24,7 +24,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = MathHelper.Pi * 0.1f; //Using minimum sized allocations forces as many resizes as possible. //Note the low solverFallbackBatchThreshold- we want the fallback batches to get tested thoroughly. - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), 1, solverFallbackBatchThreshold: 2, initialAllocationSizes: + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), 1, solverFallbackBatchThreshold: 128, initialAllocationSizes: new SimulationAllocationSizes { Bodies = 1, @@ -362,12 +362,13 @@ public override void Update(Window window, Camera camera, Input input, float dt) Simulation.Bodies.GetDescription(handle, out var description); Simulation.Shapes.RecursivelyRemoveAndDispose(description.Collidable.Shape, BufferPool); CreateBodyDescription(random, description.Pose, description.Velocity, out var newDescription); - //if (random.NextDouble() < 0.1f) - //{ - // //Occasionally make a dynamic kinematic. - // newDescription.LocalInertia = default; - //} + if (random.NextDouble() < 0.1f) + { + //Occasionally make a dynamic kinematic. + newDescription.LocalInertia = default; + } Simulation.Bodies.ApplyDescription(handle, newDescription); + Simulation.Solver.ValidateConstraintReferenceKinematicity(); } Simulation.Solver.ValidateConstrainedKinematicsSet(); From 0bfb01131178b7dcf296b4f1531912f488a0c0cb Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 8 Nov 2021 14:09:53 -0600 Subject: [PATCH 273/947] Added more validation. Filled out more of the kinematic state change logic and moved the bulk of the implementation into the Solver. Some corner cases remain. --- BepuPhysics/Bodies.cs | 117 +--------- .../CollisionDetection/ConstraintRemover.cs | 9 +- BepuPhysics/Constraints/TypeProcessor.cs | 4 +- BepuPhysics/SequentialFallbackBatch.cs | 10 +- BepuPhysics/Solver.cs | 216 +++++++++++++++++- 5 files changed, 221 insertions(+), 135 deletions(-) diff --git a/BepuPhysics/Bodies.cs b/BepuPhysics/Bodies.cs index 17eb896ed..088954b0e 100644 --- a/BepuPhysics/Bodies.cs +++ b/BepuPhysics/Bodies.cs @@ -352,41 +352,11 @@ public unsafe static bool HasLockedInertia(Symmetric3x3* inertia) } } - private struct DynamicToKinematicEnumerator : IForEach - { - public int DynamicCount; - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void LoopBody(int encodedBodyReference) - { - ++DynamicCount; - } - } - private unsafe struct KinematicToDynamicEnumerator : IForEach - { - public const int MaximumBodiesPerConstraint = 4; - - public Buffer IndexToHandle; - public int* DynamicBodyHandles; - public int DynamicCount; - public int* EncodedBodyIndices; - public int EncodedCount; - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void LoopBody(int encodedBodyReference) - { - Debug.Assert(EncodedCount < MaximumBodiesPerConstraint, "We assumed that the number of bodies per constraint was limited; if this assumption fails, it could cause a stack overrun."); - if (IsEncodedDynamicReference(encodedBodyReference)) - { - DynamicBodyHandles[DynamicCount++] = IndexToHandle[encodedBodyReference].Value; - } - EncodedBodyIndices[EncodedCount++] = encodedBodyReference; - } - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] unsafe void UpdateForKinematicStateChange(BodyHandle handle, ref BodyMemoryLocation location, ref BodySet set, bool previouslyKinematic, bool currentlyKinematic) { Debug.Assert(location.SetIndex == 0, "If we're changing kinematic state, we should have already awoken the body."); + solver.ValidateBatchReferencedHandlesVersusConstraintStoredReferences(); if (previouslyKinematic != currentlyKinematic) { ref var collidable = ref set.Collidables[location.Index]; @@ -406,88 +376,15 @@ unsafe void UpdateForKinematicStateChange(BodyHandle handle, ref BodyMemoryLocat ref var constraints = ref set.Constraints[location.Index]; if (currentlyKinematic) { - //Any constraints that connect only kinematic bodies together should be removed; they'll NaN out. - //Ideally, the user would handle this for all non-contact constraints, but it would be rather annoying to - //have to explicitly enumerate and remove all contact constraints any time you wanted to make a body kinematic. - DynamicToKinematicEnumerator enumerator; - for (int i = 0; i < constraints.Count; ++i) - { - ref var constraint = ref constraints[i]; - enumerator.DynamicCount = 0; - solver.EnumerateConnectedDynamicBodies(constraint.ConnectingConstraintHandle, ref enumerator); - if (enumerator.DynamicCount == 1) - { - //Given that *this* body is becoming kinematic, this constraint now connects only kinematic bodies; keeping it in the solver would cause a singularity. - solver.Remove(constraint.ConnectingConstraintHandle); - } - else - { - //The constraint survived, so update its kinematicity flag for this body. - solver.UpdateReferenceForBodyBecomingKinematic(constraint.ConnectingConstraintHandle, constraint.BodyIndexInConstraint); - } - } + solver.ValidateBatchReferencedHandlesVersusConstraintStoredReferences(); + solver.UpdateReferencesForBodyBecomingKinematic(handle, location.Index); + solver.ValidateBatchReferencedHandlesVersusConstraintStoredReferences(); } else { - //A kinematic body has become dynamic. Kinematic bodies do not block membership in constraint batches, dynamic bodies do. - //For any constraint connected to the new dynamic, ensure that it belongs to a constraint batch not shared by any other constraints connected to the same body. - int* dynamicBodyHandles = stackalloc int[KinematicToDynamicEnumerator.MaximumBodiesPerConstraint]; - int* encodedBodyIndices = stackalloc int[KinematicToDynamicEnumerator.MaximumBodiesPerConstraint]; - KinematicToDynamicEnumerator enumerator; - enumerator.IndexToHandle = ActiveSet.IndexToHandle; - enumerator.DynamicBodyHandles = dynamicBodyHandles; - enumerator.EncodedBodyIndices = encodedBodyIndices; - var indexToHandle = ActiveSet.IndexToHandle; - var handleToConstraint = solver.HandleToConstraint; - for (int constraintIndex = 0; constraintIndex < constraints.Count; ++constraintIndex) - { - ref var constraint = ref constraints[constraintIndex]; - enumerator.DynamicCount = 0; - enumerator.EncodedCount = 0; - solver.EnumerateConnectedRawBodyReferences(constraint.ConnectingConstraintHandle, ref enumerator); - //Since we haven't updated the constraint reference to this body's kinematicity yet, it was not included in the dynamicBodyHandles. - //Include it here. - dynamicBodyHandles[enumerator.DynamicCount++] = handle.Value; - //Remove the kinematic flag from the body's encoded index. Updating this before attempting to transfer the constraint ensures that the proper flags get stored in the new location. - encodedBodyIndices[constraint.BodyIndexInConstraint] &= BodyReferenceMask; - var dynamicBodyHandlesSpan = new Span(dynamicBodyHandles, enumerator.DynamicCount); - var encodedBodyIndicesSpan = new Span(encodedBodyIndices, enumerator.EncodedCount); - solver.GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); - var constraintLocation = handleToConstraint[constraint.ConnectingConstraintHandle.Value]; - ref var batch = ref solver.ActiveSet.Batches[constraintLocation.BatchIndex]; - ref var typeBatch = ref batch.TypeBatches[batch.TypeIndexToTypeBatchIndex[constraintLocation.TypeId]]; - int targetBatchIndex = -1; - for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) - { - if (solver.batchReferencedHandles[batchIndex].CanFit(dynamicBodyHandlesSpan)) - { - //Because we haven't removed the constraint from the simulation, it's currently blocking the constraint batch it previously lived in. - //It must have had at least one other dynamic before (otherwise it would have violated the 'constraints must have at least one dynamic in them' rule), so that body will block it. - //This causes a little bit of batch inefficiency, but the batch compressor will take care of it eventually and this codepath is reasonably rare- the simplicity of reusing TransferConstraint wins. - Debug.Assert(batchIndex != constraintLocation.BatchIndex, "It should not be possible for a newly dynamic reference to insert itself into the same batch it was in while kinematic."); - targetBatchIndex = batchIndex; - break; - } - } - if (targetBatchIndex == -1) - { - //Still need a batch. - if (fallbackExists) - { - targetBatchIndex = solver.FallbackBatchThreshold; - } - else - { - //No batch has been found that can hold the constraint, but there is room for additional constraint batches. - targetBatchIndex = solver.AllocateNewConstraintBatch(); - } - } - //Perform the transfer! - //Note that there's no need to strip kinematic flags- we stripped the flag appropriately when we created the encodedBodyIndices earlier, and those were the values that got stuck into the new allocation. - solver.TypeProcessors[constraintLocation.TypeId].TransferConstraint(ref typeBatch, constraintLocation.BatchIndex, constraintLocation.IndexInTypeBatch, solver, this, targetBatchIndex, - new Span(dynamicBodyHandles, enumerator.DynamicCount), encodedBodyIndicesSpan); - - } + solver.ValidateBatchReferencedHandlesVersusConstraintStoredReferences(); + solver.UpdateReferencesForBodyBecomingDynamic(handle, location.Index); + solver.ValidateBatchReferencedHandlesVersusConstraintStoredReferences(); } } } diff --git a/BepuPhysics/CollisionDetection/ConstraintRemover.cs b/BepuPhysics/CollisionDetection/ConstraintRemover.cs index 8e26b32af..b52980d38 100644 --- a/BepuPhysics/CollisionDetection/ConstraintRemover.cs +++ b/BepuPhysics/CollisionDetection/ConstraintRemover.cs @@ -383,7 +383,7 @@ public void RemoveConstraintsFromFallbackBatchReferencedHandles() for (int j = 0; j < removals.Count; ++j) { ref var target = ref removals[j]; - if (solver.ActiveSet.SequentialFallback.Remove(target.EncodedBodyIndex & Bodies.BodyReferenceMask, ref allocationIdsToFree)) + if (solver.ActiveSet.SequentialFallback.RemoveOneBodyReferenceFromDynamicsSet(target.EncodedBodyIndex & Bodies.BodyReferenceMask, ref allocationIdsToFree)) { //No more constraints for this body in the fallback set; it should not exist in the fallback batch's referenced handles anymore. //Debug.Assert(solver.batchReferencedHandles[target.BatchIndex].Contains(target.BodyHandle.Value) || bodies.GetBodyReference(target.BodyHandle).Kinematic, @@ -396,12 +396,7 @@ public void RemoveConstraintsFromFallbackBatchReferencedHandles() } public void TryRemoveBodyFromConstrainedKinematicsAndRemoveAllConstraintsForBodyFromFallbackBatch(BodyHandle bodyHandle, int bodyIndex) { - if (solver.ActiveSet.SequentialFallback.TryRemove(bodyIndex, ref allocationIdsToFree)) - { - Debug.Assert(solver.batchReferencedHandles[solver.FallbackBatchThreshold].Contains(bodyHandle.Value) || bodies.GetBodyReference(bodyHandle).Kinematic, - "The batch referenced handles must include all constraint-involved dynamics, but will not include kinematics."); - solver.batchReferencedHandles[solver.FallbackBatchThreshold].Unset(bodyHandle.Value); - } + solver.TryRemoveDynamicBodyFromFallback(bodyHandle, bodyIndex, ref allocationIdsToFree); //Note that we don't check kinematicity here. If it's dynamic, that's fine, this won't do anything. solver.ConstrainedKinematicHandles.FastRemove(bodyHandle.Value); } diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index fd7ef6dd3..ad342ec64 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -748,7 +748,7 @@ public unsafe override sealed void TransferConstraint(ref TypeBatch sourceTypeBa ref Buffer.Get(ref sourceTypeBatch.PrestepData, sourceBundle), sourceInner, ref Buffer.Get(ref targetReference.TypeBatch.PrestepData, targetBundle), targetInner); GatherScatter.CopyLane( - ref Buffer.Get(ref sourceTypeBatch.AccumulatedImpulses, sourceBundle), sourceInner, + ref Buffer.Get(ref sourceTypeBatch.AccumulatedImpulses, sourceBundle), sourceInner, ref Buffer.Get(ref targetReference.TypeBatch.AccumulatedImpulses, targetBundle), targetInner); //Now we can get rid of the old allocation. @@ -757,7 +757,7 @@ ref Buffer.Get(ref sourceTypeBatch.AccumulatedImpulses, sou //However, removes can result in empty batches that require resource reclamation. //Rather than reimplementing that we just reuse the solver's version. //That sort of resource cleanup isn't required on add- everything that is needed already exists, and nothing is going away. - solver.RemoveFromBatch(constraintHandle, sourceBatchIndex, typeId, indexInTypeBatch); + solver.RemoveFromBatch(sourceBatchIndex, typeId, indexInTypeBatch); //Don't forget to keep the solver's pointers consistent! We bypassed the usual add procedure, so the solver hasn't been notified yet. ref var constraintLocation = ref solver.HandleToConstraint[constraintHandle.Value]; diff --git a/BepuPhysics/SequentialFallbackBatch.cs b/BepuPhysics/SequentialFallbackBatch.cs index 04cf66259..4b4a7aa6e 100644 --- a/BepuPhysics/SequentialFallbackBatch.cs +++ b/BepuPhysics/SequentialFallbackBatch.cs @@ -96,7 +96,7 @@ internal void AllocateForInactive(Span dynamicBodyHandles, Bodies bo /// Body associated with a constraint in the fallback batch. /// Allocations that should be freed once execution is back in a safe context. /// True if the body was dynamic and no longer has any constraints associated with it in the fallback batch, false otherwise. - internal unsafe bool Remove(int bodyReference, ref QuickList allocationIdsToFree) + internal unsafe bool RemoveOneBodyReferenceFromDynamicsSet(int bodyReference, ref QuickList allocationIdsToFree) { if (!dynamicBodyConstraintCounts.GetTableIndices(ref bodyReference, out var tableIndex, out var bodyReferencesIndex)) return false; @@ -121,12 +121,12 @@ internal unsafe bool Remove(int bodyReference, ref QuickList allocationIdsT } /// - /// Removes a body from the fallback batch if it is present. + /// Removes a body from the fallback batch's dynamic body constraint counts if it is present. /// /// Reference to the body to remove from the fallback batch. /// Allocations that should be freed once execution is back in a safe context. /// True if the body was present in the fallback batch and was removed, false otherwise. - internal unsafe bool TryRemove(int bodyReference, ref QuickList allocationIdsToFree) + internal unsafe bool TryRemoveDynamicBodyFromTracking(int bodyReference, ref QuickList allocationIdsToFree) { if (dynamicBodyConstraintCounts.Keys.Allocated && dynamicBodyConstraintCounts.GetTableIndices(ref bodyReference, out var tableIndex, out var bodyReferencesIndex)) { @@ -147,7 +147,7 @@ internal unsafe bool TryRemove(int bodyReference, ref QuickList allocationI } - internal unsafe void Remove(Solver solver, BufferPool bufferPool, ref ConstraintBatch batch, ConstraintHandle constraintHandle, ref IndexSet fallbackBatchHandles, int typeId, int indexInTypeBatch) + internal unsafe void Remove(Solver solver, BufferPool bufferPool, ref ConstraintBatch batch, ref IndexSet fallbackBatchHandles, int typeId, int indexInTypeBatch) { var typeProcessor = solver.TypeProcessors[typeId]; var bodyCount = typeProcessor.BodiesPerConstraint; @@ -164,7 +164,7 @@ internal unsafe void Remove(Solver solver, BufferPool bufferPool, ref Constraint if (Bodies.IsEncodedDynamicReference(rawBodyIndex)) { var bodyIndex = rawBodyIndex & Bodies.BodyReferenceMask; - if (Remove(bodyIndex, ref allocationIdsToFree)) + if (RemoveOneBodyReferenceFromDynamicsSet(bodyIndex, ref allocationIdsToFree)) { fallbackBatchHandles.Remove(solver.bodies.ActiveSet.IndexToHandle[bodyIndex].Value); } diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index 76fcdfec0..99df98574 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -650,6 +650,32 @@ internal unsafe void ValidateFallbackBatchAccumulatedImpulses() } } + [Conditional("DEBUG")] + public unsafe void ValidateBatchReferencedHandlesVersusConstraintStoredReferences() + { + const int maximumBodyCountInConstraint = 4; + int* debugReferences = stackalloc int[maximumBodyCountInConstraint]; + for (int batchIndex = 0; batchIndex < ActiveSet.Batches.Count; ++batchIndex) + { + var batch = ActiveSet.Batches[batchIndex]; + for (int j = 0; j < batch.TypeBatches.Count; ++j) + { + var typeBatch = batch.TypeBatches[j]; + Debug.Assert(TypeProcessors[typeBatch.TypeId].BodiesPerConstraint <= maximumBodyCountInConstraint); + for (int k = 0; k < typeBatch.ConstraintCount; ++k) + { + PassthroughReferenceCollector debugEnumerator = new(debugReferences); + EnumerateConnectedRawBodyReferences(typeBatch.IndexToHandle[k], ref debugEnumerator); + for (int bodyIndexInConstraint = 0; bodyIndexInConstraint < debugEnumerator.Index; ++bodyIndexInConstraint) + { + var isInReferencedHandles = batchReferencedHandles[batchIndex].Contains(bodies.ActiveSet.IndexToHandle[debugReferences[bodyIndexInConstraint] & Bodies.BodyReferenceMask].Value); + Debug.Assert(isInReferencedHandles == Bodies.IsEncodedDynamicReference(debugReferences[bodyIndexInConstraint])); + } + } + } + } + } + [Conditional("DEBUG")] internal unsafe void ValidateExistingHandles(bool activeOnly = false) { @@ -1275,17 +1301,16 @@ internal void RemoveBatchIfEmpty(ref ConstraintBatch batch, int batchIndex) /// /// Removes a constraint from a batch, performing any necessary batch cleanup, but does not return the constraint's handle to the pool. /// - /// Handle of the constraint being removed. /// Index of the batch to remove from. /// Type id of the constraint to remove. /// Index of the constraint to remove within its type batch. - internal unsafe void RemoveFromBatch(ConstraintHandle constraintHandle, int batchIndex, int typeId, int indexInTypeBatch) + internal unsafe void RemoveFromBatch(int batchIndex, int typeId, int indexInTypeBatch) { ref var batch = ref ActiveSet.Batches[batchIndex]; if (batchIndex == FallbackBatchThreshold) { //Note that we have to remove from fallback first because it accesses the batch's information. - ActiveSet.SequentialFallback.Remove(this, pool, ref batch, constraintHandle, ref batchReferencedHandles[batchIndex], typeId, indexInTypeBatch); + ActiveSet.SequentialFallback.Remove(this, pool, ref batch, ref batchReferencedHandles[batchIndex], typeId, indexInTypeBatch); } else { @@ -1336,7 +1361,7 @@ public void Remove(ConstraintHandle handle) EnumerateConnectedRawBodyReferences(handle, ref enumerator); pairCache.RemoveReferenceIfContactConstraint(handle, constraintLocation.TypeId); - RemoveFromBatch(handle, constraintLocation.BatchIndex, constraintLocation.TypeId, constraintLocation.IndexInTypeBatch); + RemoveFromBatch(constraintLocation.BatchIndex, constraintLocation.TypeId, constraintLocation.IndexInTypeBatch); //A negative set index marks a slot in the handle->constraint mapping as unused. The other values are considered undefined. constraintLocation.SetIndex = -1; HandlePool.Return(handle.Value, pool); @@ -1523,14 +1548,183 @@ public unsafe float GetAccumulatedImpulseMagnitude(ConstraintHandle constraintHa } - internal unsafe void UpdateReferenceForBodyBecomingKinematic(ConstraintHandle connectingConstraintHandle, int bodyIndexInConstraint) + internal void TryRemoveDynamicBodyFromFallback(BodyHandle bodyHandle, int bodyIndex, ref QuickList allocationIdsToFree) + { + if (ActiveSet.SequentialFallback.TryRemoveDynamicBodyFromTracking(bodyIndex, ref allocationIdsToFree)) + { + Debug.Assert(batchReferencedHandles[FallbackBatchThreshold].Contains(bodyHandle.Value) || bodies.GetBodyReference(bodyHandle).Kinematic, + "The batch referenced handles must include all constraint-involved dynamics, but will not include kinematics."); + batchReferencedHandles[FallbackBatchThreshold].Unset(bodyHandle.Value); + } + } + + private struct DynamicToKinematicEnumerator : IForEach { - var reference = GetConstraintReference(connectingConstraintHandle); - var bodiesPerConstraint = TypeProcessors[reference.TypeBatch.TypeId].BodiesPerConstraint; - var intsPerBundle = Vector.Count * bodiesPerConstraint; - BundleIndexing.GetBundleIndices(reference.IndexInTypeBatch, out var bundleIndex, out var innerIndex); - var firstBodyReference = (uint*)reference.TypeBatch.BodyReferences.Memory + intsPerBundle * bundleIndex + innerIndex; - firstBodyReference[bodyIndexInConstraint * Vector.Count] |= Bodies.KinematicMask; + public int DynamicCount; + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public unsafe void LoopBody(int encodedBodyReference) + { + ++DynamicCount; + } + } + internal unsafe void UpdateReferencesForBodyBecomingKinematic(BodyHandle bodyHandle, int bodyIndex) + { + Debug.Assert(bodies.GetBodyReference(bodyHandle).Kinematic); + //Any constraints that connect only kinematic bodies together should be removed; they'll NaN out. + //Ideally, the user would handle this for all non-contact constraints, but it would be rather annoying to + //have to explicitly enumerate and remove all contact constraints any time you wanted to make a body kinematic. + DynamicToKinematicEnumerator enumerator; + ref var constraints = ref bodies.ActiveSet.Constraints[bodyIndex]; + bool presentInFallback = false; + //Note reverse iteration. If the solver removes a constraint for now being between two kinematics, you don't want to break enumeration. + for (int i = constraints.Count - 1; i >= 0; --i) + { + ref var constraint = ref constraints[i]; + var constraintHandle = constraint.ConnectingConstraintHandle; + enumerator.DynamicCount = 0; + EnumerateConnectedDynamicBodies(constraint.ConnectingConstraintHandle, ref enumerator); + if (enumerator.DynamicCount == 1) + { + //Given that *this* body is becoming kinematic, this constraint now connects only kinematic bodies; keeping it in the solver would cause a singularity. + Remove(constraintHandle); + } + else + { + //The constraint survived, so update its kinematicity flag for this body. + var location = HandleToConstraint[constraintHandle.Value]; + AssertConstraintHandleExists(constraintHandle); + var typeBatch = Sets[location.SetIndex].Batches[location.BatchIndex].GetTypeBatchPointer(location.TypeId); + var bodiesPerConstraint = TypeProcessors[location.TypeId].BodiesPerConstraint; + var intsPerBundle = Vector.Count * bodiesPerConstraint; + BundleIndexing.GetBundleIndices(location.IndexInTypeBatch, out var bundleIndex, out var innerIndex); + var firstBodyReference = (uint*)typeBatch->BodyReferences.Memory + intsPerBundle * bundleIndex + innerIndex; + ref var bodyReferenceSlot = ref firstBodyReference[constraint.BodyIndexInConstraint * Vector.Count]; + var oldDynamicIndex = bodyReferenceSlot; + bodyReferenceSlot = oldDynamicIndex | Bodies.KinematicMask; + if (location.BatchIndex < FallbackBatchThreshold) + { + //If this isn't a fallback batch, then the former dynamic was the only reference to the body in the batch and the reference should be removed to avoid blocking other bodies. + batchReferencedHandles[location.BatchIndex].Remove(bodyHandle.Value); + } + else + { + presentInFallback = true; + } + } + } + if (presentInFallback) + { + //Detected at least one constraint in the fallback. Since the body is now kinematic, *no* constraint in the fallback can have a reference to it, so just remove the body. + var ids = stackalloc int[3]; + QuickList allocationIdsToFree = new(new Buffer(ids, 3)); + TryRemoveDynamicBodyFromFallback(bodyHandle, bodyIndex, ref allocationIdsToFree); + for (int i = 0; i < allocationIdsToFree.Count; ++i) + { + pool.ReturnUnsafely(allocationIdsToFree[i]); + } + } + if (constraints.Count > 0) + { + //This body is now kinematic, and remains constrained. Stick it in the constrained kinematics set. + ConstrainedKinematicHandles.Add(bodyHandle.Value, pool); + } + ValidateConstrainedKinematicsSet(); + + } + + + private unsafe struct KinematicToDynamicEnumerator : IForEach + { + public const int MaximumBodiesPerConstraint = 4; + + public Buffer IndexToHandle; + public int* DynamicBodyHandles; + public int DynamicCount; + public int* EncodedBodyIndices; + public int EncodedCount; + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public unsafe void LoopBody(int encodedBodyReference) + { + Debug.Assert(EncodedCount < MaximumBodiesPerConstraint, "We assumed that the number of bodies per constraint was limited; if this assumption fails, it could cause a stack overrun."); + if (Bodies.IsEncodedDynamicReference(encodedBodyReference)) + { + DynamicBodyHandles[DynamicCount++] = IndexToHandle[encodedBodyReference].Value; + } + EncodedBodyIndices[EncodedCount++] = encodedBodyReference; + } + } + + internal unsafe void UpdateReferencesForBodyBecomingDynamic(BodyHandle bodyHandle, int bodyIndex) + { + //A kinematic body has become dynamic. Kinematic bodies do not block membership in constraint batches, dynamic bodies do. + //For any constraint connected to the new dynamic, ensure that it belongs to a constraint batch not shared by any other constraints connected to the same body. + int* dynamicBodyHandles = stackalloc int[KinematicToDynamicEnumerator.MaximumBodiesPerConstraint]; + int* encodedBodyIndices = stackalloc int[KinematicToDynamicEnumerator.MaximumBodiesPerConstraint]; + KinematicToDynamicEnumerator enumerator; + enumerator.IndexToHandle = bodies.ActiveSet.IndexToHandle; + enumerator.DynamicBodyHandles = dynamicBodyHandles; + enumerator.EncodedBodyIndices = encodedBodyIndices; + var indexToHandle = bodies.ActiveSet.IndexToHandle; + var handleToConstraint = HandleToConstraint; + ref var constraints = ref bodies.ActiveSet.Constraints[bodyIndex]; + for (int constraintIndex = 0; constraintIndex < constraints.Count; ++constraintIndex) + { + ValidateBatchReferencedHandlesVersusConstraintStoredReferences(); + ref var constraint = ref constraints[constraintIndex]; + enumerator.DynamicCount = 0; + enumerator.EncodedCount = 0; + EnumerateConnectedRawBodyReferences(constraint.ConnectingConstraintHandle, ref enumerator); + //Since we haven't updated the constraint reference to this body's kinematicity yet, it was not included in the dynamicBodyHandles. + //Include it here. + dynamicBodyHandles[enumerator.DynamicCount++] = bodyHandle.Value; + //Remove the kinematic flag from the body's encoded index. Updating this before attempting to transfer the constraint ensures that the proper flags get stored in the new location. + encodedBodyIndices[constraint.BodyIndexInConstraint] &= Bodies.BodyReferenceMask; + var dynamicBodyHandlesSpan = new Span(dynamicBodyHandles, enumerator.DynamicCount); + var encodedBodyIndicesSpan = new Span(encodedBodyIndices, enumerator.EncodedCount); + GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); + var constraintLocation = handleToConstraint[constraint.ConnectingConstraintHandle.Value]; + ref var batch = ref ActiveSet.Batches[constraintLocation.BatchIndex]; + ref var typeBatch = ref batch.TypeBatches[batch.TypeIndexToTypeBatchIndex[constraintLocation.TypeId]]; + int targetBatchIndex = -1; + + for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) + { + if (batchReferencedHandles[batchIndex].CanFit(dynamicBodyHandlesSpan)) + { + //Because we haven't removed the constraint from the simulation, it's currently blocking the constraint batch it previously lived in. + //It must have had at least one other dynamic before (otherwise it would have violated the 'constraints must have at least one dynamic in them' rule), so that body will block it. + //This causes a little bit of batch inefficiency, but the batch compressor will take care of it eventually and this codepath is reasonably rare- the simplicity of reusing TransferConstraint wins. + Debug.Assert(batchIndex != constraintLocation.BatchIndex, "It should not be possible for a newly dynamic reference to insert itself into the same batch it was in while kinematic."); + targetBatchIndex = batchIndex; + break; + } + } + if (targetBatchIndex == -1) + { + //Still need a batch. + if (fallbackExists) + { + targetBatchIndex = FallbackBatchThreshold; + } + else + { + //No batch has been found that can hold the constraint, but there is room for additional constraint batches. + targetBatchIndex = AllocateNewConstraintBatch(); + } + } + ValidateBatchReferencedHandlesVersusConstraintStoredReferences(); + //Perform the transfer! + //Note that there's no need to strip kinematic flags- we stripped the flag appropriately when we created the encodedBodyIndices earlier, and those were the values that got stuck into the new allocation. + TypeProcessors[constraintLocation.TypeId].TransferConstraint(ref typeBatch, constraintLocation.BatchIndex, constraintLocation.IndexInTypeBatch, this, bodies, targetBatchIndex, + new Span(dynamicBodyHandles, enumerator.DynamicCount), encodedBodyIndicesSpan); + ValidateBatchReferencedHandlesVersusConstraintStoredReferences(); + } + if(constraints.Count > 0) + { + ConstrainedKinematicHandles.FastRemove(bodyHandle.Value); + } + ValidateConstrainedKinematicsSet(); } internal interface IConstraintReferenceReportType { } From ded256a0f54f8f97546afb61fcf2d65406fc805b Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 8 Nov 2021 15:30:48 -0600 Subject: [PATCH 274/947] Turns out the remaining errors were excessive validation, neat! --- BepuPhysics/Solver.cs | 4 ---- 1 file changed, 4 deletions(-) diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index 99df98574..3d8c5e1b3 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -1670,7 +1670,6 @@ internal unsafe void UpdateReferencesForBodyBecomingDynamic(BodyHandle bodyHandl ref var constraints = ref bodies.ActiveSet.Constraints[bodyIndex]; for (int constraintIndex = 0; constraintIndex < constraints.Count; ++constraintIndex) { - ValidateBatchReferencedHandlesVersusConstraintStoredReferences(); ref var constraint = ref constraints[constraintIndex]; enumerator.DynamicCount = 0; enumerator.EncodedCount = 0; @@ -1713,18 +1712,15 @@ internal unsafe void UpdateReferencesForBodyBecomingDynamic(BodyHandle bodyHandl targetBatchIndex = AllocateNewConstraintBatch(); } } - ValidateBatchReferencedHandlesVersusConstraintStoredReferences(); //Perform the transfer! //Note that there's no need to strip kinematic flags- we stripped the flag appropriately when we created the encodedBodyIndices earlier, and those were the values that got stuck into the new allocation. TypeProcessors[constraintLocation.TypeId].TransferConstraint(ref typeBatch, constraintLocation.BatchIndex, constraintLocation.IndexInTypeBatch, this, bodies, targetBatchIndex, new Span(dynamicBodyHandles, enumerator.DynamicCount), encodedBodyIndicesSpan); - ValidateBatchReferencedHandlesVersusConstraintStoredReferences(); } if(constraints.Count > 0) { ConstrainedKinematicHandles.FastRemove(bodyHandle.Value); } - ValidateConstrainedKinematicsSet(); } internal interface IConstraintReferenceReportType { } From bbc6db278a921f5e14f8404066ad42525c25f4ad Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 8 Nov 2021 18:55:07 -0600 Subject: [PATCH 275/947] Kinematic transitions now pass tests. --- BepuPhysics/Bodies.cs | 5 ----- BepuPhysics/Constraints/TypeProcessor.cs | 14 +++++++------- BepuPhysics/Solver.cs | 24 ++++++++++++++---------- Demos/Demos/FountainStressTestDemo.cs | 2 +- 4 files changed, 22 insertions(+), 23 deletions(-) diff --git a/BepuPhysics/Bodies.cs b/BepuPhysics/Bodies.cs index 088954b0e..23b120520 100644 --- a/BepuPhysics/Bodies.cs +++ b/BepuPhysics/Bodies.cs @@ -356,7 +356,6 @@ public unsafe static bool HasLockedInertia(Symmetric3x3* inertia) unsafe void UpdateForKinematicStateChange(BodyHandle handle, ref BodyMemoryLocation location, ref BodySet set, bool previouslyKinematic, bool currentlyKinematic) { Debug.Assert(location.SetIndex == 0, "If we're changing kinematic state, we should have already awoken the body."); - solver.ValidateBatchReferencedHandlesVersusConstraintStoredReferences(); if (previouslyKinematic != currentlyKinematic) { ref var collidable = ref set.Collidables[location.Index]; @@ -376,15 +375,11 @@ unsafe void UpdateForKinematicStateChange(BodyHandle handle, ref BodyMemoryLocat ref var constraints = ref set.Constraints[location.Index]; if (currentlyKinematic) { - solver.ValidateBatchReferencedHandlesVersusConstraintStoredReferences(); solver.UpdateReferencesForBodyBecomingKinematic(handle, location.Index); - solver.ValidateBatchReferencedHandlesVersusConstraintStoredReferences(); } else { - solver.ValidateBatchReferencedHandlesVersusConstraintStoredReferences(); solver.UpdateReferencesForBodyBecomingDynamic(handle, location.Index); - solver.ValidateBatchReferencedHandlesVersusConstraintStoredReferences(); } } } diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index ad342ec64..9cd726156 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -751,6 +751,13 @@ ref Buffer.Get(ref sourceTypeBatch.PrestepData, sourceBundle), sou ref Buffer.Get(ref sourceTypeBatch.AccumulatedImpulses, sourceBundle), sourceInner, ref Buffer.Get(ref targetReference.TypeBatch.AccumulatedImpulses, targetBundle), targetInner); + //Don't forget to keep the solver's pointers consistent! We bypassed the usual add procedure, so the solver hasn't been notified yet. + ref var constraintLocation = ref solver.HandleToConstraint[constraintHandle.Value]; + constraintLocation.BatchIndex = targetBatchIndex; + constraintLocation.IndexInTypeBatch = targetReference.IndexInTypeBatch; + constraintLocation.TypeId = typeId; + solver.AssertConstraintHandleExists(constraintHandle); + //Now we can get rid of the old allocation. //Note the use of RemoveFromBatch instead of Remove. Solver.Remove returns the handle to the pool, which we do not want! //It may look a bit odd to use a solver-level function here, given that we are operating on batches and handling the solver state directly for the most part. @@ -758,13 +765,6 @@ ref Buffer.Get(ref sourceTypeBatch.AccumulatedImpulses, sou //Rather than reimplementing that we just reuse the solver's version. //That sort of resource cleanup isn't required on add- everything that is needed already exists, and nothing is going away. solver.RemoveFromBatch(sourceBatchIndex, typeId, indexInTypeBatch); - - //Don't forget to keep the solver's pointers consistent! We bypassed the usual add procedure, so the solver hasn't been notified yet. - ref var constraintLocation = ref solver.HandleToConstraint[constraintHandle.Value]; - constraintLocation.BatchIndex = targetBatchIndex; - constraintLocation.IndexInTypeBatch = targetReference.IndexInTypeBatch; - constraintLocation.TypeId = typeId; - } void InternalResize(ref TypeBatch typeBatch, BufferPool pool, int constraintCapacity) diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index 3d8c5e1b3..913b9462c 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -658,18 +658,22 @@ public unsafe void ValidateBatchReferencedHandlesVersusConstraintStoredReference for (int batchIndex = 0; batchIndex < ActiveSet.Batches.Count; ++batchIndex) { var batch = ActiveSet.Batches[batchIndex]; - for (int j = 0; j < batch.TypeBatches.Count; ++j) + for (int typeBatchIndex = 0; typeBatchIndex < batch.TypeBatches.Count; ++typeBatchIndex) { - var typeBatch = batch.TypeBatches[j]; + var typeBatch = batch.TypeBatches[typeBatchIndex]; Debug.Assert(TypeProcessors[typeBatch.TypeId].BodiesPerConstraint <= maximumBodyCountInConstraint); - for (int k = 0; k < typeBatch.ConstraintCount; ++k) + for (int constraintIndex = 0; constraintIndex < typeBatch.ConstraintCount; ++constraintIndex) { - PassthroughReferenceCollector debugEnumerator = new(debugReferences); - EnumerateConnectedRawBodyReferences(typeBatch.IndexToHandle[k], ref debugEnumerator); - for (int bodyIndexInConstraint = 0; bodyIndexInConstraint < debugEnumerator.Index; ++bodyIndexInConstraint) + var constraintHandle = typeBatch.IndexToHandle[constraintIndex]; + if (constraintHandle.Value >= 0) { - var isInReferencedHandles = batchReferencedHandles[batchIndex].Contains(bodies.ActiveSet.IndexToHandle[debugReferences[bodyIndexInConstraint] & Bodies.BodyReferenceMask].Value); - Debug.Assert(isInReferencedHandles == Bodies.IsEncodedDynamicReference(debugReferences[bodyIndexInConstraint])); + PassthroughReferenceCollector debugEnumerator = new(debugReferences); + EnumerateConnectedRawBodyReferences(constraintHandle, ref debugEnumerator); + for (int bodyIndexInConstraint = 0; bodyIndexInConstraint < debugEnumerator.Index; ++bodyIndexInConstraint) + { + var isInReferencedHandles = batchReferencedHandles[batchIndex].Contains(bodies.ActiveSet.IndexToHandle[debugReferences[bodyIndexInConstraint] & Bodies.BodyReferenceMask].Value); + Debug.Assert(isInReferencedHandles == Bodies.IsEncodedDynamicReference(debugReferences[bodyIndexInConstraint])); + } } } } @@ -1686,7 +1690,7 @@ internal unsafe void UpdateReferencesForBodyBecomingDynamic(BodyHandle bodyHandl ref var batch = ref ActiveSet.Batches[constraintLocation.BatchIndex]; ref var typeBatch = ref batch.TypeBatches[batch.TypeIndexToTypeBatchIndex[constraintLocation.TypeId]]; int targetBatchIndex = -1; - + for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) { if (batchReferencedHandles[batchIndex].CanFit(dynamicBodyHandlesSpan)) @@ -1717,7 +1721,7 @@ internal unsafe void UpdateReferencesForBodyBecomingDynamic(BodyHandle bodyHandl TypeProcessors[constraintLocation.TypeId].TransferConstraint(ref typeBatch, constraintLocation.BatchIndex, constraintLocation.IndexInTypeBatch, this, bodies, targetBatchIndex, new Span(dynamicBodyHandles, enumerator.DynamicCount), encodedBodyIndicesSpan); } - if(constraints.Count > 0) + if (constraints.Count > 0) { ConstrainedKinematicHandles.FastRemove(bodyHandle.Value); } diff --git a/Demos/Demos/FountainStressTestDemo.cs b/Demos/Demos/FountainStressTestDemo.cs index 80926f217..3a183fe17 100644 --- a/Demos/Demos/FountainStressTestDemo.cs +++ b/Demos/Demos/FountainStressTestDemo.cs @@ -24,7 +24,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = MathHelper.Pi * 0.1f; //Using minimum sized allocations forces as many resizes as possible. //Note the low solverFallbackBatchThreshold- we want the fallback batches to get tested thoroughly. - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), 1, solverFallbackBatchThreshold: 128, initialAllocationSizes: + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), 1, solverFallbackBatchThreshold: 2, initialAllocationSizes: new SimulationAllocationSizes { Bodies = 1, From 761bd3c61033978e75fbbf6f6a78477f16c78a64 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 10 Nov 2021 17:16:49 -0600 Subject: [PATCH 276/947] Fixed copy overrun. --- BepuPhysics/IslandAwakener.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/BepuPhysics/IslandAwakener.cs b/BepuPhysics/IslandAwakener.cs index 57aab0ceb..78f729168 100644 --- a/BepuPhysics/IslandAwakener.cs +++ b/BepuPhysics/IslandAwakener.cs @@ -297,7 +297,7 @@ internal unsafe void ExecutePhaseOneJob(int index) sourceSet.Constraints.CopyTo(job.SourceStart, targetSet.Constraints, job.TargetStart, job.Count); sourceSet.SolverStates.CopyTo(job.SourceStart, targetSet.SolverStates, job.TargetStart, job.Count); //This rescans the memory, but it should be still floating in cache ready to access. - for (int i = 0; i < sourceSet.Count; ++i) + for (int i = 0; i < job.Count; ++i) { var sourceBodyIndex = i + job.SourceStart; if (Bodies.IsKinematicUnsafeGCHole(ref sourceSet.SolverStates[sourceBodyIndex].Inertia.Local) && sourceSet.Constraints[sourceBodyIndex].Count > 0) From e4ad816d4cedbc4fc4a6f7fd2c5c777a145a3ec5 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 9 Nov 2021 18:09:50 -0600 Subject: [PATCH 277/947] First pass of modifying collidables for min/max speculative margins. Not plumbed through yet internally. --- BepuPhysics/BodyDescription.cs | 4 +- BepuPhysics/BodySet.cs | 2 - BepuPhysics/Collidables/Collidable.cs | 123 +++++++++++++----- .../Collidables/CollidableDescription.cs | 19 ++- BepuPhysics/StaticDescription.cs | 9 +- BepuPhysics/Statics.cs | 3 - DemoTests/PairDeterminismTests.cs | 21 +-- Demos/Demos/BlockChainDemo.cs | 6 +- Demos/Demos/BouncinessDemo.cs | 4 +- Demos/Demos/Cars/CarDemo.cs | 4 +- Demos/Demos/Cars/SimpleCar.cs | 4 +- Demos/Demos/Characters/CharacterDemo.cs | 12 +- Demos/Demos/Characters/CharacterInput.cs | 9 +- Demos/Demos/ClothDemo.cs | 8 +- Demos/Demos/CollisionQueryDemo.cs | 6 +- Demos/Demos/ColosseumDemo.cs | 8 +- Demos/Demos/CompoundTestDemo.cs | 45 ++----- Demos/Demos/ContactEventsDemo.cs | 4 +- .../Demos/ContinuousCollisionDetectionDemo.cs | 12 +- Demos/Demos/CustomVoxelCollidableDemo.cs | 6 +- Demos/Demos/FountainStressTestDemo.cs | 12 +- Demos/Demos/NewtDemo.cs | 4 +- Demos/Demos/PlanetDemo.cs | 4 +- Demos/Demos/PyramidDemo.cs | 4 +- Demos/Demos/RagdollDemo.cs | 4 +- Demos/Demos/RayCastingDemo.cs | 6 +- Demos/Demos/RopeStabilityDemo.cs | 10 +- Demos/Demos/RopeTwistDemo.cs | 6 +- Demos/Demos/SimpleSelfContainedDemo.cs | 4 +- Demos/Demos/SolverContactEnumerationDemo.cs | 4 +- Demos/Demos/Sponsors/SponsorDemo.cs | 16 +-- Demos/Demos/Sponsors/SponsorNewt.cs | 2 +- Demos/Demos/SubsteppingDemo.cs | 8 +- Demos/Demos/SweepDemo.cs | 4 +- Demos/Demos/Tanks/Tank.cs | 6 +- Demos/Demos/Tanks/TankDemo.cs | 4 +- .../BroadPhaseStressTestDemo.cs | 35 +---- Demos/SpecializedTests/CapsuleTestDemo.cs | 24 +--- Demos/SpecializedTests/CharacterTestDemo.cs | 4 +- Demos/SpecializedTests/ClothLatticeDemo.cs | 47 +------ .../CompoundCollisionIndicesTest.cs | 4 +- .../ConstrainedKinematicIntegrationTest.cs | 6 +- Demos/SpecializedTests/ConstraintTestDemo.cs | 14 +- Demos/SpecializedTests/ConvexHullTestDemo.cs | 20 +-- Demos/SpecializedTests/CylinderTestDemo.cs | 15 +-- Demos/SpecializedTests/GyroscopeTestDemo.cs | 4 +- Demos/SpecializedTests/Media/BedsheetDemo.cs | 12 +- .../Media/ColosseumVideoDemo.cs | 8 +- .../Media/NewtDemandingSacrificeVideoDemo.cs | 12 +- Demos/SpecializedTests/Media/NewtVideoDemo.cs | 8 +- .../Media/PyramidVideoDemo.cs | 4 +- .../Media/ShrinkwrappedNewtsVideoDemo.cs | 8 +- Demos/SpecializedTests/MeshMeshTestDemo.cs | 6 +- .../SpecializedTests/MeshReductionTestDemo.cs | 27 +--- .../MeshSerializationTestDemo.cs | 6 +- Demos/SpecializedTests/MeshTestDemo.cs | 21 +-- .../PyramidAwakenerTestDemo.cs | 4 +- Demos/SpecializedTests/RagdollTubeDemo.cs | 17 +-- Demos/SpecializedTests/ShapePileTestDemo.cs | 17 +-- Demos/SpecializedTests/SolverBatchTestDemo.cs | 49 +------ Demos/SpecializedTests/TriangleTestDemo.cs | 28 ++-- Demos/SpecializedTests/VolumeQueryTests.cs | 31 +---- 62 files changed, 328 insertions(+), 510 deletions(-) diff --git a/BepuPhysics/BodyDescription.cs b/BepuPhysics/BodyDescription.cs index 3268b8c2c..f12973e6c 100644 --- a/BepuPhysics/BodyDescription.cs +++ b/BepuPhysics/BodyDescription.cs @@ -143,7 +143,7 @@ public static BodyDescription CreateConvexDynamic( Pose = pose, Velocity = velocity, Activity = GetDefaultActivity(shape), - Collidable = new CollidableDescription(shapes.Add(shape), GetDefaultSpeculativeMargin(shape)) + Collidable = new CollidableDescription(shapes.Add(shape)) }; shape.ComputeInertia(mass, out description.LocalInertia); return description; @@ -266,7 +266,7 @@ public static BodyDescription CreateConvexKinematic( Pose = pose, Velocity = velocity, Activity = GetDefaultActivity(shape), - Collidable = new CollidableDescription(shapes.Add(shape), GetDefaultSpeculativeMargin(shape)) + Collidable = new CollidableDescription(shapes.Add(shape)) }; return description; } diff --git a/BepuPhysics/BodySet.cs b/BepuPhysics/BodySet.cs index bb359854d..953965df2 100644 --- a/BepuPhysics/BodySet.cs +++ b/BepuPhysics/BodySet.cs @@ -132,7 +132,6 @@ internal void ApplyDescriptionByIndex(int index, in BodyDescription description) state.Inertia.Local = description.LocalInertia; ref var collidable = ref Collidables[index]; collidable.Continuity = description.Collidable.Continuity; - collidable.SpeculativeMargin = description.Collidable.SpeculativeMargin; //Note that we change the shape here. If the collidable transitions from shapeless->shapeful or shapeful->shapeless, the broad phase has to be notified //so that it can create/remove an entry. That's why this function isn't public. collidable.Shape = description.Collidable.Shape; @@ -152,7 +151,6 @@ public void GetDescription(int index, out BodyDescription description) ref var collidable = ref Collidables[index]; description.Collidable.Continuity = collidable.Continuity; description.Collidable.Shape = collidable.Shape; - description.Collidable.SpeculativeMargin = collidable.SpeculativeMargin; ref var activity = ref Activity[index]; description.Activity.SleepThreshold = activity.SleepThreshold; description.Activity.MinimumTimestepCountUnderThreshold = activity.MinimumTimestepsUnderThreshold; diff --git a/BepuPhysics/Collidables/Collidable.cs b/BepuPhysics/Collidables/Collidable.cs index 0eefc8528..b06e9fcff 100644 --- a/BepuPhysics/Collidables/Collidable.cs +++ b/BepuPhysics/Collidables/Collidable.cs @@ -1,20 +1,24 @@ -using System.Runtime.CompilerServices; +using BepuPhysics.CollisionDetection; +using System.Runtime.CompilerServices; using System.Runtime.InteropServices; namespace BepuPhysics.Collidables { + /// + /// Defines how a collidable will handle collision detection in the presence of velocity. + /// public enum ContinuousDetectionMode { /// - /// No dedicated continuous detection is performed. Default speculative contact generation will occur within the speculative margin. + /// No sweep tests are performed. Default speculative contact generation will occur within the speculative margin. /// The collidable's bounding box will not be expanded by velocity beyond the speculative margin. - /// This is the cheapest mode, but it may miss collisions. Note that if a Discrete mode collidable is moving quickly, the fact that its bounding box is not expanded - /// may cause it to miss a collision even with a non-Discrete collidable. + /// This is the cheapest mode. If a Discrete mode collidable is moving quickly and the maximum speculative margin is limited, + /// the fact that its bounding box is not expanded may cause it to miss a collision even with a non-Discrete collidable. /// Discrete = 0, /// - /// No dedicated continuous detection is performed. Default speculative contact generation will occur within the speculative margin. - /// The collidable's bounding box will be expanded by velocity beyond the speculative margin if necessary. + /// No sweep tests are performed. Default speculative contact generation will occur within the speculative margin. + /// The collidable's bounding box will be expanded by velocity without being limited by the speculative margin. /// This is useful when a collidable may move quickly and does not itself require continuous detection, but there exist other collidables with continuous modes /// that should avoid missing collisions. /// @@ -26,50 +30,90 @@ public enum ContinuousDetectionMode Continuous = 2, } - [StructLayout(LayoutKind.Explicit)] - public struct ContinuousDetectionSettings + /// + /// Defines how a collidable handles collisions with significant velocity. + /// + public struct ContinuousDetection { /// /// The continuous collision detection mode. /// - [FieldOffset(0)] public ContinuousDetectionMode Mode; + + /// + /// Lower bound on the value of the speculative margin used by the collidable. + /// + /// 0 tends to be a good default value. Higher values can be chosen if velocity magnitude is a poor proxy for speculative margins, but these cases are rare. + /// In those cases, try to use the smallest value that still satisfies requirements to avoid creating unnecessary contact constraints. + public float MinimumSpeculativeMargin; + + /// + /// Upper bound on the value of the speculative margin used by the collidable. + /// + /// tends to be a good default value for discrete or passive mode collidables. + /// The speculative margin will increase in size proportional to velocity magnitude, so high values don't usually cause performance problems. + /// Smaller values are necessary when using continuous collision detection, since continuous collision detection will early out once it finds a time that puts the collidables within speculative margin. + /// + public float MaximumSpeculativeMargin; + /// - /// If using ContinuousDetectionMode.Continuous, MinimumSweepTimestep is the minimum progress that the sweep test will make when searching for the first time of impact. - /// Collisions lasting less than MinimumProgress may be missed by the sweep test. Using larger values can significantly increase the performance of sweep tests. + /// If using , this defines the minimum progress that the sweep test will make when searching for the first time of impact. + /// Collisions lasting less than may be missed by the sweep test. Using larger values can significantly increase the performance of sweep tests. /// - [FieldOffset(4)] public float MinimumSweepTimestep; + /// - /// If using ContinuousDetectionMode.Continuous, sweep tests will terminate if the time of impact region has been refined to be smaller than SweepConvergenceThreshold. + /// If using , sweep tests will terminate if the time of impact region has been refined to be smaller than . /// Values closer to zero will converge more closely to the true time of impact, but for speculative contact generation larger values usually work fine. /// Larger values allow the sweep to terminate much earlier and can significantly improve sweep performance. /// - [FieldOffset(8)] public float SweepConvergenceThreshold; - public bool AllowExpansionBeyondSpeculativeMargin { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { return (uint)Mode > 0; } } + /// + /// Gets whether the continuous collision detection configuration will permit bounding box expansion beyond the calculated speculative margin. + /// + public bool AllowExpansionBeyondSpeculativeMargin => (uint)Mode > 0; + + //Note: there's no "Discrete" property anymore. Discrete with default values of 0 and float.MaxValue would be fully equivalent to Passive with the same configuration, so better just to encourage the Passive property instead. /// - /// No dedicated continuous detection will be performed. Default speculative contact generation will occur within the speculative margin. + /// No sweep tests are performed. Default speculative contact generation will occur within the speculative margin. /// The collidable's bounding box will not be expanded by velocity beyond the speculative margin. - /// This is the cheapest mode, but it may miss collisions. Note that if a Discrete mode collidable is moving quickly, the fact that its bounding box is not expanded - /// may cause it to miss a collision even with a non-Discrete collidable. + /// This can be marginally cheaper than Passive modes if using a limited maximum speculative margin. If a Discrete mode collidable is moving quickly and the maximum speculative margin is limited, + /// the fact that its bounding box is not expanded may cause it to miss a collision even with a non-Discrete collidable. + /// Note that Discrete and Passive only differ if maximum speculative margin is restricted. /// - public static ContinuousDetectionSettings Discrete + /// Minimum speculative margin to use for the collidable. Zero is typically a good choice. + /// Maximum speculative margin to use for the collidable. + /// If using Discrete instead of Passive, this is presumably some smaller finite value to limit the number of collision pairs found during high velocity movement. + /// Detection settings for the given discrete configuration. + public static ContinuousDetection Discrete(float minimumSpeculativeMargin = 0f, float maximumSpeculativeMargin = float.MaxValue) { - get { return new ContinuousDetectionSettings(); } + return new() { Mode = ContinuousDetectionMode.Discrete, MinimumSpeculativeMargin = minimumSpeculativeMargin, MaximumSpeculativeMargin = maximumSpeculativeMargin }; } /// - /// No dedicated continuous detection is performed. Default speculative contact generation will occur within the speculative margin. - /// The collidable's bounding box will be expanded by velocity beyond the speculative margin if necessary. - /// This is useful when a collidable may move quickly and does not itself require continuous detection, but there exist other collidables with continuous modes - /// that should avoid missing collisions. + /// No sweep tests are performed. Default speculative contact generation will occur within the speculative margin. + /// The collidable's bounding box and speculative margin will be expanded by velocity. + /// This is useful when a collidable may move quickly and does not itself require continuous detection, but there exist other collidables with continuous modes that should avoid missing collisions. + /// + /// Detection settings for the passive configuration. + public static ContinuousDetection Passive + { + get { return new() { Mode = ContinuousDetectionMode.Passive, MinimumSpeculativeMargin = 0, MaximumSpeculativeMargin = float.MaxValue }; } + } + + /// + /// No sweep tests are performed. Default speculative contact generation will occur within the speculative margin. + /// The collidable's bounding box will be expanded by velocity. The speculative margin will grow with velocity but be bounded by the given interval. + /// This is useful when a collidable may move quickly and does not itself require continuous detection, but there exist other collidables with continuous modes that should avoid missing collisions. /// - public static ContinuousDetectionSettings Passive + /// Minimum speculative margin to use for the collidable. Zero is typically a good choice. + /// Maximum speculative margin to use for the collidable. + /// Detection settings for the given passive configuration. + public static ContinuousDetection CreatePassive(float minimumSpeculativeMargin, float maximumSpeculativeMargin) { - get { return new ContinuousDetectionSettings() { Mode = ContinuousDetectionMode.Passive }; } + return new() { Mode = ContinuousDetectionMode.Passive, MinimumSpeculativeMargin = minimumSpeculativeMargin, MaximumSpeculativeMargin = maximumSpeculativeMargin }; } /// @@ -82,10 +126,19 @@ public static ContinuousDetectionSettings Passive /// If the region has been refined to be smaller than SweepConvergenceThreshold, the sweep will terminate. /// Values closer to zero will converge more closely to the true time of impact, but for speculative contact generation larger values usually work fine. /// Larger values allow the sweep to terminate much earlier and can significantly improve sweep performance. - /// Settings reflecting a continuous detection mode. - public static ContinuousDetectionSettings Continuous(float minimumSweepTimestep, float sweepConvergenceThreshold) + /// Minimum speculative margin to use for the collidable. Zero is typically a good choice. + /// Maximum speculative margin to use for the collidable. + /// Detection settings for the given continuous configuration. + public static ContinuousDetection Continuous(float minimumSweepTimestep = 1e-3f, float sweepConvergenceThreshold = 1e-3f, float minimumSpeculativeMargin = 0f, float maximumSpeculativeMargin = float.MaxValue) { - return new ContinuousDetectionSettings { Mode = ContinuousDetectionMode.Continuous, MinimumSweepTimestep = minimumSweepTimestep, SweepConvergenceThreshold = sweepConvergenceThreshold }; + return new() + { + Mode = ContinuousDetectionMode.Continuous, + MinimumSpeculativeMargin = minimumSpeculativeMargin, + MaximumSpeculativeMargin = maximumSpeculativeMargin, + MinimumSweepTimestep = minimumSweepTimestep, + SweepConvergenceThreshold = sweepConvergenceThreshold + }; } } @@ -99,7 +152,7 @@ public struct Collidable /// /// Continuous collision detection settings for this collidable. Includes the collision detection mode to use and tuning variables associated with those modes. /// - public ContinuousDetectionSettings Continuity; + public ContinuousDetection Continuity; /// /// Index of the shape used by the body. While this can be changed, any transition from shapeless->shapeful or shapeful->shapeless must be reported to the broad phase. @@ -108,9 +161,13 @@ public struct Collidable public TypedIndex Shape; /// /// Size of the margin around the surface of the shape in which contacts can be generated. These contacts will have negative depth and only contribute if the frame's velocities - /// would push the shapes of a pair into overlap. This should be positive to avoid jittering. It can also be used as a form of continuous collision detection, but excessively - /// high values combined with fast motion may result in visible 'ghost collision' artifacts. - /// For continuous collision detection with less chance of ghost collisions, use the dedicated continuous collision detection modes. + /// would push the shapes of a pair into overlap. + /// This is automatically set by bounding box prediction each frame, and is bound by the and values. + /// The effective speculative margin for a collision pair can also be modified from callbacks. + /// This should be positive to avoid jittering. + /// It can also be used as a form of continuous collision detection, but excessively high values combined with fast motion may result in visible 'ghost collision' artifacts. + /// For continuous collision detection with less chance of ghost collisions, use . + /// Note that continuous collision detection will only refine collision times to within the speculative margin, so if using , consider using a smaller value. /// public float SpeculativeMargin; /// diff --git a/BepuPhysics/Collidables/CollidableDescription.cs b/BepuPhysics/Collidables/CollidableDescription.cs index 06c0b8953..b2a06d97c 100644 --- a/BepuPhysics/Collidables/CollidableDescription.cs +++ b/BepuPhysics/Collidables/CollidableDescription.cs @@ -1,21 +1,27 @@ namespace BepuPhysics.Collidables { + /// + /// Describes a collidable and how it should handle continuous collision detection. + /// public struct CollidableDescription { + /// + /// Shape of the collidable. + /// public TypedIndex Shape; - public float SpeculativeMargin; - public ContinuousDetectionSettings Continuity; + /// + /// Continuous collision detection settings used by the collidable. + /// + public ContinuousDetection Continuity; /// /// Constructs a new collidable description. /// /// Shape used by the collidable. - /// Radius of the margin in which to allow speculative contact generation. /// Continuous collision detection settings for the collidable. - public CollidableDescription(TypedIndex shape, float speculativeMargin, in ContinuousDetectionSettings continuity) + public CollidableDescription(TypedIndex shape, in ContinuousDetection continuity) { Shape = shape; - SpeculativeMargin = speculativeMargin; Continuity = continuity; } @@ -23,8 +29,7 @@ public CollidableDescription(TypedIndex shape, float speculativeMargin, in Conti /// Constructs a new collidable description with default discrete continuity. /// /// Shape used by the collidable. - /// Radius of the margin in which to allow speculative contact generation. - public CollidableDescription(TypedIndex shape, float speculativeMargin) : this(shape, speculativeMargin, default) + public CollidableDescription(TypedIndex shape) : this(shape, ContinuousDetection.Passive) { } } diff --git a/BepuPhysics/StaticDescription.cs b/BepuPhysics/StaticDescription.cs index b6499c1f7..eeaab211e 100644 --- a/BepuPhysics/StaticDescription.cs +++ b/BepuPhysics/StaticDescription.cs @@ -45,14 +45,12 @@ public StaticDescription(in Vector3 position, in CollidableDescription collidabl /// Position of the static. /// Orientation of the static. /// Index of the static's shape in the simulation shapes set. - /// Distance beyond the surface of the static to allow speculative contacts to be generated. - public StaticDescription(in Vector3 position, in Quaternion orientation, TypedIndex shapeIndex, float speculativeMargin) + public StaticDescription(in Vector3 position, in Quaternion orientation, TypedIndex shapeIndex) { Pose.Position = position; Pose.Orientation = orientation; - Collidable.Continuity = new ContinuousDetectionSettings(); + Collidable.Continuity = ContinuousDetection.Passive; Collidable.Shape = shapeIndex; - Collidable.SpeculativeMargin = speculativeMargin; } /// @@ -60,8 +58,7 @@ public StaticDescription(in Vector3 position, in Quaternion orientation, TypedIn /// /// Position of the static. /// Index of the static's shape in the simulation shapes set. - /// Distance beyond the surface of the body to allow speculative contacts to be generated. - public StaticDescription(in Vector3 position, TypedIndex shapeIndex, float speculativeMargin) : this(position, Quaternion.Identity, shapeIndex, speculativeMargin) + public StaticDescription(in Vector3 position, TypedIndex shapeIndex) : this(position, Quaternion.Identity, shapeIndex) { } } diff --git a/BepuPhysics/Statics.cs b/BepuPhysics/Statics.cs index 6e04cb8a8..56a1697d6 100644 --- a/BepuPhysics/Statics.cs +++ b/BepuPhysics/Statics.cs @@ -305,7 +305,6 @@ internal void ApplyDescriptionByIndexWithoutBroadPhaseModification diff --git a/DemoTests/PairDeterminismTests.cs b/DemoTests/PairDeterminismTests.cs index 137d240fb..ab34e3558 100644 --- a/DemoTests/PairDeterminismTests.cs +++ b/DemoTests/PairDeterminismTests.cs @@ -75,7 +75,7 @@ static void ComputeCollisions(CollisionTaskRegistry registry, Shapes shapes, Buf var index = remapIndices[i]; ref var poseA = ref posesA[index]; ref var poseB = ref posesB[index]; - batcher.Add(a.Shape, b.Shape, poseB.Position - poseA.Position, poseA.Orientation, poseB.Orientation, Math.Max(a.SpeculativeMargin, b.SpeculativeMargin), new PairContinuation(index)); + batcher.Add(a.Shape, b.Shape, poseB.Position - poseA.Position, poseA.Orientation, poseB.Orientation, Math.Max(a.Continuity.MaximumSpeculativeMargin, b.Continuity.MaximumSpeculativeMargin), new PairContinuation(index)); } batcher.Flush(); } @@ -231,12 +231,13 @@ public static void Test() var shapes = new Shapes(pool, 8); const float speculativeMargin = 0.1f; - var sphereCollidable = new CollidableDescription(shapes.Add(sphere), speculativeMargin); - var capsuleCollidable = new CollidableDescription(shapes.Add(capsule), speculativeMargin); - var boxCollidable = new CollidableDescription(shapes.Add(box), speculativeMargin); - var triangleCollidable = new CollidableDescription(shapes.Add(triangle), speculativeMargin); - var cylinderCollidable = new CollidableDescription(shapes.Add(cylinder), speculativeMargin); - var hullCollidable = new CollidableDescription(shapes.Add(convexHull), speculativeMargin); + var continuousDetection = ContinuousDetection.Discrete(speculativeMargin, speculativeMargin); + var sphereCollidable = new CollidableDescription(shapes.Add(sphere), continuousDetection); + var capsuleCollidable = new CollidableDescription(shapes.Add(capsule), continuousDetection); + var boxCollidable = new CollidableDescription(shapes.Add(box), continuousDetection); + var triangleCollidable = new CollidableDescription(shapes.Add(triangle), continuousDetection); + var cylinderCollidable = new CollidableDescription(shapes.Add(cylinder), continuousDetection); + var hullCollidable = new CollidableDescription(shapes.Add(convexHull), continuousDetection); var compoundBuilder = new CompoundBuilder(pool, shapes, 6); @@ -249,11 +250,11 @@ public static void Test() compoundBuilder.BuildKinematicCompound(out var children, out _); var compound = new Compound(children); var bigCompound = new BigCompound(children, shapes, pool); - var compoundCollidable = new CollidableDescription(shapes.Add(compound), 0.1f); - var bigCompoundCollidable = new CollidableDescription(shapes.Add(bigCompound), 0.1f); + var compoundCollidable = new CollidableDescription(shapes.Add(compound), continuousDetection); + var bigCompoundCollidable = new CollidableDescription(shapes.Add(bigCompound), continuousDetection); DemoMeshHelper.CreateDeformedPlane(2, 2, (x, y) => new Vector3(x, 0, y), Vector3.One, pool, out var mesh); - var meshCollidable = new CollidableDescription(shapes.Add(mesh), 0.1f); + var meshCollidable = new CollidableDescription(shapes.Add(mesh), continuousDetection); const int pairCount = 31; const int poseIterations = 256; diff --git a/Demos/Demos/BlockChainDemo.cs b/Demos/Demos/BlockChainDemo.cs index a2010246f..137cc4cf6 100644 --- a/Demos/Demos/BlockChainDemo.cs +++ b/Demos/Demos/BlockChainDemo.cs @@ -45,7 +45,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) new Vector3(0, 5 + blockIndex * (boxShape.Height + 1), (forkIndex - forkCount * 0.5f) * (boxShape.Length + 4)), //Make the uppermost block kinematic to hold up the rest of the chain. blockIndex == blocksPerChain - 1 ? new BodyInertia() : boxInertia, - new CollidableDescription(boxIndex, .1f), + new CollidableDescription(boxIndex), new BodyActivityDescription(.01f, 32)); blockHandles[blockIndex] = Simulation.Bodies.Add(bodyDescription); } @@ -62,12 +62,12 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) } } - Simulation.Statics.Add(new StaticDescription(new Vector3(1, -0.5f, 1), new CollidableDescription(Simulation.Shapes.Add(new Box(200, 1, 200)), 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(1, -0.5f, 1), new CollidableDescription(Simulation.Shapes.Add(new Box(200, 1, 200))))); //Build the coin description for the ponz-I mean ICO. var coinShape = new Cylinder(1.5f, 0.2f); coinShape.ComputeInertia(1, out var coinInertia); - coinDescription = BodyDescription.CreateDynamic(RigidPose.Identity, coinInertia, new CollidableDescription(Simulation.Shapes.Add(coinShape), 0.1f), new BodyActivityDescription(0.01f)); + coinDescription = BodyDescription.CreateDynamic(RigidPose.Identity, coinInertia, new CollidableDescription(Simulation.Shapes.Add(coinShape)), new BodyActivityDescription(0.01f)); } BodyDescription coinDescription; diff --git a/Demos/Demos/BouncinessDemo.cs b/Demos/Demos/BouncinessDemo.cs index 1e33ebda5..ec70dac39 100644 --- a/Demos/Demos/BouncinessDemo.cs +++ b/Demos/Demos/BouncinessDemo.cs @@ -90,7 +90,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var shape = new Sphere(1); shape.ComputeInertia(1, out var inertia); - var ballDescription = BodyDescription.CreateDynamic(RigidPose.Identity, inertia, new CollidableDescription(Simulation.Shapes.Add(shape), 20f), new BodyActivityDescription(1e-2f)); + var ballDescription = BodyDescription.CreateDynamic(RigidPose.Identity, inertia, new CollidableDescription(Simulation.Shapes.Add(shape), ContinuousDetection.Discrete(20, 20)), new BodyActivityDescription(1e-2f)); for (int i = 0; i < 100; ++i) { @@ -104,7 +104,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) } } - collidableMaterials.Allocate(Simulation.Statics.Add(new StaticDescription(new Vector3(0, -15f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(2500, 30, 2500)), 0.1f)))) = + collidableMaterials.Allocate(Simulation.Statics.Add(new StaticDescription(new Vector3(0, -15f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(2500, 30, 2500)))))) = new SimpleMaterial { FrictionCoefficient = 1, MaximumRecoveryVelocity = 2, SpringSettings = new SpringSettings(30, 1) }; } } diff --git a/Demos/Demos/Cars/CarDemo.cs b/Demos/Demos/Cars/CarDemo.cs index ff1848620..c9b30a354 100644 --- a/Demos/Demos/Cars/CarDemo.cs +++ b/Demos/Demos/Cars/CarDemo.cs @@ -95,7 +95,7 @@ public override void Initialize(ContentArchive content, Camera camera) Simulation.Statics.Add(new StaticDescription( new Vector3(0, buildingShape.HalfHeight, 0) + landmarkMin + landmarkSpan * new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, (float)random.NextDouble() * MathF.PI), - new CollidableDescription(Simulation.Shapes.Add(buildingShape), 0.1f))); + new CollidableDescription(Simulation.Shapes.Add(buildingShape)))); } } @@ -135,7 +135,7 @@ public override void Initialize(ContentArchive content, Camera camera) }, new Vector3(1, 1, 1), BufferPool, out var planeMesh); Simulation.Statics.Add(new StaticDescription(new Vector3(0, -15, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), - new CollidableDescription(Simulation.Shapes.Add(planeMesh), 0.1f))); + new CollidableDescription(Simulation.Shapes.Add(planeMesh)))); } diff --git a/Demos/Demos/Cars/SimpleCar.cs b/Demos/Demos/Cars/SimpleCar.cs index e585be0bf..ab838e4bb 100644 --- a/Demos/Demos/Cars/SimpleCar.cs +++ b/Demos/Demos/Cars/SimpleCar.cs @@ -43,7 +43,7 @@ public static WheelHandles CreateWheel(Simulation simulation, CollidableProperty RigidPose.Transform(bodyToWheelSuspension + suspensionDirection * suspensionLength, bodyPose, out wheelPose.Position); QuaternionEx.ConcatenateWithoutOverlap(localWheelOrientation, bodyPose.Orientation, out wheelPose.Orientation); WheelHandles handles; - handles.Wheel = simulation.Bodies.Add(BodyDescription.CreateDynamic(wheelPose, wheelInertia, new CollidableDescription(wheelShape, 0.1f), new BodyActivityDescription(0.01f))); + handles.Wheel = simulation.Bodies.Add(BodyDescription.CreateDynamic(wheelPose, wheelInertia, new CollidableDescription(wheelShape), new BodyActivityDescription(0.01f))); handles.SuspensionSpring = simulation.Solver.Add(bodyHandle, handles.Wheel, new LinearAxisServo { @@ -85,7 +85,7 @@ public static SimpleCar Create(Simulation simulation, CollidableProperty { return new Vector3(x * 5 - 50, 3 * MathF.Sin(x) * MathF.Sin(y), y * 5 - 50); }, Vector3.One, BufferPool, out var mesh); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), new CollidableDescription(Simulation.Shapes.Add(mesh), 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), new CollidableDescription(Simulation.Shapes.Add(mesh)))); } /// diff --git a/Demos/Demos/ColosseumDemo.cs b/Demos/Demos/ColosseumDemo.cs index 7aacfb20a..c18d171ed 100644 --- a/Demos/Demos/ColosseumDemo.cs +++ b/Demos/Demos/ColosseumDemo.cs @@ -77,7 +77,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var ringBoxShape = new Box(0.5f, 1, 3); ringBoxShape.ComputeInertia(1, out var ringBoxInertia); var boxDescription = BodyDescription.CreateDynamic(new Vector3(), ringBoxInertia, - new CollidableDescription(Simulation.Shapes.Add(ringBoxShape), 0.1f), + new CollidableDescription(Simulation.Shapes.Add(ringBoxShape)), new BodyActivityDescription(0.0001f)); var layerPosition = new Vector3(); @@ -97,15 +97,15 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) } //Console.WriteLine($"box count: {Simulation.Bodies.ActiveSet.Count}"); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(500, 1, 500)), 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(500, 1, 500))))); var bulletShape = new Sphere(0.5f); bulletShape.ComputeInertia(.1f, out var bulletInertia); - bulletDescription = BodyDescription.CreateDynamic(new Vector3(), bulletInertia, new CollidableDescription(Simulation.Shapes.Add(bulletShape), 10), new BodyActivityDescription(0.01f)); + bulletDescription = BodyDescription.CreateDynamic(new Vector3(), bulletInertia, new CollidableDescription(Simulation.Shapes.Add(bulletShape)), new BodyActivityDescription(0.01f)); var shootiePatootieShape = new Sphere(3f); shootiePatootieShape.ComputeInertia(100, out var shootiePatootieInertia); - shootiePatootieDescription = BodyDescription.CreateDynamic(new Vector3(), shootiePatootieInertia, new CollidableDescription(Simulation.Shapes.Add(shootiePatootieShape), 10), new BodyActivityDescription(0.01f)); + shootiePatootieDescription = BodyDescription.CreateDynamic(new Vector3(), shootiePatootieInertia, new CollidableDescription(Simulation.Shapes.Add(shootiePatootieShape)), new BodyActivityDescription(0.01f)); } BodyDescription bulletDescription; diff --git a/Demos/Demos/CompoundTestDemo.cs b/Demos/Demos/CompoundTestDemo.cs index add663ebe..6f069996b 100644 --- a/Demos/Demos/CompoundTestDemo.cs +++ b/Demos/Demos/CompoundTestDemo.cs @@ -38,7 +38,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) compoundBuilder.Add(boxChildShape, boxLocalPose, 1); compoundBuilder.BuildDynamicCompound(out var compoundChildren, out var compoundInertia, out var compoundCenter); compoundBuilder.Reset(); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(compoundCenter, compoundInertia, new CollidableDescription(Simulation.Shapes.Add(new Compound(compoundChildren)), 0.1f), new BodyActivityDescription(0.01f))); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(compoundCenter, compoundInertia, new CollidableDescription(Simulation.Shapes.Add(new Compound(compoundChildren))), new BodyActivityDescription(0.01f))); } //Build a stack of sphere grids to stress manifold reduction heuristics in a convex-ish situation. @@ -59,7 +59,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) compoundBuilder.BuildDynamicCompound(out var gridChildren, out var gridInertia, out var center); compoundBuilder.Reset(); var gridCompound = new Compound(gridChildren); - var bodyDescription = BodyDescription.CreateDynamic(RigidPose.Identity, gridInertia, new CollidableDescription(Simulation.Shapes.Add(gridCompound), 0.1f), new BodyActivityDescription(0.01f)); + var bodyDescription = BodyDescription.CreateDynamic(RigidPose.Identity, gridInertia, new CollidableDescription(Simulation.Shapes.Add(gridCompound)), new BodyActivityDescription(0.01f)); for (int i = 0; i < 4; ++i) { bodyDescription.Pose.Position = new Vector3(0, 2 + i * 3, 0); @@ -87,17 +87,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) compoundBuilder.BuildDynamicCompound(out var tableChildren, out var tableInertia, out var tableCenter); compoundBuilder.Reset(); var table = new Compound(tableChildren); - var tableDescription = new BodyDescription - { - Activity = new BodyActivityDescription { SleepThreshold = 0.01f, MinimumTimestepCountUnderThreshold = 32 }, - Collidable = new CollidableDescription - { - Shape = Simulation.Shapes.Add(table), - SpeculativeMargin = 0.1f, - }, - LocalInertia = tableInertia, - Pose = RigidPose.Identity - }; + var tableDescription = BodyDescription.CreateDynamic(RigidPose.Identity, tableInertia, new CollidableDescription(Simulation.Shapes.Add(table)), new BodyActivityDescription(0.01f)); //Stack some tables. { @@ -135,11 +125,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var sphereIndex = Simulation.Shapes.Add(sphereShape); var sphereDescription = new StaticDescription { - Collidable = new CollidableDescription - { - Shape = sphereIndex, - SpeculativeMargin = 0.1f, - }, + Collidable = new CollidableDescription(sphereIndex), Pose = new RigidPose { Position = new Vector3(10, 2, 0), Orientation = Quaternion.Identity } }; Simulation.Statics.Add(sphereDescription); @@ -169,17 +155,8 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) compoundBuilder.BuildDynamicCompound(out var clampChildren, out var clampInertia, out var clampCenter); compoundBuilder.Reset(); var clamp = new Compound(clampChildren); - var clampDescription = new BodyDescription - { - Activity = new BodyActivityDescription { SleepThreshold = 0.01f, MinimumTimestepCountUnderThreshold = 32 }, - Collidable = new CollidableDescription - { - Shape = Simulation.Shapes.Add(clamp), - SpeculativeMargin = 0.1f, - }, - LocalInertia = clampInertia, - Pose = new RigidPose { Position = tableDescription.Pose.Position + new Vector3(2f, 0.3f, 0), Orientation = Quaternion.Identity } - }; + var clampDescription = BodyDescription.CreateDynamic( + new RigidPose(tableDescription.Pose.Position + new Vector3(2f, 0.3f, 0)), clampInertia, new CollidableDescription(Simulation.Shapes.Add(clamp)), new BodyActivityDescription(0.01f)); Simulation.Bodies.Add(clampDescription); } @@ -215,7 +192,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var compoundIndex = Simulation.Shapes.Add(compound); for (int i = 0; i < 8; ++i) { - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 4 + 5 * i, 32), inertia, new CollidableDescription(compoundIndex, 0.1f), new BodyActivityDescription(0.01f))); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 4 + 5 * i, 32), inertia, new CollidableDescription(compoundIndex), new BodyActivityDescription(0.01f))); } } } @@ -226,11 +203,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var groundShapeIndex = Simulation.Shapes.Add(boxShape); var groundDescription = new StaticDescription { - Collidable = new CollidableDescription - { - Shape = groundShapeIndex, - SpeculativeMargin = 0.1f, - }, + Collidable = new CollidableDescription(groundShapeIndex), Pose = new RigidPose { Position = new Vector3(0, 0, 0), Orientation = Quaternion.Identity } }; Simulation.Statics.Add(groundDescription); @@ -244,7 +217,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) return new Vector3(offsetFromCenter.X, MathF.Cos(x / 4f) * MathF.Sin(y / 4f) - 0.01f * offsetFromCenter.LengthSquared(), offsetFromCenter.Y); }, new Vector3(2, 1, 2), BufferPool, out var planeMesh); Simulation.Statics.Add(new StaticDescription(new Vector3(64, 4, 32), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), - new CollidableDescription(Simulation.Shapes.Add(planeMesh), 0.1f))); + new CollidableDescription(Simulation.Shapes.Add(planeMesh)))); } } } diff --git a/Demos/Demos/ContactEventsDemo.cs b/Demos/Demos/ContactEventsDemo.cs index 2461b19f4..8d4b4cd2f 100644 --- a/Demos/Demos/ContactEventsDemo.cs +++ b/Demos/Demos/ContactEventsDemo.cs @@ -715,8 +715,8 @@ public override void Initialize(ContentArchive content, Camera camera) events.Register(Simulation.Bodies.GetBodyReference(listenedBody2).CollidableReference, eventHandler); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(30, 1, 30)), 0.04f))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, 3, 15), new CollidableDescription(Simulation.Shapes.Add(new Box(30, 5, 1)), 0.04f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(30, 1, 30))))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 3, 15), new CollidableDescription(Simulation.Shapes.Add(new Box(30, 5, 1))))); } public override void Update(Window window, Camera camera, Input input, float dt) diff --git a/Demos/Demos/ContinuousCollisionDetectionDemo.cs b/Demos/Demos/ContinuousCollisionDetectionDemo.cs index 2e503fb98..3081ea009 100644 --- a/Demos/Demos/ContinuousCollisionDetectionDemo.cs +++ b/Demos/Demos/ContinuousCollisionDetectionDemo.cs @@ -18,7 +18,7 @@ public class ContinuousCollisionDetectionDemo : Demo ConstraintHandle BuildSpinner(Vector3 initialPosition, float rotationSpeed) { - var spinnerBase = Simulation.Bodies.Add(BodyDescription.CreateDynamic(initialPosition, new BodyInertia { InverseMass = 1e-2f }, new CollidableDescription(Simulation.Shapes.Add(new Box(2, 2, 2)), 0.1f), new BodyActivityDescription(0.01f))); + var spinnerBase = Simulation.Bodies.Add(BodyDescription.CreateDynamic(initialPosition, new BodyInertia { InverseMass = 1e-2f }, new CollidableDescription(Simulation.Shapes.Add(new Box(2, 2, 2))), new BodyActivityDescription(0.01f))); var bladeShape = new Box(5, 0.01f, 1); bladeShape.ComputeInertia(1, out var bladeInertia); var shapeIndex = Simulation.Shapes.Add(bladeShape); @@ -28,14 +28,14 @@ ConstraintHandle BuildSpinner(Vector3 initialPosition, float rotationSpeed) //Note that you can likely get away with a larger sweep convergence duration. //The sweep convergence duration is the maximum size of the 'time of first impact' region that the sweep is allowed to terminate with; //using a time of impact which is a little bit off won't usually cause much of a problem. - //Minimum progression duration is far more important to keep small, since collisions with a duration below the minimum progression duration may be missed entirely. + //Minimum progression duration is far more important to keep small for this type of use case, since collisions with a duration below the minimum progression duration may be missed entirely. //Note that it's possible for the blades to still go through each other in certain corner cases- the CCD sweep only detects time of *first* impact. //It's possible for the contacts associated with the first impact to be insufficient for later collisions within the same frame. //It's pretty rare, though- if you have a situation where that sort of failure is common, consider increasing the collidable's speculative margin or using a higher update rate. //(The reason why we don't always just rely on large speculative margins is ghost collisions- the speculative contacts might not represent collisions //that would have actually happened, but get included in the constraint solution anyway. They're fairly rare, but it's something to watch out for.) - var spinnerBlade = Simulation.Bodies.Add(BodyDescription.CreateDynamic(initialPosition, bladeInertia, new CollidableDescription(shapeIndex, 0.2f, ContinuousDetectionSettings.Continuous(1e-4f, 1e-4f)), new BodyActivityDescription(0.01f))); + var spinnerBlade = Simulation.Bodies.Add(BodyDescription.CreateDynamic(initialPosition, bladeInertia, new CollidableDescription(shapeIndex, ContinuousDetection.Continuous(1e-4f, 1e-4f, maximumSpeculativeMargin: 0.2f)), new BodyActivityDescription(0.01f))); Simulation.Solver.Add(spinnerBase, spinnerBlade, new Hinge { LocalHingeAxisA = new Vector3(0, 0, 1), LocalHingeAxisB = new Vector3(0, 0, 1), LocalOffsetB = new Vector3(0, 0, -3), SpringSettings = new SpringSettings(30, 1) }); Simulation.Solver.Add(spinnerBase, spinnerBlade, new AngularAxisMotor { LocalAxisA = new Vector3(0, 0, 1), Settings = new MotorSettings(10, 1e-4f), TargetVelocity = rotationSpeed }); return Simulation.Solver.Add(spinnerBase, new OneBodyLinearServo { ServoSettings = ServoSettings.Default, SpringSettings = new SpringSettings(30, 1) }); @@ -65,7 +65,7 @@ public override void Initialize(ContentArchive content, Camera camera) { //These two falling dynamics have pretty small speculative margins. The second one uses continuous collision detection sweeps to generate speculative contacts. Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(-4 - 2 * j, 100 + (i + j) * 2, i * 2), new BodyVelocity { Linear = new Vector3(0, -150, 0) }, inertia, - new CollidableDescription(shapeIndex, 0.01f), new BodyActivityDescription(0.01f))); + new CollidableDescription(shapeIndex, ContinuousDetection.Discrete(maximumSpeculativeMargin: 0.01f)), new BodyActivityDescription(0.01f))); //The minimum progression duration parameter at 1e-3 means the CCD sweep won't miss any collisions that last at least 1e-3 units of time- so, if time is measured in seconds, //then this will capture any collision that an update rate of 1000hz would. //Note also that the sweep convergence threshold is actually pretty loose at 100hz. Despite that, it can still lead to reasonably good speculative contacts with solid impact behavior. @@ -73,7 +73,7 @@ public override void Initialize(ContentArchive content, Camera camera) //runs to create the actual contact manifold. That provides high quality contact positions and speculative depths. //If the ground that these boxes were smashing into was something like a mesh- which is infinitely thin- you may want to increase the sweep accuracy. Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(4 + 2 * j, 100 + (i + j) * 2, i * 2), new BodyVelocity { Linear = new Vector3(0, -150, 0) }, inertia, - new CollidableDescription(shapeIndex, 0.01f, ContinuousDetectionSettings.Continuous(1e-3f, 1e-2f)), new BodyActivityDescription(0.01f))); + new CollidableDescription(shapeIndex, ContinuousDetection.Continuous(1e-3f, 1e-2f, maximumSpeculativeMargin: 0.01f)), new BodyActivityDescription(0.01f))); } } rolloverInfo = new RolloverInfo(); @@ -86,7 +86,7 @@ public override void Initialize(ContentArchive content, Camera camera) spinnerMotorB = BuildSpinner(new Vector3(5, 10, -5), 59); rolloverInfo.Add(new Vector3(0, 12, -5), "High angular velocity continuous detection"); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(300, 10, 300)), 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(300, 10, 300))))); } double time; diff --git a/Demos/Demos/CustomVoxelCollidableDemo.cs b/Demos/Demos/CustomVoxelCollidableDemo.cs index 1c1dbf5d0..1effba012 100644 --- a/Demos/Demos/CustomVoxelCollidableDemo.cs +++ b/Demos/Demos/CustomVoxelCollidableDemo.cs @@ -429,19 +429,19 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) } } voxels = new Voxels(voxelIndices, new Vector3(1, 1, 1), BufferPool); - handle = Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), new CollidableDescription(Simulation.Shapes.Add(voxels), 0.1f))); + handle = Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), new CollidableDescription(Simulation.Shapes.Add(voxels)))); var random = new Random(5); var shapeToDrop = new Box(1, 1, 1); shapeToDrop.ComputeInertia(1, out var shapeToDropInertia); - var descriptionToDrop = BodyDescription.CreateDynamic(new Vector3(), shapeToDropInertia, new CollidableDescription(Simulation.Shapes.Add(shapeToDrop), 0.1f), new BodyActivityDescription(0.01f)); + var descriptionToDrop = BodyDescription.CreateDynamic(new Vector3(), shapeToDropInertia, new CollidableDescription(Simulation.Shapes.Add(shapeToDrop)), new BodyActivityDescription(0.01f)); for (int i = 0; i < 4096; ++i) { descriptionToDrop.Pose.Position = new Vector3(15 + 10 * (float)random.NextDouble(), 45 + 150 * (float)random.NextDouble(), 15 + 10 * (float)random.NextDouble()); Simulation.Bodies.Add(descriptionToDrop); } - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(300, 1, 300)), 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(300, 1, 300))))); } public override unsafe void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) diff --git a/Demos/Demos/FountainStressTestDemo.cs b/Demos/Demos/FountainStressTestDemo.cs index 3a183fe17..b80eddffc 100644 --- a/Demos/Demos/FountainStressTestDemo.cs +++ b/Demos/Demos/FountainStressTestDemo.cs @@ -57,7 +57,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) -staticGridWidthInInstances * staticSpacing * 0.5f + i * staticSpacing, -4 + 4 * (float)Math.Cos(i * 0.3) + 4 * (float)Math.Cos(j * 0.3), -staticGridWidthInInstances * staticSpacing * 0.5f + j * staticSpacing), - new CollidableDescription(staticShapeIndex, 0.1f)); + new CollidableDescription(staticShapeIndex)); Simulation.Statics.Add(staticDescription); } } @@ -76,7 +76,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) startingRadius * (float)Math.Cos(angle), 0, startingRadius * (float)Math.Sin(angle)), - new CollidableDescription(kinematicShapeIndex, 0.1f), + new CollidableDescription(kinematicShapeIndex), new BodyActivityDescription(0, 4)); kinematicHandles[i] = Simulation.Bodies.Add(description); } @@ -220,15 +220,15 @@ public void CreateBodyDescription(Random random, in RigidPose pose, in BodyVeloc { Pose = pose, LocalInertia = inertia, - Collidable = new CollidableDescription(shapeIndex, 5f), + Collidable = new CollidableDescription(shapeIndex), Activity = new BodyActivityDescription(0.1f), Velocity = velocity }; switch (random.Next(3)) { - case 0: description.Collidable.Continuity = ContinuousDetectionSettings.Discrete; break; - case 1: description.Collidable.Continuity = ContinuousDetectionSettings.Passive; break; - case 2: description.Collidable.Continuity = ContinuousDetectionSettings.Continuous(1e-3f, 1e-3f); break; + case 0: description.Collidable.Continuity = ContinuousDetection.Discrete(); break; + case 1: description.Collidable.Continuity = ContinuousDetection.Passive; break; + case 2: description.Collidable.Continuity = ContinuousDetection.Continuous(1e-3f, 1e-3f, maximumSpeculativeMargin: 0.2f); break; } } diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index c2239f40f..fe8ee3500 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -667,7 +667,7 @@ internal unsafe static void CreateDeformable(Simulation simulation, in Vector3 p vertexHandles[i] = simulation.Bodies.Add(BodyDescription.CreateDynamic(new RigidPose( position + QuaternionEx.Transform(vertices[i], orientation), orientation), vertexInertia, //Bodies don't have to have collidables. Take advantage of this for all the internal vertices. - new CollidableDescription(vertexEdgeCounts[i] == edgeCountForInternalVertex ? new TypedIndex() : vertexShapeIndex, cellSize * 0.5f), + new CollidableDescription(vertexEdgeCounts[i] == edgeCountForInternalVertex ? new TypedIndex() : vertexShapeIndex), new BodyActivityDescription(-0.01f))); ref var vertexSpatialIndex = ref vertexSpatialIndices[i]; filters.Allocate(vertexHandles[i]) = new DeformableCollisionFilter(vertexSpatialIndex.X, vertexSpatialIndex.Y, vertexSpatialIndex.Z, instanceId); @@ -861,7 +861,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 100, -.5f), 10, Simulation.Shapes, new Sphere(5))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(1500, 1, 1500)), 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(1500, 1, 1500))))); //Simulation.Statics.Add(new StaticDescription(new Vector3(0, -1.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Sphere(3)), 0.1f))); } diff --git a/Demos/Demos/PlanetDemo.cs b/Demos/Demos/PlanetDemo.cs index 0f139417f..fd29c3417 100644 --- a/Demos/Demos/PlanetDemo.cs +++ b/Demos/Demos/PlanetDemo.cs @@ -71,11 +71,11 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new PlanetaryGravityCallbacks() { PlanetCenter = new Vector3(), Gravity = 100000 }, new PositionFirstTimestepper()); - Simulation.Statics.Add(new StaticDescription(new Vector3(), new CollidableDescription(Simulation.Shapes.Add(new Sphere(50)), 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(), new CollidableDescription(Simulation.Shapes.Add(new Sphere(50))))); var orbiter = new Sphere(1f); orbiter.ComputeInertia(1, out var inertia); - var collidable = new CollidableDescription(Simulation.Shapes.Add(orbiter), 0.1f); + var collidable = new CollidableDescription(Simulation.Shapes.Add(orbiter)); var spacing = new Vector3(5); const int length = 20; for (int i = 0; i < length; ++i) diff --git a/Demos/Demos/PyramidDemo.cs b/Demos/Demos/PyramidDemo.cs index 69bd395fa..255a5ac12 100644 --- a/Demos/Demos/PyramidDemo.cs +++ b/Demos/Demos/PyramidDemo.cs @@ -45,13 +45,13 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) (rowIndex + 0.5f) * boxShape.Height, (pyramidIndex - pyramidCount * 0.5f) * (boxShape.Length + 4)), boxInertia, - new CollidableDescription(boxIndex, 0.1f), + new CollidableDescription(boxIndex), new BodyActivityDescription(0.01f))); } } } - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(2500, 1, 2500)), 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(2500, 1, 2500))))); } //We'll randomize the size of bullets. diff --git a/Demos/Demos/RagdollDemo.cs b/Demos/Demos/RagdollDemo.cs index 98e34199d..65dc5caa5 100644 --- a/Demos/Demos/RagdollDemo.cs +++ b/Demos/Demos/RagdollDemo.cs @@ -146,7 +146,7 @@ static BodyHandle AddBody(TShape shape, float mass, in RigidPose pose, S //Also, the cost of registering different shapes isn't that high for tiny implicit shapes. var shapeIndex = simulation.Shapes.Add(shape); shape.ComputeInertia(mass, out var inertia); - var description = BodyDescription.CreateDynamic(pose, inertia, new CollidableDescription(shapeIndex, 0.1f), new BodyActivityDescription(-0.01f)); + var description = BodyDescription.CreateDynamic(pose, inertia, new CollidableDescription(shapeIndex), new BodyActivityDescription(-0.01f)); return simulation.Bodies.Add(description); } @@ -556,7 +556,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) } } - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(300, 1, 300)), 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(300, 1, 300))))); } } diff --git a/Demos/Demos/RayCastingDemo.cs b/Demos/Demos/RayCastingDemo.cs index e76a1a5d5..c9b2aeb8f 100644 --- a/Demos/Demos/RayCastingDemo.cs +++ b/Demos/Demos/RayCastingDemo.cs @@ -120,11 +120,11 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) }; if ((i + j + k) % 2 == 1) { - Simulation.Bodies.Add(BodyDescription.CreateKinematic(new RigidPose(location, orientation), new CollidableDescription(shapeIndex, 0.1f), new BodyActivityDescription(-0.1f))); + Simulation.Bodies.Add(BodyDescription.CreateKinematic(new RigidPose(location, orientation), new CollidableDescription(shapeIndex), new BodyActivityDescription(-0.1f))); } else { - Simulation.Statics.Add(new StaticDescription(location, orientation, new CollidableDescription(shapeIndex, 0.1f))); + Simulation.Statics.Add(new StaticDescription(location, orientation, new CollidableDescription(shapeIndex))); } } } @@ -139,7 +139,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) }, new Vector3(1, 3, 1), BufferPool, out var planeMesh); Simulation.Statics.Add(new StaticDescription( new Vector3(0, -10, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 4), - new CollidableDescription(Simulation.Shapes.Add(planeMesh), 0.1f))); + new CollidableDescription(Simulation.Shapes.Add(planeMesh)))); int raySourceCount = 3; raySources = new QuickList>(raySourceCount, BufferPool); diff --git a/Demos/Demos/RopeStabilityDemo.cs b/Demos/Demos/RopeStabilityDemo.cs index d5dd2ada1..65b3741fc 100644 --- a/Demos/Demos/RopeStabilityDemo.cs +++ b/Demos/Demos/RopeStabilityDemo.cs @@ -19,7 +19,7 @@ namespace Demos.Demos /// public class RopeStabilityDemo : Demo { - public static BodyHandle[] BuildRopeBodies(Simulation simulation, in Vector3 start, int bodyCount, float bodySize, float bodySpacing, float massPerBody, float inverseInertiaScale, float speculativeMargin = 0.1f) + public static BodyHandle[] BuildRopeBodies(Simulation simulation, in Vector3 start, int bodyCount, float bodySize, float bodySpacing, float massPerBody, float inverseInertiaScale) { BodyHandle[] handles = new BodyHandle[bodyCount + 1]; var ropeShape = new Sphere(bodySize); @@ -31,7 +31,7 @@ public static BodyHandle[] BuildRopeBodies(Simulation simulation, in Vector3 sta { //Make the uppermost block kinematic to hold up the rest of the chain. Activity = new BodyActivityDescription(.01f), - Collidable = new CollidableDescription(ropeShapeIndex, speculativeMargin), + Collidable = new CollidableDescription(ropeShapeIndex), }; for (int linkIndex = 0; linkIndex < bodyCount + 1; ++linkIndex) { @@ -58,7 +58,7 @@ static BodyHandle CreateWreckingBall(Simulation simulation, BodyHandle[] bodyHan { var lastBodyReference = new BodyReference(bodyHandles[bodyHandles.Length - 1], simulation.Bodies); var wreckingBallPosition = lastBodyReference.Pose.Position - new Vector3(0, ropeBodyRadius + bodySpacing + wreckingBallRadius, 0); - var description = BodyDescription.CreateDynamic(wreckingBallPosition, wreckingBallInertia, new CollidableDescription(wreckingBallShapeIndex, 0.1f), new BodyActivityDescription(0.01f)); + var description = BodyDescription.CreateDynamic(wreckingBallPosition, wreckingBallInertia, new CollidableDescription(wreckingBallShapeIndex), new BodyActivityDescription(0.01f)); //Give it a little bump. description.Velocity = new BodyVelocity(new Vector3(-10, 0, 0), default); var wreckingBallBodyHandle = simulation.Bodies.Add(description); @@ -256,10 +256,10 @@ bool TryCreateConstraint(int handleIndexA, int handleIndexB) rolloverInfo.Add(startLocation, $"100:1 mass ratio, {constraintsPerBody - 1}x extra skip constraints"); } - Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(200, 1, 200)), 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(200, 1, 200))))); Simulation.Statics.Add(new StaticDescription( new Vector3(100, 70, 0), BepuUtilities.QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), MathF.PI * 0.5f), - new CollidableDescription(Simulation.Shapes.Add(new Capsule(8, 64)), 0.1f))); + new CollidableDescription(Simulation.Shapes.Add(new Capsule(8, 64))))); } diff --git a/Demos/Demos/RopeTwistDemo.cs b/Demos/Demos/RopeTwistDemo.cs index df089db99..00da22c32 100644 --- a/Demos/Demos/RopeTwistDemo.cs +++ b/Demos/Demos/RopeTwistDemo.cs @@ -102,7 +102,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) const float ropeBodyRadius = 0.1f; const int ropeBodyCount = 130; var wreckingBallPosition = startLocation - new Vector3(0, ropeBodyRadius + (ropeBodyRadius * 2 + ropeBodySpacing) * ropeBodyCount + bigWreckingBall.Radius, 0); - var description = BodyDescription.CreateDynamic(wreckingBallPosition, bigWreckingBallInertia, new CollidableDescription(bigWreckingBallIndex, 25f), new BodyActivityDescription(-0.01f)); + var description = BodyDescription.CreateDynamic(wreckingBallPosition, bigWreckingBallInertia, new CollidableDescription(bigWreckingBallIndex), new BodyActivityDescription(-0.01f)); var wreckingBallBodyHandle = Simulation.Bodies.Add(description); var wreckingBallBody = Simulation.Bodies.GetBodyReference(wreckingBallBodyHandle); wreckingBallBody.Velocity.Angular = new Vector3(0, 20, 0); @@ -116,7 +116,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var ropeStartLocation = startLocation + horizontalOffset; var springSettings = new SpringSettings(600, 1); - var bodyHandles = RopeStabilityDemo.BuildRopeBodies(Simulation, ropeStartLocation, ropeBodyCount, ropeBodyRadius, ropeBodySpacing, 1f, 0, 25f); + var bodyHandles = RopeStabilityDemo.BuildRopeBodies(Simulation, ropeStartLocation, ropeBodyCount, ropeBodyRadius, ropeBodySpacing, 1f, 0); for (int i = 0; i < bodyHandles.Length; ++i) { filters.Allocate(bodyHandles[i]) = new Filter { RopeIndex = (short)ropeIndex, IndexInRope = (short)i }; @@ -161,7 +161,7 @@ bool TryCreateConstraint(int handleIndexA, int handleIndexB) } - Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(200, 1, 200)), 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(200, 1, 200))))); } } diff --git a/Demos/Demos/SimpleSelfContainedDemo.cs b/Demos/Demos/SimpleSelfContainedDemo.cs index 2050cb5e4..b32bb6134 100644 --- a/Demos/Demos/SimpleSelfContainedDemo.cs +++ b/Demos/Demos/SimpleSelfContainedDemo.cs @@ -211,9 +211,9 @@ public static void Run() //Drop a ball on a big static box. var sphere = new Sphere(1); sphere.ComputeInertia(1, out var sphereInertia); - simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 5, 0), sphereInertia, new CollidableDescription(simulation.Shapes.Add(sphere), 0.1f), new BodyActivityDescription(0.01f))); + simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 5, 0), sphereInertia, new CollidableDescription(simulation.Shapes.Add(sphere)), new BodyActivityDescription(0.01f))); - simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), new CollidableDescription(simulation.Shapes.Add(new Box(500, 1, 500)), 0.1f))); + simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), new CollidableDescription(simulation.Shapes.Add(new Box(500, 1, 500))))); var threadDispatcher = new SimpleThreadDispatcher(Environment.ProcessorCount); diff --git a/Demos/Demos/SolverContactEnumerationDemo.cs b/Demos/Demos/SolverContactEnumerationDemo.cs index 371c35d41..e8167f87a 100644 --- a/Demos/Demos/SolverContactEnumerationDemo.cs +++ b/Demos/Demos/SolverContactEnumerationDemo.cs @@ -46,7 +46,7 @@ public override void Initialize(ContentArchive content, Camera camera) (-columnCount * 0.5f + columnIndex) * boxShape.Width, (rowIndex + 0.5f) * boxShape.Height + 10, 0), boxInertia, - new CollidableDescription(boxIndex, 0.1f), + new CollidableDescription(boxIndex), new BodyActivityDescription(0.01f))); } } @@ -63,7 +63,7 @@ public override void Initialize(ContentArchive content, Camera camera) return new Vector3(x - planeWidth / 2, 1 * MathF.Cos(x / 2f) * MathF.Sin(y / 2f), y - planeHeight / 2); }, new Vector3(2, 1, 2), BufferPool, out var planeMesh); Simulation.Statics.Add(new StaticDescription(new Vector3(0, -2, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), - new CollidableDescription(Simulation.Shapes.Add(planeMesh), 0.1f))); + new CollidableDescription(Simulation.Shapes.Add(planeMesh)))); } struct Contact diff --git a/Demos/Demos/Sponsors/SponsorDemo.cs b/Demos/Demos/Sponsors/SponsorDemo.cs index ea4021e1d..5bfea6ca7 100644 --- a/Demos/Demos/Sponsors/SponsorDemo.cs +++ b/Demos/Demos/Sponsors/SponsorDemo.cs @@ -127,15 +127,15 @@ public override void Initialize(ContentArchive content, Camera camera) const float floorSize = 240; const float wallThickness = 200; - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -10f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(floorSize, 20, floorSize)), 0.1f))); - Simulation.Statics.Add(new StaticDescription(new Vector3(floorSize * -0.5f - wallThickness * 0.5f, -5, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(wallThickness, 30, floorSize + wallThickness * 2)), 0.1f))); - Simulation.Statics.Add(new StaticDescription(new Vector3(floorSize * 0.5f + wallThickness * 0.5f, -5, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(wallThickness, 30, floorSize + wallThickness * 2)), 0.1f))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5, floorSize * -0.5f - wallThickness * 0.5f), new CollidableDescription(Simulation.Shapes.Add(new Box(floorSize, 30, wallThickness)), 0.1f))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5, floorSize * 0.5f + wallThickness * 0.5f), new CollidableDescription(Simulation.Shapes.Add(new Box(floorSize, 30, wallThickness)), 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -10f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(floorSize, 20, floorSize))))); + Simulation.Statics.Add(new StaticDescription(new Vector3(floorSize * -0.5f - wallThickness * 0.5f, -5, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(wallThickness, 30, floorSize + wallThickness * 2))))); + Simulation.Statics.Add(new StaticDescription(new Vector3(floorSize * 0.5f + wallThickness * 0.5f, -5, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(wallThickness, 30, floorSize + wallThickness * 2))))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5, floorSize * -0.5f - wallThickness * 0.5f), new CollidableDescription(Simulation.Shapes.Add(new Box(floorSize, 30, wallThickness))))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5, floorSize * 0.5f + wallThickness * 0.5f), new CollidableDescription(Simulation.Shapes.Add(new Box(floorSize, 30, wallThickness))))); const int characterCount = 1000; characterAIs = new QuickList(characterCount, BufferPool); - var characterCollidable = new CollidableDescription(Simulation.Shapes.Add(new Capsule(0.5f, 1f)), 0.1f); + var characterCollidable = new CollidableDescription(Simulation.Shapes.Add(new Capsule(0.5f, 1f))); for (int i = 0; i < characterCount; ++i) { var position2D = newtArenaMin + (newtArenaMax - newtArenaMin) * new Vector2((float)random.NextDouble(), (float)random.NextDouble()); @@ -146,7 +146,7 @@ public override void Initialize(ContentArchive content, Camera camera) const int hutCount = 30; var hutBoxShape = new Box(0.4f, 2, 3); hutBoxShape.ComputeInertia(20, out var obstacleInertia); - var obstacleDescription = BodyDescription.CreateDynamic(new Vector3(), obstacleInertia, new CollidableDescription(Simulation.Shapes.Add(hutBoxShape), 0.1f), new BodyActivityDescription(1e-2f)); + var obstacleDescription = BodyDescription.CreateDynamic(new Vector3(), obstacleInertia, new CollidableDescription(Simulation.Shapes.Add(hutBoxShape)), new BodyActivityDescription(1e-2f)); for (int i = 0; i < hutCount; ++i) { @@ -157,7 +157,7 @@ public override void Initialize(ContentArchive content, Camera camera) var overlordNewtShape = newtMesh; overlordNewtShape.Scale = new Vector3(60, 60, 60); - overlordNewtHandle = Simulation.Statics.Add(new StaticDescription(new Vector3(0, 10, -floorSize * 0.5f - 70), new CollidableDescription(Simulation.Shapes.Add(overlordNewtShape), 0.1f))); + overlordNewtHandle = Simulation.Statics.Add(new StaticDescription(new Vector3(0, 10, -floorSize * 0.5f - 70), new CollidableDescription(Simulation.Shapes.Add(overlordNewtShape)))); } diff --git a/Demos/Demos/Sponsors/SponsorNewt.cs b/Demos/Demos/Sponsors/SponsorNewt.cs index 7dfcba36b..7b27d06f6 100644 --- a/Demos/Demos/Sponsors/SponsorNewt.cs +++ b/Demos/Demos/Sponsors/SponsorNewt.cs @@ -20,7 +20,7 @@ public SponsorNewt(Simulation simulation, TypedIndex shape, float height, in Vec var arenaSpan = arenaMax - arenaMin; var position = arenaMin + arenaSpan * new Vector2((float)random.NextDouble(), (float)random.NextDouble()); var angle = MathF.PI * 2 * (float)random.NextDouble(); - BodyHandle = simulation.Bodies.Add(BodyDescription.CreateKinematic(new RigidPose(new Vector3(position.X, height, position.Y), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, angle)), new CollidableDescription(shape, 0.5f), new BodyActivityDescription(-1))); + BodyHandle = simulation.Bodies.Add(BodyDescription.CreateKinematic(new RigidPose(new Vector3(position.X, height, position.Y), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, angle)), new CollidableDescription(shape), new BodyActivityDescription(-1))); SponsorIndex = sponsorIndex; } diff --git a/Demos/Demos/SubsteppingDemo.cs b/Demos/Demos/SubsteppingDemo.cs index 35a02e730..f00f7bafd 100644 --- a/Demos/Demos/SubsteppingDemo.cs +++ b/Demos/Demos/SubsteppingDemo.cs @@ -51,7 +51,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var boxShape = new Box(4, 0.5f, 6f); boxShape.ComputeInertia(1, out var boxInertia); //Note that sleeping is disabled with a negative velocity threshold. We want to watch the stack as we change simulation settings; if it's inactive, it won't respond! - var boxDescription = BodyDescription.CreateDynamic(new Vector3(), boxInertia, new CollidableDescription(Simulation.Shapes.Add(boxShape), 0.1f), new BodyActivityDescription(-1f)); + var boxDescription = BodyDescription.CreateDynamic(new Vector3(), boxInertia, new CollidableDescription(Simulation.Shapes.Add(boxShape)), new BodyActivityDescription(-1f)); for (int i = 0; i < 20; ++i) { boxDescription.Pose = new RigidPose(new Vector3(0, 0.5f + boxShape.Height * (i + 0.5f), 0), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, MathF.PI * 0.05f * i)); @@ -61,7 +61,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) topBlockShape.ComputeInertia(200, out var topBlockInertia); Simulation.Bodies.Add( BodyDescription.CreateDynamic(boxDescription.Pose.Position + new Vector3(0, boxShape.HalfHeight + 1f, 0), topBlockInertia, - new CollidableDescription(Simulation.Shapes.Add(topBlockShape), 0.1f), new BodyActivityDescription(-1f))); + new CollidableDescription(Simulation.Shapes.Add(topBlockShape)), new BodyActivityDescription(-1f))); rolloverInfo.Add(boxDescription.Pose.Position + new Vector3(0, 4, 0), "200:1 mass ratio"); } @@ -71,7 +71,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //(Fortunately, all 5 degrees of freedom of each hinge constraint are solved analytically, so the convergence issues aren't quite as bad as they could be.) var basePosition = new Vector3(-20, 20, 0); var boxShape = new Box(0.5f, 0.5f, 3f); - var boxCollidable = new CollidableDescription(Simulation.Shapes.Add(boxShape), 0.1f); + var boxCollidable = new CollidableDescription(Simulation.Shapes.Add(boxShape)); boxShape.ComputeInertia(1, out var boxInertia); var linkDescription = BodyDescription.CreateDynamic(new Vector3(), boxInertia, boxCollidable, new BodyActivityDescription(0.01f)); @@ -108,7 +108,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) } - Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(200, 1, 200)), 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(200, 1, 200))))); } public override void Update(Window window, Camera camera, Input input, float dt) diff --git a/Demos/Demos/SweepDemo.cs b/Demos/Demos/SweepDemo.cs index 755a1f6e5..f8a67a657 100644 --- a/Demos/Demos/SweepDemo.cs +++ b/Demos/Demos/SweepDemo.cs @@ -53,7 +53,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) { var location = new Vector3(5, 5, 5) * new Vector3(i, j, k) + new Vector3(-width * 2.5f, 2.5f, -length * 2.5f); //CreateKinematic is just a helper function that sets the inertia to all zeroes. We'll set the inertia to the actual value in the following switch. - var bodyDescription = BodyDescription.CreateKinematic(location, new CollidableDescription(default, 0.1f), new BodyActivityDescription(0.1f)); + var bodyDescription = BodyDescription.CreateKinematic(location, new CollidableDescription(default), new BodyActivityDescription(0.1f)); switch (j % 3) { case 0: @@ -106,7 +106,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) { return new Vector3(x, 1 * MathF.Cos(x / 4f) * MathF.Sin(y / 4f), y); }, new Vector3(2, 3, 2), BufferPool, out var planeMesh); - Simulation.Statics.Add(new StaticDescription(new Vector3(-64, -10, -64), new CollidableDescription(Simulation.Shapes.Add(planeMesh), 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(-64, -10, -64), new CollidableDescription(Simulation.Shapes.Add(planeMesh)))); } diff --git a/Demos/Demos/Tanks/Tank.cs b/Demos/Demos/Tanks/Tank.cs index 390104566..f5308a6c6 100644 --- a/Demos/Demos/Tanks/Tank.cs +++ b/Demos/Demos/Tanks/Tank.cs @@ -167,7 +167,7 @@ public BodyHandle Fire(Simulation simulation, CollidableProperty properties, out BodyHandle handle) { RigidPose.MultiplyWithoutOverlap(part.Pose, pose, out var bodyPose); - handle = simulation.Bodies.Add(BodyDescription.CreateDynamic(bodyPose, part.Inertia, new CollidableDescription(part.Shape, 0.1f), new BodyActivityDescription(0.01f))); + handle = simulation.Bodies.Add(BodyDescription.CreateDynamic(bodyPose, part.Inertia, new CollidableDescription(part.Shape), new BodyActivityDescription(0.01f))); ref var partProperties = ref properties.Allocate(handle); partProperties = new TankDemoBodyProperties { Friction = part.Friction, TankPart = true }; return ref partProperties.Filter; diff --git a/Demos/Demos/Tanks/TankDemo.cs b/Demos/Demos/Tanks/TankDemo.cs index 374dd3a75..f61a40c33 100644 --- a/Demos/Demos/Tanks/TankDemo.cs +++ b/Demos/Demos/Tanks/TankDemo.cs @@ -116,7 +116,7 @@ public override void Initialize(ContentArchive content, Camera camera) Simulation.Statics.Add(new StaticDescription( new Vector3(0, buildingShape.HalfHeight - 4f + GetHeightForPosition(position.X, position.Z, planeWidth, inverseTerrainScale, terrainPosition), 0) + position, QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, (float)random.NextDouble() * MathF.PI), - new CollidableDescription(Simulation.Shapes.Add(buildingShape), 0.1f))); + new CollidableDescription(Simulation.Shapes.Add(buildingShape)))); } DemoMeshHelper.CreateDeformedPlane(planeWidth, planeWidth, @@ -126,7 +126,7 @@ public override void Initialize(ContentArchive content, Camera camera) return new Vector3(position2D.X, GetHeightForPosition(position2D.X, position2D.Y, planeWidth, inverseTerrainScale, terrainPosition), position2D.Y); }, new Vector3(1, 1, 1), BufferPool, out var planeMesh); Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), - new CollidableDescription(Simulation.Shapes.Add(planeMesh), 0.1f))); + new CollidableDescription(Simulation.Shapes.Add(planeMesh)))); explosions = new QuickList(32, BufferPool); diff --git a/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs b/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs index d96b28565..3c161416b 100644 --- a/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs +++ b/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs @@ -44,43 +44,12 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var location = spacing * (new Vector3(i, j, k) + new Vector3(-width, 1, -length)) + randomizationBase + r * randomizationSpan; if ((i + j + k) % 2 == 1) { - var bodyDescription = new BodyDescription - { - Activity = new BodyActivityDescription { MinimumTimestepCountUnderThreshold = 32, SleepThreshold = -0.1f }, - Pose = new RigidPose - { - Orientation = Quaternion.Identity, - Position = location - }, - Collidable = new CollidableDescription - { - Continuity = new ContinuousDetectionSettings { Mode = ContinuousDetectionMode.Discrete }, - SpeculativeMargin = 0.1f, - Shape = shapeIndex - }, - LocalInertia = sphereInertia - }; - Simulation.Bodies.Add(bodyDescription); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(location, sphereInertia, new CollidableDescription(shapeIndex), new BodyActivityDescription(-1))); } else { - var staticDescription = new StaticDescription - { - Pose = new RigidPose - { - Orientation = Quaternion.Identity, - Position = location - }, - Collidable = new CollidableDescription - { - Continuity = new ContinuousDetectionSettings { Mode = ContinuousDetectionMode.Discrete }, - SpeculativeMargin = 0.1f, - Shape = shapeIndex - } - }; - Simulation.Statics.Add(staticDescription); + Simulation.Statics.Add(new StaticDescription(location, new CollidableDescription(shapeIndex))); } - } } } diff --git a/Demos/SpecializedTests/CapsuleTestDemo.cs b/Demos/SpecializedTests/CapsuleTestDemo.cs index cc0dc3b01..363261874 100644 --- a/Demos/SpecializedTests/CapsuleTestDemo.cs +++ b/Demos/SpecializedTests/CapsuleTestDemo.cs @@ -35,17 +35,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) for (int k = 0; k < length; ++k) { var location = new Vector3(1.5f, 1.5f, 4.4f) * new Vector3(i, j, k) + new Vector3(-width * 0.5f, 0.5f, -length * 0.5f); - var bodyDescription = new BodyDescription - { - Activity = new BodyActivityDescription { MinimumTimestepCountUnderThreshold = 32, SleepThreshold = 0.01f }, - LocalInertia = localInertia, - Pose = new RigidPose - { - Orientation = QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), MathF.PI / 2), - Position = location - }, - Collidable = new CollidableDescription { SpeculativeMargin = 50.1f, Shape = shapeIndex } - }; + var bodyDescription = BodyDescription.CreateDynamic(new RigidPose(location, QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), MathF.PI / 2)), localInertia, new CollidableDescription(shapeIndex, ContinuousDetection.Discrete(50, 50)), new BodyActivityDescription(-1)); Simulation.Bodies.Add(bodyDescription); } @@ -53,16 +43,12 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) } var boxShape = new Box(0.5f, 0.5f, 2.5f); boxShape.ComputeInertia(1, out var boxLocalInertia); - var boxDescription = new BodyDescription - { - Activity = new BodyActivityDescription { MinimumTimestepCountUnderThreshold = 32, SleepThreshold = -0.01f }, - LocalInertia = boxLocalInertia, - Pose = new(new(1, -0.5f, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), 0)), - Collidable = new CollidableDescription { SpeculativeMargin = 50.1f, Shape = Simulation.Shapes.Add(boxShape) } - }; + var boxDescription = BodyDescription.CreateDynamic(new Vector3(1, -0.5f, 0), boxLocalInertia, + new CollidableDescription(Simulation.Shapes.Add(boxShape), ContinuousDetection.Discrete(50, 50)), + new BodyActivityDescription(-1)); Simulation.Bodies.Add(boxDescription); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -3, 0), new CollidableDescription { SpeculativeMargin = 0.1f, Shape = Simulation.Shapes.Add(new Box(4, 1, 4)) })); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -3, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(4, 1, 4))))); } diff --git a/Demos/SpecializedTests/CharacterTestDemo.cs b/Demos/SpecializedTests/CharacterTestDemo.cs index bc1b306f1..87994b06d 100644 --- a/Demos/SpecializedTests/CharacterTestDemo.cs +++ b/Demos/SpecializedTests/CharacterTestDemo.cs @@ -30,7 +30,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Simulation.Bodies.Add( BodyDescription.CreateDynamic( new Vector3(250 * (float)random.NextDouble() - 125, 2, 250 * (float)random.NextDouble() - 125), new BodyInertia { InverseMass = 1 }, - new CollidableDescription(Simulation.Shapes.Add(new Capsule(0.5f, 1f)), 0.1f), + new CollidableDescription(Simulation.Shapes.Add(new Capsule(0.5f, 1f))), new BodyActivityDescription(-1)))); character.CosMaximumSlope = .707f; @@ -85,7 +85,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) return new Vector3(offsetFromCenter.X, MathF.Cos(x / 2f) + MathF.Sin(y / 2f), offsetFromCenter.Y); }, new Vector3(2, 1, 2), BufferPool, out var planeMesh); Simulation.Statics.Add(new StaticDescription(new Vector3(0, -2, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), - new CollidableDescription(Simulation.Shapes.Add(planeMesh), 0.1f))); + new CollidableDescription(Simulation.Shapes.Add(planeMesh)))); removedCharacters = new QuickQueue(characters.CharacterCount, BufferPool); } diff --git a/Demos/SpecializedTests/ClothLatticeDemo.cs b/Demos/SpecializedTests/ClothLatticeDemo.cs index e47fb851c..7b1597abf 100644 --- a/Demos/SpecializedTests/ClothLatticeDemo.cs +++ b/Demos/SpecializedTests/ClothLatticeDemo.cs @@ -39,22 +39,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) for (int j = 0; j < length; ++j) { var location = new Vector3(0, 30, 0) + new Vector3(spacing, 0, spacing) * (new Vector3(i, 0, j) + new Vector3(-width * 0.5f, 0, -length * 0.5f)); - var bodyDescription = new BodyDescription - { - Activity = new BodyActivityDescription { MinimumTimestepCountUnderThreshold = 32, SleepThreshold = 0.01f }, - Pose = new RigidPose - { - Orientation = Quaternion.Identity, - Position = location - }, - Collidable = new CollidableDescription - { - Shape = clothNodeShapeIndex, - Continuity = new ContinuousDetectionSettings { Mode = ContinuousDetectionMode.Discrete }, - SpeculativeMargin = 0.1f - }, - LocalInertia = clothNodeInertia - }; + var bodyDescription = BodyDescription.CreateDynamic(location, clothNodeInertia, new CollidableDescription(clothNodeShapeIndex, ContinuousDetection.Discrete(0, 0.1f)), new BodyActivityDescription(0.01f)); nodeHandles[i][j] = Simulation.Bodies.Add(bodyDescription); } @@ -101,39 +86,13 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var bigBallShape = new Sphere(25); var bigBallShapeIndex = Simulation.Shapes.Add(bigBallShape); - var bigBallDescription = new StaticDescription - { - Collidable = new CollidableDescription - { - Continuity = new ContinuousDetectionSettings { Mode = ContinuousDetectionMode.Discrete }, - Shape = bigBallShapeIndex, - SpeculativeMargin = 0.1f - }, - Pose = new RigidPose - { - Position = new Vector3(-10, -15, 0), - Orientation = Quaternion.Identity - } - }; + var bigBallDescription = new StaticDescription(new Vector3(-10, -15, 0), new CollidableDescription(bigBallShapeIndex, ContinuousDetection.Discrete(0, 0.1f))); Simulation.Statics.Add(bigBallDescription); var groundShape = new Box(200, 1, 200); var groundShapeIndex = Simulation.Shapes.Add(groundShape); - var groundDescription = new StaticDescription - { - Collidable = new CollidableDescription - { - Continuity = new ContinuousDetectionSettings { Mode = ContinuousDetectionMode.Discrete }, - Shape = groundShapeIndex, - SpeculativeMargin = 0.1f - }, - Pose = new RigidPose - { - Position = new Vector3(0, -10, 0), - Orientation = Quaternion.Identity - } - }; + var groundDescription = new StaticDescription(new Vector3(0, -10, 0), new CollidableDescription(groundShapeIndex)); Simulation.Statics.Add(groundDescription); } diff --git a/Demos/SpecializedTests/CompoundCollisionIndicesTest.cs b/Demos/SpecializedTests/CompoundCollisionIndicesTest.cs index eda0e3b23..a051fb76e 100644 --- a/Demos/SpecializedTests/CompoundCollisionIndicesTest.cs +++ b/Demos/SpecializedTests/CompoundCollisionIndicesTest.cs @@ -80,12 +80,12 @@ public override void Initialize(ContentArchive content, Camera camera) builder.Add(new Box(1f, 1f, 1f), new RigidPose(new Vector3(1, 0, 0)), 1); builder.BuildDynamicCompound(out var children, out var inertia, out var center); - var compoundCollidable = new CollidableDescription(Simulation.Shapes.Add(new Compound(children)), 0.1f); + var compoundCollidable = new CollidableDescription(Simulation.Shapes.Add(new Compound(children))); Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 2, 0), inertia, compoundCollidable, new BodyActivityDescription(0.01f))); Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 4, 0), inertia, compoundCollidable, new BodyActivityDescription(0.01f))); - Simulation.Statics.Add(new StaticDescription(new Vector3(), new CollidableDescription(Simulation.Shapes.Add(new Box(100, 1, 100)), 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(), new CollidableDescription(Simulation.Shapes.Add(new Box(100, 1, 100))))); } } } diff --git a/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs b/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs index 7b4b6fd99..6640f46a4 100644 --- a/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs +++ b/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs @@ -23,10 +23,10 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var shapeA = new Box(.75f, 1, .5f); var shapeIndexA = Simulation.Shapes.Add(shapeA); - var collidableA = new CollidableDescription(shapeIndexA, 0.1f); + var collidableA = new CollidableDescription(shapeIndexA); var shapeB = new Box(.75f, 1, .5f); var shapeIndexB = Simulation.Shapes.Add(shapeB); - var collidableB = new CollidableDescription(shapeIndexB, 0.1f); + var collidableB = new CollidableDescription(shapeIndexB); var activity = new BodyActivityDescription(0.01f); shapeA.ComputeInertia(1, out var inertiaA); shapeA.ComputeInertia(1, out var inertiaB); @@ -54,7 +54,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //Simulation.Solver.Add(a, b, new AngularHinge { LocalHingeAxisA = new Vector3(0, 1, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); } - Simulation.Statics.Add(new StaticDescription(new Vector3(), new CollidableDescription(Simulation.Shapes.Add(new Box(8192, 1, 8192)), 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(), new CollidableDescription(Simulation.Shapes.Add(new Box(8192, 1, 8192))))); } } } diff --git a/Demos/SpecializedTests/ConstraintTestDemo.cs b/Demos/SpecializedTests/ConstraintTestDemo.cs index 7a51fd724..db410c0d8 100644 --- a/Demos/SpecializedTests/ConstraintTestDemo.cs +++ b/Demos/SpecializedTests/ConstraintTestDemo.cs @@ -29,10 +29,10 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var shapeA = new Box(.75f, 1, .5f); var shapeIndexA = Simulation.Shapes.Add(shapeA); - var collidableA = new CollidableDescription(shapeIndexA, 0.1f); + var collidableA = new CollidableDescription(shapeIndexA); var shapeB = new Box(.75f, 1, .5f); var shapeIndexB = Simulation.Shapes.Add(shapeB); - var collidableB = new CollidableDescription(shapeIndexB, 0.1f); + var collidableB = new CollidableDescription(shapeIndexB); var activity = new BodyActivityDescription(0.01f); shapeA.ComputeInertia(1, out var inertiaA); shapeA.ComputeInertia(1, out var inertiaB); @@ -161,7 +161,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var sphere = new Sphere(0.125f); //Treat each vertex as a point mass that cannot rotate. var sphereInertia = new BodyInertia { InverseMass = 1 }; - var sphereCollidable = new CollidableDescription(Simulation.Shapes.Add(sphere), 0.1f); + var sphereCollidable = new CollidableDescription(Simulation.Shapes.Add(sphere)); var a = new Vector3(x, 3, 0); var b = new Vector3(x, 4, 0); var c = new Vector3(x, 3, 1); @@ -204,7 +204,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var sphere = new Sphere(0.125f); //Treat each vertex as a point mass that cannot rotate. var sphereInertia = new BodyInertia { InverseMass = 1 }; - var sphereCollidable = new CollidableDescription(Simulation.Shapes.Add(sphere), 0.1f); + var sphereCollidable = new CollidableDescription(Simulation.Shapes.Add(sphere)); var a = new Vector3(x, 3, 0); var b = new Vector3(x, 4, 0); var c = new Vector3(x + 1, 3, 0); @@ -385,11 +385,11 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) } { var x = GetNextPosition(ref nextX); - var wheelShape = new CollidableDescription(Simulation.Shapes.Add(new Cylinder(1, 0.1f)), 0.1f); + var wheelShape = new CollidableDescription(Simulation.Shapes.Add(new Cylinder(1, 0.1f))); var wheelOrientation = Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), MathF.PI * 0.5f); var aDescription = BodyDescription.CreateDynamic(new RigidPose(new Vector3(x, 3, 0), wheelOrientation), inertiaA, wheelShape, activity); var bDescription = BodyDescription.CreateDynamic(new RigidPose(new Vector3(x, 6, 0), wheelOrientation), inertiaB, wheelShape, activity); - var cDescription = BodyDescription.CreateKinematic(new Vector3(x, 4.5f, -1), new CollidableDescription(Simulation.Shapes.Add(new Box(3, 6, 1)), 0.1f), activity); + var cDescription = BodyDescription.CreateKinematic(new Vector3(x, 4.5f, -1), new CollidableDescription(Simulation.Shapes.Add(new Box(3, 6, 1))), activity); var a = Simulation.Bodies.Add(aDescription); var b = Simulation.Bodies.Add(bDescription); var c = Simulation.Bodies.Add(cDescription); @@ -417,7 +417,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) }); } - Simulation.Statics.Add(new StaticDescription(new Vector3(), new CollidableDescription(Simulation.Shapes.Add(new Box(256, 1, 256)), 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(), new CollidableDescription(Simulation.Shapes.Add(new Box(256, 1, 256))))); } } } diff --git a/Demos/SpecializedTests/ConvexHullTestDemo.cs b/Demos/SpecializedTests/ConvexHullTestDemo.cs index 928a0c816..2e82e9ed0 100644 --- a/Demos/SpecializedTests/ConvexHullTestDemo.cs +++ b/Demos/SpecializedTests/ConvexHullTestDemo.cs @@ -110,15 +110,15 @@ public override void Initialize(ContentArchive content, Camera camera) var hullShapeIndex = Simulation.Shapes.Add(hullShape); hullShape.ComputeInertia(1, out var inertia); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 0, 0), inertia, new CollidableDescription(hullShapeIndex, 10.1f), new BodyActivityDescription(0.01f))); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 0, 0), inertia, new CollidableDescription(hullShapeIndex, ContinuousDetection.Discrete(20, 20)), new BodyActivityDescription(0.01f))); - Simulation.Statics.Add(new StaticDescription(new Vector3(-25, -5, 0), new CollidableDescription(Simulation.Shapes.Add(new Sphere(2)), 0.1f))); - Simulation.Statics.Add(new StaticDescription(new Vector3(-20, -5, 0), new CollidableDescription(Simulation.Shapes.Add(new Capsule(0.5f, 2)), 0.1f))); - Simulation.Statics.Add(new StaticDescription(new Vector3(-15, -5, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(2f, 2f, 2f)), 0.1f))); - Simulation.Statics.Add(new StaticDescription(new Vector3(-10, -5, 5), new CollidableDescription(Simulation.Shapes.Add(new Triangle { A = new Vector3(0, 0, -10), B = new Vector3(5, 0, -10), C = new Vector3(0, 0, -5) }), 0.1f))); - Simulation.Statics.Add(new StaticDescription(new Vector3(-5, -5, 0), new CollidableDescription(Simulation.Shapes.Add(new Cylinder(1, 1)), 0.1f))); - Simulation.Statics.Add(new StaticDescription(new Vector3(-5, -5, 5), new CollidableDescription(Simulation.Shapes.Add(new Cylinder(1, 1)), 0.1f))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5, 0), new CollidableDescription(hullShapeIndex, 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(-25, -5, 0), new CollidableDescription(Simulation.Shapes.Add(new Sphere(2))))); + Simulation.Statics.Add(new StaticDescription(new Vector3(-20, -5, 0), new CollidableDescription(Simulation.Shapes.Add(new Capsule(0.5f, 2))))); + Simulation.Statics.Add(new StaticDescription(new Vector3(-15, -5, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(2f, 2f, 2f))))); + Simulation.Statics.Add(new StaticDescription(new Vector3(-10, -5, 5), new CollidableDescription(Simulation.Shapes.Add(new Triangle { A = new Vector3(0, 0, -10), B = new Vector3(5, 0, -10), C = new Vector3(0, 0, -5) })))); + Simulation.Statics.Add(new StaticDescription(new Vector3(-5, -5, 0), new CollidableDescription(Simulation.Shapes.Add(new Cylinder(1, 1))))); + Simulation.Statics.Add(new StaticDescription(new Vector3(-5, -5, 5), new CollidableDescription(Simulation.Shapes.Add(new Cylinder(1, 1))))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5, 0), new CollidableDescription(hullShapeIndex))); var spacing = new Vector3(3f, 3f, 3); int width = 16; @@ -133,11 +133,11 @@ public override void Initialize(ContentArchive content, Camera camera) { Simulation.Bodies.Add(BodyDescription.CreateDynamic( new RigidPose(origin + spacing * new Vector3(i, j, k), BepuUtilities.QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathHelper.Pi * 0.05f)), - inertia, new CollidableDescription(hullShapeIndex, 1f), new BodyActivityDescription(0.01f))); + inertia, new CollidableDescription(hullShapeIndex), new BodyActivityDescription(0.01f))); } } } - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -10, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(1000, 1, 1000)), 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -10, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(1000, 1, 1000))))); } void TestConvexHullCreation() diff --git a/Demos/SpecializedTests/CylinderTestDemo.cs b/Demos/SpecializedTests/CylinderTestDemo.cs index 143dd131e..dc1bf4204 100644 --- a/Demos/SpecializedTests/CylinderTestDemo.cs +++ b/Demos/SpecializedTests/CylinderTestDemo.cs @@ -174,7 +174,7 @@ public override void Initialize(ContentArchive content, Camera camera) var cylinderShape = new Cylinder(1f, .2f); cylinderShape.ComputeInertia(1, out var cylinderInertia); - var cylinder = BodyDescription.CreateDynamic(new Vector3(10f, 3, 0), cylinderInertia, new CollidableDescription(Simulation.Shapes.Add(cylinderShape), 1000), new BodyActivityDescription(0.01f)); + var cylinder = BodyDescription.CreateDynamic(new Vector3(10f, 3, 0), cylinderInertia, new CollidableDescription(Simulation.Shapes.Add(cylinderShape), ContinuousDetection.CreatePassive(1000f, 1000f)), new BodyActivityDescription(0.01f)); Simulation.Bodies.Add(cylinder); Simulation.Bodies.Add(BodyDescription.CreateConvexKinematic(new RigidPose(new Vector3(0, -6, 0), QuaternionEx.CreateFromAxisAngle(Vector3.Normalize(new Vector3(1, 0, 1)), MathHelper.PiOver4)), Simulation.Shapes, new Sphere(2))); Simulation.Bodies.Add(BodyDescription.CreateConvexKinematic(new RigidPose(new Vector3(7, -6, 0), QuaternionEx.CreateFromAxisAngle(Vector3.Normalize(new Vector3(1, 0, 1)), MathHelper.PiOver4)), Simulation.Shapes, new Capsule(0.5f, 1f))); @@ -221,16 +221,7 @@ public override void Initialize(ContentArchive content, Camera camera) for (int k = 0; k < length; ++k) { var location = new Vector3(5, 3, 5) * new Vector3(i, j, k) + new Vector3(-width * 1.5f, 2.5f, -30 - length * 1.5f); - var bodyDescription = new BodyDescription - { - Activity = new BodyActivityDescription(-0.01f), - Pose = new(location), - Collidable = - { - Continuity = new ContinuousDetectionSettings { Mode = ContinuousDetectionMode.Discrete }, - SpeculativeMargin = 0.1f - } - }; + var bodyDescription = BodyDescription.CreateDynamic(location, default, new CollidableDescription(default), new BodyActivityDescription(-0.01f)); switch (j % 4) { case 0: @@ -269,7 +260,7 @@ public override void Initialize(ContentArchive content, Camera camera) return new Vector3(x, octave0 + octave1 + octave2 + octave3 + octave4, y); }, new Vector3(4, 1, 4), BufferPool, out var planeMesh); Simulation.Statics.Add(new StaticDescription(new Vector3(-100, -15, 100), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), - new CollidableDescription(Simulation.Shapes.Add(planeMesh), 0.1f))); + new CollidableDescription(Simulation.Shapes.Add(planeMesh)))); //Simulation.Statics.Add(new StaticDescription(new Vector3(0, -10, 0), Quaternion.CreateFromAxisAngle(Vector3.Normalize(new Vector3(1, 0, 1)), 0), Simulation.Shapes.Add(new Cylinder(100, 1f)), 0.1f)); diff --git a/Demos/SpecializedTests/GyroscopeTestDemo.cs b/Demos/SpecializedTests/GyroscopeTestDemo.cs index b546bcaa5..6cea4d871 100644 --- a/Demos/SpecializedTests/GyroscopeTestDemo.cs +++ b/Demos/SpecializedTests/GyroscopeTestDemo.cs @@ -63,7 +63,7 @@ public override void Initialize(ContentArchive content, Camera camera) //Note the lack of damping- we want the gyroscope to keep spinning. Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new GyroscopicIntegratorCallbacks(new Vector3(0, -10, 0), 0f, 0f), new SubsteppingTimestepper(4), 2); - Simulation.Statics.Add(new StaticDescription(new Vector3(), new CollidableDescription(Simulation.Shapes.Add(new Box(100, 1, 100)), 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(), new CollidableDescription(Simulation.Shapes.Add(new Box(100, 1, 100))))); var gyroBaseBody = Simulation.Bodies.Add(BodyDescription.CreateConvexKinematic(new Vector3(0, 2, 0), Simulation.Shapes, new Box(.1f, 4, .1f))); var gyroSpinnerBody = Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(2, 4, 0), new BodyVelocity(default, new Vector3(300, 0, 0)), 1, Simulation.Shapes, new Box(0.1f, 1f, 1f))); @@ -77,7 +77,7 @@ public override void Initialize(ContentArchive content, Camera camera) builder.Dispose(); var dzhanibekovShape = Simulation.Shapes.Add(new Compound(children)); var dzhanibekovSpinnerBody = Simulation.Bodies.Add( - BodyDescription.CreateDynamic(new Vector3(6, 4, 0), new BodyVelocity(new Vector3(0, 0, 1), new Vector3(5, .001f, .001f)), inertia, new CollidableDescription(dzhanibekovShape, 0.1f), new BodyActivityDescription(0.01f))); + BodyDescription.CreateDynamic(new Vector3(6, 4, 0), new BodyVelocity(new Vector3(0, 0, 1), new Vector3(5, .001f, .001f)), inertia, new CollidableDescription(dzhanibekovShape), new BodyActivityDescription(0.01f))); var dzhanibekovBaseBody = Simulation.Bodies.Add(BodyDescription.CreateConvexKinematic(new Vector3(6, 1, 0), Simulation.Shapes, new Box(.1f, 2, .1f))); Simulation.Solver.Add(dzhanibekovBaseBody, dzhanibekovSpinnerBody, new BallSocket { LocalOffsetA = new Vector3(0, 3, 0), LocalOffsetB = new Vector3(0, 0, 0), SpringSettings = new SpringSettings(30, 1) }); } diff --git a/Demos/SpecializedTests/Media/BedsheetDemo.cs b/Demos/SpecializedTests/Media/BedsheetDemo.cs index 55191425e..33bb26bb2 100644 --- a/Demos/SpecializedTests/Media/BedsheetDemo.cs +++ b/Demos/SpecializedTests/Media/BedsheetDemo.cs @@ -25,7 +25,7 @@ public class BedsheetDemo : Demo var description = new BodyDescription { Activity = new BodyActivityDescription(0.01f), - Collidable = new CollidableDescription(Simulation.Shapes.Add(new Sphere(bodyRadius)), 0.1f), + Collidable = new CollidableDescription(Simulation.Shapes.Add(new Sphere(bodyRadius))), LocalInertia = default, Pose = new RigidPose(default, orientation) }; @@ -130,12 +130,12 @@ bool FullyDynamic(int rowIndex, int columnIndex, int width, int height) - Simulation.Statics.Add(new StaticDescription(new Vector3(0, 10, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(80, 20, 80)), 0.1f))); - Simulation.Statics.Add(new StaticDescription(new Vector3(-20, 22, 30), new CollidableDescription(Simulation.Shapes.Add(new Box(34, 4, 14)), 0.1f))); - Simulation.Statics.Add(new StaticDescription(new Vector3(20, 22, 30), new CollidableDescription(Simulation.Shapes.Add(new Box(34, 4, 14)), 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 10, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(80, 20, 80))))); + Simulation.Statics.Add(new StaticDescription(new Vector3(-20, 22, 30), new CollidableDescription(Simulation.Shapes.Add(new Box(34, 4, 14))))); + Simulation.Statics.Add(new StaticDescription(new Vector3(20, 22, 30), new CollidableDescription(Simulation.Shapes.Add(new Box(34, 4, 14))))); - Simulation.Statics.Add(new StaticDescription(new Vector3(65.5f, 8f, 20), new CollidableDescription(Simulation.Shapes.Add(new Cylinder(15, 15)), 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(65.5f, 8f, 20), new CollidableDescription(Simulation.Shapes.Add(new Cylinder(15, 15))))); { @@ -152,7 +152,7 @@ bool FullyDynamic(int rowIndex, int columnIndex, int width, int height) CreateAreaConstraints(handles, new SpringSettings(30, 1)); } - Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(400, 1, 400)), 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(400, 1, 400))))); } diff --git a/Demos/SpecializedTests/Media/ColosseumVideoDemo.cs b/Demos/SpecializedTests/Media/ColosseumVideoDemo.cs index 3b900e5ca..d708ee130 100644 --- a/Demos/SpecializedTests/Media/ColosseumVideoDemo.cs +++ b/Demos/SpecializedTests/Media/ColosseumVideoDemo.cs @@ -76,7 +76,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var ringBoxShape = new Box(0.5f, 1.5f, 3); ringBoxShape.ComputeInertia(1, out var ringBoxInertia); var boxDescription = BodyDescription.CreateDynamic(new Vector3(), ringBoxInertia, - new CollidableDescription(Simulation.Shapes.Add(ringBoxShape), 0.1f), + new CollidableDescription(Simulation.Shapes.Add(ringBoxShape)), new BodyActivityDescription(0.01f)); var layerPosition = new Vector3(); @@ -96,15 +96,15 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) } Console.WriteLine($"box count: {Simulation.Bodies.ActiveSet.Count}"); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(500, 1, 500)), 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(500, 1, 500))))); var bulletShape = new Sphere(0.5f); bulletShape.ComputeInertia(.1f, out var bulletInertia); - bulletDescription = BodyDescription.CreateDynamic(new Vector3(), bulletInertia, new CollidableDescription(Simulation.Shapes.Add(bulletShape), 10), new BodyActivityDescription(0.01f)); + bulletDescription = BodyDescription.CreateDynamic(new Vector3(), bulletInertia, new CollidableDescription(Simulation.Shapes.Add(bulletShape)), new BodyActivityDescription(0.01f)); var shootiePatootieShape = new Sphere(3f); shootiePatootieShape.ComputeInertia(1000, out var shootiePatootieInertia); - shootiePatootieDescription = BodyDescription.CreateDynamic(new Vector3(), shootiePatootieInertia, new CollidableDescription(Simulation.Shapes.Add(shootiePatootieShape), 10), new BodyActivityDescription(0.01f)); + shootiePatootieDescription = BodyDescription.CreateDynamic(new Vector3(), shootiePatootieInertia, new CollidableDescription(Simulation.Shapes.Add(shootiePatootieShape)), new BodyActivityDescription(0.01f)); } bool characterActive; diff --git a/Demos/SpecializedTests/Media/NewtDemandingSacrificeVideoDemo.cs b/Demos/SpecializedTests/Media/NewtDemandingSacrificeVideoDemo.cs index 3454d1aec..334e23ff5 100644 --- a/Demos/SpecializedTests/Media/NewtDemandingSacrificeVideoDemo.cs +++ b/Demos/SpecializedTests/Media/NewtDemandingSacrificeVideoDemo.cs @@ -22,17 +22,17 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) filters = new CollidableProperty(BufferPool); Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks() { CollisionFilters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionLastTimestepper()); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(1500, 1, 1500)), 0.1f))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, 10, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(70, 20, 80)), 0.1f))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, 7.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(80, 15, 90)), 0.1f))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, 5, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(90, 10, 100)), 0.1f))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, 2.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(100, 5, 110)), 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(1500, 1, 1500))))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 10, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(70, 20, 80))))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 7.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(80, 15, 90))))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 5, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(90, 10, 100))))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 2.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(100, 5, 110))))); //High fidelity simulation isn't super important on this one. Simulation.Solver.IterationCount = 2; DemoMeshHelper.LoadModel(content, BufferPool, "Content\\newt.obj", new Vector3(30), out var mesh); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, 20, 0), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, 0), new CollidableDescription(Simulation.Shapes.Add(mesh), 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 20, 0), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, 0), new CollidableDescription(Simulation.Shapes.Add(mesh)))); } Random random = new Random(5); diff --git a/Demos/SpecializedTests/Media/NewtVideoDemo.cs b/Demos/SpecializedTests/Media/NewtVideoDemo.cs index fa5e42e58..43622427b 100644 --- a/Demos/SpecializedTests/Media/NewtVideoDemo.cs +++ b/Demos/SpecializedTests/Media/NewtVideoDemo.cs @@ -41,15 +41,15 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 100, -.5f), 10, Simulation.Shapes, new Sphere(5))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(1500, 1, 1500)), 0.1f))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -1.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Sphere(3)), 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(1500, 1, 1500))))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -1.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Sphere(3))))); var bulletShape = new Sphere(0.5f); bulletShape.ComputeInertia(.25f, out var bulletInertia); - bulletDescription = BodyDescription.CreateDynamic(RigidPose.Identity, bulletInertia, new CollidableDescription(Simulation.Shapes.Add(bulletShape), 1f), new BodyActivityDescription(0.01f)); + bulletDescription = BodyDescription.CreateDynamic(RigidPose.Identity, bulletInertia, new CollidableDescription(Simulation.Shapes.Add(bulletShape)), new BodyActivityDescription(0.01f)); DemoMeshHelper.LoadModel(content, BufferPool, "Content\\newt.obj", new Vector3(20), out var mesh); - Simulation.Statics.Add(new StaticDescription(new Vector3(200, 0.5f, 120), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, -3 * MathHelper.PiOver4), new CollidableDescription(Simulation.Shapes.Add(mesh), 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(200, 0.5f, 120), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, -3 * MathHelper.PiOver4), new CollidableDescription(Simulation.Shapes.Add(mesh)))); } BodyDescription bulletDescription; public override void Update(Window window, Camera camera, Input input, float dt) diff --git a/Demos/SpecializedTests/Media/PyramidVideoDemo.cs b/Demos/SpecializedTests/Media/PyramidVideoDemo.cs index 7bd17fb34..7727a58d1 100644 --- a/Demos/SpecializedTests/Media/PyramidVideoDemo.cs +++ b/Demos/SpecializedTests/Media/PyramidVideoDemo.cs @@ -41,13 +41,13 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) (rowIndex + 0.5f) * boxShape.Height, (pyramidIndex - pyramidCount * 0.5f) * (boxShape.Length + 4)), boxInertia, - new CollidableDescription(boxIndex, 0.1f), + new CollidableDescription(boxIndex), new BodyActivityDescription(0.01f))); } } } - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(2500, 1, 2500)), 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(2500, 1, 2500))))); } //We'll randomize the size of bullets. diff --git a/Demos/SpecializedTests/Media/ShrinkwrappedNewtsVideoDemo.cs b/Demos/SpecializedTests/Media/ShrinkwrappedNewtsVideoDemo.cs index 6d713144f..0cf2ada01 100644 --- a/Demos/SpecializedTests/Media/ShrinkwrappedNewtsVideoDemo.cs +++ b/Demos/SpecializedTests/Media/ShrinkwrappedNewtsVideoDemo.cs @@ -46,13 +46,13 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Simulation.Bodies.Add(bodyDescription); } - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -30, 250), new CollidableDescription(Simulation.Shapes.Add(new Box(1000, 60, 500)), 0.1f))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -60, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(1000, 1, 1000)), 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -30, 250), new CollidableDescription(Simulation.Shapes.Add(new Box(1000, 60, 500))))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -60, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(1000, 1, 1000))))); DemoMeshHelper.LoadModel(content, BufferPool, "Content\\newt.obj", new Vector3(1, 1.5f, 1), out mesh); - Simulation.Statics.Add(new StaticDescription(new Vector3(30, 0, 20), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, -3 * MathHelper.PiOver4), new CollidableDescription(Simulation.Shapes.Add(mesh), 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(30, 0, 20), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, -3 * MathHelper.PiOver4), new CollidableDescription(Simulation.Shapes.Add(mesh)))); } Mesh mesh; @@ -62,7 +62,7 @@ public override void Update(Window window, Camera camera, Input input, float dt) if(input.WasPushed(OpenTK.Input.Key.Z)) { mesh.Scale = new Vector3(30); - Simulation.Statics.Add(new StaticDescription(new Vector3(70, 0, 50), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, -3.1f * MathHelper.PiOver4), new CollidableDescription(Simulation.Shapes.Add(mesh), 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(70, 0, 50), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, -3.1f * MathHelper.PiOver4), new CollidableDescription(Simulation.Shapes.Add(mesh)))); } base.Update(window, camera, input, dt); } diff --git a/Demos/SpecializedTests/MeshMeshTestDemo.cs b/Demos/SpecializedTests/MeshMeshTestDemo.cs index 375adaf6c..e75c735af 100644 --- a/Demos/SpecializedTests/MeshMeshTestDemo.cs +++ b/Demos/SpecializedTests/MeshMeshTestDemo.cs @@ -30,7 +30,7 @@ public override void Initialize(ContentArchive content, Camera camera) { Simulation.Bodies.Add( BodyDescription.CreateDynamic(new Vector3(0, 2 + meshIndex * 2, 0), approximateInertia, - new CollidableDescription(meshShapeIndex, 0.1f), new BodyActivityDescription(0.01f))); + new CollidableDescription(meshShapeIndex), new BodyActivityDescription(0.01f))); } var compoundBuilder = new CompoundBuilder(BufferPool, Simulation.Shapes, 12); @@ -44,13 +44,13 @@ public override void Initialize(ContentArchive content, Camera camera) compoundBuilder.Dispose(); for (int i = 0; i < 3; ++i) { - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(5, 2 + i * 2, 0), compoundInertia, new CollidableDescription(compoundShapeIndex, 0.1f), new BodyActivityDescription(0.01f))); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(5, 2 + i * 2, 0), compoundInertia, new CollidableDescription(compoundShapeIndex), new BodyActivityDescription(0.01f))); } var staticShape = new Box(1500, 1, 1500); var staticShapeIndex = Simulation.Shapes.Add(staticShape); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(staticShapeIndex, 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(staticShapeIndex))); } } } diff --git a/Demos/SpecializedTests/MeshReductionTestDemo.cs b/Demos/SpecializedTests/MeshReductionTestDemo.cs index e147d0dc0..284175aa3 100644 --- a/Demos/SpecializedTests/MeshReductionTestDemo.cs +++ b/Demos/SpecializedTests/MeshReductionTestDemo.cs @@ -68,23 +68,23 @@ public override void Initialize(ContentArchive content, Camera camera) }, new Vector3(1, 1, 1), BufferPool, out var planeMesh); Simulation.Statics.Add(new StaticDescription(new Vector3(0, -15, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), - new CollidableDescription(Simulation.Shapes.Add(planeMesh), 0.1f))); + new CollidableDescription(Simulation.Shapes.Add(planeMesh)))); var testBox = new Box(3, 3, 3); testBox.ComputeInertia(1, out var testBoxInertia); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 10, 0), testBoxInertia, new CollidableDescription(Simulation.Shapes.Add(testBox), 10f), new BodyActivityDescription(-1))); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 10, 0), testBoxInertia, new CollidableDescription(Simulation.Shapes.Add(testBox), ContinuousDetection.Discrete(10, 10)), new BodyActivityDescription(-1))); var testSphere = new Sphere(.1f); testSphere.ComputeInertia(1, out var testSphereInertia); //testSphereInertia.InverseInertiaTensor = default; - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(10, 10, 0), testSphereInertia, new CollidableDescription(Simulation.Shapes.Add(testSphere), 10f), new BodyActivityDescription(-1))); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(10, 10, 0), testSphereInertia, new CollidableDescription(Simulation.Shapes.Add(testSphere), ContinuousDetection.Discrete(10, 10)), new BodyActivityDescription(-1))); var testCylinder = new Cylinder(1.5f, 2f); testCylinder.ComputeInertia(1, out var testCylinderInertia); //testCylinderInertia.InverseInertiaTensor = default; - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(15, 10, 0), testCylinderInertia, new CollidableDescription(Simulation.Shapes.Add(testCylinder), 10f), new BodyActivityDescription(-1))); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(15, 10, 0), testCylinderInertia, new CollidableDescription(Simulation.Shapes.Add(testCylinder), ContinuousDetection.Discrete(10, 10)), new BodyActivityDescription(-1))); var testCapsule = new Capsule(.1f, 2f); testCapsule.ComputeInertia(1, out var testCapsuleInertia); //testCapsuleInertia.InverseInertiaTensor = default; - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(18, 10, 0), testCapsuleInertia, new CollidableDescription(Simulation.Shapes.Add(testCapsule), 10f), new BodyActivityDescription(-1))); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(18, 10, 0), testCapsuleInertia, new CollidableDescription(Simulation.Shapes.Add(testCapsule), ContinuousDetection.Discrete(10, 10)), new BodyActivityDescription(-1))); var points = new QuickList(8, BufferPool); points.AllocateUnsafely() = new Vector3(0, 0, 0); @@ -97,7 +97,7 @@ public override void Initialize(ContentArchive content, Camera camera) points.AllocateUnsafely() = new Vector3(2, 2, 2); var convexHull = new ConvexHull(points, BufferPool, out _); convexHull.ComputeInertia(1, out var convexHullInertia); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(23, 10, 0), convexHullInertia, new CollidableDescription(Simulation.Shapes.Add(convexHull), 10f), new BodyActivityDescription(-1))); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(23, 10, 0), convexHullInertia, new CollidableDescription(Simulation.Shapes.Add(convexHull), ContinuousDetection.Discrete(10, 10)), new BodyActivityDescription(-1))); //var sphere = new Sphere(1.5f); //var capsule = new Capsule(1f, 1f); @@ -143,20 +143,7 @@ public override void Initialize(ContentArchive content, Camera camera) for (int k = 0; k < length; ++k) { var location = new Vector3(70, 35, 70) * new Vector3(i, j, k) + new Vector3(-width * 70 / 2f, 5f, -length * 70 / 2f); - var bodyDescription = new BodyDescription - { - Activity = new BodyActivityDescription(0.01f), - Pose = new RigidPose - { - Orientation = Quaternion.Identity, - Position = location - }, - Collidable = new CollidableDescription - { - Continuity = new ContinuousDetectionSettings { Mode = ContinuousDetectionMode.Discrete }, - SpeculativeMargin = 0.1f - } - }; + var bodyDescription = BodyDescription.CreateDynamic(location, default, new CollidableDescription(default), new BodyActivityDescription(0.01f)); var index = shapeCount++; switch (index % 5) { diff --git a/Demos/SpecializedTests/MeshSerializationTestDemo.cs b/Demos/SpecializedTests/MeshSerializationTestDemo.cs index 6fd44811f..09ae34fb2 100644 --- a/Demos/SpecializedTests/MeshSerializationTestDemo.cs +++ b/Demos/SpecializedTests/MeshSerializationTestDemo.cs @@ -21,7 +21,7 @@ public override void Initialize(ContentArchive content, Camera camera) var startTime = Stopwatch.GetTimestamp(); DemoMeshHelper.CreateDeformedPlane(1025, 1025, (x, y) => new Vector3(x * 0.125f, MathF.Sin(x) + MathF.Sin(y), y * 0.125f), Vector3.One, BufferPool, out var originalMesh); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), new CollidableDescription(Simulation.Shapes.Add(originalMesh), 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), new CollidableDescription(Simulation.Shapes.Add(originalMesh)))); var endTime = Stopwatch.GetTimestamp(); var freshConstructionTime = (endTime - startTime) / (double)Stopwatch.Frequency; Console.WriteLine($"Fresh construction time (ms): {freshConstructionTime * 1e3}"); @@ -34,7 +34,7 @@ public override void Initialize(ContentArchive content, Camera camera) var loadTime = (endTime - startTime) / (double)Stopwatch.Frequency; Console.WriteLine($"Load time (ms): {(endTime - startTime) * 1e3 / Stopwatch.Frequency}"); Console.WriteLine($"Relative speedup: {freshConstructionTime / loadTime}"); - Simulation.Statics.Add(new StaticDescription(new Vector3(128, 0, 0), new CollidableDescription(Simulation.Shapes.Add(loadedMesh), 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(128, 0, 0), new CollidableDescription(Simulation.Shapes.Add(loadedMesh)))); BufferPool.Return(ref serializedMeshBytes); @@ -42,7 +42,7 @@ public override void Initialize(ContentArchive content, Camera camera) var random = new Random(5); var shapeToDrop = new Box(1, 1, 1); shapeToDrop.ComputeInertia(1, out var shapeToDropInertia); - var descriptionToDrop = BodyDescription.CreateDynamic(new Vector3(), shapeToDropInertia, new CollidableDescription(Simulation.Shapes.Add(shapeToDrop), 0.1f), new BodyActivityDescription(0.01f)); + var descriptionToDrop = BodyDescription.CreateDynamic(new Vector3(), shapeToDropInertia, new CollidableDescription(Simulation.Shapes.Add(shapeToDrop)), new BodyActivityDescription(0.01f)); for (int i = 0; i < 1024; ++i) { descriptionToDrop.Pose.Position = new Vector3(8 + 240 * (float)random.NextDouble(), 10 + 10 * (float)random.NextDouble(), 8 + 112 * (float)random.NextDouble()); diff --git a/Demos/SpecializedTests/MeshTestDemo.cs b/Demos/SpecializedTests/MeshTestDemo.cs index 6c418850e..a80ea30f0 100644 --- a/Demos/SpecializedTests/MeshTestDemo.cs +++ b/Demos/SpecializedTests/MeshTestDemo.cs @@ -40,16 +40,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) for (int k = 0; k < length; ++k) { var location = new Vector3(5, 5, 5) * new Vector3(i, j, k);// + new Vector3(-width * 1.5f, 1.5f, -length * 1.5f); - var bodyDescription = new BodyDescription - { - Activity = new BodyActivityDescription { MinimumTimestepCountUnderThreshold = 32, SleepThreshold = 0.01f }, - Pose = new(location), - Collidable = new CollidableDescription - { - Continuity = new ContinuousDetectionSettings { Mode = ContinuousDetectionMode.Discrete }, - SpeculativeMargin = 0.1f - } - }; + var bodyDescription = BodyDescription.CreateDynamic(location, default, new CollidableDescription(default), new BodyActivityDescription(0.01f)); switch ((i + j) % 3) { case 0: @@ -79,15 +70,15 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) DemoMeshHelper.LoadModel(content, BufferPool, @"Content\newt.obj", new Vector3(5, 5, 5), out var newtMesh); newtMesh.ComputeClosedInertia(10, out var newtInertia, out _); Simulation.Bodies.Add(BodyDescription.CreateDynamic(new RigidPose(new Vector3(30, 20, 30)), newtInertia, - new CollidableDescription(Simulation.Shapes.Add(newtMesh), 0.1f), new BodyActivityDescription(0.01f))); + new CollidableDescription(Simulation.Shapes.Add(newtMesh)), new BodyActivityDescription(0.01f))); - Simulation.Statics.Add(new StaticDescription(new Vector3(30, 15, 30), new CollidableDescription(Simulation.Shapes.Add(new Box(15, 1, 15)), 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(30, 15, 30), new CollidableDescription(Simulation.Shapes.Add(new Box(15, 1, 15))))); DemoMeshHelper.LoadModel(content, BufferPool, @"Content\box.obj", new Vector3(5, 1, 5), out var boxMesh); - Simulation.Statics.Add(new StaticDescription(new Vector3(10, 5, -20), new CollidableDescription(Simulation.Shapes.Add(boxMesh), 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(10, 5, -20), new CollidableDescription(Simulation.Shapes.Add(boxMesh)))); DemoMeshHelper.CreateFan(64, 16, new Vector3(1, 1, 1), BufferPool, out var fanMesh); - Simulation.Statics.Add(new StaticDescription(new Vector3(-10, 0, -20), new CollidableDescription(Simulation.Shapes.Add(fanMesh), 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(-10, 0, -20), new CollidableDescription(Simulation.Shapes.Add(fanMesh)))); const int planeWidth = 128; const int planeHeight = 128; @@ -97,7 +88,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) return new Vector3(x - planeWidth / 2, 1 * MathF.Cos(x / 2f) * MathF.Sin(y / 2f), y - planeHeight / 2); }, new Vector3(2, 1, 2), BufferPool, out var planeMesh); Simulation.Statics.Add(new StaticDescription(new Vector3(64, -10, 64), BepuUtilities.QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), - new CollidableDescription(Simulation.Shapes.Add(planeMesh), 0.1f))); + new CollidableDescription(Simulation.Shapes.Add(planeMesh)))); } diff --git a/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs b/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs index d50f8609f..3e409319b 100644 --- a/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs +++ b/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs @@ -41,7 +41,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) (rowIndex + 0.5f) * boxShape.Height, (pyramidIndex - pyramidCount * 0.5f) * (boxShape.Length + 4)), boxInertia, - new CollidableDescription(boxIndex, 0.1f), + new CollidableDescription(boxIndex), new BodyActivityDescription(0.01f))); } } @@ -50,7 +50,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var staticShape = new Box(250, 1, 250); var staticShapeIndex = Simulation.Shapes.Add(staticShape); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(staticShapeIndex, 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(staticShapeIndex))); } diff --git a/Demos/SpecializedTests/RagdollTubeDemo.cs b/Demos/SpecializedTests/RagdollTubeDemo.cs index a38ea3704..b7b938bae 100644 --- a/Demos/SpecializedTests/RagdollTubeDemo.cs +++ b/Demos/SpecializedTests/RagdollTubeDemo.cs @@ -56,26 +56,13 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) builder.AddForKinematic(Simulation.Shapes.Add(new Box(1, 2, panelShape.Length)), new RigidPose(new Vector3(0, tubeRadius - 1, 0)), 0); builder.BuildKinematicCompound(out var children); var compound = new BigCompound(children, Simulation.Shapes, BufferPool); - tubeHandle = Simulation.Bodies.Add(BodyDescription.CreateKinematic(tubeCenter, new BodyVelocity(default, new Vector3(0, 0, .25f)), new CollidableDescription(Simulation.Shapes.Add(compound), 0.1f), new BodyActivityDescription())); + tubeHandle = Simulation.Bodies.Add(BodyDescription.CreateKinematic(tubeCenter, new BodyVelocity(default, new Vector3(0, 0, .25f)), new CollidableDescription(Simulation.Shapes.Add(compound)), new BodyActivityDescription())); filters[tubeHandle] = new SubgroupCollisionFilter(int.MaxValue); builder.Dispose(); var staticShape = new Box(300, 1, 300); var staticShapeIndex = Simulation.Shapes.Add(staticShape); - var staticDescription = new StaticDescription - { - Collidable = new CollidableDescription - { - Continuity = new ContinuousDetectionSettings { Mode = ContinuousDetectionMode.Discrete }, - Shape = staticShapeIndex, - SpeculativeMargin = 0.1f - }, - Pose = new RigidPose - { - Position = new Vector3(0, -0.5f, 0), - Orientation = Quaternion.Identity - } - }; + var staticDescription = new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(staticShapeIndex)); Simulation.Statics.Add(staticDescription); } diff --git a/Demos/SpecializedTests/ShapePileTestDemo.cs b/Demos/SpecializedTests/ShapePileTestDemo.cs index d969aaab2..d451872ff 100644 --- a/Demos/SpecializedTests/ShapePileTestDemo.cs +++ b/Demos/SpecializedTests/ShapePileTestDemo.cs @@ -65,20 +65,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) for (int k = 0; k < length; ++k) { var location = new Vector3(6, 3, 6) * new Vector3(i, j, k) + new Vector3(-width * 1.5f, 5.5f, -length * 1.5f); - var bodyDescription = new BodyDescription - { - Activity = new BodyActivityDescription(0.01f), - Pose = new RigidPose - { - Orientation = Quaternion.Identity, - Position = location - }, - Collidable = new CollidableDescription - { - Continuity = new ContinuousDetectionSettings { Mode = ContinuousDetectionMode.Discrete }, - SpeculativeMargin = 0.1f - } - }; + var bodyDescription = BodyDescription.CreateKinematic(location, new CollidableDescription(default), new BodyActivityDescription(0.01f)); var index = shapeCount++; switch (index % 5) { @@ -110,7 +97,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) } DemoMeshHelper.CreateDeformedPlane(128, 128, (x, y) => new Vector3(x - 64, 2f * (float)(Math.Sin(x * 0.5f) * Math.Sin(y * 0.5f)), y - 64), new Vector3(4, 1, 4), BufferPool, out var mesh); - Simulation.Statics.Add(new StaticDescription(new Vector3(), new CollidableDescription(Simulation.Shapes.Add(mesh), 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(), new CollidableDescription(Simulation.Shapes.Add(mesh)))); } } diff --git a/Demos/SpecializedTests/SolverBatchTestDemo.cs b/Demos/SpecializedTests/SolverBatchTestDemo.cs index 773c3f232..bd5fcbc7d 100644 --- a/Demos/SpecializedTests/SolverBatchTestDemo.cs +++ b/Demos/SpecializedTests/SolverBatchTestDemo.cs @@ -37,22 +37,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) for (int j = 0; j < length; ++j) { var location = new Vector3(0, 30, 0) + new Vector3(spacing, 0, spacing) * (new Vector3(i, 0, j) + new Vector3(-width * 0.5f, 0, -length * 0.5f)); - var bodyDescription = new BodyDescription - { - Activity = new BodyActivityDescription { MinimumTimestepCountUnderThreshold = 32, SleepThreshold = 0.01f }, - Pose = new RigidPose - { - Orientation = Quaternion.Identity, - Position = location - }, - Collidable = new CollidableDescription - { - Shape = clothNodeShapeIndex, - Continuity = new ContinuousDetectionSettings { Mode = ContinuousDetectionMode.Discrete }, - SpeculativeMargin = 0.1f - }, - LocalInertia = clothNodeInertia - }; + var bodyDescription = BodyDescription.CreateDynamic(location, clothNodeInertia, new CollidableDescription(clothNodeShapeIndex), new BodyActivityDescription(0.01f)); nodeHandles[i][j] = Simulation.Bodies.Add(bodyDescription); } @@ -99,41 +84,13 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var bigBallShape = new Sphere(45); var bigBallShapeIndex = Simulation.Shapes.Add(bigBallShape); - var bigBallDescription = new BodyDescription - { - Collidable = new CollidableDescription - { - Continuity = new ContinuousDetectionSettings { Mode = ContinuousDetectionMode.Discrete }, - Shape = bigBallShapeIndex, - SpeculativeMargin = 0.1f - }, - Activity = new BodyActivityDescription(0), - Pose = new RigidPose - { - Position = new Vector3(-10, -15, 0), - Orientation = Quaternion.Identity - } - }; + var bigBallDescription = BodyDescription.CreateKinematic(new Vector3(-10, -15, 0), new CollidableDescription(bigBallShapeIndex), new BodyActivityDescription(0)); bigBallHandle = Simulation.Bodies.Add(bigBallDescription); var groundShape = new Box(200, 1, 200); var groundShapeIndex = Simulation.Shapes.Add(groundShape); - var groundDescription = new BodyDescription - { - Collidable = new CollidableDescription - { - Continuity = new ContinuousDetectionSettings { Mode = ContinuousDetectionMode.Discrete }, - Shape = groundShapeIndex, - SpeculativeMargin = 0.1f - }, - Activity = new BodyActivityDescription(0), - Pose = new RigidPose - { - Position = new Vector3(0, -10, 0), - Orientation = Quaternion.Identity - } - }; + var groundDescription = BodyDescription.CreateKinematic(new Vector3(0, -10, 0), new CollidableDescription(groundShapeIndex), new BodyActivityDescription(0)); Simulation.Bodies.Add(groundDescription); } BodyHandle bigBallHandle; diff --git a/Demos/SpecializedTests/TriangleTestDemo.cs b/Demos/SpecializedTests/TriangleTestDemo.cs index 09a7f8017..4caf671ff 100644 --- a/Demos/SpecializedTests/TriangleTestDemo.cs +++ b/Demos/SpecializedTests/TriangleTestDemo.cs @@ -128,23 +128,23 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) ////bodyDescription.LocalInertia.InverseInertiaTensor = new Triangular3x3(); //Simulation.Bodies.Add(bodyDescription); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(200, 5, 200)), 0.1f))); - Simulation.Statics.Add(new StaticDescription(new Vector3(10, -2, 30), new CollidableDescription(Simulation.Shapes.Add(new Box(10, 5, 10)), 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(200, 5, 200))))); + Simulation.Statics.Add(new StaticDescription(new Vector3(10, -2, 30), new CollidableDescription(Simulation.Shapes.Add(new Box(10, 5, 10))))); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 2, 0), new BodyInertia { InverseMass = 1 }, new CollidableDescription(Simulation.Shapes.Add(new Sphere(1.75f)), 0.1f), new BodyActivityDescription(-1))); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new RigidPose(new Vector3(20, 2, 3), Quaternion.CreateFromYawPitchRoll(0f, 1.745329E-05f, 0f)), new BodyInertia { InverseMass = 1 }, new CollidableDescription(Simulation.Shapes.Add(new Capsule(1, 2)), 0.1f), new BodyActivityDescription(-1))); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 2, 0), new BodyInertia { InverseMass = 1 }, new CollidableDescription(Simulation.Shapes.Add(new Sphere(1.75f)), ContinuousDetection.Discrete(0.1f, 0.1f)), new BodyActivityDescription(-1))); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new RigidPose(new Vector3(20, 2, 3), Quaternion.CreateFromYawPitchRoll(0f, 1.745329E-05f, 0f)), new BodyInertia { InverseMass = 1 }, new CollidableDescription(Simulation.Shapes.Add(new Capsule(1, 2)), ContinuousDetection.Discrete(0.1f, 0.1f)), new BodyActivityDescription(-1))); var testBox = new Box(2, 3, 2); testBox.ComputeInertia(1, out var testBoxInertia); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 2, 6), testBoxInertia, new CollidableDescription(Simulation.Shapes.Add(testBox), 10.1f), new BodyActivityDescription(-1))); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 2, 6), testBoxInertia, new CollidableDescription(Simulation.Shapes.Add(testBox), ContinuousDetection.Discrete(10.1f, 10.1f)), new BodyActivityDescription(-1))); var cylinder = new Cylinder(1.75f, 2); cylinder.ComputeInertia(1, out var cylinderInertia); //cylinderInertia.InverseInertiaTensor = default; - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new RigidPose(new Vector3(20, 2, 9), Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), MathF.PI / 2f)), cylinderInertia, new CollidableDescription(Simulation.Shapes.Add(cylinder), 5f), new BodyActivityDescription(-1))); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new RigidPose(new Vector3(20, 2, 9), Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), MathF.PI / 2f)), cylinderInertia, new CollidableDescription(Simulation.Shapes.Add(cylinder), ContinuousDetection.Discrete(5f, 5f)), new BodyActivityDescription(-1))); var cylinder2 = new Cylinder(.5f, 0.5f); cylinder2.ComputeInertia(1, out var cylinder2Inertia); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new RigidPose(new Vector3(23, 2, 9), Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), 0)), cylinder2Inertia, new CollidableDescription(Simulation.Shapes.Add(cylinder2), 5f), new BodyActivityDescription(-1))); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new RigidPose(new Vector3(23, 2, 9), Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), 0)), cylinder2Inertia, new CollidableDescription(Simulation.Shapes.Add(cylinder2), ContinuousDetection.Discrete(5f, 5f)), new BodyActivityDescription(-1))); var points = new QuickList(8, BufferPool); points.AllocateUnsafely() = new Vector3(0, 0, 0); points.AllocateUnsafely() = new Vector3(0, 0, 2); @@ -156,8 +156,8 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) points.AllocateUnsafely() = new Vector3(2, 2, 2); var convexHull = new ConvexHull(points, BufferPool, out _); convexHull.ComputeInertia(1, out var convexHullInertia); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 2, 12), convexHullInertia, new CollidableDescription(Simulation.Shapes.Add(convexHull), 0.1f), new BodyActivityDescription(-1))); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(23, 2, 12), convexHullInertia, new CollidableDescription(Simulation.Shapes.Add(convexHull), 0.1f), new BodyActivityDescription(-1))); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 2, 12), convexHullInertia, new CollidableDescription(Simulation.Shapes.Add(convexHull), ContinuousDetection.Discrete(0.1f, 0.1f)), new BodyActivityDescription(-1))); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(23, 2, 12), convexHullInertia, new CollidableDescription(Simulation.Shapes.Add(convexHull), ContinuousDetection.Discrete(0.1f, 0.1f)), new BodyActivityDescription(-1))); CompoundBuilder builder = new CompoundBuilder(BufferPool, Simulation.Shapes, 2); builder.Add(new Box(1, 1, 1), RigidPose.Identity, 1); @@ -165,7 +165,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) builder.BuildDynamicCompound(out var children, out var compoundInertia); //compoundInertia.InverseInertiaTensor = default; var compound = new Compound(children); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 3, 14), compoundInertia, new CollidableDescription(Simulation.Shapes.Add(compound), 10.1f), new BodyActivityDescription(-1))); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 3, 14), compoundInertia, new CollidableDescription(Simulation.Shapes.Add(compound), ContinuousDetection.Discrete(10.1f, 10.1f)), new BodyActivityDescription(-1))); { var triangles = new QuickList(4, BufferPool); @@ -178,14 +178,14 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) triangles.AllocateUnsafely() = new Triangle { A = v0, B = v2, C = v1 }; triangles.AllocateUnsafely() = new Triangle { A = v1, B = v2, C = v3 }; var testMesh = new Mesh(triangles, Vector3.One, BufferPool); - Simulation.Statics.Add(new StaticDescription(new Vector3(30, -2.5f, 0), new CollidableDescription(Simulation.Shapes.Add(testMesh), 10.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(30, -2.5f, 0), new CollidableDescription(Simulation.Shapes.Add(testMesh), ContinuousDetection.Discrete(10.1f, 10.1f)))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -2.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Triangle(v2, v0, v1)), 10.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -2.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Triangle(v2, v0, v1)), ContinuousDetection.Discrete(10.1f, 10.1f)))); } DemoMeshHelper.LoadModel(content, BufferPool, "Content\\newt.obj", new Vector3(3), out var mesh); - var collidable = new CollidableDescription(Simulation.Shapes.Add(mesh), 2f); + var collidable = new CollidableDescription(Simulation.Shapes.Add(mesh), ContinuousDetection.Discrete(2f, 2f)); mesh.ComputeClosedInertia(1, out var newtInertia); for (int i = 0; i < 5; ++i) { @@ -201,7 +201,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) triangles.AllocateUnsafely() = new Triangle { A = v0, B = v2, C = v1 }; triangles.AllocateUnsafely() = new Triangle { A = v1, B = v2, C = v3 }; var testMesh = new Mesh(triangles, Vector3.One, BufferPool); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(22, -2.5f, 0), new BodyInertia { InverseMass = 1 }, new CollidableDescription(Simulation.Shapes.Add(testMesh), 10.1f), new BodyActivityDescription(-1f))); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(22, -2.5f, 0), new BodyInertia { InverseMass = 1 }, new CollidableDescription(Simulation.Shapes.Add(testMesh), ContinuousDetection.Discrete(10.1f, 10.1f)), new BodyActivityDescription(-1f))); } } } diff --git a/Demos/SpecializedTests/VolumeQueryTests.cs b/Demos/SpecializedTests/VolumeQueryTests.cs index 57be6f356..c753ea411 100644 --- a/Demos/SpecializedTests/VolumeQueryTests.cs +++ b/Demos/SpecializedTests/VolumeQueryTests.cs @@ -92,39 +92,12 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) if ((i + j + k) % 2 == 1) { - var bodyDescription = new BodyDescription - { - Activity = new BodyActivityDescription { MinimumTimestepCountUnderThreshold = 32, SleepThreshold = -0.1f }, - Pose = new RigidPose - { - Orientation = orientation, - Position = location - }, - Collidable = new CollidableDescription - { - Continuity = new ContinuousDetectionSettings { Mode = ContinuousDetectionMode.Discrete }, - SpeculativeMargin = 0.1f, - Shape = shapeIndex - } - }; + var bodyDescription = BodyDescription.CreateKinematic(new RigidPose(location, orientation), new CollidableDescription(shapeIndex), new BodyActivityDescription(-1)); Simulation.Bodies.Add(bodyDescription); } else { - var staticDescription = new StaticDescription - { - Pose = new RigidPose - { - Orientation = orientation, - Position = location - }, - Collidable = new CollidableDescription - { - Continuity = new ContinuousDetectionSettings { Mode = ContinuousDetectionMode.Discrete }, - SpeculativeMargin = 0.1f, - Shape = shapeIndex - } - }; + var staticDescription = new StaticDescription(location, orientation, new CollidableDescription(shapeIndex)); Simulation.Statics.Add(staticDescription); } From 5c35b474fbd689b88abf158e8ba263d038f82bb2 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 9 Nov 2021 20:13:56 -0600 Subject: [PATCH 278/947] In the middle of a bounding box batcher readaptation. --- BepuPhysics/BoundingBoxHelpers.cs | 32 ++++---- BepuPhysics/Collidables/BoundingBoxBatcher.cs | 78 +++++++++++-------- BepuPhysics/PoseIntegrator.cs | 1 - 3 files changed, 62 insertions(+), 49 deletions(-) diff --git a/BepuPhysics/BoundingBoxHelpers.cs b/BepuPhysics/BoundingBoxHelpers.cs index aa054b491..3fe5bd6c0 100644 --- a/BepuPhysics/BoundingBoxHelpers.cs +++ b/BepuPhysics/BoundingBoxHelpers.cs @@ -11,8 +11,8 @@ namespace BepuPhysics public static class BoundingBoxHelpers { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void GetAngularBoundsExpansion(in Vector angularSpeed, in Vector vectorDt, in Vector maximumRadius, - in Vector maximumAngularExpansion, out Vector angularExpansion) + public static Vector GetAngularBoundsExpansion(Vector angularSpeed, Vector vectorDt, Vector maximumRadius, + Vector maximumAngularExpansion) { /* Angular requires a bit more care. Since the goal is to create a tight bound, simply using a v = w * r approximation isn't ideal. A slightly tighter can be found: @@ -42,7 +42,7 @@ An extra few dozen ALU cycles is unlikely to meaningfully change the execution t var cosAngleMinusOne = a2 * new Vector(-1f / 2f) + a4 * new Vector(1f / 24f) - a6 * new Vector(1f / 720f); //Note that it's impossible for angular motion to cause an increase in bounding box size beyond (maximumRadius-minimumRadius) on any given axis. //That value, or a conservative approximation, is stored as the maximum angular expansion. - angularExpansion = Vector.Min(maximumAngularExpansion, + return Vector.Min(maximumAngularExpansion, Vector.SquareRoot(new Vector(-2f) * maximumRadius * maximumRadius * cosAngleMinusOne)); } @@ -100,17 +100,16 @@ There are ways to address this- all of which are a bit expensive- but CCD as imp Vector3Wide.Min(zero, linearDisplacement, out minExpansion); Vector3Wide.Max(zero, linearDisplacement, out maxExpansion); Vector3Wide.Length(angularVelocity, out var angularSpeed); - GetAngularBoundsExpansion(angularSpeed, vectorDt, maximumRadius, maximumAngularExpansion, out var angularExpansion); + var angularExpansion = GetAngularBoundsExpansion(angularSpeed, vectorDt, maximumRadius, maximumAngularExpansion); Vector3Wide.Subtract(minExpansion, angularExpansion, out minExpansion); Vector3Wide.Add(maxExpansion, angularExpansion, out maxExpansion); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ExpandBoundingBoxes(ref Vector3Wide min, ref Vector3Wide max, ref BodyVelocityWide velocities, float dt, - ref Vector maximumRadius, ref Vector maximumAngularExpansion, ref Vector maximumExpansion) + public static void ExpandBoundingBoxes(ref Vector3Wide min, ref Vector3Wide max, BodyVelocityWide velocities, float dt, + Vector maximumRadius, Vector maximumAngularExpansion, Vector maximumExpansion) { GetBoundsExpansion(velocities.Linear, velocities.Angular, dt, maximumRadius, maximumAngularExpansion, out var minDisplacement, out var maxDisplacement); - //The maximum expansion passed into this function is the speculative margin for discrete mode collidables, and ~infinity for passive or continuous ones. Vector3Wide.Max(-maximumExpansion, minDisplacement, out minDisplacement); Vector3Wide.Min(maximumExpansion, maxDisplacement, out maxDisplacement); @@ -121,8 +120,8 @@ public static void ExpandBoundingBoxes(ref Vector3Wide min, ref Vector3Wide max, //This is simply a internally vectorized version of the above. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void GetAngularBoundsExpansion(in Vector3 angularVelocity, float dt, - float maximumRadius, float maximumAngularExpansion, out Vector3 expansion) + public static Vector3 GetAngularBoundsExpansion(Vector3 angularVelocity, float dt, + float maximumRadius, float maximumAngularExpansion) { var angularVelocityMagnitude = angularVelocity.Length(); var a = MathHelper.Min(angularVelocityMagnitude * dt, MathHelper.Pi / 3f); @@ -132,8 +131,7 @@ public static void GetAngularBoundsExpansion(in Vector3 angularVelocity, float d var cosAngleMinusOne = a2 * (-1f / 2f) + a4 * (1f / 24f) - a6 * (1f / 720f); //Note that it's impossible for angular motion to cause an increase in bounding box size beyond (maximumRadius-minimumRadius) on any given axis. //That value, or a conservative approximation, is stored as the maximum angular expansion. - expansion = new Vector3(MathHelper.Min(maximumAngularExpansion, - (float)Math.Sqrt(-2f * maximumRadius * maximumRadius * cosAngleMinusOne))); + return new Vector3(MathHelper.Min(maximumAngularExpansion, (float)Math.Sqrt(-2f * maximumRadius * maximumRadius * cosAngleMinusOne))); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -144,7 +142,7 @@ public static void GetBoundsExpansion(in Vector3 linearVelocity, in Vector3 angu Vector3 zero = default; minExpansion = Vector3.Min(zero, linearDisplacement); maxExpansion = Vector3.Max(zero, linearDisplacement); - GetAngularBoundsExpansion(angularVelocity, dt, maximumRadius, maximumAngularExpansion, out var angularExpansion); + var angularExpansion = GetAngularBoundsExpansion(angularVelocity, dt, maximumRadius, maximumAngularExpansion); var maximumAllowedExpansionBroadcasted = new Vector3(maximumAllowedExpansion); minExpansion = Vector3.Max(-maximumAllowedExpansionBroadcasted, minExpansion - angularExpansion); @@ -189,7 +187,7 @@ public static unsafe void ExpandLocalBoundingBoxes(ref Vector3Wide min, ref Vect Vector3Wide.Length(localPositionA, out var radiusB); Vector3Wide.Length(localRelativeLinearVelocityA, out var linearSpeed); var worstCaseRadius = linearSpeed * dt + radiusB; - GetAngularBoundsExpansion(Vector.SquareRoot(angularSpeedBSquared), maximumRadius + worstCaseRadius, maximumAngularExpansion + worstCaseRadius, new Vector(dt), out var angularExpansionB); + var angularExpansionB = GetAngularBoundsExpansion(Vector.SquareRoot(angularSpeedBSquared), maximumRadius + worstCaseRadius, maximumAngularExpansion + worstCaseRadius, new Vector(dt)); Vector3Wide.Subtract(minExpansion, angularExpansionB, out minExpansion); Vector3Wide.Add(maxExpansion, angularExpansionB, out maxExpansion); } @@ -232,11 +230,11 @@ public static unsafe void GetLocalBoundingBoxForSweep(TypedIndex shapeIndex, Sha shapes[shapeIndex.Type].ComputeBounds(shapeIndex.Index, poseARotatedIntoBLocalSpace.Orientation, out var maximumRadiusA, out var maximumAngularExpansionA, out min, out max); //Object A could rotate around its center. var worstCaseRadiusA = shapePoseLocalToA.Position.Length(); - GetAngularBoundsExpansion(velocityA.Angular, dt, worstCaseRadiusA + maximumRadiusA, worstCaseRadiusA + maximumAngularExpansionA, out var angularExpansionA); + var angularExpansionA = GetAngularBoundsExpansion(velocityA.Angular, dt, worstCaseRadiusA + maximumRadiusA, worstCaseRadiusA + maximumAngularExpansionA); //Rotation of object B could induce an arc in object A. //The furthest the convex can be from the compound local origin is no further than the sweep pushing it directly away from the compound, while rotation swings A's local pose away. var worstCaseRadiusB = sweep.Length() + localOffsetB.Length() + worstCaseRadiusA; - GetAngularBoundsExpansion(velocityB.Angular, dt, worstCaseRadiusB + maximumRadiusA, worstCaseRadiusB + maximumAngularExpansionA, out var angularExpansionB); + var angularExpansionB = GetAngularBoundsExpansion(velocityB.Angular, dt, worstCaseRadiusB + maximumRadiusA, worstCaseRadiusB + maximumAngularExpansionA); var combinedAngularExpansion = angularExpansionA + angularExpansionB; min = localOriginToA + min - combinedAngularExpansion; @@ -258,10 +256,10 @@ public static unsafe void GetLocalBoundingBoxForSweep(ref TConvex shape QuaternionEx.ConcatenateWithoutOverlap(orientationA, inverseOrientationB, out var localOrientationA); shape.ComputeAngularExpansionData(out var maximumRadiusA, out var maximumAngularExpansionA); - BoundingBoxHelpers.GetAngularBoundsExpansion(velocityA.Angular, dt, maximumRadiusA, maximumAngularExpansionA, out var angularExpansionA); + var angularExpansionA = GetAngularBoundsExpansion(velocityA.Angular, dt, maximumRadiusA, maximumAngularExpansionA); //The furthest the convex can be from the compound is no further than the sweep pushing it directly away from the compound. var worstCaseRadiusB = sweep.Length() + localOffsetB.Length(); - BoundingBoxHelpers.GetAngularBoundsExpansion(velocityB.Angular, dt, worstCaseRadiusB + maximumRadiusA, worstCaseRadiusB + maximumAngularExpansionA, out var angularExpansionB); + var angularExpansionB = GetAngularBoundsExpansion(velocityB.Angular, dt, worstCaseRadiusB + maximumRadiusA, worstCaseRadiusB + maximumAngularExpansionA); var combinedAngularExpansion = angularExpansionA + angularExpansionB; shape.ComputeBounds(localOrientationA, out min, out max); diff --git a/BepuPhysics/Collidables/BoundingBoxBatcher.cs b/BepuPhysics/Collidables/BoundingBoxBatcher.cs index d828331df..0a630a019 100644 --- a/BepuPhysics/Collidables/BoundingBoxBatcher.cs +++ b/BepuPhysics/Collidables/BoundingBoxBatcher.cs @@ -1,5 +1,6 @@ using BepuPhysics.Collidables; using BepuPhysics.CollisionDetection; +using BepuPhysics.Constraints; using BepuUtilities; using BepuUtilities.Collections; using BepuUtilities.Memory; @@ -69,18 +70,15 @@ public static BoundsContinuation CreateCompoundChildContinuation(int compoundBod public struct BoundingBoxInstance { - public RigidPose Pose; - public BodyVelocity Velocities; public int ShapeIndex; public BoundsContinuation Continuation; } - public struct BoundingBoxInstanceWide where TShape : unmanaged, IShape where TShapeWide : unmanaged, IShapeWide + public struct BoundingBoxBatch { - public TShapeWide Shape; - public Vector MaximumExpansion; - public RigidPoseWide Pose; - public BodyVelocityWide Velocities; + public Buffer ShapeIndices; + public Buffer Continuations; + public int Count; } public struct BoundingBoxBatcher @@ -92,7 +90,7 @@ public struct BoundingBoxBatcher internal float dt; int minimumBatchIndex, maximumBatchIndex; - Buffer> batches; + Buffer batches; /// /// The number of bodies to accumulate per type before executing an AABB update. The more bodies per batch, the less virtual overhead and execution divergence. @@ -116,48 +114,66 @@ public unsafe BoundingBoxBatcher(Bodies bodies, Shapes shapes, BroadPhase broadP public unsafe void ExecuteConvexBatch(ConvexShapeBatch shapeBatch) where TShape : unmanaged, IConvexShape where TShapeWide : unmanaged, IShapeWide { - var instanceBundle = default(BoundingBoxInstanceWide); - if (instanceBundle.Shape.InternalAllocationSize > 0) //TODO: Check to make sure the JIT omits the branch. + Unsafe.SkipInit(out TShapeWide shapeWide); + if (shapeWide.InternalAllocationSize > 0) //TODO: Check to make sure the JIT omits the branch. { - var memory = stackalloc byte[instanceBundle.Shape.InternalAllocationSize]; - instanceBundle.Shape.Initialize(new RawBuffer(memory, instanceBundle.Shape.InternalAllocationSize)); + var memory = stackalloc byte[shapeWide.InternalAllocationSize]; + shapeWide.Initialize(new RawBuffer(memory, shapeWide.InternalAllocationSize)); } ref var batch = ref batches[shapeBatch.TypeId]; - ref var instancesBase = ref batch[0]; + var shapeIndices = batch.ShapeIndices; + var continuations = batch.Continuations; ref var activeSet = ref bodies.ActiveSet; + var continuationMask = new Vector(1 << 31); + var dtWide = new Vector(dt); + Span minimumMarginSpan = stackalloc float[Vector.Count]; + Span maximumMarginSpan = stackalloc float[Vector.Count]; + Span allowExpansionBeyondSpeculativeMarginSpan = stackalloc int[Vector.Count]; for (int bundleStartIndex = 0; bundleStartIndex < batch.Count; bundleStartIndex += Vector.Count) { int countInBundle = batch.Count - bundleStartIndex; if (countInBundle > Vector.Count) countInBundle = Vector.Count; - ref var bundleInstancesStart = ref Unsafe.Add(ref instancesBase, bundleStartIndex); + var emptyTrailingMask = BundleIndexing.CreateTrailingMaskForCountInBundle(countInBundle); + var continuationsBundle = Unsafe.As>(ref continuations[bundleStartIndex]); + var bodyIndices = Vector.BitwiseOr(emptyTrailingMask, Vector.AndNot(continuationsBundle, continuationMask)); + bodies.GatherState(bodyIndices, false, out var positions, out var orientations, out var velocities, out _); + shapeWide.GetBounds(ref orientations, countInBundle, out var maximumRadius, out var maximumAngularExpansion, out var bundleMin, out var bundleMax); + //BoundingBoxBatcher is responsible for updating the bounding box AND speculative margin. + //In order to know how much we're allowed to expand the bounding box, we need to know the speculative margin. + //It's defined by the velocity of the body, and bounded by the body's minimum and maximum. + var angularBoundsExpansion = BoundingBoxHelpers.GetAngularBoundsExpansion(Vector3Wide.Length(velocities.Angular), dtWide, maximumRadius, maximumAngularExpansion); + var speculativeMargin = Vector3Wide.Length(velocities.Linear) + angularBoundsExpansion; + //Note that doing a gather-scatter to enable vectorized bundle execution isn't worth it for some shape types. //We're just ignoring that fact for simplicity. Bounding box updates aren't a huge concern overall. That said, //if you want to optimize this further, the shape batches could choose between vectorized and gatherless scalar implementations on a per-type basis. + //TODO: This transposition could be significantly improved with intrinsics, as we did for the Bodies state gather operations. + for (int innerIndex = 0; innerIndex < countInBundle; ++innerIndex) { - ref var instance = ref Unsafe.Add(ref bundleInstancesStart, innerIndex); - ref var targetInstanceSlot = ref GatherScatter.GetOffsetInstance(ref instanceBundle, innerIndex); + var shapeIndex = shapeIndices[bundleStartIndex + innerIndex]; //This property should be a constant value and the JIT has type knowledge, so this branch should optimize out. - if (instanceBundle.Shape.AllowOffsetMemoryAccess) - targetInstanceSlot.Shape.WriteFirst(shapeBatch.shapes[instance.ShapeIndex]); + if (shapeWide.AllowOffsetMemoryAccess) + shapeWide.WriteFirst(shapeBatch.shapes[shapeIndex]); else - instanceBundle.Shape.WriteSlot(innerIndex, shapeBatch.shapes[instance.ShapeIndex]); - Vector3Wide.WriteFirst(instance.Pose.Position, ref targetInstanceSlot.Pose.Position); - QuaternionWide.WriteFirst(instance.Pose.Orientation, ref targetInstanceSlot.Pose.Orientation); - Vector3Wide.WriteFirst(instance.Velocities.Linear, ref targetInstanceSlot.Velocities.Linear); - Vector3Wide.WriteFirst(instance.Velocities.Angular, ref targetInstanceSlot.Velocities.Angular); - ref var collidable = ref activeSet.Collidables[instance.Continuation.BodyIndex]; - GatherScatter.GetFirst(ref targetInstanceSlot.MaximumExpansion) = - collidable.Continuity.AllowExpansionBeyondSpeculativeMargin ? float.MaxValue : collidable.SpeculativeMargin; + shapeWide.WriteSlot(innerIndex, shapeBatch.shapes[shapeIndex]); + ref var collidable = ref activeSet.Collidables[continuations[bundleStartIndex + innerIndex].BodyIndex]; + minimumMarginSpan[innerIndex] = collidable.Continuity.MinimumSpeculativeMargin; + maximumMarginSpan[innerIndex] = collidable.Continuity.MaximumSpeculativeMargin; + allowExpansionBeyondSpeculativeMarginSpan[innerIndex] = collidable.Continuity.AllowExpansionBeyondSpeculativeMargin ? -1 : 0; } - instanceBundle.Shape.GetBounds(ref instanceBundle.Pose.Orientation, countInBundle, out var maximumRadius, out var maximumAngularExpansion, out var bundleMin, out var bundleMax); - BoundingBoxHelpers.ExpandBoundingBoxes(ref bundleMin, ref bundleMax, ref instanceBundle.Velocities, dt, - ref maximumRadius, ref maximumAngularExpansion, ref instanceBundle.MaximumExpansion); + var minimumSpeculativeMargin = new Vector(minimumMarginSpan); + var maximumSpeculativeMargin = new Vector(maximumMarginSpan); + var allowExpansionBeyondSpeculativeMargin = new Vector(allowExpansionBeyondSpeculativeMarginSpan); + speculativeMargin = Vector.Max(minimumSpeculativeMargin, Vector.Min(maximumSpeculativeMargin, speculativeMargin)); + var maximumBoundsExpansion = Vector.ConditionalSelect(allowExpansionBeyondSpeculativeMargin, new Vector(float.MaxValue), speculativeMargin); + BoundingBoxHelpers.ExpandBoundingBoxes(ref bundleMin, ref bundleMax, velocities, dt, + maximumRadius, maximumAngularExpansion, maximumBoundsExpansion); //TODO: Note that this is an area that must be updated if you change the pose representation. - Vector3Wide.Add(instanceBundle.Pose.Position, bundleMin, out bundleMin); - Vector3Wide.Add(instanceBundle.Pose.Position, bundleMax, out bundleMax); + Vector3Wide.Add(positions, bundleMin, out bundleMin); + Vector3Wide.Add(positions, bundleMax, out bundleMax); for (int innerIndex = 0; innerIndex < countInBundle; ++innerIndex) { diff --git a/BepuPhysics/PoseIntegrator.cs b/BepuPhysics/PoseIntegrator.cs index 522de218b..4272b12d9 100644 --- a/BepuPhysics/PoseIntegrator.cs +++ b/BepuPhysics/PoseIntegrator.cs @@ -696,7 +696,6 @@ public void IntegrateBodiesAndUpdateBoundingBoxes(float dt, BufferPool pool, ITh public void PredictBoundingBoxes(float dt, BufferPool pool, IThreadDispatcher threadDispatcher = null) { - //No need to ensure inertias capacity here; world inertias are not computed during bounding box prediction. var workerCount = threadDispatcher == null ? 1 : threadDispatcher.ThreadCount; Callbacks.PrepareForIntegration(dt); From 0f363efbfcadd552444a971e9a25bbcc16b7a119 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 10 Nov 2021 16:04:38 -0600 Subject: [PATCH 279/947] BoundingBoxBatcher now ostensibly functioning with speculative margin update and some improvements to gathering. Partially. Suspect there's a hidden sneakybug remaining, unless my computer got hit by a cosmic ray that one time. --- BepuPhysics/Bodies_GatherScatter.cs | 125 ++++++++++++++++ BepuPhysics/BoundingBoxHelpers.cs | 44 +++--- BepuPhysics/Collidables/BoundingBoxBatcher.cs | 141 +++++++++++------- BepuPhysics/CollisionDetection/NarrowPhase.cs | 12 +- BepuPhysics/Simulation_Queries.cs | 2 +- BepuUtilities/QuaternionWide.cs | 12 ++ 6 files changed, 258 insertions(+), 78 deletions(-) diff --git a/BepuPhysics/Bodies_GatherScatter.cs b/BepuPhysics/Bodies_GatherScatter.cs index 056178bde..d048217e6 100644 --- a/BepuPhysics/Bodies_GatherScatter.cs +++ b/BepuPhysics/Bodies_GatherScatter.cs @@ -175,6 +175,131 @@ public static bool IsEncodedKinematicReference(int encodedBodyReferenceValue) return (uint)encodedBodyReferenceValue >= DynamicLimit; } + //TODO: Good argument for source generation (or at least refactoring) here. The vectorized paths don't need much in the way of maintenance, but having a bunch of duplicates is unavoidably error prone. + + /// + /// Transposes of bundle of array-of-structures layout motion states into a bundle of array-of-structures-of-arrays layout. + /// Size of buffer must be no larger than the . + /// + /// Array-of-structures data to transpose. + /// Array-of-structures-of-arrays positions. + /// Array-of-structures-of-arrays orientations. + /// Array-of-structures-of-arrays velocities. + public static unsafe void TransposeMotionStates(Buffer states, out Vector3Wide position, out QuaternionWide orientation, out BodyVelocityWide velocity) + { + Debug.Assert(states.Length > 0 && states.Length <= Vector.Count); + if (Avx.IsSupported && Vector.Count == 8) + { + var empty1 = states.Length <= 1; + var empty2 = states.Length <= 2; + var empty3 = states.Length <= 3; + var empty4 = states.Length <= 4; + var empty5 = states.Length <= 5; + var empty6 = states.Length <= 6; + var empty7 = states.Length <= 7; + + var s0 = (float*)states.Memory; + var s1 = (float*)(states.Memory + 1); + var s2 = (float*)(states.Memory + 2); + var s3 = (float*)(states.Memory + 3); + var s4 = (float*)(states.Memory + 4); + var s5 = (float*)(states.Memory + 5); + var s6 = (float*)(states.Memory + 6); + var s7 = (float*)(states.Memory + 7); + + { + //Load every body for the first half of the motion state. + //Note that buffers are allocated on cache line boundaries, so we can use aligned loads for all that matters. + var m0 = Avx.LoadAlignedVector256(s0); + var m1 = empty1 ? Vector256.Zero : Avx.LoadAlignedVector256(s1); + var m2 = empty2 ? Vector256.Zero : Avx.LoadAlignedVector256(s2); + var m3 = empty3 ? Vector256.Zero : Avx.LoadAlignedVector256(s3); + var m4 = empty4 ? Vector256.Zero : Avx.LoadAlignedVector256(s4); + var m5 = empty5 ? Vector256.Zero : Avx.LoadAlignedVector256(s5); + var m6 = empty6 ? Vector256.Zero : Avx.LoadAlignedVector256(s6); + var m7 = empty7 ? Vector256.Zero : Avx.LoadAlignedVector256(s7); + + var n0 = Avx.UnpackLow(m0, m1); + var n1 = Avx.UnpackLow(m2, m3); + var n2 = Avx.UnpackLow(m4, m5); + var n3 = Avx.UnpackLow(m6, m7); + var n4 = Avx.UnpackHigh(m0, m1); + var n5 = Avx.UnpackHigh(m2, m3); + var n6 = Avx.UnpackHigh(m4, m5); + var n7 = Avx.UnpackHigh(m6, m7); + + var o0 = Avx.Shuffle(n0, n1, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o1 = Avx.Shuffle(n2, n3, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o2 = Avx.Shuffle(n4, n5, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o3 = Avx.Shuffle(n6, n7, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o4 = Avx.Shuffle(n0, n1, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o5 = Avx.Shuffle(n2, n3, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o6 = Avx.Shuffle(n4, n5, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o7 = Avx.Shuffle(n6, n7, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + + orientation.X = Avx.Permute2x128(o0, o1, 0 | (2 << 4)).AsVector(); + orientation.Y = Avx.Permute2x128(o4, o5, 0 | (2 << 4)).AsVector(); + orientation.Z = Avx.Permute2x128(o2, o3, 0 | (2 << 4)).AsVector(); + orientation.W = Avx.Permute2x128(o6, o7, 0 | (2 << 4)).AsVector(); + + position.X = Avx.Permute2x128(o0, o1, 1 | (3 << 4)).AsVector(); + position.Y = Avx.Permute2x128(o4, o5, 1 | (3 << 4)).AsVector(); + position.Z = Avx.Permute2x128(o2, o3, 1 | (3 << 4)).AsVector(); + } + + { + //Second half. + var m0 = Avx.LoadAlignedVector256(s0 + 8); + var m1 = empty1 ? Vector256.Zero : Avx.LoadAlignedVector256(s1 + 8); + var m2 = empty2 ? Vector256.Zero : Avx.LoadAlignedVector256(s2 + 8); + var m3 = empty3 ? Vector256.Zero : Avx.LoadAlignedVector256(s3 + 8); + var m4 = empty4 ? Vector256.Zero : Avx.LoadAlignedVector256(s4 + 8); + var m5 = empty5 ? Vector256.Zero : Avx.LoadAlignedVector256(s5 + 8); + var m6 = empty6 ? Vector256.Zero : Avx.LoadAlignedVector256(s6 + 8); + var m7 = empty7 ? Vector256.Zero : Avx.LoadAlignedVector256(s7 + 8); + + var n0 = Avx.UnpackLow(m0, m1); + var n1 = Avx.UnpackLow(m2, m3); + var n2 = Avx.UnpackLow(m4, m5); + var n3 = Avx.UnpackLow(m6, m7); + var n4 = Avx.UnpackHigh(m0, m1); + var n5 = Avx.UnpackHigh(m2, m3); + var n6 = Avx.UnpackHigh(m4, m5); + var n7 = Avx.UnpackHigh(m6, m7); + + var o0 = Avx.Shuffle(n0, n1, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o1 = Avx.Shuffle(n2, n3, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o2 = Avx.Shuffle(n4, n5, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o3 = Avx.Shuffle(n6, n7, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o4 = Avx.Shuffle(n0, n1, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o5 = Avx.Shuffle(n2, n3, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + + velocity.Linear.X = Avx.Permute2x128(o0, o1, 0 | (2 << 4)).AsVector(); + velocity.Linear.Y = Avx.Permute2x128(o4, o5, 0 | (2 << 4)).AsVector(); + velocity.Linear.Z = Avx.Permute2x128(o2, o3, 0 | (2 << 4)).AsVector(); + + velocity.Angular.X = Avx.Permute2x128(o0, o1, 1 | (3 << 4)).AsVector(); + velocity.Angular.Y = Avx.Permute2x128(o4, o5, 1 | (3 << 4)).AsVector(); + velocity.Angular.Z = Avx.Permute2x128(o2, o3, 1 | (3 << 4)).AsVector(); + } + } + else + { + Unsafe.SkipInit(out position); + Unsafe.SkipInit(out orientation); + Unsafe.SkipInit(out velocity); + for (int i = 0; i < states.Length; ++i) + { + ref var state = ref states[i]; + Vector3Wide.WriteSlot(state.Pose.Position, i, ref position); + QuaternionWide.WriteSlot(state.Pose.Orientation, i, ref orientation); + Vector3Wide.WriteSlot(state.Velocity.Linear, i, ref velocity.Linear); + Vector3Wide.WriteSlot(state.Velocity.Angular, i, ref velocity.Angular); + } + } + } + + //[MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe void GatherState(Vector encodedBodyIndices, bool worldInertia, out Vector3Wide position, out QuaternionWide orientation, out BodyVelocityWide velocity, out BodyInertiaWide inertia) where TAccessFilter : unmanaged, IBodyAccessFilter diff --git a/BepuPhysics/BoundingBoxHelpers.cs b/BepuPhysics/BoundingBoxHelpers.cs index 3fe5bd6c0..d5bc67fe4 100644 --- a/BepuPhysics/BoundingBoxHelpers.cs +++ b/BepuPhysics/BoundingBoxHelpers.cs @@ -46,9 +46,23 @@ An extra few dozen ALU cycles is unlikely to meaningfully change the execution t Vector.SquareRoot(new Vector(-2f) * maximumRadius * maximumRadius * cosAngleMinusOne)); } + [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void GetBoundsExpansion(in Vector3Wide linearVelocity, in Vector3Wide angularVelocity, float dt, - in Vector maximumRadius, in Vector maximumAngularExpansion, out Vector3Wide minExpansion, out Vector3Wide maxExpansion) + public static void GetBoundsExpansion(Vector3Wide linearVelocity, Vector dtWide, Vector angularExpansion, out Vector3Wide minExpansion, out Vector3Wide maxExpansion) + { + var linearDisplacement = linearVelocity * dtWide; + var zero = Vector.Zero; + minExpansion = Vector3Wide.Min(zero, linearDisplacement); + maxExpansion = Vector3Wide.Max(zero, linearDisplacement); + Vector3Wide.Subtract(minExpansion, angularExpansion, out minExpansion); + Vector3Wide.Add(maxExpansion, angularExpansion, out maxExpansion); + } + + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void GetBoundsExpansion( + Vector3Wide linearVelocity, Vector3Wide angularVelocity, Vector dtWide, Vector maximumRadius, Vector maximumAngularExpansion, + out Vector3Wide minBoundsExpansion, out Vector3Wide maxBoundsExpansion) { /* If an object sitting on a plane had a raw (unexpanded) AABB that is just barely above the plane, no contacts would be generated. @@ -93,25 +107,18 @@ There are ways to address this- all of which are a bit expensive- but CCD as imp Linear is pretty simple- expand the bounding box in the direction of linear displacement (linearVelocity * dt). */ - Vector vectorDt = new Vector(dt); - Vector3Wide.Scale(linearVelocity, vectorDt, out var linearDisplacement); - - var zero = Vector.Zero; - Vector3Wide.Min(zero, linearDisplacement, out minExpansion); - Vector3Wide.Max(zero, linearDisplacement, out maxExpansion); Vector3Wide.Length(angularVelocity, out var angularSpeed); - var angularExpansion = GetAngularBoundsExpansion(angularSpeed, vectorDt, maximumRadius, maximumAngularExpansion); - Vector3Wide.Subtract(minExpansion, angularExpansion, out minExpansion); - Vector3Wide.Add(maxExpansion, angularExpansion, out maxExpansion); + var angularExpansion = GetAngularBoundsExpansion(angularSpeed, dtWide, maximumRadius, maximumAngularExpansion); + GetBoundsExpansion(linearVelocity, dtWide, angularExpansion, out minBoundsExpansion, out maxBoundsExpansion); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ExpandBoundingBoxes(ref Vector3Wide min, ref Vector3Wide max, BodyVelocityWide velocities, float dt, - Vector maximumRadius, Vector maximumAngularExpansion, Vector maximumExpansion) + public static void ExpandBoundingBoxes(BodyVelocityWide velocities, Vector dtWide, Vector maximumRadius, Vector maximumAngularExpansion, Vector maximumExpansion, + ref Vector3Wide min, ref Vector3Wide max) { - GetBoundsExpansion(velocities.Linear, velocities.Angular, dt, maximumRadius, maximumAngularExpansion, out var minDisplacement, out var maxDisplacement); - Vector3Wide.Max(-maximumExpansion, minDisplacement, out minDisplacement); - Vector3Wide.Min(maximumExpansion, maxDisplacement, out maxDisplacement); + GetBoundsExpansion(velocities.Linear, velocities.Angular, dtWide, maximumRadius, maximumAngularExpansion, out var minDisplacement, out var maxDisplacement); + minDisplacement = Vector3Wide.Max(-maximumExpansion, minDisplacement); + maxDisplacement = Vector3Wide.Min(maximumExpansion, maxDisplacement); Vector3Wide.Add(min, minDisplacement, out min); Vector3Wide.Add(max, maxDisplacement, out max); @@ -178,7 +185,8 @@ public static unsafe void ExpandLocalBoundingBoxes(ref Vector3Wide min, ref Vect in Vector radiusA, in Vector3Wide localPositionA, in Vector3Wide localRelativeLinearVelocityA, in Vector3Wide angularVelocityA, in Vector3Wide angularVelocityB, float dt, in Vector maximumRadius, in Vector maximumAngularExpansion, in Vector maximumAllowedExpansion) { - GetBoundsExpansion(localRelativeLinearVelocityA, angularVelocityA, dt, + var dtWide = new Vector(dt); + GetBoundsExpansion(localRelativeLinearVelocityA, angularVelocityA, dtWide, maximumRadius + radiusA, maximumAngularExpansion + radiusA, out var minExpansion, out var maxExpansion); Vector3Wide.LengthSquared(angularVelocityB, out var angularSpeedBSquared); if (Vector.GreaterThanAny(angularSpeedBSquared, Vector.Zero)) @@ -187,7 +195,7 @@ public static unsafe void ExpandLocalBoundingBoxes(ref Vector3Wide min, ref Vect Vector3Wide.Length(localPositionA, out var radiusB); Vector3Wide.Length(localRelativeLinearVelocityA, out var linearSpeed); var worstCaseRadius = linearSpeed * dt + radiusB; - var angularExpansionB = GetAngularBoundsExpansion(Vector.SquareRoot(angularSpeedBSquared), maximumRadius + worstCaseRadius, maximumAngularExpansion + worstCaseRadius, new Vector(dt)); + var angularExpansionB = GetAngularBoundsExpansion(Vector.SquareRoot(angularSpeedBSquared), maximumRadius + worstCaseRadius, maximumAngularExpansion + worstCaseRadius, dtWide); Vector3Wide.Subtract(minExpansion, angularExpansionB, out minExpansion); Vector3Wide.Add(maxExpansion, angularExpansionB, out maxExpansion); } diff --git a/BepuPhysics/Collidables/BoundingBoxBatcher.cs b/BepuPhysics/Collidables/BoundingBoxBatcher.cs index 0a630a019..f05611b53 100644 --- a/BepuPhysics/Collidables/BoundingBoxBatcher.cs +++ b/BepuPhysics/Collidables/BoundingBoxBatcher.cs @@ -78,7 +78,38 @@ public struct BoundingBoxBatch { public Buffer ShapeIndices; public Buffer Continuations; + public Buffer MotionStates; public int Count; + public bool Allocated => ShapeIndices.Allocated; + + public BoundingBoxBatch(BufferPool pool, int initialCapacity) + { + pool.Take(initialCapacity, out ShapeIndices); + pool.Take(initialCapacity, out Continuations); + pool.Take(initialCapacity, out MotionStates); + Count = 0; + } + internal void Add(int shapeIndex, RigidPose pose, BodyVelocity velocity, BoundsContinuation continuation) + { + ShapeIndices[Count] = shapeIndex; + Continuations[Count] = continuation; + ref var motionState = ref MotionStates[Count]; + motionState.Pose = pose; + motionState.Velocity = velocity; + Count++; + } + + public void Dispose(BufferPool pool) + { + if (Allocated) + { + pool.Return(ref ShapeIndices); + pool.Return(ref Continuations); + pool.Return(ref MotionStates); + Count = 0; + } + } + } public struct BoundingBoxBatcher @@ -135,65 +166,76 @@ public unsafe void ExecuteConvexBatch(ConvexShapeBatch Vector.Count) countInBundle = Vector.Count; - var emptyTrailingMask = BundleIndexing.CreateTrailingMaskForCountInBundle(countInBundle); - var continuationsBundle = Unsafe.As>(ref continuations[bundleStartIndex]); - var bodyIndices = Vector.BitwiseOr(emptyTrailingMask, Vector.AndNot(continuationsBundle, continuationMask)); - bodies.GatherState(bodyIndices, false, out var positions, out var orientations, out var velocities, out _); - shapeWide.GetBounds(ref orientations, countInBundle, out var maximumRadius, out var maximumAngularExpansion, out var bundleMin, out var bundleMax); - //BoundingBoxBatcher is responsible for updating the bounding box AND speculative margin. - //In order to know how much we're allowed to expand the bounding box, we need to know the speculative margin. - //It's defined by the velocity of the body, and bounded by the body's minimum and maximum. - var angularBoundsExpansion = BoundingBoxHelpers.GetAngularBoundsExpansion(Vector3Wide.Length(velocities.Angular), dtWide, maximumRadius, maximumAngularExpansion); - var speculativeMargin = Vector3Wide.Length(velocities.Linear) + angularBoundsExpansion; //Note that doing a gather-scatter to enable vectorized bundle execution isn't worth it for some shape types. //We're just ignoring that fact for simplicity. Bounding box updates aren't a huge concern overall. That said, //if you want to optimize this further, the shape batches could choose between vectorized and gatherless scalar implementations on a per-type basis. //TODO: This transposition could be significantly improved with intrinsics, as we did for the Bodies state gather operations. - for (int innerIndex = 0; innerIndex < countInBundle; ++innerIndex) { var shapeIndex = shapeIndices[bundleStartIndex + innerIndex]; - //This property should be a constant value and the JIT has type knowledge, so this branch should optimize out. - if (shapeWide.AllowOffsetMemoryAccess) - shapeWide.WriteFirst(shapeBatch.shapes[shapeIndex]); - else - shapeWide.WriteSlot(innerIndex, shapeBatch.shapes[shapeIndex]); + shapeWide.WriteSlot(innerIndex, shapeBatch.shapes[shapeIndex]); ref var collidable = ref activeSet.Collidables[continuations[bundleStartIndex + innerIndex].BodyIndex]; minimumMarginSpan[innerIndex] = collidable.Continuity.MinimumSpeculativeMargin; maximumMarginSpan[innerIndex] = collidable.Continuity.MaximumSpeculativeMargin; allowExpansionBeyondSpeculativeMarginSpan[innerIndex] = collidable.Continuity.AllowExpansionBeyondSpeculativeMargin ? -1 : 0; } + + Bodies.TransposeMotionStates(batch.MotionStates.Slice(bundleStartIndex, countInBundle), out var positions, out var orientations, out var velocities); + shapeWide.GetBounds(ref orientations, countInBundle, out var maximumRadius, out var maximumAngularExpansion, out var bundleMin, out var bundleMax); + //BoundingBoxBatcher is responsible for updating the bounding box AND speculative margin. + //In order to know how much we're allowed to expand the bounding box, we need to know the speculative margin. + //It's defined by the velocity of the body, and bounded by the body's minimum and maximum. + var angularBoundsExpansion = BoundingBoxHelpers.GetAngularBoundsExpansion(Vector3Wide.Length(velocities.Angular), dtWide, maximumRadius, maximumAngularExpansion); + var speculativeMargin = Vector3Wide.Length(velocities.Linear) * dtWide + angularBoundsExpansion; + + var minimumSpeculativeMargin = new Vector(minimumMarginSpan); var maximumSpeculativeMargin = new Vector(maximumMarginSpan); var allowExpansionBeyondSpeculativeMargin = new Vector(allowExpansionBeyondSpeculativeMarginSpan); speculativeMargin = Vector.Max(minimumSpeculativeMargin, Vector.Min(maximumSpeculativeMargin, speculativeMargin)); var maximumBoundsExpansion = Vector.ConditionalSelect(allowExpansionBeyondSpeculativeMargin, new Vector(float.MaxValue), speculativeMargin); - BoundingBoxHelpers.ExpandBoundingBoxes(ref bundleMin, ref bundleMax, velocities, dt, - maximumRadius, maximumAngularExpansion, maximumBoundsExpansion); + BoundingBoxHelpers.GetBoundsExpansion(velocities.Linear, dtWide, angularBoundsExpansion, out var minExpansion, out var maxExpansion); //TODO: Note that this is an area that must be updated if you change the pose representation. - Vector3Wide.Add(positions, bundleMin, out bundleMin); - Vector3Wide.Add(positions, bundleMax, out bundleMax); + bundleMin = positions + (bundleMin + minExpansion); + bundleMax = positions + (bundleMax + maxExpansion); for (int innerIndex = 0; innerIndex < countInBundle; ++innerIndex) { - ref var instance = ref Unsafe.Add(ref bundleInstancesStart, innerIndex); - broadPhase.GetActiveBoundsPointers(activeSet.Collidables[instance.Continuation.BodyIndex].BroadPhaseIndex, out var minPointer, out var maxPointer); - ref var sourceBundleMin = ref GatherScatter.GetOffsetInstance(ref bundleMin, innerIndex); - ref var sourceBundleMax = ref GatherScatter.GetOffsetInstance(ref bundleMax, innerIndex); + var continuation = continuations[bundleStartIndex + innerIndex]; + ref var collidable = ref activeSet.Collidables[continuation.BodyIndex]; + collidable.SpeculativeMargin = speculativeMargin[innerIndex]; + broadPhase.GetActiveBoundsPointers(collidable.BroadPhaseIndex, out var minPointer, out var maxPointer); + //ref var sourceBundleMin = ref GatherScatter.GetOffsetInstance(ref bundleMin, innerIndex); + //ref var sourceBundleMax = ref GatherScatter.GetOffsetInstance(ref bundleMax, innerIndex); + ////Note that we merge with the existing bounding box if the body is compound. This requires compounds to be initialized to (maxvalue, -maxvalue). + ////TODO: We bite the bullet on quite a bit of complexity to avoid merging on non-compounds. Could be better overall to simply merge on all bodies. Certainly simpler. + ////Worth checking the performance; if it's undetectable, just swap to the simpler version. + //if (continuation.CompoundChild) + //{ + // var min = new Vector3(sourceBundleMin.X[0], sourceBundleMin.Y[0], sourceBundleMin.Z[0]); + // var max = new Vector3(sourceBundleMax.X[0], sourceBundleMax.Y[0], sourceBundleMax.Z[0]); + // BoundingBox.CreateMerged(*minPointer, *maxPointer, min, max, out *minPointer, out *maxPointer); + //} + //else + //{ + // *minPointer = new Vector3(sourceBundleMin.X[0], sourceBundleMin.Y[0], sourceBundleMin.Z[0]); + // *maxPointer = new Vector3(sourceBundleMax.X[0], sourceBundleMax.Y[0], sourceBundleMax.Z[0]); + //} + //Note that we merge with the existing bounding box if the body is compound. This requires compounds to be initialized to (maxvalue, -maxvalue). //TODO: We bite the bullet on quite a bit of complexity to avoid merging on non-compounds. Could be better overall to simply merge on all bodies. Certainly simpler. //Worth checking the performance; if it's undetectable, just swap to the simpler version. - if (instance.Continuation.CompoundChild) + if (continuation.CompoundChild) { - var min = new Vector3(sourceBundleMin.X[0], sourceBundleMin.Y[0], sourceBundleMin.Z[0]); - var max = new Vector3(sourceBundleMax.X[0], sourceBundleMax.Y[0], sourceBundleMax.Z[0]); + var min = new Vector3(bundleMin.X[innerIndex], bundleMin.Y[innerIndex], bundleMin.Z[innerIndex]); + var max = new Vector3(bundleMax.X[innerIndex], bundleMax.Y[innerIndex], bundleMax.Z[innerIndex]); BoundingBox.CreateMerged(*minPointer, *maxPointer, min, max, out *minPointer, out *maxPointer); } else { - *minPointer = new Vector3(sourceBundleMin.X[0], sourceBundleMin.Y[0], sourceBundleMin.Z[0]); - *maxPointer = new Vector3(sourceBundleMax.X[0], sourceBundleMax.Y[0], sourceBundleMax.Z[0]); + *minPointer = new Vector3(bundleMin.X[innerIndex], bundleMin.Y[innerIndex], bundleMin.Z[innerIndex]); + *maxPointer = new Vector3(bundleMax.X[innerIndex], bundleMax.Y[innerIndex], bundleMax.Z[innerIndex]); } } } @@ -208,12 +250,13 @@ public unsafe void ExecuteHomogeneousCompoundBatch(CompoundShapeBatch shape var maxValue = new Vector3(-float.MaxValue); for (int i = 0; i < batch.Count; ++i) { - ref var instance = ref batch[i]; - var bodyIndex = instance.Continuation.BodyIndex; + var shapeIndex = batch.ShapeIndices[i]; + var bodyIndex = batch.Continuations[i].BodyIndex; + ref var motionState = ref batch.MotionStates[i]; //We have to clear out the bounds used by compounds, since each contributing body will merge their contribution into the whole. broadPhase.GetActiveBoundsPointers(activeSet.Collidables[bodyIndex].BroadPhaseIndex, out var min, out var max); *min = minValue; *max = maxValue; - shapeBatch.shapes[instance.ShapeIndex].AddChildBoundsToBatcher(ref this, instance.Pose, instance.Velocities, bodyIndex); + shapeBatch.shapes[batch.ShapeIndices[i]].AddChildBoundsToBatcher(ref this, motionState.Pose, motionState.Velocity, bodyIndex); } } [MethodImpl(MethodImplOptions.AggressiveInlining)] - void Add(TypedIndex shapeIndex, in RigidPose pose, in BodyVelocity velocity, BoundsContinuation continuation) + void Add(TypedIndex shapeIndex, RigidPose pose, BodyVelocity velocity, BoundsContinuation continuation) { var typeIndex = shapeIndex.Type; Debug.Assert(typeIndex >= 0 && typeIndex < batches.Length, "The preallocated type batch array should be able to hold every type index. Is the type index broken?"); ref var batchSlot = ref batches[typeIndex]; - if (!batchSlot.Span.Allocated) + if (!batchSlot.Allocated) { //No list exists for this type yet. - batchSlot = new QuickList(CollidablesPerFlush, pool); + batchSlot = new BoundingBoxBatch(pool, CollidablesPerFlush); if (typeIndex < minimumBatchIndex) minimumBatchIndex = typeIndex; if (typeIndex > maximumBatchIndex) @@ -266,16 +310,10 @@ void Add(TypedIndex shapeIndex, in RigidPose pose, in BodyVelocity velocity, Bou //It technically opens the door for vectorizing their child pose calculations, but those are almost certainly not worth vectorizing anyway. //May want to consider directly triggering a bounds dispatch for compounds here. //(Doing so WOULD be more complicated, though.) - ref var instance = ref batchSlot.AllocateUnsafely(); - var shapeBatch = shapes[typeIndex]; - instance.Pose = pose; - instance.Velocities = velocity; - instance.ShapeIndex = shapeIndex.Index; - instance.Continuation = continuation; - + batchSlot.Add(shapeIndex.Index, pose, velocity, continuation); if (batchSlot.Count == CollidablesPerFlush) { - shapeBatch.ComputeBounds(ref this); + shapes[typeIndex].ComputeBounds(ref this); batchSlot.Count = 0; } } @@ -330,10 +368,7 @@ public void Flush() shapes[i].ComputeBounds(ref this); } //Dispose of the batch and any associated buffers; since the flush is one pass, we won't be needing this again. - if (batch.Span.Allocated) - { - batch.Dispose(pool); - } + batch.Dispose(pool); } pool.Return(ref batches); } diff --git a/BepuPhysics/CollisionDetection/NarrowPhase.cs b/BepuPhysics/CollisionDetection/NarrowPhase.cs index 7d637f52d..055bbb685 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhase.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhase.cs @@ -465,12 +465,12 @@ private unsafe void AddBatchEntries(int workerIndex, ref OverlapWorker overlapWo ref RigidPose poseA, ref RigidPose poseB, ref BodyVelocity velocityA, ref BodyVelocity velocityB) { Debug.Assert(pair.A.Packed != pair.B.Packed); - //Note that the pair's margin is the larger of the two involved collidables. This is based on two observations: - //1) Values smaller than either contributor should never be used, because it may interfere with tuning. Difficult to choose substepping properties without a - //known minimum value for speculative margins. - //2) The larger the margin, the higher the risk of ghost collisions. - //Taken together, max is implied. - var speculativeMargin = Math.Max(aCollidable.SpeculativeMargin, bCollidable.SpeculativeMargin); + //Add the speculative margins, but try to obey both collidables' bounds. Note that in the case of nonoverlapping intervals, the higher min ends up used. + var speculativeMargin = + MathF.Max(aCollidable.Continuity.MinimumSpeculativeMargin, MathF.Max(bCollidable.Continuity.MinimumSpeculativeMargin, + MathF.Min(aCollidable.Continuity.MaximumSpeculativeMargin, MathF.Min(bCollidable.Continuity.MaximumSpeculativeMargin, + aCollidable.SpeculativeMargin + bCollidable.SpeculativeMargin)))); + var allowExpansion = aCollidable.Continuity.AllowExpansionBeyondSpeculativeMargin | bCollidable.Continuity.AllowExpansionBeyondSpeculativeMargin; //Note that we pick float.MaxValue for the maximum bounds expansion passive-involving pairs. //This is a compromise- looser bounds are not a correctness issue, so we're trading off potentially more subpairs diff --git a/BepuPhysics/Simulation_Queries.cs b/BepuPhysics/Simulation_Queries.cs index 394b16fdb..42b7f3ec7 100644 --- a/BepuPhysics/Simulation_Queries.cs +++ b/BepuPhysics/Simulation_Queries.cs @@ -265,7 +265,7 @@ public unsafe void Sweep(TShape shape, in RigidPose po //Build a bounding box. shape.ComputeAngularExpansionData(out var maximumRadius, out var maximumAngularExpansion); shape.ComputeBounds(pose.Orientation, out var min, out var max); - BoundingBoxHelpers.GetAngularBoundsExpansion(velocity.Angular, maximumT, maximumRadius, maximumAngularExpansion, out var angularExpansion); + var angularExpansion = BoundingBoxHelpers.GetAngularBoundsExpansion(velocity.Angular, maximumT, maximumRadius, maximumAngularExpansion); min = min - angularExpansion + pose.Position; max = max + angularExpansion + pose.Position; var direction = velocity.Linear; diff --git a/BepuUtilities/QuaternionWide.cs b/BepuUtilities/QuaternionWide.cs index 117e20b7f..f8bbeed46 100644 --- a/BepuUtilities/QuaternionWide.cs +++ b/BepuUtilities/QuaternionWide.cs @@ -615,6 +615,18 @@ public static void WriteFirst(in Quaternion source, ref QuaternionWide targetSlo GatherScatter.GetFirst(ref targetSlot.Y) = source.Y; GatherScatter.GetFirst(ref targetSlot.Z) = source.Z; GatherScatter.GetFirst(ref targetSlot.W) = source.W; + } + + /// + /// Writes a value into a slot of the target bundle. + /// + /// Source of the value to write. + /// Index of the slot to write into. + /// Bundle to write the value into. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void WriteSlot(in Quaternion source, int slotIndex, ref QuaternionWide target) + { + WriteFirst(source, ref GatherScatter.GetOffsetInstance(ref target, slotIndex)); } } } \ No newline at end of file From aa8f764e9c28a0dff3fa3ad67deb6f0fbff339ea Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 10 Nov 2021 16:52:19 -0600 Subject: [PATCH 280/947] Lil testing. --- Demos/Demos/ColosseumDemo.cs | 4 ++-- Demos/Program.cs | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/Demos/Demos/ColosseumDemo.cs b/Demos/Demos/ColosseumDemo.cs index c18d171ed..d5fa790d6 100644 --- a/Demos/Demos/ColosseumDemo.cs +++ b/Demos/Demos/ColosseumDemo.cs @@ -72,13 +72,13 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. //Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); //Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper2(3), solverIterationCount: 1); - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), solverIterationCount: 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), solverIterationCount: 1); var ringBoxShape = new Box(0.5f, 1, 3); ringBoxShape.ComputeInertia(1, out var ringBoxInertia); var boxDescription = BodyDescription.CreateDynamic(new Vector3(), ringBoxInertia, new CollidableDescription(Simulation.Shapes.Add(ringBoxShape)), - new BodyActivityDescription(0.0001f)); + new BodyActivityDescription(0.01f)); var layerPosition = new Vector3(); const int layerCount = 6; diff --git a/Demos/Program.cs b/Demos/Program.cs index 1550acdd6..55f0735c2 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -21,9 +21,9 @@ static void Main(string[] args) content = ContentArchive.Load(stream); } //HeadlessTest.Test(content, 4, 32, 512); - //HeadlessTest.Test(content, 4, 32, 512); + //HeadlessTest.Test(content, 8, 32, 1024); //HeadlessTest.Test(content, 4, 32, 512); - //HeadlessTest.Test(content, 4, 128, 512); + HeadlessTest.Test(content, 4, 128, 1024); //DeterminismTest.Test(content, 1, 16384); var demo = new DemoHarness(loop, content); loop.Run(demo); From c558f98d9715fe7468a2f9673367fb84cee42ad5 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 11 Nov 2021 19:41:30 -0600 Subject: [PATCH 281/947] QuickList->Buffer implicit cast now preserves id. --- BepuUtilities/Collections/QuickList.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/BepuUtilities/Collections/QuickList.cs b/BepuUtilities/Collections/QuickList.cs index 68f0e0caf..fe6711217 100644 --- a/BepuUtilities/Collections/QuickList.cs +++ b/BepuUtilities/Collections/QuickList.cs @@ -683,7 +683,7 @@ public unsafe static implicit operator ReadOnlySpan(in QuickList list) [MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe static implicit operator Buffer(in QuickList list) { - return new Buffer(list.Span.Memory, list.Count); + return new Buffer(list.Span.Memory, list.Count, list.Span.Id); } From 8d05c11a678543560c824379eeb04a876d3cd7ac Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 11 Nov 2021 20:16:44 -0600 Subject: [PATCH 282/947] Fixed compound/mesh bounding box calculations. Well, mostly- mesh looks pretty busted. FountainStressTestDemo now includes dynamic meshes. --- BepuPhysics/BoundingBoxHelpers.cs | 31 ++++++++----- BepuPhysics/Collidables/BoundingBoxBatcher.cs | 42 +++++++++++------ BepuPhysics/Simulation_Queries.cs | 2 +- BepuPhysics/Solver.cs | 6 +-- Demos/Demos/FountainStressTestDemo.cs | 46 ++++++++++++++----- Demos/Program.cs | 4 +- 6 files changed, 88 insertions(+), 43 deletions(-) diff --git a/BepuPhysics/BoundingBoxHelpers.cs b/BepuPhysics/BoundingBoxHelpers.cs index d5bc67fe4..29c599db7 100644 --- a/BepuPhysics/BoundingBoxHelpers.cs +++ b/BepuPhysics/BoundingBoxHelpers.cs @@ -127,10 +127,8 @@ public static void ExpandBoundingBoxes(BodyVelocityWide velocities, Vector(ref TConvex shape QuaternionEx.ConcatenateWithoutOverlap(orientationA, inverseOrientationB, out var localOrientationA); shape.ComputeAngularExpansionData(out var maximumRadiusA, out var maximumAngularExpansionA); - var angularExpansionA = GetAngularBoundsExpansion(velocityA.Angular, dt, maximumRadiusA, maximumAngularExpansionA); + var angularExpansionA = GetAngularBoundsExpansion(velocityA.Angular.Length(), dt, maximumRadiusA, maximumAngularExpansionA); //The furthest the convex can be from the compound is no further than the sweep pushing it directly away from the compound. var worstCaseRadiusB = sweep.Length() + localOffsetB.Length(); - var angularExpansionB = GetAngularBoundsExpansion(velocityB.Angular, dt, worstCaseRadiusB + maximumRadiusA, worstCaseRadiusB + maximumAngularExpansionA); - var combinedAngularExpansion = angularExpansionA + angularExpansionB; + var angularExpansionB = GetAngularBoundsExpansion(velocityB.Angular.Length(), dt, worstCaseRadiusB + maximumRadiusA, worstCaseRadiusB + maximumAngularExpansionA); + var combinedAngularExpansion = new Vector3(angularExpansionA + angularExpansionB); shape.ComputeBounds(localOrientationA, out min, out max); min = min - localOffsetB - combinedAngularExpansion; diff --git a/BepuPhysics/Collidables/BoundingBoxBatcher.cs b/BepuPhysics/Collidables/BoundingBoxBatcher.cs index f05611b53..7b2bc4a9b 100644 --- a/BepuPhysics/Collidables/BoundingBoxBatcher.cs +++ b/BepuPhysics/Collidables/BoundingBoxBatcher.cs @@ -155,8 +155,6 @@ public unsafe void ExecuteConvexBatch(ConvexShapeBatch(1 << 31); var dtWide = new Vector(dt); Span minimumMarginSpan = stackalloc float[Vector.Count]; Span maximumMarginSpan = stackalloc float[Vector.Count]; @@ -173,9 +171,10 @@ public unsafe void ExecuteConvexBatch(ConvexShapeBatch(ConvexShapeBatch(minimumMarginSpan); var maximumSpeculativeMargin = new Vector(maximumMarginSpan); var allowExpansionBeyondSpeculativeMargin = new Vector(allowExpansionBeyondSpeculativeMarginSpan); speculativeMargin = Vector.Max(minimumSpeculativeMargin, Vector.Min(maximumSpeculativeMargin, speculativeMargin)); var maximumBoundsExpansion = Vector.ConditionalSelect(allowExpansionBeyondSpeculativeMargin, new Vector(float.MaxValue), speculativeMargin); BoundingBoxHelpers.GetBoundsExpansion(velocities.Linear, dtWide, angularBoundsExpansion, out var minExpansion, out var maxExpansion); + minExpansion = Vector3Wide.Max(-maximumBoundsExpansion, minExpansion); + maxExpansion = Vector3Wide.Min(maximumBoundsExpansion, maxExpansion); //TODO: Note that this is an area that must be updated if you change the pose representation. bundleMin = positions + (bundleMin + minExpansion); bundleMax = positions + (bundleMax + maxExpansion); @@ -204,7 +204,6 @@ public unsafe void ExecuteConvexBatch(ConvexShapeBatch(ConvexShapeBatch(TShape shape, in RigidPose po //Build a bounding box. shape.ComputeAngularExpansionData(out var maximumRadius, out var maximumAngularExpansion); shape.ComputeBounds(pose.Orientation, out var min, out var max); - var angularExpansion = BoundingBoxHelpers.GetAngularBoundsExpansion(velocity.Angular, maximumT, maximumRadius, maximumAngularExpansion); + var angularExpansion = new Vector3(BoundingBoxHelpers.GetAngularBoundsExpansion(velocity.Angular.Length(), maximumT, maximumRadius, maximumAngularExpansion)); min = min - angularExpansion + pose.Position; max = max + angularExpansion + pose.Position; var direction = velocity.Linear; diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index 913b9462c..95ecaae66 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -316,7 +316,7 @@ public unsafe ConstraintReference GetConstraintReference(ConstraintHandle handle } [Conditional("DEBUG")] - public unsafe void ValidateConstraintReferenceKinematicity() + internal unsafe void ValidateConstraintReferenceKinematicity() { //Only the active set's body indices are flagged for kinematicity; the inactive sets store body handles. for (int setIndex = 0; setIndex < Sets.Length; ++setIndex) @@ -364,7 +364,7 @@ public unsafe void ValidateConstraintReferenceKinematicity() } [Conditional("DEBUG")] - unsafe public void ValidateConstrainedKinematicsSet() + internal unsafe void ValidateConstrainedKinematicsSet() { ref var set = ref bodies.ActiveSet; for (int i = 0; i < set.Count; ++i) @@ -651,7 +651,7 @@ internal unsafe void ValidateFallbackBatchAccumulatedImpulses() } [Conditional("DEBUG")] - public unsafe void ValidateBatchReferencedHandlesVersusConstraintStoredReferences() + internal unsafe void ValidateBatchReferencedHandlesVersusConstraintStoredReferences() { const int maximumBodyCountInConstraint = 4; int* debugReferences = stackalloc int[maximumBodyCountInConstraint]; diff --git a/Demos/Demos/FountainStressTestDemo.cs b/Demos/Demos/FountainStressTestDemo.cs index b80eddffc..ac935a243 100644 --- a/Demos/Demos/FountainStressTestDemo.cs +++ b/Demos/Demos/FountainStressTestDemo.cs @@ -156,6 +156,29 @@ void CreateRandomCompound(out Buffer children, out BodyInertia in } } + void CreateRandomMesh(out Mesh mesh, out BodyInertia inertia) + { + //We'll use a convex hull algorithm to generate the triangles for the mesh, rather than just spewing random triangle soups. + var pointCount = random.Next(4, 16); + BufferPool.Take(pointCount, out Buffer points); + for (int i = 0; i < pointCount; ++i) + { + points[i] = 2f * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); + } + ConvexHullHelper.CreateShape(points, BufferPool, out _, out var convexHull); + BufferPool.Return(ref points); + ConvexHull.ConvexHullTriangleSource triangleSource = new(convexHull); + QuickList triangles = new(16, BufferPool); + while (triangleSource.GetNextTriangle(out var a, out var b, out var c)) + { + triangles.Allocate(BufferPool) = new Triangle(a, b, c); + } + convexHull.Dispose(BufferPool); + + mesh = new Mesh(triangles, new Vector3(1), BufferPool); + mesh.ComputeClosedInertia(1, out inertia); + } + public void CreateBodyDescription(Random random, in RigidPose pose, in BodyVelocity velocity, out BodyDescription description) { //For the sake of the stress test, every single body has its own shape that gets removed when the body is removed. @@ -169,7 +192,7 @@ public void CreateBodyDescription(Random random, in RigidPose pose, in BodyVeloc } else { - switch (random.Next(0, 7)) + switch (random.Next(0, 8)) { default: { @@ -213,6 +236,14 @@ public void CreateBodyDescription(Random random, in RigidPose pose, in BodyVeloc shapeIndex = Simulation.Shapes.Add(new BigCompound(children, Simulation.Shapes, BufferPool)); } break; + case 7: + { + //As usual: avoid dynamic meshes. They're slow and triangles are infinitely thin, so behavior probably won't be what you want. + //But dynamic meshes do exist, and so this demo shall test them. + CreateRandomMesh(out var mesh, out inertia); + shapeIndex = Simulation.Shapes.Add(mesh); + } + break; } } @@ -226,7 +257,7 @@ public void CreateBodyDescription(Random random, in RigidPose pose, in BodyVeloc }; switch (random.Next(3)) { - case 0: description.Collidable.Continuity = ContinuousDetection.Discrete(); break; + case 0: description.Collidable.Continuity = ContinuousDetection.Discrete(0, 0.2f); break; case 1: description.Collidable.Continuity = ContinuousDetection.Passive; break; case 2: description.Collidable.Continuity = ContinuousDetection.Continuous(1e-3f, 1e-3f, maximumSpeculativeMargin: 0.2f); break; } @@ -237,7 +268,6 @@ public override void Update(Window window, Camera camera, Input input, float dt) var timestepDuration = 1f / 60f; time += timestepDuration; - Simulation.Solver.ValidateConstrainedKinematicsSet(); //Occasionally, the animation stops completely. The resulting velocities will be zero, so the kinematics will have a chance to rest (testing kinematic rest states). var dip = 0.1; var progressionMultiplier = 0.5 - dip + (1 + dip) * 0.5 * Math.Cos(time * 0.25); @@ -286,7 +316,6 @@ public override void Update(Window window, Camera camera, Input input, float dt) } } } - Simulation.Solver.ValidateConstrainedKinematicsSet(); //Remove some statics from the simulation. var missingStaticsAsymptote = 512; @@ -298,7 +327,6 @@ public override void Update(Window window, Camera camera, Input input, float dt) Simulation.Statics.RemoveAt(indexToRemove); removedStatics.Enqueue(staticDescription, BufferPool); } - Simulation.Solver.ValidateConstrainedKinematicsSet(); var staticApplyDescriptionsPerFrame = 8; for (int i = 0; i < staticApplyDescriptionsPerFrame; ++i) @@ -314,8 +342,6 @@ public override void Update(Window window, Camera camera, Input input, float dt) Simulation.Statics.ApplyDescription(handleToReapply, staticDescription); } - Simulation.Solver.ValidateConstrainedKinematicsSet(); - //Add some of the missing static bodies back into the simulation. var staticAddCount = removedStatics.Count * (staticRemovalsPerFrame / (float)missingStaticsAsymptote); for (int i = 0; i < staticAddCount; ++i) @@ -326,13 +352,12 @@ public override void Update(Window window, Camera camera, Input input, float dt) } - Simulation.Solver.ValidateConstrainedKinematicsSet(); //Spray some shapes! int newShapeCount = 1; var spawnPose = new RigidPose(new Vector3(0, 10, 0)); for (int i = 0; i < newShapeCount; ++i) { - CreateBodyDescription(random, spawnPose, new BodyVelocity(new Vector3(-60 + 120 * (float)random.NextDouble(), 0, -60 + 120 * (float)random.NextDouble()), default), out var bodyDescription); + CreateBodyDescription(random, spawnPose, new BodyVelocity(new Vector3(-30 + 60 * (float)random.NextDouble(), 0, -30 + 60 * (float)random.NextDouble()), default), out var bodyDescription); dynamicHandles.Enqueue(Simulation.Bodies.Add(bodyDescription), BufferPool); } int targetAsymptote = 65536; @@ -352,7 +377,6 @@ public override void Update(Window window, Camera camera, Input input, float dt) break; } } - Simulation.Solver.ValidateConstrainedKinematicsSet(); //Change some dynamic objects without adding/removing them to make sure all the state transition stuff works reasonably well. var dynamicApplyDescriptionsPerFrame = 8; @@ -368,10 +392,8 @@ public override void Update(Window window, Camera camera, Input input, float dt) newDescription.LocalInertia = default; } Simulation.Bodies.ApplyDescription(handle, newDescription); - Simulation.Solver.ValidateConstraintReferenceKinematicity(); } - Simulation.Solver.ValidateConstrainedKinematicsSet(); base.Update(window, camera, input, dt); if (input != null && input.WasPushed(OpenTK.Input.Key.P)) diff --git a/Demos/Program.cs b/Demos/Program.cs index 55f0735c2..88fbc389a 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -23,8 +23,8 @@ static void Main(string[] args) //HeadlessTest.Test(content, 4, 32, 512); //HeadlessTest.Test(content, 8, 32, 1024); //HeadlessTest.Test(content, 4, 32, 512); - HeadlessTest.Test(content, 4, 128, 1024); - //DeterminismTest.Test(content, 1, 16384); + //HeadlessTest.Test(content, 4, 128, 1024); + DeterminismTest.Test(content, 1, 16384); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); From 11b1013a2885a5a7d76a6ef605277edc0fbf2ee1 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 12 Nov 2021 09:51:22 -0600 Subject: [PATCH 283/947] Fixed demo mesh rendering using out of date data. --- DemoRenderer/Renderer.cs | 14 +++++++------- DemoRenderer/ShapeDrawing/ShapesExtractor.cs | 6 ++++-- Demos/Demos/FountainStressTestDemo.cs | 4 ++-- Demos/Program.cs | 2 +- 4 files changed, 14 insertions(+), 12 deletions(-) diff --git a/DemoRenderer/Renderer.cs b/DemoRenderer/Renderer.cs index 2b7220447..50b848108 100644 --- a/DemoRenderer/Renderer.cs +++ b/DemoRenderer/Renderer.cs @@ -233,16 +233,16 @@ public void Render(Camera camera) //All ray traced shapes use analytic coverage writes to get antialiasing. context.OutputMerger.SetBlendState(a2cBlendState); - SphereRenderer.Render(context, camera, Surface.Resolution, SpanConverter.AsSpan(Shapes.spheres.Span), 0, Shapes.spheres.Count); - CapsuleRenderer.Render(context, camera, Surface.Resolution, SpanConverter.AsSpan(Shapes.capsules.Span), 0, Shapes.capsules.Count); - CylinderRenderer.Render(context, camera, Surface.Resolution, SpanConverter.AsSpan(Shapes.cylinders.Span), 0, Shapes.cylinders.Count); + SphereRenderer.Render(context, camera, Surface.Resolution, Shapes.spheres.Span, 0, Shapes.spheres.Count); + CapsuleRenderer.Render(context, camera, Surface.Resolution, Shapes.capsules.Span, 0, Shapes.capsules.Count); + CylinderRenderer.Render(context, camera, Surface.Resolution, Shapes.cylinders.Span, 0, Shapes.cylinders.Count); //Non-raytraced shapes just use regular opaque rendering. context.OutputMerger.SetBlendState(opaqueBlendState); - BoxRenderer.Render(context, camera, Surface.Resolution, SpanConverter.AsSpan(Shapes.boxes.Span), 0, Shapes.boxes.Count); - TriangleRenderer.Render(context, camera, Surface.Resolution, SpanConverter.AsSpan(Shapes.triangles.Span), 0, Shapes.triangles.Count); - MeshRenderer.Render(context, camera, Surface.Resolution, SpanConverter.AsSpan(Shapes.meshes.Span), 0, Shapes.meshes.Count); - LineRenderer.Render(context, camera, Surface.Resolution, SpanConverter.AsSpan(Lines.lines.Span), 0, Lines.lines.Count); + BoxRenderer.Render(context, camera, Surface.Resolution, Shapes.boxes.Span, 0, Shapes.boxes.Count); + TriangleRenderer.Render(context, camera, Surface.Resolution, Shapes.triangles.Span, 0, Shapes.triangles.Count); + MeshRenderer.Render(context, camera, Surface.Resolution, Shapes.meshes.Span, 0, Shapes.meshes.Count); + LineRenderer.Render(context, camera, Surface.Resolution, Lines.lines.Span, 0, Lines.lines.Count); Background.Render(context, camera); diff --git a/DemoRenderer/ShapeDrawing/ShapesExtractor.cs b/DemoRenderer/ShapeDrawing/ShapesExtractor.cs index e52f8dc56..5f78240f0 100644 --- a/DemoRenderer/ShapeDrawing/ShapesExtractor.cs +++ b/DemoRenderer/ShapeDrawing/ShapesExtractor.cs @@ -134,7 +134,8 @@ public unsafe void AddShape(void* shapeData, int shapeType, Shapes shapes, ref R instance.PackedColor = Helpers.PackColor(color); instance.PackedOrientation = Helpers.PackOrientationU64(ref pose.Orientation); instance.Scale = Vector3.One; - var id = (ulong)hull.Points.Memory ^ (ulong)hull.Points.Length; + //Memory can be reused, so we slightly reduce the probability of a bad reuse by taking the first 64 bits of data into the hash. + var id = (ulong)hull.Points.Memory ^ (ulong)hull.Points.Length ^ (*(ulong*)hull.Points.Memory); if (!MeshCache.TryGetExistingMesh(id, out instance.VertexStart, out var vertices)) { int triangleCount = 0; @@ -187,7 +188,8 @@ public unsafe void AddShape(void* shapeData, int shapeType, Shapes shapes, ref R instance.PackedColor = Helpers.PackColor(color); instance.PackedOrientation = Helpers.PackOrientationU64(ref pose.Orientation); instance.Scale = mesh.Scale; - var id = (ulong)mesh.Triangles.Memory ^ (ulong)mesh.Triangles.Length; + //Memory can be reused, so we slightly reduce the probability of a bad reuse by taking the first 64 bits of data into the hash. + var id = (ulong)mesh.Triangles.Memory ^ (ulong)mesh.Triangles.Length ^ (*(ulong*)mesh.Triangles.Memory); ; instance.VertexCount = mesh.Triangles.Length * 3; if (MeshCache.Allocate(id, instance.VertexCount, out instance.VertexStart, out var vertices)) { diff --git a/Demos/Demos/FountainStressTestDemo.cs b/Demos/Demos/FountainStressTestDemo.cs index ac935a243..1e5a65d4f 100644 --- a/Demos/Demos/FountainStressTestDemo.cs +++ b/Demos/Demos/FountainStressTestDemo.cs @@ -353,11 +353,11 @@ public override void Update(Window window, Camera camera, Input input, float dt) //Spray some shapes! - int newShapeCount = 1; + int newShapeCount = 8; var spawnPose = new RigidPose(new Vector3(0, 10, 0)); for (int i = 0; i < newShapeCount; ++i) { - CreateBodyDescription(random, spawnPose, new BodyVelocity(new Vector3(-30 + 60 * (float)random.NextDouble(), 0, -30 + 60 * (float)random.NextDouble()), default), out var bodyDescription); + CreateBodyDescription(random, spawnPose, new BodyVelocity(new Vector3(-30 + 60 * (float)random.NextDouble(), 75, -30 + 60 * (float)random.NextDouble()), default), out var bodyDescription); dynamicHandles.Enqueue(Simulation.Bodies.Add(bodyDescription), BufferPool); } int targetAsymptote = 65536; diff --git a/Demos/Program.cs b/Demos/Program.cs index 88fbc389a..1e914518c 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -24,7 +24,7 @@ static void Main(string[] args) //HeadlessTest.Test(content, 8, 32, 1024); //HeadlessTest.Test(content, 4, 32, 512); //HeadlessTest.Test(content, 4, 128, 1024); - DeterminismTest.Test(content, 1, 16384); + //DeterminismTest.Test(content, 1, 16384); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); From bd6526640f3a9a5e166b2391b6ca3a9c9ff6c1ca Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 12 Nov 2021 12:20:43 -0600 Subject: [PATCH 284/947] ConstraintLineExtractor now handles fallback empty slots correctly. --- .../Constraints/ConstraintLineExtractor.cs | 27 +++++++++++++------ 1 file changed, 19 insertions(+), 8 deletions(-) diff --git a/DemoRenderer/Constraints/ConstraintLineExtractor.cs b/DemoRenderer/Constraints/ConstraintLineExtractor.cs index 99f01a50f..bdf7ecead 100644 --- a/DemoRenderer/Constraints/ConstraintLineExtractor.cs +++ b/DemoRenderer/Constraints/ConstraintLineExtractor.cs @@ -10,6 +10,7 @@ using System.Numerics; using BepuPhysics.CollisionDetection; using BepuPhysics.Constraints.Contact; +using System.Threading; namespace DemoRenderer.Constraints { @@ -105,7 +106,7 @@ struct ThreadJob public int ConstraintCount; public int LineStart; public int LinesPerConstraint; - public QuickList jobLines; + public QuickList JobLines; } public bool Enabled { get; set; } = true; @@ -169,12 +170,16 @@ public ConstraintLineExtractor(BufferPool pool) Bodies bodies; Solver solver; + QuickList targetLines; private void ExecuteJob(int jobIndex) { ref var job = ref jobs[jobIndex]; ref var typeBatch = ref solver.Sets[job.SetIndex].Batches[job.BatchIndex].TypeBatches[job.TypeBatchIndex]; Debug.Assert(lineExtractors[typeBatch.TypeId] != null, "Jobs should only be created for types which are registered and used."); - lineExtractors[typeBatch.TypeId].ExtractLines(bodies, job.SetIndex, ref typeBatch, job.ConstraintStart, job.ConstraintCount, ref job.jobLines); + lineExtractors[typeBatch.TypeId].ExtractLines(bodies, job.SetIndex, ref typeBatch, job.ConstraintStart, job.ConstraintCount, ref job.JobLines); + //Because the number of lines is not easily known ahead of time, the job accumulates a list locally and then copies it into the global buffer afterwards. + var targetStart = Interlocked.Add(ref targetLines.Count, job.JobLines.Count) - job.JobLines.Count; + job.JobLines.Span.Slice(job.JobLines.Count).CopyTo(0, targetLines, targetStart, job.JobLines.Count); } @@ -209,13 +214,14 @@ internal void AddInstances(Bodies bodies, Solver solver, bool showConstraints, b LineStart = neededLineCapacity, LinesPerConstraint = extractor.LinesPerConstraint }, pool); + //Note that this is a conservative estimate. TypeBatches in the fallback constraint batch are not necessarily contiguous, so the allocated constraint count is possibly larger than the true number of constraints. neededLineCapacity += extractor.LinesPerConstraint * typeBatch.ConstraintCount; } } } } } - var maximumJobSize = Math.Max(1, neededLineCapacity / (jobsPerThread * Environment.ProcessorCount)); + var maximumJobSize = Math.Min(32, Math.Max(1, neededLineCapacity / (jobsPerThread * Environment.ProcessorCount))); var originalJobCount = jobs.Count; //Split jobs if they're larger than desired to help load balancing a little bit. This isn't terribly important, but it's pretty easy. for (int i = 0; i < originalJobCount; ++i) @@ -246,19 +252,24 @@ internal void AddInstances(Bodies bodies, Solver solver, bool showConstraints, b } } lines.EnsureCapacity(neededLineCapacity, pool); - lines.Count = neededLineCapacity; //Line additions will be performed on suballocated lists. This count will be used by the renderer when reading line data. + targetLines = lines; for (int i = 0; i < jobs.Count; ++i) { - //Creating a local copy of the list reference and count allows additions to proceed in parallel. - jobs[i].jobLines = new QuickList(lines.Span); - //By setting the count, we work around the fact that Array doesn't support slicing. - jobs[i].jobLines.Count = jobs[i].LineStart; + //Local lists for each worker let the accumulation proceed in parallel. The results will be copied by the workers after they finish with a chunk with an interlocked claim on the main buffer. + //Could be quicker with per *worker* lists, but this doesn't really matter. + jobs[i].JobLines = new QuickList(jobs[i].ConstraintCount * jobs[i].LinesPerConstraint, pool); } this.bodies = bodies; this.solver = solver; looper.For(0, jobs.Count, executeJobDelegate); this.bodies = null; this.solver = solver; + lines = targetLines; + targetLines = default; + for (int i = 0; i < jobs.Count; ++i) + { + jobs[i].JobLines.Dispose(pool); + } } public void Dispose() From ffc5b23f4da13efd742b4e79960007cd3d6e835c Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 13 Nov 2021 15:37:53 -0600 Subject: [PATCH 285/947] Avoided some potential degenerate geometry issues for convex hulls and meshes in fountain stress test demo, temp-fixed an undefined speculative margin in statics, fixed undefined world inertias for kinematics, made math checkers debug asserts, expanded some validation. --- BepuPhysics/Bodies.cs | 22 +++++++++- BepuPhysics/BodySet.cs | 4 +- .../Constraints/TwoBodyTypeProcessor.cs | 2 +- .../EmbeddedSubsteppingTimestepper2.cs | 8 ++-- BepuPhysics/InvasiveHashDiagnostics.cs | 12 +++++- BepuPhysics/Simulation.cs | 1 - BepuPhysics/Solver.cs | 38 +++++++++-------- BepuPhysics/Statics.cs | 1 + BepuUtilities/MathChecker.cs | 42 +++++++++---------- Demos/Demos/FountainStressTestDemo.cs | 23 +++++++--- Demos/Demos/RopeTwistDemo.cs | 2 +- Demos/Program.cs | 2 +- 12 files changed, 102 insertions(+), 55 deletions(-) diff --git a/BepuPhysics/Bodies.cs b/BepuPhysics/Bodies.cs index 23b120520..501797912 100644 --- a/BepuPhysics/Bodies.cs +++ b/BepuPhysics/Bodies.cs @@ -405,10 +405,15 @@ public void SetLocalInertia(BodyHandle handle, in BodyInertia localInertia) } //Note that the HandleToLocation slot reference is still valid; it may have been updated, but handle slots don't move. ref var set = ref Sets[location.SetIndex]; + ref var inertiaReference = ref set.SolverStates[location.Index].Inertia; ref var localInertiaReference = ref set.SolverStates[location.Index].Inertia.Local; var nowKinematic = IsKinematic(localInertia); - var previouslyKinematic = IsKinematicUnsafeGCHole(ref localInertiaReference); - localInertiaReference = localInertia; + var previouslyKinematic = IsKinematicUnsafeGCHole(ref inertiaReference.Local); + inertiaReference.Local = localInertia; + //The world inertia is updated on demand and is not 'persistent' data. + //In the event that the body is now kinematic, it won't be updated by pose integration and such, so it should be initialized to zeroes. + //Since initializing it to zeroes unconditionally avoids dynamics having some undefined data lingering around in the worst case, might as well. + inertiaReference.World = default; UpdateForKinematicStateChange(handle, ref location, ref set, previouslyKinematic, nowKinematic); } @@ -564,6 +569,19 @@ internal void ValidateMotionStates() } } + + internal void ValidateAwakeMotionStatesByHash(HashDiagnosticType type) + { + var instance = InvasiveHashDiagnostics.Instance; + ref int hash = ref instance.GetHashForType(type); + ref var set = ref ActiveSet; + for (int j = 0; j < set.Count; ++j) + { + ref var state = ref set.SolverStates[j]; + instance.ContributeToHash(ref hash, state); + } + } + internal void ResizeSetsCapacity(int setsCapacity, int potentiallyAllocatedCount) { Debug.Assert(setsCapacity >= potentiallyAllocatedCount && potentiallyAllocatedCount <= Sets.Length); diff --git a/BepuPhysics/BodySet.cs b/BepuPhysics/BodySet.cs index 953965df2..a62d823c4 100644 --- a/BepuPhysics/BodySet.cs +++ b/BepuPhysics/BodySet.cs @@ -128,8 +128,10 @@ internal void ApplyDescriptionByIndex(int index, in BodyDescription description) ref var state = ref SolverStates[index]; state.Motion.Pose = description.Pose; state.Motion.Velocity = description.Velocity; - //Note that the world inertia is only valid in the velocity integration->pose integration interval, so we don't need to initialize it here. state.Inertia.Local = description.LocalInertia; + //Note that the world inertia is only valid in the velocity integration->pose integration interval, so we don't need to initialize it here for dynamics. + //Kinematics, though, can have their inertia updates skipped at runtime since the world inverse inertia should always be a bunch of zeroes, so we pre-zero it. + state.Inertia.World = default; ref var collidable = ref Collidables[index]; collidable.Continuity = description.Collidable.Continuity; //Note that we change the shape here. If the collidable transitions from shapeless->shapeful or shapeful->shapeless, the broad phase has to be notified diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 704252634..0f7c2808d 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -264,7 +264,7 @@ public unsafe override void WarmStart2(dt), accumulatedImpulses, ref prestep); - + function.WarmStart2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB); if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) diff --git a/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs b/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs index 73f5c8a7e..23e960f27 100644 --- a/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs +++ b/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs @@ -49,7 +49,7 @@ public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDi simulation.Solver.ValidateTrailingTypeBatchBodyReferences(); simulation.Solver.ValidateFallbackBatchEmptySlotReferences(); simulation.Solver.ValidateFallbackBatchAccessSafety(); - simulation.Solver.ValidateFallbackBatchAccumulatedImpulses(); + simulation.Solver.ValidateAccumulatedImpulses(); simulation.Solver.ValidateConstraintMaps(); simulation.Solver.ValidateConstraintReferenceKinematicity(); simulation.Solver.ValidateConstrainedKinematicsSet(); @@ -60,7 +60,7 @@ public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDi simulation.Solver.ValidateTrailingTypeBatchBodyReferences(); simulation.Solver.ValidateFallbackBatchEmptySlotReferences(); simulation.Solver.ValidateFallbackBatchAccessSafety(); - simulation.Solver.ValidateFallbackBatchAccumulatedImpulses(); + simulation.Solver.ValidateAccumulatedImpulses(); simulation.Solver.ValidateConstraintMaps(); simulation.Solver.ValidateConstraintReferenceKinematicity(); simulation.Solver.ValidateConstrainedKinematicsSet(); @@ -77,7 +77,7 @@ public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDi simulation.Solver.ValidateTrailingTypeBatchBodyReferences(); simulation.Solver.ValidateFallbackBatchEmptySlotReferences(); simulation.Solver.ValidateFallbackBatchAccessSafety(); - simulation.Solver.ValidateFallbackBatchAccumulatedImpulses(); + simulation.Solver.ValidateAccumulatedImpulses(); simulation.Solver.ValidateConstraintMaps(); simulation.Solver.ValidateConstraintReferenceKinematicity(); simulation.Solver.ValidateConstrainedKinematicsSet(); @@ -95,6 +95,8 @@ public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDi simulation.Solver.DisposeConstraintIntegrationResponsibilities(); SubstepsComplete?.Invoke(dt, threadDispatcher); + + simulation.Solver.ValidateAccumulatedImpulses(); simulation.IncrementallyOptimizeDataStructures(threadDispatcher); } } diff --git a/BepuPhysics/InvasiveHashDiagnostics.cs b/BepuPhysics/InvasiveHashDiagnostics.cs index 589198d08..31b881284 100644 --- a/BepuPhysics/InvasiveHashDiagnostics.cs +++ b/BepuPhysics/InvasiveHashDiagnostics.cs @@ -13,6 +13,16 @@ namespace BepuPhysics /// public enum HashDiagnosticType { + AwakeBodyStates0, + AwakeBodyStates1, + AwakeBodyStates2, + AwakeBodyStates3, + AwakeBodyStates4, + AwakeBodyStates5, + AwakeBodyStates6, + AwakeBodyStates7, + AwakeBodyStates8, + AwakeBodyStates9, AddSleepingToActiveForFallback, SolverBodyReferenceBeforeCollisionDetection, SolverBodyReferenceBeforePreflush, @@ -47,7 +57,7 @@ public class InvasiveHashDiagnostics /// /// This is meant as an internal diagnostic utility, so hardcoding some things is totally fine. /// - const int HashTypeCount = 24; + const int HashTypeCount = 34; public static InvasiveHashDiagnostics Instance; public static void Initialize(int runCount, int hashCapacityPerType) { diff --git a/BepuPhysics/Simulation.cs b/BepuPhysics/Simulation.cs index 3a6c26ab9..203149895 100644 --- a/BepuPhysics/Simulation.cs +++ b/BepuPhysics/Simulation.cs @@ -155,7 +155,6 @@ private static int ValidateAndCountShapefulBodies(ref BodySet bodySet, ref Tree return shapefulBodyCount; } - [Conditional("DEBUG")] internal void ValidateCollidables() { diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index 95ecaae66..e607ffe6a 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -606,7 +606,7 @@ internal unsafe void ValidateFallbackBodyReferencesByHash(HashDiagnosticType has { var bodyIndex = bodyVector[innerIndex]; if (bodyIndex >= 0) - hashes.ContributeToHash(ref hash, bodies.ActiveSet.IndexToHandle[bodyIndex]); + hashes.ContributeToHash(ref hash, bodies.ActiveSet.IndexToHandle[bodyIndex & Bodies.BodyReferenceMask].Value); else hashes.ContributeToHash(ref hash, bodyIndex); } @@ -621,31 +621,35 @@ internal unsafe void ValidateFallbackBodyReferencesByHash(HashDiagnosticType has } [Conditional("DEBUG")] - internal unsafe void ValidateFallbackBatchAccumulatedImpulses() + internal unsafe void ValidateAccumulatedImpulses() { - ref var set = ref ActiveSet; - if (set.Batches.Count > FallbackBatchThreshold) + var impulseMemory = stackalloc float[16]; + var impulsesEnumerator = new ValidateAccumulatedImpulsesEnumerator { AccumulatedImpulses = impulseMemory }; + for (int i = 0; i < Sets.Length; ++i) { - ref var batch = ref set.Batches[FallbackBatchThreshold]; - var impulseMemory = stackalloc float[16]; - var impulsesEnumerator = new ValidateAccumulatedImpulsesEnumerator { AccumulatedImpulses = impulseMemory }; - for (int typeBatchIndex = 0; typeBatchIndex < batch.TypeBatches.Count; ++typeBatchIndex) + ref var set = ref Sets[i]; + if (!set.Allocated) + continue; + for (int batchIndex = 0; batchIndex < set.Batches.Count; ++batchIndex) { - ref var typeBatch = ref batch.TypeBatches[typeBatchIndex]; - var dofCount = TypeProcessors[typeBatch.TypeId].ConstrainedDegreesOfFreedom; - for (int constraintIndex = 0; constraintIndex < typeBatch.ConstraintCount; ++constraintIndex) + ref var batch = ref set.Batches[batchIndex]; + for (int typeBatchIndex = 0; typeBatchIndex < batch.TypeBatches.Count; ++typeBatchIndex) { - if (typeBatch.IndexToHandle[constraintIndex].Value >= 0) + ref var typeBatch = ref batch.TypeBatches[typeBatchIndex]; + var dofCount = TypeProcessors[typeBatch.TypeId].ConstrainedDegreesOfFreedom; + for (int constraintIndex = 0; constraintIndex < typeBatch.ConstraintCount; ++constraintIndex) { - impulsesEnumerator.Index = 0; - TypeProcessors[typeBatch.TypeId].EnumerateAccumulatedImpulses(ref typeBatch, constraintIndex, ref impulsesEnumerator); - for (int dofIndex = 0; dofIndex < dofCount; ++dofIndex) + if (typeBatch.IndexToHandle[constraintIndex].Value >= 0) { - impulseMemory[dofIndex].Validate(); + impulsesEnumerator.Index = 0; + TypeProcessors[typeBatch.TypeId].EnumerateAccumulatedImpulses(ref typeBatch, constraintIndex, ref impulsesEnumerator); + for (int dofIndex = 0; dofIndex < dofCount; ++dofIndex) + { + impulseMemory[dofIndex].Validate(); + } } } } - } } } diff --git a/BepuPhysics/Statics.cs b/BepuPhysics/Statics.cs index 56a1697d6..016b95068 100644 --- a/BepuPhysics/Statics.cs +++ b/BepuPhysics/Statics.cs @@ -306,6 +306,7 @@ internal void ApplyDescriptionByIndexWithoutBroadPhaseModification - /// Checks the value to see if it is a NaN or infinite. If it is, an exception is thrown. + /// Checks the value to see if it is a NaN or infinite. If it is, a debug assertion will fail. /// This is only run when the CHECKMATH symbol is defined. /// [Conditional("CHECKMATH")] @@ -30,12 +30,12 @@ public static void Validate(this float f) { if (IsInvalid(f)) { - throw new InvalidOperationException("Invalid value."); + Debug.Fail("Invalid value."); } } /// - /// Checks the value to see if it is a NaN or infinite. If it is, an exception is thrown. + /// Checks the value to see if it is a NaN or infinite. If it is, a debug assertion will fail. /// This is only run when the CHECKMATH symbol is defined. /// [Conditional("CHECKMATH")] @@ -43,12 +43,12 @@ public static void Validate(this Vector2 v) { if (IsInvalid(v.LengthSquared())) { - throw new InvalidOperationException("Invalid value."); + Debug.Fail("Invalid value."); } } /// - /// Checks the value to see if it is a NaN or infinite. If it is, an exception is thrown. + /// Checks the value to see if it is a NaN or infinite. If it is, a debug assertion will fail. /// This is only run when the CHECKMATH symbol is defined. /// [Conditional("CHECKMATH")] @@ -56,12 +56,12 @@ public static void Validate(this Vector3 v) { if (IsInvalid(v.LengthSquared())) { - throw new InvalidOperationException("Invalid value."); + Debug.Fail("Invalid value."); } } /// - /// Checks the value to see if it is a NaN or infinite. If it is, an exception is thrown. + /// Checks the value to see if it is a NaN or infinite. If it is, a debug assertion will fail. /// This is only run when the CHECKMATH symbol is defined. /// [Conditional("CHECKMATH")] @@ -69,13 +69,13 @@ public static void Validate(this Vector4 v) { if (IsInvalid(v.LengthSquared())) { - throw new InvalidOperationException("Invalid value."); + Debug.Fail("Invalid value."); } } /// - /// Checks the value to see if it is a NaN or infinite. If it is, an exception is thrown. + /// Checks the value to see if it is a NaN or infinite. If it is, a debug assertion will fail. /// This is only run when the CHECKMATH symbol is defined. /// [Conditional("CHECKMATH")] @@ -87,7 +87,7 @@ public static void Validate(this Matrix3x3 m) } /// - /// Checks the value to see if it is a NaN or infinite. If it is, an exception is thrown. + /// Checks the value to see if it is a NaN or infinite. If it is, a debug assertion will fail. /// This is only run when the CHECKMATH symbol is defined. /// [Conditional("CHECKMATH")] @@ -100,7 +100,7 @@ public static void Validate(this Matrix m) } /// - /// Checks the value to see if it is a NaN or infinite. If it is, an exception is thrown. + /// Checks the value to see if it is a NaN or infinite. If it is, a debug assertion will fail. /// This is only run when the CHECKMATH symbol is defined. /// [Conditional("CHECKMATH")] @@ -108,12 +108,12 @@ public static void Validate(this Quaternion q) { if (IsInvalid(q.LengthSquared())) { - throw new InvalidOperationException("Invalid value."); + Debug.Fail("Invalid value."); } } /// - /// Checks the value to see if it is a NaN or infinite. If it is, an exception is thrown. + /// Checks the value to see if it is a NaN or infinite. If it is, a debug assertion will fail. /// This is only run when the CHECKMATH symbol is defined. /// [Conditional("CHECKMATH")] @@ -122,12 +122,12 @@ public static void ValidateOrientation(this Quaternion q) var lengthSquared = q.LengthSquared(); if (IsInvalid(lengthSquared) && Math.Abs(1 - lengthSquared) < 1e-5f) { - throw new InvalidOperationException("Invalid value."); + Debug.Fail("Invalid value."); } } /// - /// Checks the value to see if it is a NaN or infinite. If it is, an exception is thrown. + /// Checks the value to see if it is a NaN or infinite. If it is, a debug assertion will fail. /// This is only run when the CHECKMATH symbol is defined. /// [Conditional("CHECKMATH")] @@ -142,7 +142,7 @@ public static void Validate(this Symmetric3x3 m) } /// - /// Checks the value to see if it is a NaN or infinite. If it is, an exception is thrown. + /// Checks the value to see if it is a NaN or infinite. If it is, a debug assertion will fail. /// This is only run when the CHECKMATH symbol is defined. /// [Conditional("CHECKMATH")] @@ -153,7 +153,7 @@ public static void Validate(this AffineTransform a) } /// - /// Checks the value to see if it is a NaN or infinite. If it is, an exception is thrown. + /// Checks the value to see if it is a NaN or infinite. If it is, a debug assertion will fail. /// This is only run when the CHECKMATH symbol is defined. /// [Conditional("CHECKMATH")] @@ -164,7 +164,7 @@ public static void Validate(this BoundingBox b) } /// - /// Checks the value to see if it is a NaN or infinite. If it is, an exception is thrown. + /// Checks the value to see if it is a NaN or infinite. If it is, a debug assertion will fail. /// This is only run when the CHECKMATH symbol is defined. /// [Conditional("CHECKMATH")] @@ -178,7 +178,7 @@ public static void Validate(this BoundingSphere b) public static void Validate(this Vector f, int laneCount = -1) { if (laneCount < -1 || laneCount > Vector.Count) - throw new ArgumentException("Invalid lane count."); + Debug.Fail("Invalid lane count."); if (laneCount == -1) laneCount = Vector.Count; ref var casted = ref Unsafe.As, float>(ref f); @@ -187,7 +187,7 @@ public static void Validate(this Vector f, int laneCount = -1) var value = Unsafe.Add(ref casted, i); if (float.IsNaN(value) || float.IsInfinity(value)) { - throw new InvalidOperationException($"Invalid floating point value: {value}."); + Debug.Fail($"Invalid floating point value: {value}."); } } } @@ -205,7 +205,7 @@ public static void Validate(this Vector f, Vector lanesToTest) var value = Unsafe.Add(ref castedValues, i); if (float.IsNaN(value) || float.IsInfinity(value)) { - throw new InvalidOperationException($"Invalid floating point value: {value}."); + Debug.Fail($"Invalid floating point value: {value}."); } } } diff --git a/Demos/Demos/FountainStressTestDemo.cs b/Demos/Demos/FountainStressTestDemo.cs index 1e5a65d4f..a2144fa47 100644 --- a/Demos/Demos/FountainStressTestDemo.cs +++ b/Demos/Demos/FountainStressTestDemo.cs @@ -98,11 +98,16 @@ void AddConvexShape(in TConvex convex, out TypedIndex shapeIndex, out B ConvexHull CreateRandomHull() { - const int pointCount = 32; + const int pointCount = 16; var points = new QuickList(pointCount, BufferPool); - for (int i = 0; i < pointCount; ++i) + //Create an initial tetrahedron to guarantee our random shape isn't degenerate. + points.AllocateUnsafely() = new Vector3(0.5f, 0.25f, 0.75f); + points.AllocateUnsafely() = points[0] + new Vector3(0.1f, 0, 0); + points.AllocateUnsafely() = points[0] + new Vector3(0, 0.1f, 0); + points.AllocateUnsafely() = points[0] + new Vector3(0, 0, 0.1f); + for (int i = 4; i < pointCount; ++i) { - points.AllocateUnsafely() = new Vector3(1 * (float)random.NextDouble(), 0.5f * (float)random.NextDouble(), 1.5f * (float)random.NextDouble()); + points.AllocateUnsafely() = new Vector3(1 * random.NextSingle(), 0.5f * random.NextSingle(), 1.5f * random.NextSingle()); } var hull = new ConvexHull(points.Span.Slice(points.Count), BufferPool, out _); points.Dispose(BufferPool); @@ -159,9 +164,15 @@ void CreateRandomCompound(out Buffer children, out BodyInertia in void CreateRandomMesh(out Mesh mesh, out BodyInertia inertia) { //We'll use a convex hull algorithm to generate the triangles for the mesh, rather than just spewing random triangle soups. - var pointCount = random.Next(4, 16); + var pointCount = random.Next(5, 16); BufferPool.Take(pointCount, out Buffer points); - for (int i = 0; i < pointCount; ++i) + //Create an initial tetrahedron to guarantee our random shape isn't degenerate. + points[0] = new Vector3(1); + points[1] = new Vector3(1) + new Vector3(0.1f, 0, 0); + points[2] = new Vector3(1) + new Vector3(0, 0.1f, 0); + points[3] = new Vector3(1) + new Vector3(0, 0, 0.1f); + + for (int i = 4; i < pointCount; ++i) { points[i] = 2f * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); } @@ -357,7 +368,7 @@ public override void Update(Window window, Camera camera, Input input, float dt) var spawnPose = new RigidPose(new Vector3(0, 10, 0)); for (int i = 0; i < newShapeCount; ++i) { - CreateBodyDescription(random, spawnPose, new BodyVelocity(new Vector3(-30 + 60 * (float)random.NextDouble(), 75, -30 + 60 * (float)random.NextDouble()), default), out var bodyDescription); + CreateBodyDescription(random, spawnPose, new BodyVelocity(new Vector3(-30 + 60 * random.NextSingle(), 75, -30 + 60 * random.NextSingle()), default), out var bodyDescription); dynamicHandles.Enqueue(Simulation.Bodies.Add(bodyDescription), BufferPool); } int targetAsymptote = 65536; diff --git a/Demos/Demos/RopeTwistDemo.cs b/Demos/Demos/RopeTwistDemo.cs index 00da22c32..a51b9d88c 100644 --- a/Demos/Demos/RopeTwistDemo.cs +++ b/Demos/Demos/RopeTwistDemo.cs @@ -136,7 +136,7 @@ bool TryCreateConstraint(int handleIndexA, int handleIndexB) for (int i = 0; i < bodyHandles.Length - 1; ++i) { //Note that you could also create constraints which span even more links. For example, connect i and i+1, i+2, i+4, i+8 and i+16 rather than just the nearest bodies. - //That would make it behave a bit more like the previous cheat constraint, but it can be useful. + //That tends to make mass ratios less of an issue, but this demo is a worst case stress test. for (int j = 1; j <= constraintsPerBody; ++j) { if (!TryCreateConstraint(i, i + j)) diff --git a/Demos/Program.cs b/Demos/Program.cs index 1e914518c..5faf4c4fe 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -24,7 +24,7 @@ static void Main(string[] args) //HeadlessTest.Test(content, 8, 32, 1024); //HeadlessTest.Test(content, 4, 32, 512); //HeadlessTest.Test(content, 4, 128, 1024); - //DeterminismTest.Test(content, 1, 16384); + DeterminismTest.Test(content, 1, 65536); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); From 81c77e50bb7d97035b3e219e17426203f45d32e9 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 13 Nov 2021 15:40:16 -0600 Subject: [PATCH 286/947] NextDouble->NextSingle. --- DemoTests/InertiaTensorTests.cs | 16 ++--- DemoTests/PairDeterminismTests.cs | 2 +- Demos/Demos/BlockChainDemo.cs | 10 +-- Demos/Demos/Cars/CarDemo.cs | 12 ++-- Demos/Demos/Characters/CharacterDemo.cs | 4 +- Demos/Demos/CollisionQueryDemo.cs | 2 +- Demos/Demos/CompoundTestDemo.cs | 4 +- Demos/Demos/CustomVoxelCollidableDemo.cs | 2 +- Demos/Demos/FountainStressTestDemo.cs | 34 +++++----- Demos/Demos/NewtDemo.cs | 4 +- Demos/Demos/PyramidDemo.cs | 2 +- Demos/Demos/RayCastingDemo.cs | 14 ++-- Demos/Demos/Sponsors/SponsorDemo.cs | 6 +- Demos/Demos/Sponsors/SponsorNewt.cs | 6 +- Demos/Demos/SweepDemo.cs | 2 +- Demos/Demos/Tanks/TankDemo.cs | 10 +-- .../SpecializedTests/BatchedCollisionTests.cs | 14 ++-- .../BroadPhaseStressTestDemo.cs | 2 +- Demos/SpecializedTests/CharacterTestDemo.cs | 6 +- Demos/SpecializedTests/CompoundBoundTests.cs | 2 +- Demos/SpecializedTests/ConvexHullTestDemo.cs | 6 +- Demos/SpecializedTests/CylinderTestDemo.cs | 6 +- .../SpecializedTests/DepthRefinerTestDemo.cs | 2 +- .../IntertreeThreadingTests.cs | 2 +- .../Media/NewtDemandingSacrificeVideoDemo.cs | 4 +- .../SpecializedTests/MeshReductionTestDemo.cs | 6 +- .../MeshSerializationTestDemo.cs | 2 +- .../PyramidAwakenerTestDemo.cs | 2 +- Demos/SpecializedTests/RayTesting.cs | 64 +++++++++---------- Demos/SpecializedTests/ShapePileTestDemo.cs | 4 +- Demos/SpecializedTests/TestHelpers.cs | 6 +- Demos/SpecializedTests/TreeTest.cs | 2 +- Demos/SpecializedTests/TriangleRayTestDemo.cs | 34 +++++----- Demos/SpecializedTests/VolumeQueryTests.cs | 14 ++-- 34 files changed, 154 insertions(+), 154 deletions(-) diff --git a/DemoTests/InertiaTensorTests.cs b/DemoTests/InertiaTensorTests.cs index a95617041..e15f2f909 100644 --- a/DemoTests/InertiaTensorTests.cs +++ b/DemoTests/InertiaTensorTests.cs @@ -228,22 +228,22 @@ public static void Test() const int shapeTrials = 64; for (int i = 0; i < shapeTrials; ++i) { - var tester = new SphereInertiaTester { Sphere = new Sphere(0.01f + (float)random.NextDouble() * 10) }; + var tester = new SphereInertiaTester { Sphere = new Sphere(0.01f + random.NextSingle() * 10) }; CheckInertia(ref tester); } for (int i = 0; i < shapeTrials; ++i) { - var tester = new CapsuleInertiaTester { Capsule = new Capsule(0.01f + (float)random.NextDouble() * 10, 0.01f + (float)random.NextDouble() * 10) }; + var tester = new CapsuleInertiaTester { Capsule = new Capsule(0.01f + random.NextSingle() * 10, 0.01f + random.NextSingle() * 10) }; CheckInertia(ref tester); } for (int i = 0; i < shapeTrials; ++i) { - var tester = new CylinderInertiaTester { Cylinder = new Cylinder(0.01f + (float)random.NextDouble() * 10, 0.01f + (float)random.NextDouble() * 10) }; + var tester = new CylinderInertiaTester { Cylinder = new Cylinder(0.01f + random.NextSingle() * 10, 0.01f + random.NextSingle() * 10) }; CheckInertia(ref tester); } for (int i = 0; i < shapeTrials; ++i) { - var tester = new BoxInertiaTester { Box = new Box(0.01f + 10 * (float)random.NextDouble(), 0.01f + 10 * (float)random.NextDouble(), 0.01f + 10 * (float)random.NextDouble()) }; + var tester = new BoxInertiaTester { Box = new Box(0.01f + 10 * random.NextSingle(), 0.01f + 10 * random.NextSingle(), 0.01f + 10 * random.NextSingle()) }; CheckInertia(ref tester); } for (int i = 0; i < shapeTrials; ++i) @@ -251,9 +251,9 @@ public static void Test() var tester = new TriangleInertiaTester { Triangle = new Triangle( - new Vector3(-2 + 4 * (float)random.NextDouble(), -2 + 4 * (float)random.NextDouble(), -2 + 4 * (float)random.NextDouble()), - new Vector3(-2 + 4 * (float)random.NextDouble(), -2 + 4 * (float)random.NextDouble(), -2 + 4 * (float)random.NextDouble()), - new Vector3(-2 + 4 * (float)random.NextDouble(), -2 + 4 * (float)random.NextDouble(), -2 + 4 * (float)random.NextDouble())), + new Vector3(-2 + 4 * random.NextSingle(), -2 + 4 * random.NextSingle(), -2 + 4 * random.NextSingle()), + new Vector3(-2 + 4 * random.NextSingle(), -2 + 4 * random.NextSingle(), -2 + 4 * random.NextSingle()), + new Vector3(-2 + 4 * random.NextSingle(), -2 + 4 * random.NextSingle(), -2 + 4 * random.NextSingle())), }; CheckInertia(ref tester); } @@ -264,7 +264,7 @@ public static void Test() var pointSet = new QuickList(pointCount, pool); for (int j = 0; j < pointCount; ++j) { - pointSet.AllocateUnsafely() = new Vector3(-1 + 2 * (float)random.NextDouble(), -1 + 2 * (float)random.NextDouble(), -1 + 2 * (float)random.NextDouble()); + pointSet.AllocateUnsafely() = new Vector3(-1 + 2 * random.NextSingle(), -1 + 2 * random.NextSingle(), -1 + 2 * random.NextSingle()); } for (int j = 0; j < pointSet.Count; ++j) { diff --git a/DemoTests/PairDeterminismTests.cs b/DemoTests/PairDeterminismTests.cs index ab34e3558..f86204e96 100644 --- a/DemoTests/PairDeterminismTests.cs +++ b/DemoTests/PairDeterminismTests.cs @@ -224,7 +224,7 @@ public static void Test() var points = new QuickList(pointCount, pool); for (int i = 0; i < pointCount; ++i) { - points.AllocateUnsafely() = new Vector3(3 * (float)random.NextDouble(), 2 * (float)random.NextDouble(), (float)random.NextDouble()); + points.AllocateUnsafely() = new Vector3(3 * random.NextSingle(), 2 * random.NextSingle(), random.NextSingle()); } var pointsBuffer = points.Span.Slice(points.Count); ConvexHullHelper.CreateShape(pointsBuffer, pool, out _, out var convexHull); diff --git a/Demos/Demos/BlockChainDemo.cs b/Demos/Demos/BlockChainDemo.cs index 137cc4cf6..6c22c6d80 100644 --- a/Demos/Demos/BlockChainDemo.cs +++ b/Demos/Demos/BlockChainDemo.cs @@ -77,10 +77,10 @@ public override void Update(Window window, Camera camera, Input input, float dt) if (input.WasPushed(OpenTK.Input.Key.Z)) { //INVEST TODAY FOR INCREDIBLE RETURNS DON'T MISS OUT LOOK AT THE COINS THERE ARE A LOT OF THEM AND THEY COULD BE YOURS - var origin = new Vector3(-30, 5, -30) + new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()) * new Vector3(60, 30, 60); + var origin = new Vector3(-30, 5, -30) + new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) * new Vector3(60, 30, 60); for (int i = 0; i < 128; ++i) { - var direction = new Vector3(-1) + 2 * new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()); + var direction = new Vector3(-1) + 2 * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); var length = direction.Length(); if (length > 1e-7f) direction /= length; @@ -88,9 +88,9 @@ public override void Update(Window window, Camera camera, Input input, float dt) direction = new Vector3(0, 1, 0); coinDescription.Pose = new( - origin + direction * 10 * (float)random.NextDouble(), - QuaternionEx.Normalize(new Quaternion(0.01f + (float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()))); - coinDescription.Velocity = new(direction * (5 + 30 * (float)random.NextDouble())); + origin + direction * 10 * random.NextSingle(), + QuaternionEx.Normalize(new Quaternion(0.01f + random.NextSingle(), random.NextSingle(), random.NextSingle(), random.NextSingle()))); + coinDescription.Velocity = new(direction * (5 + 30 * random.NextSingle())); Simulation.Bodies.Add(coinDescription); } } diff --git a/Demos/Demos/Cars/CarDemo.cs b/Demos/Demos/Cars/CarDemo.cs index c9b30a354..0eb438f17 100644 --- a/Demos/Demos/Cars/CarDemo.cs +++ b/Demos/Demos/Cars/CarDemo.cs @@ -91,10 +91,10 @@ public override void Initialize(ContentArchive content, Camera camera) var landmarkSpan = new Vector3(raceTrack.QuadrantRadius, 0, raceTrack.QuadrantRadius); for (int j = 0; j < 25; ++j) { - var buildingShape = new Box(10 + (float)random.NextDouble() * 10, 20 + (float)random.NextDouble() * 20, 10 + (float)random.NextDouble() * 10); + var buildingShape = new Box(10 + random.NextSingle() * 10, 20 + random.NextSingle() * 20, 10 + random.NextSingle() * 10); Simulation.Statics.Add(new StaticDescription( - new Vector3(0, buildingShape.HalfHeight, 0) + landmarkMin + landmarkSpan * new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()), - QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, (float)random.NextDouble() * MathF.PI), + new Vector3(0, buildingShape.HalfHeight, 0) + landmarkMin + landmarkSpan * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()), + QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, random.NextSingle() * MathF.PI), new CollidableDescription(Simulation.Shapes.Add(buildingShape)))); } } @@ -105,15 +105,15 @@ public override void Initialize(ContentArchive content, Camera camera) for (int i = 0; i < aiCount; ++i) { //The AI cars are very similar, except... we handicap them a little to make the player good about themselves. - var position = min + span * new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()); - var orientation = QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), (float)random.NextDouble() * MathF.PI * 2); + var position = min + span * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); + var orientation = QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), random.NextSingle() * MathF.PI * 2); aiControllers[i].Controller = new SimpleCarController(SimpleCar.Create(Simulation, properties, new RigidPose(position, orientation), bodyShapeIndex, bodyInertia, 0.5f, wheelShapeIndex, wheelInertia, 2f, new Vector3(-x, y, frontZ), new Vector3(x, y, frontZ), new Vector3(-x, y, backZ), new Vector3(x, y, backZ), new Vector3(0, -1, 0), 0.25f, new SpringSettings(5, 0.7f), QuaternionEx.CreateFromAxisAngle(Vector3.UnitZ, MathF.PI * 0.5f)), forwardSpeed: 50, forwardForce: 5, zoomMultiplier: 2, backwardSpeed: 10, backwardForce: 4, idleForce: 0.25f, brakeForce: 7, steeringSpeed: 1.5f, maximumSteeringAngle: MathF.PI * 0.23f, wheelBaseLength: wheelBaseLength, wheelBaseWidth: wheelBaseWidth, ackermanSteering: 1); - aiControllers[i].LaneOffset = (float)random.NextDouble() * 20 - 10; + aiControllers[i].LaneOffset = random.NextSingle() * 20 - 10; } DemoMeshHelper.CreateDeformedPlane(planeWidth, planeWidth, diff --git a/Demos/Demos/Characters/CharacterDemo.cs b/Demos/Demos/Characters/CharacterDemo.cs index f1646c1de..f4c5676fc 100644 --- a/Demos/Demos/Characters/CharacterDemo.cs +++ b/Demos/Demos/Characters/CharacterDemo.cs @@ -41,8 +41,8 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) for (int j = 0; j < 12; ++j) { var position = origin + new Vector3(i, 0, j) * spacing; - var orientation = QuaternionEx.CreateFromAxisAngle(Vector3.Normalize(new Vector3(0.0001f) + new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble())), 10 * (float)random.NextDouble()); - var shape = new Box(0.1f + 0.3f * (float)random.NextDouble(), 0.1f + 0.3f * (float)random.NextDouble(), 0.1f + 0.3f * (float)random.NextDouble()); + var orientation = QuaternionEx.CreateFromAxisAngle(Vector3.Normalize(new Vector3(0.0001f) + new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle())), 10 * random.NextSingle()); + var shape = new Box(0.1f + 0.3f * random.NextSingle(), 0.1f + 0.3f * random.NextSingle(), 0.1f + 0.3f * random.NextSingle()); var collidable = new CollidableDescription(Simulation.Shapes.Add(shape)); shape.ComputeInertia(1, out var inertia); var choice = (i + j) % 3; diff --git a/Demos/Demos/CollisionQueryDemo.cs b/Demos/Demos/CollisionQueryDemo.cs index 62f8644c7..f726cde97 100644 --- a/Demos/Demos/CollisionQueryDemo.cs +++ b/Demos/Demos/CollisionQueryDemo.cs @@ -40,7 +40,7 @@ public override void Initialize(ContentArchive content, Camera camera) var descriptionToDrop = BodyDescription.CreateDynamic(new Vector3(), shapeToDropInertia, new CollidableDescription(Simulation.Shapes.Add(shapeToDrop)), new BodyActivityDescription(0.01f)); for (int i = 0; i < 128; ++i) { - descriptionToDrop.Pose.Position = new Vector3(-5 + 10 * (float)random.NextDouble(), 45 + 150 * (float)random.NextDouble(), -5 + 10 * (float)random.NextDouble()); + descriptionToDrop.Pose.Position = new Vector3(-5 + 10 * random.NextSingle(), 45 + 150 * random.NextSingle(), -5 + 10 * random.NextSingle()); Simulation.Bodies.Add(descriptionToDrop); } diff --git a/Demos/Demos/CompoundTestDemo.cs b/Demos/Demos/CompoundTestDemo.cs index 6f069996b..4929881b9 100644 --- a/Demos/Demos/CompoundTestDemo.cs +++ b/Demos/Demos/CompoundTestDemo.cs @@ -171,11 +171,11 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) for (int i = 0; i < 128; ++i) { RigidPose localPose; - localPose.Position = new Vector3(12, 6, 12) * (0.5f * new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()) - Vector3.One); + localPose.Position = new Vector3(12, 6, 12) * (0.5f * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) - Vector3.One); float orientationLengthSquared; do { - localPose.Orientation = new Quaternion((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()); + localPose.Orientation = new Quaternion(random.NextSingle(), random.NextSingle(), random.NextSingle(), random.NextSingle()); orientationLengthSquared = QuaternionEx.LengthSquared(ref localPose.Orientation); } while (orientationLengthSquared < 1e-9f); diff --git a/Demos/Demos/CustomVoxelCollidableDemo.cs b/Demos/Demos/CustomVoxelCollidableDemo.cs index 1effba012..492de85b4 100644 --- a/Demos/Demos/CustomVoxelCollidableDemo.cs +++ b/Demos/Demos/CustomVoxelCollidableDemo.cs @@ -437,7 +437,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var descriptionToDrop = BodyDescription.CreateDynamic(new Vector3(), shapeToDropInertia, new CollidableDescription(Simulation.Shapes.Add(shapeToDrop)), new BodyActivityDescription(0.01f)); for (int i = 0; i < 4096; ++i) { - descriptionToDrop.Pose.Position = new Vector3(15 + 10 * (float)random.NextDouble(), 45 + 150 * (float)random.NextDouble(), 15 + 10 * (float)random.NextDouble()); + descriptionToDrop.Pose.Position = new Vector3(15 + 10 * random.NextSingle(), 45 + 150 * random.NextSingle(), 15 + 10 * random.NextSingle()); Simulation.Bodies.Add(descriptionToDrop); } diff --git a/Demos/Demos/FountainStressTestDemo.cs b/Demos/Demos/FountainStressTestDemo.cs index a2144fa47..0a3290df2 100644 --- a/Demos/Demos/FountainStressTestDemo.cs +++ b/Demos/Demos/FountainStressTestDemo.cs @@ -126,32 +126,32 @@ void CreateRandomCompound(out Buffer children, out BodyInertia in switch (random.Next(0, 5)) { default: - AddConvexShape(new Sphere(0.35f + 0.35f * (float)random.NextDouble()), out shapeIndex, out childInertia); + AddConvexShape(new Sphere(0.35f + 0.35f * random.NextSingle()), out shapeIndex, out childInertia); break; case 1: AddConvexShape(new Capsule( - 0.35f + 0.35f * (float)random.NextDouble(), - 0.35f + 0.35f * (float)random.NextDouble()), out shapeIndex, out childInertia); + 0.35f + 0.35f * random.NextSingle(), + 0.35f + 0.35f * random.NextSingle()), out shapeIndex, out childInertia); break; case 2: AddConvexShape(new Box( - 0.35f + 0.35f * (float)random.NextDouble(), - 0.35f + 0.35f * (float)random.NextDouble(), - 0.35f + 0.35f * (float)random.NextDouble()), out shapeIndex, out childInertia); + 0.35f + 0.35f * random.NextSingle(), + 0.35f + 0.35f * random.NextSingle(), + 0.35f + 0.35f * random.NextSingle()), out shapeIndex, out childInertia); break; case 3: - AddConvexShape(new Cylinder(0.1f + (float)random.NextDouble(), 0.2f + (float)random.NextDouble()), out shapeIndex, out childInertia); + AddConvexShape(new Cylinder(0.1f + random.NextSingle(), 0.2f + random.NextSingle()), out shapeIndex, out childInertia); break; case 4: AddConvexShape(CreateRandomHull(), out shapeIndex, out childInertia); break; } RigidPose localPose; - localPose.Position = new Vector3(2, 2, 2) * (0.5f * new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()) - Vector3.One); + localPose.Position = new Vector3(2, 2, 2) * (0.5f * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) - Vector3.One); float orientationLengthSquared; do { - localPose.Orientation = new Quaternion((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()); + localPose.Orientation = new Quaternion(random.NextSingle(), random.NextSingle(), random.NextSingle(), random.NextSingle()); } while ((orientationLengthSquared = localPose.Orientation.LengthSquared()) < 1e-9f); QuaternionEx.Scale(localPose.Orientation, 1f / MathF.Sqrt(orientationLengthSquared), out localPose.Orientation); @@ -207,27 +207,27 @@ public void CreateBodyDescription(Random random, in RigidPose pose, in BodyVeloc { default: { - AddConvexShape(new Sphere(0.35f + 0.35f * (float)random.NextDouble()), out shapeIndex, out inertia); + AddConvexShape(new Sphere(0.35f + 0.35f * random.NextSingle()), out shapeIndex, out inertia); } break; case 1: { AddConvexShape(new Capsule( - 0.35f + 0.35f * (float)random.NextDouble(), - 0.35f + 0.35f * (float)random.NextDouble()), out shapeIndex, out inertia); + 0.35f + 0.35f * random.NextSingle(), + 0.35f + 0.35f * random.NextSingle()), out shapeIndex, out inertia); } break; case 2: { AddConvexShape(new Box( - 0.35f + 0.6f * (float)random.NextDouble(), - 0.35f + 0.6f * (float)random.NextDouble(), - 0.35f + 0.6f * (float)random.NextDouble()), out shapeIndex, out inertia); + 0.35f + 0.6f * random.NextSingle(), + 0.35f + 0.6f * random.NextSingle(), + 0.35f + 0.6f * random.NextSingle()), out shapeIndex, out inertia); } break; case 3: { - AddConvexShape(new Cylinder(0.1f + 0.5f * (float)random.NextDouble(), 0.2f + (float)random.NextDouble()), out shapeIndex, out inertia); + AddConvexShape(new Cylinder(0.1f + 0.5f * random.NextSingle(), 0.2f + random.NextSingle()), out shapeIndex, out inertia); } break; case 4: @@ -348,7 +348,7 @@ public override void Update(Window window, Camera camera, Input input, float dt) //Statics don't have as much in the way of transitions. They can't be shapeless, and going from one shape to another doesn't anything that a pose change doesn't. For now, we'll just test the application of descriptions with different poses. var mutatedDescription = staticDescription; mutatedDescription.Pose.Position.Y += 50; - QuaternionEx.Concatenate(mutatedDescription.Pose.Orientation, QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), (float)random.NextDouble() * MathF.PI), out mutatedDescription.Pose.Orientation); + QuaternionEx.Concatenate(mutatedDescription.Pose.Orientation, QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), random.NextSingle() * MathF.PI), out mutatedDescription.Pose.Orientation); Simulation.Statics.ApplyDescription(handleToReapply, mutatedDescription); Simulation.Statics.ApplyDescription(handleToReapply, staticDescription); } diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index fe8ee3500..9f5598e00 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -741,13 +741,13 @@ void Test2() var random = new Random(5); for (int i = 0; i < pointCount; ++i) { - points.AllocateUnsafely() = new Vector3(3 * (float)random.NextDouble(), 1 * (float)random.NextDouble(), 3 * (float)random.NextDouble()); + points.AllocateUnsafely() = new Vector3(3 * random.NextSingle(), 1 * random.NextSingle(), 3 * random.NextSingle()); } var convexHull1 = new ConvexHull(points, BufferPool, out _); points.Count = 0; for (int i = 0; i < pointCount; ++i) { - points.AllocateUnsafely() = new Vector3(3 * (float)random.NextDouble(), 1 * (float)random.NextDouble(), 3 * (float)random.NextDouble()); + points.AllocateUnsafely() = new Vector3(3 * random.NextSingle(), 1 * random.NextSingle(), 3 * random.NextSingle()); } var convexHull2 = new ConvexHull(points, BufferPool, out _); convexHull1.ComputeInertia(1, out var scalarInertiaA); diff --git a/Demos/Demos/PyramidDemo.cs b/Demos/Demos/PyramidDemo.cs index 255a5ac12..9a18589b4 100644 --- a/Demos/Demos/PyramidDemo.cs +++ b/Demos/Demos/PyramidDemo.cs @@ -61,7 +61,7 @@ public override void Update(Window window, Camera camera, Input input, float dt) if (input != null && input.WasPushed(OpenTK.Input.Key.Z)) { //Create the shape that we'll launch at the pyramids when the user presses a button. - var bulletShape = new Sphere(0.5f + 5 * (float)random.NextDouble()); + var bulletShape = new Sphere(0.5f + 5 * random.NextSingle()); //Note that the use of radius^3 for mass can produce some pretty serious mass ratios. //Observe what happens when a large ball sits on top of a few boxes with a fraction of the mass- //the collision appears much squishier and less stable. For most games, if you want to maintain rigidity, you'll want to use some combination of: diff --git a/Demos/Demos/RayCastingDemo.cs b/Demos/Demos/RayCastingDemo.cs index c9b2aeb8f..f007ef591 100644 --- a/Demos/Demos/RayCastingDemo.cs +++ b/Demos/Demos/RayCastingDemo.cs @@ -79,7 +79,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var random = new Random(5); for (int i = 0; i < pointCount; ++i) { - points.AllocateUnsafely() = new Vector3(1 * (float)random.NextDouble(), 1 * (float)random.NextDouble(), 1 * (float)random.NextDouble()); + points.AllocateUnsafely() = new Vector3(1 * random.NextSingle(), 1 * random.NextSingle(), 1 * random.NextSingle()); } var hullShape = new ConvexHull(points, BufferPool, out _); var sphereIndex = Simulation.Shapes.Add(sphere); @@ -101,14 +101,14 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) { for (int k = 0; k < length; ++k) { - var r = new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()); + var r = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); var location = spacing * (new Vector3(i, j, k) + new Vector3(-width, -height, -length) * 0.5f) + randomizationBase + r * randomizationSpan; Quaternion orientation; - orientation.X = -1 + 2 * (float)random.NextDouble(); - orientation.Y = -1 + 2 * (float)random.NextDouble(); - orientation.Z = -1 + 2 * (float)random.NextDouble(); - orientation.W = 0.01f + (float)random.NextDouble(); + orientation.X = -1 + 2 * random.NextSingle(); + orientation.Y = -1 + 2 * random.NextSingle(); + orientation.Z = -1 + 2 * random.NextSingle(); + orientation.W = 0.01f + random.NextSingle(); QuaternionEx.Normalize(ref orientation); var shapeIndex = ((i + j + k) % 5) switch { @@ -220,7 +220,7 @@ static Vector3 GetDirection(Random random) float length; do { - direction = 2 * new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()) - Vector3.One; + direction = 2 * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) - Vector3.One; length = direction.Length(); } while (length < 1e-7f); diff --git a/Demos/Demos/Sponsors/SponsorDemo.cs b/Demos/Demos/Sponsors/SponsorDemo.cs index 5bfea6ca7..e3ba5e6dc 100644 --- a/Demos/Demos/Sponsors/SponsorDemo.cs +++ b/Demos/Demos/Sponsors/SponsorDemo.cs @@ -138,8 +138,8 @@ public override void Initialize(ContentArchive content, Camera camera) var characterCollidable = new CollidableDescription(Simulation.Shapes.Add(new Capsule(0.5f, 1f))); for (int i = 0; i < characterCount; ++i) { - var position2D = newtArenaMin + (newtArenaMax - newtArenaMin) * new Vector2((float)random.NextDouble(), (float)random.NextDouble()); - var targetPosition = 0.5f * (newtArenaMin + (newtArenaMax - newtArenaMin) * new Vector2((float)random.NextDouble(), (float)random.NextDouble())); + var position2D = newtArenaMin + (newtArenaMax - newtArenaMin) * new Vector2(random.NextSingle(), random.NextSingle()); + var targetPosition = 0.5f * (newtArenaMin + (newtArenaMax - newtArenaMin) * new Vector2(random.NextSingle(), random.NextSingle())); characterAIs.AllocateUnsafely() = new SponsorCharacterAI(characterControllers, characterCollidable, new Vector3(position2D.X, 5, position2D.Y), targetPosition); } @@ -150,7 +150,7 @@ public override void Initialize(ContentArchive content, Camera camera) for (int i = 0; i < hutCount; ++i) { - var position2D = newtArenaMin + (newtArenaMax - newtArenaMin) * new Vector2((float)random.NextDouble(), (float)random.NextDouble()); + var position2D = newtArenaMin + (newtArenaMax - newtArenaMin) * new Vector2(random.NextSingle(), random.NextSingle()); ColosseumDemo.CreateRing(Simulation, new Vector3(position2D.X, 0, position2D.Y), hutBoxShape, obstacleDescription, 5, 2, random.Next(1, 5)); } diff --git a/Demos/Demos/Sponsors/SponsorNewt.cs b/Demos/Demos/Sponsors/SponsorNewt.cs index 7b27d06f6..2e34cbe2a 100644 --- a/Demos/Demos/Sponsors/SponsorNewt.cs +++ b/Demos/Demos/Sponsors/SponsorNewt.cs @@ -18,8 +18,8 @@ public struct SponsorNewt public SponsorNewt(Simulation simulation, TypedIndex shape, float height, in Vector2 arenaMin, in Vector2 arenaMax, Random random, int sponsorIndex) : this() { var arenaSpan = arenaMax - arenaMin; - var position = arenaMin + arenaSpan * new Vector2((float)random.NextDouble(), (float)random.NextDouble()); - var angle = MathF.PI * 2 * (float)random.NextDouble(); + var position = arenaMin + arenaSpan * new Vector2(random.NextSingle(), random.NextSingle()); + var angle = MathF.PI * 2 * random.NextSingle(); BodyHandle = simulation.Bodies.Add(BodyDescription.CreateKinematic(new RigidPose(new Vector3(position.X, height, position.Y), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, angle)), new CollidableDescription(shape), new BodyActivityDescription(-1))); SponsorIndex = sponsorIndex; } @@ -40,7 +40,7 @@ public void Update(Simulation simulation, double time, float height, in Vector2 //Choose a jump location. It should be within the arena, and generally somewhere ahead of the newt. QuaternionEx.TransformUnitZ(body.Pose.Orientation, out var backward); jumpStart = new Vector2(body.Pose.Position.X, body.Pose.Position.Z); - jumpEnd = jumpStart + new Vector2(backward.X + (float)random.NextDouble() * 1.4f - 0.7f, backward.Z + (float)random.NextDouble() * 1.4f - 0.7f) * -20; + jumpEnd = jumpStart + new Vector2(backward.X + random.NextSingle() * 1.4f - 0.7f, backward.Z + random.NextSingle() * 1.4f - 0.7f) * -20; jumpEnd -= jumpStart * 0.05f; jumpEnd = Vector2.Max(arenaMin, Vector2.Min(arenaMax, jumpEnd)); jumpStartTime = time; diff --git a/Demos/Demos/SweepDemo.cs b/Demos/Demos/SweepDemo.cs index f8a67a657..7176fe79b 100644 --- a/Demos/Demos/SweepDemo.cs +++ b/Demos/Demos/SweepDemo.cs @@ -81,7 +81,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var random = new Random(5); for (int i = 0; i < pointCount; ++i) { - points.AllocateUnsafely() = new Vector3((float)random.NextDouble() - 0.5f, (float)random.NextDouble() - 0.5f, (float)random.NextDouble() - 0.5f); + points.AllocateUnsafely() = new Vector3(random.NextSingle() - 0.5f, random.NextSingle() - 0.5f, random.NextSingle() - 0.5f); } ConvexHullHelper.CreateShape(points.Span.Slice(points.Count), BufferPool, out _, out hull); points.Dispose(BufferPool); diff --git a/Demos/Demos/Tanks/TankDemo.cs b/Demos/Demos/Tanks/TankDemo.cs index f61a40c33..c01753155 100644 --- a/Demos/Demos/Tanks/TankDemo.cs +++ b/Demos/Demos/Tanks/TankDemo.cs @@ -111,11 +111,11 @@ public override void Initialize(ContentArchive content, Camera camera) var landmarkSpan = landmarkMax - landmarkMin; for (int j = 0; j < 25; ++j) { - var buildingShape = new Box(10 + (float)random.NextDouble() * 10, 20 + (float)random.NextDouble() * 20, 10 + (float)random.NextDouble() * 10); - var position = landmarkMin + landmarkSpan * new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()); + var buildingShape = new Box(10 + random.NextSingle() * 10, 20 + random.NextSingle() * 20, 10 + random.NextSingle() * 10); + var position = landmarkMin + landmarkSpan * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); Simulation.Statics.Add(new StaticDescription( new Vector3(0, buildingShape.HalfHeight - 4f + GetHeightForPosition(position.X, position.Z, planeWidth, inverseTerrainScale, terrainPosition), 0) + position, - QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, (float)random.NextDouble() * MathF.PI), + QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, random.NextSingle() * MathF.PI), new CollidableDescription(Simulation.Shapes.Add(buildingShape)))); } @@ -138,13 +138,13 @@ public override void Initialize(ContentArchive content, Camera camera) var playAreaSpan = playAreaMax - playAreaMin; for (int i = 0; i < aiTankCount; ++i) { - var horizontalPosition = playAreaMin + new Vector2((float)random.NextDouble(), (float)random.NextDouble()) * playAreaSpan; + var horizontalPosition = playAreaMin + new Vector2(random.NextSingle(), random.NextSingle()) * playAreaSpan; aiTanks.AllocateUnsafely() = new AITank { Controller = new TankController( Tank.Create(Simulation, bodyProperties, BufferPool, new RigidPose( new Vector3(horizontalPosition.X, 10, horizontalPosition.Y), - QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), (float)random.NextDouble() * 0.1f)), + QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), random.NextSingle() * 0.1f)), tankDescription), 20, 5, 2, 1, 3.5f), HitPoints = 5 }; diff --git a/Demos/SpecializedTests/BatchedCollisionTests.cs b/Demos/SpecializedTests/BatchedCollisionTests.cs index 58d6ce4dd..1d361b564 100644 --- a/Demos/SpecializedTests/BatchedCollisionTests.cs +++ b/Demos/SpecializedTests/BatchedCollisionTests.cs @@ -120,17 +120,17 @@ unsafe static void Test(in TA a, in TB static void GetRandomPose(Random random, out RigidPose pose) { - pose.Position = new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()); + pose.Position = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); float orientationLengthSquared; do { pose.Orientation = new Quaternion { - X = 2 * (float)random.NextDouble() - 1, - Y = 2 * (float)random.NextDouble() - 1, - Z = 2 * (float)random.NextDouble() - 1, - W = 2 * (float)random.NextDouble() - 1 + X = 2 * random.NextSingle() - 1, + Y = 2 * random.NextSingle() - 1, + Z = 2 * random.NextSingle() - 1, + W = 2 * random.NextSingle() - 1 }; } while ((orientationLengthSquared = pose.Orientation.LengthSquared()) < 1e-5f); @@ -164,8 +164,8 @@ public static void Test() //points.Allocate(pool) = new Vector3(1, 1, 1); for (int i = 0; i < pointCount; ++i) { - points.AllocateUnsafely() = new Vector3((float)random.NextDouble(), 1 * (float)random.NextDouble(), (float)random.NextDouble()); - //points.AllocateUnsafely() = new Vector3(0, 1, 0) + Vector3.Normalize(new Vector3((float)random.NextDouble() * 2 - 1, (float)random.NextDouble() * 2 - 1, (float)random.NextDouble() * 2 - 1)) * (float)random.NextDouble(); + points.AllocateUnsafely() = new Vector3(random.NextSingle(), 1 * random.NextSingle(), random.NextSingle()); + //points.AllocateUnsafely() = new Vector3(0, 1, 0) + Vector3.Normalize(new Vector3(random.NextSingle() * 2 - 1, random.NextSingle() * 2 - 1, random.NextSingle() * 2 - 1)) * random.NextSingle(); } var pointsBuffer = points.Span.Slice(points.Count); diff --git a/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs b/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs index 3c161416b..39387e32c 100644 --- a/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs +++ b/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs @@ -40,7 +40,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) { for (int k = 0; k < length; ++k) { - var r = new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()); + var r = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); var location = spacing * (new Vector3(i, j, k) + new Vector3(-width, 1, -length)) + randomizationBase + r * randomizationSpan; if ((i + j + k) % 2 == 1) { diff --git a/Demos/SpecializedTests/CharacterTestDemo.cs b/Demos/SpecializedTests/CharacterTestDemo.cs index 87994b06d..7f5156cc1 100644 --- a/Demos/SpecializedTests/CharacterTestDemo.cs +++ b/Demos/SpecializedTests/CharacterTestDemo.cs @@ -29,7 +29,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) ref var character = ref characters.AllocateCharacter( Simulation.Bodies.Add( BodyDescription.CreateDynamic( - new Vector3(250 * (float)random.NextDouble() - 125, 2, 250 * (float)random.NextDouble() - 125), new BodyInertia { InverseMass = 1 }, + new Vector3(250 * random.NextSingle() - 125, 2, 250 * random.NextSingle() - 125), new BodyInertia { InverseMass = 1 }, new CollidableDescription(Simulation.Shapes.Add(new Capsule(0.5f, 1f))), new BodyActivityDescription(-1)))); @@ -51,8 +51,8 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) // for (int j = 0; j < 100; ++j) // { // var position = origin + new Vector3(i, 0, j) * spacing; - // var orientation = Quaternion.CreateFromAxisAngle(Vector3.Normalize(new Vector3(0.0001f) + new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble())), 10 * (float)random.NextDouble()); - // var shape = new Box(0.1f + 0.3f * (float)random.NextDouble(), 0.1f + 0.3f * (float)random.NextDouble(), 0.1f + 0.3f * (float)random.NextDouble()); + // var orientation = Quaternion.CreateFromAxisAngle(Vector3.Normalize(new Vector3(0.0001f) + new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle())), 10 * random.NextSingle()); + // var shape = new Box(0.1f + 0.3f * random.NextSingle(), 0.1f + 0.3f * random.NextSingle(), 0.1f + 0.3f * random.NextSingle()); // var collidable = new CollidableDescription(Simulation.Shapes.Add(shape), 0.1f); // shape.ComputeInertia(1, out var inertia); // var choice = (i + j) % 3; diff --git a/Demos/SpecializedTests/CompoundBoundTests.cs b/Demos/SpecializedTests/CompoundBoundTests.cs index d838407f8..5a0ba545b 100644 --- a/Demos/SpecializedTests/CompoundBoundTests.cs +++ b/Demos/SpecializedTests/CompoundBoundTests.cs @@ -137,7 +137,7 @@ void GetEstimatedExpansion(in Vector3 localPoseA, in Vector3 angularVelocityA, i Vector3 GetRandomVector(float width, Random random) { - return new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()) * width - new Vector3(width * 0.5f); + return new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) * width - new Vector3(width * 0.5f); } Random random = new Random(5); public unsafe override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) diff --git a/Demos/SpecializedTests/ConvexHullTestDemo.cs b/Demos/SpecializedTests/ConvexHullTestDemo.cs index 2e82e9ed0..e8497638d 100644 --- a/Demos/SpecializedTests/ConvexHullTestDemo.cs +++ b/Demos/SpecializedTests/ConvexHullTestDemo.cs @@ -58,8 +58,8 @@ public override void Initialize(ContentArchive content, Camera camera) var random = new Random(5); for (int i = 0; i < pointCount; ++i) { - points.AllocateUnsafely() = new Vector3(3 * (float)random.NextDouble(), 1 * (float)random.NextDouble(), 3 * (float)random.NextDouble()); - //points.AllocateUnsafely() = new Vector3(0, 1, 0) + Vector3.Normalize(new Vector3((float)random.NextDouble() * 2 - 1, (float)random.NextDouble() * 2 - 1, (float)random.NextDouble() * 2 - 1)) * (float)random.NextDouble(); + points.AllocateUnsafely() = new Vector3(3 * random.NextSingle(), 1 * random.NextSingle(), 3 * random.NextSingle()); + //points.AllocateUnsafely() = new Vector3(0, 1, 0) + Vector3.Normalize(new Vector3(random.NextSingle() * 2 - 1, random.NextSingle() * 2 - 1, random.NextSingle() * 2 - 1)) * random.NextSingle(); } var pointsBuffer = points.Span.Slice(points.Count); @@ -149,7 +149,7 @@ void TestConvexHullCreation() var points = new QuickList(pointCount, BufferPool); for (int i = 0; i < pointCount; ++i) { - points.AllocateUnsafely() = new Vector3(1 * (float)random.NextDouble(), 2 * (float)random.NextDouble(), 3 * (float)random.NextDouble()); + points.AllocateUnsafely() = new Vector3(1 * random.NextSingle(), 2 * random.NextSingle(), 3 * random.NextSingle()); } var pointsBuffer = points.Span.Slice(points.Count); diff --git a/Demos/SpecializedTests/CylinderTestDemo.cs b/Demos/SpecializedTests/CylinderTestDemo.cs index dc1bf4204..43d503c97 100644 --- a/Demos/SpecializedTests/CylinderTestDemo.cs +++ b/Demos/SpecializedTests/CylinderTestDemo.cs @@ -73,13 +73,13 @@ private static void TestSegmentCylinder() for (int i = 0; i < warmupCount + capsuleTests; ++i) { Vector3 randomPointNearCylinder; - var capsule = new Capsule(0.2f + .8f * (float)random.NextDouble(), 0.2f + 0.8f * (float)random.NextDouble()); + var capsule = new Capsule(0.2f + .8f * random.NextSingle(), 0.2f + 0.8f * random.NextSingle()); var minimumDistance = 1f * (cylinder.Radius + cylinder.HalfLength); var minimumDistanceSquared = minimumDistance * minimumDistance; while (true) { randomPointNearCylinder = new Vector3((cylinder.Radius + capsule.HalfLength) * 2, (cylinder.HalfLength + capsule.HalfLength) * 2, (cylinder.Radius + capsule.HalfLength) * 2) * - (new Vector3(2) * new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()) - Vector3.One); + (new Vector3(2) * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) - Vector3.One); var pointOnCylinderAxis = new Vector3(0, MathF.Max(-cylinder.HalfLength, MathF.Min(cylinder.HalfLength, randomPointNearCylinder.Y)), 0); var offset = randomPointNearCylinder - pointOnCylinderAxis; var lengthSquared = offset.LengthSquared(); @@ -91,7 +91,7 @@ private static void TestSegmentCylinder() float directionLengthSquared; do { - direction = new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()) * new Vector3(2) - Vector3.One; + direction = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) * new Vector3(2) - Vector3.One; directionLengthSquared = direction.LengthSquared(); } while (directionLengthSquared < 1e-8f); direction /= MathF.Sqrt(directionLengthSquared); diff --git a/Demos/SpecializedTests/DepthRefinerTestDemo.cs b/Demos/SpecializedTests/DepthRefinerTestDemo.cs index a1d822742..ccd399640 100644 --- a/Demos/SpecializedTests/DepthRefinerTestDemo.cs +++ b/Demos/SpecializedTests/DepthRefinerTestDemo.cs @@ -111,7 +111,7 @@ // var random = new Random(5); // for (int i = 0; i < pointCount; ++i) // { -// randomizedPoints.AllocateUnsafely() = new Vector3(33 * (float)random.NextDouble(), 1 * (float)random.NextDouble(), 13 * (float)random.NextDouble()); +// randomizedPoints.AllocateUnsafely() = new Vector3(33 * random.NextSingle(), 1 * random.NextSingle(), 13 * random.NextSingle()); // } // var shapeA = new ConvexHull(randomizedPoints.Span.Slice(randomizedPoints.Count), BufferPool, out var hullCenter); // //var poseA = new RigidPose(new Vector3(12, 0.5f, 12), Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), MathF.PI * 0.5f)); diff --git a/Demos/SpecializedTests/IntertreeThreadingTests.cs b/Demos/SpecializedTests/IntertreeThreadingTests.cs index eb6b3064a..3d54fd4c0 100644 --- a/Demos/SpecializedTests/IntertreeThreadingTests.cs +++ b/Demos/SpecializedTests/IntertreeThreadingTests.cs @@ -15,7 +15,7 @@ public static class IntertreeThreadingTests { static void GetRandomLocation(Random random, ref BoundingBox locationBounds, out Vector3 location) { - location = (locationBounds.Max - locationBounds.Min) * new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()) + locationBounds.Min; + location = (locationBounds.Max - locationBounds.Min) * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) + locationBounds.Min; } struct OverlapHandler : IOverlapHandler { diff --git a/Demos/SpecializedTests/Media/NewtDemandingSacrificeVideoDemo.cs b/Demos/SpecializedTests/Media/NewtDemandingSacrificeVideoDemo.cs index 334e23ff5..a896ab540 100644 --- a/Demos/SpecializedTests/Media/NewtDemandingSacrificeVideoDemo.cs +++ b/Demos/SpecializedTests/Media/NewtDemandingSacrificeVideoDemo.cs @@ -40,7 +40,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) BodyVelocity GetRandomizedVelocity(in Vector3 linearVelocity) { - return new BodyVelocity { Linear = linearVelocity, Angular = new Vector3(-20) + 40 * new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()) }; + return new BodyVelocity { Linear = linearVelocity, Angular = new Vector3(-20) + 40 * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) }; } public override void Update(Window window, Camera camera, Input input, float dt) @@ -50,7 +50,7 @@ public override void Update(Window window, Camera camera, Input input, float dt) Min = new Vector3(-10, 5, 70), Max = new Vector3(10, 15, 70) }); - var linearVelocity = Vector3.Normalize(new Vector3(-2 + 4 * (float)random.NextDouble(), 31 + 4 * (float)random.NextDouble(), 50) - pose.Position) * 40; + var linearVelocity = Vector3.Normalize(new Vector3(-2 + 4 * random.NextSingle(), 31 + 4 * random.NextSingle(), 50) - pose.Position) * 40; var handles = RagdollDemo.AddRagdoll(pose.Position, pose.Orientation, ragdollIndex++, filters, Simulation); //This could be done better, but... ... .... .......... new BodyReference(handles.Hips, Simulation.Bodies).Velocity = GetRandomizedVelocity(linearVelocity); diff --git a/Demos/SpecializedTests/MeshReductionTestDemo.cs b/Demos/SpecializedTests/MeshReductionTestDemo.cs index 284175aa3..10b1b8e3b 100644 --- a/Demos/SpecializedTests/MeshReductionTestDemo.cs +++ b/Demos/SpecializedTests/MeshReductionTestDemo.cs @@ -116,8 +116,8 @@ public override void Initialize(ContentArchive content, Camera camera) //var random = new Random(5); //for (int i = 0; i < pointCount; ++i) //{ - // points.AllocateUnsafely() = new Vector3(3 * (float)random.NextDouble(), 1 * (float)random.NextDouble(), 3 * (float)random.NextDouble()); - // //points.AllocateUnsafely() = new Vector3(0, 1, 0) + Vector3.Normalize(new Vector3((float)random.NextDouble() * 2 - 1, (float)random.NextDouble() * 2 - 1, (float)random.NextDouble() * 2 - 1)) * (float)random.NextDouble(); + // points.AllocateUnsafely() = new Vector3(3 * random.NextSingle(), 1 * random.NextSingle(), 3 * random.NextSingle()); + // //points.AllocateUnsafely() = new Vector3(0, 1, 0) + Vector3.Normalize(new Vector3(random.NextSingle() * 2 - 1, random.NextSingle() * 2 - 1, random.NextSingle() * 2 - 1)) * random.NextSingle(); //} //var convexHull = new ConvexHull(points.Span.Slice(points.Count), BufferPool, out _); //box.ComputeInertia(1, out var boxInertia); @@ -157,7 +157,7 @@ public override void Initialize(ContentArchive content, Camera camera) // break; case 2: default: - var box = new Box(1 + 128 * (float)random.NextDouble(), 1 + 128 * (float)random.NextDouble(), 1 + 128 * (float)random.NextDouble()); + var box = new Box(1 + 128 * random.NextSingle(), 1 + 128 * random.NextSingle(), 1 + 128 * random.NextSingle()); box.ComputeInertia(1, out var boxInertia); bodyDescription.Collidable.Shape = Simulation.Shapes.Add(box); bodyDescription.LocalInertia = boxInertia; diff --git a/Demos/SpecializedTests/MeshSerializationTestDemo.cs b/Demos/SpecializedTests/MeshSerializationTestDemo.cs index 09ae34fb2..343e7bd2a 100644 --- a/Demos/SpecializedTests/MeshSerializationTestDemo.cs +++ b/Demos/SpecializedTests/MeshSerializationTestDemo.cs @@ -45,7 +45,7 @@ public override void Initialize(ContentArchive content, Camera camera) var descriptionToDrop = BodyDescription.CreateDynamic(new Vector3(), shapeToDropInertia, new CollidableDescription(Simulation.Shapes.Add(shapeToDrop)), new BodyActivityDescription(0.01f)); for (int i = 0; i < 1024; ++i) { - descriptionToDrop.Pose.Position = new Vector3(8 + 240 * (float)random.NextDouble(), 10 + 10 * (float)random.NextDouble(), 8 + 112 * (float)random.NextDouble()); + descriptionToDrop.Pose.Position = new Vector3(8 + 240 * random.NextSingle(), 10 + 10 * random.NextSingle(), 8 + 112 * random.NextSingle()); Simulation.Bodies.Add(descriptionToDrop); } diff --git a/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs b/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs index 3e409319b..6f4aa5f77 100644 --- a/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs +++ b/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs @@ -61,7 +61,7 @@ public override void Update(Window window, Camera camera, Input input, float dt) frameIndex++; if (frameIndex % 64 == 0) { - var bulletShape = new Sphere(0.5f + 5 * (float)random.NextDouble()); + var bulletShape = new Sphere(0.5f + 5 * random.NextSingle()); bulletShape.ComputeInertia(bulletShape.Radius * bulletShape.Radius * bulletShape.Radius, out var bulletInertia); var bulletShapeIndex = Simulation.Shapes.Add(bulletShape); var bodyDescription = BodyDescription.CreateConvexDynamic( diff --git a/Demos/SpecializedTests/RayTesting.cs b/Demos/SpecializedTests/RayTesting.cs index 687be0972..43423d2fb 100644 --- a/Demos/SpecializedTests/RayTesting.cs +++ b/Demos/SpecializedTests/RayTesting.cs @@ -19,7 +19,7 @@ public void GetRandomShape(Random random, out Sphere shape) { const float sizeMin = 0.1f; const float sizeSpan = 200; - shape = new Sphere(sizeMin + sizeSpan * (float)random.NextDouble()); + shape = new Sphere(sizeMin + sizeSpan * random.NextSingle()); } public void GetPointInVolume(Random random, float innerMargin, ref Sphere shape, out Vector3 localPoint) { @@ -30,7 +30,7 @@ public void GetPointInVolume(Random random, float innerMargin, ref Sphere shape, min = -min; do { - localPoint = min + span * new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()); + localPoint = min + span * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); } while (localPoint.LengthSquared() > radiusSquared); } @@ -56,7 +56,7 @@ public void GetRandomShape(Random random, out Capsule shape) { const float sizeMin = 0.1f; const float sizeSpan = 200; - shape = new Capsule(sizeMin + sizeSpan * (float)random.NextDouble(), sizeMin * sizeSpan * (float)random.NextDouble()); + shape = new Capsule(sizeMin + sizeSpan * random.NextSingle(), sizeMin * sizeSpan * random.NextSingle()); } public void GetPointInVolume(Random random, float innerMargin, ref Capsule capsule, out Vector3 localPointInCapsule) { @@ -68,7 +68,7 @@ public void GetPointInVolume(Random random, float innerMargin, ref Capsule capsu min = -min; do { - localPointInCapsule = min + span * new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()); + localPointInCapsule = min + span * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); var projectedCandidate = new Vector3(0, Math.Max(-capsule.HalfLength, Math.Min(capsule.HalfLength, localPointInCapsule.Y)), 0); distanceSquared = Vector3.DistanceSquared(projectedCandidate, localPointInCapsule); @@ -85,7 +85,7 @@ public void GetSurface(Random random, ref Capsule capsule, out Vector3 localPoin min = -min; do { - localPointOnCapsule = min + span * new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()); + localPointOnCapsule = min + span * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); projectedCandidate = new Vector3(0, Math.Max(-capsule.HalfLength, Math.Min(capsule.HalfLength, localPointOnCapsule.Y)), 0); offset = localPointOnCapsule - projectedCandidate; distanceSquared = offset.LengthSquared(); @@ -111,7 +111,7 @@ public void GetRandomShape(Random random, out Cylinder shape) { const float sizeMin = 0.1f; const float sizeSpan = 200; - shape = new Cylinder(sizeMin + sizeSpan * (float)random.NextDouble(), sizeMin * sizeSpan * (float)random.NextDouble()); + shape = new Cylinder(sizeMin + sizeSpan * random.NextSingle(), sizeMin * sizeSpan * random.NextSingle()); } public void GetPointInVolume(Random random, float innerMargin, ref Cylinder cylinder, out Vector3 localPointInCylinder) { @@ -125,11 +125,11 @@ public void GetPointInVolume(Random random, float innerMargin, ref Cylinder cyli Vector2 randomHorizontal; do { - randomHorizontal = min + span * new Vector2((float)random.NextDouble(), (float)random.NextDouble()); + randomHorizontal = min + span * new Vector2(random.NextSingle(), random.NextSingle()); distanceSquared = randomHorizontal.LengthSquared(); } while (distanceSquared > radiusSquared); - localPointInCylinder = new Vector3(randomHorizontal.X, -effectiveHalfLength + 2 * effectiveHalfLength * (float)random.NextDouble(), randomHorizontal.Y); + localPointInCylinder = new Vector3(randomHorizontal.X, -effectiveHalfLength + 2 * effectiveHalfLength * random.NextSingle(), randomHorizontal.Y); } public void GetSurface(Random random, ref Cylinder cylinder, out Vector3 localPointOnCylinder, out Vector3 localNormal) @@ -149,14 +149,14 @@ public void GetSurface(Random random, ref Cylinder cylinder, out Vector3 localPo Vector2 randomHorizontal; do { - randomHorizontal = min + span * new Vector2((float)random.NextDouble(), (float)random.NextDouble()); + randomHorizontal = min + span * new Vector2(random.NextSingle(), random.NextSingle()); distanceSquared = randomHorizontal.LengthSquared(); } while (distanceSquared < 1e-7f); var horizontalNormal = randomHorizontal / (float)Math.Sqrt(distanceSquared); localNormal = new Vector3(horizontalNormal.X, 0, horizontalNormal.Y); var horizontalOffset = horizontalNormal * cylinder.Radius; - localPointOnCylinder = new Vector3(horizontalOffset.X, -cylinder.HalfLength + 2 * cylinder.HalfLength * (float)random.NextDouble(), horizontalOffset.Y); + localPointOnCylinder = new Vector3(horizontalOffset.X, -cylinder.HalfLength + 2 * cylinder.HalfLength * random.NextSingle(), horizontalOffset.Y); } else { @@ -166,7 +166,7 @@ public void GetSurface(Random random, ref Cylinder cylinder, out Vector3 localPo Vector2 randomHorizontal; do { - randomHorizontal = min + span * new Vector2((float)random.NextDouble(), (float)random.NextDouble()); + randomHorizontal = min + span * new Vector2(random.NextSingle(), random.NextSingle()); distanceSquared = randomHorizontal.LengthSquared(); } while (distanceSquared < cylinder.Radius * cylinder.Radius); @@ -206,20 +206,20 @@ public void GetRandomShape(Random random, out Box shape) { const float sizeMin = 0.1f; const float sizeSpan = 200; - shape = new Box(sizeMin + sizeSpan * (float)random.NextDouble(), sizeMin * sizeSpan * (float)random.NextDouble(), sizeMin * sizeSpan * (float)random.NextDouble()); + shape = new Box(sizeMin + sizeSpan * random.NextSingle(), sizeMin * sizeSpan * random.NextSingle(), sizeMin * sizeSpan * random.NextSingle()); } public void GetPointInVolume(Random random, float innerMargin, ref Box box, out Vector3 localPoint) { var min = new Vector3(box.HalfWidth - innerMargin, box.HalfHeight - innerMargin, box.HalfLength - innerMargin); var span = min * 2; min = -min; - localPoint = min + span * new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()); + localPoint = min + span * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); } public void GetSurface(Random random, ref Box box, out Vector3 localPoint, out Vector3 localNormal) { - var a = (float)random.NextDouble(); - var b = (float)random.NextDouble(); + var a = random.NextSingle(); + var b = random.NextSingle(); var axisSign = (float)(random.Next(2) * 2 - 1); Vector3 x, y, z; switch (random.Next(3)) @@ -267,7 +267,7 @@ internal static void GetUnitDirection(Random random, out Vector3 direction) float length; do { - direction = new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()) * new Vector3(2) - new Vector3(1); + direction = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) * new Vector3(2) - new Vector3(1); length = direction.Length(); } while (length < 1e-7f); direction /= length; @@ -279,10 +279,10 @@ static void GetUnitQuaternion(Random random, out Quaternion orientation) do { orientation = new Quaternion( - (float)random.NextDouble() * 2 - 1, - (float)random.NextDouble() * 2 - 1, - (float)random.NextDouble() * 2 - 1, - (float)random.NextDouble() * 2 - 1); + random.NextSingle() * 2 - 1, + random.NextSingle() * 2 - 1, + random.NextSingle() * 2 - 1, + random.NextSingle() * 2 - 1); length = orientation.Length(); } while (length < 1e-7f); Unsafe.As(ref orientation) /= length; @@ -294,7 +294,7 @@ static void GetPointOnPlane(Random random, float centralExclusion, float span, r var exclusionSquared = centralExclusion * centralExclusion; do { - localPoint = span * (new Vector2((float)random.NextDouble(), (float)random.NextDouble()) - new Vector2(0.5f)); + localPoint = span * (new Vector2(random.NextSingle(), random.NextSingle()) - new Vector2(0.5f)); } while (localPoint.LengthSquared() < exclusionSquared); Vector3 basisX; @@ -372,7 +372,7 @@ static void Test() where TShape : IConvexShape wher for (int transformIteration = 0; transformIteration < transformIterations; ++transformIteration) { RigidPose pose; - pose.Position = new Vector3(positionMin) + positionBoundsSpan * new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()); + pose.Position = new Vector3(positionMin) + positionBoundsSpan * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); GetUnitQuaternion(random, out pose.Orientation); Matrix3x3.CreateFromQuaternion(pose.Orientation, out var orientation); RigidPoseWide poses; @@ -381,12 +381,12 @@ static void Test() where TShape : IConvexShape wher for (int rayIndex = 0; rayIndex < outsideToInsideRays; ++rayIndex) { tester.GetSurface(random, ref shape, out var pointOnSurface, out var normal); - var localSourcePoint = pointOnSurface + normal * (outsideMinimumDistance + (float)random.NextDouble() * outsideDistanceSpan); + var localSourcePoint = pointOnSurface + normal * (outsideMinimumDistance + random.NextSingle() * outsideDistanceSpan); tester.GetPointInVolume(random, volumeInnerMargin, ref shape, out var localTargetPoint); Matrix3x3.Transform(localSourcePoint, orientation, out var sourcePoint); sourcePoint += pose.Position; - var directionScale = (0.01f + 2 * (float)random.NextDouble()); + var directionScale = (0.01f + 2 * random.NextSingle()); var localDirection = (localTargetPoint - localSourcePoint) * directionScale; Matrix3x3.Transform(localDirection, orientation, out var direction); @@ -414,7 +414,7 @@ static void Test() where TShape : IConvexShape wher Matrix3x3.Transform(localSourcePoint, orientation, out var sourcePoint); sourcePoint += pose.Position; - var directionScale = (0.01f + 100 * (float)random.NextDouble()); + var directionScale = (0.01f + 100 * random.NextSingle()); GetUnitDirection(random, out var direction); direction *= directionScale; @@ -437,11 +437,11 @@ static void Test() where TShape : IConvexShape wher { //Create a ray that lies on one of the shape's tangent planes, offset from the surface some amount to avoid numerical limitations. tester.GetSurface(random, ref shape, out var pointOnSurface, out var localNormal); - var localTargetPoint = pointOnSurface + localNormal * (tangentMinimumDistance + (float)random.NextDouble() * tangentDistanceSpan); - var exclusion = tangentCentralExclusionMin + (float)random.NextDouble() * tangentCentralExclusionSpan; - var span = 2 * exclusion + tangentSourceSpanMin + tangentSourceSpanSpan * (float)random.NextDouble(); + var localTargetPoint = pointOnSurface + localNormal * (tangentMinimumDistance + random.NextSingle() * tangentDistanceSpan); + var exclusion = tangentCentralExclusionMin + random.NextSingle() * tangentCentralExclusionSpan; + var span = 2 * exclusion + tangentSourceSpanMin + tangentSourceSpanSpan * random.NextSingle(); GetPointOnPlane(random, exclusion, span, ref localTargetPoint, ref localNormal, out var localSourcePoint); - var directionScale = (0.01f + 2 * (float)random.NextDouble()); + var directionScale = (0.01f + 2 * random.NextSingle()); var localDirection = (localTargetPoint - localSourcePoint) * directionScale; Matrix3x3.Transform(localSourcePoint, orientation, out var sourcePoint); sourcePoint += pose.Position; @@ -456,13 +456,13 @@ static void Test() where TShape : IConvexShape wher for (int rayIndex = 0; rayIndex < outwardPointingRays; ++rayIndex) { tester.GetSurface(random, ref shape, out var pointOnSurface, out var localNormal); - var localSourcePoint = pointOnSurface + localNormal * (tangentMinimumDistance + (float)random.NextDouble() * tangentDistanceSpan); + var localSourcePoint = pointOnSurface + localNormal * (tangentMinimumDistance + random.NextSingle() * tangentDistanceSpan); Vector3 localTargetPoint; do { - localTargetPoint = localSourcePoint + new Vector3(-0.5f * outwardPointingSpan) + new Vector3(outwardPointingSpan) * new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()); + localTargetPoint = localSourcePoint + new Vector3(-0.5f * outwardPointingSpan) + new Vector3(outwardPointingSpan) * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); } while (Vector3.Dot(localTargetPoint - localSourcePoint, localNormal) < 0); - var directionScale = (0.01f + 2 * (float)random.NextDouble()); + var directionScale = (0.01f + 2 * random.NextSingle()); var localDirection = (localTargetPoint - localSourcePoint) * directionScale; Matrix3x3.Transform(localSourcePoint, orientation, out var sourcePoint); sourcePoint += pose.Position; diff --git a/Demos/SpecializedTests/ShapePileTestDemo.cs b/Demos/SpecializedTests/ShapePileTestDemo.cs index d451872ff..565addd98 100644 --- a/Demos/SpecializedTests/ShapePileTestDemo.cs +++ b/Demos/SpecializedTests/ShapePileTestDemo.cs @@ -40,8 +40,8 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var random = new Random(5); for (int i = 0; i < pointCount; ++i) { - points.AllocateUnsafely() = new Vector3(3 * (float)random.NextDouble(), 1 * (float)random.NextDouble(), 3 * (float)random.NextDouble()); - //points.AllocateUnsafely() = new Vector3(0, 1, 0) + Vector3.Normalize(new Vector3((float)random.NextDouble() * 2 - 1, (float)random.NextDouble() * 2 - 1, (float)random.NextDouble() * 2 - 1)) * (float)random.NextDouble(); + points.AllocateUnsafely() = new Vector3(3 * random.NextSingle(), 1 * random.NextSingle(), 3 * random.NextSingle()); + //points.AllocateUnsafely() = new Vector3(0, 1, 0) + Vector3.Normalize(new Vector3(random.NextSingle() * 2 - 1, random.NextSingle() * 2 - 1, random.NextSingle() * 2 - 1)) * random.NextSingle(); } var convexHull = new ConvexHull(points.Span.Slice(points.Count), BufferPool, out _); box.ComputeInertia(1, out var boxInertia); diff --git a/Demos/SpecializedTests/TestHelpers.cs b/Demos/SpecializedTests/TestHelpers.cs index 622b5fc9e..5031cbf7e 100644 --- a/Demos/SpecializedTests/TestHelpers.cs +++ b/Demos/SpecializedTests/TestHelpers.cs @@ -28,14 +28,14 @@ public static RigidPose CreateRandomPose(Random random, BoundingBox positionBoun RigidPose pose; var span = positionBounds.Max - positionBounds.Min; - pose.Position = positionBounds.Min + span * new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()); - var axis = new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()); + pose.Position = positionBounds.Min + span * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); + var axis = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); var length = axis.Length(); if (length > 0) axis /= length; else axis = new Vector3(0, 1, 0); - pose.Orientation = BepuUtilities.QuaternionEx.CreateFromAxisAngle(axis, 1203f * (float)random.NextDouble()); + pose.Orientation = BepuUtilities.QuaternionEx.CreateFromAxisAngle(axis, 1203f * random.NextSingle()); return pose; } diff --git a/Demos/SpecializedTests/TreeTest.cs b/Demos/SpecializedTests/TreeTest.cs index edb20654d..c676841ba 100644 --- a/Demos/SpecializedTests/TreeTest.cs +++ b/Demos/SpecializedTests/TreeTest.cs @@ -39,7 +39,7 @@ public static void Test() var index = leafCountAlongXAxis * leafCountAlongYAxis * k + leafCountAlongXAxis * j + i; leafBounds[index].Min = new Vector3(i, j, k) * boundsSpacing; leafBounds[index].Max = leafBounds[index].Min + new Vector3(boundsSpan) + - spanRange * new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()); + spanRange * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); } } diff --git a/Demos/SpecializedTests/TriangleRayTestDemo.cs b/Demos/SpecializedTests/TriangleRayTestDemo.cs index c7c19fa37..0dc2c2607 100644 --- a/Demos/SpecializedTests/TriangleRayTestDemo.cs +++ b/Demos/SpecializedTests/TriangleRayTestDemo.cs @@ -23,9 +23,9 @@ void GetPointOnTriangle(Random random, in Triangle triangle, in RigidPose pose, float a, b, c; do { - a = (float)random.NextDouble(); - b = (float)random.NextDouble(); - c = (float)random.NextDouble(); + a = random.NextSingle(); + b = random.NextSingle(); + c = random.NextSingle(); total = a + b + c; } while (total < 1e-7f); var inverseTotal = 1f / total; @@ -46,18 +46,18 @@ void GetPointOutsideTriangle(Random random, in Triangle triangle, in RigidPose p switch (edgeIndex) { case 0: - borderPoint = triangle.A + (triangle.B - triangle.A) * (float)random.NextDouble(); + borderPoint = triangle.A + (triangle.B - triangle.A) * random.NextSingle(); break; case 1: - borderPoint = triangle.A + (triangle.C - triangle.A) * (float)random.NextDouble(); + borderPoint = triangle.A + (triangle.C - triangle.A) * random.NextSingle(); break; default: - borderPoint = triangle.B + (triangle.C - triangle.B) * (float)random.NextDouble(); + borderPoint = triangle.B + (triangle.C - triangle.B) * random.NextSingle(); break; } var center = (triangle.A + triangle.B + triangle.C) / 3f; var offsetToBorder = borderPoint - center; - var localP = center + offsetToBorder * (1.01f + 4 * (float)random.NextDouble()); + var localP = center + offsetToBorder * (1.01f + 4 * random.NextSingle()); BepuUtilities.QuaternionEx.TransformWithoutOverlap(localP, pose.Orientation, out pointOutsideTriangle); pointOutsideTriangle += pose.Position; @@ -116,24 +116,24 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) const float shapeSpan = 100; for (int i = 0; i < 10000; ++i) { - triangle.A = new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()) * shapeSpan + new Vector3(shapeMin); - triangle.B = new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()) * shapeSpan + new Vector3(shapeMin); - triangle.C = new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()) * shapeSpan + new Vector3(shapeMin); + triangle.A = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) * shapeSpan + new Vector3(shapeMin); + triangle.B = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) * shapeSpan + new Vector3(shapeMin); + triangle.C = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) * shapeSpan + new Vector3(shapeMin); var localTriangleCenter = (triangle.A + triangle.B + triangle.C) / 3f; triangle.A -= localTriangleCenter; triangle.B -= localTriangleCenter; triangle.C -= localTriangleCenter; - rayOrigin = new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()) * shapeSpan + new Vector3(shapeMin); + rayOrigin = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) * shapeSpan + new Vector3(shapeMin); float orientationLengthSquared; while (true) { - pose.Orientation.X = (float)random.NextDouble() * 2 - 1; - pose.Orientation.Y = (float)random.NextDouble() * 2 - 1; - pose.Orientation.Z = (float)random.NextDouble() * 2 - 1; - pose.Orientation.W = (float)random.NextDouble() * 2 - 1; + pose.Orientation.X = random.NextSingle() * 2 - 1; + pose.Orientation.Y = random.NextSingle() * 2 - 1; + pose.Orientation.Z = random.NextSingle() * 2 - 1; + pose.Orientation.W = random.NextSingle() * 2 - 1; orientationLengthSquared = pose.Orientation.LengthSquared(); if (orientationLengthSquared > 1e-7f) break; @@ -155,7 +155,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) GetPointOnTriangle(random, triangle, pose, out pointOnTriangle); rayDirection = pointOnTriangle - rayOrigin; } while (rayDirection.LengthSquared() < 1e-9f); - rayDirection *= (0.5f + 10 * (float)random.NextDouble()) / rayDirection.Length(); + rayDirection *= (0.5f + 10 * random.NextSingle()) / rayDirection.Length(); var shouldHit = Vector3.Dot(rayDirection, normal) < 0; TestRay(triangle, pose, rayOrigin, rayDirection, shouldHit, pointOnTriangle); @@ -165,7 +165,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) GetPointOutsideTriangle(random, triangle, pose, out pointOutsideTriangle); rayDirection = pointOutsideTriangle - rayOrigin; } while (rayDirection.LengthSquared() < 1e-9f); - rayDirection *= (0.5f + 10 * (float)random.NextDouble()) / rayDirection.Length(); + rayDirection *= (0.5f + 10 * random.NextSingle()) / rayDirection.Length(); TestRay(triangle, pose, rayOrigin, rayDirection, false, pointOutsideTriangle); } diff --git a/Demos/SpecializedTests/VolumeQueryTests.cs b/Demos/SpecializedTests/VolumeQueryTests.cs index c753ea411..f982547f2 100644 --- a/Demos/SpecializedTests/VolumeQueryTests.cs +++ b/Demos/SpecializedTests/VolumeQueryTests.cs @@ -80,14 +80,14 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) { for (int k = 0; k < length; ++k) { - var r = new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()); + var r = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); var location = spacing * (new Vector3(i, j, k) + new Vector3(-width, -height, -length) * 0.5f) + randomizationBase + r * randomizationSpan; Quaternion orientation; - orientation.X = -1 + 2 * (float)random.NextDouble(); - orientation.Y = -1 + 2 * (float)random.NextDouble(); - orientation.Z = -1 + 2 * (float)random.NextDouble(); - orientation.W = 0.01f + (float)random.NextDouble(); + orientation.X = -1 + 2 * random.NextSingle(); + orientation.Y = -1 + 2 * random.NextSingle(); + orientation.Z = -1 + 2 * random.NextSingle(); + orientation.W = 0.01f + random.NextSingle(); QuaternionEx.Normalize(ref orientation); if ((i + j + k) % 2 == 1) @@ -113,9 +113,9 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) for (int i = 0; i < boxCount; ++i) { ref var box = ref queryBoxes.AllocateUnsafely(); - var r = new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()); + var r = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); var boxOrigin = randomMin + r * randomSpan; - var boxHalfSize = new Vector3(0.25f + 0.75f * (float)random.NextDouble()); + var boxHalfSize = new Vector3(0.25f + 0.75f * random.NextSingle()); box.Min = boxOrigin - boxHalfSize; box.Max = boxOrigin + boxHalfSize; } From 27c1e4c43ef3380a2ccc3e6aae2217aec6148621 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 13 Nov 2021 22:16:41 -0600 Subject: [PATCH 287/947] A bit more validation, and a bit more avoidance of undefined data. --- BepuPhysics/Bodies.cs | 11 +++++++++++ BepuPhysics/BodySet.cs | 4 ++++ 2 files changed, 15 insertions(+) diff --git a/BepuPhysics/Bodies.cs b/BepuPhysics/Bodies.cs index 501797912..6d7861ce3 100644 --- a/BepuPhysics/Bodies.cs +++ b/BepuPhysics/Bodies.cs @@ -582,6 +582,17 @@ internal void ValidateAwakeMotionStatesByHash(HashDiagnosticType type) } } + public void ValidateAwakeCollidablesByHash(HashDiagnosticType type) + { + var instance = InvasiveHashDiagnostics.Instance; + ref int hash = ref instance.GetHashForType(type); + ref var set = ref ActiveSet; + for (int j = 0; j < set.Count; ++j) + { + instance.ContributeToHash(ref hash, set.Collidables[j]); + } + } + internal void ResizeSetsCapacity(int setsCapacity, int potentiallyAllocatedCount) { Debug.Assert(setsCapacity >= potentiallyAllocatedCount && potentiallyAllocatedCount <= Sets.Length); diff --git a/BepuPhysics/BodySet.cs b/BepuPhysics/BodySet.cs index a62d823c4..c5edbc7f7 100644 --- a/BepuPhysics/BodySet.cs +++ b/BepuPhysics/BodySet.cs @@ -137,6 +137,10 @@ internal void ApplyDescriptionByIndex(int index, in BodyDescription description) //Note that we change the shape here. If the collidable transitions from shapeless->shapeful or shapeful->shapeless, the broad phase has to be notified //so that it can create/remove an entry. That's why this function isn't public. collidable.Shape = description.Collidable.Shape; + //To avoid leaking undefined data, initialize the speculative margin to zero. + //Under normal circumstances, it should not be possible for any relevant system to see an undefined speculative margin, since PredictBoundingBoxes sets the value. + //However, corner cases like a body being added asleep and woken by the narrow phase can mean prediction does not run. + collidable.SpeculativeMargin = 0; ref var activity = ref Activity[index]; activity.SleepThreshold = description.Activity.SleepThreshold; activity.MinimumTimestepsUnderThreshold = description.Activity.MinimumTimestepCountUnderThreshold; From b2359515d42c1d9efabe11e42b13014aac1f14ab Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 18 Nov 2021 23:10:20 -0600 Subject: [PATCH 288/947] Sphere-cylinder division by zero guarded. --- .../CollisionTasks/SphereCylinderTester.cs | 10 ++++++---- .../SweepTasks/SphereCylinderDistanceTester.cs | 2 +- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/SphereCylinderTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/SphereCylinderTester.cs index 1b3b02255..997b0f384 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/SphereCylinderTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/SphereCylinderTester.cs @@ -17,7 +17,7 @@ public void Test(ref SphereWide a, ref CylinderWide b, ref Vector specula [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ComputeSphereToClosest(in CylinderWide b, in Vector3Wide offsetB, in Matrix3x3Wide orientationMatrixB, - out Vector3Wide cylinderLocalOffsetA, out Vector horizontalClampRequired, out Vector horizontalOffsetLength, out Vector inverseHorizontalOffsetLength, + out Vector3Wide cylinderLocalOffsetA, out Vector horizontalOffsetLength, out Vector inverseHorizontalOffsetLength, out Vector3Wide sphereToClosestLocalB, out Vector3Wide sphereToClosest) { //Clamp the sphere position to the cylinder's volume. @@ -26,7 +26,7 @@ public static void ComputeSphereToClosest(in CylinderWide b, in Vector3Wide offs horizontalOffsetLength = Vector.SquareRoot(cylinderLocalOffsetA.X * cylinderLocalOffsetA.X + cylinderLocalOffsetA.Z * cylinderLocalOffsetA.Z); inverseHorizontalOffsetLength = Vector.One / horizontalOffsetLength; var horizontalClampMultiplier = b.Radius * inverseHorizontalOffsetLength; - horizontalClampRequired = Vector.GreaterThan(horizontalOffsetLength, b.Radius); + var horizontalClampRequired = Vector.GreaterThan(horizontalOffsetLength, b.Radius); Vector3Wide clampedSpherePositionLocalB; clampedSpherePositionLocalB.X = Vector.ConditionalSelect(horizontalClampRequired, cylinderLocalOffsetA.X * horizontalClampMultiplier, cylinderLocalOffsetA.X); clampedSpherePositionLocalB.Y = Vector.Min(b.HalfLength, Vector.Max(-b.HalfLength, cylinderLocalOffsetA.Y)); @@ -41,17 +41,17 @@ public void Test(ref SphereWide a, ref CylinderWide b, ref Vector specula { Matrix3x3Wide.CreateFromQuaternion(orientationB, out var orientationMatrixB); ComputeSphereToClosest(b, offsetB, orientationMatrixB, - out var cylinderLocalOffsetA, out var horizontalClampRequired, out var horizontalOffsetLength, out var inverseHorizontalOffsetLength, + out var cylinderLocalOffsetA, out var horizontalOffsetLength, out var inverseHorizontalOffsetLength, out var sphereToContactLocalB, out manifold.OffsetA); //If the sphere center is inside the cylinder, then we must compute the fastest way out of the cylinder. var absY = Vector.Abs(cylinderLocalOffsetA.Y); - var useInternal = Vector.AndNot(Vector.LessThanOrEqual(absY, b.HalfLength), horizontalClampRequired); var depthY = b.HalfLength - absY; var horizontalDepth = b.Radius - horizontalOffsetLength; var useDepthY = Vector.LessThanOrEqual(depthY, horizontalDepth); var useTopCapNormal = Vector.GreaterThan(cylinderLocalOffsetA.Y, Vector.Zero); Vector3Wide localInternalNormal; + var useHorizontalFallback = Vector.LessThanOrEqual(horizontalOffsetLength, b.Radius * new Vector(1e-5f)); localInternalNormal.X = Vector.ConditionalSelect(useDepthY, Vector.Zero, Vector.ConditionalSelect(useHorizontalFallback, Vector.One, cylinderLocalOffsetA.X * inverseHorizontalOffsetLength)); localInternalNormal.Y = Vector.ConditionalSelect(useDepthY, Vector.ConditionalSelect(useTopCapNormal, Vector.One, new Vector(-1)), Vector.Zero); @@ -61,6 +61,8 @@ public void Test(ref SphereWide a, ref CylinderWide b, ref Vector specula //Note negation; normal points from B to A by convention. Vector3Wide.Scale(sphereToContactLocalB, new Vector(-1) / contactDistanceFromSphereCenter, out var localExternalNormal); + //Can't rely on the external normal if the sphere is so close to the surface that the normal isn't numerically computable. + var useInternal = Vector.LessThan(contactDistanceFromSphereCenter, new Vector(1e-7f)); Vector3Wide.ConditionalSelect(useInternal, localInternalNormal, localExternalNormal, out var localNormal); Matrix3x3Wide.TransformWithoutOverlap(localNormal, orientationMatrixB, out manifold.Normal); diff --git a/BepuPhysics/CollisionDetection/SweepTasks/SphereCylinderDistanceTester.cs b/BepuPhysics/CollisionDetection/SweepTasks/SphereCylinderDistanceTester.cs index d8149b862..e8ec564fc 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/SphereCylinderDistanceTester.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/SphereCylinderDistanceTester.cs @@ -11,7 +11,7 @@ public void Test(in SphereWide a, in CylinderWide b, in Vector3Wide offsetB, in out Vector intersected, out Vector distance, out Vector3Wide closestA, out Vector3Wide normal) { Matrix3x3Wide.CreateFromQuaternion(orientationB, out var orientationMatrixB); - SphereCylinderTester.ComputeSphereToClosest(b, offsetB, orientationMatrixB, out _, out _, out _, out _, out _, out closestA); + SphereCylinderTester.ComputeSphereToClosest(b, offsetB, orientationMatrixB, out _, out _, out _, out _, out closestA); Vector3Wide.Length(closestA, out var contactDistanceFromSphereCenter); //Note negation; normal points from B to A by convention. From 52bbaa8c04cb86427472f9c1311bca594fe52a27 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 24 Nov 2021 17:48:26 -0600 Subject: [PATCH 289/947] Fixed a capsule-cylinder determinism failure on early out. Bundlewide early out condition could differ from later per-contact contactexists. --- .../CollisionTasks/CapsuleCylinderTester.cs | 12 +++++------ BepuPhysics/InvasiveHashDiagnostics.cs | 21 ++++++++++++++++++- 2 files changed, 26 insertions(+), 7 deletions(-) diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleCylinderTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleCylinderTester.cs index 868f753b9..a92d650de 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleCylinderTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleCylinderTester.cs @@ -207,7 +207,8 @@ public void Test(ref CapsuleWide a, ref CylinderWide b, ref Vector specul } //All of the above excluded any consideration of the capsule's radius. Include it now. depth += a.Radius; - inactiveLanes = Vector.BitwiseOr(Vector.LessThan(depth, -speculativeMargin), inactiveLanes); + var negativeMargin = -speculativeMargin; + inactiveLanes = Vector.BitwiseOr(Vector.LessThan(depth, negativeMargin), inactiveLanes); if (Vector.LessThanAll(inactiveLanes, Vector.Zero)) { //All lanes have a depth which cannot create any contacts due to the speculative margin. We can early out. @@ -223,7 +224,7 @@ public void Test(ref CapsuleWide a, ref CylinderWide b, ref Vector specul //Segment-side case is handled in the same way as capsule-capsule- create an interval by projecting the segment onto the cylinder segment and then narrow the interval in response to noncoplanarity. //Segment-cap is easy too; project the segment down onto the cap plane. Clip it against the cap circle (solve a quadratic). - var useCapContacts = Vector.GreaterThan(Vector.Abs(localNormal.Y), new Vector(0.70710678118f)); + var useCapContacts = Vector.AndNot(Vector.GreaterThan(Vector.Abs(localNormal.Y), new Vector(0.70710678118f)), inactiveLanes); //First, assume non-cap contacts. //Phrase the problem as a segment-segment test. @@ -248,7 +249,7 @@ public void Test(ref CapsuleWide a, ref CylinderWide b, ref Vector specul var contactCount = Vector.ConditionalSelect(Vector.LessThan(Vector.Abs(contactTMax - contactTMin), b.HalfLength * new Vector(1e-5f)), Vector.One, new Vector(2)); - if (Vector.LessThanAny(Vector.AndNot(useCapContacts, inactiveLanes), Vector.Zero)) + if (Vector.LessThanAny(useCapContacts, Vector.Zero)) { //At least one lane requires a cap contact. //An important note: for highest quality, all clipping takes place on the *normal plane*. So segment-cap doesn't merely set the y component to zero (projecting along B's Y axis). @@ -329,9 +330,8 @@ public void Test(ref CapsuleWide a, ref CylinderWide b, ref Vector specul //In this case, both contact positions should be extremely close together anyway. var collapse = Vector.LessThan(Vector.Abs(faceNormalADotLocalNormal), new Vector(1e-7f)); manifold.Depth0 = Vector.ConditionalSelect(collapse, depth, manifold.Depth0); - var negativeMargin = -speculativeMargin; - manifold.Contact0Exists = Vector.GreaterThan(manifold.Depth0, negativeMargin); - manifold.Contact1Exists = Vector.BitwiseAnd(Vector.AndNot(Vector.Equals(contactCount, new Vector(2)), collapse), Vector.GreaterThan(manifold.Depth1, negativeMargin)); + manifold.Contact0Exists = Vector.AndNot(Vector.GreaterThanOrEqual(manifold.Depth0, negativeMargin), inactiveLanes); + manifold.Contact1Exists = Vector.AndNot(Vector.BitwiseAnd(Vector.AndNot(Vector.Equals(contactCount, new Vector(2)), collapse), Vector.GreaterThanOrEqual(manifold.Depth1, negativeMargin)), inactiveLanes); //Push the contacts into world space. Matrix3x3Wide.TransformWithoutOverlap(localNormal, worldRB, out manifold.Normal); diff --git a/BepuPhysics/InvasiveHashDiagnostics.cs b/BepuPhysics/InvasiveHashDiagnostics.cs index 31b881284..fd8497156 100644 --- a/BepuPhysics/InvasiveHashDiagnostics.cs +++ b/BepuPhysics/InvasiveHashDiagnostics.cs @@ -23,6 +23,18 @@ public enum HashDiagnosticType AwakeBodyStates7, AwakeBodyStates8, AwakeBodyStates9, + AwakeBodyStates10, + AwakeBodyCollidableStates0, + AwakeBodyCollidableStates1, + AwakeBodyCollidableStates2, + AwakeBodyCollidableStates3, + AwakeBodyCollidableStates4, + AwakeBodyCollidableStates5, + AwakeBodyCollidableStates6, + AwakeBodyCollidableStates7, + AwakeBodyCollidableStates8, + AwakeBodyCollidableStates9, + AwakeBodyCollidableStates10, AddSleepingToActiveForFallback, SolverBodyReferenceBeforeCollisionDetection, SolverBodyReferenceBeforePreflush, @@ -57,7 +69,7 @@ public class InvasiveHashDiagnostics /// /// This is meant as an internal diagnostic utility, so hardcoding some things is totally fine. /// - const int HashTypeCount = 34; + const int HashTypeCount = 46; public static InvasiveHashDiagnostics Instance; public static void Initialize(int runCount, int hashCapacityPerType) { @@ -95,6 +107,7 @@ public void MoveToNextHashFrame() return; if (CurrentHashIndex < 0) throw new ArgumentException($"Invalid hash index: {CurrentHashIndex}"); + bool anyFailed = false; for (int hashTypeIndex = 0; hashTypeIndex < Hashes[CurrentRunIndex].Length; ++hashTypeIndex) { if (CurrentHashIndex >= Hashes[CurrentRunIndex][hashTypeIndex].Length) @@ -104,9 +117,15 @@ public void MoveToNextHashFrame() if (Hashes[CurrentRunIndex][hashTypeIndex][CurrentHashIndex] != Hashes[previousRunIndex][hashTypeIndex][CurrentHashIndex]) { Console.WriteLine($"Hash failure on {(HashDiagnosticType)hashTypeIndex} frame {CurrentHashIndex}, current run {CurrentRunIndex} vs previous {previousRunIndex}: {Hashes[CurrentRunIndex][hashTypeIndex][CurrentHashIndex] } vs {Hashes[previousRunIndex][hashTypeIndex][CurrentHashIndex]}."); + anyFailed = true; } } } + if (anyFailed) + { + Console.WriteLine("Press enter to continue."); + Console.ReadLine(); + } ++CurrentHashIndex; } From 38b001d2c323969653fce44c2be454a30971ce78 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 24 Nov 2021 21:38:22 -0600 Subject: [PATCH 290/947] Pushed early out a little earlier. --- .../CollisionTasks/CapsuleCylinderTester.cs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleCylinderTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleCylinderTester.cs index a92d650de..bc3f5d6bd 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleCylinderTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleCylinderTester.cs @@ -167,7 +167,8 @@ public void Test(ref CapsuleWide a, ref CylinderWide b, ref Vector specul //Division by zero is protected by the depth selection- if distance is zero, the depth is set to infinity and this normal won't be selected. Vector3Wide.Scale(localNormal, Vector.One / distanceFromCylinderToLineSegment, out localNormal); var depth = Vector.ConditionalSelect(internalLineSegmentIntersected, new Vector(float.MaxValue), -distanceFromCylinderToLineSegment); - + var negativeMargin = -speculativeMargin; + inactiveLanes = Vector.BitwiseOr(Vector.LessThan(depth, negativeMargin), inactiveLanes); if (Vector.LessThanAny(Vector.AndNot(internalLineSegmentIntersected, inactiveLanes), Vector.Zero)) { //At least one lane is intersecting deeply, so we need to examine the other possible normals. @@ -207,7 +208,6 @@ public void Test(ref CapsuleWide a, ref CylinderWide b, ref Vector specul } //All of the above excluded any consideration of the capsule's radius. Include it now. depth += a.Radius; - var negativeMargin = -speculativeMargin; inactiveLanes = Vector.BitwiseOr(Vector.LessThan(depth, negativeMargin), inactiveLanes); if (Vector.LessThanAll(inactiveLanes, Vector.Zero)) { From f1973703a4af9da4ec81b5f8caa02c468aff7b0d Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 26 Nov 2021 14:30:00 -0600 Subject: [PATCH 291/947] Broad phase index initialized if there's no shape. FountainStressTestDemo now tries out zeroed inertia dynamics sometimes. --- BepuPhysics/Bodies.cs | 7 ++++++- BepuPhysics/EmbeddedSubsteppingTimestepper2.cs | 14 ++++++++++++++ Demos/Demos/CompoundTestDemo.cs | 2 +- Demos/Demos/FountainStressTestDemo.cs | 8 ++++++-- Demos/Program.cs | 3 ++- 5 files changed, 29 insertions(+), 5 deletions(-) diff --git a/BepuPhysics/Bodies.cs b/BepuPhysics/Bodies.cs index 6d7861ce3..18bac4d92 100644 --- a/BepuPhysics/Bodies.cs +++ b/BepuPhysics/Bodies.cs @@ -187,6 +187,11 @@ public unsafe BodyHandle Add(in BodyDescription description) { AddCollidableToBroadPhase(handle, description.Pose, description.LocalInertia, ref ActiveSet.Collidables[index]); } + else + { + //Don't want to leak undefined data into the collidable state if there's no shape. + ActiveSet.Collidables[index].BroadPhaseIndex = -1; + } return handle; } @@ -582,7 +587,7 @@ internal void ValidateAwakeMotionStatesByHash(HashDiagnosticType type) } } - public void ValidateAwakeCollidablesByHash(HashDiagnosticType type) + internal void ValidateAwakeCollidablesByHash(HashDiagnosticType type) { var instance = InvasiveHashDiagnostics.Instance; ref int hash = ref instance.GetHashForType(type); diff --git a/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs b/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs index 23e960f27..0f7c6bf3c 100644 --- a/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs +++ b/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs @@ -46,6 +46,8 @@ public EmbeddedSubsteppingTimestepper2(int substepCount) public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDispatcher = null) { + //simulation.Bodies.ValidateAwakeMotionStatesByHash(HashDiagnosticType.AwakeBodyStates0); + //simulation.Bodies.ValidateAwakeCollidablesByHash(HashDiagnosticType.AwakeBodyCollidableStates0); simulation.Solver.ValidateTrailingTypeBatchBodyReferences(); simulation.Solver.ValidateFallbackBatchEmptySlotReferences(); simulation.Solver.ValidateFallbackBatchAccessSafety(); @@ -66,14 +68,20 @@ public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDi simulation.Solver.ValidateConstrainedKinematicsSet(); simulation.Solver.ValidateFallbackBodiesAreDynamic(); //simulation.Solver.ValidateExistingHandles(); + //simulation.Bodies.ValidateAwakeMotionStatesByHash(HashDiagnosticType.AwakeBodyStates1); + //simulation.Bodies.ValidateAwakeCollidablesByHash(HashDiagnosticType.AwakeBodyCollidableStates1); simulation.PredictBoundingBoxes(dt, threadDispatcher); BeforeCollisionDetection?.Invoke(dt, threadDispatcher); + //simulation.Bodies.ValidateAwakeMotionStatesByHash(HashDiagnosticType.AwakeBodyStates2); + //simulation.Bodies.ValidateAwakeCollidablesByHash(HashDiagnosticType.AwakeBodyCollidableStates2); simulation.CollisionDetection(dt, threadDispatcher); CollisionsDetected?.Invoke(dt, threadDispatcher); Debug.Assert(SubstepCount >= 0, "Substep count should be positive."); + //simulation.Bodies.ValidateAwakeMotionStatesByHash(HashDiagnosticType.AwakeBodyStates3); + //simulation.Bodies.ValidateAwakeCollidablesByHash(HashDiagnosticType.AwakeBodyCollidableStates3); simulation.Solver.ValidateTrailingTypeBatchBodyReferences(); simulation.Solver.ValidateFallbackBatchEmptySlotReferences(); simulation.Solver.ValidateFallbackBatchAccessSafety(); @@ -88,16 +96,22 @@ public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDi simulation.Profiler.Start(simulation.Solver); simulation.Solver.SolveStep2(dt, threadDispatcher); simulation.Profiler.End(simulation.Solver); + //simulation.Bodies.ValidateAwakeMotionStatesByHash(HashDiagnosticType.AwakeBodyStates4); + //simulation.Bodies.ValidateAwakeCollidablesByHash(HashDiagnosticType.AwakeBodyCollidableStates4); ConstraintsSolved?.Invoke(dt, threadDispatcher); simulation.Profiler.Start(simulation.PoseIntegrator); simulation.PoseIntegrator.IntegrateAfterSubstepping(constrainedBodySet, dt, SubstepCount, threadDispatcher); simulation.Profiler.End(simulation.PoseIntegrator); + //simulation.Bodies.ValidateAwakeMotionStatesByHash(HashDiagnosticType.AwakeBodyStates5); + //simulation.Bodies.ValidateAwakeCollidablesByHash(HashDiagnosticType.AwakeBodyCollidableStates5); simulation.Solver.DisposeConstraintIntegrationResponsibilities(); SubstepsComplete?.Invoke(dt, threadDispatcher); simulation.Solver.ValidateAccumulatedImpulses(); simulation.IncrementallyOptimizeDataStructures(threadDispatcher); + //simulation.Bodies.ValidateAwakeMotionStatesByHash(HashDiagnosticType.AwakeBodyStates6); + //simulation.Bodies.ValidateAwakeCollidablesByHash(HashDiagnosticType.AwakeBodyCollidableStates6); } } } diff --git a/Demos/Demos/CompoundTestDemo.cs b/Demos/Demos/CompoundTestDemo.cs index 4929881b9..1a990b3e0 100644 --- a/Demos/Demos/CompoundTestDemo.cs +++ b/Demos/Demos/CompoundTestDemo.cs @@ -19,7 +19,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //The PositionFirstTimestepper is the simplest timestepping mode, but since it integrates velocity into position at the start of the frame, directly modified velocities outside of the timestep //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10f, 0)), new EmbeddedSubsteppingTimestepper2(3), 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10f, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); using (var compoundBuilder = new CompoundBuilder(BufferPool, Simulation.Shapes, 8)) { diff --git a/Demos/Demos/FountainStressTestDemo.cs b/Demos/Demos/FountainStressTestDemo.cs index 0a3290df2..13f2e9c41 100644 --- a/Demos/Demos/FountainStressTestDemo.cs +++ b/Demos/Demos/FountainStressTestDemo.cs @@ -362,7 +362,6 @@ public override void Update(Window window, Camera camera, Input input, float dt) Simulation.Statics.Add(staticDescription); } - //Spray some shapes! int newShapeCount = 8; var spawnPose = new RigidPose(new Vector3(0, 10, 0)); @@ -397,11 +396,16 @@ public override void Update(Window window, Camera camera, Input input, float dt) Simulation.Bodies.GetDescription(handle, out var description); Simulation.Shapes.RecursivelyRemoveAndDispose(description.Collidable.Shape, BufferPool); CreateBodyDescription(random, description.Pose, description.Velocity, out var newDescription); - if (random.NextDouble() < 0.1f) + if (random.NextSingle() < 0.1f) { //Occasionally make a dynamic kinematic. newDescription.LocalInertia = default; } + else if (random.NextSingle() < 0.05f) + { + //Occasionally make a rotation-locked dynamic. + newDescription.LocalInertia.InverseInertiaTensor = default; + } Simulation.Bodies.ApplyDescription(handle, newDescription); } diff --git a/Demos/Program.cs b/Demos/Program.cs index 5faf4c4fe..f7273c9ef 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -24,7 +24,8 @@ static void Main(string[] args) //HeadlessTest.Test(content, 8, 32, 1024); //HeadlessTest.Test(content, 4, 32, 512); //HeadlessTest.Test(content, 4, 128, 1024); - DeterminismTest.Test(content, 1, 65536); + //DeterminismTest.Test(content, 1, 65536); + //InvasiveHashDiagnostics.Initialize(5, 192000); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); From 1d9f7be7e5e94a552a8c6efc51b464a66a45a2ec Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 26 Nov 2021 14:45:30 -0600 Subject: [PATCH 292/947] Added a compatibility helper for collidabledescription. Still a sneaky breaking change, but not *that* breaking. probably. --- BepuPhysics/Collidables/CollidableDescription.cs | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/BepuPhysics/Collidables/CollidableDescription.cs b/BepuPhysics/Collidables/CollidableDescription.cs index b2a06d97c..3483a93e4 100644 --- a/BepuPhysics/Collidables/CollidableDescription.cs +++ b/BepuPhysics/Collidables/CollidableDescription.cs @@ -26,11 +26,21 @@ public CollidableDescription(TypedIndex shape, in ContinuousDetection continuity } /// - /// Constructs a new collidable description with default discrete continuity. + /// Constructs a new collidable description with . Will use a minimum speculative margin of 0 and a maximum of . /// /// Shape used by the collidable. + /// and are equivalent in behavior when the maximum speculative margin is since they both result in the same (unbounded) expansion of body bounding boxes in response to velocity. public CollidableDescription(TypedIndex shape) : this(shape, ContinuousDetection.Passive) { } + + /// + /// Constructs a new collidable description with . Will use a minimum speculative margin of 0 and the given maximumSpeculativeMargin. + /// + /// Shape used by the collidable. + /// Maximum speculative margin to be used with the discrete continuity configuration. + public CollidableDescription(TypedIndex shape, float maximumSpeculativeMargin) : this(shape, ContinuousDetection.Discrete(0, maximumSpeculativeMargin)) + { + } } } From b1d7a4ef1bdf7cb4c70f97e95b57668ce5c29e5c Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 26 Nov 2021 14:47:50 -0600 Subject: [PATCH 293/947] Little bit of docs. --- BepuPhysics/Collidables/CollidableReference.cs | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/BepuPhysics/Collidables/CollidableReference.cs b/BepuPhysics/Collidables/CollidableReference.cs index d9168f345..3ee9684d1 100644 --- a/BepuPhysics/Collidables/CollidableReference.cs +++ b/BepuPhysics/Collidables/CollidableReference.cs @@ -5,6 +5,9 @@ namespace BepuPhysics.Collidables { + /// + /// Represents how a collidable can interact and move. + /// public enum CollidableMobility { /// @@ -21,8 +24,14 @@ public enum CollidableMobility Static = 2 } + /// + /// Uses a bitpacked representation to refer to a body or static collidable. + /// public struct CollidableReference : IEquatable { + /// + /// Bitpacked representation of the collidable reference. + /// public uint Packed; /// @@ -102,7 +111,6 @@ public CollidableReference(CollidableMobility mobility, BodyHandle handle) /// /// Creates a collidable reference for a static. /// - /// Mobility type of the owner of the collidable. /// Handle of the owner of the collidable. [MethodImpl(MethodImplOptions.AggressiveInlining)] public CollidableReference(StaticHandle handle) From 1f93f623a4efb762d66f446c80831eead8e5fa4d Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 26 Nov 2021 18:55:11 -0600 Subject: [PATCH 294/947] Statics memory now bundled together. Plumbed through consequences. --- BepuPhysics/Collidables/Collidable.cs | 9 +- BepuPhysics/CollisionDetection/NarrowPhase.cs | 65 ++++--- BepuPhysics/IslandAwakener.cs | 2 +- BepuPhysics/Simulation.cs | 2 +- BepuPhysics/Simulation_Queries.cs | 6 +- BepuPhysics/StaticReference.cs | 22 ++- BepuPhysics/Statics.cs | 168 ++++++++++-------- DemoRenderer/ShapeDrawing/ShapesExtractor.cs | 3 +- Demos/Demos/CollisionQueryDemo.cs | 6 +- .../Demos/ContinuousCollisionDetectionDemo.cs | 3 + Demos/Demos/CustomVoxelCollidableDemo.cs | 2 +- Demos/Demos/Sponsors/SponsorDemo.cs | 2 +- 12 files changed, 170 insertions(+), 120 deletions(-) diff --git a/BepuPhysics/Collidables/Collidable.cs b/BepuPhysics/Collidables/Collidable.cs index b06e9fcff..b25eca7df 100644 --- a/BepuPhysics/Collidables/Collidable.cs +++ b/BepuPhysics/Collidables/Collidable.cs @@ -156,18 +156,19 @@ public struct Collidable /// /// Index of the shape used by the body. While this can be changed, any transition from shapeless->shapeful or shapeful->shapeless must be reported to the broad phase. - /// If you need to perform such a transition, consider using Bodies.ChangeShape or Bodies.ApplyDescription; those functions update the relevant state. + /// If you need to perform such a transition, consider using or ; those functions update the relevant state. /// public TypedIndex Shape; + /// - /// Size of the margin around the surface of the shape in which contacts can be generated. These contacts will have negative depth and only contribute if the frame's velocities + /// Automatically computed size of the margin around the surface of the shape in which contacts can be generated. These contacts will have negative depth and only contribute if the frame's velocities /// would push the shapes of a pair into overlap. - /// This is automatically set by bounding box prediction each frame, and is bound by the and values. + /// This is automatically set by bounding box prediction each frame, and is bound by the collidable's and values. /// The effective speculative margin for a collision pair can also be modified from callbacks. /// This should be positive to avoid jittering. /// It can also be used as a form of continuous collision detection, but excessively high values combined with fast motion may result in visible 'ghost collision' artifacts. /// For continuous collision detection with less chance of ghost collisions, use . - /// Note that continuous collision detection will only refine collision times to within the speculative margin, so if using , consider using a smaller value. + /// If using , consider setting to a smaller value to help filter ghost collisions. /// public float SpeculativeMargin; /// diff --git a/BepuPhysics/CollisionDetection/NarrowPhase.cs b/BepuPhysics/CollisionDetection/NarrowPhase.cs index 055bbb685..52456ea9f 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhase.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhase.cs @@ -419,8 +419,13 @@ public unsafe void HandleOverlap(int workerIndex, CollidableReference a, Collida ref var setB = ref Bodies.Sets[bodyLocationB.SetIndex]; ref var stateA = ref setA.SolverStates[bodyLocationA.Index]; ref var stateB = ref setB.SolverStates[bodyLocationB.Index]; + ref var collidableA = ref setA.Collidables[bodyLocationA.Index]; + ref var collidableB = ref setB.Collidables[bodyLocationB.Index]; AddBatchEntries(workerIndex, ref overlapWorker, ref pair, - ref setA.Collidables[bodyLocationA.Index], ref setB.Collidables[bodyLocationB.Index], + ref collidableA.Continuity, ref collidableB.Continuity, + collidableA.SpeculativeMargin, collidableB.SpeculativeMargin, + collidableA.Shape, collidableB.Shape, + collidableA.BroadPhaseIndex, collidableB.BroadPhaseIndex, ref stateA.Motion.Pose, ref stateB.Motion.Pose, ref stateA.Motion.Velocity, ref stateB.Motion.Velocity); } @@ -438,9 +443,14 @@ public unsafe void HandleOverlap(int workerIndex, CollidableReference a, Collida var zeroVelocity = default(BodyVelocity); ref var bodySet = ref Bodies.ActiveSet; ref var bodyState = ref bodySet.SolverStates[bodyLocation.Index]; + ref var collidableA = ref bodySet.Collidables[bodyLocation.Index]; + ref var collidableB = ref Statics[staticIndex]; AddBatchEntries(workerIndex, ref overlapWorker, ref pair, - ref bodySet.Collidables[bodyLocation.Index], ref Statics.Collidables[staticIndex], - ref bodyState.Motion.Pose, ref Statics.Poses[staticIndex], + ref collidableA.Continuity, ref collidableB.Continuity, + collidableA.SpeculativeMargin, 0, + collidableA.Shape, collidableB.Shape, + collidableA.BroadPhaseIndex, collidableB.BroadPhaseIndex, + ref bodyState.Motion.Pose, ref collidableB.Pose, ref bodyState.Motion.Velocity, ref zeroVelocity); } @@ -461,17 +471,22 @@ public bool AllowTest(int childA, int childB) [MethodImpl(MethodImplOptions.AggressiveInlining)] private unsafe void AddBatchEntries(int workerIndex, ref OverlapWorker overlapWorker, - ref CollidablePair pair, ref Collidable aCollidable, ref Collidable bCollidable, - ref RigidPose poseA, ref RigidPose poseB, ref BodyVelocity velocityA, ref BodyVelocity velocityB) + ref CollidablePair pair, + ref ContinuousDetection continuityA, ref ContinuousDetection continuityB, + float marginA, float marginB, + TypedIndex shapeA, TypedIndex shapeB, + int broadPhaseIndexA, int broadPhaseIndexB, + ref RigidPose poseA, ref RigidPose poseB, + ref BodyVelocity velocityA, ref BodyVelocity velocityB) { Debug.Assert(pair.A.Packed != pair.B.Packed); //Add the speculative margins, but try to obey both collidables' bounds. Note that in the case of nonoverlapping intervals, the higher min ends up used. var speculativeMargin = - MathF.Max(aCollidable.Continuity.MinimumSpeculativeMargin, MathF.Max(bCollidable.Continuity.MinimumSpeculativeMargin, - MathF.Min(aCollidable.Continuity.MaximumSpeculativeMargin, MathF.Min(bCollidable.Continuity.MaximumSpeculativeMargin, - aCollidable.SpeculativeMargin + bCollidable.SpeculativeMargin)))); + MathF.Max(continuityA.MinimumSpeculativeMargin, MathF.Max(continuityB.MinimumSpeculativeMargin, + MathF.Min(continuityA.MaximumSpeculativeMargin, MathF.Min(continuityB.MaximumSpeculativeMargin, + marginA + marginB)))); - var allowExpansion = aCollidable.Continuity.AllowExpansionBeyondSpeculativeMargin | bCollidable.Continuity.AllowExpansionBeyondSpeculativeMargin; + var allowExpansion = continuityA.AllowExpansionBeyondSpeculativeMargin | continuityB.AllowExpansionBeyondSpeculativeMargin; //Note that we pick float.MaxValue for the maximum bounds expansion passive-involving pairs. //This is a compromise- looser bounds are not a correctness issue, so we're trading off potentially more subpairs //and the need to compute a tighter maximum bound. That's not incredibly expensive, but it does add up. For now, we use the looser bound under the assumption @@ -482,9 +497,9 @@ private unsafe void AddBatchEntries(int workerIndex, ref OverlapWorker overlapWo //Note that we never create 'unilateral' CCD pairs. That is, if either collidable in a pair enables a CCD feature, we just act like both are using it. //That keeps things a little simpler. Unlike v1, we don't have to worry about the implications of 'motion clamping' here- no need for deeper configuration. CCDContinuationIndex continuationIndex = default; - if (aCollidable.Continuity.Mode == ContinuousDetectionMode.Continuous || bCollidable.Continuity.Mode == ContinuousDetectionMode.Continuous) + if (continuityA.Mode == ContinuousDetectionMode.Continuous || continuityB.Mode == ContinuousDetectionMode.Continuous) { - var sweepTask = SweepTaskRegistry.GetTask(aCollidable.Shape.Type, bCollidable.Shape.Type); + var sweepTask = SweepTaskRegistry.GetTask(shapeA.Type, shapeB.Type); if (sweepTask != null) { //Not every continuous pair requires an actual sweep test. If the maximum approaching displacement for any point on the involved shapes isn't any larger @@ -499,19 +514,19 @@ private unsafe void AddBatchEntries(int workerIndex, ref OverlapWorker overlapWo var bInStaticTree = pair.B.Mobility == CollidableMobility.Static || Simulation.Bodies.HandleToLocation[pair.B.BodyHandle.Value].SetIndex > 0; ref var aTree = ref aInStaticTree ? ref Simulation.BroadPhase.StaticTree : ref Simulation.BroadPhase.ActiveTree; ref var bTree = ref bInStaticTree ? ref Simulation.BroadPhase.StaticTree : ref Simulation.BroadPhase.ActiveTree; - BroadPhase.GetBoundsPointers(aCollidable.BroadPhaseIndex, ref aTree, out var aMin, out var aMax); - BroadPhase.GetBoundsPointers(bCollidable.BroadPhaseIndex, ref bTree, out var bMin, out var bMax); + BroadPhase.GetBoundsPointers(broadPhaseIndexA, ref aTree, out var aMin, out var aMax); + BroadPhase.GetBoundsPointers(broadPhaseIndexB, ref bTree, out var bMin, out var bMax); var maximumRadiusA = (*aMax - *aMin).Length() * 0.5f; var maximumRadiusB = (*bMax - *bMin).Length() * 0.5f; if ((velocityA.Angular.Length() * maximumRadiusA + velocityB.Angular.Length() * maximumRadiusB + (velocityB.Linear - velocityA.Linear).Length()) * timestepDuration > speculativeMargin) { - Simulation.Shapes[aCollidable.Shape.Type].GetShapeData(aCollidable.Shape.Index, out var shapeDataA, out var shapeSizeA); - Simulation.Shapes[bCollidable.Shape.Type].GetShapeData(bCollidable.Shape.Index, out var shapeDataB, out var shapeSizeB); + Simulation.Shapes[shapeA.Type].GetShapeData(shapeA.Index, out var shapeDataA, out var shapeSizeA); + Simulation.Shapes[shapeB.Type].GetShapeData(shapeB.Index, out var shapeDataB, out var shapeSizeB); float minimumSweepTimestepA, sweepConvergenceThresholdA; - if (aCollidable.Continuity.Mode == ContinuousDetectionMode.Continuous) + if (continuityA.Mode == ContinuousDetectionMode.Continuous) { - minimumSweepTimestepA = aCollidable.Continuity.MinimumSweepTimestep; - sweepConvergenceThresholdA = aCollidable.Continuity.SweepConvergenceThreshold; + minimumSweepTimestepA = continuityA.MinimumSweepTimestep; + sweepConvergenceThresholdA = continuityA.SweepConvergenceThreshold; } else { @@ -519,10 +534,10 @@ private unsafe void AddBatchEntries(int workerIndex, ref OverlapWorker overlapWo sweepConvergenceThresholdA = float.MaxValue; } float minimumSweepTimestepB, sweepConvergenceThresholdB; - if (bCollidable.Continuity.Mode == ContinuousDetectionMode.Continuous) + if (continuityB.Mode == ContinuousDetectionMode.Continuous) { - minimumSweepTimestepB = bCollidable.Continuity.MinimumSweepTimestep; - sweepConvergenceThresholdB = bCollidable.Continuity.SweepConvergenceThreshold; + minimumSweepTimestepB = continuityB.MinimumSweepTimestep; + sweepConvergenceThresholdB = continuityB.SweepConvergenceThreshold; } else { @@ -531,8 +546,8 @@ private unsafe void AddBatchEntries(int workerIndex, ref OverlapWorker overlapWo } var filter = new CCDSweepFilter { NarrowPhase = this, Pair = pair, WorkerIndex = workerIndex }; if (sweepTask.Sweep( - shapeDataA, aCollidable.Shape.Type, poseA.Orientation, velocityA, - shapeDataB, bCollidable.Shape.Type, poseB.Position - poseA.Position, poseB.Orientation, velocityB, + shapeDataA, shapeA.Type, poseA.Orientation, velocityA, + shapeDataB, shapeB.Type, poseB.Position - poseA.Position, poseB.Orientation, velocityB, timestepDuration, //Note that we use the *smaller* thresholds. This allows high fidelity objects to demand more time even if paired with low fidelity objects. Math.Min(minimumSweepTimestepA, minimumSweepTimestepB), @@ -548,7 +563,7 @@ private unsafe void AddBatchEntries(int workerIndex, ref OverlapWorker overlapWo PoseIntegration.Integrate(poseB.Orientation, velocityB.Angular, t1, out var integratedOrientationB); var offsetB = poseB.Position - poseA.Position + (velocityB.Linear - velocityA.Linear) * t1; overlapWorker.Batcher.Add( - aCollidable.Shape, bCollidable.Shape, + shapeA, shapeB, offsetB, integratedOrientationA, integratedOrientationB, velocityA, velocityB, speculativeMargin, maximumExpansion, new PairContinuation((int)continuationIndex.Packed)); } @@ -560,7 +575,7 @@ private unsafe void AddBatchEntries(int workerIndex, ref OverlapWorker overlapWo //No CCD continuation was created, so create a discrete one. continuationIndex = overlapWorker.Batcher.Callbacks.AddDiscrete(ref pair); overlapWorker.Batcher.Add( - aCollidable.Shape, bCollidable.Shape, + shapeA, shapeB, poseB.Position - poseA.Position, poseA.Orientation, poseB.Orientation, velocityA, velocityB, speculativeMargin, maximumExpansion, new PairContinuation((int)continuationIndex.Packed)); } diff --git a/BepuPhysics/IslandAwakener.cs b/BepuPhysics/IslandAwakener.cs index 78f729168..2d41f8339 100644 --- a/BepuPhysics/IslandAwakener.cs +++ b/BepuPhysics/IslandAwakener.cs @@ -361,7 +361,7 @@ internal unsafe void ExecutePhaseTwoJob(int index) { if (movedLeaf.Mobility == Collidables.CollidableMobility.Static) { - statics.Collidables[statics.HandleToIndex[movedLeaf.StaticHandle.Value]].BroadPhaseIndex = staticBroadPhaseIndexToRemove; + statics[movedLeaf.StaticHandle].BroadPhaseIndex = staticBroadPhaseIndexToRemove; } else { diff --git a/BepuPhysics/Simulation.cs b/BepuPhysics/Simulation.cs index 203149895..cd15c50e6 100644 --- a/BepuPhysics/Simulation.cs +++ b/BepuPhysics/Simulation.cs @@ -174,7 +174,7 @@ internal void ValidateCollidables() Debug.Assert(inactiveShapefulBodyCount + Statics.Count == BroadPhase.StaticTree.LeafCount); for (int i = 0; i < Statics.Count; ++i) { - ref var collidable = ref Statics.Collidables[i]; + ref var collidable = ref Statics[i]; Debug.Assert(collidable.Shape.Exists, "All static collidables must have shapes. That's their only purpose."); Debug.Assert(collidable.BroadPhaseIndex >= 0 && collidable.BroadPhaseIndex < BroadPhase.StaticTree.LeafCount); diff --git a/BepuPhysics/Simulation_Queries.cs b/BepuPhysics/Simulation_Queries.cs index b150ee7c3..872ebdc99 100644 --- a/BepuPhysics/Simulation_Queries.cs +++ b/BepuPhysics/Simulation_Queries.cs @@ -121,8 +121,10 @@ internal unsafe void GetPoseAndShape(CollidableReference reference, out RigidPos if (reference.Mobility == CollidableMobility.Static) { var index = Statics.HandleToIndex[reference.StaticHandle.Value]; - pose = Statics.Poses.Memory + index; - shape = Statics.Collidables[index].Shape; + ref var collidable = ref Statics[index]; + //Not a GC hole; the Statics holds everything in unmoving memory. + pose = (RigidPose*)Unsafe.AsPointer(ref collidable.Pose); + shape = collidable.Shape; } else { diff --git a/BepuPhysics/StaticReference.cs b/BepuPhysics/StaticReference.cs index fc2eb5787..7095aed3c 100644 --- a/BepuPhysics/StaticReference.cs +++ b/BepuPhysics/StaticReference.cs @@ -55,6 +55,18 @@ public int Index get { return Statics.HandleToIndex[Handle.Value]; } } + /// + /// Gets a reference to the entirety of the static's memory. + /// + public ref Static Static + { + [MethodImpl(MethodImplOptions.AggressiveInlining)] + get + { + return ref Statics[Handle]; + } + } + /// /// Gets a reference to the static's pose. /// @@ -63,19 +75,19 @@ public ref RigidPose Pose [MethodImpl(MethodImplOptions.AggressiveInlining)] get { - return ref Statics.Poses[Statics.HandleToIndex[Handle.Value]]; + return ref Statics[Handle].Pose; } } /// - /// Gets a reference to the static's collidable. + /// Gets a reference to the static's collision continuity settings. /// - public ref Collidable Collidable + public ref ContinuousDetection Continuity { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { - return ref Statics.Collidables[Statics.HandleToIndex[Handle.Value]]; + return ref Statics[Handle].Continuity; } } @@ -142,7 +154,7 @@ public unsafe BoundingBox BoundingBox public unsafe void GetBoundsReferencesFromBroadPhase(out Vector3* min, out Vector3* max) { var index = Index; - ref var collidable = ref Statics.Collidables[index]; + ref var collidable = ref Statics[index]; Debug.Assert(collidable.Shape.Exists, "Statics must have a shape. Something's not right here."); Statics.broadPhase.GetStaticBoundsPointers(collidable.BroadPhaseIndex, out min, out max); } diff --git a/BepuPhysics/Statics.cs b/BepuPhysics/Statics.cs index 016b95068..eea9fe745 100644 --- a/BepuPhysics/Statics.cs +++ b/BepuPhysics/Statics.cs @@ -47,13 +47,48 @@ public bool ShouldAwaken(BodyReference body) } /// - /// Collection of allocated static collidables. + /// Stores data for a static collidable in the simulation. Statics can be posed and collide, but have no velocity and no dynamic behavior. + /// + /// Unlike bodies, statics have a very simple access pattern. Most data is referenced together and there are no extreme high frequency data accesses like there are in the solver. + /// Everything can be conveniently stored within a single location contiguously. + public struct Static + { + /// + /// Pose of the static collidable. + /// + public RigidPose Pose; + + /// + /// Continuous collision detection settings for this collidable. Includes the collision detection mode to use and tuning variables associated with those modes. + /// + /// Note that statics cannot move, so there is no difference between and for them. + /// The minimum and maximum speculative margins will still constrain the margins created for each collision pair involved in the static. + /// Enabling will still require that pairs associated with the static use swept continuous collision detection. + public ContinuousDetection Continuity; + + /// + /// Index of the shape used by the static. While this can be changed, any transition from shapeless->shapeful or shapeful->shapeless must be reported to the broad phase. + /// If you need to perform such a transition, consider using or Statics.ApplyDescription; those functions update the relevant state. + /// + public TypedIndex Shape; + //Note that statics do not store a 'speculative margin' independently of the contini + /// + /// Index of the collidable in the broad phase. Used to look up the target location for bounding box scatters. Under normal circumstances, this should not be set externally. + /// + public int BroadPhaseIndex; + } + + /// + /// Collection of allocated statics. /// public class Statics { + //Unlike bodies, there's not a lot of value to tight packing for the sake of enumeration. + //This uses the same kind of handle indirection just to allow Count to be meaningful, + //but if there comes a time where maintaining this costs something, it can be pretty easily swapped out for an unmoving index approach. + /// /// Remaps a static handle integer value to the actual array index of the static. - /// The backing array index may change in response to cache optimization. /// public Buffer HandleToIndex; /// @@ -63,10 +98,10 @@ public class Statics /// /// The set of collidables owned by each static. Speculative margins, continuity settings, and shape indices can be changed directly. /// Shape indices cannot transition between pointing at a shape and pointing at nothing or vice versa without notifying the broad phase of the collidable addition or removal. + /// Consider using or to handle the bookkeeping changes automatically if changing the shape. /// - public Buffer Collidables; + public Buffer StaticsBuffer; - public Buffer Poses; public IdPool HandlePool; protected BufferPool pool; public int Count; @@ -76,6 +111,35 @@ public class Statics internal BroadPhase broadPhase; internal IslandAwakener awakener; + /// + /// Gets a reference to the raw memory backing a static collidable. + /// + /// Handle of the static to retrieve a memory reference for. + /// Direct reference to the memory backing a static collidable. + public ref Static this[StaticHandle handle] + { + get + { + ValidateExistingHandle(handle); + return ref StaticsBuffer[HandleToIndex[handle.Value]]; + } + } + + /// + /// Gets a reference to the raw memory backing a static collidable. + /// + /// Index of the static to retrieve a memory reference for. + /// Direct reference to the memory backing a static collidable. + public ref Static this[int index] + { + get + { + Debug.Assert(index >= 0 && index < Count); + ValidateExistingHandle(IndexToHandle[index]); + return ref StaticsBuffer[index]; + } + } + public unsafe Statics(BufferPool pool, Shapes shapes, Bodies bodies, BroadPhase broadPhase, int initialCapacity = 4096) { this.pool = pool; @@ -93,11 +157,10 @@ unsafe void InternalResize(int targetCapacity) Debug.Assert(targetCapacity > 0, "Resize is not meant to be used as Dispose. If you want to return everything to the pool, use Dispose instead."); //Note that we base the bundle capacities on the static capacity. This simplifies the conditions on allocation targetCapacity = BufferPool.GetCapacityForCount(targetCapacity); - Debug.Assert(Poses.Length != BufferPool.GetCapacityForCount(targetCapacity), "Should not try to use internal resize of the result won't change the size."); - pool.ResizeToAtLeast(ref Poses, targetCapacity, Count); + Debug.Assert(StaticsBuffer.Length != BufferPool.GetCapacityForCount(targetCapacity), "Should not try to use internal resize of the result won't change the size."); + pool.ResizeToAtLeast(ref StaticsBuffer, targetCapacity, Count); pool.ResizeToAtLeast(ref IndexToHandle, targetCapacity, Count); pool.ResizeToAtLeast(ref HandleToIndex, targetCapacity, Count); - pool.ResizeToAtLeast(ref Collidables, targetCapacity, Count); //Initialize all the indices beyond the copied region to -1. Unsafe.InitBlockUnaligned(HandleToIndex.Memory + Count, 0xFF, (uint)(sizeof(int) * (HandleToIndex.Length - Count))); //Note that we do NOT modify the idpool's internal queue size here. We lazily handle that during adds, and during explicit calls to EnsureCapacity, Compact, and Resize. @@ -120,7 +183,7 @@ public bool StaticExists(StaticHandle handle) } [Conditional("DEBUG")] - public void ValidateExistingHandle(StaticHandle handle) + internal void ValidateExistingHandle(StaticHandle handle) { Debug.Assert(StaticExists(handle), "Handle must exist according to the StaticExists test."); Debug.Assert(handle.Value >= 0, "Handles must be nonnegative."); @@ -175,10 +238,10 @@ void AwakenBodiesInBounds(ref BoundingBox bounds, ref TFilter filter) w } } - unsafe void AwakenBodiesInExistingBounds(ref Collidable collidable, ref TFilter filter) where TFilter : struct, IStaticChangeAwakeningFilter + unsafe void AwakenBodiesInExistingBounds(int broadPhaseIndex, ref TFilter filter) where TFilter : struct, IStaticChangeAwakeningFilter { - Debug.Assert(collidable.BroadPhaseIndex >= 0 && collidable.BroadPhaseIndex < broadPhase.StaticTree.LeafCount); - broadPhase.GetStaticBoundsPointers(collidable.BroadPhaseIndex, out var minPointer, out var maxPointer); + Debug.Assert(broadPhaseIndex >= 0 && broadPhaseIndex < broadPhase.StaticTree.LeafCount); + broadPhase.GetStaticBoundsPointers(broadPhaseIndex, out var minPointer, out var maxPointer); BoundingBox oldBounds; oldBounds.Min = *minPointer; oldBounds.Max = *maxPointer; @@ -197,9 +260,9 @@ public void RemoveAt(int index, ref TAwakeningFilter filter) w ValidateExistingHandle(IndexToHandle[index]); var handle = IndexToHandle[index]; - ref var collidable = ref Collidables[index]; + ref var collidable = ref this[index]; Debug.Assert(collidable.Shape.Exists, "Static collidables cannot lack a shape. Their only purpose is colliding."); - AwakenBodiesInExistingBounds(ref collidable, ref filter); + AwakenBodiesInExistingBounds(collidable.BroadPhaseIndex, ref filter); var removedBroadPhaseIndex = collidable.BroadPhaseIndex; if (broadPhase.RemoveStaticAt(removedBroadPhaseIndex, out var movedLeaf)) @@ -213,7 +276,7 @@ public void RemoveAt(int index, ref TAwakeningFilter filter) w if (movedLeaf.Mobility == CollidableMobility.Static) { //This is a static collidable, not a body. - Collidables[HandleToIndex[movedLeaf.StaticHandle.Value]].BroadPhaseIndex = removedBroadPhaseIndex; + this[movedLeaf.StaticHandle].BroadPhaseIndex = removedBroadPhaseIndex; } else { @@ -232,9 +295,7 @@ public void RemoveAt(int index, ref TAwakeningFilter filter) w { var movedStaticOriginalIndex = Count; //Copy the memory state of the last element down. - Poses[index] = Poses[movedStaticOriginalIndex]; - //Note that if you ever treat the world inertias as 'always updated', it would need to be copied here. - Collidables[index] = Collidables[movedStaticOriginalIndex]; + this[index] = this[movedStaticOriginalIndex]; //Point the static handles at the new location. var lastHandle = IndexToHandle[movedStaticOriginalIndex]; HandleToIndex[lastHandle.Value] = index; @@ -283,8 +344,8 @@ public void Remove(StaticHandle handle) public void UpdateBounds(StaticHandle handle) { var index = HandleToIndex[handle.Value]; - ref var collidable = ref Collidables[index]; - shapes.UpdateBounds(Poses[index], ref collidable.Shape, out var bodyBounds); + ref var collidable = ref this[index]; + shapes.UpdateBounds(collidable.Pose, ref collidable.Shape, out var bodyBounds); broadPhase.UpdateStaticBounds(collidable.BroadPhaseIndex, bodyBounds.Min, bodyBounds.Max); } @@ -296,17 +357,13 @@ void ComputeNewBoundsAndAwaken(in RigidPose pose, TypedIndex s AwakenBodiesInBounds(ref bounds, ref filter); } - - - internal void ApplyDescriptionByIndexWithoutBroadPhaseModification(int index, in StaticDescription description, ref TAwakeningFilter filter, out BoundingBox bounds) where TAwakeningFilter : struct, IStaticChangeAwakeningFilter { - Poses[index] = description.Pose; - ref var collidable = ref Collidables[index]; + ref var collidable = ref this[index]; + collidable.Pose = description.Pose; Debug.Assert(description.Collidable.Shape.Exists, "Static collidables must have a shape. Their only purpose is colliding."); collidable.Continuity = description.Collidable.Continuity; collidable.Shape = description.Collidable.Shape; - collidable.SpeculativeMargin = 0; ComputeNewBoundsAndAwaken(description.Pose, description.Collidable.Shape, ref filter, out bounds); } @@ -334,7 +391,7 @@ public StaticHandle Add(in StaticDescription description, ref IndexToHandle[index] = handle; ApplyDescriptionByIndexWithoutBroadPhaseModification(index, description, ref filter, out var bounds); //This is a new add, so we need to add it to the broad phase. - Collidables[index].BroadPhaseIndex = broadPhase.AddStatic(new CollidableReference(handle), ref bounds); + this[index].BroadPhaseIndex = broadPhase.AddStatic(new CollidableReference(handle), ref bounds); return handle; } @@ -361,10 +418,10 @@ public void SetShape(StaticHandle handle, TypedIndex newShape, ValidateExistingHandle(handle); Debug.Assert(newShape.Exists, "Statics must have a shape."); var index = HandleToIndex[handle.Value]; - ref var collidable = ref Collidables[index]; - AwakenBodiesInExistingBounds(ref collidable, ref filter); + ref var collidable = ref this[index]; + AwakenBodiesInExistingBounds(collidable.BroadPhaseIndex, ref filter); //Note: the min and max here are in absolute coordinates, which means this is a spot that has to be updated in the event that positions use a higher precision representation. - ComputeNewBoundsAndAwaken(Poses[index], newShape, ref filter, out var bounds); + ComputeNewBoundsAndAwaken(collidable.Pose, newShape, ref filter, out var bounds); broadPhase.UpdateStaticBounds(collidable.BroadPhaseIndex, bounds.Min, bounds.Max); } @@ -393,10 +450,11 @@ public unsafe void ApplyDescription(StaticHandle handle, in St var index = HandleToIndex[handle.Value]; Debug.Assert(description.Collidable.Shape.Exists, "Static collidables cannot lack a shape. Their only purpose is colliding."); //Wake all bodies up in the old bounds AND the new bounds. Sleeping bodies that may have been resting on the old static need to be aware of the new environment. - AwakenBodiesInExistingBounds(ref Collidables[index], ref filter); + var broadPhaseIndex = this[index].BroadPhaseIndex; + AwakenBodiesInExistingBounds(broadPhaseIndex, ref filter); ApplyDescriptionByIndexWithoutBroadPhaseModification(index, description, ref filter, out var bounds); //This applies to an existing static, so we should modify the static's bounds in the broad phase. - broadPhase.UpdateStaticBounds(Collidables[index].BroadPhaseIndex, bounds.Min, bounds.Max); + broadPhase.UpdateStaticBounds(broadPhaseIndex, bounds.Min, bounds.Max); } /// @@ -420,8 +478,8 @@ public void GetDescription(StaticHandle handle, out StaticDescription descriptio { ValidateExistingHandle(handle); var index = HandleToIndex[handle.Value]; - description.Pose = Poses[index]; - ref var collidable = ref Collidables[index]; + ref var collidable = ref this[index]; + description.Pose = collidable.Pose; description.Collidable.Continuity = collidable.Continuity; description.Collidable.Shape = collidable.Shape; } @@ -479,52 +537,10 @@ public void EnsureCapacity(int capacity) /// The object can be reused if it is reinitialized by using EnsureCapacity or Resize. public void Dispose() { - pool.Return(ref Poses); + pool.Return(ref StaticsBuffer); pool.Return(ref HandleToIndex); pool.Return(ref IndexToHandle); - pool.Return(ref Collidables); HandlePool.Dispose(pool); } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - private void GatherPose(ref float targetPositionBase, ref float targetOrientationBase, int targetLaneIndex, int index) - { - ref var source = ref Poses[index]; - ref var targetPositionSlot = ref Unsafe.Add(ref targetPositionBase, targetLaneIndex); - ref var targetOrientationSlot = ref Unsafe.Add(ref targetOrientationBase, targetLaneIndex); - targetPositionSlot = source.Position.X; - Unsafe.Add(ref targetPositionSlot, Vector.Count) = source.Position.Y; - Unsafe.Add(ref targetPositionSlot, 2 * Vector.Count) = source.Position.Z; - targetOrientationSlot = source.Orientation.X; - Unsafe.Add(ref targetOrientationSlot, Vector.Count) = source.Orientation.Y; - Unsafe.Add(ref targetOrientationSlot, 2 * Vector.Count) = source.Orientation.Z; - Unsafe.Add(ref targetOrientationSlot, 3 * Vector.Count) = source.Orientation.W; - } - - //This looks a little different because it's used by AABB calculation, not constraint pairs. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - internal void GatherDataForBounds(ref int start, int count, out RigidPoseWide poses, out Vector shapeIndices, out Vector maximumExpansion) - { - Debug.Assert(count <= Vector.Count); - Unsafe.SkipInit(out poses); - Unsafe.SkipInit(out shapeIndices); - Unsafe.SkipInit(out maximumExpansion); - ref var targetPositionBase = ref Unsafe.As, float>(ref poses.Position.X); - ref var targetOrientationBase = ref Unsafe.As, float>(ref poses.Orientation.X); - ref var targetShapeBase = ref Unsafe.As, int>(ref shapeIndices); - ref var targetExpansionBase = ref Unsafe.As, float>(ref maximumExpansion); - for (int i = 0; i < count; ++i) - { - var index = Unsafe.Add(ref start, i); - GatherPose(ref targetPositionBase, ref targetOrientationBase, i, index); - ref var collidable = ref Collidables[index]; - Unsafe.Add(ref targetShapeBase, i) = collidable.Shape.Index; - //Not entirely pleased with the fact that this pulls in some logic from bounds calculation. - Unsafe.Add(ref targetExpansionBase, i) = collidable.Continuity.AllowExpansionBeyondSpeculativeMargin ? float.MaxValue : collidable.SpeculativeMargin; - } - } - - - } } diff --git a/DemoRenderer/ShapeDrawing/ShapesExtractor.cs b/DemoRenderer/ShapeDrawing/ShapesExtractor.cs index 5f78240f0..887112695 100644 --- a/DemoRenderer/ShapeDrawing/ShapesExtractor.cs +++ b/DemoRenderer/ShapeDrawing/ShapesExtractor.cs @@ -280,7 +280,8 @@ void AddStaticShape(Shapes shapes, Statics statics, int index) var staticBase = new Vector3(0.1f, 0.057f, 0.014f); var staticVariationSpan = new Vector3(0.07f, 0.07f, 0.03f); var color = staticBase + staticVariationSpan * colorVariation; - AddShape(shapes, statics.Collidables[index].Shape, ref statics.Poses[index], color); + ref var collidable = ref statics[index]; + AddShape(shapes, collidable.Shape, ref collidable.Pose, color); } public void AddInstances(Simulation simulation, IThreadDispatcher threadDispatcher = null) diff --git a/Demos/Demos/CollisionQueryDemo.cs b/Demos/Demos/CollisionQueryDemo.cs index f726cde97..1d27c985e 100644 --- a/Demos/Demos/CollisionQueryDemo.cs +++ b/Demos/Demos/CollisionQueryDemo.cs @@ -120,9 +120,9 @@ void GetPoseAndShape(CollidableReference reference, out RigidPose pose, out Type //Collidables can be associated with either bodies or statics. We have to look in a different place depending on which it is. if (reference.Mobility == CollidableMobility.Static) { - var staticIndex = Simulation.Statics.HandleToIndex[reference.StaticHandle.Value]; - pose = Simulation.Statics.Poses[staticIndex]; - shapeIndex = Simulation.Statics.Collidables[staticIndex].Shape; + ref var collidable = ref Simulation.Statics[reference.StaticHandle]; + pose = collidable.Pose; + shapeIndex = collidable.Shape; } else { diff --git a/Demos/Demos/ContinuousCollisionDetectionDemo.cs b/Demos/Demos/ContinuousCollisionDetectionDemo.cs index 3081ea009..04218b75e 100644 --- a/Demos/Demos/ContinuousCollisionDetectionDemo.cs +++ b/Demos/Demos/ContinuousCollisionDetectionDemo.cs @@ -35,6 +35,9 @@ ConstraintHandle BuildSpinner(Vector3 initialPosition, float rotationSpeed) //It's pretty rare, though- if you have a situation where that sort of failure is common, consider increasing the collidable's speculative margin or using a higher update rate. //(The reason why we don't always just rely on large speculative margins is ghost collisions- the speculative contacts might not represent collisions //that would have actually happened, but get included in the constraint solution anyway. They're fairly rare, but it's something to watch out for.) + + //Using a restricted speculative margin by setting the maximumSpeculativeMargin to 0.2 means that collision detection won't accept distant contacts. + //This pretty much eliminates ghost collisions, while the continuous sweep helps avoid missed collisions. var spinnerBlade = Simulation.Bodies.Add(BodyDescription.CreateDynamic(initialPosition, bladeInertia, new CollidableDescription(shapeIndex, ContinuousDetection.Continuous(1e-4f, 1e-4f, maximumSpeculativeMargin: 0.2f)), new BodyActivityDescription(0.01f))); Simulation.Solver.Add(spinnerBase, spinnerBlade, new Hinge { LocalHingeAxisA = new Vector3(0, 0, 1), LocalHingeAxisB = new Vector3(0, 0, 1), LocalOffsetB = new Vector3(0, 0, -3), SpringSettings = new SpringSettings(30, 1) }); Simulation.Solver.Add(spinnerBase, spinnerBlade, new AngularAxisMotor { LocalAxisA = new Vector3(0, 0, 1), Settings = new MotorSettings(10, 1e-4f), TargetVelocity = rotationSpeed }); diff --git a/Demos/Demos/CustomVoxelCollidableDemo.cs b/Demos/Demos/CustomVoxelCollidableDemo.cs index 492de85b4..51b61618c 100644 --- a/Demos/Demos/CustomVoxelCollidableDemo.cs +++ b/Demos/Demos/CustomVoxelCollidableDemo.cs @@ -449,7 +449,7 @@ public override unsafe void Render(Renderer renderer, Camera camera, Input input //The renderer doesn't have a super flexible drawing system, so we'll instead just directly add the voxel shapes. Not super efficient, but it works! var shape = new Box(voxels.VoxelSize.X, voxels.VoxelSize.Y, voxels.VoxelSize.Z); var shapeDataPointer = &shape; - ref var voxelsPose = ref Simulation.Statics.Poses[Simulation.Statics.HandleToIndex[handle.Value]]; + ref var voxelsPose = ref Simulation.Statics[handle].Pose; for (int i = 0; i < voxels.ChildCount; ++i) { var localPose = new RigidPose((voxels.VoxelIndices[i] + new Vector3(0.5f)) * voxels.VoxelSize); diff --git a/Demos/Demos/Sponsors/SponsorDemo.cs b/Demos/Demos/Sponsors/SponsorDemo.cs index e3ba5e6dc..380af8903 100644 --- a/Demos/Demos/Sponsors/SponsorDemo.cs +++ b/Demos/Demos/Sponsors/SponsorDemo.cs @@ -256,7 +256,7 @@ public override void Render(Renderer renderer, Camera camera, Input input, TextB //We'll hardcode the overlord newts. Not going to be a problem, I suspect. { - var worldTextPosition = Simulation.Statics.Poses[Simulation.Statics.HandleToIndex[overlordNewtHandle.Value]].Position + new Vector3(0, 48, 0); + var worldTextPosition = Simulation.Statics[overlordNewtHandle].Pose.Position + new Vector3(0, 48, 0); Helpers.GetScreenLocation(worldTextPosition, viewProjection, resolution, out var screenspacePosition); const float nameHeight = 14; var name = sponsors3[0].Name; From 667390c297ae0342dc237ebe0a2b430fa5abc0aa Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 26 Nov 2021 19:08:45 -0600 Subject: [PATCH 295/947] Added an indexer for body references. --- BepuPhysics/Bodies.cs | 19 +++++++++++++++++++ BepuPhysics/BodyReference.cs | 1 + 2 files changed, 20 insertions(+) diff --git a/BepuPhysics/Bodies.cs b/BepuPhysics/Bodies.cs index 18bac4d92..e9ed7e4e5 100644 --- a/BepuPhysics/Bodies.cs +++ b/BepuPhysics/Bodies.cs @@ -53,6 +53,24 @@ public partial class Bodies /// Reference to the active body set. public unsafe ref BodySet ActiveSet { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { return ref *Sets.Memory; } } + /// + /// Gets a reference to a body in the collection. + /// + /// Handle of the body to pull a reference for. + /// Reference to the requested body. + /// This is an alias for the older and the constructor. They are all equivalent. + public BodyReference this[BodyHandle handle] + { + get + { + return new BodyReference(handle, this); + } + } + + + /// + /// Gets the pool used by the bodies collection to allocate and free memory. + /// public BufferPool Pool { get; private set; } internal IslandAwakener awakener; @@ -511,6 +529,7 @@ public void GetDescription(BodyHandle handle, out BodyDescription description) /// /// Handle of the body to grab a reference of. /// Reference to the desired body. + /// This is an alias for and the constructor. They are all equivalent. public BodyReference GetBodyReference(BodyHandle handle) { ValidateExistingHandle(handle); diff --git a/BepuPhysics/BodyReference.cs b/BepuPhysics/BodyReference.cs index 70b748f99..f687b3ce9 100644 --- a/BepuPhysics/BodyReference.cs +++ b/BepuPhysics/BodyReference.cs @@ -30,6 +30,7 @@ public struct BodyReference /// /// Handle of the body to refer to. /// Collection containing the body. + /// This is equivalent to and . public BodyReference(BodyHandle handle, Bodies bodies) { Handle = handle; From 973438a16db38fa32c865890d4243ed1390ed92d Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 29 Nov 2021 15:20:16 -0600 Subject: [PATCH 296/947] Replaced usages of GetBodyReference with equivalent indexer. --- BepuPhysics/Solver.cs | 8 ++++---- BepuPhysics/Statics.cs | 2 +- Demos/DemoSet.cs | 2 +- Demos/Demos/Cars/CarDemo.cs | 2 +- Demos/Demos/Characters/CharacterControllers.cs | 2 +- Demos/Demos/Characters/CharacterDemo.cs | 2 +- Demos/Demos/CollisionQueryDemo.cs | 2 +- Demos/Demos/ContactEventsDemo.cs | 8 ++++---- Demos/Demos/RopeTwistDemo.cs | 2 +- Demos/Demos/SolverContactEnumerationDemo.cs | 4 ++-- Demos/Demos/Sponsors/SponsorCharacterAI.cs | 4 ++-- Demos/Demos/Sponsors/SponsorNewt.cs | 4 ++-- Demos/Demos/Tanks/AITank.cs | 6 +++--- Demos/Demos/Tanks/Tank.cs | 8 ++++---- Demos/Demos/Tanks/TankDemo.cs | 4 ++-- Demos/Grabber.cs | 2 +- .../ConstrainedKinematicIntegrationTest.cs | 6 +++--- 17 files changed, 34 insertions(+), 34 deletions(-) diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index e607ffe6a..446988f49 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -351,7 +351,7 @@ internal unsafe void ValidateConstraintReferenceKinematicity() else { //Sleeping set references are handles. - kinematicByInertia = bodies.GetBodyReference(new BodyHandle { Value = encodedBodyReference & Bodies.BodyReferenceMask }).Kinematic; + kinematicByInertia = bodies[new BodyHandle { Value = encodedBodyReference & Bodies.BodyReferenceMask }].Kinematic; } Debug.Assert(kinematicByEncodedReference == kinematicByInertia, "Constraint reference encoded kinematicity must match actual kinematicity by inertia."); } @@ -379,7 +379,7 @@ internal unsafe void ValidateConstrainedKinematicsSet() } for (int i = 0; i < ConstrainedKinematicHandles.Count; ++i) { - var bodyReference = bodies.GetBodyReference(new BodyHandle(ConstrainedKinematicHandles[i])); + var bodyReference = bodies[new BodyHandle(ConstrainedKinematicHandles[i])]; Debug.Assert(bodyReference.Kinematic && bodyReference.Constraints.Count > 0, "Any body listed in the constrained kinematics set must be kinematic and constrained."); } } @@ -1560,7 +1560,7 @@ internal void TryRemoveDynamicBodyFromFallback(BodyHandle bodyHandle, int bodyIn { if (ActiveSet.SequentialFallback.TryRemoveDynamicBodyFromTracking(bodyIndex, ref allocationIdsToFree)) { - Debug.Assert(batchReferencedHandles[FallbackBatchThreshold].Contains(bodyHandle.Value) || bodies.GetBodyReference(bodyHandle).Kinematic, + Debug.Assert(batchReferencedHandles[FallbackBatchThreshold].Contains(bodyHandle.Value) || bodies[bodyHandle].Kinematic, "The batch referenced handles must include all constraint-involved dynamics, but will not include kinematics."); batchReferencedHandles[FallbackBatchThreshold].Unset(bodyHandle.Value); } @@ -1577,7 +1577,7 @@ public unsafe void LoopBody(int encodedBodyReference) } internal unsafe void UpdateReferencesForBodyBecomingKinematic(BodyHandle bodyHandle, int bodyIndex) { - Debug.Assert(bodies.GetBodyReference(bodyHandle).Kinematic); + Debug.Assert(bodies[bodyHandle].Kinematic); //Any constraints that connect only kinematic bodies together should be removed; they'll NaN out. //Ideally, the user would handle this for all non-contact constraints, but it would be rather annoying to //have to explicitly enumerate and remove all contact constraints any time you wanted to make a body kinematic. diff --git a/BepuPhysics/Statics.cs b/BepuPhysics/Statics.cs index eea9fe745..430b2dd64 100644 --- a/BepuPhysics/Statics.cs +++ b/BepuPhysics/Statics.cs @@ -231,7 +231,7 @@ void AwakenBodiesInBounds(ref BoundingBox bounds, ref TFilter filter) w broadPhase.StaticTree.GetOverlaps(bounds, ref collector); for (int i = 0; i < collector.SleepingBodyHandles.Count; ++i) { - if (filter.ShouldAwaken(bodies.GetBodyReference(collector.SleepingBodyHandles[i]))) + if (filter.ShouldAwaken(bodies[collector.SleepingBodyHandles[i]])) awakener.AwakenBody(collector.SleepingBodyHandles[i]); } collector.Dispose(); diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index 5f2816774..e0b0dd718 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -45,13 +45,13 @@ struct Option public DemoSet() { + AddOption(); AddOption(); AddOption(); AddOption(); AddOption(); AddOption(); AddOption(); - AddOption(); AddOption(); AddOption(); AddOption(); diff --git a/Demos/Demos/Cars/CarDemo.cs b/Demos/Demos/Cars/CarDemo.cs index 0eb438f17..ebdeb60ed 100644 --- a/Demos/Demos/Cars/CarDemo.cs +++ b/Demos/Demos/Cars/CarDemo.cs @@ -167,7 +167,7 @@ public override void Update(Window window, Camera camera, Input input, float dt) for (int i = 0; i < aiControllers.Length; ++i) { ref var ai = ref aiControllers[i]; - var body = Simulation.Bodies.GetBodyReference(ai.Controller.Car.Body); + var body = Simulation.Bodies[ai.Controller.Car.Body]; ref var pose = ref body.Pose; Matrix3x3.CreateFromQuaternion(pose.Orientation, out var orientation); var forwardVelocity = Vector3.Dot(orientation.Z, body.Velocity.Linear); diff --git a/Demos/Demos/Characters/CharacterControllers.cs b/Demos/Demos/Characters/CharacterControllers.cs index ebb921d35..be2f7dc6d 100644 --- a/Demos/Demos/Characters/CharacterControllers.cs +++ b/Demos/Demos/Characters/CharacterControllers.cs @@ -420,7 +420,7 @@ unsafe void ExpandBoundingBoxes(int start, int count) for (int i = start; i < end; ++i) { ref var character = ref characters[i]; - var characterBody = Simulation.Bodies.GetBodyReference(character.BodyHandle); + var characterBody = Simulation.Bodies[character.BodyHandle]; if (characterBody.Awake) { Simulation.BroadPhase.GetActiveBoundsPointers(characterBody.Collidable.BroadPhaseIndex, out var min, out var max); diff --git a/Demos/Demos/Characters/CharacterDemo.cs b/Demos/Demos/Characters/CharacterDemo.cs index f4c5676fc..8a3b5406d 100644 --- a/Demos/Demos/Characters/CharacterDemo.cs +++ b/Demos/Demos/Characters/CharacterDemo.cs @@ -178,7 +178,7 @@ public MovingPlatform(CollidableDescription collidable, double timeOffset, float public void Update(Simulation simulation, double time) { - var body = simulation.Bodies.GetBodyReference(BodyHandle); + var body = simulation.Bodies[BodyHandle]; ref var pose = ref body.Pose; ref var velocity = ref body.Velocity; var targetPose = PoseCreator(time + TimeOffset); diff --git a/Demos/Demos/CollisionQueryDemo.cs b/Demos/Demos/CollisionQueryDemo.cs index 1d27c985e..efc8fccdb 100644 --- a/Demos/Demos/CollisionQueryDemo.cs +++ b/Demos/Demos/CollisionQueryDemo.cs @@ -126,7 +126,7 @@ void GetPoseAndShape(CollidableReference reference, out RigidPose pose, out Type } else { - var bodyReference = Simulation.Bodies.GetBodyReference(reference.BodyHandle); + var bodyReference = Simulation.Bodies[reference.BodyHandle]; pose = bodyReference.Pose; shapeIndex = bodyReference.Collidable.Shape; } diff --git a/Demos/Demos/ContactEventsDemo.cs b/Demos/Demos/ContactEventsDemo.cs index 8d4b4cd2f..25443cd06 100644 --- a/Demos/Demos/ContactEventsDemo.cs +++ b/Demos/Demos/ContactEventsDemo.cs @@ -260,7 +260,7 @@ public void Register(CollidableReference collidable, IContactEventHandler handle /// Handlers to use for the body. public void Register(BodyHandle body, IContactEventHandler handler) { - Register(simulation.Bodies.GetBodyReference(body).CollidableReference, handler); + Register(simulation.Bodies[body].CollidableReference, handler); } /// @@ -308,7 +308,7 @@ public void Unregister(CollidableReference collidable) /// Body to stop listening for. public void Unregister(BodyHandle body) { - Unregister(simulation.Bodies.GetBodyReference(body).CollidableReference); + Unregister(simulation.Bodies[body].CollidableReference); } /// @@ -709,10 +709,10 @@ public override void Initialize(ContentArchive content, Camera camera) eventHandler = new EventHandler(Simulation, BufferPool); var listenedBody1 = Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 5, 0), 1, Simulation.Shapes, new Box(1, 2, 3))); - events.Register(Simulation.Bodies.GetBodyReference(listenedBody1).CollidableReference, eventHandler); + events.Register(Simulation.Bodies[listenedBody1].CollidableReference, eventHandler); var listenedBody2 = Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0.5f, 10, 0), 1, Simulation.Shapes, new Capsule(0.25f, 0.7f))); - events.Register(Simulation.Bodies.GetBodyReference(listenedBody2).CollidableReference, eventHandler); + events.Register(Simulation.Bodies[listenedBody2].CollidableReference, eventHandler); Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(30, 1, 30))))); diff --git a/Demos/Demos/RopeTwistDemo.cs b/Demos/Demos/RopeTwistDemo.cs index a51b9d88c..ca647c91c 100644 --- a/Demos/Demos/RopeTwistDemo.cs +++ b/Demos/Demos/RopeTwistDemo.cs @@ -104,7 +104,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var wreckingBallPosition = startLocation - new Vector3(0, ropeBodyRadius + (ropeBodyRadius * 2 + ropeBodySpacing) * ropeBodyCount + bigWreckingBall.Radius, 0); var description = BodyDescription.CreateDynamic(wreckingBallPosition, bigWreckingBallInertia, new CollidableDescription(bigWreckingBallIndex), new BodyActivityDescription(-0.01f)); var wreckingBallBodyHandle = Simulation.Bodies.Add(description); - var wreckingBallBody = Simulation.Bodies.GetBodyReference(wreckingBallBodyHandle); + var wreckingBallBody = Simulation.Bodies[wreckingBallBodyHandle]; wreckingBallBody.Velocity.Angular = new Vector3(0, 20, 0); filters.Allocate(wreckingBallBodyHandle) = new Filter { RopeIndex = (short)(16384 + twistIndex), IndexInRope = ropeBodyCount }; diff --git a/Demos/Demos/SolverContactEnumerationDemo.cs b/Demos/Demos/SolverContactEnumerationDemo.cs index e8167f87a..d1e834926 100644 --- a/Demos/Demos/SolverContactEnumerationDemo.cs +++ b/Demos/Demos/SolverContactEnumerationDemo.cs @@ -220,7 +220,7 @@ public void Dispose() public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) { - var sensorBody = Simulation.Bodies.GetBodyReference(sensorBodyHandle); + var sensorBody = Simulation.Bodies[sensorBodyHandle]; var extractor = new Extractor(BufferPool, sensorBody.Constraints.Count); //The basic idea behind the contact extractor is to submit it to a narrow phase contact accessor that is able to understand the solver's layout, //which will then call the contact extractor's relevant callbacks for the type of constraint encountered. @@ -236,7 +236,7 @@ public override void Render(Renderer renderer, Camera camera, Input input, TextB for (int manifoldIndex = 0; manifoldIndex < extractor.ConstraintContacts.Count; ++manifoldIndex) { ref var constraintContacts = ref extractor.ConstraintContacts[manifoldIndex]; - var bodyA = Simulation.Bodies.GetBodyReference(constraintContacts.BodyA); + var bodyA = Simulation.Bodies[constraintContacts.BodyA]; sensorContactCount += constraintContacts.Contacts.Count; for (int contactIndex = 0; contactIndex < constraintContacts.Contacts.Count; ++contactIndex) { diff --git a/Demos/Demos/Sponsors/SponsorCharacterAI.cs b/Demos/Demos/Sponsors/SponsorCharacterAI.cs index fbf2cca52..5c32d409f 100644 --- a/Demos/Demos/Sponsors/SponsorCharacterAI.cs +++ b/Demos/Demos/Sponsors/SponsorCharacterAI.cs @@ -31,12 +31,12 @@ public SponsorCharacterAI(CharacterControllers characters, in CollidableDescript public void Update(CharacterControllers characters, Simulation simulation, ref QuickList newts, in Vector2 arenaMin, in Vector2 arenaMax, Random random) { - var body = simulation.Bodies.GetBodyReference(bodyHandle); + var body = simulation.Bodies[bodyHandle]; Vector2 influenceSum = default; bool spooked = false; for (int i = 0; i < newts.Count; ++i) { - ref var newtPosition = ref simulation.Bodies.GetBodyReference(newts[i].BodyHandle).Pose.Position; + ref var newtPosition = ref simulation.Bodies[newts[i].BodyHandle].Pose.Position; var offset = newtPosition - body.Pose.Position; var distance = offset.Length(); if (distance > 1e-10f) diff --git a/Demos/Demos/Sponsors/SponsorNewt.cs b/Demos/Demos/Sponsors/SponsorNewt.cs index 2e34cbe2a..59acc0f96 100644 --- a/Demos/Demos/Sponsors/SponsorNewt.cs +++ b/Demos/Demos/Sponsors/SponsorNewt.cs @@ -34,7 +34,7 @@ public SponsorNewt(Simulation simulation, TypedIndex shape, float height, in Vec public void Update(Simulation simulation, double time, float height, in Vector2 arenaMin, in Vector2 arenaMax, Random random, float inverseDt) { const float jumpDuration = 1; - var body = simulation.Bodies.GetBodyReference(BodyHandle); + var body = simulation.Bodies[BodyHandle]; if (time >= nextAllowedJump) { //Choose a jump location. It should be within the arena, and generally somewhere ahead of the newt. @@ -96,7 +96,7 @@ public void Update(Simulation simulation, double time, float height, in Vector2 public readonly void Render(Simulation simulation, List sponsors, Renderer renderer, in Matrix viewProjection, in Vector2 resolution, TextBuilder text, Font font) { var name = sponsors[SponsorIndex].Name; - var body = simulation.Bodies.GetBodyReference(BodyHandle); + var body = simulation.Bodies[BodyHandle]; Helpers.GetScreenLocation(body.Pose.Position + new Vector3(0, 8, 0), viewProjection, resolution, out var screenspacePosition); const float nameHeight = 14; var nameLength = GlyphBatch.MeasureLength(name, font, nameHeight); diff --git a/Demos/Demos/Tanks/AITank.cs b/Demos/Demos/Tanks/AITank.cs index 4d6c1cf88..5e06e30d7 100644 --- a/Demos/Demos/Tanks/AITank.cs +++ b/Demos/Demos/Tanks/AITank.cs @@ -32,7 +32,7 @@ public struct AITank public void Update(Simulation simulation, CollidableProperty bodyProperties, Random random, long frameIndex, in Vector2 playAreaMin, in Vector2 playAreaMax, int aiIndex, ref QuickList aiTanks, ref int projectileCount) { - ref var currentPose = ref simulation.Bodies.GetBodyReference(Controller.Tank.Body).Pose; + ref var currentPose = ref simulation.Bodies[Controller.Tank.Body].Pose; //tankBodyPose = localTankBodyPose * tankPose //tankPose = inverse(localTankBodyPose) * tankBodyPose QuaternionEx.TransformUnitY(QuaternionEx.Concatenate(QuaternionEx.Conjugate(Controller.Tank.BodyLocalOrientation), currentPose.Orientation), out var tankUp); @@ -51,7 +51,7 @@ public void Update(Simulation simulation, CollidableProperty 50 && targetHorizontalMovementDirection.Y > 0.8f; //We're not going to compute an optimal firing solution- just aim directly at the middle of the other tank. Pretty poor choice, but that's fine. - ref var barrelPosition = ref simulation.Bodies.GetBodyReference(Controller.Tank.Barrel).Pose.Position; + ref var barrelPosition = ref simulation.Bodies[Controller.Tank.Barrel].Pose.Position; var barrelToTarget = targetTankPosition - barrelPosition; var barrelToTargetLength = barrelToTarget.Length(); barrelToTarget = barrelToTargetLength > 1e-10f ? barrelToTarget / barrelToTargetLength : new Vector3(0, 1, 0); diff --git a/Demos/Demos/Tanks/Tank.cs b/Demos/Demos/Tanks/Tank.cs index f5308a6c6..7318b9d2b 100644 --- a/Demos/Demos/Tanks/Tank.cs +++ b/Demos/Demos/Tanks/Tank.cs @@ -113,7 +113,7 @@ public readonly (float targetSwivelAngle, float targetPitchAngle) ComputeTurretA //Decompose the aim direction into target angles for the turret and barrel servos. //First, we need to compute the frame of reference and transform the aim direction into the tank's local space. //aimDirection * inverse(body.Pose.Orientation) * Tank.LocalBodyPose.Orientation * inverse(Tank.TurretBasis) - QuaternionEx.ConcatenateWithoutOverlap(QuaternionEx.Conjugate(simulation.Bodies.GetBodyReference(Body).Pose.Orientation), FromBodyLocalToTurretBasisLocal, out var toTurretBasis); + QuaternionEx.ConcatenateWithoutOverlap(QuaternionEx.Conjugate(simulation.Bodies[Body].Pose.Orientation), FromBodyLocalToTurretBasisLocal, out var toTurretBasis); //-Z in the turret basis points along the 0 angle direction for both swivel and pitch. //+Y is 90 degrees for pitch. //+X is 90 degres for swivel. @@ -150,7 +150,7 @@ public void SetAim(Simulation simulation, float targetSwivelAngle, float targetP /// Direction in which the barrel points. public readonly void ComputeBarrelDirection(Simulation simulation, out Vector3 barrelDirection) { - QuaternionEx.Transform(BarrelLocalDirection, simulation.Bodies.GetBodyReference(Barrel).Pose.Orientation, out barrelDirection); + QuaternionEx.Transform(BarrelLocalDirection, simulation.Bodies[Barrel].Pose.Orientation, out barrelDirection); } /// @@ -161,7 +161,7 @@ public readonly void ComputeBarrelDirection(Simulation simulation, out Vector3 b /// Handle of the created projectile body. public BodyHandle Fire(Simulation simulation, CollidableProperty bodyProperties) { - var barrel = simulation.Bodies.GetBodyReference(Barrel); + var barrel = simulation.Bodies[Barrel]; ref var barrelPose = ref barrel.Pose; RigidPose.Transform(BarrelLocalProjectileSpawn, barrelPose, out var projectileSpawn); QuaternionEx.Transform(BarrelLocalDirection, barrelPose.Orientation, out var barrelDirection); @@ -413,7 +413,7 @@ public void Explode(Simulation simulation, CollidableProperty Date: Mon, 29 Nov 2021 16:54:56 -0600 Subject: [PATCH 297/947] Compound speculative margins are now properly initialized and don't just stick at the maximum observed value. --- BepuPhysics/Collidables/BoundingBoxBatcher.cs | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/BepuPhysics/Collidables/BoundingBoxBatcher.cs b/BepuPhysics/Collidables/BoundingBoxBatcher.cs index 7b2bc4a9b..ad6b09624 100644 --- a/BepuPhysics/Collidables/BoundingBoxBatcher.cs +++ b/BepuPhysics/Collidables/BoundingBoxBatcher.cs @@ -296,8 +296,10 @@ public unsafe void ExecuteCompoundBatch(CompoundShapeBatch shape var shapeIndex = batch.ShapeIndices[i]; var bodyIndex = batch.Continuations[i].BodyIndex; ref var motionState = ref batch.MotionStates[i]; - //We have to clear out the bounds used by compounds, since each contributing body will merge their contribution into the whole. - broadPhase.GetActiveBoundsPointers(activeSet.Collidables[bodyIndex].BroadPhaseIndex, out var min, out var max); + //We have to clear out the bounds and speculative margin used by compounds, since each contributing body will merge their contribution into the whole. + ref var collidable = ref activeSet.Collidables[bodyIndex]; + collidable.SpeculativeMargin = 0; + broadPhase.GetActiveBoundsPointers(collidable.BroadPhaseIndex, out var min, out var max); *min = minValue; *max = maxValue; shapeBatch.shapes[batch.ShapeIndices[i]].AddChildBoundsToBatcher(ref this, motionState.Pose, motionState.Velocity, bodyIndex); From 70ab5900424dffd012179a13dfb2574a5e6d132e Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 30 Nov 2021 17:11:35 -0600 Subject: [PATCH 298/947] Fixed a cylinder cap-triangle face interior contact point projection bug. --- .../CollisionDetection/CollisionTasks/TriangleCylinderTester.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/TriangleCylinderTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/TriangleCylinderTester.cs index 9877add11..3876659c7 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/TriangleCylinderTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/TriangleCylinderTester.cs @@ -299,7 +299,7 @@ public unsafe void Test( //y = sign(localNormal.Y) * b.HalfLength //pointOnCylinder = (interiorOnCylinderN.X, y, interiorOnCylinderN.Y) //t = dot(localTriangleCenter - pointOnCylinder, triangleNormal) / dot(triangleNormal, localNormal) - var inverseDenominator = Vector.One / faceNormalADotNormal; + var inverseDenominator = new Vector(-1f) / faceNormalADotNormal; var yOffset = localTriangleCenter.Y - capCenterBY; var xOffset0 = localTriangleCenter.X - interiorOnCylinder0.X; var zOffset0 = localTriangleCenter.Z - interiorOnCylinder0.Y; From f0df5cf61f2e3fad50bd3d742bff3967a68aeceb Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 30 Nov 2021 19:29:19 -0600 Subject: [PATCH 299/947] Simplified body description creation; CollidableDescription and BodyActivityDescription both have implicit casts for the most common inputs. Demos updated to take advantage of collidable description change. --- BepuPhysics/BodyDescription.cs | 35 ++++++++++++++- .../Collidables/CollidableDescription.cs | 9 ++++ Demos/Demos/BlockChainDemo.cs | 8 ++-- Demos/Demos/BouncinessDemo.cs | 2 +- Demos/Demos/Cars/CarDemo.cs | 6 +-- Demos/Demos/Cars/SimpleCar.cs | 4 +- Demos/Demos/Characters/CharacterDemo.cs | 22 ++++----- Demos/Demos/ClothDemo.cs | 8 ++-- Demos/Demos/CollisionQueryDemo.cs | 6 +-- Demos/Demos/ColosseumDemo.cs | 10 ++--- Demos/Demos/CompoundTestDemo.cs | 25 ++++------- Demos/Demos/ContactEventsDemo.cs | 4 +- .../Demos/ContinuousCollisionDetectionDemo.cs | 6 +-- Demos/Demos/CustomVoxelCollidableDemo.cs | 6 +-- Demos/Demos/FountainStressTestDemo.cs | 8 ++-- Demos/Demos/NewtDemo.cs | 7 ++- Demos/Demos/PlanetDemo.cs | 6 +-- Demos/Demos/PyramidDemo.cs | 6 +-- Demos/Demos/RagdollDemo.cs | 4 +- Demos/Demos/RayCastingDemo.cs | 6 +-- Demos/Demos/RopeStabilityDemo.cs | 10 ++--- Demos/Demos/RopeTwistDemo.cs | 4 +- Demos/Demos/SimpleSelfContainedDemo.cs | 4 +- Demos/Demos/SolverContactEnumerationDemo.cs | 7 +-- Demos/Demos/Sponsors/SponsorDemo.cs | 16 +++---- Demos/Demos/Sponsors/SponsorNewt.cs | 2 +- Demos/Demos/SubsteppingDemo.cs | 12 ++--- Demos/Demos/SweepDemo.cs | 4 +- Demos/Demos/Tanks/Tank.cs | 4 +- Demos/Demos/Tanks/TankDemo.cs | 5 +-- .../BroadPhaseStressTestDemo.cs | 4 +- Demos/SpecializedTests/CapsuleTestDemo.cs | 2 +- Demos/SpecializedTests/CharacterTestDemo.cs | 7 ++- Demos/SpecializedTests/ClothLatticeDemo.cs | 4 +- .../CompoundCollisionIndicesTest.cs | 8 ++-- .../ConstrainedKinematicIntegrationTest.cs | 2 +- Demos/SpecializedTests/ConstraintTestDemo.cs | 4 +- Demos/SpecializedTests/ConvexHullTestDemo.cs | 20 ++++----- Demos/SpecializedTests/CylinderTestDemo.cs | 5 +-- Demos/SpecializedTests/GyroscopeTestDemo.cs | 4 +- Demos/SpecializedTests/Media/BedsheetDemo.cs | 14 +++--- .../Media/ColosseumVideoDemo.cs | 10 ++--- .../Media/NewtDemandingSacrificeVideoDemo.cs | 45 ++++++++++--------- Demos/SpecializedTests/Media/NewtVideoDemo.cs | 8 ++-- .../Media/PyramidVideoDemo.cs | 6 +-- .../Media/ShrinkwrappedNewtsVideoDemo.cs | 8 ++-- Demos/SpecializedTests/MeshMeshTestDemo.cs | 7 ++- .../SpecializedTests/MeshReductionTestDemo.cs | 15 +++---- .../MeshSerializationTestDemo.cs | 6 +-- Demos/SpecializedTests/MeshTestDemo.cs | 14 +++--- .../PyramidAwakenerTestDemo.cs | 8 ++-- Demos/SpecializedTests/RagdollTubeDemo.cs | 4 +- Demos/SpecializedTests/ShapePileTestDemo.cs | 4 +- Demos/SpecializedTests/SolverBatchTestDemo.cs | 6 +-- Demos/SpecializedTests/TriangleTestDemo.cs | 20 ++++----- Demos/SpecializedTests/VolumeQueryTests.cs | 4 +- 56 files changed, 252 insertions(+), 243 deletions(-) diff --git a/BepuPhysics/BodyDescription.cs b/BepuPhysics/BodyDescription.cs index f12973e6c..6a46d5d28 100644 --- a/BepuPhysics/BodyDescription.cs +++ b/BepuPhysics/BodyDescription.cs @@ -7,6 +7,9 @@ namespace BepuPhysics { + /// + /// Describes the thresholds for a body going to sleep. + /// public struct BodyActivityDescription { /// @@ -30,14 +33,42 @@ public BodyActivityDescription(float sleepThreshold, byte minimumTimestepCountUn SleepThreshold = sleepThreshold; MinimumTimestepCountUnderThreshold = minimumTimestepCountUnderThreshold; } + + /// + /// Creates a body activity description. Uses a of 32. + /// + /// Threshold of squared velocity under which the body is allowed to go to sleep. This is compared against dot(linearVelocity, linearVelocity) + dot(angularVelocity, angularVelocity). + /// Note that the body is not guaranteed to go to sleep immediately after meeting this minimum. + public static implicit operator BodyActivityDescription(float sleepThreshold) + { + return new BodyActivityDescription(sleepThreshold); + } } + /// + /// Describes a body's state. + /// public struct BodyDescription { + /// + /// Position and orientation of the body. + /// public RigidPose Pose; + /// + /// Linear and angular velocity of the body. + /// public BodyVelocity Velocity; + /// + /// Mass and inertia tensor of the body. + /// public BodyInertia LocalInertia; + /// + /// Shape and collision detection settings for the body. + /// public CollidableDescription Collidable; + /// + /// Sleeping settings for the body. + /// public BodyActivityDescription Activity; //Convex shape helpers. @@ -143,7 +174,7 @@ public static BodyDescription CreateConvexDynamic( Pose = pose, Velocity = velocity, Activity = GetDefaultActivity(shape), - Collidable = new CollidableDescription(shapes.Add(shape)) + Collidable = shapes.Add(shape) }; shape.ComputeInertia(mass, out description.LocalInertia); return description; @@ -266,7 +297,7 @@ public static BodyDescription CreateConvexKinematic( Pose = pose, Velocity = velocity, Activity = GetDefaultActivity(shape), - Collidable = new CollidableDescription(shapes.Add(shape)) + Collidable = shapes.Add(shape) }; return description; } diff --git a/BepuPhysics/Collidables/CollidableDescription.cs b/BepuPhysics/Collidables/CollidableDescription.cs index 3483a93e4..a0950be1e 100644 --- a/BepuPhysics/Collidables/CollidableDescription.cs +++ b/BepuPhysics/Collidables/CollidableDescription.cs @@ -42,5 +42,14 @@ public CollidableDescription(TypedIndex shape) : this(shape, ContinuousDetection public CollidableDescription(TypedIndex shape, float maximumSpeculativeMargin) : this(shape, ContinuousDetection.Discrete(0, maximumSpeculativeMargin)) { } + + /// + /// Constructs a new collidable description with . Will use a minimum speculative margin of 0 and a maximum of . + /// + /// Shape index to use for the collidable. + public static implicit operator CollidableDescription(TypedIndex shapeIndex) + { + return new CollidableDescription(shapeIndex); + } } } diff --git a/Demos/Demos/BlockChainDemo.cs b/Demos/Demos/BlockChainDemo.cs index 6c22c6d80..c82cf50d6 100644 --- a/Demos/Demos/BlockChainDemo.cs +++ b/Demos/Demos/BlockChainDemo.cs @@ -44,9 +44,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var bodyDescription = BodyDescription.CreateDynamic( new Vector3(0, 5 + blockIndex * (boxShape.Height + 1), (forkIndex - forkCount * 0.5f) * (boxShape.Length + 4)), //Make the uppermost block kinematic to hold up the rest of the chain. - blockIndex == blocksPerChain - 1 ? new BodyInertia() : boxInertia, - new CollidableDescription(boxIndex), - new BodyActivityDescription(.01f, 32)); + blockIndex == blocksPerChain - 1 ? new BodyInertia() : boxInertia, boxIndex, .01f); blockHandles[blockIndex] = Simulation.Bodies.Add(bodyDescription); } //Build the chains. @@ -62,12 +60,12 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) } } - Simulation.Statics.Add(new StaticDescription(new Vector3(1, -0.5f, 1), new CollidableDescription(Simulation.Shapes.Add(new Box(200, 1, 200))))); + Simulation.Statics.Add(new StaticDescription(new Vector3(1, -0.5f, 1), Simulation.Shapes.Add(new Box(200, 1, 200)))); //Build the coin description for the ponz-I mean ICO. var coinShape = new Cylinder(1.5f, 0.2f); coinShape.ComputeInertia(1, out var coinInertia); - coinDescription = BodyDescription.CreateDynamic(RigidPose.Identity, coinInertia, new CollidableDescription(Simulation.Shapes.Add(coinShape)), new BodyActivityDescription(0.01f)); + coinDescription = BodyDescription.CreateDynamic(RigidPose.Identity, coinInertia, Simulation.Shapes.Add(coinShape), 0.01f); } BodyDescription coinDescription; diff --git a/Demos/Demos/BouncinessDemo.cs b/Demos/Demos/BouncinessDemo.cs index ec70dac39..4e9b2d6cf 100644 --- a/Demos/Demos/BouncinessDemo.cs +++ b/Demos/Demos/BouncinessDemo.cs @@ -104,7 +104,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) } } - collidableMaterials.Allocate(Simulation.Statics.Add(new StaticDescription(new Vector3(0, -15f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(2500, 30, 2500)))))) = + collidableMaterials.Allocate(Simulation.Statics.Add(new StaticDescription(new Vector3(0, -15f, 0), Simulation.Shapes.Add(new Box(2500, 30, 2500))))) = new SimpleMaterial { FrictionCoefficient = 1, MaximumRecoveryVelocity = 2, SpringSettings = new SpringSettings(30, 1) }; } } diff --git a/Demos/Demos/Cars/CarDemo.cs b/Demos/Demos/Cars/CarDemo.cs index ebdeb60ed..c6872ce24 100644 --- a/Demos/Demos/Cars/CarDemo.cs +++ b/Demos/Demos/Cars/CarDemo.cs @@ -95,7 +95,7 @@ public override void Initialize(ContentArchive content, Camera camera) Simulation.Statics.Add(new StaticDescription( new Vector3(0, buildingShape.HalfHeight, 0) + landmarkMin + landmarkSpan * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, random.NextSingle() * MathF.PI), - new CollidableDescription(Simulation.Shapes.Add(buildingShape)))); + Simulation.Shapes.Add(buildingShape))); } } @@ -112,7 +112,7 @@ public override void Initialize(ContentArchive content, Camera camera) new SpringSettings(5, 0.7f), QuaternionEx.CreateFromAxisAngle(Vector3.UnitZ, MathF.PI * 0.5f)), forwardSpeed: 50, forwardForce: 5, zoomMultiplier: 2, backwardSpeed: 10, backwardForce: 4, idleForce: 0.25f, brakeForce: 7, steeringSpeed: 1.5f, maximumSteeringAngle: MathF.PI * 0.23f, wheelBaseLength: wheelBaseLength, wheelBaseWidth: wheelBaseWidth, ackermanSteering: 1); - + aiControllers[i].LaneOffset = random.NextSingle() * 20 - 10; } @@ -135,7 +135,7 @@ public override void Initialize(ContentArchive content, Camera camera) }, new Vector3(1, 1, 1), BufferPool, out var planeMesh); Simulation.Statics.Add(new StaticDescription(new Vector3(0, -15, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), - new CollidableDescription(Simulation.Shapes.Add(planeMesh)))); + Simulation.Shapes.Add(planeMesh))); } diff --git a/Demos/Demos/Cars/SimpleCar.cs b/Demos/Demos/Cars/SimpleCar.cs index ab838e4bb..f2eafad2b 100644 --- a/Demos/Demos/Cars/SimpleCar.cs +++ b/Demos/Demos/Cars/SimpleCar.cs @@ -43,7 +43,7 @@ public static WheelHandles CreateWheel(Simulation simulation, CollidableProperty RigidPose.Transform(bodyToWheelSuspension + suspensionDirection * suspensionLength, bodyPose, out wheelPose.Position); QuaternionEx.ConcatenateWithoutOverlap(localWheelOrientation, bodyPose.Orientation, out wheelPose.Orientation); WheelHandles handles; - handles.Wheel = simulation.Bodies.Add(BodyDescription.CreateDynamic(wheelPose, wheelInertia, new CollidableDescription(wheelShape), new BodyActivityDescription(0.01f))); + handles.Wheel = simulation.Bodies.Add(BodyDescription.CreateDynamic(wheelPose, wheelInertia, new CollidableDescription(wheelShape, 0.5f), new BodyActivityDescription(0.01f))); handles.SuspensionSpring = simulation.Solver.Add(bodyHandle, handles.Wheel, new LinearAxisServo { @@ -85,7 +85,7 @@ public static SimpleCar Create(Simulation simulation, CollidableProperty { return new Vector3(x * 5 - 50, 3 * MathF.Sin(x) * MathF.Sin(y), y * 5 - 50); }, Vector3.One, BufferPool, out var mesh); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), new CollidableDescription(Simulation.Shapes.Add(mesh)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), Simulation.Shapes.Add(mesh))); } /// diff --git a/Demos/Demos/ColosseumDemo.cs b/Demos/Demos/ColosseumDemo.cs index d5fa790d6..ac480209d 100644 --- a/Demos/Demos/ColosseumDemo.cs +++ b/Demos/Demos/ColosseumDemo.cs @@ -76,9 +76,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var ringBoxShape = new Box(0.5f, 1, 3); ringBoxShape.ComputeInertia(1, out var ringBoxInertia); - var boxDescription = BodyDescription.CreateDynamic(new Vector3(), ringBoxInertia, - new CollidableDescription(Simulation.Shapes.Add(ringBoxShape)), - new BodyActivityDescription(0.01f)); + var boxDescription = BodyDescription.CreateDynamic(new Vector3(), ringBoxInertia, Simulation.Shapes.Add(ringBoxShape), 0.01f); var layerPosition = new Vector3(); const int layerCount = 6; @@ -97,15 +95,15 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) } //Console.WriteLine($"box count: {Simulation.Bodies.ActiveSet.Count}"); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(500, 1, 500))))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), Simulation.Shapes.Add(new Box(500, 1, 500)))); var bulletShape = new Sphere(0.5f); bulletShape.ComputeInertia(.1f, out var bulletInertia); - bulletDescription = BodyDescription.CreateDynamic(new Vector3(), bulletInertia, new CollidableDescription(Simulation.Shapes.Add(bulletShape)), new BodyActivityDescription(0.01f)); + bulletDescription = BodyDescription.CreateDynamic(new Vector3(), bulletInertia, Simulation.Shapes.Add(bulletShape), 0.01f); var shootiePatootieShape = new Sphere(3f); shootiePatootieShape.ComputeInertia(100, out var shootiePatootieInertia); - shootiePatootieDescription = BodyDescription.CreateDynamic(new Vector3(), shootiePatootieInertia, new CollidableDescription(Simulation.Shapes.Add(shootiePatootieShape)), new BodyActivityDescription(0.01f)); + shootiePatootieDescription = BodyDescription.CreateDynamic(new Vector3(), shootiePatootieInertia, Simulation.Shapes.Add(shootiePatootieShape), 0.01f); } BodyDescription bulletDescription; diff --git a/Demos/Demos/CompoundTestDemo.cs b/Demos/Demos/CompoundTestDemo.cs index 1a990b3e0..2f9ba0e20 100644 --- a/Demos/Demos/CompoundTestDemo.cs +++ b/Demos/Demos/CompoundTestDemo.cs @@ -38,7 +38,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) compoundBuilder.Add(boxChildShape, boxLocalPose, 1); compoundBuilder.BuildDynamicCompound(out var compoundChildren, out var compoundInertia, out var compoundCenter); compoundBuilder.Reset(); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(compoundCenter, compoundInertia, new CollidableDescription(Simulation.Shapes.Add(new Compound(compoundChildren))), new BodyActivityDescription(0.01f))); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(compoundCenter, compoundInertia, Simulation.Shapes.Add(new Compound(compoundChildren)), 0.01f)); } //Build a stack of sphere grids to stress manifold reduction heuristics in a convex-ish situation. @@ -59,7 +59,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) compoundBuilder.BuildDynamicCompound(out var gridChildren, out var gridInertia, out var center); compoundBuilder.Reset(); var gridCompound = new Compound(gridChildren); - var bodyDescription = BodyDescription.CreateDynamic(RigidPose.Identity, gridInertia, new CollidableDescription(Simulation.Shapes.Add(gridCompound)), new BodyActivityDescription(0.01f)); + var bodyDescription = BodyDescription.CreateDynamic(RigidPose.Identity, gridInertia, Simulation.Shapes.Add(gridCompound), 0.01f); for (int i = 0; i < 4; ++i) { bodyDescription.Pose.Position = new Vector3(0, 2 + i * 3, 0); @@ -87,7 +87,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) compoundBuilder.BuildDynamicCompound(out var tableChildren, out var tableInertia, out var tableCenter); compoundBuilder.Reset(); var table = new Compound(tableChildren); - var tableDescription = BodyDescription.CreateDynamic(RigidPose.Identity, tableInertia, new CollidableDescription(Simulation.Shapes.Add(table)), new BodyActivityDescription(0.01f)); + var tableDescription = BodyDescription.CreateDynamic(RigidPose.Identity, tableInertia, Simulation.Shapes.Add(table), 0.01f); //Stack some tables. { @@ -123,11 +123,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var sphereShape = new Sphere(3); var sphereIndex = Simulation.Shapes.Add(sphereShape); - var sphereDescription = new StaticDescription - { - Collidable = new CollidableDescription(sphereIndex), - Pose = new RigidPose { Position = new Vector3(10, 2, 0), Orientation = Quaternion.Identity } - }; + var sphereDescription = new StaticDescription(new Vector3(10, 2, 0), sphereIndex); Simulation.Statics.Add(sphereDescription); } @@ -156,7 +152,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) compoundBuilder.Reset(); var clamp = new Compound(clampChildren); var clampDescription = BodyDescription.CreateDynamic( - new RigidPose(tableDescription.Pose.Position + new Vector3(2f, 0.3f, 0)), clampInertia, new CollidableDescription(Simulation.Shapes.Add(clamp)), new BodyActivityDescription(0.01f)); + new RigidPose(tableDescription.Pose.Position + new Vector3(2f, 0.3f, 0)), clampInertia, Simulation.Shapes.Add(clamp), 0.01f); Simulation.Bodies.Add(clampDescription); } @@ -192,7 +188,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var compoundIndex = Simulation.Shapes.Add(compound); for (int i = 0; i < 8; ++i) { - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 4 + 5 * i, 32), inertia, new CollidableDescription(compoundIndex), new BodyActivityDescription(0.01f))); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 4 + 5 * i, 32), inertia, compoundIndex, 0.01f)); } } } @@ -201,11 +197,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) { var boxShape = new Box(256, 1, 256); var groundShapeIndex = Simulation.Shapes.Add(boxShape); - var groundDescription = new StaticDescription - { - Collidable = new CollidableDescription(groundShapeIndex), - Pose = new RigidPose { Position = new Vector3(0, 0, 0), Orientation = Quaternion.Identity } - }; + var groundDescription = new StaticDescription(default, groundShapeIndex); Simulation.Statics.Add(groundDescription); } const int planeWidth = 48; @@ -216,8 +208,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Vector2 offsetFromCenter = new Vector2(x - planeWidth / 2, y - planeHeight / 2); return new Vector3(offsetFromCenter.X, MathF.Cos(x / 4f) * MathF.Sin(y / 4f) - 0.01f * offsetFromCenter.LengthSquared(), offsetFromCenter.Y); }, new Vector3(2, 1, 2), BufferPool, out var planeMesh); - Simulation.Statics.Add(new StaticDescription(new Vector3(64, 4, 32), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), - new CollidableDescription(Simulation.Shapes.Add(planeMesh)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(64, 4, 32), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), Simulation.Shapes.Add(planeMesh))); } } } diff --git a/Demos/Demos/ContactEventsDemo.cs b/Demos/Demos/ContactEventsDemo.cs index 25443cd06..66f6d2931 100644 --- a/Demos/Demos/ContactEventsDemo.cs +++ b/Demos/Demos/ContactEventsDemo.cs @@ -715,8 +715,8 @@ public override void Initialize(ContentArchive content, Camera camera) events.Register(Simulation.Bodies[listenedBody2].CollidableReference, eventHandler); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(30, 1, 30))))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, 3, 15), new CollidableDescription(Simulation.Shapes.Add(new Box(30, 5, 1))))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), Simulation.Shapes.Add(new Box(30, 1, 30)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 3, 15), Simulation.Shapes.Add(new Box(30, 5, 1)))); } public override void Update(Window window, Camera camera, Input input, float dt) diff --git a/Demos/Demos/ContinuousCollisionDetectionDemo.cs b/Demos/Demos/ContinuousCollisionDetectionDemo.cs index 04218b75e..67eb50207 100644 --- a/Demos/Demos/ContinuousCollisionDetectionDemo.cs +++ b/Demos/Demos/ContinuousCollisionDetectionDemo.cs @@ -18,7 +18,7 @@ public class ContinuousCollisionDetectionDemo : Demo ConstraintHandle BuildSpinner(Vector3 initialPosition, float rotationSpeed) { - var spinnerBase = Simulation.Bodies.Add(BodyDescription.CreateDynamic(initialPosition, new BodyInertia { InverseMass = 1e-2f }, new CollidableDescription(Simulation.Shapes.Add(new Box(2, 2, 2))), new BodyActivityDescription(0.01f))); + var spinnerBase = Simulation.Bodies.Add(BodyDescription.CreateDynamic(initialPosition, new BodyInertia { InverseMass = 1e-2f }, Simulation.Shapes.Add(new Box(2, 2, 2)), 0.01f)); var bladeShape = new Box(5, 0.01f, 1); bladeShape.ComputeInertia(1, out var bladeInertia); var shapeIndex = Simulation.Shapes.Add(bladeShape); @@ -38,7 +38,7 @@ ConstraintHandle BuildSpinner(Vector3 initialPosition, float rotationSpeed) //Using a restricted speculative margin by setting the maximumSpeculativeMargin to 0.2 means that collision detection won't accept distant contacts. //This pretty much eliminates ghost collisions, while the continuous sweep helps avoid missed collisions. - var spinnerBlade = Simulation.Bodies.Add(BodyDescription.CreateDynamic(initialPosition, bladeInertia, new CollidableDescription(shapeIndex, ContinuousDetection.Continuous(1e-4f, 1e-4f, maximumSpeculativeMargin: 0.2f)), new BodyActivityDescription(0.01f))); + var spinnerBlade = Simulation.Bodies.Add(BodyDescription.CreateDynamic(initialPosition, bladeInertia, new CollidableDescription(shapeIndex, ContinuousDetection.Continuous(1e-4f, 1e-4f, maximumSpeculativeMargin: 0.2f)), 0.01f)); Simulation.Solver.Add(spinnerBase, spinnerBlade, new Hinge { LocalHingeAxisA = new Vector3(0, 0, 1), LocalHingeAxisB = new Vector3(0, 0, 1), LocalOffsetB = new Vector3(0, 0, -3), SpringSettings = new SpringSettings(30, 1) }); Simulation.Solver.Add(spinnerBase, spinnerBlade, new AngularAxisMotor { LocalAxisA = new Vector3(0, 0, 1), Settings = new MotorSettings(10, 1e-4f), TargetVelocity = rotationSpeed }); return Simulation.Solver.Add(spinnerBase, new OneBodyLinearServo { ServoSettings = ServoSettings.Default, SpringSettings = new SpringSettings(30, 1) }); @@ -89,7 +89,7 @@ public override void Initialize(ContentArchive content, Camera camera) spinnerMotorB = BuildSpinner(new Vector3(5, 10, -5), 59); rolloverInfo.Add(new Vector3(0, 12, -5), "High angular velocity continuous detection"); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(300, 10, 300))))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5f, 0), Simulation.Shapes.Add(new Box(300, 10, 300)))); } double time; diff --git a/Demos/Demos/CustomVoxelCollidableDemo.cs b/Demos/Demos/CustomVoxelCollidableDemo.cs index 51b61618c..f94d02427 100644 --- a/Demos/Demos/CustomVoxelCollidableDemo.cs +++ b/Demos/Demos/CustomVoxelCollidableDemo.cs @@ -429,19 +429,19 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) } } voxels = new Voxels(voxelIndices, new Vector3(1, 1, 1), BufferPool); - handle = Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), new CollidableDescription(Simulation.Shapes.Add(voxels)))); + handle = Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), Simulation.Shapes.Add(voxels))); var random = new Random(5); var shapeToDrop = new Box(1, 1, 1); shapeToDrop.ComputeInertia(1, out var shapeToDropInertia); - var descriptionToDrop = BodyDescription.CreateDynamic(new Vector3(), shapeToDropInertia, new CollidableDescription(Simulation.Shapes.Add(shapeToDrop)), new BodyActivityDescription(0.01f)); + var descriptionToDrop = BodyDescription.CreateDynamic(new Vector3(), shapeToDropInertia, Simulation.Shapes.Add(shapeToDrop), 0.01f); for (int i = 0; i < 4096; ++i) { descriptionToDrop.Pose.Position = new Vector3(15 + 10 * random.NextSingle(), 45 + 150 * random.NextSingle(), 15 + 10 * random.NextSingle()); Simulation.Bodies.Add(descriptionToDrop); } - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(300, 1, 300))))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), Simulation.Shapes.Add(new Box(300, 1, 300)))); } public override unsafe void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) diff --git a/Demos/Demos/FountainStressTestDemo.cs b/Demos/Demos/FountainStressTestDemo.cs index 13f2e9c41..41235c98a 100644 --- a/Demos/Demos/FountainStressTestDemo.cs +++ b/Demos/Demos/FountainStressTestDemo.cs @@ -57,7 +57,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) -staticGridWidthInInstances * staticSpacing * 0.5f + i * staticSpacing, -4 + 4 * (float)Math.Cos(i * 0.3) + 4 * (float)Math.Cos(j * 0.3), -staticGridWidthInInstances * staticSpacing * 0.5f + j * staticSpacing), - new CollidableDescription(staticShapeIndex)); + staticShapeIndex); Simulation.Statics.Add(staticDescription); } } @@ -76,7 +76,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) startingRadius * (float)Math.Cos(angle), 0, startingRadius * (float)Math.Sin(angle)), - new CollidableDescription(kinematicShapeIndex), + kinematicShapeIndex, new BodyActivityDescription(0, 4)); kinematicHandles[i] = Simulation.Bodies.Add(description); } @@ -262,8 +262,8 @@ public void CreateBodyDescription(Random random, in RigidPose pose, in BodyVeloc { Pose = pose, LocalInertia = inertia, - Collidable = new CollidableDescription(shapeIndex), - Activity = new BodyActivityDescription(0.1f), + Collidable = shapeIndex, + Activity = 0.1f, Velocity = velocity }; switch (random.Next(3)) diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index 9f5598e00..8075f12f2 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -667,8 +667,7 @@ internal unsafe static void CreateDeformable(Simulation simulation, in Vector3 p vertexHandles[i] = simulation.Bodies.Add(BodyDescription.CreateDynamic(new RigidPose( position + QuaternionEx.Transform(vertices[i], orientation), orientation), vertexInertia, //Bodies don't have to have collidables. Take advantage of this for all the internal vertices. - new CollidableDescription(vertexEdgeCounts[i] == edgeCountForInternalVertex ? new TypedIndex() : vertexShapeIndex), - new BodyActivityDescription(-0.01f))); + vertexEdgeCounts[i] == edgeCountForInternalVertex ? new TypedIndex() : vertexShapeIndex, -0.01f)); ref var vertexSpatialIndex = ref vertexSpatialIndices[i]; filters.Allocate(vertexHandles[i]) = new DeformableCollisionFilter(vertexSpatialIndex.X, vertexSpatialIndex.Y, vertexSpatialIndex.Z, instanceId); } @@ -861,8 +860,8 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 100, -.5f), 10, Simulation.Shapes, new Sphere(5))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(1500, 1, 1500))))); - //Simulation.Statics.Add(new StaticDescription(new Vector3(0, -1.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Sphere(3)), 0.1f))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), Simulation.Shapes.Add(new Box(1500, 1, 1500)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -1.5f, 0), Simulation.Shapes.Add(new Sphere(3)))); } diff --git a/Demos/Demos/PlanetDemo.cs b/Demos/Demos/PlanetDemo.cs index fd29c3417..21e24d2e0 100644 --- a/Demos/Demos/PlanetDemo.cs +++ b/Demos/Demos/PlanetDemo.cs @@ -71,11 +71,11 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new PlanetaryGravityCallbacks() { PlanetCenter = new Vector3(), Gravity = 100000 }, new PositionFirstTimestepper()); - Simulation.Statics.Add(new StaticDescription(new Vector3(), new CollidableDescription(Simulation.Shapes.Add(new Sphere(50))))); + Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(new Sphere(50)))); var orbiter = new Sphere(1f); orbiter.ComputeInertia(1, out var inertia); - var collidable = new CollidableDescription(Simulation.Shapes.Add(orbiter)); + var orbiterShapeIndex = Simulation.Shapes.Add(orbiter); var spacing = new Vector3(5); const int length = 20; for (int i = 0; i < length; ++i) @@ -87,7 +87,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) for (int k = 0; k < width; ++k) { Simulation.Bodies.Add(BodyDescription.CreateDynamic( - origin + new Vector3(i, j, k) * spacing, new BodyVelocity(new Vector3(30, 0, 0)), inertia, collidable, new BodyActivityDescription(0.01f))); + origin + new Vector3(i, j, k) * spacing, new BodyVelocity(new Vector3(30, 0, 0)), inertia, orbiterShapeIndex, 0.01f)); } } } diff --git a/Demos/Demos/PyramidDemo.cs b/Demos/Demos/PyramidDemo.cs index 9a18589b4..b10989c48 100644 --- a/Demos/Demos/PyramidDemo.cs +++ b/Demos/Demos/PyramidDemo.cs @@ -44,14 +44,12 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) (-columnCount * 0.5f + columnIndex) * boxShape.Width, (rowIndex + 0.5f) * boxShape.Height, (pyramidIndex - pyramidCount * 0.5f) * (boxShape.Length + 4)), - boxInertia, - new CollidableDescription(boxIndex), - new BodyActivityDescription(0.01f))); + boxInertia, boxIndex, 0.01f)); } } } - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(2500, 1, 2500))))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), Simulation.Shapes.Add(new Box(2500, 1, 2500)))); } //We'll randomize the size of bullets. diff --git a/Demos/Demos/RagdollDemo.cs b/Demos/Demos/RagdollDemo.cs index 65dc5caa5..2b0acf10e 100644 --- a/Demos/Demos/RagdollDemo.cs +++ b/Demos/Demos/RagdollDemo.cs @@ -146,7 +146,7 @@ static BodyHandle AddBody(TShape shape, float mass, in RigidPose pose, S //Also, the cost of registering different shapes isn't that high for tiny implicit shapes. var shapeIndex = simulation.Shapes.Add(shape); shape.ComputeInertia(mass, out var inertia); - var description = BodyDescription.CreateDynamic(pose, inertia, new CollidableDescription(shapeIndex), new BodyActivityDescription(-0.01f)); + var description = BodyDescription.CreateDynamic(pose, inertia, shapeIndex, 0.01f); return simulation.Bodies.Add(description); } @@ -556,7 +556,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) } } - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(300, 1, 300))))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), Simulation.Shapes.Add(new Box(300, 1, 300)))); } } diff --git a/Demos/Demos/RayCastingDemo.cs b/Demos/Demos/RayCastingDemo.cs index f007ef591..591cd9d32 100644 --- a/Demos/Demos/RayCastingDemo.cs +++ b/Demos/Demos/RayCastingDemo.cs @@ -120,11 +120,11 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) }; if ((i + j + k) % 2 == 1) { - Simulation.Bodies.Add(BodyDescription.CreateKinematic(new RigidPose(location, orientation), new CollidableDescription(shapeIndex), new BodyActivityDescription(-0.1f))); + Simulation.Bodies.Add(BodyDescription.CreateKinematic(new RigidPose(location, orientation), shapeIndex, -0.1f)); } else { - Simulation.Statics.Add(new StaticDescription(location, orientation, new CollidableDescription(shapeIndex))); + Simulation.Statics.Add(new StaticDescription(location, orientation, shapeIndex)); } } } @@ -139,7 +139,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) }, new Vector3(1, 3, 1), BufferPool, out var planeMesh); Simulation.Statics.Add(new StaticDescription( new Vector3(0, -10, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 4), - new CollidableDescription(Simulation.Shapes.Add(planeMesh)))); + Simulation.Shapes.Add(planeMesh))); int raySourceCount = 3; raySources = new QuickList>(raySourceCount, BufferPool); diff --git a/Demos/Demos/RopeStabilityDemo.cs b/Demos/Demos/RopeStabilityDemo.cs index 65b3741fc..ff946c2e3 100644 --- a/Demos/Demos/RopeStabilityDemo.cs +++ b/Demos/Demos/RopeStabilityDemo.cs @@ -30,8 +30,8 @@ public static BodyHandle[] BuildRopeBodies(Simulation simulation, in Vector3 sta var bodyDescription = new BodyDescription { //Make the uppermost block kinematic to hold up the rest of the chain. - Activity = new BodyActivityDescription(.01f), - Collidable = new CollidableDescription(ropeShapeIndex), + Activity = .01f, + Collidable = ropeShapeIndex, }; for (int linkIndex = 0; linkIndex < bodyCount + 1; ++linkIndex) { @@ -58,7 +58,7 @@ static BodyHandle CreateWreckingBall(Simulation simulation, BodyHandle[] bodyHan { var lastBodyReference = new BodyReference(bodyHandles[bodyHandles.Length - 1], simulation.Bodies); var wreckingBallPosition = lastBodyReference.Pose.Position - new Vector3(0, ropeBodyRadius + bodySpacing + wreckingBallRadius, 0); - var description = BodyDescription.CreateDynamic(wreckingBallPosition, wreckingBallInertia, new CollidableDescription(wreckingBallShapeIndex), new BodyActivityDescription(0.01f)); + var description = BodyDescription.CreateDynamic(wreckingBallPosition, wreckingBallInertia, wreckingBallShapeIndex, 0.01f); //Give it a little bump. description.Velocity = new BodyVelocity(new Vector3(-10, 0, 0), default); var wreckingBallBodyHandle = simulation.Bodies.Add(description); @@ -256,10 +256,10 @@ bool TryCreateConstraint(int handleIndexA, int handleIndexB) rolloverInfo.Add(startLocation, $"100:1 mass ratio, {constraintsPerBody - 1}x extra skip constraints"); } - Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(200, 1, 200))))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), Simulation.Shapes.Add(new Box(200, 1, 200)))); Simulation.Statics.Add(new StaticDescription( new Vector3(100, 70, 0), BepuUtilities.QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), MathF.PI * 0.5f), - new CollidableDescription(Simulation.Shapes.Add(new Capsule(8, 64))))); + Simulation.Shapes.Add(new Capsule(8, 64)))); } diff --git a/Demos/Demos/RopeTwistDemo.cs b/Demos/Demos/RopeTwistDemo.cs index ca647c91c..d64f7c364 100644 --- a/Demos/Demos/RopeTwistDemo.cs +++ b/Demos/Demos/RopeTwistDemo.cs @@ -102,7 +102,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) const float ropeBodyRadius = 0.1f; const int ropeBodyCount = 130; var wreckingBallPosition = startLocation - new Vector3(0, ropeBodyRadius + (ropeBodyRadius * 2 + ropeBodySpacing) * ropeBodyCount + bigWreckingBall.Radius, 0); - var description = BodyDescription.CreateDynamic(wreckingBallPosition, bigWreckingBallInertia, new CollidableDescription(bigWreckingBallIndex), new BodyActivityDescription(-0.01f)); + var description = BodyDescription.CreateDynamic(wreckingBallPosition, bigWreckingBallInertia, bigWreckingBallIndex, 0.01f); var wreckingBallBodyHandle = Simulation.Bodies.Add(description); var wreckingBallBody = Simulation.Bodies[wreckingBallBodyHandle]; wreckingBallBody.Velocity.Angular = new Vector3(0, 20, 0); @@ -161,7 +161,7 @@ bool TryCreateConstraint(int handleIndexA, int handleIndexB) } - Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(200, 1, 200))))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), Simulation.Shapes.Add(new Box(200, 1, 200)))); } } diff --git a/Demos/Demos/SimpleSelfContainedDemo.cs b/Demos/Demos/SimpleSelfContainedDemo.cs index b32bb6134..520bebe4b 100644 --- a/Demos/Demos/SimpleSelfContainedDemo.cs +++ b/Demos/Demos/SimpleSelfContainedDemo.cs @@ -211,9 +211,9 @@ public static void Run() //Drop a ball on a big static box. var sphere = new Sphere(1); sphere.ComputeInertia(1, out var sphereInertia); - simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 5, 0), sphereInertia, new CollidableDescription(simulation.Shapes.Add(sphere)), new BodyActivityDescription(0.01f))); + simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 5, 0), sphereInertia, simulation.Shapes.Add(sphere), 0.01f)); - simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), new CollidableDescription(simulation.Shapes.Add(new Box(500, 1, 500))))); + simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), simulation.Shapes.Add(new Box(500, 1, 500)))); var threadDispatcher = new SimpleThreadDispatcher(Environment.ProcessorCount); diff --git a/Demos/Demos/SolverContactEnumerationDemo.cs b/Demos/Demos/SolverContactEnumerationDemo.cs index d1e834926..0678d7bf9 100644 --- a/Demos/Demos/SolverContactEnumerationDemo.cs +++ b/Demos/Demos/SolverContactEnumerationDemo.cs @@ -45,9 +45,7 @@ public override void Initialize(ContentArchive content, Camera camera) Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3( (-columnCount * 0.5f + columnIndex) * boxShape.Width, (rowIndex + 0.5f) * boxShape.Height + 10, 0), - boxInertia, - new CollidableDescription(boxIndex), - new BodyActivityDescription(0.01f))); + boxInertia, boxIndex, 0.01f)); } } @@ -62,8 +60,7 @@ public override void Initialize(ContentArchive content, Camera camera) { return new Vector3(x - planeWidth / 2, 1 * MathF.Cos(x / 2f) * MathF.Sin(y / 2f), y - planeHeight / 2); }, new Vector3(2, 1, 2), BufferPool, out var planeMesh); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -2, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), - new CollidableDescription(Simulation.Shapes.Add(planeMesh)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -2, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), Simulation.Shapes.Add(planeMesh))); } struct Contact diff --git a/Demos/Demos/Sponsors/SponsorDemo.cs b/Demos/Demos/Sponsors/SponsorDemo.cs index 380af8903..807cfe14b 100644 --- a/Demos/Demos/Sponsors/SponsorDemo.cs +++ b/Demos/Demos/Sponsors/SponsorDemo.cs @@ -127,15 +127,15 @@ public override void Initialize(ContentArchive content, Camera camera) const float floorSize = 240; const float wallThickness = 200; - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -10f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(floorSize, 20, floorSize))))); - Simulation.Statics.Add(new StaticDescription(new Vector3(floorSize * -0.5f - wallThickness * 0.5f, -5, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(wallThickness, 30, floorSize + wallThickness * 2))))); - Simulation.Statics.Add(new StaticDescription(new Vector3(floorSize * 0.5f + wallThickness * 0.5f, -5, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(wallThickness, 30, floorSize + wallThickness * 2))))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5, floorSize * -0.5f - wallThickness * 0.5f), new CollidableDescription(Simulation.Shapes.Add(new Box(floorSize, 30, wallThickness))))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5, floorSize * 0.5f + wallThickness * 0.5f), new CollidableDescription(Simulation.Shapes.Add(new Box(floorSize, 30, wallThickness))))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -10f, 0), Simulation.Shapes.Add(new Box(floorSize, 20, floorSize)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(floorSize * -0.5f - wallThickness * 0.5f, -5, 0), Simulation.Shapes.Add(new Box(wallThickness, 30, floorSize + wallThickness * 2)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(floorSize * 0.5f + wallThickness * 0.5f, -5, 0), Simulation.Shapes.Add(new Box(wallThickness, 30, floorSize + wallThickness * 2)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5, floorSize * -0.5f - wallThickness * 0.5f), Simulation.Shapes.Add(new Box(floorSize, 30, wallThickness)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5, floorSize * 0.5f + wallThickness * 0.5f), Simulation.Shapes.Add(new Box(floorSize, 30, wallThickness)))); const int characterCount = 1000; characterAIs = new QuickList(characterCount, BufferPool); - var characterCollidable = new CollidableDescription(Simulation.Shapes.Add(new Capsule(0.5f, 1f))); + var characterCollidable = Simulation.Shapes.Add(new Capsule(0.5f, 1f)); for (int i = 0; i < characterCount; ++i) { var position2D = newtArenaMin + (newtArenaMax - newtArenaMin) * new Vector2(random.NextSingle(), random.NextSingle()); @@ -146,7 +146,7 @@ public override void Initialize(ContentArchive content, Camera camera) const int hutCount = 30; var hutBoxShape = new Box(0.4f, 2, 3); hutBoxShape.ComputeInertia(20, out var obstacleInertia); - var obstacleDescription = BodyDescription.CreateDynamic(new Vector3(), obstacleInertia, new CollidableDescription(Simulation.Shapes.Add(hutBoxShape)), new BodyActivityDescription(1e-2f)); + var obstacleDescription = BodyDescription.CreateDynamic(new Vector3(), obstacleInertia, Simulation.Shapes.Add(hutBoxShape), 1e-2f); for (int i = 0; i < hutCount; ++i) { @@ -157,7 +157,7 @@ public override void Initialize(ContentArchive content, Camera camera) var overlordNewtShape = newtMesh; overlordNewtShape.Scale = new Vector3(60, 60, 60); - overlordNewtHandle = Simulation.Statics.Add(new StaticDescription(new Vector3(0, 10, -floorSize * 0.5f - 70), new CollidableDescription(Simulation.Shapes.Add(overlordNewtShape)))); + overlordNewtHandle = Simulation.Statics.Add(new StaticDescription(new Vector3(0, 10, -floorSize * 0.5f - 70), Simulation.Shapes.Add(overlordNewtShape))); } diff --git a/Demos/Demos/Sponsors/SponsorNewt.cs b/Demos/Demos/Sponsors/SponsorNewt.cs index 59acc0f96..7cc363231 100644 --- a/Demos/Demos/Sponsors/SponsorNewt.cs +++ b/Demos/Demos/Sponsors/SponsorNewt.cs @@ -20,7 +20,7 @@ public SponsorNewt(Simulation simulation, TypedIndex shape, float height, in Vec var arenaSpan = arenaMax - arenaMin; var position = arenaMin + arenaSpan * new Vector2(random.NextSingle(), random.NextSingle()); var angle = MathF.PI * 2 * random.NextSingle(); - BodyHandle = simulation.Bodies.Add(BodyDescription.CreateKinematic(new RigidPose(new Vector3(position.X, height, position.Y), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, angle)), new CollidableDescription(shape), new BodyActivityDescription(-1))); + BodyHandle = simulation.Bodies.Add(BodyDescription.CreateKinematic(new RigidPose(new Vector3(position.X, height, position.Y), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, angle)), shape, -1)); SponsorIndex = sponsorIndex; } diff --git a/Demos/Demos/SubsteppingDemo.cs b/Demos/Demos/SubsteppingDemo.cs index f00f7bafd..dd2e41962 100644 --- a/Demos/Demos/SubsteppingDemo.cs +++ b/Demos/Demos/SubsteppingDemo.cs @@ -51,7 +51,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var boxShape = new Box(4, 0.5f, 6f); boxShape.ComputeInertia(1, out var boxInertia); //Note that sleeping is disabled with a negative velocity threshold. We want to watch the stack as we change simulation settings; if it's inactive, it won't respond! - var boxDescription = BodyDescription.CreateDynamic(new Vector3(), boxInertia, new CollidableDescription(Simulation.Shapes.Add(boxShape)), new BodyActivityDescription(-1f)); + var boxDescription = BodyDescription.CreateDynamic(new Vector3(), boxInertia, Simulation.Shapes.Add(boxShape), -1f); for (int i = 0; i < 20; ++i) { boxDescription.Pose = new RigidPose(new Vector3(0, 0.5f + boxShape.Height * (i + 0.5f), 0), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, MathF.PI * 0.05f * i)); @@ -61,7 +61,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) topBlockShape.ComputeInertia(200, out var topBlockInertia); Simulation.Bodies.Add( BodyDescription.CreateDynamic(boxDescription.Pose.Position + new Vector3(0, boxShape.HalfHeight + 1f, 0), topBlockInertia, - new CollidableDescription(Simulation.Shapes.Add(topBlockShape)), new BodyActivityDescription(-1f))); + Simulation.Shapes.Add(topBlockShape), -1f)); rolloverInfo.Add(boxDescription.Pose.Position + new Vector3(0, 4, 0), "200:1 mass ratio"); } @@ -71,15 +71,15 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //(Fortunately, all 5 degrees of freedom of each hinge constraint are solved analytically, so the convergence issues aren't quite as bad as they could be.) var basePosition = new Vector3(-20, 20, 0); var boxShape = new Box(0.5f, 0.5f, 3f); - var boxCollidable = new CollidableDescription(Simulation.Shapes.Add(boxShape)); + var boxShapeIndex = Simulation.Shapes.Add(boxShape); boxShape.ComputeInertia(1, out var boxInertia); - var linkDescription = BodyDescription.CreateDynamic(new Vector3(), boxInertia, boxCollidable, new BodyActivityDescription(0.01f)); + var linkDescription = BodyDescription.CreateDynamic(new Vector3(), boxInertia, boxShapeIndex, 0.01f); for (int chainIndex = 0; chainIndex < 4; ++chainIndex) { linkDescription.Pose.Position = basePosition + new Vector3(0, 0, chainIndex * 15); - var previousLinkHandle = Simulation.Bodies.Add(BodyDescription.CreateKinematic(linkDescription.Pose.Position, boxCollidable, new BodyActivityDescription(0.01f))); + var previousLinkHandle = Simulation.Bodies.Add(BodyDescription.CreateKinematic(linkDescription.Pose.Position, boxShapeIndex, 0.01f)); for (int linkIndex = 0; linkIndex < 8; ++linkIndex) { var previousPosition = linkDescription.Pose.Position; @@ -108,7 +108,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) } - Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(200, 1, 200))))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), Simulation.Shapes.Add(new Box(200, 1, 200)))); } public override void Update(Window window, Camera camera, Input input, float dt) diff --git a/Demos/Demos/SweepDemo.cs b/Demos/Demos/SweepDemo.cs index 7176fe79b..fa0ff9c83 100644 --- a/Demos/Demos/SweepDemo.cs +++ b/Demos/Demos/SweepDemo.cs @@ -53,7 +53,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) { var location = new Vector3(5, 5, 5) * new Vector3(i, j, k) + new Vector3(-width * 2.5f, 2.5f, -length * 2.5f); //CreateKinematic is just a helper function that sets the inertia to all zeroes. We'll set the inertia to the actual value in the following switch. - var bodyDescription = BodyDescription.CreateKinematic(location, new CollidableDescription(default), new BodyActivityDescription(0.1f)); + var bodyDescription = BodyDescription.CreateKinematic(location, default, 0.1f); switch (j % 3) { case 0: @@ -106,7 +106,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) { return new Vector3(x, 1 * MathF.Cos(x / 4f) * MathF.Sin(y / 4f), y); }, new Vector3(2, 3, 2), BufferPool, out var planeMesh); - Simulation.Statics.Add(new StaticDescription(new Vector3(-64, -10, -64), new CollidableDescription(Simulation.Shapes.Add(planeMesh)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(-64, -10, -64), Simulation.Shapes.Add(planeMesh))); } diff --git a/Demos/Demos/Tanks/Tank.cs b/Demos/Demos/Tanks/Tank.cs index 7318b9d2b..c6cc3b444 100644 --- a/Demos/Demos/Tanks/Tank.cs +++ b/Demos/Demos/Tanks/Tank.cs @@ -191,7 +191,7 @@ static BodyHandle CreateWheel(Simulation simulation, CollidableProperty properties, out BodyHandle handle) { RigidPose.MultiplyWithoutOverlap(part.Pose, pose, out var bodyPose); - handle = simulation.Bodies.Add(BodyDescription.CreateDynamic(bodyPose, part.Inertia, new CollidableDescription(part.Shape), new BodyActivityDescription(0.01f))); + handle = simulation.Bodies.Add(BodyDescription.CreateDynamic(bodyPose, part.Inertia, part.Shape, 0.01f)); ref var partProperties = ref properties.Allocate(handle); partProperties = new TankDemoBodyProperties { Friction = part.Friction, TankPart = true }; return ref partProperties.Filter; diff --git a/Demos/Demos/Tanks/TankDemo.cs b/Demos/Demos/Tanks/TankDemo.cs index 959bc96ad..c7051509d 100644 --- a/Demos/Demos/Tanks/TankDemo.cs +++ b/Demos/Demos/Tanks/TankDemo.cs @@ -116,7 +116,7 @@ public override void Initialize(ContentArchive content, Camera camera) Simulation.Statics.Add(new StaticDescription( new Vector3(0, buildingShape.HalfHeight - 4f + GetHeightForPosition(position.X, position.Z, planeWidth, inverseTerrainScale, terrainPosition), 0) + position, QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, random.NextSingle() * MathF.PI), - new CollidableDescription(Simulation.Shapes.Add(buildingShape)))); + Simulation.Shapes.Add(buildingShape))); } DemoMeshHelper.CreateDeformedPlane(planeWidth, planeWidth, @@ -125,8 +125,7 @@ public override void Initialize(ContentArchive content, Camera camera) var position2D = new Vector2(vX, vY) * terrainScale + terrainPosition; return new Vector3(position2D.X, GetHeightForPosition(position2D.X, position2D.Y, planeWidth, inverseTerrainScale, terrainPosition), position2D.Y); }, new Vector3(1, 1, 1), BufferPool, out var planeMesh); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), - new CollidableDescription(Simulation.Shapes.Add(planeMesh)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), Simulation.Shapes.Add(planeMesh))); explosions = new QuickList(32, BufferPool); diff --git a/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs b/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs index 39387e32c..7d7d9d109 100644 --- a/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs +++ b/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs @@ -44,11 +44,11 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var location = spacing * (new Vector3(i, j, k) + new Vector3(-width, 1, -length)) + randomizationBase + r * randomizationSpan; if ((i + j + k) % 2 == 1) { - Simulation.Bodies.Add(BodyDescription.CreateDynamic(location, sphereInertia, new CollidableDescription(shapeIndex), new BodyActivityDescription(-1))); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(location, sphereInertia, shapeIndex, -1)); } else { - Simulation.Statics.Add(new StaticDescription(location, new CollidableDescription(shapeIndex))); + Simulation.Statics.Add(new StaticDescription(location, shapeIndex)); } } } diff --git a/Demos/SpecializedTests/CapsuleTestDemo.cs b/Demos/SpecializedTests/CapsuleTestDemo.cs index 363261874..6be4af356 100644 --- a/Demos/SpecializedTests/CapsuleTestDemo.cs +++ b/Demos/SpecializedTests/CapsuleTestDemo.cs @@ -48,7 +48,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) new BodyActivityDescription(-1)); Simulation.Bodies.Add(boxDescription); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -3, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(4, 1, 4))))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -3, 0), Simulation.Shapes.Add(new Box(4, 1, 4)))); } diff --git a/Demos/SpecializedTests/CharacterTestDemo.cs b/Demos/SpecializedTests/CharacterTestDemo.cs index 7f5156cc1..6a1f8ed45 100644 --- a/Demos/SpecializedTests/CharacterTestDemo.cs +++ b/Demos/SpecializedTests/CharacterTestDemo.cs @@ -30,8 +30,8 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Simulation.Bodies.Add( BodyDescription.CreateDynamic( new Vector3(250 * random.NextSingle() - 125, 2, 250 * random.NextSingle() - 125), new BodyInertia { InverseMass = 1 }, - new CollidableDescription(Simulation.Shapes.Add(new Capsule(0.5f, 1f))), - new BodyActivityDescription(-1)))); + Simulation.Shapes.Add(new Capsule(0.5f, 1f)), + -1))); character.CosMaximumSlope = .707f; character.LocalUp = Vector3.UnitY; @@ -84,8 +84,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Vector2 offsetFromCenter = new Vector2(x - planeWidth / 2, y - planeHeight / 2); return new Vector3(offsetFromCenter.X, MathF.Cos(x / 2f) + MathF.Sin(y / 2f), offsetFromCenter.Y); }, new Vector3(2, 1, 2), BufferPool, out var planeMesh); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -2, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), - new CollidableDescription(Simulation.Shapes.Add(planeMesh)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -2, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), Simulation.Shapes.Add(planeMesh))); removedCharacters = new QuickQueue(characters.CharacterCount, BufferPool); } diff --git a/Demos/SpecializedTests/ClothLatticeDemo.cs b/Demos/SpecializedTests/ClothLatticeDemo.cs index 7b1597abf..52eb86c20 100644 --- a/Demos/SpecializedTests/ClothLatticeDemo.cs +++ b/Demos/SpecializedTests/ClothLatticeDemo.cs @@ -86,13 +86,13 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var bigBallShape = new Sphere(25); var bigBallShapeIndex = Simulation.Shapes.Add(bigBallShape); - var bigBallDescription = new StaticDescription(new Vector3(-10, -15, 0), new CollidableDescription(bigBallShapeIndex, ContinuousDetection.Discrete(0, 0.1f))); + var bigBallDescription = new StaticDescription(new Vector3(-10, -15, 0), bigBallShapeIndex); Simulation.Statics.Add(bigBallDescription); var groundShape = new Box(200, 1, 200); var groundShapeIndex = Simulation.Shapes.Add(groundShape); - var groundDescription = new StaticDescription(new Vector3(0, -10, 0), new CollidableDescription(groundShapeIndex)); + var groundDescription = new StaticDescription(new Vector3(0, -10, 0), groundShapeIndex); Simulation.Statics.Add(groundDescription); } diff --git a/Demos/SpecializedTests/CompoundCollisionIndicesTest.cs b/Demos/SpecializedTests/CompoundCollisionIndicesTest.cs index a051fb76e..80c2e931a 100644 --- a/Demos/SpecializedTests/CompoundCollisionIndicesTest.cs +++ b/Demos/SpecializedTests/CompoundCollisionIndicesTest.cs @@ -80,12 +80,12 @@ public override void Initialize(ContentArchive content, Camera camera) builder.Add(new Box(1f, 1f, 1f), new RigidPose(new Vector3(1, 0, 0)), 1); builder.BuildDynamicCompound(out var children, out var inertia, out var center); - var compoundCollidable = new CollidableDescription(Simulation.Shapes.Add(new Compound(children))); + var compoundShapeIndex = Simulation.Shapes.Add(new Compound(children)); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 2, 0), inertia, compoundCollidable, new BodyActivityDescription(0.01f))); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 4, 0), inertia, compoundCollidable, new BodyActivityDescription(0.01f))); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 2, 0), inertia, compoundShapeIndex, 0.01f)); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 4, 0), inertia, compoundShapeIndex, 0.01f)); - Simulation.Statics.Add(new StaticDescription(new Vector3(), new CollidableDescription(Simulation.Shapes.Add(new Box(100, 1, 100))))); + Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(new Box(100, 1, 100)))); } } } diff --git a/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs b/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs index 368d646d7..cef8a3123 100644 --- a/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs +++ b/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs @@ -54,7 +54,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //Simulation.Solver.Add(a, b, new AngularHinge { LocalHingeAxisA = new Vector3(0, 1, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); } - Simulation.Statics.Add(new StaticDescription(new Vector3(), new CollidableDescription(Simulation.Shapes.Add(new Box(8192, 1, 8192))))); + Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(new Box(8192, 1, 8192)))); } } } diff --git a/Demos/SpecializedTests/ConstraintTestDemo.cs b/Demos/SpecializedTests/ConstraintTestDemo.cs index db410c0d8..0253db615 100644 --- a/Demos/SpecializedTests/ConstraintTestDemo.cs +++ b/Demos/SpecializedTests/ConstraintTestDemo.cs @@ -389,7 +389,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var wheelOrientation = Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), MathF.PI * 0.5f); var aDescription = BodyDescription.CreateDynamic(new RigidPose(new Vector3(x, 3, 0), wheelOrientation), inertiaA, wheelShape, activity); var bDescription = BodyDescription.CreateDynamic(new RigidPose(new Vector3(x, 6, 0), wheelOrientation), inertiaB, wheelShape, activity); - var cDescription = BodyDescription.CreateKinematic(new Vector3(x, 4.5f, -1), new CollidableDescription(Simulation.Shapes.Add(new Box(3, 6, 1))), activity); + var cDescription = BodyDescription.CreateKinematic(new Vector3(x, 4.5f, -1), Simulation.Shapes.Add(new Box(3, 6, 1)), activity); var a = Simulation.Bodies.Add(aDescription); var b = Simulation.Bodies.Add(bDescription); var c = Simulation.Bodies.Add(cDescription); @@ -417,7 +417,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) }); } - Simulation.Statics.Add(new StaticDescription(new Vector3(), new CollidableDescription(Simulation.Shapes.Add(new Box(256, 1, 256))))); + Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(new Box(256, 1, 256)))); } } } diff --git a/Demos/SpecializedTests/ConvexHullTestDemo.cs b/Demos/SpecializedTests/ConvexHullTestDemo.cs index e8497638d..d5cb61412 100644 --- a/Demos/SpecializedTests/ConvexHullTestDemo.cs +++ b/Demos/SpecializedTests/ConvexHullTestDemo.cs @@ -112,13 +112,13 @@ public override void Initialize(ContentArchive content, Camera camera) Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 0, 0), inertia, new CollidableDescription(hullShapeIndex, ContinuousDetection.Discrete(20, 20)), new BodyActivityDescription(0.01f))); - Simulation.Statics.Add(new StaticDescription(new Vector3(-25, -5, 0), new CollidableDescription(Simulation.Shapes.Add(new Sphere(2))))); - Simulation.Statics.Add(new StaticDescription(new Vector3(-20, -5, 0), new CollidableDescription(Simulation.Shapes.Add(new Capsule(0.5f, 2))))); - Simulation.Statics.Add(new StaticDescription(new Vector3(-15, -5, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(2f, 2f, 2f))))); - Simulation.Statics.Add(new StaticDescription(new Vector3(-10, -5, 5), new CollidableDescription(Simulation.Shapes.Add(new Triangle { A = new Vector3(0, 0, -10), B = new Vector3(5, 0, -10), C = new Vector3(0, 0, -5) })))); - Simulation.Statics.Add(new StaticDescription(new Vector3(-5, -5, 0), new CollidableDescription(Simulation.Shapes.Add(new Cylinder(1, 1))))); - Simulation.Statics.Add(new StaticDescription(new Vector3(-5, -5, 5), new CollidableDescription(Simulation.Shapes.Add(new Cylinder(1, 1))))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5, 0), new CollidableDescription(hullShapeIndex))); + Simulation.Statics.Add(new StaticDescription(new Vector3(-25, -5, 0), Simulation.Shapes.Add(new Sphere(2)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(-20, -5, 0), Simulation.Shapes.Add(new Capsule(0.5f, 2)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(-15, -5, 0), Simulation.Shapes.Add(new Box(2f, 2f, 2f)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(-10, -5, 5), Simulation.Shapes.Add(new Triangle { A = new Vector3(0, 0, -10), B = new Vector3(5, 0, -10), C = new Vector3(0, 0, -5) }))); + Simulation.Statics.Add(new StaticDescription(new Vector3(-5, -5, 0), Simulation.Shapes.Add(new Cylinder(1, 1)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(-5, -5, 5), Simulation.Shapes.Add(new Cylinder(1, 1)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5, 0), hullShapeIndex)); var spacing = new Vector3(3f, 3f, 3); int width = 16; @@ -132,12 +132,12 @@ public override void Initialize(ContentArchive content, Camera camera) for (int k = 0; k < length; ++k) { Simulation.Bodies.Add(BodyDescription.CreateDynamic( - new RigidPose(origin + spacing * new Vector3(i, j, k), BepuUtilities.QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathHelper.Pi * 0.05f)), - inertia, new CollidableDescription(hullShapeIndex), new BodyActivityDescription(0.01f))); + new RigidPose(origin + spacing * new Vector3(i, j, k), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathHelper.Pi * 0.05f)), + inertia, hullShapeIndex, 0.01f)); } } } - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -10, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(1000, 1, 1000))))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -10, 0), Simulation.Shapes.Add(new Box(1000, 1, 1000)))); } void TestConvexHullCreation() diff --git a/Demos/SpecializedTests/CylinderTestDemo.cs b/Demos/SpecializedTests/CylinderTestDemo.cs index 43d503c97..9d559e8b8 100644 --- a/Demos/SpecializedTests/CylinderTestDemo.cs +++ b/Demos/SpecializedTests/CylinderTestDemo.cs @@ -221,7 +221,7 @@ public override void Initialize(ContentArchive content, Camera camera) for (int k = 0; k < length; ++k) { var location = new Vector3(5, 3, 5) * new Vector3(i, j, k) + new Vector3(-width * 1.5f, 2.5f, -30 - length * 1.5f); - var bodyDescription = BodyDescription.CreateDynamic(location, default, new CollidableDescription(default), new BodyActivityDescription(-0.01f)); + var bodyDescription = BodyDescription.CreateDynamic(location, default, default, -0.01f); switch (j % 4) { case 0: @@ -259,8 +259,7 @@ public override void Initialize(ContentArchive content, Camera camera) var octave4 = (MathF.Sin((x + 67) * 1.50f) + MathF.Sin((y + 13) * 1.5f)) * 0.25f; return new Vector3(x, octave0 + octave1 + octave2 + octave3 + octave4, y); }, new Vector3(4, 1, 4), BufferPool, out var planeMesh); - Simulation.Statics.Add(new StaticDescription(new Vector3(-100, -15, 100), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), - new CollidableDescription(Simulation.Shapes.Add(planeMesh)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(-100, -15, 100), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), Simulation.Shapes.Add(planeMesh))); //Simulation.Statics.Add(new StaticDescription(new Vector3(0, -10, 0), Quaternion.CreateFromAxisAngle(Vector3.Normalize(new Vector3(1, 0, 1)), 0), Simulation.Shapes.Add(new Cylinder(100, 1f)), 0.1f)); diff --git a/Demos/SpecializedTests/GyroscopeTestDemo.cs b/Demos/SpecializedTests/GyroscopeTestDemo.cs index 6cea4d871..bfcabf008 100644 --- a/Demos/SpecializedTests/GyroscopeTestDemo.cs +++ b/Demos/SpecializedTests/GyroscopeTestDemo.cs @@ -63,7 +63,7 @@ public override void Initialize(ContentArchive content, Camera camera) //Note the lack of damping- we want the gyroscope to keep spinning. Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new GyroscopicIntegratorCallbacks(new Vector3(0, -10, 0), 0f, 0f), new SubsteppingTimestepper(4), 2); - Simulation.Statics.Add(new StaticDescription(new Vector3(), new CollidableDescription(Simulation.Shapes.Add(new Box(100, 1, 100))))); + Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(new Box(100, 1, 100)))); var gyroBaseBody = Simulation.Bodies.Add(BodyDescription.CreateConvexKinematic(new Vector3(0, 2, 0), Simulation.Shapes, new Box(.1f, 4, .1f))); var gyroSpinnerBody = Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(2, 4, 0), new BodyVelocity(default, new Vector3(300, 0, 0)), 1, Simulation.Shapes, new Box(0.1f, 1f, 1f))); @@ -77,7 +77,7 @@ public override void Initialize(ContentArchive content, Camera camera) builder.Dispose(); var dzhanibekovShape = Simulation.Shapes.Add(new Compound(children)); var dzhanibekovSpinnerBody = Simulation.Bodies.Add( - BodyDescription.CreateDynamic(new Vector3(6, 4, 0), new BodyVelocity(new Vector3(0, 0, 1), new Vector3(5, .001f, .001f)), inertia, new CollidableDescription(dzhanibekovShape), new BodyActivityDescription(0.01f))); + BodyDescription.CreateDynamic(new Vector3(6, 4, 0), new BodyVelocity(new Vector3(0, 0, 1), new Vector3(5, .001f, .001f)), inertia, dzhanibekovShape, 0.01f)); var dzhanibekovBaseBody = Simulation.Bodies.Add(BodyDescription.CreateConvexKinematic(new Vector3(6, 1, 0), Simulation.Shapes, new Box(.1f, 2, .1f))); Simulation.Solver.Add(dzhanibekovBaseBody, dzhanibekovSpinnerBody, new BallSocket { LocalOffsetA = new Vector3(0, 3, 0), LocalOffsetB = new Vector3(0, 0, 0), SpringSettings = new SpringSettings(30, 1) }); } diff --git a/Demos/SpecializedTests/Media/BedsheetDemo.cs b/Demos/SpecializedTests/Media/BedsheetDemo.cs index 33bb26bb2..038889df1 100644 --- a/Demos/SpecializedTests/Media/BedsheetDemo.cs +++ b/Demos/SpecializedTests/Media/BedsheetDemo.cs @@ -24,8 +24,8 @@ public class BedsheetDemo : Demo { var description = new BodyDescription { - Activity = new BodyActivityDescription(0.01f), - Collidable = new CollidableDescription(Simulation.Shapes.Add(new Sphere(bodyRadius))), + Activity = 0.01f, + Collidable = Simulation.Shapes.Add(new Sphere(bodyRadius)), LocalInertia = default, Pose = new RigidPose(default, orientation) }; @@ -130,12 +130,12 @@ bool FullyDynamic(int rowIndex, int columnIndex, int width, int height) - Simulation.Statics.Add(new StaticDescription(new Vector3(0, 10, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(80, 20, 80))))); - Simulation.Statics.Add(new StaticDescription(new Vector3(-20, 22, 30), new CollidableDescription(Simulation.Shapes.Add(new Box(34, 4, 14))))); - Simulation.Statics.Add(new StaticDescription(new Vector3(20, 22, 30), new CollidableDescription(Simulation.Shapes.Add(new Box(34, 4, 14))))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 10, 0), Simulation.Shapes.Add(new Box(80, 20, 80)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(-20, 22, 30), Simulation.Shapes.Add(new Box(34, 4, 14)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(20, 22, 30), Simulation.Shapes.Add(new Box(34, 4, 14)))); - Simulation.Statics.Add(new StaticDescription(new Vector3(65.5f, 8f, 20), new CollidableDescription(Simulation.Shapes.Add(new Cylinder(15, 15))))); + Simulation.Statics.Add(new StaticDescription(new Vector3(65.5f, 8f, 20), Simulation.Shapes.Add(new Cylinder(15, 15)))); { @@ -152,7 +152,7 @@ bool FullyDynamic(int rowIndex, int columnIndex, int width, int height) CreateAreaConstraints(handles, new SpringSettings(30, 1)); } - Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(400, 1, 400))))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), Simulation.Shapes.Add(new Box(400, 1, 400)))); } diff --git a/Demos/SpecializedTests/Media/ColosseumVideoDemo.cs b/Demos/SpecializedTests/Media/ColosseumVideoDemo.cs index d708ee130..737cc0a00 100644 --- a/Demos/SpecializedTests/Media/ColosseumVideoDemo.cs +++ b/Demos/SpecializedTests/Media/ColosseumVideoDemo.cs @@ -75,9 +75,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var ringBoxShape = new Box(0.5f, 1.5f, 3); ringBoxShape.ComputeInertia(1, out var ringBoxInertia); - var boxDescription = BodyDescription.CreateDynamic(new Vector3(), ringBoxInertia, - new CollidableDescription(Simulation.Shapes.Add(ringBoxShape)), - new BodyActivityDescription(0.01f)); + var boxDescription = BodyDescription.CreateDynamic(new Vector3(), ringBoxInertia, Simulation.Shapes.Add(ringBoxShape), 0.01f); var layerPosition = new Vector3(); const int layerCount = 10; @@ -96,15 +94,15 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) } Console.WriteLine($"box count: {Simulation.Bodies.ActiveSet.Count}"); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(500, 1, 500))))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), Simulation.Shapes.Add(new Box(500, 1, 500)))); var bulletShape = new Sphere(0.5f); bulletShape.ComputeInertia(.1f, out var bulletInertia); - bulletDescription = BodyDescription.CreateDynamic(new Vector3(), bulletInertia, new CollidableDescription(Simulation.Shapes.Add(bulletShape)), new BodyActivityDescription(0.01f)); + bulletDescription = BodyDescription.CreateDynamic(new Vector3(), bulletInertia, Simulation.Shapes.Add(bulletShape), 0.01f); var shootiePatootieShape = new Sphere(3f); shootiePatootieShape.ComputeInertia(1000, out var shootiePatootieInertia); - shootiePatootieDescription = BodyDescription.CreateDynamic(new Vector3(), shootiePatootieInertia, new CollidableDescription(Simulation.Shapes.Add(shootiePatootieShape)), new BodyActivityDescription(0.01f)); + shootiePatootieDescription = BodyDescription.CreateDynamic(new Vector3(), shootiePatootieInertia, Simulation.Shapes.Add(shootiePatootieShape), 0.01f); } bool characterActive; diff --git a/Demos/SpecializedTests/Media/NewtDemandingSacrificeVideoDemo.cs b/Demos/SpecializedTests/Media/NewtDemandingSacrificeVideoDemo.cs index a896ab540..5ef9b94e4 100644 --- a/Demos/SpecializedTests/Media/NewtDemandingSacrificeVideoDemo.cs +++ b/Demos/SpecializedTests/Media/NewtDemandingSacrificeVideoDemo.cs @@ -22,17 +22,17 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) filters = new CollidableProperty(BufferPool); Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks() { CollisionFilters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionLastTimestepper()); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(1500, 1, 1500))))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, 10, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(70, 20, 80))))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, 7.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(80, 15, 90))))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, 5, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(90, 10, 100))))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, 2.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(100, 5, 110))))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), Simulation.Shapes.Add(new Box(1500, 1, 1500)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 10, 0), Simulation.Shapes.Add(new Box(70, 20, 80)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 7.5f, 0), Simulation.Shapes.Add(new Box(80, 15, 90)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 5, 0), Simulation.Shapes.Add(new Box(90, 10, 100)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 2.5f, 0), Simulation.Shapes.Add(new Box(100, 5, 110)))); //High fidelity simulation isn't super important on this one. Simulation.Solver.IterationCount = 2; DemoMeshHelper.LoadModel(content, BufferPool, "Content\\newt.obj", new Vector3(30), out var mesh); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, 20, 0), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, 0), new CollidableDescription(Simulation.Shapes.Add(mesh)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 20, 0), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, 0), Simulation.Shapes.Add(mesh))); } Random random = new Random(5); @@ -52,23 +52,24 @@ public override void Update(Window window, Camera camera, Input input, float dt) }); var linearVelocity = Vector3.Normalize(new Vector3(-2 + 4 * random.NextSingle(), 31 + 4 * random.NextSingle(), 50) - pose.Position) * 40; var handles = RagdollDemo.AddRagdoll(pose.Position, pose.Orientation, ragdollIndex++, filters, Simulation); + var bodies = Simulation.Bodies; //This could be done better, but... ... .... .......... - new BodyReference(handles.Hips, Simulation.Bodies).Velocity = GetRandomizedVelocity(linearVelocity); - new BodyReference(handles.Abdomen, Simulation.Bodies).Velocity = GetRandomizedVelocity(linearVelocity); - new BodyReference(handles.Chest, Simulation.Bodies).Velocity =GetRandomizedVelocity(linearVelocity); - new BodyReference(handles.Head, Simulation.Bodies).Velocity = GetRandomizedVelocity(linearVelocity); - new BodyReference(handles.LeftArm.UpperArm, Simulation.Bodies).Velocity = GetRandomizedVelocity(linearVelocity); - new BodyReference(handles.LeftArm.LowerArm, Simulation.Bodies).Velocity = GetRandomizedVelocity(linearVelocity); - new BodyReference(handles.LeftArm.Hand, Simulation.Bodies).Velocity = GetRandomizedVelocity(linearVelocity); - new BodyReference(handles.RightArm.UpperArm, Simulation.Bodies).Velocity = GetRandomizedVelocity(linearVelocity); - new BodyReference(handles.RightArm.LowerArm, Simulation.Bodies).Velocity = GetRandomizedVelocity(linearVelocity); - new BodyReference(handles.RightArm.Hand, Simulation.Bodies).Velocity = GetRandomizedVelocity(linearVelocity); - new BodyReference(handles.LeftLeg.UpperLeg, Simulation.Bodies).Velocity = GetRandomizedVelocity(linearVelocity); - new BodyReference(handles.LeftLeg.LowerLeg, Simulation.Bodies).Velocity = GetRandomizedVelocity(linearVelocity); - new BodyReference(handles.LeftLeg.Foot, Simulation.Bodies).Velocity = GetRandomizedVelocity(linearVelocity); - new BodyReference(handles.RightLeg.UpperLeg, Simulation.Bodies).Velocity = GetRandomizedVelocity(linearVelocity); - new BodyReference(handles.RightLeg.LowerLeg, Simulation.Bodies).Velocity = GetRandomizedVelocity(linearVelocity); - new BodyReference(handles.RightLeg.Foot, Simulation.Bodies).Velocity = GetRandomizedVelocity(linearVelocity); + bodies[handles.Hips].Velocity = GetRandomizedVelocity(linearVelocity); + bodies[handles.Abdomen].Velocity = GetRandomizedVelocity(linearVelocity); + bodies[handles.Chest].Velocity = GetRandomizedVelocity(linearVelocity); + bodies[handles.Head].Velocity = GetRandomizedVelocity(linearVelocity); + bodies[handles.LeftArm.UpperArm].Velocity = GetRandomizedVelocity(linearVelocity); + bodies[handles.LeftArm.LowerArm].Velocity = GetRandomizedVelocity(linearVelocity); + bodies[handles.LeftArm.Hand].Velocity = GetRandomizedVelocity(linearVelocity); + bodies[handles.RightArm.UpperArm].Velocity = GetRandomizedVelocity(linearVelocity); + bodies[handles.RightArm.LowerArm].Velocity = GetRandomizedVelocity(linearVelocity); + bodies[handles.RightArm.Hand].Velocity = GetRandomizedVelocity(linearVelocity); + bodies[handles.LeftLeg.UpperLeg].Velocity = GetRandomizedVelocity(linearVelocity); + bodies[handles.LeftLeg.LowerLeg].Velocity = GetRandomizedVelocity(linearVelocity); + bodies[handles.LeftLeg.Foot].Velocity = GetRandomizedVelocity(linearVelocity); + bodies[handles.RightLeg.UpperLeg].Velocity = GetRandomizedVelocity(linearVelocity); + bodies[handles.RightLeg.LowerLeg].Velocity = GetRandomizedVelocity(linearVelocity); + bodies[handles.RightLeg.Foot].Velocity = GetRandomizedVelocity(linearVelocity); base.Update(window, camera, input, dt); } diff --git a/Demos/SpecializedTests/Media/NewtVideoDemo.cs b/Demos/SpecializedTests/Media/NewtVideoDemo.cs index 43622427b..dba025a4c 100644 --- a/Demos/SpecializedTests/Media/NewtVideoDemo.cs +++ b/Demos/SpecializedTests/Media/NewtVideoDemo.cs @@ -41,15 +41,15 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 100, -.5f), 10, Simulation.Shapes, new Sphere(5))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(1500, 1, 1500))))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -1.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Sphere(3))))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), Simulation.Shapes.Add(new Box(1500, 1, 1500)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -1.5f, 0), Simulation.Shapes.Add(new Sphere(3)))); var bulletShape = new Sphere(0.5f); bulletShape.ComputeInertia(.25f, out var bulletInertia); - bulletDescription = BodyDescription.CreateDynamic(RigidPose.Identity, bulletInertia, new CollidableDescription(Simulation.Shapes.Add(bulletShape)), new BodyActivityDescription(0.01f)); + bulletDescription = BodyDescription.CreateDynamic(RigidPose.Identity, bulletInertia, Simulation.Shapes.Add(bulletShape), 0.01f); DemoMeshHelper.LoadModel(content, BufferPool, "Content\\newt.obj", new Vector3(20), out var mesh); - Simulation.Statics.Add(new StaticDescription(new Vector3(200, 0.5f, 120), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, -3 * MathHelper.PiOver4), new CollidableDescription(Simulation.Shapes.Add(mesh)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(200, 0.5f, 120), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, -3 * MathHelper.PiOver4), Simulation.Shapes.Add(mesh))); } BodyDescription bulletDescription; public override void Update(Window window, Camera camera, Input input, float dt) diff --git a/Demos/SpecializedTests/Media/PyramidVideoDemo.cs b/Demos/SpecializedTests/Media/PyramidVideoDemo.cs index 7727a58d1..3921a5b4e 100644 --- a/Demos/SpecializedTests/Media/PyramidVideoDemo.cs +++ b/Demos/SpecializedTests/Media/PyramidVideoDemo.cs @@ -40,14 +40,12 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) (-columnCount * 0.5f + columnIndex) * boxShape.Width, (rowIndex + 0.5f) * boxShape.Height, (pyramidIndex - pyramidCount * 0.5f) * (boxShape.Length + 4)), - boxInertia, - new CollidableDescription(boxIndex), - new BodyActivityDescription(0.01f))); + boxInertia, boxIndex, 0.01f)); } } } - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(2500, 1, 2500))))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), Simulation.Shapes.Add(new Box(2500, 1, 2500)))); } //We'll randomize the size of bullets. diff --git a/Demos/SpecializedTests/Media/ShrinkwrappedNewtsVideoDemo.cs b/Demos/SpecializedTests/Media/ShrinkwrappedNewtsVideoDemo.cs index 0cf2ada01..31fdb9b6c 100644 --- a/Demos/SpecializedTests/Media/ShrinkwrappedNewtsVideoDemo.cs +++ b/Demos/SpecializedTests/Media/ShrinkwrappedNewtsVideoDemo.cs @@ -46,13 +46,13 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Simulation.Bodies.Add(bodyDescription); } - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -30, 250), new CollidableDescription(Simulation.Shapes.Add(new Box(1000, 60, 500))))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -60, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(1000, 1, 1000))))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -30, 250), Simulation.Shapes.Add(new Box(1000, 60, 500)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -60, 0), Simulation.Shapes.Add(new Box(1000, 1, 1000)))); DemoMeshHelper.LoadModel(content, BufferPool, "Content\\newt.obj", new Vector3(1, 1.5f, 1), out mesh); - Simulation.Statics.Add(new StaticDescription(new Vector3(30, 0, 20), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, -3 * MathHelper.PiOver4), new CollidableDescription(Simulation.Shapes.Add(mesh)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(30, 0, 20), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, -3 * MathHelper.PiOver4), Simulation.Shapes.Add(mesh))); } Mesh mesh; @@ -62,7 +62,7 @@ public override void Update(Window window, Camera camera, Input input, float dt) if(input.WasPushed(OpenTK.Input.Key.Z)) { mesh.Scale = new Vector3(30); - Simulation.Statics.Add(new StaticDescription(new Vector3(70, 0, 50), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, -3.1f * MathHelper.PiOver4), new CollidableDescription(Simulation.Shapes.Add(mesh)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(70, 0, 50), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, -3.1f * MathHelper.PiOver4), Simulation.Shapes.Add(mesh))); } base.Update(window, camera, input, dt); } diff --git a/Demos/SpecializedTests/MeshMeshTestDemo.cs b/Demos/SpecializedTests/MeshMeshTestDemo.cs index e75c735af..a8a410078 100644 --- a/Demos/SpecializedTests/MeshMeshTestDemo.cs +++ b/Demos/SpecializedTests/MeshMeshTestDemo.cs @@ -29,8 +29,7 @@ public override void Initialize(ContentArchive content, Camera camera) for (int meshIndex = 0; meshIndex < 3; ++meshIndex) { Simulation.Bodies.Add( - BodyDescription.CreateDynamic(new Vector3(0, 2 + meshIndex * 2, 0), approximateInertia, - new CollidableDescription(meshShapeIndex), new BodyActivityDescription(0.01f))); + BodyDescription.CreateDynamic(new Vector3(0, 2 + meshIndex * 2, 0), approximateInertia, meshShapeIndex, 0.01f)); } var compoundBuilder = new CompoundBuilder(BufferPool, Simulation.Shapes, 12); @@ -44,13 +43,13 @@ public override void Initialize(ContentArchive content, Camera camera) compoundBuilder.Dispose(); for (int i = 0; i < 3; ++i) { - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(5, 2 + i * 2, 0), compoundInertia, new CollidableDescription(compoundShapeIndex), new BodyActivityDescription(0.01f))); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(5, 2 + i * 2, 0), compoundInertia, compoundShapeIndex, 0.01f)); } var staticShape = new Box(1500, 1, 1500); var staticShapeIndex = Simulation.Shapes.Add(staticShape); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(staticShapeIndex))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), staticShapeIndex)); } } } diff --git a/Demos/SpecializedTests/MeshReductionTestDemo.cs b/Demos/SpecializedTests/MeshReductionTestDemo.cs index 10b1b8e3b..eae978517 100644 --- a/Demos/SpecializedTests/MeshReductionTestDemo.cs +++ b/Demos/SpecializedTests/MeshReductionTestDemo.cs @@ -67,24 +67,23 @@ public override void Initialize(ContentArchive content, Camera camera) return new Vector3(vertexPosition.X, terrainHeight + edgeRamp, vertexPosition.Y); }, new Vector3(1, 1, 1), BufferPool, out var planeMesh); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -15, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), - new CollidableDescription(Simulation.Shapes.Add(planeMesh)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -15, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), Simulation.Shapes.Add(planeMesh))); var testBox = new Box(3, 3, 3); testBox.ComputeInertia(1, out var testBoxInertia); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 10, 0), testBoxInertia, new CollidableDescription(Simulation.Shapes.Add(testBox), ContinuousDetection.Discrete(10, 10)), new BodyActivityDescription(-1))); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 10, 0), testBoxInertia, new CollidableDescription(Simulation.Shapes.Add(testBox), ContinuousDetection.Discrete(10, 10)), -1)); var testSphere = new Sphere(.1f); testSphere.ComputeInertia(1, out var testSphereInertia); //testSphereInertia.InverseInertiaTensor = default; - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(10, 10, 0), testSphereInertia, new CollidableDescription(Simulation.Shapes.Add(testSphere), ContinuousDetection.Discrete(10, 10)), new BodyActivityDescription(-1))); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(10, 10, 0), testSphereInertia, new CollidableDescription(Simulation.Shapes.Add(testSphere), ContinuousDetection.Discrete(10, 10)), -1)); var testCylinder = new Cylinder(1.5f, 2f); testCylinder.ComputeInertia(1, out var testCylinderInertia); //testCylinderInertia.InverseInertiaTensor = default; - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(15, 10, 0), testCylinderInertia, new CollidableDescription(Simulation.Shapes.Add(testCylinder), ContinuousDetection.Discrete(10, 10)), new BodyActivityDescription(-1))); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(15, 10, 0), testCylinderInertia, new CollidableDescription(Simulation.Shapes.Add(testCylinder), ContinuousDetection.Discrete(10, 10)), -1)); var testCapsule = new Capsule(.1f, 2f); testCapsule.ComputeInertia(1, out var testCapsuleInertia); //testCapsuleInertia.InverseInertiaTensor = default; - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(18, 10, 0), testCapsuleInertia, new CollidableDescription(Simulation.Shapes.Add(testCapsule), ContinuousDetection.Discrete(10, 10)), new BodyActivityDescription(-1))); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(18, 10, 0), testCapsuleInertia, new CollidableDescription(Simulation.Shapes.Add(testCapsule), ContinuousDetection.Discrete(10, 10)), -1)); var points = new QuickList(8, BufferPool); points.AllocateUnsafely() = new Vector3(0, 0, 0); @@ -97,7 +96,7 @@ public override void Initialize(ContentArchive content, Camera camera) points.AllocateUnsafely() = new Vector3(2, 2, 2); var convexHull = new ConvexHull(points, BufferPool, out _); convexHull.ComputeInertia(1, out var convexHullInertia); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(23, 10, 0), convexHullInertia, new CollidableDescription(Simulation.Shapes.Add(convexHull), ContinuousDetection.Discrete(10, 10)), new BodyActivityDescription(-1))); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(23, 10, 0), convexHullInertia, new CollidableDescription(Simulation.Shapes.Add(convexHull), ContinuousDetection.Discrete(10, 10)), -1)); //var sphere = new Sphere(1.5f); //var capsule = new Capsule(1f, 1f); @@ -143,7 +142,7 @@ public override void Initialize(ContentArchive content, Camera camera) for (int k = 0; k < length; ++k) { var location = new Vector3(70, 35, 70) * new Vector3(i, j, k) + new Vector3(-width * 70 / 2f, 5f, -length * 70 / 2f); - var bodyDescription = BodyDescription.CreateDynamic(location, default, new CollidableDescription(default), new BodyActivityDescription(0.01f)); + var bodyDescription = BodyDescription.CreateDynamic(location, default, default, 0.01f); var index = shapeCount++; switch (index % 5) { diff --git a/Demos/SpecializedTests/MeshSerializationTestDemo.cs b/Demos/SpecializedTests/MeshSerializationTestDemo.cs index 343e7bd2a..61a7453eb 100644 --- a/Demos/SpecializedTests/MeshSerializationTestDemo.cs +++ b/Demos/SpecializedTests/MeshSerializationTestDemo.cs @@ -21,7 +21,7 @@ public override void Initialize(ContentArchive content, Camera camera) var startTime = Stopwatch.GetTimestamp(); DemoMeshHelper.CreateDeformedPlane(1025, 1025, (x, y) => new Vector3(x * 0.125f, MathF.Sin(x) + MathF.Sin(y), y * 0.125f), Vector3.One, BufferPool, out var originalMesh); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), new CollidableDescription(Simulation.Shapes.Add(originalMesh)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), Simulation.Shapes.Add(originalMesh))); var endTime = Stopwatch.GetTimestamp(); var freshConstructionTime = (endTime - startTime) / (double)Stopwatch.Frequency; Console.WriteLine($"Fresh construction time (ms): {freshConstructionTime * 1e3}"); @@ -34,7 +34,7 @@ public override void Initialize(ContentArchive content, Camera camera) var loadTime = (endTime - startTime) / (double)Stopwatch.Frequency; Console.WriteLine($"Load time (ms): {(endTime - startTime) * 1e3 / Stopwatch.Frequency}"); Console.WriteLine($"Relative speedup: {freshConstructionTime / loadTime}"); - Simulation.Statics.Add(new StaticDescription(new Vector3(128, 0, 0), new CollidableDescription(Simulation.Shapes.Add(loadedMesh)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(128, 0, 0), Simulation.Shapes.Add(loadedMesh))); BufferPool.Return(ref serializedMeshBytes); @@ -42,7 +42,7 @@ public override void Initialize(ContentArchive content, Camera camera) var random = new Random(5); var shapeToDrop = new Box(1, 1, 1); shapeToDrop.ComputeInertia(1, out var shapeToDropInertia); - var descriptionToDrop = BodyDescription.CreateDynamic(new Vector3(), shapeToDropInertia, new CollidableDescription(Simulation.Shapes.Add(shapeToDrop)), new BodyActivityDescription(0.01f)); + var descriptionToDrop = BodyDescription.CreateDynamic(new Vector3(), shapeToDropInertia, Simulation.Shapes.Add(shapeToDrop), 0.01f); for (int i = 0; i < 1024; ++i) { descriptionToDrop.Pose.Position = new Vector3(8 + 240 * random.NextSingle(), 10 + 10 * random.NextSingle(), 8 + 112 * random.NextSingle()); diff --git a/Demos/SpecializedTests/MeshTestDemo.cs b/Demos/SpecializedTests/MeshTestDemo.cs index a80ea30f0..068e8ca5b 100644 --- a/Demos/SpecializedTests/MeshTestDemo.cs +++ b/Demos/SpecializedTests/MeshTestDemo.cs @@ -40,7 +40,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) for (int k = 0; k < length; ++k) { var location = new Vector3(5, 5, 5) * new Vector3(i, j, k);// + new Vector3(-width * 1.5f, 1.5f, -length * 1.5f); - var bodyDescription = BodyDescription.CreateDynamic(location, default, new CollidableDescription(default), new BodyActivityDescription(0.01f)); + var bodyDescription = BodyDescription.CreateDynamic(location, default, default, 0.01f); switch ((i + j) % 3) { case 0: @@ -69,16 +69,15 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) DemoMeshHelper.LoadModel(content, BufferPool, @"Content\newt.obj", new Vector3(5, 5, 5), out var newtMesh); newtMesh.ComputeClosedInertia(10, out var newtInertia, out _); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new RigidPose(new Vector3(30, 20, 30)), newtInertia, - new CollidableDescription(Simulation.Shapes.Add(newtMesh)), new BodyActivityDescription(0.01f))); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new RigidPose(new Vector3(30, 20, 30)), newtInertia, Simulation.Shapes.Add(newtMesh), 0.01f)); - Simulation.Statics.Add(new StaticDescription(new Vector3(30, 15, 30), new CollidableDescription(Simulation.Shapes.Add(new Box(15, 1, 15))))); + Simulation.Statics.Add(new StaticDescription(new Vector3(30, 15, 30), Simulation.Shapes.Add(new Box(15, 1, 15)))); DemoMeshHelper.LoadModel(content, BufferPool, @"Content\box.obj", new Vector3(5, 1, 5), out var boxMesh); - Simulation.Statics.Add(new StaticDescription(new Vector3(10, 5, -20), new CollidableDescription(Simulation.Shapes.Add(boxMesh)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(10, 5, -20), Simulation.Shapes.Add(boxMesh))); DemoMeshHelper.CreateFan(64, 16, new Vector3(1, 1, 1), BufferPool, out var fanMesh); - Simulation.Statics.Add(new StaticDescription(new Vector3(-10, 0, -20), new CollidableDescription(Simulation.Shapes.Add(fanMesh)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(-10, 0, -20), Simulation.Shapes.Add(fanMesh))); const int planeWidth = 128; const int planeHeight = 128; @@ -87,8 +86,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) { return new Vector3(x - planeWidth / 2, 1 * MathF.Cos(x / 2f) * MathF.Sin(y / 2f), y - planeHeight / 2); }, new Vector3(2, 1, 2), BufferPool, out var planeMesh); - Simulation.Statics.Add(new StaticDescription(new Vector3(64, -10, 64), BepuUtilities.QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), - new CollidableDescription(Simulation.Shapes.Add(planeMesh)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(64, -10, 64), BepuUtilities.QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), Simulation.Shapes.Add(planeMesh))); } diff --git a/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs b/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs index 6f4aa5f77..99e22dc01 100644 --- a/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs +++ b/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs @@ -40,9 +40,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) (-columnCount * 0.5f + columnIndex) * boxShape.Width, (rowIndex + 0.5f) * boxShape.Height, (pyramidIndex - pyramidCount * 0.5f) * (boxShape.Length + 4)), - boxInertia, - new CollidableDescription(boxIndex), - new BodyActivityDescription(0.01f))); + boxInertia, boxIndex, 0.01f)); } } } @@ -50,10 +48,10 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var staticShape = new Box(250, 1, 250); var staticShapeIndex = Simulation.Shapes.Add(staticShape); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(staticShapeIndex))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), staticShapeIndex)); } - + int frameIndex; Random random = new Random(5); public override void Update(Window window, Camera camera, Input input, float dt) diff --git a/Demos/SpecializedTests/RagdollTubeDemo.cs b/Demos/SpecializedTests/RagdollTubeDemo.cs index b7b938bae..23db80774 100644 --- a/Demos/SpecializedTests/RagdollTubeDemo.cs +++ b/Demos/SpecializedTests/RagdollTubeDemo.cs @@ -56,13 +56,13 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) builder.AddForKinematic(Simulation.Shapes.Add(new Box(1, 2, panelShape.Length)), new RigidPose(new Vector3(0, tubeRadius - 1, 0)), 0); builder.BuildKinematicCompound(out var children); var compound = new BigCompound(children, Simulation.Shapes, BufferPool); - tubeHandle = Simulation.Bodies.Add(BodyDescription.CreateKinematic(tubeCenter, new BodyVelocity(default, new Vector3(0, 0, .25f)), new CollidableDescription(Simulation.Shapes.Add(compound)), new BodyActivityDescription())); + tubeHandle = Simulation.Bodies.Add(BodyDescription.CreateKinematic(tubeCenter, new BodyVelocity(default, new Vector3(0, 0, .25f)), Simulation.Shapes.Add(compound), 0f)); filters[tubeHandle] = new SubgroupCollisionFilter(int.MaxValue); builder.Dispose(); var staticShape = new Box(300, 1, 300); var staticShapeIndex = Simulation.Shapes.Add(staticShape); - var staticDescription = new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(staticShapeIndex)); + var staticDescription = new StaticDescription(new Vector3(0, -0.5f, 0), staticShapeIndex); Simulation.Statics.Add(staticDescription); } diff --git a/Demos/SpecializedTests/ShapePileTestDemo.cs b/Demos/SpecializedTests/ShapePileTestDemo.cs index 565addd98..6cf1dec0e 100644 --- a/Demos/SpecializedTests/ShapePileTestDemo.cs +++ b/Demos/SpecializedTests/ShapePileTestDemo.cs @@ -65,7 +65,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) for (int k = 0; k < length; ++k) { var location = new Vector3(6, 3, 6) * new Vector3(i, j, k) + new Vector3(-width * 1.5f, 5.5f, -length * 1.5f); - var bodyDescription = BodyDescription.CreateKinematic(location, new CollidableDescription(default), new BodyActivityDescription(0.01f)); + var bodyDescription = BodyDescription.CreateKinematic(location, default, 0.01f); var index = shapeCount++; switch (index % 5) { @@ -97,7 +97,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) } DemoMeshHelper.CreateDeformedPlane(128, 128, (x, y) => new Vector3(x - 64, 2f * (float)(Math.Sin(x * 0.5f) * Math.Sin(y * 0.5f)), y - 64), new Vector3(4, 1, 4), BufferPool, out var mesh); - Simulation.Statics.Add(new StaticDescription(new Vector3(), new CollidableDescription(Simulation.Shapes.Add(mesh)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); } } diff --git a/Demos/SpecializedTests/SolverBatchTestDemo.cs b/Demos/SpecializedTests/SolverBatchTestDemo.cs index bd5fcbc7d..3317a90b1 100644 --- a/Demos/SpecializedTests/SolverBatchTestDemo.cs +++ b/Demos/SpecializedTests/SolverBatchTestDemo.cs @@ -37,7 +37,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) for (int j = 0; j < length; ++j) { var location = new Vector3(0, 30, 0) + new Vector3(spacing, 0, spacing) * (new Vector3(i, 0, j) + new Vector3(-width * 0.5f, 0, -length * 0.5f)); - var bodyDescription = BodyDescription.CreateDynamic(location, clothNodeInertia, new CollidableDescription(clothNodeShapeIndex), new BodyActivityDescription(0.01f)); + var bodyDescription = BodyDescription.CreateDynamic(location, clothNodeInertia, clothNodeShapeIndex, 0.01f); nodeHandles[i][j] = Simulation.Bodies.Add(bodyDescription); } @@ -84,13 +84,13 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var bigBallShape = new Sphere(45); var bigBallShapeIndex = Simulation.Shapes.Add(bigBallShape); - var bigBallDescription = BodyDescription.CreateKinematic(new Vector3(-10, -15, 0), new CollidableDescription(bigBallShapeIndex), new BodyActivityDescription(0)); + var bigBallDescription = BodyDescription.CreateKinematic(new Vector3(-10, -15, 0), bigBallShapeIndex, 0); bigBallHandle = Simulation.Bodies.Add(bigBallDescription); var groundShape = new Box(200, 1, 200); var groundShapeIndex = Simulation.Shapes.Add(groundShape); - var groundDescription = BodyDescription.CreateKinematic(new Vector3(0, -10, 0), new CollidableDescription(groundShapeIndex), new BodyActivityDescription(0)); + var groundDescription = BodyDescription.CreateKinematic(new Vector3(0, -10, 0), groundShapeIndex, 0); Simulation.Bodies.Add(groundDescription); } BodyHandle bigBallHandle; diff --git a/Demos/SpecializedTests/TriangleTestDemo.cs b/Demos/SpecializedTests/TriangleTestDemo.cs index 4caf671ff..9041bd545 100644 --- a/Demos/SpecializedTests/TriangleTestDemo.cs +++ b/Demos/SpecializedTests/TriangleTestDemo.cs @@ -128,11 +128,11 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) ////bodyDescription.LocalInertia.InverseInertiaTensor = new Triangular3x3(); //Simulation.Bodies.Add(bodyDescription); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(200, 5, 200))))); - Simulation.Statics.Add(new StaticDescription(new Vector3(10, -2, 30), new CollidableDescription(Simulation.Shapes.Add(new Box(10, 5, 10))))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5, 0), Simulation.Shapes.Add(new Box(200, 5, 200)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(10, -2, 30), Simulation.Shapes.Add(new Box(10, 5, 10)))); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 2, 0), new BodyInertia { InverseMass = 1 }, new CollidableDescription(Simulation.Shapes.Add(new Sphere(1.75f)), ContinuousDetection.Discrete(0.1f, 0.1f)), new BodyActivityDescription(-1))); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new RigidPose(new Vector3(20, 2, 3), Quaternion.CreateFromYawPitchRoll(0f, 1.745329E-05f, 0f)), new BodyInertia { InverseMass = 1 }, new CollidableDescription(Simulation.Shapes.Add(new Capsule(1, 2)), ContinuousDetection.Discrete(0.1f, 0.1f)), new BodyActivityDescription(-1))); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 2, 0), new BodyInertia { InverseMass = 1 }, new CollidableDescription(Simulation.Shapes.Add(new Sphere(1.75f)), ContinuousDetection.Discrete(0.1f, 0.1f)), -1)); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new RigidPose(new Vector3(20, 2, 3), Quaternion.CreateFromYawPitchRoll(0f, 1.745329E-05f, 0f)), new BodyInertia { InverseMass = 1 }, new CollidableDescription(Simulation.Shapes.Add(new Capsule(1, 2)), ContinuousDetection.Discrete(0.1f, 0.1f)), -1)); var testBox = new Box(2, 3, 2); testBox.ComputeInertia(1, out var testBoxInertia); Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 2, 6), testBoxInertia, new CollidableDescription(Simulation.Shapes.Add(testBox), ContinuousDetection.Discrete(10.1f, 10.1f)), new BodyActivityDescription(-1))); @@ -140,11 +140,11 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var cylinder = new Cylinder(1.75f, 2); cylinder.ComputeInertia(1, out var cylinderInertia); //cylinderInertia.InverseInertiaTensor = default; - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new RigidPose(new Vector3(20, 2, 9), Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), MathF.PI / 2f)), cylinderInertia, new CollidableDescription(Simulation.Shapes.Add(cylinder), ContinuousDetection.Discrete(5f, 5f)), new BodyActivityDescription(-1))); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new RigidPose(new Vector3(20, 2, 9), Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), MathF.PI / 2f)), cylinderInertia, new CollidableDescription(Simulation.Shapes.Add(cylinder), ContinuousDetection.Discrete(5f, 5f)), -1)); var cylinder2 = new Cylinder(.5f, 0.5f); cylinder2.ComputeInertia(1, out var cylinder2Inertia); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new RigidPose(new Vector3(23, 2, 9), Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), 0)), cylinder2Inertia, new CollidableDescription(Simulation.Shapes.Add(cylinder2), ContinuousDetection.Discrete(5f, 5f)), new BodyActivityDescription(-1))); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new RigidPose(new Vector3(23, 2, 9), Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), 0)), cylinder2Inertia, new CollidableDescription(Simulation.Shapes.Add(cylinder2), ContinuousDetection.Discrete(5f, 5f)), -1)); var points = new QuickList(8, BufferPool); points.AllocateUnsafely() = new Vector3(0, 0, 0); points.AllocateUnsafely() = new Vector3(0, 0, 2); @@ -156,8 +156,8 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) points.AllocateUnsafely() = new Vector3(2, 2, 2); var convexHull = new ConvexHull(points, BufferPool, out _); convexHull.ComputeInertia(1, out var convexHullInertia); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 2, 12), convexHullInertia, new CollidableDescription(Simulation.Shapes.Add(convexHull), ContinuousDetection.Discrete(0.1f, 0.1f)), new BodyActivityDescription(-1))); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(23, 2, 12), convexHullInertia, new CollidableDescription(Simulation.Shapes.Add(convexHull), ContinuousDetection.Discrete(0.1f, 0.1f)), new BodyActivityDescription(-1))); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 2, 12), convexHullInertia, new CollidableDescription(Simulation.Shapes.Add(convexHull), ContinuousDetection.Discrete(0.1f, 0.1f)), -1)); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(23, 2, 12), convexHullInertia, new CollidableDescription(Simulation.Shapes.Add(convexHull), ContinuousDetection.Discrete(0.1f, 0.1f)), -1)); CompoundBuilder builder = new CompoundBuilder(BufferPool, Simulation.Shapes, 2); builder.Add(new Box(1, 1, 1), RigidPose.Identity, 1); @@ -165,7 +165,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) builder.BuildDynamicCompound(out var children, out var compoundInertia); //compoundInertia.InverseInertiaTensor = default; var compound = new Compound(children); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 3, 14), compoundInertia, new CollidableDescription(Simulation.Shapes.Add(compound), ContinuousDetection.Discrete(10.1f, 10.1f)), new BodyActivityDescription(-1))); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 3, 14), compoundInertia, new CollidableDescription(Simulation.Shapes.Add(compound), ContinuousDetection.Discrete(10.1f, 10.1f)), -1)); { var triangles = new QuickList(4, BufferPool); @@ -201,7 +201,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) triangles.AllocateUnsafely() = new Triangle { A = v0, B = v2, C = v1 }; triangles.AllocateUnsafely() = new Triangle { A = v1, B = v2, C = v3 }; var testMesh = new Mesh(triangles, Vector3.One, BufferPool); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(22, -2.5f, 0), new BodyInertia { InverseMass = 1 }, new CollidableDescription(Simulation.Shapes.Add(testMesh), ContinuousDetection.Discrete(10.1f, 10.1f)), new BodyActivityDescription(-1f))); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(22, -2.5f, 0), new BodyInertia { InverseMass = 1 }, new CollidableDescription(Simulation.Shapes.Add(testMesh), ContinuousDetection.Discrete(10.1f, 10.1f)), -1f)); } } } diff --git a/Demos/SpecializedTests/VolumeQueryTests.cs b/Demos/SpecializedTests/VolumeQueryTests.cs index f982547f2..0f776f7dc 100644 --- a/Demos/SpecializedTests/VolumeQueryTests.cs +++ b/Demos/SpecializedTests/VolumeQueryTests.cs @@ -92,12 +92,12 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) if ((i + j + k) % 2 == 1) { - var bodyDescription = BodyDescription.CreateKinematic(new RigidPose(location, orientation), new CollidableDescription(shapeIndex), new BodyActivityDescription(-1)); + var bodyDescription = BodyDescription.CreateKinematic(new RigidPose(location, orientation), shapeIndex, -1); Simulation.Bodies.Add(bodyDescription); } else { - var staticDescription = new StaticDescription(location, orientation, new CollidableDescription(shapeIndex)); + var staticDescription = new StaticDescription(location, orientation, shapeIndex); Simulation.Statics.Add(staticDescription); } From 86d429e60bc85377b7b80f8e1d28e5a467093f01 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 30 Nov 2021 19:40:45 -0600 Subject: [PATCH 300/947] Simplified more body/collidable creation. --- Demos/Demos/BouncinessDemo.cs | 2 +- Demos/Demos/Cars/SimpleCar.cs | 4 ++-- Demos/Demos/Characters/CharacterDemo.cs | 2 +- Demos/Demos/Characters/CharacterInput.cs | 5 ++--- Demos/Demos/ClothDemo.cs | 2 +- .../Demos/ContinuousCollisionDetectionDemo.cs | 14 ++++++------- Demos/Demos/Sponsors/SponsorCharacterAI.cs | 2 +- Demos/Demos/Tanks/Tank.cs | 8 ++++---- Demos/SpecializedTests/CapsuleTestDemo.cs | 6 ++---- Demos/SpecializedTests/ClothLatticeDemo.cs | 2 +- Demos/SpecializedTests/ConvexHullTestDemo.cs | 2 +- Demos/SpecializedTests/CylinderTestDemo.cs | 2 +- .../SpecializedTests/MeshReductionTestDemo.cs | 10 +++++----- Demos/SpecializedTests/TriangleTestDemo.cs | 20 +++++++++---------- 14 files changed, 39 insertions(+), 42 deletions(-) diff --git a/Demos/Demos/BouncinessDemo.cs b/Demos/Demos/BouncinessDemo.cs index 4e9b2d6cf..5624a88fa 100644 --- a/Demos/Demos/BouncinessDemo.cs +++ b/Demos/Demos/BouncinessDemo.cs @@ -90,7 +90,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var shape = new Sphere(1); shape.ComputeInertia(1, out var inertia); - var ballDescription = BodyDescription.CreateDynamic(RigidPose.Identity, inertia, new CollidableDescription(Simulation.Shapes.Add(shape), ContinuousDetection.Discrete(20, 20)), new BodyActivityDescription(1e-2f)); + var ballDescription = BodyDescription.CreateDynamic(RigidPose.Identity, inertia, new(Simulation.Shapes.Add(shape), ContinuousDetection.Discrete(20, 20)), 1e-2f); for (int i = 0; i < 100; ++i) { diff --git a/Demos/Demos/Cars/SimpleCar.cs b/Demos/Demos/Cars/SimpleCar.cs index f2eafad2b..facbbefd3 100644 --- a/Demos/Demos/Cars/SimpleCar.cs +++ b/Demos/Demos/Cars/SimpleCar.cs @@ -43,7 +43,7 @@ public static WheelHandles CreateWheel(Simulation simulation, CollidableProperty RigidPose.Transform(bodyToWheelSuspension + suspensionDirection * suspensionLength, bodyPose, out wheelPose.Position); QuaternionEx.ConcatenateWithoutOverlap(localWheelOrientation, bodyPose.Orientation, out wheelPose.Orientation); WheelHandles handles; - handles.Wheel = simulation.Bodies.Add(BodyDescription.CreateDynamic(wheelPose, wheelInertia, new CollidableDescription(wheelShape, 0.5f), new BodyActivityDescription(0.01f))); + handles.Wheel = simulation.Bodies.Add(BodyDescription.CreateDynamic(wheelPose, wheelInertia, new(wheelShape, 0.5f), 0.01f)); handles.SuspensionSpring = simulation.Solver.Add(bodyHandle, handles.Wheel, new LinearAxisServo { @@ -85,7 +85,7 @@ public static SimpleCar Create(Simulation simulation, CollidableProperty poseCreator) { PoseCreator = poseCreator; - BodyHandle = simulation.Bodies.Add(BodyDescription.CreateKinematic(poseCreator(timeOffset), collidable, new BodyActivityDescription(-1))); + BodyHandle = simulation.Bodies.Add(BodyDescription.CreateKinematic(poseCreator(timeOffset), collidable, -1)); InverseGoalSatisfactionTime = 1f / goalSatisfactionTime; TimeOffset = timeOffset; } diff --git a/Demos/Demos/Characters/CharacterInput.cs b/Demos/Demos/Characters/CharacterInput.cs index 3653faa6d..b844dd3ae 100644 --- a/Demos/Demos/Characters/CharacterInput.cs +++ b/Demos/Demos/Characters/CharacterInput.cs @@ -39,9 +39,8 @@ public CharacterInput(CharacterControllers characters, Vector3 initialPosition, //Because characters are dynamic, they require a defined BodyInertia. For the purposes of the demos, we don't want them to rotate or fall over, so the inverse inertia tensor is left at its default value of all zeroes. //This is effectively equivalent to giving it an infinite inertia tensor- in other words, no torque will cause it to rotate. bodyHandle = characters.Simulation.Bodies.Add( - BodyDescription.CreateDynamic(initialPosition, new BodyInertia { InverseMass = 1f / mass }, - new CollidableDescription(shapeIndex, ContinuousDetection.CreatePassive(minimumSpeculativeMargin, float.MaxValue)), - new BodyActivityDescription(shape.Radius * 0.02f))); + BodyDescription.CreateDynamic(initialPosition, new BodyInertia { InverseMass = 1f / mass }, + new(shapeIndex, ContinuousDetection.CreatePassive(minimumSpeculativeMargin, float.MaxValue)), shape.Radius * 0.02f)); ref var character = ref characters.AllocateCharacter(bodyHandle); character.LocalUp = new Vector3(0, 1, 0); character.CosMaximumSlope = MathF.Cos(maximumSlope); diff --git a/Demos/Demos/ClothDemo.cs b/Demos/Demos/ClothDemo.cs index 586e75ff1..586f1f59e 100644 --- a/Demos/Demos/ClothDemo.cs +++ b/Demos/Demos/ClothDemo.cs @@ -105,7 +105,7 @@ public class ClothDemo : Demo { var description = new BodyDescription { - Activity = new BodyActivityDescription(0.01f), + Activity = 0.01f, Collidable = Simulation.Shapes.Add(new Sphere(bodyRadius)), LocalInertia = default, Pose = new RigidPose(default, orientation) diff --git a/Demos/Demos/ContinuousCollisionDetectionDemo.cs b/Demos/Demos/ContinuousCollisionDetectionDemo.cs index 67eb50207..2cbcc1a01 100644 --- a/Demos/Demos/ContinuousCollisionDetectionDemo.cs +++ b/Demos/Demos/ContinuousCollisionDetectionDemo.cs @@ -38,7 +38,7 @@ ConstraintHandle BuildSpinner(Vector3 initialPosition, float rotationSpeed) //Using a restricted speculative margin by setting the maximumSpeculativeMargin to 0.2 means that collision detection won't accept distant contacts. //This pretty much eliminates ghost collisions, while the continuous sweep helps avoid missed collisions. - var spinnerBlade = Simulation.Bodies.Add(BodyDescription.CreateDynamic(initialPosition, bladeInertia, new CollidableDescription(shapeIndex, ContinuousDetection.Continuous(1e-4f, 1e-4f, maximumSpeculativeMargin: 0.2f)), 0.01f)); + var spinnerBlade = Simulation.Bodies.Add(BodyDescription.CreateDynamic(initialPosition, bladeInertia, new(shapeIndex, ContinuousDetection.Continuous(1e-4f, 1e-4f, maximumSpeculativeMargin: 0.2f)), 0.01f)); Simulation.Solver.Add(spinnerBase, spinnerBlade, new Hinge { LocalHingeAxisA = new Vector3(0, 0, 1), LocalHingeAxisB = new Vector3(0, 0, 1), LocalOffsetB = new Vector3(0, 0, -3), SpringSettings = new SpringSettings(30, 1) }); Simulation.Solver.Add(spinnerBase, spinnerBlade, new AngularAxisMotor { LocalAxisA = new Vector3(0, 0, 1), Settings = new MotorSettings(10, 1e-4f), TargetVelocity = rotationSpeed }); return Simulation.Solver.Add(spinnerBase, new OneBodyLinearServo { ServoSettings = ServoSettings.Default, SpringSettings = new SpringSettings(30, 1) }); @@ -55,8 +55,8 @@ public override void Initialize(ContentArchive content, Camera camera) //Also note that the PositionFirstTimestepper is the simplest timestepping mode, but since it integrates velocity into position at the start of the frame, directly modified velocities outside of the timestep //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. - Simulation = Simulation.Create(BufferPool, - new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(240, 1), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 1f }, + Simulation = Simulation.Create(BufferPool, + new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(240, 1), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 1f }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); var shape = new Box(1, 1, 1); @@ -67,16 +67,16 @@ public override void Initialize(ContentArchive content, Camera camera) for (int j = 0; j < 10; ++j) { //These two falling dynamics have pretty small speculative margins. The second one uses continuous collision detection sweeps to generate speculative contacts. - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(-4 - 2 * j, 100 + (i + j) * 2, i * 2), new BodyVelocity { Linear = new Vector3(0, -150, 0) }, inertia, - new CollidableDescription(shapeIndex, ContinuousDetection.Discrete(maximumSpeculativeMargin: 0.01f)), new BodyActivityDescription(0.01f))); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(-4 - 2 * j, 100 + (i + j) * 2, i * 2), new(new Vector3(0, -150, 0)), inertia, + new(shapeIndex, ContinuousDetection.Discrete(maximumSpeculativeMargin: 0.01f)), 0.01f)); //The minimum progression duration parameter at 1e-3 means the CCD sweep won't miss any collisions that last at least 1e-3 units of time- so, if time is measured in seconds, //then this will capture any collision that an update rate of 1000hz would. //Note also that the sweep convergence threshold is actually pretty loose at 100hz. Despite that, it can still lead to reasonably good speculative contacts with solid impact behavior. //That's because the sweep does not directly generate contacts- it generates a time of impact estimate, and then the discrete contact generation //runs to create the actual contact manifold. That provides high quality contact positions and speculative depths. //If the ground that these boxes were smashing into was something like a mesh- which is infinitely thin- you may want to increase the sweep accuracy. - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(4 + 2 * j, 100 + (i + j) * 2, i * 2), new BodyVelocity { Linear = new Vector3(0, -150, 0) }, inertia, - new CollidableDescription(shapeIndex, ContinuousDetection.Continuous(1e-3f, 1e-2f, maximumSpeculativeMargin: 0.01f)), new BodyActivityDescription(0.01f))); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(4 + 2 * j, 100 + (i + j) * 2, i * 2), new(new Vector3(0, -150, 0)), inertia, + new(shapeIndex, ContinuousDetection.Continuous(1e-3f, 1e-2f, maximumSpeculativeMargin: 0.01f)), 0.01f)); } } rolloverInfo = new RolloverInfo(); diff --git a/Demos/Demos/Sponsors/SponsorCharacterAI.cs b/Demos/Demos/Sponsors/SponsorCharacterAI.cs index 5c32d409f..d10929be4 100644 --- a/Demos/Demos/Sponsors/SponsorCharacterAI.cs +++ b/Demos/Demos/Sponsors/SponsorCharacterAI.cs @@ -15,7 +15,7 @@ public struct SponsorCharacterAI Vector2 targetLocation; public SponsorCharacterAI(CharacterControllers characters, in CollidableDescription characterCollidable, in Vector3 initialPosition, in Vector2 targetLocation) { - bodyHandle = characters.Simulation.Bodies.Add(BodyDescription.CreateDynamic(initialPosition, new BodyInertia { InverseMass = 1f }, characterCollidable, new BodyActivityDescription(-1f))); + bodyHandle = characters.Simulation.Bodies.Add(BodyDescription.CreateDynamic(initialPosition, new BodyInertia { InverseMass = 1f }, characterCollidable, -1f)); ref var character = ref characters.AllocateCharacter(bodyHandle); character.LocalUp = new Vector3(0, 1, 0); diff --git a/Demos/Demos/Tanks/Tank.cs b/Demos/Demos/Tanks/Tank.cs index c6cc3b444..46691c1ea 100644 --- a/Demos/Demos/Tanks/Tank.cs +++ b/Demos/Demos/Tanks/Tank.cs @@ -165,9 +165,9 @@ public BodyHandle Fire(Simulation simulation, CollidableProperty properties, BufferPool pool) { //When the tank explodes, we just remove all the binding constraints and let it fall apart and reset body properties. - for (int i =0; i < WheelHandles.Length; ++i) + for (int i = 0; i < WheelHandles.Length; ++i) { ClearBodyProperties(ref properties[WheelHandles[i]]); } @@ -416,7 +416,7 @@ public void Explode(Simulation simulation, CollidableProperty(8, BufferPool); points.AllocateUnsafely() = new Vector3(0, 0, 0); @@ -96,7 +96,7 @@ public override void Initialize(ContentArchive content, Camera camera) points.AllocateUnsafely() = new Vector3(2, 2, 2); var convexHull = new ConvexHull(points, BufferPool, out _); convexHull.ComputeInertia(1, out var convexHullInertia); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(23, 10, 0), convexHullInertia, new CollidableDescription(Simulation.Shapes.Add(convexHull), ContinuousDetection.Discrete(10, 10)), -1)); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(23, 10, 0), convexHullInertia, new(Simulation.Shapes.Add(convexHull), ContinuousDetection.Discrete(10, 10)), -1)); //var sphere = new Sphere(1.5f); //var capsule = new Capsule(1f, 1f); diff --git a/Demos/SpecializedTests/TriangleTestDemo.cs b/Demos/SpecializedTests/TriangleTestDemo.cs index 9041bd545..6f4627bba 100644 --- a/Demos/SpecializedTests/TriangleTestDemo.cs +++ b/Demos/SpecializedTests/TriangleTestDemo.cs @@ -131,20 +131,20 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5, 0), Simulation.Shapes.Add(new Box(200, 5, 200)))); Simulation.Statics.Add(new StaticDescription(new Vector3(10, -2, 30), Simulation.Shapes.Add(new Box(10, 5, 10)))); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 2, 0), new BodyInertia { InverseMass = 1 }, new CollidableDescription(Simulation.Shapes.Add(new Sphere(1.75f)), ContinuousDetection.Discrete(0.1f, 0.1f)), -1)); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new RigidPose(new Vector3(20, 2, 3), Quaternion.CreateFromYawPitchRoll(0f, 1.745329E-05f, 0f)), new BodyInertia { InverseMass = 1 }, new CollidableDescription(Simulation.Shapes.Add(new Capsule(1, 2)), ContinuousDetection.Discrete(0.1f, 0.1f)), -1)); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 2, 0), new BodyInertia { InverseMass = 1 }, new(Simulation.Shapes.Add(new Sphere(1.75f)), ContinuousDetection.Discrete(0.1f, 0.1f)), -1)); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new RigidPose(new Vector3(20, 2, 3), Quaternion.CreateFromYawPitchRoll(0f, 1.745329E-05f, 0f)), new BodyInertia { InverseMass = 1 }, new(Simulation.Shapes.Add(new Capsule(1, 2)), ContinuousDetection.Discrete(0.1f, 0.1f)), -1)); var testBox = new Box(2, 3, 2); testBox.ComputeInertia(1, out var testBoxInertia); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 2, 6), testBoxInertia, new CollidableDescription(Simulation.Shapes.Add(testBox), ContinuousDetection.Discrete(10.1f, 10.1f)), new BodyActivityDescription(-1))); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 2, 6), testBoxInertia, new(Simulation.Shapes.Add(testBox), ContinuousDetection.Discrete(10.1f, 10.1f)), -1)); var cylinder = new Cylinder(1.75f, 2); cylinder.ComputeInertia(1, out var cylinderInertia); //cylinderInertia.InverseInertiaTensor = default; - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new RigidPose(new Vector3(20, 2, 9), Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), MathF.PI / 2f)), cylinderInertia, new CollidableDescription(Simulation.Shapes.Add(cylinder), ContinuousDetection.Discrete(5f, 5f)), -1)); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new RigidPose(new Vector3(20, 2, 9), Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), MathF.PI / 2f)), cylinderInertia, new(Simulation.Shapes.Add(cylinder), ContinuousDetection.Discrete(5f, 5f)), -1)); var cylinder2 = new Cylinder(.5f, 0.5f); cylinder2.ComputeInertia(1, out var cylinder2Inertia); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new RigidPose(new Vector3(23, 2, 9), Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), 0)), cylinder2Inertia, new CollidableDescription(Simulation.Shapes.Add(cylinder2), ContinuousDetection.Discrete(5f, 5f)), -1)); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new RigidPose(new Vector3(23, 2, 9), Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), 0)), cylinder2Inertia, new(Simulation.Shapes.Add(cylinder2), ContinuousDetection.Discrete(5f, 5f)), -1)); var points = new QuickList(8, BufferPool); points.AllocateUnsafely() = new Vector3(0, 0, 0); points.AllocateUnsafely() = new Vector3(0, 0, 2); @@ -156,8 +156,8 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) points.AllocateUnsafely() = new Vector3(2, 2, 2); var convexHull = new ConvexHull(points, BufferPool, out _); convexHull.ComputeInertia(1, out var convexHullInertia); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 2, 12), convexHullInertia, new CollidableDescription(Simulation.Shapes.Add(convexHull), ContinuousDetection.Discrete(0.1f, 0.1f)), -1)); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(23, 2, 12), convexHullInertia, new CollidableDescription(Simulation.Shapes.Add(convexHull), ContinuousDetection.Discrete(0.1f, 0.1f)), -1)); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 2, 12), convexHullInertia, new(Simulation.Shapes.Add(convexHull), ContinuousDetection.Discrete(0.1f, 0.1f)), -1)); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(23, 2, 12), convexHullInertia, new(Simulation.Shapes.Add(convexHull), ContinuousDetection.Discrete(0.1f, 0.1f)), -1)); CompoundBuilder builder = new CompoundBuilder(BufferPool, Simulation.Shapes, 2); builder.Add(new Box(1, 1, 1), RigidPose.Identity, 1); @@ -165,7 +165,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) builder.BuildDynamicCompound(out var children, out var compoundInertia); //compoundInertia.InverseInertiaTensor = default; var compound = new Compound(children); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 3, 14), compoundInertia, new CollidableDescription(Simulation.Shapes.Add(compound), ContinuousDetection.Discrete(10.1f, 10.1f)), -1)); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 3, 14), compoundInertia, new(Simulation.Shapes.Add(compound), ContinuousDetection.Discrete(10.1f, 10.1f)), -1)); { var triangles = new QuickList(4, BufferPool); @@ -189,7 +189,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) mesh.ComputeClosedInertia(1, out var newtInertia); for (int i = 0; i < 5; ++i) { - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(-20, 5 + i * 5, 0), newtInertia, collidable, new BodyActivityDescription(-1e-2f))); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(-20, 5 + i * 5, 0), newtInertia, collidable, -1e-2f)); } { @@ -201,7 +201,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) triangles.AllocateUnsafely() = new Triangle { A = v0, B = v2, C = v1 }; triangles.AllocateUnsafely() = new Triangle { A = v1, B = v2, C = v3 }; var testMesh = new Mesh(triangles, Vector3.One, BufferPool); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(22, -2.5f, 0), new BodyInertia { InverseMass = 1 }, new CollidableDescription(Simulation.Shapes.Add(testMesh), ContinuousDetection.Discrete(10.1f, 10.1f)), -1f)); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(22, -2.5f, 0), new BodyInertia { InverseMass = 1 }, new(Simulation.Shapes.Add(testMesh), ContinuousDetection.Discrete(10.1f, 10.1f)), -1f)); } } } From 42d12b1b3bdf72b36a203c82c3075230017b20a3 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 2 Dec 2021 16:43:50 -0600 Subject: [PATCH 301/947] INarrowPhaseCallbacks now exposes a pair's speculative margin as a ref parameter in AllowContactGeneration. --- .../INarrowPhaseCallbacks.cs | 11 ++- BepuPhysics/CollisionDetection/NarrowPhase.cs | 67 ++++++++++++------- BepuPhysics/IslandAwakener.cs | 2 +- BepuPhysics/StaticReference.cs | 38 +++-------- BepuPhysics/Statics.cs | 25 +++++-- Demos/DemoCallbacks.cs | 2 +- Demos/Demos/BouncinessDemo.cs | 2 +- Demos/Demos/Cars/CarCallbacks.cs | 2 +- .../CharacterNarrowphaseCallbacks.cs | 2 +- Demos/Demos/ClothDemo.cs | 2 +- Demos/Demos/CollisionQueryDemo.cs | 2 +- Demos/Demos/ContactEventsDemo.cs | 2 +- Demos/Demos/NewtDemo.cs | 2 +- Demos/Demos/RagdollDemo.cs | 2 +- Demos/Demos/RayCastingDemo.cs | 2 +- Demos/Demos/RopeTwistDemo.cs | 2 +- Demos/Demos/SimpleSelfContainedDemo.cs | 11 ++- Demos/Demos/Tanks/TankCallbacks.cs | 2 +- .../CompoundCollisionIndicesTest.cs | 2 +- Demos/SpecializedTests/VolumeQueryTests.cs | 2 +- 20 files changed, 103 insertions(+), 79 deletions(-) diff --git a/BepuPhysics/CollisionDetection/INarrowPhaseCallbacks.cs b/BepuPhysics/CollisionDetection/INarrowPhaseCallbacks.cs index edcf29859..6123fe8c0 100644 --- a/BepuPhysics/CollisionDetection/INarrowPhaseCallbacks.cs +++ b/BepuPhysics/CollisionDetection/INarrowPhaseCallbacks.cs @@ -7,6 +7,9 @@ namespace BepuPhysics.CollisionDetection { + /// + /// Material properties governing the interaction between colliding bodies. Used by the narrow phase to create constraints of the appropriate configuration. + /// public struct PairMaterialProperties { /// @@ -23,6 +26,9 @@ public struct PairMaterialProperties public SpringSettings SpringSettings; } + /// + /// Defines handlers for narrow phase events. + /// public unsafe interface INarrowPhaseCallbacks { /// @@ -37,8 +43,10 @@ public unsafe interface INarrowPhaseCallbacks /// Index of the worker that identified the overlap. /// Reference to the first collidable in the pair. /// Reference to the second collidable in the pair. + /// Reference to the speculative margin used by the pair. + /// The value was already initialized by the narrowphase by examining the speculative margins of the involved collidables, but it can be modified. /// True if collision detection should proceed, false otherwise. - bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b); + bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin); /// @@ -58,6 +66,7 @@ public unsafe interface INarrowPhaseCallbacks /// /// Chooses whether to allow contact generation to proceed for the children of two overlapping collidables in a compound-including pair. /// + /// Index of the worker thread processing this pair. /// Parent pair of the two child collidables. /// Index of the child of collidable A in the pair. If collidable A is not compound, then this is always 0. /// Index of the child of collidable B in the pair. If collidable B is not compound, then this is always 0. diff --git a/BepuPhysics/CollisionDetection/NarrowPhase.cs b/BepuPhysics/CollisionDetection/NarrowPhase.cs index 52456ea9f..e535ee792 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhase.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhase.cs @@ -405,27 +405,54 @@ public unsafe void HandleOverlap(int workerIndex, CollidableReference a, Collida Debug.Assert(a.Packed != b.Packed, "Excuse me, broad phase, but an object cannot collide with itself!"); SortCollidableReferencesForPair(a, b, out var aMobility, out var bMobility, out a, out b); Debug.Assert(aMobility != CollidableMobility.Static || bMobility != CollidableMobility.Static, "Broad phase should not be able to generate static-static pairs."); - if (!Callbacks.AllowContactGeneration(workerIndex, a, b)) + + //Two static pairs are impossible (the broad phase doesn't test stuff in the static/sleeping tree against itself), and any pair with a static will put the body in slot A. + var twoBodies = bMobility != CollidableMobility.Static; + ref var bodyLocationA = ref Bodies.HandleToLocation[a.BodyHandle.Value]; + ref var setA = ref Bodies.Sets[bodyLocationA.SetIndex]; + ref var stateA = ref setA.SolverStates[bodyLocationA.Index]; + ref var collidableA = ref setA.Collidables[bodyLocationA.Index]; + ContinuousDetection continuityB; + float speculativeMarginB; + if (twoBodies) + { + ref var bodyLocationB = ref Bodies.HandleToLocation[b.BodyHandle.Value]; + ref var collidableB = ref Bodies.Sets[bodyLocationB.SetIndex].Collidables[bodyLocationB.Index]; + continuityB = collidableB.Continuity; + speculativeMarginB = collidableB.SpeculativeMargin; + } + else + { + //Slot B is a static. + ref var staticB = ref Statics.GetDirectReference(b.StaticHandle); + continuityB = staticB.Continuity; + speculativeMarginB = 0; + } + + //Add the speculative margins, but try to obey both collidables' bounds. Note that in the case of nonoverlapping intervals, the higher min ends up used. + var speculativeMargin = + MathF.Max(collidableA.Continuity.MinimumSpeculativeMargin, MathF.Max(continuityB.MinimumSpeculativeMargin, + MathF.Min(collidableA.Continuity.MaximumSpeculativeMargin, MathF.Min(continuityB.MaximumSpeculativeMargin, + collidableA.SpeculativeMargin + speculativeMarginB)))); + + //By precalculating the speculative margin, we give the narrow phase callbacks the option of modifying it. + if (!Callbacks.AllowContactGeneration(workerIndex, a, b, ref speculativeMargin)) return; ref var overlapWorker = ref overlapWorkers[workerIndex]; var pair = new CollidablePair(a, b); - if (aMobility != CollidableMobility.Static && bMobility != CollidableMobility.Static) + if (twoBodies) { //Both references are bodies. - ref var bodyLocationA = ref Bodies.HandleToLocation[a.BodyHandle.Value]; ref var bodyLocationB = ref Bodies.HandleToLocation[b.BodyHandle.Value]; Debug.Assert(bodyLocationA.SetIndex == 0 || bodyLocationB.SetIndex == 0, "One of the two bodies must be active. Otherwise, something is busted!"); - ref var setA = ref Bodies.Sets[bodyLocationA.SetIndex]; ref var setB = ref Bodies.Sets[bodyLocationB.SetIndex]; - ref var stateA = ref setA.SolverStates[bodyLocationA.Index]; ref var stateB = ref setB.SolverStates[bodyLocationB.Index]; - ref var collidableA = ref setA.Collidables[bodyLocationA.Index]; ref var collidableB = ref setB.Collidables[bodyLocationB.Index]; AddBatchEntries(workerIndex, ref overlapWorker, ref pair, ref collidableA.Continuity, ref collidableB.Continuity, - collidableA.SpeculativeMargin, collidableB.SpeculativeMargin, collidableA.Shape, collidableB.Shape, collidableA.BroadPhaseIndex, collidableB.BroadPhaseIndex, + speculativeMargin, ref stateA.Motion.Pose, ref stateB.Motion.Pose, ref stateA.Motion.Velocity, ref stateB.Motion.Velocity); } @@ -437,21 +464,17 @@ public unsafe void HandleOverlap(int workerIndex, CollidableReference a, Collida Debug.Assert(aMobility != CollidableMobility.Static && bMobility == CollidableMobility.Static); ref var bodyLocation = ref Bodies.HandleToLocation[a.BodyHandle.Value]; Debug.Assert(bodyLocation.SetIndex == 0, "The body of a body-static pair must be active."); - var staticIndex = Statics.HandleToIndex[b.StaticHandle.Value]; //TODO: Ideally, the compiler would see this and optimize away the relevant math in AddBatchEntries. That's a longshot, though. May want to abuse some generics to force it. var zeroVelocity = default(BodyVelocity); - ref var bodySet = ref Bodies.ActiveSet; - ref var bodyState = ref bodySet.SolverStates[bodyLocation.Index]; - ref var collidableA = ref bodySet.Collidables[bodyLocation.Index]; - ref var collidableB = ref Statics[staticIndex]; + ref var staticB = ref Statics.GetDirectReference(b.StaticHandle); AddBatchEntries(workerIndex, ref overlapWorker, ref pair, - ref collidableA.Continuity, ref collidableB.Continuity, - collidableA.SpeculativeMargin, 0, - collidableA.Shape, collidableB.Shape, - collidableA.BroadPhaseIndex, collidableB.BroadPhaseIndex, - ref bodyState.Motion.Pose, ref collidableB.Pose, - ref bodyState.Motion.Velocity, ref zeroVelocity); + ref collidableA.Continuity, ref staticB.Continuity, + collidableA.Shape, staticB.Shape, + collidableA.BroadPhaseIndex, staticB.BroadPhaseIndex, + speculativeMargin, + ref stateA.Motion.Pose, ref staticB.Pose, + ref stateA.Motion.Velocity, ref zeroVelocity); } } @@ -473,19 +496,13 @@ public bool AllowTest(int childA, int childB) private unsafe void AddBatchEntries(int workerIndex, ref OverlapWorker overlapWorker, ref CollidablePair pair, ref ContinuousDetection continuityA, ref ContinuousDetection continuityB, - float marginA, float marginB, TypedIndex shapeA, TypedIndex shapeB, int broadPhaseIndexA, int broadPhaseIndexB, + float speculativeMargin, ref RigidPose poseA, ref RigidPose poseB, ref BodyVelocity velocityA, ref BodyVelocity velocityB) { Debug.Assert(pair.A.Packed != pair.B.Packed); - //Add the speculative margins, but try to obey both collidables' bounds. Note that in the case of nonoverlapping intervals, the higher min ends up used. - var speculativeMargin = - MathF.Max(continuityA.MinimumSpeculativeMargin, MathF.Max(continuityB.MinimumSpeculativeMargin, - MathF.Min(continuityA.MaximumSpeculativeMargin, MathF.Min(continuityB.MaximumSpeculativeMargin, - marginA + marginB)))); - var allowExpansion = continuityA.AllowExpansionBeyondSpeculativeMargin | continuityB.AllowExpansionBeyondSpeculativeMargin; //Note that we pick float.MaxValue for the maximum bounds expansion passive-involving pairs. //This is a compromise- looser bounds are not a correctness issue, so we're trading off potentially more subpairs diff --git a/BepuPhysics/IslandAwakener.cs b/BepuPhysics/IslandAwakener.cs index 2d41f8339..58eec08bd 100644 --- a/BepuPhysics/IslandAwakener.cs +++ b/BepuPhysics/IslandAwakener.cs @@ -361,7 +361,7 @@ internal unsafe void ExecutePhaseTwoJob(int index) { if (movedLeaf.Mobility == Collidables.CollidableMobility.Static) { - statics[movedLeaf.StaticHandle].BroadPhaseIndex = staticBroadPhaseIndexToRemove; + statics.GetDirectReference(movedLeaf.StaticHandle).BroadPhaseIndex = staticBroadPhaseIndexToRemove; } else { diff --git a/BepuPhysics/StaticReference.cs b/BepuPhysics/StaticReference.cs index 7095aed3c..3a4ee18d9 100644 --- a/BepuPhysics/StaticReference.cs +++ b/BepuPhysics/StaticReference.cs @@ -49,47 +49,27 @@ public bool Exists /// /// Gets a the static's index in the statics collection. /// - public int Index - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - get { return Statics.HandleToIndex[Handle.Value]; } - } + public int Index => Statics.HandleToIndex[Handle.Value]; /// /// Gets a reference to the entirety of the static's memory. /// - public ref Static Static - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - get - { - return ref Statics[Handle]; - } - } + public ref Static Static => ref Statics.GetDirectReference(Handle); /// /// Gets a reference to the static's pose. /// - public ref RigidPose Pose - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - get - { - return ref Statics[Handle].Pose; - } - } + public ref RigidPose Pose => ref Statics.GetDirectReference(Handle).Pose; /// /// Gets a reference to the static's collision continuity settings. /// - public ref ContinuousDetection Continuity - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - get - { - return ref Statics[Handle].Continuity; - } - } + public ref ContinuousDetection Continuity => ref Statics.GetDirectReference(Handle).Continuity; + + /// + /// Gets the shape used by the static. To set the shape, use or . + /// + public TypedIndex Shape => Statics.GetDirectReference(Handle).Shape; /// /// Gets a CollidableReference for this static. CollidableReferences uniquely identify a collidable object in a simulation by including both the dynamic/kinematic/static state of the object and its handle. diff --git a/BepuPhysics/Statics.cs b/BepuPhysics/Statics.cs index 430b2dd64..512a15b75 100644 --- a/BepuPhysics/Statics.cs +++ b/BepuPhysics/Statics.cs @@ -112,19 +112,32 @@ public class Statics internal IslandAwakener awakener; /// - /// Gets a reference to the raw memory backing a static collidable. + /// Gets a reference to the memory backing a static collidable. The type is a helper that exposes common operations for statics. /// - /// Handle of the static to retrieve a memory reference for. - /// Direct reference to the memory backing a static collidable. - public ref Static this[StaticHandle handle] + /// Handle of the static to retrieve a reference for. + /// Reference to the memory backing a static collidable. + /// Equivalent to . + public StaticReference this[StaticHandle handle] { get { ValidateExistingHandle(handle); - return ref StaticsBuffer[HandleToIndex[handle.Value]]; + return new StaticReference(handle, this); } } + /// + /// Gets a direct reference to the memory backing a static. + /// + /// Handle of the static to get a reference of. + /// Direct reference to the memory backing a static. + /// This is distinct from the indexer in that this returns the direct memory reference. includes a layer of indirection that can expose more features. + public ref Static GetDirectReference(StaticHandle handle) + { + ValidateExistingHandle(handle); + return ref StaticsBuffer[HandleToIndex[handle.Value]]; + } + /// /// Gets a reference to the raw memory backing a static collidable. /// @@ -276,7 +289,7 @@ public void RemoveAt(int index, ref TAwakeningFilter filter) w if (movedLeaf.Mobility == CollidableMobility.Static) { //This is a static collidable, not a body. - this[movedLeaf.StaticHandle].BroadPhaseIndex = removedBroadPhaseIndex; + GetDirectReference(movedLeaf.StaticHandle).BroadPhaseIndex = removedBroadPhaseIndex; } else { diff --git a/Demos/DemoCallbacks.cs b/Demos/DemoCallbacks.cs index a730b41ae..ade975f92 100644 --- a/Demos/DemoCallbacks.cs +++ b/Demos/DemoCallbacks.cs @@ -125,7 +125,7 @@ public void Initialize(Simulation simulation) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b) + public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin) { //While the engine won't even try creating pairs between statics at all, it will ask about kinematic-kinematic pairs. //Those pairs cannot emit constraints since both involved bodies have infinite inertia. Since most of the demos don't need diff --git a/Demos/Demos/BouncinessDemo.cs b/Demos/Demos/BouncinessDemo.cs index 5624a88fa..a002a486b 100644 --- a/Demos/Demos/BouncinessDemo.cs +++ b/Demos/Demos/BouncinessDemo.cs @@ -27,7 +27,7 @@ public void Initialize(Simulation simulation) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b) + public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin) { //While the engine won't even try creating pairs between statics at all, it will ask about kinematic-kinematic pairs. //Those pairs cannot emit constraints since both involved bodies have infinite inertia. Since most of the demos don't need diff --git a/Demos/Demos/Cars/CarCallbacks.cs b/Demos/Demos/Cars/CarCallbacks.cs index 5f3bae299..b2fe2997a 100644 --- a/Demos/Demos/Cars/CarCallbacks.cs +++ b/Demos/Demos/Cars/CarCallbacks.cs @@ -24,7 +24,7 @@ public void Initialize(Simulation simulation) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b) + public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin) { //It's impossible for two statics to collide, and pairs are sorted such that bodies always come before statics. if (b.Mobility != CollidableMobility.Static) diff --git a/Demos/Demos/Characters/CharacterNarrowphaseCallbacks.cs b/Demos/Demos/Characters/CharacterNarrowphaseCallbacks.cs index 792c94609..6de26860c 100644 --- a/Demos/Demos/Characters/CharacterNarrowphaseCallbacks.cs +++ b/Demos/Demos/Characters/CharacterNarrowphaseCallbacks.cs @@ -19,7 +19,7 @@ public CharacterNarrowphaseCallbacks(CharacterControllers characters) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b) + public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin) { return a.Mobility == CollidableMobility.Dynamic || b.Mobility == CollidableMobility.Dynamic; } diff --git a/Demos/Demos/ClothDemo.cs b/Demos/Demos/ClothDemo.cs index 586f1f59e..c56912b02 100644 --- a/Demos/Demos/ClothDemo.cs +++ b/Demos/Demos/ClothDemo.cs @@ -57,7 +57,7 @@ public void Initialize(Simulation simulation) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b) + public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin) { if (a.Mobility != CollidableMobility.Static && b.Mobility != CollidableMobility.Static) { diff --git a/Demos/Demos/CollisionQueryDemo.cs b/Demos/Demos/CollisionQueryDemo.cs index b7d74c442..c099fd4d1 100644 --- a/Demos/Demos/CollisionQueryDemo.cs +++ b/Demos/Demos/CollisionQueryDemo.cs @@ -120,7 +120,7 @@ void GetPoseAndShape(CollidableReference reference, out RigidPose pose, out Type //Collidables can be associated with either bodies or statics. We have to look in a different place depending on which it is. if (reference.Mobility == CollidableMobility.Static) { - ref var collidable = ref Simulation.Statics[reference.StaticHandle]; + var collidable = Simulation.Statics[reference.StaticHandle]; pose = collidable.Pose; shapeIndex = collidable.Shape; } diff --git a/Demos/Demos/ContactEventsDemo.cs b/Demos/Demos/ContactEventsDemo.cs index 66f6d2931..f0a604295 100644 --- a/Demos/Demos/ContactEventsDemo.cs +++ b/Demos/Demos/ContactEventsDemo.cs @@ -603,7 +603,7 @@ public ContactEventCallbacks(ContactEvents events) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b) + public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin) { return true; } diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index 8075f12f2..c23e97276 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -535,7 +535,7 @@ public void Initialize(Simulation simulation) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b) + public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin) { if (a.Mobility == CollidableMobility.Dynamic && b.Mobility == CollidableMobility.Dynamic) { diff --git a/Demos/Demos/RagdollDemo.cs b/Demos/Demos/RagdollDemo.cs index 2b0acf10e..c47bfb839 100644 --- a/Demos/Demos/RagdollDemo.cs +++ b/Demos/Demos/RagdollDemo.cs @@ -101,7 +101,7 @@ public void Initialize(Simulation simulation) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b) + public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin) { //It's impossible for two statics to collide, and pairs are sorted such that bodies always come before statics. if (b.Mobility != CollidableMobility.Static) diff --git a/Demos/Demos/RayCastingDemo.cs b/Demos/Demos/RayCastingDemo.cs index 591cd9d32..50ca14407 100644 --- a/Demos/Demos/RayCastingDemo.cs +++ b/Demos/Demos/RayCastingDemo.cs @@ -30,7 +30,7 @@ public void Initialize(Simulation simulation) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b) + public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin) { return false; } diff --git a/Demos/Demos/RopeTwistDemo.cs b/Demos/Demos/RopeTwistDemo.cs index d64f7c364..4b00f5c82 100644 --- a/Demos/Demos/RopeTwistDemo.cs +++ b/Demos/Demos/RopeTwistDemo.cs @@ -41,7 +41,7 @@ public void Initialize(Simulation simulation) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b) + public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin) { var aFilter = Filters[a]; var bFilter = Filters[b]; diff --git a/Demos/Demos/SimpleSelfContainedDemo.cs b/Demos/Demos/SimpleSelfContainedDemo.cs index 520bebe4b..d69727814 100644 --- a/Demos/Demos/SimpleSelfContainedDemo.cs +++ b/Demos/Demos/SimpleSelfContainedDemo.cs @@ -40,9 +40,11 @@ public void Initialize(Simulation simulation) /// Index of the worker that identified the overlap. /// Reference to the first collidable in the pair. /// Reference to the second collidable in the pair. + /// Reference to the speculative margin used by the pair. + /// The value was already initialized by the narrowphase by examining the speculative margins of the involved collidables, but it can be modified. /// True if collision detection should proceed, false otherwise. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b) + public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin) { //Before creating a narrow phase pair, the broad phase asks this callback whether to bother with a given pair of objects. //This can be used to implement arbitrary forms of collision filtering. See the RagdollDemo or NewtDemo for examples. @@ -50,12 +52,16 @@ public bool AllowContactGeneration(int workerIndex, CollidableReference a, Colli //The engine won't generate static-static pairs, but it will generate kinematic-kinematic pairs. //That's useful if you're trying to make some sort of sensor/trigger object, but since kinematic-kinematic pairs //can't generate constraints (both bodies have infinite inertia), simple simulations can just ignore such pairs. + + //This function also exposes the speculative margin. It can be validly written to, but that is a very rare use case. + //Most of the time, you can ignore this function's speculativeMargin parameter entirely. return a.Mobility == CollidableMobility.Dynamic || b.Mobility == CollidableMobility.Dynamic; } /// /// Chooses whether to allow contact generation to proceed for the children of two overlapping collidables in a compound-including pair. /// + /// Index of the worker thread processing this pair. /// Parent pair of the two child collidables. /// Index of the child of collidable A in the pair. If collidable A is not compound, then this is always 0. /// Index of the child of collidable B in the pair. If collidable B is not compound, then this is always 0. @@ -67,8 +73,7 @@ public bool AllowContactGeneration(int workerIndex, CollidableReference a, Colli [MethodImpl(MethodImplOptions.AggressiveInlining)] public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB) { - //This is similar to the top level broad phase callback above. It's called by the narrow phase before generating - //subpairs between children in parent shapes. + //This is similar to the top level broad phase callback above. It's called by the narrow phase before generating subpairs between children in parent shapes. //This only gets called in pairs that involve at least one shape type that can contain multiple children, like a Compound. return true; } diff --git a/Demos/Demos/Tanks/TankCallbacks.cs b/Demos/Demos/Tanks/TankCallbacks.cs index 34860db1f..f6de0b82b 100644 --- a/Demos/Demos/Tanks/TankCallbacks.cs +++ b/Demos/Demos/Tanks/TankCallbacks.cs @@ -60,7 +60,7 @@ public void Initialize(Simulation simulation) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b) + public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin) { //It's impossible for two statics to collide, and pairs are sorted such that bodies always come before statics. if (b.Mobility != CollidableMobility.Static) diff --git a/Demos/SpecializedTests/CompoundCollisionIndicesTest.cs b/Demos/SpecializedTests/CompoundCollisionIndicesTest.cs index 80c2e931a..6913f1f6f 100644 --- a/Demos/SpecializedTests/CompoundCollisionIndicesTest.cs +++ b/Demos/SpecializedTests/CompoundCollisionIndicesTest.cs @@ -16,7 +16,7 @@ namespace Demos.SpecializedTests public unsafe struct IndexReportingNarrowPhaseCallbacks : INarrowPhaseCallbacks { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b) + public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin) { return true; } diff --git a/Demos/SpecializedTests/VolumeQueryTests.cs b/Demos/SpecializedTests/VolumeQueryTests.cs index 0f776f7dc..b776f85ec 100644 --- a/Demos/SpecializedTests/VolumeQueryTests.cs +++ b/Demos/SpecializedTests/VolumeQueryTests.cs @@ -28,7 +28,7 @@ public void Initialize(Simulation simulation) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b) + public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin) { return false; } From 8d24899e7a485814e26b744d9ea5ba5115cefe98 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 2 Dec 2021 22:29:45 -0600 Subject: [PATCH 302/947] Disabled determinism sort; no longer needed without body layout optimizer. --- BepuPhysics/IslandSleeper.cs | 25 ------------------------- 1 file changed, 25 deletions(-) diff --git a/BepuPhysics/IslandSleeper.cs b/BepuPhysics/IslandSleeper.cs index b10502b10..024552914 100644 --- a/BepuPhysics/IslandSleeper.cs +++ b/BepuPhysics/IslandSleeper.cs @@ -954,31 +954,6 @@ internal void Update(IThreadDispatcher threadDispatcher, bool deterministic) } ++scheduleOffset; - - if (deterministic) - { - //The order in which sleeps occur affects the result of the simulation. To ensure determinism, we need to pin the sleep order to something - //which is deterministic. We will use the handle associated with each active body as the order provider. - pool.Take(bodies.ActiveSet.Count, out var sortedIndices); - for (int i = 0; i < bodies.ActiveSet.Count; ++i) - { - sortedIndices[i] = i; - } - //Handles are guaranteed to be unique; no need for three way partitioning. - HandleComparer comparer; - comparer.Handles = bodies.ActiveSet.IndexToHandle; - //TODO: This sort might end up being fairly expensive. On a very large simulation, it might even amount to 5% of the simulation time. - //It would be nice to come up with a better solution here. Some options include other sources of determinism, hiding the sort, and possibly enumerating directly over handles. - QuickSort.Sort(ref sortedIndices[0], 0, bodies.ActiveSet.Count - 1, ref comparer); - - //Now that we have a sorted set of indices, we have eliminated nondeterminism related to memory layout. The initial target body indices can be remapped onto the sorted list. - for (int i = 0; i < traversalStartBodyIndices.Count; ++i) - { - traversalStartBodyIndices[i] = sortedIndices[traversalStartBodyIndices[i]]; - Debug.Assert(traversalStartBodyIndices[i] >= 0 && traversalStartBodyIndices[i] < bodies.ActiveSet.Count); - } - pool.Return(ref sortedIndices); - } Sleep(ref traversalStartBodyIndices, threadDispatcher, deterministic, (int)Math.Ceiling(bodies.ActiveSet.Count * TargetSleptFraction), (int)Math.Ceiling(bodies.ActiveSet.Count * TargetTraversedFraction), false); traversalStartBodyIndices.Dispose(pool); From 7b4348691d83f5ff178c922e88e7682f99ff8b0f Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 3 Dec 2021 13:17:33 -0600 Subject: [PATCH 303/947] Purged BodyLayoutOptimizer and ConstraintLayoutOptimizer; they are no longer net wins in the embedded substepping model. --- BepuPhysics/BodyLayoutOptimizer.cs | 136 ------- BepuPhysics/BodySet.cs | 18 - BepuPhysics/ConstraintLayoutOptimizer.cs | 372 ------------------ BepuPhysics/Simulation.cs | 19 +- BepuPhysics/Solver.cs | 23 -- Demos/DemoHarness.cs | 3 - Demos/Program.cs | 2 +- Demos/SimulationTimeSamples.cs | 8 - .../SpecializedTests/SimulationScrambling.cs | 17 - 9 files changed, 5 insertions(+), 593 deletions(-) delete mode 100644 BepuPhysics/BodyLayoutOptimizer.cs delete mode 100644 BepuPhysics/ConstraintLayoutOptimizer.cs diff --git a/BepuPhysics/BodyLayoutOptimizer.cs b/BepuPhysics/BodyLayoutOptimizer.cs deleted file mode 100644 index 6390cb5b6..000000000 --- a/BepuPhysics/BodyLayoutOptimizer.cs +++ /dev/null @@ -1,136 +0,0 @@ -using System.Runtime.CompilerServices; -using System; -using System.Diagnostics; -using BepuUtilities.Memory; -using BepuUtilities.Collections; -using System.Runtime.InteropServices; -using System.Threading; -using BepuUtilities; -using BepuPhysics.Collidables; -using BepuPhysics.CollisionDetection; - -namespace BepuPhysics -{ - /// - /// Incrementally changes the layout of a set of bodies to minimize the cache misses associated with the solver and other systems that rely on connection following. - /// - public partial class BodyLayoutOptimizer - { - Bodies bodies; - BroadPhase broadPhase; - Solver solver; - - float optimizationFraction; - /// - /// Gets or sets the fraction of all bodies to update each frame. - /// - public float OptimizationFraction - { - get - { - return optimizationFraction; - } - set - { - if (value > 1 || value < 0) - throw new ArgumentException("Optimization fraction must be a value from 0 to 1."); - optimizationFraction = value; - } - } - - public BodyLayoutOptimizer(Bodies bodies, BroadPhase broadPhase, Solver solver, BufferPool pool, float optimizationFraction = 0.005f) - { - this.bodies = bodies; - this.broadPhase = broadPhase; - this.solver = solver; - OptimizationFraction = optimizationFraction; - - } - - public static void SwapBodyLocation(Bodies bodies, Solver solver, int a, int b) - { - Debug.Assert(a != b, "Swapping a body with itself isn't meaningful. Whaddeyer doin?"); - //Enumerate the bodies' current set of constraints, changing the reference in each to the new location. - //Note that references to both bodies must be changed- both bodies moved! - //This function does not update the actual position of the list in the graph, so we can modify both without worrying about invalidating indices. - solver.UpdateForBodyMemorySwap(a, b); - - //Update the body locations. - bodies.ActiveSet.Swap(a, b, ref bodies.HandleToLocation); - //TODO: If the body layout optimizer occurs before or after all other stages, this swap isn't required. If we move it in between other stages though, we need to keep the inertia - //coherent with the other body properties. - //Helpers.Swap(ref bodies.Inertias[a], ref bodies.Inertias[b]); - } - - int nextBodyIndex = 0; - - struct IncrementalEnumerator : IForEach - { - public Bodies bodies; - public BroadPhase broadPhase; - public Solver solver; - public int Index; - public int TargetIndexStart; - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void LoopBody(int connectedBodyIndex) - { - ++Index; - if (Index > 32) - return; - //Only pull bodies over that are to the right. This helps limit pointless fighting. - //With this condition, objects within an island will tend to move towards the position of the leftmost body. - //Without it, any progress towards island-level convergence could be undone by the next iteration. - var newLocationIndex = TargetIndexStart + Index; - if (connectedBodyIndex > newLocationIndex) - { - //Note that we update the memory location immediately. This could affect the next loop iteration. - //But this is fine; the next iteration will load from that modified data and everything will remain consistent. - - //TODO: this implementation can almost certainly be improved- - //this version goes through all the effort of diving into the type batches for references, then does it all again to move stuff around. - //A hardcoded swapping operation could do both at once, saving a few indirections. - //It won't be THAT much faster- every single indirection is already cached. - //Also, before you do that sort of thing, remember how short this stage is. - //Note that graph.EnumerateConnectedBodies explicitly excludes the body whose constraints we are enumerating, - //so we don't have to worry about having the rug pulled by this list swap. - //(Also, !(x > x) for many values of x.) - SwapBodyLocation(bodies, solver, connectedBodyIndex, newLocationIndex); - } - } - } - public void IncrementalOptimize() - { - //All this does is look for any bodies which are to the right of a given body. If it finds one, it pulls it to be adjacent. - //This converges at the island level- that is, running this on a static topology of simulation islands will eventually result in - //the islands being contiguous in memory, and at least some connected bodies being adjacent to each other. - //However, within the islands, it may continue to unnecessarily swap objects around as bodies 'fight' for ownership. - //One body doesn't know that another body has already claimed a body as a child, so this can't produce a coherent unique traversal order. - //(In fact, it won't generally converge even with a single one dimensional chain of bodies.) - - //This optimization routine requires much less overhead than other options, like full island traversals. We only request the connections of a single body, - //and the swap count is limited to the number of connected bodies. - - //Don't bother optimizing if no optimizations can be performed. This condition is assumed during worker execution. - if (bodies.ActiveSet.Count <= 2) - return; - int optimizationCount = (int)Math.Max(1, Math.Round(bodies.ActiveSet.Count * optimizationFraction)); - for (int i = 0; i < optimizationCount; ++i) - { - //No point trying to optimize the last two bodies. No optimizations are possible. - if (nextBodyIndex >= bodies.ActiveSet.Count - 2) - nextBodyIndex = 0; - - var enumerator = new IncrementalEnumerator(); - enumerator.bodies = bodies; - enumerator.broadPhase = broadPhase; - enumerator.solver = solver; - enumerator.TargetIndexStart = nextBodyIndex + 1; - enumerator.Index = 0; - bodies.EnumerateConnectedBodyIndices(nextBodyIndex, ref enumerator); - - ++nextBodyIndex; - } - - } - } -} diff --git a/BepuPhysics/BodySet.cs b/BepuPhysics/BodySet.cs index c5edbc7f7..9d348ccac 100644 --- a/BepuPhysics/BodySet.cs +++ b/BepuPhysics/BodySet.cs @@ -225,24 +225,6 @@ public bool BodyIsConstrainedBy(int bodyIndex, ConstraintHandle constraintHandle return false; } - - /// - /// Swaps the memory of two bodies. Indexed by memory slot, not by handle index. - /// - /// Memory slot of the first body to swap. - /// Memory slot of the second body to swap. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - internal void Swap(int slotA, int slotB, ref Buffer handleToIndex) - { - handleToIndex[IndexToHandle[slotA].Value].Index = slotB; - handleToIndex[IndexToHandle[slotB].Value].Index = slotA; - Helpers.Swap(ref IndexToHandle[slotA], ref IndexToHandle[slotB]); - Helpers.Swap(ref Collidables[slotA], ref Collidables[slotB]); - Helpers.Swap(ref SolverStates[slotA], ref SolverStates[slotB]); - Helpers.Swap(ref Activity[slotA], ref Activity[slotB]); - Helpers.Swap(ref Constraints[slotA], ref Constraints[slotB]); - } - internal unsafe void InternalResize(int targetBodyCapacity, BufferPool pool) { Debug.Assert(targetBodyCapacity > 0, "Resize is not meant to be used as Dispose. If you want to return everything to the pool, use Dispose instead."); diff --git a/BepuPhysics/ConstraintLayoutOptimizer.cs b/BepuPhysics/ConstraintLayoutOptimizer.cs deleted file mode 100644 index 0a081345f..000000000 --- a/BepuPhysics/ConstraintLayoutOptimizer.cs +++ /dev/null @@ -1,372 +0,0 @@ -using BepuUtilities; -using BepuUtilities.Collections; -using BepuUtilities.Memory; -using BepuPhysics.Constraints; -using System; -using System.Diagnostics; -using System.Linq; -using System.Numerics; -using System.Runtime.CompilerServices; -using System.Threading; - -namespace BepuPhysics -{ - public class ConstraintLayoutOptimizer - { - Bodies bodies; - Solver solver; - struct Optimization - { - /// - /// Index of the target constraint bundle to optimize. - /// - public int BundleIndex; - /// - /// Index of the last optimized type batch. - /// - public int TypeBatchIndex; - /// - /// Index of the last optimized batch. - /// - public int BatchIndex; - - } - - Optimization nextTarget; - - /// - /// If true, regions are offset by a half region width. Toggled each frame. Offsets allow the sorted regions to intermix, eventually converging to a full sort. - /// - bool shouldOffset; - - float optimizationFraction; - public float OptimizationFraction - { - get { return optimizationFraction; } - set - { - if (value < 0 || value > 1) - throw new ArgumentException("Optimization fraction must be from 0 to 1."); - optimizationFraction = value; - } - } - - Action generateSortKeysDelegate; - Action regatherDelegate; - Action copyToCacheAndSortDelegate; - - public ConstraintLayoutOptimizer(Bodies bodies, Solver solver, float optimizationFraction = 0.044f) - { - this.bodies = bodies; - this.solver = solver; - OptimizationFraction = optimizationFraction; - - generateSortKeysDelegate = GenerateSortKeys; - regatherDelegate = Regather; - copyToCacheAndSortDelegate = CopyToCacheAndSort; - } - - void Wrap(ref Optimization o) - { - ref var activeSet = ref solver.ActiveSet; - Debug.Assert(activeSet.Batches.Count > 0, "Shouldn't be trying to optimize zero constraints."); - while (true) - { - if (o.BatchIndex >= activeSet.Batches.Count) - { - o = new Optimization(); - } - else if (o.TypeBatchIndex >= activeSet.Batches[o.BatchIndex].TypeBatches.Count) - { - //It's possible that batches prior to the last constraint batch lack constraints. In that case, try the next one. - ++o.BatchIndex; - o.TypeBatchIndex = 0; - o.BundleIndex = 0; - } - else if (o.BundleIndex >= activeSet.Batches[o.BatchIndex].TypeBatches[o.TypeBatchIndex].BundleCount) - { - ++o.TypeBatchIndex; - o.BundleIndex = 0; - } - else - { - break; - } - } - } - - Optimization FindOffsetFrameStart(Optimization o, int maximumRegionSizeInBundles) - { - Wrap(ref o); - - ref var activeSet = ref solver.ActiveSet; - var spaceRemaining = activeSet.Batches[o.BatchIndex].TypeBatches[o.TypeBatchIndex].BundleCount - o.BundleIndex; - if (spaceRemaining <= maximumRegionSizeInBundles) - { - ++o.TypeBatchIndex; - Wrap(ref o); - } - //Note that the bundle count is not cached; the above type batch may differ. - o.BundleIndex = Math.Max(0, - Math.Min( - o.BundleIndex + maximumRegionSizeInBundles / 2, - activeSet.Batches[o.BatchIndex].TypeBatches[o.TypeBatchIndex].BundleCount - maximumRegionSizeInBundles)); - - return o; - } - - public unsafe void Update(BufferPool bufferPool, IThreadDispatcher threadDispatcher = null) - { - //TODO: It's possible that the cost associated with setting up multithreading exceeds the cost of the actual optimization for smaller simulations. - //You might want to fall back to single threaded based on some empirical testing. - //No point in optimizing if there are no constraints- this is a necessary test since we assume that 0 is a valid batch index later. - ref var activeSet = ref solver.ActiveSet; - if (activeSet.Batches.Count == 0) - return; - var regionSizeInBundles = (int)Math.Max(2, Math.Round(activeSet.BundleCount * optimizationFraction)); - //The region size in bundles should be divisible by two so that it can be offset by half. - if ((regionSizeInBundles & 1) == 1) - ++regionSizeInBundles; - //Note that we require that all regions are bundle aligned. This is important for the typebatch sorting process, which tends to use bulk copies from bundle arrays to cache. - //If not bundle aligned, those bulk copies would become complex due to the constraint AOSOA layout. - - Optimization target; - if (shouldOffset) - { - //Use the previous frame's start to create the new target. - target = FindOffsetFrameStart(nextTarget, regionSizeInBundles); - Debug.Assert(activeSet.Batches[target.BatchIndex].TypeBatches[target.TypeBatchIndex].BundleCount <= regionSizeInBundles || target.BundleIndex != 0, - "On offset frames, the only time a target bundle can be 0 is if the batch is too small for it to be anything else."); - //Console.WriteLine($"Offset frame targeting {target.BatchIndex}.{target.TypeBatchIndex}:{target.BundleIndex}"); - Debug.Assert(activeSet.Batches[target.BatchIndex].TypeBatches.Count > target.TypeBatchIndex); - } - else - { - //Since the constraint set could have changed arbitrarily since the previous execution, validate from batch down. - target = nextTarget; - Wrap(ref target); - Debug.Assert(activeSet.Batches[target.BatchIndex].TypeBatches.Count > target.TypeBatchIndex); - nextTarget = target; - nextTarget.BundleIndex += regionSizeInBundles; - //Console.WriteLine($"Normal frame targeting {target.BatchIndex}.{target.TypeBatchIndex}:{target.BundleIndex}"); - } - //Note that we have two separate parallel optimizations over multiple frames. Alternating between them on a per frame basis is a fairly simple way to guarantee - //eventual convergence in the sort. We only ever push forward the non-offset version; the offset position is based on the nonoffset version's last start position. - shouldOffset = !shouldOffset; - - - var maximumRegionSizeInConstraints = regionSizeInBundles * Vector.Count; - - var typeBatch = activeSet.Batches[target.BatchIndex].TypeBatches.GetPointer(target.TypeBatchIndex); - SortByBodyLocation(typeBatch, target.BundleIndex, Math.Min(typeBatch->ConstraintCount - target.BundleIndex * Vector.Count, maximumRegionSizeInConstraints), - solver.HandleToConstraint, bodies.ActiveSet.Count, bufferPool, threadDispatcher); - - } - - unsafe TypeBatch* typeBatchPointer; - IThreadDispatcher threadDispatcher; - Buffer handlesToConstraints; - struct MultithreadingContext - { - public Buffer SortKeys; - public Buffer SourceIndices; - public RawBuffer BodyReferencesCache; - public int SourceStartBundleIndex; - public int BundlesPerWorker; - public int BundlesPerWorkerRemainder; - public int TypeBatchConstraintCount; - - public Buffer SortedKeys; //This is only really stored for debug use. - public Buffer SortedSourceIndices; - public Buffer ScratchKeys; - public Buffer ScratchValues; - public Buffer IndexToHandleCache; - public RawBuffer PrestepDataCache; - public RawBuffer AccumulatesImpulsesCache; - public int KeyUpperBound; - public int ConstraintsInSortRegionCount; - //Note that these differ from phase 1- one of the threads in the sort is dedicated to a sort. These regard the remaining threads. - public int CopyBundlesPerWorker; - public int CopyBundlesPerWorkerRemainder; - } - MultithreadingContext context; - - unsafe void GenerateSortKeys(int workerIndex) - { - var localWorkerBundleStart = context.BundlesPerWorker * workerIndex + Math.Min(workerIndex, context.BundlesPerWorkerRemainder); - var workerBundleStart = context.SourceStartBundleIndex + localWorkerBundleStart; - var workerBundleCount = workerIndex < context.BundlesPerWorkerRemainder ? context.BundlesPerWorker + 1 : context.BundlesPerWorker; - var workerConstraintStart = workerBundleStart << BundleIndexing.VectorShift; - //Note that the number of constraints we can iterate over is clamped by the type batch's constraint count. The last bundle may not be full. - var workerConstraintCount = Math.Min(context.TypeBatchConstraintCount - workerConstraintStart, workerBundleCount << BundleIndexing.VectorShift); - if (workerConstraintCount <= 0) - return; //No work remains. - var localWorkerConstraintStart = localWorkerBundleStart << BundleIndexing.VectorShift; - - ref var typeBatch = ref *typeBatchPointer; - solver.TypeProcessors[typeBatch.TypeId].GenerateSortKeysAndCopyReferences(ref typeBatch, - workerBundleStart, localWorkerBundleStart, workerBundleCount, - workerConstraintStart, localWorkerConstraintStart, workerConstraintCount, - ref context.SortKeys[localWorkerConstraintStart], ref context.SourceIndices[localWorkerConstraintStart], ref context.BodyReferencesCache); - } - - unsafe void CopyToCacheAndSort(int workerIndex) - { - //Sorting only requires that the sort keys and indices be ready. Caching doesn't need to be done yet. - //Given that the sort is already very fast and trying to independently multithread it is a bad idea, we'll just bundle it alongside - //the remaining cache copies. This phase is extremely memory bound and the sort likely won't match the copy duration, but there is no - //room for complicated schemes at these timescales (<150us). We just try to get as much benefit as we can with a few simple tricks. - //Most likely we won't get more than about 2.5x speedup on a computer with bandwidth/compute ratios similar to a 3770K with 1600mhz memory. - if (workerIndex == threadDispatcher.ThreadCount - 1) - { - //TODO: If this ends up being the only place where you actually make use of the thread memory pools, you might as well get rid of it - //in favor of just preallocating workerCount buffers of 1024 ints each. Its original use of creating the typebatch-specific memory no longer exists. - LSBRadixSort.Sort( - ref context.SortKeys, ref context.SourceIndices, - ref context.ScratchKeys, ref context.ScratchValues, 0, context.ConstraintsInSortRegionCount, - context.KeyUpperBound, threadDispatcher.GetThreadMemoryPool(workerIndex), - out context.SortedKeys, out context.SortedSourceIndices); - } - //Note that worker 0 still copies if there's only one thread in the pool. Mainly for debugging purposes. - if (threadDispatcher.ThreadCount == 1 || workerIndex < threadDispatcher.ThreadCount - 1) - { - var localWorkerBundleStart = context.CopyBundlesPerWorker * workerIndex + Math.Min(workerIndex, context.CopyBundlesPerWorkerRemainder); - var workerBundleStart = context.SourceStartBundleIndex + localWorkerBundleStart; - var workerBundleCount = workerIndex < context.CopyBundlesPerWorkerRemainder ? context.CopyBundlesPerWorker + 1 : context.CopyBundlesPerWorker; - var workerConstraintStart = workerBundleStart << BundleIndexing.VectorShift; - //Note that the number of constraints we can iterate over is clamped by the type batch's constraint count. The last bundle may not be full. - var workerConstraintCount = Math.Min(context.TypeBatchConstraintCount - workerConstraintStart, workerBundleCount << BundleIndexing.VectorShift); - if (workerConstraintCount <= 0) - return; //No work remains. - var localWorkerConstraintStart = localWorkerBundleStart << BundleIndexing.VectorShift; - - ref var typeBatch = ref *typeBatchPointer; - solver.TypeProcessors[typeBatch.TypeId].CopyToCache(ref typeBatch, - workerBundleStart, localWorkerBundleStart, workerBundleCount, - workerConstraintStart, localWorkerConstraintStart, workerConstraintCount, - ref context.IndexToHandleCache, ref context.PrestepDataCache, ref context.AccumulatesImpulsesCache); - } - } - - unsafe void CopyToCacheAndSort(BufferPool pool) - { - LSBRadixSort.Sort( - ref context.SortKeys, ref context.SourceIndices, - ref context.ScratchKeys, ref context.ScratchValues, 0, context.ConstraintsInSortRegionCount, - context.KeyUpperBound, pool, - out context.SortedKeys, out context.SortedSourceIndices); - - var workerBundleStart = context.SourceStartBundleIndex; - var workerBundleCount = 0 < context.CopyBundlesPerWorkerRemainder ? context.CopyBundlesPerWorker + 1 : context.CopyBundlesPerWorker; - var workerConstraintStart = workerBundleStart << BundleIndexing.VectorShift; - //Note that the number of constraints we can iterate over is clamped by the type batch's constraint count. The last bundle may not be full. - var workerConstraintCount = Math.Min(context.TypeBatchConstraintCount - workerConstraintStart, workerBundleCount << BundleIndexing.VectorShift); - if (workerConstraintCount <= 0) - return; //No work remains. - - ref var typeBatch = ref *typeBatchPointer; - solver.TypeProcessors[typeBatch.TypeId].CopyToCache(ref typeBatch, - workerBundleStart, 0, workerBundleCount, - workerConstraintStart, 0, workerConstraintCount, - ref context.IndexToHandleCache, ref context.PrestepDataCache, ref context.AccumulatesImpulsesCache); - } - - unsafe void Regather(int workerIndex) - { - var localWorkerBundleStart = context.BundlesPerWorker * workerIndex + Math.Min(workerIndex, context.BundlesPerWorkerRemainder); - var workerBundleStart = context.SourceStartBundleIndex + localWorkerBundleStart; - var workerBundleCount = workerIndex < context.BundlesPerWorkerRemainder ? context.BundlesPerWorker + 1 : context.BundlesPerWorker; - var workerConstraintStart = workerBundleStart << BundleIndexing.VectorShift; - //Note that the number of constraints we can iterate over is clamped by the type batch's constraint count. The last bundle may not be full. - var workerConstraintCount = Math.Min(context.TypeBatchConstraintCount - workerConstraintStart, workerBundleCount << BundleIndexing.VectorShift); - if (workerConstraintCount <= 0) - return; //No work remains. - var localWorkerConstraintStart = localWorkerBundleStart << BundleIndexing.VectorShift; - ref var firstSourceIndex = ref context.SortedSourceIndices[localWorkerConstraintStart]; - - ref var typeBatch = ref *typeBatchPointer; - solver.TypeProcessors[typeBatch.TypeId].Regather(ref typeBatch, workerConstraintStart, workerConstraintCount, ref firstSourceIndex, - ref context.IndexToHandleCache, ref context.BodyReferencesCache, ref context.PrestepDataCache, ref context.AccumulatesImpulsesCache, ref handlesToConstraints); - - } - - - - unsafe void SortByBodyLocation(TypeBatch* typeBatch, int bundleStartIndex, int constraintCount, Buffer handlesToConstraints, int bodyCount, - BufferPool pool, IThreadDispatcher threadDispatcher) - { - int bundleCount = (constraintCount >> BundleIndexing.VectorShift); - if ((constraintCount & BundleIndexing.VectorMask) != 0) - ++bundleCount; - var threadCount = threadDispatcher == null ? 1 : threadDispatcher.ThreadCount; - - pool.TakeAtLeast(constraintCount, out context.SourceIndices); - pool.TakeAtLeast(constraintCount, out context.SortKeys); - pool.TakeAtLeast(constraintCount, out context.ScratchKeys); - pool.TakeAtLeast(constraintCount, out context.ScratchValues); - pool.TakeAtLeast(constraintCount, out context.IndexToHandleCache); - - var typeProcessor = solver.TypeProcessors[typeBatch->TypeId]; - typeProcessor.GetBundleTypeSizes(out var bodyReferencesBundleSize, out var prestepBundleSize, out var accumulatedImpulseBundleSize); - - //The typebatch invoked by the worker will cast the body references to the appropriate type. - //Using typeless buffers makes it easy to cache the buffers here in the constraint optimizer rather than in the individual type batches. - pool.TakeAtLeast(bundleCount * bodyReferencesBundleSize, out context.BodyReferencesCache); - pool.TakeAtLeast(bundleCount * prestepBundleSize, out context.PrestepDataCache); - pool.TakeAtLeast(bundleCount * accumulatedImpulseBundleSize, out context.AccumulatesImpulsesCache); - - context.BundlesPerWorker = bundleCount / threadCount; - context.BundlesPerWorkerRemainder = bundleCount - context.BundlesPerWorker * threadCount; - context.TypeBatchConstraintCount = typeBatch->ConstraintCount; - context.SourceStartBundleIndex = bundleStartIndex; - - //The second phase uses one worker to sort. - if (threadCount > 1) - { - context.CopyBundlesPerWorker = bundleCount / (threadCount - 1); - context.CopyBundlesPerWorkerRemainder = bundleCount - context.CopyBundlesPerWorker * (threadCount - 1); - } - else - { - //If there's only one worker (as is the case when this is running single-threaded), the worker will have to do the sort AND the copy. - context.CopyBundlesPerWorker = bundleCount; - context.CopyBundlesPerWorkerRemainder = 0; - } - context.ConstraintsInSortRegionCount = constraintCount; - context.KeyUpperBound = bodyCount - 1; - - this.typeBatchPointer = typeBatch; - this.threadDispatcher = threadDispatcher; - this.handlesToConstraints = handlesToConstraints; - - if (threadDispatcher == null) - { - GenerateSortKeys(0); - CopyToCacheAndSort(pool); - Regather(0); - } - else - { - threadDispatcher.DispatchWorkers(generateSortKeysDelegate); - threadDispatcher.DispatchWorkers(copyToCacheAndSortDelegate); - threadDispatcher.DispatchWorkers(regatherDelegate); - } - - this.typeBatchPointer = null; - this.threadDispatcher = null; - this.handlesToConstraints = new Buffer(); - - //This is a pure debug function. - solver.TypeProcessors[typeBatch->TypeId].VerifySortRegion(ref *typeBatch, bundleStartIndex, constraintCount, ref context.SortedKeys, ref context.SortedSourceIndices); - - pool.Return(ref context.SourceIndices); - pool.Return(ref context.SortKeys); - pool.Return(ref context.ScratchKeys); - pool.Return(ref context.ScratchValues); - pool.Return(ref context.IndexToHandleCache); - pool.Return(ref context.BodyReferencesCache); - pool.Return(ref context.PrestepDataCache); - pool.Return(ref context.AccumulatesImpulsesCache); - } - } -} diff --git a/BepuPhysics/Simulation.cs b/BepuPhysics/Simulation.cs index cd15c50e6..0cf868f50 100644 --- a/BepuPhysics/Simulation.cs +++ b/BepuPhysics/Simulation.cs @@ -25,8 +25,6 @@ public partial class Simulation : IDisposable public Bodies Bodies { get; private set; } public Statics Statics { get; private set; } public Shapes Shapes { get; private set; } - public BodyLayoutOptimizer BodyLayoutOptimizer { get; private set; } - public ConstraintLayoutOptimizer ConstraintLayoutOptimizer { get; private set; } public BatchCompressor SolverBatchCompressor { get; private set; } public Solver Solver { get; private set; } public IPoseIntegrator PoseIntegrator { get; private set; } @@ -114,8 +112,6 @@ public static Simulation Create simulation.Solver.awakener = simulation.Awakener; simulation.Bodies.Initialize(simulation.Solver, simulation.Awakener, simulation.Sleeper); simulation.SolverBatchCompressor = new BatchCompressor(simulation.Solver, simulation.Bodies); - simulation.BodyLayoutOptimizer = new BodyLayoutOptimizer(simulation.Bodies, simulation.BroadPhase, simulation.Solver, bufferPool); - simulation.ConstraintLayoutOptimizer = new ConstraintLayoutOptimizer(simulation.Bodies, simulation.Solver); simulation.Timestepper = timestepper; var narrowPhase = new NarrowPhase(simulation, @@ -317,17 +313,10 @@ public void Solve(float dt, IThreadDispatcher threadDispatcher = null) /// Thread dispatcher to use for execution, if any. public void IncrementallyOptimizeDataStructures(IThreadDispatcher threadDispatcher = null) { - //Note that constraint optimization should be performed after body optimization, since body optimization moves the bodies - and so affects the optimal constraint position. - //TODO: The order of these optimizer stages is performance relevant, even though they don't have any effect on correctness. - //You may want to try them in different locations to see how they impact cache residency. - profiler.Start(BodyLayoutOptimizer); - //BodyLayoutOptimizer.IncrementalOptimize(); - profiler.End(BodyLayoutOptimizer); - - profiler.Start(ConstraintLayoutOptimizer); - //ConstraintLayoutOptimizer.Update(BufferPool, threadDispatcher); - profiler.End(ConstraintLayoutOptimizer); - + //Previously, this handled body and constraint memory layout optimization. 2.4 significantly changed how memory accesses work in the solver + //and the optimizers were no longer net wins, so all that's left is the batch compressor. + //It pulls constraints currently living in high constraint batch indices to lower constraint batches if possible. + //Over time, that'll tend to reduce sync points in the solver and improve performance. profiler.Start(SolverBatchCompressor); SolverBatchCompressor.Compress(BufferPool, threadDispatcher, threadDispatcher != null && Deterministic); profiler.End(SolverBatchCompressor); diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index 446988f49..47c71b96e 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -1440,29 +1440,6 @@ internal void UpdateForBodyMemoryMove(int originalBodyIndex, int newBodyLocation } } - /// - /// Changes the body references of all constraints associated with two bodies in response to them swapping slots in memory. - /// - /// First swapped body index. - /// Second swapped body index. - internal void UpdateForBodyMemorySwap(int a, int b) - { - var aInFallback = UpdateConstraintsForBodyMemoryMove(a, b); - var bInFallback = UpdateConstraintsForBodyMemoryMove(b, a); - if (aInFallback && bInFallback) - { - ActiveSet.SequentialFallback.UpdateForBodyMemorySwap(a, b); - } - else if (aInFallback) - { - ActiveSet.SequentialFallback.UpdateForDynamicBodyMemoryMove(a, b); - } - else if (bInFallback) - { - ActiveSet.SequentialFallback.UpdateForDynamicBodyMemoryMove(b, a); - } - } - //TODO: Using a non-fixed time step isn't ideal to begin with, but these scaling functions are worse than they need to be. //Unfortunately, the faster alternative is quite a bit more complex- the accumulated impulses would need to be scaled alongside the warm start to minimize memory bandwidth. //Plus, none of this uses multithreading. diff --git a/Demos/DemoHarness.cs b/Demos/DemoHarness.cs index fde7751b6..2546337f9 100644 --- a/Demos/DemoHarness.cs +++ b/Demos/DemoHarness.cs @@ -101,9 +101,6 @@ public DemoHarness(GameLoop loop, ContentArchive content, timingGraph.AddSeries("Collision Test", new Vector3(0, 1, 0), 0.25f, timeSamples.CollisionTesting); timingGraph.AddSeries("Narrow Flush", new Vector3(1, 0, 1), 0.25f, timeSamples.NarrowPhaseFlush); timingGraph.AddSeries("Solver", new Vector3(1, 0, 0), 0.5f, timeSamples.Solver); - - timingGraph.AddSeries("Body Opt", new Vector3(1, 0.5f, 0), 0.125f, timeSamples.BodyOptimizer); - timingGraph.AddSeries("Constraint Opt", new Vector3(0, 0.5f, 1), 0.125f, timeSamples.ConstraintOptimizer); timingGraph.AddSeries("Batch Compress", new Vector3(0, 0.5f, 0), 0.125f, timeSamples.BatchCompressor); demoSet = new DemoSet(); diff --git a/Demos/Program.cs b/Demos/Program.cs index f7273c9ef..0ee9eca26 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -21,7 +21,7 @@ static void Main(string[] args) content = ContentArchive.Load(stream); } //HeadlessTest.Test(content, 4, 32, 512); - //HeadlessTest.Test(content, 8, 32, 1024); + HeadlessTest.Test(content, 8, 32, 1024); //HeadlessTest.Test(content, 4, 32, 512); //HeadlessTest.Test(content, 4, 128, 1024); //DeterminismTest.Test(content, 1, 65536); diff --git a/Demos/SimulationTimeSamples.cs b/Demos/SimulationTimeSamples.cs index 74f41ff05..ad60bf279 100644 --- a/Demos/SimulationTimeSamples.cs +++ b/Demos/SimulationTimeSamples.cs @@ -27,8 +27,6 @@ public class SimulationTimeSamples public TimingsRingBuffer CollisionTesting; public TimingsRingBuffer NarrowPhaseFlush; public TimingsRingBuffer Solver; - public TimingsRingBuffer BodyOptimizer; - public TimingsRingBuffer ConstraintOptimizer; public TimingsRingBuffer BatchCompressor; public SimulationTimeSamples(int frameCapacity, BufferPool pool) @@ -40,8 +38,6 @@ public SimulationTimeSamples(int frameCapacity, BufferPool pool) CollisionTesting = new TimingsRingBuffer(frameCapacity, pool); NarrowPhaseFlush = new TimingsRingBuffer(frameCapacity, pool); Solver = new TimingsRingBuffer(frameCapacity, pool); - BodyOptimizer = new TimingsRingBuffer(frameCapacity, pool); - ConstraintOptimizer = new TimingsRingBuffer(frameCapacity, pool); BatchCompressor = new TimingsRingBuffer(frameCapacity, pool); } @@ -55,8 +51,6 @@ public void RecordFrame(Simulation simulation) CollisionTesting.Add(simulation.Profiler[simulation.BroadPhaseOverlapFinder]); NarrowPhaseFlush.Add(simulation.Profiler[simulation.NarrowPhase]); Solver.Add(simulation.Profiler[simulation.Solver]); - BodyOptimizer.Add(simulation.Profiler[simulation.BodyLayoutOptimizer]); - ConstraintOptimizer.Add(simulation.Profiler[simulation.ConstraintLayoutOptimizer]); BatchCompressor.Add(simulation.Profiler[simulation.SolverBatchCompressor]); } @@ -69,8 +63,6 @@ public void Dispose() CollisionTesting.Dispose(); NarrowPhaseFlush.Dispose(); Solver.Dispose(); - BodyOptimizer.Dispose(); - ConstraintOptimizer.Dispose(); BatchCompressor.Dispose(); } } diff --git a/Demos/SpecializedTests/SimulationScrambling.cs b/Demos/SpecializedTests/SimulationScrambling.cs index b901c5b35..f0545ecb7 100644 --- a/Demos/SpecializedTests/SimulationScrambling.cs +++ b/Demos/SpecializedTests/SimulationScrambling.cs @@ -14,23 +14,6 @@ namespace Demos.SpecializedTests { public static class SimulationScrambling { - public static void ScrambleBodies(Simulation simulation) - { - //Having every single body in order is pretty unrealistic. In a real application, churn and general lack of care will result in - //scrambled body versus constraint memory access patterns. That's a big increase in cache misses. - //Scrambling the body array simulates this. - //Given a sufficiently large added overhead, it would benefit the engine to include runtime cache optimization. - //That is, move the memory location of bodies (and constraints, within type batches) to maximize the number of accesses to already-cached bodies. - - Random random = new Random(5); - for (int i = simulation.Bodies.ActiveSet.Count - 1; i >= 1; --i) - { - //This helper function handles the updates that have to be performed across all body-sensitive systems. - BodyLayoutOptimizer.SwapBodyLocation(simulation.Bodies, simulation.Solver, i, random.Next(i)); - } - - } - public static void ScrambleConstraints(Solver solver) { Random random = new Random(5); From 67d6dfeac4a2e8d65b73886fe47498d6233795cb Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 3 Dec 2021 14:04:53 -0600 Subject: [PATCH 304/947] ComputeInertia now returns rather than out. Annoying breaking change alert! --- BepuPhysics/BodyDescription.cs | 2 +- BepuPhysics/BodyProperties.cs | 69 +++++++++++++++++-- BepuPhysics/Collidables/Box.cs | 4 +- BepuPhysics/Collidables/Capsule.cs | 4 +- BepuPhysics/Collidables/CompoundHelpers.cs | 2 +- BepuPhysics/Collidables/ConvexHull.cs | 9 +-- BepuPhysics/Collidables/Cylinder.cs | 4 +- BepuPhysics/Collidables/IShape.cs | 41 ++++++++++- BepuPhysics/Collidables/Sphere.cs | 4 +- BepuPhysics/Collidables/Triangle.cs | 4 +- DemoTests/InertiaTensorTests.cs | 12 ++-- Demos/Demos/BlockChainDemo.cs | 5 +- Demos/Demos/BouncinessDemo.cs | 5 +- Demos/Demos/Cars/CarDemo.cs | 2 +- Demos/Demos/Characters/CharacterDemo.cs | 3 +- Demos/Demos/CollisionQueryDemo.cs | 3 +- Demos/Demos/ColosseumDemo.cs | 11 ++- Demos/Demos/CompoundTestDemo.cs | 8 +-- .../Demos/ContinuousCollisionDetectionDemo.cs | 4 +- Demos/Demos/CustomVoxelCollidableDemo.cs | 3 +- Demos/Demos/FountainStressTestDemo.cs | 2 +- Demos/Demos/NewtDemo.cs | 6 +- Demos/Demos/PlanetDemo.cs | 2 +- Demos/Demos/PyramidDemo.cs | 2 +- Demos/Demos/RagdollDemo.cs | 3 +- Demos/Demos/RopeStabilityDemo.cs | 8 +-- Demos/Demos/RopeTwistDemo.cs | 2 +- Demos/Demos/SimpleSelfContainedDemo.cs | 2 +- Demos/Demos/SolverContactEnumerationDemo.cs | 2 +- Demos/Demos/Sponsors/SponsorDemo.cs | 3 +- Demos/Demos/SubsteppingDemo.cs | 12 ++-- Demos/Demos/SweepDemo.cs | 6 +- Demos/Demos/Tanks/TankDemo.cs | 4 +- Demos/Demos/Tanks/TankPartDescription.cs | 2 +- Demos/Program.cs | 2 +- .../BroadPhaseStressTestDemo.cs | 2 +- Demos/SpecializedTests/CapsuleTestDemo.cs | 5 +- Demos/SpecializedTests/ClothLatticeDemo.cs | 2 +- .../ConstrainedKinematicIntegrationTest.cs | 6 +- Demos/SpecializedTests/ConstraintTestDemo.cs | 4 +- Demos/SpecializedTests/ConvexHullTestDemo.cs | 3 +- Demos/SpecializedTests/CylinderTestDemo.cs | 11 ++- .../Media/ColosseumVideoDemo.cs | 9 +-- Demos/SpecializedTests/Media/NewtVideoDemo.cs | 3 +- .../Media/PyramidVideoDemo.cs | 2 +- Demos/SpecializedTests/MeshMeshTestDemo.cs | 2 +- .../SpecializedTests/MeshReductionTestDemo.cs | 16 ++--- .../MeshSerializationTestDemo.cs | 3 +- Demos/SpecializedTests/MeshTestDemo.cs | 6 +- .../PyramidAwakenerTestDemo.cs | 6 +- Demos/SpecializedTests/ShapePileTestDemo.cs | 10 +-- Demos/SpecializedTests/SolverBatchTestDemo.cs | 2 +- Demos/SpecializedTests/TriangleTestDemo.cs | 8 +-- 53 files changed, 218 insertions(+), 139 deletions(-) diff --git a/BepuPhysics/BodyDescription.cs b/BepuPhysics/BodyDescription.cs index 6a46d5d28..61bb9b336 100644 --- a/BepuPhysics/BodyDescription.cs +++ b/BepuPhysics/BodyDescription.cs @@ -176,7 +176,7 @@ public static BodyDescription CreateConvexDynamic( Activity = GetDefaultActivity(shape), Collidable = shapes.Add(shape) }; - shape.ComputeInertia(mass, out description.LocalInertia); + description.LocalInertia = shape.ComputeInertia(mass); return description; } diff --git a/BepuPhysics/BodyProperties.cs b/BepuPhysics/BodyProperties.cs index f08ab4cc4..d52f84cf2 100644 --- a/BepuPhysics/BodyProperties.cs +++ b/BepuPhysics/BodyProperties.cs @@ -67,19 +67,38 @@ public struct RigidPose //systems needing to interact directly with this representation are often terrifically memory bound. Spending the extra ALU time to convert to a basis can actually be faster //than loading the extra 5 elements needed to express the full 3x3 rotation matrix. Also, it's marginally easier to keep the rotation normalized over time. //There may be an argument for the matrix variant to ALSO be stored for some bandwidth-unconstrained stages, but don't worry about that until there's a reason to worry about it. + /// + /// Orientation of the pose. + /// public Quaternion Orientation; + /// + /// Position of the pose. + /// public Vector3 Position; - public static RigidPose Identity { get; } = new RigidPose(new Vector3()); + /// + /// Returns a pose with a position at (0,0,0) and identity orientation. + /// + public static RigidPose Identity => new RigidPose(new Vector3()); + /// + /// Creates a rigid pose with the given position and orientation. + /// + /// Position of the pose. + /// Orientation of the pose. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public RigidPose(in Vector3 position, in Quaternion orientation) + public RigidPose(Vector3 position, Quaternion orientation) { Position = position; Orientation = orientation; } + + /// + /// Creates a rigid pose with the given position and identity orientation. + /// + /// Position of the pose. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public RigidPose(in Vector3 position) + public RigidPose(Vector3 position) { Position = position; Orientation = Quaternion.Identity; @@ -136,37 +155,68 @@ public static void MultiplyWithoutOverlap(in RigidPose a, in RigidPose b, out Ri } } + /// + /// Linear and angular velocity for a body. + /// [StructLayout(LayoutKind.Explicit, Size = 32)] public struct BodyVelocity { + /// + /// Linear velocity associated with the body. + /// [FieldOffset(0)] public Vector3 Linear; + /// + /// Angular velocity associated with the body. + /// [FieldOffset(16)] public Vector3 Angular; + /// + /// Creates a new set of body velocities. Angular velocity is set to zero. + /// + /// Linear velocity to use for the body. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public BodyVelocity(in Vector3 linear) + public BodyVelocity(Vector3 linear) { Linear = linear; Angular = default; } + /// + /// Creates a new set of body velocities. + /// + /// Linear velocity to use for the body. + /// Angular velocity to use for the body. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public BodyVelocity(in Vector3 linear, in Vector3 angular) + public BodyVelocity(Vector3 linear, Vector3 angular) { Linear = linear; Angular = angular; } } + /// + /// Stores the inertia for a body. + /// + /// This representation stores the inverse mass and inverse inertia tensor. Most of the high frequency use cases in the engine naturally use the inverse. [StructLayout(LayoutKind.Sequential, Size = 32, Pack = 4)] public struct BodyInertia { + /// + /// Inverse of the body's inertia tensor. + /// public Symmetric3x3 InverseInertiaTensor; + /// + /// Inverse of the body's mass. + /// public float InverseMass; } + /// + /// Stores the local and world views of a body's inertia, packed together for efficient access. + /// [StructLayout(LayoutKind.Sequential)] public struct BodyInertias { @@ -196,7 +246,13 @@ public struct BodyInertias [StructLayout(LayoutKind.Sequential)] public struct SolverState { + /// + /// Pose and velocity information for the body. + /// public MotionState Motion; + /// + /// Inertia information for the body. + /// public BodyInertias Inertia; } @@ -243,6 +299,9 @@ public struct BodyInertiaWide public Vector InverseMass; } + /// + /// Describes how a body sleeps, and its current state with respect to sleeping. + /// public struct BodyActivity { /// diff --git a/BepuPhysics/Collidables/Box.cs b/BepuPhysics/Collidables/Box.cs index 6fa573096..1d0368393 100644 --- a/BepuPhysics/Collidables/Box.cs +++ b/BepuPhysics/Collidables/Box.cs @@ -146,8 +146,9 @@ public readonly bool RayTest(in RigidPose pose, in Vector3 origin, in Vector3 di return true; } - public readonly void ComputeInertia(float mass, out BodyInertia inertia) + public readonly BodyInertia ComputeInertia(float mass) { + BodyInertia inertia; inertia.InverseMass = 1f / mass; var x2 = HalfWidth * HalfWidth; var y2 = HalfHeight * HalfHeight; @@ -158,6 +159,7 @@ public readonly void ComputeInertia(float mass, out BodyInertia inertia) inertia.InverseInertiaTensor.ZX = 0; inertia.InverseInertiaTensor.ZY = 0; inertia.InverseInertiaTensor.ZZ = inertia.InverseMass * 3 / (x2 + y2); + return inertia; } public readonly ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches) diff --git a/BepuPhysics/Collidables/Capsule.cs b/BepuPhysics/Collidables/Capsule.cs index 62d17da46..4490290dc 100644 --- a/BepuPhysics/Collidables/Capsule.cs +++ b/BepuPhysics/Collidables/Capsule.cs @@ -154,8 +154,9 @@ public readonly bool RayTest(in RigidPose pose, in Vector3 origin, in Vector3 di } - public readonly void ComputeInertia(float mass, out BodyInertia inertia) + public readonly BodyInertia ComputeInertia(float mass) { + BodyInertia inertia; inertia.InverseMass = 1f / mass; var r2 = Radius * Radius; var h2 = HalfLength * HalfLength; @@ -173,6 +174,7 @@ public readonly void ComputeInertia(float mass, out BodyInertia inertia) inertia.InverseInertiaTensor.ZX = 0; inertia.InverseInertiaTensor.ZY = 0; inertia.InverseInertiaTensor.ZZ = inertia.InverseInertiaTensor.XX; + return inertia; } public readonly ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches) diff --git a/BepuPhysics/Collidables/CompoundHelpers.cs b/BepuPhysics/Collidables/CompoundHelpers.cs index d300b86c7..136837d3d 100644 --- a/BepuPhysics/Collidables/CompoundHelpers.cs +++ b/BepuPhysics/Collidables/CompoundHelpers.cs @@ -56,7 +56,7 @@ public void Add(in TShape shape, in RigidPose localPose, float weight) w child.LocalPose = localPose; child.ShapeIndex = Shapes.Add(shape); child.Weight = weight; - shape.ComputeInertia(weight, out var inertia); + var inertia = shape.ComputeInertia(weight); Symmetric3x3.Invert(inertia.InverseInertiaTensor, out child.Inertia); } diff --git a/BepuPhysics/Collidables/ConvexHull.cs b/BepuPhysics/Collidables/ConvexHull.cs index b16ef217b..6cb024950 100644 --- a/BepuPhysics/Collidables/ConvexHull.cs +++ b/BepuPhysics/Collidables/ConvexHull.cs @@ -183,17 +183,14 @@ public bool GetNextTriangle(out Vector3 a, out Vector3 b, out Vector3 c) } } - /// - /// Computes the inertia of the convex hull. - /// - /// Mass to scale the inertia tensor with. - /// Inertia of the convex hull. - public readonly void ComputeInertia(float mass, out BodyInertia inertia) + public readonly BodyInertia ComputeInertia(float mass) { var triangleSource = new ConvexHullTriangleSource(this); MeshInertiaHelper.ComputeClosedInertia(ref triangleSource, mass, out _, out var inertiaTensor); + BodyInertia inertia; inertia.InverseMass = 1f / mass; Symmetric3x3.Invert(inertiaTensor, out inertia.InverseInertiaTensor); + return inertia; } public readonly ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches) diff --git a/BepuPhysics/Collidables/Cylinder.cs b/BepuPhysics/Collidables/Cylinder.cs index 91d2c256e..160b613e0 100644 --- a/BepuPhysics/Collidables/Cylinder.cs +++ b/BepuPhysics/Collidables/Cylinder.cs @@ -165,8 +165,9 @@ public readonly bool RayTest(in RigidPose pose, in Vector3 origin, in Vector3 di return true; } - public readonly void ComputeInertia(float mass, out BodyInertia inertia) + public readonly BodyInertia ComputeInertia(float mass) { + BodyInertia inertia; inertia.InverseMass = 1f / mass; float diagValue = inertia.InverseMass / ((4 * .0833333333f) * HalfLength * HalfLength + .25f * Radius * Radius); inertia.InverseInertiaTensor.XX = diagValue; @@ -175,6 +176,7 @@ public readonly void ComputeInertia(float mass, out BodyInertia inertia) inertia.InverseInertiaTensor.ZX = 0; inertia.InverseInertiaTensor.ZY = 0; inertia.InverseInertiaTensor.ZZ = diagValue; + return inertia; } public readonly ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches) diff --git a/BepuPhysics/Collidables/IShape.cs b/BepuPhysics/Collidables/IShape.cs index 44a02a1bf..193ac52df 100644 --- a/BepuPhysics/Collidables/IShape.cs +++ b/BepuPhysics/Collidables/IShape.cs @@ -14,7 +14,19 @@ namespace BepuPhysics.Collidables /// public interface IShape { + //TODO: Note that these should really be *static* as they do not need any information about an instance, but static abstract interface methods are not yet out of preview. + /// + /// Unique type id for this shape type. + /// int TypeId { get; } + /// + /// Creates a shape batch for this type of shape. + /// + /// Buffer pool used to create the batch. + /// Initial capacity to allocate within the batch. + /// The set of shapes to contain this batch. + /// Shape batch for the shape type. + /// This is typically used internally to initialize new shape collections in response to shapes being added. It is not likely to be useful outside of the engine. ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches); } @@ -26,13 +38,38 @@ public interface IShape //Note, however, that we do not bother supporting velocity expansion on the one-off variant. For the purposes of adding objects to the simulation, that is basically irrelevant. //I don't predict ever needing it, but such an implementation could be added... - + /// + /// Defines functions available on all convex shapes. Convex shapes have no hollowed out regions; any line passing through a convex shape will never enter and exit more than once. + /// public interface IConvexShape : IShape { + /// + /// Computes the bounding box of a shape given an orientation. + /// + /// Orientation of the shape to use when computing the bounding box. + /// Minimum corner of the bounding box. + /// Maximum corner of the bounding box. void ComputeBounds(in Quaternion orientation, out Vector3 min, out Vector3 max); + + /// + /// Computes information about how the bounding box should be expanded in response to angular velocity. + /// + /// + /// + /// This is typically used in the engine for predicting bounding boxes at the beginning of the frame. + /// Velocities are used to expand the bounding box so that likely future collisions will be detected. + /// Linear velocity expands the bounding box in a direct and simple way, but angular expansion requires more information about the shape. + /// Imagine a long and thin capsule versus a sphere: high angular velocity may require significant expansion on the capsule, but spheres are rotationally invariant. void ComputeAngularExpansionData(out float maximumRadius, out float maximumAngularExpansion); - void ComputeInertia(float mass, out BodyInertia inertia); + /// + /// Computes the inertia for a body given a mass. + /// + /// Mass to use to compute the body's inertia. + /// Inertia for the body. + /// Note that the returned by this stores the inverse mass and inverse inertia tensor. + /// This is because the most high frequency use of body inertia most naturally uses the inverse. + BodyInertia ComputeInertia(float mass); bool RayTest(in RigidPose pose, in Vector3 origin, in Vector3 direction, out float t, out Vector3 normal); } diff --git a/BepuPhysics/Collidables/Sphere.cs b/BepuPhysics/Collidables/Sphere.cs index a6349fcb9..e1a861a76 100644 --- a/BepuPhysics/Collidables/Sphere.cs +++ b/BepuPhysics/Collidables/Sphere.cs @@ -95,8 +95,9 @@ public readonly bool RayTest(in RigidPose pose, in Vector3 origin, in Vector3 di return true; } - public readonly void ComputeInertia(float mass, out BodyInertia inertia) + public readonly BodyInertia ComputeInertia(float mass) { + BodyInertia inertia; inertia.InverseMass = 1f / mass; inertia.InverseInertiaTensor.XX = inertia.InverseMass / ((2f / 5f) * Radius * Radius); inertia.InverseInertiaTensor.YX = 0; @@ -104,6 +105,7 @@ public readonly void ComputeInertia(float mass, out BodyInertia inertia) inertia.InverseInertiaTensor.ZX = 0; inertia.InverseInertiaTensor.ZY = 0; inertia.InverseInertiaTensor.ZZ = inertia.InverseInertiaTensor.XX; + return inertia; } public readonly ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapes) diff --git a/BepuPhysics/Collidables/Triangle.cs b/BepuPhysics/Collidables/Triangle.cs index eb02f10f4..f7616cdb6 100644 --- a/BepuPhysics/Collidables/Triangle.cs +++ b/BepuPhysics/Collidables/Triangle.cs @@ -109,11 +109,13 @@ public readonly bool RayTest(in RigidPose pose, in Vector3 origin, in Vector3 di return false; } - public readonly void ComputeInertia(float mass, out BodyInertia inertia) + public readonly BodyInertia ComputeInertia(float mass) { MeshInertiaHelper.ComputeTriangleContribution(A, B, C, mass, out var inertiaTensor); + BodyInertia inertia; Symmetric3x3.Invert(inertiaTensor, out inertia.InverseInertiaTensor); inertia.InverseMass = 1f / mass; + return inertia; } public readonly ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches) diff --git a/DemoTests/InertiaTensorTests.cs b/DemoTests/InertiaTensorTests.cs index e15f2f909..3e7e87718 100644 --- a/DemoTests/InertiaTensorTests.cs +++ b/DemoTests/InertiaTensorTests.cs @@ -23,7 +23,7 @@ public struct SphereInertiaTester : IInertiaTester public void ComputeAnalyticInertia(float mass, out BodyInertia inertia) { - Sphere.ComputeInertia(mass, out inertia); + inertia = Sphere.ComputeInertia(mass); } public void ComputeBounds(out Vector3 min, out Vector3 max) @@ -43,7 +43,7 @@ public struct CapsuleInertiaTester : IInertiaTester public void ComputeAnalyticInertia(float mass, out BodyInertia inertia) { - Capsule.ComputeInertia(mass, out inertia); + inertia = Capsule.ComputeInertia(mass); } public void ComputeBounds(out Vector3 min, out Vector3 max) { @@ -63,7 +63,7 @@ public struct CylinderInertiaTester : IInertiaTester public void ComputeAnalyticInertia(float mass, out BodyInertia inertia) { - Cylinder.ComputeInertia(mass, out inertia); + inertia = Cylinder.ComputeInertia(mass); } public void ComputeBounds(out Vector3 min, out Vector3 max) { @@ -82,7 +82,7 @@ public struct BoxInertiaTester : IInertiaTester public Box Box; public void ComputeAnalyticInertia(float mass, out BodyInertia inertia) { - Box.ComputeInertia(mass, out inertia); + inertia = Box.ComputeInertia(mass); } public void ComputeBounds(out Vector3 min, out Vector3 max) { @@ -101,7 +101,7 @@ public struct TriangleInertiaTester : IInertiaTester public Triangle Triangle; public void ComputeAnalyticInertia(float mass, out BodyInertia inertia) { - Triangle.ComputeInertia(mass, out inertia); + inertia = Triangle.ComputeInertia(mass); } public void ComputeBounds(out Vector3 min, out Vector3 max) { @@ -134,7 +134,7 @@ public struct ConvexHullInertiaTester : IInertiaTester public ConvexHull Hull; public void ComputeAnalyticInertia(float mass, out BodyInertia inertia) { - Hull.ComputeInertia(mass, out inertia); + inertia = Hull.ComputeInertia(mass); } public void ComputeBounds(out Vector3 min, out Vector3 max) { diff --git a/Demos/Demos/BlockChainDemo.cs b/Demos/Demos/BlockChainDemo.cs index c82cf50d6..b2b9e67dc 100644 --- a/Demos/Demos/BlockChainDemo.cs +++ b/Demos/Demos/BlockChainDemo.cs @@ -31,7 +31,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), 1); var boxShape = new Box(1, 1, 1); - boxShape.ComputeInertia(1, out var boxInertia); + var boxInertia = boxShape.ComputeInertia(1); var boxIndex = Simulation.Shapes.Add(boxShape); const int forkCount = 20; const int blocksPerChain = 20; @@ -64,8 +64,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //Build the coin description for the ponz-I mean ICO. var coinShape = new Cylinder(1.5f, 0.2f); - coinShape.ComputeInertia(1, out var coinInertia); - coinDescription = BodyDescription.CreateDynamic(RigidPose.Identity, coinInertia, Simulation.Shapes.Add(coinShape), 0.01f); + coinDescription = BodyDescription.CreateDynamic(RigidPose.Identity, coinShape.ComputeInertia(1), Simulation.Shapes.Add(coinShape), 0.01f); } BodyDescription coinDescription; diff --git a/Demos/Demos/BouncinessDemo.cs b/Demos/Demos/BouncinessDemo.cs index a002a486b..d63789012 100644 --- a/Demos/Demos/BouncinessDemo.cs +++ b/Demos/Demos/BouncinessDemo.cs @@ -88,9 +88,8 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var collidableMaterials = new CollidableProperty(); Simulation = Simulation.Create(BufferPool, new BounceCallbacks() { CollidableMaterials = collidableMaterials }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), solverIterationCount: 2); - var shape = new Sphere(1); - shape.ComputeInertia(1, out var inertia); - var ballDescription = BodyDescription.CreateDynamic(RigidPose.Identity, inertia, new(Simulation.Shapes.Add(shape), ContinuousDetection.Discrete(20, 20)), 1e-2f); + var shape = new Sphere(1); + var ballDescription = BodyDescription.CreateDynamic(RigidPose.Identity, shape.ComputeInertia(1), new(Simulation.Shapes.Add(shape), ContinuousDetection.Discrete(20, 20)), 1e-2f); for (int i = 0; i < 100; ++i) { diff --git a/Demos/Demos/Cars/CarDemo.cs b/Demos/Demos/Cars/CarDemo.cs index c6872ce24..95fbd8867 100644 --- a/Demos/Demos/Cars/CarDemo.cs +++ b/Demos/Demos/Cars/CarDemo.cs @@ -56,7 +56,7 @@ public override void Initialize(ContentArchive content, Camera camera) var bodyShape = new Compound(children); var bodyShapeIndex = Simulation.Shapes.Add(bodyShape); var wheelShape = new Cylinder(0.4f, .18f); - wheelShape.ComputeInertia(0.25f, out var wheelInertia); + var wheelInertia = wheelShape.ComputeInertia(0.25f); var wheelShapeIndex = Simulation.Shapes.Add(wheelShape); const float x = 0.9f; diff --git a/Demos/Demos/Characters/CharacterDemo.cs b/Demos/Demos/Characters/CharacterDemo.cs index a9cefa38e..411313c1d 100644 --- a/Demos/Demos/Characters/CharacterDemo.cs +++ b/Demos/Demos/Characters/CharacterDemo.cs @@ -44,12 +44,11 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var orientation = QuaternionEx.CreateFromAxisAngle(Vector3.Normalize(new Vector3(0.0001f) + new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle())), 10 * random.NextSingle()); var shape = new Box(0.1f + 0.3f * random.NextSingle(), 0.1f + 0.3f * random.NextSingle(), 0.1f + 0.3f * random.NextSingle()); var shapeIndex = Simulation.Shapes.Add(shape); - shape.ComputeInertia(1, out var inertia); var choice = (i + j) % 3; switch (choice) { case 0: - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new RigidPose(position, orientation), inertia, shapeIndex, 0.01f)); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new RigidPose(position, orientation), shape.ComputeInertia(1), shapeIndex, 0.01f)); break; case 1: Simulation.Bodies.Add(BodyDescription.CreateKinematic(new RigidPose(position, orientation), shapeIndex, 0.01f)); diff --git a/Demos/Demos/CollisionQueryDemo.cs b/Demos/Demos/CollisionQueryDemo.cs index c099fd4d1..b4e1fbb68 100644 --- a/Demos/Demos/CollisionQueryDemo.cs +++ b/Demos/Demos/CollisionQueryDemo.cs @@ -36,8 +36,7 @@ public override void Initialize(ContentArchive content, Camera camera) var random = new Random(5); var shapeToDrop = new Box(1, 1, 1); - shapeToDrop.ComputeInertia(1, out var shapeToDropInertia); - var descriptionToDrop = BodyDescription.CreateDynamic(new Vector3(), shapeToDropInertia, Simulation.Shapes.Add(shapeToDrop), 0.01f); + var descriptionToDrop = BodyDescription.CreateDynamic(new Vector3(), shapeToDrop.ComputeInertia(1), Simulation.Shapes.Add(shapeToDrop), 0.01f); for (int i = 0; i < 128; ++i) { descriptionToDrop.Pose.Position = new Vector3(-5 + 10 * random.NextSingle(), 45 + 150 * random.NextSingle(), -5 + 10 * random.NextSingle()); diff --git a/Demos/Demos/ColosseumDemo.cs b/Demos/Demos/ColosseumDemo.cs index ac480209d..171d44ffc 100644 --- a/Demos/Demos/ColosseumDemo.cs +++ b/Demos/Demos/ColosseumDemo.cs @@ -75,8 +75,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), solverIterationCount: 1); var ringBoxShape = new Box(0.5f, 1, 3); - ringBoxShape.ComputeInertia(1, out var ringBoxInertia); - var boxDescription = BodyDescription.CreateDynamic(new Vector3(), ringBoxInertia, Simulation.Shapes.Add(ringBoxShape), 0.01f); + var boxDescription = BodyDescription.CreateDynamic(new Vector3(), ringBoxShape.ComputeInertia(1), Simulation.Shapes.Add(ringBoxShape), 0.01f); var layerPosition = new Vector3(); const int layerCount = 6; @@ -98,12 +97,10 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), Simulation.Shapes.Add(new Box(500, 1, 500)))); var bulletShape = new Sphere(0.5f); - bulletShape.ComputeInertia(.1f, out var bulletInertia); - bulletDescription = BodyDescription.CreateDynamic(new Vector3(), bulletInertia, Simulation.Shapes.Add(bulletShape), 0.01f); + bulletDescription = BodyDescription.CreateDynamic(new Vector3(), bulletShape.ComputeInertia(.1f), Simulation.Shapes.Add(bulletShape), 0.01f); - var shootiePatootieShape = new Sphere(3f); - shootiePatootieShape.ComputeInertia(100, out var shootiePatootieInertia); - shootiePatootieDescription = BodyDescription.CreateDynamic(new Vector3(), shootiePatootieInertia, Simulation.Shapes.Add(shootiePatootieShape), 0.01f); + var shootiePatootieShape = new Sphere(3f); + shootiePatootieDescription = BodyDescription.CreateDynamic(new Vector3(), shootiePatootieShape.ComputeInertia(100), Simulation.Shapes.Add(shootiePatootieShape), 0.01f); } BodyDescription bulletDescription; diff --git a/Demos/Demos/CompoundTestDemo.cs b/Demos/Demos/CompoundTestDemo.cs index 2f9ba0e20..47486b5d3 100644 --- a/Demos/Demos/CompoundTestDemo.cs +++ b/Demos/Demos/CompoundTestDemo.cs @@ -47,7 +47,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) const float gridSpacing = 1.5f; const int gridWidth = 3; var gridShapeIndex = Simulation.Shapes.Add(gridShape); - gridShape.ComputeInertia(1, out var gridBoxInertia); + var gridBoxInertia = gridShape.ComputeInertia(1); float localPoseOffset = -0.5f * gridSpacing * (gridWidth - 1); for (int i = 0; i < gridWidth; ++i) { @@ -70,7 +70,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //Build a table and use it for a couple of different tests. { var legShape = new Box(0.2f, 1, 0.2f); - legShape.ComputeInertia(1f, out var legInverseInertia); + var legInverseInertia = legShape.ComputeInertia(1f); var legShapeIndex = Simulation.Shapes.Add(legShape); var legPose0 = new RigidPose { Position = new Vector3(-1.5f, 0, -1.5f), Orientation = Quaternion.Identity }; var legPose1 = new RigidPose { Position = new Vector3(-1.5f, 0, 1.5f), Orientation = Quaternion.Identity }; @@ -133,7 +133,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Simulation.Bodies.Add(tableDescription); var clampPieceShape = new Box(2f, 0.1f, 0.3f); - clampPieceShape.ComputeInertia(1f, out var clampPieceInverseInertia); + var clampPieceInverseInertia = clampPieceShape.ComputeInertia(1f); var clampPieceShapeIndex = Simulation.Shapes.Add(clampPieceShape); var clamp0 = new RigidPose { Position = new Vector3(0, -0.2f, -1.1f), Orientation = Quaternion.Identity }; var clamp1 = new RigidPose { Position = new Vector3(0, 0.2f, -1.1f), Orientation = Quaternion.Identity }; @@ -163,7 +163,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var random = new Random(5); var treeCompoundBoxShape = new Box(0.5f, 1.5f, 1f); var treeCompoundBoxShapeIndex = Simulation.Shapes.Add(treeCompoundBoxShape); - treeCompoundBoxShape.ComputeInertia(1, out var childInertia); + var childInertia = treeCompoundBoxShape.ComputeInertia(1); for (int i = 0; i < 128; ++i) { RigidPose localPose; diff --git a/Demos/Demos/ContinuousCollisionDetectionDemo.cs b/Demos/Demos/ContinuousCollisionDetectionDemo.cs index 2cbcc1a01..d07a84ab3 100644 --- a/Demos/Demos/ContinuousCollisionDetectionDemo.cs +++ b/Demos/Demos/ContinuousCollisionDetectionDemo.cs @@ -20,7 +20,7 @@ ConstraintHandle BuildSpinner(Vector3 initialPosition, float rotationSpeed) { var spinnerBase = Simulation.Bodies.Add(BodyDescription.CreateDynamic(initialPosition, new BodyInertia { InverseMass = 1e-2f }, Simulation.Shapes.Add(new Box(2, 2, 2)), 0.01f)); var bladeShape = new Box(5, 0.01f, 1); - bladeShape.ComputeInertia(1, out var bladeInertia); + var bladeInertia = bladeShape.ComputeInertia(1); var shapeIndex = Simulation.Shapes.Add(bladeShape); //Note that both the minimum progression duration and the sweep convergence duration are both very small at 1e-4. //That will detect collisions with a precision equal to an update rate of 10,000hz. @@ -60,7 +60,7 @@ public override void Initialize(ContentArchive content, Camera camera) new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); var shape = new Box(1, 1, 1); - shape.ComputeInertia(1, out var inertia); + var inertia = shape.ComputeInertia(1); var shapeIndex = Simulation.Shapes.Add(shape); for (int i = 0; i < 10; ++i) { diff --git a/Demos/Demos/CustomVoxelCollidableDemo.cs b/Demos/Demos/CustomVoxelCollidableDemo.cs index f94d02427..ef202ba0d 100644 --- a/Demos/Demos/CustomVoxelCollidableDemo.cs +++ b/Demos/Demos/CustomVoxelCollidableDemo.cs @@ -433,8 +433,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var random = new Random(5); var shapeToDrop = new Box(1, 1, 1); - shapeToDrop.ComputeInertia(1, out var shapeToDropInertia); - var descriptionToDrop = BodyDescription.CreateDynamic(new Vector3(), shapeToDropInertia, Simulation.Shapes.Add(shapeToDrop), 0.01f); + var descriptionToDrop = BodyDescription.CreateDynamic(new Vector3(), shapeToDrop.ComputeInertia(1), Simulation.Shapes.Add(shapeToDrop), 0.01f); for (int i = 0; i < 4096; ++i) { descriptionToDrop.Pose.Position = new Vector3(15 + 10 * random.NextSingle(), 45 + 150 * random.NextSingle(), 15 + 10 * random.NextSingle()); diff --git a/Demos/Demos/FountainStressTestDemo.cs b/Demos/Demos/FountainStressTestDemo.cs index 41235c98a..eee330935 100644 --- a/Demos/Demos/FountainStressTestDemo.cs +++ b/Demos/Demos/FountainStressTestDemo.cs @@ -93,7 +93,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) void AddConvexShape(in TConvex convex, out TypedIndex shapeIndex, out BodyInertia inertia) where TConvex : unmanaged, IConvexShape { shapeIndex = Simulation.Shapes.Add(convex); - convex.ComputeInertia(1, out inertia); + inertia = convex.ComputeInertia(1); } ConvexHull CreateRandomHull() diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index c23e97276..d8bf51f94 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -659,7 +659,7 @@ internal unsafe static void CreateDeformable(Simulation simulation, in Vector3 p pool.TakeAtLeast(vertices.Length, out var vertexHandles); var vertexShape = new Sphere(cellSize * 0.7f); var massPerVertex = density * (cellSize * cellSize * cellSize); - vertexShape.ComputeInertia(massPerVertex, out var vertexInertia); + var vertexInertia = vertexShape.ComputeInertia(massPerVertex); //vertexInertia.InverseInertiaTensor = default; var vertexShapeIndex = simulation.Shapes.Add(vertexShape); for (int i = 0; i < vertices.Length; ++i) @@ -749,8 +749,8 @@ void Test2() points.AllocateUnsafely() = new Vector3(3 * random.NextSingle(), 1 * random.NextSingle(), 3 * random.NextSingle()); } var convexHull2 = new ConvexHull(points, BufferPool, out _); - convexHull1.ComputeInertia(1, out var scalarInertiaA); - convexHull2.ComputeInertia(1, out var scalarInertiaB); + var scalarInertiaA = convexHull1.ComputeInertia(1); + var scalarInertiaB = convexHull2.ComputeInertia(1); BodyInertiaWide inertiaA; inertiaA.InverseMass = new Vector(1); diff --git a/Demos/Demos/PlanetDemo.cs b/Demos/Demos/PlanetDemo.cs index 21e24d2e0..998308593 100644 --- a/Demos/Demos/PlanetDemo.cs +++ b/Demos/Demos/PlanetDemo.cs @@ -74,7 +74,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(new Sphere(50)))); var orbiter = new Sphere(1f); - orbiter.ComputeInertia(1, out var inertia); + var inertia = orbiter.ComputeInertia(1); var orbiterShapeIndex = Simulation.Shapes.Add(orbiter); var spacing = new Vector3(5); const int length = 20; diff --git a/Demos/Demos/PyramidDemo.cs b/Demos/Demos/PyramidDemo.cs index b10989c48..8dd020574 100644 --- a/Demos/Demos/PyramidDemo.cs +++ b/Demos/Demos/PyramidDemo.cs @@ -29,7 +29,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); var boxShape = new Box(1, 1, 1); - boxShape.ComputeInertia(1, out var boxInertia); + var boxInertia = boxShape.ComputeInertia(1); var boxIndex = Simulation.Shapes.Add(boxShape); const int pyramidCount = 40; for (int pyramidIndex = 0; pyramidIndex < pyramidCount; ++pyramidIndex) diff --git a/Demos/Demos/RagdollDemo.cs b/Demos/Demos/RagdollDemo.cs index c47bfb839..49b0883b8 100644 --- a/Demos/Demos/RagdollDemo.cs +++ b/Demos/Demos/RagdollDemo.cs @@ -145,8 +145,7 @@ static BodyHandle AddBody(TShape shape, float mass, in RigidPose pose, S //Note that this always registers a new shape instance. You could be more clever/efficient and share shapes, but the goal here is to show the most basic option. //Also, the cost of registering different shapes isn't that high for tiny implicit shapes. var shapeIndex = simulation.Shapes.Add(shape); - shape.ComputeInertia(mass, out var inertia); - var description = BodyDescription.CreateDynamic(pose, inertia, shapeIndex, 0.01f); + var description = BodyDescription.CreateDynamic(pose, shape.ComputeInertia(mass), shapeIndex, 0.01f); return simulation.Bodies.Add(description); } diff --git a/Demos/Demos/RopeStabilityDemo.cs b/Demos/Demos/RopeStabilityDemo.cs index ff946c2e3..120ca29ec 100644 --- a/Demos/Demos/RopeStabilityDemo.cs +++ b/Demos/Demos/RopeStabilityDemo.cs @@ -23,7 +23,7 @@ public static BodyHandle[] BuildRopeBodies(Simulation simulation, in Vector3 sta { BodyHandle[] handles = new BodyHandle[bodyCount + 1]; var ropeShape = new Sphere(bodySize); - ropeShape.ComputeInertia(massPerBody, out var ropeInertia); + var ropeInertia = ropeShape.ComputeInertia(massPerBody); Symmetric3x3.Scale(ropeInertia.InverseInertiaTensor, inverseInertiaScale, out ropeInertia.InverseInertiaTensor); var ropeShapeIndex = simulation.Shapes.Add(ropeShape); //Build the links. @@ -96,12 +96,12 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //So, even though you can avoid the need for these kinds of hacks, it's good to know that they exist should you find yourself in a circumstance where substepping isn't viable. Simulation = Simulation.Create(BufferPool, - new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(120, 1), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 2f }, + new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(120, 1), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 2f }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); rolloverInfo = new RolloverInfo(); var smallWreckingBall = new Sphere(1); - smallWreckingBall.ComputeInertia(5, out var smallWreckingBallInertia); + var smallWreckingBallInertia = smallWreckingBall.ComputeInertia(5); var smallWreckingBallIndex = Simulation.Shapes.Add(smallWreckingBall); { //The first thing you might try when building a rope is a simple chain of low mass bodies connected by distance limits with the wrecking ball attached at the end in a natural way. @@ -117,7 +117,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) } var bigWreckingBall = new Sphere(3); //This wrecking ball is much, much heavier. - bigWreckingBall.ComputeInertia(100, out var bigWreckingBallInertia); + var bigWreckingBallInertia = bigWreckingBall.ComputeInertia(100); var bigWreckingBallIndex = Simulation.Shapes.Add(bigWreckingBall); { //Everything identical to the first rope, but now with a heavier wrecking ball. diff --git a/Demos/Demos/RopeTwistDemo.cs b/Demos/Demos/RopeTwistDemo.cs index 4b00f5c82..bbe308982 100644 --- a/Demos/Demos/RopeTwistDemo.cs +++ b/Demos/Demos/RopeTwistDemo.cs @@ -96,7 +96,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var bigWreckingBall = new Sphere(3); //This wrecking ball is much, much heavier. - bigWreckingBall.ComputeInertia(10000, out var bigWreckingBallInertia); + var bigWreckingBallInertia = bigWreckingBall.ComputeInertia(10000); var bigWreckingBallIndex = Simulation.Shapes.Add(bigWreckingBall); const float ropeBodySpacing = -0.1f; const float ropeBodyRadius = 0.1f; diff --git a/Demos/Demos/SimpleSelfContainedDemo.cs b/Demos/Demos/SimpleSelfContainedDemo.cs index d69727814..60f4fa030 100644 --- a/Demos/Demos/SimpleSelfContainedDemo.cs +++ b/Demos/Demos/SimpleSelfContainedDemo.cs @@ -215,7 +215,7 @@ public static void Run() //Drop a ball on a big static box. var sphere = new Sphere(1); - sphere.ComputeInertia(1, out var sphereInertia); + var sphereInertia = sphere.ComputeInertia(1); simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 5, 0), sphereInertia, simulation.Shapes.Add(sphere), 0.01f)); simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), simulation.Shapes.Add(new Box(500, 1, 500)))); diff --git a/Demos/Demos/SolverContactEnumerationDemo.cs b/Demos/Demos/SolverContactEnumerationDemo.cs index 0678d7bf9..3bb15421f 100644 --- a/Demos/Demos/SolverContactEnumerationDemo.cs +++ b/Demos/Demos/SolverContactEnumerationDemo.cs @@ -33,7 +33,7 @@ public override void Initialize(ContentArchive content, Camera camera) //Drop a pyramid on top of the sensor so there are more contacts to look at. var boxShape = new Box(1, 1, 1); - boxShape.ComputeInertia(1, out var boxInertia); + var boxInertia = boxShape.ComputeInertia(1); var boxIndex = Simulation.Shapes.Add(boxShape); const int rowCount = 20; diff --git a/Demos/Demos/Sponsors/SponsorDemo.cs b/Demos/Demos/Sponsors/SponsorDemo.cs index 807cfe14b..f721ac727 100644 --- a/Demos/Demos/Sponsors/SponsorDemo.cs +++ b/Demos/Demos/Sponsors/SponsorDemo.cs @@ -145,8 +145,7 @@ public override void Initialize(ContentArchive content, Camera camera) const int hutCount = 30; var hutBoxShape = new Box(0.4f, 2, 3); - hutBoxShape.ComputeInertia(20, out var obstacleInertia); - var obstacleDescription = BodyDescription.CreateDynamic(new Vector3(), obstacleInertia, Simulation.Shapes.Add(hutBoxShape), 1e-2f); + var obstacleDescription = BodyDescription.CreateDynamic(new Vector3(), hutBoxShape.ComputeInertia(20), Simulation.Shapes.Add(hutBoxShape), 1e-2f); for (int i = 0; i < hutCount; ++i) { diff --git a/Demos/Demos/SubsteppingDemo.cs b/Demos/Demos/SubsteppingDemo.cs index dd2e41962..914737c0d 100644 --- a/Demos/Demos/SubsteppingDemo.cs +++ b/Demos/Demos/SubsteppingDemo.cs @@ -24,7 +24,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = 0; camera.Pitch = 0; timestepper = new EmbeddedSubsteppingTimestepper2(8); - Simulation = Simulation.Create(BufferPool, + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(120, 120), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 2f }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), timestepper, 8); @@ -38,7 +38,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var bodyHandles = RopeStabilityDemo.BuildRope(Simulation, startLocation, 12, bodyRadius, bodySpacing, 0, 1, 0, springSettings); var bigWreckingBall = new Sphere(5); - bigWreckingBall.ComputeInertia(1000, out var bigWreckingBallInertia); + var bigWreckingBallInertia = bigWreckingBall.ComputeInertia(1000); RopeStabilityDemo.AttachWreckingBall(Simulation, bodyHandles, bodyRadius, bodySpacing, 0, bigWreckingBall.Radius, bigWreckingBallInertia, Simulation.Shapes.Add(bigWreckingBall), springSettings); rolloverInfo.Add(startLocation + new Vector3(0, 2, 0), "1000:1 mass ratio"); @@ -49,7 +49,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //It's also the reason why we need higher substeps- 120hz frequency is too high for 60hz solving! Watch what happens when you drop the substep count to 3. //(Note that the demos timestep frequency is 60hz, so 4 substeps is a 240hz solve rate- twice the 120hz contact frequency.) var boxShape = new Box(4, 0.5f, 6f); - boxShape.ComputeInertia(1, out var boxInertia); + var boxInertia = boxShape.ComputeInertia(1); //Note that sleeping is disabled with a negative velocity threshold. We want to watch the stack as we change simulation settings; if it's inactive, it won't respond! var boxDescription = BodyDescription.CreateDynamic(new Vector3(), boxInertia, Simulation.Shapes.Add(boxShape), -1f); for (int i = 0; i < 20; ++i) @@ -58,9 +58,8 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Simulation.Bodies.Add(boxDescription); } var topBlockShape = new Box(8, 2, 8); - topBlockShape.ComputeInertia(200, out var topBlockInertia); Simulation.Bodies.Add( - BodyDescription.CreateDynamic(boxDescription.Pose.Position + new Vector3(0, boxShape.HalfHeight + 1f, 0), topBlockInertia, + BodyDescription.CreateDynamic(boxDescription.Pose.Position + new Vector3(0, boxShape.HalfHeight + 1f, 0), topBlockShape.ComputeInertia(200), Simulation.Shapes.Add(topBlockShape), -1f)); rolloverInfo.Add(boxDescription.Pose.Position + new Vector3(0, 4, 0), "200:1 mass ratio"); @@ -72,9 +71,8 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var basePosition = new Vector3(-20, 20, 0); var boxShape = new Box(0.5f, 0.5f, 3f); var boxShapeIndex = Simulation.Shapes.Add(boxShape); - boxShape.ComputeInertia(1, out var boxInertia); + var boxInertia = boxShape.ComputeInertia(1); var linkDescription = BodyDescription.CreateDynamic(new Vector3(), boxInertia, boxShapeIndex, 0.01f); - for (int chainIndex = 0; chainIndex < 4; ++chainIndex) { diff --git a/Demos/Demos/SweepDemo.cs b/Demos/Demos/SweepDemo.cs index fa0ff9c83..460812c2e 100644 --- a/Demos/Demos/SweepDemo.cs +++ b/Demos/Demos/SweepDemo.cs @@ -36,9 +36,9 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var box = new Box(2f, 2f, 2f); var capsule = new Capsule(1f, 1f); var sphere = new Sphere(1.5f); - box.ComputeInertia(1, out var boxInertia); - capsule.ComputeInertia(1, out var capsuleInertia); - sphere.ComputeInertia(1, out var sphereInertia); + var boxInertia = box.ComputeInertia(1); + var capsuleInertia = capsule.ComputeInertia(1); + var sphereInertia = sphere.ComputeInertia(1); var boxIndex = Simulation.Shapes.Add(box); var capsuleIndex = Simulation.Shapes.Add(capsule); var sphereIndex = Simulation.Shapes.Add(sphere); diff --git a/Demos/Demos/Tanks/TankDemo.cs b/Demos/Demos/Tanks/TankDemo.cs index c7051509d..99bbcdd4b 100644 --- a/Demos/Demos/Tanks/TankDemo.cs +++ b/Demos/Demos/Tanks/TankDemo.cs @@ -61,11 +61,11 @@ public override void Initialize(ContentArchive content, Camera camera) var bodyShape = new Compound(children); var bodyShapeIndex = Simulation.Shapes.Add(bodyShape); var wheelShape = new Cylinder(0.4f, .18f); - wheelShape.ComputeInertia(0.25f, out var wheelInertia); + var wheelInertia = wheelShape.ComputeInertia(0.25f); var wheelShapeIndex = Simulation.Shapes.Add(wheelShape); var projectileShape = new Sphere(0.1f); - projectileShape.ComputeInertia(0.2f, out var projectileInertia); + var projectileInertia = projectileShape.ComputeInertia(0.2f); var tankDescription = new TankDescription { Body = TankPartDescription.Create(10, new Box(4f, 1, 5), RigidPose.Identity, 0.5f, Simulation.Shapes), diff --git a/Demos/Demos/Tanks/TankPartDescription.cs b/Demos/Demos/Tanks/TankPartDescription.cs index db0e93256..ddb6fcc37 100644 --- a/Demos/Demos/Tanks/TankPartDescription.cs +++ b/Demos/Demos/Tanks/TankPartDescription.cs @@ -29,7 +29,7 @@ public static TankPartDescription Create(float mass, in TShape shape, in { TankPartDescription description; description.Shape = shapes.Add(shape); - shape.ComputeInertia(mass, out description.Inertia); + description.Inertia = shape.ComputeInertia(mass); description.Pose = pose; description.Friction = friction; return description; diff --git a/Demos/Program.cs b/Demos/Program.cs index 0ee9eca26..f7273c9ef 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -21,7 +21,7 @@ static void Main(string[] args) content = ContentArchive.Load(stream); } //HeadlessTest.Test(content, 4, 32, 512); - HeadlessTest.Test(content, 8, 32, 1024); + //HeadlessTest.Test(content, 8, 32, 1024); //HeadlessTest.Test(content, 4, 32, 512); //HeadlessTest.Test(content, 4, 128, 1024); //DeterminismTest.Test(content, 1, 65536); diff --git a/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs b/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs index 7d7d9d109..8a6bfcda3 100644 --- a/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs +++ b/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs @@ -23,7 +23,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); var shape = new Sphere(0.5f); - shape.ComputeInertia(1, out var sphereInertia); + var sphereInertia = shape.ComputeInertia(1); var shapeIndex = Simulation.Shapes.Add(shape); const int width = 64; const int height = 64; diff --git a/Demos/SpecializedTests/CapsuleTestDemo.cs b/Demos/SpecializedTests/CapsuleTestDemo.cs index 842904704..98c72fd58 100644 --- a/Demos/SpecializedTests/CapsuleTestDemo.cs +++ b/Demos/SpecializedTests/CapsuleTestDemo.cs @@ -23,7 +23,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); var shape = new Capsule(.5f, .5f); - shape.ComputeInertia(1, out var localInertia); + var localInertia = shape.ComputeInertia(1); var shapeIndex = Simulation.Shapes.Add(shape); const int width = 1; const int height = 1; @@ -42,8 +42,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) } } var boxShape = new Box(0.5f, 0.5f, 2.5f); - boxShape.ComputeInertia(1, out var boxLocalInertia); - var boxDescription = BodyDescription.CreateDynamic(new Vector3(1, -0.5f, 0), boxLocalInertia, new(Simulation.Shapes.Add(boxShape), ContinuousDetection.Discrete(50, 50)), -1); + var boxDescription = BodyDescription.CreateDynamic(new Vector3(1, -0.5f, 0), boxShape.ComputeInertia(1), new(Simulation.Shapes.Add(boxShape), ContinuousDetection.Discrete(50, 50)), -1); Simulation.Bodies.Add(boxDescription); Simulation.Statics.Add(new StaticDescription(new Vector3(0, -3, 0), Simulation.Shapes.Add(new Box(4, 1, 4)))); diff --git a/Demos/SpecializedTests/ClothLatticeDemo.cs b/Demos/SpecializedTests/ClothLatticeDemo.cs index 0f0b353a3..3c5ca06e2 100644 --- a/Demos/SpecializedTests/ClothLatticeDemo.cs +++ b/Demos/SpecializedTests/ClothLatticeDemo.cs @@ -27,7 +27,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //Build a grid of shapes to be connected. var clothNodeShape = new Sphere(0.5f); - clothNodeShape.ComputeInertia(1, out var clothNodeInertia); + var clothNodeInertia = clothNodeShape.ComputeInertia(1); var clothNodeShapeIndex = Simulation.Shapes.Add(clothNodeShape); const int width = 128; const int length = 128; diff --git a/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs b/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs index cef8a3123..1046e3079 100644 --- a/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs +++ b/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs @@ -18,7 +18,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(25, 4, 40); camera.Yaw = 0; Simulation = Simulation.Create(BufferPool, - new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(30, 1), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 2f }, + new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(30, 1), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 2f }, new DemoPoseIntegratorCallbacks(new Vector3(0, -0.1f, 0)), new EmbeddedSubsteppingTimestepper2(3), 1); var shapeA = new Box(.75f, 1, .5f); @@ -28,8 +28,8 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var shapeIndexB = Simulation.Shapes.Add(shapeB); var collidableB = new CollidableDescription(shapeIndexB); var activity = new BodyActivityDescription(0.01f); - shapeA.ComputeInertia(1, out var inertiaA); - shapeA.ComputeInertia(1, out var inertiaB); + var inertiaA = shapeA.ComputeInertia(1); + var inertiaB = shapeB.ComputeInertia(1); for (int i = 0; i < 32; ++i) { diff --git a/Demos/SpecializedTests/ConstraintTestDemo.cs b/Demos/SpecializedTests/ConstraintTestDemo.cs index 0253db615..7fc59bba9 100644 --- a/Demos/SpecializedTests/ConstraintTestDemo.cs +++ b/Demos/SpecializedTests/ConstraintTestDemo.cs @@ -34,8 +34,8 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var shapeIndexB = Simulation.Shapes.Add(shapeB); var collidableB = new CollidableDescription(shapeIndexB); var activity = new BodyActivityDescription(0.01f); - shapeA.ComputeInertia(1, out var inertiaA); - shapeA.ComputeInertia(1, out var inertiaB); + var inertiaA = shapeA.ComputeInertia(1); + var inertiaB = shapeB.ComputeInertia(1); var nextX = -10f; { var x = GetNextPosition(ref nextX); diff --git a/Demos/SpecializedTests/ConvexHullTestDemo.cs b/Demos/SpecializedTests/ConvexHullTestDemo.cs index 58ba06f9d..26dc22eec 100644 --- a/Demos/SpecializedTests/ConvexHullTestDemo.cs +++ b/Demos/SpecializedTests/ConvexHullTestDemo.cs @@ -108,8 +108,7 @@ public override void Initialize(ContentArchive content, Camera camera) Console.WriteLine($"Hull computation time (us): {(end - start) * 1e6 / (iterationCount * Stopwatch.Frequency)}"); var hullShapeIndex = Simulation.Shapes.Add(hullShape); - hullShape.ComputeInertia(1, out var inertia); - + var inertia = hullShape.ComputeInertia(1); Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 0, 0), inertia, new (hullShapeIndex, ContinuousDetection.Discrete(20, 20)), 0.01f)); Simulation.Statics.Add(new StaticDescription(new Vector3(-25, -5, 0), Simulation.Shapes.Add(new Sphere(2)))); diff --git a/Demos/SpecializedTests/CylinderTestDemo.cs b/Demos/SpecializedTests/CylinderTestDemo.cs index 95a93160c..bcda7e0ee 100644 --- a/Demos/SpecializedTests/CylinderTestDemo.cs +++ b/Demos/SpecializedTests/CylinderTestDemo.cs @@ -173,8 +173,7 @@ public override void Initialize(ContentArchive content, Camera camera) Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, 0f, 0)), new PositionFirstTimestepper()); var cylinderShape = new Cylinder(1f, .2f); - cylinderShape.ComputeInertia(1, out var cylinderInertia); - var cylinder = BodyDescription.CreateDynamic(new Vector3(10f, 3, 0), cylinderInertia, new(Simulation.Shapes.Add(cylinderShape), ContinuousDetection.CreatePassive(1000f, 1000f)), 0.01f); + var cylinder = BodyDescription.CreateDynamic(new Vector3(10f, 3, 0), cylinderShape.ComputeInertia(1), new(Simulation.Shapes.Add(cylinderShape), ContinuousDetection.CreatePassive(1000f, 1000f)), 0.01f); Simulation.Bodies.Add(cylinder); Simulation.Bodies.Add(BodyDescription.CreateConvexKinematic(new RigidPose(new Vector3(0, -6, 0), QuaternionEx.CreateFromAxisAngle(Vector3.Normalize(new Vector3(1, 0, 1)), MathHelper.PiOver4)), Simulation.Shapes, new Sphere(2))); Simulation.Bodies.Add(BodyDescription.CreateConvexKinematic(new RigidPose(new Vector3(7, -6, 0), QuaternionEx.CreateFromAxisAngle(Vector3.Normalize(new Vector3(1, 0, 1)), MathHelper.PiOver4)), Simulation.Shapes, new Capsule(0.5f, 1f))); @@ -186,7 +185,7 @@ public override void Initialize(ContentArchive content, Camera camera) cylinderShape = new Cylinder(1f, 3); var cylinderShapeIndex = Simulation.Shapes.Add(cylinderShape); - cylinderShape.ComputeInertia(1, out cylinderInertia); + var cylinderInertia = cylinderShape.ComputeInertia(1); //const int rowCount = 15; //for (int rowIndex = 0; rowIndex < rowCount; ++rowIndex) //{ @@ -205,9 +204,9 @@ public override void Initialize(ContentArchive content, Camera camera) var box = new Box(1f, 3f, 2f); var capsule = new Capsule(1f, 1f); var sphere = new Sphere(1.5f); - box.ComputeInertia(1, out var boxInertia); - capsule.ComputeInertia(1, out var capsuleInertia); - sphere.ComputeInertia(1, out var sphereInertia); + var boxInertia = box.ComputeInertia(1); + var capsuleInertia = capsule.ComputeInertia(1); + var sphereInertia = sphere.ComputeInertia(1); var boxIndex = Simulation.Shapes.Add(box); var capsuleIndex = Simulation.Shapes.Add(capsule); var sphereIndex = Simulation.Shapes.Add(sphere); diff --git a/Demos/SpecializedTests/Media/ColosseumVideoDemo.cs b/Demos/SpecializedTests/Media/ColosseumVideoDemo.cs index 737cc0a00..97c90f156 100644 --- a/Demos/SpecializedTests/Media/ColosseumVideoDemo.cs +++ b/Demos/SpecializedTests/Media/ColosseumVideoDemo.cs @@ -74,8 +74,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); var ringBoxShape = new Box(0.5f, 1.5f, 3); - ringBoxShape.ComputeInertia(1, out var ringBoxInertia); - var boxDescription = BodyDescription.CreateDynamic(new Vector3(), ringBoxInertia, Simulation.Shapes.Add(ringBoxShape), 0.01f); + var boxDescription = BodyDescription.CreateDynamic(new Vector3(), ringBoxShape.ComputeInertia(1), Simulation.Shapes.Add(ringBoxShape), 0.01f); var layerPosition = new Vector3(); const int layerCount = 10; @@ -97,12 +96,10 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), Simulation.Shapes.Add(new Box(500, 1, 500)))); var bulletShape = new Sphere(0.5f); - bulletShape.ComputeInertia(.1f, out var bulletInertia); - bulletDescription = BodyDescription.CreateDynamic(new Vector3(), bulletInertia, Simulation.Shapes.Add(bulletShape), 0.01f); + bulletDescription = BodyDescription.CreateDynamic(new Vector3(), bulletShape.ComputeInertia(.1f), Simulation.Shapes.Add(bulletShape), 0.01f); var shootiePatootieShape = new Sphere(3f); - shootiePatootieShape.ComputeInertia(1000, out var shootiePatootieInertia); - shootiePatootieDescription = BodyDescription.CreateDynamic(new Vector3(), shootiePatootieInertia, Simulation.Shapes.Add(shootiePatootieShape), 0.01f); + shootiePatootieDescription = BodyDescription.CreateDynamic(new Vector3(), shootiePatootieShape.ComputeInertia(1000), Simulation.Shapes.Add(shootiePatootieShape), 0.01f); } bool characterActive; diff --git a/Demos/SpecializedTests/Media/NewtVideoDemo.cs b/Demos/SpecializedTests/Media/NewtVideoDemo.cs index dba025a4c..64d9a367b 100644 --- a/Demos/SpecializedTests/Media/NewtVideoDemo.cs +++ b/Demos/SpecializedTests/Media/NewtVideoDemo.cs @@ -45,8 +45,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Simulation.Statics.Add(new StaticDescription(new Vector3(0, -1.5f, 0), Simulation.Shapes.Add(new Sphere(3)))); var bulletShape = new Sphere(0.5f); - bulletShape.ComputeInertia(.25f, out var bulletInertia); - bulletDescription = BodyDescription.CreateDynamic(RigidPose.Identity, bulletInertia, Simulation.Shapes.Add(bulletShape), 0.01f); + bulletDescription = BodyDescription.CreateDynamic(RigidPose.Identity, bulletShape.ComputeInertia(.25f), Simulation.Shapes.Add(bulletShape), 0.01f); DemoMeshHelper.LoadModel(content, BufferPool, "Content\\newt.obj", new Vector3(20), out var mesh); Simulation.Statics.Add(new StaticDescription(new Vector3(200, 0.5f, 120), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, -3 * MathHelper.PiOver4), Simulation.Shapes.Add(mesh))); diff --git a/Demos/SpecializedTests/Media/PyramidVideoDemo.cs b/Demos/SpecializedTests/Media/PyramidVideoDemo.cs index 3921a5b4e..371caad67 100644 --- a/Demos/SpecializedTests/Media/PyramidVideoDemo.cs +++ b/Demos/SpecializedTests/Media/PyramidVideoDemo.cs @@ -25,7 +25,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); var boxShape = new Box(1, 1, 1); - boxShape.ComputeInertia(1, out var boxInertia); + var boxInertia = boxShape.ComputeInertia(1); var boxIndex = Simulation.Shapes.Add(boxShape); const int pyramidCount = 120; for (int pyramidIndex = 0; pyramidIndex < pyramidCount; ++pyramidIndex) diff --git a/Demos/SpecializedTests/MeshMeshTestDemo.cs b/Demos/SpecializedTests/MeshMeshTestDemo.cs index a8a410078..20b38d66d 100644 --- a/Demos/SpecializedTests/MeshMeshTestDemo.cs +++ b/Demos/SpecializedTests/MeshMeshTestDemo.cs @@ -24,7 +24,7 @@ public override void Initialize(ContentArchive content, Camera camera) Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); DemoMeshHelper.LoadModel(content, BufferPool, @"Content\newt.obj", Vector3.One, out var mesh); - new Box(2.5f, 1, 4).ComputeInertia(1, out var approximateInertia); + var approximateInertia = new Box(2.5f, 1, 4).ComputeInertia(1); var meshShapeIndex = Simulation.Shapes.Add(mesh); for (int meshIndex = 0; meshIndex < 3; ++meshIndex) { diff --git a/Demos/SpecializedTests/MeshReductionTestDemo.cs b/Demos/SpecializedTests/MeshReductionTestDemo.cs index 9b1b75669..77c5d6f1a 100644 --- a/Demos/SpecializedTests/MeshReductionTestDemo.cs +++ b/Demos/SpecializedTests/MeshReductionTestDemo.cs @@ -38,7 +38,7 @@ public override void Initialize(ContentArchive content, Camera camera) var bodyShape = new Compound(children); var bodyShapeIndex = Simulation.Shapes.Add(bodyShape); var wheelShape = new Cylinder(0.4f, .18f); - wheelShape.ComputeInertia(0.25f, out var wheelInertia); + var wheelInertia = wheelShape.ComputeInertia(0.25f); var wheelShapeIndex = Simulation.Shapes.Add(wheelShape); @@ -70,18 +70,18 @@ public override void Initialize(ContentArchive content, Camera camera) Simulation.Statics.Add(new StaticDescription(new Vector3(0, -15, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), Simulation.Shapes.Add(planeMesh))); var testBox = new Box(3, 3, 3); - testBox.ComputeInertia(1, out var testBoxInertia); + var testBoxInertia = testBox.ComputeInertia(1); Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 10, 0), testBoxInertia, new(Simulation.Shapes.Add(testBox), ContinuousDetection.Discrete(10, 10)), -1)); var testSphere = new Sphere(.1f); - testSphere.ComputeInertia(1, out var testSphereInertia); + var testSphereInertia = testSphere.ComputeInertia(1); //testSphereInertia.InverseInertiaTensor = default; Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(10, 10, 0), testSphereInertia, new(Simulation.Shapes.Add(testSphere), ContinuousDetection.Discrete(10, 10)), -1)); var testCylinder = new Cylinder(1.5f, 2f); - testCylinder.ComputeInertia(1, out var testCylinderInertia); + var testCylinderInertia = testCylinder.ComputeInertia(1); //testCylinderInertia.InverseInertiaTensor = default; Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(15, 10, 0), testCylinderInertia, new(Simulation.Shapes.Add(testCylinder), ContinuousDetection.Discrete(10, 10)), -1)); var testCapsule = new Capsule(.1f, 2f); - testCapsule.ComputeInertia(1, out var testCapsuleInertia); + var testCapsuleInertia = testCapsule.ComputeInertia(1); //testCapsuleInertia.InverseInertiaTensor = default; Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(18, 10, 0), testCapsuleInertia, new(Simulation.Shapes.Add(testCapsule), ContinuousDetection.Discrete(10, 10)), -1)); @@ -95,8 +95,7 @@ public override void Initialize(ContentArchive content, Camera camera) points.AllocateUnsafely() = new Vector3(2, 2, 0); points.AllocateUnsafely() = new Vector3(2, 2, 2); var convexHull = new ConvexHull(points, BufferPool, out _); - convexHull.ComputeInertia(1, out var convexHullInertia); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(23, 10, 0), convexHullInertia, new(Simulation.Shapes.Add(convexHull), ContinuousDetection.Discrete(10, 10)), -1)); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(23, 10, 0), convexHull.ComputeInertia(1), new(Simulation.Shapes.Add(convexHull), ContinuousDetection.Discrete(10, 10)), -1)); //var sphere = new Sphere(1.5f); //var capsule = new Capsule(1f, 1f); @@ -157,9 +156,8 @@ public override void Initialize(ContentArchive content, Camera camera) case 2: default: var box = new Box(1 + 128 * random.NextSingle(), 1 + 128 * random.NextSingle(), 1 + 128 * random.NextSingle()); - box.ComputeInertia(1, out var boxInertia); bodyDescription.Collidable.Shape = Simulation.Shapes.Add(box); - bodyDescription.LocalInertia = boxInertia; + bodyDescription.LocalInertia = box.ComputeInertia(1); break; //case 3: // bodyDescription.Collidable.Shape = cylinderIndex; diff --git a/Demos/SpecializedTests/MeshSerializationTestDemo.cs b/Demos/SpecializedTests/MeshSerializationTestDemo.cs index 61a7453eb..4dbec379c 100644 --- a/Demos/SpecializedTests/MeshSerializationTestDemo.cs +++ b/Demos/SpecializedTests/MeshSerializationTestDemo.cs @@ -41,8 +41,7 @@ public override void Initialize(ContentArchive content, Camera camera) var random = new Random(5); var shapeToDrop = new Box(1, 1, 1); - shapeToDrop.ComputeInertia(1, out var shapeToDropInertia); - var descriptionToDrop = BodyDescription.CreateDynamic(new Vector3(), shapeToDropInertia, Simulation.Shapes.Add(shapeToDrop), 0.01f); + var descriptionToDrop = BodyDescription.CreateDynamic(new Vector3(), shapeToDrop.ComputeInertia(1), Simulation.Shapes.Add(shapeToDrop), 0.01f); for (int i = 0; i < 1024; ++i) { descriptionToDrop.Pose.Position = new Vector3(8 + 240 * random.NextSingle(), 10 + 10 * random.NextSingle(), 8 + 112 * random.NextSingle()); diff --git a/Demos/SpecializedTests/MeshTestDemo.cs b/Demos/SpecializedTests/MeshTestDemo.cs index 068e8ca5b..f3debfb8c 100644 --- a/Demos/SpecializedTests/MeshTestDemo.cs +++ b/Demos/SpecializedTests/MeshTestDemo.cs @@ -24,9 +24,9 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var box = new Box(1f, 3f, 2f); var capsule = new Capsule(1f, 1f); var sphere = new Sphere(1f); - box.ComputeInertia(1, out var boxInertia); - capsule.ComputeInertia(1, out var capsuleInertia); - sphere.ComputeInertia(1, out var sphereInertia); + var boxInertia = box.ComputeInertia(1); + var capsuleInertia = capsule.ComputeInertia(1); + var sphereInertia = sphere.ComputeInertia(1); var boxIndex = Simulation.Shapes.Add(box); var capsuleIndex = Simulation.Shapes.Add(capsule); var sphereIndex = Simulation.Shapes.Add(sphere); diff --git a/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs b/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs index 99e22dc01..c1cd50225 100644 --- a/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs +++ b/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs @@ -25,7 +25,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); var boxShape = new Box(1, 1, 1); - boxShape.ComputeInertia(1, out var boxInertia); + var boxInertia = boxShape.ComputeInertia(1); var boxIndex = Simulation.Shapes.Add(boxShape); const int pyramidCount = 10; for (int pyramidIndex = 0; pyramidIndex < pyramidCount; ++pyramidIndex) @@ -60,10 +60,8 @@ public override void Update(Window window, Camera camera, Input input, float dt) if (frameIndex % 64 == 0) { var bulletShape = new Sphere(0.5f + 5 * random.NextSingle()); - bulletShape.ComputeInertia(bulletShape.Radius * bulletShape.Radius * bulletShape.Radius, out var bulletInertia); var bulletShapeIndex = Simulation.Shapes.Add(bulletShape); - var bodyDescription = BodyDescription.CreateConvexDynamic( - new Vector3(0, 8, -130), new BodyVelocity(new Vector3(0, 0, 350)), bulletShape.Radius * bulletShape.Radius * bulletShape.Radius, Simulation.Shapes, bulletShape); + var bodyDescription = BodyDescription.CreateDynamic(new Vector3(0, 8, -130), new(new Vector3(0, 0, 350)), bulletShape.ComputeInertia(bulletShape.Radius * bulletShape.Radius * bulletShape.Radius), Simulation.Shapes.Add(bulletShape), 0.01f); Simulation.Bodies.Add(bodyDescription); } if (frameIndex % 192 == 0) diff --git a/Demos/SpecializedTests/ShapePileTestDemo.cs b/Demos/SpecializedTests/ShapePileTestDemo.cs index 6cf1dec0e..33113b830 100644 --- a/Demos/SpecializedTests/ShapePileTestDemo.cs +++ b/Demos/SpecializedTests/ShapePileTestDemo.cs @@ -44,11 +44,11 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //points.AllocateUnsafely() = new Vector3(0, 1, 0) + Vector3.Normalize(new Vector3(random.NextSingle() * 2 - 1, random.NextSingle() * 2 - 1, random.NextSingle() * 2 - 1)) * random.NextSingle(); } var convexHull = new ConvexHull(points.Span.Slice(points.Count), BufferPool, out _); - box.ComputeInertia(1, out var boxInertia); - capsule.ComputeInertia(1, out var capsuleInertia); - sphere.ComputeInertia(1, out var sphereInertia); - cylinder.ComputeInertia(1, out var cylinderInertia); - convexHull.ComputeInertia(1, out var hullInertia); + var boxInertia = box.ComputeInertia(1); + var capsuleInertia = capsule.ComputeInertia(1); + var sphereInertia = sphere.ComputeInertia(1); + var cylinderInertia = cylinder.ComputeInertia(1); + var hullInertia = convexHull.ComputeInertia(1); var boxIndex = Simulation.Shapes.Add(box); var capsuleIndex = Simulation.Shapes.Add(capsule); var sphereIndex = Simulation.Shapes.Add(sphere); diff --git a/Demos/SpecializedTests/SolverBatchTestDemo.cs b/Demos/SpecializedTests/SolverBatchTestDemo.cs index 3317a90b1..908df66ff 100644 --- a/Demos/SpecializedTests/SolverBatchTestDemo.cs +++ b/Demos/SpecializedTests/SolverBatchTestDemo.cs @@ -25,7 +25,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //Build a grid of shapes to be connected. var clothNodeShape = new Sphere(0.5f); - clothNodeShape.ComputeInertia(1, out var clothNodeInertia); + var clothNodeInertia = clothNodeShape.ComputeInertia(1); var clothNodeShapeIndex = Simulation.Shapes.Add(clothNodeShape); const int width = 128; const int length = 128; diff --git a/Demos/SpecializedTests/TriangleTestDemo.cs b/Demos/SpecializedTests/TriangleTestDemo.cs index 6f4627bba..a27cfc210 100644 --- a/Demos/SpecializedTests/TriangleTestDemo.cs +++ b/Demos/SpecializedTests/TriangleTestDemo.cs @@ -134,16 +134,16 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 2, 0), new BodyInertia { InverseMass = 1 }, new(Simulation.Shapes.Add(new Sphere(1.75f)), ContinuousDetection.Discrete(0.1f, 0.1f)), -1)); Simulation.Bodies.Add(BodyDescription.CreateDynamic(new RigidPose(new Vector3(20, 2, 3), Quaternion.CreateFromYawPitchRoll(0f, 1.745329E-05f, 0f)), new BodyInertia { InverseMass = 1 }, new(Simulation.Shapes.Add(new Capsule(1, 2)), ContinuousDetection.Discrete(0.1f, 0.1f)), -1)); var testBox = new Box(2, 3, 2); - testBox.ComputeInertia(1, out var testBoxInertia); + var testBoxInertia = testBox.ComputeInertia(1); Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 2, 6), testBoxInertia, new(Simulation.Shapes.Add(testBox), ContinuousDetection.Discrete(10.1f, 10.1f)), -1)); var cylinder = new Cylinder(1.75f, 2); - cylinder.ComputeInertia(1, out var cylinderInertia); + var cylinderInertia = cylinder.ComputeInertia(1); //cylinderInertia.InverseInertiaTensor = default; Simulation.Bodies.Add(BodyDescription.CreateDynamic(new RigidPose(new Vector3(20, 2, 9), Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), MathF.PI / 2f)), cylinderInertia, new(Simulation.Shapes.Add(cylinder), ContinuousDetection.Discrete(5f, 5f)), -1)); var cylinder2 = new Cylinder(.5f, 0.5f); - cylinder2.ComputeInertia(1, out var cylinder2Inertia); + var cylinder2Inertia = cylinder2.ComputeInertia(1); Simulation.Bodies.Add(BodyDescription.CreateDynamic(new RigidPose(new Vector3(23, 2, 9), Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), 0)), cylinder2Inertia, new(Simulation.Shapes.Add(cylinder2), ContinuousDetection.Discrete(5f, 5f)), -1)); var points = new QuickList(8, BufferPool); points.AllocateUnsafely() = new Vector3(0, 0, 0); @@ -155,7 +155,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) points.AllocateUnsafely() = new Vector3(2, 2, 0); points.AllocateUnsafely() = new Vector3(2, 2, 2); var convexHull = new ConvexHull(points, BufferPool, out _); - convexHull.ComputeInertia(1, out var convexHullInertia); + var convexHullInertia = convexHull.ComputeInertia(1); Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 2, 12), convexHullInertia, new(Simulation.Shapes.Add(convexHull), ContinuousDetection.Discrete(0.1f, 0.1f)), -1)); Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(23, 2, 12), convexHullInertia, new(Simulation.Shapes.Add(convexHull), ContinuousDetection.Discrete(0.1f, 0.1f)), -1)); From e89bb3dae2ef0c98696df9e2829509f34660baeb Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 3 Dec 2021 16:58:01 -0600 Subject: [PATCH 305/947] RigidPose/BodyVelocity now have more helper implicit casts. --- BepuPhysics/BodyDescription.cs | 134 +----------------- BepuPhysics/BodyProperties.cs | 47 +++++- BepuPhysics/Collidables/Mesh.cs | 2 +- .../ConvexHomogeneousCompoundSweepTask.cs | 2 +- BepuPhysics/StaticDescription.cs | 19 ++- DemoTests/PairDeterminismTests.cs | 12 +- Demos/Demos/BlockChainDemo.cs | 7 +- Demos/Demos/Cars/CarDemo.cs | 6 +- Demos/Demos/Characters/CharacterDemo.cs | 4 +- Demos/Demos/ClothDemo.cs | 6 +- Demos/Demos/CollisionQueryDemo.cs | 2 +- Demos/Demos/ColosseumDemo.cs | 9 +- Demos/Demos/CompoundTestDemo.cs | 4 +- .../Demos/ContinuousCollisionDetectionDemo.cs | 4 +- Demos/Demos/CustomVoxelCollidableDemo.cs | 6 +- Demos/Demos/FountainStressTestDemo.cs | 2 +- Demos/Demos/NewtDemo.cs | 3 +- Demos/Demos/PlanetDemo.cs | 2 +- Demos/Demos/PyramidDemo.cs | 2 +- Demos/Demos/RayCastingDemo.cs | 2 +- Demos/Demos/RopeStabilityDemo.cs | 4 +- Demos/Demos/Sponsors/SponsorNewt.cs | 2 +- Demos/Demos/SubsteppingDemo.cs | 2 +- Demos/Demos/Tanks/Tank.cs | 2 +- Demos/Demos/Tanks/TankDemo.cs | 13 +- Demos/SpecializedTests/CapsuleTestDemo.cs | 2 +- .../CompoundCollisionIndicesTest.cs | 6 +- Demos/SpecializedTests/ConstraintTestDemo.cs | 4 +- Demos/SpecializedTests/ConvexHullTestDemo.cs | 2 +- Demos/SpecializedTests/CylinderTestDemo.cs | 10 +- Demos/SpecializedTests/GyroscopeTestDemo.cs | 8 +- Demos/SpecializedTests/Media/BedsheetDemo.cs | 2 +- .../Media/ColosseumVideoDemo.cs | 7 +- .../Media/PyramidVideoDemo.cs | 2 +- .../SpecializedTests/MeshReductionTestDemo.cs | 2 +- Demos/SpecializedTests/MeshTestDemo.cs | 2 +- .../PyramidAwakenerTestDemo.cs | 3 +- Demos/SpecializedTests/RagdollTubeDemo.cs | 6 +- Demos/SpecializedTests/TriangleTestDemo.cs | 6 +- Demos/SpecializedTests/VolumeQueryTests.cs | 2 +- 40 files changed, 141 insertions(+), 221 deletions(-) diff --git a/BepuPhysics/BodyDescription.cs b/BepuPhysics/BodyDescription.cs index 61bb9b336..fd49393cc 100644 --- a/BepuPhysics/BodyDescription.cs +++ b/BepuPhysics/BodyDescription.cs @@ -128,33 +128,6 @@ public static BodyDescription CreateDynamic(in RigidPose pose, in BodyInertia in return new BodyDescription { Pose = pose, LocalInertia = inertia, Activity = activity, Collidable = collidable }; } - /// - /// Creates a dynamic body description with identity orientation. - /// - /// Position of the body. - /// Initial velocity of the body. - /// Local inertia of the body. - /// Collidable to associate with the body. - /// Activity settings for the body. - /// Constructed description for the body. - public static BodyDescription CreateDynamic(in Vector3 position, in BodyVelocity velocity, in BodyInertia inertia, in CollidableDescription collidable, in BodyActivityDescription activity) - { - return new BodyDescription { Pose = new(position), Velocity = velocity, LocalInertia = inertia, Activity = activity, Collidable = collidable }; - } - - /// - /// Creates a dynamic body description with zero initial velocity and identity orientation. - /// - /// Position of the body. - /// Local inertia of the body. - /// Collidable to associate with the body. - /// Activity settings for the body. - /// Constructed description for the body. - public static BodyDescription CreateDynamic(in Vector3 position, in BodyInertia inertia, in CollidableDescription collidable, in BodyActivityDescription activity) - { - return new BodyDescription { Pose = new(position), LocalInertia = inertia, Activity = activity, Collidable = collidable }; - } - /// /// Creates a dynamic body description with collidable, inertia, and activity descriptions generated from a convex shape. Adds the shape to the given shape set. /// @@ -165,9 +138,7 @@ public static BodyDescription CreateDynamic(in Vector3 position, in BodyInertia /// Shape collection to add the shape to. /// Shape to add to the shape set and to create the body from. /// Constructed description for the body. - public static BodyDescription CreateConvexDynamic( - in RigidPose pose, in BodyVelocity velocity, float mass, Shapes shapes, in TConvexShape shape) - where TConvexShape : unmanaged, IConvexShape + public static BodyDescription CreateConvexDynamic(in RigidPose pose, in BodyVelocity velocity, float mass, Shapes shapes, in TConvexShape shape) where TConvexShape : unmanaged, IConvexShape { var description = new BodyDescription { @@ -180,23 +151,6 @@ public static BodyDescription CreateConvexDynamic( return description; } - /// - /// Creates a dynamic body description with identity orientation and collidable, inertia, and activity descriptions generated from a convex shape. Adds the shape to the given shape set. - /// - /// Type of the shape to create a body for. - /// Position of the body. - /// Initial velocity of the body. - /// Mass of the body. The inertia tensor will be calculated based on this mass and the shape. - /// Shape collection to add the shape to. - /// Shape to add to the shape set and to create the body from. - /// Constructed description for the body. - public static BodyDescription CreateConvexDynamic( - in Vector3 position, in BodyVelocity velocity, float mass, Shapes shapes, in TConvexShape shape) - where TConvexShape : unmanaged, IConvexShape - { - return CreateConvexDynamic(new RigidPose(position), velocity, mass, shapes, shape); - } - /// /// Creates a dynamic body description with zero initial velocity and collidable, inertia, and activity descriptions generated from a convex shape. Adds the shape to the given shape set. /// @@ -206,29 +160,11 @@ public static BodyDescription CreateConvexDynamic( /// Shape collection to add the shape to. /// Shape to add to the shape set and to create the body from. /// Constructed description for the body. - public static BodyDescription CreateConvexDynamic( - in RigidPose pose, float mass, Shapes shapes, in TConvexShape shape) - where TConvexShape : unmanaged, IConvexShape + public static BodyDescription CreateConvexDynamic(in RigidPose pose, float mass, Shapes shapes, in TConvexShape shape) where TConvexShape : unmanaged, IConvexShape { return CreateConvexDynamic(pose, default, mass, shapes, shape); } - /// - /// Creates a dynamic body description with zero initial velocity, identity orientation, and collidable, inertia, and activity descriptions generated from a convex shape. Adds the shape to the given shape set. - /// - /// Type of the shape to create a body for. - /// Position of the body. - /// Mass of the body. The inertia tensor will be calculated based on this mass and the shape. - /// Shape collection to add the shape to. - /// Shape to add to the shape set and to create the body from. - /// Constructed description for the body. - public static BodyDescription CreateConvexDynamic( - in Vector3 position, float mass, Shapes shapes, in TConvexShape shape) - where TConvexShape : unmanaged, IConvexShape - { - return CreateConvexDynamic(new RigidPose(position), default, mass, shapes, shape); - } - /// /// Creates a kinematic body description. /// @@ -254,31 +190,6 @@ public static BodyDescription CreateKinematic(in RigidPose pose, in CollidableDe return new BodyDescription { Pose = pose, Activity = activity, Collidable = collidable }; } - /// - /// Creates a kinematic body description with identity orientation. - /// - /// Position of the body. - /// Initial velocity of the body. - /// Collidable to associate with the body. - /// Activity settings for the body. - /// Constructed description for the body. - public static BodyDescription CreateKinematic(in Vector3 position, in BodyVelocity velocity, in CollidableDescription collidable, in BodyActivityDescription activity) - { - return new BodyDescription { Pose = new(position), Velocity = velocity, Activity = activity, Collidable = collidable }; - } - - /// - /// Creates a kinematic body description with identity orientation and zero initial velocity. - /// - /// Position of the body. - /// Collidable to associate with the body. - /// Activity settings for the body. - /// Constructed description for the body. - public static BodyDescription CreateKinematic(in Vector3 position, in CollidableDescription collidable, in BodyActivityDescription activity) - { - return new BodyDescription { Pose = new(position), Activity = activity, Collidable = collidable }; - } - /// /// Creates a kinematic body description with collidable and activity descriptions generated from a convex shape. Adds the shape to the given shape set. /// @@ -288,9 +199,7 @@ public static BodyDescription CreateKinematic(in Vector3 position, in Collidable /// Shape collection to add the shape to. /// Shape to add to the shape set and to create the body from. /// Constructed description for the body. - public static BodyDescription CreateConvexKinematic( - in RigidPose pose, in BodyVelocity velocity, Shapes shapes, in TConvexShape shape) - where TConvexShape : unmanaged, IConvexShape + public static BodyDescription CreateConvexKinematic(in RigidPose pose, in BodyVelocity velocity, Shapes shapes, in TConvexShape shape) where TConvexShape : unmanaged, IConvexShape { var description = new BodyDescription { @@ -302,21 +211,6 @@ public static BodyDescription CreateConvexKinematic( return description; } - /// - /// Creates a kinematic body description with identity orientation and collidable and activity descriptions generated from a convex shape. Adds the shape to the given shape set. - /// - /// Type of the shape to create a body for. - /// Position of the body. - /// Initial velocity of the body. - /// Shape collection to add the shape to. - /// Shape to add to the shape set and to create the body from. - /// Constructed description for the body. - public static BodyDescription CreateConvexKinematic( - in Vector3 position, in BodyVelocity velocity, Shapes shapes, in TConvexShape shape) - where TConvexShape : unmanaged, IConvexShape - { - return CreateConvexKinematic(new RigidPose(position), velocity, shapes, shape); - } /// /// Creates a kinematic body description with zero initial velocity and collidable and activity descriptions generated from a convex shape. Adds the shape to the given shape set. @@ -326,29 +220,9 @@ public static BodyDescription CreateConvexKinematic( /// Shape collection to add the shape to. /// Shape to add to the shape set and to create the body from. /// Constructed description for the body. - public static BodyDescription CreateConvexKinematic( - in RigidPose pose, Shapes shapes, in TConvexShape shape) - where TConvexShape : unmanaged, IConvexShape + public static BodyDescription CreateConvexKinematic(in RigidPose pose, Shapes shapes, in TConvexShape shape) where TConvexShape : unmanaged, IConvexShape { return CreateConvexKinematic(pose, default, shapes, shape); } - - /// - /// Creates a kinematic body description with zero initial velocity, identity orientation, and collidable and activity descriptions generated from a convex shape. Adds the shape to the given shape set. - /// - /// Type of the shape to create a body for. - /// Position of the body. - /// Shape collection to add the shape to. - /// Shape to add to the shape set and to create the body from. - /// Constructed description for the body. - public static BodyDescription CreateConvexKinematic( - in Vector3 position, Shapes shapes, in TConvexShape shape) - where TConvexShape : unmanaged, IConvexShape - { - return CreateConvexKinematic(new RigidPose(position), default, shapes, shape); - } - } - - } diff --git a/BepuPhysics/BodyProperties.cs b/BepuPhysics/BodyProperties.cs index d52f84cf2..702b5d8af 100644 --- a/BepuPhysics/BodyProperties.cs +++ b/BepuPhysics/BodyProperties.cs @@ -79,7 +79,7 @@ public struct RigidPose /// /// Returns a pose with a position at (0,0,0) and identity orientation. /// - public static RigidPose Identity => new RigidPose(new Vector3()); + public static RigidPose Identity => new RigidPose(default); /// /// Creates a rigid pose with the given position and orientation. @@ -104,6 +104,33 @@ public RigidPose(Vector3 position) Orientation = Quaternion.Identity; } + /// + /// Creates a pose by treating a as a position. Orientation is set to identity. + /// + /// Position to use in the pose. + public static implicit operator RigidPose(Vector3 position) + { + return new RigidPose(position); + } + + /// + /// Creates a pose by treating a as an orientation in the pose. Position is set to zero. + /// + /// Orientation to use in the pose. + public static implicit operator RigidPose(Quaternion orientation) + { + return new RigidPose(default, orientation); + } + + /// + /// Creates a pose from a tuple of a position and orientation. + /// + /// Position and orientation to use in the pose. + public static implicit operator RigidPose((Vector3 position, Quaternion orientation) poseComponents) + { + return new RigidPose(poseComponents.position, poseComponents.orientation); + } + /// /// Transforms a vector by the rigid pose: v * pose.Orientation + pose.Position. /// @@ -195,6 +222,24 @@ public BodyVelocity(Vector3 linear, Vector3 angular) Linear = linear; Angular = angular; } + + /// + /// Creates a body velocity by treating a as a linear velocity. Angular velocity is set to zero. + /// + /// Linear velocity to use in the body velocity. + public static implicit operator BodyVelocity(Vector3 linearVelocity) + { + return new BodyVelocity(linearVelocity); + } + + /// + /// Creates a body velocity from a tuple of linear and angular velocities.. + /// + /// Velocities to use in the body velocity. + public static implicit operator BodyVelocity((Vector3 linearVelocity, Vector3 angularVelocity) velocities) + { + return new BodyVelocity(velocities.linearVelocity, velocities.angularVelocity); + } } /// diff --git a/BepuPhysics/Collidables/Mesh.cs b/BepuPhysics/Collidables/Mesh.cs index 5381c6851..cc5e28534 100644 --- a/BepuPhysics/Collidables/Mesh.cs +++ b/BepuPhysics/Collidables/Mesh.cs @@ -151,7 +151,7 @@ public readonly unsafe void GetLocalChild(int triangleIndex, out Triangle target public readonly unsafe void GetPosedLocalChild(int triangleIndex, out Triangle target, out RigidPose childPose) { GetLocalChild(triangleIndex, out target); - childPose = new RigidPose((target.A + target.B + target.C) * (1f / 3f)); + childPose = (target.A + target.B + target.C) * (1f / 3f); target.A -= childPose.Position; target.B -= childPose.Position; target.C -= childPose.Position; diff --git a/BepuPhysics/CollisionDetection/SweepTasks/ConvexHomogeneousCompoundSweepTask.cs b/BepuPhysics/CollisionDetection/SweepTasks/ConvexHomogeneousCompoundSweepTask.cs index 9b7198ee0..d78aa244c 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/ConvexHomogeneousCompoundSweepTask.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/ConvexHomogeneousCompoundSweepTask.cs @@ -45,7 +45,7 @@ protected override unsafe bool PreorderedTypeSweep( { compound.GetPosedLocalChild(childIndex, out var childShape, out var childPose); if (task.Sweep( - shapeDataA, ShapeTypeIndexA, new RigidPose(Vector3.Zero, Quaternion.Identity), orientationA, velocityA, + shapeDataA, ShapeTypeIndexA, RigidPose.Identity, orientationA, velocityA, Unsafe.AsPointer(ref childShape), Triangle.Id, childPose, offsetB, orientationB, velocityB, maximumT, minimumProgression, convergenceThreshold, maximumIterationCount, out var t0Candidate, out var t1Candidate, out var hitLocationCandidate, out var hitNormalCandidate)) diff --git a/BepuPhysics/StaticDescription.cs b/BepuPhysics/StaticDescription.cs index eeaab211e..e6f86c380 100644 --- a/BepuPhysics/StaticDescription.cs +++ b/BepuPhysics/StaticDescription.cs @@ -17,13 +17,24 @@ public struct StaticDescription /// public CollidableDescription Collidable; + /// + /// Builds a new static description. + /// + /// Pose of the static collidable. + /// Collidable description for the static. + public StaticDescription(RigidPose pose, CollidableDescription collidable) + { + Pose = pose; + Collidable = collidable; + } + /// /// Builds a new static description. /// /// Position of the static. /// Orientation of the static. /// Collidable description for the static. - public StaticDescription(in Vector3 position, in Quaternion orientation, in CollidableDescription collidable) + public StaticDescription(Vector3 position, Quaternion orientation, CollidableDescription collidable) { Pose.Position = position; Pose.Orientation = orientation; @@ -35,7 +46,7 @@ public StaticDescription(in Vector3 position, in Quaternion orientation, in Coll /// /// Position of the static. /// Collidable description for the static. - public StaticDescription(in Vector3 position, in CollidableDescription collidable) : this(position, Quaternion.Identity, collidable) + public StaticDescription(Vector3 position, CollidableDescription collidable) : this(position, Quaternion.Identity, collidable) { } @@ -45,7 +56,7 @@ public StaticDescription(in Vector3 position, in CollidableDescription collidabl /// Position of the static. /// Orientation of the static. /// Index of the static's shape in the simulation shapes set. - public StaticDescription(in Vector3 position, in Quaternion orientation, TypedIndex shapeIndex) + public StaticDescription(Vector3 position, Quaternion orientation, TypedIndex shapeIndex) { Pose.Position = position; Pose.Orientation = orientation; @@ -58,7 +69,7 @@ public StaticDescription(in Vector3 position, in Quaternion orientation, TypedIn /// /// Position of the static. /// Index of the static's shape in the simulation shapes set. - public StaticDescription(in Vector3 position, TypedIndex shapeIndex) : this(position, Quaternion.Identity, shapeIndex) + public StaticDescription(Vector3 position, TypedIndex shapeIndex) : this(position, Quaternion.Identity, shapeIndex) { } } diff --git a/DemoTests/PairDeterminismTests.cs b/DemoTests/PairDeterminismTests.cs index f86204e96..f80c6334c 100644 --- a/DemoTests/PairDeterminismTests.cs +++ b/DemoTests/PairDeterminismTests.cs @@ -241,12 +241,12 @@ public static void Test() var compoundBuilder = new CompoundBuilder(pool, shapes, 6); - compoundBuilder.AddForKinematic(sphereCollidable.Shape, new RigidPose(new Vector3(2, 0, 0)), 1); - compoundBuilder.AddForKinematic(capsuleCollidable.Shape, new RigidPose(new Vector3(0, 2, 0)), 1); - compoundBuilder.AddForKinematic(boxCollidable.Shape, new RigidPose(new Vector3(0, 0, 2)), 1); - compoundBuilder.AddForKinematic(triangleCollidable.Shape, new RigidPose(new Vector3(-2, 0, 1)), 1); - compoundBuilder.AddForKinematic(cylinderCollidable.Shape, new RigidPose(new Vector3(0, -2, 1)), 1); - compoundBuilder.AddForKinematic(hullCollidable.Shape, new RigidPose(new Vector3(0, 0, -2)), 1); + compoundBuilder.AddForKinematic(sphereCollidable.Shape, new Vector3(2, 0, 0), 1); + compoundBuilder.AddForKinematic(capsuleCollidable.Shape, new Vector3(0, 2, 0), 1); + compoundBuilder.AddForKinematic(boxCollidable.Shape, new Vector3(0, 0, 2), 1); + compoundBuilder.AddForKinematic(triangleCollidable.Shape, new Vector3(-2, 0, 1), 1); + compoundBuilder.AddForKinematic(cylinderCollidable.Shape, new Vector3(0, -2, 1), 1); + compoundBuilder.AddForKinematic(hullCollidable.Shape, new Vector3(0, 0, -2), 1); compoundBuilder.BuildKinematicCompound(out var children, out _); var compound = new Compound(children); var bigCompound = new BigCompound(children, shapes, pool); diff --git a/Demos/Demos/BlockChainDemo.cs b/Demos/Demos/BlockChainDemo.cs index b2b9e67dc..8295f390e 100644 --- a/Demos/Demos/BlockChainDemo.cs +++ b/Demos/Demos/BlockChainDemo.cs @@ -84,10 +84,9 @@ public override void Update(Window window, Camera camera, Input input, float dt) else direction = new Vector3(0, 1, 0); - coinDescription.Pose = new( - origin + direction * 10 * random.NextSingle(), - QuaternionEx.Normalize(new Quaternion(0.01f + random.NextSingle(), random.NextSingle(), random.NextSingle(), random.NextSingle()))); - coinDescription.Velocity = new(direction * (5 + 30 * random.NextSingle())); + coinDescription.Pose = (origin + direction * 10 * random.NextSingle(), + QuaternionEx.Normalize(new(0.01f + random.NextSingle(), random.NextSingle(), random.NextSingle(), random.NextSingle()))); + coinDescription.Velocity = direction * (5 + 30 * random.NextSingle()); Simulation.Bodies.Add(coinDescription); } } diff --git a/Demos/Demos/Cars/CarDemo.cs b/Demos/Demos/Cars/CarDemo.cs index 95fbd8867..c7b3907d4 100644 --- a/Demos/Demos/Cars/CarDemo.cs +++ b/Demos/Demos/Cars/CarDemo.cs @@ -50,7 +50,7 @@ public override void Initialize(ContentArchive content, Camera camera) var builder = new CompoundBuilder(BufferPool, Simulation.Shapes, 2); builder.Add(new Box(1.85f, 0.7f, 4.73f), RigidPose.Identity, 10); - builder.Add(new Box(1.85f, 0.6f, 2.5f), new RigidPose(new Vector3(0, 0.65f, -0.35f)), 0.5f); + builder.Add(new Box(1.85f, 0.6f, 2.5f), new Vector3(0, 0.65f, -0.35f), 0.5f); builder.BuildDynamicCompound(out var children, out var bodyInertia, out _); builder.Dispose(); var bodyShape = new Compound(children); @@ -66,7 +66,7 @@ public override void Initialize(ContentArchive content, Camera camera) const float wheelBaseWidth = x * 2; const float wheelBaseLength = frontZ - backZ; - playerController = new SimpleCarController(SimpleCar.Create(Simulation, properties, new RigidPose(new Vector3(0, 10, 0), Quaternion.Identity), bodyShapeIndex, bodyInertia, 0.5f, wheelShapeIndex, wheelInertia, 2f, + playerController = new SimpleCarController(SimpleCar.Create(Simulation, properties, new Vector3(0, 10, 0), bodyShapeIndex, bodyInertia, 0.5f, wheelShapeIndex, wheelInertia, 2f, new Vector3(-x, y, frontZ), new Vector3(x, y, frontZ), new Vector3(-x, y, backZ), new Vector3(x, y, backZ), new Vector3(0, -1, 0), 0.25f, new SpringSettings(5f, 0.7f), QuaternionEx.CreateFromAxisAngle(Vector3.UnitZ, MathF.PI * 0.5f)), forwardSpeed: 75, forwardForce: 6, zoomMultiplier: 2, backwardSpeed: 30, backwardForce: 4, idleForce: 0.25f, brakeForce: 7, steeringSpeed: 1.5f, maximumSteeringAngle: MathF.PI * 0.23f, @@ -107,7 +107,7 @@ public override void Initialize(ContentArchive content, Camera camera) //The AI cars are very similar, except... we handicap them a little to make the player good about themselves. var position = min + span * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); var orientation = QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), random.NextSingle() * MathF.PI * 2); - aiControllers[i].Controller = new SimpleCarController(SimpleCar.Create(Simulation, properties, new RigidPose(position, orientation), bodyShapeIndex, bodyInertia, 0.5f, wheelShapeIndex, wheelInertia, 2f, + aiControllers[i].Controller = new SimpleCarController(SimpleCar.Create(Simulation, properties, (position, orientation), bodyShapeIndex, bodyInertia, 0.5f, wheelShapeIndex, wheelInertia, 2f, new Vector3(-x, y, frontZ), new Vector3(x, y, frontZ), new Vector3(-x, y, backZ), new Vector3(x, y, backZ), new Vector3(0, -1, 0), 0.25f, new SpringSettings(5, 0.7f), QuaternionEx.CreateFromAxisAngle(Vector3.UnitZ, MathF.PI * 0.5f)), forwardSpeed: 50, forwardForce: 5, zoomMultiplier: 2, backwardSpeed: 10, backwardForce: 4, idleForce: 0.25f, brakeForce: 7, steeringSpeed: 1.5f, maximumSteeringAngle: MathF.PI * 0.23f, diff --git a/Demos/Demos/Characters/CharacterDemo.cs b/Demos/Demos/Characters/CharacterDemo.cs index 411313c1d..ffd7e5581 100644 --- a/Demos/Demos/Characters/CharacterDemo.cs +++ b/Demos/Demos/Characters/CharacterDemo.cs @@ -48,10 +48,10 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) switch (choice) { case 0: - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new RigidPose(position, orientation), shape.ComputeInertia(1), shapeIndex, 0.01f)); + Simulation.Bodies.Add(BodyDescription.CreateDynamic((position, orientation), shape.ComputeInertia(1), shapeIndex, 0.01f)); break; case 1: - Simulation.Bodies.Add(BodyDescription.CreateKinematic(new RigidPose(position, orientation), shapeIndex, 0.01f)); + Simulation.Bodies.Add(BodyDescription.CreateKinematic((position, orientation), shapeIndex, 0.01f)); break; case 2: Simulation.Statics.Add(new StaticDescription(position, orientation, shapeIndex)); diff --git a/Demos/Demos/ClothDemo.cs b/Demos/Demos/ClothDemo.cs index c56912b02..9cfe18b79 100644 --- a/Demos/Demos/ClothDemo.cs +++ b/Demos/Demos/ClothDemo.cs @@ -71,7 +71,7 @@ public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int chi { return true; } - + [MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold { @@ -100,7 +100,7 @@ public class ClothDemo : Demo { delegate bool KinematicDecider(int rowIndex, int columnIndex, int width, int height); - BodyHandle[,] CreateBodyGrid(in Vector3 position, in Quaternion orientation, int width, int height, float spacing, float bodyRadius, float massPerBody, + BodyHandle[,] CreateBodyGrid(Vector3 position, Quaternion orientation, int width, int height, float spacing, float bodyRadius, float massPerBody, int instanceId, CollidableProperty filters, KinematicDecider isKinematic) { var description = new BodyDescription @@ -108,7 +108,7 @@ public class ClothDemo : Demo Activity = 0.01f, Collidable = Simulation.Shapes.Add(new Sphere(bodyRadius)), LocalInertia = default, - Pose = new RigidPose(default, orientation) + Pose = orientation }; var inverseMass = 1f / massPerBody; BodyHandle[,] handles = new BodyHandle[height, width]; diff --git a/Demos/Demos/CollisionQueryDemo.cs b/Demos/Demos/CollisionQueryDemo.cs index b4e1fbb68..7f8bc6fa8 100644 --- a/Demos/Demos/CollisionQueryDemo.cs +++ b/Demos/Demos/CollisionQueryDemo.cs @@ -231,7 +231,7 @@ public unsafe override void Render(Renderer renderer, Camera camera, Input input { for (int j = 0; j < 5; ++j) { - queries.Allocate(BufferPool) = new Query { Box = new Box(1, 1, 1), Pose = new RigidPose(basePosition + querySpacing * new Vector3(i, 0, j)) }; + queries.Allocate(BufferPool) = new Query { Box = new Box(1, 1, 1), Pose = basePosition + querySpacing * new Vector3(i, 0, j) }; } } //Note that the callbacks and set are value types, so you have to be a little careful about copying. diff --git a/Demos/Demos/ColosseumDemo.cs b/Demos/Demos/ColosseumDemo.cs index 171d44ffc..6b5b9e03f 100644 --- a/Demos/Demos/ColosseumDemo.cs +++ b/Demos/Demos/ColosseumDemo.cs @@ -25,9 +25,7 @@ public static void CreateRingWall(Simulation simulation, Vector3 position, Box r for (int i = 0; i < boxCountPerRing; i++) { var angle = ((ringIndex & 1) == 0 ? i + 0.5f : i) * increment; - bodyDescription.Pose = new RigidPose( - position + new Vector3(-MathF.Cos(angle) * radius, (ringIndex + 0.5f) * ringBoxShape.Height, MathF.Sin(angle) * radius), - QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, angle)); + bodyDescription.Pose = (position + new Vector3(-MathF.Cos(angle) * radius, (ringIndex + 0.5f) * ringBoxShape.Height, MathF.Sin(angle) * radius), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, angle)); simulation.Bodies.Add(bodyDescription); } } @@ -41,8 +39,7 @@ public static void CreateRingPlatform(Simulation simulation, Vector3 position, B for (int i = 0; i < boxCount; i++) { var angle = i * increment; - bodyDescription.Pose = new RigidPose( - position + new Vector3(-MathF.Cos(angle) * radius, ringBoxShape.HalfWidth, MathF.Sin(angle) * radius), + bodyDescription.Pose = (position + new Vector3(-MathF.Cos(angle) * radius, ringBoxShape.HalfWidth, MathF.Sin(angle) * radius), QuaternionEx.Concatenate(QuaternionEx.CreateFromAxisAngle(Vector3.UnitZ, MathF.PI * 0.5f), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, angle + MathF.PI * 0.5f))); simulation.Bodies.Add(bodyDescription); } @@ -99,7 +96,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var bulletShape = new Sphere(0.5f); bulletDescription = BodyDescription.CreateDynamic(new Vector3(), bulletShape.ComputeInertia(.1f), Simulation.Shapes.Add(bulletShape), 0.01f); - var shootiePatootieShape = new Sphere(3f); + var shootiePatootieShape = new Sphere(3f); shootiePatootieDescription = BodyDescription.CreateDynamic(new Vector3(), shootiePatootieShape.ComputeInertia(100), Simulation.Shapes.Add(shootiePatootieShape), 0.01f); } diff --git a/Demos/Demos/CompoundTestDemo.cs b/Demos/Demos/CompoundTestDemo.cs index 47486b5d3..c284141ef 100644 --- a/Demos/Demos/CompoundTestDemo.cs +++ b/Demos/Demos/CompoundTestDemo.cs @@ -53,7 +53,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) { for (int j = 0; j < gridWidth; ++j) { - compoundBuilder.Add(gridShapeIndex, new RigidPose(new Vector3(localPoseOffset, 0, localPoseOffset) + new Vector3(gridSpacing) * new Vector3(i, 0, j)), gridBoxInertia.InverseInertiaTensor, 1); + compoundBuilder.Add(gridShapeIndex, new Vector3(localPoseOffset, 0, localPoseOffset) + new Vector3(gridSpacing) * new Vector3(i, 0, j), gridBoxInertia.InverseInertiaTensor, 1); } } compoundBuilder.BuildDynamicCompound(out var gridChildren, out var gridInertia, out var center); @@ -152,7 +152,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) compoundBuilder.Reset(); var clamp = new Compound(clampChildren); var clampDescription = BodyDescription.CreateDynamic( - new RigidPose(tableDescription.Pose.Position + new Vector3(2f, 0.3f, 0)), clampInertia, Simulation.Shapes.Add(clamp), 0.01f); + tableDescription.Pose.Position + new Vector3(2f, 0.3f, 0), clampInertia, Simulation.Shapes.Add(clamp), 0.01f); Simulation.Bodies.Add(clampDescription); } diff --git a/Demos/Demos/ContinuousCollisionDetectionDemo.cs b/Demos/Demos/ContinuousCollisionDetectionDemo.cs index d07a84ab3..e9195f7d2 100644 --- a/Demos/Demos/ContinuousCollisionDetectionDemo.cs +++ b/Demos/Demos/ContinuousCollisionDetectionDemo.cs @@ -67,7 +67,7 @@ public override void Initialize(ContentArchive content, Camera camera) for (int j = 0; j < 10; ++j) { //These two falling dynamics have pretty small speculative margins. The second one uses continuous collision detection sweeps to generate speculative contacts. - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(-4 - 2 * j, 100 + (i + j) * 2, i * 2), new(new Vector3(0, -150, 0)), inertia, + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(-4 - 2 * j, 100 + (i + j) * 2, i * 2), new Vector3(0, -150, 0), inertia, new(shapeIndex, ContinuousDetection.Discrete(maximumSpeculativeMargin: 0.01f)), 0.01f)); //The minimum progression duration parameter at 1e-3 means the CCD sweep won't miss any collisions that last at least 1e-3 units of time- so, if time is measured in seconds, //then this will capture any collision that an update rate of 1000hz would. @@ -75,7 +75,7 @@ public override void Initialize(ContentArchive content, Camera camera) //That's because the sweep does not directly generate contacts- it generates a time of impact estimate, and then the discrete contact generation //runs to create the actual contact manifold. That provides high quality contact positions and speculative depths. //If the ground that these boxes were smashing into was something like a mesh- which is infinitely thin- you may want to increase the sweep accuracy. - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(4 + 2 * j, 100 + (i + j) * 2, i * 2), new(new Vector3(0, -150, 0)), inertia, + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(4 + 2 * j, 100 + (i + j) * 2, i * 2), new Vector3(0, -150, 0), inertia, new(shapeIndex, ContinuousDetection.Continuous(1e-3f, 1e-2f, maximumSpeculativeMargin: 0.01f)), 0.01f)); } } diff --git a/Demos/Demos/CustomVoxelCollidableDemo.cs b/Demos/Demos/CustomVoxelCollidableDemo.cs index ef202ba0d..88d88818e 100644 --- a/Demos/Demos/CustomVoxelCollidableDemo.cs +++ b/Demos/Demos/CustomVoxelCollidableDemo.cs @@ -112,7 +112,7 @@ public unsafe void TestLeaf(int leafIndex, RayData* ray, float* maximumT) { ref var voxelIndex = ref VoxelIndices[leafIndex]; //Note that you could make use of the voxel grid's regular structure to save some work dealing with orientations. - if (VoxelShape.RayTest(new RigidPose((voxelIndex + new Vector3(0.5f)) * VoxelSize), ray->Origin, ray->Direction, out var t, out var normal) && t <= *maximumT) + if (VoxelShape.RayTest(voxelIndex + new Vector3(0.5f) * VoxelSize, ray->Origin, ray->Direction, out var t, out var normal) && t <= *maximumT) { //Bring the ray normal back into world space. Matrix3x3.Transform(normal, Orientation, out normal); @@ -189,7 +189,7 @@ public readonly void GetLocalChild(int childIndex, out Box childShape) public readonly void GetPosedLocalChild(int childIndex, out Box childShape, out RigidPose childPose) { GetLocalChild(childIndex, out childShape); - childPose = new RigidPose((VoxelIndices[childIndex] + new Vector3(0.5f)) * VoxelSize); + childPose = (VoxelIndices[childIndex] + new Vector3(0.5f) * VoxelSize); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -451,7 +451,7 @@ public override unsafe void Render(Renderer renderer, Camera camera, Input input ref var voxelsPose = ref Simulation.Statics[handle].Pose; for (int i = 0; i < voxels.ChildCount; ++i) { - var localPose = new RigidPose((voxels.VoxelIndices[i] + new Vector3(0.5f)) * voxels.VoxelSize); + var localPose = (voxels.VoxelIndices[i] + new Vector3(0.5f) * voxels.VoxelSize); Compound.GetRotatedChildPose(localPose, voxelsPose.Orientation, out var childPose); childPose.Position += voxelsPose.Position; renderer.Shapes.AddShape(shapeDataPointer, Box.Id, Simulation.Shapes, ref childPose, new Vector3(0.8f, 0.2f, 0.2f)); diff --git a/Demos/Demos/FountainStressTestDemo.cs b/Demos/Demos/FountainStressTestDemo.cs index eee330935..7f33e197a 100644 --- a/Demos/Demos/FountainStressTestDemo.cs +++ b/Demos/Demos/FountainStressTestDemo.cs @@ -367,7 +367,7 @@ public override void Update(Window window, Camera camera, Input input, float dt) var spawnPose = new RigidPose(new Vector3(0, 10, 0)); for (int i = 0; i < newShapeCount; ++i) { - CreateBodyDescription(random, spawnPose, new BodyVelocity(new Vector3(-30 + 60 * random.NextSingle(), 75, -30 + 60 * random.NextSingle()), default), out var bodyDescription); + CreateBodyDescription(random, spawnPose, new Vector3(-30 + 60 * random.NextSingle(), 75, -30 + 60 * random.NextSingle()), out var bodyDescription); dynamicHandles.Enqueue(Simulation.Bodies.Add(bodyDescription), BufferPool); } int targetAsymptote = 65536; diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index d8bf51f94..090d90e3c 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -664,8 +664,7 @@ internal unsafe static void CreateDeformable(Simulation simulation, in Vector3 p var vertexShapeIndex = simulation.Shapes.Add(vertexShape); for (int i = 0; i < vertices.Length; ++i) { - vertexHandles[i] = simulation.Bodies.Add(BodyDescription.CreateDynamic(new RigidPose( - position + QuaternionEx.Transform(vertices[i], orientation), orientation), vertexInertia, + vertexHandles[i] = simulation.Bodies.Add(BodyDescription.CreateDynamic((position + QuaternionEx.Transform(vertices[i], orientation), orientation), vertexInertia, //Bodies don't have to have collidables. Take advantage of this for all the internal vertices. vertexEdgeCounts[i] == edgeCountForInternalVertex ? new TypedIndex() : vertexShapeIndex, -0.01f)); ref var vertexSpatialIndex = ref vertexSpatialIndices[i]; diff --git a/Demos/Demos/PlanetDemo.cs b/Demos/Demos/PlanetDemo.cs index 998308593..54dc81d94 100644 --- a/Demos/Demos/PlanetDemo.cs +++ b/Demos/Demos/PlanetDemo.cs @@ -87,7 +87,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) for (int k = 0; k < width; ++k) { Simulation.Bodies.Add(BodyDescription.CreateDynamic( - origin + new Vector3(i, j, k) * spacing, new BodyVelocity(new Vector3(30, 0, 0)), inertia, orbiterShapeIndex, 0.01f)); + origin + new Vector3(i, j, k) * spacing, new Vector3(30, 0, 0), inertia, orbiterShapeIndex, 0.01f)); } } } diff --git a/Demos/Demos/PyramidDemo.cs b/Demos/Demos/PyramidDemo.cs index 8dd020574..37f6ba94c 100644 --- a/Demos/Demos/PyramidDemo.cs +++ b/Demos/Demos/PyramidDemo.cs @@ -70,7 +70,7 @@ public override void Update(Window window, Camera camera, Input input, float dt) //Unfortunately, at the moment, bepuphysics v2 does not contain any alternative solvers, so if you can't afford to brute force the the problem away, //the best solution is to cheat as much as possible to avoid the corner cases. var bodyDescription = BodyDescription.CreateConvexDynamic( - new Vector3(0, 8, -130), new BodyVelocity(new Vector3(0, 0, 150)), bulletShape.Radius * bulletShape.Radius * bulletShape.Radius, Simulation.Shapes, bulletShape); + new Vector3(0, 8, -130), new Vector3(0, 0, 150), bulletShape.Radius * bulletShape.Radius * bulletShape.Radius, Simulation.Shapes, bulletShape); Simulation.Bodies.Add(bodyDescription); } base.Update(window, camera, input, dt); diff --git a/Demos/Demos/RayCastingDemo.cs b/Demos/Demos/RayCastingDemo.cs index 50ca14407..45dc3ec81 100644 --- a/Demos/Demos/RayCastingDemo.cs +++ b/Demos/Demos/RayCastingDemo.cs @@ -120,7 +120,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) }; if ((i + j + k) % 2 == 1) { - Simulation.Bodies.Add(BodyDescription.CreateKinematic(new RigidPose(location, orientation), shapeIndex, -0.1f)); + Simulation.Bodies.Add(BodyDescription.CreateKinematic((location, orientation), shapeIndex, -0.1f)); } else { diff --git a/Demos/Demos/RopeStabilityDemo.cs b/Demos/Demos/RopeStabilityDemo.cs index 120ca29ec..d13c39999 100644 --- a/Demos/Demos/RopeStabilityDemo.cs +++ b/Demos/Demos/RopeStabilityDemo.cs @@ -36,7 +36,7 @@ public static BodyHandle[] BuildRopeBodies(Simulation simulation, in Vector3 sta for (int linkIndex = 0; linkIndex < bodyCount + 1; ++linkIndex) { bodyDescription.LocalInertia = linkIndex == 0 ? new BodyInertia() : ropeInertia; - bodyDescription.Pose = new RigidPose(start - new Vector3(0, linkIndex * (bodySpacing + 2 * bodySize), 0)); + bodyDescription.Pose = start - new Vector3(0, linkIndex * (bodySpacing + 2 * bodySize), 0); handles[linkIndex] = simulation.Bodies.Add(bodyDescription); } @@ -60,7 +60,7 @@ static BodyHandle CreateWreckingBall(Simulation simulation, BodyHandle[] bodyHan var wreckingBallPosition = lastBodyReference.Pose.Position - new Vector3(0, ropeBodyRadius + bodySpacing + wreckingBallRadius, 0); var description = BodyDescription.CreateDynamic(wreckingBallPosition, wreckingBallInertia, wreckingBallShapeIndex, 0.01f); //Give it a little bump. - description.Velocity = new BodyVelocity(new Vector3(-10, 0, 0), default); + description.Velocity = new Vector3(-10, 0, 0); var wreckingBallBodyHandle = simulation.Bodies.Add(description); return wreckingBallBodyHandle; } diff --git a/Demos/Demos/Sponsors/SponsorNewt.cs b/Demos/Demos/Sponsors/SponsorNewt.cs index 7cc363231..fd79aa8e3 100644 --- a/Demos/Demos/Sponsors/SponsorNewt.cs +++ b/Demos/Demos/Sponsors/SponsorNewt.cs @@ -20,7 +20,7 @@ public SponsorNewt(Simulation simulation, TypedIndex shape, float height, in Vec var arenaSpan = arenaMax - arenaMin; var position = arenaMin + arenaSpan * new Vector2(random.NextSingle(), random.NextSingle()); var angle = MathF.PI * 2 * random.NextSingle(); - BodyHandle = simulation.Bodies.Add(BodyDescription.CreateKinematic(new RigidPose(new Vector3(position.X, height, position.Y), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, angle)), shape, -1)); + BodyHandle = simulation.Bodies.Add(BodyDescription.CreateKinematic((new Vector3(position.X, height, position.Y), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, angle)), shape, -1)); SponsorIndex = sponsorIndex; } diff --git a/Demos/Demos/SubsteppingDemo.cs b/Demos/Demos/SubsteppingDemo.cs index 914737c0d..de8545169 100644 --- a/Demos/Demos/SubsteppingDemo.cs +++ b/Demos/Demos/SubsteppingDemo.cs @@ -54,7 +54,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var boxDescription = BodyDescription.CreateDynamic(new Vector3(), boxInertia, Simulation.Shapes.Add(boxShape), -1f); for (int i = 0; i < 20; ++i) { - boxDescription.Pose = new RigidPose(new Vector3(0, 0.5f + boxShape.Height * (i + 0.5f), 0), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, MathF.PI * 0.05f * i)); + boxDescription.Pose = (new Vector3(0, 0.5f + boxShape.Height * (i + 0.5f), 0), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, MathF.PI * 0.05f * i)); Simulation.Bodies.Add(boxDescription); } var topBlockShape = new Box(8, 2, 8); diff --git a/Demos/Demos/Tanks/Tank.cs b/Demos/Demos/Tanks/Tank.cs index 46691c1ea..6968b2d99 100644 --- a/Demos/Demos/Tanks/Tank.cs +++ b/Demos/Demos/Tanks/Tank.cs @@ -165,7 +165,7 @@ public BodyHandle Fire(Simulation simulation, CollidableProperty(8, BufferPool); points.AllocateUnsafely() = new Vector3(0, 0, 0); points.AllocateUnsafely() = new Vector3(0, 0, 2); diff --git a/Demos/SpecializedTests/VolumeQueryTests.cs b/Demos/SpecializedTests/VolumeQueryTests.cs index b776f85ec..8f42066e2 100644 --- a/Demos/SpecializedTests/VolumeQueryTests.cs +++ b/Demos/SpecializedTests/VolumeQueryTests.cs @@ -92,7 +92,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) if ((i + j + k) % 2 == 1) { - var bodyDescription = BodyDescription.CreateKinematic(new RigidPose(location, orientation), shapeIndex, -1); + var bodyDescription = BodyDescription.CreateKinematic((location, orientation), shapeIndex, -1); Simulation.Bodies.Add(bodyDescription); } else From 42c0557b72320fb6b907da13fcf4f443139cda33 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 3 Dec 2021 17:20:56 -0600 Subject: [PATCH 306/947] Some fiddling. --- BepuPhysics/BodyDescription.cs | 16 ++--- Demos/Demos/NewtDemo.cs | 112 --------------------------------- Demos/Demos/PyramidDemo.cs | 2 +- 3 files changed, 9 insertions(+), 121 deletions(-) diff --git a/BepuPhysics/BodyDescription.cs b/BepuPhysics/BodyDescription.cs index fd49393cc..22541db57 100644 --- a/BepuPhysics/BodyDescription.cs +++ b/BepuPhysics/BodyDescription.cs @@ -110,7 +110,7 @@ public static BodyActivityDescription GetDefaultActivity(in TShape shape /// Collidable to associate with the body. /// Activity settings for the body. /// Constructed description for the body. - public static BodyDescription CreateDynamic(in RigidPose pose, in BodyVelocity velocity, in BodyInertia inertia, in CollidableDescription collidable, in BodyActivityDescription activity) + public static BodyDescription CreateDynamic(RigidPose pose, BodyVelocity velocity, BodyInertia inertia, CollidableDescription collidable, BodyActivityDescription activity) { return new BodyDescription { Pose = pose, Velocity = velocity, LocalInertia = inertia, Activity = activity, Collidable = collidable }; } @@ -123,7 +123,7 @@ public static BodyDescription CreateDynamic(in RigidPose pose, in BodyVelocity v /// Collidable to associate with the body. /// Activity settings for the body. /// Constructed description for the body. - public static BodyDescription CreateDynamic(in RigidPose pose, in BodyInertia inertia, in CollidableDescription collidable, in BodyActivityDescription activity) + public static BodyDescription CreateDynamic(RigidPose pose, BodyInertia inertia, CollidableDescription collidable, BodyActivityDescription activity) { return new BodyDescription { Pose = pose, LocalInertia = inertia, Activity = activity, Collidable = collidable }; } @@ -138,7 +138,7 @@ public static BodyDescription CreateDynamic(in RigidPose pose, in BodyInertia in /// Shape collection to add the shape to. /// Shape to add to the shape set and to create the body from. /// Constructed description for the body. - public static BodyDescription CreateConvexDynamic(in RigidPose pose, in BodyVelocity velocity, float mass, Shapes shapes, in TConvexShape shape) where TConvexShape : unmanaged, IConvexShape + public static BodyDescription CreateConvexDynamic(RigidPose pose, BodyVelocity velocity, float mass, Shapes shapes, in TConvexShape shape) where TConvexShape : unmanaged, IConvexShape { var description = new BodyDescription { @@ -160,7 +160,7 @@ public static BodyDescription CreateConvexDynamic(in RigidPose pos /// Shape collection to add the shape to. /// Shape to add to the shape set and to create the body from. /// Constructed description for the body. - public static BodyDescription CreateConvexDynamic(in RigidPose pose, float mass, Shapes shapes, in TConvexShape shape) where TConvexShape : unmanaged, IConvexShape + public static BodyDescription CreateConvexDynamic(RigidPose pose, float mass, Shapes shapes, in TConvexShape shape) where TConvexShape : unmanaged, IConvexShape { return CreateConvexDynamic(pose, default, mass, shapes, shape); } @@ -173,7 +173,7 @@ public static BodyDescription CreateConvexDynamic(in RigidPose pos /// Collidable to associate with the body. /// Activity settings for the body. /// Constructed description for the body. - public static BodyDescription CreateKinematic(in RigidPose pose, in BodyVelocity velocity, in CollidableDescription collidable, in BodyActivityDescription activity) + public static BodyDescription CreateKinematic(RigidPose pose, BodyVelocity velocity, CollidableDescription collidable, BodyActivityDescription activity) { return new BodyDescription { Pose = pose, Velocity = velocity, Activity = activity, Collidable = collidable }; } @@ -185,7 +185,7 @@ public static BodyDescription CreateKinematic(in RigidPose pose, in BodyVelocity /// Collidable to associate with the body. /// Activity settings for the body. /// Constructed description for the body. - public static BodyDescription CreateKinematic(in RigidPose pose, in CollidableDescription collidable, in BodyActivityDescription activity) + public static BodyDescription CreateKinematic(RigidPose pose, CollidableDescription collidable, BodyActivityDescription activity) { return new BodyDescription { Pose = pose, Activity = activity, Collidable = collidable }; } @@ -199,7 +199,7 @@ public static BodyDescription CreateKinematic(in RigidPose pose, in CollidableDe /// Shape collection to add the shape to. /// Shape to add to the shape set and to create the body from. /// Constructed description for the body. - public static BodyDescription CreateConvexKinematic(in RigidPose pose, in BodyVelocity velocity, Shapes shapes, in TConvexShape shape) where TConvexShape : unmanaged, IConvexShape + public static BodyDescription CreateConvexKinematic(RigidPose pose, BodyVelocity velocity, Shapes shapes, in TConvexShape shape) where TConvexShape : unmanaged, IConvexShape { var description = new BodyDescription { @@ -220,7 +220,7 @@ public static BodyDescription CreateConvexKinematic(in RigidPose p /// Shape collection to add the shape to. /// Shape to add to the shape set and to create the body from. /// Constructed description for the body. - public static BodyDescription CreateConvexKinematic(in RigidPose pose, Shapes shapes, in TConvexShape shape) where TConvexShape : unmanaged, IConvexShape + public static BodyDescription CreateConvexKinematic(RigidPose pose, Shapes shapes, TConvexShape shape) where TConvexShape : unmanaged, IConvexShape { return CreateConvexKinematic(pose, default, shapes, shape); } diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index 090d90e3c..d3ff7c783 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -698,118 +698,6 @@ internal unsafe static void CreateDeformable(Simulation simulation, in Vector3 p edges.Dispose(pool); } - [MethodImpl(MethodImplOptions.NoInlining)] - void Test() - { - Unsafe.SkipInit(out Vector3Wide a); - Unsafe.SkipInit(out Vector3Wide b); - Unsafe.SkipInit(out Vector3Wide c); - Unsafe.SkipInit(out Vector d); - Unsafe.SkipInit(out Vector e); - Unsafe.SkipInit(out Symmetric3x3Wide s1); - Unsafe.SkipInit(out Matrix3x3Wide s2); - Unsafe.SkipInit(out QuaternionWide q1); - Unsafe.SkipInit(out QuaternionWide q2); - - - var r = Vector3Wide.Min(Vector3Wide.Max(Vector3Wide.Dot(a - b, b - c), a), b); - var p = Vector3Wide.Cross((d * r) * d, -Vector3Wide.Abs(a)); - var n1 = Vector3Wide.Length(Vector3Wide.ConditionallyNegate(e, p)); - var n2 = (Vector3Wide.ConditionallyNegate(e, b)).Length(); - var uh = n1 + n2; - uh += Vector3Wide.Dot(a * b, Vector3Wide.Broadcast(Vector3.Zero)); - - var ehe = Vector3Wide.Normalize(a * q1 * q2); - uh += ehe.LengthSquared() * q1.LengthSquared(); - - var m0 = s1 * s2; - uh += Vector3Wide.Dot(a * m0, a); - - uh += Vector3Wide.Dot(a * s1, a); - var uh2 = a * s1; - - Console.WriteLine("$uah" + uh2); - Console.WriteLine($"ree: {uh}"); - } - - void Test2() - { - const int pointCount = 16; - var points = new QuickList(pointCount * 2, BufferPool); - var random = new Random(5); - for (int i = 0; i < pointCount; ++i) - { - points.AllocateUnsafely() = new Vector3(3 * random.NextSingle(), 1 * random.NextSingle(), 3 * random.NextSingle()); - } - var convexHull1 = new ConvexHull(points, BufferPool, out _); - points.Count = 0; - for (int i = 0; i < pointCount; ++i) - { - points.AllocateUnsafely() = new Vector3(3 * random.NextSingle(), 1 * random.NextSingle(), 3 * random.NextSingle()); - } - var convexHull2 = new ConvexHull(points, BufferPool, out _); - var scalarInertiaA = convexHull1.ComputeInertia(1); - var scalarInertiaB = convexHull2.ComputeInertia(1); - - BodyInertiaWide inertiaA; - inertiaA.InverseMass = new Vector(1); - inertiaA.InverseInertiaTensor.XX = new Vector(scalarInertiaA.InverseInertiaTensor.XX); - inertiaA.InverseInertiaTensor.YX = new Vector(scalarInertiaA.InverseInertiaTensor.YX); - inertiaA.InverseInertiaTensor.YY = new Vector(scalarInertiaA.InverseInertiaTensor.YY); - inertiaA.InverseInertiaTensor.ZX = new Vector(scalarInertiaA.InverseInertiaTensor.ZX); - inertiaA.InverseInertiaTensor.ZY = new Vector(scalarInertiaA.InverseInertiaTensor.ZY); - inertiaA.InverseInertiaTensor.ZZ = new Vector(scalarInertiaA.InverseInertiaTensor.ZZ); - Vector3Wide.Broadcast(new Vector3(1, 3, 4), out var offset); - BodyInertiaWide inertiaB; - inertiaB.InverseMass = new Vector(1); - inertiaB.InverseInertiaTensor.XX = new Vector(scalarInertiaB.InverseInertiaTensor.XX); - inertiaB.InverseInertiaTensor.YX = new Vector(scalarInertiaB.InverseInertiaTensor.YX); - inertiaB.InverseInertiaTensor.YY = new Vector(scalarInertiaB.InverseInertiaTensor.YY); - inertiaB.InverseInertiaTensor.ZX = new Vector(scalarInertiaB.InverseInertiaTensor.ZX); - inertiaB.InverseInertiaTensor.ZY = new Vector(scalarInertiaB.InverseInertiaTensor.ZY); - inertiaB.InverseInertiaTensor.ZZ = new Vector(scalarInertiaB.InverseInertiaTensor.ZZ); - Vector3Wide.Broadcast(new Vector3(5, 4, 3), out var orientationCSV); - Vector3Wide.Broadcast(new Vector3(-5, -4, -3), out var offsetCSV); - - convexHull1.Dispose(BufferPool); - convexHull2.Dispose(BufferPool); - points.Dispose(BufferPool); - - var jmjtA = inertiaA.InverseInertiaTensor + inertiaB.InverseInertiaTensor; - Matrix3x3Wide.CreateCrossProduct(offset, out var xAB); - var jmjtB = inertiaA.InverseInertiaTensor * xAB; - Symmetric3x3Wide.CompleteMatrixSandwichTranspose(xAB, jmjtB, out var jmjtD); - var diagonalAdd = inertiaA.InverseMass + inertiaB.InverseMass; - jmjtD.XX += diagonalAdd; - jmjtD.YY += diagonalAdd; - jmjtD.ZZ += diagonalAdd; - - var inverseEffectiveMass = new Symmetric6x6Wide { A = jmjtA, B = jmjtB, D = jmjtD }; - - const int iterationCount = 1000000; - var startLDLT = Stopwatch.GetTimestamp(); - for (int i = 0; i < iterationCount; ++i) - { - Symmetric6x6Wide.LDLTSolve(orientationCSV, offsetCSV, jmjtA, jmjtB, jmjtD, out var orientationCSI, out var offsetCSI); - //Symmetric6x6Wide.TransformWithoutOverlap(orientationCSI, offsetCSI, inverseEffectiveMass, out var roundtripOrientationCSV, out var roundtripOffsetCSV); - } - var endLDLT = Stopwatch.GetTimestamp(); - var startInverse = Stopwatch.GetTimestamp(); - for (int i = 0; i < iterationCount; ++i) - { - Symmetric6x6Wide.Invert(jmjtA, jmjtB, jmjtD, out var testEffectiveMass); - Symmetric6x6Wide.TransformWithoutOverlap(orientationCSV, offsetCSV, testEffectiveMass, out var orientationCSI2, out var offsetCSI2); - //Symmetric6x6Wide.TransformWithoutOverlap(orientationCSI2, offsetCSI2, inverseEffectiveMass, out var roundtripOrientationCSV2, out var roundtripOffsetCSV2); - } - var endInverse = Stopwatch.GetTimestamp(); - double ldltTime = (endLDLT - startLDLT) / (iterationCount * (double)Stopwatch.Frequency); - double inverseTime = (endInverse - startInverse) / (iterationCount * (double)Stopwatch.Frequency); - - Console.WriteLine($"LDLT time (ns): {ldltTime * 1e9}"); - Console.WriteLine($"Inverse time (ns): {inverseTime * 1e9}"); - Console.WriteLine($"inverse / ldlt: {inverseTime / ldltTime}"); - - } public unsafe override void Initialize(ContentArchive content, Camera camera) { diff --git a/Demos/Demos/PyramidDemo.cs b/Demos/Demos/PyramidDemo.cs index 37f6ba94c..d1ac1e8c4 100644 --- a/Demos/Demos/PyramidDemo.cs +++ b/Demos/Demos/PyramidDemo.cs @@ -53,7 +53,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) } //We'll randomize the size of bullets. - Random random = new Random(5); + Random random = new(5); public override void Update(Window window, Camera camera, Input input, float dt) { if (input != null && input.WasPushed(OpenTK.Input.Key.Z)) From b27c143f76fa900ce28332d83cea8899bb910c6a Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 3 Dec 2021 20:27:19 -0600 Subject: [PATCH 307/947] Does not build. In the middle of cleaning up obsolete pose integrator paths and timesteppers. --- BepuPhysics/PoseIntegrator.cs | 278 +++++++--------------------------- BepuPhysics/Simulation.cs | 72 --------- 2 files changed, 52 insertions(+), 298 deletions(-) diff --git a/BepuPhysics/PoseIntegrator.cs b/BepuPhysics/PoseIntegrator.cs index 4272b12d9..86839f945 100644 --- a/BepuPhysics/PoseIntegrator.cs +++ b/BepuPhysics/PoseIntegrator.cs @@ -14,11 +14,7 @@ namespace BepuPhysics { public interface IPoseIntegrator { - void IntegrateBodiesAndUpdateBoundingBoxes(float dt, BufferPool pool, IThreadDispatcher threadDispatcher = null); void PredictBoundingBoxes(float dt, BufferPool pool, IThreadDispatcher threadDispatcher = null); - void IntegrateVelocitiesBoundsAndInertias(float dt, BufferPool pool, IThreadDispatcher threadDispatcher = null); - void IntegrateVelocitiesAndUpdateInertias(float dt, BufferPool pool, IThreadDispatcher threadDispatcher = null); - void IntegratePoses(float dt, BufferPool pool, IThreadDispatcher threadDispatcher = null); void IntegrateAfterSubstepping(IndexSet constrainedBodies, float dt, int substepCount, IThreadDispatcher threadDispatcher = null); } @@ -53,20 +49,19 @@ public interface IPoseIntegratorCallbacks /// /// Gets whether the integrator should use only one step for unconstrained bodies when using a substepping solver. - /// If true, unconstrained bodies use a single step of length equal to the dt provided to Simulation.Timestep. + /// If true, unconstrained bodies use a single step of length equal to the dt provided to . /// If false, unconstrained bodies will be integrated with the same number of substeps as the constrained bodies in the solver. /// bool AllowSubstepsForUnconstrainedBodies { get; } /// /// Gets whether the velocity integration callback should be called for kinematic bodies. - /// If true, IntegrateVelocity will be called for bundles including kinematic bodies. + /// If true, will be called for bundles including kinematic bodies. /// If false, kinematic bodies will just continue using whatever velocity they have set. /// Most use cases should set this to false. /// bool IntegrateVelocityForKinematics { get; } - /// /// Performs any required initialization logic after the Simulation instance has been constructed. /// @@ -79,16 +74,6 @@ public interface IPoseIntegratorCallbacks /// Current time step duration. void PrepareForIntegration(float dt); - /// - /// Callback called for each active body within the simulation during body integration. - /// - /// Index of the body being visited. - /// Body's current pose. - /// Body's current local inertia. - /// Index of the worker thread processing this body. - /// Reference to the body's current velocity to integrate. - void IntegrateVelocity(int bodyIndex, in RigidPose pose, in BodyInertia localInertia, int workerIndex, ref BodyVelocity velocity); - /// /// Callback for a bundle of bodies being integrated. @@ -97,7 +82,7 @@ public interface IPoseIntegratorCallbacks /// Current body positions. /// Current body orientations. /// Body's current local inertia. - /// Mask indicating which lanes are active in the bundle. Active lanes will contain 0xFFFFFFFF, inactive lanes will contain 0. Lanes beyond bodyIndices.Length are undefined. + /// Mask indicating which lanes are active in the bundle. Active lanes will contain 0xFFFFFFFF, inactive lanes will contain 0. /// Index of the worker thread processing this bundle. /// Durations to integrate the velocity over. Can vary over lanes. /// Velocity of bodies in the bundle. Any changes to lanes which are not active by the integrationMask will be discarded. @@ -195,7 +180,7 @@ static void FallbackIfInertiaIncompatible(in Vector3Wide previousAngularVelocity { var infinity = new Vector(float.PositiveInfinity); var useNewVelocity = Vector.BitwiseAnd(Vector.LessThan(Vector.Abs(angularVelocity.X), infinity), Vector.BitwiseAnd( - Vector.LessThan(Vector.Abs(angularVelocity.Y), infinity), + Vector.LessThan(Vector.Abs(angularVelocity.Y), infinity), Vector.LessThan(Vector.Abs(angularVelocity.Z), infinity))); angularVelocity.X = Vector.ConditionalSelect(useNewVelocity, angularVelocity.X, previousAngularVelocity.X); angularVelocity.Y = Vector.ConditionalSelect(useNewVelocity, angularVelocity.Y, previousAngularVelocity.Y); @@ -291,22 +276,14 @@ public class PoseIntegrator : IPoseIntegrator where TCallbacks : IPo public TCallbacks Callbacks; - Action integrateBodiesAndUpdateBoundingBoxesWorker; Action predictBoundingBoxesWorker; - Action integrateVelocitiesBoundsAndInertiasWorker; - Action integrateVelocitiesWorker; - Action integratePosesWorker; public PoseIntegrator(Bodies bodies, Shapes shapes, BroadPhase broadPhase, TCallbacks callbacks) { this.bodies = bodies; this.shapes = shapes; this.broadPhase = broadPhase; Callbacks = callbacks; - integrateBodiesAndUpdateBoundingBoxesWorker = IntegrateBodiesAndUpdateBoundingBoxesWorker; predictBoundingBoxesWorker = PredictBoundingBoxesWorker; - integrateVelocitiesBoundsAndInertiasWorker = IntegrateVelocitiesBoundsAndInertiasWorker; - integrateVelocitiesWorker = IntegrateVelocitiesWorker; - integratePosesWorker = IntegratePosesWorker; integrateAfterSubsteppingWorker = IntegrateAfterSubsteppingWorker; } @@ -480,62 +457,49 @@ unsafe void IntegrateBodiesAndUpdateBoundingBoxes(int startIndex, int endIndex, } } - unsafe void PredictBoundingBoxes(int startIndex, int endIndex, float dt, ref BoundingBoxBatcher boundingBoxBatcher, int workerIndex) + unsafe void PredictBoundingBoxes(int startBundleIndex, int endBundleIndex, float dt, ref BoundingBoxBatcher boundingBoxBatcher, int workerIndex) { - ref var baseStates = ref bodies.ActiveSet.SolverStates[0]; - ref var baseActivity = ref bodies.ActiveSet.Activity[0]; - ref var baseCollidable = ref bodies.ActiveSet.Collidables[0]; - for (int i = startIndex; i < endIndex; ++i) - { - ref var state = ref Unsafe.Add(ref baseStates, i); - ref var motion = ref state.Motion; - motion.Pose.Position.Validate(); - motion.Pose.Orientation.ValidateOrientation(); - motion.Velocity.Linear.Validate(); - motion.Velocity.Angular.Validate(); - - UpdateSleepCandidacy(ref motion.Velocity, ref Unsafe.Add(ref baseActivity, i)); + var activities = bodies.ActiveSet.Activity; + var collidables = bodies.ActiveSet.Collidables; - //Bounding box prediction does not need to update inertia tensors. - var integratedVelocity = motion.Velocity; - Callbacks.IntegrateVelocity(i, motion.Pose, state.Inertia.Local, workerIndex, ref integratedVelocity); - - //Note that we do not include fancier angular integration for the bounding box prediction- it's not very important. - boundingBoxBatcher.Add(i, motion.Pose, integratedVelocity, Unsafe.Add(ref baseCollidable, i)); - } - } - - unsafe void IntegrateVelocitiesBoundsAndInertias(int startIndex, int endIndex, float dt, ref BoundingBoxBatcher boundingBoxBatcher, int workerIndex) - { - ref var baseStates = ref bodies.ActiveSet.SolverStates[0]; - ref var baseActivity = ref bodies.ActiveSet.Activity[0]; - ref var baseCollidable = ref bodies.ActiveSet.Collidables[0]; - for (int i = startIndex; i < endIndex; ++i) + var bundleCount = endBundleIndex - startBundleIndex; + Helpers.FillVectorWithLaneIndices(out var laneIndexOffsets); + var dtWide = new Vector(dt); + var bodyCount = bodies.ActiveSet.Count; + for (int bundleIndex = 0; bundleIndex < bundleCount; ++bundleIndex) { - ref var state = ref Unsafe.Add(ref baseStates, i); - ref var motion = ref state.Motion; - motion.Pose.Position.Validate(); - motion.Pose.Orientation.ValidateOrientation(); - motion.Velocity.Linear.Validate(); - motion.Velocity.Angular.Validate(); + var bundleStartBodyIndex = bundleIndex * Vector.Count; + var countInBundle = bodyCount - bundleStartBodyIndex; + if (countInBundle > Vector.Count) + countInBundle = Vector.Count; + var laneIndices = new Vector(bundleStartBodyIndex) + laneIndexOffsets; + bodies.GatherState(laneIndices, false, out var position, out var orientation, out var velocity, out var inertia); - UpdateSleepCandidacy(ref motion.Velocity, ref Unsafe.Add(ref baseActivity, i)); + Vector integrationMask; + if (Callbacks.IntegrateVelocityForKinematics) + { + integrationMask = BundleIndexing.CreateMaskForCountInBundle(countInBundle); + } + else + { + integrationMask = Vector.AndNot(BundleIndexing.CreateMaskForCountInBundle(countInBundle), IsKinematic(inertia)); + } + var sleepEnergy = velocity.Linear.LengthSquared() + velocity.Angular.LengthSquared(); - ref var inertia = ref state.Inertia; - PoseIntegration.RotateInverseInertia(inertia.Local.InverseInertiaTensor, motion.Pose.Orientation, out inertia.World.InverseInertiaTensor); - inertia.World.InverseMass = inertia.Local.InverseMass; + //Note that we're not storing out the integrated velocities. The integrated velocities are only used for bounding box prediction. + if (Vector.LessThanAny(integrationMask, Vector.Zero)) + Callbacks.IntegrateVelocity(laneIndices, position, orientation, inertia, integrationMask, workerIndex, dtWide, ref velocity); - IntegrateAngularVelocity(motion.Pose, inertia.Local, inertia.World, ref motion.Velocity.Angular, dt); - Callbacks.IntegrateVelocity(i, motion.Pose, inertia.Local, workerIndex, ref motion.Velocity); + for (int i = 0; i < countInBundle; ++i) + { + var bodyIndex = i + bundleStartBodyIndex; + UpdateSleepCandidacy(sleepEnergy[bodyIndex], ref activities[bodyIndex]); - boundingBoxBatcher.Add(i, motion.Pose, motion.Velocity, Unsafe.Add(ref baseCollidable, i)); - } - } + boundingBoxBatcher.Add(bodyIndex, bodyPose, bodyVelocity, collidablles[bodyIndex]); + } + } - unsafe void IntegrateVelocities(int startIndex, int endIndex, float dt, int workerIndex) - { - ref var baseStates = ref bodies.ActiveSet.SolverStates[0]; for (int i = startIndex; i < endIndex; ++i) { ref var state = ref Unsafe.Add(ref baseStates, i); @@ -545,28 +509,14 @@ unsafe void IntegrateVelocities(int startIndex, int endIndex, float dt, int work motion.Velocity.Linear.Validate(); motion.Velocity.Angular.Validate(); - ref var inertia = ref state.Inertia; - PoseIntegration.RotateInverseInertia(inertia.Local.InverseInertiaTensor, motion.Pose.Orientation, out inertia.World.InverseInertiaTensor); - inertia.World.InverseMass = inertia.Local.InverseMass; - - IntegrateAngularVelocity(motion.Pose, inertia.Local, inertia.World, ref motion.Velocity.Angular, dt); - Callbacks.IntegrateVelocity(i, motion.Pose, inertia.Local, workerIndex, ref motion.Velocity); - } - } + UpdateSleepCandidacy(ref motion.Velocity, ref Unsafe.Add(ref baseActivity, i)); - unsafe void IntegratePoses(int startIndex, int endIndex, float dt, int workerIndex) - { - ref var baseStates = ref bodies.ActiveSet.SolverStates[0]; - for (int i = startIndex; i < endIndex; ++i) - { - ref var state = ref Unsafe.Add(ref baseStates, i); - ref var motion = ref state.Motion; - motion.Pose.Position.Validate(); - motion.Pose.Orientation.ValidateOrientation(); - motion.Velocity.Linear.Validate(); - motion.Velocity.Angular.Validate(); + //Bounding box prediction does not need to update inertia tensors. + var integratedVelocity = motion.Velocity; + Callbacks.IntegrateVelocity(i, motion.Pose, state.Inertia.Local, workerIndex, ref integratedVelocity); - PoseIntegration.Integrate(motion.Pose, motion.Velocity, dt, out motion.Pose); + //Note that we do not include fancier angular integration for the bounding box prediction- it's not very important. + boundingBoxBatcher.Add(i, motion.Pose, integratedVelocity, Unsafe.Add(ref baseCollidable, i)); } } @@ -598,18 +548,6 @@ bool TryGetJob(int maximumJobInterval, out int start, out int exclusiveEnd) return true; } - void IntegrateBodiesAndUpdateBoundingBoxesWorker(int workerIndex) - { - var boundingBoxUpdater = new BoundingBoxBatcher(bodies, shapes, broadPhase, threadDispatcher.GetThreadMemoryPool(workerIndex), cachedDt); - var bodyCount = bodies.ActiveSet.Count; - while (TryGetJob(bodyCount, out var start, out var exclusiveEnd)) - { - IntegrateBodiesAndUpdateBoundingBoxes(start, exclusiveEnd, cachedDt, ref boundingBoxUpdater, workerIndex); - } - boundingBoxUpdater.Flush(); - - } - void PredictBoundingBoxesWorker(int workerIndex) { var boundingBoxUpdater = new BoundingBoxBatcher(bodies, shapes, broadPhase, threadDispatcher.GetThreadMemoryPool(workerIndex), cachedDt); @@ -621,35 +559,6 @@ void PredictBoundingBoxesWorker(int workerIndex) boundingBoxUpdater.Flush(); } - void IntegrateVelocitiesBoundsAndInertiasWorker(int workerIndex) - { - var boundingBoxUpdater = new BoundingBoxBatcher(bodies, shapes, broadPhase, threadDispatcher.GetThreadMemoryPool(workerIndex), cachedDt); - var bodyCount = bodies.ActiveSet.Count; - while (TryGetJob(bodyCount, out var start, out var exclusiveEnd)) - { - IntegrateVelocitiesBoundsAndInertias(start, exclusiveEnd, cachedDt, ref boundingBoxUpdater, workerIndex); - } - boundingBoxUpdater.Flush(); - } - - void IntegrateVelocitiesWorker(int workerIndex) - { - var bodyCount = bodies.ActiveSet.Count; - while (TryGetJob(bodyCount, out var start, out var exclusiveEnd)) - { - IntegrateVelocities(start, exclusiveEnd, cachedDt, workerIndex); - } - } - - void IntegratePosesWorker(int workerIndex) - { - var bodyCount = bodies.ActiveSet.Count; - while (TryGetJob(bodyCount, out var start, out var exclusiveEnd)) - { - IntegratePoses(start, exclusiveEnd, cachedDt, workerIndex); - } - } - void PrepareForMultithreadedExecution(int loopIterationCount, float dt, int workerCount, int substepCount = 1) { cachedDt = dt; @@ -664,36 +573,6 @@ void PrepareForMultithreadedExecution(int loopIterationCount, float dt, int work ++availableJobCount; } - - public void IntegrateBodiesAndUpdateBoundingBoxes(float dt, BufferPool pool, IThreadDispatcher threadDispatcher = null) - { - var workerCount = threadDispatcher == null ? 1 : threadDispatcher.ThreadCount; - - Callbacks.PrepareForIntegration(dt); - if (threadDispatcher != null) - { - //While we do technically support multithreading here, scaling is going to be really, really bad if the simulation gets kicked out of L3 cache in between frames. - //The ratio of memory loads to actual compute work in this stage is extremely high, so getting scaling of 1.2x on a quad core is quite possible. - //On the upside, it is a very short stage. With any luck, one or more of the following will hold: - //1) the system has silly fast RAM, - //2) the CPU supports octochannel memory and just brute forces the issue, - //3) whatever the application is doing doesn't evict the entire L3 cache between frames. - - //Note that this bottleneck means the fact that we're working through bodies in a nonvectorized fashion (in favor of optimizing storage for solver access) is not a problem. - - PrepareForMultithreadedExecution(bodies.ActiveSet.Count, dt, threadDispatcher.ThreadCount); - this.threadDispatcher = threadDispatcher; - threadDispatcher.DispatchWorkers(integrateBodiesAndUpdateBoundingBoxesWorker); - this.threadDispatcher = null; - } - else - { - var boundingBoxUpdater = new BoundingBoxBatcher(bodies, shapes, broadPhase, pool, dt); - IntegrateBodiesAndUpdateBoundingBoxes(0, bodies.ActiveSet.Count, dt, ref boundingBoxUpdater, 0); - boundingBoxUpdater.Flush(); - } - } - public void PredictBoundingBoxes(float dt, BufferPool pool, IThreadDispatcher threadDispatcher = null) { var workerCount = threadDispatcher == null ? 1 : threadDispatcher.ThreadCount; @@ -715,63 +594,6 @@ public void PredictBoundingBoxes(float dt, BufferPool pool, IThreadDispatcher th } - public void IntegrateVelocitiesBoundsAndInertias(float dt, BufferPool pool, IThreadDispatcher threadDispatcher = null) - { - var workerCount = threadDispatcher == null ? 1 : threadDispatcher.ThreadCount; - - Callbacks.PrepareForIntegration(dt); - if (threadDispatcher != null) - { - PrepareForMultithreadedExecution(bodies.ActiveSet.Count, dt, threadDispatcher.ThreadCount); - this.threadDispatcher = threadDispatcher; - threadDispatcher.DispatchWorkers(integrateVelocitiesBoundsAndInertiasWorker); - this.threadDispatcher = null; - } - else - { - var boundingBoxUpdater = new BoundingBoxBatcher(bodies, shapes, broadPhase, pool, dt); - IntegrateVelocitiesBoundsAndInertias(0, bodies.ActiveSet.Count, dt, ref boundingBoxUpdater, 0); - boundingBoxUpdater.Flush(); - } - } - - public void IntegrateVelocitiesAndUpdateInertias(float dt, BufferPool pool, IThreadDispatcher threadDispatcher = null) - { - var workerCount = threadDispatcher == null ? 1 : threadDispatcher.ThreadCount; - - Callbacks.PrepareForIntegration(dt); - if (threadDispatcher != null) - { - PrepareForMultithreadedExecution(bodies.ActiveSet.Count, dt, threadDispatcher.ThreadCount); - this.threadDispatcher = threadDispatcher; - threadDispatcher.DispatchWorkers(integrateVelocitiesWorker); - this.threadDispatcher = null; - } - else - { - IntegrateVelocities(0, bodies.ActiveSet.Count, dt, 0); - } - } - - public void IntegratePoses(float dt, BufferPool pool, IThreadDispatcher threadDispatcher = null) - { - //This path is used with some other velocity/bounding box integration that handles world inertia calculation, so we don't need to worry about it. - var workerCount = threadDispatcher == null ? 1 : threadDispatcher.ThreadCount; - - Callbacks.PrepareForIntegration(dt); - if (threadDispatcher != null) - { - PrepareForMultithreadedExecution(bodies.ActiveSet.Count, dt, threadDispatcher.ThreadCount); - this.threadDispatcher = threadDispatcher; - threadDispatcher.DispatchWorkers(integratePosesWorker); - this.threadDispatcher = null; - } - else - { - IntegratePoses(0, bodies.ActiveSet.Count, dt, 0); - } - } - /// /// Integrates the velocities of kinematic bodies as a prepass to the first substep during solving. @@ -861,6 +683,13 @@ internal unsafe void IntegrateKinematicPosesAndVelocities(Buffer bodyHandle } } + static Vector IsKinematic(BodyInertiaWide inertia) + { + return Vector.Equals(Vector.BitwiseOr( + Vector.BitwiseOr(Vector.BitwiseOr(inertia.InverseMass, inertia.InverseInertiaTensor.XX), Vector.BitwiseOr(inertia.InverseInertiaTensor.YX, inertia.InverseInertiaTensor.YY)), + Vector.BitwiseOr(Vector.BitwiseOr(inertia.InverseInertiaTensor.ZX, inertia.InverseInertiaTensor.ZY), inertia.InverseInertiaTensor.ZZ)), Vector.Zero); + } + unsafe void IntegrateBundlesAfterSubstepping(ref IndexSet mergedConstrainedBodyHandles, int bundleStartIndex, int bundleEndIndex, float dt, float substepDt, int substepCount, int workerIndex) { var bodyCount = bodies.ActiveSet.Count; @@ -929,10 +758,7 @@ unsafe void IntegrateBundlesAfterSubstepping(ref IndexSet mergedConstrainedBodyH } else { - var isKinematic = - Vector.Equals(Vector.BitwiseOr( - Vector.BitwiseOr(Vector.BitwiseOr(localInertia.InverseMass, localInertia.InverseInertiaTensor.XX), Vector.BitwiseOr(localInertia.InverseInertiaTensor.YX, localInertia.InverseInertiaTensor.YY)), - Vector.BitwiseOr(Vector.BitwiseOr(localInertia.InverseInertiaTensor.ZX, localInertia.InverseInertiaTensor.ZY), localInertia.InverseInertiaTensor.ZZ)), Vector.Zero); + var isKinematic = IsKinematic(localInertia); unconstrainedVelocityIntegrationMask = Vector.AndNot(unconstrainedMask, isKinematic); anyBodyInBundleNeedsVelocityIntegration = Vector.LessThanAny(unconstrainedVelocityIntegrationMask, Vector.Zero); } diff --git a/BepuPhysics/Simulation.cs b/BepuPhysics/Simulation.cs index 0cf868f50..6b4168f0e 100644 --- a/BepuPhysics/Simulation.cs +++ b/BepuPhysics/Simulation.cs @@ -203,18 +203,6 @@ public void Sleep(IThreadDispatcher threadDispatcher = null) profiler.End(Sleeper); } - /// - /// Updates the position, velocity, world inertia, deactivation candidacy and bounding boxes of active bodies. - /// - /// Duration of the time step. - /// Thread dispatcher to use for execution, if any. - public void IntegrateBodiesAndUpdateBoundingBoxes(float dt, IThreadDispatcher threadDispatcher = null) - { - profiler.Start(PoseIntegrator); - PoseIntegrator.IntegrateBodiesAndUpdateBoundingBoxes(dt, BufferPool, threadDispatcher); - profiler.End(PoseIntegrator); - } - /// /// Predicts the bounding boxes of active bodies by speculatively integrating velocity. Does not actually modify body velocities. Updates deactivation candidacy. /// @@ -227,42 +215,6 @@ public void PredictBoundingBoxes(float dt, IThreadDispatcher threadDispatcher = profiler.End(PoseIntegrator); } - /// - /// Updates the velocities, world space inertias, bounding boxes, and deactivation candidacy of active bodies. - /// - /// Duration of the time step. - /// Thread dispatcher to use for execution, if any. - public void IntegrateVelocitiesBoundsAndInertias(float dt, IThreadDispatcher threadDispatcher = null) - { - profiler.Start(PoseIntegrator); - PoseIntegrator.IntegrateVelocitiesBoundsAndInertias(dt, BufferPool, threadDispatcher); - profiler.End(PoseIntegrator); - } - - /// - /// Updates the velocities and world space inertias of active bodies. - /// - /// Duration of the time step. - /// Thread dispatcher to use for execution, if any. - public void IntegrateVelocitiesAndUpdateInertias(float dt, IThreadDispatcher threadDispatcher = null) - { - profiler.Start(PoseIntegrator); - PoseIntegrator.IntegrateVelocitiesAndUpdateInertias(dt, BufferPool, threadDispatcher); - profiler.End(PoseIntegrator); - } - - /// - /// Updates the poses of active bodies. - /// - /// Duration of the time step. - /// Thread dispatcher to use for execution, if any. - public void IntegratePoses(float dt, IThreadDispatcher threadDispatcher = null) - { - profiler.Start(PoseIntegrator); - PoseIntegrator.IntegratePoses(dt, BufferPool, threadDispatcher); - profiler.End(PoseIntegrator); - } - /// /// Updates the broad phase structure for the current body bounding boxes, finds potentially colliding pairs, and then executes the narrow phase for all such pairs. Generates contact constraints for the solver. /// @@ -283,30 +235,6 @@ public void CollisionDetection(float dt, IThreadDispatcher threadDispatcher = nu profiler.End(NarrowPhase); } - /// - /// Uses the current body velocities to incrementally update all active contact constraint penetration depths. - /// - /// Duration of the time step. - /// Thread dispatcher to use for execution, if any. - public void IncrementallyUpdateContactConstraints(float dt, IThreadDispatcher threadDispatcher = null) - { - profiler.Start(Solver); - Solver.IncrementallyUpdateContactConstraints(dt, threadDispatcher); - profiler.End(Solver); - } - - /// - /// Solves all active constraints in the simulation. - /// - /// Duration of the time step. - /// Thread dispatcher to use for execution, if any. - public void Solve(float dt, IThreadDispatcher threadDispatcher = null) - { - profiler.Start(Solver); - Solver.Solve(dt, threadDispatcher); - profiler.End(Solver); - } - /// /// Incrementally improves body and constraint storage for better performance. /// From a63c53d91ede1400ba7457900d584c73f5e12f2e Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 4 Dec 2021 19:46:32 -0600 Subject: [PATCH 308/947] Finished up vectorized predict path and purged obsolete timesteppers. --- BepuPhysics/Bodies.cs | 13 ++ BepuPhysics/PoseIntegrator.cs | 216 +++--------------- BepuPhysics/PositionFirstTimestepper.cs | 71 ------ BepuPhysics/PositionLastTimestepper.cs | 55 ----- BepuPhysics/SubsteppingTimestepper.cs | 107 --------- BepuUtilities/QuaternionWide.cs | 13 ++ BepuUtilities/Vector3Wide.cs | 1 - .../ConstraintDescriptionMappingTests.cs | 2 +- Demos/Demos/BouncinessDemo.cs | 2 +- Demos/Demos/ClothDemo.cs | 5 +- Demos/Demos/CollisionQueryDemo.cs | 5 +- Demos/Demos/ColosseumDemo.cs | 5 - Demos/Demos/ContactEventsDemo.cs | 5 +- .../Demos/ContinuousCollisionDetectionDemo.cs | 5 +- Demos/Demos/CustomVoxelCollidableDemo.cs | 5 +- Demos/Demos/NewtDemo.cs | 4 +- Demos/Demos/PlanetDemo.cs | 5 +- Demos/Demos/RayCastingDemo.cs | 5 +- Demos/Demos/RopeStabilityDemo.cs | 4 +- Demos/Demos/SimpleSelfContainedDemo.cs | 2 +- Demos/Demos/SolverContactEnumerationDemo.cs | 5 +- Demos/Demos/Sponsors/SponsorDemo.cs | 4 +- Demos/Demos/SweepDemo.cs | 2 +- .../BroadPhaseStressTestDemo.cs | 2 +- Demos/SpecializedTests/CapsuleTestDemo.cs | 2 +- Demos/SpecializedTests/CharacterTestDemo.cs | 2 +- Demos/SpecializedTests/ClothLatticeDemo.cs | 5 +- Demos/SpecializedTests/CompoundBoundTests.cs | 2 +- .../CompoundCollisionIndicesTest.cs | 2 +- Demos/SpecializedTests/ConvexHullTestDemo.cs | 2 +- Demos/SpecializedTests/CylinderTestDemo.cs | 2 +- Demos/SpecializedTests/GyroscopeTestDemo.cs | 2 +- Demos/SpecializedTests/Media/BedsheetDemo.cs | 5 +- .../Media/ColosseumVideoDemo.cs | 2 +- .../Media/NewtDemandingSacrificeVideoDemo.cs | 2 +- Demos/SpecializedTests/Media/NewtVideoDemo.cs | 2 +- .../Media/PyramidVideoDemo.cs | 2 +- .../Media/ShrinkwrappedNewtsVideoDemo.cs | 2 +- Demos/SpecializedTests/MeshMeshTestDemo.cs | 2 +- .../SpecializedTests/MeshReductionTestDemo.cs | 5 +- .../MeshSerializationTestDemo.cs | 2 +- Demos/SpecializedTests/MeshTestDemo.cs | 2 +- .../PyramidAwakenerTestDemo.cs | 2 +- Demos/SpecializedTests/ShapePileTestDemo.cs | 2 +- Demos/SpecializedTests/SolverBatchTestDemo.cs | 2 +- Demos/SpecializedTests/TriangleRayTestDemo.cs | 2 +- Demos/SpecializedTests/TriangleTestDemo.cs | 2 +- Demos/SpecializedTests/VolumeQueryTests.cs | 2 +- 48 files changed, 98 insertions(+), 502 deletions(-) delete mode 100644 BepuPhysics/PositionFirstTimestepper.cs delete mode 100644 BepuPhysics/PositionLastTimestepper.cs delete mode 100644 BepuPhysics/SubsteppingTimestepper.cs diff --git a/BepuPhysics/Bodies.cs b/BepuPhysics/Bodies.cs index e9ed7e4e5..fc36e2079 100644 --- a/BepuPhysics/Bodies.cs +++ b/BepuPhysics/Bodies.cs @@ -319,6 +319,19 @@ internal unsafe static bool IsKinematicUnsafeGCHole(ref BodyInertia inertia) return IsKinematic((BodyInertia*)Unsafe.AsPointer(ref inertia)); } + /// + /// Checks inertia lanes for kinematicity (all inverse mass and inertia values are zero). + /// + /// Inertia to examine for kinematicity. + /// Mask of lanes which contain zeroed inverse masses and inertias. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector IsKinematic(BodyInertiaWide inertia) + { + return Vector.Equals(Vector.BitwiseOr( + Vector.BitwiseOr(Vector.BitwiseOr(inertia.InverseMass, inertia.InverseInertiaTensor.XX), Vector.BitwiseOr(inertia.InverseInertiaTensor.YX, inertia.InverseInertiaTensor.YY)), + Vector.BitwiseOr(Vector.BitwiseOr(inertia.InverseInertiaTensor.ZX, inertia.InverseInertiaTensor.ZY), inertia.InverseInertiaTensor.ZZ)), Vector.Zero); + } + /// /// Gets whether the inertia matches that of a kinematic body (that is, all inverse mass and inertia components are zero). /// diff --git a/BepuPhysics/PoseIntegrator.cs b/BepuPhysics/PoseIntegrator.cs index 86839f945..30d57176d 100644 --- a/BepuPhysics/PoseIntegrator.cs +++ b/BepuPhysics/PoseIntegrator.cs @@ -288,9 +288,8 @@ public PoseIntegrator(Bodies bodies, Shapes shapes, BroadPhase broadPhase, TCall } [MethodImpl(MethodImplOptions.AggressiveInlining)] - void UpdateSleepCandidacy(ref BodyVelocity velocity, ref BodyActivity activity) + void UpdateSleepCandidacy(float velocityHeuristic, ref BodyActivity activity) { - var velocityHeuristic = velocity.Linear.LengthSquared() + velocity.Angular.LengthSquared(); if (velocityHeuristic > activity.SleepThreshold) { activity.TimestepsUnderThresholdCount = 0; @@ -307,154 +306,6 @@ void UpdateSleepCandidacy(ref BodyVelocity velocity, ref BodyActivity activity) } } } - - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - void IntegrateAngularVelocityConserving(in Quaternion previousOrientation, in RigidPose pose, in BodyInertia localInertia, in BodyInertia inertia, ref Vector3 angularVelocity, float dt) - { - previousOrientation.ValidateOrientation(); - pose.Orientation.ValidateOrientation(); - angularVelocity.Validate(); - - if (Callbacks.AngularIntegrationMode == AngularIntegrationMode.ConserveMomentum) - { - //Note that this effectively recomputes the previous frame's inertia. There may not have been a previous inertia stored in the inertias buffer. - //This just avoids the need for quite a bit of complexity around keeping the world inertias buffer updated with adds/removes/moves and other state changes that we can't easily track. - //Also, even if it were cached, the memory bandwidth requirements of loading another inertia tensor would hurt multithreaded scaling enough to eliminate any performance advantage. - Matrix3x3.CreateFromQuaternion(previousOrientation, out var previousOrientationMatrix); - Matrix3x3.TransformTranspose(angularVelocity, previousOrientationMatrix, out var localPreviousAngularVelocity); - Symmetric3x3.Invert(localInertia.InverseInertiaTensor, out var localInertiaTensor); - Symmetric3x3.TransformWithoutOverlap(localPreviousAngularVelocity, localInertiaTensor, out var localAngularMomentum); - Matrix3x3.Transform(localAngularMomentum, previousOrientationMatrix, out var angularMomentum); - Symmetric3x3.TransformWithoutOverlap(angularMomentum, inertia.InverseInertiaTensor, out angularVelocity); - } - - //Note that this mode branch is optimized out for any callbacks that return a constant value. - if (Callbacks.AngularIntegrationMode == AngularIntegrationMode.ConserveMomentumWithGyroscopicTorque) - { - //Integrating the gyroscopic force explicitly can result in some instability, so we'll use an approximate implicit approach. - //angularVelocity1 * inertia1 = angularVelocity0 * inertia1 + dt * ((angularVelocity1 * inertia1) x angularVelocity1) - //Note that this includes a reference to inertia1 which doesn't exist yet. We do, however, have the local inertia, so we'll - //transform all velocities into local space using the current orientation for the calculation. - //So: - //localAngularVelocity1 * localInertia = localAngularVelocity0 * localInertia - dt * (localAngularVelocity1 x (localAngularVelocity1 * localInertia)) - //localAngularVelocity1 * localInertia - localAngularVelocity0 * localInertia + dt * (localAngularVelocity1 x (localAngularVelocity1 * localInertia)) = 0 - //f(localAngularVelocity1) = (localAngularVelocity1 - localAngularVelocity0) * localInertia + dt * (localAngularVelocity1 x (localAngularVelocity1 * localInertia)) - //Not trivial to solve for localAngularVelocity1 so we'll do so numerically with a newton iteration. - //(For readers familiar with Bullet's BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY, this is basically identical.) - - //We'll start with an initial guess of localAngularVelocity1 = localAngularVelocity0, and update with a newton step of f(localAngularVelocity1) * invert(df/dw1(localAngularVelocity1)) - //df/dw1x(localAngularVelocity1) * localInertia + dt * (df/dw1x(localAngularVelocity1) x (localAngularVelocity1 * localInertia) + localAngularVelocity1 x df/dw1x(localAngularVelocity1 * localInertia)) - //df/dw1x(localAngularVelocity1) = (1,0,0) - //df/dw1x(f(localAngularVelocity1)) = (1, 0, 0) * localInertia + dt * ((1, 0, 0) x (localAngularVelocity1 * localInertia) + localAngularVelocity1 x ((1, 0, 0) * localInertia)) - //df/dw1x(f(localAngularVelocity1)) = (0, 1, 0) * localInertia + dt * ((0, 1, 0) x (localAngularVelocity1 * localInertia) + localAngularVelocity1 x ((0, 1, 0) * localInertia)) - //df/dw1x(f(localAngularVelocity1)) = (0, 0, 1) * localInertia + dt * ((0, 0, 1) x (localAngularVelocity1 * localInertia) + localAngularVelocity1 x ((0, 0, 1) * localInertia)) - //This can be expressed a bit more concisely, given a x b = skew(a) * b, where skew(a) is a skew symmetric matrix representing a cross product: - //df/dw1(f(localAngularVelocity1)) = localInertia + dt * (skew(localAngularVelocity1) * localInertia - skew(localAngularVelocity1 * localInertia)) - Matrix3x3.CreateFromQuaternion(pose.Orientation, out var orientationMatrix); - //Using localAngularVelocity0 as the first guess for localAngularVelocity1. - Matrix3x3.TransformTranspose(angularVelocity, orientationMatrix, out var localAngularVelocity); - Symmetric3x3.Invert(localInertia.InverseInertiaTensor, out var localInertiaTensor); - - Symmetric3x3.TransformWithoutOverlap(localAngularVelocity, localInertiaTensor, out var localAngularMomentum); - var residual = dt * Vector3.Cross(localAngularMomentum, localAngularVelocity); - - Matrix3x3.CreateCrossProduct(localAngularMomentum, out var skewMomentum); - Matrix3x3.CreateCrossProduct(localAngularVelocity, out var skewVelocity); - Symmetric3x3.Multiply(skewVelocity, localInertiaTensor, out var transformedSkewVelocity); - Matrix3x3.Subtract(transformedSkewVelocity, skewMomentum, out var changeOverDt); - Matrix3x3.Scale(changeOverDt, dt, out var change); - Symmetric3x3.Add(change, localInertiaTensor, out var jacobian); - - Matrix3x3.Invert(jacobian, out var inverseJacobian); - Matrix3x3.Transform(residual, inverseJacobian, out var newtonStep); - localAngularVelocity -= newtonStep; - - Matrix3x3.Transform(localAngularVelocity, orientationMatrix, out angularVelocity); - } - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - void IntegrateAngularVelocity(in Quaternion previousOrientation, in RigidPose pose, in BodyInertia localInertia, in BodyInertia inertia, ref Vector3 angularVelocity, float dt) - { - //Note that this mode branch is optimized out for any callbacks that return a constant value. - if ((int)Callbacks.AngularIntegrationMode >= (int)AngularIntegrationMode.ConserveMomentum) - { - if (!Bodies.HasLockedInertia(localInertia.InverseInertiaTensor)) - { - IntegrateAngularVelocityConserving(previousOrientation, pose, localInertia, inertia, ref angularVelocity, dt); - } - } - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - void IntegrateAngularVelocity(in RigidPose pose, in BodyInertia localInertia, in BodyInertia inertia, ref Vector3 angularVelocity, float dt) - { - //We didn't have a previous orientation available. Reconstruct it by integrating backwards. - //(In single threaded terms, caching this information could be faster, but it adds a lot of complexity and could end up reducing performance on higher core counts.) - //Note that this mode branch is optimized out for any callbacks that return a constant value. - if ((int)Callbacks.AngularIntegrationMode >= (int)AngularIntegrationMode.ConserveMomentum) - { - if (!Bodies.HasLockedInertia(localInertia.InverseInertiaTensor)) - { - PoseIntegration.Integrate(pose.Orientation, angularVelocity, -dt, out var previousOrientation); - IntegrateAngularVelocityConserving(previousOrientation, pose, localInertia, inertia, ref angularVelocity, dt); - } - } - } - - unsafe void IntegrateBodiesAndUpdateBoundingBoxes(int startIndex, int endIndex, float dt, ref BoundingBoxBatcher boundingBoxBatcher, int workerIndex) - { - ref var baseStates = ref bodies.ActiveSet.SolverStates[0]; - ref var baseActivity = ref bodies.ActiveSet.Activity[0]; - ref var baseCollidable = ref bodies.ActiveSet.Collidables[0]; - for (int i = startIndex; i < endIndex; ++i) - { - ref var state = ref Unsafe.Add(ref baseStates, i); - - var previousOrientation = state.Motion.Pose.Orientation; //This is unused if conservation of angular momentum is disabled... compiler *may* remove it... - ref var motion = ref state.Motion; - ref var inertia = ref state.Inertia; - PoseIntegration.Integrate(motion.Pose, motion.Velocity, dt, out motion.Pose); - - //Note that this generally is used before velocity integration. That means an object can go inactive with gravity-induced velocity. - //That is actually intended: when the narrowphase wakes up an island, the accumulated impulses in the island will be ready for gravity's influence. - //To do otherwise would hurt the solver's guess, reducing the quality of the solve and possibly causing a little bump. - //This is only relevant when the update order actually puts the sleeper after gravity. For ease of use, this fact may be ignored by the simulation update order. - UpdateSleepCandidacy(ref state.Motion.Velocity, ref Unsafe.Add(ref baseActivity, i)); - - //Update the inertia tensors for the new orientation. - //TODO: If the pose integrator is positioned at the end of an update, the first frame after any out-of-timestep orientation change or local inertia change - //has to get is inertia tensors calculated elsewhere. Either they would need to be computed on addition or something- which is a bit gross, but doable- - //or we would need to move this calculation to the beginning of the frame to guarantee that all inertias are up to date. - //This would require a scan through all pose memory to support, but if you do it at the same time as AABB update, that's fine- that stage uses the pose too.Inertias, i); - PoseIntegration.RotateInverseInertia(inertia.Local.InverseInertiaTensor, motion.Pose.Orientation, out inertia.World.InverseInertiaTensor); - //While it's a bit goofy just to copy over the inverse mass every frame even if it doesn't change, - //it's virtually always gathered together with the inertia tensor and having a duplicate means we can sometimes avoid loading a lane - //(i.e. loading only the last 32 bytes of the cache line into a Vector256). - inertia.World.InverseMass = inertia.Local.InverseMass; - - IntegrateAngularVelocity(previousOrientation, motion.Pose, inertia.Local, inertia.World, ref motion.Velocity.Angular, dt); - Callbacks.IntegrateVelocity(i, motion.Pose, inertia.Local, workerIndex, ref motion.Velocity); - - //Bounding boxes are accumulated in a scalar fashion, but the actual bounding box calculations are deferred until a sufficient number of collidables are accumulated to make - //executing a bundle worthwhile. This does two things: - //1) SIMD can be used to do the mathy bits of bounding box calculation. The calculations are usually pretty cheap, - //but they will often be more expensive than the pose stuff above. - //2) The number of virtual function invocations required is reduced by a factor equal to the size of the accumulator cache. - //Note that the accumulator caches are kept relatively small so that it is very likely that the pose and velocity of the collidable's body will still be in L1 cache - //when it comes time to actually compute bounding boxes. - - //Note that any collidable that lacks a collidable, or any reference that is beyond the set of collidables, will have a specially formed index. - //The accumulator will detect that and not try to add a nonexistent collidable. - boundingBoxBatcher.Add(i, motion.Pose, motion.Velocity, Unsafe.Add(ref baseCollidable, i)); - - //It's helpful to do the bounding box update here in the pose integrator because they share information. If the phases were split, there could be a penalty - //associated with loading all the body poses and velocities from memory again. Even if the L3 cache persisted, it would still be worse than looking into L1 or L2. - //Also, the pose integrator in isolation is extremely memory bound to the point where it can hardly benefit from multithreading. By interleaving some less memory bound - //work into the mix, we can hopefully fill some execution gaps. - } } unsafe void PredictBoundingBoxes(int startBundleIndex, int endBundleIndex, float dt, ref BoundingBoxBatcher boundingBoxBatcher, int workerIndex) @@ -462,16 +313,17 @@ unsafe void PredictBoundingBoxes(int startBundleIndex, int endBundleIndex, float var activities = bodies.ActiveSet.Activity; var collidables = bodies.ActiveSet.Collidables; - var bundleCount = endBundleIndex - startBundleIndex; Helpers.FillVectorWithLaneIndices(out var laneIndexOffsets); var dtWide = new Vector(dt); var bodyCount = bodies.ActiveSet.Count; - for (int bundleIndex = 0; bundleIndex < bundleCount; ++bundleIndex) + for (int bundleIndex = startBundleIndex; bundleIndex < endBundleIndex; ++bundleIndex) { var bundleStartBodyIndex = bundleIndex * Vector.Count; var countInBundle = bodyCount - bundleStartBodyIndex; if (countInBundle > Vector.Count) countInBundle = Vector.Count; + //Note that this is bundle-fied primarily to avoid requiring velocity integration callbacks to implement a scalar and vector version. + //Performance wise, I don't expect a meaningful improvement over a scalar version; there's too little work being done. var laneIndices = new Vector(bundleStartBodyIndex) + laneIndexOffsets; bodies.GatherState(laneIndices, false, out var position, out var orientation, out var velocity, out var inertia); @@ -482,7 +334,7 @@ unsafe void PredictBoundingBoxes(int startBundleIndex, int endBundleIndex, float } else { - integrationMask = Vector.AndNot(BundleIndexing.CreateMaskForCountInBundle(countInBundle), IsKinematic(inertia)); + integrationMask = Vector.AndNot(BundleIndexing.CreateMaskForCountInBundle(countInBundle), Bodies.IsKinematic(inertia)); } var sleepEnergy = velocity.Linear.LengthSquared() + velocity.Angular.LengthSquared(); @@ -493,30 +345,29 @@ unsafe void PredictBoundingBoxes(int startBundleIndex, int endBundleIndex, float for (int i = 0; i < countInBundle; ++i) { var bodyIndex = i + bundleStartBodyIndex; - UpdateSleepCandidacy(sleepEnergy[bodyIndex], ref activities[bodyIndex]); - - boundingBoxBatcher.Add(bodyIndex, bodyPose, bodyVelocity, collidablles[bodyIndex]); + UpdateSleepCandidacy(sleepEnergy[i], ref activities[bodyIndex]); + + //TODO: A vectorized transposition, like what the GatherState function uses, would speed this up a good bit. + //PredictBoundingBoxes isn't a huge cost overall so I didn't do it immediately, but it's an available optimization. + RigidPose bodyPose; + Vector3Wide.ReadSlot(ref position, i, out bodyPose.Position); + QuaternionWide.ReadSlot(ref orientation, i, out bodyPose.Orientation); + BodyVelocity bodyVelocity; + Vector3Wide.ReadSlot(ref velocity.Linear, i, out bodyVelocity.Linear); + Vector3Wide.ReadSlot(ref velocity.Angular, i, out bodyVelocity.Angular); + + //Bounding boxes are accumulated in a scalar fashion, but the actual bounding box calculations are deferred until a sufficient number of collidables are accumulated to make + //executing a bundle worthwhile. This does two things: + //1) SIMD can be used to do the mathy bits of bounding box calculation. The calculations are usually pretty cheap, + //but they will often be more expensive than the pose stuff above. + //2) The number of virtual function invocations required is reduced by a factor equal to the size of the accumulator cache. + //Note that the accumulator caches are kept relatively small so that it is very likely that the pose and velocity of the collidable's body will still be in L1 cache + //when it comes time to actually compute bounding boxes. + + //Note that any collidable that lacks a collidable, or any reference that is beyond the set of collidables, will have a specially formed index. + //The accumulator will detect that and not try to add a nonexistent collidable. + boundingBoxBatcher.Add(bodyIndex, bodyPose, bodyVelocity, collidables[bodyIndex]); } - - } - - for (int i = startIndex; i < endIndex; ++i) - { - ref var state = ref Unsafe.Add(ref baseStates, i); - ref var motion = ref state.Motion; - motion.Pose.Position.Validate(); - motion.Pose.Orientation.ValidateOrientation(); - motion.Velocity.Linear.Validate(); - motion.Velocity.Angular.Validate(); - - UpdateSleepCandidacy(ref motion.Velocity, ref Unsafe.Add(ref baseActivity, i)); - - //Bounding box prediction does not need to update inertia tensors. - var integratedVelocity = motion.Velocity; - Callbacks.IntegrateVelocity(i, motion.Pose, state.Inertia.Local, workerIndex, ref integratedVelocity); - - //Note that we do not include fancier angular integration for the bounding box prediction- it's not very important. - boundingBoxBatcher.Add(i, motion.Pose, integratedVelocity, Unsafe.Add(ref baseCollidable, i)); } } @@ -580,7 +431,7 @@ public void PredictBoundingBoxes(float dt, BufferPool pool, IThreadDispatcher th Callbacks.PrepareForIntegration(dt); if (threadDispatcher != null) { - PrepareForMultithreadedExecution(bodies.ActiveSet.Count, dt, threadDispatcher.ThreadCount); + PrepareForMultithreadedExecution(BundleIndexing.GetBundleCount(bodies.ActiveSet.Count), dt, threadDispatcher.ThreadCount); this.threadDispatcher = threadDispatcher; threadDispatcher.DispatchWorkers(predictBoundingBoxesWorker); this.threadDispatcher = null; @@ -588,7 +439,7 @@ public void PredictBoundingBoxes(float dt, BufferPool pool, IThreadDispatcher th else { var boundingBoxUpdater = new BoundingBoxBatcher(bodies, shapes, broadPhase, pool, dt); - PredictBoundingBoxes(0, bodies.ActiveSet.Count, dt, ref boundingBoxUpdater, 0); + PredictBoundingBoxes(0, BundleIndexing.GetBundleCount(bodies.ActiveSet.Count), dt, ref boundingBoxUpdater, 0); boundingBoxUpdater.Flush(); } @@ -683,13 +534,6 @@ internal unsafe void IntegrateKinematicPosesAndVelocities(Buffer bodyHandle } } - static Vector IsKinematic(BodyInertiaWide inertia) - { - return Vector.Equals(Vector.BitwiseOr( - Vector.BitwiseOr(Vector.BitwiseOr(inertia.InverseMass, inertia.InverseInertiaTensor.XX), Vector.BitwiseOr(inertia.InverseInertiaTensor.YX, inertia.InverseInertiaTensor.YY)), - Vector.BitwiseOr(Vector.BitwiseOr(inertia.InverseInertiaTensor.ZX, inertia.InverseInertiaTensor.ZY), inertia.InverseInertiaTensor.ZZ)), Vector.Zero); - } - unsafe void IntegrateBundlesAfterSubstepping(ref IndexSet mergedConstrainedBodyHandles, int bundleStartIndex, int bundleEndIndex, float dt, float substepDt, int substepCount, int workerIndex) { var bodyCount = bodies.ActiveSet.Count; @@ -758,7 +602,7 @@ unsafe void IntegrateBundlesAfterSubstepping(ref IndexSet mergedConstrainedBodyH } else { - var isKinematic = IsKinematic(localInertia); + var isKinematic = Bodies.IsKinematic(localInertia); unconstrainedVelocityIntegrationMask = Vector.AndNot(unconstrainedMask, isKinematic); anyBodyInBundleNeedsVelocityIntegration = Vector.LessThanAny(unconstrainedVelocityIntegrationMask, Vector.Zero); } diff --git a/BepuPhysics/PositionFirstTimestepper.cs b/BepuPhysics/PositionFirstTimestepper.cs deleted file mode 100644 index 9fa619912..000000000 --- a/BepuPhysics/PositionFirstTimestepper.cs +++ /dev/null @@ -1,71 +0,0 @@ -using System; -using System.Collections.Generic; -using System.Text; -using BepuUtilities; - -namespace BepuPhysics -{ - /// - /// Updates the simulation in the order of: sleeper -> integrate body poses, velocity and bounding boxes -> collision detection -> solver -> data structure optimization. - /// - public class PositionFirstTimestepper : ITimestepper - { - /// - /// Fires after the sleeper completes and before bodies are integrated. - /// - public event TimestepperStageHandler Slept; - /// - /// Fires after bodies have had their position, velocity, and bounding boxes updated, but before collision detection begins. - /// - public event TimestepperStageHandler BeforeCollisionDetection; - /// - /// Fires after all collisions have been identified, but before constraints are solved. - /// - public event TimestepperStageHandler CollisionsDetected; - /// - /// Fires after the solver executes and before data structures are incrementally optimized. - /// - public event TimestepperStageHandler ConstraintsSolved; - - public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDispatcher = null) - { - //Note that there is a reason to put the sleep *after* velocity integration. That sounds a little weird, but there's a good reason: - //When the narrow phase activates a bunch of objects in a pile, their accumulated impulses will represent all forces acting on them at the time of sleep. - //That includes gravity. If we sleep objects *before* gravity is applied in a given frame, then when those bodies are awakened, the accumulated impulses - //will be less accurate because they assume that gravity has already been applied. This can cause a small bump. - //So, velocity integration (and deactivation candidacy management) could come before sleep. - - //Sleep at the start, on the other hand, stops some forms of unintuitive behavior when using direct awakenings. Just a matter of preference. - simulation.Sleep(threadDispatcher); - Slept?.Invoke(dt, threadDispatcher); - - //Note that pose integrator comes before collision detection and solving. This is a shift from v1, where collision detection went first. - //This is a tradeoff: - //1) Any externally set velocities will be integrated without input from the solver. The v1-style external velocity control won't work as well- - //the user would instead have to change velocities after the pose integrator runs. This isn't perfect either, since the pose integrator is also responsible - //for updating the bounding boxes used for collision detection. - //2) By bundling bounding box calculation with pose integration, you avoid redundant pose and velocity memory accesses. - //3) Generated contact positions are in sync with the integrated poses. - //That's often helpful for gameplay purposes- you don't have to reinterpret contact data when creating graphical effects or positioning sound sources. - - //#1 is a difficult problem, though. There is no fully 'correct' place to change velocities. We might just have to bite the bullet and create a - //inertia tensor/bounding box update separate from pose integration. If the cache gets evicted in between (virtually guaranteed unless no stages run), - //this basically means an extra 100-200 microseconds per frame on a processor with ~20GBps bandwidth simulating 32768 bodies. - - //Note that the reason why the pose integrator comes first instead of, say, the solver, is that the solver relies on world space inertias calculated by the pose integration. - //If the pose integrator doesn't run first, we either need - //1) complicated on demand updates of world inertia when objects are added or local inertias are changed or - //2) local->world inertia calculation before the solver. - simulation.IntegrateBodiesAndUpdateBoundingBoxes(dt, threadDispatcher); - BeforeCollisionDetection?.Invoke(dt, threadDispatcher); - - simulation.CollisionDetection(dt, threadDispatcher); - CollisionsDetected?.Invoke(dt, threadDispatcher); - - simulation.Solve(dt, threadDispatcher); - ConstraintsSolved?.Invoke(dt, threadDispatcher); - - simulation.IncrementallyOptimizeDataStructures(threadDispatcher); - } - } -} diff --git a/BepuPhysics/PositionLastTimestepper.cs b/BepuPhysics/PositionLastTimestepper.cs deleted file mode 100644 index e58b6f5cb..000000000 --- a/BepuPhysics/PositionLastTimestepper.cs +++ /dev/null @@ -1,55 +0,0 @@ -using System; -using System.Collections.Generic; -using System.Diagnostics; -using System.Text; -using BepuUtilities; - -namespace BepuPhysics -{ - /// - /// Updates the simulation in the order of: sleeper -> integrate velocities and update body bounding boxes -> collision detection -> solver -> integrate body poses -> data structure optimization. - /// - public class PositionLastTimestepper : ITimestepper - { - /// - /// Fires after the sleeper completes and before bodies are integrated. - /// - public event TimestepperStageHandler Slept; - /// - /// Fires after bodies have had their velocities and bounding boxes updated, but before collision detection begins. - /// - public event TimestepperStageHandler BeforeCollisionDetection; - /// - /// Fires after all collisions have been identified, but before constraints are solved. - /// - public event TimestepperStageHandler CollisionsDetected; - /// - /// Fires after the solver executes and before body poses are integrated. - /// - public event TimestepperStageHandler ConstraintsSolved; - /// - /// Fires after bodies have their poses integrated and before data structures are incrementally optimized. - /// - public event TimestepperStageHandler PosesIntegrated; - - public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDispatcher = null) - { - simulation.Sleep(threadDispatcher); - Slept?.Invoke(dt, threadDispatcher); - - simulation.IntegrateVelocitiesBoundsAndInertias(dt, threadDispatcher); - BeforeCollisionDetection?.Invoke(dt, threadDispatcher); - - simulation.CollisionDetection(dt, threadDispatcher); - CollisionsDetected?.Invoke(dt, threadDispatcher); - - simulation.Solve(dt, threadDispatcher); - ConstraintsSolved?.Invoke(dt, threadDispatcher); - - simulation.IntegratePoses(dt, threadDispatcher); - PosesIntegrated?.Invoke(dt, threadDispatcher); - - simulation.IncrementallyOptimizeDataStructures(threadDispatcher); - } - } -} diff --git a/BepuPhysics/SubsteppingTimestepper.cs b/BepuPhysics/SubsteppingTimestepper.cs deleted file mode 100644 index 460f2c517..000000000 --- a/BepuPhysics/SubsteppingTimestepper.cs +++ /dev/null @@ -1,107 +0,0 @@ -using System; -using System.Collections.Generic; -using System.Diagnostics; -using System.Text; -using BepuUtilities; - -namespace BepuPhysics -{ - /// - /// Updates the simulation in the order of: sleeper -> predict body bounding boxes -> collision detection -> LOOP { contact data update (if on iteration > 0) -> integrate body velocities -> solver -> integrate body poses } -> data structure optimization. - /// Each inner loop execution simulates a sub-timestep of length dt/substepCount. - /// Useful for simulations with difficult to solve constraint systems that need shorter timestep durations but which don't require high frequency collision detection. - /// - public class SubsteppingTimestepper : ITimestepper - { - /// - /// Gets or sets the number of substeps to execute during each timestep. - /// - public int SubstepCount { get; set; } - - /// - /// Fires after the sleeper completes and before bodies are integrated. - /// - public event TimestepperStageHandler Slept; - /// - /// Fires after bodies have their bounding boxes updated for the frame's predicted motion and before collision detection. - /// - public event TimestepperStageHandler BeforeCollisionDetection; - /// - /// Fires after all collisions have been identified, but before the substep loop begins. - /// - public event TimestepperStageHandler CollisionsDetected; - /// - /// Fires at the beginning of a substep. - /// - public event TimestepperSubstepStageHandler SubstepStarted; - /// - /// Fires after contact constraints are incrementally updated at the beginning of substeps after the first and before velocities are integrated. - /// - public event TimestepperSubstepStageHandler ContactConstraintsUpdatedForSubstep; - /// - /// Fires after bodies have their velocities integrated and before the solver executes. - /// - public event TimestepperSubstepStageHandler VelocitiesIntegrated; - /// - /// Fires after the solver executes and before body poses are integrated. - /// - public event TimestepperSubstepStageHandler ConstraintsSolved; - /// - /// Fires after bodies have their poses integrated and before the substep ends. - /// - public event TimestepperSubstepStageHandler PosesIntegrated; - /// - /// Fires at the end of a substep. - /// - public event TimestepperSubstepStageHandler SubstepEnded; - /// - /// Fires after all substeps are finished executing and before data structures are incrementally optimized. - /// - public event TimestepperStageHandler SubstepsComplete; - - public SubsteppingTimestepper(int substepCount) - { - SubstepCount = substepCount; - } - - public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDispatcher = null) - { - simulation.Sleep(threadDispatcher); - Slept?.Invoke(dt, threadDispatcher); - - simulation.PredictBoundingBoxes(dt, threadDispatcher); - BeforeCollisionDetection?.Invoke(dt, threadDispatcher); - - simulation.CollisionDetection(dt, threadDispatcher); - CollisionsDetected?.Invoke(dt, threadDispatcher); - - Debug.Assert(SubstepCount >= 0, "Substep count should be positive."); - var substepDt = dt / SubstepCount; - - for (int substepIndex = 0; substepIndex < SubstepCount; ++substepIndex) - { - SubstepStarted?.Invoke(substepIndex, dt, threadDispatcher); - if (substepIndex > 0) - { - //This takes the place of collision detection for the substeps. It uses the current velocity to update penetration depths. - //It's definitely an approximation, but it's important for avoiding some obviously weird behavior. - //Note that we do not run this on the first iteration- the actual collision detection above takes care of it. - simulation.IncrementallyUpdateContactConstraints(substepDt, threadDispatcher); - ContactConstraintsUpdatedForSubstep?.Invoke(substepIndex, dt, threadDispatcher); - } - simulation.IntegrateVelocitiesAndUpdateInertias(substepDt, threadDispatcher); - VelocitiesIntegrated?.Invoke(substepIndex, dt, threadDispatcher); - - simulation.Solve(substepDt, threadDispatcher); - ConstraintsSolved?.Invoke(substepIndex, dt, threadDispatcher); - - simulation.IntegratePoses(substepDt, threadDispatcher); - PosesIntegrated?.Invoke(substepIndex, dt, threadDispatcher); - SubstepEnded?.Invoke(substepIndex, dt, threadDispatcher); - } - SubstepsComplete?.Invoke(dt, threadDispatcher); - - simulation.IncrementallyOptimizeDataStructures(threadDispatcher); - } - } -} diff --git a/BepuUtilities/QuaternionWide.cs b/BepuUtilities/QuaternionWide.cs index f8bbeed46..faeb4ad99 100644 --- a/BepuUtilities/QuaternionWide.cs +++ b/BepuUtilities/QuaternionWide.cs @@ -628,5 +628,18 @@ public static void WriteSlot(in Quaternion source, int slotIndex, ref Quaternion { WriteFirst(source, ref GatherScatter.GetOffsetInstance(ref target, slotIndex)); } + + /// + /// Pulls one lane out of the wide representation. + /// + /// Source of the lane. + /// Index of the lane within the wide representation to read. + /// Non-SIMD type to store the lane in. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void ReadSlot(ref QuaternionWide wide, int slotIndex, out Quaternion narrow) + { + ref var offset = ref GatherScatter.GetOffsetInstance(ref wide, slotIndex); + ReadFirst(offset, out narrow); + } } } \ No newline at end of file diff --git a/BepuUtilities/Vector3Wide.cs b/BepuUtilities/Vector3Wide.cs index f95bebe9d..fd975df3e 100644 --- a/BepuUtilities/Vector3Wide.cs +++ b/BepuUtilities/Vector3Wide.cs @@ -559,7 +559,6 @@ public static void ReadSlot(ref Vector3Wide wide, int slotIndex, out Vector3 nar ReadFirst(offset, out narrow); } - /// /// Pulls one lane out of the wide representation. /// diff --git a/DemoTests/ConstraintDescriptionMappingTests.cs b/DemoTests/ConstraintDescriptionMappingTests.cs index f0d8e4ae1..9d8c9b28f 100644 --- a/DemoTests/ConstraintDescriptionMappingTests.cs +++ b/DemoTests/ConstraintDescriptionMappingTests.cs @@ -25,7 +25,7 @@ static void FillWithRandomBytes(ref T item, Random random) where T : struct } static void Test(BufferPool pool, Random random, int constraintTypeBodyCount) where T : unmanaged, IConstraintDescription { - var simulation = Simulation.Create(pool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(), new PositionFirstTimestepper()); + var simulation = Simulation.Create(pool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(), new EmbeddedSubsteppingTimestepper2(4), 1); const int bodyCount = 2048; diff --git a/Demos/Demos/BouncinessDemo.cs b/Demos/Demos/BouncinessDemo.cs index d63789012..cf8432692 100644 --- a/Demos/Demos/BouncinessDemo.cs +++ b/Demos/Demos/BouncinessDemo.cs @@ -86,7 +86,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //That allows higher stiffnesses to be used since collisions last longer relative to the solver timestep duration. //(Note that substepping tends to be an extremely strong simulation stabilizer, so you can usually get away with lower solver iteration counts for better performance.) var collidableMaterials = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new BounceCallbacks() { CollidableMaterials = collidableMaterials }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), solverIterationCount: 2); + Simulation = Simulation.Create(BufferPool, new BounceCallbacks() { CollidableMaterials = collidableMaterials }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), solverIterationCount: 2); var shape = new Sphere(1); var ballDescription = BodyDescription.CreateDynamic(RigidPose.Identity, shape.ComputeInertia(1), new(Simulation.Shapes.Add(shape), ContinuousDetection.Discrete(20, 20)), 1e-2f); diff --git a/Demos/Demos/ClothDemo.cs b/Demos/Demos/ClothDemo.cs index 9cfe18b79..69c14c007 100644 --- a/Demos/Demos/ClothDemo.cs +++ b/Demos/Demos/ClothDemo.cs @@ -194,10 +194,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0; var filters = new CollidableProperty(); - //The PositionFirstTimestepper is the simplest timestepping mode, but since it integrates velocity into position at the start of the frame, directly modified velocities outside of the timestep - //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. - //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. - Simulation = Simulation.Create(BufferPool, new ClothCallbacks() { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + Simulation = Simulation.Create(BufferPool, new ClothCallbacks() { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); rolloverInfo = new RolloverInfo(); bool KinematicTopCorners(int rowIndex, int columnIndex, int width, int height) diff --git a/Demos/Demos/CollisionQueryDemo.cs b/Demos/Demos/CollisionQueryDemo.cs index 7f8bc6fa8..76f0b6eb1 100644 --- a/Demos/Demos/CollisionQueryDemo.cs +++ b/Demos/Demos/CollisionQueryDemo.cs @@ -27,10 +27,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Yaw = 0; camera.Pitch = 0; - //The PositionFirstTimestepper is the simplest timestepping mode, but since it integrates velocity into position at the start of the frame, directly modified velocities outside of the timestep - //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. - //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(new Box(100, 1, 100)))); diff --git a/Demos/Demos/ColosseumDemo.cs b/Demos/Demos/ColosseumDemo.cs index 6b5b9e03f..b13f12898 100644 --- a/Demos/Demos/ColosseumDemo.cs +++ b/Demos/Demos/ColosseumDemo.cs @@ -64,11 +64,6 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = MathHelper.Pi * 0.2f; - //The PositionFirstTimestepper is the simplest timestepping mode, but since it integrates velocity into position at the start of the frame, directly modified velocities outside of the timestep - //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. - //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. - //Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); - //Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper2(3), solverIterationCount: 1); Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), solverIterationCount: 1); var ringBoxShape = new Box(0.5f, 1, 3); diff --git a/Demos/Demos/ContactEventsDemo.cs b/Demos/Demos/ContactEventsDemo.cs index f0a604295..7eb0ad3ac 100644 --- a/Demos/Demos/ContactEventsDemo.cs +++ b/Demos/Demos/ContactEventsDemo.cs @@ -702,10 +702,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi; events = new ContactEvents(ThreadDispatcher, BufferPool); - //The PositionFirstTimestepper is the simplest timestepping mode, but since it integrates velocity into position at the start of the frame, directly modified velocities outside of the timestep - //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. - //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. - Simulation = Simulation.Create(BufferPool, new ContactEventCallbacks(events), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + Simulation = Simulation.Create(BufferPool, new ContactEventCallbacks(events), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); eventHandler = new EventHandler(Simulation, BufferPool); var listenedBody1 = Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 5, 0), 1, Simulation.Shapes, new Box(1, 2, 3))); diff --git a/Demos/Demos/ContinuousCollisionDetectionDemo.cs b/Demos/Demos/ContinuousCollisionDetectionDemo.cs index e9195f7d2..c237bfa40 100644 --- a/Demos/Demos/ContinuousCollisionDetectionDemo.cs +++ b/Demos/Demos/ContinuousCollisionDetectionDemo.cs @@ -52,12 +52,9 @@ public override void Initialize(ContentArchive content, Camera camera) //Note the higher stiffness on contacts for this demo. That's not ideal for general stability at the demo timestep duration default of 60hz, but //this demo doesn't have any significant solver complexity and we want to see the CCD in action more clearly- which means more rigid contact. //Having objects bounce a bunch on impact makes it harder to see. - //Also note that the PositionFirstTimestepper is the simplest timestepping mode, but since it integrates velocity into position at the start of the frame, directly modified velocities outside of the timestep - //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. - //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(240, 1), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 1f }, - new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 2); var shape = new Box(1, 1, 1); var inertia = shape.ComputeInertia(1); diff --git a/Demos/Demos/CustomVoxelCollidableDemo.cs b/Demos/Demos/CustomVoxelCollidableDemo.cs index 88d88818e..12a01b514 100644 --- a/Demos/Demos/CustomVoxelCollidableDemo.cs +++ b/Demos/Demos/CustomVoxelCollidableDemo.cs @@ -374,10 +374,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = MathHelper.Pi * 0.05f; - //The PositionFirstTimestepper is the simplest timestepping mode, but since it integrates velocity into position at the start of the frame, directly modified velocities outside of the timestep - //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. - //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); //The narrow phase must be notified about the existence of the new collidable type. For every pair type we want to support, a collision task must be registered. //All of the default engine types are registered upon simulation creation by a call to DefaultTypes.CreateDefaultCollisionTaskRegistry. diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index d3ff7c783..d05d7c518 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -666,7 +666,7 @@ internal unsafe static void CreateDeformable(Simulation simulation, in Vector3 p { vertexHandles[i] = simulation.Bodies.Add(BodyDescription.CreateDynamic((position + QuaternionEx.Transform(vertices[i], orientation), orientation), vertexInertia, //Bodies don't have to have collidables. Take advantage of this for all the internal vertices. - vertexEdgeCounts[i] == edgeCountForInternalVertex ? new TypedIndex() : vertexShapeIndex, -0.01f)); + vertexEdgeCounts[i] == edgeCountForInternalVertex ? new TypedIndex() : vertexShapeIndex, 0.01f)); ref var vertexSpatialIndex = ref vertexSpatialIndices[i]; filters.Allocate(vertexHandles[i]) = new DeformableCollisionFilter(vertexSpatialIndex.X, vertexSpatialIndex.Y, vertexSpatialIndex.Z, instanceId); } @@ -710,7 +710,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper(1)); - Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0), 0, 0), new EmbeddedSubsteppingTimestepper2(3), solverIterationCount: 1, 512); + Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0), 0, 0), new EmbeddedSubsteppingTimestepper2(3), solverIterationCount: 1); //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper2(), solverIterationCount: 60); //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)), new SubsteppingTimestepper2(3), solverIterationCount: 1); diff --git a/Demos/Demos/PlanetDemo.cs b/Demos/Demos/PlanetDemo.cs index 54dc81d94..022cf895a 100644 --- a/Demos/Demos/PlanetDemo.cs +++ b/Demos/Demos/PlanetDemo.cs @@ -66,10 +66,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi; camera.Pitch = 0; - //The PositionFirstTimestepper is the simplest timestepping mode, but since it integrates velocity into position at the start of the frame, directly modified velocities outside of the timestep - //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. - //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new PlanetaryGravityCallbacks() { PlanetCenter = new Vector3(), Gravity = 100000 }, new PositionFirstTimestepper()); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new PlanetaryGravityCallbacks() { PlanetCenter = new Vector3(), Gravity = 100000 }, new EmbeddedSubsteppingTimestepper2(4), 1); Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(new Sphere(50)))); diff --git a/Demos/Demos/RayCastingDemo.cs b/Demos/Demos/RayCastingDemo.cs index 45dc3ec81..d7de2b859 100644 --- a/Demos/Demos/RayCastingDemo.cs +++ b/Demos/Demos/RayCastingDemo.cs @@ -63,10 +63,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-20f, 13, -20f); camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = MathHelper.Pi * 0.1f; - //The PositionFirstTimestepper is the simplest timestepping mode, but since it integrates velocity into position at the start of the frame, directly modified velocities outside of the timestep - //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. - //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. - Simulation = Simulation.Create(BufferPool, new NoCollisionCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + Simulation = Simulation.Create(BufferPool, new NoCollisionCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); diff --git a/Demos/Demos/RopeStabilityDemo.cs b/Demos/Demos/RopeStabilityDemo.cs index d13c39999..a78d051b9 100644 --- a/Demos/Demos/RopeStabilityDemo.cs +++ b/Demos/Demos/RopeStabilityDemo.cs @@ -92,12 +92,12 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //In this simulation, using 4 substeps with 1 velocity iteration each costs about 25% more than the non-substepping version with 8 velocity iterations. Not too bad for the quality increase. //Also note that both of these simulation configurations are using a higher than demo-usual contact stiffness. That's just so that you can wrap the rope around the nearby capsule. //In a simulation with lots of stacking, high contact stiffness would require substepping or a higher update rate for stability. - //Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(120, 1) }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); + //Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(120, 1) }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); //So, even though you can avoid the need for these kinds of hacks, it's good to know that they exist should you find yourself in a circumstance where substepping isn't viable. Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(120, 1), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 2f }, - new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); rolloverInfo = new RolloverInfo(); var smallWreckingBall = new Sphere(1); diff --git a/Demos/Demos/SimpleSelfContainedDemo.cs b/Demos/Demos/SimpleSelfContainedDemo.cs index 60f4fa030..b41bbef94 100644 --- a/Demos/Demos/SimpleSelfContainedDemo.cs +++ b/Demos/Demos/SimpleSelfContainedDemo.cs @@ -211,7 +211,7 @@ public static void Run() //PositionLastTimestepper avoids that by running collision detection and the solver first at the cost of a tiny amount of overhead. //(You could avoid the issue with PositionFirstTimestepper by modifying velocities in the PositionFirstTimestepper's BeforeCollisionDetection callback //instead of outside the timestep, too, but it's a little more complicated.) - var simulation = Simulation.Create(bufferPool, new NarrowPhaseCallbacks(), new PoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionLastTimestepper()); + var simulation = Simulation.Create(bufferPool, new NarrowPhaseCallbacks(), new PoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); //Drop a ball on a big static box. var sphere = new Sphere(1); diff --git a/Demos/Demos/SolverContactEnumerationDemo.cs b/Demos/Demos/SolverContactEnumerationDemo.cs index 3bb15421f..18e2d54f9 100644 --- a/Demos/Demos/SolverContactEnumerationDemo.cs +++ b/Demos/Demos/SolverContactEnumerationDemo.cs @@ -26,10 +26,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Yaw = 0; camera.Pitch = 0; - //The PositionFirstTimestepper is the simplest timestepping mode, but since it integrates velocity into position at the start of the frame, directly modified velocities outside of the timestep - //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. - //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); //Drop a pyramid on top of the sensor so there are more contacts to look at. var boxShape = new Box(1, 1, 1); diff --git a/Demos/Demos/Sponsors/SponsorDemo.cs b/Demos/Demos/Sponsors/SponsorDemo.cs index f721ac727..00479f4e9 100644 --- a/Demos/Demos/Sponsors/SponsorDemo.cs +++ b/Demos/Demos/Sponsors/SponsorDemo.cs @@ -109,9 +109,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0.4f; characterControllers = new CharacterControllers(BufferPool); - //Using a PositionLastTimestepper since we control the newts by velocity. Not as critical since they're kinematic and the position targets won't seek to cause undesired penetrations anyway, - //but it'll avoid integrating velocities into positions before the solver has a chance to intervene. - Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characterControllers), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionLastTimestepper()); + Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characterControllers), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); DemoMeshHelper.LoadModel(content, BufferPool, @"Content\newt.obj", new Vector3(-10, 10, -10), out var newtMesh); var newtShape = Simulation.Shapes.Add(newtMesh); diff --git a/Demos/Demos/SweepDemo.cs b/Demos/Demos/SweepDemo.cs index 460812c2e..173c1c80d 100644 --- a/Demos/Demos/SweepDemo.cs +++ b/Demos/Demos/SweepDemo.cs @@ -31,7 +31,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(0, 10, 40); camera.Yaw = 0; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); var box = new Box(2f, 2f, 2f); var capsule = new Capsule(1f, 1f); diff --git a/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs b/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs index 8a6bfcda3..c4ea18ed7 100644 --- a/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs +++ b/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs @@ -20,7 +20,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-20f, 13, -20f); camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = MathHelper.Pi * 0.1f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); var shape = new Sphere(0.5f); var sphereInertia = shape.ComputeInertia(1); diff --git a/Demos/SpecializedTests/CapsuleTestDemo.cs b/Demos/SpecializedTests/CapsuleTestDemo.cs index 6979cfbb4..0ed56e247 100644 --- a/Demos/SpecializedTests/CapsuleTestDemo.cs +++ b/Demos/SpecializedTests/CapsuleTestDemo.cs @@ -20,7 +20,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //camera.Yaw = MathHelper.Pi ; camera.Yaw = MathHelper.Pi * 3f / 4; //camera.Pitch = MathHelper.Pi * 0.1f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); var shape = new Capsule(.5f, .5f); var localInertia = shape.ComputeInertia(1); diff --git a/Demos/SpecializedTests/CharacterTestDemo.cs b/Demos/SpecializedTests/CharacterTestDemo.cs index 6a1f8ed45..e20e1c802 100644 --- a/Demos/SpecializedTests/CharacterTestDemo.cs +++ b/Demos/SpecializedTests/CharacterTestDemo.cs @@ -21,7 +21,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = MathHelper.Pi * 0.05f; var masks = new CollidableProperty(); characters = new CharacterControllers(BufferPool); - Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); var random = new Random(5); for (int i = 0; i < 8192; ++i) diff --git a/Demos/SpecializedTests/ClothLatticeDemo.cs b/Demos/SpecializedTests/ClothLatticeDemo.cs index 3c5ca06e2..09e695f39 100644 --- a/Demos/SpecializedTests/ClothLatticeDemo.cs +++ b/Demos/SpecializedTests/ClothLatticeDemo.cs @@ -20,10 +20,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-120, 30, -120); camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = 0.1f; - //The PositionFirstTimestepper is the simplest timestepping mode, but since it integrates velocity into position at the start of the frame, directly modified velocities outside of the timestep - //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. - //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); //Build a grid of shapes to be connected. var clothNodeShape = new Sphere(0.5f); diff --git a/Demos/SpecializedTests/CompoundBoundTests.cs b/Demos/SpecializedTests/CompoundBoundTests.cs index 5a0ba545b..91ff8144a 100644 --- a/Demos/SpecializedTests/CompoundBoundTests.cs +++ b/Demos/SpecializedTests/CompoundBoundTests.cs @@ -18,7 +18,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(-10, 0, -10); camera.Yaw = MathHelper.Pi * 3f / 4; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); } diff --git a/Demos/SpecializedTests/CompoundCollisionIndicesTest.cs b/Demos/SpecializedTests/CompoundCollisionIndicesTest.cs index 18e0a5bb1..566c0fb1a 100644 --- a/Demos/SpecializedTests/CompoundCollisionIndicesTest.cs +++ b/Demos/SpecializedTests/CompoundCollisionIndicesTest.cs @@ -72,7 +72,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(0, 4, -6); camera.Yaw = MathHelper.Pi; - Simulation = Simulation.Create(BufferPool, new IndexReportingNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, 0f, 0)), new PositionFirstTimestepper()); + Simulation = Simulation.Create(BufferPool, new IndexReportingNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, 0f, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); var builder = new CompoundBuilder(BufferPool, Simulation.Shapes, 4); builder.Add(new Sphere(0.5f), new Vector3(-1, 0, 0), 1); diff --git a/Demos/SpecializedTests/ConvexHullTestDemo.cs b/Demos/SpecializedTests/ConvexHullTestDemo.cs index bee2df2be..5a9c4830f 100644 --- a/Demos/SpecializedTests/ConvexHullTestDemo.cs +++ b/Demos/SpecializedTests/ConvexHullTestDemo.cs @@ -27,7 +27,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Yaw = 0; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), solverIterationCount: 1); //var meshContent = content.Load("Content\\newt.obj"); diff --git a/Demos/SpecializedTests/CylinderTestDemo.cs b/Demos/SpecializedTests/CylinderTestDemo.cs index 6dfae84cf..d92f61ccb 100644 --- a/Demos/SpecializedTests/CylinderTestDemo.cs +++ b/Demos/SpecializedTests/CylinderTestDemo.cs @@ -170,7 +170,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0; camera.Yaw = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, 0f, 0)), new PositionFirstTimestepper()); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, 0f, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); var cylinderShape = new Cylinder(1f, .2f); var cylinder = BodyDescription.CreateDynamic(new Vector3(10f, 3, 0), cylinderShape.ComputeInertia(1), new(Simulation.Shapes.Add(cylinderShape), ContinuousDetection.CreatePassive(1000f, 1000f)), 0.01f); diff --git a/Demos/SpecializedTests/GyroscopeTestDemo.cs b/Demos/SpecializedTests/GyroscopeTestDemo.cs index 700e6d226..269314b35 100644 --- a/Demos/SpecializedTests/GyroscopeTestDemo.cs +++ b/Demos/SpecializedTests/GyroscopeTestDemo.cs @@ -61,7 +61,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0; //Note the lack of damping- we want the gyroscope to keep spinning. - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new GyroscopicIntegratorCallbacks(new Vector3(0, -10, 0), 0f, 0f), new SubsteppingTimestepper(4), 2); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new GyroscopicIntegratorCallbacks(new Vector3(0, -10, 0), 0f, 0f), new EmbeddedSubsteppingTimestepper2(4), 1); Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(new Box(100, 1, 100)))); diff --git a/Demos/SpecializedTests/Media/BedsheetDemo.cs b/Demos/SpecializedTests/Media/BedsheetDemo.cs index ba9feb736..71fbb9343 100644 --- a/Demos/SpecializedTests/Media/BedsheetDemo.cs +++ b/Demos/SpecializedTests/Media/BedsheetDemo.cs @@ -113,10 +113,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = MathF.PI * 0.1f; var filters = new CollidableProperty(); - //The PositionFirstTimestepper is the simplest timestepping mode, but since it integrates velocity into position at the start of the frame, directly modified velocities outside of the timestep - //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. - //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. - Simulation = Simulation.Create(BufferPool, new ClothCallbacks() { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -50, 0)), new PositionFirstTimestepper()); + Simulation = Simulation.Create(BufferPool, new ClothCallbacks() { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -50, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); rolloverInfo = new RolloverInfo(); bool FullyDynamic(int rowIndex, int columnIndex, int width, int height) diff --git a/Demos/SpecializedTests/Media/ColosseumVideoDemo.cs b/Demos/SpecializedTests/Media/ColosseumVideoDemo.cs index 823de7563..eb17ad602 100644 --- a/Demos/SpecializedTests/Media/ColosseumVideoDemo.cs +++ b/Demos/SpecializedTests/Media/ColosseumVideoDemo.cs @@ -68,7 +68,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = MathHelper.Pi * 0.2f; characters = new CharacterControllers(BufferPool); - Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); var ringBoxShape = new Box(0.5f, 1.5f, 3); var boxDescription = BodyDescription.CreateDynamic(new Vector3(), ringBoxShape.ComputeInertia(1), Simulation.Shapes.Add(ringBoxShape), 0.01f); diff --git a/Demos/SpecializedTests/Media/NewtDemandingSacrificeVideoDemo.cs b/Demos/SpecializedTests/Media/NewtDemandingSacrificeVideoDemo.cs index 5ef9b94e4..73404e8bd 100644 --- a/Demos/SpecializedTests/Media/NewtDemandingSacrificeVideoDemo.cs +++ b/Demos/SpecializedTests/Media/NewtDemandingSacrificeVideoDemo.cs @@ -20,7 +20,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = MathHelper.Pi * -0.05f; filters = new CollidableProperty(BufferPool); - Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks() { CollisionFilters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionLastTimestepper()); + Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks() { CollisionFilters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), Simulation.Shapes.Add(new Box(1500, 1, 1500)))); Simulation.Statics.Add(new StaticDescription(new Vector3(0, 10, 0), Simulation.Shapes.Add(new Box(70, 20, 80)))); diff --git a/Demos/SpecializedTests/Media/NewtVideoDemo.cs b/Demos/SpecializedTests/Media/NewtVideoDemo.cs index 64d9a367b..ec3501757 100644 --- a/Demos/SpecializedTests/Media/NewtVideoDemo.cs +++ b/Demos/SpecializedTests/Media/NewtVideoDemo.cs @@ -21,7 +21,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = MathHelper.Pi * 0.15f; var filters = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); var meshContent = content.Load("Content\\newt.obj"); float cellSize = 0.1f; diff --git a/Demos/SpecializedTests/Media/PyramidVideoDemo.cs b/Demos/SpecializedTests/Media/PyramidVideoDemo.cs index 2cfaaefb9..16b881270 100644 --- a/Demos/SpecializedTests/Media/PyramidVideoDemo.cs +++ b/Demos/SpecializedTests/Media/PyramidVideoDemo.cs @@ -22,7 +22,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-70, 8, 318); camera.Yaw = MathHelper.Pi * 1f / 4; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); var boxShape = new Box(1, 1, 1); var boxInertia = boxShape.ComputeInertia(1); diff --git a/Demos/SpecializedTests/Media/ShrinkwrappedNewtsVideoDemo.cs b/Demos/SpecializedTests/Media/ShrinkwrappedNewtsVideoDemo.cs index 31fdb9b6c..a7a7a61e0 100644 --- a/Demos/SpecializedTests/Media/ShrinkwrappedNewtsVideoDemo.cs +++ b/Demos/SpecializedTests/Media/ShrinkwrappedNewtsVideoDemo.cs @@ -18,7 +18,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = 3 * MathHelper.Pi / 4; camera.Pitch = 0;// MathHelper.Pi * 0.15f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); var meshContent = content.Load("Content\\newt.obj"); diff --git a/Demos/SpecializedTests/MeshMeshTestDemo.cs b/Demos/SpecializedTests/MeshMeshTestDemo.cs index 20b38d66d..64fc5cf45 100644 --- a/Demos/SpecializedTests/MeshMeshTestDemo.cs +++ b/Demos/SpecializedTests/MeshMeshTestDemo.cs @@ -21,7 +21,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(0, 8, -10); camera.Yaw = MathHelper.Pi; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); DemoMeshHelper.LoadModel(content, BufferPool, @"Content\newt.obj", Vector3.One, out var mesh); var approximateInertia = new Box(2.5f, 1, 4).ComputeInertia(1); diff --git a/Demos/SpecializedTests/MeshReductionTestDemo.cs b/Demos/SpecializedTests/MeshReductionTestDemo.cs index 9d488aea8..fad71d5f6 100644 --- a/Demos/SpecializedTests/MeshReductionTestDemo.cs +++ b/Demos/SpecializedTests/MeshReductionTestDemo.cs @@ -23,12 +23,9 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Yaw = 0; camera.Pitch = 0; - //The PositionFirstTimestepper is the simplest timestepping mode, but since it integrates velocity into position at the start of the frame, directly modified velocities outside of the timestep - //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. - //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks() { ContactSpringiness = new(30, 1), MaximumRecoveryVelocity = 2, FrictionCoefficient = 0 }, - new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); var builder = new CompoundBuilder(BufferPool, Simulation.Shapes, 2); builder.Add(new Box(1.85f, 0.7f, 4.73f), RigidPose.Identity, 10); diff --git a/Demos/SpecializedTests/MeshSerializationTestDemo.cs b/Demos/SpecializedTests/MeshSerializationTestDemo.cs index 4dbec379c..93b04558a 100644 --- a/Demos/SpecializedTests/MeshSerializationTestDemo.cs +++ b/Demos/SpecializedTests/MeshSerializationTestDemo.cs @@ -17,7 +17,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); var startTime = Stopwatch.GetTimestamp(); DemoMeshHelper.CreateDeformedPlane(1025, 1025, (x, y) => new Vector3(x * 0.125f, MathF.Sin(x) + MathF.Sin(y), y * 0.125f), Vector3.One, BufferPool, out var originalMesh); diff --git a/Demos/SpecializedTests/MeshTestDemo.cs b/Demos/SpecializedTests/MeshTestDemo.cs index 841d807de..8d80978c5 100644 --- a/Demos/SpecializedTests/MeshTestDemo.cs +++ b/Demos/SpecializedTests/MeshTestDemo.cs @@ -19,7 +19,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //camera.Yaw = MathHelper.Pi ; camera.Yaw = MathHelper.Pi * 3f / 4; //camera.Pitch = MathHelper.PiOver2 * 0.999f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); var box = new Box(1f, 3f, 2f); var capsule = new Capsule(1f, 1f); diff --git a/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs b/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs index 6b58098dd..08e5ebab4 100644 --- a/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs +++ b/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs @@ -22,7 +22,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-30, 8, -110); camera.Yaw = MathHelper.Pi * 3f / 4; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); var boxShape = new Box(1, 1, 1); var boxInertia = boxShape.ComputeInertia(1); diff --git a/Demos/SpecializedTests/ShapePileTestDemo.cs b/Demos/SpecializedTests/ShapePileTestDemo.cs index 33113b830..796d1ad0b 100644 --- a/Demos/SpecializedTests/ShapePileTestDemo.cs +++ b/Demos/SpecializedTests/ShapePileTestDemo.cs @@ -20,7 +20,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //camera.Yaw = MathHelper.Pi ; camera.Yaw = MathHelper.Pi * 3f / 4; //camera.Pitch = MathHelper.PiOver2 * 0.999f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); Simulation.Deterministic = true; var sphere = new Sphere(1.5f); diff --git a/Demos/SpecializedTests/SolverBatchTestDemo.cs b/Demos/SpecializedTests/SolverBatchTestDemo.cs index 908df66ff..b034f9944 100644 --- a/Demos/SpecializedTests/SolverBatchTestDemo.cs +++ b/Demos/SpecializedTests/SolverBatchTestDemo.cs @@ -20,7 +20,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-120, 30, -120); camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = 0.1f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); Simulation.Solver.IterationCount = 8; //Build a grid of shapes to be connected. diff --git a/Demos/SpecializedTests/TriangleRayTestDemo.cs b/Demos/SpecializedTests/TriangleRayTestDemo.cs index 0dc2c2607..2bad3f82f 100644 --- a/Demos/SpecializedTests/TriangleRayTestDemo.cs +++ b/Demos/SpecializedTests/TriangleRayTestDemo.cs @@ -94,7 +94,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-30, 8, -60); camera.Yaw = MathHelper.Pi * 3f / 4; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); Triangle triangle; triangle.A = new Vector3(0, 0, 0); diff --git a/Demos/SpecializedTests/TriangleTestDemo.cs b/Demos/SpecializedTests/TriangleTestDemo.cs index f32801e37..946da8229 100644 --- a/Demos/SpecializedTests/TriangleTestDemo.cs +++ b/Demos/SpecializedTests/TriangleTestDemo.cs @@ -92,7 +92,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks() { ContactSpringiness = new BepuPhysics.Constraints.SpringSettings(30, 1), FrictionCoefficient = 1, MaximumRecoveryVelocity = 1 }, - new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); //var triangleDescription = new StaticDescription //{ diff --git a/Demos/SpecializedTests/VolumeQueryTests.cs b/Demos/SpecializedTests/VolumeQueryTests.cs index 8f42066e2..48cfc59a1 100644 --- a/Demos/SpecializedTests/VolumeQueryTests.cs +++ b/Demos/SpecializedTests/VolumeQueryTests.cs @@ -61,7 +61,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-20f, 13, -20f); camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = MathHelper.Pi * 0.1f; - Simulation = Simulation.Create(BufferPool, new NoCollisionCallbacks(), new DemoPoseIntegratorCallbacks(), new PositionFirstTimestepper()); + Simulation = Simulation.Create(BufferPool, new NoCollisionCallbacks(), new DemoPoseIntegratorCallbacks(), new EmbeddedSubsteppingTimestepper2(4), 1); var sphere = new Sphere(0.5f); var shapeIndex = Simulation.Shapes.Add(sphere); From d929fee55e7c60d792610aecc938a316bf9145aa Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 5 Dec 2021 13:05:02 -0600 Subject: [PATCH 309/947] Simplified some body descriptions. --- Demos/Demos/ClothDemo.cs | 8 +------- Demos/Demos/FountainStressTestDemo.cs | 9 +-------- Demos/SpecializedTests/Media/BedsheetDemo.cs | 8 +------- 3 files changed, 3 insertions(+), 22 deletions(-) diff --git a/Demos/Demos/ClothDemo.cs b/Demos/Demos/ClothDemo.cs index 69c14c007..bf699cbdf 100644 --- a/Demos/Demos/ClothDemo.cs +++ b/Demos/Demos/ClothDemo.cs @@ -103,13 +103,7 @@ public class ClothDemo : Demo BodyHandle[,] CreateBodyGrid(Vector3 position, Quaternion orientation, int width, int height, float spacing, float bodyRadius, float massPerBody, int instanceId, CollidableProperty filters, KinematicDecider isKinematic) { - var description = new BodyDescription - { - Activity = 0.01f, - Collidable = Simulation.Shapes.Add(new Sphere(bodyRadius)), - LocalInertia = default, - Pose = orientation - }; + var description = BodyDescription.CreateDynamic(orientation, default, Simulation.Shapes.Add(new Sphere(bodyRadius)), 0.01f); var inverseMass = 1f / massPerBody; BodyHandle[,] handles = new BodyHandle[height, width]; for (int rowIndex = 0; rowIndex < height; ++rowIndex) diff --git a/Demos/Demos/FountainStressTestDemo.cs b/Demos/Demos/FountainStressTestDemo.cs index 7f33e197a..523345d59 100644 --- a/Demos/Demos/FountainStressTestDemo.cs +++ b/Demos/Demos/FountainStressTestDemo.cs @@ -258,14 +258,7 @@ public void CreateBodyDescription(Random random, in RigidPose pose, in BodyVeloc } } - description = new BodyDescription - { - Pose = pose, - LocalInertia = inertia, - Collidable = shapeIndex, - Activity = 0.1f, - Velocity = velocity - }; + description = BodyDescription.CreateDynamic(pose, velocity, inertia, shapeIndex, 0.1f); switch (random.Next(3)) { case 0: description.Collidable.Continuity = ContinuousDetection.Discrete(0, 0.2f); break; diff --git a/Demos/SpecializedTests/Media/BedsheetDemo.cs b/Demos/SpecializedTests/Media/BedsheetDemo.cs index 71fbb9343..b40010c0e 100644 --- a/Demos/SpecializedTests/Media/BedsheetDemo.cs +++ b/Demos/SpecializedTests/Media/BedsheetDemo.cs @@ -22,13 +22,7 @@ public class BedsheetDemo : Demo BodyHandle[,] CreateBodyGrid(in Vector3 position, in Quaternion orientation, int width, int height, float spacing, float bodyRadius, float massPerBody, int instanceId, CollidableProperty filters, KinematicDecider isKinematic) { - var description = new BodyDescription - { - Activity = 0.01f, - Collidable = Simulation.Shapes.Add(new Sphere(bodyRadius)), - LocalInertia = default, - Pose = orientation - }; + var description = BodyDescription.CreateKinematic(orientation, Simulation.Shapes.Add(new Sphere(bodyRadius)), 0.01f); var inverseMass = 1f / massPerBody; BodyHandle[,] handles = new BodyHandle[height, width]; for (int rowIndex = 0; rowIndex < height; ++rowIndex) From 47de46e7105b4870ebb90ad06244cda9d8eb8aef Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 5 Dec 2021 17:17:24 -0600 Subject: [PATCH 310/947] Some commentary fillins, and division. --- BepuUtilities/Vector3Wide.cs | 198 ++++++++++++++++++++++++++++++++++- 1 file changed, 195 insertions(+), 3 deletions(-) diff --git a/BepuUtilities/Vector3Wide.cs b/BepuUtilities/Vector3Wide.cs index fd975df3e..3f30d6bea 100644 --- a/BepuUtilities/Vector3Wide.cs +++ b/BepuUtilities/Vector3Wide.cs @@ -4,12 +4,28 @@ namespace BepuUtilities { + /// + /// Three dimensional vector with (with generic type argument of ) SIMD lanes. + /// public struct Vector3Wide { + /// + /// First component of the vector. + /// public Vector X; + /// + /// Second component of the vector. + /// public Vector Y; + /// + /// Third component of the vector. + /// public Vector Z; + /// + /// Creates a vector by populating each component with the given scalar. + /// + /// Scalar to copy into all lanes of the vector. [MethodImpl(MethodImplOptions.AggressiveInlining)] public Vector3Wide(ref Vector s) { @@ -17,6 +33,11 @@ public Vector3Wide(ref Vector s) Y = s; Z = s; } + + /// + /// Creates a vector by populating each component with the given scalar. + /// + /// Scalar to copy into all lanes of the vector. [MethodImpl(MethodImplOptions.AggressiveInlining)] public Vector3Wide(Vector s) { @@ -25,6 +46,12 @@ public Vector3Wide(Vector s) Z = s; } + /// + /// Performs a component wide add between two vectors. + /// + /// First vector to add. + /// Second vector to add. + /// Sum of a and b. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Add(in Vector3Wide a, in Vector3Wide b, out Vector3Wide result) { @@ -159,13 +186,24 @@ public static void Subtract(in Vector s, in Vector3Wide v, out Vector3Wid return result; } - + /// + /// Computes the inner product between two vectors. + /// + /// First vector to dot. + /// Second vector to dot. + /// Dot product of a and b. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Dot(in Vector3Wide a, in Vector3Wide b, out Vector result) { result = a.X * b.X + a.Y * b.Y + a.Z * b.Z; } + /// + /// Computes the inner product between two vectors. + /// + /// First vector to dot. + /// Second vector to dot. + /// Dot product of a and b. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Vector Dot(Vector3Wide a, Vector3Wide b) { @@ -243,6 +281,7 @@ public static void Max(in Vector s, in Vector3Wide v, out Vector3Wide res result.Y = Vector.Max(s, v.Y); result.Z = Vector.Max(s, v.Z); } + /// /// Computes the per-component maximum of two vectors. /// @@ -289,6 +328,12 @@ public static Vector3Wide Max(Vector3Wide a, Vector3Wide b) } + /// + /// Scales a vector by a scalar. + /// + /// Vector to scale. + /// Scalar to apply to the vector. + /// Scaled result vector. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Scale(in Vector3Wide vector, in Vector scalar, out Vector3Wide result) { @@ -297,6 +342,29 @@ public static void Scale(in Vector3Wide vector, in Vector scalar, out Vec result.Z = vector.Z * scalar; } + /// + /// Divides each component of the vector by the scalar. + /// + /// Vector to divide. + /// Scalar to divide the vector by. + /// Value of the vector divided by the scalar. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3Wide operator /(Vector3Wide vector, Vector scalar) + { + Vector3Wide result; + var inverse = Vector.One / scalar; + result.X = vector.X * inverse; + result.Y = vector.Y * inverse; + result.Z = vector.Z * inverse; + return result; + } + + /// + /// Scales a vector by a scalar. + /// + /// Vector to scale. + /// Scalar to apply to the vector. + /// Scaled result vector. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Vector3Wide operator *(Vector3Wide vector, Vector scalar) { @@ -306,6 +374,13 @@ public static void Scale(in Vector3Wide vector, in Vector scalar, out Vec result.Z = vector.Z * scalar; return result; } + + /// + /// Scales a vector by a scalar. + /// + /// Vector to scale. + /// Scalar to apply to the vector. + /// Scaled result vector. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Vector3Wide operator *(Vector scalar, Vector3Wide vector) { @@ -316,6 +391,11 @@ public static void Scale(in Vector3Wide vector, in Vector scalar, out Vec return result; } + /// + /// Computes the absolute value of a vector. + /// + /// Vector to take the absolute value of. + /// Absolute value of the input vector. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Abs(in Vector3Wide vector, out Vector3Wide result) { @@ -323,6 +403,12 @@ public static void Abs(in Vector3Wide vector, out Vector3Wide result) result.Y = Vector.Abs(vector.Y); result.Z = Vector.Abs(vector.Z); } + + /// + /// Computes the absolute value of a vector. + /// + /// Vector to take the absolute value of. + /// Absolute value of the input vector. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Vector3Wide Abs(Vector3Wide vector) { @@ -333,6 +419,11 @@ public static Vector3Wide Abs(Vector3Wide vector) return result; } + /// + /// Negates a vector. + /// + /// Vector to negate. + /// Negated vector. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Negate(in Vector3Wide v, out Vector3Wide result) { @@ -341,6 +432,11 @@ public static void Negate(in Vector3Wide v, out Vector3Wide result) result.Z = -v.Z; } + /// + /// Negates a vector in place and returns a reference to it. + /// + /// Vector to negate. + /// Reference to the input parameter, mutated. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static ref Vector3Wide Negate(ref Vector3Wide v) { @@ -350,6 +446,11 @@ public static ref Vector3Wide Negate(ref Vector3Wide v) return ref v; } + /// + /// Negates a vector. + /// + /// Vector to negate. + /// Negated vector. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Vector3Wide operator -(Vector3Wide v) { @@ -360,7 +461,11 @@ public static ref Vector3Wide Negate(ref Vector3Wide v) return result; } - //TODO: Look into codegen for conditional select. + /// + /// Conditionally negates lanes of the vector. + /// + /// Mask indicating which lanes should be negated. + /// Reference to the vector to be conditionally negated. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ConditionallyNegate(in Vector shouldNegate, ref Vector3Wide v) { @@ -369,6 +474,12 @@ public static void ConditionallyNegate(in Vector shouldNegate, ref Vector3W v.Z = Vector.ConditionalSelect(shouldNegate, -v.Z, v.Z); } + /// + /// Conditionally negates lanes of the vector. + /// + /// Mask indicating which lanes should be negated. + /// Vector to be conditionally negated. + /// Conditionally negated result. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ConditionallyNegate(in Vector shouldNegate, in Vector3Wide v, out Vector3Wide negated) { @@ -377,6 +488,12 @@ public static void ConditionallyNegate(in Vector shouldNegate, in Vector3Wi negated.Z = Vector.ConditionalSelect(shouldNegate, -v.Z, v.Z); } + /// + /// Conditionally negates lanes of the vector. + /// + /// Mask indicating which lanes should be negated. + /// Vector to be conditionally negated. + /// Conditionally negated vector. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Vector3Wide ConditionallyNegate(Vector shouldNegate, Vector3Wide v) { @@ -387,6 +504,12 @@ public static Vector3Wide ConditionallyNegate(Vector shouldNegate, Vector3W return negated; } + /// + /// Computes the cross product between two vectors, assuming that the vector references are not aliased. + /// + /// First vector to cross. + /// Second vector to cross. + /// Result of the cross product. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void CrossWithoutOverlap(in Vector3Wide a, in Vector3Wide b, out Vector3Wide result) { @@ -395,6 +518,13 @@ public static void CrossWithoutOverlap(in Vector3Wide a, in Vector3Wide b, out V result.Y = a.Z * b.X - a.X * b.Z; result.Z = a.X * b.Y - a.Y * b.X; } + + /// + /// Computes the cross product between two vectors. + /// + /// First vector to cross. + /// Second vector to cross. + /// Result of the cross product. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Cross(in Vector3Wide a, in Vector3Wide b, out Vector3Wide result) { @@ -402,6 +532,12 @@ public static void Cross(in Vector3Wide a, in Vector3Wide b, out Vector3Wide res result = temp; } + /// + /// Computes the cross product between two vectors. + /// + /// First vector to cross. + /// Second vector to cross. + /// Result of the cross product. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Vector3Wide Cross(Vector3Wide a, Vector3Wide b) { @@ -412,38 +548,76 @@ public static Vector3Wide Cross(Vector3Wide a, Vector3Wide b) return result; } - + /// + /// Computes the squared length of a vector. + /// + /// Vector to compute the squared length of. + /// Squared length of the input vector. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void LengthSquared(in Vector3Wide v, out Vector lengthSquared) { lengthSquared = v.X * v.X + v.Y * v.Y + v.Z * v.Z; } + + /// + /// Computes the length of a vector. + /// + /// Vector to compute the length of. + /// Length of the input vector. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Length(in Vector3Wide v, out Vector length) { length = Vector.SquareRoot(v.X * v.X + v.Y * v.Y + v.Z * v.Z); } + + /// + /// Computes the squared length of a vector. + /// + /// Vector to compute the squared length of. + /// Squared length of the input vector. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Vector LengthSquared(Vector3Wide v) { return v.X * v.X + v.Y * v.Y + v.Z * v.Z; } + + /// + /// Computes the length of a vector. + /// + /// Vector to compute the length of. + /// Length of the input vector. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Vector Length(Vector3Wide v) { return Vector.SquareRoot(v.X * v.X + v.Y * v.Y + v.Z * v.Z); } + + /// + /// Computes the squared length of the vector. + /// + /// Squared length of this vector. [MethodImpl(MethodImplOptions.AggressiveInlining)] public Vector LengthSquared() { return X * X + Y * Y + Z * Z; } + + /// + /// Computes the length of the vector. + /// + /// Length of this vector. [MethodImpl(MethodImplOptions.AggressiveInlining)] public Vector Length() { return Vector.SquareRoot(X * X + Y * Y + Z * Z); } + /// + /// Computes the distance between two vectors. + /// + /// First vector in the pair. + /// Second vector in the pair. + /// Distance between a and b. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Distance(in Vector3Wide a, in Vector3Wide b, out Vector distance) { @@ -453,6 +627,12 @@ public static void Distance(in Vector3Wide a, in Vector3Wide b, out Vector + /// Computes the squared distance between two vectors. + /// + /// First vector in the pair. + /// Second vector in the pair. + /// Squared distance between a and b. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void DistanceSquared(in Vector3Wide a, in Vector3Wide b, out Vector distanceSquared) { @@ -462,6 +642,12 @@ public static void DistanceSquared(in Vector3Wide a, in Vector3Wide b, out Vecto distanceSquared = x * x + y * y + z * z; } + /// + /// Computes the distance between two vectors. + /// + /// First vector in the pair. + /// Second vector in the pair. + /// Distance between a and b. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Vector Distance(Vector3Wide a, Vector3Wide b) { @@ -471,6 +657,12 @@ public static Vector Distance(Vector3Wide a, Vector3Wide b) return Vector.SquareRoot(x * x + y * y + z * z); } + /// + /// Computes the squared distance between two vectors. + /// + /// First vector in the pair. + /// Second vector in the pair. + /// Squared distance between a and b. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Vector DistanceSquared(Vector3Wide a, Vector3Wide b) { From f0e26ff141a6d326e1f6f0f2a67a64f7897b85de Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 5 Dec 2021 19:03:45 -0600 Subject: [PATCH 311/947] Bit more documenting. --- BepuUtilities/Vector3Wide.cs | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/BepuUtilities/Vector3Wide.cs b/BepuUtilities/Vector3Wide.cs index 3f30d6bea..8f8931ae7 100644 --- a/BepuUtilities/Vector3Wide.cs +++ b/BepuUtilities/Vector3Wide.cs @@ -673,6 +673,12 @@ public static Vector DistanceSquared(Vector3Wide a, Vector3Wide b) } //TODO: We have better intrinsics options here for a fast rsqrt path. + + /// + /// Computes a unit length vector pointing in the same direction as the input. + /// + /// Vector to normalize. + /// Vector pointing in the same direction as the input, but with unit length. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Normalize(in Vector3Wide v, out Vector3Wide result) { @@ -681,6 +687,11 @@ public static void Normalize(in Vector3Wide v, out Vector3Wide result) Scale(v, scale, out result); } + /// + /// Computes a unit length vector pointing in the same direction as the input. + /// + /// Vector to normalize. + /// Vector pointing in the same direction as the input, but with unit length. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Vector3Wide Normalize(Vector3Wide v) { @@ -689,6 +700,13 @@ public static Vector3Wide Normalize(Vector3Wide v) return v * scale; } + /// + /// Selects the left or right input for each lane depending on a mask. + /// + /// Mask to use to decide between the left and right value for each lane.. + /// Value to choose if the condition mask is set. + /// Value to choose if the condition mask is unset. + /// Blended result. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ConditionalSelect(in Vector condition, in Vector3Wide left, in Vector3Wide right, out Vector3Wide result) { @@ -697,6 +715,13 @@ public static void ConditionalSelect(in Vector condition, in Vector3Wide le result.Z = Vector.ConditionalSelect(condition, left.Z, right.Z); } + /// + /// Selects the left or right input for each lane depending on a mask. + /// + /// Mask to use to decide between the left and right value for each lane.. + /// Value to choose if the condition mask is set. + /// Value to choose if the condition mask is unset. + /// Blended result. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Vector3Wide ConditionalSelect(Vector condition, Vector3Wide left, Vector3Wide right) { From 367ec97c68e5415b8b00a9f59ee1047b1b8e5525 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 5 Dec 2021 19:56:51 -0600 Subject: [PATCH 312/947] PrepareForIntegration callback now actually called when it should be. Cleaned out old IntegrateVelocity implementations. --- BepuPhysics/PoseIntegrator.cs | 15 +++-- BepuPhysics/Solver_SubsteppingSolve2.cs | 4 ++ Demos/DemoCallbacks.cs | 59 +++++++++---------- Demos/Demos/CompoundTestDemo.cs | 2 +- Demos/Demos/PlanetDemo.cs | 15 ++--- Demos/Demos/SimpleSelfContainedDemo.cs | 38 ++++++------ .../ConstrainedKinematicIntegrationTest.cs | 2 +- Demos/SpecializedTests/GyroscopeTestDemo.cs | 6 +- 8 files changed, 71 insertions(+), 70 deletions(-) diff --git a/BepuPhysics/PoseIntegrator.cs b/BepuPhysics/PoseIntegrator.cs index 30d57176d..e53321dca 100644 --- a/BepuPhysics/PoseIntegrator.cs +++ b/BepuPhysics/PoseIntegrator.cs @@ -69,12 +69,15 @@ public interface IPoseIntegratorCallbacks void Initialize(Simulation simulation); /// - /// Called prior to integrating the simulation's active bodies. When used with a substepping timestepper, this could be called multiple times per frame with different time step values. + /// Callback invoked ahead of dispatches that may call into . + /// It may be called more than once with different values over a frame. For example, when performing bounding box prediction, velocity is integrated with a full frame time step duration. + /// During substepped solves, integration is split into substepCount steps, each with fullFrameDuration / substepCount duration. + /// The final integration pass for unconstrained bodies may be either fullFrameDuration or fullFrameDuration / substepCount, depending on the value of AllowSubstepsForUnconstrainedBodies. /// - /// Current time step duration. + /// Current integration time step duration. + /// This is typically used for precomputing anything expensive that will be used across velocity integration. void PrepareForIntegration(float dt); - /// /// Callback for a bundle of bodies being integrated. /// @@ -697,6 +700,10 @@ private void IntegrateAfterSubsteppingWorker(int workerIndex) public void IntegrateAfterSubstepping(IndexSet constrainedBodies, float dt, int substepCount, IThreadDispatcher threadDispatcher) { + //The only bodies undergoing *velocity* integration during the post-integration step are unconstrained. + var substepDt = dt / substepCount; + var velocityIntegrationTimestep = Callbacks.AllowSubstepsForUnconstrainedBodies ? substepDt : dt; + Callbacks.PrepareForIntegration(velocityIntegrationTimestep); if (threadDispatcher != null && threadDispatcher.ThreadCount > 1) { PrepareForMultithreadedExecution(BundleIndexing.GetBundleCount(bodies.ActiveSet.Count), dt, threadDispatcher.ThreadCount, substepCount); @@ -708,7 +715,7 @@ public void IntegrateAfterSubstepping(IndexSet constrainedBodies, float dt, int } else { - IntegrateBundlesAfterSubstepping(ref constrainedBodies, 0, BundleIndexing.GetBundleCount(bodies.ActiveSet.Count), dt, dt / substepCount, substepCount, 0); + IntegrateBundlesAfterSubstepping(ref constrainedBodies, 0, BundleIndexing.GetBundleCount(bodies.ActiveSet.Count), dt, substepDt, substepCount, 0); } } } diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs index 0b8e70a6d..d57f533a1 100644 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ b/BepuPhysics/Solver_SubsteppingSolve2.cs @@ -111,6 +111,9 @@ public Solver(Bodies bodies, BufferPool pool, int iterationCount, int fallbackBa constraintIntegrationResponsibilitiesWorker = ConstraintIntegrationResponsibilitiesWorker; } + /// + /// Pose integrator used by the simulation. + /// public PoseIntegrator PoseIntegrator { get; private set; } @@ -1213,6 +1216,7 @@ public override void DisposeConstraintIntegrationResponsibilities() public override void SolveStep2(float totalDt, IThreadDispatcher threadDispatcher = null) { var substepDt = totalDt / substepCount; + PoseIntegrator.Callbacks.PrepareForIntegration(substepDt); if (threadDispatcher == null) { var inverseDt = 1f / substepDt; diff --git a/Demos/DemoCallbacks.cs b/Demos/DemoCallbacks.cs index ade975f92..5c575e483 100644 --- a/Demos/DemoCallbacks.cs +++ b/Demos/DemoCallbacks.cs @@ -25,9 +25,6 @@ public struct DemoPoseIntegratorCallbacks : IPoseIntegratorCallbacks /// public float AngularDamping; - Vector3 gravityDt; - float linearDampingDt; - float angularDampingDt; /// /// Gets how the pose integrator should handle angular velocity integration. @@ -68,43 +65,45 @@ public DemoPoseIntegratorCallbacks(Vector3 gravity, float linearDamping = .03f, AngularDamping = angularDamping; } + Vector3Wide gravityWideDt; + Vector linearDampingDt; + Vector angularDampingDt; + + /// + /// Callback invoked ahead of dispatches that may call into . + /// It may be called more than once with different values over a frame. For example, when performing bounding box prediction, velocity is integrated with a full frame time step duration. + /// During substepped solves, integration is split into substepCount steps, each with fullFrameDuration / substepCount duration. + /// The final integration pass for unconstrained bodies may be either fullFrameDuration or fullFrameDuration / substepCount, depending on the value of AllowSubstepsForUnconstrainedBodies. + /// + /// Current integration time step duration. + /// This is typically used for precomputing anything expensive that will be used across velocity integration. public void PrepareForIntegration(float dt) { //No reason to recalculate gravity * dt for every body; just cache it ahead of time. - gravityDt = Gravity * dt; //Since these callbacks don't use per-body damping values, we can precalculate everything. - linearDampingDt = MathF.Pow(MathHelper.Clamp(1 - LinearDamping, 0, 1), dt); - angularDampingDt = MathF.Pow(MathHelper.Clamp(1 - AngularDamping, 0, 1), dt); - gravityWide = Vector3Wide.Broadcast(Gravity); - } - Vector3Wide gravityWide; - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IntegrateVelocity(int bodyIndex, in RigidPose pose, in BodyInertia localInertia, int workerIndex, ref BodyVelocity velocity) - { - //Note that we avoid accelerating kinematics. Kinematics are any body with an inverse mass of zero (so a mass of ~infinity). No force can move them. - if (localInertia.InverseMass > 0) - { - velocity.Linear = (velocity.Linear + gravityDt) * linearDampingDt; - velocity.Angular = velocity.Angular * angularDampingDt; - } - //Implementation sidenote: Why aren't kinematics all bundled together separately from dynamics to avoid this per-body condition? - //Because kinematics can have a velocity- that is what distinguishes them from a static object. The solver must read velocities of all bodies involved in a constraint. - //Under ideal conditions, those bodies will be near in memory to increase the chances of a cache hit. If kinematics are separately bundled, the the number of cache - //misses necessarily increases. Slowing down the solver in order to speed up the pose integrator is a really, really bad trade, especially when the benefit is a few ALU ops. - - //Note that you CAN technically modify the pose in IntegrateVelocity by directly accessing it through the Simulation.Bodies.ActiveSet.Poses, it just requires a little care and isn't directly exposed. - //If the PositionFirstTimestepper is being used, then the pose integrator has already integrated the pose. - //If the PositionLastTimestepper or SubsteppingTimestepper are in use, the pose has not yet been integrated. - //If your pose modification depends on the order of integration, you'll want to take this into account. - - //This is also a handy spot to implement things like position dependent gravity or per-body damping. + linearDampingDt = new Vector(MathF.Pow(MathHelper.Clamp(1 - LinearDamping, 0, 1), dt)); + angularDampingDt = new Vector(MathF.Pow(MathHelper.Clamp(1 - AngularDamping, 0, 1), dt)); + gravityWideDt = Vector3Wide.Broadcast(Gravity * dt); } + /// + /// Callback for a bundle of bodies being integrated. + /// + /// Indices of the bodies being integrated in this bundle. + /// Current body positions. + /// Current body orientations. + /// Body's current local inertia. + /// Mask indicating which lanes are active in the bundle. Active lanes will contain 0xFFFFFFFF, inactive lanes will contain 0. + /// Index of the worker thread processing this bundle. + /// Durations to integrate the velocity over. Can vary over lanes. + /// Velocity of bodies in the bundle. Any changes to lanes which are not active by the integrationMask will be discarded. [MethodImpl(MethodImplOptions.AggressiveInlining)] public void IntegrateVelocity(in Vector bodyIndices, in Vector3Wide position, in QuaternionWide orientation, in BodyInertiaWide localInertia, in Vector integrationMask, int workerIndex, in Vector dt, ref BodyVelocityWide velocity) { + //This is also a handy spot to implement things like position dependent gravity or per-body damping. Here, //Note that we don't have to check for kinematics; IntegrateVelocityForKinematics returns false, so we'll never see them in this callback. - velocity.Linear += gravityWide * dt; + velocity.Linear = (velocity.Linear + gravityWideDt) * linearDampingDt; + velocity.Angular = velocity.Angular * angularDampingDt; } } public unsafe struct DemoNarrowPhaseCallbacks : INarrowPhaseCallbacks diff --git a/Demos/Demos/CompoundTestDemo.cs b/Demos/Demos/CompoundTestDemo.cs index c284141ef..a1e249ace 100644 --- a/Demos/Demos/CompoundTestDemo.cs +++ b/Demos/Demos/CompoundTestDemo.cs @@ -19,7 +19,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //The PositionFirstTimestepper is the simplest timestepping mode, but since it integrates velocity into position at the start of the frame, directly modified velocities outside of the timestep //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10f, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10f, 0)), new EmbeddedSubsteppingTimestepper2(8), 1); using (var compoundBuilder = new CompoundBuilder(BufferPool, Simulation.Shapes, 8)) { diff --git a/Demos/Demos/PlanetDemo.cs b/Demos/Demos/PlanetDemo.cs index 022cf895a..7f4b2e3b4 100644 --- a/Demos/Demos/PlanetDemo.cs +++ b/Demos/Demos/PlanetDemo.cs @@ -25,7 +25,6 @@ struct PlanetaryGravityCallbacks : IPoseIntegratorCallbacks { public Vector3 PlanetCenter; public float Gravity; - float gravityDt; public readonly AngularIntegrationMode AngularIntegrationMode => AngularIntegrationMode.Nonconserving; @@ -37,25 +36,19 @@ public void Initialize(Simulation simulation) { } + float gravityDt; public void PrepareForIntegration(float dt) { //No point in repeating this for every body; cache it. gravityDt = dt * Gravity; } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IntegrateVelocity(int bodyIndex, in RigidPose pose, in BodyInertia localInertia, int workerIndex, ref BodyVelocity velocity) - { - if (localInertia.InverseMass > 0) //Ignore kinematics. - { - var offset = pose.Position - PlanetCenter; - var distance = offset.Length(); - velocity.Linear -= gravityDt * offset / MathF.Max(1f, distance * distance * distance); - } - } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void IntegrateVelocity(in Vector bodyIndices, in Vector3Wide position, in QuaternionWide orientation, in BodyInertiaWide localInertia, in Vector integrationMask, int workerIndex, in Vector dt, ref BodyVelocityWide velocity) { + var offset = position - Vector3Wide.Broadcast(PlanetCenter); + var distance = offset.Length(); + velocity.Linear -= new Vector(gravityDt) * offset / Vector.Max(Vector.One, distance * distance * distance); } } diff --git a/Demos/Demos/SimpleSelfContainedDemo.cs b/Demos/Demos/SimpleSelfContainedDemo.cs index b41bbef94..cc44e2128 100644 --- a/Demos/Demos/SimpleSelfContainedDemo.cs +++ b/Demos/Demos/SimpleSelfContainedDemo.cs @@ -130,7 +130,6 @@ public void Dispose() public struct PoseIntegratorCallbacks : IPoseIntegratorCallbacks { public Vector3 Gravity; - Vector3 gravityDt; /// /// Performs any required initialization logic after the Simulation instance has been constructed. @@ -167,36 +166,39 @@ public PoseIntegratorCallbacks(Vector3 gravity) : this() Gravity = gravity; } + //Note that velocity integration uses "wide" types. These are array-of-struct-of-arrays types that use SIMD accelerated types underneath. + //Rather than handling a single body at a time, the callback handles up to Vector.Count bodies simultaneously. + Vector3Wide gravityWideDt; + /// - /// Called prior to integrating the simulation's active bodies. When used with a substepping timestepper, this could be called multiple times per frame with different time step values. + /// Callback invoked ahead of dispatches that may call into . + /// It may be called more than once with different values over a frame. For example, when performing bounding box prediction, velocity is integrated with a full frame time step duration. + /// During substepped solves, integration is split into substepCount steps, each with fullFrameDuration / substepCount duration. + /// The final integration pass for unconstrained bodies may be either fullFrameDuration or fullFrameDuration / substepCount, depending on the value of AllowSubstepsForUnconstrainedBodies. /// - /// Current time step duration. + /// Current integration time step duration. + /// This is typically used for precomputing anything expensive that will be used across velocity integration. public void PrepareForIntegration(float dt) { //No reason to recalculate gravity * dt for every body; just cache it ahead of time. - gravityDt = Gravity * dt; + gravityWideDt = Vector3Wide.Broadcast(Gravity * dt); } /// - /// Callback called for each active body within the simulation during body integration. + /// Callback for a bundle of bodies being integrated. /// - /// Index of the body being visited. - /// Body's current pose. + /// Indices of the bodies being integrated in this bundle. + /// Current body positions. + /// Current body orientations. /// Body's current local inertia. - /// Index of the worker thread processing this body. - /// Reference to the body's current velocity to integrate. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IntegrateVelocity(int bodyIndex, in RigidPose pose, in BodyInertia localInertia, int workerIndex, ref BodyVelocity velocity) - { - //Note that we avoid accelerating kinematics. Kinematics are any body with an inverse mass of zero (so a mass of ~infinity). No force can move them. - if (localInertia.InverseMass > 0) - { - velocity.Linear = velocity.Linear + gravityDt; - } - } + /// Mask indicating which lanes are active in the bundle. Active lanes will contain 0xFFFFFFFF, inactive lanes will contain 0. + /// Index of the worker thread processing this bundle. + /// Durations to integrate the velocity over. Can vary over lanes. + /// Velocity of bodies in the bundle. Any changes to lanes which are not active by the integrationMask will be discarded. [MethodImpl(MethodImplOptions.AggressiveInlining)] public void IntegrateVelocity(in Vector bodyIndices, in Vector3Wide position, in QuaternionWide orientation, in BodyInertiaWide localInertia, in Vector integrationMask, int workerIndex, in Vector dt, ref BodyVelocityWide velocity) { + velocity.Linear += gravityWideDt; } } diff --git a/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs b/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs index 1046e3079..49500b430 100644 --- a/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs +++ b/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs @@ -19,7 +19,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = 0; Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(30, 1), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 2f }, - new DemoPoseIntegratorCallbacks(new Vector3(0, -0.1f, 0)), new EmbeddedSubsteppingTimestepper2(3), 1); + new DemoPoseIntegratorCallbacks(new Vector3(0, -0.1f, 0), 0, 0), new EmbeddedSubsteppingTimestepper2(3), 1); var shapeA = new Box(.75f, 1, .5f); var shapeIndexA = Simulation.Shapes.Add(shapeA); diff --git a/Demos/SpecializedTests/GyroscopeTestDemo.cs b/Demos/SpecializedTests/GyroscopeTestDemo.cs index 269314b35..72c4cc665 100644 --- a/Demos/SpecializedTests/GyroscopeTestDemo.cs +++ b/Demos/SpecializedTests/GyroscopeTestDemo.cs @@ -40,14 +40,10 @@ public void PrepareForIntegration(float dt) innerCallbacks.PrepareForIntegration(dt); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IntegrateVelocity(int bodyIndex, in RigidPose pose, in BodyInertia localInertia, int workerIndex, ref BodyVelocity velocity) - { - innerCallbacks.IntegrateVelocity(bodyIndex, pose, localInertia, workerIndex, ref velocity); - } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void IntegrateVelocity(in Vector bodyIndices, in Vector3Wide position, in QuaternionWide orientation, in BodyInertiaWide localInertia, in Vector integrationMask, int workerIndex, in Vector dt, ref BodyVelocityWide velocity) { + innerCallbacks.IntegrateVelocity(bodyIndices, position, orientation, localInertia, integrationMask, workerIndex, dt, ref velocity); } } From 1635ded7615c25dec17f2abc3b588aeda83f3655 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 6 Dec 2021 15:26:25 -0600 Subject: [PATCH 313/947] Does not build. In the middle of purging old solver API. --- BepuPhysics/ConstraintLocation.cs | 30 + BepuPhysics/ConstraintReference.cs | 40 + .../EmbeddedSubsteppingTimestepper2.cs | 117 -- BepuPhysics/ITimestepper.cs | 1 + BepuPhysics/LocalSpinWait.cs | 49 + BepuPhysics/Simulation.cs | 23 +- BepuPhysics/Solver.cs | 114 +- .../Solver_IncrementalContactUpdate.cs | 83 - BepuPhysics/Solver_Solve.cs | 1716 ++++++++++++----- BepuPhysics/Solver_SubsteppingSolve2.cs | 1284 ------------ BepuPhysics/SubsteppingTimestepper.cs | 49 + .../ConstraintDescriptionMappingTests.cs | 2 +- Demos/Demos/BlockChainDemo.cs | 2 +- Demos/Demos/BouncinessDemo.cs | 2 +- Demos/Demos/Cars/CarDemo.cs | 2 +- Demos/Demos/Characters/CharacterDemo.cs | 2 +- Demos/Demos/ClothDemo.cs | 2 +- Demos/Demos/CollisionQueryDemo.cs | 2 +- Demos/Demos/ColosseumDemo.cs | 2 +- Demos/Demos/CompoundTestDemo.cs | 2 +- Demos/Demos/ContactEventsDemo.cs | 2 +- .../Demos/ContinuousCollisionDetectionDemo.cs | 2 +- Demos/Demos/CustomVoxelCollidableDemo.cs | 2 +- Demos/Demos/FountainStressTestDemo.cs | 2 +- Demos/Demos/NewtDemo.cs | 2 +- Demos/Demos/PlanetDemo.cs | 2 +- Demos/Demos/PyramidDemo.cs | 2 +- Demos/Demos/RagdollDemo.cs | 2 +- Demos/Demos/RayCastingDemo.cs | 2 +- Demos/Demos/RopeStabilityDemo.cs | 2 +- Demos/Demos/RopeTwistDemo.cs | 2 +- Demos/Demos/SimpleSelfContainedDemo.cs | 2 +- Demos/Demos/SolverContactEnumerationDemo.cs | 2 +- Demos/Demos/Sponsors/SponsorDemo.cs | 2 +- Demos/Demos/SubsteppingDemo.cs | 20 +- Demos/Demos/SweepDemo.cs | 2 +- Demos/Demos/Tanks/TankDemo.cs | 2 +- .../BroadPhaseStressTestDemo.cs | 2 +- Demos/SpecializedTests/CapsuleTestDemo.cs | 2 +- Demos/SpecializedTests/CharacterTestDemo.cs | 2 +- Demos/SpecializedTests/ClothLatticeDemo.cs | 2 +- Demos/SpecializedTests/CompoundBoundTests.cs | 2 +- .../CompoundCollisionIndicesTest.cs | 2 +- .../ConstrainedKinematicIntegrationTest.cs | 2 +- Demos/SpecializedTests/ConstraintTestDemo.cs | 2 +- Demos/SpecializedTests/ConvexHullTestDemo.cs | 2 +- Demos/SpecializedTests/CylinderTestDemo.cs | 2 +- Demos/SpecializedTests/GyroscopeTestDemo.cs | 2 +- Demos/SpecializedTests/Media/BedsheetDemo.cs | 2 +- .../Media/ColosseumVideoDemo.cs | 2 +- .../Media/NewtDemandingSacrificeVideoDemo.cs | 4 +- Demos/SpecializedTests/Media/NewtVideoDemo.cs | 2 +- .../Media/PyramidVideoDemo.cs | 2 +- .../Media/ShrinkwrappedNewtsVideoDemo.cs | 2 +- Demos/SpecializedTests/MeshMeshTestDemo.cs | 2 +- .../SpecializedTests/MeshReductionTestDemo.cs | 2 +- .../MeshSerializationTestDemo.cs | 2 +- Demos/SpecializedTests/MeshTestDemo.cs | 2 +- .../PyramidAwakenerTestDemo.cs | 2 +- Demos/SpecializedTests/RagdollTubeDemo.cs | 2 +- Demos/SpecializedTests/ShapePileTestDemo.cs | 2 +- Demos/SpecializedTests/SolverBatchTestDemo.cs | 4 +- Demos/SpecializedTests/TriangleRayTestDemo.cs | 2 +- Demos/SpecializedTests/TriangleTestDemo.cs | 2 +- Demos/SpecializedTests/VolumeQueryTests.cs | 2 +- 65 files changed, 1487 insertions(+), 2149 deletions(-) create mode 100644 BepuPhysics/ConstraintLocation.cs create mode 100644 BepuPhysics/ConstraintReference.cs delete mode 100644 BepuPhysics/EmbeddedSubsteppingTimestepper2.cs create mode 100644 BepuPhysics/LocalSpinWait.cs delete mode 100644 BepuPhysics/Solver_IncrementalContactUpdate.cs delete mode 100644 BepuPhysics/Solver_SubsteppingSolve2.cs create mode 100644 BepuPhysics/SubsteppingTimestepper.cs diff --git a/BepuPhysics/ConstraintLocation.cs b/BepuPhysics/ConstraintLocation.cs new file mode 100644 index 000000000..c21fd5bf5 --- /dev/null +++ b/BepuPhysics/ConstraintLocation.cs @@ -0,0 +1,30 @@ +namespace BepuPhysics +{ + /// + /// Location in memory where a constraint is stored. + /// + public struct ConstraintLocation + { + //Note that the type id is included, even though we can extract it from a type parameter. + //This is required for body memory swap induced reference changes- it is not efficient to include type metadata in the per-body connections, + //so instead we keep a type id cached. + //(You could pack these a bit- it's pretty reasonable to say you can't have more than 2^24 constraints of a given type and 2^8 constraint types... + //It's just not that valuable, unless proven otherwise.) + /// + /// Index of the constraint set that owns the constraint. If zero, the constraint is attached to bodies that are awake. + /// + public int SetIndex; + /// + /// Index of the constraint batch the constraint belongs to. + /// + public int BatchIndex; + /// + /// Type id of the constraint. Used to look up the type batch index in a constraint batch's type id to type batch index table. + /// + public int TypeId; + /// + /// Index of the constraint in a type batch. + /// + public int IndexInTypeBatch; + } +} diff --git a/BepuPhysics/ConstraintReference.cs b/BepuPhysics/ConstraintReference.cs new file mode 100644 index 000000000..1feb88ddd --- /dev/null +++ b/BepuPhysics/ConstraintReference.cs @@ -0,0 +1,40 @@ +using BepuPhysics.Constraints; +using System.Runtime.CompilerServices; + +namespace BepuPhysics +{ + /// + /// Reference to a constraint's memory location in the solver. + /// + public unsafe struct ConstraintReference + { + internal TypeBatch* typeBatchPointer; + /// + /// Gets a reference to the type batch holding the constraint. + /// + public ref TypeBatch TypeBatch + { + [MethodImpl(MethodImplOptions.AggressiveInlining)] + get + { + return ref *typeBatchPointer; + } + } + /// + /// Index in the type batch where the constraint is allocated. + /// + public readonly int IndexInTypeBatch; + + /// + /// Creates a new constraint reference from a constraint memory location. + /// + /// Pointer to the type batch where the constraint lives. + /// Index in the type batch where the constraint is allocated. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public ConstraintReference(TypeBatch* typeBatchPointer, int indexInTypeBatch) + { + this.typeBatchPointer = typeBatchPointer; + IndexInTypeBatch = indexInTypeBatch; + } + } +} diff --git a/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs b/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs deleted file mode 100644 index 0f7c6bf3c..000000000 --- a/BepuPhysics/EmbeddedSubsteppingTimestepper2.cs +++ /dev/null @@ -1,117 +0,0 @@ -using System; -using System.Collections.Generic; -using System.Diagnostics; -using System.Text; -using BepuUtilities; - -namespace BepuPhysics -{ - /// - /// Updates the simulation in the order of: sleeper -> predict body bounding boxes -> collision detection -> LOOP { contact data update (if on iteration > 0) -> integrate body velocities -> solver -> integrate body poses } -> data structure optimization. - /// Each inner loop execution simulates a sub-timestep of length dt/substepCount. - /// Useful for simulations with difficult to solve constraint systems that need shorter timestep durations but which don't require high frequency collision detection. - /// - public class EmbeddedSubsteppingTimestepper2 : ITimestepper - { - /// - /// Gets or sets the number of substeps to execute during each timestep. - /// - public int SubstepCount { get; set; } - - /// - /// Fires after the sleeper completes and before bodies are integrated. - /// - public event TimestepperStageHandler Slept; - /// - /// Fires after bodies have their bounding boxes updated for the frame's predicted motion and before collision detection. - /// - public event TimestepperStageHandler BeforeCollisionDetection; - /// - /// Fires after all collisions have been identified, but before the substep loop begins. - /// - public event TimestepperStageHandler CollisionsDetected; - /// - /// Fires after the solver executes and before the final integration step. - /// - public event TimestepperStageHandler ConstraintsSolved; - /// - /// Fires after all substeps are finished executing and before data structures are incrementally optimized. - /// - public event TimestepperStageHandler SubstepsComplete; - - public EmbeddedSubsteppingTimestepper2(int substepCount) - { - SubstepCount = substepCount; - } - - public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDispatcher = null) - { - //simulation.Bodies.ValidateAwakeMotionStatesByHash(HashDiagnosticType.AwakeBodyStates0); - //simulation.Bodies.ValidateAwakeCollidablesByHash(HashDiagnosticType.AwakeBodyCollidableStates0); - simulation.Solver.ValidateTrailingTypeBatchBodyReferences(); - simulation.Solver.ValidateFallbackBatchEmptySlotReferences(); - simulation.Solver.ValidateFallbackBatchAccessSafety(); - simulation.Solver.ValidateAccumulatedImpulses(); - simulation.Solver.ValidateConstraintMaps(); - simulation.Solver.ValidateConstraintReferenceKinematicity(); - simulation.Solver.ValidateConstrainedKinematicsSet(); - simulation.Solver.ValidateFallbackBodiesAreDynamic(); - //simulation.Solver.ValidateExistingHandles(); - simulation.Sleep(threadDispatcher); - Slept?.Invoke(dt, threadDispatcher); - simulation.Solver.ValidateTrailingTypeBatchBodyReferences(); - simulation.Solver.ValidateFallbackBatchEmptySlotReferences(); - simulation.Solver.ValidateFallbackBatchAccessSafety(); - simulation.Solver.ValidateAccumulatedImpulses(); - simulation.Solver.ValidateConstraintMaps(); - simulation.Solver.ValidateConstraintReferenceKinematicity(); - simulation.Solver.ValidateConstrainedKinematicsSet(); - simulation.Solver.ValidateFallbackBodiesAreDynamic(); - //simulation.Solver.ValidateExistingHandles(); - //simulation.Bodies.ValidateAwakeMotionStatesByHash(HashDiagnosticType.AwakeBodyStates1); - //simulation.Bodies.ValidateAwakeCollidablesByHash(HashDiagnosticType.AwakeBodyCollidableStates1); - - simulation.PredictBoundingBoxes(dt, threadDispatcher); - BeforeCollisionDetection?.Invoke(dt, threadDispatcher); - - //simulation.Bodies.ValidateAwakeMotionStatesByHash(HashDiagnosticType.AwakeBodyStates2); - //simulation.Bodies.ValidateAwakeCollidablesByHash(HashDiagnosticType.AwakeBodyCollidableStates2); - simulation.CollisionDetection(dt, threadDispatcher); - CollisionsDetected?.Invoke(dt, threadDispatcher); - Debug.Assert(SubstepCount >= 0, "Substep count should be positive."); - - //simulation.Bodies.ValidateAwakeMotionStatesByHash(HashDiagnosticType.AwakeBodyStates3); - //simulation.Bodies.ValidateAwakeCollidablesByHash(HashDiagnosticType.AwakeBodyCollidableStates3); - simulation.Solver.ValidateTrailingTypeBatchBodyReferences(); - simulation.Solver.ValidateFallbackBatchEmptySlotReferences(); - simulation.Solver.ValidateFallbackBatchAccessSafety(); - simulation.Solver.ValidateAccumulatedImpulses(); - simulation.Solver.ValidateConstraintMaps(); - simulation.Solver.ValidateConstraintReferenceKinematicity(); - simulation.Solver.ValidateConstrainedKinematicsSet(); - simulation.Solver.ValidateFallbackBodiesAreDynamic(); - //simulation.Solver.ValidateExistingHandles(); - - var constrainedBodySet = simulation.Solver.PrepareConstraintIntegrationResponsibilities(SubstepCount, threadDispatcher); - simulation.Profiler.Start(simulation.Solver); - simulation.Solver.SolveStep2(dt, threadDispatcher); - simulation.Profiler.End(simulation.Solver); - //simulation.Bodies.ValidateAwakeMotionStatesByHash(HashDiagnosticType.AwakeBodyStates4); - //simulation.Bodies.ValidateAwakeCollidablesByHash(HashDiagnosticType.AwakeBodyCollidableStates4); - ConstraintsSolved?.Invoke(dt, threadDispatcher); - simulation.Profiler.Start(simulation.PoseIntegrator); - simulation.PoseIntegrator.IntegrateAfterSubstepping(constrainedBodySet, dt, SubstepCount, threadDispatcher); - simulation.Profiler.End(simulation.PoseIntegrator); - //simulation.Bodies.ValidateAwakeMotionStatesByHash(HashDiagnosticType.AwakeBodyStates5); - //simulation.Bodies.ValidateAwakeCollidablesByHash(HashDiagnosticType.AwakeBodyCollidableStates5); - simulation.Solver.DisposeConstraintIntegrationResponsibilities(); - SubstepsComplete?.Invoke(dt, threadDispatcher); - - - simulation.Solver.ValidateAccumulatedImpulses(); - simulation.IncrementallyOptimizeDataStructures(threadDispatcher); - //simulation.Bodies.ValidateAwakeMotionStatesByHash(HashDiagnosticType.AwakeBodyStates6); - //simulation.Bodies.ValidateAwakeCollidablesByHash(HashDiagnosticType.AwakeBodyCollidableStates6); - } - } -} diff --git a/BepuPhysics/ITimestepper.cs b/BepuPhysics/ITimestepper.cs index 68bfe375b..b27c3e184 100644 --- a/BepuPhysics/ITimestepper.cs +++ b/BepuPhysics/ITimestepper.cs @@ -38,6 +38,7 @@ public interface ITimestepper /// /// Performs one timestep of the given length. /// + /// Simulation to be stepped forward in time. /// Duration of the time step. /// Thread dispatcher to use for execution, if any. void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDispatcher = null); diff --git a/BepuPhysics/LocalSpinWait.cs b/BepuPhysics/LocalSpinWait.cs new file mode 100644 index 000000000..00dfc116f --- /dev/null +++ b/BepuPhysics/LocalSpinWait.cs @@ -0,0 +1,49 @@ +using System.Runtime.CompilerServices; +using System.Threading; + +namespace BepuPhysics +{ + /// + /// Behaves like a framework SpinWait, but never voluntarily relinquishes the timeslice to off-core threads. + /// + /// There are two big reasons for using this over the regular framework SpinWait: + /// 1) The framework spinwait relies on spins for quite a while before resorting to any form of timeslice surrender. + /// Empirically, this is not ideal for the solver- if the sync condition isn't met within several nanoseconds, it will tend to be some microseconds away. + /// This spinwait is much more aggressive about moving to yields. + /// 2) After a number of yields, the framework SpinWait will resort to calling Sleep. + /// This widens the potential set of schedulable threads to those not native to the current core. If we permit that transition, it is likely to evict cached solver data. + /// (For very large simulations, the use of Sleep(0) isn't that concerning- every iteration can be large enough to evict all of cache- + /// but there still isn't much benefit to using it over yields in context.) + /// SpinWait will also fall back to Sleep(1) by default which obliterates performance, but that behavior can be disabled. + /// Note that this isn't an indication that the framework SpinWait should be changed, but rather that the solver's requirements are extremely specific and don't match + /// a general purpose solution very well. + internal struct LocalSpinWait + { + public int WaitCount; + + //Empirically, being pretty aggressive about yielding produces the best results. This is pretty reasonable- + //a single constraint bundle can take hundreds of nanoseconds to finish. + //That would be a whole lot of spinning that could be used by some other thread. At worst, we're being friendlier to other applications on the system. + //This thread will likely be rescheduled on the same core, so it's unlikely that we'll lose any cache warmth (that we wouldn't have lost anyway). + public const int YieldThreshold = 3; + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void SpinOnce() + { + if (WaitCount >= YieldThreshold) + { + Thread.Yield(); + } + else + { + //We are sacrificing one important feature of the newer framework provided waits- normalized spinning (RuntimeThread.OptimalMaxSpinWaitsPerSpinIteration). + //Different platforms can spin at significantly different speeds, so a single constant value for the maximum spin duration doesn't map well to all hardware. + //On the upside, we tend to be concerned about two modes- waiting a very short time, and waiting a medium amount of time. + //The specific length of the 'short' time doesn't matter too much, so long as it's fairly short. + Thread.SpinWait(1 << WaitCount); + ++WaitCount; + } + + } + } +} diff --git a/BepuPhysics/Simulation.cs b/BepuPhysics/Simulation.cs index 6b4168f0e..4c76dcd0f 100644 --- a/BepuPhysics/Simulation.cs +++ b/BepuPhysics/Simulation.cs @@ -57,7 +57,7 @@ public partial class Simulation : IDisposable /// public bool Deterministic { get; set; } - /// + /// /// Constructs a simulation supporting dynamic movement and constraints with the specified narrow phase callbacks. /// /// Buffer pool used to fill persistent structures and main thread ephemeral resources across the engine. @@ -103,7 +103,7 @@ public static Simulation Create simulation.Solver = new Solver(simulation.Bodies, simulation.BufferPool, solverIterationCount, solverFallbackBatchThreshold, initialCapacity: initialAllocationSizes.Value.Constraints, - initialIslandCapacity: initialAllocationSizes.Value.Islands, + initialIslandCapacity: initialAllocationSizes.Value.Islands, minimumCapacityPerTypeBatch: initialAllocationSizes.Value.ConstraintsPerTypeBatch, poseIntegrator); simulation.constraintRemover = new ConstraintRemover(simulation.BufferPool, simulation.Bodies, simulation.Solver); simulation.Sleeper = new IslandSleeper(simulation.Bodies, simulation.Solver, simulation.BroadPhase, simulation.constraintRemover, simulation.BufferPool); @@ -235,6 +235,25 @@ public void CollisionDetection(float dt, IThreadDispatcher threadDispatcher = nu profiler.End(NarrowPhase); } + /// + /// Updates the broad phase structure for the current body bounding boxes, finds potentially colliding pairs, and then executes the narrow phase for all such pairs. Generates contact constraints for the solver. + /// + /// Duration of the time step. + /// Thread dispatcher to use for execution, if any. + public void Solve(float dt, IThreadDispatcher threadDispatcher = null) + { + Profiler.Start(Solver); + var constrainedBodySet = Solver.PrepareConstraintIntegrationResponsibilities(threadDispatcher); + Solver.Solve(dt, threadDispatcher); + Profiler.End(Solver); + + Profiler.Start(PoseIntegrator); + PoseIntegrator.IntegrateAfterSubstepping(constrainedBodySet, dt, Solver.SubstepCount, threadDispatcher); + Profiler.End(PoseIntegrator); + + Solver.DisposeConstraintIntegrationResponsibilities(); + } + /// /// Incrementally improves body and constraint storage for better performance. /// diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index 47c71b96e..5bf68bca7 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -12,70 +12,7 @@ namespace BepuPhysics { - /// - /// Reference to a constraint's memory location in the solver. - /// - public unsafe struct ConstraintReference - { - internal TypeBatch* typeBatchPointer; - /// - /// Gets a reference to the type batch holding the constraint. - /// - public ref TypeBatch TypeBatch - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - get - { - return ref *typeBatchPointer; - } - } - /// - /// Index in the type batch where the constraint is allocated. - /// - public readonly int IndexInTypeBatch; - - /// - /// Creates a new constraint reference from a constraint memory location. - /// - /// Pointer to the type batch where the constraint lives. - /// Index in the type batch where the constraint is allocated. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ConstraintReference(TypeBatch* typeBatchPointer, int indexInTypeBatch) - { - this.typeBatchPointer = typeBatchPointer; - IndexInTypeBatch = indexInTypeBatch; - } - } - - /// - /// Location in memory where a constraint is stored. - /// - public struct ConstraintLocation - { - //Note that the type id is included, even though we can extract it from a type parameter. - //This is required for body memory swap induced reference changes- it is not efficient to include type metadata in the per-body connections, - //so instead we keep a type id cached. - //(You could pack these a bit- it's pretty reasonable to say you can't have more than 2^24 constraints of a given type and 2^8 constraint types... - //It's just not that valuable, unless proven otherwise.) - /// - /// Index of the constraint set that owns the constraint. If zero, the constraint is attached to bodies that are awake. - /// - public int SetIndex; - /// - /// Index of the constraint batch the constraint belongs to. - /// - public int BatchIndex; - /// - /// Type id of the constraint. Used to look up the type batch index in a constraint batch's type id to type batch index table. - /// - public int TypeId; - /// - /// Index of the constraint in a type batch. - /// - public int IndexInTypeBatch; - } - - public partial class Solver + public abstract partial class Solver { /// @@ -124,20 +61,35 @@ public partial class Solver /// public QuickSet> ConstrainedKinematicHandles; - int iterationCount; + + protected int substepCount; + /// + /// Gets or sets the number of substeps the solver will simulate per call to Solve. + /// + public int SubstepCount + { + get { return substepCount; } + set + { + if (substepCount < 1) + throw new ArgumentException("Substep count must be positive."); + substepCount = value; + } + } + int velocityIterationCount; /// - /// Gets or sets the number of solver iterations to compute per call to Update. + /// Gets or sets the number of solver velocity iterations to compute per substep. /// - public int IterationCount + public int VelocityIterationCount { - get { return iterationCount; } + get { return velocityIterationCount; } set { if (value < 1) { throw new ArgumentException("Iteration count must be positive."); } - iterationCount = value; + velocityIterationCount = value; } } @@ -247,7 +199,7 @@ public Solver(Bodies bodies, BufferPool pool, int iterationCount, int fallbackBa int initialIslandCapacity, int minimumCapacityPerTypeBatch) { - this.iterationCount = iterationCount; + this.velocityIterationCount = iterationCount; this.minimumCapacityPerTypeBatch = minimumCapacityPerTypeBatch; this.bodies = bodies; this.pool = pool; @@ -257,11 +209,15 @@ public Solver(Bodies bodies, BufferPool pool, int iterationCount, int fallbackBa ActiveSet = new ConstraintSet(pool, fallbackBatchThreshold + 1); batchReferencedHandles = new QuickList(fallbackBatchThreshold + 1, pool); ResizeHandleCapacity(initialCapacity); - solveWorker = SolveWorker; - incrementalContactUpdateWorker = IncrementalContactUpdateWorker; ConstrainedKinematicHandles = new QuickSet>(bodies.HandleToLocation.Length, pool); } + /// + /// Registers a constraint type with the solver, creating a type processor for the type internally and allowing constraints of that type to be added to the solver. + /// + /// Type of the constraint to register with the solver. + /// Fired when another constraint type of the same id has already been registered. + /// is called during simuation creation and registers all the built in types. Calling manually is only necessary if custom types are used. public void Register() where TDescription : unmanaged, IConstraintDescription { var description = default(TDescription); @@ -1375,6 +1331,12 @@ public void Remove(ConstraintHandle handle) HandlePool.Return(handle.Value, pool); } + /// + /// Gets the constraint description associated with a constraint reference. + /// + /// Type of the constraint description to retrieve. + /// Reference to the constraint to retrieve. + /// Retrieved description of the constraint. public void GetDescription(ConstraintReference constraintReference, out TConstraintDescription description) where TConstraintDescription : unmanaged, IConstraintDescription { @@ -1386,6 +1348,12 @@ public void GetDescription(ConstraintReference constrain default(TConstraintDescription).BuildDescription(ref constraintReference.TypeBatch, bundleIndex, innerIndex, out description); } + /// + /// Gets the constraint description associated with a constraint handle. + /// + /// Type of the constraint description to retrieve. + /// Handle of the constraint to retrieve. + /// Retrieved description of the constraint. public void GetDescription(ConstraintHandle handle, out TConstraintDescription description) where TConstraintDescription : unmanaged, IConstraintDescription { @@ -1991,7 +1959,5 @@ public void Dispose() pool.Return(ref HandleToConstraint); HandlePool.Dispose(pool); } - - } } diff --git a/BepuPhysics/Solver_IncrementalContactUpdate.cs b/BepuPhysics/Solver_IncrementalContactUpdate.cs deleted file mode 100644 index df771c121..000000000 --- a/BepuPhysics/Solver_IncrementalContactUpdate.cs +++ /dev/null @@ -1,83 +0,0 @@ -using BepuPhysics.CollisionDetection; -using BepuUtilities; -using BepuUtilities.Memory; -using System; -using System.Collections.Generic; -using System.Diagnostics; -using System.Runtime.CompilerServices; -using System.Text; - -namespace BepuPhysics -{ - public partial class Solver - { - protected struct IncrementalContactDataUpdateFilter : ITypeBatchSolveFilter - { - public bool IncludeFallbackBatchForWorkBlocks { get { return true; } } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool AllowType(int typeId) - { - return NarrowPhase.IsContactConstraintType(typeId); - } - } - - struct IncrementalContactUpdateStageFunction : IStageFunction - { - public float Dt; - public float InverseDt; - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Execute(Solver solver, int blockIndex, int workerIndex) - { - ref var block = ref solver.context.ConstraintBlocks.Blocks[blockIndex]; - ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; - solver.TypeProcessors[typeBatch.TypeId].IncrementallyUpdateContactData(ref typeBatch, solver.bodies, Dt, InverseDt, block.StartBundle, block.End); - } - } - - void IncrementalContactUpdateWorker(int workerIndex) - { - int start = GetUniformlyDistributedStart(workerIndex, context.ConstraintBlocks.Blocks.Count, context.WorkerCount, 0); - - int syncStage = 0; - //The claimed and unclaimed state swap after every usage of both pingpong claims buffers. - int claimedState = 1; - int unclaimedState = 0; - var bounds = context.WorkerBoundsA; - var boundsBackBuffer = context.WorkerBoundsB; - //Note that every batch has a different start position. Each covers a different subset of constraints, so they require different start locations. - //The same concept applies to the prestep- the prestep covers all constraints at once, rather than batch by batch. - var incrementalContactUpdateStage = new IncrementalContactUpdateStageFunction { Dt = context.Dt, InverseDt = 1f / context.Dt }; - Debug.Assert(ActiveSet.Batches.Count > 0, "Don't dispatch if there are no constraints."); - //Technically this could mutate prestep starts, but at the moment we rebuild starts every frame anyway so it doesn't matter one way or the other. - ExecuteStage(ref incrementalContactUpdateStage, ref context.ConstraintBlocks, ref bounds, ref boundsBackBuffer, workerIndex, 0, context.ConstraintBlocks.Blocks.Count, - ref start, ref syncStage, claimedState, unclaimedState); - } - - internal void IncrementallyUpdateContactConstraints(float dt, IThreadDispatcher threadDispatcher = null) - { - if (threadDispatcher == null) - { - var inverseDt = 1f / dt; - ref var activeSet = ref ActiveSet; - for (int i = 0; i < activeSet.Batches.Count; ++i) - { - ref var batch = ref activeSet.Batches[i]; - for (int j = 0; j < batch.TypeBatches.Count; ++j) - { - ref var typeBatch = ref batch.TypeBatches[j]; - if (NarrowPhase.IsContactConstraintType(typeBatch.TypeId)) - { - TypeProcessors[typeBatch.TypeId].IncrementallyUpdateContactData(ref typeBatch, bodies, dt, inverseDt, 0, typeBatch.BundleCount); - } - } - } - } - else - { - ExecuteMultithreaded(dt, threadDispatcher, incrementalContactUpdateWorker); - } - } - } -} diff --git a/BepuPhysics/Solver_Solve.cs b/BepuPhysics/Solver_Solve.cs index 478267adb..38b900745 100644 --- a/BepuPhysics/Solver_Solve.cs +++ b/BepuPhysics/Solver_Solve.cs @@ -9,72 +9,41 @@ using System.Runtime.InteropServices; using System.Text; using System.Threading; +using System.Runtime.Intrinsics.X86; +using System.Numerics; +using System.Runtime.Intrinsics; +using static BepuPhysics.Solver; +using BepuPhysics.CollisionDetection; namespace BepuPhysics { public partial class Solver { + protected enum SolverStageType + { + IncrementalUpdate, + IntegrateConstrainedKinematics, + WarmStart, + Solve, + } - //This is going to look a bit more complicated than would be expected for a series of forloops. - //A naive implementation would look something like: - //1) PRESTEP: Parallel dispatch over all constraints, regardless of batch. (Presteps do not write shared data, so there is no need to redispatch per-batch.) - //2) WARMSTART: Loop over all constraint batches, parallel dispatch over all constraints in batch. (Warmstarts read and write velocities, so batch borders must be respected.) - //3) SOLVE ITERATIONS: Loop over iterations, loop over all constraint batches, parallel dispatch over all constraints in batch. (Solve iterations also read/write.) - - //There are a few problems with this approach: - //1) Fork-join dispatches are not free. Expect ~2us overhead on the main thread for each one, regardless of the workload. - //If there are 10 constraint batches and 10 iterations, you're up to 1 + 10 + 10 * 10 = 111 dispatches. Over a fifth of a millisecond in pure overhead. - //This is just a byproduct of general purpose dispatchers not being able to make use of extremely fine grained application knowledge. - //Every dispatch has to get the threads rolling and scheduled, then the threads have to figure out when to go back into a blocked state - //when no more work is unavailable, and so on. Over and over and over again. - - //2) The forloop provider is not guaranteed to maintain a relationship between forloop index and underlying hardware threads across multiple dispatches. - //In fact, we should expect the opposite. Work stealing is an important feature for threadpools to avoid pointless idle time. - //Unfortunately, this can destroy potential cache locality across solver iterations. This matters only a little bit for smaller simulations on a single processor- - //if a single core's solver iteration data can fit in L2, then having 'sticky' scheduling will help over multiple iterations. For a 256 KiB per-core L2, - //that would be a simulation of only about 700 big constraints per core. (That is actually quite a few back in BEPUphysics v1 land... not so much in v2.) - - //Sticky scheduling becomes more important when talking about larger simulations. Consider L3; an 8 MiB L3 cache can hold over 20000 heavy constraints - //worth of solver iteration data. This is usually shared across all cores of a processor, so the stickiness isn't always useful. However, consider - //a multiprocessor system. When there are multiple processors, there are multiple L3 caches. Limiting the amount of communication between processors - //(and to potentially remote parts of system memory) is important, since those accesses tend to have longer latency and lower total bandwidth than direct L3 accesses. - //But you don't have to resort to big servers to see something like this- some processors, notably the recent Ryzen line, actually behave a bit like - //multiple processors that happen to be stuck on the same chip. If the application requires tons of intercore communication, performance will suffer. - //And of course, cache misses just suck. - - //3) Work stealing implementations that lack application knowledge will tend to make a given worker operate across noncontiguous regions, harming locality and forcing cache misses. - - //So what do we do? We have special guarantees: - //1) We have to do a bunch of solver iterations in sequence, covering the exact same data over and over. Even the prestep and warmstart cover a lot of the same data. - //2) We can control the dispatch sizes within a frame. They're going to be the same, over and over, and the next dispatch follows immediately after the last. - //3) We can guarantee that individual work blocks are fairly small. (A handful of microseconds.) - - //So, there's a few parts to the solution as implemented: - //1) Dispatch *once* and perform fine grained synchronization with busy waits to block at constraint batch borders. Unless the operating system - //reschedules a thread (which is very possible, but not a constant occurrence), a worker index will stay associated with the same underlying hardware. - //2) Worker start locations are spaced across the work blocks so that each worker has a high probability of claiming multiple blocks contiguously. - //3) Workers track the largest contiguous region that they've been able to claim within an iteration. This is used to provide the next iteration a better starting guess. - - //So, for the most part, the same core/processor will tend to work on the same data over the course of the solve. Hooray! - - //A couple of notes: - //1) We explicitly don't care about maintaining worker-data relationships between frames. The cache will likely be trashed by the rest of the program- even other parts - //of the physics simulation will evict stuff. We're primarily concerned about scheduling within the solver. - //2) Note that neither the prestep nor the warmstart are used to modify the work distribution for the solve- neither of those stages is proportional to the solve iteration load. - - //3) Core-data stickiness doesn't really offer much value for L1/L2 caches. It doesn't take much to evict the entirety of the old data- a 3770K only holds 256KB in its L2. - //Even if we optimized every constraint to require no more than 350B per iteration for the heaviest constraint - //(when this was written, it was at 602B per iteration), a single core's L2 could only hold up to about 750 constraints. - //So, the 3770K under ideal circumstances would avoid evicting on a per-iteration basis if the simulation had a total of less than 3000 such constraints. - //A single thread of a 3700K at 4.5ghz could do prestep-warmstart-8iterations for that in ~2.6 milliseconds. In other words, it's a pretty small simulation. - - //Sticky scheduling only becomes more useful when dealing with multiprocessor systems (or multiprocessor-ish systems, like ryzen) and big datasets, like you might find in an MMO server. - //A 3770K has 8MB of L3 cache shared across all cores, enough to hold a little under 24000 large constraint solves worth of data between iterations, which is a pretty large chunk. - //If you had four similar processors, you could ideally handle almost 100,000 constraints without suffering significant evictions in each processor's L3 during iterations. - //Without sticky scheduling, memory bandwidth use could skyrocket during iterations as the L3 gets missed over and over. - - - protected internal struct WorkBlock + protected struct SolverSyncStage + { + public Buffer Claims; + public int BatchIndex; + public int WorkBlockStartIndex; + public SolverStageType StageType; + + public SolverSyncStage(Buffer claims, int workBlockStartIndex, SolverStageType type, int batchIndex = -1) + { + Claims = claims; + BatchIndex = batchIndex; + WorkBlockStartIndex = workBlockStartIndex; + StageType = type; + } + } + + protected struct WorkBlock { public int BatchIndex; public int TypeBatchIndex; @@ -88,442 +57,375 @@ protected internal struct WorkBlock public int End; } - protected interface ITypeBatchSolveFilter + protected struct IntegrationWorkBlock { - bool IncludeFallbackBatchForWorkBlocks { get; } - bool AllowType(int typeId); + public int StartBundleIndex; + public int EndBundleIndex; } - protected struct MainSolveFilter : ITypeBatchSolveFilter + //This is up in Solver, instead of Solver, due to explicit layout. + [StructLayout(LayoutKind.Explicit)] + protected struct SubstepMultithreadingContext { - public bool IncludeFallbackBatchForWorkBlocks - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - get - { - return false; - } - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool AllowType(int typeId) - { - return true; - } - } - - protected unsafe void BuildWorkBlocks( - BufferPool pool, int minimumBlockSizeInBundles, int maximumBlockSizeInBundles, int targetBlocksPerBatch, ref TTypeBatchFilter typeBatchFilter, - out QuickList workBlocks, out Buffer batchBoundaries) where TTypeBatchFilter : ITypeBatchSolveFilter - { - ref var activeSet = ref ActiveSet; - int batchCount; - if (typeBatchFilter.IncludeFallbackBatchForWorkBlocks) - { - batchCount = activeSet.Batches.Count; - } - else - { - GetSynchronizedBatchCount(out batchCount, out _); - } - workBlocks = new QuickList(targetBlocksPerBatch * batchCount, pool); - pool.Take(batchCount, out batchBoundaries); - var inverseMinimumBlockSizeInBundles = 1f / minimumBlockSizeInBundles; - var inverseMaximumBlockSizeInBundles = 1f / maximumBlockSizeInBundles; - for (int batchIndex = 0; batchIndex < batchCount; ++batchIndex) - { - ref var typeBatches = ref activeSet.Batches[batchIndex].TypeBatches; - var bundleCount = 0; - for (int typeBatchIndex = 0; typeBatchIndex < typeBatches.Count; ++typeBatchIndex) - { - if (typeBatchFilter.AllowType(typeBatches[typeBatchIndex].TypeId)) - { - bundleCount += typeBatches[typeBatchIndex].BundleCount; - } - } - for (int typeBatchIndex = 0; typeBatchIndex < typeBatches.Count; ++typeBatchIndex) - { - ref var typeBatch = ref typeBatches[typeBatchIndex]; - if (!typeBatchFilter.AllowType(typeBatch.TypeId)) - { - continue; - } - var typeBatchSizeFraction = typeBatch.BundleCount / (float)bundleCount; //note: pre-inverting this doesn't necessarily work well due to numerical issues. - var typeBatchMaximumBlockCount = typeBatch.BundleCount * inverseMinimumBlockSizeInBundles; - var typeBatchMinimumBlockCount = typeBatch.BundleCount * inverseMaximumBlockSizeInBundles; - var typeBatchBlockCount = Math.Max(1, (int)Math.Min(typeBatchMaximumBlockCount, Math.Max(typeBatchMinimumBlockCount, targetBlocksPerBatch * typeBatchSizeFraction))); - int previousEnd = 0; - var baseBlockSizeInBundles = typeBatch.BundleCount / typeBatchBlockCount; - var remainder = typeBatch.BundleCount - baseBlockSizeInBundles * typeBatchBlockCount; - for (int newBlockIndex = 0; newBlockIndex < typeBatchBlockCount; ++newBlockIndex) - { - ref var block = ref workBlocks.Allocate(pool); - var blockBundleCount = newBlockIndex < remainder ? baseBlockSizeInBundles + 1 : baseBlockSizeInBundles; - block.BatchIndex = batchIndex; - block.TypeBatchIndex = typeBatchIndex; - block.StartBundle = previousEnd; - block.End = previousEnd + blockBundleCount; - previousEnd = block.End; - Debug.Assert(block.StartBundle >= 0 && block.StartBundle < typeBatch.BundleCount); - Debug.Assert(block.End >= block.StartBundle + Math.Min(minimumBlockSizeInBundles, typeBatch.BundleCount) && block.End <= typeBatch.BundleCount); - } - } - batchBoundaries[batchIndex] = workBlocks.Count; - } - } + [FieldOffset(0)] + public Buffer Stages; + [FieldOffset(16)] + public Buffer IncrementalUpdateBlocks; + [FieldOffset(32)] + public Buffer KinematicIntegrationBlocks; + [FieldOffset(48)] + public Buffer ConstraintBlocks; + [FieldOffset(64)] + public Buffer ConstraintBatchBoundaries; + [FieldOffset(80)] + public float Dt; + [FieldOffset(84)] + public float InverseDt; + [FieldOffset(88)] + public int WorkerCount; - protected struct WorkerBounds - { + //This index is written during multithreaded execution; don't want to infest any of the more frequently read properties, so it's shoved out of any dangerous cache line. /// - /// Inclusive start of blocks known to be claimed by any worker. + /// Monotonically increasing index of executed stages during a frame. /// - public int Min; + [FieldOffset(256)] + public int SyncIndex; + /// - /// Exclusive end of blocks known to be claimed by any worker. + /// Counter of work completed for the current stage. /// - public int Max; - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Merge(ref WorkerBounds current, ref WorkerBounds mergeSource) - { - if (mergeSource.Min < current.Min) - current.Min = mergeSource.Min; - if (mergeSource.Max > current.Max) - current.Max = mergeSource.Max; - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static bool BoundsTouch(ref WorkerBounds a, ref WorkerBounds b) - { - //Note that touching is sufficient reason to merge. They don't have to actually intersect. - return a.Min - b.Max <= 0 && b.Min - a.Max <= 0; - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void MergeIfTouching(ref WorkerBounds current, ref WorkerBounds other) - { - //Could be a little more clever here if it matters. - if (BoundsTouch(ref current, ref other)) - Merge(ref current, ref other); + [FieldOffset(384)] + public int CompletedWorkBlockCount; - } - } - protected struct WorkBlocks where T : unmanaged - { - public QuickList Blocks; - public Buffer Claims; - public void CreateClaims(BufferPool pool) - { - pool.TakeAtLeast(Blocks.Count, out Claims); - Claims.Clear(0, Blocks.Count); - } - public void Dispose(BufferPool pool) - { - Blocks.Dispose(pool); - pool.Return(ref Claims); - } } - //Just bundling these up to avoid polluting the this. intellisense. - protected struct MultithreadingParameters - { - public float Dt; - public WorkBlocks ConstraintBlocks; - public Buffer BatchBoundaries; - public int WorkerCompletedCount; - public int WorkerCount; - - public WorkBlocks IncrementalUpdateBlocks; - public Buffer IncrementalUpdateBatchBoundaries; - - public Buffer WorkerBoundsA; - public Buffer WorkerBoundsB; + public abstract IndexSet PrepareConstraintIntegrationResponsibilities(IThreadDispatcher threadDispatcher = null); + public abstract void DisposeConstraintIntegrationResponsibilities(); + public abstract void Solve(float dt, IThreadDispatcher threadDispatcher = null); + } - public Buffer SyncStageWorkCounters; + /// + /// Handles integration-aware substepped solving. + /// + /// Type of integration callbacks being used during the substepped solve. + public class Solver : Solver where TIntegrationCallbacks : struct, IPoseIntegratorCallbacks + { + /* + There are two significant sources of complexity here: + 1. The solver takes substeps, which means velocity/pose integration must be embedded into the solving process. + 2. There are many sync points, and thread dispatches must manage these in a low overhead way. + + Looking at the single threaded implementation at the very bottom would be helpful for understanding the general flow of execution. + + In order to reduce overall dispatch count and to share memory loads as much as possible, the first execution of a constraint affecting a body is responsible for that body's integration. + There are some special cases around this- kinematics must be handled in a separate prepass, since kinematics can appear in a given constraint batch more than once. + Unconstrained bodies will be integrated separately outside of the solver. + + To reduce sync point overhead, worker threads do not enter blocking states. The main orchestrator thread never even yields- it only spins. + The main thread kicks off jobs to all available workers. If a worker has yielded previously, it may miss waking up, but that will not block the execution of other threads. + Work is scheduled such that the same thread operates on the same data each iteration/substep, if possible. This makes it more likely that a core will find relevant data in its local caches. + There is a tradeoff with workstealing- to keep overhead low while maintaining this consistent scheduling, threads only look for incrementally adjacent blocks. This can sometimes result in imbalanced workloads. + (This will likely need to be updated to be cleverer as heterogeneous architectures gain popularity.) + */ + + public Solver(Bodies bodies, BufferPool pool, int iterationCount, int fallbackBatchThreshold, + int initialCapacity, + int initialIslandCapacity, + int minimumCapacityPerTypeBatch, PoseIntegrator poseIntegrator) + : base(bodies, pool, iterationCount, fallbackBatchThreshold, initialCapacity, initialIslandCapacity, minimumCapacityPerTypeBatch) + { + PoseIntegrator = poseIntegrator; + solveStep2Worker2 = SolveStep2Worker2; + constraintIntegrationResponsibilitiesWorker = ConstraintIntegrationResponsibilitiesWorker; } - protected MultithreadingParameters context; - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - void MergeWorkerBounds(ref WorkerBounds bounds, ref Buffer allWorkerBounds, int workerIndex) + /// + /// Pose integrator used by the simulation. + /// + public PoseIntegrator PoseIntegrator { get; private set; } + protected interface ITypeBatchSolveFilter { - for (int i = 0; i < workerIndex; ++i) - { - WorkerBounds.MergeIfTouching(ref bounds, ref allWorkerBounds[i]); - } - for (int i = workerIndex + 1; i < context.WorkerCount; ++i) + bool IncludeFallbackBatchForWorkBlocks { get; } + bool AllowType(int typeId); + } + protected struct IncrementalContactDataUpdateFilter : ITypeBatchSolveFilter + { + public bool IncludeFallbackBatchForWorkBlocks { get { return true; } } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool AllowType(int typeId) { - WorkerBounds.MergeIfTouching(ref bounds, ref allWorkerBounds[i]); + return NarrowPhase.IsContactConstraintType(typeId); } } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - int TraverseForwardUntilBlocked(ref TStageFunction stageFunction, ref WorkBlocks blocks, int blockIndex, ref WorkerBounds bounds, ref Buffer allWorkerBounds, int workerIndex, - int batchEnd, int claimedState, int unclaimedState) - where TStageFunction : IStageFunction - where TBlock : unmanaged + protected struct MainSolveFilter : ITypeBatchSolveFilter { - //If no claim is made, this defaults to an invalid interval endpoint. - int highestLocallyClaimedIndex = -1; - while (true) + public bool IncludeFallbackBatchForWorkBlocks { - if (Interlocked.CompareExchange(ref blocks.Claims[blockIndex], claimedState, unclaimedState) == unclaimedState) - { - highestLocallyClaimedIndex = blockIndex; - bounds.Max = blockIndex + 1; //Exclusive bound. - Debug.Assert(blockIndex < batchEnd); - stageFunction.Execute(this, blockIndex, workerIndex); - //Increment or exit. - if (++blockIndex == batchEnd) - break; - } - else + [MethodImpl(MethodImplOptions.AggressiveInlining)] + get { - //Already claimed. - bounds.Max = blockIndex + 1; //Exclusive bound. - break; + return false; } } - Debug.Assert(bounds.Max <= batchEnd); - MergeWorkerBounds(ref bounds, ref allWorkerBounds, workerIndex); - Debug.Assert(bounds.Max <= batchEnd); - return highestLocallyClaimedIndex; + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool AllowType(int typeId) + { + return true; + } } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] - int TraverseBackwardUntilBlocked(ref TStageFunction stageFunction, ref WorkBlocks blocks, int blockIndex, ref WorkerBounds bounds, ref Buffer allWorkerBounds, int workerIndex, - int batchStart, int claimedState, int unclaimedState) - where TStageFunction : IStageFunction - where TBlock : unmanaged + private void WarmStartBlock(int workerIndex, int batchIndex, int typeBatchIndex, int startBundle, int endBundle, ref TypeBatch typeBatch, TypeProcessor typeProcessor, float dt, float inverseDt) + where TBatchShouldIntegratePoses : unmanaged, IBatchPoseIntegrationAllowed { - //If no claim is made, this defaults to an invalid interval endpoint. - int lowestLocallyClaimedIndex = blocks.Blocks.Count; - while (true) + if (batchIndex == 0) + { + Buffer noFlagsRequired = default; + typeProcessor.WarmStart2( + ref typeBatch, ref noFlagsRequired, bodies, ref PoseIntegrator.Callbacks, + dt, inverseDt, startBundle, endBundle, workerIndex); + } + else { - if (Interlocked.CompareExchange(ref blocks.Claims[blockIndex], claimedState, unclaimedState) == unclaimedState) + if (coarseBatchIntegrationResponsibilities[batchIndex][typeBatchIndex]) { - lowestLocallyClaimedIndex = blockIndex; - bounds.Min = blockIndex; - Debug.Assert(blockIndex >= batchStart); - stageFunction.Execute(this, blockIndex, workerIndex); - //Decrement or exit. - if (blockIndex == batchStart) - break; - --blockIndex; + typeProcessor.WarmStart2( + ref typeBatch, ref integrationFlags[batchIndex][typeBatchIndex], bodies, ref PoseIntegrator.Callbacks, + dt, inverseDt, startBundle, endBundle, workerIndex); } else { - //Already claimed. - bounds.Min = blockIndex; - break; + typeProcessor.WarmStart2( + ref typeBatch, ref integrationFlags[batchIndex][typeBatchIndex], bodies, ref PoseIntegrator.Callbacks, + dt, inverseDt, startBundle, endBundle, workerIndex); } } - MergeWorkerBounds(ref bounds, ref allWorkerBounds, workerIndex); - return lowestLocallyClaimedIndex; } protected interface IStageFunction { void Execute(Solver solver, int blockIndex, int workerIndex); } - struct PrestepStageFunction : IStageFunction + + //Split the solve process into a warmstart and solve, where warmstart doesn't try to store out anything. It just computes jacobians and modifies velocities according to the accumulated impulse. + //The solve step then *recomputes* jacobians from prestep data and pose information. + //Why? Memory bandwidth. Redoing the calculation is cheaper than storing it out. + struct WarmStartStep2StageFunction : IStageFunction { public float Dt; public float InverseDt; + public int SubstepIndex; + public Solver solver; [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Execute(Solver solver, int blockIndex, int workerIndex) { - ref var block = ref solver.context.ConstraintBlocks.Blocks[blockIndex]; + ref var block = ref this.solver.substepContext.ConstraintBlocks[blockIndex]; ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; - typeProcessor.Prestep(ref typeBatch, solver.bodies, Dt, InverseDt, block.StartBundle, block.End); + if (SubstepIndex == 0) + { + this.solver.WarmStartBlock(workerIndex, block.BatchIndex, block.TypeBatchIndex, block.StartBundle, block.End, ref typeBatch, typeProcessor, Dt, InverseDt); + } + else + { + this.solver.WarmStartBlock(workerIndex, block.BatchIndex, block.TypeBatchIndex, block.StartBundle, block.End, ref typeBatch, typeProcessor, Dt, InverseDt); + } + } } - struct WarmStartStageFunction : IStageFunction + + struct SolveStep2StageFunction : IStageFunction { + public float Dt; + public float InverseDt; + public Solver solver; + [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Execute(Solver solver, int blockIndex, int workerIndex) { - ref var block = ref solver.context.ConstraintBlocks.Blocks[blockIndex]; + ref var block = ref this.solver.substepContext.ConstraintBlocks[blockIndex]; ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; - typeProcessor.WarmStart(ref typeBatch, solver.bodies, block.StartBundle, block.End); - + typeProcessor.SolveStep2(ref typeBatch, solver.bodies, Dt, InverseDt, block.StartBundle, block.End); } } - struct SolveStageFunction : IStageFunction + + struct IncrementalUpdateStageFunction : IStageFunction { + public float Dt; + public float InverseDt; + public Solver solver; + [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Execute(Solver solver, int blockIndex, int workerIndex) { - ref var block = ref solver.context.ConstraintBlocks.Blocks[blockIndex]; + ref var block = ref this.solver.substepContext.IncrementalUpdateBlocks[blockIndex]; ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; - var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; - typeProcessor.SolveIteration(ref typeBatch, solver.bodies, block.StartBundle, block.End); + solver.TypeProcessors[typeBatch.TypeId].IncrementallyUpdateContactData(ref typeBatch, solver.bodies, Dt, InverseDt, block.StartBundle, block.End); } } - - - /// - /// Behaves like a framework SpinWait, but never voluntarily relinquishes the timeslice to off-core threads. - /// - /// There are two big reasons for using this over the regular framework SpinWait: - /// 1) The framework spinwait relies on spins for quite a while before resorting to any form of timeslice surrender. - /// Empirically, this is not ideal for the solver- if the sync condition isn't met within several nanoseconds, it will tend to be some microseconds away. - /// This spinwait is much more aggressive about moving to yields. - /// 2) After a number of yields, the framework SpinWait will resort to calling Sleep. - /// This widens the potential set of schedulable threads to those not native to the current core. If we permit that transition, it is likely to evict cached solver data. - /// (For very large simulations, the use of Sleep(0) isn't that concerning- every iteration can be large enough to evict all of cache- - /// but there still isn't much benefit to using it over yields in context.) - /// SpinWait will also fall back to Sleep(1) by default which obliterates performance, but that behavior can be disabled. - /// Note that this isn't an indication that the framework SpinWait should be changed, but rather that the solver's requirements are extremely specific and don't match - /// a general purpose solution very well. - protected struct LocalSpinWait + struct IntegrateConstrainedKinematicsStageFunction : IStageFunction { - public int WaitCount; - - //Empirically, being pretty aggressive about yielding produces the best results. This is pretty reasonable- - //a single constraint bundle can take hundreds of nanoseconds to finish. - //That would be a whole lot of spinning that could be used by some other thread. At worst, we're being friendlier to other applications on the system. - //This thread will likely be rescheduled on the same core, so it's unlikely that we'll lose any cache warmth (that we wouldn't have lost anyway). - public const int YieldThreshold = 3; + public float Dt; + public float InverseDt; + public int SubstepIndex; + public Solver solver; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void SpinOnce() + public void Execute(Solver solver, int blockIndex, int workerIndex) { - if (WaitCount >= YieldThreshold) + ref var block = ref this.solver.substepContext.KinematicIntegrationBlocks[blockIndex]; + if (SubstepIndex == 0) { - Thread.Yield(); + this.solver.PoseIntegrator.IntegrateKinematicVelocities(solver.ConstrainedKinematicHandles.Span.Slice(solver.ConstrainedKinematicHandles.Count), block.StartBundleIndex, block.EndBundleIndex, Dt, workerIndex); } else { - //We are sacrificing one important feature of the newer framework provided waits- normalized spinning (RuntimeThread.OptimalMaxSpinWaitsPerSpinIteration). - //Different platforms can spin at significantly different speeds, so a single constant value for the maximum spin duration doesn't map well to all hardware. - //On the upside, we tend to be concerned about two modes- waiting a very short time, and waiting a medium amount of time. - //The specific length of the 'short' time doesn't matter too much, so long as it's fairly short. - Thread.SpinWait(1 << WaitCount); - ++WaitCount; + this.solver.PoseIntegrator.IntegrateKinematicPosesAndVelocities(solver.ConstrainedKinematicHandles.Span.Slice(solver.ConstrainedKinematicHandles.Count), block.StartBundleIndex, block.EndBundleIndex, Dt, workerIndex); } - } } - - protected void InterstageSync(ref int syncStageIndex) + unsafe void ExecuteWorkerStage(ref TStageFunction stageFunction, int workerIndex, int workerStart, int availableBlocksStartIndex, ref Buffer claims, int previousSyncIndex, int syncIndex, ref int completedWorkBlocks) where TStageFunction : IStageFunction { - //No more work is available to claim, but not every thread is necessarily done with the work they claimed. So we need a dedicated sync- upon completing its local work, - //a worker increments the 'workerCompleted' counter, and the spins on that counter reaching workerCount * stageIndex. - ++syncStageIndex; - var neededCompletionCount = context.WorkerCount * syncStageIndex; - if (Interlocked.Increment(ref context.WorkerCompletedCount) != neededCompletionCount) - { - var spinWait = new LocalSpinWait(); - while (Volatile.Read(ref context.WorkerCompletedCount) < neededCompletionCount) - { - spinWait.SpinOnce(); - } + if (workerStart == -1) + { + //Thread count exceeds work block count; nothing for this worker to do. + //(Technically, there's a possibility that an earlier thread would fail to wake and allowing this thread to steal that block COULD help, + //but on average it's better to keep jobs scheduled to the same core to avoid excess memory traffic, and we can rely on some other active worker to take care of it.) + return; } - } - - protected void ExecuteStage(ref TStageFunction stageFunction, ref WorkBlocks blocks, - ref Buffer allWorkerBounds, ref Buffer previousWorkerBounds, int workerIndex, - int batchStart, int batchEnd, ref int workerStart, ref int syncStage, - int claimedState, int unclaimedState) - where TStageFunction : IStageFunction - where TBlock : unmanaged - { - //It is possible for a worker to not have any job available in a particular batch. This can only happen when there are more workers than work blocks in the batch. - //The workers with indices beyond the available work blocks will have their starts all set to -1 by the scheduler. - //All previous workers will have tightly packed contiguous indices and won't be able to worksteal at all. - if (workerStart > -1) - { - Debug.Assert(workerStart >= batchStart && workerStart < batchEnd); - var blockIndex = workerStart; - - ref var bounds = ref allWorkerBounds[workerIndex]; - - //Just assume the min will be claimed. There's a chance the thread will get preempted or the value will be read before it's actually claimed, but - //that's a very small risk and doesn't affect long-term correctness. (It would just somewhat reduce workstealing effectiveness, and so performance.) - bounds.Min = blockIndex; - Debug.Assert(bounds.Max <= batchEnd); - - //Note that initialization guarantees a start index in the batch; no test required. - //Note that we track the largest contiguous region over the course of the stage execution. The batch start of this worker will be set to the - //minimum slot of the largest contiguous region so that following iterations will tend to have a better initial work distribution with less work stealing. - Debug.Assert(batchStart <= blockIndex && batchEnd > blockIndex); - var highestLocalClaim = TraverseForwardUntilBlocked(ref stageFunction, ref blocks, blockIndex, ref bounds, ref allWorkerBounds, workerIndex, batchEnd, claimedState, unclaimedState); - - Debug.Assert(bounds.Max <= batchEnd); - //By now, we've reached the end of the contiguous region in the forward direction. Try walking the other way. - blockIndex = workerStart - 1; - //Note that there is no guarantee that the block will be in the batch- this could be the leftmost worker. - int lowestLocalClaim; - if (blockIndex >= batchStart) + int workBlockIndex = workerStart; + int locallyCompletedCount = 0; + //Try to claim blocks by traversing forward until we're blocked by another claim. + while (Interlocked.CompareExchange(ref claims[workBlockIndex], syncIndex, previousSyncIndex) == previousSyncIndex) + { + //Successfully claimed a work block. + stageFunction.Execute(this, availableBlocksStartIndex + workBlockIndex, workerIndex); + ++locallyCompletedCount; + ++workBlockIndex; + if (workBlockIndex >= claims.Length) { - lowestLocalClaim = TraverseBackwardUntilBlocked(ref stageFunction, ref blocks, blockIndex, ref bounds, ref allWorkerBounds, workerIndex, batchStart, claimedState, unclaimedState); + //Wrap around. + workBlockIndex = 0; } - else + } + //Try to claim work blocks going backward. + workBlockIndex = workerStart - 1; + while (true) + { + if (workBlockIndex < 0) { - lowestLocalClaim = batchStart; + //Wrap around. + workBlockIndex = claims.Length - 1; } - Debug.Assert(bounds.Max <= batchEnd); - //These are actually two inclusive bounds, so this is count - 1, but as long as we're consistent it's fine. - //For this first region, we need to check that it's actually a valid region- if the claims were blocked, it might not be. - var largestContiguousRegionSize = highestLocalClaim - lowestLocalClaim; - if (largestContiguousRegionSize >= 0) - workerStart = lowestLocalClaim; - else - largestContiguousRegionSize = 0; //It was an invalid region, but later invalid regions should be rejected by size. Setting to zero guarantees that later regions have to have at least one open slot. - - - //All contiguous slots have been claimed. Now just traverse to the end along the right direction. - while (bounds.Max < batchEnd) + //Note the comparison: equal *or greater* blocks. + //Consider what happens if this thread was heavily delayed and the stage it was dispatched for has already ended. + //Other threads could be working on the next sync index. A mere equality test could result in this thread thinking there's work to be done, so it starts claiming for an *earlier* stage. + //Then everything dies. + if (Interlocked.CompareExchange(ref claims[workBlockIndex], syncIndex, previousSyncIndex) != previousSyncIndex) { - //Each of these iterations may find a contiguous region larger than our previous attempt. - lowestLocalClaim = bounds.Max; - highestLocalClaim = TraverseForwardUntilBlocked(ref stageFunction, ref blocks, bounds.Max, ref bounds, ref allWorkerBounds, workerIndex, batchEnd, claimedState, unclaimedState); - //If the claim at index lowestLocalClaim was blocked, highestLocalClaim will be -1, so the size will be negative. - var regionSize = highestLocalClaim - lowestLocalClaim; //again, actually count - 1 - if (regionSize > largestContiguousRegionSize) - { - workerStart = lowestLocalClaim; - largestContiguousRegionSize = regionSize; - } - Debug.Assert(bounds.Max <= batchEnd); + break; } + //Successfully claimed a work block. + stageFunction.Execute(this, availableBlocksStartIndex + workBlockIndex, workerIndex); + ++locallyCompletedCount; + workBlockIndex--; + } + //No more adjacent work blocks are available. This thread is done! + Interlocked.Add(ref completedWorkBlocks, locallyCompletedCount); + + + //debugStageWorkBlocksCompleted[syncIndex - 1][workerIndex] = locallyCompletedCount; + //if (workerIndex == 3) + //{ + //Console.WriteLine($"Worker {workerIndex}, stage {typeof(TStageFunction).Name}, sync index {syncIndex} completed {locallyCompletedCount / (double)claims.Length:G2} ({locallyCompletedCount} of {claims.Length})."); + //} + //for (int i = 0; i < claims.Length; ++i) + //{ + // if (claims[i] != syncIndex) + // { + // Console.WriteLine($"Failed to claim index {i}, claim value is {claims[i]} instead of {syncIndex}, previous claim should have been {previousSyncIndex}, worker start {workerStart}"); + // } + //} - //Traverse backwards. - while (bounds.Min > batchStart) + } + void ExecuteMainStage(ref TStageFunction stageFunction, int workerIndex, int workerStart, ref SolverSyncStage stage, int previousSyncIndex, int syncIndex) where TStageFunction : IStageFunction + { + var availableBlocksCount = stage.Claims.Length; + if (availableBlocksCount == 0) + return; + + //for (int i = 0; i < availableBlocksCount; ++i) + //{ + // stageFunction.Execute(this, stage.WorkBlockStartIndex + i, workerIndex); + //} + //return; + //Console.WriteLine($"Main executing {typeof(TStageFunction).Name} for sync index {syncIndex}, expected claim {syncIndex - previousSyncIndexOffset}"); + if (availableBlocksCount == 1) + { + //Console.WriteLine($"Main thread is executing {syncIndex} by itself; stage function: {stageFunction.GetType().Name}"); + //There is only one work block available. There's no reason to notify other threads about it or do any claims management; just execute it sequentially. + stageFunction.Execute(this, stage.WorkBlockStartIndex, workerIndex); + } + else + { + //Console.WriteLine($"Main thread is requesting workers begin for sync index {syncIndex}; stage function: {stageFunction.GetType().Name}"); + //Write the new stage index so other spinning threads will begin work on it. + Volatile.Write(ref substepContext.SyncIndex, syncIndex); + ExecuteWorkerStage(ref stageFunction, workerIndex, workerStart, stage.WorkBlockStartIndex, ref stage.Claims, previousSyncIndex, syncIndex, ref substepContext.CompletedWorkBlockCount); + + //Since we asked other threads to do work, we must wait until the requested work is done before proceeding. + //Note that we DO NOT yield on the main thread! + //This significantly increases the chance *some* progress will be made on the available work, even if all other workers are stuck unscheduled. + //The reasoning here is that the OS is not likely to unschedule an active thread, but will be far less aggressive about scheduling a *currently unscheduled* thread. + //Critically, yielding threads are not in any kind of execution queue- from the OS's perspective, they aren't asking to be woken up. + //If another thread comes in with significant work, they could be stalled for (from the solver's perspective) an arbitrarily long time. + //By having the main thread never yield, the only way for all progress to halt is for the OS to aggressively unschedule the main thread. + //That is very rare when dealing with CPUs with plenty of cores to go around relative to the scheduled work. + //(Why not notify the OS that waiting threads actually want to be executed? Just overhead. Feel free to experiment with different approaches, but so far this has won empirically.) + while (Volatile.Read(ref substepContext.CompletedWorkBlockCount) != availableBlocksCount) { - //Note bounds.Min - 1; Min is inclusive, so in order to access a new location, it must be pushed out. - //Note that the above condition uses a > to handle this. - highestLocalClaim = bounds.Min - 1; - lowestLocalClaim = TraverseBackwardUntilBlocked(ref stageFunction, ref blocks, highestLocalClaim, ref bounds, ref allWorkerBounds, workerIndex, batchStart, claimedState, unclaimedState); - //If the claim at highestLocalClaim was blocked, lowestLocalClaim will be workblocks.Count, so the size will be negative. - var regionSize = highestLocalClaim - lowestLocalClaim; //again, actually count - 1 - if (regionSize > largestContiguousRegionSize) - { - workerStart = lowestLocalClaim; - largestContiguousRegionSize = regionSize; - } - Debug.Assert(bounds.Max <= batchEnd); + Thread.SpinWait(3); } - - Debug.Assert(bounds.Min == batchStart && bounds.Max == batchEnd); - + //Console.WriteLine($"Completed blocks count: {substepContext.CompletedWorkBlockCount}."); + //All workers are done. We can safely reset the counter for the next time this stage is used. + substepContext.CompletedWorkBlockCount = 0; } - //Clear the previous bounds array before the sync so the next stage has fresh data. - //Note that this clear is unconditional- the previous worker data must be cleared out or trash data may find its way into the next stage. - previousWorkerBounds[workerIndex].Min = int.MaxValue; - previousWorkerBounds[workerIndex].Max = int.MinValue; + } + protected SubstepMultithreadingContext substepContext; - InterstageSync(ref syncStage); - //Swap the bounds buffers being used before proceeding. - var tempWorkerBounds = allWorkerBounds; - allWorkerBounds = previousWorkerBounds; - previousWorkerBounds = tempWorkerBounds; + [MethodImpl(MethodImplOptions.AggressiveInlining)] + int GetPreviousSyncIndexForIncrementalUpdate(int substepIndex, int syncIndex, int syncStagesPerSubstep) + { + return substepIndex == 1 ? 0 : Math.Max(0, syncIndex - syncStagesPerSubstep); + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + int GetPreviousSyncIndexForIntegrateConstrainedKinematics(int substepIndex, int syncIndex, int syncStagesPerSubstep) + { + //If kinematics have their velocities integrated, then the first substep will have executed and left the claims at 1. Otherwise, the first substep will leave them cleared at 0. + //The second substep and later will always run (since kinematics need their poses integrated regardless) so their sync index isn't weirdly conditional. + return substepIndex == 1 ? PoseIntegrator.Callbacks.IntegrateVelocityForKinematics ? 2 : 0 : Math.Max(0, syncIndex - syncStagesPerSubstep); + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + int GetPreviousSyncIndexForWarmStart(int syncIndex, int synchronizedBatchCount) + { + //The claims for warmstarts and solves are shared. So we want to look back to the last solve's claims, which would be beyond the incremental update and integrate constrained kinematics. + return Math.Max(0, syncIndex - synchronizedBatchCount - 2); + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + int GetPreviousSyncIndexForSolve(int syncIndex, int synchronizedBatchCount) + { + return Math.Max(0, syncIndex - synchronizedBatchCount); } protected static int GetUniformlyDistributedStart(int workerIndex, int blockCount, int workerCount, int offset) @@ -538,213 +440,981 @@ protected static int GetUniformlyDistributedStart(int workerIndex, int blockCoun return offset + blocksPerWorker * workerIndex + Math.Min(remainder, workerIndex); } - void SolveWorker(int workerIndex) + Action solveStep2Worker2; + void SolveStep2Worker2(int workerIndex) { - int prestepStart = GetUniformlyDistributedStart(workerIndex, context.ConstraintBlocks.Blocks.Count, context.WorkerCount, 0); + //The solver has two codepaths: one thread, acting as an orchestrator, and the others, just waiting to be used. + //There is no requirement that a worker thread above index 0 actually runs at all for a given dispatch. + //If a worker fails to schedule for a long time because the OS went with a different thread, that's perfectly fine- + //another thread will consume the work that would have otherwise been handled by it, and the execution as a whole + //will continue on unimpeded. + //There's still nothing done if the OS unschedules an active worker that claimed work, but that's a far, far rarer concern. + //Note that this attempts to maintain a given worker's relationship to a set of work blocks. This increases the probability that + //data will remain in some cache that's reasonably close to the core. + int workerCount = substepContext.WorkerCount; + var incrementalUpdateWorkerStart = GetUniformlyDistributedStart(workerIndex, substepContext.IncrementalUpdateBlocks.Length, workerCount, 0); + var kinematicIntegrationWorkerStart = GetUniformlyDistributedStart(workerIndex, substepContext.KinematicIntegrationBlocks.Length, workerCount, 0); Buffer batchStarts; ref var activeSet = ref ActiveSet; unsafe { - //stackalloc is actually a little bit slow since the localsinit behavior forces a zeroing. - //Fortunately, this executes once per thread per frame. With 32 batches, it would add... a few nanoseconds per frame. We can accept that overhead. - //This is preferred over preallocating on the heap- we might write to these values and we don't want to risk false sharing for no reason. - //A single instance of false sharing would cost far more than the overhead of zeroing out the array. var batchStartsData = stackalloc int[activeSet.Batches.Count]; batchStarts = new Buffer(batchStartsData, activeSet.Batches.Count); } - for (int batchIndex = 0; batchIndex < activeSet.Batches.Count; ++batchIndex) + GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); + for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) { - var batchOffset = batchIndex > 0 ? context.BatchBoundaries[batchIndex - 1] : 0; - var batchCount = context.BatchBoundaries[batchIndex] - batchOffset; - batchStarts[batchIndex] = GetUniformlyDistributedStart(workerIndex, batchCount, context.WorkerCount, batchOffset); + var batchOffset = batchIndex > 0 ? substepContext.ConstraintBatchBoundaries[batchIndex - 1] : 0; + var batchCount = substepContext.ConstraintBatchBoundaries[batchIndex] - batchOffset; + batchStarts[batchIndex] = GetUniformlyDistributedStart(workerIndex, batchCount, workerCount, 0); } - - int syncStage = 0; - //The claimed and unclaimed state swap after every usage of both pingpong claims buffers. - int claimedState = 1; - int unclaimedState = 0; - var bounds = context.WorkerBoundsA; - var boundsBackBuffer = context.WorkerBoundsB; - //Note that every batch has a different start position. Each covers a different subset of constraints, so they require different start locations. - //The same concept applies to the prestep- the prestep covers all constraints at once, rather than batch by batch. - var prestepStage = new PrestepStageFunction { Dt = context.Dt, InverseDt = 1f / context.Dt }; Debug.Assert(activeSet.Batches.Count > 0, "Don't dispatch if there are no constraints."); - //Technically this could mutate prestep starts, but at the moment we rebuild starts every frame anyway so it doesn't matter one way or the other. - ExecuteStage(ref prestepStage, ref context.ConstraintBlocks, ref bounds, ref boundsBackBuffer, workerIndex, 0, context.ConstraintBlocks.Blocks.Count, - ref prestepStart, ref syncStage, claimedState, unclaimedState); - GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); - claimedState ^= 1; - unclaimedState ^= 1; - var warmStartStage = new WarmStartStageFunction(); - for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) + //TODO: Every single one of these offers up the same parameters. Could avoid the need to initialize any of them. + var incrementalUpdateStage = new IncrementalUpdateStageFunction { - var batchOffset = batchIndex > 0 ? context.BatchBoundaries[batchIndex - 1] : 0; - //Don't use the warm start to guess at the solve iteration work distribution. - var workerBatchStartCopy = batchStarts[batchIndex]; - ExecuteStage(ref warmStartStage, ref context.ConstraintBlocks, ref bounds, ref boundsBackBuffer, workerIndex, batchOffset, context.BatchBoundaries[batchIndex], - ref workerBatchStartCopy, ref syncStage, claimedState, unclaimedState); - } - claimedState ^= 1; - unclaimedState ^= 1; + Dt = substepContext.Dt, + InverseDt = substepContext.InverseDt, + solver = this + }; + var integrateConstrainedKinematicsStage = new IntegrateConstrainedKinematicsStageFunction + { + Dt = substepContext.Dt, + InverseDt = substepContext.InverseDt, + solver = this + }; + var warmstartStage = new WarmStartStep2StageFunction + { + Dt = substepContext.Dt, + InverseDt = substepContext.InverseDt, + solver = this + }; + var solveStage = new SolveStep2StageFunction + { + Dt = substepContext.Dt, + InverseDt = substepContext.InverseDt, + solver = this + }; - var solveStage = new SolveStageFunction(); - for (int iterationIndex = 0; iterationIndex < iterationCount; ++iterationIndex) + var syncStagesPerSubstep = 2 + synchronizedBatchCount * (1 + VelocityIterationCount); + if (workerIndex == 0) { - for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) + //This is the main 'orchestrator' thread. It tracks execution progress and notifies other threads that's it's time to work. + int syncIndex = 0; + for (int substepIndex = 0; substepIndex < substepCount; ++substepIndex) { - var batchOffset = batchIndex > 0 ? context.BatchBoundaries[batchIndex - 1] : 0; - ExecuteStage(ref solveStage, ref context.ConstraintBlocks, ref bounds, ref boundsBackBuffer, workerIndex, batchOffset, context.BatchBoundaries[batchIndex], - ref batchStarts[batchIndex], ref syncStage, claimedState, unclaimedState); + //Note that the main thread's view of the sync index increments every single dispatch, even if there is no work. + //This ensures that the workers are able to advance to the appropriate stage by examining the sync index snapshot. + ++syncIndex; + if (substepIndex > 0) + { + ExecuteMainStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStart, ref substepContext.Stages[0], GetPreviousSyncIndexForIncrementalUpdate(substepIndex, syncIndex, syncStagesPerSubstep), syncIndex); + } + //Note that we do not invoke velocity integration on the first substep if kinematics do not need velocity integration. + ++syncIndex; + if (substepIndex > 0 || PoseIntegrator.Callbacks.IntegrateVelocityForKinematics) + { + integrateConstrainedKinematicsStage.SubstepIndex = substepIndex; + ExecuteMainStage(ref integrateConstrainedKinematicsStage, workerIndex, kinematicIntegrationWorkerStart, ref substepContext.Stages[1], GetPreviousSyncIndexForIntegrateConstrainedKinematics(substepIndex, syncIndex, syncStagesPerSubstep), syncIndex); + } + warmstartStage.SubstepIndex = substepIndex; + for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) + { + ++syncIndex; + ExecuteMainStage(ref warmstartStage, workerIndex, batchStarts[batchIndex], ref substepContext.Stages[batchIndex + 2], GetPreviousSyncIndexForWarmStart(syncIndex, synchronizedBatchCount), syncIndex); + } + if (fallbackExists) + { + //The fallback runs only on the main thread. + ref var batch = ref activeSet.Batches[FallbackBatchThreshold]; + ref var integrationFlagsForBatch = ref integrationFlags[FallbackBatchThreshold]; + for (int j = 0; j < batch.TypeBatches.Count; ++j) + { + ref var typeBatch = ref batch.TypeBatches[j]; + if (substepIndex == 0) + { + WarmStartBlock(0, FallbackBatchThreshold, j, 0, typeBatch.BundleCount, ref typeBatch, TypeProcessors[typeBatch.TypeId], substepContext.Dt, substepContext.InverseDt); + } + else + { + WarmStartBlock(0, FallbackBatchThreshold, j, 0, typeBatch.BundleCount, ref typeBatch, TypeProcessors[typeBatch.TypeId], substepContext.Dt, substepContext.InverseDt); + } + } + } + for (int iterationIndex = 0; iterationIndex < VelocityIterationCount; ++iterationIndex) + { + for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) + { + //Note that this is using a 'different' stage by index than the worker thread if the iteration index > 1. + //That's totally fine- the warmstart/iteration stages share the same claims buffers per batch. They're redundant for the sake of easier indexing. + ++syncIndex; + ExecuteMainStage(ref solveStage, workerIndex, batchStarts[batchIndex], ref substepContext.Stages[batchIndex + 2], GetPreviousSyncIndexForSolve(syncIndex, synchronizedBatchCount), syncIndex); + } + if (fallbackExists) + { + //The fallback runs only on the main thread. + ref var batch = ref activeSet.Batches[FallbackBatchThreshold]; + for (int j = 0; j < batch.TypeBatches.Count; ++j) + { + ref var typeBatch = ref batch.TypeBatches[j]; + TypeProcessors[typeBatch.TypeId].SolveStep2(ref typeBatch, bodies, substepContext.Dt, substepContext.InverseDt, 0, typeBatch.BundleCount); + } + } + } } - claimedState ^= 1; - unclaimedState ^= 1; + //All done; notify waiting threads to join. + Volatile.Write(ref substepContext.SyncIndex, int.MinValue); } - } - - [Conditional("DEBUG")] - void ValidateWorkBlocks(ref TTypeBatchSolveFilter filter) where TTypeBatchSolveFilter : ITypeBatchSolveFilter - { - ref var activeSet = ref ActiveSet; - int[][][] batches = new int[activeSet.Batches.Count][][]; - for (int i = 0; i < activeSet.Batches.Count; ++i) + else { - var typeBatches = batches[i] = new int[activeSet.Batches[i].TypeBatches.Count][]; - for (int j = 0; j < typeBatches.Length; ++j) + //This is a worker thread. It does not need to track execution progress; it only checks to see if there's any work that needs to be done, and if there is, does it, then goes back into a wait. + int latestCompletedSyncIndex = 0; + int syncIndexInSubstep = -1; + int substepIndex = 0; + + while (true) { - typeBatches[j] = new int[activeSet.Batches[i].TypeBatches[j].BundleCount]; + var spinWait = new LocalSpinWait(); + int syncIndex; + while (latestCompletedSyncIndex == (syncIndex = Volatile.Read(ref substepContext.SyncIndex))) + { + //No work yet available. + spinWait.SpinOnce(); + } + //Stages were set up prior to execution. Note that we don't attempt to ping pong buffers or anything; workblock claim indices monotonically increase across the execution of the solver. + //This guarantees that a worker thread can go idle and miss an arbitrary number of stages without blocking any progress. + if (syncIndex == int.MinValue) + { + //No more stages; exit the work loop. + break; + } + //Extract the job type, stage index, and substep index from the sync index. + var syncStepsSinceLast = syncIndex - latestCompletedSyncIndex; + syncIndexInSubstep += syncStepsSinceLast; + while (true) + { + if (syncIndexInSubstep >= syncStagesPerSubstep) + { + syncIndexInSubstep -= syncStagesPerSubstep; + ++substepIndex; + } + else + { + break; + } + } + //Console.WriteLine($"Worker working on sync index {syncIndex}, sync index in substep: {syncIndexInSubstep}"); + //Note that we're going to do a compare exchange that prevents any claim on work blocks that *arent* of the previous sync index, which means we need the previous sync index. + //Storing that in a reliable way is annoying, so we derive it from syncIndex. + ref var stage = ref substepContext.Stages[syncIndexInSubstep]; + //Console.WriteLine($"Worker {workerIndex} executing {stage.StageType} for sync index {syncIndex}, stage index {syncIndexInSubstep}"); + switch (stage.StageType) + { + case SolverStageType.IncrementalUpdate: + ExecuteWorkerStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStart, 0, ref stage.Claims, GetPreviousSyncIndexForIncrementalUpdate(substepIndex, syncIndex, syncStagesPerSubstep), syncIndex, ref substepContext.CompletedWorkBlockCount); + break; + case SolverStageType.IntegrateConstrainedKinematics: + integrateConstrainedKinematicsStage.SubstepIndex = substepIndex; + ExecuteWorkerStage(ref integrateConstrainedKinematicsStage, workerIndex, kinematicIntegrationWorkerStart, 0, ref stage.Claims, GetPreviousSyncIndexForIntegrateConstrainedKinematics(substepIndex, syncIndex, syncStagesPerSubstep), syncIndex, ref substepContext.CompletedWorkBlockCount); + break; + case SolverStageType.WarmStart: + warmstartStage.SubstepIndex = substepIndex; + ExecuteWorkerStage(ref warmstartStage, workerIndex, batchStarts[stage.BatchIndex], stage.WorkBlockStartIndex, ref stage.Claims, GetPreviousSyncIndexForWarmStart(syncIndex, synchronizedBatchCount), syncIndex, ref substepContext.CompletedWorkBlockCount); + break; + case SolverStageType.Solve: + ExecuteWorkerStage(ref solveStage, workerIndex, batchStarts[stage.BatchIndex], stage.WorkBlockStartIndex, ref stage.Claims, GetPreviousSyncIndexForSolve(syncIndex, synchronizedBatchCount), syncIndex, ref substepContext.CompletedWorkBlockCount); + break; + } + latestCompletedSyncIndex = syncIndex; + } } - for (int blockIndex = 0; blockIndex < context.ConstraintBlocks.Blocks.Count; ++blockIndex) + } + + Buffer BuildKinematicIntegrationWorkBlocks(int minimumBlockSizeInBundles, int maximumBlockSizeInBundles, int targetBlockCount) + { + var bundleCount = BundleIndexing.GetBundleCount(ConstrainedKinematicHandles.Count); + if (bundleCount > 0) { - ref var block = ref context.ConstraintBlocks.Blocks[blockIndex]; - for (int bundleIndex = block.StartBundle; bundleIndex < block.End; ++bundleIndex) + var targetBundlesPerBlock = bundleCount / targetBlockCount; + if (targetBundlesPerBlock < minimumBlockSizeInBundles) + targetBundlesPerBlock = minimumBlockSizeInBundles; + if (targetBundlesPerBlock > maximumBlockSizeInBundles) + targetBundlesPerBlock = maximumBlockSizeInBundles; + var blockCount = (bundleCount + targetBundlesPerBlock - 1) / targetBundlesPerBlock; + var bundlesPerBlock = bundleCount / blockCount; + var remainder = bundleCount - bundlesPerBlock * blockCount; + var previousEnd = 0; + pool.Take(blockCount, out Buffer workBlocks); + for (int i = 0; i < blockCount; ++i) { - ref var visitedCount = ref batches[block.BatchIndex][block.TypeBatchIndex][bundleIndex]; - ++visitedCount; - Debug.Assert(visitedCount == 1); + var bundleCountForBlock = bundlesPerBlock; + if (i < remainder) + ++bundleCountForBlock; + workBlocks[i] = new IntegrationWorkBlock { StartBundleIndex = previousEnd, EndBundleIndex = previousEnd += bundleCountForBlock }; } + return workBlocks; + } + return default; + } - for (int batchIndex = 0; batchIndex < batches.Length; ++batchIndex) + protected unsafe void BuildWorkBlocks( + BufferPool pool, int minimumBlockSizeInBundles, int maximumBlockSizeInBundles, int targetBlocksPerBatch, ref TTypeBatchFilter typeBatchFilter, + out QuickList workBlocks, out Buffer batchBoundaries) where TTypeBatchFilter : ITypeBatchSolveFilter + { + ref var activeSet = ref ActiveSet; + int batchCount; + if (typeBatchFilter.IncludeFallbackBatchForWorkBlocks) + { + batchCount = activeSet.Batches.Count; + } + else + { + GetSynchronizedBatchCount(out batchCount, out _); + } + workBlocks = new QuickList(targetBlocksPerBatch * batchCount, pool); + pool.Take(batchCount, out batchBoundaries); + var inverseMinimumBlockSizeInBundles = 1f / minimumBlockSizeInBundles; + var inverseMaximumBlockSizeInBundles = 1f / maximumBlockSizeInBundles; + for (int batchIndex = 0; batchIndex < batchCount; ++batchIndex) { - for (int typeBatchIndex = 0; typeBatchIndex < batches[batchIndex].Length; ++typeBatchIndex) + ref var typeBatches = ref activeSet.Batches[batchIndex].TypeBatches; + var bundleCount = 0; + for (int typeBatchIndex = 0; typeBatchIndex < typeBatches.Count; ++typeBatchIndex) { - ref var typeBatch = ref ActiveSet.Batches[batchIndex].TypeBatches[typeBatchIndex]; - if (filter.AllowType(typeBatch.TypeId)) + if (typeBatchFilter.AllowType(typeBatches[typeBatchIndex].TypeId)) { - for (int constraintIndex = 0; constraintIndex < batches[batchIndex][typeBatchIndex].Length; ++constraintIndex) - { - Debug.Assert(batches[batchIndex][typeBatchIndex][constraintIndex] == 1); - } + bundleCount += typeBatches[typeBatchIndex].BundleCount; + } + } + for (int typeBatchIndex = 0; typeBatchIndex < typeBatches.Count; ++typeBatchIndex) + { + ref var typeBatch = ref typeBatches[typeBatchIndex]; + if (!typeBatchFilter.AllowType(typeBatch.TypeId)) + { + continue; + } + var typeBatchSizeFraction = typeBatch.BundleCount / (float)bundleCount; //note: pre-inverting this doesn't necessarily work well due to numerical issues. + var typeBatchMaximumBlockCount = typeBatch.BundleCount * inverseMinimumBlockSizeInBundles; + var typeBatchMinimumBlockCount = typeBatch.BundleCount * inverseMaximumBlockSizeInBundles; + var typeBatchBlockCount = Math.Max(1, (int)Math.Min(typeBatchMaximumBlockCount, Math.Max(typeBatchMinimumBlockCount, targetBlocksPerBatch * typeBatchSizeFraction))); + int previousEnd = 0; + var baseBlockSizeInBundles = typeBatch.BundleCount / typeBatchBlockCount; + var remainder = typeBatch.BundleCount - baseBlockSizeInBundles * typeBatchBlockCount; + for (int newBlockIndex = 0; newBlockIndex < typeBatchBlockCount; ++newBlockIndex) + { + ref var block = ref workBlocks.Allocate(pool); + var blockBundleCount = newBlockIndex < remainder ? baseBlockSizeInBundles + 1 : baseBlockSizeInBundles; + block.BatchIndex = batchIndex; + block.TypeBatchIndex = typeBatchIndex; + block.StartBundle = previousEnd; + block.End = previousEnd + blockBundleCount; + previousEnd = block.End; + Debug.Assert(block.StartBundle >= 0 && block.StartBundle < typeBatch.BundleCount); + Debug.Assert(block.End >= block.StartBundle + Math.Min(minimumBlockSizeInBundles, typeBatch.BundleCount) && block.End <= typeBatch.BundleCount); } } + batchBoundaries[batchIndex] = workBlocks.Count; } - } - protected void ExecuteMultithreaded(float dt, IThreadDispatcher threadDispatcher, Action workDelegate, bool includeIncrementalUpdate = false) where TTypeBatchSolveFilter : struct, ITypeBatchSolveFilter + //Buffer> debugStageWorkBlocksCompleted; + protected void ExecuteMultithreaded2(float dt, IThreadDispatcher threadDispatcher, Action workDelegate) { - var filter = default(TTypeBatchSolveFilter); - var workerCount = context.WorkerCount = threadDispatcher.ThreadCount; - context.WorkerCompletedCount = 0; - context.Dt = dt; + var workerCount = substepContext.WorkerCount = threadDispatcher.ThreadCount; + substepContext.Dt = dt; + substepContext.InverseDt = 1f / dt; //First build a set of work blocks. //The block size should be relatively small to give the workstealer something to do, but we don't want to go crazy with the number of blocks. //These values are found by empirical tuning. The optimal values may vary by architecture. //The goal here is to have just enough blocks that, in the event that we end up some underpowered threads (due to competition or hyperthreading), //there are enough blocks that workstealing will still generally allow the extra threads to be useful. - const int targetBlocksPerBatchPerWorker = 1; + const int targetBlocksPerBatchPerWorker = 4; const int minimumBlockSizeInBundles = 1; const int maximumBlockSizeInBundles = 1024; var targetBlocksPerBatch = workerCount * targetBlocksPerBatchPerWorker; - BuildWorkBlocks(pool, minimumBlockSizeInBundles, maximumBlockSizeInBundles, targetBlocksPerBatch, ref filter, out context.ConstraintBlocks.Blocks, out context.BatchBoundaries); - if (includeIncrementalUpdate) + var mainFilter = new MainSolveFilter(); + var incrementalFilter = new IncrementalContactDataUpdateFilter(); + BuildWorkBlocks(pool, minimumBlockSizeInBundles, maximumBlockSizeInBundles, targetBlocksPerBatch, ref mainFilter, out var constraintBlocks, out substepContext.ConstraintBatchBoundaries); + BuildWorkBlocks(pool, minimumBlockSizeInBundles, maximumBlockSizeInBundles, targetBlocksPerBatch, ref incrementalFilter, out var incrementalBlocks, out var incrementalUpdateBatchBoundaries); + pool.Return(ref incrementalUpdateBatchBoundaries); //TODO: No need to create this in the first place. Doesn't really cost anything, but... + substepContext.ConstraintBlocks = constraintBlocks.Span.Slice(constraintBlocks.Count); + substepContext.IncrementalUpdateBlocks = incrementalBlocks.Span.Slice(incrementalBlocks.Count); + substepContext.KinematicIntegrationBlocks = BuildKinematicIntegrationWorkBlocks(minimumBlockSizeInBundles, maximumBlockSizeInBundles, targetBlocksPerBatch); + + //Not every batch will actually have work blocks associated with it; the batch compressor could be falling behind, which means older constraints could be at higher batches than they need to be, leaving gaps. + //We don't want to include those empty batches as sync points in the solver. + var batchCount = ActiveSet.Batches.Count; + substepContext.SyncIndex = 0; + var totalConstraintBatchWorkBlockCount = substepContext.ConstraintBatchBoundaries.Length == 0 ? 0 : substepContext.ConstraintBatchBoundaries[^1]; + var totalClaimCount = incrementalBlocks.Count + substepContext.KinematicIntegrationBlocks.Length + totalConstraintBatchWorkBlockCount; + GetSynchronizedBatchCount(out var stagesPerIteration, out var fallbackExists); + pool.Take(2 + stagesPerIteration * (1 + VelocityIterationCount), out substepContext.Stages); + //Claims will be monotonically increasing throughout execution. All should start at zero to match with the initial sync index. + pool.Take(totalClaimCount, out var claims); + claims.Clear(0, claims.Length); + substepContext.Stages[0] = new(claims.Slice(incrementalBlocks.Count), 0, SolverStageType.IncrementalUpdate); + substepContext.Stages[1] = new(claims.Slice(incrementalBlocks.Count, substepContext.KinematicIntegrationBlocks.Length), 0, SolverStageType.IntegrateConstrainedKinematics); + //Note that we create redundant stages that share the same workblock targets and claims buffers. + //This is just to make indexing a little simpler during the multithreaded work. + int targetStageIndex = 2; + //Warm start. + var preambleClaimCount = incrementalBlocks.Count + substepContext.KinematicIntegrationBlocks.Length; + int claimStart = preambleClaimCount; + for (int batchIndex = 0; batchIndex < stagesPerIteration; ++batchIndex) { - //This looks a little weird- having a filter, which is sometimes the incremental filter, as the main filter- then having this internal filter that's conditionally used... - //It's just a hack to deal with the fact that we're in the middle of a fairly major restructuring in the solver. Once the old style incremental update is gone, the weirdness can be purged. - var incrementalFilter = new IncrementalContactDataUpdateFilter(); - BuildWorkBlocks(pool, minimumBlockSizeInBundles, maximumBlockSizeInBundles, targetBlocksPerBatch, ref incrementalFilter, out context.IncrementalUpdateBlocks.Blocks, out context.IncrementalUpdateBatchBoundaries); + var stageIndex = targetStageIndex++; + var batchStart = batchIndex == 0 ? 0 : substepContext.ConstraintBatchBoundaries[batchIndex - 1]; + var workBlocksInBatch = substepContext.ConstraintBatchBoundaries[batchIndex] - batchStart; + substepContext.Stages[stageIndex] = new(claims.Slice(claimStart, workBlocksInBatch), batchStart, SolverStageType.WarmStart, batchIndex); + claimStart += workBlocksInBatch; } - ValidateWorkBlocks(ref filter); - - //Note the clear; the block claims must be initialized to 0 so that the first worker stage knows that the data is available to claim. - context.ConstraintBlocks.CreateClaims(pool); - if (includeIncrementalUpdate) - context.IncrementalUpdateBlocks.CreateClaims(pool); - pool.Take(workerCount, out context.WorkerBoundsA); - pool.Take(workerCount, out context.WorkerBoundsB); - //The worker bounds front buffer should be initialized to avoid trash interval data from messing up the workstealing. - //The worker bounds back buffer will be cleared by the worker before moving on to the next stage. - for (int i = 0; i < workerCount; ++i) + for (int iterationIndex = 0; iterationIndex < VelocityIterationCount; ++iterationIndex) { - context.WorkerBoundsA[i] = new WorkerBounds { Min = int.MaxValue, Max = int.MinValue }; + //Solve. Note that we're reusing the same claims as were used in the warm start for these stages; the stages just tell the workers what kind of work to do. + claimStart = preambleClaimCount; + for (int batchIndex = 0; batchIndex < stagesPerIteration; ++batchIndex) + { + var stageIndex = targetStageIndex++; + var batchStart = batchIndex == 0 ? 0 : substepContext.ConstraintBatchBoundaries[batchIndex - 1]; + var workBlocksInBatch = substepContext.ConstraintBatchBoundaries[batchIndex] - batchStart; + substepContext.Stages[stageIndex] = new(claims.Slice(claimStart, workBlocksInBatch), batchStart, SolverStageType.Solve, batchIndex); + claimStart += workBlocksInBatch; + } } - pool.Take(2, out context.SyncStageWorkCounters); - context.SyncStageWorkCounters[0] = -1; - context.SyncStageWorkCounters[1] = -1; + //for (int i = 0; i < iterationCountPlusOne; ++i) + //{ + // int claimStart = incrementalBlocks.Count; + // for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) + // { + // var stageIndex = targetStageIndex++; + // var batchStart = batchIndex == 0 ? 0 : substepContext.ConstraintBatchBoundaries[batchIndex - 1]; + // var workBlocksInBatch = substepContext.ConstraintBatchBoundaries[batchIndex] - batchStart; + // substepContext.Stages[stageIndex] = new(claims.Slice(claimStart, workBlocksInBatch), batchStart, batchIndex); + // claimStart += workBlocksInBatch; + // } + //} + + //var syncCount = substepCount * (1 + synchronizedBatchCount * (1 + IterationCount)) - 1; + //pool.Take(syncCount, out debugStageWorkBlocksCompleted); + //pool.Take(syncCount * workerCount, out var workBlocksCompleted); + //workBlocksCompleted.Clear(0, workBlocksCompleted.Length); + //for (int i = 0; i < syncCount; ++i) + //{ + // debugStageWorkBlocksCompleted[i] = workBlocksCompleted.Slice(i * workerCount, workerCount); + //} //While we could be a little more aggressive about culling work with this condition, it doesn't matter much. Have to do it for correctness; worker relies on it. if (ActiveSet.Batches.Count > 0) + { + //workDelegate(0); threadDispatcher.DispatchWorkers(workDelegate); + } + + //pool.Take(syncCount, out var availableCountPerSync); + //var syncIndex = 0; + //for (int substepIndex = 0; substepIndex < substepCount; ++substepIndex) + //{ + // if (substepIndex > 0) + // { + // availableCountPerSync[syncIndex] = incrementalBlocks.Count; + // ++syncIndex; + // } + // for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) + // { + // var batchStart = batchIndex == 0 ? 0 : substepContext.ConstraintBatchBoundaries[batchIndex - 1]; + // var workBlocksInBatch = substepContext.ConstraintBatchBoundaries[batchIndex] - batchStart; + // availableCountPerSync[syncIndex] = workBlocksInBatch; + // ++syncIndex; + // } + // for (int i = 0; i < IterationCount; ++i) + // { + // for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) + // { + // var batchStart = batchIndex == 0 ? 0 : substepContext.ConstraintBatchBoundaries[batchIndex - 1]; + // var workBlocksInBatch = substepContext.ConstraintBatchBoundaries[batchIndex] - batchStart; + // availableCountPerSync[syncIndex] = workBlocksInBatch; + // ++syncIndex; + // } + // } + //} + + //pool.Take(workerCount, out var workerBlocksCompletedSums); + //pool.Take(workerCount, out var workerFractionSum); + //workerBlocksCompletedSums.Clear(0, workerBlocksCompletedSums.Length); + //var availableCountSum = 0; + //for (int i = 0; i < syncCount; ++i) + //{ + // if (availableCountPerSync[i] <= 1) + // continue; + // Console.WriteLine($"Sync {i}, available {availableCountPerSync[i]}, ideal {(availableCountPerSync[i] / (double)workerCount):G2}:"); + // var stageWorkerBlockCounts = debugStageWorkBlocksCompleted[i]; + // for (int j = 0; j < workerCount; ++j) + // { + // workerBlocksCompletedSums[j] += stageWorkerBlockCounts[j]; + // var expectedCount = availableCountPerSync[i] / (double)workerCount; + // if (j >= availableCountPerSync[i]) + // workerFractionSum[j] += 1; + // else + // workerFractionSum[j] += stageWorkerBlockCounts[j] / expectedCount; + // Console.WriteLine($"{j}: {stageWorkerBlockCounts[j]}"); + // } + // availableCountSum += availableCountPerSync[i]; + //} + ////var idealOccupancy = 1.0 / workerCount; + ////Console.WriteLine($"Worker occupancy (ideal {idealOccupancy}):"); + ////for (int i = 0; i < workerCount; ++i) + ////{ + //// //Console.WriteLine($"{i}: {(workerBlocksCompletedSums[i] / (double)availableCountSum):G3}"); + //// Console.WriteLine($"{i}: {workerFractionSum[i] / syncCount:G3}"); + ////} + + //pool.Return(ref workerBlocksCompletedSums); + //pool.Return(ref workBlocksCompleted); + //pool.Return(ref debugStageWorkBlocksCompleted); + //pool.Return(ref availableCountPerSync); + + pool.Return(ref claims); + pool.Return(ref substepContext.Stages); + pool.Return(ref substepContext.ConstraintBatchBoundaries); + pool.Return(ref substepContext.IncrementalUpdateBlocks); + if (substepContext.KinematicIntegrationBlocks.Allocated) + pool.Return(ref substepContext.KinematicIntegrationBlocks); + pool.Return(ref substepContext.ConstraintBlocks); + + - context.ConstraintBlocks.Dispose(pool); - if (includeIncrementalUpdate) - context.IncrementalUpdateBlocks.Dispose(pool); - pool.Return(ref context.BatchBoundaries); - pool.Return(ref context.WorkerBoundsA); - pool.Return(ref context.WorkerBoundsB); - pool.Return(ref context.SyncStageWorkCounters); } + struct IsFallbackBatch { } + struct IsNotFallbackBatch { } + unsafe bool ComputeIntegrationResponsibilitiesForConstraintRegion(int batchIndex, int typeBatchIndex, int constraintStart, int exclusiveConstraintEnd) where TFallbackness : unmanaged + { + ref var firstObservedForBatch = ref bodiesFirstObservedInBatches[batchIndex]; + ref var integrationFlagsForTypeBatch = ref integrationFlags[batchIndex][typeBatchIndex]; + ref var typeBatch = ref ActiveSet.Batches[batchIndex].TypeBatches[typeBatchIndex]; + var typeBatchBodyReferences = typeBatch.BodyReferences.As(); + var bodiesPerConstraintInTypeBatch = TypeProcessors[typeBatch.TypeId].BodiesPerConstraint; + var intsPerBundle = Vector.Count * bodiesPerConstraintInTypeBatch; + var bundleStartIndex = constraintStart / Vector.Count; + var bundleEndIndex = (exclusiveConstraintEnd + Vector.Count - 1) / Vector.Count; + Debug.Assert(bundleStartIndex >= 0 && bundleEndIndex <= typeBatch.BundleCount); + ref var activeSet = ref bodies.ActiveSet; + + for (int bundleIndex = bundleStartIndex; bundleIndex < bundleEndIndex; ++bundleIndex) + { + int bundleStartIndexInConstraints = bundleIndex * Vector.Count; + int countInBundle = Math.Min(Vector.Count, typeBatch.ConstraintCount - bundleStartIndexInConstraints); + //Body references are stored in AOSOA layout. + var bundleBodyReferencesStart = typeBatchBodyReferences.Memory + bundleIndex * intsPerBundle; + for (int bodyIndexInConstraint = 0; bodyIndexInConstraint < bodiesPerConstraintInTypeBatch; ++bodyIndexInConstraint) + { + ref var integrationFlagsForBodyInConstraint = ref integrationFlagsForTypeBatch[bodyIndexInConstraint]; + var bundleStart = bundleBodyReferencesStart + bodyIndexInConstraint * Vector.Count; + for (int bundleInnerIndex = 0; bundleInnerIndex < countInBundle; ++bundleInnerIndex) + { + //Constraints refer to bodies by index when they're in the active set, so we need to transform to handle to look up our merged batch results. + int bodyIndex; + if (typeof(TFallbackness) == typeof(IsFallbackBatch)) + { + //Fallback batches can contain empty lanes; there's no guarantee of constraint contiguity. Such lanes are marked with -1 in the body references. + //Just skip over them. + var rawBodyIndex = bundleStart[bundleInnerIndex]; + if (rawBodyIndex == -1) + { + continue; + } + bodyIndex = rawBodyIndex & Bodies.BodyReferenceMask; + } + else + { + bodyIndex = bundleStart[bundleInnerIndex] & Bodies.BodyReferenceMask; + } + var bodyHandle = activeSet.IndexToHandle[bodyIndex].Value; + if (firstObservedForBatch.Contains(bodyHandle)) + { + if (typeof(TFallbackness) == typeof(IsFallbackBatch)) + { + //This is a fallback. Being contained is not sufficient to require integration; it must also be the *first* constraint that will be executed. + //This is guaranteed by the index of the constraint in the type batch, and the type batch's index. + //Note that, since the fallback batch was the first time this body was seen, we know that *all* constraints associated with this body must be in the fallback batch. + //This could be significantly optimized by not recalculating the earliest candidate every single time a body is encountered in the fallback batch, but + //the fallback batch should effectively *never* contain any integration responsibilities. + ulong earliestIndex = ulong.MaxValue; + ref var constraintsForBody = ref activeSet.Constraints[bodyIndex]; + for (int constraintIndexInBody = 0; constraintIndexInBody < constraintsForBody.Count; ++constraintIndexInBody) + { + ref var fallbackBatch = ref ActiveSet.Batches[FallbackBatchThreshold]; + ref var location = ref HandleToConstraint[constraintsForBody[constraintIndexInBody].ConnectingConstraintHandle.Value]; + var typeBatchIndexForCandidate = fallbackBatch.TypeIndexToTypeBatchIndex[location.TypeId]; + var candidate = ((ulong)typeBatchIndexForCandidate << 32) | (uint)location.IndexInTypeBatch; + if (candidate < earliestIndex) + earliestIndex = candidate; + } + var indexInTypeBatch = bundleStartIndexInConstraints + bundleInnerIndex; + var currentSlot = ((ulong)typeBatchIndex << 32) | (uint)indexInTypeBatch; + if (currentSlot == earliestIndex) + { + integrationFlagsForBodyInConstraint.AddUnsafely(indexInTypeBatch); + } + } + else + { + //Not a fallback; being contained in the observed set is sufficient. + integrationFlagsForBodyInConstraint.AddUnsafely(bundleStartIndexInConstraints + bundleInnerIndex); + } + } + } + } + } + //Precompute which type batches have *any* integration responsibilities, allowing us to use a all-or-nothing test before dispatching a workblock. + //Note that this could be vectorized the same way we did in the batch merging, but... less likely to be useful. Less constraints in sequence in type batches, + //and we're already within a multithreaded context. Saving 1 microsecond not terribly meaningful. + var flagBundleCount = IndexSet.GetBundleCapacity(typeBatch.ConstraintCount); + ulong mergedFlagBundles = 0; + for (int bodyIndexInConstraint = 0; bodyIndexInConstraint < bodiesPerConstraintInTypeBatch; ++bodyIndexInConstraint) + { + ref var integrationFlagsForBodyInTypeBatch = ref integrationFlagsForTypeBatch[bodyIndexInConstraint]; + for (int i = 0; i < flagBundleCount; ++i) + { + mergedFlagBundles |= integrationFlagsForBodyInTypeBatch.Flags[i]; + } + } + return mergedFlagBundles != 0; + } - public void Solve(float dt, IThreadDispatcher threadDispatcher = null) + void ConstraintIntegrationResponsibilitiesWorker(int workerIndex) { - if (threadDispatcher == null) + int jobIndex; + while ((jobIndex = Interlocked.Increment(ref nextConstraintIntegrationResponsibilityJobIndex) - 1) < integrationResponsibilityPrepassJobs.Count) { - var inverseDt = 1f / dt; - ref var activeSet = ref ActiveSet; - var batchCount = activeSet.Batches.Count; - for (int i = 0; i < batchCount; ++i) + ref var job = ref integrationResponsibilityPrepassJobs[jobIndex]; + if (job.batch == FallbackBatchThreshold) + jobAlignedIntegrationResponsibilities[jobIndex] = ComputeIntegrationResponsibilitiesForConstraintRegion(job.batch, job.typeBatch, job.start, job.end); + else + jobAlignedIntegrationResponsibilities[jobIndex] = ComputeIntegrationResponsibilitiesForConstraintRegion(job.batch, job.typeBatch, job.start, job.end); + } + } + + int nextConstraintIntegrationResponsibilityJobIndex; + QuickList<(int batch, int typeBatch, int start, int end)> integrationResponsibilityPrepassJobs; + Buffer jobAlignedIntegrationResponsibilities; + Buffer bodiesFirstObservedInBatches; + Buffer>> integrationFlags; + /// + /// Caches a single bool for whether type batches within batches have constraints with any integration responsibilities. + /// Type batches with no integration responsibilities can use a codepath with no integration checks at all. + /// + Buffer> coarseBatchIntegrationResponsibilities; + Action constraintIntegrationResponsibilitiesWorker; + IndexSet mergedConstrainedBodyHandles; + + public override unsafe IndexSet PrepareConstraintIntegrationResponsibilities(IThreadDispatcher threadDispatcher = null) + { + if (ActiveSet.Batches.Count > 0) + { + pool.Take(ActiveSet.Batches.Count, out integrationFlags); + integrationFlags[0] = default; + pool.Take(ActiveSet.Batches.Count, out coarseBatchIntegrationResponsibilities); + for (int batchIndex = 1; batchIndex < integrationFlags.Length; ++batchIndex) { - ref var batch = ref activeSet.Batches[i]; - for (int j = 0; j < batch.TypeBatches.Count; ++j) + ref var batch = ref ActiveSet.Batches[batchIndex]; + ref var flagsForBatch = ref integrationFlags[batchIndex]; + pool.Take(batch.TypeBatches.Count, out flagsForBatch); + pool.Take(batch.TypeBatches.Count, out coarseBatchIntegrationResponsibilities[batchIndex]); + for (int typeBatchIndex = 0; typeBatchIndex < flagsForBatch.Length; ++typeBatchIndex) { - ref var typeBatch = ref batch.TypeBatches[j]; - TypeProcessors[typeBatch.TypeId].Prestep(ref typeBatch, bodies, dt, inverseDt, 0, typeBatch.BundleCount); + ref var flagsForTypeBatch = ref flagsForBatch[typeBatchIndex]; + ref var typeBatch = ref batch.TypeBatches[typeBatchIndex]; + var bodiesPerConstraint = TypeProcessors[typeBatch.TypeId].BodiesPerConstraint; + pool.Take(bodiesPerConstraint, out flagsForTypeBatch); + for (int bodyIndexInConstraint = 0; bodyIndexInConstraint < bodiesPerConstraint; ++bodyIndexInConstraint) + { + flagsForTypeBatch[bodyIndexInConstraint] = new IndexSet(pool, typeBatch.ConstraintCount); + } } } - for (int i = 0; i < batchCount; ++i) + + //Brute force fallback for debugging: + //for (int i = 0; i < bodies.ActiveSet.Count; ++i) + //{ + // ref var constraints = ref bodies.ActiveSet.Constraints[i]; + // ConstraintHandle minimumConstraint; + // minimumConstraint.Value = -1; + // int minimumBatchIndex = int.MaxValue; + // int minimumIndexInConstraint = -1; + // for (int j = 0; j < constraints.Count; ++j) + // { + // ref var constraint = ref constraints[j]; + // var batchIndex = HandleToConstraint[constraint.ConnectingConstraintHandle.Value].BatchIndex; + // if (batchIndex < minimumBatchIndex) + // { + // minimumBatchIndex = batchIndex; + // minimumIndexInConstraint = constraint.BodyIndexInConstraint; + // minimumConstraint = constraint.ConnectingConstraintHandle; + // } + // } + // if (minimumConstraint.Value >= 0) + // { + // ref var location = ref HandleToConstraint[minimumConstraint.Value]; + // var typeBatchIndex = ActiveSet.Batches[location.BatchIndex].TypeIndexToTypeBatchIndex[location.TypeId]; + // ref var indexSet = ref integrationFlags[location.BatchIndex][typeBatchIndex][minimumIndexInConstraint]; + // indexSet.AddUnsafely(location.IndexInTypeBatch); + // } + //} + + pool.Take(batchReferencedHandles.Count, out bodiesFirstObservedInBatches); + //We don't have to consider the first batch, since we know ahead of time that the first batch will be the first time we see any bodies in it. + //Just copy directly from the first batch into the merged to initialize it. + //Note "+ 64" instead of "+ 63": the highest possibly claimed id is inclusive! + pool.Take((bodies.HandlePool.HighestPossiblyClaimedId + 64) / 64, out mergedConstrainedBodyHandles.Flags); + var copyLength = Math.Min(mergedConstrainedBodyHandles.Flags.Length, batchReferencedHandles[0].Flags.Length); + batchReferencedHandles[0].Flags.CopyTo(0, mergedConstrainedBodyHandles.Flags, 0, copyLength); + batchReferencedHandles[0].Flags.Clear(copyLength, batchReferencedHandles[0].Flags.Length - copyLength); + + //Yup, we're just leaving the first slot unallocated to avoid having to offset indices all over the place. Slight wonk, but not a big deal. + bodiesFirstObservedInBatches[0] = default; + pool.Take(batchReferencedHandles.Count, out var batchHasAnyIntegrationResponsibilities); + for (int batchIndex = 1; batchIndex < bodiesFirstObservedInBatches.Length; ++batchIndex) + { + ref var batchHandles = ref batchReferencedHandles[batchIndex]; + var bundleCount = Math.Min(mergedConstrainedBodyHandles.Flags.Length, batchHandles.Flags.Length); + //Note that we bypass the constructor to avoid zeroing unnecessarily. Every bundle will be fully assigned. + pool.Take(bundleCount, out bodiesFirstObservedInBatches[batchIndex].Flags); + } + GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); + //Note that we are not multithreading the batch merging phase. This typically takes a handful of microseconds. + //You'd likely need millions of bodies before you'd see any substantial benefit from multithreading this. + var batchCount = ActiveSet.Batches.Count; + for (int batchIndex = 1; batchIndex < batchCount; ++batchIndex) { - ref var batch = ref activeSet.Batches[i]; - for (int j = 0; j < batch.TypeBatches.Count; ++j) + ref var batchHandles = ref batchReferencedHandles[batchIndex]; + ref var firstObservedInBatch = ref bodiesFirstObservedInBatches[batchIndex]; + var flagBundleCount = Math.Min(mergedConstrainedBodyHandles.Flags.Length, batchHandles.Flags.Length); + + if (Avx2.IsSupported) + { + var avxBundleCount = flagBundleCount / 4; + var horizontalAvxMerge = Vector256.Zero; + for (int avxBundleIndex = 0; avxBundleIndex < avxBundleCount; ++avxBundleIndex) + { + //These will *almost* always be aligned, but guaranteeing it is not worth the complexity. + var mergeBundle = Avx2.LoadVector256((ulong*)((Vector256*)mergedConstrainedBodyHandles.Flags.Memory + avxBundleIndex)); + var batchBundle = Avx2.LoadVector256((ulong*)((Vector256*)batchHandles.Flags.Memory + avxBundleIndex)); + Avx.Store((ulong*)((Vector256*)mergedConstrainedBodyHandles.Flags.Memory + avxBundleIndex), Avx2.Or(mergeBundle, batchBundle)); + //If this batch contains a body, and the merged set does not, then it's the first batch that sees a body and it will have integration responsibility. + var firstObservedBundle = Avx2.AndNot(mergeBundle, batchBundle); + horizontalAvxMerge = Avx2.Or(firstObservedBundle, horizontalAvxMerge); + Avx.Store((ulong*)((Vector256*)firstObservedInBatch.Flags.Memory + avxBundleIndex), firstObservedBundle); + } + var notEqual = Avx2.Xor(Avx2.CompareEqual(horizontalAvxMerge, Vector256.Zero), Vector256.AllBitsSet); + ulong horizontalMerge = (ulong)Avx2.MoveMask(notEqual.AsDouble()); + + //Cleanup loop. + for (int flagBundleIndex = avxBundleCount * 4; flagBundleIndex < flagBundleCount; ++flagBundleIndex) + { + var mergeBundle = mergedConstrainedBodyHandles.Flags[flagBundleIndex]; + var batchBundle = batchHandles.Flags[flagBundleIndex]; + mergedConstrainedBodyHandles.Flags[flagBundleIndex] = mergeBundle | batchBundle; + //If this batch contains a body, and the merged set does not, then it's the first batch that sees a body and it will have integration responsibility. + var firstObservedBundle = ~mergeBundle & batchBundle; + horizontalMerge |= firstObservedBundle; + firstObservedInBatch.Flags[flagBundleIndex] = firstObservedBundle; + } + batchHasAnyIntegrationResponsibilities[batchIndex] = horizontalMerge != 0; + } + else { - ref var typeBatch = ref batch.TypeBatches[j]; - TypeProcessors[typeBatch.TypeId].WarmStart(ref typeBatch, bodies, 0, typeBatch.BundleCount); + ulong horizontalMerge = 0; + for (int flagBundleIndex = 0; flagBundleIndex < flagBundleCount; ++flagBundleIndex) + { + var mergeBundle = mergedConstrainedBodyHandles.Flags[flagBundleIndex]; + var batchBundle = batchHandles.Flags[flagBundleIndex]; + mergedConstrainedBodyHandles.Flags[flagBundleIndex] = mergeBundle | batchBundle; + //If this batch contains a body, and the merged set does not, then it's the first batch that sees a body and it will have integration responsibility. + var firstObservedBundle = ~mergeBundle & batchBundle; + horizontalMerge |= firstObservedBundle; + firstObservedInBatch.Flags[flagBundleIndex] = firstObservedBundle; + } + batchHasAnyIntegrationResponsibilities[batchIndex] = horizontalMerge != 0; } } - for (int iterationIndex = 0; iterationIndex < iterationCount; ++iterationIndex) + + //var start = Stopwatch.GetTimestamp(); + //We now have index sets representing the first time each body handle is observed in a batch. + //This process is significantly more expensive than the batch merging phase and can benefit from multithreading. + //It is still fairly cheap, though- we can't use really fine grained jobs or the cost of swapping jobs will exceed productive work. + + //Note that we arbitrarily use single threaded execution if the job is small enough. Dispatching isn't free. + bool useSingleThreadedPath = true; + if (threadDispatcher != null && threadDispatcher.ThreadCount > 1) { + integrationResponsibilityPrepassJobs = new(128, pool); + int constraintCount = 0; + const int targetJobSize = 2048; + Debug.Assert(targetJobSize % 64 == 0, "Target job size must be a multiple of the index set bundles to avoid threads working on the same flag bundle."); + for (int batchIndex = 1; batchIndex < bodiesFirstObservedInBatches.Length; ++batchIndex) + { + if (!batchHasAnyIntegrationResponsibilities[batchIndex]) + continue; + ref var batch = ref ActiveSet.Batches[batchIndex]; + for (int typeBatchIndex = 0; typeBatchIndex < batch.TypeBatches.Count; ++typeBatchIndex) + { + ref var typeBatch = ref batch.TypeBatches[typeBatchIndex]; + constraintCount += typeBatch.ConstraintCount; + int jobCountForTypeBatch = (typeBatch.ConstraintCount + targetJobSize - 1) / targetJobSize; + for (int i = 0; i < jobCountForTypeBatch; ++i) + { + var jobStart = i * targetJobSize; + var jobEnd = Math.Min(jobStart + targetJobSize, typeBatch.ConstraintCount); + integrationResponsibilityPrepassJobs.Allocate(pool) = (batchIndex, typeBatchIndex, jobStart, jobEnd); + } + } + } + if (constraintCount > 4096 + threadDispatcher.ThreadCount * 1024) + { + nextConstraintIntegrationResponsibilityJobIndex = 0; + useSingleThreadedPath = false; + pool.Take(integrationResponsibilityPrepassJobs.Count, out jobAlignedIntegrationResponsibilities); + //for (int i = 0; i < integrationResponsibilityPrepassJobs.Count; ++i) + //{ + // ref var job = ref integrationResponsibilityPrepassJobs[i]; + // jobAlignedIntegrationResponsibilities[i] = ComputeIntegrationResponsibilitiesForConstraintRegion(job.batch, job.typeBatch, job.start, job.end); + //} + threadDispatcher.DispatchWorkers(constraintIntegrationResponsibilitiesWorker); + + //Coarse batch integration responsibilities start uninitialized. Possible to have multiple jobs per type batch in multithreaded case, so we need to init to merge. + for (int i = 1; i < ActiveSet.Batches.Count; ++i) + { + ref var batch = ref ActiveSet.Batches[i]; + for (int j = 0; j < batch.TypeBatches.Count; ++j) + { + coarseBatchIntegrationResponsibilities[i][j] = false; + } + } + for (int i = 0; i < integrationResponsibilityPrepassJobs.Count; ++i) + { + ref var job = ref integrationResponsibilityPrepassJobs[i]; + coarseBatchIntegrationResponsibilities[job.batch][job.typeBatch] |= jobAlignedIntegrationResponsibilities[i]; + } + pool.Return(ref jobAlignedIntegrationResponsibilities); + } + integrationResponsibilityPrepassJobs.Dispose(pool); + } + if (useSingleThreadedPath) + { + for (int i = 1; i < synchronizedBatchCount; ++i) + { + if (!batchHasAnyIntegrationResponsibilities[i]) + continue; + ref var batch = ref ActiveSet.Batches[i]; + for (int j = 0; j < batch.TypeBatches.Count; ++j) + { + ref var typeBatch = ref batch.TypeBatches[j]; + coarseBatchIntegrationResponsibilities[i][j] = ComputeIntegrationResponsibilitiesForConstraintRegion(i, j, 0, typeBatch.ConstraintCount); + } + } + if (fallbackExists && batchHasAnyIntegrationResponsibilities[FallbackBatchThreshold]) + { + ref var batch = ref ActiveSet.Batches[FallbackBatchThreshold]; + for (int j = 0; j < batch.TypeBatches.Count; ++j) + { + ref var typeBatch = ref batch.TypeBatches[j]; + coarseBatchIntegrationResponsibilities[FallbackBatchThreshold][j] = ComputeIntegrationResponsibilitiesForConstraintRegion(FallbackBatchThreshold, j, 0, typeBatch.ConstraintCount); + } + } + } + //var end = Stopwatch.GetTimestamp(); + //Console.WriteLine($"time (ms): {(end - start) * 1e3 / Stopwatch.Frequency}"); + + ////Validation: + //for (int i = 0; i < bodies.ActiveSet.Count; ++i) + //{ + // ref var constraints = ref bodies.ActiveSet.Constraints[i]; + // ConstraintHandle minimumConstraint; + // minimumConstraint.Value = -1; + // int minimumBatchIndex = int.MaxValue; + // int minimumBodyIndexInConstraint = -1; + // ulong earliestSlotInFallback = ulong.MaxValue; + // ConstraintHandle detectedConstraint; + // detectedConstraint.Value = -1; + // for (int j = 0; j < constraints.Count; ++j) + // { + // ref var constraint = ref constraints[j]; + // ref var location = ref HandleToConstraint[constraint.ConnectingConstraintHandle.Value]; + // var typeBatchIndex = ActiveSet.Batches[location.BatchIndex].TypeIndexToTypeBatchIndex[location.TypeId]; + + // if (location.BatchIndex <= minimumBatchIndex) //Note that the only time it can be equal is if this is in the fallback batch. + // { + // if (location.BatchIndex == FallbackBatchThreshold) + // { + // var encodedSlotCandidate = ((ulong)typeBatchIndex << 32) | (uint)location.IndexInTypeBatch; + // if (encodedSlotCandidate < earliestSlotInFallback) + // { + // earliestSlotInFallback = encodedSlotCandidate; + // //We should only accept another fallback constraint as minimal if it has an earlier typebatch index/index in type batch. + // minimumBatchIndex = location.BatchIndex; + // minimumBodyIndexInConstraint = constraint.BodyIndexInConstraint; + // minimumConstraint = constraint.ConnectingConstraintHandle; + // } + // } + // else + // { + // minimumBatchIndex = location.BatchIndex; + // minimumBodyIndexInConstraint = constraint.BodyIndexInConstraint; + // minimumConstraint = constraint.ConnectingConstraintHandle; + // } + // } + + // if (location.BatchIndex > 0) + // { + // ref var indexSet = ref integrationFlags[location.BatchIndex][typeBatchIndex][constraint.BodyIndexInConstraint]; + // if (indexSet.Contains(location.IndexInTypeBatch)) + // { + // //This constraint has integration responsibility for this body. + // Debug.Assert(detectedConstraint.Value == -1, "Only one constraint should have integration responsibility for a given body."); + // detectedConstraint = constraint.ConnectingConstraintHandle; + // } + // } + // } + // if (constraints.Count > 0 && minimumBatchIndex > 0) + // Debug.Assert(detectedConstraint.Value >= 0, "At least one constraint must have integration responsibility for a body if it has a constraint."); + // if (minimumConstraint.Value >= 0) + // { + // Debug.Assert(minimumBatchIndex == 0 || detectedConstraint.Value == minimumConstraint.Value); + // ref var location = ref HandleToConstraint[minimumConstraint.Value]; + // var typeBatchIndex = ActiveSet.Batches[location.BatchIndex].TypeIndexToTypeBatchIndex[location.TypeId]; + // if (location.BatchIndex > 0) + // { + // ref var indexSet = ref integrationFlags[location.BatchIndex][typeBatchIndex][minimumBodyIndexInConstraint]; + // Debug.Assert(indexSet.Contains(location.IndexInTypeBatch)); + // } + // } + //} + + pool.Return(ref batchHasAnyIntegrationResponsibilities); + + Debug.Assert(!bodiesFirstObservedInBatches[0].Flags.Allocated, "Remember, we're assuming we're just leaving the first batch's slot empty to avoid indexing complexity."); + for (int batchIndex = 1; batchIndex < bodiesFirstObservedInBatches.Length; ++batchIndex) + { + bodiesFirstObservedInBatches[batchIndex].Dispose(pool); + } + pool.Return(ref bodiesFirstObservedInBatches); + + //Add the constrained kinematics to the constrained body handles. The kinematics were absent from batch referenced handles. + //TODO: This assumes the number of kinematics is low relative to the number of bodies and does not need to be multithreaded. + //This assumption is *usually* fine, but we should probably have a fallback that is more efficient if this assumption is wrong. + //Could maintain an indexset parallel to the ConstrainedKinematicHandles- same set, just different format. + //If we detect a lot of constrained kinematics, just do an indexset merge. + //That would be fast enough even if all bodies were kinematic. + for (int i = 0; i < ConstrainedKinematicHandles.Count; ++i) + { + mergedConstrainedBodyHandles.AddUnsafely(ConstrainedKinematicHandles[i]); + } + return mergedConstrainedBodyHandles; + } + else + { + return new IndexSet(); + } + } + public override void DisposeConstraintIntegrationResponsibilities() + { + if (ActiveSet.Batches.Count > 0) + { + Debug.Assert(!integrationFlags[0].Allocated, "Remember, we're assuming we're just leaving the first batch's slot empty to avoid indexing complexity."); + for (int batchIndex = 1; batchIndex < integrationFlags.Length; ++batchIndex) + { + ref var flagsForBatch = ref integrationFlags[batchIndex]; + for (int typeBatchIndex = 0; typeBatchIndex < flagsForBatch.Length; ++typeBatchIndex) + { + ref var flagsForTypeBatch = ref flagsForBatch[typeBatchIndex]; + for (int bodyIndexInConstraint = 0; bodyIndexInConstraint < flagsForTypeBatch.Length; ++bodyIndexInConstraint) + { + flagsForTypeBatch[bodyIndexInConstraint].Dispose(pool); + } + pool.Return(ref flagsForTypeBatch); + } + pool.Return(ref flagsForBatch); + pool.Return(ref coarseBatchIntegrationResponsibilities[batchIndex]); + } + pool.Return(ref integrationFlags); + pool.Return(ref coarseBatchIntegrationResponsibilities); + mergedConstrainedBodyHandles.Dispose(pool); + } + } + + public override void Solve(float totalDt, IThreadDispatcher threadDispatcher = null) + { + var substepDt = totalDt / substepCount; + PoseIntegrator.Callbacks.PrepareForIntegration(substepDt); + if (threadDispatcher == null) + { + var inverseDt = 1f / substepDt; + ref var activeSet = ref ActiveSet; + var batchCount = activeSet.Batches.Count; + var incrementalUpdateFilter = default(IncrementalContactDataUpdateFilter); + for (int substepIndex = 0; substepIndex < substepCount; ++substepIndex) + { + if (substepIndex > 0) + { + for (int i = 0; i < batchCount; ++i) + { + ref var batch = ref activeSet.Batches[i]; + for (int j = 0; j < batch.TypeBatches.Count; ++j) + { + ref var typeBatch = ref batch.TypeBatches[j]; + if (incrementalUpdateFilter.AllowType(typeBatch.TypeId)) + TypeProcessors[typeBatch.TypeId].IncrementallyUpdateContactData(ref typeBatch, bodies, substepDt, inverseDt, 0, typeBatch.BundleCount); + } + } + PoseIntegrator.IntegrateKinematicPosesAndVelocities(ConstrainedKinematicHandles.Span.Slice(ConstrainedKinematicHandles.Count), 0, BundleIndexing.GetBundleCount(ConstrainedKinematicHandles.Count), substepDt, 0); + } + else + { + if (PoseIntegrator.Callbacks.IntegrateVelocityForKinematics) + PoseIntegrator.IntegrateKinematicVelocities(ConstrainedKinematicHandles.Span.Slice(ConstrainedKinematicHandles.Count), 0, BundleIndexing.GetBundleCount(ConstrainedKinematicHandles.Count), substepDt, 0); + } for (int i = 0; i < batchCount; ++i) { ref var batch = ref activeSet.Batches[i]; + ref var integrationFlagsForBatch = ref integrationFlags[i]; for (int j = 0; j < batch.TypeBatches.Count; ++j) { ref var typeBatch = ref batch.TypeBatches[j]; - TypeProcessors[typeBatch.TypeId].SolveIteration(ref typeBatch, bodies, 0, typeBatch.BundleCount); + if (substepIndex == 0) + { + WarmStartBlock(0, i, j, 0, typeBatch.BundleCount, ref typeBatch, TypeProcessors[typeBatch.TypeId], substepDt, inverseDt); + } + else + { + WarmStartBlock(0, i, j, 0, typeBatch.BundleCount, ref typeBatch, TypeProcessors[typeBatch.TypeId], substepDt, inverseDt); + } + } + } + for (int iterationIndex = 0; iterationIndex < VelocityIterationCount; ++iterationIndex) + { + for (int i = 0; i < batchCount; ++i) + { + ref var batch = ref activeSet.Batches[i]; + for (int j = 0; j < batch.TypeBatches.Count; ++j) + { + ref var typeBatch = ref batch.TypeBatches[j]; + TypeProcessors[typeBatch.TypeId].SolveStep2(ref typeBatch, bodies, substepDt, inverseDt, 0, typeBatch.BundleCount); + } } } } } else { - ExecuteMultithreaded(dt, threadDispatcher, solveWorker); + ExecuteMultithreaded2(substepDt, threadDispatcher, solveStep2Worker2); } } - } } diff --git a/BepuPhysics/Solver_SubsteppingSolve2.cs b/BepuPhysics/Solver_SubsteppingSolve2.cs deleted file mode 100644 index d57f533a1..000000000 --- a/BepuPhysics/Solver_SubsteppingSolve2.cs +++ /dev/null @@ -1,1284 +0,0 @@ -using BepuUtilities; -using BepuUtilities.Collections; -using BepuUtilities.Memory; -using BepuPhysics.Constraints; -using System; -using System.Collections.Generic; -using System.Diagnostics; -using System.Runtime.CompilerServices; -using System.Runtime.InteropServices; -using System.Text; -using System.Threading; -using System.Runtime.Intrinsics.X86; -using System.Numerics; -using System.Runtime.Intrinsics; -using static BepuPhysics.Solver; - -namespace BepuPhysics -{ - internal enum SolverStageType - { - IncrementalUpdate, - IntegrateConstrainedKinematics, - WarmStart, - Solve, - } - - internal struct SolverSyncStage - { - public Buffer Claims; - public int BatchIndex; - public int WorkBlockStartIndex; - public SolverStageType StageType; - - public SolverSyncStage(Buffer claims, int workBlockStartIndex, SolverStageType type, int batchIndex = -1) - { - Claims = claims; - BatchIndex = batchIndex; - WorkBlockStartIndex = workBlockStartIndex; - StageType = type; - } - } - - internal struct IntegrationWorkBlock - { - public int StartBundleIndex; - public int EndBundleIndex; - } - - [StructLayout(LayoutKind.Explicit)] - internal struct SubstepMultithreadingContext - { - [FieldOffset(0)] - public Buffer Stages; - [FieldOffset(16)] - public Buffer IncrementalUpdateBlocks; - [FieldOffset(32)] - public Buffer KinematicIntegrationBlocks; - [FieldOffset(48)] - public Buffer ConstraintBlocks; - [FieldOffset(64)] - public Buffer ConstraintBatchBoundaries; - [FieldOffset(80)] - public float Dt; - [FieldOffset(84)] - public float InverseDt; - [FieldOffset(88)] - public int WorkerCount; - - - //This index is written during multithreaded execution; don't want to infest any of the more frequently read properties, so it's shoved out of any dangerous cache line. - /// - /// Monotonically increasing index of executed stages during a frame. - /// - [FieldOffset(256)] - public int SyncIndex; - - /// - /// Counter of work completed for the current stage. - /// - [FieldOffset(384)] - public int CompletedWorkBlockCount; - } - - - public partial class Solver - { - public virtual IndexSet PrepareConstraintIntegrationResponsibilities(int substepCount, IThreadDispatcher threadDispatcher = null) - { - throw new NotImplementedException(); - } - public virtual void DisposeConstraintIntegrationResponsibilities() - { - throw new NotImplementedException(); - } - - public virtual void SolveStep2(float dt, IThreadDispatcher threadDispatcher = null) - { - throw new NotImplementedException(); - } - } - public class Solver : Solver where TIntegrationCallbacks : struct, IPoseIntegratorCallbacks - { - public Solver(Bodies bodies, BufferPool pool, int iterationCount, int fallbackBatchThreshold, - int initialCapacity, - int initialIslandCapacity, - int minimumCapacityPerTypeBatch, PoseIntegrator poseIntegrator) - : base(bodies, pool, iterationCount, fallbackBatchThreshold, initialCapacity, initialIslandCapacity, minimumCapacityPerTypeBatch) - { - PoseIntegrator = poseIntegrator; - solveStep2Worker2 = SolveStep2Worker2; - constraintIntegrationResponsibilitiesWorker = ConstraintIntegrationResponsibilitiesWorker; - } - - /// - /// Pose integrator used by the simulation. - /// - public PoseIntegrator PoseIntegrator { get; private set; } - - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - private void WarmStartBlock(int workerIndex, int batchIndex, int typeBatchIndex, int startBundle, int endBundle, ref TypeBatch typeBatch, TypeProcessor typeProcessor, float dt, float inverseDt) - where TBatchShouldIntegratePoses : unmanaged, IBatchPoseIntegrationAllowed - { - if (batchIndex == 0) - { - Buffer noFlagsRequired = default; - typeProcessor.WarmStart2( - ref typeBatch, ref noFlagsRequired, bodies, ref PoseIntegrator.Callbacks, - dt, inverseDt, startBundle, endBundle, workerIndex); - } - else - { - if (coarseBatchIntegrationResponsibilities[batchIndex][typeBatchIndex]) - { - typeProcessor.WarmStart2( - ref typeBatch, ref integrationFlags[batchIndex][typeBatchIndex], bodies, ref PoseIntegrator.Callbacks, - dt, inverseDt, startBundle, endBundle, workerIndex); - } - else - { - typeProcessor.WarmStart2( - ref typeBatch, ref integrationFlags[batchIndex][typeBatchIndex], bodies, ref PoseIntegrator.Callbacks, - dt, inverseDt, startBundle, endBundle, workerIndex); - } - } - } - - //Split the solve process into a warmstart and solve, where warmstart doesn't try to store out anything. It just computes jacobians and modifies velocities according to the accumulated impulse. - //The solve step then *recomputes* jacobians from prestep data and pose information. - //Why? Memory bandwidth. Redoing the calculation is cheaper than storing it out. - struct WarmStartStep2StageFunction : IStageFunction - { - public float Dt; - public float InverseDt; - public int SubstepIndex; - public Solver solver; - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Execute(Solver solver, int blockIndex, int workerIndex) - { - ref var block = ref this.solver.substepContext.ConstraintBlocks[blockIndex]; - ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; - var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; - if (SubstepIndex == 0) - { - this.solver.WarmStartBlock(workerIndex, block.BatchIndex, block.TypeBatchIndex, block.StartBundle, block.End, ref typeBatch, typeProcessor, Dt, InverseDt); - } - else - { - this.solver.WarmStartBlock(workerIndex, block.BatchIndex, block.TypeBatchIndex, block.StartBundle, block.End, ref typeBatch, typeProcessor, Dt, InverseDt); - } - - } - } - - struct SolveStep2StageFunction : IStageFunction - { - public float Dt; - public float InverseDt; - public Solver solver; - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Execute(Solver solver, int blockIndex, int workerIndex) - { - ref var block = ref this.solver.substepContext.ConstraintBlocks[blockIndex]; - ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; - var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; - typeProcessor.SolveStep2(ref typeBatch, solver.bodies, Dt, InverseDt, block.StartBundle, block.End); - } - } - - struct IncrementalUpdateStageFunction : IStageFunction - { - public float Dt; - public float InverseDt; - public Solver solver; - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Execute(Solver solver, int blockIndex, int workerIndex) - { - ref var block = ref this.solver.substepContext.IncrementalUpdateBlocks[blockIndex]; - ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; - solver.TypeProcessors[typeBatch.TypeId].IncrementallyUpdateContactData(ref typeBatch, solver.bodies, Dt, InverseDt, block.StartBundle, block.End); - } - } - - struct IntegrateConstrainedKinematicsStageFunction : IStageFunction - { - public float Dt; - public float InverseDt; - public int SubstepIndex; - public Solver solver; - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Execute(Solver solver, int blockIndex, int workerIndex) - { - ref var block = ref this.solver.substepContext.KinematicIntegrationBlocks[blockIndex]; - if (SubstepIndex == 0) - { - this.solver.PoseIntegrator.IntegrateKinematicVelocities(solver.ConstrainedKinematicHandles.Span.Slice(solver.ConstrainedKinematicHandles.Count), block.StartBundleIndex, block.EndBundleIndex, Dt, workerIndex); - } - else - { - this.solver.PoseIntegrator.IntegrateKinematicPosesAndVelocities(solver.ConstrainedKinematicHandles.Span.Slice(solver.ConstrainedKinematicHandles.Count), block.StartBundleIndex, block.EndBundleIndex, Dt, workerIndex); - } - } - } - - unsafe void ExecuteWorkerStage(ref TStageFunction stageFunction, int workerIndex, int workerStart, int availableBlocksStartIndex, ref Buffer claims, int previousSyncIndex, int syncIndex, ref int completedWorkBlocks) where TStageFunction : IStageFunction - { - if (workerStart == -1) - { - //Thread count exceeds work block count; nothing for this worker to do. - //(Technically, there's a possibility that an earlier thread would fail to wake and allowing this thread to steal that block COULD help, - //but on average it's better to keep jobs scheduled to the same core to avoid excess memory traffic, and we can rely on some other active worker to take care of it.) - return; - } - int workBlockIndex = workerStart; - int locallyCompletedCount = 0; - //Try to claim blocks by traversing forward until we're blocked by another claim. - while (Interlocked.CompareExchange(ref claims[workBlockIndex], syncIndex, previousSyncIndex) == previousSyncIndex) - { - //Successfully claimed a work block. - stageFunction.Execute(this, availableBlocksStartIndex + workBlockIndex, workerIndex); - ++locallyCompletedCount; - ++workBlockIndex; - if (workBlockIndex >= claims.Length) - { - //Wrap around. - workBlockIndex = 0; - } - } - //Try to claim work blocks going backward. - workBlockIndex = workerStart - 1; - while (true) - { - if (workBlockIndex < 0) - { - //Wrap around. - workBlockIndex = claims.Length - 1; - } - //Note the comparison: equal *or greater* blocks. - //Consider what happens if this thread was heavily delayed and the stage it was dispatched for has already ended. - //Other threads could be working on the next sync index. A mere equality test could result in this thread thinking there's work to be done, so it starts claiming for an *earlier* stage. - //Then everything dies. - if (Interlocked.CompareExchange(ref claims[workBlockIndex], syncIndex, previousSyncIndex) != previousSyncIndex) - { - break; - } - //Successfully claimed a work block. - stageFunction.Execute(this, availableBlocksStartIndex + workBlockIndex, workerIndex); - ++locallyCompletedCount; - workBlockIndex--; - } - //No more adjacent work blocks are available. This thread is done! - Interlocked.Add(ref completedWorkBlocks, locallyCompletedCount); - - - //debugStageWorkBlocksCompleted[syncIndex - 1][workerIndex] = locallyCompletedCount; - //if (workerIndex == 3) - //{ - //Console.WriteLine($"Worker {workerIndex}, stage {typeof(TStageFunction).Name}, sync index {syncIndex} completed {locallyCompletedCount / (double)claims.Length:G2} ({locallyCompletedCount} of {claims.Length})."); - //} - //for (int i = 0; i < claims.Length; ++i) - //{ - // if (claims[i] != syncIndex) - // { - // Console.WriteLine($"Failed to claim index {i}, claim value is {claims[i]} instead of {syncIndex}, previous claim should have been {previousSyncIndex}, worker start {workerStart}"); - // } - //} - - } - void ExecuteMainStage(ref TStageFunction stageFunction, int workerIndex, int workerStart, ref SolverSyncStage stage, int previousSyncIndex, int syncIndex) where TStageFunction : IStageFunction - { - var availableBlocksCount = stage.Claims.Length; - if (availableBlocksCount == 0) - return; - - //for (int i = 0; i < availableBlocksCount; ++i) - //{ - // stageFunction.Execute(this, stage.WorkBlockStartIndex + i, workerIndex); - //} - //return; - //Console.WriteLine($"Main executing {typeof(TStageFunction).Name} for sync index {syncIndex}, expected claim {syncIndex - previousSyncIndexOffset}"); - if (availableBlocksCount == 1) - { - //Console.WriteLine($"Main thread is executing {syncIndex} by itself; stage function: {stageFunction.GetType().Name}"); - //There is only one work block available. There's no reason to notify other threads about it or do any claims management; just execute it sequentially. - stageFunction.Execute(this, stage.WorkBlockStartIndex, workerIndex); - } - else - { - //Console.WriteLine($"Main thread is requesting workers begin for sync index {syncIndex}; stage function: {stageFunction.GetType().Name}"); - //Write the new stage index so other spinning threads will begin work on it. - Volatile.Write(ref substepContext.SyncIndex, syncIndex); - ExecuteWorkerStage(ref stageFunction, workerIndex, workerStart, stage.WorkBlockStartIndex, ref stage.Claims, previousSyncIndex, syncIndex, ref substepContext.CompletedWorkBlockCount); - - //Since we asked other threads to do work, we must wait until the requested work is done before proceeding. - //Note that we DO NOT yield on the main thread! - //This significantly increases the chance *some* progress will be made on the available work, even if all other workers are stuck unscheduled. - //The reasoning here is that the OS is not likely to unschedule an active thread, but will be far less aggressive about scheduling a *currently unscheduled* thread. - //Critically, yielding threads are not in any kind of execution queue- from the OS's perspective, they aren't asking to be woken up. - //If another thread comes in with significant work, they could be stalled for (from the solver's perspective) an arbitrarily long time. - //By having the main thread never yield, the only way for all progress to halt is for the OS to aggressively unschedule the main thread. - //That is very rare when dealing with CPUs with plenty of cores to go around relative to the scheduled work. - //(Why not notify the OS that waiting threads actually want to be executed? Just overhead. Feel free to experiment with different approaches, but so far this has won empirically.) - while (Volatile.Read(ref substepContext.CompletedWorkBlockCount) != availableBlocksCount) - { - Thread.SpinWait(3); - } - //Console.WriteLine($"Completed blocks count: {substepContext.CompletedWorkBlockCount}."); - //All workers are done. We can safely reset the counter for the next time this stage is used. - substepContext.CompletedWorkBlockCount = 0; - } - } - internal SubstepMultithreadingContext substepContext; - - - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - int GetPreviousSyncIndexForIncrementalUpdate(int substepIndex, int syncIndex, int syncStagesPerSubstep) - { - return substepIndex == 1 ? 0 : Math.Max(0, syncIndex - syncStagesPerSubstep); - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - int GetPreviousSyncIndexForIntegrateConstrainedKinematics(int substepIndex, int syncIndex, int syncStagesPerSubstep) - { - //If kinematics have their velocities integrated, then the first substep will have executed and left the claims at 1. Otherwise, the first substep will leave them cleared at 0. - //The second substep and later will always run (since kinematics need their poses integrated regardless) so their sync index isn't weirdly conditional. - return substepIndex == 1 ? PoseIntegrator.Callbacks.IntegrateVelocityForKinematics ? 2 : 0 : Math.Max(0, syncIndex - syncStagesPerSubstep); - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - int GetPreviousSyncIndexForWarmStart(int syncIndex, int synchronizedBatchCount) - { - //The claims for warmstarts and solves are shared. So we want to look back to the last solve's claims, which would be beyond the incremental update and integrate constrained kinematics. - return Math.Max(0, syncIndex - synchronizedBatchCount - 2); - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - int GetPreviousSyncIndexForSolve(int syncIndex, int synchronizedBatchCount) - { - return Math.Max(0, syncIndex - synchronizedBatchCount); - } - - Action solveStep2Worker2; - void SolveStep2Worker2(int workerIndex) - { - //The solver has two codepaths: one thread, acting as an orchestrator, and the others, just waiting to be used. - //There is no requirement that a worker thread above index 0 actually runs at all for a given dispatch. - //If a worker fails to schedule for a long time because the OS went with a different thread, that's perfectly fine- - //another thread will consume the work that would have otherwise been handled by it, and the execution as a whole - //will continue on unimpeded. - //There's still nothing done if the OS unschedules an active worker that claimed work, but that's a far, far rarer concern. - //Note that this attempts to maintain a given worker's relationship to a set of work blocks. This increases the probability that - //data will remain in some cache that's reasonably close to the core. - int workerCount = substepContext.WorkerCount; - var incrementalUpdateWorkerStart = GetUniformlyDistributedStart(workerIndex, substepContext.IncrementalUpdateBlocks.Length, workerCount, 0); - var kinematicIntegrationWorkerStart = GetUniformlyDistributedStart(workerIndex, substepContext.KinematicIntegrationBlocks.Length, workerCount, 0); - Buffer batchStarts; - ref var activeSet = ref ActiveSet; - unsafe - { - var batchStartsData = stackalloc int[activeSet.Batches.Count]; - batchStarts = new Buffer(batchStartsData, activeSet.Batches.Count); - } - GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); - for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) - { - var batchOffset = batchIndex > 0 ? substepContext.ConstraintBatchBoundaries[batchIndex - 1] : 0; - var batchCount = substepContext.ConstraintBatchBoundaries[batchIndex] - batchOffset; - batchStarts[batchIndex] = GetUniformlyDistributedStart(workerIndex, batchCount, workerCount, 0); - } - - Debug.Assert(activeSet.Batches.Count > 0, "Don't dispatch if there are no constraints."); - - //TODO: Every single one of these offers up the same parameters. Could avoid the need to initialize any of them. - var incrementalUpdateStage = new IncrementalUpdateStageFunction - { - Dt = substepContext.Dt, - InverseDt = substepContext.InverseDt, - solver = this - }; - var integrateConstrainedKinematicsStage = new IntegrateConstrainedKinematicsStageFunction - { - Dt = substepContext.Dt, - InverseDt = substepContext.InverseDt, - solver = this - }; - var warmstartStage = new WarmStartStep2StageFunction - { - Dt = substepContext.Dt, - InverseDt = substepContext.InverseDt, - solver = this - }; - var solveStage = new SolveStep2StageFunction - { - Dt = substepContext.Dt, - InverseDt = substepContext.InverseDt, - solver = this - }; - - var syncStagesPerSubstep = 2 + synchronizedBatchCount * (1 + IterationCount); - if (workerIndex == 0) - { - //This is the main 'orchestrator' thread. It tracks execution progress and notifies other threads that's it's time to work. - int syncIndex = 0; - for (int substepIndex = 0; substepIndex < substepCount; ++substepIndex) - { - //Note that the main thread's view of the sync index increments every single dispatch, even if there is no work. - //This ensures that the workers are able to advance to the appropriate stage by examining the sync index snapshot. - ++syncIndex; - if (substepIndex > 0) - { - ExecuteMainStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStart, ref substepContext.Stages[0], GetPreviousSyncIndexForIncrementalUpdate(substepIndex, syncIndex, syncStagesPerSubstep), syncIndex); - } - //Note that we do not invoke velocity integration on the first substep if kinematics do not need velocity integration. - ++syncIndex; - if (substepIndex > 0 || PoseIntegrator.Callbacks.IntegrateVelocityForKinematics) - { - integrateConstrainedKinematicsStage.SubstepIndex = substepIndex; - ExecuteMainStage(ref integrateConstrainedKinematicsStage, workerIndex, kinematicIntegrationWorkerStart, ref substepContext.Stages[1], GetPreviousSyncIndexForIntegrateConstrainedKinematics(substepIndex, syncIndex, syncStagesPerSubstep), syncIndex); - } - warmstartStage.SubstepIndex = substepIndex; - for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) - { - ++syncIndex; - ExecuteMainStage(ref warmstartStage, workerIndex, batchStarts[batchIndex], ref substepContext.Stages[batchIndex + 2], GetPreviousSyncIndexForWarmStart(syncIndex, synchronizedBatchCount), syncIndex); - } - if (fallbackExists) - { - //The fallback runs only on the main thread. - ref var batch = ref activeSet.Batches[FallbackBatchThreshold]; - ref var integrationFlagsForBatch = ref integrationFlags[FallbackBatchThreshold]; - for (int j = 0; j < batch.TypeBatches.Count; ++j) - { - ref var typeBatch = ref batch.TypeBatches[j]; - if (substepIndex == 0) - { - WarmStartBlock(0, FallbackBatchThreshold, j, 0, typeBatch.BundleCount, ref typeBatch, TypeProcessors[typeBatch.TypeId], substepContext.Dt, substepContext.InverseDt); - } - else - { - WarmStartBlock(0, FallbackBatchThreshold, j, 0, typeBatch.BundleCount, ref typeBatch, TypeProcessors[typeBatch.TypeId], substepContext.Dt, substepContext.InverseDt); - } - } - } - for (int iterationIndex = 0; iterationIndex < IterationCount; ++iterationIndex) - { - for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) - { - //Note that this is using a 'different' stage by index than the worker thread if the iteration index > 1. - //That's totally fine- the warmstart/iteration stages share the same claims buffers per batch. They're redundant for the sake of easier indexing. - ++syncIndex; - ExecuteMainStage(ref solveStage, workerIndex, batchStarts[batchIndex], ref substepContext.Stages[batchIndex + 2], GetPreviousSyncIndexForSolve(syncIndex, synchronizedBatchCount), syncIndex); - } - if (fallbackExists) - { - //The fallback runs only on the main thread. - ref var batch = ref activeSet.Batches[FallbackBatchThreshold]; - for (int j = 0; j < batch.TypeBatches.Count; ++j) - { - ref var typeBatch = ref batch.TypeBatches[j]; - TypeProcessors[typeBatch.TypeId].SolveStep2(ref typeBatch, bodies, substepContext.Dt, substepContext.InverseDt, 0, typeBatch.BundleCount); - } - } - } - } - //All done; notify waiting threads to join. - Volatile.Write(ref substepContext.SyncIndex, int.MinValue); - } - else - { - //This is a worker thread. It does not need to track execution progress; it only checks to see if there's any work that needs to be done, and if there is, does it, then goes back into a wait. - int latestCompletedSyncIndex = 0; - int syncIndexInSubstep = -1; - int substepIndex = 0; - - while (true) - { - var spinWait = new LocalSpinWait(); - int syncIndex; - while (latestCompletedSyncIndex == (syncIndex = Volatile.Read(ref substepContext.SyncIndex))) - { - //No work yet available. - spinWait.SpinOnce(); - } - //Stages were set up prior to execution. Note that we don't attempt to ping pong buffers or anything; workblock claim indices monotonically increase across the execution of the solver. - //This guarantees that a worker thread can go idle and miss an arbitrary number of stages without blocking any progress. - if (syncIndex == int.MinValue) - { - //No more stages; exit the work loop. - break; - } - //Extract the job type, stage index, and substep index from the sync index. - var syncStepsSinceLast = syncIndex - latestCompletedSyncIndex; - syncIndexInSubstep += syncStepsSinceLast; - while (true) - { - if (syncIndexInSubstep >= syncStagesPerSubstep) - { - syncIndexInSubstep -= syncStagesPerSubstep; - ++substepIndex; - } - else - { - break; - } - } - //Console.WriteLine($"Worker working on sync index {syncIndex}, sync index in substep: {syncIndexInSubstep}"); - //Note that we're going to do a compare exchange that prevents any claim on work blocks that *arent* of the previous sync index, which means we need the previous sync index. - //Storing that in a reliable way is annoying, so we derive it from syncIndex. - ref var stage = ref substepContext.Stages[syncIndexInSubstep]; - //Console.WriteLine($"Worker {workerIndex} executing {stage.StageType} for sync index {syncIndex}, stage index {syncIndexInSubstep}"); - switch (stage.StageType) - { - case SolverStageType.IncrementalUpdate: - ExecuteWorkerStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStart, 0, ref stage.Claims, GetPreviousSyncIndexForIncrementalUpdate(substepIndex, syncIndex, syncStagesPerSubstep), syncIndex, ref substepContext.CompletedWorkBlockCount); - break; - case SolverStageType.IntegrateConstrainedKinematics: - integrateConstrainedKinematicsStage.SubstepIndex = substepIndex; - ExecuteWorkerStage(ref integrateConstrainedKinematicsStage, workerIndex, kinematicIntegrationWorkerStart, 0, ref stage.Claims, GetPreviousSyncIndexForIntegrateConstrainedKinematics(substepIndex, syncIndex, syncStagesPerSubstep), syncIndex, ref substepContext.CompletedWorkBlockCount); - break; - case SolverStageType.WarmStart: - warmstartStage.SubstepIndex = substepIndex; - ExecuteWorkerStage(ref warmstartStage, workerIndex, batchStarts[stage.BatchIndex], stage.WorkBlockStartIndex, ref stage.Claims, GetPreviousSyncIndexForWarmStart(syncIndex, synchronizedBatchCount), syncIndex, ref substepContext.CompletedWorkBlockCount); - break; - case SolverStageType.Solve: - ExecuteWorkerStage(ref solveStage, workerIndex, batchStarts[stage.BatchIndex], stage.WorkBlockStartIndex, ref stage.Claims, GetPreviousSyncIndexForSolve(syncIndex, synchronizedBatchCount), syncIndex, ref substepContext.CompletedWorkBlockCount); - break; - } - latestCompletedSyncIndex = syncIndex; - - } - } - - } - - Buffer BuildKinematicIntegrationWorkBlocks(int minimumBlockSizeInBundles, int maximumBlockSizeInBundles, int targetBlockCount) - { - var bundleCount = BundleIndexing.GetBundleCount(ConstrainedKinematicHandles.Count); - if (bundleCount > 0) - { - var targetBundlesPerBlock = bundleCount / targetBlockCount; - if (targetBundlesPerBlock < minimumBlockSizeInBundles) - targetBundlesPerBlock = minimumBlockSizeInBundles; - if (targetBundlesPerBlock > maximumBlockSizeInBundles) - targetBundlesPerBlock = maximumBlockSizeInBundles; - var blockCount = (bundleCount + targetBundlesPerBlock - 1) / targetBundlesPerBlock; - var bundlesPerBlock = bundleCount / blockCount; - var remainder = bundleCount - bundlesPerBlock * blockCount; - var previousEnd = 0; - pool.Take(blockCount, out Buffer workBlocks); - for (int i = 0; i < blockCount; ++i) - { - var bundleCountForBlock = bundlesPerBlock; - if (i < remainder) - ++bundleCountForBlock; - workBlocks[i] = new IntegrationWorkBlock { StartBundleIndex = previousEnd, EndBundleIndex = previousEnd += bundleCountForBlock }; - } - return workBlocks; - - } - return default; - } - - //Buffer> debugStageWorkBlocksCompleted; - protected void ExecuteMultithreaded2(float dt, IThreadDispatcher threadDispatcher, Action workDelegate) - { - var workerCount = substepContext.WorkerCount = threadDispatcher.ThreadCount; - substepContext.Dt = dt; - substepContext.InverseDt = 1f / dt; - //First build a set of work blocks. - //The block size should be relatively small to give the workstealer something to do, but we don't want to go crazy with the number of blocks. - //These values are found by empirical tuning. The optimal values may vary by architecture. - //The goal here is to have just enough blocks that, in the event that we end up some underpowered threads (due to competition or hyperthreading), - //there are enough blocks that workstealing will still generally allow the extra threads to be useful. - const int targetBlocksPerBatchPerWorker = 4; - const int minimumBlockSizeInBundles = 1; - const int maximumBlockSizeInBundles = 1024; - - var targetBlocksPerBatch = workerCount * targetBlocksPerBatchPerWorker; - var mainFilter = new MainSolveFilter(); - var incrementalFilter = new IncrementalContactDataUpdateFilter(); - BuildWorkBlocks(pool, minimumBlockSizeInBundles, maximumBlockSizeInBundles, targetBlocksPerBatch, ref mainFilter, out var constraintBlocks, out substepContext.ConstraintBatchBoundaries); - BuildWorkBlocks(pool, minimumBlockSizeInBundles, maximumBlockSizeInBundles, targetBlocksPerBatch, ref incrementalFilter, out var incrementalBlocks, out var incrementalUpdateBatchBoundaries); - pool.Return(ref incrementalUpdateBatchBoundaries); //TODO: No need to create this in the first place. Doesn't really cost anything, but... - substepContext.ConstraintBlocks = constraintBlocks.Span.Slice(constraintBlocks.Count); - substepContext.IncrementalUpdateBlocks = incrementalBlocks.Span.Slice(incrementalBlocks.Count); - substepContext.KinematicIntegrationBlocks = BuildKinematicIntegrationWorkBlocks(minimumBlockSizeInBundles, maximumBlockSizeInBundles, targetBlocksPerBatch); - - //Not every batch will actually have work blocks associated with it; the batch compressor could be falling behind, which means older constraints could be at higher batches than they need to be, leaving gaps. - //We don't want to include those empty batches as sync points in the solver. - var batchCount = ActiveSet.Batches.Count; - substepContext.SyncIndex = 0; - var totalConstraintBatchWorkBlockCount = substepContext.ConstraintBatchBoundaries.Length == 0 ? 0 : substepContext.ConstraintBatchBoundaries[^1]; - var totalClaimCount = incrementalBlocks.Count + substepContext.KinematicIntegrationBlocks.Length + totalConstraintBatchWorkBlockCount; - GetSynchronizedBatchCount(out var stagesPerIteration, out var fallbackExists); - pool.Take(2 + stagesPerIteration * (1 + IterationCount), out substepContext.Stages); - //Claims will be monotonically increasing throughout execution. All should start at zero to match with the initial sync index. - pool.Take(totalClaimCount, out var claims); - claims.Clear(0, claims.Length); - substepContext.Stages[0] = new(claims.Slice(incrementalBlocks.Count), 0, SolverStageType.IncrementalUpdate); - substepContext.Stages[1] = new(claims.Slice(incrementalBlocks.Count, substepContext.KinematicIntegrationBlocks.Length), 0, SolverStageType.IntegrateConstrainedKinematics); - //Note that we create redundant stages that share the same workblock targets and claims buffers. - //This is just to make indexing a little simpler during the multithreaded work. - int targetStageIndex = 2; - //Warm start. - var preambleClaimCount = incrementalBlocks.Count + substepContext.KinematicIntegrationBlocks.Length; - int claimStart = preambleClaimCount; - for (int batchIndex = 0; batchIndex < stagesPerIteration; ++batchIndex) - { - var stageIndex = targetStageIndex++; - var batchStart = batchIndex == 0 ? 0 : substepContext.ConstraintBatchBoundaries[batchIndex - 1]; - var workBlocksInBatch = substepContext.ConstraintBatchBoundaries[batchIndex] - batchStart; - substepContext.Stages[stageIndex] = new(claims.Slice(claimStart, workBlocksInBatch), batchStart, SolverStageType.WarmStart, batchIndex); - claimStart += workBlocksInBatch; - } - for (int iterationIndex = 0; iterationIndex < IterationCount; ++iterationIndex) - { - //Solve. Note that we're reusing the same claims as were used in the warm start for these stages; the stages just tell the workers what kind of work to do. - claimStart = preambleClaimCount; - for (int batchIndex = 0; batchIndex < stagesPerIteration; ++batchIndex) - { - var stageIndex = targetStageIndex++; - var batchStart = batchIndex == 0 ? 0 : substepContext.ConstraintBatchBoundaries[batchIndex - 1]; - var workBlocksInBatch = substepContext.ConstraintBatchBoundaries[batchIndex] - batchStart; - substepContext.Stages[stageIndex] = new(claims.Slice(claimStart, workBlocksInBatch), batchStart, SolverStageType.Solve, batchIndex); - claimStart += workBlocksInBatch; - } - } - - //for (int i = 0; i < iterationCountPlusOne; ++i) - //{ - // int claimStart = incrementalBlocks.Count; - // for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) - // { - // var stageIndex = targetStageIndex++; - // var batchStart = batchIndex == 0 ? 0 : substepContext.ConstraintBatchBoundaries[batchIndex - 1]; - // var workBlocksInBatch = substepContext.ConstraintBatchBoundaries[batchIndex] - batchStart; - // substepContext.Stages[stageIndex] = new(claims.Slice(claimStart, workBlocksInBatch), batchStart, batchIndex); - // claimStart += workBlocksInBatch; - // } - //} - - //var syncCount = substepCount * (1 + synchronizedBatchCount * (1 + IterationCount)) - 1; - //pool.Take(syncCount, out debugStageWorkBlocksCompleted); - //pool.Take(syncCount * workerCount, out var workBlocksCompleted); - //workBlocksCompleted.Clear(0, workBlocksCompleted.Length); - //for (int i = 0; i < syncCount; ++i) - //{ - // debugStageWorkBlocksCompleted[i] = workBlocksCompleted.Slice(i * workerCount, workerCount); - //} - - //While we could be a little more aggressive about culling work with this condition, it doesn't matter much. Have to do it for correctness; worker relies on it. - if (ActiveSet.Batches.Count > 0) - { - //workDelegate(0); - threadDispatcher.DispatchWorkers(workDelegate); - } - - //pool.Take(syncCount, out var availableCountPerSync); - //var syncIndex = 0; - //for (int substepIndex = 0; substepIndex < substepCount; ++substepIndex) - //{ - // if (substepIndex > 0) - // { - // availableCountPerSync[syncIndex] = incrementalBlocks.Count; - // ++syncIndex; - // } - // for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) - // { - // var batchStart = batchIndex == 0 ? 0 : substepContext.ConstraintBatchBoundaries[batchIndex - 1]; - // var workBlocksInBatch = substepContext.ConstraintBatchBoundaries[batchIndex] - batchStart; - // availableCountPerSync[syncIndex] = workBlocksInBatch; - // ++syncIndex; - // } - // for (int i = 0; i < IterationCount; ++i) - // { - // for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) - // { - // var batchStart = batchIndex == 0 ? 0 : substepContext.ConstraintBatchBoundaries[batchIndex - 1]; - // var workBlocksInBatch = substepContext.ConstraintBatchBoundaries[batchIndex] - batchStart; - // availableCountPerSync[syncIndex] = workBlocksInBatch; - // ++syncIndex; - // } - // } - //} - - //pool.Take(workerCount, out var workerBlocksCompletedSums); - //pool.Take(workerCount, out var workerFractionSum); - //workerBlocksCompletedSums.Clear(0, workerBlocksCompletedSums.Length); - //var availableCountSum = 0; - //for (int i = 0; i < syncCount; ++i) - //{ - // if (availableCountPerSync[i] <= 1) - // continue; - // Console.WriteLine($"Sync {i}, available {availableCountPerSync[i]}, ideal {(availableCountPerSync[i] / (double)workerCount):G2}:"); - // var stageWorkerBlockCounts = debugStageWorkBlocksCompleted[i]; - // for (int j = 0; j < workerCount; ++j) - // { - // workerBlocksCompletedSums[j] += stageWorkerBlockCounts[j]; - // var expectedCount = availableCountPerSync[i] / (double)workerCount; - // if (j >= availableCountPerSync[i]) - // workerFractionSum[j] += 1; - // else - // workerFractionSum[j] += stageWorkerBlockCounts[j] / expectedCount; - // Console.WriteLine($"{j}: {stageWorkerBlockCounts[j]}"); - // } - // availableCountSum += availableCountPerSync[i]; - //} - ////var idealOccupancy = 1.0 / workerCount; - ////Console.WriteLine($"Worker occupancy (ideal {idealOccupancy}):"); - ////for (int i = 0; i < workerCount; ++i) - ////{ - //// //Console.WriteLine($"{i}: {(workerBlocksCompletedSums[i] / (double)availableCountSum):G3}"); - //// Console.WriteLine($"{i}: {workerFractionSum[i] / syncCount:G3}"); - ////} - - //pool.Return(ref workerBlocksCompletedSums); - //pool.Return(ref workBlocksCompleted); - //pool.Return(ref debugStageWorkBlocksCompleted); - //pool.Return(ref availableCountPerSync); - - pool.Return(ref claims); - pool.Return(ref substepContext.Stages); - pool.Return(ref substepContext.ConstraintBatchBoundaries); - pool.Return(ref substepContext.IncrementalUpdateBlocks); - if (substepContext.KinematicIntegrationBlocks.Allocated) - pool.Return(ref substepContext.KinematicIntegrationBlocks); - pool.Return(ref substepContext.ConstraintBlocks); - - - - } - - struct IsFallbackBatch { } - struct IsNotFallbackBatch { } - unsafe bool ComputeIntegrationResponsibilitiesForConstraintRegion(int batchIndex, int typeBatchIndex, int constraintStart, int exclusiveConstraintEnd) where TFallbackness : unmanaged - { - ref var firstObservedForBatch = ref bodiesFirstObservedInBatches[batchIndex]; - ref var integrationFlagsForTypeBatch = ref integrationFlags[batchIndex][typeBatchIndex]; - ref var typeBatch = ref ActiveSet.Batches[batchIndex].TypeBatches[typeBatchIndex]; - var typeBatchBodyReferences = typeBatch.BodyReferences.As(); - var bodiesPerConstraintInTypeBatch = TypeProcessors[typeBatch.TypeId].BodiesPerConstraint; - var intsPerBundle = Vector.Count * bodiesPerConstraintInTypeBatch; - var bundleStartIndex = constraintStart / Vector.Count; - var bundleEndIndex = (exclusiveConstraintEnd + Vector.Count - 1) / Vector.Count; - Debug.Assert(bundleStartIndex >= 0 && bundleEndIndex <= typeBatch.BundleCount); - ref var activeSet = ref bodies.ActiveSet; - - for (int bundleIndex = bundleStartIndex; bundleIndex < bundleEndIndex; ++bundleIndex) - { - int bundleStartIndexInConstraints = bundleIndex * Vector.Count; - int countInBundle = Math.Min(Vector.Count, typeBatch.ConstraintCount - bundleStartIndexInConstraints); - //Body references are stored in AOSOA layout. - var bundleBodyReferencesStart = typeBatchBodyReferences.Memory + bundleIndex * intsPerBundle; - for (int bodyIndexInConstraint = 0; bodyIndexInConstraint < bodiesPerConstraintInTypeBatch; ++bodyIndexInConstraint) - { - ref var integrationFlagsForBodyInConstraint = ref integrationFlagsForTypeBatch[bodyIndexInConstraint]; - var bundleStart = bundleBodyReferencesStart + bodyIndexInConstraint * Vector.Count; - for (int bundleInnerIndex = 0; bundleInnerIndex < countInBundle; ++bundleInnerIndex) - { - //Constraints refer to bodies by index when they're in the active set, so we need to transform to handle to look up our merged batch results. - int bodyIndex; - if (typeof(TFallbackness) == typeof(IsFallbackBatch)) - { - //Fallback batches can contain empty lanes; there's no guarantee of constraint contiguity. Such lanes are marked with -1 in the body references. - //Just skip over them. - var rawBodyIndex = bundleStart[bundleInnerIndex]; - if (rawBodyIndex == -1) - { - continue; - } - bodyIndex = rawBodyIndex & Bodies.BodyReferenceMask; - } - else - { - bodyIndex = bundleStart[bundleInnerIndex] & Bodies.BodyReferenceMask; - } - var bodyHandle = activeSet.IndexToHandle[bodyIndex].Value; - if (firstObservedForBatch.Contains(bodyHandle)) - { - if (typeof(TFallbackness) == typeof(IsFallbackBatch)) - { - //This is a fallback. Being contained is not sufficient to require integration; it must also be the *first* constraint that will be executed. - //This is guaranteed by the index of the constraint in the type batch, and the type batch's index. - //Note that, since the fallback batch was the first time this body was seen, we know that *all* constraints associated with this body must be in the fallback batch. - //This could be significantly optimized by not recalculating the earliest candidate every single time a body is encountered in the fallback batch, but - //the fallback batch should effectively *never* contain any integration responsibilities. - ulong earliestIndex = ulong.MaxValue; - ref var constraintsForBody = ref activeSet.Constraints[bodyIndex]; - for (int constraintIndexInBody = 0; constraintIndexInBody < constraintsForBody.Count; ++constraintIndexInBody) - { - ref var fallbackBatch = ref ActiveSet.Batches[FallbackBatchThreshold]; - ref var location = ref HandleToConstraint[constraintsForBody[constraintIndexInBody].ConnectingConstraintHandle.Value]; - var typeBatchIndexForCandidate = fallbackBatch.TypeIndexToTypeBatchIndex[location.TypeId]; - var candidate = ((ulong)typeBatchIndexForCandidate << 32) | (uint)location.IndexInTypeBatch; - if (candidate < earliestIndex) - earliestIndex = candidate; - } - var indexInTypeBatch = bundleStartIndexInConstraints + bundleInnerIndex; - var currentSlot = ((ulong)typeBatchIndex << 32) | (uint)indexInTypeBatch; - if (currentSlot == earliestIndex) - { - integrationFlagsForBodyInConstraint.AddUnsafely(indexInTypeBatch); - } - } - else - { - //Not a fallback; being contained in the observed set is sufficient. - integrationFlagsForBodyInConstraint.AddUnsafely(bundleStartIndexInConstraints + bundleInnerIndex); - } - } - } - } - } - //Precompute which type batches have *any* integration responsibilities, allowing us to use a all-or-nothing test before dispatching a workblock. - //Note that this could be vectorized the same way we did in the batch merging, but... less likely to be useful. Less constraints in sequence in type batches, - //and we're already within a multithreaded context. Saving 1 microsecond not terribly meaningful. - var flagBundleCount = IndexSet.GetBundleCapacity(typeBatch.ConstraintCount); - ulong mergedFlagBundles = 0; - for (int bodyIndexInConstraint = 0; bodyIndexInConstraint < bodiesPerConstraintInTypeBatch; ++bodyIndexInConstraint) - { - ref var integrationFlagsForBodyInTypeBatch = ref integrationFlagsForTypeBatch[bodyIndexInConstraint]; - for (int i = 0; i < flagBundleCount; ++i) - { - mergedFlagBundles |= integrationFlagsForBodyInTypeBatch.Flags[i]; - } - } - return mergedFlagBundles != 0; - } - - void ConstraintIntegrationResponsibilitiesWorker(int workerIndex) - { - int jobIndex; - while ((jobIndex = Interlocked.Increment(ref nextConstraintIntegrationResponsibilityJobIndex) - 1) < integrationResponsibilityPrepassJobs.Count) - { - ref var job = ref integrationResponsibilityPrepassJobs[jobIndex]; - if (job.batch == FallbackBatchThreshold) - jobAlignedIntegrationResponsibilities[jobIndex] = ComputeIntegrationResponsibilitiesForConstraintRegion(job.batch, job.typeBatch, job.start, job.end); - else - jobAlignedIntegrationResponsibilities[jobIndex] = ComputeIntegrationResponsibilitiesForConstraintRegion(job.batch, job.typeBatch, job.start, job.end); - } - } - - int nextConstraintIntegrationResponsibilityJobIndex; - QuickList<(int batch, int typeBatch, int start, int end)> integrationResponsibilityPrepassJobs; - Buffer jobAlignedIntegrationResponsibilities; - Buffer bodiesFirstObservedInBatches; - Buffer>> integrationFlags; - /// - /// Caches a single bool for whether type batches within batches have constraints with any integration responsibilities. - /// Type batches with no integration responsibilities can use a codepath with no integration checks at all. - /// - Buffer> coarseBatchIntegrationResponsibilities; - Action constraintIntegrationResponsibilitiesWorker; - IndexSet mergedConstrainedBodyHandles; - - int substepCount; - public override unsafe IndexSet PrepareConstraintIntegrationResponsibilities(int substepCount, IThreadDispatcher threadDispatcher = null) - { - //TODO: we're caching it on a per call basis because we are still using the old substeppingtimestepper frame externally. Once we bite the bullet 100% on bundling, we can make it equivalent to IterationCount. - this.substepCount = substepCount; - if (ActiveSet.Batches.Count > 0) - { - pool.Take(ActiveSet.Batches.Count, out integrationFlags); - integrationFlags[0] = default; - pool.Take(ActiveSet.Batches.Count, out coarseBatchIntegrationResponsibilities); - for (int batchIndex = 1; batchIndex < integrationFlags.Length; ++batchIndex) - { - ref var batch = ref ActiveSet.Batches[batchIndex]; - ref var flagsForBatch = ref integrationFlags[batchIndex]; - pool.Take(batch.TypeBatches.Count, out flagsForBatch); - pool.Take(batch.TypeBatches.Count, out coarseBatchIntegrationResponsibilities[batchIndex]); - for (int typeBatchIndex = 0; typeBatchIndex < flagsForBatch.Length; ++typeBatchIndex) - { - ref var flagsForTypeBatch = ref flagsForBatch[typeBatchIndex]; - ref var typeBatch = ref batch.TypeBatches[typeBatchIndex]; - var bodiesPerConstraint = TypeProcessors[typeBatch.TypeId].BodiesPerConstraint; - pool.Take(bodiesPerConstraint, out flagsForTypeBatch); - for (int bodyIndexInConstraint = 0; bodyIndexInConstraint < bodiesPerConstraint; ++bodyIndexInConstraint) - { - flagsForTypeBatch[bodyIndexInConstraint] = new IndexSet(pool, typeBatch.ConstraintCount); - } - } - } - - //Brute force fallback for debugging: - //for (int i = 0; i < bodies.ActiveSet.Count; ++i) - //{ - // ref var constraints = ref bodies.ActiveSet.Constraints[i]; - // ConstraintHandle minimumConstraint; - // minimumConstraint.Value = -1; - // int minimumBatchIndex = int.MaxValue; - // int minimumIndexInConstraint = -1; - // for (int j = 0; j < constraints.Count; ++j) - // { - // ref var constraint = ref constraints[j]; - // var batchIndex = HandleToConstraint[constraint.ConnectingConstraintHandle.Value].BatchIndex; - // if (batchIndex < minimumBatchIndex) - // { - // minimumBatchIndex = batchIndex; - // minimumIndexInConstraint = constraint.BodyIndexInConstraint; - // minimumConstraint = constraint.ConnectingConstraintHandle; - // } - // } - // if (minimumConstraint.Value >= 0) - // { - // ref var location = ref HandleToConstraint[minimumConstraint.Value]; - // var typeBatchIndex = ActiveSet.Batches[location.BatchIndex].TypeIndexToTypeBatchIndex[location.TypeId]; - // ref var indexSet = ref integrationFlags[location.BatchIndex][typeBatchIndex][minimumIndexInConstraint]; - // indexSet.AddUnsafely(location.IndexInTypeBatch); - // } - //} - - pool.Take(batchReferencedHandles.Count, out bodiesFirstObservedInBatches); - //We don't have to consider the first batch, since we know ahead of time that the first batch will be the first time we see any bodies in it. - //Just copy directly from the first batch into the merged to initialize it. - //Note "+ 64" instead of "+ 63": the highest possibly claimed id is inclusive! - pool.Take((bodies.HandlePool.HighestPossiblyClaimedId + 64) / 64, out mergedConstrainedBodyHandles.Flags); - var copyLength = Math.Min(mergedConstrainedBodyHandles.Flags.Length, batchReferencedHandles[0].Flags.Length); - batchReferencedHandles[0].Flags.CopyTo(0, mergedConstrainedBodyHandles.Flags, 0, copyLength); - batchReferencedHandles[0].Flags.Clear(copyLength, batchReferencedHandles[0].Flags.Length - copyLength); - - //Yup, we're just leaving the first slot unallocated to avoid having to offset indices all over the place. Slight wonk, but not a big deal. - bodiesFirstObservedInBatches[0] = default; - pool.Take(batchReferencedHandles.Count, out var batchHasAnyIntegrationResponsibilities); - for (int batchIndex = 1; batchIndex < bodiesFirstObservedInBatches.Length; ++batchIndex) - { - ref var batchHandles = ref batchReferencedHandles[batchIndex]; - var bundleCount = Math.Min(mergedConstrainedBodyHandles.Flags.Length, batchHandles.Flags.Length); - //Note that we bypass the constructor to avoid zeroing unnecessarily. Every bundle will be fully assigned. - pool.Take(bundleCount, out bodiesFirstObservedInBatches[batchIndex].Flags); - } - GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); - //Note that we are not multithreading the batch merging phase. This typically takes a handful of microseconds. - //You'd likely need millions of bodies before you'd see any substantial benefit from multithreading this. - var batchCount = ActiveSet.Batches.Count; - for (int batchIndex = 1; batchIndex < batchCount; ++batchIndex) - { - ref var batchHandles = ref batchReferencedHandles[batchIndex]; - ref var firstObservedInBatch = ref bodiesFirstObservedInBatches[batchIndex]; - var flagBundleCount = Math.Min(mergedConstrainedBodyHandles.Flags.Length, batchHandles.Flags.Length); - - if (Avx2.IsSupported) - { - var avxBundleCount = flagBundleCount / 4; - var horizontalAvxMerge = Vector256.Zero; - for (int avxBundleIndex = 0; avxBundleIndex < avxBundleCount; ++avxBundleIndex) - { - //These will *almost* always be aligned, but guaranteeing it is not worth the complexity. - var mergeBundle = Avx2.LoadVector256((ulong*)((Vector256*)mergedConstrainedBodyHandles.Flags.Memory + avxBundleIndex)); - var batchBundle = Avx2.LoadVector256((ulong*)((Vector256*)batchHandles.Flags.Memory + avxBundleIndex)); - Avx.Store((ulong*)((Vector256*)mergedConstrainedBodyHandles.Flags.Memory + avxBundleIndex), Avx2.Or(mergeBundle, batchBundle)); - //If this batch contains a body, and the merged set does not, then it's the first batch that sees a body and it will have integration responsibility. - var firstObservedBundle = Avx2.AndNot(mergeBundle, batchBundle); - horizontalAvxMerge = Avx2.Or(firstObservedBundle, horizontalAvxMerge); - Avx.Store((ulong*)((Vector256*)firstObservedInBatch.Flags.Memory + avxBundleIndex), firstObservedBundle); - } - var notEqual = Avx2.Xor(Avx2.CompareEqual(horizontalAvxMerge, Vector256.Zero), Vector256.AllBitsSet); - ulong horizontalMerge = (ulong)Avx2.MoveMask(notEqual.AsDouble()); - - //Cleanup loop. - for (int flagBundleIndex = avxBundleCount * 4; flagBundleIndex < flagBundleCount; ++flagBundleIndex) - { - var mergeBundle = mergedConstrainedBodyHandles.Flags[flagBundleIndex]; - var batchBundle = batchHandles.Flags[flagBundleIndex]; - mergedConstrainedBodyHandles.Flags[flagBundleIndex] = mergeBundle | batchBundle; - //If this batch contains a body, and the merged set does not, then it's the first batch that sees a body and it will have integration responsibility. - var firstObservedBundle = ~mergeBundle & batchBundle; - horizontalMerge |= firstObservedBundle; - firstObservedInBatch.Flags[flagBundleIndex] = firstObservedBundle; - } - batchHasAnyIntegrationResponsibilities[batchIndex] = horizontalMerge != 0; - } - else - { - ulong horizontalMerge = 0; - for (int flagBundleIndex = 0; flagBundleIndex < flagBundleCount; ++flagBundleIndex) - { - var mergeBundle = mergedConstrainedBodyHandles.Flags[flagBundleIndex]; - var batchBundle = batchHandles.Flags[flagBundleIndex]; - mergedConstrainedBodyHandles.Flags[flagBundleIndex] = mergeBundle | batchBundle; - //If this batch contains a body, and the merged set does not, then it's the first batch that sees a body and it will have integration responsibility. - var firstObservedBundle = ~mergeBundle & batchBundle; - horizontalMerge |= firstObservedBundle; - firstObservedInBatch.Flags[flagBundleIndex] = firstObservedBundle; - } - batchHasAnyIntegrationResponsibilities[batchIndex] = horizontalMerge != 0; - } - } - - //var start = Stopwatch.GetTimestamp(); - //We now have index sets representing the first time each body handle is observed in a batch. - //This process is significantly more expensive than the batch merging phase and can benefit from multithreading. - //It is still fairly cheap, though- we can't use really fine grained jobs or the cost of swapping jobs will exceed productive work. - - //Note that we arbitrarily use single threaded execution if the job is small enough. Dispatching isn't free. - bool useSingleThreadedPath = true; - if (threadDispatcher != null && threadDispatcher.ThreadCount > 1) - { - integrationResponsibilityPrepassJobs = new(128, pool); - int constraintCount = 0; - const int targetJobSize = 2048; - Debug.Assert(targetJobSize % 64 == 0, "Target job size must be a multiple of the index set bundles to avoid threads working on the same flag bundle."); - for (int batchIndex = 1; batchIndex < bodiesFirstObservedInBatches.Length; ++batchIndex) - { - if (!batchHasAnyIntegrationResponsibilities[batchIndex]) - continue; - ref var batch = ref ActiveSet.Batches[batchIndex]; - for (int typeBatchIndex = 0; typeBatchIndex < batch.TypeBatches.Count; ++typeBatchIndex) - { - ref var typeBatch = ref batch.TypeBatches[typeBatchIndex]; - constraintCount += typeBatch.ConstraintCount; - int jobCountForTypeBatch = (typeBatch.ConstraintCount + targetJobSize - 1) / targetJobSize; - for (int i = 0; i < jobCountForTypeBatch; ++i) - { - var jobStart = i * targetJobSize; - var jobEnd = Math.Min(jobStart + targetJobSize, typeBatch.ConstraintCount); - integrationResponsibilityPrepassJobs.Allocate(pool) = (batchIndex, typeBatchIndex, jobStart, jobEnd); - } - } - } - if (constraintCount > 4096 + threadDispatcher.ThreadCount * 1024) - { - nextConstraintIntegrationResponsibilityJobIndex = 0; - useSingleThreadedPath = false; - pool.Take(integrationResponsibilityPrepassJobs.Count, out jobAlignedIntegrationResponsibilities); - //for (int i = 0; i < integrationResponsibilityPrepassJobs.Count; ++i) - //{ - // ref var job = ref integrationResponsibilityPrepassJobs[i]; - // jobAlignedIntegrationResponsibilities[i] = ComputeIntegrationResponsibilitiesForConstraintRegion(job.batch, job.typeBatch, job.start, job.end); - //} - threadDispatcher.DispatchWorkers(constraintIntegrationResponsibilitiesWorker); - - //Coarse batch integration responsibilities start uninitialized. Possible to have multiple jobs per type batch in multithreaded case, so we need to init to merge. - for (int i = 1; i < ActiveSet.Batches.Count; ++i) - { - ref var batch = ref ActiveSet.Batches[i]; - for (int j = 0; j < batch.TypeBatches.Count; ++j) - { - coarseBatchIntegrationResponsibilities[i][j] = false; - } - } - for (int i = 0; i < integrationResponsibilityPrepassJobs.Count; ++i) - { - ref var job = ref integrationResponsibilityPrepassJobs[i]; - coarseBatchIntegrationResponsibilities[job.batch][job.typeBatch] |= jobAlignedIntegrationResponsibilities[i]; - } - pool.Return(ref jobAlignedIntegrationResponsibilities); - } - integrationResponsibilityPrepassJobs.Dispose(pool); - } - if (useSingleThreadedPath) - { - for (int i = 1; i < synchronizedBatchCount; ++i) - { - if (!batchHasAnyIntegrationResponsibilities[i]) - continue; - ref var batch = ref ActiveSet.Batches[i]; - for (int j = 0; j < batch.TypeBatches.Count; ++j) - { - ref var typeBatch = ref batch.TypeBatches[j]; - coarseBatchIntegrationResponsibilities[i][j] = ComputeIntegrationResponsibilitiesForConstraintRegion(i, j, 0, typeBatch.ConstraintCount); - } - } - if (fallbackExists && batchHasAnyIntegrationResponsibilities[FallbackBatchThreshold]) - { - ref var batch = ref ActiveSet.Batches[FallbackBatchThreshold]; - for (int j = 0; j < batch.TypeBatches.Count; ++j) - { - ref var typeBatch = ref batch.TypeBatches[j]; - coarseBatchIntegrationResponsibilities[FallbackBatchThreshold][j] = ComputeIntegrationResponsibilitiesForConstraintRegion(FallbackBatchThreshold, j, 0, typeBatch.ConstraintCount); - } - } - } - //var end = Stopwatch.GetTimestamp(); - //Console.WriteLine($"time (ms): {(end - start) * 1e3 / Stopwatch.Frequency}"); - - ////Validation: - //for (int i = 0; i < bodies.ActiveSet.Count; ++i) - //{ - // ref var constraints = ref bodies.ActiveSet.Constraints[i]; - // ConstraintHandle minimumConstraint; - // minimumConstraint.Value = -1; - // int minimumBatchIndex = int.MaxValue; - // int minimumBodyIndexInConstraint = -1; - // ulong earliestSlotInFallback = ulong.MaxValue; - // ConstraintHandle detectedConstraint; - // detectedConstraint.Value = -1; - // for (int j = 0; j < constraints.Count; ++j) - // { - // ref var constraint = ref constraints[j]; - // ref var location = ref HandleToConstraint[constraint.ConnectingConstraintHandle.Value]; - // var typeBatchIndex = ActiveSet.Batches[location.BatchIndex].TypeIndexToTypeBatchIndex[location.TypeId]; - - // if (location.BatchIndex <= minimumBatchIndex) //Note that the only time it can be equal is if this is in the fallback batch. - // { - // if (location.BatchIndex == FallbackBatchThreshold) - // { - // var encodedSlotCandidate = ((ulong)typeBatchIndex << 32) | (uint)location.IndexInTypeBatch; - // if (encodedSlotCandidate < earliestSlotInFallback) - // { - // earliestSlotInFallback = encodedSlotCandidate; - // //We should only accept another fallback constraint as minimal if it has an earlier typebatch index/index in type batch. - // minimumBatchIndex = location.BatchIndex; - // minimumBodyIndexInConstraint = constraint.BodyIndexInConstraint; - // minimumConstraint = constraint.ConnectingConstraintHandle; - // } - // } - // else - // { - // minimumBatchIndex = location.BatchIndex; - // minimumBodyIndexInConstraint = constraint.BodyIndexInConstraint; - // minimumConstraint = constraint.ConnectingConstraintHandle; - // } - // } - - // if (location.BatchIndex > 0) - // { - // ref var indexSet = ref integrationFlags[location.BatchIndex][typeBatchIndex][constraint.BodyIndexInConstraint]; - // if (indexSet.Contains(location.IndexInTypeBatch)) - // { - // //This constraint has integration responsibility for this body. - // Debug.Assert(detectedConstraint.Value == -1, "Only one constraint should have integration responsibility for a given body."); - // detectedConstraint = constraint.ConnectingConstraintHandle; - // } - // } - // } - // if (constraints.Count > 0 && minimumBatchIndex > 0) - // Debug.Assert(detectedConstraint.Value >= 0, "At least one constraint must have integration responsibility for a body if it has a constraint."); - // if (minimumConstraint.Value >= 0) - // { - // Debug.Assert(minimumBatchIndex == 0 || detectedConstraint.Value == minimumConstraint.Value); - // ref var location = ref HandleToConstraint[minimumConstraint.Value]; - // var typeBatchIndex = ActiveSet.Batches[location.BatchIndex].TypeIndexToTypeBatchIndex[location.TypeId]; - // if (location.BatchIndex > 0) - // { - // ref var indexSet = ref integrationFlags[location.BatchIndex][typeBatchIndex][minimumBodyIndexInConstraint]; - // Debug.Assert(indexSet.Contains(location.IndexInTypeBatch)); - // } - // } - //} - - pool.Return(ref batchHasAnyIntegrationResponsibilities); - - Debug.Assert(!bodiesFirstObservedInBatches[0].Flags.Allocated, "Remember, we're assuming we're just leaving the first batch's slot empty to avoid indexing complexity."); - for (int batchIndex = 1; batchIndex < bodiesFirstObservedInBatches.Length; ++batchIndex) - { - bodiesFirstObservedInBatches[batchIndex].Dispose(pool); - } - pool.Return(ref bodiesFirstObservedInBatches); - - //Add the constrained kinematics to the constrained body handles. The kinematics were absent from batch referenced handles. - //TODO: This assumes the number of kinematics is low relative to the number of bodies and does not need to be multithreaded. - //This assumption is *usually* fine, but we should probably have a fallback that is more efficient if this assumption is wrong. - //Could maintain an indexset parallel to the ConstrainedKinematicHandles- same set, just different format. - //If we detect a lot of constrained kinematics, just do an indexset merge. - //That would be fast enough even if all bodies were kinematic. - for (int i = 0; i < ConstrainedKinematicHandles.Count; ++i) - { - mergedConstrainedBodyHandles.AddUnsafely(ConstrainedKinematicHandles[i]); - } - return mergedConstrainedBodyHandles; - } - else - { - return new IndexSet(); - } - } - public override void DisposeConstraintIntegrationResponsibilities() - { - if (ActiveSet.Batches.Count > 0) - { - Debug.Assert(!integrationFlags[0].Allocated, "Remember, we're assuming we're just leaving the first batch's slot empty to avoid indexing complexity."); - for (int batchIndex = 1; batchIndex < integrationFlags.Length; ++batchIndex) - { - ref var flagsForBatch = ref integrationFlags[batchIndex]; - for (int typeBatchIndex = 0; typeBatchIndex < flagsForBatch.Length; ++typeBatchIndex) - { - ref var flagsForTypeBatch = ref flagsForBatch[typeBatchIndex]; - for (int bodyIndexInConstraint = 0; bodyIndexInConstraint < flagsForTypeBatch.Length; ++bodyIndexInConstraint) - { - flagsForTypeBatch[bodyIndexInConstraint].Dispose(pool); - } - pool.Return(ref flagsForTypeBatch); - } - pool.Return(ref flagsForBatch); - pool.Return(ref coarseBatchIntegrationResponsibilities[batchIndex]); - } - pool.Return(ref integrationFlags); - pool.Return(ref coarseBatchIntegrationResponsibilities); - mergedConstrainedBodyHandles.Dispose(pool); - } - } - - public override void SolveStep2(float totalDt, IThreadDispatcher threadDispatcher = null) - { - var substepDt = totalDt / substepCount; - PoseIntegrator.Callbacks.PrepareForIntegration(substepDt); - if (threadDispatcher == null) - { - var inverseDt = 1f / substepDt; - ref var activeSet = ref ActiveSet; - var batchCount = activeSet.Batches.Count; - var incrementalUpdateFilter = default(IncrementalContactDataUpdateFilter); - for (int substepIndex = 0; substepIndex < substepCount; ++substepIndex) - { - if (substepIndex > 0) - { - for (int i = 0; i < batchCount; ++i) - { - ref var batch = ref activeSet.Batches[i]; - for (int j = 0; j < batch.TypeBatches.Count; ++j) - { - ref var typeBatch = ref batch.TypeBatches[j]; - if (incrementalUpdateFilter.AllowType(typeBatch.TypeId)) - TypeProcessors[typeBatch.TypeId].IncrementallyUpdateContactData(ref typeBatch, bodies, substepDt, inverseDt, 0, typeBatch.BundleCount); - } - } - PoseIntegrator.IntegrateKinematicPosesAndVelocities(ConstrainedKinematicHandles.Span.Slice(ConstrainedKinematicHandles.Count), 0, BundleIndexing.GetBundleCount(ConstrainedKinematicHandles.Count), substepDt, 0); - } - else - { - if (PoseIntegrator.Callbacks.IntegrateVelocityForKinematics) - PoseIntegrator.IntegrateKinematicVelocities(ConstrainedKinematicHandles.Span.Slice(ConstrainedKinematicHandles.Count), 0, BundleIndexing.GetBundleCount(ConstrainedKinematicHandles.Count), substepDt, 0); - } - for (int i = 0; i < batchCount; ++i) - { - ref var batch = ref activeSet.Batches[i]; - ref var integrationFlagsForBatch = ref integrationFlags[i]; - for (int j = 0; j < batch.TypeBatches.Count; ++j) - { - ref var typeBatch = ref batch.TypeBatches[j]; - if (substepIndex == 0) - { - WarmStartBlock(0, i, j, 0, typeBatch.BundleCount, ref typeBatch, TypeProcessors[typeBatch.TypeId], substepDt, inverseDt); - } - else - { - WarmStartBlock(0, i, j, 0, typeBatch.BundleCount, ref typeBatch, TypeProcessors[typeBatch.TypeId], substepDt, inverseDt); - } - } - } - for (int iterationIndex = 0; iterationIndex < IterationCount; ++iterationIndex) - { - for (int i = 0; i < batchCount; ++i) - { - ref var batch = ref activeSet.Batches[i]; - for (int j = 0; j < batch.TypeBatches.Count; ++j) - { - ref var typeBatch = ref batch.TypeBatches[j]; - TypeProcessors[typeBatch.TypeId].SolveStep2(ref typeBatch, bodies, substepDt, inverseDt, 0, typeBatch.BundleCount); - } - } - } - } - } - else - { - ExecuteMultithreaded2(substepDt, threadDispatcher, solveStep2Worker2); - } - } - } -} diff --git a/BepuPhysics/SubsteppingTimestepper.cs b/BepuPhysics/SubsteppingTimestepper.cs new file mode 100644 index 000000000..287bed0b3 --- /dev/null +++ b/BepuPhysics/SubsteppingTimestepper.cs @@ -0,0 +1,49 @@ +using System; +using System.Collections.Generic; +using System.Diagnostics; +using System.Text; +using BepuUtilities; + +namespace BepuPhysics +{ + /// + /// Updates the simulation in the order of: sleeper -> predict body bounding boxes -> collision detection -> substepping solve -> data structure optimization. + /// Each substep of the solve simulates and integrates a sub-timestep of length dt/substepCount. + /// + public class SubsteppingTimestepper : ITimestepper + { + /// + /// Fires after the sleeper completes and before bodies are integrated. + /// + public event TimestepperStageHandler Slept; + /// + /// Fires after bodies have their bounding boxes updated for the frame's predicted motion and before collision detection. + /// + public event TimestepperStageHandler BeforeCollisionDetection; + /// + /// Fires after all collisions have been identified, but before the substep loop begins. + /// + public event TimestepperStageHandler CollisionsDetected; + /// + /// Fires after the solver executes and before the final integration step. + /// + public event TimestepperStageHandler ConstraintsSolved; + + public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDispatcher = null) + { + simulation.Sleep(threadDispatcher); + Slept?.Invoke(dt, threadDispatcher); + + simulation.PredictBoundingBoxes(dt, threadDispatcher); + BeforeCollisionDetection?.Invoke(dt, threadDispatcher); + + simulation.CollisionDetection(dt, threadDispatcher); + CollisionsDetected?.Invoke(dt, threadDispatcher); + + simulation.Solve(dt); + ConstraintsSolved?.Invoke(dt, threadDispatcher); + + simulation.IncrementallyOptimizeDataStructures(threadDispatcher); + } + } +} diff --git a/DemoTests/ConstraintDescriptionMappingTests.cs b/DemoTests/ConstraintDescriptionMappingTests.cs index 9d8c9b28f..0881ffd18 100644 --- a/DemoTests/ConstraintDescriptionMappingTests.cs +++ b/DemoTests/ConstraintDescriptionMappingTests.cs @@ -25,7 +25,7 @@ static void FillWithRandomBytes(ref T item, Random random) where T : struct } static void Test(BufferPool pool, Random random, int constraintTypeBodyCount) where T : unmanaged, IConstraintDescription { - var simulation = Simulation.Create(pool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(), new EmbeddedSubsteppingTimestepper2(4), 1); + var simulation = Simulation.Create(pool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(), new SubsteppingTimestepper(4), 1); const int bodyCount = 2048; diff --git a/Demos/Demos/BlockChainDemo.cs b/Demos/Demos/BlockChainDemo.cs index 8295f390e..7d504820b 100644 --- a/Demos/Demos/BlockChainDemo.cs +++ b/Demos/Demos/BlockChainDemo.cs @@ -28,7 +28,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //The PositionFirstTimestepper is the simplest timestepping mode, but since it integrates velocity into position at the start of the frame, directly modified velocities outside of the timestep //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(3), 1); var boxShape = new Box(1, 1, 1); var boxInertia = boxShape.ComputeInertia(1); diff --git a/Demos/Demos/BouncinessDemo.cs b/Demos/Demos/BouncinessDemo.cs index cf8432692..d63789012 100644 --- a/Demos/Demos/BouncinessDemo.cs +++ b/Demos/Demos/BouncinessDemo.cs @@ -86,7 +86,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //That allows higher stiffnesses to be used since collisions last longer relative to the solver timestep duration. //(Note that substepping tends to be an extremely strong simulation stabilizer, so you can usually get away with lower solver iteration counts for better performance.) var collidableMaterials = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new BounceCallbacks() { CollidableMaterials = collidableMaterials }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), solverIterationCount: 2); + Simulation = Simulation.Create(BufferPool, new BounceCallbacks() { CollidableMaterials = collidableMaterials }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), solverIterationCount: 2); var shape = new Sphere(1); var ballDescription = BodyDescription.CreateDynamic(RigidPose.Identity, shape.ComputeInertia(1), new(Simulation.Shapes.Add(shape), ContinuousDetection.Discrete(20, 20)), 1e-2f); diff --git a/Demos/Demos/Cars/CarDemo.cs b/Demos/Demos/Cars/CarDemo.cs index c7b3907d4..46646e749 100644 --- a/Demos/Demos/Cars/CarDemo.cs +++ b/Demos/Demos/Cars/CarDemo.cs @@ -45,7 +45,7 @@ public override void Initialize(ContentArchive content, Camera camera) //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. //Simulation = Simulation.Create(BufferPool, new CarCallbacks() { Properties = properties }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); - Simulation = Simulation.Create(BufferPool, new CarCallbacks() { Properties = properties }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), 1); + Simulation = Simulation.Create(BufferPool, new CarCallbacks() { Properties = properties }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(3), 1); var builder = new CompoundBuilder(BufferPool, Simulation.Shapes, 2); diff --git a/Demos/Demos/Characters/CharacterDemo.cs b/Demos/Demos/Characters/CharacterDemo.cs index ffd7e5581..40ea4f511 100644 --- a/Demos/Demos/Characters/CharacterDemo.cs +++ b/Demos/Demos/Characters/CharacterDemo.cs @@ -28,7 +28,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //The PositionFirstTimestepper is the simplest timestepping mode, but since it integrates velocity into position at the start of the frame, directly modified velocities outside of the timestep //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. - Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), 1); + Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(3), 1); CreateCharacter(new Vector3(0, 2, -4)); diff --git a/Demos/Demos/ClothDemo.cs b/Demos/Demos/ClothDemo.cs index bf699cbdf..3174db417 100644 --- a/Demos/Demos/ClothDemo.cs +++ b/Demos/Demos/ClothDemo.cs @@ -188,7 +188,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0; var filters = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new ClothCallbacks() { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); + Simulation = Simulation.Create(BufferPool, new ClothCallbacks() { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); rolloverInfo = new RolloverInfo(); bool KinematicTopCorners(int rowIndex, int columnIndex, int width, int height) diff --git a/Demos/Demos/CollisionQueryDemo.cs b/Demos/Demos/CollisionQueryDemo.cs index 76f0b6eb1..5e6d3bcad 100644 --- a/Demos/Demos/CollisionQueryDemo.cs +++ b/Demos/Demos/CollisionQueryDemo.cs @@ -27,7 +27,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Yaw = 0; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(new Box(100, 1, 100)))); diff --git a/Demos/Demos/ColosseumDemo.cs b/Demos/Demos/ColosseumDemo.cs index b13f12898..ff4f47ce3 100644 --- a/Demos/Demos/ColosseumDemo.cs +++ b/Demos/Demos/ColosseumDemo.cs @@ -64,7 +64,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = MathHelper.Pi * 0.2f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), solverIterationCount: 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), solverIterationCount: 1); var ringBoxShape = new Box(0.5f, 1, 3); var boxDescription = BodyDescription.CreateDynamic(new Vector3(), ringBoxShape.ComputeInertia(1), Simulation.Shapes.Add(ringBoxShape), 0.01f); diff --git a/Demos/Demos/CompoundTestDemo.cs b/Demos/Demos/CompoundTestDemo.cs index a1e249ace..5df6c7f01 100644 --- a/Demos/Demos/CompoundTestDemo.cs +++ b/Demos/Demos/CompoundTestDemo.cs @@ -19,7 +19,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //The PositionFirstTimestepper is the simplest timestepping mode, but since it integrates velocity into position at the start of the frame, directly modified velocities outside of the timestep //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10f, 0)), new EmbeddedSubsteppingTimestepper2(8), 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10f, 0)), new SubsteppingTimestepper(8), 1); using (var compoundBuilder = new CompoundBuilder(BufferPool, Simulation.Shapes, 8)) { diff --git a/Demos/Demos/ContactEventsDemo.cs b/Demos/Demos/ContactEventsDemo.cs index 7eb0ad3ac..025d35dc1 100644 --- a/Demos/Demos/ContactEventsDemo.cs +++ b/Demos/Demos/ContactEventsDemo.cs @@ -702,7 +702,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi; events = new ContactEvents(ThreadDispatcher, BufferPool); - Simulation = Simulation.Create(BufferPool, new ContactEventCallbacks(events), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); + Simulation = Simulation.Create(BufferPool, new ContactEventCallbacks(events), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); eventHandler = new EventHandler(Simulation, BufferPool); var listenedBody1 = Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 5, 0), 1, Simulation.Shapes, new Box(1, 2, 3))); diff --git a/Demos/Demos/ContinuousCollisionDetectionDemo.cs b/Demos/Demos/ContinuousCollisionDetectionDemo.cs index c237bfa40..db1c7124d 100644 --- a/Demos/Demos/ContinuousCollisionDetectionDemo.cs +++ b/Demos/Demos/ContinuousCollisionDetectionDemo.cs @@ -54,7 +54,7 @@ public override void Initialize(ContentArchive content, Camera camera) //Having objects bounce a bunch on impact makes it harder to see. Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(240, 1), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 1f }, - new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 2); + new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 2); var shape = new Box(1, 1, 1); var inertia = shape.ComputeInertia(1); diff --git a/Demos/Demos/CustomVoxelCollidableDemo.cs b/Demos/Demos/CustomVoxelCollidableDemo.cs index 12a01b514..42bc649a7 100644 --- a/Demos/Demos/CustomVoxelCollidableDemo.cs +++ b/Demos/Demos/CustomVoxelCollidableDemo.cs @@ -374,7 +374,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = MathHelper.Pi * 0.05f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); //The narrow phase must be notified about the existence of the new collidable type. For every pair type we want to support, a collision task must be registered. //All of the default engine types are registered upon simulation creation by a call to DefaultTypes.CreateDefaultCollisionTaskRegistry. diff --git a/Demos/Demos/FountainStressTestDemo.cs b/Demos/Demos/FountainStressTestDemo.cs index 523345d59..2ca53cb67 100644 --- a/Demos/Demos/FountainStressTestDemo.cs +++ b/Demos/Demos/FountainStressTestDemo.cs @@ -24,7 +24,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = MathHelper.Pi * 0.1f; //Using minimum sized allocations forces as many resizes as possible. //Note the low solverFallbackBatchThreshold- we want the fallback batches to get tested thoroughly. - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), 1, solverFallbackBatchThreshold: 2, initialAllocationSizes: + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(3), 1, solverFallbackBatchThreshold: 2, initialAllocationSizes: new SimulationAllocationSizes { Bodies = 1, diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index d05d7c518..5f292d3cc 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -710,7 +710,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper(1)); - Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0), 0, 0), new EmbeddedSubsteppingTimestepper2(3), solverIterationCount: 1); + Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0), 0, 0), new SubsteppingTimestepper(3), solverIterationCount: 1); //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper2(), solverIterationCount: 60); //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)), new SubsteppingTimestepper2(3), solverIterationCount: 1); diff --git a/Demos/Demos/PlanetDemo.cs b/Demos/Demos/PlanetDemo.cs index 7f4b2e3b4..a09a2e3a8 100644 --- a/Demos/Demos/PlanetDemo.cs +++ b/Demos/Demos/PlanetDemo.cs @@ -59,7 +59,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new PlanetaryGravityCallbacks() { PlanetCenter = new Vector3(), Gravity = 100000 }, new EmbeddedSubsteppingTimestepper2(4), 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new PlanetaryGravityCallbacks() { PlanetCenter = new Vector3(), Gravity = 100000 }, new SubsteppingTimestepper(4), 1); Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(new Sphere(50)))); diff --git a/Demos/Demos/PyramidDemo.cs b/Demos/Demos/PyramidDemo.cs index d1ac1e8c4..fd5c082a9 100644 --- a/Demos/Demos/PyramidDemo.cs +++ b/Demos/Demos/PyramidDemo.cs @@ -25,7 +25,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //The PositionFirstTimestepper is the simplest timestepping mode, but since it integrates velocity into position at the start of the frame, directly modified velocities outside of the timestep //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), solverIterationCount: 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(3), solverIterationCount: 1); //Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); var boxShape = new Box(1, 1, 1); diff --git a/Demos/Demos/RagdollDemo.cs b/Demos/Demos/RagdollDemo.cs index 49b0883b8..0952f77dd 100644 --- a/Demos/Demos/RagdollDemo.cs +++ b/Demos/Demos/RagdollDemo.cs @@ -536,7 +536,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //The PositionFirstTimestepper is the simplest timestepping mode, but since it integrates velocity into position at the start of the frame, directly modified velocities outside of the timestep //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. - Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks { CollisionFilters = collisionFilters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), 1); + Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks { CollisionFilters = collisionFilters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(3), 1); int ragdollIndex = 0; var spacing = new Vector3(2f, 3, 1); diff --git a/Demos/Demos/RayCastingDemo.cs b/Demos/Demos/RayCastingDemo.cs index d7de2b859..37204376e 100644 --- a/Demos/Demos/RayCastingDemo.cs +++ b/Demos/Demos/RayCastingDemo.cs @@ -63,7 +63,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-20f, 13, -20f); camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = MathHelper.Pi * 0.1f; - Simulation = Simulation.Create(BufferPool, new NoCollisionCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); + Simulation = Simulation.Create(BufferPool, new NoCollisionCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); diff --git a/Demos/Demos/RopeStabilityDemo.cs b/Demos/Demos/RopeStabilityDemo.cs index a78d051b9..336a9cb41 100644 --- a/Demos/Demos/RopeStabilityDemo.cs +++ b/Demos/Demos/RopeStabilityDemo.cs @@ -97,7 +97,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //So, even though you can avoid the need for these kinds of hacks, it's good to know that they exist should you find yourself in a circumstance where substepping isn't viable. Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(120, 1), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 2f }, - new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); + new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); rolloverInfo = new RolloverInfo(); var smallWreckingBall = new Sphere(1); diff --git a/Demos/Demos/RopeTwistDemo.cs b/Demos/Demos/RopeTwistDemo.cs index bbe308982..8e66b9995 100644 --- a/Demos/Demos/RopeTwistDemo.cs +++ b/Demos/Demos/RopeTwistDemo.cs @@ -83,7 +83,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var filters = new CollidableProperty(); Simulation = Simulation.Create(BufferPool, new RopeNarrowPhaseCallbacks { ContactSpringiness = new SpringSettings(2000, 1), Filters = filters }, - new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(60), solverIterationCount: 1, solverFallbackBatchThreshold: 64); + new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(60), solverIterationCount: 1, solverFallbackBatchThreshold: 64); //Simulation = Simulation.Create(BufferPool, // new RopeNarrowPhaseCallbacks { ContactSpringiness = new SpringSettings(2000, 1), Filters = filters }, // new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper2(60), solverIterationCount: 1, solverFallbackBatchThreshold: 512); diff --git a/Demos/Demos/SimpleSelfContainedDemo.cs b/Demos/Demos/SimpleSelfContainedDemo.cs index cc44e2128..cb803cc1b 100644 --- a/Demos/Demos/SimpleSelfContainedDemo.cs +++ b/Demos/Demos/SimpleSelfContainedDemo.cs @@ -213,7 +213,7 @@ public static void Run() //PositionLastTimestepper avoids that by running collision detection and the solver first at the cost of a tiny amount of overhead. //(You could avoid the issue with PositionFirstTimestepper by modifying velocities in the PositionFirstTimestepper's BeforeCollisionDetection callback //instead of outside the timestep, too, but it's a little more complicated.) - var simulation = Simulation.Create(bufferPool, new NarrowPhaseCallbacks(), new PoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); + var simulation = Simulation.Create(bufferPool, new NarrowPhaseCallbacks(), new PoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); //Drop a ball on a big static box. var sphere = new Sphere(1); diff --git a/Demos/Demos/SolverContactEnumerationDemo.cs b/Demos/Demos/SolverContactEnumerationDemo.cs index 18e2d54f9..7d23f37bc 100644 --- a/Demos/Demos/SolverContactEnumerationDemo.cs +++ b/Demos/Demos/SolverContactEnumerationDemo.cs @@ -26,7 +26,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Yaw = 0; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); //Drop a pyramid on top of the sensor so there are more contacts to look at. var boxShape = new Box(1, 1, 1); diff --git a/Demos/Demos/Sponsors/SponsorDemo.cs b/Demos/Demos/Sponsors/SponsorDemo.cs index 00479f4e9..f54458cc8 100644 --- a/Demos/Demos/Sponsors/SponsorDemo.cs +++ b/Demos/Demos/Sponsors/SponsorDemo.cs @@ -109,7 +109,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0.4f; characterControllers = new CharacterControllers(BufferPool); - Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characterControllers), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); + Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characterControllers), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); DemoMeshHelper.LoadModel(content, BufferPool, @"Content\newt.obj", new Vector3(-10, 10, -10), out var newtMesh); var newtShape = Simulation.Shapes.Add(newtMesh); diff --git a/Demos/Demos/SubsteppingDemo.cs b/Demos/Demos/SubsteppingDemo.cs index de8545169..e8e2a8e8c 100644 --- a/Demos/Demos/SubsteppingDemo.cs +++ b/Demos/Demos/SubsteppingDemo.cs @@ -17,16 +17,14 @@ public class SubsteppingDemo : Demo { RolloverInfo rolloverInfo; - EmbeddedSubsteppingTimestepper2 timestepper; public unsafe override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(0, 25, 80); camera.Yaw = 0; camera.Pitch = 0; - timestepper = new EmbeddedSubsteppingTimestepper2(8); Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(120, 120), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 2f }, - new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), timestepper, 8); + new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(), 8); rolloverInfo = new RolloverInfo(); { @@ -111,31 +109,31 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) } public override void Update(Window window, Camera camera, Input input, float dt) { - var substepCountChange = (int)MathF.Max(1f, timestepper.SubstepCount * 0.25f); - var iterationCountChange = (int)MathF.Max(1f, Simulation.Solver.IterationCount * 0.25f); + var substepCountChange = (int)MathF.Max(1f, Simulation.Solver.SubstepCount * 0.25f); + var iterationCountChange = (int)MathF.Max(1f, Simulation.Solver.VelocityIterationCount * 0.25f); if (input.WasPushed(OpenTK.Input.Key.Z)) { - timestepper.SubstepCount = Math.Max(1, timestepper.SubstepCount - substepCountChange); + Simulation.Solver.SubstepCount = Math.Max(1, Simulation.Solver.SubstepCount - substepCountChange); } if (input.WasPushed(OpenTK.Input.Key.X)) { - timestepper.SubstepCount = Math.Min(8192, timestepper.SubstepCount + substepCountChange); + Simulation.Solver.SubstepCount = Math.Min(8192, Simulation.Solver.SubstepCount + substepCountChange); } if (input.WasPushed(OpenTK.Input.Key.C)) { - Simulation.Solver.IterationCount = Math.Max(1, Simulation.Solver.IterationCount - iterationCountChange); + Simulation.Solver.VelocityIterationCount = Math.Max(1, Simulation.Solver.VelocityIterationCount - iterationCountChange); } if (input.WasPushed(OpenTK.Input.Key.V)) { - Simulation.Solver.IterationCount = Math.Min(8192, Simulation.Solver.IterationCount + iterationCountChange); + Simulation.Solver.VelocityIterationCount = Math.Min(8192, Simulation.Solver.VelocityIterationCount + iterationCountChange); } base.Update(window, camera, input, dt); } public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) { - renderer.TextBatcher.Write(text.Clear().Append("Substep count: ").Append(timestepper.SubstepCount), new Vector2(16, renderer.Surface.Resolution.Y - 64), 16, new Vector3(1), font); - renderer.TextBatcher.Write(text.Clear().Append("Solver iteration count: ").Append(Simulation.Solver.IterationCount), new Vector2(16, renderer.Surface.Resolution.Y - 48), 16, new Vector3(1), font); + renderer.TextBatcher.Write(text.Clear().Append("Substep count: ").Append(Simulation.Solver.SubstepCount), new Vector2(16, renderer.Surface.Resolution.Y - 64), 16, new Vector3(1), font); + renderer.TextBatcher.Write(text.Clear().Append("Solver iteration count: ").Append(Simulation.Solver.VelocityIterationCount), new Vector2(16, renderer.Surface.Resolution.Y - 48), 16, new Vector3(1), font); renderer.TextBatcher.Write(text.Clear().Append("Press Z/X to change substep count, C/V to change solver iteration count."), new Vector2(16, renderer.Surface.Resolution.Y - 32), 16, new Vector3(1), font); rolloverInfo.Render(renderer, camera, input, text, font); base.Render(renderer, camera, input, text, font); diff --git a/Demos/Demos/SweepDemo.cs b/Demos/Demos/SweepDemo.cs index 173c1c80d..674c8f9de 100644 --- a/Demos/Demos/SweepDemo.cs +++ b/Demos/Demos/SweepDemo.cs @@ -31,7 +31,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(0, 10, 40); camera.Yaw = 0; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); var box = new Box(2f, 2f, 2f); var capsule = new Capsule(1f, 1f); diff --git a/Demos/Demos/Tanks/TankDemo.cs b/Demos/Demos/Tanks/TankDemo.cs index 25c6ce37a..063941410 100644 --- a/Demos/Demos/Tanks/TankDemo.cs +++ b/Demos/Demos/Tanks/TankDemo.cs @@ -51,7 +51,7 @@ public override void Initialize(ContentArchive content, Camera camera) bodyProperties = new CollidableProperty(); //We assign velocities outside of the timestep to fire bullets, so using the PositionLastTimestepper avoids integrating those velocities into positions before the solver has a chance to intervene. //We could have also modified velocities in the PositionFirstTimestepper's BeforeCollisionDetection callback, but it's just a little simpler to do this with very little cost. - Simulation = Simulation.Create(BufferPool, new TankCallbacks() { Properties = bodyProperties }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), 1); + Simulation = Simulation.Create(BufferPool, new TankCallbacks() { Properties = bodyProperties }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(3), 1); var builder = new CompoundBuilder(BufferPool, Simulation.Shapes, 2); builder.Add(new Box(1.85f, 0.7f, 4.73f), RigidPose.Identity, 10); diff --git a/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs b/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs index c4ea18ed7..e1c38eeb3 100644 --- a/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs +++ b/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs @@ -20,7 +20,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-20f, 13, -20f); camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = MathHelper.Pi * 0.1f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); var shape = new Sphere(0.5f); var sphereInertia = shape.ComputeInertia(1); diff --git a/Demos/SpecializedTests/CapsuleTestDemo.cs b/Demos/SpecializedTests/CapsuleTestDemo.cs index 0ed56e247..36efd6751 100644 --- a/Demos/SpecializedTests/CapsuleTestDemo.cs +++ b/Demos/SpecializedTests/CapsuleTestDemo.cs @@ -20,7 +20,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //camera.Yaw = MathHelper.Pi ; camera.Yaw = MathHelper.Pi * 3f / 4; //camera.Pitch = MathHelper.Pi * 0.1f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); var shape = new Capsule(.5f, .5f); var localInertia = shape.ComputeInertia(1); diff --git a/Demos/SpecializedTests/CharacterTestDemo.cs b/Demos/SpecializedTests/CharacterTestDemo.cs index e20e1c802..2707f558f 100644 --- a/Demos/SpecializedTests/CharacterTestDemo.cs +++ b/Demos/SpecializedTests/CharacterTestDemo.cs @@ -21,7 +21,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = MathHelper.Pi * 0.05f; var masks = new CollidableProperty(); characters = new CharacterControllers(BufferPool); - Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); + Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); var random = new Random(5); for (int i = 0; i < 8192; ++i) diff --git a/Demos/SpecializedTests/ClothLatticeDemo.cs b/Demos/SpecializedTests/ClothLatticeDemo.cs index 09e695f39..821b322c1 100644 --- a/Demos/SpecializedTests/ClothLatticeDemo.cs +++ b/Demos/SpecializedTests/ClothLatticeDemo.cs @@ -20,7 +20,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-120, 30, -120); camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = 0.1f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); //Build a grid of shapes to be connected. var clothNodeShape = new Sphere(0.5f); diff --git a/Demos/SpecializedTests/CompoundBoundTests.cs b/Demos/SpecializedTests/CompoundBoundTests.cs index 91ff8144a..570a09f35 100644 --- a/Demos/SpecializedTests/CompoundBoundTests.cs +++ b/Demos/SpecializedTests/CompoundBoundTests.cs @@ -18,7 +18,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(-10, 0, -10); camera.Yaw = MathHelper.Pi * 3f / 4; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); } diff --git a/Demos/SpecializedTests/CompoundCollisionIndicesTest.cs b/Demos/SpecializedTests/CompoundCollisionIndicesTest.cs index 566c0fb1a..8d579df30 100644 --- a/Demos/SpecializedTests/CompoundCollisionIndicesTest.cs +++ b/Demos/SpecializedTests/CompoundCollisionIndicesTest.cs @@ -72,7 +72,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(0, 4, -6); camera.Yaw = MathHelper.Pi; - Simulation = Simulation.Create(BufferPool, new IndexReportingNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, 0f, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); + Simulation = Simulation.Create(BufferPool, new IndexReportingNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, 0f, 0)), new SubsteppingTimestepper(4), 1); var builder = new CompoundBuilder(BufferPool, Simulation.Shapes, 4); builder.Add(new Sphere(0.5f), new Vector3(-1, 0, 0), 1); diff --git a/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs b/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs index 49500b430..5d6fd32cb 100644 --- a/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs +++ b/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs @@ -19,7 +19,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = 0; Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(30, 1), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 2f }, - new DemoPoseIntegratorCallbacks(new Vector3(0, -0.1f, 0), 0, 0), new EmbeddedSubsteppingTimestepper2(3), 1); + new DemoPoseIntegratorCallbacks(new Vector3(0, -0.1f, 0), 0, 0), new SubsteppingTimestepper(3), 1); var shapeA = new Box(.75f, 1, .5f); var shapeIndexA = Simulation.Shapes.Add(shapeA); diff --git a/Demos/SpecializedTests/ConstraintTestDemo.cs b/Demos/SpecializedTests/ConstraintTestDemo.cs index f4796d85e..9c5a7b946 100644 --- a/Demos/SpecializedTests/ConstraintTestDemo.cs +++ b/Demos/SpecializedTests/ConstraintTestDemo.cs @@ -24,7 +24,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(25, 4, 40); camera.Yaw = 0; Simulation = Simulation.Create(BufferPool, - new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(30, 1), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 2f }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), 1); + new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(30, 1), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 2f }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(3), 1); //new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(30, 1), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 2f }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper(), 8); var shapeA = new Box(.75f, 1, .5f); diff --git a/Demos/SpecializedTests/ConvexHullTestDemo.cs b/Demos/SpecializedTests/ConvexHullTestDemo.cs index 5a9c4830f..2d9911133 100644 --- a/Demos/SpecializedTests/ConvexHullTestDemo.cs +++ b/Demos/SpecializedTests/ConvexHullTestDemo.cs @@ -27,7 +27,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Yaw = 0; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), solverIterationCount: 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), solverIterationCount: 1); //var meshContent = content.Load("Content\\newt.obj"); diff --git a/Demos/SpecializedTests/CylinderTestDemo.cs b/Demos/SpecializedTests/CylinderTestDemo.cs index d92f61ccb..f4fc48e4c 100644 --- a/Demos/SpecializedTests/CylinderTestDemo.cs +++ b/Demos/SpecializedTests/CylinderTestDemo.cs @@ -170,7 +170,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0; camera.Yaw = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, 0f, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, 0f, 0)), new SubsteppingTimestepper(4), 1); var cylinderShape = new Cylinder(1f, .2f); var cylinder = BodyDescription.CreateDynamic(new Vector3(10f, 3, 0), cylinderShape.ComputeInertia(1), new(Simulation.Shapes.Add(cylinderShape), ContinuousDetection.CreatePassive(1000f, 1000f)), 0.01f); diff --git a/Demos/SpecializedTests/GyroscopeTestDemo.cs b/Demos/SpecializedTests/GyroscopeTestDemo.cs index 72c4cc665..05357a04d 100644 --- a/Demos/SpecializedTests/GyroscopeTestDemo.cs +++ b/Demos/SpecializedTests/GyroscopeTestDemo.cs @@ -57,7 +57,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0; //Note the lack of damping- we want the gyroscope to keep spinning. - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new GyroscopicIntegratorCallbacks(new Vector3(0, -10, 0), 0f, 0f), new EmbeddedSubsteppingTimestepper2(4), 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new GyroscopicIntegratorCallbacks(new Vector3(0, -10, 0), 0f, 0f), new SubsteppingTimestepper(4), 1); Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(new Box(100, 1, 100)))); diff --git a/Demos/SpecializedTests/Media/BedsheetDemo.cs b/Demos/SpecializedTests/Media/BedsheetDemo.cs index b40010c0e..2e1fd566c 100644 --- a/Demos/SpecializedTests/Media/BedsheetDemo.cs +++ b/Demos/SpecializedTests/Media/BedsheetDemo.cs @@ -107,7 +107,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = MathF.PI * 0.1f; var filters = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new ClothCallbacks() { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -50, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); + Simulation = Simulation.Create(BufferPool, new ClothCallbacks() { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -50, 0)), new SubsteppingTimestepper(4), 1); rolloverInfo = new RolloverInfo(); bool FullyDynamic(int rowIndex, int columnIndex, int width, int height) diff --git a/Demos/SpecializedTests/Media/ColosseumVideoDemo.cs b/Demos/SpecializedTests/Media/ColosseumVideoDemo.cs index eb17ad602..fdf8df16a 100644 --- a/Demos/SpecializedTests/Media/ColosseumVideoDemo.cs +++ b/Demos/SpecializedTests/Media/ColosseumVideoDemo.cs @@ -68,7 +68,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = MathHelper.Pi * 0.2f; characters = new CharacterControllers(BufferPool); - Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); + Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); var ringBoxShape = new Box(0.5f, 1.5f, 3); var boxDescription = BodyDescription.CreateDynamic(new Vector3(), ringBoxShape.ComputeInertia(1), Simulation.Shapes.Add(ringBoxShape), 0.01f); diff --git a/Demos/SpecializedTests/Media/NewtDemandingSacrificeVideoDemo.cs b/Demos/SpecializedTests/Media/NewtDemandingSacrificeVideoDemo.cs index 73404e8bd..7f4dd55ab 100644 --- a/Demos/SpecializedTests/Media/NewtDemandingSacrificeVideoDemo.cs +++ b/Demos/SpecializedTests/Media/NewtDemandingSacrificeVideoDemo.cs @@ -20,7 +20,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = MathHelper.Pi * -0.05f; filters = new CollidableProperty(BufferPool); - Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks() { CollisionFilters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); + Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks() { CollisionFilters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), Simulation.Shapes.Add(new Box(1500, 1, 1500)))); Simulation.Statics.Add(new StaticDescription(new Vector3(0, 10, 0), Simulation.Shapes.Add(new Box(70, 20, 80)))); @@ -29,7 +29,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Simulation.Statics.Add(new StaticDescription(new Vector3(0, 2.5f, 0), Simulation.Shapes.Add(new Box(100, 5, 110)))); //High fidelity simulation isn't super important on this one. - Simulation.Solver.IterationCount = 2; + Simulation.Solver.VelocityIterationCount = 2; DemoMeshHelper.LoadModel(content, BufferPool, "Content\\newt.obj", new Vector3(30), out var mesh); Simulation.Statics.Add(new StaticDescription(new Vector3(0, 20, 0), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, 0), Simulation.Shapes.Add(mesh))); diff --git a/Demos/SpecializedTests/Media/NewtVideoDemo.cs b/Demos/SpecializedTests/Media/NewtVideoDemo.cs index ec3501757..0c94fd410 100644 --- a/Demos/SpecializedTests/Media/NewtVideoDemo.cs +++ b/Demos/SpecializedTests/Media/NewtVideoDemo.cs @@ -21,7 +21,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = MathHelper.Pi * 0.15f; var filters = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); + Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); var meshContent = content.Load("Content\\newt.obj"); float cellSize = 0.1f; diff --git a/Demos/SpecializedTests/Media/PyramidVideoDemo.cs b/Demos/SpecializedTests/Media/PyramidVideoDemo.cs index 16b881270..24bf9761e 100644 --- a/Demos/SpecializedTests/Media/PyramidVideoDemo.cs +++ b/Demos/SpecializedTests/Media/PyramidVideoDemo.cs @@ -22,7 +22,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-70, 8, 318); camera.Yaw = MathHelper.Pi * 1f / 4; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); var boxShape = new Box(1, 1, 1); var boxInertia = boxShape.ComputeInertia(1); diff --git a/Demos/SpecializedTests/Media/ShrinkwrappedNewtsVideoDemo.cs b/Demos/SpecializedTests/Media/ShrinkwrappedNewtsVideoDemo.cs index a7a7a61e0..ef6a56b82 100644 --- a/Demos/SpecializedTests/Media/ShrinkwrappedNewtsVideoDemo.cs +++ b/Demos/SpecializedTests/Media/ShrinkwrappedNewtsVideoDemo.cs @@ -18,7 +18,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = 3 * MathHelper.Pi / 4; camera.Pitch = 0;// MathHelper.Pi * 0.15f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); var meshContent = content.Load("Content\\newt.obj"); diff --git a/Demos/SpecializedTests/MeshMeshTestDemo.cs b/Demos/SpecializedTests/MeshMeshTestDemo.cs index 64fc5cf45..ab0d49ab8 100644 --- a/Demos/SpecializedTests/MeshMeshTestDemo.cs +++ b/Demos/SpecializedTests/MeshMeshTestDemo.cs @@ -21,7 +21,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(0, 8, -10); camera.Yaw = MathHelper.Pi; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); DemoMeshHelper.LoadModel(content, BufferPool, @"Content\newt.obj", Vector3.One, out var mesh); var approximateInertia = new Box(2.5f, 1, 4).ComputeInertia(1); diff --git a/Demos/SpecializedTests/MeshReductionTestDemo.cs b/Demos/SpecializedTests/MeshReductionTestDemo.cs index fad71d5f6..13610a47a 100644 --- a/Demos/SpecializedTests/MeshReductionTestDemo.cs +++ b/Demos/SpecializedTests/MeshReductionTestDemo.cs @@ -25,7 +25,7 @@ public override void Initialize(ContentArchive content, Camera camera) Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks() { ContactSpringiness = new(30, 1), MaximumRecoveryVelocity = 2, FrictionCoefficient = 0 }, - new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); + new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); var builder = new CompoundBuilder(BufferPool, Simulation.Shapes, 2); builder.Add(new Box(1.85f, 0.7f, 4.73f), RigidPose.Identity, 10); diff --git a/Demos/SpecializedTests/MeshSerializationTestDemo.cs b/Demos/SpecializedTests/MeshSerializationTestDemo.cs index 93b04558a..48f938cbc 100644 --- a/Demos/SpecializedTests/MeshSerializationTestDemo.cs +++ b/Demos/SpecializedTests/MeshSerializationTestDemo.cs @@ -17,7 +17,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); var startTime = Stopwatch.GetTimestamp(); DemoMeshHelper.CreateDeformedPlane(1025, 1025, (x, y) => new Vector3(x * 0.125f, MathF.Sin(x) + MathF.Sin(y), y * 0.125f), Vector3.One, BufferPool, out var originalMesh); diff --git a/Demos/SpecializedTests/MeshTestDemo.cs b/Demos/SpecializedTests/MeshTestDemo.cs index 8d80978c5..099f94523 100644 --- a/Demos/SpecializedTests/MeshTestDemo.cs +++ b/Demos/SpecializedTests/MeshTestDemo.cs @@ -19,7 +19,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //camera.Yaw = MathHelper.Pi ; camera.Yaw = MathHelper.Pi * 3f / 4; //camera.Pitch = MathHelper.PiOver2 * 0.999f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); var box = new Box(1f, 3f, 2f); var capsule = new Capsule(1f, 1f); diff --git a/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs b/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs index 08e5ebab4..04e5d2196 100644 --- a/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs +++ b/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs @@ -22,7 +22,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-30, 8, -110); camera.Yaw = MathHelper.Pi * 3f / 4; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); var boxShape = new Box(1, 1, 1); var boxInertia = boxShape.ComputeInertia(1); diff --git a/Demos/SpecializedTests/RagdollTubeDemo.cs b/Demos/SpecializedTests/RagdollTubeDemo.cs index b4115f1f2..fc658b1ed 100644 --- a/Demos/SpecializedTests/RagdollTubeDemo.cs +++ b/Demos/SpecializedTests/RagdollTubeDemo.cs @@ -21,7 +21,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi; camera.Pitch = 0; var filters = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks { CollisionFilters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(3), 1, 64); + Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks { CollisionFilters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(3), 1, 64); int ragdollIndex = 0; var spacing = new Vector3(1.7f, 1.8f, 0.5f); diff --git a/Demos/SpecializedTests/ShapePileTestDemo.cs b/Demos/SpecializedTests/ShapePileTestDemo.cs index 796d1ad0b..845bfa84f 100644 --- a/Demos/SpecializedTests/ShapePileTestDemo.cs +++ b/Demos/SpecializedTests/ShapePileTestDemo.cs @@ -20,7 +20,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //camera.Yaw = MathHelper.Pi ; camera.Yaw = MathHelper.Pi * 3f / 4; //camera.Pitch = MathHelper.PiOver2 * 0.999f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); Simulation.Deterministic = true; var sphere = new Sphere(1.5f); diff --git a/Demos/SpecializedTests/SolverBatchTestDemo.cs b/Demos/SpecializedTests/SolverBatchTestDemo.cs index b034f9944..7b9e00fa4 100644 --- a/Demos/SpecializedTests/SolverBatchTestDemo.cs +++ b/Demos/SpecializedTests/SolverBatchTestDemo.cs @@ -20,8 +20,8 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-120, 30, -120); camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = 0.1f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); - Simulation.Solver.IterationCount = 8; + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); + Simulation.Solver.VelocityIterationCount = 8; //Build a grid of shapes to be connected. var clothNodeShape = new Sphere(0.5f); diff --git a/Demos/SpecializedTests/TriangleRayTestDemo.cs b/Demos/SpecializedTests/TriangleRayTestDemo.cs index 2bad3f82f..85f3433dc 100644 --- a/Demos/SpecializedTests/TriangleRayTestDemo.cs +++ b/Demos/SpecializedTests/TriangleRayTestDemo.cs @@ -94,7 +94,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-30, 8, -60); camera.Yaw = MathHelper.Pi * 3f / 4; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); Triangle triangle; triangle.A = new Vector3(0, 0, 0); diff --git a/Demos/SpecializedTests/TriangleTestDemo.cs b/Demos/SpecializedTests/TriangleTestDemo.cs index 946da8229..28baf0ca2 100644 --- a/Demos/SpecializedTests/TriangleTestDemo.cs +++ b/Demos/SpecializedTests/TriangleTestDemo.cs @@ -92,7 +92,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks() { ContactSpringiness = new BepuPhysics.Constraints.SpringSettings(30, 1), FrictionCoefficient = 1, MaximumRecoveryVelocity = 1 }, - new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); + new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); //var triangleDescription = new StaticDescription //{ diff --git a/Demos/SpecializedTests/VolumeQueryTests.cs b/Demos/SpecializedTests/VolumeQueryTests.cs index 48cfc59a1..e9ab2259d 100644 --- a/Demos/SpecializedTests/VolumeQueryTests.cs +++ b/Demos/SpecializedTests/VolumeQueryTests.cs @@ -61,7 +61,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-20f, 13, -20f); camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = MathHelper.Pi * 0.1f; - Simulation = Simulation.Create(BufferPool, new NoCollisionCallbacks(), new DemoPoseIntegratorCallbacks(), new EmbeddedSubsteppingTimestepper2(4), 1); + Simulation = Simulation.Create(BufferPool, new NoCollisionCallbacks(), new DemoPoseIntegratorCallbacks(), new SubsteppingTimestepper(4), 1); var sphere = new Sphere(0.5f); var shapeIndex = Simulation.Shapes.Add(sphere); From a0612eef203d213f1fd430cdcb8f8b0f65ad80c2 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 6 Dec 2021 17:10:51 -0600 Subject: [PATCH 314/947] SolveDescription added to unify solver configuration for Simulation.Create. --- BepuPhysics/Simulation.cs | 12 +- BepuPhysics/SolveDescription.cs | 120 ++++++++++++++++++ BepuPhysics/Solver.cs | 29 +++-- BepuPhysics/Solver_Solve.cs | 12 +- .../ConstraintDescriptionMappingTests.cs | 2 +- Demos/Demos/BlockChainDemo.cs | 5 +- Demos/Demos/BouncinessDemo.cs | 6 +- Demos/Demos/Cars/CarDemo.cs | 7 +- Demos/Demos/Characters/CharacterDemo.cs | 5 +- Demos/Demos/ClothDemo.cs | 2 +- Demos/Demos/CollisionQueryDemo.cs | 2 +- Demos/Demos/ColosseumDemo.cs | 2 +- Demos/Demos/CompoundTestDemo.cs | 5 +- Demos/Demos/ContactEventsDemo.cs | 2 +- .../Demos/ContinuousCollisionDetectionDemo.cs | 2 +- Demos/Demos/CustomVoxelCollidableDemo.cs | 2 +- Demos/Demos/FountainStressTestDemo.cs | 2 +- Demos/Demos/NewtDemo.cs | 8 +- Demos/Demos/PlanetDemo.cs | 2 +- Demos/Demos/PyramidDemo.cs | 6 +- Demos/Demos/RagdollDemo.cs | 5 +- Demos/Demos/RayCastingDemo.cs | 2 +- Demos/Demos/RopeStabilityDemo.cs | 2 +- Demos/Demos/RopeTwistDemo.cs | 6 +- Demos/Demos/SimpleSelfContainedDemo.cs | 2 +- Demos/Demos/SolverContactEnumerationDemo.cs | 2 +- Demos/Demos/Sponsors/SponsorDemo.cs | 2 +- Demos/Demos/SubsteppingDemo.cs | 2 +- Demos/Demos/SweepDemo.cs | 2 +- Demos/Demos/Tanks/TankDemo.cs | 4 +- .../BroadPhaseStressTestDemo.cs | 2 +- Demos/SpecializedTests/CapsuleTestDemo.cs | 2 +- Demos/SpecializedTests/CharacterTestDemo.cs | 2 +- Demos/SpecializedTests/ClothLatticeDemo.cs | 2 +- Demos/SpecializedTests/CompoundBoundTests.cs | 2 +- .../CompoundCollisionIndicesTest.cs | 2 +- .../ConstrainedKinematicIntegrationTest.cs | 2 +- Demos/SpecializedTests/ConstraintTestDemo.cs | 2 +- Demos/SpecializedTests/ConvexHullTestDemo.cs | 2 +- Demos/SpecializedTests/CylinderTestDemo.cs | 2 +- Demos/SpecializedTests/GyroscopeTestDemo.cs | 2 +- Demos/SpecializedTests/Media/BedsheetDemo.cs | 2 +- .../Media/ColosseumVideoDemo.cs | 2 +- .../Media/NewtDemandingSacrificeVideoDemo.cs | 2 +- Demos/SpecializedTests/Media/NewtVideoDemo.cs | 2 +- .../Media/PyramidVideoDemo.cs | 2 +- .../Media/ShrinkwrappedNewtsVideoDemo.cs | 2 +- Demos/SpecializedTests/MeshMeshTestDemo.cs | 2 +- .../SpecializedTests/MeshReductionTestDemo.cs | 2 +- .../MeshSerializationTestDemo.cs | 2 +- Demos/SpecializedTests/MeshTestDemo.cs | 2 +- .../PyramidAwakenerTestDemo.cs | 2 +- Demos/SpecializedTests/RagdollTubeDemo.cs | 2 +- Demos/SpecializedTests/ShapePileTestDemo.cs | 2 +- Demos/SpecializedTests/SolverBatchTestDemo.cs | 2 +- Demos/SpecializedTests/TriangleRayTestDemo.cs | 2 +- Demos/SpecializedTests/TriangleTestDemo.cs | 2 +- Demos/SpecializedTests/VolumeQueryTests.cs | 2 +- 58 files changed, 209 insertions(+), 109 deletions(-) create mode 100644 BepuPhysics/SolveDescription.cs diff --git a/BepuPhysics/Simulation.cs b/BepuPhysics/Simulation.cs index 4c76dcd0f..f82fdc07f 100644 --- a/BepuPhysics/Simulation.cs +++ b/BepuPhysics/Simulation.cs @@ -63,14 +63,12 @@ public partial class Simulation : IDisposable /// Buffer pool used to fill persistent structures and main thread ephemeral resources across the engine. /// Callbacks to use in the narrow phase. /// Callbacks to use in the pose integrator. - /// Timestepper that defines how the simulation state should be updated. - /// Number of iterations the solver should use. - /// Number of synchronized batches the solver should maintain before falling back to a lower quality jacobi hybrid solver. + /// Timestepper that defines how the simulation state should be updated. If null, is used. + /// Describes how the solver should execute, including the number of substeps and the number of velocity iterations per substep. /// Allocation sizes to initialize the simulation with. If left null, default values are chosen. /// New simulation. public static Simulation Create( - BufferPool bufferPool, TNarrowPhaseCallbacks narrowPhaseCallbacks, TPoseIntegratorCallbacks poseIntegratorCallbacks, ITimestepper timestepper, - int solverIterationCount = 8, int solverFallbackBatchThreshold = 64, SimulationAllocationSizes? initialAllocationSizes = null) + BufferPool bufferPool, TNarrowPhaseCallbacks narrowPhaseCallbacks, TPoseIntegratorCallbacks poseIntegratorCallbacks, SolveDescription solveDescription, ITimestepper timestepper = null, SimulationAllocationSizes? initialAllocationSizes = null) where TNarrowPhaseCallbacks : struct, INarrowPhaseCallbacks where TPoseIntegratorCallbacks : struct, IPoseIntegratorCallbacks { @@ -101,7 +99,7 @@ public static Simulation Create var poseIntegrator = new PoseIntegrator(simulation.Bodies, simulation.Shapes, simulation.BroadPhase, poseIntegratorCallbacks); simulation.PoseIntegrator = poseIntegrator; - simulation.Solver = new Solver(simulation.Bodies, simulation.BufferPool, solverIterationCount, solverFallbackBatchThreshold, + simulation.Solver = new Solver(simulation.Bodies, simulation.BufferPool, solveDescription, initialCapacity: initialAllocationSizes.Value.Constraints, initialIslandCapacity: initialAllocationSizes.Value.Islands, minimumCapacityPerTypeBatch: initialAllocationSizes.Value.ConstraintsPerTypeBatch, poseIntegrator); @@ -112,7 +110,7 @@ public static Simulation Create simulation.Solver.awakener = simulation.Awakener; simulation.Bodies.Initialize(simulation.Solver, simulation.Awakener, simulation.Sleeper); simulation.SolverBatchCompressor = new BatchCompressor(simulation.Solver, simulation.Bodies); - simulation.Timestepper = timestepper; + simulation.Timestepper = timestepper ?? new SubsteppingTimestepper(); var narrowPhase = new NarrowPhase(simulation, DefaultTypes.CreateDefaultCollisionTaskRegistry(), DefaultTypes.CreateDefaultSweepTaskRegistry(), diff --git a/BepuPhysics/SolveDescription.cs b/BepuPhysics/SolveDescription.cs new file mode 100644 index 000000000..fcbb18bd0 --- /dev/null +++ b/BepuPhysics/SolveDescription.cs @@ -0,0 +1,120 @@ +using System; + +namespace BepuPhysics +{ + /// + /// Callback executed to determine how many velocity iterations should be used for a given substep. + /// + /// Index of the substep to schedule velocity iterations for. + /// Number of velocity iterations to use for the substep. If nonpositive, will be used for the substep instead. + public delegate int SubstepVelocityIterationScheduler(int substepIndex); + + /// + /// Describes how the solver should schedule substeps and velocity iterations. + /// + public struct SolveDescription + { + /// + /// Number of substeps to execute each time the solver runs. + /// + public int SubstepCount; + /// + /// Number of velocity iterations to use in the solver if there is no or if it returns a non-positive value for a substep. + /// + public int VelocityIterationCount; + /// + /// Number of synchronzed constraint batches to use before using a fallback approach. + /// + public int FallbackBatchThreshold; + /// + /// Callback executed to determine how many velocity iterations should be used for a given substep. If null, or if it returns a non-positive value, the will be used instead. + /// + public SubstepVelocityIterationScheduler VelocityIterationScheduler; + + /// + /// Default number of synchronized constraint batches to use before falling back to an alternative solving method. + /// + public const int DefaultFallbackBatchThreshold = 64; + + internal void ValidateDescription() + { + if (SubstepCount < 1) + throw new ArgumentException("Substep count must be positive."); + if (VelocityIterationCount < 1) + throw new ArgumentException("Velocity iteration count must be positive."); + if (FallbackBatchThreshold < 1) + throw new ArgumentException("Fallback batch threshold must be positive."); + } + /// + /// Creates a solve description. + /// + /// Number of substeps in the solve. + /// Number of velocity iterations per substep. + /// Number of synchronzed constraint batches to use before using a fallback approach. + public SolveDescription(int substepCount, int velocityIterationCount = 1, int fallbackBatchThreshold = DefaultFallbackBatchThreshold) + { + SubstepCount = substepCount; + VelocityIterationCount = velocityIterationCount; + FallbackBatchThreshold = fallbackBatchThreshold; + VelocityIterationScheduler = null; + ValidateDescription(); + } + + /// + /// Creates a solve description. + /// + /// Number of substeps in the solve. + /// + /// Number of velocity iterations per substep for any substep that is not given a positive number of velocity iterations by the scheduler. + /// Number of synchronzed constraint batches to use before using a fallback approach. + public SolveDescription(int substepCount, SubstepVelocityIterationScheduler velocityIterationScheduler, int fallbackVelocityIterationCount = 1, int fallbackBatchThreshold = DefaultFallbackBatchThreshold) + { + SubstepCount = substepCount; + VelocityIterationCount = fallbackVelocityIterationCount; + FallbackBatchThreshold = fallbackBatchThreshold; + VelocityIterationScheduler = velocityIterationScheduler; + ValidateDescription(); + } + + /// + /// Creates a solve description. + /// + /// Number of velocity iterations to use in each substep. Number of substeps will be determined by the length of the span. + /// Number of velocity iterations per substep for any substep that is not given a positive number of velocity iterations by the scheduler. + /// Number of synchronzed constraint batches to use before using a fallback approach. + public SolveDescription(Span substepVelocityIterations, int fallbackVelocityIterationCount = 1, int fallbackBatchThreshold = DefaultFallbackBatchThreshold) + { + SubstepCount = substepVelocityIterations.Length; + VelocityIterationCount = fallbackVelocityIterationCount; + FallbackBatchThreshold = fallbackBatchThreshold; + var copy = substepVelocityIterations.ToArray(); + VelocityIterationScheduler = substepIndex => copy[substepIndex]; + ValidateDescription(); + } + + /// + /// Creates a solve description with 1 velocity iteration per substep and a fallback threshold of . + /// + /// Number of substeps per solve. + public static implicit operator SolveDescription(int substepCount) + { + return new SolveDescription(substepCount); + } + /// + /// Creates a solve description with the given number of substeps and velocity iterations per substep and a fallback threshold of . + /// + /// Number of substeps and iterations per solve. + public static implicit operator SolveDescription((int substepCount, int iterationsPerSubstep) schedule) + { + return new SolveDescription(schedule.substepCount, schedule.iterationsPerSubstep); + } + /// + /// Creates a solve description with the given number of substeps and velocity iterations per substep and a fallback threshold of . + /// + /// Number of velocity iterations to use in each substep. Number of substeps will be determined by the length of the span. + public static implicit operator SolveDescription(Span substepVelocityIterations) + { + return new SolveDescription(substepVelocityIterations); + } + } +} diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index 5bf68bca7..805414dec 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -12,9 +12,12 @@ namespace BepuPhysics { + + /// + /// Holds and solves constraints between bodies in a simulation. + /// public abstract partial class Solver { - /// /// Buffer containing all constraint sets. The first slot is dedicated to the active set; subsequent slots may be occupied by the constraints associated with inactive islands. /// @@ -29,6 +32,9 @@ public abstract partial class Solver //inactive islands do not store the referenced handles since no new constraints are ever added. internal QuickList batchReferencedHandles; + /// + /// Set of processors applied to batches of constraints of particular types, indexed by the constraint type id. + /// public TypeProcessor[] TypeProcessors; internal Bodies bodies; @@ -61,7 +67,6 @@ public abstract partial class Solver /// public QuickSet> ConstrainedKinematicHandles; - protected int substepCount; /// /// Gets or sets the number of substeps the solver will simulate per call to Solve. @@ -71,11 +76,12 @@ public int SubstepCount get { return substepCount; } set { - if (substepCount < 1) + if (value < 1) throw new ArgumentException("Substep count must be positive."); substepCount = value; } } + int velocityIterationCount; /// /// Gets or sets the number of solver velocity iterations to compute per substep. @@ -93,6 +99,11 @@ public int VelocityIterationCount } } + /// + /// Callback executed to determine how many velocity iterations should be used for a given substep. If null, or if it returns a non-positive value, the will be used instead. + /// + public SubstepVelocityIterationScheduler VelocityIterationScheduler { get; set; } + int minimumCapacityPerTypeBatch; /// /// Gets or sets the minimum amount of space, in constraints, initially allocated in any new type batch. @@ -194,20 +205,22 @@ public int CountConstraints() Action solveWorker; Action incrementalContactUpdateWorker; - public Solver(Bodies bodies, BufferPool pool, int iterationCount, int fallbackBatchThreshold, + protected Solver(Bodies bodies, BufferPool pool, SolveDescription solveDescription, int initialCapacity, int initialIslandCapacity, int minimumCapacityPerTypeBatch) { - this.velocityIterationCount = iterationCount; + SubstepCount = solveDescription.SubstepCount; + VelocityIterationCount = solveDescription.VelocityIterationCount; + VelocityIterationScheduler = solveDescription.VelocityIterationScheduler; + FallbackBatchThreshold = solveDescription.FallbackBatchThreshold; this.minimumCapacityPerTypeBatch = minimumCapacityPerTypeBatch; this.bodies = bodies; this.pool = pool; HandlePool = new IdPool(128, pool); ResizeSetsCapacity(initialIslandCapacity + 1, 0); - FallbackBatchThreshold = fallbackBatchThreshold; - ActiveSet = new ConstraintSet(pool, fallbackBatchThreshold + 1); - batchReferencedHandles = new QuickList(fallbackBatchThreshold + 1, pool); + ActiveSet = new ConstraintSet(pool, FallbackBatchThreshold + 1); + batchReferencedHandles = new QuickList(FallbackBatchThreshold + 1, pool); ResizeHandleCapacity(initialCapacity); ConstrainedKinematicHandles = new QuickSet>(bodies.HandleToLocation.Length, pool); } diff --git a/BepuPhysics/Solver_Solve.cs b/BepuPhysics/Solver_Solve.cs index 38b900745..a87bd52e4 100644 --- a/BepuPhysics/Solver_Solve.cs +++ b/BepuPhysics/Solver_Solve.cs @@ -131,14 +131,14 @@ Unconstrained bodies will be integrated separately outside of the solver. (This will likely need to be updated to be cleverer as heterogeneous architectures gain popularity.) */ - public Solver(Bodies bodies, BufferPool pool, int iterationCount, int fallbackBatchThreshold, + public Solver(Bodies bodies, BufferPool pool, SolveDescription solveDescription, int initialCapacity, int initialIslandCapacity, int minimumCapacityPerTypeBatch, PoseIntegrator poseIntegrator) - : base(bodies, pool, iterationCount, fallbackBatchThreshold, initialCapacity, initialIslandCapacity, minimumCapacityPerTypeBatch) + : base(bodies, pool, solveDescription, initialCapacity, initialIslandCapacity, minimumCapacityPerTypeBatch) { PoseIntegrator = poseIntegrator; - solveStep2Worker2 = SolveStep2Worker2; + solveWorker = SolveWorker; constraintIntegrationResponsibilitiesWorker = ConstraintIntegrationResponsibilitiesWorker; } @@ -440,8 +440,8 @@ protected static int GetUniformlyDistributedStart(int workerIndex, int blockCoun return offset + blocksPerWorker * workerIndex + Math.Min(remainder, workerIndex); } - Action solveStep2Worker2; - void SolveStep2Worker2(int workerIndex) + Action solveWorker; + void SolveWorker(int workerIndex) { //The solver has two codepaths: one thread, acting as an orchestrator, and the others, just waiting to be used. //There is no requirement that a worker thread above index 0 actually runs at all for a given dispatch. @@ -1413,7 +1413,7 @@ public override void Solve(float totalDt, IThreadDispatcher threadDispatcher = n } else { - ExecuteMultithreaded2(substepDt, threadDispatcher, solveStep2Worker2); + ExecuteMultithreaded2(substepDt, threadDispatcher, solveWorker); } } } diff --git a/DemoTests/ConstraintDescriptionMappingTests.cs b/DemoTests/ConstraintDescriptionMappingTests.cs index 0881ffd18..e901aad58 100644 --- a/DemoTests/ConstraintDescriptionMappingTests.cs +++ b/DemoTests/ConstraintDescriptionMappingTests.cs @@ -25,7 +25,7 @@ static void FillWithRandomBytes(ref T item, Random random) where T : struct } static void Test(BufferPool pool, Random random, int constraintTypeBodyCount) where T : unmanaged, IConstraintDescription { - var simulation = Simulation.Create(pool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(), new SubsteppingTimestepper(4), 1); + var simulation = Simulation.Create(pool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(), 4); const int bodyCount = 2048; diff --git a/Demos/Demos/BlockChainDemo.cs b/Demos/Demos/BlockChainDemo.cs index 7d504820b..d2c60beee 100644 --- a/Demos/Demos/BlockChainDemo.cs +++ b/Demos/Demos/BlockChainDemo.cs @@ -25,10 +25,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = 0; - //The PositionFirstTimestepper is the simplest timestepping mode, but since it integrates velocity into position at the start of the frame, directly modified velocities outside of the timestep - //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. - //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(3), 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); var boxShape = new Box(1, 1, 1); var boxInertia = boxShape.ComputeInertia(1); diff --git a/Demos/Demos/BouncinessDemo.cs b/Demos/Demos/BouncinessDemo.cs index d63789012..feab53951 100644 --- a/Demos/Demos/BouncinessDemo.cs +++ b/Demos/Demos/BouncinessDemo.cs @@ -82,11 +82,11 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0; //This type of position-based bounciness requires feedback from position error to drive the corrective impulses that make stuff bounce. //The briefer a collision is, the more damped the bounce becomes relative to the physical ideal. - //To counteract this, a substepping timestepper is used. In the demos, we update the simulation at 60hz, so a substep count of 4 means the solver and integrator will run at 240hz. + //To counteract this, a substepping timestepper is used. In the demos, we update the simulation at 60hz, so a substep count of 8 means the solver and integrator will run at 480hz. //That allows higher stiffnesses to be used since collisions last longer relative to the solver timestep duration. - //(Note that substepping tends to be an extremely strong simulation stabilizer, so you can usually get away with lower solver iteration counts for better performance.) + //(Note that substepping tends to be an extremely strong simulation stabilizer, so you can usually get away with lower solver iteration counts for better performance. It defaults to 1 velocity iteration per substep.) var collidableMaterials = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new BounceCallbacks() { CollidableMaterials = collidableMaterials }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), solverIterationCount: 2); + Simulation = Simulation.Create(BufferPool, new BounceCallbacks() { CollidableMaterials = collidableMaterials }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 8); var shape = new Sphere(1); var ballDescription = BodyDescription.CreateDynamic(RigidPose.Identity, shape.ComputeInertia(1), new(Simulation.Shapes.Add(shape), ContinuousDetection.Discrete(20, 20)), 1e-2f); diff --git a/Demos/Demos/Cars/CarDemo.cs b/Demos/Demos/Cars/CarDemo.cs index 46646e749..975b78df4 100644 --- a/Demos/Demos/Cars/CarDemo.cs +++ b/Demos/Demos/Cars/CarDemo.cs @@ -41,11 +41,8 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0; var properties = new CollidableProperty(); - //The PositionFirstTimestepper is the simplest timestepping mode, but since it integrates velocity into position at the start of the frame, directly modified velocities outside of the timestep - //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. - //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. - //Simulation = Simulation.Create(BufferPool, new CarCallbacks() { Properties = properties }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); - Simulation = Simulation.Create(BufferPool, new CarCallbacks() { Properties = properties }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(3), 1); + + Simulation = Simulation.Create(BufferPool, new CarCallbacks() { Properties = properties }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); var builder = new CompoundBuilder(BufferPool, Simulation.Shapes, 2); diff --git a/Demos/Demos/Characters/CharacterDemo.cs b/Demos/Demos/Characters/CharacterDemo.cs index 40ea4f511..02422d1a6 100644 --- a/Demos/Demos/Characters/CharacterDemo.cs +++ b/Demos/Demos/Characters/CharacterDemo.cs @@ -25,10 +25,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0; characters = new CharacterControllers(BufferPool); - //The PositionFirstTimestepper is the simplest timestepping mode, but since it integrates velocity into position at the start of the frame, directly modified velocities outside of the timestep - //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. - //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. - Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(3), 1); + Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); CreateCharacter(new Vector3(0, 2, -4)); diff --git a/Demos/Demos/ClothDemo.cs b/Demos/Demos/ClothDemo.cs index 3174db417..8f37720eb 100644 --- a/Demos/Demos/ClothDemo.cs +++ b/Demos/Demos/ClothDemo.cs @@ -188,7 +188,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0; var filters = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new ClothCallbacks() { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); + Simulation = Simulation.Create(BufferPool, new ClothCallbacks() { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); rolloverInfo = new RolloverInfo(); bool KinematicTopCorners(int rowIndex, int columnIndex, int width, int height) diff --git a/Demos/Demos/CollisionQueryDemo.cs b/Demos/Demos/CollisionQueryDemo.cs index 5e6d3bcad..a3da4ce19 100644 --- a/Demos/Demos/CollisionQueryDemo.cs +++ b/Demos/Demos/CollisionQueryDemo.cs @@ -27,7 +27,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Yaw = 0; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(new Box(100, 1, 100)))); diff --git a/Demos/Demos/ColosseumDemo.cs b/Demos/Demos/ColosseumDemo.cs index ff4f47ce3..3c816907b 100644 --- a/Demos/Demos/ColosseumDemo.cs +++ b/Demos/Demos/ColosseumDemo.cs @@ -64,7 +64,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = MathHelper.Pi * 0.2f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), solverIterationCount: 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); var ringBoxShape = new Box(0.5f, 1, 3); var boxDescription = BodyDescription.CreateDynamic(new Vector3(), ringBoxShape.ComputeInertia(1), Simulation.Shapes.Add(ringBoxShape), 0.01f); diff --git a/Demos/Demos/CompoundTestDemo.cs b/Demos/Demos/CompoundTestDemo.cs index 5df6c7f01..295374cc7 100644 --- a/Demos/Demos/CompoundTestDemo.cs +++ b/Demos/Demos/CompoundTestDemo.cs @@ -16,10 +16,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = MathHelper.Pi * 0.05f; - //The PositionFirstTimestepper is the simplest timestepping mode, but since it integrates velocity into position at the start of the frame, directly modified velocities outside of the timestep - //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. - //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10f, 0)), new SubsteppingTimestepper(8), 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10f, 0)), 8); using (var compoundBuilder = new CompoundBuilder(BufferPool, Simulation.Shapes, 8)) { diff --git a/Demos/Demos/ContactEventsDemo.cs b/Demos/Demos/ContactEventsDemo.cs index 025d35dc1..f5f96164a 100644 --- a/Demos/Demos/ContactEventsDemo.cs +++ b/Demos/Demos/ContactEventsDemo.cs @@ -702,7 +702,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi; events = new ContactEvents(ThreadDispatcher, BufferPool); - Simulation = Simulation.Create(BufferPool, new ContactEventCallbacks(events), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); + Simulation = Simulation.Create(BufferPool, new ContactEventCallbacks(events), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); eventHandler = new EventHandler(Simulation, BufferPool); var listenedBody1 = Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 5, 0), 1, Simulation.Shapes, new Box(1, 2, 3))); diff --git a/Demos/Demos/ContinuousCollisionDetectionDemo.cs b/Demos/Demos/ContinuousCollisionDetectionDemo.cs index db1c7124d..cde37b054 100644 --- a/Demos/Demos/ContinuousCollisionDetectionDemo.cs +++ b/Demos/Demos/ContinuousCollisionDetectionDemo.cs @@ -54,7 +54,7 @@ public override void Initialize(ContentArchive content, Camera camera) //Having objects bounce a bunch on impact makes it harder to see. Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(240, 1), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 1f }, - new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 2); + new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 8); var shape = new Box(1, 1, 1); var inertia = shape.ComputeInertia(1); diff --git a/Demos/Demos/CustomVoxelCollidableDemo.cs b/Demos/Demos/CustomVoxelCollidableDemo.cs index 42bc649a7..addf8b75b 100644 --- a/Demos/Demos/CustomVoxelCollidableDemo.cs +++ b/Demos/Demos/CustomVoxelCollidableDemo.cs @@ -374,7 +374,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = MathHelper.Pi * 0.05f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); //The narrow phase must be notified about the existence of the new collidable type. For every pair type we want to support, a collision task must be registered. //All of the default engine types are registered upon simulation creation by a call to DefaultTypes.CreateDefaultCollisionTaskRegistry. diff --git a/Demos/Demos/FountainStressTestDemo.cs b/Demos/Demos/FountainStressTestDemo.cs index 2ca53cb67..c42b95124 100644 --- a/Demos/Demos/FountainStressTestDemo.cs +++ b/Demos/Demos/FountainStressTestDemo.cs @@ -24,7 +24,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = MathHelper.Pi * 0.1f; //Using minimum sized allocations forces as many resizes as possible. //Note the low solverFallbackBatchThreshold- we want the fallback batches to get tested thoroughly. - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(3), 1, solverFallbackBatchThreshold: 2, initialAllocationSizes: + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(4, fallbackBatchThreshold: 2), initialAllocationSizes: new SimulationAllocationSizes { Bodies = 1, diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index 5f292d3cc..1a3e31ddc 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -706,13 +706,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = MathHelper.Pi * 0.15f; var filters = new CollidableProperty(); - //The PositionFirstTimestepper is the simplest timestepping mode, but since it integrates velocity into position at the start of the frame, directly modified velocities outside of the timestep - //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. - //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. - //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper(1)); - Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0), 0, 0), new SubsteppingTimestepper(3), solverIterationCount: 1); - //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper2(), solverIterationCount: 60); - //Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)), new SubsteppingTimestepper2(3), solverIterationCount: 1); + Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0), 0, 0), 4); var meshContent = content.Load("Content\\newt.obj"); float cellSize = 0.1f; diff --git a/Demos/Demos/PlanetDemo.cs b/Demos/Demos/PlanetDemo.cs index a09a2e3a8..aa2351ad8 100644 --- a/Demos/Demos/PlanetDemo.cs +++ b/Demos/Demos/PlanetDemo.cs @@ -59,7 +59,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new PlanetaryGravityCallbacks() { PlanetCenter = new Vector3(), Gravity = 100000 }, new SubsteppingTimestepper(4), 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new PlanetaryGravityCallbacks() { PlanetCenter = new Vector3(), Gravity = 100000 }, 4); Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(new Sphere(50)))); diff --git a/Demos/Demos/PyramidDemo.cs b/Demos/Demos/PyramidDemo.cs index fd5c082a9..c09a4624b 100644 --- a/Demos/Demos/PyramidDemo.cs +++ b/Demos/Demos/PyramidDemo.cs @@ -22,11 +22,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-30, 8, -110); camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = 0; - //The PositionFirstTimestepper is the simplest timestepping mode, but since it integrates velocity into position at the start of the frame, directly modified velocities outside of the timestep - //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. - //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(3), solverIterationCount: 1); - //Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper()); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); var boxShape = new Box(1, 1, 1); var boxInertia = boxShape.ComputeInertia(1); diff --git a/Demos/Demos/RagdollDemo.cs b/Demos/Demos/RagdollDemo.cs index 0952f77dd..41c9dd330 100644 --- a/Demos/Demos/RagdollDemo.cs +++ b/Demos/Demos/RagdollDemo.cs @@ -533,10 +533,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = MathHelper.Pi * 0.05f; var collisionFilters = new CollidableProperty(); - //The PositionFirstTimestepper is the simplest timestepping mode, but since it integrates velocity into position at the start of the frame, directly modified velocities outside of the timestep - //will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper. - //Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection. - Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks { CollisionFilters = collisionFilters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(3), 1); + Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks { CollisionFilters = collisionFilters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); int ragdollIndex = 0; var spacing = new Vector3(2f, 3, 1); diff --git a/Demos/Demos/RayCastingDemo.cs b/Demos/Demos/RayCastingDemo.cs index 37204376e..4c186cdb5 100644 --- a/Demos/Demos/RayCastingDemo.cs +++ b/Demos/Demos/RayCastingDemo.cs @@ -63,7 +63,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-20f, 13, -20f); camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = MathHelper.Pi * 0.1f; - Simulation = Simulation.Create(BufferPool, new NoCollisionCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); + Simulation = Simulation.Create(BufferPool, new NoCollisionCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); diff --git a/Demos/Demos/RopeStabilityDemo.cs b/Demos/Demos/RopeStabilityDemo.cs index 336a9cb41..0930e2b71 100644 --- a/Demos/Demos/RopeStabilityDemo.cs +++ b/Demos/Demos/RopeStabilityDemo.cs @@ -97,7 +97,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //So, even though you can avoid the need for these kinds of hacks, it's good to know that they exist should you find yourself in a circumstance where substepping isn't viable. Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(120, 1), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 2f }, - new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); + new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); rolloverInfo = new RolloverInfo(); var smallWreckingBall = new Sphere(1); diff --git a/Demos/Demos/RopeTwistDemo.cs b/Demos/Demos/RopeTwistDemo.cs index 8e66b9995..6a3b1a250 100644 --- a/Demos/Demos/RopeTwistDemo.cs +++ b/Demos/Demos/RopeTwistDemo.cs @@ -83,11 +83,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var filters = new CollidableProperty(); Simulation = Simulation.Create(BufferPool, new RopeNarrowPhaseCallbacks { ContactSpringiness = new SpringSettings(2000, 1), Filters = filters }, - new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(60), solverIterationCount: 1, solverFallbackBatchThreshold: 64); - //Simulation = Simulation.Create(BufferPool, - // new RopeNarrowPhaseCallbacks { ContactSpringiness = new SpringSettings(2000, 1), Filters = filters }, - // new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper2(60), solverIterationCount: 1, solverFallbackBatchThreshold: 512); - + new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 60); for (int twistIndex = 0; twistIndex < 1; ++twistIndex) { diff --git a/Demos/Demos/SimpleSelfContainedDemo.cs b/Demos/Demos/SimpleSelfContainedDemo.cs index cb803cc1b..53484d892 100644 --- a/Demos/Demos/SimpleSelfContainedDemo.cs +++ b/Demos/Demos/SimpleSelfContainedDemo.cs @@ -213,7 +213,7 @@ public static void Run() //PositionLastTimestepper avoids that by running collision detection and the solver first at the cost of a tiny amount of overhead. //(You could avoid the issue with PositionFirstTimestepper by modifying velocities in the PositionFirstTimestepper's BeforeCollisionDetection callback //instead of outside the timestep, too, but it's a little more complicated.) - var simulation = Simulation.Create(bufferPool, new NarrowPhaseCallbacks(), new PoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); + var simulation = Simulation.Create(bufferPool, new NarrowPhaseCallbacks(), new PoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); //Drop a ball on a big static box. var sphere = new Sphere(1); diff --git a/Demos/Demos/SolverContactEnumerationDemo.cs b/Demos/Demos/SolverContactEnumerationDemo.cs index 7d23f37bc..340963124 100644 --- a/Demos/Demos/SolverContactEnumerationDemo.cs +++ b/Demos/Demos/SolverContactEnumerationDemo.cs @@ -26,7 +26,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Yaw = 0; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); //Drop a pyramid on top of the sensor so there are more contacts to look at. var boxShape = new Box(1, 1, 1); diff --git a/Demos/Demos/Sponsors/SponsorDemo.cs b/Demos/Demos/Sponsors/SponsorDemo.cs index f54458cc8..5d043cf4d 100644 --- a/Demos/Demos/Sponsors/SponsorDemo.cs +++ b/Demos/Demos/Sponsors/SponsorDemo.cs @@ -109,7 +109,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0.4f; characterControllers = new CharacterControllers(BufferPool); - Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characterControllers), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); + Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characterControllers), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); DemoMeshHelper.LoadModel(content, BufferPool, @"Content\newt.obj", new Vector3(-10, 10, -10), out var newtMesh); var newtShape = Simulation.Shapes.Add(newtMesh); diff --git a/Demos/Demos/SubsteppingDemo.cs b/Demos/Demos/SubsteppingDemo.cs index e8e2a8e8c..ab5ebb09e 100644 --- a/Demos/Demos/SubsteppingDemo.cs +++ b/Demos/Demos/SubsteppingDemo.cs @@ -24,7 +24,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0; Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(120, 120), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 2f }, - new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(), 8); + new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 8); rolloverInfo = new RolloverInfo(); { diff --git a/Demos/Demos/SweepDemo.cs b/Demos/Demos/SweepDemo.cs index 674c8f9de..faf0f1e56 100644 --- a/Demos/Demos/SweepDemo.cs +++ b/Demos/Demos/SweepDemo.cs @@ -31,7 +31,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(0, 10, 40); camera.Yaw = 0; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); var box = new Box(2f, 2f, 2f); var capsule = new Capsule(1f, 1f); diff --git a/Demos/Demos/Tanks/TankDemo.cs b/Demos/Demos/Tanks/TankDemo.cs index 063941410..663c5998f 100644 --- a/Demos/Demos/Tanks/TankDemo.cs +++ b/Demos/Demos/Tanks/TankDemo.cs @@ -49,9 +49,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0; bodyProperties = new CollidableProperty(); - //We assign velocities outside of the timestep to fire bullets, so using the PositionLastTimestepper avoids integrating those velocities into positions before the solver has a chance to intervene. - //We could have also modified velocities in the PositionFirstTimestepper's BeforeCollisionDetection callback, but it's just a little simpler to do this with very little cost. - Simulation = Simulation.Create(BufferPool, new TankCallbacks() { Properties = bodyProperties }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(3), 1); + Simulation = Simulation.Create(BufferPool, new TankCallbacks() { Properties = bodyProperties }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); var builder = new CompoundBuilder(BufferPool, Simulation.Shapes, 2); builder.Add(new Box(1.85f, 0.7f, 4.73f), RigidPose.Identity, 10); diff --git a/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs b/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs index e1c38eeb3..e65efaa1f 100644 --- a/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs +++ b/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs @@ -20,7 +20,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-20f, 13, -20f); camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = MathHelper.Pi * 0.1f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); var shape = new Sphere(0.5f); var sphereInertia = shape.ComputeInertia(1); diff --git a/Demos/SpecializedTests/CapsuleTestDemo.cs b/Demos/SpecializedTests/CapsuleTestDemo.cs index 36efd6751..c746bb6f8 100644 --- a/Demos/SpecializedTests/CapsuleTestDemo.cs +++ b/Demos/SpecializedTests/CapsuleTestDemo.cs @@ -20,7 +20,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //camera.Yaw = MathHelper.Pi ; camera.Yaw = MathHelper.Pi * 3f / 4; //camera.Pitch = MathHelper.Pi * 0.1f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); var shape = new Capsule(.5f, .5f); var localInertia = shape.ComputeInertia(1); diff --git a/Demos/SpecializedTests/CharacterTestDemo.cs b/Demos/SpecializedTests/CharacterTestDemo.cs index 2707f558f..f2d3fa0a2 100644 --- a/Demos/SpecializedTests/CharacterTestDemo.cs +++ b/Demos/SpecializedTests/CharacterTestDemo.cs @@ -21,7 +21,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = MathHelper.Pi * 0.05f; var masks = new CollidableProperty(); characters = new CharacterControllers(BufferPool); - Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); + Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); var random = new Random(5); for (int i = 0; i < 8192; ++i) diff --git a/Demos/SpecializedTests/ClothLatticeDemo.cs b/Demos/SpecializedTests/ClothLatticeDemo.cs index 821b322c1..881bf5fca 100644 --- a/Demos/SpecializedTests/ClothLatticeDemo.cs +++ b/Demos/SpecializedTests/ClothLatticeDemo.cs @@ -20,7 +20,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-120, 30, -120); camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = 0.1f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); //Build a grid of shapes to be connected. var clothNodeShape = new Sphere(0.5f); diff --git a/Demos/SpecializedTests/CompoundBoundTests.cs b/Demos/SpecializedTests/CompoundBoundTests.cs index 570a09f35..435bcfb5f 100644 --- a/Demos/SpecializedTests/CompoundBoundTests.cs +++ b/Demos/SpecializedTests/CompoundBoundTests.cs @@ -18,7 +18,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(-10, 0, -10); camera.Yaw = MathHelper.Pi * 3f / 4; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); } diff --git a/Demos/SpecializedTests/CompoundCollisionIndicesTest.cs b/Demos/SpecializedTests/CompoundCollisionIndicesTest.cs index 8d579df30..76fde339f 100644 --- a/Demos/SpecializedTests/CompoundCollisionIndicesTest.cs +++ b/Demos/SpecializedTests/CompoundCollisionIndicesTest.cs @@ -72,7 +72,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(0, 4, -6); camera.Yaw = MathHelper.Pi; - Simulation = Simulation.Create(BufferPool, new IndexReportingNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, 0f, 0)), new SubsteppingTimestepper(4), 1); + Simulation = Simulation.Create(BufferPool, new IndexReportingNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, 0f, 0)), 4); var builder = new CompoundBuilder(BufferPool, Simulation.Shapes, 4); builder.Add(new Sphere(0.5f), new Vector3(-1, 0, 0), 1); diff --git a/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs b/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs index 5d6fd32cb..f651d045b 100644 --- a/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs +++ b/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs @@ -19,7 +19,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = 0; Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(30, 1), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 2f }, - new DemoPoseIntegratorCallbacks(new Vector3(0, -0.1f, 0), 0, 0), new SubsteppingTimestepper(3), 1); + new DemoPoseIntegratorCallbacks(new Vector3(0, -0.1f, 0), 0, 0), 4); var shapeA = new Box(.75f, 1, .5f); var shapeIndexA = Simulation.Shapes.Add(shapeA); diff --git a/Demos/SpecializedTests/ConstraintTestDemo.cs b/Demos/SpecializedTests/ConstraintTestDemo.cs index 9c5a7b946..6b7c509b0 100644 --- a/Demos/SpecializedTests/ConstraintTestDemo.cs +++ b/Demos/SpecializedTests/ConstraintTestDemo.cs @@ -24,7 +24,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(25, 4, 40); camera.Yaw = 0; Simulation = Simulation.Create(BufferPool, - new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(30, 1), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 2f }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(3), 1); + new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(30, 1), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 2f }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); //new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(30, 1), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 2f }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper(), 8); var shapeA = new Box(.75f, 1, .5f); diff --git a/Demos/SpecializedTests/ConvexHullTestDemo.cs b/Demos/SpecializedTests/ConvexHullTestDemo.cs index 2d9911133..87a1d9bdf 100644 --- a/Demos/SpecializedTests/ConvexHullTestDemo.cs +++ b/Demos/SpecializedTests/ConvexHullTestDemo.cs @@ -27,7 +27,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Yaw = 0; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), solverIterationCount: 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); //var meshContent = content.Load("Content\\newt.obj"); diff --git a/Demos/SpecializedTests/CylinderTestDemo.cs b/Demos/SpecializedTests/CylinderTestDemo.cs index f4fc48e4c..012c46a09 100644 --- a/Demos/SpecializedTests/CylinderTestDemo.cs +++ b/Demos/SpecializedTests/CylinderTestDemo.cs @@ -170,7 +170,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0; camera.Yaw = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, 0f, 0)), new SubsteppingTimestepper(4), 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, 0f, 0)), 4); var cylinderShape = new Cylinder(1f, .2f); var cylinder = BodyDescription.CreateDynamic(new Vector3(10f, 3, 0), cylinderShape.ComputeInertia(1), new(Simulation.Shapes.Add(cylinderShape), ContinuousDetection.CreatePassive(1000f, 1000f)), 0.01f); diff --git a/Demos/SpecializedTests/GyroscopeTestDemo.cs b/Demos/SpecializedTests/GyroscopeTestDemo.cs index 05357a04d..792d10948 100644 --- a/Demos/SpecializedTests/GyroscopeTestDemo.cs +++ b/Demos/SpecializedTests/GyroscopeTestDemo.cs @@ -57,7 +57,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0; //Note the lack of damping- we want the gyroscope to keep spinning. - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new GyroscopicIntegratorCallbacks(new Vector3(0, -10, 0), 0f, 0f), new SubsteppingTimestepper(4), 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new GyroscopicIntegratorCallbacks(new Vector3(0, -10, 0), 0f, 0f), 4); Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(new Box(100, 1, 100)))); diff --git a/Demos/SpecializedTests/Media/BedsheetDemo.cs b/Demos/SpecializedTests/Media/BedsheetDemo.cs index 2e1fd566c..25026a4ca 100644 --- a/Demos/SpecializedTests/Media/BedsheetDemo.cs +++ b/Demos/SpecializedTests/Media/BedsheetDemo.cs @@ -107,7 +107,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = MathF.PI * 0.1f; var filters = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new ClothCallbacks() { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -50, 0)), new SubsteppingTimestepper(4), 1); + Simulation = Simulation.Create(BufferPool, new ClothCallbacks() { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -50, 0)), 4); rolloverInfo = new RolloverInfo(); bool FullyDynamic(int rowIndex, int columnIndex, int width, int height) diff --git a/Demos/SpecializedTests/Media/ColosseumVideoDemo.cs b/Demos/SpecializedTests/Media/ColosseumVideoDemo.cs index fdf8df16a..fc0f8431e 100644 --- a/Demos/SpecializedTests/Media/ColosseumVideoDemo.cs +++ b/Demos/SpecializedTests/Media/ColosseumVideoDemo.cs @@ -68,7 +68,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = MathHelper.Pi * 0.2f; characters = new CharacterControllers(BufferPool); - Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); + Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); var ringBoxShape = new Box(0.5f, 1.5f, 3); var boxDescription = BodyDescription.CreateDynamic(new Vector3(), ringBoxShape.ComputeInertia(1), Simulation.Shapes.Add(ringBoxShape), 0.01f); diff --git a/Demos/SpecializedTests/Media/NewtDemandingSacrificeVideoDemo.cs b/Demos/SpecializedTests/Media/NewtDemandingSacrificeVideoDemo.cs index 7f4dd55ab..172a96aa8 100644 --- a/Demos/SpecializedTests/Media/NewtDemandingSacrificeVideoDemo.cs +++ b/Demos/SpecializedTests/Media/NewtDemandingSacrificeVideoDemo.cs @@ -20,7 +20,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = MathHelper.Pi * -0.05f; filters = new CollidableProperty(BufferPool); - Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks() { CollisionFilters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); + Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks() { CollisionFilters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), Simulation.Shapes.Add(new Box(1500, 1, 1500)))); Simulation.Statics.Add(new StaticDescription(new Vector3(0, 10, 0), Simulation.Shapes.Add(new Box(70, 20, 80)))); diff --git a/Demos/SpecializedTests/Media/NewtVideoDemo.cs b/Demos/SpecializedTests/Media/NewtVideoDemo.cs index 0c94fd410..79f410719 100644 --- a/Demos/SpecializedTests/Media/NewtVideoDemo.cs +++ b/Demos/SpecializedTests/Media/NewtVideoDemo.cs @@ -21,7 +21,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = MathHelper.Pi * 0.15f; var filters = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); + Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); var meshContent = content.Load("Content\\newt.obj"); float cellSize = 0.1f; diff --git a/Demos/SpecializedTests/Media/PyramidVideoDemo.cs b/Demos/SpecializedTests/Media/PyramidVideoDemo.cs index 24bf9761e..946dda4b6 100644 --- a/Demos/SpecializedTests/Media/PyramidVideoDemo.cs +++ b/Demos/SpecializedTests/Media/PyramidVideoDemo.cs @@ -22,7 +22,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-70, 8, 318); camera.Yaw = MathHelper.Pi * 1f / 4; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); var boxShape = new Box(1, 1, 1); var boxInertia = boxShape.ComputeInertia(1); diff --git a/Demos/SpecializedTests/Media/ShrinkwrappedNewtsVideoDemo.cs b/Demos/SpecializedTests/Media/ShrinkwrappedNewtsVideoDemo.cs index ef6a56b82..7a636ebb7 100644 --- a/Demos/SpecializedTests/Media/ShrinkwrappedNewtsVideoDemo.cs +++ b/Demos/SpecializedTests/Media/ShrinkwrappedNewtsVideoDemo.cs @@ -18,7 +18,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = 3 * MathHelper.Pi / 4; camera.Pitch = 0;// MathHelper.Pi * 0.15f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); var meshContent = content.Load("Content\\newt.obj"); diff --git a/Demos/SpecializedTests/MeshMeshTestDemo.cs b/Demos/SpecializedTests/MeshMeshTestDemo.cs index ab0d49ab8..2bb51e784 100644 --- a/Demos/SpecializedTests/MeshMeshTestDemo.cs +++ b/Demos/SpecializedTests/MeshMeshTestDemo.cs @@ -21,7 +21,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(0, 8, -10); camera.Yaw = MathHelper.Pi; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); DemoMeshHelper.LoadModel(content, BufferPool, @"Content\newt.obj", Vector3.One, out var mesh); var approximateInertia = new Box(2.5f, 1, 4).ComputeInertia(1); diff --git a/Demos/SpecializedTests/MeshReductionTestDemo.cs b/Demos/SpecializedTests/MeshReductionTestDemo.cs index 13610a47a..6a45d1110 100644 --- a/Demos/SpecializedTests/MeshReductionTestDemo.cs +++ b/Demos/SpecializedTests/MeshReductionTestDemo.cs @@ -25,7 +25,7 @@ public override void Initialize(ContentArchive content, Camera camera) Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks() { ContactSpringiness = new(30, 1), MaximumRecoveryVelocity = 2, FrictionCoefficient = 0 }, - new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); + new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); var builder = new CompoundBuilder(BufferPool, Simulation.Shapes, 2); builder.Add(new Box(1.85f, 0.7f, 4.73f), RigidPose.Identity, 10); diff --git a/Demos/SpecializedTests/MeshSerializationTestDemo.cs b/Demos/SpecializedTests/MeshSerializationTestDemo.cs index 48f938cbc..dd979cbee 100644 --- a/Demos/SpecializedTests/MeshSerializationTestDemo.cs +++ b/Demos/SpecializedTests/MeshSerializationTestDemo.cs @@ -17,7 +17,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); var startTime = Stopwatch.GetTimestamp(); DemoMeshHelper.CreateDeformedPlane(1025, 1025, (x, y) => new Vector3(x * 0.125f, MathF.Sin(x) + MathF.Sin(y), y * 0.125f), Vector3.One, BufferPool, out var originalMesh); diff --git a/Demos/SpecializedTests/MeshTestDemo.cs b/Demos/SpecializedTests/MeshTestDemo.cs index 099f94523..4785c4761 100644 --- a/Demos/SpecializedTests/MeshTestDemo.cs +++ b/Demos/SpecializedTests/MeshTestDemo.cs @@ -19,7 +19,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //camera.Yaw = MathHelper.Pi ; camera.Yaw = MathHelper.Pi * 3f / 4; //camera.Pitch = MathHelper.PiOver2 * 0.999f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); var box = new Box(1f, 3f, 2f); var capsule = new Capsule(1f, 1f); diff --git a/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs b/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs index 04e5d2196..5897e5750 100644 --- a/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs +++ b/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs @@ -22,7 +22,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-30, 8, -110); camera.Yaw = MathHelper.Pi * 3f / 4; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); var boxShape = new Box(1, 1, 1); var boxInertia = boxShape.ComputeInertia(1); diff --git a/Demos/SpecializedTests/RagdollTubeDemo.cs b/Demos/SpecializedTests/RagdollTubeDemo.cs index fc658b1ed..06acb8b55 100644 --- a/Demos/SpecializedTests/RagdollTubeDemo.cs +++ b/Demos/SpecializedTests/RagdollTubeDemo.cs @@ -21,7 +21,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi; camera.Pitch = 0; var filters = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks { CollisionFilters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(3), 1, 64); + Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks { CollisionFilters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); int ragdollIndex = 0; var spacing = new Vector3(1.7f, 1.8f, 0.5f); diff --git a/Demos/SpecializedTests/ShapePileTestDemo.cs b/Demos/SpecializedTests/ShapePileTestDemo.cs index 845bfa84f..0f498397a 100644 --- a/Demos/SpecializedTests/ShapePileTestDemo.cs +++ b/Demos/SpecializedTests/ShapePileTestDemo.cs @@ -20,7 +20,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //camera.Yaw = MathHelper.Pi ; camera.Yaw = MathHelper.Pi * 3f / 4; //camera.Pitch = MathHelper.PiOver2 * 0.999f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); Simulation.Deterministic = true; var sphere = new Sphere(1.5f); diff --git a/Demos/SpecializedTests/SolverBatchTestDemo.cs b/Demos/SpecializedTests/SolverBatchTestDemo.cs index 7b9e00fa4..d45c04259 100644 --- a/Demos/SpecializedTests/SolverBatchTestDemo.cs +++ b/Demos/SpecializedTests/SolverBatchTestDemo.cs @@ -20,7 +20,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-120, 30, -120); camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = 0.1f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); Simulation.Solver.VelocityIterationCount = 8; //Build a grid of shapes to be connected. diff --git a/Demos/SpecializedTests/TriangleRayTestDemo.cs b/Demos/SpecializedTests/TriangleRayTestDemo.cs index 85f3433dc..5617fb67a 100644 --- a/Demos/SpecializedTests/TriangleRayTestDemo.cs +++ b/Demos/SpecializedTests/TriangleRayTestDemo.cs @@ -94,7 +94,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-30, 8, -60); camera.Yaw = MathHelper.Pi * 3f / 4; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); Triangle triangle; triangle.A = new Vector3(0, 0, 0); diff --git a/Demos/SpecializedTests/TriangleTestDemo.cs b/Demos/SpecializedTests/TriangleTestDemo.cs index 28baf0ca2..6702b1dc0 100644 --- a/Demos/SpecializedTests/TriangleTestDemo.cs +++ b/Demos/SpecializedTests/TriangleTestDemo.cs @@ -92,7 +92,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks() { ContactSpringiness = new BepuPhysics.Constraints.SpringSettings(30, 1), FrictionCoefficient = 1, MaximumRecoveryVelocity = 1 }, - new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SubsteppingTimestepper(4), 1); + new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); //var triangleDescription = new StaticDescription //{ diff --git a/Demos/SpecializedTests/VolumeQueryTests.cs b/Demos/SpecializedTests/VolumeQueryTests.cs index e9ab2259d..e4a7c76f7 100644 --- a/Demos/SpecializedTests/VolumeQueryTests.cs +++ b/Demos/SpecializedTests/VolumeQueryTests.cs @@ -61,7 +61,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-20f, 13, -20f); camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = MathHelper.Pi * 0.1f; - Simulation = Simulation.Create(BufferPool, new NoCollisionCallbacks(), new DemoPoseIntegratorCallbacks(), new SubsteppingTimestepper(4), 1); + Simulation = Simulation.Create(BufferPool, new NoCollisionCallbacks(), new DemoPoseIntegratorCallbacks(), 4); var sphere = new Sphere(0.5f); var shapeIndex = Simulation.Shapes.Add(sphere); From 00fb3f3c177c3b3826fced028fb307daa139f23b Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 6 Dec 2021 17:23:46 -0600 Subject: [PATCH 315/947] Oops, multithreading. --- BepuPhysics/SubsteppingTimestepper.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/BepuPhysics/SubsteppingTimestepper.cs b/BepuPhysics/SubsteppingTimestepper.cs index 287bed0b3..2c4bd01ae 100644 --- a/BepuPhysics/SubsteppingTimestepper.cs +++ b/BepuPhysics/SubsteppingTimestepper.cs @@ -40,7 +40,7 @@ public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDi simulation.CollisionDetection(dt, threadDispatcher); CollisionsDetected?.Invoke(dt, threadDispatcher); - simulation.Solve(dt); + simulation.Solve(dt, threadDispatcher); ConstraintsSolved?.Invoke(dt, threadDispatcher); simulation.IncrementallyOptimizeDataStructures(threadDispatcher); From 1950d6cac6f9f5b8c5f656a090cdd3ea0094c9bf Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 7 Dec 2021 17:10:37 -0600 Subject: [PATCH 316/947] Solver now allows per substep velocity iteration counts. --- BepuPhysics/Collidables/BoundingBoxBatcher.cs | 16 ---- BepuPhysics/SolveDescription.cs | 23 ++++- BepuPhysics/Solver_Solve.cs | 95 +++++++++++++++---- Demos/Demos/FountainStressTestDemo.cs | 2 +- Demos/Program.cs | 2 +- 5 files changed, 96 insertions(+), 42 deletions(-) diff --git a/BepuPhysics/Collidables/BoundingBoxBatcher.cs b/BepuPhysics/Collidables/BoundingBoxBatcher.cs index ad6b09624..4ffac68a2 100644 --- a/BepuPhysics/Collidables/BoundingBoxBatcher.cs +++ b/BepuPhysics/Collidables/BoundingBoxBatcher.cs @@ -205,22 +205,6 @@ public unsafe void ExecuteConvexBatch(ConvexShapeBatchNumber of velocity iterations to use in each substep. Number of substeps will be determined by the length of the span. /// Number of velocity iterations per substep for any substep that is not given a positive number of velocity iterations by the scheduler. /// Number of synchronzed constraint batches to use before using a fallback approach. - public SolveDescription(Span substepVelocityIterations, int fallbackVelocityIterationCount = 1, int fallbackBatchThreshold = DefaultFallbackBatchThreshold) + public SolveDescription(ReadOnlySpan substepVelocityIterations, int fallbackVelocityIterationCount = 1, int fallbackBatchThreshold = DefaultFallbackBatchThreshold) { SubstepCount = substepVelocityIterations.Length; VelocityIterationCount = fallbackVelocityIterationCount; @@ -112,7 +113,23 @@ public static implicit operator SolveDescription((int substepCount, int iteratio /// Creates a solve description with the given number of substeps and velocity iterations per substep and a fallback threshold of . /// /// Number of velocity iterations to use in each substep. Number of substeps will be determined by the length of the span. - public static implicit operator SolveDescription(Span substepVelocityIterations) + public static implicit operator SolveDescription(ReadOnlySpan substepVelocityIterations) + { + return new SolveDescription(substepVelocityIterations); + } + /// + /// Creates a solve description with the given number of substeps and velocity iterations per substep and a fallback threshold of . + /// + /// Number of velocity iterations to use in each substep. Number of substeps will be determined by the length of the span. + public static implicit operator SolveDescription(int[] substepVelocityIterations) + { + return new SolveDescription(substepVelocityIterations); + } + /// + /// Creates a solve description with the given number of substeps and velocity iterations per substep and a fallback threshold of . + /// + /// Number of velocity iterations to use in each substep. Number of substeps will be determined by the length of the span. + public static implicit operator SolveDescription(Buffer substepVelocityIterations) { return new SolveDescription(substepVelocityIterations); } diff --git a/BepuPhysics/Solver_Solve.cs b/BepuPhysics/Solver_Solve.cs index a87bd52e4..faa3b020d 100644 --- a/BepuPhysics/Solver_Solve.cs +++ b/BepuPhysics/Solver_Solve.cs @@ -83,6 +83,10 @@ protected struct SubstepMultithreadingContext public float InverseDt; [FieldOffset(88)] public int WorkerCount; + [FieldOffset(92)] + public int HighestVelocityIterationCount; + [FieldOffset(96)] + public Buffer VelocityIterationCounts; //This index is written during multithreaded execution; don't want to infest any of the more frequently read properties, so it's shoved out of any dangerous cache line. @@ -417,10 +421,23 @@ int GetPreviousSyncIndexForIntegrateConstrainedKinematics(int substepIndex, int return substepIndex == 1 ? PoseIntegrator.Callbacks.IntegrateVelocityForKinematics ? 2 : 0 : Math.Max(0, syncIndex - syncStagesPerSubstep); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - int GetPreviousSyncIndexForWarmStart(int syncIndex, int synchronizedBatchCount) + int GetWarmStartLookback(int substepIndex, int synchronizedBatchCount) + { + //Warm start and solve share the same claims buffer, so we want to look back to the last execution of the solver. + //Variable velocity iteration counts make this slightly tricky- we must skip over + //"+ 2" is just for the first two stages- incremental update and integration of constrained kinematics. + var warmStartLookback = synchronizedBatchCount + 2; + if (substepIndex > 0) + { + warmStartLookback += synchronizedBatchCount * (substepContext.HighestVelocityIterationCount - substepContext.VelocityIterationCounts[substepIndex - 1]); + } + return warmStartLookback; + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + int GetPreviousSyncIndexForWarmStart(int syncIndex, int warmStartLookback) { //The claims for warmstarts and solves are shared. So we want to look back to the last solve's claims, which would be beyond the incremental update and integrate constrained kinematics. - return Math.Max(0, syncIndex - synchronizedBatchCount - 2); + return Math.Max(0, syncIndex - warmStartLookback); } [MethodImpl(MethodImplOptions.AggressiveInlining)] int GetPreviousSyncIndexForSolve(int syncIndex, int synchronizedBatchCount) @@ -497,32 +514,36 @@ void SolveWorker(int workerIndex) solver = this }; - var syncStagesPerSubstep = 2 + synchronizedBatchCount * (1 + VelocityIterationCount); + var maximumSyncStagesPerSubstep = 2 + synchronizedBatchCount * (1 + substepContext.HighestVelocityIterationCount); if (workerIndex == 0) { //This is the main 'orchestrator' thread. It tracks execution progress and notifies other threads that's it's time to work. - int syncIndex = 0; for (int substepIndex = 0; substepIndex < substepCount; ++substepIndex) { + //Note that variable velocity iteration counts per substep means that not every substep will exhaust the entirety of the allocated sync points. + //That's fine; we just need to ensure that each substep starts at a point that the worker threads can recognize is in the appropriate substep. + //Easiest to have a consistent size for each substep so the workers can simply divide the sync index to get the substep index. + //(The +1 here is just because the first dispatch expects 0 as a previous value and goes to 1, and the current sync index is what's going to be written as a claim next.) + int syncIndex = substepIndex * maximumSyncStagesPerSubstep + 1; //Note that the main thread's view of the sync index increments every single dispatch, even if there is no work. //This ensures that the workers are able to advance to the appropriate stage by examining the sync index snapshot. - ++syncIndex; if (substepIndex > 0) { - ExecuteMainStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStart, ref substepContext.Stages[0], GetPreviousSyncIndexForIncrementalUpdate(substepIndex, syncIndex, syncStagesPerSubstep), syncIndex); + ExecuteMainStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStart, ref substepContext.Stages[0], GetPreviousSyncIndexForIncrementalUpdate(substepIndex, syncIndex, maximumSyncStagesPerSubstep), syncIndex); } //Note that we do not invoke velocity integration on the first substep if kinematics do not need velocity integration. ++syncIndex; if (substepIndex > 0 || PoseIntegrator.Callbacks.IntegrateVelocityForKinematics) { integrateConstrainedKinematicsStage.SubstepIndex = substepIndex; - ExecuteMainStage(ref integrateConstrainedKinematicsStage, workerIndex, kinematicIntegrationWorkerStart, ref substepContext.Stages[1], GetPreviousSyncIndexForIntegrateConstrainedKinematics(substepIndex, syncIndex, syncStagesPerSubstep), syncIndex); + ExecuteMainStage(ref integrateConstrainedKinematicsStage, workerIndex, kinematicIntegrationWorkerStart, ref substepContext.Stages[1], GetPreviousSyncIndexForIntegrateConstrainedKinematics(substepIndex, syncIndex, maximumSyncStagesPerSubstep), syncIndex); } warmstartStage.SubstepIndex = substepIndex; + var warmStartLookback = GetWarmStartLookback(substepIndex, synchronizedBatchCount); for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) { ++syncIndex; - ExecuteMainStage(ref warmstartStage, workerIndex, batchStarts[batchIndex], ref substepContext.Stages[batchIndex + 2], GetPreviousSyncIndexForWarmStart(syncIndex, synchronizedBatchCount), syncIndex); + ExecuteMainStage(ref warmstartStage, workerIndex, batchStarts[batchIndex], ref substepContext.Stages[batchIndex + 2], GetPreviousSyncIndexForWarmStart(syncIndex, warmStartLookback), syncIndex); } if (fallbackExists) { @@ -542,7 +563,8 @@ void SolveWorker(int workerIndex) } } } - for (int iterationIndex = 0; iterationIndex < VelocityIterationCount; ++iterationIndex) + var velocityIterationCountForSubstep = substepContext.VelocityIterationCounts[substepIndex]; + for (int iterationIndex = 0; iterationIndex < velocityIterationCountForSubstep; ++iterationIndex) { for (int batchIndex = 0; batchIndex < synchronizedBatchCount; ++batchIndex) { @@ -594,9 +616,9 @@ void SolveWorker(int workerIndex) syncIndexInSubstep += syncStepsSinceLast; while (true) { - if (syncIndexInSubstep >= syncStagesPerSubstep) + if (syncIndexInSubstep >= maximumSyncStagesPerSubstep) { - syncIndexInSubstep -= syncStagesPerSubstep; + syncIndexInSubstep -= maximumSyncStagesPerSubstep; ++substepIndex; } else @@ -612,15 +634,15 @@ void SolveWorker(int workerIndex) switch (stage.StageType) { case SolverStageType.IncrementalUpdate: - ExecuteWorkerStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStart, 0, ref stage.Claims, GetPreviousSyncIndexForIncrementalUpdate(substepIndex, syncIndex, syncStagesPerSubstep), syncIndex, ref substepContext.CompletedWorkBlockCount); + ExecuteWorkerStage(ref incrementalUpdateStage, workerIndex, incrementalUpdateWorkerStart, 0, ref stage.Claims, GetPreviousSyncIndexForIncrementalUpdate(substepIndex, syncIndex, maximumSyncStagesPerSubstep), syncIndex, ref substepContext.CompletedWorkBlockCount); break; case SolverStageType.IntegrateConstrainedKinematics: integrateConstrainedKinematicsStage.SubstepIndex = substepIndex; - ExecuteWorkerStage(ref integrateConstrainedKinematicsStage, workerIndex, kinematicIntegrationWorkerStart, 0, ref stage.Claims, GetPreviousSyncIndexForIntegrateConstrainedKinematics(substepIndex, syncIndex, syncStagesPerSubstep), syncIndex, ref substepContext.CompletedWorkBlockCount); + ExecuteWorkerStage(ref integrateConstrainedKinematicsStage, workerIndex, kinematicIntegrationWorkerStart, 0, ref stage.Claims, GetPreviousSyncIndexForIntegrateConstrainedKinematics(substepIndex, syncIndex, maximumSyncStagesPerSubstep), syncIndex, ref substepContext.CompletedWorkBlockCount); break; case SolverStageType.WarmStart: warmstartStage.SubstepIndex = substepIndex; - ExecuteWorkerStage(ref warmstartStage, workerIndex, batchStarts[stage.BatchIndex], stage.WorkBlockStartIndex, ref stage.Claims, GetPreviousSyncIndexForWarmStart(syncIndex, synchronizedBatchCount), syncIndex, ref substepContext.CompletedWorkBlockCount); + ExecuteWorkerStage(ref warmstartStage, workerIndex, batchStarts[stage.BatchIndex], stage.WorkBlockStartIndex, ref stage.Claims, GetPreviousSyncIndexForWarmStart(syncIndex, GetWarmStartLookback(substepIndex, synchronizedBatchCount)), syncIndex, ref substepContext.CompletedWorkBlockCount); break; case SolverStageType.Solve: ExecuteWorkerStage(ref solveStage, workerIndex, batchStarts[stage.BatchIndex], stage.WorkBlockStartIndex, ref stage.Claims, GetPreviousSyncIndexForSolve(syncIndex, synchronizedBatchCount), syncIndex, ref substepContext.CompletedWorkBlockCount); @@ -630,9 +652,7 @@ void SolveWorker(int workerIndex) } } - } - Buffer BuildKinematicIntegrationWorkBlocks(int minimumBlockSizeInBundles, int maximumBlockSizeInBundles, int targetBlockCount) { var bundleCount = BundleIndexing.GetBundleCount(ConstrainedKinematicHandles.Count); @@ -721,12 +741,38 @@ protected unsafe void BuildWorkBlocks( } } + int GetVelocityIterationCountForSubstepIndex(int substepIndex) + { + if (VelocityIterationScheduler != null) + { + var scheduledCount = VelocityIterationScheduler(substepIndex); + return scheduledCount < 1 ? VelocityIterationCount : scheduledCount; + } + return VelocityIterationCount; + } //Buffer> debugStageWorkBlocksCompleted; - protected void ExecuteMultithreaded2(float dt, IThreadDispatcher threadDispatcher, Action workDelegate) + protected void ExecuteMultithreaded(float dt, IThreadDispatcher threadDispatcher, Action workDelegate) { var workerCount = substepContext.WorkerCount = threadDispatcher.ThreadCount; substepContext.Dt = dt; substepContext.InverseDt = 1f / dt; + pool.Take(substepCount, out substepContext.VelocityIterationCounts); + //Each substep can have a different number of velocity iterations. + if (VelocityIterationScheduler == null) + { + for (int i = 0; i < substepCount; ++i) + { + substepContext.VelocityIterationCounts[i] = VelocityIterationCount; + } + } + else + { + for (int i = 0; i < substepCount; ++i) + { + substepContext.VelocityIterationCounts[i] = GetVelocityIterationCountForSubstepIndex(i); + } + } + //First build a set of work blocks. //The block size should be relatively small to give the workstealer something to do, but we don't want to go crazy with the number of blocks. //These values are found by empirical tuning. The optimal values may vary by architecture. @@ -753,7 +799,12 @@ protected void ExecuteMultithreaded2(float dt, IThreadDispatcher threadDispatche var totalConstraintBatchWorkBlockCount = substepContext.ConstraintBatchBoundaries.Length == 0 ? 0 : substepContext.ConstraintBatchBoundaries[^1]; var totalClaimCount = incrementalBlocks.Count + substepContext.KinematicIntegrationBlocks.Length + totalConstraintBatchWorkBlockCount; GetSynchronizedBatchCount(out var stagesPerIteration, out var fallbackExists); - pool.Take(2 + stagesPerIteration * (1 + VelocityIterationCount), out substepContext.Stages); + substepContext.HighestVelocityIterationCount = 0; + for (int i = 0; i < substepContext.VelocityIterationCounts.Length; ++i) + { + substepContext.HighestVelocityIterationCount = Math.Max(substepContext.VelocityIterationCounts[i], substepContext.HighestVelocityIterationCount); + } + pool.Take(2 + stagesPerIteration * (1 + substepContext.HighestVelocityIterationCount), out substepContext.Stages); //Claims will be monotonically increasing throughout execution. All should start at zero to match with the initial sync index. pool.Take(totalClaimCount, out var claims); claims.Clear(0, claims.Length); @@ -773,7 +824,7 @@ protected void ExecuteMultithreaded2(float dt, IThreadDispatcher threadDispatche substepContext.Stages[stageIndex] = new(claims.Slice(claimStart, workBlocksInBatch), batchStart, SolverStageType.WarmStart, batchIndex); claimStart += workBlocksInBatch; } - for (int iterationIndex = 0; iterationIndex < VelocityIterationCount; ++iterationIndex) + for (int iterationIndex = 0; iterationIndex < substepContext.HighestVelocityIterationCount; ++iterationIndex) { //Solve. Note that we're reusing the same claims as were used in the warm start for these stages; the stages just tell the workers what kind of work to do. claimStart = preambleClaimCount; @@ -886,6 +937,7 @@ protected void ExecuteMultithreaded2(float dt, IThreadDispatcher threadDispatche if (substepContext.KinematicIntegrationBlocks.Allocated) pool.Return(ref substepContext.KinematicIntegrationBlocks); pool.Return(ref substepContext.ConstraintBlocks); + pool.Return(ref substepContext.VelocityIterationCounts); @@ -1397,7 +1449,8 @@ public override void Solve(float totalDt, IThreadDispatcher threadDispatcher = n } } } - for (int iterationIndex = 0; iterationIndex < VelocityIterationCount; ++iterationIndex) + var velocityIterationCount = GetVelocityIterationCountForSubstepIndex(substepIndex); + for (int iterationIndex = 0; iterationIndex < velocityIterationCount; ++iterationIndex) { for (int i = 0; i < batchCount; ++i) { @@ -1413,7 +1466,7 @@ public override void Solve(float totalDt, IThreadDispatcher threadDispatcher = n } else { - ExecuteMultithreaded2(substepDt, threadDispatcher, solveWorker); + ExecuteMultithreaded(substepDt, threadDispatcher, solveWorker); } } } diff --git a/Demos/Demos/FountainStressTestDemo.cs b/Demos/Demos/FountainStressTestDemo.cs index c42b95124..de2de2b17 100644 --- a/Demos/Demos/FountainStressTestDemo.cs +++ b/Demos/Demos/FountainStressTestDemo.cs @@ -24,7 +24,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = MathHelper.Pi * 0.1f; //Using minimum sized allocations forces as many resizes as possible. //Note the low solverFallbackBatchThreshold- we want the fallback batches to get tested thoroughly. - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(4, fallbackBatchThreshold: 2), initialAllocationSizes: + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(new[] { 2, 1, 1 }, fallbackBatchThreshold: 2), initialAllocationSizes: new SimulationAllocationSizes { Bodies = 1, diff --git a/Demos/Program.cs b/Demos/Program.cs index f7273c9ef..0f77e0571 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -24,7 +24,7 @@ static void Main(string[] args) //HeadlessTest.Test(content, 8, 32, 1024); //HeadlessTest.Test(content, 4, 32, 512); //HeadlessTest.Test(content, 4, 128, 1024); - //DeterminismTest.Test(content, 1, 65536); + //DeterminismTest.Test(content, 1, 20000); //InvasiveHashDiagnostics.Initialize(5, 192000); var demo = new DemoHarness(loop, content); loop.Run(demo); From 623c9f347e51561b95680112d516ea4f3f099806 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 8 Dec 2021 16:03:03 -0600 Subject: [PATCH 317/947] Solver now exposes SubstepStarted and SubstepEnded events. --- BepuPhysics/ITimestepper.cs | 8 -------- BepuPhysics/Solver.cs | 27 +++++++++++++++++++++++++++ BepuPhysics/Solver_Solve.cs | 4 ++++ Demos/Demos/Cars/CarDemo.cs | 1 - 4 files changed, 31 insertions(+), 9 deletions(-) diff --git a/BepuPhysics/ITimestepper.cs b/BepuPhysics/ITimestepper.cs index b27c3e184..d37f49edb 100644 --- a/BepuPhysics/ITimestepper.cs +++ b/BepuPhysics/ITimestepper.cs @@ -12,14 +12,6 @@ namespace BepuPhysics /// Thread dispatcher used for this timestep. public delegate void TimestepperStageHandler(float dt, IThreadDispatcher threadDispatcher); - /// - /// Delegate used by ITimesteppers for stage callbacks within substepping loops. - /// - /// Index of the substep executing this stage. - /// Time step duration. - /// Thread dispatcher used for this timestep. - public delegate void TimestepperSubstepStageHandler(int substepIndex, float dt, IThreadDispatcher threadDispatcher); - /// /// Defines a type capable of updating the simulation state for a given elapsed time. /// diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index 805414dec..99485a5df 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -118,6 +118,33 @@ public int MinimumCapacityPerTypeBatch } int[] minimumInitialCapacityPerTypeBatch = new int[0]; + /// + /// Delegate type of solver substep begin/end events. + /// + /// Index of the substep that the event is about. + public delegate void SubstepEvent(int substepIndex); + /// + /// Event invoked when the solver begins a substep. If the solver is executing on multiple threads, this will be invoked within the multithreaded dispatch on worker thread 0. + /// + /// Take care when attempting to dispatch multithreaded operations from within this event. + /// If using the same instance as the solver, the dispatcher implementation must be reentrant. The demos implementation is not. + public event SubstepEvent SubstepStarted; + /// + /// Event invoked when the solver completes a substep. If the solver is executing on multiple threads, this will be invoked within the multithreaded dispatch on worker thread 0. + /// + /// Take care when attempting to dispatch multithreaded operations from within this event. + /// If using the same instance as the solver, the dispatcher implementation must be reentrant. The demos implementation is not. + public event SubstepEvent SubstepEnded; + + protected void OnSubstepStarted(int substepIndex) + { + SubstepStarted?.Invoke(substepIndex); + } + protected void OnSubstepEnded(int substepIndex) + { + SubstepEnded?.Invoke(substepIndex); + } + /// /// Sets the minimum capacity initially allocated to a new type batch of the given type. /// diff --git a/BepuPhysics/Solver_Solve.cs b/BepuPhysics/Solver_Solve.cs index faa3b020d..95d275e17 100644 --- a/BepuPhysics/Solver_Solve.cs +++ b/BepuPhysics/Solver_Solve.cs @@ -520,6 +520,7 @@ void SolveWorker(int workerIndex) //This is the main 'orchestrator' thread. It tracks execution progress and notifies other threads that's it's time to work. for (int substepIndex = 0; substepIndex < substepCount; ++substepIndex) { + OnSubstepStarted(substepIndex); //Note that variable velocity iteration counts per substep means that not every substep will exhaust the entirety of the allocated sync points. //That's fine; we just need to ensure that each substep starts at a point that the worker threads can recognize is in the appropriate substep. //Easiest to have a consistent size for each substep so the workers can simply divide the sync index to get the substep index. @@ -584,6 +585,7 @@ void SolveWorker(int workerIndex) } } } + OnSubstepEnded(substepIndex); } //All done; notify waiting threads to join. Volatile.Write(ref substepContext.SyncIndex, int.MinValue); @@ -1413,6 +1415,7 @@ public override void Solve(float totalDt, IThreadDispatcher threadDispatcher = n var incrementalUpdateFilter = default(IncrementalContactDataUpdateFilter); for (int substepIndex = 0; substepIndex < substepCount; ++substepIndex) { + OnSubstepStarted(substepIndex); if (substepIndex > 0) { for (int i = 0; i < batchCount; ++i) @@ -1462,6 +1465,7 @@ public override void Solve(float totalDt, IThreadDispatcher threadDispatcher = n } } } + OnSubstepEnded(substepIndex); } } else diff --git a/Demos/Demos/Cars/CarDemo.cs b/Demos/Demos/Cars/CarDemo.cs index 975b78df4..bf16882c9 100644 --- a/Demos/Demos/Cars/CarDemo.cs +++ b/Demos/Demos/Cars/CarDemo.cs @@ -44,7 +44,6 @@ public override void Initialize(ContentArchive content, Camera camera) Simulation = Simulation.Create(BufferPool, new CarCallbacks() { Properties = properties }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); - var builder = new CompoundBuilder(BufferPool, Simulation.Shapes, 2); builder.Add(new Box(1.85f, 0.7f, 4.73f), RigidPose.Identity, 10); builder.Add(new Box(1.85f, 0.6f, 2.5f), new Vector3(0, 0.65f, -0.35f), 0.5f); From 3dfce9566a22b81f1bc94154ead8e234ca40ce91 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 8 Dec 2021 16:21:17 -0600 Subject: [PATCH 318/947] Simplified pose integrator callbacks signature thanks to improved codegen. --- BepuPhysics/PoseIntegrator.cs | 4 ++-- Demos/Demo.cs | 2 +- Demos/DemoCallbacks.cs | 3 +-- Demos/Demos/NewtDemo.cs | 2 +- Demos/Demos/PlanetDemo.cs | 3 +-- Demos/Demos/SimpleSelfContainedDemo.cs | 3 +-- Demos/SpecializedTests/GyroscopeTestDemo.cs | 4 +--- 7 files changed, 8 insertions(+), 13 deletions(-) diff --git a/BepuPhysics/PoseIntegrator.cs b/BepuPhysics/PoseIntegrator.cs index e53321dca..e65b5814c 100644 --- a/BepuPhysics/PoseIntegrator.cs +++ b/BepuPhysics/PoseIntegrator.cs @@ -90,8 +90,8 @@ public interface IPoseIntegratorCallbacks /// Durations to integrate the velocity over. Can vary over lanes. /// Velocity of bodies in the bundle. Any changes to lanes which are not active by the integrationMask will be discarded. void IntegrateVelocity( - in Vector bodyIndices, in Vector3Wide position, in QuaternionWide orientation, in BodyInertiaWide localInertia, - in Vector integrationMask, int workerIndex, in Vector dt, ref BodyVelocityWide velocity); + Vector bodyIndices, Vector3Wide position, QuaternionWide orientation, BodyInertiaWide localInertia, + Vector integrationMask, int workerIndex, Vector dt, ref BodyVelocityWide velocity); } /// diff --git a/Demos/Demo.cs b/Demos/Demo.cs index 37ebe5190..3d49f0863 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -41,7 +41,7 @@ protected Demo() //For example, if you're using the 64 core quad memory channel AMD 3990x on a scene composed of thousands of ragdolls, //there won't be enough memory bandwidth to even feed half the physical cores. Using all 128 logical cores would just add overhead. - //It may be worth using something like hwloc to extract extra information to reason about. + //It may be worth using something like hwloc or CPUID to extract extra information to reason about. //var targetThreadCount = Math.Max(1, Environment.ProcessorCount > 4 ? Environment.ProcessorCount - 2 : Environment.ProcessorCount - 1); var targetThreadCount = 30; ThreadDispatcher = new SimpleThreadDispatcher(targetThreadCount); diff --git a/Demos/DemoCallbacks.cs b/Demos/DemoCallbacks.cs index 5c575e483..c758ca08f 100644 --- a/Demos/DemoCallbacks.cs +++ b/Demos/DemoCallbacks.cs @@ -97,8 +97,7 @@ public void PrepareForIntegration(float dt) /// Index of the worker thread processing this bundle. /// Durations to integrate the velocity over. Can vary over lanes. /// Velocity of bodies in the bundle. Any changes to lanes which are not active by the integrationMask will be discarded. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IntegrateVelocity(in Vector bodyIndices, in Vector3Wide position, in QuaternionWide orientation, in BodyInertiaWide localInertia, in Vector integrationMask, int workerIndex, in Vector dt, ref BodyVelocityWide velocity) + public void IntegrateVelocity(Vector bodyIndices, Vector3Wide position, QuaternionWide orientation, BodyInertiaWide localInertia, Vector integrationMask, int workerIndex, Vector dt, ref BodyVelocityWide velocity) { //This is also a handy spot to implement things like position dependent gravity or per-body damping. Here, //Note that we don't have to check for kinematics; IntegrateVelocityForKinematics returns false, so we'll never see them in this callback. diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index 1a3e31ddc..0a65e89b4 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -698,7 +698,7 @@ internal unsafe static void CreateDeformable(Simulation simulation, in Vector3 p edges.Dispose(pool); } - + public unsafe override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(-5f, 5.5f, 5f); diff --git a/Demos/Demos/PlanetDemo.cs b/Demos/Demos/PlanetDemo.cs index aa2351ad8..bcbdf1a4b 100644 --- a/Demos/Demos/PlanetDemo.cs +++ b/Demos/Demos/PlanetDemo.cs @@ -43,8 +43,7 @@ public void PrepareForIntegration(float dt) gravityDt = dt * Gravity; } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IntegrateVelocity(in Vector bodyIndices, in Vector3Wide position, in QuaternionWide orientation, in BodyInertiaWide localInertia, in Vector integrationMask, int workerIndex, in Vector dt, ref BodyVelocityWide velocity) + public void IntegrateVelocity(Vector bodyIndices, Vector3Wide position, QuaternionWide orientation, BodyInertiaWide localInertia, Vector integrationMask, int workerIndex, Vector dt, ref BodyVelocityWide velocity) { var offset = position - Vector3Wide.Broadcast(PlanetCenter); var distance = offset.Length(); diff --git a/Demos/Demos/SimpleSelfContainedDemo.cs b/Demos/Demos/SimpleSelfContainedDemo.cs index 53484d892..6cf49cc6c 100644 --- a/Demos/Demos/SimpleSelfContainedDemo.cs +++ b/Demos/Demos/SimpleSelfContainedDemo.cs @@ -195,8 +195,7 @@ public void PrepareForIntegration(float dt) /// Index of the worker thread processing this bundle. /// Durations to integrate the velocity over. Can vary over lanes. /// Velocity of bodies in the bundle. Any changes to lanes which are not active by the integrationMask will be discarded. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IntegrateVelocity(in Vector bodyIndices, in Vector3Wide position, in QuaternionWide orientation, in BodyInertiaWide localInertia, in Vector integrationMask, int workerIndex, in Vector dt, ref BodyVelocityWide velocity) + public void IntegrateVelocity(Vector bodyIndices, Vector3Wide position, QuaternionWide orientation, BodyInertiaWide localInertia, Vector integrationMask, int workerIndex, Vector dt, ref BodyVelocityWide velocity) { velocity.Linear += gravityWideDt; } diff --git a/Demos/SpecializedTests/GyroscopeTestDemo.cs b/Demos/SpecializedTests/GyroscopeTestDemo.cs index 792d10948..386f8d0b7 100644 --- a/Demos/SpecializedTests/GyroscopeTestDemo.cs +++ b/Demos/SpecializedTests/GyroscopeTestDemo.cs @@ -34,14 +34,12 @@ public GyroscopicIntegratorCallbacks(Vector3 gravity, float linearDamping, float innerCallbacks = new DemoPoseIntegratorCallbacks(gravity, linearDamping, angularDamping); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] public void PrepareForIntegration(float dt) { innerCallbacks.PrepareForIntegration(dt); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IntegrateVelocity(in Vector bodyIndices, in Vector3Wide position, in QuaternionWide orientation, in BodyInertiaWide localInertia, in Vector integrationMask, int workerIndex, in Vector dt, ref BodyVelocityWide velocity) + public void IntegrateVelocity(Vector bodyIndices, Vector3Wide position, QuaternionWide orientation, BodyInertiaWide localInertia, Vector integrationMask, int workerIndex, Vector dt, ref BodyVelocityWide velocity) { innerCallbacks.IntegrateVelocity(bodyIndices, position, orientation, localInertia, integrationMask, workerIndex, dt, ref velocity); } From 900b17bfc57d3846485724b98ef1c340e5d98d49 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 8 Dec 2021 19:55:08 -0600 Subject: [PATCH 319/947] Continuous detection demo tuning. --- Demos/DemoCallbacks.cs | 7 +++++++ Demos/Demos/ContinuousCollisionDetectionDemo.cs | 8 +++++--- Demos/Program.cs | 2 +- 3 files changed, 13 insertions(+), 4 deletions(-) diff --git a/Demos/DemoCallbacks.cs b/Demos/DemoCallbacks.cs index c758ca08f..0ada9262b 100644 --- a/Demos/DemoCallbacks.cs +++ b/Demos/DemoCallbacks.cs @@ -111,6 +111,13 @@ public unsafe struct DemoNarrowPhaseCallbacks : INarrowPhaseCallbacks public float MaximumRecoveryVelocity; public float FrictionCoefficient; + public DemoNarrowPhaseCallbacks(SpringSettings contactSpringiness, float maximumRecoveryVelocity = 2f, float frictionCoefficient = 1f) + { + ContactSpringiness = contactSpringiness; + MaximumRecoveryVelocity = maximumRecoveryVelocity; + FrictionCoefficient = frictionCoefficient; + } + public void Initialize(Simulation simulation) { //Use a default if the springiness value wasn't initialized... at least until struct field initializers are supported outside of previews. diff --git a/Demos/Demos/ContinuousCollisionDetectionDemo.cs b/Demos/Demos/ContinuousCollisionDetectionDemo.cs index cde37b054..1a5d547e5 100644 --- a/Demos/Demos/ContinuousCollisionDetectionDemo.cs +++ b/Demos/Demos/ContinuousCollisionDetectionDemo.cs @@ -51,10 +51,12 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0; //Note the higher stiffness on contacts for this demo. That's not ideal for general stability at the demo timestep duration default of 60hz, but //this demo doesn't have any significant solver complexity and we want to see the CCD in action more clearly- which means more rigid contact. - //Having objects bounce a bunch on impact makes it harder to see. + //Having objects bounce (or even squish through each other if they're thin enough!) makes things harder to see. + //Note that this demo only uses 1 substep. For high impact velocities, more velocity iterations can avoid inducing rotations due to incomplete contact solves in any given substep. + //That's handy for keeping the impact more controlled and visualizing the difference between discrete and continuous modes. Simulation = Simulation.Create(BufferPool, - new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(240, 1), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 1f }, - new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 8); + new DemoNarrowPhaseCallbacks(new SpringSettings(120, 1), maximumRecoveryVelocity: 1f), + new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), (1, 8)); var shape = new Box(1, 1, 1); var inertia = shape.ComputeInertia(1); diff --git a/Demos/Program.cs b/Demos/Program.cs index 0f77e0571..3bfce3982 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -21,7 +21,7 @@ static void Main(string[] args) content = ContentArchive.Load(stream); } //HeadlessTest.Test(content, 4, 32, 512); - //HeadlessTest.Test(content, 8, 32, 1024); + //HeadlessTest.Test(content, 4, 32, 512); //HeadlessTest.Test(content, 4, 32, 512); //HeadlessTest.Test(content, 4, 128, 1024); //DeterminismTest.Test(content, 1, 20000); From 24e35c5c5f9d286d2403a04c7d97b88ffa270bda Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 10 Dec 2021 11:39:13 -0600 Subject: [PATCH 320/947] Character demo constraints updated for substepping style. --- .../Characters/CharacterMotionConstraint.cs | 174 ++++++++++++++++-- .../Characters/CharacterMotionConstraint.tt | 106 ++++++++++- 2 files changed, 257 insertions(+), 23 deletions(-) diff --git a/Demos/Demos/Characters/CharacterMotionConstraint.cs b/Demos/Demos/Characters/CharacterMotionConstraint.cs index b1d272891..d5fc98b25 100644 --- a/Demos/Demos/Characters/CharacterMotionConstraint.cs +++ b/Demos/Demos/Characters/CharacterMotionConstraint.cs @@ -306,19 +306,85 @@ public void Solve(ref BodyVelocityWide velocityA, ref StaticCharacterMotionProje ApplyVerticalImpulse(basis, verticalAngularJacobianA, verticalCorrectiveImpulse, projection.InertiaA, ref velocityA); } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref StaticCharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA) - { - throw new NotImplementedException(); + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref StaticCharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide velocityA) + { + ComputeJacobians(prestep.OffsetFromCharacter, prestep.SurfaceBasis, + out var basis, out var horizontalAngularJacobianA, out var verticalAngularJacobianA); + ApplyHorizontalImpulse(basis, horizontalAngularJacobianA, accumulatedImpulses.Horizontal, inertiaA, ref velocityA); + ApplyVerticalImpulse(basis, verticalAngularJacobianA, accumulatedImpulses.Vertical, inertiaA, ref velocityA); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref StaticCharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA) - { - throw new NotImplementedException(); + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref StaticCharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide velocityA) + { + //The motion constraint is split into two parts: the horizontal constraint, and the vertical constraint. + //The horizontal constraint acts almost exactly like the TangentFriction, but we'll duplicate some of the logic to keep this implementation self-contained. + ComputeJacobians(prestep.OffsetFromCharacter, prestep.SurfaceBasis, + out var basis, out var horizontalAngularJacobianA, out var verticalAngularJacobianA); + + //Compute the velocity error by projecting the body velocity into constraint space using the transposed jacobian. + Vector2Wide horizontalLinearA; + Vector3Wide.Dot(basis.X, velocityA.Linear, out horizontalLinearA.X); + Vector3Wide.Dot(basis.Z, velocityA.Linear, out horizontalLinearA.Y); + Matrix2x3Wide.TransformByTransposeWithoutOverlap(velocityA.Angular, horizontalAngularJacobianA, out var horizontalAngularA); + Vector2Wide.Add(horizontalLinearA, horizontalAngularA, out var horizontalVelocity); + + //I'll omit the details of where this comes from, but you can check out the other constraints or the sorta-tutorial Inequality1DOF constraint to explain the details, + //plus some other references. The idea is that we need a way to transform the constraint space velocity (that we get from transforming body velocities + //by the transpose jacobian) into a corrective impulse for the solver iterations. That corrective impulse is then used to update the velocities on each iteration execution. + //This transform is the 'effective mass', representing the mass felt by the constraint in its local space. + //In concept, this constraint is actually two separate constraints solved iteratively, so we have two separate such effective mass transforms. + Symmetric3x3Wide.MatrixSandwich(horizontalAngularJacobianA, inertiaA.InverseInertiaTensor, out var inverseHorizontalEffectiveMass); + //The linear jacobians are unit length vectors, so J * M^-1 * JT is just M^-1. + inverseHorizontalEffectiveMass.XX += inertiaA.InverseMass; + inverseHorizontalEffectiveMass.YY += inertiaA.InverseMass; + Symmetric2x2Wide.InvertWithoutOverlap(inverseHorizontalEffectiveMass, out var horizontalEffectiveMass); + + Vector2Wide horizontalConstraintSpaceVelocityChange; + horizontalConstraintSpaceVelocityChange.X = prestep.TargetVelocity.X - horizontalVelocity.X; + //The surface basis's Z axis points in the opposite direction to the view direction, so negate the target velocity along the Z axis to point it in the expected direction. + horizontalConstraintSpaceVelocityChange.Y = -prestep.TargetVelocity.Y - horizontalVelocity.Y; + Symmetric2x2Wide.TransformWithoutOverlap(horizontalConstraintSpaceVelocityChange, horizontalEffectiveMass, out var horizontalCorrectiveImpulse); + + //Limit the force applied by the horizontal motion constraint. Note that this clamps the *accumulated* impulse applied this time step, not just this one iterations' value. + var previousHorizontalAccumulatedImpulse = accumulatedImpulses.Horizontal; + Vector2Wide.Add(accumulatedImpulses.Horizontal, horizontalCorrectiveImpulse, out accumulatedImpulses.Horizontal); + Vector2Wide.Length(accumulatedImpulses.Horizontal, out var horizontalImpulseMagnitude); + //Note division by zero guard. + var dtWide = new Vector(dt); + var maximumHorizontalImpulse = prestep.MaximumHorizontalForce * dtWide; + var scale = Vector.Min(Vector.One, maximumHorizontalImpulse / Vector.Max(new Vector(1e-16f), horizontalImpulseMagnitude)); + Vector2Wide.Scale(accumulatedImpulses.Horizontal, scale, out accumulatedImpulses.Horizontal); + Vector2Wide.Subtract(accumulatedImpulses.Horizontal, previousHorizontalAccumulatedImpulse, out horizontalCorrectiveImpulse); + + ApplyHorizontalImpulse(basis, horizontalAngularJacobianA, horizontalCorrectiveImpulse, inertiaA, ref velocityA); + + //Same thing for the vertical constraint. + Vector3Wide.Dot(basis.Y, velocityA.Linear, out var verticalLinearA); + Vector3Wide.Dot(velocityA.Angular, verticalAngularJacobianA, out var verticalAngularA); + //If the character is deeply penetrating, the vertical motion constraint will allow some separating velocity- just enough for one frame of integration to reach zero depth. + var verticalBiasVelocity = Vector.Max(Vector.Zero, prestep.Depth * inverseDt); + + //The vertical constraint just targets zero velocity, but does not attempt to fight any velocity which would merely push the character out of penetration. + //Note that many characters will just have zero inverse inertia tensors to prevent them from rotating, so this could be optimized. + //(Removing a transform wouldn't matter, but avoiding the storage of an inertia tensor in the projection would be useful.) + //We don't take advantage of this optimization for simplicity, and so that you could use this constraint unchanged in a simulation + //where the orientation is instead controlled by some other constraint or torque- imagine a game with gravity that points in different directions. + Symmetric3x3Wide.VectorSandwich(verticalAngularJacobianA, inertiaA.InverseInertiaTensor, out var verticalAngularContributionA); + var inverseVerticalEffectiveMass = verticalAngularContributionA + inertiaA.InverseMass; + var verticalCorrectiveImpulse = (verticalBiasVelocity - verticalLinearA - verticalAngularA) / inverseVerticalEffectiveMass; + + //Clamp the vertical constraint's impulse, but note that this is a bit different than above- the vertical constraint is not allowed to *push*, so there's an extra bound at zero. + var previousVerticalAccumulatedImpulse = accumulatedImpulses.Vertical; + var maximumVerticalImpulse = prestep.MaximumVerticalForce * dtWide; + accumulatedImpulses.Vertical = Vector.Min(Vector.Zero, Vector.Max(accumulatedImpulses.Vertical + verticalCorrectiveImpulse, -maximumVerticalImpulse)); + verticalCorrectiveImpulse = accumulatedImpulses.Vertical - previousVerticalAccumulatedImpulse; + + ApplyVerticalImpulse(basis, verticalAngularJacobianA, verticalCorrectiveImpulse, inertiaA, ref velocityA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide velocityA, in Vector dt, in CharacterMotionAccumulatedImpulse accumulatedImpulses, ref StaticCharacterMotionPrestep prestep) { } @@ -654,20 +720,98 @@ public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB ApplyVerticalImpulse(basis, verticalAngularJacobianA, verticalAngularJacobianB, verticalCorrectiveImpulse, projection.InertiaA, projection.InertiaB, ref velocityA, ref velocityB); } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref DynamicCharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) - { - throw new NotImplementedException(); + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref DynamicCharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB) + { + ComputeJacobians(prestep.OffsetFromCharacter, prestep.OffsetFromSupport, prestep.SurfaceBasis, + out var basis, out var horizontalAngularJacobianA, out var horizontalAngularJacobianB, out var verticalAngularJacobianA, out var verticalAngularJacobianB); + ApplyHorizontalImpulse(basis, horizontalAngularJacobianA, horizontalAngularJacobianB, accumulatedImpulses.Horizontal, inertiaA, inertiaB, ref velocityA, ref velocityB); + ApplyVerticalImpulse(basis, verticalAngularJacobianA, verticalAngularJacobianB, accumulatedImpulses.Vertical, inertiaA, inertiaB, ref velocityA, ref velocityB); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref DynamicCharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) - { - throw new NotImplementedException(); + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref DynamicCharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB) + { + //The motion constraint is split into two parts: the horizontal constraint, and the vertical constraint. + //The horizontal constraint acts almost exactly like the TangentFriction, but we'll duplicate some of the logic to keep this implementation self-contained. + ComputeJacobians(prestep.OffsetFromCharacter, prestep.OffsetFromSupport, prestep.SurfaceBasis, + out var basis, out var horizontalAngularJacobianA, out var horizontalAngularJacobianB, out var verticalAngularJacobianA, out var verticalAngularJacobianB); + + //Compute the velocity error by projecting the body velocity into constraint space using the transposed jacobian. + Vector2Wide horizontalLinearA; + Vector3Wide.Dot(basis.X, velocityA.Linear, out horizontalLinearA.X); + Vector3Wide.Dot(basis.Z, velocityA.Linear, out horizontalLinearA.Y); + Matrix2x3Wide.TransformByTransposeWithoutOverlap(velocityA.Angular, horizontalAngularJacobianA, out var horizontalAngularA); + Vector2Wide negatedHorizontalLinearB; + Vector3Wide.Dot(basis.X, velocityB.Linear, out negatedHorizontalLinearB.X); + Vector3Wide.Dot(basis.Z, velocityB.Linear, out negatedHorizontalLinearB.Y); + Matrix2x3Wide.TransformByTransposeWithoutOverlap(velocityB.Angular, horizontalAngularJacobianB, out var horizontalAngularB); + Vector2Wide.Add(horizontalAngularA, horizontalAngularB, out var horizontalAngular); + Vector2Wide.Subtract(horizontalLinearA, negatedHorizontalLinearB, out var horizontalLinear); + Vector2Wide.Add(horizontalAngular, horizontalLinear, out var horizontalVelocity); + + //I'll omit the details of where this comes from, but you can check out the other constraints or the sorta-tutorial Inequality1DOF constraint to explain the details, + //plus some other references. The idea is that we need a way to transform the constraint space velocity (that we get from transforming body velocities + //by the transpose jacobian) into a corrective impulse for the solver iterations. That corrective impulse is then used to update the velocities on each iteration execution. + //This transform is the 'effective mass', representing the mass felt by the constraint in its local space. + //In concept, this constraint is actually two separate constraints solved iteratively, so we have two separate such effective mass transforms. + Symmetric3x3Wide.MatrixSandwich(horizontalAngularJacobianA, inertiaA.InverseInertiaTensor, out var horizontalAngularContributionA); + Symmetric3x3Wide.MatrixSandwich(horizontalAngularJacobianB, inertiaB.InverseInertiaTensor, out var horizontalAngularContributionB); + Symmetric2x2Wide.Add(horizontalAngularContributionA, horizontalAngularContributionB, out var inverseHorizontalEffectiveMass); + //The linear jacobians are unit length vectors, so J * M^-1 * JT is just M^-1. + var linearContribution = inertiaA.InverseMass + inertiaB.InverseMass; + inverseHorizontalEffectiveMass.XX += linearContribution; + inverseHorizontalEffectiveMass.YY += linearContribution; + Symmetric2x2Wide.InvertWithoutOverlap(inverseHorizontalEffectiveMass, out var horizontalEffectiveMass); + + Vector2Wide horizontalConstraintSpaceVelocityChange; + horizontalConstraintSpaceVelocityChange.X = prestep.TargetVelocity.X - horizontalVelocity.X; + //The surface basis's Z axis points in the opposite direction to the view direction, so negate the target velocity along the Z axis to point it in the expected direction. + horizontalConstraintSpaceVelocityChange.Y = -prestep.TargetVelocity.Y - horizontalVelocity.Y; + Symmetric2x2Wide.TransformWithoutOverlap(horizontalConstraintSpaceVelocityChange, horizontalEffectiveMass, out var horizontalCorrectiveImpulse); + + //Limit the force applied by the horizontal motion constraint. Note that this clamps the *accumulated* impulse applied this time step, not just this one iterations' value. + var previousHorizontalAccumulatedImpulse = accumulatedImpulses.Horizontal; + Vector2Wide.Add(accumulatedImpulses.Horizontal, horizontalCorrectiveImpulse, out accumulatedImpulses.Horizontal); + Vector2Wide.Length(accumulatedImpulses.Horizontal, out var horizontalImpulseMagnitude); + //Note division by zero guard. + var dtWide = new Vector(dt); + var maximumHorizontalImpulse = prestep.MaximumHorizontalForce * dtWide; + var scale = Vector.Min(Vector.One, maximumHorizontalImpulse / Vector.Max(new Vector(1e-16f), horizontalImpulseMagnitude)); + Vector2Wide.Scale(accumulatedImpulses.Horizontal, scale, out accumulatedImpulses.Horizontal); + Vector2Wide.Subtract(accumulatedImpulses.Horizontal, previousHorizontalAccumulatedImpulse, out horizontalCorrectiveImpulse); + + ApplyHorizontalImpulse(basis, horizontalAngularJacobianA, horizontalAngularJacobianB, horizontalCorrectiveImpulse, inertiaA, inertiaB, ref velocityA, ref velocityB); + + //Same thing for the vertical constraint. + Vector3Wide.Dot(basis.Y, velocityA.Linear, out var verticalLinearA); + Vector3Wide.Dot(velocityA.Angular, verticalAngularJacobianA, out var verticalAngularA); + Vector3Wide.Dot(basis.Y, velocityB.Linear, out var negatedVerticalLinearB); + Vector3Wide.Dot(velocityB.Angular, verticalAngularJacobianB, out var verticalAngularB); + //If the character is deeply penetrating, the vertical motion constraint will allow some separating velocity- just enough for one frame of integration to reach zero depth. + var verticalBiasVelocity = Vector.Max(Vector.Zero, prestep.Depth * inverseDt); + + //The vertical constraint just targets zero velocity, but does not attempt to fight any velocity which would merely push the character out of penetration. + //Note that many characters will just have zero inverse inertia tensors to prevent them from rotating, so this could be optimized. + //(Removing a transform wouldn't matter, but avoiding the storage of an inertia tensor in the projection would be useful.) + //We don't take advantage of this optimization for simplicity, and so that you could use this constraint unchanged in a simulation + //where the orientation is instead controlled by some other constraint or torque- imagine a game with gravity that points in different directions. + Symmetric3x3Wide.VectorSandwich(verticalAngularJacobianA, inertiaA.InverseInertiaTensor, out var verticalAngularContributionA); + Symmetric3x3Wide.VectorSandwich(verticalAngularJacobianB, inertiaB.InverseInertiaTensor, out var verticalAngularContributionB); + var inverseVerticalEffectiveMass = verticalAngularContributionA + verticalAngularContributionB + linearContribution; + var verticalCorrectiveImpulse = (verticalBiasVelocity - verticalLinearA + negatedVerticalLinearB - verticalAngularA - verticalAngularB) / inverseVerticalEffectiveMass; + + //Clamp the vertical constraint's impulse, but note that this is a bit different than above- the vertical constraint is not allowed to *push*, so there's an extra bound at zero. + var previousVerticalAccumulatedImpulse = accumulatedImpulses.Vertical; + var maximumVerticalImpulse = prestep.MaximumVerticalForce * dtWide; + accumulatedImpulses.Vertical = Vector.Min(Vector.Zero, Vector.Max(accumulatedImpulses.Vertical + verticalCorrectiveImpulse, -maximumVerticalImpulse)); + verticalCorrectiveImpulse = accumulatedImpulses.Vertical - previousVerticalAccumulatedImpulse; + + ApplyVerticalImpulse(basis, verticalAngularJacobianA, verticalAngularJacobianB, verticalCorrectiveImpulse, inertiaA, inertiaB, ref velocityA, ref velocityB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide velocityA, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide velocityB, in Vector dt, in CharacterMotionAccumulatedImpulse accumulatedImpulses, ref DynamicCharacterMotionPrestep prestep) { } diff --git a/Demos/Demos/Characters/CharacterMotionConstraint.tt b/Demos/Demos/Characters/CharacterMotionConstraint.tt index a4e79219d..676612c34 100644 --- a/Demos/Demos/Characters/CharacterMotionConstraint.tt +++ b/Demos/Demos/Characters/CharacterMotionConstraint.tt @@ -393,21 +393,111 @@ namespace Demos.Demos.Characters ApplyVerticalImpulse(basis, verticalAngularJacobianA, <#if (dynamic) {#>verticalAngularJacobianB, <#}#>verticalCorrectiveImpulse, projection.InertiaA, <#if (dynamic) {#>projection.InertiaB, <#}#>ref velocityA<#if (dynamic) {#>, ref velocityB<#}#>); } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(dynamic) {#>in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>ref <#=prefix#>CharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA<#if(dynamic) {#>, ref BodyVelocityWide wsvB<#}#>) - { - throw new NotImplementedException(); + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(dynamic) {#>in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>ref <#=prefix#>CharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide velocityA<#if(dynamic) {#>, ref BodyVelocityWide velocityB<#}#>) + { + ComputeJacobians(prestep.OffsetFromCharacter, <#if (dynamic) {#>prestep.OffsetFromSupport, <#}#>prestep.SurfaceBasis, + out var basis, out var horizontalAngularJacobianA, <#if (dynamic) {#>out var horizontalAngularJacobianB, <#}#>out var verticalAngularJacobianA<#if (dynamic) {#>, out var verticalAngularJacobianB<#}#>); + ApplyHorizontalImpulse(basis, horizontalAngularJacobianA, <#if (dynamic) {#>horizontalAngularJacobianB, <#}#>accumulatedImpulses.Horizontal, inertiaA, <#if (dynamic) {#>inertiaB, <#}#>ref velocityA<#if (dynamic) {#>, ref velocityB<#}#>); + ApplyVerticalImpulse(basis, verticalAngularJacobianA, <#if (dynamic) {#>verticalAngularJacobianB, <#}#>accumulatedImpulses.Vertical, inertiaA, <#if (dynamic) {#>inertiaB, <#}#>ref velocityA<#if (dynamic) {#>, ref velocityB<#}#>); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(dynamic) {#>in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>float dt, float inverseDt, ref <#=prefix#>CharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA<#if(dynamic) {#>, ref BodyVelocityWide wsvB<#}#>) - { - throw new NotImplementedException(); + public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(dynamic) {#>in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>float dt, float inverseDt, ref <#=prefix#>CharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide velocityA<#if(dynamic) {#>, ref BodyVelocityWide velocityB<#}#>) + { + //The motion constraint is split into two parts: the horizontal constraint, and the vertical constraint. + //The horizontal constraint acts almost exactly like the TangentFriction, but we'll duplicate some of the logic to keep this implementation self-contained. + ComputeJacobians(prestep.OffsetFromCharacter, <#if (dynamic) {#>prestep.OffsetFromSupport, <#}#>prestep.SurfaceBasis, + out var basis, out var horizontalAngularJacobianA, <#if (dynamic) {#>out var horizontalAngularJacobianB, <#}#>out var verticalAngularJacobianA<#if (dynamic) {#>, out var verticalAngularJacobianB<#}#>); + + //Compute the velocity error by projecting the body velocity into constraint space using the transposed jacobian. + Vector2Wide horizontalLinearA; + Vector3Wide.Dot(basis.X, velocityA.Linear, out horizontalLinearA.X); + Vector3Wide.Dot(basis.Z, velocityA.Linear, out horizontalLinearA.Y); + Matrix2x3Wide.TransformByTransposeWithoutOverlap(velocityA.Angular, horizontalAngularJacobianA, out var horizontalAngularA); +<#if (dynamic) {#> + Vector2Wide negatedHorizontalLinearB; + Vector3Wide.Dot(basis.X, velocityB.Linear, out negatedHorizontalLinearB.X); + Vector3Wide.Dot(basis.Z, velocityB.Linear, out negatedHorizontalLinearB.Y); + Matrix2x3Wide.TransformByTransposeWithoutOverlap(velocityB.Angular, horizontalAngularJacobianB, out var horizontalAngularB); + Vector2Wide.Add(horizontalAngularA, horizontalAngularB, out var horizontalAngular); + Vector2Wide.Subtract(horizontalLinearA, negatedHorizontalLinearB, out var horizontalLinear); + Vector2Wide.Add(horizontalAngular, horizontalLinear, out var horizontalVelocity); +<#} else {#> + Vector2Wide.Add(horizontalLinearA, horizontalAngularA, out var horizontalVelocity); +<#}#> + + //I'll omit the details of where this comes from, but you can check out the other constraints or the sorta-tutorial Inequality1DOF constraint to explain the details, + //plus some other references. The idea is that we need a way to transform the constraint space velocity (that we get from transforming body velocities + //by the transpose jacobian) into a corrective impulse for the solver iterations. That corrective impulse is then used to update the velocities on each iteration execution. + //This transform is the 'effective mass', representing the mass felt by the constraint in its local space. + //In concept, this constraint is actually two separate constraints solved iteratively, so we have two separate such effective mass transforms. + Symmetric3x3Wide.MatrixSandwich(horizontalAngularJacobianA, inertiaA.InverseInertiaTensor, out var <#=dynamic ? "horizontalAngularContributionA" : "inverseHorizontalEffectiveMass"#>); +<#if (dynamic) {#> + Symmetric3x3Wide.MatrixSandwich(horizontalAngularJacobianB, inertiaB.InverseInertiaTensor, out var horizontalAngularContributionB); + Symmetric2x2Wide.Add(horizontalAngularContributionA, horizontalAngularContributionB, out var inverseHorizontalEffectiveMass); +<#}#> + //The linear jacobians are unit length vectors, so J * M^-1 * JT is just M^-1. +<#if (dynamic) {#> + var linearContribution = inertiaA.InverseMass + inertiaB.InverseMass; +<#}#> + inverseHorizontalEffectiveMass.XX += <#=dynamic ? "linearContribution" : "inertiaA.InverseMass"#>; + inverseHorizontalEffectiveMass.YY += <#=dynamic ? "linearContribution" : "inertiaA.InverseMass"#>; + Symmetric2x2Wide.InvertWithoutOverlap(inverseHorizontalEffectiveMass, out var horizontalEffectiveMass); + + Vector2Wide horizontalConstraintSpaceVelocityChange; + horizontalConstraintSpaceVelocityChange.X = prestep.TargetVelocity.X - horizontalVelocity.X; + //The surface basis's Z axis points in the opposite direction to the view direction, so negate the target velocity along the Z axis to point it in the expected direction. + horizontalConstraintSpaceVelocityChange.Y = -prestep.TargetVelocity.Y - horizontalVelocity.Y; + Symmetric2x2Wide.TransformWithoutOverlap(horizontalConstraintSpaceVelocityChange, horizontalEffectiveMass, out var horizontalCorrectiveImpulse); + + //Limit the force applied by the horizontal motion constraint. Note that this clamps the *accumulated* impulse applied this time step, not just this one iterations' value. + var previousHorizontalAccumulatedImpulse = accumulatedImpulses.Horizontal; + Vector2Wide.Add(accumulatedImpulses.Horizontal, horizontalCorrectiveImpulse, out accumulatedImpulses.Horizontal); + Vector2Wide.Length(accumulatedImpulses.Horizontal, out var horizontalImpulseMagnitude); + //Note division by zero guard. + var dtWide = new Vector(dt); + var maximumHorizontalImpulse = prestep.MaximumHorizontalForce * dtWide; + var scale = Vector.Min(Vector.One, maximumHorizontalImpulse / Vector.Max(new Vector(1e-16f), horizontalImpulseMagnitude)); + Vector2Wide.Scale(accumulatedImpulses.Horizontal, scale, out accumulatedImpulses.Horizontal); + Vector2Wide.Subtract(accumulatedImpulses.Horizontal, previousHorizontalAccumulatedImpulse, out horizontalCorrectiveImpulse); + + ApplyHorizontalImpulse(basis, horizontalAngularJacobianA, <#if (dynamic) {#>horizontalAngularJacobianB, <#}#>horizontalCorrectiveImpulse, inertiaA, <#if (dynamic) {#>inertiaB, <#}#>ref velocityA<#if (dynamic) {#>, ref velocityB<#}#>); + + //Same thing for the vertical constraint. + Vector3Wide.Dot(basis.Y, velocityA.Linear, out var verticalLinearA); + Vector3Wide.Dot(velocityA.Angular, verticalAngularJacobianA, out var verticalAngularA); +<#if (dynamic) {#> + Vector3Wide.Dot(basis.Y, velocityB.Linear, out var negatedVerticalLinearB); + Vector3Wide.Dot(velocityB.Angular, verticalAngularJacobianB, out var verticalAngularB); +<#}#> + //If the character is deeply penetrating, the vertical motion constraint will allow some separating velocity- just enough for one frame of integration to reach zero depth. + var verticalBiasVelocity = Vector.Max(Vector.Zero, prestep.Depth * inverseDt); + + //The vertical constraint just targets zero velocity, but does not attempt to fight any velocity which would merely push the character out of penetration. + //Note that many characters will just have zero inverse inertia tensors to prevent them from rotating, so this could be optimized. + //(Removing a transform wouldn't matter, but avoiding the storage of an inertia tensor in the projection would be useful.) + //We don't take advantage of this optimization for simplicity, and so that you could use this constraint unchanged in a simulation + //where the orientation is instead controlled by some other constraint or torque- imagine a game with gravity that points in different directions. + Symmetric3x3Wide.VectorSandwich(verticalAngularJacobianA, inertiaA.InverseInertiaTensor, out var verticalAngularContributionA); +<#if (dynamic) {#> + Symmetric3x3Wide.VectorSandwich(verticalAngularJacobianB, inertiaB.InverseInertiaTensor, out var verticalAngularContributionB); +<#}#> + var inverseVerticalEffectiveMass = verticalAngularContributionA + <#=dynamic ? "verticalAngularContributionB + linearContribution" : "inertiaA.InverseMass"#>; + var verticalCorrectiveImpulse = (verticalBiasVelocity - verticalLinearA<#if (dynamic) {#> + negatedVerticalLinearB<#}#> - verticalAngularA<#if (dynamic) {#> - verticalAngularB<#}#>) / inverseVerticalEffectiveMass; + + //Clamp the vertical constraint's impulse, but note that this is a bit different than above- the vertical constraint is not allowed to *push*, so there's an extra bound at zero. + var previousVerticalAccumulatedImpulse = accumulatedImpulses.Vertical; + var maximumVerticalImpulse = prestep.MaximumVerticalForce * dtWide; + accumulatedImpulses.Vertical = Vector.Min(Vector.Zero, Vector.Max(accumulatedImpulses.Vertical + verticalCorrectiveImpulse, -maximumVerticalImpulse)); + verticalCorrectiveImpulse = accumulatedImpulses.Vertical - previousVerticalAccumulatedImpulse; + + ApplyVerticalImpulse(basis, verticalAngularJacobianA, <#if (dynamic) {#>verticalAngularJacobianB, <#}#>verticalCorrectiveImpulse, inertiaA, <#if (dynamic) {#>inertiaB, <#}#>ref velocityA<#if (dynamic) {#>, ref velocityB<#}#>); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, + in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide velocityA, <#if(dynamic){#> - in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide velocityB, <#}#> in Vector dt, in CharacterMotionAccumulatedImpulse accumulatedImpulses, ref <#=prefix#>CharacterMotionPrestep prestep) { From fc60f3b847337b296dc096f0758d9c61c55fe444 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 10 Dec 2021 17:18:57 -0600 Subject: [PATCH 321/947] IncrementallyUpdateForSubstep plumbed through all constraints. --- .../Constraints/AngularAxisGearMotor.cs | 8 +--- BepuPhysics/Constraints/AngularAxisMotor.cs | 8 +--- BepuPhysics/Constraints/AngularHinge.cs | 8 +--- BepuPhysics/Constraints/AngularMotor.cs | 8 +--- BepuPhysics/Constraints/AngularServo.cs | 8 +--- BepuPhysics/Constraints/AngularSwivelHinge.cs | 8 +--- BepuPhysics/Constraints/AreaConstraint.cs | 9 +--- BepuPhysics/Constraints/BallSocket.cs | 8 +--- BepuPhysics/Constraints/BallSocketMotor.cs | 8 +--- BepuPhysics/Constraints/BallSocketServo.cs | 8 +--- .../Constraints/CenterDistanceConstraint.cs | 9 +--- .../Constraints/Contact/ContactConvexTypes.cs | 48 ++++++++++++------- .../Constraints/Contact/ContactConvexTypes.tt | 6 ++- .../Contact/ContactNonconvexCommon.cs | 42 ++++++++-------- BepuPhysics/Constraints/DistanceLimit.cs | 11 ++--- BepuPhysics/Constraints/DistanceServo.cs | 8 +--- .../Constraints/FourBodyTypeProcessor.cs | 30 +++++++++--- BepuPhysics/Constraints/Hinge.cs | 8 +--- BepuPhysics/Constraints/LinearAxisLimit.cs | 8 +--- BepuPhysics/Constraints/LinearAxisMotor.cs | 8 +--- BepuPhysics/Constraints/LinearAxisServo.cs | 8 +--- .../Constraints/OneBodyAngularMotor.cs | 9 ++-- .../Constraints/OneBodyAngularServo.cs | 7 +-- BepuPhysics/Constraints/OneBodyLinearMotor.cs | 9 ++-- BepuPhysics/Constraints/OneBodyLinearServo.cs | 7 +-- .../Constraints/OneBodyTypeProcessor.cs | 46 ++++++++---------- BepuPhysics/Constraints/PointOnLineServo.cs | 8 +--- BepuPhysics/Constraints/SwingLimit.cs | 8 +--- BepuPhysics/Constraints/SwivelHinge.cs | 8 +--- .../Constraints/ThreeBodyTypeProcessor.cs | 28 +++++++++-- BepuPhysics/Constraints/TwistLimit.cs | 8 +--- BepuPhysics/Constraints/TwistMotor.cs | 8 +--- BepuPhysics/Constraints/TwistServo.cs | 8 +--- .../Constraints/TwoBodyTypeProcessor.cs | 48 ++++++++----------- BepuPhysics/Constraints/TypeProcessor.cs | 8 +++- BepuPhysics/Constraints/VolumeConstraint.cs | 10 +--- BepuPhysics/Constraints/Weld.cs | 11 ++--- BepuPhysics/Solver.cs | 3 -- BepuPhysics/Solver_Solve.cs | 4 +- .../Characters/CharacterMotionConstraint.cs | 17 ++++--- .../Characters/CharacterMotionConstraint.tt | 11 ++--- Demos/Demos/SubsteppingDemo.cs | 2 +- 42 files changed, 230 insertions(+), 305 deletions(-) diff --git a/BepuPhysics/Constraints/AngularAxisGearMotor.cs b/BepuPhysics/Constraints/AngularAxisGearMotor.cs index 005a7c412..e7a6be335 100644 --- a/BepuPhysics/Constraints/AngularAxisGearMotor.cs +++ b/BepuPhysics/Constraints/AngularAxisGearMotor.cs @@ -168,13 +168,9 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in ApplyImpulse(impulseToVelocityA, negatedImpulseToVelocityB, accumulatedImpulses, ref wsvA.Angular, ref wsvB.Angular); } + public bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, - in Vector dt, in Vector accumulatedImpulses, ref AngularAxisGearMotorPrestepData prestep) - { - } + public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref AngularAxisGearMotorPrestepData prestepData) { } } public class AngularAxisGearMotorTypeProcessor : TwoBodyTypeProcessor, AngularAxisGearMotorFunctions, AccessOnlyAngular, AccessOnlyAngularWithoutPose, AccessOnlyAngular, AccessOnlyAngularWithoutPose> diff --git a/BepuPhysics/Constraints/AngularAxisMotor.cs b/BepuPhysics/Constraints/AngularAxisMotor.cs index cb314005d..16877c1a8 100644 --- a/BepuPhysics/Constraints/AngularAxisMotor.cs +++ b/BepuPhysics/Constraints/AngularAxisMotor.cs @@ -157,13 +157,9 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in ApplyImpulse(jIA, jIB, csi, ref wsvA.Angular, ref wsvB.Angular); } + public bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, - in Vector dt, in Vector accumulatedImpulses, ref AngularAxisMotorPrestepData prestep) - { - } + public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref AngularAxisMotorPrestepData prestepData) { } } public class AngularAxisMotorTypeProcessor : TwoBodyTypeProcessor, AngularAxisMotorFunctions, AccessOnlyAngular, AccessOnlyAngularWithoutPose, AccessOnlyAngular, AccessOnlyAngular> diff --git a/BepuPhysics/Constraints/AngularHinge.cs b/BepuPhysics/Constraints/AngularHinge.cs index 6978d6cdb..5c6111bd4 100644 --- a/BepuPhysics/Constraints/AngularHinge.cs +++ b/BepuPhysics/Constraints/AngularHinge.cs @@ -333,13 +333,9 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in ApplyImpulse(impulseToVelocityA, negatedImpulseToVelocityB, csi, ref wsvA.Angular, ref wsvB.Angular); } + public bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, - in Vector dt, in Vector2Wide accumulatedImpulses, ref AngularHingePrestepData prestep) - { - } + public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref AngularHingePrestepData prestepData) { } } public class AngularHingeTypeProcessor : TwoBodyTypeProcessor diff --git a/BepuPhysics/Constraints/AngularMotor.cs b/BepuPhysics/Constraints/AngularMotor.cs index b28b8074a..de330d569 100644 --- a/BepuPhysics/Constraints/AngularMotor.cs +++ b/BepuPhysics/Constraints/AngularMotor.cs @@ -129,13 +129,9 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in AngularServoFunctions.ApplyImpulse(ref wsvA.Angular, ref wsvB.Angular, inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, csi); } + public bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, - in Vector dt, in Vector3Wide accumulatedImpulses, ref AngularMotorPrestepData prestep) - { - } + public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref AngularMotorPrestepData prestepData) { } } public class AngularMotorTypeProcessor : TwoBodyTypeProcessor diff --git a/BepuPhysics/Constraints/AngularServo.cs b/BepuPhysics/Constraints/AngularServo.cs index de2860fd0..215a0331e 100644 --- a/BepuPhysics/Constraints/AngularServo.cs +++ b/BepuPhysics/Constraints/AngularServo.cs @@ -176,13 +176,9 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in ApplyImpulse(ref wsvA.Angular, ref wsvB.Angular, inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, csi); } + public bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, - in Vector dt, in Vector3Wide accumulatedImpulses, ref AngularServoPrestepData prestep) - { - } + public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref AngularServoPrestepData prestepData) { } } public class AngularServoTypeProcessor : TwoBodyTypeProcessor diff --git a/BepuPhysics/Constraints/AngularSwivelHinge.cs b/BepuPhysics/Constraints/AngularSwivelHinge.cs index ee1492257..20ee3f376 100644 --- a/BepuPhysics/Constraints/AngularSwivelHinge.cs +++ b/BepuPhysics/Constraints/AngularSwivelHinge.cs @@ -226,13 +226,9 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in } + public bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, - in Vector dt, in Vector accumulatedImpulses, ref AngularSwivelHingePrestepData prestep) - { - } + public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref AngularSwivelHingePrestepData prestepData) { } } public class AngularSwivelHingeTypeProcessor : TwoBodyTypeProcessor, AngularSwivelHingeFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular> diff --git a/BepuPhysics/Constraints/AreaConstraint.cs b/BepuPhysics/Constraints/AreaConstraint.cs index 02011c75f..8480bb5b9 100644 --- a/BepuPhysics/Constraints/AreaConstraint.cs +++ b/BepuPhysics/Constraints/AreaConstraint.cs @@ -276,14 +276,9 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in ApplyImpulse(inertiaA.InverseMass, inertiaB.InverseMass, inertiaC.InverseMass, negatedJacobianA, jacobianB, jacobianC, csi, ref wsvA, ref wsvB, ref wsvC); } + public bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, - in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in BodyVelocityWide wsvC, - in Vector dt, in Vector accumulatedImpulses, ref AreaConstraintPrestepData prestep) - { - } + public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, in BodyVelocityWide wsvC, ref AreaConstraintPrestepData prestepData) { } } diff --git a/BepuPhysics/Constraints/BallSocket.cs b/BepuPhysics/Constraints/BallSocket.cs index 8ed618dc2..489b67911 100644 --- a/BepuPhysics/Constraints/BallSocket.cs +++ b/BepuPhysics/Constraints/BallSocket.cs @@ -134,13 +134,9 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BallSocketShared.Solve(ref wsvA, ref wsvB, offsetA, offsetB, biasVelocity, effectiveMass, softnessImpulseScale, ref accumulatedImpulses, inertiaA, inertiaB); } + public bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, - in Vector dt, in Vector3Wide accumulatedImpulses, ref BallSocketPrestepData prestep) - { - } + public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref BallSocketPrestepData prestepData) { } } diff --git a/BepuPhysics/Constraints/BallSocketMotor.cs b/BepuPhysics/Constraints/BallSocketMotor.cs index 5f2e4d722..17d94b566 100644 --- a/BepuPhysics/Constraints/BallSocketMotor.cs +++ b/BepuPhysics/Constraints/BallSocketMotor.cs @@ -130,13 +130,9 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BallSocketShared.Solve(ref wsvA, ref wsvB, offsetA, targetOffsetB, biasVelocity, effectiveMass, softnessImpulseScale, maximumImpulse, ref accumulatedImpulses, inertiaA, inertiaB); } + public bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, - in Vector dt, in Vector3Wide accumulatedImpulses, ref BallSocketMotorPrestepData prestep) - { - } + public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref BallSocketMotorPrestepData prestepData) { } } diff --git a/BepuPhysics/Constraints/BallSocketServo.cs b/BepuPhysics/Constraints/BallSocketServo.cs index 0daa35bbf..e72cb44a7 100644 --- a/BepuPhysics/Constraints/BallSocketServo.cs +++ b/BepuPhysics/Constraints/BallSocketServo.cs @@ -141,13 +141,9 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BallSocketShared.Solve(ref wsvA, ref wsvB, offsetA, offsetB, biasVelocity, effectiveMass, softnessImpulseScale, maximumImpulse, ref accumulatedImpulses, inertiaA, inertiaB); } + public bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, - in Vector dt, in Vector3Wide accumulatedImpulses, ref BallSocketServoPrestepData prestep) - { - } + public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref BallSocketServoPrestepData prestepData) { } } diff --git a/BepuPhysics/Constraints/CenterDistanceConstraint.cs b/BepuPhysics/Constraints/CenterDistanceConstraint.cs index c3839e3f2..6b97937ba 100644 --- a/BepuPhysics/Constraints/CenterDistanceConstraint.cs +++ b/BepuPhysics/Constraints/CenterDistanceConstraint.cs @@ -180,14 +180,9 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in ApplyImpulse(jacobianA, inertiaA.InverseMass, inertiaB.InverseMass, csi, ref wsvA, ref wsvB); } - + public bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, - in Vector dt, in Vector accumulatedImpulses, ref CenterDistancePrestepData prestep) - { - } + public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref CenterDistancePrestepData prestepData) { } } diff --git a/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs b/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs index bd07c3164..c72e67cdf 100644 --- a/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs +++ b/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs @@ -300,7 +300,7 @@ public unsafe struct Contact1OneBodyProjection public TwistFrictionProjection Twist; } - public struct Contact1OneBodyFunctions : IOneBodyContactConstraintFunctions + public struct Contact1OneBodyFunctions : IOneBodyConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Prestep( @@ -345,8 +345,10 @@ public void Solve(ref BodyVelocityWide wsvA, ref Contact1OneBodyProjection proje TwistFrictionOneBody.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); } + public bool RequiresIncrementalSubstepUpdates => true; + [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityWide velocityA, ref Contact1OneBodyPrestepData prestep) + public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide velocityA, ref Contact1OneBodyPrestepData prestep) { PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, velocityA, ref prestep.Contact0.Depth); } @@ -510,7 +512,7 @@ public unsafe struct Contact2OneBodyProjection public TwistFrictionProjection Twist; } - public struct Contact2OneBodyFunctions : IOneBodyContactConstraintFunctions + public struct Contact2OneBodyFunctions : IOneBodyConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Prestep( @@ -559,8 +561,10 @@ public void Solve(ref BodyVelocityWide wsvA, ref Contact2OneBodyProjection proje TwistFrictionOneBody.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); } + public bool RequiresIncrementalSubstepUpdates => true; + [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityWide velocityA, ref Contact2OneBodyPrestepData prestep) + public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide velocityA, ref Contact2OneBodyPrestepData prestep) { PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, velocityA, ref prestep.Contact0.Depth); PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.Normal, velocityA, ref prestep.Contact1.Depth); @@ -739,7 +743,7 @@ public unsafe struct Contact3OneBodyProjection public TwistFrictionProjection Twist; } - public struct Contact3OneBodyFunctions : IOneBodyContactConstraintFunctions + public struct Contact3OneBodyFunctions : IOneBodyConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Prestep( @@ -793,8 +797,10 @@ public void Solve(ref BodyVelocityWide wsvA, ref Contact3OneBodyProjection proje TwistFrictionOneBody.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); } + public bool RequiresIncrementalSubstepUpdates => true; + [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityWide velocityA, ref Contact3OneBodyPrestepData prestep) + public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide velocityA, ref Contact3OneBodyPrestepData prestep) { PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, velocityA, ref prestep.Contact0.Depth); PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.Normal, velocityA, ref prestep.Contact1.Depth); @@ -986,7 +992,7 @@ public unsafe struct Contact4OneBodyProjection public TwistFrictionProjection Twist; } - public struct Contact4OneBodyFunctions : IOneBodyContactConstraintFunctions + public struct Contact4OneBodyFunctions : IOneBodyConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Prestep( @@ -1045,8 +1051,10 @@ public void Solve(ref BodyVelocityWide wsvA, ref Contact4OneBodyProjection proje TwistFrictionOneBody.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); } + public bool RequiresIncrementalSubstepUpdates => true; + [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityWide velocityA, ref Contact4OneBodyPrestepData prestep) + public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide velocityA, ref Contact4OneBodyPrestepData prestep) { PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, velocityA, ref prestep.Contact0.Depth); PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.Normal, velocityA, ref prestep.Contact1.Depth); @@ -1231,7 +1239,7 @@ public unsafe struct Contact1Projection public TwistFrictionProjection Twist; } - public struct Contact1Functions : IContactConstraintFunctions + public struct Contact1Functions : ITwoBodyConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Prestep( @@ -1280,8 +1288,10 @@ public void Solve(ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref Cont TwistFriction.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); } + public bool RequiresIncrementalSubstepUpdates => true; + [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityWide velocityA, in BodyVelocityWide velocityB, ref Contact1PrestepData prestep) + public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide velocityA, in BodyVelocityWide velocityB, ref Contact1PrestepData prestep) { PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact0.Depth); } @@ -1461,7 +1471,7 @@ public unsafe struct Contact2Projection public TwistFrictionProjection Twist; } - public struct Contact2Functions : IContactConstraintFunctions + public struct Contact2Functions : ITwoBodyConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Prestep( @@ -1515,8 +1525,10 @@ public void Solve(ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref Cont TwistFriction.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); } + public bool RequiresIncrementalSubstepUpdates => true; + [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityWide velocityA, in BodyVelocityWide velocityB, ref Contact2PrestepData prestep) + public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide velocityA, in BodyVelocityWide velocityB, ref Contact2PrestepData prestep) { PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact0.Depth); PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact1.Depth); @@ -1711,7 +1723,7 @@ public unsafe struct Contact3Projection public TwistFrictionProjection Twist; } - public struct Contact3Functions : IContactConstraintFunctions + public struct Contact3Functions : ITwoBodyConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Prestep( @@ -1771,8 +1783,10 @@ public void Solve(ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref Cont TwistFriction.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); } + public bool RequiresIncrementalSubstepUpdates => true; + [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityWide velocityA, in BodyVelocityWide velocityB, ref Contact3PrestepData prestep) + public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide velocityA, in BodyVelocityWide velocityB, ref Contact3PrestepData prestep) { PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact0.Depth); PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact1.Depth); @@ -1980,7 +1994,7 @@ public unsafe struct Contact4Projection public TwistFrictionProjection Twist; } - public struct Contact4Functions : IContactConstraintFunctions + public struct Contact4Functions : ITwoBodyConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Prestep( @@ -2046,8 +2060,10 @@ public void Solve(ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref Cont TwistFriction.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); } + public bool RequiresIncrementalSubstepUpdates => true; + [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityWide velocityA, in BodyVelocityWide velocityB, ref Contact4PrestepData prestep) + public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide velocityA, in BodyVelocityWide velocityB, ref Contact4PrestepData prestep) { PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact0.Depth); PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact1.Depth); diff --git a/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt b/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt index a2892ec98..7277801f2 100644 --- a/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt +++ b/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt @@ -235,7 +235,7 @@ for (int i = 0; i < contactCount; ++i) public TwistFrictionProjection Twist; } - public struct Contact<#=contactCount#><#=suffix#>Functions : I<#=suffix#>ContactConstraintFunctions<#=suffix#>PrestepData, Contact<#=contactCount#><#=suffix#>Projection, Contact<#=contactCount#>AccumulatedImpulses> + public struct Contact<#=contactCount#><#=suffix#>Functions : I<#=bodyCount == 1 ? "OneBody" : "TwoBody"#>ConstraintFunctions<#=suffix#>PrestepData, Contact<#=contactCount#><#=suffix#>Projection, Contact<#=contactCount#>AccumulatedImpulses> { [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Prestep( @@ -312,8 +312,10 @@ for (int i = 0; i < contactCount; ++i) TwistFriction<#=suffix#>.Solve(ref projection.Normal, ref projection.InertiaA, <#if (bodyCount == 2) {#>ref projection.InertiaB, <#}#>ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); } + public bool RequiresIncrementalSubstepUpdates => true; + [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityWide velocityA,<#if(bodyCount == 2) {#> in BodyVelocityWide velocityB,<#}#> ref Contact<#=contactCount#><#=suffix#>PrestepData prestep) + public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide velocityA,<#if(bodyCount == 2) {#> in BodyVelocityWide velocityB,<#}#> ref Contact<#=contactCount#><#=suffix#>PrestepData prestep) { <#for (int i = 0; i < contactCount; ++i) {#> PenetrationLimit<#=suffix#>.UpdatePenetrationDepth(dt, prestep.Contact<#=i#>.OffsetA, <#if(bodyCount == 2) {#>prestep.OffsetB, <#}#>prestep.Normal, velocityA, <#if (bodyCount == 2) {#>velocityB, <#}#>ref prestep.Contact<#=i#>.Depth); diff --git a/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs b/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs index c158cdeb3..8b0051e59 100644 --- a/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs +++ b/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs @@ -211,7 +211,7 @@ public interface INonconvexTwoBodyProjection where TProjection : IN } public struct ContactNonconvexOneBodyFunctions : - IOneBodyContactConstraintFunctions + IOneBodyConstraintFunctions where TPrestep : struct, INonconvexContactPrestep where TProjection : struct, INonconvexOneBodyProjection where TAccumulatedImpulses : struct @@ -337,10 +337,21 @@ public void UpdateForNewPose( { throw new System.NotImplementedException(); } + + public bool RequiresIncrementalSubstepUpdates => true; + public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, ref TPrestep prestep) + { + ref var prestepContactStart = ref prestep.GetContact(ref prestep, 0); + for (int i = 0; i < prestep.ContactCount; ++i) + { + ref var prestepContact = ref Unsafe.Add(ref prestepContactStart, i); + PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestepContact.Offset, prestepContact.Normal, wsvA, ref prestepContact.Depth); + } + } } public struct ContactNonconvexTwoBodyFunctions : - IContactConstraintFunctions + ITwoBodyConstraintFunctions where TPrestep : struct, ITwoBodyNonconvexContactPrestep where TProjection : struct, INonconvexTwoBodyProjection where TAccumulatedImpulses : struct @@ -414,17 +425,6 @@ public void Solve(ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref TPro } } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityWide velocityA, in BodyVelocityWide velocityB, ref TPrestep prestep) - { - ref var prestepOffsetB = ref prestep.GetOffsetB(ref prestep); - ref var prestepContactStart = ref prestep.GetContact(ref prestep, 0); - for (int i = 0; i < prestep.ContactCount; ++i) - { - ref var prestepContact = ref Unsafe.Add(ref prestepContactStart, i); - PenetrationLimit.UpdatePenetrationDepth(dt, prestepContact.Offset, prestepOffsetB, prestepContact.Normal, velocityA, velocityB, ref prestepContact.Depth); - } - } public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref TPrestep prestep, ref TAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { @@ -466,13 +466,17 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in } } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, - in Vector dt, in TAccumulatedImpulses accumulatedImpulses, ref TPrestep prestep) + + public bool RequiresIncrementalSubstepUpdates => true; + public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref TPrestep prestep) { - throw new System.NotImplementedException(); + ref var prestepOffsetB = ref prestep.GetOffsetB(ref prestep); + ref var prestepContactStart = ref prestep.GetContact(ref prestep, 0); + for (int i = 0; i < prestep.ContactCount; ++i) + { + ref var prestepContact = ref Unsafe.Add(ref prestepContactStart, i); + PenetrationLimit.UpdatePenetrationDepth(dt, prestepContact.Offset, prestepOffsetB, prestepContact.Normal, wsvA, wsvB, ref prestepContact.Depth); + } } } } diff --git a/BepuPhysics/Constraints/DistanceLimit.cs b/BepuPhysics/Constraints/DistanceLimit.cs index ff6178df6..340d612fa 100644 --- a/BepuPhysics/Constraints/DistanceLimit.cs +++ b/BepuPhysics/Constraints/DistanceLimit.cs @@ -228,15 +228,10 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in ApplyImpulse(direction, angularJA, angularJB, inertiaA, inertiaB, csi, ref wsvA, ref wsvB); } - - + + public bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, - in Vector dt, in Vector accumulatedImpulses, ref DistanceLimitPrestepData prestep) - { - } + public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref DistanceLimitPrestepData prestepData) { } } diff --git a/BepuPhysics/Constraints/DistanceServo.cs b/BepuPhysics/Constraints/DistanceServo.cs index 4ceea8383..e370f0959 100644 --- a/BepuPhysics/Constraints/DistanceServo.cs +++ b/BepuPhysics/Constraints/DistanceServo.cs @@ -340,13 +340,9 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in ApplyImpulse(inertiaA.InverseMass, inertiaB.InverseMass, direction, angularImpulseToVelocityA, angularImpulseToVelocityB, csi, ref wsvA, ref wsvB); } + public bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, - in Vector dt, in Vector accumulatedImpulses, ref DistanceServoPrestepData prestep) - { - } + public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref DistanceServoPrestepData prestepData) { } } diff --git a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs index 52c514e1f..6a29403e9 100644 --- a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs @@ -48,12 +48,11 @@ void Solve2( in Vector3Wide positionD, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, float dt, float inverseDt, ref TPrestepData prestep, ref TAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC, ref BodyVelocityWide wsvD); - void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, - in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in BodyVelocityWide wsvC, - in Vector3Wide positionD, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, in BodyVelocityWide wsvD, - in Vector dt, in TAccumulatedImpulse accumulatedImpulses, ref TPrestepData prestep); + /// + /// Gets whether this constraint type requires incremental updates for each substep taken beyond the first. + /// + bool RequiresIncrementalSubstepUpdates { get; } + void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, in BodyVelocityWide wsvC, in BodyVelocityWide wsvD, ref TPrestepData prestepData); } /// @@ -257,5 +256,24 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f } } + public override bool RequiresIncrementalSubstepUpdates => default(TConstraintFunctions).RequiresIncrementalSubstepUpdates; + public unsafe override void IncrementallyUpdateForSubstep(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + { + var prestepBundles = typeBatch.PrestepData.As(); + var bodyReferencesBundles = typeBatch.BodyReferences.As(); + var function = default(TConstraintFunctions); + var dtWide = new Vector(dt); + for (int i = startBundle; i < exclusiveEndBundle; ++i) + { + ref var prestep = ref prestepBundles[i]; + ref var references = ref bodyReferencesBundles[i]; + bodies.GatherState(references.IndexA, true, out _, out _, out var wsvA, out _); + bodies.GatherState(references.IndexB, true, out _, out _, out var wsvB, out _); + bodies.GatherState(references.IndexC, true, out _, out _, out var wsvC, out _); + bodies.GatherState(references.IndexD, true, out _, out _, out var wsvD, out _); + function.IncrementallyUpdateForSubstep(dtWide, wsvA, wsvB, wsvC, wsvD, ref prestep); + } + } + } } diff --git a/BepuPhysics/Constraints/Hinge.cs b/BepuPhysics/Constraints/Hinge.cs index 1ccfb2f28..4b63d9bcb 100644 --- a/BepuPhysics/Constraints/Hinge.cs +++ b/BepuPhysics/Constraints/Hinge.cs @@ -350,13 +350,9 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in ApplyImpulse(offsetA, offsetB, hingeJacobian, inertiaA, inertiaB, csi, ref wsvA, ref wsvB); } + public bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, - in Vector dt, in HingeAccumulatedImpulses accumulatedImpulses, ref HingePrestepData prestep) - { - } + public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref HingePrestepData prestepData) { } } public class HingeTypeProcessor : TwoBodyTypeProcessor diff --git a/BepuPhysics/Constraints/LinearAxisLimit.cs b/BepuPhysics/Constraints/LinearAxisLimit.cs index a7dd9d349..a3e8812ac 100644 --- a/BepuPhysics/Constraints/LinearAxisLimit.cs +++ b/BepuPhysics/Constraints/LinearAxisLimit.cs @@ -206,13 +206,9 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in LinearAxisServoFunctions.ApplyImpulse(normal, angularImpulseToVelocityA, angularImpulseToVelocityB, inertiaA, inertiaB, csi, ref wsvA, ref wsvB); } + public bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, - in Vector dt, in Vector accumulatedImpulses, ref LinearAxisLimitPrestepData prestep) - { - } + public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref LinearAxisLimitPrestepData prestepData) { } } public class LinearAxisLimitTypeProcessor : TwoBodyTypeProcessor, LinearAxisLimitFunctions, AccessAll, AccessAll, AccessAll, AccessAll> diff --git a/BepuPhysics/Constraints/LinearAxisMotor.cs b/BepuPhysics/Constraints/LinearAxisMotor.cs index 3251cdd1f..f281f8fc3 100644 --- a/BepuPhysics/Constraints/LinearAxisMotor.cs +++ b/BepuPhysics/Constraints/LinearAxisMotor.cs @@ -137,13 +137,9 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in LinearAxisServoFunctions.ApplyImpulse(normal, angularImpulseToVelocityA, angularImpulseToVelocityB, inertiaA, inertiaB, csi, ref wsvA, ref wsvB); } + public bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, - in Vector dt, in Vector accumulatedImpulses, ref LinearAxisMotorPrestepData prestep) - { - } + public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref LinearAxisMotorPrestepData prestepData) { } } public class LinearAxisMotorTypeProcessor : TwoBodyTypeProcessor, LinearAxisMotorFunctions, AccessAll, AccessAll, AccessAll, AccessAll> diff --git a/BepuPhysics/Constraints/LinearAxisServo.cs b/BepuPhysics/Constraints/LinearAxisServo.cs index 3cdadecff..624d5ad7a 100644 --- a/BepuPhysics/Constraints/LinearAxisServo.cs +++ b/BepuPhysics/Constraints/LinearAxisServo.cs @@ -293,13 +293,9 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in ApplyImpulse(normal, angularImpulseToVelocityA, angularImpulseToVelocityB, inertiaA, inertiaB, csi, ref wsvA, ref wsvB); } + public bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, - in Vector dt, in Vector accumulatedImpulses, ref LinearAxisServoPrestepData prestep) - { - } + public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref LinearAxisServoPrestepData prestepData) { } } public class LinearAxisServoTypeProcessor : TwoBodyTypeProcessor, LinearAxisServoFunctions, AccessAll, AccessAll, AccessAll, AccessAll> diff --git a/BepuPhysics/Constraints/OneBodyAngularMotor.cs b/BepuPhysics/Constraints/OneBodyAngularMotor.cs index e9c91e4fa..7d9e61a1a 100644 --- a/BepuPhysics/Constraints/OneBodyAngularMotor.cs +++ b/BepuPhysics/Constraints/OneBodyAngularMotor.cs @@ -98,7 +98,7 @@ public static void Solve(ref BodyVelocityWide velocityA, Vector3Wide.Subtract(biasImpulse, softnessComponent, out var csi); Vector3Wide.Subtract(csi, csiVelocityComponent, out csi); - ServoSettingsWide.ClampImpulse(maximumImpulse, ref accumulatedImpulse, ref csi); + ServoSettingsWide.ClampImpulse(maximumImpulse, ref accumulatedImpulse, ref csi); ApplyImpulse(ref velocityA.Angular, impulseToVelocityA, csi); } @@ -130,12 +130,9 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in ApplyImpulse(ref wsvA.Angular, inertiaA.InverseInertiaTensor, csi); } + public bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in Vector dt, in Vector3Wide accumulatedImpulses, ref OneBodyAngularMotorPrestepData prestep) - { - } + public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, ref OneBodyAngularMotorPrestepData prestepData) { } } public class OneBodyAngularMotorTypeProcessor : OneBodyTypeProcessor diff --git a/BepuPhysics/Constraints/OneBodyAngularServo.cs b/BepuPhysics/Constraints/OneBodyAngularServo.cs index b8b52856f..1e58e62a3 100644 --- a/BepuPhysics/Constraints/OneBodyAngularServo.cs +++ b/BepuPhysics/Constraints/OneBodyAngularServo.cs @@ -169,12 +169,9 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in ApplyImpulse(inertiaA.InverseInertiaTensor, csi, ref wsvA.Angular); } + public bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in Vector dt, in Vector3Wide accumulatedImpulses, ref OneBodyAngularServoPrestepData prestep) - { - } + public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, ref OneBodyAngularServoPrestepData prestepData) { } } public class OneBodyAngularServoTypeProcessor : OneBodyTypeProcessor diff --git a/BepuPhysics/Constraints/OneBodyLinearMotor.cs b/BepuPhysics/Constraints/OneBodyLinearMotor.cs index 387112a70..ae30b4bdc 100644 --- a/BepuPhysics/Constraints/OneBodyLinearMotor.cs +++ b/BepuPhysics/Constraints/OneBodyLinearMotor.cs @@ -120,13 +120,10 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in ServoSettingsWide.ClampImpulse(maximumImpulse, ref accumulatedImpulses, ref csi); OneBodyLinearServoFunctions.ApplyImpulse(offset, inertiaA, ref wsvA, csi); } - + + public bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in Vector dt, in Vector3Wide accumulatedImpulses, ref OneBodyLinearMotorPrestepData prestep) - { - } + public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, ref OneBodyLinearMotorPrestepData prestepData) { } } public class OneBodyLinearMotorTypeProcessor : OneBodyTypeProcessor { diff --git a/BepuPhysics/Constraints/OneBodyLinearServo.cs b/BepuPhysics/Constraints/OneBodyLinearServo.cs index 190d946f3..bc6847443 100644 --- a/BepuPhysics/Constraints/OneBodyLinearServo.cs +++ b/BepuPhysics/Constraints/OneBodyLinearServo.cs @@ -216,12 +216,9 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in ApplyImpulse(offset, inertiaA, ref wsvA, csi); } + public bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in Vector dt, in Vector3Wide accumulatedImpulses, ref OneBodyLinearServoPrestepData prestep) - { - } + public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, ref OneBodyLinearServoPrestepData prestepData) { } } public class OneBodyLinearServoTypeProcessor : OneBodyTypeProcessor diff --git a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs index 75e41150f..65ca9c785 100644 --- a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs @@ -25,20 +25,11 @@ void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in Bod void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref TPrestepData prestep, ref TAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA); - void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in Vector dt, in TAccumulatedImpulse accumulatedImpulses, ref TPrestepData prestep); - } - - /// - /// Prestep, warm start, solve iteration, and incremental contact update functions for a one body contact constraint type. - /// - /// Type of the prestep data used by the constraint. - /// Type of the accumulated impulses used by the constraint. - /// Type of the projection to input. - public interface IOneBodyContactConstraintFunctions : IOneBodyConstraintFunctions - { - void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityWide velocity, ref TPrestepData prestepData); + /// + /// Gets whether this constraint type requires incremental updates for each substep taken beyond the first. + /// + bool RequiresIncrementalSubstepUpdates { get; } + void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide velocity, ref TPrestepData prestepData); } //Not a big fan of complex generic-filled inheritance hierarchies, but this is the shortest evolutionary step to removing duplicates. @@ -198,27 +189,30 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f bodies.ScatterVelocities(ref wsvA, ref references); } } - } - public abstract class OneBodyContactTypeProcessor - : OneBodyTypeProcessor - where TPrestepData : unmanaged where TProjection : unmanaged where TAccumulatedImpulse : unmanaged - where TConstraintFunctions : unmanaged, IOneBodyContactConstraintFunctions - { - public unsafe override void IncrementallyUpdateContactData(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + public override bool RequiresIncrementalSubstepUpdates => default(TConstraintFunctions).RequiresIncrementalSubstepUpdates; + public unsafe override void IncrementallyUpdateForSubstep(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { - ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); - ref var bodyReferencesBase = ref Unsafe.AsRef>(typeBatch.BodyReferences.Memory); + var prestepBundles = typeBatch.PrestepData.As(); + var bodyReferencesBundles = typeBatch.BodyReferences.As>(); var function = default(TConstraintFunctions); var dtWide = new Vector(dt); for (int i = startBundle; i < exclusiveEndBundle; ++i) { - ref var prestep = ref Unsafe.Add(ref prestepBase, i); - ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); + ref var prestep = ref prestepBundles[i]; + ref var references = ref bodyReferencesBundles[i]; bodies.GatherState(references, true, out _, out _, out var wsvA, out _); - function.IncrementallyUpdateContactData(dtWide, wsvA, ref prestep); + function.IncrementallyUpdateForSubstep(dtWide, wsvA, ref prestep); } } } + public abstract class OneBodyContactTypeProcessor + : OneBodyTypeProcessor + where TPrestepData : unmanaged where TProjection : unmanaged where TAccumulatedImpulse : unmanaged + where TConstraintFunctions : unmanaged, IOneBodyConstraintFunctions + { + + } + } diff --git a/BepuPhysics/Constraints/PointOnLineServo.cs b/BepuPhysics/Constraints/PointOnLineServo.cs index 02af61e62..44578c86c 100644 --- a/BepuPhysics/Constraints/PointOnLineServo.cs +++ b/BepuPhysics/Constraints/PointOnLineServo.cs @@ -308,13 +308,9 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in ApplyImpulse(ref wsvA, ref wsvB, linearJacobian, angularJA, angularJB, inertiaA, inertiaB, ref csi); } + public bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, - in Vector dt, in Vector2Wide accumulatedImpulses, ref PointOnLineServoPrestepData prestep) - { - } + public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref PointOnLineServoPrestepData prestepData) { } } public class PointOnLineServoTypeProcessor : TwoBodyTypeProcessor diff --git a/BepuPhysics/Constraints/SwingLimit.cs b/BepuPhysics/Constraints/SwingLimit.cs index ec9f5fd1a..743c0c0ac 100644 --- a/BepuPhysics/Constraints/SwingLimit.cs +++ b/BepuPhysics/Constraints/SwingLimit.cs @@ -252,13 +252,9 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in ApplyImpulse(impulseToVelocityA, negatedImpulseToVelocityB, csi, ref wsvA.Angular, ref wsvB.Angular); } + public bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, - in Vector dt, in Vector accumulatedImpulses, ref SwingLimitPrestepData prestep) - { - } + public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref SwingLimitPrestepData prestepData) { } } public class SwingLimitTypeProcessor : TwoBodyTypeProcessor, SwingLimitFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular> diff --git a/BepuPhysics/Constraints/SwivelHinge.cs b/BepuPhysics/Constraints/SwivelHinge.cs index ba84ddd94..d705d068d 100644 --- a/BepuPhysics/Constraints/SwivelHinge.cs +++ b/BepuPhysics/Constraints/SwivelHinge.cs @@ -331,13 +331,9 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in ApplyImpulse(offsetA, offsetB, swivelHingeJacobian, inertiaA, inertiaB, ref csi, ref wsvA, ref wsvB); } + public bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, - in Vector dt, in Vector4Wide accumulatedImpulses, ref SwivelHingePrestepData prestep) - { - } + public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref SwivelHingePrestepData prestepData) { } } public class SwivelHingeTypeProcessor : TwoBodyTypeProcessor diff --git a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs index c2312a3b3..0bf5878ab 100644 --- a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs @@ -42,11 +42,11 @@ void Solve2( in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, float dt, float inverseDt, ref TPrestepData prestep, ref TAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC); - void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, - in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in BodyVelocityWide wsvC, - in Vector dt, in TAccumulatedImpulse accumulatedImpulses, ref TPrestepData prestep); + /// + /// Gets whether this constraint type requires incremental updates for each substep taken beyond the first. + /// + bool RequiresIncrementalSubstepUpdates { get; } + void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, in BodyVelocityWide wsvC, ref TPrestepData prestepData); } /// @@ -235,5 +235,23 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f bodies.ScatterVelocities(ref wsvC, ref references.IndexC); } } + + public override bool RequiresIncrementalSubstepUpdates => default(TConstraintFunctions).RequiresIncrementalSubstepUpdates; + public unsafe override void IncrementallyUpdateForSubstep(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + { + var prestepBundles = typeBatch.PrestepData.As(); + var bodyReferencesBundles = typeBatch.BodyReferences.As(); + var function = default(TConstraintFunctions); + var dtWide = new Vector(dt); + for (int i = startBundle; i < exclusiveEndBundle; ++i) + { + ref var prestep = ref prestepBundles[i]; + ref var references = ref bodyReferencesBundles[i]; + bodies.GatherState(references.IndexA, true, out _, out _, out var wsvA, out _); + bodies.GatherState(references.IndexB, true, out _, out _, out var wsvB, out _); + bodies.GatherState(references.IndexC, true, out _, out _, out var wsvC, out _); + function.IncrementallyUpdateForSubstep(dtWide, wsvA, wsvB, wsvC, ref prestep); + } + } } } diff --git a/BepuPhysics/Constraints/TwistLimit.cs b/BepuPhysics/Constraints/TwistLimit.cs index e7a41fcec..babe9a1eb 100644 --- a/BepuPhysics/Constraints/TwistLimit.cs +++ b/BepuPhysics/Constraints/TwistLimit.cs @@ -191,13 +191,9 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in TwistServoFunctions.ApplyImpulse(ref wsvA.Angular, ref wsvB.Angular, impulseToVelocityA, negatedImpulseToVelocityB, csi); } + public bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, - in Vector dt, in Vector accumulatedImpulses, ref TwistLimitPrestepData prestep) - { - } + public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref TwistLimitPrestepData prestepData) { } } public class TwistLimitTypeProcessor : TwoBodyTypeProcessor, TwistLimitFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular> diff --git a/BepuPhysics/Constraints/TwistMotor.cs b/BepuPhysics/Constraints/TwistMotor.cs index 3e68e2274..159a5d74b 100644 --- a/BepuPhysics/Constraints/TwistMotor.cs +++ b/BepuPhysics/Constraints/TwistMotor.cs @@ -175,13 +175,9 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in TwistServoFunctions.ApplyImpulse(ref wsvA.Angular, ref wsvB.Angular, impulseToVelocityA, negatedImpulseToVelocityB, csi); } + public bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, - in Vector dt, in Vector accumulatedImpulses, ref TwistMotorPrestepData prestep) - { - } + public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref TwistMotorPrestepData prestepData) { } } public class TwistMotorTypeProcessor : TwoBodyTypeProcessor, TwistMotorFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular> diff --git a/BepuPhysics/Constraints/TwistServo.cs b/BepuPhysics/Constraints/TwistServo.cs index 8869e0feb..16d43e5e9 100644 --- a/BepuPhysics/Constraints/TwistServo.cs +++ b/BepuPhysics/Constraints/TwistServo.cs @@ -266,13 +266,9 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in ApplyImpulse(ref wsvA.Angular, ref wsvB.Angular, impulseToVelocityA, negatedImpulseToVelocityB, csi); } + public bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, - in Vector dt, in Vector accumulatedImpulses, ref TwistServoPrestepData prestep) - { - } + public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref TwistServoPrestepData prestepData) { } } public class TwistServoTypeProcessor : TwoBodyTypeProcessor, TwistServoFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular> diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 0f7c2808d..efcf1bfd9 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -38,21 +38,11 @@ void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in Bod void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref TPrestepData prestep, ref TAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB); - void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, - in Vector dt, in TAccumulatedImpulse accumulatedImpulses, ref TPrestepData prestep); - } - - /// - /// Prestep, warm start, solve iteration, and incremental contact update functions for a two body contact constraint type. - /// - /// Type of the prestep data used by the constraint. - /// Type of the accumulated impulses used by the constraint. - /// Type of the projection to input. - public interface IContactConstraintFunctions : ITwoBodyConstraintFunctions - { - void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityWide velocityA, in BodyVelocityWide velocityB, ref TPrestepData prestepData); + /// + /// Gets whether this constraint type requires incremental updates for each substep taken beyond the first. + /// + bool RequiresIncrementalSubstepUpdates { get; } + void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref TPrestepData prestepData); } //Not a big fan of complex generic-filled inheritance hierarchies, but this is the shortest evolutionary step to removing duplicates. @@ -264,7 +254,7 @@ public unsafe override void WarmStart2(dt), accumulatedImpulses, ref prestep); - + function.WarmStart2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB); if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) @@ -306,27 +296,29 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f bodies.ScatterVelocities(ref wsvB, ref references.IndexB); } } - } - public abstract class TwoBodyContactTypeProcessor - : TwoBodyTypeProcessor - where TPrestepData : unmanaged where TProjection : unmanaged where TAccumulatedImpulse : unmanaged - where TConstraintFunctions : unmanaged, IContactConstraintFunctions - { - public unsafe override void IncrementallyUpdateContactData(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + public override bool RequiresIncrementalSubstepUpdates => default(TConstraintFunctions).RequiresIncrementalSubstepUpdates; + public unsafe override void IncrementallyUpdateForSubstep(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { - ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); - ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); + var prestepBundles = typeBatch.PrestepData.As(); + var bodyReferencesBundles = typeBatch.BodyReferences.As(); var function = default(TConstraintFunctions); var dtWide = new Vector(dt); for (int i = startBundle; i < exclusiveEndBundle; ++i) { - ref var prestep = ref Unsafe.Add(ref prestepBase, i); - ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); + ref var prestep = ref prestepBundles[i]; + ref var references = ref bodyReferencesBundles[i]; bodies.GatherState(references.IndexA, true, out _, out _, out var wsvA, out _); bodies.GatherState(references.IndexB, true, out _, out _, out var wsvB, out _); - function.IncrementallyUpdateContactData(dtWide, wsvA, wsvB, ref prestep); + function.IncrementallyUpdateForSubstep(dtWide, wsvA, wsvB, ref prestep); } } } + + public abstract class TwoBodyContactTypeProcessor + : TwoBodyTypeProcessor + where TPrestepData : unmanaged where TProjection : unmanaged where TAccumulatedImpulse : unmanaged + where TConstraintFunctions : unmanaged, ITwoBodyConstraintFunctions + { + } } diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index 9cd726156..1da982b60 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -213,9 +213,13 @@ public abstract void WarmStart2 + /// Gets whether this type requires incremental updates for each substep in a frame beyond the first. + /// + public abstract bool RequiresIncrementalSubstepUpdates { get; } + public virtual void IncrementallyUpdateForSubstep(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int end) { - Debug.Fail("A contact data update was scheduled for a type batch that does not have a contact data update implementation."); + Debug.Fail("An incremental update was scheduled for a type batch that does not have a contact data update implementation."); } [MethodImpl(MethodImplOptions.AggressiveInlining)] diff --git a/BepuPhysics/Constraints/VolumeConstraint.cs b/BepuPhysics/Constraints/VolumeConstraint.cs index 484a73a17..9ea44400d 100644 --- a/BepuPhysics/Constraints/VolumeConstraint.cs +++ b/BepuPhysics/Constraints/VolumeConstraint.cs @@ -304,15 +304,9 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in ApplyImpulse(inertiaA.InverseMass, inertiaB.InverseMass, inertiaC.InverseMass, inertiaD.InverseMass, negatedJA, jacobianB, jacobianC, jacobianD, csi, ref wsvA, ref wsvB, ref wsvC, ref wsvD); } + public bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, - in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in BodyVelocityWide wsvC, - in Vector3Wide positionD, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, in BodyVelocityWide wsvD, - in Vector dt, in Vector accumulatedImpulses, ref VolumeConstraintPrestepData prestep) - { - } + public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, in BodyVelocityWide wsvC, in BodyVelocityWide wsvD, ref VolumeConstraintPrestepData prestepData) { } } diff --git a/BepuPhysics/Constraints/Weld.cs b/BepuPhysics/Constraints/Weld.cs index 84bb45e20..8a5bf993d 100644 --- a/BepuPhysics/Constraints/Weld.cs +++ b/BepuPhysics/Constraints/Weld.cs @@ -93,6 +93,7 @@ public struct WeldAccumulatedImpulses public struct WeldFunctions : ITwoBodyConstraintFunctions { + [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref WeldPrestepData prestep, out WeldProjection projection) @@ -356,14 +357,10 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in ApplyImpulse(inertiaA, inertiaB, offset, orientationCSI, offsetCSI, ref wsvA, ref wsvB); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, - in Vector dt, in WeldAccumulatedImpulses accumulatedImpulses, ref WeldPrestepData prestep) - { - } + public bool RequiresIncrementalSubstepUpdates => false; + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref WeldPrestepData prestepData) { } } diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index 99485a5df..e458ac6c8 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -229,9 +229,6 @@ public int CountConstraints() return count; } - - Action solveWorker; - Action incrementalContactUpdateWorker; protected Solver(Bodies bodies, BufferPool pool, SolveDescription solveDescription, int initialCapacity, int initialIslandCapacity, diff --git a/BepuPhysics/Solver_Solve.cs b/BepuPhysics/Solver_Solve.cs index 95d275e17..fe57b9317 100644 --- a/BepuPhysics/Solver_Solve.cs +++ b/BepuPhysics/Solver_Solve.cs @@ -271,7 +271,7 @@ public void Execute(Solver solver, int blockIndex, int workerIndex) { ref var block = ref this.solver.substepContext.IncrementalUpdateBlocks[blockIndex]; ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; - solver.TypeProcessors[typeBatch.TypeId].IncrementallyUpdateContactData(ref typeBatch, solver.bodies, Dt, InverseDt, block.StartBundle, block.End); + solver.TypeProcessors[typeBatch.TypeId].IncrementallyUpdateForSubstep(ref typeBatch, solver.bodies, Dt, InverseDt, block.StartBundle, block.End); } } @@ -1425,7 +1425,7 @@ public override void Solve(float totalDt, IThreadDispatcher threadDispatcher = n { ref var typeBatch = ref batch.TypeBatches[j]; if (incrementalUpdateFilter.AllowType(typeBatch.TypeId)) - TypeProcessors[typeBatch.TypeId].IncrementallyUpdateContactData(ref typeBatch, bodies, substepDt, inverseDt, 0, typeBatch.BundleCount); + TypeProcessors[typeBatch.TypeId].IncrementallyUpdateForSubstep(ref typeBatch, bodies, substepDt, inverseDt, 0, typeBatch.BundleCount); } } PoseIntegrator.IntegrateKinematicPosesAndVelocities(ConstrainedKinematicHandles.Span.Slice(ConstrainedKinematicHandles.Count), 0, BundleIndexing.GetBundleCount(ConstrainedKinematicHandles.Count), substepDt, 0); diff --git a/Demos/Demos/Characters/CharacterMotionConstraint.cs b/Demos/Demos/Characters/CharacterMotionConstraint.cs index d5fc98b25..973538f44 100644 --- a/Demos/Demos/Characters/CharacterMotionConstraint.cs +++ b/Demos/Demos/Characters/CharacterMotionConstraint.cs @@ -382,11 +382,11 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in ApplyVerticalImpulse(basis, verticalAngularJacobianA, verticalCorrectiveImpulse, inertiaA, ref velocityA); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide velocityA, - in Vector dt, in CharacterMotionAccumulatedImpulse accumulatedImpulses, ref StaticCharacterMotionPrestep prestep) + + public bool RequiresIncrementalSubstepUpdates => true; + public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide velocityA, ref StaticCharacterMotionPrestep prestep) { + } } @@ -808,12 +808,11 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in ApplyVerticalImpulse(basis, verticalAngularJacobianA, verticalAngularJacobianB, verticalCorrectiveImpulse, inertiaA, inertiaB, ref velocityA, ref velocityB); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide velocityA, - in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide velocityB, - in Vector dt, in CharacterMotionAccumulatedImpulse accumulatedImpulses, ref DynamicCharacterMotionPrestep prestep) + + public bool RequiresIncrementalSubstepUpdates => true; + public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide velocityA, in BodyVelocityWide velocityB, ref DynamicCharacterMotionPrestep prestep) { + } } diff --git a/Demos/Demos/Characters/CharacterMotionConstraint.tt b/Demos/Demos/Characters/CharacterMotionConstraint.tt index 676612c34..605e87d13 100644 --- a/Demos/Demos/Characters/CharacterMotionConstraint.tt +++ b/Demos/Demos/Characters/CharacterMotionConstraint.tt @@ -493,14 +493,11 @@ namespace Demos.Demos.Characters ApplyVerticalImpulse(basis, verticalAngularJacobianA, <#if (dynamic) {#>verticalAngularJacobianB, <#}#>verticalCorrectiveImpulse, inertiaA, <#if (dynamic) {#>inertiaB, <#}#>ref velocityA<#if (dynamic) {#>, ref velocityB<#}#>); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide velocityA, -<#if(dynamic){#> - in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide velocityB, -<#}#> - in Vector dt, in CharacterMotionAccumulatedImpulse accumulatedImpulses, ref <#=prefix#>CharacterMotionPrestep prestep) + + public bool RequiresIncrementalSubstepUpdates => true; + public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide velocityA, <#if(dynamic){#>in BodyVelocityWide velocityB, <#}#>ref <#=prefix#>CharacterMotionPrestep prestep) { + } } diff --git a/Demos/Demos/SubsteppingDemo.cs b/Demos/Demos/SubsteppingDemo.cs index ab5ebb09e..e925e10e2 100644 --- a/Demos/Demos/SubsteppingDemo.cs +++ b/Demos/Demos/SubsteppingDemo.cs @@ -24,7 +24,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0; Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(120, 120), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 2f }, - new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 8); + new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), (8, 8)); rolloverInfo = new RolloverInfo(); { From 0caea6dab579acad1b64184cc02417fc9d8ed077 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 11 Dec 2021 17:04:06 -0600 Subject: [PATCH 322/947] Solver now calls incremental update for types that request incemental updates. CharacterMotionConstraint updated accordingly. --- BepuPhysics/Solver_Solve.cs | 15 +++++----- .../Characters/CharacterMotionConstraint.cs | 29 +++++++++++++++++-- .../Characters/CharacterMotionConstraint.tt | 18 ++++++++++++ 3 files changed, 53 insertions(+), 9 deletions(-) diff --git a/BepuPhysics/Solver_Solve.cs b/BepuPhysics/Solver_Solve.cs index fe57b9317..fd4a8d218 100644 --- a/BepuPhysics/Solver_Solve.cs +++ b/BepuPhysics/Solver_Solve.cs @@ -155,14 +155,15 @@ protected interface ITypeBatchSolveFilter bool IncludeFallbackBatchForWorkBlocks { get; } bool AllowType(int typeId); } - protected struct IncrementalContactDataUpdateFilter : ITypeBatchSolveFilter + protected struct IncrementalUpdateForSubstepFilter : ITypeBatchSolveFilter { + public TypeProcessor[] TypeProcessors; public bool IncludeFallbackBatchForWorkBlocks { get { return true; } } [MethodImpl(MethodImplOptions.AggressiveInlining)] public bool AllowType(int typeId) { - return NarrowPhase.IsContactConstraintType(typeId); + return TypeProcessors[typeId].RequiresIncrementalSubstepUpdates; } } protected struct MainSolveFilter : ITypeBatchSolveFilter @@ -786,9 +787,9 @@ protected void ExecuteMultithreaded(float dt, IThreadDispatcher threadDispatcher var targetBlocksPerBatch = workerCount * targetBlocksPerBatchPerWorker; var mainFilter = new MainSolveFilter(); - var incrementalFilter = new IncrementalContactDataUpdateFilter(); + var incrementalUpdateFilter = new IncrementalUpdateForSubstepFilter { TypeProcessors = TypeProcessors }; BuildWorkBlocks(pool, minimumBlockSizeInBundles, maximumBlockSizeInBundles, targetBlocksPerBatch, ref mainFilter, out var constraintBlocks, out substepContext.ConstraintBatchBoundaries); - BuildWorkBlocks(pool, minimumBlockSizeInBundles, maximumBlockSizeInBundles, targetBlocksPerBatch, ref incrementalFilter, out var incrementalBlocks, out var incrementalUpdateBatchBoundaries); + BuildWorkBlocks(pool, minimumBlockSizeInBundles, maximumBlockSizeInBundles, targetBlocksPerBatch, ref incrementalUpdateFilter, out var incrementalBlocks, out var incrementalUpdateBatchBoundaries); pool.Return(ref incrementalUpdateBatchBoundaries); //TODO: No need to create this in the first place. Doesn't really cost anything, but... substepContext.ConstraintBlocks = constraintBlocks.Span.Slice(constraintBlocks.Count); substepContext.IncrementalUpdateBlocks = incrementalBlocks.Span.Slice(incrementalBlocks.Count); @@ -1412,7 +1413,6 @@ public override void Solve(float totalDt, IThreadDispatcher threadDispatcher = n var inverseDt = 1f / substepDt; ref var activeSet = ref ActiveSet; var batchCount = activeSet.Batches.Count; - var incrementalUpdateFilter = default(IncrementalContactDataUpdateFilter); for (int substepIndex = 0; substepIndex < substepCount; ++substepIndex) { OnSubstepStarted(substepIndex); @@ -1424,8 +1424,9 @@ public override void Solve(float totalDt, IThreadDispatcher threadDispatcher = n for (int j = 0; j < batch.TypeBatches.Count; ++j) { ref var typeBatch = ref batch.TypeBatches[j]; - if (incrementalUpdateFilter.AllowType(typeBatch.TypeId)) - TypeProcessors[typeBatch.TypeId].IncrementallyUpdateForSubstep(ref typeBatch, bodies, substepDt, inverseDt, 0, typeBatch.BundleCount); + var processor = TypeProcessors[typeBatch.TypeId]; + if (processor.RequiresIncrementalSubstepUpdates) + processor.IncrementallyUpdateForSubstep(ref typeBatch, bodies, substepDt, inverseDt, 0, typeBatch.BundleCount); } } PoseIntegrator.IntegrateKinematicPosesAndVelocities(ConstrainedKinematicHandles.Span.Slice(ConstrainedKinematicHandles.Count), 0, BundleIndexing.GetBundleCount(ConstrainedKinematicHandles.Count), substepDt, 0); diff --git a/Demos/Demos/Characters/CharacterMotionConstraint.cs b/Demos/Demos/Characters/CharacterMotionConstraint.cs index 973538f44..2910216d1 100644 --- a/Demos/Demos/Characters/CharacterMotionConstraint.cs +++ b/Demos/Demos/Characters/CharacterMotionConstraint.cs @@ -386,7 +386,18 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in public bool RequiresIncrementalSubstepUpdates => true; public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide velocityA, ref StaticCharacterMotionPrestep prestep) { - + //Since collision detection doesn't run for every substep, we approximate the change in depth for the vertical motion constraint by integrating the velocity along the support normal. + //This is pretty subtle. If you disable it entirely (return false from "RequiresIncrementalSubstepUpdates" above), you might not even notice. + //If you disable the vertical motion constraint, then it can definitely be disabled. + + //Any movement of the character or its support along N results in a change in the vertical motion constraint's perception of depth. + //estimatedPenetrationDepthChange = dot(normal, velocityDtA.Linear + velocityDtA.Angular x contactOffsetA) - dot(normal, velocityDtB.Linear + velocityDtB.Angular x contactOffsetB) + Vector3Wide.CrossWithoutOverlap(velocityA.Angular, prestep.OffsetFromCharacter, out var wxra); + Vector3Wide.Add(wxra, velocityA.Linear, out var contactVelocityA); + + var normal = QuaternionWide.TransformUnitY(prestep.SurfaceBasis); + Vector3Wide.Dot(normal, contactVelocityA, out var estimatedDepthChangeVelocity); + prestep.Depth -= estimatedDepthChangeVelocity * dt; } } @@ -812,7 +823,21 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in public bool RequiresIncrementalSubstepUpdates => true; public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide velocityA, in BodyVelocityWide velocityB, ref DynamicCharacterMotionPrestep prestep) { - + //Since collision detection doesn't run for every substep, we approximate the change in depth for the vertical motion constraint by integrating the velocity along the support normal. + //This is pretty subtle. If you disable it entirely (return false from "RequiresIncrementalSubstepUpdates" above), you might not even notice. + //If you disable the vertical motion constraint, then it can definitely be disabled. + + //Any movement of the character or its support along N results in a change in the vertical motion constraint's perception of depth. + //estimatedPenetrationDepthChange = dot(normal, velocityDtA.Linear + velocityDtA.Angular x contactOffsetA) - dot(normal, velocityDtB.Linear + velocityDtB.Angular x contactOffsetB) + Vector3Wide.CrossWithoutOverlap(velocityA.Angular, prestep.OffsetFromCharacter, out var wxra); + Vector3Wide.Add(wxra, velocityA.Linear, out var contactVelocityA); + + var normal = QuaternionWide.TransformUnitY(prestep.SurfaceBasis); + Vector3Wide.CrossWithoutOverlap(velocityB.Angular, prestep.OffsetFromSupport, out var wxrb); + Vector3Wide.Add(wxrb, velocityB.Linear, out var contactVelocityB); + Vector3Wide.Subtract(contactVelocityA, contactVelocityB, out var contactVelocityDifference); + Vector3Wide.Dot(normal, contactVelocityDifference, out var estimatedDepthChangeVelocity); + prestep.Depth -= estimatedDepthChangeVelocity * dt; } } diff --git a/Demos/Demos/Characters/CharacterMotionConstraint.tt b/Demos/Demos/Characters/CharacterMotionConstraint.tt index 605e87d13..9f21c68d5 100644 --- a/Demos/Demos/Characters/CharacterMotionConstraint.tt +++ b/Demos/Demos/Characters/CharacterMotionConstraint.tt @@ -497,7 +497,25 @@ namespace Demos.Demos.Characters public bool RequiresIncrementalSubstepUpdates => true; public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide velocityA, <#if(dynamic){#>in BodyVelocityWide velocityB, <#}#>ref <#=prefix#>CharacterMotionPrestep prestep) { + //Since collision detection doesn't run for every substep, we approximate the change in depth for the vertical motion constraint by integrating the velocity along the support normal. + //This is pretty subtle. If you disable it entirely (return false from "RequiresIncrementalSubstepUpdates" above), you might not even notice. + //If you disable the vertical motion constraint, then it can definitely be disabled. + //Any movement of the character or its support along N results in a change in the vertical motion constraint's perception of depth. + //estimatedPenetrationDepthChange = dot(normal, velocityDtA.Linear + velocityDtA.Angular x contactOffsetA) - dot(normal, velocityDtB.Linear + velocityDtB.Angular x contactOffsetB) + Vector3Wide.CrossWithoutOverlap(velocityA.Angular, prestep.OffsetFromCharacter, out var wxra); + Vector3Wide.Add(wxra, velocityA.Linear, out var contactVelocityA); + + var normal = QuaternionWide.TransformUnitY(prestep.SurfaceBasis); +<#if (dynamic) {#> + Vector3Wide.CrossWithoutOverlap(velocityB.Angular, prestep.OffsetFromSupport, out var wxrb); + Vector3Wide.Add(wxrb, velocityB.Linear, out var contactVelocityB); + Vector3Wide.Subtract(contactVelocityA, contactVelocityB, out var contactVelocityDifference); + Vector3Wide.Dot(normal, contactVelocityDifference, out var estimatedDepthChangeVelocity); +<#} else {#> + Vector3Wide.Dot(normal, contactVelocityA, out var estimatedDepthChangeVelocity); +<#}#> + prestep.Depth -= estimatedDepthChangeVelocity * dt; } } From cb3e7c4036b54dde8f1677e9c4a4f851caf8d544 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 11 Dec 2021 17:13:14 -0600 Subject: [PATCH 323/947] Fixed incorrect comment. --- BepuPhysics/PoseIntegrator.cs | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/BepuPhysics/PoseIntegrator.cs b/BepuPhysics/PoseIntegrator.cs index e65b5814c..f3e13f486 100644 --- a/BepuPhysics/PoseIntegrator.cs +++ b/BepuPhysics/PoseIntegrator.cs @@ -263,14 +263,8 @@ public static unsafe void Integrate(in RigidPose pose, in BodyVelocity velocity, /// - /// Integrates the velocity of mobile bodies over time into changes in position and orientation. Also applies gravitational acceleration to dynamic bodies. + /// Handles body integration work that isn't bundled into the solver's execution. Predicts bounding boxes, integrates velocity and poses for unconstrained bodies, and does final post-substepping pose integration for constrained bodies. /// - /// - /// This variant of the integrator uses a single global gravity. Other integrators that provide per-entity gravity could exist later. - /// This integrator also assumes that the bodies positions are stored in terms of single precision floats. Later on, we will likely modify the Bodies - /// storage to allow different representations for larger simulations. That will require changes in this integrator, the relative position calculation of collision detection, - /// the bounding box calculation, and potentially even in the broadphase in extreme cases (64 bit per component positions). - /// public class PoseIntegrator : IPoseIntegrator where TCallbacks : IPoseIntegratorCallbacks { Bodies bodies; From 755cd88597980c28a103ad8cd598f344edfb59d5 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 11 Dec 2021 17:29:46 -0600 Subject: [PATCH 324/947] Fixed/improved integrate velocity comment. --- Demos/DemoCallbacks.cs | 8 ++++++-- Demos/Demos/SimpleSelfContainedDemo.cs | 5 +++++ 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/Demos/DemoCallbacks.cs b/Demos/DemoCallbacks.cs index 0ada9262b..3c0749966 100644 --- a/Demos/DemoCallbacks.cs +++ b/Demos/DemoCallbacks.cs @@ -99,8 +99,12 @@ public void PrepareForIntegration(float dt) /// Velocity of bodies in the bundle. Any changes to lanes which are not active by the integrationMask will be discarded. public void IntegrateVelocity(Vector bodyIndices, Vector3Wide position, QuaternionWide orientation, BodyInertiaWide localInertia, Vector integrationMask, int workerIndex, Vector dt, ref BodyVelocityWide velocity) { - //This is also a handy spot to implement things like position dependent gravity or per-body damping. Here, - //Note that we don't have to check for kinematics; IntegrateVelocityForKinematics returns false, so we'll never see them in this callback. + //This is a handy spot to implement things like position dependent gravity or per-body damping. + //This implementation uses a single damping value for all bodies that allows it to be precomputed. + //We don't have to check for kinematics; IntegrateVelocityForKinematics returns false, so we'll never see them in this callback. + //Note that these are SIMD operations and "Wide" types. There are Vector.Count lanes of execution being evaluated simultaneously. + //The types are laid out in array-of-structures-of-arrays (AOSOA) format. That's because this function is frequently called from vectorized contexts within the solver. + //Transforming to "array of structures" (AOS) format for the callback and then back to AOSOA would involve a lot of overhead, so instead the callback works on the AOSOA representation directly. velocity.Linear = (velocity.Linear + gravityWideDt) * linearDampingDt; velocity.Angular = velocity.Angular * angularDampingDt; } diff --git a/Demos/Demos/SimpleSelfContainedDemo.cs b/Demos/Demos/SimpleSelfContainedDemo.cs index 6cf49cc6c..5ef6004bc 100644 --- a/Demos/Demos/SimpleSelfContainedDemo.cs +++ b/Demos/Demos/SimpleSelfContainedDemo.cs @@ -197,6 +197,11 @@ public void PrepareForIntegration(float dt) /// Velocity of bodies in the bundle. Any changes to lanes which are not active by the integrationMask will be discarded. public void IntegrateVelocity(Vector bodyIndices, Vector3Wide position, QuaternionWide orientation, BodyInertiaWide localInertia, Vector integrationMask, int workerIndex, Vector dt, ref BodyVelocityWide velocity) { + //This also is a handy spot to implement things like position dependent gravity or per-body damping. + //We don't have to check for kinematics; IntegrateVelocityForKinematics returns false in this type, so we'll never see them in this callback. + //Note that these are SIMD operations and "Wide" types. There are Vector.Count lanes of execution being evaluated simultaneously. + //The types are laid out in array-of-structures-of-arrays (AOSOA) format. That's because this function is frequently called from vectorized contexts within the solver. + //Transforming to "array of structures" (AOS) format for the callback and then back to AOSOA would involve a lot of overhead, so instead the callback works on the AOSOA representation directly. velocity.Linear += gravityWideDt; } From 1bb21321edd286cd8768274fcbf3851f7d2289fd Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 12 Dec 2021 17:11:08 -0600 Subject: [PATCH 325/947] Fixed capsule-cylinder inactivation bug. --- .../CollisionTasks/CapsuleCylinderTester.cs | 2 +- Demos/SpecializedTests/ShapePileTestDemo.cs | 2 +- Demos/SpecializedTests/TriangleTestDemo.cs | 7 ++++--- 3 files changed, 6 insertions(+), 5 deletions(-) diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleCylinderTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleCylinderTester.cs index bc3f5d6bd..882342957 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleCylinderTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleCylinderTester.cs @@ -168,7 +168,7 @@ public void Test(ref CapsuleWide a, ref CylinderWide b, ref Vector specul Vector3Wide.Scale(localNormal, Vector.One / distanceFromCylinderToLineSegment, out localNormal); var depth = Vector.ConditionalSelect(internalLineSegmentIntersected, new Vector(float.MaxValue), -distanceFromCylinderToLineSegment); var negativeMargin = -speculativeMargin; - inactiveLanes = Vector.BitwiseOr(Vector.LessThan(depth, negativeMargin), inactiveLanes); + inactiveLanes = Vector.BitwiseOr(Vector.LessThan(depth + a.Radius, negativeMargin), inactiveLanes); if (Vector.LessThanAny(Vector.AndNot(internalLineSegmentIntersected, inactiveLanes), Vector.Zero)) { //At least one lane is intersecting deeply, so we need to examine the other possible normals. diff --git a/Demos/SpecializedTests/ShapePileTestDemo.cs b/Demos/SpecializedTests/ShapePileTestDemo.cs index 0f498397a..032502bee 100644 --- a/Demos/SpecializedTests/ShapePileTestDemo.cs +++ b/Demos/SpecializedTests/ShapePileTestDemo.cs @@ -65,7 +65,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) for (int k = 0; k < length; ++k) { var location = new Vector3(6, 3, 6) * new Vector3(i, j, k) + new Vector3(-width * 1.5f, 5.5f, -length * 1.5f); - var bodyDescription = BodyDescription.CreateKinematic(location, default, 0.01f); + var bodyDescription = BodyDescription.CreateKinematic(location, new(default, ContinuousDetection.Passive), 0.01f); var index = shapeCount++; switch (index % 5) { diff --git a/Demos/SpecializedTests/TriangleTestDemo.cs b/Demos/SpecializedTests/TriangleTestDemo.cs index 6702b1dc0..3a1a9c0f6 100644 --- a/Demos/SpecializedTests/TriangleTestDemo.cs +++ b/Demos/SpecializedTests/TriangleTestDemo.cs @@ -91,7 +91,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0; Simulation = Simulation.Create(BufferPool, - new DemoNarrowPhaseCallbacks() { ContactSpringiness = new BepuPhysics.Constraints.SpringSettings(30, 1), FrictionCoefficient = 1, MaximumRecoveryVelocity = 1 }, + new DemoNarrowPhaseCallbacks() { ContactSpringiness = new BepuPhysics.Constraints.SpringSettings(30, 1), FrictionCoefficient = 1, MaximumRecoveryVelocity = 5 }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); //var triangleDescription = new StaticDescription @@ -132,12 +132,13 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Simulation.Statics.Add(new StaticDescription(new Vector3(10, -2, 30), Simulation.Shapes.Add(new Box(10, 5, 10)))); Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 2, 0), new BodyInertia { InverseMass = 1 }, new(Simulation.Shapes.Add(new Sphere(1.75f)), ContinuousDetection.Discrete(0.1f, 0.1f)), -1)); - Simulation.Bodies.Add(BodyDescription.CreateDynamic((new Vector3(20, 2, 3), Quaternion.CreateFromYawPitchRoll(0f, 1.745329E-05f, 0f)), new BodyInertia { InverseMass = 1 }, new(Simulation.Shapes.Add(new Capsule(1, 2)), ContinuousDetection.Discrete(0.1f, 0.1f)), -1)); + var capsule = new Capsule(2, 2); + Simulation.Bodies.Add(BodyDescription.CreateDynamic((new Vector3(20, 2, 3), Quaternion.CreateFromYawPitchRoll(0f, 1.745329E-05f, 0f)), capsule.ComputeInertia(1), new(Simulation.Shapes.Add(capsule), ContinuousDetection.Discrete(0.1f, 0.1f)), -1)); var testBox = new Box(2, 3, 2); var testBoxInertia = testBox.ComputeInertia(1); Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 2, 6), testBoxInertia, new(Simulation.Shapes.Add(testBox), ContinuousDetection.Discrete(10.1f, 10.1f)), -1)); - var cylinder = new Cylinder(1.75f, 2); + var cylinder = new Cylinder(1.75f, 0.5f); var cylinderInertia = cylinder.ComputeInertia(1); //cylinderInertia.InverseInertiaTensor = default; Simulation.Bodies.Add(BodyDescription.CreateDynamic((new Vector3(20, 2, 9), Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), MathF.PI / 2f)), cylinderInertia, new(Simulation.Shapes.Add(cylinder), ContinuousDetection.Discrete(5f, 5f)), -1)); From 072c8b6f91cf5b99c3e7a7c5967cd3eaf77b26c2 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 12 Dec 2021 18:32:12 -0600 Subject: [PATCH 326/947] Fixed an oopsy in AngularMotor. --- BepuPhysics/Constraints/AngularMotor.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/BepuPhysics/Constraints/AngularMotor.cs b/BepuPhysics/Constraints/AngularMotor.cs index de330d569..4a310b125 100644 --- a/BepuPhysics/Constraints/AngularMotor.cs +++ b/BepuPhysics/Constraints/AngularMotor.cs @@ -111,7 +111,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in //Jacobians are just the identity matrix. MotorSettingsWide.ComputeSoftness(prestep.Settings, dt, out var effectiveMassCFMScale, out var softnessImpulseScale, out var maximumImpulse); - Symmetric3x3Wide.Add(inertiaB.InverseInertiaTensor, inertiaB.InverseInertiaTensor, out var unsoftenedInverseEffectiveMass); + Symmetric3x3Wide.Add(inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, out var unsoftenedInverseEffectiveMass); Symmetric3x3Wide.Invert(unsoftenedInverseEffectiveMass, out var unsoftenedEffectiveMass); //Note that we don't scale the effective mass directly; instead scale CSI. From a67f6cf723fe6852d5fced891e2fb6c087ce6ac8 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 13 Dec 2021 15:55:00 -0600 Subject: [PATCH 327/947] Fixed a NaNsplode in SwivelHinge, and made the threshold consistent between SwivelHinge and AngularSwivelHinge. --- BepuPhysics/Constraints/AngularSwivelHinge.cs | 2 +- BepuPhysics/Constraints/SwivelHinge.cs | 9 +++++++-- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/BepuPhysics/Constraints/AngularSwivelHinge.cs b/BepuPhysics/Constraints/AngularSwivelHinge.cs index 20ee3f376..5d010ded4 100644 --- a/BepuPhysics/Constraints/AngularSwivelHinge.cs +++ b/BepuPhysics/Constraints/AngularSwivelHinge.cs @@ -173,7 +173,7 @@ static void ComputeJacobian(in Vector3Wide localSwivelAxisA, in Vector3Wide loca //Note that this causes a discontinuity in jacobian length at the poles. We just don't worry about it. Helpers.FindPerpendicular(swivelAxis, out var fallbackJacobian); Vector3Wide.Dot(jacobianA, jacobianA, out var jacobianLengthSquared); - var useFallback = Vector.LessThan(jacobianLengthSquared, new Vector(1e-7f)); + var useFallback = Vector.LessThan(jacobianLengthSquared, new Vector(1e-3f)); Vector3Wide.ConditionalSelect(useFallback, fallbackJacobian, jacobianA, out jacobianA); } diff --git a/BepuPhysics/Constraints/SwivelHinge.cs b/BepuPhysics/Constraints/SwivelHinge.cs index d705d068d..8d6d20808 100644 --- a/BepuPhysics/Constraints/SwivelHinge.cs +++ b/BepuPhysics/Constraints/SwivelHinge.cs @@ -247,11 +247,16 @@ private static void ComputeJacobian(in Vector3Wide localOffsetA, in Vector3Wide Matrix3x3Wide.TransformWithoutOverlap(localOffsetB, orientationMatrixB, out offsetB); Matrix3x3Wide.TransformWithoutOverlap(localHingeAxisB, orientationMatrixB, out hingeAxis); Vector3Wide.CrossWithoutOverlap(swivelAxis, hingeAxis, out swivelHingeJacobian); + //If the axes are aligned, then it'll be zero length and the effective mass can get NaNsploded. + var lengthSquared = swivelHingeJacobian.LengthSquared(); + var useFallbackJacobian = Vector.LessThan(lengthSquared, new Vector(1e-3f)); + //This causes a discontinuity, but a discontinuity is better than a NaNsplode. + swivelHingeJacobian = Vector3Wide.ConditionalSelect(useFallbackJacobian, hingeAxis, swivelHingeJacobian); } public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref SwivelHingePrestepData prestep, ref Vector4Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - ComputeJacobian(prestep.LocalOffsetA, prestep.LocalSwivelAxisA, prestep.LocalOffsetB, prestep.LocalHingeAxisB, orientationA, orientationB, + ComputeJacobian(prestep.LocalOffsetA, prestep.LocalSwivelAxisA, prestep.LocalOffsetB, prestep.LocalHingeAxisB, orientationA, orientationB, out _, out _, out var offsetA, out var offsetB, out var swivelHingeJacobian); ApplyImpulse(offsetA, offsetB, swivelHingeJacobian, inertiaA, inertiaB, ref accumulatedImpulses, ref wsvA, ref wsvB); } @@ -262,7 +267,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in //[ I, skew(offsetA), -I, -skew(offsetB) ] //[ 0, swivelA x hingeB, 0, -swivelA x hingeB ] - ComputeJacobian(prestep.LocalOffsetA, prestep.LocalSwivelAxisA, prestep.LocalOffsetB, prestep.LocalHingeAxisB, orientationA, orientationB, + ComputeJacobian(prestep.LocalOffsetA, prestep.LocalSwivelAxisA, prestep.LocalOffsetB, prestep.LocalHingeAxisB, orientationA, orientationB, out var swivelAxis, out var hingeAxis, out var offsetA, out var offsetB, out var swivelHingeJacobian); //The upper left 3x3 block is just the ball socket. From 154f189fc65834529668026e3251d9bc178b4884 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 13 Dec 2021 16:32:44 -0600 Subject: [PATCH 328/947] Ancient oopsy fixed; hinge and swivel hinge actually make use of accumulated impulses properly. --- BepuPhysics/Constraints/Hinge.cs | 3 + BepuPhysics/Constraints/SwivelHinge.cs | 2 + .../Constraints/TwoBodyTypeProcessor.cs | 3 - BepuUtilities/Vector2Wide.cs | 57 ++++++++++++++ BepuUtilities/Vector3Wide.cs | 8 +- BepuUtilities/Vector4Wide.cs | 74 ++++++++++++++++++- 6 files changed, 142 insertions(+), 5 deletions(-) diff --git a/BepuPhysics/Constraints/Hinge.cs b/BepuPhysics/Constraints/Hinge.cs index 4b63d9bcb..02177052f 100644 --- a/BepuPhysics/Constraints/Hinge.cs +++ b/BepuPhysics/Constraints/Hinge.cs @@ -347,6 +347,9 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in Vector2Wide.Scale(accumulatedImpulses.Hinge, softnessImpulseScale, out var hingeSoftnessContribution); Vector2Wide.Subtract(csi.Hinge, hingeSoftnessContribution, out csi.Hinge); + accumulatedImpulses.BallSocket += csi.BallSocket; + accumulatedImpulses.Hinge += csi.Hinge; + ApplyImpulse(offsetA, offsetB, hingeJacobian, inertiaA, inertiaB, csi, ref wsvA, ref wsvB); } diff --git a/BepuPhysics/Constraints/SwivelHinge.cs b/BepuPhysics/Constraints/SwivelHinge.cs index 8d6d20808..4c6dc7216 100644 --- a/BepuPhysics/Constraints/SwivelHinge.cs +++ b/BepuPhysics/Constraints/SwivelHinge.cs @@ -333,6 +333,8 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in Vector4Wide.Scale(accumulatedImpulses, softnessImpulseScale, out var softnessContribution); Vector4Wide.Subtract(csi, softnessContribution, out csi); + accumulatedImpulses += csi; + ApplyImpulse(offsetA, offsetB, swivelHingeJacobian, inertiaA, inertiaB, ref csi, ref wsvA, ref wsvB); } diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index efcf1bfd9..cdf79892d 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -252,9 +252,6 @@ public unsafe override void WarmStart2(bodies, ref integratorCallbacks, ref integrationFlags, 1, dt, workerIndex, i, ref references.IndexB, out var positionB, out var orientationB, out var wsvB, out var inertiaB); - //if (typeof(TAllowPoseIntegration) == typeof(AllowPoseIntegration)) - // function.UpdateForNewPose(positionA, orientationA, inertiaA, wsvA, positionB, orientationB, inertiaB, wsvB, new Vector(dt), accumulatedImpulses, ref prestep); - function.WarmStart2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB); if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) diff --git a/BepuUtilities/Vector2Wide.cs b/BepuUtilities/Vector2Wide.cs index 40d44128a..ec7fdab77 100644 --- a/BepuUtilities/Vector2Wide.cs +++ b/BepuUtilities/Vector2Wide.cs @@ -4,17 +4,74 @@ namespace BepuUtilities { + /// + /// Two dimensional vector with (with generic type argument of ) SIMD lanes. + /// public struct Vector2Wide { + /// + /// First component of the vector. + /// public Vector X; + /// + /// Second component of the vector. + /// public Vector Y; + /// + /// Performs a componentwise add between two vectors. + /// + /// First vector to add. + /// Second vector to add. + /// Sum of a and b. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Add(in Vector2Wide a, in Vector2Wide b, out Vector2Wide result) { result.X = a.X + b.X; result.Y = a.Y + b.Y; } + /// + /// Performs a componentwise add between two vectors. + /// + /// First vector to add. + /// Second vector to add. + /// Sum of a and b. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector2Wide operator +(Vector2Wide a, Vector2Wide b) + { + Vector2Wide result; + result.X = a.X + b.X; + result.Y = a.Y + b.Y; + return result; + } + /// + /// Finds the result of adding a scalar to every component of a vector. + /// + /// Vector to add to. + /// Scalar to add to every component of the vector. + /// Vector with components equal to the input vector added to the input scalar. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector2Wide operator +(Vector2Wide v, Vector s) + { + Vector2Wide result; + result.X = v.X + s; + result.Y = v.Y + s; + return result; + } + /// + /// Finds the result of adding a scalar to every component of a vector. + /// + /// Vector to add to. + /// Scalar to add to every component of the vector. + /// Vector with components equal to the input vector added to the input scalar. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector2Wide operator +(Vector s, Vector2Wide v) + { + Vector2Wide result; + result.X = v.X + s; + result.Y = v.Y + s; + return result; + } [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Subtract(in Vector2Wide a, in Vector2Wide b, out Vector2Wide result) diff --git a/BepuUtilities/Vector3Wide.cs b/BepuUtilities/Vector3Wide.cs index 8f8931ae7..c36c33589 100644 --- a/BepuUtilities/Vector3Wide.cs +++ b/BepuUtilities/Vector3Wide.cs @@ -47,7 +47,7 @@ public Vector3Wide(Vector s) } /// - /// Performs a component wide add between two vectors. + /// Performs a componentwise add between two vectors. /// /// First vector to add. /// Second vector to add. @@ -72,6 +72,12 @@ public static void Add(in Vector3Wide v, in Vector s, out Vector3Wide res result.Y = v.Y + s; result.Z = v.Z + s; } + /// + /// Performs a componentwise add between two vectors. + /// + /// First vector to add. + /// Second vector to add. + /// Sum of a and b. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Vector3Wide operator +(Vector3Wide a, Vector3Wide b) { diff --git a/BepuUtilities/Vector4Wide.cs b/BepuUtilities/Vector4Wide.cs index 0f37009bc..86aa32f56 100644 --- a/BepuUtilities/Vector4Wide.cs +++ b/BepuUtilities/Vector4Wide.cs @@ -4,6 +4,9 @@ namespace BepuUtilities { + /// + /// Four dimensional vector with (with generic type argument of ) SIMD lanes. + /// public struct Vector4Wide { public Vector X; @@ -20,14 +23,83 @@ public static void Broadcast(in Vector4 source, out Vector4Wide broadcasted) broadcasted.W = new Vector(source.W); } + + /// + /// Performs a componentwise add between two vectors. + /// + /// First vector to add. + /// Second vector to add. + /// Sum of a and b. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Add(in Vector4Wide a, in Vector4Wide b, out Vector4Wide result) + public static void Add(Vector4Wide a, Vector4Wide b, out Vector4Wide result) { result.X = a.X + b.X; result.Y = a.Y + b.Y; result.Z = a.Z + b.Z; result.W = a.W + b.W; } + /// + /// Finds the result of adding a scalar to every component of a vector. + /// + /// Vector to add to. + /// Scalar to add to every component of the vector. + /// Vector with components equal to the input vector added to the input scalar. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Add(Vector4Wide v, Vector s, out Vector4Wide result) + { + result.X = v.X + s; + result.Y = v.Y + s; + result.Z = v.Z + s; + result.W = v.W + s; + } + /// + /// Performs a componentwise add between two vectors. + /// + /// First vector to add. + /// Second vector to add. + /// Sum of a and b. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector4Wide operator +(Vector4Wide a, Vector4Wide b) + { + Vector4Wide result; + result.X = a.X + b.X; + result.Y = a.Y + b.Y; + result.Z = a.Z + b.Z; + result.W = a.W + b.W; + return result; + } + /// + /// Finds the result of adding a scalar to every component of a vector. + /// + /// Vector to add to. + /// Scalar to add to every component of the vector. + /// Vector with components equal to the input vector added to the input scalar. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector4Wide operator +(Vector4Wide v, Vector s) + { + Vector4Wide result; + result.X = v.X + s; + result.Y = v.Y + s; + result.Z = v.Z + s; + result.W = v.W + s; + return result; + } + /// + /// Finds the result of adding a scalar to every component of a vector. + /// + /// Vector to add to. + /// Scalar to add to every component of the vector. + /// Vector with components equal to the input vector added to the input scalar. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector4Wide operator +(Vector s, Vector4Wide v) + { + Vector4Wide result; + result.X = v.X + s; + result.Y = v.Y + s; + result.Z = v.Z + s; + result.W = v.W + s; + return result; + } [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Subtract(in Vector4Wide a, in Vector4Wide b, out Vector4Wide result) From c1efe2435b5282654bf44a7509c4d65d50ac689f Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 13 Dec 2021 16:41:11 -0600 Subject: [PATCH 329/947] Internal-ized Inequality1DOF, and added a TODO that will definitely get addressed at some point. --- BepuPhysics/Constraints/Inequality1DOF.cs | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/BepuPhysics/Constraints/Inequality1DOF.cs b/BepuPhysics/Constraints/Inequality1DOF.cs index f0d88ca2b..2fd322a21 100644 --- a/BepuPhysics/Constraints/Inequality1DOF.cs +++ b/BepuPhysics/Constraints/Inequality1DOF.cs @@ -5,9 +5,13 @@ using System.Runtime.InteropServices; namespace BepuPhysics.Constraints -{ - - public struct TwoBody1DOFJacobians +{ + //TODO: These are notes about the mathy bits underlying constraints. They were written for the original pre-2.4 version of the solver. + //Most of it's still applicable, but 2.4 and up no longer have separate 'projection' states, and while packing is still important, it applies to *prestep data*. + //There's a lot less room for tricky premultiplication to save memory bandwidth, simply because those quantities are all in registers/L1 cache during a constraint solve in 2.4+. + //Would be nice to update those parts. + + internal struct TwoBody1DOFJacobians { public Vector3Wide LinearA; public Vector3Wide AngularA; @@ -15,9 +19,7 @@ public struct TwoBody1DOFJacobians public Vector3Wide AngularB; } - - - public struct Projection2Body1DOF + internal struct Projection2Body1DOF { //Rather than projecting from world space to constraint space *velocity* using JT, we precompute JT * effective mass //and go directly from world space velocity to constraint space impulse. @@ -43,7 +45,7 @@ public struct Projection2Body1DOF public Vector3Wide CSIToWSVAngularB; } - public static class Inequality2Body1DOF + internal static class Inequality2Body1DOF { [MethodImpl(MethodImplOptions.AggressiveInlining)] From f5c2272ff82f11b5b7432e2f836d76547ccb5909 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 13 Dec 2021 19:40:41 -0600 Subject: [PATCH 330/947] Some cleanup. --- .../Constraints/Contact/ContactConvexTypes.cs | 100 ++---------------- .../Constraints/Contact/ContactConvexTypes.tt | 18 +--- Demos/Demos/RagdollDemo.cs | 2 + Demos/SpecializedTests/GyroscopeTestDemo.cs | 8 +- 4 files changed, 15 insertions(+), 113 deletions(-) diff --git a/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs b/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs index c72e67cdf..bb9c4083e 100644 --- a/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs +++ b/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs @@ -378,15 +378,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in //That can cause some subtle behavioral issues sometimes, so we approximate lever arm with the contact depth, assuming that the contact surface area will increase as the depth increases. var maximumTwistImpulse = prestep.MaterialProperties.FrictionCoefficient * accumulatedImpulses.Penetration0 * prestep.Contact0.Depth; TwistFrictionOneBody.Solve2(prestep.Normal, inertiaA, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in Vector dt, in Contact1AccumulatedImpulses accumulatedImpulses, ref Contact1OneBodyPrestepData prestep) - { - PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, wsvA, ref prestep.Contact0.Depth); - } + } } /// @@ -600,16 +592,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact0.OffsetA) + accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact1.OffsetA)); TwistFrictionOneBody.Solve2(prestep.Normal, inertiaA, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in Vector dt, in Contact2AccumulatedImpulses accumulatedImpulses, ref Contact2OneBodyPrestepData prestep) - { - PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, wsvA, ref prestep.Contact0.Depth); - PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.Normal, wsvA, ref prestep.Contact1.Depth); - } + } } /// @@ -840,17 +823,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact1.OffsetA) + accumulatedImpulses.Penetration2 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact2.OffsetA)); TwistFrictionOneBody.Solve2(prestep.Normal, inertiaA, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in Vector dt, in Contact3AccumulatedImpulses accumulatedImpulses, ref Contact3OneBodyPrestepData prestep) - { - PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, wsvA, ref prestep.Contact0.Depth); - PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.Normal, wsvA, ref prestep.Contact1.Depth); - PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact2.OffsetA, prestep.Normal, wsvA, ref prestep.Contact2.Depth); - } + } } /// @@ -1098,18 +1071,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in accumulatedImpulses.Penetration2 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact2.OffsetA) + accumulatedImpulses.Penetration3 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact3.OffsetA)); TwistFrictionOneBody.Solve2(prestep.Normal, inertiaA, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in Vector dt, in Contact4AccumulatedImpulses accumulatedImpulses, ref Contact4OneBodyPrestepData prestep) - { - PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, wsvA, ref prestep.Contact0.Depth); - PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.Normal, wsvA, ref prestep.Contact1.Depth); - PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact2.OffsetA, prestep.Normal, wsvA, ref prestep.Contact2.Depth); - PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact3.OffsetA, prestep.Normal, wsvA, ref prestep.Contact3.Depth); - } + } } /// @@ -1323,17 +1285,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in //That can cause some subtle behavioral issues sometimes, so we approximate lever arm with the contact depth, assuming that the contact surface area will increase as the depth increases. var maximumTwistImpulse = prestep.MaterialProperties.FrictionCoefficient * accumulatedImpulses.Penetration0 * prestep.Contact0.Depth; TwistFriction.Solve2(prestep.Normal, inertiaA, inertiaB, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, - in Vector dt, in Contact1AccumulatedImpulses accumulatedImpulses, ref Contact1PrestepData prestep) - { - prestep.OffsetB = positionB - positionA; - PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact0.Depth); - } + } } /// @@ -1566,18 +1518,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact0.OffsetA) + accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact1.OffsetA)); TwistFriction.Solve2(prestep.Normal, inertiaA, inertiaB, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, - in Vector dt, in Contact2AccumulatedImpulses accumulatedImpulses, ref Contact2PrestepData prestep) - { - prestep.OffsetB = positionB - positionA; - PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact0.Depth); - PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact1.Depth); - } + } } /// @@ -1828,19 +1769,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact1.OffsetA) + accumulatedImpulses.Penetration2 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact2.OffsetA)); TwistFriction.Solve2(prestep.Normal, inertiaA, inertiaB, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, - in Vector dt, in Contact3AccumulatedImpulses accumulatedImpulses, ref Contact3PrestepData prestep) - { - prestep.OffsetB = positionB - positionA; - PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact0.Depth); - PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact1.Depth); - PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact2.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact2.Depth); - } + } } /// @@ -2109,20 +2038,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in accumulatedImpulses.Penetration2 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact2.OffsetA) + accumulatedImpulses.Penetration3 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact3.OffsetA)); TwistFriction.Solve2(prestep.Normal, inertiaA, inertiaB, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, - in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, - in Vector dt, in Contact4AccumulatedImpulses accumulatedImpulses, ref Contact4PrestepData prestep) - { - prestep.OffsetB = positionB - positionA; - PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact0.Depth); - PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact1.Depth); - PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact2.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact2.Depth); - PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact3.OffsetA, prestep.OffsetB, prestep.Normal, wsvA, wsvB, ref prestep.Contact3.Depth); - } + } } /// diff --git a/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt b/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt index 7277801f2..42900edc7 100644 --- a/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt +++ b/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt @@ -377,23 +377,7 @@ for (int i = 0; i < contactCount; ++i) <#}#> <#}#> TwistFriction<#=suffix#>.Solve2(prestep.Normal, inertiaA, <#if (bodyCount == 2) {#>inertiaB, <#}#>maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateForNewPose( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in BodyVelocityWide wsvA, -<# if(bodyCount == 2) {#> - in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in BodyVelocityWide wsvB, -<#}#> - in Vector dt, in Contact<#= contactCount #>AccumulatedImpulses accumulatedImpulses, ref Contact<#= contactCount #><#=suffix#>PrestepData prestep) - { -<#if (bodyCount == 2) {#> - prestep.OffsetB = positionB - positionA; -<#}#> -<#for (int i = 0; i < contactCount; ++i) {#> - PenetrationLimit<#=suffix#>.UpdatePenetrationDepth(dt, prestep.Contact<#=i#>.OffsetA, <#if(bodyCount == 2) {#>prestep.OffsetB, <#}#>prestep.Normal, wsvA, <#if (bodyCount == 2) {#>wsvB, <#}#>ref prestep.Contact<#=i#>.Depth); -<#}#> - } + } } /// diff --git a/Demos/Demos/RagdollDemo.cs b/Demos/Demos/RagdollDemo.cs index 41c9dd330..042defdac 100644 --- a/Demos/Demos/RagdollDemo.cs +++ b/Demos/Demos/RagdollDemo.cs @@ -453,6 +453,7 @@ public static RagdollHandles AddRagdoll(Vector3 position, Quaternion orientation SpringSettings = springSettings }); simulation.Solver.Add(handles.Hips, handles.Abdomen, BuildAngularMotor()); + //Abdomen-Chest var upperSpine = (abdomenPose.Position + chestPose.Position) * 0.5f; simulation.Solver.Add(handles.Abdomen, handles.Chest, new BallSocket @@ -477,6 +478,7 @@ public static RagdollHandles AddRagdoll(Vector3 position, Quaternion orientation SpringSettings = springSettings }); simulation.Solver.Add(handles.Abdomen, handles.Chest, BuildAngularMotor()); + //Chest-Head var neck = (headPose.Position + chestPose.Position) * 0.5f; simulation.Solver.Add(handles.Chest, handles.Head, new BallSocket diff --git a/Demos/SpecializedTests/GyroscopeTestDemo.cs b/Demos/SpecializedTests/GyroscopeTestDemo.cs index 386f8d0b7..16ce9e324 100644 --- a/Demos/SpecializedTests/GyroscopeTestDemo.cs +++ b/Demos/SpecializedTests/GyroscopeTestDemo.cs @@ -18,7 +18,7 @@ struct GyroscopicIntegratorCallbacks : IPoseIntegratorCallbacks //Pose integration isn't very expensive so using the higher quality option isn't that much of an issue, but it's also pretty subtle. //Unless your simulation requires the extra fidelity, there's not much reason to spend the extra time on it. DemoPoseIntegratorCallbacks innerCallbacks; - public readonly AngularIntegrationMode AngularIntegrationMode => AngularIntegrationMode.ConserveMomentumWithGyroscopicTorque; + public readonly AngularIntegrationMode AngularIntegrationMode => AngularIntegrationMode.ConserveMomentum; //For this demo, we'll allow substepping for unconstrained bodies. public readonly bool AllowSubstepsForUnconstrainedBodies => true; @@ -55,7 +55,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0; //Note the lack of damping- we want the gyroscope to keep spinning. - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new GyroscopicIntegratorCallbacks(new Vector3(0, -10, 0), 0f, 0f), 4); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new GyroscopicIntegratorCallbacks(new Vector3(0, -10, 0), 0f, 0f), 8); Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(new Box(100, 1, 100)))); @@ -66,12 +66,12 @@ public override void Initialize(ContentArchive content, Camera camera) var builder = new CompoundBuilder(BufferPool, Simulation.Shapes, 2); builder.Add(new Box(1, 0.3f, 0.3f), new Vector3(-0.5f, 0, 0), 1); - builder.Add(new Box(0.3f, 2f, 0.3f), new Vector3(0.15f, 0, 0), 2); + builder.Add(new Box(0.3f, 1.5f, 0.3f), new Vector3(0.15f, 0, 0), 3); builder.BuildDynamicCompound(out var children, out var inertia, out _); builder.Dispose(); var dzhanibekovShape = Simulation.Shapes.Add(new Compound(children)); var dzhanibekovSpinnerBody = Simulation.Bodies.Add( - BodyDescription.CreateDynamic(new Vector3(6, 4, 0), (new Vector3(0, 0, 1), new Vector3(5, .001f, .001f)), inertia, dzhanibekovShape, 0.01f)); + BodyDescription.CreateDynamic(new Vector3(6, 4, 0), (new Vector3(0, 0, 1), new Vector3(3, 1e-5f, 0)), inertia, dzhanibekovShape, 0.01f)); var dzhanibekovBaseBody = Simulation.Bodies.Add(BodyDescription.CreateConvexKinematic(new Vector3(6, 1, 0), Simulation.Shapes, new Box(.1f, 2, .1f))); Simulation.Solver.Add(dzhanibekovBaseBody, dzhanibekovSpinnerBody, new BallSocket { LocalOffsetA = new Vector3(0, 3, 0), LocalOffsetB = new Vector3(0, 0, 0), SpringSettings = new SpringSettings(30, 1) }); } From 2f5fbcc529452fe7c83d269c34be168f9b150a14 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 14 Dec 2021 17:12:38 -0600 Subject: [PATCH 331/947] Some demo comment updates. --- Demos/Demos/SimpleSelfContainedDemo.cs | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/Demos/Demos/SimpleSelfContainedDemo.cs b/Demos/Demos/SimpleSelfContainedDemo.cs index 5ef6004bc..fe853d7cd 100644 --- a/Demos/Demos/SimpleSelfContainedDemo.cs +++ b/Demos/Demos/SimpleSelfContainedDemo.cs @@ -13,7 +13,7 @@ namespace Demos.Demos { /// - /// Shows a completely isolated usage of the engine without using any of the other demo types. + /// Shows a completely isolated usage of the engine without using any of the other demo physics-related types. /// public static class SimpleSelfContainedDemo { @@ -94,7 +94,8 @@ public unsafe bool ConfigureContactManifold(int workerIndex, Collidab //The engine does not define any per-body material properties. Instead, all material lookup and blending operations are handled by the callbacks. //For the purposes of this demo, we'll use the same settings for all pairs. - //(Note that there's no bounciness property! See here for more details: https://github.com/bepu/bepuphysics2/issues/3 and check out the BouncinessDemo for some options.) + //(Note that there's no 'bounciness' or 'coefficient of restitution' property! + //Bounciness is handled through the contact spring settings instead. Setting See here for more details: https://github.com/bepu/bepuphysics2/issues/3 and check out the BouncinessDemo for some options.) pairMaterial.FrictionCoefficient = 1f; pairMaterial.MaximumRecoveryVelocity = 2f; pairMaterial.SpringSettings = new SpringSettings(30, 1); @@ -129,8 +130,6 @@ public void Dispose() //Note that the engine does not require any particular form of gravity- it, like all the contact callbacks, is managed by a callback. public struct PoseIntegratorCallbacks : IPoseIntegratorCallbacks { - public Vector3 Gravity; - /// /// Performs any required initialization logic after the Simulation instance has been constructed. /// @@ -161,6 +160,8 @@ public void Initialize(Simulation simulation) /// public readonly bool IntegrateVelocityForKinematics => false; + public Vector3 Gravity; + public PoseIntegratorCallbacks(Vector3 gravity) : this() { Gravity = gravity; @@ -211,12 +212,8 @@ public static void Run() { //The buffer pool is a source of raw memory blobs for the engine to use. var bufferPool = new BufferPool(); - //Note that you can also control the order of internal stage execution using a different ITimestepper implementation. - //The PositionFirstTimestepper is the simplest timestepping mode in a technical sense, but since it integrates velocity into position at the start of the frame, - //directly modified velocities outside of the timestep will be integrated before collision detection or the solver has a chance to intervene. - //PositionLastTimestepper avoids that by running collision detection and the solver first at the cost of a tiny amount of overhead. - //(You could avoid the issue with PositionFirstTimestepper by modifying velocities in the PositionFirstTimestepper's BeforeCollisionDetection callback - //instead of outside the timestep, too, but it's a little more complicated.) + //The following sets up a simulation with the callbacks defined above, and tells it to use 4 solver substeps per frame with 1 velocity iteration per substep. + //It uses the default SubsteppingTimestepper. You could use a custom ITimestepper implementation to customize when stages run relative to each other, or to insert more callbacks. var simulation = Simulation.Create(bufferPool, new NarrowPhaseCallbacks(), new PoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); //Drop a ball on a big static box. @@ -226,6 +223,7 @@ public static void Run() simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), simulation.Shapes.Add(new Box(500, 1, 500)))); + //Any IThreadDispatcher implementation can be used for multithreading. Here, we use the demos SimpleThreadDispatcher. var threadDispatcher = new SimpleThreadDispatcher(Environment.ProcessorCount); //Now take 100 time steps! @@ -233,6 +231,9 @@ public static void Run() { //Multithreading is pretty pointless for a simulation of one ball, but passing a IThreadDispatcher instance is all you have to do to enable multithreading. //If you don't want to use multithreading, don't pass a IThreadDispatcher. + + //Note that each timestep is 0.01 units in duration, so all 100 time steps will last 1 unit of time. + //(Usually, units of time are defined to be seconds, but the engine has no preconceived notions about units. All it sees are the numbers.) simulation.Timestep(0.01f, threadDispatcher); } From 6a010ecd9ef1791a9d39ff097c876878654be110 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 14 Dec 2021 20:01:30 -0600 Subject: [PATCH 332/947] Purged old constraint implementations. --- .../Constraints/AngularAxisGearMotor.cs | 64 +- BepuPhysics/Constraints/AngularAxisMotor.cs | 61 +- BepuPhysics/Constraints/AngularHinge.cs | 120 +--- BepuPhysics/Constraints/AngularMotor.cs | 46 +- BepuPhysics/Constraints/AngularServo.cs | 79 +-- BepuPhysics/Constraints/AngularSwivelHinge.cs | 88 +-- BepuPhysics/Constraints/AreaConstraint.cs | 114 +--- BepuPhysics/Constraints/BallSocket.cs | 48 +- BepuPhysics/Constraints/BallSocketMotor.cs | 45 +- BepuPhysics/Constraints/BallSocketServo.cs | 48 +- .../Constraints/CenterDistanceConstraint.cs | 65 +- .../Constraints/Contact/ContactConvexTypes.cs | 592 +----------------- .../Constraints/Contact/ContactConvexTypes.tt | 100 +-- .../Contact/ContactNonconvexCommon.cs | 191 +----- .../Contact/ContactNonconvexTypes.cs | 168 +---- .../Contact/ContactNonconvexTypes.tt | 60 +- BepuPhysics/Constraints/DistanceLimit.cs | 64 +- BepuPhysics/Constraints/DistanceServo.cs | 128 +--- .../Constraints/FourBodyTypeProcessor.cs | 88 +-- BepuPhysics/Constraints/Hinge.cs | 143 +---- BepuPhysics/Constraints/LinearAxisLimit.cs | 65 +- BepuPhysics/Constraints/LinearAxisMotor.cs | 37 +- BepuPhysics/Constraints/LinearAxisServo.cs | 55 +- .../Constraints/OneBodyAngularMotor.cs | 48 +- .../Constraints/OneBodyAngularServo.cs | 71 +-- BepuPhysics/Constraints/OneBodyLinearMotor.cs | 30 +- BepuPhysics/Constraints/OneBodyLinearServo.cs | 80 +-- .../Constraints/OneBodyTypeProcessor.cs | 72 +-- BepuPhysics/Constraints/PointOnLineServo.cs | 125 +--- BepuPhysics/Constraints/SwingLimit.cs | 93 +-- BepuPhysics/Constraints/SwivelHinge.cs | 134 +--- .../Constraints/ThreeBodyTypeProcessor.cs | 81 +-- BepuPhysics/Constraints/TwistLimit.cs | 65 +- BepuPhysics/Constraints/TwistMotor.cs | 60 +- BepuPhysics/Constraints/TwistServo.cs | 55 +- .../Constraints/TwoBodyTypeProcessor.cs | 93 +-- BepuPhysics/Constraints/TypeBatch.cs | 4 - BepuPhysics/Constraints/TypeProcessor.cs | 10 +- BepuPhysics/Constraints/VolumeConstraint.cs | 136 +--- BepuPhysics/Constraints/Weld.cs | 152 +---- .../Constraints/ConstraintLineExtractor.cs | 72 +-- .../Characters/CharacterMotionConstraint.cs | 276 +------- .../Characters/CharacterMotionConstraint.tt | 165 +---- Demos/Demos/Tanks/TankDemo.cs | 7 +- 44 files changed, 220 insertions(+), 4078 deletions(-) diff --git a/BepuPhysics/Constraints/AngularAxisGearMotor.cs b/BepuPhysics/Constraints/AngularAxisGearMotor.cs index e7a6be335..f0b71fc20 100644 --- a/BepuPhysics/Constraints/AngularAxisGearMotor.cs +++ b/BepuPhysics/Constraints/AngularAxisGearMotor.cs @@ -67,68 +67,8 @@ public struct AngularAxisGearMotorPrestepData public MotorSettingsWide Settings; } - public struct AngularAxisGearMotorProjection + public struct AngularAxisGearMotorFunctions : ITwoBodyConstraintFunctions> { - public Vector3Wide NegatedVelocityToImpulseB; - public Vector VelocityScale; - public Vector SoftnessImpulseScale; - public Vector MaximumImpulse; - public Vector3Wide ImpulseToVelocityA; - public Vector3Wide NegatedImpulseToVelocityB; - } - - - public struct AngularAxisGearMotorFunctions : ITwoBodyConstraintFunctions> - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, - float dt, float inverseDt, ref AngularAxisGearMotorPrestepData prestep, out AngularAxisGearMotorProjection projection) - { - //Velocity level constraint that acts directly on the given axes. Jacobians just the axes, nothing complicated. 1DOF, so we do premultiplication. - //This is mildly more complex than the AngularAxisMotor: - //dot(wa, axis) - dot(wb, axis) * velocityScale = 0, so jacobianB is actually -axis * velocityScale, not just -axis. - QuaternionWide.TransformWithoutOverlap(prestep.LocalAxisA, orientationA, out var axis); - Vector3Wide.Scale(axis, prestep.VelocityScale, out var jA); - Symmetric3x3Wide.TransformWithoutOverlap(jA, inertiaA.InverseInertiaTensor, out projection.ImpulseToVelocityA); - Vector3Wide.Dot(jA, projection.ImpulseToVelocityA, out var contributionA); - Symmetric3x3Wide.TransformWithoutOverlap(axis, inertiaB.InverseInertiaTensor, out projection.NegatedImpulseToVelocityB); - Vector3Wide.Dot(axis, projection.NegatedImpulseToVelocityB, out var contributionB); - MotorSettingsWide.ComputeSoftness(prestep.Settings, dt, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale, out projection.MaximumImpulse); - var effectiveMass = effectiveMassCFMScale / (contributionA + contributionB); - - Vector3Wide.Scale(axis, effectiveMass, out projection.NegatedVelocityToImpulseB); - projection.VelocityScale = prestep.VelocityScale; - - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ApplyImpulse(ref Vector3Wide angularVelocityA, ref Vector3Wide angularVelocityB, in AngularAxisGearMotorProjection projection, in Vector csi) - { - Vector3Wide.Scale(projection.ImpulseToVelocityA, csi, out var velocityChangeA); - Vector3Wide.Scale(projection.NegatedImpulseToVelocityB, csi, out var negatedVelocityChangeB); - Vector3Wide.Add(angularVelocityA, velocityChangeA, out angularVelocityA); - Vector3Wide.Subtract(angularVelocityB, negatedVelocityChangeB, out angularVelocityB); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref AngularAxisGearMotorProjection projection, ref Vector accumulatedImpulse) - { - ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, projection, accumulatedImpulse); - } - - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref AngularAxisGearMotorProjection projection, ref Vector accumulatedImpulse) - { - //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); - Vector3Wide.Dot(velocityA.Angular, projection.NegatedVelocityToImpulseB, out var unscaledCSIA); - Vector3Wide.Dot(velocityB.Angular, projection.NegatedVelocityToImpulseB, out var negatedCSIB); - var csi = -accumulatedImpulse * projection.SoftnessImpulseScale - (unscaledCSIA * projection.VelocityScale - negatedCSIB); - ServoSettingsWide.ClampImpulse(projection.MaximumImpulse, ref accumulatedImpulse, ref csi); - ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, projection, csi); - - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ApplyImpulse(in Vector3Wide impulseToVelocityA, in Vector3Wide negatedImpulseToVelocityB, in Vector csi, ref Vector3Wide angularVelocityA, ref Vector3Wide angularVelocityB) { @@ -173,7 +113,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref AngularAxisGearMotorPrestepData prestepData) { } } - public class AngularAxisGearMotorTypeProcessor : TwoBodyTypeProcessor, AngularAxisGearMotorFunctions, AccessOnlyAngular, AccessOnlyAngularWithoutPose, AccessOnlyAngular, AccessOnlyAngularWithoutPose> + public class AngularAxisGearMotorTypeProcessor : TwoBodyTypeProcessor, AngularAxisGearMotorFunctions, AccessOnlyAngular, AccessOnlyAngularWithoutPose, AccessOnlyAngular, AccessOnlyAngularWithoutPose> { public const int BatchTypeId = 54; } diff --git a/BepuPhysics/Constraints/AngularAxisMotor.cs b/BepuPhysics/Constraints/AngularAxisMotor.cs index 16877c1a8..44ce6e16a 100644 --- a/BepuPhysics/Constraints/AngularAxisMotor.cs +++ b/BepuPhysics/Constraints/AngularAxisMotor.cs @@ -66,65 +66,8 @@ public struct AngularAxisMotorPrestepData public MotorSettingsWide Settings; } - public struct AngularAxisMotorProjection + public struct AngularAxisMotorFunctions : ITwoBodyConstraintFunctions> { - public Vector3Wide VelocityToImpulseA; - public Vector BiasImpulse; - public Vector SoftnessImpulseScale; - public Vector MaximumImpulse; - public Vector3Wide ImpulseToVelocityA; - public Vector3Wide NegatedImpulseToVelocityB; - } - - - public struct AngularAxisMotorFunctions : ITwoBodyConstraintFunctions> - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, - float dt, float inverseDt, ref AngularAxisMotorPrestepData prestep, out AngularAxisMotorProjection projection) - { - //Velocity level constraint that acts directly on the given axes. Jacobians just the axes, nothing complicated. 1DOF, so we do premultiplication. - QuaternionWide.TransformWithoutOverlap(prestep.LocalAxisA, orientationA, out var axis); - Symmetric3x3Wide.TransformWithoutOverlap(axis, inertiaA.InverseInertiaTensor, out projection.ImpulseToVelocityA); - Vector3Wide.Dot(axis, projection.ImpulseToVelocityA, out var contributionA); - Symmetric3x3Wide.TransformWithoutOverlap(axis, inertiaB.InverseInertiaTensor, out projection.NegatedImpulseToVelocityB); - Vector3Wide.Dot(axis, projection.NegatedImpulseToVelocityB, out var contributionB); - MotorSettingsWide.ComputeSoftness(prestep.Settings, dt, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale, out projection.MaximumImpulse); - var effectiveMass = effectiveMassCFMScale / (contributionA + contributionB); - - Vector3Wide.Scale(axis, effectiveMass, out projection.VelocityToImpulseA); - - projection.BiasImpulse = prestep.TargetVelocity * effectiveMass; - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ApplyImpulse(ref Vector3Wide angularVelocityA, ref Vector3Wide angularVelocityB, in AngularAxisMotorProjection projection, in Vector csi) - { - Vector3Wide.Scale(projection.ImpulseToVelocityA, csi, out var velocityChangeA); - Vector3Wide.Scale(projection.NegatedImpulseToVelocityB, csi, out var negatedVelocityChangeB); - Vector3Wide.Add(angularVelocityA, velocityChangeA, out angularVelocityA); - Vector3Wide.Subtract(angularVelocityB, negatedVelocityChangeB, out angularVelocityB); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref AngularAxisMotorProjection projection, ref Vector accumulatedImpulse) - { - ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, projection, accumulatedImpulse); - } - - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref AngularAxisMotorProjection projection, ref Vector accumulatedImpulse) - { - //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); - Vector3Wide.Dot(velocityA.Angular, projection.VelocityToImpulseA, out var csiA); - Vector3Wide.Dot(velocityB.Angular, projection.VelocityToImpulseA, out var negatedCSIB); - var csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiA - negatedCSIB); - ServoSettingsWide.ClampImpulse(projection.MaximumImpulse, ref accumulatedImpulse, ref csi); - ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, projection, csi); - - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ApplyImpulse(in Vector3Wide impulseToVelocityA, in Vector3Wide negatedImpulseToVelocityB, in Vector csi, ref Vector3Wide angularVelocityA, ref Vector3Wide angularVelocityB) { @@ -162,7 +105,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref AngularAxisMotorPrestepData prestepData) { } } - public class AngularAxisMotorTypeProcessor : TwoBodyTypeProcessor, AngularAxisMotorFunctions, AccessOnlyAngular, AccessOnlyAngularWithoutPose, AccessOnlyAngular, AccessOnlyAngular> + public class AngularAxisMotorTypeProcessor : TwoBodyTypeProcessor, AngularAxisMotorFunctions, AccessOnlyAngular, AccessOnlyAngularWithoutPose, AccessOnlyAngular, AccessOnlyAngular> { public const int BatchTypeId = 41; } diff --git a/BepuPhysics/Constraints/AngularHinge.cs b/BepuPhysics/Constraints/AngularHinge.cs index 5c6111bd4..557b315f2 100644 --- a/BepuPhysics/Constraints/AngularHinge.cs +++ b/BepuPhysics/Constraints/AngularHinge.cs @@ -68,18 +68,7 @@ public struct AngularHingePrestepData public SpringSettingsWide SpringSettings; } - public struct AngularHingeProjection - { - //JacobianB = -JacobianA, so no need to store it explicitly. - public Matrix2x3Wide VelocityToImpulseA; - public Vector2Wide BiasImpulse; - public Vector SoftnessImpulseScale; - public Matrix2x3Wide ImpulseToVelocityA; - public Matrix2x3Wide NegatedImpulseToVelocityB; - } - - - public struct AngularHingeFunctions : ITwoBodyConstraintFunctions + public struct AngularHingeFunctions : ITwoBodyConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void GetErrorAngles(in Vector3Wide hingeAxisA, in Vector3Wide hingeAxisB, in Matrix2x3Wide jacobianA, out Vector2Wide errorAngles) @@ -121,111 +110,6 @@ public static void GetErrorAngles(in Vector3Wide hingeAxisA, in Vector3Wide hing errorAngles.Y = Vector.ConditionalSelect(Vector.LessThan(hbyax, Vector.Zero), -errorAngles.Y, errorAngles.Y); } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, - float dt, float inverseDt, ref AngularHingePrestepData prestep, out AngularHingeProjection projection) - { - //Note that we build the tangents in local space first to avoid inconsistencies. - Helpers.BuildOrthonormalBasis(prestep.LocalHingeAxisA, out var localAX, out var localAY); - Matrix3x3Wide.CreateFromQuaternion(orientationA, out var orientationMatrixA); - Matrix3x3Wide.TransformWithoutOverlap(prestep.LocalHingeAxisA, orientationMatrixA, out var hingeAxisA); - Matrix2x3Wide jacobianA; - Matrix3x3Wide.TransformWithoutOverlap(localAX, orientationMatrixA, out jacobianA.X); - Matrix3x3Wide.TransformWithoutOverlap(localAY, orientationMatrixA, out jacobianA.Y); - QuaternionWide.TransformWithoutOverlap(prestep.LocalHingeAxisB, orientationB, out var hingeAxisB); - - //We project hingeAxisB onto the planes defined by A's axis X and and axis Y, and treat them as constant with respect to A's velocity. - //This hand waves away a bit of complexity related to the fact that A's axes have velocity too, but it works out pretty nicely in the end. - //hingeAxisBOnPlaneX = hingeAxisB - dot(constraintAxisX, hingeAxisB) * constraintAxisX - //hingeAxisBOnPlaneY = hingeAxisB - dot(constraintAxisY, hingeAxisB) * constraintAxisY - //Note that we actually make use of inverse trig here. This is largely for the sake of the formulation, and the derivative will end up collapsing nicely. - //C = [atan(dot(hingeAxisBOnPlaneX, hingeAxisA), dot(hingeAxisBOnPlaneX, constraintAxisAY))] = [0] - // [atan(dot(hingeAxisBOnPlaneY, hingeAxisA), dot(hingeAxisBOnPlaneY, constraintAxisAX))] [0] - //Focusing on the hingeAxisOnPlaneX jacobian: - //C' = (dot(hingeAxisBOnPlaneX, hingeAxisA) * d/dt(dot(hingeAxisBOnPlaneX, constraintAxisAY)) - - // d/dt(dot(hingeAxisBOnPlaneX, hingeAxisA)) * dot(hingeAxisBOnPlaneX, constraintAxisAY)) * denom - //where denom = 1f / (dot(hingeAxisBOnPlaneX, hingeAxisA)^2 + dot(hingeAxisBOnPlaneX, constraintAxisAY)^2) - //C' = (dot(hingeAxisBOnPlaneX, hingeAxisA) * (dot(d/dt(hingeAxisBOnPlaneX), constraintAxisAY) + dot(hingeAxisBOnPlaneX, d/dt(constraintAxisAY))) - - // (dot(d/dt(hingeAxisBOnPlaneX), hingeAxisA) + dot(hingeAxisBOnPlaneX, d/dt(hingeAxisA))) * dot(hingeAxisBOnPlaneX, constraintAxisAY)) * denom - //C' = (dot(hingeAxisBOnPlaneX, hingeAxisA) * (dot(wB x hingeAxisBOnPlaneX, constraintAxisAY) + dot(hingeAxisBOnPlaneX, wA x constraintAxisAY)) - - // (dot(wB x hingeAxisBOnPlaneX, hingeAxisA) + dot(hingeAxisBOnPlaneX, wA x hingeAxisA)) * dot(hingeAxisBOnPlaneX, constraintAxisAY)) * denom - //C' = (dot(hingeAxisBOnPlaneX, hingeAxisA) * (dot(hingeAxisBOnPlaneX x constraintAxisAY, wB) + dot(wA, constraintAxisAY x hingeAxisBOnPlaneX)) - - // (dot(hingeAxisBOnPlaneX x hingeAxisA, wB) + dot(wA, hingeAxisA x hingeAxisBOnPlaneX)) * dot(hingeAxisBOnPlaneX, constraintAxisAY)) * denom - //C' = ((dot(dot(hingeAxisBOnPlaneX, hingeAxisA) * (hingeAxisBOnPlaneX x constraintAxisAY), wB) + dot(wA, dot(hingeAxisBOnPlaneX, hingeAxisA) * (constraintAxisAY x hingeAxisBOnPlaneX))) - - // (dot((hingeAxisBOnPlaneX x hingeAxisA) * dot(hingeAxisBOnPlaneX, constraintAxisAY), wB) + dot(wA, (hingeAxisA x hingeAxisBOnPlaneX) * dot(hingeAxisBOnPlaneX, constraintAxisAY)))) * denom - - //C' = ((dot(dot(hingeAxisBOnPlaneX, hingeAxisA) * (hingeAxisBOnPlaneX x constraintAxisAY) - (hingeAxisBOnPlaneX x hingeAxisA) * dot(hingeAxisBOnPlaneX, constraintAxisAY), wB) + - // dot(wA, dot(hingeAxisBOnPlaneX, hingeAxisA) * (constraintAxisAY x hingeAxisBOnPlaneX) - (hingeAxisA x hingeAxisBOnPlaneX) * dot(hingeAxisBOnPlaneX, constraintAxisAY)))) * denom - //C' = (dot(wB, dot(hingeAxisBOnPlaneX, hingeAxisA) * (hingeAxisBOnPlaneX x constraintAxisAY) - - // dot(hingeAxisBOnPlaneX, constraintAxisAY) * (hingeAxisBOnPlaneX x hingeAxisA) + - // dot(wA, dot(hingeAxisBOnPlaneX, hingeAxisA) * (constraintAxisAY x hingeAxisBOnPlaneX) - - // dot(hingeAxisBOnPlaneX, constraintAxisAY) * (hingeAxisA x hingeAxisBOnPlaneX)) * denom - //Note that both contributing vectors of jacobian A, constraintAxisAY x hingeAxisBOnPlaneX and hingeAxisA x hingeAxisBOnPlaneX, are aligned with constraintAxisAX. - //The only remaining question is the scale. Measure it by dotting with the constraintAxisAX. - //(Switching notation for conciseness here: a = hingeAxisA, b = hingeAxisBOnPlaneX, x = constraintAxisAX, y = constraintAxisAY) - //(dot(b, a) * cross(y, b) - dot(b, y) * cross(a, b)) / (dot(b, a)^2 + dot(b,y)^2) - //scale = dot((dot(b, a) * cross(y, b) - dot(b, y) * cross(a, b)) / (dot(b, a)^2 + dot(b,y)^2), x) - //scale = (dot(b, a) * dot(x, cross(y, b)) - dot(b, y) * dot(x, cross(a, b))) / (dot(b, a)^2 + dot(b,y)^2) - //scale = (dot(b, a) * dot(b, cross(x, y)) - dot(b, y) * dot(b, cross(x, a))) / (dot(b, a)^2 + dot(b,y)^2) - //scale = (dot(b, a) * dot(b, a) - dot(b, y) * dot(b, -y)) / (dot(b, a)^2 + dot(b,y)^2) - //scale = (dot(b, a) * dot(b, a) + dot(b, y) * dot(b, y)) / (dot(b, a)^2 + dot(b,y)^2) - //scale = 1 - //How convenient! - //jacobianA = [constraintAxisAX] - // [constraintAxisAY] - //jacobianB = -jacobianA - - //Note that JA = -JB, but for the purposes of calculating the effective mass the sign is irrelevant. - //This computes the effective mass using the usual (J * M^-1 * JT)^-1 formulation, but we actually make use of the intermediate result J * M^-1 so we compute it directly. - Symmetric3x3Wide.MultiplyWithoutOverlap(jacobianA, inertiaA.InverseInertiaTensor, out projection.ImpulseToVelocityA); - //Note that we don't use -jacobianA here, so we're actually storing out the negated version of the transform. That's fine; we'll simply subtract in the iteration. - Symmetric3x3Wide.MultiplyWithoutOverlap(jacobianA, inertiaB.InverseInertiaTensor, out projection.NegatedImpulseToVelocityB); - Symmetric2x2Wide.CompleteMatrixSandwich(projection.ImpulseToVelocityA, jacobianA, out var angularA); - Symmetric2x2Wide.CompleteMatrixSandwich(projection.NegatedImpulseToVelocityB, jacobianA, out var angularB); - Symmetric2x2Wide.Add(angularA, angularB, out var inverseEffectiveMass); - Symmetric2x2Wide.InvertWithoutOverlap(inverseEffectiveMass, out var effectiveMass); - - SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); - Symmetric2x2Wide.Scale(effectiveMass, effectiveMassCFMScale, out effectiveMass); - Symmetric2x2Wide.MultiplyTransposed(jacobianA, effectiveMass, out projection.VelocityToImpulseA); - GetErrorAngles(hingeAxisA, hingeAxisB, jacobianA, out var errorAngle); - //Note the negation: we want to oppose the separation. TODO: arguably, should bake the negation into positionErrorToVelocity, given its name. - Vector2Wide.Scale(errorAngle, -positionErrorToVelocity, out var biasVelocity); - Symmetric2x2Wide.TransformWithoutOverlap(biasVelocity, effectiveMass, out projection.BiasImpulse); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - private static void ApplyImpulse(ref Vector3Wide angularVelocityA, ref Vector3Wide angularVelocityB, ref AngularHingeProjection projection, ref Vector2Wide csi) - { - Matrix2x3Wide.Transform(csi, projection.ImpulseToVelocityA, out var velocityChangeA); - Vector3Wide.Add(angularVelocityA, velocityChangeA, out angularVelocityA); - Matrix2x3Wide.Transform(csi, projection.NegatedImpulseToVelocityB, out var negatedVelocityChangeB); - Vector3Wide.Subtract(angularVelocityB, negatedVelocityChangeB, out angularVelocityB); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref AngularHingeProjection projection, ref Vector2Wide accumulatedImpulse) - { - ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, ref projection, ref accumulatedImpulse); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref AngularHingeProjection projection, ref Vector2Wide accumulatedImpulse) - { - //JB = -JA. This is (angularVelocityA * JA + angularVelocityB * JB) * effectiveMass => (angularVelocityA - angularVelocityB) * (JA * effectiveMass) - Vector3Wide.Subtract(velocityA.Angular, velocityB.Angular, out var difference); - Matrix2x3Wide.TransformByTransposeWithoutOverlap(difference, projection.VelocityToImpulseA, out var csi); - //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); - Vector2Wide.Scale(accumulatedImpulse, projection.SoftnessImpulseScale, out var softnessContribution); - Vector2Wide.Add(softnessContribution, csi, out csi); - Vector2Wide.Subtract(projection.BiasImpulse, csi, out csi); - - Vector2Wide.Add(accumulatedImpulse, csi, out accumulatedImpulse); - - ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, ref projection, ref csi); - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] private static void ApplyImpulse(in Matrix2x3Wide impulseToVelocityA, in Matrix2x3Wide negatedImpulseToVelocityB, in Vector2Wide csi, ref Vector3Wide angularVelocityA, ref Vector3Wide angularVelocityB) { @@ -338,7 +222,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref AngularHingePrestepData prestepData) { } } - public class AngularHingeTypeProcessor : TwoBodyTypeProcessor + public class AngularHingeTypeProcessor : TwoBodyTypeProcessor { public const int BatchTypeId = 23; } diff --git a/BepuPhysics/Constraints/AngularMotor.cs b/BepuPhysics/Constraints/AngularMotor.cs index 4a310b125..447e412b0 100644 --- a/BepuPhysics/Constraints/AngularMotor.cs +++ b/BepuPhysics/Constraints/AngularMotor.cs @@ -57,50 +57,8 @@ public struct AngularMotorPrestepData public MotorSettingsWide Settings; } - public struct AngularMotorProjection + public struct AngularMotorFunctions : ITwoBodyConstraintFunctions { - public Symmetric3x3Wide EffectiveMass; - public Vector3Wide BiasImpulse; - public Vector SoftnessImpulseScale; - public Vector MaximumImpulse; - public Symmetric3x3Wide ImpulseToVelocityA; - public Symmetric3x3Wide NegatedImpulseToVelocityB; - } - - - public struct AngularMotorFunctions : ITwoBodyConstraintFunctions - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, - float dt, float inverseDt, ref AngularMotorPrestepData prestep, out AngularMotorProjection projection) - { - projection.ImpulseToVelocityA = inertiaA.InverseInertiaTensor; - projection.NegatedImpulseToVelocityB = inertiaB.InverseInertiaTensor; - - //Jacobians are just the identity matrix. - MotorSettingsWide.ComputeSoftness(prestep.Settings, dt, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale, out projection.MaximumImpulse); - - Symmetric3x3Wide.Add(projection.ImpulseToVelocityA, projection.NegatedImpulseToVelocityB, out var unsoftenedInverseEffectiveMass); - Symmetric3x3Wide.Invert(unsoftenedInverseEffectiveMass, out var unsoftenedEffectiveMass); - Symmetric3x3Wide.Scale(unsoftenedEffectiveMass, effectiveMassCFMScale, out projection.EffectiveMass); - - QuaternionWide.TransformWithoutOverlap(prestep.TargetVelocityLocalA, orientationA, out var biasVelocity); - Symmetric3x3Wide.TransformWithoutOverlap(biasVelocity, projection.EffectiveMass, out projection.BiasImpulse); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref AngularMotorProjection projection, ref Vector3Wide accumulatedImpulse) - { - AngularServoFunctions.ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, projection.ImpulseToVelocityA, projection.NegatedImpulseToVelocityB, accumulatedImpulse); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref AngularMotorProjection projection, ref Vector3Wide accumulatedImpulse) - { - AngularServoFunctions.Solve(ref velocityA, ref velocityB, projection.EffectiveMass, projection.SoftnessImpulseScale, projection.BiasImpulse, - projection.MaximumImpulse, projection.ImpulseToVelocityA, projection.NegatedImpulseToVelocityB, ref accumulatedImpulse); - } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref AngularMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { AngularServoFunctions.ApplyImpulse(ref wsvA.Angular, ref wsvB.Angular, inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, accumulatedImpulses); @@ -134,7 +92,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref AngularMotorPrestepData prestepData) { } } - public class AngularMotorTypeProcessor : TwoBodyTypeProcessor + public class AngularMotorTypeProcessor : TwoBodyTypeProcessor { public const int BatchTypeId = 30; } diff --git a/BepuPhysics/Constraints/AngularServo.cs b/BepuPhysics/Constraints/AngularServo.cs index 215a0331e..81737e40c 100644 --- a/BepuPhysics/Constraints/AngularServo.cs +++ b/BepuPhysics/Constraints/AngularServo.cs @@ -65,42 +65,8 @@ public struct AngularServoPrestepData public ServoSettingsWide ServoSettings; } - public struct AngularServoProjection + public struct AngularServoFunctions : ITwoBodyConstraintFunctions { - public Symmetric3x3Wide EffectiveMass; - public Vector3Wide BiasImpulse; - public Vector SoftnessImpulseScale; - public Vector MaximumImpulse; - public Symmetric3x3Wide ImpulseToVelocityA; - public Symmetric3x3Wide NegatedImpulseToVelocityB; - } - - - public struct AngularServoFunctions : ITwoBodyConstraintFunctions - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, - float dt, float inverseDt, ref AngularServoPrestepData prestep, out AngularServoProjection projection) - { - projection.ImpulseToVelocityA = inertiaA.InverseInertiaTensor; - projection.NegatedImpulseToVelocityB = inertiaB.InverseInertiaTensor; - - //Jacobians are just the identity matrix. - - QuaternionWide.ConcatenateWithoutOverlap(prestep.TargetRelativeRotationLocalA, orientationA, out var targetOrientationB); - QuaternionWide.Conjugate(targetOrientationB, out var inverseTarget); - QuaternionWide.ConcatenateWithoutOverlap(inverseTarget, orientationB, out var errorRotation); - - QuaternionWide.GetApproximateAxisAngleFromQuaternion(errorRotation, out var errorAxis, out var errorLength); - - SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); - Symmetric3x3Wide.Add(projection.ImpulseToVelocityA, projection.NegatedImpulseToVelocityB, out var unsoftenedInverseEffectiveMass); - Symmetric3x3Wide.Invert(unsoftenedInverseEffectiveMass, out var unsoftenedEffectiveMass); - Symmetric3x3Wide.Scale(unsoftenedEffectiveMass, effectiveMassCFMScale, out projection.EffectiveMass); - - ServoSettingsWide.ComputeClampedBiasVelocity(errorAxis, errorLength, positionErrorToVelocity, prestep.ServoSettings, dt, inverseDt, out var clampedBiasVelocity, out projection.MaximumImpulse); - Symmetric3x3Wide.TransformWithoutOverlap(clampedBiasVelocity, projection.EffectiveMass, out projection.BiasImpulse); - } [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ApplyImpulse(ref Vector3Wide angularVelocityA, ref Vector3Wide angularVelocityB, in Symmetric3x3Wide impulseToVelocityA, in Symmetric3x3Wide negatedImpulseToVelocityB, in Vector3Wide csi) @@ -111,36 +77,25 @@ public static void ApplyImpulse(ref Vector3Wide angularVelocityA, ref Vector3Wid Vector3Wide.Subtract(angularVelocityB, negatedVelocityChangeB, out angularVelocityB); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref AngularServoProjection projection, ref Vector3Wide accumulatedImpulse) - { - ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, projection.ImpulseToVelocityA, projection.NegatedImpulseToVelocityB, accumulatedImpulse); - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, - in Symmetric3x3Wide effectiveMass, in Vector softnessImpulseScale, in Vector3Wide biasImpulse, in Vector maximumImpulse, - in Symmetric3x3Wide impulseToVelocityA, in Symmetric3x3Wide negatedImpulseToVelocityB, ref Vector3Wide accumulatedImpulse) - { - //Jacobians are just I and -I. - Vector3Wide.Subtract(velocityA.Angular, velocityB.Angular, out var csv); - Symmetric3x3Wide.TransformWithoutOverlap(csv, effectiveMass, out var csiVelocityComponent); - //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); - Vector3Wide.Scale(accumulatedImpulse, softnessImpulseScale, out var softnessComponent); - Vector3Wide.Subtract(biasImpulse, softnessComponent, out var csi); - Vector3Wide.Subtract(csi, csiVelocityComponent, out csi); + //[MethodImpl(MethodImplOptions.AggressiveInlining)] + //public static void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, + // in Symmetric3x3Wide effectiveMass, in Vector softnessImpulseScale, in Vector3Wide biasImpulse, in Vector maximumImpulse, + // in Symmetric3x3Wide impulseToVelocityA, in Symmetric3x3Wide negatedImpulseToVelocityB, ref Vector3Wide accumulatedImpulse) + //{ + // //Jacobians are just I and -I. + // Vector3Wide.Subtract(velocityA.Angular, velocityB.Angular, out var csv); + // Symmetric3x3Wide.TransformWithoutOverlap(csv, effectiveMass, out var csiVelocityComponent); + // //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); + // Vector3Wide.Scale(accumulatedImpulse, softnessImpulseScale, out var softnessComponent); + // Vector3Wide.Subtract(biasImpulse, softnessComponent, out var csi); + // Vector3Wide.Subtract(csi, csiVelocityComponent, out csi); - ServoSettingsWide.ClampImpulse(maximumImpulse, ref accumulatedImpulse, ref csi); + // ServoSettingsWide.ClampImpulse(maximumImpulse, ref accumulatedImpulse, ref csi); - ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, impulseToVelocityA, negatedImpulseToVelocityB, csi); - } + // ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, impulseToVelocityA, negatedImpulseToVelocityB, csi); + //} - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref AngularServoProjection projection, ref Vector3Wide accumulatedImpulse) - { - Solve(ref velocityA, ref velocityB, projection.EffectiveMass, projection.SoftnessImpulseScale, projection.BiasImpulse, - projection.MaximumImpulse, projection.ImpulseToVelocityA, projection.NegatedImpulseToVelocityB, ref accumulatedImpulse); - } public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref AngularServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { @@ -181,7 +136,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref AngularServoPrestepData prestepData) { } } - public class AngularServoTypeProcessor : TwoBodyTypeProcessor + public class AngularServoTypeProcessor : TwoBodyTypeProcessor { public const int BatchTypeId = 29; } diff --git a/BepuPhysics/Constraints/AngularSwivelHinge.cs b/BepuPhysics/Constraints/AngularSwivelHinge.cs index 5d010ded4..8995ec8f5 100644 --- a/BepuPhysics/Constraints/AngularSwivelHinge.cs +++ b/BepuPhysics/Constraints/AngularSwivelHinge.cs @@ -68,92 +68,8 @@ public struct AngularSwivelHingePrestepData public SpringSettingsWide SpringSettings; } - public struct AngularSwivelHingeProjection + public struct AngularSwivelHingeFunctions : ITwoBodyConstraintFunctions> { - //JacobianB = -JacobianA, so no need to store it explicitly. - public Vector3Wide VelocityToImpulseA; - public Vector BiasImpulse; - public Vector SoftnessImpulseScale; - public Vector3Wide ImpulseToVelocityA; - public Vector3Wide NegatedImpulseToVelocityB; - } - - public struct AngularSwivelHingeFunctions : ITwoBodyConstraintFunctions> - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, - float dt, float inverseDt, ref AngularSwivelHingePrestepData prestep, out AngularSwivelHingeProjection projection) - { - //The swivel hinge attempts to keep an axis on body A separated 90 degrees from an axis on body B. In other words, this is the same as a hinge joint, but with one fewer DOF. - //C = dot(swivelA, hingeB) = 0 - //C' = dot(d/dt(swivelA), hingeB) + dot(swivelA, d/dt(hingeB)) = 0 - //C' = dot(angularVelocityB x hingeB, swivelA) + dot(hingeB, angularVelocityA x swivelA) = 0 - //C' = dot(hingeB x swivelA, angularVelocityB) + dot(angularVelocityA, swivelA x hingeB) = 0 - //Providing jacobians of: - //JA = swivelA x hingeB - //JB = hingeB x swivelA - //a x b == -b x a, so JB == -JA. - - //Now, we choose the storage representation. The default approach would be to store JA, the effective mass, and both inverse inertias, requiring 6 + 1 + 6 + 6 scalars. - //The alternative is to store JAT * effectiveMass, and then also JA * inverseInertiaTensor(A/B), requiring only 3 + 3 + 3 scalars. - //So, overall, prebaking saves us 10 scalars and a bit of iteration-time ALU. - QuaternionWide.TransformWithoutOverlap(prestep.LocalSwivelAxisA, orientationA, out var swivelAxis); - QuaternionWide.TransformWithoutOverlap(prestep.LocalHingeAxisB, orientationB, out var hingeAxis); - Vector3Wide.CrossWithoutOverlap(swivelAxis, hingeAxis, out var jacobianA); - //In the event that the axes are parallel, there is no unique jacobian. Arbitrarily pick one. - //Note that this causes a discontinuity in jacobian length at the poles. We just don't worry about it. - Helpers.FindPerpendicular(swivelAxis, out var fallbackJacobian); - Vector3Wide.Dot(jacobianA, jacobianA, out var jacobianLengthSquared); - var useFallback = Vector.LessThan(jacobianLengthSquared, new Vector(1e-7f)); - Vector3Wide.ConditionalSelect(useFallback, fallbackJacobian, jacobianA, out jacobianA); - - //Note that JA = -JB, but for the purposes of calculating the effective mass the sign is irrelevant. - - //This computes the effective mass using the usual (J * M^-1 * JT)^-1 formulation, but we actually make use of the intermediate result J * M^-1 so we compute it directly. - Symmetric3x3Wide.TransformWithoutOverlap(jacobianA, inertiaA.InverseInertiaTensor, out projection.ImpulseToVelocityA); - //Note that we don't use -jacobianA here, so we're actually storing out the negated version of the transform. That's fine; we'll simply subtract in the iteration. - Symmetric3x3Wide.TransformWithoutOverlap(jacobianA, inertiaB.InverseInertiaTensor, out projection.NegatedImpulseToVelocityB); - Vector3Wide.Dot(projection.ImpulseToVelocityA, jacobianA, out var angularA); - Vector3Wide.Dot(projection.NegatedImpulseToVelocityB, jacobianA, out var angularB); - - SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); - var effectiveMass = effectiveMassCFMScale / (angularA + angularB); - Vector3Wide.Scale(jacobianA, effectiveMass, out projection.VelocityToImpulseA); - - Vector3Wide.Dot(hingeAxis, swivelAxis, out var error); - //Note the negation: we want to oppose the separation. TODO: arguably, should bake the negation into positionErrorToVelocity, given its name. - projection.BiasImpulse = -effectiveMass * positionErrorToVelocity * error; - - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - private static void ApplyImpulse(ref Vector3Wide angularVelocityA, ref Vector3Wide angularVelocityB, ref AngularSwivelHingeProjection projection, ref Vector csi) - { - Vector3Wide.Scale(projection.ImpulseToVelocityA, csi, out var velocityChangeA); - Vector3Wide.Add(angularVelocityA, velocityChangeA, out angularVelocityA); - Vector3Wide.Scale(projection.NegatedImpulseToVelocityB, csi, out var negatedVelocityChangeB); - Vector3Wide.Subtract(angularVelocityB, negatedVelocityChangeB, out angularVelocityB); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref AngularSwivelHingeProjection projection, ref Vector accumulatedImpulse) - { - ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, ref projection, ref accumulatedImpulse); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref AngularSwivelHingeProjection projection, ref Vector accumulatedImpulse) - { - //JB = -JA. This is (angularVelocityA * JA + angularVelocityB * JB) * effectiveMass => (angularVelocityA - angularVelocityB) * (JA * effectiveMass) - Vector3Wide.Subtract(velocityA.Angular, velocityB.Angular, out var difference); - Vector3Wide.Dot(difference, projection.VelocityToImpulseA, out var csi); - //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); - csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - csi; - - accumulatedImpulse += csi; - ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, ref projection, ref csi); - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] private static void ApplyImpulse(in Vector3Wide impulseToVelocityA, in Vector3Wide negatedImpulseToVelocityB, in Vector csi, ref Vector3Wide angularVelocityA, ref Vector3Wide angularVelocityB) { @@ -231,7 +147,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref AngularSwivelHingePrestepData prestepData) { } } - public class AngularSwivelHingeTypeProcessor : TwoBodyTypeProcessor, AngularSwivelHingeFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular> + public class AngularSwivelHingeTypeProcessor : TwoBodyTypeProcessor, AngularSwivelHingeFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular> { public const int BatchTypeId = 24; } diff --git a/BepuPhysics/Constraints/AreaConstraint.cs b/BepuPhysics/Constraints/AreaConstraint.cs index 8480bb5b9..ea950bde6 100644 --- a/BepuPhysics/Constraints/AreaConstraint.cs +++ b/BepuPhysics/Constraints/AreaConstraint.cs @@ -73,118 +73,8 @@ public struct AreaConstraintPrestepData public SpringSettingsWide SpringSettings; } - public struct AreaConstraintProjection + public struct AreaConstraintFunctions : IThreeBodyConstraintFunctions> { - public Vector3Wide JacobianB; - public Vector3Wide JacobianC; - public Vector EffectiveMass; - public Vector BiasImpulse; - public Vector SoftnessImpulseScale; - public Vector InverseMassA; - public Vector InverseMassB; - public Vector InverseMassC; - } - - public struct AreaConstraintFunctions : IThreeBodyConstraintFunctions> - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep( - in QuaternionWide orientationA, in BodyInertiaWide inertiaA, - in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, - in Vector3Wide ac, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, - float dt, float inverseDt, ref AreaConstraintPrestepData prestep, out AreaConstraintProjection projection) - { - //Area of a triangle with vertices a, b, and c is: - //||ab x ac|| * 0.5 - //So the constraint is: - //OriginalArea * 2 = ||ab x ac|| - //Leading to a velocity constraint: - //d/dt(OriginalArea * 2) = d/dt(||ab x ac||) - //0 = d/dt(dot(ab x ac, ab x ac)^0.5) - //0 = d/dt(dot(ab x ac, ab x ac)) * 0.5 * dot(ab x ac, ab x ac)^-0.5) - //0 = (dot(d/dt(ab x ac), ab x ac) + dot(ab x ac, d/dt(ab x ac))) * 0.5 * dot(ab x ac, ab x ac)^-0.5) - //0 = (2 * dot(d/dt(ab x ac), ab x ac)) * 0.5 * dot(ab x ac, ab x ac)^-0.5) - //0 = (2 * dot(d/dt(ab) x ac + ab x d/dt(ac), ab x ac)) * 0.5 * dot(ab x ac, ab x ac)^-0.5) - //0 = dot(d/dt(ab) x ac + ab x d/dt(ac), ab x ac) * dot(ab x ac, ab x ac)^-0.5) - //0 = dot(d/dt(ab) x ac + ab x d/dt(ac), ab x ac) / ||ab x ac|| - //0 = (dot(d/dt(ab) x ac, ab x ac) + dot(ab x d/dt(ac), ab x ac)) / ||ab x ac|| - //0 = (dot(ac x (ab x ac), d/dt(ab)) + dot((ab x ac) x ab, d/dt(ac))) / ||ab x ac|| - //0 = dot(ac x ((ab x ac) / ||ab x ac||), d/dt(ab)) + dot(((ab x ac) / ||ab x ac||) x ab, d/dt(ac)) - Vector3Wide.CrossWithoutOverlap(ab, ac, out var abxac); - Vector3Wide.Length(abxac, out var normalLength); - //The triangle normal length can be zero if the edges are parallel or antiparallel. Protect against the potential division by zero. - Vector3Wide.Scale(abxac, Vector.ConditionalSelect(Vector.GreaterThan(normalLength, new Vector(1e-10f)), Vector.One / normalLength, Vector.Zero), out var normal); - - Vector3Wide.CrossWithoutOverlap(ac, normal, out projection.JacobianB); - Vector3Wide.CrossWithoutOverlap(normal, ab, out projection.JacobianC); - //Similar to the volume constraint, we could create a similar expression for jacobianA, but it's cheap to just do a couple of adds. - Vector3Wide.Add(projection.JacobianB, projection.JacobianC, out var negatedJacobianA); - - //We can store: - //Jacobians (2 * 3) - //Effective mass (1) - //Inverse inertia (1 * 3 since we don't need angular inertia) - //Since we don't need the inertia tensor, this is better than the premultiplied variant. - - Vector3Wide.Dot(negatedJacobianA, negatedJacobianA, out var contributionA); - Vector3Wide.Dot(projection.JacobianB, projection.JacobianB, out var contributionB); - Vector3Wide.Dot(projection.JacobianC, projection.JacobianC, out var contributionC); - - //Protect against singularity by padding the jacobian contributions. This is very much a hack, but it's a pretty simple hack. - //Less sensitive to tuning than attempting to guard the inverseEffectiveMass itself, since that is sensitive to both scale AND mass. - - //Choose an epsilon based on the target area. Note that area ~= width^2 and our jacobian contributions are things like (ac x N) * (ac x N). - //Given that N is perpendicular to AC, ||(ac x N)|| == ||ac||, so the contribution is just ||ac||^2. Given the square, it's proportional to area and the area is a decent epsilon source. - var epsilon = 5e-4f * prestep.TargetScaledArea; - contributionA = Vector.Max(epsilon, contributionA); - contributionB = Vector.Max(epsilon, contributionB); - contributionC = Vector.Max(epsilon, contributionC); - var inverseEffectiveMass = contributionA * inertiaA.InverseMass + contributionB * inertiaB.InverseMass + contributionC * inertiaC.InverseMass; - projection.InverseMassA = inertiaA.InverseMass; - projection.InverseMassB = inertiaB.InverseMass; - projection.InverseMassC = inertiaC.InverseMass; - - SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); - - projection.EffectiveMass = effectiveMassCFMScale / inverseEffectiveMass; - //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. - projection.BiasImpulse = (prestep.TargetScaledArea - normalLength) * (1f / 2f) * positionErrorToVelocity * projection.EffectiveMass; - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - private static void ApplyImpulse(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref BodyVelocityWide velocityC, - ref AreaConstraintProjection projection, ref Vector3Wide negatedJacobianA, ref Vector impulse) - { - Vector3Wide.Scale(negatedJacobianA, projection.InverseMassA * impulse, out var negativeVelocityChangeA); - Vector3Wide.Scale(projection.JacobianB, projection.InverseMassB * impulse, out var velocityChangeB); - Vector3Wide.Scale(projection.JacobianC, projection.InverseMassC * impulse, out var velocityChangeC); - Vector3Wide.Subtract(velocityA.Linear, negativeVelocityChangeA, out velocityA.Linear); - Vector3Wide.Add(velocityB.Linear, velocityChangeB, out velocityB.Linear); - Vector3Wide.Add(velocityC.Linear, velocityChangeC, out velocityC.Linear); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref BodyVelocityWide velocityC, ref AreaConstraintProjection projection, ref Vector accumulatedImpulse) - { - Vector3Wide.Add(projection.JacobianB, projection.JacobianC, out var negatedJacobianA); - ApplyImpulse(ref velocityA, ref velocityB, ref velocityC, ref projection, ref negatedJacobianA, ref accumulatedImpulse); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref BodyVelocityWide velocityC, ref AreaConstraintProjection projection, ref Vector accumulatedImpulse) - { - //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); - Vector3Wide.Add(projection.JacobianB, projection.JacobianC, out var negatedJacobianA); - Vector3Wide.Dot(negatedJacobianA, velocityA.Linear, out var negatedContributionA); - Vector3Wide.Dot(projection.JacobianB, velocityB.Linear, out var contributionB); - Vector3Wide.Dot(projection.JacobianC, velocityC.Linear, out var contributionC); - var csv = contributionB + contributionC - negatedContributionA; - var csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - csv * projection.EffectiveMass; - accumulatedImpulse += csi; - - ApplyImpulse(ref velocityA, ref velocityB, ref velocityC, ref projection, ref negatedJacobianA, ref csi); - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] private static void ApplyImpulse(in Vector inverseMassA, in Vector inverseMassB, in Vector inverseMassC, in Vector3Wide negatedJacobianA, in Vector3Wide jacobianB, in Vector3Wide jacobianC, in Vector impulse, @@ -285,7 +175,7 @@ public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWi /// /// Handles the solve iterations of a bunch of area constraints. /// - public class AreaConstraintTypeProcessor : ThreeBodyTypeProcessor, AreaConstraintFunctions, AccessOnlyLinear, AccessOnlyLinear, AccessOnlyLinear, AccessOnlyLinear, AccessOnlyLinear, AccessOnlyLinear> + public class AreaConstraintTypeProcessor : ThreeBodyTypeProcessor, AreaConstraintFunctions, AccessOnlyLinear, AccessOnlyLinear, AccessOnlyLinear, AccessOnlyLinear, AccessOnlyLinear, AccessOnlyLinear> { public const int BatchTypeId = 36; } diff --git a/BepuPhysics/Constraints/BallSocket.cs b/BepuPhysics/Constraints/BallSocket.cs index 489b67911..2880478a2 100644 --- a/BepuPhysics/Constraints/BallSocket.cs +++ b/BepuPhysics/Constraints/BallSocket.cs @@ -63,52 +63,8 @@ public struct BallSocketPrestepData public Vector3Wide LocalOffsetB; public SpringSettingsWide SpringSettings; } - - public struct BallSocketProjection + public struct BallSocketFunctions : ITwoBodyConstraintFunctions { - public Vector3Wide OffsetA; - public Vector3Wide OffsetB; - public Vector3Wide BiasVelocity; - public Symmetric3x3Wide EffectiveMass; - public Vector SoftnessImpulseScale; - public BodyInertiaWide InertiaA; - public BodyInertiaWide InertiaB; - } - - public struct BallSocketFunctions : ITwoBodyConstraintFunctions - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, - float dt, float inverseDt, ref BallSocketPrestepData prestep, out BallSocketProjection projection) - { - projection.InertiaA = inertiaA; - projection.InertiaB = inertiaB; - - //Note that we must reconstruct the world offsets from the body orientations since we do not store world offsets. - QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetA, orientationA, out projection.OffsetA); - QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetB, orientationB, out projection.OffsetB); - SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); - BallSocketShared.ComputeEffectiveMass(inertiaA, inertiaB, projection.OffsetA, projection.OffsetB, effectiveMassCFMScale, out projection.EffectiveMass); - - //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. - Vector3Wide.Add(ab, projection.OffsetB, out var anchorB); - Vector3Wide.Subtract(anchorB, projection.OffsetA, out var error); - Vector3Wide.Scale(error, positionErrorToVelocity, out projection.BiasVelocity); - } - - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref BallSocketProjection projection, ref Vector3Wide accumulatedImpulse) - { - BallSocketShared.ApplyImpulse(ref velocityA, ref velocityB, projection.OffsetA, projection.OffsetB, projection.InertiaA, projection.InertiaB, accumulatedImpulse); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref BallSocketProjection projection, ref Vector3Wide accumulatedImpulse) - { - BallSocketShared.Solve(ref velocityA, ref velocityB, projection.OffsetA, projection.OffsetB, projection.BiasVelocity, projection.EffectiveMass, projection.SoftnessImpulseScale, ref accumulatedImpulse, projection.InertiaA, projection.InertiaB); - } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref BallSocketPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { @@ -143,7 +99,7 @@ public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWi /// /// Handles the solve iterations of a bunch of ball socket constraints. /// - public class BallSocketTypeProcessor : TwoBodyTypeProcessor + public class BallSocketTypeProcessor : TwoBodyTypeProcessor { public const int BatchTypeId = 22; } diff --git a/BepuPhysics/Constraints/BallSocketMotor.cs b/BepuPhysics/Constraints/BallSocketMotor.cs index 17d94b566..7fe1731e5 100644 --- a/BepuPhysics/Constraints/BallSocketMotor.cs +++ b/BepuPhysics/Constraints/BallSocketMotor.cs @@ -67,49 +67,8 @@ public struct BallSocketMotorPrestepData public MotorSettingsWide Settings; } - public struct BallSocketMotorProjection + public struct BallSocketMotorFunctions : ITwoBodyConstraintFunctions { - public Vector3Wide OffsetA; - public Vector3Wide OffsetB; - public Vector3Wide BiasVelocity; - public Symmetric3x3Wide EffectiveMass; - public Vector SoftnessImpulseScale; - public Vector MaximumImpulse; - public BodyInertiaWide InertiaA; - public BodyInertiaWide InertiaB; - } - - public struct BallSocketMotorFunctions : ITwoBodyConstraintFunctions - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, - float dt, float inverseDt, ref BallSocketMotorPrestepData prestep, out BallSocketMotorProjection projection) - { - projection.InertiaA = inertiaA; - projection.InertiaB = inertiaB; - - //The offset for A just goes directly to B's anchor. - QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetB, orientationB, out projection.OffsetB); - Vector3Wide.Add(ab, projection.OffsetB, out projection.OffsetA); - MotorSettingsWide.ComputeSoftness(prestep.Settings, dt, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale, out projection.MaximumImpulse); - BallSocketShared.ComputeEffectiveMass(inertiaA, inertiaB, projection.OffsetA, projection.OffsetB, effectiveMassCFMScale, out projection.EffectiveMass); - - QuaternionWide.Transform(prestep.TargetVelocityLocalA, orientationA, out projection.BiasVelocity); - Vector3Wide.Negate(projection.BiasVelocity, out projection.BiasVelocity); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref BallSocketMotorProjection projection, ref Vector3Wide accumulatedImpulse) - { - BallSocketShared.ApplyImpulse(ref velocityA, ref velocityB, projection.OffsetA, projection.OffsetB, projection.InertiaA, projection.InertiaB, accumulatedImpulse); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref BallSocketMotorProjection projection, ref Vector3Wide accumulatedImpulse) - { - BallSocketShared.Solve(ref velocityA, ref velocityB, projection.OffsetA, projection.OffsetB, projection.BiasVelocity, projection.EffectiveMass, projection.SoftnessImpulseScale, projection.MaximumImpulse, ref accumulatedImpulse, projection.InertiaA, projection.InertiaB); - } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref BallSocketMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetB, orientationB, out var targetOffsetB); @@ -139,7 +98,7 @@ public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWi /// /// Handles the solve iterations of a bunch of ball socket motor constraints. /// - public class BallSocketMotorTypeProcessor : TwoBodyTypeProcessor + public class BallSocketMotorTypeProcessor : TwoBodyTypeProcessor { public const int BatchTypeId = 52; } diff --git a/BepuPhysics/Constraints/BallSocketServo.cs b/BepuPhysics/Constraints/BallSocketServo.cs index e72cb44a7..943156b92 100644 --- a/BepuPhysics/Constraints/BallSocketServo.cs +++ b/BepuPhysics/Constraints/BallSocketServo.cs @@ -72,52 +72,8 @@ public struct BallSocketServoPrestepData public ServoSettingsWide ServoSettings; } - public struct BallSocketServoProjection + public struct BallSocketServoFunctions : ITwoBodyConstraintFunctions { - public Vector3Wide OffsetA; - public Vector3Wide OffsetB; - public Vector3Wide BiasVelocity; - public Symmetric3x3Wide EffectiveMass; - public Vector SoftnessImpulseScale; - public Vector MaximumImpulse; - public BodyInertiaWide InertiaA; - public BodyInertiaWide InertiaB; - } - - public struct BallSocketServoFunctions : ITwoBodyConstraintFunctions - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, - float dt, float inverseDt, ref BallSocketServoPrestepData prestep, out BallSocketServoProjection projection) - { - projection.InertiaA = inertiaA; - projection.InertiaB = inertiaB; - - //Note that we must reconstruct the world offsets from the body orientations since we do not store world offsets. - QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetA, orientationA, out projection.OffsetA); - QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetB, orientationB, out projection.OffsetB); - SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); - BallSocketShared.ComputeEffectiveMass(inertiaA, inertiaB, projection.OffsetA, projection.OffsetB, effectiveMassCFMScale, out projection.EffectiveMass); - - //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. - Vector3Wide.Add(ab, projection.OffsetB, out var anchorB); - Vector3Wide.Subtract(anchorB, projection.OffsetA, out var error); - ServoSettingsWide.ComputeClampedBiasVelocity(error, positionErrorToVelocity, prestep.ServoSettings, dt, inverseDt, out projection.BiasVelocity, out projection.MaximumImpulse); - } - - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref BallSocketServoProjection projection, ref Vector3Wide accumulatedImpulse) - { - BallSocketShared.ApplyImpulse(ref velocityA, ref velocityB, projection.OffsetA, projection.OffsetB, projection.InertiaA, projection.InertiaB, accumulatedImpulse); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref BallSocketServoProjection projection, ref Vector3Wide accumulatedImpulse) - { - BallSocketShared.Solve(ref velocityA, ref velocityB, projection.OffsetA, projection.OffsetB, projection.BiasVelocity, projection.EffectiveMass, projection.SoftnessImpulseScale, projection.MaximumImpulse, ref accumulatedImpulse, projection.InertiaA, projection.InertiaB); - } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref BallSocketServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetA, orientationA, out var offsetA); @@ -150,7 +106,7 @@ public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWi /// /// Handles the solve iterations of a bunch of ball socket servo constraints. /// - public class BallSocketServoTypeProcessor : TwoBodyTypeProcessor + public class BallSocketServoTypeProcessor : TwoBodyTypeProcessor { public const int BatchTypeId = 53; } diff --git a/BepuPhysics/Constraints/CenterDistanceConstraint.cs b/BepuPhysics/Constraints/CenterDistanceConstraint.cs index 6b97937ba..223d9002a 100644 --- a/BepuPhysics/Constraints/CenterDistanceConstraint.cs +++ b/BepuPhysics/Constraints/CenterDistanceConstraint.cs @@ -66,67 +66,8 @@ public struct CenterDistancePrestepData public SpringSettingsWide SpringSettings; } - public struct CenterDistanceProjection + public struct CenterDistanceConstraintFunctions : ITwoBodyConstraintFunctions> { - public Vector3Wide JacobianA; - public Vector BiasVelocity; - public Vector SoftnessImpulseScale; - public Vector EffectiveMass; - public Vector InverseMassA; - public Vector InverseMassB; - } - - public struct CenterDistanceConstraintFunctions : ITwoBodyConstraintFunctions> - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, - float dt, float inverseDt, ref CenterDistancePrestepData prestep, out CenterDistanceProjection projection) - { - Vector3Wide.Length(ab, out var distance); - Vector3Wide.Scale(ab, Vector.One / distance, out projection.JacobianA); - - var useFallback = Vector.LessThan(distance, new Vector(1e-10f)); - projection.JacobianA.X = Vector.ConditionalSelect(useFallback, Vector.One, projection.JacobianA.X); - projection.JacobianA.Y = Vector.ConditionalSelect(useFallback, Vector.Zero, projection.JacobianA.Y); - projection.JacobianA.Z = Vector.ConditionalSelect(useFallback, Vector.Zero, projection.JacobianA.Z); - - SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); - //Jacobian is just the unit length direction, so the effective mass is simple: - projection.EffectiveMass = effectiveMassCFMScale / (inertiaA.InverseMass + inertiaB.InverseMass); - projection.InverseMassA = inertiaA.InverseMass; - projection.InverseMassB = inertiaB.InverseMass; - - //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. - projection.BiasVelocity = (distance - prestep.TargetDistance) * positionErrorToVelocity; - - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - static void ApplyImpulse(ref BodyVelocityWide a, ref BodyVelocityWide b, ref CenterDistanceProjection projection, ref Vector impulse) - { - Vector3Wide.Scale(projection.JacobianA, impulse * projection.InverseMassA, out var changeA); - Vector3Wide.Scale(projection.JacobianA, impulse * projection.InverseMassB, out var negatedChangeB); - Vector3Wide.Add(a.Linear, changeA, out a.Linear); - Vector3Wide.Subtract(b.Linear, negatedChangeB, out b.Linear); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref CenterDistanceProjection projection, ref Vector accumulatedImpulse) - { - ApplyImpulse(ref velocityA, ref velocityB, ref projection, ref accumulatedImpulse); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref CenterDistanceProjection projection, ref Vector accumulatedImpulse) - { - //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); - Vector3Wide.Dot(velocityA.Linear, projection.JacobianA, out var linearCSVA); - Vector3Wide.Dot(velocityB.Linear, projection.JacobianA, out var negatedCSVB); - var csi = (projection.BiasVelocity - (linearCSVA - negatedCSVB)) * projection.EffectiveMass - accumulatedImpulse * projection.SoftnessImpulseScale; - accumulatedImpulse += csi; - ApplyImpulse(ref velocityA, ref velocityB, ref projection, ref csi); - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] static void ApplyImpulse(in Vector3Wide jacobianA, in Vector inverseMassA, in Vector inverseMassB, in Vector impulse, ref BodyVelocityWide a, ref BodyVelocityWide b) { @@ -167,7 +108,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); //Jacobian is just the unit length direction, so the effective mass is simple: - var effectiveMass = effectiveMassCFMScale / (inertiaA.InverseMass + inertiaB.InverseMass); + var effectiveMass = effectiveMassCFMScale / (inertiaA.InverseMass + inertiaB.InverseMass); //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. var biasVelocity = (distance - prestep.TargetDistance) * positionErrorToVelocity; @@ -190,7 +131,7 @@ public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWi /// /// Handles the solve iterations of a bunch of distance servos. /// - public class CenterDistanceTypeProcessor : TwoBodyTypeProcessor, CenterDistanceConstraintFunctions, AccessOnlyLinear, AccessOnlyLinear, AccessOnlyLinear, AccessOnlyLinear> + public class CenterDistanceTypeProcessor : TwoBodyTypeProcessor, CenterDistanceConstraintFunctions, AccessOnlyLinear, AccessOnlyLinear, AccessOnlyLinear, AccessOnlyLinear> { public const int BatchTypeId = 35; } diff --git a/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs b/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs index bb9c4083e..548a80c5f 100644 --- a/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs +++ b/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs @@ -287,64 +287,9 @@ public ref MaterialPropertiesWide GetMaterialProperties(ref Contact1OneBodyPrest } - public unsafe struct Contact1OneBodyProjection - { - public BodyInertiaWide InertiaA; - public Vector PremultipliedFrictionCoefficient; - public Vector3Wide Normal; - public TangentFrictionOneBody.Projection Tangent; - public Vector SoftnessImpulseScale; - public PenetrationLimitOneBodyProjection Penetration0; - //Lever arms aren't included in the twist projection because the number of arms required varies independently of the twist projection itself. - public Vector LeverArm0; - public TwistFrictionProjection Twist; - } - - public struct Contact1OneBodyFunctions : IOneBodyConstraintFunctions - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref Contact1OneBodyPrestepData prestep, out Contact1OneBodyProjection projection) - { - //Be careful about the execution order here. It should be aligned with the prestep data layout to ensure prefetching works well. - projection.InertiaA = inertiaA; - projection.PremultipliedFrictionCoefficient = prestep.MaterialProperties.FrictionCoefficient; - projection.Normal = prestep.Normal; - Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); - TangentFrictionOneBody.Prestep(ref x, ref z, ref prestep.Contact0.OffsetA, ref projection.InertiaA, out projection.Tangent); - SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); - PenetrationLimitOneBody.Prestep(projection.InertiaA, prestep.Contact0.OffsetA, prestep.Normal, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration0); - //If there's only one contact, then the contact patch as determined by contact distance would be zero. - //That can cause some subtle behavioral issues sometimes, so we approximate lever arm with the contact depth, assuming that the contact surface area will increase as the depth increases. - projection.LeverArm0 = Vector.Max(Vector.Zero, prestep.Contact0.Depth); - TwistFrictionOneBody.Prestep(ref projection.InertiaA, ref prestep.Normal, out projection.Twist); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocityWide wsvA, ref Contact1OneBodyProjection projection, ref Contact1AccumulatedImpulses accumulatedImpulses) - { - Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - TangentFrictionOneBody.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref accumulatedImpulses.Tangent, ref wsvA); - PenetrationLimitOneBody.WarmStart(projection.Penetration0, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration0, ref wsvA); - TwistFrictionOneBody.WarmStart(ref projection.Normal, ref projection.InertiaA, ref accumulatedImpulses.Twist, ref wsvA); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocityWide wsvA, ref Contact1OneBodyProjection projection, ref Contact1AccumulatedImpulses accumulatedImpulses) - { - Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * - (accumulatedImpulses.Penetration0); - TangentFrictionOneBody.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); - //Note that we solve the penetration constraints after the friction constraints. - //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. - //It's a pretty minor effect either way. - PenetrationLimitOneBody.Solve(projection.Penetration0, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); - var maximumTwistImpulse = projection.PremultipliedFrictionCoefficient * ( - accumulatedImpulses.Penetration0 * projection.LeverArm0); - TwistFrictionOneBody.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); - } + public struct Contact1OneBodyFunctions : IOneBodyConstraintFunctions + { public bool RequiresIncrementalSubstepUpdates => true; [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -385,7 +330,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in /// Handles the solve iterations of a bunch of 1-contact one body manifold constraints. /// public class Contact1OneBodyTypeProcessor : - OneBodyContactTypeProcessor + OneBodyContactTypeProcessor { //Matches UpdateConstraintForManifold's manifoldTypeAsConstraintType computation. public const int BatchTypeId = 0; @@ -489,70 +434,9 @@ public ref MaterialPropertiesWide GetMaterialProperties(ref Contact2OneBodyPrest } - public unsafe struct Contact2OneBodyProjection - { - public BodyInertiaWide InertiaA; - public Vector PremultipliedFrictionCoefficient; - public Vector3Wide Normal; - public TangentFrictionOneBody.Projection Tangent; - public Vector SoftnessImpulseScale; - public PenetrationLimitOneBodyProjection Penetration0; - public PenetrationLimitOneBodyProjection Penetration1; - //Lever arms aren't included in the twist projection because the number of arms required varies independently of the twist projection itself. - public Vector LeverArm0; - public Vector LeverArm1; - public TwistFrictionProjection Twist; - } - - public struct Contact2OneBodyFunctions : IOneBodyConstraintFunctions - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref Contact2OneBodyPrestepData prestep, out Contact2OneBodyProjection projection) - { - //Be careful about the execution order here. It should be aligned with the prestep data layout to ensure prefetching works well. - projection.InertiaA = inertiaA; - FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, out var offsetToManifoldCenterA); - projection.PremultipliedFrictionCoefficient = (1f / 2f) * prestep.MaterialProperties.FrictionCoefficient; - projection.Normal = prestep.Normal; - Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); - TangentFrictionOneBody.Prestep(ref x, ref z, ref offsetToManifoldCenterA, ref projection.InertiaA, out projection.Tangent); - SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); - PenetrationLimitOneBody.Prestep(projection.InertiaA, prestep.Contact0.OffsetA, prestep.Normal, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration0); - PenetrationLimitOneBody.Prestep(projection.InertiaA, prestep.Contact1.OffsetA, prestep.Normal, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration1); - Vector3Wide.Distance(prestep.Contact0.OffsetA, offsetToManifoldCenterA, out projection.LeverArm0); - Vector3Wide.Distance(prestep.Contact1.OffsetA, offsetToManifoldCenterA, out projection.LeverArm1); - TwistFrictionOneBody.Prestep(ref projection.InertiaA, ref prestep.Normal, out projection.Twist); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocityWide wsvA, ref Contact2OneBodyProjection projection, ref Contact2AccumulatedImpulses accumulatedImpulses) - { - Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - TangentFrictionOneBody.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref accumulatedImpulses.Tangent, ref wsvA); - PenetrationLimitOneBody.WarmStart(projection.Penetration0, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration0, ref wsvA); - PenetrationLimitOneBody.WarmStart(projection.Penetration1, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration1, ref wsvA); - TwistFrictionOneBody.WarmStart(ref projection.Normal, ref projection.InertiaA, ref accumulatedImpulses.Twist, ref wsvA); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocityWide wsvA, ref Contact2OneBodyProjection projection, ref Contact2AccumulatedImpulses accumulatedImpulses) - { - Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * - (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1); - TangentFrictionOneBody.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); - //Note that we solve the penetration constraints after the friction constraints. - //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. - //It's a pretty minor effect either way. - PenetrationLimitOneBody.Solve(projection.Penetration0, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); - PenetrationLimitOneBody.Solve(projection.Penetration1, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA); - var maximumTwistImpulse = projection.PremultipliedFrictionCoefficient * ( - accumulatedImpulses.Penetration0 * projection.LeverArm0 + - accumulatedImpulses.Penetration1 * projection.LeverArm1); - TwistFrictionOneBody.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); - } + public struct Contact2OneBodyFunctions : IOneBodyConstraintFunctions + { public bool RequiresIncrementalSubstepUpdates => true; [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -599,7 +483,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in /// Handles the solve iterations of a bunch of 2-contact one body manifold constraints. /// public class Contact2OneBodyTypeProcessor : - OneBodyContactTypeProcessor + OneBodyContactTypeProcessor { //Matches UpdateConstraintForManifold's manifoldTypeAsConstraintType computation. public const int BatchTypeId = 1; @@ -709,77 +593,9 @@ public ref MaterialPropertiesWide GetMaterialProperties(ref Contact3OneBodyPrest } - public unsafe struct Contact3OneBodyProjection - { - public BodyInertiaWide InertiaA; - public Vector PremultipliedFrictionCoefficient; - public Vector3Wide Normal; - public TangentFrictionOneBody.Projection Tangent; - public Vector SoftnessImpulseScale; - public PenetrationLimitOneBodyProjection Penetration0; - public PenetrationLimitOneBodyProjection Penetration1; - public PenetrationLimitOneBodyProjection Penetration2; - //Lever arms aren't included in the twist projection because the number of arms required varies independently of the twist projection itself. - public Vector LeverArm0; - public Vector LeverArm1; - public Vector LeverArm2; - public TwistFrictionProjection Twist; - } - - public struct Contact3OneBodyFunctions : IOneBodyConstraintFunctions - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref Contact3OneBodyPrestepData prestep, out Contact3OneBodyProjection projection) - { - //Be careful about the execution order here. It should be aligned with the prestep data layout to ensure prefetching works well. - projection.InertiaA = inertiaA; - FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, out var offsetToManifoldCenterA); - projection.PremultipliedFrictionCoefficient = (1f / 3f) * prestep.MaterialProperties.FrictionCoefficient; - projection.Normal = prestep.Normal; - Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); - TangentFrictionOneBody.Prestep(ref x, ref z, ref offsetToManifoldCenterA, ref projection.InertiaA, out projection.Tangent); - SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); - PenetrationLimitOneBody.Prestep(projection.InertiaA, prestep.Contact0.OffsetA, prestep.Normal, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration0); - PenetrationLimitOneBody.Prestep(projection.InertiaA, prestep.Contact1.OffsetA, prestep.Normal, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration1); - PenetrationLimitOneBody.Prestep(projection.InertiaA, prestep.Contact2.OffsetA, prestep.Normal, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration2); - Vector3Wide.Distance(prestep.Contact0.OffsetA, offsetToManifoldCenterA, out projection.LeverArm0); - Vector3Wide.Distance(prestep.Contact1.OffsetA, offsetToManifoldCenterA, out projection.LeverArm1); - Vector3Wide.Distance(prestep.Contact2.OffsetA, offsetToManifoldCenterA, out projection.LeverArm2); - TwistFrictionOneBody.Prestep(ref projection.InertiaA, ref prestep.Normal, out projection.Twist); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocityWide wsvA, ref Contact3OneBodyProjection projection, ref Contact3AccumulatedImpulses accumulatedImpulses) - { - Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - TangentFrictionOneBody.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref accumulatedImpulses.Tangent, ref wsvA); - PenetrationLimitOneBody.WarmStart(projection.Penetration0, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration0, ref wsvA); - PenetrationLimitOneBody.WarmStart(projection.Penetration1, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration1, ref wsvA); - PenetrationLimitOneBody.WarmStart(projection.Penetration2, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration2, ref wsvA); - TwistFrictionOneBody.WarmStart(ref projection.Normal, ref projection.InertiaA, ref accumulatedImpulses.Twist, ref wsvA); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocityWide wsvA, ref Contact3OneBodyProjection projection, ref Contact3AccumulatedImpulses accumulatedImpulses) - { - Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * - (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2); - TangentFrictionOneBody.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); - //Note that we solve the penetration constraints after the friction constraints. - //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. - //It's a pretty minor effect either way. - PenetrationLimitOneBody.Solve(projection.Penetration0, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); - PenetrationLimitOneBody.Solve(projection.Penetration1, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA); - PenetrationLimitOneBody.Solve(projection.Penetration2, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA); - var maximumTwistImpulse = projection.PremultipliedFrictionCoefficient * ( - accumulatedImpulses.Penetration0 * projection.LeverArm0 + - accumulatedImpulses.Penetration1 * projection.LeverArm1 + - accumulatedImpulses.Penetration2 * projection.LeverArm2); - TwistFrictionOneBody.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); - } + public struct Contact3OneBodyFunctions : IOneBodyConstraintFunctions + { public bool RequiresIncrementalSubstepUpdates => true; [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -830,7 +646,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in /// Handles the solve iterations of a bunch of 3-contact one body manifold constraints. /// public class Contact3OneBodyTypeProcessor : - OneBodyContactTypeProcessor + OneBodyContactTypeProcessor { //Matches UpdateConstraintForManifold's manifoldTypeAsConstraintType computation. public const int BatchTypeId = 2; @@ -946,84 +762,9 @@ public ref MaterialPropertiesWide GetMaterialProperties(ref Contact4OneBodyPrest } - public unsafe struct Contact4OneBodyProjection - { - public BodyInertiaWide InertiaA; - public Vector PremultipliedFrictionCoefficient; - public Vector3Wide Normal; - public TangentFrictionOneBody.Projection Tangent; - public Vector SoftnessImpulseScale; - public PenetrationLimitOneBodyProjection Penetration0; - public PenetrationLimitOneBodyProjection Penetration1; - public PenetrationLimitOneBodyProjection Penetration2; - public PenetrationLimitOneBodyProjection Penetration3; - //Lever arms aren't included in the twist projection because the number of arms required varies independently of the twist projection itself. - public Vector LeverArm0; - public Vector LeverArm1; - public Vector LeverArm2; - public Vector LeverArm3; - public TwistFrictionProjection Twist; - } - - public struct Contact4OneBodyFunctions : IOneBodyConstraintFunctions - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref Contact4OneBodyPrestepData prestep, out Contact4OneBodyProjection projection) - { - //Be careful about the execution order here. It should be aligned with the prestep data layout to ensure prefetching works well. - projection.InertiaA = inertiaA; - FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact3.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, prestep.Contact3.Depth, out var offsetToManifoldCenterA); - projection.PremultipliedFrictionCoefficient = (1f / 4f) * prestep.MaterialProperties.FrictionCoefficient; - projection.Normal = prestep.Normal; - Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); - TangentFrictionOneBody.Prestep(ref x, ref z, ref offsetToManifoldCenterA, ref projection.InertiaA, out projection.Tangent); - SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); - PenetrationLimitOneBody.Prestep(projection.InertiaA, prestep.Contact0.OffsetA, prestep.Normal, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration0); - PenetrationLimitOneBody.Prestep(projection.InertiaA, prestep.Contact1.OffsetA, prestep.Normal, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration1); - PenetrationLimitOneBody.Prestep(projection.InertiaA, prestep.Contact2.OffsetA, prestep.Normal, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration2); - PenetrationLimitOneBody.Prestep(projection.InertiaA, prestep.Contact3.OffsetA, prestep.Normal, prestep.Contact3.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration3); - Vector3Wide.Distance(prestep.Contact0.OffsetA, offsetToManifoldCenterA, out projection.LeverArm0); - Vector3Wide.Distance(prestep.Contact1.OffsetA, offsetToManifoldCenterA, out projection.LeverArm1); - Vector3Wide.Distance(prestep.Contact2.OffsetA, offsetToManifoldCenterA, out projection.LeverArm2); - Vector3Wide.Distance(prestep.Contact3.OffsetA, offsetToManifoldCenterA, out projection.LeverArm3); - TwistFrictionOneBody.Prestep(ref projection.InertiaA, ref prestep.Normal, out projection.Twist); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocityWide wsvA, ref Contact4OneBodyProjection projection, ref Contact4AccumulatedImpulses accumulatedImpulses) - { - Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - TangentFrictionOneBody.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref accumulatedImpulses.Tangent, ref wsvA); - PenetrationLimitOneBody.WarmStart(projection.Penetration0, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration0, ref wsvA); - PenetrationLimitOneBody.WarmStart(projection.Penetration1, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration1, ref wsvA); - PenetrationLimitOneBody.WarmStart(projection.Penetration2, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration2, ref wsvA); - PenetrationLimitOneBody.WarmStart(projection.Penetration3, projection.InertiaA, projection.Normal, accumulatedImpulses.Penetration3, ref wsvA); - TwistFrictionOneBody.WarmStart(ref projection.Normal, ref projection.InertiaA, ref accumulatedImpulses.Twist, ref wsvA); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocityWide wsvA, ref Contact4OneBodyProjection projection, ref Contact4AccumulatedImpulses accumulatedImpulses) - { - Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * - (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2 + accumulatedImpulses.Penetration3); - TangentFrictionOneBody.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); - //Note that we solve the penetration constraints after the friction constraints. - //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. - //It's a pretty minor effect either way. - PenetrationLimitOneBody.Solve(projection.Penetration0, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); - PenetrationLimitOneBody.Solve(projection.Penetration1, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA); - PenetrationLimitOneBody.Solve(projection.Penetration2, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA); - PenetrationLimitOneBody.Solve(projection.Penetration3, projection.InertiaA, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration3, ref wsvA); - var maximumTwistImpulse = projection.PremultipliedFrictionCoefficient * ( - accumulatedImpulses.Penetration0 * projection.LeverArm0 + - accumulatedImpulses.Penetration1 * projection.LeverArm1 + - accumulatedImpulses.Penetration2 * projection.LeverArm2 + - accumulatedImpulses.Penetration3 * projection.LeverArm3); - TwistFrictionOneBody.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); - } + public struct Contact4OneBodyFunctions : IOneBodyConstraintFunctions + { public bool RequiresIncrementalSubstepUpdates => true; [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -1078,7 +819,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in /// Handles the solve iterations of a bunch of 4-contact one body manifold constraints. /// public class Contact4OneBodyTypeProcessor : - OneBodyContactTypeProcessor + OneBodyContactTypeProcessor { //Matches UpdateConstraintForManifold's manifoldTypeAsConstraintType computation. public const int BatchTypeId = 3; @@ -1187,69 +928,9 @@ public ref Vector3Wide GetOffsetB(ref Contact1PrestepData prestep) } } - public unsafe struct Contact1Projection - { - public BodyInertiaWide InertiaA; - public BodyInertiaWide InertiaB; - public Vector PremultipliedFrictionCoefficient; - public Vector3Wide Normal; - public TangentFriction.Projection Tangent; - public Vector SoftnessImpulseScale; - public PenetrationLimitProjection Penetration0; - //Lever arms aren't included in the twist projection because the number of arms required varies independently of the twist projection itself. - public Vector LeverArm0; - public TwistFrictionProjection Twist; - } - - public struct Contact1Functions : ITwoBodyConstraintFunctions - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep( - in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref Contact1PrestepData prestep, out Contact1Projection projection) - { - //Be careful about the execution order here. It should be aligned with the prestep data layout to ensure prefetching works well. - projection.InertiaA = inertiaA; - projection.InertiaB = inertiaB; - Vector3Wide.Subtract(prestep.Contact0.OffsetA, prestep.OffsetB, out var offsetToManifoldCenterB); - projection.PremultipliedFrictionCoefficient = prestep.MaterialProperties.FrictionCoefficient; - projection.Normal = prestep.Normal; - Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); - TangentFriction.Prestep(ref x, ref z, ref prestep.Contact0.OffsetA, ref offsetToManifoldCenterB, ref projection.InertiaA, ref projection.InertiaB, out projection.Tangent); - SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); - Vector3Wide contactOffsetB; - Vector3Wide.Subtract(prestep.Contact0.OffsetA, prestep.OffsetB, out contactOffsetB); - PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, prestep.Contact0.OffsetA, contactOffsetB, prestep.Normal, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration0); - //If there's only one contact, then the contact patch as determined by contact distance would be zero. - //That can cause some subtle behavioral issues sometimes, so we approximate lever arm with the contact depth, assuming that the contact surface area will increase as the depth increases. - projection.LeverArm0 = Vector.Max(Vector.Zero, prestep.Contact0.Depth); - TwistFriction.Prestep(ref projection.InertiaA, ref projection.InertiaB, ref prestep.Normal, out projection.Twist); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref Contact1Projection projection, ref Contact1AccumulatedImpulses accumulatedImpulses) - { - Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - TangentFriction.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart(projection.Penetration0, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - TwistFriction.WarmStart(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref Contact1Projection projection, ref Contact1AccumulatedImpulses accumulatedImpulses) - { - Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * - (accumulatedImpulses.Penetration0); - TangentFriction.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref projection.InertiaB, ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); - //Note that we solve the penetration constraints after the friction constraints. - //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. - //It's a pretty minor effect either way. - PenetrationLimit.Solve(projection.Penetration0, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - var maximumTwistImpulse = projection.PremultipliedFrictionCoefficient * ( - accumulatedImpulses.Penetration0 * projection.LeverArm0); - TwistFriction.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); - } + public struct Contact1Functions : ITwoBodyConstraintFunctions + { public bool RequiresIncrementalSubstepUpdates => true; [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -1292,7 +973,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in /// Handles the solve iterations of a bunch of 1-contact two body manifold constraints. /// public class Contact1TypeProcessor : - TwoBodyContactTypeProcessor + TwoBodyContactTypeProcessor { //Matches UpdateConstraintForManifold's manifoldTypeAsConstraintType computation. public const int BatchTypeId = 4; @@ -1407,76 +1088,9 @@ public ref Vector3Wide GetOffsetB(ref Contact2PrestepData prestep) } } - public unsafe struct Contact2Projection - { - public BodyInertiaWide InertiaA; - public BodyInertiaWide InertiaB; - public Vector PremultipliedFrictionCoefficient; - public Vector3Wide Normal; - public TangentFriction.Projection Tangent; - public Vector SoftnessImpulseScale; - public PenetrationLimitProjection Penetration0; - public PenetrationLimitProjection Penetration1; - //Lever arms aren't included in the twist projection because the number of arms required varies independently of the twist projection itself. - public Vector LeverArm0; - public Vector LeverArm1; - public TwistFrictionProjection Twist; - } - - public struct Contact2Functions : ITwoBodyConstraintFunctions - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep( - in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref Contact2PrestepData prestep, out Contact2Projection projection) - { - //Be careful about the execution order here. It should be aligned with the prestep data layout to ensure prefetching works well. - projection.InertiaA = inertiaA; - projection.InertiaB = inertiaB; - FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, out var offsetToManifoldCenterA); - Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); - projection.PremultipliedFrictionCoefficient = (1f / 2f) * prestep.MaterialProperties.FrictionCoefficient; - projection.Normal = prestep.Normal; - Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); - TangentFriction.Prestep(ref x, ref z, ref offsetToManifoldCenterA, ref offsetToManifoldCenterB, ref projection.InertiaA, ref projection.InertiaB, out projection.Tangent); - SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); - Vector3Wide contactOffsetB; - Vector3Wide.Subtract(prestep.Contact0.OffsetA, prestep.OffsetB, out contactOffsetB); - PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, prestep.Contact0.OffsetA, contactOffsetB, prestep.Normal, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration0); - Vector3Wide.Subtract(prestep.Contact1.OffsetA, prestep.OffsetB, out contactOffsetB); - PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, prestep.Contact1.OffsetA, contactOffsetB, prestep.Normal, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration1); - Vector3Wide.Distance(prestep.Contact0.OffsetA, offsetToManifoldCenterA, out projection.LeverArm0); - Vector3Wide.Distance(prestep.Contact1.OffsetA, offsetToManifoldCenterA, out projection.LeverArm1); - TwistFriction.Prestep(ref projection.InertiaA, ref projection.InertiaB, ref prestep.Normal, out projection.Twist); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref Contact2Projection projection, ref Contact2AccumulatedImpulses accumulatedImpulses) - { - Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - TangentFriction.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart(projection.Penetration0, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart(projection.Penetration1, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); - TwistFriction.WarmStart(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref Contact2Projection projection, ref Contact2AccumulatedImpulses accumulatedImpulses) - { - Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * - (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1); - TangentFriction.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref projection.InertiaB, ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); - //Note that we solve the penetration constraints after the friction constraints. - //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. - //It's a pretty minor effect either way. - PenetrationLimit.Solve(projection.Penetration0, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - PenetrationLimit.Solve(projection.Penetration1, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); - var maximumTwistImpulse = projection.PremultipliedFrictionCoefficient * ( - accumulatedImpulses.Penetration0 * projection.LeverArm0 + - accumulatedImpulses.Penetration1 * projection.LeverArm1); - TwistFriction.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); - } + public struct Contact2Functions : ITwoBodyConstraintFunctions + { public bool RequiresIncrementalSubstepUpdates => true; [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -1525,7 +1139,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in /// Handles the solve iterations of a bunch of 2-contact two body manifold constraints. /// public class Contact2TypeProcessor : - TwoBodyContactTypeProcessor + TwoBodyContactTypeProcessor { //Matches UpdateConstraintForManifold's manifoldTypeAsConstraintType computation. public const int BatchTypeId = 5; @@ -1646,84 +1260,9 @@ public ref Vector3Wide GetOffsetB(ref Contact3PrestepData prestep) } } - public unsafe struct Contact3Projection - { - public BodyInertiaWide InertiaA; - public BodyInertiaWide InertiaB; - public Vector PremultipliedFrictionCoefficient; - public Vector3Wide Normal; - public TangentFriction.Projection Tangent; - public Vector SoftnessImpulseScale; - public PenetrationLimitProjection Penetration0; - public PenetrationLimitProjection Penetration1; - public PenetrationLimitProjection Penetration2; - //Lever arms aren't included in the twist projection because the number of arms required varies independently of the twist projection itself. - public Vector LeverArm0; - public Vector LeverArm1; - public Vector LeverArm2; - public TwistFrictionProjection Twist; - } - - public struct Contact3Functions : ITwoBodyConstraintFunctions - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep( - in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref Contact3PrestepData prestep, out Contact3Projection projection) - { - //Be careful about the execution order here. It should be aligned with the prestep data layout to ensure prefetching works well. - projection.InertiaA = inertiaA; - projection.InertiaB = inertiaB; - FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, out var offsetToManifoldCenterA); - Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); - projection.PremultipliedFrictionCoefficient = (1f / 3f) * prestep.MaterialProperties.FrictionCoefficient; - projection.Normal = prestep.Normal; - Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); - TangentFriction.Prestep(ref x, ref z, ref offsetToManifoldCenterA, ref offsetToManifoldCenterB, ref projection.InertiaA, ref projection.InertiaB, out projection.Tangent); - SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); - Vector3Wide contactOffsetB; - Vector3Wide.Subtract(prestep.Contact0.OffsetA, prestep.OffsetB, out contactOffsetB); - PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, prestep.Contact0.OffsetA, contactOffsetB, prestep.Normal, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration0); - Vector3Wide.Subtract(prestep.Contact1.OffsetA, prestep.OffsetB, out contactOffsetB); - PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, prestep.Contact1.OffsetA, contactOffsetB, prestep.Normal, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration1); - Vector3Wide.Subtract(prestep.Contact2.OffsetA, prestep.OffsetB, out contactOffsetB); - PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, prestep.Contact2.OffsetA, contactOffsetB, prestep.Normal, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration2); - Vector3Wide.Distance(prestep.Contact0.OffsetA, offsetToManifoldCenterA, out projection.LeverArm0); - Vector3Wide.Distance(prestep.Contact1.OffsetA, offsetToManifoldCenterA, out projection.LeverArm1); - Vector3Wide.Distance(prestep.Contact2.OffsetA, offsetToManifoldCenterA, out projection.LeverArm2); - TwistFriction.Prestep(ref projection.InertiaA, ref projection.InertiaB, ref prestep.Normal, out projection.Twist); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref Contact3Projection projection, ref Contact3AccumulatedImpulses accumulatedImpulses) - { - Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - TangentFriction.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart(projection.Penetration0, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart(projection.Penetration1, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart(projection.Penetration2, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); - TwistFriction.WarmStart(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref Contact3Projection projection, ref Contact3AccumulatedImpulses accumulatedImpulses) - { - Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * - (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2); - TangentFriction.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref projection.InertiaB, ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); - //Note that we solve the penetration constraints after the friction constraints. - //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. - //It's a pretty minor effect either way. - PenetrationLimit.Solve(projection.Penetration0, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - PenetrationLimit.Solve(projection.Penetration1, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); - PenetrationLimit.Solve(projection.Penetration2, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); - var maximumTwistImpulse = projection.PremultipliedFrictionCoefficient * ( - accumulatedImpulses.Penetration0 * projection.LeverArm0 + - accumulatedImpulses.Penetration1 * projection.LeverArm1 + - accumulatedImpulses.Penetration2 * projection.LeverArm2); - TwistFriction.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); - } + public struct Contact3Functions : ITwoBodyConstraintFunctions + { public bool RequiresIncrementalSubstepUpdates => true; [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -1776,7 +1315,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in /// Handles the solve iterations of a bunch of 3-contact two body manifold constraints. /// public class Contact3TypeProcessor : - TwoBodyContactTypeProcessor + TwoBodyContactTypeProcessor { //Matches UpdateConstraintForManifold's manifoldTypeAsConstraintType computation. public const int BatchTypeId = 6; @@ -1903,92 +1442,9 @@ public ref Vector3Wide GetOffsetB(ref Contact4PrestepData prestep) } } - public unsafe struct Contact4Projection - { - public BodyInertiaWide InertiaA; - public BodyInertiaWide InertiaB; - public Vector PremultipliedFrictionCoefficient; - public Vector3Wide Normal; - public TangentFriction.Projection Tangent; - public Vector SoftnessImpulseScale; - public PenetrationLimitProjection Penetration0; - public PenetrationLimitProjection Penetration1; - public PenetrationLimitProjection Penetration2; - public PenetrationLimitProjection Penetration3; - //Lever arms aren't included in the twist projection because the number of arms required varies independently of the twist projection itself. - public Vector LeverArm0; - public Vector LeverArm1; - public Vector LeverArm2; - public Vector LeverArm3; - public TwistFrictionProjection Twist; - } - - public struct Contact4Functions : ITwoBodyConstraintFunctions - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep( - in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref Contact4PrestepData prestep, out Contact4Projection projection) - { - //Be careful about the execution order here. It should be aligned with the prestep data layout to ensure prefetching works well. - projection.InertiaA = inertiaA; - projection.InertiaB = inertiaB; - FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact3.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, prestep.Contact3.Depth, out var offsetToManifoldCenterA); - Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); - projection.PremultipliedFrictionCoefficient = (1f / 4f) * prestep.MaterialProperties.FrictionCoefficient; - projection.Normal = prestep.Normal; - Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); - TangentFriction.Prestep(ref x, ref z, ref offsetToManifoldCenterA, ref offsetToManifoldCenterB, ref projection.InertiaA, ref projection.InertiaB, out projection.Tangent); - SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); - Vector3Wide contactOffsetB; - Vector3Wide.Subtract(prestep.Contact0.OffsetA, prestep.OffsetB, out contactOffsetB); - PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, prestep.Contact0.OffsetA, contactOffsetB, prestep.Normal, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration0); - Vector3Wide.Subtract(prestep.Contact1.OffsetA, prestep.OffsetB, out contactOffsetB); - PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, prestep.Contact1.OffsetA, contactOffsetB, prestep.Normal, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration1); - Vector3Wide.Subtract(prestep.Contact2.OffsetA, prestep.OffsetB, out contactOffsetB); - PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, prestep.Contact2.OffsetA, contactOffsetB, prestep.Normal, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration2); - Vector3Wide.Subtract(prestep.Contact3.OffsetA, prestep.OffsetB, out contactOffsetB); - PenetrationLimit.Prestep(projection.InertiaA, projection.InertiaB, prestep.Contact3.OffsetA, contactOffsetB, prestep.Normal, prestep.Contact3.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration3); - Vector3Wide.Distance(prestep.Contact0.OffsetA, offsetToManifoldCenterA, out projection.LeverArm0); - Vector3Wide.Distance(prestep.Contact1.OffsetA, offsetToManifoldCenterA, out projection.LeverArm1); - Vector3Wide.Distance(prestep.Contact2.OffsetA, offsetToManifoldCenterA, out projection.LeverArm2); - Vector3Wide.Distance(prestep.Contact3.OffsetA, offsetToManifoldCenterA, out projection.LeverArm3); - TwistFriction.Prestep(ref projection.InertiaA, ref projection.InertiaB, ref prestep.Normal, out projection.Twist); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref Contact4Projection projection, ref Contact4AccumulatedImpulses accumulatedImpulses) - { - Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - TangentFriction.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart(projection.Penetration0, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart(projection.Penetration1, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart(projection.Penetration2, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart(projection.Penetration3, projection.InertiaA, projection.InertiaB, projection.Normal, accumulatedImpulses.Penetration3, ref wsvA, ref wsvB); - TwistFriction.WarmStart(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref Contact4Projection projection, ref Contact4AccumulatedImpulses accumulatedImpulses) - { - Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * - (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2 + accumulatedImpulses.Penetration3); - TangentFriction.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref projection.InertiaB, ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); - //Note that we solve the penetration constraints after the friction constraints. - //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. - //It's a pretty minor effect either way. - PenetrationLimit.Solve(projection.Penetration0, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - PenetrationLimit.Solve(projection.Penetration1, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); - PenetrationLimit.Solve(projection.Penetration2, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); - PenetrationLimit.Solve(projection.Penetration3, projection.InertiaA, projection.InertiaB, projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration3, ref wsvA, ref wsvB); - var maximumTwistImpulse = projection.PremultipliedFrictionCoefficient * ( - accumulatedImpulses.Penetration0 * projection.LeverArm0 + - accumulatedImpulses.Penetration1 * projection.LeverArm1 + - accumulatedImpulses.Penetration2 * projection.LeverArm2 + - accumulatedImpulses.Penetration3 * projection.LeverArm3); - TwistFriction.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); - } + public struct Contact4Functions : ITwoBodyConstraintFunctions + { public bool RequiresIncrementalSubstepUpdates => true; [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -2045,7 +1501,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in /// Handles the solve iterations of a bunch of 4-contact two body manifold constraints. /// public class Contact4TypeProcessor : - TwoBodyContactTypeProcessor + TwoBodyContactTypeProcessor { //Matches UpdateConstraintForManifold's manifoldTypeAsConstraintType computation. public const int BatchTypeId = 7; diff --git a/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt b/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt index 42900edc7..507edbd76 100644 --- a/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt +++ b/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt @@ -215,103 +215,9 @@ for (int i = 0; i < contactCount; ++i) <#}#> } - public unsafe struct Contact<#= contactCount #><#=suffix#>Projection - { - public BodyInertiaWide InertiaA; -<# if (bodyCount == 2) { #> - public BodyInertiaWide InertiaB; -<#}#> - public Vector PremultipliedFrictionCoefficient; - public Vector3Wide Normal; - public TangentFriction<#=suffix#>.Projection Tangent; - public Vector SoftnessImpulseScale; -<#for (int i = 0; i < contactCount ; ++i) {#> - public PenetrationLimit<#=suffix#>Projection Penetration<#=i#>; -<#}#> - //Lever arms aren't included in the twist projection because the number of arms required varies independently of the twist projection itself. -<#for (int i = 0; i < contactCount ; ++i) {#> - public Vector LeverArm<#=i#>; -<#}#> - public TwistFrictionProjection Twist; - } - - public struct Contact<#=contactCount#><#=suffix#>Functions : I<#=bodyCount == 1 ? "OneBody" : "TwoBody"#>ConstraintFunctions<#=suffix#>PrestepData, Contact<#=contactCount#><#=suffix#>Projection, Contact<#=contactCount#>AccumulatedImpulses> - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep( - <#if(bodyCount == 1) {#>in Vector3Wide positionA,<#}#> in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(bodyCount == 2) {#>in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>float dt, float inverseDt, ref Contact<#=contactCount#><#=suffix#>PrestepData prestep, out Contact<#=contactCount#><#=suffix#>Projection projection) - { - //Be careful about the execution order here. It should be aligned with the prestep data layout to ensure prefetching works well. - projection.InertiaA = inertiaA; -<#if (bodyCount == 2) {#> - projection.InertiaB = inertiaB; -<#}#> -<#if (contactCount > 1) {#> - FrictionHelpers.ComputeFrictionCenter(<#for (int i = 0; i < contactCount; ++i) {#>prestep.Contact<#=i#>.OffsetA, <#}#><#for (int i = 0; i < contactCount; ++i) {#>prestep.Contact<#=i#>.Depth, <#}#>out var offsetToManifoldCenterA); -<#if (bodyCount == 2) {#> - Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); -<#}#> -<#} else if(bodyCount == 2) {#> - Vector3Wide.Subtract(prestep.Contact0.OffsetA, prestep.OffsetB, out var offsetToManifoldCenterB); -<#}#> - projection.PremultipliedFrictionCoefficient = <#if (contactCount > 1) {#>(1f / <#=contactCount#>f) * <#}#>prestep.MaterialProperties.FrictionCoefficient; - projection.Normal = prestep.Normal; - Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); -<#var offsetName = contactCount == 1 ? "prestep.Contact0.OffsetA" : "offsetToManifoldCenterA";#> - TangentFriction<#=suffix#>.Prestep(ref x, ref z, ref <#=offsetName#>, <#if (bodyCount == 2) {#>ref offsetToManifoldCenterB, <#}#>ref projection.InertiaA, <#if (bodyCount == 2) {#>ref projection.InertiaB, <#}#>out projection.Tangent); - SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); -<#if (bodyCount == 2) {#> - Vector3Wide contactOffsetB; -<#}#> -<#for (int i = 0; i < contactCount; ++i) {#> -<#if (bodyCount == 2) {#> - Vector3Wide.Subtract(prestep.Contact<#=i#>.OffsetA, prestep.OffsetB, out contactOffsetB); -<#}#> - PenetrationLimit<#=suffix#>.Prestep(projection.InertiaA, <#if (bodyCount == 2) {#>projection.InertiaB, <#}#>prestep.Contact<#=i#>.OffsetA, <#if (bodyCount == 2) {#>contactOffsetB, <#}#>prestep.Normal, prestep.Contact<#=i#>.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDt, out projection.Penetration<#=i#>); -<#}#> -<#if (contactCount == 1) {#> - //If there's only one contact, then the contact patch as determined by contact distance would be zero. - //That can cause some subtle behavioral issues sometimes, so we approximate lever arm with the contact depth, assuming that the contact surface area will increase as the depth increases. - projection.LeverArm0 = Vector.Max(Vector.Zero, prestep.Contact0.Depth); -<#} else {#> -<#for (int i = 0; i < contactCount; ++i) {#> - Vector3Wide.Distance(prestep.Contact<#=i#>.OffsetA, <#=offsetName#>, out projection.LeverArm<#=i#>); -<#}}#> - TwistFriction<#=suffix#>.Prestep(ref projection.InertiaA, <#if (bodyCount == 2) {#>ref projection.InertiaB, <#}#>ref prestep.Normal, out projection.Twist); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocityWide wsvA, <#if(bodyCount == 2) {#>ref BodyVelocityWide wsvB, <#}#>ref Contact<#=contactCount#><#=suffix#>Projection projection, ref Contact<#=contactCount#>AccumulatedImpulses accumulatedImpulses) - { - Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - TangentFriction<#=suffix#>.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, <#if (bodyCount == 2) {#>ref projection.InertiaB, <#}#>ref accumulatedImpulses.Tangent, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); -<#for (int i = 0; i < contactCount; ++i) {#> - PenetrationLimit<#=suffix#>.WarmStart(projection.Penetration<#=i#>, projection.InertiaA, <#if (bodyCount == 2) {#>projection.InertiaB, <#}#>projection.Normal, accumulatedImpulses.Penetration<#=i#>, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); -<#}#> - TwistFriction<#=suffix#>.WarmStart(ref projection.Normal, ref projection.InertiaA, <#if (bodyCount == 2) {#>ref projection.InertiaB, <#}#>ref accumulatedImpulses.Twist, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocityWide wsvA, <#if(bodyCount == 2) {#>ref BodyVelocityWide wsvB, <#}#>ref Contact<#=contactCount#><#=suffix#>Projection projection, ref Contact<#=contactCount#>AccumulatedImpulses accumulatedImpulses) - { - Helpers.BuildOrthonormalBasis(projection.Normal, out var x, out var z); - var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * - (<#for (int i = 0; i < contactCount; ++i) {#>accumulatedImpulses.Penetration<#=i#><#if(i < contactCount - 1){#> + <#}}#>); - TangentFriction<#=suffix#>.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, <#if (bodyCount == 2) {#>ref projection.InertiaB, <#}#>ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); - //Note that we solve the penetration constraints after the friction constraints. - //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. - //It's a pretty minor effect either way. -<#for (int i = 0; i < contactCount; ++i) {#> - PenetrationLimit<#=suffix#>.Solve(projection.Penetration<#=i#>, projection.InertiaA, <#if (bodyCount == 2) {#>projection.InertiaB, <#}#>projection.Normal, projection.SoftnessImpulseScale, ref accumulatedImpulses.Penetration<#=i#>, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); -<#}#> - var maximumTwistImpulse = projection.PremultipliedFrictionCoefficient * ( -<#for (int i = 0; i < contactCount; ++i) {#> - accumulatedImpulses.Penetration<#=i#> * projection.LeverArm<#=i#><#if (i < contactCount - 1){#> +<#} else{#>);<#}#> - -<#}#> - TwistFriction<#=suffix#>.Solve(ref projection.Normal, ref projection.InertiaA, <#if (bodyCount == 2) {#>ref projection.InertiaB, <#}#>ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); - } + public struct Contact<#=contactCount#><#=suffix#>Functions : I<#=bodyCount == 1 ? "OneBody" : "TwoBody"#>ConstraintFunctions<#=suffix#>PrestepData, Contact<#=contactCount#>AccumulatedImpulses> + { public bool RequiresIncrementalSubstepUpdates => true; [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -384,7 +290,7 @@ for (int i = 0; i < contactCount; ++i) /// Handles the solve iterations of a bunch of <#= contactCount #>-contact <#Write(bodyCount == 1 ? "one" : "two");#> body manifold constraints. /// public class Contact<#= contactCount #><#=suffix#>TypeProcessor : - <#Write(bodyCount == 2 ? "Two" : "One");#>BodyContactTypeProcessor<#=suffix#>PrestepData, Contact<#= contactCount #><#=suffix#>Projection, Contact<#= contactCount #>AccumulatedImpulses, Contact<#= contactCount #><#=suffix#>Functions> + <#Write(bodyCount == 2 ? "Two" : "One");#>BodyContactTypeProcessor<#=suffix#>PrestepData, Contact<#= contactCount #>AccumulatedImpulses, Contact<#= contactCount #><#=suffix#>Functions> { //Matches UpdateConstraintForManifold's manifoldTypeAsConstraintType computation. public const int BatchTypeId = <#Write(bodyCount == 1 ? (contactCount - 1).ToString() : (3 + contactCount).ToString());#>; diff --git a/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs b/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs index 8b0051e59..f0fcc2fff 100644 --- a/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs +++ b/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs @@ -162,127 +162,17 @@ public static void BuildOneBodyDescription(ref TypeBatch } } - public struct NonconvexAccumulatedImpulses { public Vector2Wide Tangent; public Vector Penetration; } - public struct NonconvexOneBodyProjectionCommon - { - public BodyInertiaWide InertiaA; - public Vector FrictionCoefficient; - public Vector SoftnessImpulseScale; - } - public struct NonconvexTwoBodyProjectionCommon - { - public BodyInertiaWide InertiaA; - public BodyInertiaWide InertiaB; - public Vector FrictionCoefficient; - public Vector SoftnessImpulseScale; - } - public struct ContactNonconvexOneBodyProjection - { - public Vector3Wide Normal; - public TangentFrictionOneBody.Projection Tangent; - public PenetrationLimitOneBodyProjection Penetration; - } - public struct ContactNonconvexTwoBodyProjection - { - public Vector3Wide Normal; - public TangentFriction.Projection Tangent; - public PenetrationLimitProjection Penetration; - } - - public interface INonconvexOneBodyProjection where TProjection : INonconvexOneBodyProjection - { - ref ContactNonconvexOneBodyProjection GetFirstContact(ref TProjection description); - int ContactCount { get; } - - ref NonconvexOneBodyProjectionCommon GetCommonProperties(ref TProjection projection); - } - public interface INonconvexTwoBodyProjection where TProjection : INonconvexTwoBodyProjection - { - ref ContactNonconvexTwoBodyProjection GetFirstContact(ref TProjection description); - int ContactCount { get; } - - ref NonconvexTwoBodyProjectionCommon GetCommonProperties(ref TProjection projection); - } - - public struct ContactNonconvexOneBodyFunctions : - IOneBodyConstraintFunctions + public struct ContactNonconvexOneBodyFunctions : + IOneBodyConstraintFunctions where TPrestep : struct, INonconvexContactPrestep - where TProjection : struct, INonconvexOneBodyProjection where TAccumulatedImpulses : struct { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertia, - float dt, float inverseDt, ref TPrestep prestep, out TProjection projection) - { - //TODO: This is another area where it's highly doubtful that the compiler will ever figure out that this initialization is unnecessary. - //While we could jump through some nasty contortions now to resolve this, we'll instead opt for a little inefficient simplicity while waiting for generic pointer support - //to more cleanly fix the issue. - projection = default; - ref var prestepMaterial = ref prestep.GetMaterialProperties(ref prestep); - ref var projectionCommon = ref projection.GetCommonProperties(ref projection); - projectionCommon.InertiaA = inertia; - projectionCommon.FrictionCoefficient = prestepMaterial.FrictionCoefficient; - ref var prestepContactStart = ref prestep.GetContact(ref prestep, 0); - ref var projectionContactStart = ref projection.GetFirstContact(ref projection); - SpringSettingsWide.ComputeSpringiness(prestepMaterial.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projectionCommon.SoftnessImpulseScale); - for (int i = 0; i < projection.ContactCount; ++i) - { - ref var prestepContact = ref Unsafe.Add(ref prestepContactStart, i); - ref var projectionContact = ref Unsafe.Add(ref projectionContactStart, i); - projectionContact.Normal = prestepContact.Normal; - Helpers.BuildOrthonormalBasis(prestepContact.Normal, out var x, out var z); - TangentFrictionOneBody.Prestep(ref x, ref z, ref prestepContact.Offset, ref projectionCommon.InertiaA, out projectionContact.Tangent); - PenetrationLimitOneBody.Prestep(projectionCommon.InertiaA, - prestepContact.Offset, prestepContact.Normal, prestepContact.Depth, - positionErrorToVelocity, effectiveMassCFMScale, prestepMaterial.MaximumRecoveryVelocity, inverseDt, - out projectionContact.Penetration); - } - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void WarmStart(ref BodyVelocityWide wsvA, ref TProjection projection, ref TAccumulatedImpulses accumulatedImpulses) - { - //Note that, unlike convex manifolds, we simply solve every contact in sequence rather than tangent->penetration. - //This is not for any principled reason- only simplicity. May want to reconsider later, but remember the significant change in access pattern. - ref var common = ref projection.GetCommonProperties(ref projection); - ref var contactStart = ref projection.GetFirstContact(ref projection); - ref var accumulatedImpulsesStart = ref Unsafe.As(ref accumulatedImpulses); - for (int i = 0; i < projection.ContactCount; ++i) - { - ref var contact = ref Unsafe.Add(ref contactStart, i); - ref var contactImpulse = ref Unsafe.Add(ref accumulatedImpulsesStart, i); - Helpers.BuildOrthonormalBasis(contact.Normal, out var x, out var z); - TangentFrictionOneBody.WarmStart(ref x, ref z, ref contact.Tangent, ref common.InertiaA, ref contactImpulse.Tangent, ref wsvA); - PenetrationLimitOneBody.WarmStart(contact.Penetration, common.InertiaA, contact.Normal, contactImpulse.Penetration, ref wsvA); - } - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocityWide wsvA, ref TProjection projection, ref TAccumulatedImpulses accumulatedImpulses) - { - //Note that, unlike convex manifolds, we simply solve every contact in sequence rather than tangent->penetration. - //This is not for any principled reason- only simplicity. May want to reconsider later, but remember the significant change in access pattern. - ref var common = ref projection.GetCommonProperties(ref projection); - ref var contactStart = ref projection.GetFirstContact(ref projection); - ref var accumulatedImpulsesStart = ref Unsafe.As(ref accumulatedImpulses); - for (int i = 0; i < projection.ContactCount; ++i) - { - ref var contact = ref Unsafe.Add(ref contactStart, i); - ref var contactImpulse = ref Unsafe.Add(ref accumulatedImpulsesStart, i); - Helpers.BuildOrthonormalBasis(contact.Normal, out var x, out var z); - var maximumTangentImpulse = common.FrictionCoefficient * contactImpulse.Penetration; - TangentFrictionOneBody.Solve(ref x, ref z, ref contact.Tangent, ref common.InertiaA, ref maximumTangentImpulse, ref contactImpulse.Tangent, ref wsvA); - PenetrationLimitOneBody.Solve(contact.Penetration, common.InertiaA, contact.Normal, common.SoftnessImpulseScale, - ref contactImpulse.Penetration, ref wsvA); - } - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityWide velocity, ref TPrestep prestep) { @@ -310,7 +200,7 @@ public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, } public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref TPrestep prestep, ref TAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) - { + { //Note that, unlike convex manifolds, we simply solve every contact in sequence rather than tangent->penetration. //This is not for any principled reason- only simplicity. May want to reconsider later, but remember the significant change in access pattern. ref var prestepMaterial = ref prestep.GetMaterialProperties(ref prestep); @@ -350,82 +240,11 @@ public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWi } } - public struct ContactNonconvexTwoBodyFunctions : - ITwoBodyConstraintFunctions + public struct ContactNonconvexTwoBodyFunctions : + ITwoBodyConstraintFunctions where TPrestep : struct, ITwoBodyNonconvexContactPrestep - where TProjection : struct, INonconvexTwoBodyProjection where TAccumulatedImpulses : struct { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, - float dt, float inverseDt, ref TPrestep prestep, out TProjection projection) - { - //TODO: This is another area where it's highly doubtful that the compiler will ever figure out that this initialization is unnecessary. - //While we could jump through some nasty contortions now to resolve this, we'll instead opt for a little inefficient simplicity while waiting for generic pointer support - //to more cleanly fix the issue. - projection = default; - ref var prestepMaterial = ref prestep.GetMaterialProperties(ref prestep); - ref var prestepOffsetB = ref prestep.GetOffsetB(ref prestep); - ref var projectionCommon = ref projection.GetCommonProperties(ref projection); - projectionCommon.InertiaA = inertiaA; - projectionCommon.InertiaB = inertiaB; - projectionCommon.FrictionCoefficient = prestepMaterial.FrictionCoefficient; - ref var prestepContactStart = ref prestep.GetContact(ref prestep, 0); - ref var projectionContactStart = ref projection.GetFirstContact(ref projection); - SpringSettingsWide.ComputeSpringiness(prestepMaterial.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projectionCommon.SoftnessImpulseScale); - for (int i = 0; i < projection.ContactCount; ++i) - { - ref var prestepContact = ref Unsafe.Add(ref prestepContactStart, i); - ref var projectionContact = ref Unsafe.Add(ref projectionContactStart, i); - projectionContact.Normal = prestepContact.Normal; - Helpers.BuildOrthonormalBasis(prestepContact.Normal, out var x, out var z); - Vector3Wide.Subtract(prestepContact.Offset, prestepOffsetB, out var contactOffsetB); - TangentFriction.Prestep(ref x, ref z, ref prestepContact.Offset, ref contactOffsetB, ref projectionCommon.InertiaA, ref projectionCommon.InertiaB, out projectionContact.Tangent); - PenetrationLimit.Prestep(projectionCommon.InertiaA, projectionCommon.InertiaB, - prestepContact.Offset, contactOffsetB, prestepContact.Normal, prestepContact.Depth, - positionErrorToVelocity, effectiveMassCFMScale, prestepMaterial.MaximumRecoveryVelocity, inverseDt, - out projectionContact.Penetration); - } - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void WarmStart(ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref TProjection projection, ref TAccumulatedImpulses accumulatedImpulses) - { - //Note that, unlike convex manifolds, we simply solve every contact in sequence rather than tangent->penetration. - //This is not for any principled reason- only simplicity. May want to reconsider later, but remember the significant change in access pattern. - ref var common = ref projection.GetCommonProperties(ref projection); - ref var contactStart = ref projection.GetFirstContact(ref projection); - ref var accumulatedImpulsesStart = ref Unsafe.As(ref accumulatedImpulses); - for (int i = 0; i < projection.ContactCount; ++i) - { - ref var contact = ref Unsafe.Add(ref contactStart, i); - ref var contactImpulse = ref Unsafe.Add(ref accumulatedImpulsesStart, i); - Helpers.BuildOrthonormalBasis(contact.Normal, out var x, out var z); - TangentFriction.WarmStart(ref x, ref z, ref contact.Tangent, ref common.InertiaA, ref common.InertiaB, ref contactImpulse.Tangent, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart(contact.Penetration, common.InertiaA, common.InertiaB, contact.Normal, contactImpulse.Penetration, ref wsvA, ref wsvB); - } - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref TProjection projection, ref TAccumulatedImpulses accumulatedImpulses) - { - //Note that, unlike convex manifolds, we simply solve every contact in sequence rather than tangent->penetration. - //This is not for any principled reason- only simplicity. May want to reconsider later, but remember the significant change in access pattern. - ref var common = ref projection.GetCommonProperties(ref projection); - ref var contactStart = ref projection.GetFirstContact(ref projection); - ref var accumulatedImpulsesStart = ref Unsafe.As(ref accumulatedImpulses); - for (int i = 0; i < projection.ContactCount; ++i) - { - ref var contact = ref Unsafe.Add(ref contactStart, i); - ref var contactImpulse = ref Unsafe.Add(ref accumulatedImpulsesStart, i); - Helpers.BuildOrthonormalBasis(contact.Normal, out var x, out var z); - var maximumTangentImpulse = common.FrictionCoefficient * contactImpulse.Penetration; - TangentFriction.Solve(ref x, ref z, ref contact.Tangent, ref common.InertiaA, ref common.InertiaB, ref maximumTangentImpulse, ref contactImpulse.Tangent, ref wsvA, ref wsvB); - PenetrationLimit.Solve(contact.Penetration, common.InertiaA, common.InertiaB, contact.Normal, common.SoftnessImpulseScale, ref contactImpulse.Penetration, ref wsvA, ref wsvB); - } - } - - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref TPrestep prestep, ref TAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ref var prestepMaterial = ref prestep.GetMaterialProperties(ref prestep); diff --git a/BepuPhysics/Constraints/Contact/ContactNonconvexTypes.cs b/BepuPhysics/Constraints/Contact/ContactNonconvexTypes.cs index a206af275..5b8c29ca7 100644 --- a/BepuPhysics/Constraints/Contact/ContactNonconvexTypes.cs +++ b/BepuPhysics/Constraints/Contact/ContactNonconvexTypes.cs @@ -96,34 +96,13 @@ public ref NonconvexAccumulatedImpulses GetImpulsesForContact(ref Contact2Noncon return ref Unsafe.Add(ref impulses.Contact0, index); } } - - public unsafe struct Contact2NonconvexProjection : INonconvexTwoBodyProjection - { - public NonconvexTwoBodyProjectionCommon Common; - //Nonprimitive fixed would be pretty convenient. - public ContactNonconvexTwoBodyProjection Contact0; - public ContactNonconvexTwoBodyProjection Contact1; - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref ContactNonconvexTwoBodyProjection GetFirstContact(ref Contact2NonconvexProjection projection) - { - return ref projection.Contact0; - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref NonconvexTwoBodyProjectionCommon GetCommonProperties(ref Contact2NonconvexProjection projection) - { - return ref projection.Common; - } - - public readonly int ContactCount => 2; - } - + /// /// Handles the solve iterations of a bunch of 2-contact nonconvex two body manifold constraints. /// public class Contact2NonconvexTypeProcessor : - TwoBodyContactTypeProcessor> + TwoBodyContactTypeProcessor> { //Matches UpdateConstraintForManifold's manifoldTypeAsConstraintType computation. public const int BatchTypeId = 15; @@ -198,35 +177,14 @@ public ref NonconvexContactPrestepData GetContact(ref Contact2NonconvexOneBodyPr public readonly int ContactCount => 2; public readonly int BodyCount => 1; - } - - public unsafe struct Contact2NonconvexOneBodyProjection : INonconvexOneBodyProjection - { - public NonconvexOneBodyProjectionCommon Common; - //Nonprimitive fixed would be pretty convenient. - public ContactNonconvexOneBodyProjection Contact0; - public ContactNonconvexOneBodyProjection Contact1; - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref ContactNonconvexOneBodyProjection GetFirstContact(ref Contact2NonconvexOneBodyProjection projection) - { - return ref projection.Contact0; - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref NonconvexOneBodyProjectionCommon GetCommonProperties(ref Contact2NonconvexOneBodyProjection projection) - { - return ref projection.Common; - } - - public readonly int ContactCount => 2; - } + } /// /// Handles the solve iterations of a bunch of 2-contact nonconvex one body manifold constraints. /// public class Contact2NonconvexOneBodyTypeProcessor : - OneBodyContactTypeProcessor> + OneBodyContactTypeProcessor> { //Matches UpdateConstraintForManifold's manifoldTypeAsConstraintType computation. public const int BatchTypeId = 8; @@ -327,35 +285,13 @@ public ref NonconvexAccumulatedImpulses GetImpulsesForContact(ref Contact3Noncon return ref Unsafe.Add(ref impulses.Contact0, index); } } - - public unsafe struct Contact3NonconvexProjection : INonconvexTwoBodyProjection - { - public NonconvexTwoBodyProjectionCommon Common; - //Nonprimitive fixed would be pretty convenient. - public ContactNonconvexTwoBodyProjection Contact0; - public ContactNonconvexTwoBodyProjection Contact1; - public ContactNonconvexTwoBodyProjection Contact2; - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref ContactNonconvexTwoBodyProjection GetFirstContact(ref Contact3NonconvexProjection projection) - { - return ref projection.Contact0; - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref NonconvexTwoBodyProjectionCommon GetCommonProperties(ref Contact3NonconvexProjection projection) - { - return ref projection.Common; - } - - public readonly int ContactCount => 3; - } - + /// /// Handles the solve iterations of a bunch of 3-contact nonconvex two body manifold constraints. /// public class Contact3NonconvexTypeProcessor : - TwoBodyContactTypeProcessor> + TwoBodyContactTypeProcessor> { //Matches UpdateConstraintForManifold's manifoldTypeAsConstraintType computation. public const int BatchTypeId = 16; @@ -432,36 +368,14 @@ public ref NonconvexContactPrestepData GetContact(ref Contact3NonconvexOneBodyPr public readonly int ContactCount => 3; public readonly int BodyCount => 1; - } - - public unsafe struct Contact3NonconvexOneBodyProjection : INonconvexOneBodyProjection - { - public NonconvexOneBodyProjectionCommon Common; - //Nonprimitive fixed would be pretty convenient. - public ContactNonconvexOneBodyProjection Contact0; - public ContactNonconvexOneBodyProjection Contact1; - public ContactNonconvexOneBodyProjection Contact2; - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref ContactNonconvexOneBodyProjection GetFirstContact(ref Contact3NonconvexOneBodyProjection projection) - { - return ref projection.Contact0; - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref NonconvexOneBodyProjectionCommon GetCommonProperties(ref Contact3NonconvexOneBodyProjection projection) - { - return ref projection.Common; - } - - public readonly int ContactCount => 3; - } + } /// /// Handles the solve iterations of a bunch of 3-contact nonconvex one body manifold constraints. /// public class Contact3NonconvexOneBodyTypeProcessor : - OneBodyContactTypeProcessor> + OneBodyContactTypeProcessor> { //Matches UpdateConstraintForManifold's manifoldTypeAsConstraintType computation. public const int BatchTypeId = 9; @@ -565,36 +479,13 @@ public ref NonconvexAccumulatedImpulses GetImpulsesForContact(ref Contact4Noncon return ref Unsafe.Add(ref impulses.Contact0, index); } } - - public unsafe struct Contact4NonconvexProjection : INonconvexTwoBodyProjection - { - public NonconvexTwoBodyProjectionCommon Common; - //Nonprimitive fixed would be pretty convenient. - public ContactNonconvexTwoBodyProjection Contact0; - public ContactNonconvexTwoBodyProjection Contact1; - public ContactNonconvexTwoBodyProjection Contact2; - public ContactNonconvexTwoBodyProjection Contact3; - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref ContactNonconvexTwoBodyProjection GetFirstContact(ref Contact4NonconvexProjection projection) - { - return ref projection.Contact0; - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref NonconvexTwoBodyProjectionCommon GetCommonProperties(ref Contact4NonconvexProjection projection) - { - return ref projection.Common; - } - - public readonly int ContactCount => 4; - } - + /// /// Handles the solve iterations of a bunch of 4-contact nonconvex two body manifold constraints. /// public class Contact4NonconvexTypeProcessor : - TwoBodyContactTypeProcessor> + TwoBodyContactTypeProcessor> { //Matches UpdateConstraintForManifold's manifoldTypeAsConstraintType computation. public const int BatchTypeId = 17; @@ -673,37 +564,14 @@ public ref NonconvexContactPrestepData GetContact(ref Contact4NonconvexOneBodyPr public readonly int ContactCount => 4; public readonly int BodyCount => 1; - } - - public unsafe struct Contact4NonconvexOneBodyProjection : INonconvexOneBodyProjection - { - public NonconvexOneBodyProjectionCommon Common; - //Nonprimitive fixed would be pretty convenient. - public ContactNonconvexOneBodyProjection Contact0; - public ContactNonconvexOneBodyProjection Contact1; - public ContactNonconvexOneBodyProjection Contact2; - public ContactNonconvexOneBodyProjection Contact3; - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref ContactNonconvexOneBodyProjection GetFirstContact(ref Contact4NonconvexOneBodyProjection projection) - { - return ref projection.Contact0; - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref NonconvexOneBodyProjectionCommon GetCommonProperties(ref Contact4NonconvexOneBodyProjection projection) - { - return ref projection.Common; - } - - public readonly int ContactCount => 4; - } + } /// /// Handles the solve iterations of a bunch of 4-contact nonconvex one body manifold constraints. /// public class Contact4NonconvexOneBodyTypeProcessor : - OneBodyContactTypeProcessor> + OneBodyContactTypeProcessor> { //Matches UpdateConstraintForManifold's manifoldTypeAsConstraintType computation. public const int BatchTypeId = 10; diff --git a/BepuPhysics/Constraints/Contact/ContactNonconvexTypes.tt b/BepuPhysics/Constraints/Contact/ContactNonconvexTypes.tt index 325ee861b..54c76a0c7 100644 --- a/BepuPhysics/Constraints/Contact/ContactNonconvexTypes.tt +++ b/BepuPhysics/Constraints/Contact/ContactNonconvexTypes.tt @@ -115,37 +115,13 @@ for (int i = 0; i < contactCount ; ++i) return ref Unsafe.Add(ref impulses.Contact0, index); } } - - public unsafe struct Contact<#= contactCount #>NonconvexProjection : INonconvexTwoBodyProjectionNonconvexProjection> - { - public NonconvexTwoBodyProjectionCommon Common; - //Nonprimitive fixed would be pretty convenient. -<# -for (int i = 0; i < contactCount ; ++i) -{#> - public ContactNonconvexTwoBodyProjection Contact<#=i#>; -<#}#> - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref ContactNonconvexTwoBodyProjection GetFirstContact(ref Contact<#= contactCount #>NonconvexProjection projection) - { - return ref projection.Contact0; - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref NonconvexTwoBodyProjectionCommon GetCommonProperties(ref Contact<#= contactCount #>NonconvexProjection projection) - { - return ref projection.Common; - } - - public readonly int ContactCount => <#= contactCount #>; - } - + /// /// Handles the solve iterations of a bunch of <#= contactCount #>-contact nonconvex two body manifold constraints. /// public class Contact<#= contactCount #>NonconvexTypeProcessor : - TwoBodyContactTypeProcessorNonconvexPrestepData, Contact<#= contactCount #>NonconvexProjection, Contact<#= contactCount #>NonconvexAccumulatedImpulses, - ContactNonconvexTwoBodyFunctionsNonconvexPrestepData, Contact<#= contactCount #>NonconvexProjection, Contact<#= contactCount #>NonconvexAccumulatedImpulses>> + TwoBodyContactTypeProcessorNonconvexPrestepData, Contact<#= contactCount #>NonconvexAccumulatedImpulses, + ContactNonconvexTwoBodyFunctionsNonconvexPrestepData, Contact<#= contactCount #>NonconvexAccumulatedImpulses>> { //Matches UpdateConstraintForManifold's manifoldTypeAsConstraintType computation. public const int BatchTypeId = <#= 13 + contactCount #>; @@ -226,38 +202,14 @@ for (int i = 0; i < contactCount ; ++i) public readonly int ContactCount => <#= contactCount #>; public readonly int BodyCount => 1; - } - - public unsafe struct Contact<#= contactCount #>NonconvexOneBodyProjection : INonconvexOneBodyProjectionNonconvexOneBodyProjection> - { - public NonconvexOneBodyProjectionCommon Common; - //Nonprimitive fixed would be pretty convenient. -<# -for (int i = 0; i < contactCount ; ++i) -{#> - public ContactNonconvexOneBodyProjection Contact<#=i#>; -<#}#> - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref ContactNonconvexOneBodyProjection GetFirstContact(ref Contact<#= contactCount #>NonconvexOneBodyProjection projection) - { - return ref projection.Contact0; - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref NonconvexOneBodyProjectionCommon GetCommonProperties(ref Contact<#= contactCount #>NonconvexOneBodyProjection projection) - { - return ref projection.Common; - } - - public readonly int ContactCount => <#= contactCount #>; - } + } /// /// Handles the solve iterations of a bunch of <#= contactCount #>-contact nonconvex one body manifold constraints. /// public class Contact<#= contactCount #>NonconvexOneBodyTypeProcessor : - OneBodyContactTypeProcessorNonconvexOneBodyPrestepData, Contact<#= contactCount #>NonconvexOneBodyProjection, Contact<#= contactCount #>NonconvexAccumulatedImpulses, - ContactNonconvexOneBodyFunctionsNonconvexOneBodyPrestepData, Contact<#= contactCount #>NonconvexOneBodyProjection, Contact<#= contactCount #>NonconvexAccumulatedImpulses>> + OneBodyContactTypeProcessorNonconvexOneBodyPrestepData, Contact<#= contactCount #>NonconvexAccumulatedImpulses, + ContactNonconvexOneBodyFunctionsNonconvexOneBodyPrestepData, Contact<#= contactCount #>NonconvexAccumulatedImpulses>> { //Matches UpdateConstraintForManifold's manifoldTypeAsConstraintType computation. public const int BatchTypeId = <#= 6 + contactCount #>; diff --git a/BepuPhysics/Constraints/DistanceLimit.cs b/BepuPhysics/Constraints/DistanceLimit.cs index 340d612fa..fd147a225 100644 --- a/BepuPhysics/Constraints/DistanceLimit.cs +++ b/BepuPhysics/Constraints/DistanceLimit.cs @@ -100,66 +100,8 @@ public struct DistanceLimitPrestepData public SpringSettingsWide SpringSettings; } - public struct DistanceLimitProjection + public struct DistanceLimitFunctions : ITwoBodyConstraintFunctions> { - public Vector3Wide LinearVelocityToImpulseA; - public Vector3Wide AngularVelocityToImpulseA; - public Vector3Wide AngularVelocityToImpulseB; - public Vector BiasImpulse; - public Vector SoftnessImpulseScale; - public Vector3Wide LinearImpulseToVelocityA; - public Vector3Wide AngularImpulseToVelocityA; - public Vector3Wide LinearImpulseToVelocityB; - public Vector3Wide AngularImpulseToVelocityB; - } - - public struct DistanceLimitFunctions : ITwoBodyConstraintFunctions> - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, - float dt, float inverseDt, ref DistanceLimitPrestepData prestep, out DistanceLimitProjection projection) - { - DistanceServoFunctions.GetDistance(orientationA, ab, orientationB, prestep.LocalOffsetA, prestep.LocalOffsetB, - out var anchorOffsetA, out var anchorOffsetB, out var anchorOffset, out var distance); - //If the current distance is closer to the minimum, calibrate for the minimum. Otherwise, calibrate for the maximum. - var useMinimum = Vector.LessThan(Vector.Abs(distance - prestep.MinimumDistance), Vector.Abs(distance - prestep.MaximumDistance)); - var sign = Vector.ConditionalSelect(useMinimum, new Vector(-1f), Vector.One); - Vector3Wide.Scale(anchorOffset, sign / distance, out var direction); - DistanceServoFunctions.ComputeTransforms(inertiaA, inertiaB, anchorOffsetA, anchorOffsetB, distance, ref direction, dt, - prestep.SpringSettings, out var positionErrorToVelocity, out projection.SoftnessImpulseScale, out var effectiveMass, - out projection.LinearVelocityToImpulseA, out projection.AngularVelocityToImpulseA, out projection.AngularVelocityToImpulseB, - out projection.LinearImpulseToVelocityA, out projection.AngularImpulseToVelocityA, out projection.LinearImpulseToVelocityB, out projection.AngularImpulseToVelocityB); - - //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. - var error = Vector.ConditionalSelect(useMinimum, prestep.MinimumDistance - distance, distance - prestep.MaximumDistance); - InequalityHelpers.ComputeBiasVelocity(error, positionErrorToVelocity, inverseDt, out var biasVelocity); - projection.BiasImpulse = biasVelocity * effectiveMass; - - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref DistanceLimitProjection projection, ref Vector accumulatedImpulse) - { - DistanceServoFunctions.ApplyImpulse(ref velocityA, ref velocityB, - projection.LinearImpulseToVelocityA, projection.AngularImpulseToVelocityA, projection.LinearImpulseToVelocityB, projection.AngularImpulseToVelocityB, accumulatedImpulse); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref DistanceLimitProjection projection, ref Vector accumulatedImpulse) - { - //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); - Vector3Wide.Dot(velocityA.Linear, projection.LinearVelocityToImpulseA, out var linearCSIA); - Vector3Wide.Dot(velocityB.Linear, projection.LinearVelocityToImpulseA, out var negatedLinearCSIB); - Vector3Wide.Dot(velocityA.Angular, projection.AngularVelocityToImpulseA, out var angularCSIA); - Vector3Wide.Dot(velocityB.Angular, projection.AngularVelocityToImpulseB, out var angularCSIB); - var csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (linearCSIA + angularCSIA - negatedLinearCSIB + angularCSIB); - InequalityHelpers.ClampPositive(ref accumulatedImpulse, ref csi); - DistanceServoFunctions.ApplyImpulse(ref velocityA, ref velocityB, - projection.LinearImpulseToVelocityA, projection.AngularImpulseToVelocityA, projection.LinearImpulseToVelocityB, projection.AngularImpulseToVelocityB, csi); - - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ApplyImpulse(in Vector3Wide linearJacobianA, in Vector3Wide angularJacobianA, in Vector3Wide angularJacobianB, in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, in Vector csi, ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB) @@ -228,7 +170,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in ApplyImpulse(direction, angularJA, angularJB, inertiaA, inertiaB, csi, ref wsvA, ref wsvB); } - + public bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref DistanceLimitPrestepData prestepData) { } @@ -238,7 +180,7 @@ public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWi /// /// Handles the solve iterations of a bunch of distance servos. /// - public class DistanceLimitTypeProcessor : TwoBodyTypeProcessor, DistanceLimitFunctions, AccessAll, AccessAll, AccessAll, AccessAll> + public class DistanceLimitTypeProcessor : TwoBodyTypeProcessor, DistanceLimitFunctions, AccessAll, AccessAll, AccessAll, AccessAll> { public const int BatchTypeId = 34; } diff --git a/BepuPhysics/Constraints/DistanceServo.cs b/BepuPhysics/Constraints/DistanceServo.cs index e370f0959..4713d4774 100644 --- a/BepuPhysics/Constraints/DistanceServo.cs +++ b/BepuPhysics/Constraints/DistanceServo.cs @@ -104,21 +104,7 @@ public struct DistanceServoPrestepData public SpringSettingsWide SpringSettings; } - public struct DistanceServoProjection - { - public Vector3Wide LinearVelocityToImpulseA; - public Vector3Wide AngularVelocityToImpulseA; - public Vector3Wide AngularVelocityToImpulseB; - public Vector BiasImpulse; - public Vector SoftnessImpulseScale; - public Vector MaximumImpulse; - public Vector3Wide LinearImpulseToVelocityA; - public Vector3Wide AngularImpulseToVelocityA; - public Vector3Wide LinearImpulseToVelocityB; - public Vector3Wide AngularImpulseToVelocityB; - } - - public struct DistanceServoFunctions : ITwoBodyConstraintFunctions> + public struct DistanceServoFunctions : ITwoBodyConstraintFunctions> { public static void GetDistance(in QuaternionWide orientationA, in Vector3Wide ab, in QuaternionWide orientationB, in Vector3Wide localOffsetA, in Vector3Wide localOffsetB, out Vector3Wide anchorOffsetA, out Vector3Wide anchorOffsetB, out Vector3Wide anchorOffset, out Vector distance) @@ -131,116 +117,6 @@ public static void GetDistance(in QuaternionWide orientationA, in Vector3Wide ab Vector3Wide.Length(anchorOffset, out distance); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ComputeTransforms( - in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, in Vector3Wide anchorOffsetA, in Vector3Wide anchorOffsetB, in Vector distance, ref Vector3Wide direction, - float dt, in SpringSettingsWide springSettings, - out Vector positionErrorToVelocity, out Vector softnessImpulseScale, out Vector effectiveMass, - out Vector3Wide linearVelocityToImpulseA, out Vector3Wide angularVelocityToImpulseA, out Vector3Wide angularVelocityToImpulseB, - out Vector3Wide linearImpulseToVelocityA, out Vector3Wide angularImpulseToVelocityA, out Vector3Wide linearImpulseToVelocityB, out Vector3Wide angularImpulseToVelocityB) - { - //Position constraint: - //||positionA + localOffsetA * orientationA - positionB - localOffsetB * orientationB|| = distance - //Skipping a bunch of algebra, the velocity constraint applies to the change in velocity along the separating axis. - //dot(linearA + angularA x (localOffsetA * orientationA) - linearB - angularA x (localOffsetB * orientationB), normalize(positionA + localOffsetA * orientationA - positionB - localOffsetB * orientationB)) = 0 - //dot(linearA, direction) + dot(angularA x offsetA, direction) + dot(linearB, -direction) + dot(angularB x offsetB, -direction) = 0 - //dot(linearA, direction) + dot(offsetA x direction, angularA) + dot(linearB, -direction) + dot(offsetB x -direction, angularB) = 0 - //dot(linearA, direction) + dot(offsetA x direction, angularA) - dot(linearB, direction) + dot(direction x offsetB, angularB) = 0 - //Jacobians are direction, -direction, offsetA x direction, and direction x offsetB. - //That's 9 unique scalars. - //We can either store those 9 plus 14 for the inverse masses, or we can premultiply. - //V * JT * Me * J * I^-1 - //If you premultiply JT * Me and J * I^-1, you get 9 scalars for velocity->impulse and 12 for impulse->velocity. - //If you don't premultiply, it takes 9 for jacobians, 14 for inverse inertia, and then 1 for effective mass. - //That's 21 versus 24. On top of that, premultiplying saves some ALU work. - - //Note that we're working with the distance instead of distance squared. That makes it easier to use and reason about at the cost of a square root in the prestep. - //That really, really doesn't matter. - - //If the distance is zero, there is no valid offset direction. Pick one arbitrarily. - var needFallback = Vector.LessThan(distance, new Vector(1e-9f)); - direction.X = Vector.ConditionalSelect(needFallback, Vector.One, direction.X); - direction.Y = Vector.ConditionalSelect(needFallback, Vector.Zero, direction.Y); - direction.Z = Vector.ConditionalSelect(needFallback, Vector.Zero, direction.Z); - - Vector3Wide.CrossWithoutOverlap(anchorOffsetA, direction, out var angularJA); - Vector3Wide.CrossWithoutOverlap(direction, anchorOffsetB, out var angularJB); //Note flip negation. - - //The linear jacobian contributions are just a scalar multiplication by 1 since it's a unit length vector. - Symmetric3x3Wide.VectorSandwich(angularJA, inertiaA.InverseInertiaTensor, out var angularContributionA); - Symmetric3x3Wide.VectorSandwich(angularJB, inertiaB.InverseInertiaTensor, out var angularContributionB); - var inverseEffectiveMass = inertiaA.InverseMass + inertiaB.InverseMass + angularContributionA + angularContributionB; - - SpringSettingsWide.ComputeSpringiness(springSettings, dt, out positionErrorToVelocity, out var effectiveMassCFMScale, out softnessImpulseScale); - effectiveMass = effectiveMassCFMScale / inverseEffectiveMass; - Vector3Wide.Scale(direction, effectiveMass, out linearVelocityToImpulseA); - Vector3Wide.Scale(angularJA, effectiveMass, out angularVelocityToImpulseA); - Vector3Wide.Scale(angularJB, effectiveMass, out angularVelocityToImpulseB); - - Vector3Wide.Scale(direction, inertiaA.InverseMass, out linearImpulseToVelocityA); - Symmetric3x3Wide.TransformWithoutOverlap(angularJA, inertiaA.InverseInertiaTensor, out angularImpulseToVelocityA); - Vector3Wide.Scale(direction, -inertiaB.InverseMass, out linearImpulseToVelocityB); - Symmetric3x3Wide.TransformWithoutOverlap(angularJB, inertiaB.InverseInertiaTensor, out angularImpulseToVelocityB); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, - float dt, float inverseDt, ref DistanceServoPrestepData prestep, out DistanceServoProjection projection) - { - GetDistance(orientationA, ab, orientationB, prestep.LocalOffsetA, prestep.LocalOffsetB, out var anchorOffsetA, out var anchorOffsetB, out var anchorOffset, out var distance); - - Vector3Wide.Scale(anchorOffset, Vector.One / distance, out var direction); - - ComputeTransforms(inertiaA, inertiaB, anchorOffsetA, anchorOffsetB, distance, ref direction, dt, prestep.SpringSettings, - out var positionErrorToVelocity, out projection.SoftnessImpulseScale, out var effectiveMass, - out projection.LinearVelocityToImpulseA, out projection.AngularVelocityToImpulseA, out projection.AngularVelocityToImpulseB, - out projection.LinearImpulseToVelocityA, out projection.AngularImpulseToVelocityA, out projection.LinearImpulseToVelocityB, out projection.AngularImpulseToVelocityB); - - //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. - var error = distance - prestep.TargetDistance; - ServoSettingsWide.ComputeClampedBiasVelocity(error, positionErrorToVelocity, prestep.ServoSettings, dt, inverseDt, out var clampedBiasVelocity, out projection.MaximumImpulse); - projection.BiasImpulse = clampedBiasVelocity * effectiveMass; - - } - - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ApplyImpulse(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, - in Vector3Wide linearImpulseToVelocityA, in Vector3Wide angularImpulseToVelocityA, in Vector3Wide linearImpulseToVelocityB, in Vector3Wide angularImpulseToVelocityB, - in Vector csi) - { - Vector3Wide.Scale(linearImpulseToVelocityA, csi, out var linearVelocityChangeA); - Vector3Wide.Scale(angularImpulseToVelocityA, csi, out var angularVelocityChangeA); - Vector3Wide.Add(linearVelocityChangeA, velocityA.Linear, out velocityA.Linear); - Vector3Wide.Add(angularVelocityChangeA, velocityA.Angular, out velocityA.Angular); - Vector3Wide.Scale(linearImpulseToVelocityB, csi, out var linearVelocityChangeB); - Vector3Wide.Scale(angularImpulseToVelocityB, csi, out var angularVelocityChangeB); - Vector3Wide.Add(linearVelocityChangeB, velocityB.Linear, out velocityB.Linear); - Vector3Wide.Add(angularVelocityChangeB, velocityB.Angular, out velocityB.Angular); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref DistanceServoProjection projection, ref Vector accumulatedImpulse) - { - ApplyImpulse(ref velocityA, ref velocityB, - projection.LinearImpulseToVelocityA, projection.AngularImpulseToVelocityA, projection.LinearImpulseToVelocityB, projection.AngularImpulseToVelocityB, accumulatedImpulse); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref DistanceServoProjection projection, ref Vector accumulatedImpulse) - { - //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); - Vector3Wide.Dot(velocityA.Linear, projection.LinearVelocityToImpulseA, out var linearCSIA); - Vector3Wide.Dot(velocityB.Linear, projection.LinearVelocityToImpulseA, out var negatedLinearCSIB); - Vector3Wide.Dot(velocityA.Angular, projection.AngularVelocityToImpulseA, out var angularCSIA); - Vector3Wide.Dot(velocityB.Angular, projection.AngularVelocityToImpulseB, out var angularCSIB); - var csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (linearCSIA + angularCSIA - negatedLinearCSIB + angularCSIB); - ServoSettingsWide.ClampImpulse(projection.MaximumImpulse, ref accumulatedImpulse, ref csi); - - ApplyImpulse(ref velocityA, ref velocityB, - projection.LinearImpulseToVelocityA, projection.AngularImpulseToVelocityA, projection.LinearImpulseToVelocityB, projection.AngularImpulseToVelocityB, csi); - } - public static void ComputeJacobian(in Vector distance, in Vector3Wide anchorOffsetA, in Vector3Wide anchorOffsetB, ref Vector3Wide direction, out Vector3Wide angularJA, out Vector3Wide angularJB) { //If the distance is zero, there is no valid offset direction. Pick one arbitrarily. @@ -349,7 +225,7 @@ public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWi /// /// Handles the solve iterations of a bunch of distance servos. /// - public class DistanceServoTypeProcessor : TwoBodyTypeProcessor, DistanceServoFunctions, AccessAll, AccessAll, AccessAll, AccessAll> + public class DistanceServoTypeProcessor : TwoBodyTypeProcessor, DistanceServoFunctions, AccessAll, AccessAll, AccessAll, AccessAll> { public const int BatchTypeId = 33; } diff --git a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs index 6a29403e9..d1b5b3090 100644 --- a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs @@ -24,17 +24,8 @@ public struct FourBodyReferences /// /// Type of the prestep data used by the constraint. /// Type of the accumulated impulses used by the constraint. - /// Type of the projection to input. - public interface IFourBodyConstraintFunctions + public interface IFourBodyConstraintFunctions { - void Prestep( - in QuaternionWide orientationA, in BodyInertiaWide inertiaA, - in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, - in Vector3Wide ac, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, - in Vector3Wide ad, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, - float dt, float inverseDt, ref TPrestepData prestepData, out TProjection projection); - void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref BodyVelocityWide velocityC, ref BodyVelocityWide velocityD, ref TProjection projection, ref TAccumulatedImpulse accumulatedImpulse); - void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref BodyVelocityWide velocityC, ref BodyVelocityWide velocityD, ref TProjection projection, ref TAccumulatedImpulse accumulatedImpulse); void WarmStart2( in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, @@ -58,11 +49,11 @@ void Solve2( /// /// Shared implementation across all four body constraints. /// - public abstract class FourBodyTypeProcessor - : TypeProcessor - where TPrestepData : unmanaged where TProjection : unmanaged where TAccumulatedImpulse : unmanaged - where TConstraintFunctions : unmanaged, IFourBodyConstraintFunctions + : TypeProcessor + where TPrestepData : unmanaged where TAccumulatedImpulse : unmanaged + where TConstraintFunctions : unmanaged, IFourBodyConstraintFunctions where TWarmStartAccessFilterA : unmanaged, IBodyAccessFilter where TWarmStartAccessFilterB : unmanaged, IBodyAccessFilter where TWarmStartAccessFilterC : unmanaged, IBodyAccessFilter @@ -113,75 +104,6 @@ internal sealed override void VerifySortRegion(ref TypeBatch typeBatch, int bund VerifySortRegion(ref typeBatch, bundleStartIndex, constraintCount, ref sortedKeys, ref sortedSourceIndices); } - public unsafe override void Prestep(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) - { - ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); - ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); - ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); - var function = default(TConstraintFunctions); - for (int i = startBundle; i < exclusiveEndBundle; ++i) - { - ref var prestep = ref Unsafe.Add(ref prestepBase, i); - ref var projection = ref Unsafe.Add(ref projectionBase, i); - ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); - bodies.GatherState(ref references, - out var orientationA, out var wsvA, out var inertiaA, - out var ab, out var orientationB, out var wsvB, out var inertiaB, - out var ac, out var orientationC, out var wsvC, out var inertiaC, - out var ad, out var orientationD, out var wsvD, out var inertiaD); - function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, ac, orientationC, inertiaC, ad, orientationD, inertiaD, dt, inverseDt, ref prestep, out projection); - } - } - - public unsafe override void WarmStart(ref TypeBatch typeBatch, Bodies bodies, int startBundle, int exclusiveEndBundle) - { - ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); - ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); - ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); - var function = default(TConstraintFunctions); - for (int i = startBundle; i < exclusiveEndBundle; ++i) - { - ref var projection = ref Unsafe.Add(ref projectionBase, i); - ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); - ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); - bodies.GatherState(ref bodyReferences, - out var orientationA, out var wsvA, out var inertiaA, - out var ab, out var orientationB, out var wsvB, out var inertiaB, - out var ac, out var orientationC, out var wsvC, out var inertiaC, - out var ad, out var orientationD, out var wsvD, out var inertiaD); - function.WarmStart(ref wsvA, ref wsvB, ref wsvC, ref wsvD, ref projection, ref accumulatedImpulses); - bodies.ScatterVelocities(ref wsvA, ref bodyReferences.IndexA); - bodies.ScatterVelocities(ref wsvB, ref bodyReferences.IndexB); - bodies.ScatterVelocities(ref wsvC, ref bodyReferences.IndexC); - bodies.ScatterVelocities(ref wsvD, ref bodyReferences.IndexD); - - } - } - - public unsafe override void SolveIteration(ref TypeBatch typeBatch, Bodies bodies, int startBundle, int exclusiveEndBundle) - { - ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); - ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); - ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); - var function = default(TConstraintFunctions); - for (int i = startBundle; i < exclusiveEndBundle; ++i) - { - ref var projection = ref Unsafe.Add(ref projectionBase, i); - ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); - ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); - bodies.GatherState(ref bodyReferences, - out var orientationA, out var wsvA, out var inertiaA, - out var ab, out var orientationB, out var wsvB, out var inertiaB, - out var ac, out var orientationC, out var wsvC, out var inertiaC, - out var ad, out var orientationD, out var wsvD, out var inertiaD); - function.Solve(ref wsvA, ref wsvB, ref wsvC, ref wsvD, ref projection, ref accumulatedImpulses); - bodies.ScatterVelocities(ref wsvA, ref bodyReferences.IndexA); - bodies.ScatterVelocities(ref wsvB, ref bodyReferences.IndexB); - bodies.ScatterVelocities(ref wsvC, ref bodyReferences.IndexC); - bodies.ScatterVelocities(ref wsvD, ref bodyReferences.IndexD); - } - } - public unsafe override void WarmStart2( ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) diff --git a/BepuPhysics/Constraints/Hinge.cs b/BepuPhysics/Constraints/Hinge.cs index 02177052f..297c286e4 100644 --- a/BepuPhysics/Constraints/Hinge.cs +++ b/BepuPhysics/Constraints/Hinge.cs @@ -80,153 +80,16 @@ public struct HingePrestepData public SpringSettingsWide SpringSettings; } - public struct HingeProjection - { - public Vector3Wide OffsetA; - public Vector3Wide OffsetB; - public Matrix2x3Wide HingeJacobian; - public Vector3Wide BallSocketBiasVelocity; - public Vector2Wide HingeBiasVelocity; - public Symmetric5x5Wide EffectiveMass; - public Vector SoftnessImpulseScale; - public BodyInertiaWide InertiaA; - public BodyInertiaWide InertiaB; - } - public struct HingeAccumulatedImpulses { public Vector3Wide BallSocket; public Vector2Wide Hinge; } - public struct HingeFunctions : ITwoBodyConstraintFunctions + public struct HingeFunctions : ITwoBodyConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, - float dt, float inverseDt, ref HingePrestepData prestep, out HingeProjection projection) - { - projection.InertiaA = inertiaA; - projection.InertiaB = inertiaB; - - //5x12 jacobians, from BallSocket and AngularHinge: - //[ I, skew(offsetA), -I, -skew(offsetB) ] - //[ 0, constraintAxisAX, 0, -constraintAxisAX ] - //[ 0, constraintAxisAY, 0, -constraintAxisAY ] - - Matrix3x3Wide.CreateFromQuaternion(orientationA, out var orientationMatrixA); - Matrix3x3Wide.CreateFromQuaternion(orientationB, out var orientationMatrixB); - Matrix3x3Wide.TransformWithoutOverlap(prestep.LocalOffsetA, orientationMatrixA, out projection.OffsetA); - Matrix3x3Wide.TransformWithoutOverlap(prestep.LocalHingeAxisA, orientationMatrixA, out var hingeAxisA); - Matrix3x3Wide.TransformWithoutOverlap(prestep.LocalOffsetB, orientationMatrixB, out projection.OffsetB); - Matrix3x3Wide.TransformWithoutOverlap(prestep.LocalHingeAxisB, orientationMatrixB, out var hingeAxisB); - Helpers.BuildOrthonormalBasis(prestep.LocalHingeAxisA, out var localAX, out var localAY); - Matrix3x3Wide.TransformWithoutOverlap(localAX, orientationMatrixA, out projection.HingeJacobian.X); - Matrix3x3Wide.TransformWithoutOverlap(localAY, orientationMatrixA, out projection.HingeJacobian.Y); - - //The upper left 3x3 block is just the ball socket. - Symmetric3x3Wide.SkewSandwichWithoutOverlap(projection.OffsetA, inertiaA.InverseInertiaTensor, out var ballSocketContributionAngularA); - Symmetric3x3Wide.SkewSandwichWithoutOverlap(projection.OffsetB, inertiaB.InverseInertiaTensor, out var ballSocketContributionAngularB); - Symmetric5x5Wide inverseEffectiveMass; - Symmetric3x3Wide.Add(ballSocketContributionAngularA, ballSocketContributionAngularB, out inverseEffectiveMass.A); - var linearContribution = projection.InertiaA.InverseMass + projection.InertiaB.InverseMass; - inverseEffectiveMass.A.XX += linearContribution; - inverseEffectiveMass.A.YY += linearContribution; - inverseEffectiveMass.A.ZZ += linearContribution; - - //The lower right 2x2 block is the AngularHinge. - Symmetric3x3Wide.MultiplyWithoutOverlap(projection.HingeJacobian, inertiaA.InverseInertiaTensor, out var hingeInertiaA); - Symmetric3x3Wide.MultiplyWithoutOverlap(projection.HingeJacobian, inertiaB.InverseInertiaTensor, out var hingeInertiaB); - Symmetric2x2Wide.CompleteMatrixSandwich(hingeInertiaA, projection.HingeJacobian, out var hingeContributionAngularA); - Symmetric2x2Wide.CompleteMatrixSandwich(hingeInertiaB, projection.HingeJacobian, out var hingeContributionAngularB); - Symmetric2x2Wide.Add(hingeContributionAngularA, hingeContributionAngularB, out inverseEffectiveMass.D); - - //The remaining off-diagonal region is skew(offsetA) * Ia^-1 * hingeJacobianV + skew(offsetB) * Ib^-1 * hingeJacobianA - //skew(offsetA) * (Ia^-1 * hingeJacobianA) = [ (Ia^-1 * hingeJacobianA.X) x offsetA ] - // [ (Ia^-1 * hingeJacobianA.Y) x offsetA ] - //Careful with cross order/signs! - Vector3Wide.CrossWithoutOverlap(hingeInertiaA.X, projection.OffsetA, out var offDiagonalContributionAX); - Vector3Wide.CrossWithoutOverlap(hingeInertiaA.Y, projection.OffsetA, out var offDiagonalContributionAY); - Vector3Wide.CrossWithoutOverlap(hingeInertiaB.X, projection.OffsetB, out var offDiagonalContributionBX); - Vector3Wide.CrossWithoutOverlap(hingeInertiaB.Y, projection.OffsetB, out var offDiagonalContributionBY); - Vector3Wide.Add(offDiagonalContributionAX, offDiagonalContributionBX, out inverseEffectiveMass.B.X); - Vector3Wide.Add(offDiagonalContributionAY, offDiagonalContributionBY, out inverseEffectiveMass.B.Y); - - Symmetric5x5Wide.InvertWithoutOverlap(inverseEffectiveMass, out projection.EffectiveMass); - SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); - Symmetric5x5Wide.Scale(projection.EffectiveMass, effectiveMassCFMScale, out projection.EffectiveMass); - - //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. - Vector3Wide.Add(ab, projection.OffsetB, out var anchorB); - Vector3Wide.Subtract(anchorB, projection.OffsetA, out var ballSocketError); - Vector3Wide.Scale(ballSocketError, positionErrorToVelocity, out projection.BallSocketBiasVelocity); - - AngularHingeFunctions.GetErrorAngles(hingeAxisA, hingeAxisB, projection.HingeJacobian, out var errorAngles); - //Note the negation: we want to oppose the separation. TODO: arguably, should bake the negation into positionErrorToVelocity, given its name. - Vector2Wide.Scale(errorAngles, -positionErrorToVelocity, out projection.HingeBiasVelocity); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - private static void ApplyImpulse(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref HingeProjection projection, ref HingeAccumulatedImpulses csi) - { - //[ csi ] * [ I, skew(offsetA), -I, -skew(offsetB) ] - // [ 0, constraintAxisAX, 0, -constraintAxisAX ] - // [ 0, constraintAxisAY, 0, -constraintAxisAY ] - Vector3Wide.Scale(csi.BallSocket, projection.InertiaA.InverseMass, out var linearChangeA); - Vector3Wide.Add(velocityA.Linear, linearChangeA, out velocityA.Linear); - - Vector3Wide.CrossWithoutOverlap(projection.OffsetA, csi.BallSocket, out var ballSocketAngularImpulseA); - Matrix2x3Wide.Transform(csi.Hinge, projection.HingeJacobian, out var hingeAngularImpulseA); - Vector3Wide.Add(ballSocketAngularImpulseA, hingeAngularImpulseA, out var angularImpulseA); - Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseA, projection.InertiaA.InverseInertiaTensor, out var angularChangeA); - Vector3Wide.Add(velocityA.Angular, angularChangeA, out velocityA.Angular); - - //Note cross order flip for negation. - Vector3Wide.Scale(csi.BallSocket, projection.InertiaB.InverseMass, out var negatedLinearChangeB); - Vector3Wide.Subtract(velocityB.Linear, negatedLinearChangeB, out velocityB.Linear); - Vector3Wide.CrossWithoutOverlap(csi.BallSocket, projection.OffsetB, out var ballSocketAngularImpulseB); - Vector3Wide.Subtract(ballSocketAngularImpulseB, hingeAngularImpulseA, out var angularImpulseB); - Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseB, projection.InertiaB.InverseInertiaTensor, out var angularChangeB); - Vector3Wide.Add(velocityB.Angular, angularChangeB, out velocityB.Angular); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref HingeProjection projection, ref HingeAccumulatedImpulses accumulatedImpulse) - { - ApplyImpulse(ref velocityA, ref velocityB, ref projection, ref accumulatedImpulse); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref HingeProjection projection, ref HingeAccumulatedImpulses accumulatedImpulse) - { - //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); - // [ I, skew(offsetA), -I, -skew(offsetB) ] - //J = [ 0, constraintAxisAX, 0, -constraintAxisAX ] - // [ 0, constraintAxisAY, 0, -constraintAxisAY ] - Vector3Wide.CrossWithoutOverlap(velocityA.Angular, projection.OffsetA, out var ballSocketAngularCSVA); - Matrix2x3Wide.TransformByTransposeWithoutOverlap(velocityA.Angular, projection.HingeJacobian, out var hingeCSVA); - Vector3Wide.CrossWithoutOverlap(projection.OffsetB, velocityB.Angular, out var ballSocketAngularCSVB); - Matrix2x3Wide.TransformByTransposeWithoutOverlap(velocityB.Angular, projection.HingeJacobian, out var negatedHingeCSVB); - - Vector3Wide.Add(ballSocketAngularCSVA, ballSocketAngularCSVB, out var ballSocketAngularCSV); - Vector3Wide.Subtract(velocityA.Linear, velocityB.Linear, out var ballSocketLinearCSV); - Vector3Wide.Add(ballSocketAngularCSV, ballSocketLinearCSV, out var ballSocketCSV); - Vector3Wide.Subtract(projection.BallSocketBiasVelocity, ballSocketCSV, out ballSocketCSV); - Vector2Wide.Subtract(hingeCSVA, negatedHingeCSVB, out var hingeCSV); - Vector2Wide.Subtract(projection.HingeBiasVelocity, hingeCSV, out hingeCSV); - - HingeAccumulatedImpulses csi; - Symmetric5x5Wide.TransformWithoutOverlap(ballSocketCSV, hingeCSV, projection.EffectiveMass, out csi.BallSocket, out csi.Hinge); - Vector3Wide.Scale(accumulatedImpulse.BallSocket, projection.SoftnessImpulseScale, out var ballSocketSoftnessContribution); - Vector3Wide.Subtract(csi.BallSocket, ballSocketSoftnessContribution, out csi.BallSocket); - Vector2Wide.Scale(accumulatedImpulse.Hinge, projection.SoftnessImpulseScale, out var hingeSoftnessContribution); - Vector2Wide.Subtract(csi.Hinge, hingeSoftnessContribution, out csi.Hinge); - - ApplyImpulse(ref velocityA, ref velocityB, ref projection, ref csi); - } - - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - private static void ApplyImpulse(in Vector3Wide offsetA, in Vector3Wide offsetB, in Matrix2x3Wide hingeJacobian, in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, in HingeAccumulatedImpulses csi, + private static void ApplyImpulse(in Vector3Wide offsetA, in Vector3Wide offsetB, in Matrix2x3Wide hingeJacobian, in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, in HingeAccumulatedImpulses csi, ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB) { //[ csi ] * [ I, skew(offsetA), -I, -skew(offsetB) ] @@ -358,7 +221,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref HingePrestepData prestepData) { } } - public class HingeTypeProcessor : TwoBodyTypeProcessor + public class HingeTypeProcessor : TwoBodyTypeProcessor { public const int BatchTypeId = 47; } diff --git a/BepuPhysics/Constraints/LinearAxisLimit.cs b/BepuPhysics/Constraints/LinearAxisLimit.cs index a3e8812ac..14e7e4d35 100644 --- a/BepuPhysics/Constraints/LinearAxisLimit.cs +++ b/BepuPhysics/Constraints/LinearAxisLimit.cs @@ -87,69 +87,8 @@ public struct LinearAxisLimitPrestepData public SpringSettingsWide SpringSettings; } - public struct LinearAxisLimitFunctions : ITwoBodyConstraintFunctions> + public struct LinearAxisLimitFunctions : ITwoBodyConstraintFunctions> { - public struct LimitJacobianModifier : LinearAxisServoFunctions.IJacobianModifier - { - public Vector MinimumOffset; - public Vector MaximumOffset; - public Vector Error; - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Modify(in Vector3Wide anchorA, in Vector3Wide anchorB, ref Vector3Wide normal) - { - Vector3Wide.Subtract(anchorB, anchorA, out var anchorOffset); - Vector3Wide.Dot(anchorOffset, normal, out var planeNormalDot); - - var minimumError = MinimumOffset - planeNormalDot; - var maximumError = planeNormalDot - MaximumOffset; - var useMin = Vector.LessThan(Vector.Abs(minimumError), Vector.Abs(maximumError)); - - Error = Vector.ConditionalSelect(useMin, minimumError, maximumError); - normal.X = Vector.ConditionalSelect(useMin, -normal.X, normal.X); - normal.Y = Vector.ConditionalSelect(useMin, -normal.Y, normal.Y); - normal.Z = Vector.ConditionalSelect(useMin, -normal.Z, normal.Z); - } - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, - float dt, float inverseDt, ref LinearAxisLimitPrestepData prestep, out LinearAxisServoProjection projection) - { - Unsafe.SkipInit(out projection); - SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); - LimitJacobianModifier modifier; - modifier.MinimumOffset = prestep.MinimumOffset; - modifier.MaximumOffset = prestep.MaximumOffset; - modifier.Error = default; - LinearAxisServoFunctions.ComputeTransforms(ref modifier, prestep.LocalOffsetA, prestep.LocalOffsetB, prestep.LocalPlaneNormal, orientationA, inertiaA, ab, orientationB, inertiaB, effectiveMassCFMScale, - out var anchorA, out var anchorB, out var normal, out var effectiveMass, - out projection.LinearVelocityToImpulseA, out projection.AngularVelocityToImpulseA, out projection.AngularVelocityToImpulseB, - out projection.LinearImpulseToVelocityA, out projection.AngularImpulseToVelocityA, out projection.NegatedLinearImpulseToVelocityB, out projection.AngularImpulseToVelocityB); - - InequalityHelpers.ComputeBiasVelocity(modifier.Error, positionErrorToVelocity, inverseDt, out var biasVelocity); - projection.BiasImpulse = biasVelocity * effectiveMass; - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref LinearAxisServoProjection projection, ref Vector accumulatedImpulse) - { - LinearAxisServoFunctions.ApplyImpulse(ref velocityA, ref velocityB, - projection.LinearImpulseToVelocityA, projection.AngularImpulseToVelocityA, projection.NegatedLinearImpulseToVelocityB, projection.AngularImpulseToVelocityB, - ref accumulatedImpulse); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref LinearAxisServoProjection projection, ref Vector accumulatedImpulse) - { - LinearAxisServoFunctions.ComputeCorrectiveImpulse(ref velocityA, ref velocityB, projection.LinearVelocityToImpulseA, projection.AngularVelocityToImpulseA, projection.AngularVelocityToImpulseB, - projection.BiasImpulse, projection.SoftnessImpulseScale, accumulatedImpulse, out var csi); - InequalityHelpers.ClampPositive(ref accumulatedImpulse, ref csi); - LinearAxisServoFunctions.ApplyImpulse(ref velocityA, ref velocityB, - projection.LinearImpulseToVelocityA, projection.AngularImpulseToVelocityA, projection.NegatedLinearImpulseToVelocityB, projection.AngularImpulseToVelocityB, - ref csi); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] static void ComputeJacobians( in Vector3Wide ab, in QuaternionWide orientationA, in QuaternionWide orientationB, in Vector3Wide localPlaneNormal, in Vector3Wide localOffsetA, in Vector3Wide localOffsetB, in Vector minimumOffset, in Vector maximumOffset, @@ -211,7 +150,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref LinearAxisLimitPrestepData prestepData) { } } - public class LinearAxisLimitTypeProcessor : TwoBodyTypeProcessor, LinearAxisLimitFunctions, AccessAll, AccessAll, AccessAll, AccessAll> + public class LinearAxisLimitTypeProcessor : TwoBodyTypeProcessor, LinearAxisLimitFunctions, AccessAll, AccessAll, AccessAll, AccessAll> { public const int BatchTypeId = 40; } diff --git a/BepuPhysics/Constraints/LinearAxisMotor.cs b/BepuPhysics/Constraints/LinearAxisMotor.cs index f281f8fc3..5c43e4650 100644 --- a/BepuPhysics/Constraints/LinearAxisMotor.cs +++ b/BepuPhysics/Constraints/LinearAxisMotor.cs @@ -79,41 +79,8 @@ public struct LinearAxisMotorPrestepData public MotorSettingsWide Settings; } - public struct LinearAxisMotorFunctions : ITwoBodyConstraintFunctions> + public struct LinearAxisMotorFunctions : ITwoBodyConstraintFunctions> { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, - float dt, float inverseDt, ref LinearAxisMotorPrestepData prestep, out LinearAxisServoProjection projection) - { - MotorSettingsWide.ComputeSoftness(prestep.Settings, dt, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale, out projection.MaximumImpulse); - var modifier = new LinearAxisServoFunctions.NoChangeModifier(); - LinearAxisServoFunctions.ComputeTransforms(ref modifier, prestep.LocalOffsetA, prestep.LocalOffsetB, prestep.LocalPlaneNormal, orientationA, inertiaA, ab, orientationB, inertiaB, effectiveMassCFMScale, - out _, out _, out _, out var effectiveMass, - out projection.LinearVelocityToImpulseA, out projection.AngularVelocityToImpulseA, out projection.AngularVelocityToImpulseB, - out projection.LinearImpulseToVelocityA, out projection.AngularImpulseToVelocityA, out projection.NegatedLinearImpulseToVelocityB, out projection.AngularImpulseToVelocityB); - - projection.BiasImpulse = -prestep.TargetVelocity * effectiveMass; - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref LinearAxisServoProjection projection, ref Vector accumulatedImpulse) - { - LinearAxisServoFunctions.ApplyImpulse(ref velocityA, ref velocityB, - projection.LinearImpulseToVelocityA, projection.AngularImpulseToVelocityA, projection.NegatedLinearImpulseToVelocityB, projection.AngularImpulseToVelocityB, - ref accumulatedImpulse); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref LinearAxisServoProjection projection, ref Vector accumulatedImpulse) - { - LinearAxisServoFunctions.ComputeCorrectiveImpulse(ref velocityA, ref velocityB, projection.LinearVelocityToImpulseA, projection.AngularVelocityToImpulseA, projection.AngularVelocityToImpulseB, - projection.BiasImpulse, projection.SoftnessImpulseScale, accumulatedImpulse, out var csi); - ServoSettingsWide.ClampImpulse(projection.MaximumImpulse, ref accumulatedImpulse, ref csi); - LinearAxisServoFunctions.ApplyImpulse(ref velocityA, ref velocityB, - projection.LinearImpulseToVelocityA, projection.AngularImpulseToVelocityA, projection.NegatedLinearImpulseToVelocityB, projection.AngularImpulseToVelocityB, - ref csi); - } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref LinearAxisMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { LinearAxisServoFunctions.ComputeJacobians(positionB - positionA, orientationA, orientationB, prestep.LocalPlaneNormal, prestep.LocalOffsetA, prestep.LocalOffsetB, out _, out var normal, out var angularJA, out var angularJB); @@ -142,7 +109,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref LinearAxisMotorPrestepData prestepData) { } } - public class LinearAxisMotorTypeProcessor : TwoBodyTypeProcessor, LinearAxisMotorFunctions, AccessAll, AccessAll, AccessAll, AccessAll> + public class LinearAxisMotorTypeProcessor : TwoBodyTypeProcessor, LinearAxisMotorFunctions, AccessAll, AccessAll, AccessAll, AccessAll> { public const int BatchTypeId = 39; } diff --git a/BepuPhysics/Constraints/LinearAxisServo.cs b/BepuPhysics/Constraints/LinearAxisServo.cs index 624d5ad7a..eb7a782f1 100644 --- a/BepuPhysics/Constraints/LinearAxisServo.cs +++ b/BepuPhysics/Constraints/LinearAxisServo.cs @@ -86,21 +86,7 @@ public struct LinearAxisServoPrestepData public SpringSettingsWide SpringSettings; } - public struct LinearAxisServoProjection - { - public Vector3Wide LinearVelocityToImpulseA; - public Vector3Wide AngularVelocityToImpulseA; - public Vector3Wide AngularVelocityToImpulseB; - public Vector BiasImpulse; - public Vector SoftnessImpulseScale; - public Vector MaximumImpulse; - public Vector3Wide LinearImpulseToVelocityA; - public Vector3Wide NegatedLinearImpulseToVelocityB; - public Vector3Wide AngularImpulseToVelocityA; - public Vector3Wide AngularImpulseToVelocityB; - } - - public struct LinearAxisServoFunctions : ITwoBodyConstraintFunctions> + public struct LinearAxisServoFunctions : ITwoBodyConstraintFunctions> { public interface IJacobianModifier { @@ -160,25 +146,6 @@ public static void ComputeTransforms(ref TJacobianModifier ja Vector3Wide.Scale(angularB, effectiveMass, out angularVelocityToImpulseB); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, - float dt, float inverseDt, ref LinearAxisServoPrestepData prestep, out LinearAxisServoProjection projection) - { - SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); - var modifier = new NoChangeModifier(); - ComputeTransforms(ref modifier, prestep.LocalOffsetA, prestep.LocalOffsetB, prestep.LocalPlaneNormal, orientationA, inertiaA, ab, orientationB, inertiaB, effectiveMassCFMScale, - out var anchorA, out var anchorB, out var normal, out var effectiveMass, - out projection.LinearVelocityToImpulseA, out projection.AngularVelocityToImpulseA, out projection.AngularVelocityToImpulseB, - out projection.LinearImpulseToVelocityA, out projection.AngularImpulseToVelocityA, out projection.NegatedLinearImpulseToVelocityB, out projection.AngularImpulseToVelocityB); - - Vector3Wide.Subtract(anchorB, anchorA, out var anchorOffset); - Vector3Wide.Dot(anchorOffset, normal, out var planeNormalDot); - - //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. - ServoSettingsWide.ComputeClampedBiasVelocity(planeNormalDot - prestep.TargetOffset, positionErrorToVelocity, prestep.ServoSettings, dt, inverseDt, out projection.BiasImpulse, out projection.MaximumImpulse); - projection.BiasImpulse *= effectiveMass; - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ApplyImpulse(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, @@ -195,14 +162,6 @@ public static void ApplyImpulse(ref BodyVelocityWide velocityA, ref BodyVelocity Vector3Wide.Add(angularChangeB, velocityB.Angular, out velocityB.Angular); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref LinearAxisServoProjection projection, ref Vector accumulatedImpulse) - { - ApplyImpulse(ref velocityA, ref velocityB, - projection.LinearImpulseToVelocityA, projection.AngularImpulseToVelocityA, projection.NegatedLinearImpulseToVelocityB, projection.AngularImpulseToVelocityB, - ref accumulatedImpulse); - } - public static void ComputeCorrectiveImpulse(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, in Vector3Wide linearVelocityToImpulseA, in Vector3Wide angularVelocityToImpulseA, in Vector3Wide angularVelocityToImpulseB, in Vector biasImpulse, in Vector softnessImpulseScale, in Vector accumulatedImpulse, out Vector csi) @@ -216,16 +175,6 @@ public static void ComputeCorrectiveImpulse(ref BodyVelocityWide velocityA, ref csi = biasImpulse - accumulatedImpulse * softnessImpulseScale - (linearA + angularA - negatedLinearB + angularB); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref LinearAxisServoProjection projection, ref Vector accumulatedImpulse) - { - ComputeCorrectiveImpulse(ref velocityA, ref velocityB, projection.LinearVelocityToImpulseA, projection.AngularVelocityToImpulseA, projection.AngularVelocityToImpulseB, - projection.BiasImpulse, projection.SoftnessImpulseScale, accumulatedImpulse, out var csi); - ServoSettingsWide.ClampImpulse(projection.MaximumImpulse, ref accumulatedImpulse, ref csi); - ApplyImpulse(ref velocityA, ref velocityB, - projection.LinearImpulseToVelocityA, projection.AngularImpulseToVelocityA, projection.NegatedLinearImpulseToVelocityB, projection.AngularImpulseToVelocityB, - ref csi); - } [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ApplyImpulse(in Vector3Wide linearJA, in Vector3Wide angularImpulseToVelocityA, in Vector3Wide angularImpulseToVelocityB, in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, @@ -298,7 +247,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref LinearAxisServoPrestepData prestepData) { } } - public class LinearAxisServoTypeProcessor : TwoBodyTypeProcessor, LinearAxisServoFunctions, AccessAll, AccessAll, AccessAll, AccessAll> + public class LinearAxisServoTypeProcessor : TwoBodyTypeProcessor, LinearAxisServoFunctions, AccessAll, AccessAll, AccessAll, AccessAll> { public const int BatchTypeId = 38; } diff --git a/BepuPhysics/Constraints/OneBodyAngularMotor.cs b/BepuPhysics/Constraints/OneBodyAngularMotor.cs index 7d9e61a1a..03768f6ce 100644 --- a/BepuPhysics/Constraints/OneBodyAngularMotor.cs +++ b/BepuPhysics/Constraints/OneBodyAngularMotor.cs @@ -57,22 +57,8 @@ public struct OneBodyAngularMotorPrestepData public MotorSettingsWide Settings; } - public struct OneBodyAngularMotorFunctions : IOneBodyConstraintFunctions + public struct OneBodyAngularMotorFunctions : IOneBodyConstraintFunctions { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, - float dt, float inverseDt, ref OneBodyAngularMotorPrestepData prestep, out OneBodyAngularServoProjection projection) - { - projection.ImpulseToVelocity = inertiaA.InverseInertiaTensor; - - //Jacobians are just the identity matrix. - MotorSettingsWide.ComputeSoftness(prestep.Settings, dt, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale, out projection.MaximumImpulse); - Symmetric3x3Wide.Invert(inertiaA.InverseInertiaTensor, out projection.VelocityToImpulse); - Symmetric3x3Wide.Scale(projection.VelocityToImpulse, effectiveMassCFMScale, out projection.VelocityToImpulse); - - Symmetric3x3Wide.TransformWithoutOverlap(prestep.TargetVelocity, projection.VelocityToImpulse, out projection.BiasImpulse); - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ApplyImpulse(ref Vector3Wide angularVelocity, in Symmetric3x3Wide impulseToVelocity, in Vector3Wide csi) { @@ -80,36 +66,6 @@ public static void ApplyImpulse(ref Vector3Wide angularVelocity, in Symmetric3x3 Vector3Wide.Add(angularVelocity, velocityChange, out angularVelocity); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocityWide velocityA, ref OneBodyAngularServoProjection projection, ref Vector3Wide accumulatedImpulse) - { - ApplyImpulse(ref velocityA.Angular, projection.ImpulseToVelocity, accumulatedImpulse); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Solve(ref BodyVelocityWide velocityA, - in Symmetric3x3Wide effectiveMass, in Vector softnessImpulseScale, in Vector3Wide biasImpulse, in Vector maximumImpulse, - in Symmetric3x3Wide impulseToVelocityA, ref Vector3Wide accumulatedImpulse) - { - //Jacobians are just I. - Symmetric3x3Wide.TransformWithoutOverlap(velocityA.Angular, effectiveMass, out var csiVelocityComponent); - //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - csiaAngular; - Vector3Wide.Scale(accumulatedImpulse, softnessImpulseScale, out var softnessComponent); - Vector3Wide.Subtract(biasImpulse, softnessComponent, out var csi); - Vector3Wide.Subtract(csi, csiVelocityComponent, out csi); - - ServoSettingsWide.ClampImpulse(maximumImpulse, ref accumulatedImpulse, ref csi); - - ApplyImpulse(ref velocityA.Angular, impulseToVelocityA, csi); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocityWide velocityA, ref OneBodyAngularServoProjection projection, ref Vector3Wide accumulatedImpulse) - { - Solve(ref velocityA, projection.VelocityToImpulse, projection.SoftnessImpulseScale, projection.BiasImpulse, - projection.MaximumImpulse, projection.ImpulseToVelocity, ref accumulatedImpulse); - } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref OneBodyAngularMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) { ApplyImpulse(ref wsvA.Angular, inertiaA.InverseInertiaTensor, accumulatedImpulses); @@ -135,7 +91,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, ref OneBodyAngularMotorPrestepData prestepData) { } } - public class OneBodyAngularMotorTypeProcessor : OneBodyTypeProcessor + public class OneBodyAngularMotorTypeProcessor : OneBodyTypeProcessor { public const int BatchTypeId = 43; } diff --git a/BepuPhysics/Constraints/OneBodyAngularServo.cs b/BepuPhysics/Constraints/OneBodyAngularServo.cs index 1e58e62a3..d6a4d3c85 100644 --- a/BepuPhysics/Constraints/OneBodyAngularServo.cs +++ b/BepuPhysics/Constraints/OneBodyAngularServo.cs @@ -65,75 +65,8 @@ public struct OneBodyAngularServoPrestepData public ServoSettingsWide ServoSettings; } - public struct OneBodyAngularServoProjection + public struct OneBodyAngularServoFunctions : IOneBodyConstraintFunctions { - public Symmetric3x3Wide VelocityToImpulse; - public Vector3Wide BiasImpulse; - public Vector SoftnessImpulseScale; - public Vector MaximumImpulse; - public Symmetric3x3Wide ImpulseToVelocity; - } - - public struct OneBodyAngularServoFunctions : IOneBodyConstraintFunctions - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, - float dt, float inverseDt, ref OneBodyAngularServoPrestepData prestep, out OneBodyAngularServoProjection projection) - { - projection.ImpulseToVelocity = inertiaA.InverseInertiaTensor; - - //Jacobians are just the identity matrix. - - QuaternionWide.Conjugate(orientationA, out var inverseOrientation); - QuaternionWide.ConcatenateWithoutOverlap(inverseOrientation, prestep.TargetOrientation, out var errorRotation); - - QuaternionWide.GetApproximateAxisAngleFromQuaternion(errorRotation, out var errorAxis, out var errorLength); - - SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); - Symmetric3x3Wide.Invert(inertiaA.InverseInertiaTensor, out projection.VelocityToImpulse); - Symmetric3x3Wide.Scale(projection.VelocityToImpulse, effectiveMassCFMScale, out projection.VelocityToImpulse); - - ServoSettingsWide.ComputeClampedBiasVelocity(errorAxis, errorLength, positionErrorToVelocity, prestep.ServoSettings, dt, inverseDt, out var clampedBiasVelocity, out projection.MaximumImpulse); - Symmetric3x3Wide.TransformWithoutOverlap(clampedBiasVelocity, projection.VelocityToImpulse, out projection.BiasImpulse); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ApplyImpulse(ref Vector3Wide angularVelocity, in Symmetric3x3Wide impulseToVelocity, in Vector3Wide csi) - { - Symmetric3x3Wide.TransformWithoutOverlap(csi, impulseToVelocity, out var velocityChange); - Vector3Wide.Add(angularVelocity, velocityChange, out angularVelocity); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocityWide velocityA, ref OneBodyAngularServoProjection projection, ref Vector3Wide accumulatedImpulse) - { - ApplyImpulse(ref velocityA.Angular, projection.ImpulseToVelocity, accumulatedImpulse); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Solve(ref BodyVelocityWide velocityA, - in Symmetric3x3Wide effectiveMass, in Vector softnessImpulseScale, in Vector3Wide biasImpulse, in Vector maximumImpulse, - in Symmetric3x3Wide impulseToVelocityA, ref Vector3Wide accumulatedImpulse) - { - //Jacobians are just I. - Symmetric3x3Wide.TransformWithoutOverlap(velocityA.Angular, effectiveMass, out var csiVelocityComponent); - //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - csiaAngular; - Vector3Wide.Scale(accumulatedImpulse, softnessImpulseScale, out var softnessComponent); - Vector3Wide.Subtract(biasImpulse, softnessComponent, out var csi); - Vector3Wide.Subtract(csi, csiVelocityComponent, out csi); - - ServoSettingsWide.ClampImpulse(maximumImpulse, ref accumulatedImpulse, ref csi); - - ApplyImpulse(ref velocityA.Angular, impulseToVelocityA, csi); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocityWide velocityA, ref OneBodyAngularServoProjection projection, ref Vector3Wide accumulatedImpulse) - { - Solve(ref velocityA, projection.VelocityToImpulse, projection.SoftnessImpulseScale, projection.BiasImpulse, - projection.MaximumImpulse, projection.ImpulseToVelocity, ref accumulatedImpulse); - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ApplyImpulse(in Symmetric3x3Wide inverseInertia, in Vector3Wide csi, ref Vector3Wide angularVelocity) { @@ -174,7 +107,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, ref OneBodyAngularServoPrestepData prestepData) { } } - public class OneBodyAngularServoTypeProcessor : OneBodyTypeProcessor + public class OneBodyAngularServoTypeProcessor : OneBodyTypeProcessor { public const int BatchTypeId = 42; } diff --git a/BepuPhysics/Constraints/OneBodyLinearMotor.cs b/BepuPhysics/Constraints/OneBodyLinearMotor.cs index ae30b4bdc..b521587ad 100644 --- a/BepuPhysics/Constraints/OneBodyLinearMotor.cs +++ b/BepuPhysics/Constraints/OneBodyLinearMotor.cs @@ -64,34 +64,8 @@ public struct OneBodyLinearMotorPrestepData public MotorSettingsWide Settings; } - public struct OneBodyLinearMotorFunctions : IOneBodyConstraintFunctions + public struct OneBodyLinearMotorFunctions : IOneBodyConstraintFunctions { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, - float dt, float inverseDt, ref OneBodyLinearMotorPrestepData prestep, out OneBodyLinearServoProjection projection) - { - //TODO: Note that this grabs a world position. That poses a problem for different position representations. - projection.Inertia = inertiaA; - - MotorSettingsWide.ComputeSoftness(prestep.Settings, dt, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale, out projection.MaximumImpulse); - - OneBodyLinearServoFunctions.ComputeTransforms(prestep.LocalOffset, orientationA, effectiveMassCFMScale, inertiaA, out projection.Offset, out projection.EffectiveMass); - projection.BiasVelocity = prestep.TargetVelocity; - } - - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocityWide velocityA, ref OneBodyLinearServoProjection projection, ref Vector3Wide accumulatedImpulse) - { - OneBodyLinearServoFunctions.ApplyImpulse(ref velocityA, projection, ref accumulatedImpulse); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocityWide velocityA, ref OneBodyLinearServoProjection projection, ref Vector3Wide accumulatedImpulse) - { - OneBodyLinearServoFunctions.SharedSolve(ref velocityA, projection, ref accumulatedImpulse); - } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref OneBodyLinearMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) { QuaternionWide.TransformWithoutOverlap(prestep.LocalOffset, orientationA, out var offset); @@ -125,7 +99,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in [MethodImpl(MethodImplOptions.AggressiveInlining)] public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, ref OneBodyLinearMotorPrestepData prestepData) { } } - public class OneBodyLinearMotorTypeProcessor : OneBodyTypeProcessor + public class OneBodyLinearMotorTypeProcessor : OneBodyTypeProcessor { public const int BatchTypeId = 45; } diff --git a/BepuPhysics/Constraints/OneBodyLinearServo.cs b/BepuPhysics/Constraints/OneBodyLinearServo.cs index bc6847443..93a141650 100644 --- a/BepuPhysics/Constraints/OneBodyLinearServo.cs +++ b/BepuPhysics/Constraints/OneBodyLinearServo.cs @@ -74,17 +74,7 @@ public struct OneBodyLinearServoPrestepData public ServoSettingsWide ServoSettings; } - public struct OneBodyLinearServoProjection - { - public Vector3Wide Offset; - public Vector3Wide BiasVelocity; - public Symmetric3x3Wide EffectiveMass; - public Vector SoftnessImpulseScale; - public Vector MaximumImpulse; - public BodyInertiaWide Inertia; - } - - public struct OneBodyLinearServoFunctions : IOneBodyConstraintFunctions + public struct OneBodyLinearServoFunctions : IOneBodyConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ComputeTransforms(in Vector3Wide localOffset, in QuaternionWide orientation, in Vector effectiveMassCFMScale, @@ -102,72 +92,6 @@ public static void ComputeTransforms(in Vector3Wide localOffset, in QuaternionWi Symmetric3x3Wide.Scale(effectiveMass, effectiveMassCFMScale, out effectiveMass); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, - float dt, float inverseDt, ref OneBodyLinearServoPrestepData prestep, out OneBodyLinearServoProjection projection) - { - //TODO: Note that this grabs a world position. That poses a problem for different position representations. - projection.Inertia = inertiaA; - - SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); - - ComputeTransforms(prestep.LocalOffset, orientationA, effectiveMassCFMScale, inertiaA, out projection.Offset, out projection.EffectiveMass); - - //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. - Vector3Wide.Add(projection.Offset, positionA, out var worldGrabPoint); - Vector3Wide.Subtract(prestep.Target, worldGrabPoint, out var error); - ServoSettingsWide.ComputeClampedBiasVelocity(error, positionErrorToVelocity, prestep.ServoSettings, dt, inverseDt, out projection.BiasVelocity, out projection.MaximumImpulse); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ApplyImpulse(ref BodyVelocityWide velocityA, in OneBodyLinearServoProjection projection, ref Vector3Wide csi) - { - Vector3Wide.CrossWithoutOverlap(projection.Offset, csi, out var wsi); - Symmetric3x3Wide.TransformWithoutOverlap(wsi, projection.Inertia.InverseInertiaTensor, out var change); - Vector3Wide.Add(velocityA.Angular, change, out velocityA.Angular); - - Vector3Wide.Scale(csi, projection.Inertia.InverseMass, out change); - Vector3Wide.Add(velocityA.Linear, change, out velocityA.Linear); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocityWide velocityA, ref OneBodyLinearServoProjection projection, ref Vector3Wide accumulatedImpulse) - { - ApplyImpulse(ref velocityA, projection, ref accumulatedImpulse); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void SharedSolve(ref BodyVelocityWide velocities, in OneBodyLinearServoProjection projection, ref Vector3Wide accumulatedImpulse) - { - //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular); - Vector3Wide.CrossWithoutOverlap(velocities.Angular, projection.Offset, out var angularCSV); - Vector3Wide.Add(velocities.Linear, angularCSV, out var csv); - Vector3Wide.Subtract(projection.BiasVelocity, csv, out csv); - - Symmetric3x3Wide.TransformWithoutOverlap(csv, projection.EffectiveMass, out var csi); - Vector3Wide.Scale(accumulatedImpulse, projection.SoftnessImpulseScale, out var softness); - Vector3Wide.Subtract(csi, softness, out csi); - - //The motor has a limited maximum force, so clamp the accumulated impulse. Watch out for division by zero. - ServoSettingsWide.ClampImpulse(projection.MaximumImpulse, ref accumulatedImpulse, ref csi); - var previous = accumulatedImpulse; - Vector3Wide.Add(accumulatedImpulse, csi, out accumulatedImpulse); - Vector3Wide.Length(accumulatedImpulse, out var impulseMagnitude); - var newMagnitude = Vector.Min(impulseMagnitude, projection.MaximumImpulse); - var scale = newMagnitude / impulseMagnitude; - Vector3Wide.Scale(accumulatedImpulse, scale, out accumulatedImpulse); - Vector3Wide.ConditionalSelect(Vector.GreaterThan(impulseMagnitude, Vector.Zero), accumulatedImpulse, previous, out accumulatedImpulse); - Vector3Wide.Subtract(accumulatedImpulse, previous, out csi); - - ApplyImpulse(ref velocities, projection, ref csi); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocityWide velocityA, ref OneBodyLinearServoProjection projection, ref Vector3Wide accumulatedImpulse) - { - SharedSolve(ref velocityA, projection, ref accumulatedImpulse); - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ApplyImpulse(in Vector3Wide offset, in BodyInertiaWide inertia, ref BodyVelocityWide velocityA, in Vector3Wide csi) { @@ -221,7 +145,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, ref OneBodyLinearServoPrestepData prestepData) { } } - public class OneBodyLinearServoTypeProcessor : OneBodyTypeProcessor + public class OneBodyLinearServoTypeProcessor : OneBodyTypeProcessor { public const int BatchTypeId = 44; } diff --git a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs index 65ca9c785..5260d77c6 100644 --- a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs @@ -13,13 +13,8 @@ namespace BepuPhysics.Constraints /// /// Type of the prestep data used by the constraint. /// Type of the accumulated impulses used by the constraint. - /// Type of the projection to input. - public interface IOneBodyConstraintFunctions + public interface IOneBodyConstraintFunctions { - void Prestep(in Vector3Wide position, in QuaternionWide orientation, in BodyInertiaWide inertia, float dt, float inverseDt, ref TPrestepData prestepData, out TProjection projection); - void WarmStart(ref BodyVelocityWide velocity, ref TProjection projection, ref TAccumulatedImpulse accumulatedImpulse); - void Solve(ref BodyVelocityWide velocity, ref TProjection projection, ref TAccumulatedImpulse accumulatedImpulse); - void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref TPrestepData prestep, ref TAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA); void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, @@ -37,11 +32,11 @@ void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyIne /// /// Shared implementation across all one body constraints. /// - public abstract class OneBodyTypeProcessor - : TypeProcessor, TPrestepData, TProjection, TAccumulatedImpulse> - where TPrestepData : unmanaged where TProjection : unmanaged where TAccumulatedImpulse : unmanaged - where TConstraintFunctions : unmanaged, IOneBodyConstraintFunctions + : TypeProcessor, TPrestepData, TAccumulatedImpulse> + where TPrestepData : unmanaged where TAccumulatedImpulse : unmanaged + where TConstraintFunctions : unmanaged, IOneBodyConstraintFunctions where TWarmStartAccessFilterA : unmanaged, IBodyAccessFilter where TSolveAccessFilterA : unmanaged, IBodyAccessFilter { @@ -85,55 +80,6 @@ internal sealed override void VerifySortRegion(ref TypeBatch typeBatch, int bund //By providing the overrides at this level, the concrete implementation (assuming it inherits from one of the prestep-providing variants) //only has to specify *type* arguments associated with the interface-implementing struct-delegates. It's going to look very strange, but it's low overhead //and minimizes per-type duplication. - public unsafe override void Prestep(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) - { - ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); - ref var bodyReferencesBase = ref Unsafe.AsRef>(typeBatch.BodyReferences.Memory); - ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); - var function = default(TConstraintFunctions); - for (int i = startBundle; i < exclusiveEndBundle; ++i) - { - ref var prestep = ref Unsafe.Add(ref prestepBase, i); - ref var projection = ref Unsafe.Add(ref projectionBase, i); - ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); - bodies.GatherState(ref references, out var position, out var orientation, out _, out var inertia); - function.Prestep(position, orientation, inertia, dt, inverseDt, ref prestep, out projection); - } - } - - public unsafe override void WarmStart(ref TypeBatch typeBatch, Bodies bodies, int startBundle, int exclusiveEndBundle) - { - ref var bodyReferencesBase = ref Unsafe.AsRef>(typeBatch.BodyReferences.Memory); - ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); - ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); - var function = default(TConstraintFunctions); - for (int i = startBundle; i < exclusiveEndBundle; ++i) - { - ref var projection = ref Unsafe.Add(ref projectionBase, i); - ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); - ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); - bodies.GatherState(ref bodyReferences, out _, out _, out var wsvA, out _); - function.WarmStart(ref wsvA, ref projection, ref accumulatedImpulses); - bodies.ScatterVelocities(ref wsvA, ref bodyReferences); - } - } - - public unsafe override void SolveIteration(ref TypeBatch typeBatch, Bodies bodies, int startBundle, int exclusiveEndBundle) - { - ref var bodyReferencesBase = ref Unsafe.AsRef>(typeBatch.BodyReferences.Memory); - ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); - ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); - var function = default(TConstraintFunctions); - for (int i = startBundle; i < exclusiveEndBundle; ++i) - { - ref var projection = ref Unsafe.Add(ref projectionBase, i); - ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); - ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); - bodies.GatherState(ref bodyReferences, out _, out _, out var wsvA, out _); - function.Solve(ref wsvA, ref projection, ref accumulatedImpulses); - bodies.ScatterVelocities(ref wsvA, ref bodyReferences); - } - } public unsafe override void WarmStart2( ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, @@ -207,10 +153,10 @@ public unsafe override void IncrementallyUpdateForSubstep(ref TypeBatch typeBatc } } - public abstract class OneBodyContactTypeProcessor - : OneBodyTypeProcessor - where TPrestepData : unmanaged where TProjection : unmanaged where TAccumulatedImpulse : unmanaged - where TConstraintFunctions : unmanaged, IOneBodyConstraintFunctions + public abstract class OneBodyContactTypeProcessor + : OneBodyTypeProcessor + where TPrestepData : unmanaged where TAccumulatedImpulse : unmanaged + where TConstraintFunctions : unmanaged, IOneBodyConstraintFunctions { } diff --git a/BepuPhysics/Constraints/PointOnLineServo.cs b/BepuPhysics/Constraints/PointOnLineServo.cs index 44578c86c..dcda2d73f 100644 --- a/BepuPhysics/Constraints/PointOnLineServo.cs +++ b/BepuPhysics/Constraints/PointOnLineServo.cs @@ -79,103 +79,8 @@ public struct PointOnLineServoPrestepData public SpringSettingsWide SpringSettings; } - public struct PointOnLineServoProjection + public struct PointOnLineServoFunctions : ITwoBodyConstraintFunctions { - public Matrix2x3Wide LinearJacobian; - public Vector3Wide OffsetA; - public Vector3Wide OffsetB; - public Vector2Wide BiasVelocity; - public Symmetric2x2Wide EffectiveMass; - public Vector SoftnessImpulseScale; - public Vector MaximumImpulse; - public BodyInertiaWide InertiaA; - public BodyInertiaWide InertiaB; - } - - public struct PointOnLineServoFunctions : ITwoBodyConstraintFunctions - { - static void GetAngularJacobians(in Matrix2x3Wide linearJacobians, in Vector3Wide offsetA, in Vector3Wide offsetB, out Matrix2x3Wide angularJacobianA, out Matrix2x3Wide angularJacobianB) - { - Vector3Wide.CrossWithoutOverlap(offsetA, linearJacobians.X, out angularJacobianA.X); - Vector3Wide.CrossWithoutOverlap(offsetA, linearJacobians.Y, out angularJacobianA.Y); - Vector3Wide.CrossWithoutOverlap(linearJacobians.X, offsetB, out angularJacobianB.X); - Vector3Wide.CrossWithoutOverlap(linearJacobians.Y, offsetB, out angularJacobianB.Y); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, - float dt, float inverseDt, ref PointOnLineServoPrestepData prestep, out PointOnLineServoProjection projection) - { - //This constrains a point on B to a line attached to A. It works on two degrees of freedom at the same time; those are the tangent axes to the line direction. - //The error is measured as closest offset from the line. In other words: - //dot(closestPointOnLineToAnchorB - anchorB, t1) = 0 - //dot(closestPointOnLineToAnchorB - anchorB, t2) = 0 - //where closestPointOnLineToAnchorB = dot(anchorB - anchorA, lineDirection) * lineDirection + anchorA - //For the purposes of this derivation, we'll treat t1, t2, and lineDirection as constant with respect to time. - //In the following, offsetA from the center of A to the closestPointOnLineToAnchorB, and offsetB refers to the LocalOffsetB * orientationB. - //dot(positionA + offsetA - (positionB + offsetB), t1) = 0 - //dot(positionA + offsetA - (positionB + offsetB), t2) = 0 - //Velocity constraint for t1: - //dot(d/dt(positionA + offsetA - (positionB + offsetB), t1) + dot(positionA + offsetA - (positionB + offsetB), d/dt(t1)) = 0 - //Treat d/dt(t1) as constant: - //dot(d/dt(positionA + offsetA - (positionB + offsetB), t1) = 0 - //dot(linearA + angularA x offsetA - linearB - angularB x offsetB, t1) = 0 - //dot(linearA, t1) + dot(angularA x offsetA, t1) + dot(linearB, -t1) + dot(offsetB x angularB, t1) = 0 - //dot(linearA, t1) + dot(offsetA x t1, angularA) + dot(linearB, -t1) + dot(t1 x offsetB, angularB) = 0 - //Following the same pattern for the second degree of freedom, the jacobians are: - //linearA: t1, t2 - //angularA: offsetA x t1, offsetA x t2 - //linearB: -t1, -t2 - //angularB: t1 x offsetB, t2 x offsetB - - //Options for storage: - //1) Reconstruct from direction and the two offsets. Still stores effective mass and inertia tensors (3 + 14 scalars), but requires only 9 scalars for all jacobians. - //2) Store t1, t2, and the two offsets. Saves reconstruction ALU cost but increases storage cost relative to #1 by 3 scalars. - //3) Store JT * Me and J * I^-1. Requires 3 * 6 scalars for JT * Me and 3 * 8 scalars for J * I^-1. - //Memory bandwidth is the primary target, so #1 is attractive. However, recomputing the tangent basis based on the world direction would be unreliable unless - //additional information was stored because there is no unique basis for a single direction. This is less of a concern when building the basis off the local direction - //because it isn't expected to change frequently. - //Instead, we'll go with #2. - //#3 would be the fastest on a single core by virtue of requiring significantly less ALU work, but it requires more memory bandwidth. - - Matrix3x3Wide.CreateFromQuaternion(orientationA, out var orientationMatrixA); - Matrix3x3Wide.TransformWithoutOverlap(prestep.LocalOffsetA, orientationMatrixA, out var anchorA); - QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetB, orientationB, out projection.OffsetB); - - //Find offsetA by computing the closest point on the line to anchorB. - Matrix3x3Wide.TransformWithoutOverlap(prestep.LocalDirection, orientationMatrixA, out var direction); - Vector3Wide.Add(projection.OffsetB, ab, out var anchorB); - Vector3Wide.Subtract(anchorB, anchorA, out var anchorOffset); - Vector3Wide.Dot(anchorOffset, direction, out var d); - Vector3Wide.Scale(direction, d, out var lineStartToClosestPointOnLine); - Vector3Wide.Add(lineStartToClosestPointOnLine, anchorA, out projection.OffsetA); - - //Note again that the basis is created in local space to avoid rapidly changing jacobians. - Helpers.BuildOrthonormalBasis(prestep.LocalDirection, out var localTangentX, out var localTangentY); - Matrix3x3Wide.TransformWithoutOverlap(localTangentX, orientationMatrixA, out projection.LinearJacobian.X); - Matrix3x3Wide.TransformWithoutOverlap(localTangentY, orientationMatrixA, out projection.LinearJacobian.Y); - GetAngularJacobians(projection.LinearJacobian, projection.OffsetA, projection.OffsetB, out var angularJacobianA, out var angularJacobianB); - Symmetric2x2Wide.SandwichScale(projection.LinearJacobian, inertiaA.InverseMass + inertiaB.InverseMass, out var linearContribution); - Symmetric3x3Wide.MatrixSandwich(angularJacobianA, inertiaA.InverseInertiaTensor, out var angularContributionA); - Symmetric3x3Wide.MatrixSandwich(angularJacobianB, inertiaB.InverseInertiaTensor, out var angularContributionB); - Symmetric2x2Wide.Add(angularContributionA, angularContributionB, out var inverseEffectiveMass); - Symmetric2x2Wide.Add(inverseEffectiveMass, linearContribution, out inverseEffectiveMass); - - Symmetric2x2Wide.InvertWithoutOverlap(inverseEffectiveMass, out projection.EffectiveMass); - projection.InertiaA = inertiaA; - projection.InertiaB = inertiaB; - - SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); - Symmetric2x2Wide.Scale(projection.EffectiveMass, effectiveMassCFMScale, out projection.EffectiveMass); - - //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. - Vector2Wide error; - Vector3Wide.Dot(anchorOffset, projection.LinearJacobian.X, out error.X); - Vector3Wide.Dot(anchorOffset, projection.LinearJacobian.Y, out error.Y); - ServoSettingsWide.ComputeClampedBiasVelocity(error, positionErrorToVelocity, prestep.ServoSettings, dt, inverseDt, out projection.BiasVelocity, out projection.MaximumImpulse); - - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ApplyImpulse(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, in Matrix2x3Wide linearJacobian, in Matrix2x3Wide angularJacobianA, in Matrix2x3Wide angularJacobianB, in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, ref Vector2Wide csi) @@ -194,32 +99,6 @@ public static void ApplyImpulse(ref BodyVelocityWide velocityA, ref BodyVelocity Vector3Wide.Add(angularChangeB, velocityB.Angular, out velocityB.Angular); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref PointOnLineServoProjection projection, ref Vector2Wide accumulatedImpulse) - { - GetAngularJacobians(projection.LinearJacobian, projection.OffsetA, projection.OffsetB, out var angularA, out var angularB); - ApplyImpulse(ref velocityA, ref velocityB, projection.LinearJacobian, angularA, angularB, projection.InertiaA, projection.InertiaB, ref accumulatedImpulse); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref PointOnLineServoProjection projection, ref Vector2Wide accumulatedImpulse) - { - //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); - GetAngularJacobians(projection.LinearJacobian, projection.OffsetA, projection.OffsetB, out var angularA, out var angularB); - Matrix2x3Wide.TransformByTransposeWithoutOverlap(velocityA.Linear, projection.LinearJacobian, out var linearCSVA); - Matrix2x3Wide.TransformByTransposeWithoutOverlap(velocityB.Linear, projection.LinearJacobian, out var negatedLinearCSVB); - Matrix2x3Wide.TransformByTransposeWithoutOverlap(velocityA.Angular, angularA, out var angularCSVA); - Matrix2x3Wide.TransformByTransposeWithoutOverlap(velocityB.Angular, angularB, out var angularCSVB); - Vector2Wide.Subtract(linearCSVA, negatedLinearCSVB, out var linearCSV); - Vector2Wide.Add(angularCSVA, angularCSVB, out var angularCSV); - Vector2Wide.Add(linearCSV, angularCSV, out var csv); - Vector2Wide.Subtract(projection.BiasVelocity, csv, out csv); - Symmetric2x2Wide.TransformWithoutOverlap(csv, projection.EffectiveMass, out var csi); - Vector2Wide.Scale(accumulatedImpulse, projection.SoftnessImpulseScale, out var softnessContribution); - Vector2Wide.Subtract(csi, softnessContribution, out csi); - ServoSettingsWide.ClampImpulse(projection.MaximumImpulse, ref accumulatedImpulse, ref csi); - ApplyImpulse(ref velocityA, ref velocityB, projection.LinearJacobian, angularA, angularB, projection.InertiaA, projection.InertiaB, ref csi); - } [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ComputeJacobians(in Vector3Wide ab, in QuaternionWide orientationA, in QuaternionWide orientationB, in Vector3Wide localDirection, in Vector3Wide localOffsetA, in Vector3Wide localOffsetB, @@ -313,7 +192,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref PointOnLineServoPrestepData prestepData) { } } - public class PointOnLineServoTypeProcessor : TwoBodyTypeProcessor + public class PointOnLineServoTypeProcessor : TwoBodyTypeProcessor { public const int BatchTypeId = 37; } diff --git a/BepuPhysics/Constraints/SwingLimit.cs b/BepuPhysics/Constraints/SwingLimit.cs index 743c0c0ac..8c8ff74af 100644 --- a/BepuPhysics/Constraints/SwingLimit.cs +++ b/BepuPhysics/Constraints/SwingLimit.cs @@ -89,97 +89,8 @@ public struct SwingLimitPrestepData public SpringSettingsWide SpringSettings; } - public struct SwingLimitProjection + public struct SwingLimitFunctions : ITwoBodyConstraintFunctions> { - //JacobianB = -JacobianA, so no need to store it explicitly. - public Vector3Wide VelocityToImpulseA; - public Vector BiasImpulse; - public Vector SoftnessImpulseScale; - public Vector3Wide ImpulseToVelocityA; - public Vector3Wide NegatedImpulseToVelocityB; - } - - public struct SwingLimitFunctions : ITwoBodyConstraintFunctions> - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, - float dt, float inverseDt, ref SwingLimitPrestepData prestep, out SwingLimitProjection projection) - { - //The swing limit attempts to keep an axis on body A within from an axis on body B. In other words, this is the same as a hinge joint, but with one fewer DOF. - //(Note that the jacobians are extremely similar to the AngularSwivelHinge; the difference is that this is a speculative inequality constraint.) - //C = dot(axisA, axisB) >= MinimumDot - //C' = dot(d/dt(axisA), axisB) + dot(axisA, d/dt(axisB)) >= 0 - //C' = dot(angularVelocityA x axisA, axisB) + dot(axisA, angularVelocityB x axisB) >= 0 - //C' = dot(axisA x axisB, angularVelocityA) + dot(angularVelocityB, axisB x axisA) >= 0 - //Providing jacobians of: - //JA = axisA x axisB - //JB = axisB x axisA - //a x b == -b x a, so JB == -JA. - - //Now, we choose the storage representation. The default approach would be to store JA, the effective mass, and both inverse inertias, requiring 6 + 1 + 6 + 6 scalars. - //The alternative is to store JAT * effectiveMass, and then also JA * inverseInertiaTensor(A/B), requiring only 3 + 3 + 3 scalars. - //So, overall, prebaking saves us 10 scalars and a bit of iteration-time ALU. - QuaternionWide.TransformWithoutOverlap(prestep.AxisLocalA, orientationA, out var axisA); - QuaternionWide.TransformWithoutOverlap(prestep.AxisLocalB, orientationB, out var axisB); - Vector3Wide.CrossWithoutOverlap(axisA, axisB, out var jacobianA); - //In the event that the axes are parallel, there is no unique jacobian. Arbitrarily pick one. - //Note that this causes a discontinuity in jacobian length at the poles. We just don't worry about it. - Helpers.FindPerpendicular(axisA, out var fallbackJacobian); - Vector3Wide.Dot(jacobianA, jacobianA, out var jacobianLengthSquared); - var useFallback = Vector.LessThan(jacobianLengthSquared, new Vector(1e-7f)); - Vector3Wide.ConditionalSelect(useFallback, fallbackJacobian, jacobianA, out jacobianA); - - //Note that JA = -JB, but for the purposes of calculating the effective mass the sign is irrelevant. - - //This computes the effective mass using the usual (J * M^-1 * JT)^-1 formulation, but we actually make use of the intermediate result J * M^-1 so we compute it directly. - Symmetric3x3Wide.TransformWithoutOverlap(jacobianA, inertiaA.InverseInertiaTensor, out projection.ImpulseToVelocityA); - //Note that we don't use -jacobianA here, so we're actually storing out the negated version of the transform. That's fine; we'll simply subtract in the iteration. - Symmetric3x3Wide.TransformWithoutOverlap(jacobianA, inertiaB.InverseInertiaTensor, out projection.NegatedImpulseToVelocityB); - Vector3Wide.Dot(projection.ImpulseToVelocityA, jacobianA, out var angularA); - Vector3Wide.Dot(projection.NegatedImpulseToVelocityB, jacobianA, out var angularB); - - SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); - var effectiveMass = effectiveMassCFMScale / (angularA + angularB); - Vector3Wide.Scale(jacobianA, effectiveMass, out projection.VelocityToImpulseA); - - Vector3Wide.Dot(axisA, axisB, out var axisDot); - var error = axisDot - prestep.MinimumDot; - //Note the negation: we want to oppose the separation. TODO: arguably, should bake the negation into positionErrorToVelocity, given its name. - var biasVelocity = -Vector.Min(error * new Vector(inverseDt), error * positionErrorToVelocity); - projection.BiasImpulse = effectiveMass * biasVelocity; - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - private static void ApplyImpulse(ref Vector3Wide angularVelocityA, ref Vector3Wide angularVelocityB, ref SwingLimitProjection projection, ref Vector csi) - { - Vector3Wide.Scale(projection.ImpulseToVelocityA, csi, out var velocityChangeA); - Vector3Wide.Add(angularVelocityA, velocityChangeA, out angularVelocityA); - Vector3Wide.Scale(projection.NegatedImpulseToVelocityB, csi, out var negatedVelocityChangeB); - Vector3Wide.Subtract(angularVelocityB, negatedVelocityChangeB, out angularVelocityB); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref SwingLimitProjection projection, ref Vector accumulatedImpulse) - { - ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, ref projection, ref accumulatedImpulse); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref SwingLimitProjection projection, ref Vector accumulatedImpulse) - { - //JB = -JA. This is (angularVelocityA * JA + angularVelocityB * JB) * effectiveMass => (angularVelocityA - angularVelocityB) * (JA * effectiveMass) - Vector3Wide.Subtract(velocityA.Angular, velocityB.Angular, out var difference); - Vector3Wide.Dot(difference, projection.VelocityToImpulseA, out var csi); - //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); - csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - csi; - - var previousAccumulatedImpulse = accumulatedImpulse; - accumulatedImpulse = Vector.Max(Vector.Zero, accumulatedImpulse + csi); - csi = accumulatedImpulse - previousAccumulatedImpulse; - ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, ref projection, ref csi); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] private static void ApplyImpulse(in Vector3Wide impulseToVelocityA, in Vector3Wide negatedImpulseToVelocityB, in Vector csi, ref Vector3Wide angularVelocityA, ref Vector3Wide angularVelocityB) { @@ -257,7 +168,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref SwingLimitPrestepData prestepData) { } } - public class SwingLimitTypeProcessor : TwoBodyTypeProcessor, SwingLimitFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular> + public class SwingLimitTypeProcessor : TwoBodyTypeProcessor, SwingLimitFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular> { public const int BatchTypeId = 25; } diff --git a/BepuPhysics/Constraints/SwivelHinge.cs b/BepuPhysics/Constraints/SwivelHinge.cs index 4c6dc7216..278f7750f 100644 --- a/BepuPhysics/Constraints/SwivelHinge.cs +++ b/BepuPhysics/Constraints/SwivelHinge.cs @@ -80,138 +80,8 @@ public struct SwivelHingePrestepData public SpringSettingsWide SpringSettings; } - public struct SwivelHingeProjection + public struct SwivelHingeFunctions : ITwoBodyConstraintFunctions { - public Vector3Wide OffsetA; - public Vector3Wide OffsetB; - public Vector3Wide SwivelHingeJacobian; - public Vector4Wide BiasVelocity; - public Symmetric4x4Wide EffectiveMass; - public Vector SoftnessImpulseScale; - public BodyInertiaWide InertiaA; - public BodyInertiaWide InertiaB; - } - - public struct SwivelHingeFunctions : ITwoBodyConstraintFunctions - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, - float dt, float inverseDt, ref SwivelHingePrestepData prestep, out SwivelHingeProjection projection) - { - projection.InertiaA = inertiaA; - projection.InertiaB = inertiaB; - - //4x12 jacobians, from BallSocket and AngularSwivelHinge: - //[ I, skew(offsetA), -I, -skew(offsetB) ] - //[ 0, swivelA x hingeB, 0, -swivelA x hingeB ] - - Matrix3x3Wide.CreateFromQuaternion(orientationA, out var orientationMatrixA); - Matrix3x3Wide.CreateFromQuaternion(orientationB, out var orientationMatrixB); - Matrix3x3Wide.TransformWithoutOverlap(prestep.LocalOffsetA, orientationMatrixA, out projection.OffsetA); - Matrix3x3Wide.TransformWithoutOverlap(prestep.LocalSwivelAxisA, orientationMatrixA, out var swivelAxis); - Matrix3x3Wide.TransformWithoutOverlap(prestep.LocalOffsetB, orientationMatrixB, out projection.OffsetB); - Matrix3x3Wide.TransformWithoutOverlap(prestep.LocalHingeAxisB, orientationMatrixB, out var hingeAxis); - Vector3Wide.CrossWithoutOverlap(swivelAxis, hingeAxis, out projection.SwivelHingeJacobian); - - //The upper left 3x3 block is just the ball socket. - Symmetric3x3Wide.SkewSandwichWithoutOverlap(projection.OffsetA, inertiaA.InverseInertiaTensor, out var ballSocketContributionAngularA); - Symmetric3x3Wide.SkewSandwichWithoutOverlap(projection.OffsetB, inertiaB.InverseInertiaTensor, out var ballSocketContributionAngularB); - Unsafe.SkipInit(out Symmetric4x4Wide inverseEffectiveMass); - ref var upperLeft = ref Symmetric4x4Wide.GetUpperLeft3x3Block(ref inverseEffectiveMass); - Symmetric3x3Wide.Add(ballSocketContributionAngularA, ballSocketContributionAngularB, out upperLeft); - var linearContribution = projection.InertiaA.InverseMass + projection.InertiaB.InverseMass; - upperLeft.XX += linearContribution; - upperLeft.YY += linearContribution; - upperLeft.ZZ += linearContribution; - - //The lower right 1x1 block is the AngularSwivelHinge. - Symmetric3x3Wide.TransformWithoutOverlap(projection.SwivelHingeJacobian, inertiaA.InverseInertiaTensor, out var swivelHingeInertiaA); - Symmetric3x3Wide.TransformWithoutOverlap(projection.SwivelHingeJacobian, inertiaB.InverseInertiaTensor, out var swivelHingeInertiaB); - Vector3Wide.Dot(swivelHingeInertiaA, projection.SwivelHingeJacobian, out var swivelHingeContributionAngularA); - Vector3Wide.Dot(swivelHingeInertiaB, projection.SwivelHingeJacobian, out var swivelHingeContributionAngularB); - inverseEffectiveMass.WW = swivelHingeContributionAngularA + swivelHingeContributionAngularB; - - //The remaining off-diagonal region is skew(offsetA) * Ia^-1 * (swivelAxis x hingeAxis) + skew(offsetB) * Ib^-1 * (swivelAxis x hingeAxis) - //skew(offsetA) * (Ia^-1 * (swivelAxis x hingeAxis) = (Ia^-1 * (swivelAxis x hingeAxis)) x offsetA - //Careful with cross order/signs! - Vector3Wide.CrossWithoutOverlap(swivelHingeInertiaA, projection.OffsetA, out var offDiagonalContributionA); - Vector3Wide.CrossWithoutOverlap(swivelHingeInertiaB, projection.OffsetB, out var offDiagonalContributionB); - Vector3Wide.Add(offDiagonalContributionA, offDiagonalContributionB, out Symmetric4x4Wide.GetUpperRight3x1Block(ref inverseEffectiveMass)); - - Symmetric4x4Wide.InvertWithoutOverlap(inverseEffectiveMass, out projection.EffectiveMass); - SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); - Symmetric4x4Wide.Scale(projection.EffectiveMass, effectiveMassCFMScale, out projection.EffectiveMass); - - //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. - Vector3Wide.Add(ab, projection.OffsetB, out var anchorB); - Vector3Wide.Subtract(anchorB, projection.OffsetA, out var ballSocketError); - projection.BiasVelocity.X = ballSocketError.X * positionErrorToVelocity; - projection.BiasVelocity.Y = ballSocketError.Y * positionErrorToVelocity; - projection.BiasVelocity.Z = ballSocketError.Z * positionErrorToVelocity; - - Vector3Wide.Dot(hingeAxis, swivelAxis, out var error); - //Note the negation: we want to oppose the separation. TODO: arguably, should bake the negation into positionErrorToVelocity, given its name. - projection.BiasVelocity.W = positionErrorToVelocity * -error; - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - private static void ApplyImpulse(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref SwivelHingeProjection projection, ref Vector4Wide csi) - { - //[ csi ] * [ I, skew(offsetA), -I, -skew(offsetB) ] - // [ 0, swivelA x hingeB, 0, -swivelA x hingeB ] - ref var ballSocketCSI = ref Unsafe.As, Vector3Wide>(ref csi.X); - Vector3Wide.Scale(ballSocketCSI, projection.InertiaA.InverseMass, out var linearChangeA); - Vector3Wide.Add(velocityA.Linear, linearChangeA, out velocityA.Linear); - - Vector3Wide.CrossWithoutOverlap(projection.OffsetA, ballSocketCSI, out var ballSocketAngularImpulseA); - Vector3Wide.Scale(projection.SwivelHingeJacobian, csi.W, out var swivelHingeAngularImpulseA); - Vector3Wide.Add(ballSocketAngularImpulseA, swivelHingeAngularImpulseA, out var angularImpulseA); - Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseA, projection.InertiaA.InverseInertiaTensor, out var angularChangeA); - Vector3Wide.Add(velocityA.Angular, angularChangeA, out velocityA.Angular); - - //Note cross order flip for negation. - Vector3Wide.Scale(ballSocketCSI, projection.InertiaB.InverseMass, out var negatedLinearChangeB); - Vector3Wide.Subtract(velocityB.Linear, negatedLinearChangeB, out velocityB.Linear); - Vector3Wide.CrossWithoutOverlap(ballSocketCSI, projection.OffsetB, out var ballSocketAngularImpulseB); - Vector3Wide.Subtract(ballSocketAngularImpulseB, swivelHingeAngularImpulseA, out var angularImpulseB); - Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseB, projection.InertiaB.InverseInertiaTensor, out var angularChangeB); - Vector3Wide.Add(velocityB.Angular, angularChangeB, out velocityB.Angular); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref SwivelHingeProjection projection, ref Vector4Wide accumulatedImpulse) - { - ApplyImpulse(ref velocityA, ref velocityB, ref projection, ref accumulatedImpulse); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref SwivelHingeProjection projection, ref Vector4Wide accumulatedImpulse) - { - //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); - //[ csi ] * [ I, skew(offsetA), -I, -skew(offsetB) ] - // [ 0, swivelA x hingeB, 0, -swivelA x hingeB ] - Vector3Wide.CrossWithoutOverlap(velocityA.Angular, projection.OffsetA, out var ballSocketAngularCSVA); - Vector3Wide.Dot(projection.SwivelHingeJacobian, velocityA.Angular, out var swivelHingeCSVA); - Vector3Wide.CrossWithoutOverlap(projection.OffsetB, velocityB.Angular, out var ballSocketAngularCSVB); - Vector3Wide.Dot(projection.SwivelHingeJacobian, velocityB.Angular, out var negatedSwivelHingeCSVB); - - Vector3Wide.Add(ballSocketAngularCSVA, ballSocketAngularCSVB, out var ballSocketAngularCSV); - Vector3Wide.Subtract(velocityA.Linear, velocityB.Linear, out var ballSocketLinearCSV); - Vector4Wide csv; - csv.X = ballSocketAngularCSV.X + ballSocketLinearCSV.X; - csv.Y = ballSocketAngularCSV.Y + ballSocketLinearCSV.Y; - csv.Z = ballSocketAngularCSV.Z + ballSocketLinearCSV.Z; - csv.W = swivelHingeCSVA - negatedSwivelHingeCSVB; - Vector4Wide.Subtract(projection.BiasVelocity, csv, out csv); - - Symmetric4x4Wide.TransformWithoutOverlap(csv, projection.EffectiveMass, out var csi); - Vector4Wide.Scale(accumulatedImpulse, projection.SoftnessImpulseScale, out var softnessContribution); - Vector4Wide.Subtract(csi, softnessContribution, out csi); - - ApplyImpulse(ref velocityA, ref velocityB, ref projection, ref csi); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] private static void ApplyImpulse(in Vector3Wide offsetA, in Vector3Wide offsetB, in Vector3Wide swivelHingeJacobian, in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, ref Vector4Wide csi, ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB) { @@ -343,7 +213,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref SwivelHingePrestepData prestepData) { } } - public class SwivelHingeTypeProcessor : TwoBodyTypeProcessor + public class SwivelHingeTypeProcessor : TwoBodyTypeProcessor { public const int BatchTypeId = 46; } diff --git a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs index 0bf5878ab..e1a1535e5 100644 --- a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs @@ -23,14 +23,8 @@ public struct ThreeBodyReferences /// /// Type of the prestep data used by the constraint. /// Type of the accumulated impulses used by the constraint. - /// Type of the projection to input. - public interface IThreeBodyConstraintFunctions + public interface IThreeBodyConstraintFunctions { - void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide ac, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, - float dt, float inverseDt, ref TPrestepData prestepData, out TProjection projection); - void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref BodyVelocityWide velocityC, ref TProjection projection, ref TAccumulatedImpulse accumulatedImpulse); - void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref BodyVelocityWide velocityC, ref TProjection projection, ref TAccumulatedImpulse accumulatedImpulse); - void WarmStart2( in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, @@ -52,11 +46,11 @@ void Solve2( /// /// Shared implementation across all three body constraints. /// - public abstract class ThreeBodyTypeProcessor - : TypeProcessor - where TPrestepData : unmanaged where TProjection : unmanaged where TAccumulatedImpulse : unmanaged - where TConstraintFunctions : unmanaged, IThreeBodyConstraintFunctions + : TypeProcessor + where TPrestepData : unmanaged where TAccumulatedImpulse : unmanaged + where TConstraintFunctions : unmanaged, IThreeBodyConstraintFunctions where TWarmStartAccessFilterA : unmanaged, IBodyAccessFilter where TWarmStartAccessFilterB : unmanaged, IBodyAccessFilter where TWarmStartAccessFilterC : unmanaged, IBodyAccessFilter @@ -104,70 +98,7 @@ internal sealed override void VerifySortRegion(ref TypeBatch typeBatch, int bund { VerifySortRegion(ref typeBatch, bundleStartIndex, constraintCount, ref sortedKeys, ref sortedSourceIndices); } - - public unsafe override void Prestep(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) - { - ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); - ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); - ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); - var function = default(TConstraintFunctions); - for (int i = startBundle; i < exclusiveEndBundle; ++i) - { - ref var prestep = ref Unsafe.Add(ref prestepBase, i); - ref var projection = ref Unsafe.Add(ref projectionBase, i); - ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); - bodies.GatherState(ref references, - out var orientationA, out var wsvA, out var inertiaA, - out var ab, out var orientationB, out var wsvB, out var inertiaB, - out var ac, out var orientationC, out var wsvC, out var inertiaC); - function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, ac, orientationC, inertiaC, dt, inverseDt, ref prestep, out projection); - } - } - - public unsafe override void WarmStart(ref TypeBatch typeBatch, Bodies bodies, int startBundle, int exclusiveEndBundle) - { - ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); - ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); - ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); - var function = default(TConstraintFunctions); - for (int i = startBundle; i < exclusiveEndBundle; ++i) - { - ref var projection = ref Unsafe.Add(ref projectionBase, i); - ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); - ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); - bodies.GatherState(ref bodyReferences, - out var orientationA, out var wsvA, out var inertiaA, - out var ab, out var orientationB, out var wsvB, out var inertiaB, - out var ac, out var orientationC, out var wsvC, out var inertiaC); - function.WarmStart(ref wsvA, ref wsvB, ref wsvC, ref projection, ref accumulatedImpulses); - bodies.ScatterVelocities(ref wsvA, ref bodyReferences.IndexA); - bodies.ScatterVelocities(ref wsvB, ref bodyReferences.IndexB); - bodies.ScatterVelocities(ref wsvC, ref bodyReferences.IndexC); - - } - } - - public unsafe override void SolveIteration(ref TypeBatch typeBatch, Bodies bodies, int startBundle, int exclusiveEndBundle) - { - ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); - ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); - ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); - var function = default(TConstraintFunctions); - for (int i = startBundle; i < exclusiveEndBundle; ++i) - { - ref var projection = ref Unsafe.Add(ref projectionBase, i); - ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); - ref var bodyReferences = ref Unsafe.Add(ref bodyReferencesBase, i); - bodies.GatherState(ref bodyReferences, - out var orientationA, out var wsvA, out var inertiaA, - out var ab, out var orientationB, out var wsvB, out var inertiaB, - out var ac, out var orientationC, out var wsvC, out var inertiaC); - function.Solve(ref wsvA, ref wsvB, ref wsvC, ref projection, ref accumulatedImpulses); - bodies.ScatterVelocities(ref wsvA, ref bodyReferences.IndexA); - bodies.ScatterVelocities(ref wsvB, ref bodyReferences.IndexB); - bodies.ScatterVelocities(ref wsvC, ref bodyReferences.IndexC); - } - } + public unsafe override void WarmStart2( ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) diff --git a/BepuPhysics/Constraints/TwistLimit.cs b/BepuPhysics/Constraints/TwistLimit.cs index babe9a1eb..381935623 100644 --- a/BepuPhysics/Constraints/TwistLimit.cs +++ b/BepuPhysics/Constraints/TwistLimit.cs @@ -82,69 +82,8 @@ public struct TwistLimitPrestepData public SpringSettingsWide SpringSettings; } - public struct TwistLimitProjection + public struct TwistLimitFunctions : ITwoBodyConstraintFunctions> { - public Vector3Wide VelocityToImpulseA; - public Vector BiasImpulse; - public Vector SoftnessImpulseScale; - public Vector3Wide ImpulseToVelocityA; - public Vector3Wide NegatedImpulseToVelocityB; - } - - - public struct TwistLimitFunctions : ITwoBodyConstraintFunctions> - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, - float dt, float inverseDt, ref TwistLimitPrestepData prestep, out TwistLimitProjection projection) - { - Unsafe.SkipInit(out projection); - TwistServoFunctions.ComputeJacobian(orientationA, orientationB, prestep.LocalBasisA, prestep.LocalBasisB, - out var basisBX, out var basisBZ, out var basisA, out var jacobianA); - - TwistServoFunctions.ComputeCurrentAngle(basisBX, basisBZ, basisA, out var angle); - - //For simplicity, the solve iterations can only apply a positive impulse. So, the jacobians get flipped when necessary to make that consistent. - //To figure out which way to flip, take the angular distance from minimum to current angle, and maximum to current angle. - MathHelper.GetSignedAngleDifference(prestep.MinimumAngle, angle, out var minError); - MathHelper.GetSignedAngleDifference(prestep.MaximumAngle, angle, out var maxError); - var useMin = Vector.LessThan(Vector.Abs(minError), Vector.Abs(maxError)); - - //If we use the maximum bound, flip the jacobian. - var error = Vector.ConditionalSelect(useMin, -minError, maxError); - Vector3Wide.Negate(jacobianA, out var negatedJacobianA); - Vector3Wide.ConditionalSelect(useMin, negatedJacobianA, jacobianA, out jacobianA); - - TwistServoFunctions.ComputeEffectiveMass(dt, prestep.SpringSettings, inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, jacobianA, - out projection.ImpulseToVelocityA, out projection.NegatedImpulseToVelocityB, - out var positionErrorToVelocity, out projection.SoftnessImpulseScale, out var effectiveMass, out projection.VelocityToImpulseA); - - //In the speculative case, allow the limit to be approached. - var biasVelocity = Vector.ConditionalSelect(Vector.LessThan(error, Vector.Zero), error * inverseDt, error * positionErrorToVelocity); - projection.BiasImpulse = biasVelocity * effectiveMass; - } - - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref TwistLimitProjection projection, ref Vector accumulatedImpulse) - { - TwistServoFunctions.ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, projection.ImpulseToVelocityA, projection.NegatedImpulseToVelocityB, accumulatedImpulse); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref TwistLimitProjection projection, ref Vector accumulatedImpulse) - { - Vector3Wide.Subtract(velocityA.Angular, velocityB.Angular, out var netVelocity); - Vector3Wide.Dot(netVelocity, projection.VelocityToImpulseA, out var csiVelocityComponent); - //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); - var csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - csiVelocityComponent; - var previousAccumulatedImpulse = accumulatedImpulse; - accumulatedImpulse = Vector.Max(Vector.Zero, accumulatedImpulse + csi); - csi = accumulatedImpulse - previousAccumulatedImpulse; - - TwistServoFunctions.ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, projection.ImpulseToVelocityA, projection.NegatedImpulseToVelocityB, csi); - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] static void ComputeJacobian(in QuaternionWide orientationA, in QuaternionWide orientationB, in QuaternionWide localBasisA, in QuaternionWide localBasisB, in Vector minimumAngle, in Vector maximumAngle, out Vector error, out Vector3Wide jacobianA) @@ -196,7 +135,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref TwistLimitPrestepData prestepData) { } } - public class TwistLimitTypeProcessor : TwoBodyTypeProcessor, TwistLimitFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular> + public class TwistLimitTypeProcessor : TwoBodyTypeProcessor, TwistLimitFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular> { public const int BatchTypeId = 27; } diff --git a/BepuPhysics/Constraints/TwistMotor.cs b/BepuPhysics/Constraints/TwistMotor.cs index 159a5d74b..67b76ce9e 100644 --- a/BepuPhysics/Constraints/TwistMotor.cs +++ b/BepuPhysics/Constraints/TwistMotor.cs @@ -73,64 +73,8 @@ public struct TwistMotorPrestepData public MotorSettingsWide Settings; } - public struct TwistMotorProjection + public struct TwistMotorFunctions : ITwoBodyConstraintFunctions> { - public Vector3Wide VelocityToImpulseA; - public Vector SoftnessImpulseScale; - public Vector BiasImpulse; - public Vector MaximumImpulse; - public Vector3Wide ImpulseToVelocityA; - public Vector3Wide NegatedImpulseToVelocityB; - } - - - public struct TwistMotorFunctions : ITwoBodyConstraintFunctions> - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, - float dt, float inverseDt, ref TwistMotorPrestepData prestep, out TwistMotorProjection projection) - { - Unsafe.SkipInit(out projection); - //We don't need any measurement basis in a velocity motor, so the prestep data needs only the axes. - QuaternionWide.TransformWithoutOverlap(prestep.LocalAxisA, orientationA, out var axisA); - QuaternionWide.TransformWithoutOverlap(prestep.LocalAxisB, orientationB, out var axisB); - Vector3Wide.Add(axisA, axisB, out var jacobianA); - Vector3Wide.Length(jacobianA, out var length); - Vector3Wide.Scale(jacobianA, Vector.One / length, out jacobianA); - Vector3Wide.ConditionalSelect(Vector.LessThan(length, new Vector(1e-10f)), axisA, jacobianA, out jacobianA); - - TwistServoFunctions.ComputeEffectiveMassContributions(inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, jacobianA, - out projection.ImpulseToVelocityA, out projection.NegatedImpulseToVelocityB, out var unsoftenedInverseEffectiveMass); - - MotorSettingsWide.ComputeSoftness(prestep.Settings, dt, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale, out projection.MaximumImpulse); - var effectiveMass = effectiveMassCFMScale / unsoftenedInverseEffectiveMass; - Vector3Wide.Scale(jacobianA, effectiveMass, out projection.VelocityToImpulseA); - - projection.BiasImpulse = prestep.TargetVelocity * effectiveMass; - } - - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref TwistMotorProjection projection, ref Vector accumulatedImpulse) - { - TwistServoFunctions.ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, projection.ImpulseToVelocityA, projection.NegatedImpulseToVelocityB, accumulatedImpulse); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref TwistMotorProjection projection, ref Vector accumulatedImpulse) - { - Vector3Wide.Subtract(velocityA.Angular, velocityB.Angular, out var netVelocity); - Vector3Wide.Dot(netVelocity, projection.VelocityToImpulseA, out var csiVelocityComponent); - //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); - var csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - csiVelocityComponent; - var previousAccumulatedImpulse = accumulatedImpulse; - accumulatedImpulse = Vector.Max(Vector.Min(accumulatedImpulse + csi, projection.MaximumImpulse), -projection.MaximumImpulse); - csi = accumulatedImpulse - previousAccumulatedImpulse; - - TwistServoFunctions.ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, projection.ImpulseToVelocityA, projection.NegatedImpulseToVelocityB, csi); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ComputeJacobian(in QuaternionWide orientationA, in QuaternionWide orientationB, in Vector3Wide localAxisA, in Vector3Wide localAxisB, out Vector3Wide jacobianA) { @@ -180,7 +124,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref TwistMotorPrestepData prestepData) { } } - public class TwistMotorTypeProcessor : TwoBodyTypeProcessor, TwistMotorFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular> + public class TwistMotorTypeProcessor : TwoBodyTypeProcessor, TwistMotorFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular> { public const int BatchTypeId = 28; } diff --git a/BepuPhysics/Constraints/TwistServo.cs b/BepuPhysics/Constraints/TwistServo.cs index 16d43e5e9..6c7806ce5 100644 --- a/BepuPhysics/Constraints/TwistServo.cs +++ b/BepuPhysics/Constraints/TwistServo.cs @@ -82,18 +82,7 @@ public struct TwistServoPrestepData public ServoSettingsWide ServoSettings; } - public struct TwistServoProjection - { - public Vector3Wide VelocityToImpulseA; - public Vector BiasImpulse; - public Vector SoftnessImpulseScale; - public Vector MaximumImpulse; - public Vector3Wide ImpulseToVelocityA; - public Vector3Wide NegatedImpulseToVelocityB; - } - - - public struct TwistServoFunctions : ITwoBodyConstraintFunctions> + public struct TwistServoFunctions : ITwoBodyConstraintFunctions> { [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ComputeJacobian(in QuaternionWide orientationA, in QuaternionWide orientationB, in QuaternionWide localBasisA, in QuaternionWide localBasisB, @@ -166,25 +155,6 @@ public static void ComputeEffectiveMass(float dt, in SpringSettingsWide springSe Vector3Wide.Scale(jacobianA, effectiveMass, out velocityToImpulseA); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, - float dt, float inverseDt, ref TwistServoPrestepData prestep, out TwistServoProjection projection) - { - Unsafe.SkipInit(out projection); - ComputeJacobian(orientationA, orientationB, prestep.LocalBasisA, prestep.LocalBasisB, - out var basisBX, out var basisBZ, out var basisA, out var jacobianA); - - ComputeEffectiveMass(dt, prestep.SpringSettings, inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, jacobianA, - out projection.ImpulseToVelocityA, out projection.NegatedImpulseToVelocityB, - out var positionErrorToVelocity, out projection.SoftnessImpulseScale, out var effectiveMass, out projection.VelocityToImpulseA); - - ComputeCurrentAngle(basisBX, basisBZ, basisA, out var angle); - - MathHelper.GetSignedAngleDifference(prestep.TargetAngle, angle, out var error); - - ServoSettingsWide.ComputeClampedBiasVelocity(error, positionErrorToVelocity, prestep.ServoSettings, dt, inverseDt, out var clampedBiasVelocity, out projection.MaximumImpulse); - projection.BiasImpulse = clampedBiasVelocity * effectiveMass; - } [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ApplyImpulse(ref Vector3Wide angularVelocityA, ref Vector3Wide angularVelocityB, in Vector3Wide impulseToVelocityA, in Vector3Wide negatedImpulseToVelocityB, in Vector csi) @@ -195,27 +165,6 @@ public static void ApplyImpulse(ref Vector3Wide angularVelocityA, ref Vector3Wid Vector3Wide.Subtract(angularVelocityB, negatedVelocityChangeB, out angularVelocityB); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref TwistServoProjection projection, ref Vector accumulatedImpulse) - { - ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, projection.ImpulseToVelocityA, projection.NegatedImpulseToVelocityB, accumulatedImpulse); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref TwistServoProjection projection, ref Vector accumulatedImpulse) - { - Vector3Wide.Subtract(velocityA.Angular, velocityB.Angular, out var netVelocity); - Vector3Wide.Dot(netVelocity, projection.VelocityToImpulseA, out var csiVelocityComponent); - //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); - var csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - csiVelocityComponent; - var previousAccumulatedImpulse = accumulatedImpulse; - accumulatedImpulse = Vector.Min(Vector.Max(accumulatedImpulse + csi, -projection.MaximumImpulse), projection.MaximumImpulse); - csi = accumulatedImpulse - previousAccumulatedImpulse; - - ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, projection.ImpulseToVelocityA, projection.NegatedImpulseToVelocityB, csi); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ComputeJacobian(in QuaternionWide orientationA, in QuaternionWide orientationB, in QuaternionWide localBasisA, in QuaternionWide localBasisB, out Vector3Wide jacobianA) { @@ -271,7 +220,7 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref TwistServoPrestepData prestepData) { } } - public class TwistServoTypeProcessor : TwoBodyTypeProcessor, TwistServoFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular> + public class TwistServoTypeProcessor : TwoBodyTypeProcessor, TwistServoFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular> { public const int BatchTypeId = 26; } diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index cdf79892d..369568939 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -26,13 +26,8 @@ public struct TwoBodyReferences /// /// Type of the prestep data used by the constraint. /// Type of the accumulated impulses used by the constraint. - /// Type of the projection to input. - public interface ITwoBodyConstraintFunctions + public interface ITwoBodyConstraintFunctions { - void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref TPrestepData prestepData, out TProjection projection); - void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref TProjection projection, ref TAccumulatedImpulse accumulatedImpulse); - void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref TProjection projection, ref TAccumulatedImpulse accumulatedImpulse); - void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref TPrestepData prestep, ref TAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB); void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, @@ -50,11 +45,11 @@ void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyIne /// /// Shared implementation across all two body constraints. /// - public abstract class TwoBodyTypeProcessor - : TypeProcessor - where TPrestepData : unmanaged where TProjection : unmanaged where TAccumulatedImpulse : unmanaged - where TConstraintFunctions : unmanaged, ITwoBodyConstraintFunctions + : TypeProcessor + where TPrestepData : unmanaged where TAccumulatedImpulse : unmanaged + where TConstraintFunctions : unmanaged, ITwoBodyConstraintFunctions where TWarmStartAccessFilterA : unmanaged, IBodyAccessFilter where TWarmStartAccessFilterB : unmanaged, IBodyAccessFilter where TSolveAccessFilterA : unmanaged, IBodyAccessFilter @@ -114,69 +109,6 @@ internal sealed override void VerifySortRegion(ref TypeBatch typeBatch, int bund VerifySortRegion(ref typeBatch, bundleStartIndex, constraintCount, ref sortedKeys, ref sortedSourceIndices); } - - - //The following covers the common loop logic for all two body constraints. Each iteration invokes the warm start function type. - //This abstraction should, in theory, have zero overhead if the implementation of the interface is in a struct with aggressive inlining. - - //By providing the overrides at this level, the concrete implementation (assuming it inherits from one of the prestep-providing variants) - //only has to specify *type* arguments associated with the interface-implementing struct-delegates. It's going to look very strange, but it's low overhead - //and minimizes per-type duplication. - - - public unsafe override void Prestep(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) - { - ref var prestepBase = ref Unsafe.AsRef(typeBatch.PrestepData.Memory); - ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); - ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); - var function = default(TConstraintFunctions); - for (int i = startBundle; i < exclusiveEndBundle; ++i) - { - ref var prestep = ref Unsafe.Add(ref prestepBase, i); - ref var projection = ref Unsafe.Add(ref projectionBase, i); - ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); - bodies.GatherState(ref references, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB); - function.Prestep(orientationA, inertiaA, ab, orientationB, inertiaB, dt, inverseDt, ref prestep, out projection); - } - } - - public unsafe override void WarmStart(ref TypeBatch typeBatch, Bodies bodies, int startBundle, int exclusiveEndBundle) - { - ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); - ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); - ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); - var function = default(TConstraintFunctions); - for (int i = startBundle; i < exclusiveEndBundle; ++i) - { - ref var projection = ref Unsafe.Add(ref projectionBase, i); - ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); - ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); - bodies.GatherState(ref references, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB); - function.WarmStart(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); - bodies.ScatterVelocities(ref wsvA, ref references.IndexA); - bodies.ScatterVelocities(ref wsvB, ref references.IndexB); - - } - } - - public unsafe override void SolveIteration(ref TypeBatch typeBatch, Bodies bodies, int startBundle, int exclusiveEndBundle) - { - ref var bodyReferencesBase = ref Unsafe.AsRef(typeBatch.BodyReferences.Memory); - ref var accumulatedImpulsesBase = ref Unsafe.AsRef(typeBatch.AccumulatedImpulses.Memory); - ref var projectionBase = ref Unsafe.AsRef(typeBatch.Projection.Memory); - var function = default(TConstraintFunctions); - for (int i = startBundle; i < exclusiveEndBundle; ++i) - { - ref var projection = ref Unsafe.Add(ref projectionBase, i); - ref var accumulatedImpulses = ref Unsafe.Add(ref accumulatedImpulsesBase, i); - ref var references = ref Unsafe.Add(ref bodyReferencesBase, i); - bodies.GatherState(ref references, out var orientationA, out var wsvA, out var inertiaA, out var ab, out var orientationB, out var wsvB, out var inertiaB); - function.Solve(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulses); - bodies.ScatterVelocities(ref wsvA, ref references.IndexA); - bodies.ScatterVelocities(ref wsvB, ref references.IndexB); - } - } - //public const int WarmStartPrefetchDistance = 8; //public const int SolvePrefetchDistance = 4; //[MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -231,6 +163,13 @@ public unsafe override void SolveIteration(ref TypeBatch typeBatch, Bodies bodie + //The following covers the common loop logic for all two body constraints. Each iteration invokes the warm start function type. + //This abstraction should, in theory, have zero overhead if the implementation of the interface is in a struct with aggressive inlining. + + //By providing the overrides at this level, the concrete implementation (assuming it inherits from one of the prestep-providing variants) + //only has to specify *type* arguments associated with the interface-implementing struct-delegates. It's going to look very strange, but it's low overhead + //and minimizes per-type duplication. + public unsafe override void WarmStart2( ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) @@ -312,10 +251,10 @@ public unsafe override void IncrementallyUpdateForSubstep(ref TypeBatch typeBatc } } - public abstract class TwoBodyContactTypeProcessor - : TwoBodyTypeProcessor - where TPrestepData : unmanaged where TProjection : unmanaged where TAccumulatedImpulse : unmanaged - where TConstraintFunctions : unmanaged, ITwoBodyConstraintFunctions + public abstract class TwoBodyContactTypeProcessor + : TwoBodyTypeProcessor + where TPrestepData : unmanaged where TAccumulatedImpulse : unmanaged + where TConstraintFunctions : unmanaged, ITwoBodyConstraintFunctions { } } diff --git a/BepuPhysics/Constraints/TypeBatch.cs b/BepuPhysics/Constraints/TypeBatch.cs index 9b1944fc1..ea754fd30 100644 --- a/BepuPhysics/Constraints/TypeBatch.cs +++ b/BepuPhysics/Constraints/TypeBatch.cs @@ -17,9 +17,6 @@ public struct TypeBatch public RawBuffer BodyReferences; public RawBuffer PrestepData; public RawBuffer AccumulatedImpulses; - //TODO: Note that we still include a projection buffer here- even though sleeping islands using this struct will never allocate space for it, - //and even though we may end up not even persisting the allocation between frames. We may later pull this out and store it strictly ephemerally in the solver. - public RawBuffer Projection; public Buffer IndexToHandle; public int ConstraintCount; public int TypeId; @@ -35,7 +32,6 @@ public int BundleCount public void Dispose(BufferPool pool) { - pool.Return(ref Projection); pool.Return(ref BodyReferences); pool.Return(ref PrestepData); pool.Return(ref AccumulatedImpulses); diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index 1da982b60..ef07768d6 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -201,10 +201,6 @@ internal unsafe abstract void CopySleepingToActive( public abstract void Initialize(ref TypeBatch typeBatch, int initialCapacity, BufferPool pool); public abstract void Resize(ref TypeBatch typeBatch, int newCapacity, BufferPool pool); - public abstract void Prestep(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); - public abstract void WarmStart(ref TypeBatch typeBatch, Bodies bodies, int startBundle, int exclusiveEndBundle); - public abstract void SolveIteration(ref TypeBatch typeBatch, Bodies bodies, int startBundle, int exclusiveEndBundle); - public abstract void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks poseIntegratorCallbacks, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) @@ -242,7 +238,7 @@ public interface ISortKeyGenerator where TBodyReferences : unma //Note that the only reason to have generics at the type level here is to avoid the need to specify them for each individual function. It's functionally equivalent, but this just //cuts down on the syntax noise a little bit. //Really, you could use a bunch of composed static generic helpers. - public abstract class TypeProcessor : TypeProcessor where TBodyReferences : unmanaged where TPrestepData : unmanaged where TProjection : unmanaged where TAccumulatedImpulse : unmanaged + public abstract class TypeProcessor : TypeProcessor where TBodyReferences : unmanaged where TPrestepData : unmanaged where TAccumulatedImpulse : unmanaged { protected override int InternalConstrainedDegreesOfFreedom { @@ -332,7 +328,6 @@ public unsafe sealed override int AllocateInTypeBatch(ref TypeBatch typeBatch, C GatherScatter.ClearLane(ref Buffer.Get(ref typeBatch.AccumulatedImpulses, bundleIndex), innerIndex); var bundleCount = typeBatch.BundleCount; Debug.Assert(typeBatch.PrestepData.Length >= bundleCount * Unsafe.SizeOf()); - Debug.Assert(typeBatch.Projection.Length >= bundleCount * Unsafe.SizeOf()); Debug.Assert(typeBatch.BodyReferences.Length >= bundleCount * Unsafe.SizeOf()); Debug.Assert(typeBatch.AccumulatedImpulses.Length >= bundleCount * Unsafe.SizeOf()); Debug.Assert(typeBatch.IndexToHandle.Length >= typeBatch.ConstraintCount); @@ -540,7 +535,6 @@ public unsafe sealed override int AllocateInTypeBatchForFallback(ref TypeBatch t //Clear the slot's accumulated impulse. The backing memory could be initialized to any value. GatherScatter.ClearLane(ref Buffer.Get(ref typeBatch.AccumulatedImpulses, bundleCount), 0); Debug.Assert(typeBatch.PrestepData.Length >= typeBatch.BundleCount * Unsafe.SizeOf()); - Debug.Assert(typeBatch.Projection.Length >= typeBatch.BundleCount * Unsafe.SizeOf()); Debug.Assert(typeBatch.BodyReferences.Length >= typeBatch.BundleCount * Unsafe.SizeOf()); Debug.Assert(typeBatch.AccumulatedImpulses.Length >= typeBatch.BundleCount * Unsafe.SizeOf()); Debug.Assert(typeBatch.IndexToHandle.Length >= typeBatch.ConstraintCount); @@ -568,7 +562,6 @@ public unsafe sealed override int AllocateInTypeBatchForFallback(ref TypeBatch t typeBatch.IndexToHandle[indexInTypeBatch] = handle; Debug.Assert(typeBatch.ConstraintCount <= typeBatch.IndexToHandle.Length); Debug.Assert(typeBatch.PrestepData.Length >= bundleCount * Unsafe.SizeOf()); - Debug.Assert(typeBatch.Projection.Length >= bundleCount * Unsafe.SizeOf()); Debug.Assert(typeBatch.BodyReferences.Length >= bundleCount * Unsafe.SizeOf()); Debug.Assert(typeBatch.AccumulatedImpulses.Length >= bundleCount * Unsafe.SizeOf()); //ValidateEmptyFallbackSlots(ref typeBatch); @@ -780,7 +773,6 @@ void InternalResize(ref TypeBatch typeBatch, BufferPool pool, int constraintCapa var bundleCapacity = BundleIndexing.GetBundleCount(typeBatch.IndexToHandle.Length); //Note that the projection is not copied over. It is ephemeral data. (In the same vein as above, if memory is an issue, we could just allocate projections on demand.) var bundleCount = typeBatch.BundleCount; - pool.ResizeToAtLeast(ref typeBatch.Projection, bundleCapacity * Unsafe.SizeOf(), 0); pool.ResizeToAtLeast(ref typeBatch.BodyReferences, bundleCapacity * Unsafe.SizeOf(), bundleCount * Unsafe.SizeOf()); pool.ResizeToAtLeast(ref typeBatch.PrestepData, bundleCapacity * Unsafe.SizeOf(), bundleCount * Unsafe.SizeOf()); pool.ResizeToAtLeast(ref typeBatch.AccumulatedImpulses, bundleCapacity * Unsafe.SizeOf(), bundleCount * Unsafe.SizeOf()); diff --git a/BepuPhysics/Constraints/VolumeConstraint.cs b/BepuPhysics/Constraints/VolumeConstraint.cs index 9ea44400d..5e45daa6c 100644 --- a/BepuPhysics/Constraints/VolumeConstraint.cs +++ b/BepuPhysics/Constraints/VolumeConstraint.cs @@ -73,140 +73,8 @@ public struct VolumeConstraintPrestepData public SpringSettingsWide SpringSettings; } - public struct VolumeConstraintProjection + public struct VolumeConstraintFunctions : IFourBodyConstraintFunctions> { - public Vector3Wide JacobianB; - public Vector3Wide JacobianC; - public Vector3Wide JacobianD; - public Vector EffectiveMass; - public Vector BiasImpulse; - public Vector SoftnessImpulseScale; - public Vector InverseMassA; - public Vector InverseMassB; - public Vector InverseMassC; - public Vector InverseMassD; - } - - public struct VolumeConstraintFunctions : IFourBodyConstraintFunctions> - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep( - in QuaternionWide orientationA, in BodyInertiaWide inertiaA, - in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, - in Vector3Wide ac, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, - in Vector3Wide ad, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, - float dt, float inverseDt, ref VolumeConstraintPrestepData prestep, out VolumeConstraintProjection projection) - { - //Volume of parallelepiped with vertices a, b, c, d is: - //(ab x ac) * ad - //A tetrahedron with the same edges will have one sixth of this volume. As a constant factor, it's not relevant. So the constraint is just: - //OriginalVolume * 6 = (ab x ac) * ad - //Taking the derivative to get the velocity constraint: - //0 = d/dt(ab x ac) * ad + (ab x ac) * d/dt(ad) - //0 = (d/dt(ab) x ac + ab x d/dt(ac)) * ad + (ab x ac) * d/dt(ad) - //0 = (d/dt(ab) x ac) * ad + (ab x d/dt(ac)) * ad + (ab x ac) * d/dt(ad) - //0 = (ac x ad) * d/dt(ab) + (ad x ab) * d/dt(ac) + (ab x ac) * d/dt(ad) - //Giving the linear jacobians: - //JA: -ac x ad - ad x ab - ab x ac - //JB: ac x ad - //JC: ad x ab - //JD: ab x ac - //JA could be compressed down to a form similar to the other jacobians with some algebra, but there's no need since it's cheap to just perform a few subtractions. - //Note that we don't store out the jacobian for A either. A's jacobian is cheaply found from B, C, and D. - //We're not blending the jacobians into the effective mass or inverse mass either- even though that would save ALU time, the goal here is to minimize memory bandwidth since that - //tends to be the bottleneck for any multithreaded simulation. (Despite being a 1DOF constraint, this doesn't need to output inverse inertia tensors, so premultiplying isn't a win.) - - Vector3Wide.CrossWithoutOverlap(ac, ad, out projection.JacobianB); - Vector3Wide.CrossWithoutOverlap(ad, ab, out projection.JacobianC); - Vector3Wide.CrossWithoutOverlap(ab, ac, out projection.JacobianD); - Vector3Wide.Add(projection.JacobianB, projection.JacobianC, out var negatedJA); - Vector3Wide.Add(projection.JacobianD, negatedJA, out negatedJA); - - Vector3Wide.Dot(negatedJA, negatedJA, out var contributionA); - Vector3Wide.Dot(projection.JacobianB, projection.JacobianB, out var contributionB); - Vector3Wide.Dot(projection.JacobianC, projection.JacobianC, out var contributionC); - Vector3Wide.Dot(projection.JacobianD, projection.JacobianD, out var contributionD); - - //Protect against singularity by padding the jacobian contributions. This is very much a hack, but it's a pretty simple hack. - //Less sensitive to tuning than attempting to guard the inverseEffectiveMass itself, since that is sensitive to both scale AND mass. - - //Choose an epsilon based on the target volume. Note that volume ~= width^3, whereas our jacobian contributions are things like (ac x ad) * (ac x ad), which is proportional - //to the area of the triangle acd squared. In other words, the contribution is ~ width^4. - //Scaling the volume by a constant factor will not match the growth rate of the jacobian contributions. - //We're going to ignore this until proven to be a noticeable problem because Vector does not expose exp or pow and this is cheap. - //Could still implement it, but it's not super high value. - var epsilon = 5e-4f * prestep.TargetScaledVolume; - contributionA = Vector.Max(epsilon, contributionA); - contributionB = Vector.Max(epsilon, contributionB); - contributionC = Vector.Max(epsilon, contributionC); - contributionD = Vector.Max(epsilon, contributionD); - var inverseEffectiveMass = contributionA * inertiaA.InverseMass + contributionB * inertiaB.InverseMass + contributionC * inertiaC.InverseMass + contributionD * inertiaD.InverseMass; - projection.InverseMassA = inertiaA.InverseMass; - projection.InverseMassB = inertiaB.InverseMass; - projection.InverseMassC = inertiaC.InverseMass; - projection.InverseMassD = inertiaD.InverseMass; - - SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); - - projection.EffectiveMass = effectiveMassCFMScale / inverseEffectiveMass; - //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. - Vector3Wide.Dot(projection.JacobianD, ad, out var unscaledVolume); - projection.BiasImpulse = (prestep.TargetScaledVolume - unscaledVolume) * (1f / 6f) * positionErrorToVelocity * projection.EffectiveMass; - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - private static void ApplyImpulse(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref BodyVelocityWide velocityC, ref BodyVelocityWide velocityD, - ref VolumeConstraintProjection projection, ref Vector3Wide negatedJacobianA, ref Vector impulse) - { - Vector3Wide.Scale(negatedJacobianA, projection.InverseMassA * impulse, out var negativeVelocityChangeA); - Vector3Wide.Scale(projection.JacobianB, projection.InverseMassB * impulse, out var velocityChangeB); - Vector3Wide.Scale(projection.JacobianC, projection.InverseMassC * impulse, out var velocityChangeC); - Vector3Wide.Scale(projection.JacobianD, projection.InverseMassD * impulse, out var velocityChangeD); - Vector3Wide.Subtract(velocityA.Linear, negativeVelocityChangeA, out velocityA.Linear); - Vector3Wide.Add(velocityB.Linear, velocityChangeB, out velocityB.Linear); - Vector3Wide.Add(velocityC.Linear, velocityChangeC, out velocityC.Linear); - Vector3Wide.Add(velocityD.Linear, velocityChangeD, out velocityD.Linear); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - private static void GetNegatedJacobianA(in VolumeConstraintProjection projection, out Vector3Wide jacobianA) - { - Vector3Wide.Add(projection.JacobianB, projection.JacobianC, out jacobianA); - Vector3Wide.Add(projection.JacobianD, jacobianA, out jacobianA); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref BodyVelocityWide velocityC, ref BodyVelocityWide velocityD, ref VolumeConstraintProjection projection, ref Vector accumulatedImpulse) - { - //Unlike most constraints, the jacobians in a volume constraint can change magnitude and direction wildly in some cases. - //Reusing the previous frame's accumulated impulse can result in catastrophically wrong guesses which require many iterations to correct. - //Instead, for now, we simply clear the accumulated impulse. The constraint will be a little softer during sustained forces because of this, but it helps avoid - //explosions in the worst case and the slight softness isn't usually a big issue for volume constraints. - //TODO: This is a fairly hacky approach since we already loaded the velocities despite not doing anything with them. - //Two options: fix the underlying issue by updating the accumulated impulse in response to changes in the jacobian, or special case this by not loading the velocities at all. - accumulatedImpulse = default; - //A true warm start would look like this: - //GetNegatedJacobianA(projection, out var negatedJacobianA); - //ApplyImpulse(ref velocityA, ref velocityB, ref velocityC, ref velocityD, ref projection, ref negatedJacobianA, ref accumulatedImpulse); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref BodyVelocityWide velocityC, ref BodyVelocityWide velocityD, ref VolumeConstraintProjection projection, ref Vector accumulatedImpulse) - { - //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); - GetNegatedJacobianA(projection, out var negatedJacobianA); - Vector3Wide.Dot(negatedJacobianA, velocityA.Linear, out var negatedContributionA); - Vector3Wide.Dot(projection.JacobianB, velocityB.Linear, out var contributionB); - Vector3Wide.Dot(projection.JacobianC, velocityC.Linear, out var contributionC); - Vector3Wide.Dot(projection.JacobianD, velocityD.Linear, out var contributionD); - var csv = contributionB + contributionC + contributionD - negatedContributionA; - var csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - csv * projection.EffectiveMass; - accumulatedImpulse += csi; - - ApplyImpulse(ref velocityA, ref velocityB, ref velocityC, ref velocityD, ref projection, ref negatedJacobianA, ref csi); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] private static void ApplyImpulse( in Vector inverseMassA, in Vector inverseMassB, in Vector inverseMassC, in Vector inverseMassD, @@ -313,7 +181,7 @@ public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWi /// /// Handles the solve iterations of a bunch of volume constraints. /// - public class VolumeConstraintTypeProcessor : FourBodyTypeProcessor, VolumeConstraintFunctions, AccessOnlyLinear, AccessOnlyLinear, AccessOnlyLinear, AccessOnlyLinear, AccessOnlyLinear, AccessOnlyLinear, AccessOnlyLinear, AccessOnlyLinear> + public class VolumeConstraintTypeProcessor : FourBodyTypeProcessor, VolumeConstraintFunctions, AccessOnlyLinear, AccessOnlyLinear, AccessOnlyLinear, AccessOnlyLinear, AccessOnlyLinear, AccessOnlyLinear, AccessOnlyLinear, AccessOnlyLinear> { public const int BatchTypeId = 32; } diff --git a/BepuPhysics/Constraints/Weld.cs b/BepuPhysics/Constraints/Weld.cs index 8a5bf993d..c8f0253d1 100644 --- a/BepuPhysics/Constraints/Weld.cs +++ b/BepuPhysics/Constraints/Weld.cs @@ -74,162 +74,14 @@ public struct WeldPrestepData public SpringSettingsWide SpringSettings; } - public struct WeldProjection - { - public Vector3Wide Offset; - public Vector3Wide OffsetBiasVelocity; - public Vector3Wide OrientationBiasVelocity; - public Symmetric6x6Wide EffectiveMass; - public Vector SoftnessImpulseScale; - public BodyInertiaWide InertiaA; - public BodyInertiaWide InertiaB; - } - public struct WeldAccumulatedImpulses { public Vector3Wide Orientation; public Vector3Wide Offset; } - public struct WeldFunctions : ITwoBodyConstraintFunctions + public struct WeldFunctions : ITwoBodyConstraintFunctions { - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep(in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, - ref WeldPrestepData prestep, out WeldProjection projection) - { - projection.InertiaA = inertiaA; - projection.InertiaB = inertiaB; - - //The weld constraint handles 6 degrees of freedom simultaneously. The constraints are: - //localOrientation * orientationA = orientationB - //positionA + localOffset * orientationA = positionB - //The velocity derivatives: - //angularVelocityA = angularVelocityB - //linearVelocityA + angularVelocityA x (localOffset * orientationA) = linearVelocityB - //Note that the position constraint is similar a ball socket joint, except the anchor point is on top of the center of mass of object B. - - //From the above, the jacobians ordered as [linearA, angularA, linearB, angularB] are: - //J = [ 0, I, 0, -I ] - // [ I, skewSymmetric(localOffset * orientationA), -I, 0 ] - //where I is the 3x3 identity matrix. - //Effective mass = (J * M^-1 * JT)^-1, which is going to be a little tricky because J * M^-1 * JT is a 6x6 matrix: - //J * M^-1 * JT = [ Ia^-1 + Ib^-1, Ia^-1 * transpose(skewSymmetric(localOffset * orientationA)) ] - // [ skewSymmetric(localOffset * orientationA) * Ia^-1, Ma^-1 + Mb^-1 + skewSymmetric(localOffset * orientationA) * Ia^-1 * transpose(skewSymmetric(localOffset * orientationA)) ] - //where Ia^-1 and Ib^-1 are the inverse inertia tensors for a and b and Ma^-1 and Mb^-1 are the inverse masses of A and B expanded to 3x3 diagonal matrices. - //It's worth noting that the effective mass- and its inverse- are symmetric, so we can cut down the inverse computations. - Symmetric3x3Wide.Add(inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, out var jmjtA); - QuaternionWide.TransformWithoutOverlap(prestep.LocalOffset, orientationA, out projection.Offset); - Matrix3x3Wide.CreateCrossProduct(projection.Offset, out var xAB); - Symmetric3x3Wide.Multiply(inertiaA.InverseInertiaTensor, xAB, out var jmjtB); - Symmetric3x3Wide.CompleteMatrixSandwichTranspose(xAB, jmjtB, out var jmjtD); - var diagonalAdd = inertiaA.InverseMass + inertiaB.InverseMass; - jmjtD.XX += diagonalAdd; - jmjtD.YY += diagonalAdd; - jmjtD.ZZ += diagonalAdd; - Symmetric6x6Wide.Invert(jmjtA, jmjtB, jmjtD, out projection.EffectiveMass); - - SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); - Symmetric6x6Wide.Scale(projection.EffectiveMass, effectiveMassCFMScale, out projection.EffectiveMass); - - //Compute the current constraint error for all 6 degrees of freedom. - //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. - Vector3Wide.Subtract(ab, projection.Offset, out var positionError); - QuaternionWide.ConcatenateWithoutOverlap(prestep.LocalOrientation, orientationA, out var targetOrientationB); - QuaternionWide.Conjugate(targetOrientationB, out var inverseTarget); - QuaternionWide.ConcatenateWithoutOverlap(inverseTarget, orientationB, out var rotationError); - QuaternionWide.GetApproximateAxisAngleFromQuaternion(rotationError, out var rotationErrorAxis, out var rotationErrorLength); - - Vector3Wide.Scale(positionError, positionErrorToVelocity, out projection.OffsetBiasVelocity); - Vector3Wide.Scale(rotationErrorAxis, rotationErrorLength * positionErrorToVelocity, out projection.OrientationBiasVelocity); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - private static void ApplyImpulse(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref WeldProjection projection, ref Vector3Wide orientationCSI, ref Vector3Wide offsetCSI) - { - //Recall the jacobians: - //J = [ 0, I, 0, -I ] - // [ I, skewSymmetric(localOffset * orientationA), -I, 0 ] - //The velocity changes are: - // csi * J * I^-1 - //linearImpulseA = offsetCSI - //angularImpulseA = orientationCSI + worldOffset x offsetCSI - //linearImpulseB = -offsetCSI - //angularImpulseB = -orientationCSI - Vector3Wide.Scale(offsetCSI, projection.InertiaA.InverseMass, out var linearChangeA); - Vector3Wide.Add(velocityA.Linear, linearChangeA, out velocityA.Linear); - - //Note order of cross relative to the SolveIteration. - //SolveIteration transforms velocity into constraint space velocity using JT, while this converts constraint space to world space using J. - //The elements are transposed, and transposed skew symmetric matrices are negated. Flipping the cross product is equivalent to a negation. - Vector3Wide.CrossWithoutOverlap(projection.Offset, offsetCSI, out var offsetWorldImpulse); - Vector3Wide.Add(offsetWorldImpulse, orientationCSI, out var angularImpulseA); - Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseA, projection.InertiaA.InverseInertiaTensor, out var angularChangeA); - Vector3Wide.Add(velocityA.Angular, angularChangeA, out velocityA.Angular); - - Vector3Wide.Scale(offsetCSI, projection.InertiaB.InverseMass, out var negatedLinearChangeB); - Vector3Wide.Subtract(velocityB.Linear, negatedLinearChangeB, out velocityB.Linear); //note subtraction; the jacobian is -I - - Symmetric3x3Wide.TransformWithoutOverlap(orientationCSI, projection.InertiaB.InverseInertiaTensor, out var negatedAngularChangeB); - Vector3Wide.Subtract(velocityB.Angular, negatedAngularChangeB, out velocityB.Angular); //note subtraction; the jacobian is -I - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref WeldProjection projection, ref WeldAccumulatedImpulses accumulatedImpulse) - { - ApplyImpulse(ref velocityA, ref velocityB, ref projection, ref accumulatedImpulse.Orientation, ref accumulatedImpulse.Offset); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref WeldProjection projection, ref WeldAccumulatedImpulses accumulatedImpulse) - { - //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); - //csv = V * JT - Vector3Wide.Subtract(velocityA.Angular, velocityB.Angular, out var orientationCSV); - Vector3Wide.Subtract(velocityA.Linear, velocityB.Linear, out var offsetCSV); - - Vector3Wide.CrossWithoutOverlap(velocityA.Angular, projection.Offset, out var offsetAngularCSV); - Vector3Wide.Add(offsetCSV, offsetAngularCSV, out offsetCSV); - - //Note subtraction: this is computing biasVelocity - csv, and later we'll compute (biasVelocity-csv) - softness. - Vector3Wide.Subtract(projection.OrientationBiasVelocity, orientationCSV, out orientationCSV); - Vector3Wide.Subtract(projection.OffsetBiasVelocity, offsetCSV, out offsetCSV); - - Symmetric6x6Wide.TransformWithoutOverlap(orientationCSV, offsetCSV, projection.EffectiveMass, out var orientationCSI, out var offsetCSI); - Vector3Wide.Scale(accumulatedImpulse.Offset, projection.SoftnessImpulseScale, out var offsetSoftness); - Vector3Wide.Scale(accumulatedImpulse.Orientation, projection.SoftnessImpulseScale, out var orientationSoftness); - Vector3Wide.Subtract(offsetCSI, offsetSoftness, out offsetCSI); - Vector3Wide.Subtract(orientationCSI, orientationSoftness, out orientationCSI); - Vector3Wide.Add(accumulatedImpulse.Orientation, orientationCSI, out accumulatedImpulse.Orientation); - Vector3Wide.Add(accumulatedImpulse.Offset, offsetCSI, out accumulatedImpulse.Offset); - - ApplyImpulse(ref velocityA, ref velocityB, ref projection, ref orientationCSI, ref offsetCSI); - } - - - ////[MethodImpl(MethodImplOptions.AggressiveInlining)] - //private static void ApplyImpulse(in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, in Vector3Wide offset, in Vector3Wide orientationCSI, in Vector3Wide offsetCSI, ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB) - //{ - // //Recall the jacobians: - // //J = [ 0, I, 0, -I ] - // // [ I, skewSymmetric(localOffset * orientationA), -I, 0 ] - // //The velocity changes are: - // // csi * J * I^-1 - // //linearImpulseA = offsetCSI - // //angularImpulseA = orientationCSI + worldOffset x offsetCSI - // //linearImpulseB = -offsetCSI - // //angularImpulseB = -orientationCSI - // velocityA.Linear += offsetCSI * inertiaA.InverseMass; - - // //Note order of cross relative to the SolveIteration. - // //SolveIteration transforms velocity into constraint space velocity using JT, while this converts constraint space to world space using J. - // //The elements are transposed, and transposed skew symmetric matrices are negated. Flipping the cross product is equivalent to a negation. - // velocityA.Angular += (Cross(offset, offsetCSI) + orientationCSI) * inertiaA.InverseInertiaTensor; - - // velocityB.Linear -= offsetCSI * inertiaB.InverseMass;//note subtraction; the jacobian is -I - // velocityB.Angular -= orientationCSI * inertiaB.InverseInertiaTensor;//note subtraction; the jacobian is -I - //} - //[MethodImpl(MethodImplOptions.AggressiveInlining)] private static void ApplyImpulse(in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, in Vector3Wide offset, in Vector3Wide orientationCSI, in Vector3Wide offsetCSI, ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB) { @@ -367,7 +219,7 @@ public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWi /// /// Handles the solve iterations of a bunch of ball socket constraints. /// - public class WeldTypeProcessor : TwoBodyTypeProcessor + public class WeldTypeProcessor : TwoBodyTypeProcessor { public const int BatchTypeId = 31; } diff --git a/DemoRenderer/Constraints/ConstraintLineExtractor.cs b/DemoRenderer/Constraints/ConstraintLineExtractor.cs index bdf7ecead..52c01e3ab 100644 --- a/DemoRenderer/Constraints/ConstraintLineExtractor.cs +++ b/DemoRenderer/Constraints/ConstraintLineExtractor.cs @@ -26,7 +26,7 @@ abstract class TypeLineExtractor public abstract void ExtractLines(Bodies bodies, int setIndex, ref TypeBatch typeBatch, int constraintStart, int constraintCount, ref QuickList lines); } - class TypeLineExtractor : TypeLineExtractor + class TypeLineExtractor : TypeLineExtractor where TBodyReferences : unmanaged where TPrestep : unmanaged where T : struct, IConstraintLineExtractor @@ -123,45 +123,45 @@ public ConstraintLineExtractor(BufferPool pool) { this.pool = pool; lineExtractors = new TypeLineExtractor[32]; - AllocateSlot(BallSocketTypeProcessor.BatchTypeId) = new TypeLineExtractor(); - AllocateSlot(BallSocketServoTypeProcessor.BatchTypeId) = new TypeLineExtractor(); - AllocateSlot(BallSocketMotorTypeProcessor.BatchTypeId) = new TypeLineExtractor(); - AllocateSlot(WeldTypeProcessor.BatchTypeId) = new TypeLineExtractor(); - AllocateSlot(DistanceServoTypeProcessor.BatchTypeId) = new TypeLineExtractor>(); - AllocateSlot(DistanceLimitTypeProcessor.BatchTypeId) = new TypeLineExtractor>(); - AllocateSlot(CenterDistanceTypeProcessor.BatchTypeId) = new TypeLineExtractor>(); - AllocateSlot(PointOnLineServoTypeProcessor.BatchTypeId) = new TypeLineExtractor(); - AllocateSlot(LinearAxisServoTypeProcessor.BatchTypeId) = new TypeLineExtractor>(); - AllocateSlot(AngularSwivelHingeTypeProcessor.BatchTypeId) = new TypeLineExtractor>(); - AllocateSlot(SwivelHingeTypeProcessor.BatchTypeId) = new TypeLineExtractor(); - AllocateSlot(HingeTypeProcessor.BatchTypeId) = new TypeLineExtractor(); - AllocateSlot(OneBodyLinearServoTypeProcessor.BatchTypeId) = new TypeLineExtractor, OneBodyLinearServoPrestepData, OneBodyLinearServoProjection, Vector>(); + AllocateSlot(BallSocketTypeProcessor.BatchTypeId) = new TypeLineExtractor(); + AllocateSlot(BallSocketServoTypeProcessor.BatchTypeId) = new TypeLineExtractor(); + AllocateSlot(BallSocketMotorTypeProcessor.BatchTypeId) = new TypeLineExtractor(); + AllocateSlot(WeldTypeProcessor.BatchTypeId) = new TypeLineExtractor(); + AllocateSlot(DistanceServoTypeProcessor.BatchTypeId) = new TypeLineExtractor>(); + AllocateSlot(DistanceLimitTypeProcessor.BatchTypeId) = new TypeLineExtractor>(); + AllocateSlot(CenterDistanceTypeProcessor.BatchTypeId) = new TypeLineExtractor>(); + AllocateSlot(PointOnLineServoTypeProcessor.BatchTypeId) = new TypeLineExtractor(); + AllocateSlot(LinearAxisServoTypeProcessor.BatchTypeId) = new TypeLineExtractor>(); + AllocateSlot(AngularSwivelHingeTypeProcessor.BatchTypeId) = new TypeLineExtractor>(); + AllocateSlot(SwivelHingeTypeProcessor.BatchTypeId) = new TypeLineExtractor(); + AllocateSlot(HingeTypeProcessor.BatchTypeId) = new TypeLineExtractor(); + AllocateSlot(OneBodyLinearServoTypeProcessor.BatchTypeId) = new TypeLineExtractor, OneBodyLinearServoPrestepData, Vector>(); - AllocateSlot(Contact1OneBodyTypeProcessor.BatchTypeId) = new TypeLineExtractor, Contact1OneBodyPrestepData, Contact1OneBodyProjection, Contact1AccumulatedImpulses>(); - AllocateSlot(Contact2OneBodyTypeProcessor.BatchTypeId) = new TypeLineExtractor, Contact2OneBodyPrestepData, Contact2OneBodyProjection, Contact2AccumulatedImpulses>(); - AllocateSlot(Contact3OneBodyTypeProcessor.BatchTypeId) = new TypeLineExtractor, Contact3OneBodyPrestepData, Contact3OneBodyProjection, Contact3AccumulatedImpulses>(); - AllocateSlot(Contact4OneBodyTypeProcessor.BatchTypeId) = new TypeLineExtractor, Contact4OneBodyPrestepData, Contact4OneBodyProjection, Contact4AccumulatedImpulses>(); + AllocateSlot(Contact1OneBodyTypeProcessor.BatchTypeId) = new TypeLineExtractor, Contact1OneBodyPrestepData, Contact1AccumulatedImpulses>(); + AllocateSlot(Contact2OneBodyTypeProcessor.BatchTypeId) = new TypeLineExtractor, Contact2OneBodyPrestepData, Contact2AccumulatedImpulses>(); + AllocateSlot(Contact3OneBodyTypeProcessor.BatchTypeId) = new TypeLineExtractor, Contact3OneBodyPrestepData, Contact3AccumulatedImpulses>(); + AllocateSlot(Contact4OneBodyTypeProcessor.BatchTypeId) = new TypeLineExtractor, Contact4OneBodyPrestepData, Contact4AccumulatedImpulses>(); - AllocateSlot(Contact1TypeProcessor.BatchTypeId) = new TypeLineExtractor(); - AllocateSlot(Contact2TypeProcessor.BatchTypeId) = new TypeLineExtractor(); - AllocateSlot(Contact3TypeProcessor.BatchTypeId) = new TypeLineExtractor(); - AllocateSlot(Contact4TypeProcessor.BatchTypeId) = new TypeLineExtractor(); + AllocateSlot(Contact1TypeProcessor.BatchTypeId) = new TypeLineExtractor(); + AllocateSlot(Contact2TypeProcessor.BatchTypeId) = new TypeLineExtractor(); + AllocateSlot(Contact3TypeProcessor.BatchTypeId) = new TypeLineExtractor(); + AllocateSlot(Contact4TypeProcessor.BatchTypeId) = new TypeLineExtractor(); - AllocateSlot(Contact2NonconvexOneBodyTypeProcessor.BatchTypeId) = new TypeLineExtractor, Contact2NonconvexOneBodyPrestepData, Contact2NonconvexOneBodyProjection, Contact2NonconvexAccumulatedImpulses>(); - AllocateSlot(Contact3NonconvexOneBodyTypeProcessor.BatchTypeId) = new TypeLineExtractor, Contact3NonconvexOneBodyPrestepData, Contact3NonconvexOneBodyProjection, Contact3NonconvexAccumulatedImpulses>(); - AllocateSlot(Contact4NonconvexOneBodyTypeProcessor.BatchTypeId) = new TypeLineExtractor, Contact4NonconvexOneBodyPrestepData, Contact4NonconvexOneBodyProjection, Contact4NonconvexAccumulatedImpulses>(); - //AllocateSlot(Contact5NonconvexOneBodyTypeProcessor.BatchTypeId) = new TypeLineExtractor, Contact5NonconvexOneBodyPrestepData, Contact5NonconvexOneBodyProjection, Contact5NonconvexAccumulatedImpulses>(); - //AllocateSlot(Contact6NonconvexOneBodyTypeProcessor.BatchTypeId) = new TypeLineExtractor, Contact6NonconvexOneBodyPrestepData, Contact6NonconvexOneBodyProjection, Contact6NonconvexAccumulatedImpulses>(); - //AllocateSlot(Contact7NonconvexOneBodyTypeProcessor.BatchTypeId) = new TypeLineExtractor, Contact7NonconvexOneBodyPrestepData, Contact7NonconvexOneBodyProjection, Contact7NonconvexAccumulatedImpulses>(); - //AllocateSlot(Contact8NonconvexOneBodyTypeProcessor.BatchTypeId) = new TypeLineExtractor, Contact8NonconvexOneBodyPrestepData, Contact8NonconvexOneBodyProjection, Contact8NonconvexAccumulatedImpulses>(); + AllocateSlot(Contact2NonconvexOneBodyTypeProcessor.BatchTypeId) = new TypeLineExtractor, Contact2NonconvexOneBodyPrestepData, Contact2NonconvexAccumulatedImpulses>(); + AllocateSlot(Contact3NonconvexOneBodyTypeProcessor.BatchTypeId) = new TypeLineExtractor, Contact3NonconvexOneBodyPrestepData, Contact3NonconvexAccumulatedImpulses>(); + AllocateSlot(Contact4NonconvexOneBodyTypeProcessor.BatchTypeId) = new TypeLineExtractor, Contact4NonconvexOneBodyPrestepData, Contact4NonconvexAccumulatedImpulses>(); + //AllocateSlot(Contact5NonconvexOneBodyTypeProcessor.BatchTypeId) = new TypeLineExtractor, Contact5NonconvexOneBodyPrestepData, Contact5NonconvexAccumulatedImpulses>(); + //AllocateSlot(Contact6NonconvexOneBodyTypeProcessor.BatchTypeId) = new TypeLineExtractor, Contact6NonconvexOneBodyPrestepData, Contact6NonconvexAccumulatedImpulses>(); + //AllocateSlot(Contact7NonconvexOneBodyTypeProcessor.BatchTypeId) = new TypeLineExtractor, Contact7NonconvexOneBodyPrestepData, Contact7NonconvexAccumulatedImpulses>(); + //AllocateSlot(Contact8NonconvexOneBodyTypeProcessor.BatchTypeId) = new TypeLineExtractor, Contact8NonconvexOneBodyPrestepData, Contact8NonconvexAccumulatedImpulses>(); - AllocateSlot(Contact2NonconvexTypeProcessor.BatchTypeId) = new TypeLineExtractor(); - AllocateSlot(Contact3NonconvexTypeProcessor.BatchTypeId) = new TypeLineExtractor(); - AllocateSlot(Contact4NonconvexTypeProcessor.BatchTypeId) = new TypeLineExtractor(); - //AllocateSlot(Contact5NonconvexTypeProcessor.BatchTypeId) = new TypeLineExtractor(); - //AllocateSlot(Contact6NonconvexTypeProcessor.BatchTypeId) = new TypeLineExtractor(); - //AllocateSlot(Contact7NonconvexTypeProcessor.BatchTypeId) = new TypeLineExtractor(); - //AllocateSlot(Contact8NonconvexTypeProcessor.BatchTypeId) = new TypeLineExtractor(); + AllocateSlot(Contact2NonconvexTypeProcessor.BatchTypeId) = new TypeLineExtractor(); + AllocateSlot(Contact3NonconvexTypeProcessor.BatchTypeId) = new TypeLineExtractor(); + AllocateSlot(Contact4NonconvexTypeProcessor.BatchTypeId) = new TypeLineExtractor(); + //AllocateSlot(Contact5NonconvexTypeProcessor.BatchTypeId) = new TypeLineExtractor(); + //AllocateSlot(Contact6NonconvexTypeProcessor.BatchTypeId) = new TypeLineExtractor(); + //AllocateSlot(Contact7NonconvexTypeProcessor.BatchTypeId) = new TypeLineExtractor(); + //AllocateSlot(Contact8NonconvexTypeProcessor.BatchTypeId) = new TypeLineExtractor(); jobs = new QuickList(Environment.ProcessorCount * (jobsPerThread + 1), pool); diff --git a/Demos/Demos/Characters/CharacterMotionConstraint.cs b/Demos/Demos/Characters/CharacterMotionConstraint.cs index 2910216d1..6c148e32a 100644 --- a/Demos/Demos/Characters/CharacterMotionConstraint.cs +++ b/Demos/Demos/Characters/CharacterMotionConstraint.cs @@ -118,23 +118,7 @@ public struct StaticCharacterMotionPrestep public Vector3Wide OffsetFromCharacter; } - //Using the prestep data plus some current body state, the solver computes the information required to execute velocity iterations. The main purpose of this intermediate data - //is to describe the projection from body velocities into constraint space impulses, and from constraint space impulses to body velocities again. - public struct StaticCharacterMotionProjection - { - public QuaternionWide SurfaceBasis; - public Vector3Wide OffsetFromCharacter; - public Vector2Wide TargetVelocity; - public Symmetric2x2Wide HorizontalEffectiveMass; - public Vector MaximumHorizontalImpulse; - public BodyInertiaWide InertiaA; - public Vector VerticalBiasVelocity; - public Vector VerticalEffectiveMass; - public Vector MaximumVerticalForce; - - } - - public struct StaticCharacterMotionFunctions : IOneBodyConstraintFunctions + public struct StaticCharacterMotionFunctions : IOneBodyConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] static void ComputeJacobians(in Vector3Wide offsetA, in QuaternionWide basisQuaternion, @@ -165,64 +149,6 @@ static void ComputeJacobians(in Vector3Wide offsetA, in QuaternionWide basisQuat Vector3Wide.CrossWithoutOverlap(offsetA, basis.Z, out horizontalAngularJacobianA.Y); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep( - in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref StaticCharacterMotionPrestep prestepData, out StaticCharacterMotionProjection projection) - { - //The motion constraint is split into two parts: the horizontal constraint, and the vertical constraint. - //The horizontal constraint acts almost exactly like the TangentFriction, but we'll duplicate some of the logic to keep this implementation self-contained. - ComputeJacobians(prestepData.OffsetFromCharacter, prestepData.SurfaceBasis, - out var basis, out var horizontalAngularJacobianA, out var verticalAngularJacobianA); - - //I'll omit the details of where this comes from, but you can check out the other constraints or the sorta-tutorial Inequality1DOF constraint to explain the details, - //plus some other references. The idea is that we need a way to transform the constraint space velocity (that we get from transforming body velocities - //by the transpose jacobian) into a corrective impulse for the solver iterations. That corrective impulse is then used to update the velocities on each iteration execution. - //This transform is the 'effective mass', representing the mass felt by the constraint in its local space. - //In concept, this constraint is actually two separate constraints solved iteratively, so we have two separate such effective mass transforms. - Symmetric3x3Wide.MatrixSandwich(horizontalAngularJacobianA, inertiaA.InverseInertiaTensor, out var inverseHorizontalEffectiveMass); - //The linear jacobians are unit length vectors, so J * M^-1 * JT is just M^-1. - inverseHorizontalEffectiveMass.XX += inertiaA.InverseMass; - inverseHorizontalEffectiveMass.YY += inertiaA.InverseMass; - Symmetric2x2Wide.InvertWithoutOverlap(inverseHorizontalEffectiveMass, out projection.HorizontalEffectiveMass); - - //Note that many characters will just have zero inverse inertia tensors to prevent them from rotating, so this could be optimized. - //(Removing a transform wouldn't matter, but avoiding the storage of an inertia tensor in the projection would be useful.) - //We don't take advantage of this optimization for simplicity, and so that you could use this constraint unchanged in a simulation - //where the orientation is instead controlled by some other constraint or torque- imagine a game with gravity that points in different directions. - Symmetric3x3Wide.VectorSandwich(verticalAngularJacobianA, inertiaA.InverseInertiaTensor, out var verticalAngularContributionA); - var inverseVerticalEffectiveMass = verticalAngularContributionA + inertiaA.InverseMass; - projection.VerticalEffectiveMass = Vector.One / inverseVerticalEffectiveMass; - - //Note that we still use the packed representation in the projection information, even though we unpacked it in the prestep. - //The solver iterations will redo that math rather than storing the full jacobians. This saves quite a bit of memory bandwidth. - //Storing every jacobian (except duplicate linear jacobians) would require 2x3 * 3 + 1x3 * 3 = 27 wide scalars, - //while storing the quaternion basis and two offsets requires only 4 + 3 + 3 = 10 scalars. - - //(That might sound irrelevant, but on an AVX2 system, 17 extra scalars means 544 extra bytes per solve iteration. - //If a machine has 40GBps of main memory bandwidth, those extra bytes require ~13.5 nanoseconds. - //A quad core AVX2 processor could easily perform over 300 instructions in that time. The story only gets more bandwidth-limited - //as the core count scales up on pretty much all modern processors.) - - //If you're wondering why we're just copying all of this into the projection rather than loading it again from the prestep data and body data, - //it's (once again) to minimize memory bandwidth and cache misses. By copying it all into one contiguous struct, the solver iterations - //have effectively optimal cache line efficiency (outside of their body velocity gathers and scatters, but that's unavoidable in this solver type). - projection.SurfaceBasis = prestepData.SurfaceBasis; - projection.OffsetFromCharacter = prestepData.OffsetFromCharacter; - - projection.TargetVelocity.X = prestepData.TargetVelocity.X; - //The surface basis's Z axis points in the opposite direction to the view direction, so negate the target velocity along the Z axis to point it in the expected direction. - projection.TargetVelocity.Y = -prestepData.TargetVelocity.Y; - projection.InertiaA = inertiaA; - projection.MaximumHorizontalImpulse = prestepData.MaximumHorizontalForce * dt; - projection.MaximumVerticalForce = prestepData.MaximumVerticalForce * dt; - //If the character is deeply penetrating, the vertical motion constraint will allow some separating velocity- just enough for one frame of integration to reach zero depth. - projection.VerticalBiasVelocity = Vector.Max(Vector.Zero, prestepData.Depth * inverseDt); - - //Note that there are other ways to store constraints efficiently, some of which can actually reduce the amount of compute work required by the solver iterations. - //Their use depends on the number of DOFs in the constraint and sometimes special properties of specific constraints. - //For more details, take a look at the Inequality1DOF sample constraint. - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] private static void ApplyHorizontalImpulse(in Matrix3x3Wide basis, in Matrix2x3Wide angularJacobianA, in Vector2Wide constraintSpaceImpulse, @@ -255,56 +181,7 @@ private static void ApplyVerticalImpulse(in Matrix3x3Wide basis, Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseA, inertiaA.InverseInertiaTensor, out var angularChangeA); Vector3Wide.Add(velocityA.Angular, angularChangeA, out velocityA.Angular); } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocityWide velocityA, ref StaticCharacterMotionProjection projection, ref CharacterMotionAccumulatedImpulse accumulatedImpulse) - { - ComputeJacobians(projection.OffsetFromCharacter, projection.SurfaceBasis, - out var basis, out var horizontalAngularJacobianA, out var verticalAngularJacobianA); - ApplyHorizontalImpulse(basis, horizontalAngularJacobianA, accumulatedImpulse.Horizontal, projection.InertiaA, ref velocityA); - ApplyVerticalImpulse(basis, verticalAngularJacobianA, accumulatedImpulse.Vertical, projection.InertiaA, ref velocityA); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocityWide velocityA, ref StaticCharacterMotionProjection projection, ref CharacterMotionAccumulatedImpulse accumulatedImpulse) - { - ComputeJacobians(projection.OffsetFromCharacter, projection.SurfaceBasis, - out var basis, out var horizontalAngularJacobianA, out var verticalAngularJacobianA); - - //Compute the velocity error by projecting the body velocity into constraint space using the transposed jacobian. - Vector2Wide horizontalLinearA; - Vector3Wide.Dot(basis.X, velocityA.Linear, out horizontalLinearA.X); - Vector3Wide.Dot(basis.Z, velocityA.Linear, out horizontalLinearA.Y); - Matrix2x3Wide.TransformByTransposeWithoutOverlap(velocityA.Angular, horizontalAngularJacobianA, out var horizontalAngularA); - Vector2Wide.Add(horizontalLinearA, horizontalAngularA, out var horizontalVelocity); - - Vector2Wide.Subtract(projection.TargetVelocity, horizontalVelocity, out var horizontalConstraintSpaceVelocityChange); - Symmetric2x2Wide.TransformWithoutOverlap(horizontalConstraintSpaceVelocityChange, projection.HorizontalEffectiveMass, out var horizontalCorrectiveImpulse); - - //Limit the force applied by the horizontal motion constraint. Note that this clamps the *accumulated* impulse applied this time step, not just this one iterations' value. - var previousHorizontalAccumulatedImpulse = accumulatedImpulse.Horizontal; - Vector2Wide.Add(accumulatedImpulse.Horizontal, horizontalCorrectiveImpulse, out accumulatedImpulse.Horizontal); - Vector2Wide.Length(accumulatedImpulse.Horizontal, out var horizontalImpulseMagnitude); - //Note division by zero guard. - var scale = Vector.Min(Vector.One, projection.MaximumHorizontalImpulse / Vector.Max(new Vector(1e-16f), horizontalImpulseMagnitude)); - Vector2Wide.Scale(accumulatedImpulse.Horizontal, scale, out accumulatedImpulse.Horizontal); - Vector2Wide.Subtract(accumulatedImpulse.Horizontal, previousHorizontalAccumulatedImpulse, out horizontalCorrectiveImpulse); - - ApplyHorizontalImpulse(basis, horizontalAngularJacobianA, horizontalCorrectiveImpulse, projection.InertiaA, ref velocityA); - - //Same thing for the vertical constraint. - Vector3Wide.Dot(basis.Y, velocityA.Linear, out var verticalLinearA); - Vector3Wide.Dot(velocityA.Angular, verticalAngularJacobianA, out var verticalAngularA); - //The vertical constraint just targets zero velocity, but does not attempt to fight any velocity which would merely push the character out of penetration. - var verticalCorrectiveImpulse = (projection.VerticalBiasVelocity - verticalLinearA - verticalAngularA) * projection.VerticalEffectiveMass; - - //Clamp the vertical constraint's impulse, but note that this is a bit different than above- the vertical constraint is not allowed to *push*, so there's an extra bound at zero. - var previousVerticalAccumulatedImpulse = accumulatedImpulse.Vertical; - accumulatedImpulse.Vertical = Vector.Min(Vector.Zero, Vector.Max(accumulatedImpulse.Vertical + verticalCorrectiveImpulse, -projection.MaximumVerticalForce)); - verticalCorrectiveImpulse = accumulatedImpulse.Vertical - previousVerticalAccumulatedImpulse; - - ApplyVerticalImpulse(basis, verticalAngularJacobianA, verticalCorrectiveImpulse, projection.InertiaA, ref velocityA); - } + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref StaticCharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide velocityA) { @@ -366,7 +243,6 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in //The vertical constraint just targets zero velocity, but does not attempt to fight any velocity which would merely push the character out of penetration. //Note that many characters will just have zero inverse inertia tensors to prevent them from rotating, so this could be optimized. - //(Removing a transform wouldn't matter, but avoiding the storage of an inertia tensor in the projection would be useful.) //We don't take advantage of this optimization for simplicity, and so that you could use this constraint unchanged in a simulation //where the orientation is instead controlled by some other constraint or torque- imagine a game with gravity that points in different directions. Symmetric3x3Wide.VectorSandwich(verticalAngularJacobianA, inertiaA.InverseInertiaTensor, out var verticalAngularContributionA); @@ -403,7 +279,7 @@ public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWi //Each constraint type has its own 'type processor'- it acts as the outer loop that handles all the common logic across batches of constraints and invokes //the per-constraint logic as needed. The CharacterMotionFunctions type provides the actual implementation. - public class StaticCharacterMotionTypeProcessor : OneBodyTypeProcessor + public class StaticCharacterMotionTypeProcessor : OneBodyTypeProcessor { /// /// Simulation-wide unique id for the character motion constraint. Every type has needs a unique compile time id; this is a little bit annoying to guarantee given that there is no central @@ -514,25 +390,7 @@ public struct DynamicCharacterMotionPrestep public Vector3Wide OffsetFromSupport; } - //Using the prestep data plus some current body state, the solver computes the information required to execute velocity iterations. The main purpose of this intermediate data - //is to describe the projection from body velocities into constraint space impulses, and from constraint space impulses to body velocities again. - public struct DynamicCharacterMotionProjection - { - public QuaternionWide SurfaceBasis; - public Vector3Wide OffsetFromCharacter; - public Vector3Wide OffsetFromSupport; - public Vector2Wide TargetVelocity; - public Symmetric2x2Wide HorizontalEffectiveMass; - public Vector MaximumHorizontalImpulse; - public BodyInertiaWide InertiaA; - public BodyInertiaWide InertiaB; - public Vector VerticalBiasVelocity; - public Vector VerticalEffectiveMass; - public Vector MaximumVerticalForce; - - } - - public struct DynamicCharacterMotionFunctions : ITwoBodyConstraintFunctions + public struct DynamicCharacterMotionFunctions : ITwoBodyConstraintFunctions { [MethodImpl(MethodImplOptions.AggressiveInlining)] static void ComputeJacobians(in Vector3Wide offsetA, in Vector3Wide offsetB, in QuaternionWide basisQuaternion, @@ -566,70 +424,6 @@ static void ComputeJacobians(in Vector3Wide offsetA, in Vector3Wide offsetB, in Vector3Wide.CrossWithoutOverlap(basis.Z, offsetB, out horizontalAngularJacobianB.Y); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep( - in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref DynamicCharacterMotionPrestep prestepData, out DynamicCharacterMotionProjection projection) - { - //The motion constraint is split into two parts: the horizontal constraint, and the vertical constraint. - //The horizontal constraint acts almost exactly like the TangentFriction, but we'll duplicate some of the logic to keep this implementation self-contained. - ComputeJacobians(prestepData.OffsetFromCharacter, prestepData.OffsetFromSupport, prestepData.SurfaceBasis, - out var basis, out var horizontalAngularJacobianA, out var horizontalAngularJacobianB, out var verticalAngularJacobianA, out var verticalAngularJacobianB); - - //I'll omit the details of where this comes from, but you can check out the other constraints or the sorta-tutorial Inequality1DOF constraint to explain the details, - //plus some other references. The idea is that we need a way to transform the constraint space velocity (that we get from transforming body velocities - //by the transpose jacobian) into a corrective impulse for the solver iterations. That corrective impulse is then used to update the velocities on each iteration execution. - //This transform is the 'effective mass', representing the mass felt by the constraint in its local space. - //In concept, this constraint is actually two separate constraints solved iteratively, so we have two separate such effective mass transforms. - Symmetric3x3Wide.MatrixSandwich(horizontalAngularJacobianA, inertiaA.InverseInertiaTensor, out var horizontalAngularContributionA); - Symmetric3x3Wide.MatrixSandwich(horizontalAngularJacobianB, inertiaB.InverseInertiaTensor, out var horizontalAngularContributionB); - Symmetric2x2Wide.Add(horizontalAngularContributionA, horizontalAngularContributionB, out var inverseHorizontalEffectiveMass); - //The linear jacobians are unit length vectors, so J * M^-1 * JT is just M^-1. - var linearContribution = inertiaA.InverseMass + inertiaB.InverseMass; - inverseHorizontalEffectiveMass.XX += linearContribution; - inverseHorizontalEffectiveMass.YY += linearContribution; - Symmetric2x2Wide.InvertWithoutOverlap(inverseHorizontalEffectiveMass, out projection.HorizontalEffectiveMass); - - //Note that many characters will just have zero inverse inertia tensors to prevent them from rotating, so this could be optimized. - //(Removing a transform wouldn't matter, but avoiding the storage of an inertia tensor in the projection would be useful.) - //We don't take advantage of this optimization for simplicity, and so that you could use this constraint unchanged in a simulation - //where the orientation is instead controlled by some other constraint or torque- imagine a game with gravity that points in different directions. - Symmetric3x3Wide.VectorSandwich(verticalAngularJacobianA, inertiaA.InverseInertiaTensor, out var verticalAngularContributionA); - Symmetric3x3Wide.VectorSandwich(verticalAngularJacobianB, inertiaB.InverseInertiaTensor, out var verticalAngularContributionB); - var inverseVerticalEffectiveMass = verticalAngularContributionA + verticalAngularContributionB + linearContribution; - projection.VerticalEffectiveMass = Vector.One / inverseVerticalEffectiveMass; - - //Note that we still use the packed representation in the projection information, even though we unpacked it in the prestep. - //The solver iterations will redo that math rather than storing the full jacobians. This saves quite a bit of memory bandwidth. - //Storing every jacobian (except duplicate linear jacobians) would require 2x3 * 3 + 1x3 * 3 = 27 wide scalars, - //while storing the quaternion basis and two offsets requires only 4 + 3 + 3 = 10 scalars. - - //(That might sound irrelevant, but on an AVX2 system, 17 extra scalars means 544 extra bytes per solve iteration. - //If a machine has 40GBps of main memory bandwidth, those extra bytes require ~13.5 nanoseconds. - //A quad core AVX2 processor could easily perform over 300 instructions in that time. The story only gets more bandwidth-limited - //as the core count scales up on pretty much all modern processors.) - - //If you're wondering why we're just copying all of this into the projection rather than loading it again from the prestep data and body data, - //it's (once again) to minimize memory bandwidth and cache misses. By copying it all into one contiguous struct, the solver iterations - //have effectively optimal cache line efficiency (outside of their body velocity gathers and scatters, but that's unavoidable in this solver type). - projection.SurfaceBasis = prestepData.SurfaceBasis; - projection.OffsetFromCharacter = prestepData.OffsetFromCharacter; - projection.OffsetFromSupport = prestepData.OffsetFromSupport; - - projection.TargetVelocity.X = prestepData.TargetVelocity.X; - //The surface basis's Z axis points in the opposite direction to the view direction, so negate the target velocity along the Z axis to point it in the expected direction. - projection.TargetVelocity.Y = -prestepData.TargetVelocity.Y; - projection.InertiaA = inertiaA; - projection.InertiaB = inertiaB; - projection.MaximumHorizontalImpulse = prestepData.MaximumHorizontalForce * dt; - projection.MaximumVerticalForce = prestepData.MaximumVerticalForce * dt; - //If the character is deeply penetrating, the vertical motion constraint will allow some separating velocity- just enough for one frame of integration to reach zero depth. - projection.VerticalBiasVelocity = Vector.Max(Vector.Zero, prestepData.Depth * inverseDt); - - //Note that there are other ways to store constraints efficiently, some of which can actually reduce the amount of compute work required by the solver iterations. - //Their use depends on the number of DOFs in the constraint and sometimes special properties of specific constraints. - //For more details, take a look at the Inequality1DOF sample constraint. - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] private static void ApplyHorizontalImpulse(in Matrix3x3Wide basis, in Matrix2x3Wide angularJacobianA, in Matrix2x3Wide angularJacobianB, in Vector2Wide constraintSpaceImpulse, @@ -672,64 +466,7 @@ private static void ApplyVerticalImpulse(in Matrix3x3Wide basis, Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseB, inertiaB.InverseInertiaTensor, out var angularChangeB); Vector3Wide.Add(velocityB.Angular, angularChangeB, out velocityB.Angular); } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref DynamicCharacterMotionProjection projection, ref CharacterMotionAccumulatedImpulse accumulatedImpulse) - { - ComputeJacobians(projection.OffsetFromCharacter, projection.OffsetFromSupport, projection.SurfaceBasis, - out var basis, out var horizontalAngularJacobianA, out var horizontalAngularJacobianB, out var verticalAngularJacobianA, out var verticalAngularJacobianB); - ApplyHorizontalImpulse(basis, horizontalAngularJacobianA, horizontalAngularJacobianB, accumulatedImpulse.Horizontal, projection.InertiaA, projection.InertiaB, ref velocityA, ref velocityB); - ApplyVerticalImpulse(basis, verticalAngularJacobianA, verticalAngularJacobianB, accumulatedImpulse.Vertical, projection.InertiaA, projection.InertiaB, ref velocityA, ref velocityB); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB, ref DynamicCharacterMotionProjection projection, ref CharacterMotionAccumulatedImpulse accumulatedImpulse) - { - ComputeJacobians(projection.OffsetFromCharacter, projection.OffsetFromSupport, projection.SurfaceBasis, - out var basis, out var horizontalAngularJacobianA, out var horizontalAngularJacobianB, out var verticalAngularJacobianA, out var verticalAngularJacobianB); - - //Compute the velocity error by projecting the body velocity into constraint space using the transposed jacobian. - Vector2Wide horizontalLinearA; - Vector3Wide.Dot(basis.X, velocityA.Linear, out horizontalLinearA.X); - Vector3Wide.Dot(basis.Z, velocityA.Linear, out horizontalLinearA.Y); - Matrix2x3Wide.TransformByTransposeWithoutOverlap(velocityA.Angular, horizontalAngularJacobianA, out var horizontalAngularA); - Vector2Wide negatedHorizontalLinearB; - Vector3Wide.Dot(basis.X, velocityB.Linear, out negatedHorizontalLinearB.X); - Vector3Wide.Dot(basis.Z, velocityB.Linear, out negatedHorizontalLinearB.Y); - Matrix2x3Wide.TransformByTransposeWithoutOverlap(velocityB.Angular, horizontalAngularJacobianB, out var horizontalAngularB); - Vector2Wide.Add(horizontalAngularA, horizontalAngularB, out var horizontalAngular); - Vector2Wide.Subtract(horizontalLinearA, negatedHorizontalLinearB, out var horizontalLinear); - Vector2Wide.Add(horizontalAngular, horizontalLinear, out var horizontalVelocity); - - Vector2Wide.Subtract(projection.TargetVelocity, horizontalVelocity, out var horizontalConstraintSpaceVelocityChange); - Symmetric2x2Wide.TransformWithoutOverlap(horizontalConstraintSpaceVelocityChange, projection.HorizontalEffectiveMass, out var horizontalCorrectiveImpulse); - - //Limit the force applied by the horizontal motion constraint. Note that this clamps the *accumulated* impulse applied this time step, not just this one iterations' value. - var previousHorizontalAccumulatedImpulse = accumulatedImpulse.Horizontal; - Vector2Wide.Add(accumulatedImpulse.Horizontal, horizontalCorrectiveImpulse, out accumulatedImpulse.Horizontal); - Vector2Wide.Length(accumulatedImpulse.Horizontal, out var horizontalImpulseMagnitude); - //Note division by zero guard. - var scale = Vector.Min(Vector.One, projection.MaximumHorizontalImpulse / Vector.Max(new Vector(1e-16f), horizontalImpulseMagnitude)); - Vector2Wide.Scale(accumulatedImpulse.Horizontal, scale, out accumulatedImpulse.Horizontal); - Vector2Wide.Subtract(accumulatedImpulse.Horizontal, previousHorizontalAccumulatedImpulse, out horizontalCorrectiveImpulse); - - ApplyHorizontalImpulse(basis, horizontalAngularJacobianA, horizontalAngularJacobianB, horizontalCorrectiveImpulse, projection.InertiaA, projection.InertiaB, ref velocityA, ref velocityB); - - //Same thing for the vertical constraint. - Vector3Wide.Dot(basis.Y, velocityA.Linear, out var verticalLinearA); - Vector3Wide.Dot(velocityA.Angular, verticalAngularJacobianA, out var verticalAngularA); - Vector3Wide.Dot(basis.Y, velocityB.Linear, out var negatedVerticalLinearB); - Vector3Wide.Dot(velocityB.Angular, verticalAngularJacobianB, out var verticalAngularB); - //The vertical constraint just targets zero velocity, but does not attempt to fight any velocity which would merely push the character out of penetration. - var verticalCorrectiveImpulse = (projection.VerticalBiasVelocity - verticalLinearA + negatedVerticalLinearB - verticalAngularA - verticalAngularB) * projection.VerticalEffectiveMass; - - //Clamp the vertical constraint's impulse, but note that this is a bit different than above- the vertical constraint is not allowed to *push*, so there's an extra bound at zero. - var previousVerticalAccumulatedImpulse = accumulatedImpulse.Vertical; - accumulatedImpulse.Vertical = Vector.Min(Vector.Zero, Vector.Max(accumulatedImpulse.Vertical + verticalCorrectiveImpulse, -projection.MaximumVerticalForce)); - verticalCorrectiveImpulse = accumulatedImpulse.Vertical - previousVerticalAccumulatedImpulse; - - ApplyVerticalImpulse(basis, verticalAngularJacobianA, verticalAngularJacobianB, verticalCorrectiveImpulse, projection.InertiaA, projection.InertiaB, ref velocityA, ref velocityB); - } + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref DynamicCharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB) { @@ -802,7 +539,6 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in //The vertical constraint just targets zero velocity, but does not attempt to fight any velocity which would merely push the character out of penetration. //Note that many characters will just have zero inverse inertia tensors to prevent them from rotating, so this could be optimized. - //(Removing a transform wouldn't matter, but avoiding the storage of an inertia tensor in the projection would be useful.) //We don't take advantage of this optimization for simplicity, and so that you could use this constraint unchanged in a simulation //where the orientation is instead controlled by some other constraint or torque- imagine a game with gravity that points in different directions. Symmetric3x3Wide.VectorSandwich(verticalAngularJacobianA, inertiaA.InverseInertiaTensor, out var verticalAngularContributionA); @@ -843,7 +579,7 @@ public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWi //Each constraint type has its own 'type processor'- it acts as the outer loop that handles all the common logic across batches of constraints and invokes //the per-constraint logic as needed. The CharacterMotionFunctions type provides the actual implementation. - public class DynamicCharacterMotionTypeProcessor : TwoBodyTypeProcessor + public class DynamicCharacterMotionTypeProcessor : TwoBodyTypeProcessor { /// /// Simulation-wide unique id for the character motion constraint. Every type has needs a unique compile time id; this is a little bit annoying to guarantee given that there is no central diff --git a/Demos/Demos/Characters/CharacterMotionConstraint.tt b/Demos/Demos/Characters/CharacterMotionConstraint.tt index 9f21c68d5..7596e72d6 100644 --- a/Demos/Demos/Characters/CharacterMotionConstraint.tt +++ b/Demos/Demos/Characters/CharacterMotionConstraint.tt @@ -144,29 +144,7 @@ namespace Demos.Demos.Characters <#}#> } - //Using the prestep data plus some current body state, the solver computes the information required to execute velocity iterations. The main purpose of this intermediate data - //is to describe the projection from body velocities into constraint space impulses, and from constraint space impulses to body velocities again. - public struct <#=prefix#>CharacterMotionProjection - { - public QuaternionWide SurfaceBasis; - public Vector3Wide OffsetFromCharacter; -<#if(dynamic) {#> - public Vector3Wide OffsetFromSupport; -<#}#> - public Vector2Wide TargetVelocity; - public Symmetric2x2Wide HorizontalEffectiveMass; - public Vector MaximumHorizontalImpulse; - public BodyInertiaWide InertiaA; -<#if(dynamic) {#> - public BodyInertiaWide InertiaB; -<#}#> - public Vector VerticalBiasVelocity; - public Vector VerticalEffectiveMass; - public Vector MaximumVerticalForce; - - } - - public struct <#=prefix#>CharacterMotionFunctions : I<#if(dynamic) { Write("TwoBody"); } else { Write("OneBody"); }#>ConstraintFunctions<<#=prefix#>CharacterMotionPrestep, <#=prefix#>CharacterMotionProjection, CharacterMotionAccumulatedImpulse> + public struct <#=prefix#>CharacterMotionFunctions : I<#if(dynamic) { Write("TwoBody"); } else { Write("OneBody"); }#>ConstraintFunctions<<#=prefix#>CharacterMotionPrestep, CharacterMotionAccumulatedImpulse> { [MethodImpl(MethodImplOptions.AggressiveInlining)] static void ComputeJacobians(in Vector3Wide offsetA, <#if (dynamic) {#>in Vector3Wide offsetB, <#}#>in QuaternionWide basisQuaternion, @@ -203,79 +181,6 @@ namespace Demos.Demos.Characters <#}#> } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Prestep( - <#if(!dynamic) {#>in Vector3Wide positionA,<#}#> in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(dynamic) {#>in Vector3Wide ab, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>float dt, float inverseDt, ref <#=prefix#>CharacterMotionPrestep prestepData, out <#=prefix#>CharacterMotionProjection projection) - { - //The motion constraint is split into two parts: the horizontal constraint, and the vertical constraint. - //The horizontal constraint acts almost exactly like the TangentFriction, but we'll duplicate some of the logic to keep this implementation self-contained. - ComputeJacobians(prestepData.OffsetFromCharacter, <#if (dynamic) {#>prestepData.OffsetFromSupport, <#}#>prestepData.SurfaceBasis, - out var basis, out var horizontalAngularJacobianA, <#if (dynamic) {#>out var horizontalAngularJacobianB, <#}#>out var verticalAngularJacobianA<#if (dynamic) {#>, out var verticalAngularJacobianB<#}#>); - - //I'll omit the details of where this comes from, but you can check out the other constraints or the sorta-tutorial Inequality1DOF constraint to explain the details, - //plus some other references. The idea is that we need a way to transform the constraint space velocity (that we get from transforming body velocities - //by the transpose jacobian) into a corrective impulse for the solver iterations. That corrective impulse is then used to update the velocities on each iteration execution. - //This transform is the 'effective mass', representing the mass felt by the constraint in its local space. - //In concept, this constraint is actually two separate constraints solved iteratively, so we have two separate such effective mass transforms. - Symmetric3x3Wide.MatrixSandwich(horizontalAngularJacobianA, inertiaA.InverseInertiaTensor, out var <#=dynamic ? "horizontalAngularContributionA" : "inverseHorizontalEffectiveMass"#>); -<#if (dynamic) {#> - Symmetric3x3Wide.MatrixSandwich(horizontalAngularJacobianB, inertiaB.InverseInertiaTensor, out var horizontalAngularContributionB); - Symmetric2x2Wide.Add(horizontalAngularContributionA, horizontalAngularContributionB, out var inverseHorizontalEffectiveMass); -<#}#> - //The linear jacobians are unit length vectors, so J * M^-1 * JT is just M^-1. -<#if (dynamic) {#> - var linearContribution = inertiaA.InverseMass + inertiaB.InverseMass; -<#}#> - inverseHorizontalEffectiveMass.XX += <#=dynamic ? "linearContribution" : "inertiaA.InverseMass"#>; - inverseHorizontalEffectiveMass.YY += <#=dynamic ? "linearContribution" : "inertiaA.InverseMass"#>; - Symmetric2x2Wide.InvertWithoutOverlap(inverseHorizontalEffectiveMass, out projection.HorizontalEffectiveMass); - - //Note that many characters will just have zero inverse inertia tensors to prevent them from rotating, so this could be optimized. - //(Removing a transform wouldn't matter, but avoiding the storage of an inertia tensor in the projection would be useful.) - //We don't take advantage of this optimization for simplicity, and so that you could use this constraint unchanged in a simulation - //where the orientation is instead controlled by some other constraint or torque- imagine a game with gravity that points in different directions. - Symmetric3x3Wide.VectorSandwich(verticalAngularJacobianA, inertiaA.InverseInertiaTensor, out var verticalAngularContributionA); -<#if (dynamic) {#> - Symmetric3x3Wide.VectorSandwich(verticalAngularJacobianB, inertiaB.InverseInertiaTensor, out var verticalAngularContributionB); -<#}#> - var inverseVerticalEffectiveMass = verticalAngularContributionA + <#=dynamic ? "verticalAngularContributionB + linearContribution" : "inertiaA.InverseMass"#>; - projection.VerticalEffectiveMass = Vector.One / inverseVerticalEffectiveMass; - - //Note that we still use the packed representation in the projection information, even though we unpacked it in the prestep. - //The solver iterations will redo that math rather than storing the full jacobians. This saves quite a bit of memory bandwidth. - //Storing every jacobian (except duplicate linear jacobians) would require 2x3 * 3 + 1x3 * 3 = 27 wide scalars, - //while storing the quaternion basis and two offsets requires only 4 + 3 + 3 = 10 scalars. - - //(That might sound irrelevant, but on an AVX2 system, 17 extra scalars means 544 extra bytes per solve iteration. - //If a machine has 40GBps of main memory bandwidth, those extra bytes require ~13.5 nanoseconds. - //A quad core AVX2 processor could easily perform over 300 instructions in that time. The story only gets more bandwidth-limited - //as the core count scales up on pretty much all modern processors.) - - //If you're wondering why we're just copying all of this into the projection rather than loading it again from the prestep data and body data, - //it's (once again) to minimize memory bandwidth and cache misses. By copying it all into one contiguous struct, the solver iterations - //have effectively optimal cache line efficiency (outside of their body velocity gathers and scatters, but that's unavoidable in this solver type). - projection.SurfaceBasis = prestepData.SurfaceBasis; - projection.OffsetFromCharacter = prestepData.OffsetFromCharacter; -<#if (dynamic) {#> - projection.OffsetFromSupport = prestepData.OffsetFromSupport; -<#}#> - projection.TargetVelocity.X = prestepData.TargetVelocity.X; - //The surface basis's Z axis points in the opposite direction to the view direction, so negate the target velocity along the Z axis to point it in the expected direction. - projection.TargetVelocity.Y = -prestepData.TargetVelocity.Y; - projection.InertiaA = inertiaA; -<#if (dynamic) {#> - projection.InertiaB = inertiaB; -<#}#> - projection.MaximumHorizontalImpulse = prestepData.MaximumHorizontalForce * dt; - projection.MaximumVerticalForce = prestepData.MaximumVerticalForce * dt; - //If the character is deeply penetrating, the vertical motion constraint will allow some separating velocity- just enough for one frame of integration to reach zero depth. - projection.VerticalBiasVelocity = Vector.Max(Vector.Zero, prestepData.Depth * inverseDt); - - //Note that there are other ways to store constraints efficiently, some of which can actually reduce the amount of compute work required by the solver iterations. - //Their use depends on the number of DOFs in the constraint and sometimes special properties of specific constraints. - //For more details, take a look at the Inequality1DOF sample constraint. - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] private static void ApplyHorizontalImpulse(in Matrix3x3Wide basis, in Matrix2x3Wide angularJacobianA, <#if (dynamic) {#>in Matrix2x3Wide angularJacobianB, <#}#>in Vector2Wide constraintSpaceImpulse, @@ -328,70 +233,7 @@ namespace Demos.Demos.Characters Vector3Wide.Add(velocityB.Angular, angularChangeB, out velocityB.Angular); <#}#> } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(ref BodyVelocityWide velocityA, <#if (dynamic) {#>ref BodyVelocityWide velocityB, <#}#>ref <#=prefix#>CharacterMotionProjection projection, ref CharacterMotionAccumulatedImpulse accumulatedImpulse) - { - ComputeJacobians(projection.OffsetFromCharacter, <#if (dynamic) {#>projection.OffsetFromSupport, <#}#>projection.SurfaceBasis, - out var basis, out var horizontalAngularJacobianA, <#if (dynamic) {#>out var horizontalAngularJacobianB, <#}#>out var verticalAngularJacobianA<#if (dynamic) {#>, out var verticalAngularJacobianB<#}#>); - ApplyHorizontalImpulse(basis, horizontalAngularJacobianA, <#if (dynamic) {#>horizontalAngularJacobianB, <#}#>accumulatedImpulse.Horizontal, projection.InertiaA, <#if (dynamic) {#>projection.InertiaB, <#}#>ref velocityA<#if (dynamic) {#>, ref velocityB<#}#>); - ApplyVerticalImpulse(basis, verticalAngularJacobianA, <#if (dynamic) {#>verticalAngularJacobianB, <#}#>accumulatedImpulse.Vertical, projection.InertiaA, <#if (dynamic) {#>projection.InertiaB, <#}#>ref velocityA<#if (dynamic) {#>, ref velocityB<#}#>); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(ref BodyVelocityWide velocityA, <#if (dynamic) {#>ref BodyVelocityWide velocityB, <#}#>ref <#=prefix#>CharacterMotionProjection projection, ref CharacterMotionAccumulatedImpulse accumulatedImpulse) - { - ComputeJacobians(projection.OffsetFromCharacter, <#if (dynamic) {#>projection.OffsetFromSupport, <#}#>projection.SurfaceBasis, - out var basis, out var horizontalAngularJacobianA, <#if (dynamic) {#>out var horizontalAngularJacobianB, <#}#>out var verticalAngularJacobianA<#if (dynamic) {#>, out var verticalAngularJacobianB<#}#>); - - //Compute the velocity error by projecting the body velocity into constraint space using the transposed jacobian. - Vector2Wide horizontalLinearA; - Vector3Wide.Dot(basis.X, velocityA.Linear, out horizontalLinearA.X); - Vector3Wide.Dot(basis.Z, velocityA.Linear, out horizontalLinearA.Y); - Matrix2x3Wide.TransformByTransposeWithoutOverlap(velocityA.Angular, horizontalAngularJacobianA, out var horizontalAngularA); -<#if (dynamic) {#> - Vector2Wide negatedHorizontalLinearB; - Vector3Wide.Dot(basis.X, velocityB.Linear, out negatedHorizontalLinearB.X); - Vector3Wide.Dot(basis.Z, velocityB.Linear, out negatedHorizontalLinearB.Y); - Matrix2x3Wide.TransformByTransposeWithoutOverlap(velocityB.Angular, horizontalAngularJacobianB, out var horizontalAngularB); - Vector2Wide.Add(horizontalAngularA, horizontalAngularB, out var horizontalAngular); - Vector2Wide.Subtract(horizontalLinearA, negatedHorizontalLinearB, out var horizontalLinear); - Vector2Wide.Add(horizontalAngular, horizontalLinear, out var horizontalVelocity); -<#} else {#> - Vector2Wide.Add(horizontalLinearA, horizontalAngularA, out var horizontalVelocity); -<#}#> - - Vector2Wide.Subtract(projection.TargetVelocity, horizontalVelocity, out var horizontalConstraintSpaceVelocityChange); - Symmetric2x2Wide.TransformWithoutOverlap(horizontalConstraintSpaceVelocityChange, projection.HorizontalEffectiveMass, out var horizontalCorrectiveImpulse); - - //Limit the force applied by the horizontal motion constraint. Note that this clamps the *accumulated* impulse applied this time step, not just this one iterations' value. - var previousHorizontalAccumulatedImpulse = accumulatedImpulse.Horizontal; - Vector2Wide.Add(accumulatedImpulse.Horizontal, horizontalCorrectiveImpulse, out accumulatedImpulse.Horizontal); - Vector2Wide.Length(accumulatedImpulse.Horizontal, out var horizontalImpulseMagnitude); - //Note division by zero guard. - var scale = Vector.Min(Vector.One, projection.MaximumHorizontalImpulse / Vector.Max(new Vector(1e-16f), horizontalImpulseMagnitude)); - Vector2Wide.Scale(accumulatedImpulse.Horizontal, scale, out accumulatedImpulse.Horizontal); - Vector2Wide.Subtract(accumulatedImpulse.Horizontal, previousHorizontalAccumulatedImpulse, out horizontalCorrectiveImpulse); - - ApplyHorizontalImpulse(basis, horizontalAngularJacobianA, <#if (dynamic) {#>horizontalAngularJacobianB, <#}#>horizontalCorrectiveImpulse, projection.InertiaA, <#if (dynamic) {#>projection.InertiaB, <#}#>ref velocityA<#if (dynamic) {#>, ref velocityB<#}#>); - - //Same thing for the vertical constraint. - Vector3Wide.Dot(basis.Y, velocityA.Linear, out var verticalLinearA); - Vector3Wide.Dot(velocityA.Angular, verticalAngularJacobianA, out var verticalAngularA); -<#if (dynamic) {#> - Vector3Wide.Dot(basis.Y, velocityB.Linear, out var negatedVerticalLinearB); - Vector3Wide.Dot(velocityB.Angular, verticalAngularJacobianB, out var verticalAngularB); -<#}#> - //The vertical constraint just targets zero velocity, but does not attempt to fight any velocity which would merely push the character out of penetration. - var verticalCorrectiveImpulse = (projection.VerticalBiasVelocity - verticalLinearA<#if (dynamic) {#> + negatedVerticalLinearB<#}#> - verticalAngularA<#if (dynamic) {#> - verticalAngularB<#}#>) * projection.VerticalEffectiveMass; - - //Clamp the vertical constraint's impulse, but note that this is a bit different than above- the vertical constraint is not allowed to *push*, so there's an extra bound at zero. - var previousVerticalAccumulatedImpulse = accumulatedImpulse.Vertical; - accumulatedImpulse.Vertical = Vector.Min(Vector.Zero, Vector.Max(accumulatedImpulse.Vertical + verticalCorrectiveImpulse, -projection.MaximumVerticalForce)); - verticalCorrectiveImpulse = accumulatedImpulse.Vertical - previousVerticalAccumulatedImpulse; - - ApplyVerticalImpulse(basis, verticalAngularJacobianA, <#if (dynamic) {#>verticalAngularJacobianB, <#}#>verticalCorrectiveImpulse, projection.InertiaA, <#if (dynamic) {#>projection.InertiaB, <#}#>ref velocityA<#if (dynamic) {#>, ref velocityB<#}#>); - } + public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(dynamic) {#>in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>ref <#=prefix#>CharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide velocityA<#if(dynamic) {#>, ref BodyVelocityWide velocityB<#}#>) { @@ -474,7 +316,6 @@ namespace Demos.Demos.Characters //The vertical constraint just targets zero velocity, but does not attempt to fight any velocity which would merely push the character out of penetration. //Note that many characters will just have zero inverse inertia tensors to prevent them from rotating, so this could be optimized. - //(Removing a transform wouldn't matter, but avoiding the storage of an inertia tensor in the projection would be useful.) //We don't take advantage of this optimization for simplicity, and so that you could use this constraint unchanged in a simulation //where the orientation is instead controlled by some other constraint or torque- imagine a game with gravity that points in different directions. Symmetric3x3Wide.VectorSandwich(verticalAngularJacobianA, inertiaA.InverseInertiaTensor, out var verticalAngularContributionA); @@ -521,7 +362,7 @@ namespace Demos.Demos.Characters //Each constraint type has its own 'type processor'- it acts as the outer loop that handles all the common logic across batches of constraints and invokes //the per-constraint logic as needed. The CharacterMotionFunctions type provides the actual implementation. - public class <#=prefix#>CharacterMotionTypeProcessor : <#=dynamic ? "Two" : "One"#>BodyTypeProcessor<<#=prefix#>CharacterMotionPrestep, <#=prefix#>CharacterMotionProjection, CharacterMotionAccumulatedImpulse, <#=prefix#>CharacterMotionFunctions, AccessAll, AccessAll<#=dynamic ? (", AccessAll, AccessAll") : ""#>> + public class <#=prefix#>CharacterMotionTypeProcessor : <#=dynamic ? "Two" : "One"#>BodyTypeProcessor<<#=prefix#>CharacterMotionPrestep, CharacterMotionAccumulatedImpulse, <#=prefix#>CharacterMotionFunctions, AccessAll, AccessAll<#=dynamic ? (", AccessAll, AccessAll") : ""#>> { /// /// Simulation-wide unique id for the character motion constraint. Every type has needs a unique compile time id; this is a little bit annoying to guarantee given that there is no central diff --git a/Demos/Demos/Tanks/TankDemo.cs b/Demos/Demos/Tanks/TankDemo.cs index 663c5998f..4aee539af 100644 --- a/Demos/Demos/Tanks/TankDemo.cs +++ b/Demos/Demos/Tanks/TankDemo.cs @@ -49,7 +49,12 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0; bodyProperties = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new TankCallbacks() { Properties = bodyProperties }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); + //Note that this demo uses only one substep and 8 velocity iterations. + //That's partly to show that you can do such a thing, and partly because of (as of 2.4's initial release), there are situations where + //contact data can become a little out of date during substepping, since the contact data is only updated once per frame rather than substep (apart from the depths, which are incrementally updated every substep). + //In this demo, when using substepping, a wheel resting on another wheel from a destroyed tank can keep rocking back and forth for a long time as the error in contact offsets over substeps can introduce energy. + //(I'd like to address this issue more directly to make substepping an unconditional win.) + Simulation = Simulation.Create(BufferPool, new TankCallbacks() { Properties = bodyProperties }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), (1, 8)); var builder = new CompoundBuilder(BufferPool, Simulation.Shapes, 2); builder.Add(new Box(1.85f, 0.7f, 4.73f), RigidPose.Identity, 10); From ff2b8698ac2f8c36fa7da46663a1ed0f0e233fe6 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 15 Dec 2021 11:17:16 -0600 Subject: [PATCH 333/947] Purged more old constraintstuff. De-2'd names. --- .../Constraints/AngularAxisGearMotor.cs | 4 +- BepuPhysics/Constraints/AngularAxisMotor.cs | 4 +- BepuPhysics/Constraints/AngularHinge.cs | 4 +- BepuPhysics/Constraints/AngularMotor.cs | 4 +- BepuPhysics/Constraints/AngularServo.cs | 4 +- BepuPhysics/Constraints/AngularSwivelHinge.cs | 4 +- BepuPhysics/Constraints/AreaConstraint.cs | 4 +- BepuPhysics/Constraints/BallSocket.cs | 4 +- BepuPhysics/Constraints/BallSocketMotor.cs | 4 +- BepuPhysics/Constraints/BallSocketServo.cs | 4 +- .../Constraints/CenterDistanceConstraint.cs | 4 +- .../Constraints/Contact/ContactConvexTypes.cs | 176 +++++++++--------- .../Constraints/Contact/ContactConvexTypes.tt | 4 +- .../Contact/ContactNonconvexCommon.cs | 24 +-- .../Constraints/Contact/PenetrationLimit.cs | 111 +---------- .../Contact/PenetrationLimitOneBody.cs | 66 +------ .../Constraints/Contact/TangentFriction.cs | 58 +----- .../Contact/TangentFrictionOneBody.cs | 50 +---- .../Constraints/Contact/TwistFriction.cs | 50 +---- .../Contact/TwistFrictionOneBody.cs | 41 +--- BepuPhysics/Constraints/DistanceLimit.cs | 4 +- BepuPhysics/Constraints/DistanceServo.cs | 4 +- .../Constraints/FourBodyTypeProcessor.cs | 12 +- BepuPhysics/Constraints/Hinge.cs | 4 +- BepuPhysics/Constraints/LinearAxisLimit.cs | 4 +- BepuPhysics/Constraints/LinearAxisMotor.cs | 4 +- BepuPhysics/Constraints/LinearAxisServo.cs | 4 +- .../Constraints/OneBodyAngularMotor.cs | 4 +- .../Constraints/OneBodyAngularServo.cs | 4 +- BepuPhysics/Constraints/OneBodyLinearMotor.cs | 4 +- BepuPhysics/Constraints/OneBodyLinearServo.cs | 4 +- .../Constraints/OneBodyTypeProcessor.cs | 12 +- BepuPhysics/Constraints/PointOnLineServo.cs | 4 +- BepuPhysics/Constraints/SwingLimit.cs | 4 +- BepuPhysics/Constraints/SwivelHinge.cs | 4 +- .../Constraints/ThreeBodyTypeProcessor.cs | 12 +- BepuPhysics/Constraints/TwistLimit.cs | 4 +- BepuPhysics/Constraints/TwistMotor.cs | 4 +- BepuPhysics/Constraints/TwistServo.cs | 4 +- .../Constraints/TwoBodyTypeProcessor.cs | 12 +- BepuPhysics/Constraints/TypeProcessor.cs | 4 +- BepuPhysics/Constraints/VolumeConstraint.cs | 4 +- BepuPhysics/Constraints/Weld.cs | 4 +- BepuPhysics/Solver_Solve.cs | 20 +- .../Characters/CharacterMotionConstraint.cs | 8 +- 45 files changed, 213 insertions(+), 563 deletions(-) diff --git a/BepuPhysics/Constraints/AngularAxisGearMotor.cs b/BepuPhysics/Constraints/AngularAxisGearMotor.cs index f0b71fc20..0569bacb1 100644 --- a/BepuPhysics/Constraints/AngularAxisGearMotor.cs +++ b/BepuPhysics/Constraints/AngularAxisGearMotor.cs @@ -77,7 +77,7 @@ public static void ApplyImpulse(in Vector3Wide impulseToVelocityA, in Vector3Wid } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref AngularAxisGearMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref AngularAxisGearMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { QuaternionWide.TransformWithoutOverlap(prestep.LocalAxisA, orientationA, out var axis); Vector3Wide.Scale(axis, prestep.VelocityScale, out var jA); @@ -87,7 +87,7 @@ public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref AngularAxisGearMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref AngularAxisGearMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { //This is mildly more complex than the AngularAxisMotor: //dot(wa, axis) * velocityScale - dot(wb, axis) = 0, so jacobianA is actually axis * velocityScale, not just -axis. diff --git a/BepuPhysics/Constraints/AngularAxisMotor.cs b/BepuPhysics/Constraints/AngularAxisMotor.cs index 44ce6e16a..fea064a6a 100644 --- a/BepuPhysics/Constraints/AngularAxisMotor.cs +++ b/BepuPhysics/Constraints/AngularAxisMotor.cs @@ -76,7 +76,7 @@ public static void ApplyImpulse(in Vector3Wide impulseToVelocityA, in Vector3Wid } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref AngularAxisMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref AngularAxisMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { QuaternionWide.TransformWithoutOverlap(prestep.LocalAxisA, orientationA, out var axis); Symmetric3x3Wide.TransformWithoutOverlap(axis, inertiaA.InverseInertiaTensor, out var jIA); @@ -85,7 +85,7 @@ public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref AngularAxisMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref AngularAxisMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { QuaternionWide.TransformWithoutOverlap(prestep.LocalAxisA, orientationA, out var jA); Symmetric3x3Wide.TransformWithoutOverlap(jA, inertiaA.InverseInertiaTensor, out var jIA); diff --git a/BepuPhysics/Constraints/AngularHinge.cs b/BepuPhysics/Constraints/AngularHinge.cs index 557b315f2..acf0a5c0d 100644 --- a/BepuPhysics/Constraints/AngularHinge.cs +++ b/BepuPhysics/Constraints/AngularHinge.cs @@ -130,7 +130,7 @@ private static void ComputeJacobians(in Vector3Wide localHingeAxisA, in Quaterni Matrix3x3Wide.TransformWithoutOverlap(localAY, orientationMatrixA, out jacobianA.Y); } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref AngularHingePrestepData prestep, ref Vector2Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref AngularHingePrestepData prestep, ref Vector2Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeJacobians(prestep.LocalHingeAxisA, orientationA, out _, out var jacobianA); Symmetric3x3Wide.MultiplyWithoutOverlap(jacobianA, inertiaA.InverseInertiaTensor, out var impulseToVelocityA); @@ -138,7 +138,7 @@ public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, ApplyImpulse(impulseToVelocityA, negatedImpulseToVelocityB, accumulatedImpulses, ref wsvA.Angular, ref wsvB.Angular); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref AngularHingePrestepData prestep, ref Vector2Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref AngularHingePrestepData prestep, ref Vector2Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { //Note that we build the tangents in local space first to avoid inconsistencies. ComputeJacobians(prestep.LocalHingeAxisA, orientationA, out var hingeAxisA, out var jacobianA); diff --git a/BepuPhysics/Constraints/AngularMotor.cs b/BepuPhysics/Constraints/AngularMotor.cs index 447e412b0..7083e7b0b 100644 --- a/BepuPhysics/Constraints/AngularMotor.cs +++ b/BepuPhysics/Constraints/AngularMotor.cs @@ -59,12 +59,12 @@ public struct AngularMotorPrestepData public struct AngularMotorFunctions : ITwoBodyConstraintFunctions { - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref AngularMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref AngularMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { AngularServoFunctions.ApplyImpulse(ref wsvA.Angular, ref wsvB.Angular, inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, accumulatedImpulses); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref AngularMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref AngularMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { //Jacobians are just the identity matrix. MotorSettingsWide.ComputeSoftness(prestep.Settings, dt, out var effectiveMassCFMScale, out var softnessImpulseScale, out var maximumImpulse); diff --git a/BepuPhysics/Constraints/AngularServo.cs b/BepuPhysics/Constraints/AngularServo.cs index 81737e40c..b6a968106 100644 --- a/BepuPhysics/Constraints/AngularServo.cs +++ b/BepuPhysics/Constraints/AngularServo.cs @@ -97,12 +97,12 @@ public static void ApplyImpulse(ref Vector3Wide angularVelocityA, ref Vector3Wid //} - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref AngularServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref AngularServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ApplyImpulse(ref wsvA.Angular, ref wsvB.Angular, inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, accumulatedImpulses); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref AngularServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref AngularServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { //Jacobians are just I and -I. QuaternionWide.ConcatenateWithoutOverlap(prestep.TargetRelativeRotationLocalA, orientationA, out var targetOrientationB); diff --git a/BepuPhysics/Constraints/AngularSwivelHinge.cs b/BepuPhysics/Constraints/AngularSwivelHinge.cs index 8995ec8f5..bb7994769 100644 --- a/BepuPhysics/Constraints/AngularSwivelHinge.cs +++ b/BepuPhysics/Constraints/AngularSwivelHinge.cs @@ -93,7 +93,7 @@ static void ComputeJacobian(in Vector3Wide localSwivelAxisA, in Vector3Wide loca Vector3Wide.ConditionalSelect(useFallback, fallbackJacobian, jacobianA, out jacobianA); } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref AngularSwivelHingePrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref AngularSwivelHingePrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeJacobian(prestep.LocalSwivelAxisA, prestep.LocalHingeAxisB, orientationA, orientationB, out _, out _, out var jacobianA); Symmetric3x3Wide.TransformWithoutOverlap(jacobianA, inertiaA.InverseInertiaTensor, out var impulseToVelocityA); @@ -101,7 +101,7 @@ public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, ApplyImpulse(impulseToVelocityA, negatedImpulseToVelocityB, accumulatedImpulses, ref wsvA.Angular, ref wsvB.Angular); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref AngularSwivelHingePrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref AngularSwivelHingePrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { //The swivel hinge attempts to keep an axis on body A separated 90 degrees from an axis on body B. In other words, this is the same as a hinge joint, but with one fewer DOF. //C = dot(swivelA, hingeB) = 0 diff --git a/BepuPhysics/Constraints/AreaConstraint.cs b/BepuPhysics/Constraints/AreaConstraint.cs index ea950bde6..9586c7468 100644 --- a/BepuPhysics/Constraints/AreaConstraint.cs +++ b/BepuPhysics/Constraints/AreaConstraint.cs @@ -120,7 +120,7 @@ static void ComputeJacobian(in Vector3Wide positionA, in Vector3Wide positionB, Vector3Wide.Add(jacobianB, jacobianC, out negatedJacobianA); } - public void WarmStart2( + public void WarmStart( in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, @@ -130,7 +130,7 @@ public void WarmStart2( ApplyImpulse(inertiaA.InverseMass, inertiaB.InverseMass, inertiaC.InverseMass, negatedJacobianA, jacobianB, jacobianC, accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, float dt, float inverseDt, ref AreaConstraintPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC) + public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, float dt, float inverseDt, ref AreaConstraintPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC) { ComputeJacobian(positionA, positionB, positionC, out var normalLength, out var negatedJacobianA, out var jacobianB, out var jacobianC); diff --git a/BepuPhysics/Constraints/BallSocket.cs b/BepuPhysics/Constraints/BallSocket.cs index 2880478a2..ce3278e06 100644 --- a/BepuPhysics/Constraints/BallSocket.cs +++ b/BepuPhysics/Constraints/BallSocket.cs @@ -65,7 +65,7 @@ public struct BallSocketPrestepData } public struct BallSocketFunctions : ITwoBodyConstraintFunctions { - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, + public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref BallSocketPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetA, orientationA, out var offsetA); @@ -73,7 +73,7 @@ public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, BallSocketShared.ApplyImpulse(ref wsvA, ref wsvB, offsetA, offsetB, inertiaA, inertiaB, accumulatedImpulses); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, + public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref BallSocketPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetA, orientationA, out var offsetA); diff --git a/BepuPhysics/Constraints/BallSocketMotor.cs b/BepuPhysics/Constraints/BallSocketMotor.cs index 7fe1731e5..6042a6d57 100644 --- a/BepuPhysics/Constraints/BallSocketMotor.cs +++ b/BepuPhysics/Constraints/BallSocketMotor.cs @@ -69,13 +69,13 @@ public struct BallSocketMotorPrestepData public struct BallSocketMotorFunctions : ITwoBodyConstraintFunctions { - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref BallSocketMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref BallSocketMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetB, orientationB, out var targetOffsetB); BallSocketShared.ApplyImpulse(ref wsvA, ref wsvB, (positionB - positionA) + targetOffsetB, targetOffsetB, inertiaA, inertiaB, accumulatedImpulses); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref BallSocketMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref BallSocketMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetB, orientationB, out var targetOffsetB); var offsetA = (positionB - positionA) + targetOffsetB; diff --git a/BepuPhysics/Constraints/BallSocketServo.cs b/BepuPhysics/Constraints/BallSocketServo.cs index 943156b92..b85fc516f 100644 --- a/BepuPhysics/Constraints/BallSocketServo.cs +++ b/BepuPhysics/Constraints/BallSocketServo.cs @@ -74,14 +74,14 @@ public struct BallSocketServoPrestepData public struct BallSocketServoFunctions : ITwoBodyConstraintFunctions { - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref BallSocketServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref BallSocketServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetA, orientationA, out var offsetA); QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetB, orientationB, out var offsetB); BallSocketShared.ApplyImpulse(ref wsvA, ref wsvB, offsetA, offsetB, inertiaA, inertiaB, accumulatedImpulses); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref BallSocketServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref BallSocketServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetA, orientationA, out var offsetA); QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetB, orientationB, out var offsetB); diff --git a/BepuPhysics/Constraints/CenterDistanceConstraint.cs b/BepuPhysics/Constraints/CenterDistanceConstraint.cs index 223d9002a..e347d8c9f 100644 --- a/BepuPhysics/Constraints/CenterDistanceConstraint.cs +++ b/BepuPhysics/Constraints/CenterDistanceConstraint.cs @@ -78,7 +78,7 @@ static void ApplyImpulse(in Vector3Wide jacobianA, in Vector inverseMassA } //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, + public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref CenterDistancePrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { var ab = positionB - positionA; @@ -93,7 +93,7 @@ public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, ApplyImpulse(jacobianA, inertiaA.InverseMass, inertiaB.InverseMass, accumulatedImpulses, ref wsvA, ref wsvB); } //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, + public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref CenterDistancePrestepData prestep, ref Vector accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { //Note that we need the actual length for error calculation. diff --git a/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs b/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs index 548a80c5f..975f8d433 100644 --- a/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs +++ b/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs @@ -299,30 +299,30 @@ public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWi } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref Contact1OneBodyPrestepData prestep, ref Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) + public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref Contact1OneBodyPrestepData prestep, ref Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); - TangentFrictionOneBody.WarmStart2(x, z, prestep.Contact0.OffsetA, inertiaA, accumulatedImpulses.Tangent, ref wsvA); - PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, accumulatedImpulses.Penetration0, ref wsvA); - TwistFrictionOneBody.WarmStart2(prestep.Normal, inertiaA, accumulatedImpulses.Twist, ref wsvA); + TangentFrictionOneBody.WarmStart(x, z, prestep.Contact0.OffsetA, inertiaA, accumulatedImpulses.Tangent, ref wsvA); + PenetrationLimitOneBody.WarmStart(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, accumulatedImpulses.Penetration0, ref wsvA); + TwistFrictionOneBody.WarmStart(prestep.Normal, inertiaA, accumulatedImpulses.Twist, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref Contact1OneBodyPrestepData prestep, ref Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) + public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref Contact1OneBodyPrestepData prestep, ref Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); var maximumTangentImpulse = prestep.MaterialProperties.FrictionCoefficient * (accumulatedImpulses.Penetration0); - TangentFrictionOneBody.Solve2(x, z, prestep.Contact0.OffsetA, inertiaA, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); + TangentFrictionOneBody.Solve(x, z, prestep.Contact0.OffsetA, inertiaA, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); //Note that we solve the penetration constraints after the friction constraints. //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. //It's a pretty minor effect either way. SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); var inverseDtWide = new Vector(inverseDt); - PenetrationLimitOneBody.Solve2(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); + PenetrationLimitOneBody.Solve(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); //If there's only one contact, then the contact patch as determined by contact distance would be zero. //That can cause some subtle behavioral issues sometimes, so we approximate lever arm with the contact depth, assuming that the contact surface area will increase as the depth increases. var maximumTwistImpulse = prestep.MaterialProperties.FrictionCoefficient * accumulatedImpulses.Penetration0 * prestep.Contact0.Depth; - TwistFrictionOneBody.Solve2(prestep.Normal, inertiaA, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); + TwistFrictionOneBody.Solve(prestep.Normal, inertiaA, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); } } @@ -447,35 +447,35 @@ public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWi } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref Contact2OneBodyPrestepData prestep, ref Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) + public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref Contact2OneBodyPrestepData prestep, ref Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, out var offsetToManifoldCenterA); - TangentFrictionOneBody.WarmStart2(x, z, offsetToManifoldCenterA, inertiaA, accumulatedImpulses.Tangent, ref wsvA); - PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, accumulatedImpulses.Penetration0, ref wsvA); - PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact1.OffsetA, accumulatedImpulses.Penetration1, ref wsvA); - TwistFrictionOneBody.WarmStart2(prestep.Normal, inertiaA, accumulatedImpulses.Twist, ref wsvA); + TangentFrictionOneBody.WarmStart(x, z, offsetToManifoldCenterA, inertiaA, accumulatedImpulses.Tangent, ref wsvA); + PenetrationLimitOneBody.WarmStart(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, accumulatedImpulses.Penetration0, ref wsvA); + PenetrationLimitOneBody.WarmStart(inertiaA, prestep.Normal, prestep.Contact1.OffsetA, accumulatedImpulses.Penetration1, ref wsvA); + TwistFrictionOneBody.WarmStart(prestep.Normal, inertiaA, accumulatedImpulses.Twist, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref Contact2OneBodyPrestepData prestep, ref Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) + public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref Contact2OneBodyPrestepData prestep, ref Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); var premultipliedFrictionCoefficient = new Vector(1f / 2f) * prestep.MaterialProperties.FrictionCoefficient; var maximumTangentImpulse = premultipliedFrictionCoefficient * (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1); FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, out var offsetToManifoldCenterA); - TangentFrictionOneBody.Solve2(x, z, offsetToManifoldCenterA, inertiaA, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); + TangentFrictionOneBody.Solve(x, z, offsetToManifoldCenterA, inertiaA, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); //Note that we solve the penetration constraints after the friction constraints. //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. //It's a pretty minor effect either way. SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); var inverseDtWide = new Vector(inverseDt); - PenetrationLimitOneBody.Solve2(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); - PenetrationLimitOneBody.Solve2(inertiaA, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA); + PenetrationLimitOneBody.Solve(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); + PenetrationLimitOneBody.Solve(inertiaA, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA); var maximumTwistImpulse = premultipliedFrictionCoefficient * ( accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact0.OffsetA) + accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact1.OffsetA)); - TwistFrictionOneBody.Solve2(prestep.Normal, inertiaA, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); + TwistFrictionOneBody.Solve(prestep.Normal, inertiaA, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); } } @@ -607,38 +607,38 @@ public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWi } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref Contact3OneBodyPrestepData prestep, ref Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) + public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref Contact3OneBodyPrestepData prestep, ref Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, out var offsetToManifoldCenterA); - TangentFrictionOneBody.WarmStart2(x, z, offsetToManifoldCenterA, inertiaA, accumulatedImpulses.Tangent, ref wsvA); - PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, accumulatedImpulses.Penetration0, ref wsvA); - PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact1.OffsetA, accumulatedImpulses.Penetration1, ref wsvA); - PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact2.OffsetA, accumulatedImpulses.Penetration2, ref wsvA); - TwistFrictionOneBody.WarmStart2(prestep.Normal, inertiaA, accumulatedImpulses.Twist, ref wsvA); + TangentFrictionOneBody.WarmStart(x, z, offsetToManifoldCenterA, inertiaA, accumulatedImpulses.Tangent, ref wsvA); + PenetrationLimitOneBody.WarmStart(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, accumulatedImpulses.Penetration0, ref wsvA); + PenetrationLimitOneBody.WarmStart(inertiaA, prestep.Normal, prestep.Contact1.OffsetA, accumulatedImpulses.Penetration1, ref wsvA); + PenetrationLimitOneBody.WarmStart(inertiaA, prestep.Normal, prestep.Contact2.OffsetA, accumulatedImpulses.Penetration2, ref wsvA); + TwistFrictionOneBody.WarmStart(prestep.Normal, inertiaA, accumulatedImpulses.Twist, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref Contact3OneBodyPrestepData prestep, ref Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) + public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref Contact3OneBodyPrestepData prestep, ref Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); var premultipliedFrictionCoefficient = new Vector(1f / 3f) * prestep.MaterialProperties.FrictionCoefficient; var maximumTangentImpulse = premultipliedFrictionCoefficient * (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2); FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, out var offsetToManifoldCenterA); - TangentFrictionOneBody.Solve2(x, z, offsetToManifoldCenterA, inertiaA, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); + TangentFrictionOneBody.Solve(x, z, offsetToManifoldCenterA, inertiaA, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); //Note that we solve the penetration constraints after the friction constraints. //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. //It's a pretty minor effect either way. SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); var inverseDtWide = new Vector(inverseDt); - PenetrationLimitOneBody.Solve2(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); - PenetrationLimitOneBody.Solve2(inertiaA, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA); - PenetrationLimitOneBody.Solve2(inertiaA, prestep.Normal, prestep.Contact2.OffsetA, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA); + PenetrationLimitOneBody.Solve(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); + PenetrationLimitOneBody.Solve(inertiaA, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA); + PenetrationLimitOneBody.Solve(inertiaA, prestep.Normal, prestep.Contact2.OffsetA, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA); var maximumTwistImpulse = premultipliedFrictionCoefficient * ( accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact0.OffsetA) + accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact1.OffsetA) + accumulatedImpulses.Penetration2 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact2.OffsetA)); - TwistFrictionOneBody.Solve2(prestep.Normal, inertiaA, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); + TwistFrictionOneBody.Solve(prestep.Normal, inertiaA, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); } } @@ -777,41 +777,41 @@ public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWi } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref Contact4OneBodyPrestepData prestep, ref Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) + public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref Contact4OneBodyPrestepData prestep, ref Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact3.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, prestep.Contact3.Depth, out var offsetToManifoldCenterA); - TangentFrictionOneBody.WarmStart2(x, z, offsetToManifoldCenterA, inertiaA, accumulatedImpulses.Tangent, ref wsvA); - PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, accumulatedImpulses.Penetration0, ref wsvA); - PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact1.OffsetA, accumulatedImpulses.Penetration1, ref wsvA); - PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact2.OffsetA, accumulatedImpulses.Penetration2, ref wsvA); - PenetrationLimitOneBody.WarmStart2(inertiaA, prestep.Normal, prestep.Contact3.OffsetA, accumulatedImpulses.Penetration3, ref wsvA); - TwistFrictionOneBody.WarmStart2(prestep.Normal, inertiaA, accumulatedImpulses.Twist, ref wsvA); + TangentFrictionOneBody.WarmStart(x, z, offsetToManifoldCenterA, inertiaA, accumulatedImpulses.Tangent, ref wsvA); + PenetrationLimitOneBody.WarmStart(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, accumulatedImpulses.Penetration0, ref wsvA); + PenetrationLimitOneBody.WarmStart(inertiaA, prestep.Normal, prestep.Contact1.OffsetA, accumulatedImpulses.Penetration1, ref wsvA); + PenetrationLimitOneBody.WarmStart(inertiaA, prestep.Normal, prestep.Contact2.OffsetA, accumulatedImpulses.Penetration2, ref wsvA); + PenetrationLimitOneBody.WarmStart(inertiaA, prestep.Normal, prestep.Contact3.OffsetA, accumulatedImpulses.Penetration3, ref wsvA); + TwistFrictionOneBody.WarmStart(prestep.Normal, inertiaA, accumulatedImpulses.Twist, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref Contact4OneBodyPrestepData prestep, ref Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) + public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref Contact4OneBodyPrestepData prestep, ref Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); var premultipliedFrictionCoefficient = new Vector(1f / 4f) * prestep.MaterialProperties.FrictionCoefficient; var maximumTangentImpulse = premultipliedFrictionCoefficient * (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2 + accumulatedImpulses.Penetration3); FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact3.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, prestep.Contact3.Depth, out var offsetToManifoldCenterA); - TangentFrictionOneBody.Solve2(x, z, offsetToManifoldCenterA, inertiaA, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); + TangentFrictionOneBody.Solve(x, z, offsetToManifoldCenterA, inertiaA, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); //Note that we solve the penetration constraints after the friction constraints. //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. //It's a pretty minor effect either way. SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); var inverseDtWide = new Vector(inverseDt); - PenetrationLimitOneBody.Solve2(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); - PenetrationLimitOneBody.Solve2(inertiaA, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA); - PenetrationLimitOneBody.Solve2(inertiaA, prestep.Normal, prestep.Contact2.OffsetA, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA); - PenetrationLimitOneBody.Solve2(inertiaA, prestep.Normal, prestep.Contact3.OffsetA, prestep.Contact3.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration3, ref wsvA); + PenetrationLimitOneBody.Solve(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); + PenetrationLimitOneBody.Solve(inertiaA, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA); + PenetrationLimitOneBody.Solve(inertiaA, prestep.Normal, prestep.Contact2.OffsetA, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA); + PenetrationLimitOneBody.Solve(inertiaA, prestep.Normal, prestep.Contact3.OffsetA, prestep.Contact3.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration3, ref wsvA); var maximumTwistImpulse = premultipliedFrictionCoefficient * ( accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact0.OffsetA) + accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact1.OffsetA) + accumulatedImpulses.Penetration2 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact2.OffsetA) + accumulatedImpulses.Penetration3 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact3.OffsetA)); - TwistFrictionOneBody.Solve2(prestep.Normal, inertiaA, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); + TwistFrictionOneBody.Solve(prestep.Normal, inertiaA, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); } } @@ -940,32 +940,32 @@ public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWi } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref Contact1PrestepData prestep, ref Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref Contact1PrestepData prestep, ref Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); Vector3Wide.Subtract(prestep.Contact0.OffsetA, prestep.OffsetB, out var offsetToManifoldCenterB); - TangentFriction.WarmStart2(x, z, prestep.Contact0.OffsetA, offsetToManifoldCenterB, inertiaA, inertiaB, accumulatedImpulses.Tangent, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - TwistFriction.WarmStart2(prestep.Normal, inertiaA, inertiaB, accumulatedImpulses.Twist, ref wsvA, ref wsvB); + TangentFriction.WarmStart(x, z, prestep.Contact0.OffsetA, offsetToManifoldCenterB, inertiaA, inertiaB, accumulatedImpulses.Tangent, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + TwistFriction.WarmStart(prestep.Normal, inertiaA, inertiaB, accumulatedImpulses.Twist, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref Contact1PrestepData prestep, ref Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref Contact1PrestepData prestep, ref Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); var maximumTangentImpulse = prestep.MaterialProperties.FrictionCoefficient * (accumulatedImpulses.Penetration0); Vector3Wide.Subtract(prestep.Contact0.OffsetA, prestep.OffsetB, out var offsetToManifoldCenterB); - TangentFriction.Solve2(x, z, prestep.Contact0.OffsetA, offsetToManifoldCenterB, inertiaA, inertiaB, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); + TangentFriction.Solve(x, z, prestep.Contact0.OffsetA, offsetToManifoldCenterB, inertiaA, inertiaB, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); //Note that we solve the penetration constraints after the friction constraints. //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. //It's a pretty minor effect either way. SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); var inverseDtWide = new Vector(inverseDt); - PenetrationLimit.Solve2(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + PenetrationLimit.Solve(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); //If there's only one contact, then the contact patch as determined by contact distance would be zero. //That can cause some subtle behavioral issues sometimes, so we approximate lever arm with the contact depth, assuming that the contact surface area will increase as the depth increases. var maximumTwistImpulse = prestep.MaterialProperties.FrictionCoefficient * accumulatedImpulses.Penetration0 * prestep.Contact0.Depth; - TwistFriction.Solve2(prestep.Normal, inertiaA, inertiaB, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); + TwistFriction.Solve(prestep.Normal, inertiaA, inertiaB, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); } } @@ -1101,37 +1101,37 @@ public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWi } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref Contact2PrestepData prestep, ref Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref Contact2PrestepData prestep, ref Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, out var offsetToManifoldCenterA); Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); - TangentFriction.WarmStart2(x, z, offsetToManifoldCenterA, offsetToManifoldCenterB, inertiaA, inertiaB, accumulatedImpulses.Tangent, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); - TwistFriction.WarmStart2(prestep.Normal, inertiaA, inertiaB, accumulatedImpulses.Twist, ref wsvA, ref wsvB); + TangentFriction.WarmStart(x, z, offsetToManifoldCenterA, offsetToManifoldCenterB, inertiaA, inertiaB, accumulatedImpulses.Tangent, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart(inertiaA, inertiaB, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); + TwistFriction.WarmStart(prestep.Normal, inertiaA, inertiaB, accumulatedImpulses.Twist, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref Contact2PrestepData prestep, ref Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref Contact2PrestepData prestep, ref Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); var premultipliedFrictionCoefficient = new Vector(1f / 2f) * prestep.MaterialProperties.FrictionCoefficient; var maximumTangentImpulse = premultipliedFrictionCoefficient * (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1); FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, out var offsetToManifoldCenterA); Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); - TangentFriction.Solve2(x, z, offsetToManifoldCenterA, offsetToManifoldCenterB, inertiaA, inertiaB, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); + TangentFriction.Solve(x, z, offsetToManifoldCenterA, offsetToManifoldCenterB, inertiaA, inertiaB, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); //Note that we solve the penetration constraints after the friction constraints. //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. //It's a pretty minor effect either way. SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); var inverseDtWide = new Vector(inverseDt); - PenetrationLimit.Solve2(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - PenetrationLimit.Solve2(inertiaA, inertiaB, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.OffsetA - prestep.OffsetB, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); + PenetrationLimit.Solve(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + PenetrationLimit.Solve(inertiaA, inertiaB, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.OffsetA - prestep.OffsetB, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); var maximumTwistImpulse = premultipliedFrictionCoefficient * ( accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact0.OffsetA) + accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact1.OffsetA)); - TwistFriction.Solve2(prestep.Normal, inertiaA, inertiaB, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); + TwistFriction.Solve(prestep.Normal, inertiaA, inertiaB, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); } } @@ -1274,40 +1274,40 @@ public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWi } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref Contact3PrestepData prestep, ref Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref Contact3PrestepData prestep, ref Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, out var offsetToManifoldCenterA); Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); - TangentFriction.WarmStart2(x, z, offsetToManifoldCenterA, offsetToManifoldCenterB, inertiaA, inertiaB, accumulatedImpulses.Tangent, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact2.OffsetA, prestep.Contact2.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); - TwistFriction.WarmStart2(prestep.Normal, inertiaA, inertiaB, accumulatedImpulses.Twist, ref wsvA, ref wsvB); + TangentFriction.WarmStart(x, z, offsetToManifoldCenterA, offsetToManifoldCenterB, inertiaA, inertiaB, accumulatedImpulses.Tangent, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart(inertiaA, inertiaB, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart(inertiaA, inertiaB, prestep.Normal, prestep.Contact2.OffsetA, prestep.Contact2.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); + TwistFriction.WarmStart(prestep.Normal, inertiaA, inertiaB, accumulatedImpulses.Twist, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref Contact3PrestepData prestep, ref Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref Contact3PrestepData prestep, ref Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); var premultipliedFrictionCoefficient = new Vector(1f / 3f) * prestep.MaterialProperties.FrictionCoefficient; var maximumTangentImpulse = premultipliedFrictionCoefficient * (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2); FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, out var offsetToManifoldCenterA); Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); - TangentFriction.Solve2(x, z, offsetToManifoldCenterA, offsetToManifoldCenterB, inertiaA, inertiaB, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); + TangentFriction.Solve(x, z, offsetToManifoldCenterA, offsetToManifoldCenterB, inertiaA, inertiaB, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); //Note that we solve the penetration constraints after the friction constraints. //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. //It's a pretty minor effect either way. SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); var inverseDtWide = new Vector(inverseDt); - PenetrationLimit.Solve2(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - PenetrationLimit.Solve2(inertiaA, inertiaB, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.OffsetA - prestep.OffsetB, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); - PenetrationLimit.Solve2(inertiaA, inertiaB, prestep.Normal, prestep.Contact2.OffsetA, prestep.Contact2.OffsetA - prestep.OffsetB, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); + PenetrationLimit.Solve(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + PenetrationLimit.Solve(inertiaA, inertiaB, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.OffsetA - prestep.OffsetB, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); + PenetrationLimit.Solve(inertiaA, inertiaB, prestep.Normal, prestep.Contact2.OffsetA, prestep.Contact2.OffsetA - prestep.OffsetB, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); var maximumTwistImpulse = premultipliedFrictionCoefficient * ( accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact0.OffsetA) + accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact1.OffsetA) + accumulatedImpulses.Penetration2 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact2.OffsetA)); - TwistFriction.Solve2(prestep.Normal, inertiaA, inertiaB, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); + TwistFriction.Solve(prestep.Normal, inertiaA, inertiaB, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); } } @@ -1457,43 +1457,43 @@ public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWi } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref Contact4PrestepData prestep, ref Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref Contact4PrestepData prestep, ref Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact3.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, prestep.Contact3.Depth, out var offsetToManifoldCenterA); Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); - TangentFriction.WarmStart2(x, z, offsetToManifoldCenterA, offsetToManifoldCenterB, inertiaA, inertiaB, accumulatedImpulses.Tangent, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact2.OffsetA, prestep.Contact2.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestep.Normal, prestep.Contact3.OffsetA, prestep.Contact3.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration3, ref wsvA, ref wsvB); - TwistFriction.WarmStart2(prestep.Normal, inertiaA, inertiaB, accumulatedImpulses.Twist, ref wsvA, ref wsvB); + TangentFriction.WarmStart(x, z, offsetToManifoldCenterA, offsetToManifoldCenterB, inertiaA, inertiaB, accumulatedImpulses.Tangent, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart(inertiaA, inertiaB, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart(inertiaA, inertiaB, prestep.Normal, prestep.Contact2.OffsetA, prestep.Contact2.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart(inertiaA, inertiaB, prestep.Normal, prestep.Contact3.OffsetA, prestep.Contact3.OffsetA - prestep.OffsetB, accumulatedImpulses.Penetration3, ref wsvA, ref wsvB); + TwistFriction.WarmStart(prestep.Normal, inertiaA, inertiaB, accumulatedImpulses.Twist, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref Contact4PrestepData prestep, ref Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref Contact4PrestepData prestep, ref Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); var premultipliedFrictionCoefficient = new Vector(1f / 4f) * prestep.MaterialProperties.FrictionCoefficient; var maximumTangentImpulse = premultipliedFrictionCoefficient * (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2 + accumulatedImpulses.Penetration3); FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact3.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, prestep.Contact3.Depth, out var offsetToManifoldCenterA); Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); - TangentFriction.Solve2(x, z, offsetToManifoldCenterA, offsetToManifoldCenterB, inertiaA, inertiaB, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); + TangentFriction.Solve(x, z, offsetToManifoldCenterA, offsetToManifoldCenterB, inertiaA, inertiaB, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); //Note that we solve the penetration constraints after the friction constraints. //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. //It's a pretty minor effect either way. SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); var inverseDtWide = new Vector(inverseDt); - PenetrationLimit.Solve2(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - PenetrationLimit.Solve2(inertiaA, inertiaB, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.OffsetA - prestep.OffsetB, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); - PenetrationLimit.Solve2(inertiaA, inertiaB, prestep.Normal, prestep.Contact2.OffsetA, prestep.Contact2.OffsetA - prestep.OffsetB, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); - PenetrationLimit.Solve2(inertiaA, inertiaB, prestep.Normal, prestep.Contact3.OffsetA, prestep.Contact3.OffsetA - prestep.OffsetB, prestep.Contact3.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration3, ref wsvA, ref wsvB); + PenetrationLimit.Solve(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + PenetrationLimit.Solve(inertiaA, inertiaB, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.OffsetA - prestep.OffsetB, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); + PenetrationLimit.Solve(inertiaA, inertiaB, prestep.Normal, prestep.Contact2.OffsetA, prestep.Contact2.OffsetA - prestep.OffsetB, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); + PenetrationLimit.Solve(inertiaA, inertiaB, prestep.Normal, prestep.Contact3.OffsetA, prestep.Contact3.OffsetA - prestep.OffsetB, prestep.Contact3.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration3, ref wsvA, ref wsvB); var maximumTwistImpulse = premultipliedFrictionCoefficient * ( accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact0.OffsetA) + accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact1.OffsetA) + accumulatedImpulses.Penetration2 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact2.OffsetA) + accumulatedImpulses.Penetration3 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact3.OffsetA)); - TwistFriction.Solve2(prestep.Normal, inertiaA, inertiaB, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); + TwistFriction.Solve(prestep.Normal, inertiaA, inertiaB, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); } } diff --git a/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt b/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt index 507edbd76..8cf939e8f 100644 --- a/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt +++ b/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt @@ -229,7 +229,7 @@ for (int i = 0; i < contactCount; ++i) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(bodyCount == 2) {#>in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>ref Contact<#=contactCount#><#=suffix#>PrestepData prestep, ref Contact<#=contactCount#>AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA<#if(bodyCount == 2) {#>, ref BodyVelocityWide wsvB<#}#>) + public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(bodyCount == 2) {#>in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>ref Contact<#=contactCount#><#=suffix#>PrestepData prestep, ref Contact<#=contactCount#>AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA<#if(bodyCount == 2) {#>, ref BodyVelocityWide wsvB<#}#>) { Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); <#if (contactCount > 1) {#> @@ -248,7 +248,7 @@ for (int i = 0; i < contactCount; ++i) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(bodyCount == 2) {#>in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>float dt, float inverseDt, ref Contact<#=contactCount#><#=suffix#>PrestepData prestep, ref Contact<#=contactCount#>AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA<#if(bodyCount == 2) {#>, ref BodyVelocityWide wsvB<#}#>) + public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(bodyCount == 2) {#>in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>float dt, float inverseDt, ref Contact<#=contactCount#><#=suffix#>PrestepData prestep, ref Contact<#=contactCount#>AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA<#if(bodyCount == 2) {#>, ref BodyVelocityWide wsvB<#}#>) { Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); <#if (contactCount > 1) {#> diff --git a/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs b/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs index f0fcc2fff..ef9fdcf0b 100644 --- a/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs +++ b/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs @@ -184,7 +184,7 @@ public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityW } } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref TPrestep prestep, ref TAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) + public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref TPrestep prestep, ref TAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { ref var prestepMaterial = ref prestep.GetMaterialProperties(ref prestep); ref var prestepContactStart = ref prestep.GetContact(ref prestep, 0); @@ -194,12 +194,12 @@ public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, ref var prestepContact = ref Unsafe.Add(ref prestepContactStart, i); Helpers.BuildOrthonormalBasis(prestepContact.Normal, out var x, out var z); ref var contactImpulse = ref Unsafe.Add(ref accumulatedImpulsesStart, i); - TangentFrictionOneBody.WarmStart2(x, z, prestepContact.Offset, inertiaA, contactImpulse.Tangent, ref wsvA); - PenetrationLimitOneBody.WarmStart2(inertiaA, prestepContact.Normal, prestepContact.Offset, contactImpulse.Penetration, ref wsvA); + TangentFrictionOneBody.WarmStart(x, z, prestepContact.Offset, inertiaA, contactImpulse.Tangent, ref wsvA); + PenetrationLimitOneBody.WarmStart(inertiaA, prestepContact.Normal, prestepContact.Offset, contactImpulse.Penetration, ref wsvA); } } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref TPrestep prestep, ref TAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) + public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref TPrestep prestep, ref TAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { //Note that, unlike convex manifolds, we simply solve every contact in sequence rather than tangent->penetration. //This is not for any principled reason- only simplicity. May want to reconsider later, but remember the significant change in access pattern. @@ -214,8 +214,8 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in ref var contactImpulse = ref Unsafe.Add(ref accumulatedImpulsesStart, i); Helpers.BuildOrthonormalBasis(contact.Normal, out var x, out var z); var maximumTangentImpulse = prestepMaterial.FrictionCoefficient * contactImpulse.Penetration; - TangentFrictionOneBody.Solve2(x, z, contact.Offset, inertiaA, maximumTangentImpulse, ref contactImpulse.Tangent, ref wsvA); - PenetrationLimitOneBody.Solve2(inertiaA, contact.Normal, contact.Offset, contact.Depth, + TangentFrictionOneBody.Solve(x, z, contact.Offset, inertiaA, maximumTangentImpulse, ref contactImpulse.Tangent, ref wsvA); + PenetrationLimitOneBody.Solve(inertiaA, contact.Normal, contact.Offset, contact.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestepMaterial.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref contactImpulse.Penetration, ref wsvA); } } @@ -245,7 +245,7 @@ public struct ContactNonconvexTwoBodyFunctions : where TPrestep : struct, ITwoBodyNonconvexContactPrestep where TAccumulatedImpulses : struct { - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref TPrestep prestep, ref TAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref TPrestep prestep, ref TAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ref var prestepMaterial = ref prestep.GetMaterialProperties(ref prestep); ref var prestepOffsetB = ref prestep.GetOffsetB(ref prestep); @@ -257,12 +257,12 @@ public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, Helpers.BuildOrthonormalBasis(prestepContact.Normal, out var x, out var z); Vector3Wide.Subtract(prestepContact.Offset, prestepOffsetB, out var contactOffsetB); ref var contactImpulse = ref Unsafe.Add(ref accumulatedImpulsesStart, i); - TangentFriction.WarmStart2(x, z, prestepContact.Offset, contactOffsetB, inertiaA, inertiaB, contactImpulse.Tangent, ref wsvA, ref wsvB); - PenetrationLimit.WarmStart2(inertiaA, inertiaB, prestepContact.Normal, prestepContact.Offset, contactOffsetB, contactImpulse.Penetration, ref wsvA, ref wsvB); + TangentFriction.WarmStart(x, z, prestepContact.Offset, contactOffsetB, inertiaA, inertiaB, contactImpulse.Tangent, ref wsvA, ref wsvB); + PenetrationLimit.WarmStart(inertiaA, inertiaB, prestepContact.Normal, prestepContact.Offset, contactOffsetB, contactImpulse.Penetration, ref wsvA, ref wsvB); } } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref TPrestep prestep, ref TAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref TPrestep prestep, ref TAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { //Note that, unlike convex manifolds, we simply solve every contact in sequence rather than tangent->penetration. //This is not for any principled reason- only simplicity. May want to reconsider later, but remember the significant change in access pattern. @@ -279,8 +279,8 @@ public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in Vector3Wide.Subtract(contact.Offset, prestepOffsetB, out var contactOffsetB); Helpers.BuildOrthonormalBasis(contact.Normal, out var x, out var z); var maximumTangentImpulse = prestepMaterial.FrictionCoefficient * contactImpulse.Penetration; - TangentFriction.Solve2(x, z, contact.Offset, contactOffsetB, inertiaA, inertiaB, maximumTangentImpulse, ref contactImpulse.Tangent, ref wsvA, ref wsvB); - PenetrationLimit.Solve2(inertiaA, inertiaB, contact.Normal, contact.Offset, contactOffsetB, contact.Depth, + TangentFriction.Solve(x, z, contact.Offset, contactOffsetB, inertiaA, inertiaB, maximumTangentImpulse, ref contactImpulse.Tangent, ref wsvA, ref wsvB); + PenetrationLimit.Solve(inertiaA, inertiaB, contact.Normal, contact.Offset, contactOffsetB, contact.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestepMaterial.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref contactImpulse.Penetration, ref wsvA, ref wsvB); } } diff --git a/BepuPhysics/Constraints/Contact/PenetrationLimit.cs b/BepuPhysics/Constraints/Contact/PenetrationLimit.cs index e0840b4ab..04908adc6 100644 --- a/BepuPhysics/Constraints/Contact/PenetrationLimit.cs +++ b/BepuPhysics/Constraints/Contact/PenetrationLimit.cs @@ -6,105 +6,8 @@ namespace BepuPhysics.Constraints.Contact { - public struct PenetrationLimitProjection - { - //Note that these are just the raw jacobians, no precomputation with the JT*EffectiveMass. - public Vector3Wide AngularA; - public Vector3Wide AngularB; - public Vector EffectiveMass; - public Vector BiasVelocity; - } - public static class PenetrationLimit { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Prestep(in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, - in Vector3Wide contactOffsetA, in Vector3Wide contactOffsetB, in Vector3Wide normal, in Vector depth, - in Vector positionErrorToVelocity, in Vector effectiveMassCFMScale, in Vector maximumRecoveryVelocity, - float inverseDt, out PenetrationLimitProjection projection) - { - //We directly take the prestep data here since the jacobians and error don't undergo any processing. - - //The contact penetration constraint takes the form: - //dot(positionA + offsetA, N) >= dot(positionB + offsetB, N) - //Or: - //dot(positionA + offsetA, N) - dot(positionB + offsetB, N) >= 0 - //dot(positionA + offsetA - positionB - offsetB, N) >= 0 - //where positionA and positionB are the center of mass positions of the bodies offsetA and offsetB are world space offsets from the center of mass to the contact, - //and N is a unit length vector calibrated to point from B to A. (The normal pointing direction is important; it changes the sign.) - //In practice, we'll use the collision detection system's penetration depth instead of trying to recompute the error here. - - //So, treating the normal as constant, the velocity constraint is: - //dot(d/dt(positionA + offsetA - positionB - offsetB), N) >= 0 - //dot(linearVelocityA + d/dt(offsetA) - linearVelocityB - d/dt(offsetB)), N) >= 0 - //The velocity of the offsets are defined by the angular velocity. - //dot(linearVelocityA + angularVelocityA x offsetA - linearVelocityB - angularVelocityB x offsetB), N) >= 0 - //dot(linearVelocityA, N) + dot(angularVelocityA x offsetA, N) - dot(linearVelocityB, N) - dot(angularVelocityB x offsetB), N) >= 0 - //Use the properties of the scalar triple product: - //dot(linearVelocityA, N) + dot(offsetA x N, angularVelocityA) - dot(linearVelocityB, N) - dot(offsetB x N, angularVelocityB) >= 0 - //Bake in the negations: - //dot(linearVelocityA, N) + dot(offsetA x N, angularVelocityA) + dot(linearVelocityB, -N) + dot(-offsetB x N, angularVelocityB) >= 0 - //A x B = -B x A: - //dot(linearVelocityA, N) + dot(offsetA x N, angularVelocityA) + dot(linearVelocityB, -N) + dot(N x offsetB, angularVelocityB) >= 0 - //And there you go, the jacobians! - //linearA: N - //angularA: offsetA x N - //linearB: -N - //angularB: N x offsetB - //Note that we leave the penetration depth as is, even when it's negative. Speculative contacts! - Vector3Wide.CrossWithoutOverlap(contactOffsetA, normal, out projection.AngularA); - Vector3Wide.CrossWithoutOverlap(normal, contactOffsetB, out projection.AngularB); - - //effective mass - Symmetric3x3Wide.VectorSandwich(projection.AngularA, inertiaA.InverseInertiaTensor, out var angularA0); - Symmetric3x3Wide.VectorSandwich(projection.AngularB, inertiaB.InverseInertiaTensor, out var angularB0); - - //Linear effective mass contribution notes: - //1) The J * M^-1 * JT can be reordered to J * JT * M^-1 for the linear components, since M^-1 is a scalar and dot(n * scalar, n) = dot(n, n) * scalar. - //2) dot(normal, normal) == 1, so the contribution from each body is just its inverse mass. - var linear = inertiaA.InverseMass + inertiaB.InverseMass; - //Note that we don't precompute the JT * effectiveMass term. Since the jacobians are shared, we have to do that multiply anyway. - projection.EffectiveMass = effectiveMassCFMScale / (linear + angularA0 + angularB0); - - //If depth is negative, the bias velocity will permit motion up until the depth hits zero. This works because positionErrorToVelocity * dt will always be <=1. - projection.BiasVelocity = Vector.Min( - depth * new Vector(inverseDt), - Vector.Min(depth * positionErrorToVelocity, maximumRecoveryVelocity)); - } - - - /// - /// Transforms an impulse from constraint space to world space, uses it to modify the cached world space velocities of the bodies. - /// - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ApplyImpulse(in PenetrationLimitProjection projection, in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, in Vector3Wide normal, - in Vector correctiveImpulse, - ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) - { - var linearVelocityChangeA = correctiveImpulse * inertiaA.InverseMass; - Vector3Wide.Scale(normal, linearVelocityChangeA, out var correctiveVelocityALinearVelocity); - Vector3Wide.Scale(projection.AngularA, correctiveImpulse, out var correctiveAngularImpulseA); - Symmetric3x3Wide.TransformWithoutOverlap(correctiveAngularImpulseA, inertiaA.InverseInertiaTensor, out var correctiveVelocityAAngularVelocity); - - var linearVelocityChangeB = correctiveImpulse * inertiaB.InverseMass; - Vector3Wide.Scale(normal, linearVelocityChangeB, out var correctiveVelocityBLinearVelocity); - Vector3Wide.Scale(projection.AngularB, correctiveImpulse, out var correctiveAngularImpulseB); - Symmetric3x3Wide.TransformWithoutOverlap(correctiveAngularImpulseB, inertiaB.InverseInertiaTensor, out var correctiveVelocityBAngularVelocity); - - Vector3Wide.Add(wsvA.Linear, correctiveVelocityALinearVelocity, out wsvA.Linear); - Vector3Wide.Add(wsvA.Angular, correctiveVelocityAAngularVelocity, out wsvA.Angular); - Vector3Wide.Subtract(wsvB.Linear, correctiveVelocityBLinearVelocity, out wsvB.Linear); //Note subtract; normal = -jacobianLinearB - Vector3Wide.Add(wsvB.Angular, correctiveVelocityBAngularVelocity, out wsvB.Angular); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void WarmStart( - in PenetrationLimitProjection projection, in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, in Vector3Wide normal, - in Vector accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) - { - ApplyImpulse(projection, inertiaA, inertiaB, normal, accumulatedImpulse, ref wsvA, ref wsvB); - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ComputeCorrectiveImpulse(in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, in Vector3Wide normal, in Vector3Wide angularA, in Vector3Wide angularB, in Vector biasVelocity, in Vector softnessImpulseScale, in Vector effectiveMass, @@ -124,14 +27,6 @@ public static void ComputeCorrectiveImpulse(in BodyVelocityWide wsvA, in BodyVel correctiveCSI = accumulatedImpulse - previousAccumulated; } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Solve(in PenetrationLimitProjection projection, in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, in Vector3Wide normal, - in Vector softnessImpulseScale, ref Vector accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) - { - ComputeCorrectiveImpulse(wsvA, wsvB, normal, projection.AngularA, projection.AngularB, projection.BiasVelocity, softnessImpulseScale, projection.EffectiveMass, ref accumulatedImpulse, out var correctiveCSI); - ApplyImpulse(projection, inertiaA, inertiaB, normal, correctiveCSI, ref wsvA, ref wsvB); - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void UpdatePenetrationDepth(in Vector dt, in Vector3Wide contactOffsetA, in Vector3Wide offsetB, in Vector3Wide normal, in BodyVelocityWide velocityA, in BodyVelocityWide velocityB, ref Vector penetrationDepth) { @@ -171,7 +66,7 @@ public static void ApplyImpulse(in BodyInertiaWide inertiaA, in BodyInertiaWide } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void WarmStart2( + public static void WarmStart( in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, in Vector3Wide normal, in Vector3Wide contactOffsetA, in Vector3Wide contactOffsetB, in Vector accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { @@ -182,8 +77,8 @@ public static void WarmStart2( [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Solve2( - in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, in Vector3Wide normal, in Vector3Wide contactOffsetA, in Vector3Wide contactOffsetB, + public static void Solve( + in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, in Vector3Wide normal, in Vector3Wide contactOffsetA, in Vector3Wide contactOffsetB, in Vector depth, in Vector positionErrorToVelocity, in Vector effectiveMassCFMScale, in Vector maximumRecoveryVelocity, in Vector inverseDt, in Vector softnessImpulseScale, ref Vector accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { diff --git a/BepuPhysics/Constraints/Contact/PenetrationLimitOneBody.cs b/BepuPhysics/Constraints/Contact/PenetrationLimitOneBody.cs index 883595026..8a3ffe34d 100644 --- a/BepuPhysics/Constraints/Contact/PenetrationLimitOneBody.cs +++ b/BepuPhysics/Constraints/Contact/PenetrationLimitOneBody.cs @@ -6,62 +6,8 @@ namespace BepuPhysics.Constraints.Contact { - public struct PenetrationLimitOneBodyProjection - { - //Note that these are just the raw jacobians, no precomputation with the JT*EffectiveMass. - public Vector3Wide AngularA; - public Vector EffectiveMass; - public Vector BiasVelocity; - } - public static class PenetrationLimitOneBody { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Prestep(in BodyInertiaWide inertiaA, - in Vector3Wide contactOffsetA, in Vector3Wide normal, in Vector depth, - in Vector positionErrorToVelocity, in Vector effectiveMassCFMScale, in Vector maximumRecoveryVelocity, - float inverseDt, out PenetrationLimitOneBodyProjection projection) - { - //See PenetrationLimit.cs for a derivation. - Vector3Wide.CrossWithoutOverlap(contactOffsetA, normal, out projection.AngularA); - - //effective mass - Symmetric3x3Wide.VectorSandwich(projection.AngularA, inertiaA.InverseInertiaTensor, out var angularA0); - - //Note that we don't precompute the JT * effectiveMass term. Since the jacobians are shared, we have to do that multiply anyway. - projection.EffectiveMass = effectiveMassCFMScale / (inertiaA.InverseMass + angularA0); - - //If depth is negative, the bias velocity will permit motion up until the depth hits zero. This works because positionErrorToVelocity * dt will always be <=1. - projection.BiasVelocity = Vector.Min( - depth * new Vector(inverseDt), - Vector.Min(depth * positionErrorToVelocity, maximumRecoveryVelocity)); - } - - - /// - /// Transforms an impulse from constraint space to world space, uses it to modify the cached world space velocities of the bodies. - /// - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ApplyImpulse(in PenetrationLimitOneBodyProjection projection, in BodyInertiaWide inertiaA, in Vector3Wide normal, - in Vector correctiveImpulse, ref BodyVelocityWide wsvA) - { - var linearVelocityChangeA = correctiveImpulse * inertiaA.InverseMass; - Vector3Wide.Scale(normal, linearVelocityChangeA, out var correctiveVelocityALinearVelocity); - Vector3Wide.Scale(projection.AngularA, correctiveImpulse, out var correctiveAngularImpulseA); - Symmetric3x3Wide.TransformWithoutOverlap(correctiveAngularImpulseA, inertiaA.InverseInertiaTensor, out var correctiveVelocityAAngularVelocity); - - Vector3Wide.Add(wsvA.Linear, correctiveVelocityALinearVelocity, out wsvA.Linear); - Vector3Wide.Add(wsvA.Angular, correctiveVelocityAAngularVelocity, out wsvA.Angular); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void WarmStart( - in PenetrationLimitOneBodyProjection projection, in BodyInertiaWide inertiaA, in Vector3Wide normal, - in Vector accumulatedImpulse, ref BodyVelocityWide wsvA) - { - ApplyImpulse(projection, inertiaA, normal, accumulatedImpulse, ref wsvA); - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ComputeCorrectiveImpulse(in BodyVelocityWide wsvA, in Vector3Wide normal, in Vector3Wide angularA, in Vector biasVelocity, in Vector softnessImpulseScale, in Vector effectiveMass, @@ -79,14 +25,6 @@ public static void ComputeCorrectiveImpulse(in BodyVelocityWide wsvA, correctiveCSI = accumulatedImpulse - previousAccumulated; } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Solve(in PenetrationLimitOneBodyProjection projection, in BodyInertiaWide inertiaA, in Vector3Wide normal, - in Vector softnessImpulseScale, ref Vector accumulatedImpulse, ref BodyVelocityWide wsvA) - { - ComputeCorrectiveImpulse(wsvA, normal, projection.AngularA, projection.BiasVelocity, softnessImpulseScale, projection.EffectiveMass, ref accumulatedImpulse, out var correctiveCSI); - ApplyImpulse(projection, inertiaA, normal, correctiveCSI, ref wsvA); - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void UpdatePenetrationDepth(in Vector dt, in Vector3Wide contactOffset, in Vector3Wide normal, in BodyVelocityWide velocity, ref Vector penetrationDepth) { @@ -113,7 +51,7 @@ public static void ApplyImpulse(in BodyInertiaWide inertiaA, in Vector3Wide norm } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void WarmStart2(in BodyInertiaWide inertiaA, in Vector3Wide normal, in Vector3Wide contactOffsetA, in Vector accumulatedImpulse, ref BodyVelocityWide wsvA) + public static void WarmStart(in BodyInertiaWide inertiaA, in Vector3Wide normal, in Vector3Wide contactOffsetA, in Vector accumulatedImpulse, ref BodyVelocityWide wsvA) { Vector3Wide.CrossWithoutOverlap(contactOffsetA, normal, out var angularA); ApplyImpulse(inertiaA, normal, angularA, accumulatedImpulse, ref wsvA); @@ -121,7 +59,7 @@ public static void WarmStart2(in BodyInertiaWide inertiaA, in Vector3Wide normal [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Solve2( + public static void Solve( in BodyInertiaWide inertiaA, in Vector3Wide normal, in Vector3Wide contactOffsetA, in Vector depth, in Vector positionErrorToVelocity, in Vector effectiveMassCFMScale, in Vector maximumRecoveryVelocity, in Vector inverseDt, in Vector softnessImpulseScale, ref Vector accumulatedImpulse, ref BodyVelocityWide wsvA) diff --git a/BepuPhysics/Constraints/Contact/TangentFriction.cs b/BepuPhysics/Constraints/Contact/TangentFriction.cs index 57ec56654..544a30a04 100644 --- a/BepuPhysics/Constraints/Contact/TangentFriction.cs +++ b/BepuPhysics/Constraints/Contact/TangentFriction.cs @@ -10,16 +10,6 @@ namespace BepuPhysics.Constraints.Contact /// public static class TangentFriction { - public struct Projection - { - //Jacobians are generated on the fly from the tangents and offsets. - //The tangents are reconstructed from the surface basis. - //This saves 11 floats per constraint relative to the seminaive baseline of two shared linear jacobians and four angular jacobians. - public Vector3Wide OffsetA; - public Vector3Wide OffsetB; - public Symmetric2x2Wide EffectiveMass; - } - public struct Jacobians { public Matrix2x3Wide LinearA; @@ -63,30 +53,6 @@ public static void ComputeJacobians(in Vector3Wide tangentX, in Vector3Wide tang Vector3Wide.CrossWithoutOverlap(tangentY, offsetB, out jacobians.AngularB.Y); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Prestep(ref Vector3Wide tangentX, ref Vector3Wide tangentY, ref Vector3Wide offsetA, ref Vector3Wide offsetB, - ref BodyInertiaWide inertiaA, ref BodyInertiaWide inertiaB, - out Projection projection) - { - ComputeJacobians(tangentX, tangentY, offsetA, offsetB, out var jacobians); - //Compute effective mass matrix contributions. - Symmetric2x2Wide.SandwichScale(jacobians.LinearA, inertiaA.InverseMass, out var linearContributionA); - Symmetric2x2Wide.SandwichScale(jacobians.LinearA, inertiaB.InverseMass, out var linearContributionB); - - Symmetric3x3Wide.MatrixSandwich(jacobians.AngularA, inertiaA.InverseInertiaTensor, out var angularContributionA); - Symmetric3x3Wide.MatrixSandwich(jacobians.AngularB, inertiaB.InverseInertiaTensor, out var angularContributionB); - - //No softening; this constraint is rigid by design. (It does support a maximum force, but that is distinct from a proper damping ratio/natural frequency.) - Symmetric2x2Wide.Add(linearContributionA, linearContributionB, out var linear); - Symmetric2x2Wide.Add(angularContributionA, angularContributionB, out var angular); - Symmetric2x2Wide.Add(linear, angular, out var inverseEffectiveMass); - Symmetric2x2Wide.InvertWithoutOverlap(inverseEffectiveMass, out projection.EffectiveMass); - projection.OffsetA = offsetA; - projection.OffsetB = offsetB; - - //Note that friction constraints have no bias velocity. They target zero velocity. - } - /// /// Transforms an impulse from constraint space to world space, uses it to modify the cached world space velocities of the bodies. /// @@ -108,16 +74,6 @@ public static void ApplyImpulse(in Jacobians jacobians, in BodyInertiaWide inert Vector3Wide.Add(wsvB.Angular, correctiveVelocityB.Angular, out wsvB.Angular); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void WarmStart(ref Vector3Wide tangentX, ref Vector3Wide tangentY, ref TangentFriction.Projection projection, ref BodyInertiaWide inertiaA, ref BodyInertiaWide inertiaB, - ref Vector2Wide accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) - { - ComputeJacobians(tangentX, tangentY, projection.OffsetA, projection.OffsetB, out var jacobians); - //TODO: If the previous frame and current frame are associated with different time steps, the previous frame's solution won't be a good solution anymore. - //To compensate for this, the accumulated impulse should be scaled if dt changes. - ApplyImpulse(jacobians, inertiaA, inertiaB, accumulatedImpulse, ref wsvA, ref wsvB); - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ComputeCorrectiveImpulse(in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, in Symmetric2x2Wide effectiveMass, in Jacobians jacobians, in Vector maximumImpulse, ref Vector2Wide accumulatedImpulse, out Vector2Wide correctiveCSI) @@ -149,17 +105,7 @@ public static void ComputeCorrectiveImpulse(in BodyVelocityWide wsvA, in BodyVel } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Solve(ref Vector3Wide tangentX, ref Vector3Wide tangentY, ref TangentFriction.Projection projection, ref BodyInertiaWide inertiaA, ref BodyInertiaWide inertiaB, ref Vector maximumImpulse, ref Vector2Wide accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) - { - ComputeJacobians(tangentX, tangentY, projection.OffsetA, projection.OffsetB, out var jacobians); - ComputeCorrectiveImpulse(wsvA, wsvB, projection.EffectiveMass, jacobians, maximumImpulse, ref accumulatedImpulse, out var correctiveCSI); - ApplyImpulse(jacobians, inertiaA, inertiaB, correctiveCSI, ref wsvA, ref wsvB); - - } - - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void WarmStart2(in Vector3Wide tangentX, in Vector3Wide tangentY, in Vector3Wide offsetToManifoldCenterA, in Vector3Wide offsetToManifoldCenterB, in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, + public static void WarmStart(in Vector3Wide tangentX, in Vector3Wide tangentY, in Vector3Wide offsetToManifoldCenterA, in Vector3Wide offsetToManifoldCenterB, in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, in Vector2Wide accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeJacobians(tangentX, tangentY, offsetToManifoldCenterA, offsetToManifoldCenterB, out var jacobians); @@ -170,7 +116,7 @@ public static void WarmStart2(in Vector3Wide tangentX, in Vector3Wide tangentY, [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Solve2(in Vector3Wide tangentX, in Vector3Wide tangentY, in Vector3Wide offsetToManifoldCenterA, in Vector3Wide offsetToManifoldCenterB, in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, + public static void Solve(in Vector3Wide tangentX, in Vector3Wide tangentY, in Vector3Wide offsetToManifoldCenterA, in Vector3Wide offsetToManifoldCenterB, in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, in Vector maximumImpulse, ref Vector2Wide accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeJacobians(tangentX, tangentY, offsetToManifoldCenterA, offsetToManifoldCenterB, out var jacobians); diff --git a/BepuPhysics/Constraints/Contact/TangentFrictionOneBody.cs b/BepuPhysics/Constraints/Contact/TangentFrictionOneBody.cs index 4178f8d55..68b909a43 100644 --- a/BepuPhysics/Constraints/Contact/TangentFrictionOneBody.cs +++ b/BepuPhysics/Constraints/Contact/TangentFrictionOneBody.cs @@ -14,14 +14,6 @@ public struct Jacobians public Matrix2x3Wide LinearA; public Matrix2x3Wide AngularA; } - public struct Projection - { - //Jacobians are generated on the fly from the tangents and offsets. - //The tangents are reconstructed from the surface basis. - //This saves 11 floats per constraint relative to the seminaive baseline of two shared linear jacobians and four angular jacobians. - public Vector3Wide OffsetA; - public Symmetric2x2Wide EffectiveMass; - } //Since this is an unshared specialized implementation, the jacobian calculation is kept in here rather than in the batch. [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -34,23 +26,6 @@ public static void ComputeJacobians(in Vector3Wide tangentX, in Vector3Wide tang Vector3Wide.CrossWithoutOverlap(offsetA, tangentY, out jacobians.AngularA.Y); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Prestep(ref Vector3Wide tangentX, ref Vector3Wide tangentY, ref Vector3Wide offsetA, ref BodyInertiaWide inertiaA, - out Projection projection) - { - ComputeJacobians(tangentX, tangentY, offsetA, out var jacobians); - //Compute effective mass matrix contributions. - Symmetric2x2Wide.SandwichScale(jacobians.LinearA, inertiaA.InverseMass, out var linearContributionA); - Symmetric3x3Wide.MatrixSandwich(jacobians.AngularA, inertiaA.InverseInertiaTensor, out var angularContributionA); - - //No softening; this constraint is rigid by design. (It does support a maximum force, but that is distinct from a proper damping ratio/natural frequency.) - Symmetric2x2Wide.Add(linearContributionA, angularContributionA, out var inverseEffectiveMass); - Symmetric2x2Wide.InvertWithoutOverlap(inverseEffectiveMass, out projection.EffectiveMass); - projection.OffsetA = offsetA; - - //Note that friction constraints have no bias velocity. They target zero velocity. - } - /// /// Transforms an impulse from constraint space to world space, uses it to modify the cached world space velocities of the bodies. /// @@ -67,16 +42,6 @@ public static void ApplyImpulse(in Jacobians jacobians, in BodyInertiaWide inert Vector3Wide.Add(wsvA.Angular, correctiveVelocityA.Angular, out wsvA.Angular); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void WarmStart(ref Vector3Wide tangentX, ref Vector3Wide tangentY, ref Projection projection, ref BodyInertiaWide inertiaA, - ref Vector2Wide accumulatedImpulse, ref BodyVelocityWide wsvA) - { - ComputeJacobians(tangentX, tangentY, projection.OffsetA, out var jacobians); - //TODO: If the previous frame and current frame are associated with different time steps, the previous frame's solution won't be a good solution anymore. - //To compensate for this, the accumulated impulse should be scaled if dt changes. - ApplyImpulse(jacobians, inertiaA, accumulatedImpulse, ref wsvA); - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ComputeCorrectiveImpulse(in BodyVelocityWide wsvA, in Symmetric2x2Wide effectiveMass, in Jacobians jacobians, in Vector maximumImpulse, ref Vector2Wide accumulatedImpulse, out Vector2Wide correctiveCSI) @@ -100,18 +65,7 @@ public static void ComputeCorrectiveImpulse(in BodyVelocityWide wsvA, in Symmetr } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Solve(ref Vector3Wide tangentX, ref Vector3Wide tangentY, - ref Projection projection, ref BodyInertiaWide inertiaA, ref Vector maximumImpulse, ref Vector2Wide accumulatedImpulse, ref BodyVelocityWide wsvA) - { - ComputeJacobians(tangentX, tangentY, projection.OffsetA, out var jacobians); - ComputeCorrectiveImpulse(wsvA, projection.EffectiveMass, jacobians, maximumImpulse, ref accumulatedImpulse, out var correctiveCSI); - ApplyImpulse(jacobians, inertiaA, correctiveCSI, ref wsvA); - - } - - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void WarmStart2(in Vector3Wide tangentX, in Vector3Wide tangentY, in Vector3Wide offsetToManifoldCenterA, in BodyInertiaWide inertiaA, in Vector2Wide accumulatedImpulse, ref BodyVelocityWide wsvA) + public static void WarmStart(in Vector3Wide tangentX, in Vector3Wide tangentY, in Vector3Wide offsetToManifoldCenterA, in BodyInertiaWide inertiaA, in Vector2Wide accumulatedImpulse, ref BodyVelocityWide wsvA) { ComputeJacobians(tangentX, tangentY, offsetToManifoldCenterA, out var jacobians); //TODO: If the previous frame and current frame are associated with different time steps, the previous frame's solution won't be a good solution anymore. @@ -120,7 +74,7 @@ public static void WarmStart2(in Vector3Wide tangentX, in Vector3Wide tangentY, } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Solve2(in Vector3Wide tangentX, in Vector3Wide tangentY, in Vector3Wide offsetToManifoldCenterA, in BodyInertiaWide inertiaA, + public static void Solve(in Vector3Wide tangentX, in Vector3Wide tangentY, in Vector3Wide offsetToManifoldCenterA, in BodyInertiaWide inertiaA, in Vector maximumImpulse, ref Vector2Wide accumulatedImpulse, ref BodyVelocityWide wsvA) { ComputeJacobians(tangentX, tangentY, offsetToManifoldCenterA, out var jacobians); diff --git a/BepuPhysics/Constraints/Contact/TwistFriction.cs b/BepuPhysics/Constraints/Contact/TwistFriction.cs index e39f180a2..b26605251 100644 --- a/BepuPhysics/Constraints/Contact/TwistFriction.cs +++ b/BepuPhysics/Constraints/Contact/TwistFriction.cs @@ -5,41 +5,11 @@ namespace BepuPhysics.Constraints.Contact { - //For in depth explanations of constraints, check the Inequality1DOF.cs implementation. - //The details are omitted for brevity in other implementations. - - public struct TwistFrictionProjection - { - //Jacobians and inertia are shared with other constraints. - public Vector EffectiveMass; - } - /// /// Handles the tangent friction implementation. /// public static class TwistFriction { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Prestep(ref BodyInertiaWide inertiaA, ref BodyInertiaWide inertiaB, ref Vector3Wide angularJacobianA, - out TwistFrictionProjection projection) - { - //Compute effective mass matrix contributions. No linear contributions for the twist constraint. - //Note that we use the angularJacobianA (that is, the normal) for both, despite angularJacobianB = -angularJacobianA. That's fine- J * M * JT is going to be positive regardless. - Symmetric3x3Wide.VectorSandwich(angularJacobianA, inertiaA.InverseInertiaTensor, out var angularA); - Symmetric3x3Wide.VectorSandwich(angularJacobianA, inertiaB.InverseInertiaTensor, out var angularB); - - //No softening; this constraint is rigid by design. (It does support a maximum force, but that is distinct from a proper damping ratio/natural frequency.) - //Note that we have to guard against two bodies with infinite inertias. This is a valid state! - //(We do not have to do such guarding on constraints with linear jacobians; dynamic bodies cannot have zero *mass*.) - //(Also note that there's no need for epsilons here... users shouldn't be setting their inertias to the absurd values it would take to cause a problem. - //Invalid conditions can't arise dynamically.) - var inverseEffectiveMass = angularA + angularB; - var inverseIsZero = Vector.Equals(Vector.Zero, inverseEffectiveMass); - projection.EffectiveMass = Vector.ConditionalSelect(inverseIsZero, Vector.Zero, Vector.One / inverseEffectiveMass); - - //Note that friction constraints have no bias velocity. They target zero velocity. - } - /// /// Transforms an impulse from constraint space to world space, uses it to modify the cached world space velocities of the bodies. /// @@ -54,13 +24,6 @@ public static void ApplyImpulse(in Vector3Wide angularJacobianA, in BodyInertiaW Vector3Wide.Subtract(wsvB.Angular, worldCorrectiveVelocityB, out wsvB.Angular); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void WarmStart(ref Vector3Wide angularJacobianA, ref BodyInertiaWide inertiaA, ref BodyInertiaWide inertiaB, - ref Vector accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) - { - ApplyImpulse(angularJacobianA, inertiaA, inertiaB, accumulatedImpulse, ref wsvA, ref wsvB); - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ComputeCorrectiveImpulse(in Vector3Wide angularJacobianA, in Vector effectiveMass, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, in Vector maximumImpulse, @@ -78,23 +41,14 @@ public static void ComputeCorrectiveImpulse(in Vector3Wide angularJacobianA, in } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Solve(ref Vector3Wide angularJacobianA, ref BodyInertiaWide inertiaA, ref BodyInertiaWide inertiaB, ref TwistFrictionProjection projection, - ref Vector maximumImpulse, ref Vector accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) - { - ComputeCorrectiveImpulse(angularJacobianA, projection.EffectiveMass, wsvA, wsvB, maximumImpulse, ref accumulatedImpulse, out var correctiveCSI); - ApplyImpulse(angularJacobianA, inertiaA, inertiaB, correctiveCSI, ref wsvA, ref wsvB); - - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void WarmStart2(in Vector3Wide angularJacobianA, in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, + public static void WarmStart(in Vector3Wide angularJacobianA, in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, in Vector accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ApplyImpulse(angularJacobianA, inertiaA, inertiaB, accumulatedImpulse, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Solve2(in Vector3Wide angularJacobianA, in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, + public static void Solve(in Vector3Wide angularJacobianA, in BodyInertiaWide inertiaA, in BodyInertiaWide inertiaB, in Vector maximumImpulse, ref Vector accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { //Compute effective mass matrix contributions. No linear contributions for the twist constraint. diff --git a/BepuPhysics/Constraints/Contact/TwistFrictionOneBody.cs b/BepuPhysics/Constraints/Contact/TwistFrictionOneBody.cs index b35322c30..ad00025fe 100644 --- a/BepuPhysics/Constraints/Contact/TwistFrictionOneBody.cs +++ b/BepuPhysics/Constraints/Contact/TwistFrictionOneBody.cs @@ -12,26 +12,6 @@ namespace BepuPhysics.Constraints.Contact /// public static class TwistFrictionOneBody { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Prestep(ref BodyInertiaWide inertiaA, ref Vector3Wide angularJacobianA, - out TwistFrictionProjection projection) - { - //Compute effective mass matrix contributions. No linear contributions for the twist constraint. - Symmetric3x3Wide.VectorSandwich(angularJacobianA, inertiaA.InverseInertiaTensor, out var inverseEffectiveMass); - - //No softening; this constraint is rigid by design. (It does support a maximum force, but that is distinct from a proper damping ratio/natural frequency.) - //Note that we have to guard against two bodies with infinite inertias. This is a valid state! - //(We do not have to do such guarding on constraints with linear jacobians; dynamic bodies cannot have zero *mass*.) - //(Also note that there's no need for epsilons here... users shouldn't be setting their inertias to the absurd values it would take to cause a problem. - //Invalid conditions can't arise dynamically.) - var inverseIsZero = Vector.Equals(Vector.Zero, inverseEffectiveMass); - projection.EffectiveMass = Vector.ConditionalSelect(inverseIsZero, Vector.Zero, Vector.One / inverseEffectiveMass); - - //Note that friction constraints have no bias velocity. They target zero velocity. - - - } - /// /// Transforms an impulse from constraint space to world space, uses it to modify the cached world space velocities of the bodies. /// @@ -44,13 +24,6 @@ public static void ApplyImpulse(in Vector3Wide angularJacobianA, in BodyInertiaW Vector3Wide.Add(wsvA.Angular, worldCorrectiveVelocityA, out wsvA.Angular); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void WarmStart(ref Vector3Wide angularJacobianA, ref BodyInertiaWide inertiaA, - ref Vector accumulatedImpulse, ref BodyVelocityWide wsvA) - { - ApplyImpulse(angularJacobianA, inertiaA, accumulatedImpulse, ref wsvA); - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ComputeCorrectiveImpulse(in Vector3Wide angularJacobianA, in Vector effectiveMass, in BodyVelocityWide wsvA, in Vector maximumImpulse, @@ -68,23 +41,13 @@ public static void ComputeCorrectiveImpulse(in Vector3Wide angularJacobianA, in } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Solve(ref Vector3Wide angularJacobianA, ref BodyInertiaWide inertiaA, ref TwistFrictionProjection projection, - ref Vector maximumImpulse, ref Vector accumulatedImpulse, ref BodyVelocityWide wsvA) - { - ComputeCorrectiveImpulse(angularJacobianA, projection.EffectiveMass, wsvA, maximumImpulse, ref accumulatedImpulse, out var correctiveCSI); - ApplyImpulse(angularJacobianA, inertiaA, correctiveCSI, ref wsvA); - - } - - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void WarmStart2(in Vector3Wide angularJacobianA, in BodyInertiaWide inertiaA, in Vector accumulatedImpulse, ref BodyVelocityWide wsvA) + public static void WarmStart(in Vector3Wide angularJacobianA, in BodyInertiaWide inertiaA, in Vector accumulatedImpulse, ref BodyVelocityWide wsvA) { ApplyImpulse(angularJacobianA, inertiaA, accumulatedImpulse, ref wsvA); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Solve2(in Vector3Wide angularJacobianA, in BodyInertiaWide inertiaA, in Vector maximumImpulse, ref Vector accumulatedImpulse, ref BodyVelocityWide wsvA) + public static void Solve(in Vector3Wide angularJacobianA, in BodyInertiaWide inertiaA, in Vector maximumImpulse, ref Vector accumulatedImpulse, ref BodyVelocityWide wsvA) { //Compute effective mass matrix contributions. No linear contributions for the twist constraint. //Note that we use the angularJacobianA (that is, the normal) for both, despite angularJacobianB = -angularJacobianA. That's fine- J * M * JT is going to be positive regardless. diff --git a/BepuPhysics/Constraints/DistanceLimit.cs b/BepuPhysics/Constraints/DistanceLimit.cs index fd147a225..a593f8e7e 100644 --- a/BepuPhysics/Constraints/DistanceLimit.cs +++ b/BepuPhysics/Constraints/DistanceLimit.cs @@ -138,14 +138,14 @@ public void ComputeJacobians( } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref DistanceLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref DistanceLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeJacobians(prestep.LocalOffsetA, positionA, orientationA, prestep.LocalOffsetB, positionB, orientationB, prestep.MinimumDistance, prestep.MaximumDistance, out _, out _, out var direction, out var angularJA, out var angularJB); ApplyImpulse(direction, angularJA, angularJB, inertiaA, inertiaB, accumulatedImpulses, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref DistanceLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref DistanceLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeJacobians(prestep.LocalOffsetA, positionA, orientationA, prestep.LocalOffsetB, positionB, orientationB, prestep.MinimumDistance, prestep.MaximumDistance, out var useMinimum, out var distance, out var direction, out var angularJA, out var angularJB); diff --git a/BepuPhysics/Constraints/DistanceServo.cs b/BepuPhysics/Constraints/DistanceServo.cs index 4713d4774..2d78f192e 100644 --- a/BepuPhysics/Constraints/DistanceServo.cs +++ b/BepuPhysics/Constraints/DistanceServo.cs @@ -182,7 +182,7 @@ public static void ApplyImpulse( } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref DistanceServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref DistanceServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { GetDistance(orientationA, positionB - positionA, orientationB, prestep.LocalOffsetA, prestep.LocalOffsetB, out var anchorOffsetA, out var anchorOffsetB, out var anchorOffset, out var distance); Vector3Wide.Scale(anchorOffset, Vector.One / distance, out var direction); @@ -192,7 +192,7 @@ public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, ApplyImpulse(inertiaA.InverseMass, inertiaB.InverseMass, direction, angularImpulseToVelocityA, angularImpulseToVelocityB, accumulatedImpulses, ref wsvA, ref wsvB); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref DistanceServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref DistanceServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { GetDistance(orientationA, positionB - positionA, orientationB, prestep.LocalOffsetA, prestep.LocalOffsetB, out var anchorOffsetA, out var anchorOffsetB, out var anchorOffset, out var distance); diff --git a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs index d1b5b3090..f248438dc 100644 --- a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs @@ -26,13 +26,13 @@ public struct FourBodyReferences /// Type of the accumulated impulses used by the constraint. public interface IFourBodyConstraintFunctions { - void WarmStart2( + void WarmStart( in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in Vector3Wide positionD, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, ref TPrestepData prestep, ref TAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC, ref BodyVelocityWide wsvD); - void Solve2( + void Solve( in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, @@ -104,7 +104,7 @@ internal sealed override void VerifySortRegion(ref TypeBatch typeBatch, int bund VerifySortRegion(ref typeBatch, bundleStartIndex, constraintCount, ref sortedKeys, ref sortedSourceIndices); } - public unsafe override void WarmStart2( + public unsafe override void WarmStart( ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { @@ -130,7 +130,7 @@ public unsafe override void WarmStart2(dt), accumulatedImpulses, ref prestep); - function.WarmStart2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, positionD, orientationD, inertiaD, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC, ref wsvD); + function.WarmStart(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, positionD, orientationD, inertiaD, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC, ref wsvD); if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) { @@ -152,7 +152,7 @@ public unsafe override void WarmStart2(); var bodyReferencesBundles = typeBatch.BodyReferences.As(); @@ -169,7 +169,7 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f bodies.GatherState(references.IndexC, true, out var positionC, out var orientationC, out var wsvC, out var inertiaC); bodies.GatherState(references.IndexD, true, out var positionD, out var orientationD, out var wsvD, out var inertiaD); - function.Solve2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, positionD, orientationD, inertiaD, dt, inverseDt, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC, ref wsvD); + function.Solve(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, positionD, orientationD, inertiaD, dt, inverseDt, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC, ref wsvD); bodies.ScatterVelocities(ref wsvA, ref references.IndexA); bodies.ScatterVelocities(ref wsvB, ref references.IndexB); diff --git a/BepuPhysics/Constraints/Hinge.cs b/BepuPhysics/Constraints/Hinge.cs index 297c286e4..4723aeca4 100644 --- a/BepuPhysics/Constraints/Hinge.cs +++ b/BepuPhysics/Constraints/Hinge.cs @@ -113,7 +113,7 @@ private static void ApplyImpulse(in Vector3Wide offsetA, in Vector3Wide offsetB, Vector3Wide.Add(velocityB.Angular, angularChangeB, out velocityB.Angular); } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref HingePrestepData prestep, ref HingeAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref HingePrestepData prestep, ref HingeAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { Matrix3x3Wide.CreateFromQuaternion(orientationA, out var orientationMatrixA); Matrix3x3Wide.TransformWithoutOverlap(prestep.LocalOffsetA, orientationMatrixA, out var offsetA); @@ -125,7 +125,7 @@ public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, ApplyImpulse(offsetA, offsetB, hingeJacobian, inertiaA, inertiaB, accumulatedImpulses, ref wsvA, ref wsvB); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref HingePrestepData prestep, ref HingeAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref HingePrestepData prestep, ref HingeAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { //5x12 jacobians, from BallSocket and AngularHinge: //[ I, skew(offsetA), -I, -skew(offsetB) ] diff --git a/BepuPhysics/Constraints/LinearAxisLimit.cs b/BepuPhysics/Constraints/LinearAxisLimit.cs index 14e7e4d35..2ed51935d 100644 --- a/BepuPhysics/Constraints/LinearAxisLimit.cs +++ b/BepuPhysics/Constraints/LinearAxisLimit.cs @@ -118,7 +118,7 @@ static void ComputeJacobians( Vector3Wide.CrossWithoutOverlap(normal, offsetB, out angularJB); } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref LinearAxisLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref LinearAxisLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeJacobians(positionB - positionA, orientationA, orientationB, prestep.LocalPlaneNormal, prestep.LocalOffsetA, prestep.LocalOffsetB, prestep.MinimumOffset, prestep.MaximumOffset, out _, out var normal, out var angularJA, out var angularJB); Symmetric3x3Wide.TransformWithoutOverlap(angularJA, inertiaA.InverseInertiaTensor, out var angularImpulseToVelocityA); @@ -126,7 +126,7 @@ public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, LinearAxisServoFunctions.ApplyImpulse(normal, angularImpulseToVelocityA, angularImpulseToVelocityB, inertiaA, inertiaB, accumulatedImpulses, ref wsvA, ref wsvB); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref LinearAxisLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref LinearAxisLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeJacobians(positionB - positionA, orientationA, orientationB, prestep.LocalPlaneNormal, prestep.LocalOffsetA, prestep.LocalOffsetB, prestep.MinimumOffset, prestep.MaximumOffset, out var error, out var normal, out var angularJA, out var angularJB); diff --git a/BepuPhysics/Constraints/LinearAxisMotor.cs b/BepuPhysics/Constraints/LinearAxisMotor.cs index 5c43e4650..cce43ac24 100644 --- a/BepuPhysics/Constraints/LinearAxisMotor.cs +++ b/BepuPhysics/Constraints/LinearAxisMotor.cs @@ -81,7 +81,7 @@ public struct LinearAxisMotorPrestepData public struct LinearAxisMotorFunctions : ITwoBodyConstraintFunctions> { - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref LinearAxisMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref LinearAxisMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { LinearAxisServoFunctions.ComputeJacobians(positionB - positionA, orientationA, orientationB, prestep.LocalPlaneNormal, prestep.LocalOffsetA, prestep.LocalOffsetB, out _, out var normal, out var angularJA, out var angularJB); Symmetric3x3Wide.TransformWithoutOverlap(angularJA, inertiaA.InverseInertiaTensor, out var angularImpulseToVelocityA); @@ -89,7 +89,7 @@ public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, LinearAxisServoFunctions.ApplyImpulse(normal, angularImpulseToVelocityA, angularImpulseToVelocityB, inertiaA, inertiaB, accumulatedImpulses, ref wsvA, ref wsvB); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref LinearAxisMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref LinearAxisMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { LinearAxisServoFunctions.ComputeJacobians(positionB - positionA, orientationA, orientationB, prestep.LocalPlaneNormal, prestep.LocalOffsetA, prestep.LocalOffsetB, out _, out var normal, out var angularJA, out var angularJB); MotorSettingsWide.ComputeSoftness(prestep.Settings, dt, out var effectiveMassCFMScale, out var softnessImpulseScale, out var maximumImpulse); diff --git a/BepuPhysics/Constraints/LinearAxisServo.cs b/BepuPhysics/Constraints/LinearAxisServo.cs index eb7a782f1..47d41bead 100644 --- a/BepuPhysics/Constraints/LinearAxisServo.cs +++ b/BepuPhysics/Constraints/LinearAxisServo.cs @@ -215,7 +215,7 @@ public static void ComputeEffectiveMass(in Vector3Wide angularJA, in Vector3Wide effectiveMass = effectiveMassCFMScale / (inertiaA.InverseMass + inertiaB.InverseMass + angularContributionA + angularContributionB); } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref LinearAxisServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref LinearAxisServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeJacobians(positionB - positionA, orientationA, orientationB, prestep.LocalPlaneNormal, prestep.LocalOffsetA, prestep.LocalOffsetB, out _, out var normal, out var angularJA, out var angularJB); Symmetric3x3Wide.TransformWithoutOverlap(angularJA, inertiaA.InverseInertiaTensor, out var angularImpulseToVelocityA); @@ -223,7 +223,7 @@ public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, ApplyImpulse(normal, angularImpulseToVelocityA, angularImpulseToVelocityB, inertiaA, inertiaB, accumulatedImpulses, ref wsvA, ref wsvB); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref LinearAxisServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref LinearAxisServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeJacobians(positionB - positionA, orientationA, orientationB, prestep.LocalPlaneNormal, prestep.LocalOffsetA, prestep.LocalOffsetB, out var planeNormalDot, out var normal, out var angularJA, out var angularJB); SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); diff --git a/BepuPhysics/Constraints/OneBodyAngularMotor.cs b/BepuPhysics/Constraints/OneBodyAngularMotor.cs index 03768f6ce..636f23e44 100644 --- a/BepuPhysics/Constraints/OneBodyAngularMotor.cs +++ b/BepuPhysics/Constraints/OneBodyAngularMotor.cs @@ -66,12 +66,12 @@ public static void ApplyImpulse(ref Vector3Wide angularVelocity, in Symmetric3x3 Vector3Wide.Add(angularVelocity, velocityChange, out angularVelocity); } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref OneBodyAngularMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) + public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref OneBodyAngularMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) { ApplyImpulse(ref wsvA.Angular, inertiaA.InverseInertiaTensor, accumulatedImpulses); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref OneBodyAngularMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) + public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref OneBodyAngularMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) { //Jacobians are just the identity matrix. MotorSettingsWide.ComputeSoftness(prestep.Settings, dt, out var effectiveMassCFMScale, out var softnessImpulseScale, out var maximumImpulse); diff --git a/BepuPhysics/Constraints/OneBodyAngularServo.cs b/BepuPhysics/Constraints/OneBodyAngularServo.cs index d6a4d3c85..f2909f283 100644 --- a/BepuPhysics/Constraints/OneBodyAngularServo.cs +++ b/BepuPhysics/Constraints/OneBodyAngularServo.cs @@ -75,13 +75,13 @@ public static void ApplyImpulse(in Symmetric3x3Wide inverseInertia, in Vector3Wi } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref OneBodyAngularServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) + public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref OneBodyAngularServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) { ApplyImpulse(inertiaA.InverseInertiaTensor, accumulatedImpulses, ref wsvA.Angular); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref OneBodyAngularServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) + public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref OneBodyAngularServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) { //Jacobians are just the identity matrix. QuaternionWide.Conjugate(orientationA, out var inverseOrientation); diff --git a/BepuPhysics/Constraints/OneBodyLinearMotor.cs b/BepuPhysics/Constraints/OneBodyLinearMotor.cs index b521587ad..fe15b058f 100644 --- a/BepuPhysics/Constraints/OneBodyLinearMotor.cs +++ b/BepuPhysics/Constraints/OneBodyLinearMotor.cs @@ -66,13 +66,13 @@ public struct OneBodyLinearMotorPrestepData public struct OneBodyLinearMotorFunctions : IOneBodyConstraintFunctions { - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref OneBodyLinearMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) + public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref OneBodyLinearMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) { QuaternionWide.TransformWithoutOverlap(prestep.LocalOffset, orientationA, out var offset); OneBodyLinearServoFunctions.ApplyImpulse(offset, inertiaA, ref wsvA, accumulatedImpulses); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref OneBodyLinearMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) + public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref OneBodyLinearMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) { QuaternionWide.TransformWithoutOverlap(prestep.LocalOffset, orientationA, out var offset); MotorSettingsWide.ComputeSoftness(prestep.Settings, dt, out var effectiveMassCFMScale, out var softnessImpulseScale, out var maximumImpulse); diff --git a/BepuPhysics/Constraints/OneBodyLinearServo.cs b/BepuPhysics/Constraints/OneBodyLinearServo.cs index 93a141650..1a72056e3 100644 --- a/BepuPhysics/Constraints/OneBodyLinearServo.cs +++ b/BepuPhysics/Constraints/OneBodyLinearServo.cs @@ -104,14 +104,14 @@ public static void ApplyImpulse(in Vector3Wide offset, in BodyInertiaWide inerti } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref OneBodyLinearServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) + public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref OneBodyLinearServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) { QuaternionWide.TransformWithoutOverlap(prestep.LocalOffset, orientationA, out var offset); ApplyImpulse(offset, inertiaA, ref wsvA, accumulatedImpulses); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref OneBodyLinearServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) + public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref OneBodyLinearServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) { QuaternionWide.TransformWithoutOverlap(prestep.LocalOffset, orientationA, out var offset); SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); diff --git a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs index 5260d77c6..112c2d427 100644 --- a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs @@ -15,9 +15,9 @@ namespace BepuPhysics.Constraints /// Type of the accumulated impulses used by the constraint. public interface IOneBodyConstraintFunctions { - void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, + void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref TPrestepData prestep, ref TAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA); - void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, + void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref TPrestepData prestep, ref TAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA); /// @@ -81,7 +81,7 @@ internal sealed override void VerifySortRegion(ref TypeBatch typeBatch, int bund //only has to specify *type* arguments associated with the interface-implementing struct-delegates. It's going to look very strange, but it's low overhead //and minimizes per-type duplication. - public unsafe override void WarmStart2( + public unsafe override void WarmStart( ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { @@ -101,7 +101,7 @@ public unsafe override void WarmStart2(dt), accumulatedImpulses, ref prestep); - function.WarmStart2(positionA, orientationA, inertiaA, ref prestep, ref accumulatedImpulses, ref wsvA); + function.WarmStart(positionA, orientationA, inertiaA, ref prestep, ref accumulatedImpulses, ref wsvA); if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) { @@ -116,7 +116,7 @@ public unsafe override void WarmStart2(); var bodyReferencesBundles = typeBatch.BodyReferences.As>(); @@ -130,7 +130,7 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f ref var references = ref bodyReferencesBundles[i]; bodies.GatherState(references, true, out var positionA, out var orientationA, out var wsvA, out var inertiaA); - function.Solve2(positionA, orientationA, inertiaA, dt, inverseDt, ref prestep, ref accumulatedImpulses, ref wsvA); + function.Solve(positionA, orientationA, inertiaA, dt, inverseDt, ref prestep, ref accumulatedImpulses, ref wsvA); bodies.ScatterVelocities(ref wsvA, ref references); } diff --git a/BepuPhysics/Constraints/PointOnLineServo.cs b/BepuPhysics/Constraints/PointOnLineServo.cs index dcda2d73f..1812d14e8 100644 --- a/BepuPhysics/Constraints/PointOnLineServo.cs +++ b/BepuPhysics/Constraints/PointOnLineServo.cs @@ -125,13 +125,13 @@ public static void ComputeJacobians(in Vector3Wide ab, in QuaternionWide orienta Vector3Wide.CrossWithoutOverlap(linearJacobian.X, offsetB, out angularJB.X); Vector3Wide.CrossWithoutOverlap(linearJacobian.Y, offsetB, out angularJB.Y); } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref PointOnLineServoPrestepData prestep, ref Vector2Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref PointOnLineServoPrestepData prestep, ref Vector2Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeJacobians(positionB - positionA, orientationA, orientationB, prestep.LocalDirection, prestep.LocalOffsetA, prestep.LocalOffsetB, out _, out var linearJacobian, out var angularJA, out var angularJB); ApplyImpulse(ref wsvA, ref wsvB, linearJacobian, angularJA, angularJB, inertiaA, inertiaB, ref accumulatedImpulses); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref PointOnLineServoPrestepData prestep, ref Vector2Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref PointOnLineServoPrestepData prestep, ref Vector2Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { //This constrains a point on B to a line attached to A. It works on two degrees of freedom at the same time; those are the tangent axes to the line direction. //The error is measured as closest offset from the line. In other words: diff --git a/BepuPhysics/Constraints/SwingLimit.cs b/BepuPhysics/Constraints/SwingLimit.cs index 8c8ff74af..8f0915473 100644 --- a/BepuPhysics/Constraints/SwingLimit.cs +++ b/BepuPhysics/Constraints/SwingLimit.cs @@ -113,7 +113,7 @@ static void ComputeJacobian(in Vector3Wide axisLocalA, in Vector3Wide axisLocalB var useFallback = Vector.LessThan(jacobianLengthSquared, new Vector(1e-7f)); Vector3Wide.ConditionalSelect(useFallback, fallbackJacobian, jacobianA, out jacobianA); } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref SwingLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref SwingLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeJacobian(prestep.AxisLocalA, prestep.AxisLocalB, orientationA, orientationB, out _, out _, out var jacobianA); Symmetric3x3Wide.TransformWithoutOverlap(jacobianA, inertiaA.InverseInertiaTensor, out var impulseToVelocityA); @@ -121,7 +121,7 @@ public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, ApplyImpulse(impulseToVelocityA, negatedImpulseToVelocityB, accumulatedImpulses, ref wsvA.Angular, ref wsvB.Angular); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref SwingLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref SwingLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { //The swing limit attempts to keep an axis on body A within from an axis on body B. In other words, this is the same as a hinge joint, but with one fewer DOF. //(Note that the jacobians are extremely similar to the AngularSwivelHinge; the difference is that this is a speculative inequality constraint.) diff --git a/BepuPhysics/Constraints/SwivelHinge.cs b/BepuPhysics/Constraints/SwivelHinge.cs index 278f7750f..c2f0a819d 100644 --- a/BepuPhysics/Constraints/SwivelHinge.cs +++ b/BepuPhysics/Constraints/SwivelHinge.cs @@ -124,14 +124,14 @@ private static void ComputeJacobian(in Vector3Wide localOffsetA, in Vector3Wide swivelHingeJacobian = Vector3Wide.ConditionalSelect(useFallbackJacobian, hingeAxis, swivelHingeJacobian); } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref SwivelHingePrestepData prestep, ref Vector4Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref SwivelHingePrestepData prestep, ref Vector4Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeJacobian(prestep.LocalOffsetA, prestep.LocalSwivelAxisA, prestep.LocalOffsetB, prestep.LocalHingeAxisB, orientationA, orientationB, out _, out _, out var offsetA, out var offsetB, out var swivelHingeJacobian); ApplyImpulse(offsetA, offsetB, swivelHingeJacobian, inertiaA, inertiaB, ref accumulatedImpulses, ref wsvA, ref wsvB); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref SwivelHingePrestepData prestep, ref Vector4Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref SwivelHingePrestepData prestep, ref Vector4Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { //4x12 jacobians, from BallSocket and AngularSwivelHinge: //[ I, skew(offsetA), -I, -skew(offsetB) ] diff --git a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs index e1a1535e5..31f4f640b 100644 --- a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs @@ -25,12 +25,12 @@ public struct ThreeBodyReferences /// Type of the accumulated impulses used by the constraint. public interface IThreeBodyConstraintFunctions { - void WarmStart2( + void WarmStart( in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, ref TPrestepData prestep, ref TAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC); - void Solve2( + void Solve( in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, float dt, float inverseDt, @@ -99,7 +99,7 @@ internal sealed override void VerifySortRegion(ref TypeBatch typeBatch, int bund VerifySortRegion(ref typeBatch, bundleStartIndex, constraintCount, ref sortedKeys, ref sortedSourceIndices); } - public unsafe override void WarmStart2( + public unsafe override void WarmStart( ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { @@ -123,7 +123,7 @@ public unsafe override void WarmStart2(dt), accumulatedImpulses, ref prestep); - function.WarmStart2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC); + function.WarmStart(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC); if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) { @@ -143,7 +143,7 @@ public unsafe override void WarmStart2(); var bodyReferencesBundles = typeBatch.BodyReferences.As(); @@ -159,7 +159,7 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f bodies.GatherState(references.IndexB, true, out var positionB, out var orientationB, out var wsvB, out var inertiaB); bodies.GatherState(references.IndexC, true, out var positionC, out var orientationC, out var wsvC, out var inertiaC); - function.Solve2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, dt, inverseDt, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC); + function.Solve(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, dt, inverseDt, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC); bodies.ScatterVelocities(ref wsvA, ref references.IndexA); bodies.ScatterVelocities(ref wsvB, ref references.IndexB); diff --git a/BepuPhysics/Constraints/TwistLimit.cs b/BepuPhysics/Constraints/TwistLimit.cs index 381935623..005259bae 100644 --- a/BepuPhysics/Constraints/TwistLimit.cs +++ b/BepuPhysics/Constraints/TwistLimit.cs @@ -101,7 +101,7 @@ static void ComputeJacobian(in QuaternionWide orientationA, in QuaternionWide or Vector3Wide.Negate(jacobianA, out var negatedJacobianA); Vector3Wide.ConditionalSelect(useMin, negatedJacobianA, jacobianA, out jacobianA); } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref TwistLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref TwistLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeJacobian(orientationA, orientationB, prestep.LocalBasisA, prestep.LocalBasisB, prestep.MinimumAngle, prestep.MaximumAngle, out _, out var jacobianA); Symmetric3x3Wide.TransformWithoutOverlap(jacobianA, inertiaA.InverseInertiaTensor, out var impulseToVelocityA); @@ -109,7 +109,7 @@ public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, TwistServoFunctions.ApplyImpulse(ref wsvA.Angular, ref wsvB.Angular, impulseToVelocityA, negatedImpulseToVelocityB, accumulatedImpulses); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref TwistLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref TwistLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeJacobian(orientationA, orientationB, prestep.LocalBasisA, prestep.LocalBasisB, prestep.MinimumAngle, prestep.MaximumAngle, out var error, out var jacobianA); diff --git a/BepuPhysics/Constraints/TwistMotor.cs b/BepuPhysics/Constraints/TwistMotor.cs index 67b76ce9e..243872384 100644 --- a/BepuPhysics/Constraints/TwistMotor.cs +++ b/BepuPhysics/Constraints/TwistMotor.cs @@ -87,7 +87,7 @@ public static void ComputeJacobian(in QuaternionWide orientationA, in Quaternion Vector3Wide.ConditionalSelect(Vector.LessThan(length, new Vector(1e-10f)), axisA, jacobianA, out jacobianA); } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref TwistMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref TwistMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeJacobian(orientationA, orientationB, prestep.LocalAxisA, prestep.LocalAxisB, out var jacobianA); Symmetric3x3Wide.TransformWithoutOverlap(jacobianA, inertiaA.InverseInertiaTensor, out var impulseToVelocityA); @@ -95,7 +95,7 @@ public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, TwistServoFunctions.ApplyImpulse(ref wsvA.Angular, ref wsvB.Angular, impulseToVelocityA, negatedImpulseToVelocityB, accumulatedImpulses); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref TwistMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref TwistMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeJacobian(orientationA, orientationB, prestep.LocalAxisA, prestep.LocalAxisB, out var jacobianA); diff --git a/BepuPhysics/Constraints/TwistServo.cs b/BepuPhysics/Constraints/TwistServo.cs index 6c7806ce5..4cac8d4ca 100644 --- a/BepuPhysics/Constraints/TwistServo.cs +++ b/BepuPhysics/Constraints/TwistServo.cs @@ -180,7 +180,7 @@ public static void ComputeJacobian(in QuaternionWide orientationA, in Quaternion Vector3Wide.ConditionalSelect(Vector.LessThan(length, new Vector(1e-10f)), basisAZ, jacobianA, out jacobianA); } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref TwistServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref TwistServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeJacobian(orientationA, orientationB, prestep.LocalBasisA, prestep.LocalBasisB, out var jacobianA); Symmetric3x3Wide.TransformWithoutOverlap(jacobianA, inertiaA.InverseInertiaTensor, out var impulseToVelocityA); @@ -188,7 +188,7 @@ public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, ApplyImpulse(ref wsvA.Angular, ref wsvB.Angular, impulseToVelocityA, negatedImpulseToVelocityB, accumulatedImpulses); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref TwistServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref TwistServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeJacobian(orientationA, orientationB, prestep.LocalBasisA, prestep.LocalBasisB, out var basisBX, out var basisBZ, out var basisA, out var jacobianA); diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 369568939..e7b7b803c 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -28,9 +28,9 @@ public struct TwoBodyReferences /// Type of the accumulated impulses used by the constraint. public interface ITwoBodyConstraintFunctions { - void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, + void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref TPrestepData prestep, ref TAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB); - void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, + void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref TPrestepData prestep, ref TAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB); /// @@ -170,7 +170,7 @@ internal sealed override void VerifySortRegion(ref TypeBatch typeBatch, int bund //only has to specify *type* arguments associated with the interface-implementing struct-delegates. It's going to look very strange, but it's low overhead //and minimizes per-type duplication. - public unsafe override void WarmStart2( + public unsafe override void WarmStart( ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { @@ -191,7 +191,7 @@ public unsafe override void WarmStart2(bodies, ref integratorCallbacks, ref integrationFlags, 1, dt, workerIndex, i, ref references.IndexB, out var positionB, out var orientationB, out var wsvB, out var inertiaB); - function.WarmStart2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB); + function.WarmStart(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB); if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) { @@ -209,7 +209,7 @@ public unsafe override void WarmStart2(); var bodyReferencesBundles = typeBatch.BodyReferences.As(); @@ -226,7 +226,7 @@ public unsafe override void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, f bodies.GatherState(references.IndexA, true, out var positionA, out var orientationA, out var wsvA, out var inertiaA); bodies.GatherState(references.IndexB, true, out var positionB, out var orientationB, out var wsvB, out var inertiaB); - function.Solve2(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, dt, inverseDt, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB); + function.Solve(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, dt, inverseDt, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB); bodies.ScatterVelocities(ref wsvA, ref references.IndexA); bodies.ScatterVelocities(ref wsvB, ref references.IndexB); diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index ef07768d6..ffea14a68 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -201,13 +201,13 @@ internal unsafe abstract void CopySleepingToActive( public abstract void Initialize(ref TypeBatch typeBatch, int initialCapacity, BufferPool pool); public abstract void Resize(ref TypeBatch typeBatch, int newCapacity, BufferPool pool); - public abstract void WarmStart2(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, + public abstract void WarmStart(ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks poseIntegratorCallbacks, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) where TIntegratorCallbacks : struct, IPoseIntegratorCallbacks where TBatchIntegrationMode : unmanaged, IBatchIntegrationMode where TAllowPoseIntegration : unmanaged, IBatchPoseIntegrationAllowed; - public abstract void SolveStep2(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); + public abstract void Solve(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle); /// /// Gets whether this type requires incremental updates for each substep in a frame beyond the first. diff --git a/BepuPhysics/Constraints/VolumeConstraint.cs b/BepuPhysics/Constraints/VolumeConstraint.cs index 5e45daa6c..4c5e1dc8f 100644 --- a/BepuPhysics/Constraints/VolumeConstraint.cs +++ b/BepuPhysics/Constraints/VolumeConstraint.cs @@ -105,14 +105,14 @@ static void ComputeJacobian(in Vector3Wide positionA, in Vector3Wide positionB, Vector3Wide.Add(jacobianB, jacobianC, out negatedJA); Vector3Wide.Add(jacobianD, negatedJA, out negatedJA); } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in Vector3Wide positionD, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, ref VolumeConstraintPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC, ref BodyVelocityWide wsvD) + public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in Vector3Wide positionD, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, ref VolumeConstraintPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC, ref BodyVelocityWide wsvD) { ComputeJacobian(positionA, positionB, positionC, positionD, out var ad, out var negatedJA, out var jacobianB, out var jacobianC, out var jacobianD); //Vector3Wide.Dot(jacobianD, ad, out var unscaledVolume); ApplyImpulse(inertiaA.InverseMass, inertiaB.InverseMass, inertiaC.InverseMass, inertiaD.InverseMass, negatedJA, jacobianB, jacobianC, jacobianD, accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC, ref wsvD); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in Vector3Wide positionD, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, float dt, float inverseDt, ref VolumeConstraintPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC, ref BodyVelocityWide wsvD) + public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in Vector3Wide positionD, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, float dt, float inverseDt, ref VolumeConstraintPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC, ref BodyVelocityWide wsvD) { //Volume of parallelepiped with vertices a, b, c, d is: //(ab x ac) * ad diff --git a/BepuPhysics/Constraints/Weld.cs b/BepuPhysics/Constraints/Weld.cs index c8f0253d1..5c2e39454 100644 --- a/BepuPhysics/Constraints/Weld.cs +++ b/BepuPhysics/Constraints/Weld.cs @@ -113,14 +113,14 @@ private static void ApplyImpulse(in BodyInertiaWide inertiaA, in BodyInertiaWide } //[MethodImpl(MethodImplOptions.NoInlining)] - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, + public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref WeldPrestepData prestep, ref WeldAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { Transform(prestep.LocalOffset, orientationA, out var offset); ApplyImpulse(inertiaA, inertiaB, offset, accumulatedImpulses.Orientation, accumulatedImpulses.Offset, ref wsvA, ref wsvB); } //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, + public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref WeldPrestepData prestep, ref WeldAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { //The weld constraint handles 6 degrees of freedom simultaneously. The constraints are: diff --git a/BepuPhysics/Solver_Solve.cs b/BepuPhysics/Solver_Solve.cs index fd4a8d218..939e116bb 100644 --- a/BepuPhysics/Solver_Solve.cs +++ b/BepuPhysics/Solver_Solve.cs @@ -192,7 +192,7 @@ private void WarmStartBlock(int workerIndex, int bat if (batchIndex == 0) { Buffer noFlagsRequired = default; - typeProcessor.WarmStart2( + typeProcessor.WarmStart( ref typeBatch, ref noFlagsRequired, bodies, ref PoseIntegrator.Callbacks, dt, inverseDt, startBundle, endBundle, workerIndex); } @@ -200,13 +200,13 @@ private void WarmStartBlock(int workerIndex, int bat { if (coarseBatchIntegrationResponsibilities[batchIndex][typeBatchIndex]) { - typeProcessor.WarmStart2( + typeProcessor.WarmStart( ref typeBatch, ref integrationFlags[batchIndex][typeBatchIndex], bodies, ref PoseIntegrator.Callbacks, dt, inverseDt, startBundle, endBundle, workerIndex); } else { - typeProcessor.WarmStart2( + typeProcessor.WarmStart( ref typeBatch, ref integrationFlags[batchIndex][typeBatchIndex], bodies, ref PoseIntegrator.Callbacks, dt, inverseDt, startBundle, endBundle, workerIndex); } @@ -220,7 +220,7 @@ protected interface IStageFunction //Split the solve process into a warmstart and solve, where warmstart doesn't try to store out anything. It just computes jacobians and modifies velocities according to the accumulated impulse. //The solve step then *recomputes* jacobians from prestep data and pose information. //Why? Memory bandwidth. Redoing the calculation is cheaper than storing it out. - struct WarmStartStep2StageFunction : IStageFunction + struct WarmStartStageFunction : IStageFunction { public float Dt; public float InverseDt; @@ -245,7 +245,7 @@ public void Execute(Solver solver, int blockIndex, int workerIndex) } } - struct SolveStep2StageFunction : IStageFunction + struct SolveStageFunction : IStageFunction { public float Dt; public float InverseDt; @@ -257,7 +257,7 @@ public void Execute(Solver solver, int blockIndex, int workerIndex) ref var block = ref this.solver.substepContext.ConstraintBlocks[blockIndex]; ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex]; var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; - typeProcessor.SolveStep2(ref typeBatch, solver.bodies, Dt, InverseDt, block.StartBundle, block.End); + typeProcessor.Solve(ref typeBatch, solver.bodies, Dt, InverseDt, block.StartBundle, block.End); } } @@ -502,13 +502,13 @@ void SolveWorker(int workerIndex) InverseDt = substepContext.InverseDt, solver = this }; - var warmstartStage = new WarmStartStep2StageFunction + var warmstartStage = new WarmStartStageFunction { Dt = substepContext.Dt, InverseDt = substepContext.InverseDt, solver = this }; - var solveStage = new SolveStep2StageFunction + var solveStage = new SolveStageFunction { Dt = substepContext.Dt, InverseDt = substepContext.InverseDt, @@ -582,7 +582,7 @@ void SolveWorker(int workerIndex) for (int j = 0; j < batch.TypeBatches.Count; ++j) { ref var typeBatch = ref batch.TypeBatches[j]; - TypeProcessors[typeBatch.TypeId].SolveStep2(ref typeBatch, bodies, substepContext.Dt, substepContext.InverseDt, 0, typeBatch.BundleCount); + TypeProcessors[typeBatch.TypeId].Solve(ref typeBatch, bodies, substepContext.Dt, substepContext.InverseDt, 0, typeBatch.BundleCount); } } } @@ -1462,7 +1462,7 @@ public override void Solve(float totalDt, IThreadDispatcher threadDispatcher = n for (int j = 0; j < batch.TypeBatches.Count; ++j) { ref var typeBatch = ref batch.TypeBatches[j]; - TypeProcessors[typeBatch.TypeId].SolveStep2(ref typeBatch, bodies, substepDt, inverseDt, 0, typeBatch.BundleCount); + TypeProcessors[typeBatch.TypeId].Solve(ref typeBatch, bodies, substepDt, inverseDt, 0, typeBatch.BundleCount); } } } diff --git a/Demos/Demos/Characters/CharacterMotionConstraint.cs b/Demos/Demos/Characters/CharacterMotionConstraint.cs index 6c148e32a..74e9aa6bf 100644 --- a/Demos/Demos/Characters/CharacterMotionConstraint.cs +++ b/Demos/Demos/Characters/CharacterMotionConstraint.cs @@ -183,7 +183,7 @@ private static void ApplyVerticalImpulse(in Matrix3x3Wide basis, } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref StaticCharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide velocityA) + public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref StaticCharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide velocityA) { ComputeJacobians(prestep.OffsetFromCharacter, prestep.SurfaceBasis, out var basis, out var horizontalAngularJacobianA, out var verticalAngularJacobianA); @@ -191,7 +191,7 @@ public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, ApplyVerticalImpulse(basis, verticalAngularJacobianA, accumulatedImpulses.Vertical, inertiaA, ref velocityA); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref StaticCharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide velocityA) + public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref StaticCharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide velocityA) { //The motion constraint is split into two parts: the horizontal constraint, and the vertical constraint. //The horizontal constraint acts almost exactly like the TangentFriction, but we'll duplicate some of the logic to keep this implementation self-contained. @@ -468,7 +468,7 @@ private static void ApplyVerticalImpulse(in Matrix3x3Wide basis, } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref DynamicCharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB) + public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref DynamicCharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB) { ComputeJacobians(prestep.OffsetFromCharacter, prestep.OffsetFromSupport, prestep.SurfaceBasis, out var basis, out var horizontalAngularJacobianA, out var horizontalAngularJacobianB, out var verticalAngularJacobianA, out var verticalAngularJacobianB); @@ -476,7 +476,7 @@ public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, ApplyVerticalImpulse(basis, verticalAngularJacobianA, verticalAngularJacobianB, accumulatedImpulses.Vertical, inertiaA, inertiaB, ref velocityA, ref velocityB); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref DynamicCharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB) + public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref DynamicCharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB) { //The motion constraint is split into two parts: the horizontal constraint, and the vertical constraint. //The horizontal constraint acts almost exactly like the TangentFriction, but we'll duplicate some of the logic to keep this implementation self-contained. From ad56ad94f78a051af09f7a0540e6a644c44df9f8 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 15 Dec 2021 13:26:00 -0600 Subject: [PATCH 334/947] Old gatherscatter cleanup. --- BepuPhysics/Bodies_GatherScatter.cs | 225 ---------------------------- 1 file changed, 225 deletions(-) diff --git a/BepuPhysics/Bodies_GatherScatter.cs b/BepuPhysics/Bodies_GatherScatter.cs index d048217e6..32f218f56 100644 --- a/BepuPhysics/Bodies_GatherScatter.cs +++ b/BepuPhysics/Bodies_GatherScatter.cs @@ -514,231 +514,6 @@ public unsafe void GatherState(Vector encodedBodyIndices, bo } } - - ////[MethodImpl(MethodImplOptions.AggressiveInlining)] - //unsafe void GatherInertia(ref Vector bodyIndices, int count, int offsetInFloats, out BodyInertiaWide inertia) where TAccessFilter : unmanaged, IBodyAccessFilter - //{ - // var inertias = ActiveSet.SolverStates.Memory; - // if (Avx.IsSupported && Vector.Count == 8) - // { - // Unsafe.SkipInit(out TAccessFilter filter); - // var indices = (int*)Unsafe.AsPointer(ref bodyIndices); - // { - // var s0 = (float*)(inertias + indices[0]) + offsetInFloats; - // var s1 = (float*)(inertias + indices[1]) + offsetInFloats; - // var s2 = (float*)(inertias + indices[2]) + offsetInFloats; - // var s3 = (float*)(inertias + indices[3]) + offsetInFloats; - // var s4 = (float*)(inertias + indices[4]) + offsetInFloats; - // var s5 = (float*)(inertias + indices[5]) + offsetInFloats; - // var s6 = (float*)(inertias + indices[6]) + offsetInFloats; - // var s7 = (float*)(inertias + indices[7]) + offsetInFloats; - - // //Load every inertia vector. - // var m0 = Avx.LoadAlignedVector256(s0); - // var m1 = count > 1 ? Avx.LoadAlignedVector256(s1) : Vector256.Zero; - // var m2 = count > 2 ? Avx.LoadAlignedVector256(s2) : Vector256.Zero; - // var m3 = count > 3 ? Avx.LoadAlignedVector256(s3) : Vector256.Zero; - // var m4 = count > 4 ? Avx.LoadAlignedVector256(s4) : Vector256.Zero; - // var m5 = count > 5 ? Avx.LoadAlignedVector256(s5) : Vector256.Zero; - // var m6 = count > 6 ? Avx.LoadAlignedVector256(s6) : Vector256.Zero; - // var m7 = count > 7 ? Avx.LoadAlignedVector256(s7) : Vector256.Zero; - - // var n0 = Avx.UnpackLow(m0, m1); - // var n1 = Avx.UnpackLow(m2, m3); - // var n2 = Avx.UnpackLow(m4, m5); - // var n3 = Avx.UnpackLow(m6, m7); - // var n4 = Avx.UnpackHigh(m0, m1); - // var n5 = Avx.UnpackHigh(m2, m3); - // var n6 = Avx.UnpackHigh(m4, m5); - // var n7 = Avx.UnpackHigh(m6, m7); - - // var o0 = Avx.Shuffle(n0, n1, 0 | (1 << 2) | (0 << 4) | (1 << 6)); - // var o1 = Avx.Shuffle(n2, n3, 0 | (1 << 2) | (0 << 4) | (1 << 6)); - // var o2 = Avx.Shuffle(n4, n5, 0 | (1 << 2) | (0 << 4) | (1 << 6)); - // var o3 = Avx.Shuffle(n6, n7, 0 | (1 << 2) | (0 << 4) | (1 << 6)); - // var o4 = Avx.Shuffle(n0, n1, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - // var o5 = Avx.Shuffle(n2, n3, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - // var o6 = Avx.Shuffle(n4, n5, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - // var o7 = Avx.Shuffle(n6, n7, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - - // if (filter.GatherInertiaTensor) - // { - // inertia.InverseInertiaTensor.XX = Avx.Permute2x128(o0, o1, 0 | (2 << 4)).AsVector(); - // inertia.InverseInertiaTensor.YX = Avx.Permute2x128(o4, o5, 0 | (2 << 4)).AsVector(); - // inertia.InverseInertiaTensor.YY = Avx.Permute2x128(o2, o3, 0 | (2 << 4)).AsVector(); - // inertia.InverseInertiaTensor.ZX = Avx.Permute2x128(o6, o7, 0 | (2 << 4)).AsVector(); - // inertia.InverseInertiaTensor.ZY = Avx.Permute2x128(o0, o1, 1 | (3 << 4)).AsVector(); - // inertia.InverseInertiaTensor.ZZ = Avx.Permute2x128(o4, o5, 1 | (3 << 4)).AsVector(); - // } - // else - // { - // Unsafe.SkipInit(out inertia.InverseInertiaTensor); - // } - // if (filter.GatherMass) - // { - // inertia.InverseMass = Avx.Permute2x128(o2, o3, 1 | (3 << 4)).AsVector(); - // } - // else - // { - // Unsafe.SkipInit(out inertia.InverseMass); - // } - // } - // } - // else - // { - // Unsafe.SkipInit(out inertia); - // FallbackGatherInertia(count, ActiveSet.SolverStates.Memory, ref bodyIndices, ref inertia, offsetInFloats); - // } - //} - - //[MethodImpl(MethodImplOptions.AggressiveInlining)] - //public unsafe void GatherWorldInertia(ref Vector bodyIndices, int count, out BodyInertiaWide inertia) where TAccessFilter : unmanaged, IBodyAccessFilter - //{ - // GatherInertia(ref bodyIndices, count, 24, out inertia); - //} - //[MethodImpl(MethodImplOptions.AggressiveInlining)] - //public unsafe void GatherLocalInertia(ref Vector bodyIndices, int count, out BodyInertiaWide inertia) where TAccessFilter : unmanaged, IBodyAccessFilter - //{ - // GatherInertia(ref bodyIndices, count, 16, out inertia); - //} - - /// - /// Gathers orientations and relative positions for a two body bundle into an AOSOA bundle. - /// - /// Active body indices being gathered. - /// Number of body pairs in the bundle. - /// Gathered offsets from body A to body B. - /// Gathered orientation of body A. - /// Gathered orientation of body B. - /// Gathered velocity of body A. - /// Gathered velocity of body B. - /// Gathered inertia of body A. - /// Gathered inertia of body B. - //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void GatherState(ref TwoBodyReferences references, - out QuaternionWide orientationA, out BodyVelocityWide velocityA, out BodyInertiaWide inertiaA, - out Vector3Wide ab, - out QuaternionWide orientationB, out BodyVelocityWide velocityB, out BodyInertiaWide inertiaB) - { - ref var states = ref ActiveSet.SolverStates; - - GatherState(references.IndexA, true, out var positionA, out orientationA, out velocityA, out inertiaA); - GatherState(references.IndexB, true, out var positionB, out orientationB, out velocityB, out inertiaB); - - ////Grab the base references for the body indices. Note that we make use of the references memory layout again. - //ref var baseIndexA = ref Unsafe.As, int>(ref references.IndexA); - //ref var baseIndexB = ref Unsafe.As, int>(ref references.IndexB); - //for (int i = 0; i < count; ++i) - //{ - // //WriteGatherState(ref baseIndexA, i, ref states, ref positionA, ref orientationA, ref velocityA); - // //WriteGatherInertia(ref baseIndexA, i, ref Inertias, ref inertiaA); - // //WriteGatherState(ref baseIndexB, i, ref states, ref positionB, ref orientationB, ref velocityB); - // //WriteGatherInertia(ref baseIndexB, i, ref Inertias, ref inertiaB); - // WriteGatherState(ref baseIndexA, i, ref states, ref positionA, ref orientationA, ref velocityA, ref inertiaA); - // WriteGatherState(ref baseIndexB, i, ref states, ref positionB, ref orientationB, ref velocityB, ref inertiaB); - //} - - //TODO: In future versions, we will likely store the body position in different forms to allow for extremely large worlds. - //That will be an opt-in feature. The default implementation will use the FP32 representation, but the user could choose to swap it out for a fp64 or fixed64 representation. - //This affects other systems- AABB calculation, pose integration, solving, and in extreme (64 bit) cases, the broadphase. - //We want to insulate other systems from direct knowledge about the implementation of positions when possible. - //These functions support the solver's needs while hiding absolute positions. - //In order to support other absolute positions, we'll need alternate implementations of this and other functions. - //But for the most part, we don't want to pay the overhead of an abstract invocation within the inner loop of the solver. - //Given the current limits of C# and the compiler, the best option seems to be conditional compilation. - Vector3Wide.Subtract(positionB, positionA, out ab); - } - - - /// - /// Gathers orientations and relative positions for a three body bundle into an AOSOA bundle. - /// - /// Active body indices being gathered. - /// Number of body pairs in the bundle. - /// Gathered offsets from body A to body B. - /// Gathered offsets from body A to body C. - /// Gathered orientation of body A. - /// Gathered orientation of body B. - /// Gathered orientation of body C. - /// Gathered velocity of body A. - /// Gathered velocity of body B. - /// Gathered velocity of body C. - /// Gathered inertia of body A. - /// Gathered inertia of body B. - /// Gathered inertia of body C. - //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public void GatherState(ref ThreeBodyReferences references, - out QuaternionWide orientationA, out BodyVelocityWide velocityA, out BodyInertiaWide inertiaA, - out Vector3Wide ab, - out QuaternionWide orientationB, out BodyVelocityWide velocityB, out BodyInertiaWide inertiaB, - out Vector3Wide ac, - out QuaternionWide orientationC, out BodyVelocityWide velocityC, out BodyInertiaWide inertiaC) - { - - GatherState(references.IndexA, true, out var positionA, out orientationA, out velocityA, out inertiaA); - GatherState(references.IndexB, true, out var positionB, out orientationB, out velocityB, out inertiaB); - GatherState(references.IndexC, true, out var positionC, out orientationC, out velocityC, out inertiaC); - - //TODO: In future versions, we will likely store the body position in different forms to allow for extremely large worlds. - //That will be an opt-in feature. The default implementation will use the FP32 representation, but the user could choose to swap it out for a fp64 or fixed64 representation. - //This affects other systems- AABB calculation, pose integration, solving, and in extreme (64 bit) cases, the broadphase. - //We want to insulate other systems from direct knowledge about the implementation of positions when possible. - //These functions support the solver's needs while hiding absolute positions. - //In order to support other absolute positions, we'll need alternate implementations of this and other functions. - //But for the most part, we don't want to pay the overhead of an abstract invocation within the inner loop of the solver. - //Given the current limits of C# and the compiler, the best option seems to be conditional compilation. - Vector3Wide.Subtract(positionB, positionA, out ab); - Vector3Wide.Subtract(positionC, positionA, out ac); - } - - /// - /// Gathers orientations and relative positions for a four body bundle into an AOSOA bundle. - /// - /// Active body indices being gathered. - /// Number of body pairs in the bundle. - /// Gathered offsets from body A to body B. - /// Gathered offsets from body A to body C. - /// Gathered offsets from body A to body D. - /// Gathered orientation of body A. - /// Gathered orientation of body B. - /// Gathered orientation of body C. - /// Gathered orientation of body D. - /// Gathered velocity of body A. - /// Gathered velocity of body B. - /// Gathered velocity of body C. - /// Gathered velocity of body D. - /// Gathered inertia of body A. - /// Gathered inertia of body B. - /// Gathered inertia of body C. - /// Gathered inertia of body C. - //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public void GatherState(ref FourBodyReferences references, - out QuaternionWide orientationA, out BodyVelocityWide velocityA, out BodyInertiaWide inertiaA, - out Vector3Wide ab, - out QuaternionWide orientationB, out BodyVelocityWide velocityB, out BodyInertiaWide inertiaB, - out Vector3Wide ac, - out QuaternionWide orientationC, out BodyVelocityWide velocityC, out BodyInertiaWide inertiaC, - out Vector3Wide ad, - out QuaternionWide orientationD, out BodyVelocityWide velocityD, out BodyInertiaWide inertiaD) - { - - GatherState(references.IndexA, true, out var positionA, out orientationA, out velocityA, out inertiaA); - GatherState(references.IndexB, true, out var positionB, out orientationB, out velocityB, out inertiaB); - GatherState(references.IndexC, true, out var positionC, out orientationC, out velocityC, out inertiaC); - GatherState(references.IndexD, true, out var positionD, out orientationD, out velocityD, out inertiaD); - //TODO: In future versions, we will likely store the body position in different forms to allow for extremely large worlds. - //That will be an opt-in feature. The default implementation will use the FP32 representation, but the user could choose to swap it out for a fp64 or fixed64 representation. - //This affects other systems- AABB calculation, pose integration, solving, and in extreme (64 bit) cases, the broadphase. - //We want to insulate other systems from direct knowledge about the implementation of positions when possible. - //These functions support the solver's needs while hiding absolute positions. - //In order to support other absolute positions, we'll need alternate implementations of this and other functions. - //But for the most part, we don't want to pay the overhead of an abstract invocation within the inner loop of the solver. - //Given the current limits of C# and the compiler, the best option seems to be conditional compilation. - Vector3Wide.Subtract(positionB, positionA, out ab); - Vector3Wide.Subtract(positionC, positionA, out ac); - Vector3Wide.Subtract(positionD, positionA, out ad); - } - //Note that ScatterPose and ScatterInertia do not need to check body references for empty lanes or for kinematicity. //The mask is only set for lanes which were subject to constraint integration responsibility; empty lanes and kinematics cannot be integrated by constraints. From b691776751322625c5ad29efb3dda2f67e939f9c Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 15 Dec 2021 16:06:45 -0600 Subject: [PATCH 335/947] Fixed a validation bug in Statics. Filled out fallbacks for gatherscatter. --- BepuPhysics/Bodies_GatherScatter.cs | 72 ++++++++++++----------------- BepuPhysics/Statics.cs | 2 +- Demos/Program.cs | 2 +- 3 files changed, 32 insertions(+), 44 deletions(-) diff --git a/BepuPhysics/Bodies_GatherScatter.cs b/BepuPhysics/Bodies_GatherScatter.cs index 32f218f56..bc7ce6b91 100644 --- a/BepuPhysics/Bodies_GatherScatter.cs +++ b/BepuPhysics/Bodies_GatherScatter.cs @@ -41,43 +41,10 @@ private static void WriteGatherMotionState(int index, int bodyIndexInBundle, ref Vector3Wide.WriteFirst(state.Velocity.Angular, ref GetOffsetInstance(ref velocity.Angular, bodyIndexInBundle)); } - /// - /// Gathers motion state information for a body bundle into an AOSOA bundle. - /// - /// Active body indices being gathered. - /// Gathered absolute position of the body. - /// Gathered orientation of the body. - /// Gathered velocity of the body. - /// Gathered inertia of the body. - //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public void GatherState(ref Vector references, - out Vector3Wide position, out QuaternionWide orientation, out BodyVelocityWide velocity, out BodyInertiaWide inertia) - { - Unsafe.SkipInit(out position); - Unsafe.SkipInit(out orientation); - Unsafe.SkipInit(out velocity); - Unsafe.SkipInit(out inertia); - //TODO: This function and its users (which should be relatively few) is a problem for large world position precision. - //It directly reports the position, thereby infecting vectorized logic with the high precision representation. - //You might be able to redesign the users of this function to not need it, but that comes with its own difficulties - //(for example, making the grab motor rely on having its goal offset updated every frame by the user). - //Grab the base references for the body indices. Note that we make use of the references memory layout again. - ref var baseIndex = ref Unsafe.As, int>(ref references); - - for (int i = 0; i < Vector.Count; ++i) - { - var index = Unsafe.Add(ref baseIndex, i); - if (index < 0) - continue; - WriteGatherMotionState(index, i, ref ActiveSet.SolverStates, ref position, ref orientation, ref velocity); - WriteGatherInertia(index, i, ref ActiveSet.SolverStates, ref inertia); - } - } [MethodImpl(MethodImplOptions.AggressiveInlining)] - unsafe static void FallbackGatherMotionState(SolverState* states, Vector baseIndex, ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocityWide velocity) + unsafe static void FallbackGatherMotionState(SolverState* states, Vector encodedBodyIndices, ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocityWide velocity) { - var indices = (int*)Unsafe.AsPointer(ref baseIndex); var pPositionX = (float*)Unsafe.AsPointer(ref position.X); var pPositionY = (float*)Unsafe.AsPointer(ref position.Y); var pPositionZ = (float*)Unsafe.AsPointer(ref position.Z); @@ -94,10 +61,10 @@ unsafe static void FallbackGatherMotionState(SolverState* states, Vector ba for (int i = 0; i < Vector.Count; ++i) { - var index = indices[i]; - if (index < 0) + var encodedBodyIndex = encodedBodyIndices[i]; + if (encodedBodyIndex < 0) continue; - var stateValues = (float*)(states + index); + var stateValues = (float*)(states + (encodedBodyIndex & BodyReferenceMask)); pPositionX[i] = stateValues[MotionState.OffsetToPositionX]; pPositionY[i] = stateValues[MotionState.OffsetToPositionY]; pPositionZ[i] = stateValues[MotionState.OffsetToPositionZ]; @@ -114,9 +81,8 @@ unsafe static void FallbackGatherMotionState(SolverState* states, Vector ba } } [MethodImpl(MethodImplOptions.AggressiveInlining)] - unsafe static void FallbackGatherInertia(SolverState* states, Vector baseIndex, ref BodyInertiaWide inertia, int offsetInFloats) + unsafe static void FallbackGatherInertia(SolverState* states, Vector encodedBodyIndices, ref BodyInertiaWide inertia, int offsetInFloats) { - var indices = (int*)Unsafe.AsPointer(ref baseIndex); var pMass = (float*)Unsafe.AsPointer(ref inertia.InverseMass); var pInertiaXX = (float*)Unsafe.AsPointer(ref inertia.InverseInertiaTensor.XX); var pInertiaYX = (float*)Unsafe.AsPointer(ref inertia.InverseInertiaTensor.YX); @@ -127,10 +93,10 @@ unsafe static void FallbackGatherInertia(SolverState* states, Vector baseIn for (int i = 0; i < Vector.Count; ++i) { - var index = indices[i]; - if (index < 0) + var encodedBodyIndex = encodedBodyIndices[i]; + if (encodedBodyIndex < 0) continue; - var inertiaValues = (float*)(states + index) + offsetInFloats; + var inertiaValues = (float*)(states + (encodedBodyIndex & BodyReferenceMask)) + offsetInFloats; pInertiaXX[i] = inertiaValues[0]; pInertiaYX[i] = inertiaValues[1]; pInertiaYY[i] = inertiaValues[2]; @@ -572,6 +538,15 @@ public unsafe void ScatterPose( } else { + for (int innerIndex = 0; innerIndex < Vector.Count; ++innerIndex) + { + if (mask[innerIndex] == 0) + continue; + ref var pose = ref ActiveSet.SolverStates[encodedBodyIndices[innerIndex]].Motion.Pose; + pose.Position = new Vector3(position.X[innerIndex], position.Y[innerIndex], position.Z[innerIndex]); + pose.Orientation = new Quaternion(orientation.X[innerIndex], orientation.Y[innerIndex], orientation.Z[innerIndex], orientation.W[innerIndex]); + + } } } @@ -633,6 +608,19 @@ public unsafe void ScatterInertia( } else { + for (int innerIndex = 0; innerIndex < Vector.Count; ++innerIndex) + { + if (mask[innerIndex] == 0) + continue; + ref var target = ref ActiveSet.SolverStates[encodedBodyIndices[innerIndex]].Inertia.World; + target.InverseInertiaTensor.XX = inertia.InverseInertiaTensor.XX[innerIndex]; + target.InverseInertiaTensor.YX = inertia.InverseInertiaTensor.YX[innerIndex]; + target.InverseInertiaTensor.YY = inertia.InverseInertiaTensor.YY[innerIndex]; + target.InverseInertiaTensor.ZX = inertia.InverseInertiaTensor.ZX[innerIndex]; + target.InverseInertiaTensor.ZY = inertia.InverseInertiaTensor.ZY[innerIndex]; + target.InverseInertiaTensor.ZZ = inertia.InverseInertiaTensor.ZZ[innerIndex]; + target.InverseMass = inertia.InverseMass[innerIndex]; + } } } diff --git a/BepuPhysics/Statics.cs b/BepuPhysics/Statics.cs index 512a15b75..d81cb73e1 100644 --- a/BepuPhysics/Statics.cs +++ b/BepuPhysics/Statics.cs @@ -308,7 +308,7 @@ public void RemoveAt(int index, ref TAwakeningFilter filter) w { var movedStaticOriginalIndex = Count; //Copy the memory state of the last element down. - this[index] = this[movedStaticOriginalIndex]; + this[index] = StaticsBuffer[movedStaticOriginalIndex]; //Point the static handles at the new location. var lastHandle = IndexToHandle[movedStaticOriginalIndex]; HandleToIndex[lastHandle.Value] = index; diff --git a/Demos/Program.cs b/Demos/Program.cs index 3bfce3982..25be47890 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -24,7 +24,7 @@ static void Main(string[] args) //HeadlessTest.Test(content, 4, 32, 512); //HeadlessTest.Test(content, 4, 32, 512); //HeadlessTest.Test(content, 4, 128, 1024); - //DeterminismTest.Test(content, 1, 20000); + //DeterminismTest.Test(content, 1, 32768); //InvasiveHashDiagnostics.Initialize(5, 192000); var demo = new DemoHarness(loop, content); loop.Run(demo); From 0bc0c4b8b8b5f6629bb7f2765d7d7626fb761169 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 15 Dec 2021 17:32:09 -0600 Subject: [PATCH 336/947] Updating lane masking to intrinsified versions. --- .../CollisionTasks/BoxConvexHullTester.cs | 2 +- .../CollisionTasks/BoxCylinderTester.cs | 2 +- .../CollisionTasks/BoxPairTester.cs | 2 +- .../CollisionTasks/BoxTriangleTester.cs | 2 +- .../CollisionTasks/CapsuleConvexHullTester.cs | 2 +- .../CollisionTasks/CapsuleCylinderTester.cs | 2 +- .../CollisionTasks/CapsuleTriangleTester.cs | 2 +- .../CollisionTasks/ConvexHullPairTester.cs | 2 +- .../CylinderConvexHullTester.cs | 2 +- .../CollisionTasks/CylinderPairTester.cs | 2 +- .../CollisionTasks/ManifoldCandidateHelper.cs | 21 ------------------- .../CollisionTasks/SphereConvexHullTester.cs | 2 +- .../CollisionTasks/SphereTriangleTester.cs | 2 +- .../TriangleConvexHullTester.cs | 3 +-- .../CollisionTasks/TriangleCylinderTester.cs | 2 +- .../CollisionTasks/TrianglePairTester.cs | 2 +- 16 files changed, 15 insertions(+), 37 deletions(-) diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/BoxConvexHullTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/BoxConvexHullTester.cs index 349d99aa4..0ebbab7df 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/BoxConvexHullTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/BoxConvexHullTester.cs @@ -28,7 +28,7 @@ public unsafe void Test(ref BoxWide a, ref ConvexHullWide b, ref Vector s initialNormal.Z = Vector.ConditionalSelect(useInitialFallback, Vector.Zero, initialNormal.Z); var hullSupportFinder = default(ConvexHullSupportFinder); var boxSupportFinder = default(BoxSupportFinder); - ManifoldCandidateHelper.CreateInactiveMask(pairCount, out var inactiveLanes); + var inactiveLanes = BundleIndexing.CreateTrailingMaskForCountInBundle(pairCount); b.EstimateEpsilonScale(inactiveLanes, out var hullEpsilonScale); var epsilonScale = Vector.Min(Vector.Max(a.HalfWidth, Vector.Max(a.HalfHeight, a.HalfLength)), hullEpsilonScale); var depthThreshold = -speculativeMargin; diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/BoxCylinderTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/BoxCylinderTester.cs index 4cefb5331..66903c6d5 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/BoxCylinderTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/BoxCylinderTester.cs @@ -123,7 +123,7 @@ public unsafe void Test( CylinderSupportFinder cylinderSupportFinder = default; //We now have a decent estimate for the local normal and an initial simplex to work from. Refine it to a local minimum. - ManifoldCandidateHelper.CreateInactiveMask(pairCount, out var inactiveLanes); + var inactiveLanes = BundleIndexing.CreateTrailingMaskForCountInBundle(pairCount); var depthThreshold = -speculativeMargin; var epsilonScale = Vector.Min(Vector.Max(a.HalfWidth, Vector.Max(a.HalfHeight, a.HalfLength)), Vector.Max(b.HalfLength, b.Radius)); diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/BoxPairTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/BoxPairTester.cs index 15377bac2..38f1ca3a1 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/BoxPairTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/BoxPairTester.cs @@ -382,7 +382,7 @@ public unsafe void Test( var faceBZDepth = b.HalfLength + a.HalfWidth * absRBZ.X + a.HalfHeight * absRBZ.Y + a.HalfLength * absRBZ.Z - Vector.Abs(bLocalOffsetB.Z); Select(ref depth, ref localNormal, ref faceBZDepth, ref rB.Z.X, ref rB.Z.Y, ref rB.Z.Z); - ManifoldCandidateHelper.CreateActiveMask(pairCount, out var activeLanes); + var activeLanes = BundleIndexing.CreateMaskForCountInBundle(pairCount); var minimumDepth = -speculativeMargin; var allowContacts = Vector.BitwiseAnd(activeLanes, Vector.GreaterThanOrEqual(depth, minimumDepth)); if (Vector.EqualsAll(allowContacts, Vector.Zero)) diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/BoxTriangleTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/BoxTriangleTester.cs index 2c5103b21..98a68a021 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/BoxTriangleTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/BoxTriangleTester.cs @@ -333,7 +333,7 @@ public unsafe void Test( Vector.Abs(triangleNormal.X) * a.HalfWidth + Vector.Abs(triangleNormal.Y) * a.HalfHeight + Vector.Abs(triangleNormal.Z) * a.HalfLength - Vector.Abs(trianglePlaneOffset); Select(ref depth, ref localNormal, triangleFaceDepth, calibratedTriangleNormal); - ManifoldCandidateHelper.CreateActiveMask(pairCount, out var activeLanes); + var activeLanes = BundleIndexing.CreateMaskForCountInBundle(pairCount); //The following was created for MeshReduction when it demanded all contact normals be correct during separation. //Other pairs don't have that requirement, and we ended modifying MeshReduction to be a little less picky. //This remains for posterity because, hey, it works, and if you need it, there it is. diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleConvexHullTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleConvexHullTester.cs index d736224fe..51149b3a2 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleConvexHullTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleConvexHullTester.cs @@ -28,7 +28,7 @@ public void Test(ref CapsuleWide a, ref ConvexHullWide b, ref Vector spec initialNormal.Z = Vector.ConditionalSelect(useInitialFallback, Vector.Zero, initialNormal.Z); var hullSupportFinder = default(ConvexHullSupportFinder); var capsuleSupportFinder = default(CapsuleSupportFinder); - ManifoldCandidateHelper.CreateInactiveMask(pairCount, out var inactiveLanes); + var inactiveLanes = BundleIndexing.CreateTrailingMaskForCountInBundle(pairCount); b.EstimateEpsilonScale(inactiveLanes, out var hullEpsilonScale); var epsilonScale = Vector.Min(a.Radius, hullEpsilonScale); var depthThreshold = -speculativeMargin; diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleCylinderTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleCylinderTester.cs index 882342957..dfc2061dd 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleCylinderTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleCylinderTester.cs @@ -159,7 +159,7 @@ public void Test(ref CapsuleWide a, ref CylinderWide b, ref Vector specul Matrix3x3Wide.TransformByTransposedWithoutOverlap(offsetB, worldRB, out var localOffsetB); Vector3Wide.Negate(localOffsetB, out var localOffsetA); - ManifoldCandidateHelper.CreateInactiveMask(pairCount, out var inactiveLanes); + var inactiveLanes = BundleIndexing.CreateTrailingMaskForCountInBundle(pairCount); GetClosestPointBetweenLineSegmentAndCylinder(localOffsetA, capsuleAxis, a.HalfLength, b, inactiveLanes, out var t, out var localNormal); Vector3Wide.LengthSquared(localNormal, out var distanceFromCylinderToLineSegmentSquared); var internalLineSegmentIntersected = Vector.LessThan(distanceFromCylinderToLineSegmentSquared, new Vector(1e-12f)); diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleTriangleTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleTriangleTester.cs index 3e1d139fb..e120c16e9 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleTriangleTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleTriangleTester.cs @@ -193,7 +193,7 @@ public void Test( Vector3Wide.ConditionalSelect(useEdge, edgeNormal, faceNormal, out var localNormal); Vector3Wide.Dot(localNormal, faceNormal, out var localNormalDotFaceNormal); var collidingWithSolidSide = Vector.GreaterThanOrEqual(localNormalDotFaceNormal, new Vector(TriangleWide.BackfaceNormalDotRejectionThreshold)); - ManifoldCandidateHelper.CreateActiveMask(pairCount, out var activeLanes); + var activeLanes = BundleIndexing.CreateMaskForCountInBundle(pairCount); TriangleWide.ComputeNondegenerateTriangleMask(ab, ac, faceNormalLength, out _, out var nondegenerateMask); var negativeMargin = -speculativeMargin; var allowContacts = Vector.BitwiseAnd(Vector.BitwiseAnd(Vector.GreaterThanOrEqual(depth + a.Radius, negativeMargin), activeLanes), Vector.BitwiseAnd(collidingWithSolidSide, nondegenerateMask)); diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexHullPairTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexHullPairTester.cs index 02f83d997..2b8bce7f1 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexHullPairTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexHullPairTester.cs @@ -33,7 +33,7 @@ public unsafe void Test(ref ConvexHullWide a, ref ConvexHullWide b, ref Vector.One, initialNormal.Y); initialNormal.Z = Vector.ConditionalSelect(useInitialFallback, Vector.Zero, initialNormal.Z); var hullSupportFinder = default(ConvexHullSupportFinder); - ManifoldCandidateHelper.CreateInactiveMask(pairCount, out var inactiveLanes); + var inactiveLanes = BundleIndexing.CreateTrailingMaskForCountInBundle(pairCount); a.EstimateEpsilonScale(inactiveLanes, out var aEpsilonScale); b.EstimateEpsilonScale(inactiveLanes, out var bEpsilonScale); var epsilonScale = Vector.Min(aEpsilonScale, bEpsilonScale); diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CylinderConvexHullTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CylinderConvexHullTester.cs index 368f3ad17..12afc4e92 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CylinderConvexHullTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CylinderConvexHullTester.cs @@ -98,7 +98,7 @@ public unsafe void Test(ref CylinderWide a, ref ConvexHullWide b, ref Vector.Zero, initialNormal.Z); var hullSupportFinder = default(ConvexHullSupportFinder); var cylinderSupportFinder = default(CylinderSupportFinder); - ManifoldCandidateHelper.CreateInactiveMask(pairCount, out var inactiveLanes); + var inactiveLanes = BundleIndexing.CreateTrailingMaskForCountInBundle(pairCount); b.EstimateEpsilonScale(inactiveLanes, out var hullEpsilonScale); var epsilonScale = Vector.Min(Vector.Max(a.HalfLength, a.Radius), hullEpsilonScale); var depthThreshold = -speculativeMargin; diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CylinderPairTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CylinderPairTester.cs index e1e3e8c0d..4f0ea3463 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CylinderPairTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CylinderPairTester.cs @@ -152,7 +152,7 @@ public void Test( //Blindly trying a bunch of low probability feature pairs- especially those which would fall into the above- isn't very wise. //We now have a decent estimate for the local normal and an initial simplex to work from. Refine it to a local minimum. - ManifoldCandidateHelper.CreateInactiveMask(pairCount, out var inactiveLanes); + var inactiveLanes = BundleIndexing.CreateTrailingMaskForCountInBundle(pairCount); var depthThreshold = -speculativeMargin; var epsilonScale = Vector.Min(Vector.Max(a.HalfLength, a.Radius), Vector.Max(b.HalfLength, b.Radius)); diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/ManifoldCandidateHelper.cs b/BepuPhysics/CollisionDetection/CollisionTasks/ManifoldCandidateHelper.cs index b9a1cd45a..5ddc41171 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/ManifoldCandidateHelper.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/ManifoldCandidateHelper.cs @@ -24,27 +24,6 @@ public struct ManifoldCandidate } public static class ManifoldCandidateHelper { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void CreateInactiveMask(int pairCount, out Vector inactiveLanes) - { - inactiveLanes = Vector.Zero; - ref var laneMasks = ref Unsafe.As, int>(ref inactiveLanes); - for (int i = Vector.Count - 1; i >= pairCount; --i) - { - Unsafe.Add(ref laneMasks, i) = -1; - } - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void CreateActiveMask(int pairCount, out Vector activeLanes) - { - activeLanes = Vector.Zero; - ref var laneMasks = ref Unsafe.As, int>(ref activeLanes); - for (int i = 0; i < pairCount; ++i) - { - Unsafe.Add(ref laneMasks, i) = -1; - } - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void AddCandidate(ref ManifoldCandidate candidates, ref Vector count, in ManifoldCandidate candidate, in Vector newContactExists, int pairCount) { diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/SphereConvexHullTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/SphereConvexHullTester.cs index 4f7628e1b..da9acb59a 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/SphereConvexHullTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/SphereConvexHullTester.cs @@ -29,7 +29,7 @@ public void Test(ref SphereWide a, ref ConvexHullWide b, ref Vector specu initialNormal.Z = Vector.ConditionalSelect(useInitialFallback, Vector.Zero, initialNormal.Z); var hullSupportFinder = default(ConvexHullSupportFinder); var sphereSupportFinder = default(SphereSupportFinder); - ManifoldCandidateHelper.CreateInactiveMask(pairCount, out var inactiveLanes); + var inactiveLanes = BundleIndexing.CreateTrailingMaskForCountInBundle(pairCount); b.EstimateEpsilonScale(inactiveLanes, out var hullEpsilonScale); var epsilonScale = Vector.Min(a.Radius, hullEpsilonScale); DepthRefiner.FindMinimumDepth( diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/SphereTriangleTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/SphereTriangleTester.cs index f4b8a0bc5..e88df66d9 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/SphereTriangleTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/SphereTriangleTester.cs @@ -67,7 +67,7 @@ public void Test(ref SphereWide a, ref TriangleWide b, ref Vector specula var outsideAnyEdge = Vector.BitwiseOr(outsideAB, Vector.BitwiseOr(outsideAC, outsideBC)); Unsafe.SkipInit(out Vector3Wide localClosestOnTriangle); var negativeOne = new Vector(-1); - ManifoldCandidateHelper.CreateActiveMask(pairCount, out var activeLanes); + var activeLanes = BundleIndexing.CreateMaskForCountInBundle(pairCount); if (Vector.EqualsAny(Vector.BitwiseAnd(activeLanes, outsideAnyEdge), negativeOne)) { //At least one lane detected a point outside of the triangle. Choose one edge which is outside as the representative. diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/TriangleConvexHullTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/TriangleConvexHullTester.cs index eb6b4f7bf..1dc6ac991 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/TriangleConvexHullTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/TriangleConvexHullTester.cs @@ -59,8 +59,7 @@ public unsafe void Test(ref TriangleWide a, ref ConvexHullWide b, ref Vector.Zero), Vector.LessThanOrEqual(caPlaneTest, Vector.Zero))); var hullInsideAndBelowTriangle = Vector.BitwiseAnd(hullBelowPlane, hullInsideTriangleEdgePlanes); - - ManifoldCandidateHelper.CreateInactiveMask(pairCount, out var inactiveLanes); + var inactiveLanes = BundleIndexing.CreateTrailingMaskForCountInBundle(pairCount); TriangleWide.ComputeNondegenerateTriangleMask(triangleAB, triangleCA, triangleNormalLength, out var triangleEpsilonScale, out var nondegenerateMask); b.EstimateEpsilonScale(inactiveLanes, out var hullEpsilonScale); var epsilonScale = Vector.Min(triangleEpsilonScale, hullEpsilonScale); diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/TriangleCylinderTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/TriangleCylinderTester.cs index 3876659c7..6d2234d7c 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/TriangleCylinderTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/TriangleCylinderTester.cs @@ -160,7 +160,7 @@ public unsafe void Test( Vector.LessThanOrEqual(caPlaneTest, Vector.Zero))); var cylinderInsideAndBelowTriangle = Vector.BitwiseAnd(cylinderInsideTriangleEdgePlanes, cylinderBelowPlane); - ManifoldCandidateHelper.CreateInactiveMask(pairCount, out var inactiveLanes); + var inactiveLanes = BundleIndexing.CreateTrailingMaskForCountInBundle(pairCount); TriangleWide.ComputeNondegenerateTriangleMask(triangleAB, triangleCA, triangleNormalLength, out var triangleEpsilonScale, out var nondegenerateMask); inactiveLanes = Vector.BitwiseOr(Vector.OnesComplement(nondegenerateMask), inactiveLanes); inactiveLanes = Vector.BitwiseOr(cylinderInsideAndBelowTriangle, inactiveLanes); diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs index 9a073430c..a6b13349a 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs @@ -331,7 +331,7 @@ public unsafe void Test( Vector3Wide.LengthSquared(abB, out var abBLengthSquared); Vector3Wide.LengthSquared(caA, out var caALengthSquared); Vector3Wide.LengthSquared(caB, out var caBLengthSquared); - ManifoldCandidateHelper.CreateActiveMask(pairCount, out var allowContacts); + var allowContacts = BundleIndexing.CreateMaskForCountInBundle(pairCount); //The following was created for MeshReduction when it demanded all contact normals be correct during separation. //Other pairs don't have that requirement, and we ended modifying MeshReduction to be a little less picky. //This remains for posterity because, hey, it works, and if you need it, there it is. From 121b7db73f3bd2f7ed012e02093082518cb4d1f4 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 15 Dec 2021 18:49:02 -0600 Subject: [PATCH 337/947] Bumped version number. --- BepuPhysics/BepuPhysics.csproj | 2 +- BepuUtilities/BepuUtilities.csproj | 2 +- Demos/Demos.csproj | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/BepuPhysics/BepuPhysics.csproj b/BepuPhysics/BepuPhysics.csproj index 396beef48..b5593c2ea 100644 --- a/BepuPhysics/BepuPhysics.csproj +++ b/BepuPhysics/BepuPhysics.csproj @@ -1,7 +1,7 @@  net5.0 - 2.4.0-beta8 + 2.4.0-beta9 Bepu Entertainment LLC Ross Nordby Speedy real time physics simulation library. diff --git a/BepuUtilities/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj index fd1cefeaa..efe5660a4 100644 --- a/BepuUtilities/BepuUtilities.csproj +++ b/BepuUtilities/BepuUtilities.csproj @@ -3,7 +3,7 @@ BepuUtilities BepuUtilities net5.0 - 2.4.0-beta8 + 2.4.0-beta9 Bepu Entertainment LLC Ross Nordby Supporting utilities library for BEPUphysics v2. diff --git a/Demos/Demos.csproj b/Demos/Demos.csproj index 6752aa2a3..70b17f982 100644 --- a/Demos/Demos.csproj +++ b/Demos/Demos.csproj @@ -5,7 +5,7 @@ True Debug;Release latest - false + From 18a7cf092e6e8c6384e3050c6023a4b4b17aa94c Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 15 Dec 2021 19:03:29 -0600 Subject: [PATCH 338/947] Update dotnet-core.yml --- .github/workflows/dotnet-core.yml | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/.github/workflows/dotnet-core.yml b/.github/workflows/dotnet-core.yml index 3bd6cdd6b..e73b2c743 100644 --- a/.github/workflows/dotnet-core.yml +++ b/.github/workflows/dotnet-core.yml @@ -12,7 +12,10 @@ jobs: runs-on: windows-latest steps: - - uses: actions/checkout@v2 + - uses: actions/checkout@v2 + - uses: actions/setup-dotnet@v1 + with: + dotnet-version: 6.x - name: Install dependencies run: | dotnet restore DemoContentBuilder From 9f3f36f44f552ddd229b2d4d31b1b236d17d8cd0 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 15 Dec 2021 19:08:15 -0600 Subject: [PATCH 339/947] As is tradition, fixed workflow error --- .github/workflows/dotnet-core.yml | 4 +++- BepuPhysics/BepuPhysics.csproj | 2 +- BepuUtilities/BepuUtilities.csproj | 2 +- 3 files changed, 5 insertions(+), 3 deletions(-) diff --git a/.github/workflows/dotnet-core.yml b/.github/workflows/dotnet-core.yml index e73b2c743..32d08bb70 100644 --- a/.github/workflows/dotnet-core.yml +++ b/.github/workflows/dotnet-core.yml @@ -15,7 +15,9 @@ jobs: - uses: actions/checkout@v2 - uses: actions/setup-dotnet@v1 with: - dotnet-version: 6.x + dotnet-version: | + 5.x + 6.x - name: Install dependencies run: | dotnet restore DemoContentBuilder diff --git a/BepuPhysics/BepuPhysics.csproj b/BepuPhysics/BepuPhysics.csproj index b5593c2ea..fdb7cee40 100644 --- a/BepuPhysics/BepuPhysics.csproj +++ b/BepuPhysics/BepuPhysics.csproj @@ -20,7 +20,7 @@ false key.snk true - false + diff --git a/BepuUtilities/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj index efe5660a4..1ae2adc1e 100644 --- a/BepuUtilities/BepuUtilities.csproj +++ b/BepuUtilities/BepuUtilities.csproj @@ -21,7 +21,7 @@ false key.snk true - false + From d5ad67d3152ce0e830d9177aeadf4a49f6a5124f Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 15 Dec 2021 19:22:47 -0600 Subject: [PATCH 340/947] Some BouncinessDemo cleanup; pulled it into the visible demoset. --- Demos/DemoSet.cs | 1 + Demos/Demos/BouncinessDemo.cs | 114 +++++++++++++++++----------------- 2 files changed, 57 insertions(+), 58 deletions(-) diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index e0b0dd718..2b7dbc630 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -70,6 +70,7 @@ public DemoSet() AddOption(); AddOption(); AddOption(); + AddOption(); AddOption(); } diff --git a/Demos/Demos/BouncinessDemo.cs b/Demos/Demos/BouncinessDemo.cs index feab53951..99ca41b99 100644 --- a/Demos/Demos/BouncinessDemo.cs +++ b/Demos/Demos/BouncinessDemo.cs @@ -10,71 +10,69 @@ namespace Demos.Demos { - public struct SimpleMaterial - { - public SpringSettings SpringSettings; - public float FrictionCoefficient; - public float MaximumRecoveryVelocity; - } - public unsafe struct BounceCallbacks : INarrowPhaseCallbacks + /// + /// Shows how to configure things to bounce in the absence of a coefficient of restitution. + /// + /// + /// v2 does not support traditional coefficients of restitution because it conflicts with speculative contacts. + /// All contacts are, however, springs. With a little configuration, you can give objects physically reasonable bounciness. + /// + public class BouncinessDemo : Demo { - public CollidableProperty CollidableMaterials; - - public void Initialize(Simulation simulation) + public struct SimpleMaterial { - //The callbacks get created before the simulation so that they can be given to the simulation. The property needs a simulation reference, so we hand it over in the initialize. - CollidableMaterials.Initialize(simulation); + public SpringSettings SpringSettings; + public float FrictionCoefficient; + public float MaximumRecoveryVelocity; } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin) + public unsafe struct BounceCallbacks : INarrowPhaseCallbacks { - //While the engine won't even try creating pairs between statics at all, it will ask about kinematic-kinematic pairs. - //Those pairs cannot emit constraints since both involved bodies have infinite inertia. Since most of the demos don't need - //to collect information about kinematic-kinematic pairs, we'll require that at least one of the bodies needs to be dynamic. - return a.Mobility == CollidableMobility.Dynamic || b.Mobility == CollidableMobility.Dynamic; - } + public CollidableProperty CollidableMaterials; - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB) - { - return true; - } + public void Initialize(Simulation simulation) + { + //The callbacks get created before the simulation so that they can be given to the simulation. The property needs a simulation reference, so we hand it over in the initialize. + CollidableMaterials.Initialize(simulation); + } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold - { - //For the purposes of this demo, we'll use multiplicative blending for the friction and choose spring properties according to which collidable has a higher maximum recovery velocity. - var a = CollidableMaterials[pair.A]; - var b = CollidableMaterials[pair.B]; - pairMaterial.FrictionCoefficient = a.FrictionCoefficient * b.FrictionCoefficient; - pairMaterial.MaximumRecoveryVelocity = MathF.Max(a.MaximumRecoveryVelocity, b.MaximumRecoveryVelocity); - pairMaterial.SpringSettings = pairMaterial.MaximumRecoveryVelocity == a.MaximumRecoveryVelocity ? a.SpringSettings : b.SpringSettings; - return true; - } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin) + { + //While the engine won't even try creating pairs between statics at all, it will ask about kinematic-kinematic pairs. + //Those pairs cannot emit constraints since both involved bodies have infinite inertia. Since most of the demos don't need + //to collect information about kinematic-kinematic pairs, we'll require that at least one of the bodies needs to be dynamic. + return a.Mobility == CollidableMobility.Dynamic || b.Mobility == CollidableMobility.Dynamic; + } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold) - { - return true; - } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB) + { + return true; + } - public void Dispose() - { + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public unsafe bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold + { + //For the purposes of this demo, we'll use multiplicative blending for the friction and choose spring properties according to which collidable has a higher maximum recovery velocity. + var a = CollidableMaterials[pair.A]; + var b = CollidableMaterials[pair.B]; + pairMaterial.FrictionCoefficient = a.FrictionCoefficient * b.FrictionCoefficient; + pairMaterial.MaximumRecoveryVelocity = MathF.Max(a.MaximumRecoveryVelocity, b.MaximumRecoveryVelocity); + pairMaterial.SpringSettings = pairMaterial.MaximumRecoveryVelocity == a.MaximumRecoveryVelocity ? a.SpringSettings : b.SpringSettings; + return true; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold) + { + return true; + } + + public void Dispose() + { + } } - } - /// - /// Shows how to configure things to bounce in the absence of a coefficient of restitution. - /// - /// - /// v2 does not currently support traditional coefficients of restitution because it conflicts with speculative contacts. - /// While it could be added later in a limited way- trusting the user to configure bounciness/speculative margins such that things mostly work- - /// for now, there is no traditional 0 to 1 bounciness value. All contacts are, however, springs. - /// With a little configuration, you can give objects physically reasonable bounciness. - /// - public class BouncinessDemo : Demo - { public unsafe override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(0, 40, 200); @@ -86,10 +84,10 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //That allows higher stiffnesses to be used since collisions last longer relative to the solver timestep duration. //(Note that substepping tends to be an extremely strong simulation stabilizer, so you can usually get away with lower solver iteration counts for better performance. It defaults to 1 velocity iteration per substep.) var collidableMaterials = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new BounceCallbacks() { CollidableMaterials = collidableMaterials }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 8); + Simulation = Simulation.Create(BufferPool, new BounceCallbacks() { CollidableMaterials = collidableMaterials }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0), 0, 0), 8); - var shape = new Sphere(1); - var ballDescription = BodyDescription.CreateDynamic(RigidPose.Identity, shape.ComputeInertia(1), new(Simulation.Shapes.Add(shape), ContinuousDetection.Discrete(20, 20)), 1e-2f); + var shape = new Sphere(1); + var ballDescription = BodyDescription.CreateDynamic(RigidPose.Identity, shape.ComputeInertia(1), Simulation.Shapes.Add(shape), 1e-2f); for (int i = 0; i < 100; ++i) { From d87fff75c250bbc10afed01912ad054f9643d1b7 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 16 Dec 2021 18:22:10 -0600 Subject: [PATCH 341/947] Fixed text template for refactor. --- .../Constraints/Contact/ContactConvexTypes.tt | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt b/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt index 8cf939e8f..13dd1dd20 100644 --- a/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt +++ b/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt @@ -240,11 +240,11 @@ for (int i = 0; i < contactCount; ++i) <#} else if(bodyCount == 2) {#> Vector3Wide.Subtract(prestep.Contact0.OffsetA, prestep.OffsetB, out var offsetToManifoldCenterB); <#}#> - TangentFriction<#=suffix#>.WarmStart2(x, z, <#if(contactCount > 1) {#>offsetToManifoldCenterA<#} else {#>prestep.Contact0.OffsetA<#}#>, <#if(bodyCount == 2) {#>offsetToManifoldCenterB, <#}#>inertiaA, <#if(bodyCount == 2) {#>inertiaB, <#}#>accumulatedImpulses.Tangent, ref wsvA<#if(bodyCount == 2) {#>, ref wsvB<#}#>); + TangentFriction<#=suffix#>.WarmStart(x, z, <#if(contactCount > 1) {#>offsetToManifoldCenterA<#} else {#>prestep.Contact0.OffsetA<#}#>, <#if(bodyCount == 2) {#>offsetToManifoldCenterB, <#}#>inertiaA, <#if(bodyCount == 2) {#>inertiaB, <#}#>accumulatedImpulses.Tangent, ref wsvA<#if(bodyCount == 2) {#>, ref wsvB<#}#>); <#for (int i = 0; i < contactCount; ++i) {#> - PenetrationLimit<#=suffix#>.WarmStart2(inertiaA, <#if(bodyCount == 2) {#>inertiaB, <#}#>prestep.Normal, prestep.Contact<#=i#>.OffsetA, <#if(bodyCount == 2) {#>prestep.Contact<#=i#>.OffsetA - prestep.OffsetB, <#}#>accumulatedImpulses.Penetration<#=i#>, ref wsvA<#if(bodyCount == 2) {#>, ref wsvB<#}#>); + PenetrationLimit<#=suffix#>.WarmStart(inertiaA, <#if(bodyCount == 2) {#>inertiaB, <#}#>prestep.Normal, prestep.Contact<#=i#>.OffsetA, <#if(bodyCount == 2) {#>prestep.Contact<#=i#>.OffsetA - prestep.OffsetB, <#}#>accumulatedImpulses.Penetration<#=i#>, ref wsvA<#if(bodyCount == 2) {#>, ref wsvB<#}#>); <#}#> - TwistFriction<#=suffix#>.WarmStart2(prestep.Normal, inertiaA, <#if(bodyCount == 2) {#>inertiaB, <#}#>accumulatedImpulses.Twist, ref wsvA<#if(bodyCount == 2) {#>, ref wsvB<#}#>); + TwistFriction<#=suffix#>.WarmStart(prestep.Normal, inertiaA, <#if(bodyCount == 2) {#>inertiaB, <#}#>accumulatedImpulses.Twist, ref wsvA<#if(bodyCount == 2) {#>, ref wsvB<#}#>); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -263,14 +263,14 @@ for (int i = 0; i < contactCount; ++i) <#} else if(bodyCount == 2) {#> Vector3Wide.Subtract(prestep.Contact0.OffsetA, prestep.OffsetB, out var offsetToManifoldCenterB); <#}#> - TangentFriction<#=suffix#>.Solve2(x, z, <#=contactCount == 1 ? "prestep.Contact0.OffsetA" : "offsetToManifoldCenterA"#><#=bodyCount == 2 ? ", offsetToManifoldCenterB" : ""#>, inertiaA<#=bodyCount == 2 ? ", inertiaB" : ""#>, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); + TangentFriction<#=suffix#>.Solve(x, z, <#=contactCount == 1 ? "prestep.Contact0.OffsetA" : "offsetToManifoldCenterA"#><#=bodyCount == 2 ? ", offsetToManifoldCenterB" : ""#>, inertiaA<#=bodyCount == 2 ? ", inertiaB" : ""#>, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); //Note that we solve the penetration constraints after the friction constraints. //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. //It's a pretty minor effect either way. SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); var inverseDtWide = new Vector(inverseDt); <#for (int i = 0; i < contactCount; ++i) {#> - PenetrationLimit<#=suffix#>.Solve2(inertiaA<#=bodyCount == 2 ? ", inertiaB" : ""#>, prestep.Normal, prestep.Contact<#=i#>.OffsetA, <#if(bodyCount == 2) {#>prestep.Contact<#=i#>.OffsetA - prestep.OffsetB, <#}#>prestep.Contact<#=i#>.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration<#=i#>, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); + PenetrationLimit<#=suffix#>.Solve(inertiaA<#=bodyCount == 2 ? ", inertiaB" : ""#>, prestep.Normal, prestep.Contact<#=i#>.OffsetA, <#if(bodyCount == 2) {#>prestep.Contact<#=i#>.OffsetA - prestep.OffsetB, <#}#>prestep.Contact<#=i#>.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration<#=i#>, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); <#}#> <#if (contactCount == 1) {#> //If there's only one contact, then the contact patch as determined by contact distance would be zero. @@ -282,7 +282,7 @@ for (int i = 0; i < contactCount; ++i) accumulatedImpulses.Penetration<#=i#> * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact<#=i#>.OffsetA)<#=i == contactCount - 1 ? ");" : " +"#> <#}#> <#}#> - TwistFriction<#=suffix#>.Solve2(prestep.Normal, inertiaA, <#if (bodyCount == 2) {#>inertiaB, <#}#>maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); + TwistFriction<#=suffix#>.Solve(prestep.Normal, inertiaA, <#if (bodyCount == 2) {#>inertiaB, <#}#>maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); } } From a05a189650fc594a38f66c0c828f85ec698436c0 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 17 Dec 2021 17:59:12 -0600 Subject: [PATCH 342/947] Bounciness note. --- Demos/Demo.cs | 3 +-- Demos/Demos/BouncinessDemo.cs | 8 +++++++- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/Demos/Demo.cs b/Demos/Demo.cs index 3d49f0863..3bf4f8873 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -42,8 +42,7 @@ protected Demo() //there won't be enough memory bandwidth to even feed half the physical cores. Using all 128 logical cores would just add overhead. //It may be worth using something like hwloc or CPUID to extract extra information to reason about. - //var targetThreadCount = Math.Max(1, Environment.ProcessorCount > 4 ? Environment.ProcessorCount - 2 : Environment.ProcessorCount - 1); - var targetThreadCount = 30; + var targetThreadCount = Math.Max(1, Environment.ProcessorCount > 4 ? Environment.ProcessorCount - 2 : Environment.ProcessorCount - 1); ThreadDispatcher = new SimpleThreadDispatcher(targetThreadCount); } diff --git a/Demos/Demos/BouncinessDemo.cs b/Demos/Demos/BouncinessDemo.cs index 99ca41b99..9002a87a7 100644 --- a/Demos/Demos/BouncinessDemo.cs +++ b/Demos/Demos/BouncinessDemo.cs @@ -96,8 +96,14 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //We'll drop balls in a grid. From left to right, we increase stiffness, and from back to front (relative to the camera), we'll increase damping. //Note that higher frequency values tend to result in smaller bounces even at 0 damping. This is not physically realistic; it's a byproduct of the solver timestep being too long to properly handle extremely brief contacts. //(Try increasing the substep count above to higher values and watch how the bounce gets closer and closer to equal height across frequency values.) + + //We choose a relatively high MaximumRecoveryVeloctity of 1000 so that the spring can actually push things back into the air, but be careful about assigning it to float.MaxValue. + //Substepping uses an approximate contact update rather than re-running the entirety of collision detection over and over. + //It can accumulate error in contact penetration depths. If using many substeps with high stiffness/low damping collisions, these small depth errors can result in very large velocities. + //If you need extremely high contact spring frequencies, you may need to run collision detection more often (that is, call Simulation.Timestep more often with fewer solver substeps to compensate), + //but limiting the recovery velocity will at least stop it from exploding. ballDescription.Pose.Position = new Vector3(i * 3 - 99f * 3f / 2f, 100, j * 3 - 230); - collidableMaterials.Allocate(Simulation.Bodies.Add(ballDescription)) = new SimpleMaterial { FrictionCoefficient = 1, MaximumRecoveryVelocity = float.MaxValue, SpringSettings = new SpringSettings(5 + 0.25f * i, j * j / 10000f) }; + collidableMaterials.Allocate(Simulation.Bodies.Add(ballDescription)) = new SimpleMaterial { FrictionCoefficient = 1, MaximumRecoveryVelocity = 1000, SpringSettings = new SpringSettings(5 + 0.25f * i, j * j / 10000f) }; } } From f9b9933ef4bdfe4c92594a5cf03d56d34a346716 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 18 Dec 2021 19:46:08 -0600 Subject: [PATCH 343/947] Improved pair determinism tests. --- DemoTests/PairDeterminismTests.cs | 21 +++++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) diff --git a/DemoTests/PairDeterminismTests.cs b/DemoTests/PairDeterminismTests.cs index f80c6334c..8aaafecaf 100644 --- a/DemoTests/PairDeterminismTests.cs +++ b/DemoTests/PairDeterminismTests.cs @@ -67,15 +67,23 @@ public unsafe void OnPairCompleted(int pairId, ref TManifold manifold } static void ComputeCollisions(CollisionTaskRegistry registry, Shapes shapes, BufferPool pool, - ref Manifolds manifolds, CollidableDescription a, CollidableDescription b, ref Buffer posesA, ref Buffer posesB, Buffer remapIndices, int pairCount) + ref Manifolds manifolds, CollidableDescription a, CollidableDescription b, ref Buffer posesA, ref Buffer posesB, Buffer remapIndices, int pairCount, Random random) { - var batcher = new CollisionBatcher(pool, shapes, registry, 1 / 60f, new BatcherCallbacks { Pool = pool, Manifolds = manifolds }); + const float dt = 1 / 60f; + var callbacks = new BatcherCallbacks { Pool = pool, Manifolds = manifolds }; + var batcher = new CollisionBatcher(pool, shapes, registry, dt, callbacks); + int flushInterval = random.Next(Math.Max(1, pairCount / 5), pairCount); for (int i = 0; i < pairCount; ++i) { var index = remapIndices[i]; ref var poseA = ref posesA[index]; ref var poseB = ref posesB[index]; batcher.Add(a.Shape, b.Shape, poseB.Position - poseA.Position, poseA.Orientation, poseB.Orientation, Math.Max(a.Continuity.MaximumSpeculativeMargin, b.Continuity.MaximumSpeculativeMargin), new PairContinuation(index)); + if (i % flushInterval == flushInterval - 1) + { + batcher.Flush(); + batcher = new CollisionBatcher(pool, shapes, registry, dt, callbacks); + } } batcher.Flush(); } @@ -99,7 +107,7 @@ private static void TestPair(CollidableDescription a, CollidableDescription b, B posesB[i] = TestHelpers.CreateRandomPose(random, positionBounds); remapIndices[i] = i; }; - ComputeCollisions(registry, shapes, pool, ref originalManifolds, a, b, ref posesA, ref posesB, remapIndices, pairCount); + ComputeCollisions(registry, shapes, pool, ref originalManifolds, a, b, ref posesA, ref posesB, remapIndices, pairCount, random); for (int i = 0; i < pairCount; ++i) { @@ -149,7 +157,7 @@ private static void TestPair(CollidableDescription a, CollidableDescription b, B remapIndices[i] = remainingIndices[toTake]; remainingIndices.FastRemoveAt(toTake); } - ComputeCollisions(registry, shapes, pool, ref comparisonManifolds, a, b, ref posesA, ref posesB, remapIndices, pairCount); + ComputeCollisions(registry, shapes, pool, ref comparisonManifolds, a, b, ref posesA, ref posesB, remapIndices, pairCount, random); for (int i = 0; i < pairCount; ++i) { Assert.True(originalManifolds.ManifoldIsConvex[i] == comparisonManifolds.ManifoldIsConvex[i], $"{pairName} manifolds don't even have the same convexity state! {originalManifolds.ManifoldIsConvex[i]} versus {comparisonManifolds.ManifoldIsConvex[i]}"); @@ -256,8 +264,8 @@ public static void Test() DemoMeshHelper.CreateDeformedPlane(2, 2, (x, y) => new Vector3(x, 0, y), Vector3.One, pool, out var mesh); var meshCollidable = new CollidableDescription(shapes.Add(mesh), continuousDetection); - const int pairCount = 31; - const int poseIterations = 256; + const int pairCount = 128; + const int poseIterations = 64; const int remapIterations = 64; var bounds = new BoundingBox(new Vector3(-6), new Vector3(6)); const int randomSeed = 5; @@ -316,6 +324,7 @@ public static void Test() TestPair(meshCollidable, meshCollidable, bounds, pairCount, poseIterations, remapIterations, registry, pool, shapes, randomSeed); + pool.Clear(); } } } From 4f367c65b5cff0e23e432e758565971cdf8df128 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 19 Dec 2021 19:22:32 -0600 Subject: [PATCH 344/947] Updated some docs. --- Documentation/Building.md | 8 ++++---- Documentation/GettingStarted.md | 23 ++++++++++------------- README.md | 4 ++-- 3 files changed, 16 insertions(+), 19 deletions(-) diff --git a/Documentation/Building.md b/Documentation/Building.md index 76c237782..31e95e46b 100644 --- a/Documentation/Building.md +++ b/Documentation/Building.md @@ -2,9 +2,9 @@ ## Library -The easiest way to build the library is using the latest version of Visual Studio with the .NET Core workload installed to open and build the `Library.sln`. +The easiest way to build the library is using the latest version of Visual Studio with the .NET desktop development workload installed to open and build the `Library.sln`. -The library tends to use the latest C# language features. At the time of writing, it requires C# 8.0. It does not use any of 8.0's runtime specific features, so it should be consumable in .NET Framework projects. +The library tends to use the latest C# language features. At the time of writing, it requires C# 9.0. `BepuPhysics.csproj` uses T4 templates for code generation in a few places. If changes are made to the templates, you'll need a build pipeline that can process them (like Visual Studio). The repository contains the original generated .cs files, so if no changes are made, the templates do not need to be evaluated. @@ -12,7 +12,7 @@ The libraries target .NET 5. ## Demos -`Demos.sln` contains all the projects necessary to build and run the demos application. The default demo renderer uses DX11, and the content pipeline's shader compiler requires the Windows SDK. The demos application targets .NET 5. +`Demos.sln` contains all the projects necessary to build and run the demos application. The default demo renderer uses DX11, and the content pipeline's shader compiler requires the Windows SDK. The demos application targets .NET 6. There's also an [OpenGL version of the demos](https://github.com/bepu/bepuphysics2/tree/master/Demos.GL). You can run it from the command line in the repository root using `dotnet run --project Demos.GL/Demos.csproj -c Release`. @@ -34,7 +34,7 @@ Some extra checks for data validity can be enabled with the `CHECKMATH` compilat ## Runtime -The library makes heavy use of SIMD intrinsics through `System.Numerics.Vectors`. Good performance requires a IL to native assembly compiler which is aware of these intrinsics. Right now, that means something like CoreCLR's RyuJIT. Other runtimes may not support the intrinsics and may suffer massive slowdowns- sometimes 10 to 100 times slower, if they run at all. +The library makes heavy use of SIMD intrinsics through `System.Numerics.Vectors` and `System.Runtime.Intrinsics`. Good performance requires a IL to native assembly compiler which is aware of these intrinsics. Right now, that means something like CoreCLR's RyuJIT. Other runtimes may not support the intrinsics and may suffer massive slowdowns- sometimes 10 to 100 times slower, if they run at all. Performance scales up with higher SIMD machine widths. Machines with full rate AVX2 will tend to significantly outperform SSE-limited machines. diff --git a/Documentation/GettingStarted.md b/Documentation/GettingStarted.md index a7bff84da..654b980c6 100644 --- a/Documentation/GettingStarted.md +++ b/Documentation/GettingStarted.md @@ -26,14 +26,13 @@ Note that `TNarrowPhaseCallbacks` and `TPoseIntegratorCallbacks` are required to A similar pattern is used in many places across the engine. You can think about these as compile time delegates or closures, just with nastier syntax. -`Simulation.Create` also has a set of optional parameters to initialize a couple of Solver properties and the initial allocations. The most interesting optional parameter is the `ITimeStepper`, which defines the order of stage execution within the engine. +`SolveDescription` describes how the simulation should schedule updates. You can set the number of velocity iterations and substeps that occur in each simulation timestep. For simulations with difficult constraint configurations, using more substeps can help stabilize the simulation far more cheaply than increasing velocity iterations can. For advanced use cases, you can schedule the number of velocity iterations for each substep individually. -The engine includes a few `ITimeStepper` types out of the box: -1. [`PositionFirstTimestepper`](../BepuPhysics/PositionFirstTimestepper.cs) first integrates positions and velocities, then detects collisions and solves. In between calls to Timestep, the velocities are in a freshly-solved state and the body position is in the same position as when contacts were created. Velocities modified between `Timestep` calls will be trusted and integrated directly into body poses regardless of collisions or constraints, so using the exposed stage events to make velocity modifications may be wise. -2. [`PositionLastTimestepper`](../BepuPhysics/PositionLastTimestepper.cs) flips things around and integrates positions last. Velocities set between `Timestep` calls will be run through the solver before being integrated, but poses won't be the same as the poses which generated the latest contacts. Has a very small performance penalty compared to `PositionFirstTimestepper` due to splitting velocity and position integration. -3. [`SubsteppingTimestepper`](../BepuPhysics/SubsteppingTimestepper.cs) allows pose integration and solving to run at a higher rate than collision detection. This can be very helpful for simulations with complex constraint configurations with high robustness requirements. The [`SubsteppingDemo`](../Demos/Demos/SubsteppingDemo.cs) shows an example of how effective substeps can be for pathologically difficult constraint configurations. +There's also a fallback batch threshold, which you can safely leave at the default value almost always- it's the number of synchronized constraint batches that the simulation will create before falling back to a special case solve when individual bodies have excessive numbers of constraints connected to them. -The default `ITimeStepper` is the `PositionFirstTimestepper`. +`Simulation.Create` also has a couple of optional parameters: initial allocation sizes to pull from the resource pool (to avoid unnecessary resizing later), and the `ITimestepper` which defines the order of stage execution within the engine. + +The engine includes only one `ITimeStepper` type out of the box, which gets used if no other `ITimestepper` is provided: the [`DefaultTimestepper`](../BepuPhysics/DefaultTimestepper.cs). It checks candidates for sleeping, computes bounding boxes, performs collision detection, solves constraints (which includes any necessary body velocity/pose integration), then does some incremental optimization work on internal data structures. There are callbacks between stages that can be hooked into. A custom `ITimestepper` could be provided that changes what stages execute or their order. ## Timestepping/updating @@ -59,11 +58,11 @@ Creating a body with zero inverse mass and inverse inertia will create a kinemat The `CollidableDescription` takes a reference to a shape allocated in the `Simulation.Shapes` set. Shapes are allocated independently from bodies. Multiple bodies can refer to the same allocated shape. Collidables are also allowed to refer to no shape at all which can be useful for creating some constraint systems. -Note that there is no internal list of `BodyDescription` instances, nor a single "Body" type anywhere. Instead, all body properties are split across several buffers. Further, there are multiple `BodySet` instances, each with their own set of buffers. These buffers are set up for efficient internal access, with the first `BodySet` storing all active bodies and the later sets containing inactive body data. The `BodyDescription` is decomposed into these separate pieces upon being added. +Note that there is no internal list of `BodyDescription` instances, nor a single "Body" type anywhere. Instead, body properties are split across different buffers depending on internal memory access patterns. Further, there are multiple `BodySet` instances, each with their own set of buffers. These buffers are set up for efficient internal access, with the first `BodySet` storing all active bodies and the later sets containing inactive body data. The `BodyDescription` is decomposed into these separate pieces upon being added. -To access an existing body's data, the body's current memory location must be looked up using the handle returned by the `Add` call. The `BodyReference` convenience type can make this a little easier by hiding the lookup process. +To access an existing body's data, the body's current memory location must be looked up using the handle returned by the `Add` call. The `BodyReference` convenience type can make this a little easier by hiding the lookup process. You can get a `BodyReference` by indexing: `Simulation.Bodies[BodyHandle]`. -Bodies may move around in memory during execution or when other bodies are added, removed, awoken, or slept. Holding onto the raw memory location through one of these changes may result in the pointer pointing to undefined data; the handle should be used to perform a fresh lookup any time the memory location could have been invalidated. +Bodies may move around in memory during execution or when other bodies are added, removed, awoken, or slept. Holding onto the raw memory location through one of these changes may result in the pointer pointing to undefined data. The `BodyReference` type performs lookups on demand, and remains valid so long as the wrapped `BodyHandle` does. ## Statics @@ -80,14 +79,12 @@ Constraints can be used to control the relative motion of bodies. There are a [w Some examples of constraints in the demos include: 1. [`RagdollDemo`](../Demos/Demos/RagdollDemo.cs) is a... demo of ragdolls. 2. [`CarDemo`](../Demos/Demos/CarDemo.cs) shows how to build a simple constraint based car (and bad AI drivers). -3. [`RopeStabilityDemo`](../Demos/Demos/RopeStabilityDemo.cs) shows some constraint failure modes and how to fix them. +3. [`RopeStabilityDemo`](../Demos/Demos/RopeStabilityDemo.cs) shows some constraint configuration failure modes and how to fix them. 4. [`NewtDemo`](../Demos/Demos/NewtDemo.cs) shows how to make a squishy newt. 5. [`ClothDemo`](../Demos/Demos/ClothDemo.cs) shows how to make sheets of cloth with different properties. Existing raw constraint data is more difficult to access than body data. There is a similar handle->memory location lookup, but the data itself is stored a few layers deep in array-of-structures-of-arrays format for performance. Pulling data out of this representation is not very convenient, so the `Solver` has `ApplyDescription` and `GetDescription` for accessing constraint data. Custom descriptions can be created to access only subsets of a constraint's full data. -Note that `Simulation.Solver.Add` has a few overloads for different body counts, but it does not check to ensure that the appropriate overload is called for the constraint type at compile time. When compiled with the `DEBUG` symbol, a `Debug.Assert` will catch the problem at runtime. (I'll probably rework this into a compile time error later.) - ## Queries The engine supports scene-wide queries through `Simulation.RayCast` and `Simulation.Sweep`. Sweeps support both linear and angular motion for convex shapes. Both functions make use of hit handlers- `IRayHitHandler` and `ISweepHitHandler`. The handlers can filter out objects and respond to found impacts. @@ -118,4 +115,4 @@ As the above suggests, the engine uses a lot of idioms which are historically un In summary, it's a very low level API. The intent is to maximize performance, then expose as much as possible to let application-specific convenient abstractions be built on top. -All of this puts a heavier burden on users. They must be familiar with value type semantics, new performance minded language features, pointers, and all sorts of other unusual-for-C# stuff. If you've got questions, feel free to post them on the [forum](https://github.com/bepu/bepuphysics2/discussions). \ No newline at end of file +All of this puts a heavier burden on users. They must be familiar with value type semantics, new performance minded language features, pointers, and all sorts of other unusual-for-C# stuff. If you've got questions, feel free to post them in the [discussions](https://github.com/bepu/bepuphysics2/discussions). \ No newline at end of file diff --git a/README.md b/README.md index 31842ba90..ae3b4e2bf 100644 --- a/README.md +++ b/README.md @@ -3,11 +3,11 @@ This is the repo for the bepuphysics v2 library, a complete rewrite of the C# 3d rigid body physics engine [BEPUphysics v1](https://github.com/bepu/bepuphysics1). -The BepuPhysics and BepuUtilities libraries target .NET 5 and should work on any supported platform. The demos target .NET Core 5.0 and use DX11 by default. There is also an [OpenGL version of the demos](https://github.com/bepu/bepuphysics2/tree/master/Demos.GL) for other platforms that you can run from the command line in the repository root using `dotnet run --project Demos.GL/Demos.csproj -c Release`. +The BepuPhysics and BepuUtilities libraries target .NET 5 and should work on any supported platform. The demos target .NET 6.0 and use DX11 by default. There is also an [OpenGL version of the demos](https://github.com/bepu/bepuphysics2/tree/master/Demos.GL) for other platforms that you can run from the command line in the repository root using `dotnet run --project Demos.GL/Demos.csproj -c Release`. The physics engine heavily uses System.Numerics.Vectors types, so to get good performance, you'll need a compiler which can consume those types (like RyuJIT). -To build the source, you'll need a recent version of Visual Studio with the .NET Core workload installed. Demos.sln references all relevant projects. For more information, see [Building](Documentation/Building.md). +To build the source, you'll need a recent version of Visual Studio with the .NET desktop development workload installed. Demos.sln references all relevant projects. For more information, see [Building](Documentation/Building.md). ## Features From b8e638f0daa2ea2777f1c357e070f5d9583218fc Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 19 Dec 2021 19:23:53 -0600 Subject: [PATCH 345/947] Tiny solver refactoring. --- .../{SubsteppingTimestepper.cs => DefaultTimestepper.cs} | 2 +- BepuPhysics/Simulation.cs | 4 ++-- BepuPhysics/Solver.cs | 4 ++-- 3 files changed, 5 insertions(+), 5 deletions(-) rename BepuPhysics/{SubsteppingTimestepper.cs => DefaultTimestepper.cs} (97%) diff --git a/BepuPhysics/SubsteppingTimestepper.cs b/BepuPhysics/DefaultTimestepper.cs similarity index 97% rename from BepuPhysics/SubsteppingTimestepper.cs rename to BepuPhysics/DefaultTimestepper.cs index 2c4bd01ae..264571bc6 100644 --- a/BepuPhysics/SubsteppingTimestepper.cs +++ b/BepuPhysics/DefaultTimestepper.cs @@ -10,7 +10,7 @@ namespace BepuPhysics /// Updates the simulation in the order of: sleeper -> predict body bounding boxes -> collision detection -> substepping solve -> data structure optimization. /// Each substep of the solve simulates and integrates a sub-timestep of length dt/substepCount. /// - public class SubsteppingTimestepper : ITimestepper + public class DefaultTimestepper : ITimestepper { /// /// Fires after the sleeper completes and before bodies are integrated. diff --git a/BepuPhysics/Simulation.cs b/BepuPhysics/Simulation.cs index f82fdc07f..53eed00e6 100644 --- a/BepuPhysics/Simulation.cs +++ b/BepuPhysics/Simulation.cs @@ -63,7 +63,7 @@ public partial class Simulation : IDisposable /// Buffer pool used to fill persistent structures and main thread ephemeral resources across the engine. /// Callbacks to use in the narrow phase. /// Callbacks to use in the pose integrator. - /// Timestepper that defines how the simulation state should be updated. If null, is used. + /// Timestepper that defines how the simulation state should be updated. If null, is used. /// Describes how the solver should execute, including the number of substeps and the number of velocity iterations per substep. /// Allocation sizes to initialize the simulation with. If left null, default values are chosen. /// New simulation. @@ -110,7 +110,7 @@ public static Simulation Create simulation.Solver.awakener = simulation.Awakener; simulation.Bodies.Initialize(simulation.Solver, simulation.Awakener, simulation.Sleeper); simulation.SolverBatchCompressor = new BatchCompressor(simulation.Solver, simulation.Bodies); - simulation.Timestepper = timestepper ?? new SubsteppingTimestepper(); + simulation.Timestepper = timestepper ?? new DefaultTimestepper(); var narrowPhase = new NarrowPhase(simulation, DefaultTypes.CreateDefaultCollisionTaskRegistry(), DefaultTypes.CreateDefaultSweepTaskRegistry(), diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index e458ac6c8..3a82a965e 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -563,7 +563,7 @@ internal void ValidateSetOwnership() unsafe struct ValidateAccumulatedImpulsesEnumerator : IForEach { - public int Index = 0; + public int Index; public float* AccumulatedImpulses; public void LoopBody(float impulse) { @@ -617,7 +617,7 @@ internal unsafe void ValidateFallbackBodyReferencesByHash(HashDiagnosticType has internal unsafe void ValidateAccumulatedImpulses() { var impulseMemory = stackalloc float[16]; - var impulsesEnumerator = new ValidateAccumulatedImpulsesEnumerator { AccumulatedImpulses = impulseMemory }; + var impulsesEnumerator = new ValidateAccumulatedImpulsesEnumerator { Index = 0, AccumulatedImpulses = impulseMemory }; for (int i = 0; i < Sets.Length; ++i) { ref var set = ref Sets[i]; From 80ea165d4172969229495076dec442c38bfc01c1 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 20 Dec 2021 14:43:36 -0600 Subject: [PATCH 346/947] Fixed an oopsy in predict bounding box job scheduling that could AVE. --- BepuPhysics/PoseIntegrator.cs | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/BepuPhysics/PoseIntegrator.cs b/BepuPhysics/PoseIntegrator.cs index f3e13f486..37da29755 100644 --- a/BepuPhysics/PoseIntegrator.cs +++ b/BepuPhysics/PoseIntegrator.cs @@ -399,8 +399,8 @@ bool TryGetJob(int maximumJobInterval, out int start, out int exclusiveEnd) void PredictBoundingBoxesWorker(int workerIndex) { var boundingBoxUpdater = new BoundingBoxBatcher(bodies, shapes, broadPhase, threadDispatcher.GetThreadMemoryPool(workerIndex), cachedDt); - var bodyCount = bodies.ActiveSet.Count; - while (TryGetJob(bodyCount, out var start, out var exclusiveEnd)) + var bundleCount = BundleIndexing.GetBundleCount(bodies.ActiveSet.Count); + while (TryGetJob(bundleCount, out var start, out var exclusiveEnd)) { PredictBoundingBoxes(start, exclusiveEnd, cachedDt, ref boundingBoxUpdater, workerIndex); } @@ -431,6 +431,7 @@ public void PredictBoundingBoxes(float dt, BufferPool pool, IThreadDispatcher th PrepareForMultithreadedExecution(BundleIndexing.GetBundleCount(bodies.ActiveSet.Count), dt, threadDispatcher.ThreadCount); this.threadDispatcher = threadDispatcher; threadDispatcher.DispatchWorkers(predictBoundingBoxesWorker); + //predictBoundingBoxesWorker(0); this.threadDispatcher = null; } else From b54d013eee454f4c23dc7b88d57fcdceae6b3009 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 20 Dec 2021 16:51:55 -0600 Subject: [PATCH 347/947] More doc updates. --- Documentation/PackagingAndVersioning.md | 2 +- Documentation/QuestionsAndAnswers.md | 6 ++---- Documentation/StabilityTips.md | 18 +++++++++--------- Documentation/UpgradingFromV1.md | 2 +- Documentation/roadmap.md | 2 +- 5 files changed, 14 insertions(+), 16 deletions(-) diff --git a/Documentation/PackagingAndVersioning.md b/Documentation/PackagingAndVersioning.md index 81d04e0b1..a731637d3 100644 --- a/Documentation/PackagingAndVersioning.md +++ b/Documentation/PackagingAndVersioning.md @@ -8,4 +8,4 @@ NuGet packages will be made available, but they will not cover all possible feat The library has a variety of conditional compilation symbols. Rather than publishing a combinatorial mess to NuGet, the expectation is that users of any conditional logic will clone the source. -Given the above and the general nature of the library's API, cloning the source and referencing the project is often the best way to include the library. +Given the above and the general nature of the library's API, cloning the source and referencing the project is often the best way to include the library. \ No newline at end of file diff --git a/Documentation/QuestionsAndAnswers.md b/Documentation/QuestionsAndAnswers.md index 895840fd1..92b2f3c04 100644 --- a/Documentation/QuestionsAndAnswers.md +++ b/Documentation/QuestionsAndAnswers.md @@ -49,9 +49,9 @@ Hope that they happen to have exactly the same architecture so that every single If the target hardware is known to be identical (maybe a networked console game where users only play against other users of the exact same hardware), you might be fine. Sometimes, you'll even get lucky and end up with two different desktop processors that produce identical results. -But, in general, the only way to guarantee cross platform determinism is to avoid generating any instructions which may differ between hardware. A common solution here is to use fixed point rather than floating point math. +But, in general, the only way to guarantee cross platform determinism is to avoid generating any instructions which may differ between hardware. A common solution here is to use software floating point or fixed point rather than native floating point math. -At the moment, BEPUphysics v2 does not support fixed point math out of the box, and it would be a pretty enormous undertaking to port it all over without destroying performance. +At the moment, BEPUphysics v2 does not support software floats or fixed math out of the box, and it would be a pretty enormous undertaking to port it all over without destroying performance. I may look into conditionally compiled alternative scalar types in the future. I can't guarantee when or if I'll get around to it, though; don't wait for me! @@ -87,8 +87,6 @@ Frequency and damping ratio can achieve some of the same effects as restitution The reason for the lack of a traditional coefficient of restitution is speculative contacts. v1 used them too, but v2 pushes their usage much further and uses them as the primary form of continuous collision detection. Most of the problems caused by speculative contacts (like ghost contacts) have been smoothed over, but the naive implementation of velocity-flip restitution simply doesn't work with speculative contacts. -I'd like to see if the frequency/damping ratio can suffice for most use cases. If this is a critical problem for what you are trying to do, let me know. I can't guarantee I'll fix it in the near term, but if it becomes a blocking problem for a large number of people (or myself) there's a better chance that I'll spend the time to add a workaround. - ### Swept shape tests against the backfaces of meshes don't go through, but collisions do. What's going on? While ray and collision testing against triangles is always one sided, swept shape tests are double sided. This is pretty strange, and there isn't a secret good reason for it. diff --git a/Documentation/StabilityTips.md b/Documentation/StabilityTips.md index 3d3186612..4bfc21de5 100644 --- a/Documentation/StabilityTips.md +++ b/Documentation/StabilityTips.md @@ -19,13 +19,13 @@ There are ways around this issue, though. Reducing lever arms, adjusting inertia The second class of failure, excessive stiffness, is more difficult to hack away. If you configure a constraint with a frequency of 120hz and your simulation is running at 60hz, the integrator is going to have trouble representing the resulting motion. It won't *always* explode, but if you throw a bunch of 240hz constraints together at a 60hz solver rate, bad things are likely. -If you can, avoid using a constraint frequency greater than half of your solver update rate. That is, if the solver is running at 60hz, stick to 30hz or below for your constraints' spring settings. +If you can, avoid using a constraint frequency greater than half of your solver update rate. That is, if the solver is running at 60hz, stick to 30hz or below for your constraints' spring settings. If using very low velocity iteration counts (like 1), you may need to be more conservative with the constraint frequencies relative to the solver rate. -Sometimes, though, you can't use tricks or hacks (as admirable as they are) to stabilize a simulation, or you just want very stiff and stable response. Sometimes you don't have enough control over the simulation to add specific countermeasures- user generated content is rarely simulation friendly. At this point, the solver just needs to run more frequently to compensate. +Sometimes, though, you can't use tricks or hacks to stabilize a simulation, or you just want very stiff and stable response. Sometimes you don't have enough control over the simulation to add specific countermeasures- user generated content is rarely simulation friendly. At this point, the solver just needs to run more frequently to compensate. The obvious way to increase the solver's execution rate is to call `Simulation.Timestep` more frequently with a smaller `dt` parameter. If you can afford it, this is the highest quality option since it also performs collision detection more frequently. -If you're only concerned about solver stability, then you can instead use the `SubsteppingTimestepper` or another custom `ITimestepper` that works similarly. When calling `Simulation.Create`, pass the SubsteppingTimestepper with the desired number of substeps. For example, if the SubsteppingTimestepper uses 4 substeps and Simulation.Timestep is called at a rate of 60hz, then the solver and integrator will actually run at 240hz. Notably, because increasing the update rate is such a powerful stabilizer, you can usually drop the number of solver velocity iterations to save some simulation time. +If you're only concerned about solver stability, then you can instead use the solver's substepping feature. When calling `Simulation.Create`, pass a `SolveDescription` with the desired number of substeps. For example, if the solver uses 4 substeps and Simulation.Timestep is called at a rate of 60hz, then the solver and integrator will actually run at 240hz. Notably, because increasing the update rate is such a powerful stabilizer, you can usually drop the number of solver velocity iterations to save some simulation time. Using higher update rates can enable the simulation of otherwise impossible mass ratios, like 1000:1, even with fairly low velocity iterations. Here's a rope connected by 240hz frequency constraints with a 1000:1 mass ratio wrecking ball at the end, showing how the number of substeps affects quality: @@ -34,22 +34,22 @@ Using higher update rates can enable the simulation of otherwise impossible mass For more examples of substepping, check out the [SubsteppingDemo](../Demos/Demos/SubsteppingDemo.cs). So, if you're encountering constraint instability, here are some general guidelines for debugging: -1. First, try increasing the update rate to a really high value (600hz or more). If the problem goes away, then it's probably related to the difficulty of the simulation and a lack of convergence and not to a configuration error. +1. First, try increasing the `Simulation.Timestep` update rate to a really high value (600hz or more). If the problem goes away, then it's probably related to the difficulty of the simulation and a lack of convergence and not to a configuration error. 2. Drop back down to a normal update rate and increase the solver iteration count. If going up to 10 or 15 solver iterations fixes it, then it was likely just a mild convergence failure. If you can't identify any issues in the configuration that would make convergence more difficult than it needs to be (and there aren't any tricks available like the rope stuff described above), then using more solver iterations might just be required. -3. If you need way more solver iterations- 30, 50, 100, or even 10000 isn't fixing it- then a higher update is likely required. This is especially true if you are observing constraint 'explosions' where bodies start flying all over the place. Try using the `SubsteppingTimestepper` and gradually increase the number of substeps until the simulation becomes stable. To preserve performance, try also dropping the number of solver velocity iterations as you increase the substeps. Using more than 4 velocity iterations with 4+ substeps is often pointless. -4. If using a substepping timestepper does not fix the problem but increasing full simulation update rate does, it's possible that collision detection requires the extra temporal resolution. This is pretty rare and may imply that the speculative margins surrounding shapes need to be larger. +3. If you need way more solver iterations- 30, 50, 100, or even 10000 isn't fixing it- then a higher update is likely required. This is especially true if you are observing constraint 'explosions' where bodies start flying all over the place. Try using a `SolveDescription` with higher substep counts. Gradually increase the number of substeps until the simulation becomes stable. To preserve performance, try also dropping the number of solver velocity iterations as you increase the substeps. Using more than 4 velocity iterations with 4+ substeps is often pointless, and using only 1 velocity iteration with substepping is often the sweet spot. +4. If using a substepping timestepper does not fix the problem but increasing full simulation update rate does, it's possible that collision detection requires the extra temporal resolution. This is pretty rare, but it can happen when the incremental contact update used by substepping is a poor match for the true contact manifold. Some general guidelines: -1. While many simple simulations can work fine with only 1 solver iteration, using a minimum of 2 is recommended for most simulations. Simulations with greater degrees of complexity- articulated robots, ragdolls, stacks- will often need more. (Unless you're using a high solver update rate!) +1. While many simple simulations can work fine with only 1 solver iteration, using a minimum of 2 is recommended for most simulations if you're not using substepping. Simulations with greater degrees of complexity- articulated robots, ragdolls, stacks- will often need more (or just more substeps!). 2. The "mass ratios" problem: avoid making heavy objects depend on light objects. A tiny box can sit on top of a tank without any issues at all, but a tank would squish a tiny box. Larger mass ratios yield larger stability problems. 3. If constraints are taking too many iterations to converge and the design allows it, try softening the constraints. A little bit of softness can significantly stabilize a constraint system and avoid the need for higher update rates. 4. Avoid configuring constraints to 'fight' each other. Opposing constraints tend to require more iterations to converge, and if they're extremely rigid, it can require shorter timesteps or substepping. -5. When tuning the `SpringSettings.Frequency` of constraints, prefer values smaller than `0.5 / timeStepDuration`. Higher values increase the risk of instability. +5. When tuning the `SpringSettings.Frequency` of constraints with one substep and multiple solver iterations, prefer values smaller than `0.5 / timeStepDuration`. Higher values increase the risk of instability. If using aggressive substepping with only one velocity iteration per substep, a good initial guess for the required number of substeps is `substepCount = 6 * constraintFrequency * timeStepDuration`. 6. If your simulation requires a lot of solver velocity iterations to be stable, try using substepping with lower velocity iteration counts. It might end up more stable *and* faster! ## Contact Generation -While nonzero speculative margins are required for stable contact, overly large margins can sometimes cause 'ghost' contacts when objects are moving quickly relative to each other. It might look like one object is bouncing off the air a foot away from the other shape. To avoid this, use a smaller speculative margin and consider explicitly enabling continuous collision detection for the shape. +While nonzero speculative margins are required for stable contact, overly large margins can sometimes cause 'ghost' contacts when objects are moving quickly relative to each other. It might look like one object is bouncing off the air a foot away from the other shape. To avoid this, use a smaller maximum speculative margin and consider explicitly enabling continuous collision detection for the shape. Prefer simpler shapes. In particular, avoid highly complex convex hulls with a bunch of faces separated by only a few degrees. The solver likes temporally coherent contacts, and excess complexity can cause the set of generated contacts to vary significantly with small rotations. Also, complicated hulls are slower! diff --git a/Documentation/UpgradingFromV1.md b/Documentation/UpgradingFromV1.md index 45aedc860..f32132606 100644 --- a/Documentation/UpgradingFromV1.md +++ b/Documentation/UpgradingFromV1.md @@ -10,7 +10,7 @@ Some of this might look way more complicated, and it might seem like features we | Mobile dynamic and kinematic objects | `Entity` | Body, but there is no `Body` type- see `Simulation.Bodies` to allocate, access, and delete bodies. Creating a body using `Simulation.Bodies.Add` returns a handle that uniquely identifies the body for the duration of its existence, and `Simulation.Bodies.HandleToLocation` finds the current memory location of a body. `BodyReference` can be used to handle lookups for you.| | Mobile object properties | `Entity` properties, like `Position` and `LinearVelocity` | Create a `BodyReference` from the body handle, then access properties like `Pose` and `Velocity`. Can also manually perform the lookup into the `Simulation.Bodies` sets and their raw property buffers. | | Collision events | `entity.CollisionInformation.Events` | No out of the box events; [`ContactEventsDemo`](../Demos/Demos/ContactEventsDemo.cs) shows how to use narrow phase callbacks to create events. | -| Enumerating existing collisions | `entity.CollisionInformation.Pairs` | Collision data is not explicitly cached anywhere. Narrow phase callbacks can be used to collect collision information. Collision-created contact constraints (and all other connected constraints) can be enumerated using the Constraints body property. | +| Enumerating existing collisions | `entity.CollisionInformation.Pairs` | Collision data is not explicitly cached anywhere. Narrow phase callbacks can be used to collect collision information. Collision-created contact constraints (and all other connected constraints) can be enumerated using the Constraints body property. See the [`SolverContactEnumerationDemo`](../Demos/Demos/SolverContactEnumerationDemo.cs) for an example of enumerating contact constraints. | | Collision filtering | `e.CollisionInformation.CollisionRules` and `CollisionRules` static functions | `INarrowPhaseCallbacks` has `AllowContactGeneration` and `ConfigureContactManifold` which return a boolean that controls whether narrow phase testing and constraint generation should proceed. See [`RagdollDemo`](../Demos/Demos/RagdollDemo.cs) for an example of collision filtering. | | Custom gravity | `entity.Gravity` | `IPoseIntegratorCallbacks` can be used to implement any form of gravity or other per-body velocity influence. See [PlanetDemo](../Demos/Demos/PlanetDemo.cs) for an example. | | Object velocity damping | `entity.LinearDamping` and `entity.AngularDamping` | `IPoseIntegratorCallbacks` again- damping is just a velocity influence. See [DemoCallbacks](../Demos/DemoCallbacks.cs) for an example. | diff --git a/Documentation/roadmap.md b/Documentation/roadmap.md index 627f536c4..204a1b382 100644 --- a/Documentation/roadmap.md +++ b/Documentation/roadmap.md @@ -28,4 +28,4 @@ Here, we get into the realm of the highly speculative. I make no guarantees abou - Generalized boundary smoothing to support non-mesh compounds. Would help avoid annoying hitches when scooting around complex geometry composed of a bunch of convex hulls or other non-triangle convexes. - Buoyancy. In particular, a form of buoyancy that supports deterministic animated (nonplanar) surfaces. It would almost certainly still be a heightmap, but the goal would be to support synchronized multiplayer boats or similar use cases. Ideally, it would also allow for some approximate handling of concavity. Hollowed concave shapes would displace the appropriate amount of water, including handling of the case where a boat capsizes and the internal volume fills with water (though likely in a pretty hacky way). Would likely be easy to extend this to simple heightmap fluid simulation if the determinism requirement is relaxed. -- Fixed point math for cross platform determinism, or swapping to a restricted subset of reliable floating point operations. This one is pretty questionable, but it would be nice to support deterministic physics across any platform. (It's important to note, however, that fixed point math alone is merely necessary and not necessarily *sufficient* to guarantee cross platform determinism...) It is unlikely that I will personally make use of this feature, so the likelihood of it being implemented is lower unless I can find a low effort path. \ No newline at end of file +- Software floating point or fixed point math for cross platform determinism, or swapping to a restricted subset of reliable floating point operations. This one is pretty questionable, but it would be nice to support deterministic physics across any platform. (It's important to note, however, that fixed point math alone is merely necessary and not necessarily *sufficient* to guarantee cross platform determinism...) It is unlikely that I will personally make use of this feature, so the likelihood of it being implemented is lower unless I can find a low effort path. \ No newline at end of file From dd482dc52e831f2be92bcf995075541b95fbde7d Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 20 Dec 2021 18:16:05 -0600 Subject: [PATCH 348/947] Roadmap update. --- Documentation/roadmap.md | 25 ++++++++++++++----------- 1 file changed, 14 insertions(+), 11 deletions(-) diff --git a/Documentation/roadmap.md b/Documentation/roadmap.md index 204a1b382..3187c9460 100644 --- a/Documentation/roadmap.md +++ b/Documentation/roadmap.md @@ -2,24 +2,27 @@ This is a high level plan for future development. All dates and features are speculative. For a detailed breakdown of tasks in progress, check the [issues](https://github.com/bepu/bepuphysics2/issues) page. -## Near term (H2 2021) +## Near term (H1 2022) -2.4.0 will launch with a revamped solver that should be dramatically faster. The library will also take a dependency on .NET 5 and assumes a runtime roughly as capable as the version of CoreCLR associated with .NET 5. +With 2.4's launch, most of my development will probably be moving back to things actually using bepuphysics2 for a while. The usual maintenance plus a few smallish fixes and improvements punted from 2.4 are likely. -I'll be working in the background on other projects using bepuphysics2. Their requirements will drive various changes and future planning. +I may fiddle with some of the items listed under medium term, but I suspect I will not get them to a fully released state in this period. The most likely of the bunch would be high precision poses and ARM dedicated codepaths. + +There's a chance I'll also try for a kinda-sorta cross platform determinism by reimplementing a few likely sources of architecture nondeterminism in software. It'll probably work only across x64 machines (if at all), but who knows! ## Medium term The timing on these features are uncertain, but they're relatively low hanging fruit and I would like to get to them eventually. -1. Tree revamp. Should help with both initialization costs (faster Mesh construction, for example) as well as faster and more flexible broad phase incremental refinement. -2. Lower deactivation/reactivation spike overhead. As a part of improving the Tree, I'd like to add batched subtree insertions with better multithreaded scaling. As insertions are a major sequential bottleneck in the current activation system, this could drop the sleep/wake costs by a lot. -3. High precision body and static poses, plus associated broad phase changes, for worlds exceeding 32 bit floating point precision. This isn't actually too difficult, but it would come with tradeoffs. See https://github.com/bepu/bepuphysics2/issues/13. -4. Mesh/compound intersection optimization, especially in pairs with higher angular velocity. -5. Warm starting depth refinement for some expensive convex pair types. -6. Better allocators for temporary data on threads (could significantly reduce memory requirements on some simulations). -7. Convex hull tooling improvements, like in-library simplification utilities. -8. Ray cast optimization, particularly with large batches of rays. +1. High precision body and static poses, plus associated broad phase changes, for worlds exceeding 32 bit floating point precision. This isn't actually too difficult, but it would come with tradeoffs. See https://github.com/bepu/bepuphysics2/issues/13. 2.4's revamp of the solver and body data layouts intentionally left the door open for higher precision poses. +2. ARM specialized paths: https://github.com/bepu/bepuphysics2/issues/184 +3. Convex hull tooling improvements, like in-library simplification utilities. +4. Tree revamp. Should help with both initialization costs (faster Mesh construction, for example) as well as faster and more flexible broad phase incremental refinement. +5. Lower deactivation/reactivation spike overhead. As a part of improving the Tree, I'd like to add batched subtree insertions with better multithreaded scaling. As insertions are a major sequential bottleneck in the current activation system, this could drop the sleep/wake costs by a lot. +6. Mesh/compound intersection optimization, especially in pairs with higher angular velocity. +7. Warm starting depth refinement for some expensive convex pair types. +8. Better allocators for temporary data on threads (could significantly reduce memory requirements on some simulations). +9. Ray cast optimization, particularly with large batches of rays. ## Long term From c6904177dfa55609ce21330569f861bdf2484992 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 20 Dec 2021 18:21:33 -0600 Subject: [PATCH 349/947] Perf tips update. --- Documentation/PerformanceTips.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Documentation/PerformanceTips.md b/Documentation/PerformanceTips.md index 5a3490b19..ca9c22bf4 100644 --- a/Documentation/PerformanceTips.md +++ b/Documentation/PerformanceTips.md @@ -36,4 +36,4 @@ Note that cylinders and convex hulls will likely [become faster](https://github. ## Solver Optimization -Try using the minimum number of iterations sufficient to retain stability. The cost of the solver stage is linear with the number of iterations, and some simulations can get by with very few. --For some simulations with very complex constraint configurations, there may be no practical number of solver iterations. In these cases, you may need to instead use a shorter time step duration for the entire simulation or use the `SubsteppingTimestepper`. See the [`SubsteppingDemo`](../Demos/SubsteppingDemo.cs) for an example. \ No newline at end of file +-For some simulations with very complex constraint configurations, there may be no practical number of solver iterations that can stabilize the simulation. In these cases, you may need to instead use substepping or a shorter time step duration for the entire simulation. More frequent solver execution can massively improve simulation quality, allowing you to drop velocity iteration counts massively (even to just 1 per substep). See the [`SubsteppingDemo`](../Demos/SubsteppingDemo.cs) for an example of substepping in action. \ No newline at end of file From 76618245bee90ac356e3b212fc8b37d71c0f405e Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 21 Dec 2021 20:15:51 -0600 Subject: [PATCH 350/947] it still counts as documentation if it's 1/9th done right --- Documentation/Substepping.md | 14 ++++++++++++++ 1 file changed, 14 insertions(+) create mode 100644 Documentation/Substepping.md diff --git a/Documentation/Substepping.md b/Documentation/Substepping.md new file mode 100644 index 000000000..fefdbe94f --- /dev/null +++ b/Documentation/Substepping.md @@ -0,0 +1,14 @@ +# Substepping + +Each call to `Simulation.Timestep(dt, ...)` simulates one frame with duration equal to `dt`. In the [`DefaultTimestepper`](../BepuPhysics/DefaultTimestepper.cs) (which, as the name implies, is the `ITimestepper` implementation used if no other is specified) executes a frame like so: + +```cs +simulation.Sleep(); +simulation.PredictBoundingBoxes(dt, threadDispatcher); +simulation.CollisionDetection(dt, threadDispatcher); +simulation.Solve(dt, threadDispatcher); +simulation.IncrementallyOptimizeDataStructures(threadDispatcher); +``` +There's only one execution of collision each stage per call to `Timestep`, each responsible for covering the specified `dt`. + +The solver, however, can be configured to take multiple timesteps internally for every call to `Simulation.Timestep`. These are called substeps. When creating a simulation: From 96c91f56a888ea8978f98aaa0e433d593fb80331 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 22 Dec 2021 19:17:10 -0600 Subject: [PATCH 351/947] Substep words growing. --- Documentation/Substepping.md | 28 +++++++++++++++++++++++++--- 1 file changed, 25 insertions(+), 3 deletions(-) diff --git a/Documentation/Substepping.md b/Documentation/Substepping.md index fefdbe94f..4ef428772 100644 --- a/Documentation/Substepping.md +++ b/Documentation/Substepping.md @@ -1,7 +1,13 @@ -# Substepping +# What's substepping? +Substepping integrates body velocities and positions and solves constraints more than once per call to `Simulation.Timestep`. For some simulations with complex constraint configurations, high stiffness, or high mass ratios, substepping is the fastest way to find a stable solution. -Each call to `Simulation.Timestep(dt, ...)` simulates one frame with duration equal to `dt`. In the [`DefaultTimestepper`](../BepuPhysics/DefaultTimestepper.cs) (which, as the name implies, is the `ITimestepper` implementation used if no other is specified) executes a frame like so: +You can configure a simulation to use substepping by passing a `SolveDescription` to `Simulation.Create` that has more than one substep. For example, to create a simulation that uses 8 substeps and 1 velocity iteration per substep: +```cs +var simulation = Simulation.Create(BufferPool, new NarrowPhaseCallbacks(), new PoseIntegratorCallbacks(), new SolveDescription(velocityIterationCount: 1, substepCount: 8)); +``` +# How substepping fits into a timestep +Each call to `Simulation.Timestep(dt, ...)` simulates one frame with duration equal to `dt`. In the [`DefaultTimestepper`](../BepuPhysics/DefaultTimestepper.cs) (which, as the name implies, is the `ITimestepper` implementation used if no other is specified) executes a frame like so: ```cs simulation.Sleep(); simulation.PredictBoundingBoxes(dt, threadDispatcher); @@ -11,4 +17,20 @@ simulation.IncrementallyOptimizeDataStructures(threadDispatcher); ``` There's only one execution of collision each stage per call to `Timestep`, each responsible for covering the specified `dt`. -The solver, however, can be configured to take multiple timesteps internally for every call to `Simulation.Timestep`. These are called substeps. When creating a simulation: +When configured to use more than one substep, `Simulation.Solve` will integrate bodies and solve constraints as if `Simulation.Timestep` was called `Simulation.Solver.SubstepCount` times, each time with a duration equal to `dt / Simulation.Solver.SubstepCount`. + +The difference between using substepping and explicitly calling `Timestep` more frequently is that none of the other stages run during substeps. For example, contact constraints are incrementally updated in an approximate way, but full collision detection is not run. This allows substeps to be much faster than full timesteps. + +# Velocity iteration scheduling +While the simplest approach is to use the same number of velocity iterations for all substeps, they are allowed to vary. You can provide a `VelocityIterationScheduler` callback in the `SolveDescription` to define how many velocity iterations each substep should take. There's also a helper that takes a span of integers defining the velocity iteration counts to use for each substep. +```cs +var simulation = Simulation.Create(BufferPool, new NarrowPhaseCallbacks(), new PoseIntegratorCallbacks(), new SolveDescription(new[] {2, 1, 1})); +``` +The above snippet would use 3 substeps with 2 velocity iterations on the first substep, then one velocity iteration on the second and third substeps. + +# Callbacks + +The solver exposes events that fire at the beginning and end of each substep: `SubstepStarted` and `SubstepEnded`. These events are called from worker thread 0 in the solver's thread dispatch; the dispatch does not end in between substeps to keep overhead low. + +(Note that attempting to dispatch multithreaded work from the same `IThreadDispatcher` instance that dispatched the solver's workers requires that the `IThreadDispatcher` implementation is reentrant. The demos `SimpleThreadDispatcher` is not.) + From b485479384bfc4f7aa35cab6ade772cc7903c859 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 22 Dec 2021 19:20:26 -0600 Subject: [PATCH 352/947] Motion state validation now avoids undefined data in the hash. --- BepuPhysics/Bodies.cs | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/BepuPhysics/Bodies.cs b/BepuPhysics/Bodies.cs index fc36e2079..2fdae69b7 100644 --- a/BepuPhysics/Bodies.cs +++ b/BepuPhysics/Bodies.cs @@ -615,7 +615,14 @@ internal void ValidateAwakeMotionStatesByHash(HashDiagnosticType type) for (int j = 0; j < set.Count; ++j) { ref var state = ref set.SolverStates[j]; - instance.ContributeToHash(ref hash, state); + instance.ContributeToHash(ref hash, state.Motion.Pose.Position); + instance.ContributeToHash(ref hash, state.Motion.Pose.Orientation); + instance.ContributeToHash(ref hash, state.Motion.Velocity.Linear); + instance.ContributeToHash(ref hash, state.Motion.Velocity.Angular); + instance.ContributeToHash(ref hash, state.Inertia.Local.InverseInertiaTensor); + instance.ContributeToHash(ref hash, state.Inertia.Local.InverseMass); + instance.ContributeToHash(ref hash, state.Inertia.World.InverseInertiaTensor); + instance.ContributeToHash(ref hash, state.Inertia.World.InverseMass); } } From 103b933400590de1782113def36d44058430f86a Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 22 Dec 2021 23:46:04 -0600 Subject: [PATCH 353/947] IslandAwakener disposal now properly handles with zero sets. Static induced awakening now handles awakenings in determinism compatible way. --- BepuPhysics/IslandAwakener.cs | 16 ++++++++++------ BepuPhysics/Statics.cs | 30 ++++++++++++++++-------------- 2 files changed, 26 insertions(+), 20 deletions(-) diff --git a/BepuPhysics/IslandAwakener.cs b/BepuPhysics/IslandAwakener.cs index 58eec08bd..1f9dd70c1 100644 --- a/BepuPhysics/IslandAwakener.cs +++ b/BepuPhysics/IslandAwakener.cs @@ -802,6 +802,7 @@ void EnsurePairCacheTypeCapacities(ref TypeAllocationSizes cache internal void DisposeForCompletedAwakenings(ref QuickList setIndices) { + Debug.Assert(setIndices.Count > 0 == phaseOneJobs.Span.Allocated && setIndices.Count > 0 == phaseTwoJobs.Span.Allocated); for (int i = 0; i < setIndices.Count; ++i) { var setIndex = setIndices[i]; @@ -821,16 +822,19 @@ internal void DisposeForCompletedAwakenings(ref QuickList setIndices) sleeper.ReturnSetId(setIndex); } - phaseOneJobs.Dispose(pool); - for (int i = 0; i < phaseTwoJobs.Count; ++i) + if (phaseOneJobs.Span.Allocated) { - ref var job = ref phaseTwoJobs[i]; - if (job.Type == PhaseTwoJobType.AddFallbackTypeBatchConstraints) + phaseOneJobs.Dispose(pool); + for (int i = 0; i < phaseTwoJobs.Count; ++i) { - pool.Return(ref job.AddFallbackTypeBatchConstraints.Sources); + ref var job = ref phaseTwoJobs[i]; + if (job.Type == PhaseTwoJobType.AddFallbackTypeBatchConstraints) + { + pool.Return(ref job.AddFallbackTypeBatchConstraints.Sources); + } } + phaseTwoJobs.Dispose(pool); } - phaseTwoJobs.Dispose(pool); } } } diff --git a/BepuPhysics/Statics.cs b/BepuPhysics/Statics.cs index d81cb73e1..bdfb63134 100644 --- a/BepuPhysics/Statics.cs +++ b/BepuPhysics/Statics.cs @@ -204,24 +204,27 @@ internal void ValidateExistingHandle(StaticHandle handle) "This static handle doesn't seem to exist, or the mappings are out of sync. If a handle exists, both directions should match."); } - struct SleepingBodyCollector : IBreakableForEach + struct SleepingBodyCollector : IBreakableForEach where TFilter : struct, IStaticChangeAwakeningFilter { + Bodies bodies; BroadPhase broadPhase; BufferPool pool; - public QuickList SleepingBodyHandles; + public QuickList SleepingSets; + public TFilter Filter; - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public SleepingBodyCollector(BroadPhase broadPhase, BufferPool pool) + public SleepingBodyCollector(Bodies bodies, BroadPhase broadPhase, BufferPool pool, ref TFilter filter) { - this.pool = pool; + this.bodies = bodies; this.broadPhase = broadPhase; - SleepingBodyHandles = new QuickList(32, pool); + this.pool = pool; + SleepingSets = new QuickList(32, pool); + Filter = filter; } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Dispose() { - SleepingBodyHandles.Dispose(pool); + SleepingSets.Dispose(pool); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -230,7 +233,8 @@ public bool LoopBody(int leafIndex) ref var leaf = ref broadPhase.staticLeaves[leafIndex]; if (leaf.Mobility != CollidableMobility.Static) { - SleepingBodyHandles.Add(leaf.BodyHandle, pool); + if (Filter.ShouldAwaken(bodies[leaf.BodyHandle])) + SleepingSets.Add(bodies.HandleToLocation[leaf.BodyHandle.Value].SetIndex, pool); } return true; } @@ -240,13 +244,11 @@ void AwakenBodiesInBounds(ref BoundingBox bounds, ref TFilter filter) w { if (filter.AllowAwakening) { - var collector = new SleepingBodyCollector(broadPhase, pool); + var collector = new SleepingBodyCollector(bodies, broadPhase, pool, ref filter); broadPhase.StaticTree.GetOverlaps(bounds, ref collector); - for (int i = 0; i < collector.SleepingBodyHandles.Count; ++i) - { - if (filter.ShouldAwaken(bodies[collector.SleepingBodyHandles[i]])) - awakener.AwakenBody(collector.SleepingBodyHandles[i]); - } + awakener.AwakenSets(ref collector.SleepingSets); + //Just in case the filter did some internal mutation, preserve the changes. + filter = collector.Filter; collector.Dispose(); } } From 434deb232fdcc7e24297e06fb25242391b59e904 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 23 Dec 2021 13:53:27 -0600 Subject: [PATCH 354/947] Breaking change: swapped velocityIterationCouint and substepCount in SolveDescription. Made demos all use the constructor for educational reasons. --- BepuPhysics/SolveDescription.cs | 22 +++++++++---------- .../ConstraintDescriptionMappingTests.cs | 4 ++-- Demos/Demos/BlockChainDemo.cs | 4 ++-- Demos/Demos/BouncinessDemo.cs | 2 +- Demos/Demos/Cars/CarDemo.cs | 2 +- Demos/Demos/Characters/CharacterDemo.cs | 2 +- Demos/Demos/ClothDemo.cs | 2 +- Demos/Demos/CollisionQueryDemo.cs | 2 +- Demos/Demos/ColosseumDemo.cs | 2 +- Demos/Demos/CompoundTestDemo.cs | 2 +- Demos/Demos/ContactEventsDemo.cs | 2 +- .../Demos/ContinuousCollisionDetectionDemo.cs | 2 +- Demos/Demos/CustomVoxelCollidableDemo.cs | 2 +- Demos/Demos/NewtDemo.cs | 2 +- Demos/Demos/PlanetDemo.cs | 2 +- Demos/Demos/PyramidDemo.cs | 2 +- Demos/Demos/RagdollDemo.cs | 4 ++-- Demos/Demos/RayCastingDemo.cs | 2 +- Demos/Demos/RopeStabilityDemo.cs | 2 +- Demos/Demos/RopeTwistDemo.cs | 2 +- Demos/Demos/SimpleSelfContainedDemo.cs | 2 +- Demos/Demos/SolverContactEnumerationDemo.cs | 2 +- Demos/Demos/Sponsors/SponsorDemo.cs | 2 +- Demos/Demos/SubsteppingDemo.cs | 2 +- Demos/Demos/SweepDemo.cs | 2 +- Demos/Demos/Tanks/TankDemo.cs | 4 ++-- .../BroadPhaseStressTestDemo.cs | 2 +- Demos/SpecializedTests/CapsuleTestDemo.cs | 2 +- Demos/SpecializedTests/CharacterTestDemo.cs | 2 +- Demos/SpecializedTests/ClothLatticeDemo.cs | 2 +- Demos/SpecializedTests/CompoundBoundTests.cs | 2 +- .../CompoundCollisionIndicesTest.cs | 2 +- .../ConstrainedKinematicIntegrationTest.cs | 2 +- Demos/SpecializedTests/ConstraintTestDemo.cs | 3 +-- Demos/SpecializedTests/ConvexHullTestDemo.cs | 2 +- Demos/SpecializedTests/CylinderTestDemo.cs | 2 +- Demos/SpecializedTests/GyroscopeTestDemo.cs | 2 +- Demos/SpecializedTests/Media/BedsheetDemo.cs | 2 +- .../Media/ColosseumVideoDemo.cs | 2 +- .../Media/NewtDemandingSacrificeVideoDemo.cs | 2 +- Demos/SpecializedTests/Media/NewtVideoDemo.cs | 2 +- .../Media/PyramidVideoDemo.cs | 2 +- .../Media/ShrinkwrappedNewtsVideoDemo.cs | 2 +- Demos/SpecializedTests/MeshMeshTestDemo.cs | 2 +- .../SpecializedTests/MeshReductionTestDemo.cs | 2 +- .../MeshSerializationTestDemo.cs | 2 +- Demos/SpecializedTests/MeshTestDemo.cs | 2 +- .../PyramidAwakenerTestDemo.cs | 2 +- Demos/SpecializedTests/RagdollTubeDemo.cs | 2 +- Demos/SpecializedTests/ShapePileTestDemo.cs | 2 +- Demos/SpecializedTests/SolverBatchTestDemo.cs | 2 +- Demos/SpecializedTests/TriangleRayTestDemo.cs | 2 +- Demos/SpecializedTests/TriangleTestDemo.cs | 2 +- Demos/SpecializedTests/VolumeQueryTests.cs | 2 +- 54 files changed, 68 insertions(+), 69 deletions(-) diff --git a/BepuPhysics/SolveDescription.cs b/BepuPhysics/SolveDescription.cs index b4f3ae75a..bf6661d99 100644 --- a/BepuPhysics/SolveDescription.cs +++ b/BepuPhysics/SolveDescription.cs @@ -15,15 +15,15 @@ namespace BepuPhysics /// public struct SolveDescription { - /// - /// Number of substeps to execute each time the solver runs. - /// - public int SubstepCount; /// /// Number of velocity iterations to use in the solver if there is no or if it returns a non-positive value for a substep. /// public int VelocityIterationCount; /// + /// Number of substeps to execute each time the solver runs. + /// + public int SubstepCount; + /// /// Number of synchronzed constraint batches to use before using a fallback approach. /// public int FallbackBatchThreshold; @@ -49,10 +49,10 @@ internal void ValidateDescription() /// /// Creates a solve description. /// - /// Number of substeps in the solve. /// Number of velocity iterations per substep. + /// Number of substeps in the solve. /// Number of synchronzed constraint batches to use before using a fallback approach. - public SolveDescription(int substepCount, int velocityIterationCount = 1, int fallbackBatchThreshold = DefaultFallbackBatchThreshold) + public SolveDescription(int velocityIterationCount, int substepCount, int fallbackBatchThreshold = DefaultFallbackBatchThreshold) { SubstepCount = substepCount; VelocityIterationCount = velocityIterationCount; @@ -94,18 +94,18 @@ public SolveDescription(ReadOnlySpan substepVelocityIterations, int fallbac } /// - /// Creates a solve description with 1 velocity iteration per substep and a fallback threshold of . + /// Creates a solve description with the given number of velocity iterations and a single substep, with a fallback threshold of . /// - /// Number of substeps per solve. - public static implicit operator SolveDescription(int substepCount) + /// Number of velocity iterations per substep. + public static implicit operator SolveDescription(int velocityIterationCount) { - return new SolveDescription(substepCount); + return new SolveDescription(velocityIterationCount, 1); } /// /// Creates a solve description with the given number of substeps and velocity iterations per substep and a fallback threshold of . /// /// Number of substeps and iterations per solve. - public static implicit operator SolveDescription((int substepCount, int iterationsPerSubstep) schedule) + public static implicit operator SolveDescription((int iterationsPerSubstep, int substepCount) schedule) { return new SolveDescription(schedule.substepCount, schedule.iterationsPerSubstep); } diff --git a/DemoTests/ConstraintDescriptionMappingTests.cs b/DemoTests/ConstraintDescriptionMappingTests.cs index e901aad58..50816e1d3 100644 --- a/DemoTests/ConstraintDescriptionMappingTests.cs +++ b/DemoTests/ConstraintDescriptionMappingTests.cs @@ -25,7 +25,7 @@ static void FillWithRandomBytes(ref T item, Random random) where T : struct } static void Test(BufferPool pool, Random random, int constraintTypeBodyCount) where T : unmanaged, IConstraintDescription { - var simulation = Simulation.Create(pool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(), 4); + var simulation = Simulation.Create(pool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(), new SolveDescription(1, 4)); const int bodyCount = 2048; @@ -96,7 +96,7 @@ static bool CheckEquality(string parentString, Type type, ValueType a, ValueType [Fact] public static void TestMappings() { - var pool = new BufferPool(); + var pool = new BufferPool(); var random = new Random(5); Test(pool, random, 1); Test(pool, random, 1); diff --git a/Demos/Demos/BlockChainDemo.cs b/Demos/Demos/BlockChainDemo.cs index d2c60beee..7d071d04a 100644 --- a/Demos/Demos/BlockChainDemo.cs +++ b/Demos/Demos/BlockChainDemo.cs @@ -25,7 +25,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 3)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0), angularDamping: 0.2f), new SolveDescription(8, 1)); var boxShape = new Box(1, 1, 1); var boxInertia = boxShape.ComputeInertia(1); @@ -61,7 +61,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //Build the coin description for the ponz-I mean ICO. var coinShape = new Cylinder(1.5f, 0.2f); - coinDescription = BodyDescription.CreateDynamic(RigidPose.Identity, coinShape.ComputeInertia(1), Simulation.Shapes.Add(coinShape), 0.01f); + coinDescription = BodyDescription.CreateDynamic(RigidPose.Identity, coinShape.ComputeInertia(1), Simulation.Shapes.Add(coinShape), 0.03f); } BodyDescription coinDescription; diff --git a/Demos/Demos/BouncinessDemo.cs b/Demos/Demos/BouncinessDemo.cs index 9002a87a7..23a2ad111 100644 --- a/Demos/Demos/BouncinessDemo.cs +++ b/Demos/Demos/BouncinessDemo.cs @@ -84,7 +84,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //That allows higher stiffnesses to be used since collisions last longer relative to the solver timestep duration. //(Note that substepping tends to be an extremely strong simulation stabilizer, so you can usually get away with lower solver iteration counts for better performance. It defaults to 1 velocity iteration per substep.) var collidableMaterials = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new BounceCallbacks() { CollidableMaterials = collidableMaterials }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0), 0, 0), 8); + Simulation = Simulation.Create(BufferPool, new BounceCallbacks() { CollidableMaterials = collidableMaterials }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0), 0, 0), new SolveDescription(1, 8)); var shape = new Sphere(1); var ballDescription = BodyDescription.CreateDynamic(RigidPose.Identity, shape.ComputeInertia(1), Simulation.Shapes.Add(shape), 1e-2f); diff --git a/Demos/Demos/Cars/CarDemo.cs b/Demos/Demos/Cars/CarDemo.cs index bf16882c9..8f8d81f51 100644 --- a/Demos/Demos/Cars/CarDemo.cs +++ b/Demos/Demos/Cars/CarDemo.cs @@ -42,7 +42,7 @@ public override void Initialize(ContentArchive content, Camera camera) var properties = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new CarCallbacks() { Properties = properties }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); + Simulation = Simulation.Create(BufferPool, new CarCallbacks() { Properties = properties }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); var builder = new CompoundBuilder(BufferPool, Simulation.Shapes, 2); builder.Add(new Box(1.85f, 0.7f, 4.73f), RigidPose.Identity, 10); diff --git a/Demos/Demos/Characters/CharacterDemo.cs b/Demos/Demos/Characters/CharacterDemo.cs index 02422d1a6..7b9f392e5 100644 --- a/Demos/Demos/Characters/CharacterDemo.cs +++ b/Demos/Demos/Characters/CharacterDemo.cs @@ -25,7 +25,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0; characters = new CharacterControllers(BufferPool); - Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); + Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); CreateCharacter(new Vector3(0, 2, -4)); diff --git a/Demos/Demos/ClothDemo.cs b/Demos/Demos/ClothDemo.cs index 8f37720eb..a3e83b152 100644 --- a/Demos/Demos/ClothDemo.cs +++ b/Demos/Demos/ClothDemo.cs @@ -188,7 +188,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0; var filters = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new ClothCallbacks() { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); + Simulation = Simulation.Create(BufferPool, new ClothCallbacks() { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); rolloverInfo = new RolloverInfo(); bool KinematicTopCorners(int rowIndex, int columnIndex, int width, int height) diff --git a/Demos/Demos/CollisionQueryDemo.cs b/Demos/Demos/CollisionQueryDemo.cs index a3da4ce19..dd7895957 100644 --- a/Demos/Demos/CollisionQueryDemo.cs +++ b/Demos/Demos/CollisionQueryDemo.cs @@ -27,7 +27,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Yaw = 0; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(new Box(100, 1, 100)))); diff --git a/Demos/Demos/ColosseumDemo.cs b/Demos/Demos/ColosseumDemo.cs index 3c816907b..588c7c0d6 100644 --- a/Demos/Demos/ColosseumDemo.cs +++ b/Demos/Demos/ColosseumDemo.cs @@ -64,7 +64,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = MathHelper.Pi * 0.2f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); var ringBoxShape = new Box(0.5f, 1, 3); var boxDescription = BodyDescription.CreateDynamic(new Vector3(), ringBoxShape.ComputeInertia(1), Simulation.Shapes.Add(ringBoxShape), 0.01f); diff --git a/Demos/Demos/CompoundTestDemo.cs b/Demos/Demos/CompoundTestDemo.cs index 295374cc7..c19a4df5b 100644 --- a/Demos/Demos/CompoundTestDemo.cs +++ b/Demos/Demos/CompoundTestDemo.cs @@ -16,7 +16,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = MathHelper.Pi * 0.05f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10f, 0)), 8); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10f, 0)), new SolveDescription(8, 1)); using (var compoundBuilder = new CompoundBuilder(BufferPool, Simulation.Shapes, 8)) { diff --git a/Demos/Demos/ContactEventsDemo.cs b/Demos/Demos/ContactEventsDemo.cs index f5f96164a..a31f43e30 100644 --- a/Demos/Demos/ContactEventsDemo.cs +++ b/Demos/Demos/ContactEventsDemo.cs @@ -702,7 +702,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi; events = new ContactEvents(ThreadDispatcher, BufferPool); - Simulation = Simulation.Create(BufferPool, new ContactEventCallbacks(events), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); + Simulation = Simulation.Create(BufferPool, new ContactEventCallbacks(events), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); eventHandler = new EventHandler(Simulation, BufferPool); var listenedBody1 = Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 5, 0), 1, Simulation.Shapes, new Box(1, 2, 3))); diff --git a/Demos/Demos/ContinuousCollisionDetectionDemo.cs b/Demos/Demos/ContinuousCollisionDetectionDemo.cs index 1a5d547e5..509b76642 100644 --- a/Demos/Demos/ContinuousCollisionDetectionDemo.cs +++ b/Demos/Demos/ContinuousCollisionDetectionDemo.cs @@ -56,7 +56,7 @@ public override void Initialize(ContentArchive content, Camera camera) //That's handy for keeping the impact more controlled and visualizing the difference between discrete and continuous modes. Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(120, 1), maximumRecoveryVelocity: 1f), - new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), (1, 8)); + new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); var shape = new Box(1, 1, 1); var inertia = shape.ComputeInertia(1); diff --git a/Demos/Demos/CustomVoxelCollidableDemo.cs b/Demos/Demos/CustomVoxelCollidableDemo.cs index addf8b75b..18b3285be 100644 --- a/Demos/Demos/CustomVoxelCollidableDemo.cs +++ b/Demos/Demos/CustomVoxelCollidableDemo.cs @@ -374,7 +374,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = MathHelper.Pi * 0.05f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); //The narrow phase must be notified about the existence of the new collidable type. For every pair type we want to support, a collision task must be registered. //All of the default engine types are registered upon simulation creation by a call to DefaultTypes.CreateDefaultCollisionTaskRegistry. diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index 0a65e89b4..34a2cdfef 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -706,7 +706,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = MathHelper.Pi * 0.15f; var filters = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0), 0, 0), 4); + Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0), 0, 0), new SolveDescription(1, 4)); var meshContent = content.Load("Content\\newt.obj"); float cellSize = 0.1f; diff --git a/Demos/Demos/PlanetDemo.cs b/Demos/Demos/PlanetDemo.cs index bcbdf1a4b..92c0fd18a 100644 --- a/Demos/Demos/PlanetDemo.cs +++ b/Demos/Demos/PlanetDemo.cs @@ -58,7 +58,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new PlanetaryGravityCallbacks() { PlanetCenter = new Vector3(), Gravity = 100000 }, 4); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new PlanetaryGravityCallbacks() { PlanetCenter = new Vector3(), Gravity = 100000 }, new SolveDescription(1, 4)); Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(new Sphere(50)))); diff --git a/Demos/Demos/PyramidDemo.cs b/Demos/Demos/PyramidDemo.cs index c09a4624b..549df6d84 100644 --- a/Demos/Demos/PyramidDemo.cs +++ b/Demos/Demos/PyramidDemo.cs @@ -22,7 +22,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-30, 8, -110); camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); var boxShape = new Box(1, 1, 1); var boxInertia = boxShape.ComputeInertia(1); diff --git a/Demos/Demos/RagdollDemo.cs b/Demos/Demos/RagdollDemo.cs index 042defdac..48d09ce60 100644 --- a/Demos/Demos/RagdollDemo.cs +++ b/Demos/Demos/RagdollDemo.cs @@ -156,7 +156,7 @@ static RigidPose GetWorldPose(Vector3 localPosition, Quaternion localOrientation QuaternionEx.ConcatenateWithoutOverlap(localOrientation, ragdollPose.Orientation, out worldPose.Orientation); return worldPose; } - static void GetCapsuleForLineSegment(Vector3 start, Vector3 end, float radius, out Capsule capsule, out Vector3 position, out Quaternion orientation) + public static void GetCapsuleForLineSegment(Vector3 start, Vector3 end, float radius, out Capsule capsule, out Vector3 position, out Quaternion orientation) { position = 0.5f * (start + end); @@ -535,7 +535,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = MathHelper.Pi * 0.05f; var collisionFilters = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks { CollisionFilters = collisionFilters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); + Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks { CollisionFilters = collisionFilters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); int ragdollIndex = 0; var spacing = new Vector3(2f, 3, 1); diff --git a/Demos/Demos/RayCastingDemo.cs b/Demos/Demos/RayCastingDemo.cs index 4c186cdb5..b666063ed 100644 --- a/Demos/Demos/RayCastingDemo.cs +++ b/Demos/Demos/RayCastingDemo.cs @@ -63,7 +63,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-20f, 13, -20f); camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = MathHelper.Pi * 0.1f; - Simulation = Simulation.Create(BufferPool, new NoCollisionCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); + Simulation = Simulation.Create(BufferPool, new NoCollisionCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); diff --git a/Demos/Demos/RopeStabilityDemo.cs b/Demos/Demos/RopeStabilityDemo.cs index 0930e2b71..bcd2efb31 100644 --- a/Demos/Demos/RopeStabilityDemo.cs +++ b/Demos/Demos/RopeStabilityDemo.cs @@ -97,7 +97,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //So, even though you can avoid the need for these kinds of hacks, it's good to know that they exist should you find yourself in a circumstance where substepping isn't viable. Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(120, 1), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 2f }, - new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); + new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); rolloverInfo = new RolloverInfo(); var smallWreckingBall = new Sphere(1); diff --git a/Demos/Demos/RopeTwistDemo.cs b/Demos/Demos/RopeTwistDemo.cs index 6a3b1a250..df4e0433a 100644 --- a/Demos/Demos/RopeTwistDemo.cs +++ b/Demos/Demos/RopeTwistDemo.cs @@ -83,7 +83,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var filters = new CollidableProperty(); Simulation = Simulation.Create(BufferPool, new RopeNarrowPhaseCallbacks { ContactSpringiness = new SpringSettings(2000, 1), Filters = filters }, - new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 60); + new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 60)); for (int twistIndex = 0; twistIndex < 1; ++twistIndex) { diff --git a/Demos/Demos/SimpleSelfContainedDemo.cs b/Demos/Demos/SimpleSelfContainedDemo.cs index fe853d7cd..c59d81780 100644 --- a/Demos/Demos/SimpleSelfContainedDemo.cs +++ b/Demos/Demos/SimpleSelfContainedDemo.cs @@ -214,7 +214,7 @@ public static void Run() var bufferPool = new BufferPool(); //The following sets up a simulation with the callbacks defined above, and tells it to use 4 solver substeps per frame with 1 velocity iteration per substep. //It uses the default SubsteppingTimestepper. You could use a custom ITimestepper implementation to customize when stages run relative to each other, or to insert more callbacks. - var simulation = Simulation.Create(bufferPool, new NarrowPhaseCallbacks(), new PoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); + var simulation = Simulation.Create(bufferPool, new NarrowPhaseCallbacks(), new PoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); //Drop a ball on a big static box. var sphere = new Sphere(1); diff --git a/Demos/Demos/SolverContactEnumerationDemo.cs b/Demos/Demos/SolverContactEnumerationDemo.cs index 340963124..dc64f07e3 100644 --- a/Demos/Demos/SolverContactEnumerationDemo.cs +++ b/Demos/Demos/SolverContactEnumerationDemo.cs @@ -26,7 +26,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Yaw = 0; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); //Drop a pyramid on top of the sensor so there are more contacts to look at. var boxShape = new Box(1, 1, 1); diff --git a/Demos/Demos/Sponsors/SponsorDemo.cs b/Demos/Demos/Sponsors/SponsorDemo.cs index 5d043cf4d..bbafb435a 100644 --- a/Demos/Demos/Sponsors/SponsorDemo.cs +++ b/Demos/Demos/Sponsors/SponsorDemo.cs @@ -109,7 +109,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0.4f; characterControllers = new CharacterControllers(BufferPool); - Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characterControllers), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); + Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characterControllers), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); DemoMeshHelper.LoadModel(content, BufferPool, @"Content\newt.obj", new Vector3(-10, 10, -10), out var newtMesh); var newtShape = Simulation.Shapes.Add(newtMesh); diff --git a/Demos/Demos/SubsteppingDemo.cs b/Demos/Demos/SubsteppingDemo.cs index e925e10e2..485f34381 100644 --- a/Demos/Demos/SubsteppingDemo.cs +++ b/Demos/Demos/SubsteppingDemo.cs @@ -24,7 +24,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0; Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(120, 120), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 2f }, - new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), (8, 8)); + new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 8)); rolloverInfo = new RolloverInfo(); { diff --git a/Demos/Demos/SweepDemo.cs b/Demos/Demos/SweepDemo.cs index faf0f1e56..3c975dddf 100644 --- a/Demos/Demos/SweepDemo.cs +++ b/Demos/Demos/SweepDemo.cs @@ -31,7 +31,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(0, 10, 40); camera.Yaw = 0; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); var box = new Box(2f, 2f, 2f); var capsule = new Capsule(1f, 1f); diff --git a/Demos/Demos/Tanks/TankDemo.cs b/Demos/Demos/Tanks/TankDemo.cs index 4aee539af..2839e4951 100644 --- a/Demos/Demos/Tanks/TankDemo.cs +++ b/Demos/Demos/Tanks/TankDemo.cs @@ -49,12 +49,12 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0; bodyProperties = new CollidableProperty(); - //Note that this demo uses only one substep and 8 velocity iterations. + //Note that this demo uses only 1 substep and 8 velocity iterations. //That's partly to show that you can do such a thing, and partly because of (as of 2.4's initial release), there are situations where //contact data can become a little out of date during substepping, since the contact data is only updated once per frame rather than substep (apart from the depths, which are incrementally updated every substep). //In this demo, when using substepping, a wheel resting on another wheel from a destroyed tank can keep rocking back and forth for a long time as the error in contact offsets over substeps can introduce energy. //(I'd like to address this issue more directly to make substepping an unconditional win.) - Simulation = Simulation.Create(BufferPool, new TankCallbacks() { Properties = bodyProperties }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), (1, 8)); + Simulation = Simulation.Create(BufferPool, new TankCallbacks() { Properties = bodyProperties }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); var builder = new CompoundBuilder(BufferPool, Simulation.Shapes, 2); builder.Add(new Box(1.85f, 0.7f, 4.73f), RigidPose.Identity, 10); diff --git a/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs b/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs index e65efaa1f..a9852f7ba 100644 --- a/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs +++ b/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs @@ -20,7 +20,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-20f, 13, -20f); camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = MathHelper.Pi * 0.1f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); var shape = new Sphere(0.5f); var sphereInertia = shape.ComputeInertia(1); diff --git a/Demos/SpecializedTests/CapsuleTestDemo.cs b/Demos/SpecializedTests/CapsuleTestDemo.cs index c746bb6f8..2301467ed 100644 --- a/Demos/SpecializedTests/CapsuleTestDemo.cs +++ b/Demos/SpecializedTests/CapsuleTestDemo.cs @@ -20,7 +20,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //camera.Yaw = MathHelper.Pi ; camera.Yaw = MathHelper.Pi * 3f / 4; //camera.Pitch = MathHelper.Pi * 0.1f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); var shape = new Capsule(.5f, .5f); var localInertia = shape.ComputeInertia(1); diff --git a/Demos/SpecializedTests/CharacterTestDemo.cs b/Demos/SpecializedTests/CharacterTestDemo.cs index f2d3fa0a2..ee3dd2925 100644 --- a/Demos/SpecializedTests/CharacterTestDemo.cs +++ b/Demos/SpecializedTests/CharacterTestDemo.cs @@ -21,7 +21,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = MathHelper.Pi * 0.05f; var masks = new CollidableProperty(); characters = new CharacterControllers(BufferPool); - Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); + Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); var random = new Random(5); for (int i = 0; i < 8192; ++i) diff --git a/Demos/SpecializedTests/ClothLatticeDemo.cs b/Demos/SpecializedTests/ClothLatticeDemo.cs index 881bf5fca..e80965099 100644 --- a/Demos/SpecializedTests/ClothLatticeDemo.cs +++ b/Demos/SpecializedTests/ClothLatticeDemo.cs @@ -20,7 +20,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-120, 30, -120); camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = 0.1f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); //Build a grid of shapes to be connected. var clothNodeShape = new Sphere(0.5f); diff --git a/Demos/SpecializedTests/CompoundBoundTests.cs b/Demos/SpecializedTests/CompoundBoundTests.cs index 435bcfb5f..cbfb6381d 100644 --- a/Demos/SpecializedTests/CompoundBoundTests.cs +++ b/Demos/SpecializedTests/CompoundBoundTests.cs @@ -18,7 +18,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(-10, 0, -10); camera.Yaw = MathHelper.Pi * 3f / 4; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); } diff --git a/Demos/SpecializedTests/CompoundCollisionIndicesTest.cs b/Demos/SpecializedTests/CompoundCollisionIndicesTest.cs index 76fde339f..128d41397 100644 --- a/Demos/SpecializedTests/CompoundCollisionIndicesTest.cs +++ b/Demos/SpecializedTests/CompoundCollisionIndicesTest.cs @@ -72,7 +72,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(0, 4, -6); camera.Yaw = MathHelper.Pi; - Simulation = Simulation.Create(BufferPool, new IndexReportingNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, 0f, 0)), 4); + Simulation = Simulation.Create(BufferPool, new IndexReportingNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, 0f, 0)), new SolveDescription(1, 4)); var builder = new CompoundBuilder(BufferPool, Simulation.Shapes, 4); builder.Add(new Sphere(0.5f), new Vector3(-1, 0, 0), 1); diff --git a/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs b/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs index f651d045b..7c151a5bc 100644 --- a/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs +++ b/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs @@ -19,7 +19,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = 0; Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(30, 1), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 2f }, - new DemoPoseIntegratorCallbacks(new Vector3(0, -0.1f, 0), 0, 0), 4); + new DemoPoseIntegratorCallbacks(new Vector3(0, -0.1f, 0), 0, 0), new SolveDescription(1, 4)); var shapeA = new Box(.75f, 1, .5f); var shapeIndexA = Simulation.Shapes.Add(shapeA); diff --git a/Demos/SpecializedTests/ConstraintTestDemo.cs b/Demos/SpecializedTests/ConstraintTestDemo.cs index 6b7c509b0..a6cf9cbda 100644 --- a/Demos/SpecializedTests/ConstraintTestDemo.cs +++ b/Demos/SpecializedTests/ConstraintTestDemo.cs @@ -24,8 +24,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(25, 4, 40); camera.Yaw = 0; Simulation = Simulation.Create(BufferPool, - new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(30, 1), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 2f }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); - //new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(30, 1), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 2f }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper(), 8); + new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(30, 1), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 2f }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); var shapeA = new Box(.75f, 1, .5f); var shapeIndexA = Simulation.Shapes.Add(shapeA); diff --git a/Demos/SpecializedTests/ConvexHullTestDemo.cs b/Demos/SpecializedTests/ConvexHullTestDemo.cs index 87a1d9bdf..d8bf4353b 100644 --- a/Demos/SpecializedTests/ConvexHullTestDemo.cs +++ b/Demos/SpecializedTests/ConvexHullTestDemo.cs @@ -27,7 +27,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Yaw = 0; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); //var meshContent = content.Load("Content\\newt.obj"); diff --git a/Demos/SpecializedTests/CylinderTestDemo.cs b/Demos/SpecializedTests/CylinderTestDemo.cs index 012c46a09..a8c3b5c21 100644 --- a/Demos/SpecializedTests/CylinderTestDemo.cs +++ b/Demos/SpecializedTests/CylinderTestDemo.cs @@ -170,7 +170,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0; camera.Yaw = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, 0f, 0)), 4); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, 0f, 0)), new SolveDescription(1, 4)); var cylinderShape = new Cylinder(1f, .2f); var cylinder = BodyDescription.CreateDynamic(new Vector3(10f, 3, 0), cylinderShape.ComputeInertia(1), new(Simulation.Shapes.Add(cylinderShape), ContinuousDetection.CreatePassive(1000f, 1000f)), 0.01f); diff --git a/Demos/SpecializedTests/GyroscopeTestDemo.cs b/Demos/SpecializedTests/GyroscopeTestDemo.cs index 16ce9e324..a68400010 100644 --- a/Demos/SpecializedTests/GyroscopeTestDemo.cs +++ b/Demos/SpecializedTests/GyroscopeTestDemo.cs @@ -55,7 +55,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0; //Note the lack of damping- we want the gyroscope to keep spinning. - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new GyroscopicIntegratorCallbacks(new Vector3(0, -10, 0), 0f, 0f), 8); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new GyroscopicIntegratorCallbacks(new Vector3(0, -10, 0), 0f, 0f), new SolveDescription(1, 8)); Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(new Box(100, 1, 100)))); diff --git a/Demos/SpecializedTests/Media/BedsheetDemo.cs b/Demos/SpecializedTests/Media/BedsheetDemo.cs index 25026a4ca..171b713ec 100644 --- a/Demos/SpecializedTests/Media/BedsheetDemo.cs +++ b/Demos/SpecializedTests/Media/BedsheetDemo.cs @@ -107,7 +107,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = MathF.PI * 0.1f; var filters = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new ClothCallbacks() { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -50, 0)), 4); + Simulation = Simulation.Create(BufferPool, new ClothCallbacks() { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -50, 0)), new SolveDescription(1, 4)); rolloverInfo = new RolloverInfo(); bool FullyDynamic(int rowIndex, int columnIndex, int width, int height) diff --git a/Demos/SpecializedTests/Media/ColosseumVideoDemo.cs b/Demos/SpecializedTests/Media/ColosseumVideoDemo.cs index fc0f8431e..a0e612f72 100644 --- a/Demos/SpecializedTests/Media/ColosseumVideoDemo.cs +++ b/Demos/SpecializedTests/Media/ColosseumVideoDemo.cs @@ -68,7 +68,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = MathHelper.Pi * 0.2f; characters = new CharacterControllers(BufferPool); - Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); + Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); var ringBoxShape = new Box(0.5f, 1.5f, 3); var boxDescription = BodyDescription.CreateDynamic(new Vector3(), ringBoxShape.ComputeInertia(1), Simulation.Shapes.Add(ringBoxShape), 0.01f); diff --git a/Demos/SpecializedTests/Media/NewtDemandingSacrificeVideoDemo.cs b/Demos/SpecializedTests/Media/NewtDemandingSacrificeVideoDemo.cs index 172a96aa8..a6507ec9d 100644 --- a/Demos/SpecializedTests/Media/NewtDemandingSacrificeVideoDemo.cs +++ b/Demos/SpecializedTests/Media/NewtDemandingSacrificeVideoDemo.cs @@ -20,7 +20,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = MathHelper.Pi * -0.05f; filters = new CollidableProperty(BufferPool); - Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks() { CollisionFilters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); + Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks() { CollisionFilters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), Simulation.Shapes.Add(new Box(1500, 1, 1500)))); Simulation.Statics.Add(new StaticDescription(new Vector3(0, 10, 0), Simulation.Shapes.Add(new Box(70, 20, 80)))); diff --git a/Demos/SpecializedTests/Media/NewtVideoDemo.cs b/Demos/SpecializedTests/Media/NewtVideoDemo.cs index 79f410719..69322f3cf 100644 --- a/Demos/SpecializedTests/Media/NewtVideoDemo.cs +++ b/Demos/SpecializedTests/Media/NewtVideoDemo.cs @@ -21,7 +21,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = MathHelper.Pi * 0.15f; var filters = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); + Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); var meshContent = content.Load("Content\\newt.obj"); float cellSize = 0.1f; diff --git a/Demos/SpecializedTests/Media/PyramidVideoDemo.cs b/Demos/SpecializedTests/Media/PyramidVideoDemo.cs index 946dda4b6..50bebe119 100644 --- a/Demos/SpecializedTests/Media/PyramidVideoDemo.cs +++ b/Demos/SpecializedTests/Media/PyramidVideoDemo.cs @@ -22,7 +22,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-70, 8, 318); camera.Yaw = MathHelper.Pi * 1f / 4; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); var boxShape = new Box(1, 1, 1); var boxInertia = boxShape.ComputeInertia(1); diff --git a/Demos/SpecializedTests/Media/ShrinkwrappedNewtsVideoDemo.cs b/Demos/SpecializedTests/Media/ShrinkwrappedNewtsVideoDemo.cs index 7a636ebb7..aedc9d484 100644 --- a/Demos/SpecializedTests/Media/ShrinkwrappedNewtsVideoDemo.cs +++ b/Demos/SpecializedTests/Media/ShrinkwrappedNewtsVideoDemo.cs @@ -18,7 +18,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = 3 * MathHelper.Pi / 4; camera.Pitch = 0;// MathHelper.Pi * 0.15f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); var meshContent = content.Load("Content\\newt.obj"); diff --git a/Demos/SpecializedTests/MeshMeshTestDemo.cs b/Demos/SpecializedTests/MeshMeshTestDemo.cs index 2bb51e784..9197fc1dd 100644 --- a/Demos/SpecializedTests/MeshMeshTestDemo.cs +++ b/Demos/SpecializedTests/MeshMeshTestDemo.cs @@ -21,7 +21,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(0, 8, -10); camera.Yaw = MathHelper.Pi; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); DemoMeshHelper.LoadModel(content, BufferPool, @"Content\newt.obj", Vector3.One, out var mesh); var approximateInertia = new Box(2.5f, 1, 4).ComputeInertia(1); diff --git a/Demos/SpecializedTests/MeshReductionTestDemo.cs b/Demos/SpecializedTests/MeshReductionTestDemo.cs index 6a45d1110..590156f6d 100644 --- a/Demos/SpecializedTests/MeshReductionTestDemo.cs +++ b/Demos/SpecializedTests/MeshReductionTestDemo.cs @@ -25,7 +25,7 @@ public override void Initialize(ContentArchive content, Camera camera) Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks() { ContactSpringiness = new(30, 1), MaximumRecoveryVelocity = 2, FrictionCoefficient = 0 }, - new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); + new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); var builder = new CompoundBuilder(BufferPool, Simulation.Shapes, 2); builder.Add(new Box(1.85f, 0.7f, 4.73f), RigidPose.Identity, 10); diff --git a/Demos/SpecializedTests/MeshSerializationTestDemo.cs b/Demos/SpecializedTests/MeshSerializationTestDemo.cs index dd979cbee..9639edb4e 100644 --- a/Demos/SpecializedTests/MeshSerializationTestDemo.cs +++ b/Demos/SpecializedTests/MeshSerializationTestDemo.cs @@ -17,7 +17,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); var startTime = Stopwatch.GetTimestamp(); DemoMeshHelper.CreateDeformedPlane(1025, 1025, (x, y) => new Vector3(x * 0.125f, MathF.Sin(x) + MathF.Sin(y), y * 0.125f), Vector3.One, BufferPool, out var originalMesh); diff --git a/Demos/SpecializedTests/MeshTestDemo.cs b/Demos/SpecializedTests/MeshTestDemo.cs index 4785c4761..25a70b375 100644 --- a/Demos/SpecializedTests/MeshTestDemo.cs +++ b/Demos/SpecializedTests/MeshTestDemo.cs @@ -19,7 +19,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //camera.Yaw = MathHelper.Pi ; camera.Yaw = MathHelper.Pi * 3f / 4; //camera.Pitch = MathHelper.PiOver2 * 0.999f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); var box = new Box(1f, 3f, 2f); var capsule = new Capsule(1f, 1f); diff --git a/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs b/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs index 5897e5750..e357b57b2 100644 --- a/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs +++ b/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs @@ -22,7 +22,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-30, 8, -110); camera.Yaw = MathHelper.Pi * 3f / 4; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); var boxShape = new Box(1, 1, 1); var boxInertia = boxShape.ComputeInertia(1); diff --git a/Demos/SpecializedTests/RagdollTubeDemo.cs b/Demos/SpecializedTests/RagdollTubeDemo.cs index 06acb8b55..6a1ae1979 100644 --- a/Demos/SpecializedTests/RagdollTubeDemo.cs +++ b/Demos/SpecializedTests/RagdollTubeDemo.cs @@ -21,7 +21,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi; camera.Pitch = 0; var filters = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks { CollisionFilters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); + Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks { CollisionFilters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); int ragdollIndex = 0; var spacing = new Vector3(1.7f, 1.8f, 0.5f); diff --git a/Demos/SpecializedTests/ShapePileTestDemo.cs b/Demos/SpecializedTests/ShapePileTestDemo.cs index 032502bee..0881bfc34 100644 --- a/Demos/SpecializedTests/ShapePileTestDemo.cs +++ b/Demos/SpecializedTests/ShapePileTestDemo.cs @@ -20,7 +20,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //camera.Yaw = MathHelper.Pi ; camera.Yaw = MathHelper.Pi * 3f / 4; //camera.Pitch = MathHelper.PiOver2 * 0.999f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); Simulation.Deterministic = true; var sphere = new Sphere(1.5f); diff --git a/Demos/SpecializedTests/SolverBatchTestDemo.cs b/Demos/SpecializedTests/SolverBatchTestDemo.cs index d45c04259..5bd8bb192 100644 --- a/Demos/SpecializedTests/SolverBatchTestDemo.cs +++ b/Demos/SpecializedTests/SolverBatchTestDemo.cs @@ -20,7 +20,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-120, 30, -120); camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = 0.1f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); Simulation.Solver.VelocityIterationCount = 8; //Build a grid of shapes to be connected. diff --git a/Demos/SpecializedTests/TriangleRayTestDemo.cs b/Demos/SpecializedTests/TriangleRayTestDemo.cs index 5617fb67a..2f06019be 100644 --- a/Demos/SpecializedTests/TriangleRayTestDemo.cs +++ b/Demos/SpecializedTests/TriangleRayTestDemo.cs @@ -94,7 +94,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-30, 8, -60); camera.Yaw = MathHelper.Pi * 3f / 4; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); Triangle triangle; triangle.A = new Vector3(0, 0, 0); diff --git a/Demos/SpecializedTests/TriangleTestDemo.cs b/Demos/SpecializedTests/TriangleTestDemo.cs index 3a1a9c0f6..f07cb591e 100644 --- a/Demos/SpecializedTests/TriangleTestDemo.cs +++ b/Demos/SpecializedTests/TriangleTestDemo.cs @@ -92,7 +92,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks() { ContactSpringiness = new BepuPhysics.Constraints.SpringSettings(30, 1), FrictionCoefficient = 1, MaximumRecoveryVelocity = 5 }, - new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), 4); + new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); //var triangleDescription = new StaticDescription //{ diff --git a/Demos/SpecializedTests/VolumeQueryTests.cs b/Demos/SpecializedTests/VolumeQueryTests.cs index e4a7c76f7..732db8230 100644 --- a/Demos/SpecializedTests/VolumeQueryTests.cs +++ b/Demos/SpecializedTests/VolumeQueryTests.cs @@ -61,7 +61,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-20f, 13, -20f); camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = MathHelper.Pi * 0.1f; - Simulation = Simulation.Create(BufferPool, new NoCollisionCallbacks(), new DemoPoseIntegratorCallbacks(), 4); + Simulation = Simulation.Create(BufferPool, new NoCollisionCallbacks(), new DemoPoseIntegratorCallbacks(), new SolveDescription(1, 4)); var sphere = new Sphere(0.5f); var shapeIndex = Simulation.Shapes.Add(sphere); From 7b4af30b3a7c78b9c18149dff4752124d536dda6 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 24 Dec 2021 12:59:41 -0600 Subject: [PATCH 355/947] GL path updated. --- DemoRenderer.GL/ShapeDrawing/ShapesExtractor.cs | 9 +++++---- Demos.GL/Demos.csproj | 2 +- Demos/Demos/RopeTwistDemo.cs | 6 ------ 3 files changed, 6 insertions(+), 11 deletions(-) diff --git a/DemoRenderer.GL/ShapeDrawing/ShapesExtractor.cs b/DemoRenderer.GL/ShapeDrawing/ShapesExtractor.cs index 704a9f924..248ad93df 100644 --- a/DemoRenderer.GL/ShapeDrawing/ShapesExtractor.cs +++ b/DemoRenderer.GL/ShapeDrawing/ShapesExtractor.cs @@ -235,10 +235,10 @@ void AddBodyShape(Shapes shapes, Bodies bodies, int setIndex, int indexInSet) //3) Activity state //The handle is hashed to get variation. ref var activity = ref set.Activity[indexInSet]; - ref var inertia = ref set.LocalInertias[indexInSet]; + ref var state = ref set.SolverStates[indexInSet]; Vector3 color; Helpers.UnpackColor((uint)HashHelper.Rehash(handle.Value), out Vector3 colorVariation); - if (Bodies.IsKinematic(inertia)) + if (Bodies.IsKinematic(state.Inertia.Local)) { var kinematicBase = new Vector3(0, 0.609f, 0.37f); var kinematicVariationSpan = new Vector3(0.1f, 0.1f, 0.1f); @@ -265,7 +265,7 @@ void AddBodyShape(Shapes shapes, Bodies bodies, int setIndex, int indexInSet) color *= sleepTint; } - AddShape(shapes, set.Collidables[indexInSet].Shape, ref set.Poses[indexInSet], color); + AddShape(shapes, set.Collidables[indexInSet].Shape, ref state.Motion.Pose, color); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -277,7 +277,8 @@ void AddStaticShape(Shapes shapes, Statics statics, int index) var staticBase = new Vector3(0.1f, 0.057f, 0.014f); var staticVariationSpan = new Vector3(0.07f, 0.07f, 0.03f); var color = staticBase + staticVariationSpan * colorVariation; - AddShape(shapes, statics.Collidables[index].Shape, ref statics.Poses[index], color); + ref var collidable = ref statics[index]; + AddShape(shapes, collidable.Shape, ref collidable.Pose, color); } public void AddInstances(Simulation simulation, IThreadDispatcher threadDispatcher = null) diff --git a/Demos.GL/Demos.csproj b/Demos.GL/Demos.csproj index b2e46843d..7a83761f9 100644 --- a/Demos.GL/Demos.csproj +++ b/Demos.GL/Demos.csproj @@ -1,7 +1,7 @@  Exe - net5.0 + net6.0 True Debug;Release;ReleaseStrip latest diff --git a/Demos/Demos/RopeTwistDemo.cs b/Demos/Demos/RopeTwistDemo.cs index df4e0433a..e72c0796d 100644 --- a/Demos/Demos/RopeTwistDemo.cs +++ b/Demos/Demos/RopeTwistDemo.cs @@ -2,17 +2,11 @@ using BepuPhysics.Collidables; using BepuPhysics.CollisionDetection; using BepuPhysics.Constraints; -using BepuUtilities; using DemoContentLoader; using DemoRenderer; -using DemoRenderer.UI; -using DemoUtilities; -using SharpDX.Direct3D11; using System; -using System.Collections.Generic; using System.Numerics; using System.Runtime.CompilerServices; -using System.Text; namespace Demos.Demos { From 45f47a4084c39104a14fcedd3771031eef127e29 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 24 Dec 2021 19:36:23 -0600 Subject: [PATCH 356/947] Bumped to .NET 6. --- BepuPhysics/BepuPhysics.csproj | 2 +- BepuUtilities/BepuUtilities.csproj | 2 +- DemoContentBuilder/DemoContentBuilder.csproj | 2 +- DemoContentLoader/DemoContentLoader.csproj | 2 +- DemoRenderer.GL/DemoRenderer.csproj | 2 +- DemoRenderer/DemoRenderer.csproj | 6 +++--- DemoUtilities/DemoUtilities.csproj | 2 +- Demos/Demos.csproj | 4 ++-- 8 files changed, 11 insertions(+), 11 deletions(-) diff --git a/BepuPhysics/BepuPhysics.csproj b/BepuPhysics/BepuPhysics.csproj index fdb7cee40..99e9a8e04 100644 --- a/BepuPhysics/BepuPhysics.csproj +++ b/BepuPhysics/BepuPhysics.csproj @@ -1,6 +1,6 @@  - net5.0 + net6.0 2.4.0-beta9 Bepu Entertainment LLC Ross Nordby diff --git a/BepuUtilities/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj index 1ae2adc1e..d097dabb8 100644 --- a/BepuUtilities/BepuUtilities.csproj +++ b/BepuUtilities/BepuUtilities.csproj @@ -2,7 +2,7 @@ BepuUtilities BepuUtilities - net5.0 + net6.0 2.4.0-beta9 Bepu Entertainment LLC Ross Nordby diff --git a/DemoContentBuilder/DemoContentBuilder.csproj b/DemoContentBuilder/DemoContentBuilder.csproj index bc3518ae2..928edc10a 100644 --- a/DemoContentBuilder/DemoContentBuilder.csproj +++ b/DemoContentBuilder/DemoContentBuilder.csproj @@ -2,7 +2,7 @@ Exe - net5.0 + net6.0 x64 latest true diff --git a/DemoContentLoader/DemoContentLoader.csproj b/DemoContentLoader/DemoContentLoader.csproj index f544a3068..4953d4e7e 100644 --- a/DemoContentLoader/DemoContentLoader.csproj +++ b/DemoContentLoader/DemoContentLoader.csproj @@ -1,7 +1,7 @@  - net5.0 + net6.0 latest True diff --git a/DemoRenderer.GL/DemoRenderer.csproj b/DemoRenderer.GL/DemoRenderer.csproj index ecf92ad0a..8f42e7fa0 100644 --- a/DemoRenderer.GL/DemoRenderer.csproj +++ b/DemoRenderer.GL/DemoRenderer.csproj @@ -1,6 +1,6 @@  - net5.0 + net6.0 latest True diff --git a/DemoRenderer/DemoRenderer.csproj b/DemoRenderer/DemoRenderer.csproj index dbe8bd341..9afeb8667 100644 --- a/DemoRenderer/DemoRenderer.csproj +++ b/DemoRenderer/DemoRenderer.csproj @@ -1,13 +1,13 @@  - net5.0 + net6.0 latest True - - + + diff --git a/DemoUtilities/DemoUtilities.csproj b/DemoUtilities/DemoUtilities.csproj index 112e72e6a..00c05bfeb 100644 --- a/DemoUtilities/DemoUtilities.csproj +++ b/DemoUtilities/DemoUtilities.csproj @@ -1,7 +1,7 @@  - net5.0 + net6.0 latest true diff --git a/Demos/Demos.csproj b/Demos/Demos.csproj index 70b17f982..f949c6848 100644 --- a/Demos/Demos.csproj +++ b/Demos/Demos.csproj @@ -9,8 +9,8 @@ - - + + From 816b34e4ecc2b1c8b2d049f12ac5c164ee2e360d Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 24 Dec 2021 19:47:59 -0600 Subject: [PATCH 357/947] Update content pipeline dependency. --- DemoContentBuilder/DemoContentBuilder.csproj | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/DemoContentBuilder/DemoContentBuilder.csproj b/DemoContentBuilder/DemoContentBuilder.csproj index 928edc10a..f01857a7c 100644 --- a/DemoContentBuilder/DemoContentBuilder.csproj +++ b/DemoContentBuilder/DemoContentBuilder.csproj @@ -14,7 +14,7 @@ - + From a40dba61b9fe96c2b871cbba7c97b0dba0ca0de2 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 24 Dec 2021 19:51:18 -0600 Subject: [PATCH 358/947] Fixed texture2dbuilder. --- DemoContentBuilder/Texture2DBuilder.cs | 37 ++++++++++---------------- 1 file changed, 14 insertions(+), 23 deletions(-) diff --git a/DemoContentBuilder/Texture2DBuilder.cs b/DemoContentBuilder/Texture2DBuilder.cs index dfa72ca2f..e99ab8bb3 100644 --- a/DemoContentBuilder/Texture2DBuilder.cs +++ b/DemoContentBuilder/Texture2DBuilder.cs @@ -15,31 +15,22 @@ public static class Texture2DBuilder { public unsafe static Texture2DContent Build(Stream dataStream) { - using (var rawImage = SixLabors.ImageSharp.Image.Load(dataStream)) - using (var image = rawImage.CloneAs()) + using var rawImage = SixLabors.ImageSharp.Image.Load(dataStream); + using var image = rawImage.CloneAs(); + //We're only supporting R8G8B8A8 right now, so texel size in bytes is always 4. + //We don't compute mips during at content time. We could, but... there's not much reason to. + //The font builder does because it uses a nonstandard mip process, but this builder is expected to be used to with normal data. + var content = new Texture2DContent(image.Width, image.Height, 1, sizeof(Rgba32)); + var data = (Rgba32*)content.Pin(); + //Copy the image data into the Texture2DContent. + for (int rowIndex = 0; rowIndex < image.Height; ++rowIndex) { - var pixels = image.GetPixelSpan(); - var imageData = new int[pixels.Length]; - fixed (int* imageDataPointer = imageData) - { - var casted = new Span(imageDataPointer, imageData.Length); - pixels.CopyTo(casted); - } - //We're only supporting R8G8B8A8 right now, so texel size in bytes is always 4. - //We don't compute mips during at content time. We could, but... there's not much reason to. - //The font builder does because it uses a nonstandard mip process, but this builder is expected to be used to with normal data. - var content = new Texture2DContent(image.Width, image.Height, 1, sizeof(Rgba32)); - var data = (Rgba32*)content.Pin(); - //Copy the image data into the Texture2DContent. - for (int rowIndex = 0; rowIndex < image.Height; ++rowIndex) - { - var sourceRow = image.GetPixelRowSpan(rowIndex); - var targetRow = data + content.GetRowOffsetForMip0(rowIndex); - Unsafe.CopyBlockUnaligned(ref *(byte*)targetRow, ref Unsafe.As(ref sourceRow[0]), (uint)(sizeof(Rgba32) * image.Width)); - } - content.Unpin(); - return content; + var sourceRow = image.GetPixelRowSpan(rowIndex); + var targetRow = data + content.GetRowOffsetForMip0(rowIndex); + Unsafe.CopyBlockUnaligned(ref *(byte*)targetRow, ref Unsafe.As(ref sourceRow[0]), (uint)(sizeof(Rgba32) * image.Width)); } + content.Unpin(); + return content; } } } From d59612258084d9d528794befa8a259050b7da874 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 24 Dec 2021 20:00:33 -0600 Subject: [PATCH 359/947] Updated references to .NET version. --- Documentation/Building.md | 2 +- README.md | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/Documentation/Building.md b/Documentation/Building.md index 31e95e46b..51110963b 100644 --- a/Documentation/Building.md +++ b/Documentation/Building.md @@ -8,7 +8,7 @@ The library tends to use the latest C# language features. At the time of writing `BepuPhysics.csproj` uses T4 templates for code generation in a few places. If changes are made to the templates, you'll need a build pipeline that can process them (like Visual Studio). The repository contains the original generated .cs files, so if no changes are made, the templates do not need to be evaluated. -The libraries target .NET 5. +The libraries target .NET 6. ## Demos diff --git a/README.md b/README.md index ae3b4e2bf..d2af1157d 100644 --- a/README.md +++ b/README.md @@ -3,9 +3,9 @@ This is the repo for the bepuphysics v2 library, a complete rewrite of the C# 3d rigid body physics engine [BEPUphysics v1](https://github.com/bepu/bepuphysics1). -The BepuPhysics and BepuUtilities libraries target .NET 5 and should work on any supported platform. The demos target .NET 6.0 and use DX11 by default. There is also an [OpenGL version of the demos](https://github.com/bepu/bepuphysics2/tree/master/Demos.GL) for other platforms that you can run from the command line in the repository root using `dotnet run --project Demos.GL/Demos.csproj -c Release`. +The BepuPhysics and BepuUtilities libraries target .NET 6 and should work on any supported platform. The demos use DX11 by default. There is also an [OpenGL version of the demos](https://github.com/bepu/bepuphysics2/tree/master/Demos.GL) for other platforms that you can run from the command line in the repository root using `dotnet run --project Demos.GL/Demos.csproj -c Release`. -The physics engine heavily uses System.Numerics.Vectors types, so to get good performance, you'll need a compiler which can consume those types (like RyuJIT). +The physics engine heavily uses `System.Numerics.Vectors` types, so to get good performance, you'll need a compiler which can consume those types (like RyuJIT). To build the source, you'll need a recent version of Visual Studio with the .NET desktop development workload installed. Demos.sln references all relevant projects. For more information, see [Building](Documentation/Building.md). From 71b11d832702e5c3b539a704234aeb4a9aba8e0e Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 24 Dec 2021 20:01:50 -0600 Subject: [PATCH 360/947] Update dotnet-core.yml Workflow update. --- .github/workflows/dotnet-core.yml | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/.github/workflows/dotnet-core.yml b/.github/workflows/dotnet-core.yml index 32d08bb70..2a7b78c28 100644 --- a/.github/workflows/dotnet-core.yml +++ b/.github/workflows/dotnet-core.yml @@ -15,9 +15,7 @@ jobs: - uses: actions/checkout@v2 - uses: actions/setup-dotnet@v1 with: - dotnet-version: | - 5.x - 6.x + dotnet-version: '6.x' - name: Install dependencies run: | dotnet restore DemoContentBuilder From 87ca89a42a9a8c564fc986113b758ead89f1811f Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 26 Dec 2021 19:44:24 -0600 Subject: [PATCH 361/947] BufferPool now uses NativeMemory. --- BepuUtilities/Memory/BufferPool.cs | 220 ++++--------------- BepuUtilities/Memory/IUnmanagedMemoryPool.cs | 2 +- 2 files changed, 45 insertions(+), 177 deletions(-) diff --git a/BepuUtilities/Memory/BufferPool.cs b/BepuUtilities/Memory/BufferPool.cs index 72a165fd1..2d5488458 100644 --- a/BepuUtilities/Memory/BufferPool.cs +++ b/BepuUtilities/Memory/BufferPool.cs @@ -14,92 +14,9 @@ namespace BepuUtilities.Memory /// This currently works by allocating large managed arrays and pinning them under the assumption that they'll end up in the large object heap. public class BufferPool : IUnmanagedMemoryPool, IDisposable { - unsafe struct Block + unsafe struct PowerPool { - public byte[] Array; - public GCHandle Handle; - public byte* Pointer; - - public Block(int blockSize) - { - //While the runtime does have some alignment guarantees, we hedge against the possibility that the runtime could change (or another runtime is in use), - //or that the runtime isn't aligning to a size sufficiently large for wide SIMD types, or some type expects cache line size alignment. - //I suspect that the combination of the jit's tendency to use unaligned instructions regardless and modern processors' performance on unaligned instructions - //will make this *almost* irrelevant, but it costs roughly nothing. - //Suballocations from the block will always occur on pow2 boundaries, so the only way for a suballocation to violate this alignment is if an individual - //suballocation is smaller than the alignment- in which case it doesn't require the alignment to be that wide. Also, since the alignment and - //suballocations are both pow2 sized, they won't drift out of sync. - //We pick 128 bytes to allow alignment with cache line pairs: https://www.intel.com/content/dam/www/public/us/en/documents/manuals/64-ia-32-architectures-optimization-manual.pdf#page=162 - const int allocationAlignment = 128; - Array = new byte[blockSize + allocationAlignment]; - Handle = GCHandle.Alloc(Array, GCHandleType.Pinned); - Pointer = (byte*)Handle.AddrOfPinnedObject(); - var mask = allocationAlignment - 1; - var offset = (uint)Pointer & mask; - Pointer += allocationAlignment - offset; - } - - - public byte* Allocate(int indexInBlock, int suballocationSize) - { - Debug.Assert(Allocated); - Debug.Assert(Pinned); - Debug.Assert(indexInBlock >= 0 && indexInBlock * suballocationSize < Array.Length); - return Pointer + indexInBlock * suballocationSize; - } - - public bool Allocated - { - get - { - return Array != null; - } - } - - public bool Pinned - { - get - { - return Array != null && Handle.IsAllocated; - } - set - { - - Debug.Assert(Array != null); - if (value) - { - Debug.Assert(!Handle.IsAllocated); - Handle = GCHandle.Alloc(Array); - Pointer = (byte*)Handle.AddrOfPinnedObject(); - } - else - { - Debug.Assert(Handle.IsAllocated); - Handle.Free(); - Pointer = null; - } - } - } - - /// - /// Unpins and drops the reference to the underlying array. - /// - public void Clear() - { - Debug.Assert(Array != null); - //It's not guaranteed that the array is actually pinned if we support unpinning. - if (Handle.IsAllocated) - { - Pinned = false; - } - Array = null; - } - - } - - struct PowerPool - { - public Block[] Blocks; + public byte*[] Blocks; /// /// Pool of slots available to this power level. /// @@ -130,7 +47,7 @@ public PowerPool(int power, int minimumBlockSize, int expectedPooledCount) SuballocationsPerBlock = BlockSize / SuballocationSize; SuballocationsPerBlockShift = SpanHelper.GetContainingPowerOf2(SuballocationsPerBlock); SuballocationsPerBlockMask = (1 << SuballocationsPerBlockShift) - 1; - Blocks = new Block[1]; + Blocks = new byte*[1]; BlockCount = 0; #if DEBUG @@ -141,16 +58,42 @@ public PowerPool(int power, int minimumBlockSize, int expectedPooledCount) #endif } + void Resize(int newSize) + { + var newBlocks = new byte*[newSize]; + Array.Copy(Blocks, newBlocks, Blocks.Length); + Blocks = newBlocks; + } + + void AllocateBlock(int blockIndex) + { +#if DEBUG + for (int i = 0; i < blockIndex; ++i) + { + Debug.Assert(Blocks[i] != null, "If we are allocating a block, all previous blocks should be allocated already."); + } +#endif + Debug.Assert(Blocks[blockIndex] == null); + //Suballocations from the block will always occur on pow2 boundaries, so the only way for a suballocation to violate this alignment is if an individual + //suballocation is smaller than the alignment- in which case it doesn't require the alignment to be that wide. Also, since the alignment and + //suballocations are both pow2 sized, they won't drift out of sync. + //We pick 128 bytes to allow alignment with cache line pairs: https://www.intel.com/content/dam/www/public/us/en/documents/manuals/64-ia-32-architectures-optimization-manual.pdf#page=162 + Blocks[blockIndex] = (byte*)NativeMemory.AlignedAlloc((nuint)BlockSize, 128); + BlockCount = blockIndex + 1; + } + public void EnsureCapacity(int capacity) { var neededBlockCount = (int)Math.Ceiling((double)capacity / BlockSize); if (BlockCount < neededBlockCount) { if (neededBlockCount > Blocks.Length) - Array.Resize(ref Blocks, neededBlockCount); + { + Resize(neededBlockCount); + } for (int i = BlockCount; i < neededBlockCount; ++i) { - Blocks[i] = new Block(BlockSize); + AllocateBlock(i); } BlockCount = neededBlockCount; } @@ -161,7 +104,7 @@ public void EnsureCapacity(int capacity) { var blockIndex = slot >> SuballocationsPerBlockShift; var indexInBlock = slot & SuballocationsPerBlockMask; - return Blocks[blockIndex].Pointer + indexInBlock * SuballocationSize; + return Blocks[blockIndex] + indexInBlock * SuballocationSize; } public unsafe void Take(out RawBuffer buffer) @@ -170,23 +113,15 @@ public unsafe void Take(out RawBuffer buffer) var blockIndex = slot >> SuballocationsPerBlockShift; if (blockIndex >= Blocks.Length) { - Array.Resize(ref Blocks, 1 << SpanHelper.GetContainingPowerOf2(blockIndex + 1)); + Resize(1 << SpanHelper.GetContainingPowerOf2(blockIndex + 1)); } if (blockIndex >= BlockCount) { -#if DEBUG - for (int i = 0; i < blockIndex; ++i) - { - Debug.Assert(Blocks[i].Allocated, "If a block index is found to exceed the current block count, then every block preceding the index should be allocated."); - } -#endif - BlockCount = blockIndex + 1; - Debug.Assert(!Blocks[blockIndex].Allocated); - Blocks[blockIndex] = new Block(BlockSize); + AllocateBlock(blockIndex); } var indexInBlock = slot & SuballocationsPerBlockMask; - buffer = new RawBuffer(Blocks[blockIndex].Allocate(indexInBlock, SuballocationSize), SuballocationSize, (Power << IdPowerShift) | slot); + buffer = new RawBuffer(Blocks[blockIndex] + indexInBlock * SuballocationSize, SuballocationSize, (Power << IdPowerShift) | slot); Debug.Assert(buffer.Id >= 0 && Power >= 0 && Power < 32, "Slot/power should be safely encoded in a 32 bit integer."); #if DEBUG const int maximumOutstandingCount = 1 << 26; @@ -217,12 +152,12 @@ internal unsafe void ValidateBufferIsContained(ref RawBuffer buffer) "A buffer taken from a pool should have a specific size."); Debug.Assert(blockIndex >= 0 && blockIndex < BlockCount, "The block pointed to by a returned buffer should actually exist within the pool."); - var memoryOffset = buffer.Memory - Blocks[blockIndex].Pointer; - Debug.Assert(memoryOffset >= 0 && memoryOffset < Blocks[blockIndex].Array.Length, + var memoryOffset = buffer.Memory - Blocks[blockIndex]; + Debug.Assert(memoryOffset >= 0 && memoryOffset < BlockSize, "If a raw buffer points to a given block as its source, the address should be within the block's memory region."); - Debug.Assert(Blocks[blockIndex].Pointer + indexInAllocatorBlock * SuballocationSize == buffer.Memory, + Debug.Assert(Blocks[blockIndex] + indexInAllocatorBlock * SuballocationSize == buffer.Memory, "The implied address of a buffer in its block should match its actual address."); - Debug.Assert(buffer.Length + indexInAllocatorBlock * SuballocationSize <= Blocks[blockIndex].Array.Length, + Debug.Assert(buffer.Length + indexInAllocatorBlock * SuballocationSize <= BlockSize, "The extent of the buffer should fit within the block."); } @@ -262,7 +197,8 @@ public void Clear() #endif for (int i = 0; i < BlockCount; ++i) { - Blocks[i].Clear(); + NativeMemory.AlignedFree(Blocks[i]); + Blocks[i] = null; } Slots.Clear(); BlockCount = 0; @@ -302,7 +238,6 @@ public BufferPool(int minimumBlockAllocationSize = 131072, int expectedPooledRes public void EnsureCapacityForPower(int byteCount, int power) { SpanHelper.ValidatePower(power); - ValidatePinnedState(true); pools[power].EnsureCapacity(byteCount); } @@ -372,12 +307,11 @@ public void Take(int count, out Buffer buffer) where T : unmanaged /// /// Takes a buffer large enough to contain a number of bytes given by a power, where the number of bytes is 2^power. /// - /// Number of bytes that should fit within the buffer as an exponent, where the number of bytes is 2^power. + /// Number of bytes that should fit within the buffer as an exponent, where the number of bytes is 2^power. /// Buffer that can hold the bytes. [MethodImpl(MethodImplOptions.AggressiveInlining)] public void TakeForPower(int power, out RawBuffer buffer) { - ValidatePinnedState(true); Debug.Assert(power >= 0 && power <= SpanHelper.MaximumSpanSizePower); pools[power].Take(out buffer); } @@ -392,14 +326,13 @@ internal static void DecomposeId(int bufferId, out int powerIndex, out int slotI /// /// Returns a buffer to the pool by id. /// - /// Buffer to return to the pool. + /// Id of the buffer to return to the pool. /// Typed buffer pools zero out the passed-in buffer by convention. /// This costs very little and avoids a wide variety of bugs (either directly or by forcing fast failure). For consistency, BufferPool.Return does the same thing. /// This "Unsafe" overload should be used only in cases where there's a reason to bypass the clear; the naming is intended to dissuade casual use. [MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe void ReturnUnsafely(int id) { - ValidatePinnedState(true); DecomposeId(id, out var powerIndex, out var slotIndex); pools[powerIndex].Return(slotIndex); } @@ -531,19 +464,6 @@ public void Resize(ref Buffer buffer, int targetSize, int copyCount) where buffer.length = targetSize; } - [Conditional("LEAKDEBUG")] - void ValidatePinnedState(bool pinned) - { - for (int i = 0; i < pools.Length; ++i) - { - var pool = pools[i]; - for (int j = 0; j < pool.BlockCount; ++j) - { - Debug.Assert(pool.Blocks[j].Pinned == pinned, $"For this operation, all blocks must share the same pinned state of {pinned}."); - } - } - } - [Conditional("DEBUG")] public void AssertEmpty() { @@ -566,58 +486,6 @@ public void AssertEmpty() #endif } - /// - /// Gets or sets whether the BufferPool's backing resources are pinned. If no blocks are allocated internally, this returns true. - /// Setting this to false invalidates all outstanding pointers, and any attempt to take or return buffers while unpinned will fail (though not necessarily immediately). - /// The only valid operations while unpinned are setting Pinned to true and clearing the pool. - /// - public bool Pinned - { - get - { - //If no blocks exist, we just call it pinned- that's the default state. - bool pinned = true; - for (int i = 0; i < pools.Length; ++i) - { - if (pools[i].BlockCount > 0) - { - pinned = pools[i].Blocks[0].Pinned; - break; - } - } - ValidatePinnedState(pinned); - return pinned; - } - set - { - void ChangePinnedState(bool pinned) - { - for (int i = 0; i < pools.Length; ++i) - { - var pool = pools[i]; - for (int j = 0; j < pool.BlockCount; ++j) - { - pool.Blocks[j].Pinned = pinned; - } - } - } - if (value) - { - if (!Pinned) - { - ChangePinnedState(true); - } - } - else - { - if (Pinned) - { - ChangePinnedState(false); - } - } - } - } - /// /// Unpins and drops reference to all memory. Any outstanding buffers will be invalidated silently. /// @@ -658,7 +526,7 @@ int IUnmanagedMemoryPool.GetCapacityForCount(int count) } //If block count is zero, pinned just returns true since that's the default. If there's a nonzero number of blocks, then they have to be explicitly unpinned //in order for a finalizer to be valid. - Debug.Assert(totalBlockCount == 0 || !Pinned, "Memory leak warning! Don't let a buffer pool die without unpinning it!"); + Debug.Assert(totalBlockCount == 0, "Memory leak warning! Don't let a buffer pool die without clearing it!"); } #endif diff --git a/BepuUtilities/Memory/IUnmanagedMemoryPool.cs b/BepuUtilities/Memory/IUnmanagedMemoryPool.cs index a9d3de5c9..28543700f 100644 --- a/BepuUtilities/Memory/IUnmanagedMemoryPool.cs +++ b/BepuUtilities/Memory/IUnmanagedMemoryPool.cs @@ -1,7 +1,7 @@ namespace BepuUtilities.Memory { /// - /// Defines a type that is capable of pooling blocks of unmanaged memory. + /// Defines a type that is capable of rapidly serving requests for allocation and deallocation of unmanaged memory. /// public interface IUnmanagedMemoryPool { From 9d5d9c168f1c20a2d485860c1e14a6a5d909a516 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 26 Dec 2021 20:41:57 -0600 Subject: [PATCH 362/947] RawBuffer->Buffer. --- BepuPhysics/Collidables/BoundingBoxBatcher.cs | 2 +- BepuPhysics/Collidables/Box.cs | 2 +- BepuPhysics/Collidables/Capsule.cs | 2 +- BepuPhysics/Collidables/ConvexHull.cs | 2 +- BepuPhysics/Collidables/Cylinder.cs | 2 +- BepuPhysics/Collidables/IShape.cs | 2 +- BepuPhysics/Collidables/Shapes.cs | 4 +- BepuPhysics/Collidables/Sphere.cs | 2 +- BepuPhysics/Collidables/Triangle.cs | 2 +- .../CollisionDetection/CollisionBatcher.cs | 2 +- .../CollisionTasks/ConvexCollisionTask.cs | 4 +- .../ConvexCompoundOverlapFinder.cs | 2 +- BepuPhysics/CollisionDetection/PairCache.cs | 2 +- .../SweepTasks/ConvexSweepTaskCommon.cs | 4 +- BepuPhysics/CollisionDetection/UntypedList.cs | 4 +- .../CollisionDetection/WideRayTester.cs | 2 +- .../Constraints/FourBodyTypeProcessor.cs | 2 +- .../Constraints/OneBodyTypeProcessor.cs | 2 +- .../Constraints/ThreeBodyTypeProcessor.cs | 2 +- .../Constraints/TwoBodyTypeProcessor.cs | 2 +- BepuPhysics/Constraints/TypeBatch.cs | 6 +- BepuPhysics/Constraints/TypeProcessor.cs | 12 +- BepuPhysics/Trees/Tree_BinnedRefine.cs | 2 +- BepuUtilities/Memory/Buffer.cs | 16 +- BepuUtilities/Memory/BufferPool.cs | 152 ++++----- BepuUtilities/Memory/RawBuffer.cs | 312 +++++++++--------- .../MeshSerializationTestDemo.cs | 2 +- Demos/SpecializedTests/MinkowskiVisualizer.cs | 4 +- 28 files changed, 260 insertions(+), 294 deletions(-) diff --git a/BepuPhysics/Collidables/BoundingBoxBatcher.cs b/BepuPhysics/Collidables/BoundingBoxBatcher.cs index 4ffac68a2..8c843ff16 100644 --- a/BepuPhysics/Collidables/BoundingBoxBatcher.cs +++ b/BepuPhysics/Collidables/BoundingBoxBatcher.cs @@ -149,7 +149,7 @@ public unsafe void ExecuteConvexBatch(ConvexShapeBatch 0) //TODO: Check to make sure the JIT omits the branch. { var memory = stackalloc byte[shapeWide.InternalAllocationSize]; - shapeWide.Initialize(new RawBuffer(memory, shapeWide.InternalAllocationSize)); + shapeWide.Initialize(new Buffer(memory, shapeWide.InternalAllocationSize)); } ref var batch = ref batches[shapeBatch.TypeId]; var shapeIndices = batch.ShapeIndices; diff --git a/BepuPhysics/Collidables/Box.cs b/BepuPhysics/Collidables/Box.cs index 1d0368393..c8db148af 100644 --- a/BepuPhysics/Collidables/Box.cs +++ b/BepuPhysics/Collidables/Box.cs @@ -199,7 +199,7 @@ public void WriteFirst(in Box source) public bool AllowOffsetMemoryAccess => true; public int InternalAllocationSize => 0; - public void Initialize(in RawBuffer memory) { } + public void Initialize(in Buffer memory) { } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WriteSlot(int index, in Box source) diff --git a/BepuPhysics/Collidables/Capsule.cs b/BepuPhysics/Collidables/Capsule.cs index 4490290dc..254ac3cd4 100644 --- a/BepuPhysics/Collidables/Capsule.cs +++ b/BepuPhysics/Collidables/Capsule.cs @@ -212,7 +212,7 @@ public void WriteFirst(in Capsule source) public bool AllowOffsetMemoryAccess => true; public int InternalAllocationSize => 0; - public void Initialize(in RawBuffer memory) { } + public void Initialize(in Buffer memory) { } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WriteSlot(int index, in Capsule source) diff --git a/BepuPhysics/Collidables/ConvexHull.cs b/BepuPhysics/Collidables/ConvexHull.cs index 6cb024950..a8d085f2e 100644 --- a/BepuPhysics/Collidables/ConvexHull.cs +++ b/BepuPhysics/Collidables/ConvexHull.cs @@ -296,7 +296,7 @@ public struct ConvexHullWide : IShapeWide public bool AllowOffsetMemoryAccess => false; public int InternalAllocationSize => Vector.Count * Unsafe.SizeOf(); [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Initialize(in RawBuffer memory) + public void Initialize(in Buffer memory) { Debug.Assert(memory.Length == InternalAllocationSize); Hulls = memory.As(); diff --git a/BepuPhysics/Collidables/Cylinder.cs b/BepuPhysics/Collidables/Cylinder.cs index 160b613e0..2ceb1b72b 100644 --- a/BepuPhysics/Collidables/Cylinder.cs +++ b/BepuPhysics/Collidables/Cylinder.cs @@ -212,7 +212,7 @@ public void WriteFirst(in Cylinder source) public bool AllowOffsetMemoryAccess => true; public int InternalAllocationSize => 0; - public void Initialize(in RawBuffer memory) { } + public void Initialize(in Buffer memory) { } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WriteSlot(int index, in Cylinder source) diff --git a/BepuPhysics/Collidables/IShape.cs b/BepuPhysics/Collidables/IShape.cs index 193ac52df..59dfa1e4a 100644 --- a/BepuPhysics/Collidables/IShape.cs +++ b/BepuPhysics/Collidables/IShape.cs @@ -130,7 +130,7 @@ public interface IShapeWide where TShape : IShape /// Memory should be assumed to be stack allocated. /// /// Memory to use for internal allocations in the wide shape. - void Initialize(in RawBuffer memory); + void Initialize(in Buffer memory); /// /// Places the specified AOS-formatted shape into the first lane of the wide 'this' reference. diff --git a/BepuPhysics/Collidables/Shapes.cs b/BepuPhysics/Collidables/Shapes.cs index 5404b74e8..ce8297317 100644 --- a/BepuPhysics/Collidables/Shapes.cs +++ b/BepuPhysics/Collidables/Shapes.cs @@ -12,7 +12,7 @@ namespace BepuPhysics.Collidables { public abstract class ShapeBatch { - protected RawBuffer shapesData; + protected Buffer shapesData; protected int shapeDataSize; /// /// Gets the number of shapes that the batch can currently hold without resizing. @@ -146,7 +146,7 @@ void InternalResize(int shapeCount, int oldCopyLength) { shapeDataSize = Unsafe.SizeOf(); var requiredSizeInBytes = shapeCount * Unsafe.SizeOf(); - pool.TakeAtLeast(requiredSizeInBytes, out var newShapesData); + pool.TakeAtLeast(requiredSizeInBytes, out var newShapesData); var newShapes = newShapesData.As(); #if DEBUG //In debug mode, unused slots are kept at the default value. This helps catch misuse. diff --git a/BepuPhysics/Collidables/Sphere.cs b/BepuPhysics/Collidables/Sphere.cs index e1a861a76..b1217db3f 100644 --- a/BepuPhysics/Collidables/Sphere.cs +++ b/BepuPhysics/Collidables/Sphere.cs @@ -140,7 +140,7 @@ public void WriteFirst(in Sphere source) public bool AllowOffsetMemoryAccess => true; public int InternalAllocationSize => 0; - public void Initialize(in RawBuffer memory) { } + public void Initialize(in Buffer memory) { } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WriteSlot(int index, in Sphere source) diff --git a/BepuPhysics/Collidables/Triangle.cs b/BepuPhysics/Collidables/Triangle.cs index f7616cdb6..7e89c40c6 100644 --- a/BepuPhysics/Collidables/Triangle.cs +++ b/BepuPhysics/Collidables/Triangle.cs @@ -155,7 +155,7 @@ public void WriteFirst(in Triangle source) public bool AllowOffsetMemoryAccess => true; public int InternalAllocationSize => 0; - public void Initialize(in RawBuffer memory) { } + public void Initialize(in Buffer memory) { } [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WriteSlot(int index, in Triangle source) diff --git a/BepuPhysics/CollisionDetection/CollisionBatcher.cs b/BepuPhysics/CollisionDetection/CollisionBatcher.cs index e26515649..2892c74d0 100644 --- a/BepuPhysics/CollisionDetection/CollisionBatcher.cs +++ b/BepuPhysics/CollisionDetection/CollisionBatcher.cs @@ -11,7 +11,7 @@ namespace BepuPhysics.CollisionDetection { unsafe struct UntypedBlob { - public RawBuffer Buffer; + public Buffer Buffer; public int ByteCount; [MethodImpl(MethodImplOptions.AggressiveInlining)] diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCollisionTask.cs b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCollisionTask.cs index 5bdfcedb4..789437bee 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCollisionTask.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCollisionTask.cs @@ -58,12 +58,12 @@ public override unsafe void ExecuteBatch(ref UntypedList batch, ref if (aWide.InternalAllocationSize > 0) { var memory = stackalloc byte[aWide.InternalAllocationSize]; - aWide.Initialize(new RawBuffer(memory, aWide.InternalAllocationSize)); + aWide.Initialize(new Buffer(memory, aWide.InternalAllocationSize)); } if (bWide.InternalAllocationSize > 0) { var memory = stackalloc byte[bWide.InternalAllocationSize]; - bWide.Initialize(new RawBuffer(memory, bWide.InternalAllocationSize)); + bWide.Initialize(new Buffer(memory, bWide.InternalAllocationSize)); } TManifoldWide manifoldWide; var defaultPairTester = default(TPairTester); diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundOverlapFinder.cs b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundOverlapFinder.cs index c1b843e52..0677b00bb 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundOverlapFinder.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundOverlapFinder.cs @@ -47,7 +47,7 @@ public unsafe void FindLocalOverlaps(ref Buffer pairs, int pai if (convexWide.InternalAllocationSize > 0) { var memory = stackalloc byte[convexWide.InternalAllocationSize]; - convexWide.Initialize(new RawBuffer(memory, convexWide.InternalAllocationSize)); + convexWide.Initialize(new Buffer(memory, convexWide.InternalAllocationSize)); } for (int i = 0; i < pairCount; i += Vector.Count) { diff --git a/BepuPhysics/CollisionDetection/PairCache.cs b/BepuPhysics/CollisionDetection/PairCache.cs index 085d6dacc..af74b0555 100644 --- a/BepuPhysics/CollisionDetection/PairCache.cs +++ b/BepuPhysics/CollisionDetection/PairCache.cs @@ -102,7 +102,7 @@ public partial class PairCache /// atomic setting behavior for data types no larger than the native pointer size. Further, smaller sizes actually pay a higher price in terms of increased false sharing. /// Choice of data type is a balancing act between the memory bandwidth of the post analysis and the frequency of false sharing. /// - internal RawBuffer PairFreshness; + internal Buffer PairFreshness; BufferPool pool; int minimumPendingSize; int minimumPerTypeCapacity; diff --git a/BepuPhysics/CollisionDetection/SweepTasks/ConvexSweepTaskCommon.cs b/BepuPhysics/CollisionDetection/SweepTasks/ConvexSweepTaskCommon.cs index c13c40fde..4eb5e7aff 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/ConvexSweepTaskCommon.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/ConvexSweepTaskCommon.cs @@ -285,12 +285,12 @@ static unsafe bool Sweep( if (wideA.InternalAllocationSize > 0) { var memory = stackalloc byte[wideA.InternalAllocationSize]; - wideA.Initialize(new RawBuffer(memory, wideA.InternalAllocationSize)); + wideA.Initialize(new Buffer(memory, wideA.InternalAllocationSize)); } if (wideB.InternalAllocationSize > 0) { var memory = stackalloc byte[wideB.InternalAllocationSize]; - wideB.Initialize(new RawBuffer(memory, wideB.InternalAllocationSize)); + wideB.Initialize(new Buffer(memory, wideB.InternalAllocationSize)); } wideA.Broadcast(shapeA); wideB.Broadcast(shapeB); diff --git a/BepuPhysics/CollisionDetection/UntypedList.cs b/BepuPhysics/CollisionDetection/UntypedList.cs index e3dec4878..2dc7476bc 100644 --- a/BepuPhysics/CollisionDetection/UntypedList.cs +++ b/BepuPhysics/CollisionDetection/UntypedList.cs @@ -7,7 +7,7 @@ namespace BepuPhysics.CollisionDetection { public struct UntypedList { - public RawBuffer Buffer; + public Buffer Buffer; public int Count; public int ByteCount; public int ElementSizeInBytes; @@ -106,7 +106,7 @@ public unsafe int Allocate(int elementSizeInBytes, int minimumElementCount, Buff if (newSize > Buffer.Length) { //This will bump up to the next allocated block size, so we don't have to worry about constant micro-resizes. - pool.TakeAtLeast(newSize, out var newBuffer); + pool.TakeAtLeast(newSize, out var newBuffer); Unsafe.CopyBlockUnaligned(newBuffer.Memory, Buffer.Memory, (uint)Buffer.Length); pool.ReturnUnsafely(Buffer.Id); Buffer = newBuffer; diff --git a/BepuPhysics/CollisionDetection/WideRayTester.cs b/BepuPhysics/CollisionDetection/WideRayTester.cs index 36aca4cbb..489a9d166 100644 --- a/BepuPhysics/CollisionDetection/WideRayTester.cs +++ b/BepuPhysics/CollisionDetection/WideRayTester.cs @@ -24,7 +24,7 @@ public unsafe static void Test(r if (wide.InternalAllocationSize > 0) { var memory = stackalloc byte[wide.InternalAllocationSize]; - wide.Initialize(new RawBuffer(memory, wide.InternalAllocationSize)); + wide.Initialize(new Buffer(memory, wide.InternalAllocationSize)); } wide.Broadcast(shape); RigidPoseWide poses; diff --git a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs index f248438dc..b358b5293 100644 --- a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs @@ -90,7 +90,7 @@ internal sealed override void GenerateSortKeysAndCopyReferences( ref TypeBatch typeBatch, int bundleStart, int localBundleStart, int bundleCount, int constraintStart, int localConstraintStart, int constraintCount, - ref int firstSortKey, ref int firstSourceIndex, ref RawBuffer bodyReferencesCache) + ref int firstSortKey, ref int firstSourceIndex, ref Buffer bodyReferencesCache) { GenerateSortKeysAndCopyReferences( ref typeBatch, diff --git a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs index 112c2d427..489dd965a 100644 --- a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs @@ -59,7 +59,7 @@ internal sealed override void GenerateSortKeysAndCopyReferences( ref TypeBatch typeBatch, int bundleStart, int localBundleStart, int bundleCount, int constraintStart, int localConstraintStart, int constraintCount, - ref int firstSortKey, ref int firstSourceIndex, ref RawBuffer bodyReferencesCache) + ref int firstSortKey, ref int firstSourceIndex, ref Buffer bodyReferencesCache) { GenerateSortKeysAndCopyReferences( ref typeBatch, diff --git a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs index 31f4f640b..00ab98229 100644 --- a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs @@ -85,7 +85,7 @@ internal sealed override void GenerateSortKeysAndCopyReferences( ref TypeBatch typeBatch, int bundleStart, int localBundleStart, int bundleCount, int constraintStart, int localConstraintStart, int constraintCount, - ref int firstSortKey, ref int firstSourceIndex, ref RawBuffer bodyReferencesCache) + ref int firstSortKey, ref int firstSourceIndex, ref Buffer bodyReferencesCache) { GenerateSortKeysAndCopyReferences( ref typeBatch, diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index e7b7b803c..091a0a404 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -95,7 +95,7 @@ internal sealed override void GenerateSortKeysAndCopyReferences( ref TypeBatch typeBatch, int bundleStart, int localBundleStart, int bundleCount, int constraintStart, int localConstraintStart, int constraintCount, - ref int firstSortKey, ref int firstSourceIndex, ref RawBuffer bodyReferencesCache) + ref int firstSortKey, ref int firstSourceIndex, ref Buffer bodyReferencesCache) { GenerateSortKeysAndCopyReferences( ref typeBatch, diff --git a/BepuPhysics/Constraints/TypeBatch.cs b/BepuPhysics/Constraints/TypeBatch.cs index ea754fd30..7845a78df 100644 --- a/BepuPhysics/Constraints/TypeBatch.cs +++ b/BepuPhysics/Constraints/TypeBatch.cs @@ -14,9 +14,9 @@ namespace BepuPhysics.Constraints public struct TypeBatch { //Note the constraint data is all stored untyped. It is up to the user to read from these pointers correctly. - public RawBuffer BodyReferences; - public RawBuffer PrestepData; - public RawBuffer AccumulatedImpulses; + public Buffer BodyReferences; + public Buffer PrestepData; + public Buffer AccumulatedImpulses; public Buffer IndexToHandle; public int ConstraintCount; public int TypeId; diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index ffea14a68..8130f9b6f 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -168,18 +168,18 @@ internal abstract void GenerateSortKeysAndCopyReferences( ref TypeBatch typeBatch, int bundleStart, int localBundleStart, int bundleCount, int constraintStart, int localConstraintStart, int constraintCount, - ref int firstSortKey, ref int firstSourceIndex, ref RawBuffer bodyReferencesCache); + ref int firstSortKey, ref int firstSourceIndex, ref Buffer bodyReferencesCache); internal abstract void CopyToCache( ref TypeBatch typeBatch, int bundleStart, int localBundleStart, int bundleCount, int constraintStart, int localConstraintStart, int constraintCount, - ref Buffer indexToHandleCache, ref RawBuffer prestepCache, ref RawBuffer accumulatedImpulsesCache); + ref Buffer indexToHandleCache, ref Buffer prestepCache, ref Buffer accumulatedImpulsesCache); internal abstract void Regather( ref TypeBatch typeBatch, int constraintStart, int constraintCount, ref int firstSourceIndex, - ref Buffer indexToHandleCache, ref RawBuffer bodyReferencesCache, ref RawBuffer prestepCache, ref RawBuffer accumulatedImpulsesCache, + ref Buffer indexToHandleCache, ref Buffer bodyReferencesCache, ref Buffer prestepCache, ref Buffer accumulatedImpulsesCache, ref Buffer handlesToConstraints); internal unsafe abstract void GatherActiveConstraints(Bodies bodies, Solver solver, ref QuickList sourceHandles, int startIndex, int endIndex, ref TypeBatch targetTypeBatch); @@ -824,7 +824,7 @@ protected void GenerateSortKeysAndCopyReferences( ref TypeBatch typeBatch, int bundleStart, int localBundleStart, int bundleCount, int constraintStart, int localConstraintStart, int constraintCount, - ref int firstSortKey, ref int firstSourceIndex, ref RawBuffer bodyReferencesCache) + ref int firstSortKey, ref int firstSourceIndex, ref Buffer bodyReferencesCache) where TSortKeyGenerator : struct, ISortKeyGenerator { var sortKeyGenerator = default(TSortKeyGenerator); @@ -863,7 +863,7 @@ internal unsafe sealed override void CopyToCache( ref TypeBatch typeBatch, int bundleStart, int localBundleStart, int bundleCount, int constraintStart, int localConstraintStart, int constraintCount, - ref Buffer indexToHandleCache, ref RawBuffer prestepCache, ref RawBuffer accumulatedImpulsesCache) + ref Buffer indexToHandleCache, ref Buffer prestepCache, ref Buffer accumulatedImpulsesCache) { typeBatch.IndexToHandle.CopyTo(constraintStart, indexToHandleCache, localConstraintStart, constraintCount); Unsafe.CopyBlockUnaligned( @@ -878,7 +878,7 @@ internal unsafe sealed override void CopyToCache( internal sealed override void Regather( ref TypeBatch typeBatch, int constraintStart, int constraintCount, ref int firstSourceIndex, - ref Buffer indexToHandleCache, ref RawBuffer bodyReferencesCache, ref RawBuffer prestepCache, ref RawBuffer accumulatedImpulsesCache, + ref Buffer indexToHandleCache, ref Buffer bodyReferencesCache, ref Buffer prestepCache, ref Buffer accumulatedImpulsesCache, ref Buffer handlesToConstraints) { var typedBodyReferencesCache = bodyReferencesCache.As(); diff --git a/BepuPhysics/Trees/Tree_BinnedRefine.cs b/BepuPhysics/Trees/Tree_BinnedRefine.cs index 9e0411cf9..1d2a8623f 100644 --- a/BepuPhysics/Trees/Tree_BinnedRefine.cs +++ b/BepuPhysics/Trees/Tree_BinnedRefine.cs @@ -80,7 +80,7 @@ partial struct Tree return toReturn; } - public static unsafe void CreateBinnedResources(BufferPool bufferPool, int maximumSubtreeCount, out RawBuffer buffer, out BinnedResources resources) + public static unsafe void CreateBinnedResources(BufferPool bufferPool, int maximumSubtreeCount, out Buffer buffer, out BinnedResources resources) { //TODO: This is a holdover from the pre-BufferPool tree design. It's pretty ugly. While some preallocation is useful (there's no reason to suffer the overhead of //pulling things out of the BufferPool over and over and over again), the degree to which this preallocates has a negative impact on L1 cache for subtree refines. diff --git a/BepuUtilities/Memory/Buffer.cs b/BepuUtilities/Memory/Buffer.cs index 72deefd5d..353f9ec68 100644 --- a/BepuUtilities/Memory/Buffer.cs +++ b/BepuUtilities/Memory/Buffer.cs @@ -41,7 +41,7 @@ public static ref T Get(byte* memory, int index) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static ref T Get(ref RawBuffer buffer, int index) + public static ref T Get(ref Buffer buffer, int index) { Debug.Assert(index >= 0 && index * Unsafe.SizeOf() < buffer.Length, "Index out of range."); return ref Get(buffer.Memory, index); @@ -299,17 +299,15 @@ public int IndexOf(ref TPredicate predicate, int start, int count) w } /// - /// Creates an untyped buffer containing the same data as the Buffer. + /// Creates a typed region from the raw buffer with the largest capacity that can fit within the allocated bytes. /// - /// Untyped buffer containing the same data as the source buffer. + /// Type of the buffer. + /// Typed buffer of maximum extent within the current buffer. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public RawBuffer AsRaw() + public Buffer As() where TCast : unmanaged { - RawBuffer buffer; - buffer.Memory = (byte*)Memory; - buffer.Length = length * Unsafe.SizeOf(); - buffer.Id = Id; - return buffer; + var count = Length * Unsafe.SizeOf() / Unsafe.SizeOf(); + return new Buffer(Memory, count, Id); } [Conditional("DEBUG")] diff --git a/BepuUtilities/Memory/BufferPool.cs b/BepuUtilities/Memory/BufferPool.cs index 2d5488458..e334fea4f 100644 --- a/BepuUtilities/Memory/BufferPool.cs +++ b/BepuUtilities/Memory/BufferPool.cs @@ -107,7 +107,7 @@ public void EnsureCapacity(int capacity) return Blocks[blockIndex] + indexInBlock * SuballocationSize; } - public unsafe void Take(out RawBuffer buffer) + public unsafe void Take(out Buffer buffer) { var slot = Slots.Take(); var blockIndex = slot >> SuballocationsPerBlockShift; @@ -121,7 +121,7 @@ public unsafe void Take(out RawBuffer buffer) } var indexInBlock = slot & SuballocationsPerBlockMask; - buffer = new RawBuffer(Blocks[blockIndex] + indexInBlock * SuballocationSize, SuballocationSize, (Power << IdPowerShift) | slot); + buffer = new Buffer(Blocks[blockIndex] + indexInBlock * SuballocationSize, SuballocationSize, (Power << IdPowerShift) | slot); Debug.Assert(buffer.Id >= 0 && Power >= 0 && Power < 32, "Slot/power should be safely encoded in a 32 bit integer."); #if DEBUG const int maximumOutstandingCount = 1 << 26; @@ -142,8 +142,9 @@ public unsafe void Take(out RawBuffer buffer) } [Conditional("DEBUG")] - internal unsafe void ValidateBufferIsContained(ref RawBuffer buffer) + internal unsafe void ValidateBufferIsContained(ref Buffer typedBuffer) where T : unmanaged { + var buffer = typedBuffer.As(); //There are a lot of ways to screw this up. Try to catch as many as possible! var slotIndex = buffer.Id & ((1 << IdPowerShift) - 1); var blockIndex = slotIndex >> SuballocationsPerBlockShift; @@ -252,29 +253,6 @@ public int GetCapacityForPower(int power) return pools[power].BlockCount * pools[power].BlockSize; } - /// - /// Takes a buffer large enough to contain a number of bytes. Capacity may be larger than requested. - /// - /// Desired minimum capacity of the buffer in bytes. - /// Buffer that can hold the bytes. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void TakeAtLeast(int count, out RawBuffer buffer) - { - TakeForPower(SpanHelper.GetContainingPowerOf2(count), out buffer); - } - - /// - /// Takes a buffer of the requested size from the pool. - /// - /// Desired capacity of the buffer in bytes. - /// Buffer of the requested size. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Take(int count, out RawBuffer buffer) - { - TakeAtLeast(count, out buffer); - buffer.Length = count; - } - /// /// Takes a buffer large enough to contain a number of elements of a given type. Capacity may be larger than requested. /// @@ -287,7 +265,7 @@ public void TakeAtLeast(int count, out Buffer buffer) where T : unmanaged //Avoid returning a zero length span because 1 byte / Unsafe.SizeOf() happens to be zero. if (count == 0) count = 1; - TakeAtLeast(count * Unsafe.SizeOf(), out var rawBuffer); + TakeForPower(SpanHelper.GetContainingPowerOf2(count * Unsafe.SizeOf()), out var rawBuffer); buffer = rawBuffer.As(); } @@ -310,7 +288,7 @@ public void Take(int count, out Buffer buffer) where T : unmanaged /// Number of bytes that should fit within the buffer as an exponent, where the number of bytes is 2^power. /// Buffer that can hold the bytes. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void TakeForPower(int power, out RawBuffer buffer) + public void TakeForPower(int power, out Buffer buffer) { Debug.Assert(power >= 0 && power <= SpanHelper.MaximumSpanSizePower); pools[power].Take(out buffer); @@ -342,7 +320,7 @@ public unsafe void ReturnUnsafely(int id) /// /// Buffer to return to the pool. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void Return(ref RawBuffer buffer) + public unsafe void Return(ref Buffer buffer) where T : unmanaged { #if DEBUG DecomposeId(buffer.Id, out var powerIndex, out var slotIndex); @@ -351,72 +329,62 @@ public unsafe void Return(ref RawBuffer buffer) ReturnUnsafely(buffer.Id); buffer = default; } - /// - /// Returns a buffer to the pool. - /// - /// Buffer to return to the pool. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void Return(ref Buffer buffer) where T : unmanaged - { - ReturnUnsafely(buffer.Id); - buffer = default; - } //The resizes aren't particularly clever. They are more aggressive about reallocating than they need to be, but //the assumption is that they will be used only when it's already known that a resize is necessary. - /// - /// Resizes a buffer to the smallest size available in the pool which contains the target size. Copies a subset of elements into the new buffer. - /// Final buffer size is at least as large as the target size and may be larger. - /// - /// Buffer reference to resize. - /// Number of bytes to resize the buffer for. - /// Number of bytes to copy into the new buffer from the old buffer. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void ResizeToAtLeast(ref RawBuffer buffer, int targetSize, int copyCount) - { - Debug.Assert(copyCount <= buffer.Length, "Can't copy more than the capacity of the buffer."); - Debug.Assert(copyCount <= targetSize, "Can't copy more than the target size."); - //Only do anything if the new size is actually different from the current size. - targetSize = 1 << (SpanHelper.GetContainingPowerOf2(targetSize)); - if (buffer.Allocated) - { - DecomposeId(buffer.Id, out var powerIndex, out var slotIndex); - var currentSize = 1 << powerIndex; - if (currentSize != targetSize || pools[powerIndex].GetStartPointerForSlot(slotIndex) != buffer.Memory) - { - TakeAtLeast(targetSize, out var newBuffer); - Buffer.MemoryCopy(buffer.Memory, newBuffer.Memory, buffer.Length, copyCount); - pools[powerIndex].Return(slotIndex); - buffer = newBuffer; - } - else - { - //While the allocation size is equal to the target size, the buffer might not be. - //Fortunately, if the allocation stays the same size and the buffer start is at its original location, we can skip doing any work. - //(With more work, you could expand *backwards*, we just didn't bother since this is exceptionally rare anyway. - //The typed codepath doesn't bother doing this at all, and that's fine.) - buffer.Length = targetSize; - } - } - else - { - //Nothing to return or copy. - TakeAtLeast(targetSize, out buffer); - } - } - - /// - /// Resizes a buffer to the target size. Copies a subset of elements into the new buffer. - /// - /// Buffer reference to resize. - /// Number of bytes to resize the buffer for. - /// Number of bytes to copy into the new buffer from the old buffer. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void Resize(ref RawBuffer buffer, int targetSize, int copyCount) - { - ResizeToAtLeast(ref buffer, targetSize, copyCount); - buffer.Length = targetSize; - } + ///// + ///// Resizes a buffer to the smallest size available in the pool which contains the target size. Copies a subset of elements into the new buffer. + ///// Final buffer size is at least as large as the target size and may be larger. + ///// + ///// Buffer reference to resize. + ///// Number of bytes to resize the buffer for. + ///// Number of bytes to copy into the new buffer from the old buffer. + //[MethodImpl(MethodImplOptions.AggressiveInlining)] + //public unsafe void ResizeToAtLeast(ref Buffer buffer, int targetSize, int copyCount) + //{ + // Debug.Assert(copyCount <= buffer.Length, "Can't copy more than the capacity of the buffer."); + // Debug.Assert(copyCount <= targetSize, "Can't copy more than the target size."); + // //Only do anything if the new size is actually different from the current size. + // targetSize = 1 << (SpanHelper.GetContainingPowerOf2(targetSize)); + // if (buffer.Allocated) + // { + // DecomposeId(buffer.Id, out var powerIndex, out var slotIndex); + // var currentSize = 1 << powerIndex; + // if (currentSize != targetSize || pools[powerIndex].GetStartPointerForSlot(slotIndex) != buffer.Memory) + // { + // TakeAtLeast(targetSize, out var newBuffer); + // Buffer.MemoryCopy(buffer.Memory, newBuffer.Memory, buffer.Length, copyCount); + // pools[powerIndex].Return(slotIndex); + // buffer = newBuffer; + // } + // else + // { + // //While the allocation size is equal to the target size, the buffer might not be. + // //Fortunately, if the allocation stays the same size and the buffer start is at its original location, we can skip doing any work. + // //(With more work, you could expand *backwards*, we just didn't bother since this is exceptionally rare anyway. + // //The typed codepath doesn't bother doing this at all, and that's fine.) + // buffer.Length = targetSize; + // } + // } + // else + // { + // //Nothing to return or copy. + // TakeAtLeast(targetSize, out buffer); + // } + //} + + ///// + ///// Resizes a buffer to the target size. Copies a subset of elements into the new buffer. + ///// + ///// Buffer reference to resize. + ///// Number of bytes to resize the buffer for. + ///// Number of bytes to copy into the new buffer from the old buffer. + //[MethodImpl(MethodImplOptions.AggressiveInlining)] + //public unsafe void Resize(ref Buffer buffer, int targetSize, int copyCount) + //{ + // ResizeToAtLeast(ref buffer, targetSize, copyCount); + // buffer.Length = targetSize; + //} /// /// Resizes a typed buffer to the smallest size available in the pool which contains the target size. Copies a subset of elements into the new buffer. diff --git a/BepuUtilities/Memory/RawBuffer.cs b/BepuUtilities/Memory/RawBuffer.cs index f7abefa7d..fa50fbec1 100644 --- a/BepuUtilities/Memory/RawBuffer.cs +++ b/BepuUtilities/Memory/RawBuffer.cs @@ -4,160 +4,160 @@ namespace BepuUtilities.Memory { - /// - /// Raw byte buffer with some helpers for interoperating with typed spans. - /// - public unsafe struct RawBuffer : IEquatable - { - public byte* Memory; - public int Length; - //We're primarily interested in x64, so memory + length is 12 bytes. This struct would/should get padded to 16 bytes for alignment reasons anyway, - //so making use of the last 4 bytes to speed up the case where the raw buffer is taken from a pool (which is basically always) is a good option. - - /// - /// Implementation specific identifier of the raw buffer set by its source. If taken from a BufferPool, Id represents the packed power and internal power pool index from which it was taken. - /// - public int Id; - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public RawBuffer(void* memory, int length, int id = -1) - { - Memory = (byte*)memory; - Length = length; - Id = id; - } - - public bool Allocated - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - get - { - return Memory != null; - } - } - - public ref byte this[int index] - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - get - { - return ref *(Memory + index); - } - } - - /// - /// Interprets the bytes at the memory location as a given type. - /// - /// Type to interpret the memory as. - /// Memory location to interpret. - /// Reference to the memory as a given type. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref T Interpret(int byteIndex) - { - ValidateRegion(byteIndex, Unsafe.SizeOf()); - return ref Unsafe.As(ref *(Memory + byteIndex)); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public RawBuffer Slice(int start, int count) - { - ValidateRegion(start, count); - return new RawBuffer(Memory + start, count, Id); - } - - /// - /// Takes a region of the raw buffer as a typed buffer. - /// - /// Type to interpret the region as. - /// Start of the region in terms of the type's size. - /// Number of elements in the region in terms of the type. - /// A typed buffer. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public Buffer Slice(int start, int count) where T : unmanaged - { - ValidateRegion(start, count); - return new Buffer(Memory + start * Unsafe.SizeOf(), count * Unsafe.SizeOf(), Id); - } - - /// - /// Creates a typed region from the raw buffer with the largest capacity that can fit within the allocated bytes. - /// - /// Type of the buffer. - /// Typed buffer of maximum extent within the current raw buffer. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public Buffer As() where T : unmanaged - { - var count = Length / Unsafe.SizeOf(); - return new Buffer(Memory, count, Id); - } - - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Clear(int start, int count) - { - Unsafe.InitBlockUnaligned(Memory + start, 0, (uint)count); - } - - //TODO: Some copies could be helpful, but let's wait until we actually need them. - - [Conditional("DEBUG")] - void ValidateRegion(int startInElements, int countInElements) - { - Debug.Assert(startInElements * Unsafe.SizeOf() >= 0, "The start of a region must be within the buffer's extent."); - Debug.Assert((startInElements + countInElements) * Unsafe.SizeOf() <= Length, "The end of a region must be within the buffer's extent."); - } - - [Conditional("DEBUG")] - void ValidateRegion(int start, int count) - { - Debug.Assert(start >= 0, "The start of a region must be within the buffer's extent."); - Debug.Assert(start + count <= Length, "The end of a region must be within the buffer's extent."); - } - - //These are primarily used for debug purposes in the buffer pool. - public override unsafe int GetHashCode() - { - if (IntPtr.Size == 4) - { - var temp = Memory; - return *((int*)&temp); - } - else - { - var temp = Memory; - return (*((long*)&temp)).GetHashCode(); - } - } - - public bool Equals(RawBuffer other) - { - return other.Memory == Memory && other.Length == Length; - } - - public override bool Equals(object obj) - { - if (obj is RawBuffer buffer) - return Equals(buffer); - return false; - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static implicit operator Span(in RawBuffer buffer) - { - return new Span(buffer.Memory, buffer.Length); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static implicit operator ReadOnlySpan(in RawBuffer buffer) - { - return new ReadOnlySpan(buffer.Memory, buffer.Length); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static implicit operator Buffer(in RawBuffer buffer) - { - return new Buffer(buffer.Memory, buffer.Length); - } - - } + ///// + ///// Raw byte buffer with some helpers for interoperating with typed spans. + ///// + //public unsafe struct RawBuffer : IEquatable + //{ + // public byte* Memory; + // public int Length; + // //We're primarily interested in x64, so memory + length is 12 bytes. This struct would/should get padded to 16 bytes for alignment reasons anyway, + // //so making use of the last 4 bytes to speed up the case where the raw buffer is taken from a pool (which is basically always) is a good option. + + // /// + // /// Implementation specific identifier of the raw buffer set by its source. If taken from a BufferPool, Id represents the packed power and internal power pool index from which it was taken. + // /// + // public int Id; + + // [MethodImpl(MethodImplOptions.AggressiveInlining)] + // public RawBuffer(void* memory, int length, int id = -1) + // { + // Memory = (byte*)memory; + // Length = length; + // Id = id; + // } + + // public bool Allocated + // { + // [MethodImpl(MethodImplOptions.AggressiveInlining)] + // get + // { + // return Memory != null; + // } + // } + + // public ref byte this[int index] + // { + // [MethodImpl(MethodImplOptions.AggressiveInlining)] + // get + // { + // return ref *(Memory + index); + // } + // } + + // /// + // /// Interprets the bytes at the memory location as a given type. + // /// + // /// Type to interpret the memory as. + // /// Memory location to interpret. + // /// Reference to the memory as a given type. + // [MethodImpl(MethodImplOptions.AggressiveInlining)] + // public ref T Interpret(int byteIndex) + // { + // ValidateRegion(byteIndex, Unsafe.SizeOf()); + // return ref Unsafe.As(ref *(Memory + byteIndex)); + // } + + // [MethodImpl(MethodImplOptions.AggressiveInlining)] + // public RawBuffer Slice(int start, int count) + // { + // ValidateRegion(start, count); + // return new RawBuffer(Memory + start, count, Id); + // } + + // /// + // /// Takes a region of the raw buffer as a typed buffer. + // /// + // /// Type to interpret the region as. + // /// Start of the region in terms of the type's size. + // /// Number of elements in the region in terms of the type. + // /// A typed buffer. + // [MethodImpl(MethodImplOptions.AggressiveInlining)] + // public Buffer Slice(int start, int count) where T : unmanaged + // { + // ValidateRegion(start, count); + // return new Buffer(Memory + start * Unsafe.SizeOf(), count * Unsafe.SizeOf(), Id); + // } + + // /// + // /// Creates a typed region from the raw buffer with the largest capacity that can fit within the allocated bytes. + // /// + // /// Type of the buffer. + // /// Typed buffer of maximum extent within the current raw buffer. + // [MethodImpl(MethodImplOptions.AggressiveInlining)] + // public Buffer As() where T : unmanaged + // { + // var count = Length / Unsafe.SizeOf(); + // return new Buffer(Memory, count, Id); + // } + + + // [MethodImpl(MethodImplOptions.AggressiveInlining)] + // public void Clear(int start, int count) + // { + // Unsafe.InitBlockUnaligned(Memory + start, 0, (uint)count); + // } + + // //TODO: Some copies could be helpful, but let's wait until we actually need them. + + // [Conditional("DEBUG")] + // void ValidateRegion(int startInElements, int countInElements) + // { + // Debug.Assert(startInElements * Unsafe.SizeOf() >= 0, "The start of a region must be within the buffer's extent."); + // Debug.Assert((startInElements + countInElements) * Unsafe.SizeOf() <= Length, "The end of a region must be within the buffer's extent."); + // } + + // [Conditional("DEBUG")] + // void ValidateRegion(int start, int count) + // { + // Debug.Assert(start >= 0, "The start of a region must be within the buffer's extent."); + // Debug.Assert(start + count <= Length, "The end of a region must be within the buffer's extent."); + // } + + // //These are primarily used for debug purposes in the buffer pool. + // public override unsafe int GetHashCode() + // { + // if (IntPtr.Size == 4) + // { + // var temp = Memory; + // return *((int*)&temp); + // } + // else + // { + // var temp = Memory; + // return (*((long*)&temp)).GetHashCode(); + // } + // } + + // public bool Equals(RawBuffer other) + // { + // return other.Memory == Memory && other.Length == Length; + // } + + // public override bool Equals(object obj) + // { + // if (obj is RawBuffer buffer) + // return Equals(buffer); + // return false; + // } + + // [MethodImpl(MethodImplOptions.AggressiveInlining)] + // public static implicit operator Span(in RawBuffer buffer) + // { + // return new Span(buffer.Memory, buffer.Length); + // } + + // [MethodImpl(MethodImplOptions.AggressiveInlining)] + // public static implicit operator ReadOnlySpan(in RawBuffer buffer) + // { + // return new ReadOnlySpan(buffer.Memory, buffer.Length); + // } + + // [MethodImpl(MethodImplOptions.AggressiveInlining)] + // public static implicit operator Buffer(in RawBuffer buffer) + // { + // return new Buffer(buffer.Memory, buffer.Length); + // } + + //} } \ No newline at end of file diff --git a/Demos/SpecializedTests/MeshSerializationTestDemo.cs b/Demos/SpecializedTests/MeshSerializationTestDemo.cs index 9639edb4e..ecb30bd9f 100644 --- a/Demos/SpecializedTests/MeshSerializationTestDemo.cs +++ b/Demos/SpecializedTests/MeshSerializationTestDemo.cs @@ -26,7 +26,7 @@ public override void Initialize(ContentArchive content, Camera camera) var freshConstructionTime = (endTime - startTime) / (double)Stopwatch.Frequency; Console.WriteLine($"Fresh construction time (ms): {freshConstructionTime * 1e3}"); - BufferPool.Take(originalMesh.GetSerializedByteCount(), out var serializedMeshBytes); + BufferPool.Take(originalMesh.GetSerializedByteCount(), out var serializedMeshBytes); originalMesh.Serialize(serializedMeshBytes); startTime = Stopwatch.GetTimestamp(); var loadedMesh = new Mesh(serializedMeshBytes, BufferPool); diff --git a/Demos/SpecializedTests/MinkowskiVisualizer.cs b/Demos/SpecializedTests/MinkowskiVisualizer.cs index 396396d8c..9062deef5 100644 --- a/Demos/SpecializedTests/MinkowskiVisualizer.cs +++ b/Demos/SpecializedTests/MinkowskiVisualizer.cs @@ -74,12 +74,12 @@ public unsafe static Buffer CreateLines 0) { var memory = stackalloc byte[aWide.InternalAllocationSize]; - aWide.Initialize(new RawBuffer(memory, aWide.InternalAllocationSize)); + aWide.Initialize(new Buffer(memory, aWide.InternalAllocationSize)); } if (bWide.InternalAllocationSize > 0) { var memory = stackalloc byte[bWide.InternalAllocationSize]; - bWide.Initialize(new RawBuffer(memory, bWide.InternalAllocationSize)); + bWide.Initialize(new Buffer(memory, bWide.InternalAllocationSize)); } aWide.Broadcast(a); bWide.Broadcast(b); From ed44a942a1b71deabba576b05b6fd5dfceef3b43 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 26 Dec 2021 20:42:33 -0600 Subject: [PATCH 363/947] (-rawbuffer) --- BepuUtilities/Memory/RawBuffer.cs | 163 ------------------------------ 1 file changed, 163 deletions(-) delete mode 100644 BepuUtilities/Memory/RawBuffer.cs diff --git a/BepuUtilities/Memory/RawBuffer.cs b/BepuUtilities/Memory/RawBuffer.cs deleted file mode 100644 index fa50fbec1..000000000 --- a/BepuUtilities/Memory/RawBuffer.cs +++ /dev/null @@ -1,163 +0,0 @@ -using System; -using System.Diagnostics; -using System.Runtime.CompilerServices; - -namespace BepuUtilities.Memory -{ - ///// - ///// Raw byte buffer with some helpers for interoperating with typed spans. - ///// - //public unsafe struct RawBuffer : IEquatable - //{ - // public byte* Memory; - // public int Length; - // //We're primarily interested in x64, so memory + length is 12 bytes. This struct would/should get padded to 16 bytes for alignment reasons anyway, - // //so making use of the last 4 bytes to speed up the case where the raw buffer is taken from a pool (which is basically always) is a good option. - - // /// - // /// Implementation specific identifier of the raw buffer set by its source. If taken from a BufferPool, Id represents the packed power and internal power pool index from which it was taken. - // /// - // public int Id; - - // [MethodImpl(MethodImplOptions.AggressiveInlining)] - // public RawBuffer(void* memory, int length, int id = -1) - // { - // Memory = (byte*)memory; - // Length = length; - // Id = id; - // } - - // public bool Allocated - // { - // [MethodImpl(MethodImplOptions.AggressiveInlining)] - // get - // { - // return Memory != null; - // } - // } - - // public ref byte this[int index] - // { - // [MethodImpl(MethodImplOptions.AggressiveInlining)] - // get - // { - // return ref *(Memory + index); - // } - // } - - // /// - // /// Interprets the bytes at the memory location as a given type. - // /// - // /// Type to interpret the memory as. - // /// Memory location to interpret. - // /// Reference to the memory as a given type. - // [MethodImpl(MethodImplOptions.AggressiveInlining)] - // public ref T Interpret(int byteIndex) - // { - // ValidateRegion(byteIndex, Unsafe.SizeOf()); - // return ref Unsafe.As(ref *(Memory + byteIndex)); - // } - - // [MethodImpl(MethodImplOptions.AggressiveInlining)] - // public RawBuffer Slice(int start, int count) - // { - // ValidateRegion(start, count); - // return new RawBuffer(Memory + start, count, Id); - // } - - // /// - // /// Takes a region of the raw buffer as a typed buffer. - // /// - // /// Type to interpret the region as. - // /// Start of the region in terms of the type's size. - // /// Number of elements in the region in terms of the type. - // /// A typed buffer. - // [MethodImpl(MethodImplOptions.AggressiveInlining)] - // public Buffer Slice(int start, int count) where T : unmanaged - // { - // ValidateRegion(start, count); - // return new Buffer(Memory + start * Unsafe.SizeOf(), count * Unsafe.SizeOf(), Id); - // } - - // /// - // /// Creates a typed region from the raw buffer with the largest capacity that can fit within the allocated bytes. - // /// - // /// Type of the buffer. - // /// Typed buffer of maximum extent within the current raw buffer. - // [MethodImpl(MethodImplOptions.AggressiveInlining)] - // public Buffer As() where T : unmanaged - // { - // var count = Length / Unsafe.SizeOf(); - // return new Buffer(Memory, count, Id); - // } - - - // [MethodImpl(MethodImplOptions.AggressiveInlining)] - // public void Clear(int start, int count) - // { - // Unsafe.InitBlockUnaligned(Memory + start, 0, (uint)count); - // } - - // //TODO: Some copies could be helpful, but let's wait until we actually need them. - - // [Conditional("DEBUG")] - // void ValidateRegion(int startInElements, int countInElements) - // { - // Debug.Assert(startInElements * Unsafe.SizeOf() >= 0, "The start of a region must be within the buffer's extent."); - // Debug.Assert((startInElements + countInElements) * Unsafe.SizeOf() <= Length, "The end of a region must be within the buffer's extent."); - // } - - // [Conditional("DEBUG")] - // void ValidateRegion(int start, int count) - // { - // Debug.Assert(start >= 0, "The start of a region must be within the buffer's extent."); - // Debug.Assert(start + count <= Length, "The end of a region must be within the buffer's extent."); - // } - - // //These are primarily used for debug purposes in the buffer pool. - // public override unsafe int GetHashCode() - // { - // if (IntPtr.Size == 4) - // { - // var temp = Memory; - // return *((int*)&temp); - // } - // else - // { - // var temp = Memory; - // return (*((long*)&temp)).GetHashCode(); - // } - // } - - // public bool Equals(RawBuffer other) - // { - // return other.Memory == Memory && other.Length == Length; - // } - - // public override bool Equals(object obj) - // { - // if (obj is RawBuffer buffer) - // return Equals(buffer); - // return false; - // } - - // [MethodImpl(MethodImplOptions.AggressiveInlining)] - // public static implicit operator Span(in RawBuffer buffer) - // { - // return new Span(buffer.Memory, buffer.Length); - // } - - // [MethodImpl(MethodImplOptions.AggressiveInlining)] - // public static implicit operator ReadOnlySpan(in RawBuffer buffer) - // { - // return new ReadOnlySpan(buffer.Memory, buffer.Length); - // } - - // [MethodImpl(MethodImplOptions.AggressiveInlining)] - // public static implicit operator Buffer(in RawBuffer buffer) - // { - // return new Buffer(buffer.Memory, buffer.Length); - // } - - //} -} \ No newline at end of file From c643105ef304c11830690dc96dd577daa4927b01 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 27 Dec 2021 17:20:46 -0600 Subject: [PATCH 364/947] Substepping documentation progress. --- Documentation/Substepping.md | 21 ++++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/Documentation/Substepping.md b/Documentation/Substepping.md index 4ef428772..86e19a64d 100644 --- a/Documentation/Substepping.md +++ b/Documentation/Substepping.md @@ -6,6 +6,10 @@ You can configure a simulation to use substepping by passing a `SolveDescription var simulation = Simulation.Create(BufferPool, new NarrowPhaseCallbacks(), new PoseIntegratorCallbacks(), new SolveDescription(velocityIterationCount: 1, substepCount: 8)); ``` +# Why use it? + +# Limitations + # How substepping fits into a timestep Each call to `Simulation.Timestep(dt, ...)` simulates one frame with duration equal to `dt`. In the [`DefaultTimestepper`](../BepuPhysics/DefaultTimestepper.cs) (which, as the name implies, is the `ITimestepper` implementation used if no other is specified) executes a frame like so: ```cs @@ -26,7 +30,22 @@ While the simplest approach is to use the same number of velocity iterations for ```cs var simulation = Simulation.Create(BufferPool, new NarrowPhaseCallbacks(), new PoseIntegratorCallbacks(), new SolveDescription(new[] {2, 1, 1})); ``` -The above snippet would use 3 substeps with 2 velocity iterations on the first substep, then one velocity iteration on the second and third substeps. +The above snippet would use 3 substeps with 2 velocity iterations on the first substep, then 1 velocity iteration on the second and third substeps. + +This can be helpful when trying to find the absolute cheapest configuration that is still stable for a particular simulation. For example, a simulation with 1 velocity iteration per substep could be observed to be stable at 4 substeps but not at 3 substeps, and adding an extra velocity iteration to the first substep could make 3 substeps stable at a lower cost than 4 substeps. In other words, variable velocity iterations let you manage the simulation budget in a finer grained way. + +There are some cases where intentionally frontloading iterations could be useful as well. If you know the simulation has changed significantly since the last timestep- perhaps you've moved a bunch of bodies around such that the previous frame's guess at a constraint solution will be very wrong- then running a few more velocity iterations on the first timestep can avoid accumulating error. + +# Dynamic changes to substep and velocity iteration counts +The solver is sensitive to the effective timestep duration. It caches a best guess of the constraint solution which is sensitive to the amount of time passing between solves, so large changes can ruin the guess and harm stability. It's best to use the same timestep duration (`dt` passed into `Simulation.Timestep`) and the same number of substeps if possible, since the effective timestep duration seen by the solver is `dt / substepCount`. + +Incremental changes to the `dt` value can still work if they're reasonably small. 'Reasonably small' here has no precise definition; it will vary depending on the stability requirements of the simulation and how much error the application can tolerate. Changing `dt` by 1% per frame is probably okay for most simulations. Changing it by 50% per frame probably isn't. + +Changing the number of substeps is harder to do in a continuous way. Going from 4 to 3 substeps with a 60hz outer timestep rate means going from 240hz to 180hz effective solve rate instantly. That could be enough to cause problems for some simulations. You'd likely want to increase the number of velocity iterations in the first substep of the next frame to try to correct some of the induced error. + +It's also possible to update the cached guess in response to a timestep change using `Solver.ScaleAccumulatedImpulses`. The scale should be `newEffectiveTimestepDuration / oldEffectiveTimestepDuration`, or in other words `(newDt / newSubstepCount) / (oldDt / oldSubstepCount)`. This operation is not very cheap: it touches all accumulated impulses memory. + +Changing the number of *velocity iterations* from frame to frame is safe. The more velocity iterations there are, the closer the solution will converge to an optimum during the substep. # Callbacks From bed4ee01973bcb86679c66a4b9b3ed5396c4a5b5 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 28 Dec 2021 19:46:52 -0600 Subject: [PATCH 365/947] SpanHelper->BitOperations. --- .../Trees/Tree_MultithreadedRefitRefine.cs | 2 +- BepuUtilities/Collections/IndexSet.cs | 5 +- BepuUtilities/Collections/QuickQueue.cs | 5 +- BepuUtilities/Memory/BufferPool.cs | 60 +------------------ BepuUtilities/Memory/SpanHelper.cs | 46 ++------------ DemoRenderer/ShapeDrawing/MeshCache.cs | 2 +- DemoUtilities/TextBuilder.cs | 5 +- Demos/Program.cs | 2 +- 8 files changed, 19 insertions(+), 108 deletions(-) diff --git a/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs b/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs index 66ef6d835..6d56d7a3e 100644 --- a/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs +++ b/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs @@ -336,7 +336,7 @@ unsafe void RefitAndMark(int workerIndex) unsafe void Refine(int workerIndex) { var threadPool = threadDispatcher.GetThreadMemoryPool(workerIndex); - var subtreeCountEstimate = 1 << SpanHelper.GetContainingPowerOf2(MaximumSubtrees); + var subtreeCountEstimate = (int)BitOperations.RoundUpToPowerOf2((uint)MaximumSubtrees); var subtreeReferences = new QuickList(subtreeCountEstimate, threadPool); var treeletInternalNodes = new QuickList(subtreeCountEstimate, threadPool); diff --git a/BepuUtilities/Collections/IndexSet.cs b/BepuUtilities/Collections/IndexSet.cs index 2547b9321..e272d1582 100644 --- a/BepuUtilities/Collections/IndexSet.cs +++ b/BepuUtilities/Collections/IndexSet.cs @@ -1,6 +1,7 @@ using BepuUtilities.Memory; using System; using System.Diagnostics; +using System.Numerics; using System.Runtime.CompilerServices; namespace BepuUtilities.Collections @@ -112,7 +113,7 @@ public void Set(int index, BufferPool pool) if (bundleIndex >= Flags.Length) { //Note that the bundle index may be larger than two times the current capacity, since indices are not guaranteed to be appended. - InternalResizeForBundleCount(pool, 1 << SpanHelper.GetContainingPowerOf2(bundleIndex + 1)); + InternalResizeForBundleCount(pool, (int)BitOperations.RoundUpToPowerOf2((uint)(bundleIndex + 1))); } SetUnsafely(index, index >> shift); } @@ -153,7 +154,7 @@ public void Add(int index, BufferPool pool) if (bundleIndex >= Flags.Length) { //Note that the bundle index may be larger than two times the current capacity, since indices are not guaranteed to be appended. - InternalResizeForBundleCount(pool, 1 << SpanHelper.GetContainingPowerOf2(bundleIndex + 1)); + InternalResizeForBundleCount(pool, (int)BitOperations.RoundUpToPowerOf2((uint)(bundleIndex + 1))); } Debug.Assert((Flags[index >> shift] & (1ul << (index & mask))) == 0, "Cannot add if it's already present!"); SetUnsafely(index, bundleIndex); diff --git a/BepuUtilities/Collections/QuickQueue.cs b/BepuUtilities/Collections/QuickQueue.cs index ec62172b7..de3cae56c 100644 --- a/BepuUtilities/Collections/QuickQueue.cs +++ b/BepuUtilities/Collections/QuickQueue.cs @@ -3,6 +3,7 @@ using BepuUtilities.Memory; using System.Collections.Generic; using System.Runtime.CompilerServices; +using System.Numerics; namespace BepuUtilities.Collections { @@ -82,7 +83,7 @@ public ref T this[int index] [MethodImpl(MethodImplOptions.AggressiveInlining)] static int GetCapacityMask(int spanLength) { - return (1 << SpanHelper.GetPowerOf2(spanLength)) - 1; + return (1 << BitOperations.Log2((uint)spanLength)) - 1; } /// @@ -485,7 +486,7 @@ void ValidateIndex(int index) [Conditional("DEBUG")] static void ValidateSpanCapacity(ref Buffer span, int capacityMask) { - Debug.Assert((1 << SpanHelper.GetPowerOf2(span.Length)) - 1 == capacityMask, + Debug.Assert((1 << BitOperations.Log2((uint)span.Length)) - 1 == capacityMask, "Capacity mask should be the largest power of 2 that fits in the allocated span, minus one. This is necessary for efficient modulo operations."); } diff --git a/BepuUtilities/Memory/BufferPool.cs b/BepuUtilities/Memory/BufferPool.cs index e334fea4f..d0e06bd9e 100644 --- a/BepuUtilities/Memory/BufferPool.cs +++ b/BepuUtilities/Memory/BufferPool.cs @@ -113,7 +113,7 @@ public unsafe void Take(out Buffer buffer) var blockIndex = slot >> SuballocationsPerBlockShift; if (blockIndex >= Blocks.Length) { - Resize(1 << SpanHelper.GetContainingPowerOf2(blockIndex + 1)); + Resize((int)BitOperations.RoundUpToPowerOf2((uint)(blockIndex + 1))); } if (blockIndex >= BlockCount) { @@ -330,62 +330,6 @@ public unsafe void Return(ref Buffer buffer) where T : unmanaged buffer = default; } - //The resizes aren't particularly clever. They are more aggressive about reallocating than they need to be, but - //the assumption is that they will be used only when it's already known that a resize is necessary. - ///// - ///// Resizes a buffer to the smallest size available in the pool which contains the target size. Copies a subset of elements into the new buffer. - ///// Final buffer size is at least as large as the target size and may be larger. - ///// - ///// Buffer reference to resize. - ///// Number of bytes to resize the buffer for. - ///// Number of bytes to copy into the new buffer from the old buffer. - //[MethodImpl(MethodImplOptions.AggressiveInlining)] - //public unsafe void ResizeToAtLeast(ref Buffer buffer, int targetSize, int copyCount) - //{ - // Debug.Assert(copyCount <= buffer.Length, "Can't copy more than the capacity of the buffer."); - // Debug.Assert(copyCount <= targetSize, "Can't copy more than the target size."); - // //Only do anything if the new size is actually different from the current size. - // targetSize = 1 << (SpanHelper.GetContainingPowerOf2(targetSize)); - // if (buffer.Allocated) - // { - // DecomposeId(buffer.Id, out var powerIndex, out var slotIndex); - // var currentSize = 1 << powerIndex; - // if (currentSize != targetSize || pools[powerIndex].GetStartPointerForSlot(slotIndex) != buffer.Memory) - // { - // TakeAtLeast(targetSize, out var newBuffer); - // Buffer.MemoryCopy(buffer.Memory, newBuffer.Memory, buffer.Length, copyCount); - // pools[powerIndex].Return(slotIndex); - // buffer = newBuffer; - // } - // else - // { - // //While the allocation size is equal to the target size, the buffer might not be. - // //Fortunately, if the allocation stays the same size and the buffer start is at its original location, we can skip doing any work. - // //(With more work, you could expand *backwards*, we just didn't bother since this is exceptionally rare anyway. - // //The typed codepath doesn't bother doing this at all, and that's fine.) - // buffer.Length = targetSize; - // } - // } - // else - // { - // //Nothing to return or copy. - // TakeAtLeast(targetSize, out buffer); - // } - //} - - ///// - ///// Resizes a buffer to the target size. Copies a subset of elements into the new buffer. - ///// - ///// Buffer reference to resize. - ///// Number of bytes to resize the buffer for. - ///// Number of bytes to copy into the new buffer from the old buffer. - //[MethodImpl(MethodImplOptions.AggressiveInlining)] - //public unsafe void Resize(ref Buffer buffer, int targetSize, int copyCount) - //{ - // ResizeToAtLeast(ref buffer, targetSize, copyCount); - // buffer.Length = targetSize; - //} - /// /// Resizes a typed buffer to the smallest size available in the pool which contains the target size. Copies a subset of elements into the new buffer. /// Final buffer size is at least as large as the target size and may be larger. @@ -475,7 +419,7 @@ public static int GetCapacityForCount(int count) { if (count == 0) count = 1; - return (1 << SpanHelper.GetContainingPowerOf2(count * Unsafe.SizeOf())) / Unsafe.SizeOf(); + return ((int)BitOperations.RoundUpToPowerOf2((uint)(count * Unsafe.SizeOf()))) / Unsafe.SizeOf(); } [MethodImpl(MethodImplOptions.AggressiveInlining)] diff --git a/BepuUtilities/Memory/SpanHelper.cs b/BepuUtilities/Memory/SpanHelper.cs index eae243181..d74ad0196 100644 --- a/BepuUtilities/Memory/SpanHelper.cs +++ b/BepuUtilities/Memory/SpanHelper.cs @@ -1,7 +1,7 @@ using System; using System.Diagnostics; +using System.Numerics; using System.Runtime.CompilerServices; -using System.Runtime.InteropServices; namespace BepuUtilities.Memory { @@ -17,41 +17,7 @@ internal static void ValidatePower(int power) { Debug.Assert(power >= 0 && power <= MaximumSpanSizePower, $"Power must be from 0 to {MaximumSpanSizePower}, inclusive."); } - /// - /// Computes the largest integer N such that 2^N is less than or equal to i. - /// - /// Integer to compute the power of. - /// Lowest integer N such that 2^N is less than or equal to i. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static int GetPowerOf2(int i) - { - int log = 0; - if ((i & 0xFFFF0000) > 0) - { - i >>= 16; - log |= 16; - } - if ((i & 0xFF00) > 0) - { - i >>= 8; - log |= 8; - } - if ((i & 0xF0) > 0) - { - i >>= 4; - log |= 4; - } - if ((i & 0xC) > 0) - { - i >>= 2; - log |= 2; - } - if ((i & 0x2) > 0) - { - log |= 1; - } - return log; - } + /// /// Computes the lowest integer N such that 2^N >= i. /// @@ -60,11 +26,9 @@ public static int GetPowerOf2(int i) [MethodImpl(MethodImplOptions.AggressiveInlining)] public static int GetContainingPowerOf2(int i) { - Debug.Assert(i >= 0 && i <= (1 << MaximumSpanSizePower), "i must be from 0 to " + ((1 << MaximumSpanSizePower) - 1) + ", inclusive."); - //We want the buffer which would fully contain the count, so it should be effectively Ceiling(Log(i)). - //Doubling the value (and subtracting one, to avoid the already-a-power-of-two case) takes care of this. - i = ((i > 0 ? i : 1) << 1) - 1; - return GetPowerOf2(i); + var unsigned = i == 0 ? 1u : (uint)i; + return 32 - BitOperations.LeadingZeroCount(unsigned - 1); + } /// diff --git a/DemoRenderer/ShapeDrawing/MeshCache.cs b/DemoRenderer/ShapeDrawing/MeshCache.cs index 7edc1cf83..53fb29f51 100644 --- a/DemoRenderer/ShapeDrawing/MeshCache.cs +++ b/DemoRenderer/ShapeDrawing/MeshCache.cs @@ -69,7 +69,7 @@ public unsafe bool Allocate(ulong id, int vertexCount, out int start, out Buffer } //Didn't fit. We need to resize. var copyCount = TriangleBuffer.Capacity + vertexCount; - var newSize = 1 << SpanHelper.GetContainingPowerOf2(copyCount); + var newSize = (int)BitOperations.RoundUpToPowerOf2((uint)copyCount); Pool.ResizeToAtLeast(ref this.vertices, newSize, copyCount); allocator.Capacity = newSize; allocator.Allocate(id, vertexCount, out longStart); diff --git a/DemoUtilities/TextBuilder.cs b/DemoUtilities/TextBuilder.cs index 1b9864430..66f946e91 100644 --- a/DemoUtilities/TextBuilder.cs +++ b/DemoUtilities/TextBuilder.cs @@ -3,6 +3,7 @@ using System; using System.Collections.Generic; using System.Diagnostics; +using System.Numerics; using System.Runtime.CompilerServices; using System.Text; @@ -60,7 +61,7 @@ public TextBuilder Append(string text, int start, int count) { var newCount = this.count + count; if (newCount > characters.Length) - Array.Resize(ref characters, SpanHelper.GetContainingPowerOf2(newCount)); + Array.Resize(ref characters, (int)BitOperations.RoundUpToPowerOf2((uint)newCount)); int end = start + count; for (int i = start; i < end; ++i) { @@ -85,7 +86,7 @@ char GetCharForDigit(int digit) void Add(char character) { if (characters.Length == count) - Array.Resize(ref characters, SpanHelper.GetContainingPowerOf2(count * 2)); + Array.Resize(ref characters, (int)BitOperations.RoundUpToPowerOf2((uint)count + 1)); characters[count++] = character; } diff --git a/Demos/Program.cs b/Demos/Program.cs index 25be47890..504f59b69 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -24,7 +24,7 @@ static void Main(string[] args) //HeadlessTest.Test(content, 4, 32, 512); //HeadlessTest.Test(content, 4, 32, 512); //HeadlessTest.Test(content, 4, 128, 1024); - //DeterminismTest.Test(content, 1, 32768); + //DeterminismTest.Test(content, 1, 16384); //InvasiveHashDiagnostics.Initialize(5, 192000); var demo = new DemoHarness(loop, content); loop.Run(demo); From c3fae54d6f804facd1d70ab8e4ccb98636a007e0 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 1 Jan 2022 17:15:39 -0600 Subject: [PATCH 366/947] Deleted redundant overloads. --- BepuPhysics/StaticDescription.cs | 32 -------------------------------- 1 file changed, 32 deletions(-) diff --git a/BepuPhysics/StaticDescription.cs b/BepuPhysics/StaticDescription.cs index e6f86c380..1fa521d95 100644 --- a/BepuPhysics/StaticDescription.cs +++ b/BepuPhysics/StaticDescription.cs @@ -40,38 +40,6 @@ public StaticDescription(Vector3 position, Quaternion orientation, CollidableDes Pose.Orientation = orientation; Collidable = collidable; } - - /// - /// Builds a new static description. - /// - /// Position of the static. - /// Collidable description for the static. - public StaticDescription(Vector3 position, CollidableDescription collidable) : this(position, Quaternion.Identity, collidable) - { - } - - /// - /// Builds a new static description with discrete continuity. - /// - /// Position of the static. - /// Orientation of the static. - /// Index of the static's shape in the simulation shapes set. - public StaticDescription(Vector3 position, Quaternion orientation, TypedIndex shapeIndex) - { - Pose.Position = position; - Pose.Orientation = orientation; - Collidable.Continuity = ContinuousDetection.Passive; - Collidable.Shape = shapeIndex; - } - - /// - /// Builds a new static description with discrete continuity. - /// - /// Position of the static. - /// Index of the static's shape in the simulation shapes set. - public StaticDescription(Vector3 position, TypedIndex shapeIndex) : this(position, Quaternion.Identity, shapeIndex) - { - } } } From a653da00c3f915527e3dec6ebdd7f4510268bbb8 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 1 Jan 2022 19:19:57 -0600 Subject: [PATCH 367/947] The dancer prepares. --- Demos/DemoSet.cs | 1 + Demos/Demos/DancerDemo.cs | 222 ++++++++++++++++++++++++++++++++++++++ Demos/Program.cs | 2 +- 3 files changed, 224 insertions(+), 1 deletion(-) create mode 100644 Demos/Demos/DancerDemo.cs diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index 2b7dbc630..e7ccc0611 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -45,6 +45,7 @@ struct Option public DemoSet() { + AddOption(); AddOption(); AddOption(); AddOption(); diff --git a/Demos/Demos/DancerDemo.cs b/Demos/Demos/DancerDemo.cs new file mode 100644 index 000000000..17cc3897f --- /dev/null +++ b/Demos/Demos/DancerDemo.cs @@ -0,0 +1,222 @@ +using BepuPhysics; +using BepuPhysics.Collidables; +using BepuPhysics.CollisionDetection; +using BepuPhysics.Constraints; +using BepuUtilities; +using DemoContentLoader; +using DemoRenderer; +using System; +using System.Numerics; +using System.Runtime.CompilerServices; + +namespace Demos.Demos +{ + public class DancerDemo : Demo + { + struct DollBodyHandles + { + public BodyHandle Hips; + public BodyHandle Abdomen; + public BodyHandle Chest; + public BodyHandle Head; + public BodyHandle UpperLeftLeg; + public BodyHandle LowerLeftLeg; + public BodyHandle UpperRightLeg; + public BodyHandle LowerRightLeg; + public BodyHandle UpperLeftArm; + public BodyHandle LowerLeftArm; + public BodyHandle UpperRightArm; + public BodyHandle LowerRightArm; + } + + struct Dancer + { + //Each dancer has its own simulation. The goal is to show a common use case for cosmetic physics- single threaded simulations that don't interact with the main simulation, running in parallel with other simulations. + public Simulation Simulation; + //The body handles are cached in each simulation so the source states can be mapped onto each dancer. + public DollBodyHandles BodyHandles; + } + + DollBodyHandles sourceBodyHandles; + Dancer[] dancers; + + + struct Control + { + public ConstraintHandle Servo; + public Vector3 LocalOffset; + public ServoSettings ServoSettings; + public SpringSettings SpringSettings; + + public Control(Simulation simulation, BodyHandle body, Vector3 worldControlPoint, ServoSettings servoSettings, SpringSettings springSettings) + { + + ServoSettings = servoSettings; + SpringSettings = springSettings; + var pose = simulation.Bodies[body].Pose; + LocalOffset = QuaternionEx.Transform(worldControlPoint - pose.Position, Quaternion.Conjugate(pose.Orientation)); + Servo = simulation.Solver.Add(body, new OneBodyLinearServo { ServoSettings = servoSettings, SpringSettings = springSettings, LocalOffset = LocalOffset, Target = worldControlPoint }); + } + + public void UpdateTarget(Simulation simulation, Vector3 target) + { + simulation.Solver.ApplyDescription(Servo, new OneBodyLinearServo { ServoSettings = ServoSettings, SpringSettings = SpringSettings, LocalOffset = LocalOffset, Target = target }); + } + + } + Control hipsControl; + Control leftFootControl; + Control rightFootControl; + Control leftHandControl; + Control rightHandControl; + + + + + static void Connect(Simulation simulation, BodyHandle a, BodyHandle b, Vector3 jointLocation, SpringSettings springSettings, ref SubgroupCollisionFilter filterA, ref SubgroupCollisionFilter filterB) + { + var poseA = simulation.Bodies[a].Pose; + var poseB = simulation.Bodies[b].Pose; + simulation.Solver.Add(a, b, + new BallSocket + { + LocalOffsetA = QuaternionEx.Transform(jointLocation - poseA.Position, Quaternion.Conjugate(poseA.Orientation)), + LocalOffsetB = QuaternionEx.Transform(jointLocation - poseB.Position, Quaternion.Conjugate(poseB.Orientation)), + SpringSettings = springSettings + }); + SubgroupCollisionFilter.DisableCollision(ref filterA, ref filterB); + } + + static void CreateLimb(Simulation simulation, CollidableProperty collisionFilters, + BodyHandle body, BodyHandle upperLimb, BodyHandle lowerLimb, + Vector3 bodyToUpperJointLocation, Vector3 upperToLowerJointLocation, SpringSettings springSettings, + int groupIndex, int limbSubgroupIndexStart) + { + ref var bodyFilter = ref collisionFilters[body]; + ref var upperFilter = ref collisionFilters.Allocate(upperLimb); + ref var lowerFilter = ref collisionFilters.Allocate(lowerLimb); + upperFilter = new SubgroupCollisionFilter(groupIndex, limbSubgroupIndexStart); + lowerFilter = new SubgroupCollisionFilter(groupIndex, limbSubgroupIndexStart + 1); + Connect(simulation, body, upperLimb, bodyToUpperJointLocation, springSettings, ref bodyFilter, ref upperFilter); + Connect(simulation, upperLimb, lowerLimb, upperToLowerJointLocation, springSettings, ref upperFilter, ref lowerFilter); + + } + + static void ConstrainOrientation(Simulation simulation, BodyHandle a, BodyHandle b) + { + simulation.Solver.Add(a, b, new AngularServo + { + ServoSettings = ServoSettings.Default, + SpringSettings = new SpringSettings(6, 1), + TargetRelativeRotationLocalA = Quaternion.Concatenate(simulation.Bodies[b].Pose.Orientation, Quaternion.Conjugate(simulation.Bodies[a].Pose.Orientation)) + }); + } + + public unsafe override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(0, 4, 10); + camera.Yaw = 0; + camera.Pitch = 0; + + var collisionFilters = new CollidableProperty(); + //Note very high damping on the main ragdoll simulation; makes it easier to pose. + Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks { CollisionFilters = collisionFilters }, new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0), 0.95f, 0.95f), new SolveDescription(8, 1)); + + var hipsPosition = new Vector3(0, 0, 0); + var abdomenPosition = hipsPosition + new Vector3(0, 0.25f, 0); + var chestPosition = abdomenPosition + new Vector3(0, 0.25f, 0); + var headPosition = chestPosition + new Vector3(0, 0.4f, 0); + //It's helpful to have joint locations for the limbs so we can create capsules from the endpoints. + //There's not going to be a foot or hand body in this demo, since those aren't important for scooting a dress around. + var kneePosition = hipsPosition - new Vector3(0, 0.5f, 0); + var anklePosition = kneePosition - new Vector3(0, 0.5f, 0); + var elbowPosition = chestPosition + new Vector3(0, 0.39f, 0); + var wristPosition = elbowPosition + new Vector3(0, 0.39f, 0); + var armOffset = new Vector3(0.25f, 0, 0); + var legOffset = new Vector3(0.135f, 0, 0); + const int groupIndex = 0; + + //Build the torso and head bodies. + RagdollDemo.GetCapsuleForLineSegment(hipsPosition - legOffset, hipsPosition + legOffset, 0.14f, out var hipShape, out _, out var hipOrientation); + sourceBodyHandles.Hips = Simulation.Bodies.Add(BodyDescription.CreateDynamic((hipsPosition, hipOrientation), hipShape.ComputeInertia(1), Simulation.Shapes.Add(hipShape), 0.01f)); + ref var hipsFilter = ref collisionFilters.Allocate(sourceBodyHandles.Hips); + hipsFilter = new SubgroupCollisionFilter(groupIndex, 0); + + RagdollDemo.GetCapsuleForLineSegment(abdomenPosition - legOffset * 0.8f, abdomenPosition + legOffset * 0.8f, 0.13f, out var abdomenShape, out _, out var abdomenOrientation); + sourceBodyHandles.Abdomen = Simulation.Bodies.Add(BodyDescription.CreateDynamic((abdomenPosition, abdomenOrientation), abdomenShape.ComputeInertia(1), Simulation.Shapes.Add(abdomenShape), 0.01f)); + ref var abdomenFilter = ref collisionFilters.Allocate(sourceBodyHandles.Abdomen); + abdomenFilter = new SubgroupCollisionFilter(groupIndex, 1); + + RagdollDemo.GetCapsuleForLineSegment(abdomenPosition - legOffset * 0.8f, abdomenPosition + legOffset * 0.8f, 0.165f, out var chestShape, out _, out var chestOrientation); + sourceBodyHandles.Chest = Simulation.Bodies.Add(BodyDescription.CreateDynamic((chestPosition, chestOrientation), chestShape.ComputeInertia(1), Simulation.Shapes.Add(chestShape), 0.01f)); + ref var chestFilter = ref collisionFilters.Allocate(sourceBodyHandles.Chest); + chestFilter = new SubgroupCollisionFilter(groupIndex, 2); + + var headShape = new Sphere(0.17f); + sourceBodyHandles.Head = Simulation.Bodies.Add(BodyDescription.CreateDynamic(headPosition, headShape.ComputeInertia(1), Simulation.Shapes.Add(headShape), 1e-2f)); + ref var headFilter = ref collisionFilters.Allocate(sourceBodyHandles.Head); + headFilter = new SubgroupCollisionFilter(groupIndex, 3); + + var springSettings = new SpringSettings(30, 1); + Connect(Simulation, sourceBodyHandles.Hips, sourceBodyHandles.Abdomen, 0.5f * (hipsPosition + abdomenPosition), springSettings, ref hipsFilter, ref abdomenFilter); + ConstrainOrientation(Simulation, sourceBodyHandles.Hips, sourceBodyHandles.Abdomen); + Connect(Simulation, sourceBodyHandles.Abdomen, sourceBodyHandles.Chest, 0.5f * (abdomenPosition + chestPosition), springSettings, ref abdomenFilter, ref chestFilter); + ConstrainOrientation(Simulation, sourceBodyHandles.Abdomen, sourceBodyHandles.Chest); + Connect(Simulation, sourceBodyHandles.Chest, sourceBodyHandles.Head, 0.5f * (chestPosition + headPosition), springSettings, ref chestFilter, ref headFilter); + ConstrainOrientation(Simulation, sourceBodyHandles.Chest, sourceBodyHandles.Head); + + //Create the legs. + RagdollDemo.GetCapsuleForLineSegment(hipsPosition, kneePosition, 0.11f, out var upperLegShape, out var upperLegPosition, out var upperLegOrientation); + RagdollDemo.GetCapsuleForLineSegment(kneePosition, anklePosition, 0.1f, out var lowerLegShape, out var lowerLegPosition, out var lowerLegOrientation); + var upperLegDescription = BodyDescription.CreateDynamic((upperLegPosition, upperLegOrientation), upperLegShape.ComputeInertia(1), Simulation.Shapes.Add(upperLegShape), 0.01f); + var lowerLegDescription = BodyDescription.CreateDynamic((lowerLegPosition, lowerLegOrientation), lowerLegShape.ComputeInertia(1), Simulation.Shapes.Add(lowerLegShape), 0.01f); + + upperLegDescription.Pose.Position -= legOffset; + lowerLegDescription.Pose.Position -= legOffset; + sourceBodyHandles.UpperLeftLeg = Simulation.Bodies.Add(upperLegDescription); + sourceBodyHandles.LowerLeftLeg = Simulation.Bodies.Add(lowerLegDescription); + upperLegDescription.Pose.Position += 2 * legOffset; + lowerLegDescription.Pose.Position += 2 * legOffset; + sourceBodyHandles.UpperRightLeg = Simulation.Bodies.Add(upperLegDescription); + sourceBodyHandles.LowerRightLeg = Simulation.Bodies.Add(lowerLegDescription); + + CreateLimb(Simulation, collisionFilters, sourceBodyHandles.Hips, sourceBodyHandles.UpperLeftLeg, sourceBodyHandles.LowerLeftLeg, hipsPosition - legOffset, kneePosition - legOffset, springSettings, groupIndex, 4); + CreateLimb(Simulation, collisionFilters, sourceBodyHandles.Hips, sourceBodyHandles.UpperRightLeg, sourceBodyHandles.LowerRightLeg, hipsPosition + legOffset, kneePosition + legOffset, springSettings, groupIndex, 6); + + //Create the arms. + RagdollDemo.GetCapsuleForLineSegment(chestPosition, elbowPosition, 0.08f, out var upperArmShape, out var upperArmPosition, out var upperArmOrientation); + RagdollDemo.GetCapsuleForLineSegment(elbowPosition, wristPosition, 0.075f, out var lowerArmShape, out var lowerArmPosition, out var lowerArmOrientation); + var upperArmDescription = BodyDescription.CreateDynamic((upperArmPosition, upperArmOrientation), upperArmShape.ComputeInertia(1), Simulation.Shapes.Add(upperArmShape), 0.01f); + var lowerArmDescription = BodyDescription.CreateDynamic((lowerArmPosition, lowerArmOrientation), lowerArmShape.ComputeInertia(1), Simulation.Shapes.Add(lowerArmShape), 0.01f); + + upperArmDescription.Pose.Position -= armOffset; + lowerArmDescription.Pose.Position -= armOffset; + sourceBodyHandles.UpperLeftArm = Simulation.Bodies.Add(upperArmDescription); + sourceBodyHandles.LowerLeftArm = Simulation.Bodies.Add(lowerArmDescription); + upperArmDescription.Pose.Position += 2 * armOffset; + lowerArmDescription.Pose.Position += 2 * armOffset; + sourceBodyHandles.UpperRightArm = Simulation.Bodies.Add(upperArmDescription); + sourceBodyHandles.LowerRightArm = Simulation.Bodies.Add(lowerArmDescription); + + CreateLimb(Simulation, collisionFilters, sourceBodyHandles.Chest, sourceBodyHandles.UpperLeftArm, sourceBodyHandles.LowerLeftArm, chestPosition - armOffset, elbowPosition - armOffset, springSettings, groupIndex, 8); + CreateLimb(Simulation, collisionFilters, sourceBodyHandles.Chest, sourceBodyHandles.UpperRightArm, sourceBodyHandles.LowerRightArm, chestPosition + armOffset, elbowPosition + armOffset, springSettings, groupIndex, 10); + + //Create controls. + hipsControl = new Control(Simulation, sourceBodyHandles.Hips, hipsPosition, ServoSettings.Default, new SpringSettings(30, 1)); + Simulation.Solver.Add(sourceBodyHandles.Hips, new OneBodyAngularServo { ServoSettings = ServoSettings.Default, SpringSettings = new SpringSettings(30, 1), TargetOrientation = Simulation.Bodies[sourceBodyHandles.Hips].Pose.Orientation }); + + var limbServoSettings = ServoSettings.Default; + var limbSpringSettings = new SpringSettings(4, 1); + leftFootControl = new Control(Simulation, sourceBodyHandles.LowerLeftLeg, anklePosition - legOffset, limbServoSettings, limbSpringSettings); + rightFootControl = new Control(Simulation, sourceBodyHandles.LowerRightLeg, anklePosition + legOffset, limbServoSettings, limbSpringSettings); + leftHandControl = new Control(Simulation, sourceBodyHandles.LowerLeftArm, wristPosition - armOffset, limbServoSettings, limbSpringSettings); + rightHandControl = new Control(Simulation, sourceBodyHandles.LowerRightArm, wristPosition + armOffset, limbServoSettings, limbSpringSettings); + + + Simulation.Statics.Add(new(new Vector3(0, -10.5f, 0), Simulation.Shapes.Add(new Box(1000, 1, 1000)))); + + + + } + } +} diff --git a/Demos/Program.cs b/Demos/Program.cs index 504f59b69..402a08dee 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -24,7 +24,7 @@ static void Main(string[] args) //HeadlessTest.Test(content, 4, 32, 512); //HeadlessTest.Test(content, 4, 32, 512); //HeadlessTest.Test(content, 4, 128, 1024); - //DeterminismTest.Test(content, 1, 16384); + //DeterminismTest.Test(content, 1, 65536); //InvasiveHashDiagnostics.Initialize(5, 192000); var demo = new DemoHarness(loop, content); loop.Run(demo); From bacc899035294b2db8df6567d84065ac88fa90ff Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 5 Jan 2022 11:47:34 -0600 Subject: [PATCH 368/947] Dancer dancing a bit. --- Demos/Demos/DancerDemo.cs | 85 +++++++++++++++++++++++++++++++++++++-- 1 file changed, 82 insertions(+), 3 deletions(-) diff --git a/Demos/Demos/DancerDemo.cs b/Demos/Demos/DancerDemo.cs index 17cc3897f..82e5e6123 100644 --- a/Demos/Demos/DancerDemo.cs +++ b/Demos/Demos/DancerDemo.cs @@ -5,6 +5,7 @@ using BepuUtilities; using DemoContentLoader; using DemoRenderer; +using DemoUtilities; using System; using System.Numerics; using System.Runtime.CompilerServices; @@ -99,6 +100,34 @@ static void CreateLimb(Simulation simulation, CollidableProperty Date: Wed, 5 Jan 2022 20:06:46 -0600 Subject: [PATCH 369/947] Dancer's intertemporal shadow assistants added. --- BepuPhysics/Bodies.cs | 14 ++++++ Demos/Demos/DancerDemo.cs | 89 ++++++++++++++++++++++++++++++++++++++- 2 files changed, 101 insertions(+), 2 deletions(-) diff --git a/BepuPhysics/Bodies.cs b/BepuPhysics/Bodies.cs index 2fdae69b7..fe602de5a 100644 --- a/BepuPhysics/Bodies.cs +++ b/BepuPhysics/Bodies.cs @@ -537,6 +537,20 @@ public void GetDescription(BodyHandle handle, out BodyDescription description) set.GetDescription(location.Index, out description); } + /// + /// Gets the description of a body by handle. + /// + /// Handle of the body to look up. + /// Description of the body. + public BodyDescription GetDescription(BodyHandle handle) + { + ValidateExistingHandle(handle); + ref var location = ref HandleToLocation[handle.Value]; + ref var set = ref Sets[location.SetIndex]; + set.GetDescription(location.Index, out var description); + return description; + } + /// /// Gets a reference to a body by its handle. /// diff --git a/Demos/Demos/DancerDemo.cs b/Demos/Demos/DancerDemo.cs index 82e5e6123..211a8a264 100644 --- a/Demos/Demos/DancerDemo.cs +++ b/Demos/Demos/DancerDemo.cs @@ -3,17 +3,22 @@ using BepuPhysics.CollisionDetection; using BepuPhysics.Constraints; using BepuUtilities; +using BepuUtilities.Collections; +using BepuUtilities.Memory; using DemoContentLoader; using DemoRenderer; +using DemoRenderer.UI; using DemoUtilities; using System; using System.Numerics; using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; namespace Demos.Demos { public class DancerDemo : Demo { + [StructLayout(LayoutKind.Sequential, Pack = 4)] struct DollBodyHandles { public BodyHandle Hips; @@ -28,6 +33,11 @@ struct DollBodyHandles public BodyHandle LowerLeftArm; public BodyHandle UpperRightArm; public BodyHandle LowerRightArm; + + internal static unsafe Buffer AsBuffer(DollBodyHandles* sourceBodyHandles) + { + return new Buffer(sourceBodyHandles, 12); + } } struct Dancer @@ -245,8 +255,47 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Simulation.Statics.Add(new(new Vector3(0, -10.5f, 0), Simulation.Shapes.Add(new Box(1000, 1, 1000)))); + + + //All the background dancers read different historical motion states so that they're not just all doing the exact same thing. + //Keep the states in a queue. Each batch of 12 motion states is the state for a single frame. + motionHistory = new QuickQueue(historyLength * 12, BufferPool); + + dancers = new Dancer[16]; + static BodyHandle CreateCopyForDancer(Simulation sourceSimulation, BodyHandle sourceHandle, TypedIndex shapeIndexInTargetSimulation, Simulation targetSimulation) + { + var description = sourceSimulation.Bodies.GetDescription(sourceHandle); + description.Collidable.Shape = shapeIndexInTargetSimulation; + description.LocalInertia = default; + return targetSimulation.Bodies.Add(description); + } + + for (int i = 0; i < dancers.Length; ++i) + { + ref var dancer = ref dancers[i]; + dancer.Simulation = Simulation.Create(new BufferPool(), new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(default), new SolveDescription(1, 4)); + dancer.BodyHandles.Hips = CreateCopyForDancer(Simulation, sourceBodyHandles.Hips, dancer.Simulation.Shapes.Add(hipShape), dancer.Simulation); + dancer.BodyHandles.Abdomen = CreateCopyForDancer(Simulation, sourceBodyHandles.Abdomen, dancer.Simulation.Shapes.Add(abdomenShape), dancer.Simulation); + dancer.BodyHandles.Chest = CreateCopyForDancer(Simulation, sourceBodyHandles.Chest, dancer.Simulation.Shapes.Add(chestShape), dancer.Simulation); + dancer.BodyHandles.Head = CreateCopyForDancer(Simulation, sourceBodyHandles.Head, dancer.Simulation.Shapes.Add(headShape), dancer.Simulation); + + var upperLegShapeInTarget = dancer.Simulation.Shapes.Add(upperLegShape); + var lowerLegShapeInTarget = dancer.Simulation.Shapes.Add(lowerLegShape); + dancer.BodyHandles.UpperLeftLeg = CreateCopyForDancer(Simulation, sourceBodyHandles.UpperLeftLeg, upperLegShapeInTarget, dancer.Simulation); + dancer.BodyHandles.LowerLeftLeg = CreateCopyForDancer(Simulation, sourceBodyHandles.LowerLeftLeg, lowerLegShapeInTarget, dancer.Simulation); + dancer.BodyHandles.UpperRightLeg = CreateCopyForDancer(Simulation, sourceBodyHandles.UpperRightLeg, upperLegShapeInTarget, dancer.Simulation); + dancer.BodyHandles.LowerRightLeg = CreateCopyForDancer(Simulation, sourceBodyHandles.LowerRightLeg, lowerLegShapeInTarget, dancer.Simulation); + + var upperArmShapeInTarget = dancer.Simulation.Shapes.Add(upperArmShape); + var lowerArmShapeInTarget = dancer.Simulation.Shapes.Add(lowerArmShape); + dancer.BodyHandles.UpperLeftArm = CreateCopyForDancer(Simulation, sourceBodyHandles.UpperLeftArm, upperArmShapeInTarget, dancer.Simulation); + dancer.BodyHandles.LowerLeftArm = CreateCopyForDancer(Simulation, sourceBodyHandles.LowerLeftArm, lowerArmShapeInTarget, dancer.Simulation); + dancer.BodyHandles.UpperRightArm = CreateCopyForDancer(Simulation, sourceBodyHandles.UpperRightArm, upperArmShapeInTarget, dancer.Simulation); + dancer.BodyHandles.LowerRightArm = CreateCopyForDancer(Simulation, sourceBodyHandles.LowerRightArm, lowerArmShapeInTarget, dancer.Simulation); + } } + const int historyLength = 256; double time; float Smoothstep(float v) @@ -274,12 +323,14 @@ Vector3 CreateArmTarget(float t) return new Vector3(-xOffset - armOffsetX, yOffset, zOffset); } - public override void Update(Window window, Camera camera, Input input, float dt) + QuickQueue motionHistory; + + public unsafe override void Update(Window window, Camera camera, Input input, float dt) { time += dt; var hipsTarget = new Vector3(0, 0, 3 * (float)Math.Sin(time / 4)); hipsControl.UpdateTarget(Simulation, hipsTarget); - const float stepDuration = 3; + const float stepDuration = 1.5f; var scaledTime = time / stepDuration; var t = (float)(scaledTime - Math.Floor(scaledTime)); var tOffset = (t + 0.5f) % 1; @@ -295,7 +346,41 @@ public override void Update(Window window, Camera camera, Input input, float dt) rightArmLocalTarget.X *= -1; leftHandControl.UpdateTarget(Simulation, hipsTarget + leftArmLocalTarget); rightHandControl.UpdateTarget(Simulation, hipsTarget + rightArmLocalTarget); + + //Record the latest motion state from the source dancer. + var sourceHandleBuffer = DollBodyHandles.AsBuffer((DollBodyHandles*)Unsafe.AsPointer(ref sourceBodyHandles)); + if (motionHistory.Count == historyLength) + for (int i = 0; i < sourceHandleBuffer.Length; ++i) + motionHistory.Dequeue(); + + for (int i = 0; i < sourceHandleBuffer.Length; ++i) + { + motionHistory.EnqueueUnsafely(Simulation.Bodies[sourceHandleBuffer[i]].MotionState); + } + + //Copy historical motion states to the dancers. + for (int i = 0; i < dancers.Length; ++i) + { + ref var dancer = ref dancers[i]; + var historicalStateStartIndex = motionHistory.Count - sourceHandleBuffer.Length * (i * 4 + 1); + if (historicalStateStartIndex < 0) + historicalStateStartIndex = 0; + var targetHandleBuffer = DollBodyHandles.AsBuffer((DollBodyHandles*)Unsafe.AsPointer(ref dancer.BodyHandles)); + for (int j = 0; j < sourceHandleBuffer.Length; ++j) + { + dancer.Simulation.Bodies[targetHandleBuffer[j]].MotionState = motionHistory[historicalStateStartIndex + j]; + } + } base.Update(window, camera, input, dt); } + + public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + { + for (int i = 0; i < dancers.Length; ++i) + { + renderer.Shapes.AddInstances(dancers[i].Simulation); + } + base.Render(renderer, camera, input, text, font); + } } } From a0a7a0b534caf2fe888918d0e97ac79eca21321b Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 6 Jan 2022 19:04:47 -0600 Subject: [PATCH 370/947] Dancer swarm now wearing ponchos. --- BepuPhysics/BodyReference.cs | 9 ++ Demos/Demos/ClothDemo.cs | 25 +++-- Demos/Demos/DancerDemo.cs | 203 +++++++++++++++++++++++++++++------ 3 files changed, 194 insertions(+), 43 deletions(-) diff --git a/BepuPhysics/BodyReference.cs b/BepuPhysics/BodyReference.cs index f687b3ce9..8bd062b4f 100644 --- a/BepuPhysics/BodyReference.cs +++ b/BepuPhysics/BodyReference.cs @@ -450,5 +450,14 @@ public void ApplyAngularImpulse(in Vector3 angularImpulse) PoseIntegration.RotateInverseInertia(state.Inertia.Local.InverseInertiaTensor, state.Motion.Pose.Orientation, out var inverseInertia); ApplyAngularImpulse(angularImpulse, inverseInertia, ref state.Motion.Velocity.Angular); } + + /// + /// Implicitly converts a to the that the body reference was created from. + /// + /// Body reference to extract the handle from. + public static implicit operator BodyHandle(BodyReference reference) + { + return reference.Handle; + } } } diff --git a/Demos/Demos/ClothDemo.cs b/Demos/Demos/ClothDemo.cs index a3e83b152..4175dcbd2 100644 --- a/Demos/Demos/ClothDemo.cs +++ b/Demos/Demos/ClothDemo.cs @@ -36,7 +36,7 @@ public static bool Test(in ClothCollisionFilter a, in ClothCollisionFilter b) if (a.instanceId != b.instanceId) return true; //Disallow collisions between vertices which are near each other. We measure distance as max(abs(ax - bx), abs(ay - by), abs(az - bz)). - const int minimumDistance = 1; + const int minimumDistance = 2; var differenceX = a.x - b.x; if (differenceX < -minimumDistance || differenceX > minimumDistance) return true; @@ -51,9 +51,16 @@ public static bool Test(in ClothCollisionFilter a, in ClothCollisionFilter b) struct ClothCallbacks : INarrowPhaseCallbacks { public CollidableProperty Filters; + public PairMaterialProperties Material; public void Initialize(Simulation simulation) { Filters.Initialize(simulation); + if (Material.SpringSettings.AngularFrequency == 0 && Material.SpringSettings.TwiceDampingRatio == 0) + { + Material.SpringSettings = new SpringSettings(30, 1); + Material.FrictionCoefficient = 0.25f; + Material.MaximumRecoveryVelocity = 2f; + } } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -75,9 +82,7 @@ public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int chi [MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold { - pairMaterial.FrictionCoefficient = 0.25f; - pairMaterial.MaximumRecoveryVelocity = 2f; - pairMaterial.SpringSettings = new SpringSettings(30, 1); + pairMaterial = Material; return true; } @@ -132,10 +137,10 @@ void CreateAreaConstraints(BodyHandle[,] bodyHandles, SpringSettings springSetti var bHandle = bodyHandles[rowIndex + 1, columnIndex]; var cHandle = bodyHandles[rowIndex, columnIndex + 1]; var dHandle = bodyHandles[rowIndex + 1, columnIndex + 1]; - var a = new BodyReference(aHandle, Simulation.Bodies); - var b = new BodyReference(bHandle, Simulation.Bodies); - var c = new BodyReference(cHandle, Simulation.Bodies); - var d = new BodyReference(dHandle, Simulation.Bodies); + var a = Simulation.Bodies[aHandle]; + var b = Simulation.Bodies[bHandle]; + var c = Simulation.Bodies[cHandle]; + var d = Simulation.Bodies[dHandle]; //Not worried about kinematics here- we create at most one row of kinematics in this demo. These are three body constraints that operate in a local quad, so //there's no way for them to all be kinematic. Simulation.Solver.Add(aHandle, bHandle, cHandle, new AreaConstraint(a.Pose.Position, b.Pose.Position, c.Pose.Position, springSettings)); @@ -147,8 +152,8 @@ void CreateDistanceConstraints(BodyHandle[,] bodyHandles, SpringSettings springS { void CreateConstraintBetweenBodies(BodyHandle aHandle, BodyHandle bHandle) { - var a = new BodyReference(aHandle, Simulation.Bodies); - var b = new BodyReference(bHandle, Simulation.Bodies); + var a = Simulation.Bodies[aHandle]; + var b = Simulation.Bodies[bHandle]; //Don't create constraints between two kinematic bodies. if (a.LocalInertia.InverseMass > 0 || b.LocalInertia.InverseMass > 0) { diff --git a/Demos/Demos/DancerDemo.cs b/Demos/Demos/DancerDemo.cs index 211a8a264..1aa664272 100644 --- a/Demos/Demos/DancerDemo.cs +++ b/Demos/Demos/DancerDemo.cs @@ -10,6 +10,7 @@ using DemoRenderer.UI; using DemoUtilities; using System; +using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; using System.Runtime.InteropServices; @@ -49,6 +50,7 @@ struct Dancer } DollBodyHandles sourceBodyHandles; + ParallelLooper looper; Dancer[] dancers; @@ -153,6 +155,114 @@ static void ConstrainOrientation(Simulation simulation, BodyHandle a, BodyHandle const float legOffsetX = 0.135f; const float armOffsetX = 0.25f; + + + QuickQueue motionHistory; + static Vector3 GetOffsetForDancer(int i) + { + const float spacing = 2; + return new Vector3(dancerCount * spacing / -2 + (i + 0.5f) * spacing, 0, -5); + } + + static BodyHandle[,] CreateBodyGrid(Vector3 position, int widthInNodes, int lengthInNodes, float spacing, float bodyRadius, float massPerBody, + int instanceId, Simulation simulation, CollidableProperty filters) + { + var description = BodyDescription.CreateDynamic(QuaternionEx.Identity, new BodyInertia { InverseMass = 1f / massPerBody }, simulation.Shapes.Add(new Sphere(bodyRadius)), 0.01f); + BodyHandle[,] handles = new BodyHandle[lengthInNodes, widthInNodes]; + var armHoleCenter = new Vector2(armOffsetX + 0.05f, 0); + var armHoleRadius = 0.05f; + var armHoleRadiusSquared = armHoleRadius * armHoleRadius; + for (int rowIndex = 0; rowIndex < lengthInNodes; ++rowIndex) + { + for (int columnIndex = 0; columnIndex < widthInNodes; ++columnIndex) + { + Vector2 horizontalPosition = new(columnIndex * spacing - widthInNodes * spacing / 2f, rowIndex * spacing - lengthInNodes * spacing / 2); + var distance0 = Vector2.DistanceSquared(horizontalPosition, armHoleCenter); + var distance1 = Vector2.DistanceSquared(horizontalPosition, -armHoleCenter); + if (distance0 < armHoleRadiusSquared || distance1 < armHoleRadiusSquared) + { + //Too close to an arm, don't create any bodies here. + handles[rowIndex, columnIndex] = new BodyHandle { Value = -1 }; + } + else + { + description.Pose.Position = new Vector3(horizontalPosition.X, 0, horizontalPosition.Y) + position; + var handle = simulation.Bodies.Add(description); + handles[rowIndex, columnIndex] = handle; + if (filters != null) + filters.Allocate(handle) = new ClothCollisionFilter(rowIndex, columnIndex, instanceId); + } + } + } + return handles; + } + + static void CreateAreaConstraints(BodyHandle[,] bodyHandles, SpringSettings springSettings, Simulation simulation) + { + for (int rowIndex = 0; rowIndex < bodyHandles.GetLength(0) - 1; ++rowIndex) + { + for (int columnIndex = 0; columnIndex < bodyHandles.GetLength(1) - 1; ++columnIndex) + { + var aHandle = bodyHandles[rowIndex, columnIndex]; + var bHandle = bodyHandles[rowIndex + 1, columnIndex]; + var cHandle = bodyHandles[rowIndex, columnIndex + 1]; + var dHandle = bodyHandles[rowIndex + 1, columnIndex + 1]; + //Only create a constraint if bodies on all sides of the quad actually exist. + //In this demo, we use -1 in the body handle slot to represent 'no body'. + if (aHandle.Value >= 0 && bHandle.Value >= 0 && cHandle.Value >= 0 && dHandle.Value >= 0) + { + var a = simulation.Bodies[aHandle]; + var b = simulation.Bodies[bHandle]; + var c = simulation.Bodies[cHandle]; + var d = simulation.Bodies[dHandle]; + simulation.Solver.Add(aHandle, bHandle, cHandle, new AreaConstraint(a.Pose.Position, b.Pose.Position, c.Pose.Position, springSettings)); + simulation.Solver.Add(bHandle, cHandle, dHandle, new AreaConstraint(b.Pose.Position, c.Pose.Position, d.Pose.Position, springSettings)); + } + } + } + } + static void CreateDistanceConstraints(BodyHandle[,] bodyHandles, SpringSettings springSettings, Simulation simulation) + { + void CreateConstraintBetweenBodies(BodyHandle aHandle, BodyHandle bHandle) + { + //Only create a constraint if bodies on both sides of the pair actually exist. + //In this demo, we use -1 in the body handle slot to represent 'no body'. + if (aHandle.Value >= 0 && bHandle.Value >= 0) + { + var a = simulation.Bodies[aHandle]; + var b = simulation.Bodies[bHandle]; + simulation.Solver.Add(aHandle, bHandle, new CenterDistanceConstraint(Vector3.Distance(a.Pose.Position, b.Pose.Position), springSettings)); + } + } + for (int rowIndex = 0; rowIndex < bodyHandles.GetLength(0); ++rowIndex) + { + for (int columnIndex = 0; columnIndex < bodyHandles.GetLength(1) - 1; ++columnIndex) + { + CreateConstraintBetweenBodies(bodyHandles[rowIndex, columnIndex], bodyHandles[rowIndex, columnIndex + 1]); + } + } + for (int rowIndex = 0; rowIndex < bodyHandles.GetLength(0) - 1; ++rowIndex) + { + for (int columnIndex = 0; columnIndex < bodyHandles.GetLength(1); ++columnIndex) + { + CreateConstraintBetweenBodies(bodyHandles[rowIndex, columnIndex], bodyHandles[rowIndex + 1, columnIndex]); + } + } + for (int rowIndex = 0; rowIndex < bodyHandles.GetLength(0) - 1; ++rowIndex) + { + for (int columnIndex = 0; columnIndex < bodyHandles.GetLength(1) - 1; ++columnIndex) + { + CreateConstraintBetweenBodies(bodyHandles[rowIndex, columnIndex], bodyHandles[rowIndex + 1, columnIndex + 1]); + CreateConstraintBetweenBodies(bodyHandles[rowIndex, columnIndex + 1], bodyHandles[rowIndex + 1, columnIndex]); + } + } + } + + static void TailorDress(Simulation simulation, CollidableProperty filters, int dancerIndex) + { + var bodies = CreateBodyGrid(new Vector3(0, 0.8f, 0) + GetOffsetForDancer(dancerIndex), 20, 20, 0.1f, 0.05f, 0.01f, dancerIndex, simulation, filters); + CreateDistanceConstraints(bodies, new SpringSettings(30, 1), simulation); + } public unsafe override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(0, 4, 10); @@ -161,7 +271,9 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var collisionFilters = new CollidableProperty(); //Note very high damping on the main ragdoll simulation; makes it easier to pose. - Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks { CollisionFilters = collisionFilters }, new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0), 0.95f, 0.95f), new SolveDescription(8, 1)); + Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks { CollisionFilters = collisionFilters }, new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0), 0, 0), new SolveDescription(8, 1)); + + looper = new ParallelLooper() { Dispatcher = ThreadDispatcher }; var hipsPosition = new Vector3(0, 0, 0); var abdomenPosition = hipsPosition + new Vector3(0, 0.25f, 0); @@ -254,17 +366,17 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) rightHandControl = new Control(Simulation, sourceBodyHandles.LowerRightArm, wristPosition + armOffset, limbServoSettings, limbSpringSettings); - Simulation.Statics.Add(new(new Vector3(0, -10.5f, 0), Simulation.Shapes.Add(new Box(1000, 1, 1000)))); - + Simulation.Statics.Add(new(new Vector3(0, -1.24f, 0), Simulation.Shapes.Add(new Box(1000, 1, 1000)))); //All the background dancers read different historical motion states so that they're not just all doing the exact same thing. //Keep the states in a queue. Each batch of 12 motion states is the state for a single frame. motionHistory = new QuickQueue(historyLength * 12, BufferPool); - dancers = new Dancer[16]; - static BodyHandle CreateCopyForDancer(Simulation sourceSimulation, BodyHandle sourceHandle, TypedIndex shapeIndexInTargetSimulation, Simulation targetSimulation) + dancers = new Dancer[dancerCount]; + static BodyHandle CreateCopyForDancer(Simulation sourceSimulation, BodyHandle sourceHandle, TypedIndex shapeIndexInTargetSimulation, Simulation targetSimulation, int dancerIndex) { var description = sourceSimulation.Bodies.GetDescription(sourceHandle); + description.Pose.Position += GetOffsetForDancer(dancerIndex); description.Collidable.Shape = shapeIndexInTargetSimulation; description.LocalInertia = default; return targetSimulation.Bodies.Add(description); @@ -273,28 +385,41 @@ static BodyHandle CreateCopyForDancer(Simulation sourceSimulation, BodyHandle so for (int i = 0; i < dancers.Length; ++i) { ref var dancer = ref dancers[i]; - dancer.Simulation = Simulation.Create(new BufferPool(), new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(default), new SolveDescription(1, 4)); - dancer.BodyHandles.Hips = CreateCopyForDancer(Simulation, sourceBodyHandles.Hips, dancer.Simulation.Shapes.Add(hipShape), dancer.Simulation); - dancer.BodyHandles.Abdomen = CreateCopyForDancer(Simulation, sourceBodyHandles.Abdomen, dancer.Simulation.Shapes.Add(abdomenShape), dancer.Simulation); - dancer.BodyHandles.Chest = CreateCopyForDancer(Simulation, sourceBodyHandles.Chest, dancer.Simulation.Shapes.Add(chestShape), dancer.Simulation); - dancer.BodyHandles.Head = CreateCopyForDancer(Simulation, sourceBodyHandles.Head, dancer.Simulation.Shapes.Add(headShape), dancer.Simulation); + var dancerFilters = new CollidableProperty(); + dancer.Simulation = Simulation.Create(new BufferPool(), new ClothCallbacks + { + Filters = dancerFilters, + Material = new PairMaterialProperties + { + SpringSettings = new SpringSettings(120, 1), + FrictionCoefficient = .4f, + MaximumRecoveryVelocity = 20 + } + }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(3, 3)); + dancer.BodyHandles.Hips = CreateCopyForDancer(Simulation, sourceBodyHandles.Hips, dancer.Simulation.Shapes.Add(hipShape), dancer.Simulation, i); + dancer.BodyHandles.Abdomen = CreateCopyForDancer(Simulation, sourceBodyHandles.Abdomen, dancer.Simulation.Shapes.Add(abdomenShape), dancer.Simulation, i); + dancer.BodyHandles.Chest = CreateCopyForDancer(Simulation, sourceBodyHandles.Chest, dancer.Simulation.Shapes.Add(chestShape), dancer.Simulation, i); + dancer.BodyHandles.Head = CreateCopyForDancer(Simulation, sourceBodyHandles.Head, dancer.Simulation.Shapes.Add(headShape), dancer.Simulation, i); var upperLegShapeInTarget = dancer.Simulation.Shapes.Add(upperLegShape); var lowerLegShapeInTarget = dancer.Simulation.Shapes.Add(lowerLegShape); - dancer.BodyHandles.UpperLeftLeg = CreateCopyForDancer(Simulation, sourceBodyHandles.UpperLeftLeg, upperLegShapeInTarget, dancer.Simulation); - dancer.BodyHandles.LowerLeftLeg = CreateCopyForDancer(Simulation, sourceBodyHandles.LowerLeftLeg, lowerLegShapeInTarget, dancer.Simulation); - dancer.BodyHandles.UpperRightLeg = CreateCopyForDancer(Simulation, sourceBodyHandles.UpperRightLeg, upperLegShapeInTarget, dancer.Simulation); - dancer.BodyHandles.LowerRightLeg = CreateCopyForDancer(Simulation, sourceBodyHandles.LowerRightLeg, lowerLegShapeInTarget, dancer.Simulation); + dancer.BodyHandles.UpperLeftLeg = CreateCopyForDancer(Simulation, sourceBodyHandles.UpperLeftLeg, upperLegShapeInTarget, dancer.Simulation, i); + dancer.BodyHandles.LowerLeftLeg = CreateCopyForDancer(Simulation, sourceBodyHandles.LowerLeftLeg, lowerLegShapeInTarget, dancer.Simulation, i); + dancer.BodyHandles.UpperRightLeg = CreateCopyForDancer(Simulation, sourceBodyHandles.UpperRightLeg, upperLegShapeInTarget, dancer.Simulation, i); + dancer.BodyHandles.LowerRightLeg = CreateCopyForDancer(Simulation, sourceBodyHandles.LowerRightLeg, lowerLegShapeInTarget, dancer.Simulation, i); var upperArmShapeInTarget = dancer.Simulation.Shapes.Add(upperArmShape); var lowerArmShapeInTarget = dancer.Simulation.Shapes.Add(lowerArmShape); - dancer.BodyHandles.UpperLeftArm = CreateCopyForDancer(Simulation, sourceBodyHandles.UpperLeftArm, upperArmShapeInTarget, dancer.Simulation); - dancer.BodyHandles.LowerLeftArm = CreateCopyForDancer(Simulation, sourceBodyHandles.LowerLeftArm, lowerArmShapeInTarget, dancer.Simulation); - dancer.BodyHandles.UpperRightArm = CreateCopyForDancer(Simulation, sourceBodyHandles.UpperRightArm, upperArmShapeInTarget, dancer.Simulation); - dancer.BodyHandles.LowerRightArm = CreateCopyForDancer(Simulation, sourceBodyHandles.LowerRightArm, lowerArmShapeInTarget, dancer.Simulation); + dancer.BodyHandles.UpperLeftArm = CreateCopyForDancer(Simulation, sourceBodyHandles.UpperLeftArm, upperArmShapeInTarget, dancer.Simulation, i); + dancer.BodyHandles.LowerLeftArm = CreateCopyForDancer(Simulation, sourceBodyHandles.LowerLeftArm, lowerArmShapeInTarget, dancer.Simulation, i); + dancer.BodyHandles.UpperRightArm = CreateCopyForDancer(Simulation, sourceBodyHandles.UpperRightArm, upperArmShapeInTarget, dancer.Simulation, i); + dancer.BodyHandles.LowerRightArm = CreateCopyForDancer(Simulation, sourceBodyHandles.LowerRightArm, lowerArmShapeInTarget, dancer.Simulation, i); + + TailorDress(dancer.Simulation, dancerFilters, i); } } + const int dancerCount = 128; const int historyLength = 256; double time; @@ -323,14 +448,13 @@ Vector3 CreateArmTarget(float t) return new Vector3(-xOffset - armOffsetX, yOffset, zOffset); } - QuickQueue motionHistory; public unsafe override void Update(Window window, Camera camera, Input input, float dt) { - time += dt; + time += 1 / 60f; var hipsTarget = new Vector3(0, 0, 3 * (float)Math.Sin(time / 4)); hipsControl.UpdateTarget(Simulation, hipsTarget); - const float stepDuration = 1.5f; + const float stepDuration = 2.5f; var scaledTime = time / stepDuration; var t = (float)(scaledTime - Math.Floor(scaledTime)); var tOffset = (t + 0.5f) % 1; @@ -349,7 +473,7 @@ public unsafe override void Update(Window window, Camera camera, Input input, fl //Record the latest motion state from the source dancer. var sourceHandleBuffer = DollBodyHandles.AsBuffer((DollBodyHandles*)Unsafe.AsPointer(ref sourceBodyHandles)); - if (motionHistory.Count == historyLength) + if (motionHistory.Count == historyLength * sourceHandleBuffer.Length) for (int i = 0; i < sourceHandleBuffer.Length; ++i) motionHistory.Dequeue(); @@ -357,21 +481,34 @@ public unsafe override void Update(Window window, Camera camera, Input input, fl { motionHistory.EnqueueUnsafely(Simulation.Bodies[sourceHandleBuffer[i]].MotionState); } + var startTime = Stopwatch.GetTimestamp(); + looper.For(0, dancers.Length, ExecuteDancer); + var endTime = Stopwatch.GetTimestamp(); + var executionTime = (endTime - startTime) / (double)Stopwatch.Frequency; + Console.WriteLine($"Time (ms): {executionTime * 1000}"); + Console.WriteLine($"Time per dancer, amortized (us): {executionTime * 1e6 / dancers.Length}"); + base.Update(window, camera, input, dt); + } + unsafe void ExecuteDancer(int dancerIndex) + { //Copy historical motion states to the dancers. - for (int i = 0; i < dancers.Length; ++i) + ref var dancer = ref dancers[dancerIndex]; + var sourceHandleBuffer = DollBodyHandles.AsBuffer((DollBodyHandles*)Unsafe.AsPointer(ref sourceBodyHandles)); + //Delay is greater for the dancers that are further away, plus a little randomized component to desynchronize them. + var distanceFromMiddle = Math.Abs(dancerIndex - dancerCount / 2); + var historicalStateStartIndex = motionHistory.Count - sourceHandleBuffer.Length * (distanceFromMiddle * 4 + 1 + (HashHelper.Rehash(dancerIndex) & 0xF)); + if (historicalStateStartIndex < 0) + historicalStateStartIndex = 0; + var targetHandleBuffer = DollBodyHandles.AsBuffer((DollBodyHandles*)Unsafe.AsPointer(ref dancer.BodyHandles)); + for (int j = 0; j < sourceHandleBuffer.Length; ++j) { - ref var dancer = ref dancers[i]; - var historicalStateStartIndex = motionHistory.Count - sourceHandleBuffer.Length * (i * 4 + 1); - if (historicalStateStartIndex < 0) - historicalStateStartIndex = 0; - var targetHandleBuffer = DollBodyHandles.AsBuffer((DollBodyHandles*)Unsafe.AsPointer(ref dancer.BodyHandles)); - for (int j = 0; j < sourceHandleBuffer.Length; ++j) - { - dancer.Simulation.Bodies[targetHandleBuffer[j]].MotionState = motionHistory[historicalStateStartIndex + j]; - } + ref var targetMotionState = ref dancer.Simulation.Bodies[targetHandleBuffer[j]].MotionState; + targetMotionState = motionHistory[historicalStateStartIndex + j]; + targetMotionState.Pose.Position += GetOffsetForDancer(dancerIndex); } - base.Update(window, camera, input, dt); + //Update the simulation for the dancer. + dancer.Simulation.Timestep(1 / 60f); } public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) From a073c7320c113640857b39a0b44977f133ac8dea Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 7 Jan 2022 17:15:49 -0600 Subject: [PATCH 371/947] Dancers practicing different formations. --- Demos/Demos/DancerDemo.cs | 82 ++++++++++++++++++++++----------------- 1 file changed, 47 insertions(+), 35 deletions(-) diff --git a/Demos/Demos/DancerDemo.cs b/Demos/Demos/DancerDemo.cs index 1aa664272..426d3d425 100644 --- a/Demos/Demos/DancerDemo.cs +++ b/Demos/Demos/DancerDemo.cs @@ -156,30 +156,40 @@ static void ConstrainOrientation(Simulation simulation, BodyHandle a, BodyHandle const float legOffsetX = 0.135f; const float armOffsetX = 0.25f; + static (int columnIndex, int rowIndex) GetRowAndColumnForDancer(int dancerIndex) + { + var rowIndex = dancerIndex / dancerGridWidth; + return (dancerIndex - rowIndex * dancerGridWidth, rowIndex); + } QuickQueue motionHistory; static Vector3 GetOffsetForDancer(int i) { const float spacing = 2; - return new Vector3(dancerCount * spacing / -2 + (i + 0.5f) * spacing, 0, -5); + var (columnIndex, rowIndex) = GetRowAndColumnForDancer(i); + return new Vector3(dancerGridWidth * spacing / -2 + (columnIndex + 0.5f) * spacing, 0, -2 + rowIndex * -spacing); } - static BodyHandle[,] CreateBodyGrid(Vector3 position, int widthInNodes, int lengthInNodes, float spacing, float bodyRadius, float massPerBody, + static BodyHandle[,] CreateBodyGrid(Vector3 position, int widthInNodes, float spacing, float bodyRadius, float massPerBody, int instanceId, Simulation simulation, CollidableProperty filters) { var description = BodyDescription.CreateDynamic(QuaternionEx.Identity, new BodyInertia { InverseMass = 1f / massPerBody }, simulation.Shapes.Add(new Sphere(bodyRadius)), 0.01f); - BodyHandle[,] handles = new BodyHandle[lengthInNodes, widthInNodes]; + BodyHandle[,] handles = new BodyHandle[widthInNodes, widthInNodes]; var armHoleCenter = new Vector2(armOffsetX + 0.05f, 0); - var armHoleRadius = 0.05f; + var armHoleRadius = 0.07f; var armHoleRadiusSquared = armHoleRadius * armHoleRadius; - for (int rowIndex = 0; rowIndex < lengthInNodes; ++rowIndex) + var halfWidth = widthInNodes * spacing / 2; + var halfWidthSquared = halfWidth * halfWidth; + var halfWidthOffset = new Vector2(halfWidth); + for (int rowIndex = 0; rowIndex < widthInNodes; ++rowIndex) { for (int columnIndex = 0; columnIndex < widthInNodes; ++columnIndex) { - Vector2 horizontalPosition = new(columnIndex * spacing - widthInNodes * spacing / 2f, rowIndex * spacing - lengthInNodes * spacing / 2); - var distance0 = Vector2.DistanceSquared(horizontalPosition, armHoleCenter); - var distance1 = Vector2.DistanceSquared(horizontalPosition, -armHoleCenter); - if (distance0 < armHoleRadiusSquared || distance1 < armHoleRadiusSquared) + var horizontalPosition = new Vector2(columnIndex, rowIndex) * spacing - halfWidthOffset; + var distanceSquared0 = Vector2.DistanceSquared(horizontalPosition, armHoleCenter); + var distanceSquared1 = Vector2.DistanceSquared(horizontalPosition, -armHoleCenter); + var centerDistanceSquared = horizontalPosition.LengthSquared(); + if (distanceSquared0 < armHoleRadiusSquared || distanceSquared1 < armHoleRadiusSquared || centerDistanceSquared > halfWidthSquared) { //Too close to an arm, don't create any bodies here. handles[rowIndex, columnIndex] = new BodyHandle { Value = -1 }; @@ -260,7 +270,7 @@ void CreateConstraintBetweenBodies(BodyHandle aHandle, BodyHandle bHandle) static void TailorDress(Simulation simulation, CollidableProperty filters, int dancerIndex) { - var bodies = CreateBodyGrid(new Vector3(0, 0.8f, 0) + GetOffsetForDancer(dancerIndex), 20, 20, 0.1f, 0.05f, 0.01f, dancerIndex, simulation, filters); + var bodies = CreateBodyGrid(new Vector3(0, 0.8f, 0) + GetOffsetForDancer(dancerIndex), 40, 0.05f, 0.025f, 0.01f, dancerIndex, simulation, filters); CreateDistanceConstraints(bodies, new SpringSettings(30, 1), simulation); } public unsafe override void Initialize(ContentArchive content, Camera camera) @@ -395,7 +405,7 @@ static BodyHandle CreateCopyForDancer(Simulation sourceSimulation, BodyHandle so FrictionCoefficient = .4f, MaximumRecoveryVelocity = 20 } - }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(3, 3)); + }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); dancer.BodyHandles.Hips = CreateCopyForDancer(Simulation, sourceBodyHandles.Hips, dancer.Simulation.Shapes.Add(hipShape), dancer.Simulation, i); dancer.BodyHandles.Abdomen = CreateCopyForDancer(Simulation, sourceBodyHandles.Abdomen, dancer.Simulation.Shapes.Add(abdomenShape), dancer.Simulation, i); dancer.BodyHandles.Chest = CreateCopyForDancer(Simulation, sourceBodyHandles.Chest, dancer.Simulation.Shapes.Add(chestShape), dancer.Simulation, i); @@ -419,7 +429,9 @@ static BodyHandle CreateCopyForDancer(Simulation sourceSimulation, BodyHandle so } } - const int dancerCount = 128; + const int dancerGridWidth = 6; + const int dancerGridLength = 6; + const int dancerCount = dancerGridWidth * dancerGridLength; const int historyLength = 256; double time; @@ -447,14 +459,35 @@ Vector3 CreateArmTarget(float t) var yOffset = 0.9f - 0.2f * offset; return new Vector3(-xOffset - armOffsetX, yOffset, zOffset); } - + unsafe void ExecuteDancer(int dancerIndex) + { + //Copy historical motion states to the dancers. + ref var dancer = ref dancers[dancerIndex]; + var sourceHandleBuffer = DollBodyHandles.AsBuffer((DollBodyHandles*)Unsafe.AsPointer(ref sourceBodyHandles)); + //Delay is greater for the dancers that are further away, plus a little randomized component to desynchronize them. + var (columnIndex, rowIndex) = GetRowAndColumnForDancer(dancerIndex); + var offsetX = columnIndex - (dancerGridWidth / 2 - 0.5f); + var distanceFromMainDancer = (int)MathF.Sqrt(offsetX * offsetX + rowIndex * rowIndex); + var historicalStateStartIndex = motionHistory.Count - sourceHandleBuffer.Length * (distanceFromMainDancer * 8 + 1 + (HashHelper.Rehash(dancerIndex) & 0xF)); + if (historicalStateStartIndex < 0) + historicalStateStartIndex = 0; + var targetHandleBuffer = DollBodyHandles.AsBuffer((DollBodyHandles*)Unsafe.AsPointer(ref dancer.BodyHandles)); + for (int j = 0; j < sourceHandleBuffer.Length; ++j) + { + ref var targetMotionState = ref dancer.Simulation.Bodies[targetHandleBuffer[j]].MotionState; + targetMotionState = motionHistory[historicalStateStartIndex + j]; + targetMotionState.Pose.Position += GetOffsetForDancer(dancerIndex); + } + //Update the simulation for the dancer. + dancer.Simulation.Timestep(1 / 60f); + } public unsafe override void Update(Window window, Camera camera, Input input, float dt) { time += 1 / 60f; var hipsTarget = new Vector3(0, 0, 3 * (float)Math.Sin(time / 4)); hipsControl.UpdateTarget(Simulation, hipsTarget); - const float stepDuration = 2.5f; + const float stepDuration = 3.5f; var scaledTime = time / stepDuration; var t = (float)(scaledTime - Math.Floor(scaledTime)); var tOffset = (t + 0.5f) % 1; @@ -490,27 +523,6 @@ public unsafe override void Update(Window window, Camera camera, Input input, fl base.Update(window, camera, input, dt); } - unsafe void ExecuteDancer(int dancerIndex) - { - //Copy historical motion states to the dancers. - ref var dancer = ref dancers[dancerIndex]; - var sourceHandleBuffer = DollBodyHandles.AsBuffer((DollBodyHandles*)Unsafe.AsPointer(ref sourceBodyHandles)); - //Delay is greater for the dancers that are further away, plus a little randomized component to desynchronize them. - var distanceFromMiddle = Math.Abs(dancerIndex - dancerCount / 2); - var historicalStateStartIndex = motionHistory.Count - sourceHandleBuffer.Length * (distanceFromMiddle * 4 + 1 + (HashHelper.Rehash(dancerIndex) & 0xF)); - if (historicalStateStartIndex < 0) - historicalStateStartIndex = 0; - var targetHandleBuffer = DollBodyHandles.AsBuffer((DollBodyHandles*)Unsafe.AsPointer(ref dancer.BodyHandles)); - for (int j = 0; j < sourceHandleBuffer.Length; ++j) - { - ref var targetMotionState = ref dancer.Simulation.Bodies[targetHandleBuffer[j]].MotionState; - targetMotionState = motionHistory[historicalStateStartIndex + j]; - targetMotionState.Pose.Position += GetOffsetForDancer(dancerIndex); - } - //Update the simulation for the dancer. - dancer.Simulation.Timestep(1 / 60f); - } - public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) { for (int i = 0; i < dancers.Length; ++i) From 6dbe29d983b2d89b8423c506267d857d4566b3c7 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 7 Jan 2022 19:14:39 -0600 Subject: [PATCH 372/947] Dancers now have semiLOD. Dresses are overstarched. --- Demos/Demos/ClothDemo.cs | 2 +- Demos/Demos/DancerDemo.cs | 120 ++++++++++++++++++++------------------ 2 files changed, 63 insertions(+), 59 deletions(-) diff --git a/Demos/Demos/ClothDemo.cs b/Demos/Demos/ClothDemo.cs index 4175dcbd2..d0af0f0ac 100644 --- a/Demos/Demos/ClothDemo.cs +++ b/Demos/Demos/ClothDemo.cs @@ -36,7 +36,7 @@ public static bool Test(in ClothCollisionFilter a, in ClothCollisionFilter b) if (a.instanceId != b.instanceId) return true; //Disallow collisions between vertices which are near each other. We measure distance as max(abs(ax - bx), abs(ay - by), abs(az - bz)). - const int minimumDistance = 2; + const int minimumDistance = 3; var differenceX = a.x - b.x; if (differenceX < -minimumDistance || differenceX > minimumDistance) return true; diff --git a/Demos/Demos/DancerDemo.cs b/Demos/Demos/DancerDemo.cs index 426d3d425..c14d9428d 100644 --- a/Demos/Demos/DancerDemo.cs +++ b/Demos/Demos/DancerDemo.cs @@ -53,6 +53,13 @@ struct Dancer ParallelLooper looper; Dancer[] dancers; + const float legOffsetX = 0.135f; + const float armOffsetX = 0.25f; + const int dancerGridWidth = 6; + const int dancerGridLength = 6; + const int dancerCount = dancerGridWidth * dancerGridLength; + const int historyLength = 256; + struct Control { @@ -153,8 +160,6 @@ static void ConstrainOrientation(Simulation simulation, BodyHandle a, BodyHandle }); } - const float legOffsetX = 0.135f; - const float armOffsetX = 0.25f; static (int columnIndex, int rowIndex) GetRowAndColumnForDancer(int dancerIndex) { @@ -175,7 +180,7 @@ static Vector3 GetOffsetForDancer(int i) { var description = BodyDescription.CreateDynamic(QuaternionEx.Identity, new BodyInertia { InverseMass = 1f / massPerBody }, simulation.Shapes.Add(new Sphere(bodyRadius)), 0.01f); BodyHandle[,] handles = new BodyHandle[widthInNodes, widthInNodes]; - var armHoleCenter = new Vector2(armOffsetX + 0.05f, 0); + var armHoleCenter = new Vector2(armOffsetX, 0); var armHoleRadius = 0.07f; var armHoleRadiusSquared = armHoleRadius * armHoleRadius; var halfWidth = widthInNodes * spacing / 2; @@ -191,7 +196,7 @@ static Vector3 GetOffsetForDancer(int i) var centerDistanceSquared = horizontalPosition.LengthSquared(); if (distanceSquared0 < armHoleRadiusSquared || distanceSquared1 < armHoleRadiusSquared || centerDistanceSquared > halfWidthSquared) { - //Too close to an arm, don't create any bodies here. + //Too close to an arm or too far from the center, don't create any bodies here. handles[rowIndex, columnIndex] = new BodyHandle { Value = -1 }; } else @@ -206,31 +211,6 @@ static Vector3 GetOffsetForDancer(int i) } return handles; } - - static void CreateAreaConstraints(BodyHandle[,] bodyHandles, SpringSettings springSettings, Simulation simulation) - { - for (int rowIndex = 0; rowIndex < bodyHandles.GetLength(0) - 1; ++rowIndex) - { - for (int columnIndex = 0; columnIndex < bodyHandles.GetLength(1) - 1; ++columnIndex) - { - var aHandle = bodyHandles[rowIndex, columnIndex]; - var bHandle = bodyHandles[rowIndex + 1, columnIndex]; - var cHandle = bodyHandles[rowIndex, columnIndex + 1]; - var dHandle = bodyHandles[rowIndex + 1, columnIndex + 1]; - //Only create a constraint if bodies on all sides of the quad actually exist. - //In this demo, we use -1 in the body handle slot to represent 'no body'. - if (aHandle.Value >= 0 && bHandle.Value >= 0 && cHandle.Value >= 0 && dHandle.Value >= 0) - { - var a = simulation.Bodies[aHandle]; - var b = simulation.Bodies[bHandle]; - var c = simulation.Bodies[cHandle]; - var d = simulation.Bodies[dHandle]; - simulation.Solver.Add(aHandle, bHandle, cHandle, new AreaConstraint(a.Pose.Position, b.Pose.Position, c.Pose.Position, springSettings)); - simulation.Solver.Add(bHandle, cHandle, dHandle, new AreaConstraint(b.Pose.Position, c.Pose.Position, d.Pose.Position, springSettings)); - } - } - } - } static void CreateDistanceConstraints(BodyHandle[,] bodyHandles, SpringSettings springSettings, Simulation simulation) { void CreateConstraintBetweenBodies(BodyHandle aHandle, BodyHandle bHandle) @@ -268,10 +248,30 @@ void CreateConstraintBetweenBodies(BodyHandle aHandle, BodyHandle bHandle) } } + static float GetDistanceFromMainDancer(int dancerIndex) + { + var (columnIndex, rowIndex) = GetRowAndColumnForDancer(dancerIndex); + var offsetX = columnIndex - (dancerGridWidth / 2 - 0.5f); + return MathF.Sqrt(offsetX * offsetX + rowIndex * rowIndex); + } + static void TailorDress(Simulation simulation, CollidableProperty filters, int dancerIndex) { - var bodies = CreateBodyGrid(new Vector3(0, 0.8f, 0) + GetOffsetForDancer(dancerIndex), 40, 0.05f, 0.025f, 0.01f, dancerIndex, simulation, filters); - CreateDistanceConstraints(bodies, new SpringSettings(30, 1), simulation); + //The demo uses lower resolution grids on dancers further away from the main dancer. + //This is a sorta-example of level of detail. In a 'real' use case, you'd probably want to transition between levels of detail dynamically as the camera moved around. + //That's a little trickier, but doable. Going low to high, for example, requires creating bodies at interpolated positions between existing bodies, while going to a lower level of detail removes them. + var distanceFromMainDancer = GetDistanceFromMainDancer(dancerIndex); + var levelOfDetail = Math.Min(2, MathF.Ceiling(MathF.Log2(MathF.Max(1, distanceFromMainDancer / 2)))); + var targetDressDiameter = 2.2f; + var fullDetailWidthInBodies = 40; + float spacingAtFullDetail = targetDressDiameter / fullDetailWidthInBodies; + float bodyRadiusAtFullDetail = spacingAtFullDetail / 1.5f; + var scale = MathF.Pow(2, levelOfDetail); + var widthInBodies = (int)MathF.Ceiling(fullDetailWidthInBodies / scale); + var spacing = spacingAtFullDetail * scale; + var bodyRadius = bodyRadiusAtFullDetail * MathF.Pow(2, MathF.Max(0, levelOfDetail - 1)); + var bodies = CreateBodyGrid(new Vector3(0, 0.8f, 0) + GetOffsetForDancer(dancerIndex), widthInBodies, spacing, bodyRadius, 0.01f, dancerIndex, simulation, filters); + CreateDistanceConstraints(bodies, new SpringSettings(60, 1), simulation); } public unsafe override void Initialize(ContentArchive content, Camera camera) { @@ -429,11 +429,25 @@ static BodyHandle CreateCopyForDancer(Simulation sourceSimulation, BodyHandle so } } - const int dancerGridWidth = 6; - const int dancerGridLength = 6; - const int dancerCount = dancerGridWidth * dancerGridLength; - const int historyLength = 256; - double time; + unsafe void ExecuteDancer(int dancerIndex) + { + //Copy historical motion states to the dancers. + ref var dancer = ref dancers[dancerIndex]; + var sourceHandleBuffer = DollBodyHandles.AsBuffer((DollBodyHandles*)Unsafe.AsPointer(ref sourceBodyHandles)); + //Delay is greater for the dancers that are further away, plus a little randomized component to desynchronize them. + var historicalStateStartIndex = motionHistory.Count - sourceHandleBuffer.Length * ((int)GetDistanceFromMainDancer(dancerIndex) * 8 + 1 + (HashHelper.Rehash(dancerIndex) & 0xF)); + if (historicalStateStartIndex < 0) + historicalStateStartIndex = 0; + var targetHandleBuffer = DollBodyHandles.AsBuffer((DollBodyHandles*)Unsafe.AsPointer(ref dancer.BodyHandles)); + for (int j = 0; j < sourceHandleBuffer.Length; ++j) + { + ref var targetMotionState = ref dancer.Simulation.Bodies[targetHandleBuffer[j]].MotionState; + targetMotionState = motionHistory[historicalStateStartIndex + j]; + targetMotionState.Pose.Position += GetOffsetForDancer(dancerIndex); + } + //Update the simulation for the dancer. + dancer.Simulation.Timestep(1 / 60f); + } float Smoothstep(float v) { @@ -459,29 +473,8 @@ Vector3 CreateArmTarget(float t) var yOffset = 0.9f - 0.2f * offset; return new Vector3(-xOffset - armOffsetX, yOffset, zOffset); } - unsafe void ExecuteDancer(int dancerIndex) - { - //Copy historical motion states to the dancers. - ref var dancer = ref dancers[dancerIndex]; - var sourceHandleBuffer = DollBodyHandles.AsBuffer((DollBodyHandles*)Unsafe.AsPointer(ref sourceBodyHandles)); - //Delay is greater for the dancers that are further away, plus a little randomized component to desynchronize them. - var (columnIndex, rowIndex) = GetRowAndColumnForDancer(dancerIndex); - var offsetX = columnIndex - (dancerGridWidth / 2 - 0.5f); - var distanceFromMainDancer = (int)MathF.Sqrt(offsetX * offsetX + rowIndex * rowIndex); - var historicalStateStartIndex = motionHistory.Count - sourceHandleBuffer.Length * (distanceFromMainDancer * 8 + 1 + (HashHelper.Rehash(dancerIndex) & 0xF)); - if (historicalStateStartIndex < 0) - historicalStateStartIndex = 0; - var targetHandleBuffer = DollBodyHandles.AsBuffer((DollBodyHandles*)Unsafe.AsPointer(ref dancer.BodyHandles)); - for (int j = 0; j < sourceHandleBuffer.Length; ++j) - { - ref var targetMotionState = ref dancer.Simulation.Bodies[targetHandleBuffer[j]].MotionState; - targetMotionState = motionHistory[historicalStateStartIndex + j]; - targetMotionState.Pose.Position += GetOffsetForDancer(dancerIndex); - } - //Update the simulation for the dancer. - dancer.Simulation.Timestep(1 / 60f); - } + double time; public unsafe override void Update(Window window, Camera camera, Input input, float dt) { time += 1 / 60f; @@ -531,5 +524,16 @@ public override void Render(Renderer renderer, Camera camera, Input input, TextB } base.Render(renderer, camera, input, text, font); } + protected override void OnDispose() + { + //While the main simulation and pool is disposed by the Demo.cs Dispose function, the dancers have their own pools and need to be cleared. + //Note that we don't bother disposing the simulation here- all resources in the simulation were taken from the associated pool, and the simulation will not be used anymore. + //We can just clear the buffer pool and let the GC eat the simulation. + for (int i = 0; i < dancers.Length; ++i) + { + dancers[i].Simulation.BufferPool.Clear(); + } + base.OnDispose(); + } } } From d114de0cc1a39ba720048add6837bb19b973b54a Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 8 Jan 2022 17:51:04 -0600 Subject: [PATCH 373/947] Added CenterDistanceLimit. --- .../Constraints/CenterDistanceConstraint.cs | 3 +- .../Constraints/CenterDistanceLimit.cs | 138 ++++++++++++++++++ BepuPhysics/DefaultTypes.cs | 1 + .../CenterDistanceLimitLineExtractor.cs | 61 ++++++++ .../Constraints/ConstraintLineExtractor.cs | 1 + .../ConstraintDescriptionMappingTests.cs | 2 + Demos/Demos/ClothDemo.cs | 11 +- Demos/Program.cs | 6 - Demos/SpecializedTests/ConstraintTestDemo.cs | 23 +++ Demos/SpecializedTests/Media/BedsheetDemo.cs | 5 +- 10 files changed, 239 insertions(+), 12 deletions(-) create mode 100644 BepuPhysics/Constraints/CenterDistanceLimit.cs create mode 100644 DemoRenderer/Constraints/CenterDistanceLimitLineExtractor.cs diff --git a/BepuPhysics/Constraints/CenterDistanceConstraint.cs b/BepuPhysics/Constraints/CenterDistanceConstraint.cs index e347d8c9f..ff81896bc 100644 --- a/BepuPhysics/Constraints/CenterDistanceConstraint.cs +++ b/BepuPhysics/Constraints/CenterDistanceConstraint.cs @@ -69,7 +69,7 @@ public struct CenterDistancePrestepData public struct CenterDistanceConstraintFunctions : ITwoBodyConstraintFunctions> { [MethodImpl(MethodImplOptions.AggressiveInlining)] - static void ApplyImpulse(in Vector3Wide jacobianA, in Vector inverseMassA, in Vector inverseMassB, in Vector impulse, ref BodyVelocityWide a, ref BodyVelocityWide b) + public static void ApplyImpulse(in Vector3Wide jacobianA, in Vector inverseMassA, in Vector inverseMassB, in Vector impulse, ref BodyVelocityWide a, ref BodyVelocityWide b) { Vector3Wide.Scale(jacobianA, impulse * inverseMassA, out var changeA); Vector3Wide.Scale(jacobianA, impulse * inverseMassB, out var negatedChangeB); @@ -77,6 +77,7 @@ static void ApplyImpulse(in Vector3Wide jacobianA, in Vector inverseMassA Vector3Wide.Subtract(b.Linear, negatedChangeB, out b.Linear); } + //[MethodImpl(MethodImplOptions.AggressiveInlining)] public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref CenterDistancePrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) diff --git a/BepuPhysics/Constraints/CenterDistanceLimit.cs b/BepuPhysics/Constraints/CenterDistanceLimit.cs new file mode 100644 index 000000000..7694ccbd9 --- /dev/null +++ b/BepuPhysics/Constraints/CenterDistanceLimit.cs @@ -0,0 +1,138 @@ +using BepuPhysics.CollisionDetection; +using BepuUtilities; +using BepuUtilities.Memory; +using System; +using System.Diagnostics; +using System.Numerics; +using System.Runtime.CompilerServices; +using static BepuUtilities.GatherScatter; +namespace BepuPhysics.Constraints +{ + + /// + /// Constrains the center of two bodies to be separated by a distance within a range. + /// + public struct CenterDistanceLimit : ITwoBodyConstraintDescription + { + /// + /// Minimum distance between the body centers. + /// + public float MinimumDistance; + /// + /// Maximum distance between the body centers. + /// + public float MaximumDistance; + /// + /// Spring frequency and damping parameters. + /// + public SpringSettings SpringSettings; + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public CenterDistanceLimit(float minimumDistance, float maximumDistance, in SpringSettings springSettings) + { + MinimumDistance = minimumDistance; + MaximumDistance = maximumDistance; + SpringSettings = springSettings; + } + + public readonly int ConstraintTypeId + { + [MethodImpl(MethodImplOptions.AggressiveInlining)] + get + { + return CenterDistanceLimitTypeProcessor.BatchTypeId; + } + } + + public readonly Type TypeProcessorType => typeof(CenterDistanceLimitTypeProcessor); + + public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) + { + Debug.Assert(MinimumDistance >= 0, "CenterDistanceLimit.MinimumDistance must be nonnegative."); + Debug.Assert(MaximumDistance >= 0, "CenterDistanceLimit.MaximumDistance must be nonnegative."); + ConstraintChecker.AssertValid(SpringSettings, nameof(CenterDistanceLimit)); + Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type."); + ref var target = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); + GatherScatter.GetFirst(ref target.MinimumDistance) = MinimumDistance; + GatherScatter.GetFirst(ref target.MaximumDistance) = MaximumDistance; + SpringSettingsWide.WriteFirst(SpringSettings, ref target.SpringSettings); + } + + public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out CenterDistanceLimit description) + { + Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type."); + ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); + description.MinimumDistance = GatherScatter.GetFirst(ref source.MinimumDistance); + description.MaximumDistance = GatherScatter.GetFirst(ref source.MaximumDistance); + SpringSettingsWide.ReadFirst(source.SpringSettings, out description.SpringSettings); + } + } + + public struct CenterDistanceLimitPrestepData + { + public Vector MinimumDistance; + public Vector MaximumDistance; + public SpringSettingsWide SpringSettings; + } + + public struct CenterDistanceLimitFunctions : ITwoBodyConstraintFunctions> + { + [MethodImpl(MethodImplOptions.AggressiveInlining)] + static void ComputeJacobian(Vector minimumDistance, Vector maximumDistance, in Vector3Wide positionA, in Vector3Wide positionB, out Vector3Wide jacobianA, out Vector distance, out Vector useMinimum) + { + //Note that we need the actual length in both warmstart and solve in the limit version of the constraint; since the min/max bound determines jacobian sign. + var ab = positionB - positionA; + distance = ab.Length(); + var inverseDistance = MathHelper.FastReciprocal(distance); + var useFallback = Vector.LessThan(distance, new Vector(1e-5f)); + Vector3Wide.Scale(ab, inverseDistance, out jacobianA); + jacobianA.X = Vector.ConditionalSelect(useFallback, Vector.One, jacobianA.X); + jacobianA.Y = Vector.ConditionalSelect(useFallback, Vector.Zero, jacobianA.Y); + jacobianA.Z = Vector.ConditionalSelect(useFallback, Vector.Zero, jacobianA.Z); + + //If the current distance is closer to the minimum, calibrate for the minimum. Otherwise, calibrate for the maximum. + useMinimum = Vector.LessThan(Vector.Abs(distance - minimumDistance), Vector.Abs(distance - maximumDistance)); + jacobianA = Vector3Wide.ConditionalSelect(useMinimum, -jacobianA, jacobianA); + } + //[MethodImpl(MethodImplOptions.AggressiveInlining)] + public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, + ref CenterDistanceLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + ComputeJacobian(prestep.MinimumDistance, prestep.MaximumDistance, positionA, positionB, out var jacobianA, out _, out _); + CenterDistanceConstraintFunctions.ApplyImpulse(jacobianA, inertiaA.InverseMass, inertiaB.InverseMass, accumulatedImpulses, ref wsvA, ref wsvB); + } + //[MethodImpl(MethodImplOptions.AggressiveInlining)] + public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, + ref CenterDistanceLimitPrestepData prestep, ref Vector accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + ComputeJacobian(prestep.MinimumDistance, prestep.MaximumDistance, positionA, positionB, out var jacobianA, out var distance, out var useMinimum); + + SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); + //Jacobian is just the unit length direction, so the effective mass is simple: + var effectiveMass = effectiveMassCFMScale / (inertiaA.InverseMass + inertiaB.InverseMass); + + var error = Vector.ConditionalSelect(useMinimum, prestep.MinimumDistance - distance, distance - prestep.MaximumDistance); + InequalityHelpers.ComputeBiasVelocity(error, positionErrorToVelocity, inverseDt, out var biasVelocity); + var csv = Vector3Wide.Dot(wsvA.Linear, jacobianA) - Vector3Wide.Dot(wsvB.Linear, jacobianA); + //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); + var csi = -accumulatedImpulse * softnessImpulseScale - effectiveMass * (csv - biasVelocity); + InequalityHelpers.ClampPositive(ref accumulatedImpulse, ref csi); + + CenterDistanceConstraintFunctions.ApplyImpulse(jacobianA, inertiaA.InverseMass, inertiaB.InverseMass, csi, ref wsvA, ref wsvB); + } + + public bool RequiresIncrementalSubstepUpdates => false; + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref CenterDistanceLimitPrestepData prestepData) { } + + } + + + /// + /// Handles the solve iterations of a bunch of distance servos. + /// + public class CenterDistanceLimitTypeProcessor : TwoBodyTypeProcessor, CenterDistanceLimitFunctions, AccessOnlyLinear, AccessOnlyLinear, AccessOnlyLinear, AccessOnlyLinear> + { + public const int BatchTypeId = 55; + } +} diff --git a/BepuPhysics/DefaultTypes.cs b/BepuPhysics/DefaultTypes.cs index 6d14b9868..73b673dce 100644 --- a/BepuPhysics/DefaultTypes.cs +++ b/BepuPhysics/DefaultTypes.cs @@ -46,6 +46,7 @@ public static void RegisterDefaults(Solver solver, NarrowPhase narrowPhase) solver.Register(); solver.Register(); solver.Register(); + solver.Register(); solver.Register(); solver.Register(); diff --git a/DemoRenderer/Constraints/CenterDistanceLimitLineExtractor.cs b/DemoRenderer/Constraints/CenterDistanceLimitLineExtractor.cs new file mode 100644 index 000000000..67086ca7f --- /dev/null +++ b/DemoRenderer/Constraints/CenterDistanceLimitLineExtractor.cs @@ -0,0 +1,61 @@ +using BepuUtilities.Collections; +using BepuUtilities.Memory; +using BepuPhysics; +using BepuPhysics.Constraints; +using System.Numerics; +using BepuUtilities; + +namespace DemoRenderer.Constraints +{ + struct CenterDistanceLimitLineExtractor : IConstraintLineExtractor + { + public int LinesPerConstraint => 5; + + public unsafe void ExtractLines(ref CenterDistanceLimitPrestepData prestepBundle, int setIndex, int* bodyIndices, + Bodies bodies, ref Vector3 tint, ref QuickList lines) + { + //Could do bundles of constraints at a time, but eh. + ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; + ref var poseB = ref bodies.Sets[setIndex].SolverStates[bodyIndices[1]].Motion.Pose; + var minimumDistance = GatherScatter.GetFirst(ref prestepBundle.MinimumDistance); + var maximumDistance = GatherScatter.GetFirst(ref prestepBundle.MaximumDistance); + 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, poseA.Position, packedColor, 0); + lines.AllocateUnsafely() = new LineInstance(poseB.Position, poseB.Position, packedColor, 0); + 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 packedFarColor = Helpers.PackColor(color * 0.5f); + var packedNearColor = Helpers.PackColor(color * 0.25f); + var minimumPoint = poseA.Position + direction * minimumDistance; + if (length >= minimumDistance && length <= maximumDistance) + { + //Create a darker bar to signify the minimum limit. + lines.AllocateUnsafely() = new LineInstance(poseA.Position, minimumPoint, packedNearColor, 0); + lines.AllocateUnsafely() = new LineInstance(minimumPoint, poseB.Position, 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(poseA.Position, poseB.Position, packedNearColor, 0); + lines.AllocateUnsafely() = new LineInstance(poseB.Position, poseA.Position + 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 = poseA.Position + direction * maximumDistance; + lines.AllocateUnsafely() = new LineInstance(poseA.Position, minimumPoint, packedNearColor, 0); + lines.AllocateUnsafely() = new LineInstance(minimumPoint, targetEnd, packedFarColor, 0); + lines.AllocateUnsafely() = new LineInstance(targetEnd, poseB.Position, packedErrorColor, 0); + } + + } + } +} diff --git a/DemoRenderer/Constraints/ConstraintLineExtractor.cs b/DemoRenderer/Constraints/ConstraintLineExtractor.cs index 52c01e3ab..37ae689b4 100644 --- a/DemoRenderer/Constraints/ConstraintLineExtractor.cs +++ b/DemoRenderer/Constraints/ConstraintLineExtractor.cs @@ -130,6 +130,7 @@ public ConstraintLineExtractor(BufferPool pool) AllocateSlot(DistanceServoTypeProcessor.BatchTypeId) = new TypeLineExtractor>(); AllocateSlot(DistanceLimitTypeProcessor.BatchTypeId) = new TypeLineExtractor>(); AllocateSlot(CenterDistanceTypeProcessor.BatchTypeId) = new TypeLineExtractor>(); + AllocateSlot(CenterDistanceLimitTypeProcessor.BatchTypeId) = new TypeLineExtractor>(); AllocateSlot(PointOnLineServoTypeProcessor.BatchTypeId) = new TypeLineExtractor(); AllocateSlot(LinearAxisServoTypeProcessor.BatchTypeId) = new TypeLineExtractor>(); AllocateSlot(AngularSwivelHingeTypeProcessor.BatchTypeId) = new TypeLineExtractor>(); diff --git a/DemoTests/ConstraintDescriptionMappingTests.cs b/DemoTests/ConstraintDescriptionMappingTests.cs index 50816e1d3..6bce7ce0f 100644 --- a/DemoTests/ConstraintDescriptionMappingTests.cs +++ b/DemoTests/ConstraintDescriptionMappingTests.cs @@ -145,6 +145,8 @@ public static void TestMappings() Test(pool, random, 1); Test(pool, random, 1); Test(pool, random, 1); + Test(pool, random, 2); + Test(pool, random, 2); pool.Clear(); } diff --git a/Demos/Demos/ClothDemo.cs b/Demos/Demos/ClothDemo.cs index d0af0f0ac..ed95e372d 100644 --- a/Demos/Demos/ClothDemo.cs +++ b/Demos/Demos/ClothDemo.cs @@ -157,7 +157,10 @@ void CreateConstraintBetweenBodies(BodyHandle aHandle, BodyHandle bHandle) //Don't create constraints between two kinematic bodies. if (a.LocalInertia.InverseMass > 0 || b.LocalInertia.InverseMass > 0) { - Simulation.Solver.Add(aHandle, bHandle, new CenterDistanceConstraint(Vector3.Distance(a.Pose.Position, b.Pose.Position), springSettings)); + //Note the use of a limit; the distance is allowed to go smaller. + //This helps stop the cloth from having unnatural rigidity. + var distance = Vector3.Distance(a.Pose.Position, b.Pose.Position); + Simulation.Solver.Add(aHandle, bHandle, new CenterDistanceLimit(distance * 0.015f, distance, springSettings)); } } for (int rowIndex = 0; rowIndex < bodyHandles.GetLength(0); ++rowIndex) @@ -223,15 +226,15 @@ bool FullyDynamic(int rowIndex, int columnIndex, int width, int height) { var position = new Vector3(-50, 40, 0); var handles = CreateBodyGrid(position, initialRotation, 10, 30, 1f, 0.65f, 1, clothInstanceId++, filters, KinematicTopCorners); - CreateDistanceConstraints(handles, new SpringSettings(3, 1)); + CreateDistanceConstraints(handles, new SpringSettings(5, 1)); rolloverInfo.Add(position + new Vector3(5, 2, 0), "Soft distance constraints only, no area constraints"); } { var position = new Vector3(-30, 40, 0); var handles = CreateBodyGrid(position, initialRotation, 10, 30, 1f, 0.65f, 1, clothInstanceId++, filters, KinematicTopCorners); - CreateDistanceConstraints(handles, new SpringSettings(3, 1)); + CreateDistanceConstraints(handles, new SpringSettings(5, 1)); CreateAreaConstraints(handles, new SpringSettings(30, 1)); - rolloverInfo.Add(position + new Vector3(5, 2, 0), "Soft distance constraints with area constraints (note narrowing)"); + rolloverInfo.Add(position + new Vector3(5, 2, 0), "Soft distance constraints with area constraints"); } diff --git a/Demos/Program.cs b/Demos/Program.cs index 402a08dee..ddd1a8cd7 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -20,12 +20,6 @@ static void Main(string[] args) { content = ContentArchive.Load(stream); } - //HeadlessTest.Test(content, 4, 32, 512); - //HeadlessTest.Test(content, 4, 32, 512); - //HeadlessTest.Test(content, 4, 32, 512); - //HeadlessTest.Test(content, 4, 128, 1024); - //DeterminismTest.Test(content, 1, 65536); - //InvasiveHashDiagnostics.Initialize(5, 192000); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); diff --git a/Demos/SpecializedTests/ConstraintTestDemo.cs b/Demos/SpecializedTests/ConstraintTestDemo.cs index a6cf9cbda..5af760298 100644 --- a/Demos/SpecializedTests/ConstraintTestDemo.cs +++ b/Demos/SpecializedTests/ConstraintTestDemo.cs @@ -219,6 +219,29 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Simulation.Solver.Add(bHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(b, c), distanceSpringiness)); Simulation.Solver.Add(aHandle, bHandle, cHandle, new AreaConstraint(a, b, c, new SpringSettings(30, 1))); } + { + var x = GetNextPosition(ref nextX); + var sphere = new Sphere(0.125f); + //Treat each vertex as a point mass that cannot rotate. + var sphereInertia = new BodyInertia { InverseMass = 1 }; + var sphereCollidable = new CollidableDescription(Simulation.Shapes.Add(sphere)); + var a = new Vector3(x, 3, 0); + var b = new Vector3(x, 4, 0); + var c = new Vector3(x + 1, 3, 0); + var aDescription = BodyDescription.CreateDynamic(a, sphereInertia, sphereCollidable, activity); + var bDescription = BodyDescription.CreateDynamic(b, sphereInertia, sphereCollidable, activity); + var cDescription = BodyDescription.CreateDynamic(c, sphereInertia, sphereCollidable, activity); + var aHandle = Simulation.Bodies.Add(aDescription); + var bHandle = Simulation.Bodies.Add(bDescription); + var cHandle = Simulation.Bodies.Add(cDescription); + var distanceSpringiness = new SpringSettings(3f, 1); + var distanceAB = Vector3.Distance(a, b); + var distanceBC = Vector3.Distance(b, c); + var distanceCA = Vector3.Distance(c, a); + Simulation.Solver.Add(aHandle, bHandle, new CenterDistanceLimit(distanceAB * 0.15f, distanceAB, distanceSpringiness)); + Simulation.Solver.Add(aHandle, cHandle, new CenterDistanceLimit(distanceBC * 0.15f, distanceBC, distanceSpringiness)); + Simulation.Solver.Add(bHandle, cHandle, new CenterDistanceLimit(distanceCA * 0.15f, distanceCA, distanceSpringiness)); + } { var x = GetNextPosition(ref nextX); var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), default, collidableA, activity); diff --git a/Demos/SpecializedTests/Media/BedsheetDemo.cs b/Demos/SpecializedTests/Media/BedsheetDemo.cs index 171b713ec..e5fdef856 100644 --- a/Demos/SpecializedTests/Media/BedsheetDemo.cs +++ b/Demos/SpecializedTests/Media/BedsheetDemo.cs @@ -71,7 +71,10 @@ void CreateConstraintBetweenBodies(BodyHandle aHandle, BodyHandle bHandle) //Don't create constraints between two kinematic bodies. if (a.LocalInertia.InverseMass > 0 || b.LocalInertia.InverseMass > 0) { - Simulation.Solver.Add(aHandle, bHandle, new CenterDistanceConstraint(Vector3.Distance(a.Pose.Position, b.Pose.Position), springSettings)); + //Note the use of a limit; the distance is allowed to go smaller. + //This helps stop the cloth from having unnatural rigidity. + var distance = Vector3.Distance(a.Pose.Position, b.Pose.Position); + Simulation.Solver.Add(aHandle, bHandle, new CenterDistanceLimit(distance * 0.015f, distance, springSettings)); } } for (int rowIndex = 0; rowIndex < bodyHandles.GetLength(0); ++rowIndex) From 3f96f39d7830dbf51d9f3ddcc2b9f1a2446010c0 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 9 Jan 2022 16:37:48 -0600 Subject: [PATCH 374/947] Self collision on distant dancer cloth disabled. Fixed dancer 0's tendency to phase through its clothing. --- .../INarrowPhaseCallbacks.cs | 14 ++++ Demos/Demos/ClothDemo.cs | 31 +++++--- Demos/Demos/DancerDemo.cs | 73 ++++++++++--------- Demos/SpecializedTests/Media/BedsheetDemo.cs | 4 +- 4 files changed, 74 insertions(+), 48 deletions(-) diff --git a/BepuPhysics/CollisionDetection/INarrowPhaseCallbacks.cs b/BepuPhysics/CollisionDetection/INarrowPhaseCallbacks.cs index 6123fe8c0..794afe84d 100644 --- a/BepuPhysics/CollisionDetection/INarrowPhaseCallbacks.cs +++ b/BepuPhysics/CollisionDetection/INarrowPhaseCallbacks.cs @@ -12,6 +12,7 @@ namespace BepuPhysics.CollisionDetection /// public struct PairMaterialProperties { + /// /// Coefficient of friction to apply for the constraint. Maximum friction force will be equal to the normal force times the friction coefficient. /// @@ -24,6 +25,19 @@ public struct PairMaterialProperties /// Defines the constraint's penetration recovery spring properties. /// public SpringSettings SpringSettings; + + /// + /// Constructs a pair's material properties. + /// + /// Coefficient of friction to apply for the constraint. Maximum friction force will be equal to the normal force times the friction coefficient. + /// Maximum relative velocity along the contact normal at which the collision constraint will recover from penetration. Clamps the velocity goal created from the spring settings. + /// Defines the constraint's penetration recovery spring properties. + public PairMaterialProperties(float frictionCoefficient, float maximumRecoveryVelocity, SpringSettings springSettings) + { + FrictionCoefficient = frictionCoefficient; + MaximumRecoveryVelocity = maximumRecoveryVelocity; + SpringSettings = springSettings; + } } /// diff --git a/Demos/Demos/ClothDemo.cs b/Demos/Demos/ClothDemo.cs index ed95e372d..9bd68736f 100644 --- a/Demos/Demos/ClothDemo.cs +++ b/Demos/Demos/ClothDemo.cs @@ -31,12 +31,11 @@ public ClothCollisionFilter(int x, int y, int instanceId) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static bool Test(in ClothCollisionFilter a, in ClothCollisionFilter b) + public static bool Test(ClothCollisionFilter a, ClothCollisionFilter b, int minimumDistance) { if (a.instanceId != b.instanceId) return true; //Disallow collisions between vertices which are near each other. We measure distance as max(abs(ax - bx), abs(ay - by), abs(az - bz)). - const int minimumDistance = 3; var differenceX = a.x - b.x; if (differenceX < -minimumDistance || differenceX > minimumDistance) return true; @@ -52,15 +51,25 @@ struct ClothCallbacks : INarrowPhaseCallbacks { public CollidableProperty Filters; public PairMaterialProperties Material; + /// + /// Minimum manhattan distance in cloth nodes required for two cloth nodes to collide. Stops adjacent cloth nodes from generating contacts and interfering with clothy behavior. + /// + public int MinimumDistanceForSelfCollisions; + + public ClothCallbacks(CollidableProperty filters, PairMaterialProperties material, int minimumDistanceForSelfCollisions = 3) + { + Filters = filters; + Material = material; + MinimumDistanceForSelfCollisions = minimumDistanceForSelfCollisions; + } + public ClothCallbacks(CollidableProperty filters, int minimumDistanceForSelfCollisions = 3) + : this(filters, new PairMaterialProperties { SpringSettings = new SpringSettings(30, 1), FrictionCoefficient = 0.25f, MaximumRecoveryVelocity = 2f }) + { + } + public void Initialize(Simulation simulation) { Filters.Initialize(simulation); - if (Material.SpringSettings.AngularFrequency == 0 && Material.SpringSettings.TwiceDampingRatio == 0) - { - Material.SpringSettings = new SpringSettings(30, 1); - Material.FrictionCoefficient = 0.25f; - Material.MaximumRecoveryVelocity = 2f; - } } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -68,7 +77,7 @@ public bool AllowContactGeneration(int workerIndex, CollidableReference a, Colli { if (a.Mobility != CollidableMobility.Static && b.Mobility != CollidableMobility.Static) { - return ClothCollisionFilter.Test(Filters[a.BodyHandle], Filters[b.BodyHandle]); + return ClothCollisionFilter.Test(Filters[a.BodyHandle], Filters[b.BodyHandle], MinimumDistanceForSelfCollisions); } return a.Mobility == CollidableMobility.Dynamic || b.Mobility == CollidableMobility.Dynamic; } @@ -160,7 +169,7 @@ void CreateConstraintBetweenBodies(BodyHandle aHandle, BodyHandle bHandle) //Note the use of a limit; the distance is allowed to go smaller. //This helps stop the cloth from having unnatural rigidity. var distance = Vector3.Distance(a.Pose.Position, b.Pose.Position); - Simulation.Solver.Add(aHandle, bHandle, new CenterDistanceLimit(distance * 0.015f, distance, springSettings)); + Simulation.Solver.Add(aHandle, bHandle, new CenterDistanceLimit(distance * 0.15f, distance, springSettings)); } } for (int rowIndex = 0; rowIndex < bodyHandles.GetLength(0); ++rowIndex) @@ -196,7 +205,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0; var filters = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new ClothCallbacks() { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new ClothCallbacks(filters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); rolloverInfo = new RolloverInfo(); bool KinematicTopCorners(int rowIndex, int columnIndex, int width, int height) diff --git a/Demos/Demos/DancerDemo.cs b/Demos/Demos/DancerDemo.cs index c14d9428d..9e6515175 100644 --- a/Demos/Demos/DancerDemo.cs +++ b/Demos/Demos/DancerDemo.cs @@ -55,8 +55,8 @@ struct Dancer const float legOffsetX = 0.135f; const float armOffsetX = 0.25f; - const int dancerGridWidth = 6; - const int dancerGridLength = 6; + const int dancerGridWidth = 16; + const int dancerGridLength = 16; const int dancerCount = dancerGridWidth * dancerGridLength; const int historyLength = 256; @@ -221,7 +221,10 @@ void CreateConstraintBetweenBodies(BodyHandle aHandle, BodyHandle bHandle) { var a = simulation.Bodies[aHandle]; var b = simulation.Bodies[bHandle]; - simulation.Solver.Add(aHandle, bHandle, new CenterDistanceConstraint(Vector3.Distance(a.Pose.Position, b.Pose.Position), springSettings)); + //Note the use of a limit; the distance is allowed to go smaller. + //This helps stop the cloth from having unnatural rigidity. + var distance = Vector3.Distance(a.Pose.Position, b.Pose.Position); + simulation.Solver.Add(aHandle, bHandle, new CenterDistanceLimit(distance * 0.15f, distance, springSettings)); } } for (int rowIndex = 0; rowIndex < bodyHandles.GetLength(0); ++rowIndex) @@ -255,21 +258,18 @@ static float GetDistanceFromMainDancer(int dancerIndex) return MathF.Sqrt(offsetX * offsetX + rowIndex * rowIndex); } - static void TailorDress(Simulation simulation, CollidableProperty filters, int dancerIndex) + static void TailorDress(Simulation simulation, CollidableProperty filters, int dancerIndex, float levelOfDetail) { //The demo uses lower resolution grids on dancers further away from the main dancer. //This is a sorta-example of level of detail. In a 'real' use case, you'd probably want to transition between levels of detail dynamically as the camera moved around. //That's a little trickier, but doable. Going low to high, for example, requires creating bodies at interpolated positions between existing bodies, while going to a lower level of detail removes them. - var distanceFromMainDancer = GetDistanceFromMainDancer(dancerIndex); - var levelOfDetail = Math.Min(2, MathF.Ceiling(MathF.Log2(MathF.Max(1, distanceFromMainDancer / 2)))); - var targetDressDiameter = 2.2f; - var fullDetailWidthInBodies = 40; + var targetDressDiameter = 2.6f; + var fullDetailWidthInBodies = 35; float spacingAtFullDetail = targetDressDiameter / fullDetailWidthInBodies; - float bodyRadiusAtFullDetail = spacingAtFullDetail / 1.5f; + float bodyRadius = spacingAtFullDetail / 1.75f; var scale = MathF.Pow(2, levelOfDetail); var widthInBodies = (int)MathF.Ceiling(fullDetailWidthInBodies / scale); var spacing = spacingAtFullDetail * scale; - var bodyRadius = bodyRadiusAtFullDetail * MathF.Pow(2, MathF.Max(0, levelOfDetail - 1)); var bodies = CreateBodyGrid(new Vector3(0, 0.8f, 0) + GetOffsetForDancer(dancerIndex), widthInBodies, spacing, bodyRadius, 0.01f, dancerIndex, simulation, filters); CreateDistanceConstraints(bodies, new SpringSettings(60, 1), simulation); } @@ -383,49 +383,51 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) motionHistory = new QuickQueue(historyLength * 12, BufferPool); dancers = new Dancer[dancerCount]; - static BodyHandle CreateCopyForDancer(Simulation sourceSimulation, BodyHandle sourceHandle, TypedIndex shapeIndexInTargetSimulation, Simulation targetSimulation, int dancerIndex) + static BodyHandle CreateCopyForDancer(Simulation sourceSimulation, BodyHandle sourceHandle, TypedIndex shapeIndexInTargetSimulation, Simulation targetSimulation, int dancerIndex, CollidableProperty filters) { var description = sourceSimulation.Bodies.GetDescription(sourceHandle); description.Pose.Position += GetOffsetForDancer(dancerIndex); description.Collidable.Shape = shapeIndexInTargetSimulation; description.LocalInertia = default; - return targetSimulation.Bodies.Add(description); + var handle = targetSimulation.Bodies.Add(description); + //Kinematic bodies collide with all cloth, so give it a -1 instance id. + filters.Allocate(handle) = new ClothCollisionFilter(0, 0, -1); + return handle; } for (int i = 0; i < dancers.Length; ++i) { ref var dancer = ref dancers[i]; var dancerFilters = new CollidableProperty(); - dancer.Simulation = Simulation.Create(new BufferPool(), new ClothCallbacks - { - Filters = dancerFilters, - Material = new PairMaterialProperties - { - SpringSettings = new SpringSettings(120, 1), - FrictionCoefficient = .4f, - MaximumRecoveryVelocity = 20 - } - }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); - dancer.BodyHandles.Hips = CreateCopyForDancer(Simulation, sourceBodyHandles.Hips, dancer.Simulation.Shapes.Add(hipShape), dancer.Simulation, i); - dancer.BodyHandles.Abdomen = CreateCopyForDancer(Simulation, sourceBodyHandles.Abdomen, dancer.Simulation.Shapes.Add(abdomenShape), dancer.Simulation, i); - dancer.BodyHandles.Chest = CreateCopyForDancer(Simulation, sourceBodyHandles.Chest, dancer.Simulation.Shapes.Add(chestShape), dancer.Simulation, i); - dancer.BodyHandles.Head = CreateCopyForDancer(Simulation, sourceBodyHandles.Head, dancer.Simulation.Shapes.Add(headShape), dancer.Simulation, i); + //Distance from the main dancer is used to select clothing level of detail. This isn't dynamic based on camera motion, but shows the general idea. + //Since we don't have to worry about transitions, the level of detail is a continuous value here. + //If the required detail goes low enough, note that this demo disables cloth self collision to save some extra time. + //The ClothCallbacks specify a minimum distance required for self collision, and low detail (higher 'level of detail' values) results in MaxValue minimum distance. + var distanceFromMainDancer = GetDistanceFromMainDancer(i); + var levelOfDetail = MathF.Max(0f, MathF.Min(1.5f, MathF.Log2(MathF.Max(1, distanceFromMainDancer) - 0.8f))); + dancer.Simulation = Simulation.Create(new BufferPool(), + new ClothCallbacks(dancerFilters, new PairMaterialProperties(0.4f, 20, new SpringSettings(120, 1)), levelOfDetail <= 0.5f ? 3 : int.MaxValue), + new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + dancer.BodyHandles.Hips = CreateCopyForDancer(Simulation, sourceBodyHandles.Hips, dancer.Simulation.Shapes.Add(hipShape), dancer.Simulation, i, dancerFilters); + dancer.BodyHandles.Abdomen = CreateCopyForDancer(Simulation, sourceBodyHandles.Abdomen, dancer.Simulation.Shapes.Add(abdomenShape), dancer.Simulation, i, dancerFilters); + dancer.BodyHandles.Chest = CreateCopyForDancer(Simulation, sourceBodyHandles.Chest, dancer.Simulation.Shapes.Add(chestShape), dancer.Simulation, i, dancerFilters); + dancer.BodyHandles.Head = CreateCopyForDancer(Simulation, sourceBodyHandles.Head, dancer.Simulation.Shapes.Add(headShape), dancer.Simulation, i, dancerFilters); var upperLegShapeInTarget = dancer.Simulation.Shapes.Add(upperLegShape); var lowerLegShapeInTarget = dancer.Simulation.Shapes.Add(lowerLegShape); - dancer.BodyHandles.UpperLeftLeg = CreateCopyForDancer(Simulation, sourceBodyHandles.UpperLeftLeg, upperLegShapeInTarget, dancer.Simulation, i); - dancer.BodyHandles.LowerLeftLeg = CreateCopyForDancer(Simulation, sourceBodyHandles.LowerLeftLeg, lowerLegShapeInTarget, dancer.Simulation, i); - dancer.BodyHandles.UpperRightLeg = CreateCopyForDancer(Simulation, sourceBodyHandles.UpperRightLeg, upperLegShapeInTarget, dancer.Simulation, i); - dancer.BodyHandles.LowerRightLeg = CreateCopyForDancer(Simulation, sourceBodyHandles.LowerRightLeg, lowerLegShapeInTarget, dancer.Simulation, i); + dancer.BodyHandles.UpperLeftLeg = CreateCopyForDancer(Simulation, sourceBodyHandles.UpperLeftLeg, upperLegShapeInTarget, dancer.Simulation, i, dancerFilters); + dancer.BodyHandles.LowerLeftLeg = CreateCopyForDancer(Simulation, sourceBodyHandles.LowerLeftLeg, lowerLegShapeInTarget, dancer.Simulation, i, dancerFilters); + dancer.BodyHandles.UpperRightLeg = CreateCopyForDancer(Simulation, sourceBodyHandles.UpperRightLeg, upperLegShapeInTarget, dancer.Simulation, i, dancerFilters); + dancer.BodyHandles.LowerRightLeg = CreateCopyForDancer(Simulation, sourceBodyHandles.LowerRightLeg, lowerLegShapeInTarget, dancer.Simulation, i, dancerFilters); var upperArmShapeInTarget = dancer.Simulation.Shapes.Add(upperArmShape); var lowerArmShapeInTarget = dancer.Simulation.Shapes.Add(lowerArmShape); - dancer.BodyHandles.UpperLeftArm = CreateCopyForDancer(Simulation, sourceBodyHandles.UpperLeftArm, upperArmShapeInTarget, dancer.Simulation, i); - dancer.BodyHandles.LowerLeftArm = CreateCopyForDancer(Simulation, sourceBodyHandles.LowerLeftArm, lowerArmShapeInTarget, dancer.Simulation, i); - dancer.BodyHandles.UpperRightArm = CreateCopyForDancer(Simulation, sourceBodyHandles.UpperRightArm, upperArmShapeInTarget, dancer.Simulation, i); - dancer.BodyHandles.LowerRightArm = CreateCopyForDancer(Simulation, sourceBodyHandles.LowerRightArm, lowerArmShapeInTarget, dancer.Simulation, i); + dancer.BodyHandles.UpperLeftArm = CreateCopyForDancer(Simulation, sourceBodyHandles.UpperLeftArm, upperArmShapeInTarget, dancer.Simulation, i, dancerFilters); + dancer.BodyHandles.LowerLeftArm = CreateCopyForDancer(Simulation, sourceBodyHandles.LowerLeftArm, lowerArmShapeInTarget, dancer.Simulation, i, dancerFilters); + dancer.BodyHandles.UpperRightArm = CreateCopyForDancer(Simulation, sourceBodyHandles.UpperRightArm, upperArmShapeInTarget, dancer.Simulation, i, dancerFilters); + dancer.BodyHandles.LowerRightArm = CreateCopyForDancer(Simulation, sourceBodyHandles.LowerRightArm, lowerArmShapeInTarget, dancer.Simulation, i, dancerFilters); - TailorDress(dancer.Simulation, dancerFilters, i); + TailorDress(dancer.Simulation, dancerFilters, i, levelOfDetail); } } @@ -521,6 +523,7 @@ public override void Render(Renderer renderer, Camera camera, Input input, TextB for (int i = 0; i < dancers.Length; ++i) { renderer.Shapes.AddInstances(dancers[i].Simulation); + renderer.Lines.Extract(dancers[i].Simulation.Bodies, dancers[i].Simulation.Solver, dancers[i].Simulation.BroadPhase, true, false, false); } base.Render(renderer, camera, input, text, font); } diff --git a/Demos/SpecializedTests/Media/BedsheetDemo.cs b/Demos/SpecializedTests/Media/BedsheetDemo.cs index e5fdef856..101a2f3d5 100644 --- a/Demos/SpecializedTests/Media/BedsheetDemo.cs +++ b/Demos/SpecializedTests/Media/BedsheetDemo.cs @@ -74,7 +74,7 @@ void CreateConstraintBetweenBodies(BodyHandle aHandle, BodyHandle bHandle) //Note the use of a limit; the distance is allowed to go smaller. //This helps stop the cloth from having unnatural rigidity. var distance = Vector3.Distance(a.Pose.Position, b.Pose.Position); - Simulation.Solver.Add(aHandle, bHandle, new CenterDistanceLimit(distance * 0.015f, distance, springSettings)); + Simulation.Solver.Add(aHandle, bHandle, new CenterDistanceLimit(distance * 0.15f, distance, springSettings)); } } for (int rowIndex = 0; rowIndex < bodyHandles.GetLength(0); ++rowIndex) @@ -110,7 +110,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = MathF.PI * 0.1f; var filters = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new ClothCallbacks() { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -50, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new ClothCallbacks(filters), new DemoPoseIntegratorCallbacks(new Vector3(0, -50, 0)), new SolveDescription(1, 4)); rolloverInfo = new RolloverInfo(); bool FullyDynamic(int rowIndex, int columnIndex, int width, int height) From 1899769d3469bfc10026c6eb8a2ded6a547b1174 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 9 Jan 2022 19:10:17 -0600 Subject: [PATCH 375/947] Dancers now a little less memory hungry. --- BepuPhysics/SimulationAllocationSizes.cs | 24 +++++++++++++++ BepuUtilities/Memory/BufferPool.cs | 4 +-- Demos/Demos/DancerDemo.cs | 39 +++++++++++++++++++----- 3 files changed, 57 insertions(+), 10 deletions(-) diff --git a/BepuPhysics/SimulationAllocationSizes.cs b/BepuPhysics/SimulationAllocationSizes.cs index add852625..f4dbf0132 100644 --- a/BepuPhysics/SimulationAllocationSizes.cs +++ b/BepuPhysics/SimulationAllocationSizes.cs @@ -38,5 +38,29 @@ public struct SimulationAllocationSizes /// public int ConstraintCountPerBodyEstimate; + /// + /// Constructs a description of simulation allocations. + /// + /// The number of bodies to allocate space for. + /// The number of inactive islands to allocate space for. + /// The number of inactive islands to allocate space for. + /// Minimum number of shapes to allocate space for in each shape type batch. + /// The number of constraints to allocate bookkeeping space for. This does not affect actual type batch allocation sizes, only the solver-level constraint handle storage. + /// The minimum number of constraints to allocate space for in each individual type batch. + /// New type batches will be given enough memory for this number of constraints, and any compaction will not reduce the allocations below it. + /// The number of constraints can vary greatly across types- there are usually far more contacts than ragdoll constraints. + /// Per type estimates can be assigned within the Solver.TypeBatchAllocation if necessary. This value acts as a lower bound for all types. + /// The minimum number of constraints to allocate space for in each body's constraint list. + /// New bodies will be given enough memory for this number of constraints, and any compaction will not reduce the allocations below it. + public SimulationAllocationSizes(int bodies, int statics, int islands, int shapesPerType, int constraints, int constraintsPerTypeBatch, int constraintCountPerBodyEstimate) + { + Bodies = bodies; + Statics = statics; + Islands = islands; + ShapesPerType = shapesPerType; + Constraints = constraints; + ConstraintsPerTypeBatch = constraintsPerTypeBatch; + ConstraintCountPerBodyEstimate = constraintCountPerBodyEstimate; + } } } diff --git a/BepuUtilities/Memory/BufferPool.cs b/BepuUtilities/Memory/BufferPool.cs index d0e06bd9e..0d34243c5 100644 --- a/BepuUtilities/Memory/BufferPool.cs +++ b/BepuUtilities/Memory/BufferPool.cs @@ -215,9 +215,7 @@ public void Clear() /// /// Minimum size of individual block allocations. Must be a power of 2. /// Pools with single allocations larger than the minimum will use the minimum value necessary to hold one element. - /// Buffers will be suballocated from blocks. - /// Use a value larger than the large object heap cutoff (85000 bytes as of this writing in the microsoft runtime) - /// to avoid interfering with generational garbage collection. + /// Buffers will be suballocated from blocks. /// Number of suballocations to preallocate reference space for. /// This does not preallocate actual blocks, just the space to hold references that are waiting in the pool. public BufferPool(int minimumBlockAllocationSize = 131072, int expectedPooledResourceCount = 16) diff --git a/Demos/Demos/DancerDemo.cs b/Demos/Demos/DancerDemo.cs index 9e6515175..8b484e501 100644 --- a/Demos/Demos/DancerDemo.cs +++ b/Demos/Demos/DancerDemo.cs @@ -401,13 +401,24 @@ static BodyHandle CreateCopyForDancer(Simulation sourceSimulation, BodyHandle so var dancerFilters = new CollidableProperty(); //Distance from the main dancer is used to select clothing level of detail. This isn't dynamic based on camera motion, but shows the general idea. //Since we don't have to worry about transitions, the level of detail is a continuous value here. - //If the required detail goes low enough, note that this demo disables cloth self collision to save some extra time. - //The ClothCallbacks specify a minimum distance required for self collision, and low detail (higher 'level of detail' values) results in MaxValue minimum distance. var distanceFromMainDancer = GetDistanceFromMainDancer(i); var levelOfDetail = MathF.Max(0f, MathF.Min(1.5f, MathF.Log2(MathF.Max(1, distanceFromMainDancer) - 0.8f))); - dancer.Simulation = Simulation.Create(new BufferPool(), + //Note that we use a smaller allocation block size for dancer simulations. + //This demo is creating a *lot* of buffer pools just because that's the simplest way to keep things thread safe. + //If you wanted to reduce the amount of pool-induced memory overhead, you could consider sharing buffer pools between multiple simulations + //and making sure those simulations never run on multiple threads at the same time to avoid allocation related race conditions. + //Depending on the simulation, it could also be worth having multiple characters simulated in the same simulation even if you don't care about their interactions. + //For example, if you wanted to train a motorized ragdoll using reinforcement learning, it is likely that having multiple ragdolls in each simulation would improve hardware utilization. + //All narrow phase collision tests and constraint solves are vectorized over multiple pairs; having only one ragdoll would likely leave many bundles partially filled. + //In this demo, occupancy is less of a concern since there are a decent number of constraints associated with a single dancer. + dancer.Simulation = Simulation.Create(new BufferPool(16384), + //If the required detail goes low enough, note that this demo disables cloth self collision to save some extra time. + //The ClothCallbacks specify a minimum distance required for self collision, and low detail (higher 'level of detail' values) results in MaxValue minimum distance. new ClothCallbacks(dancerFilters, new PairMaterialProperties(0.4f, 20, new SpringSettings(120, 1)), levelOfDetail <= 0.5f ? 3 : int.MaxValue), - new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4), + //To save some memory, initialize the dancer simulations with smaller starting sizes. For the higher level of detail simulations this could require some resizing. + //More precise estimates could be made without too much work, but the demo will keep it simple. + initialAllocationSizes: new SimulationAllocationSizes(128, 1, 1, 8, 512, 64, 8)); dancer.BodyHandles.Hips = CreateCopyForDancer(Simulation, sourceBodyHandles.Hips, dancer.Simulation.Shapes.Add(hipShape), dancer.Simulation, i, dancerFilters); dancer.BodyHandles.Abdomen = CreateCopyForDancer(Simulation, sourceBodyHandles.Abdomen, dancer.Simulation.Shapes.Add(abdomenShape), dancer.Simulation, i, dancerFilters); dancer.BodyHandles.Chest = CreateCopyForDancer(Simulation, sourceBodyHandles.Chest, dancer.Simulation.Shapes.Add(chestShape), dancer.Simulation, i, dancerFilters); @@ -428,8 +439,12 @@ static BodyHandle CreateCopyForDancer(Simulation sourceSimulation, BodyHandle so dancer.BodyHandles.LowerRightArm = CreateCopyForDancer(Simulation, sourceBodyHandles.LowerRightArm, lowerArmShapeInTarget, dancer.Simulation, i, dancerFilters); TailorDress(dancer.Simulation, dancerFilters, i, levelOfDetail); + dancerBodyCount += dancer.Simulation.Bodies.ActiveSet.Count; + dancerConstraintCount += dancer.Simulation.Solver.CountConstraints(); } } + int dancerBodyCount; + int dancerConstraintCount; unsafe void ExecuteDancer(int dancerIndex) { @@ -477,6 +492,7 @@ Vector3 CreateArmTarget(float t) } double time; + double executionTime; public unsafe override void Update(Window window, Camera camera, Input input, float dt) { time += 1 / 60f; @@ -512,9 +528,7 @@ public unsafe override void Update(Window window, Camera camera, Input input, fl var startTime = Stopwatch.GetTimestamp(); looper.For(0, dancers.Length, ExecuteDancer); var endTime = Stopwatch.GetTimestamp(); - var executionTime = (endTime - startTime) / (double)Stopwatch.Frequency; - Console.WriteLine($"Time (ms): {executionTime * 1000}"); - Console.WriteLine($"Time per dancer, amortized (us): {executionTime * 1e6 / dancers.Length}"); + executionTime = (endTime - startTime) / (double)Stopwatch.Frequency; base.Update(window, camera, input, dt); } @@ -525,6 +539,17 @@ public override void Render(Renderer renderer, Camera camera, Input input, TextB renderer.Shapes.AddInstances(dancers[i].Simulation); renderer.Lines.Extract(dancers[i].Simulation.Bodies, dancers[i].Simulation.Solver, dancers[i].Simulation.BroadPhase, true, false, false); } + + var resolution = renderer.Surface.Resolution; + renderer.TextBatcher.Write(text.Clear().Append("Cosmetic simulations, like cloth, often don't need to be in a game's main simulation."), new Vector2(16, resolution.Y - 144), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Every background dancer in this demo has its own simulation. All dancers can be easily updated in parallel."), new Vector2(16, resolution.Y - 128), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Dancers further from the main dancer use sparser cloth and disable self collision for extra performance."), new Vector2(16, resolution.Y - 112), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Dancer count: ").Append(dancers.Length), new Vector2(16, resolution.Y - 80), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Total cloth body count: ").Append(dancerBodyCount), new Vector2(16, resolution.Y - 64), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Total cloth constraint count: ").Append(dancerConstraintCount), new Vector2(16, resolution.Y - 48), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Total dancer execution time (ms): ").Append(executionTime * 1000, 2), new Vector2(16, resolution.Y - 32), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Amortized execution time per dancer (us): ").Append(executionTime * 1e6 / dancers.Length, 1), new Vector2(16, resolution.Y - 16), 16, Vector3.One, font); + base.Render(renderer, camera, input, text, font); } protected override void OnDispose() From f06b3c9a310fbd1077c190c6d4f22808b5ceedab Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 10 Jan 2022 16:18:00 -0600 Subject: [PATCH 376/947] Dancers now more securely attach their dresses and are less likely to fling them into the void. --- Demos/Demos/DancerDemo.cs | 61 ++++++++++++++++++++++++++++++--------- 1 file changed, 48 insertions(+), 13 deletions(-) diff --git a/Demos/Demos/DancerDemo.cs b/Demos/Demos/DancerDemo.cs index 8b484e501..eb9d82f62 100644 --- a/Demos/Demos/DancerDemo.cs +++ b/Demos/Demos/DancerDemo.cs @@ -20,7 +20,7 @@ namespace Demos.Demos public class DancerDemo : Demo { [StructLayout(LayoutKind.Sequential, Pack = 4)] - struct DollBodyHandles + struct DancerBodyHandles { public BodyHandle Hips; public BodyHandle Abdomen; @@ -35,7 +35,7 @@ struct DollBodyHandles public BodyHandle UpperRightArm; public BodyHandle LowerRightArm; - internal static unsafe Buffer AsBuffer(DollBodyHandles* sourceBodyHandles) + internal static unsafe Buffer AsBuffer(DancerBodyHandles* sourceBodyHandles) { return new Buffer(sourceBodyHandles, 12); } @@ -46,10 +46,10 @@ struct Dancer //Each dancer has its own simulation. The goal is to show a common use case for cosmetic physics- single threaded simulations that don't interact with the main simulation, running in parallel with other simulations. public Simulation Simulation; //The body handles are cached in each simulation so the source states can be mapped onto each dancer. - public DollBodyHandles BodyHandles; + public DancerBodyHandles BodyHandles; } - DollBodyHandles sourceBodyHandles; + DancerBodyHandles sourceBodyHandles; ParallelLooper looper; Dancer[] dancers; @@ -175,13 +175,13 @@ static Vector3 GetOffsetForDancer(int i) return new Vector3(dancerGridWidth * spacing / -2 + (columnIndex + 0.5f) * spacing, 0, -2 + rowIndex * -spacing); } - static BodyHandle[,] CreateBodyGrid(Vector3 position, int widthInNodes, float spacing, float bodyRadius, float massPerBody, + static BodyHandle[,] CreateDressBodyGrid(Vector3 position, int widthInNodes, float spacing, float bodyRadius, float massPerBody, int instanceId, Simulation simulation, CollidableProperty filters) { var description = BodyDescription.CreateDynamic(QuaternionEx.Identity, new BodyInertia { InverseMass = 1f / massPerBody }, simulation.Shapes.Add(new Sphere(bodyRadius)), 0.01f); BodyHandle[,] handles = new BodyHandle[widthInNodes, widthInNodes]; - var armHoleCenter = new Vector2(armOffsetX, 0); - var armHoleRadius = 0.07f; + var armHoleCenter = new Vector2(armOffsetX + 0.065f, 0); + var armHoleRadius = 0.095f; var armHoleRadiusSquared = armHoleRadius * armHoleRadius; var halfWidth = widthInNodes * spacing / 2; var halfWidthSquared = halfWidth * halfWidth; @@ -258,7 +258,7 @@ static float GetDistanceFromMainDancer(int dancerIndex) return MathF.Sqrt(offsetX * offsetX + rowIndex * rowIndex); } - static void TailorDress(Simulation simulation, CollidableProperty filters, int dancerIndex, float levelOfDetail) + static void TailorDress(Simulation simulation, CollidableProperty filters, DancerBodyHandles bodyHandles, int dancerIndex, float levelOfDetail) { //The demo uses lower resolution grids on dancers further away from the main dancer. //This is a sorta-example of level of detail. In a 'real' use case, you'd probably want to transition between levels of detail dynamically as the camera moved around. @@ -270,7 +270,42 @@ static void TailorDress(Simulation simulation, CollidableProperty(chest.Collidable.Shape.Index); + var topOfChestHeight = chest.Pose.Position.Y + chestShape.Radius + bodyRadius; + var bodies = CreateDressBodyGrid(new Vector3(0, topOfChestHeight, 0) + GetOffsetForDancer(dancerIndex), widthInBodies, spacing, bodyRadius, 0.01f, dancerIndex, simulation, filters); + //Create constraints that bind the cloth bodies closest to the chest, to the chest. This keeps the dress from sliding around. + //In the higher resolution simulations, the arm holes and cloth bodies can actually handle it with no help, but for lower levels of detail it can be useful. + //Also, it's very common to want to control how cloth sticks to a character. You could extend this approach to, for example, keep cloth near the body at the waist like a belt. + //This demo uses constraints to attach a subset of the cloth bodies to the chest. + //You could also either treat the bodies as kinematic and have them follow the body, or attach any constraints that would have involved the cloth body to the body instead. + //Using constraints gives you more options in configuration- the attachment doesn't have to be perfectly rigid. + //For the purposes of this demo, it's also simpler to just use some more constraints. + var midpoint = (widthInBodies * 0.5f - 0.5f); + var zRange = (chestShape.Radius * 0.65f) / spacing; + var xRange = (chestShape.Radius * 0.5f + chestShape.HalfLength) / spacing; + var minX = (int)MathF.Ceiling(midpoint - xRange); + var maxX = (int)(midpoint + xRange); + var minZ = (int)MathF.Ceiling(midpoint - zRange); + var maxZ = (int)(midpoint + zRange); + for (int z = minZ; z <= maxZ; ++z) + { + for (int x = minX; x <= maxX; ++x) + { + var clothNodeHandle = bodies[z, x]; + //When creating bodies, we set handles for bodies that don't exist to -1. + if (clothNodeHandle.Value >= 0) + { + var clothNodeBody = simulation.Bodies[clothNodeHandle]; + simulation.Solver.Add(chest.Handle, clothNodeBody.Handle, + new BallSocket + { + LocalOffsetA = QuaternionEx.Transform(clothNodeBody.Pose.Position - chest.Pose.Position, Quaternion.Conjugate(chest.Pose.Orientation)), + SpringSettings = new SpringSettings(30, 1) + }); + } + } + } CreateDistanceConstraints(bodies, new SpringSettings(60, 1), simulation); } public unsafe override void Initialize(ContentArchive content, Camera camera) @@ -438,7 +473,7 @@ static BodyHandle CreateCopyForDancer(Simulation sourceSimulation, BodyHandle so dancer.BodyHandles.UpperRightArm = CreateCopyForDancer(Simulation, sourceBodyHandles.UpperRightArm, upperArmShapeInTarget, dancer.Simulation, i, dancerFilters); dancer.BodyHandles.LowerRightArm = CreateCopyForDancer(Simulation, sourceBodyHandles.LowerRightArm, lowerArmShapeInTarget, dancer.Simulation, i, dancerFilters); - TailorDress(dancer.Simulation, dancerFilters, i, levelOfDetail); + TailorDress(dancer.Simulation, dancerFilters, dancer.BodyHandles, i, levelOfDetail); dancerBodyCount += dancer.Simulation.Bodies.ActiveSet.Count; dancerConstraintCount += dancer.Simulation.Solver.CountConstraints(); } @@ -450,12 +485,12 @@ unsafe void ExecuteDancer(int dancerIndex) { //Copy historical motion states to the dancers. ref var dancer = ref dancers[dancerIndex]; - var sourceHandleBuffer = DollBodyHandles.AsBuffer((DollBodyHandles*)Unsafe.AsPointer(ref sourceBodyHandles)); + var sourceHandleBuffer = DancerBodyHandles.AsBuffer((DancerBodyHandles*)Unsafe.AsPointer(ref sourceBodyHandles)); //Delay is greater for the dancers that are further away, plus a little randomized component to desynchronize them. var historicalStateStartIndex = motionHistory.Count - sourceHandleBuffer.Length * ((int)GetDistanceFromMainDancer(dancerIndex) * 8 + 1 + (HashHelper.Rehash(dancerIndex) & 0xF)); if (historicalStateStartIndex < 0) historicalStateStartIndex = 0; - var targetHandleBuffer = DollBodyHandles.AsBuffer((DollBodyHandles*)Unsafe.AsPointer(ref dancer.BodyHandles)); + var targetHandleBuffer = DancerBodyHandles.AsBuffer((DancerBodyHandles*)Unsafe.AsPointer(ref dancer.BodyHandles)); for (int j = 0; j < sourceHandleBuffer.Length; ++j) { ref var targetMotionState = ref dancer.Simulation.Bodies[targetHandleBuffer[j]].MotionState; @@ -516,7 +551,7 @@ public unsafe override void Update(Window window, Camera camera, Input input, fl rightHandControl.UpdateTarget(Simulation, hipsTarget + rightArmLocalTarget); //Record the latest motion state from the source dancer. - var sourceHandleBuffer = DollBodyHandles.AsBuffer((DollBodyHandles*)Unsafe.AsPointer(ref sourceBodyHandles)); + var sourceHandleBuffer = DancerBodyHandles.AsBuffer((DancerBodyHandles*)Unsafe.AsPointer(ref sourceBodyHandles)); if (motionHistory.Count == historyLength * sourceHandleBuffer.Length) for (int i = 0; i < sourceHandleBuffer.Length; ++i) motionHistory.Dequeue(); From 979bfa4192a0883c4a05723ef0475bcf15f13697 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 12 Jan 2022 19:17:18 -0600 Subject: [PATCH 377/947] DemoRenderer ShapesExtractor now actually uses the thread dispatcher it's given, and can handle multiple simulations simultaneously. --- .../Constraints/BoundingBoxLineExtractor.cs | 4 +- .../Constraints/ConstraintLineExtractor.cs | 4 +- DemoRenderer/Helpers.cs | 8 +- DemoRenderer/ParallelLooper.cs | 41 ++- DemoRenderer/Renderer.cs | 12 +- DemoRenderer/ShapeDrawing/ShapesExtractor.cs | 331 +++++++++++++++--- Demos/Demos/CollisionQueryDemo.cs | 2 +- Demos/Demos/CompoundTestDemo.cs | 2 +- Demos/Demos/ContactEventsDemo.cs | 2 +- Demos/Demos/CustomVoxelCollidableDemo.cs | 2 +- Demos/Demos/DancerDemo.cs | 89 ++--- Demos/Demos/SolverContactEnumerationDemo.cs | 2 +- Demos/Demos/SweepDemo.cs | 20 +- Demos/Demos/Tanks/TankDemo.cs | 2 +- 14 files changed, 382 insertions(+), 139 deletions(-) diff --git a/DemoRenderer/Constraints/BoundingBoxLineExtractor.cs b/DemoRenderer/Constraints/BoundingBoxLineExtractor.cs index 3991825dc..5409fa144 100644 --- a/DemoRenderer/Constraints/BoundingBoxLineExtractor.cs +++ b/DemoRenderer/Constraints/BoundingBoxLineExtractor.cs @@ -28,7 +28,7 @@ struct ThreadJob } BufferPool pool; - Action workDelegate; + LooperAction workDelegate; public BoundingBoxLineExtractor(BufferPool pool) { this.pool = pool; @@ -62,7 +62,7 @@ public static void WriteBoundsLines(in Vector3 min, in Vector3 max, in Vector3 c { WriteBoundsLines(min, max, Helpers.PackColor(color), Helpers.PackColor(backgroundColor), ref targetLines); } - private unsafe void Work(int jobIndex) + private unsafe void Work(int jobIndex, int workerIndex) { ref var job = ref jobs[jobIndex]; var end = job.LeafStart + job.LeafCount; diff --git a/DemoRenderer/Constraints/ConstraintLineExtractor.cs b/DemoRenderer/Constraints/ConstraintLineExtractor.cs index 37ae689b4..32de2d814 100644 --- a/DemoRenderer/Constraints/ConstraintLineExtractor.cs +++ b/DemoRenderer/Constraints/ConstraintLineExtractor.cs @@ -111,7 +111,7 @@ struct ThreadJob public bool Enabled { get; set; } = true; - Action executeJobDelegate; + LooperAction executeJobDelegate; ref TypeLineExtractor AllocateSlot(int typeId) { @@ -172,7 +172,7 @@ public ConstraintLineExtractor(BufferPool pool) Bodies bodies; Solver solver; QuickList targetLines; - private void ExecuteJob(int jobIndex) + private void ExecuteJob(int jobIndex, int workerIndex) { ref var job = ref jobs[jobIndex]; ref var typeBatch = ref solver.Sets[job.SetIndex].Batches[job.BatchIndex].TypeBatches[job.TypeBatchIndex]; diff --git a/DemoRenderer/Helpers.cs b/DemoRenderer/Helpers.cs index a3ba21cc8..9b808c8eb 100644 --- a/DemoRenderer/Helpers.cs +++ b/DemoRenderer/Helpers.cs @@ -146,7 +146,7 @@ public static uint[] GetBoxIndices(int boxCount) /// RGB color to pack. /// Color packed into 32 bits. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static uint PackColor(in Vector3 color) + public static uint PackColor(Vector3 color) { const uint RScale = (1 << 11) - 1; const uint GScale = (1 << 11) - 1; @@ -184,7 +184,7 @@ public static void UnpackColor(uint packedColor, out Vector3 color) /// RGBA color to pack. /// Color packed into 32 bits. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static uint PackColor(in Vector4 color) + public static uint PackColor(Vector4 color) { var scaledColor = Vector4.Max(Vector4.Zero, Vector4.Min(Vector4.One, color)) * 255; return (uint)scaledColor.X | ((uint)scaledColor.Y << 8) | ((uint)scaledColor.Z << 16) | ((uint)scaledColor.W << 24); @@ -211,7 +211,7 @@ public static void UnpackColor(uint packedColor, out Vector4 color) /// Orientation to pack. /// W-less packed orientation, with remaining components negated to guarantee that the reconstructed positive W component is valid. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void PackOrientation(in Quaternion source, out Vector3 packed) + public static void PackOrientation(Quaternion source, out Vector3 packed) { packed = new Vector3(source.X, source.Y, source.Z); if (source.W < 0) @@ -238,7 +238,7 @@ static void PackDuplicateZeroSNORM(float source, out ushort packed) /// Orientation to pack. /// Packed orientation. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe static ulong PackOrientationU64(ref Quaternion source) + public unsafe static ulong PackOrientationU64(Quaternion source) { //This isn't exactly a clever packing, but with 64 bits, cleverness isn't required. ref var vectorSource = ref Unsafe.As(ref source.X); diff --git a/DemoRenderer/ParallelLooper.cs b/DemoRenderer/ParallelLooper.cs index 43fd80ce8..eeaddd3a6 100644 --- a/DemoRenderer/ParallelLooper.cs +++ b/DemoRenderer/ParallelLooper.cs @@ -6,6 +6,19 @@ namespace DemoRenderer { + /// + /// Function called by the for each job. + /// + /// Index of the job to execute. + /// Index of the worker executing the job. + public delegate void LooperAction(int jobIndex, int workerIndex); + + /// + /// Function called by the when a worker determines that there is no more work available. + /// + /// Index of the worker that finished all available work. + public delegate void LooperWorkerDone(int workerIndex); + /// /// Simple multithreaded for loop provider built on an IThreadDispatcher. Performs an atomic operation for every object in the loop, so pre-chunking the works into jobs is important. /// @@ -13,12 +26,12 @@ namespace DemoRenderer /// having zero allocations under normal execution makes it easier to notice when the physics simulation itself is allocating inappropriately. public class ParallelLooper { - Action workerDelegate; + Action dispatcherWorker; public IThreadDispatcher Dispatcher { get; set; } - + public ParallelLooper() { - workerDelegate = Worker; + dispatcherWorker = Worker; } void Worker(int workerIndex) @@ -28,29 +41,35 @@ void Worker(int workerIndex) var index = Interlocked.Increment(ref start); if (index >= end) break; - work(index); + iteration?.Invoke(index, workerIndex); } + workerDone?.Invoke(workerIndex); + } int start, end; - Action work; + LooperAction iteration; + LooperWorkerDone workerDone; - public void For(int start, int exclusiveEnd, Action work) + public void For(int start, int exclusiveEnd, LooperAction workAction, LooperWorkerDone workerDone = null) { - if(Dispatcher == null) + if (Dispatcher == null) { for (int i = start; i < exclusiveEnd; ++i) { - work(i); + workAction?.Invoke(i, 0); } + workerDone?.Invoke(0); } else { this.start = start - 1; this.end = exclusiveEnd; - this.work = work; - Dispatcher.DispatchWorkers(workerDelegate); - this.work = null; + this.iteration = workAction; + this.workerDone = workerDone; + Dispatcher.DispatchWorkers(dispatcherWorker); + this.iteration = null; + this.workerDone = workerDone; } } } diff --git a/DemoRenderer/Renderer.cs b/DemoRenderer/Renderer.cs index 50b848108..7657bf34e 100644 --- a/DemoRenderer/Renderer.cs +++ b/DemoRenderer/Renderer.cs @@ -233,15 +233,15 @@ public void Render(Camera camera) //All ray traced shapes use analytic coverage writes to get antialiasing. context.OutputMerger.SetBlendState(a2cBlendState); - SphereRenderer.Render(context, camera, Surface.Resolution, Shapes.spheres.Span, 0, Shapes.spheres.Count); - CapsuleRenderer.Render(context, camera, Surface.Resolution, Shapes.capsules.Span, 0, Shapes.capsules.Count); - CylinderRenderer.Render(context, camera, Surface.Resolution, Shapes.cylinders.Span, 0, Shapes.cylinders.Count); + SphereRenderer.Render(context, camera, Surface.Resolution, Shapes.ShapeCache.Spheres.Span, 0, Shapes.ShapeCache.Spheres.Count); + CapsuleRenderer.Render(context, camera, Surface.Resolution, Shapes.ShapeCache.Capsules.Span, 0, Shapes.ShapeCache.Capsules.Count); + CylinderRenderer.Render(context, camera, Surface.Resolution, Shapes.ShapeCache.Cylinders.Span, 0, Shapes.ShapeCache.Cylinders.Count); //Non-raytraced shapes just use regular opaque rendering. context.OutputMerger.SetBlendState(opaqueBlendState); - BoxRenderer.Render(context, camera, Surface.Resolution, Shapes.boxes.Span, 0, Shapes.boxes.Count); - TriangleRenderer.Render(context, camera, Surface.Resolution, Shapes.triangles.Span, 0, Shapes.triangles.Count); - MeshRenderer.Render(context, camera, Surface.Resolution, Shapes.meshes.Span, 0, Shapes.meshes.Count); + BoxRenderer.Render(context, camera, Surface.Resolution, Shapes.ShapeCache.Boxes.Span, 0, Shapes.ShapeCache.Boxes.Count); + TriangleRenderer.Render(context, camera, Surface.Resolution, Shapes.ShapeCache.Triangles.Span, 0, Shapes.ShapeCache.Triangles.Count); + MeshRenderer.Render(context, camera, Surface.Resolution, Shapes.ShapeCache.Meshes.Span, 0, Shapes.ShapeCache.Meshes.Count); LineRenderer.Render(context, camera, Surface.Resolution, Lines.lines.Span, 0, Lines.lines.Count); Background.Render(context, camera); diff --git a/DemoRenderer/ShapeDrawing/ShapesExtractor.cs b/DemoRenderer/ShapeDrawing/ShapesExtractor.cs index 887112695..25410b8eb 100644 --- a/DemoRenderer/ShapeDrawing/ShapesExtractor.cs +++ b/DemoRenderer/ShapeDrawing/ShapesExtractor.cs @@ -7,31 +7,59 @@ using System.Runtime.CompilerServices; using SharpDX.Direct3D11; using System; +using System.Diagnostics; namespace DemoRenderer.ShapeDrawing { + public struct ShapeCache + { + internal QuickList Spheres; + internal QuickList Capsules; + internal QuickList Cylinders; + internal QuickList Boxes; + internal QuickList Triangles; + internal QuickList Meshes; + + public ShapeCache(int initialCapacityPerShapeType, BufferPool pool) + { + Spheres = new QuickList(initialCapacityPerShapeType, pool); + Capsules = new QuickList(initialCapacityPerShapeType, pool); + Cylinders = new QuickList(initialCapacityPerShapeType, pool); + Boxes = new QuickList(initialCapacityPerShapeType, pool); + Triangles = new QuickList(initialCapacityPerShapeType, pool); + Meshes = new QuickList(initialCapacityPerShapeType, pool); + } + public void Clear() + { + Spheres.Count = 0; + Capsules.Count = 0; + Cylinders.Count = 0; + Boxes.Count = 0; + Triangles.Count = 0; + Meshes.Count = 0; + } + public void Dispose(BufferPool pool) + { + Spheres.Dispose(pool); + Capsules.Dispose(pool); + Cylinders.Dispose(pool); + Boxes.Dispose(pool); + Triangles.Dispose(pool); + Meshes.Dispose(pool); + } + } + public class ShapesExtractor : IDisposable { - //For now, we only have spheres. Later, once other shapes exist, this will be responsible for bucketing the different shape types and when necessary caching shape models. - internal QuickList spheres; - internal QuickList capsules; - internal QuickList cylinders; - internal QuickList boxes; - internal QuickList triangles; - internal QuickList meshes; BufferPool pool; + public ShapeCache ShapeCache; public MeshCache MeshCache; ParallelLooper looper; public ShapesExtractor(Device device, ParallelLooper looper, BufferPool pool, int initialCapacityPerShapeType = 1024) { - spheres = new QuickList(initialCapacityPerShapeType, pool); - capsules = new QuickList(initialCapacityPerShapeType, pool); - cylinders = new QuickList(initialCapacityPerShapeType, pool); - boxes = new QuickList(initialCapacityPerShapeType, pool); - triangles = new QuickList(initialCapacityPerShapeType, pool); - meshes = new QuickList(initialCapacityPerShapeType, pool); + ShapeCache = new ShapeCache(initialCapacityPerShapeType, pool); this.MeshCache = new MeshCache(device, pool); this.pool = pool; this.looper = looper; @@ -39,26 +67,21 @@ public ShapesExtractor(Device device, ParallelLooper looper, BufferPool pool, in public void ClearInstances() { - spheres.Count = 0; - capsules.Count = 0; - cylinders.Count = 0; - boxes.Count = 0; - triangles.Count = 0; - meshes.Count = 0; + ShapeCache.Clear(); } - private unsafe void AddCompoundChildren(ref Buffer children, Shapes shapes, in RigidPose pose, in Vector3 color) + private unsafe void AddCompoundChildren(ref Buffer children, Shapes shapes, RigidPose pose, Vector3 color, ref ShapeCache shapeCache, BufferPool pool) { for (int i = 0; i < children.Length; ++i) { ref var child = ref children[i]; Compound.GetWorldPose(child.LocalPose, pose, out var childPose); - AddShape(shapes, child.ShapeIndex, ref childPose, color); + AddShape(shapes, child.ShapeIndex, childPose, color, ref shapeCache, pool); } } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void AddShape(void* shapeData, int shapeType, Shapes shapes, ref RigidPose pose, in Vector3 color) + unsafe void AddShape(void* shapeData, int shapeType, Shapes shapes, RigidPose pose, Vector3 color, ref ShapeCache shapeCache, BufferPool pool) { //TODO: This should likely be swapped over to a registration-based virtualized table approach to more easily support custom shape extractors- //generic terrain windows and examples like voxel grids would benefit. @@ -71,7 +94,7 @@ public unsafe void AddShape(void* shapeData, int shapeType, Shapes shapes, ref R instance.Radius = Unsafe.AsRef(shapeData).Radius; Helpers.PackOrientation(pose.Orientation, out instance.PackedOrientation); instance.PackedColor = Helpers.PackColor(color); - spheres.Add(instance, pool); + shapeCache.Spheres.Add(instance, pool); } break; case Capsule.Id: @@ -81,9 +104,9 @@ public unsafe void AddShape(void* shapeData, int shapeType, Shapes shapes, ref R ref var capsule = ref Unsafe.AsRef(shapeData); instance.Radius = capsule.Radius; instance.HalfLength = capsule.HalfLength; - instance.PackedOrientation = Helpers.PackOrientationU64(ref pose.Orientation); + instance.PackedOrientation = Helpers.PackOrientationU64(pose.Orientation); instance.PackedColor = Helpers.PackColor(color); - capsules.Add(instance, pool); + shapeCache.Capsules.Add(instance, pool); } break; case Box.Id: @@ -96,7 +119,7 @@ public unsafe void AddShape(void* shapeData, int shapeType, Shapes shapes, ref R instance.HalfWidth = box.HalfWidth; instance.HalfHeight = box.HalfHeight; instance.HalfLength = box.HalfLength; - boxes.Add(instance, pool); + shapeCache.Boxes.Add(instance, pool); } break; case Triangle.Id: @@ -107,11 +130,11 @@ public unsafe void AddShape(void* shapeData, int shapeType, Shapes shapes, ref R instance.PackedColor = Helpers.PackColor(color); instance.B = triangle.B; instance.C = triangle.C; - instance.PackedOrientation = Helpers.PackOrientationU64(ref pose.Orientation); + instance.PackedOrientation = Helpers.PackOrientationU64(pose.Orientation); instance.X = pose.Position.X; instance.Y = pose.Position.Y; instance.Z = pose.Position.Z; - triangles.Add(instance, pool); + shapeCache.Triangles.Add(instance, pool); } break; case Cylinder.Id: @@ -121,9 +144,9 @@ public unsafe void AddShape(void* shapeData, int shapeType, Shapes shapes, ref R ref var cylinder = ref Unsafe.AsRef(shapeData); instance.Radius = cylinder.Radius; instance.HalfLength = cylinder.HalfLength; - instance.PackedOrientation = Helpers.PackOrientationU64(ref pose.Orientation); + instance.PackedOrientation = Helpers.PackOrientationU64(pose.Orientation); instance.PackedColor = Helpers.PackColor(color); - cylinders.Add(instance, pool); + shapeCache.Cylinders.Add(instance, pool); } break; case ConvexHull.Id: @@ -132,11 +155,17 @@ public unsafe void AddShape(void* shapeData, int shapeType, Shapes shapes, ref R MeshInstance instance; instance.Position = pose.Position; instance.PackedColor = Helpers.PackColor(color); - instance.PackedOrientation = Helpers.PackOrientationU64(ref pose.Orientation); + instance.PackedOrientation = Helpers.PackOrientationU64(pose.Orientation); instance.Scale = Vector3.One; //Memory can be reused, so we slightly reduce the probability of a bad reuse by taking the first 64 bits of data into the hash. var id = (ulong)hull.Points.Memory ^ (ulong)hull.Points.Length ^ (*(ulong*)hull.Points.Memory); - if (!MeshCache.TryGetExistingMesh(id, out instance.VertexStart, out var vertices)) + bool meshExisted; + Buffer vertices; + lock (MeshCache) + { + meshExisted = MeshCache.TryGetExistingMesh(id, out instance.VertexStart, out vertices); + } + if (!meshExisted) { int triangleCount = 0; for (int i = 0; i < hull.FaceToVertexIndicesStart.Length; ++i) @@ -145,7 +174,10 @@ public unsafe void AddShape(void* shapeData, int shapeType, Shapes shapes, ref R triangleCount += faceVertexIndices.Length - 2; } instance.VertexCount = triangleCount * 3; - MeshCache.Allocate(id, instance.VertexCount, out instance.VertexStart, out vertices); + lock (MeshCache) + { + MeshCache.Allocate(id, instance.VertexCount, out instance.VertexStart, out vertices); + } //This is a fresh allocation, so we need to upload vertex data. int targetVertexIndex = 0; for (int i = 0; i < hull.FaceToVertexIndicesStart.Length; ++i) @@ -167,17 +199,17 @@ public unsafe void AddShape(void* shapeData, int shapeType, Shapes shapes, ref R { instance.VertexCount = vertices.Length; } - meshes.Add(instance, pool); + shapeCache.Meshes.Add(instance, pool); } break; case Compound.Id: { - AddCompoundChildren(ref Unsafe.AsRef(shapeData).Children, shapes, pose, color); + AddCompoundChildren(ref Unsafe.AsRef(shapeData).Children, shapes, pose, color, ref shapeCache, pool); } break; case BigCompound.Id: { - AddCompoundChildren(ref Unsafe.AsRef(shapeData).Children, shapes, pose, color); + AddCompoundChildren(ref Unsafe.AsRef(shapeData).Children, shapes, pose, color, ref shapeCache, pool); } break; case Mesh.Id: @@ -186,12 +218,18 @@ public unsafe void AddShape(void* shapeData, int shapeType, Shapes shapes, ref R MeshInstance instance; instance.Position = pose.Position; instance.PackedColor = Helpers.PackColor(color); - instance.PackedOrientation = Helpers.PackOrientationU64(ref pose.Orientation); + instance.PackedOrientation = Helpers.PackOrientationU64(pose.Orientation); instance.Scale = mesh.Scale; //Memory can be reused, so we slightly reduce the probability of a bad reuse by taking the first 64 bits of data into the hash. var id = (ulong)mesh.Triangles.Memory ^ (ulong)mesh.Triangles.Length ^ (*(ulong*)mesh.Triangles.Memory); ; instance.VertexCount = mesh.Triangles.Length * 3; - if (MeshCache.Allocate(id, instance.VertexCount, out instance.VertexStart, out var vertices)) + bool newAllocation; + Buffer vertices; + lock (MeshCache) + { + newAllocation = MeshCache.Allocate(id, instance.VertexCount, out instance.VertexStart, out vertices); + } + if (newAllocation) { //This is a fresh allocation, so we need to upload vertex data. for (int i = 0; i < mesh.Triangles.Length; ++i) @@ -204,31 +242,45 @@ public unsafe void AddShape(void* shapeData, int shapeType, Shapes shapes, ref R vertices[baseVertexIndex + 2] = triangle.B; } } - meshes.Add(instance, pool); + shapeCache.Meshes.Add(instance, pool); } break; } } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public unsafe void AddShape(void* shapeData, int shapeType, Shapes shapes, RigidPose pose, Vector3 color) + { + AddShape(shapeData, shapeType, shapes, pose, color, ref ShapeCache, pool); + } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void AddShape(Shapes shapes, TypedIndex shapeIndex, ref RigidPose pose, in Vector3 color) + unsafe void AddShape(Shapes shapes, TypedIndex shapeIndex, RigidPose pose, Vector3 color, ref ShapeCache shapeCache, BufferPool pool) + { + if (shapeIndex.Exists) + { + shapes[shapeIndex.Type].GetShapeData(shapeIndex.Index, out var shapeData, out _); + AddShape(shapeData, shapeIndex.Type, shapes, pose, color, ref shapeCache, pool); + } + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public unsafe void AddShape(Shapes shapes, TypedIndex shapeIndex, RigidPose pose, Vector3 color) { if (shapeIndex.Exists) { shapes[shapeIndex.Type].GetShapeData(shapeIndex.Index, out var shapeData, out _); - AddShape(shapeData, shapeIndex.Type, shapes, ref pose, color); + AddShape(shapeData, shapeIndex.Type, shapes, pose, color, ref ShapeCache, pool); } } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void AddShape(TShape shape, Shapes shapes, ref RigidPose pose, in Vector3 color) where TShape : IShape + public unsafe void AddShape(TShape shape, Shapes shapes, RigidPose pose, Vector3 color) where TShape : IShape { - AddShape(Unsafe.AsPointer(ref shape), shape.TypeId, shapes, ref pose, color); + AddShape(Unsafe.AsPointer(ref shape), shape.TypeId, shapes, pose, color, ref ShapeCache, pool); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - void AddBodyShape(Shapes shapes, Bodies bodies, int setIndex, int indexInSet) + void AddBodyShape(Shapes shapes, Bodies bodies, int setIndex, int indexInSet, ref ShapeCache shapeCache, BufferPool pool) { ref var set = ref bodies.Sets[setIndex]; var handle = set.IndexToHandle[indexInSet]; @@ -268,11 +320,11 @@ void AddBodyShape(Shapes shapes, Bodies bodies, int setIndex, int indexInSet) color *= sleepTint; } - AddShape(shapes, set.Collidables[indexInSet].Shape, ref state.Motion.Pose, color); + AddShape(shapes, set.Collidables[indexInSet].Shape, state.Motion.Pose, color, ref shapeCache, pool); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - void AddStaticShape(Shapes shapes, Statics statics, int index) + void AddStaticShape(Shapes shapes, Statics statics, int index, ref ShapeCache shapeCache, BufferPool pool) { var handle = statics.IndexToHandle[index]; //Statics don't have any activity states. Just some simple variation on a central static color. @@ -281,10 +333,141 @@ void AddStaticShape(Shapes shapes, Statics statics, int index) var staticVariationSpan = new Vector3(0.07f, 0.07f, 0.03f); var color = staticBase + staticVariationSpan * colorVariation; ref var collidable = ref statics[index]; - AddShape(shapes, collidable.Shape, ref collidable.Pose, color); + AddShape(shapes, collidable.Shape, collidable.Pose, color, ref shapeCache, pool); } - public void AddInstances(Simulation simulation, IThreadDispatcher threadDispatcher = null) + struct Job + { + public int SimulationIndex; + //If the job is about statics, the set index will be -1. + public int SetIndex; + public int StartIndex; + public int Count; + } + QuickList jobs; + //The extractor can operate over one or multiple simulations. We cache them locally for threads to access. + Simulation[] simulations; + Simulation simulation; + Buffer workerCaches; + + void PrepareForMultithreadedExecution(IThreadDispatcher threadDispatcher) + { + jobs = new QuickList(128, pool); + looper.Dispatcher = threadDispatcher; + pool.Take(threadDispatcher.ThreadCount, out workerCaches); + for (int i = 0; i < workerCaches.Length; ++i) + { + workerCaches[i] = new ShapeCache(128, threadDispatcher.GetThreadMemoryPool(i)); + } + } + + void EndMultithreadedExecution() + { + jobs.Dispose(pool); + for (int i = 0; i < workerCaches.Length; ++i) + { + workerCaches[i].Dispose(looper.Dispatcher.GetThreadMemoryPool(i)); + } + looper.Dispatcher = null; + pool.Return(ref workerCaches); + } + + static void CreateJobs(Simulation simulation, int simulationIndex, ref QuickList jobs, BufferPool pool) + { + const int targetBodiesPerJob = 1024; + for (int setIndex = 0; setIndex < simulation.Bodies.Sets.Length; ++setIndex) + { + ref var set = ref simulation.Bodies.Sets[setIndex]; + if (set.Allocated && set.Count > 0) //active set can be allocated and have no bodies in it. + { + var jobCount = (set.Count + targetBodiesPerJob - 1) / targetBodiesPerJob; + var bodiesPerJob = set.Count / jobCount; + var remainder = set.Count - bodiesPerJob * jobCount; + var previousEnd = 0; + for (int j = 0; j < jobCount; ++j) + { + var count = j < remainder ? bodiesPerJob + 1 : bodiesPerJob; + jobs.Allocate(pool) = new Job { SimulationIndex = simulationIndex, SetIndex = setIndex, Count = count, StartIndex = previousEnd }; + previousEnd += count; + } + } + } + { + if (simulation.Statics.Count > 0) + { + var jobCount = (simulation.Statics.Count + targetBodiesPerJob - 1) / targetBodiesPerJob; + var bodiesPerJob = simulation.Statics.Count / jobCount; + var remainder = simulation.Statics.Count - bodiesPerJob * jobCount; + var previousEnd = 0; + for (int j = 0; j < jobCount; ++j) + { + var count = j < remainder ? bodiesPerJob + 1 : bodiesPerJob; + jobs.Allocate(pool) = new Job { SimulationIndex = simulationIndex, SetIndex = -1, Count = count, StartIndex = previousEnd }; + previousEnd += count; + } + } + } + } + + void AddShapesForJob(int jobIndex, int workerIndex) + { + var job = jobs[jobIndex]; + var simulation = simulations == null ? this.simulation : this.simulations[job.SimulationIndex]; + var pool = looper.Dispatcher.GetThreadMemoryPool(workerIndex); + + if (job.SetIndex >= 0) + { + ref var set = ref simulation.Bodies.Sets[job.SetIndex]; + var endIndex = job.StartIndex + job.Count; + Debug.Assert(endIndex <= set.Count); + for (int bodyIndex = job.StartIndex; bodyIndex < endIndex; ++bodyIndex) + { + AddBodyShape(simulation.Shapes, simulation.Bodies, job.SetIndex, bodyIndex, ref workerCaches[workerIndex], pool); + } + } + else + { + //It's a static. + var endIndex = job.StartIndex + job.Count; + Debug.Assert(endIndex <= simulation.Statics.Count); + for (int staticIndex = job.StartIndex; staticIndex < endIndex; ++staticIndex) + { + AddStaticShape(simulation.Shapes, simulation.Statics, staticIndex, ref workerCaches[workerIndex], pool); + } + } + } + + object workerShapeMergeLocker = new object(); + + void CopyWorkerCacheToMainCache(ref QuickList workerCache, ref QuickList mainCache) where TShape : unmanaged + { + if (workerCache.Count > 0) + { + int copyStartLocation; + lock (workerShapeMergeLocker) + { + var newCount = mainCache.Count + workerCache.Count; + mainCache.EnsureCapacity(newCount, pool); + copyStartLocation = mainCache.Count; + mainCache.Count = newCount; + } + workerCache.Span.CopyTo(0, mainCache.Span, copyStartLocation, workerCache.Count); + } + } + + void WorkerDone(int workerIndex) + { + //This fires when a worker finishes its work. We should copy the results into the main buffer. + ref var workerCache = ref workerCaches[workerIndex]; + CopyWorkerCacheToMainCache(ref workerCache.Spheres, ref ShapeCache.Spheres); + CopyWorkerCacheToMainCache(ref workerCache.Capsules, ref ShapeCache.Capsules); + CopyWorkerCacheToMainCache(ref workerCache.Boxes, ref ShapeCache.Boxes); + CopyWorkerCacheToMainCache(ref workerCache.Cylinders, ref ShapeCache.Cylinders); + CopyWorkerCacheToMainCache(ref workerCache.Triangles, ref ShapeCache.Triangles); + CopyWorkerCacheToMainCache(ref workerCache.Meshes, ref ShapeCache.Meshes); + } + + void AddShapesSequentially(Simulation simulation) { for (int i = 0; i < simulation.Bodies.Sets.Length; ++i) { @@ -293,25 +476,61 @@ public void AddInstances(Simulation simulation, IThreadDispatcher threadDispatch { for (int bodyIndex = 0; bodyIndex < set.Count; ++bodyIndex) { - AddBodyShape(simulation.Shapes, simulation.Bodies, i, bodyIndex); + AddBodyShape(simulation.Shapes, simulation.Bodies, i, bodyIndex, ref ShapeCache, pool); } } } for (int i = 0; i < simulation.Statics.Count; ++i) { - AddStaticShape(simulation.Shapes, simulation.Statics, i); + AddStaticShape(simulation.Shapes, simulation.Statics, i, ref ShapeCache, pool); + } + } + + + public void AddInstances(Simulation[] simulations, IThreadDispatcher threadDispatcher = null) + { + if (threadDispatcher != null && threadDispatcher.ThreadCount > 1) + { + this.simulations = simulations; + PrepareForMultithreadedExecution(threadDispatcher); + for (int simulationIndex = 0; simulationIndex < simulations.Length; ++simulationIndex) + { + CreateJobs(simulations[simulationIndex], simulationIndex, ref jobs, pool); + } + looper.For(0, jobs.Count, AddShapesForJob, WorkerDone); + EndMultithreadedExecution(); + this.simulations = default; + } + else + { + for (int simulationIndex = 0; simulationIndex < simulations.Length; ++simulationIndex) + { + AddShapesSequentially(simulations[simulationIndex]); + } + } + } + + public void AddInstances(Simulation simulation, IThreadDispatcher threadDispatcher = null) + { + if (threadDispatcher != null) + { + this.simulation = simulation; + PrepareForMultithreadedExecution(threadDispatcher); + CreateJobs(simulation, 0, ref jobs, pool); + looper.For(0, jobs.Count, AddShapesForJob, WorkerDone); + EndMultithreadedExecution(); + this.simulation = null; + } + else + { + AddShapesSequentially(simulation); } } public void Dispose() { + ShapeCache.Dispose(pool); MeshCache.Dispose(); - spheres.Dispose(pool); - capsules.Dispose(pool); - cylinders.Dispose(pool); - boxes.Dispose(pool); - triangles.Dispose(pool); - meshes.Dispose(pool); } } } diff --git a/Demos/Demos/CollisionQueryDemo.cs b/Demos/Demos/CollisionQueryDemo.cs index dd7895957..11e57d8cb 100644 --- a/Demos/Demos/CollisionQueryDemo.cs +++ b/Demos/Demos/CollisionQueryDemo.cs @@ -250,7 +250,7 @@ public unsafe override void Render(Renderer renderer, Camera camera, Input input for (int i = 0; i < queries.Count; ++i) { ref var query = ref queries[i]; - renderer.Shapes.AddShape(query.Box, Simulation.Shapes, ref query.Pose, queryWasTouched[i] ? new Vector3(0, 1, 0) : new Vector3(1, 0, 0)); + renderer.Shapes.AddShape(query.Box, Simulation.Shapes, query.Pose, queryWasTouched[i] ? new Vector3(0, 1, 0) : new Vector3(1, 0, 0)); } BufferPool.Return(ref queryWasTouched); diff --git a/Demos/Demos/CompoundTestDemo.cs b/Demos/Demos/CompoundTestDemo.cs index c19a4df5b..99d49c87e 100644 --- a/Demos/Demos/CompoundTestDemo.cs +++ b/Demos/Demos/CompoundTestDemo.cs @@ -194,7 +194,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) { var boxShape = new Box(256, 1, 256); var groundShapeIndex = Simulation.Shapes.Add(boxShape); - var groundDescription = new StaticDescription(default, groundShapeIndex); + var groundDescription = new StaticDescription(RigidPose.Identity, groundShapeIndex); Simulation.Statics.Add(groundDescription); } const int planeWidth = 48; diff --git a/Demos/Demos/ContactEventsDemo.cs b/Demos/Demos/ContactEventsDemo.cs index a31f43e30..1bb8f2d21 100644 --- a/Demos/Demos/ContactEventsDemo.cs +++ b/Demos/Demos/ContactEventsDemo.cs @@ -753,7 +753,7 @@ public override void Render(Renderer renderer, Camera camera, Input input, TextB ref var particle = ref particles[i]; var radius = particle.Age * (particle.Age * (0.135f - 2.7f * particle.Age) + 1.35f); var pose = new RigidPose(particle.Position); - renderer.Shapes.AddShape(new Sphere(radius), Simulation.Shapes, ref pose, new Vector3(0, 1, 0)); + renderer.Shapes.AddShape(new Sphere(radius), Simulation.Shapes, pose, new Vector3(0, 1, 0)); } base.Render(renderer, camera, input, text, font); } diff --git a/Demos/Demos/CustomVoxelCollidableDemo.cs b/Demos/Demos/CustomVoxelCollidableDemo.cs index 18b3285be..c8cc1f51f 100644 --- a/Demos/Demos/CustomVoxelCollidableDemo.cs +++ b/Demos/Demos/CustomVoxelCollidableDemo.cs @@ -451,7 +451,7 @@ public override unsafe void Render(Renderer renderer, Camera camera, Input input var localPose = (voxels.VoxelIndices[i] + new Vector3(0.5f) * voxels.VoxelSize); Compound.GetRotatedChildPose(localPose, voxelsPose.Orientation, out var childPose); childPose.Position += voxelsPose.Position; - renderer.Shapes.AddShape(shapeDataPointer, Box.Id, Simulation.Shapes, ref childPose, new Vector3(0.8f, 0.2f, 0.2f)); + renderer.Shapes.AddShape(shapeDataPointer, Box.Id, Simulation.Shapes, childPose, new Vector3(0.8f, 0.2f, 0.2f)); } base.Render(renderer, camera, input, text, font); } diff --git a/Demos/Demos/DancerDemo.cs b/Demos/Demos/DancerDemo.cs index eb9d82f62..cf98f1bc6 100644 --- a/Demos/Demos/DancerDemo.cs +++ b/Demos/Demos/DancerDemo.cs @@ -43,15 +43,17 @@ internal static unsafe Buffer AsBuffer(DancerBodyHandles* sourceBody struct Dancer { - //Each dancer has its own simulation. The goal is to show a common use case for cosmetic physics- single threaded simulations that don't interact with the main simulation, running in parallel with other simulations. public Simulation Simulation; - //The body handles are cached in each simulation so the source states can be mapped onto each dancer. public DancerBodyHandles BodyHandles; } DancerBodyHandles sourceBodyHandles; ParallelLooper looper; - Dancer[] dancers; + //Each dancer has its own simulation. The goal is to show a common use case for cosmetic physics- single threaded simulations that don't interact with the main simulation, running in parallel with other simulations. + //(The simulations and handles are kept separate just because the simulations are handed over as a group to the renderer to draw stuff.) + Simulation[] dancerSimulations; + //The body handles are cached in each simulation so the source states can be mapped onto each dancer. + DancerBodyHandles[] dancerHandles; const float legOffsetX = 0.135f; const float armOffsetX = 0.25f; @@ -417,7 +419,8 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //Keep the states in a queue. Each batch of 12 motion states is the state for a single frame. motionHistory = new QuickQueue(historyLength * 12, BufferPool); - dancers = new Dancer[dancerCount]; + dancerHandles = new DancerBodyHandles[dancerCount]; + dancerSimulations = new Simulation[dancerCount]; static BodyHandle CreateCopyForDancer(Simulation sourceSimulation, BodyHandle sourceHandle, TypedIndex shapeIndexInTargetSimulation, Simulation targetSimulation, int dancerIndex, CollidableProperty filters) { var description = sourceSimulation.Bodies.GetDescription(sourceHandle); @@ -430,9 +433,9 @@ static BodyHandle CreateCopyForDancer(Simulation sourceSimulation, BodyHandle so return handle; } - for (int i = 0; i < dancers.Length; ++i) + for (int i = 0; i < dancerHandles.Length; ++i) { - ref var dancer = ref dancers[i]; + ref var dancer = ref dancerHandles[i]; var dancerFilters = new CollidableProperty(); //Distance from the main dancer is used to select clothing level of detail. This isn't dynamic based on camera motion, but shows the general idea. //Since we don't have to worry about transitions, the level of detail is a continuous value here. @@ -446,7 +449,7 @@ static BodyHandle CreateCopyForDancer(Simulation sourceSimulation, BodyHandle so //For example, if you wanted to train a motorized ragdoll using reinforcement learning, it is likely that having multiple ragdolls in each simulation would improve hardware utilization. //All narrow phase collision tests and constraint solves are vectorized over multiple pairs; having only one ragdoll would likely leave many bundles partially filled. //In this demo, occupancy is less of a concern since there are a decent number of constraints associated with a single dancer. - dancer.Simulation = Simulation.Create(new BufferPool(16384), + var dancerSimulation = Simulation.Create(new BufferPool(16384), //If the required detail goes low enough, note that this demo disables cloth self collision to save some extra time. //The ClothCallbacks specify a minimum distance required for self collision, and low detail (higher 'level of detail' values) results in MaxValue minimum distance. new ClothCallbacks(dancerFilters, new PairMaterialProperties(0.4f, 20, new SpringSettings(120, 1)), levelOfDetail <= 0.5f ? 3 : int.MaxValue), @@ -454,51 +457,53 @@ static BodyHandle CreateCopyForDancer(Simulation sourceSimulation, BodyHandle so //To save some memory, initialize the dancer simulations with smaller starting sizes. For the higher level of detail simulations this could require some resizing. //More precise estimates could be made without too much work, but the demo will keep it simple. initialAllocationSizes: new SimulationAllocationSizes(128, 1, 1, 8, 512, 64, 8)); - dancer.BodyHandles.Hips = CreateCopyForDancer(Simulation, sourceBodyHandles.Hips, dancer.Simulation.Shapes.Add(hipShape), dancer.Simulation, i, dancerFilters); - dancer.BodyHandles.Abdomen = CreateCopyForDancer(Simulation, sourceBodyHandles.Abdomen, dancer.Simulation.Shapes.Add(abdomenShape), dancer.Simulation, i, dancerFilters); - dancer.BodyHandles.Chest = CreateCopyForDancer(Simulation, sourceBodyHandles.Chest, dancer.Simulation.Shapes.Add(chestShape), dancer.Simulation, i, dancerFilters); - dancer.BodyHandles.Head = CreateCopyForDancer(Simulation, sourceBodyHandles.Head, dancer.Simulation.Shapes.Add(headShape), dancer.Simulation, i, dancerFilters); - - var upperLegShapeInTarget = dancer.Simulation.Shapes.Add(upperLegShape); - var lowerLegShapeInTarget = dancer.Simulation.Shapes.Add(lowerLegShape); - dancer.BodyHandles.UpperLeftLeg = CreateCopyForDancer(Simulation, sourceBodyHandles.UpperLeftLeg, upperLegShapeInTarget, dancer.Simulation, i, dancerFilters); - dancer.BodyHandles.LowerLeftLeg = CreateCopyForDancer(Simulation, sourceBodyHandles.LowerLeftLeg, lowerLegShapeInTarget, dancer.Simulation, i, dancerFilters); - dancer.BodyHandles.UpperRightLeg = CreateCopyForDancer(Simulation, sourceBodyHandles.UpperRightLeg, upperLegShapeInTarget, dancer.Simulation, i, dancerFilters); - dancer.BodyHandles.LowerRightLeg = CreateCopyForDancer(Simulation, sourceBodyHandles.LowerRightLeg, lowerLegShapeInTarget, dancer.Simulation, i, dancerFilters); - - var upperArmShapeInTarget = dancer.Simulation.Shapes.Add(upperArmShape); - var lowerArmShapeInTarget = dancer.Simulation.Shapes.Add(lowerArmShape); - dancer.BodyHandles.UpperLeftArm = CreateCopyForDancer(Simulation, sourceBodyHandles.UpperLeftArm, upperArmShapeInTarget, dancer.Simulation, i, dancerFilters); - dancer.BodyHandles.LowerLeftArm = CreateCopyForDancer(Simulation, sourceBodyHandles.LowerLeftArm, lowerArmShapeInTarget, dancer.Simulation, i, dancerFilters); - dancer.BodyHandles.UpperRightArm = CreateCopyForDancer(Simulation, sourceBodyHandles.UpperRightArm, upperArmShapeInTarget, dancer.Simulation, i, dancerFilters); - dancer.BodyHandles.LowerRightArm = CreateCopyForDancer(Simulation, sourceBodyHandles.LowerRightArm, lowerArmShapeInTarget, dancer.Simulation, i, dancerFilters); - - TailorDress(dancer.Simulation, dancerFilters, dancer.BodyHandles, i, levelOfDetail); - dancerBodyCount += dancer.Simulation.Bodies.ActiveSet.Count; - dancerConstraintCount += dancer.Simulation.Solver.CountConstraints(); + dancer.Hips = CreateCopyForDancer(Simulation, sourceBodyHandles.Hips, dancerSimulation.Shapes.Add(hipShape), dancerSimulation, i, dancerFilters); + dancer.Abdomen = CreateCopyForDancer(Simulation, sourceBodyHandles.Abdomen, dancerSimulation.Shapes.Add(abdomenShape), dancerSimulation, i, dancerFilters); + dancer.Chest = CreateCopyForDancer(Simulation, sourceBodyHandles.Chest, dancerSimulation.Shapes.Add(chestShape), dancerSimulation, i, dancerFilters); + dancer.Head = CreateCopyForDancer(Simulation, sourceBodyHandles.Head, dancerSimulation.Shapes.Add(headShape), dancerSimulation, i, dancerFilters); + + var upperLegShapeInTarget = dancerSimulation.Shapes.Add(upperLegShape); + var lowerLegShapeInTarget = dancerSimulation.Shapes.Add(lowerLegShape); + dancer.UpperLeftLeg = CreateCopyForDancer(Simulation, sourceBodyHandles.UpperLeftLeg, upperLegShapeInTarget, dancerSimulation, i, dancerFilters); + dancer.LowerLeftLeg = CreateCopyForDancer(Simulation, sourceBodyHandles.LowerLeftLeg, lowerLegShapeInTarget, dancerSimulation, i, dancerFilters); + dancer.UpperRightLeg = CreateCopyForDancer(Simulation, sourceBodyHandles.UpperRightLeg, upperLegShapeInTarget, dancerSimulation, i, dancerFilters); + dancer.LowerRightLeg = CreateCopyForDancer(Simulation, sourceBodyHandles.LowerRightLeg, lowerLegShapeInTarget, dancerSimulation, i, dancerFilters); + + var upperArmShapeInTarget = dancerSimulation.Shapes.Add(upperArmShape); + var lowerArmShapeInTarget = dancerSimulation.Shapes.Add(lowerArmShape); + dancer.UpperLeftArm = CreateCopyForDancer(Simulation, sourceBodyHandles.UpperLeftArm, upperArmShapeInTarget, dancerSimulation, i, dancerFilters); + dancer.LowerLeftArm = CreateCopyForDancer(Simulation, sourceBodyHandles.LowerLeftArm, lowerArmShapeInTarget, dancerSimulation, i, dancerFilters); + dancer.UpperRightArm = CreateCopyForDancer(Simulation, sourceBodyHandles.UpperRightArm, upperArmShapeInTarget, dancerSimulation, i, dancerFilters); + dancer.LowerRightArm = CreateCopyForDancer(Simulation, sourceBodyHandles.LowerRightArm, lowerArmShapeInTarget, dancerSimulation, i, dancerFilters); + + TailorDress(dancerSimulation, dancerFilters, dancer, i, levelOfDetail); + dancerBodyCount += dancerSimulation.Bodies.ActiveSet.Count; + dancerConstraintCount += dancerSimulation.Solver.CountConstraints(); + dancerSimulations[i] = dancerSimulation; } } int dancerBodyCount; int dancerConstraintCount; - unsafe void ExecuteDancer(int dancerIndex) + unsafe void ExecuteDancer(int dancerIndex, int workerIndex) { //Copy historical motion states to the dancers. - ref var dancer = ref dancers[dancerIndex]; + ref var dancerHandles = ref this.dancerHandles[dancerIndex]; + var dancerSimulation = dancerSimulations[dancerIndex]; var sourceHandleBuffer = DancerBodyHandles.AsBuffer((DancerBodyHandles*)Unsafe.AsPointer(ref sourceBodyHandles)); //Delay is greater for the dancers that are further away, plus a little randomized component to desynchronize them. var historicalStateStartIndex = motionHistory.Count - sourceHandleBuffer.Length * ((int)GetDistanceFromMainDancer(dancerIndex) * 8 + 1 + (HashHelper.Rehash(dancerIndex) & 0xF)); if (historicalStateStartIndex < 0) historicalStateStartIndex = 0; - var targetHandleBuffer = DancerBodyHandles.AsBuffer((DancerBodyHandles*)Unsafe.AsPointer(ref dancer.BodyHandles)); + var targetHandleBuffer = DancerBodyHandles.AsBuffer((DancerBodyHandles*)Unsafe.AsPointer(ref dancerHandles)); for (int j = 0; j < sourceHandleBuffer.Length; ++j) { - ref var targetMotionState = ref dancer.Simulation.Bodies[targetHandleBuffer[j]].MotionState; + ref var targetMotionState = ref dancerSimulation.Bodies[targetHandleBuffer[j]].MotionState; targetMotionState = motionHistory[historicalStateStartIndex + j]; targetMotionState.Pose.Position += GetOffsetForDancer(dancerIndex); } //Update the simulation for the dancer. - dancer.Simulation.Timestep(1 / 60f); + dancerSimulation.Timestep(1 / 60f); } float Smoothstep(float v) @@ -561,7 +566,7 @@ public unsafe override void Update(Window window, Camera camera, Input input, fl motionHistory.EnqueueUnsafely(Simulation.Bodies[sourceHandleBuffer[i]].MotionState); } var startTime = Stopwatch.GetTimestamp(); - looper.For(0, dancers.Length, ExecuteDancer); + looper.For(0, dancerHandles.Length, ExecuteDancer); var endTime = Stopwatch.GetTimestamp(); executionTime = (endTime - startTime) / (double)Stopwatch.Frequency; base.Update(window, camera, input, dt); @@ -569,21 +574,21 @@ public unsafe override void Update(Window window, Camera camera, Input input, fl public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) { - for (int i = 0; i < dancers.Length; ++i) + renderer.Shapes.AddInstances(dancerSimulations, ThreadDispatcher); + for (int i = 0; i < dancerHandles.Length; ++i) { - renderer.Shapes.AddInstances(dancers[i].Simulation); - renderer.Lines.Extract(dancers[i].Simulation.Bodies, dancers[i].Simulation.Solver, dancers[i].Simulation.BroadPhase, true, false, false); + renderer.Lines.Extract(dancerSimulations[i].Bodies, dancerSimulations[i].Solver, dancerSimulations[i].BroadPhase, true, false, false); } var resolution = renderer.Surface.Resolution; renderer.TextBatcher.Write(text.Clear().Append("Cosmetic simulations, like cloth, often don't need to be in a game's main simulation."), new Vector2(16, resolution.Y - 144), 16, Vector3.One, font); renderer.TextBatcher.Write(text.Clear().Append("Every background dancer in this demo has its own simulation. All dancers can be easily updated in parallel."), new Vector2(16, resolution.Y - 128), 16, Vector3.One, font); renderer.TextBatcher.Write(text.Clear().Append("Dancers further from the main dancer use sparser cloth and disable self collision for extra performance."), new Vector2(16, resolution.Y - 112), 16, Vector3.One, font); - renderer.TextBatcher.Write(text.Clear().Append("Dancer count: ").Append(dancers.Length), new Vector2(16, resolution.Y - 80), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Dancer count: ").Append(dancerHandles.Length), new Vector2(16, resolution.Y - 80), 16, Vector3.One, font); renderer.TextBatcher.Write(text.Clear().Append("Total cloth body count: ").Append(dancerBodyCount), new Vector2(16, resolution.Y - 64), 16, Vector3.One, font); renderer.TextBatcher.Write(text.Clear().Append("Total cloth constraint count: ").Append(dancerConstraintCount), new Vector2(16, resolution.Y - 48), 16, Vector3.One, font); renderer.TextBatcher.Write(text.Clear().Append("Total dancer execution time (ms): ").Append(executionTime * 1000, 2), new Vector2(16, resolution.Y - 32), 16, Vector3.One, font); - renderer.TextBatcher.Write(text.Clear().Append("Amortized execution time per dancer (us): ").Append(executionTime * 1e6 / dancers.Length, 1), new Vector2(16, resolution.Y - 16), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Amortized execution time per dancer (us): ").Append(executionTime * 1e6 / dancerHandles.Length, 1), new Vector2(16, resolution.Y - 16), 16, Vector3.One, font); base.Render(renderer, camera, input, text, font); } @@ -592,9 +597,9 @@ protected override void OnDispose() //While the main simulation and pool is disposed by the Demo.cs Dispose function, the dancers have their own pools and need to be cleared. //Note that we don't bother disposing the simulation here- all resources in the simulation were taken from the associated pool, and the simulation will not be used anymore. //We can just clear the buffer pool and let the GC eat the simulation. - for (int i = 0; i < dancers.Length; ++i) + for (int i = 0; i < dancerSimulations.Length; ++i) { - dancers[i].Simulation.BufferPool.Clear(); + dancerSimulations[i].BufferPool.Clear(); } base.OnDispose(); } diff --git a/Demos/Demos/SolverContactEnumerationDemo.cs b/Demos/Demos/SolverContactEnumerationDemo.cs index dc64f07e3..c84e394eb 100644 --- a/Demos/Demos/SolverContactEnumerationDemo.cs +++ b/Demos/Demos/SolverContactEnumerationDemo.cs @@ -250,7 +250,7 @@ public override void Render(Renderer renderer, Camera camera, Input input, TextB RigidPose contactVisualPose; contactVisualPose.Position = contactPosition + contact.Normal * contactVisualShape.HalfLength; QuaternionEx.CreateFromRotationMatrix(basisPose, out contactVisualPose.Orientation); - renderer.Shapes.AddShape(contactVisualShape, Simulation.Shapes, ref contactVisualPose, contact.Depth < 0 ? new Vector3(0, 0, 1) : new Vector3(0, 1, 0)); + renderer.Shapes.AddShape(contactVisualShape, Simulation.Shapes, contactVisualPose, contact.Depth < 0 ? new Vector3(0, 0, 1) : new Vector3(0, 1, 0)); } } renderer.TextBatcher.Write(text.Clear().Append("Sensor manifold constraint count: ").Append(extractor.ConstraintContacts.Count).Append(", contact count: ").Append(sensorContactCount), new Vector2(32, 32), 20, Vector3.One, font); diff --git a/Demos/Demos/SweepDemo.cs b/Demos/Demos/SweepDemo.cs index 3c975dddf..178061fc8 100644 --- a/Demos/Demos/SweepDemo.cs +++ b/Demos/Demos/SweepDemo.cs @@ -110,7 +110,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) } - void DrawShape(ref TShape shape, ref RigidPose pose, in Vector3 color, Shapes shapes, Renderer renderer) + void DrawShape(ref TShape shape, in RigidPose pose, in Vector3 color, Shapes shapes, Renderer renderer) where TShape : struct, IShape { if (typeof(TShape) == typeof(Triangle)) @@ -122,22 +122,22 @@ void DrawShape(ref TShape shape, ref RigidPose pose, in Vector3 color, S flippedTriangle.C = triangle.C; flippedTriangle.A = triangle.B; flippedTriangle.B = triangle.A; - renderer.Shapes.AddShape(triangle, shapes, ref pose, color); - renderer.Shapes.AddShape(flippedTriangle, shapes, ref pose, color); + renderer.Shapes.AddShape(triangle, shapes, pose, color); + renderer.Shapes.AddShape(flippedTriangle, shapes, pose, color); } else { - renderer.Shapes.AddShape(shape, shapes, ref pose, color); + renderer.Shapes.AddShape(shape, shapes, pose, color); } } - unsafe void DrawSweep(TShape shape, ref RigidPose pose, in BodyVelocity velocity, int steps, + unsafe void DrawSweep(TShape shape, in RigidPose pose, in BodyVelocity velocity, int steps, float t, Renderer renderer, in Vector3 color) where TShape : struct, IShape { if (steps == 1) { - DrawShape(ref shape, ref pose, color, Simulation.Shapes, renderer); + DrawShape(ref shape, pose, color, Simulation.Shapes, renderer); } else { @@ -148,7 +148,7 @@ unsafe void DrawSweep(TShape shape, ref RigidPose pose, in BodyVelocity var stepT = stepProgression * t; PoseIntegration.Integrate(pose, velocity, stepT, out var stepPose); var stepColor = color * (0.2f + 0.8f * stepProgression); - DrawShape(ref shape, ref stepPose, stepColor, Simulation.Shapes, renderer); + DrawShape(ref shape, stepPose, stepColor, Simulation.Shapes, renderer); } } } @@ -185,8 +185,8 @@ unsafe void TestSweep( var stepCount = (intersected && t1 > 0) || !intersected ? 100 : 1; var visualizedT = intersected ? t1 : maximumT; - DrawSweep(a, ref poseA, velocityA, stepCount, visualizedT, renderer, colorA); - DrawSweep(b, ref poseB, velocityB, stepCount, visualizedT, renderer, colorB); + DrawSweep(a, poseA, velocityA, stepCount, visualizedT, renderer, colorA); + DrawSweep(b, poseB, velocityB, stepCount, visualizedT, renderer, colorB); if (intersected && t1 > 0) { @@ -378,7 +378,7 @@ public override void Render(Renderer renderer, Camera camera, Input input, TextB var initialPose = new RigidPose { Position = sweepOrigin, Orientation = Quaternion.Identity }; var sweepVelocity = new BodyVelocity { Linear = sweepDirection }; Simulation.Sweep(shape, initialPose, sweepVelocity, 10, BufferPool, ref hitHandler); - DrawSweep(shape, ref initialPose, sweepVelocity, 20, hitHandler.T, renderer, + DrawSweep(shape, initialPose, sweepVelocity, 20, hitHandler.T, renderer, hitHandler.T < float.MaxValue ? new Vector3(0.25f, 1, 0.25f) : new Vector3(1, 0.25f, 0.25f)); if (hitHandler.T < float.MaxValue && hitHandler.T > 0) diff --git a/Demos/Demos/Tanks/TankDemo.cs b/Demos/Demos/Tanks/TankDemo.cs index 2839e4951..5c462ef73 100644 --- a/Demos/Demos/Tanks/TankDemo.cs +++ b/Demos/Demos/Tanks/TankDemo.cs @@ -334,7 +334,7 @@ public override void Render(Renderer renderer, Camera camera, Input input, TextB ref var explosion = ref explosions[i]; var pose = new RigidPose(explosion.Position); //The age is measured in frames, so it's not framerate independent. That's fine for a demo. - renderer.Shapes.AddShape(new Sphere(explosion.Scale * (0.25f + MathF.Sqrt(explosion.Age))), Simulation.Shapes, ref pose, explosion.Color); + renderer.Shapes.AddShape(new Sphere(explosion.Scale * (0.25f + MathF.Sqrt(explosion.Age))), Simulation.Shapes, pose, explosion.Color); if (explosion.Age > 5) { explosions.FastRemoveAt(i); From 30698b86393a6e13301245535c655799a27e45f1 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 14 Jan 2022 12:32:27 -0600 Subject: [PATCH 378/947] Line extraction can now execute over many simulations in parallel. --- BepuPhysics/Solver.cs | 37 ++++++ .../Constraints/BoundingBoxLineExtractor.cs | 61 ++++------ .../Constraints/ConstraintLineExtractor.cs | 115 +++++------------- DemoRenderer/Constraints/LineExtractor.cs | 102 ++++++++++++++-- Demos/DemoHarness.cs | 7 +- Demos/Demos/DancerDemo.cs | 5 +- 6 files changed, 185 insertions(+), 142 deletions(-) diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index 3a82a965e..5242425f2 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -184,6 +184,43 @@ public void ResetPerTypeInitialCapacities() Array.Clear(minimumInitialCapacityPerTypeBatch, 0, minimumInitialCapacityPerTypeBatch.Length); } + /// + /// Counts the number of constraints in a particular type batch. + /// + /// Index of the set containing the type batch. + /// Index of the batch containing the type batch. + /// Index of the type batch within the batch. + /// Number of constraints in the type batch. + /// This handles whether the type batch is in the fallback batch or not. Active fallback batches are not guaranteed to have contiguous constraints, so the value is an upper bound and there may be gaps. + public int CountConstraintsInTypeBatch(int setIndex, int batchIndex, int typeBatchIndex) + { + Debug.Assert(setIndex >= 0 && setIndex < Sets.Length && Sets[setIndex].Allocated && batchIndex >= 0 && batchIndex < Sets[setIndex].Batches.Count && typeBatchIndex >= 0 && typeBatchIndex < Sets[setIndex].Batches[batchIndex].TypeBatches.Count, + "Set index, batch index, and type batch index must point at an existing type batch."); + ref var set = ref Sets[setIndex]; + ref var batch = ref set.Batches[batchIndex]; + ref var typeBatch = ref batch.TypeBatches[typeBatchIndex]; + if (setIndex == 0 && batchIndex > FallbackBatchThreshold) + { + //Sequential fallback batches in the active set currently store constraints noncontiguously to guarantee that each bundle does not share any body references. + //Counting constraints requires skipping over any empty slots. + + var constraintCount = typeBatch.ConstraintCount; + var indexToHandle = typeBatch.IndexToHandle; + int count = 0; + for (int i = 0; i < constraintCount; ++i) + { + //Empty slots are marked with a -1 in the index to handles mapping (and in body references). + if (indexToHandle[i].Value >= 0) + ++count; + } + return count; + } + else + { + return typeBatch.ConstraintCount; + } + } + /// /// Gets the total number of constraints across all sets, batches, and types. Requires enumerating /// all type batches; this can be expensive. diff --git a/DemoRenderer/Constraints/BoundingBoxLineExtractor.cs b/DemoRenderer/Constraints/BoundingBoxLineExtractor.cs index 5409fa144..6a672628a 100644 --- a/DemoRenderer/Constraints/BoundingBoxLineExtractor.cs +++ b/DemoRenderer/Constraints/BoundingBoxLineExtractor.cs @@ -14,27 +14,15 @@ namespace DemoRenderer.Constraints { public class BoundingBoxLineExtractor { - const int jobsPerThread = 4; - QuickList jobs; - BroadPhase broadPhase; - int masterLinesCount; - Buffer masterLinesSpan; - - struct ThreadJob + internal struct ThreadJob { + public int SimulationIndex; public int LeafStart; public int LeafCount; + public int TargetLineStart; public bool CoversActiveCollidables; } - BufferPool pool; - LooperAction workDelegate; - public BoundingBoxLineExtractor(BufferPool pool) - { - this.pool = pool; - jobs = new QuickList(Environment.ProcessorCount * jobsPerThread, pool); - workDelegate = Work; - } [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void WriteBoundsLines(in Vector3 min, in Vector3 max, uint packedColor, uint packedBackgroundColor, ref LineInstance targetLines) { @@ -62,12 +50,10 @@ public static void WriteBoundsLines(in Vector3 min, in Vector3 max, in Vector3 c { WriteBoundsLines(min, max, Helpers.PackColor(color), Helpers.PackColor(backgroundColor), ref targetLines); } - private unsafe void Work(int jobIndex, int workerIndex) + internal unsafe void ExecuteJob(Buffer lines, Simulation simulation, ThreadJob job) { - ref var job = ref jobs[jobIndex]; var end = job.LeafStart + job.LeafCount; var lineCount = 12 * job.LeafCount; - var masterStart = Interlocked.Add(ref masterLinesCount, lineCount) - lineCount; var color = new Vector3(0, 1, 0); var backgroundColor = new Vector3(0, 0, 0); if (!job.CoversActiveCollidables) @@ -84,18 +70,17 @@ private unsafe void Work(int jobIndex, int workerIndex) Vector3* min, max; if (job.CoversActiveCollidables) - broadPhase.GetActiveBoundsPointers(broadPhaseIndex, out min, out max); + simulation.BroadPhase.GetActiveBoundsPointers(broadPhaseIndex, out min, out max); else - broadPhase.GetStaticBoundsPointers(broadPhaseIndex, out min, out max); - var outputStartIndex = masterStart + i * 12; - WriteBoundsLines(*min, *max, packedColor, packedBackgroundColor, ref masterLinesSpan[outputStartIndex]); + simulation.BroadPhase.GetStaticBoundsPointers(broadPhaseIndex, out min, out max); + var outputStartIndex = job.TargetLineStart + i * 12; + WriteBoundsLines(*min, *max, packedColor, packedBackgroundColor, ref lines[outputStartIndex]); } } - - void CreateJobsForTree(in Tree tree, bool active, ref QuickList jobs) + void CreateJobsForTree(in Tree tree, bool active, int simulationIndex, ref QuickList jobs, ref int newLinesCount, ref int nextStart, BufferPool pool) { - var maximumJobCount = jobsPerThread * Environment.ProcessorCount; + var maximumJobCount = 4 * Environment.ProcessorCount; var possibleLeavesPerJob = tree.LeafCount / maximumJobCount; var remainder = tree.LeafCount - possibleLeavesPerJob * maximumJobCount; int jobbedLeafCount = 0; @@ -106,34 +91,30 @@ void CreateJobsForTree(in Tree tree, bool active, ref QuickList jobs) if (jobLeafCount > 0) { ref var job = ref jobs.AllocateUnsafely(); + job.SimulationIndex = simulationIndex; job.LeafCount = jobLeafCount; job.LeafStart = jobbedLeafCount; job.CoversActiveCollidables = active; + job.TargetLineStart = nextStart; jobbedLeafCount += jobLeafCount; + newLinesCount += jobLeafCount * 12; + nextStart += jobLeafCount * 12; } else break; } } - internal unsafe void AddInstances(BroadPhase broadPhase, ref QuickList lines, ParallelLooper looper, BufferPool pool) + internal unsafe void CreateJobs(Simulation simulation, int simulationIndex, ref QuickList lines, ref QuickList jobs, BufferPool pool) { //For now, we only pull the bounding boxes of objects that are active. - lines.EnsureCapacity(lines.Count + 12 * (broadPhase.ActiveTree.LeafCount + broadPhase.StaticTree.LeafCount), pool); - CreateJobsForTree(broadPhase.ActiveTree, true, ref jobs); - CreateJobsForTree(broadPhase.StaticTree, false, ref jobs); - masterLinesSpan = lines.Span; - masterLinesCount = lines.Count; - this.broadPhase = broadPhase; - looper.For(0, jobs.Count, workDelegate); - lines.Count = masterLinesCount; - this.broadPhase = null; - jobs.Count = 0; + lines.EnsureCapacity(lines.Count + 12 * (simulation.BroadPhase.ActiveTree.LeafCount + simulation.BroadPhase.StaticTree.LeafCount), pool); + int newLinesCount = 0; + int nextStart = lines.Count; + CreateJobsForTree(simulation.BroadPhase.ActiveTree, true, simulationIndex, ref jobs, ref newLinesCount, ref nextStart, pool); + CreateJobsForTree(simulation.BroadPhase.StaticTree, false, simulationIndex, ref jobs, ref newLinesCount, ref nextStart, pool); + lines.Allocate(newLinesCount, pool); } - public void Dispose() - { - jobs.Dispose(pool); - } } } diff --git a/DemoRenderer/Constraints/ConstraintLineExtractor.cs b/DemoRenderer/Constraints/ConstraintLineExtractor.cs index 32de2d814..6dcc7b2cf 100644 --- a/DemoRenderer/Constraints/ConstraintLineExtractor.cs +++ b/DemoRenderer/Constraints/ConstraintLineExtractor.cs @@ -90,28 +90,26 @@ public unsafe override void ExtractLines(Bodies bodies, int setIndex, ref TypeBa } } - internal class ConstraintLineExtractor : IDisposable + internal class ConstraintLineExtractor { TypeLineExtractor[] lineExtractors; - const int jobsPerThread = 4; - QuickList jobs; - BufferPool pool; - struct ThreadJob + internal struct ThreadJob { + public int SimulationIndex; public int SetIndex; public int BatchIndex; public int TypeBatchIndex; - public int ConstraintStart; - public int ConstraintCount; - public int LineStart; + public int SourceConstraintStart; + //Source and target constraint counts can differ because of noncontiguous fallback batches. + public int SourceConstraintCount; + public int TargetLineStart; + public int TargetConstraintCount; public int LinesPerConstraint; - public QuickList JobLines; } public bool Enabled { get; set; } = true; - LooperAction executeJobDelegate; ref TypeLineExtractor AllocateSlot(int typeId) { @@ -119,9 +117,8 @@ ref TypeLineExtractor AllocateSlot(int typeId) Array.Resize(ref lineExtractors, typeId + 1); return ref lineExtractors[typeId]; } - public ConstraintLineExtractor(BufferPool pool) + public ConstraintLineExtractor() { - this.pool = pool; lineExtractors = new TypeLineExtractor[32]; AllocateSlot(BallSocketTypeProcessor.BatchTypeId) = new TypeLineExtractor(); AllocateSlot(BallSocketServoTypeProcessor.BatchTypeId) = new TypeLineExtractor(); @@ -163,31 +160,22 @@ public ConstraintLineExtractor(BufferPool pool) //AllocateSlot(Contact6NonconvexTypeProcessor.BatchTypeId) = new TypeLineExtractor(); //AllocateSlot(Contact7NonconvexTypeProcessor.BatchTypeId) = new TypeLineExtractor(); //AllocateSlot(Contact8NonconvexTypeProcessor.BatchTypeId) = new TypeLineExtractor(); - - jobs = new QuickList(Environment.ProcessorCount * (jobsPerThread + 1), pool); - - executeJobDelegate = ExecuteJob; } - Bodies bodies; - Solver solver; - QuickList targetLines; - private void ExecuteJob(int jobIndex, int workerIndex) + internal void ExecuteJob(Buffer lines, Simulation simulation, ThreadJob job) { - ref var job = ref jobs[jobIndex]; - ref var typeBatch = ref solver.Sets[job.SetIndex].Batches[job.BatchIndex].TypeBatches[job.TypeBatchIndex]; + ref var typeBatch = ref simulation.Solver.Sets[job.SetIndex].Batches[job.BatchIndex].TypeBatches[job.TypeBatchIndex]; Debug.Assert(lineExtractors[typeBatch.TypeId] != null, "Jobs should only be created for types which are registered and used."); - lineExtractors[typeBatch.TypeId].ExtractLines(bodies, job.SetIndex, ref typeBatch, job.ConstraintStart, job.ConstraintCount, ref job.JobLines); - //Because the number of lines is not easily known ahead of time, the job accumulates a list locally and then copies it into the global buffer afterwards. - var targetStart = Interlocked.Add(ref targetLines.Count, job.JobLines.Count) - job.JobLines.Count; - job.JobLines.Span.Slice(job.JobLines.Count).CopyTo(0, targetLines, targetStart, job.JobLines.Count); + var targetLines = new QuickList(lines.Slice(job.TargetLineStart, job.TargetConstraintCount * job.LinesPerConstraint)); + lineExtractors[typeBatch.TypeId].ExtractLines(simulation.Bodies, job.SetIndex, ref typeBatch, job.SourceConstraintStart, job.SourceConstraintCount, ref targetLines); } - internal void AddInstances(Bodies bodies, Solver solver, bool showConstraints, bool showContacts, ref QuickList lines, ParallelLooper looper) + internal void CreateJobs(Simulation simulation, int simulationIndex, bool showConstraints, bool showContacts, ref QuickList lines, ref QuickList jobs, BufferPool pool) { int neededLineCapacity = lines.Count; - jobs.Count = 0; + int newLineCount = 0; + var solver = simulation.Solver; for (int setIndex = 0; setIndex < solver.Sets.Length; ++setIndex) { ref var set = ref solver.Sets[setIndex]; @@ -200,82 +188,35 @@ internal void AddInstances(Bodies bodies, Solver solver, bool showConstraints, b { ref var typeBatch = ref batch.TypeBatches[typeBatchIndex]; if (typeBatch.TypeId >= lineExtractors.Length) - continue; //No registered extractor for this type, clearly. + continue; //No registered extractor for this type. var extractor = lineExtractors[typeBatch.TypeId]; var isContactBatch = NarrowPhase.IsContactConstraintType(typeBatch.TypeId); if (extractor != null && ((isContactBatch && showContacts) || (!isContactBatch && showConstraints))) { + //Note that we need to handle the case of fallback batches, hence the call to this helper function rather than directly trusting the potentially conservative typeBatch.ConstraintCount. + var constraintCount = solver.CountConstraintsInTypeBatch(setIndex, batchIndex, typeBatchIndex); jobs.Add(new ThreadJob { + SimulationIndex = simulationIndex, SetIndex = setIndex, BatchIndex = batchIndex, TypeBatchIndex = typeBatchIndex, - ConstraintStart = 0, - ConstraintCount = typeBatch.ConstraintCount, - LineStart = neededLineCapacity, + SourceConstraintStart = 0, + SourceConstraintCount = typeBatch.ConstraintCount, + TargetLineStart = neededLineCapacity, + TargetConstraintCount = constraintCount, LinesPerConstraint = extractor.LinesPerConstraint }, pool); - //Note that this is a conservative estimate. TypeBatches in the fallback constraint batch are not necessarily contiguous, so the allocated constraint count is possibly larger than the true number of constraints. - neededLineCapacity += extractor.LinesPerConstraint * typeBatch.ConstraintCount; + var linesForTypeBatch = extractor.LinesPerConstraint * constraintCount; + neededLineCapacity += linesForTypeBatch; + newLineCount += linesForTypeBatch; } } } } } - var maximumJobSize = Math.Min(32, Math.Max(1, neededLineCapacity / (jobsPerThread * Environment.ProcessorCount))); - var originalJobCount = jobs.Count; - //Split jobs if they're larger than desired to help load balancing a little bit. This isn't terribly important, but it's pretty easy. - for (int i = 0; i < originalJobCount; ++i) - { - ref var job = ref jobs[i]; - if (job.ConstraintCount > maximumJobSize) - { - var subjobCount = (int)Math.Round(0.5 + job.ConstraintCount / (double)maximumJobSize); - var constraintsPerSubjob = job.ConstraintCount / subjobCount; - var remainder = job.ConstraintCount - constraintsPerSubjob * subjobCount; - //Modify the first job in place. - job.ConstraintCount = constraintsPerSubjob; - if (remainder > 0) - ++job.ConstraintCount; - //Append the remaining jobs. - var previousJob = job; - for (int j = 1; j < subjobCount; ++j) - { - var newJob = previousJob; - newJob.LineStart += previousJob.ConstraintCount * newJob.LinesPerConstraint; - newJob.ConstraintStart += previousJob.ConstraintCount; - newJob.ConstraintCount = constraintsPerSubjob; - if (remainder > j) - ++newJob.ConstraintCount; - jobs.Add(newJob, pool); - previousJob = newJob; - } - } - } - lines.EnsureCapacity(neededLineCapacity, pool); - targetLines = lines; - for (int i = 0; i < jobs.Count; ++i) - { - //Local lists for each worker let the accumulation proceed in parallel. The results will be copied by the workers after they finish with a chunk with an interlocked claim on the main buffer. - //Could be quicker with per *worker* lists, but this doesn't really matter. - jobs[i].JobLines = new QuickList(jobs[i].ConstraintCount * jobs[i].LinesPerConstraint, pool); - } - this.bodies = bodies; - this.solver = solver; - looper.For(0, jobs.Count, executeJobDelegate); - this.bodies = null; - this.solver = solver; - lines = targetLines; - targetLines = default; - for (int i = 0; i < jobs.Count; ++i) - { - jobs[i].JobLines.Dispose(pool); - } + lines.Allocate(newLineCount, pool); } - public void Dispose() - { - jobs.Dispose(pool); - } } } diff --git a/DemoRenderer/Constraints/LineExtractor.cs b/DemoRenderer/Constraints/LineExtractor.cs index 20ee3cd57..845cf1c2b 100644 --- a/DemoRenderer/Constraints/LineExtractor.cs +++ b/DemoRenderer/Constraints/LineExtractor.cs @@ -15,22 +15,108 @@ public class LineExtractor : IDisposable BufferPool pool; ParallelLooper looper; + + public bool ShowConstraints = true; + public bool ShowContacts; + public bool ShowBoundingBoxes; + public LineExtractor(BufferPool pool, ParallelLooper looper, int initialLineCapacity = 8192) { lines = new QuickList(initialLineCapacity, pool); - constraints = new ConstraintLineExtractor(pool); - boundingBoxes = new BoundingBoxLineExtractor(pool); + constraints = new ConstraintLineExtractor(); + boundingBoxes = new BoundingBoxLineExtractor(); this.pool = pool; this.looper = looper; } - public void Extract(Bodies bodies, Solver solver, BroadPhase broadPhase, bool showConstraints = true, bool showContacts = false, bool showBoundingBoxes = false, IThreadDispatcher threadDispatcher = null) + Simulation[] simulations; + Simulation simulation; + QuickList constraintJobs; + QuickList boundingBoxJobs; + + void ExecuteJob(int jobIndex, int workerIndex) { + if (jobIndex >= constraintJobs.Count) + { + //This is a bounding box job. + var job = boundingBoxJobs[jobIndex - constraintJobs.Count]; + var simulation = simulations == null ? this.simulation : simulations[job.SimulationIndex]; + boundingBoxes.ExecuteJob(lines.Span, simulation, job); + } + else + { + //This is a constraint job. + var job = constraintJobs[jobIndex]; + var simulation = simulations == null ? this.simulation : simulations[job.SimulationIndex]; + constraints.ExecuteJob(lines.Span, simulation, job); + } + } + + public void Extract(Simulation[] simulations, IThreadDispatcher threadDispatcher = null) + { + if (ShowConstraints || ShowContacts) + { + constraintJobs = new QuickList(128, pool); + for (int i = 0; i < simulations.Length; ++i) + { + constraints.CreateJobs(simulations[i], i, ShowConstraints, ShowContacts, ref lines, ref constraintJobs, pool); + } + } + if (ShowBoundingBoxes) + { + boundingBoxJobs = new QuickList(128, pool); + for (int i = 0; i < simulations.Length; ++i) + { + boundingBoxes.CreateJobs(simulations[i], i, ref lines, ref boundingBoxJobs, pool); + } + } + this.simulations = simulations; looper.Dispatcher = threadDispatcher; - if (showConstraints || showContacts) - constraints.AddInstances(bodies, solver, showConstraints, showContacts, ref lines, looper); - if (showBoundingBoxes) - boundingBoxes.AddInstances(broadPhase, ref lines, looper, pool); + looper.For(0, constraintJobs.Count + boundingBoxJobs.Count, ExecuteJob); + looper.Dispatcher = null; + + if (constraintJobs.Span.Allocated) + { + constraintJobs.Dispose(pool); + constraintJobs = default; + } + if (boundingBoxJobs.Span.Allocated) + { + boundingBoxJobs.Dispose(pool); + boundingBoxJobs = default; + } + this.simulations = null; + + } + + public void Extract(Simulation simulation, IThreadDispatcher threadDispatcher = null) + { + this.simulation = simulation; + if (ShowConstraints || ShowContacts) + { + constraintJobs = new QuickList(128, pool); + constraints.CreateJobs(simulation, 0, ShowConstraints, ShowContacts, ref lines, ref constraintJobs, pool); + } + if (ShowBoundingBoxes) + { + boundingBoxJobs = new QuickList(128, pool); + boundingBoxes.CreateJobs(simulation, 0, ref lines, ref boundingBoxJobs, pool); + } + looper.Dispatcher = threadDispatcher; + looper.For(0, constraintJobs.Count + boundingBoxJobs.Count, ExecuteJob); + looper.Dispatcher = null; + + if (constraintJobs.Span.Allocated) + { + constraintJobs.Dispose(pool); + constraintJobs = default; + } + if (boundingBoxJobs.Span.Allocated) + { + boundingBoxJobs.Dispose(pool); + boundingBoxJobs = default; + } + this.simulation = null; } public ref LineInstance Allocate() @@ -51,8 +137,6 @@ public void ClearInstances() public void Dispose() { lines.Dispose(pool); - constraints.Dispose(); - boundingBoxes.Dispose(); } } } diff --git a/Demos/DemoHarness.cs b/Demos/DemoHarness.cs index 2546337f9..dd4bf12e4 100644 --- a/Demos/DemoHarness.cs +++ b/Demos/DemoHarness.cs @@ -309,7 +309,7 @@ public void Update(float dt) } timeSamples.RecordFrame(demo.Simulation); } - + TextBuilder uiText = new TextBuilder(128); public void Render(Renderer renderer) { @@ -407,7 +407,10 @@ void WriteHoldableName(string controlName, HoldableBind control) } grabber.Draw(renderer.Lines, loop.Camera, loop.Input.MouseLocked, controls.Grab.IsDown(loop.Input), loop.Window.GetNormalizedMousePosition(loop.Input.MousePosition)); renderer.Shapes.AddInstances(demo.Simulation, demo.ThreadDispatcher); - renderer.Lines.Extract(demo.Simulation.Bodies, demo.Simulation.Solver, demo.Simulation.BroadPhase, showConstraints, showContacts, showBoundingBoxes, demo.ThreadDispatcher); + renderer.Lines.ShowConstraints = showConstraints; + renderer.Lines.ShowContacts = showContacts; + renderer.Lines.ShowBoundingBoxes = showBoundingBoxes; + renderer.Lines.Extract(demo.Simulation, demo.ThreadDispatcher); } bool disposed; diff --git a/Demos/Demos/DancerDemo.cs b/Demos/Demos/DancerDemo.cs index cf98f1bc6..457c6cb4e 100644 --- a/Demos/Demos/DancerDemo.cs +++ b/Demos/Demos/DancerDemo.cs @@ -575,10 +575,7 @@ public unsafe override void Update(Window window, Camera camera, Input input, fl public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) { renderer.Shapes.AddInstances(dancerSimulations, ThreadDispatcher); - for (int i = 0; i < dancerHandles.Length; ++i) - { - renderer.Lines.Extract(dancerSimulations[i].Bodies, dancerSimulations[i].Solver, dancerSimulations[i].BroadPhase, true, false, false); - } + renderer.Lines.Extract(dancerSimulations, ThreadDispatcher); var resolution = renderer.Surface.Resolution; renderer.TextBatcher.Write(text.Clear().Append("Cosmetic simulations, like cloth, often don't need to be in a game's main simulation."), new Vector2(16, resolution.Y - 144), 16, Vector3.One, font); From 8fc394cae748b9641b91915c5aa65d126c1a0731 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 15 Jan 2022 18:09:36 -0600 Subject: [PATCH 379/947] Turned down grid density to be kinder to lower end systems. --- Demos/Demos/DancerDemo.cs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Demos/Demos/DancerDemo.cs b/Demos/Demos/DancerDemo.cs index 457c6cb4e..53d6d13e4 100644 --- a/Demos/Demos/DancerDemo.cs +++ b/Demos/Demos/DancerDemo.cs @@ -266,7 +266,7 @@ static void TailorDress(Simulation simulation, CollidableProperty Date: Sat, 15 Jan 2022 20:29:55 -0600 Subject: [PATCH 380/947] Refactored DemoDancers out of DancerDemo to support a PlumpDancerDemo. Moved references to 1/60 fixed timestep to a constant. --- DemoTests/PairDeterminismTests.cs | 2 +- DemoTests/TestUtilities.cs | 2 +- Demos/Demo.cs | 4 +- Demos/DemoSet.cs | 1 + Demos/Demos/Cars/CarDemo.cs | 2 +- Demos/Demos/Characters/CharacterDemo.cs | 5 +- Demos/Demos/ClothDemo.cs | 8 +- Demos/Demos/DancerDemo.cs | 604 ------------------ Demos/Demos/Dancers/DancerDemo.cs | 196 ++++++ Demos/Demos/Dancers/DemoDancers.cs | 467 ++++++++++++++ Demos/Demos/RayCastingDemo.cs | 2 +- Demos/Demos/Sponsors/SponsorDemo.cs | 7 +- Demos/Demos/SweepDemo.cs | 2 +- .../SpecializedTests/BatchedCollisionTests.cs | 2 +- Demos/SpecializedTests/DeterminismTest.cs | 2 +- Demos/SpecializedTests/HeadlessDemo.cs | 4 +- .../Media/ColosseumVideoDemo.cs | 2 +- Demos/SpecializedTests/SolverBatchTestDemo.cs | 2 +- 18 files changed, 688 insertions(+), 626 deletions(-) delete mode 100644 Demos/Demos/DancerDemo.cs create mode 100644 Demos/Demos/Dancers/DancerDemo.cs create mode 100644 Demos/Demos/Dancers/DemoDancers.cs diff --git a/DemoTests/PairDeterminismTests.cs b/DemoTests/PairDeterminismTests.cs index 8aaafecaf..1d8da3209 100644 --- a/DemoTests/PairDeterminismTests.cs +++ b/DemoTests/PairDeterminismTests.cs @@ -69,7 +69,7 @@ public unsafe void OnPairCompleted(int pairId, ref TManifold manifold static void ComputeCollisions(CollisionTaskRegistry registry, Shapes shapes, BufferPool pool, ref Manifolds manifolds, CollidableDescription a, CollidableDescription b, ref Buffer posesA, ref Buffer posesB, Buffer remapIndices, int pairCount, Random random) { - const float dt = 1 / 60f; + const float dt = Demo.TimestepDuration; var callbacks = new BatcherCallbacks { Pool = pool, Manifolds = manifolds }; var batcher = new CollisionBatcher(pool, shapes, registry, dt, callbacks); int flushInterval = random.Next(Math.Max(1, pairCount / 5), pairCount); diff --git a/DemoTests/TestUtilities.cs b/DemoTests/TestUtilities.cs index 88308c4ca..a381fbfc1 100644 --- a/DemoTests/TestUtilities.cs +++ b/DemoTests/TestUtilities.cs @@ -34,7 +34,7 @@ public static long ComputeHash(ref Vector3 v, long constant) demo.Initialize(content, new DemoRenderer.Camera(1, 1, 1, 1)); for (int i = 0; i < frameCount; ++i) { - demo.Update(null, null, null, 1 / 60f); + demo.Update(null, null, null, Demo.TimestepDuration); } long hash = 0; diff --git a/Demos/Demo.cs b/Demos/Demo.cs index 3bf4f8873..152c5988c 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -52,7 +52,7 @@ public virtual void LoadGraphicalContent(ContentArchive content, RenderSurface s public abstract void Initialize(ContentArchive content, Camera camera); - + public const float TimestepDuration = 1 / 60f; public virtual void Update(Window window, Camera camera, Input input, float dt) { //In the demos, we use one time step per frame. We don't bother modifying the physics time step duration for different monitors so different refresh rates @@ -61,7 +61,7 @@ public virtual void Update(Window window, Camera camera, Input input, float dt) //fully decouple simulation and rendering rates across different threads. //(In either case, you'd also want to interpolate or extrapolate simulation results during rendering for smoothness.) //Note that taking steps of variable length can reduce stability. Gradual or one-off changes can work reasonably well. - Simulation.Timestep(1 / 60f, ThreadDispatcher); + Simulation.Timestep(TimestepDuration, ThreadDispatcher); ////Here's an example of how it would look to use more frequent updates, but still with a fixed amount of time simulated per update call: //const float timeToSimulate = 1 / 60f; diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index e7ccc0611..48f6f586f 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -3,6 +3,7 @@ using Demos.Demos; using Demos.Demos.Cars; using Demos.Demos.Characters; +using Demos.Demos.Dancers; using Demos.Demos.Sponsors; using Demos.Demos.Tanks; using Demos.SpecializedTests; diff --git a/Demos/Demos/Cars/CarDemo.cs b/Demos/Demos/Cars/CarDemo.cs index 8f8d81f51..ba51eedd8 100644 --- a/Demos/Demos/Cars/CarDemo.cs +++ b/Demos/Demos/Cars/CarDemo.cs @@ -143,7 +143,7 @@ public override void Update(Window window, Camera camera, Input input, float dt) playerControlActive = !playerControlActive; //For control purposes, we'll match the fixed update rate of the simulation. Could decouple it- this dt isn't //vulnerable to the same instabilities as the simulation itself with variable durations. - const float controlDt = 1 / 60f; + const float controlDt = TimestepDuration; if (playerControlActive) { float steeringSum = 0; diff --git a/Demos/Demos/Characters/CharacterDemo.cs b/Demos/Demos/Characters/CharacterDemo.cs index 7b9f392e5..407319690 100644 --- a/Demos/Demos/Characters/CharacterDemo.cs +++ b/Demos/Demos/Characters/CharacterDemo.cs @@ -197,7 +197,6 @@ void CreateCharacter(Vector3 position) public override void Update(Window window, Camera camera, Input input, float dt) { - const float simulationDt = 1 / 60f; if (input.WasPushed(Key.C)) { if (characterActive) @@ -213,10 +212,10 @@ public override void Update(Window window, Camera camera, Input input, float dt) if (characterActive) { //Console.WriteLine($"Supported: {characters.GetCharacterByBodyHandle(character.BodyHandle).Supported}"); - character.UpdateCharacterGoals(input, camera, simulationDt); + character.UpdateCharacterGoals(input, camera, TimestepDuration); } //Using a fixed time per update to match the demos simulation update rate. - time += 1 / 60f; + time += TimestepDuration; for (int i = 0; i < movingPlatforms.Length; ++i) { movingPlatforms[i].Update(Simulation, time); diff --git a/Demos/Demos/ClothDemo.cs b/Demos/Demos/ClothDemo.cs index 9bd68736f..a679539c7 100644 --- a/Demos/Demos/ClothDemo.cs +++ b/Demos/Demos/ClothDemo.cs @@ -47,7 +47,7 @@ public static bool Test(ClothCollisionFilter a, ClothCollisionFilter b, int mini } - struct ClothCallbacks : INarrowPhaseCallbacks + struct ClothCallbacks : INarrowPhaseCallbacks, Dancers.IDancerNarrowPhaseCallbacks //"IDancerNarrowPhaseCallbacks" just means this is a INarrowPhaseCallbacks usable with the DemoDancers. { public CollidableProperty Filters; public PairMaterialProperties Material; @@ -62,8 +62,12 @@ public ClothCallbacks(CollidableProperty filters, PairMate Material = material; MinimumDistanceForSelfCollisions = minimumDistanceForSelfCollisions; } + ClothCallbacks Dancers.IDancerNarrowPhaseCallbacks.Create(CollidableProperty filters, PairMaterialProperties pairMaterialProperties, int minimumDistanceForSelfCollisions) + { + return new ClothCallbacks(filters, pairMaterialProperties, minimumDistanceForSelfCollisions); + } public ClothCallbacks(CollidableProperty filters, int minimumDistanceForSelfCollisions = 3) - : this(filters, new PairMaterialProperties { SpringSettings = new SpringSettings(30, 1), FrictionCoefficient = 0.25f, MaximumRecoveryVelocity = 2f }) + : this(filters, new PairMaterialProperties { SpringSettings = new SpringSettings(30, 1), FrictionCoefficient = 0.25f, MaximumRecoveryVelocity = 2f }, minimumDistanceForSelfCollisions) { } diff --git a/Demos/Demos/DancerDemo.cs b/Demos/Demos/DancerDemo.cs deleted file mode 100644 index 53d6d13e4..000000000 --- a/Demos/Demos/DancerDemo.cs +++ /dev/null @@ -1,604 +0,0 @@ -using BepuPhysics; -using BepuPhysics.Collidables; -using BepuPhysics.CollisionDetection; -using BepuPhysics.Constraints; -using BepuUtilities; -using BepuUtilities.Collections; -using BepuUtilities.Memory; -using DemoContentLoader; -using DemoRenderer; -using DemoRenderer.UI; -using DemoUtilities; -using System; -using System.Diagnostics; -using System.Numerics; -using System.Runtime.CompilerServices; -using System.Runtime.InteropServices; - -namespace Demos.Demos -{ - public class DancerDemo : Demo - { - [StructLayout(LayoutKind.Sequential, Pack = 4)] - struct DancerBodyHandles - { - public BodyHandle Hips; - public BodyHandle Abdomen; - public BodyHandle Chest; - public BodyHandle Head; - public BodyHandle UpperLeftLeg; - public BodyHandle LowerLeftLeg; - public BodyHandle UpperRightLeg; - public BodyHandle LowerRightLeg; - public BodyHandle UpperLeftArm; - public BodyHandle LowerLeftArm; - public BodyHandle UpperRightArm; - public BodyHandle LowerRightArm; - - internal static unsafe Buffer AsBuffer(DancerBodyHandles* sourceBodyHandles) - { - return new Buffer(sourceBodyHandles, 12); - } - } - - struct Dancer - { - public Simulation Simulation; - public DancerBodyHandles BodyHandles; - } - - DancerBodyHandles sourceBodyHandles; - ParallelLooper looper; - //Each dancer has its own simulation. The goal is to show a common use case for cosmetic physics- single threaded simulations that don't interact with the main simulation, running in parallel with other simulations. - //(The simulations and handles are kept separate just because the simulations are handed over as a group to the renderer to draw stuff.) - Simulation[] dancerSimulations; - //The body handles are cached in each simulation so the source states can be mapped onto each dancer. - DancerBodyHandles[] dancerHandles; - - const float legOffsetX = 0.135f; - const float armOffsetX = 0.25f; - const int dancerGridWidth = 16; - const int dancerGridLength = 16; - const int dancerCount = dancerGridWidth * dancerGridLength; - const int historyLength = 256; - - - struct Control - { - public ConstraintHandle Servo; - public Vector3 LocalOffset; - public ServoSettings ServoSettings; - public SpringSettings SpringSettings; - - public Control(Simulation simulation, BodyHandle body, Vector3 worldControlPoint, ServoSettings servoSettings, SpringSettings springSettings) - { - - ServoSettings = servoSettings; - SpringSettings = springSettings; - var pose = simulation.Bodies[body].Pose; - LocalOffset = QuaternionEx.Transform(worldControlPoint - pose.Position, Quaternion.Conjugate(pose.Orientation)); - Servo = simulation.Solver.Add(body, new OneBodyLinearServo { ServoSettings = servoSettings, SpringSettings = springSettings, LocalOffset = LocalOffset, Target = worldControlPoint }); - } - - public void UpdateTarget(Simulation simulation, Vector3 target) - { - simulation.Solver.ApplyDescription(Servo, new OneBodyLinearServo { ServoSettings = ServoSettings, SpringSettings = SpringSettings, LocalOffset = LocalOffset, Target = target }); - } - - } - Control hipsControl; - Control leftFootControl; - Control rightFootControl; - Control leftHandControl; - Control rightHandControl; - - - - - static void Connect(Simulation simulation, BodyHandle a, BodyHandle b, Vector3 jointLocation, SpringSettings springSettings, ref SubgroupCollisionFilter filterA, ref SubgroupCollisionFilter filterB) - { - var poseA = simulation.Bodies[a].Pose; - var poseB = simulation.Bodies[b].Pose; - simulation.Solver.Add(a, b, - new BallSocket - { - LocalOffsetA = QuaternionEx.Transform(jointLocation - poseA.Position, Quaternion.Conjugate(poseA.Orientation)), - LocalOffsetB = QuaternionEx.Transform(jointLocation - poseB.Position, Quaternion.Conjugate(poseB.Orientation)), - SpringSettings = springSettings - }); - SubgroupCollisionFilter.DisableCollision(ref filterA, ref filterB); - } - - static void CreateLimb(Simulation simulation, CollidableProperty collisionFilters, - BodyHandle body, BodyHandle upperLimb, BodyHandle lowerLimb, - Vector3 bodyToUpperJointLocation, Vector3 upperToLowerJointLocation, SpringSettings springSettings, - int groupIndex, int limbSubgroupIndexStart) - { - ref var bodyFilter = ref collisionFilters[body]; - ref var upperFilter = ref collisionFilters.Allocate(upperLimb); - ref var lowerFilter = ref collisionFilters.Allocate(lowerLimb); - upperFilter = new SubgroupCollisionFilter(groupIndex, limbSubgroupIndexStart); - lowerFilter = new SubgroupCollisionFilter(groupIndex, limbSubgroupIndexStart + 1); - Connect(simulation, body, upperLimb, bodyToUpperJointLocation, springSettings, ref bodyFilter, ref upperFilter); - Connect(simulation, upperLimb, lowerLimb, upperToLowerJointLocation, springSettings, ref upperFilter, ref lowerFilter); - //While this demo isn't trying to make a full ragdoll, it's useful to stop the joints from doing obviously gross stuff. - var bodyPose = simulation.Bodies[body].Pose; - var upperPose = simulation.Bodies[upperLimb].Pose; - var lowerPose = simulation.Bodies[lowerLimb].Pose; - //Prevent the hip from spinning 360 degrees. - simulation.Solver.Add(body, upperLimb, new TwistServo - { - LocalBasisA = QuaternionEx.Concatenate(RagdollDemo.CreateBasis(new Vector3(0, -1, 0), new Vector3(0, 0, 1)), Quaternion.Conjugate(bodyPose.Orientation)), - LocalBasisB = QuaternionEx.Concatenate(RagdollDemo.CreateBasis(new Vector3(0, -1, 0), new Vector3(0, 0, 1)), Quaternion.Conjugate(upperPose.Orientation)), - ServoSettings = ServoSettings.Default, - SpringSettings = new SpringSettings(30, 1) - }); - //Stop knee from flopping every which way. - simulation.Solver.Add(upperLimb, lowerLimb, new AngularHinge - { - LocalHingeAxisA = QuaternionEx.Transform(Vector3.UnitX, Quaternion.Conjugate(upperPose.Orientation)), - LocalHingeAxisB = QuaternionEx.Transform(Vector3.UnitX, Quaternion.Conjugate(lowerPose.Orientation)), - SpringSettings = new SpringSettings(30, 1) - }); - //Prevent knee hyperextension. - var swingLimit = new SwingLimit - { - AxisLocalA = QuaternionEx.Transform(new Vector3(0, 0, 1), QuaternionEx.Conjugate(upperPose.Orientation)), - AxisLocalB = QuaternionEx.Transform(new Vector3(0, 1, 0), QuaternionEx.Conjugate(lowerPose.Orientation)), - MaximumSwingAngle = MathF.PI * 0.4f, - SpringSettings = new SpringSettings(15, 1), - }; - simulation.Solver.Add(upperLimb, lowerLimb, swingLimit); - - } - - static void ConstrainOrientation(Simulation simulation, BodyHandle a, BodyHandle b) - { - simulation.Solver.Add(a, b, new AngularServo - { - ServoSettings = ServoSettings.Default, - SpringSettings = new SpringSettings(6, 1), - TargetRelativeRotationLocalA = Quaternion.Concatenate(simulation.Bodies[b].Pose.Orientation, Quaternion.Conjugate(simulation.Bodies[a].Pose.Orientation)) - }); - } - - - static (int columnIndex, int rowIndex) GetRowAndColumnForDancer(int dancerIndex) - { - var rowIndex = dancerIndex / dancerGridWidth; - return (dancerIndex - rowIndex * dancerGridWidth, rowIndex); - } - - QuickQueue motionHistory; - static Vector3 GetOffsetForDancer(int i) - { - const float spacing = 2; - var (columnIndex, rowIndex) = GetRowAndColumnForDancer(i); - return new Vector3(dancerGridWidth * spacing / -2 + (columnIndex + 0.5f) * spacing, 0, -2 + rowIndex * -spacing); - } - - static BodyHandle[,] CreateDressBodyGrid(Vector3 position, int widthInNodes, float spacing, float bodyRadius, float massPerBody, - int instanceId, Simulation simulation, CollidableProperty filters) - { - var description = BodyDescription.CreateDynamic(QuaternionEx.Identity, new BodyInertia { InverseMass = 1f / massPerBody }, simulation.Shapes.Add(new Sphere(bodyRadius)), 0.01f); - BodyHandle[,] handles = new BodyHandle[widthInNodes, widthInNodes]; - var armHoleCenter = new Vector2(armOffsetX + 0.065f, 0); - var armHoleRadius = 0.095f; - var armHoleRadiusSquared = armHoleRadius * armHoleRadius; - var halfWidth = widthInNodes * spacing / 2; - var halfWidthSquared = halfWidth * halfWidth; - var halfWidthOffset = new Vector2(halfWidth); - for (int rowIndex = 0; rowIndex < widthInNodes; ++rowIndex) - { - for (int columnIndex = 0; columnIndex < widthInNodes; ++columnIndex) - { - var horizontalPosition = new Vector2(columnIndex, rowIndex) * spacing - halfWidthOffset; - var distanceSquared0 = Vector2.DistanceSquared(horizontalPosition, armHoleCenter); - var distanceSquared1 = Vector2.DistanceSquared(horizontalPosition, -armHoleCenter); - var centerDistanceSquared = horizontalPosition.LengthSquared(); - if (distanceSquared0 < armHoleRadiusSquared || distanceSquared1 < armHoleRadiusSquared || centerDistanceSquared > halfWidthSquared) - { - //Too close to an arm or too far from the center, don't create any bodies here. - handles[rowIndex, columnIndex] = new BodyHandle { Value = -1 }; - } - else - { - description.Pose.Position = new Vector3(horizontalPosition.X, 0, horizontalPosition.Y) + position; - var handle = simulation.Bodies.Add(description); - handles[rowIndex, columnIndex] = handle; - if (filters != null) - filters.Allocate(handle) = new ClothCollisionFilter(rowIndex, columnIndex, instanceId); - } - } - } - return handles; - } - static void CreateDistanceConstraints(BodyHandle[,] bodyHandles, SpringSettings springSettings, Simulation simulation) - { - void CreateConstraintBetweenBodies(BodyHandle aHandle, BodyHandle bHandle) - { - //Only create a constraint if bodies on both sides of the pair actually exist. - //In this demo, we use -1 in the body handle slot to represent 'no body'. - if (aHandle.Value >= 0 && bHandle.Value >= 0) - { - var a = simulation.Bodies[aHandle]; - var b = simulation.Bodies[bHandle]; - //Note the use of a limit; the distance is allowed to go smaller. - //This helps stop the cloth from having unnatural rigidity. - var distance = Vector3.Distance(a.Pose.Position, b.Pose.Position); - simulation.Solver.Add(aHandle, bHandle, new CenterDistanceLimit(distance * 0.15f, distance, springSettings)); - } - } - for (int rowIndex = 0; rowIndex < bodyHandles.GetLength(0); ++rowIndex) - { - for (int columnIndex = 0; columnIndex < bodyHandles.GetLength(1) - 1; ++columnIndex) - { - CreateConstraintBetweenBodies(bodyHandles[rowIndex, columnIndex], bodyHandles[rowIndex, columnIndex + 1]); - } - } - for (int rowIndex = 0; rowIndex < bodyHandles.GetLength(0) - 1; ++rowIndex) - { - for (int columnIndex = 0; columnIndex < bodyHandles.GetLength(1); ++columnIndex) - { - CreateConstraintBetweenBodies(bodyHandles[rowIndex, columnIndex], bodyHandles[rowIndex + 1, columnIndex]); - } - } - for (int rowIndex = 0; rowIndex < bodyHandles.GetLength(0) - 1; ++rowIndex) - { - for (int columnIndex = 0; columnIndex < bodyHandles.GetLength(1) - 1; ++columnIndex) - { - CreateConstraintBetweenBodies(bodyHandles[rowIndex, columnIndex], bodyHandles[rowIndex + 1, columnIndex + 1]); - CreateConstraintBetweenBodies(bodyHandles[rowIndex, columnIndex + 1], bodyHandles[rowIndex + 1, columnIndex]); - } - } - } - - static float GetDistanceFromMainDancer(int dancerIndex) - { - var (columnIndex, rowIndex) = GetRowAndColumnForDancer(dancerIndex); - var offsetX = columnIndex - (dancerGridWidth / 2 - 0.5f); - return MathF.Sqrt(offsetX * offsetX + rowIndex * rowIndex); - } - - static void TailorDress(Simulation simulation, CollidableProperty filters, DancerBodyHandles bodyHandles, int dancerIndex, float levelOfDetail) - { - //The demo uses lower resolution grids on dancers further away from the main dancer. - //This is a sorta-example of level of detail. In a 'real' use case, you'd probably want to transition between levels of detail dynamically as the camera moved around. - //That's a little trickier, but doable. Going low to high, for example, requires creating bodies at interpolated positions between existing bodies, while going to a lower level of detail removes them. - var targetDressDiameter = 2.6f; - var fullDetailWidthInBodies = 29; - float spacingAtFullDetail = targetDressDiameter / fullDetailWidthInBodies; - float bodyRadius = spacingAtFullDetail / 1.75f; - var scale = MathF.Pow(2, levelOfDetail); - var widthInBodies = (int)MathF.Ceiling(fullDetailWidthInBodies / scale); - var spacing = spacingAtFullDetail * scale; - var chest = simulation.Bodies[bodyHandles.Chest]; - ref var chestShape = ref simulation.Shapes.GetShape(chest.Collidable.Shape.Index); - var topOfChestHeight = chest.Pose.Position.Y + chestShape.Radius + bodyRadius; - var bodies = CreateDressBodyGrid(new Vector3(0, topOfChestHeight, 0) + GetOffsetForDancer(dancerIndex), widthInBodies, spacing, bodyRadius, 0.01f, dancerIndex, simulation, filters); - //Create constraints that bind the cloth bodies closest to the chest, to the chest. This keeps the dress from sliding around. - //In the higher resolution simulations, the arm holes and cloth bodies can actually handle it with no help, but for lower levels of detail it can be useful. - //Also, it's very common to want to control how cloth sticks to a character. You could extend this approach to, for example, keep cloth near the body at the waist like a belt. - //This demo uses constraints to attach a subset of the cloth bodies to the chest. - //You could also either treat the bodies as kinematic and have them follow the body, or attach any constraints that would have involved the cloth body to the body instead. - //Using constraints gives you more options in configuration- the attachment doesn't have to be perfectly rigid. - //For the purposes of this demo, it's also simpler to just use some more constraints. - var midpoint = (widthInBodies * 0.5f - 0.5f); - var zRange = (chestShape.Radius * 0.65f) / spacing; - var xRange = (chestShape.Radius * 0.5f + chestShape.HalfLength) / spacing; - var minX = (int)MathF.Ceiling(midpoint - xRange); - var maxX = (int)(midpoint + xRange); - var minZ = (int)MathF.Ceiling(midpoint - zRange); - var maxZ = (int)(midpoint + zRange); - for (int z = minZ; z <= maxZ; ++z) - { - for (int x = minX; x <= maxX; ++x) - { - var clothNodeHandle = bodies[z, x]; - //When creating bodies, we set handles for bodies that don't exist to -1. - if (clothNodeHandle.Value >= 0) - { - var clothNodeBody = simulation.Bodies[clothNodeHandle]; - simulation.Solver.Add(chest.Handle, clothNodeBody.Handle, - new BallSocket - { - LocalOffsetA = QuaternionEx.Transform(clothNodeBody.Pose.Position - chest.Pose.Position, Quaternion.Conjugate(chest.Pose.Orientation)), - SpringSettings = new SpringSettings(30, 1) - }); - } - } - } - CreateDistanceConstraints(bodies, new SpringSettings(60, 1), simulation); - } - public unsafe override void Initialize(ContentArchive content, Camera camera) - { - camera.Position = new Vector3(0, 2, 10); - camera.Yaw = 0; - camera.Pitch = 0; - - var collisionFilters = new CollidableProperty(); - //Note very high damping on the main ragdoll simulation; makes it easier to pose. - Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks { CollisionFilters = collisionFilters }, new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0), 0, 0), new SolveDescription(8, 1)); - - looper = new ParallelLooper() { Dispatcher = ThreadDispatcher }; - - var hipsPosition = new Vector3(0, 0, 0); - var abdomenPosition = hipsPosition + new Vector3(0, 0.25f, 0); - var chestPosition = abdomenPosition + new Vector3(0, 0.25f, 0); - var headPosition = chestPosition + new Vector3(0, 0.4f, 0); - //It's helpful to have joint locations for the limbs so we can create capsules from the endpoints. - //There's not going to be a foot or hand body in this demo, since those aren't important for scooting a dress around. - var kneePosition = hipsPosition - new Vector3(0, 0.5f, 0); - var anklePosition = kneePosition - new Vector3(0, 0.5f, 0); - var elbowPosition = chestPosition + new Vector3(0, 0.39f, 0); - var wristPosition = elbowPosition + new Vector3(0, 0.39f, 0); - var armOffset = new Vector3(armOffsetX, 0, 0); - var legOffset = new Vector3(legOffsetX, 0, 0); - const int groupIndex = 0; - - //Build the torso and head bodies. - RagdollDemo.GetCapsuleForLineSegment(hipsPosition - legOffset, hipsPosition + legOffset, 0.14f, out var hipShape, out _, out var hipOrientation); - sourceBodyHandles.Hips = Simulation.Bodies.Add(BodyDescription.CreateDynamic((hipsPosition, hipOrientation), hipShape.ComputeInertia(1), Simulation.Shapes.Add(hipShape), 0.01f)); - ref var hipsFilter = ref collisionFilters.Allocate(sourceBodyHandles.Hips); - hipsFilter = new SubgroupCollisionFilter(groupIndex, 0); - - RagdollDemo.GetCapsuleForLineSegment(abdomenPosition - legOffset * 0.8f, abdomenPosition + legOffset * 0.8f, 0.13f, out var abdomenShape, out _, out var abdomenOrientation); - sourceBodyHandles.Abdomen = Simulation.Bodies.Add(BodyDescription.CreateDynamic((abdomenPosition, abdomenOrientation), abdomenShape.ComputeInertia(1), Simulation.Shapes.Add(abdomenShape), 0.01f)); - ref var abdomenFilter = ref collisionFilters.Allocate(sourceBodyHandles.Abdomen); - abdomenFilter = new SubgroupCollisionFilter(groupIndex, 1); - - RagdollDemo.GetCapsuleForLineSegment(abdomenPosition - legOffset * 0.8f, abdomenPosition + legOffset * 0.8f, 0.165f, out var chestShape, out _, out var chestOrientation); - sourceBodyHandles.Chest = Simulation.Bodies.Add(BodyDescription.CreateDynamic((chestPosition, chestOrientation), chestShape.ComputeInertia(1), Simulation.Shapes.Add(chestShape), 0.01f)); - ref var chestFilter = ref collisionFilters.Allocate(sourceBodyHandles.Chest); - chestFilter = new SubgroupCollisionFilter(groupIndex, 2); - - var headShape = new Sphere(0.17f); - sourceBodyHandles.Head = Simulation.Bodies.Add(BodyDescription.CreateDynamic(headPosition, headShape.ComputeInertia(1), Simulation.Shapes.Add(headShape), 1e-2f)); - ref var headFilter = ref collisionFilters.Allocate(sourceBodyHandles.Head); - headFilter = new SubgroupCollisionFilter(groupIndex, 3); - - var springSettings = new SpringSettings(30, 1); - Connect(Simulation, sourceBodyHandles.Hips, sourceBodyHandles.Abdomen, 0.5f * (hipsPosition + abdomenPosition), springSettings, ref hipsFilter, ref abdomenFilter); - ConstrainOrientation(Simulation, sourceBodyHandles.Hips, sourceBodyHandles.Abdomen); - Connect(Simulation, sourceBodyHandles.Abdomen, sourceBodyHandles.Chest, 0.5f * (abdomenPosition + chestPosition), springSettings, ref abdomenFilter, ref chestFilter); - ConstrainOrientation(Simulation, sourceBodyHandles.Abdomen, sourceBodyHandles.Chest); - Connect(Simulation, sourceBodyHandles.Chest, sourceBodyHandles.Head, 0.5f * (chestPosition + headPosition), springSettings, ref chestFilter, ref headFilter); - ConstrainOrientation(Simulation, sourceBodyHandles.Chest, sourceBodyHandles.Head); - - //Create the legs. - RagdollDemo.GetCapsuleForLineSegment(hipsPosition, kneePosition, 0.11f, out var upperLegShape, out var upperLegPosition, out var upperLegOrientation); - RagdollDemo.GetCapsuleForLineSegment(kneePosition, anklePosition, 0.1f, out var lowerLegShape, out var lowerLegPosition, out var lowerLegOrientation); - var upperLegDescription = BodyDescription.CreateDynamic((upperLegPosition, upperLegOrientation), upperLegShape.ComputeInertia(1), Simulation.Shapes.Add(upperLegShape), 0.01f); - var lowerLegDescription = BodyDescription.CreateDynamic((lowerLegPosition, lowerLegOrientation), lowerLegShape.ComputeInertia(1), Simulation.Shapes.Add(lowerLegShape), 0.01f); - - upperLegDescription.Pose.Position -= legOffset; - lowerLegDescription.Pose.Position -= legOffset; - sourceBodyHandles.UpperLeftLeg = Simulation.Bodies.Add(upperLegDescription); - sourceBodyHandles.LowerLeftLeg = Simulation.Bodies.Add(lowerLegDescription); - upperLegDescription.Pose.Position += 2 * legOffset; - lowerLegDescription.Pose.Position += 2 * legOffset; - sourceBodyHandles.UpperRightLeg = Simulation.Bodies.Add(upperLegDescription); - sourceBodyHandles.LowerRightLeg = Simulation.Bodies.Add(lowerLegDescription); - - CreateLimb(Simulation, collisionFilters, sourceBodyHandles.Hips, sourceBodyHandles.UpperLeftLeg, sourceBodyHandles.LowerLeftLeg, hipsPosition - legOffset, kneePosition - legOffset, springSettings, groupIndex, 4); - CreateLimb(Simulation, collisionFilters, sourceBodyHandles.Hips, sourceBodyHandles.UpperRightLeg, sourceBodyHandles.LowerRightLeg, hipsPosition + legOffset, kneePosition + legOffset, springSettings, groupIndex, 6); - - //Create the arms. - RagdollDemo.GetCapsuleForLineSegment(chestPosition, elbowPosition, 0.08f, out var upperArmShape, out var upperArmPosition, out var upperArmOrientation); - RagdollDemo.GetCapsuleForLineSegment(elbowPosition, wristPosition, 0.075f, out var lowerArmShape, out var lowerArmPosition, out var lowerArmOrientation); - var upperArmDescription = BodyDescription.CreateDynamic((upperArmPosition, upperArmOrientation), upperArmShape.ComputeInertia(1), Simulation.Shapes.Add(upperArmShape), 0.01f); - var lowerArmDescription = BodyDescription.CreateDynamic((lowerArmPosition, lowerArmOrientation), lowerArmShape.ComputeInertia(1), Simulation.Shapes.Add(lowerArmShape), 0.01f); - - upperArmDescription.Pose.Position -= armOffset; - lowerArmDescription.Pose.Position -= armOffset; - sourceBodyHandles.UpperLeftArm = Simulation.Bodies.Add(upperArmDescription); - sourceBodyHandles.LowerLeftArm = Simulation.Bodies.Add(lowerArmDescription); - upperArmDescription.Pose.Position += 2 * armOffset; - lowerArmDescription.Pose.Position += 2 * armOffset; - sourceBodyHandles.UpperRightArm = Simulation.Bodies.Add(upperArmDescription); - sourceBodyHandles.LowerRightArm = Simulation.Bodies.Add(lowerArmDescription); - - CreateLimb(Simulation, collisionFilters, sourceBodyHandles.Chest, sourceBodyHandles.UpperLeftArm, sourceBodyHandles.LowerLeftArm, chestPosition - armOffset, elbowPosition - armOffset, springSettings, groupIndex, 8); - CreateLimb(Simulation, collisionFilters, sourceBodyHandles.Chest, sourceBodyHandles.UpperRightArm, sourceBodyHandles.LowerRightArm, chestPosition + armOffset, elbowPosition + armOffset, springSettings, groupIndex, 10); - - //Create controls. - hipsControl = new Control(Simulation, sourceBodyHandles.Hips, hipsPosition, ServoSettings.Default, new SpringSettings(5, 1)); - Simulation.Solver.Add(sourceBodyHandles.Hips, new OneBodyAngularServo { ServoSettings = ServoSettings.Default, SpringSettings = new SpringSettings(30, 1), TargetOrientation = Simulation.Bodies[sourceBodyHandles.Hips].Pose.Orientation }); - - var limbServoSettings = ServoSettings.Default; - var limbSpringSettings = new SpringSettings(4, 1); - leftFootControl = new Control(Simulation, sourceBodyHandles.LowerLeftLeg, anklePosition - legOffset, limbServoSettings, limbSpringSettings); - rightFootControl = new Control(Simulation, sourceBodyHandles.LowerRightLeg, anklePosition + legOffset, limbServoSettings, limbSpringSettings); - leftHandControl = new Control(Simulation, sourceBodyHandles.LowerLeftArm, wristPosition - armOffset, limbServoSettings, limbSpringSettings); - rightHandControl = new Control(Simulation, sourceBodyHandles.LowerRightArm, wristPosition + armOffset, limbServoSettings, limbSpringSettings); - - - Simulation.Statics.Add(new(new Vector3(0, -1.24f, 0), Simulation.Shapes.Add(new Box(1000, 1, 1000)))); - - //All the background dancers read different historical motion states so that they're not just all doing the exact same thing. - //Keep the states in a queue. Each batch of 12 motion states is the state for a single frame. - motionHistory = new QuickQueue(historyLength * 12, BufferPool); - - dancerHandles = new DancerBodyHandles[dancerCount]; - dancerSimulations = new Simulation[dancerCount]; - static BodyHandle CreateCopyForDancer(Simulation sourceSimulation, BodyHandle sourceHandle, TypedIndex shapeIndexInTargetSimulation, Simulation targetSimulation, int dancerIndex, CollidableProperty filters) - { - var description = sourceSimulation.Bodies.GetDescription(sourceHandle); - description.Pose.Position += GetOffsetForDancer(dancerIndex); - description.Collidable.Shape = shapeIndexInTargetSimulation; - description.LocalInertia = default; - var handle = targetSimulation.Bodies.Add(description); - //Kinematic bodies collide with all cloth, so give it a -1 instance id. - filters.Allocate(handle) = new ClothCollisionFilter(0, 0, -1); - return handle; - } - - for (int i = 0; i < dancerHandles.Length; ++i) - { - ref var dancer = ref dancerHandles[i]; - var dancerFilters = new CollidableProperty(); - //Distance from the main dancer is used to select clothing level of detail. This isn't dynamic based on camera motion, but shows the general idea. - //Since we don't have to worry about transitions, the level of detail is a continuous value here. - var distanceFromMainDancer = GetDistanceFromMainDancer(i); - var levelOfDetail = MathF.Max(0f, MathF.Min(1.5f, MathF.Log2(MathF.Max(1, distanceFromMainDancer) - 0.8f))); - //Note that we use a smaller allocation block size for dancer simulations. - //This demo is creating a *lot* of buffer pools just because that's the simplest way to keep things thread safe. - //If you wanted to reduce the amount of pool-induced memory overhead, you could consider sharing buffer pools between multiple simulations - //and making sure those simulations never run on multiple threads at the same time to avoid allocation related race conditions. - //Depending on the simulation, it could also be worth having multiple characters simulated in the same simulation even if you don't care about their interactions. - //For example, if you wanted to train a motorized ragdoll using reinforcement learning, it is likely that having multiple ragdolls in each simulation would improve hardware utilization. - //All narrow phase collision tests and constraint solves are vectorized over multiple pairs; having only one ragdoll would likely leave many bundles partially filled. - //In this demo, occupancy is less of a concern since there are a decent number of constraints associated with a single dancer. - var dancerSimulation = Simulation.Create(new BufferPool(16384), - //If the required detail goes low enough, note that this demo disables cloth self collision to save some extra time. - //The ClothCallbacks specify a minimum distance required for self collision, and low detail (higher 'level of detail' values) results in MaxValue minimum distance. - new ClothCallbacks(dancerFilters, new PairMaterialProperties(0.4f, 20, new SpringSettings(120, 1)), levelOfDetail <= 0.5f ? 3 : int.MaxValue), - new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4), - //To save some memory, initialize the dancer simulations with smaller starting sizes. For the higher level of detail simulations this could require some resizing. - //More precise estimates could be made without too much work, but the demo will keep it simple. - initialAllocationSizes: new SimulationAllocationSizes(128, 1, 1, 8, 512, 64, 8)); - dancer.Hips = CreateCopyForDancer(Simulation, sourceBodyHandles.Hips, dancerSimulation.Shapes.Add(hipShape), dancerSimulation, i, dancerFilters); - dancer.Abdomen = CreateCopyForDancer(Simulation, sourceBodyHandles.Abdomen, dancerSimulation.Shapes.Add(abdomenShape), dancerSimulation, i, dancerFilters); - dancer.Chest = CreateCopyForDancer(Simulation, sourceBodyHandles.Chest, dancerSimulation.Shapes.Add(chestShape), dancerSimulation, i, dancerFilters); - dancer.Head = CreateCopyForDancer(Simulation, sourceBodyHandles.Head, dancerSimulation.Shapes.Add(headShape), dancerSimulation, i, dancerFilters); - - var upperLegShapeInTarget = dancerSimulation.Shapes.Add(upperLegShape); - var lowerLegShapeInTarget = dancerSimulation.Shapes.Add(lowerLegShape); - dancer.UpperLeftLeg = CreateCopyForDancer(Simulation, sourceBodyHandles.UpperLeftLeg, upperLegShapeInTarget, dancerSimulation, i, dancerFilters); - dancer.LowerLeftLeg = CreateCopyForDancer(Simulation, sourceBodyHandles.LowerLeftLeg, lowerLegShapeInTarget, dancerSimulation, i, dancerFilters); - dancer.UpperRightLeg = CreateCopyForDancer(Simulation, sourceBodyHandles.UpperRightLeg, upperLegShapeInTarget, dancerSimulation, i, dancerFilters); - dancer.LowerRightLeg = CreateCopyForDancer(Simulation, sourceBodyHandles.LowerRightLeg, lowerLegShapeInTarget, dancerSimulation, i, dancerFilters); - - var upperArmShapeInTarget = dancerSimulation.Shapes.Add(upperArmShape); - var lowerArmShapeInTarget = dancerSimulation.Shapes.Add(lowerArmShape); - dancer.UpperLeftArm = CreateCopyForDancer(Simulation, sourceBodyHandles.UpperLeftArm, upperArmShapeInTarget, dancerSimulation, i, dancerFilters); - dancer.LowerLeftArm = CreateCopyForDancer(Simulation, sourceBodyHandles.LowerLeftArm, lowerArmShapeInTarget, dancerSimulation, i, dancerFilters); - dancer.UpperRightArm = CreateCopyForDancer(Simulation, sourceBodyHandles.UpperRightArm, upperArmShapeInTarget, dancerSimulation, i, dancerFilters); - dancer.LowerRightArm = CreateCopyForDancer(Simulation, sourceBodyHandles.LowerRightArm, lowerArmShapeInTarget, dancerSimulation, i, dancerFilters); - - TailorDress(dancerSimulation, dancerFilters, dancer, i, levelOfDetail); - dancerBodyCount += dancerSimulation.Bodies.ActiveSet.Count; - dancerConstraintCount += dancerSimulation.Solver.CountConstraints(); - dancerSimulations[i] = dancerSimulation; - } - } - int dancerBodyCount; - int dancerConstraintCount; - - unsafe void ExecuteDancer(int dancerIndex, int workerIndex) - { - //Copy historical motion states to the dancers. - ref var dancerHandles = ref this.dancerHandles[dancerIndex]; - var dancerSimulation = dancerSimulations[dancerIndex]; - var sourceHandleBuffer = DancerBodyHandles.AsBuffer((DancerBodyHandles*)Unsafe.AsPointer(ref sourceBodyHandles)); - //Delay is greater for the dancers that are further away, plus a little randomized component to desynchronize them. - var historicalStateStartIndex = motionHistory.Count - sourceHandleBuffer.Length * ((int)GetDistanceFromMainDancer(dancerIndex) * 8 + 1 + (HashHelper.Rehash(dancerIndex) & 0xF)); - if (historicalStateStartIndex < 0) - historicalStateStartIndex = 0; - var targetHandleBuffer = DancerBodyHandles.AsBuffer((DancerBodyHandles*)Unsafe.AsPointer(ref dancerHandles)); - for (int j = 0; j < sourceHandleBuffer.Length; ++j) - { - ref var targetMotionState = ref dancerSimulation.Bodies[targetHandleBuffer[j]].MotionState; - targetMotionState = motionHistory[historicalStateStartIndex + j]; - targetMotionState.Pose.Position += GetOffsetForDancer(dancerIndex); - } - //Update the simulation for the dancer. - dancerSimulation.Timestep(1 / 60f); - } - - float Smoothstep(float v) - { - var v2 = v * v; - return 3 * v2 - 2 * v2 * v; - } - - Vector3 CreateLegTarget(float t) - { - var z = MathF.Cos(t * MathF.Tau); - var zOffset = (Smoothstep(z * 0.5f + 0.5f) * 2 - 1) * 0.7f; - var offset = 0.5f + 0.5f * MathF.Cos(MathF.PI + t * 4 * MathF.PI); - var xOffset = -0.2f + 0.4f * offset; - var yOffset = -0.7f + 0.2f * offset; - return new Vector3(-xOffset - legOffsetX, yOffset, zOffset); - } - Vector3 CreateArmTarget(float t) - { - var z = MathF.Cos(t * MathF.Tau); - var zOffset = (Smoothstep(z * 0.5f + 0.5f) * 2 - 1); - var offset = 0.5f + 0.5f * MathF.Cos(MathF.PI + t * 4 * MathF.PI); - var xOffset = -0.2f + 0.6f * offset; - var yOffset = 0.9f - 0.2f * offset; - return new Vector3(-xOffset - armOffsetX, yOffset, zOffset); - } - - double time; - double executionTime; - public unsafe override void Update(Window window, Camera camera, Input input, float dt) - { - time += 1 / 60f; - var hipsTarget = new Vector3(0, 0, 3 * (float)Math.Sin(time / 4)); - hipsControl.UpdateTarget(Simulation, hipsTarget); - const float stepDuration = 3.5f; - var scaledTime = time / stepDuration; - var t = (float)(scaledTime - Math.Floor(scaledTime)); - var tOffset = (t + 0.5f) % 1; - var leftFootLocalTarget = CreateLegTarget(t); - var rightFootLocalTarget = CreateLegTarget(tOffset); - rightFootLocalTarget.X *= -1; - leftFootControl.UpdateTarget(Simulation, hipsTarget + leftFootLocalTarget); - rightFootControl.UpdateTarget(Simulation, hipsTarget + rightFootLocalTarget); - - - var leftArmLocalTarget = CreateArmTarget(tOffset); - var rightArmLocalTarget = CreateArmTarget(t); - rightArmLocalTarget.X *= -1; - leftHandControl.UpdateTarget(Simulation, hipsTarget + leftArmLocalTarget); - rightHandControl.UpdateTarget(Simulation, hipsTarget + rightArmLocalTarget); - - //Record the latest motion state from the source dancer. - var sourceHandleBuffer = DancerBodyHandles.AsBuffer((DancerBodyHandles*)Unsafe.AsPointer(ref sourceBodyHandles)); - if (motionHistory.Count == historyLength * sourceHandleBuffer.Length) - for (int i = 0; i < sourceHandleBuffer.Length; ++i) - motionHistory.Dequeue(); - - for (int i = 0; i < sourceHandleBuffer.Length; ++i) - { - motionHistory.EnqueueUnsafely(Simulation.Bodies[sourceHandleBuffer[i]].MotionState); - } - var startTime = Stopwatch.GetTimestamp(); - looper.For(0, dancerHandles.Length, ExecuteDancer); - var endTime = Stopwatch.GetTimestamp(); - executionTime = (endTime - startTime) / (double)Stopwatch.Frequency; - base.Update(window, camera, input, dt); - } - - public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) - { - renderer.Shapes.AddInstances(dancerSimulations, ThreadDispatcher); - renderer.Lines.Extract(dancerSimulations, ThreadDispatcher); - - var resolution = renderer.Surface.Resolution; - renderer.TextBatcher.Write(text.Clear().Append("Cosmetic simulations, like cloth, often don't need to be in a game's main simulation."), new Vector2(16, resolution.Y - 144), 16, Vector3.One, font); - renderer.TextBatcher.Write(text.Clear().Append("Every background dancer in this demo has its own simulation. All dancers can be easily updated in parallel."), new Vector2(16, resolution.Y - 128), 16, Vector3.One, font); - renderer.TextBatcher.Write(text.Clear().Append("Dancers further from the main dancer use sparser cloth and disable self collision for extra performance."), new Vector2(16, resolution.Y - 112), 16, Vector3.One, font); - renderer.TextBatcher.Write(text.Clear().Append("Dancer count: ").Append(dancerHandles.Length), new Vector2(16, resolution.Y - 80), 16, Vector3.One, font); - renderer.TextBatcher.Write(text.Clear().Append("Total cloth body count: ").Append(dancerBodyCount), new Vector2(16, resolution.Y - 64), 16, Vector3.One, font); - renderer.TextBatcher.Write(text.Clear().Append("Total cloth constraint count: ").Append(dancerConstraintCount), new Vector2(16, resolution.Y - 48), 16, Vector3.One, font); - renderer.TextBatcher.Write(text.Clear().Append("Total dancer execution time (ms): ").Append(executionTime * 1000, 2), new Vector2(16, resolution.Y - 32), 16, Vector3.One, font); - renderer.TextBatcher.Write(text.Clear().Append("Amortized execution time per dancer (us): ").Append(executionTime * 1e6 / dancerHandles.Length, 1), new Vector2(16, resolution.Y - 16), 16, Vector3.One, font); - - base.Render(renderer, camera, input, text, font); - } - protected override void OnDispose() - { - //While the main simulation and pool is disposed by the Demo.cs Dispose function, the dancers have their own pools and need to be cleared. - //Note that we don't bother disposing the simulation here- all resources in the simulation were taken from the associated pool, and the simulation will not be used anymore. - //We can just clear the buffer pool and let the GC eat the simulation. - for (int i = 0; i < dancerSimulations.Length; ++i) - { - dancerSimulations[i].BufferPool.Clear(); - } - base.OnDispose(); - } - } -} diff --git a/Demos/Demos/Dancers/DancerDemo.cs b/Demos/Demos/Dancers/DancerDemo.cs new file mode 100644 index 000000000..e212a33db --- /dev/null +++ b/Demos/Demos/Dancers/DancerDemo.cs @@ -0,0 +1,196 @@ +using BepuPhysics; +using BepuPhysics.Collidables; +using BepuPhysics.CollisionDetection; +using BepuPhysics.Constraints; +using BepuUtilities; +using BepuUtilities.Memory; +using DemoContentLoader; +using DemoRenderer; +using DemoRenderer.UI; +using DemoUtilities; +using System; +using System.Numerics; +using System.Runtime.InteropServices; + +namespace Demos.Demos.Dancers +{ + /// + /// A bunch of background dancers struggle to keep up with the masterful purple prancer while wearing dresses made of out of balls connected by constraints. + /// + public class DancerDemo : Demo + { + //This demo relies on the DemoDancers to manage all the ragdolls and their simulations. + //All this demo needs to do is make a dress out of balls and drape it onto them. + DemoDancers dancers; + static BodyHandle[,] CreateDressBodyGrid(Vector3 position, int widthInNodes, float spacing, float bodyRadius, float massPerBody, + int instanceId, Simulation simulation, CollidableProperty filters) + { + var description = BodyDescription.CreateDynamic(QuaternionEx.Identity, new BodyInertia { InverseMass = 1f / massPerBody }, simulation.Shapes.Add(new Sphere(bodyRadius)), 0.01f); + BodyHandle[,] handles = new BodyHandle[widthInNodes, widthInNodes]; + var armHoleCenter = new Vector2(DemoDancers.ArmOffsetX + 0.065f, 0); + var armHoleRadius = 0.095f; + var armHoleRadiusSquared = armHoleRadius * armHoleRadius; + var halfWidth = widthInNodes * spacing / 2; + var halfWidthSquared = halfWidth * halfWidth; + var halfWidthOffset = new Vector2(halfWidth); + for (int rowIndex = 0; rowIndex < widthInNodes; ++rowIndex) + { + for (int columnIndex = 0; columnIndex < widthInNodes; ++columnIndex) + { + var horizontalPosition = new Vector2(columnIndex, rowIndex) * spacing - halfWidthOffset; + var distanceSquared0 = Vector2.DistanceSquared(horizontalPosition, armHoleCenter); + var distanceSquared1 = Vector2.DistanceSquared(horizontalPosition, -armHoleCenter); + var centerDistanceSquared = horizontalPosition.LengthSquared(); + if (distanceSquared0 < armHoleRadiusSquared || distanceSquared1 < armHoleRadiusSquared || centerDistanceSquared > halfWidthSquared) + { + //Too close to an arm or too far from the center, don't create any bodies here. + handles[rowIndex, columnIndex] = new BodyHandle { Value = -1 }; + } + else + { + description.Pose.Position = new Vector3(horizontalPosition.X, 0, horizontalPosition.Y) + position; + var handle = simulation.Bodies.Add(description); + handles[rowIndex, columnIndex] = handle; + if (filters != null) + filters.Allocate(handle) = new ClothCollisionFilter(rowIndex, columnIndex, instanceId); + } + } + } + return handles; + } + static void CreateDistanceConstraints(BodyHandle[,] bodyHandles, SpringSettings springSettings, Simulation simulation) + { + void CreateConstraintBetweenBodies(BodyHandle aHandle, BodyHandle bHandle) + { + //Only create a constraint if bodies on both sides of the pair actually exist. + //In this demo, we use -1 in the body handle slot to represent 'no body'. + if (aHandle.Value >= 0 && bHandle.Value >= 0) + { + var a = simulation.Bodies[aHandle]; + var b = simulation.Bodies[bHandle]; + //Note the use of a limit; the distance is allowed to go smaller. + //This helps stop the cloth from having unnatural rigidity. + var distance = Vector3.Distance(a.Pose.Position, b.Pose.Position); + simulation.Solver.Add(aHandle, bHandle, new CenterDistanceLimit(distance * 0.15f, distance, springSettings)); + } + } + for (int rowIndex = 0; rowIndex < bodyHandles.GetLength(0); ++rowIndex) + { + for (int columnIndex = 0; columnIndex < bodyHandles.GetLength(1) - 1; ++columnIndex) + { + CreateConstraintBetweenBodies(bodyHandles[rowIndex, columnIndex], bodyHandles[rowIndex, columnIndex + 1]); + } + } + for (int rowIndex = 0; rowIndex < bodyHandles.GetLength(0) - 1; ++rowIndex) + { + for (int columnIndex = 0; columnIndex < bodyHandles.GetLength(1); ++columnIndex) + { + CreateConstraintBetweenBodies(bodyHandles[rowIndex, columnIndex], bodyHandles[rowIndex + 1, columnIndex]); + } + } + for (int rowIndex = 0; rowIndex < bodyHandles.GetLength(0) - 1; ++rowIndex) + { + for (int columnIndex = 0; columnIndex < bodyHandles.GetLength(1) - 1; ++columnIndex) + { + CreateConstraintBetweenBodies(bodyHandles[rowIndex, columnIndex], bodyHandles[rowIndex + 1, columnIndex + 1]); + CreateConstraintBetweenBodies(bodyHandles[rowIndex, columnIndex + 1], bodyHandles[rowIndex + 1, columnIndex]); + } + } + } + + + static void TailorDress(Simulation simulation, CollidableProperty filters, DancerBodyHandles bodyHandles, int dancerIndex, float levelOfDetail) + { + //The demo uses lower resolution grids on dancers further away from the main dancer. + //This is a sorta-example of level of detail. In a 'real' use case, you'd probably want to transition between levels of detail dynamically as the camera moved around. + //That's a little trickier, but doable. Going low to high, for example, requires creating bodies at interpolated positions between existing bodies, while going to a lower level of detail removes them. + var targetDressDiameter = 2.6f; + var fullDetailWidthInBodies = 29; + float spacingAtFullDetail = targetDressDiameter / fullDetailWidthInBodies; + float bodyRadius = spacingAtFullDetail / 1.75f; + var scale = MathF.Pow(2, levelOfDetail); + var widthInBodies = (int)MathF.Ceiling(fullDetailWidthInBodies / scale); + var spacing = spacingAtFullDetail * scale; + var chest = simulation.Bodies[bodyHandles.Chest]; + ref var chestShape = ref simulation.Shapes.GetShape(chest.Collidable.Shape.Index); + var topOfChestHeight = chest.Pose.Position.Y + chestShape.Radius + bodyRadius; + var bodies = CreateDressBodyGrid(new Vector3(0, topOfChestHeight, 0) + DemoDancers.GetOffsetForDancer(dancerIndex), widthInBodies, spacing, bodyRadius, 0.01f, dancerIndex, simulation, filters); + //Create constraints that bind the cloth bodies closest to the chest, to the chest. This keeps the dress from sliding around. + //In the higher resolution simulations, the arm holes and cloth bodies can actually handle it with no help, but for lower levels of detail it can be useful. + //Also, it's very common to want to control how cloth sticks to a character. You could extend this approach to, for example, keep cloth near the body at the waist like a belt. + //This demo uses constraints to attach a subset of the cloth bodies to the chest. + //You could also either treat the bodies as kinematic and have them follow the body, or attach any constraints that would have involved the cloth body to the body instead. + //Using constraints gives you more options in configuration- the attachment doesn't have to be perfectly rigid. + //For the purposes of this demo, it's also simpler to just use some more constraints. + var midpoint = (widthInBodies * 0.5f - 0.5f); + var zRange = (chestShape.Radius * 0.65f) / spacing; + var xRange = (chestShape.Radius * 0.5f + chestShape.HalfLength) / spacing; + var minX = (int)MathF.Ceiling(midpoint - xRange); + var maxX = (int)(midpoint + xRange); + var minZ = (int)MathF.Ceiling(midpoint - zRange); + var maxZ = (int)(midpoint + zRange); + for (int z = minZ; z <= maxZ; ++z) + { + for (int x = minX; x <= maxX; ++x) + { + var clothNodeHandle = bodies[z, x]; + //When creating bodies, we set handles for bodies that don't exist to -1. + if (clothNodeHandle.Value >= 0) + { + var clothNodeBody = simulation.Bodies[clothNodeHandle]; + simulation.Solver.Add(chest.Handle, clothNodeBody.Handle, + new BallSocket + { + LocalOffsetA = QuaternionEx.Transform(clothNodeBody.Pose.Position - chest.Pose.Position, Quaternion.Conjugate(chest.Pose.Orientation)), + SpringSettings = new SpringSettings(30, 1) + }); + } + } + } + CreateDistanceConstraints(bodies, new SpringSettings(60, 1), simulation); + } + + + public unsafe override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(0, 2, 10); + camera.Yaw = 0; + camera.Pitch = 0; + + var collisionFilters = new CollidableProperty(); + //Note very high damping on the main ragdoll simulation; makes it easier to pose. + Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks { CollisionFilters = collisionFilters }, new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0), 0, 0), new SolveDescription(8, 1)); + + dancers = new DemoDancers().Initialize(Simulation, collisionFilters, ThreadDispatcher, BufferPool, TailorDress, new ClothCollisionFilter(0, 0, -1)); + + } + public unsafe override void Update(Window window, Camera camera, Input input, float dt) + { + dancers.UpdateTargets(Simulation); + base.Update(window, camera, input, dt); + } + + public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + { + renderer.Shapes.AddInstances(dancers.Simulations, ThreadDispatcher); + renderer.Lines.Extract(dancers.Simulations, ThreadDispatcher); + + var resolution = renderer.Surface.Resolution; + renderer.TextBatcher.Write(text.Clear().Append("Cosmetic simulations, like cloth, often don't need to be in a game's main simulation."), new Vector2(16, resolution.Y - 144), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Every background dancer in this demo has its own simulation. All dancers can be easily updated in parallel."), new Vector2(16, resolution.Y - 128), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Dancers further from the main dancer use sparser cloth and disable self collision for extra performance."), new Vector2(16, resolution.Y - 112), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Dancer count: ").Append(dancers.Handles.Length), new Vector2(16, resolution.Y - 80), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Total cloth body count: ").Append(dancers.BodyCount), new Vector2(16, resolution.Y - 64), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Total cloth constraint count: ").Append(dancers.ConstraintCount), new Vector2(16, resolution.Y - 48), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Total dancer execution time (ms): ").Append(dancers.ExecutionTime * 1000, 2), new Vector2(16, resolution.Y - 32), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Amortized execution time per dancer (us): ").Append(dancers.ExecutionTime * 1e6 / dancers.Handles.Length, 1), new Vector2(16, resolution.Y - 16), 16, Vector3.One, font); + + base.Render(renderer, camera, input, text, font); + } + protected override void OnDispose() + { + dancers.Dispose(BufferPool); + + } + } +} diff --git a/Demos/Demos/Dancers/DemoDancers.cs b/Demos/Demos/Dancers/DemoDancers.cs new file mode 100644 index 000000000..a23436436 --- /dev/null +++ b/Demos/Demos/Dancers/DemoDancers.cs @@ -0,0 +1,467 @@ +using BepuPhysics; +using BepuPhysics.Collidables; +using BepuPhysics.CollisionDetection; +using BepuPhysics.Constraints; +using BepuUtilities; +using BepuUtilities.Collections; +using BepuUtilities.Memory; +using DemoRenderer; +using System; +using System.Diagnostics; +using System.Numerics; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +namespace Demos.Demos.Dancers +{ + //This file contains all the shared helpers used in the DancerDemo and PlumpDancerDemo. + //It is responsible for creating the main dancer, its constraints, its control mechanisms, and making it dance. + //The DancerDemo and PlumpDancerDemo simply add whatever attachments they want on top of the dancers created by this infrastructure. + + [StructLayout(LayoutKind.Sequential, Pack = 4)] + public struct DancerBodyHandles + { + public BodyHandle Hips; + public BodyHandle Abdomen; + public BodyHandle Chest; + public BodyHandle Head; + public BodyHandle UpperLeftLeg; + public BodyHandle LowerLeftLeg; + public BodyHandle UpperRightLeg; + public BodyHandle LowerRightLeg; + public BodyHandle UpperLeftArm; + public BodyHandle LowerLeftArm; + public BodyHandle UpperRightArm; + public BodyHandle LowerRightArm; + + internal static unsafe Buffer AsBuffer(DancerBodyHandles* sourceBodyHandles) + { + return new Buffer(sourceBodyHandles, 12); + } + } + + /// + /// Controls one of the main dancer's limbs by yanking it around. + /// + struct DancerControl + { + public ConstraintHandle Servo; + public Vector3 LocalOffset; + public ServoSettings ServoSettings; + public SpringSettings SpringSettings; + + public DancerControl(Simulation simulation, BodyHandle body, Vector3 worldControlPoint, ServoSettings servoSettings, SpringSettings springSettings) + { + + ServoSettings = servoSettings; + SpringSettings = springSettings; + var pose = simulation.Bodies[body].Pose; + LocalOffset = QuaternionEx.Transform(worldControlPoint - pose.Position, Quaternion.Conjugate(pose.Orientation)); + Servo = simulation.Solver.Add(body, new OneBodyLinearServo { ServoSettings = servoSettings, SpringSettings = springSettings, LocalOffset = LocalOffset, Target = worldControlPoint }); + } + + public void UpdateTarget(Simulation simulation, Vector3 target) + { + simulation.Solver.ApplyDescription(Servo, new OneBodyLinearServo { ServoSettings = ServoSettings, SpringSettings = SpringSettings, LocalOffset = LocalOffset, Target = target }); + } + + } + + /// + /// Marks an as usable with the DemoDancers. + /// + /// Type of the callbacks to use. + /// Type of the callback filters to use. + public interface IDancerNarrowPhaseCallbacks where TCallbacks : INarrowPhaseCallbacks, IDancerNarrowPhaseCallbacks where TFilter : unmanaged + { + TCallbacks Create(CollidableProperty filters, PairMaterialProperties pairMaterialProperties, int minimumDistanceForSelfCollisions); + } + + /// + /// Used by and to coordinate lots of background dancers. + /// + public class DemoDancers + { + ParallelLooper looper; + public DancerBodyHandles SourceBodyHandles; + //Each dancer has its own simulation. The goal is to show a common use case for cosmetic physics- single threaded simulations that don't interact with the main simulation, running in parallel with other simulations. + //(The simulations and handles are kept separate just because the simulations are handed over as a group to the renderer to draw stuff.) + public Simulation[] Simulations; + //The body handles are cached in each simulation so the source states can be mapped onto each dancer. + public DancerBodyHandles[] Handles; + + public const float LegOffsetX = 0.135f; + public const float ArmOffsetX = 0.25f; + public const int DancerGridWidth = 16; + public const int DancerGridLength = 16; + public const int DancerCount = DancerGridWidth * DancerGridLength; + public const int HistoryLength = 256; + + + DancerControl hipsControl; + DancerControl leftFootControl; + DancerControl rightFootControl; + DancerControl leftHandControl; + DancerControl rightHandControl; + + public delegate void DressUpDancer(Simulation simulation, CollidableProperty filters, DancerBodyHandles bodyHandles, int dancerIndex, float levelOfDetail) where TCollisionFilter : unmanaged; + public DemoDancers Initialize(Simulation mainSimulation, CollidableProperty mainCollisionFilters, IThreadDispatcher threadDispatcher, BufferPool pool, + DressUpDancer dressUpDancer, TCollisionFilter filterForDancerBodies) + where TNarrowPhaseCallbacks : struct, INarrowPhaseCallbacks, IDancerNarrowPhaseCallbacks + where TCollisionFilter : unmanaged + { + looper = new ParallelLooper() { Dispatcher = threadDispatcher }; + + var hipsPosition = new Vector3(0, 0, 0); + var abdomenPosition = hipsPosition + new Vector3(0, 0.25f, 0); + var chestPosition = abdomenPosition + new Vector3(0, 0.25f, 0); + var headPosition = chestPosition + new Vector3(0, 0.4f, 0); + //It's helpful to have joint locations for the limbs so we can create capsules from the endpoints. + //There's not going to be a foot or hand body in this demo, since those aren't important for scooting a dress around. + var kneePosition = hipsPosition - new Vector3(0, 0.5f, 0); + var anklePosition = kneePosition - new Vector3(0, 0.5f, 0); + var elbowPosition = chestPosition + new Vector3(0, 0.39f, 0); + var wristPosition = elbowPosition + new Vector3(0, 0.39f, 0); + var armOffset = new Vector3(ArmOffsetX, 0, 0); + var legOffset = new Vector3(LegOffsetX, 0, 0); + const int groupIndex = 0; + + //Build the torso and head bodies. + RagdollDemo.GetCapsuleForLineSegment(hipsPosition - legOffset, hipsPosition + legOffset, 0.14f, out var hipShape, out _, out var hipOrientation); + SourceBodyHandles.Hips = mainSimulation.Bodies.Add(BodyDescription.CreateDynamic((hipsPosition, hipOrientation), hipShape.ComputeInertia(1), mainSimulation.Shapes.Add(hipShape), 0.01f)); + ref var hipsFilter = ref mainCollisionFilters.Allocate(SourceBodyHandles.Hips); + hipsFilter = new SubgroupCollisionFilter(groupIndex, 0); + + RagdollDemo.GetCapsuleForLineSegment(abdomenPosition - legOffset * 0.8f, abdomenPosition + legOffset * 0.8f, 0.13f, out var abdomenShape, out _, out var abdomenOrientation); + SourceBodyHandles.Abdomen = mainSimulation.Bodies.Add(BodyDescription.CreateDynamic((abdomenPosition, abdomenOrientation), abdomenShape.ComputeInertia(1), mainSimulation.Shapes.Add(abdomenShape), 0.01f)); + ref var abdomenFilter = ref mainCollisionFilters.Allocate(SourceBodyHandles.Abdomen); + abdomenFilter = new SubgroupCollisionFilter(groupIndex, 1); + + RagdollDemo.GetCapsuleForLineSegment(abdomenPosition - legOffset * 0.8f, abdomenPosition + legOffset * 0.8f, 0.165f, out var chestShape, out _, out var chestOrientation); + SourceBodyHandles.Chest = mainSimulation.Bodies.Add(BodyDescription.CreateDynamic((chestPosition, chestOrientation), chestShape.ComputeInertia(1), mainSimulation.Shapes.Add(chestShape), 0.01f)); + ref var chestFilter = ref mainCollisionFilters.Allocate(SourceBodyHandles.Chest); + chestFilter = new SubgroupCollisionFilter(groupIndex, 2); + + var headShape = new Sphere(0.17f); + SourceBodyHandles.Head = mainSimulation.Bodies.Add(BodyDescription.CreateDynamic(headPosition, headShape.ComputeInertia(1), mainSimulation.Shapes.Add(headShape), 1e-2f)); + ref var headFilter = ref mainCollisionFilters.Allocate(SourceBodyHandles.Head); + headFilter = new SubgroupCollisionFilter(groupIndex, 3); + + var springSettings = new SpringSettings(30, 1); + Connect(mainSimulation, SourceBodyHandles.Hips, SourceBodyHandles.Abdomen, 0.5f * (hipsPosition + abdomenPosition), springSettings, ref hipsFilter, ref abdomenFilter); + ConstrainOrientation(mainSimulation, SourceBodyHandles.Hips, SourceBodyHandles.Abdomen); + Connect(mainSimulation, SourceBodyHandles.Abdomen, SourceBodyHandles.Chest, 0.5f * (abdomenPosition + chestPosition), springSettings, ref abdomenFilter, ref chestFilter); + ConstrainOrientation(mainSimulation, SourceBodyHandles.Abdomen, SourceBodyHandles.Chest); + Connect(mainSimulation, SourceBodyHandles.Chest, SourceBodyHandles.Head, 0.5f * (chestPosition + headPosition), springSettings, ref chestFilter, ref headFilter); + ConstrainOrientation(mainSimulation, SourceBodyHandles.Chest, SourceBodyHandles.Head); + + //Create the legs. + RagdollDemo.GetCapsuleForLineSegment(hipsPosition, kneePosition, 0.11f, out var upperLegShape, out var upperLegPosition, out var upperLegOrientation); + RagdollDemo.GetCapsuleForLineSegment(kneePosition, anklePosition, 0.1f, out var lowerLegShape, out var lowerLegPosition, out var lowerLegOrientation); + var upperLegDescription = BodyDescription.CreateDynamic((upperLegPosition, upperLegOrientation), upperLegShape.ComputeInertia(1), mainSimulation.Shapes.Add(upperLegShape), 0.01f); + var lowerLegDescription = BodyDescription.CreateDynamic((lowerLegPosition, lowerLegOrientation), lowerLegShape.ComputeInertia(1), mainSimulation.Shapes.Add(lowerLegShape), 0.01f); + + upperLegDescription.Pose.Position -= legOffset; + lowerLegDescription.Pose.Position -= legOffset; + SourceBodyHandles.UpperLeftLeg = mainSimulation.Bodies.Add(upperLegDescription); + SourceBodyHandles.LowerLeftLeg = mainSimulation.Bodies.Add(lowerLegDescription); + upperLegDescription.Pose.Position += 2 * legOffset; + lowerLegDescription.Pose.Position += 2 * legOffset; + SourceBodyHandles.UpperRightLeg = mainSimulation.Bodies.Add(upperLegDescription); + SourceBodyHandles.LowerRightLeg = mainSimulation.Bodies.Add(lowerLegDescription); + + CreateLimb(mainSimulation, mainCollisionFilters, SourceBodyHandles.Hips, SourceBodyHandles.UpperLeftLeg, SourceBodyHandles.LowerLeftLeg, hipsPosition - legOffset, kneePosition - legOffset, springSettings, groupIndex, 4); + CreateLimb(mainSimulation, mainCollisionFilters, SourceBodyHandles.Hips, SourceBodyHandles.UpperRightLeg, SourceBodyHandles.LowerRightLeg, hipsPosition + legOffset, kneePosition + legOffset, springSettings, groupIndex, 6); + + //Create the arms. + RagdollDemo.GetCapsuleForLineSegment(chestPosition, elbowPosition, 0.08f, out var upperArmShape, out var upperArmPosition, out var upperArmOrientation); + RagdollDemo.GetCapsuleForLineSegment(elbowPosition, wristPosition, 0.075f, out var lowerArmShape, out var lowerArmPosition, out var lowerArmOrientation); + var upperArmDescription = BodyDescription.CreateDynamic((upperArmPosition, upperArmOrientation), upperArmShape.ComputeInertia(1), mainSimulation.Shapes.Add(upperArmShape), 0.01f); + var lowerArmDescription = BodyDescription.CreateDynamic((lowerArmPosition, lowerArmOrientation), lowerArmShape.ComputeInertia(1), mainSimulation.Shapes.Add(lowerArmShape), 0.01f); + + upperArmDescription.Pose.Position -= armOffset; + lowerArmDescription.Pose.Position -= armOffset; + SourceBodyHandles.UpperLeftArm = mainSimulation.Bodies.Add(upperArmDescription); + SourceBodyHandles.LowerLeftArm = mainSimulation.Bodies.Add(lowerArmDescription); + upperArmDescription.Pose.Position += 2 * armOffset; + lowerArmDescription.Pose.Position += 2 * armOffset; + SourceBodyHandles.UpperRightArm = mainSimulation.Bodies.Add(upperArmDescription); + SourceBodyHandles.LowerRightArm = mainSimulation.Bodies.Add(lowerArmDescription); + + CreateLimb(mainSimulation, mainCollisionFilters, SourceBodyHandles.Chest, SourceBodyHandles.UpperLeftArm, SourceBodyHandles.LowerLeftArm, chestPosition - armOffset, elbowPosition - armOffset, springSettings, groupIndex, 8); + CreateLimb(mainSimulation, mainCollisionFilters, SourceBodyHandles.Chest, SourceBodyHandles.UpperRightArm, SourceBodyHandles.LowerRightArm, chestPosition + armOffset, elbowPosition + armOffset, springSettings, groupIndex, 10); + + //Create controls. + hipsControl = new DancerControl(mainSimulation, SourceBodyHandles.Hips, hipsPosition, ServoSettings.Default, new SpringSettings(5, 1)); + mainSimulation.Solver.Add(SourceBodyHandles.Hips, new OneBodyAngularServo { ServoSettings = ServoSettings.Default, SpringSettings = new SpringSettings(30, 1), TargetOrientation = mainSimulation.Bodies[SourceBodyHandles.Hips].Pose.Orientation }); + + var limbServoSettings = ServoSettings.Default; + var limbSpringSettings = new SpringSettings(4, 1); + leftFootControl = new DancerControl(mainSimulation, SourceBodyHandles.LowerLeftLeg, anklePosition - legOffset, limbServoSettings, limbSpringSettings); + rightFootControl = new DancerControl(mainSimulation, SourceBodyHandles.LowerRightLeg, anklePosition + legOffset, limbServoSettings, limbSpringSettings); + leftHandControl = new DancerControl(mainSimulation, SourceBodyHandles.LowerLeftArm, wristPosition - armOffset, limbServoSettings, limbSpringSettings); + rightHandControl = new DancerControl(mainSimulation, SourceBodyHandles.LowerRightArm, wristPosition + armOffset, limbServoSettings, limbSpringSettings); + + + mainSimulation.Statics.Add(new(new Vector3(0, -1.24f, 0), mainSimulation.Shapes.Add(new Box(1000, 1, 1000)))); + + //All the background dancers read different historical motion states so that they're not just all doing the exact same thing. + //Keep the states in a queue. Each batch of 12 motion states is the state for a single frame. + MotionHistory = new QuickQueue(HistoryLength * 12, pool); + + Handles = new DancerBodyHandles[DancerCount]; + Simulations = new Simulation[DancerCount]; + static BodyHandle CreateCopyForDancer(Simulation sourceSimulation, BodyHandle sourceHandle, TypedIndex shapeIndexInTargetSimulation, Simulation targetSimulation, int dancerIndex, CollidableProperty filters, TCollisionFilter bodyFilter) + { + var description = sourceSimulation.Bodies.GetDescription(sourceHandle); + description.Pose.Position += GetOffsetForDancer(dancerIndex); + description.Collidable.Shape = shapeIndexInTargetSimulation; + description.LocalInertia = default; + var handle = targetSimulation.Bodies.Add(description); + filters.Allocate(handle) = bodyFilter; + return handle; + } + + for (int i = 0; i < Handles.Length; ++i) + { + ref var dancer = ref Handles[i]; + var dancerFilters = new CollidableProperty(); + //Distance from the main dancer is used to select clothing level of detail. This isn't dynamic based on camera motion, but shows the general idea. + //Since we don't have to worry about transitions, the level of detail is a continuous value here. + var distanceFromMainDancer = GetDistanceFromMainDancer(i); + var levelOfDetail = MathF.Max(0f, MathF.Min(1.5f, MathF.Log2(MathF.Max(1, distanceFromMainDancer) - 0.8f))); + //Note that we use a smaller allocation block size for dancer simulations. + //This demo is creating a *lot* of buffer pools just because that's the simplest way to keep things thread safe. + //If you wanted to reduce the amount of pool-induced memory overhead, you could consider sharing buffer pools between multiple simulations + //and making sure those simulations never run on multiple threads at the same time to avoid allocation related race conditions. + //Depending on the simulation, it could also be worth having multiple characters simulated in the same simulation even if you don't care about their interactions. + //For example, if you wanted to train a motorized ragdoll using reinforcement learning, it is likely that having multiple ragdolls in each simulation would improve hardware utilization. + //All narrow phase collision tests and constraint solves are vectorized over multiple pairs; having only one ragdoll would likely leave many bundles partially filled. + //In this demo, occupancy is less of a concern since there are a decent number of constraints associated with a single dancer. + + //If the required detail goes low enough, note that this demo disables cloth self collision to save some extra time. + //The ClothCallbacks specify a minimum distance required for self collision, and low detail (higher 'level of detail' values) results in MaxValue minimum distance. + var narrowPhaseCallbacks = default(TNarrowPhaseCallbacks).Create(dancerFilters, new PairMaterialProperties(0.4f, 20, new SpringSettings(120, 1)), levelOfDetail <= 0.5f ? 3 : int.MaxValue); + var dancerSimulation = Simulation.Create(new BufferPool(16384), narrowPhaseCallbacks, + new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4), + //To save some memory, initialize the dancer simulations with smaller starting sizes. For the higher level of detail simulations this could require some resizing. + //More precise estimates could be made without too much work, but the demo will keep it simple. + initialAllocationSizes: new SimulationAllocationSizes(128, 1, 1, 8, 512, 64, 8)); + dancer.Hips = CreateCopyForDancer(mainSimulation, SourceBodyHandles.Hips, dancerSimulation.Shapes.Add(hipShape), dancerSimulation, i, dancerFilters, filterForDancerBodies); + dancer.Abdomen = CreateCopyForDancer(mainSimulation, SourceBodyHandles.Abdomen, dancerSimulation.Shapes.Add(abdomenShape), dancerSimulation, i, dancerFilters, filterForDancerBodies); + dancer.Chest = CreateCopyForDancer(mainSimulation, SourceBodyHandles.Chest, dancerSimulation.Shapes.Add(chestShape), dancerSimulation, i, dancerFilters, filterForDancerBodies); + dancer.Head = CreateCopyForDancer(mainSimulation, SourceBodyHandles.Head, dancerSimulation.Shapes.Add(headShape), dancerSimulation, i, dancerFilters, filterForDancerBodies); + + var upperLegShapeInTarget = dancerSimulation.Shapes.Add(upperLegShape); + var lowerLegShapeInTarget = dancerSimulation.Shapes.Add(lowerLegShape); + dancer.UpperLeftLeg = CreateCopyForDancer(mainSimulation, SourceBodyHandles.UpperLeftLeg, upperLegShapeInTarget, dancerSimulation, i, dancerFilters, filterForDancerBodies); + dancer.LowerLeftLeg = CreateCopyForDancer(mainSimulation, SourceBodyHandles.LowerLeftLeg, lowerLegShapeInTarget, dancerSimulation, i, dancerFilters, filterForDancerBodies); + dancer.UpperRightLeg = CreateCopyForDancer(mainSimulation, SourceBodyHandles.UpperRightLeg, upperLegShapeInTarget, dancerSimulation, i, dancerFilters, filterForDancerBodies); + dancer.LowerRightLeg = CreateCopyForDancer(mainSimulation, SourceBodyHandles.LowerRightLeg, lowerLegShapeInTarget, dancerSimulation, i, dancerFilters, filterForDancerBodies); + + var upperArmShapeInTarget = dancerSimulation.Shapes.Add(upperArmShape); + var lowerArmShapeInTarget = dancerSimulation.Shapes.Add(lowerArmShape); + dancer.UpperLeftArm = CreateCopyForDancer(mainSimulation, SourceBodyHandles.UpperLeftArm, upperArmShapeInTarget, dancerSimulation, i, dancerFilters, filterForDancerBodies); + dancer.LowerLeftArm = CreateCopyForDancer(mainSimulation, SourceBodyHandles.LowerLeftArm, lowerArmShapeInTarget, dancerSimulation, i, dancerFilters, filterForDancerBodies); + dancer.UpperRightArm = CreateCopyForDancer(mainSimulation, SourceBodyHandles.UpperRightArm, upperArmShapeInTarget, dancerSimulation, i, dancerFilters, filterForDancerBodies); + dancer.LowerRightArm = CreateCopyForDancer(mainSimulation, SourceBodyHandles.LowerRightArm, lowerArmShapeInTarget, dancerSimulation, i, dancerFilters, filterForDancerBodies); + + dressUpDancer(dancerSimulation, dancerFilters, dancer, i, levelOfDetail); + BodyCount += dancerSimulation.Bodies.ActiveSet.Count; + ConstraintCount += dancerSimulation.Solver.CountConstraints(); + Simulations[i] = dancerSimulation; + } + return this; + } + public int BodyCount; + public int ConstraintCount; + + + static void Connect(Simulation simulation, BodyHandle a, BodyHandle b, Vector3 jointLocation, SpringSettings springSettings, ref SubgroupCollisionFilter filterA, ref SubgroupCollisionFilter filterB) + { + var poseA = simulation.Bodies[a].Pose; + var poseB = simulation.Bodies[b].Pose; + simulation.Solver.Add(a, b, + new BallSocket + { + LocalOffsetA = QuaternionEx.Transform(jointLocation - poseA.Position, Quaternion.Conjugate(poseA.Orientation)), + LocalOffsetB = QuaternionEx.Transform(jointLocation - poseB.Position, Quaternion.Conjugate(poseB.Orientation)), + SpringSettings = springSettings + }); + SubgroupCollisionFilter.DisableCollision(ref filterA, ref filterB); + } + + static void CreateLimb(Simulation simulation, CollidableProperty collisionFilters, + BodyHandle body, BodyHandle upperLimb, BodyHandle lowerLimb, + Vector3 bodyToUpperJointLocation, Vector3 upperToLowerJointLocation, SpringSettings springSettings, + int groupIndex, int limbSubgroupIndexStart) + { + ref var bodyFilter = ref collisionFilters[body]; + ref var upperFilter = ref collisionFilters.Allocate(upperLimb); + ref var lowerFilter = ref collisionFilters.Allocate(lowerLimb); + upperFilter = new SubgroupCollisionFilter(groupIndex, limbSubgroupIndexStart); + lowerFilter = new SubgroupCollisionFilter(groupIndex, limbSubgroupIndexStart + 1); + Connect(simulation, body, upperLimb, bodyToUpperJointLocation, springSettings, ref bodyFilter, ref upperFilter); + Connect(simulation, upperLimb, lowerLimb, upperToLowerJointLocation, springSettings, ref upperFilter, ref lowerFilter); + //While this demo isn't trying to make a full ragdoll, it's useful to stop the joints from doing obviously gross stuff. + var bodyPose = simulation.Bodies[body].Pose; + var upperPose = simulation.Bodies[upperLimb].Pose; + var lowerPose = simulation.Bodies[lowerLimb].Pose; + //Prevent the hip from spinning 360 degrees. + simulation.Solver.Add(body, upperLimb, new TwistServo + { + LocalBasisA = QuaternionEx.Concatenate(RagdollDemo.CreateBasis(new Vector3(0, -1, 0), new Vector3(0, 0, 1)), Quaternion.Conjugate(bodyPose.Orientation)), + LocalBasisB = QuaternionEx.Concatenate(RagdollDemo.CreateBasis(new Vector3(0, -1, 0), new Vector3(0, 0, 1)), Quaternion.Conjugate(upperPose.Orientation)), + ServoSettings = ServoSettings.Default, + SpringSettings = new SpringSettings(30, 1) + }); + //Stop knee from flopping every which way. + simulation.Solver.Add(upperLimb, lowerLimb, new AngularHinge + { + LocalHingeAxisA = QuaternionEx.Transform(Vector3.UnitX, Quaternion.Conjugate(upperPose.Orientation)), + LocalHingeAxisB = QuaternionEx.Transform(Vector3.UnitX, Quaternion.Conjugate(lowerPose.Orientation)), + SpringSettings = new SpringSettings(30, 1) + }); + //Prevent knee hyperextension. + var swingLimit = new SwingLimit + { + AxisLocalA = QuaternionEx.Transform(new Vector3(0, 0, 1), QuaternionEx.Conjugate(upperPose.Orientation)), + AxisLocalB = QuaternionEx.Transform(new Vector3(0, 1, 0), QuaternionEx.Conjugate(lowerPose.Orientation)), + MaximumSwingAngle = MathF.PI * 0.4f, + SpringSettings = new SpringSettings(15, 1), + }; + simulation.Solver.Add(upperLimb, lowerLimb, swingLimit); + + } + + static void ConstrainOrientation(Simulation simulation, BodyHandle a, BodyHandle b) + { + simulation.Solver.Add(a, b, new AngularServo + { + ServoSettings = ServoSettings.Default, + SpringSettings = new SpringSettings(6, 1), + TargetRelativeRotationLocalA = Quaternion.Concatenate(simulation.Bodies[b].Pose.Orientation, Quaternion.Conjugate(simulation.Bodies[a].Pose.Orientation)) + }); + } + + + static (int columnIndex, int rowIndex) GetRowAndColumnForDancer(int dancerIndex) + { + var rowIndex = dancerIndex / DancerGridWidth; + return (dancerIndex - rowIndex * DancerGridWidth, rowIndex); + } + + public QuickQueue MotionHistory; + public static Vector3 GetOffsetForDancer(int i) + { + const float spacing = 2; + var (columnIndex, rowIndex) = GetRowAndColumnForDancer(i); + return new Vector3(DancerGridWidth * spacing / -2 + (columnIndex + 0.5f) * spacing, 0, -2 + rowIndex * -spacing); + } + + public static float GetDistanceFromMainDancer(int dancerIndex) + { + var (columnIndex, rowIndex) = GetRowAndColumnForDancer(dancerIndex); + var offsetX = columnIndex - (DancerGridWidth / 2 - 0.5f); + return MathF.Sqrt(offsetX * offsetX + rowIndex * rowIndex); + } + + float Smoothstep(float v) + { + var v2 = v * v; + return 3 * v2 - 2 * v2 * v; + } + + Vector3 CreateLegTarget(float t) + { + var z = MathF.Cos(t * MathF.Tau); + var zOffset = (Smoothstep(z * 0.5f + 0.5f) * 2 - 1) * 0.7f; + var offset = 0.5f + 0.5f * MathF.Cos(MathF.PI + t * 4 * MathF.PI); + var xOffset = -0.2f + 0.4f * offset; + var yOffset = -0.7f + 0.2f * offset; + return new Vector3(-xOffset - LegOffsetX, yOffset, zOffset); + } + Vector3 CreateArmTarget(float t) + { + var z = MathF.Cos(t * MathF.Tau); + var zOffset = (Smoothstep(z * 0.5f + 0.5f) * 2 - 1); + var offset = 0.5f + 0.5f * MathF.Cos(MathF.PI + t * 4 * MathF.PI); + var xOffset = -0.2f + 0.6f * offset; + var yOffset = 0.9f - 0.2f * offset; + return new Vector3(-xOffset - ArmOffsetX, yOffset, zOffset); + } + unsafe void ExecuteDancer(int dancerIndex, int workerIndex) + { + //Copy historical motion states to the dancers. + ref var dancerHandles = ref Handles[dancerIndex]; + var dancerSimulation = Simulations[dancerIndex]; + var sourceHandleBuffer = DancerBodyHandles.AsBuffer((DancerBodyHandles*)Unsafe.AsPointer(ref SourceBodyHandles)); + //Delay is greater for the dancers that are further away, plus a little randomized component to desynchronize them. + var historicalStateStartIndex = MotionHistory.Count - sourceHandleBuffer.Length * ((int)GetDistanceFromMainDancer(dancerIndex) * 8 + 1 + (HashHelper.Rehash(dancerIndex) & 0xF)); + if (historicalStateStartIndex < 0) + historicalStateStartIndex = 0; + var targetHandleBuffer = DancerBodyHandles.AsBuffer((DancerBodyHandles*)Unsafe.AsPointer(ref dancerHandles)); + for (int j = 0; j < sourceHandleBuffer.Length; ++j) + { + ref var targetMotionState = ref dancerSimulation.Bodies[targetHandleBuffer[j]].MotionState; + targetMotionState = MotionHistory[historicalStateStartIndex + j]; + targetMotionState.Pose.Position += GetOffsetForDancer(dancerIndex); + } + //Update the simulation for the dancer. + dancerSimulation.Timestep(Demo.TimestepDuration); + } + + double time; + public double ExecutionTime; + public unsafe void UpdateTargets(Simulation mainSimulation) + { + //Using a fixed interval here, matching the time used in the Demo. + time += Demo.TimestepDuration; + var hipsTarget = new Vector3(0, 0, 3 * (float)Math.Sin(time / 4)); + hipsControl.UpdateTarget(mainSimulation, hipsTarget); + const float stepDuration = 3.5f; + var scaledTime = time / stepDuration; + var t = (float)(scaledTime - Math.Floor(scaledTime)); + var tOffset = (t + 0.5f) % 1; + var leftFootLocalTarget = CreateLegTarget(t); + var rightFootLocalTarget = CreateLegTarget(tOffset); + rightFootLocalTarget.X *= -1; + leftFootControl.UpdateTarget(mainSimulation, hipsTarget + leftFootLocalTarget); + rightFootControl.UpdateTarget(mainSimulation, hipsTarget + rightFootLocalTarget); + + + var leftArmLocalTarget = CreateArmTarget(tOffset); + var rightArmLocalTarget = CreateArmTarget(t); + rightArmLocalTarget.X *= -1; + leftHandControl.UpdateTarget(mainSimulation, hipsTarget + leftArmLocalTarget); + rightHandControl.UpdateTarget(mainSimulation, hipsTarget + rightArmLocalTarget); + + //Record the latest motion state from the source dancer. + var sourceHandleBuffer = DancerBodyHandles.AsBuffer((DancerBodyHandles*)Unsafe.AsPointer(ref SourceBodyHandles)); + if (MotionHistory.Count == HistoryLength * sourceHandleBuffer.Length) + for (int i = 0; i < sourceHandleBuffer.Length; ++i) + MotionHistory.Dequeue(); + + for (int i = 0; i < sourceHandleBuffer.Length; ++i) + { + MotionHistory.EnqueueUnsafely(mainSimulation.Bodies[sourceHandleBuffer[i]].MotionState); + } + var startTime = Stopwatch.GetTimestamp(); + looper.For(0, Handles.Length, ExecuteDancer); + var endTime = Stopwatch.GetTimestamp(); + ExecutionTime = (endTime - startTime) / (double)Stopwatch.Frequency; + } + + public void Dispose(BufferPool pool) + { + //While the main simulation and pool is disposed by the Demo.cs Dispose function, the dancers have their own pools and need to be cleared. + //Note that we don't bother disposing the simulation here- all resources in the simulation were taken from the associated pool, and the simulation will not be used anymore. + //We can just clear the buffer pool and let the GC eat the simulation. + for (int i = 0; i < Simulations.Length; ++i) + { + Simulations[i].BufferPool.Clear(); + } + MotionHistory.Dispose(pool); + } + } +} diff --git a/Demos/Demos/RayCastingDemo.cs b/Demos/Demos/RayCastingDemo.cs index b666063ed..6ceaddcf0 100644 --- a/Demos/Demos/RayCastingDemo.cs +++ b/Demos/Demos/RayCastingDemo.cs @@ -432,7 +432,7 @@ public unsafe override void Update(Window window, Camera camera, Input input, fl frameCount = 0; if (shouldRotate) { - rotation += (MathF.PI * 1e-2f * (1 / 60f)) % (2 * MathF.PI); + rotation += (MathF.PI * 1e-2f * (TimestepDuration)) % (2 * MathF.PI); } if (shouldCycle) { diff --git a/Demos/Demos/Sponsors/SponsorDemo.cs b/Demos/Demos/Sponsors/SponsorDemo.cs index bbafb435a..ee7c877b6 100644 --- a/Demos/Demos/Sponsors/SponsorDemo.cs +++ b/Demos/Demos/Sponsors/SponsorDemo.cs @@ -228,17 +228,16 @@ float DrawSponsors(string groupTitle, List sponsors, Vector2 position, R double simulationTime; public override void Update(Window window, Camera camera, Input input, float dt) { - const float simulationDt = 1 / 60f; - Simulation.Timestep(simulationDt, ThreadDispatcher); + Simulation.Timestep(TimestepDuration, ThreadDispatcher); for (int i = 0; i < newts.Count; ++i) { - newts[i].Update(Simulation, simulationTime, 0, newtArenaMin, newtArenaMax, random, 1f / simulationDt); + newts[i].Update(Simulation, simulationTime, 0, newtArenaMin, newtArenaMax, random, 1f / TimestepDuration); } for (int i = 0; i < characterAIs.Count; ++i) { characterAIs[i].Update(characterControllers, Simulation, ref newts, newtArenaMin, newtArenaMax, random); } - simulationTime += simulationDt; + simulationTime += TimestepDuration; realTime += dt; } public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) diff --git a/Demos/Demos/SweepDemo.cs b/Demos/Demos/SweepDemo.cs index 178061fc8..59bcf5f65 100644 --- a/Demos/Demos/SweepDemo.cs +++ b/Demos/Demos/SweepDemo.cs @@ -199,7 +199,7 @@ public override void Update(Window window, Camera camera, Input input, float dt) base.Update(window, camera, input, dt); if (!input.WasDown(OpenTK.Input.Key.P)) - animationT = (animationT + 1 / 60f) % (128); + animationT = (animationT + TimestepDuration) % (128); } float animationT; diff --git a/Demos/SpecializedTests/BatchedCollisionTests.cs b/Demos/SpecializedTests/BatchedCollisionTests.cs index 1d361b564..4ec03e9f1 100644 --- a/Demos/SpecializedTests/BatchedCollisionTests.cs +++ b/Demos/SpecializedTests/BatchedCollisionTests.cs @@ -51,7 +51,7 @@ static void TestPair(ref TA a, ref TB b, ref Buffer posesA, r ref TestCollisionCallbacks callbacks, BufferPool pool, Shapes shapes, CollisionTaskRegistry registry, int iterationCount) where TA : struct, IShape where TB : struct, IShape { - var batcher = new CollisionBatcher(pool, shapes, registry, 1 / 60f, callbacks); + var batcher = new CollisionBatcher(pool, shapes, registry, Demo.TimestepDuration, callbacks); for (int i = 0; i < iterationCount; ++i) { ref var poseA = ref posesA[i]; diff --git a/Demos/SpecializedTests/DeterminismTest.cs b/Demos/SpecializedTests/DeterminismTest.cs index 5f1bcc684..ffcc30da5 100644 --- a/Demos/SpecializedTests/DeterminismTest.cs +++ b/Demos/SpecializedTests/DeterminismTest.cs @@ -19,7 +19,7 @@ static Dictionary ExecuteSimulation(ContentArchive content, in Console.Write("Completed frames: "); for (int i = 0; i < frameCount; ++i) { - demo.Update(null, null, null, 1 / 60f); + demo.Update(null, null, null, Demo.TimestepDuration); //InvasiveHashDiagnostics.Instance.MoveToNextHashFrame(); if ((i + 1) % 32 == 0) Console.Write($"{i + 1}, "); diff --git a/Demos/SpecializedTests/HeadlessDemo.cs b/Demos/SpecializedTests/HeadlessDemo.cs index ba3445f73..d9932db95 100644 --- a/Demos/SpecializedTests/HeadlessDemo.cs +++ b/Demos/SpecializedTests/HeadlessDemo.cs @@ -21,7 +21,7 @@ static class HeadlessTest GC.Collect(3, GCCollectionMode.Forced, true, true); for (int i = 0; i < warmUpFrames; ++i) { - demo.Update(null, null, null, 1 / 60f); + demo.Update(null, null, null, Demo.TimestepDuration); } Console.WriteLine($"Warmup {runIndex} complete"); double time = 0; @@ -31,7 +31,7 @@ static class HeadlessTest { //CacheBlaster.Blast(); var start = Stopwatch.GetTimestamp(); - demo.Update(null, null, null, 1 / 60f); + demo.Update(null, null, null, Demo.TimestepDuration); var end = Stopwatch.GetTimestamp(); time += (end - start) / (double)Stopwatch.Frequency; if (i % 32 == 0) diff --git a/Demos/SpecializedTests/Media/ColosseumVideoDemo.cs b/Demos/SpecializedTests/Media/ColosseumVideoDemo.cs index a0e612f72..58b45a6cc 100644 --- a/Demos/SpecializedTests/Media/ColosseumVideoDemo.cs +++ b/Demos/SpecializedTests/Media/ColosseumVideoDemo.cs @@ -128,7 +128,7 @@ public override void Update(Window window, Camera camera, Input input, float dt) } if (characterActive) { - character.UpdateCharacterGoals(input, camera, 1 / 60f); + character.UpdateCharacterGoals(input, camera, Demo.TimestepDuration); } if (input.WasPushed(Key.Z)) diff --git a/Demos/SpecializedTests/SolverBatchTestDemo.cs b/Demos/SpecializedTests/SolverBatchTestDemo.cs index 5bd8bb192..eca172de3 100644 --- a/Demos/SpecializedTests/SolverBatchTestDemo.cs +++ b/Demos/SpecializedTests/SolverBatchTestDemo.cs @@ -98,7 +98,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) public override void Update(Window window, Camera camera, Input input, float dt) { var bigBall = new BodyReference(bigBallHandle, Simulation.Bodies); - timeAccumulator += 1 / 60f; + timeAccumulator += TimestepDuration; if (timeAccumulator > MathF.PI * 128) timeAccumulator -= MathF.PI * 128; if (!bigBall.Awake) From 547d3e28c5181b48bc1b077ff89f3f8d06ac8818 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 18 Jan 2022 19:47:02 -0600 Subject: [PATCH 381/947] Dancers with a highly peculiar distribution of fat take the stage. --- Demos/DemoHarness.cs | 2 +- Demos/DemoSet.cs | 1 + Demos/Demos/Dancers/DancerDemo.cs | 7 +- Demos/Demos/Dancers/DemoDancers.cs | 78 +++--- Demos/Demos/Dancers/PlumpDancerDemo.cs | 262 ++++++++++++++++++ Demos/Demos/NewtDemo.cs | 27 +- Demos/SpecializedTests/Media/NewtVideoDemo.cs | 2 +- 7 files changed, 333 insertions(+), 46 deletions(-) create mode 100644 Demos/Demos/Dancers/PlumpDancerDemo.cs diff --git a/Demos/DemoHarness.cs b/Demos/DemoHarness.cs index dd4bf12e4..5396c016a 100644 --- a/Demos/DemoHarness.cs +++ b/Demos/DemoHarness.cs @@ -303,7 +303,7 @@ public void Update(float dt) input.MouseLocked = false; } ++frameCount; - if (!controls.SlowTimesteps.IsDown(input) || frameCount % 20 == 0) + if (!controls.SlowTimesteps.IsDown(input) || frameCount % 120 == 0) { demo.Update(window, camera, input, dt); } diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index 48f6f586f..5ab5b622a 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -46,6 +46,7 @@ struct Option public DemoSet() { + AddOption(); AddOption(); AddOption(); AddOption(); diff --git a/Demos/Demos/Dancers/DancerDemo.cs b/Demos/Demos/Dancers/DancerDemo.cs index e212a33db..5501d1456 100644 --- a/Demos/Demos/Dancers/DancerDemo.cs +++ b/Demos/Demos/Dancers/DancerDemo.cs @@ -16,6 +16,7 @@ namespace Demos.Demos.Dancers { /// /// A bunch of background dancers struggle to keep up with the masterful purple prancer while wearing dresses made of out of balls connected by constraints. + /// Combined with the implementation, this provides a starting point for cosmetic cloth attached to characters. /// public class DancerDemo : Demo { @@ -99,7 +100,7 @@ void CreateConstraintBetweenBodies(BodyHandle aHandle, BodyHandle bHandle) } - static void TailorDress(Simulation simulation, CollidableProperty filters, DancerBodyHandles bodyHandles, int dancerIndex, float levelOfDetail) + static void TailorDress(Simulation simulation, CollidableProperty filters, DancerBodyHandles bodyHandles, int dancerIndex, int dancerGridWidth, float levelOfDetail) { //The demo uses lower resolution grids on dancers further away from the main dancer. //This is a sorta-example of level of detail. In a 'real' use case, you'd probably want to transition between levels of detail dynamically as the camera moved around. @@ -114,7 +115,7 @@ static void TailorDress(Simulation simulation, CollidableProperty(chest.Collidable.Shape.Index); var topOfChestHeight = chest.Pose.Position.Y + chestShape.Radius + bodyRadius; - var bodies = CreateDressBodyGrid(new Vector3(0, topOfChestHeight, 0) + DemoDancers.GetOffsetForDancer(dancerIndex), widthInBodies, spacing, bodyRadius, 0.01f, dancerIndex, simulation, filters); + var bodies = CreateDressBodyGrid(new Vector3(0, topOfChestHeight, 0) + DemoDancers.GetOffsetForDancer(dancerIndex, dancerGridWidth), widthInBodies, spacing, bodyRadius, 0.01f, dancerIndex, simulation, filters); //Create constraints that bind the cloth bodies closest to the chest, to the chest. This keeps the dress from sliding around. //In the higher resolution simulations, the arm holes and cloth bodies can actually handle it with no help, but for lower levels of detail it can be useful. //Also, it's very common to want to control how cloth sticks to a character. You could extend this approach to, for example, keep cloth near the body at the waist like a belt. @@ -161,7 +162,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //Note very high damping on the main ragdoll simulation; makes it easier to pose. Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks { CollisionFilters = collisionFilters }, new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0), 0, 0), new SolveDescription(8, 1)); - dancers = new DemoDancers().Initialize(Simulation, collisionFilters, ThreadDispatcher, BufferPool, TailorDress, new ClothCollisionFilter(0, 0, -1)); + dancers = new DemoDancers().Initialize(16, 16, Simulation, collisionFilters, ThreadDispatcher, BufferPool, TailorDress, new ClothCollisionFilter(0, 0, -1)); } public unsafe override void Update(Window window, Camera camera, Input input, float dt) diff --git a/Demos/Demos/Dancers/DemoDancers.cs b/Demos/Demos/Dancers/DemoDancers.cs index a23436436..bda0169ae 100644 --- a/Demos/Demos/Dancers/DemoDancers.cs +++ b/Demos/Demos/Dancers/DemoDancers.cs @@ -21,10 +21,6 @@ namespace Demos.Demos.Dancers [StructLayout(LayoutKind.Sequential, Pack = 4)] public struct DancerBodyHandles { - public BodyHandle Hips; - public BodyHandle Abdomen; - public BodyHandle Chest; - public BodyHandle Head; public BodyHandle UpperLeftLeg; public BodyHandle LowerLeftLeg; public BodyHandle UpperRightLeg; @@ -33,6 +29,10 @@ public struct DancerBodyHandles public BodyHandle LowerLeftArm; public BodyHandle UpperRightArm; public BodyHandle LowerRightArm; + public BodyHandle Hips; + public BodyHandle Abdomen; + public BodyHandle Chest; + public BodyHandle Head; internal static unsafe Buffer AsBuffer(DancerBodyHandles* sourceBodyHandles) { @@ -92,10 +92,9 @@ public class DemoDancers public const float LegOffsetX = 0.135f; public const float ArmOffsetX = 0.25f; - public const int DancerGridWidth = 16; - public const int DancerGridLength = 16; - public const int DancerCount = DancerGridWidth * DancerGridLength; public const int HistoryLength = 256; + public int DancerGridWidth = 16; + public int DancerGridLength = 16; DancerControl hipsControl; @@ -104,13 +103,15 @@ public class DemoDancers DancerControl leftHandControl; DancerControl rightHandControl; - public delegate void DressUpDancer(Simulation simulation, CollidableProperty filters, DancerBodyHandles bodyHandles, int dancerIndex, float levelOfDetail) where TCollisionFilter : unmanaged; - public DemoDancers Initialize(Simulation mainSimulation, CollidableProperty mainCollisionFilters, IThreadDispatcher threadDispatcher, BufferPool pool, + public delegate void DressUpDancer(Simulation simulation, CollidableProperty filters, DancerBodyHandles bodyHandles, int dancerIndex, int dancerGridWidth, float levelOfDetail) where TCollisionFilter : unmanaged; + public DemoDancers Initialize(int dancerGridWidth, int dancerGridLength, Simulation mainSimulation, CollidableProperty mainCollisionFilters, IThreadDispatcher threadDispatcher, BufferPool pool, DressUpDancer dressUpDancer, TCollisionFilter filterForDancerBodies) where TNarrowPhaseCallbacks : struct, INarrowPhaseCallbacks, IDancerNarrowPhaseCallbacks where TCollisionFilter : unmanaged { looper = new ParallelLooper() { Dispatcher = threadDispatcher }; + this.DancerGridWidth = dancerGridWidth; + this.DancerGridLength = dancerGridLength; var hipsPosition = new Vector3(0, 0, 0); var abdomenPosition = hipsPosition + new Vector3(0, 0.25f, 0); @@ -209,14 +210,15 @@ public DemoDancers Initialize(Simulatio //Keep the states in a queue. Each batch of 12 motion states is the state for a single frame. MotionHistory = new QuickQueue(HistoryLength * 12, pool); - Handles = new DancerBodyHandles[DancerCount]; - Simulations = new Simulation[DancerCount]; - static BodyHandle CreateCopyForDancer(Simulation sourceSimulation, BodyHandle sourceHandle, TypedIndex shapeIndexInTargetSimulation, Simulation targetSimulation, int dancerIndex, CollidableProperty filters, TCollisionFilter bodyFilter) + Handles = new DancerBodyHandles[dancerGridWidth * dancerGridLength]; + Simulations = new Simulation[dancerGridWidth * dancerGridLength]; + static BodyHandle CreateCopyForDancer(Simulation sourceSimulation, BodyHandle sourceHandle, TypedIndex shapeIndexInTargetSimulation, Simulation targetSimulation, int dancerIndex, int dancerGridWidth, CollidableProperty filters, TCollisionFilter bodyFilter) { var description = sourceSimulation.Bodies.GetDescription(sourceHandle); - description.Pose.Position += GetOffsetForDancer(dancerIndex); + description.Pose.Position += GetOffsetForDancer(dancerIndex, dancerGridWidth); description.Collidable.Shape = shapeIndexInTargetSimulation; description.LocalInertia = default; + description.Activity.SleepThreshold = -1; var handle = targetSimulation.Bodies.Add(description); filters.Allocate(handle) = bodyFilter; return handle; @@ -228,7 +230,7 @@ static BodyHandle CreateCopyForDancer(Simulation sourceSimulation, BodyHandle so var dancerFilters = new CollidableProperty(); //Distance from the main dancer is used to select clothing level of detail. This isn't dynamic based on camera motion, but shows the general idea. //Since we don't have to worry about transitions, the level of detail is a continuous value here. - var distanceFromMainDancer = GetDistanceFromMainDancer(i); + var distanceFromMainDancer = GetDistanceFromMainDancer(i, dancerGridWidth); var levelOfDetail = MathF.Max(0f, MathF.Min(1.5f, MathF.Log2(MathF.Max(1, distanceFromMainDancer) - 0.8f))); //Note that we use a smaller allocation block size for dancer simulations. //This demo is creating a *lot* of buffer pools just because that's the simplest way to keep things thread safe. @@ -247,26 +249,26 @@ static BodyHandle CreateCopyForDancer(Simulation sourceSimulation, BodyHandle so //To save some memory, initialize the dancer simulations with smaller starting sizes. For the higher level of detail simulations this could require some resizing. //More precise estimates could be made without too much work, but the demo will keep it simple. initialAllocationSizes: new SimulationAllocationSizes(128, 1, 1, 8, 512, 64, 8)); - dancer.Hips = CreateCopyForDancer(mainSimulation, SourceBodyHandles.Hips, dancerSimulation.Shapes.Add(hipShape), dancerSimulation, i, dancerFilters, filterForDancerBodies); - dancer.Abdomen = CreateCopyForDancer(mainSimulation, SourceBodyHandles.Abdomen, dancerSimulation.Shapes.Add(abdomenShape), dancerSimulation, i, dancerFilters, filterForDancerBodies); - dancer.Chest = CreateCopyForDancer(mainSimulation, SourceBodyHandles.Chest, dancerSimulation.Shapes.Add(chestShape), dancerSimulation, i, dancerFilters, filterForDancerBodies); - dancer.Head = CreateCopyForDancer(mainSimulation, SourceBodyHandles.Head, dancerSimulation.Shapes.Add(headShape), dancerSimulation, i, dancerFilters, filterForDancerBodies); + dancer.Hips = CreateCopyForDancer(mainSimulation, SourceBodyHandles.Hips, dancerSimulation.Shapes.Add(hipShape), dancerSimulation, i, dancerGridWidth, dancerFilters, filterForDancerBodies); + dancer.Abdomen = CreateCopyForDancer(mainSimulation, SourceBodyHandles.Abdomen, dancerSimulation.Shapes.Add(abdomenShape), dancerSimulation, i, dancerGridWidth, dancerFilters, filterForDancerBodies); + dancer.Chest = CreateCopyForDancer(mainSimulation, SourceBodyHandles.Chest, dancerSimulation.Shapes.Add(chestShape), dancerSimulation, i, dancerGridWidth, dancerFilters, filterForDancerBodies); + dancer.Head = CreateCopyForDancer(mainSimulation, SourceBodyHandles.Head, dancerSimulation.Shapes.Add(headShape), dancerSimulation, i, dancerGridWidth, dancerFilters, filterForDancerBodies); var upperLegShapeInTarget = dancerSimulation.Shapes.Add(upperLegShape); var lowerLegShapeInTarget = dancerSimulation.Shapes.Add(lowerLegShape); - dancer.UpperLeftLeg = CreateCopyForDancer(mainSimulation, SourceBodyHandles.UpperLeftLeg, upperLegShapeInTarget, dancerSimulation, i, dancerFilters, filterForDancerBodies); - dancer.LowerLeftLeg = CreateCopyForDancer(mainSimulation, SourceBodyHandles.LowerLeftLeg, lowerLegShapeInTarget, dancerSimulation, i, dancerFilters, filterForDancerBodies); - dancer.UpperRightLeg = CreateCopyForDancer(mainSimulation, SourceBodyHandles.UpperRightLeg, upperLegShapeInTarget, dancerSimulation, i, dancerFilters, filterForDancerBodies); - dancer.LowerRightLeg = CreateCopyForDancer(mainSimulation, SourceBodyHandles.LowerRightLeg, lowerLegShapeInTarget, dancerSimulation, i, dancerFilters, filterForDancerBodies); + dancer.UpperLeftLeg = CreateCopyForDancer(mainSimulation, SourceBodyHandles.UpperLeftLeg, upperLegShapeInTarget, dancerSimulation, i, dancerGridWidth, dancerFilters, filterForDancerBodies); + dancer.LowerLeftLeg = CreateCopyForDancer(mainSimulation, SourceBodyHandles.LowerLeftLeg, lowerLegShapeInTarget, dancerSimulation, i, dancerGridWidth, dancerFilters, filterForDancerBodies); + dancer.UpperRightLeg = CreateCopyForDancer(mainSimulation, SourceBodyHandles.UpperRightLeg, upperLegShapeInTarget, dancerSimulation, i, dancerGridWidth, dancerFilters, filterForDancerBodies); + dancer.LowerRightLeg = CreateCopyForDancer(mainSimulation, SourceBodyHandles.LowerRightLeg, lowerLegShapeInTarget, dancerSimulation, i, dancerGridWidth, dancerFilters, filterForDancerBodies); var upperArmShapeInTarget = dancerSimulation.Shapes.Add(upperArmShape); var lowerArmShapeInTarget = dancerSimulation.Shapes.Add(lowerArmShape); - dancer.UpperLeftArm = CreateCopyForDancer(mainSimulation, SourceBodyHandles.UpperLeftArm, upperArmShapeInTarget, dancerSimulation, i, dancerFilters, filterForDancerBodies); - dancer.LowerLeftArm = CreateCopyForDancer(mainSimulation, SourceBodyHandles.LowerLeftArm, lowerArmShapeInTarget, dancerSimulation, i, dancerFilters, filterForDancerBodies); - dancer.UpperRightArm = CreateCopyForDancer(mainSimulation, SourceBodyHandles.UpperRightArm, upperArmShapeInTarget, dancerSimulation, i, dancerFilters, filterForDancerBodies); - dancer.LowerRightArm = CreateCopyForDancer(mainSimulation, SourceBodyHandles.LowerRightArm, lowerArmShapeInTarget, dancerSimulation, i, dancerFilters, filterForDancerBodies); + dancer.UpperLeftArm = CreateCopyForDancer(mainSimulation, SourceBodyHandles.UpperLeftArm, upperArmShapeInTarget, dancerSimulation, i, dancerGridWidth, dancerFilters, filterForDancerBodies); + dancer.LowerLeftArm = CreateCopyForDancer(mainSimulation, SourceBodyHandles.LowerLeftArm, lowerArmShapeInTarget, dancerSimulation, i, dancerGridWidth, dancerFilters, filterForDancerBodies); + dancer.UpperRightArm = CreateCopyForDancer(mainSimulation, SourceBodyHandles.UpperRightArm, upperArmShapeInTarget, dancerSimulation, i, dancerGridWidth, dancerFilters, filterForDancerBodies); + dancer.LowerRightArm = CreateCopyForDancer(mainSimulation, SourceBodyHandles.LowerRightArm, lowerArmShapeInTarget, dancerSimulation, i, dancerGridWidth, dancerFilters, filterForDancerBodies); - dressUpDancer(dancerSimulation, dancerFilters, dancer, i, levelOfDetail); + dressUpDancer(dancerSimulation, dancerFilters, dancer, i, dancerGridWidth, levelOfDetail); BodyCount += dancerSimulation.Bodies.ActiveSet.Count; ConstraintCount += dancerSimulation.Solver.CountConstraints(); Simulations[i] = dancerSimulation; @@ -345,24 +347,24 @@ static void ConstrainOrientation(Simulation simulation, BodyHandle a, BodyHandle } - static (int columnIndex, int rowIndex) GetRowAndColumnForDancer(int dancerIndex) + static (int columnIndex, int rowIndex) GetRowAndColumnForDancer(int dancerIndex, int dancerGridWidth) { - var rowIndex = dancerIndex / DancerGridWidth; - return (dancerIndex - rowIndex * DancerGridWidth, rowIndex); + var rowIndex = dancerIndex / dancerGridWidth; + return (dancerIndex - rowIndex * dancerGridWidth, rowIndex); } public QuickQueue MotionHistory; - public static Vector3 GetOffsetForDancer(int i) + public static Vector3 GetOffsetForDancer(int i, int dancerGridWidth) { const float spacing = 2; - var (columnIndex, rowIndex) = GetRowAndColumnForDancer(i); - return new Vector3(DancerGridWidth * spacing / -2 + (columnIndex + 0.5f) * spacing, 0, -2 + rowIndex * -spacing); + var (columnIndex, rowIndex) = GetRowAndColumnForDancer(i, dancerGridWidth); + return new Vector3(dancerGridWidth * spacing / -2 + (columnIndex + 0.5f) * spacing, 0, -2 + rowIndex * -spacing); } - public static float GetDistanceFromMainDancer(int dancerIndex) + public static float GetDistanceFromMainDancer(int dancerIndex, int dancerGridWidth) { - var (columnIndex, rowIndex) = GetRowAndColumnForDancer(dancerIndex); - var offsetX = columnIndex - (DancerGridWidth / 2 - 0.5f); + var (columnIndex, rowIndex) = GetRowAndColumnForDancer(dancerIndex, dancerGridWidth); + var offsetX = columnIndex - (dancerGridWidth / 2 - 0.5f); return MathF.Sqrt(offsetX * offsetX + rowIndex * rowIndex); } @@ -397,7 +399,7 @@ unsafe void ExecuteDancer(int dancerIndex, int workerIndex) var dancerSimulation = Simulations[dancerIndex]; var sourceHandleBuffer = DancerBodyHandles.AsBuffer((DancerBodyHandles*)Unsafe.AsPointer(ref SourceBodyHandles)); //Delay is greater for the dancers that are further away, plus a little randomized component to desynchronize them. - var historicalStateStartIndex = MotionHistory.Count - sourceHandleBuffer.Length * ((int)GetDistanceFromMainDancer(dancerIndex) * 8 + 1 + (HashHelper.Rehash(dancerIndex) & 0xF)); + var historicalStateStartIndex = MotionHistory.Count - sourceHandleBuffer.Length * ((int)GetDistanceFromMainDancer(dancerIndex, DancerGridWidth) * 8 + 1 + (HashHelper.Rehash(dancerIndex) & 0xF)); if (historicalStateStartIndex < 0) historicalStateStartIndex = 0; var targetHandleBuffer = DancerBodyHandles.AsBuffer((DancerBodyHandles*)Unsafe.AsPointer(ref dancerHandles)); @@ -405,7 +407,7 @@ unsafe void ExecuteDancer(int dancerIndex, int workerIndex) { ref var targetMotionState = ref dancerSimulation.Bodies[targetHandleBuffer[j]].MotionState; targetMotionState = MotionHistory[historicalStateStartIndex + j]; - targetMotionState.Pose.Position += GetOffsetForDancer(dancerIndex); + targetMotionState.Pose.Position += GetOffsetForDancer(dancerIndex, DancerGridWidth); } //Update the simulation for the dancer. dancerSimulation.Timestep(Demo.TimestepDuration); diff --git a/Demos/Demos/Dancers/PlumpDancerDemo.cs b/Demos/Demos/Dancers/PlumpDancerDemo.cs new file mode 100644 index 000000000..470f55247 --- /dev/null +++ b/Demos/Demos/Dancers/PlumpDancerDemo.cs @@ -0,0 +1,262 @@ +using BepuPhysics; +using BepuPhysics.Collidables; +using BepuPhysics.CollisionDetection; +using BepuPhysics.Constraints; +using BepuUtilities; +using BepuUtilities.Memory; +using DemoContentLoader; +using DemoRenderer; +using DemoRenderer.UI; +using DemoUtilities; +using System; +using System.Diagnostics; +using System.Numerics; +using System.Runtime.InteropServices; + +namespace Demos.Demos.Dancers +{ + /// + /// A bunch of somewhat overweight background dancers struggle to keep up with the masterful purple prancer. + /// Combined with the implementation, this shows an example of how cosmetic deformable physics could be applied to characters. + /// + public class PlumpDancerDemo : Demo + { + //This demo relies on the DemoDancers to manage all the ragdolls and their simulations. + //All this demo needs to do is make the fatsuits. + DemoDancers dancers; + + //While creating the fatsuits, we'll precompute some stuff to make testing a little quicker. + //Could make this significantly faster still by being trickier with vectorization, but the demo launch time is already reasonable. + struct TestCapsule + { + public Vector3 Start; + public Vector3 Direction; + public float Length; + public float Radius; + } + static TestCapsule CreateTestCapsule(Simulation simulation, BodyHandle handle) + { + var body = simulation.Bodies[handle]; + Debug.Assert(body.Collidable.Shape.Type == Capsule.Id, "For the purposes of this demo, we assume that all of the bodies that are being tested are capsules."); + ref var shape = ref simulation.Shapes.GetShape(body.Collidable.Shape.Index); + var pose = body.Pose; + TestCapsule toReturn; + QuaternionEx.TransformUnitY(pose.Orientation, out toReturn.Direction); + toReturn.Start = pose.Position - toReturn.Direction * shape.HalfLength; + toReturn.Radius = shape.Radius; + toReturn.Length = shape.HalfLength * 2; + return toReturn; + + } + unsafe static void CreateBodyGrid(DancerBodyHandles bodyHandles, Int3 axisSizeInBodies, Vector3 gridMinimum, Vector3 gridMaximum, float bodyRadius, float massPerBody, + int instanceId, Simulation simulation, CollidableProperty filters) + { + var shape = new Sphere(bodyRadius); + var shapeIndex = simulation.Shapes.Add(shape); + //Note that, unlike the DancerDemo where cloth nodes cannot rotate, the deformable sub-bodies can rotate. + //That's because this demo is going to connect bodies together using Weld constraints, which control all six degrees of freedom. + //You could also use a CenterDistanceConstraint/Limit with VolumeConstraints to maintain shape, but a bunch of Weld constraints is a little simpler. + var description = BodyDescription.CreateDynamic(QuaternionEx.Identity, shape.ComputeInertia(massPerBody), shapeIndex, 0.01f); + BodyHandle[,,] handles = new BodyHandle[axisSizeInBodies.X, axisSizeInBodies.Y, axisSizeInBodies.Z]; + BodyHandle[,,] nearestHandles = new BodyHandle[axisSizeInBodies.X, axisSizeInBodies.Y, axisSizeInBodies.Z]; + var gridSpan = gridMaximum - gridMinimum; + var gridSpacing = gridSpan / new Vector3(axisSizeInBodies.X - 1, axisSizeInBodies.Y - 1, axisSizeInBodies.Z - 1); + Span testCapsules = stackalloc TestCapsule[11]; + + //DancerBodyHandles stores the head last, so we can just check the first 11 bodies that are all capsules. The head isn't going to be covered in the fatsuit, so it doesn't need to be checked anyway. + var handlesBuffer = DancerBodyHandles.AsBuffer(&bodyHandles); + for (int i = 0; i < 11; ++i) + { + testCapsules[i] = CreateTestCapsule(simulation, handlesBuffer[i]); + } + + for (int x = 0; x < axisSizeInBodies.X; ++x) + { + for (int y = 0; y < axisSizeInBodies.Y; ++y) + { + for (int z = 0; z < axisSizeInBodies.Z; ++z) + { + var position = gridMinimum + gridSpacing * new Vector3(x, y, z); + float minimumDistance = float.MaxValue; + int minimumIndex = 0; + for (int i = 0; i < testCapsules.Length; ++i) + { + var testCapsule = testCapsules[i]; + var distance = Vector3.Distance(position, testCapsule.Start + MathF.Max(0, MathF.Min(testCapsule.Length, Vector3.Dot(position - testCapsule.Start, testCapsule.Direction))) * testCapsule.Direction) - testCapsule.Radius; + if (distance < minimumDistance) + { + minimumDistance = distance; + minimumIndex = i; + } + } + nearestHandles[x, y, z] = handlesBuffer[minimumIndex]; + + var maximumDistanceForCreatingNodes = bodyRadius * 3; + if (minimumDistance < bodyRadius) + { + //Intersecting; don't create a body. -2 for this demo marks the body as intersecting, so we can disambiguate it from slots that are just empty due to being too far away. + handles[x, y, z] = new BodyHandle { Value = -2 }; + } + else if (minimumDistance > maximumDistanceForCreatingNodes) + { + //-1 means too far. + handles[x, y, z] = new BodyHandle { Value = -1 }; + } + else + { + //Nearby. Create and attach it to the nearest body part. + description.Pose.Position = position; + var handle = simulation.Bodies.Add(description); + handles[x, y, z] = handle; + if (filters != null) + filters.Allocate(handle) = new DeformableCollisionFilter(x, y, z, instanceId); + + var nearestHandle = handlesBuffer[minimumIndex]; + var nearestPose = simulation.Bodies[nearestHandle].Pose; + var conjugate = Quaternion.Conjugate(nearestPose.Orientation); + //simulation.Solver.Add(nearestHandle, handle, new Weld + //{ + // LocalOffset = QuaternionEx.Transform(position - nearestPose.Position, conjugate), + // LocalOrientation = conjugate, + // SpringSettings = new SpringSettings(2, 1) + //}); + + } + } + } + } + for (int x = 0; x < axisSizeInBodies.X; ++x) + { + for (int y = 0; y < axisSizeInBodies.Y; ++y) + { + for (int z = 0; z < axisSizeInBodies.Z; ++z) + { + //Kind of hacky, but simple: for every node that is exposed to the air (a neighbor has a body handle flagged as -1), make sure it has a collidable. + //Anything inside doesn't need a collidable. + var handle = handles[x, y, z]; + if (handle.Value >= 0) + { + var needsAnchor = + (x != 0 && handles[x - 1, y, z].Value == -2) || + (x != handles.GetLength(0) - 1 && handles[x + 1, y, z].Value == -2) || + (y != 0 && handles[x, y - 1, z].Value == -2) || + (y != handles.GetLength(1) - 1 && handles[x, y + 1, z].Value == -2) || + (z != 0 && handles[x, y, z - 1].Value == -2) || + (z != handles.GetLength(2) - 1 && handles[x, y, z + 1].Value == -2); + var source = simulation.Bodies[handle]; + if (needsAnchor) + { + var nearestHandle = nearestHandles[x, y, z]; + var nearestPose = simulation.Bodies[nearestHandle].Pose; + var conjugate = Quaternion.Conjugate(nearestPose.Orientation); + simulation.Solver.Add(nearestHandle, handle, new Weld + { + LocalOffset = QuaternionEx.Transform(source.Pose.Position - nearestPose.Position, conjugate), + LocalOrientation = conjugate, + SpringSettings = new SpringSettings(5, 1) + }); + } + var needsCollidable = + (x == 0 || handles[x - 1, y, z].Value == -1) || (x == handles.GetLength(0) - 1 || handles[x + 1, y, z].Value == -1) || + (y == 0 || handles[x, y - 1, z].Value == -1) || (y == handles.GetLength(1) - 1 || handles[x, y + 1, z].Value == -1) || + (z == 0 || handles[x, y, z - 1].Value == -1) || (z == handles.GetLength(2) - 1 || handles[x, y, z + 1].Value == -1); + if (!needsCollidable) + { + source.SetShape(default); + } + + static void TryAdd(Simulation simulation, BodyReference source, BodyHandle targetHandle) + { + if (targetHandle.Value >= 0) + { + var target = simulation.Bodies[targetHandle]; + simulation.Solver.Add(source.Handle, targetHandle, new Weld { LocalOffset = target.Pose.Position - source.Pose.Position, LocalOrientation = Quaternion.Identity, SpringSettings = new SpringSettings(5, 1) }); + } + } + if (x < handles.GetLength(0) - 1) + { + TryAdd(simulation, source, handles[x + 1, y, z]); + } + if (y < handles.GetLength(1) - 1) + { + TryAdd(simulation, source, handles[x, y + 1, z]); + } + if (z < handles.GetLength(2) - 1) + { + TryAdd(simulation, source, handles[x, y, z + 1]); + } + } + } + } + } + + } + + + static void CreateFatSuit(Simulation simulation, CollidableProperty filters, DancerBodyHandles bodyHandles, int dancerIndex, int dancerGridWidth, float levelOfDetail) + { + //The demo uses lower resolution grids on dancers further away from the main dancer. + //This is a sorta-example of level of detail. In a 'real' use case, you'd probably want to transition between levels of detail dynamically as the camera moved around. + //That's a little trickier, but doable. Going low to high, for example, requires creating bodies at interpolated positions between existing bodies, while going to a lower level of detail removes them. + var targetDressDiameter = 2.6f; + var fullDetailWidthInBodies = 29; + float spacingAtFullDetail = targetDressDiameter / fullDetailWidthInBodies; + float bodyRadius = spacingAtFullDetail / 1.75f; + var scale = MathF.Pow(2, levelOfDetail); + var widthInBodies = (int)MathF.Ceiling(fullDetailWidthInBodies / scale); + var spacing = spacingAtFullDetail * scale; + var chest = simulation.Bodies[bodyHandles.Chest]; + ref var chestShape = ref simulation.Shapes.GetShape(chest.Collidable.Shape.Index); + var topOfChestHeight = chest.Pose.Position.Y + chestShape.Radius + bodyRadius; + var axisBodyCounts = new Int3 { X = 15, Y = 15, Z = 15 }; + var topOfChestPosition = new Vector3(0, topOfChestHeight, 0) + DemoDancers.GetOffsetForDancer(dancerIndex, dancerGridWidth); + var suitSize = new Vector3(1, 1f, 1); + var suitMinimum = topOfChestPosition - suitSize * new Vector3(0.5f, 1f, 0.5f); + var suitMaximum = suitMinimum + suitSize; + CreateBodyGrid(bodyHandles, axisBodyCounts, suitMinimum, suitMaximum, bodyRadius, 0.01f, dancerIndex, simulation, filters); + } + + + public unsafe override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(0, 2, 10); + camera.Yaw = 0; + camera.Pitch = 0; + + var collisionFilters = new CollidableProperty(); + //Note very high damping on the main ragdoll simulation; makes it easier to pose. + Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks { CollisionFilters = collisionFilters }, new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0), 0, 0), new SolveDescription(8, 1)); + + dancers = new DemoDancers().Initialize(8, 8, Simulation, collisionFilters, ThreadDispatcher, BufferPool, CreateFatSuit, new DeformableCollisionFilter(0, 0, 0, -1)); + + } + public unsafe override void Update(Window window, Camera camera, Input input, float dt) + { + dancers.UpdateTargets(Simulation); + base.Update(window, camera, input, dt); + } + + public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + { + renderer.Shapes.AddInstances(dancers.Simulations, ThreadDispatcher); + renderer.Lines.Extract(dancers.Simulations, ThreadDispatcher); + + var resolution = renderer.Surface.Resolution; + renderer.TextBatcher.Write(text.Clear().Append("Cosmetic simulations, like cloth, often don't need to be in a game's main simulation."), new Vector2(16, resolution.Y - 144), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Every background dancer in this demo has its own simulation. All dancers can be easily updated in parallel."), new Vector2(16, resolution.Y - 128), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Dancers further from the main dancer use sparser cloth and disable self collision for extra performance."), new Vector2(16, resolution.Y - 112), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Dancer count: ").Append(dancers.Handles.Length), new Vector2(16, resolution.Y - 80), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Total cloth body count: ").Append(dancers.BodyCount), new Vector2(16, resolution.Y - 64), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Total cloth constraint count: ").Append(dancers.ConstraintCount), new Vector2(16, resolution.Y - 48), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Total dancer execution time (ms): ").Append(dancers.ExecutionTime * 1000, 2), new Vector2(16, resolution.Y - 32), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Amortized execution time per dancer (us): ").Append(dancers.ExecutionTime * 1e6 / dancers.Handles.Length, 1), new Vector2(16, resolution.Y - 16), 16, Vector3.One, font); + + base.Render(renderer, camera, input, text, font); + } + protected override void OnDispose() + { + dancers.Dispose(BufferPool); + + } + } +} diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index 34a2cdfef..4fe63800a 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -526,14 +526,35 @@ public static bool Test(in DeformableCollisionFilter a, in DeformableCollisionFi } - struct DeformableCallbacks : INarrowPhaseCallbacks + struct DeformableCallbacks : INarrowPhaseCallbacks, Dancers.IDancerNarrowPhaseCallbacks //"IDancerNarrowPhaseCallbacks" just means this is a INarrowPhaseCallbacks usable with the DemoDancers. { public CollidableProperty Filters; + public PairMaterialProperties Material; + /// + /// Minimum manhattan distance in cloth nodes required for two cloth nodes to collide. Stops adjacent cloth nodes from generating contacts and interfering with clothy behavior. + /// + public int MinimumDistanceForSelfCollisions; public void Initialize(Simulation simulation) { Filters.Initialize(simulation); } + public DeformableCallbacks(CollidableProperty filters, PairMaterialProperties material, int minimumDistanceForSelfCollisions = 3) + { + Filters = filters; + Material = material; + MinimumDistanceForSelfCollisions = minimumDistanceForSelfCollisions; + } + public DeformableCallbacks(CollidableProperty filters, int minimumDistanceForSelfCollisions = 3) + :this(filters, new PairMaterialProperties(1, 2, new SpringSettings(30, 1)), minimumDistanceForSelfCollisions) + { + } + //This slightly awkward factory is just here for the dancer demos. + DeformableCallbacks Dancers.IDancerNarrowPhaseCallbacks.Create(CollidableProperty filters, PairMaterialProperties pairMaterialProperties, int minimumDistanceForSelfCollisions) + { + return new DeformableCallbacks(filters, pairMaterialProperties, minimumDistanceForSelfCollisions); + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin) { @@ -698,7 +719,7 @@ internal unsafe static void CreateDeformable(Simulation simulation, in Vector3 p edges.Dispose(pool); } - + public unsafe override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(-5f, 5.5f, 5f); @@ -706,7 +727,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = MathHelper.Pi * 0.15f; var filters = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0), 0, 0), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DeformableCallbacks(filters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0), 0, 0), new SolveDescription(1, 4)); var meshContent = content.Load("Content\\newt.obj"); float cellSize = 0.1f; diff --git a/Demos/SpecializedTests/Media/NewtVideoDemo.cs b/Demos/SpecializedTests/Media/NewtVideoDemo.cs index 69322f3cf..7da477356 100644 --- a/Demos/SpecializedTests/Media/NewtVideoDemo.cs +++ b/Demos/SpecializedTests/Media/NewtVideoDemo.cs @@ -21,7 +21,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = MathHelper.Pi * 0.15f; var filters = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new DeformableCallbacks { Filters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DeformableCallbacks(filters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); var meshContent = content.Load("Content\\newt.obj"); float cellSize = 0.1f; From a7f4153e0d93805058505c64b4a549e294951bca Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 19 Jan 2022 18:10:48 -0600 Subject: [PATCH 382/947] Plump dancers now slightly more reasonable. --- Demos/Demos/Dancers/DancerDemo.cs | 3 +- Demos/Demos/Dancers/DemoDancers.cs | 9 ++--- Demos/Demos/Dancers/PlumpDancerDemo.cs | 46 +++++++++++--------------- 3 files changed, 27 insertions(+), 31 deletions(-) diff --git a/Demos/Demos/Dancers/DancerDemo.cs b/Demos/Demos/Dancers/DancerDemo.cs index 5501d1456..aa300a745 100644 --- a/Demos/Demos/Dancers/DancerDemo.cs +++ b/Demos/Demos/Dancers/DancerDemo.cs @@ -105,6 +105,7 @@ static void TailorDress(Simulation simulation, CollidableProperty(16, 16, Simulation, collisionFilters, ThreadDispatcher, BufferPool, TailorDress, new ClothCollisionFilter(0, 0, -1)); + dancers = new DemoDancers().Initialize(16, 16, Simulation, collisionFilters, ThreadDispatcher, BufferPool, new SolveDescription(1, 4), TailorDress, new ClothCollisionFilter(0, 0, -1)); } public unsafe override void Update(Window window, Camera camera, Input input, float dt) diff --git a/Demos/Demos/Dancers/DemoDancers.cs b/Demos/Demos/Dancers/DemoDancers.cs index bda0169ae..e524f67d8 100644 --- a/Demos/Demos/Dancers/DemoDancers.cs +++ b/Demos/Demos/Dancers/DemoDancers.cs @@ -104,8 +104,9 @@ public class DemoDancers DancerControl rightHandControl; public delegate void DressUpDancer(Simulation simulation, CollidableProperty filters, DancerBodyHandles bodyHandles, int dancerIndex, int dancerGridWidth, float levelOfDetail) where TCollisionFilter : unmanaged; - public DemoDancers Initialize(int dancerGridWidth, int dancerGridLength, Simulation mainSimulation, CollidableProperty mainCollisionFilters, IThreadDispatcher threadDispatcher, BufferPool pool, - DressUpDancer dressUpDancer, TCollisionFilter filterForDancerBodies) + public DemoDancers Initialize( + int dancerGridWidth, int dancerGridLength, Simulation mainSimulation, CollidableProperty mainCollisionFilters, + IThreadDispatcher threadDispatcher, BufferPool pool, SolveDescription dancerSolveDescription, DressUpDancer dressUpDancer, TCollisionFilter filterForDancerBodies) where TNarrowPhaseCallbacks : struct, INarrowPhaseCallbacks, IDancerNarrowPhaseCallbacks where TCollisionFilter : unmanaged { @@ -231,7 +232,7 @@ static BodyHandle CreateCopyForDancer(Simulation sourceSimulation, BodyHandle so //Distance from the main dancer is used to select clothing level of detail. This isn't dynamic based on camera motion, but shows the general idea. //Since we don't have to worry about transitions, the level of detail is a continuous value here. var distanceFromMainDancer = GetDistanceFromMainDancer(i, dancerGridWidth); - var levelOfDetail = MathF.Max(0f, MathF.Min(1.5f, MathF.Log2(MathF.Max(1, distanceFromMainDancer) - 0.8f))); + var levelOfDetail = MathF.Log2(MathF.Max(1, distanceFromMainDancer) - 0.8f); //Note that we use a smaller allocation block size for dancer simulations. //This demo is creating a *lot* of buffer pools just because that's the simplest way to keep things thread safe. //If you wanted to reduce the amount of pool-induced memory overhead, you could consider sharing buffer pools between multiple simulations @@ -245,7 +246,7 @@ static BodyHandle CreateCopyForDancer(Simulation sourceSimulation, BodyHandle so //The ClothCallbacks specify a minimum distance required for self collision, and low detail (higher 'level of detail' values) results in MaxValue minimum distance. var narrowPhaseCallbacks = default(TNarrowPhaseCallbacks).Create(dancerFilters, new PairMaterialProperties(0.4f, 20, new SpringSettings(120, 1)), levelOfDetail <= 0.5f ? 3 : int.MaxValue); var dancerSimulation = Simulation.Create(new BufferPool(16384), narrowPhaseCallbacks, - new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4), + new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), dancerSolveDescription, //To save some memory, initialize the dancer simulations with smaller starting sizes. For the higher level of detail simulations this could require some resizing. //More precise estimates could be made without too much work, but the demo will keep it simple. initialAllocationSizes: new SimulationAllocationSizes(128, 1, 1, 8, 512, 64, 8)); diff --git a/Demos/Demos/Dancers/PlumpDancerDemo.cs b/Demos/Demos/Dancers/PlumpDancerDemo.cs index 470f55247..8a3eb9f0f 100644 --- a/Demos/Demos/Dancers/PlumpDancerDemo.cs +++ b/Demos/Demos/Dancers/PlumpDancerDemo.cs @@ -69,6 +69,7 @@ unsafe static void CreateBodyGrid(DancerBodyHandles bodyHandles, Int3 axisSizeIn { testCapsules[i] = CreateTestCapsule(simulation, handlesBuffer[i]); } + var center = (gridMinimum + gridMaximum) * 0.5f; for (int x = 0; x < axisSizeInBodies.X; ++x) { @@ -91,7 +92,7 @@ unsafe static void CreateBodyGrid(DancerBodyHandles bodyHandles, Int3 axisSizeIn } nearestHandles[x, y, z] = handlesBuffer[minimumIndex]; - var maximumDistanceForCreatingNodes = bodyRadius * 3; + var maximumDistanceForCreatingNodes = MathF.Max(0.1f, 0.8f - 1.5f * Vector3.Distance(position, center)); if (minimumDistance < bodyRadius) { //Intersecting; don't create a body. -2 for this demo marks the body as intersecting, so we can disambiguate it from slots that are just empty due to being too far away. @@ -114,12 +115,6 @@ unsafe static void CreateBodyGrid(DancerBodyHandles bodyHandles, Int3 axisSizeIn var nearestHandle = handlesBuffer[minimumIndex]; var nearestPose = simulation.Bodies[nearestHandle].Pose; var conjugate = Quaternion.Conjugate(nearestPose.Orientation); - //simulation.Solver.Add(nearestHandle, handle, new Weld - //{ - // LocalOffset = QuaternionEx.Transform(position - nearestPose.Position, conjugate), - // LocalOrientation = conjugate, - // SpringSettings = new SpringSettings(2, 1) - //}); } } @@ -137,11 +132,11 @@ unsafe static void CreateBodyGrid(DancerBodyHandles bodyHandles, Int3 axisSizeIn if (handle.Value >= 0) { var needsAnchor = - (x != 0 && handles[x - 1, y, z].Value == -2) || + (x != 0 && handles[x - 1, y, z].Value == -2) || (x != handles.GetLength(0) - 1 && handles[x + 1, y, z].Value == -2) || - (y != 0 && handles[x, y - 1, z].Value == -2) || + (y != 0 && handles[x, y - 1, z].Value == -2) || (y != handles.GetLength(1) - 1 && handles[x, y + 1, z].Value == -2) || - (z != 0 && handles[x, y, z - 1].Value == -2) || + (z != 0 && handles[x, y, z - 1].Value == -2) || (z != handles.GetLength(2) - 1 && handles[x, y, z + 1].Value == -2); var source = simulation.Bodies[handle]; if (needsAnchor) @@ -153,7 +148,7 @@ unsafe static void CreateBodyGrid(DancerBodyHandles bodyHandles, Int3 axisSizeIn { LocalOffset = QuaternionEx.Transform(source.Pose.Position - nearestPose.Position, conjugate), LocalOrientation = conjugate, - SpringSettings = new SpringSettings(5, 1) + SpringSettings = new SpringSettings(6, 0.4f) }); } var needsCollidable = @@ -170,7 +165,7 @@ static void TryAdd(Simulation simulation, BodyReference source, BodyHandle targe if (targetHandle.Value >= 0) { var target = simulation.Bodies[targetHandle]; - simulation.Solver.Add(source.Handle, targetHandle, new Weld { LocalOffset = target.Pose.Position - source.Pose.Position, LocalOrientation = Quaternion.Identity, SpringSettings = new SpringSettings(5, 1) }); + simulation.Solver.Add(source.Handle, targetHandle, new Weld { LocalOffset = target.Pose.Position - source.Pose.Position, LocalOrientation = Quaternion.Identity, SpringSettings = new SpringSettings(6, 0.4f) }); } } if (x < handles.GetLength(0) - 1) @@ -198,19 +193,17 @@ static void CreateFatSuit(Simulation simulation, CollidableProperty(chest.Collidable.Shape.Index); - var topOfChestHeight = chest.Pose.Position.Y + chestShape.Radius + bodyRadius; - var axisBodyCounts = new Int3 { X = 15, Y = 15, Z = 15 }; + var topOfChestHeight = chest.Pose.Position.Y + chestShape.Radius; var topOfChestPosition = new Vector3(0, topOfChestHeight, 0) + DemoDancers.GetOffsetForDancer(dancerIndex, dancerGridWidth); - var suitSize = new Vector3(1, 1f, 1); var suitMinimum = topOfChestPosition - suitSize * new Vector3(0.5f, 1f, 0.5f); var suitMaximum = suitMinimum + suitSize; CreateBodyGrid(bodyHandles, axisBodyCounts, suitMinimum, suitMaximum, bodyRadius, 0.01f, dancerIndex, simulation, filters); @@ -227,7 +220,8 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //Note very high damping on the main ragdoll simulation; makes it easier to pose. Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks { CollisionFilters = collisionFilters }, new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0), 0, 0), new SolveDescription(8, 1)); - dancers = new DemoDancers().Initialize(8, 8, Simulation, collisionFilters, ThreadDispatcher, BufferPool, CreateFatSuit, new DeformableCollisionFilter(0, 0, 0, -1)); + //Note that, because the constraints in the fat suit are quite soft, we can get away with extremely minimal solving time. There's one substep with one velocity iteration. + dancers = new DemoDancers().Initialize(8, 8, Simulation, collisionFilters, ThreadDispatcher, BufferPool, new SolveDescription(1, 1), CreateFatSuit, new DeformableCollisionFilter(0, 0, 0, -1)); } public unsafe override void Update(Window window, Camera camera, Input input, float dt) @@ -242,12 +236,12 @@ public override void Render(Renderer renderer, Camera camera, Input input, TextB renderer.Lines.Extract(dancers.Simulations, ThreadDispatcher); var resolution = renderer.Surface.Resolution; - renderer.TextBatcher.Write(text.Clear().Append("Cosmetic simulations, like cloth, often don't need to be in a game's main simulation."), new Vector2(16, resolution.Y - 144), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Cosmetic simulations, like character blubber, often don't need to be in a game's main simulation."), new Vector2(16, resolution.Y - 144), 16, Vector3.One, font); renderer.TextBatcher.Write(text.Clear().Append("Every background dancer in this demo has its own simulation. All dancers can be easily updated in parallel."), new Vector2(16, resolution.Y - 128), 16, Vector3.One, font); - renderer.TextBatcher.Write(text.Clear().Append("Dancers further from the main dancer use sparser cloth and disable self collision for extra performance."), new Vector2(16, resolution.Y - 112), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Dancers further from the main dancer use sparser body grids and disable self collision for extra performance."), new Vector2(16, resolution.Y - 112), 16, Vector3.One, font); renderer.TextBatcher.Write(text.Clear().Append("Dancer count: ").Append(dancers.Handles.Length), new Vector2(16, resolution.Y - 80), 16, Vector3.One, font); - renderer.TextBatcher.Write(text.Clear().Append("Total cloth body count: ").Append(dancers.BodyCount), new Vector2(16, resolution.Y - 64), 16, Vector3.One, font); - renderer.TextBatcher.Write(text.Clear().Append("Total cloth constraint count: ").Append(dancers.ConstraintCount), new Vector2(16, resolution.Y - 48), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Total deformable body count: ").Append(dancers.BodyCount), new Vector2(16, resolution.Y - 64), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Total deformable constraint count: ").Append(dancers.ConstraintCount), new Vector2(16, resolution.Y - 48), 16, Vector3.One, font); renderer.TextBatcher.Write(text.Clear().Append("Total dancer execution time (ms): ").Append(dancers.ExecutionTime * 1000, 2), new Vector2(16, resolution.Y - 32), 16, Vector3.One, font); renderer.TextBatcher.Write(text.Clear().Append("Amortized execution time per dancer (us): ").Append(dancers.ExecutionTime * 1e6 / dancers.Handles.Length, 1), new Vector2(16, resolution.Y - 16), 16, Vector3.One, font); From 1b97d7d1f0e458e518461d1c1c206032242a64d8 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 20 Jan 2022 12:27:27 -0600 Subject: [PATCH 383/947] Incorrect comment removed. --- Demos/Demos/Dancers/DancerDemo.cs | 3 +-- Demos/Demos/Dancers/PlumpDancerDemo.cs | 3 +-- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/Demos/Demos/Dancers/DancerDemo.cs b/Demos/Demos/Dancers/DancerDemo.cs index aa300a745..31fadc9ae 100644 --- a/Demos/Demos/Dancers/DancerDemo.cs +++ b/Demos/Demos/Dancers/DancerDemo.cs @@ -160,8 +160,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0; var collisionFilters = new CollidableProperty(); - //Note very high damping on the main ragdoll simulation; makes it easier to pose. - Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks { CollisionFilters = collisionFilters }, new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0), 0, 0), new SolveDescription(8, 1)); + Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks { CollisionFilters = collisionFilters }, new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)), new SolveDescription(8, 1)); dancers = new DemoDancers().Initialize(16, 16, Simulation, collisionFilters, ThreadDispatcher, BufferPool, new SolveDescription(1, 4), TailorDress, new ClothCollisionFilter(0, 0, -1)); diff --git a/Demos/Demos/Dancers/PlumpDancerDemo.cs b/Demos/Demos/Dancers/PlumpDancerDemo.cs index 8a3eb9f0f..bba0463d0 100644 --- a/Demos/Demos/Dancers/PlumpDancerDemo.cs +++ b/Demos/Demos/Dancers/PlumpDancerDemo.cs @@ -217,8 +217,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0; var collisionFilters = new CollidableProperty(); - //Note very high damping on the main ragdoll simulation; makes it easier to pose. - Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks { CollisionFilters = collisionFilters }, new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0), 0, 0), new SolveDescription(8, 1)); + Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks { CollisionFilters = collisionFilters }, new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)), new SolveDescription(8, 1)); //Note that, because the constraints in the fat suit are quite soft, we can get away with extremely minimal solving time. There's one substep with one velocity iteration. dancers = new DemoDancers().Initialize(8, 8, Simulation, collisionFilters, ThreadDispatcher, BufferPool, new SolveDescription(1, 1), CreateFatSuit, new DeformableCollisionFilter(0, 0, 0, -1)); From e17246c9167b5f4cef744f5ebb08d90abd70fc57 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 20 Jan 2022 15:52:55 -0600 Subject: [PATCH 384/947] RopeStabilityDemo updates. --- Demos/DemoSet.cs | 5 +---- Demos/Demos/RopeStabilityDemo.cs | 31 ++++++++++++++++++------------- 2 files changed, 19 insertions(+), 17 deletions(-) diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index 5ab5b622a..0cd046ce8 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -49,11 +49,7 @@ public DemoSet() AddOption(); AddOption(); AddOption(); - AddOption(); - AddOption(); AddOption(); - AddOption(); - AddOption(); AddOption(); AddOption(); AddOption(); @@ -67,6 +63,7 @@ public DemoSet() AddOption(); AddOption(); AddOption(); + AddOption(); AddOption(); AddOption(); AddOption(); diff --git a/Demos/Demos/RopeStabilityDemo.cs b/Demos/Demos/RopeStabilityDemo.cs index bcd2efb31..c4e76cad0 100644 --- a/Demos/Demos/RopeStabilityDemo.cs +++ b/Demos/Demos/RopeStabilityDemo.cs @@ -31,7 +31,9 @@ public static BodyHandle[] BuildRopeBodies(Simulation simulation, in Vector3 sta { //Make the uppermost block kinematic to hold up the rest of the chain. Activity = .01f, - Collidable = ropeShapeIndex, + //Note the use of a limited speculative margin. The demo is intentionally showing an unstable configuration (naive 100:1). + //Allowing unlimited speculative margins can cause the instability to feed on itself and explode into NaNville. + Collidable = new CollidableDescription(ropeShapeIndex, 0.1f), }; for (int linkIndex = 0; linkIndex < bodyCount + 1; ++linkIndex) { @@ -84,20 +86,17 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0; //Most of this demo is concerned with working around behavioral issues without making significant changes to the simulation configuration. - //If you are willing to change the simulation configuration, a lot of the following tricks are completely unnecessary. For example, try using one of the solver substepping timesteppers. - //The following takes 4 pose integration/solver substeps per main timestep- effectively sharing collision detection and other bookkeeping over multiple frames. - //Note that the number of solver velocity iterations is dropped to 1, so each solver substep is less expensive than a regular solver execution too. + //If you are willing to change the simulation configuration, a lot of the following tricks are completely unnecessary. + //For example, try using substepping in the solver by passing "new SolveDescription(1, 4)". It'll use 4 substeps with one velocity iteration per step. + //That effectively shares collision detection and other bookkeeping over multiple solver 'frames'. See the Substepping.md documentation for more information. + //Note that the number of solver velocity iterations is dropped to 1, so each solver substep is less expensive. //We can get away with that because increasing the update rate is by far the most powerful way to stabilize a simulation. - //In fact, in particularly difficult simulations, increasing the update rate, removing other stabilizing workarounds, and reducing solver iteration counts can actually be *faster*. - //In this simulation, using 4 substeps with 1 velocity iteration each costs about 25% more than the non-substepping version with 8 velocity iterations. Not too bad for the quality increase. - //Also note that both of these simulation configurations are using a higher than demo-usual contact stiffness. That's just so that you can wrap the rope around the nearby capsule. - //In a simulation with lots of stacking, high contact stiffness would require substepping or a higher update rate for stability. - //Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(120, 1) }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new EmbeddedSubsteppingTimestepper2(4), 1); + //For most simulations, 1 substep with 8 velocity iterations is about as expensive as 4 substeps with 1 velocity iteration per. + //So, even though you can avoid the need for these kinds of hacks, it's good to know that they exist should you find yourself in a circumstance where higher update rates/substepping aren't viable. - //So, even though you can avoid the need for these kinds of hacks, it's good to know that they exist should you find yourself in a circumstance where substepping isn't viable. - Simulation = Simulation.Create(BufferPool, - new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(120, 1), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 2f }, - new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + //Also note that both of these simulation configurations are using a higher than demo-usual contact stiffness. That's just so that you can wrap the rope around the nearby capsule. + //In a simulation with lots of stacking, high contact stiffness would require substepping or a higher solve rate for stability. + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1), 20), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); rolloverInfo = new RolloverInfo(); var smallWreckingBall = new Sphere(1); @@ -266,6 +265,12 @@ bool TryCreateConstraint(int handleIndexA, int handleIndexB) public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) { rolloverInfo.Render(renderer, camera, input, text, font); + + var bottomY = renderer.Surface.Resolution.Y; + renderer.TextBatcher.Write(text.Clear().Append("Heavy objects depending on light objects through constraints can lead to instability at slow solver update rates."), new Vector2(16, bottomY - 48), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("You could just increase the solver rate by calling Timestep more often or using substepping (see SubsteppingDemo), but this demo shows some alternatives."), new Vector2(16, bottomY - 32), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Even with a poor simulation configuration, decent body/constraint configuration can still support extreme mass ratios."), new Vector2(16, bottomY - 16), 16, Vector3.One, font); + base.Render(renderer, camera, input, text, font); } From 173b4732aab0ead67249a4c04853ca3c004749a8 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 21 Jan 2022 17:11:10 -0600 Subject: [PATCH 385/947] Fixed oops in character motion constraints generator. --- Demos/Demos/Characters/CharacterMotionConstraint.tt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Demos/Demos/Characters/CharacterMotionConstraint.tt b/Demos/Demos/Characters/CharacterMotionConstraint.tt index 7596e72d6..1da30ad74 100644 --- a/Demos/Demos/Characters/CharacterMotionConstraint.tt +++ b/Demos/Demos/Characters/CharacterMotionConstraint.tt @@ -235,7 +235,7 @@ namespace Demos.Demos.Characters } - public void WarmStart2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(dynamic) {#>in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>ref <#=prefix#>CharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide velocityA<#if(dynamic) {#>, ref BodyVelocityWide velocityB<#}#>) + public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(dynamic) {#>in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>ref <#=prefix#>CharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide velocityA<#if(dynamic) {#>, ref BodyVelocityWide velocityB<#}#>) { ComputeJacobians(prestep.OffsetFromCharacter, <#if (dynamic) {#>prestep.OffsetFromSupport, <#}#>prestep.SurfaceBasis, out var basis, out var horizontalAngularJacobianA, <#if (dynamic) {#>out var horizontalAngularJacobianB, <#}#>out var verticalAngularJacobianA<#if (dynamic) {#>, out var verticalAngularJacobianB<#}#>); @@ -243,7 +243,7 @@ namespace Demos.Demos.Characters ApplyVerticalImpulse(basis, verticalAngularJacobianA, <#if (dynamic) {#>verticalAngularJacobianB, <#}#>accumulatedImpulses.Vertical, inertiaA, <#if (dynamic) {#>inertiaB, <#}#>ref velocityA<#if (dynamic) {#>, ref velocityB<#}#>); } - public void Solve2(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(dynamic) {#>in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>float dt, float inverseDt, ref <#=prefix#>CharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide velocityA<#if(dynamic) {#>, ref BodyVelocityWide velocityB<#}#>) + public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(dynamic) {#>in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>float dt, float inverseDt, ref <#=prefix#>CharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide velocityA<#if(dynamic) {#>, ref BodyVelocityWide velocityB<#}#>) { //The motion constraint is split into two parts: the horizontal constraint, and the vertical constraint. //The horizontal constraint acts almost exactly like the TangentFriction, but we'll duplicate some of the logic to keep this implementation self-contained. From ca5c04f927e610ab5bf0b175198fa9dd57552620 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 22 Jan 2022 15:07:21 -0600 Subject: [PATCH 386/947] Fixed major oops in contact constraints; twist friction no longer can use negative maximum impulses! Some types of explosion fixed. --- .../Constraints/Contact/ContactConvexTypes.cs | 4 +- .../Constraints/Contact/ContactConvexTypes.tt | 2 +- Demos/Demos/NewtDemo.cs | 49 +++++++------------ Demos/Demos/RopeStabilityDemo.cs | 6 +-- 4 files changed, 22 insertions(+), 39 deletions(-) diff --git a/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs b/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs index 975f8d433..daef19398 100644 --- a/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs +++ b/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs @@ -321,7 +321,7 @@ public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in B PenetrationLimitOneBody.Solve(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); //If there's only one contact, then the contact patch as determined by contact distance would be zero. //That can cause some subtle behavioral issues sometimes, so we approximate lever arm with the contact depth, assuming that the contact surface area will increase as the depth increases. - var maximumTwistImpulse = prestep.MaterialProperties.FrictionCoefficient * accumulatedImpulses.Penetration0 * prestep.Contact0.Depth; + var maximumTwistImpulse = prestep.MaterialProperties.FrictionCoefficient * accumulatedImpulses.Penetration0 * Vector.Max(Vector.Zero, prestep.Contact0.Depth); TwistFrictionOneBody.Solve(prestep.Normal, inertiaA, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); } } @@ -964,7 +964,7 @@ public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in B PenetrationLimit.Solve(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); //If there's only one contact, then the contact patch as determined by contact distance would be zero. //That can cause some subtle behavioral issues sometimes, so we approximate lever arm with the contact depth, assuming that the contact surface area will increase as the depth increases. - var maximumTwistImpulse = prestep.MaterialProperties.FrictionCoefficient * accumulatedImpulses.Penetration0 * prestep.Contact0.Depth; + var maximumTwistImpulse = prestep.MaterialProperties.FrictionCoefficient * accumulatedImpulses.Penetration0 * Vector.Max(Vector.Zero, prestep.Contact0.Depth); TwistFriction.Solve(prestep.Normal, inertiaA, inertiaB, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); } } diff --git a/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt b/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt index 13dd1dd20..bb75d103a 100644 --- a/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt +++ b/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt @@ -275,7 +275,7 @@ for (int i = 0; i < contactCount; ++i) <#if (contactCount == 1) {#> //If there's only one contact, then the contact patch as determined by contact distance would be zero. //That can cause some subtle behavioral issues sometimes, so we approximate lever arm with the contact depth, assuming that the contact surface area will increase as the depth increases. - var maximumTwistImpulse = <#=contactCount > 1 ? "premultipliedFrictionCoefficient" : "prestep.MaterialProperties.FrictionCoefficient"#> * accumulatedImpulses.Penetration0 * prestep.Contact0.Depth; + var maximumTwistImpulse = <#=contactCount > 1 ? "premultipliedFrictionCoefficient" : "prestep.MaterialProperties.FrictionCoefficient"#> * accumulatedImpulses.Penetration0 * Vector.Max(Vector.Zero, prestep.Contact0.Depth); <#} else {#> var maximumTwistImpulse = <#=contactCount > 1 ? "premultipliedFrictionCoefficient" : "prestep.MaterialProperties.FrictionCoefficient"#> * ( <#for (int i = 0; i < contactCount; ++i) {#> diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index 4fe63800a..329c4d852 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -2,6 +2,7 @@ using BepuPhysics.Collidables; using BepuPhysics.CollisionDetection; using BepuPhysics.Constraints; +using BepuPhysics.Constraints.Contact; using BepuUtilities; using BepuUtilities.Collections; using BepuUtilities.Memory; @@ -546,7 +547,7 @@ public DeformableCallbacks(CollidableProperty filters MinimumDistanceForSelfCollisions = minimumDistanceForSelfCollisions; } public DeformableCallbacks(CollidableProperty filters, int minimumDistanceForSelfCollisions = 3) - :this(filters, new PairMaterialProperties(1, 2, new SpringSettings(30, 1)), minimumDistanceForSelfCollisions) + : this(filters, new PairMaterialProperties(1, 2, new SpringSettings(30, 1)), minimumDistanceForSelfCollisions) { } //This slightly awkward factory is just here for the dancer demos. @@ -574,9 +575,7 @@ public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int chi [MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold { - pairMaterial.FrictionCoefficient = 1; - pairMaterial.MaximumRecoveryVelocity = 2f; - pairMaterial.SpringSettings = new SpringSettings(30, 1); + pairMaterial = Material; return true; } @@ -681,7 +680,6 @@ internal unsafe static void CreateDeformable(Simulation simulation, in Vector3 p var vertexShape = new Sphere(cellSize * 0.7f); var massPerVertex = density * (cellSize * cellSize * cellSize); var vertexInertia = vertexShape.ComputeInertia(massPerVertex); - //vertexInertia.InverseInertiaTensor = default; var vertexShapeIndex = simulation.Shapes.Add(vertexShape); for (int i = 0; i < vertices.Length; ++i) { @@ -703,17 +701,16 @@ internal unsafe static void CreateDeformable(Simulation simulation, in Vector3 p LocalOrientation = Quaternion.Identity, SpringSettings = weldSpringiness }); - //simulation.Solver.Add(vertexHandles[edge.A], vertexHandles[edge.B], - // new CenterDistanceConstraint(offset.Length(), weldSpringiness)); - //simulation.Solver.Add(vertexHandles[edge.A], vertexHandles[edge.B], - // new BallSocket { LocalOffsetA = offset * 0.5f, LocalOffsetB = offset * -0.5f, SpringSettings = weldSpringiness }); } - //for (int i = 0; i < tetrahedraVertexIndices.Length; ++i) - //{ - // ref var tetrahedron = ref tetrahedraVertexIndices[i]; - // simulation.Solver.Add(vertexHandles[tetrahedron.A], vertexHandles[tetrahedron.B], vertexHandles[tetrahedron.C], vertexHandles[tetrahedron.D], - // new VolumeConstraint(vertices[tetrahedron.A], vertices[tetrahedron.B], vertices[tetrahedron.C], vertices[tetrahedron.D], volumeSpringiness)); - //} + //Volume constraints add a fairly subtle effect, especially when dealing with already stiff weld constraints. + //They're included here as an example, but you'll notice in the PlumpDancerDemo that there are no volume constraints. + //There, we're primarily concerned about scaling up simulations to many characters, so adding tons of additional constraints for minimal behavioral difference doesn't make sense. + for (int i = 0; i < tetrahedraVertexIndices.Length; ++i) + { + ref var tetrahedron = ref tetrahedraVertexIndices[i]; + simulation.Solver.Add(vertexHandles[tetrahedron.A], vertexHandles[tetrahedron.B], vertexHandles[tetrahedron.C], vertexHandles[tetrahedron.D], + new VolumeConstraint(vertices[tetrahedron.A], vertices[tetrahedron.B], vertices[tetrahedron.C], vertices[tetrahedron.D], volumeSpringiness)); + } pool.Return(ref vertexEdgeCounts); edges.Dispose(pool); @@ -727,7 +724,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = MathHelper.Pi * 0.15f; var filters = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new DeformableCallbacks(filters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0), 0, 0), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DeformableCallbacks(filters, new PairMaterialProperties(1f, 2f, new SpringSettings(30, 1))), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0), 0, 0), new SolveDescription(1, 4)); var meshContent = content.Load("Content\\newt.obj"); float cellSize = 0.1f; @@ -735,37 +732,25 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) out var vertices, out var vertexSpatialIndices, out var cellVertexIndices, out var tetrahedraVertexIndices); var weldSpringiness = new SpringSettings(30f, 1f); var volumeSpringiness = new SpringSettings(30f, 1); - //int terboTotal = 0; - //var terboShape = new Box(0.5f, 0.5f, 3); - //terboShape.ComputeInertia(1, out var terboInertia); - //var bodyDescription = BodyDescription.CreateDynamic(new RigidPose(new Vector3(0, 10, 0)), new BodyVelocity(default, new Vector3(1, 2, 3)), terboInertia, new CollidableDescription(Simulation.Shapes.Add(terboShape), 0.1f), new BodyActivityDescription(-1)); - for (int i = 0; i < 40; ++i) + for (int i = 0; i < 8; ++i) { - //CreateDeformable(Simulation, new Vector3(i * 3, 5 + i * 1.5f, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), MathF.PI * (i * 0.55f)), 1f, cellSize, weldSpringiness, volumeSpringiness, i, filters, ref vertices, ref vertexSpatialIndices, ref cellVertexIndices, ref tetrahedraVertexIndices); - CreateDeformable(Simulation, new Vector3(i * 3, 5 + cellSize * 2f + i * 0f, 0), Quaternion.Identity, 1f, cellSize, weldSpringiness, volumeSpringiness, i, filters, ref vertices, ref vertexSpatialIndices, ref cellVertexIndices, ref tetrahedraVertexIndices); - - //for (int terbo = 0; terbo < 17; ++terbo) - //{ - // bodyDescription.Pose.Position = new Vector3((terboTotal++) * 6, 10, 3); - // Simulation.Bodies.Add(bodyDescription); - //} + CreateDeformable(Simulation, new Vector3(i * 3, 5 + i * 1.5f, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), MathF.PI * (i * 0.55f)), 1f, cellSize, weldSpringiness, volumeSpringiness, i, filters, ref vertices, ref vertexSpatialIndices, ref cellVertexIndices, ref tetrahedraVertexIndices); } //Console.WriteLine($"body count: {Simulation.Bodies.ActiveSet.Count}"); //Console.WriteLine($"constraint count: {Simulation.Solver.CountConstraints()}"); - //Simulation.Bodies.GetBodyReference(new BodyHandle(55)).Pose.Position += new Vector3(10, 5, 0); BufferPool.Return(ref vertices); vertexSpatialIndices.Dispose(BufferPool); BufferPool.Return(ref cellVertexIndices); BufferPool.Return(ref tetrahedraVertexIndices); - //Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 100, -.5f), 10, Simulation.Shapes, new Sphere(5))); + //Drop something heavy on one of the newts. The newt probably won't mind. + Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 100, -.5f), 10, Simulation.Shapes, new Sphere(5))); Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), Simulation.Shapes.Add(new Box(1500, 1, 1500)))); Simulation.Statics.Add(new StaticDescription(new Vector3(0, -1.5f, 0), Simulation.Shapes.Add(new Sphere(3)))); } - } } diff --git a/Demos/Demos/RopeStabilityDemo.cs b/Demos/Demos/RopeStabilityDemo.cs index c4e76cad0..10acf6296 100644 --- a/Demos/Demos/RopeStabilityDemo.cs +++ b/Demos/Demos/RopeStabilityDemo.cs @@ -29,14 +29,12 @@ public static BodyHandle[] BuildRopeBodies(Simulation simulation, in Vector3 sta //Build the links. var bodyDescription = new BodyDescription { - //Make the uppermost block kinematic to hold up the rest of the chain. Activity = .01f, - //Note the use of a limited speculative margin. The demo is intentionally showing an unstable configuration (naive 100:1). - //Allowing unlimited speculative margins can cause the instability to feed on itself and explode into NaNville. - Collidable = new CollidableDescription(ropeShapeIndex, 0.1f), + Collidable = ropeShapeIndex }; for (int linkIndex = 0; linkIndex < bodyCount + 1; ++linkIndex) { + //Make the uppermost block kinematic to hold up the rest of the chain. bodyDescription.LocalInertia = linkIndex == 0 ? new BodyInertia() : ropeInertia; bodyDescription.Pose = start - new Vector3(0, linkIndex * (bodySpacing + 2 * bodySize), 0); handles[linkIndex] = simulation.Bodies.Add(bodyDescription); From 8c5215b78fb27e4f1d519e118bdfab1649f46b5d Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 22 Jan 2022 19:19:19 -0600 Subject: [PATCH 387/947] SimpleThreadDispatcher moved into BepuUtilities to reduce annoyance. Now supports limited worker dispatches and low work special casing. --- BepuPhysics/BatchCompressor.cs | 2 +- .../CollidableOverlapFinder.cs | 12 +++- BepuPhysics/CollisionDetection/NarrowPhase.cs | 2 +- .../CollisionDetection/NarrowPhasePreflush.cs | 6 +- BepuPhysics/IslandAwakener.cs | 4 +- BepuPhysics/IslandSleeper.cs | 6 +- BepuPhysics/PoseIntegrator.cs | 4 +- BepuPhysics/Solver_Solve.cs | 7 ++- .../Trees/Tree_MultithreadedRefitRefine.cs | 8 +-- BepuUtilities/IThreadDispatcher.cs | 5 +- .../SimpleThreadDispatcher.cs | 61 +++++++++++++------ DemoRenderer/ParallelLooper.cs | 15 ++++- Demos/Demo.cs | 1 + Demos/Demos/Cars/CarDemo.cs | 36 +++++------ .../Demos/Characters/CharacterControllers.cs | 4 +- Demos/Program.cs | 5 +- .../IntertreeThreadingTests.cs | 2 +- Demos/SpecializedTests/RagdollTubeDemo.cs | 14 +---- 18 files changed, 115 insertions(+), 79 deletions(-) rename {Demos => BepuUtilities}/SimpleThreadDispatcher.cs (54%) diff --git a/BepuPhysics/BatchCompressor.cs b/BepuPhysics/BatchCompressor.cs index e0d7c0ebd..2f9d01a06 100644 --- a/BepuPhysics/BatchCompressor.cs +++ b/BepuPhysics/BatchCompressor.cs @@ -316,7 +316,7 @@ public void Compress(BufferPool pool, IThreadDispatcher threadDispatcher = null, { analysisJobIndex = -1; this.threadDispatcher = threadDispatcher; - threadDispatcher.DispatchWorkers(analysisWorkerDelegate); + threadDispatcher.DispatchWorkers(analysisWorkerDelegate, analysisJobs.Count); this.threadDispatcher = null; } else diff --git a/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs b/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs index 7aede53cc..e7847f87d 100644 --- a/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs +++ b/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs @@ -127,7 +127,17 @@ public override void DispatchOverlaps(float dt, IThreadDispatcher threadDispatch selfTestContext.PrepareJobs(ref broadPhase.ActiveTree, selfHandlers, threadDispatcher.ThreadCount); intertreeTestContext.PrepareJobs(ref broadPhase.ActiveTree, ref broadPhase.StaticTree, intertreeHandlers, threadDispatcher.ThreadCount); nextJobIndex = -1; - threadDispatcher.DispatchWorkers(workerAction); + var totalJobCount = selfTestContext.JobCount + intertreeTestContext.JobCount; + //We dispatch over parts of the tree are not yet analyzed, but the job creation phase may have put some work into the batcher. + //If the total job count is zero, that means there's no further work to be done (implying the tree was very tiny), but we may need to flush. + if (totalJobCount == 0) + { + narrowPhase.overlapWorkers[0].Batcher.Flush(); + } + else + { + threadDispatcher.DispatchWorkers(workerAction, totalJobCount); + } //workerAction(0); selfTestContext.CompleteSelfTest(); intertreeTestContext.CompleteTest(); diff --git a/BepuPhysics/CollisionDetection/NarrowPhase.cs b/BepuPhysics/CollisionDetection/NarrowPhase.cs index e535ee792..53086aae0 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhase.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhase.cs @@ -270,7 +270,7 @@ public void Flush(IThreadDispatcher threadDispatcher = null) { flushJobIndex = -1; this.threadDispatcher = threadDispatcher; - threadDispatcher.DispatchWorkers(flushWorkerLoop); + threadDispatcher.DispatchWorkers(flushWorkerLoop, flushJobs.Count); //flushWorkerLoop(0); this.threadDispatcher = null; } diff --git a/BepuPhysics/CollisionDetection/NarrowPhasePreflush.cs b/BepuPhysics/CollisionDetection/NarrowPhasePreflush.cs index 2ff6fabc6..b9036b3e3 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhasePreflush.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhasePreflush.cs @@ -322,7 +322,7 @@ protected override void OnPreflush(IThreadDispatcher threadDispatcher, bool dete var originalPairCacheMappingCount = PairCache.Mapping.Count; //var start = Stopwatch.GetTimestamp(); preflushJobIndex = -1; - threadDispatcher.DispatchWorkers(preflushWorkerLoop); + threadDispatcher.DispatchWorkers(preflushWorkerLoop, preflushJobs.Count); //preflushWorkerLoop(0); //var end = Stopwatch.GetTimestamp(); //Console.WriteLine($"Preflush phase 1 time (us): {1e6 * (end - start) / Stopwatch.Frequency}"); @@ -337,7 +337,7 @@ protected override void OnPreflush(IThreadDispatcher threadDispatcher, bool dete } //start = Stopwatch.GetTimestamp(); preflushJobIndex = -1; - threadDispatcher.DispatchWorkers(preflushWorkerLoop); + threadDispatcher.DispatchWorkers(preflushWorkerLoop, preflushJobs.Count); //preflushWorkerLoop(0); //end = Stopwatch.GetTimestamp(); //Console.WriteLine($"Preflush phase 2 time (us): {1e6 * (end - start) / Stopwatch.Frequency}"); @@ -361,7 +361,7 @@ protected override void OnPreflush(IThreadDispatcher threadDispatcher, bool dete FreshnessChecker.CreateJobs(threadCount, ref preflushJobs, Pool, originalPairCacheMappingCount); //start = Stopwatch.GetTimestamp(); preflushJobIndex = -1; - threadDispatcher.DispatchWorkers(preflushWorkerLoop); + threadDispatcher.DispatchWorkers(preflushWorkerLoop, preflushJobs.Count); //preflushWorkerLoop(0); //end = Stopwatch.GetTimestamp(); //Console.WriteLine($"Preflush phase 3 time (us): {1e6 * (end - start) / Stopwatch.Frequency}"); diff --git a/BepuPhysics/IslandAwakener.cs b/BepuPhysics/IslandAwakener.cs index 1f9dd70c1..7fd304497 100644 --- a/BepuPhysics/IslandAwakener.cs +++ b/BepuPhysics/IslandAwakener.cs @@ -101,7 +101,7 @@ public void AwakenSets(ref QuickList setIndices, IThreadDispatcher threadDi { this.jobIndex = -1; this.jobCount = phaseOneJobCount; - threadDispatcher.DispatchWorkers(phaseOneWorkerDelegate); + threadDispatcher.DispatchWorkers(phaseOneWorkerDelegate, phaseOneJobCount); } else { @@ -115,7 +115,7 @@ public void AwakenSets(ref QuickList setIndices, IThreadDispatcher threadDi { this.jobIndex = -1; this.jobCount = phaseTwoJobCount; - threadDispatcher.DispatchWorkers(phaseTwoWorkerDelegate); + threadDispatcher.DispatchWorkers(phaseTwoWorkerDelegate, phaseTwoJobCount); } else { diff --git a/BepuPhysics/IslandSleeper.cs b/BepuPhysics/IslandSleeper.cs index 024552914..c17e03621 100644 --- a/BepuPhysics/IslandSleeper.cs +++ b/BepuPhysics/IslandSleeper.cs @@ -793,7 +793,7 @@ void DisposeWorkerTraversalResults() jobIndex = -1; if (threadCount > 1) { - threadDispatcher.DispatchWorkers(gatherDelegate); + threadDispatcher.DispatchWorkers(gatherDelegate, gatheringJobs.Count); } else { @@ -821,7 +821,7 @@ void DisposeWorkerTraversalResults() jobIndex = -1; if (threadCount > 1) { - threadDispatcher.DispatchWorkers(executeRemovalWorkDelegate); + threadDispatcher.DispatchWorkers(executeRemovalWorkDelegate, removalJobs.Count); } else { @@ -844,7 +844,7 @@ void DisposeWorkerTraversalResults() jobIndex = -1; if (threadCount > 1) { - threadDispatcher.DispatchWorkers(typeBatchConstraintRemovalDelegate); + threadDispatcher.DispatchWorkers(typeBatchConstraintRemovalDelegate, typeBatchConstraintRemovalJobCount); //typeBatchConstraintRemovalDelegate(0); } else diff --git a/BepuPhysics/PoseIntegrator.cs b/BepuPhysics/PoseIntegrator.cs index 37da29755..25338dcc3 100644 --- a/BepuPhysics/PoseIntegrator.cs +++ b/BepuPhysics/PoseIntegrator.cs @@ -430,7 +430,7 @@ public void PredictBoundingBoxes(float dt, BufferPool pool, IThreadDispatcher th { PrepareForMultithreadedExecution(BundleIndexing.GetBundleCount(bodies.ActiveSet.Count), dt, threadDispatcher.ThreadCount); this.threadDispatcher = threadDispatcher; - threadDispatcher.DispatchWorkers(predictBoundingBoxesWorker); + threadDispatcher.DispatchWorkers(predictBoundingBoxesWorker, availableJobCount); //predictBoundingBoxesWorker(0); this.threadDispatcher = null; } @@ -704,7 +704,7 @@ public void IntegrateAfterSubstepping(IndexSet constrainedBodies, float dt, int PrepareForMultithreadedExecution(BundleIndexing.GetBundleCount(bodies.ActiveSet.Count), dt, threadDispatcher.ThreadCount, substepCount); this.constrainedBodies = constrainedBodies; this.threadDispatcher = threadDispatcher; - threadDispatcher.DispatchWorkers(integrateAfterSubsteppingWorker); + threadDispatcher.DispatchWorkers(integrateAfterSubsteppingWorker, availableJobCount); this.threadDispatcher = null; this.constrainedBodies = default; } diff --git a/BepuPhysics/Solver_Solve.cs b/BepuPhysics/Solver_Solve.cs index 939e116bb..947fd4f06 100644 --- a/BepuPhysics/Solver_Solve.cs +++ b/BepuPhysics/Solver_Solve.cs @@ -819,6 +819,7 @@ protected void ExecuteMultithreaded(float dt, IThreadDispatcher threadDispatcher //Warm start. var preambleClaimCount = incrementalBlocks.Count + substepContext.KinematicIntegrationBlocks.Length; int claimStart = preambleClaimCount; + int highestJobCountInSolve = 0; for (int batchIndex = 0; batchIndex < stagesPerIteration; ++batchIndex) { var stageIndex = targetStageIndex++; @@ -826,6 +827,7 @@ protected void ExecuteMultithreaded(float dt, IThreadDispatcher threadDispatcher var workBlocksInBatch = substepContext.ConstraintBatchBoundaries[batchIndex] - batchStart; substepContext.Stages[stageIndex] = new(claims.Slice(claimStart, workBlocksInBatch), batchStart, SolverStageType.WarmStart, batchIndex); claimStart += workBlocksInBatch; + highestJobCountInSolve = Math.Max(highestJobCountInSolve, workBlocksInBatch); } for (int iterationIndex = 0; iterationIndex < substepContext.HighestVelocityIterationCount; ++iterationIndex) { @@ -838,6 +840,7 @@ protected void ExecuteMultithreaded(float dt, IThreadDispatcher threadDispatcher var workBlocksInBatch = substepContext.ConstraintBatchBoundaries[batchIndex] - batchStart; substepContext.Stages[stageIndex] = new(claims.Slice(claimStart, workBlocksInBatch), batchStart, SolverStageType.Solve, batchIndex); claimStart += workBlocksInBatch; + highestJobCountInSolve = Math.Max(highestJobCountInSolve, workBlocksInBatch); } } @@ -867,7 +870,7 @@ protected void ExecuteMultithreaded(float dt, IThreadDispatcher threadDispatcher if (ActiveSet.Batches.Count > 0) { //workDelegate(0); - threadDispatcher.DispatchWorkers(workDelegate); + threadDispatcher.DispatchWorkers(workDelegate, highestJobCountInSolve); } //pool.Take(syncCount, out var availableCountPerSync); @@ -1241,7 +1244,7 @@ public override unsafe IndexSet PrepareConstraintIntegrationResponsibilities(ITh // ref var job = ref integrationResponsibilityPrepassJobs[i]; // jobAlignedIntegrationResponsibilities[i] = ComputeIntegrationResponsibilitiesForConstraintRegion(job.batch, job.typeBatch, job.start, job.end); //} - threadDispatcher.DispatchWorkers(constraintIntegrationResponsibilitiesWorker); + threadDispatcher.DispatchWorkers(constraintIntegrationResponsibilitiesWorker, integrationResponsibilityPrepassJobs.Count); //Coarse batch integration responsibilities start uninitialized. Possible to have multiple jobs per type batch in multithreaded case, so we need to init to merge. for (int i = 1; i < ActiveSet.Batches.Count; ++i) diff --git a/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs b/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs index 6d56d7a3e..07b786cd4 100644 --- a/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs +++ b/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs @@ -76,7 +76,7 @@ public unsafe void RefitAndRefine(ref Tree tree, BufferPool pool, IThreadDispatc pool, threadDispatcher.GetThreadMemoryPool(0)); RefitNodeIndex = -1; - threadDispatcher.DispatchWorkers(RefitAndMarkAction); + threadDispatcher.DispatchWorkers(RefitAndMarkAction, RefitNodes.Count); //Condense the set of candidates into a set of targets. int refinementCandidatesCount = 0; for (int i = 0; i < threadDispatcher.ThreadCount; ++i) @@ -118,7 +118,7 @@ public unsafe void RefitAndRefine(ref Tree tree, BufferPool pool, IThreadDispatc } RefineIndex = -1; - threadDispatcher.DispatchWorkers(RefineAction); + threadDispatcher.DispatchWorkers(RefineAction, RefinementTargets.Count); //Note that we defer the refine flag clear until after the refinements complete. If we did it within the refine action itself, //it would introduce nondeterminism by allowing refines to progress based on their order of completion. for (int i = 0; i < RefinementTargets.Count; ++i) @@ -130,7 +130,7 @@ public unsafe void RefitAndRefine(ref Tree tree, BufferPool pool, IThreadDispatc //Note that more cache optimization is required with more threads, since spreading it out more slightly lessens its effectiveness. var cacheOptimizeCount = Tree.GetCacheOptimizeTuning(MaximumSubtrees, RefitCostChange, (Math.Max(1, threadDispatcher.ThreadCount * 0.25f)) * cacheOptimizeAggressivenessScale); - var cacheOptimizationTasks = threadDispatcher.ThreadCount * 2; + var cacheOptimizationTasks = threadDispatcher.ThreadCount; PerWorkerCacheOptimizeCount = cacheOptimizeCount / cacheOptimizationTasks; var startIndex = (int)(((long)frameIndex * PerWorkerCacheOptimizeCount) % Tree.nodeCount); CacheOptimizeStarts = new QuickList(cacheOptimizationTasks, pool); @@ -157,7 +157,7 @@ public unsafe void RefitAndRefine(ref Tree tree, BufferPool pool, IThreadDispatc CacheOptimizeStarts.AddUnsafely(startIndex); } - threadDispatcher.DispatchWorkers(CacheOptimizeAction); + threadDispatcher.DispatchWorkers(CacheOptimizeAction, CacheOptimizeStarts.Count); for (int i = 0; i < threadDispatcher.ThreadCount; ++i) { diff --git a/BepuUtilities/IThreadDispatcher.cs b/BepuUtilities/IThreadDispatcher.cs index 0c5d01ad9..eb8305d02 100644 --- a/BepuUtilities/IThreadDispatcher.cs +++ b/BepuUtilities/IThreadDispatcher.cs @@ -27,8 +27,9 @@ public interface IThreadDispatcher /// /// Dispatches all the available workers. /// - /// Delegate to be invoked on for every worker. - void DispatchWorkers(Action workerBody); + /// Delegate to be invoked on every worker. + /// Maximum number of workers to dispatch. + void DispatchWorkers(Action workerBody, int maximumWorkerCount = int.MaxValue); /// /// Gets the memory pool associated with a given worker index. It is guaranteed that no other workers will share the same pool for the duration of the worker's execution. diff --git a/Demos/SimpleThreadDispatcher.cs b/BepuUtilities/SimpleThreadDispatcher.cs similarity index 54% rename from Demos/SimpleThreadDispatcher.cs rename to BepuUtilities/SimpleThreadDispatcher.cs index 15936558a..55e07bc2e 100644 --- a/Demos/SimpleThreadDispatcher.cs +++ b/BepuUtilities/SimpleThreadDispatcher.cs @@ -1,15 +1,19 @@ -using BepuPhysics; -using System; +using System; using System.Diagnostics; using System.Threading; using BepuUtilities.Memory; -using BepuUtilities; -namespace Demos +namespace BepuUtilities { + /// + /// Provides a simple implementation. Not reentrant. + /// public class SimpleThreadDispatcher : IThreadDispatcher, IDisposable { int threadCount; + /// + /// Gets the number of threads to dispatch work on. + /// public int ThreadCount => threadCount; struct Worker { @@ -22,6 +26,10 @@ struct Worker BufferPool[] bufferPools; + /// + /// Creates a new simple thread dispatcher with the given number of threads. + /// + /// Number of threads to dispatch on each invocation. public SimpleThreadDispatcher(int threadCount) { this.threadCount = threadCount; @@ -45,7 +53,7 @@ void DispatchThread(int workerIndex) Debug.Assert(workerBody != null); workerBody(workerIndex); - if (Interlocked.Increment(ref completedWorkerCounter) == threadCount) + if (Interlocked.Decrement(ref remainingWorkerCounter) == -1) { finished.Set(); } @@ -53,7 +61,7 @@ void DispatchThread(int workerIndex) volatile Action workerBody; int workerIndex; - int completedWorkerCounter; + int remainingWorkerCounter; void WorkerLoop(object untypedSignal) { @@ -67,34 +75,49 @@ void WorkerLoop(object untypedSignal) } } - void SignalThreads() + void SignalThreads(int maximumWorkerCount) { - for (int i = 0; i < workers.Length; ++i) + //Worker 0 is not signalled; it's the executing thread. + //So if we want 4 total executing threads, we should signal 3 workers. + int maximumWorkersToSignal = maximumWorkerCount - 1; + var workersToSignal = maximumWorkersToSignal < workers.Length ? maximumWorkersToSignal : workers.Length; + remainingWorkerCounter = workersToSignal; + for (int i = 0; i < workersToSignal; ++i) { workers[i].Signal.Set(); } } - public void DispatchWorkers(Action workerBody) + public void DispatchWorkers(Action workerBody, int maximumWorkerCount = int.MaxValue) { - Debug.Assert(this.workerBody == null); - workerIndex = 1; //Just make the inline thread worker 0. While the other threads might start executing first, the user should never rely on the dispatch order. - completedWorkerCounter = 0; - this.workerBody = workerBody; - SignalThreads(); - //Calling thread does work. No reason to spin up another worker and block this one! - DispatchThread(0); - finished.WaitOne(); - this.workerBody = null; + if (maximumWorkerCount > 1) + { + Debug.Assert(this.workerBody == null); + workerIndex = 1; //Just make the inline thread worker 0. While the other threads might start executing first, the user should never rely on the dispatch order. + this.workerBody = workerBody; + SignalThreads(maximumWorkerCount); + //Calling thread does work. No reason to spin up another worker and block this one! + DispatchThread(0); + finished.WaitOne(); + this.workerBody = null; + } + else if (maximumWorkerCount == 1) + { + workerBody(0); + } } volatile bool disposed; + + /// + /// Waits for all pending work to complete and then disposes all workers. + /// public void Dispose() { if (!disposed) { disposed = true; - SignalThreads(); + SignalThreads(threadCount); for (int i = 0; i < bufferPools.Length; ++i) { bufferPools[i].Clear(); diff --git a/DemoRenderer/ParallelLooper.cs b/DemoRenderer/ParallelLooper.cs index eeaddd3a6..8b5743e22 100644 --- a/DemoRenderer/ParallelLooper.cs +++ b/DemoRenderer/ParallelLooper.cs @@ -20,13 +20,17 @@ namespace DemoRenderer public delegate void LooperWorkerDone(int workerIndex); /// - /// Simple multithreaded for loop provider built on an IThreadDispatcher. Performs an atomic operation for every object in the loop, so pre-chunking the works into jobs is important. + /// Simple multithreaded for loop provider built on an . Performs an atomic operation for every object in the loop, so pre-chunking the works into jobs is important. /// /// This helps avoid some unnecessary allocations associated with the TPL implementation. While a little garbage from the renderer in the demos isn't exactly a catastrophe, /// having zero allocations under normal execution makes it easier to notice when the physics simulation itself is allocating inappropriately. public class ParallelLooper { Action dispatcherWorker; + + /// + /// Gets or sets the dispatcher used by the looper. + /// public IThreadDispatcher Dispatcher { get; set; } public ParallelLooper() @@ -51,6 +55,13 @@ void Worker(int workerIndex) LooperAction iteration; LooperWorkerDone workerDone; + /// + /// Executes an action for each index in the given range. + /// + /// Inclusive start index of the execution range. + /// Exclusive end index of the execution range. + /// Delegate to invoke for each index. + /// Delegate to invoke after all workers are done. public void For(int start, int exclusiveEnd, LooperAction workAction, LooperWorkerDone workerDone = null) { if (Dispatcher == null) @@ -67,7 +78,7 @@ public void For(int start, int exclusiveEnd, LooperAction workAction, LooperWork this.end = exclusiveEnd; this.iteration = workAction; this.workerDone = workerDone; - Dispatcher.DispatchWorkers(dispatcherWorker); + Dispatcher.DispatchWorkers(dispatcherWorker, exclusiveEnd - start); this.iteration = null; this.workerDone = workerDone; } diff --git a/Demos/Demo.cs b/Demos/Demo.cs index 152c5988c..b12629b3c 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -5,6 +5,7 @@ using System; using DemoRenderer.UI; using DemoContentLoader; +using BepuUtilities; namespace Demos { diff --git a/Demos/Demos/Cars/CarDemo.cs b/Demos/Demos/Cars/CarDemo.cs index ba51eedd8..3c5f56193 100644 --- a/Demos/Demos/Cars/CarDemo.cs +++ b/Demos/Demos/Cars/CarDemo.cs @@ -139,25 +139,27 @@ public override void Initialize(ContentArchive content, Camera camera) bool playerControlActive = true; public override void Update(Window window, Camera camera, Input input, float dt) { - if (input.WasPushed(ToggleCar)) - playerControlActive = !playerControlActive; - //For control purposes, we'll match the fixed update rate of the simulation. Could decouple it- this dt isn't - //vulnerable to the same instabilities as the simulation itself with variable durations. - const float controlDt = TimestepDuration; - if (playerControlActive) + if (input != null) { - float steeringSum = 0; - if (input.IsDown(Left)) - { - steeringSum += 1; - } - if (input.IsDown(Right)) + if (input.WasPushed(ToggleCar)) + playerControlActive = !playerControlActive; + if (playerControlActive) { - steeringSum -= 1; + float steeringSum = 0; + if (input.IsDown(Left)) + { + steeringSum += 1; + } + if (input.IsDown(Right)) + { + steeringSum -= 1; + } + var targetSpeedFraction = input.IsDown(Forward) ? 1f : input.IsDown(Backward) ? -1f : 0; + var zoom = input.IsDown(Zoom); + //For control purposes, we'll match the fixed update rate of the simulation. Could decouple it- this dt isn't + //vulnerable to the same instabilities as the simulation itself with variable durations. + playerController.Update(Simulation, TimestepDuration, steeringSum, targetSpeedFraction, zoom, input.IsDown(Brake) || input.IsDown(BrakeAlternate)); } - var targetSpeedFraction = input.IsDown(Forward) ? 1f : input.IsDown(Backward) ? -1f : 0; - var zoom = input.IsDown(Zoom); - playerController.Update(Simulation, controlDt, steeringSum, targetSpeedFraction, zoom, input.IsDown(Brake) || input.IsDown(BrakeAlternate)); } for (int i = 0; i < aiControllers.Length; ++i) @@ -185,7 +187,7 @@ public override void Update(Window window, Camera camera, Input input, float dt) var speedFraction = 0.25f + MathF.Min(0.75f, MathF.Max(0, 0.75f * (MathF.Abs(steeringAngle) - 0.2f) / -0.4f)); if (orientation.Y.Y < 0.4f) speedFraction = 0; - ai.Controller.Update(Simulation, controlDt, steeringAngle, speedFraction, steeringAngle < 0.05f, steeringAngle > MathF.PI * 0.2f && forwardVelocity > ai.Controller.ForwardSpeed * 0.6f); + ai.Controller.Update(Simulation, TimestepDuration, steeringAngle, speedFraction, steeringAngle < 0.05f, steeringAngle > MathF.PI * 0.2f && forwardVelocity > ai.Controller.ForwardSpeed * 0.6f); } base.Update(window, camera, input, dt); diff --git a/Demos/Demos/Characters/CharacterControllers.cs b/Demos/Demos/Characters/CharacterControllers.cs index be2f7dc6d..cddcc2647 100644 --- a/Demos/Demos/Characters/CharacterControllers.cs +++ b/Demos/Demos/Characters/CharacterControllers.cs @@ -488,7 +488,7 @@ void PrepareForContacts(float dt, IThreadDispatcher threadDispatcher = null) } boundingBoxExpansionJobIndex = -1; - threadDispatcher.DispatchWorkers(expandBoundingBoxesWorker); + threadDispatcher.DispatchWorkers(expandBoundingBoxesWorker, boundingBoxExpansionJobs.Length); pool.Return(ref boundingBoxExpansionJobs); } @@ -784,7 +784,7 @@ void AnalyzeContacts(float dt, IThreadDispatcher threadDispatcher) previousEnd = job.ExclusiveEnd; } analysisJobIndex = -1; - threadDispatcher.DispatchWorkers(analyzeContactsWorker); + threadDispatcher.DispatchWorkers(analyzeContactsWorker, analysisJobCount); pool.Return(ref jobs); } } diff --git a/Demos/Program.cs b/Demos/Program.cs index ddd1a8cd7..189962aa9 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -1,8 +1,5 @@ -using BepuPhysics; -using BepuUtilities; +using BepuUtilities; using DemoContentLoader; -using Demos.Demos; -using Demos.SpecializedTests; using DemoUtilities; using OpenTK; diff --git a/Demos/SpecializedTests/IntertreeThreadingTests.cs b/Demos/SpecializedTests/IntertreeThreadingTests.cs index 3d54fd4c0..cbd237dcb 100644 --- a/Demos/SpecializedTests/IntertreeThreadingTests.cs +++ b/Demos/SpecializedTests/IntertreeThreadingTests.cs @@ -100,7 +100,7 @@ static void TestTrees(BufferPool pool, IThreadDispatcher threadDispatcher, Rando handlers[i].Pairs = new List<(int a, int b)>(); } context.PrepareJobs(ref treeA, ref treeB, handlers, threadDispatcher.ThreadCount); - threadDispatcher.DispatchWorkers(context.PairTest); + threadDispatcher.DispatchWorkers(context.PairTest, context.JobCount); context.CompleteTest(); List<(int a, int b)> multithreadedResults = new List<(int, int)>(); for (int i = 0; i < threadDispatcher.ThreadCount; ++i) diff --git a/Demos/SpecializedTests/RagdollTubeDemo.cs b/Demos/SpecializedTests/RagdollTubeDemo.cs index 6a1ae1979..4aa8fc61b 100644 --- a/Demos/SpecializedTests/RagdollTubeDemo.cs +++ b/Demos/SpecializedTests/RagdollTubeDemo.cs @@ -56,7 +56,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) builder.AddForKinematic(Simulation.Shapes.Add(new Box(1, 2, panelShape.Length)), new Vector3(0, tubeRadius - 1, 0), 0); builder.BuildKinematicCompound(out var children); var compound = new BigCompound(children, Simulation.Shapes, BufferPool); - tubeHandle = Simulation.Bodies.Add(BodyDescription.CreateKinematic(tubeCenter, (default, new Vector3(0, 0, .25f)), Simulation.Shapes.Add(compound), 0f)); + var tubeHandle = Simulation.Bodies.Add(BodyDescription.CreateKinematic(tubeCenter, (default, new Vector3(0, 0, .25f)), Simulation.Shapes.Add(compound), 0f)); filters[tubeHandle] = new SubgroupCollisionFilter(int.MaxValue); builder.Dispose(); @@ -65,18 +65,6 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var staticDescription = new StaticDescription(new Vector3(0, -0.5f, 0), staticShapeIndex); Simulation.Statics.Add(staticDescription); } - - BodyHandle tubeHandle; - - //public override void Update(Window window, Camera camera, Input input, float dt) - //{ - // base.Update(window, camera, input, dt); - - // Console.WriteLine($"Constraint count: {Simulation.Solver.CountConstraints()}"); - - // Console.WriteLine($"Constraints affecting tube: {Simulation.Bodies.GetBodyReference(tubeHandle).Constraints.Count}"); - //} - } } From 6695003f16252537f5a91748b4bfe2378ededf5b Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 22 Jan 2022 19:21:45 -0600 Subject: [PATCH 388/947] Fountain stress test moved into SpecializedTests. --- DemoTests/TestUtilities.cs | 2 +- Demos/{Demos => SpecializedTests}/FountainStressTestDemo.cs | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) rename Demos/{Demos => SpecializedTests}/FountainStressTestDemo.cs (99%) diff --git a/DemoTests/TestUtilities.cs b/DemoTests/TestUtilities.cs index a381fbfc1..111a638ae 100644 --- a/DemoTests/TestUtilities.cs +++ b/DemoTests/TestUtilities.cs @@ -10,7 +10,7 @@ public static class TestUtilities { public static ContentArchive GetDemosContentArchive() { - using (var stream = typeof(Demos.Demos.FountainStressTestDemo).Assembly.GetManifestResourceStream("Demos.Demos.contentarchive")) + using (var stream = typeof(Demos.SpecializedTests.FountainStressTestDemo).Assembly.GetManifestResourceStream("Demos.Demos.contentarchive")) { return ContentArchive.Load(stream); } diff --git a/Demos/Demos/FountainStressTestDemo.cs b/Demos/SpecializedTests/FountainStressTestDemo.cs similarity index 99% rename from Demos/Demos/FountainStressTestDemo.cs rename to Demos/SpecializedTests/FountainStressTestDemo.cs index de2de2b17..ae3d3aa64 100644 --- a/Demos/Demos/FountainStressTestDemo.cs +++ b/Demos/SpecializedTests/FountainStressTestDemo.cs @@ -10,7 +10,7 @@ using System.Diagnostics; using DemoContentLoader; -namespace Demos.Demos +namespace Demos.SpecializedTests { public class FountainStressTestDemo : Demo { From efeccd443fde93948748cb67e22744f581ed8b56 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 22 Jan 2022 20:27:48 -0600 Subject: [PATCH 389/947] Disabled broad phase cache optimizers; no longer a net win on modern manycore systems. Future tree/broadphase updates should implicitly cache optimize anyway. --- .../Trees/Tree_MultithreadedRefitRefine.cs | 56 +------------------ .../Trees/Tree_RefinementScheduling.cs | 28 +--------- Demos/Demos/Cars/CarDemo.cs | 2 +- Demos/Demos/ColosseumDemo.cs | 2 +- 4 files changed, 5 insertions(+), 83 deletions(-) diff --git a/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs b/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs index 07b786cd4..e8784ca9f 100644 --- a/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs +++ b/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs @@ -32,25 +32,20 @@ public class RefitAndRefineMultithreadedContext int MaximumSubtrees; Action RefineAction; - QuickList CacheOptimizeStarts; - int PerWorkerCacheOptimizeCount; - Action CacheOptimizeAction; - IThreadDispatcher threadDispatcher; public RefitAndRefineMultithreadedContext() { RefitAndMarkAction = RefitAndMark; RefineAction = Refine; - CacheOptimizeAction = CacheOptimize; } public unsafe void RefitAndRefine(ref Tree tree, BufferPool pool, IThreadDispatcher threadDispatcher, int frameIndex, - float refineAggressivenessScale = 1, float cacheOptimizeAggressivenessScale = 1) + float refineAggressivenessScale = 1) { if (tree.leafCount <= 2) { - //If there are 2 or less leaves, then refit/refine/cache optimize doesn't do anything at all. + //If there are 2 or less leaves, then refit/refine doesn't do anything at all. //(The root node has no parent, so it does not have a bounding box, and the SAH won't change no matter how we swap the children of the root.) //Avoiding this case also gives the other codepath a guarantee that it will be working with nodes with two children. return; @@ -126,39 +121,6 @@ public unsafe void RefitAndRefine(ref Tree tree, BufferPool pool, IThreadDispatc Tree.Metanodes[RefinementTargets[i]].RefineFlag = 0; } - //To multithread this, give each worker a contiguous chunk of nodes. You want to do the biggest chunks possible to chain decent cache behavior as far as possible. - //Note that more cache optimization is required with more threads, since spreading it out more slightly lessens its effectiveness. - var cacheOptimizeCount = Tree.GetCacheOptimizeTuning(MaximumSubtrees, RefitCostChange, (Math.Max(1, threadDispatcher.ThreadCount * 0.25f)) * cacheOptimizeAggressivenessScale); - - var cacheOptimizationTasks = threadDispatcher.ThreadCount; - PerWorkerCacheOptimizeCount = cacheOptimizeCount / cacheOptimizationTasks; - var startIndex = (int)(((long)frameIndex * PerWorkerCacheOptimizeCount) % Tree.nodeCount); - CacheOptimizeStarts = new QuickList(cacheOptimizationTasks, pool); - CacheOptimizeStarts.AddUnsafely(startIndex); - - var optimizationSpacing = Tree.nodeCount / threadDispatcher.ThreadCount; - var optimizationSpacingWithExtra = optimizationSpacing + 1; - var optimizationRemainder = Tree.nodeCount - optimizationSpacing * threadDispatcher.ThreadCount; - - for (int i = 1; i < cacheOptimizationTasks; ++i) - { - if (optimizationRemainder > 0) - { - startIndex += optimizationSpacingWithExtra; - --optimizationRemainder; - } - else - { - startIndex += optimizationSpacing; - } - if (startIndex >= Tree.nodeCount) - startIndex -= Tree.nodeCount; - Debug.Assert(startIndex >= 0 && startIndex < Tree.nodeCount); - CacheOptimizeStarts.AddUnsafely(startIndex); - } - - threadDispatcher.DispatchWorkers(CacheOptimizeAction, CacheOptimizeStarts.Count); - for (int i = 0; i < threadDispatcher.ThreadCount; ++i) { //Note the use of the thread memory pool. Each thread allocated their own memory for the list since resizes were possible. @@ -167,7 +129,6 @@ public unsafe void RefitAndRefine(ref Tree tree, BufferPool pool, IThreadDispatc pool.Return(ref RefinementCandidates); RefitNodes.Dispose(pool); RefinementTargets.Dispose(pool); - CacheOptimizeStarts.Dispose(pool); Tree = default; this.threadDispatcher = null; } @@ -356,19 +317,6 @@ unsafe void Refine(int workerIndex) } - - void CacheOptimize(int workerIndex) - { - var startIndex = CacheOptimizeStarts[workerIndex]; - - //We could wrap around. But we could also not do that because it doesn't really matter! - var end = Math.Min(Tree.nodeCount, startIndex + PerWorkerCacheOptimizeCount); - for (int i = startIndex; i < end; ++i) - { - Tree.IncrementalCacheOptimizeThreadSafe(i); - } - - } } unsafe void CheckForRefinementOverlaps(int nodeIndex, ref QuickList refinementTargets) diff --git a/BepuPhysics/Trees/Tree_RefinementScheduling.cs b/BepuPhysics/Trees/Tree_RefinementScheduling.cs index a60d4cc92..1f0f9b2ce 100644 --- a/BepuPhysics/Trees/Tree_RefinementScheduling.cs +++ b/BepuPhysics/Trees/Tree_RefinementScheduling.cs @@ -178,21 +178,7 @@ readonly void GetRefineTuning(int frameIndex, int refinementCandidatesCount, flo targetRefinementCount = Math.Min(refinementCandidatesCount, (int)targetRefinementScale); } - public int GetCacheOptimizeTuning(int maximumSubtrees, float costChange, float cacheOptimizeAggressivenessScale) - { - //TODO: Using cost change as the heuristic for cache optimization isn't a great idea. They don't always or even frequently correlate. - //The best heuristic would be directly measuring the degree of adjacency. We could do that in the refit. I'm not addressing this yet - //because there's a good chance the cache optimization approach will change significantly (for example, refit outputting into a new tree with heuristically perfect layout). - var cacheOptimizeAggressiveness = Math.Max(0, costChange * cacheOptimizeAggressivenessScale); - float cacheOptimizePortion = Math.Min(1, 0.03f + 85f * (maximumSubtrees / (float)leafCount) * cacheOptimizeAggressiveness); - //float cacheOptimizePortion = Math.Min(1, 0.03f + cacheOptimizeAggressiveness * 0.5f); - //Console.WriteLine($"cache optimization portion: {cacheOptimizePortion}"); - return (int)Math.Ceiling(cacheOptimizePortion * nodeCount); - } - - - - public unsafe void RefitAndRefine(BufferPool pool, int frameIndex, float refineAggressivenessScale = 1, float cacheOptimizeAggressivenessScale = 1) + public unsafe void RefitAndRefine(BufferPool pool, int frameIndex, float refineAggressivenessScale = 1) { //Don't proceed if the tree has no refitting or refinement required. This also guarantees that any nodes that do exist have two children. if (leafCount <= 2) @@ -250,18 +236,6 @@ public unsafe void RefitAndRefine(BufferPool pool, int frameIndex, float refineA subtreeReferences.Dispose(pool); treeletInternalNodes.Dispose(pool); refinementTargets.Dispose(pool); - - var cacheOptimizeCount = GetCacheOptimizeTuning(maximumSubtrees, costChange, cacheOptimizeAggressivenessScale); - - var startIndex = (int)(((long)frameIndex * cacheOptimizeCount) % nodeCount); - - //We could wrap around. But we could also not do that because it doesn't really matter! - var end = Math.Min(NodeCount, startIndex + cacheOptimizeCount); - for (int i = startIndex; i < end; ++i) - { - IncrementalCacheOptimize(i); - } - } diff --git a/Demos/Demos/Cars/CarDemo.cs b/Demos/Demos/Cars/CarDemo.cs index 3c5f56193..d4cc8ae8c 100644 --- a/Demos/Demos/Cars/CarDemo.cs +++ b/Demos/Demos/Cars/CarDemo.cs @@ -42,7 +42,7 @@ public override void Initialize(ContentArchive content, Camera camera) var properties = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new CarCallbacks() { Properties = properties }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new CarCallbacks() { Properties = properties }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(6, 1)); var builder = new CompoundBuilder(BufferPool, Simulation.Shapes, 2); builder.Add(new Box(1.85f, 0.7f, 4.73f), RigidPose.Identity, 10); diff --git a/Demos/Demos/ColosseumDemo.cs b/Demos/Demos/ColosseumDemo.cs index 588c7c0d6..8516bc5cb 100644 --- a/Demos/Demos/ColosseumDemo.cs +++ b/Demos/Demos/ColosseumDemo.cs @@ -64,7 +64,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = MathHelper.Pi * 0.2f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); var ringBoxShape = new Box(0.5f, 1, 3); var boxDescription = BodyDescription.CreateDynamic(new Vector3(), ringBoxShape.ComputeInertia(1), Simulation.Shapes.Add(ringBoxShape), 0.01f); From 481565b64b731d655d808cc9db47e01bf4a27f19 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 24 Jan 2022 17:10:13 -0600 Subject: [PATCH 390/947] A magic trick in two parts; part one: --- BepuPhysics/PoseIntegrator.cs | 1 - BepuUtilities/SimpleThreadDispatcher2.cs | 137 ++++++++++++++ BepuUtilities/SimpleThreadDispatcher3.cs | 141 +++++++++++++++ BepuUtilities/SimpleThreadDispatcher4.cs | 146 +++++++++++++++ BepuUtilities/ThreadDispatcher.cs | 149 +++++++++++++++ BepuUtilities/ThreadDispatcher2.cs | 169 ++++++++++++++++++ BepuUtilities/ThreadDispatcher3.cs | 142 +++++++++++++++ BepuUtilities/ThreadDispatcher4.cs | 157 ++++++++++++++++ BepuUtilities/ThreadDispatcher5.cs | 156 ++++++++++++++++ BepuUtilities/ThreadDispatcher6.cs | 137 ++++++++++++++ BepuUtilities/ThreadDispatcher7.cs | 134 ++++++++++++++ Demos/Demo.cs | 4 +- Demos/Demos/RayCastingDemo.cs | 2 +- Demos/Demos/SimpleSelfContainedDemo.cs | 2 +- Demos/Program.cs | 43 +++++ .../IntertreeThreadingTests.cs | 2 +- Demos/SpecializedTests/TreeTest.cs | 2 +- Demos/SpecializedTests/VolumeQueryTests.cs | 2 +- 18 files changed, 1518 insertions(+), 8 deletions(-) create mode 100644 BepuUtilities/SimpleThreadDispatcher2.cs create mode 100644 BepuUtilities/SimpleThreadDispatcher3.cs create mode 100644 BepuUtilities/SimpleThreadDispatcher4.cs create mode 100644 BepuUtilities/ThreadDispatcher.cs create mode 100644 BepuUtilities/ThreadDispatcher2.cs create mode 100644 BepuUtilities/ThreadDispatcher3.cs create mode 100644 BepuUtilities/ThreadDispatcher4.cs create mode 100644 BepuUtilities/ThreadDispatcher5.cs create mode 100644 BepuUtilities/ThreadDispatcher6.cs create mode 100644 BepuUtilities/ThreadDispatcher7.cs diff --git a/BepuPhysics/PoseIntegrator.cs b/BepuPhysics/PoseIntegrator.cs index 25338dcc3..0e25c17cd 100644 --- a/BepuPhysics/PoseIntegrator.cs +++ b/BepuPhysics/PoseIntegrator.cs @@ -392,7 +392,6 @@ bool TryGetJob(int maximumJobInterval, out int start, out int exclusiveEnd) exclusiveEnd = start + jobSize; if (exclusiveEnd > maximumJobInterval) exclusiveEnd = maximumJobInterval; - Debug.Assert(exclusiveEnd > start, "Jobs that would involve bundles beyond the body count should not be created."); return true; } diff --git a/BepuUtilities/SimpleThreadDispatcher2.cs b/BepuUtilities/SimpleThreadDispatcher2.cs new file mode 100644 index 000000000..fed650488 --- /dev/null +++ b/BepuUtilities/SimpleThreadDispatcher2.cs @@ -0,0 +1,137 @@ +using System; +using System.Diagnostics; +using System.Threading; +using BepuUtilities.Memory; + +namespace BepuUtilities +{ + /// + /// Provides a implementation. Not reentrant. + /// + public class SimpleThreadDispatcher2 : IThreadDispatcher, IDisposable + { + int threadCount; + /// + /// Gets the number of threads to dispatch work on. + /// + public int ThreadCount => threadCount; + struct Worker + { + public Thread Thread; + public AutoResetEvent Signal; + } + + Worker[] workers; + AutoResetEvent finished; + + BufferPool[] bufferPools; + + /// + /// Creates a new thread dispatcher with the given number of threads. + /// + /// Number of threads to dispatch on each invocation. + public SimpleThreadDispatcher2(int threadCount) + { + this.threadCount = threadCount; + workers = new Worker[threadCount - 1]; + for (int i = 0; i < workers.Length; ++i) + { + workers[i] = new Worker { Thread = new Thread(WorkerLoop), Signal = new AutoResetEvent(false) }; + workers[i].Thread.IsBackground = true; + workers[i].Thread.Start((workers[i].Signal, i + 1)); + } + finished = new AutoResetEvent(false); + bufferPools = new BufferPool[threadCount]; + for (int i = 0; i < bufferPools.Length; ++i) + { + bufferPools[i] = new BufferPool(); + } + } + + void DispatchThread(int workerIndex) + { + Debug.Assert(workerBody != null); + workerBody(workerIndex); + + if (Interlocked.Decrement(ref remainingWorkerCounter) == -1) + { + finished.Set(); + } + } + + volatile Action workerBody; + int remainingWorkerCounter; + + void WorkerLoop(object untypedSignal) + { + var (signal, workerIndex) = ((AutoResetEvent, int))untypedSignal; + while (true) + { + signal.WaitOne(); + if (disposed) + return; + DispatchThread(workerIndex); + } + } + + void SignalThreads(int maximumWorkerCount) + { + //Worker 0 is not signalled; it's the executing thread. + //So if we want 4 total executing threads, we should signal 3 workers. + int maximumWorkersToSignal = maximumWorkerCount - 1; + var workersToSignal = maximumWorkersToSignal < workers.Length ? maximumWorkersToSignal : workers.Length; + remainingWorkerCounter = workersToSignal; + for (int i = 0; i < workersToSignal; ++i) + { + workers[i].Signal.Set(); + } + } + + public void DispatchWorkers(Action workerBody, int maximumWorkerCount = int.MaxValue) + { + if (maximumWorkerCount > 1) + { + Debug.Assert(this.workerBody == null); + this.workerBody = workerBody; + SignalThreads(maximumWorkerCount); + //Calling thread does work. No reason to spin up another worker and block this one! + DispatchThread(0); + finished.WaitOne(); + this.workerBody = null; + } + else if (maximumWorkerCount == 1) + { + workerBody(0); + } + } + + volatile bool disposed; + + /// + /// Waits for all pending work to complete and then disposes all workers. + /// + public void Dispose() + { + if (!disposed) + { + disposed = true; + SignalThreads(threadCount); + for (int i = 0; i < bufferPools.Length; ++i) + { + bufferPools[i].Clear(); + } + foreach (var worker in workers) + { + worker.Thread.Join(); + worker.Signal.Dispose(); + } + } + } + + public BufferPool GetThreadMemoryPool(int workerIndex) + { + return bufferPools[workerIndex]; + } + } + +} diff --git a/BepuUtilities/SimpleThreadDispatcher3.cs b/BepuUtilities/SimpleThreadDispatcher3.cs new file mode 100644 index 000000000..d17127609 --- /dev/null +++ b/BepuUtilities/SimpleThreadDispatcher3.cs @@ -0,0 +1,141 @@ +using System; +using System.Diagnostics; +using System.Threading; +using BepuUtilities.Memory; + +namespace BepuUtilities +{ + /// + /// Provides a implementation. Not reentrant. + /// + public class SimpleThreadDispatcher3 : IThreadDispatcher, IDisposable + { + int threadCount; + /// + /// Gets the number of threads to dispatch work on. + /// + public int ThreadCount => threadCount; + struct Worker + { + public Thread Thread; + } + + Worker[] workers; + ManualResetEventSlim signal; + AutoResetEvent finished; + + BufferPool[] bufferPools; + + /// + /// Creates a new thread dispatcher with the given number of threads. + /// + /// Number of threads to dispatch on each invocation. + public SimpleThreadDispatcher3(int threadCount) + { + this.threadCount = threadCount; + workers = new Worker[threadCount - 1]; + signal = new ManualResetEventSlim(); + finished = new AutoResetEvent(false); + for (int i = 0; i < workers.Length; ++i) + { + workers[i] = new Worker { Thread = new Thread(WorkerLoop) }; + workers[i].Thread.IsBackground = true; + workers[i].Thread.Start(i + 1); + } + bufferPools = new BufferPool[threadCount]; + for (int i = 0; i < bufferPools.Length; ++i) + { + bufferPools[i] = new BufferPool(); + } + } + + void DispatchThread(int workerIndex) + { + Debug.Assert(workerBody != null); + var unclaimedCount = Interlocked.Decrement(ref remainingUnclaimedCounter); + if (unclaimedCount == 0) + { + //No more work, so stop workers from trying to do anything. + signal.Reset(); + } + if (unclaimedCount < 0) + return; + workerBody(unclaimedCount); + + if (Interlocked.Decrement(ref remainingWorkerCounter) == 0) + { + finished.Set(); + } + } + + volatile Action workerBody; + int remainingUnclaimedCounter; + int remainingWorkerCounter; + + void WorkerLoop(object untypedWokrerIndex) + { + var workerIndex = (int)untypedWokrerIndex; + while (true) + { + signal.Wait(); + if (disposed) + return; + DispatchThread(workerIndex); + } + } + + void SignalThreads(int maximumWorkerCount) + { + var workersToSignal = maximumWorkerCount < threadCount ? maximumWorkerCount : threadCount; + remainingUnclaimedCounter = workersToSignal; + remainingWorkerCounter = workersToSignal; + signal.Set(); + } + + public void DispatchWorkers(Action workerBody, int maximumWorkerCount = int.MaxValue) + { + if (maximumWorkerCount > 1) + { + Debug.Assert(this.workerBody == null); + this.workerBody = workerBody; + SignalThreads(maximumWorkerCount); + //Calling thread does work. No reason to spin up another worker and block this one! + DispatchThread(0); + finished.WaitOne(); + this.workerBody = null; + } + else if (maximumWorkerCount == 1) + { + workerBody(0); + } + } + + volatile bool disposed; + + /// + /// Waits for all pending work to complete and then disposes all workers. + /// + public void Dispose() + { + if (!disposed) + { + disposed = true; + SignalThreads(threadCount); + for (int i = 0; i < bufferPools.Length; ++i) + { + bufferPools[i].Clear(); + } + foreach (var worker in workers) + { + worker.Thread.Join(); + } + } + } + + public BufferPool GetThreadMemoryPool(int workerIndex) + { + return bufferPools[workerIndex]; + } + } + +} diff --git a/BepuUtilities/SimpleThreadDispatcher4.cs b/BepuUtilities/SimpleThreadDispatcher4.cs new file mode 100644 index 000000000..b492fa379 --- /dev/null +++ b/BepuUtilities/SimpleThreadDispatcher4.cs @@ -0,0 +1,146 @@ +using System; +using System.Diagnostics; +using System.Threading; +using BepuUtilities.Memory; + +namespace BepuUtilities +{ + /// + /// Provides a implementation. Not reentrant. + /// + public class SimpleThreadDispatcher4 : IThreadDispatcher, IDisposable + { + int threadCount; + /// + /// Gets the number of threads to dispatch work on. + /// + public int ThreadCount => threadCount; + struct Worker + { + public Thread Thread; + } + + Worker[] workers; + ManualResetEvent signalA; + ManualResetEvent signalB; + AutoResetEvent finished; + + BufferPool[] bufferPools; + + /// + /// Creates a new thread dispatcher with the given number of threads. + /// + /// Number of threads to dispatch on each invocation. + public SimpleThreadDispatcher4(int threadCount) + { + this.threadCount = threadCount; + workers = new Worker[threadCount - 1]; + signalA = new ManualResetEvent(false); + signalB = new ManualResetEvent(false); + finished = new AutoResetEvent(false); + for (int i = 0; i < workers.Length; ++i) + { + workers[i] = new Worker { Thread = new Thread(WorkerLoop) }; + workers[i].Thread.IsBackground = true; + workers[i].Thread.Start(i + 1); + } + bufferPools = new BufferPool[threadCount]; + for (int i = 0; i < bufferPools.Length; ++i) + { + bufferPools[i] = new BufferPool(); + } + } + + void DispatchThread(int workerIndex) + { + Debug.Assert(workerBody != null); + Debug.Assert(remainingWorkerCounter >= 0); + workerBody(workerIndex); + + if (Interlocked.Decrement(ref remainingWorkerCounter) == 0) + { + finished.Set(); + } + } + + volatile Action workerBody; + int remainingWorkerCounter; + + void WorkerLoop(object untypedWorkerIndex) + { + var workerIndex = (int)untypedWorkerIndex; + var signal = this.signalA; + var otherSignal = this.signalB; + while (true) + { + //Signals get ping ponged. + signal.WaitOne(); + if (disposed) + return; + var temp = signal; + signal = otherSignal; + otherSignal = temp; + DispatchThread(workerIndex); + } + } + + void SignalThreads(int maximumWorkerCount) + { + var workersToSignal = maximumWorkerCount < threadCount ? maximumWorkerCount : threadCount; + remainingWorkerCounter = workersToSignal; + signalA.Set(); + } + + public void DispatchWorkers(Action workerBody, int maximumWorkerCount = int.MaxValue) + { + if (maximumWorkerCount > 1) + { + Debug.Assert(this.workerBody == null); + this.workerBody = workerBody; + SignalThreads(maximumWorkerCount); + //Calling thread does work. No reason to spin up another worker and block this one! + DispatchThread(0); + finished.WaitOne(); + signalA.Reset(); + var temp = signalA; + signalA = signalB; + signalB = temp; + this.workerBody = null; + } + else if (maximumWorkerCount == 1) + { + workerBody(0); + } + } + + volatile bool disposed; + + /// + /// Waits for all pending work to complete and then disposes all workers. + /// + public void Dispose() + { + if (!disposed) + { + disposed = true; + SignalThreads(threadCount); + for (int i = 0; i < bufferPools.Length; ++i) + { + bufferPools[i].Clear(); + } + foreach (var worker in workers) + { + worker.Thread.Join(); + } + signalA.Dispose(); + signalB.Dispose(); + } + } + + public BufferPool GetThreadMemoryPool(int workerIndex) + { + return bufferPools[workerIndex]; + } + } + +} diff --git a/BepuUtilities/ThreadDispatcher.cs b/BepuUtilities/ThreadDispatcher.cs new file mode 100644 index 000000000..8c97d7a56 --- /dev/null +++ b/BepuUtilities/ThreadDispatcher.cs @@ -0,0 +1,149 @@ +using System; +using System.Diagnostics; +using System.Threading; +using BepuUtilities.Memory; + +namespace BepuUtilities +{ + /// + /// Provides a implementation. Not reentrant. + /// + public class ThreadDispatcher : IThreadDispatcher, IDisposable + { + int threadCount; + /// + /// Gets the number of threads to dispatch work on. + /// + public int ThreadCount => threadCount; + struct Worker + { + public Thread Thread; + } + + Worker[] workers; + ManualResetEventSlim resetEvent; + AutoResetEvent finished; + + BufferPool[] bufferPools; + + /// + /// Creates a new thread dispatcher with the given number of threads. + /// + /// Number of threads to dispatch on each invocation. + public ThreadDispatcher(int threadCount) + { + this.threadCount = threadCount; + resetEvent = new ManualResetEventSlim(false); + workers = new Worker[threadCount - 1]; + for (int i = 0; i < workers.Length; ++i) + { + workers[i] = new Worker { Thread = new Thread(WorkerLoop) }; + workers[i].Thread.IsBackground = true; + workers[i].Thread.Start(i + 1); + } + finished = new AutoResetEvent(false); + bufferPools = new BufferPool[threadCount]; + for (int i = 0; i < bufferPools.Length; ++i) + { + bufferPools[i] = new BufferPool(); + } + } + + void DispatchThread(int workerIndex) + { + while (true) + { + var localJobIndex = Interlocked.Increment(ref jobIndex); + if (localJobIndex < jobCount) + { + if (localJobIndex == jobCount - 1) + { + //No further jobs are available, so workers should start waiting again. + resetEvent.Reset(); + } + Debug.Assert(workerBody != null); + workerBody(localJobIndex); + if (Interlocked.Decrement(ref remainingJobCounter) == 0) + { + finished.Set(); + } + } + else + { + return; + } + } + } + + volatile Action workerBody; + int jobIndex; + int remainingJobCounter; + int jobCount; + + void WorkerLoop(object untypedSignal) + { + var workerIndex = (int)untypedSignal; + while (true) + { + resetEvent.Wait(); + if (disposed) + return; + DispatchThread(workerIndex); + } + } + + void SignalThreads(int jobCount) + { + this.jobCount = jobCount; + jobIndex = -1; + remainingJobCounter = jobCount; + resetEvent.Set(); + } + + public void DispatchWorkers(Action workerBody, int maximumWorkerCount = int.MaxValue) + { + if (maximumWorkerCount > 1) + { + Debug.Assert(this.workerBody == null); + this.workerBody = workerBody; + SignalThreads(Math.Min(threadCount, maximumWorkerCount)); + //Calling thread does work. No reason to spin up another worker and block this one! + DispatchThread(0); + finished.WaitOne(); + this.workerBody = null; + } + else if (maximumWorkerCount == 1) + { + workerBody(0); + } + } + + volatile bool disposed; + + /// + /// Waits for all pending work to complete and then disposes all workers. + /// + public void Dispose() + { + if (!disposed) + { + disposed = true; + SignalThreads(threadCount); + for (int i = 0; i < bufferPools.Length; ++i) + { + bufferPools[i].Clear(); + } + foreach (var worker in workers) + { + worker.Thread.Join(); + } + } + } + + public BufferPool GetThreadMemoryPool(int workerIndex) + { + return bufferPools[workerIndex]; + } + } + +} diff --git a/BepuUtilities/ThreadDispatcher2.cs b/BepuUtilities/ThreadDispatcher2.cs new file mode 100644 index 000000000..4a95188b3 --- /dev/null +++ b/BepuUtilities/ThreadDispatcher2.cs @@ -0,0 +1,169 @@ +using System; +using System.Diagnostics; +using System.Threading; +using BepuUtilities.Memory; + +namespace BepuUtilities +{ + + /// + /// Provides a implementation. Not reentrant. + /// + public class ThreadDispatcher2 : IThreadDispatcher, IDisposable + { + int threadCount; + /// + /// Gets the number of threads to dispatch work on. + /// + public int ThreadCount => threadCount; + struct Worker + { + public Thread Thread; + } + + Worker[] workers; + AutoResetEvent finished; + BufferPool[] bufferPools; + + /// + /// Creates a new thread dispatcher with the given number of threads. + /// + /// Number of threads to dispatch on each invocation. + /// Size of allocation blocks in the worker thread buffer pools. + public ThreadDispatcher2(int threadCount, int workerPoolBlockAllocationSize = 16384) + { + this.threadCount = threadCount; + workers = new Worker[threadCount - 1]; + for (int i = 0; i < workers.Length; ++i) + { + workers[i] = new Worker { Thread = new Thread(WorkerLoop) }; + workers[i].Thread.IsBackground = true; + //The main thread will be used as worker 0. + workers[i].Thread.Start(i + 1); + } + finished = new AutoResetEvent(false); + bufferPools = new BufferPool[threadCount]; + for (int i = 0; i < bufferPools.Length; ++i) + { + bufferPools[i] = new BufferPool(workerPoolBlockAllocationSize); + } + } + + volatile Action workerBody; + int remainingUnclaimedJobCount; + int remainingUncompletedJobCount; + + object waiter = new object(); + + void DispatchThread(int workerIndex) + { + int jobIndex; + lock (waiter) + { + //Getting here either means 1) this is the startup and there is no job to do, or 2) the local thread just executed a workerBody and we should increment completed work. + if (workerBody != null) + { + //Just finished work; mark it as completed. + if (--remainingUncompletedJobCount == 0) + { + workerBody = null; + finished.Set(); + return; + } + } + if (remainingUnclaimedJobCount == 0 || disposed) + { + return; + } + //Grab the next job. + jobIndex = --remainingUnclaimedJobCount; + } + workerBody(jobIndex); + } + + void WorkerLoop(object untypedWorkerIndex) + { + var workerIndex = (int)untypedWorkerIndex; + while (true) + { + int jobIndex; + lock (waiter) + { + //Getting here either means 1) this is the startup and there is no job to do, or 2) the local thread just executed a workerBody and we should increment completed work. + if (workerBody != null) + { + //Just finished work; mark it as completed. + if (--remainingUncompletedJobCount == 0) + { + workerBody = null; + finished.Set(); + } + } + while (workerBody == null || remainingUnclaimedJobCount == 0) + { + if (disposed) + return; + Monitor.Wait(waiter); + } + //Grab the next job. + jobIndex = --remainingUnclaimedJobCount; + } + workerBody(jobIndex); + } + } + + public void DispatchWorkers(Action workerBody, int maximumWorkerCount = int.MaxValue) + { + if (maximumWorkerCount > 1) + { + lock (waiter) + { + remainingUnclaimedJobCount = maximumWorkerCount < threadCount ? maximumWorkerCount : threadCount; + remainingUncompletedJobCount = remainingUnclaimedJobCount; + Debug.Assert(this.workerBody == null); + this.workerBody = workerBody; + Monitor.PulseAll(waiter); + } + //Calling thread does work. No reason to spin up another worker and block this one! + //DispatchThread(0); + finished.WaitOne(); + } + else if (maximumWorkerCount == 1) + { + workerBody(0); + } + } + + volatile bool disposed; + + /// + /// Waits for all pending work to complete and then disposes all workers. + /// + public void Dispose() + { + if (!disposed) + { + disposed = true; + lock (waiter) + { + Debug.Assert(this.workerBody == null); + Monitor.PulseAll(waiter); + } + for (int i = 0; i < bufferPools.Length; ++i) + { + bufferPools[i].Clear(); + } + foreach (var worker in workers) + { + worker.Thread.Join(); + } + } + } + + public BufferPool GetThreadMemoryPool(int workerIndex) + { + return bufferPools[workerIndex]; + } + } + +} diff --git a/BepuUtilities/ThreadDispatcher3.cs b/BepuUtilities/ThreadDispatcher3.cs new file mode 100644 index 000000000..4e8b61307 --- /dev/null +++ b/BepuUtilities/ThreadDispatcher3.cs @@ -0,0 +1,142 @@ +using System; +using System.Diagnostics; +using System.Threading; +using BepuUtilities.Memory; + +namespace BepuUtilities +{ + + /// + /// Provides a implementation. Not reentrant. + /// + public class ThreadDispatcher3 : IThreadDispatcher, IDisposable + { + int threadCount; + /// + /// Gets the number of threads to dispatch work on. + /// + public int ThreadCount => threadCount; + struct Worker + { + public Thread Thread; + } + + Worker[] workers; + AutoResetEvent finished; + BufferPool[] bufferPools; + + /// + /// Creates a new thread dispatcher with the given number of threads. + /// + /// Number of threads to dispatch on each invocation. + /// Size of allocation blocks in the worker thread buffer pools. + public ThreadDispatcher3(int threadCount, int workerPoolBlockAllocationSize = 16384) + { + this.threadCount = threadCount; + semaphore = new SemaphoreSlim(0); + workers = new Worker[threadCount - 1]; + for (int i = 0; i < workers.Length; ++i) + { + workers[i] = new Worker { Thread = new Thread(WorkerLoop) }; + workers[i].Thread.IsBackground = true; + //The main thread will be used as worker 0. + workers[i].Thread.Start(i + 1); + } + finished = new AutoResetEvent(false); + bufferPools = new BufferPool[threadCount]; + for (int i = 0; i < bufferPools.Length; ++i) + { + bufferPools[i] = new BufferPool(workerPoolBlockAllocationSize); + } + } + + volatile Action workerBody; + int remainingUnclaimedJobCount; + int remainingUncompletedJobCount; + + SemaphoreSlim semaphore; + + void DispatchThread(int workerIndex) + { + int jobIndex; + int completedJobCount = 0; + while ((jobIndex = Interlocked.Decrement(ref remainingUnclaimedJobCount)) >= 0) + { + workerBody(jobIndex); + ++completedJobCount; + } + //No more jobs are available. + if (Interlocked.Add(ref remainingUncompletedJobCount, -completedJobCount) == 0) + { + //All jobs are done, no worker is still executing anything. + workerBody = null; + finished.Set(); + } + } + void WorkerLoop(object untypedWorkerIndex) + { + var workerIndex = (int)untypedWorkerIndex; + while (true) + { + semaphore.Wait(); + if (disposed) + return; + DispatchThread(workerIndex); + } + } + + void SignalThreads(int jobCount, Action workerBody) + { + remainingUnclaimedJobCount = jobCount < threadCount ? jobCount : threadCount; + remainingUncompletedJobCount = remainingUnclaimedJobCount; + Debug.Assert(this.workerBody == null); + this.workerBody = workerBody; + semaphore.Release(remainingUnclaimedJobCount); + } + + public void DispatchWorkers(Action workerBody, int maximumWorkerCount = int.MaxValue) + { + if (maximumWorkerCount > 1) + { + SignalThreads(maximumWorkerCount, workerBody); + //Calling thread does work. No reason to spin up another worker and block this one! + DispatchThread(0); + finished.WaitOne(); + } + else if (maximumWorkerCount == 1) + { + workerBody(0); + } + } + + volatile bool disposed; + + /// + /// Waits for all pending work to complete and then disposes all workers. + /// + public void Dispose() + { + if (!disposed) + { + disposed = true; + Debug.Assert(workerBody == null); + SignalThreads(threadCount - 1, null); + finished.WaitOne(); + for (int i = 0; i < bufferPools.Length; ++i) + { + bufferPools[i].Clear(); + } + foreach (var worker in workers) + { + worker.Thread.Join(); + } + } + } + + public BufferPool GetThreadMemoryPool(int workerIndex) + { + return bufferPools[workerIndex]; + } + } + +} diff --git a/BepuUtilities/ThreadDispatcher4.cs b/BepuUtilities/ThreadDispatcher4.cs new file mode 100644 index 000000000..1610f33e7 --- /dev/null +++ b/BepuUtilities/ThreadDispatcher4.cs @@ -0,0 +1,157 @@ +using System; +using System.Diagnostics; +using System.Threading; +using BepuUtilities.Memory; + +namespace BepuUtilities +{ + + /// + /// Provides a implementation. Not reentrant. + /// + public class ThreadDispatcher4 : IThreadDispatcher, IDisposable + { + int threadCount; + /// + /// Gets the number of threads to dispatch work on. + /// + public int ThreadCount => threadCount; + struct Worker + { + public Thread Thread; + } + + Worker[] workers; + AutoResetEvent finished; + BufferPool[] bufferPools; + + /// + /// Creates a new thread dispatcher with the given number of threads. + /// + /// Number of threads to dispatch on each invocation. + /// Size of allocation blocks in the worker thread buffer pools. + public ThreadDispatcher4(int threadCount, int workerPoolBlockAllocationSize = 16384) + { + this.threadCount = threadCount; + semaphore = new SemaphoreSlim(0); + workers = new Worker[threadCount - 1]; + for (int i = 0; i < workers.Length; ++i) + { + workers[i] = new Worker { Thread = new Thread(WorkerLoop) }; + workers[i].Thread.IsBackground = true; + //The main thread will be used as worker 0. + workers[i].Thread.Start(i + 1); + } + finished = new AutoResetEvent(false); + bufferPools = new BufferPool[threadCount]; + for (int i = 0; i < bufferPools.Length; ++i) + { + bufferPools[i] = new BufferPool(workerPoolBlockAllocationSize); + } + workClaimed = new int[threadCount]; + } + + Action workerBody; + int remainingUnclaimedJobCount; + int remainingUncompletedJobCount; + + SemaphoreSlim semaphore; + object locker = new object(); + + public int[] workClaimed; + + void DispatchThread(int workerIndex) + { + Action localWorkerBody = null; + int jobIndex; + while (true) + { + lock (locker) + { + if (localWorkerBody != null && --remainingUncompletedJobCount == 0) + { + //This thread just finished up the last chunk of work relevant to the previously cached workerBody. + this.workerBody = null; + finished.Set(); + break; + } + if (remainingUnclaimedJobCount > 0) + jobIndex = --remainingUnclaimedJobCount; + else + break; + localWorkerBody = this.workerBody; + ++workClaimed[workerIndex]; + } + Debug.Assert(jobIndex >= 0 && localWorkerBody != null); + localWorkerBody(jobIndex); + } + } + void WorkerLoop(object untypedWorkerIndex) + { + var workerIndex = (int)untypedWorkerIndex; + while (true) + { + semaphore.Wait(); + if (disposed) + return; + DispatchThread(workerIndex); + } + } + + void SignalThreads(int jobCount, Action workerBody) + { + lock (locker) + { + jobCount = remainingUnclaimedJobCount = jobCount < threadCount ? jobCount : threadCount; + remainingUncompletedJobCount = jobCount; + Debug.Assert(this.workerBody == null); + this.workerBody = workerBody; + } + semaphore.Release(jobCount); + } + + public void DispatchWorkers(Action workerBody, int maximumWorkerCount = int.MaxValue) + { + if (maximumWorkerCount > 1) + { + SignalThreads(maximumWorkerCount, workerBody); + //Calling thread does work. No reason to spin up another worker and block this one! + DispatchThread(0); + finished.WaitOne(); + } + else if (maximumWorkerCount == 1) + { + workerBody(0); + } + } + + volatile bool disposed; + + /// + /// Waits for all pending work to complete and then disposes all workers. + /// + public void Dispose() + { + if (!disposed) + { + disposed = true; + Debug.Assert(workerBody == null); + SignalThreads(threadCount - 1, null); + for (int i = 0; i < bufferPools.Length; ++i) + { + bufferPools[i].Clear(); + } + foreach (var worker in workers) + { + worker.Thread.Join(); + } + } + } + + public BufferPool GetThreadMemoryPool(int workerIndex) + { + return bufferPools[workerIndex]; + } + } + +} diff --git a/BepuUtilities/ThreadDispatcher5.cs b/BepuUtilities/ThreadDispatcher5.cs new file mode 100644 index 000000000..9e00edbce --- /dev/null +++ b/BepuUtilities/ThreadDispatcher5.cs @@ -0,0 +1,156 @@ +using System; +using System.Diagnostics; +using System.Threading; +using BepuUtilities.Memory; + +namespace BepuUtilities +{ + /// + /// Provides a implementation. Not reentrant. + /// + public class ThreadDispatcher5 : IThreadDispatcher, IDisposable + { + int threadCount; + /// + /// Gets the number of threads to dispatch work on. + /// + public int ThreadCount => threadCount; + struct Worker + { + public Thread Thread; + public AutoResetEvent Signal; + } + + Worker[] workers; + AutoResetEvent finished; + + BufferPool[] bufferPools; + + /// + /// Creates a new thread dispatcher with the given number of threads. + /// + /// Number of threads to dispatch on each invocation. + public ThreadDispatcher5(int threadCount) + { + this.threadCount = threadCount; + workers = new Worker[threadCount - 1]; + for (int i = 0; i < workers.Length; ++i) + { + workers[i] = new Worker { Thread = new Thread(WorkerLoop), Signal = new AutoResetEvent(false) }; + workers[i].Thread.IsBackground = true; + workers[i].Thread.Start((workers[i].Signal, i + 1)); + } + finished = new AutoResetEvent(false); + bufferPools = new BufferPool[threadCount]; + for (int i = 0; i < bufferPools.Length; ++i) + { + bufferPools[i] = new BufferPool(); + } + } + + void DispatchThread(Action localAction, int jobIndex, int workerIndex) + { + Debug.Assert(workerBody != null); + localAction(jobIndex); + + if (Interlocked.Decrement(ref remainingWorkerCounter) == 0) + { + finished.Set(); + } + } + + volatile Action workerBody; + int jobIndex; + int jobCount; + int remainingWorkerCounter; + object locker = new object(); + void WorkerLoop(object untypedSignal) + { + (AutoResetEvent signal, int workerIndex) = ((AutoResetEvent, int))untypedSignal; + while (true) + { + if (disposed) + return; + signal.WaitOne(); + while (true) + { + Action localAction; + int jobIndex; + lock (locker) + { + localAction = workerBody; + jobIndex = this.jobIndex++; + if (localAction == null || jobIndex >= jobCount) + break; + } + DispatchThread(localAction, jobIndex, workerIndex); + } + } + } + + void SignalThreads(int maximumWorkerCount) + { + //Worker 0 is not signalled; it's the executing thread. + //So if we want 4 total executing threads, we should signal 3 workers. + int maximumWorkersToSignal = maximumWorkerCount - 1; + var workersToSignal = maximumWorkersToSignal < workers.Length ? maximumWorkersToSignal : workers.Length; + for (int i = 0; i < workersToSignal; ++i) + { + workers[i].Signal.Set(); + } + } + + public void DispatchWorkers(Action workerBody, int maximumWorkerCount = int.MaxValue) + { + if (maximumWorkerCount > 1) + { + Debug.Assert(this.workerBody == null); + lock (locker) + { + jobIndex = 1; //Just make the inline thread worker 0. While the other threads might start executing first, the user should never rely on the dispatch order. + jobCount = Math.Min(maximumWorkerCount, threadCount); + remainingWorkerCounter = jobCount; + this.workerBody = workerBody; + } + SignalThreads(maximumWorkerCount); + //Calling thread does work. No reason to spin up another worker and block this one! + DispatchThread(workerBody, 0, 0); + finished.WaitOne(); + this.workerBody = null; + } + else if (maximumWorkerCount == 1) + { + workerBody(0); + } + } + + volatile bool disposed; + + /// + /// Waits for all pending work to complete and then disposes all workers. + /// + public void Dispose() + { + if (!disposed) + { + disposed = true; + SignalThreads(threadCount); + for (int i = 0; i < bufferPools.Length; ++i) + { + bufferPools[i].Clear(); + } + foreach (var worker in workers) + { + worker.Thread.Join(); + worker.Signal.Dispose(); + } + } + } + + public BufferPool GetThreadMemoryPool(int workerIndex) + { + return bufferPools[workerIndex]; + } + } + +} diff --git a/BepuUtilities/ThreadDispatcher6.cs b/BepuUtilities/ThreadDispatcher6.cs new file mode 100644 index 000000000..57f7adb1d --- /dev/null +++ b/BepuUtilities/ThreadDispatcher6.cs @@ -0,0 +1,137 @@ +using System; +using System.Diagnostics; +using System.Threading; +using BepuUtilities.Memory; + +namespace BepuUtilities +{ + /// + /// Provides a implementation. Not reentrant. + /// + public class ThreadDispatcher6 : IThreadDispatcher, IDisposable + { + int threadCount; + /// + /// Gets the number of threads to dispatch work on. + /// + public int ThreadCount => threadCount; + struct Worker + { + public Thread Thread; + } + + Worker[] workers; + SemaphoreSlim semaphore; + AutoResetEvent finished; + + BufferPool[] bufferPools; + + /// + /// Creates a new thread dispatcher with the given number of threads. + /// + /// Number of threads to dispatch on each invocation. + public ThreadDispatcher6(int threadCount) + { + this.threadCount = threadCount; + semaphore = new SemaphoreSlim(0, threadCount); + workers = new Worker[threadCount - 1]; + for (int i = 0; i < workers.Length; ++i) + { + workers[i] = new Worker { Thread = new Thread(WorkerLoop) }; + workers[i].Thread.IsBackground = true; + workers[i].Thread.Start(i + 1); + } + finished = new AutoResetEvent(false); + bufferPools = new BufferPool[threadCount]; + for (int i = 0; i < bufferPools.Length; ++i) + { + bufferPools[i] = new BufferPool(); + } + } + + void DispatchThread(int jobIndex, int workerIndex) + { + Debug.Assert(workerBody != null); + workerBody(jobIndex); + + if (Interlocked.Decrement(ref remainingWorkerCounter) == -1) + { + finished.Set(); + } + } + + volatile Action workerBody; + int jobIndex; + int remainingWorkerCounter; + + void WorkerLoop(object untypedSignal) + { + var workerIndex = (int)untypedSignal; + while (true) + { + semaphore.Wait(); + if (disposed) + return; + var localJobIndex = Interlocked.Increment(ref jobIndex); + DispatchThread(localJobIndex, workerIndex); + } + } + + void SignalThreads(int maximumWorkerCount) + { + //Worker 0 is not signalled; it's the executing thread. + //So if we want 4 total executing threads, we should signal 3 workers. + int maximumWorkersToSignal = maximumWorkerCount - 1; + var workersToSignal = maximumWorkersToSignal < workers.Length ? maximumWorkersToSignal : workers.Length; + remainingWorkerCounter = workersToSignal; + jobIndex = 0; + semaphore.Release(workersToSignal); + } + + public void DispatchWorkers(Action workerBody, int maximumWorkerCount = int.MaxValue) + { + if (maximumWorkerCount > 1) + { + Debug.Assert(this.workerBody == null); + this.workerBody = workerBody; + SignalThreads(maximumWorkerCount); + //Calling thread does work. No reason to spin up another worker and block this one! + DispatchThread(0, 0); + finished.WaitOne(); + this.workerBody = null; + } + else if (maximumWorkerCount == 1) + { + workerBody(0); + } + } + + volatile bool disposed; + + /// + /// Waits for all pending work to complete and then disposes all workers. + /// + public void Dispose() + { + if (!disposed) + { + disposed = true; + SignalThreads(threadCount); + for (int i = 0; i < bufferPools.Length; ++i) + { + bufferPools[i].Clear(); + } + foreach (var worker in workers) + { + worker.Thread.Join(); + } + } + } + + public BufferPool GetThreadMemoryPool(int workerIndex) + { + return bufferPools[workerIndex]; + } + } + +} diff --git a/BepuUtilities/ThreadDispatcher7.cs b/BepuUtilities/ThreadDispatcher7.cs new file mode 100644 index 000000000..a8ead522f --- /dev/null +++ b/BepuUtilities/ThreadDispatcher7.cs @@ -0,0 +1,134 @@ +using System; +using System.Diagnostics; +using System.Threading; +using BepuUtilities.Memory; + +namespace BepuUtilities +{ + + /// + /// Provides a implementation. Not reentrant. + /// + public class ThreadDispatcher7 : IThreadDispatcher, IDisposable + { + int threadCount; + /// + /// Gets the number of threads to dispatch work on. + /// + public int ThreadCount => threadCount; + struct Worker + { + public Thread Thread; + } + + Worker[] workers; + AutoResetEvent finished; + BufferPool[] bufferPools; + + /// + /// Creates a new thread dispatcher with the given number of threads. + /// + /// Number of threads to dispatch on each invocation. + /// Size of allocation blocks in the worker thread buffer pools. + public ThreadDispatcher7(int threadCount, int workerPoolBlockAllocationSize = 16384) + { + this.threadCount = threadCount; + semaphore = new SemaphoreSlim(0); + workers = new Worker[threadCount]; + for (int i = 0; i < workers.Length; ++i) + { + workers[i] = new Worker { Thread = new Thread(WorkerLoop) }; + workers[i].Thread.IsBackground = true; + workers[i].Thread.Start(i); + } + finished = new AutoResetEvent(false); + bufferPools = new BufferPool[threadCount]; + for (int i = 0; i < bufferPools.Length; ++i) + { + bufferPools[i] = new BufferPool(workerPoolBlockAllocationSize); + } + } + + volatile Action workerBody; + int remainingUnclaimedJobCount; + int remainingUncompletedJobCount; + + SemaphoreSlim semaphore; + + void DispatchThread(int workerIndex) + { + var jobIndex = Interlocked.Decrement(ref remainingUnclaimedJobCount); + workerBody(jobIndex); + + //No more jobs are available. + if (Interlocked.Decrement(ref remainingUncompletedJobCount) == 0) + { + //All jobs are done, no worker is still executing anything. + workerBody = null; + finished.Set(); + } + } + void WorkerLoop(object untypedWorkerIndex) + { + var workerIndex = (int)untypedWorkerIndex; + while (true) + { + semaphore.Wait(); + if (disposed) + return; + DispatchThread(workerIndex); + } + } + + void SignalThreads(int jobCount, Action workerBody) + { + remainingUnclaimedJobCount = jobCount < threadCount ? jobCount : threadCount; + remainingUncompletedJobCount = remainingUnclaimedJobCount; + Debug.Assert(this.workerBody == null); + this.workerBody = workerBody; + semaphore.Release(remainingUnclaimedJobCount); + } + + public void DispatchWorkers(Action workerBody, int maximumWorkerCount = int.MaxValue) + { + if (maximumWorkerCount > 1) + { + SignalThreads(maximumWorkerCount, workerBody); + finished.WaitOne(); + } + else if (maximumWorkerCount == 1) + { + workerBody(0); + } + } + + volatile bool disposed; + + /// + /// Waits for all pending work to complete and then disposes all workers. + /// + public void Dispose() + { + if (!disposed) + { + disposed = true; + Debug.Assert(workerBody == null); + SignalThreads(threadCount, null); + for (int i = 0; i < bufferPools.Length; ++i) + { + bufferPools[i].Clear(); + } + foreach (var worker in workers) + { + worker.Thread.Join(); + } + } + } + + public BufferPool GetThreadMemoryPool(int workerIndex) + { + return bufferPools[workerIndex]; + } + } + +} diff --git a/Demos/Demo.cs b/Demos/Demo.cs index b12629b3c..884f0d61b 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -26,7 +26,7 @@ public abstract class Demo : IDisposable /// /// Gets the thread dispatcher available for use by the simulation. /// - public SimpleThreadDispatcher ThreadDispatcher { get; private set; } + public ThreadDispatcher7 ThreadDispatcher { get; private set; } protected Demo() { @@ -44,7 +44,7 @@ protected Demo() //It may be worth using something like hwloc or CPUID to extract extra information to reason about. var targetThreadCount = Math.Max(1, Environment.ProcessorCount > 4 ? Environment.ProcessorCount - 2 : Environment.ProcessorCount - 1); - ThreadDispatcher = new SimpleThreadDispatcher(targetThreadCount); + ThreadDispatcher = new ThreadDispatcher7(targetThreadCount); } public virtual void LoadGraphicalContent(ContentArchive content, RenderSurface surface) diff --git a/Demos/Demos/RayCastingDemo.cs b/Demos/Demos/RayCastingDemo.cs index 6ceaddcf0..c19a2bdd0 100644 --- a/Demos/Demos/RayCastingDemo.cs +++ b/Demos/Demos/RayCastingDemo.cs @@ -269,7 +269,7 @@ void ExecuteWorker(int workerIndex) Interlocked.Add(ref IntersectionCount, intersectionCount); } - public void Execute(ref QuickList rays, SimpleThreadDispatcher dispatcher) + public void Execute(ref QuickList rays, IThreadDispatcher dispatcher) { CacheBlaster.Blast(); for (int i = 0; i < rays.Count; ++i) diff --git a/Demos/Demos/SimpleSelfContainedDemo.cs b/Demos/Demos/SimpleSelfContainedDemo.cs index c59d81780..c99018c8f 100644 --- a/Demos/Demos/SimpleSelfContainedDemo.cs +++ b/Demos/Demos/SimpleSelfContainedDemo.cs @@ -224,7 +224,7 @@ public static void Run() simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), simulation.Shapes.Add(new Box(500, 1, 500)))); //Any IThreadDispatcher implementation can be used for multithreading. Here, we use the demos SimpleThreadDispatcher. - var threadDispatcher = new SimpleThreadDispatcher(Environment.ProcessorCount); + var threadDispatcher = new ThreadDispatcher4(Environment.ProcessorCount); //Now take 100 time steps! for (int i = 0; i < 100; ++i) diff --git a/Demos/Program.cs b/Demos/Program.cs index 189962aa9..56de75fb7 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -1,14 +1,55 @@ using BepuUtilities; using DemoContentLoader; +using Demos.Demos; +using Demos.Demos.Cars; +using Demos.SpecializedTests; using DemoUtilities; using OpenTK; +using System; +using System.Diagnostics; +using System.Runtime.CompilerServices; +using System.Threading; namespace Demos { class Program { + [MethodImpl(MethodImplOptions.NoInlining)] + static void Test(int testCount) + { + int workerCount = 512; + var dispatcher = new SimpleThreadDispatcher2(workerCount); + long[] slots = new long[workerCount]; + + long total = 0; + long expectedSum = 0; + for (int q = 0; q < testCount; ++q) + { + var start = Stopwatch.GetTimestamp(); + dispatcher.DispatchWorkers(i => { slots[i] += q * i; }, workerCount); + var stop = Stopwatch.GetTimestamp(); + total += stop - start; + + long sum = 0; + for (int i = 0; i < workerCount; ++i) + { + sum += slots[i]; + expectedSum += q * i; + } + Console.WriteLine($"Sum: {sum}, expected: {expectedSum}"); + } + dispatcher.Dispose(); + Console.WriteLine($"Time per dispatch (us): {total * 1e6 / (testCount * Stopwatch.Frequency)}"); + } static void Main(string[] args) { + //for (int i = 0; i < 32; ++i) + //{ + // Test(32); + //} + //Test(1024); + //return; + var window = new Window("pretty cool multicolored window", new Int2((int)(DisplayDevice.Default.Width * 0.75f), (int)(DisplayDevice.Default.Height * 0.75f)), WindowMode.Windowed); var loop = new GameLoop(window); @@ -17,6 +58,8 @@ static void Main(string[] args) { content = ContentArchive.Load(stream); } + //DeterminismTest.Test(content, 2, 32768); + //HeadlessTest.Test(content, 4, 64, 512); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); diff --git a/Demos/SpecializedTests/IntertreeThreadingTests.cs b/Demos/SpecializedTests/IntertreeThreadingTests.cs index cbd237dcb..ed8ba7c2a 100644 --- a/Demos/SpecializedTests/IntertreeThreadingTests.cs +++ b/Demos/SpecializedTests/IntertreeThreadingTests.cs @@ -181,7 +181,7 @@ public static void Test() { var random = new Random(5); var pool = new BufferPool(); - var threadDispatcher = new SimpleThreadDispatcher(Environment.ProcessorCount); + var threadDispatcher = new ThreadDispatcher(Environment.ProcessorCount); for (int i = 0; i < 1000; ++i) { TestTrees(pool, threadDispatcher, random); diff --git a/Demos/SpecializedTests/TreeTest.cs b/Demos/SpecializedTests/TreeTest.cs index c676841ba..f9529273f 100644 --- a/Demos/SpecializedTests/TreeTest.cs +++ b/Demos/SpecializedTests/TreeTest.cs @@ -68,7 +68,7 @@ public static void Test() const int iterations = 100000; const int maximumChangesPerIteration = 20; - var threadDispatcher = new SimpleThreadDispatcher(Environment.ProcessorCount); + var threadDispatcher = new ThreadDispatcher(Environment.ProcessorCount); var refineContext = new Tree.RefitAndRefineMultithreadedContext(); var selfTestContext = new Tree.MultithreadedSelfTest(pool); var overlapHandlers = new OverlapHandler[threadDispatcher.ThreadCount]; diff --git a/Demos/SpecializedTests/VolumeQueryTests.cs b/Demos/SpecializedTests/VolumeQueryTests.cs index 732db8230..188a24e47 100644 --- a/Demos/SpecializedTests/VolumeQueryTests.cs +++ b/Demos/SpecializedTests/VolumeQueryTests.cs @@ -154,7 +154,7 @@ void ExecuteWorker(int workerIndex) Interlocked.Add(ref IntersectionCount, intersectionCount); } - public void Execute(ref QuickList boxes, SimpleThreadDispatcher dispatcher) + public void Execute(ref QuickList boxes, IThreadDispatcher dispatcher) { CacheBlaster.Blast(); JobIndex = -1; From 3dd639060c122e4be24d6d189aa9ae83fd29a2c4 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 24 Jan 2022 17:14:25 -0600 Subject: [PATCH 391/947] Part 2: poof! --- BepuUtilities/SimpleThreadDispatcher.cs | 139 ------------------- BepuUtilities/SimpleThreadDispatcher2.cs | 137 ------------------ BepuUtilities/SimpleThreadDispatcher3.cs | 141 ------------------- BepuUtilities/SimpleThreadDispatcher4.cs | 146 -------------------- BepuUtilities/ThreadDispatcher.cs | 63 ++++----- BepuUtilities/ThreadDispatcher2.cs | 169 ----------------------- BepuUtilities/ThreadDispatcher3.cs | 142 ------------------- BepuUtilities/ThreadDispatcher4.cs | 157 --------------------- BepuUtilities/ThreadDispatcher5.cs | 156 --------------------- BepuUtilities/ThreadDispatcher6.cs | 137 ------------------ BepuUtilities/ThreadDispatcher7.cs | 134 ------------------ Demos/Demo.cs | 4 +- Demos/Demos/SimpleSelfContainedDemo.cs | 4 +- Demos/Program.cs | 34 ----- 14 files changed, 30 insertions(+), 1533 deletions(-) delete mode 100644 BepuUtilities/SimpleThreadDispatcher.cs delete mode 100644 BepuUtilities/SimpleThreadDispatcher2.cs delete mode 100644 BepuUtilities/SimpleThreadDispatcher3.cs delete mode 100644 BepuUtilities/SimpleThreadDispatcher4.cs delete mode 100644 BepuUtilities/ThreadDispatcher2.cs delete mode 100644 BepuUtilities/ThreadDispatcher3.cs delete mode 100644 BepuUtilities/ThreadDispatcher4.cs delete mode 100644 BepuUtilities/ThreadDispatcher5.cs delete mode 100644 BepuUtilities/ThreadDispatcher6.cs delete mode 100644 BepuUtilities/ThreadDispatcher7.cs diff --git a/BepuUtilities/SimpleThreadDispatcher.cs b/BepuUtilities/SimpleThreadDispatcher.cs deleted file mode 100644 index 55e07bc2e..000000000 --- a/BepuUtilities/SimpleThreadDispatcher.cs +++ /dev/null @@ -1,139 +0,0 @@ -using System; -using System.Diagnostics; -using System.Threading; -using BepuUtilities.Memory; - -namespace BepuUtilities -{ - /// - /// Provides a simple implementation. Not reentrant. - /// - public class SimpleThreadDispatcher : IThreadDispatcher, IDisposable - { - int threadCount; - /// - /// Gets the number of threads to dispatch work on. - /// - public int ThreadCount => threadCount; - struct Worker - { - public Thread Thread; - public AutoResetEvent Signal; - } - - Worker[] workers; - AutoResetEvent finished; - - BufferPool[] bufferPools; - - /// - /// Creates a new simple thread dispatcher with the given number of threads. - /// - /// Number of threads to dispatch on each invocation. - public SimpleThreadDispatcher(int threadCount) - { - this.threadCount = threadCount; - workers = new Worker[threadCount - 1]; - for (int i = 0; i < workers.Length; ++i) - { - workers[i] = new Worker { Thread = new Thread(WorkerLoop), Signal = new AutoResetEvent(false) }; - workers[i].Thread.IsBackground = true; - workers[i].Thread.Start(workers[i].Signal); - } - finished = new AutoResetEvent(false); - bufferPools = new BufferPool[threadCount]; - for (int i = 0; i < bufferPools.Length; ++i) - { - bufferPools[i] = new BufferPool(); - } - } - - void DispatchThread(int workerIndex) - { - Debug.Assert(workerBody != null); - workerBody(workerIndex); - - if (Interlocked.Decrement(ref remainingWorkerCounter) == -1) - { - finished.Set(); - } - } - - volatile Action workerBody; - int workerIndex; - int remainingWorkerCounter; - - void WorkerLoop(object untypedSignal) - { - var signal = (AutoResetEvent)untypedSignal; - while (true) - { - signal.WaitOne(); - if (disposed) - return; - DispatchThread(Interlocked.Increment(ref workerIndex) - 1); - } - } - - void SignalThreads(int maximumWorkerCount) - { - //Worker 0 is not signalled; it's the executing thread. - //So if we want 4 total executing threads, we should signal 3 workers. - int maximumWorkersToSignal = maximumWorkerCount - 1; - var workersToSignal = maximumWorkersToSignal < workers.Length ? maximumWorkersToSignal : workers.Length; - remainingWorkerCounter = workersToSignal; - for (int i = 0; i < workersToSignal; ++i) - { - workers[i].Signal.Set(); - } - } - - public void DispatchWorkers(Action workerBody, int maximumWorkerCount = int.MaxValue) - { - if (maximumWorkerCount > 1) - { - Debug.Assert(this.workerBody == null); - workerIndex = 1; //Just make the inline thread worker 0. While the other threads might start executing first, the user should never rely on the dispatch order. - this.workerBody = workerBody; - SignalThreads(maximumWorkerCount); - //Calling thread does work. No reason to spin up another worker and block this one! - DispatchThread(0); - finished.WaitOne(); - this.workerBody = null; - } - else if (maximumWorkerCount == 1) - { - workerBody(0); - } - } - - volatile bool disposed; - - /// - /// Waits for all pending work to complete and then disposes all workers. - /// - public void Dispose() - { - if (!disposed) - { - disposed = true; - SignalThreads(threadCount); - for (int i = 0; i < bufferPools.Length; ++i) - { - bufferPools[i].Clear(); - } - foreach (var worker in workers) - { - worker.Thread.Join(); - worker.Signal.Dispose(); - } - } - } - - public BufferPool GetThreadMemoryPool(int workerIndex) - { - return bufferPools[workerIndex]; - } - } - -} diff --git a/BepuUtilities/SimpleThreadDispatcher2.cs b/BepuUtilities/SimpleThreadDispatcher2.cs deleted file mode 100644 index fed650488..000000000 --- a/BepuUtilities/SimpleThreadDispatcher2.cs +++ /dev/null @@ -1,137 +0,0 @@ -using System; -using System.Diagnostics; -using System.Threading; -using BepuUtilities.Memory; - -namespace BepuUtilities -{ - /// - /// Provides a implementation. Not reentrant. - /// - public class SimpleThreadDispatcher2 : IThreadDispatcher, IDisposable - { - int threadCount; - /// - /// Gets the number of threads to dispatch work on. - /// - public int ThreadCount => threadCount; - struct Worker - { - public Thread Thread; - public AutoResetEvent Signal; - } - - Worker[] workers; - AutoResetEvent finished; - - BufferPool[] bufferPools; - - /// - /// Creates a new thread dispatcher with the given number of threads. - /// - /// Number of threads to dispatch on each invocation. - public SimpleThreadDispatcher2(int threadCount) - { - this.threadCount = threadCount; - workers = new Worker[threadCount - 1]; - for (int i = 0; i < workers.Length; ++i) - { - workers[i] = new Worker { Thread = new Thread(WorkerLoop), Signal = new AutoResetEvent(false) }; - workers[i].Thread.IsBackground = true; - workers[i].Thread.Start((workers[i].Signal, i + 1)); - } - finished = new AutoResetEvent(false); - bufferPools = new BufferPool[threadCount]; - for (int i = 0; i < bufferPools.Length; ++i) - { - bufferPools[i] = new BufferPool(); - } - } - - void DispatchThread(int workerIndex) - { - Debug.Assert(workerBody != null); - workerBody(workerIndex); - - if (Interlocked.Decrement(ref remainingWorkerCounter) == -1) - { - finished.Set(); - } - } - - volatile Action workerBody; - int remainingWorkerCounter; - - void WorkerLoop(object untypedSignal) - { - var (signal, workerIndex) = ((AutoResetEvent, int))untypedSignal; - while (true) - { - signal.WaitOne(); - if (disposed) - return; - DispatchThread(workerIndex); - } - } - - void SignalThreads(int maximumWorkerCount) - { - //Worker 0 is not signalled; it's the executing thread. - //So if we want 4 total executing threads, we should signal 3 workers. - int maximumWorkersToSignal = maximumWorkerCount - 1; - var workersToSignal = maximumWorkersToSignal < workers.Length ? maximumWorkersToSignal : workers.Length; - remainingWorkerCounter = workersToSignal; - for (int i = 0; i < workersToSignal; ++i) - { - workers[i].Signal.Set(); - } - } - - public void DispatchWorkers(Action workerBody, int maximumWorkerCount = int.MaxValue) - { - if (maximumWorkerCount > 1) - { - Debug.Assert(this.workerBody == null); - this.workerBody = workerBody; - SignalThreads(maximumWorkerCount); - //Calling thread does work. No reason to spin up another worker and block this one! - DispatchThread(0); - finished.WaitOne(); - this.workerBody = null; - } - else if (maximumWorkerCount == 1) - { - workerBody(0); - } - } - - volatile bool disposed; - - /// - /// Waits for all pending work to complete and then disposes all workers. - /// - public void Dispose() - { - if (!disposed) - { - disposed = true; - SignalThreads(threadCount); - for (int i = 0; i < bufferPools.Length; ++i) - { - bufferPools[i].Clear(); - } - foreach (var worker in workers) - { - worker.Thread.Join(); - worker.Signal.Dispose(); - } - } - } - - public BufferPool GetThreadMemoryPool(int workerIndex) - { - return bufferPools[workerIndex]; - } - } - -} diff --git a/BepuUtilities/SimpleThreadDispatcher3.cs b/BepuUtilities/SimpleThreadDispatcher3.cs deleted file mode 100644 index d17127609..000000000 --- a/BepuUtilities/SimpleThreadDispatcher3.cs +++ /dev/null @@ -1,141 +0,0 @@ -using System; -using System.Diagnostics; -using System.Threading; -using BepuUtilities.Memory; - -namespace BepuUtilities -{ - /// - /// Provides a implementation. Not reentrant. - /// - public class SimpleThreadDispatcher3 : IThreadDispatcher, IDisposable - { - int threadCount; - /// - /// Gets the number of threads to dispatch work on. - /// - public int ThreadCount => threadCount; - struct Worker - { - public Thread Thread; - } - - Worker[] workers; - ManualResetEventSlim signal; - AutoResetEvent finished; - - BufferPool[] bufferPools; - - /// - /// Creates a new thread dispatcher with the given number of threads. - /// - /// Number of threads to dispatch on each invocation. - public SimpleThreadDispatcher3(int threadCount) - { - this.threadCount = threadCount; - workers = new Worker[threadCount - 1]; - signal = new ManualResetEventSlim(); - finished = new AutoResetEvent(false); - for (int i = 0; i < workers.Length; ++i) - { - workers[i] = new Worker { Thread = new Thread(WorkerLoop) }; - workers[i].Thread.IsBackground = true; - workers[i].Thread.Start(i + 1); - } - bufferPools = new BufferPool[threadCount]; - for (int i = 0; i < bufferPools.Length; ++i) - { - bufferPools[i] = new BufferPool(); - } - } - - void DispatchThread(int workerIndex) - { - Debug.Assert(workerBody != null); - var unclaimedCount = Interlocked.Decrement(ref remainingUnclaimedCounter); - if (unclaimedCount == 0) - { - //No more work, so stop workers from trying to do anything. - signal.Reset(); - } - if (unclaimedCount < 0) - return; - workerBody(unclaimedCount); - - if (Interlocked.Decrement(ref remainingWorkerCounter) == 0) - { - finished.Set(); - } - } - - volatile Action workerBody; - int remainingUnclaimedCounter; - int remainingWorkerCounter; - - void WorkerLoop(object untypedWokrerIndex) - { - var workerIndex = (int)untypedWokrerIndex; - while (true) - { - signal.Wait(); - if (disposed) - return; - DispatchThread(workerIndex); - } - } - - void SignalThreads(int maximumWorkerCount) - { - var workersToSignal = maximumWorkerCount < threadCount ? maximumWorkerCount : threadCount; - remainingUnclaimedCounter = workersToSignal; - remainingWorkerCounter = workersToSignal; - signal.Set(); - } - - public void DispatchWorkers(Action workerBody, int maximumWorkerCount = int.MaxValue) - { - if (maximumWorkerCount > 1) - { - Debug.Assert(this.workerBody == null); - this.workerBody = workerBody; - SignalThreads(maximumWorkerCount); - //Calling thread does work. No reason to spin up another worker and block this one! - DispatchThread(0); - finished.WaitOne(); - this.workerBody = null; - } - else if (maximumWorkerCount == 1) - { - workerBody(0); - } - } - - volatile bool disposed; - - /// - /// Waits for all pending work to complete and then disposes all workers. - /// - public void Dispose() - { - if (!disposed) - { - disposed = true; - SignalThreads(threadCount); - for (int i = 0; i < bufferPools.Length; ++i) - { - bufferPools[i].Clear(); - } - foreach (var worker in workers) - { - worker.Thread.Join(); - } - } - } - - public BufferPool GetThreadMemoryPool(int workerIndex) - { - return bufferPools[workerIndex]; - } - } - -} diff --git a/BepuUtilities/SimpleThreadDispatcher4.cs b/BepuUtilities/SimpleThreadDispatcher4.cs deleted file mode 100644 index b492fa379..000000000 --- a/BepuUtilities/SimpleThreadDispatcher4.cs +++ /dev/null @@ -1,146 +0,0 @@ -using System; -using System.Diagnostics; -using System.Threading; -using BepuUtilities.Memory; - -namespace BepuUtilities -{ - /// - /// Provides a implementation. Not reentrant. - /// - public class SimpleThreadDispatcher4 : IThreadDispatcher, IDisposable - { - int threadCount; - /// - /// Gets the number of threads to dispatch work on. - /// - public int ThreadCount => threadCount; - struct Worker - { - public Thread Thread; - } - - Worker[] workers; - ManualResetEvent signalA; - ManualResetEvent signalB; - AutoResetEvent finished; - - BufferPool[] bufferPools; - - /// - /// Creates a new thread dispatcher with the given number of threads. - /// - /// Number of threads to dispatch on each invocation. - public SimpleThreadDispatcher4(int threadCount) - { - this.threadCount = threadCount; - workers = new Worker[threadCount - 1]; - signalA = new ManualResetEvent(false); - signalB = new ManualResetEvent(false); - finished = new AutoResetEvent(false); - for (int i = 0; i < workers.Length; ++i) - { - workers[i] = new Worker { Thread = new Thread(WorkerLoop) }; - workers[i].Thread.IsBackground = true; - workers[i].Thread.Start(i + 1); - } - bufferPools = new BufferPool[threadCount]; - for (int i = 0; i < bufferPools.Length; ++i) - { - bufferPools[i] = new BufferPool(); - } - } - - void DispatchThread(int workerIndex) - { - Debug.Assert(workerBody != null); - Debug.Assert(remainingWorkerCounter >= 0); - workerBody(workerIndex); - - if (Interlocked.Decrement(ref remainingWorkerCounter) == 0) - { - finished.Set(); - } - } - - volatile Action workerBody; - int remainingWorkerCounter; - - void WorkerLoop(object untypedWorkerIndex) - { - var workerIndex = (int)untypedWorkerIndex; - var signal = this.signalA; - var otherSignal = this.signalB; - while (true) - { - //Signals get ping ponged. - signal.WaitOne(); - if (disposed) - return; - var temp = signal; - signal = otherSignal; - otherSignal = temp; - DispatchThread(workerIndex); - } - } - - void SignalThreads(int maximumWorkerCount) - { - var workersToSignal = maximumWorkerCount < threadCount ? maximumWorkerCount : threadCount; - remainingWorkerCounter = workersToSignal; - signalA.Set(); - } - - public void DispatchWorkers(Action workerBody, int maximumWorkerCount = int.MaxValue) - { - if (maximumWorkerCount > 1) - { - Debug.Assert(this.workerBody == null); - this.workerBody = workerBody; - SignalThreads(maximumWorkerCount); - //Calling thread does work. No reason to spin up another worker and block this one! - DispatchThread(0); - finished.WaitOne(); - signalA.Reset(); - var temp = signalA; - signalA = signalB; - signalB = temp; - this.workerBody = null; - } - else if (maximumWorkerCount == 1) - { - workerBody(0); - } - } - - volatile bool disposed; - - /// - /// Waits for all pending work to complete and then disposes all workers. - /// - public void Dispose() - { - if (!disposed) - { - disposed = true; - SignalThreads(threadCount); - for (int i = 0; i < bufferPools.Length; ++i) - { - bufferPools[i].Clear(); - } - foreach (var worker in workers) - { - worker.Thread.Join(); - } - signalA.Dispose(); - signalB.Dispose(); - } - } - - public BufferPool GetThreadMemoryPool(int workerIndex) - { - return bufferPools[workerIndex]; - } - } - -} diff --git a/BepuUtilities/ThreadDispatcher.cs b/BepuUtilities/ThreadDispatcher.cs index 8c97d7a56..c0a6ad652 100644 --- a/BepuUtilities/ThreadDispatcher.cs +++ b/BepuUtilities/ThreadDispatcher.cs @@ -18,10 +18,10 @@ public class ThreadDispatcher : IThreadDispatcher, IDisposable struct Worker { public Thread Thread; + public AutoResetEvent Signal; } Worker[] workers; - ManualResetEventSlim resetEvent; AutoResetEvent finished; BufferPool[] bufferPools; @@ -30,74 +30,62 @@ struct Worker /// Creates a new thread dispatcher with the given number of threads. /// /// Number of threads to dispatch on each invocation. - public ThreadDispatcher(int threadCount) + /// Size of memory blocks to allocate for thread pools. + public ThreadDispatcher(int threadCount, int threadPoolBlockAllocationSize = 16384) { this.threadCount = threadCount; - resetEvent = new ManualResetEventSlim(false); workers = new Worker[threadCount - 1]; for (int i = 0; i < workers.Length; ++i) { - workers[i] = new Worker { Thread = new Thread(WorkerLoop) }; + workers[i] = new Worker { Thread = new Thread(WorkerLoop), Signal = new AutoResetEvent(false) }; workers[i].Thread.IsBackground = true; - workers[i].Thread.Start(i + 1); + workers[i].Thread.Start((workers[i].Signal, i + 1)); } finished = new AutoResetEvent(false); bufferPools = new BufferPool[threadCount]; for (int i = 0; i < bufferPools.Length; ++i) { - bufferPools[i] = new BufferPool(); + bufferPools[i] = new BufferPool(threadPoolBlockAllocationSize); } } void DispatchThread(int workerIndex) { - while (true) + Debug.Assert(workerBody != null); + workerBody(workerIndex); + + if (Interlocked.Decrement(ref remainingWorkerCounter) == -1) { - var localJobIndex = Interlocked.Increment(ref jobIndex); - if (localJobIndex < jobCount) - { - if (localJobIndex == jobCount - 1) - { - //No further jobs are available, so workers should start waiting again. - resetEvent.Reset(); - } - Debug.Assert(workerBody != null); - workerBody(localJobIndex); - if (Interlocked.Decrement(ref remainingJobCounter) == 0) - { - finished.Set(); - } - } - else - { - return; - } + finished.Set(); } } volatile Action workerBody; - int jobIndex; - int remainingJobCounter; - int jobCount; + int remainingWorkerCounter; void WorkerLoop(object untypedSignal) { - var workerIndex = (int)untypedSignal; + var (signal, workerIndex) = ((AutoResetEvent, int))untypedSignal; while (true) { - resetEvent.Wait(); + signal.WaitOne(); if (disposed) return; DispatchThread(workerIndex); } } - void SignalThreads(int jobCount) + void SignalThreads(int maximumWorkerCount) { - this.jobCount = jobCount; - jobIndex = -1; - remainingJobCounter = jobCount; - resetEvent.Set(); + //Worker 0 is not signalled; it's the executing thread. + //So if we want 4 total executing threads, we should signal 3 workers. + int maximumWorkersToSignal = maximumWorkerCount - 1; + var workersToSignal = maximumWorkersToSignal < workers.Length ? maximumWorkersToSignal : workers.Length; + remainingWorkerCounter = workersToSignal; + for (int i = 0; i < workersToSignal; ++i) + { + workers[i].Signal.Set(); + } } public void DispatchWorkers(Action workerBody, int maximumWorkerCount = int.MaxValue) @@ -106,7 +94,7 @@ public void DispatchWorkers(Action workerBody, int maximumWorkerCount = int { Debug.Assert(this.workerBody == null); this.workerBody = workerBody; - SignalThreads(Math.Min(threadCount, maximumWorkerCount)); + SignalThreads(maximumWorkerCount); //Calling thread does work. No reason to spin up another worker and block this one! DispatchThread(0); finished.WaitOne(); @@ -136,6 +124,7 @@ public void Dispose() foreach (var worker in workers) { worker.Thread.Join(); + worker.Signal.Dispose(); } } } diff --git a/BepuUtilities/ThreadDispatcher2.cs b/BepuUtilities/ThreadDispatcher2.cs deleted file mode 100644 index 4a95188b3..000000000 --- a/BepuUtilities/ThreadDispatcher2.cs +++ /dev/null @@ -1,169 +0,0 @@ -using System; -using System.Diagnostics; -using System.Threading; -using BepuUtilities.Memory; - -namespace BepuUtilities -{ - - /// - /// Provides a implementation. Not reentrant. - /// - public class ThreadDispatcher2 : IThreadDispatcher, IDisposable - { - int threadCount; - /// - /// Gets the number of threads to dispatch work on. - /// - public int ThreadCount => threadCount; - struct Worker - { - public Thread Thread; - } - - Worker[] workers; - AutoResetEvent finished; - BufferPool[] bufferPools; - - /// - /// Creates a new thread dispatcher with the given number of threads. - /// - /// Number of threads to dispatch on each invocation. - /// Size of allocation blocks in the worker thread buffer pools. - public ThreadDispatcher2(int threadCount, int workerPoolBlockAllocationSize = 16384) - { - this.threadCount = threadCount; - workers = new Worker[threadCount - 1]; - for (int i = 0; i < workers.Length; ++i) - { - workers[i] = new Worker { Thread = new Thread(WorkerLoop) }; - workers[i].Thread.IsBackground = true; - //The main thread will be used as worker 0. - workers[i].Thread.Start(i + 1); - } - finished = new AutoResetEvent(false); - bufferPools = new BufferPool[threadCount]; - for (int i = 0; i < bufferPools.Length; ++i) - { - bufferPools[i] = new BufferPool(workerPoolBlockAllocationSize); - } - } - - volatile Action workerBody; - int remainingUnclaimedJobCount; - int remainingUncompletedJobCount; - - object waiter = new object(); - - void DispatchThread(int workerIndex) - { - int jobIndex; - lock (waiter) - { - //Getting here either means 1) this is the startup and there is no job to do, or 2) the local thread just executed a workerBody and we should increment completed work. - if (workerBody != null) - { - //Just finished work; mark it as completed. - if (--remainingUncompletedJobCount == 0) - { - workerBody = null; - finished.Set(); - return; - } - } - if (remainingUnclaimedJobCount == 0 || disposed) - { - return; - } - //Grab the next job. - jobIndex = --remainingUnclaimedJobCount; - } - workerBody(jobIndex); - } - - void WorkerLoop(object untypedWorkerIndex) - { - var workerIndex = (int)untypedWorkerIndex; - while (true) - { - int jobIndex; - lock (waiter) - { - //Getting here either means 1) this is the startup and there is no job to do, or 2) the local thread just executed a workerBody and we should increment completed work. - if (workerBody != null) - { - //Just finished work; mark it as completed. - if (--remainingUncompletedJobCount == 0) - { - workerBody = null; - finished.Set(); - } - } - while (workerBody == null || remainingUnclaimedJobCount == 0) - { - if (disposed) - return; - Monitor.Wait(waiter); - } - //Grab the next job. - jobIndex = --remainingUnclaimedJobCount; - } - workerBody(jobIndex); - } - } - - public void DispatchWorkers(Action workerBody, int maximumWorkerCount = int.MaxValue) - { - if (maximumWorkerCount > 1) - { - lock (waiter) - { - remainingUnclaimedJobCount = maximumWorkerCount < threadCount ? maximumWorkerCount : threadCount; - remainingUncompletedJobCount = remainingUnclaimedJobCount; - Debug.Assert(this.workerBody == null); - this.workerBody = workerBody; - Monitor.PulseAll(waiter); - } - //Calling thread does work. No reason to spin up another worker and block this one! - //DispatchThread(0); - finished.WaitOne(); - } - else if (maximumWorkerCount == 1) - { - workerBody(0); - } - } - - volatile bool disposed; - - /// - /// Waits for all pending work to complete and then disposes all workers. - /// - public void Dispose() - { - if (!disposed) - { - disposed = true; - lock (waiter) - { - Debug.Assert(this.workerBody == null); - Monitor.PulseAll(waiter); - } - for (int i = 0; i < bufferPools.Length; ++i) - { - bufferPools[i].Clear(); - } - foreach (var worker in workers) - { - worker.Thread.Join(); - } - } - } - - public BufferPool GetThreadMemoryPool(int workerIndex) - { - return bufferPools[workerIndex]; - } - } - -} diff --git a/BepuUtilities/ThreadDispatcher3.cs b/BepuUtilities/ThreadDispatcher3.cs deleted file mode 100644 index 4e8b61307..000000000 --- a/BepuUtilities/ThreadDispatcher3.cs +++ /dev/null @@ -1,142 +0,0 @@ -using System; -using System.Diagnostics; -using System.Threading; -using BepuUtilities.Memory; - -namespace BepuUtilities -{ - - /// - /// Provides a implementation. Not reentrant. - /// - public class ThreadDispatcher3 : IThreadDispatcher, IDisposable - { - int threadCount; - /// - /// Gets the number of threads to dispatch work on. - /// - public int ThreadCount => threadCount; - struct Worker - { - public Thread Thread; - } - - Worker[] workers; - AutoResetEvent finished; - BufferPool[] bufferPools; - - /// - /// Creates a new thread dispatcher with the given number of threads. - /// - /// Number of threads to dispatch on each invocation. - /// Size of allocation blocks in the worker thread buffer pools. - public ThreadDispatcher3(int threadCount, int workerPoolBlockAllocationSize = 16384) - { - this.threadCount = threadCount; - semaphore = new SemaphoreSlim(0); - workers = new Worker[threadCount - 1]; - for (int i = 0; i < workers.Length; ++i) - { - workers[i] = new Worker { Thread = new Thread(WorkerLoop) }; - workers[i].Thread.IsBackground = true; - //The main thread will be used as worker 0. - workers[i].Thread.Start(i + 1); - } - finished = new AutoResetEvent(false); - bufferPools = new BufferPool[threadCount]; - for (int i = 0; i < bufferPools.Length; ++i) - { - bufferPools[i] = new BufferPool(workerPoolBlockAllocationSize); - } - } - - volatile Action workerBody; - int remainingUnclaimedJobCount; - int remainingUncompletedJobCount; - - SemaphoreSlim semaphore; - - void DispatchThread(int workerIndex) - { - int jobIndex; - int completedJobCount = 0; - while ((jobIndex = Interlocked.Decrement(ref remainingUnclaimedJobCount)) >= 0) - { - workerBody(jobIndex); - ++completedJobCount; - } - //No more jobs are available. - if (Interlocked.Add(ref remainingUncompletedJobCount, -completedJobCount) == 0) - { - //All jobs are done, no worker is still executing anything. - workerBody = null; - finished.Set(); - } - } - void WorkerLoop(object untypedWorkerIndex) - { - var workerIndex = (int)untypedWorkerIndex; - while (true) - { - semaphore.Wait(); - if (disposed) - return; - DispatchThread(workerIndex); - } - } - - void SignalThreads(int jobCount, Action workerBody) - { - remainingUnclaimedJobCount = jobCount < threadCount ? jobCount : threadCount; - remainingUncompletedJobCount = remainingUnclaimedJobCount; - Debug.Assert(this.workerBody == null); - this.workerBody = workerBody; - semaphore.Release(remainingUnclaimedJobCount); - } - - public void DispatchWorkers(Action workerBody, int maximumWorkerCount = int.MaxValue) - { - if (maximumWorkerCount > 1) - { - SignalThreads(maximumWorkerCount, workerBody); - //Calling thread does work. No reason to spin up another worker and block this one! - DispatchThread(0); - finished.WaitOne(); - } - else if (maximumWorkerCount == 1) - { - workerBody(0); - } - } - - volatile bool disposed; - - /// - /// Waits for all pending work to complete and then disposes all workers. - /// - public void Dispose() - { - if (!disposed) - { - disposed = true; - Debug.Assert(workerBody == null); - SignalThreads(threadCount - 1, null); - finished.WaitOne(); - for (int i = 0; i < bufferPools.Length; ++i) - { - bufferPools[i].Clear(); - } - foreach (var worker in workers) - { - worker.Thread.Join(); - } - } - } - - public BufferPool GetThreadMemoryPool(int workerIndex) - { - return bufferPools[workerIndex]; - } - } - -} diff --git a/BepuUtilities/ThreadDispatcher4.cs b/BepuUtilities/ThreadDispatcher4.cs deleted file mode 100644 index 1610f33e7..000000000 --- a/BepuUtilities/ThreadDispatcher4.cs +++ /dev/null @@ -1,157 +0,0 @@ -using System; -using System.Diagnostics; -using System.Threading; -using BepuUtilities.Memory; - -namespace BepuUtilities -{ - - /// - /// Provides a implementation. Not reentrant. - /// - public class ThreadDispatcher4 : IThreadDispatcher, IDisposable - { - int threadCount; - /// - /// Gets the number of threads to dispatch work on. - /// - public int ThreadCount => threadCount; - struct Worker - { - public Thread Thread; - } - - Worker[] workers; - AutoResetEvent finished; - BufferPool[] bufferPools; - - /// - /// Creates a new thread dispatcher with the given number of threads. - /// - /// Number of threads to dispatch on each invocation. - /// Size of allocation blocks in the worker thread buffer pools. - public ThreadDispatcher4(int threadCount, int workerPoolBlockAllocationSize = 16384) - { - this.threadCount = threadCount; - semaphore = new SemaphoreSlim(0); - workers = new Worker[threadCount - 1]; - for (int i = 0; i < workers.Length; ++i) - { - workers[i] = new Worker { Thread = new Thread(WorkerLoop) }; - workers[i].Thread.IsBackground = true; - //The main thread will be used as worker 0. - workers[i].Thread.Start(i + 1); - } - finished = new AutoResetEvent(false); - bufferPools = new BufferPool[threadCount]; - for (int i = 0; i < bufferPools.Length; ++i) - { - bufferPools[i] = new BufferPool(workerPoolBlockAllocationSize); - } - workClaimed = new int[threadCount]; - } - - Action workerBody; - int remainingUnclaimedJobCount; - int remainingUncompletedJobCount; - - SemaphoreSlim semaphore; - object locker = new object(); - - public int[] workClaimed; - - void DispatchThread(int workerIndex) - { - Action localWorkerBody = null; - int jobIndex; - while (true) - { - lock (locker) - { - if (localWorkerBody != null && --remainingUncompletedJobCount == 0) - { - //This thread just finished up the last chunk of work relevant to the previously cached workerBody. - this.workerBody = null; - finished.Set(); - break; - } - if (remainingUnclaimedJobCount > 0) - jobIndex = --remainingUnclaimedJobCount; - else - break; - localWorkerBody = this.workerBody; - ++workClaimed[workerIndex]; - } - Debug.Assert(jobIndex >= 0 && localWorkerBody != null); - localWorkerBody(jobIndex); - } - } - void WorkerLoop(object untypedWorkerIndex) - { - var workerIndex = (int)untypedWorkerIndex; - while (true) - { - semaphore.Wait(); - if (disposed) - return; - DispatchThread(workerIndex); - } - } - - void SignalThreads(int jobCount, Action workerBody) - { - lock (locker) - { - jobCount = remainingUnclaimedJobCount = jobCount < threadCount ? jobCount : threadCount; - remainingUncompletedJobCount = jobCount; - Debug.Assert(this.workerBody == null); - this.workerBody = workerBody; - } - semaphore.Release(jobCount); - } - - public void DispatchWorkers(Action workerBody, int maximumWorkerCount = int.MaxValue) - { - if (maximumWorkerCount > 1) - { - SignalThreads(maximumWorkerCount, workerBody); - //Calling thread does work. No reason to spin up another worker and block this one! - DispatchThread(0); - finished.WaitOne(); - } - else if (maximumWorkerCount == 1) - { - workerBody(0); - } - } - - volatile bool disposed; - - /// - /// Waits for all pending work to complete and then disposes all workers. - /// - public void Dispose() - { - if (!disposed) - { - disposed = true; - Debug.Assert(workerBody == null); - SignalThreads(threadCount - 1, null); - for (int i = 0; i < bufferPools.Length; ++i) - { - bufferPools[i].Clear(); - } - foreach (var worker in workers) - { - worker.Thread.Join(); - } - } - } - - public BufferPool GetThreadMemoryPool(int workerIndex) - { - return bufferPools[workerIndex]; - } - } - -} diff --git a/BepuUtilities/ThreadDispatcher5.cs b/BepuUtilities/ThreadDispatcher5.cs deleted file mode 100644 index 9e00edbce..000000000 --- a/BepuUtilities/ThreadDispatcher5.cs +++ /dev/null @@ -1,156 +0,0 @@ -using System; -using System.Diagnostics; -using System.Threading; -using BepuUtilities.Memory; - -namespace BepuUtilities -{ - /// - /// Provides a implementation. Not reentrant. - /// - public class ThreadDispatcher5 : IThreadDispatcher, IDisposable - { - int threadCount; - /// - /// Gets the number of threads to dispatch work on. - /// - public int ThreadCount => threadCount; - struct Worker - { - public Thread Thread; - public AutoResetEvent Signal; - } - - Worker[] workers; - AutoResetEvent finished; - - BufferPool[] bufferPools; - - /// - /// Creates a new thread dispatcher with the given number of threads. - /// - /// Number of threads to dispatch on each invocation. - public ThreadDispatcher5(int threadCount) - { - this.threadCount = threadCount; - workers = new Worker[threadCount - 1]; - for (int i = 0; i < workers.Length; ++i) - { - workers[i] = new Worker { Thread = new Thread(WorkerLoop), Signal = new AutoResetEvent(false) }; - workers[i].Thread.IsBackground = true; - workers[i].Thread.Start((workers[i].Signal, i + 1)); - } - finished = new AutoResetEvent(false); - bufferPools = new BufferPool[threadCount]; - for (int i = 0; i < bufferPools.Length; ++i) - { - bufferPools[i] = new BufferPool(); - } - } - - void DispatchThread(Action localAction, int jobIndex, int workerIndex) - { - Debug.Assert(workerBody != null); - localAction(jobIndex); - - if (Interlocked.Decrement(ref remainingWorkerCounter) == 0) - { - finished.Set(); - } - } - - volatile Action workerBody; - int jobIndex; - int jobCount; - int remainingWorkerCounter; - object locker = new object(); - void WorkerLoop(object untypedSignal) - { - (AutoResetEvent signal, int workerIndex) = ((AutoResetEvent, int))untypedSignal; - while (true) - { - if (disposed) - return; - signal.WaitOne(); - while (true) - { - Action localAction; - int jobIndex; - lock (locker) - { - localAction = workerBody; - jobIndex = this.jobIndex++; - if (localAction == null || jobIndex >= jobCount) - break; - } - DispatchThread(localAction, jobIndex, workerIndex); - } - } - } - - void SignalThreads(int maximumWorkerCount) - { - //Worker 0 is not signalled; it's the executing thread. - //So if we want 4 total executing threads, we should signal 3 workers. - int maximumWorkersToSignal = maximumWorkerCount - 1; - var workersToSignal = maximumWorkersToSignal < workers.Length ? maximumWorkersToSignal : workers.Length; - for (int i = 0; i < workersToSignal; ++i) - { - workers[i].Signal.Set(); - } - } - - public void DispatchWorkers(Action workerBody, int maximumWorkerCount = int.MaxValue) - { - if (maximumWorkerCount > 1) - { - Debug.Assert(this.workerBody == null); - lock (locker) - { - jobIndex = 1; //Just make the inline thread worker 0. While the other threads might start executing first, the user should never rely on the dispatch order. - jobCount = Math.Min(maximumWorkerCount, threadCount); - remainingWorkerCounter = jobCount; - this.workerBody = workerBody; - } - SignalThreads(maximumWorkerCount); - //Calling thread does work. No reason to spin up another worker and block this one! - DispatchThread(workerBody, 0, 0); - finished.WaitOne(); - this.workerBody = null; - } - else if (maximumWorkerCount == 1) - { - workerBody(0); - } - } - - volatile bool disposed; - - /// - /// Waits for all pending work to complete and then disposes all workers. - /// - public void Dispose() - { - if (!disposed) - { - disposed = true; - SignalThreads(threadCount); - for (int i = 0; i < bufferPools.Length; ++i) - { - bufferPools[i].Clear(); - } - foreach (var worker in workers) - { - worker.Thread.Join(); - worker.Signal.Dispose(); - } - } - } - - public BufferPool GetThreadMemoryPool(int workerIndex) - { - return bufferPools[workerIndex]; - } - } - -} diff --git a/BepuUtilities/ThreadDispatcher6.cs b/BepuUtilities/ThreadDispatcher6.cs deleted file mode 100644 index 57f7adb1d..000000000 --- a/BepuUtilities/ThreadDispatcher6.cs +++ /dev/null @@ -1,137 +0,0 @@ -using System; -using System.Diagnostics; -using System.Threading; -using BepuUtilities.Memory; - -namespace BepuUtilities -{ - /// - /// Provides a implementation. Not reentrant. - /// - public class ThreadDispatcher6 : IThreadDispatcher, IDisposable - { - int threadCount; - /// - /// Gets the number of threads to dispatch work on. - /// - public int ThreadCount => threadCount; - struct Worker - { - public Thread Thread; - } - - Worker[] workers; - SemaphoreSlim semaphore; - AutoResetEvent finished; - - BufferPool[] bufferPools; - - /// - /// Creates a new thread dispatcher with the given number of threads. - /// - /// Number of threads to dispatch on each invocation. - public ThreadDispatcher6(int threadCount) - { - this.threadCount = threadCount; - semaphore = new SemaphoreSlim(0, threadCount); - workers = new Worker[threadCount - 1]; - for (int i = 0; i < workers.Length; ++i) - { - workers[i] = new Worker { Thread = new Thread(WorkerLoop) }; - workers[i].Thread.IsBackground = true; - workers[i].Thread.Start(i + 1); - } - finished = new AutoResetEvent(false); - bufferPools = new BufferPool[threadCount]; - for (int i = 0; i < bufferPools.Length; ++i) - { - bufferPools[i] = new BufferPool(); - } - } - - void DispatchThread(int jobIndex, int workerIndex) - { - Debug.Assert(workerBody != null); - workerBody(jobIndex); - - if (Interlocked.Decrement(ref remainingWorkerCounter) == -1) - { - finished.Set(); - } - } - - volatile Action workerBody; - int jobIndex; - int remainingWorkerCounter; - - void WorkerLoop(object untypedSignal) - { - var workerIndex = (int)untypedSignal; - while (true) - { - semaphore.Wait(); - if (disposed) - return; - var localJobIndex = Interlocked.Increment(ref jobIndex); - DispatchThread(localJobIndex, workerIndex); - } - } - - void SignalThreads(int maximumWorkerCount) - { - //Worker 0 is not signalled; it's the executing thread. - //So if we want 4 total executing threads, we should signal 3 workers. - int maximumWorkersToSignal = maximumWorkerCount - 1; - var workersToSignal = maximumWorkersToSignal < workers.Length ? maximumWorkersToSignal : workers.Length; - remainingWorkerCounter = workersToSignal; - jobIndex = 0; - semaphore.Release(workersToSignal); - } - - public void DispatchWorkers(Action workerBody, int maximumWorkerCount = int.MaxValue) - { - if (maximumWorkerCount > 1) - { - Debug.Assert(this.workerBody == null); - this.workerBody = workerBody; - SignalThreads(maximumWorkerCount); - //Calling thread does work. No reason to spin up another worker and block this one! - DispatchThread(0, 0); - finished.WaitOne(); - this.workerBody = null; - } - else if (maximumWorkerCount == 1) - { - workerBody(0); - } - } - - volatile bool disposed; - - /// - /// Waits for all pending work to complete and then disposes all workers. - /// - public void Dispose() - { - if (!disposed) - { - disposed = true; - SignalThreads(threadCount); - for (int i = 0; i < bufferPools.Length; ++i) - { - bufferPools[i].Clear(); - } - foreach (var worker in workers) - { - worker.Thread.Join(); - } - } - } - - public BufferPool GetThreadMemoryPool(int workerIndex) - { - return bufferPools[workerIndex]; - } - } - -} diff --git a/BepuUtilities/ThreadDispatcher7.cs b/BepuUtilities/ThreadDispatcher7.cs deleted file mode 100644 index a8ead522f..000000000 --- a/BepuUtilities/ThreadDispatcher7.cs +++ /dev/null @@ -1,134 +0,0 @@ -using System; -using System.Diagnostics; -using System.Threading; -using BepuUtilities.Memory; - -namespace BepuUtilities -{ - - /// - /// Provides a implementation. Not reentrant. - /// - public class ThreadDispatcher7 : IThreadDispatcher, IDisposable - { - int threadCount; - /// - /// Gets the number of threads to dispatch work on. - /// - public int ThreadCount => threadCount; - struct Worker - { - public Thread Thread; - } - - Worker[] workers; - AutoResetEvent finished; - BufferPool[] bufferPools; - - /// - /// Creates a new thread dispatcher with the given number of threads. - /// - /// Number of threads to dispatch on each invocation. - /// Size of allocation blocks in the worker thread buffer pools. - public ThreadDispatcher7(int threadCount, int workerPoolBlockAllocationSize = 16384) - { - this.threadCount = threadCount; - semaphore = new SemaphoreSlim(0); - workers = new Worker[threadCount]; - for (int i = 0; i < workers.Length; ++i) - { - workers[i] = new Worker { Thread = new Thread(WorkerLoop) }; - workers[i].Thread.IsBackground = true; - workers[i].Thread.Start(i); - } - finished = new AutoResetEvent(false); - bufferPools = new BufferPool[threadCount]; - for (int i = 0; i < bufferPools.Length; ++i) - { - bufferPools[i] = new BufferPool(workerPoolBlockAllocationSize); - } - } - - volatile Action workerBody; - int remainingUnclaimedJobCount; - int remainingUncompletedJobCount; - - SemaphoreSlim semaphore; - - void DispatchThread(int workerIndex) - { - var jobIndex = Interlocked.Decrement(ref remainingUnclaimedJobCount); - workerBody(jobIndex); - - //No more jobs are available. - if (Interlocked.Decrement(ref remainingUncompletedJobCount) == 0) - { - //All jobs are done, no worker is still executing anything. - workerBody = null; - finished.Set(); - } - } - void WorkerLoop(object untypedWorkerIndex) - { - var workerIndex = (int)untypedWorkerIndex; - while (true) - { - semaphore.Wait(); - if (disposed) - return; - DispatchThread(workerIndex); - } - } - - void SignalThreads(int jobCount, Action workerBody) - { - remainingUnclaimedJobCount = jobCount < threadCount ? jobCount : threadCount; - remainingUncompletedJobCount = remainingUnclaimedJobCount; - Debug.Assert(this.workerBody == null); - this.workerBody = workerBody; - semaphore.Release(remainingUnclaimedJobCount); - } - - public void DispatchWorkers(Action workerBody, int maximumWorkerCount = int.MaxValue) - { - if (maximumWorkerCount > 1) - { - SignalThreads(maximumWorkerCount, workerBody); - finished.WaitOne(); - } - else if (maximumWorkerCount == 1) - { - workerBody(0); - } - } - - volatile bool disposed; - - /// - /// Waits for all pending work to complete and then disposes all workers. - /// - public void Dispose() - { - if (!disposed) - { - disposed = true; - Debug.Assert(workerBody == null); - SignalThreads(threadCount, null); - for (int i = 0; i < bufferPools.Length; ++i) - { - bufferPools[i].Clear(); - } - foreach (var worker in workers) - { - worker.Thread.Join(); - } - } - } - - public BufferPool GetThreadMemoryPool(int workerIndex) - { - return bufferPools[workerIndex]; - } - } - -} diff --git a/Demos/Demo.cs b/Demos/Demo.cs index 884f0d61b..690a83f81 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -26,7 +26,7 @@ public abstract class Demo : IDisposable /// /// Gets the thread dispatcher available for use by the simulation. /// - public ThreadDispatcher7 ThreadDispatcher { get; private set; } + public ThreadDispatcher ThreadDispatcher { get; private set; } protected Demo() { @@ -44,7 +44,7 @@ protected Demo() //It may be worth using something like hwloc or CPUID to extract extra information to reason about. var targetThreadCount = Math.Max(1, Environment.ProcessorCount > 4 ? Environment.ProcessorCount - 2 : Environment.ProcessorCount - 1); - ThreadDispatcher = new ThreadDispatcher7(targetThreadCount); + ThreadDispatcher = new ThreadDispatcher(targetThreadCount); } public virtual void LoadGraphicalContent(ContentArchive content, RenderSurface surface) diff --git a/Demos/Demos/SimpleSelfContainedDemo.cs b/Demos/Demos/SimpleSelfContainedDemo.cs index c99018c8f..1bc9df7ca 100644 --- a/Demos/Demos/SimpleSelfContainedDemo.cs +++ b/Demos/Demos/SimpleSelfContainedDemo.cs @@ -223,8 +223,8 @@ public static void Run() simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), simulation.Shapes.Add(new Box(500, 1, 500)))); - //Any IThreadDispatcher implementation can be used for multithreading. Here, we use the demos SimpleThreadDispatcher. - var threadDispatcher = new ThreadDispatcher4(Environment.ProcessorCount); + //Any IThreadDispatcher implementation can be used for multithreading. Here, we use the BepuUtilities.ThreadDispatcher implementation. + var threadDispatcher = new ThreadDispatcher(Environment.ProcessorCount); //Now take 100 time steps! for (int i = 0; i < 100; ++i) diff --git a/Demos/Program.cs b/Demos/Program.cs index 56de75fb7..a8431594b 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -14,42 +14,8 @@ namespace Demos { class Program { - [MethodImpl(MethodImplOptions.NoInlining)] - static void Test(int testCount) - { - int workerCount = 512; - var dispatcher = new SimpleThreadDispatcher2(workerCount); - long[] slots = new long[workerCount]; - - long total = 0; - long expectedSum = 0; - for (int q = 0; q < testCount; ++q) - { - var start = Stopwatch.GetTimestamp(); - dispatcher.DispatchWorkers(i => { slots[i] += q * i; }, workerCount); - var stop = Stopwatch.GetTimestamp(); - total += stop - start; - - long sum = 0; - for (int i = 0; i < workerCount; ++i) - { - sum += slots[i]; - expectedSum += q * i; - } - Console.WriteLine($"Sum: {sum}, expected: {expectedSum}"); - } - dispatcher.Dispose(); - Console.WriteLine($"Time per dispatch (us): {total * 1e6 / (testCount * Stopwatch.Frequency)}"); - } static void Main(string[] args) { - //for (int i = 0; i < 32; ++i) - //{ - // Test(32); - //} - //Test(1024); - //return; - var window = new Window("pretty cool multicolored window", new Int2((int)(DisplayDevice.Default.Width * 0.75f), (int)(DisplayDevice.Default.Height * 0.75f)), WindowMode.Windowed); var loop = new GameLoop(window); From c8764b6339d3b60720b3a40818f26aad1ec7ad12 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 24 Jan 2022 19:45:48 -0600 Subject: [PATCH 392/947] Broad phase now executes active/static tree updates in parallel. --- BepuPhysics/CollisionDetection/BroadPhase.cs | 81 +++++- .../Trees/Tree_MultithreadedRefitRefine.cs | 258 ++++++++++-------- .../Trees/Tree_RefinementScheduling.cs | 2 +- Demos/Program.cs | 2 +- 4 files changed, 220 insertions(+), 123 deletions(-) diff --git a/BepuPhysics/CollisionDetection/BroadPhase.cs b/BepuPhysics/CollisionDetection/BroadPhase.cs index 5c79f3821..e50470d64 100644 --- a/BepuPhysics/CollisionDetection/BroadPhase.cs +++ b/BepuPhysics/CollisionDetection/BroadPhase.cs @@ -6,6 +6,8 @@ using System.Runtime.CompilerServices; using System.Numerics; using BepuPhysics.Trees; +using System.Threading; +using BepuUtilities.Collections; namespace BepuPhysics.CollisionDetection { @@ -137,29 +139,84 @@ public void UpdateStaticBounds(int broadPhaseIndex, in Vector3 min, in Vector3 m } int frameIndex; - public void Update(IThreadDispatcher threadDispatcher = null) + int remainingJobCount; + IThreadDispatcher threadDispatcher; + void ExecuteRefitAndMark(int workerIndex) { - if (frameIndex == int.MaxValue) - frameIndex = 0; - if (threadDispatcher != null) + var threadPool = threadDispatcher.GetThreadMemoryPool(workerIndex); + while (true) { - activeRefineContext.RefitAndRefine(ref ActiveTree, Pool, threadDispatcher, frameIndex); + var jobIndex = Interlocked.Decrement(ref remainingJobCount); + if (jobIndex < 0) + break; + if (jobIndex < activeRefineContext.RefitNodes.Count) + { + activeRefineContext.ExecuteRefitAndMarkJob(threadPool, workerIndex, jobIndex); + } + else + { + jobIndex -= activeRefineContext.RefitNodes.Count; + Debug.Assert(jobIndex >= 0 && jobIndex < staticRefineContext.RefitNodes.Count); + staticRefineContext.ExecuteRefitAndMarkJob(threadPool, workerIndex, jobIndex); + } } - else + } + + void ExecuteRefine(int workerIndex) + { + var threadPool = threadDispatcher.GetThreadMemoryPool(workerIndex); + var maximumSubtrees = Math.Max(activeRefineContext.MaximumSubtrees, staticRefineContext.MaximumSubtrees); + var subtreeReferences = new QuickList(maximumSubtrees, threadPool); + var treeletInternalNodes = new QuickList(maximumSubtrees, threadPool); + Tree.CreateBinnedResources(threadPool, maximumSubtrees, out var buffer, out var resources); + while (true) { - ActiveTree.RefitAndRefine(Pool, frameIndex); + var jobIndex = Interlocked.Decrement(ref remainingJobCount); + if (jobIndex < 0) + break; + if (jobIndex < activeRefineContext.RefinementTargets.Count) + { + activeRefineContext.ExecuteRefineJob(ref subtreeReferences, ref treeletInternalNodes, ref resources, threadPool, jobIndex); + } + else + { + jobIndex -= activeRefineContext.RefinementTargets.Count; + Debug.Assert(jobIndex >= 0 && jobIndex < staticRefineContext.RefinementTargets.Count); + staticRefineContext.ExecuteRefineJob(ref subtreeReferences, ref treeletInternalNodes, ref resources, threadPool, jobIndex); + } } + subtreeReferences.Dispose(threadPool); + treeletInternalNodes.Dispose(threadPool); + threadPool.Return(ref buffer); + } - //TODO: for now, the inactive/static tree is simply updated like another active tree. This is enormously inefficient compared to the ideal- - //by nature, static and inactive objects do not move every frame! - //This should be replaced by a dedicated inactive/static refinement approach. It should also run alongside the active tree to extract more parallelism; - //in other words, generate jobs from both trees and dispatch over all of them together. No internal dispatch. + public void Update(IThreadDispatcher threadDispatcher = null) + { + if (frameIndex == int.MaxValue) + frameIndex = 0; if (threadDispatcher != null) { - staticRefineContext.RefitAndRefine(ref StaticTree, Pool, threadDispatcher, frameIndex); + this.threadDispatcher = threadDispatcher; + activeRefineContext.CreateRefitAndMarkJobs(ref ActiveTree, Pool, threadDispatcher); + staticRefineContext.CreateRefitAndMarkJobs(ref StaticTree, Pool, threadDispatcher); + remainingJobCount = activeRefineContext.RefitNodes.Count + staticRefineContext.RefitNodes.Count; + threadDispatcher.DispatchWorkers(ExecuteRefitAndMark, remainingJobCount); + activeRefineContext.CreateRefinementJobs(Pool, threadDispatcher, frameIndex, 1f); + //TODO: for now, the inactive/static tree is simply updated like another active tree. This is enormously inefficient compared to the ideal- + //by nature, static and inactive objects do not move every frame! + //However, the refinement system rarely generates enough work to fill modern beefy machine. Even a million objects might only be 16 refinement jobs. + //To really get the benefit of incremental updates, the tree needs to be reworked to output finer grained work. + //Since the jobs are large, reducing the refinement aggressiveness doesn't change much here. + staticRefineContext.CreateRefinementJobs(Pool, threadDispatcher, frameIndex, 1f); + remainingJobCount = activeRefineContext.RefinementTargets.Count + staticRefineContext.RefinementTargets.Count; + threadDispatcher.DispatchWorkers(ExecuteRefine, remainingJobCount); + activeRefineContext.CleanUpForRefitAndRefine(Pool); + staticRefineContext.CleanUpForRefitAndRefine(Pool); + this.threadDispatcher = null; } else { + ActiveTree.RefitAndRefine(Pool, frameIndex); StaticTree.RefitAndRefine(Pool, frameIndex); } ++frameIndex; diff --git a/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs b/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs index e8784ca9f..fb1688aa2 100644 --- a/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs +++ b/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs @@ -20,7 +20,7 @@ public class RefitAndRefineMultithreadedContext Tree Tree; int RefitNodeIndex; - QuickList RefitNodes; + public QuickList RefitNodes; float RefitCostChange; int RefinementLeafCountThreshold; @@ -28,20 +28,20 @@ public class RefitAndRefineMultithreadedContext Action RefitAndMarkAction; int RefineIndex; - QuickList RefinementTargets; - int MaximumSubtrees; + public QuickList RefinementTargets; + public int MaximumSubtrees; Action RefineAction; IThreadDispatcher threadDispatcher; public RefitAndRefineMultithreadedContext() { - RefitAndMarkAction = RefitAndMark; - RefineAction = Refine; + RefitAndMarkAction = RefitAndMarkForWorker; + RefineAction = RefineForWorker; } - public unsafe void RefitAndRefine(ref Tree tree, BufferPool pool, IThreadDispatcher threadDispatcher, int frameIndex, - float refineAggressivenessScale = 1) + + public unsafe void CreateRefitAndMarkJobs(ref Tree tree, BufferPool pool, IThreadDispatcher threadDispatcher) { if (tree.leafCount <= 2) { @@ -71,7 +71,17 @@ public unsafe void RefitAndRefine(ref Tree tree, BufferPool pool, IThreadDispatc pool, threadDispatcher.GetThreadMemoryPool(0)); RefitNodeIndex = -1; - threadDispatcher.DispatchWorkers(RefitAndMarkAction, RefitNodes.Count); + } + + public unsafe void CreateRefinementJobs(BufferPool pool, IThreadDispatcher threadDispatcher, int frameIndex, float refineAggressivenessScale = 1) + { + if (Tree.leafCount <= 2) + { + //If there are 2 or less leaves, then refit/refine doesn't do anything at all. + //(The root node has no parent, so it does not have a bounding box, and the SAH won't change no matter how we swap the children of the root.) + //Avoiding this case also gives the other codepath a guarantee that it will be working with nodes with two children. + return; + } //Condense the set of candidates into a set of targets. int refinementCandidatesCount = 0; for (int i = 0; i < threadDispatcher.ThreadCount; ++i) @@ -112,8 +122,17 @@ public unsafe void RefitAndRefine(ref Tree tree, BufferPool pool, IThreadDispatc Tree.Metanodes[0].RefineFlag = 1; } RefineIndex = -1; + } - threadDispatcher.DispatchWorkers(RefineAction, RefinementTargets.Count); + public unsafe void CleanUpForRefitAndRefine(BufferPool pool) + { + if (Tree.leafCount <= 2) + { + //If there are 2 or less leaves, then refit/refine doesn't do anything at all. + //(The root node has no parent, so it does not have a bounding box, and the SAH won't change no matter how we swap the children of the root.) + //Avoiding this case also gives the other codepath a guarantee that it will be working with nodes with two children. + return; + } //Note that we defer the refine flag clear until after the refinements complete. If we did it within the refine action itself, //it would introduce nondeterminism by allowing refines to progress based on their order of completion. for (int i = 0; i < RefinementTargets.Count; ++i) @@ -133,6 +152,16 @@ public unsafe void RefitAndRefine(ref Tree tree, BufferPool pool, IThreadDispatc this.threadDispatcher = null; } + public unsafe void RefitAndRefine(ref Tree tree, BufferPool pool, IThreadDispatcher threadDispatcher, int frameIndex, + float refineAggressivenessScale = 1) + { + CreateRefitAndMarkJobs(ref tree, pool, threadDispatcher); + threadDispatcher.DispatchWorkers(RefitAndMarkAction, RefitNodes.Count); + CreateRefinementJobs(pool, threadDispatcher, frameIndex, refineAggressivenessScale); + threadDispatcher.DispatchWorkers(RefineAction, RefinementTargets.Count); + CleanUpForRefitAndRefine(pool); + } + unsafe void CollectNodesForMultithreadedRefit(int nodeIndex, int multithreadingLeafCountThreshold, ref QuickList refitAndMarkTargets, int refinementLeafCountThreshold, ref QuickList refinementCandidates, BufferPool pool, BufferPool threadPool) @@ -172,130 +201,143 @@ unsafe void CollectNodesForMultithreadedRefit(int nodeIndex, } } - unsafe void RefitAndMark(int workerIndex) + public unsafe void ExecuteRefitAndMarkJob(BufferPool threadPool, int workerIndex, int refitIndex) { - //Since resizes may occur, we have to use the thread's buffer pool. - //The main thread already created the refinement candidate list using the worker's pool. - var threadPool = threadDispatcher.GetThreadMemoryPool(workerIndex); - int refitIndex; - Debug.Assert(Tree.leafCount > 2); - while ((refitIndex = Interlocked.Increment(ref RefitNodeIndex)) < RefitNodes.Count) + var nodeIndex = RefitNodes[refitIndex]; + bool shouldUseMark; + if (nodeIndex < 0) + { + //Node was already marked as a wavefront. Should proceed with a RefitAndMeasure instead of RefitAndMark. + nodeIndex = Encode(nodeIndex); + shouldUseMark = false; + } + else { + shouldUseMark = true; + } - var nodeIndex = RefitNodes[refitIndex]; - bool shouldUseMark; - if (nodeIndex < 0) - { - //Node was already marked as a wavefront. Should proceed with a RefitAndMeasure instead of RefitAndMark. - nodeIndex = Encode(nodeIndex); - shouldUseMark = false; - } - else - { - shouldUseMark = true; - } + ref var node = ref Tree.Nodes[nodeIndex]; + ref var metanode = ref Tree.Metanodes[nodeIndex]; + Debug.Assert(metanode.Parent >= 0, "The root should not be marked for refit."); + ref var parent = ref Tree.Nodes[metanode.Parent]; + ref var childInParent = ref Unsafe.Add(ref parent.A, metanode.IndexInParent); + if (shouldUseMark) + { + var costChange = Tree.RefitAndMark(ref childInParent, RefinementLeafCountThreshold, ref RefinementCandidates[workerIndex], threadPool); + metanode.LocalCostChange = costChange; + } + else + { + var costChange = Tree.RefitAndMeasure(ref childInParent); + metanode.LocalCostChange = costChange; + } - ref var node = ref Tree.Nodes[nodeIndex]; - ref var metanode = ref Tree.Metanodes[nodeIndex]; - Debug.Assert(metanode.Parent >= 0, "The root should not be marked for refit."); - ref var parent = ref Tree.Nodes[metanode.Parent]; - ref var childInParent = ref Unsafe.Add(ref parent.A, metanode.IndexInParent); - if (shouldUseMark) - { - var costChange = Tree.RefitAndMark(ref childInParent, RefinementLeafCountThreshold, ref RefinementCandidates[workerIndex], threadPool); - metanode.LocalCostChange = costChange; - } - else - { - var costChange = Tree.RefitAndMeasure(ref childInParent); - metanode.LocalCostChange = costChange; - } + //int foundLeafCount; + //Tree.Validate(RefitNodes.Elements[refitNodeIndex], node->Parent, node->IndexInParent, ref *boundingBoxInParent, out foundLeafCount); - //int foundLeafCount; - //Tree.Validate(RefitNodes.Elements[refitNodeIndex], node->Parent, node->IndexInParent, ref *boundingBoxInParent, out foundLeafCount); + //Walk up the tree. + node = ref parent; + metanode = ref Tree.Metanodes[metanode.Parent]; + while (true) + { - //Walk up the tree. - node = ref parent; - metanode = ref Tree.Metanodes[metanode.Parent]; - while (true) + if (Interlocked.Decrement(ref metanode.RefineFlag) == 0) { - - if (Interlocked.Decrement(ref metanode.RefineFlag) == 0) + //Compute the child contributions to this node's volume change. + ref var children = ref node.A; + metanode.LocalCostChange = 0; + for (int i = 0; i < 2; ++i) { - //Compute the child contributions to this node's volume change. - ref var children = ref node.A; - metanode.LocalCostChange = 0; - for (int i = 0; i < 2; ++i) + ref var child = ref Unsafe.Add(ref children, i); + if (child.Index >= 0) { - ref var child = ref Unsafe.Add(ref children, i); - if (child.Index >= 0) - { - ref var childMetadata = ref Tree.Metanodes[child.Index]; - metanode.LocalCostChange += childMetadata.LocalCostChange; - //Clear the refine flag (unioned). - childMetadata.RefineFlag = 0; - - } + ref var childMetadata = ref Tree.Metanodes[child.Index]; + metanode.LocalCostChange += childMetadata.LocalCostChange; + //Clear the refine flag (unioned). + childMetadata.RefineFlag = 0; + } + } - //This thread is the last thread to visit this node, so it must handle this node. - //Merge all the child bounding boxes into one. - if (metanode.Parent < 0) + //This thread is the last thread to visit this node, so it must handle this node. + //Merge all the child bounding boxes into one. + if (metanode.Parent < 0) + { + //Root node. + //Don't bother including the root's change in volume. + //Refinement can't change the root's bounds, so the fact that the world got bigger or smaller + //doesn't really have any bearing on how much refinement should be done. + //We do, however, need to divide by root volume so that we get the change in cost metric rather than volume. + var merged = new BoundingBox { Min = new Vector3(float.MaxValue), Max = new Vector3(float.MinValue) }; + for (int i = 0; i < 2; ++i) { - //Root node. - //Don't bother including the root's change in volume. - //Refinement can't change the root's bounds, so the fact that the world got bigger or smaller - //doesn't really have any bearing on how much refinement should be done. - //We do, however, need to divide by root volume so that we get the change in cost metric rather than volume. - var merged = new BoundingBox { Min = new Vector3(float.MaxValue), Max = new Vector3(float.MinValue) }; - for (int i = 0; i < 2; ++i) - { - ref var child = ref Unsafe.Add(ref children, i); - BoundingBox.CreateMerged(child.Min, child.Max, merged.Min, merged.Max, out merged.Min, out merged.Max); - } - var postmetric = ComputeBoundsMetric(ref merged); - if (postmetric > 1e-9f) - RefitCostChange = metanode.LocalCostChange / postmetric; - else - RefitCostChange = 0; - //Clear the root's refine flag (unioned). - metanode.RefineFlag = 0; - break; + ref var child = ref Unsafe.Add(ref children, i); + BoundingBox.CreateMerged(child.Min, child.Max, merged.Min, merged.Max, out merged.Min, out merged.Max); } + var postmetric = ComputeBoundsMetric(ref merged); + if (postmetric > 1e-9f) + RefitCostChange = metanode.LocalCostChange / postmetric; else - { - parent = ref Tree.Nodes[metanode.Parent]; - childInParent = ref Unsafe.Add(ref parent.A, metanode.IndexInParent); - var premetric = ComputeBoundsMetric(ref childInParent.Min, ref childInParent.Max); - childInParent.Min = new Vector3(float.MaxValue); - childInParent.Max = new Vector3(float.MinValue); - for (int i = 0; i < 2; ++i) - { - ref var child = ref Unsafe.Add(ref children, i); - BoundingBox.CreateMerged(child.Min, child.Max, childInParent.Min, childInParent.Max, out childInParent.Min, out childInParent.Max); - } - var postmetric = ComputeBoundsMetric(ref childInParent.Min, ref childInParent.Max); - metanode.LocalCostChange += postmetric - premetric; - node = ref parent; - metanode = ref Tree.Metanodes[metanode.Parent]; - } + RefitCostChange = 0; + //Clear the root's refine flag (unioned). + metanode.RefineFlag = 0; + break; } else { - //This thread wasn't the last to visit this node, so it should die. Some other thread will handle it later. - break; + parent = ref Tree.Nodes[metanode.Parent]; + childInParent = ref Unsafe.Add(ref parent.A, metanode.IndexInParent); + var premetric = ComputeBoundsMetric(ref childInParent.Min, ref childInParent.Max); + childInParent.Min = new Vector3(float.MaxValue); + childInParent.Max = new Vector3(float.MinValue); + for (int i = 0; i < 2; ++i) + { + ref var child = ref Unsafe.Add(ref children, i); + BoundingBox.CreateMerged(child.Min, child.Max, childInParent.Min, childInParent.Max, out childInParent.Min, out childInParent.Max); + } + var postmetric = ComputeBoundsMetric(ref childInParent.Min, ref childInParent.Max); + metanode.LocalCostChange += postmetric - premetric; + node = ref parent; + metanode = ref Tree.Metanodes[metanode.Parent]; } } - + else + { + //This thread wasn't the last to visit this node, so it should die. Some other thread will handle it later. + break; + } + } + } + public unsafe void RefitAndMarkForWorker(int workerIndex) + { + if (RefitNodes.Count == 0) + return; + //Since resizes may occur, we have to use the thread's buffer pool. + //The main thread already created the refinement candidate list using the worker's pool. + var threadPool = threadDispatcher.GetThreadMemoryPool(workerIndex); + int refitIndex; + Debug.Assert(Tree.leafCount > 2); + while ((refitIndex = Interlocked.Increment(ref RefitNodeIndex)) < RefitNodes.Count) + { + ExecuteRefitAndMarkJob(threadPool, workerIndex, refitIndex); } } - unsafe void Refine(int workerIndex) + public unsafe void ExecuteRefineJob(ref QuickList subtreeReferences, ref QuickList treeletInternalNodes, ref BinnedResources resources, BufferPool threadPool, int refineIndex) { + Tree.BinnedRefine(RefinementTargets[refineIndex], ref subtreeReferences, MaximumSubtrees, ref treeletInternalNodes, ref resources, threadPool); + subtreeReferences.Count = 0; + treeletInternalNodes.Count = 0; + } + + public unsafe void RefineForWorker(int workerIndex) + { + if (RefinementTargets.Count == 0) + return; var threadPool = threadDispatcher.GetThreadMemoryPool(workerIndex); var subtreeCountEstimate = (int)BitOperations.RoundUpToPowerOf2((uint)MaximumSubtrees); var subtreeReferences = new QuickList(subtreeCountEstimate, threadPool); @@ -306,9 +348,7 @@ unsafe void Refine(int workerIndex) int refineIndex; while ((refineIndex = Interlocked.Increment(ref RefineIndex)) < RefinementTargets.Count) { - Tree.BinnedRefine(RefinementTargets[refineIndex], ref subtreeReferences, MaximumSubtrees, ref treeletInternalNodes, ref resources, threadPool); - subtreeReferences.Count = 0; - treeletInternalNodes.Count = 0; + ExecuteRefineJob(ref subtreeReferences, ref treeletInternalNodes, ref resources, threadPool, refineIndex); } subtreeReferences.Dispose(threadPool); diff --git a/BepuPhysics/Trees/Tree_RefinementScheduling.cs b/BepuPhysics/Trees/Tree_RefinementScheduling.cs index 1f0f9b2ce..2cfe3ba01 100644 --- a/BepuPhysics/Trees/Tree_RefinementScheduling.cs +++ b/BepuPhysics/Trees/Tree_RefinementScheduling.cs @@ -170,7 +170,7 @@ readonly void GetRefineTuning(int frameIndex, int refinementCandidatesCount, flo var refineAggressiveness = Math.Max(0, costChange * refineAggressivenessScale); float refinePortion = Math.Min(1, refineAggressiveness * 0.25f); - var targetRefinementScale = Math.Min(nodeCount, Math.Max(2, (float)Math.Ceiling(refinementCandidatesCount * 0.03f)) + refinementCandidatesCount * refinePortion); + var targetRefinementScale = Math.Min(nodeCount, Math.Max(2, (float)Math.Ceiling(refinementCandidatesCount * refineAggressivenessScale * 0.03f)) + refinementCandidatesCount * refinePortion); //Note that the refinementCandidatesCount is used as a maximum instead of refinementCandidates + 1 for simplicity, since there's a chance //that the root would already be a refinementCandidate. Doesn't really have a significant effect either way. refinementPeriod = Math.Max(1, (int)(refinementCandidatesCount / targetRefinementScale)); diff --git a/Demos/Program.cs b/Demos/Program.cs index a8431594b..2dddd9ea8 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -25,7 +25,7 @@ static void Main(string[] args) content = ContentArchive.Load(stream); } //DeterminismTest.Test(content, 2, 32768); - //HeadlessTest.Test(content, 4, 64, 512); + //HeadlessTest.Test(content, 16, 64, 512); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); From 3a9e525ffa8d115876219129035ec59ac07381e8 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 24 Jan 2022 19:55:36 -0600 Subject: [PATCH 393/947] Skip multithreading on very small sleep attempts. --- BepuPhysics/IslandSleeper.cs | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/BepuPhysics/IslandSleeper.cs b/BepuPhysics/IslandSleeper.cs index c17e03621..68b010ae6 100644 --- a/BepuPhysics/IslandSleeper.cs +++ b/BepuPhysics/IslandSleeper.cs @@ -954,6 +954,10 @@ internal void Update(IThreadDispatcher threadDispatcher, bool deterministic) } ++scheduleOffset; + //If the simulation is too small to generate parallel work, don't bother using threading. (Passing a null thread dispatcher forces a single threaded codepath.) + if (bodies.ActiveSet.Count < 2 / TestedFractionPerFrame) + threadDispatcher = null; + Sleep(ref traversalStartBodyIndices, threadDispatcher, deterministic, (int)Math.Ceiling(bodies.ActiveSet.Count * TargetSleptFraction), (int)Math.Ceiling(bodies.ActiveSet.Count * TargetTraversedFraction), false); traversalStartBodyIndices.Dispose(pool); From eb939ca8cc295622b31c83f7976b0bd43994e598 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 25 Jan 2022 13:33:55 -0600 Subject: [PATCH 394/947] Prevented old data in RefitNodes/RefinementTargets from sticking around. --- BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs | 2 ++ 1 file changed, 2 insertions(+) diff --git a/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs b/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs index fb1688aa2..4589bafcd 100644 --- a/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs +++ b/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs @@ -48,6 +48,7 @@ public unsafe void CreateRefitAndMarkJobs(ref Tree tree, BufferPool pool, IThrea //If there are 2 or less leaves, then refit/refine doesn't do anything at all. //(The root node has no parent, so it does not have a bounding box, and the SAH won't change no matter how we swap the children of the root.) //Avoiding this case also gives the other codepath a guarantee that it will be working with nodes with two children. + RefitNodes = default; return; } this.threadDispatcher = threadDispatcher; @@ -80,6 +81,7 @@ public unsafe void CreateRefinementJobs(BufferPool pool, IThreadDispatcher threa //If there are 2 or less leaves, then refit/refine doesn't do anything at all. //(The root node has no parent, so it does not have a bounding box, and the SAH won't change no matter how we swap the children of the root.) //Avoiding this case also gives the other codepath a guarantee that it will be working with nodes with two children. + RefinementTargets = default; return; } //Condense the set of candidates into a set of targets. From 69dd2deabe06ab60ce53fbf11f2b334cc9846998 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 25 Jan 2022 14:11:50 -0600 Subject: [PATCH 395/947] Newton's semifunctional cradle. --- Demos/SpecializedTests/NewtonsCradleDemo.cs | 61 +++++++++++++++++++++ 1 file changed, 61 insertions(+) create mode 100644 Demos/SpecializedTests/NewtonsCradleDemo.cs diff --git a/Demos/SpecializedTests/NewtonsCradleDemo.cs b/Demos/SpecializedTests/NewtonsCradleDemo.cs new file mode 100644 index 000000000..52c688c92 --- /dev/null +++ b/Demos/SpecializedTests/NewtonsCradleDemo.cs @@ -0,0 +1,61 @@ +using BepuPhysics; +using BepuPhysics.Collidables; +using BepuPhysics.Constraints; +using DemoContentLoader; +using DemoRenderer; +using DemoUtilities; +using System; +using System.Numerics; + +namespace Demos.SpecializedTests +{ + /// + /// Shows a newton's cradle, primarily for behavioral experimentation (in case an alternative solver is ever implemented). + /// The type of solver currently used does not handle the conservation of momentum over the constraint graph in the expected way. + /// The bounce gets distributed fuzzily. + /// + public class NewtonsCradleDemo : Demo + { + public unsafe override void Initialize(ContentArchive content, Camera camera) + { + camera.Yaw = 0; + camera.Pitch = 0; + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(20, 0), float.MaxValue, 0), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0), 0, 0), new SolveDescription(1, 1)); + + const int ballCount = 50; + const float ballRadius = 0.5f; + const float ballSpacing = 0.08f; + const float ballHangHeight = 12f; + const float barSpacing = 3f; + + var barShape = new Box(ballCount * ballRadius * 2 + (ballCount - 1) * ballSpacing, 0.2f, 0.2f); + var barShapeIndex = Simulation.Shapes.Add(barShape); + var bar0 = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(barShape.HalfWidth, ballHangHeight, barSpacing * -0.5f), barShapeIndex, 0f)); + var bar1 = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(barShape.HalfWidth, ballHangHeight, barSpacing * 0.5f), barShapeIndex, 0f)); + + camera.Position = new Vector3(barShape.HalfWidth, ballHangHeight * 0.5f, 2 + Math.Max(ballHangHeight, barShape.HalfWidth)); + + var ballShape = new Sphere(ballRadius); + var ballShapeIndex = Simulation.Shapes.Add(ballShape); + var ballInertia = ballShape.ComputeInertia(1); + var ballConstraintSpringSettings = new SpringSettings(300, 1); + for (int i = 0; i < ballCount; ++i) + { + var ballPosition = new Vector3(ballRadius + i * (ballSpacing + ballRadius * 2), 0, 0); + var ball = Simulation.Bodies.Add(BodyDescription.CreateDynamic(ballPosition, ballInertia, new CollidableDescription(ballShapeIndex, 0), 0.0f)); + Simulation.Solver.Add(ball, bar0, new BallSocket { LocalOffsetA = new Vector3(0, ballHangHeight, -barSpacing * 0.5f), LocalOffsetB = new Vector3(ballPosition.X - barShape.HalfWidth, 0, 0), SpringSettings = ballConstraintSpringSettings }); + Simulation.Solver.Add(ball, bar1, new BallSocket { LocalOffsetA = new Vector3(0, ballHangHeight, barSpacing * 0.5f), LocalOffsetB = new Vector3(ballPosition.X - barShape.HalfWidth, 0, 0), SpringSettings = ballConstraintSpringSettings }); + } + + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -ballHangHeight - ballRadius - 1 - 0.5f, 0), Simulation.Shapes.Add(new Box(2500, 1, 2500)))); + } + + public override void Update(Window window, Camera camera, Input input, float dt) + { + const int substeps = 100; + for (int i = 0; i < substeps; ++i) + Simulation.Timestep(1f / (60f * substeps)); + } + + } +} From 94dd4231a9ca484183efacc994162e5f667716a9 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 27 Jan 2022 17:53:57 -0600 Subject: [PATCH 396/947] Added callbacks->contact material tuning breadcrumb to demos. Tuned substepping demo. --- .../ConstraintDescriptionMappingTests.cs | 2 +- Demos/Demos/CollisionQueryDemo.cs | 3 +- Demos/Demos/ColosseumDemo.cs | 3 +- Demos/Demos/CompoundTestDemo.cs | 3 +- Demos/Demos/CustomVoxelCollidableDemo.cs | 3 +- Demos/Demos/Dancers/DancerDemo.cs | 2 +- Demos/Demos/Dancers/PlumpDancerDemo.cs | 2 +- Demos/Demos/PlanetDemo.cs | 2 +- Demos/Demos/PyramidDemo.cs | 3 +- Demos/Demos/RagdollDemo.cs | 20 ++++-- Demos/Demos/RopeTwistDemo.cs | 20 +++--- Demos/Demos/SolverContactEnumerationDemo.cs | 3 +- Demos/Demos/SubsteppingDemo.cs | 63 ++++++++++++++----- Demos/Demos/SweepDemo.cs | 3 +- Demos/Program.cs | 2 +- .../BroadPhaseStressTestDemo.cs | 3 +- Demos/SpecializedTests/CapsuleTestDemo.cs | 3 +- Demos/SpecializedTests/ClothLatticeDemo.cs | 2 +- Demos/SpecializedTests/CompoundBoundTests.cs | 3 +- .../ConstrainedKinematicIntegrationTest.cs | 5 +- Demos/SpecializedTests/ConstraintTestDemo.cs | 3 +- Demos/SpecializedTests/ConvexHullTestDemo.cs | 3 +- Demos/SpecializedTests/CylinderTestDemo.cs | 3 +- .../FountainStressTestDemo.cs | 3 +- Demos/SpecializedTests/GyroscopeTestDemo.cs | 2 +- .../Media/NewtDemandingSacrificeVideoDemo.cs | 2 +- .../Media/PyramidVideoDemo.cs | 3 +- .../Media/ShrinkwrappedNewtsVideoDemo.cs | 3 +- Demos/SpecializedTests/MeshMeshTestDemo.cs | 3 +- .../SpecializedTests/MeshReductionTestDemo.cs | 4 +- .../MeshSerializationTestDemo.cs | 3 +- Demos/SpecializedTests/MeshTestDemo.cs | 3 +- .../PyramidAwakenerTestDemo.cs | 3 +- Demos/SpecializedTests/RagdollTubeDemo.cs | 4 +- Demos/SpecializedTests/ShapePileTestDemo.cs | 3 +- Demos/SpecializedTests/SolverBatchTestDemo.cs | 2 +- Demos/SpecializedTests/TriangleRayTestDemo.cs | 2 +- Demos/SpecializedTests/TriangleTestDemo.cs | 5 +- 38 files changed, 132 insertions(+), 72 deletions(-) diff --git a/DemoTests/ConstraintDescriptionMappingTests.cs b/DemoTests/ConstraintDescriptionMappingTests.cs index 6bce7ce0f..2250fcb32 100644 --- a/DemoTests/ConstraintDescriptionMappingTests.cs +++ b/DemoTests/ConstraintDescriptionMappingTests.cs @@ -25,7 +25,7 @@ static void FillWithRandomBytes(ref T item, Random random) where T : struct } static void Test(BufferPool pool, Random random, int constraintTypeBodyCount) where T : unmanaged, IConstraintDescription { - var simulation = Simulation.Create(pool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(), new SolveDescription(1, 4)); + var simulation = Simulation.Create(pool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(), new SolveDescription(1, 4)); const int bodyCount = 2048; diff --git a/Demos/Demos/CollisionQueryDemo.cs b/Demos/Demos/CollisionQueryDemo.cs index 11e57d8cb..40f4f8d41 100644 --- a/Demos/Demos/CollisionQueryDemo.cs +++ b/Demos/Demos/CollisionQueryDemo.cs @@ -1,6 +1,7 @@ using BepuPhysics; using BepuPhysics.Collidables; using BepuPhysics.CollisionDetection; +using BepuPhysics.Constraints; using BepuUtilities; using BepuUtilities.Collections; using BepuUtilities.Memory; @@ -27,7 +28,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Yaw = 0; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(new Box(100, 1, 100)))); diff --git a/Demos/Demos/ColosseumDemo.cs b/Demos/Demos/ColosseumDemo.cs index 8516bc5cb..d6b46f3ee 100644 --- a/Demos/Demos/ColosseumDemo.cs +++ b/Demos/Demos/ColosseumDemo.cs @@ -1,5 +1,6 @@ using BepuPhysics; using BepuPhysics.Collidables; +using BepuPhysics.Constraints; using BepuUtilities; using DemoContentLoader; using DemoRenderer; @@ -64,7 +65,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = MathHelper.Pi * 0.2f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); var ringBoxShape = new Box(0.5f, 1, 3); var boxDescription = BodyDescription.CreateDynamic(new Vector3(), ringBoxShape.ComputeInertia(1), Simulation.Shapes.Add(ringBoxShape), 0.01f); diff --git a/Demos/Demos/CompoundTestDemo.cs b/Demos/Demos/CompoundTestDemo.cs index 99d49c87e..5dc8c1e38 100644 --- a/Demos/Demos/CompoundTestDemo.cs +++ b/Demos/Demos/CompoundTestDemo.cs @@ -5,6 +5,7 @@ using System; using System.Numerics; using DemoContentLoader; +using BepuPhysics.Constraints; namespace Demos.Demos { @@ -16,7 +17,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = MathHelper.Pi * 0.05f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10f, 0)), new SolveDescription(8, 1)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10f, 0)), new SolveDescription(8, 1)); using (var compoundBuilder = new CompoundBuilder(BufferPool, Simulation.Shapes, 8)) { diff --git a/Demos/Demos/CustomVoxelCollidableDemo.cs b/Demos/Demos/CustomVoxelCollidableDemo.cs index c8cc1f51f..8315ab13c 100644 --- a/Demos/Demos/CustomVoxelCollidableDemo.cs +++ b/Demos/Demos/CustomVoxelCollidableDemo.cs @@ -14,6 +14,7 @@ using DemoUtilities; using System; using BepuPhysics.CollisionDetection.SweepTasks; +using BepuPhysics.Constraints; namespace Demos.Demos { @@ -374,7 +375,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = MathHelper.Pi * 0.05f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); //The narrow phase must be notified about the existence of the new collidable type. For every pair type we want to support, a collision task must be registered. //All of the default engine types are registered upon simulation creation by a call to DefaultTypes.CreateDefaultCollisionTaskRegistry. diff --git a/Demos/Demos/Dancers/DancerDemo.cs b/Demos/Demos/Dancers/DancerDemo.cs index 31fadc9ae..e6a41038b 100644 --- a/Demos/Demos/Dancers/DancerDemo.cs +++ b/Demos/Demos/Dancers/DancerDemo.cs @@ -160,7 +160,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0; var collisionFilters = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks { CollisionFilters = collisionFilters }, new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)), new SolveDescription(8, 1)); + Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks(collisionFilters), new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)), new SolveDescription(8, 1)); dancers = new DemoDancers().Initialize(16, 16, Simulation, collisionFilters, ThreadDispatcher, BufferPool, new SolveDescription(1, 4), TailorDress, new ClothCollisionFilter(0, 0, -1)); diff --git a/Demos/Demos/Dancers/PlumpDancerDemo.cs b/Demos/Demos/Dancers/PlumpDancerDemo.cs index bba0463d0..ce333dd62 100644 --- a/Demos/Demos/Dancers/PlumpDancerDemo.cs +++ b/Demos/Demos/Dancers/PlumpDancerDemo.cs @@ -217,7 +217,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0; var collisionFilters = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks { CollisionFilters = collisionFilters }, new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)), new SolveDescription(8, 1)); + Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks(collisionFilters), new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)), new SolveDescription(8, 1)); //Note that, because the constraints in the fat suit are quite soft, we can get away with extremely minimal solving time. There's one substep with one velocity iteration. dancers = new DemoDancers().Initialize(8, 8, Simulation, collisionFilters, ThreadDispatcher, BufferPool, new SolveDescription(1, 1), CreateFatSuit, new DeformableCollisionFilter(0, 0, 0, -1)); diff --git a/Demos/Demos/PlanetDemo.cs b/Demos/Demos/PlanetDemo.cs index 92c0fd18a..a0a248fd6 100644 --- a/Demos/Demos/PlanetDemo.cs +++ b/Demos/Demos/PlanetDemo.cs @@ -58,7 +58,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new PlanetaryGravityCallbacks() { PlanetCenter = new Vector3(), Gravity = 100000 }, new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new PlanetaryGravityCallbacks() { PlanetCenter = new Vector3(), Gravity = 100000 }, new SolveDescription(1, 4)); Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(new Sphere(50)))); diff --git a/Demos/Demos/PyramidDemo.cs b/Demos/Demos/PyramidDemo.cs index 549df6d84..bfdad9322 100644 --- a/Demos/Demos/PyramidDemo.cs +++ b/Demos/Demos/PyramidDemo.cs @@ -1,5 +1,6 @@ using BepuPhysics; using BepuPhysics.Collidables; +using BepuPhysics.Constraints; using BepuUtilities; using DemoContentLoader; using DemoRenderer; @@ -22,7 +23,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-30, 8, -110); camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); var boxShape = new Box(1, 1, 1); var boxInertia = boxShape.ComputeInertia(1); diff --git a/Demos/Demos/RagdollDemo.cs b/Demos/Demos/RagdollDemo.cs index 48d09ce60..ffcd0df2e 100644 --- a/Demos/Demos/RagdollDemo.cs +++ b/Demos/Demos/RagdollDemo.cs @@ -95,6 +95,20 @@ public static bool AllowCollision(in SubgroupCollisionFilter a, in SubgroupColli struct SubgroupFilteredCallbacks : INarrowPhaseCallbacks { public CollidableProperty CollisionFilters; + public PairMaterialProperties Material; + + public SubgroupFilteredCallbacks(CollidableProperty filters) + { + CollisionFilters = filters; + Material = new PairMaterialProperties(1, 2, new SpringSettings(30, 1)); + } + public SubgroupFilteredCallbacks(CollidableProperty filters, PairMaterialProperties material) + { + CollisionFilters = filters; + Material = material; + } + + public void Initialize(Simulation simulation) { CollisionFilters.Initialize(simulation); @@ -120,9 +134,7 @@ public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int chi [MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold { - pairMaterial.FrictionCoefficient = 1; - pairMaterial.MaximumRecoveryVelocity = 2f; - pairMaterial.SpringSettings = new SpringSettings(30, 1); + pairMaterial = Material; return true; } @@ -535,7 +547,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = MathHelper.Pi * 0.05f; var collisionFilters = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks { CollisionFilters = collisionFilters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks(collisionFilters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); int ragdollIndex = 0; var spacing = new Vector3(2f, 3, 1); diff --git a/Demos/Demos/RopeTwistDemo.cs b/Demos/Demos/RopeTwistDemo.cs index e72c0796d..395e928af 100644 --- a/Demos/Demos/RopeTwistDemo.cs +++ b/Demos/Demos/RopeTwistDemo.cs @@ -24,14 +24,20 @@ struct Filter unsafe struct RopeNarrowPhaseCallbacks : INarrowPhaseCallbacks { public CollidableProperty Filters; - public SpringSettings ContactSpringiness; + PairMaterialProperties Material; + + public RopeNarrowPhaseCallbacks(CollidableProperty filters, PairMaterialProperties contactMaterial) + { + Filters = filters; + Material = contactMaterial; + } + public RopeNarrowPhaseCallbacks(CollidableProperty filters) : this(filters, new PairMaterialProperties(1, 2, new SpringSettings(30, 1))) + { + } public void Initialize(Simulation simulation) { Filters.Initialize(simulation); - //Use a default if the springiness value wasn't initialized. - if (ContactSpringiness.AngularFrequency == 0 && ContactSpringiness.TwiceDampingRatio == 0) - ContactSpringiness = new SpringSettings(30, 1); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -51,9 +57,7 @@ public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int chi [MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold { - pairMaterial.FrictionCoefficient = 0f; - pairMaterial.MaximumRecoveryVelocity = 200f; - pairMaterial.SpringSettings = ContactSpringiness; + pairMaterial = Material; return true; } @@ -76,7 +80,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var filters = new CollidableProperty(); Simulation = Simulation.Create(BufferPool, - new RopeNarrowPhaseCallbacks { ContactSpringiness = new SpringSettings(2000, 1), Filters = filters }, + new RopeNarrowPhaseCallbacks(filters, new PairMaterialProperties(2, float.MaxValue, new SpringSettings(2000, 1))), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 60)); for (int twistIndex = 0; twistIndex < 1; ++twistIndex) diff --git a/Demos/Demos/SolverContactEnumerationDemo.cs b/Demos/Demos/SolverContactEnumerationDemo.cs index c84e394eb..d17b50e00 100644 --- a/Demos/Demos/SolverContactEnumerationDemo.cs +++ b/Demos/Demos/SolverContactEnumerationDemo.cs @@ -1,6 +1,7 @@ using BepuPhysics; using BepuPhysics.Collidables; using BepuPhysics.CollisionDetection; +using BepuPhysics.Constraints; using BepuPhysics.Constraints.Contact; using BepuUtilities; using BepuUtilities.Collections; @@ -26,7 +27,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Yaw = 0; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); //Drop a pyramid on top of the sensor so there are more contacts to look at. var boxShape = new Box(1, 1, 1); diff --git a/Demos/Demos/SubsteppingDemo.cs b/Demos/Demos/SubsteppingDemo.cs index 485f34381..d7d149405 100644 --- a/Demos/Demos/SubsteppingDemo.cs +++ b/Demos/Demos/SubsteppingDemo.cs @@ -2,6 +2,8 @@ using BepuPhysics.Collidables; using BepuPhysics.Constraints; using BepuUtilities; +using BepuUtilities.Collections; +using BepuUtilities.Memory; using DemoContentLoader; using DemoRenderer; using DemoRenderer.UI; @@ -22,24 +24,23 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(0, 25, 80); camera.Yaw = 0; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, - new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(120, 120), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 2f }, - new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 8)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(640, 480), float.MaxValue, 1), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(2, 48)); rolloverInfo = new RolloverInfo(); { - //We'll create a 0 level arm rope like the one from the RopeStabilityDemo. No skip constraints, though- and the mass ratio will be 1000:1 instead of 100:1! + //We'll create a 0 level arm rope like the one from the RopeStabilityDemo. No skip constraints, though- and the mass ratio will be 10000:1 instead of 100:1! var startLocation = new Vector3(15, 40, 0); const float bodySpacing = 0.3f; const float bodyRadius = 0.5f; - var springSettings = new SpringSettings(240, 480); + var springSettings = new SpringSettings(480, 480); var bodyHandles = RopeStabilityDemo.BuildRope(Simulation, startLocation, 12, bodyRadius, bodySpacing, 0, 1, 0, springSettings); var bigWreckingBall = new Sphere(5); - var bigWreckingBallInertia = bigWreckingBall.ComputeInertia(1000); + const float mass = 10000; + var bigWreckingBallInertia = bigWreckingBall.ComputeInertia(mass); RopeStabilityDemo.AttachWreckingBall(Simulation, bodyHandles, bodyRadius, bodySpacing, 0, bigWreckingBall.Radius, bigWreckingBallInertia, Simulation.Shapes.Add(bigWreckingBall), springSettings); - rolloverInfo.Add(startLocation + new Vector3(0, 2, 0), "1000:1 mass ratio"); + rolloverInfo.Add(startLocation + new Vector3(0, 2, 0), $"{mass}:1 mass ratio"); } { @@ -48,19 +49,19 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //(Note that the demos timestep frequency is 60hz, so 4 substeps is a 240hz solve rate- twice the 120hz contact frequency.) var boxShape = new Box(4, 0.5f, 6f); var boxInertia = boxShape.ComputeInertia(1); - //Note that sleeping is disabled with a negative velocity threshold. We want to watch the stack as we change simulation settings; if it's inactive, it won't respond! - var boxDescription = BodyDescription.CreateDynamic(new Vector3(), boxInertia, Simulation.Shapes.Add(boxShape), -1f); + var boxDescription = BodyDescription.CreateDynamic(new Vector3(), boxInertia, Simulation.Shapes.Add(boxShape), 0.01f); for (int i = 0; i < 20; ++i) { boxDescription.Pose = (new Vector3(0, 0.5f + boxShape.Height * (i + 0.5f), 0), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, MathF.PI * 0.05f * i)); Simulation.Bodies.Add(boxDescription); } var topBlockShape = new Box(8, 2, 8); + const float mass = 10000; Simulation.Bodies.Add( - BodyDescription.CreateDynamic(boxDescription.Pose.Position + new Vector3(0, boxShape.HalfHeight + 1f, 0), topBlockShape.ComputeInertia(200), - Simulation.Shapes.Add(topBlockShape), -1f)); + BodyDescription.CreateDynamic(boxDescription.Pose.Position + new Vector3(0, boxShape.HalfHeight + 1f, 0), topBlockShape.ComputeInertia(mass), + Simulation.Shapes.Add(topBlockShape), .01f)); - rolloverInfo.Add(boxDescription.Pose.Position + new Vector3(0, 4, 0), "200:1 mass ratio"); + rolloverInfo.Add(boxDescription.Pose.Position + new Vector3(0, 4, 0), $"{mass}:1 mass ratio"); } { @@ -78,7 +79,6 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var previousLinkHandle = Simulation.Bodies.Add(BodyDescription.CreateKinematic(linkDescription.Pose.Position, boxShapeIndex, 0.01f)); for (int linkIndex = 0; linkIndex < 8; ++linkIndex) { - var previousPosition = linkDescription.Pose.Position; var offset = new Vector3(boxShape.Width * 1.05f, 0, boxShape.Length - boxShape.Width); linkDescription.Pose.Position += offset; var linkHandle = Simulation.Bodies.Add(linkDescription); @@ -89,7 +89,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) LocalOffsetA = offset * 0.5f, LocalOffsetB = offset * -0.5f, //Once again, the choice of high stiffness makes this potentially unstable without substepping. - SpringSettings = new SpringSettings(120, 1) + SpringSettings = new SpringSettings(480, 1) }); Simulation.Solver.Add(previousLinkHandle, linkHandle, new AngularAxisMotor { @@ -107,6 +107,24 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), Simulation.Shapes.Add(new Box(200, 1, 200)))); } + + unsafe void AwakenAllBodies() + { + //Any time the simulation configuration changes, it could change behavior. + //For example, reducing substep/iteration count to very low values will cause severe instability. + //Sleeping objects don't move, though, so wake them up so the result of the change can be seen! + var sleepingSetsMemory = stackalloc int[Simulation.Bodies.Sets.Length - 1]; + var sleepingSets = new QuickList(new Buffer(sleepingSetsMemory, Simulation.Bodies.Sets.Length - 1)); + for (int i = 1; i < Simulation.Bodies.Sets.Length; ++i) + { + if (Simulation.Bodies.Sets[i].Allocated) + { + sleepingSets.AllocateUnsafely() = i; + } + } + Simulation.Awakener.AwakenSets(ref sleepingSets); + } + public override void Update(Window window, Camera camera, Input input, float dt) { var substepCountChange = (int)MathF.Max(1f, Simulation.Solver.SubstepCount * 0.25f); @@ -114,27 +132,38 @@ public override void Update(Window window, Camera camera, Input input, float dt) if (input.WasPushed(OpenTK.Input.Key.Z)) { Simulation.Solver.SubstepCount = Math.Max(1, Simulation.Solver.SubstepCount - substepCountChange); + AwakenAllBodies(); } if (input.WasPushed(OpenTK.Input.Key.X)) { Simulation.Solver.SubstepCount = Math.Min(8192, Simulation.Solver.SubstepCount + substepCountChange); + AwakenAllBodies(); } if (input.WasPushed(OpenTK.Input.Key.C)) { Simulation.Solver.VelocityIterationCount = Math.Max(1, Simulation.Solver.VelocityIterationCount - iterationCountChange); + AwakenAllBodies(); } if (input.WasPushed(OpenTK.Input.Key.V)) { Simulation.Solver.VelocityIterationCount = Math.Min(8192, Simulation.Solver.VelocityIterationCount + iterationCountChange); + AwakenAllBodies(); } base.Update(window, camera, input, dt); } public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) { - renderer.TextBatcher.Write(text.Clear().Append("Substep count: ").Append(Simulation.Solver.SubstepCount), new Vector2(16, renderer.Surface.Resolution.Y - 64), 16, new Vector3(1), font); - renderer.TextBatcher.Write(text.Clear().Append("Solver iteration count: ").Append(Simulation.Solver.VelocityIterationCount), new Vector2(16, renderer.Surface.Resolution.Y - 48), 16, new Vector3(1), font); - renderer.TextBatcher.Write(text.Clear().Append("Press Z/X to change substep count, C/V to change solver iteration count."), new Vector2(16, renderer.Surface.Resolution.Y - 32), 16, new Vector3(1), font); + var resolution = renderer.Surface.Resolution; + renderer.TextBatcher.Write(text.Clear().Append("Substepping makes the solver run multiple mini timesteps for each call to Simulation.Timestep."), new Vector2(16, resolution.Y - 160), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Substeps can make extreme mass ratios and difficult constraint articulations stable at low costs."), new Vector2(16, resolution.Y - 144), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Simulations with substepping can use fewer velocity iterations per substep while remaining stable."), new Vector2(16, resolution.Y - 128), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Some difficult simulations will be vastly cheaper when using substepping than just increasing velocity iterations."), new Vector2(16, resolution.Y - 112), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Try modifying the substep/iteration counts to observe the effect on simulation stability."), new Vector2(16, resolution.Y - 96), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Check the Substepping documentation for more information."), new Vector2(16, resolution.Y - 80), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Substep count: ").Append(Simulation.Solver.SubstepCount), new Vector2(16, renderer.Surface.Resolution.Y - 48), 16, new Vector3(1), font); + renderer.TextBatcher.Write(text.Clear().Append("Solver iteration count: ").Append(Simulation.Solver.VelocityIterationCount), new Vector2(16, renderer.Surface.Resolution.Y - 32), 16, new Vector3(1), font); + renderer.TextBatcher.Write(text.Clear().Append("Press Z/X to change substep count, C/V to change solver iteration count."), new Vector2(16, renderer.Surface.Resolution.Y - 16), 16, new Vector3(1), font); rolloverInfo.Render(renderer, camera, input, text, font); base.Render(renderer, camera, input, text, font); } diff --git a/Demos/Demos/SweepDemo.cs b/Demos/Demos/SweepDemo.cs index 59bcf5f65..67eed696b 100644 --- a/Demos/Demos/SweepDemo.cs +++ b/Demos/Demos/SweepDemo.cs @@ -1,6 +1,7 @@ using BepuPhysics; using BepuPhysics.Collidables; using BepuPhysics.CollisionDetection; +using BepuPhysics.Constraints; using BepuUtilities; using BepuUtilities.Collections; using DemoContentLoader; @@ -31,7 +32,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(0, 10, 40); camera.Yaw = 0; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); var box = new Box(2f, 2f, 2f); var capsule = new Capsule(1f, 1f); diff --git a/Demos/Program.cs b/Demos/Program.cs index 2dddd9ea8..27877d24c 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -25,7 +25,7 @@ static void Main(string[] args) content = ContentArchive.Load(stream); } //DeterminismTest.Test(content, 2, 32768); - //HeadlessTest.Test(content, 16, 64, 512); + //HeadlessTest.Test(content, 4, 32, 512); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); diff --git a/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs b/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs index a9852f7ba..6cfc18bef 100644 --- a/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs +++ b/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs @@ -10,6 +10,7 @@ using BepuUtilities.Collections; using System.Runtime.CompilerServices; using DemoContentLoader; +using BepuPhysics.Constraints; namespace Demos.SpecializedTests { @@ -20,7 +21,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-20f, 13, -20f); camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = MathHelper.Pi * 0.1f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); var shape = new Sphere(0.5f); var sphereInertia = shape.ComputeInertia(1); diff --git a/Demos/SpecializedTests/CapsuleTestDemo.cs b/Demos/SpecializedTests/CapsuleTestDemo.cs index 2301467ed..21f37544b 100644 --- a/Demos/SpecializedTests/CapsuleTestDemo.cs +++ b/Demos/SpecializedTests/CapsuleTestDemo.cs @@ -9,6 +9,7 @@ using BepuUtilities.Memory; using BepuUtilities.Collections; using DemoContentLoader; +using BepuPhysics.Constraints; namespace Demos.SpecializedTests { @@ -20,7 +21,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //camera.Yaw = MathHelper.Pi ; camera.Yaw = MathHelper.Pi * 3f / 4; //camera.Pitch = MathHelper.Pi * 0.1f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); var shape = new Capsule(.5f, .5f); var localInertia = shape.ComputeInertia(1); diff --git a/Demos/SpecializedTests/ClothLatticeDemo.cs b/Demos/SpecializedTests/ClothLatticeDemo.cs index e80965099..5da1033c7 100644 --- a/Demos/SpecializedTests/ClothLatticeDemo.cs +++ b/Demos/SpecializedTests/ClothLatticeDemo.cs @@ -20,7 +20,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-120, 30, -120); camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = 0.1f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); //Build a grid of shapes to be connected. var clothNodeShape = new Sphere(0.5f); diff --git a/Demos/SpecializedTests/CompoundBoundTests.cs b/Demos/SpecializedTests/CompoundBoundTests.cs index cbfb6381d..573c78ce8 100644 --- a/Demos/SpecializedTests/CompoundBoundTests.cs +++ b/Demos/SpecializedTests/CompoundBoundTests.cs @@ -9,6 +9,7 @@ using DemoContentLoader; using DemoRenderer.UI; using DemoRenderer.Constraints; +using BepuPhysics.Constraints; namespace Demos.Demos { @@ -18,7 +19,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(-10, 0, -10); camera.Yaw = MathHelper.Pi * 3f / 4; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); } diff --git a/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs b/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs index 7c151a5bc..0cbadf9c5 100644 --- a/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs +++ b/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs @@ -12,14 +12,11 @@ namespace Demos.SpecializedTests { public class ConstrainedKinematicIntegrationTest : Demo { - public unsafe override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(25, 4, 40); camera.Yaw = 0; - Simulation = Simulation.Create(BufferPool, - new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(30, 1), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 2f }, - new DemoPoseIntegratorCallbacks(new Vector3(0, -0.1f, 0), 0, 0), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1), 2, 1), new DemoPoseIntegratorCallbacks(new Vector3(0, -0.1f, 0), 0, 0), new SolveDescription(1, 4)); var shapeA = new Box(.75f, 1, .5f); var shapeIndexA = Simulation.Shapes.Add(shapeA); diff --git a/Demos/SpecializedTests/ConstraintTestDemo.cs b/Demos/SpecializedTests/ConstraintTestDemo.cs index 5af760298..aa9b159c5 100644 --- a/Demos/SpecializedTests/ConstraintTestDemo.cs +++ b/Demos/SpecializedTests/ConstraintTestDemo.cs @@ -23,8 +23,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(25, 4, 40); camera.Yaw = 0; - Simulation = Simulation.Create(BufferPool, - new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(30, 1), FrictionCoefficient = 1f, MaximumRecoveryVelocity = 2f }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1), 2, 1), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); var shapeA = new Box(.75f, 1, .5f); var shapeIndexA = Simulation.Shapes.Add(shapeA); diff --git a/Demos/SpecializedTests/ConvexHullTestDemo.cs b/Demos/SpecializedTests/ConvexHullTestDemo.cs index d8bf4353b..c5083b05d 100644 --- a/Demos/SpecializedTests/ConvexHullTestDemo.cs +++ b/Demos/SpecializedTests/ConvexHullTestDemo.cs @@ -14,6 +14,7 @@ using System.Diagnostics; using BepuUtilities; using BepuPhysics.Constraints.Contact; +using BepuPhysics.Constraints; namespace Demos.SpecializedTests { @@ -27,7 +28,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Yaw = 0; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); //var meshContent = content.Load("Content\\newt.obj"); diff --git a/Demos/SpecializedTests/CylinderTestDemo.cs b/Demos/SpecializedTests/CylinderTestDemo.cs index a8c3b5c21..5c4518c11 100644 --- a/Demos/SpecializedTests/CylinderTestDemo.cs +++ b/Demos/SpecializedTests/CylinderTestDemo.cs @@ -7,6 +7,7 @@ using BepuPhysics.Collidables; using BepuPhysics.CollisionDetection.CollisionTasks; using System.Diagnostics; +using BepuPhysics.Constraints; namespace Demos.SpecializedTests { @@ -170,7 +171,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0; camera.Yaw = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, 0f, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, 0f, 0)), new SolveDescription(1, 4)); var cylinderShape = new Cylinder(1f, .2f); var cylinder = BodyDescription.CreateDynamic(new Vector3(10f, 3, 0), cylinderShape.ComputeInertia(1), new(Simulation.Shapes.Add(cylinderShape), ContinuousDetection.CreatePassive(1000f, 1000f)), 0.01f); diff --git a/Demos/SpecializedTests/FountainStressTestDemo.cs b/Demos/SpecializedTests/FountainStressTestDemo.cs index ae3d3aa64..523317b33 100644 --- a/Demos/SpecializedTests/FountainStressTestDemo.cs +++ b/Demos/SpecializedTests/FountainStressTestDemo.cs @@ -9,6 +9,7 @@ using BepuUtilities.Collections; using System.Diagnostics; using DemoContentLoader; +using BepuPhysics.Constraints; namespace Demos.SpecializedTests { @@ -24,7 +25,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = MathHelper.Pi * 0.1f; //Using minimum sized allocations forces as many resizes as possible. //Note the low solverFallbackBatchThreshold- we want the fallback batches to get tested thoroughly. - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(new[] { 2, 1, 1 }, fallbackBatchThreshold: 2), initialAllocationSizes: + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(new[] { 2, 1, 1 }, fallbackBatchThreshold: 2), initialAllocationSizes: new SimulationAllocationSizes { Bodies = 1, diff --git a/Demos/SpecializedTests/GyroscopeTestDemo.cs b/Demos/SpecializedTests/GyroscopeTestDemo.cs index a68400010..1116fb53d 100644 --- a/Demos/SpecializedTests/GyroscopeTestDemo.cs +++ b/Demos/SpecializedTests/GyroscopeTestDemo.cs @@ -55,7 +55,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0; //Note the lack of damping- we want the gyroscope to keep spinning. - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new GyroscopicIntegratorCallbacks(new Vector3(0, -10, 0), 0f, 0f), new SolveDescription(1, 8)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new GyroscopicIntegratorCallbacks(new Vector3(0, -10, 0), 0f, 0f), new SolveDescription(1, 8)); Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(new Box(100, 1, 100)))); diff --git a/Demos/SpecializedTests/Media/NewtDemandingSacrificeVideoDemo.cs b/Demos/SpecializedTests/Media/NewtDemandingSacrificeVideoDemo.cs index a6507ec9d..1ea39cfb6 100644 --- a/Demos/SpecializedTests/Media/NewtDemandingSacrificeVideoDemo.cs +++ b/Demos/SpecializedTests/Media/NewtDemandingSacrificeVideoDemo.cs @@ -20,7 +20,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = MathHelper.Pi * -0.05f; filters = new CollidableProperty(BufferPool); - Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks() { CollisionFilters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks(filters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), Simulation.Shapes.Add(new Box(1500, 1, 1500)))); Simulation.Statics.Add(new StaticDescription(new Vector3(0, 10, 0), Simulation.Shapes.Add(new Box(70, 20, 80)))); diff --git a/Demos/SpecializedTests/Media/PyramidVideoDemo.cs b/Demos/SpecializedTests/Media/PyramidVideoDemo.cs index 50bebe119..0ce6c76f6 100644 --- a/Demos/SpecializedTests/Media/PyramidVideoDemo.cs +++ b/Demos/SpecializedTests/Media/PyramidVideoDemo.cs @@ -1,5 +1,6 @@ using BepuPhysics; using BepuPhysics.Collidables; +using BepuPhysics.Constraints; using BepuUtilities; using DemoContentLoader; using DemoRenderer; @@ -22,7 +23,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-70, 8, 318); camera.Yaw = MathHelper.Pi * 1f / 4; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); var boxShape = new Box(1, 1, 1); var boxInertia = boxShape.ComputeInertia(1); diff --git a/Demos/SpecializedTests/Media/ShrinkwrappedNewtsVideoDemo.cs b/Demos/SpecializedTests/Media/ShrinkwrappedNewtsVideoDemo.cs index aedc9d484..aa3ffdc9f 100644 --- a/Demos/SpecializedTests/Media/ShrinkwrappedNewtsVideoDemo.cs +++ b/Demos/SpecializedTests/Media/ShrinkwrappedNewtsVideoDemo.cs @@ -1,5 +1,6 @@ using BepuPhysics; using BepuPhysics.Collidables; +using BepuPhysics.Constraints; using BepuUtilities; using BepuUtilities.Collections; using DemoContentLoader; @@ -18,7 +19,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = 3 * MathHelper.Pi / 4; camera.Pitch = 0;// MathHelper.Pi * 0.15f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); var meshContent = content.Load("Content\\newt.obj"); diff --git a/Demos/SpecializedTests/MeshMeshTestDemo.cs b/Demos/SpecializedTests/MeshMeshTestDemo.cs index 9197fc1dd..f7726f9ee 100644 --- a/Demos/SpecializedTests/MeshMeshTestDemo.cs +++ b/Demos/SpecializedTests/MeshMeshTestDemo.cs @@ -4,6 +4,7 @@ using System.Text; using BepuPhysics; using BepuPhysics.Collidables; +using BepuPhysics.Constraints; using BepuUtilities; using DemoContentLoader; using DemoRenderer; @@ -21,7 +22,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(0, 8, -10); camera.Yaw = MathHelper.Pi; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); DemoMeshHelper.LoadModel(content, BufferPool, @"Content\newt.obj", Vector3.One, out var mesh); var approximateInertia = new Box(2.5f, 1, 4).ComputeInertia(1); diff --git a/Demos/SpecializedTests/MeshReductionTestDemo.cs b/Demos/SpecializedTests/MeshReductionTestDemo.cs index 590156f6d..91a36425e 100644 --- a/Demos/SpecializedTests/MeshReductionTestDemo.cs +++ b/Demos/SpecializedTests/MeshReductionTestDemo.cs @@ -23,9 +23,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Yaw = 0; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, - new DemoNarrowPhaseCallbacks() { ContactSpringiness = new(30, 1), MaximumRecoveryVelocity = 2, FrictionCoefficient = 0 }, - new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1), 2, 0), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); var builder = new CompoundBuilder(BufferPool, Simulation.Shapes, 2); builder.Add(new Box(1.85f, 0.7f, 4.73f), RigidPose.Identity, 10); diff --git a/Demos/SpecializedTests/MeshSerializationTestDemo.cs b/Demos/SpecializedTests/MeshSerializationTestDemo.cs index ecb30bd9f..760879438 100644 --- a/Demos/SpecializedTests/MeshSerializationTestDemo.cs +++ b/Demos/SpecializedTests/MeshSerializationTestDemo.cs @@ -6,6 +6,7 @@ using BepuPhysics; using BepuPhysics.Collidables; using System.Diagnostics; +using BepuPhysics.Constraints; namespace Demos.SpecializedTests { @@ -17,7 +18,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); var startTime = Stopwatch.GetTimestamp(); DemoMeshHelper.CreateDeformedPlane(1025, 1025, (x, y) => new Vector3(x * 0.125f, MathF.Sin(x) + MathF.Sin(y), y * 0.125f), Vector3.One, BufferPool, out var originalMesh); diff --git a/Demos/SpecializedTests/MeshTestDemo.cs b/Demos/SpecializedTests/MeshTestDemo.cs index 25a70b375..470a275d2 100644 --- a/Demos/SpecializedTests/MeshTestDemo.cs +++ b/Demos/SpecializedTests/MeshTestDemo.cs @@ -8,6 +8,7 @@ using System.Diagnostics; using BepuUtilities.Collections; using DemoContentLoader; +using BepuPhysics.Constraints; namespace Demos.SpecializedTests { @@ -19,7 +20,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //camera.Yaw = MathHelper.Pi ; camera.Yaw = MathHelper.Pi * 3f / 4; //camera.Pitch = MathHelper.PiOver2 * 0.999f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); var box = new Box(1f, 3f, 2f); var capsule = new Capsule(1f, 1f); diff --git a/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs b/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs index e357b57b2..52e90228d 100644 --- a/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs +++ b/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs @@ -1,5 +1,6 @@ using BepuPhysics; using BepuPhysics.Collidables; +using BepuPhysics.Constraints; using BepuUtilities; using DemoContentLoader; using DemoRenderer; @@ -22,7 +23,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-30, 8, -110); camera.Yaw = MathHelper.Pi * 3f / 4; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); var boxShape = new Box(1, 1, 1); var boxInertia = boxShape.ComputeInertia(1); diff --git a/Demos/SpecializedTests/RagdollTubeDemo.cs b/Demos/SpecializedTests/RagdollTubeDemo.cs index 4aa8fc61b..6bf03f63f 100644 --- a/Demos/SpecializedTests/RagdollTubeDemo.cs +++ b/Demos/SpecializedTests/RagdollTubeDemo.cs @@ -7,6 +7,8 @@ using DemoContentLoader; using Demos.Demos; using DemoUtilities; +using BepuPhysics.CollisionDetection; +using BepuPhysics.Constraints; namespace Demos.SpecializedTests { @@ -21,7 +23,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi; camera.Pitch = 0; var filters = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks { CollisionFilters = filters }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks(filters, new PairMaterialProperties(2, 2, new SpringSettings(30, 1))), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(4, 1)); int ragdollIndex = 0; var spacing = new Vector3(1.7f, 1.8f, 0.5f); diff --git a/Demos/SpecializedTests/ShapePileTestDemo.cs b/Demos/SpecializedTests/ShapePileTestDemo.cs index 0881bfc34..6b410650f 100644 --- a/Demos/SpecializedTests/ShapePileTestDemo.cs +++ b/Demos/SpecializedTests/ShapePileTestDemo.cs @@ -9,6 +9,7 @@ using BepuUtilities.Memory; using BepuUtilities.Collections; using DemoContentLoader; +using BepuPhysics.Constraints; namespace Demos.SpecializedTests { @@ -20,7 +21,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //camera.Yaw = MathHelper.Pi ; camera.Yaw = MathHelper.Pi * 3f / 4; //camera.Pitch = MathHelper.PiOver2 * 0.999f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); Simulation.Deterministic = true; var sphere = new Sphere(1.5f); diff --git a/Demos/SpecializedTests/SolverBatchTestDemo.cs b/Demos/SpecializedTests/SolverBatchTestDemo.cs index eca172de3..16e366f63 100644 --- a/Demos/SpecializedTests/SolverBatchTestDemo.cs +++ b/Demos/SpecializedTests/SolverBatchTestDemo.cs @@ -20,7 +20,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-120, 30, -120); camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = 0.1f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); Simulation.Solver.VelocityIterationCount = 8; //Build a grid of shapes to be connected. diff --git a/Demos/SpecializedTests/TriangleRayTestDemo.cs b/Demos/SpecializedTests/TriangleRayTestDemo.cs index 2f06019be..5b75dd6b9 100644 --- a/Demos/SpecializedTests/TriangleRayTestDemo.cs +++ b/Demos/SpecializedTests/TriangleRayTestDemo.cs @@ -94,7 +94,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-30, 8, -60); camera.Yaw = MathHelper.Pi * 3f / 4; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); Triangle triangle; triangle.A = new Vector3(0, 0, 0); diff --git a/Demos/SpecializedTests/TriangleTestDemo.cs b/Demos/SpecializedTests/TriangleTestDemo.cs index f07cb591e..c323d3a1a 100644 --- a/Demos/SpecializedTests/TriangleTestDemo.cs +++ b/Demos/SpecializedTests/TriangleTestDemo.cs @@ -10,6 +10,7 @@ using BepuUtilities.Collections; using BepuPhysics.CollisionDetection.CollisionTasks; using DemoContentLoader; +using BepuPhysics.Constraints; namespace Demos.SpecializedTests { @@ -90,9 +91,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathF.PI; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, - new DemoNarrowPhaseCallbacks() { ContactSpringiness = new BepuPhysics.Constraints.SpringSettings(30, 1), FrictionCoefficient = 1, MaximumRecoveryVelocity = 5 }, - new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1), 5, 1), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); //var triangleDescription = new StaticDescription //{ From b51cf1c11c1476402ef95a7c81f6b1afc99bd756 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 27 Jan 2022 20:02:11 -0600 Subject: [PATCH 397/947] RopeTwistDemo notes. --- Demos/Demos/RopeTwistDemo.cs | 20 +++++++++++++++++--- 1 file changed, 17 insertions(+), 3 deletions(-) diff --git a/Demos/Demos/RopeTwistDemo.cs b/Demos/Demos/RopeTwistDemo.cs index 395e928af..f9d338585 100644 --- a/Demos/Demos/RopeTwistDemo.cs +++ b/Demos/Demos/RopeTwistDemo.cs @@ -4,6 +4,8 @@ using BepuPhysics.Constraints; using DemoContentLoader; using DemoRenderer; +using DemoRenderer.UI; +using DemoUtilities; using System; using System.Numerics; using System.Runtime.CompilerServices; @@ -24,7 +26,7 @@ struct Filter unsafe struct RopeNarrowPhaseCallbacks : INarrowPhaseCallbacks { public CollidableProperty Filters; - PairMaterialProperties Material; + public PairMaterialProperties Material; public RopeNarrowPhaseCallbacks(CollidableProperty filters, PairMaterialProperties contactMaterial) { @@ -80,7 +82,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var filters = new CollidableProperty(); Simulation = Simulation.Create(BufferPool, - new RopeNarrowPhaseCallbacks(filters, new PairMaterialProperties(2, float.MaxValue, new SpringSettings(2000, 1))), + new RopeNarrowPhaseCallbacks(filters, new PairMaterialProperties(0.0f, float.MaxValue, new SpringSettings(1200, 1))), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 60)); for (int twistIndex = 0; twistIndex < 1; ++twistIndex) @@ -109,7 +111,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var horizontalOffset = ropeDistributionRadius * new Vector3(MathF.Sin(angle), 0, MathF.Cos(angle)); var ropeStartLocation = startLocation + horizontalOffset; - var springSettings = new SpringSettings(600, 1); + var springSettings = new SpringSettings(600, 100); var bodyHandles = RopeStabilityDemo.BuildRopeBodies(Simulation, ropeStartLocation, ropeBodyCount, ropeBodyRadius, ropeBodySpacing, 1f, 0); for (int i = 0; i < bodyHandles.Length; ++i) { @@ -156,7 +158,19 @@ bool TryCreateConstraint(int handleIndexA, int handleIndexB) Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), Simulation.Shapes.Add(new Box(200, 1, 200)))); + } + + public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + { + var resolution = renderer.Surface.Resolution; + renderer.TextBatcher.Write(text.Clear().Append("The ball is 10,000 times heavier than the rope bodies and the ropes use no skip connections."), new Vector2(16, resolution.Y - 112), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("This is intended as a worst case scenario simulation:"), new Vector2(16, resolution.Y - 96), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("extremely high mass ratios, extremely high stiffness, extremely difficult to parallelize, no cheats."), new Vector2(16, resolution.Y - 80), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("While this wouldn't be a very practical simulation for a game, it does work thanks to substepping!"), new Vector2(16, resolution.Y - 48), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("This demo uses 60 substeps with 1 iteration each."), new Vector2(16, resolution.Y - 32), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Check the SubsteppingDemo, RopeStabilityDemo, and Substepping documentation for more information."), new Vector2(16, resolution.Y - 16), 16, Vector3.One, font); + base.Render(renderer, camera, input, text, font); } } } From 8c35122d53e04e2e9da4d524d7549d3b0f5a26a1 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 28 Jan 2022 14:32:08 -0600 Subject: [PATCH 398/947] ContactEventsDemo notes. --- Demos/Demos/ContactEventsDemo.cs | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/Demos/Demos/ContactEventsDemo.cs b/Demos/Demos/ContactEventsDemo.cs index 1bb8f2d21..88cb01e92 100644 --- a/Demos/Demos/ContactEventsDemo.cs +++ b/Demos/Demos/ContactEventsDemo.cs @@ -702,7 +702,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi; events = new ContactEvents(ThreadDispatcher, BufferPool); - Simulation = Simulation.Create(BufferPool, new ContactEventCallbacks(events), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new ContactEventCallbacks(events), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); eventHandler = new EventHandler(Simulation, BufferPool); var listenedBody1 = Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 5, 0), 1, Simulation.Shapes, new Box(1, 2, 3))); @@ -755,6 +755,14 @@ public override void Render(Renderer renderer, Camera camera, Input input, TextB var pose = new RigidPose(particle.Position); renderer.Shapes.AddShape(new Sphere(radius), Simulation.Shapes, pose, new Vector3(0, 1, 0)); } + + var resolution = renderer.Surface.Resolution; + renderer.TextBatcher.Write(text.Clear().Append("The library does not have a built-in concept of contact events like 'contact added' and 'contact removed'."), new Vector2(16, resolution.Y - 80), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("The INarrowPhaseCallbacks interface exposes callbacks that can be used to create such events, though."), new Vector2(16, resolution.Y - 64), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("This demo shows how such events could be implemented. Green particles are spawned on contact add."), new Vector2(16, resolution.Y - 48), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("The full list of events supported in the demo's source:"), new Vector2(16, resolution.Y - 32), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("OnContactAdded, OnContactRemoved, OnStartedTouching, OnTouching, OnStoppedTouching, OnPairCreated, OnPairUpdated, and OnPairEnded."), new Vector2(16, resolution.Y - 16), 16, Vector3.One, font); + base.Render(renderer, camera, input, text, font); } } From 47a8a63176b557849df7c1d2bd86cc41727429c0 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 28 Jan 2022 14:50:31 -0600 Subject: [PATCH 399/947] Bounciness notes. --- Demos/Demos/BouncinessDemo.cs | 29 +++++++++++++++++++++-------- 1 file changed, 21 insertions(+), 8 deletions(-) diff --git a/Demos/Demos/BouncinessDemo.cs b/Demos/Demos/BouncinessDemo.cs index 23a2ad111..4ec94b6c6 100644 --- a/Demos/Demos/BouncinessDemo.cs +++ b/Demos/Demos/BouncinessDemo.cs @@ -4,6 +4,8 @@ using BepuPhysics.Constraints; using DemoContentLoader; using DemoRenderer; +using DemoRenderer.UI; +using DemoUtilities; using System; using System.Numerics; using System.Runtime.CompilerServices; @@ -95,20 +97,31 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) { //We'll drop balls in a grid. From left to right, we increase stiffness, and from back to front (relative to the camera), we'll increase damping. //Note that higher frequency values tend to result in smaller bounces even at 0 damping. This is not physically realistic; it's a byproduct of the solver timestep being too long to properly handle extremely brief contacts. - //(Try increasing the substep count above to higher values and watch how the bounce gets closer and closer to equal height across frequency values.) - - //We choose a relatively high MaximumRecoveryVeloctity of 1000 so that the spring can actually push things back into the air, but be careful about assigning it to float.MaxValue. - //Substepping uses an approximate contact update rather than re-running the entirety of collision detection over and over. - //It can accumulate error in contact penetration depths. If using many substeps with high stiffness/low damping collisions, these small depth errors can result in very large velocities. - //If you need extremely high contact spring frequencies, you may need to run collision detection more often (that is, call Simulation.Timestep more often with fewer solver substeps to compensate), - //but limiting the recovery velocity will at least stop it from exploding. + //(Try increasing the substep count above to higher values and watch how the bounce gets closer and closer to equal height across frequency values. ballDescription.Pose.Position = new Vector3(i * 3 - 99f * 3f / 2f, 100, j * 3 - 230); - collidableMaterials.Allocate(Simulation.Bodies.Add(ballDescription)) = new SimpleMaterial { FrictionCoefficient = 1, MaximumRecoveryVelocity = 1000, SpringSettings = new SpringSettings(5 + 0.25f * i, j * j / 10000f) }; + collidableMaterials.Allocate(Simulation.Bodies.Add(ballDescription)) = new SimpleMaterial { FrictionCoefficient = 1, MaximumRecoveryVelocity = float.MaxValue, SpringSettings = new SpringSettings(5 + 0.25f * i, j * j / 10000f) }; } } collidableMaterials.Allocate(Simulation.Statics.Add(new StaticDescription(new Vector3(0, -15f, 0), Simulation.Shapes.Add(new Box(2500, 30, 2500))))) = new SimpleMaterial { FrictionCoefficient = 1, MaximumRecoveryVelocity = 2, SpringSettings = new SpringSettings(30, 1) }; } + + public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + { + var resolution = renderer.Surface.Resolution; + renderer.TextBatcher.Write(text.Clear().Append("The library does not use a coefficient of restitution."), new Vector2(16, resolution.Y - 192), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Traditional implementations of restitution don't work well with speculative contacts (which are used aggressively)."), new Vector2(16, resolution.Y - 176), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("All contact constraints, however, are springs."), new Vector2(16, resolution.Y - 160), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("By modifying contact material properties, bouncy behavior can be achieved."), new Vector2(16, resolution.Y - 144), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("From left to right, the spheres have increasing spring frequency. From far to near, they have increasing damping ratio."), new Vector2(16, resolution.Y - 128), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Bounciness is dominated by damping ratio; setting it to zero minimizes energy loss on impact."), new Vector2(16, resolution.Y - 112), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Counterintuitively, increasing spring frequency can make impacts less bouncy."), new Vector2(16, resolution.Y - 80), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("This happens because the integration rate becomes too slow to represent the motion and it gets damped away."), new Vector2(16, resolution.Y - 64), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Increasing the substepping rate or using more timesteps preserves bounciness with higher frequencies."), new Vector2(16, resolution.Y - 48), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Note that with infinite integration rate, increasing the spring frequency does not increase bounce magnitude."), new Vector2(16, resolution.Y - 32), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("High frequencies just make each bounce's contact duration shorter."), new Vector2(16, resolution.Y - 16), 16, Vector3.One, font); + base.Render(renderer, camera, input, text, font); + } } } From afeac58745ce1af4d3c8ab149503b2cc6cabb64e Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 28 Jan 2022 14:59:49 -0600 Subject: [PATCH 400/947] SolverContactEnumerationDemo notes. --- Demos/Demos/SolverContactEnumerationDemo.cs | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/Demos/Demos/SolverContactEnumerationDemo.cs b/Demos/Demos/SolverContactEnumerationDemo.cs index d17b50e00..e73f43c92 100644 --- a/Demos/Demos/SolverContactEnumerationDemo.cs +++ b/Demos/Demos/SolverContactEnumerationDemo.cs @@ -27,7 +27,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Yaw = 0; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); //Drop a pyramid on top of the sensor so there are more contacts to look at. var boxShape = new Box(1, 1, 1); @@ -254,9 +254,15 @@ public override void Render(Renderer renderer, Camera camera, Input input, TextB renderer.Shapes.AddShape(contactVisualShape, Simulation.Shapes, contactVisualPose, contact.Depth < 0 ? new Vector3(0, 0, 1) : new Vector3(0, 1, 0)); } } - renderer.TextBatcher.Write(text.Clear().Append("Sensor manifold constraint count: ").Append(extractor.ConstraintContacts.Count).Append(", contact count: ").Append(sensorContactCount), new Vector2(32, 32), 20, Vector3.One, font); extractor.Dispose(); + + var resolution = renderer.Surface.Resolution; + renderer.TextBatcher.Write(text.Clear().Append("The solver stores data in an optimized array-of-structures-of-arrays format that makes it difficult to directly read."), new Vector2(16, resolution.Y - 96), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("This demo implements an ISolverContactDataExtractor that pulls data out of the solver and puts it into a simpler format."), new Vector2(16, resolution.Y - 80), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Pulling data this way can sometimes be more convenient than tracking contacts from INarrowPhaseCallbacks."), new Vector2(16, resolution.Y - 64), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Contacts associated with the large box are visualized. The size of a contact corresponds to the contact's force."), new Vector2(16, resolution.Y - 48), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Sensor manifold constraint count: ").Append(extractor.ConstraintContacts.Count).Append(", contact count: ").Append(sensorContactCount), new Vector2(16, resolution.Y - 20), 20, Vector3.One, font); base.Render(renderer, camera, input, text, font); } } From aee53588f73caa4b92c0981ebdaea7468008518d Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 28 Jan 2022 16:19:04 -0600 Subject: [PATCH 401/947] RagdollTubeDemo notes. SweepDemo notes. DemoSet reorg. --- Demos/DemoSet.cs | 25 +++++++++--------- Demos/Demos/RagdollDemo.cs | 2 +- Demos/Demos/SweepDemo.cs | 8 +++++- Demos/SpecializedTests/RagdollTubeDemo.cs | 31 ++++++++++++++++++++++- 4 files changed, 50 insertions(+), 16 deletions(-) diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index 0cd046ce8..723b9f126 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -46,31 +46,30 @@ struct Option public DemoSet() { - AddOption(); - AddOption(); AddOption(); - AddOption(); - AddOption(); - AddOption(); AddOption(); - AddOption(); + AddOption(); AddOption(); - AddOption(); - AddOption(); + AddOption(); + AddOption(); + AddOption(); AddOption(); + AddOption(); + AddOption(); + AddOption(); AddOption(); AddOption(); - AddOption(); - AddOption(); - AddOption(); - AddOption(); AddOption(); AddOption(); + AddOption(); + AddOption(); + AddOption(); + AddOption(); AddOption(); AddOption(); AddOption(); AddOption(); - AddOption(); + AddOption(); AddOption(); } diff --git a/Demos/Demos/RagdollDemo.cs b/Demos/Demos/RagdollDemo.cs index ffcd0df2e..de119d877 100644 --- a/Demos/Demos/RagdollDemo.cs +++ b/Demos/Demos/RagdollDemo.cs @@ -547,7 +547,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = MathHelper.Pi * 0.05f; var collisionFilters = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks(collisionFilters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks(collisionFilters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); int ragdollIndex = 0; var spacing = new Vector3(2f, 3, 1); diff --git a/Demos/Demos/SweepDemo.cs b/Demos/Demos/SweepDemo.cs index 67eed696b..b8c377621 100644 --- a/Demos/Demos/SweepDemo.cs +++ b/Demos/Demos/SweepDemo.cs @@ -387,8 +387,14 @@ public override void Render(Renderer renderer, Camera camera, Input input, TextB DrawImpact(renderer, ref hitHandler.HitLocation, ref hitHandler.HitNormal); } } - } + var bottomY = renderer.Surface.Resolution.Y; + renderer.TextBatcher.Write(text.Clear().Append("The library supports sweeps that include both linear and angular motion."), new Vector2(16, bottomY - 48), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("In the foreground, sweeps are tested against the simulation."), new Vector2(16, bottomY - 32), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("In the background, sweeps with linear and angular components between every pair of shape types are visualized."), new Vector2(16, bottomY - 16), 16, Vector3.One, font); + + base.Render(renderer, camera, input, text, font); + } } diff --git a/Demos/SpecializedTests/RagdollTubeDemo.cs b/Demos/SpecializedTests/RagdollTubeDemo.cs index 6bf03f63f..409edbf61 100644 --- a/Demos/SpecializedTests/RagdollTubeDemo.cs +++ b/Demos/SpecializedTests/RagdollTubeDemo.cs @@ -9,6 +9,7 @@ using DemoUtilities; using BepuPhysics.CollisionDetection; using BepuPhysics.Constraints; +using DemoRenderer.UI; namespace Demos.SpecializedTests { @@ -23,7 +24,10 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi; camera.Pitch = 0; var filters = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks(filters, new PairMaterialProperties(2, 2, new SpringSettings(30, 1))), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(4, 1)); + //Note the lowered material stiffness compared to many of the other demos. Ragdolls aren't made of concrete. + //Increasing the maximum recovery velocity helps keep deeper contacts strong, stopping objects from interpenetrating. + //Higher friction helps the bodies clump and flop, rather than just sliding down the slope in the tube. + Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks(filters, new PairMaterialProperties(2, float.MaxValue, new SpringSettings(10, 1))), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(4, 1)); int ragdollIndex = 0; var spacing = new Vector3(1.7f, 1.8f, 0.5f); @@ -42,6 +46,10 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) } } + ragdollCount = ragdollIndex; + ragdollBodyCount = Simulation.Bodies.ActiveSet.Count; + ragdollConstraintCount = Simulation.Solver.CountConstraints(); + var tubeCenter = new Vector3(0, 8, 0); const int panelCount = 20; const float tubeRadius = 6; @@ -66,6 +74,27 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var staticShapeIndex = Simulation.Shapes.Add(staticShape); var staticDescription = new StaticDescription(new Vector3(0, -0.5f, 0), staticShapeIndex); Simulation.Statics.Add(staticDescription); + + + } + int ragdollBodyCount; + int ragdollConstraintCount; + int ragdollCount; + + public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + { + var resolution = renderer.Surface.Resolution; + renderer.TextBatcher.Write(text.Clear().Append("Ragdoll count:"), new Vector2(16, resolution.Y - 64), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Ragdoll body count:"), new Vector2(16, resolution.Y - 48), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Ragdoll constraint count:"), new Vector2(16, resolution.Y - 32), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Collision constraint count:"), new Vector2(16, resolution.Y - 16), 16, Vector3.One, font); + const float xOffset = 192; + renderer.TextBatcher.Write(text.Clear().Append(ragdollCount), new Vector2(xOffset, resolution.Y - 64), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append(ragdollBodyCount), new Vector2(xOffset, resolution.Y - 48), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append(ragdollConstraintCount), new Vector2(xOffset, resolution.Y - 32), 16, Vector3.One, font); + var collisionConstraintCount = Simulation.Solver.CountConstraints() - ragdollConstraintCount; + renderer.TextBatcher.Write(text.Clear().Append(collisionConstraintCount), new Vector2(xOffset, resolution.Y - 16), 16, Vector3.One, font); + base.Render(renderer, camera, input, text, font); } } } From 9d4319f3c4142d9d4e0bf9fad142fc26ec2b191d Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 28 Jan 2022 18:35:08 -0600 Subject: [PATCH 402/947] Cloth and planet demo notes. --- Demos/DemoSet.cs | 2 +- Demos/Demos/ClothDemo.cs | 3 +++ Demos/Demos/PlanetDemo.cs | 19 +++++++++++++------ 3 files changed, 17 insertions(+), 7 deletions(-) diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index 723b9f126..aca575a85 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -49,9 +49,9 @@ public DemoSet() AddOption(); AddOption(); AddOption(); + AddOption(); AddOption(); AddOption(); - AddOption(); AddOption(); AddOption(); AddOption(); diff --git a/Demos/Demos/ClothDemo.cs b/Demos/Demos/ClothDemo.cs index a679539c7..c3e21e4d7 100644 --- a/Demos/Demos/ClothDemo.cs +++ b/Demos/Demos/ClothDemo.cs @@ -272,6 +272,9 @@ bool FullyDynamic(int rowIndex, int columnIndex, int width, int height) public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) { + var resolution = renderer.Surface.Resolution; + renderer.TextBatcher.Write(text.Clear().Append("The library does not include any special cases for cloth simulation, but standard bodies and constraints work well."), new Vector2(16, resolution.Y - 32), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("This demo shows a few different configurations- different spring stiffnesses, and with/without area constraints."), new Vector2(16, resolution.Y - 16), 16, Vector3.One, font); rolloverInfo.Render(renderer, camera, input, text, font); base.Render(renderer, camera, input, text, font); } diff --git a/Demos/Demos/PlanetDemo.cs b/Demos/Demos/PlanetDemo.cs index a0a248fd6..6b75c561c 100644 --- a/Demos/Demos/PlanetDemo.cs +++ b/Demos/Demos/PlanetDemo.cs @@ -54,11 +54,11 @@ public void IntegrateVelocity(Vector bodyIndices, Vector3Wide position, Qua public unsafe override void Initialize(ContentArchive content, Camera camera) { - camera.Position = new Vector3(0, 0, -300); - camera.Yaw = MathHelper.Pi; - camera.Pitch = 0; + camera.Position = new Vector3(110, -80, 12); + camera.Yaw = 0; + camera.Pitch = MathF.PI * -0.5f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new PlanetaryGravityCallbacks() { PlanetCenter = new Vector3(), Gravity = 100000 }, new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new PlanetaryGravityCallbacks() { PlanetCenter = new Vector3(), Gravity = 100000 }, new SolveDescription(4, 1)); Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(new Sphere(50)))); @@ -66,7 +66,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var inertia = orbiter.ComputeInertia(1); var orbiterShapeIndex = Simulation.Shapes.Add(orbiter); var spacing = new Vector3(5); - const int length = 20; + const int length = 40; for (int i = 0; i < length; ++i) { for (int j = 0; j < 20; ++j) @@ -83,6 +83,13 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) } - + public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + { + var bottomY = renderer.Surface.Resolution.Y; + renderer.TextBatcher.Write(text.Clear().Append("The library does not prescribe any particular kind of gravity."), new Vector2(16, bottomY - 48), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("The IPoseIntegratorCallbacks provided to the simulation is responsible for telling the simulation how to integrate."), new Vector2(16, bottomY - 32), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("In this demo, all bodies are pulled towards the center of the planet."), new Vector2(16, bottomY - 16), 16, Vector3.One, font); + base.Render(renderer, camera, input, text, font); + } } } From cfbfe69f4673b517b631ceb8af85c93867a326f8 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 28 Jan 2022 18:41:06 -0600 Subject: [PATCH 403/947] CompoundDemo notes. --- Demos/DemoSet.cs | 2 +- .../Demos/{CompoundTestDemo.cs => CompoundDemo.cs} | 14 +++++++++++++- 2 files changed, 14 insertions(+), 2 deletions(-) rename Demos/Demos/{CompoundTestDemo.cs => CompoundDemo.cs} (92%) diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index aca575a85..df69783fc 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -58,7 +58,7 @@ public DemoSet() AddOption(); AddOption(); AddOption(); - AddOption(); + AddOption(); AddOption(); AddOption(); AddOption(); diff --git a/Demos/Demos/CompoundTestDemo.cs b/Demos/Demos/CompoundDemo.cs similarity index 92% rename from Demos/Demos/CompoundTestDemo.cs rename to Demos/Demos/CompoundDemo.cs index 5dc8c1e38..8ceef2e81 100644 --- a/Demos/Demos/CompoundTestDemo.cs +++ b/Demos/Demos/CompoundDemo.cs @@ -6,10 +6,12 @@ using System.Numerics; using DemoContentLoader; using BepuPhysics.Constraints; +using DemoRenderer.UI; +using DemoUtilities; namespace Demos.Demos { - public class CompoundTestDemo : Demo + public class CompoundDemo : Demo { public unsafe override void Initialize(ContentArchive content, Camera camera) { @@ -208,5 +210,15 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) }, new Vector3(2, 1, 2), BufferPool, out var planeMesh); Simulation.Statics.Add(new StaticDescription(new Vector3(64, 4, 32), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), Simulation.Shapes.Add(planeMesh))); } + + public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + { + var bottomY = renderer.Surface.Resolution.Y; + renderer.TextBatcher.Write(text.Clear().Append("There are two type of compounds built in: Compound and BigCompound."), new Vector2(16, bottomY - 80), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Compounds lack an acceleration structure, so they're good for low overhead compounds with few children."), new Vector2(16, bottomY - 64), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("BigCompounds contain an acceleration structure to keep more complex shapes fast."), new Vector2(16, bottomY - 48), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("This demo throws some different compounds together and stress tests their contact generation behavior."), new Vector2(16, bottomY - 16), 16, Vector3.One, font); + base.Render(renderer, camera, input, text, font); + } } } From d023472c7a0e54a7502bc18b8e6d2a1b078dc68e Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 28 Jan 2022 21:03:02 -0600 Subject: [PATCH 404/947] Continuous collision detection demo enhanced and notes added. --- Demos/Demos/ContactEventsDemo.cs | 4 +- .../Demos/ContinuousCollisionDetectionDemo.cs | 77 +++++++++++++++---- Demos/Demos/RopeTwistDemo.cs | 2 +- 3 files changed, 63 insertions(+), 20 deletions(-) diff --git a/Demos/Demos/ContactEventsDemo.cs b/Demos/Demos/ContactEventsDemo.cs index 88cb01e92..000e5ca64 100644 --- a/Demos/Demos/ContactEventsDemo.cs +++ b/Demos/Demos/ContactEventsDemo.cs @@ -758,8 +758,8 @@ public override void Render(Renderer renderer, Camera camera, Input input, TextB var resolution = renderer.Surface.Resolution; renderer.TextBatcher.Write(text.Clear().Append("The library does not have a built-in concept of contact events like 'contact added' and 'contact removed'."), new Vector2(16, resolution.Y - 80), 16, Vector3.One, font); - renderer.TextBatcher.Write(text.Clear().Append("The INarrowPhaseCallbacks interface exposes callbacks that can be used to create such events, though."), new Vector2(16, resolution.Y - 64), 16, Vector3.One, font); - renderer.TextBatcher.Write(text.Clear().Append("This demo shows how such events could be implemented. Green particles are spawned on contact add."), new Vector2(16, resolution.Y - 48), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("The INarrowPhaseCallbacks interface exposes callbacks that can be used to create such events."), new Vector2(16, resolution.Y - 64), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("This demo shows how events could be implemented. Green particles are spawned on contact add."), new Vector2(16, resolution.Y - 48), 16, Vector3.One, font); renderer.TextBatcher.Write(text.Clear().Append("The full list of events supported in the demo's source:"), new Vector2(16, resolution.Y - 32), 16, Vector3.One, font); renderer.TextBatcher.Write(text.Clear().Append("OnContactAdded, OnContactRemoved, OnStartedTouching, OnTouching, OnStoppedTouching, OnPairCreated, OnPairUpdated, and OnPairEnded."), new Vector2(16, resolution.Y - 16), 16, Vector3.One, font); diff --git a/Demos/Demos/ContinuousCollisionDetectionDemo.cs b/Demos/Demos/ContinuousCollisionDetectionDemo.cs index 509b76642..e38d8dcc7 100644 --- a/Demos/Demos/ContinuousCollisionDetectionDemo.cs +++ b/Demos/Demos/ContinuousCollisionDetectionDemo.cs @@ -12,11 +12,10 @@ namespace Demos.Demos { public class ContinuousCollisionDetectionDemo : Demo { - ConstraintHandle spinnerMotorA; - ConstraintHandle spinnerMotorB; + ConstraintHandle spinnerMotorDefaultA, spinnerMotorDefaultB, spinnerMotorSweepA, spinnerMotorSweepB; RolloverInfo rolloverInfo; - ConstraintHandle BuildSpinner(Vector3 initialPosition, float rotationSpeed) + ConstraintHandle BuildSpinner(Vector3 initialPosition, float rotationSpeed, ContinuousDetection continuousDetection) { var spinnerBase = Simulation.Bodies.Add(BodyDescription.CreateDynamic(initialPosition, new BodyInertia { InverseMass = 1e-2f }, Simulation.Shapes.Add(new Box(2, 2, 2)), 0.01f)); var bladeShape = new Box(5, 0.01f, 1); @@ -38,7 +37,7 @@ ConstraintHandle BuildSpinner(Vector3 initialPosition, float rotationSpeed) //Using a restricted speculative margin by setting the maximumSpeculativeMargin to 0.2 means that collision detection won't accept distant contacts. //This pretty much eliminates ghost collisions, while the continuous sweep helps avoid missed collisions. - var spinnerBlade = Simulation.Bodies.Add(BodyDescription.CreateDynamic(initialPosition, bladeInertia, new(shapeIndex, ContinuousDetection.Continuous(1e-4f, 1e-4f, maximumSpeculativeMargin: 0.2f)), 0.01f)); + var spinnerBlade = Simulation.Bodies.Add(BodyDescription.CreateDynamic(initialPosition, bladeInertia, new(shapeIndex, continuousDetection), 0.01f)); Simulation.Solver.Add(spinnerBase, spinnerBlade, new Hinge { LocalHingeAxisA = new Vector3(0, 0, 1), LocalHingeAxisB = new Vector3(0, 0, 1), LocalOffsetB = new Vector3(0, 0, -3), SpringSettings = new SpringSettings(30, 1) }); Simulation.Solver.Add(spinnerBase, spinnerBlade, new AngularAxisMotor { LocalAxisA = new Vector3(0, 0, 1), Settings = new MotorSettings(10, 1e-4f), TargetVelocity = rotationSpeed }); return Simulation.Solver.Add(spinnerBase, new OneBodyLinearServo { ServoSettings = ServoSettings.Default, SpringSettings = new SpringSettings(30, 1) }); @@ -46,7 +45,7 @@ ConstraintHandle BuildSpinner(Vector3 initialPosition, float rotationSpeed) public override void Initialize(ContentArchive content, Camera camera) { - camera.Position = new Vector3(0, 10, 40); + camera.Position = new Vector3(0, 10, 30); camera.Yaw = 0; camera.Pitch = 0; //Note the higher stiffness on contacts for this demo. That's not ideal for general stability at the demo timestep duration default of 60hz, but @@ -65,28 +64,50 @@ public override void Initialize(ContentArchive content, Camera camera) { for (int j = 0; j < 10; ++j) { - //These two falling dynamics have pretty small speculative margins. The second one uses continuous collision detection sweeps to generate speculative contacts. - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(-4 - 2 * j, 100 + (i + j) * 2, i * 2), new Vector3(0, -150, 0), inertia, + //The first set of boxes are forced to use very small speculative margins. They're going to tunnel into the ground, since no contacts will be created to stop it. + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(-37 + 2 * j, 100 + (i + j) * 2, -30 + i * 2), new Vector3(0, -150, 0), inertia, new(shapeIndex, ContinuousDetection.Discrete(maximumSpeculativeMargin: 0.01f)), 0.01f)); + + //The second set of boxes are not using explicit continuous collision sweeps, but have unlimited speculative margins. + //This configuration is the most common one you're likely to see in the demos (and use in your own applications). + //"ContinuousDetection.Passive" here just sets the maximum speculative margin to float.MaxValue in passive mode. + //Discrete vs Passive mode just controls how the bounding box is expanded: + //in Discrete mode, the bounding box can only be expanded by velocity up to the speculative margin. + //In Passive mode, the bounding box will expand to encompass the whole velocity. + //If Discrete is given a maximum speculative margin of float.MaxValue, they're functionally equivalent. + //(The difference exists because bounding box expansion is sometimes required to catch collisions from *other* continuous bodies. + //Without expanding the bounding box on a discrete body with low margin, another continuous body might not know the unexpanded body even exists and fly right by. + //If you don't care about that corner case for a given body, then using discrete is fine.) + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(-9 + 2 * j, 100 + (i + j) * 2, -30 + i * 2), new Vector3(0, -150, 0), inertia, + new(shapeIndex, ContinuousDetection.Passive), 0.01f)); + + //The third set of boxes uses explicit continuous sweeps. Because of the sweeps, the speculative margin can be kept very small. //The minimum progression duration parameter at 1e-3 means the CCD sweep won't miss any collisions that last at least 1e-3 units of time- so, if time is measured in seconds, //then this will capture any collision that an update rate of 1000hz would. //Note also that the sweep convergence threshold is actually pretty loose at 100hz. Despite that, it can still lead to reasonably good speculative contacts with solid impact behavior. //That's because the sweep does not directly generate contacts- it generates a time of impact estimate, and then the discrete contact generation //runs to create the actual contact manifold. That provides high quality contact positions and speculative depths. //If the ground that these boxes were smashing into was something like a mesh- which is infinitely thin- you may want to increase the sweep accuracy. - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(4 + 2 * j, 100 + (i + j) * 2, i * 2), new Vector3(0, -150, 0), inertia, + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(17 + 2 * j, 100 + (i + j) * 2, -30 + i * 2), new Vector3(0, -150, 0), inertia, new(shapeIndex, ContinuousDetection.Continuous(1e-3f, 1e-2f, maximumSpeculativeMargin: 0.01f)), 0.01f)); } } rolloverInfo = new RolloverInfo(); - rolloverInfo.Add(new Vector3(-12, 2, 0), "Discrete"); - rolloverInfo.Add(new Vector3(12, 2, 0), "Continuous"); + rolloverInfo.Add(new Vector3(-25, 2, -30), "Small speculative margin"); + rolloverInfo.Add(new Vector3(0, 2, -30), "Unlimited speculative margin"); + rolloverInfo.Add(new Vector3(25, 2, -30), "Small margin, continuous sweep"); //Build a couple of spinners to ram into each other to showcase angular CCD. Note that the spin speeds are slightly different- that helps avoid //synchronization that makes the blades frequently miss each other, which sorta ruins a CCD demo. - spinnerMotorA = BuildSpinner(new Vector3(-5, 10, -5), 53); - spinnerMotorB = BuildSpinner(new Vector3(5, 10, -5), 59); - rolloverInfo.Add(new Vector3(0, 12, -5), "High angular velocity continuous detection"); + var onlySpeculativeMargin = ContinuousDetection.Passive; + spinnerMotorDefaultA = BuildSpinner(new Vector3(-20, 14, 0), 53, onlySpeculativeMargin); + spinnerMotorDefaultB = BuildSpinner(new Vector3(-10, 14, 0), 59, onlySpeculativeMargin); + rolloverInfo.Add(new Vector3(-15, 14, -5), "Unlimited speculative margin"); + + var continuous = ContinuousDetection.Continuous(1e-4f, 1e-4f, maximumSpeculativeMargin: 0.2f); + spinnerMotorSweepA = BuildSpinner(new Vector3(10, 14, 0), 53, continuous); + spinnerMotorSweepB = BuildSpinner(new Vector3(20, 14, 0), 59, continuous); + rolloverInfo.Add(new Vector3(15, 14, -5), "Small margin, continuous sweep"); Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5f, 0), Simulation.Shapes.Add(new Box(300, 10, 300)))); } @@ -96,10 +117,16 @@ public override void Update(Window window, Camera camera, Input input, float dt) { //Scoot the spinners around. var servo = new OneBodyLinearServo { ServoSettings = ServoSettings.Default, SpringSettings = new SpringSettings(30, 1) }; - servo.Target = new Vector3(-5 - 3.5f * (float)Math.Sin(time), 10, -5); - Simulation.Solver.ApplyDescription(spinnerMotorA, servo); - servo.Target = new Vector3(5 + 3.5f * (float)Math.Sin(time), 10, -5); - Simulation.Solver.ApplyDescription(spinnerMotorB, servo); + var leftServoTarget = new Vector3(-3.5f * (float)Math.Sin(time), 10, -5); + var rightServoTarget = new Vector3(3.5f * (float)Math.Sin(time), 10, -5); + servo.Target = new Vector3(-20, 0, 0) + leftServoTarget; + Simulation.Solver.ApplyDescription(spinnerMotorDefaultA, servo); + servo.Target = new Vector3(-10, 0, 0) + rightServoTarget; + Simulation.Solver.ApplyDescription(spinnerMotorDefaultB, servo); + servo.Target = new Vector3(10, 0, 0) + leftServoTarget; + Simulation.Solver.ApplyDescription(spinnerMotorSweepA, servo); + servo.Target = new Vector3(20, 0, 0) + rightServoTarget; + Simulation.Solver.ApplyDescription(spinnerMotorSweepB, servo); time += dt; base.Update(window, camera, input, dt); } @@ -107,6 +134,22 @@ public override void Update(Window window, Camera camera, Input input, float dt) public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) { rolloverInfo.Render(renderer, camera, input, text, font); + var bottomY = renderer.Surface.Resolution.Y; + renderer.TextBatcher.Write(text.Clear().Append("The library uses speculative contacts for collision detection. That means their penetration depth can be negative."), new Vector2(16, bottomY - 240), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Continuous collision detection can be handled through solving these negative depth constraints in a nice unified way."), new Vector2(16, bottomY - 224), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("You can observe default speculative margin behavior in the middle set of boxes that fell in the background, or the left spinners."), new Vector2(16, bottomY - 208), 16, Vector3.One, font); + + renderer.TextBatcher.Write(text.Clear().Append("You can limit speculative margins so contacts won't be created for more distant pairs."), new Vector2(16, bottomY - 176), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("The left set of boxes shows this. When moving quickly, low-margin bodies will tend to tunnel into other bodies."), new Vector2(16, bottomY - 160), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("However, unlimited speculative contacts can produce 'ghost collisions'- contacts generated and solved for collisions that wouldn't have actually occurred."), new Vector2(16, bottomY - 144), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("This is often a subtle effect. Try turning on contact visualization (K) and slowing down timestepping (middle mouse) and watch the left spinners."), new Vector2(16, bottomY - 128), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Sometimes, the spinners will seem to interact and slow down even when they wouldn't have touched. That's a ghost collision."), new Vector2(16, bottomY - 112), 16, Vector3.One, font); + + renderer.TextBatcher.Write(text.Clear().Append("If you need to avoid this for a particular body, you can enable swept continuous collision detection."), new Vector2(16, bottomY - 80), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Instead of using the current body poses, a sweep test identifies a time of future impact and creates contacts at that point."), new Vector2(16, bottomY - 64), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Speculative contacts generated in this way are far less likely to cause ghost collisions."), new Vector2(16, bottomY - 48), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Sweeps do cost a bit more, but they are only used when necessary."), new Vector2(16, bottomY - 32), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("The right spinners and the rightmost set of boxes use swept collision detection."), new Vector2(16, bottomY - 16), 16, Vector3.One, font); base.Render(renderer, camera, input, text, font); } } diff --git a/Demos/Demos/RopeTwistDemo.cs b/Demos/Demos/RopeTwistDemo.cs index f9d338585..d661a8ea0 100644 --- a/Demos/Demos/RopeTwistDemo.cs +++ b/Demos/Demos/RopeTwistDemo.cs @@ -164,7 +164,7 @@ bool TryCreateConstraint(int handleIndexA, int handleIndexB) public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) { var resolution = renderer.Surface.Resolution; - renderer.TextBatcher.Write(text.Clear().Append("The ball is 10,000 times heavier than the rope bodies and the ropes use no skip connections."), new Vector2(16, resolution.Y - 112), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("The ball is 10,000 times heavier than the rope bodies, and the ropes use no skip connections."), new Vector2(16, resolution.Y - 112), 16, Vector3.One, font); renderer.TextBatcher.Write(text.Clear().Append("This is intended as a worst case scenario simulation:"), new Vector2(16, resolution.Y - 96), 16, Vector3.One, font); renderer.TextBatcher.Write(text.Clear().Append("extremely high mass ratios, extremely high stiffness, extremely difficult to parallelize, no cheats."), new Vector2(16, resolution.Y - 80), 16, Vector3.One, font); renderer.TextBatcher.Write(text.Clear().Append("While this wouldn't be a very practical simulation for a game, it does work thanks to substepping!"), new Vector2(16, resolution.Y - 48), 16, Vector3.One, font); From 3e753f1c76823e1060fe59b3222a3477cd095080 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 28 Jan 2022 21:39:51 -0600 Subject: [PATCH 405/947] Collision query demo notes. --- Demos/Demos/CollisionQueryDemo.cs | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/Demos/Demos/CollisionQueryDemo.cs b/Demos/Demos/CollisionQueryDemo.cs index 40f4f8d41..43e79f4d7 100644 --- a/Demos/Demos/CollisionQueryDemo.cs +++ b/Demos/Demos/CollisionQueryDemo.cs @@ -256,6 +256,13 @@ public unsafe override void Render(Renderer renderer, Camera camera, Input input BufferPool.Return(ref queryWasTouched); queries.Dispose(BufferPool); + + var bottomY = renderer.Surface.Resolution.Y; + renderer.TextBatcher.Write(text.Clear().Append("The broad phase exposes queries to collect bodies within bounding volumes, and the CollisionBatcher can be used to perform contact generation."), new Vector2(16, bottomY - 80), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("The boxes in the floating grid represent shape queries against the simulation. Broad phase collected candidates are handed to a CollisionBatcher for testing."), new Vector2(16, bottomY - 64), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("If positive depth contacts are detected, the box turns green."), new Vector2(16, bottomY - 48), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("(Note that triangle backfaces do not generate contacts, so queries intersecting just the mesh from below do not turn green.)"), new Vector2(16, bottomY - 16), 16, Vector3.One, font); + base.Render(renderer, camera, input, text, font); } } } From d3672a338d2c799887db9d001d940b2d0c530c81 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 29 Jan 2022 12:57:16 -0600 Subject: [PATCH 406/947] Voxel notes. --- Demos/Demos/CustomVoxelCollidableDemo.cs | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/Demos/Demos/CustomVoxelCollidableDemo.cs b/Demos/Demos/CustomVoxelCollidableDemo.cs index 8315ab13c..dd95cfec8 100644 --- a/Demos/Demos/CustomVoxelCollidableDemo.cs +++ b/Demos/Demos/CustomVoxelCollidableDemo.cs @@ -375,7 +375,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = MathHelper.Pi * 0.05f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); //The narrow phase must be notified about the existence of the new collidable type. For every pair type we want to support, a collision task must be registered. //All of the default engine types are registered upon simulation creation by a call to DefaultTypes.CreateDefaultCollisionTaskRegistry. @@ -454,6 +454,12 @@ public override unsafe void Render(Renderer renderer, Camera camera, Input input childPose.Position += voxelsPose.Position; renderer.Shapes.AddShape(shapeDataPointer, Box.Id, Simulation.Shapes, childPose, new Vector3(0.8f, 0.2f, 0.2f)); } + + var bottomY = renderer.Surface.Resolution.Y; + renderer.TextBatcher.Write(text.Clear().Append("Custom collidable types can be created and registered with the library."), new Vector2(16, bottomY - 80), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("This demo implements a simple voxel grid collidable."), new Vector2(16, bottomY - 64), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("For a real game with larger scale voxel worlds, you'd want to improve the voxel representation,"), new Vector2(16, bottomY - 32), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("but the demo shows what functions need to be implemented and how to connect them to the library."), new Vector2(16, bottomY - 16), 16, Vector3.One, font); base.Render(renderer, camera, input, text, font); } } From b76a964113374f8d9ffc98a139bc06b24ffcff2d Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 29 Jan 2022 13:12:01 -0600 Subject: [PATCH 407/947] Defaulted to nonsubstepping solver for demos where it doesn't matter for extra simplicity. Added notes to newtdemo. --- DemoTests/ConstraintDescriptionMappingTests.cs | 2 +- Demos/Demos/Characters/CharacterDemo.cs | 2 +- Demos/Demos/ClothDemo.cs | 2 +- Demos/Demos/CollisionQueryDemo.cs | 2 +- Demos/Demos/NewtDemo.cs | 12 +++++++++++- Demos/Demos/PyramidDemo.cs | 2 +- Demos/Demos/RayCastingDemo.cs | 2 +- Demos/Demos/SimpleSelfContainedDemo.cs | 4 ++-- Demos/Demos/Sponsors/SponsorDemo.cs | 2 +- Demos/Demos/SweepDemo.cs | 2 +- Demos/SpecializedTests/BroadPhaseStressTestDemo.cs | 2 +- Demos/SpecializedTests/CapsuleTestDemo.cs | 2 +- Demos/SpecializedTests/CharacterTestDemo.cs | 2 +- Demos/SpecializedTests/ClothLatticeDemo.cs | 2 +- Demos/SpecializedTests/CompoundBoundTests.cs | 2 +- .../SpecializedTests/CompoundCollisionIndicesTest.cs | 2 +- .../ConstrainedKinematicIntegrationTest.cs | 2 +- Demos/SpecializedTests/ConstraintTestDemo.cs | 2 +- Demos/SpecializedTests/ConvexHullTestDemo.cs | 2 +- Demos/SpecializedTests/CylinderTestDemo.cs | 2 +- Demos/SpecializedTests/Media/BedsheetDemo.cs | 2 +- Demos/SpecializedTests/Media/ColosseumVideoDemo.cs | 2 +- .../Media/NewtDemandingSacrificeVideoDemo.cs | 2 +- Demos/SpecializedTests/Media/NewtVideoDemo.cs | 2 +- Demos/SpecializedTests/Media/PyramidVideoDemo.cs | 2 +- .../Media/ShrinkwrappedNewtsVideoDemo.cs | 2 +- Demos/SpecializedTests/MeshMeshTestDemo.cs | 2 +- Demos/SpecializedTests/MeshReductionTestDemo.cs | 2 +- Demos/SpecializedTests/MeshSerializationTestDemo.cs | 2 +- Demos/SpecializedTests/MeshTestDemo.cs | 2 +- Demos/SpecializedTests/PyramidAwakenerTestDemo.cs | 2 +- Demos/SpecializedTests/ShapePileTestDemo.cs | 2 +- Demos/SpecializedTests/SolverBatchTestDemo.cs | 2 +- Demos/SpecializedTests/TriangleRayTestDemo.cs | 2 +- Demos/SpecializedTests/TriangleTestDemo.cs | 2 +- Demos/SpecializedTests/VolumeQueryTests.cs | 2 +- 36 files changed, 47 insertions(+), 37 deletions(-) diff --git a/DemoTests/ConstraintDescriptionMappingTests.cs b/DemoTests/ConstraintDescriptionMappingTests.cs index 2250fcb32..9a29241ef 100644 --- a/DemoTests/ConstraintDescriptionMappingTests.cs +++ b/DemoTests/ConstraintDescriptionMappingTests.cs @@ -25,7 +25,7 @@ static void FillWithRandomBytes(ref T item, Random random) where T : struct } static void Test(BufferPool pool, Random random, int constraintTypeBodyCount) where T : unmanaged, IConstraintDescription { - var simulation = Simulation.Create(pool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(), new SolveDescription(1, 4)); + var simulation = Simulation.Create(pool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(), new SolveDescription(8, 1)); const int bodyCount = 2048; diff --git a/Demos/Demos/Characters/CharacterDemo.cs b/Demos/Demos/Characters/CharacterDemo.cs index 407319690..631bbcb08 100644 --- a/Demos/Demos/Characters/CharacterDemo.cs +++ b/Demos/Demos/Characters/CharacterDemo.cs @@ -25,7 +25,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0; characters = new CharacterControllers(BufferPool); - Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); CreateCharacter(new Vector3(0, 2, -4)); diff --git a/Demos/Demos/ClothDemo.cs b/Demos/Demos/ClothDemo.cs index c3e21e4d7..c8b5528a0 100644 --- a/Demos/Demos/ClothDemo.cs +++ b/Demos/Demos/ClothDemo.cs @@ -209,7 +209,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0; var filters = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new ClothCallbacks(filters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new ClothCallbacks(filters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); rolloverInfo = new RolloverInfo(); bool KinematicTopCorners(int rowIndex, int columnIndex, int width, int height) diff --git a/Demos/Demos/CollisionQueryDemo.cs b/Demos/Demos/CollisionQueryDemo.cs index 43e79f4d7..906de38dd 100644 --- a/Demos/Demos/CollisionQueryDemo.cs +++ b/Demos/Demos/CollisionQueryDemo.cs @@ -28,7 +28,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Yaw = 0; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(new Box(100, 1, 100)))); diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index 329c4d852..d4acc4880 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -8,6 +8,7 @@ using BepuUtilities.Memory; using DemoContentLoader; using DemoRenderer; +using DemoRenderer.UI; using DemoUtilities; using System; using System.Diagnostics; @@ -724,7 +725,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = MathHelper.Pi * 0.15f; var filters = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new DeformableCallbacks(filters, new PairMaterialProperties(1f, 2f, new SpringSettings(30, 1))), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0), 0, 0), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DeformableCallbacks(filters, new PairMaterialProperties(1f, 2f, new SpringSettings(30, 1))), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0), 0, 0), new SolveDescription(8, 1)); var meshContent = content.Load("Content\\newt.obj"); float cellSize = 0.1f; @@ -752,5 +753,14 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Simulation.Statics.Add(new StaticDescription(new Vector3(0, -1.5f, 0), Simulation.Shapes.Add(new Sphere(3)))); } + public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + { + var resolution = renderer.Surface.Resolution; + renderer.TextBatcher.Write(text.Clear().Append("The library does not include any special cases for deformable simulation, but standard bodies and springy constraints work well."), new Vector2(16, resolution.Y - 64), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Here, welds and volume constraints are used to make squishy newts. The PlumpDancerDemo is similar, but doesn't have volume constraints."), new Vector2(16, resolution.Y - 48), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("The difference is subtle- for example, volume constraints make the newt squish outward more when the ball falls on it."), new Vector2(16, resolution.Y - 32), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Note that bodies inside the newts have no collision shapes; they're unnecessary and avoiding them reduces cost."), new Vector2(16, resolution.Y - 16), 16, Vector3.One, font); + base.Render(renderer, camera, input, text, font); + } } } diff --git a/Demos/Demos/PyramidDemo.cs b/Demos/Demos/PyramidDemo.cs index bfdad9322..015563d1d 100644 --- a/Demos/Demos/PyramidDemo.cs +++ b/Demos/Demos/PyramidDemo.cs @@ -23,7 +23,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-30, 8, -110); camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); var boxShape = new Box(1, 1, 1); var boxInertia = boxShape.ComputeInertia(1); diff --git a/Demos/Demos/RayCastingDemo.cs b/Demos/Demos/RayCastingDemo.cs index c19a2bdd0..0e1cc893d 100644 --- a/Demos/Demos/RayCastingDemo.cs +++ b/Demos/Demos/RayCastingDemo.cs @@ -63,7 +63,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-20f, 13, -20f); camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = MathHelper.Pi * 0.1f; - Simulation = Simulation.Create(BufferPool, new NoCollisionCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new NoCollisionCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); diff --git a/Demos/Demos/SimpleSelfContainedDemo.cs b/Demos/Demos/SimpleSelfContainedDemo.cs index 1bc9df7ca..9c11c1b68 100644 --- a/Demos/Demos/SimpleSelfContainedDemo.cs +++ b/Demos/Demos/SimpleSelfContainedDemo.cs @@ -212,9 +212,9 @@ public static void Run() { //The buffer pool is a source of raw memory blobs for the engine to use. var bufferPool = new BufferPool(); - //The following sets up a simulation with the callbacks defined above, and tells it to use 4 solver substeps per frame with 1 velocity iteration per substep. + //The following sets up a simulation with the callbacks defined above, and tells it to use 8 velocity iterations per substep and only one substep per solve. //It uses the default SubsteppingTimestepper. You could use a custom ITimestepper implementation to customize when stages run relative to each other, or to insert more callbacks. - var simulation = Simulation.Create(bufferPool, new NarrowPhaseCallbacks(), new PoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + var simulation = Simulation.Create(bufferPool, new NarrowPhaseCallbacks(), new PoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); //Drop a ball on a big static box. var sphere = new Sphere(1); diff --git a/Demos/Demos/Sponsors/SponsorDemo.cs b/Demos/Demos/Sponsors/SponsorDemo.cs index ee7c877b6..4735b5d5d 100644 --- a/Demos/Demos/Sponsors/SponsorDemo.cs +++ b/Demos/Demos/Sponsors/SponsorDemo.cs @@ -109,7 +109,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0.4f; characterControllers = new CharacterControllers(BufferPool); - Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characterControllers), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characterControllers), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); DemoMeshHelper.LoadModel(content, BufferPool, @"Content\newt.obj", new Vector3(-10, 10, -10), out var newtMesh); var newtShape = Simulation.Shapes.Add(newtMesh); diff --git a/Demos/Demos/SweepDemo.cs b/Demos/Demos/SweepDemo.cs index b8c377621..666db15ef 100644 --- a/Demos/Demos/SweepDemo.cs +++ b/Demos/Demos/SweepDemo.cs @@ -32,7 +32,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(0, 10, 40); camera.Yaw = 0; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); var box = new Box(2f, 2f, 2f); var capsule = new Capsule(1f, 1f); diff --git a/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs b/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs index 6cfc18bef..f7871edbd 100644 --- a/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs +++ b/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs @@ -21,7 +21,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-20f, 13, -20f); camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = MathHelper.Pi * 0.1f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); var shape = new Sphere(0.5f); var sphereInertia = shape.ComputeInertia(1); diff --git a/Demos/SpecializedTests/CapsuleTestDemo.cs b/Demos/SpecializedTests/CapsuleTestDemo.cs index 21f37544b..a7fa43135 100644 --- a/Demos/SpecializedTests/CapsuleTestDemo.cs +++ b/Demos/SpecializedTests/CapsuleTestDemo.cs @@ -21,7 +21,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //camera.Yaw = MathHelper.Pi ; camera.Yaw = MathHelper.Pi * 3f / 4; //camera.Pitch = MathHelper.Pi * 0.1f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); var shape = new Capsule(.5f, .5f); var localInertia = shape.ComputeInertia(1); diff --git a/Demos/SpecializedTests/CharacterTestDemo.cs b/Demos/SpecializedTests/CharacterTestDemo.cs index ee3dd2925..c1dd6b7e0 100644 --- a/Demos/SpecializedTests/CharacterTestDemo.cs +++ b/Demos/SpecializedTests/CharacterTestDemo.cs @@ -21,7 +21,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = MathHelper.Pi * 0.05f; var masks = new CollidableProperty(); characters = new CharacterControllers(BufferPool); - Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); var random = new Random(5); for (int i = 0; i < 8192; ++i) diff --git a/Demos/SpecializedTests/ClothLatticeDemo.cs b/Demos/SpecializedTests/ClothLatticeDemo.cs index 5da1033c7..9427a0679 100644 --- a/Demos/SpecializedTests/ClothLatticeDemo.cs +++ b/Demos/SpecializedTests/ClothLatticeDemo.cs @@ -20,7 +20,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-120, 30, -120); camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = 0.1f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); //Build a grid of shapes to be connected. var clothNodeShape = new Sphere(0.5f); diff --git a/Demos/SpecializedTests/CompoundBoundTests.cs b/Demos/SpecializedTests/CompoundBoundTests.cs index 573c78ce8..0eb474d3e 100644 --- a/Demos/SpecializedTests/CompoundBoundTests.cs +++ b/Demos/SpecializedTests/CompoundBoundTests.cs @@ -19,7 +19,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(-10, 0, -10); camera.Yaw = MathHelper.Pi * 3f / 4; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); } diff --git a/Demos/SpecializedTests/CompoundCollisionIndicesTest.cs b/Demos/SpecializedTests/CompoundCollisionIndicesTest.cs index 128d41397..0dbb5bf96 100644 --- a/Demos/SpecializedTests/CompoundCollisionIndicesTest.cs +++ b/Demos/SpecializedTests/CompoundCollisionIndicesTest.cs @@ -72,7 +72,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(0, 4, -6); camera.Yaw = MathHelper.Pi; - Simulation = Simulation.Create(BufferPool, new IndexReportingNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, 0f, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new IndexReportingNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, 0f, 0)), new SolveDescription(8, 1)); var builder = new CompoundBuilder(BufferPool, Simulation.Shapes, 4); builder.Add(new Sphere(0.5f), new Vector3(-1, 0, 0), 1); diff --git a/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs b/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs index 0cbadf9c5..400b06d0f 100644 --- a/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs +++ b/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs @@ -16,7 +16,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(25, 4, 40); camera.Yaw = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1), 2, 1), new DemoPoseIntegratorCallbacks(new Vector3(0, -0.1f, 0), 0, 0), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1), 2, 1), new DemoPoseIntegratorCallbacks(new Vector3(0, -0.1f, 0), 0, 0), new SolveDescription(8, 1)); var shapeA = new Box(.75f, 1, .5f); var shapeIndexA = Simulation.Shapes.Add(shapeA); diff --git a/Demos/SpecializedTests/ConstraintTestDemo.cs b/Demos/SpecializedTests/ConstraintTestDemo.cs index aa9b159c5..d21571e49 100644 --- a/Demos/SpecializedTests/ConstraintTestDemo.cs +++ b/Demos/SpecializedTests/ConstraintTestDemo.cs @@ -23,7 +23,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(25, 4, 40); camera.Yaw = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1), 2, 1), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1), 2, 1), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); var shapeA = new Box(.75f, 1, .5f); var shapeIndexA = Simulation.Shapes.Add(shapeA); diff --git a/Demos/SpecializedTests/ConvexHullTestDemo.cs b/Demos/SpecializedTests/ConvexHullTestDemo.cs index c5083b05d..9ae72e124 100644 --- a/Demos/SpecializedTests/ConvexHullTestDemo.cs +++ b/Demos/SpecializedTests/ConvexHullTestDemo.cs @@ -28,7 +28,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Yaw = 0; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); //var meshContent = content.Load("Content\\newt.obj"); diff --git a/Demos/SpecializedTests/CylinderTestDemo.cs b/Demos/SpecializedTests/CylinderTestDemo.cs index 5c4518c11..def62009c 100644 --- a/Demos/SpecializedTests/CylinderTestDemo.cs +++ b/Demos/SpecializedTests/CylinderTestDemo.cs @@ -171,7 +171,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0; camera.Yaw = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, 0f, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, 0f, 0)), new SolveDescription(8, 1)); var cylinderShape = new Cylinder(1f, .2f); var cylinder = BodyDescription.CreateDynamic(new Vector3(10f, 3, 0), cylinderShape.ComputeInertia(1), new(Simulation.Shapes.Add(cylinderShape), ContinuousDetection.CreatePassive(1000f, 1000f)), 0.01f); diff --git a/Demos/SpecializedTests/Media/BedsheetDemo.cs b/Demos/SpecializedTests/Media/BedsheetDemo.cs index 101a2f3d5..8d271c51b 100644 --- a/Demos/SpecializedTests/Media/BedsheetDemo.cs +++ b/Demos/SpecializedTests/Media/BedsheetDemo.cs @@ -110,7 +110,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = MathF.PI * 0.1f; var filters = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new ClothCallbacks(filters), new DemoPoseIntegratorCallbacks(new Vector3(0, -50, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new ClothCallbacks(filters), new DemoPoseIntegratorCallbacks(new Vector3(0, -50, 0)), new SolveDescription(8, 1)); rolloverInfo = new RolloverInfo(); bool FullyDynamic(int rowIndex, int columnIndex, int width, int height) diff --git a/Demos/SpecializedTests/Media/ColosseumVideoDemo.cs b/Demos/SpecializedTests/Media/ColosseumVideoDemo.cs index 58b45a6cc..bab1a80b6 100644 --- a/Demos/SpecializedTests/Media/ColosseumVideoDemo.cs +++ b/Demos/SpecializedTests/Media/ColosseumVideoDemo.cs @@ -68,7 +68,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = MathHelper.Pi * 0.2f; characters = new CharacterControllers(BufferPool); - Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); var ringBoxShape = new Box(0.5f, 1.5f, 3); var boxDescription = BodyDescription.CreateDynamic(new Vector3(), ringBoxShape.ComputeInertia(1), Simulation.Shapes.Add(ringBoxShape), 0.01f); diff --git a/Demos/SpecializedTests/Media/NewtDemandingSacrificeVideoDemo.cs b/Demos/SpecializedTests/Media/NewtDemandingSacrificeVideoDemo.cs index 1ea39cfb6..87ea5cd76 100644 --- a/Demos/SpecializedTests/Media/NewtDemandingSacrificeVideoDemo.cs +++ b/Demos/SpecializedTests/Media/NewtDemandingSacrificeVideoDemo.cs @@ -20,7 +20,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = MathHelper.Pi * -0.05f; filters = new CollidableProperty(BufferPool); - Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks(filters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks(filters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(4, 1)); Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), Simulation.Shapes.Add(new Box(1500, 1, 1500)))); Simulation.Statics.Add(new StaticDescription(new Vector3(0, 10, 0), Simulation.Shapes.Add(new Box(70, 20, 80)))); diff --git a/Demos/SpecializedTests/Media/NewtVideoDemo.cs b/Demos/SpecializedTests/Media/NewtVideoDemo.cs index 7da477356..d2e794dce 100644 --- a/Demos/SpecializedTests/Media/NewtVideoDemo.cs +++ b/Demos/SpecializedTests/Media/NewtVideoDemo.cs @@ -21,7 +21,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = MathHelper.Pi * 0.15f; var filters = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new DeformableCallbacks(filters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DeformableCallbacks(filters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); var meshContent = content.Load("Content\\newt.obj"); float cellSize = 0.1f; diff --git a/Demos/SpecializedTests/Media/PyramidVideoDemo.cs b/Demos/SpecializedTests/Media/PyramidVideoDemo.cs index 0ce6c76f6..a35211992 100644 --- a/Demos/SpecializedTests/Media/PyramidVideoDemo.cs +++ b/Demos/SpecializedTests/Media/PyramidVideoDemo.cs @@ -23,7 +23,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-70, 8, 318); camera.Yaw = MathHelper.Pi * 1f / 4; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); var boxShape = new Box(1, 1, 1); var boxInertia = boxShape.ComputeInertia(1); diff --git a/Demos/SpecializedTests/Media/ShrinkwrappedNewtsVideoDemo.cs b/Demos/SpecializedTests/Media/ShrinkwrappedNewtsVideoDemo.cs index aa3ffdc9f..eb5e883e2 100644 --- a/Demos/SpecializedTests/Media/ShrinkwrappedNewtsVideoDemo.cs +++ b/Demos/SpecializedTests/Media/ShrinkwrappedNewtsVideoDemo.cs @@ -19,7 +19,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = 3 * MathHelper.Pi / 4; camera.Pitch = 0;// MathHelper.Pi * 0.15f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); var meshContent = content.Load("Content\\newt.obj"); diff --git a/Demos/SpecializedTests/MeshMeshTestDemo.cs b/Demos/SpecializedTests/MeshMeshTestDemo.cs index f7726f9ee..6c0c5992d 100644 --- a/Demos/SpecializedTests/MeshMeshTestDemo.cs +++ b/Demos/SpecializedTests/MeshMeshTestDemo.cs @@ -22,7 +22,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(0, 8, -10); camera.Yaw = MathHelper.Pi; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); DemoMeshHelper.LoadModel(content, BufferPool, @"Content\newt.obj", Vector3.One, out var mesh); var approximateInertia = new Box(2.5f, 1, 4).ComputeInertia(1); diff --git a/Demos/SpecializedTests/MeshReductionTestDemo.cs b/Demos/SpecializedTests/MeshReductionTestDemo.cs index 91a36425e..9a4649c9b 100644 --- a/Demos/SpecializedTests/MeshReductionTestDemo.cs +++ b/Demos/SpecializedTests/MeshReductionTestDemo.cs @@ -23,7 +23,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Yaw = 0; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1), 2, 0), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1), 2, 0), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); var builder = new CompoundBuilder(BufferPool, Simulation.Shapes, 2); builder.Add(new Box(1.85f, 0.7f, 4.73f), RigidPose.Identity, 10); diff --git a/Demos/SpecializedTests/MeshSerializationTestDemo.cs b/Demos/SpecializedTests/MeshSerializationTestDemo.cs index 760879438..d347f8570 100644 --- a/Demos/SpecializedTests/MeshSerializationTestDemo.cs +++ b/Demos/SpecializedTests/MeshSerializationTestDemo.cs @@ -18,7 +18,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); var startTime = Stopwatch.GetTimestamp(); DemoMeshHelper.CreateDeformedPlane(1025, 1025, (x, y) => new Vector3(x * 0.125f, MathF.Sin(x) + MathF.Sin(y), y * 0.125f), Vector3.One, BufferPool, out var originalMesh); diff --git a/Demos/SpecializedTests/MeshTestDemo.cs b/Demos/SpecializedTests/MeshTestDemo.cs index 470a275d2..1e6aa716a 100644 --- a/Demos/SpecializedTests/MeshTestDemo.cs +++ b/Demos/SpecializedTests/MeshTestDemo.cs @@ -20,7 +20,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //camera.Yaw = MathHelper.Pi ; camera.Yaw = MathHelper.Pi * 3f / 4; //camera.Pitch = MathHelper.PiOver2 * 0.999f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); var box = new Box(1f, 3f, 2f); var capsule = new Capsule(1f, 1f); diff --git a/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs b/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs index 52e90228d..349d779b3 100644 --- a/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs +++ b/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs @@ -23,7 +23,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-30, 8, -110); camera.Yaw = MathHelper.Pi * 3f / 4; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); var boxShape = new Box(1, 1, 1); var boxInertia = boxShape.ComputeInertia(1); diff --git a/Demos/SpecializedTests/ShapePileTestDemo.cs b/Demos/SpecializedTests/ShapePileTestDemo.cs index 6b410650f..6f6ef1793 100644 --- a/Demos/SpecializedTests/ShapePileTestDemo.cs +++ b/Demos/SpecializedTests/ShapePileTestDemo.cs @@ -21,7 +21,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //camera.Yaw = MathHelper.Pi ; camera.Yaw = MathHelper.Pi * 3f / 4; //camera.Pitch = MathHelper.PiOver2 * 0.999f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); Simulation.Deterministic = true; var sphere = new Sphere(1.5f); diff --git a/Demos/SpecializedTests/SolverBatchTestDemo.cs b/Demos/SpecializedTests/SolverBatchTestDemo.cs index 16e366f63..0c2537f99 100644 --- a/Demos/SpecializedTests/SolverBatchTestDemo.cs +++ b/Demos/SpecializedTests/SolverBatchTestDemo.cs @@ -20,7 +20,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-120, 30, -120); camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = 0.1f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); Simulation.Solver.VelocityIterationCount = 8; //Build a grid of shapes to be connected. diff --git a/Demos/SpecializedTests/TriangleRayTestDemo.cs b/Demos/SpecializedTests/TriangleRayTestDemo.cs index 5b75dd6b9..3b89c9629 100644 --- a/Demos/SpecializedTests/TriangleRayTestDemo.cs +++ b/Demos/SpecializedTests/TriangleRayTestDemo.cs @@ -94,7 +94,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-30, 8, -60); camera.Yaw = MathHelper.Pi * 3f / 4; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); Triangle triangle; triangle.A = new Vector3(0, 0, 0); diff --git a/Demos/SpecializedTests/TriangleTestDemo.cs b/Demos/SpecializedTests/TriangleTestDemo.cs index c323d3a1a..a1965b4ee 100644 --- a/Demos/SpecializedTests/TriangleTestDemo.cs +++ b/Demos/SpecializedTests/TriangleTestDemo.cs @@ -91,7 +91,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathF.PI; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1), 5, 1), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1), 5, 1), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); //var triangleDescription = new StaticDescription //{ diff --git a/Demos/SpecializedTests/VolumeQueryTests.cs b/Demos/SpecializedTests/VolumeQueryTests.cs index 188a24e47..4e9c65199 100644 --- a/Demos/SpecializedTests/VolumeQueryTests.cs +++ b/Demos/SpecializedTests/VolumeQueryTests.cs @@ -61,7 +61,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-20f, 13, -20f); camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = MathHelper.Pi * 0.1f; - Simulation = Simulation.Create(BufferPool, new NoCollisionCallbacks(), new DemoPoseIntegratorCallbacks(), new SolveDescription(1, 4)); + Simulation = Simulation.Create(BufferPool, new NoCollisionCallbacks(), new DemoPoseIntegratorCallbacks(), new SolveDescription(8, 1)); var sphere = new Sphere(0.5f); var shapeIndex = Simulation.Shapes.Add(sphere); From fd3ae1dc533322b7f84d577f07429d407567616e Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 29 Jan 2022 18:30:57 -0600 Subject: [PATCH 408/947] First pass chain fountain. Bit of a tangled mess. --- Demos/DemoHarness.cs | 2 +- Demos/DemoSet.cs | 1 + Demos/Demos/ChainFountainDemo.cs | 99 ++++++++++++++++++++++++++++ Demos/Demos/RopeTwistDemo.cs | 110 +++++++++++++++++-------------- 4 files changed, 160 insertions(+), 52 deletions(-) create mode 100644 Demos/Demos/ChainFountainDemo.cs diff --git a/Demos/DemoHarness.cs b/Demos/DemoHarness.cs index 5396c016a..1b2a88e97 100644 --- a/Demos/DemoHarness.cs +++ b/Demos/DemoHarness.cs @@ -303,7 +303,7 @@ public void Update(float dt) input.MouseLocked = false; } ++frameCount; - if (!controls.SlowTimesteps.IsDown(input) || frameCount % 120 == 0) + if (!controls.SlowTimesteps.IsDown(input) || frameCount % 60 == 0) { demo.Update(window, camera, input, dt); } diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index df69783fc..6f6889469 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -46,6 +46,7 @@ struct Option public DemoSet() { + AddOption(); AddOption(); AddOption(); AddOption(); diff --git a/Demos/Demos/ChainFountainDemo.cs b/Demos/Demos/ChainFountainDemo.cs new file mode 100644 index 000000000..f54fce3a8 --- /dev/null +++ b/Demos/Demos/ChainFountainDemo.cs @@ -0,0 +1,99 @@ +using BepuPhysics; +using BepuPhysics.Collidables; +using BepuPhysics.CollisionDetection; +using BepuPhysics.Constraints; +using BepuUtilities; +using DemoContentLoader; +using DemoRenderer; +using DemoRenderer.UI; +using DemoUtilities; +using System; +using System.Numerics; + +namespace Demos.Demos +{ + /// + /// A string of beads launches itself out of a container. + /// + public class ChainFountainDemo : Demo + { + public unsafe override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(-30, 40, -30); + camera.Yaw = MathHelper.Pi * 3f / 4; + camera.Pitch = MathHelper.Pi * 0.2f; + + var filters = new CollidableProperty(); + Simulation = Simulation.Create(BufferPool, new RopeNarrowPhaseCallbacks(filters, new PairMaterialProperties(0.1f, float.MaxValue, new SpringSettings(480, 1)), 3), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 32)); + + var beadSpacing = 0.3f; + var beadShape = new Capsule(0.05f, beadSpacing); + var beadDescription = BodyDescription.CreateDynamic(new Vector3(), beadShape.ComputeInertia(1), Simulation.Shapes.Add(beadShape), 0.01f); + + const int beadCount = 4096; + var handles = new BodyHandle[beadCount]; + for (int i = 0; i < beadCount; ++i) + { + beadDescription.Pose.Position = new Vector3(0, i * beadSpacing + 4, 0); + //beadDescription.Velocity.Linear.X = i * 0.00013f; + handles[i] = Simulation.Bodies.Add(beadDescription); + filters.Allocate(handles[i]) = new RopeFilter { RopeIndex = 1, IndexInRope = (short)i }; + } + for (int i = 1; i < beadCount; ++i) + { + Simulation.Solver.Add(handles[i - 1], handles[i], new BallSocket { LocalOffsetA = new Vector3(0, beadSpacing * 0.5f, 0), LocalOffsetB = new Vector3(0, beadSpacing * -0.5f, 0), SpringSettings = new SpringSettings(480, 1) }); + Simulation.Solver.Add(handles[i - 1], handles[i], new SwingLimit { AxisLocalA = Vector3.UnitY, AxisLocalB = Vector3.UnitY, SpringSettings = new SpringSettings(480, 1), MaximumSwingAngle = MathF.PI * 0.15f }); + } + var radius = 2f; + var anglePerIteration = 2 * MathF.Asin(beadSpacing / (2 * radius)); + var heightPerIteration = beadShape.Radius * 2 / (MathF.PI * 2 / anglePerIteration); + for (int i = 0; i < beadCount; ++i) + { + var bead = Simulation.Bodies[handles[i]]; + var angle = i * anglePerIteration; + var nextAngle = (i + 1) * anglePerIteration; + + var currentPosition = new Vector3(3 + MathF.Sin(angle) * radius, 0.5f + heightPerIteration * i, MathF.Cos(angle) * radius); + var nextPosition = new Vector3(3 + MathF.Sin(nextAngle) * radius, 0.5f + heightPerIteration * (i + 1), MathF.Cos(nextAngle) * radius); + + //The constraints were built along the local Y axis, so get the shortest rotation from Y to the current orientation. + var offset = currentPosition - nextPosition; + var cross = Vector3.Cross(Vector3.Normalize(offset), new Vector3(0, 1, 0)); + var crossLength = cross.Length(); + var orientation = crossLength > 1e-8f ? QuaternionEx.CreateFromAxisAngle(cross / crossLength, (float)Math.Asin(crossLength)) : Quaternion.Identity; + + bead.Pose = new RigidPose(currentPosition, orientation); + + } + + for (int i = beadCount - 128; i < beadCount; ++i) + { + Simulation.Bodies[handles[i]].Velocity.Linear = new Vector3(20, 5, 0); + } + + + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0f, 0), Simulation.Shapes.Add(new Box(11, .2f, 40)))); + var wall = Simulation.Shapes.Add(new Box(.5f, 3, 40)); + Simulation.Statics.Add(new StaticDescription(new Vector3(5.75f, 2.4f - 1.5f, 0), wall)); + Simulation.Statics.Add(new StaticDescription(new Vector3(-5.75f, 2.4f - 1.5f, 0), wall)); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -500f, 0), Simulation.Shapes.Add(new Box(500, 1, 500)))); + + } + public override void Update(Window window, Camera camera, Input input, float dt) + { + //const float outerLoopSubsteps = 16; + //for (int i = 0; i < outerLoopSubsteps; ++i) + // Simulation.Timestep(TimestepDuration / outerLoopSubsteps, ThreadDispatcher); + base.Update(window, camera, input, dt); + } + + public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + { + var resolution = renderer.Surface.Resolution; + renderer.TextBatcher.Write(text.Clear().Append("The chain fountain is sometimes called Newton's beads or the Mould Effect."), new Vector2(16, resolution.Y - 48), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("A segmented rope gets yanked upwards and over the lip by falling segments."), new Vector2(16, resolution.Y - 32), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Peculiarly, the rope sometimes climbs to heights far over the container as bits of the rope 'kick' off the floor on their way out."), new Vector2(16, resolution.Y - 16), 16, Vector3.One, font); + base.Render(renderer, camera, input, text, font); + } + } +} diff --git a/Demos/Demos/RopeTwistDemo.cs b/Demos/Demos/RopeTwistDemo.cs index d661a8ea0..d983d4e33 100644 --- a/Demos/Demos/RopeTwistDemo.cs +++ b/Demos/Demos/RopeTwistDemo.cs @@ -13,74 +13,82 @@ namespace Demos.Demos { /// - /// Shows a bundle of ropes being tangled up by spinning weights. + /// Filter for a body in a rope, used by the . /// - public class RopeTwistDemo : Demo + struct RopeFilter { - struct Filter + public short RopeIndex; + public short IndexInRope; + } + + /// + /// Narrow phase callbacks that include collision filters designed for ropes. Adjacent bodies in a rope do not collide with each other. + /// + unsafe struct RopeNarrowPhaseCallbacks : INarrowPhaseCallbacks + { + public CollidableProperty Filters; + public PairMaterialProperties Material; + public int MinimumDistanceForCollisions; + + public RopeNarrowPhaseCallbacks(CollidableProperty filters, PairMaterialProperties contactMaterial, int minimumDistanceForCollisions = 3) { - public short RopeIndex; - public short IndexInRope; + Filters = filters; + Material = contactMaterial; + MinimumDistanceForCollisions = minimumDistanceForCollisions; } - - unsafe struct RopeNarrowPhaseCallbacks : INarrowPhaseCallbacks + public RopeNarrowPhaseCallbacks(CollidableProperty filters, int minimumDistanceForCollisions = 3) : this(filters, new PairMaterialProperties(1, 2, new SpringSettings(30, 1)), minimumDistanceForCollisions) { - public CollidableProperty Filters; - public PairMaterialProperties Material; - - public RopeNarrowPhaseCallbacks(CollidableProperty filters, PairMaterialProperties contactMaterial) - { - Filters = filters; - Material = contactMaterial; - } - public RopeNarrowPhaseCallbacks(CollidableProperty filters) : this(filters, new PairMaterialProperties(1, 2, new SpringSettings(30, 1))) - { - } + } - public void Initialize(Simulation simulation) - { - Filters.Initialize(simulation); - } + public void Initialize(Simulation simulation) + { + Filters.Initialize(simulation); + } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin) - { - var aFilter = Filters[a]; - var bFilter = Filters[b]; - return (aFilter.RopeIndex != bFilter.RopeIndex || Math.Abs(aFilter.IndexInRope - bFilter.IndexInRope) > 3) && (a.Mobility == CollidableMobility.Dynamic || b.Mobility == CollidableMobility.Dynamic); - } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin) + { + var aFilter = Filters[a]; + var bFilter = Filters[b]; + return (aFilter.RopeIndex != bFilter.RopeIndex || Math.Abs(aFilter.IndexInRope - bFilter.IndexInRope) > MinimumDistanceForCollisions) && (a.Mobility == CollidableMobility.Dynamic || b.Mobility == CollidableMobility.Dynamic); + } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB) - { - return true; - } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB) + { + return true; + } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold - { - pairMaterial = Material; - return true; - } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public unsafe bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold + { + pairMaterial = Material; + return true; + } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold) - { - return true; - } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold) + { + return true; + } - public void Dispose() - { - } + public void Dispose() + { } + } + /// + /// Shows a bundle of ropes being tangled up by spinning weights. + /// + public class RopeTwistDemo : Demo + { public unsafe override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(0, 20, 20); camera.Yaw = 0; camera.Pitch = 0; - var filters = new CollidableProperty(); + var filters = new CollidableProperty(); Simulation = Simulation.Create(BufferPool, new RopeNarrowPhaseCallbacks(filters, new PairMaterialProperties(0.0f, float.MaxValue, new SpringSettings(1200, 1))), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 60)); @@ -102,7 +110,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var wreckingBallBodyHandle = Simulation.Bodies.Add(description); var wreckingBallBody = Simulation.Bodies[wreckingBallBodyHandle]; wreckingBallBody.Velocity.Angular = new Vector3(0, 20, 0); - filters.Allocate(wreckingBallBodyHandle) = new Filter { RopeIndex = (short)(16384 + twistIndex), IndexInRope = ropeBodyCount }; + filters.Allocate(wreckingBallBodyHandle) = new RopeFilter { RopeIndex = (short)(16384 + twistIndex), IndexInRope = ropeBodyCount }; for (int ropeIndex = 0; ropeIndex < ropeCount; ++ropeIndex) { @@ -115,7 +123,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var bodyHandles = RopeStabilityDemo.BuildRopeBodies(Simulation, ropeStartLocation, ropeBodyCount, ropeBodyRadius, ropeBodySpacing, 1f, 0); for (int i = 0; i < bodyHandles.Length; ++i) { - filters.Allocate(bodyHandles[i]) = new Filter { RopeIndex = (short)ropeIndex, IndexInRope = (short)i }; + filters.Allocate(bodyHandles[i]) = new RopeFilter { RopeIndex = (short)ropeIndex, IndexInRope = (short)i }; } bool TryCreateConstraint(int handleIndexA, int handleIndexB) From 0a28ab4e2381e4261105f727d0051dcadc57aa15 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 29 Jan 2022 22:25:51 -0600 Subject: [PATCH 409/947] Tuned and added some extra bits to chain fountain demo. --- Demos/Demos/ChainFountainDemo.cs | 107 ++++--- Demos/Demos/SolverContactEnumerationDemo.cs | 332 +++++++++++--------- 2 files changed, 242 insertions(+), 197 deletions(-) diff --git a/Demos/Demos/ChainFountainDemo.cs b/Demos/Demos/ChainFountainDemo.cs index f54fce3a8..45080fe72 100644 --- a/Demos/Demos/ChainFountainDemo.cs +++ b/Demos/Demos/ChainFountainDemo.cs @@ -13,18 +13,18 @@ namespace Demos.Demos { /// - /// A string of beads launches itself out of a container. + /// A segmented rope chain thing launches itself out of a container. See also: https://en.wikipedia.org/wiki/Chain_fountain /// public class ChainFountainDemo : Demo { public unsafe override void Initialize(ContentArchive content, Camera camera) { - camera.Position = new Vector3(-30, 40, -30); - camera.Yaw = MathHelper.Pi * 3f / 4; - camera.Pitch = MathHelper.Pi * 0.2f; + camera.Position = new Vector3(5.65f, 3, -23); + camera.Yaw = MathF.PI; + camera.Pitch = 0; var filters = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new RopeNarrowPhaseCallbacks(filters, new PairMaterialProperties(0.1f, float.MaxValue, new SpringSettings(480, 1)), 3), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 32)); + Simulation = Simulation.Create(BufferPool, new RopeNarrowPhaseCallbacks(filters, new PairMaterialProperties(0.1f, float.MaxValue, new SpringSettings(240, 0)), 3), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 12)); var beadSpacing = 0.3f; var beadShape = new Capsule(0.05f, beadSpacing); @@ -32,67 +32,92 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) const int beadCount = 4096; var handles = new BodyHandle[beadCount]; - for (int i = 0; i < beadCount; ++i) - { - beadDescription.Pose.Position = new Vector3(0, i * beadSpacing + 4, 0); - //beadDescription.Velocity.Linear.X = i * 0.00013f; - handles[i] = Simulation.Bodies.Add(beadDescription); - filters.Allocate(handles[i]) = new RopeFilter { RopeIndex = 1, IndexInRope = (short)i }; - } - for (int i = 1; i < beadCount; ++i) - { - Simulation.Solver.Add(handles[i - 1], handles[i], new BallSocket { LocalOffsetA = new Vector3(0, beadSpacing * 0.5f, 0), LocalOffsetB = new Vector3(0, beadSpacing * -0.5f, 0), SpringSettings = new SpringSettings(480, 1) }); - Simulation.Solver.Add(handles[i - 1], handles[i], new SwingLimit { AxisLocalA = Vector3.UnitY, AxisLocalB = Vector3.UnitY, SpringSettings = new SpringSettings(480, 1), MaximumSwingAngle = MathF.PI * 0.15f }); - } - var radius = 2f; + var radius = 2.5f; var anglePerIteration = 2 * MathF.Asin(beadSpacing / (2 * radius)); var heightPerIteration = beadShape.Radius * 2 / (MathF.PI * 2 / anglePerIteration); for (int i = 0; i < beadCount; ++i) { - var bead = Simulation.Bodies[handles[i]]; - var angle = i * anglePerIteration; - var nextAngle = (i + 1) * anglePerIteration; - - var currentPosition = new Vector3(3 + MathF.Sin(angle) * radius, 0.5f + heightPerIteration * i, MathF.Cos(angle) * radius); - var nextPosition = new Vector3(3 + MathF.Sin(nextAngle) * radius, 0.5f + heightPerIteration * (i + 1), MathF.Cos(nextAngle) * radius); + var angle = MathF.PI + i * anglePerIteration; + var nextAngle = MathF.PI + (i + 1) * anglePerIteration; + var currentPosition = new Vector3(2.8f + MathF.Sin(angle) * radius, 0.5f + heightPerIteration * i, -15 + MathF.Cos(angle) * radius); + var nextPosition = new Vector3(2.8f + MathF.Sin(nextAngle) * radius, 0.5f + heightPerIteration * (i + 1), -15 + MathF.Cos(nextAngle) * radius); //The constraints were built along the local Y axis, so get the shortest rotation from Y to the current orientation. var offset = currentPosition - nextPosition; var cross = Vector3.Cross(Vector3.Normalize(offset), new Vector3(0, 1, 0)); var crossLength = cross.Length(); var orientation = crossLength > 1e-8f ? QuaternionEx.CreateFromAxisAngle(cross / crossLength, (float)Math.Asin(crossLength)) : Quaternion.Identity; - bead.Pose = new RigidPose(currentPosition, orientation); - - } + //Include a little nudge. This is going to create constraint error, but that's fine. It distributes the rope over the platform to avoid tangles. + beadDescription.Pose = new RigidPose(currentPosition + new Vector3(0, 0, i * 0.006f), orientation); + //Throw the tip of the rope off the edge. + if (i > beadCount - 32) + beadDescription.Velocity.Linear = new(20, 0, 0); + handles[i] = Simulation.Bodies.Add(beadDescription); + filters.Allocate(handles[i]) = new RopeFilter { RopeIndex = 1, IndexInRope = (short)i }; - for (int i = beadCount - 128; i < beadCount; ++i) - { - Simulation.Bodies[handles[i]].Velocity.Linear = new Vector3(20, 5, 0); + if (i > 0) + { + Simulation.Solver.Add(handles[i - 1], handles[i], new BallSocket { LocalOffsetA = new Vector3(0, beadSpacing * 0.5f, 0), LocalOffsetB = new Vector3(0, beadSpacing * -0.5f, 0), SpringSettings = new SpringSettings(120, 1) }); + Simulation.Solver.Add(handles[i - 1], handles[i], new SwingLimit { AxisLocalA = Vector3.UnitY, AxisLocalB = Vector3.UnitY, SpringSettings = new SpringSettings(120, 1), MaximumSwingAngle = MathF.PI * 0.05f }); + } } - - Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0f, 0), Simulation.Shapes.Add(new Box(11, .2f, 40)))); - var wall = Simulation.Shapes.Add(new Box(.5f, 3, 40)); - Simulation.Statics.Add(new StaticDescription(new Vector3(5.75f, 2.4f - 1.5f, 0), wall)); - Simulation.Statics.Add(new StaticDescription(new Vector3(-5.75f, 2.4f - 1.5f, 0), wall)); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0f, 0), Simulation.Shapes.Add(new Box(11.6f, .2f, 40)))); + var wall = Simulation.Shapes.Add(new Box(.4f, 1, 40)); + Simulation.Statics.Add(new StaticDescription(new Vector3(5.65f, 2.4f - 2f, 0), wall)); + Simulation.Statics.Add(new StaticDescription(new Vector3(-5.65f, 2.4f - 2f, 0), wall)); Simulation.Statics.Add(new StaticDescription(new Vector3(0, -500f, 0), Simulation.Shapes.Add(new Box(500, 1, 500)))); } public override void Update(Window window, Camera camera, Input input, float dt) { - //const float outerLoopSubsteps = 16; - //for (int i = 0; i < outerLoopSubsteps; ++i) - // Simulation.Timestep(TimestepDuration / outerLoopSubsteps, ThreadDispatcher); + //If you want smooth slow mo (instead of discrete slow mo by holding middle mouse), try uncommenting this (and commenting the base.Update). + //Simulation.Timestep(TimestepDuration / 16, ThreadDispatcher); base.Update(window, camera, input, dt); } public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) { + ////If you'd like to see forces visualized, you can uncomment this section. It reuses the SolverContactEnumerationDemo extractor to pull contact forces. + ////Technically, you could be a little pickier about what data you collect- checking the impulses before trying to load offsets/normals. + //var extractor = new SolverContactDataExtractor(BufferPool, 4); + //var impulseToForce = Simulation.Solver.SubstepCount / TimestepDuration; + //for (int i = 0; i < Simulation.Bodies.ActiveSet.Count; ++i) + //{ + // var constraintsForBody = Simulation.Bodies.ActiveSet.Constraints[i]; + // for (int j = 0; j < constraintsForBody.Count; ++j) + // { + // if (Simulation.NarrowPhase.TryExtractSolverContactData(constraintsForBody[j].ConnectingConstraintHandle, ref extractor)) + // { + // for (int k = 0; k < extractor.Constraints.Count; ++k) + // { + // ref var manifold = ref extractor.Constraints[k]; + // //This is a collision against a static, so we can consider visualizing it. + // var contacts = manifold.Contacts; + // var bodyA = manifold.BodyA; + // for (int l = 0; l < contacts.Count; ++l) + // { + // ref var contact = ref contacts[l]; + // var force = contact.PenetrationImpulse * impulseToForce; + // if (force > 100) + // { + // var cylinder = new Cylinder(0.15f, force * 0.0005f); + // renderer.Shapes.AddShape(cylinder, Simulation.Shapes, new RigidPose(contact.OffsetA + Simulation.Bodies[bodyA].Pose.Position + new Vector3(0, cylinder.HalfLength, 0), QuaternionEx.Identity), new Vector3(0, 1, 1)); + // } + + // } + // } + + // } + // } + // extractor.Reset(); + //} var resolution = renderer.Surface.Resolution; - renderer.TextBatcher.Write(text.Clear().Append("The chain fountain is sometimes called Newton's beads or the Mould Effect."), new Vector2(16, resolution.Y - 48), 16, Vector3.One, font); - renderer.TextBatcher.Write(text.Clear().Append("A segmented rope gets yanked upwards and over the lip by falling segments."), new Vector2(16, resolution.Y - 32), 16, Vector3.One, font); - renderer.TextBatcher.Write(text.Clear().Append("Peculiarly, the rope sometimes climbs to heights far over the container as bits of the rope 'kick' off the floor on their way out."), new Vector2(16, resolution.Y - 16), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("The chain fountain is sometimes called Newton's beads or the Mould Effect."), new Vector2(16, resolution.Y - 64), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("A stiff segmented rope gets yanked upwards and over the lip by falling segments."), new Vector2(16, resolution.Y - 48), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Peculiarly, the rope sometimes climbs to heights far higher than the container edge as bits of the rope 'kick' off the floor on their way out."), new Vector2(16, resolution.Y - 32), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("The effect tends to be more visible with chains that resist bending more."), new Vector2(16, resolution.Y - 16), 16, Vector3.One, font); base.Render(renderer, camera, input, text, font); } } diff --git a/Demos/Demos/SolverContactEnumerationDemo.cs b/Demos/Demos/SolverContactEnumerationDemo.cs index e73f43c92..7fd8729f3 100644 --- a/Demos/Demos/SolverContactEnumerationDemo.cs +++ b/Demos/Demos/SolverContactEnumerationDemo.cs @@ -15,6 +15,177 @@ namespace Demos.Demos { + /// + /// Stores the easy to read data associated with a single contact extracted from solver contact constraints. + /// + struct ExtractedContact + { + public Vector3 OffsetA; + public float Depth; + //For the purposes of the demo, we'll store a normal for every single contact, even though the original manifold might have been convex (and so shared one normal across all its contacts). + public Vector3 Normal; + public float PenetrationImpulse; + //We'll also derive friction impulse from different sources depending on convexity- convex manifolds have a single twist/tangent friction constraint, while nonconvex manifolds have a per-contact tangent friction. + public float FrictionImpulseMagnitude; + } + + /// + /// Stores the connected bodies and contacts for a contact manifold constraint extracted from the solver. + /// + struct ExtractedManifold + { + public QuickList Contacts; + public BodyHandle BodyA; + //For one body constraints, this will be -1. + public BodyHandle BodyB; + + public ExtractedManifold(BufferPool pool, BodyHandle a, BodyHandle b) + { + //Nonconvex manifolds will never have less than the convex count, so we'll preallocate enough space for a nonconvex manifold. + Contacts = new QuickList(NonconvexContactManifold.MaximumContactCount, pool); + BodyA = a; + BodyB = b; + } + + public ExtractedManifold(BufferPool pool, BodyHandle a) : this(pool, a, new BodyHandle(-1)) { } + + public void Dispose(BufferPool pool) + { + Contacts.Dispose(pool); + } + } + + /// + /// Example implementation of a that pulls contact data into an easier to read format. + /// + struct SolverContactDataExtractor : ISolverContactDataExtractor + { + //We'll pull the solver data into a different form- just a list of contacts with basic data about each one. + //What you store depends on your application's needs- there's nothing saying you have to use this layout. + //The ISolverContactDataExtractor can be thought of as the foundation on which to build other (more convenient) abstractions. + public QuickList Constraints; + public BufferPool Pool; + + public SolverContactDataExtractor(BufferPool pool, int initialCapacity) + { + Pool = pool; + Constraints = new QuickList(initialCapacity, pool); + } + + //The callbacks distinguish between convex and nonconvex because the underlying data is different. + //Nonconvex manifolds can have different normals at each contact, while convex manifolds share one normal across all contacts. + //This also affects the accumulated impulses- nonconvex impulses need to include per-contact tangent friction impulses due to the potentially differing normals. + + //In all of the following, you might notice an unusual pattern- things like prestep.GetNormal(ref prestep...). + //This is a bit of a hack to work around the fact that + //1) struct member functions cannot return a reference to the 'this' instance, and + //2) there is no way in the current version of C# for an interface to require a static function. (This will change with static abstracts!) + //It's another weird little tradeoff for minimal overhead. + + void ExtractConvexData(ref ExtractedManifold constraintContacts, ref TPrestep prestep, ref TAccumulatedImpulses impulses) + where TPrestep : struct, IConvexContactPrestep + where TAccumulatedImpulses : struct, IConvexContactAccumulatedImpulses + { + //Note that we narrow from the raw vectorized reference into a more application-convenient AOS representation. + Vector3Wide.ReadFirst(prestep.GetNormal(ref prestep), out var normal); + + //We'll approximate the per-contact friction by allocating the shared friction impulses to contacts weighted by their penetration impulse. + float totalPenetrationImpulse = 0; + for (int i = 0; i < prestep.ContactCount; ++i) + { + ref var sourceContact = ref prestep.GetContact(ref prestep, i); + ref var targetContact = ref constraintContacts.Contacts.AllocateUnsafely(); + Vector3Wide.ReadFirst(sourceContact.OffsetA, out targetContact.OffsetA); + //We can use [0] to access the slot because the prestep bundle memory reference was already offset for us. + targetContact.Depth = sourceContact.Depth[0]; + targetContact.Normal = normal; + targetContact.PenetrationImpulse = impulses.GetPenetrationImpulseForContact(ref impulses, i)[0]; + totalPenetrationImpulse += targetContact.PenetrationImpulse; + } + Vector2Wide.ReadFirst(impulses.GetTangentFriction(ref impulses), out var tangentFriction); + var twistFriction = impulses.GetTwistFriction(ref impulses)[0]; + //This isn't a 'correct' allocation of impulses, we just want a rough sense. + var frictionMagnitudeApproximation = MathF.Sqrt(tangentFriction.LengthSquared() + twistFriction * twistFriction); + var impulseScale = totalPenetrationImpulse > 0 ? frictionMagnitudeApproximation / totalPenetrationImpulse : 0; + for (int i = 0; i < prestep.ContactCount; ++i) + { + ref var contact = ref constraintContacts.Contacts[i]; + contact.FrictionImpulseMagnitude = contact.PenetrationImpulse * impulseScale; + } + } + + public void ConvexOneBody(BodyHandle bodyHandle, ref TPrestep prestep, ref TAccumulatedImpulses impulses) + where TPrestep : struct, IConvexContactPrestep + where TAccumulatedImpulses : struct, IConvexContactAccumulatedImpulses + { + ref var constraintContacts = ref Constraints.Allocate(Pool); + constraintContacts = new ExtractedManifold(Pool, bodyHandle); + ExtractConvexData(ref constraintContacts, ref prestep, ref impulses); + } + + public void ConvexTwoBody(BodyHandle bodyHandleA, BodyHandle bodyHandleB, ref TPrestep prestep, ref TAccumulatedImpulses impulses) + where TPrestep : struct, ITwoBodyConvexContactPrestep + where TAccumulatedImpulses : struct, IConvexContactAccumulatedImpulses + { + ref var constraintContacts = ref Constraints.Allocate(Pool); + constraintContacts = new ExtractedManifold(Pool, bodyHandleA, bodyHandleB); + ExtractConvexData(ref constraintContacts, ref prestep, ref impulses); + } + + void ExtractNonconvexData(ref ExtractedManifold constraintContacts, ref TPrestep prestep, ref TAccumulatedImpulses impulses) + where TPrestep : struct, INonconvexContactPrestep + where TAccumulatedImpulses : struct, INonconvexContactAccumulatedImpulses + { + //Nonconvex types require no approximation of friction; we can pull it directly from the solved results. + for (int i = 0; i < prestep.ContactCount; ++i) + { + ref var sourceContact = ref prestep.GetContact(ref prestep, i); + ref var targetContact = ref constraintContacts.Contacts.AllocateUnsafely(); + Vector3Wide.ReadFirst(sourceContact.Offset, out targetContact.OffsetA); + targetContact.Depth = sourceContact.Depth[0]; + Vector3Wide.ReadFirst(sourceContact.Normal, out targetContact.Normal); + + ref var contactImpulses = ref impulses.GetImpulsesForContact(ref impulses, i); + targetContact.PenetrationImpulse = contactImpulses.Penetration[0]; + Vector2Wide.ReadFirst(contactImpulses.Tangent, out var tangentImpulses); + targetContact.FrictionImpulseMagnitude = tangentImpulses.Length(); + } + } + + public void NonconvexOneBody(BodyHandle bodyHandle, ref TPrestep prestep, ref TAccumulatedImpulses impulses) + where TPrestep : struct, INonconvexContactPrestep + where TAccumulatedImpulses : struct, INonconvexContactAccumulatedImpulses + { + ref var constraintContacts = ref Constraints.Allocate(Pool); + constraintContacts = new ExtractedManifold(Pool, bodyHandle); + ExtractNonconvexData(ref constraintContacts, ref prestep, ref impulses); + } + + public void NonconvexTwoBody(BodyHandle bodyHandleA, BodyHandle bodyHandleB, ref TPrestep prestep, ref TAccumulatedImpulses impulses) + where TPrestep : struct, ITwoBodyNonconvexContactPrestep + where TAccumulatedImpulses : struct, INonconvexContactAccumulatedImpulses + { + ref var constraintContacts = ref Constraints.Allocate(Pool); + constraintContacts = new ExtractedManifold(Pool, bodyHandleA, bodyHandleB); + ExtractNonconvexData(ref constraintContacts, ref prestep, ref impulses); + } + + public void Reset() + { + for (int i = 0; i < Constraints.Count; ++i) + { + Constraints[i].Dispose(Pool); + } + Constraints.Count = 0; + } + + public void Dispose() + { + Reset(); + Constraints.Dispose(Pool); + } + } + /// /// Shows how to enumerate contact constraints in the solver using contact accessors and ISolverContactDataExtractor. /// @@ -61,162 +232,11 @@ public override void Initialize(ContentArchive content, Camera camera) Simulation.Statics.Add(new StaticDescription(new Vector3(0, -2, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), Simulation.Shapes.Add(planeMesh))); } - struct Contact - { - public Vector3 OffsetA; - public float Depth; - //For the purposes of the demo, we'll store a normal for every single contact, even though the original manifold might have been convex (and so shared one normal across all its contacts). - public Vector3 Normal; - public float PenetrationImpulse; - //We'll also derive friction impulse from different sources depending on convexity- convex manifolds have a single twist/tangent friction constraint, while nonconvex manifolds have a per-contact tangent friction. - public float FrictionImpulseMagnitude; - } - - struct ConstraintContacts - { - public QuickList Contacts; - public BodyHandle BodyA; - //For one body constraints, this will be -1. - public BodyHandle BodyB; - - public ConstraintContacts(BufferPool pool, BodyHandle a, BodyHandle b) - { - //Nonconvex manifolds will never have less than the convex count, so we'll preallocate enough space for a nonconvex manifold. - Contacts = new QuickList(NonconvexContactManifold.MaximumContactCount, pool); - BodyA = a; - BodyB = b; - } - - public ConstraintContacts(BufferPool pool, BodyHandle a) : this(pool, a, new BodyHandle(-1)) { } - } - - struct Extractor : ISolverContactDataExtractor - { - //We'll pull the solver data into a different form- just a list of contacts with basic data about each one. - //What you store depends on your application's needs- there's nothing saying you have to use this layout. - //The ISolverContactDataExtractor can be thought of as the foundation on which to build other (more convenient) abstractions. - public QuickList ConstraintContacts; - public BufferPool Pool; - - public Extractor(BufferPool pool, int initialCapacity) - { - Pool = pool; - ConstraintContacts = new QuickList(initialCapacity, pool); - } - - //The callbacks distinguish between convex and nonconvex because the underlying data is different. - //Nonconvex manifolds can have different normals at each contact, while convex manifolds share one normal across all contacts. - //This also affects the accumulated impulses- nonconvex impulses need to include per-contact tangent friction impulses due to the potentially differing normals. - - //In all of the following, you might notice an unusual pattern- things like prestep.GetNormal(ref prestep...). - //This is a bit of a hack to work around the fact that - //1) struct member functions cannot return a reference to the 'this' instance, and - //2) there is no way in the current version of C# for an interface to require a static function. - //It's another weird little tradeoff for minimal overhead. - - void ExtractConvexData(ref ConstraintContacts constraintContacts, ref TPrestep prestep, ref TAccumulatedImpulses impulses) - where TPrestep : struct, IConvexContactPrestep - where TAccumulatedImpulses : struct, IConvexContactAccumulatedImpulses - { - //Note that we narrow from the raw vectorized reference into a more application-convenient AOS representation. - Vector3Wide.ReadFirst(prestep.GetNormal(ref prestep), out var normal); - - //We'll approximate the per-contact friction by allocating the shared friction impulses to contacts weighted by their penetration impulse. - float totalPenetrationImpulse = 0; - for (int i = 0; i < prestep.ContactCount; ++i) - { - ref var sourceContact = ref prestep.GetContact(ref prestep, i); - ref var targetContact = ref constraintContacts.Contacts.AllocateUnsafely(); - Vector3Wide.ReadFirst(sourceContact.OffsetA, out targetContact.OffsetA); - //We can use [0] to access the slot because the prestep bundle memory reference was already offset for us. - targetContact.Depth = sourceContact.Depth[0]; - targetContact.Normal = normal; - targetContact.PenetrationImpulse = impulses.GetPenetrationImpulseForContact(ref impulses, i)[0]; - totalPenetrationImpulse += targetContact.PenetrationImpulse; - } - Vector2Wide.ReadFirst(impulses.GetTangentFriction(ref impulses), out var tangentFriction); - var twistFriction = impulses.GetTwistFriction(ref impulses)[0]; - //This isn't a 'correct' allocation of impulses, we just want a rough sense. - var frictionMagnitudeApproximation = MathF.Sqrt(tangentFriction.LengthSquared() + twistFriction * twistFriction); - var impulseScale = totalPenetrationImpulse > 0 ? frictionMagnitudeApproximation / totalPenetrationImpulse : 0; - for (int i = 0; i < prestep.ContactCount; ++i) - { - ref var contact = ref constraintContacts.Contacts[i]; - contact.FrictionImpulseMagnitude = contact.PenetrationImpulse * impulseScale; - } - } - - public void ConvexOneBody(BodyHandle bodyHandle, ref TPrestep prestep, ref TAccumulatedImpulses impulses) - where TPrestep : struct, IConvexContactPrestep - where TAccumulatedImpulses : struct, IConvexContactAccumulatedImpulses - { - ref var constraintContacts = ref ConstraintContacts.Allocate(Pool); - constraintContacts = new ConstraintContacts(Pool, bodyHandle); - ExtractConvexData(ref constraintContacts, ref prestep, ref impulses); - } - - public void ConvexTwoBody(BodyHandle bodyHandleA, BodyHandle bodyHandleB, ref TPrestep prestep, ref TAccumulatedImpulses impulses) - where TPrestep : struct, ITwoBodyConvexContactPrestep - where TAccumulatedImpulses : struct, IConvexContactAccumulatedImpulses - { - ref var constraintContacts = ref ConstraintContacts.Allocate(Pool); - constraintContacts = new ConstraintContacts(Pool, bodyHandleA, bodyHandleB); - ExtractConvexData(ref constraintContacts, ref prestep, ref impulses); - } - - void ExtractNonconvexData(ref ConstraintContacts constraintContacts, ref TPrestep prestep, ref TAccumulatedImpulses impulses) - where TPrestep : struct, INonconvexContactPrestep - where TAccumulatedImpulses : struct, INonconvexContactAccumulatedImpulses - { - //Nonconvex types require no approximation of friction; we can pull it directly from the solved results. - for (int i = 0; i < prestep.ContactCount; ++i) - { - ref var sourceContact = ref prestep.GetContact(ref prestep, i); - ref var targetContact = ref constraintContacts.Contacts.AllocateUnsafely(); - Vector3Wide.ReadFirst(sourceContact.Offset, out targetContact.OffsetA); - targetContact.Depth = sourceContact.Depth[0]; - Vector3Wide.ReadFirst(sourceContact.Normal, out targetContact.Normal); - - ref var contactImpulses = ref impulses.GetImpulsesForContact(ref impulses, i); - targetContact.PenetrationImpulse = contactImpulses.Penetration[0]; - Vector2Wide.ReadFirst(contactImpulses.Tangent, out var tangentImpulses); - targetContact.FrictionImpulseMagnitude = tangentImpulses.Length(); - } - } - - public void NonconvexOneBody(BodyHandle bodyHandle, ref TPrestep prestep, ref TAccumulatedImpulses impulses) - where TPrestep : struct, INonconvexContactPrestep - where TAccumulatedImpulses : struct, INonconvexContactAccumulatedImpulses - { - ref var constraintContacts = ref ConstraintContacts.Allocate(Pool); - constraintContacts = new ConstraintContacts(Pool, bodyHandle); - ExtractNonconvexData(ref constraintContacts, ref prestep, ref impulses); - } - - public void NonconvexTwoBody(BodyHandle bodyHandleA, BodyHandle bodyHandleB, ref TPrestep prestep, ref TAccumulatedImpulses impulses) - where TPrestep : struct, ITwoBodyNonconvexContactPrestep - where TAccumulatedImpulses : struct, INonconvexContactAccumulatedImpulses - { - ref var constraintContacts = ref ConstraintContacts.Allocate(Pool); - constraintContacts = new ConstraintContacts(Pool, bodyHandleA, bodyHandleB); - ExtractNonconvexData(ref constraintContacts, ref prestep, ref impulses); - } - - public void Dispose() - { - for (int i = 0; i < ConstraintContacts.Count; ++i) - { - ref var constraintContacts = ref ConstraintContacts[i]; - constraintContacts.Contacts.Dispose(Pool); - } - ConstraintContacts.Dispose(Pool); - } - } public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) { var sensorBody = Simulation.Bodies[sensorBodyHandle]; - var extractor = new Extractor(BufferPool, sensorBody.Constraints.Count); + var extractor = new SolverContactDataExtractor(BufferPool, sensorBody.Constraints.Count); //The basic idea behind the contact extractor is to submit it to a narrow phase contact accessor that is able to understand the solver's layout, //which will then call the contact extractor's relevant callbacks for the type of constraint encountered. //Here, we'll enumerate over all the constraints currently affecting the sensor body, attempting to extract contact data from each one. @@ -228,9 +248,9 @@ public override void Render(Renderer renderer, Camera camera, Input input, TextB //We now have extracted contact data. Let's analyze it! //For the purposes of the demo, we'll draw debug shapes at the contacts to represent the different properties. int sensorContactCount = 0; - for (int manifoldIndex = 0; manifoldIndex < extractor.ConstraintContacts.Count; ++manifoldIndex) + for (int manifoldIndex = 0; manifoldIndex < extractor.Constraints.Count; ++manifoldIndex) { - ref var constraintContacts = ref extractor.ConstraintContacts[manifoldIndex]; + ref var constraintContacts = ref extractor.Constraints[manifoldIndex]; var bodyA = Simulation.Bodies[constraintContacts.BodyA]; sensorContactCount += constraintContacts.Contacts.Count; for (int contactIndex = 0; contactIndex < constraintContacts.Contacts.Count; ++contactIndex) @@ -261,8 +281,8 @@ public override void Render(Renderer renderer, Camera camera, Input input, TextB renderer.TextBatcher.Write(text.Clear().Append("The solver stores data in an optimized array-of-structures-of-arrays format that makes it difficult to directly read."), new Vector2(16, resolution.Y - 96), 16, Vector3.One, font); renderer.TextBatcher.Write(text.Clear().Append("This demo implements an ISolverContactDataExtractor that pulls data out of the solver and puts it into a simpler format."), new Vector2(16, resolution.Y - 80), 16, Vector3.One, font); renderer.TextBatcher.Write(text.Clear().Append("Pulling data this way can sometimes be more convenient than tracking contacts from INarrowPhaseCallbacks."), new Vector2(16, resolution.Y - 64), 16, Vector3.One, font); - renderer.TextBatcher.Write(text.Clear().Append("Contacts associated with the large box are visualized. The size of a contact corresponds to the contact's force."), new Vector2(16, resolution.Y - 48), 16, Vector3.One, font); - renderer.TextBatcher.Write(text.Clear().Append("Sensor manifold constraint count: ").Append(extractor.ConstraintContacts.Count).Append(", contact count: ").Append(sensorContactCount), new Vector2(16, resolution.Y - 20), 20, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Contacts associated with the large box are visualized. The size of a contact corresponds to the contact's force."), new Vector2(16, resolution.Y - 48), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Sensor manifold constraint count: ").Append(extractor.Constraints.Count).Append(", contact count: ").Append(sensorContactCount), new Vector2(16, resolution.Y - 20), 20, Vector3.One, font); base.Render(renderer, camera, input, text, font); } } From e28e15e9579460f8953835ef3a82b5f0c04980a0 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 30 Jan 2022 16:06:14 -0600 Subject: [PATCH 410/947] Contact visualization memory leak! --- Demos/DemoSet.cs | 2 +- Demos/Demos/ChainFountainDemo.cs | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index 6f6889469..0cf3a1a5e 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -46,7 +46,6 @@ struct Option public DemoSet() { - AddOption(); AddOption(); AddOption(); AddOption(); @@ -62,6 +61,7 @@ public DemoSet() AddOption(); AddOption(); AddOption(); + AddOption(); AddOption(); AddOption(); AddOption(); diff --git a/Demos/Demos/ChainFountainDemo.cs b/Demos/Demos/ChainFountainDemo.cs index 45080fe72..c65fa8d9c 100644 --- a/Demos/Demos/ChainFountainDemo.cs +++ b/Demos/Demos/ChainFountainDemo.cs @@ -113,6 +113,7 @@ public override void Render(Renderer renderer, Camera camera, Input input, TextB // } // extractor.Reset(); //} + //extractor.Dispose(); var resolution = renderer.Surface.Resolution; renderer.TextBatcher.Write(text.Clear().Append("The chain fountain is sometimes called Newton's beads or the Mould Effect."), new Vector2(16, resolution.Y - 64), 16, Vector3.One, font); renderer.TextBatcher.Write(text.Clear().Append("A stiff segmented rope gets yanked upwards and over the lip by falling segments."), new Vector2(16, resolution.Y - 48), 16, Vector3.One, font); From 045c4b041c007cf41a64f9b679d8d8e6b36f4410 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 30 Jan 2022 16:34:25 -0600 Subject: [PATCH 411/947] Grabber a bit more consistent. --- Demos/Grabber.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Demos/Grabber.cs b/Demos/Grabber.cs index a05c78b0e..ac3b9ce03 100644 --- a/Demos/Grabber.cs +++ b/Demos/Grabber.cs @@ -61,7 +61,7 @@ readonly void CreateMotorDescription(in Vector3 target, float inverseMass, out O angularDescription = new OneBodyAngularServo { TargetOrientation = targetOrientation, - ServoSettings = new ServoSettings(float.MaxValue, 0, 360 / inverseMass), + ServoSettings = new ServoSettings(float.MaxValue, 0, localGrabPoint.Length() * 180 / inverseMass), SpringSettings = new SpringSettings(5, 2), }; } From d5f23531397a5314401e92cd6dd94207b2a660a8 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 30 Jan 2022 17:12:19 -0600 Subject: [PATCH 412/947] Substepping notes. --- Documentation/Substepping.md | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/Documentation/Substepping.md b/Documentation/Substepping.md index 86e19a64d..0e25260a8 100644 --- a/Documentation/Substepping.md +++ b/Documentation/Substepping.md @@ -3,12 +3,22 @@ Substepping integrates body velocities and positions and solves constraints more You can configure a simulation to use substepping by passing a `SolveDescription` to `Simulation.Create` that has more than one substep. For example, to create a simulation that uses 8 substeps and 1 velocity iteration per substep: ```cs -var simulation = Simulation.Create(BufferPool, new NarrowPhaseCallbacks(), new PoseIntegratorCallbacks(), new SolveDescription(velocityIterationCount: 1, substepCount: 8)); +var simulation = Simulation.Create( + BufferPool, new NarrowPhaseCallbacks(), new PoseIntegratorCallbacks(), + new SolveDescription(velocityIterationCount: 1, substepCount: 8)); ``` # Why use it? -# Limitations +It makes difficult constraint configurations easy for the solver. The easier things are for the solver, the faster it can go. + +If you have a really complex constraint graph, especially one containing high mass ratios (heavy objects depending on light objects, like a wrecking ball hanging from a rope or a tank smashing a small box) and high constraint stiffnesses, a non-substepping solver can struggle to converge to an equilibrium in a low number of velocity iterations. + +Further, for constraints with high stiffness (`SpringSettings` with `Frequency` values approaching or exceeding the simulation timestep frequency), even a stable equilibrium will result in damping out unrepresentable motion. A constraint that wants to oscillate at 120 hertz simply can't in a 60 hertz simulation. + +Substepping means running the solver and integrator multiple times for each call to `Simulation.Timestep`. If you take 8 substeps and call `Simulation.Timestep(1f / 60f)`, the solver sees 8 substeps each of length `1f / 480f`. Since the solver and integrator are running at 480 hertz, that 120 hertz constraint would be able to wiggle to its heart's content. + +In the above example, you could get similar solver stability out of simply calling `Simulation.Timestep(1f / 480f)` 8 times for each frame, but that would re-run collision detection 8 times too. Further, by tightly bundling execution together, the substepping solver can avoid a large amount of synchronization and memory bandwidth overhead. Overall, when it is an appropriate solution, substepping will tend to be the fastest option. # How substepping fits into a timestep Each call to `Simulation.Timestep(dt, ...)` simulates one frame with duration equal to `dt`. In the [`DefaultTimestepper`](../BepuPhysics/DefaultTimestepper.cs) (which, as the name implies, is the `ITimestepper` implementation used if no other is specified) executes a frame like so: @@ -53,3 +63,4 @@ The solver exposes events that fire at the beginning and end of each substep: `S (Note that attempting to dispatch multithreaded work from the same `IThreadDispatcher` instance that dispatched the solver's workers requires that the `IThreadDispatcher` implementation is reentrant. The demos `SimpleThreadDispatcher` is not.) +# Limitations From 5b6ac43a76d538dafc36d0e50c4b75c0f23fd52a Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 30 Jan 2022 18:06:15 -0600 Subject: [PATCH 413/947] Substepping limitations documentation. --- Documentation/Substepping.md | 21 +++++++++++++++++++-- 1 file changed, 19 insertions(+), 2 deletions(-) diff --git a/Documentation/Substepping.md b/Documentation/Substepping.md index 0e25260a8..23346c9bd 100644 --- a/Documentation/Substepping.md +++ b/Documentation/Substepping.md @@ -9,7 +9,6 @@ var simulation = Simulation.Create( ``` # Why use it? - It makes difficult constraint configurations easy for the solver. The easier things are for the solver, the faster it can go. If you have a really complex constraint graph, especially one containing high mass ratios (heavy objects depending on light objects, like a wrecking ball hanging from a rope or a tank smashing a small box) and high constraint stiffnesses, a non-substepping solver can struggle to converge to an equilibrium in a low number of velocity iterations. @@ -58,9 +57,27 @@ It's also possible to update the cached guess in response to a timestep change u Changing the number of *velocity iterations* from frame to frame is safe. The more velocity iterations there are, the closer the solution will converge to an optimum during the substep. # Callbacks - The solver exposes events that fire at the beginning and end of each substep: `SubstepStarted` and `SubstepEnded`. These events are called from worker thread 0 in the solver's thread dispatch; the dispatch does not end in between substeps to keep overhead low. (Note that attempting to dispatch multithreaded work from the same `IThreadDispatcher` instance that dispatched the solver's workers requires that the `IThreadDispatcher` implementation is reentrant. The demos `SimpleThreadDispatcher` is not.) # Limitations +Unfortunately, substepping isn't magic. The entire point is to avoid running other parts of the engine at the same rate as the solver, so contacts do not get fully updated for each substep. They *do* undergo an incremental update process that tries to fix up the most obvious issues (like penetration depth changes over time), but without a full collision test the contact manifolds can go out of date. + +This incremental update is usually fine, but out of date contacts can sometimes introduce energy. For example, an out of date contact lever arm can let a body 'fall' into another body ever so slightly, which over many substeps ends up sustaining oscillation. + +You can see an example of this behavior [here](https://youtu.be/70IAdC-4Sa0). + +To mitigate this issue, you can try: +1. damping the relevant bodies more heavily in the integrator, +2. increasing the damping of contacts associated with the relevant bodies, +3. increasing the sleeping velocity threshold (`BodyActivityDescription.SleepThreshold` passed into the `BodyDescription`) for the relevant bodies such that they take a nap instead of wiggling, +4. increasing the inertia of the problematic bodies to increase the period of oscillation (possibly making it easier to mitigate with sleeping/damping) +5. avoiding shapes or situations that are likely to cause the problem, +6. or just don't use solver substepping. You can always resort to calling `Simulation.Timestep` more frequently. It'll cost more than solver-only substepping, but it'll keep all your contact data up to date, and the library's pretty dang fast anyway. + +Another far more subtle effect arises from accumulated numerical error. Even without using substepping, some slight numerical drift will occur on every frame. Even with enough velocity iterations to converge, there might still be a 1e-7 error in the relative velocity. Integrating those errors into the position over time causes drift. + +With sleeping enabled and reasonable simulation configuration, this is effectively invisible. However, disabling sleeping and using extreme substepping (such as effective solver rates in the tens or hundreds of thousands of hertz) can make it obvious. [See here for an example](https://youtu.be/0kkHebYnARs). + +[#167](https://github.com/bepu/bepuphysics2/issues/167) tracks one solution to this- friction with explicit position goals. Another option that can be implemented externally and would work for all constraint types (contact or not) is to quantize body positions. By forcing body positions onto a grid with spacing larger than the per-frame drift, the drift cannot accumulate. \ No newline at end of file From 57fd7677d8cf43904e16881898e6f77b617d24d1 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 30 Jan 2022 19:12:02 -0600 Subject: [PATCH 414/947] Substepping doctweaks. --- Documentation/Substepping.md | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/Documentation/Substepping.md b/Documentation/Substepping.md index 23346c9bd..e4ad6ef65 100644 --- a/Documentation/Substepping.md +++ b/Documentation/Substepping.md @@ -4,10 +4,12 @@ Substepping integrates body velocities and positions and solves constraints more You can configure a simulation to use substepping by passing a `SolveDescription` to `Simulation.Create` that has more than one substep. For example, to create a simulation that uses 8 substeps and 1 velocity iteration per substep: ```cs var simulation = Simulation.Create( - BufferPool, new NarrowPhaseCallbacks(), new PoseIntegratorCallbacks(), + BufferPool, new YourNarrowPhaseCallbacks(), new YourPoseIntegratorCallbacks(), new SolveDescription(velocityIterationCount: 1, substepCount: 8)); ``` +See the [SubsteppingDemo](../Demos/Demos/SubsteppingDemo.cs) for an interactive example. The [RopeTwistDemo](../Demos/Demos/RopeTwistDemo.cs), [ChainFountainDemo](../Demos/Demos/ChainFountainDemo.cs) and [BouncinessDemo](../Demos/Demos/BouncinessDemo.cs) also all use substepping. + # Why use it? It makes difficult constraint configurations easy for the solver. The easier things are for the solver, the faster it can go. @@ -59,7 +61,7 @@ Changing the number of *velocity iterations* from frame to frame is safe. The mo # Callbacks The solver exposes events that fire at the beginning and end of each substep: `SubstepStarted` and `SubstepEnded`. These events are called from worker thread 0 in the solver's thread dispatch; the dispatch does not end in between substeps to keep overhead low. -(Note that attempting to dispatch multithreaded work from the same `IThreadDispatcher` instance that dispatched the solver's workers requires that the `IThreadDispatcher` implementation is reentrant. The demos `SimpleThreadDispatcher` is not.) +(Note that attempting to dispatch multithreaded work from the same `IThreadDispatcher` instance that dispatched the solver's workers requires that the `IThreadDispatcher` implementation is reentrant. The `BepuUtilities.ThreadDispatcher` is not.) # Limitations Unfortunately, substepping isn't magic. The entire point is to avoid running other parts of the engine at the same rate as the solver, so contacts do not get fully updated for each substep. They *do* undergo an incremental update process that tries to fix up the most obvious issues (like penetration depth changes over time), but without a full collision test the contact manifolds can go out of date. From fbe7f91ec66e60ed464d423107b6d0143ad40372 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 30 Jan 2022 19:18:20 -0600 Subject: [PATCH 415/947] Documentation crosslinks. --- Documentation/PerformanceTips.md | 2 +- Documentation/StabilityTips.md | 2 +- Documentation/Substepping.md | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/Documentation/PerformanceTips.md b/Documentation/PerformanceTips.md index ca9c22bf4..4258a7728 100644 --- a/Documentation/PerformanceTips.md +++ b/Documentation/PerformanceTips.md @@ -36,4 +36,4 @@ Note that cylinders and convex hulls will likely [become faster](https://github. ## Solver Optimization -Try using the minimum number of iterations sufficient to retain stability. The cost of the solver stage is linear with the number of iterations, and some simulations can get by with very few. --For some simulations with very complex constraint configurations, there may be no practical number of solver iterations that can stabilize the simulation. In these cases, you may need to instead use substepping or a shorter time step duration for the entire simulation. More frequent solver execution can massively improve simulation quality, allowing you to drop velocity iteration counts massively (even to just 1 per substep). See the [`SubsteppingDemo`](../Demos/SubsteppingDemo.cs) for an example of substepping in action. \ No newline at end of file +-For some simulations with very complex constraint configurations, there may be no practical number of solver iterations that can stabilize the simulation. In these cases, you may need to instead use substepping or a shorter time step duration for the entire simulation. More frequent solver execution can massively improve simulation quality, allowing you to drop velocity iteration counts massively (even to just 1 per substep). See the [`SubsteppingDemo`](../Demos/SubsteppingDemo.cs) for an example of substepping in action, and the [Substepping documentation](Substepping.md) for more details. \ No newline at end of file diff --git a/Documentation/StabilityTips.md b/Documentation/StabilityTips.md index 4bfc21de5..f83460d57 100644 --- a/Documentation/StabilityTips.md +++ b/Documentation/StabilityTips.md @@ -31,7 +31,7 @@ Using higher update rates can enable the simulation of otherwise impossible mass ![](images/massratiosubstepping.gif) -For more examples of substepping, check out the [SubsteppingDemo](../Demos/Demos/SubsteppingDemo.cs). +For more examples of substepping, check out the [SubsteppingDemo](../Demos/Demos/SubsteppingDemo.cs). For more information about substepping, see the [substepping documentation](Substepping.md). So, if you're encountering constraint instability, here are some general guidelines for debugging: 1. First, try increasing the `Simulation.Timestep` update rate to a really high value (600hz or more). If the problem goes away, then it's probably related to the difficulty of the simulation and a lack of convergence and not to a configuration error. diff --git a/Documentation/Substepping.md b/Documentation/Substepping.md index e4ad6ef65..f5a32bdec 100644 --- a/Documentation/Substepping.md +++ b/Documentation/Substepping.md @@ -8,7 +8,7 @@ var simulation = Simulation.Create( new SolveDescription(velocityIterationCount: 1, substepCount: 8)); ``` -See the [SubsteppingDemo](../Demos/Demos/SubsteppingDemo.cs) for an interactive example. The [RopeTwistDemo](../Demos/Demos/RopeTwistDemo.cs), [ChainFountainDemo](../Demos/Demos/ChainFountainDemo.cs) and [BouncinessDemo](../Demos/Demos/BouncinessDemo.cs) also all use substepping. +See the [SubsteppingDemo](../Demos/Demos/SubsteppingDemo.cs) for an interactive example. The [RopeTwistDemo](../Demos/Demos/RopeTwistDemo.cs), [ChainFountainDemo](../Demos/Demos/ChainFountainDemo.cs) and [BouncinessDemo](../Demos/Demos/BouncinessDemo.cs) also all use substepping. The [stability tips documentation](StabilityTips.md) contains some more information about tuning. # Why use it? It makes difficult constraint configurations easy for the solver. The easier things are for the solver, the faster it can go. From 504fdd37df541caf65bfd2bf748e92d49149d234 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 1 Feb 2022 18:57:51 -0600 Subject: [PATCH 416/947] Improved numerical behavior of sin/cos and reshuffled contact subconstraint execution order. Substepping drift mitigated. --- .../Constraints/Contact/ContactConvexTypes.cs | 126 +++++++------- .../Constraints/Contact/ContactConvexTypes.tt | 17 +- BepuPhysics/PoseIntegrator.cs | 4 +- BepuUtilities/MathHelper.cs | 161 ++++++++++-------- Demos/Demos/SubsteppingDemo.cs | 4 +- Demos/Program.cs | 11 +- 6 files changed, 174 insertions(+), 149 deletions(-) diff --git a/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs b/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs index daef19398..764325799 100644 --- a/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs +++ b/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs @@ -310,15 +310,16 @@ public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref Contact1OneBodyPrestepData prestep, ref Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { - Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); - var maximumTangentImpulse = prestep.MaterialProperties.FrictionCoefficient * (accumulatedImpulses.Penetration0); - TangentFrictionOneBody.Solve(x, z, prestep.Contact0.OffsetA, inertiaA, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); - //Note that we solve the penetration constraints after the friction constraints. - //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. - //It's a pretty minor effect either way. + //Note that we solve the penetration constraints before the friction constraints. + //This makes the friction constraints more authoritative, since they happen last. + //It's a pretty minor effect either way, but penetration constraints have error correction feedback- penetration depth. + //Friction is velocity only and has no error correction, so introducing error there might cause drift. SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); var inverseDtWide = new Vector(inverseDt); PenetrationLimitOneBody.Solve(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + var maximumTangentImpulse = prestep.MaterialProperties.FrictionCoefficient * (accumulatedImpulses.Penetration0); + TangentFrictionOneBody.Solve(x, z, prestep.Contact0.OffsetA, inertiaA, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); //If there's only one contact, then the contact patch as determined by contact distance would be zero. //That can cause some subtle behavioral issues sometimes, so we approximate lever arm with the contact depth, assuming that the contact surface area will increase as the depth increases. var maximumTwistImpulse = prestep.MaterialProperties.FrictionCoefficient * accumulatedImpulses.Penetration0 * Vector.Max(Vector.Zero, prestep.Contact0.Depth); @@ -460,18 +461,19 @@ public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref Contact2OneBodyPrestepData prestep, ref Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { + //Note that we solve the penetration constraints before the friction constraints. + //This makes the friction constraints more authoritative, since they happen last. + //It's a pretty minor effect either way, but penetration constraints have error correction feedback- penetration depth. + //Friction is velocity only and has no error correction, so introducing error there might cause drift. + SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); + var inverseDtWide = new Vector(inverseDt); + PenetrationLimitOneBody.Solve(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); + PenetrationLimitOneBody.Solve(inertiaA, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA); Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); var premultipliedFrictionCoefficient = new Vector(1f / 2f) * prestep.MaterialProperties.FrictionCoefficient; var maximumTangentImpulse = premultipliedFrictionCoefficient * (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1); FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, out var offsetToManifoldCenterA); TangentFrictionOneBody.Solve(x, z, offsetToManifoldCenterA, inertiaA, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); - //Note that we solve the penetration constraints after the friction constraints. - //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. - //It's a pretty minor effect either way. - SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); - var inverseDtWide = new Vector(inverseDt); - PenetrationLimitOneBody.Solve(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); - PenetrationLimitOneBody.Solve(inertiaA, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA); var maximumTwistImpulse = premultipliedFrictionCoefficient * ( accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact0.OffsetA) + accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact1.OffsetA)); @@ -621,19 +623,20 @@ public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref Contact3OneBodyPrestepData prestep, ref Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { - Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); - var premultipliedFrictionCoefficient = new Vector(1f / 3f) * prestep.MaterialProperties.FrictionCoefficient; - var maximumTangentImpulse = premultipliedFrictionCoefficient * (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2); - FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, out var offsetToManifoldCenterA); - TangentFrictionOneBody.Solve(x, z, offsetToManifoldCenterA, inertiaA, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); - //Note that we solve the penetration constraints after the friction constraints. - //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. - //It's a pretty minor effect either way. + //Note that we solve the penetration constraints before the friction constraints. + //This makes the friction constraints more authoritative, since they happen last. + //It's a pretty minor effect either way, but penetration constraints have error correction feedback- penetration depth. + //Friction is velocity only and has no error correction, so introducing error there might cause drift. SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); var inverseDtWide = new Vector(inverseDt); PenetrationLimitOneBody.Solve(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); PenetrationLimitOneBody.Solve(inertiaA, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA); PenetrationLimitOneBody.Solve(inertiaA, prestep.Normal, prestep.Contact2.OffsetA, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA); + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + var premultipliedFrictionCoefficient = new Vector(1f / 3f) * prestep.MaterialProperties.FrictionCoefficient; + var maximumTangentImpulse = premultipliedFrictionCoefficient * (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2); + FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, out var offsetToManifoldCenterA); + TangentFrictionOneBody.Solve(x, z, offsetToManifoldCenterA, inertiaA, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); var maximumTwistImpulse = premultipliedFrictionCoefficient * ( accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact0.OffsetA) + accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact1.OffsetA) + @@ -792,20 +795,21 @@ public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref Contact4OneBodyPrestepData prestep, ref Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { - Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); - var premultipliedFrictionCoefficient = new Vector(1f / 4f) * prestep.MaterialProperties.FrictionCoefficient; - var maximumTangentImpulse = premultipliedFrictionCoefficient * (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2 + accumulatedImpulses.Penetration3); - FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact3.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, prestep.Contact3.Depth, out var offsetToManifoldCenterA); - TangentFrictionOneBody.Solve(x, z, offsetToManifoldCenterA, inertiaA, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); - //Note that we solve the penetration constraints after the friction constraints. - //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. - //It's a pretty minor effect either way. + //Note that we solve the penetration constraints before the friction constraints. + //This makes the friction constraints more authoritative, since they happen last. + //It's a pretty minor effect either way, but penetration constraints have error correction feedback- penetration depth. + //Friction is velocity only and has no error correction, so introducing error there might cause drift. SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); var inverseDtWide = new Vector(inverseDt); PenetrationLimitOneBody.Solve(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); PenetrationLimitOneBody.Solve(inertiaA, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA); PenetrationLimitOneBody.Solve(inertiaA, prestep.Normal, prestep.Contact2.OffsetA, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA); PenetrationLimitOneBody.Solve(inertiaA, prestep.Normal, prestep.Contact3.OffsetA, prestep.Contact3.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration3, ref wsvA); + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + var premultipliedFrictionCoefficient = new Vector(1f / 4f) * prestep.MaterialProperties.FrictionCoefficient; + var maximumTangentImpulse = premultipliedFrictionCoefficient * (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2 + accumulatedImpulses.Penetration3); + FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact3.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, prestep.Contact3.Depth, out var offsetToManifoldCenterA); + TangentFrictionOneBody.Solve(x, z, offsetToManifoldCenterA, inertiaA, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); var maximumTwistImpulse = premultipliedFrictionCoefficient * ( accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact0.OffsetA) + accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact1.OffsetA) + @@ -952,16 +956,17 @@ public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref Contact1PrestepData prestep, ref Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { + //Note that we solve the penetration constraints before the friction constraints. + //This makes the friction constraints more authoritative, since they happen last. + //It's a pretty minor effect either way, but penetration constraints have error correction feedback- penetration depth. + //Friction is velocity only and has no error correction, so introducing error there might cause drift. + SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); + var inverseDtWide = new Vector(inverseDt); + PenetrationLimit.Solve(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); var maximumTangentImpulse = prestep.MaterialProperties.FrictionCoefficient * (accumulatedImpulses.Penetration0); Vector3Wide.Subtract(prestep.Contact0.OffsetA, prestep.OffsetB, out var offsetToManifoldCenterB); TangentFriction.Solve(x, z, prestep.Contact0.OffsetA, offsetToManifoldCenterB, inertiaA, inertiaB, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); - //Note that we solve the penetration constraints after the friction constraints. - //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. - //It's a pretty minor effect either way. - SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); - var inverseDtWide = new Vector(inverseDt); - PenetrationLimit.Solve(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); //If there's only one contact, then the contact patch as determined by contact distance would be zero. //That can cause some subtle behavioral issues sometimes, so we approximate lever arm with the contact depth, assuming that the contact surface area will increase as the depth increases. var maximumTwistImpulse = prestep.MaterialProperties.FrictionCoefficient * accumulatedImpulses.Penetration0 * Vector.Max(Vector.Zero, prestep.Contact0.Depth); @@ -1115,19 +1120,20 @@ public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref Contact2PrestepData prestep, ref Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { + //Note that we solve the penetration constraints before the friction constraints. + //This makes the friction constraints more authoritative, since they happen last. + //It's a pretty minor effect either way, but penetration constraints have error correction feedback- penetration depth. + //Friction is velocity only and has no error correction, so introducing error there might cause drift. + SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); + var inverseDtWide = new Vector(inverseDt); + PenetrationLimit.Solve(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + PenetrationLimit.Solve(inertiaA, inertiaB, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.OffsetA - prestep.OffsetB, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); var premultipliedFrictionCoefficient = new Vector(1f / 2f) * prestep.MaterialProperties.FrictionCoefficient; var maximumTangentImpulse = premultipliedFrictionCoefficient * (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1); FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, out var offsetToManifoldCenterA); Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); TangentFriction.Solve(x, z, offsetToManifoldCenterA, offsetToManifoldCenterB, inertiaA, inertiaB, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); - //Note that we solve the penetration constraints after the friction constraints. - //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. - //It's a pretty minor effect either way. - SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); - var inverseDtWide = new Vector(inverseDt); - PenetrationLimit.Solve(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - PenetrationLimit.Solve(inertiaA, inertiaB, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.OffsetA - prestep.OffsetB, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); var maximumTwistImpulse = premultipliedFrictionCoefficient * ( accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact0.OffsetA) + accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact1.OffsetA)); @@ -1289,20 +1295,21 @@ public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref Contact3PrestepData prestep, ref Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { + //Note that we solve the penetration constraints before the friction constraints. + //This makes the friction constraints more authoritative, since they happen last. + //It's a pretty minor effect either way, but penetration constraints have error correction feedback- penetration depth. + //Friction is velocity only and has no error correction, so introducing error there might cause drift. + SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); + var inverseDtWide = new Vector(inverseDt); + PenetrationLimit.Solve(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); + PenetrationLimit.Solve(inertiaA, inertiaB, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.OffsetA - prestep.OffsetB, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); + PenetrationLimit.Solve(inertiaA, inertiaB, prestep.Normal, prestep.Contact2.OffsetA, prestep.Contact2.OffsetA - prestep.OffsetB, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); var premultipliedFrictionCoefficient = new Vector(1f / 3f) * prestep.MaterialProperties.FrictionCoefficient; var maximumTangentImpulse = premultipliedFrictionCoefficient * (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2); FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, out var offsetToManifoldCenterA); Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); TangentFriction.Solve(x, z, offsetToManifoldCenterA, offsetToManifoldCenterB, inertiaA, inertiaB, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); - //Note that we solve the penetration constraints after the friction constraints. - //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. - //It's a pretty minor effect either way. - SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); - var inverseDtWide = new Vector(inverseDt); - PenetrationLimit.Solve(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); - PenetrationLimit.Solve(inertiaA, inertiaB, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.OffsetA - prestep.OffsetB, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); - PenetrationLimit.Solve(inertiaA, inertiaB, prestep.Normal, prestep.Contact2.OffsetA, prestep.Contact2.OffsetA - prestep.OffsetB, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); var maximumTwistImpulse = premultipliedFrictionCoefficient * ( accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact0.OffsetA) + accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact1.OffsetA) + @@ -1473,21 +1480,22 @@ public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref Contact4PrestepData prestep, ref Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); - var premultipliedFrictionCoefficient = new Vector(1f / 4f) * prestep.MaterialProperties.FrictionCoefficient; - var maximumTangentImpulse = premultipliedFrictionCoefficient * (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2 + accumulatedImpulses.Penetration3); - FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact3.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, prestep.Contact3.Depth, out var offsetToManifoldCenterA); - Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); - TangentFriction.Solve(x, z, offsetToManifoldCenterA, offsetToManifoldCenterB, inertiaA, inertiaB, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); - //Note that we solve the penetration constraints after the friction constraints. - //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. - //It's a pretty minor effect either way. + //Note that we solve the penetration constraints before the friction constraints. + //This makes the friction constraints more authoritative, since they happen last. + //It's a pretty minor effect either way, but penetration constraints have error correction feedback- penetration depth. + //Friction is velocity only and has no error correction, so introducing error there might cause drift. SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); var inverseDtWide = new Vector(inverseDt); PenetrationLimit.Solve(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); PenetrationLimit.Solve(inertiaA, inertiaB, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.OffsetA - prestep.OffsetB, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); PenetrationLimit.Solve(inertiaA, inertiaB, prestep.Normal, prestep.Contact2.OffsetA, prestep.Contact2.OffsetA - prestep.OffsetB, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); PenetrationLimit.Solve(inertiaA, inertiaB, prestep.Normal, prestep.Contact3.OffsetA, prestep.Contact3.OffsetA - prestep.OffsetB, prestep.Contact3.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration3, ref wsvA, ref wsvB); + Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); + var premultipliedFrictionCoefficient = new Vector(1f / 4f) * prestep.MaterialProperties.FrictionCoefficient; + var maximumTangentImpulse = premultipliedFrictionCoefficient * (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2 + accumulatedImpulses.Penetration3); + FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact3.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, prestep.Contact3.Depth, out var offsetToManifoldCenterA); + Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); + TangentFriction.Solve(x, z, offsetToManifoldCenterA, offsetToManifoldCenterB, inertiaA, inertiaB, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); var maximumTwistImpulse = premultipliedFrictionCoefficient * ( accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact0.OffsetA) + accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact1.OffsetA) + diff --git a/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt b/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt index bb75d103a..fd86fbfcb 100644 --- a/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt +++ b/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt @@ -250,6 +250,15 @@ for (int i = 0; i < contactCount; ++i) [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(bodyCount == 2) {#>in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>float dt, float inverseDt, ref Contact<#=contactCount#><#=suffix#>PrestepData prestep, ref Contact<#=contactCount#>AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA<#if(bodyCount == 2) {#>, ref BodyVelocityWide wsvB<#}#>) { + //Note that we solve the penetration constraints before the friction constraints. + //This makes the friction constraints more authoritative, since they happen last. + //It's a pretty minor effect either way, but penetration constraints have error correction feedback- penetration depth. + //Friction is velocity only and has no error correction, so introducing error there might cause drift. + SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); + var inverseDtWide = new Vector(inverseDt); +<#for (int i = 0; i < contactCount; ++i) {#> + PenetrationLimit<#=suffix#>.Solve(inertiaA<#=bodyCount == 2 ? ", inertiaB" : ""#>, prestep.Normal, prestep.Contact<#=i#>.OffsetA, <#if(bodyCount == 2) {#>prestep.Contact<#=i#>.OffsetA - prestep.OffsetB, <#}#>prestep.Contact<#=i#>.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration<#=i#>, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); +<#}#> Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); <#if (contactCount > 1) {#> var premultipliedFrictionCoefficient = new Vector(1f / <#=contactCount#>f) * prestep.MaterialProperties.FrictionCoefficient; @@ -264,14 +273,6 @@ for (int i = 0; i < contactCount; ++i) Vector3Wide.Subtract(prestep.Contact0.OffsetA, prestep.OffsetB, out var offsetToManifoldCenterB); <#}#> TangentFriction<#=suffix#>.Solve(x, z, <#=contactCount == 1 ? "prestep.Contact0.OffsetA" : "offsetToManifoldCenterA"#><#=bodyCount == 2 ? ", offsetToManifoldCenterB" : ""#>, inertiaA<#=bodyCount == 2 ? ", inertiaB" : ""#>, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); - //Note that we solve the penetration constraints after the friction constraints. - //This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence. - //It's a pretty minor effect either way. - SpringSettingsWide.ComputeSpringiness(prestep.MaterialProperties.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); - var inverseDtWide = new Vector(inverseDt); -<#for (int i = 0; i < contactCount; ++i) {#> - PenetrationLimit<#=suffix#>.Solve(inertiaA<#=bodyCount == 2 ? ", inertiaB" : ""#>, prestep.Normal, prestep.Contact<#=i#>.OffsetA, <#if(bodyCount == 2) {#>prestep.Contact<#=i#>.OffsetA - prestep.OffsetB, <#}#>prestep.Contact<#=i#>.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration<#=i#>, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); -<#}#> <#if (contactCount == 1) {#> //If there's only one contact, then the contact patch as determined by contact distance would be zero. //That can cause some subtle behavioral issues sometimes, so we approximate lever arm with the contact depth, assuming that the contact surface area will increase as the depth increases. diff --git a/BepuPhysics/PoseIntegrator.cs b/BepuPhysics/PoseIntegrator.cs index 0e25c17cd..6e809b46d 100644 --- a/BepuPhysics/PoseIntegrator.cs +++ b/BepuPhysics/PoseIntegrator.cs @@ -149,12 +149,12 @@ public static void Integrate(in QuaternionWide start, in Vector3Wide angularVelo Vector3Wide.Length(angularVelocity, out var speed); var halfAngle = speed * halfDt; QuaternionWide q; - MathHelper.Sin(halfAngle, out var s); + var s = MathHelper.Sin(halfAngle); var scale = s / speed; q.X = angularVelocity.X * scale; q.Y = angularVelocity.Y * scale; q.Z = angularVelocity.Z * scale; - MathHelper.Cos(halfAngle, out q.W); + q.W = MathHelper.Cos(halfAngle); QuaternionWide.ConcatenateWithoutOverlap(start, q, out var end); end = QuaternionWide.Normalize(end); var speedValid = Vector.GreaterThan(speed, new Vector(1e-15f)); diff --git a/BepuUtilities/MathHelper.cs b/BepuUtilities/MathHelper.cs index 22fcc425d..ad19843a3 100644 --- a/BepuUtilities/MathHelper.cs +++ b/BepuUtilities/MathHelper.cs @@ -195,119 +195,142 @@ public static float BinarySign(float x) //Note that these cos/sin implementations are not here for performance, but rather to: //1) Provide a SIMD accelerated version for wide processing, and //2) Provide a scalar implementation that is consistent with the SIMD version for systems which need to match its behavior. - //The main motivating use case is the pose integrator (which is scalar) and the sweep tests (which are widely vectorized). /// - /// Computes an approximation of cosine. Maximum error a little above 3e-6. + /// Computes an approximation of cosine. Maximum error a little below 8e-7 for the interval -2 * Pi to 2 * Pi. Values further from the interval near zero have gracefully degrading error. /// /// Value to take the cosine of. /// Approximate cosine of the input value. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static float Cos(float x) { - //This exists primarily for consistency between the PoseIntegrator and sweeps, not necessarily for raw performance relative to Math.Cos. - if (x < 0) - x = -x; - var intervalIndex = x * (1f / TwoPi); - x -= (int)intervalIndex * TwoPi; + //Rational approximation over [0, pi/2], use symmetry for the rest. + var periodCount = x * (float)(0.5 / Math.PI); + var periodFraction = periodCount - MathF.Floor(periodCount); //This is a source of error as you get away from 0. + var periodX = periodFraction * TwoPi; //[0, pi/2] = f(x) //(pi/2, pi] = -f(Pi - x) //(pi, 3 * pi / 2] = -f(x - Pi) //(3*pi/2, 2*pi] = f(2 * Pi - x) - //This could be done more cleverly. - bool negate; - if (x < Pi) - { - if (x < PiOver2) - { - negate = false; - } - else - { - x = Pi - x; - negate = true; - } - } - else - { - if (x < 3 * PiOver2) - { - x = x - Pi; - negate = true; - } - else - { - x = TwoPi - x; - negate = false; - } - } + float y = periodX > 3 * PiOver2 ? TwoPi - periodX : periodX > Pi ? periodX - Pi : periodX > PiOver2 ? Pi - periodX : periodX; - //The expression is a rational interpolation from 0 to Pi/2. Maximum error is a little more than 3e-6. - var x2 = x * x; - var x3 = x2 * x; - //TODO: This could be reorganized into two streams of FMAs if that was available. - var numerator = 1 - 0.24f * x - 0.4266f * x2 + 0.110838f * x3; - var denominator = 1 - 0.240082f * x + 0.0741637f * x2 - 0.0118786f * x3; + //Using a fifth degree numerator and denominator. + //This will be precise beyond a single's useful representation most of the time, but we're not *that* worried about performance here. + //TODO: FMA could help here, primarily in terms of precision. + var numerator = ((((-0.003436308368583229f * y + 0.021317031205957775f) * y + 0.06955843390178032f) * y - 0.4578088075324152f) * y - 0.15082367674208508f) * y + 1f; + var denominator = ((((-0.00007650398834677185f * y + 0.0007451378206294365f) * y - 0.00585321045829395f) * y + 0.04219116713777847f) * y - 0.15082367538305258f) * y + 1f; var result = numerator / denominator; - return negate ? -result : result; + return periodX > PiOver2 && periodX < 3 * PiOver2 ? -result : result; } /// - /// Computes an approximation of sine. Maximum error a little above 3e-6. + /// Computes an approximation of sine. Maximum error a little below 5e-7 for the interval -2 * Pi to 2 * Pi. Values further from the interval near zero have gracefully degrading error. /// /// Value to take the sine of. /// Approximate sine of the input value. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static float Sin(float x) { - return Cos(x - PiOver2); + //Similar to cos, use a rational approximation for the region of sin from [0, pi/2]. Use symmetry to cover the rest. + //This has its own implementation rather than just calling into Cos because we want maximum fidelity near 0. + var periodCount = x * (float)(0.5 / Math.PI); + var periodFraction = periodCount - MathF.Floor(periodCount); //This is a source of error as you get away from 0. + var periodX = periodFraction * TwoPi; + + //[0, pi/2] = f(x) + //(pi/2, pi] = f(pi - x) + //(pi, 3/2 * pi] = -f(x - pi) + //(3/2 * pi, 2*pi] = -f(2 * pi - x) + float y = periodX > 3 * PiOver2 ? TwoPi - periodX : periodX > Pi ? periodX - Pi : periodX > PiOver2 ? Pi - periodX : periodX; + + //Using a fifth degree numerator and denominator. + //This will be precise beyond a single's useful representation most of the time, but we're not *that* worried about performance here. + //TODO: FMA could help here, primarily in terms of precision. + var numerator = ((((0.0040507708755727605f * y - 0.006685815219853882f) * y - 0.13993701695343166f) * y + 0.06174562337697123f) * y + 1.00000000151466040f) * y; + var denominator = ((((0.00009018370615921334f * y + 0.0001700784176413186f) * y + 0.003606014457152456f) * y + 0.02672943625500751f) * y + 0.061745651499203795f) * y + 1f; + var result = numerator / denominator; + return periodX > Pi ? -result : result; } /// - /// Computes an approximation of cosine. Maximum error a little above 3e-6. + /// Computes an approximation of cosine. Maximum error a little below 8e-7 for the interval -2 * Pi to 2 * Pi. Values further from the interval near zero have gracefully degrading error. /// /// Values to take the cosine of. /// Approximate cosine of the input values. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Cos(in Vector x, out Vector result) + public static Vector Cos(Vector x) { - //This exists primarily for consistency between the PoseIntegrator and sweeps, not necessarily for raw performance relative to Math.Cos. - var periodX = Vector.Abs(x); - //TODO: No floor or truncate available... may want to revisit later. - periodX = periodX - TwoPi * Vector.ConvertToSingle(Vector.ConvertToInt32(periodX * (1f / TwoPi))); + //Rational approximation over [0, pi/2], use symmetry for the rest. + var periodCount = x * (float)(0.5 / Math.PI); + var periodFraction = periodCount - Vector.Floor(periodCount); //This is a source of error as you get away from 0. + var twoPi = new Vector(TwoPi); + var periodX = periodFraction * twoPi; //[0, pi/2] = f(x) //(pi/2, pi] = -f(Pi - x) //(pi, 3 * pi / 2] = -f(x - Pi) //(3*pi/2, 2*pi] = f(2 * Pi - x) - //This could be done more cleverly. Vector y; - y = Vector.ConditionalSelect(Vector.GreaterThan(periodX, new Vector(PiOver2)), new Vector(Pi) - periodX, periodX); - y = Vector.ConditionalSelect(Vector.GreaterThan(periodX, new Vector(Pi)), new Vector(-Pi) + periodX, y); - y = Vector.ConditionalSelect(Vector.GreaterThan(periodX, new Vector(3 * PiOver2)), new Vector(TwoPi) - periodX, y); - - //The expression is a rational interpolation from 0 to Pi/2. Maximum error is a little more than 3e-6. - var y2 = y * y; - var y3 = y2 * y; - //TODO: This could be reorganized into two streams of FMAs if that was available. - var numerator = Vector.One - 0.24f * y - 0.4266f * y2 + 0.110838f * y3; - var denominator = Vector.One - 0.240082f * y + 0.0741637f * y2 - 0.0118786f * y3; - result = numerator / denominator; - result = Vector.ConditionalSelect( - Vector.BitwiseAnd( - Vector.GreaterThan(periodX, new Vector(PiOver2)), - Vector.LessThan(periodX, new Vector(3 * PiOver2))), -result, result); + var piOver2 = new Vector(PiOver2); + var pi = new Vector(Pi); + var pi3Over2 = new Vector(3 * PiOver2); + y = Vector.ConditionalSelect(Vector.GreaterThan(periodX, piOver2), pi - periodX, periodX); + y = Vector.ConditionalSelect(Vector.GreaterThan(periodX, pi), periodX - pi, y); + y = Vector.ConditionalSelect(Vector.GreaterThan(periodX, pi3Over2), new Vector(TwoPi) - periodX, y); + + //Using a fifth degree numerator and denominator. + //This will be precise beyond a single's useful representation most of the time, but we're not *that* worried about performance here. + //TODO: FMA could help here, primarily in terms of precision. + //var y2 = y * y; + //var y3 = y2 * y; + //var y4 = y2 * y2; + //var y5 = y3 * y2; + //var numerator = Vector.One - 0.15082367674208508f * y - 0.4578088075324152f * y2 + 0.06955843390178032f * y3 + 0.021317031205957775f * y4 - 0.003436308368583229f * y5; + //var denominator = Vector.One - 0.15082367538305258f * y + 0.04219116713777847f * y2 - 0.00585321045829395f * y3 + 0.0007451378206294365f * y4 - 0.00007650398834677185f * y5; + var numerator = ((((new Vector(-0.003436308368583229f) * y + new Vector(0.021317031205957775f)) * y + new Vector(0.06955843390178032f)) * y - new Vector(0.4578088075324152f)) * y - new Vector(0.15082367674208508f)) * y + Vector.One; + var denominator = ((((new Vector(-0.00007650398834677185f) * y + new Vector(0.0007451378206294365f)) * y - new Vector(0.00585321045829395f)) * y + new Vector(0.04219116713777847f)) * y - new Vector(0.15082367538305258f)) * y + Vector.One; + var result = numerator / denominator; + return Vector.ConditionalSelect(Vector.BitwiseAnd(Vector.GreaterThan(periodX, piOver2), Vector.LessThan(periodX, pi3Over2)), -result, result); } /// - /// Computes an approximation of sine. Maximum error a little above 3e-6. + /// Computes an approximation of sine. Maximum error a little below 5e-7 for the interval -2 * Pi to 2 * Pi. Values further from the interval near zero have gracefully degrading error. /// /// Value to take the sine of. /// Approximate sine of the input value. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Sin(in Vector x, out Vector result) + public static Vector Sin(Vector x) { - Cos(x - new Vector(PiOver2), out result); + //Similar to cos, use a rational approximation for the region of sin from [0, pi/2]. Use symmetry to cover the rest. + //This has its own implementation rather than just calling into Cos because we want maximum fidelity near 0. + var periodCount = x * (float)(0.5 / Math.PI); + var periodFraction = periodCount - Vector.Floor(periodCount); //This is a source of error as you get away from 0. + var twoPi = new Vector(TwoPi); + var periodX = periodFraction * twoPi; + //[0, pi/2] = f(x) + //(pi/2, pi] = f(pi - x) + //(pi, 3/2 * pi] = -f(x - pi) + //(3/2 * pi, 2*pi] = -f(2 * pi - x) + Vector y; + var pi = new Vector(Pi); + var piOver2 = new Vector(PiOver2); + y = Vector.ConditionalSelect(Vector.GreaterThan(periodX, piOver2), pi - periodX, periodX); + var inSecondHalf = Vector.GreaterThan(periodX, pi); + y = Vector.ConditionalSelect(inSecondHalf, periodX - pi, y); + y = Vector.ConditionalSelect(Vector.GreaterThan(periodX, new Vector(3 * PiOver2)), twoPi - periodX, y); + + //Using a fifth degree numerator and denominator. + //This will be precise beyond a single's useful representation most of the time, but we're not *that* worried about performance here. + //TODO: FMA could help here, primarily in terms of precision. + //var y2 = y * y; + //var y3 = y2 * y; + //var y4 = y2 * y2; + //var y5 = y3 * y2; + //var numerator = 1.0000000015146604f * y + 0.06174562337697123f * y2 - 0.13993701695343166f * y3 - 0.006685815219853882f * y4 + 0.0040507708755727605f * y5; + //var denominator = Vector.One + 0.061745651499203795f * y + 0.02672943625500751f * y2 + 0.003606014457152456f * y3 + 0.0001700784176413186f * y4 + 0.00009018370615921334f * y5; + var numerator = ((((0.0040507708755727605f * y - new Vector(0.006685815219853882f)) * y - new Vector(0.13993701695343166f)) * y + new Vector(0.06174562337697123f)) * y + new Vector(1.00000000151466040f)) * y; + var denominator = ((((new Vector(0.00009018370615921334f) * y + new Vector(0.0001700784176413186f)) * y + new Vector(0.003606014457152456f)) * y + new Vector(0.02672943625500751f)) * y + new Vector(0.061745651499203795f)) * y + Vector.One; + var result = numerator / denominator; + return Vector.ConditionalSelect(inSecondHalf, -result, result); } diff --git a/Demos/Demos/SubsteppingDemo.cs b/Demos/Demos/SubsteppingDemo.cs index d7d149405..c2c96b766 100644 --- a/Demos/Demos/SubsteppingDemo.cs +++ b/Demos/Demos/SubsteppingDemo.cs @@ -136,7 +136,7 @@ public override void Update(Window window, Camera camera, Input input, float dt) } if (input.WasPushed(OpenTK.Input.Key.X)) { - Simulation.Solver.SubstepCount = Math.Min(8192, Simulation.Solver.SubstepCount + substepCountChange); + Simulation.Solver.SubstepCount = Math.Min(512, Simulation.Solver.SubstepCount + substepCountChange); AwakenAllBodies(); } if (input.WasPushed(OpenTK.Input.Key.C)) @@ -146,7 +146,7 @@ public override void Update(Window window, Camera camera, Input input, float dt) } if (input.WasPushed(OpenTK.Input.Key.V)) { - Simulation.Solver.VelocityIterationCount = Math.Min(8192, Simulation.Solver.VelocityIterationCount + iterationCountChange); + Simulation.Solver.VelocityIterationCount = Math.Min(512, Simulation.Solver.VelocityIterationCount + iterationCountChange); AwakenAllBodies(); } base.Update(window, camera, input, dt); diff --git a/Demos/Program.cs b/Demos/Program.cs index 27877d24c..4d5b7a5eb 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -1,20 +1,13 @@ using BepuUtilities; using DemoContentLoader; -using Demos.Demos; -using Demos.Demos.Cars; -using Demos.SpecializedTests; using DemoUtilities; using OpenTK; -using System; -using System.Diagnostics; -using System.Runtime.CompilerServices; -using System.Threading; namespace Demos { class Program - { - static void Main(string[] args) + { + static void Main() { var window = new Window("pretty cool multicolored window", new Int2((int)(DisplayDevice.Default.Width * 0.75f), (int)(DisplayDevice.Default.Height * 0.75f)), WindowMode.Windowed); From 188b29f30149c6c285f616a9115a4a4f0681d152 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 1 Feb 2022 20:49:01 -0600 Subject: [PATCH 417/947] Substepping documentation update for drift mitigation. --- Documentation/Substepping.md | 10 ++-------- 1 file changed, 2 insertions(+), 8 deletions(-) diff --git a/Documentation/Substepping.md b/Documentation/Substepping.md index f5a32bdec..86dfd77b9 100644 --- a/Documentation/Substepping.md +++ b/Documentation/Substepping.md @@ -68,7 +68,7 @@ Unfortunately, substepping isn't magic. The entire point is to avoid running oth This incremental update is usually fine, but out of date contacts can sometimes introduce energy. For example, an out of date contact lever arm can let a body 'fall' into another body ever so slightly, which over many substeps ends up sustaining oscillation. -You can see an example of this behavior [here](https://youtu.be/70IAdC-4Sa0). +You can see an example of this behavior [here](https://youtu.be/qMX1ZLmfrEo). To mitigate this issue, you can try: 1. damping the relevant bodies more heavily in the integrator, @@ -76,10 +76,4 @@ To mitigate this issue, you can try: 3. increasing the sleeping velocity threshold (`BodyActivityDescription.SleepThreshold` passed into the `BodyDescription`) for the relevant bodies such that they take a nap instead of wiggling, 4. increasing the inertia of the problematic bodies to increase the period of oscillation (possibly making it easier to mitigate with sleeping/damping) 5. avoiding shapes or situations that are likely to cause the problem, -6. or just don't use solver substepping. You can always resort to calling `Simulation.Timestep` more frequently. It'll cost more than solver-only substepping, but it'll keep all your contact data up to date, and the library's pretty dang fast anyway. - -Another far more subtle effect arises from accumulated numerical error. Even without using substepping, some slight numerical drift will occur on every frame. Even with enough velocity iterations to converge, there might still be a 1e-7 error in the relative velocity. Integrating those errors into the position over time causes drift. - -With sleeping enabled and reasonable simulation configuration, this is effectively invisible. However, disabling sleeping and using extreme substepping (such as effective solver rates in the tens or hundreds of thousands of hertz) can make it obvious. [See here for an example](https://youtu.be/0kkHebYnARs). - -[#167](https://github.com/bepu/bepuphysics2/issues/167) tracks one solution to this- friction with explicit position goals. Another option that can be implemented externally and would work for all constraint types (contact or not) is to quantize body positions. By forcing body positions onto a grid with spacing larger than the per-frame drift, the drift cannot accumulate. \ No newline at end of file +6. or just don't use solver substepping. You can always resort to calling `Simulation.Timestep` more frequently. It'll cost more than solver-only substepping, but it'll keep all your contact data up to date, and the library's pretty dang fast anyway. \ No newline at end of file From 6d9e46261b9a83bf24a0284c780d9b2f995aefcd Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 1 Feb 2022 21:04:08 -0600 Subject: [PATCH 418/947] Substepping link. --- README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README.md b/README.md index d2af1157d..f379cc16a 100644 --- a/README.md +++ b/README.md @@ -43,6 +43,8 @@ By user request, there's a [discord server](https://discord.gg/ssa2XpY). I'll be [Performance Tips](Documentation/PerformanceTips.md) +[Substepping](Documentation/Substepping.md) + [Contributing](CONTRIBUTING.md) [Upgrading from v1, concept mapping](Documentation/UpgradingFromV1.md) From 3d9b4e76c93b07c0e3a343896528bbe5255860f6 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 2 Feb 2022 16:43:42 -0600 Subject: [PATCH 419/947] CollidableOverlapFinder now properly flushes all workers in the zero work corner case, fixing a memory leak. --- BepuPhysics/CollisionDetection/BroadPhase.cs | 4 ++-- .../CollisionDetection/CollidableOverlapFinder.cs | 10 +++++++--- BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs | 6 ++---- 3 files changed, 11 insertions(+), 9 deletions(-) diff --git a/BepuPhysics/CollisionDetection/BroadPhase.cs b/BepuPhysics/CollisionDetection/BroadPhase.cs index e50470d64..876ea5400 100644 --- a/BepuPhysics/CollisionDetection/BroadPhase.cs +++ b/BepuPhysics/CollisionDetection/BroadPhase.cs @@ -201,13 +201,13 @@ public void Update(IThreadDispatcher threadDispatcher = null) staticRefineContext.CreateRefitAndMarkJobs(ref StaticTree, Pool, threadDispatcher); remainingJobCount = activeRefineContext.RefitNodes.Count + staticRefineContext.RefitNodes.Count; threadDispatcher.DispatchWorkers(ExecuteRefitAndMark, remainingJobCount); - activeRefineContext.CreateRefinementJobs(Pool, threadDispatcher, frameIndex, 1f); + activeRefineContext.CreateRefinementJobs(Pool, frameIndex, 1f); //TODO: for now, the inactive/static tree is simply updated like another active tree. This is enormously inefficient compared to the ideal- //by nature, static and inactive objects do not move every frame! //However, the refinement system rarely generates enough work to fill modern beefy machine. Even a million objects might only be 16 refinement jobs. //To really get the benefit of incremental updates, the tree needs to be reworked to output finer grained work. //Since the jobs are large, reducing the refinement aggressiveness doesn't change much here. - staticRefineContext.CreateRefinementJobs(Pool, threadDispatcher, frameIndex, 1f); + staticRefineContext.CreateRefinementJobs(Pool, frameIndex, 1f); remainingJobCount = activeRefineContext.RefinementTargets.Count + staticRefineContext.RefinementTargets.Count; threadDispatcher.DispatchWorkers(ExecuteRefine, remainingJobCount); activeRefineContext.CleanUpForRefitAndRefine(Pool); diff --git a/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs b/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs index e7847f87d..bbf53d8f0 100644 --- a/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs +++ b/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs @@ -128,11 +128,15 @@ public override void DispatchOverlaps(float dt, IThreadDispatcher threadDispatch intertreeTestContext.PrepareJobs(ref broadPhase.ActiveTree, ref broadPhase.StaticTree, intertreeHandlers, threadDispatcher.ThreadCount); nextJobIndex = -1; var totalJobCount = selfTestContext.JobCount + intertreeTestContext.JobCount; - //We dispatch over parts of the tree are not yet analyzed, but the job creation phase may have put some work into the batcher. - //If the total job count is zero, that means there's no further work to be done (implying the tree was very tiny), but we may need to flush. if (totalJobCount == 0) { - narrowPhase.overlapWorkers[0].Batcher.Flush(); + //We dispatch over parts of the tree are not yet analyzed, but the job creation phase may have put some work into the batcher. + //If the total job count is zero, that means there's no further work to be done (implying the tree was very tiny), but we may need to flush. + //Flushing also disposes of any resources taken by the batcher. + for (int i = 0; i < threadDispatcher.ThreadCount; ++i) + { + narrowPhase.overlapWorkers[i].Batcher.Flush(); + } } else { diff --git a/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs b/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs index 4589bafcd..1f99c2b34 100644 --- a/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs +++ b/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs @@ -74,7 +74,7 @@ public unsafe void CreateRefitAndMarkJobs(ref Tree tree, BufferPool pool, IThrea RefitNodeIndex = -1; } - public unsafe void CreateRefinementJobs(BufferPool pool, IThreadDispatcher threadDispatcher, int frameIndex, float refineAggressivenessScale = 1) + public unsafe void CreateRefinementJobs(BufferPool pool, int frameIndex, float refineAggressivenessScale = 1) { if (Tree.leafCount <= 2) { @@ -131,8 +131,6 @@ public unsafe void CleanUpForRefitAndRefine(BufferPool pool) if (Tree.leafCount <= 2) { //If there are 2 or less leaves, then refit/refine doesn't do anything at all. - //(The root node has no parent, so it does not have a bounding box, and the SAH won't change no matter how we swap the children of the root.) - //Avoiding this case also gives the other codepath a guarantee that it will be working with nodes with two children. return; } //Note that we defer the refine flag clear until after the refinements complete. If we did it within the refine action itself, @@ -159,7 +157,7 @@ public unsafe void RefitAndRefine(ref Tree tree, BufferPool pool, IThreadDispatc { CreateRefitAndMarkJobs(ref tree, pool, threadDispatcher); threadDispatcher.DispatchWorkers(RefitAndMarkAction, RefitNodes.Count); - CreateRefinementJobs(pool, threadDispatcher, frameIndex, refineAggressivenessScale); + CreateRefinementJobs(pool, frameIndex, refineAggressivenessScale); threadDispatcher.DispatchWorkers(RefineAction, RefinementTargets.Count); CleanUpForRefitAndRefine(pool); } From 8c05bfdabe00b7df3a657223e2c152863487d136 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 2 Feb 2022 19:34:24 -0600 Subject: [PATCH 420/947] MathHelper.ApproximateAcos->MathHelper.Acos, with a couple of orders of magnitude improvement in precision. --- BepuPhysics/Constraints/AngularHinge.cs | 4 +- BepuPhysics/Constraints/AngularServo.cs | 2 +- .../Constraints/OneBodyAngularServo.cs | 2 +- BepuPhysics/Constraints/TwistServo.cs | 2 +- BepuPhysics/Constraints/Weld.cs | 2 +- BepuUtilities/MathHelper.cs | 64 ++++++++----------- BepuUtilities/QuaternionWide.cs | 8 +-- Demos/Demos/Tanks/TankDemo.cs | 4 +- Demos/Program.cs | 3 +- 9 files changed, 40 insertions(+), 51 deletions(-) diff --git a/BepuPhysics/Constraints/AngularHinge.cs b/BepuPhysics/Constraints/AngularHinge.cs index acf0a5c0d..50704d242 100644 --- a/BepuPhysics/Constraints/AngularHinge.cs +++ b/BepuPhysics/Constraints/AngularHinge.cs @@ -102,8 +102,8 @@ public static void GetErrorAngles(in Vector3Wide hingeAxisA, in Vector3Wide hing Vector3Wide.Dot(hingeAxisBOnPlaneX, hingeAxisA, out var hbxha); Vector3Wide.Dot(hingeAxisBOnPlaneY, hingeAxisA, out var hbyha); //We could probably get away with an acos approximation of something like (1 - x) * pi/2, but we'll do just a little more work: - MathHelper.ApproximateAcos(hbxha, out errorAngles.X); - MathHelper.ApproximateAcos(hbyha, out errorAngles.Y); + errorAngles.X = MathHelper.Acos(hbxha); + errorAngles.Y = MathHelper.Acos(hbyha); Vector3Wide.Dot(hingeAxisBOnPlaneX, jacobianA.Y, out var hbxay); Vector3Wide.Dot(hingeAxisBOnPlaneY, jacobianA.X, out var hbyax); errorAngles.X = Vector.ConditionalSelect(Vector.LessThan(hbxay, Vector.Zero), errorAngles.X, -errorAngles.X); diff --git a/BepuPhysics/Constraints/AngularServo.cs b/BepuPhysics/Constraints/AngularServo.cs index b6a968106..f1a64577d 100644 --- a/BepuPhysics/Constraints/AngularServo.cs +++ b/BepuPhysics/Constraints/AngularServo.cs @@ -109,7 +109,7 @@ public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in B QuaternionWide.Conjugate(targetOrientationB, out var inverseTarget); QuaternionWide.ConcatenateWithoutOverlap(inverseTarget, orientationB, out var errorRotation); - QuaternionWide.GetApproximateAxisAngleFromQuaternion(errorRotation, out var errorAxis, out var errorLength); + QuaternionWide.GetAxisAngleFromQuaternion(errorRotation, out var errorAxis, out var errorLength); SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); Symmetric3x3Wide.Add(inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, out var unsoftenedInverseEffectiveMass); diff --git a/BepuPhysics/Constraints/OneBodyAngularServo.cs b/BepuPhysics/Constraints/OneBodyAngularServo.cs index f2909f283..aca4df12d 100644 --- a/BepuPhysics/Constraints/OneBodyAngularServo.cs +++ b/BepuPhysics/Constraints/OneBodyAngularServo.cs @@ -86,7 +86,7 @@ public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in B //Jacobians are just the identity matrix. QuaternionWide.Conjugate(orientationA, out var inverseOrientation); QuaternionWide.ConcatenateWithoutOverlap(inverseOrientation, prestep.TargetOrientation, out var errorRotation); - QuaternionWide.GetApproximateAxisAngleFromQuaternion(errorRotation, out var errorAxis, out var errorLength); + QuaternionWide.GetAxisAngleFromQuaternion(errorRotation, out var errorAxis, out var errorLength); SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); Symmetric3x3Wide.Invert(inertiaA.InverseInertiaTensor, out var effectiveMass); diff --git a/BepuPhysics/Constraints/TwistServo.cs b/BepuPhysics/Constraints/TwistServo.cs index 4cac8d4ca..a3a0843de 100644 --- a/BepuPhysics/Constraints/TwistServo.cs +++ b/BepuPhysics/Constraints/TwistServo.cs @@ -124,7 +124,7 @@ public static void ComputeCurrentAngle(in Vector3Wide basisBX, in Vector3Wide ba QuaternionWide.TransformWithoutOverlap(basisBX, aligningRotation, out var alignedBasisBX); Vector3Wide.Dot(alignedBasisBX, basisA.X, out var x); Vector3Wide.Dot(alignedBasisBX, basisA.Y, out var y); - MathHelper.ApproximateAcos(x, out var absAngle); + var absAngle = MathHelper.Acos(x); angle = Vector.ConditionalSelect(Vector.LessThan(y, Vector.Zero), -absAngle, absAngle); } diff --git a/BepuPhysics/Constraints/Weld.cs b/BepuPhysics/Constraints/Weld.cs index 5c2e39454..bf7079205 100644 --- a/BepuPhysics/Constraints/Weld.cs +++ b/BepuPhysics/Constraints/Weld.cs @@ -158,7 +158,7 @@ public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in B var targetOrientationB = prestep.LocalOrientation * orientationA; //ConcatenateWithoutOverlap(prestep.LocalOrientation, orientationA, out var targetOrientationB); ConcatenateWithoutOverlap(Conjugate(targetOrientationB), orientationB, out var rotationError); - GetApproximateAxisAngleFromQuaternion(rotationError, out var rotationErrorAxis, out var rotationErrorLength); + GetAxisAngleFromQuaternion(rotationError, out var rotationErrorAxis, out var rotationErrorLength); SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); var orientationBiasVelocity = rotationErrorAxis * (rotationErrorLength * positionErrorToVelocity); diff --git a/BepuUtilities/MathHelper.cs b/BepuUtilities/MathHelper.cs index ad19843a3..293558cbc 100644 --- a/BepuUtilities/MathHelper.cs +++ b/BepuUtilities/MathHelper.cs @@ -201,7 +201,6 @@ public static float BinarySign(float x) /// /// Value to take the cosine of. /// Approximate cosine of the input value. - [MethodImpl(MethodImplOptions.AggressiveInlining)] public static float Cos(float x) { //Rational approximation over [0, pi/2], use symmetry for the rest. @@ -229,7 +228,6 @@ public static float Cos(float x) /// /// Value to take the sine of. /// Approximate sine of the input value. - [MethodImpl(MethodImplOptions.AggressiveInlining)] public static float Sin(float x) { //Similar to cos, use a rational approximation for the region of sin from [0, pi/2]. Use symmetry to cover the rest. @@ -253,12 +251,27 @@ public static float Sin(float x) return periodX > Pi ? -result : result; } + /// + /// Computes an approximation of arccos. Inputs outside of [-1, 1] are clamped. Maximum error less than 5.17e-07. + /// + /// Input value to the arccos function. + /// Result of the arccos function. + public static float Acos(float x) + { + var negativeInput = x < 0; + x = MathF.Min(1f, MathF.Abs(x)); + //Rational approximation (scaling sqrt(1-x)) over [0, 1], use symmetry for the rest. TODO: FMA would help with precision. + var numerator = MathF.Sqrt(1f - x) * (62.95741097600742f + x * (69.6550664543659f + x * (17.54512349463405f + x * 0.6022076120669532f))); + var denominator = 40.07993264439811f + x * (49.81949855726789f + x * (15.703851745284796f + x)); + var result = numerator / denominator; + return negativeInput ? Pi - result : result; + } + /// /// Computes an approximation of cosine. Maximum error a little below 8e-7 for the interval -2 * Pi to 2 * Pi. Values further from the interval near zero have gracefully degrading error. /// /// Values to take the cosine of. /// Approximate cosine of the input values. - [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Vector Cos(Vector x) { //Rational approximation over [0, pi/2], use symmetry for the rest. @@ -333,43 +346,20 @@ public static Vector Sin(Vector x) return Vector.ConditionalSelect(inSecondHalf, -result, result); } - /// - /// Computes an approximation of arccos. Maximum error less than 6.8e-5. + /// Computes an approximation of arccos. Inputs outside of [-1, 1] are clamped. Maximum error less than 5.17e-07. /// /// Input value to the arccos function. - /// Result of the arccos function. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ApproximateAcos(Vector x, out Vector acos) + /// Result of the arccos function. + public static Vector Acos(Vector x) { - //Adapted from Handbook of Mathematical Functions by Milton Abramowitz and Irene A. Stegun. - var negate = Vector.ConditionalSelect(Vector.LessThan(x, Vector.Zero), Vector.One, Vector.Zero); - x = Vector.Abs(x); - acos = new Vector(-0.0187293f) * x + new Vector(0.0742610f); - acos = (acos * x - new Vector(0.2121144f)) * x + new Vector(1.5707288f); - acos *= Vector.SquareRoot(Vector.Max(Vector.Zero, Vector.One - x)); - acos -= new Vector(2) * negate * acos; - acos = negate * new Vector(3.14159265358979f) + acos; - } - - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector Floor(Vector v) - { - if (Avx.IsSupported && Vector.Count == 8) - { - return Avx.Floor(v.AsVector256()).AsVector(); - } - else if (Sse41.IsSupported && Vector.Count == 4) - { - return Sse41.Floor(v.AsVector128()).AsVector(); - } - else - { - var intX = Vector.ConvertToInt32(v); - return Vector.ConvertToSingle(Vector.ConditionalSelect(Vector.LessThan(v, Vector.Zero), intX - Vector.One, intX)); - } - //TODO: Arm! + var negativeInput = Vector.LessThan(x, Vector.Zero); + x = Vector.Min(Vector.One, Vector.Abs(x)); + //Rational approximation (scaling sqrt(1-x)) over [0, 1], use symmetry for the rest. TODO: FMA would help with precision. + var numerator = Vector.SquareRoot(Vector.One - x) * (new Vector(62.95741097600742f) + x * (new Vector(69.6550664543659f) + x * (new Vector(17.54512349463405f) + x * 0.6022076120669532f))); + var denominator = new Vector(40.07993264439811f) + x * (new Vector(49.81949855726789f) + x * (new Vector(15.703851745284796f) + x)); + var result = numerator / denominator; + return Vector.ConditionalSelect(negativeInput, new Vector(Pi) - result, result); } /// @@ -383,7 +373,7 @@ public static void GetSignedAngleDifference(in Vector a, in Vector { var half = new Vector(0.5f); var x = (b - a) * new Vector(1f / TwoPi) + half; - difference = (x - Floor(x) - half) * new Vector(TwoPi); + difference = (x - Vector.Floor(x) - half) * new Vector(TwoPi); } diff --git a/BepuUtilities/QuaternionWide.cs b/BepuUtilities/QuaternionWide.cs index faeb4ad99..c286b3324 100644 --- a/BepuUtilities/QuaternionWide.cs +++ b/BepuUtilities/QuaternionWide.cs @@ -219,13 +219,13 @@ public static QuaternionWide GetQuaternionBetweenNormalizedVectors(Vector3Wide v } /// - /// Gets an axis and angle representation of the rotation stored in a quaternion. Angle is approximated. + /// Gets an axis and angle representation of the rotation stored in a quaternion. /// /// Quaternion to extract an axis-angle representation from. /// Axis of rotation extracted from the quaternion. - /// Approximated angle of rotation extracted from the quaternion. + /// Angle of rotation extracted from the quaternion. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void GetApproximateAxisAngleFromQuaternion(in QuaternionWide q, out Vector3Wide axis, out Vector angle) + public static void GetAxisAngleFromQuaternion(in QuaternionWide q, out Vector3Wide axis, out Vector angle) { var shouldNegate = Vector.LessThan(q.W, Vector.Zero); axis.X = Vector.ConditionalSelect(shouldNegate, -q.X, q.X); @@ -239,7 +239,7 @@ public static void GetApproximateAxisAngleFromQuaternion(in QuaternionWide q, ou axis.X = Vector.ConditionalSelect(useFallback, Vector.One, axis.X); axis.Y = Vector.ConditionalSelect(useFallback, Vector.Zero, axis.Y); axis.Z = Vector.ConditionalSelect(useFallback, Vector.Zero, axis.Z); - MathHelper.ApproximateAcos(qw, out var halfAngle); + var halfAngle = MathHelper.Acos(qw); angle = new Vector(2) * halfAngle; } diff --git a/Demos/Demos/Tanks/TankDemo.cs b/Demos/Demos/Tanks/TankDemo.cs index 5c462ef73..515f0b527 100644 --- a/Demos/Demos/Tanks/TankDemo.cs +++ b/Demos/Demos/Tanks/TankDemo.cs @@ -49,12 +49,12 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0; bodyProperties = new CollidableProperty(); - //Note that this demo uses only 1 substep and 8 velocity iterations. + //Note that this demo uses only 1 substep and 6 velocity iterations. //That's partly to show that you can do such a thing, and partly because of (as of 2.4's initial release), there are situations where //contact data can become a little out of date during substepping, since the contact data is only updated once per frame rather than substep (apart from the depths, which are incrementally updated every substep). //In this demo, when using substepping, a wheel resting on another wheel from a destroyed tank can keep rocking back and forth for a long time as the error in contact offsets over substeps can introduce energy. //(I'd like to address this issue more directly to make substepping an unconditional win.) - Simulation = Simulation.Create(BufferPool, new TankCallbacks() { Properties = bodyProperties }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); + Simulation = Simulation.Create(BufferPool, new TankCallbacks() { Properties = bodyProperties }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(6, 1)); var builder = new CompoundBuilder(BufferPool, Simulation.Shapes, 2); builder.Add(new Box(1.85f, 0.7f, 4.73f), RigidPose.Identity, 10); diff --git a/Demos/Program.cs b/Demos/Program.cs index 4d5b7a5eb..dcd36b1e3 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -6,7 +6,7 @@ namespace Demos { class Program - { + { static void Main() { var window = new Window("pretty cool multicolored window", @@ -18,7 +18,6 @@ static void Main() content = ContentArchive.Load(stream); } //DeterminismTest.Test(content, 2, 32768); - //HeadlessTest.Test(content, 4, 32, 512); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); From 54617b73a5853dcb1f04d06d912df94ec8c23262 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 3 Feb 2022 15:12:09 -0600 Subject: [PATCH 421/947] ComputeClosedInertia/ComputeOpenInertia now return BodyInertia rather than using out parameters. --- BepuPhysics/Collidables/Mesh.cs | 26 ++++++++++++------- Demos/Demos/Cars/CarDemo.cs | 3 +-- Demos/Program.cs | 1 + .../FountainStressTestDemo.cs | 2 +- Demos/SpecializedTests/MeshTestDemo.cs | 3 +-- Demos/SpecializedTests/TriangleTestDemo.cs | 2 +- 6 files changed, 22 insertions(+), 15 deletions(-) diff --git a/BepuPhysics/Collidables/Mesh.cs b/BepuPhysics/Collidables/Mesh.cs index cc5e28534..d4acd04ec 100644 --- a/BepuPhysics/Collidables/Mesh.cs +++ b/BepuPhysics/Collidables/Mesh.cs @@ -340,7 +340,7 @@ public bool GetNextTriangle(out Vector3 a, out Vector3 b, out Vector3 c) /// Subtracts the newCenter from all points in the mesh hull. /// /// New center that all points will be made relative to. - public unsafe void Recenter(in Vector3 newCenter) + public unsafe void Recenter(Vector3 newCenter) { var scaledOffset = newCenter * inverseScale; for (int i = 0; i < Triangles.Length; ++i) @@ -365,17 +365,19 @@ public unsafe void Recenter(in Vector3 newCenter) /// Assumes the mesh is closed and should be treated as solid. /// /// Mass to scale the inertia tensor with. - /// Inertia tensor of the closed mesh. /// Center of the closed mesh. - public void ComputeClosedInertia(float mass, out BodyInertia inertia, out Vector3 center) + /// Inertia tensor of the closed mesh. + public BodyInertia ComputeClosedInertia(float mass, out Vector3 center) { var triangleSource = new MeshTriangleSource(this); MeshInertiaHelper.ComputeClosedInertia(ref triangleSource, mass, out _, out var inertiaTensor, out center); MeshInertiaHelper.GetInertiaOffset(mass, center, out var inertiaOffset); Symmetric3x3.Add(inertiaTensor, inertiaOffset, out var recenteredInertia); Recenter(center); + BodyInertia inertia; Symmetric3x3.Invert(recenteredInertia, out inertia.InverseInertiaTensor); inertia.InverseMass = 1f / mass; + return inertia; } /// @@ -383,13 +385,15 @@ public void ComputeClosedInertia(float mass, out BodyInertia inertia, out Vector /// Assumes the mesh is closed and should be treated as solid. /// /// Mass to scale the inertia tensor with. - /// Inertia of the closed mesh. - public readonly void ComputeClosedInertia(float mass, out BodyInertia inertia) + /// Inertia tensor of the closed mesh. + public readonly BodyInertia ComputeClosedInertia(float mass) { var triangleSource = new MeshTriangleSource(this); MeshInertiaHelper.ComputeClosedInertia(ref triangleSource, mass, out _, out var inertiaTensor); + BodyInertia inertia; inertia.InverseMass = 1f / mass; Symmetric3x3.Invert(inertiaTensor, out inertia.InverseInertiaTensor); + return inertia; } /// @@ -420,17 +424,19 @@ public readonly Vector3 ComputeClosedCenterOfMass() /// Assumes the mesh is open and should be treated as a triangle soup. /// /// Mass to scale the inertia tensor with. - /// Inertia tensor of the closed mesh. /// Center of the open mesh. - public void ComputeOpenInertia(float mass, out BodyInertia inertia, out Vector3 center) + /// Inertia tensor of the closed mesh. + public BodyInertia ComputeOpenInertia(float mass, out Vector3 center) { var triangleSource = new MeshTriangleSource(this); MeshInertiaHelper.ComputeOpenInertia(ref triangleSource, mass, out var inertiaTensor, out center); MeshInertiaHelper.GetInertiaOffset(mass, center, out var inertiaOffset); Symmetric3x3.Add(inertiaTensor, inertiaOffset, out var recenteredInertia); Recenter(center); + BodyInertia inertia; Symmetric3x3.Invert(recenteredInertia, out inertia.InverseInertiaTensor); inertia.InverseMass = 1f / mass; + return inertia; } /// @@ -438,13 +444,15 @@ public void ComputeOpenInertia(float mass, out BodyInertia inertia, out Vector3 /// Assumes the mesh is open and should be treated as a triangle soup. /// /// Mass to scale the inertia tensor with. - /// Inertia of the open mesh. - public readonly void ComputeOpenInertia(float mass, out BodyInertia inertia) + /// Inertia of the open mesh. + public readonly BodyInertia ComputeOpenInertia(float mass) { var triangleSource = new MeshTriangleSource(this); MeshInertiaHelper.ComputeOpenInertia(ref triangleSource, mass, out var inertiaTensor); + BodyInertia inertia; inertia.InverseMass = 1f / mass; Symmetric3x3.Invert(inertiaTensor, out inertia.InverseInertiaTensor); + return inertia; } /// diff --git a/Demos/Demos/Cars/CarDemo.cs b/Demos/Demos/Cars/CarDemo.cs index d4cc8ae8c..bb94ef684 100644 --- a/Demos/Demos/Cars/CarDemo.cs +++ b/Demos/Demos/Cars/CarDemo.cs @@ -130,8 +130,7 @@ public override void Initialize(ContentArchive content, Camera camera) return new Vector3(vertexPosition.X, height + edgeRamp, vertexPosition.Y); }, new Vector3(1, 1, 1), BufferPool, out var planeMesh); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -15, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), - Simulation.Shapes.Add(planeMesh))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -15, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), Simulation.Shapes.Add(planeMesh))); } diff --git a/Demos/Program.cs b/Demos/Program.cs index dcd36b1e3..1feb64570 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -1,5 +1,6 @@ using BepuUtilities; using DemoContentLoader; +using Demos.SpecializedTests; using DemoUtilities; using OpenTK; diff --git a/Demos/SpecializedTests/FountainStressTestDemo.cs b/Demos/SpecializedTests/FountainStressTestDemo.cs index 523317b33..c0d5776d2 100644 --- a/Demos/SpecializedTests/FountainStressTestDemo.cs +++ b/Demos/SpecializedTests/FountainStressTestDemo.cs @@ -188,7 +188,7 @@ void CreateRandomMesh(out Mesh mesh, out BodyInertia inertia) convexHull.Dispose(BufferPool); mesh = new Mesh(triangles, new Vector3(1), BufferPool); - mesh.ComputeClosedInertia(1, out inertia); + inertia = mesh.ComputeClosedInertia(1); } public void CreateBodyDescription(Random random, in RigidPose pose, in BodyVelocity velocity, out BodyDescription description) diff --git a/Demos/SpecializedTests/MeshTestDemo.cs b/Demos/SpecializedTests/MeshTestDemo.cs index 1e6aa716a..d1fe43b0f 100644 --- a/Demos/SpecializedTests/MeshTestDemo.cs +++ b/Demos/SpecializedTests/MeshTestDemo.cs @@ -69,8 +69,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) DemoMeshHelper.LoadModel(content, BufferPool, @"Content\newt.obj", new Vector3(5, 5, 5), out var newtMesh); - newtMesh.ComputeClosedInertia(10, out var newtInertia, out _); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(30, 20, 30), newtInertia, Simulation.Shapes.Add(newtMesh), 0.01f)); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(30, 20, 30), newtMesh.ComputeClosedInertia(10), Simulation.Shapes.Add(newtMesh), 0.01f)); Simulation.Statics.Add(new StaticDescription(new Vector3(30, 15, 30), Simulation.Shapes.Add(new Box(15, 1, 15)))); diff --git a/Demos/SpecializedTests/TriangleTestDemo.cs b/Demos/SpecializedTests/TriangleTestDemo.cs index a1965b4ee..5b4074d91 100644 --- a/Demos/SpecializedTests/TriangleTestDemo.cs +++ b/Demos/SpecializedTests/TriangleTestDemo.cs @@ -186,7 +186,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) DemoMeshHelper.LoadModel(content, BufferPool, "Content\\newt.obj", new Vector3(3), out var mesh); var collidable = new CollidableDescription(Simulation.Shapes.Add(mesh), ContinuousDetection.Discrete(2f, 2f)); - mesh.ComputeClosedInertia(1, out var newtInertia); + var newtInertia = mesh.ComputeClosedInertia(1); for (int i = 0; i < 5; ++i) { Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(-20, 5 + i * 5, 0), newtInertia, collidable, -1e-2f)); From 3dd49c31fc7cffad026680e9d7c74f6c8dac282d Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 3 Feb 2022 17:02:27 -0600 Subject: [PATCH 422/947] Starting speculative margins documentation. --- Documentation/SpeculativeMargins.md | 37 +++++++++++++++++++++++++++++ 1 file changed, 37 insertions(+) create mode 100644 Documentation/SpeculativeMargins.md diff --git a/Documentation/SpeculativeMargins.md b/Documentation/SpeculativeMargins.md new file mode 100644 index 000000000..e09e8f9fe --- /dev/null +++ b/Documentation/SpeculativeMargins.md @@ -0,0 +1,37 @@ +# What's a speculative margin? +The maximum distance at which collision pairs generate speculative contacts. + +Speculative contacts are contacts with negative depth. They're still solved, but they don't push back against motion unless the velocity is high enough that the involved collidables are expected to come into contact within the next frame. + +Here's a picture! + +The ball is heading towards the ground with a high enough velocity that the velocity expanded bounding box intersects the ground's bounding box. Similarly, since the collidables are configured to have no maximum speculative margin in this example, a speculative contact is created. The solver will detect and push back the part of velocity which would result in penetration. In the next frame, the ball and ground are in contact. + +This is a form of continuous collision detection in the sense that it can avoid bodies tunneling through each other. + +# Do I need to care about speculative margins? +Most of the time, you don't. Consider a body or static created by just specifying the collision shape like so: +```cs +var dynamicBoxShape = new Box(1, 1, 1); +Simulation.Bodies.Add(BodyDescription.CreateDynamic( + new Vector3(10, 5, 0), dynamicBoxShape.ComputeInertia(1), Simulation.Shapes.Add(dynamicBoxShape), 0.01f)); +Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), Simulation.Shapes.Add(new Box(2500, 1, 2500)))); +``` +This creates `CollidableDescription` from the `TypedIndex` returned by `Simulation.Shapes.Add`. When no other information is specified, a `CollidableDescription` defaults to a `ContinuousDetection` mode of `ContinuousDetection.Passive`. More details about what that means will come later, but the short version is that: +1. The bounding box is expanded by the whole velocity of the body, if the collidable is associated with a body. +2. The maximum speculative margin is `float.MaxValue`. In other words, there's no upper limit. +3. No sweep tests are used. Contacts are simply created from closest features. + +Taken together, this makes most stuff just work. Performance stays high since speculative contacts only get created if the velocity is high enough to warrant them, and high velocity collisions tend to have robust behavior since speculative contacts get generated. + +For most use cases, sticking with the default is a high performance and high quality option. + +# What are ghost collisions? + +# Do speculative margins have any other surprising side effects? +Yes! Speculative contacts are mostly incompatible with the traditional approach to bounciness- a coefficient of restitution which sets an opposing velocity goal along a contact normal proportional to the incoming velocity. That's why you won't find a 0 to 1 `CoefficientOfRestitution` anywhere in the library. + +Instead, all contacts are springs. In `INarrowPhaseCallbacks.ConfigureContactManifold` you can customize a pair's `PairMaterialProperties` which include a `SpringSettings` and `MaximumRecoveryVelocity`. Using a sufficiently high `MaximumRecoveryVelocity` and reducing the `SpringSettings.DampingRatio` to 0 will minimize the amount of energy damped out during a bounce. There is a bit complexity here- the `Frequency` must be low enough that the simulation can actually represent it. If the contact is trying to make a bounce happen at 240hz, but the integrator timestep is only 60hz, the unrepresentable motion will get damped out and the body won't bounce as much. + +For more information, see the [BouncinessDemo](../Demos/Demos/BouncinessDemo.cs). + From 2d887ddf84573aa5ae26bb47da21209793b2797c Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 3 Feb 2022 17:25:27 -0600 Subject: [PATCH 423/947] More speculativedocs. --- Documentation/SpeculativeMargins.md | 24 ++++++++++++++++++++---- 1 file changed, 20 insertions(+), 4 deletions(-) diff --git a/Documentation/SpeculativeMargins.md b/Documentation/SpeculativeMargins.md index e09e8f9fe..ab9b102aa 100644 --- a/Documentation/SpeculativeMargins.md +++ b/Documentation/SpeculativeMargins.md @@ -3,8 +3,6 @@ The maximum distance at which collision pairs generate speculative contacts. Speculative contacts are contacts with negative depth. They're still solved, but they don't push back against motion unless the velocity is high enough that the involved collidables are expected to come into contact within the next frame. -Here's a picture! - The ball is heading towards the ground with a high enough velocity that the velocity expanded bounding box intersects the ground's bounding box. Similarly, since the collidables are configured to have no maximum speculative margin in this example, a speculative contact is created. The solver will detect and push back the part of velocity which would result in penetration. In the next frame, the ball and ground are in contact. This is a form of continuous collision detection in the sense that it can avoid bodies tunneling through each other. @@ -17,7 +15,7 @@ Simulation.Bodies.Add(BodyDescription.CreateDynamic( new Vector3(10, 5, 0), dynamicBoxShape.ComputeInertia(1), Simulation.Shapes.Add(dynamicBoxShape), 0.01f)); Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), Simulation.Shapes.Add(new Box(2500, 1, 2500)))); ``` -This creates `CollidableDescription` from the `TypedIndex` returned by `Simulation.Shapes.Add`. When no other information is specified, a `CollidableDescription` defaults to a `ContinuousDetection` mode of `ContinuousDetection.Passive`. More details about what that means will come later, but the short version is that: +For both the dynamic and the static, this creates a `CollidableDescription` from the `TypedIndex` returned by `Simulation.Shapes.Add`. When no other information is specified, a `CollidableDescription` defaults to a `ContinuousDetection` mode of `ContinuousDetection.Passive`. See the later section for more details, but the short version is that: 1. The bounding box is expanded by the whole velocity of the body, if the collidable is associated with a body. 2. The maximum speculative margin is `float.MaxValue`. In other words, there's no upper limit. 3. No sweep tests are used. Contacts are simply created from closest features. @@ -27,9 +25,27 @@ Taken together, this makes most stuff just work. Performance stays high since sp For most use cases, sticking with the default is a high performance and high quality option. # What are ghost collisions? +A bad thing that speculative contacts can do! In the solver, a contact constraint (speculative or not) acts like a plane. As far as the solver is concerned, the contact surface has unlimited horizontal extent. This is a perfectly fine approximation when the contacts are created at a reasonable location, but it can fail when objects are moving very quickly past each other. + +The ball smacks into the plane created by the speculative contact, sending both the box and ball flying in unexpected directions. + +You can mitigate ghost collisions by either using a higher `Simulation.Timestep` rate or by shrinking the maximum speculative margin on the involved bodies. To shrink the margin, instead of passing in just the shape index as your `CollidableDescription`, provide the `BodyDescription` or `StaticDescription` a full `CollidableDescription` like so: +```cs +Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), + new CollidableDescription(Simulation.Shapes.Add(new Box(100, 1, 100)), ContinuousDetection.CreatePassive(0, 1)))); +``` +This still uses a 'passive' continuous collision detection mode (explained in the next section) like the default, but limits the speculative margin for any pairs involving this static collidable to between 0 and 1. Collision pairs with this static cannot generate speculative contacts more than 1 unit away from the surface. + +Using a smaller maximum speculative margin means that you can miss high velocity non-ghost collisions, though: + + + +# What other configuration options exist for continuous collision detection? + + # Do speculative margins have any other surprising side effects? -Yes! Speculative contacts are mostly incompatible with the traditional approach to bounciness- a coefficient of restitution which sets an opposing velocity goal along a contact normal proportional to the incoming velocity. That's why you won't find a 0 to 1 `CoefficientOfRestitution` anywhere in the library. +Speculative contacts are mostly incompatible with the traditional approach to bounciness- a coefficient of restitution which sets an opposing velocity goal along a contact normal proportional to the incoming velocity. That's why you won't find a 0 to 1 `CoefficientOfRestitution` anywhere in the library. Instead, all contacts are springs. In `INarrowPhaseCallbacks.ConfigureContactManifold` you can customize a pair's `PairMaterialProperties` which include a `SpringSettings` and `MaximumRecoveryVelocity`. Using a sufficiently high `MaximumRecoveryVelocity` and reducing the `SpringSettings.DampingRatio` to 0 will minimize the amount of energy damped out during a bounce. There is a bit complexity here- the `Frequency` must be low enough that the simulation can actually represent it. If the contact is trying to make a bounce happen at 240hz, but the integrator timestep is only 60hz, the unrepresentable motion will get damped out and the body won't bounce as much. From 46db9c81e8ea6a6f0da0be8fc7e89f6d2bbbfed6 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 3 Feb 2022 18:06:26 -0600 Subject: [PATCH 424/947] Continuing speculative margins... --- Documentation/SpeculativeMargins.md | 31 +++++++++++++++++++++++++++-- 1 file changed, 29 insertions(+), 2 deletions(-) diff --git a/Documentation/SpeculativeMargins.md b/Documentation/SpeculativeMargins.md index ab9b102aa..ccc617374 100644 --- a/Documentation/SpeculativeMargins.md +++ b/Documentation/SpeculativeMargins.md @@ -7,6 +7,8 @@ The ball is heading towards the ground with a high enough velocity that the velo This is a form of continuous collision detection in the sense that it can avoid bodies tunneling through each other. +See the [ContinuousCollisionDetectionDemo](../Demos/Demos/ContinuousCollisionDetectionDemo.cs) for more information. + # Do I need to care about speculative margins? Most of the time, you don't. Consider a body or static created by just specifying the collision shape like so: ```cs @@ -34,13 +36,38 @@ You can mitigate ghost collisions by either using a higher `Simulation.Timestep` Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(100, 1, 100)), ContinuousDetection.CreatePassive(0, 1)))); ``` -This still uses a 'passive' continuous collision detection mode (explained in the next section) like the default, but limits the speculative margin for any pairs involving this static collidable to between 0 and 1. Collision pairs with this static cannot generate speculative contacts more than 1 unit away from the surface. +This still uses a 'passive' continuous collision detection mode (explained in a couple of sections) like the default, but limits the speculative margin for any pairs involving this static collidable to between 0 and 1. Collision pairs with this static cannot generate speculative contacts more than 1 unit away from the surface. Using a smaller maximum speculative margin means that you can miss high velocity non-ghost collisions, though: +# What about swept continuous collision detection? +Specifying `ContinuousDetection.Continuous` in the `CollidableDescription` means that pairs involving the collidable will use sweep-tested collision detection. That is, rather than computing contacts based on where the bodies are as of the last frame, a sweep test will determine where the bodies are likely to be *at the time of impact* during this frame. Contacts are then created at that time of impact. + +This avoids almost all ghost collisions, since bodies passing each other at high speed will be detected as having no impact. + +Swept testing can miss *secondary* contacts that large-margin speculative contacts wouldn't, though. But you can combine both! Speculative contacts work with sweep testing; they are not mutually exclusive. To demonstrate this, consider the configuration options for the `Continuous` mode. + +The first parameter is a `minimumSweepTimestep`. While the sweep test uses a fancy algorithm that narrows the time of possible impact very rapidly with each step of execution, you can allow it to run faster by specifying a larger `minimumSweepTimestep`. It's effectively your maximum desired temporal resolution. If you don't care about collisions that last less than a millisecond (and your simulated units of time are seconds), then a `minimumSweepTimestep` of `1e-3f` ensures that the search always makes at least that much progress in a single step. + +You can also speed up the search by increasing the `sweepConvergenceThreshold`. The search algorithm works by narrowing an interval of possible collision step by step; if that interval becomes smaller than the convergence threshold (again in units of time), the search will stop. + +By default, both of these values are 1e-3f. Increasing them will make the search faster, but result in larger error in the final time of impact estimate. But that's fine, because speculative margins still exist! + +The goal is to find a rough time *close* to the time of impact such that the speculative contacts created by narrow phase testing won't cause ghost collisions. That's a pretty forgiving problem. + +Overall, using `Continuous` will be pretty fast since it only uses sweeps when the velocity in a given pair is high enough to warrant it. Of course, when the sweep test does run, it's not completely free, so prefer the simpler modes if they do what you want. Especially for really complicated compound shapes or meshes. (And preferably, don't have really complicated dynamic compounds or meshes.) + +# What other configuration options exist? +There are three continuous collision detection modes: +1. `Discrete`: No sweep tests are performed. Default speculative contact generation will occur within the speculative margin. The collidable's bounding box will not be expanded by velocity beyond the speculative margin. This is the cheapest mode when the maximum speculative margin is small, since more potential collision pairs are filtered out by the smaller bounding box. If a `Discrete` mode collidable is moving quickly and the maximum speculative margin is limited, the fact that its bounding box is not expanded may cause it to miss a collision with another collidable even if that collidable is `Passive` or `Continuous`. +2. `Passive`: No sweep tests are performed. Default speculative contact generation will occur within the speculative margin. The collidable's bounding box *will* be expanded by velocity without being limited by the speculative margin.This is useful when a collidable may move quickly and does not itself require continuous detection, but there exist other collidables with continuous modes that should avoid missing collisions. +3. `Continuous`: Collision detection will start with a sweep test to identify a likely time of impact. Speculative contacts will be generated for the predicted collision. The collidable's bounding box *will* be expanded by velocity without being limited by the speculative margin. This mode can do well with high velocity motion and very few ghost collisions. With restricted maximum speculative margins, this mode can miss secondary collisions that would have occurred due to the primary impact's velocity change. + +Note that, if the maximum speculative margin is set to `float.MaxValue`, there's no difference between `Discrete` and `Passive` since the bounding box will get expanded either way. +You can also set the *minimum* speculative margin to a nonzero value, though this is unlikely to be useful. The *effective* speculative margin used in a pair is based on the velocities of the bodies clamped by the minimum and maximums from each body. -# What other configuration options exist for continuous collision detection? +TODO From 27ef0192b2cd56fedfa0e602701a72d440d6616f Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 3 Feb 2022 18:40:14 -0600 Subject: [PATCH 425/947] Speculative progress. --- Documentation/SpeculativeMargins.md | 23 ++++++++++++----------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/Documentation/SpeculativeMargins.md b/Documentation/SpeculativeMargins.md index ccc617374..8fa8b4175 100644 --- a/Documentation/SpeculativeMargins.md +++ b/Documentation/SpeculativeMargins.md @@ -1,13 +1,18 @@ -# What's a speculative margin? -The maximum distance at which collision pairs generate speculative contacts. +# What is continuous collision detection? +Continuous collision detection is a family of techniques that try to stop bodies from tunneling into (or through) each other at high velocities. Generating normal contact constraints at discrete points in time will tend to miss such fast moving collisions or respond to them too late. -Speculative contacts are contacts with negative depth. They're still solved, but they don't push back against motion unless the velocity is high enough that the involved collidables are expected to come into contact within the next frame. +In bepuphysics2, continuous collision detection is handled mostly through speculative contacts. When those aren't sufficient, the library offers a mode that performs sweep testing to find a time of impact. -The ball is heading towards the ground with a high enough velocity that the velocity expanded bounding box intersects the ground's bounding box. Similarly, since the collidables are configured to have no maximum speculative margin in this example, a speculative contact is created. The solver will detect and push back the part of velocity which would result in penetration. In the next frame, the ball and ground are in contact. +See the [ContinuousCollisionDetectionDemo](../Demos/Demos/ContinuousCollisionDetectionDemo.cs) for more information about the topics covered here. + +# What's a speculative contact? +Speculative contacts are contacts with negative depth. They're still solved, but they don't apply any forces unless the velocity is high enough that the involved collidables are expected to come into contact within the next frame. -This is a form of continuous collision detection in the sense that it can avoid bodies tunneling through each other. +The speculative *margin* is the maximum distance at which a collision pair will generate speculative contacts. -See the [ContinuousCollisionDetectionDemo](../Demos/Demos/ContinuousCollisionDetectionDemo.cs) for more information. +TODO PICTURE + +The ball is heading towards the ground with a high enough velocity that the velocity expanded bounding box intersects the ground's bounding box. Similarly, since the collidables are configured to have no maximum speculative margin in this example, a speculative contact is created. The solver will detect and push back the part of velocity which would result in penetration. In the next frame, the ball and ground are in contact. # Do I need to care about speculative margins? Most of the time, you don't. Consider a body or static created by just specifying the collision shape like so: @@ -65,11 +70,7 @@ There are three continuous collision detection modes: Note that, if the maximum speculative margin is set to `float.MaxValue`, there's no difference between `Discrete` and `Passive` since the bounding box will get expanded either way. -You can also set the *minimum* speculative margin to a nonzero value, though this is unlikely to be useful. The *effective* speculative margin used in a pair is based on the velocities of the bodies clamped by the minimum and maximums from each body. - -TODO - - +You can also set the *minimum* speculative margin to a nonzero value, though this is rarely useful. The *effective* speculative margin used in a pair is based on the velocities of the bodies clamped by the minimum and maximums from each body. If bodies aren't moving, the speculative margins will tend to be very small. Setting a nonzero minimum could make sense if you expect there to be a lot of velocity introduced in the middle of a timestep (perhaps by other constraints) that make the velocity-estimated effective speculative margin insufficient. Usually, though, just leave it at zero. # Do speculative margins have any other surprising side effects? Speculative contacts are mostly incompatible with the traditional approach to bounciness- a coefficient of restitution which sets an opposing velocity goal along a contact normal proportional to the incoming velocity. That's why you won't find a 0 to 1 `CoefficientOfRestitution` anywhere in the library. From 54a141b944ffa847c43ce1923d56d419d748f9d9 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 3 Feb 2022 18:40:41 -0600 Subject: [PATCH 426/947] Rename! --- .../{SpeculativeMargins.md => ContinuousCollisionDetection.md} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename Documentation/{SpeculativeMargins.md => ContinuousCollisionDetection.md} (100%) diff --git a/Documentation/SpeculativeMargins.md b/Documentation/ContinuousCollisionDetection.md similarity index 100% rename from Documentation/SpeculativeMargins.md rename to Documentation/ContinuousCollisionDetection.md From 9b0df00dd7a1ecf8f82f71ccfeb32ca8f464467d Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 3 Feb 2022 18:52:01 -0600 Subject: [PATCH 427/947] More speculative progress. --- Documentation/ContinuousCollisionDetection.md | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/Documentation/ContinuousCollisionDetection.md b/Documentation/ContinuousCollisionDetection.md index 8fa8b4175..c8737fb8e 100644 --- a/Documentation/ContinuousCollisionDetection.md +++ b/Documentation/ContinuousCollisionDetection.md @@ -1,6 +1,8 @@ # What is continuous collision detection? Continuous collision detection is a family of techniques that try to stop bodies from tunneling into (or through) each other at high velocities. Generating normal contact constraints at discrete points in time will tend to miss such fast moving collisions or respond to them too late. +TODO PICTURE + In bepuphysics2, continuous collision detection is handled mostly through speculative contacts. When those aren't sufficient, the library offers a mode that performs sweep testing to find a time of impact. See the [ContinuousCollisionDetectionDemo](../Demos/Demos/ContinuousCollisionDetectionDemo.cs) for more information about the topics covered here. @@ -32,9 +34,11 @@ Taken together, this makes most stuff just work. Performance stays high since sp For most use cases, sticking with the default is a high performance and high quality option. # What are ghost collisions? -A bad thing that speculative contacts can do! In the solver, a contact constraint (speculative or not) acts like a plane. As far as the solver is concerned, the contact surface has unlimited horizontal extent. This is a perfectly fine approximation when the contacts are created at a reasonable location, but it can fail when objects are moving very quickly past each other. +In the solver, a contact constraint (speculative or not) acts like a plane. As far as the solver is concerned, the contact surface has unlimited horizontal extent. This is a perfectly fine approximation when the contacts are created at a reasonable location, but it can fail when objects are moving very quickly past each other. + +TODO PICTURE -The ball smacks into the plane created by the speculative contact, sending both the box and ball flying in unexpected directions. +The ball smacks into the plane created by the speculative contact, sending both the box and ball flying in unexpected directions. That's a ghost collision. You can mitigate ghost collisions by either using a higher `Simulation.Timestep` rate or by shrinking the maximum speculative margin on the involved bodies. To shrink the margin, instead of passing in just the shape index as your `CollidableDescription`, provide the `BodyDescription` or `StaticDescription` a full `CollidableDescription` like so: ```cs @@ -45,6 +49,8 @@ This still uses a 'passive' continuous collision detection mode (explained in a Using a smaller maximum speculative margin means that you can miss high velocity non-ghost collisions, though: +TODO PICTURE + # What about swept continuous collision detection? Specifying `ContinuousDetection.Continuous` in the `CollidableDescription` means that pairs involving the collidable will use sweep-tested collision detection. That is, rather than computing contacts based on where the bodies are as of the last frame, a sweep test will determine where the bodies are likely to be *at the time of impact* during this frame. Contacts are then created at that time of impact. From dcadbad8ee8a16616f18741a45db21fb29c9b152 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 4 Feb 2022 14:51:06 -0600 Subject: [PATCH 428/947] Documentation updates. --- Documentation/ContinuousCollisionDetection.md | 6 ++-- Documentation/QuestionsAndAnswers.md | 32 ++++++++++-------- .../ContinuousCollisionDetection/ccd.png | Bin 0 -> 7497 bytes .../speculativeContact.png | Bin 0 -> 15172 bytes README.md | 2 ++ 5 files changed, 23 insertions(+), 17 deletions(-) create mode 100644 Documentation/images/ContinuousCollisionDetection/ccd.png create mode 100644 Documentation/images/ContinuousCollisionDetection/speculativeContact.png diff --git a/Documentation/ContinuousCollisionDetection.md b/Documentation/ContinuousCollisionDetection.md index c8737fb8e..73ff4b25e 100644 --- a/Documentation/ContinuousCollisionDetection.md +++ b/Documentation/ContinuousCollisionDetection.md @@ -1,7 +1,7 @@ # What is continuous collision detection? Continuous collision detection is a family of techniques that try to stop bodies from tunneling into (or through) each other at high velocities. Generating normal contact constraints at discrete points in time will tend to miss such fast moving collisions or respond to them too late. -TODO PICTURE +

In bepuphysics2, continuous collision detection is handled mostly through speculative contacts. When those aren't sufficient, the library offers a mode that performs sweep testing to find a time of impact. @@ -12,9 +12,9 @@ Speculative contacts are contacts with negative depth. They're still solved, but The speculative *margin* is the maximum distance at which a collision pair will generate speculative contacts. -TODO PICTURE +

-The ball is heading towards the ground with a high enough velocity that the velocity expanded bounding box intersects the ground's bounding box. Similarly, since the collidables are configured to have no maximum speculative margin in this example, a speculative contact is created. The solver will detect and push back the part of velocity which would result in penetration. In the next frame, the ball and ground are in contact. +The ball is heading towards the ground with a high enough velocity that the velocity expanded bounding box intersects the ground's bounding box. Similarly, since the collidables in this picture are configured to have an unlimited speculative margin, a speculative contact is created. The solver will detect and push back the part of velocity which would result in penetration. In the next frame, the ball and ground are in contact. # Do I need to care about speculative margins? Most of the time, you don't. Consider a body or static created by just specifying the collision shape like so: diff --git a/Documentation/QuestionsAndAnswers.md b/Documentation/QuestionsAndAnswers.md index 92b2f3c04..2d63403ef 100644 --- a/Documentation/QuestionsAndAnswers.md +++ b/Documentation/QuestionsAndAnswers.md @@ -1,17 +1,18 @@ # Q&A -### I'm seeing spikes in the time it takes to simulate a timestep, what's going on? +## I'm seeing spikes in the time it takes to simulate a timestep, what's going on? -If it quits happening a little while after application startup, it's probably JIT compilation. If it keeps happening, the operating system might be struggling with a bunch of threads competing for timeslices, resulting in stalls. In that case, try using fewer threads for the physics- leaving one free might be all it takes. See the -[Performance Tips](PerformanceTips.md#general) for more. +If it quits happening a little while after application startup, it's probably JIT compilation. You could consider warming up the simulation so all the relevant codepaths are seen by the JIT ahead of time. You may also want to look into ahead of time compilation like NativeAOT. -### How can I make a convex shape rotate around a point other than its volumetric center? +If it keeps happening, and the spikes are in the range of a handful of milliseconds, the operating system might be struggling with a bunch of threads competing for timeslices, resulting in stalls. In that case, try using fewer threads for the physics- leaving one free might be all it takes. See the [Performance Tips](PerformanceTips.md#general) for more. + +## How can I make a convex shape rotate around a point other than its volumetric center? Other than triangles, all convex shapes are centered on their volumetric center, and there is no property in the `CollidableDescription` to offset a body's shape. However, you can create a `Compound` with just one child and give that child an offset local pose. The overhead is pretty tiny. -### How do I make an object that can't be moved by outside influences, like other colliding dynamic bodies, but can still have a velocity? +## How do I make an object that can't be moved by outside influences, like other colliding dynamic bodies, but can still have a velocity? Use a kinematic body. To create one, use `BodyDescription.CreateKinematic` or set the inverse mass and all components of the inverse inertia to zero in the `BodyDescription` passed to `Simulation.Bodies.Add`. Kinematic bodies have effectively infinite mass and cannot be moved by any force. You can still change their velocity directly, though. @@ -21,7 +22,10 @@ Be careful when using kinematics- they are both unstoppable forces and immovable Also, if two kinematic bodies collide, a constraint will not be generated. Kinematics cannot respond to collisions, not even with other infinitely massive objects. They will simply continue to move along the path defined by their velocity. -### I made a body with zero inverse mass and nonzero inverse inertia and the simulation exploded/crashed! Why? +## The heck is a 'speculative margin'/'speculative contact'? +A way of solving for predicted collisions to stop penetration and tunneling. See the [continuous collision detection documentation](ContinuousCollisionDetection.md) for more details. + +## I made a body with zero inverse mass and nonzero inverse inertia and the simulation exploded/crashed! Why? While dynamic bodies with zero inverse mass and nonzero inverse inertia tensors are technically allowed, they require extreme care. It is possible for constraints to be configured such that there is no solution, resulting in a division by zero. `NaN` values will propagate through the simulation and make everything explode. @@ -33,7 +37,7 @@ Generally, avoid creating dynamic bodies with zero inverse mass unless you can a (You can also just use a constraint to keep an object positioned in one spot rather than setting its inverse mass to zero!) -### How can I ensure that the results of a simulation are deterministic (given the same inputs, the simulation produces the same physical result) on a single machine? +## How can I ensure that the results of a simulation are deterministic (given the same inputs, the simulation produces the same physical result) on a single machine? Take great care to ensure that every interaction with the physics simulation is reproduced in exactly the same order on each execution. This even includes the order of adds and removes! @@ -43,7 +47,7 @@ Assuming that all external interactions with the engine are deterministic, the s The `Deterministic` property defaults to false. Ensuring determinism has a slight performance impact. It should be trivial for most simulations, but large and extremely chaotic simulations may take a noticeable hit. -### What do I do if I want determinism across different computers? +## What do I do if I want determinism across different computers? Hope that they happen to have exactly the same architecture so that every single instruction produces bitwise identical results. :( @@ -53,9 +57,9 @@ But, in general, the only way to guarantee cross platform determinism is to avoi At the moment, BEPUphysics v2 does not support software floats or fixed math out of the box, and it would be a pretty enormous undertaking to port it all over without destroying performance. -I may look into conditionally compiled alternative scalar types in the future. I can't guarantee when or if I'll get around to it, though; don't wait for me! +I may try to get something working here in the future. I can't guarantee when or if I'll get around to it, though; don't wait for me! -### I updated to the latest version of the physics library and simulations are producing different results than before, even though I set the `Simulation.Deterministic` property to true! What do? +## I updated to the latest version of the physics library and simulations are producing different results than before, even though I set the `Simulation.Deterministic` property to true! What do? Different versions of the library are not guaranteed to produce identical simulation results. Guaranteeing cross-version determinism would constrain development to an unacceptable degree. @@ -63,13 +67,13 @@ If you need determinism of results over long periods (for example, storing game Such a drift correcting mechanism also compensates for the differences between processor architectures, so you'd gain the ability to share the replay across different hardware as a bonus. -### I was trying to simulate the behavior of a spinning multitool in zero gravity and noted a CLEAR lack of the Dzhanibekov effect! +## I was trying to simulate the behavior of a spinning multitool in zero gravity and noted a CLEAR lack of the Dzhanibekov effect! By default, angular momentum is not explicitly tracked; angular velocity will remain constant during rotation without outside impulses. Momentum-conserving angular integration can be chosen by returning a different value from the `IPoseIntegratorCallbacks.AngularIntegrationMode` property. Gyroscopes tend to work better with `ConserveMomentum`, while `ConserveMomentumWithGyroscopicTorque` is less prone to velocity drift toward lower inertia axes. -## Surprises +# Surprises Simulating real physics is slightly difficult, so corners are cut. Lots of corners. Sometimes this results in unexpected behavior. Sometimes different physics libraries cut different corners, so the unexpected behavior differs. @@ -79,7 +83,7 @@ This is a list of some things that might frustrate or raise an eyebrow (and what This list will probably change over time. -### ...Where is the bounciness/restitution material property? +## ...Where is the bounciness/restitution material property? You're not crazy- it doesn't exist! Instead, at the time of writing, there is friction, frequency, damping ratio, and maximum recovery velocity. @@ -87,7 +91,7 @@ Frequency and damping ratio can achieve some of the same effects as restitution The reason for the lack of a traditional coefficient of restitution is speculative contacts. v1 used them too, but v2 pushes their usage much further and uses them as the primary form of continuous collision detection. Most of the problems caused by speculative contacts (like ghost contacts) have been smoothed over, but the naive implementation of velocity-flip restitution simply doesn't work with speculative contacts. -### Swept shape tests against the backfaces of meshes don't go through, but collisions do. What's going on? +## Swept shape tests against the backfaces of meshes don't go through, but collisions do. What's going on? While ray and collision testing against triangles is always one sided, swept shape tests are double sided. This is pretty strange, and there isn't a secret good reason for it. diff --git a/Documentation/images/ContinuousCollisionDetection/ccd.png b/Documentation/images/ContinuousCollisionDetection/ccd.png new file mode 100644 index 0000000000000000000000000000000000000000..774af626f10ba244d93c0dc3f5f103c7037af99f GIT binary patch literal 7497 zcmbVxXIxWD_ipG-K$Id9K|w&JcLD+`CG;AkcO)vEP^3uj(mMo1q<3k8bZH?Hg7hv( z3DO0mBizk7?>*)H-}~X-5BX)DnYGtmGi&y&^{k1|REJWM-y{ctK$J>~a@rse-gV#y zC&dSP$X`i_1All3ZRitF1s1#t07CdAs1rdPBoD>ZZAP`mOi!kh9c7ZSlx!|4n!#wZIOTpKHT%Vh(oxzQ2kgzr{5K_ zz!HMm-OZ^$-0fhHzCQ%?0fYdV zsGegKx5)z1g=1w(<{~bPU><+M4jU495Q0pc=~RIT+|w<4wpU!B;l5l>j&5}QtszJO zdVnHED4@QbgY`OZrFo)6bQ~1XC!M3)M0vjrC)5iF9`!4VtaSGxw>v1tp($F@6SUdG z{SG;v0;}cL+922)7{VturIL)RK-FBa{g^8|*gCtvMIA@Rl|OmOG?eWr zO~Lap#XY19&wnbdC$ns7n)!POY_KnZXNPA`j9uFIB%zGZzXTYHrjegxiTCTI*|nEj z{nX54oxl2nG)oK`<(Z)_=wkC@rZ?CN7vdi008*t9S{ZBC*~)*YyCsM#Op~aeg;)zr z&#zCLKn<%cB!Ch7xtmHTJ z$F~w6M)*dB#@6U0aoyw}Z@Vs>w#(*Ysi^*}Ch{(Z0-nJvMNIl-?mKL*T zReBkmfJ%s@65;=>3$rs}_H)sBI;ORO5?-90vPO$Oj9k55}q?^;fyVDW4!Kk zzUwp4HJ*C~t~$MX`-`igop?JS5!K9WG@rcWcOa;{GAP{CX7vfd_TC{o4PSL!Bt^_5XZR--5aRaMQ@Q6m$5&QLL=^;TKQ|y>_KHpnv zKkWH|Cj01%a3lS}3ZW}3{`l``3&g^5&y=@G0$zM`R!ai0?uf(o2V?9?bi$$5%Y}E+ zJNXpQ8=#`AN;s#xDXzZP5|-Hv-nQ?RQ#?#O!+zzA@@3Mko8ZiEfyZGe{3YUFQlTy) zB~wW9l`fJH%M~+jG|N3wX3Y0aZ{I7gJEcjd0gYM)LBQSphxDffWwU0M8$oqGEGAas z?TezNz&<;7Eec*G*96gq+2fqfAV&}Hw&E5y8P{1KeB?+gvJ{XmM2adT4;-pKObpuI zC#(qS$B_zqNRvt%)VZpBDa`QM%BR@J;T^KVha*BO&y89og#FMdNk5YM4oS@7p!oCG z_%qnrkK?qz)k#&H$V|KKNhPKblszir}TK6?#1h0)6SG!Tpl*`gjV)ZWHo`uwn4p_y2h;2g>$;&K@D+4O+x7 zz7}fjl{`iZWg-cT@xrzsO#>#%rOiq=K}C#V89Jk5+ind>!XQnd7~e2+Pff#%#TDSy zFP&>I?O37*z3dspf#z@i^Wgss!DaTh_%7pQhd_@)#8F`5b>fj21ZLIymtFUm>mQw) zZZU9}Qvawgfb7Nl!S>z#fuYR*0U#LVKjTKGqx$5ssS5>Yf8HA5RL-P%A3KM5YNxjC z7bO!w3Jcxqhonuo=Df=S%p}8#73QyhuRo zTN=eOf%o8%0S@%;UqS7i9OCBG_TqLUUP!LoAC^ih3tyV!@x5V;^4`smR_w74{Hfmd zbzq{N_eL_WH;(<`*Gm8f(Ng_$)=3W7pd1EZg_WNS;_{xlem4|*r%G#2RpCYuDZdJ} zeO#p*678Wx0P23{ir8J-Cd@8lr3BFi4fT6UNt~~ki96oBN0pBx@D~jDM?emgzQvC(R@h9wCt7}hFh zd=zhj#OXxR3H)9DwVzHfjJ}Xa+gbl%t!&8UT=DkPg8+Cjw-Q zYDF5AYs??U=U&yOLL35l_(>)rn52R%$o46k6GXw>WG!E5Q^(^C5n}blGL_FB>CJh! z{=fyi(R-NNH%G0y2hn~y3lHHkiyJt%3(~G*pdn|U*hl7zk&^~nD#-&(2gzR`TUa3DX=SKBQN557f zt`>)aDbh)oX2G+11wj!uZOnI+KJoTd{YSI7U(#US_=~I}MrZH&RUncCFCddZLg+@b z-C?*Zmhj*YedKp<7g!Ed_QO^O_{rF3ReTaj!qPQHgCM&Eh^4z{hoO}nyTy&E>;as% z)P=Qi&+K}5ar#msSl`G8WNB=iT!BM#`{SX%dH3FS%Il4vR;oqFipv@cw1X#qZ22`# zaHf|!^7hpi^lioZsp?vyHzZiK1uM+4M@Q4|IabWry+jXBpwq5_kS9;=MmK+#M_luV za8^%?$!QWzsp z@0O1lOL8io7W6Ae1+XeJ$^JUWyt;6nLwf9wSyeMTJ>a!e*EPDpb>`=NztS+iwN3PV z97?Oxp=rR}b==u{aRe^Kl(xyH5UO{(eY-23|a^-2Yl zX><%COyX{?6<;)!cf<5avlrV^cewOYYed8fo8uMF0~Z+Sr`QX6F5YqP^AkIG3bbKV z3MI}$n7{e#1Otbju{SHdyK0Qeoc23!>E$%&wVYe(mPS`G+Fz=u_{OQ?qmy#PfiP%f z59tH8*JUpd8vIW4ca=QHW%fF=k#(_FUE5jq4KWaG{`O#v?w#~? zYIQ^{$5QEsz_c=^Bs%TtO;>wmPX+_+D5`$rH@du;nKoks*@XNk6F4;jBh6puMW1=3 z$i>q9~-dK(f(j4Jom?p2>H(jIyrNkyaciD5@UnYq3_+3YI_&CvZGg!LHR# zXk<(0j-&H7Vy-36#K|LAQRDzxfa6rXV`YM$7$l`~F>W1EiL zq#aYQR7T&(>`Oe@Z=Fya&)O*m|G-dTgFPV#!8VTZw3M87nkOPi#o;mVFY{4xLHF&a z7faBV*XAi^i>i-TY$}?MByvwfR@{K3|T5V|Ls>O?-Iwe76 zt-CiOEqV)3l5W-MQvLnqW$^Ej+DQ!UJa&clgiX7mger98W%e@p)=r)y%iB_wq*mA8 z&Txd;`_!X#S_u)VVkBvc%C8iX$BZ{QDNzZqsZSU)nuHpE2 zip53F2_*``50&73u5Po*vp8^cOBLp#qG?B|dnlx8tvo$$;c1PBU{R~!!37H+?ySc50Z=^iY&oJ_zWpW2bpEpbidMeYK zu00R-rX+y~W^_`H&q1RZ=2=Ac$3kl3pY}WVgm&T~HlFm?1rV7kL4|C79T+OR#Tq@Y z*g1B)LP~_PfE9ajA0X4Mw2MVEI`7=G5cDroVL184*6+tmCD>C{=|pHA)AIai@!T74 z%7lzEiTWCXd+%iTUG3Z(xbd|Po$BMyQTe_)Vu~V6JrfQw%-SOYA3jEVad#}sjg5$CDL+z2ILxt_HXJW-Ho4Fx_?;H=P^cD4WFeF9K>F&RImwJ_ z%WN+26sw%o&P00<3`vuYz5ay|6}cgxq?od^9I6hrK>$g&L(A8KxHG4Xp*5)x80Y&T zyV%Mg2&pB)oGS1GA?`M*%AXDKJJQZ-FzWY>?O-VC6t*ZSeIJkp5a&|Rd=%y2D1a(1 z(R%o_6c3k_rv54{cSO{9V zUL?$qN;ja(*k(3;;>KD+igC8_6?cDOpeZpLQN+rRLK`d;?nm0clb~59!HGivUMI#} zp8p5>%i6yh_gH=5hECewp8;KzvQ~JrFo3R!s`-&`5C%D(7!!?xrQ+r z)zXb#%UEMzTp2)s=A`Rsxz$eI*v4>jpwq`Z<_jQV)BTbAPjpf(QP#^@5wRu*2LJ8b zzr3A0_A0vrNr=6I(X#zUsd&0`qrwdV*lY+SqM1w#V(@q_mU0o^U9}tunlK_~KAnmWGUxX@`YQIg2x z%lWd1xy$fj#~b6ZOsIz0mbCZ%vqOdbWwGh%UP(_qAD&TkoIPi0F(4!i=aur>O-RbX zj~mXMl$Em_c9i#vBTh?h2h(RrvN+wL(W^A}-rc3eZO3583s%owZ@&hcX*Vu=p~VKDud64?%3b4!gMw%5A*vu3Jhz&(mul3?f zo?=V^be=azHYg;j-E6P(ha5a}wac8ruORxLDw~AFA6|25Bpz@4bb}v7L)ZhO7hNzE zOU1{D2hN;GPOIxChRXh<=!N?+bId3!sp#zvLV!P)4pxPjD*4F($!3hgN>B2~rkL8L z;Ouk~9H_}jaBr<&x^Lx!+o245{YJ=Z%<9OGLlFnVb7BnMf#uS^zEWRH(Nan@O*fYx zR(}Io?SvQtqS@u?ES4kPQsO;dm38i(+=6kwZWjP)SD*R8ctxeiY(Ba6yE*#csB!-2 zBwCBk_JHowCYNDL{m<~NlYcXnAr)p0CT&c*#Fn5{BZE9YNiQ~+lh31Tyn=KrB-2(r z&1-bqdUnbzvUo|Dnzml*s*CvMn#cE$xeY#GbsrVP;hk#ci}?;4Sh?l zXB5diD6URF+q*hKtlePWsAoT^0*K6GFf+`rB<6$!a*jCkenljEw7w~O2g|uu-ej>6 z(Oq`RF4Izii`+gP#aZsG1XoWUO%x0qvVS#X;FU^&2t{U^C8eo~+E{v*lbvvfOc zX^cS3K-HJ#6G>~Od6;?3!(THOr+X%ou$(vW=}CqAEVvv0o0$NEu7@%}#ehop$1q3L zF4ZX8+j8eR2J(%gdhcULTBn)==cuEW8lYlJ{W|t7P$JJGH#xAAG(l>AlTV?|l@%Jd zOs3#0OyA$*#KSm`-=725#G3JB>xgagbFnN={p{udOf-#ZHdsR{(=~2P(&`!xxx{kc zkJMyzFdVx39Bq(Af4Vvh|0R(QOL*NcU=*Y9b-E+g+G{MZ)`I@xb+|f0M4KwyBF4_A z*(B8vkqT6TYYY>4_B9OD9N6;Exs_3$92^iYl#V(COATApsf1B>5S_A(l*DD}V4&)ov%M ze&d1_`oAboTsc)LXkJbg<$*3v)iq=aJf$$9zS#R|s|4=3BdG0~a39c`r{{JGF3Xc7h3OwZS!y^`M;5Xh6_$W$(wTMu~bs+YOki}mX#UpZ?<-T75U0`*C4H2d*Q)3gcek3F~FhsPM-W%5EDY> zeU$4LsDyOpWH7%Jt$;Z4&RWgX2AhYVZ|oYS(KhB;(5JF>svauXwu8L~f1-XL;@37^ zyLpf0E>0h}zx?<`8-vQR2Ce+P=X-?+mEQ#pao2>S(tE&CpZ+d&NNV5g`-~69x!~f6aAaK3-AuA%{g2a%%2ovBn S;Q(BJf|TUdnzK_Lz7LLzP<+*qL=5FlF>)Hpp0EzyovJ|=Rd@e0 zG}DyQa>0N&!$O5JMUFl@y5H!AJt6TP&E0-yn&apwVG7rS>;scD8H1MJ>APC3T==8= zN9j)U9_{O)0F@%~?O%Jh6ANNqH(T&fMlG_FWBJkf1f_|1L>FP4=)1p^9Ezo8Za%K775U-i) zT=Uh1NkEp}Z>0!cR*t~+|8^=9E*TCMpQs_xfG=+eGlh*3l)?G^M+p~oXF9R`; z4MTo@9Vfk_AJ?zG6Thb#o+?A=va4P{XzC|@4wh>F=C#vZHe#(fwPP}ApNda^r}HOy z09I&}C0(>p>MK$*`sunq**Jx@uw9b7$ElF8jpZ;xLDNvx+MMwcRFKW@y4 z9{9$!~5n6OjqnVX?|i zU#5hmoqD;x^dWQMe>37iT`|9AwbWQrL(Et~hu-*uP5NMRPE6IMekRp2WK=YnS7$$C7!vH6NhIU{;kR zt4^kUtSyiRljme&4OsesX5P<@f0ITae|p8-6Q|6)659Tv=9-SqusoS!UH;5>Qd$0y zejg`zkBOW$yxN1yVf@~IDGk# ztD|B2Ip<{Ft-l=k+FF9P^!r+3;Edm+AbKm_!BQ+KCo%EkAfft$cL>X1f2M;$cLG;E zc^ealM`b^QARGi!r!}R+GmyplAP~#s2>8i-$DnVgkO5dqXxK zU0?-Ty*D7MtxzMc@RQw&wLHLf|82C%;k0@m()-HIWSqTeXhElOejw-I4)6Mez#GX{ zS`at)Ts5D$ztu(I!Rtsofi~5VoQT?bKu%20$=%&uBRmVMu3WW`72%)D=K{#jH>n?R zDxe@Z3WKTK24dtmTH$(a&$u|>{cp-ilxI6lY6q@T?`x^2^{6hwN}16o-yuh(>K=OE zzZk`rn&Ue-%YlvW?GzSSzZIp;83stV>Rx+aYK_a>{>9&$z;Z$AuWXIIAIc&AAEioK zFvT;T9J1)Pqwd_g=6JLB=6>%Mk)apxze=w;sikNp7Cr!`(tDVBw>q&CNO2-I4yK9I zG_6?|2i8OCgjdE%l*D6;?|3xe-f3-u3pJUg{N?9N0gKH2y>cWYaQ#s)>VSkiwTg2c z1DHWTJ+gEHd2f5^Hqr7F(TWY2*0-#uJH>E`kr~4>?R!RF=wp@bN7U!=n@=%9`0H{_ zT*!zr{;H|t6|-8X_!9#zNS-OPQ4DVyoj1MnX>f zIdXYz6GklkAKm>}HnpF<0fj+oS0}S^Q<}T$T&_K_E#p;9>$V^6O^B!-gt6YPEXnD9 ze?aqy;**&nltl5Gs>w@O=A(OHJ~M?H3l11!Tqo55c@@rlo{QREo>W-sHz z{}&mm@(YUu9N$RVX_R5Ug~u+ws56p3>~3!SJn#qMzjroY=x-#@#D#%uR8UU~|KG9J zeBBkHOYfQO8vf(7xd>2X2=oRa< zx@O8NMR{;lIXHm77)X3AOtw|{{r8aYp8xZIF(Dt0M`T{Iyv(XG@tQte;|$@a(7OF; zC{gFKQW*9gibo*Oy0-dO7aeds$RMXVu-!qPOq%%qzopJBMpVyu+O3PI+=((cfmNRu z5{O2t0~$RFS}G<`wzMB zd!6mKN<@s|wMPGO=;WcSqOa#44~6`je+tlyvjm)Mh9uFm$6V;|XT1zYsdrY^j$!*+ z4{zK@kyG8CFuHw@=}#d-%eMm>7%1%&y#{%BX=|f4DTBs*!)g(eLw}=x2<{g-5z?eD z2(plIi}Ii~a6WIFGhLBjkM_<@kUFx5^>n2if-to#{yN8#cOx=u_wodHR{w>0k?r+w z-^Vg&RUEcyocjqL&0_*a$%#_zM|rmyw*MmRobc!8P4-ivmMajN(?-1+y%h-1tIIxe z>Mgp+uEO0B5=1i`k+hB1&OxXE{%Qc$X>0cm9y+FH^p1Cu5UZI--fIrbR02Y+C4t4f zqsPEGim)B9WLf+Zo!w(zX4}-?W)_(qlC4kuhao$~Yka&-I z!TLgQYzNy;CXEgcsIrZNpHCDqyXJVrm;LfS0~hEj^Dd-*azE`5P2u=-&d`1(xA{0? z6#1^@60p*6xAhsgZUFjSfpmpUvu!ZEmF8Z*6=;`RutlFLt#g7+FVp^Gz!-94;9BNQ z*j(5!!vne|7am*92Tfd0T8{Cax<8n(cn=tQQ$z8>cf@2GR6$uTt>0Kfw>|75+7-e# zvsu#w6y|u)=Sv9=Q(Ax9;&w4{#`xyKJgeq{ZViG3tb|T!_D?N^yk!C9nj@2&Zfx(K zK26ZE*?o_nb7tXrK0x0G$MVp7>GQ|xIEIqZ*9-kNY--q^EAux{Fbnu<>)Z1qj)a!U z2CyT{rC!E<#=D3A37F|IXzy7>(MSJ9+(!Q(>)``n$zjUon zktDu8wWn!CyS;hyjG5))!xlIZ!)ryQT!Lnrd+#ifYok~fHE>Q^JgsTQWRDxIB^lf> zC?3Y_EW$)o)j!4<+2yG!hjW-F`6xHa#0^XxDnpFCgXRwz*VIp+#wOs>Z53ivzIp60 z^JrqhS{JNy^k7_0afdetwB`d@LfVeK-`y(w_+fIjz+1NG@s0hare!Ow5avA~)NPEg zXGzG-nQHD9!t#ob`ThJ2pZ14o7dy>9A9-HNTaikrJfmwR`Xx{2%Ua_TS3AL(Pu-WI zS@8yHt{30AwBpQ5<;40(r`N=$e?e46QGX>o_-xINZd$Kn!^=$D_wTP^VFJPNBLhTU zfq9?rl_zYbckdplUqhD)a;bwJd`7HZr=ci z+f@=UlCA!oZSlZ2Ix3u!)G-M8Dl+3oy@Rs7+?dEm6 z{6afm&$5$+w(IKu6po-cUw-#QH{t9>bnSC4?9cE_59f7pB`d-(cuR$PG%b#ntM6Xr zq8rYu;xgQ=-NK*y92~um<$I(+-3J6eH(#LbV^X^xWVtx)ICl-n#}2i?3yi;>OgWwC zF#;c@4B{{`G^#aobCnZku3XJf=RUm@U{bOK`>ND)&Bo{k_>ts$0}_3gxs{9ac~WiI z-2x?TW~Y7lVh9q4RsY$%N$qt?*K53?rNY7K;4hOhR+9{Miyf+Hw&N=O#4>>0Mu)%~ zG^j`%aQH{2J-B0R&l;Y2`D5t^-IsjjLS{Gr9{-nL&-JAe_7*7k1m(S!ZVx0tATQY1 zdMI z%(O}gWSFm}`Oh&qa|FM_yYk+*Y9A|*F5EL?7vZ^CZ7qEM($qg~BliQeB zv5KvW(}M=|QA}~VuTKjP(K)U)i^CYkg!D@aaPfU_ zX4Z4HB8_efydQf^I^o}*b54l4%GyEzG%K`6KDJZenV7=e@AUW04-LE~HSOdA?i5!; zrY9X}S8xeFj!qit5I)0UH*fYNJ`P{rZ{T8%94M$Fx!VoRz*aAW@BC!$?@u_#^njH= zG;)b+9+x-Vrx{+P@@NMlW!Gd{DmhsEqn=8u9sIR?A>!P zL)-0S!k<)|N&n#i`frGrKaaCm^YN}m`gf@k4Vs-h(n6+z=ylx{W?C70&jV)Zu#m}X z`oIAQ!^Zbkk*6mD?d$vP7}Sef((LdIRn1`U-Gx7&=o#-UXWnxN+gBAb8bg zl2EFn_*}4sMV-Y_$mV8N%NJ`-U_IYmzG~AP87MkYafSN*ei!i5^RqY1NW6TZ#nfAN zrN`WUdV{`Vg0=iGlTYo^zF85PZ=EmrT29)Y-2rjfCT^<})V)OIhr;SO=V5$ZbJoLd%U9BJctF@H{7@c@F{eBrs37SWl?4BsOPEudHyJe$o8t zwf|b%N@$w5iF6&}xkcZ}D%)+>Xu2_mj z2=m*|hSe$o2VBJ=BKey`X4hn!g>JA(F8?02u^f+Gh09Dxcv3J%f%? zhD@p2bt&EX`ewcpv4-5vngkj-3p7PSkI9G`5>yzNy#q5k+0Ye)=0V|mclUX%ndf$CS=uY0_t^R=%|eJ;BQ~jJilO`mbfkVK zhMFY1y*!u;OgIXC)4F2guHCbqz4KJKUe|?ygOwlDeYn22DkOdI?Rfo9c%^Wc_(1ab zdL6_I?K8yCIoIFT{-ZLv)tg%>%AM1*Qq`b493-B} zo20p{j|XdoC9V4rJe9n;pQa$umb@&Z-;IhI!-A1(z@n93oNV&(pTcKsG>6LlLTpE^ z3=e1q%T|RzWwU1o-?uAe+&i*KrcV@bC066$A42+T9RN_t2~4kQ=v8*6VI2H5BZ&SO zvlFYEPTO*YQLd8ysQuH}$Sq8g@m)*2k0kO|SZxyryARxZ*3Ln2Ybd=qiq zaN@SbK<2$s`p|T!mu;YKTO|B-2Y=~6I0G9{p?A17x+2yIs$L!0JQrJPA}INQn&qF$ zJT&)%Y?e^79D_Ogh!rKO!iWF3Zx>D$_1BC7Se#i?e|O{H{U1Hgt?S7Z8lyalt?y&n z5I%$-m#-PDtJr6f`GWg*s}8Zl`5rk2&WCsKlXL5QGO9Tu6-nXKw_co5(^GPkDV}*I zxWr+=fUi&^)^l`vne@1eqQq$3doIF$$k;ofCQ5lo`zg5)YXr~FxG>uGKp9)iy-{{~ znOf~1kNvqkG^`R4wD>5vF~%drBXa#I{(Iv@`9cVX@Y+|*e3KDn+)dF=JkZ1ZB1hve zc&4a=Y#X09lXWMeUAD10uCS!WN6=x8eBvt47s6rI=5Y5`AajL8qhwASsuV%#A6L-% zu=OWD{UtveYF+`1d-gkPG4-*S2i0U%US$drbbRv$Y!TkHt-q8}@&zVZu`{t&a{gfO z%|@Kk^>^wHyp!I2%3jFCVnPM?qf>a@Y)tRHXVKKf*9*H~{PWFwd9)fHtXw*$F3|z9 z%n#+KBG~U)-&o#GVH)PW$UgvGUg=&({+3SqoFy8vPTtEY^(f`@0G_YOH@1`0_BURx z0%p%r)&teT|BP0Y`r~K)D5QPdt@<(7?2UlV?DM$_PF-;SGj=AvWr;l7!~k&xlB;k2 zv72LKspNTDzIu%kDKf?3&ufX1K@a`ur#<3AR!Z<53X`4_STw}eM70UNSwF9j&=coDSk*D!01gqm`JohTEb#=0yXpYHRDdoX5j? zuYO%a<_xliq?4z3D1^cj+gA0je`}6_u9V}AY41cEW%jgb^Xvyyz+yisan0w%23(l2 ztLEnuygWIeNk(?=p>(4tHs^2xIQ&m%axvt`NY^n|q8CbHq0;cTO6+Bjn`vkIQ0+5yp$cE5`U%Ead^UkB3T%|sMdskkf} z9QIyJe5FKqcDsWGmn>~E+nC@5oMZEl zIUI&Z;9*VV>iAlU7nx8Y$oIE3DBjv(swyMUs8K3M>&#C@(J65Zwjaii_`dpOtPZ{= zNqRH^G3ou%#;Q}Oh0_0wr=}57Ti(z_SuP}-D}I%vu99@_Wo#1FqpOW%JC!YGaOf04 zvGLm(cUQdzXGgqc!1m*`z-D~DlT%b6s)EM~G&_(~Xf@7YO zyAwEyhoi#CM4|aeJ@d{wZc(vJE6R^^0BhO{*7JVeyFRR$lE3t20-Q6YxxD43QyY-8 z35#^IsNpObrb3eDUm{<$6-YVi^P2(ivUwb$l>|)0_joQ_N)K`f*KeMfn68WzEHWj7 zj*U3d3_`pVs$nwlEK%1umGQi71g~vfHgjY&ODv6?iL*#A#3g@F+wyW&;7Wd$n$BFq zY*FwYsYIm|Q+N$A48EQsd3KkqPqF*i@*sg)LDd6uQ4F#ff~JFiuP zN2V-aRgle{e%4t;xW}GN%OL+7exgj~+YWj+MQ{}S>um>JTZ6-4)z<{Kcm&dBSjGQ* zLEJmveXYk>&Hq_YJsp7>hHnlzy;bL>5(N|NLx8FA@VOFqIi_tHDaEY%7nNu3WD2{o zic(Qe)QTOU1+F$|toy9z>;?o6q+`B`n43z>)y88!G$Pp6g*sZN(TCQ4RrMbBDiLf; zFkm5{QxBb*qx~*XR(%p8{qu9dtdD4}n2OJN2q&CF#$~puLBa*I&Rzg}&7l9N8)7kL zGuJbwH2<{m59zAW0aPN1p1bX0f-~qQ}OWy4TZpwXfiEZiZTaA z(Tkh$$ptDpJ!ik=mdtCv02)0wx`zAZ?b%f#m1*4);QetSctz4gxT&9$luT_n zBS8!U0RLPTA+!%lzr|PXkr?o;1pMH7(fP!SvGv$0EhQ26T72Ax=0uOUrv8E=lj5rq zm|AIfYEP;q9Gz-Io4EX6klM5R4^@bqA9ycz`i1Mb{3iW15NU^<2}Q>L{v(wl(Tjk} z;ELwMKd$z3^f$WymuD}85>MMzdWFURHEHS20}g?O=wWFBYVBOwZwzSw+QuWc$+k(q z|Dt)-F@F0gVR$LuvjFNHQduyyye?_61^qG*J!1ZbB$qPBb?%2vKUb84mWZB#=lIXQUh zpkVETdyH^gDBt|7*Gjnh55a=e4Mr(%tZ&I7(21O!U{LTCS>wtd|5g&9>einBU3#3A zPZ(P-|LVro!LvLeMk^<@`6JCRg^Q^JfWKcz@^9j|Mp2m<0I9x2Qps52P1-95tFWcl zsT)QctNRS@ca?j3Q+8;5j=HDa{YWD<< zJf@-FFI@YzS0B!ED(J^IM=um?R1wzP;1;`8YiqMTLt~bwWc~7IHchM158p!8vmHSx zYJw)N-y+m0Q*S0Vq&3c|DEABQgSiIE9%beK=Qmaw%~GNJrt0>@TAC%(TPgq#41o%X zj6qzox0QUvw`WKuls`c6o^62?uu%6m(=Gm9wA)UCRB3U6g>&HqgdSv>_Du;e1pt0k zrA2f5zlURaSm|3EgVTWkR_>$z4ZcHYemDGww|p$;J@muLkoN(SIsTbEzxjvKA82Lv zd+EhQ4(AZ7Z{XcfZ-K-|1MIIn(2asv0Kh@dZyH@REj8676Nw)$HEI1D{V zJ|heM;k$v~8&_2?dgJ(KSNk&{572EG`_J>6t3*sJrhNf`4^T$D&V?^MJIQGFp1t2h z#=*4v|9z|mQyolAI7hR!Apv{BjlQS+;Xo_XcX+n~_BTNFn#4nIsVWlA?H#}S?5(8H zR>VIh^f45PO`gpAmH?Ap7AW4&wZ~}1E*y@A`Srs%u#aAy$>Y_kV*TK*yv%M zoqCP(|1qiSWF8eEAtrk9(Dun#YO;h&p~%S3Nb+YqcQ6K5v%LTCffPbmEd#N)-?21o z*Dv-B3c!$NdH0XNhRuc74gM`M9RHN!9^rq{=RTT9FUdsv+;J2k`KpQCGt6`S9 zH*#|#oJfy5Y1%ob84TktAKu-c#UupMYxj#Wwc{}j30fTe z{sHSz^TlQyW8vj4h?3IUa>VQjQRERo*s+%4Q$7}nHq=7(FuXm9{$z1iGP@zFb5%_1 zXQ{1=YdHe{)UPLh?b8{aAjUX?QY?Nk?ig&yoA_c1vZy&TV<*dDbj_oCkhoOA<4HlYP(Yj${#WR>vb?8Pwrx4 zvWZvSDR55bRwcty02{#gUu4P(!RbE?QaL}2C^dHNgcuvIU2gXO`kJ703ZNnXytYQK zSu9-ycLLNy2-bN(47$5!M3A&rHNfYz2Upck|AjJ^<>j@LQD!G7(w*$HF1t{$G=jt& zAmR43AgRtx;Q1+t_8}cSygtS|)i}EMrH!uhkoo!=BXT-v0%JuP-h;w9tm$vjeam>B z1ZWpdCYEM8*^rw%1+YwY#rwrNSQC*!FOX>MPt(3x`j4LjA%=pF$(-(Y7bLl&P3RZ= zNc{nYx8A1lJYiPL2dFiNvHU$Qy>Mg`d!}nQ7BR}>h~*k?u8wC*B30P{|48IP_B1#< zP>P+8P^-cELj*LRjXlxGg4w8Cmg0!|pGmTu?Icz{5ucAH&Tft(rBQFY1?~s{SPcPT zOtZr*PORF^l}pKEo)$N<&ste4udUtD&R@D$Xvdsrq#>iH*^aSP2In_p5@=N#Up=wOJm&l`)uJ zz*M?UO?&nP(v8Q6R7@9%3ivCCAOYIeHR_~s(R2{7Q`uBQu3I3Qj&KpF=&+cL zy#ln{1-T#eQVIHYe}JmLS-8_oufO77uyX?Il;|^)fn;fp=saLlbq`9N?7pWNm2MPb zx7s89_oAX~$Q!8aljFrHH>-gQt3qB&lw@yn*Z;9Fm? zPIXN`jRwQ7j;-ldOJkdsZ4_N5Z%ZXBI1jz;3`wd<6)H@G2 z?vAwDm4LUm2Q0IqjRokO)3{;*6Lo+$(zI8X_h=g(Mfym}q%61}xZ9)KCCbb`7_y|X1A$O212?QX0h&MRqeXNe_ zHCZZ$AAF5E+&G7Krp#lZ5IyyaNt6~$gQ;>9u0&00G#+D!!?l#8ox*-rqub|K{h!S{ z8{Hgo;hB|Aw7z?S0Y>jN#vo=7EiIOvcm3$uCm^f66^*xPl&f<@n*)uq;HUPT501wa z*re#IBZ2tFKd0Hh-|7Q4+VV_(1~lsmErY6tGF}BXy4mHzMT~3014-Xbg8EqSfRLw* z>puD0VRqAkFKwby(x@DmZq(Ha>t*CH{U%G1h|WjUli=Ya$biBjQ)83c zsDMi}w!4OXETrf~qFnX1N|BSn!8|oVIOS%nV~_(9ddThJoy#fWJ`e$t3Bcndlf~d$ z_?ePqxtZh`1gfB-tu3wQ=EfBBFc+RY?gW)PiK1E1Qe$niJ46RGExN3$6r|1Fqh}?DcW<#(&2*Lp6hFZXkqATE_h;EC!U5$fhy=%N< z5Q{{ZLbh-C5FE!CKLn~v*b1s3*8>mJ)ow*hXm{6B_oVofZ-Ja=Ya&{?-B%R*Odg}n zh7MILZaYxX9_-`0iEGwa1W5B=yp1E)j>)$Vr&*#^^WA>Rm*eRTXxn1$kzBTiR96&%<@M0WJb&_AJxcmg| zfXc@l1z-Lc30!}B5PT<`u|#o6ij_YG@e_PY0iRAvx@Y|MiEDRUbXGnOb}tp>;c4bg z@8@5Q9o8g|C6tV|*m$;|2r6WFQXhs7U3XS%H_GcN6*z+^rLHe#>ZLaaJfkdx#n6H1 zNjjW!cdE*l?~k;l`O~ADOCc^x`N9`fR-4<>!eE6So{`1oV_i4;zE26K9>;q%w#T-8 zEMh~OpaH-uE*4l@{p2W)(pPp1;fr^tY8Nm1e8P%Rig$!Te_aE>rQC%cWa~vas*{C# z2Y=QFpMUrxd*n&>dH>I8Rz#}|;m~d&gVap>kLwEsibNYja1;TfmPKHSt-S)pQL6QR zp^nacINuQM6ON+ozO%Y3P{15(xru5(?84WQi|CxR7Uzp`p2}>pp{+t;d(~JPgs=Pg z50Nh14KFfwvD9RKNTF63@5iFy??iuH zv+3`*O1$9!+q&@rD-7{)Qit0!*%|np74#_o@JzQ+< zHjq>98Dgf9i|b-$^MC041(fCfHFy?_zFMxX zlOvsJGCS4B1yI+_aD7(y3*TaR*QeCVBEJd)>cl zwc2lnow;ql-#!516SArGqVX4TXZexYv^ zTyEc1EPm8XPp^$1VE??tTf}TvOE{Wz6^UO`H0oC-;l55(HX8`0|L-L@;vX3Y)y|C9 zMYU>VZfD%dGpet7e~Rxht&v~@fYO`}s9{vsfOabvA9_>h866U3cUfp0{V4ub*>+w0zCS?h-V234paO2j5 z<|9V}_PENf=TH{&tAV(yyuJA9^ZLmuxmcTGh;Q%6uciNC-m~L;5MBzy8XVKbZ+VsY zVJPyA6!3y!**MS;f~b_oECyr>KMpkUdzXQT1j% z0@5VX3Xy&P#KUA*sf~gaPgFSP@lA=?Ld>r5s9+6T2v4gXUip49TB@gKZ>5wiyoPq! zjNnr&H>B+Qv?$)IfRq8+Zu`r)1g2cKgr$DsbV08i2 zT-jIEW)DsBRhKnc`3cE?i--XA^_BI|eoROG419Aw@lhdD4<YgPqHQdM0hVuex)FMZ9Q!kC)5s&7auf6 zRs2JXW4mjiO=q3jj9%uxrycYoTxOv|eThx|>#;5lKM7-k_>0B%MzN4ydKL_qe{7Ds zDfcibFXX_DERUc`_p(0_?Eyj>iWjSecsXA)u?xeJ1++hW=wrVAGG_yh(pX=fgB-gi zAlAwEfh{~a9|BQYv?aJfcS?Lcm-Lf=Apz!T&S z8xyt*{1x7XEgzgp_@Zk(V$Vy+D9Ywpz8QC_NP|w*N^HGDexip0)RHKL6Gz+{-%0ds zvzpoE>*xtZV22cwO~ogOAw&x-6Cz?gJf(NA_>j_Oe<5E+X?Lv>JPP~77WBdnd0T5b^Ic)o2a5J~xB6vBdyjj&QUzG&ZdSK~KnN7u`KTBAFmfr6h;tf!2)edCErfJDMo5Lqu-FSf6`Hy!` ztxi6LZ_%1>fMoW^Paws`d!sIgEeiPsdG~SePXTx4!U^{v#Xp||6`ooYu)VAOA0hEg zq#;cJ>+|RMnal+sZ@HBS zKun!*{vmi`p9Pn_Vi)w;l;Z^V^vSlh&FqEHLU0aneSuvyR}S3YKghZm(D{n_Bh?QJ zX-nNFHE=J6HaZaxzD@F079ZVL`UhHk;NkR63(WKFBHLfwoN_~$O=__>*J^3}2z@Rw zR;y74PTrAEvyfy4y&8KC#--X7|E^6sXA#G&tLAlnbL*4z?BMKxv&XbTtQ(!&(BloQ zsr5lN-j6MRSm4&>m%TJTxoVe{pS4tJ4^eziVa?m$``;QUtl-C)t*@=vJQuX1$X-U( zI??(^u3fa--~HX2#|fKK_VM*iVAg)oT~pNcVTE~<SEk~#Ang7O101PR?8bz&bYf8ib2o(--lkzLVBV+C5 zsxaWv%iRI;@%eMr6z^j*%j!|4%+v) zQbI~&CfW#hWi9K7hhJCfM0L>wT)eVy(H!vf`|O}H`>D)StsV-#1##&MCzFA3n>Uba ze|`8?KzQ`q1eq|}XI#Q}V~bUpYB@_@6@eBjNuV&0?J$jYTaDY3TE}8t zJB80=F5T2F)`!;9(vHQIG`R^MoeP6UbdIy(?4i5dfwSd5GY>OZ zS_|C^*gG^8B#%oFWDjOFH&)gPS345q3`8a{BNMzz3mb}99VfOq`@+r)#_yfuvp0S4 zVCwO9{#J%kdE+dQ*dZEDdloun=kL2>b^iNDi9xBq+H-H%ic4*LnsY$EC#&RT;X*h1w-9IYm;|QKn6-bx_v8(9B5uI7Fz;Da!66dMRsHHZ7!nE!2cWL6rDnu>cWLx%vWtG?Brc#2w7=0KDx3!^TE}QCC zs_E+-=eXqOZiu=YOhd?1qD(s>C88PvyO~ZJ6XXWMwMxRh>z`>$~KnWl)v#B+;l}#tkU36^1Xd#9z=L-S4ea2lfeJf5c7xNtuFEn`JYPujCD@w$5bOF#A!r&SoND!gcAvRmB- zUH-o-f!PXpHFzS7hf`nEuX}i(*Ds$3#<9 z(#Dp%)WSPy0Lw45Bo7g4#A$UQt1vn8dAsm>GTi(kCFp8vcYWm74Y4tEHft;$D2?7;$9p+8&0|f%?r~oLy@Bg+rig~A#Vq#YSj6{%@P)0p+a<;WicyHNxbbEG0 zYv+*vHwu!caD+E0auq%K9bbKcTd_d-W- zE1{t6^l}0gi~>^G1wJLnOoG{MgHBC=>6wxynA@N~gN?U3C6XZ#y5g$wlYd3A0TA-< zW+BG+Qsz3VqqTFVYCC*?pJ{!(=T>9%P zb{-Y?%GvcbDgz~awQJcwsSS&lwk9KGWu1*t7O)6SA?dtpK2 zc;M1ikH+M(G=Ubh6+JcAA&Z*}A4ufZPxqlGR&&)W@ ziI7tE(s>*sqQae31&eIWC!?gR!}bIMAFltv@(NR}D2k)EIXvg(d`W2u7ptO_DJ-gy znV-y!`bdzJ(#_9S?`-dEDd`r9B5rcUY7O}VBMofFl}oV3M}N&I`~m&tL`0^D)O-W0!T>j;U{|Eao<46Di literal 0 HcmV?d00001 diff --git a/README.md b/README.md index f379cc16a..53a82499f 100644 --- a/README.md +++ b/README.md @@ -45,6 +45,8 @@ By user request, there's a [discord server](https://discord.gg/ssa2XpY). I'll be [Substepping](Documentation/Substepping.md) +[Continuous Collision Detection](Documentation/ContinuousCollisionDetection.md) + [Contributing](CONTRIBUTING.md) [Upgrading from v1, concept mapping](Documentation/UpgradingFromV1.md) From 30fb9e6f4e93997b7066cfa3c6a4f131e2f43802 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 4 Feb 2022 15:29:34 -0600 Subject: [PATCH 429/947] Pretty pictures. --- Documentation/ContinuousCollisionDetection.md | 8 +++++--- .../ghostCollision.png | Bin 0 -> 14453 bytes .../smallMarginNoCollision.png | Bin 0 -> 9039 bytes .../smallMarginNoGhostCollision.png | Bin 0 -> 9050 bytes 4 files changed, 5 insertions(+), 3 deletions(-) create mode 100644 Documentation/images/ContinuousCollisionDetection/ghostCollision.png create mode 100644 Documentation/images/ContinuousCollisionDetection/smallMarginNoCollision.png create mode 100644 Documentation/images/ContinuousCollisionDetection/smallMarginNoGhostCollision.png diff --git a/Documentation/ContinuousCollisionDetection.md b/Documentation/ContinuousCollisionDetection.md index 73ff4b25e..5f6f325c4 100644 --- a/Documentation/ContinuousCollisionDetection.md +++ b/Documentation/ContinuousCollisionDetection.md @@ -36,9 +36,9 @@ For most use cases, sticking with the default is a high performance and high qua # What are ghost collisions? In the solver, a contact constraint (speculative or not) acts like a plane. As far as the solver is concerned, the contact surface has unlimited horizontal extent. This is a perfectly fine approximation when the contacts are created at a reasonable location, but it can fail when objects are moving very quickly past each other. -TODO PICTURE +

-The ball smacks into the plane created by the speculative contact, sending both the box and ball flying in unexpected directions. That's a ghost collision. +The ball smacks into the plane created by the speculative contact, sending the ball flying off to the side. That's a ghost collision. You can mitigate ghost collisions by either using a higher `Simulation.Timestep` rate or by shrinking the maximum speculative margin on the involved bodies. To shrink the margin, instead of passing in just the shape index as your `CollidableDescription`, provide the `BodyDescription` or `StaticDescription` a full `CollidableDescription` like so: ```cs @@ -47,9 +47,11 @@ Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), ``` This still uses a 'passive' continuous collision detection mode (explained in a couple of sections) like the default, but limits the speculative margin for any pairs involving this static collidable to between 0 and 1. Collision pairs with this static cannot generate speculative contacts more than 1 unit away from the surface. +

+ Using a smaller maximum speculative margin means that you can miss high velocity non-ghost collisions, though: -TODO PICTURE +

# What about swept continuous collision detection? Specifying `ContinuousDetection.Continuous` in the `CollidableDescription` means that pairs involving the collidable will use sweep-tested collision detection. That is, rather than computing contacts based on where the bodies are as of the last frame, a sweep test will determine where the bodies are likely to be *at the time of impact* during this frame. Contacts are then created at that time of impact. diff --git a/Documentation/images/ContinuousCollisionDetection/ghostCollision.png b/Documentation/images/ContinuousCollisionDetection/ghostCollision.png new file mode 100644 index 0000000000000000000000000000000000000000..101b8b5902fba0fb2a5e5c6e153d9db75e05f537 GIT binary patch literal 14453 zcmYMbby(az&@Q|{aks^#6xZSoixntPtUz%o?(Po7Wzpj96ex>36nA%bclWc;`@P>e zf9$o_FUdqElSwl7op7aZ( }C;$KeT~z%G5>fp{( z04@mbzTD|?$SaX<^t9F4V%b<_zFX>6%+10?&}*5MSJ>EC=~mR^cLmUE;Y+L8`=i7o zaTsHYf$m;9xep-dr?-7E00ICwD(LIu*wXkzWgj-k zyOtXYm_`1pnZg_uKzV+1QxcBQgAOgXm7o6!88`ZWB%NK9m9ZHjG5_l!q8ZG8CFi-g zl^xjsH%(fbBqU2LCc=WE$#>)9ti`oS41`nI`dMb*dWw9NmLc{A?leQO+*TwKq3h`1on<2<3nNjIp_Ee1E7c z%QKxc?tA`uXbb~p`IEP!dC&8iE}MqEGfPzZdOz}Ykvfx`)t+C-qHmtSu4CYAe!0CR zvne+KV;#@Gw1hsD_?VwxOM5XT3qvlYg`1jDB$OErR9{-2f90qG%>FhA+oCA?CUQ|) zQBj&1U7D7LQfd^SpV}ZJA(nFV?45(w5>N zoQd3=S$J(RUiUlUTq5xT1mfax%F3urF0mbe3ywFw;;!MDnN6_K?*_S)TwLGD$SPQW z(Pbm~h?RZlTiKGgimt(oY=!&`6dFWf7}HKEAO_@?lniJA`CR#FkJPdj5spX+<)xKVC8)${_W^Lm~l4KA>Y0pmF#vm9-cu4YRaln^XH*WeFr zN`cGK^N(zQY~*ED3@ytTd3cHh-`#(xD=U;yOH7cCQ|@59hYWqjYxdhPML@a0R%U`5 zz=enZt|~FK^$Egke_@?iSBJgkBFl_|;JHM+L@X*h2N0Vm1+28EXk^3DM_~#Rh7mav zKf!}!_)rH%PV(QSii_JA*f}TZst%chos5xfdCK^ zAF8tfsqS~l1o*ryh(eSA4XOdclR(!!t}LbNr2N8cz`)GxpbW4RQQ;F@CeZd17L1L} z?Nr23589>Th@^wzg)0K!0I)&^j$`%*r+MzL5tOVeFbOEXByiP74H|EL5R1kDP=3gO z$9vS*Tu+jm0_Id+e-zu#VPK#!ah*zJ7LUQWB175{G)4lYmzA4PqZ(M24HeAre^*OX zaT2un3N(~P6yEdrh-V-1O!aZ^kdoHO-5W_0fP`!Yvpcrx_Tgh1b4~%Zj)+%Min0n- zj{@G;h=^akNz_=pv77F0aZXcX7$?StuS&QMqg_4g3SaL;9q$Os4R0i9T;qWS8+tV$$E8q zY*bv9S5lG#1}EM!)AHXdkR-5cXlRfel_h|XPUd{O3)}O0F3cP~(Gx){(tsQ?V_MV+ zEJikhvlt{3nqKmlh#!pNgF#<=|%6iH+76nqU0OK#3MBKgp zjp2DLAPI4oEB@NLnL`Yq*3!`z1c5fMC4q%hd|-00XoOd0nyTh(t%aIbq98?lv z43?M}>hGv5P$!1I4#`lW{mi#TpT=^Yo;AMtd$c-TR0a`vxg6GzO)WIR&yf1@ZLOsp zqmLQ@GEGb#`U=d+<%RD}N}_6f*HD!h?OOA;_iK*N+nPS0Hr3HGRJM3qcB?NE==0oq zhnib;JkC)U^%iZ~WS4_&{bCbU%Rdb%&B?hX_W@9IO#PRY@~DR%wB{soO$2{t2G%cH zTND>K3P5+iDAjLc%Q5m6S^5`eHZbEZVvXenhDGD{kj`JhBmXcPbVc0u@u5Q2?)NdC zLUE1ZPz~DLgf{;uDBO+z;a!lqyN`!{bJ7d;b`-ENdfhh5sm;)(CQ(NwXufZK`V zNQ;VWQy`K*1_N^exgM)+-XQG4zTPXuX|0{bdf%lz1VLc02!Rh9oWdgfNl{U5H^^Z| z$|ERq^sa)m$?taHhPO9?=Yx;io~-4V;Mgz#tj>pe5b?hAdjR5}TiHDRBE~Sdh@W(t zp)k2LrKGFC-7bQ?fgx_`dA9cm$Nv#=Rcqa>YXkItIP^7w;!*!n*i%&C5*<3CnJ zZ8L?Dk9=_^FxD@C^jO*n{k9?V`$t$hL?=WqCmnibW*?&Lv-e9SFI18BxYG7M%h>i@ zFJV%aTkn_^?Q5Tf)qSC8BtSv;aswn8{;yPm*BJSvu!zX5u7g)4Wv4lp)>-;LLQ}|j zURVCJW}KGqj9QXNT4&>5r@n_b9%)u7wiO~qF;yHG%CHx2V*?P>J8d!h`v}dN^hT{I zX44g!?Ikb<%+Agh7MSku?NB^ACJ%aNrqB0fic69W4Gj4`G3WkHvh`n|c@r+# zC!U!>akz5$VHX*c9o?51_D~xbP7;U#FJ-ce>fB8QFuLAD=is6?xjgKl`~8zQtAG?U zdUS+oa(a4ha&l%)MdSO3QNj>d85jMuY6xp*v{#@(|)kH z>>w*jE1BP&`pQn1YmGGjWJPbbEsRi82Bv~SVkXNSYN!A&s{LP;D?3qz7J|$;uwG74 zS%J)JY)I_&?yedNS6V{dTG$#eU$Z!);E1nGaCW_jD-llh zv;9p5e`}D)GZB{1At2S>-hr9Kr-nusDd0x(b$xA4V;-dWi-MG_zO;gVOWiW#b;J;X zA8xay`g%B=jjUQrM$0oJCG$541&ceGV%Z>pTE+=mFnroq84xb5(mSx>8xeu|uj;Ij zgd{ySK?%f2+lpeq2uOO=F8q0&SFe2xXlK2pJnmSLB?gMI3%wU|3i217%0+}a> zn$z8uZ@T;^+OnPmj)w2`-;aK`#5wRA69IHT36nuUUaezC696&XCjrahqNd=p+k<`M zKoWSSJN*u#)UU}f6N~o1zqQmR)$v8lo!*%p#`m-I$`rNuc;-%D1T!zw1)LhY*LidrJ2wYCN~+9#O5vJtJp!L;$BIxo&=Kxf*PHc*!DYt`jWDG)zMV9Dj)OU% zuF{tPnim&N>oi-G7CB#KEp}_Id%fjc3!0WNGO;dTH^b9%0P>RJ14l-vH!qD-ZlG)W zxnH6py2YrT2iIp*t)?U#Era>C{>As5Gi3k}_EiN3QuE2gAz_GlqW8NRo@iPpU&GI* zVCcG(FtVSfd?5d?mk1F5#I&Qe*98sS-wJL`M5!Fx<*0r-(JbZu;b@>1YFcq1R%*dW zW&4#~N@TBFcFa<>wYib$W&ygG=!Y*RdtH(UAQ@!jfv$kWUWKEY&O<-VWyRox9MoF~ z=Z!Ac&P?(kcU@gg!-wthPH&c*m9HS*-hK%n7STC;(3E4OVsKx4qm?GHMb)iiOY`{j z&=0R}#fJHJ_vfxic?g=n`UdmHfy~erE$TLGdnDq!PY^zkrascIZYGI9;fx64HTQQ7 z7#f)O*6^%lF3F*Z0CGxAm64Ld*(2Kp|6(?<&Dm{n4@>wUG;BX-AF7(Z{%j8W>A^RU z&WkGOC`}960s=EIPj!4L4hsT?|JyZY3JlruJNndr;Hxg`D>7%+m2=R;i_80gto`Cd z!LV583atQowcwu|pLFk^GL+_t_S4XMTr0|fnrZu2g2kVi0nR@;KB5j_3v#YTQ=m0A z+tmleJPp~|c|Q)X_OsOta!@uH{Zt;$Dqw~&{)|dpKp?ZUk&#t@G-bi6aqrR0pL)Xo zYX10kJfB?vHUVL@xd&uq%4)(67y#Wh)OWp3-XaTxaOBX4(LJi^I4(#x-S_r}?_b5o z)?z*l`@akfiOw#)Z_hT;i%EH+K&eV zg$Mt(P#|antq0~aDuyP%s% z`iHYiKGo&>J0y z>3brc2&pvYRkn!7-_`I5g6r2RoriPi{x0BO&0l|pHamvZ0pU2}GF)$`PDHzkhi5Fa zl#wNFHSEa^iGsu~f@8z8$56f=h(;eYqW;1&wOr6|O;6@{>Of{;!o4aG>$2 zn`0dO*7RDXpmXGhsqVJlk8b7Cr+Okn{N?6bPc?|+U;arVAbHqhl3d`-Qi_{-z7RGw z$3uF+k<1(BrDi4N?aw!#aOCjKwBgpJB;FPU@PF?==v@iE0cbhEMv7TEZ#cZ>ENsHC z>%9^`s;=LfS(~rF=?HD7f&xdhcILG#F69l?E@vg5i6l0=_Ia_q>k# zrN(NicoTJmq)q*k9qVc_jXAQqoo*_IYQ7C8PBIy0@OQSNaJb~ZZ&(J<@qZ@4&CJ)A zKTzcTSk0BrKJtc{Js3%Q+xkK|*uB4{&-+7d){!GF;37XL{*w?XI;qT1ykranV4BCq zt$~GP1ho{3xtL2`E@~bIJLu3wv;C5_Ck}CmjQ5g+j7)xs$cwzMmosh3HvXKd8@G+$ zo9qfh;xfl#ki&T#g5@ZU4Vt`*4JCi}RI!knh3eh!dcXUDYf{nJ9=aCJtDh5i5Li}- zJTI(gdc9=lKTQXAC-}8rSRbzW_5VSB*t*b7F{?mtWclp=8CoOpx7>mSy&;&OA=7n@ zJcjrmUlY~8Ta_@zfzKoP0%Y#(m%CE=r5QSsfdgTuhhYIMKeT zkNaz5G{ueI3x^jK)JMX0(Q+*KFjDdGkN0*pigsswNA6x6Gf$O%f6wD|dm|Dn>-2li z#r`Ct`ETI%VB;k2o2{_C@mt)^t5?d&lkr1tUH&>=vI5j`oS*j(R(vzQ=$~CEk1au- zq|!`{O(n|sYG`3m!kjY2%UWT-E*dMFl+oftf==>f2yil2YjtMp5HoA}R`CA6ltH4?bFNXMbtWO6aa7WOiHoBBgdhm+2oO`ECb#v03#9%Q2!qd^YwfilfJ!87oSbT#n)@Jb2I8w>#6T zX-rNtS`=iJ4VNy+z1}<%6B*p5=`TCc1C;Y+G_OEFu=LPZ6pFue94QHgK(NT$_|<) zhT-0$umY*Vab0DpK4(G->8%V0Bcs35j)rLs(22mfYUMrq*eg0@NJhI^$m+^OI z^-gexN!m0yNHo@i04|x%zZK#TG;<~g%d2rj3@zx0riNdZXY)huYClg+w_b+5pUU26 z7k_D$p$oWp64ci5qpac;+Y0fo;pAqeETu%{(&DeFN`p+fnPq{0j0qW=+&OSq?NUJ^ z8#IwyGvT{#j}7lGG|9`7Q{;wA29X4v@8kMY1vqpq)9ob;PJXB)%YfnCA6Us>!jkaa z1+#h_w@pyi+AJ;^Y%;iny*TSjbYs6MX{bSJ=G2DbQl{A_M=I07FHbH5uv3x>QiFEk zUT#jhIK$D^gQM(9oT29PJwEI(cYoxi8yXV&T{fy0S zNqGtc%`q=Ob~oW)HKs>ZCAXj5l;mXBv;IW1>iUFs?7Hof6PTa=+}TsXq|AW;&V?si zN&da+gjD(&!5Rk?L3gAle&Pg>EwwQo5bt+Fj#{Ite+vRNR!DAph|0$f5(`)%izqU4 zPLI!GC#DE%r7~0oY(_jHxs($@WnIrCyshURc}^JwJ)YB-?W2qQ4sF*M>f4zE=vIBv z9y(vbHb;AAS1LS<#{ZB-Gud68$=e}RZmxPY)n2ey>Pl?)^NxjlyRb&u_ocJUGalpE zS?mpm#u9US3fo7$RpkTgC0k`W-disj;b0EU8S^>RaJY<~tdYf?&qFK%iI!p84(ZKN zpC=*+KE`(T3BE6w%wbSu^@(FG&3&!`{LCWIQ=DKpf|sX7W#@V3ePK5n9XUvL`>c*N z9a+PqQu>kJ|9$Dj)ma=~=v^CX~1)3l@1C zZIF(?E7cUDc`{L`-1JNX!ycstir`ooOZjf?ew&(959DaUxU`)_OE)b>lbDNolx*K% z3k&u0jb~0+tre`H7T@U#Oxx_9z+{h-)xPC_)MqB$yY7hd&5m+~0DFC|{>Z$%UcFB? zK2oxVwZ(`up)bn*+!sb`VsmpPLeWpPji{aZz6SvngQh%# zCYk7-A1WB=OND%Qd;P|j{jzizQ%Ec)ZP1?h(qbrPe4PE34SoI~-a?!U({EirJDk}D z-@9sG+XDLpXrT%gAQng~J1YD7HA4K|k&&@1j-yF9H*(^Y|J3V_9$+S<7*-SXaNF22 zW{Zs#pd;-SAgTA|yse*s$NAZV>khjI83MBU6J6&+fO#qGs=t>_Yi;r;Q-7w(2?zvg z%vo}Ip$g36m_i=NmL6G+c(KyzE0FW6gJq^uADIyR)j7c#FrF<+8@B8ak?s5#@; z=1-*y+rM;?AfROt(*7UEyAggV)X7plC($_5>+v~G#=Gt1>>CO2FAd7w##y&!8!6gC zZs2O4kazx-YC9dM7VeAsj+Xx|gv@uYuty%(8u??^nO1q+dhsL>DSY*|$Jki*Ia(JS zNa(uB@dS>@1U%ynBzflH)kSBZTL|@s56@`A-Fd*=A;ZsMj6Xq@OwvgNRy`vI!+)4s z)^lWoB|sMB)Matm-5D86^Q$N+;1Dh@=;Mm)S!rDu$E%_c+IH?8lo6S&Hp|wcyQzOK0J|qz+ZaEc4caEw~w&312 zL+be!Ud%jFeYhCHJpr<;*&aI7ShetgP4+4F_@pEa`lrc&w6tc#an=6j;ogjIJ-<-R z_vBTQ-Jus$RVAJ3yU@Ro2uqw{EHd;Ysf0{BVeHogsef72=;t=JKXavwJnYX%jbTz} z73A>Ua$jSH_qaaD72mRYYXEQK_0QYdaHiHoOpE-E7#uK9&m|iLFSg**IDQ4=RLRwV z)HF?Is=W-g$2x|Y8=E&rh9gz@QD@T@E1>}rh`_HojcbIgj$Zj;;OH&3R4+ck`QHlx zbmtkFBq1aeadI37ez1QtD{7B^j0lIjS<3mZ2PXDe6E(ImAq-8QrP%A)yYM zvxdAW*3jV%)$V6LLXUBZoAHiGO^<)8niDz>nlyUj8OqmmoVQzRoo{z3Uy?nX z$3GT5>=c$%+p0&dZ(omyYXV6kRa7Q=!l@GxbQ9q#y{>h5dn;aS4p}-3aAacjTtf<| zBZZ^|Z1q0VZ+MQJx}pQ2+w1$`N&X{l#&5-QiLxDO5Fm{+G_g=p!x(1nSMm9jb_*%Z z=2GNKFewgT@bl8_Qx%IUBNLOK$n%9oM$Y!HKRdkrf%z(&SFwKF*u^+1Od4;Gh#oFG zYjErb5zJt^PP+q}KxZBOj~XLrT=$iP)xMS5j#_JO#b^4ciZb$E+G?tD!1?m>a=B5) z##_F};u=hfz5>>a&Igxm_Yeitgvi1A7PmME2Qf!5t}N0A4xg9nibH8&pRQ*z-ksyoIK?lDRz2k(JdVS)$NKPxi$u*VBo4Nd>}HA@$GI54 zHL}Sewylunp-i%m+jk>lUa9!E&we%E-q@bbLy2M&=qTLZ-`dN$)wsJmJkO7wIB8jv zeF&PeUoRTkQCAh7f9UDDnH~@%99bTpowo@bcCI9b?c}uOe)Hudi+6JR$w_;1+g!sb zDXXyPvl={{omKDXW}kKH>bt2t9~*isNT~nLVc)Y?iZ5k?jc6zTXrXs?%D1AF=HWcK z-Q9qydfk7Pg1_e_)~#eUQT#LL_xq#|X}3eX+yx{7PCa^#V-|hc+jrzH^_&Bh{MFfN z$qsdkO`2}iKtcFL`Qe_|zREK3yp}{C@oLWk(%tY z!wc-}7qN4zUpC(IvD-Fxq*I5gIX|j%Mw*e$XgOMkVhG9IX9ZNn8J_bZo62{asMOYM48L2N<0+dFKP9V;?i%-ElolS+13p5Q%!xT`I7POT6>(7LFyuDz(5$BYv{5x?fr^sc7TJ(QQTe`z)M z!bNW|-34)OeW+GY0wK(!G@g-g_B68dpf5xssQ`D+8 zHG(lB#JQ$6OfqA`nfXSg8y788K9T!7XLeD_U3D32mFG1Fs-3Qb>1K<3YpN@ss)mMA zdg?8gnx-neo#|4LBpFkiX}U$wZ-^iFR!&uHmXMgM3XiqCwi|NdIGtyhw~6MFOXbC$s(XhJVY_3pX3- zfXJg<>(V%z`#F6TVGypiW*E5a$Qk7Do5o>3(sk%!OAMWY?Qh**v)R&CoTezXzcs0H zd4Kv#ponf11$FDkjauyHJ1Q+753Ox_bf9>e`}ovwZ`iDal6rJd1+F(p zE3J>9$Jg*N6#cs0IbU~sp?q$}Y^V59pD84(%SN49P{#u95aqHP4RK-cnk%#>jBp&V z*99rh;C{SMSO<@Y5pWou3F@b_T8phhju%oCUi?}kpy$qfV4Vx8ZE}~*G-<& zIw25;IDpUZEN%R;Ni`|jZs=c&&a3uF#D%Rp>YAeVC?AJ<$Dr@jLl=;c6s^$p5GZP? z`qF_Cbxxe-t!1mw-iC08%g@wxOwohVCVl$&GKcGU^-wN*^7~Yii@P}qlR0QCQ$k&R zU^F^->Wfy5|C>C4v;BI*-18X?p`2_^tI5P<_V$y< z{4cEc_o?_k>|&Z{zLcrI<^C-J`vyKENJwKp40DBIvo-2T+P9B(YpX;K;UlWE@XNF@ zuR?Tahz((IUTQ(=kVwe(+eIOv58_XsW{0;z`Z2vp6?C?;YMY#$6$0XG;Bz=3j z0)s|PpBF+~5On<3{_yKoZE~lJ5Gp+~tLDZu2jm?VD;RbpLdP6bnlIpnJ}wF!?=0c1YeqYoos z`)v}h$EZQZ!O#84^Cy=H%o!%d{!9-13|RX*%`--H8{@J4sk+uT=1)rN)1GraLjuCb zv25wBJ-AZqz+)D9>-F=;YWzy8Cktt9Lh6lq@JHdV+xnR!+TCjz$)c9H3L0znWc8g^ zlxf~xw>R^O|JKp!-eM;wxce@d?pCdmd+Aky%vuS!I`6I(4aCMn=N+n=ptQk>x}fNv zdt{-Q198r04p&7b3Gk03OXeTtaFu<%UIzE-g81NdT4hGL8Al6Tb#kUR=9on&^Rpid z{e#OhB$&9refeG7O=do5*O6N&C#z}MgdGj^%=|8_J8MFdjYvyJLI2P?M;ug?QOr&`!(=&_1``&n{ z^5vB?e>9q)H(`ZNI3B#u=WOaA<_Q-{l3DkQ z?TZp)Wu;E^{f~QuGX|UO^JRzw2I9|m%14xSS(G0O_d)k)ULOiO46J4b|4)u)>VdT| zZ91mdZ%stZygDmcO?40us~8?PvqGWvRWT%iP^R%`Ke&*FSTH#l0t@j^d1NNch<`nk zJV3>Plf64_fXricQlf@sujAbRO4vEMT$?)RK{hrGp;e^KMGIi2^u6HwbokeeO{*a$ z(F7IBcKTqyg9NGnl2Fp$Fho883qUfsx3y%1A%fQgL99iw--dhlsR1}!p%f)d@Fr(7 zAh-e8c#}Hmb!hlF7kiKz!AL_jsQnSaW=T?9^8-}oz@pqncdPw&A&5ZMP6i)~Ed#bM zRyqhVK?x`Z1oQcjMYU&%e&)cS{S*giYBHZ^z3C;OPE-cNQ%~OH-d)oPBmlxD$1-Q| zy2m)))?uMEEf)97z-G6D<0@JD7D`mWL?D^q#NOz@Kv*yY72=N<7FF(dM|r>jjhINs zPW;x7S1N)wWV8Hbl$J^n+GVBl8;itLhH};L zTIig87r?<$_zL=7Rt9e1bqQ2@g9b0Z#-=XzS9Jy#QvFixY;-XH_W@@JcK}w!;yC^z zjG+S*{st2|E4lMMy}BK(eQbg%nlbgr>WP*7#O+1QTMMQXmh5s?<2wpCNE_iHkeRXq zfn!7@{q)FuF}f#b6@wIHX?c*KC`0cD?Q=CWL@kS4t8GNZ9X@cjN}VVNhWGbgZj5WT zDsi*J8KN101;vmFf7PYjI8_k$fEvP=7a%EFCK7Hk%BN3oHVYtO2bnXdjuTi3Jja9nm2EQ7Hli&x9IQ>pwAN(6ro62)S7seFTwrNsM6@s~t@# z)J%HeU=*QQ@o5nv@1&>eisDU(nxLWwR$|E9aP)^98moB~O@oEcI77k%upCmlYT=0> zISev!509DeR5|3vFqDdU`IBbZxj~4sNDELMdt4nB&)oj=c>-hoh4xJ|luc;a<>(_a zU~fApD!Q~DumK&2DNFVw9qa9Bsb4B}veR!iRWn3PGOCXou$rJ`8(pEDg=`t2T8g$hR|50$RXC=A+P zub=49pjeA*92OUxCSageyP+f$l~hDtyc5bQq-;S!EhP%}eT3)dk0bss~3>3t?dN8@0`-!=wvL64V{&XdLtzF9N4vbX`RVwM>dbjbbPD4fi z*>8HtA3b_^U*5tP;PgO-LmheobsvB%n42HhMAjyBcI z4Ufv}Cw$;QF7{VQFDZ_N>-sNfZq=$ID|^SR9YRW4lKLwE>N@~L5Ic&9=)x>+gkTux zI3k?(OPKAqJ|O>>InWY$k0T>nChQjjWCqbGQh9l)z~AB%kgw;vdOCv)BCexNab_Fg52`vQW=ac+ zT$m}$`Y>wwd4OpMFB4lTSF+V@gqsOQ2*2p3@MKpkNGon+ke!RJ-SiJF%41HJ7o?G5 zM}^$zM3@;B0=}ewo*0|cK|Ft99QvxJ#tl!iyXa9UOJtW#w*%$Ld~<4`I5t&yi-OXb zK#m+qu-@J!&+s2UXy&MKtz=O3-l<>^e&=~^SJ^j%nczO_H-3O3JI@5T7lxvYBKz}^ zNq?oYqQUYCVG)ZZ0*Oz}fMjxQ76!4J3zrx;8 z^0Ks@7#>7w@0J58b5M^j7!g;o>;jq9td@UCQd4JJbKw?lNSTT7m)`iJ>B6J|3584d zpX|bbR*=S6^+(TVqljenPgR1&K{vIEHXB2Yf1*}M?3i59ox`zV{ZVK1Oom%QeGrgU zGA(T;LCU`bYAOmom&3Yny3seSQ;2LFGWfr*hKOO@P8cLk`h4P{MgH}^ERq1!#Y_x54+u-DDj8&>noBt*+v~pa=M!}xNGE{k zwd-^Kes;d3QqiE}&cTH)lAiF1g{dJ+e0&<52IlS2miQ}19Hg9vptz>oxoA2DYT!h@ z&A|}9^Cs-nEDd*#(|;nvGcWi36TEDsLV2+?rIx8mnXuNi>7yOuCNQF+Wyto2I2>C} zzi_Zv24MYF)O1OJsRBK{|E6TT*4+EfcDsww4l7G5#euw^tbtH^P`v(+ko8|Z72Os7 z8wN)dxVS2C0=UL7Adiuuyk$c3+4H;A+L>U^5Vzctho za`4;RXWl4qpbQ-u^aEq)QH?GlebS}sewDgKu=ssT?? zGrrC|<<5Jr# z@BNba0tvm zBRktQ7RJqmZWDQ_M-4DBeo_lmR?nNQcP4p`r=jP@*KLd7;*yE(i7*{n^WKAw7{1v5 zD-7brqBFNzF(&@mBLz3NOXK`kSd{5dv3z##6sNtME61Z|06hFy(tAc0%dB6Y96#Hq zG_v5*Pfnk*SlQ31&b$ym{NTK&`noNhH`l!)#!~onXRNl1?a!wE>+tKGPn+d8il#|r%| z6!VQ*{&dm8Zb+J~sR3?`@kk4iDbbjGNH;l_7jJVpN!;5~V}d`Tr%8GK5b^Fg5Pb~c z#ns@gJxUd+DQQUxqqQ5oG2KX;JJYv(Oi)@37JfwOs=?=j=d&|YNnmzL{y6SR2MaYe z=m{<}RYN>*EKXG^uVCQTOK$i}DN-qMzP|5A?$PSNiTaQxIKu1J3vxo|2-1sG5$!;% z)fU5K!@hcuodC+xKatuksew;_FDpN~rq*>A=T=V~rKK|WES#)g^k;B$=4&tX{}7XB z8>^Jr{gne)Ybbht_S&4WFYD!+*rGF}BWQMGey=ddLNU0q_5ITUJ$+BWIFq~I2YQTJ zEqKwNfZHM+Yaym6PAzmA!x>sm!!=3lLL$~W)Gr(Qa@hrs2#z-J|BK?uW&cHoT0@~9 z=CoOrBU8Dc5H@I7udrQB)zB>9c)s*dVxC1D;ROD)JE8|sy}Zu5&QuvFk+SsLTSF#ckYHQ(B4BSqAtjdT}KzVjA6Pzi5af{h$8o|Y&8W%opQd6Z^rm+Q(bX1 z^K$E~1N(Sd5%yvqrrb_@%=cpm?@&g^K zsu}viKKz0i?hiqs<8h3w(f-3&RUi;*a^}YRCf?&4FA_7R~07$F7H8;k8?&Dc`i`V){O| z^NXQdneK`%!S!QCh4~A$kaj>v+rns62!q9ye6mJxQ(#L__KqmyX+h--Mki!`Bhal zn7k~i%bM%|K$;Ii%4UZ$F3y(?K$xY@NZxM z1PyU0;MyNxIl9s&frg;6NGI8TO6%=2V}94P1prN08yHIe2$Wh=^kyf-*9$n}7hn7}gl479KQgX*JpGaL=q_%b!J-pD@XyD` znKr&8 z0nkR+p(Q_1++Ai-V2haqcwEcSfW0O@fiY=l4$7-atY8kE@hC7E0QN!5l!zGQlCG`? zr`m}rVcy;{G=N*^g{M}vxMbV!2ce5<=Vtf7RQwEFCww2g@l2td`MqtKzaT7>l28fj ePZ9ivd6!Us!pQf;0DUS5kd^u-St)K1_FyVkqjyVhCv?0wJO=bnA{-Q{yn)JrWDG7>rx002OyrmCn50N_2u zHI93CaGbs<2nu(?bJtao2UHF|UdIsxHgcMB06D3!T98K(^YU~aG-0%FS;r(_l zg#rMqD{6{z`ab5{SYjVNgXz0_$;FN>#s2FrNU=$^>4AE9I#kDDhL%*ccseSfuC!Ci zBviHxHp+KrJFEdJN>sLT3^WfC7@+WcKFt;Y_B3$*@>6nlrAM!(n{!gu>FK%Te4z96 zF!W$Z>hG-k&?97hJqsYDPh;)&E})c12p<4R00A;WQGgC0ln4N0h5!Wc5qKe>9v}cE z3jWQqv1F>Bfd-UqB#G>+r6(D{G}bv;apTBq}8(Z}L@2Ldc3+N@lbW zM8^p%B%*Y3(u1K!WF_aq?d0q0OAFpz{rwX`M6Rq5S&+vp9Po?|LPQ2C@L>gvLGK}c zFkL5eq7jg_pM(@iHCE_S9o45wH#a@M@A~?&s;gflOCgbYZITB(-n&UL>>NpLixU!_ zPcsGa2XbPsuGX~g1|3S&s!SW*+Re?-@I8v*d(YbGL2Lo{?*X{J`c!%!q#<> z4j?SK?EDcg#3CT{WJ~V&*!CC60gIPQ-sKcEL+t&>%zTYT7E1P*gt>=+z~L;^0)D^5 zE;Bhf(oaOZdabL%)OOeWXf@py`yBZOzqTg-5^qhOd~oP$4?j~l3fXmY?L0CuVPIK} zE<|(Y=FK$f$l)CvE46x$UIRSxN0&96z|%-#v(|_PXHGMkZ*GSi=j9}%==obMqA7rF zN$WD|=9=qSMz&;P|1nr#ag_Xa?If}>eolxE1#9Em`ZbzkPgfH_n`qx<3?yC9QqzWE9Ddq zxjf02xsHn202|An(k7TT4u9k?SRZ}k(9z?X0h;}Mb7AkX$4n;{wcnAxNi;}PfoE!3yCRw|e-g$VW2ivChUCzxdL00w@z=&3cQBd~w z@t&Ux7}SgwD6N>RiP+l``zX>M4oKf*e2u~jUEipmu-pfHA31xbr>8*OrK+i9Ztg+; z^5<^^8NEky7mH5Dho==Pbt&s+~$1#=6gzyX7XANYl4fdiw zsjE9)R_qaB=~T@W+~Ed+j)9OMaaMOde|mQbdoQQME>%5BzXP>{fw$I5rvNX4kCFFl z7i^z?BSMsYVWqsWw>N%%8l2Cya|`|^I4zxk`Nmyt$5sBNs?y%h3I2$LkajN0;-$Sk z&s=S-fP3{L@kx^m664nQ{<8b~EBD`n!AJt$o-)47C82|kGe;y0L4-J;za75ZO_u1c z#EJ|qX7`;)bwA@@H z*^un)9vmaz{$T-~YC+{w=TfP42j9%^cpx=8(glP7Pz4TXa zJ07n;jhj0-a63HqY;0)A^S`vV<2c%LVHBIbg*u$nHfpa>8;#y=3Y}-2mF(nu)j>lC zy+oid*@xfhdEUakP4Sb98SA~M3D=J=^VVZV|P zjbu$?W$$AO00b58%zd80?+7LSxb9;z1w%q!RSQH&-#ZFxLG<>qHojTx+F`57$pOn# zI-0kyuCKZs_$j}(W2rC>pD*2VXu(!xgufBw621`VWn+nonPXK_`FSb%4$thR7=)0p zN7LTo>iXGPIcf|wiM(+YyY$ zpP%)ct%sw@`q9BcpDWDI4 zVR=rl%*+1e={aJN@+nny!TR_ z6bsW-u=*HF)~a{0?Z7wU)(8KQgE16hk!279?VQIjf1}Ie92Gb~}*36zok6_|zAjnec7x>>wfrP7Rx$KxU8qJMZHjZ~2kvop{Co)f93gQELx9 zAy&@uGC3Yu>?r=Nvaeq?6Rh2(4AJpg44gNsi*F>X zNBGcB$!qbzrCA^vn(@KWGVLc6OZ-riQf1x-l7mxKt$rsv!}=#^j_6>ZV*>XYF*uvq~epIw88w)F3T5A3I$FiAO znPv=Qw1bAo*5@3}NBo?TZDwDRl>4X##KLW7P4NAcv8c1N1a+?|=v!6+F-puBX4J%5 zA%KWDT>ep~p^ll@*r!$doYB$V8NxGpEQ)|2;TUENbA%h3=MO%LO9E(*=z&xn9mAT- zqDnrdan>EDq$$sVJ7_jw%1@QHavK`zsK=|tpBw2KiU&TqEdRs7&O&^&bgqC!eNf~Q zc!2$`sqQs7vw#|#z=#Z+%zSxjmy^+R^9i`$D(v3?AHMsB^YBx3ZP)EVD9)7-<4L>Q6U+u_z6vI%K@iK8i7g0`i+;*$_iFqPoRWmj6 zo@r^UQSFh*QXoArU%8gnK+xvrvJ>8+434Gc-mP79Aub}z)uAM>g)eUWTLw7XA&dUO zK}8ic(mbXE%9(|Qw8RAAzaDufLFUbl_c8!lhOkcO`?J}R-nh6o+g#*Wydkeh=h{#= z+qf@CWB$Z&#BS^30FUByChDFc0}iuQ4HHdlR5i2-B&y$&4Slwt=UamzGYzPbvC0>Rq+H66V=& z?XBZZEj|p1dxl|q-3WqA(|Ge|LR5qYQAKLd@jq6-ZF{4BR5r|{Y2GYupX2gVq{x_g z%hUGclpI$Xeu;8swnAnIL^~(@{PK9^=T!l>oYroI?^&`K<0nbfryC*pv_Hi&GVPg} z1AmTXQd4bjgOCB@e0cG;0rN0ma6Y5thoJNDG?S4bsVu*aSR^w0;3_GzZ@L@PAUgZr zy{3_F3B2?7Bn16e*J`k2Xa14wsr3UYMn_RT#!X`WMmfsO6J+^dY?7YjAH(_2l6Jmi(1a0vL z%iZ3HVMQO<)5Kk0_K(`;o3(l;b8Px(4I=Ig;*zX02?MZ7`^^h=E6l709c&YWrr3*1 z6AHWDcIJ8-?L=SqNuu}Y-r#QLy=R5rP;Gm6jJfveLR+^k$_kX}c|;e(_->XaTT;_P z;*sefVDRg?fMnjfx3+_GeRDyw^xj6@cV~K3n@l3+k(mf?y?>JL<4rjH86#+$U5Ik$ zc|bB|<9sOi&ETN49bVo_*~SA1ua=1qs93Q62txc zqa!W^KH>o_K@E|R!_QeR{wNH!;n+YyXK zUBvR{i16QSJNlY{7hRz|>*tR=jEuZ@@=9{#o!dgq{i1N?LD(wFQm;6n9*(I{U<%ea zrTtLGCTTdx@cp_Y6~mZLiz~En#iRX1((WD0fXN}CsIL&>pv|5J(Qf-P$6ud9c(SJY zYyUUmpfkLI3^|UTuO_(EldV$?R-MR(o~m0zfrOg7^Bhbhe+<-I2W{@Q$EWcrsKh4` zmvy{yO$DK_4A&?06ePWGI9Wn>gsGZ*PY(K_AGVg1j=Cs4I0arjhAFR)YWM1ju&_i3 z6UDl}yS24Ksr5YXSL9Cbk7Q=JjxQ(qn$p$=R z1PohxfTBT?QDirR-M8Cwn%)~(xxva--3h(LQBi&OQA0+%bxm7D0uP?|#po&$w0J~m z$ozO14R7cEF5MKE^%#Gml6@i}@R01FpmEkGP)=3PyRntQ{w1PrW%CgytIp4!XeQZg;E-nGiZAEh$g_4v6)>XsE1Q z>m=7P*lqkd#_>4bdyXZt^>`EANnY3{DI_=8a{XSzIfC5PtG=;-P(b=31@ze_ho#Z6 z*#*zoIPvCmZ?Q-(|5)ky?kvkk+9lD+QXMY#aFou6k8C9;;aj{)q&|wbLWNXRikWuH z-T8r+31!@fN)GB|!TD0IS^;5cIspSS&B4kAD;nv{MGTD=%YI*S>D>G=-`@*+qY z9*AvKF?6x@ogrHs`{GG7=XeTlq42)>{yR`8lBlI)pYCr&GJ=tWr006vh0)Dj z$5rqtzlw-RX?`e_m?_;QXlpAWnbOUjjqO)Z5O0OJmsn%KuNltU{gUb_~74vWFZhY4+ZZzXtC62}10FzcBry~MR5c#%S0rgtPcG0v&f(U?6At67C_~w#l z6pnXxU#O5l>n&*uA+GoT(4y_~FOCJKA@R%*6R*EPy&KpPQxm`T>*ou2Juzuy^h?44 zA0V$`omL5;ZPVM?rh|=$p&$XI2ziJdGvw=sSAwf0<)=<1pgF$zju0Hjg~D`eUf3q3 zowNckUKTdFWLcgeK;m=vKa+v=^~HwyEYENQ%gdbo)-BqDydS1v7l?N%316M=vtO+X z3O*e6WA=reRz?TsSp1Cq+-1K(m%Q^xE#Q-5MSgN&U{qSV`oJOM7fTm5<}1N~&0oC@ zhJTQ-cdaOmfK_`YXQOe59@JIl$9WfOO9nmNVPG(ko@V6REI}OZ;xJJwygyFzv8^Wj z5MFUdPJKChUmB;IViC$&YuHG6Bu6Z%?lonyRgxR}>a5^mZbCyy5oD|s=Y?r_`>Ia4 z-zh7HPqa5@yy-pWsQ|nbw6mON4qK3jmyn(VKcHm{`4@L2d2sv^3J&m=%(u>(k=3p}C1cEvw1ueaqZV-6%_t7zbUnG$q z&dhf2;7i(n+58yIV@u{ihNB5oAJ8r=E;*>!=h-KK@GY&0c`zFf|HUG$ESGkRY+#*^ z&pnBStSsMMNx2`Ye|+&2PWJ3BLQsG;5%HvhM_i<}g=ptfF?|B4)5_UKYHXt7JEKq` zjtpB_{Ij<8q?mAl`d_efG3-m9Nekg5PJj<#`07Ec_<$xKl8849McXD>n*IuLHk zB2!_81T08Q8rZTnrJKvzcX0FowFwA)q85vXvqMmO@1vqRrqF5BA)5bcJt9|k_XpN- z>JDPDWMleb>vdDBWAOC!^cuB70w^fy%ok=&lb&XPrn4o7M&$DHDTU(Y9E{DjJa2-^ z_A^7;R#z-}i1#`c^F@X!dVsm&&pSZ$+hPBjOm{vRd&#Q4(Znc)EkMZXF@dUsRjQj< z$!EeHr*iI|m>L&v_v*U3ZWVeHd`~%8%!k}6Q{3Ujh$0`p2kmYmxbXLDmKeQLN+@LY z8l@5LL0U@s@7tv1_q7|uUb=Xjb~8h|c{+v)3u;CA+fy*IA_<^ncHxJ()z7K(^YT;` zw}y}~LzD-m`&hX;3L7dDF|vdSAgjMyPfk3%iwIxh{i|#|Y)Z<{T6+8tREXAR>+#;Q zDUtf>E#+WlNM3SbSnSY(95-CtuIkzvh~v;}d#jkzm#+xFim20001YSKz7(s8W#{X_ z$6cghbm=a3{#V&VKxV1B1SdSg9w4o(qhsea@Pj7jKYhy(8#TCfkeql$EF+W%*r9Jr zLA8MJ6DujGjo1-#N&pE6n9}#>$?{9oh*ZSVLy1be#;4+(KvMs0cFG-7Kn%RRojB3w zo+bg3Q8jM;Tfe9bn9D-JKJTTDnX`?R94Uj^5MN_my-W4@elD72Zb`QWK8M<{fmghX zX^XZg1{$Aq379~Bn8D(5H1kcrB#QKrk2B7_X-~z-vXobk%ou3kMD&7Xe5C?=9m>AN zn$Lbcgl?C`U{L7}hQlTzL(&wgjkHT(h=v(g^h|cs->aA*Te72G8qOqe_g`EFCR*WX z=RA#m--nc9$+8^~S{nj*UhVH&GKuZ`t60GuHJBl?npx>u0oPLxOlT?pdvQ2?A6Zy7 z^=ik76oF^5C+G3HE3_ZEAkhOHekbzA>Wdw{%(-+9ri}=JH~p|CpbwWW+#dKpuQ~umLAp}ls)|MT?@H@V_HtwA1b5yzW;xx zycXVbYcA!=cio}w?Zxo@yh=e#I;PokE#<23Q(H9Mw^kM~l*khG8!+Z}^!h2*j#HiA zceC7n)F}9}zy4_L(0dn}mOfFB5ouga7rV-N-SqctyF`@H;^u@nJ2&%!<3geZr&L-~ zD}YoHG}Q}O$Uet!ez`a$6YO#l{j~$Fwzczz_BrINMlDEB#aEkd@%rV5p=3S$^g)Ji z)~lV%5x!=sDF3Z0w150OCX#1{84@Dl3`S%a=^maBc}q)DH`+}gibW~!-mU48hr*sZ zD3n&WrQN*t#+%bRWK66)?|ff`IyhgP&uvZBWE#eK*_SGRQ$*n@z;ub3{&2w~&Wma% zbkk6fMpH(Dj8&UOkZ+)U`s(q8%;h>)gWpAd-fzzHcF(=(?@W~I=4Ln_m{WiqKPC1* z_Q)N=qCR|#*=E$(1{@$~=~dg5-);glIv`oh%5 zXy>vo_FpUY;4Z+^VCq4}prh9th)Io>k2wF@5uRH$cPqu+UR+jD(;U9D5gQ#zhw2`= zT3=5JW@yow={Tkp#!bAqT~(D8je?^azef!EJrbA{?D!pZ`O68XPS2|E{wB-7<=Qi|jEuP=-$wY}wGfX!&04Qdog4%L>Ey@;#>ErX4Kggv@z(R?4Sg#1H*^R8Ur zd2;%N$I*Uq3Fl(u<{JTjgVe0td7M;YkKBEo(pD1DbCoBCM>Thm$cD1H&pLGLGF;q+ zf2&2xaZ{TaCA(d>F~CD#<~4S3tFoZV*}yTndsFxmx3ZIy3kHaq0V!GI*qBo>8g z$@!5pYhUPIH>Vm(`n(*vEL!eZm_+3=J{V)ydZ7^y$w>ZWZEPUL_fL%DA{GiWrQ~L7 z|2dA%q{cQ+no3JflZs6$pEqY9NKz%ndty?x_$FC}` zTGQ7nI=|v|$OC>PJ6~&IGY_y#8_!AA!)V8USR!g&5`QNdj0yy^I~2|^p_WpR<=pklBMqTxU zs$5V9YfgAyV^0fY?5BKAOR^~<+N3={HZtN0T#qaph8E@aGf{;RXpoDXdDCY#wJHOI zM#X%A(`eV@4>*AF$~RVAm7Jq?uR>6@w%Ka?(R$q400mW@!G3)#lYBnuEi47KYMwy!r9`ZoV-Np$B2=x>W8(moj`&F|R|yJgSF=kIvJ%3b>}S z$1ckg7$jLYZw``KRgMm5NHhsb$rfeeFi-CtdC@M-)!mM^<1mi{a#up3(fHwCXZRCZ z&))?}Q}uAt$KG47Ri}$;zp+l#T~#2Z_71?@Qz|=?~nMmZ@$ zdj9;uAvnk)xHpWyy1zVuDaLm}r>y9-eHde+-+z#2sOSE%8RVS{?TIm2sX0@xKh;)~ zA!kpBB%+|xG%VKlGWpcpblL|HleXvxITF zVQnj~rCDHdeK_bJJ4DMQsajj9qy2SGrVSV@x@BToLD7X|kFcu(KlN3-KW7bMGG9EA zzCRV7inLTw8USVvXIio+O`3ga!v192GrGd zNJdTY>2TIMo7D0yHAbXbggV=&s$;%Clsa2rZaHSDZd}krN&4#|@fRkdqC^SR;AdY0 zX^k30c%$RI)!5<}@2mY)IQso0tM>s$wn$7+H@exM)dyIwjP3=pk(-40; zcaDp>lvNx^yQFszNxtggYsxl=ihD8}p>st2L4XQ-I;roR(wJzw%g*Ef@Cc+Dspu*i zraR}(s#|YE9^2i{6Th+5!ymKm15+xvSYPD&P5xX<26daG+a114kOw;pX`>LlLW&qpn8vCnL+)?lF5l73gN`Oua?PSV&CcBi zRfMw4crCb@QTloOd}(6edw1R(yEl8LSk6P73ip>6MStTB69jaGPjt$^T@^1g{*1&# zU-$ZB%C8MEdc%>0k%Ixeao|m@Eo>olcXkY{~ojSTY>nx!NZVk%a(KQZIJ2sM;2UmT?wy)R7I%^H}dJrP;UT2AT z?Ja&N(IBX3B{yr%Ou)_x?Frw!OWI)KFAnh9`m?-D6pZvLR-fk~aRCV7djNvh3JtY> z-5N}cDQ!zI1#)m3rhO!5GdtANn@`Cx3Ic<#51E#}U#&Nc&Floc5Scew3pLqO;z1ayfa_S8l<{t4+BC%o?t~K0Jvj3A)v!0+e0r zr3mWW3>&Xf;zPa;YD{%bYe+~l6ilWjH^jGEys*A(zwKF9;Fp_~;RyJ{SZ(r*va&xQd zKONcrVxsBf91%~6I$~<}jc@Vq{e)|EWHDO7cn8CgPql&Csh%IOg_Fjzs`M}hlDoPl z5d;P%k))BQQL3oCw^ZQeQ}dT9E@#ir&-)!?WgGwPQBozc(0G+ZEM{1T0;uKEoLJv@ zZ2LeUMJ;r5Nv_IY@@knNE=!ArWok3vFZIXOC?~|Iz6XKl9<_vD%FQ`lKeK<9T6|9e zm(`XI1`j7wF!U%xN?p{M#S~U0)jL+a_a@+E;D%Xu*lVdUyXQN@+HOTeG+C-dJm{2s z)*siSr@lkM>jk5q?D^ii>0KGob)0X<7*v_vyU1!jR22-JY6Loik68Jaa7eLG5-BYQRSpN>llMx zJDZ#yYhUT&RmB;gM!IF4Z1(O=TVec;w{o3aejwr%MsC3-v=50{R}s|$hWaQ74lB`6 zx+_Ig82*E}F}>rZVEBkC@7o?0J+lE!yYnoRHARYbtIn)mHp@Hg3~5JT-$}S&ml1IJ z8FU9q-}Oa1zjq#>YkPZkniU=pi>y?HicdEnr{Wdn{lz5Qf{ct~V_BzhSp;0A8aIvW zz6}ru9^umIrX*?0u$^pF-VUuspHKS%2ITqqOf4KgKb{A48kvgudA93atOuL#J}-|1 zoKQT`OcESQ6Iq*+Smy}8-V=gMaZ6_<|F9c9^lm|a*WAEhB%3s5=x=U>(m~=MLmttz zo66=qBh`M#ED1cGFkQo-y(1-{x1JC}#|sy5kh~gw76GFbK(zJd1^xnA!<@AJiKnTy{RusM7)7a-{>D7HIM*&*F})2^;V z-`jpN=bl&o9ezT}KY$E*=ybePTnK&G$D#?<%CLyE!Gd>g-aFF!B{W7+sj|P{*$GO_ z`9&DvJyFVhcNyNm5l%#Gxv9^2nMZWO-j=mF%>2Ugf$11!(U{%~A7Rst$*xe?r`M{# zH@$cHj`ynl=DEzry&~doJB@&C z=FDGny@|Z6#tRQtCdg$ zB7A_IguX|;a+*El)(7E37g@*A7O3@VF*6}H(fr`xy{o;owWXzVew~hSkzLr%4sD5j zii6`xBzF+ncTS?wyr_We&i2J@Q!+pQqd2vSzX8`<>VQLZ(&TFjE>Iw**T7O;eKj9{ zkD@)Y;GnV=j#6d+O%)E&=ws?;;|buT$-rmpl9}@v6AE zn8DyZ12(2qq4i|;{q$_ekRd!NPSr9?A$;mtE&NrVBNe>qqItjY`tqFiMZGJcH^(k{ z=vT73&X4Sw!FN|-3jD6Ix7<vYq4k^2tB;MX`KUPXZf|90w4&lq z8!R-mZ*j5dXBG%`T|WAX=0ssJKc3@CG(|kNJ$Jl{G=leQUkRY4X|gH05SC2mO3^n7 zOCEgPe3aP?7fpc}3+&_9@k7;o!`-)<5;Fe6B$KJTvSS{C3)TlL=sO9u<#0!~RSy^7 z2~)=;1PqxBXb(vVAAf&eK*D5mORb|B*FfJ|w<86M{Tq*a6)9`=Ot)lpFYL&h{!B0l zOXx-zW*ao6loA;XsIHL(Jtk)b0r=xEw^wzY6z2t*L(dO)c{Xz50s_E`44{kFC&tfd zj!rCLUyJtPxhdkT+_8%6PL^#}K`RtV4yreCkPCYzNx^mn84~G_8fa1Btp)ToG+R0g zP8DI|JdHrO&x>(D`9!Y4BVMJH5$UVj3r^>WTLNjuv9v!m=Yj^)E3p&s_CBT;DQse} zv>@9g_D{N5SP&I~BH?`<7V zna+F5yAcS!-RQw$X;~@%n#V*AMbUIf zTb*-Bs?e1vaw0@7-5t+)kVSUrF)N6aG_NYoRlOiq*u?agH!}&3WB*5kqnDmwcs3 zj0v5i=T52ynEKB|*1>EiRN-Y(_RvD*;7F$a@k+d5qi@=HnsZbyPvxv2%X>cqcr%H( z)q)xK&!6b~VCwEkM90X2s0d&1WM=U%=cDtjnQ;=$v_>n#q1cDBcvST6+SnEqsFS`8hc3B885j^Eop!CV>@N4$=_QP4~)`>C1DR>0vYZ}P3wx_42PGE#<VaG%KK9X+B`{?52LJNm;HNFdoC z_quqa^W<3Fr6H#xag6dop;qMu5w_V-UzAwi?+o)bzF?x`{4`2_KJF;E$S1(4+N<}6 zPdS5BmuZBQ!<60?EQ;opmR8khTrTbGh3=gIX2pH^Bw;dJ~)_O$Z z8Z+k!SbTuw07DNN!uO`yVEJnYPUsmV4BV0t*kFCw{{h_m_7+`P8({a5mv#pncunQs zR8v(wZKlkwxx$trjcOP2;M^TJobFKPvcGPsF#DiltlOKf5v;{vr&;I-|A49rZbaRN zu4XbkYmJLm)3ECICT>k3`1T#Tobxl+y6MU|CL3+5&VJ@utEHyN|9pGK6W{xatuSsV zl=PrBU<4oF9j7qjWpnM(Lh_Z#0SbhEnwQUQaV&HtQkJ>6!#H-vUyuy+{neC=*VP%D z_H>GRCRQOXBGO!5#d5w}8EF$WA$FI=d;8UyLpx5$w&(T&OONEnjxN#hsMX#u5_m$_ zS~^}!ptZ^05Lq+*t`SRn06%yT{#M6G4{QK7(=|3TFwoaA^5GJ?n`SMpRFy2Ux{*t7Qx~QA$v?b1~DDs-dtJd2SdoW zHsGrfqy*P?Xra)nKJKag<* zD8n@(@&M8uWx0v@yrN1DbQoc&vLRU%waw38%TJza!hQ8j6KXSP71>vAicqh9N~dl! zz{1Q7$G47U2aWY5I?c$2>3cpqx_U0?66Aiwbanbh^n#Fx%^2F=-oqN5Vi&#sr?`|1 z=7okx@wfC`m)GOSH9R9GJL}`wBT6bW^|pVRga~n#`EY>f06_Wvl<0RNp{V_GU?~E#9%a!rRn!I3qgTH#7Hs553Vw zje5+9*Iz_dciZ8YD(aiO%JPdVHRGA}H_0hfr3X4pzkpn11>OWF7g+Ap%lZ9M2l^*y zq_z*n-p`Mh2906uIP2O=zY=H<_~J~+u!u*@$!hdo zVvsksLQj?OYK55MsZzjIC@dvm2;vZ(Ik>KrF6v|vk8cWl&){&vYzQrCI%GGk`;Ge3 z;*X6Bpp8>qE+8YFnZY=>bkSG<*aBcehNa?v9`#TIxnHLFojx|jUoFgrMn=?2j@`BV z{#?0BcHAg&{gsN>BDitnW>imr@;EF!O(Nc`MDxVbfk1mx6L_#=ce1mo0xi4P`DJzH zAUa0FGf*pQ(P>3SCgRC$iaBi9oZ7x-XM?z05|atfHiOW-5z)34)@!N@_cro4+7*-( zV{XyChUT#d#!bG0Kcg#s?Yh&`PMM9Ryyx5x9xX*#*!XaQ^ki|a_QfDM)^?nxn=>I! zi(1N2?*q^4ocAvIQ+{O;M=yjk#vy>L%ggM@P+yEUqoq9W`Oj&&kj-0y#R&v`8tWY=I(-N|^b+6sR;Cn%`LYq9da2#1yxvZWjH(9D55 zRVbXHHzJaiG4QWe3e#HK!?bVkW91(o$na7NE}A`z^ZzQ0{$Mmh6zW`(L_J+7-Ng;$VyH1f;*8TwTVj_fNwO zqE)qQEYmT!5s6P5ps~Eh?X&49)Q4}GbeP><4RCO(VAavqNKO|=_HW~*8^2K)f2yQm zRyzL&bN{aIib{&)hK8avw>!V`#F`k@(p^TO{IPb$iUK~$G(aiZ9Ir(-2??j*^GpYa zsoDWGwHw`$5x>#JmEHac&WsnKYwMNfn{2oHD}StL+@Pl*P)~cPQ;zM7e@V%E9*O(- zzaKX8nf8RblnVYR9Ip~W+6>vClK|#cncZehMqjsDNPh@Xl1oK=-HqQTOk!9b1dye!zgzrE<09@zerefHKa858!VS zeie9e}#5bCFifOfi8r}-rh-4D1L1tZsmR5+Rrk+NCO4D^xE7e z-ZaWIo*5oGflv4ks^Ie<@KEO)L#$ykgKJW#nZdJ2_D2j$z3H+LLT$Rn56Sq~U;Fx| zsEg~8GNl#XC{{m?^eDV|!-X!fbReZMVe}=&Rf}`+o{|C-2VeeQ$*bM9d%1Y8 zOAeY>dk5|AH^m@`b*MsAO!_U`Uq7e9!hyR&PL3RCZ(OAJ4?KR~cDmMolyf5o_Fnlm zm8Q@IoP00`2ftVIqiJl7p`7PVh-{|6psJOUj)$xb|HTpR>L{fOM zE2PTk&Dey<5xamUp*C3xP!PHnv(WE09UY+n_2~|x$(;a6e+O8kkppo)|3`9l zOmzEUe>|8#QitfpA^yb=p8ysw$+NP=whM6U40gm;*i6-^^FAL@hpr1+k~vmWo)@5K zHE`Q4pE#9`R|>xKy{p$VvcdOayVt=4>VB^(1A}1#!kgmxD%!S~K{)sRBg#8#)1a37 z-9*cR%G(n8L(SC}laxk6okh%`?w+B335d%-;mN3bAC!`sF$AF+MR$PSYVNQUqZde| z<4k1^K|lu@g*bv*OIeq}wn9`rRit3Gscn98`L+M+|S#eQy~D)4x_&udJv{m_g+-7FwrWM;Gb zWag3=>ZScDde^)#^oZ7^t!ypFndg1`1%3bJ`5y_eLbN^P+o7IwNWa>^Nk z=zK)CwC&_NK)j{QfoXRmLk>2AJDU!&?x)yQO_8VjuPVJ#RBe12JF!e|QdDK*~~-kOL1 z<}P*9=2l*{+^0~0K3P|F7>qpJM{fVN`9e<)RH9+ndb)S`x#_kCZ&^1Dz)1Z@&*XLl z#^Fs8CkK7PiFM)!d8cEi^59@$a^U@Ak4iLjq*GSL>)*0<`%C5*vj3t#fxn5oFzVI4 zW!?JYlEy1n%<+J$tvAUER>|PVAh#&{o_TtJOj1&Ry6iK;=;O+c0>O7!81wRWmDP2H z9HoS+iILRX?)?cG{Vu2n{}OJ0~BlX%3m2UdOGMAqYQg8f5-Odop0y zlQ8<~80A5~Wtk^tA@cBlO+Y3<CUCTSD&A3<4$(hK_odp=s zpBUrw3B21@SArW5g%EVcR6R3#ny4yNY*xCgIXN|J6zTxS+P}2ic}er@r=w5e!s_vK zu*znW^q(vb#MU=)DxTslf1WTRC}r4x6oXBN7}PDS*Bm*W_M|n2Ra|5**KT9YQQNbj z@H44VMAqanrSw^n99+PkkgNwW-InIc6%^XcCKz#7v}>YxSCI#A{MlH(?ff}Q&lIuo zbcfEPK`Al$WB+r29`&Y1@J?I(;bno_>Rmqfg-+BEz0$zMd|~TU=8A=5;v2o~6${J!y0L9qb#8|(FwgI7 z(4XJ4ePhaQvx540+?>2Ed(T1NvI0_!=LK*K_%WxUBUFU|)p z^U#1hF)0{}hvMC+LuP*Ht2;Xv-Nb-gP0JwvjUjq<-fum_J*#^!`Cj1>x@zXQ>~&53yqSAV!g!6 zf?}FxFDh+`O|qmXHPEeF*dRq|_gtG6U}RvHZKl^|TEWTvtGu{uQwjax%stSmFDgA? zG@xNq*fkCdX(=YDLJw4U>jM4g-B@UQ2}C~5bWV3@zhuFMREXn|uOx~rO?lxNAcVl@ zPph=#Tt)P3*W5=itm~*3hu;ff4|6ndy>6~jp6KwvV#@i}D==$-yR39~jan_)W;BT4 z^ru<97scIDEid-C=#Oq^khMy@b9C+BJ*RnLJ3Pl=d9RL9JUMl7WI^LCR>98A`s-Hd zayQ(mCgSN@|=NEaiSdeu`AOCZ&{l?9r;tp^+ zefdDE-faEAX>;4b-1!eZj1xQ0p~{*$9CI(Qu$yPiL&gmu+aMc{AF$xEr>@QNf2PG2 zfLb-J2;vCrO^Q7Q!)2kaEHIJdGugF|?A7%!UpfxOS(9HXq}x*H=L3AKeo`6xP#)&{ z`TDhGLmzizlAA1@Grt3{m!-ypT3K(|VM5lMjg7exP1BP~1#(j#YSNX=HH`PTl#_EfXww8G3cF1VAvXFlDvaKZml;iO&E2o!4ND*%CN7DXuEwuqL%qZEx1Fh)QdG0^CLryb#(Zpn0A{FTLgTYKsim|jt)b|7rV!*9;xF>xTY zI4pq5J0J}>3O427V~(p8CUYPd(qJS4XMWtj_xab5hVNUCrtKZt8Nb@ye6tr(naRCo z7r$yFMQZYMg+ylW)Qt$!%kQn2z=_f_mh?^Pe7<7@DaOG8JD!a^g_>T2v-8W}V*_q| zyqs$t)q}HJ(d596yk~KVhWyqef4>(n&UFs>UHmkw;*G`UYiacRW@FvH!W36ux*eGz zt;j-Ip2;{}lUe1{J8t$Se5h2DIW6DK@U9Y*TFWte<)E!Lro1cD#;X;9;tTZDdHYsa zb8G{GL>`!rCiMR3>UC}<+TVaW_a|r5i^2C>hugXv3nKC2z~z%+vSCGKlOxTH{LjLU zs5w81-maPD&N z|GRIVR~(Vyxt~)$L{M{)Todql0Ibxv(PT41W+b$XXI`a!~UyWpW zyDs6`waDx_B0o4>t1A2@W$GWQr7<$66e@*cQ9I>2_}`O8{Sry^=DwZJ(`yB>0pZ=+ zI-jVr)_bQ9&fR+XTQEC#ZtKZZ0h^+LIjD;puNIrj8Gj zjT}gqbA(yLk=mw_9qbH&1}39+cDBV`vTtRU)g?vC7*`OsEV+_*gDyKBX0U8~ANk-$ zKaI$R1Zge#nltV+_O|I#li|Id-MhR3t`LX@;eH?sUpnBuB;=(e^bI7 zefA6+2g#zK-HJg~oBLRzvBiE>(YRld2Ia+5hMKp&U+#>C-;1KgX$D?hKbjdwrVThE z_}>lDOQHK13&*q8Oec|t&kgt*jw2~^g=bVJE|Bar6T6NZeZT5OJG5ZaH>Su(rbzhP zzMUc-B=U3}=}K}7aYS~Ly!7DCHAq7aeP_4TqF){Q$16SBe=>5QiTn~2!Sc||w3@$h zdxF+5Z0tJrLEr;XJNuX1?c0YPk8P)_RoI6E)2ps$iUO5a#%*1`8W7|%-CA{wv!zx4@wnP}b8YCh&u@?7-fB__w=x9WZe7mr9`1t##{w5CBjHp-y3R?}!Jb9m zE3Ro{IqetS)RP3T^=iZ5D5%=?_EFDEbjRCyScXE z_?HQ9G{k(zyqjP_yk4xr`ty-2S&9epQ&ab4*CB|lPc38iqTteJMn>8jzhWIr#)hCa zCOss$!0ZEYuz_($I{wvR))V!}?(vgul!-DS*=Y&@NbRojFf zZtlWCo$Jj$WZ@Y<_KF#W_p)LnI6O8FZCe-8<6Mu%37(F7N|I&XUbdk|dOgI2k(!u3eI+7Xa>oe=~cRP@bKhNsLGOmCq^~rx@kNZs0s3~bFmdjfN{SUO@+y?*v literal 0 HcmV?d00001 From 5de5735636b9fd8916f59b1c7cd5288803242aec Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 4 Feb 2022 16:15:13 -0600 Subject: [PATCH 430/947] CCD documentation ostensibly done. --- Documentation/ContinuousCollisionDetection.md | 4 +++- .../smallMarginSweepCollision.png | Bin 0 -> 19913 bytes 2 files changed, 3 insertions(+), 1 deletion(-) create mode 100644 Documentation/images/ContinuousCollisionDetection/smallMarginSweepCollision.png diff --git a/Documentation/ContinuousCollisionDetection.md b/Documentation/ContinuousCollisionDetection.md index 5f6f325c4..d34f57143 100644 --- a/Documentation/ContinuousCollisionDetection.md +++ b/Documentation/ContinuousCollisionDetection.md @@ -66,7 +66,9 @@ You can also speed up the search by increasing the `sweepConvergenceThreshold`. By default, both of these values are 1e-3f. Increasing them will make the search faster, but result in larger error in the final time of impact estimate. But that's fine, because speculative margins still exist! -The goal is to find a rough time *close* to the time of impact such that the speculative contacts created by narrow phase testing won't cause ghost collisions. That's a pretty forgiving problem. +

+ +In the above picture, the sweep was configured such that it left a pretty noticable error in the time of impact, but it's still well within the speculative margin. While the speculative margin would have been too small to detect contacts had the test been performed at the T = 0 location, it finds appropriate contacts at T = 0.28. The goal is to find a rough time *close* to the time of impact such that the speculative contacts created by narrow phase testing won't cause ghost collisions. That's a pretty forgiving problem. Overall, using `Continuous` will be pretty fast since it only uses sweeps when the velocity in a given pair is high enough to warrant it. Of course, when the sweep test does run, it's not completely free, so prefer the simpler modes if they do what you want. Especially for really complicated compound shapes or meshes. (And preferably, don't have really complicated dynamic compounds or meshes.) diff --git a/Documentation/images/ContinuousCollisionDetection/smallMarginSweepCollision.png b/Documentation/images/ContinuousCollisionDetection/smallMarginSweepCollision.png new file mode 100644 index 0000000000000000000000000000000000000000..f322b366464805784c274f551beb645d19c47d98 GIT binary patch literal 19913 zcmce7RahKM&o)-vrMTOJyDm;~_u}sEE=7yGTPYNGDek&xS=`+g_u}97J^j!B1D0#A zoyla9Op?jmiB?sXK|>}&hJu1Zlau|T4h01R0|f<*i3s_GQksp;fc%4YSC^51s+}e| zfqa3p7FQC7f@(-ac{Tk6`HtiwtLqL0g+BQ24|>$O+yXMsNbZZcrjOD227&>u%o^0y z(Q!BaNqIR*%*HZG#!vyGnIboll5^0BFU~9tGDGQOZG*HFdvr7hZ~bsWgxi1JkDEK= zt(|;*qy`&+?F9dK(XB4NCLOQDZeOqQX_qflSQO!yoSM@6A6OEIgfytIh?Ew8*ik9M zlf(zvLog-a(FUO)KXCv5{$cO?7pfb~A}2TE*v$Ok11lvWxe4(?$&?Rsm(`C!Q!>gh zGh2&?opQ99Pt_E*C4&%_rOMga-|ozHV-u2^oA;|@y35q}Y7fv|np*3d>l-*)>)H8u zcsOCGd&K^LRu}&)gO>joXv}ScO&T~YmO-{*Kqj@|$AS37&@*^5fIfwHXN-U5~7k(S$0 z_8<85&QKNS`z?9(N-~`C#G?twR}XG#M6mxUC-A}|_^tp1x_U3c{{FQ|2H{EQee5Cg zO-KBf-Q7zt@czF3?J>C@T1Wdf0JFb+rzdzS0UnJ-?eY6m;m^2{&?S+EAEjr)+2f># zdnV3Gjq^i0F0XIGcKT8?LC-Qwz1jc9p+m-1n(H)rlPM4iEw2p>#91^*Ezl6HRvUe= z=9FlStdV^Ag#8psr4Hoc!6BC3O#0lMEz^hvPK1au9XD-3=Aw(+#95&oWZNi+t&USZP(VCJE7#E1pqY~yV+wVz z0^txFrs&v8uOh0GTFxF%i=eYzfUKJv(F7182peT*zvG4eMgbrbc|qL_#NYIRHS&vo ze_XY%_vLa}gBN{+ixS|#XN$SHm6PnnT>xOW8Gy5+J;KGxKl<52(rj!GFkDWPD?_r1 zyWg&>c%gnn0c0~2k&v$T60gph*v;YFI>TD|s-N?w*_6W94KR`Wfk**?{WBQr>wWXM zjEvi0t>qRJqW~;AMJNRY0v=>CKSYtgPzmHwIYfs_opSr8x{0W05H){M$n67-htl|k z)YMpdcGs6MLj54y#suPZ1`>-r!F^_d@%R7Q`I*%)v`@_Eh_uZ$tQL(XNLuu$0Kdcz zRtY@CDWGchpXl29=*bZY-t3TjbAMM_xF2|W;4ScsWXIc%X5 z^Y)#sY@f&(RS4SK6L7(6*F`3Zgtqb9IE1tH{9wY=Y_4zlWsih_AwLYuAY{adM|n&J z6aGw&3M-4%BlaI|3^PxUu_6c7=+AlsxP$vEIl4Gs%*EOgeNV5;$UBd}hs1&eZS`a& zG`+Dtt2kJu#FQdrjg2PLBptQe+fc>^sk6k)Ova_H#{1^)869l1|+1fWU)qXSUARH}GHhb?D z;T~8JC4XL<|*RTC|3lbC4>3ioS6H65xafz?>?PAdF>UiOn^4{j{ewBYJ z88R$Bpb;$q`Yaul1T3!kiORBg&v7~*kY`=6<5Ju<^P1B5sS2iiYk)ljx1os_C;E## z8@Ti8-nFHviJO+2AkKN%}ApUOYdhe9;L(AmNu6l>; zuS%lfgDLU%jX@|p(NL)OPi<11&ws!RE|09s%Y9cnS8L8RI7LCw|2SNqZ)@mDrkOXj zTh(n;;r}?Q%GKV7i;MF=+<-2YQS7023CWe@YJ$?W^fbJp;1ZBjuYr<=I)fXdCJY># zPwUBae01`;6zB=b z^72$#xV*Mjmp{7NG%0JP+rbkW(E=X50ZD3=&l+90c7B;)BtC2+Q;tsJ85P3g#k(!E zr5pU~e}{T`w-WPu)RZ54O8$+o(kTW=Xz?ry3-=18IU0#`C~*2gjy^sL=XD?4N0~@N zDkguS_+efp)B_U+zd85*vaZSEjOb|~q!f-ag<@!Z-t0SD2OV1RI3p``{FRt5ZXKNw zZo4>-%WDMKK3Rl?gHk{;7hHVidWkSyamB66y}v<~CGu?-Z>wNG;EtSR){_h#1_{{k zbaA!`FYuMaVO`b@6ZFr$x)n_GsxTw5GBVQJ;wiSdSZwI9ks7q>3X#~1nItaqihDUl z`&50ElB)SpbA2tPj`KO5(C%hrP@7)F)1&@`Gu#7?%SARbgE5yKdb+TzOyrjYjD#Nw zL>53C1?g42`2+k)T%^*%Wj@h|HRr9r47Lu4Ko~@lT|;d(9nfammj#x_TjVXDuM2Xg zzb_pA?)N3F5#mETB{3Ur?|+$5W5n!(#TYBhMyJe=G=-{QylKd`w50uY;ARDO#=n`F zW^kPq?Rqf0_+gSV%+AAuPO@uhs*HhzjR>c^;(L0St3p@=hIzC<(qGokl{$Yge!93f zeiu=3^Bk%*yn9AK`7BoDcW#Av0t2yGEfFVJkw7O4FE1k`u1(`o-BGW9W~RaxtRdJ} zXdzeM($L)6WM}KBpfJLGT8QHNe#F*z~8MyHEiIz*a7g8w%05vX=T zX3bg0xgjhqmzP{nxeSWJ9Mac^!xoBC@-D*9zozNLj09VFbtOJ=DY98AC-R~bi z*4`$gq(n5KH?))t&9%LD(ob#YLUYh%RwCkxIuVF+zHEIIJ!nc{N5Wdb{F&<(W*6RG z%RD;sxfRjzpD=z`#&*8o>hzWrhwetElkf|r9w=3$F?KS)zMD%tVt$V;`4#YqLMVSb z_{Qgei@b|aIB+-u2}Mb1N>M3JfMgpbqK3th#8g8{t0@f48i+ghQkD&nQ>4~2Wb-rA zKL`7>ln12z`b`&N4`m9qCmYGkgU0f(SS=cw9)s*B4dUBm-~U!JL+`M+`Xq{9A6k-( z4oRVL)d=9h&?4|xp}%?&LlF#(-%Rv+>dPg*>LN_lkXY*%#fBw+Kow)bDs>xJ>InL- zypD=izP$S8A{l6nqIhZ`xqRBTgc41eoeEttlR@{&=j(UA;5SZLKw&kGluLyq2`n{5 zrNQi|$5{VDP=skn3d&`5NVxW2t{98*lll9_#7DmO*N*%y)uK=stKD`TXa~8-=F-pa z(e?uLwTC4ZIu5=L;CYk#%qHujvcm1o%j4V$RP0D-;m|->S=dpHofnV4L5!!qTZeI6 z{+DvZXl78uD1Ey9agrH_bP8qY@8IDTe_oceZUQmiU8D|1NOD6Hom2~nEX%vMJ0I^D zJ}8Be)SDzuMtohew+ ze-_JOQ|5=8LN(lvGqJnVg-2;D-6@LaDc-z{xP&-V`bnr)y1Liz9%!=q?p@h$6$k|J zMD5!pS1<%}^;gwN1PTMiWgkrl#H#Z|&^S1*W@TuQn4p=VKysDNaL{?CBj^=&cJ)Vq zQ@YW>2D3wg!bUq@rfLT7tw5KLx4Ux6a1Xpl=%MkE_^bC96Q&iW7XmIPnGpv_M0dvS zc~yGDBRjo!WX_hYN1=$dC?A=e_qpB|uR8pc;?db0z^lMKCKjj`{0e=24G~2GBpsZ8 zykFWrModilbg-riU5&5}UCu$?LPM-jOsg@;`=(x~!`OZpr~ z9jvVphfAasi>N^9Ol2|>a}Ax(Jd*p^0mqs{!3VhsHiDJUvoa@S_uc*w85{mP@h5{A zrtbk+LfJH(KUqgE&B1WY3eyXZdM|*k@mtT;k9K9cjnua@-PC{aIjF@|pDzH7>j7AFf6I+qJ|<*NhtmPn)=UsRNBN0E=2 zhkL;OqjJ8s(=a+_p6=6%L1Gq+ao&?UmD|d9^{F}v>;u)Nz^b)4hkcRp`l_>=^6~a= zIf-VZ+&UemRYk#r=d?)!5djp0O3vw0QTg{CN^N0k=+AbXdw&MqSwazV9G<08)1wf$ zJ~n`_t)n%0IBjpikAV*Z%v?Boh9Bw(n^VP!AcbY>5-wC8)Gtx4{CVu%xi?i9-u_!6 z&+-}Zx{AcYFW)5Vs+;X>@dEJ%-LN&_GqV$)$y30-&_?JgTcS5_^kvB&orl^YZ#?S8 zbP|G+=j?ruJ7W5mlu+I(A$6GhxQk$1TXC$4{p>SRAw&$pIkocq#$NyGq0~VECQw!o z3{pjBC_jn->4&xf#Z#k0;3Qq&S{BXMpAxgaR`EF^bE|Y&Z)SIE_>9wuxd<`|Nd|*+ z#AWuK%XpIjGs0gx6-aj=vYkjfj`*vz;*&qaCMVljMS?tM}*N-nIuKW|KLJFhL({|#PH z3DRs8t)=CN)iAPGeA_$dzdTwqdaVrFoLIO#vuR6nn7Y$CGb!ymB~JK#$RrW@cDGyY zyxW6Fb|&wZyy$80T{V+}1_k)bsTebpSdq*VbMzt0#Rx8wT8rh!2R2AxH!^fuK^Eu* z3k=|B*FT9Nd;6WDqUx5;qKlEPC7Jf7dxtQa=&Lqgq-LGYpIPcMGmNmQ93V_}RD&A~ zEAQReN)o%7`z7L4>-<14{>9}#e4mDwj!Qhgn=5IjOnQXa;J8Hf{v=)bf*3TBlwK7l z_S}fjx~tWKmujcm2Y70nxg|2MSFmNP4!N6N>lkmqZ?t>obC^zia*tP!u+t3sd;^lH z-Z;k~n(@OS9&nhS^LCQM`#UV@)Xtf1ip8rR^Hbt2VLVGfigeD7lROq=!xRzoh_I7v zx`#7Hae#~%%nEG$uHD$J`Q>iXPabwK={Q8wuZ|tf#HyZy$Mw@$MLtZlJiKw8-gqlH zjT}jgMzpcE44S4y&FM0QpW_G+QyJ9{g0+vuu?&}>S?AD4=8f<~3Z+F8LLX*+8YzE* zV#FTvVX_=8w`k>|A`L3>Kr4TjLQ^%A31sI}ZCo!MzizlgN@ldrgrU|b3I5jg<$6vU ziW$D#OgUW$sB?qu4js^8Y%$h+&gX}C;@?T!dged^n7m2FH^`i-zxGP)tnJPX1BS|J z>leBh2@QL_Y@yc$9vSG}Gp#J@98QI$(gV#S6RXaL_Xm{{x>U$ov z<;cgMPv-C#C|J!jhj1<8rUw*YZYRpd0S#F$U|V87cuv#o{w|?3Us^2}jo`|7i?FXp z6L`*+o4UJUHj|497%6mn8#J5ZKx?_Q4KwhCuXVzmKEsniwn~!gHm8TPgC$#8(QykJ zwUmO)pU%zu({xY7Mvl(;r%6ewS!%`Uaa`9eC}-M0vj}I0OGee3%WM_&$w1#NebM1f zplGs@hx$e^`2`^0lBtuG?nBwDOoc?0HAf_0C_~k7DS-WAy6|XDP7gtIp4$?C$pUX7 zO_L{y8BfUjZLn?*Jt>@ORmjlNgeYP^4fVTfo!a-i^FP8t*gNgT3}LniHoX_I8w!Rn zQ8MaFfu#6xcVS|zZ(#^zi7`98j0XdB`T8(Sn)&cT(ZMoymR$4=`KbY*+U%(iKPq1pQHzq3C~9+D+rRb2ZYtmtdc0t4B#ZGuOI=zb z^9r)N4C>5ULeB}(kbPn+>eiCE*8YT@jXw_h1?A{msr}KUx$K^fY`-9)bK&&(lf%2x zWQYGZxuVul*iZEe!KC>yn9V~r{yb8btIh#ax9f+zo>MnZSxOlWy`=P=!khfK+wqi=z;?uDIvf* z9S`OoM4#ml&9VRVsPd|K(Oq3_-4v4ciUXep%I8r0$uIdeR{2L3+dJsbRES|D)QGf$ zQp8oE`2ZKR{IG73zP1}BTMU_a$=Diz=(9>ww#YbWV%X+QVgf5naSZ-Gp1`8jwEcQ* zEtTmsMpm1Rm@lD+PybVs^b5&{#Z!!~WZfo%QOo^*zy)3~Od&GVhXF8~k72u_9;t~Q z_2$uooh$V@vI3l$S#{n#+mpyl)NR*azNV|C@b3Nhs=dcBjL23#pIywb!;5$v6y1(( zEuHvB=gs3Kt}O)70FOx4o0%(KYQ151S4{kpN(!RfeSA5?&6@55`L`L+VnNk|VtZ!| zgVxKsB93p@x8IGA+O7*xG0^M(k!_Ioalgw`vE>a+@~6pKksy;WAH@p>lCB-wSa-QJ zrt=H9vA+;d@Vd60HmlJUUVP-<8{iWW@*q2H(IskOaxOl0)0dt{e-#7i-47RqV>}q< zi+F91zl0+A3ZxiB!1J#$XLFntJyBzf{OZ2nd*T&&!P`qy>y0I-ki2KdB$ghA0(DU< zt@nr{2bOlp#+eAh4~Yi*!C|p^UT)eHw4Lkc0e|g^XL=X2kqr-0;W^_Khbhq?0vls# z1kpWh*>-0ElI1tLDf1idRwOlX+H{tMRS;iHb5Wit2Oi(xzIV#0V>yXV-qi0Hpf~J3 z{X+VkAAal@HyQQYAoPXxVK%0iUF5F$1T!uP|3)L&2#~Uvf^dYoeUMYAa`uZbss!$n z8Xn`#SIghpR{Lt5^rC{-6I*fu!!0y5Ju+LOUev{}C-q3;u!gc3;{hAzODv>w^K=0L zYpzQ8ju{FIb9fEKeguf|MK9|orA8H3Oz-^~`f?iZhDKMvrH9pr4PLfIOZ_Pr8C$5? zRkMl?<~uzT5=+SDJ|S>TDqL$*c}dBvjK}`SEs>zM#K6Mg(~^p4!9VSqnnwW?RE%4z zX%XJYJ{33@B_tzdqk@~tVUL_*97JytTGgN1W4t>vvr4S3{PZ+1Brw$R@AF5c-Zlo0 z`>Q$jWyU<*Jm996rzROFx#0guQ1iJ~WU|i`POpRT@EaUNF#>p-SI>_}$m2vr zMEddZ%WN&}CSX-I@0X7k&Yq&EPF7VF)gzhVJJiweSmzKsnUOJX?N>QnwhIEx;!cGB zK<55K`N$P&^Y?6FgyLIjF8^)H7qen&pVR99b43=dBT-(ER|!bQbytEP$NpU|8I=H8 zG`2H>wLd#sZgsD4H?Wx8JD~)5bsR&G#5imA zN9+GNy|{W(*Omh-$695uz<<8%+REC@?DUpN!+%kAi(BcA88Ul+sBS!)dvQ~?E6-u5VfK{4{bKfdGg*b}oS&;|eP z8CXyHRlY8>3i6w2!^6;ff0tMJ_+6nemYA(&_6oVtAGc*NRfLvhs;MN^)EoSreUH3H zVF}aqBpMxA$|d0GYThO4(|{V3bz(zA4n4LZ5XqxyIbY4k1``EOV)CVAcnMNI&V}(w zqkD?FMl=5zAwBN!gVqu-ciw7Pp}OC!xtk+4jc7KjRedrl`xH z=|t&7Ff`d%@`@n=le|PK9Pd<7Qj&&K3Hh7fE~OdCl9)Z?2(5xBfmxNUFv5GPY}0=a zcc6!d_gyh$$C*ElTkUsp6JLGyy|mSq>_#)AoYwh#+1aXYeJVx~66llRm~PA90zQt; z*RdZ+@yZ_|?>#XDI-1(6-sh=jgcs*r=)g7=MusraNgbe*>V3$sIcBHQCfi)HV^cj{ z>hkL1+MGb>An4SQjX+3MGk}GK0+T!2o`1F%+y+%N*+*FcNExsE zBt5ciQ}t?qMri)q1$&|+HnG!InM3Ot!$Mo_qNe?bgyctMjL-!KHGD@`v)THG_J$-V z?yc-nnPewFi>~cg`S$)cV!dwX9~{hJ>5O|#!}-}7Rn#Y^|+s|2ssF< zqDx?s_Q8&%+Sy`uic1k@sjS)TM&q9*8f?dFDmr+DY@D-q%7(*&Q8V(o7-D!p-Q94k zr~{whYlL=+-jabKYDab71i=(`m*q_f7I&CjT$tahD7wL``JnzyW!+Em8C%hv8J>sI zymIB#eQOQ1Lup&3!Nr((rz?dlT;p9T847c`?3kSrz$s|imvXm;s8O6KGTg0b&_S8y zpn_=y|Q8PYT$3V?ALRdTog?*e$B1_np;x3g2xjG`R}T_`W03wabcp~l=RP< zew|XdoPyq}reJOFOqrnq0}WET zuAXFIe8K`eH!JkBeO*aVIP~>CoZDfHYBr>FTg?Ivg4Nc}8zIY~m6kBEpb>^f>IStm zi-jCOF=&5=(LpHg!sgNQ$(hJu!mhMR;6)*_Y1}Knk+j*1b&Gj|HrhaFRW{A`EEvKhp-rH2jmKA)U7*TZ=)oXDR0)WQeO|`59fw1nQvb+17Vm%{5!j}FPy$J zFmel8cl|J8*xSDp_QQnF%4P;=8>qPt?|Iz~iR9ahd1;=Oix%0T5Y1V`a~)yKIyzbi z5)PmGg!<@FR`d$8nGp(iM{{%H<}G#~3=%>x<`7fy2Ihl02byI<#R_|X9YFL*va;+0GI@36~T#o7b1X4gh zV~}0ks1p5S(<9_OS>7yjvWYqczoK2c63yJ_`Oo~Q$Gmuc8ST2e+V?I*_Pw1wnTgzL z0{0}xmZIhJuJZZRcP4iF?ZmL*GTL33DW_a0czHXhdqryWuCwzq2ZiplWsc)}Q(=={ zGJm{giV$}Obi|Q2()f1vtA4e)+n3+7n~P!>%S6wg192SqTer)xt;p8-thZ~6;P94| zHKfLlZh%}R#+Z|bvv6o#iibE)C*vwB)HHdd;&cTX&2zLse<*pl-0hgz3|{FoSwn|W zRGc#SC$bOdft-$p$?ci|(ekwd8?g~nva7~#1J%=fmqECO?wZSzgvJqZ(bcrog#sN# zC5^OAKV$?M!Nd{QNK?1;j08mXlc*%m#S>^~daCLYD(p+0jeM3Q# zBrud_xBCFuGL4Ys8gbNKr-eSDc*+*Wj|Y#X>|el3xfY4D-qZAaOB*vmg>}2e7NqyJ zVn48Y*-C#09n|?$r&Z2M06Y5Ui5V^dDO^0gK8u#ot8qVlUx)oc?GCV#ek$A)d+eVE zUjs@YhLGS1RI|i5Ak7!jv~9fTU!nalsHG#+^Sm?Ldz+igJI762Zh&}{^ZA=TNOAw_ zjs{`{Ve#z|*{;OB)(Dlni{H-lwKr7Qh72p1&4cwQ7&7wU`D^tHl) zHF3R@pSt%%yF+@8le`nOYPqc{PWW-P($R8@H>JNcjb?($wU?B^NLy3m;n)`y)70%a zL~)cSp+w)N{iE&Y=CXu^B@2BcBflKC@Zj*o&+h;zn~k8z4Nqa>^tU7}>eHq9Uw@9> zvH}}HZCM53&jlUu|ZGzcH4h0%sq(*syoo|(nm4+)a_xTKy#*7CO2)~ zQv@gZ8EauU6{S|0fakkc0$UMNG26hq#lo3{_AAWw6qA?Sjf@i*A8n*}m=9>3)gvh7HL>TKRVu_tB|* z5zzDK)(YQ5_%sB+v_rzLB+YptqJ3(^q!(DHH%F%0`4tUQQa$H~GFa@qprSHIc>2*w zPfikNBEm0Ce)>O8R2!P;3*6Tq&EAo73dFN!5*6dn8(TkMl!C{waY@WOS@rQeP43t? zFkmPD{9qk2Zn(iqY>9A+1g|O$#WALyt!Gy5M$gbMBuh(zzxyCxrlhvePkv=M^grf} zlV6GcPiZ8#Rj6a&X+p1Z)+#CtPh`m!{QEQUC>ASEL#$E z`_#Z6VayfG9jq4mN_yrf@;p;|r+8A3Mo7=pa@X<-m!q)G;-)EQ$opjb?`%gSEyC1jj`YBm^CULNEXNS=X&RTwNa+8a zM;QIpx8t#TbQ~9~f}uf2)6)_Wv>~5^#z=ATi3A%!J_vz`upT(7OGG#fr|Gpm%;F=J z1C<($PA;QXt9~fgIugUv6lExUPtg-HK{mOl#18?KzcR3>i6V4^u_*IOrdv zOAe&_2{xMJpF=v5wxCtNaDM&Kvv9o!+%q~lb!v6m`Hqtbyh&Ykd$zMmDx8Z`+oWN^ z2$UZ)YRsK_nv2f%s=kFQy<7(SSmVh$o??MHi_`-`(DVuT3il z#;R{?BfR9!fsV}YZw-XR@DY-lxIi#;^?Q%4we$HKqu2M`;-}Z>MR8a_9k>eHe!Mo_ z`eI^7gm2z6v?(3m_IZZU4iMCSt*v+KUZky1W@ia=ZeV2@sslYA+zRkEc@G$)YASz7 zrxy|2Ik6J;O!GronMlpk^?U8T6VCn2zox&dH{|z4C8NYTMs_(36b0Q>c4AdtJkY0K zC{lmaMwqwI#$Z795WKDc6)ZCvFGw&`hHpe}$1!A}+Ij1AfL8I8N_ylo&y3oZy{V~9 zi9to^0HH zd#P{igxDSh2$6^24XOP&)nzZv%-^Iu2;6%+S+%`c;Q@mO6RL9>gOfa2d@k61`&UWL z$KL10!DHic93YAGGpYKI1AY63gCy-Lx)|i3C*5id1ubnAF5TKmTRB>801f&B5 zQnL>h`7Tqt1CX@@whyizNtQ~=xDhEnQ52MDdc5+#)qtUD3+EE+rW7Vv0d5!Qo%*pA z|GC`_Y|IoC@>u725oM~fL6Uc|{Mx(o!?!wrio8joUvuY%v7h^mgOf+~hS$sHIG9iX zi7?;Yrps^*v^0(71XY*SnanHg%+04ePiwr%ont!t{V=;ZJl$Yk2ji%;(C}c}46Z=U zq|*AY2+NR?)b_c<{lPZ{I~JCt6s-rz$%Vzm#cnJVx=(7?1Shn@$gt&?%!bIpud{PZ z$+jkev!1j}hDc98cXL~4D5plJd```IGc~xgsRkk;R|TdJJL+o;^3q;)Lx~01Egzo& z|3nog=9>4{*4X(^L9XrVEjLwmxgVYRkLG9hRluiz4{dwa)@=g2Fn4~6Djna^`Zl?A zkFXo#TR7T_KQ9pL3=0M}l``)>T^hYOg9gGm0=M#5=C*Hj_;rp~u=We4BwT`5hfNb$ z4={48zgsFnH;S64SjHz%7Q zSG|l@h63|GrG~{l9EciDVa-%*tl?4P^c7KbQ($ZXBdd zTfz*T>!f~tEtb1yxV+4;=oL79Zg8!@7uCIIUHRc}1$QJY1b7Na@piYLVJ)ufbNRU8 z?YZ{vRid-irK8Kw_dbx_e@>Lui)y+4R8tcGtt6{M5GPPNB3)WaXI0ACW6DKcl-hD~ z#5Rdd&+$JDLx8iyes3Qe!$h2IbL(<;8S-l(HKF^gX&JDzQ=?P z0Y8(GsWWk!4gAQL{lkp*5rZ5F&_?yg&G1k|Q=Rf$jWp$yF7fpz78W%XMZEvfKO+`n z#3INw$nUu;a2Ia+T%1AD%hT}#MEggfxFIoZ&6ZJ_pHWi2XNJ8lGYW65(UoPg7x*w>mmfEIn!}9d*QCkf16$QQ zqXSde1w;;SCf|9N(M|x6XKF1e5lJ_MPb$$GAKDe0EnH5&I|Eyy%<<%^fIwwj5gp7x zP}`5VgQ<~tmP4R#PnGqEzkpT}bEKvDM%o<02g)pPO)?1@!p6Ld)5W+g{bO=?ZKZAV zCLTE_h;6N3FFzfd2KDAMYd$5v$3O`L%tTXO-qfve+|Ik-{EJX`Erj!JsK~#wJTmWo4BH0_I0xwyBHiubohnDY%KeOprnE#6qJhb@Q{Y& zVoM{lKQ2VvFYR2hDL20T!wNaQuO=1uri7y_jFAKs;1AtT)7#!pb<502tl7X*6943vEOMfSd{gm{7u5ry!{I?~g-LHBn!dZ+kl7ViVUhD5j=1>hsuu3Gch z&Rt$_kU+PMFi=W16N2Kf12o)4zr6q5?5RvE@mzL3vO@fl*AE*b*Rd4+g~aIFV_cZ% zmfj1+12V-11T<1nv&^)8)?I}^6eJ1iaI{`0!C%u;DXYa%tvb-oQ4?pFxG>Az6M9b{ z$66B(Fi2Aj2v;qHV6d0@i5|L~t<0`GK_3+p;2dV}(Wtm9psNMI=s zWPvW2%PEoiP15}W>#P0%x%GTj>om)Ir2cw5pspI1elAvcQVEW!v;c=53xoxk@82u> znq&RG5p9i;+Ux}I^tbTJwoigjHhD65f8(;OHl$AVl|p-b6Ylz`In?)_9?46&6dvpL z@VatLk(Nb(NSa8X*~mplGWK4$b%6~E=BnHCH z(VI%S4bYk5KRuPGOAS*k{5RLr*2>22DlIB_Sr*h$)3f5HvwlWVC`3=(9?M)=-lO)= zM|qjWRcq};=j0n#(pgd6DhIqZ!f{(2`5A3NIqwaDpRn-XY%2t4G}RkkCO11xuy}9y zM;5q_f#U8t?_5r6uZuJ|J7O!K*w30^qt#Sy9PwCY^i@v`bgkDo_An&^5eK0_2^%v3 z$3|&9q7V8hi;jFv2nEU{6~MkCHOWt2+CR7Ueo=ihAJ~|!3-&)9w6=o{92P(>!S~N1 zLeW+p{3cnR2q{YHHQlx+!s4(^3g2)+0EcZQ+mqOQO7Q3r(>s|u?$ZYJqEs&>)f{ZW z>30|A2O43+0`kB1U;cJxOcF-JMyVP@AXCky{Ubr6ku%bU!W@@lDl`hMq+RUY5zZv+ zKfaeN;EQAyYB<3x=DtfQ#Dqa`6df@ zKEXP|S(O;~3J~{@u^`y?{ukW(e$smPbF{5!IKX=SOsp=zdQC6oS1I&pT|)44YKiwE zK5KzQ!xAJZs8jJsRR^2ZO(7z6_MFBnRq3LwP$F z2ydi4ltKMDXvvAn(SR?F9$EqFE_%r_Nh)M=OcuErqOMA5SjH~CMUk!1)E@<&maA`t-QC^;PoiYczNu0;FN7cd{Y9}Fxq4HP zKK~K&@4NpObTi%>`&miZ&z}@%hcD84PBe48NctekWYjam?KIxje*)Rs48-vL489p} zz-@tRxmR-H-qkR*%ZN*Tm(Uy`2mv=j792$Zr7z-YU%|E>qJqyQF zqNygEWLXz=O7Z~hj#`iYHd{I`pxt&izD?=XiUQ9^W+ZV%qAm|0C!f=PH@?DFBjcYAtc2qxWYua)}O} zJsEzsm!)y^8Q;0^2zV52Y?aBHG&B#s>X|2Up%-mJg2w&tU~(Tnea$#~HRYiyl+pr0 zF*i>+19kR$wlTmA1EuF#-{0a_-_&(|Kf1Wdt*?#O;SS9$%a!48d zyU};QyJ(fdT$i2iy7pWv>-z8;a)=vm?~6y)@Ed~ucGu>G4gxN=R>#wSF#!{F!-QgCLC5|JPUAO4I-=F72b=-*c;JlER7*7Ig z@=9q^Z}}CF`ivz8;XyymB)V%&!Twe!biF=Hz|?HEX;K zdi9Eid(+aJSe#Ki8=>kN@rIzn?V`kSDZn!mxI_GJV?%^eYh5=QKj8bjpfzVe(w|VR zURU>AUGu7^>K(b(Mg+A)x!MbkJwz#({ybNUqKINCe1j>{3s)jmWh%oFtmx%L2T8r= zol|$(7M9HdR+<`OFn{_Bi-^xpTVlD#&l8cWSbQ^ZnJLYC86(9lgSfbIsx{HQ-zwUT9KAKmfFB+q!4bJCzhY%-!u0dIakNrx z8tOm0mJk^!>gzKT+dG0Q&nhQK8Dk8_ZwzDI*HO4gnPP5U!yx&cH5y?$7Jtv|YC)G+ z(TO;4uAKo%H!NHtJe^pt-8+kbkJ%9b?DR4mw(;13=Fyry{i2`n<# zdQ4-U0!lf|Mo;B@X)UO-XQZqBo3kAqJ1-s~At4eRog>S97#-7^AD~Rnf60#i5m~3M z5jyA>8EkAfeCnh6Tw`=5d`BkD0i&^D<5AqrjQr%asFyjj+$lCH>@&(>NNpec56Y%R zD{AOMYFuO`#Z|?r2RZxr&B(qX06mfDH5k=aoEde?XfSCy3@9-0F zCEmVNWlJ^vQc7(xjBVFa8G;(G{fkK)``H+Og$uo+Hs>7Gzb0TVO=#?~aqa3+AmIl#A+ao$JS02X}6hnpAK*Ws^0 z6L1{r*B8ekhNO2OM7FJ$I(wWp5nnn71a!4&Isz$sc9;J;=ey&b<6xsOAzgQvp!o>R zi}SN@259i;EcpyJ4!e0>1Mi1NfCwX2ua?*mLt5K#O-Q=~CClGiu&@>yucYt_1_^TE zgo-?0$F%uJY6+Ou9KlkV$SG6YxF4mg)4Hr1s~ZSH1zxd=KX@jxaco zh}Ia5ALaWwi8)*|1VZwB8zs(rc;>;K#j~l7PS@_n4X@qai(cF@)l$9o1Dt&w$nS%6 zLJHBbt&(m0+XYwC@i)ngc$%uE^!eqZjLvGLXf9rC2sKHlFez5QIuhJ(I3XZcZ@U)-Z$P&TRl^T+wakw~d*WOC`S=rva zif=-YP(}K0`XJ=U;!yXCL74eZt0k`xJ@?m9!|N1sxHQAlX6&f(`I;XRNc zomU5yvKVM|EgA)-1h1bb#h;?M2_TQA4#{b!FJqHFi!UM1v=E)%r^E=ZZCG1vPDOEq zh*MvO?e`S@i`8Yb1cme>^nWa9>3|~DKiY^U`I~J7gR{tAKCv-)#|oWS=a6OjM3*F+ zLUJipXMmTvgKN6vj}h1??*55KvWrweey#*)ECiC6mt&lX;Ft|zp?k1Vo*0dylH3@w zKL|qN>}O8Re=86J_N@K+HGTQDQ@EL^u+nszNciXfI25EEb^2L#MZSdG{gc|Wv{pBo z7CpgE?y1R!SmV6+^>5cZIh>vlOPc@cS|1RQ=xB<1x{xzhrK@-#gp&RdiZYlX2%t=_0`^g&Bm52Ee<1(2 zvoll6&8~o(V4C=6NLd8&Bm<{a`Y65Ag@=H@Ie=A^y3k!9`VAA57O+K07SR+*pUe$j z`gh*lAM$e4od^ic;G1~f2`BEY&DpUksmD)W4>E4S_juI((- zmhl)<#kE@tt?h=YwWjBvovKkrzSL$BCe}LQ?VtL_bubT$N^Gp6`TDzGznZEv&ZlKq za}LQAw^-(#6OF*_2ND7Rb(6^!KpmrnZ3pY?CzPfk;Qy0LaR0taG@7JA%2#7$-qLJ8 z0A#Ys=@o9Y=?hww6U!aLTEFZjaYQx7w#QEee(ol5m^iCpRHm)AacrW{y@|&l*%s|n zekB+Tfz;hXa3Lk}f}rQ?yy0NTt{s=4&B|du_oZ0eed8{@|LdFb&msTo%t1V9bcJey zdiHY7qJ&a{dXYo^*K&;sa(#sP3tL}!E<+LC8dNRn(ve-VLeM{tq@02aHvT8W7j4)q zsFKvQoVfVyXxa?X*Ah;rni^LcfuX=#w|C+$l&( ze+#jwm_jz;?^A{{Xm0HN->`afDsgP63E!b$aDdFq9nqW8Iq!p50p5rF<|umk6CC23D4W!Sjk2!4(%NMwWJF-rx-R zkd*IMmIw&xX`u{kM*k6d-$ywozkl-B@$((K^ zvKAn1Hbgm{%Jg%~RL^e#6BEXK;ay+fj$b_2g6vnWRo141p9-Z~g>G#J=dG&>p4*&e zq7PFT5kgEtNTFKx(>thnsJ`QTR*=Hqlx77=xIx^MtjZIlCpe|TE!U#l_DKXlqo!D$qq<(teTU!OIybEKcSc9W`Zve%fm8g! znD`}0+yKIaIm1IDOC~cINk|oVW(B792)lLYYdLR~*|`1oj$g!1*6KIeGnHgn<(Wz` zx*3s`?Y45(^q&Q7iGq~213t0F#A?BY?J>3!Y=co8w|-{JfO7|_z^AWEcpvqn>}gt6tGZK* z8HXs%yML9se1D|jjhBHi?IxC~Y%EHdWs^gt?)+DO`Qy|ly?UJXSbE(IVOTLOt829^ zbDez0oHQ*B9N*#o=;TODTDErs>}85QGZE^(6_2<;{bt{3FUkKAkXM5Q%cC)3qx*&7 z9bK?7(z#Ixxvg?81({~3O&49=nE5WG)fOJ=DdNN|1r8(5d~ybrf#zjU@E5fgM@Fn; z66;`<9>16Uj04=hPb@swbbJtJlHwrx-G4=4%dE*x-u;y>@P{&V@7H~~^lIf(!O%Np zn`>|HX3vg~l}PYqfS=Kz>(o7gZVRpR)-HEucWEq@hO9f(Cn?&@$9xk{)+a4PEgNgR za79bqne8EIqA>YyYk$fs%501@gNojVM(BM*mIh~r{XLgol<`hL{ag7ng(zI>6##}s z9@4x(Bs=U?zq?<4F7%-))6|5IbVi4zl+tQcBtZ&Jw~OVDSzBjj^+IO%R;wFxHrdMn zqXpz4owLN;Qp4{FCH^Y^B-Zl$rC`)Ao=b6d%(kba`FR99q8P6*o zPj+Vas8`7JaJq_Tjo#Cjh3nnJRxH?K*N<;V3kk&hBA%jp`MiCG4h^6_6;fuKQqN(zA%n`!iwzGCv<*~T z#V1+30g6rYhj0N0sR5i4oU^pR275~}2rr4@h#MY-eEHZh-GjI9cGC(W<8h@m4W!Bv zy|F8Z^dD{4y`4F?{vuZ>)4M#aq0i$8Ld=(`wxN?i=-}PwSk9auE`RM{+S!J6sAGqH zZB=?v6-H;H$T{^w`TPzbb+ua@T`5YO_UR z#*^&P*bRU{*M;0YJ?I4i0SPNvdU~ib;>@Cgn~N8tKZb}r01$TM;RJ!S!PY5tKAo(c z4b5KIUm*N_0z(gs&j!h<;CM+E57!qLcsrJ#Nu}U^P+&rkZRp zCgoM|9#s#3V~siKLdKHg%+BJNDyxGkqJ~)9IgyRAA2+C(_i)sHJe~#bWek30tT@5G z(JJ91?-dGVS8&doo z7J)jS0q>=4%LRA+x^BONk%lzvyZUW8TF7JU{W~HjX1)vn_0_g=FB|@q4HQaLAVv8^QB6aXny)Kw|Nn&KH)`h z!pCth<49Rj7{KOgSmQl>4}Mh(LD;(msn#pcBug=uijSBZqGqR>L$Rg-Fv%C zP^=-KogGa(ZnKWvn#P_IrDoV|Zb@r)Ag9_hm9cU}s;rx{WUG%Gw5rUEUwA+(T6F6Y z{6b+7C4VC)r0ml;c=)H&&&yi^^Wd7Y>IOf5o9k>bLwA>xcoP%3Upuwfll>9vxhj3K ze$Mefil)|x3&s);Ok7leHA^Pw4AS|G>j$jAYO%bmM-A~fD1pM8AeSq>BeyOF4EFL# zyYC{cEL`ti_E9;%2?=uKX?B*asZU{C(}txgPJkOpMf6+q=-306Fw4(t5l+$xD) zZmW}~%YhxgpYrB^Hcm1&%ZG-$3WjlN%J#o_#zCC+e&xh5z=B9y2}pi)`MF*9=~S~_ zuq!MhN{jPnB}hPKq?ID(yys8?951sAm2TK@HQ;*w`2|bLN+O6xz=l5!7PmHZt;Bq{ zydmix4u+ifnRg_xw&qYsyaL6!qVLl?7#vR^aS7pgyvCvFvX@-H9CM(m%GC@jY*r3= zooxzPGeilg+y6oQQJG{m|JE0cHj^9||7@C$_lE_h?axq5MH1B3xW2bOMyDU6H0fnA z*eTOjbqvx9^UVDs)+k=-qoD=D%u%80nAtD(*4egW8-Evbr?z!Hj0=&6x`yqBbxPh0 zO)X{*_ENd!W5Uy7Ww&BH8*q#o^f5jb7t_`?q~HEKHb$}Osatjs#qPJdZz(!Tz>HLB zAPN~u#)IkRSc1O!6zOXfwKYiCli3rT>>2Kz>9AUi+xMm(HQut6btVe80e8zydB#1R z%11vxkej2hm1vbV*N%Nr${(T;_RuLugdDvNg{&3?VC#8Tbypih!R z(*)!77kw8V)c!rge{*zPM38&iyjUCoNNQRiC)iH7JMy1&#RHmk{N}&K`5OI>z}<~B76N(bv~&FRv<&eT zeB(uh8RG>3vuAN4+y;+v^$x^2mseM(l*OgQsg8R|@^k(Qa|C>Z+WYCW-p_3l=;jJN z8a(UGiK~&02pCYOn}{k?5aIJdiUEfN|7B;Dg E7lOwlcmMzZ literal 0 HcmV?d00001 From 56b08a4bff0ee80e1980c5f9060d4c8993d90bdf Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 4 Feb 2022 23:41:21 -0600 Subject: [PATCH 431/947] Bump version number. --- BepuPhysics/BepuPhysics.csproj | 2 +- BepuPhysics/CollisionDetection/NarrowPhase.cs | 3 +++ BepuUtilities/BepuUtilities.csproj | 2 +- Demos/Demos/ContinuousCollisionDetectionDemo.cs | 4 ++++ Demos/Demos/Sponsors/SponsorDemo.cs | 2 +- Demos/Program.cs | 2 -- 6 files changed, 10 insertions(+), 5 deletions(-) diff --git a/BepuPhysics/BepuPhysics.csproj b/BepuPhysics/BepuPhysics.csproj index 99e9a8e04..d2af20fda 100644 --- a/BepuPhysics/BepuPhysics.csproj +++ b/BepuPhysics/BepuPhysics.csproj @@ -1,7 +1,7 @@  net6.0 - 2.4.0-beta9 + 2.4.0-beta10 Bepu Entertainment LLC Ross Nordby Speedy real time physics simulation library. diff --git a/BepuPhysics/CollisionDetection/NarrowPhase.cs b/BepuPhysics/CollisionDetection/NarrowPhase.cs index 53086aae0..dd0c4cea7 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhase.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhase.cs @@ -430,6 +430,9 @@ public unsafe void HandleOverlap(int workerIndex, CollidableReference a, Collida } //Add the speculative margins, but try to obey both collidables' bounds. Note that in the case of nonoverlapping intervals, the higher min ends up used. + //Note that this margin *could* be kept smaller within a pair by only storing out the angular contribution to the speculative margin target + //and then expanding the pair by the magnitude of the relative linear velocity, rather than (effectively) the sum of each body's velocity. + //However, loading the velocities here isn't free. In tests, it usually came out slower than just using the more generous speculative margin. var speculativeMargin = MathF.Max(collidableA.Continuity.MinimumSpeculativeMargin, MathF.Max(continuityB.MinimumSpeculativeMargin, MathF.Min(collidableA.Continuity.MaximumSpeculativeMargin, MathF.Min(continuityB.MaximumSpeculativeMargin, diff --git a/BepuUtilities/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj index d097dabb8..8646c158b 100644 --- a/BepuUtilities/BepuUtilities.csproj +++ b/BepuUtilities/BepuUtilities.csproj @@ -3,7 +3,7 @@ BepuUtilities BepuUtilities net6.0 - 2.4.0-beta9 + 2.4.0-beta10 Bepu Entertainment LLC Ross Nordby Supporting utilities library for BEPUphysics v2. diff --git a/Demos/Demos/ContinuousCollisionDetectionDemo.cs b/Demos/Demos/ContinuousCollisionDetectionDemo.cs index e38d8dcc7..4e766be9e 100644 --- a/Demos/Demos/ContinuousCollisionDetectionDemo.cs +++ b/Demos/Demos/ContinuousCollisionDetectionDemo.cs @@ -10,6 +10,10 @@ namespace Demos.Demos { + /// + /// Shows how different kinds of continuous collision detection work. + /// See the ContinuousCollisionDetection.md documentation for more information. + /// public class ContinuousCollisionDetectionDemo : Demo { ConstraintHandle spinnerMotorDefaultA, spinnerMotorDefaultB, spinnerMotorSweepA, spinnerMotorSweepB; diff --git a/Demos/Demos/Sponsors/SponsorDemo.cs b/Demos/Demos/Sponsors/SponsorDemo.cs index 4735b5d5d..1a7f5c835 100644 --- a/Demos/Demos/Sponsors/SponsorDemo.cs +++ b/Demos/Demos/Sponsors/SponsorDemo.cs @@ -94,7 +94,7 @@ public override void LoadGraphicalContent(ContentArchive content, RenderSurface Add(sponsors2, @"F.", @"Content\Sponsors\tootbush.png", content, surface); //These supporters are those who gave 1000 dollars a month (or historical backers of roughly equivalent or greater total contribution). - Add(sponsors3, @"K. H. & F.", @"Content\Sponsors\spooky.png", content, surface); + Add(sponsors3, @"Neos VR", @"Content\Sponsors\spooky.png", content, surface); } diff --git a/Demos/Program.cs b/Demos/Program.cs index 1feb64570..f72a43955 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -1,6 +1,5 @@ using BepuUtilities; using DemoContentLoader; -using Demos.SpecializedTests; using DemoUtilities; using OpenTK; @@ -18,7 +17,6 @@ static void Main() { content = ContentArchive.Load(stream); } - //DeterminismTest.Test(content, 2, 32768); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); From d6ffaa4644034bfa79f2d0861ab44a9212241747 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 6 Feb 2022 12:33:14 -0600 Subject: [PATCH 432/947] Improved effective speculative margin heuristic. --- BepuPhysics/CollisionDetection/NarrowPhase.cs | 16 ++++++---------- 1 file changed, 6 insertions(+), 10 deletions(-) diff --git a/BepuPhysics/CollisionDetection/NarrowPhase.cs b/BepuPhysics/CollisionDetection/NarrowPhase.cs index dd0c4cea7..0a45bd3c3 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhase.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhase.cs @@ -412,31 +412,27 @@ public unsafe void HandleOverlap(int workerIndex, CollidableReference a, Collida ref var setA = ref Bodies.Sets[bodyLocationA.SetIndex]; ref var stateA = ref setA.SolverStates[bodyLocationA.Index]; ref var collidableA = ref setA.Collidables[bodyLocationA.Index]; - ContinuousDetection continuityB; float speculativeMarginB; if (twoBodies) { ref var bodyLocationB = ref Bodies.HandleToLocation[b.BodyHandle.Value]; ref var collidableB = ref Bodies.Sets[bodyLocationB.SetIndex].Collidables[bodyLocationB.Index]; - continuityB = collidableB.Continuity; speculativeMarginB = collidableB.SpeculativeMargin; } else { //Slot B is a static. - ref var staticB = ref Statics.GetDirectReference(b.StaticHandle); - continuityB = staticB.Continuity; speculativeMarginB = 0; } - //Add the speculative margins, but try to obey both collidables' bounds. Note that in the case of nonoverlapping intervals, the higher min ends up used. + //Add the speculative margins. This is conservative; the speculative margins were computed as a worst case based on the velocity of the body, + //then clamped by the collidable's min/max margin values. Adding them together means an unlimited margin will result in speculative contacts + //being generated for the pair if the velocity would bring them into contact. + //Note that this margin *could* be kept smaller within a pair by only storing out the angular contribution to the speculative margin target - //and then expanding the pair by the magnitude of the relative linear velocity, rather than (effectively) the sum of each body's velocity. + //and then expanding the pair by the magnitude of the relative linear velocity. //However, loading the velocities here isn't free. In tests, it usually came out slower than just using the more generous speculative margin. - var speculativeMargin = - MathF.Max(collidableA.Continuity.MinimumSpeculativeMargin, MathF.Max(continuityB.MinimumSpeculativeMargin, - MathF.Min(collidableA.Continuity.MaximumSpeculativeMargin, MathF.Min(continuityB.MaximumSpeculativeMargin, - collidableA.SpeculativeMargin + speculativeMarginB)))); + var speculativeMargin = collidableA.SpeculativeMargin + speculativeMarginB; //By precalculating the speculative margin, we give the narrow phase callbacks the option of modifying it. if (!Callbacks.AllowContactGeneration(workerIndex, a, b, ref speculativeMargin)) From dd6a1d152a31635522f38035aa128c8c1c149593 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 6 Feb 2022 12:38:07 -0600 Subject: [PATCH 433/947] Small clarification. --- Documentation/ContinuousCollisionDetection.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Documentation/ContinuousCollisionDetection.md b/Documentation/ContinuousCollisionDetection.md index d34f57143..5da183a94 100644 --- a/Documentation/ContinuousCollisionDetection.md +++ b/Documentation/ContinuousCollisionDetection.md @@ -56,7 +56,7 @@ Using a smaller maximum speculative margin means that you can miss high velocity # What about swept continuous collision detection? Specifying `ContinuousDetection.Continuous` in the `CollidableDescription` means that pairs involving the collidable will use sweep-tested collision detection. That is, rather than computing contacts based on where the bodies are as of the last frame, a sweep test will determine where the bodies are likely to be *at the time of impact* during this frame. Contacts are then created at that time of impact. -This avoids almost all ghost collisions, since bodies passing each other at high speed will be detected as having no impact. +This avoids almost all ghost collisions, since bodies passing each other at high speed will still be correctly detected as having no impact. Swept testing can miss *secondary* contacts that large-margin speculative contacts wouldn't, though. But you can combine both! Speculative contacts work with sweep testing; they are not mutually exclusive. To demonstrate this, consider the configuration options for the `Continuous` mode. From e336f2a43c784045023d7b94e1e1b36ef8dcf083 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 6 Feb 2022 12:47:39 -0600 Subject: [PATCH 434/947] Extra note on why you might want to limit margins. --- Documentation/ContinuousCollisionDetection.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Documentation/ContinuousCollisionDetection.md b/Documentation/ContinuousCollisionDetection.md index 5da183a94..2be8b43cc 100644 --- a/Documentation/ContinuousCollisionDetection.md +++ b/Documentation/ContinuousCollisionDetection.md @@ -29,9 +29,9 @@ For both the dynamic and the static, this creates a `CollidableDescription` from 2. The maximum speculative margin is `float.MaxValue`. In other words, there's no upper limit. 3. No sweep tests are used. Contacts are simply created from closest features. -Taken together, this makes most stuff just work. Performance stays high since speculative contacts only get created if the velocity is high enough to warrant them, and high velocity collisions tend to have robust behavior since speculative contacts get generated. +Taken together, this makes most stuff just work. Performance stays high since speculative contacts only get created if the velocity is high enough to warrant them, and high velocity collisions tend to have robust behavior since speculative contacts get generated. For most use cases, sticking with the default is a high performance and high quality option. -For most use cases, sticking with the default is a high performance and high quality option. +Sometimes, when the details of the simulation don't matter as much, it can be useful to limit the speculative margin to reduce the number of constraints that get generated. Perhaps you have a giant building that's collapsing and tens of thousands of bodies are moving rapidly in close proximity- that'll generate a lot of speculative contacts. You could probably get a noticeable speed boost by reducing the maximum margin on the building chunks to a small nonzero value with no noticeable impact on quality. # What are ghost collisions? In the solver, a contact constraint (speculative or not) acts like a plane. As far as the solver is concerned, the contact surface has unlimited horizontal extent. This is a perfectly fine approximation when the contacts are created at a reasonable location, but it can fail when objects are moving very quickly past each other. From 165874d3f739140881676658af4d0c54ae4a520d Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 6 Feb 2022 14:45:52 -0600 Subject: [PATCH 435/947] Bumping beta number. --- BepuPhysics/BepuPhysics.csproj | 2 +- BepuUtilities/BepuUtilities.csproj | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/BepuPhysics/BepuPhysics.csproj b/BepuPhysics/BepuPhysics.csproj index d2af20fda..0df0b65d9 100644 --- a/BepuPhysics/BepuPhysics.csproj +++ b/BepuPhysics/BepuPhysics.csproj @@ -1,7 +1,7 @@  net6.0 - 2.4.0-beta10 + 2.4.0-beta11 Bepu Entertainment LLC Ross Nordby Speedy real time physics simulation library. diff --git a/BepuUtilities/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj index 8646c158b..cf003f256 100644 --- a/BepuUtilities/BepuUtilities.csproj +++ b/BepuUtilities/BepuUtilities.csproj @@ -3,7 +3,7 @@ BepuUtilities BepuUtilities net6.0 - 2.4.0-beta10 + 2.4.0-beta11 Bepu Entertainment LLC Ross Nordby Supporting utilities library for BEPUphysics v2. From 9d41f18c2fafac286dc2ea2d41ab6859b6134a40 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 7 Feb 2022 16:04:45 -0600 Subject: [PATCH 436/947] Updated Demos.GL path and added a Demos.GL.sln. --- DemoRenderer.GL/Helpers.cs | 24 +- DemoRenderer.GL/Renderer.cs | 12 +- .../ShapeDrawing/ShapesExtractor.cs | 338 +++++++++++++++--- Demos.GL.sln | 286 +++++++++++++++ 4 files changed, 587 insertions(+), 73 deletions(-) create mode 100644 Demos.GL.sln diff --git a/DemoRenderer.GL/Helpers.cs b/DemoRenderer.GL/Helpers.cs index 4ead92849..dc1085c84 100644 --- a/DemoRenderer.GL/Helpers.cs +++ b/DemoRenderer.GL/Helpers.cs @@ -8,6 +8,7 @@ namespace DemoRenderer { public static class Helpers { + /// /// Creates an index buffer of the specified size for screenspace quads. /// @@ -41,7 +42,7 @@ public static uint[] GetQuadIndices(int quadCount) /// /// Creates an index buffer of the specified size for boxes. /// - /// Number of boxes to create indices for. + /// Number of boxes to create indices for. /// Index buffer for boxes. /// Using redundant indices for batches avoids a slow path for low triangle count instancing. This is hardware/driver specific; it may change on newer cards. public static uint[] GetBoxIndices(int boxCount) @@ -106,7 +107,7 @@ public static uint[] GetBoxIndices(int boxCount) /// RGB color to pack. /// Color packed into 32 bits. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static uint PackColor(in Vector3 color) + public static uint PackColor(Vector3 color) { const uint RScale = (1 << 11) - 1; const uint GScale = (1 << 11) - 1; @@ -144,7 +145,7 @@ public static void UnpackColor(uint packedColor, out Vector3 color) /// RGBA color to pack. /// Color packed into 32 bits. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static uint PackColor(in Vector4 color) + public static uint PackColor(Vector4 color) { var scaledColor = Vector4.Max(Vector4.Zero, Vector4.Min(Vector4.One, color)) * 255; return (uint)scaledColor.X | ((uint)scaledColor.Y << 8) | ((uint)scaledColor.Z << 16) | ((uint)scaledColor.W << 24); @@ -171,7 +172,7 @@ public static void UnpackColor(uint packedColor, out Vector4 color) /// Orientation to pack. /// W-less packed orientation, with remaining components negated to guarantee that the reconstructed positive W component is valid. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void PackOrientation(in Quaternion source, out Vector3 packed) + public static void PackOrientation(Quaternion source, out Vector3 packed) { packed = new Vector3(source.X, source.Y, source.Z); if (source.W < 0) @@ -198,13 +199,13 @@ static void PackDuplicateZeroSNORM(float source, out ushort packed) /// Orientation to pack. /// Packed orientation. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe static ulong PackOrientationU64(ref Quaternion source) + public unsafe static ulong PackOrientationU64(Quaternion source) { //This isn't exactly a clever packing, but with 64 bits, cleverness isn't required. ref var vectorSource = ref Unsafe.As(ref source.X); var clamped = Vector4.Max(new Vector4(-1), Vector4.Min(new Vector4(1), vectorSource)); - ulong packed; - ref var packedShorts = ref Unsafe.As(ref *&packed); + Unsafe.SkipInit(out ulong packed); + ref var packedShorts = ref Unsafe.As(ref packed); PackDuplicateZeroSNORM(clamped.X, out packedShorts); PackDuplicateZeroSNORM(clamped.Y, out Unsafe.Add(ref packedShorts, 1)); PackDuplicateZeroSNORM(clamped.Z, out Unsafe.Add(ref packedShorts, 2)); @@ -216,7 +217,7 @@ public unsafe static ulong PackOrientationU64(ref Quaternion source) static unsafe float UnpackDuplicateZeroSNORM(ushort packed) { var unpacked = (packed & ((1 << 15) - 1)) * (1f / ((1 << 15) - 1)); - ref var reinterpreted = ref Unsafe.As(ref *&unpacked); + ref var reinterpreted = ref Unsafe.As(ref unpacked); //Set the sign bit. reinterpreted |= (packed & (1u << 15)) << 16; return unpacked; @@ -275,5 +276,12 @@ public static void CheckForUndisposed(bool disposed, object o) { Debug.Assert(disposed, "An object of type " + o.GetType() + " was not disposed prior to finalization."); } + + public static void Dispose(ref T disposable) where T : IDisposable + { + if (disposable != null) + disposable.Dispose(); + disposable = default(T); + } } } diff --git a/DemoRenderer.GL/Renderer.cs b/DemoRenderer.GL/Renderer.cs index 48002b215..ac50453cc 100644 --- a/DemoRenderer.GL/Renderer.cs +++ b/DemoRenderer.GL/Renderer.cs @@ -128,15 +128,15 @@ public void Render(Camera camera) //All ray traced shapes use analytic coverage writes to get antialiasing. GL.Enable(EnableCap.SampleAlphaToCoverage); - SphereRenderer.Render(camera, Surface.Resolution, SpanConverter.AsSpan(Shapes.spheres.Span), 0, Shapes.spheres.Count); - CapsuleRenderer.Render(camera, Surface.Resolution, SpanConverter.AsSpan(Shapes.capsules.Span), 0, Shapes.capsules.Count); - CylinderRenderer.Render(camera, Surface.Resolution, SpanConverter.AsSpan(Shapes.cylinders.Span), 0, Shapes.cylinders.Count); + SphereRenderer.Render(camera, Surface.Resolution, SpanConverter.AsSpan(Shapes.ShapeCache.Spheres.Span), 0, Shapes.ShapeCache.Spheres.Count); + CapsuleRenderer.Render(camera, Surface.Resolution, SpanConverter.AsSpan(Shapes.ShapeCache.Capsules.Span), 0, Shapes.ShapeCache.Capsules.Count); + CylinderRenderer.Render(camera, Surface.Resolution, SpanConverter.AsSpan(Shapes.ShapeCache.Cylinders.Span), 0, Shapes.ShapeCache.Cylinders.Count); //Non-raytraced shapes just use regular opaque rendering. GL.Disable(EnableCap.SampleAlphaToCoverage); - BoxRenderer.Render(camera, Surface.Resolution, SpanConverter.AsSpan(Shapes.boxes.Span), 0, Shapes.boxes.Count); - TriangleRenderer.Render(camera, Surface.Resolution, SpanConverter.AsSpan(Shapes.triangles.Span), 0, Shapes.triangles.Count); - MeshRenderer.Render(camera, Surface.Resolution, SpanConverter.AsSpan(Shapes.meshes.Span), 0, Shapes.meshes.Count); + BoxRenderer.Render(camera, Surface.Resolution, SpanConverter.AsSpan(Shapes.ShapeCache.Boxes.Span), 0, Shapes.ShapeCache.Boxes.Count); + TriangleRenderer.Render(camera, Surface.Resolution, SpanConverter.AsSpan(Shapes.ShapeCache.Triangles.Span), 0, Shapes.ShapeCache.Triangles.Count); + MeshRenderer.Render(camera, Surface.Resolution, SpanConverter.AsSpan(Shapes.ShapeCache.Meshes.Span), 0, Shapes.ShapeCache.Meshes.Count); LineRenderer.Render(camera, Surface.Resolution, SpanConverter.AsSpan(Lines.lines.Span), 0, Lines.lines.Count); Background.Render(camera); diff --git a/DemoRenderer.GL/ShapeDrawing/ShapesExtractor.cs b/DemoRenderer.GL/ShapeDrawing/ShapesExtractor.cs index 248ad93df..450515338 100644 --- a/DemoRenderer.GL/ShapeDrawing/ShapesExtractor.cs +++ b/DemoRenderer.GL/ShapeDrawing/ShapesExtractor.cs @@ -6,18 +6,50 @@ using System; using System.Numerics; using System.Runtime.CompilerServices; +using System.Diagnostics; namespace DemoRenderer.ShapeDrawing { + public struct ShapeCache + { + internal QuickList Spheres; + internal QuickList Capsules; + internal QuickList Cylinders; + internal QuickList Boxes; + internal QuickList Triangles; + internal QuickList Meshes; + + public ShapeCache(int initialCapacityPerShapeType, BufferPool pool) + { + Spheres = new QuickList(initialCapacityPerShapeType, pool); + Capsules = new QuickList(initialCapacityPerShapeType, pool); + Cylinders = new QuickList(initialCapacityPerShapeType, pool); + Boxes = new QuickList(initialCapacityPerShapeType, pool); + Triangles = new QuickList(initialCapacityPerShapeType, pool); + Meshes = new QuickList(initialCapacityPerShapeType, pool); + } + public void Clear() + { + Spheres.Count = 0; + Capsules.Count = 0; + Cylinders.Count = 0; + Boxes.Count = 0; + Triangles.Count = 0; + Meshes.Count = 0; + } + public void Dispose(BufferPool pool) + { + Spheres.Dispose(pool); + Capsules.Dispose(pool); + Cylinders.Dispose(pool); + Boxes.Dispose(pool); + Triangles.Dispose(pool); + Meshes.Dispose(pool); + } + } public class ShapesExtractor : IDisposable { - //For now, we only have spheres. Later, once other shapes exist, this will be responsible for bucketing the different shape types and when necessary caching shape models. - internal QuickList spheres; - internal QuickList capsules; - internal QuickList cylinders; - internal QuickList boxes; - internal QuickList triangles; - internal QuickList meshes; + public ShapeCache ShapeCache; BufferPool pool; public MeshCache MeshCache; @@ -25,12 +57,7 @@ public class ShapesExtractor : IDisposable ParallelLooper looper; public ShapesExtractor(ParallelLooper looper, BufferPool pool, int initialCapacityPerShapeType = 1024) { - spheres = new QuickList(initialCapacityPerShapeType, pool); - capsules = new QuickList(initialCapacityPerShapeType, pool); - cylinders = new QuickList(initialCapacityPerShapeType, pool); - boxes = new QuickList(initialCapacityPerShapeType, pool); - triangles = new QuickList(initialCapacityPerShapeType, pool); - meshes = new QuickList(initialCapacityPerShapeType, pool); + ShapeCache = new ShapeCache(initialCapacityPerShapeType, pool); this.MeshCache = new MeshCache(pool); this.pool = pool; this.looper = looper; @@ -38,26 +65,21 @@ public ShapesExtractor(ParallelLooper looper, BufferPool pool, int initialCapaci public void ClearInstances() { - spheres.Count = 0; - capsules.Count = 0; - cylinders.Count = 0; - boxes.Count = 0; - triangles.Count = 0; - meshes.Count = 0; + ShapeCache.Clear(); } - private unsafe void AddCompoundChildren(ref Buffer children, Shapes shapes, in RigidPose pose, in Vector3 color) + private unsafe void AddCompoundChildren(ref Buffer children, Shapes shapes, RigidPose pose, Vector3 color, ref ShapeCache shapeCache, BufferPool pool) { for (int i = 0; i < children.Length; ++i) { ref var child = ref children[i]; Compound.GetWorldPose(child.LocalPose, pose, out var childPose); - AddShape(shapes, child.ShapeIndex, ref childPose, color); + AddShape(shapes, child.ShapeIndex, childPose, color, ref shapeCache, pool); } } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void AddShape(void* shapeData, int shapeType, Shapes shapes, ref RigidPose pose, in Vector3 color) + unsafe void AddShape(void* shapeData, int shapeType, Shapes shapes, RigidPose pose, Vector3 color, ref ShapeCache shapeCache, BufferPool pool) { //TODO: This should likely be swapped over to a registration-based virtualized table approach to more easily support custom shape extractors- //generic terrain windows and examples like voxel grids would benefit. @@ -70,7 +92,7 @@ public unsafe void AddShape(void* shapeData, int shapeType, Shapes shapes, ref R instance.Radius = Unsafe.AsRef(shapeData).Radius; Helpers.PackOrientation(pose.Orientation, out instance.PackedOrientation); instance.PackedColor = Helpers.PackColor(color); - spheres.Add(instance, pool); + shapeCache.Spheres.Add(instance, pool); } break; case Capsule.Id: @@ -80,9 +102,9 @@ public unsafe void AddShape(void* shapeData, int shapeType, Shapes shapes, ref R ref var capsule = ref Unsafe.AsRef(shapeData); instance.Radius = capsule.Radius; instance.HalfLength = capsule.HalfLength; - instance.PackedOrientation = Helpers.PackOrientationU64(ref pose.Orientation); + instance.PackedOrientation = Helpers.PackOrientationU64(pose.Orientation); instance.PackedColor = Helpers.PackColor(color); - capsules.Add(instance, pool); + shapeCache.Capsules.Add(instance, pool); } break; case Box.Id: @@ -95,7 +117,7 @@ public unsafe void AddShape(void* shapeData, int shapeType, Shapes shapes, ref R instance.HalfWidth = box.HalfWidth; instance.HalfHeight = box.HalfHeight; instance.HalfLength = box.HalfLength; - boxes.Add(instance, pool); + shapeCache.Boxes.Add(instance, pool); } break; case Triangle.Id: @@ -106,11 +128,11 @@ public unsafe void AddShape(void* shapeData, int shapeType, Shapes shapes, ref R instance.PackedColor = Helpers.PackColor(color); instance.B = triangle.B; instance.C = triangle.C; - instance.PackedOrientation = Helpers.PackOrientationU64(ref pose.Orientation); + instance.PackedOrientation = Helpers.PackOrientationU64(pose.Orientation); instance.X = pose.Position.X; instance.Y = pose.Position.Y; instance.Z = pose.Position.Z; - triangles.Add(instance, pool); + shapeCache.Triangles.Add(instance, pool); } break; case Cylinder.Id: @@ -120,9 +142,9 @@ public unsafe void AddShape(void* shapeData, int shapeType, Shapes shapes, ref R ref var cylinder = ref Unsafe.AsRef(shapeData); instance.Radius = cylinder.Radius; instance.HalfLength = cylinder.HalfLength; - instance.PackedOrientation = Helpers.PackOrientationU64(ref pose.Orientation); + instance.PackedOrientation = Helpers.PackOrientationU64(pose.Orientation); instance.PackedColor = Helpers.PackColor(color); - cylinders.Add(instance, pool); + shapeCache.Cylinders.Add(instance, pool); } break; case ConvexHull.Id: @@ -131,10 +153,17 @@ public unsafe void AddShape(void* shapeData, int shapeType, Shapes shapes, ref R MeshInstance instance; instance.Position = pose.Position; instance.PackedColor = Helpers.PackColor(color); - instance.PackedOrientation = Helpers.PackOrientationU64(ref pose.Orientation); + instance.PackedOrientation = Helpers.PackOrientationU64(pose.Orientation); instance.Scale = Vector3.One; - var id = (ulong)hull.Points.Memory ^ (ulong)hull.Points.Length; - if (!MeshCache.TryGetExistingMesh(id, out instance.VertexStart, out var vertices)) + //Memory can be reused, so we slightly reduce the probability of a bad reuse by taking the first 64 bits of data into the hash. + var id = (ulong)hull.Points.Memory ^ (ulong)hull.Points.Length ^ (*(ulong*)hull.Points.Memory); + bool meshExisted; + Buffer vertices; + lock (MeshCache) + { + meshExisted = MeshCache.TryGetExistingMesh(id, out instance.VertexStart, out vertices); + } + if (!meshExisted) { int triangleCount = 0; for (int i = 0; i < hull.FaceToVertexIndicesStart.Length; ++i) @@ -143,7 +172,10 @@ public unsafe void AddShape(void* shapeData, int shapeType, Shapes shapes, ref R triangleCount += faceVertexIndices.Length - 2; } instance.VertexCount = triangleCount * 3; - MeshCache.Allocate(id, instance.VertexCount, out instance.VertexStart, out vertices); + lock (MeshCache) + { + MeshCache.Allocate(id, instance.VertexCount, out instance.VertexStart, out vertices); + } //This is a fresh allocation, so we need to upload vertex data. int targetVertexIndex = 0; for (int i = 0; i < hull.FaceToVertexIndicesStart.Length; ++i) @@ -165,17 +197,17 @@ public unsafe void AddShape(void* shapeData, int shapeType, Shapes shapes, ref R { instance.VertexCount = vertices.Length; } - meshes.Add(instance, pool); + shapeCache.Meshes.Add(instance, pool); } break; case Compound.Id: { - AddCompoundChildren(ref Unsafe.AsRef(shapeData).Children, shapes, pose, color); + AddCompoundChildren(ref Unsafe.AsRef(shapeData).Children, shapes, pose, color, ref shapeCache, pool); } break; case BigCompound.Id: { - AddCompoundChildren(ref Unsafe.AsRef(shapeData).Children, shapes, pose, color); + AddCompoundChildren(ref Unsafe.AsRef(shapeData).Children, shapes, pose, color, ref shapeCache, pool); } break; case Mesh.Id: @@ -184,11 +216,18 @@ public unsafe void AddShape(void* shapeData, int shapeType, Shapes shapes, ref R MeshInstance instance; instance.Position = pose.Position; instance.PackedColor = Helpers.PackColor(color); - instance.PackedOrientation = Helpers.PackOrientationU64(ref pose.Orientation); + instance.PackedOrientation = Helpers.PackOrientationU64(pose.Orientation); instance.Scale = mesh.Scale; - var id = (ulong)mesh.Triangles.Memory ^ (ulong)mesh.Triangles.Length; + //Memory can be reused, so we slightly reduce the probability of a bad reuse by taking the first 64 bits of data into the hash. + var id = (ulong)mesh.Triangles.Memory ^ (ulong)mesh.Triangles.Length ^ (*(ulong*)mesh.Triangles.Memory); ; instance.VertexCount = mesh.Triangles.Length * 3; - if (MeshCache.Allocate(id, instance.VertexCount, out instance.VertexStart, out var vertices)) + bool newAllocation; + Buffer vertices; + lock (MeshCache) + { + newAllocation = MeshCache.Allocate(id, instance.VertexCount, out instance.VertexStart, out vertices); + } + if (newAllocation) { //This is a fresh allocation, so we need to upload vertex data. for (int i = 0; i < mesh.Triangles.Length; ++i) @@ -201,31 +240,45 @@ public unsafe void AddShape(void* shapeData, int shapeType, Shapes shapes, ref R vertices[baseVertexIndex + 2] = new Vector4(triangle.B, 1.0f); } } - meshes.Add(instance, pool); + shapeCache.Meshes.Add(instance, pool); } break; } } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public unsafe void AddShape(void* shapeData, int shapeType, Shapes shapes, RigidPose pose, Vector3 color) + { + AddShape(shapeData, shapeType, shapes, pose, color, ref ShapeCache, pool); + } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void AddShape(Shapes shapes, TypedIndex shapeIndex, ref RigidPose pose, in Vector3 color) + unsafe void AddShape(Shapes shapes, TypedIndex shapeIndex, RigidPose pose, Vector3 color, ref ShapeCache shapeCache, BufferPool pool) { if (shapeIndex.Exists) { shapes[shapeIndex.Type].GetShapeData(shapeIndex.Index, out var shapeData, out _); - AddShape(shapeData, shapeIndex.Type, shapes, ref pose, color); + AddShape(shapeData, shapeIndex.Type, shapes, pose, color, ref shapeCache, pool); + } + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public unsafe void AddShape(Shapes shapes, TypedIndex shapeIndex, RigidPose pose, Vector3 color) + { + if (shapeIndex.Exists) + { + shapes[shapeIndex.Type].GetShapeData(shapeIndex.Index, out var shapeData, out _); + AddShape(shapeData, shapeIndex.Type, shapes, pose, color, ref ShapeCache, pool); } } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void AddShape(TShape shape, Shapes shapes, ref RigidPose pose, in Vector3 color) where TShape : IShape + public unsafe void AddShape(TShape shape, Shapes shapes, RigidPose pose, Vector3 color) where TShape : IShape { - AddShape(Unsafe.AsPointer(ref shape), shape.TypeId, shapes, ref pose, color); + AddShape(Unsafe.AsPointer(ref shape), shape.TypeId, shapes, pose, color, ref ShapeCache, pool); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - void AddBodyShape(Shapes shapes, Bodies bodies, int setIndex, int indexInSet) + void AddBodyShape(Shapes shapes, Bodies bodies, int setIndex, int indexInSet, ref ShapeCache shapeCache, BufferPool pool) { ref var set = ref bodies.Sets[setIndex]; var handle = set.IndexToHandle[indexInSet]; @@ -235,9 +288,9 @@ void AddBodyShape(Shapes shapes, Bodies bodies, int setIndex, int indexInSet) //3) Activity state //The handle is hashed to get variation. ref var activity = ref set.Activity[indexInSet]; - ref var state = ref set.SolverStates[indexInSet]; Vector3 color; Helpers.UnpackColor((uint)HashHelper.Rehash(handle.Value), out Vector3 colorVariation); + ref var state = ref set.SolverStates[indexInSet]; if (Bodies.IsKinematic(state.Inertia.Local)) { var kinematicBase = new Vector3(0, 0.609f, 0.37f); @@ -265,11 +318,11 @@ void AddBodyShape(Shapes shapes, Bodies bodies, int setIndex, int indexInSet) color *= sleepTint; } - AddShape(shapes, set.Collidables[indexInSet].Shape, ref state.Motion.Pose, color); + AddShape(shapes, set.Collidables[indexInSet].Shape, state.Motion.Pose, color, ref shapeCache, pool); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - void AddStaticShape(Shapes shapes, Statics statics, int index) + void AddStaticShape(Shapes shapes, Statics statics, int index, ref ShapeCache shapeCache, BufferPool pool) { var handle = statics.IndexToHandle[index]; //Statics don't have any activity states. Just some simple variation on a central static color. @@ -278,10 +331,141 @@ void AddStaticShape(Shapes shapes, Statics statics, int index) var staticVariationSpan = new Vector3(0.07f, 0.07f, 0.03f); var color = staticBase + staticVariationSpan * colorVariation; ref var collidable = ref statics[index]; - AddShape(shapes, collidable.Shape, ref collidable.Pose, color); + AddShape(shapes, collidable.Shape, collidable.Pose, color, ref shapeCache, pool); } - public void AddInstances(Simulation simulation, IThreadDispatcher threadDispatcher = null) + struct Job + { + public int SimulationIndex; + //If the job is about statics, the set index will be -1. + public int SetIndex; + public int StartIndex; + public int Count; + } + QuickList jobs; + //The extractor can operate over one or multiple simulations. We cache them locally for threads to access. + Simulation[] simulations; + Simulation simulation; + Buffer workerCaches; + + void PrepareForMultithreadedExecution(IThreadDispatcher threadDispatcher) + { + jobs = new QuickList(128, pool); + looper.Dispatcher = threadDispatcher; + pool.Take(threadDispatcher.ThreadCount, out workerCaches); + for (int i = 0; i < workerCaches.Length; ++i) + { + workerCaches[i] = new ShapeCache(128, threadDispatcher.GetThreadMemoryPool(i)); + } + } + + void EndMultithreadedExecution() + { + jobs.Dispose(pool); + for (int i = 0; i < workerCaches.Length; ++i) + { + workerCaches[i].Dispose(looper.Dispatcher.GetThreadMemoryPool(i)); + } + looper.Dispatcher = null; + pool.Return(ref workerCaches); + } + + static void CreateJobs(Simulation simulation, int simulationIndex, ref QuickList jobs, BufferPool pool) + { + const int targetBodiesPerJob = 1024; + for (int setIndex = 0; setIndex < simulation.Bodies.Sets.Length; ++setIndex) + { + ref var set = ref simulation.Bodies.Sets[setIndex]; + if (set.Allocated && set.Count > 0) //active set can be allocated and have no bodies in it. + { + var jobCount = (set.Count + targetBodiesPerJob - 1) / targetBodiesPerJob; + var bodiesPerJob = set.Count / jobCount; + var remainder = set.Count - bodiesPerJob * jobCount; + var previousEnd = 0; + for (int j = 0; j < jobCount; ++j) + { + var count = j < remainder ? bodiesPerJob + 1 : bodiesPerJob; + jobs.Allocate(pool) = new Job { SimulationIndex = simulationIndex, SetIndex = setIndex, Count = count, StartIndex = previousEnd }; + previousEnd += count; + } + } + } + { + if (simulation.Statics.Count > 0) + { + var jobCount = (simulation.Statics.Count + targetBodiesPerJob - 1) / targetBodiesPerJob; + var bodiesPerJob = simulation.Statics.Count / jobCount; + var remainder = simulation.Statics.Count - bodiesPerJob * jobCount; + var previousEnd = 0; + for (int j = 0; j < jobCount; ++j) + { + var count = j < remainder ? bodiesPerJob + 1 : bodiesPerJob; + jobs.Allocate(pool) = new Job { SimulationIndex = simulationIndex, SetIndex = -1, Count = count, StartIndex = previousEnd }; + previousEnd += count; + } + } + } + } + + void AddShapesForJob(int jobIndex, int workerIndex) + { + var job = jobs[jobIndex]; + var simulation = simulations == null ? this.simulation : this.simulations[job.SimulationIndex]; + var pool = looper.Dispatcher.GetThreadMemoryPool(workerIndex); + + if (job.SetIndex >= 0) + { + ref var set = ref simulation.Bodies.Sets[job.SetIndex]; + var endIndex = job.StartIndex + job.Count; + Debug.Assert(endIndex <= set.Count); + for (int bodyIndex = job.StartIndex; bodyIndex < endIndex; ++bodyIndex) + { + AddBodyShape(simulation.Shapes, simulation.Bodies, job.SetIndex, bodyIndex, ref workerCaches[workerIndex], pool); + } + } + else + { + //It's a static. + var endIndex = job.StartIndex + job.Count; + Debug.Assert(endIndex <= simulation.Statics.Count); + for (int staticIndex = job.StartIndex; staticIndex < endIndex; ++staticIndex) + { + AddStaticShape(simulation.Shapes, simulation.Statics, staticIndex, ref workerCaches[workerIndex], pool); + } + } + } + + object workerShapeMergeLocker = new object(); + + void CopyWorkerCacheToMainCache(ref QuickList workerCache, ref QuickList mainCache) where TShape : unmanaged + { + if (workerCache.Count > 0) + { + int copyStartLocation; + lock (workerShapeMergeLocker) + { + var newCount = mainCache.Count + workerCache.Count; + mainCache.EnsureCapacity(newCount, pool); + copyStartLocation = mainCache.Count; + mainCache.Count = newCount; + } + workerCache.Span.CopyTo(0, mainCache.Span, copyStartLocation, workerCache.Count); + } + } + + void WorkerDone(int workerIndex) + { + //This fires when a worker finishes its work. We should copy the results into the main buffer. + ref var workerCache = ref workerCaches[workerIndex]; + CopyWorkerCacheToMainCache(ref workerCache.Spheres, ref ShapeCache.Spheres); + CopyWorkerCacheToMainCache(ref workerCache.Capsules, ref ShapeCache.Capsules); + CopyWorkerCacheToMainCache(ref workerCache.Boxes, ref ShapeCache.Boxes); + CopyWorkerCacheToMainCache(ref workerCache.Cylinders, ref ShapeCache.Cylinders); + CopyWorkerCacheToMainCache(ref workerCache.Triangles, ref ShapeCache.Triangles); + CopyWorkerCacheToMainCache(ref workerCache.Meshes, ref ShapeCache.Meshes); + } + + void AddShapesSequentially(Simulation simulation) { for (int i = 0; i < simulation.Bodies.Sets.Length; ++i) { @@ -290,25 +474,61 @@ public void AddInstances(Simulation simulation, IThreadDispatcher threadDispatch { for (int bodyIndex = 0; bodyIndex < set.Count; ++bodyIndex) { - AddBodyShape(simulation.Shapes, simulation.Bodies, i, bodyIndex); + AddBodyShape(simulation.Shapes, simulation.Bodies, i, bodyIndex, ref ShapeCache, pool); } } } for (int i = 0; i < simulation.Statics.Count; ++i) { - AddStaticShape(simulation.Shapes, simulation.Statics, i); + AddStaticShape(simulation.Shapes, simulation.Statics, i, ref ShapeCache, pool); + } + } + + + public void AddInstances(Simulation[] simulations, IThreadDispatcher threadDispatcher = null) + { + if (threadDispatcher != null && threadDispatcher.ThreadCount > 1) + { + this.simulations = simulations; + PrepareForMultithreadedExecution(threadDispatcher); + for (int simulationIndex = 0; simulationIndex < simulations.Length; ++simulationIndex) + { + CreateJobs(simulations[simulationIndex], simulationIndex, ref jobs, pool); + } + looper.For(0, jobs.Count, AddShapesForJob, WorkerDone); + EndMultithreadedExecution(); + this.simulations = default; + } + else + { + for (int simulationIndex = 0; simulationIndex < simulations.Length; ++simulationIndex) + { + AddShapesSequentially(simulations[simulationIndex]); + } + } + } + + public void AddInstances(Simulation simulation, IThreadDispatcher threadDispatcher = null) + { + if (threadDispatcher != null) + { + this.simulation = simulation; + PrepareForMultithreadedExecution(threadDispatcher); + CreateJobs(simulation, 0, ref jobs, pool); + looper.For(0, jobs.Count, AddShapesForJob, WorkerDone); + EndMultithreadedExecution(); + this.simulation = null; + } + else + { + AddShapesSequentially(simulation); } } public void Dispose() { + ShapeCache.Dispose(pool); MeshCache.Dispose(); - spheres.Dispose(pool); - capsules.Dispose(pool); - cylinders.Dispose(pool); - boxes.Dispose(pool); - triangles.Dispose(pool); - meshes.Dispose(pool); } } } diff --git a/Demos.GL.sln b/Demos.GL.sln new file mode 100644 index 000000000..49b62c2f0 --- /dev/null +++ b/Demos.GL.sln @@ -0,0 +1,286 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Version 17 +VisualStudioVersion = 17.0.31423.177 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "DemoContentLoader", "DemoContentLoader\DemoContentLoader.csproj", "{FABD2BE3-697B-4B57-85D0-1077A3198C5C}" +EndProject +Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "DemoUtilities", "DemoUtilities\DemoUtilities.csproj", "{499C899F-CD56-476E-AFF8-85A8C29B19BF}" +EndProject +Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "DemoContentBuilder", "DemoContentBuilder\DemoContentBuilder.csproj", "{6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}" +EndProject +Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "BepuUtilities", "BepuUtilities\BepuUtilities.csproj", "{8D3FB6BE-2726-4479-8AF2-13F593314AC0}" +EndProject +Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "BepuPhysics", "BepuPhysics\BepuPhysics.csproj", "{5FBC743A-8911-4DE6-B136-C0B274E1B185}" +EndProject +Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "Demos", "Demos.GL\Demos.csproj", "{76B75BB7-7AC8-4942-A3EA-314CB04C0B85}" +EndProject +Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "DemoRenderer", "DemoRenderer.GL\DemoRenderer.csproj", "{85C39598-1A63-4944-B619-25F3CD76C7A2}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Debug|ARM = Debug|ARM + Debug|x64 = Debug|x64 + Debug|x86 = Debug|x86 + Release|Any CPU = Release|Any CPU + Release|ARM = Release|ARM + Release|x64 = Release|x64 + Release|x86 = Release|x86 + ReleaseNoProfiling|Any CPU = ReleaseNoProfiling|Any CPU + ReleaseNoProfiling|ARM = ReleaseNoProfiling|ARM + ReleaseNoProfiling|x64 = ReleaseNoProfiling|x64 + ReleaseNoProfiling|x86 = ReleaseNoProfiling|x86 + ReleaseStrip|Any CPU = ReleaseStrip|Any CPU + ReleaseStrip|ARM = ReleaseStrip|ARM + ReleaseStrip|x64 = ReleaseStrip|x64 + ReleaseStrip|x86 = ReleaseStrip|x86 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.Debug|Any CPU.Build.0 = Debug|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.Debug|ARM.ActiveCfg = Debug|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.Debug|ARM.Build.0 = Debug|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.Debug|x64.ActiveCfg = Debug|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.Debug|x64.Build.0 = Debug|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.Debug|x86.ActiveCfg = Debug|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.Debug|x86.Build.0 = Debug|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.Release|Any CPU.ActiveCfg = Release|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.Release|Any CPU.Build.0 = Release|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.Release|ARM.ActiveCfg = Release|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.Release|ARM.Build.0 = Release|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.Release|x64.ActiveCfg = Release|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.Release|x64.Build.0 = Release|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.Release|x86.ActiveCfg = Release|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.Release|x86.Build.0 = Release|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.ReleaseNoProfiling|Any CPU.ActiveCfg = Release|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.ReleaseNoProfiling|Any CPU.Build.0 = Release|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.ReleaseNoProfiling|ARM.ActiveCfg = Release|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.ReleaseNoProfiling|ARM.Build.0 = Release|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.ReleaseNoProfiling|x64.ActiveCfg = Release|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.ReleaseNoProfiling|x64.Build.0 = Release|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.ReleaseNoProfiling|x86.ActiveCfg = Release|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.ReleaseNoProfiling|x86.Build.0 = Release|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.ReleaseStrip|Any CPU.ActiveCfg = Release|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.ReleaseStrip|Any CPU.Build.0 = Release|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.ReleaseStrip|ARM.ActiveCfg = Release|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.ReleaseStrip|ARM.Build.0 = Release|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.ReleaseStrip|x64.ActiveCfg = Release|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.ReleaseStrip|x64.Build.0 = Release|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.ReleaseStrip|x86.ActiveCfg = Release|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.ReleaseStrip|x86.Build.0 = Release|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.Debug|Any CPU.Build.0 = Debug|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.Debug|ARM.ActiveCfg = Debug|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.Debug|ARM.Build.0 = Debug|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.Debug|x64.ActiveCfg = Debug|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.Debug|x64.Build.0 = Debug|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.Debug|x86.ActiveCfg = Debug|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.Debug|x86.Build.0 = Debug|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.Release|Any CPU.ActiveCfg = Release|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.Release|Any CPU.Build.0 = Release|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.Release|ARM.ActiveCfg = Release|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.Release|ARM.Build.0 = Release|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.Release|x64.ActiveCfg = Release|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.Release|x64.Build.0 = Release|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.Release|x86.ActiveCfg = Release|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.Release|x86.Build.0 = Release|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.ReleaseNoProfiling|Any CPU.ActiveCfg = Release|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.ReleaseNoProfiling|Any CPU.Build.0 = Release|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.ReleaseNoProfiling|ARM.ActiveCfg = Release|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.ReleaseNoProfiling|ARM.Build.0 = Release|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.ReleaseNoProfiling|x64.ActiveCfg = Release|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.ReleaseNoProfiling|x64.Build.0 = Release|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.ReleaseNoProfiling|x86.ActiveCfg = Release|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.ReleaseNoProfiling|x86.Build.0 = Release|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.ReleaseStrip|Any CPU.ActiveCfg = Release|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.ReleaseStrip|Any CPU.Build.0 = Release|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.ReleaseStrip|ARM.ActiveCfg = Release|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.ReleaseStrip|ARM.Build.0 = Release|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.ReleaseStrip|x64.ActiveCfg = Release|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.ReleaseStrip|x64.Build.0 = Release|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.ReleaseStrip|x86.ActiveCfg = Release|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.ReleaseStrip|x86.Build.0 = Release|Any CPU + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.Debug|Any CPU.ActiveCfg = Debug|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.Debug|Any CPU.Build.0 = Debug|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.Debug|ARM.ActiveCfg = Debug|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.Debug|ARM.Build.0 = Debug|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.Debug|x64.ActiveCfg = Debug|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.Debug|x64.Build.0 = Debug|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.Debug|x86.ActiveCfg = Debug|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.Debug|x86.Build.0 = Debug|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.Release|Any CPU.ActiveCfg = Release|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.Release|Any CPU.Build.0 = Release|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.Release|ARM.ActiveCfg = Release|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.Release|ARM.Build.0 = Release|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.Release|x64.ActiveCfg = Release|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.Release|x64.Build.0 = Release|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.Release|x86.ActiveCfg = Release|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.Release|x86.Build.0 = Release|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.ReleaseNoProfiling|Any CPU.ActiveCfg = Release|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.ReleaseNoProfiling|Any CPU.Build.0 = Release|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.ReleaseNoProfiling|ARM.ActiveCfg = Release|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.ReleaseNoProfiling|ARM.Build.0 = Release|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.ReleaseNoProfiling|x64.ActiveCfg = Release|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.ReleaseNoProfiling|x64.Build.0 = Release|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.ReleaseNoProfiling|x86.ActiveCfg = Release|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.ReleaseNoProfiling|x86.Build.0 = Release|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.ReleaseStrip|Any CPU.ActiveCfg = Release|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.ReleaseStrip|Any CPU.Build.0 = Release|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.ReleaseStrip|ARM.ActiveCfg = Release|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.ReleaseStrip|ARM.Build.0 = Release|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.ReleaseStrip|x64.ActiveCfg = Release|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.ReleaseStrip|x64.Build.0 = Release|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.ReleaseStrip|x86.ActiveCfg = Release|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.ReleaseStrip|x86.Build.0 = Release|x64 + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.Debug|Any CPU.Build.0 = Debug|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.Debug|ARM.ActiveCfg = Debug|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.Debug|ARM.Build.0 = Debug|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.Debug|x64.ActiveCfg = Debug|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.Debug|x64.Build.0 = Debug|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.Debug|x86.ActiveCfg = Debug|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.Debug|x86.Build.0 = Debug|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.Release|Any CPU.ActiveCfg = Release|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.Release|Any CPU.Build.0 = Release|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.Release|ARM.ActiveCfg = Release|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.Release|ARM.Build.0 = Release|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.Release|x64.ActiveCfg = Release|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.Release|x64.Build.0 = Release|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.Release|x86.ActiveCfg = Release|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.Release|x86.Build.0 = Release|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.ReleaseNoProfiling|Any CPU.ActiveCfg = Release|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.ReleaseNoProfiling|Any CPU.Build.0 = Release|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.ReleaseNoProfiling|ARM.ActiveCfg = Release|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.ReleaseNoProfiling|ARM.Build.0 = Release|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.ReleaseNoProfiling|x64.ActiveCfg = Release|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.ReleaseNoProfiling|x64.Build.0 = Release|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.ReleaseNoProfiling|x86.ActiveCfg = Release|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.ReleaseNoProfiling|x86.Build.0 = Release|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.ReleaseStrip|Any CPU.ActiveCfg = Release|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.ReleaseStrip|Any CPU.Build.0 = Release|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.ReleaseStrip|ARM.ActiveCfg = Release|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.ReleaseStrip|ARM.Build.0 = Release|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.ReleaseStrip|x64.ActiveCfg = Release|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.ReleaseStrip|x64.Build.0 = Release|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.ReleaseStrip|x86.ActiveCfg = Release|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.ReleaseStrip|x86.Build.0 = Release|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.Debug|Any CPU.Build.0 = Debug|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.Debug|ARM.ActiveCfg = Debug|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.Debug|ARM.Build.0 = Debug|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.Debug|x64.ActiveCfg = Debug|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.Debug|x64.Build.0 = Debug|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.Debug|x86.ActiveCfg = Debug|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.Debug|x86.Build.0 = Debug|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.Release|Any CPU.ActiveCfg = Release|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.Release|Any CPU.Build.0 = Release|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.Release|ARM.ActiveCfg = Release|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.Release|ARM.Build.0 = Release|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.Release|x64.ActiveCfg = Release|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.Release|x64.Build.0 = Release|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.Release|x86.ActiveCfg = Release|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.Release|x86.Build.0 = Release|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.ReleaseNoProfiling|Any CPU.ActiveCfg = ReleaseNoProfiling|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.ReleaseNoProfiling|Any CPU.Build.0 = ReleaseNoProfiling|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.ReleaseNoProfiling|ARM.ActiveCfg = ReleaseNoProfiling|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.ReleaseNoProfiling|ARM.Build.0 = ReleaseNoProfiling|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.ReleaseNoProfiling|x64.ActiveCfg = ReleaseNoProfiling|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.ReleaseNoProfiling|x64.Build.0 = ReleaseNoProfiling|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.ReleaseNoProfiling|x86.ActiveCfg = ReleaseNoProfiling|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.ReleaseNoProfiling|x86.Build.0 = ReleaseNoProfiling|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.ReleaseStrip|Any CPU.ActiveCfg = ReleaseNoProfiling|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.ReleaseStrip|Any CPU.Build.0 = ReleaseNoProfiling|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.ReleaseStrip|ARM.ActiveCfg = ReleaseNoProfiling|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.ReleaseStrip|ARM.Build.0 = ReleaseNoProfiling|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.ReleaseStrip|x64.ActiveCfg = ReleaseNoProfiling|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.ReleaseStrip|x64.Build.0 = ReleaseNoProfiling|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.ReleaseStrip|x86.ActiveCfg = ReleaseNoProfiling|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.ReleaseStrip|x86.Build.0 = ReleaseNoProfiling|Any CPU + {76B75BB7-7AC8-4942-A3EA-314CB04C0B85}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {76B75BB7-7AC8-4942-A3EA-314CB04C0B85}.Debug|Any CPU.Build.0 = Debug|Any CPU + {76B75BB7-7AC8-4942-A3EA-314CB04C0B85}.Debug|ARM.ActiveCfg = Debug|Any CPU + {76B75BB7-7AC8-4942-A3EA-314CB04C0B85}.Debug|ARM.Build.0 = Debug|Any CPU + {76B75BB7-7AC8-4942-A3EA-314CB04C0B85}.Debug|x64.ActiveCfg = Debug|Any CPU + {76B75BB7-7AC8-4942-A3EA-314CB04C0B85}.Debug|x64.Build.0 = Debug|Any CPU + {76B75BB7-7AC8-4942-A3EA-314CB04C0B85}.Debug|x86.ActiveCfg = Debug|Any CPU + {76B75BB7-7AC8-4942-A3EA-314CB04C0B85}.Debug|x86.Build.0 = Debug|Any CPU + {76B75BB7-7AC8-4942-A3EA-314CB04C0B85}.Release|Any CPU.ActiveCfg = Release|Any CPU + {76B75BB7-7AC8-4942-A3EA-314CB04C0B85}.Release|Any CPU.Build.0 = Release|Any CPU + {76B75BB7-7AC8-4942-A3EA-314CB04C0B85}.Release|ARM.ActiveCfg = Release|Any CPU + {76B75BB7-7AC8-4942-A3EA-314CB04C0B85}.Release|ARM.Build.0 = Release|Any CPU + {76B75BB7-7AC8-4942-A3EA-314CB04C0B85}.Release|x64.ActiveCfg = Release|Any CPU + {76B75BB7-7AC8-4942-A3EA-314CB04C0B85}.Release|x64.Build.0 = Release|Any CPU + {76B75BB7-7AC8-4942-A3EA-314CB04C0B85}.Release|x86.ActiveCfg = Release|Any CPU + {76B75BB7-7AC8-4942-A3EA-314CB04C0B85}.Release|x86.Build.0 = Release|Any CPU + {76B75BB7-7AC8-4942-A3EA-314CB04C0B85}.ReleaseNoProfiling|Any CPU.ActiveCfg = Release|Any CPU + {76B75BB7-7AC8-4942-A3EA-314CB04C0B85}.ReleaseNoProfiling|Any CPU.Build.0 = Release|Any CPU + {76B75BB7-7AC8-4942-A3EA-314CB04C0B85}.ReleaseNoProfiling|ARM.ActiveCfg = Release|Any CPU + {76B75BB7-7AC8-4942-A3EA-314CB04C0B85}.ReleaseNoProfiling|ARM.Build.0 = Release|Any CPU + {76B75BB7-7AC8-4942-A3EA-314CB04C0B85}.ReleaseNoProfiling|x64.ActiveCfg = Release|Any CPU + {76B75BB7-7AC8-4942-A3EA-314CB04C0B85}.ReleaseNoProfiling|x64.Build.0 = Release|Any CPU + {76B75BB7-7AC8-4942-A3EA-314CB04C0B85}.ReleaseNoProfiling|x86.ActiveCfg = Release|Any CPU + {76B75BB7-7AC8-4942-A3EA-314CB04C0B85}.ReleaseNoProfiling|x86.Build.0 = Release|Any CPU + {76B75BB7-7AC8-4942-A3EA-314CB04C0B85}.ReleaseStrip|Any CPU.ActiveCfg = ReleaseStrip|Any CPU + {76B75BB7-7AC8-4942-A3EA-314CB04C0B85}.ReleaseStrip|Any CPU.Build.0 = ReleaseStrip|Any CPU + {76B75BB7-7AC8-4942-A3EA-314CB04C0B85}.ReleaseStrip|ARM.ActiveCfg = ReleaseStrip|Any CPU + {76B75BB7-7AC8-4942-A3EA-314CB04C0B85}.ReleaseStrip|ARM.Build.0 = ReleaseStrip|Any CPU + {76B75BB7-7AC8-4942-A3EA-314CB04C0B85}.ReleaseStrip|x64.ActiveCfg = ReleaseStrip|Any CPU + {76B75BB7-7AC8-4942-A3EA-314CB04C0B85}.ReleaseStrip|x64.Build.0 = ReleaseStrip|Any CPU + {76B75BB7-7AC8-4942-A3EA-314CB04C0B85}.ReleaseStrip|x86.ActiveCfg = ReleaseStrip|Any CPU + {76B75BB7-7AC8-4942-A3EA-314CB04C0B85}.ReleaseStrip|x86.Build.0 = ReleaseStrip|Any CPU + {85C39598-1A63-4944-B619-25F3CD76C7A2}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {85C39598-1A63-4944-B619-25F3CD76C7A2}.Debug|Any CPU.Build.0 = Debug|Any CPU + {85C39598-1A63-4944-B619-25F3CD76C7A2}.Debug|ARM.ActiveCfg = Debug|Any CPU + {85C39598-1A63-4944-B619-25F3CD76C7A2}.Debug|ARM.Build.0 = Debug|Any CPU + {85C39598-1A63-4944-B619-25F3CD76C7A2}.Debug|x64.ActiveCfg = Debug|Any CPU + {85C39598-1A63-4944-B619-25F3CD76C7A2}.Debug|x64.Build.0 = Debug|Any CPU + {85C39598-1A63-4944-B619-25F3CD76C7A2}.Debug|x86.ActiveCfg = Debug|Any CPU + {85C39598-1A63-4944-B619-25F3CD76C7A2}.Debug|x86.Build.0 = Debug|Any CPU + {85C39598-1A63-4944-B619-25F3CD76C7A2}.Release|Any CPU.ActiveCfg = Release|Any CPU + {85C39598-1A63-4944-B619-25F3CD76C7A2}.Release|Any CPU.Build.0 = Release|Any CPU + {85C39598-1A63-4944-B619-25F3CD76C7A2}.Release|ARM.ActiveCfg = Release|Any CPU + {85C39598-1A63-4944-B619-25F3CD76C7A2}.Release|ARM.Build.0 = Release|Any CPU + {85C39598-1A63-4944-B619-25F3CD76C7A2}.Release|x64.ActiveCfg = Release|Any CPU + {85C39598-1A63-4944-B619-25F3CD76C7A2}.Release|x64.Build.0 = Release|Any CPU + {85C39598-1A63-4944-B619-25F3CD76C7A2}.Release|x86.ActiveCfg = Release|Any CPU + {85C39598-1A63-4944-B619-25F3CD76C7A2}.Release|x86.Build.0 = Release|Any CPU + {85C39598-1A63-4944-B619-25F3CD76C7A2}.ReleaseNoProfiling|Any CPU.ActiveCfg = Release|Any CPU + {85C39598-1A63-4944-B619-25F3CD76C7A2}.ReleaseNoProfiling|Any CPU.Build.0 = Release|Any CPU + {85C39598-1A63-4944-B619-25F3CD76C7A2}.ReleaseNoProfiling|ARM.ActiveCfg = Release|Any CPU + {85C39598-1A63-4944-B619-25F3CD76C7A2}.ReleaseNoProfiling|ARM.Build.0 = Release|Any CPU + {85C39598-1A63-4944-B619-25F3CD76C7A2}.ReleaseNoProfiling|x64.ActiveCfg = Release|Any CPU + {85C39598-1A63-4944-B619-25F3CD76C7A2}.ReleaseNoProfiling|x64.Build.0 = Release|Any CPU + {85C39598-1A63-4944-B619-25F3CD76C7A2}.ReleaseNoProfiling|x86.ActiveCfg = Release|Any CPU + {85C39598-1A63-4944-B619-25F3CD76C7A2}.ReleaseNoProfiling|x86.Build.0 = Release|Any CPU + {85C39598-1A63-4944-B619-25F3CD76C7A2}.ReleaseStrip|Any CPU.ActiveCfg = Release|Any CPU + {85C39598-1A63-4944-B619-25F3CD76C7A2}.ReleaseStrip|Any CPU.Build.0 = Release|Any CPU + {85C39598-1A63-4944-B619-25F3CD76C7A2}.ReleaseStrip|ARM.ActiveCfg = Release|Any CPU + {85C39598-1A63-4944-B619-25F3CD76C7A2}.ReleaseStrip|ARM.Build.0 = Release|Any CPU + {85C39598-1A63-4944-B619-25F3CD76C7A2}.ReleaseStrip|x64.ActiveCfg = Release|Any CPU + {85C39598-1A63-4944-B619-25F3CD76C7A2}.ReleaseStrip|x64.Build.0 = Release|Any CPU + {85C39598-1A63-4944-B619-25F3CD76C7A2}.ReleaseStrip|x86.ActiveCfg = Release|Any CPU + {85C39598-1A63-4944-B619-25F3CD76C7A2}.ReleaseStrip|x86.Build.0 = Release|Any CPU + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection + GlobalSection(ExtensibilityGlobals) = postSolution + SolutionGuid = {29758AC0-E221-4C61-AC3D-16DD9A722844} + EndGlobalSection + GlobalSection(Performance) = preSolution + HasPerformanceSessions = true + EndGlobalSection + GlobalSection(Performance) = preSolution + HasPerformanceSessions = true + EndGlobalSection + GlobalSection(Performance) = preSolution + HasPerformanceSessions = true + EndGlobalSection + GlobalSection(Performance) = preSolution + HasPerformanceSessions = true + EndGlobalSection + GlobalSection(Performance) = preSolution + HasPerformanceSessions = true + EndGlobalSection +EndGlobal From ce64ee2e2d5083073ca73de06e28cacea4183e18 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 7 Feb 2022 16:57:32 -0600 Subject: [PATCH 437/947] Updated readme/building for demos.gl.sln. --- Documentation/Building.md | 4 ++-- README.md | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/Documentation/Building.md b/Documentation/Building.md index 51110963b..542c435a3 100644 --- a/Documentation/Building.md +++ b/Documentation/Building.md @@ -12,9 +12,9 @@ The libraries target .NET 6. ## Demos -`Demos.sln` contains all the projects necessary to build and run the demos application. The default demo renderer uses DX11, and the content pipeline's shader compiler requires the Windows SDK. The demos application targets .NET 6. +`Demos.sln` contains all the projects necessary to build and run the demos application. The default demo renderer uses DX11, and the content pipeline's shader compiler requires the Windows SDK. There is also a Demos.GL.sln that uses OpenGL and should run on other platforms. The demos can be run from the command line (in the repo root directory) with `dotnet run --project Demos/Demos.csproj -c Release` or `dotnet run --project Demos.GL/Demos.csproj -c Release`. -There's also an [OpenGL version of the demos](https://github.com/bepu/bepuphysics2/tree/master/Demos.GL). You can run it from the command line in the repository root using `dotnet run --project Demos.GL/Demos.csproj -c Release`. +The demos applications target .NET 6. ## Build Configurations diff --git a/README.md b/README.md index 53a82499f..7a8118830 100644 --- a/README.md +++ b/README.md @@ -3,7 +3,7 @@ This is the repo for the bepuphysics v2 library, a complete rewrite of the C# 3d rigid body physics engine [BEPUphysics v1](https://github.com/bepu/bepuphysics1). -The BepuPhysics and BepuUtilities libraries target .NET 6 and should work on any supported platform. The demos use DX11 by default. There is also an [OpenGL version of the demos](https://github.com/bepu/bepuphysics2/tree/master/Demos.GL) for other platforms that you can run from the command line in the repository root using `dotnet run --project Demos.GL/Demos.csproj -c Release`. +The BepuPhysics and BepuUtilities libraries target .NET 6 and should work on any supported platform. The demos application, Demos.sln, uses DX11 by default. There is also a Demos.GL.sln that uses OpenGL and should run on other platforms. The demos can be run from the command line (in the repo root directory) with `dotnet run --project Demos/Demos.csproj -c Release` or `dotnet run --project Demos.GL/Demos.csproj -c Release`. The physics engine heavily uses `System.Numerics.Vectors` types, so to get good performance, you'll need a compiler which can consume those types (like RyuJIT). From 309e695864594a5877fdba4f783a78a27e419503 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 8 Feb 2022 16:18:11 -0600 Subject: [PATCH 438/947] GetBoundsPointers and UpdateBounds moved into Tree. --- BepuPhysics/CollisionDetection/BroadPhase.cs | 50 ++++++++----------- BepuPhysics/CollisionDetection/NarrowPhase.cs | 4 +- BepuPhysics/Trees/Tree.cs | 48 +++++++++++++++++- BepuPhysics/Trees/Tree_Refit.cs | 9 ++-- 4 files changed, 74 insertions(+), 37 deletions(-) diff --git a/BepuPhysics/CollisionDetection/BroadPhase.cs b/BepuPhysics/CollisionDetection/BroadPhase.cs index 876ea5400..73794b982 100644 --- a/BepuPhysics/CollisionDetection/BroadPhase.cs +++ b/BepuPhysics/CollisionDetection/BroadPhase.cs @@ -82,39 +82,27 @@ public bool RemoveStaticAt(int index, out CollidableReference movedLeaf) return RemoveAt(index, ref StaticTree, ref staticLeaves, out movedLeaf); } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void GetBoundsPointers(int broadPhaseIndex, ref Tree tree, out Vector3* minPointer, out Vector3* maxPointer) - { - var leaf = tree.Leaves[broadPhaseIndex]; - var nodeChild = (&tree.Nodes.Memory[leaf.NodeIndex].A) + leaf.ChildIndex; - minPointer = &nodeChild->Min; - maxPointer = &nodeChild->Max; - } + /// + /// Gets pointers to the leaf's bounds stored in the broad phase's active tree. + /// + /// Index of the active collidable to examine. + /// Pointer to the minimum bounds in the tree. + /// Pointer to the maximum bounds in the tree. [MethodImpl(MethodImplOptions.AggressiveInlining)] public void GetActiveBoundsPointers(int index, out Vector3* minPointer, out Vector3* maxPointer) { - GetBoundsPointers(index, ref ActiveTree, out minPointer, out maxPointer); + ActiveTree.GetBoundsPointers(index, out minPointer, out maxPointer); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void GetStaticBoundsPointers(int index, out Vector3* minPointer, out Vector3* maxPointer) - { - GetBoundsPointers(index, ref StaticTree, out minPointer, out maxPointer); - } - /// - /// Applies updated bounds to the given leaf index in the given tree, refitting the tree to match. + /// Gets pointers to the leaf's bounds stored in the broad phase's static tree. /// - /// Index of the leaf in the tree to update. - /// Tree containing the leaf to update. - /// New minimum bounds for the leaf. - /// New maximum bounds for the leaf. - public unsafe static void UpdateBounds(int broadPhaseIndex, ref Tree tree, in Vector3 min, in Vector3 max) + /// Index of the static to examine. + /// Pointer to the minimum bounds in the tree. + /// Pointer to the maximum bounds in the tree. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void GetStaticBoundsPointers(int index, out Vector3* minPointer, out Vector3* maxPointer) { - GetBoundsPointers(broadPhaseIndex, ref tree, out var minPointer, out var maxPointer); - *minPointer = min; - *maxPointer = max; - tree.RefitForNodeBoundsChange(tree.Leaves[broadPhaseIndex].NodeIndex); + StaticTree.GetBoundsPointers(index, out minPointer, out maxPointer); } /// @@ -123,9 +111,10 @@ public unsafe static void UpdateBounds(int broadPhaseIndex, ref Tree tree, in Ve /// Index of the leaf to update. /// New minimum bounds for the leaf. /// New maximum bounds for the leaf. - public void UpdateActiveBounds(int broadPhaseIndex, in Vector3 min, in Vector3 max) + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void UpdateActiveBounds(int broadPhaseIndex, Vector3 min, Vector3 max) { - UpdateBounds(broadPhaseIndex, ref ActiveTree, min, max); + ActiveTree.UpdateBounds(broadPhaseIndex, min, max); } /// /// Applies updated bounds to the given active leaf index, refitting the tree to match. @@ -133,9 +122,10 @@ public void UpdateActiveBounds(int broadPhaseIndex, in Vector3 min, in Vector3 m /// Index of the leaf to update. /// New minimum bounds for the leaf. /// New maximum bounds for the leaf. - public void UpdateStaticBounds(int broadPhaseIndex, in Vector3 min, in Vector3 max) + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void UpdateStaticBounds(int broadPhaseIndex, Vector3 min, Vector3 max) { - UpdateBounds(broadPhaseIndex, ref StaticTree, min, max); + StaticTree.UpdateBounds(broadPhaseIndex, min, max); } int frameIndex; diff --git a/BepuPhysics/CollisionDetection/NarrowPhase.cs b/BepuPhysics/CollisionDetection/NarrowPhase.cs index 0a45bd3c3..4e06ebafb 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhase.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhase.cs @@ -530,8 +530,8 @@ private unsafe void AddBatchEntries(int workerIndex, ref OverlapWorker overlapWo var bInStaticTree = pair.B.Mobility == CollidableMobility.Static || Simulation.Bodies.HandleToLocation[pair.B.BodyHandle.Value].SetIndex > 0; ref var aTree = ref aInStaticTree ? ref Simulation.BroadPhase.StaticTree : ref Simulation.BroadPhase.ActiveTree; ref var bTree = ref bInStaticTree ? ref Simulation.BroadPhase.StaticTree : ref Simulation.BroadPhase.ActiveTree; - BroadPhase.GetBoundsPointers(broadPhaseIndexA, ref aTree, out var aMin, out var aMax); - BroadPhase.GetBoundsPointers(broadPhaseIndexB, ref bTree, out var bMin, out var bMax); + aTree.GetBoundsPointers(broadPhaseIndexA, out var aMin, out var aMax); + bTree.GetBoundsPointers(broadPhaseIndexB, out var bMin, out var bMax); var maximumRadiusA = (*aMax - *aMin).Length() * 0.5f; var maximumRadiusB = (*bMax - *bMin).Length() * 0.5f; if ((velocityA.Angular.Length() * maximumRadiusA + velocityB.Angular.Length() * maximumRadiusB + (velocityB.Linear - velocityA.Linear).Length()) * timestepDuration > speculativeMargin) diff --git a/BepuPhysics/Trees/Tree.cs b/BepuPhysics/Trees/Tree.cs index 017212fd0..66117334e 100644 --- a/BepuPhysics/Trees/Tree.cs +++ b/BepuPhysics/Trees/Tree.cs @@ -1,6 +1,7 @@ using BepuUtilities.Memory; using System; using System.Diagnostics; +using System.Numerics; using System.Runtime.CompilerServices; @@ -8,9 +9,18 @@ namespace BepuPhysics.Trees { public unsafe partial struct Tree { + /// + /// Buffer of nodes in the tree. + /// public Buffer Nodes; + /// + /// Buffer of metanodes in the tree. Metanodes contain metadata that aren't read during most query operations but are useful for bookkeeping. + /// public Buffer Metanodes; int nodeCount; + /// + /// Gets or sets the number of nodes in the tree. + /// public int NodeCount { readonly get @@ -23,8 +33,14 @@ readonly get } } + /// + /// Buffer of leaves in the tree. + /// public Buffer Leaves; int leafCount; + /// + /// Gets or sets the number of leaves in the tree. + /// public int LeafCount { readonly get @@ -54,10 +70,40 @@ int AddLeaf(int nodeIndex, int childIndex) return leafCount++; } + /// + /// Gets bounds pointerse for a leaf in the tree. + /// + /// Index of the leaf in the tree. + /// Pointer to the minimum bounds vector in the tree. + /// Pointer to the maximum bounds vector in the tree. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public readonly void GetBoundsPointers(int leafIndex, out Vector3* minPointer, out Vector3* maxPointer) + { + var leaf = Leaves[leafIndex]; + var nodeChild = (&Nodes.Memory[leaf.NodeIndex].A) + leaf.ChildIndex; + minPointer = &nodeChild->Min; + maxPointer = &nodeChild->Max; + } + + /// + /// Applies updated bounds to the given leaf index in the tree, refitting the tree to match. + /// + /// Index of the leaf in the tree to update. + /// New minimum bounds for the leaf. + /// New maximum bounds for the leaf. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public unsafe readonly void UpdateBounds(int leafIndex, Vector3 min, Vector3 max) + { + GetBoundsPointers(leafIndex, out var minPointer, out var maxPointer); + *minPointer = min; + *maxPointer = max; + RefitForNodeBoundsChange(Leaves[leafIndex].NodeIndex); + } /// /// Constructs an empty tree. /// + /// Buffer pool to use to allocate resources in the tree. /// Initial number of leaves to allocate room for. public unsafe Tree(BufferPool pool, int initialLeafCapacity = 4096) : this() { @@ -98,7 +144,6 @@ public Tree(Span data, BufferPool pool) /// /// Gets the number of bytes required to store the tree. /// - /// Tree to measure. /// Number of bytes required to store the tree. public readonly int GetSerializedByteCount() { @@ -108,7 +153,6 @@ public readonly int GetSerializedByteCount() /// /// Writes a tree into a byte buffer. /// - /// Tree to write into the buffer. /// Buffer to hold the tree's data. public readonly void Serialize(Span bytes) { diff --git a/BepuPhysics/Trees/Tree_Refit.cs b/BepuPhysics/Trees/Tree_Refit.cs index 85f13dfde..8072c1941 100644 --- a/BepuPhysics/Trees/Tree_Refit.cs +++ b/BepuPhysics/Trees/Tree_Refit.cs @@ -12,7 +12,7 @@ partial struct Tree /// Refits the bounding box of every parent of the node recursively to the root. /// /// Node to propagate a node change for. - public unsafe void RefitForNodeBoundsChange(int nodeIndex) + public unsafe readonly void RefitForNodeBoundsChange(int nodeIndex) { //Note that no attempt is made to refit the root node. Note that the root node is the only node that can have a number of children less than 2. ref var node = ref Nodes[nodeIndex]; @@ -31,7 +31,7 @@ public unsafe void RefitForNodeBoundsChange(int nodeIndex) //TODO: Recursive approach is a bit silly. Our earlier nonrecursive implementations weren't great, but we could do better. //This is especially true if we end up changing the memory layout. If we go back to a contiguous array per level, refit becomes trivial. //That would only happen if it turns out useful for other parts of the execution, though- optimizing refits at the cost of self-tests would be a terrible idea. - unsafe void Refit(int nodeIndex, out Vector3 min, out Vector3 max) + readonly unsafe void Refit(int nodeIndex, out Vector3 min, out Vector3 max) { Debug.Assert(leafCount >= 2); ref var node = ref Nodes[nodeIndex]; @@ -48,7 +48,10 @@ unsafe void Refit(int nodeIndex, out Vector3 min, out Vector3 max) BoundingBox.CreateMerged(a.Min, a.Max, b.Min, b.Max, out min, out max); } - public unsafe void Refit() + /// + /// Updates the bounding boxes of all internal nodes in the tree. + /// + public unsafe readonly void Refit() { //No point in refitting a tree with no internal nodes! if (leafCount <= 2) From b86a90b016f0c6783d9d79e15eb67093d959e208 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 8 Feb 2022 16:26:24 -0600 Subject: [PATCH 439/947] Media updates. --- .../Media/2.0/BedsheetDemo.cs | 160 ++++++++ .../Media/2.0/ColosseumVideoDemo.cs | 161 ++++++++ .../2.0/NewtDemandingSacrificeVideoDemo.cs | 78 ++++ .../Media/2.0/NewtVideoDemo.cs | 67 ++++ .../Media/2.0/PyramidVideoDemo.cs | 84 ++++ .../Media/2.0/ShrinkwrappedNewtsVideoDemo.cs | 71 ++++ .../Media/2.4/ExcessivePyramidVideoDemo.cs | 85 ++++ .../Media/2.4/TankSwarmDemo.cs | 366 ++++++++++++++++++ 8 files changed, 1072 insertions(+) create mode 100644 Demos/SpecializedTests/Media/2.0/BedsheetDemo.cs create mode 100644 Demos/SpecializedTests/Media/2.0/ColosseumVideoDemo.cs create mode 100644 Demos/SpecializedTests/Media/2.0/NewtDemandingSacrificeVideoDemo.cs create mode 100644 Demos/SpecializedTests/Media/2.0/NewtVideoDemo.cs create mode 100644 Demos/SpecializedTests/Media/2.0/PyramidVideoDemo.cs create mode 100644 Demos/SpecializedTests/Media/2.0/ShrinkwrappedNewtsVideoDemo.cs create mode 100644 Demos/SpecializedTests/Media/2.4/ExcessivePyramidVideoDemo.cs create mode 100644 Demos/SpecializedTests/Media/2.4/TankSwarmDemo.cs diff --git a/Demos/SpecializedTests/Media/2.0/BedsheetDemo.cs b/Demos/SpecializedTests/Media/2.0/BedsheetDemo.cs new file mode 100644 index 000000000..a65fe64a7 --- /dev/null +++ b/Demos/SpecializedTests/Media/2.0/BedsheetDemo.cs @@ -0,0 +1,160 @@ +using BepuPhysics; +using BepuPhysics.Collidables; +using BepuPhysics.Constraints; +using BepuUtilities; +using DemoContentLoader; +using DemoRenderer; +using DemoRenderer.UI; +using DemoUtilities; +using System; +using System.Numerics; + +namespace Demos.Demos.Media +{ + + /// + /// Shows a few different examples of cloth-ish constraint lattices. + /// + public class BedsheetDemo : Demo + { + delegate bool KinematicDecider(int rowIndex, int columnIndex, int width, int height); + + BodyHandle[,] CreateBodyGrid(in Vector3 position, in Quaternion orientation, int width, int height, float spacing, float bodyRadius, float massPerBody, + int instanceId, CollidableProperty filters, KinematicDecider isKinematic) + { + var description = BodyDescription.CreateKinematic(orientation, Simulation.Shapes.Add(new Sphere(bodyRadius)), 0.01f); + var inverseMass = 1f / massPerBody; + BodyHandle[,] handles = new BodyHandle[height, width]; + for (int rowIndex = 0; rowIndex < height; ++rowIndex) + { + for (int columnIndex = 0; columnIndex < width; ++columnIndex) + { + description.LocalInertia.InverseMass = isKinematic(rowIndex, columnIndex, width, height) ? 0 : inverseMass; + var localPosition = new Vector3(columnIndex * spacing, rowIndex * -spacing, 0); + QuaternionEx.TransformWithoutOverlap(localPosition, orientation, out var rotatedPosition); + description.Pose.Position = rotatedPosition + position; + var handle = Simulation.Bodies.Add(description); + handles[rowIndex, columnIndex] = handle; + filters.Allocate(handle) = new ClothCollisionFilter(rowIndex, columnIndex, instanceId); + } + } + return handles; + } + + void CreateAreaConstraints(BodyHandle[,] bodyHandles, SpringSettings springSettings) + { + for (int rowIndex = 0; rowIndex < bodyHandles.GetLength(0) - 1; ++rowIndex) + { + for (int columnIndex = 0; columnIndex < bodyHandles.GetLength(1) - 1; ++columnIndex) + { + var aHandle = bodyHandles[rowIndex, columnIndex]; + var bHandle = bodyHandles[rowIndex + 1, columnIndex]; + var cHandle = bodyHandles[rowIndex, columnIndex + 1]; + var dHandle = bodyHandles[rowIndex + 1, columnIndex + 1]; + var a = new BodyReference(aHandle, Simulation.Bodies); + var b = new BodyReference(bHandle, Simulation.Bodies); + var c = new BodyReference(cHandle, Simulation.Bodies); + var d = new BodyReference(dHandle, Simulation.Bodies); + //Not worried about kinematics here- we create at most one row of kinematics in this demo. These are three body constraints that operate in a local quad, so + //there's no way for them to all be kinematic. + Simulation.Solver.Add(aHandle, bHandle, cHandle, new AreaConstraint(a.Pose.Position, b.Pose.Position, c.Pose.Position, springSettings)); + Simulation.Solver.Add(bHandle, cHandle, dHandle, new AreaConstraint(b.Pose.Position, c.Pose.Position, d.Pose.Position, springSettings)); + } + } + } + void CreateDistanceConstraints(BodyHandle[,] bodyHandles, SpringSettings springSettings) + { + void CreateConstraintBetweenBodies(BodyHandle aHandle, BodyHandle bHandle) + { + var a = new BodyReference(aHandle, Simulation.Bodies); + var b = new BodyReference(bHandle, Simulation.Bodies); + //Don't create constraints between two kinematic bodies. + if (a.LocalInertia.InverseMass > 0 || b.LocalInertia.InverseMass > 0) + { + //Note the use of a limit; the distance is allowed to go smaller. + //This helps stop the cloth from having unnatural rigidity. + var distance = Vector3.Distance(a.Pose.Position, b.Pose.Position); + Simulation.Solver.Add(aHandle, bHandle, new CenterDistanceLimit(distance * 0.15f, distance, springSettings)); + } + } + for (int rowIndex = 0; rowIndex < bodyHandles.GetLength(0); ++rowIndex) + { + for (int columnIndex = 0; columnIndex < bodyHandles.GetLength(1) - 1; ++columnIndex) + { + CreateConstraintBetweenBodies(bodyHandles[rowIndex, columnIndex], bodyHandles[rowIndex, columnIndex + 1]); + } + } + for (int rowIndex = 0; rowIndex < bodyHandles.GetLength(0) - 1; ++rowIndex) + { + for (int columnIndex = 0; columnIndex < bodyHandles.GetLength(1); ++columnIndex) + { + CreateConstraintBetweenBodies(bodyHandles[rowIndex, columnIndex], bodyHandles[rowIndex + 1, columnIndex]); + } + } + for (int rowIndex = 0; rowIndex < bodyHandles.GetLength(0) - 1; ++rowIndex) + { + for (int columnIndex = 0; columnIndex < bodyHandles.GetLength(1) - 1; ++columnIndex) + { + CreateConstraintBetweenBodies(bodyHandles[rowIndex, columnIndex], bodyHandles[rowIndex + 1, columnIndex + 1]); + CreateConstraintBetweenBodies(bodyHandles[rowIndex, columnIndex + 1], bodyHandles[rowIndex + 1, columnIndex]); + } + } + } + + RolloverInfo rolloverInfo; + + public unsafe override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(70, 40, -80); + camera.Yaw = -MathF.PI * 0.8f; + camera.Pitch = MathF.PI * 0.1f; + + var filters = new CollidableProperty(); + Simulation = Simulation.Create(BufferPool, new ClothCallbacks(filters), new DemoPoseIntegratorCallbacks(new Vector3(0, -50, 0)), new SolveDescription(8, 1)); + rolloverInfo = new RolloverInfo(); + + bool FullyDynamic(int rowIndex, int columnIndex, int width, int height) + { + return false; + } + + int clothInstanceId = 0; + var initialRotation = QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), MathF.PI * -0.5f); + + + + + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 10, 0), Simulation.Shapes.Add(new Box(80, 20, 80)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(-20, 22, 30), Simulation.Shapes.Add(new Box(34, 4, 14)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(20, 22, 30), Simulation.Shapes.Add(new Box(34, 4, 14)))); + + + Simulation.Statics.Add(new StaticDescription(new Vector3(65.5f, 8f, 20), Simulation.Shapes.Add(new Cylinder(15, 15)))); + + + { + var position = new Vector3(96 * 1.15f * -0.5f, 30, 86 * 1.15f * -0.5f); + var handles = CreateBodyGrid(position, initialRotation, 96, 86, 1.15f, 1f, 1, clothInstanceId++, filters, FullyDynamic); + CreateDistanceConstraints(handles, new SpringSettings(20, 1)); + CreateAreaConstraints(handles, new SpringSettings(30, 1)); + } + + { + var position = new Vector3(65.5f + 56 * 0.8f * -0.5f, 25, 20 + 56 * 0.8f * -0.5f); + var handles = CreateBodyGrid(position, initialRotation, 56, 56, 0.8f, 0.65f, 1, clothInstanceId++, filters, FullyDynamic); + CreateDistanceConstraints(handles, new SpringSettings(20, 1)); + CreateAreaConstraints(handles, new SpringSettings(30, 1)); + } + + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), Simulation.Shapes.Add(new Box(400, 1, 400)))); + + } + + public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + { + rolloverInfo.Render(renderer, camera, input, text, font); + base.Render(renderer, camera, input, text, font); + } + + } +} diff --git a/Demos/SpecializedTests/Media/2.0/ColosseumVideoDemo.cs b/Demos/SpecializedTests/Media/2.0/ColosseumVideoDemo.cs new file mode 100644 index 000000000..bab1a80b6 --- /dev/null +++ b/Demos/SpecializedTests/Media/2.0/ColosseumVideoDemo.cs @@ -0,0 +1,161 @@ +using BepuPhysics; +using BepuPhysics.Collidables; +using BepuUtilities; +using DemoContentLoader; +using DemoRenderer; +using DemoRenderer.UI; +using Demos.Demos.Characters; +using DemoUtilities; +using OpenTK.Input; +using System; +using System.Numerics; + +namespace Demos.SpecializedTests.Media +{ + /// + /// Version of the colosseum demo for video purposes. + /// + public class ColosseumVideoDemo : Demo + { + void CreateRingWall(Vector3 position, Box ringBoxShape, BodyDescription bodyDescription, int height, float radius) + { + var circumference = MathF.PI * 2 * radius; + var boxCountPerRing = (int)(0.9f * circumference / ringBoxShape.Length); + float increment = MathHelper.TwoPi / boxCountPerRing; + for (int ringIndex = 0; ringIndex < height; ringIndex++) + { + for (int i = 0; i < boxCountPerRing; i++) + { + var angle = ((ringIndex & 1) == 0 ? i + 0.5f : i) * increment; + bodyDescription.Pose = (position + new Vector3(-MathF.Cos(angle) * radius, (ringIndex + 0.5f) * ringBoxShape.Height, MathF.Sin(angle) * radius), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, angle)); + Simulation.Bodies.Add(bodyDescription); + } + } + } + + void CreateRingPlatform(Vector3 position, Box ringBoxShape, BodyDescription bodyDescription, float radius) + { + var innerCircumference = MathF.PI * 2 * (radius - ringBoxShape.HalfLength); + var boxCount = (int)(0.95f * innerCircumference / ringBoxShape.Height); + float increment = MathHelper.TwoPi / boxCount; + for (int i = 0; i < boxCount; i++) + { + var angle = i * increment; + bodyDescription.Pose = (position + new Vector3(-MathF.Cos(angle) * radius, ringBoxShape.HalfWidth, MathF.Sin(angle) * radius), + QuaternionEx.Concatenate(QuaternionEx.CreateFromAxisAngle(Vector3.UnitZ, MathF.PI * 0.5f), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, angle + MathF.PI * 0.5f))); + Simulation.Bodies.Add(bodyDescription); + } + } + + Vector3 CreateRing(Vector3 position, Box ringBoxShape, BodyDescription bodyDescription, float radius, int heightPerPlatformLevel, int platformLevels) + { + for (int platformIndex = 0; platformIndex < platformLevels; ++platformIndex) + { + var wallOffset = ringBoxShape.HalfLength - ringBoxShape.HalfWidth; + CreateRingWall(position, ringBoxShape, bodyDescription, heightPerPlatformLevel, radius + wallOffset); + CreateRingWall(position, ringBoxShape, bodyDescription, heightPerPlatformLevel, radius - wallOffset); + CreateRingPlatform(position + new Vector3(0, heightPerPlatformLevel * ringBoxShape.Height, 0), ringBoxShape, bodyDescription, radius); + position.Y += heightPerPlatformLevel * ringBoxShape.Height + ringBoxShape.Width; + } + return position; + } + + CharacterControllers characters; + public unsafe override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(-30, 40, -30); + camera.Yaw = MathHelper.Pi * 3f / 4; + camera.Pitch = MathHelper.Pi * 0.2f; + + characters = new CharacterControllers(BufferPool); + Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); + + var ringBoxShape = new Box(0.5f, 1.5f, 3); + var boxDescription = BodyDescription.CreateDynamic(new Vector3(), ringBoxShape.ComputeInertia(1), Simulation.Shapes.Add(ringBoxShape), 0.01f); + + var layerPosition = new Vector3(); + const int layerCount = 10; + var innerRadius = 5f; + var heightPerPlatform = 2; + var platformsPerLayer = 1; + var ringSpacing = 0.5f; + for (int layerIndex = 0; layerIndex < layerCount; ++layerIndex) + { + var ringCount = layerCount - layerIndex; + for (int ringIndex = 0; ringIndex < ringCount; ++ringIndex) + { + CreateRing(layerPosition, ringBoxShape, boxDescription, innerRadius + ringIndex * (ringBoxShape.Length + ringSpacing) + layerIndex * (ringBoxShape.Length - ringBoxShape.Width), heightPerPlatform, platformsPerLayer); + } + layerPosition.Y += platformsPerLayer * (ringBoxShape.Height * heightPerPlatform + ringBoxShape.Width); + } + + Console.WriteLine($"box count: {Simulation.Bodies.ActiveSet.Count}"); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), Simulation.Shapes.Add(new Box(500, 1, 500)))); + + var bulletShape = new Sphere(0.5f); + bulletDescription = BodyDescription.CreateDynamic(new Vector3(), bulletShape.ComputeInertia(.1f), Simulation.Shapes.Add(bulletShape), 0.01f); + + var shootiePatootieShape = new Sphere(3f); + shootiePatootieDescription = BodyDescription.CreateDynamic(new Vector3(), shootiePatootieShape.ComputeInertia(1000), Simulation.Shapes.Add(shootiePatootieShape), 0.01f); + } + + bool characterActive; + CharacterInput character; + void CreateCharacter(Vector3 position) + { + characterActive = true; + character = new CharacterInput(characters, position, new Capsule(0.5f, 1), 0.1f, 1, 20, 100, 6, 4, MathF.PI * 0.4f); + } + + + BodyDescription bulletDescription; + BodyDescription shootiePatootieDescription; + public override void Update(Window window, Camera camera, Input input, float dt) + { + if (input != null) + { + if (input.WasPushed(Key.C)) + { + if (characterActive) + { + character.Dispose(); + characterActive = false; + } + else + { + CreateCharacter(camera.Position); + } + } + if (characterActive) + { + character.UpdateCharacterGoals(input, camera, Demo.TimestepDuration); + } + + if (input.WasPushed(Key.Z)) + { + bulletDescription.Pose.Position = camera.Position; + bulletDescription.Velocity.Linear = camera.GetRayDirection(input.MouseLocked, window.GetNormalizedMousePosition(input.MousePosition)) * 400; + Simulation.Bodies.Add(bulletDescription); + } + else if (input.WasPushed(Key.X)) + { + shootiePatootieDescription.Pose.Position = camera.Position; + shootiePatootieDescription.Velocity.Linear = camera.GetRayDirection(input.MouseLocked, window.GetNormalizedMousePosition(input.MousePosition)) * 100; + Simulation.Bodies.Add(shootiePatootieDescription); + } + } + base.Update(window, camera, input, dt); + } + + public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + { + if (characterActive) + { + character.UpdateCameraPosition(camera, 0); + } + //text.Clear().Append("Press Z to shoot a bullet, press X to super shootie patootie!"); + //renderer.TextBatcher.Write(text, new Vector2(20, renderer.Surface.Resolution.Y - 20), 16, new Vector3(1, 1, 1), font); + base.Render(renderer, camera, input, text, font); + } + } +} diff --git a/Demos/SpecializedTests/Media/2.0/NewtDemandingSacrificeVideoDemo.cs b/Demos/SpecializedTests/Media/2.0/NewtDemandingSacrificeVideoDemo.cs new file mode 100644 index 000000000..5e16bd365 --- /dev/null +++ b/Demos/SpecializedTests/Media/2.0/NewtDemandingSacrificeVideoDemo.cs @@ -0,0 +1,78 @@ +using BepuPhysics; +using BepuPhysics.Collidables; +using BepuUtilities; +using DemoContentLoader; +using DemoRenderer; +using Demos.Demos; +using DemoUtilities; +using System; +using System.Numerics; + +namespace Demos.SpecializedTests.Media +{ + public class NewtDemandingSacrificeVideoDemo : Demo + { + CollidableProperty filters; + public unsafe override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(-32f, 20.5f, 61f); + camera.Yaw = MathHelper.Pi * 0.3f; + camera.Pitch = MathHelper.Pi * -0.05f; + + filters = new CollidableProperty(BufferPool); + Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks(filters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(4, 1)); + + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), Simulation.Shapes.Add(new Box(1500, 1, 1500)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 10, 0), Simulation.Shapes.Add(new Box(70, 20, 80)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 7.5f, 0), Simulation.Shapes.Add(new Box(80, 15, 90)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 5, 0), Simulation.Shapes.Add(new Box(90, 10, 100)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 2.5f, 0), Simulation.Shapes.Add(new Box(100, 5, 110)))); + + //High fidelity simulation isn't super important on this one. + Simulation.Solver.VelocityIterationCount = 2; + + DemoMeshHelper.LoadModel(content, BufferPool, "Content\\newt.obj", new Vector3(30), out var mesh); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 20, 0), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, 0), Simulation.Shapes.Add(mesh))); + } + + Random random = new Random(5); + int ragdollIndex = 0; + + BodyVelocity GetRandomizedVelocity(in Vector3 linearVelocity) + { + return new BodyVelocity { Linear = linearVelocity, Angular = new Vector3(-20) + 40 * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) }; + } + + public override void Update(Window window, Camera camera, Input input, float dt) + { + var pose = TestHelpers.CreateRandomPose(random, new BoundingBox + { + Min = new Vector3(-10, 5, 70), + Max = new Vector3(10, 15, 70) + }); + var linearVelocity = Vector3.Normalize(new Vector3(-2 + 4 * random.NextSingle(), 31 + 4 * random.NextSingle(), 50) - pose.Position) * 40; + var handles = RagdollDemo.AddRagdoll(pose.Position, pose.Orientation, ragdollIndex++, filters, Simulation); + var bodies = Simulation.Bodies; + //This could be done better, but... ... .... .......... + bodies[handles.Hips].Velocity = GetRandomizedVelocity(linearVelocity); + bodies[handles.Abdomen].Velocity = GetRandomizedVelocity(linearVelocity); + bodies[handles.Chest].Velocity = GetRandomizedVelocity(linearVelocity); + bodies[handles.Head].Velocity = GetRandomizedVelocity(linearVelocity); + bodies[handles.LeftArm.UpperArm].Velocity = GetRandomizedVelocity(linearVelocity); + bodies[handles.LeftArm.LowerArm].Velocity = GetRandomizedVelocity(linearVelocity); + bodies[handles.LeftArm.Hand].Velocity = GetRandomizedVelocity(linearVelocity); + bodies[handles.RightArm.UpperArm].Velocity = GetRandomizedVelocity(linearVelocity); + bodies[handles.RightArm.LowerArm].Velocity = GetRandomizedVelocity(linearVelocity); + bodies[handles.RightArm.Hand].Velocity = GetRandomizedVelocity(linearVelocity); + bodies[handles.LeftLeg.UpperLeg].Velocity = GetRandomizedVelocity(linearVelocity); + bodies[handles.LeftLeg.LowerLeg].Velocity = GetRandomizedVelocity(linearVelocity); + bodies[handles.LeftLeg.Foot].Velocity = GetRandomizedVelocity(linearVelocity); + bodies[handles.RightLeg.UpperLeg].Velocity = GetRandomizedVelocity(linearVelocity); + bodies[handles.RightLeg.LowerLeg].Velocity = GetRandomizedVelocity(linearVelocity); + bodies[handles.RightLeg.Foot].Velocity = GetRandomizedVelocity(linearVelocity); + + base.Update(window, camera, input, dt); + } + + } +} diff --git a/Demos/SpecializedTests/Media/2.0/NewtVideoDemo.cs b/Demos/SpecializedTests/Media/2.0/NewtVideoDemo.cs new file mode 100644 index 000000000..242aee926 --- /dev/null +++ b/Demos/SpecializedTests/Media/2.0/NewtVideoDemo.cs @@ -0,0 +1,67 @@ +using BepuPhysics; +using BepuPhysics.Collidables; +using BepuPhysics.Constraints; +using BepuUtilities; +using BepuUtilities.Memory; +using DemoContentLoader; +using DemoRenderer; +using Demos.Demos; +using DemoUtilities; +using System; +using System.Numerics; + +namespace Demos.SpecializedTests.Media +{ + public class NewtVideoDemo : Demo + { + public unsafe override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(-5f, 5.5f, 5f); + camera.Yaw = MathHelper.Pi / 4; + camera.Pitch = MathHelper.Pi * 0.15f; + + var filters = new CollidableProperty(); + Simulation = Simulation.Create(BufferPool, new DeformableCallbacks(filters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); + + var meshContent = content.Load("Content\\newt.obj"); + float cellSize = 0.1f; + DumbTetrahedralizer.Tetrahedralize(meshContent.Triangles, cellSize, BufferPool, + out var vertices, out var vertexSpatialIndices, out var cellVertexIndices, out var tetrahedraVertexIndices); + var weldSpringiness = new SpringSettings(30f, 0); + var volumeSpringiness = new SpringSettings(30f, 1); + for (int i = 0; i < 5; ++i) + { + NewtDemo.CreateDeformable(Simulation, new Vector3(i * 3, 5 + i * 1.5f, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), MathF.PI * (i * 0.55f)), 1f, cellSize, weldSpringiness, volumeSpringiness, i, filters, ref vertices, ref vertexSpatialIndices, ref cellVertexIndices, ref tetrahedraVertexIndices); + } + + BufferPool.Return(ref vertices); + vertexSpatialIndices.Dispose(BufferPool); + BufferPool.Return(ref cellVertexIndices); + BufferPool.Return(ref tetrahedraVertexIndices); + + Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 100, -.5f), 10, Simulation.Shapes, new Sphere(5))); + + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), Simulation.Shapes.Add(new Box(1500, 1, 1500)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -1.5f, 0), Simulation.Shapes.Add(new Sphere(3)))); + + var bulletShape = new Sphere(0.5f); + bulletDescription = BodyDescription.CreateDynamic(RigidPose.Identity, bulletShape.ComputeInertia(.25f), Simulation.Shapes.Add(bulletShape), 0.01f); + + DemoMeshHelper.LoadModel(content, BufferPool, "Content\\newt.obj", new Vector3(20), out var mesh); + Simulation.Statics.Add(new StaticDescription(new Vector3(200, 0.5f, 120), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, -3 * MathHelper.PiOver4), Simulation.Shapes.Add(mesh))); + } + BodyDescription bulletDescription; + public override void Update(Window window, Camera camera, Input input, float dt) + { + if (input.WasPushed(OpenTK.Input.Key.Z)) + { + bulletDescription.Pose.Position = camera.Position; + bulletDescription.Velocity.Linear = camera.Forward * 40; + Simulation.Bodies.Add(bulletDescription); + } + base.Update(window, camera, input, dt); + } + + + } +} diff --git a/Demos/SpecializedTests/Media/2.0/PyramidVideoDemo.cs b/Demos/SpecializedTests/Media/2.0/PyramidVideoDemo.cs new file mode 100644 index 000000000..f6231d30a --- /dev/null +++ b/Demos/SpecializedTests/Media/2.0/PyramidVideoDemo.cs @@ -0,0 +1,84 @@ +using BepuPhysics; +using BepuPhysics.Collidables; +using BepuPhysics.Constraints; +using BepuUtilities; +using DemoContentLoader; +using DemoRenderer; +using DemoRenderer.UI; +using DemoUtilities; +using System; +using System.Collections.Generic; +using System.Numerics; +using System.Text; + +namespace Demos.Demos.Media +{ + /// + /// A pyramid of boxes, because you can't have a physics engine without pyramids of boxes. + /// + public class PyramidVideoDemo : Demo + { + public unsafe override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(-70, 8, 318); + camera.Yaw = MathHelper.Pi * 1f / 4; + camera.Pitch = 0; + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); + + var boxShape = new Box(1, 1, 1); + var boxInertia = boxShape.ComputeInertia(1); + var boxIndex = Simulation.Shapes.Add(boxShape); + const int pyramidCount = 120; + for (int pyramidIndex = 0; pyramidIndex < pyramidCount; ++pyramidIndex) + { + const int rowCount = 20; + for (int rowIndex = 0; rowIndex < rowCount; ++rowIndex) + { + int columnCount = rowCount - rowIndex; + for (int columnIndex = 0; columnIndex < columnCount; ++columnIndex) + { + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3( + (-columnCount * 0.5f + columnIndex) * boxShape.Width, + (rowIndex + 0.5f) * boxShape.Height, + (pyramidIndex - pyramidCount * 0.5f) * (boxShape.Length + 4)), + boxInertia, boxIndex, 0.01f)); + } + } + } + + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), Simulation.Shapes.Add(new Box(2500, 1, 2500)))); + } + + //We'll randomize the size of bullets. + Random random = new Random(5); + public override void Update(Window window, Camera camera, Input input, float dt) + { + if (input != null && input.WasPushed(OpenTK.Input.Key.Z)) + { + //Create the shape that we'll launch at the pyramids when the user presses a button. + var bulletShape = new Sphere(6); + //Note that the use of radius^3 for mass can produce some pretty serious mass ratios. + //Observe what happens when a large ball sits on top of a few boxes with a fraction of the mass- + //the collision appears much squishier and less stable. For most games, if you want to maintain rigidity, you'll want to use some combination of: + //1) Limit the ratio of heavy object masses to light object masses when those heavy objects depend on the light objects. + //2) Use a shorter timestep duration and update more frequently. + //3) Use a greater number of solver iterations. + //#2 and #3 can become very expensive. In pathological cases, it can end up slower than using a quality-focused solver for the same simulation. + //Unfortunately, at the moment, bepuphysics v2 does not contain any alternative solvers, so if you can't afford to brute force the the problem away, + //the best solution is to cheat as much as possible to avoid the corner cases. + var bodyDescription = BodyDescription.CreateConvexDynamic( + new Vector3(0, 8, -500), new Vector3(0, 0, 110), 50000, Simulation.Shapes, bulletShape); + Simulation.Bodies.Add(bodyDescription); + } + base.Update(window, camera, input, dt); + } + + public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + { + text.Clear().Append("Press Z to launch a ball!"); + renderer.TextBatcher.Write(text, new Vector2(20, renderer.Surface.Resolution.Y - 20), 16, new Vector3(1, 1, 1), font); + base.Render(renderer, camera, input, text, font); + } + + } +} diff --git a/Demos/SpecializedTests/Media/2.0/ShrinkwrappedNewtsVideoDemo.cs b/Demos/SpecializedTests/Media/2.0/ShrinkwrappedNewtsVideoDemo.cs new file mode 100644 index 000000000..ccd44adaf --- /dev/null +++ b/Demos/SpecializedTests/Media/2.0/ShrinkwrappedNewtsVideoDemo.cs @@ -0,0 +1,71 @@ +using BepuPhysics; +using BepuPhysics.Collidables; +using BepuPhysics.Constraints; +using BepuUtilities; +using BepuUtilities.Collections; +using DemoContentLoader; +using DemoRenderer; +using DemoUtilities; +using System; +using System.Numerics; + +namespace Demos.SpecializedTests.Media +{ + public class ShrinkwrappedNewtsVideoDemo : Demo + { + public unsafe override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(25f, 1.5f, 15f); + camera.Yaw = 3 * MathHelper.Pi / 4; + camera.Pitch = 0;// MathHelper.Pi * 0.15f; + + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); + + var meshContent = content.Load("Content\\newt.obj"); + + //This is actually a pretty good example of how *not* to make a convex hull shape. + //Generating it directly from a graphical data source tends to have way more surface complexity than needed, + //and it tends to have a lot of near-but-not-quite-coplanar surfaces which can make the contact manifold less stable. + //Prefer a simpler source with more distinct features, possibly created with an automated content-time tool. + var points = new QuickList(meshContent.Triangles.Length * 3, BufferPool); + for (int i = 0; i < meshContent.Triangles.Length; ++i) + { + ref var triangle = ref meshContent.Triangles[i]; + //resisting the urge to just reinterpret the memory + points.AllocateUnsafely() = triangle.A * new Vector3(1, 1.5f, 1); + points.AllocateUnsafely() = triangle.B * new Vector3(1, 1.5f, 1); + points.AllocateUnsafely() = triangle.C * new Vector3(1, 1.5f, 1); + } + + var newtHull = new ConvexHull(points.Span.Slice(points.Count), BufferPool, out _); + var bodyDescription = BodyDescription.CreateConvexDynamic(RigidPose.Identity, 1, Simulation.Shapes, newtHull); + Random random = new Random(5); + var poseBounds = new BoundingBox { Min = new Vector3(-20, 1, 5), Max = new Vector3(20, 10, 50) }; + for (int i = 0; i < 512; ++i) + { + bodyDescription.Pose = TestHelpers.CreateRandomPose(random, poseBounds); + Simulation.Bodies.Add(bodyDescription); + } + + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -30, 250), Simulation.Shapes.Add(new Box(1000, 60, 500)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -60, 0), Simulation.Shapes.Add(new Box(1000, 1, 1000)))); + + + + DemoMeshHelper.LoadModel(content, BufferPool, "Content\\newt.obj", new Vector3(1, 1.5f, 1), out mesh); + Simulation.Statics.Add(new StaticDescription(new Vector3(30, 0, 20), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, -3 * MathHelper.PiOver4), Simulation.Shapes.Add(mesh))); + } + + Mesh mesh; + + public override void Update(Window window, Camera camera, Input input, float dt) + { + if(input.WasPushed(OpenTK.Input.Key.Z)) + { + mesh.Scale = new Vector3(30); + Simulation.Statics.Add(new StaticDescription(new Vector3(70, 0, 50), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, -3.1f * MathHelper.PiOver4), Simulation.Shapes.Add(mesh))); + } + base.Update(window, camera, input, dt); + } + } +} diff --git a/Demos/SpecializedTests/Media/2.4/ExcessivePyramidVideoDemo.cs b/Demos/SpecializedTests/Media/2.4/ExcessivePyramidVideoDemo.cs new file mode 100644 index 000000000..b1ba70499 --- /dev/null +++ b/Demos/SpecializedTests/Media/2.4/ExcessivePyramidVideoDemo.cs @@ -0,0 +1,85 @@ +using BepuPhysics; +using BepuPhysics.Collidables; +using BepuPhysics.Constraints; +using BepuUtilities; +using DemoContentLoader; +using DemoRenderer; +using DemoRenderer.UI; +using DemoUtilities; +using System; +using System.Collections.Generic; +using System.Numerics; +using System.Text; + +namespace Demos.SpecializedTests.Media +{ + /// + /// A pyramid of boxes, because you can't have a physics engine without pyramids of boxes. + /// + public class ExcessivePyramidVideoDemo : Demo + { + public unsafe override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(-120, 16, 1045); + camera.Yaw = MathHelper.Pi * 1f / 4; + camera.Pitch = 0; + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1), frictionCoefficient: 2), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); + + var boxShape = new Box(1, 1, 1); + var boxInertia = boxShape.ComputeInertia(1); + var boxIndex = Simulation.Shapes.Add(boxShape); + const int pyramidCount = 420; + for (int pyramidIndex = 0; pyramidIndex < pyramidCount; ++pyramidIndex) + { + const int rowCount = 20; + for (int rowIndex = 0; rowIndex < rowCount; ++rowIndex) + { + int columnCount = rowCount - rowIndex; + for (int columnIndex = 0; columnIndex < columnCount; ++columnIndex) + { + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3( + (-columnCount * 0.5f + columnIndex) * boxShape.Width, + (rowIndex + 0.5f) * boxShape.Height, + (pyramidIndex - pyramidCount * 0.5f) * (boxShape.Length + 4)), + boxInertia, new CollidableDescription(boxIndex, 0.1f), 0.01f)); + } + } + } + Console.WriteLine($"bodies count: {Simulation.Bodies.ActiveSet.Count}"); + + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), Simulation.Shapes.Add(new Box(2500, 1, 2500)))); + } + + int frameCount; + public override void Update(Window window, Camera camera, Input input, float dt) + { + ++frameCount; + if (frameCount == 128 || (input != null && input.WasPushed(OpenTK.Input.Key.Z))) + { + //Create the shape that we'll launch at the pyramids when the user presses a button. + var bulletShape = new Sphere(6); + //Note that the use of radius^3 for mass can produce some pretty serious mass ratios. + //Observe what happens when a large ball sits on top of a few boxes with a fraction of the mass- + //the collision appears much squishier and less stable. For most games, if you want to maintain rigidity, you'll want to use some combination of: + //1) Limit the ratio of heavy object masses to light object masses when those heavy objects depend on the light objects. + //2) Use a shorter timestep duration and update more frequently. + //3) Use a greater number of solver iterations. + //#2 and #3 can become very expensive. In pathological cases, it can end up slower than using a quality-focused solver for the same simulation. + //Unfortunately, at the moment, bepuphysics v2 does not contain any alternative solvers, so if you can't afford to brute force the the problem away, + //the best solution is to cheat as much as possible to avoid the corner cases. + var bodyDescription = BodyDescription.CreateConvexDynamic( + new Vector3(0, 8, -1200), new Vector3(0, 0, 230), 5000000, Simulation.Shapes, bulletShape); + Simulation.Bodies.Add(bodyDescription); + } + base.Update(window, camera, input, dt); + } + + public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + { + text.Clear().Append("Press Z to launch a ball!"); + renderer.TextBatcher.Write(text, new Vector2(20, renderer.Surface.Resolution.Y - 20), 16, new Vector3(1, 1, 1), font); + base.Render(renderer, camera, input, text, font); + } + + } +} diff --git a/Demos/SpecializedTests/Media/2.4/TankSwarmDemo.cs b/Demos/SpecializedTests/Media/2.4/TankSwarmDemo.cs new file mode 100644 index 000000000..a34460b0c --- /dev/null +++ b/Demos/SpecializedTests/Media/2.4/TankSwarmDemo.cs @@ -0,0 +1,366 @@ +using System; +using System.Numerics; +using BepuPhysics; +using BepuPhysics.Collidables; +using BepuPhysics.CollisionDetection; +using BepuPhysics.Constraints; +using BepuUtilities; +using BepuUtilities.Collections; +using DemoContentLoader; +using DemoRenderer; +using DemoRenderer.UI; +using Demos.Demos.Tanks; +using DemoUtilities; +using OpenTK.Input; + +namespace Demos.SpecializedTests.Media +{ + public class TankSwarmDemo : Demo + { + CollidableProperty bodyProperties; + TankController playerController; + + QuickList aiTanks; + Random random; + Vector2 playAreaMin, playAreaMax; + + //We want to create a little graphical explosion at projectile impact points. Since it's not an instant thing, we'll have to track it over a period of time. + struct Explosion + { + public Vector3 Position; + public float Scale; + public Vector3 Color; + public int Age; + } + QuickList explosions; + + static MouseButton Fire = MouseButton.Left; + static Key Forward = Key.W; + static Key Backward = Key.S; + static Key Right = Key.D; + static Key Left = Key.A; + static Key Zoom = Key.LShift; + static Key Brake = Key.Space; + static Key BrakeAlternate = Key.BackSpace; //I have a weird keyboard. + static Key ToggleTank = Key.C; + public override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(0, 5, 10); + camera.Yaw = 0; + camera.Pitch = 0; + + bodyProperties = new CollidableProperty(); + //Note that this demo uses only 1 substep and 6 velocity iterations. + //That's partly to show that you can do such a thing, and partly because of (as of 2.4's initial release), there are situations where + //contact data can become a little out of date during substepping, since the contact data is only updated once per frame rather than substep (apart from the depths, which are incrementally updated every substep). + //In this demo, when using substepping, a wheel resting on another wheel from a destroyed tank can keep rocking back and forth for a long time as the error in contact offsets over substeps can introduce energy. + //(I'd like to address this issue more directly to make substepping an unconditional win.) + Simulation = Simulation.Create(BufferPool, new TankCallbacks() { Properties = bodyProperties }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(6, 1)); + + var builder = new CompoundBuilder(BufferPool, Simulation.Shapes, 2); + builder.Add(new Box(1.85f, 0.7f, 4.73f), RigidPose.Identity, 10); + builder.Add(new Box(1.85f, 0.6f, 2.5f), new Vector3(0, 0.65f, -0.35f), 0.5f); + builder.BuildDynamicCompound(out var children, out var bodyInertia, out _); + builder.Dispose(); + var bodyShape = new Compound(children); + var bodyShapeIndex = Simulation.Shapes.Add(bodyShape); + var wheelShape = new Cylinder(0.4f, .18f); + var wheelInertia = wheelShape.ComputeInertia(0.25f); + var wheelShapeIndex = Simulation.Shapes.Add(wheelShape); + + var projectileShape = new Sphere(0.1f); + var projectileInertia = projectileShape.ComputeInertia(0.2f); + var tankDescription = new TankDescription + { + Body = TankPartDescription.Create(10, new Box(4f, 1, 5), RigidPose.Identity, 0.5f, Simulation.Shapes), + Turret = TankPartDescription.Create(1, new Box(1.5f, 0.7f, 2f), new Vector3(0, 0.85f, 0.4f), 0.5f, Simulation.Shapes), + Barrel = TankPartDescription.Create(0.5f, new Box(0.2f, 0.2f, 3f), new Vector3(0, 0.85f, 0.4f - 1f - 1.5f), 0.5f, Simulation.Shapes), + TurretAnchor = new Vector3(0f, 0.5f, 0.4f), + BarrelAnchor = new Vector3(0, 0.5f + 0.35f, 0.4f - 1f), + TurretBasis = Quaternion.Identity, + TurretServo = new ServoSettings(1f, 0f, 40f), + TurretSpring = new SpringSettings(10f, 1f), + BarrelServo = new ServoSettings(1f, 0f, 40f), + BarrelSpring = new SpringSettings(10f, 1f), + + ProjectileShape = Simulation.Shapes.Add(projectileShape), + ProjectileSpeed = 100f, + BarrelLocalProjectileSpawn = new Vector3(0, 0, -1.5f), + ProjectileInertia = projectileInertia, + + LeftTreadOffset = new Vector3(-1.9f, 0f, 0), + RightTreadOffset = new Vector3(1.9f, 0f, 0), + SuspensionLength = 1f, + SuspensionSettings = new SpringSettings(2.5f, 1.5f), + WheelShape = wheelShapeIndex, + WheelInertia = wheelInertia, + WheelFriction = 2f, + TreadSpacing = 1f, + WheelCountPerTread = 5, + WheelOrientation = QuaternionEx.CreateFromAxisAngle(Vector3.UnitZ, MathF.PI * -0.5f), + }; + + playerController = new TankController(Tank.Create(Simulation, bodyProperties, BufferPool, (new Vector3(0, 10, 0), Quaternion.Identity), tankDescription), 20, 5, 2, 1, 3.5f); + + + const int planeWidth = 257; + const float terrainScale = 3; + const float inverseTerrainScale = 1f / terrainScale; + var terrainPosition = new Vector2(1 - planeWidth, 1 - planeWidth) * terrainScale * 0.5f; + random = new Random(5); + + //Add some building-ish landmarks. + var landmarkMin = new Vector3(planeWidth * terrainScale * -0.45f, 0, planeWidth * terrainScale * -0.45f); + var landmarkMax = new Vector3(planeWidth * terrainScale * 0.45f, 0, planeWidth * terrainScale * 0.45f); + var landmarkSpan = landmarkMax - landmarkMin; + for (int j = 0; j < 25; ++j) + { + var buildingShape = new Box(10 + random.NextSingle() * 10, 20 + random.NextSingle() * 20, 10 + random.NextSingle() * 10); + var position = landmarkMin + landmarkSpan * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); + Simulation.Statics.Add(new StaticDescription( + new Vector3(0, buildingShape.HalfHeight - 4f + GetHeightForPosition(position.X, position.Z, planeWidth, inverseTerrainScale, terrainPosition), 0) + position, + QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, random.NextSingle() * MathF.PI), + Simulation.Shapes.Add(buildingShape))); + } + + DemoMeshHelper.CreateDeformedPlane(planeWidth, planeWidth, + (int vX, int vY) => + { + var position2D = new Vector2(vX, vY) * terrainScale + terrainPosition; + return new Vector3(position2D.X, GetHeightForPosition(position2D.X, position2D.Y, planeWidth, inverseTerrainScale, terrainPosition), position2D.Y); + }, new Vector3(1, 1, 1), BufferPool, out var planeMesh); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), Simulation.Shapes.Add(planeMesh))); + + explosions = new QuickList(32, BufferPool); + + //Create the AI tanks. + const int aiTankCount = 3072; + aiTanks = new QuickList(aiTankCount, BufferPool); + playAreaMin = new Vector2(landmarkMin.X, landmarkMin.Z); + playAreaMax = new Vector2(landmarkMax.X, landmarkMax.Z); + var playAreaSpan = playAreaMax - playAreaMin; + for (int i = 0; i < aiTankCount; ++i) + { + var horizontalPosition = playAreaMin + new Vector2(random.NextSingle(), random.NextSingle()) * playAreaSpan; + aiTanks.AllocateUnsafely() = new AITank + { + Controller = new TankController( + Tank.Create(Simulation, bodyProperties, BufferPool, + (new Vector3(horizontalPosition.X, 10, horizontalPosition.Y), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), random.NextSingle() * 0.1f)), + tankDescription), 20, 5, 2, 1, 3.5f), + HitPoints = 5 + }; + } + Console.WriteLine($"body count: {Simulation.Bodies.ActiveSet.Count}"); + } + + float GetHeightForPosition(float x, float y, int planeWidth, float inverseTerrainScale, in Vector2 terrainPosition) + { + var normalizedX = (x - terrainPosition.X) * inverseTerrainScale; + var normalizedY = (y - terrainPosition.Y) * inverseTerrainScale; + var octave0 = (MathF.Sin((normalizedX + 5f) * 0.05f) + MathF.Sin((normalizedY + 11) * 0.05f)) * 3.8f; + var octave1 = (MathF.Sin((normalizedX + 17) * 0.15f) + MathF.Sin((normalizedY + 47) * 0.15f)) * 1.5f; + var octave2 = (MathF.Sin((normalizedX + 37) * 0.35f) + MathF.Sin((normalizedY + 93) * 0.35f)) * 0.5f; + var octave3 = (MathF.Sin((normalizedX + 53) * 0.65f) + MathF.Sin((normalizedY + 131) * 0.65f)) * 0.3f; + var octave4 = (MathF.Sin((normalizedX + 67) * 1.50f) + MathF.Sin((normalizedY + 13) * 1.5f)) * 0.1525f; + var distanceToEdge = planeWidth / 2 - Math.Max(Math.Abs(normalizedX - planeWidth / 2), Math.Abs(normalizedY - planeWidth / 2)); + //Flatten an area in the middle. + var offsetX = planeWidth * 0.5f - normalizedX; + var offsetY = planeWidth * 0.5f - normalizedY; + var distanceToCenterSquared = offsetX * offsetX + offsetY * offsetY; + const float centerCircleSize = 30f; + const float fadeoutBoundary = 50f; + var outsideWeight = MathF.Min(1f, MathF.Max(0, distanceToCenterSquared - centerCircleSize * centerCircleSize) / (fadeoutBoundary * fadeoutBoundary - centerCircleSize * centerCircleSize)); + var edgeRamp = 25f / (5 * distanceToEdge + 1); + return outsideWeight * (octave0 + octave1 + octave2 + octave3 + octave4 + edgeRamp); + } + + bool playerControlActive = true; + long frameIndex; + long lastPlayerShotFrameIndex; + int projectileCount; + public override void Update(Window window, Camera camera, Input input, float dt) + { + if (input.WasPushed(ToggleTank)) + playerControlActive = !playerControlActive; + if (playerControlActive) + { + float leftTargetSpeedFraction = 0; + float rightTargetSpeedFraction = 0; + var left = input.IsDown(Left); + var right = input.IsDown(Right); + var forward = input.IsDown(Forward); + var backward = input.IsDown(Backward); + if (forward) + { + if ((left && right) || (!left && !right)) + { + leftTargetSpeedFraction = 1f; + rightTargetSpeedFraction = 1f; + } + //Note turns require a bit of help from the opposing track to overcome friction. + else if (left) + { + leftTargetSpeedFraction = 0.5f; + rightTargetSpeedFraction = 1f; + } + else if (right) + { + leftTargetSpeedFraction = 1f; + rightTargetSpeedFraction = 0.5f; + } + } + else if (backward) + { + if ((left && right) || (!left && !right)) + { + leftTargetSpeedFraction = -1f; + rightTargetSpeedFraction = -1f; + } + else if (left) + { + leftTargetSpeedFraction = -0.5f; + rightTargetSpeedFraction = -1f; + } + else if (right) + { + leftTargetSpeedFraction = -1f; + rightTargetSpeedFraction = -0.5f; + } + } + else + { + //Not trying to move. Turn? + if (left && !right) + { + leftTargetSpeedFraction = -1f; + rightTargetSpeedFraction = 1f; + } + else if (right && !left) + { + leftTargetSpeedFraction = 1f; + rightTargetSpeedFraction = -1f; + } + } + + var zoom = input.IsDown(Zoom); + var brake = input.IsDown(Brake) || input.IsDown(BrakeAlternate); + playerController.UpdateMovementAndAim(Simulation, leftTargetSpeedFraction, rightTargetSpeedFraction, zoom, brake, brake, camera.Forward); + + if (input.WasPushed(Fire) && frameIndex > lastPlayerShotFrameIndex + 60) + { + playerController.Tank.Fire(Simulation, bodyProperties); + lastPlayerShotFrameIndex = frameIndex; + ++projectileCount; + } + } + + for (int i = 0; i < aiTanks.Count; ++i) + { + aiTanks[i].Update(Simulation, bodyProperties, random, frameIndex, playAreaMin, playAreaMax, i, ref aiTanks, ref projectileCount); + } + + + frameIndex++; + //Ensure that the callbacks list of exploding projectiles can contain all projectiles that exist. + //(We cast the narrowphase to the generic subtype so that we can grab the callbacks. This isn't the only way- + //notice that we cached the bodyProperties reference outside of the callbacks for direct access. + //The exploding projectiles list, however, is a QuickList value type. If we tried to cache it outside we'd only have a copy of it. + //So, rather than trying to set up some pinned memory or replacing it with a reference type, we just cast our way in.) + ref var projectileImpacts = ref ((NarrowPhase)Simulation.NarrowPhase).Callbacks.ProjectileImpacts; + projectileImpacts.EnsureCapacity(projectileCount, BufferPool); + base.Update(window, camera, input, dt); + //Remove any projectile that hit something. + for (int i = 0; i < projectileImpacts.Count; ++i) + { + ref var impact = ref projectileImpacts[i]; + ref var explosion = ref explosions.Allocate(BufferPool); + explosion.Age = 0; + explosion.Position = Simulation.Bodies[impact.ProjectileHandle].Pose.Position; + explosion.Scale = 1f; + explosion.Color = new Vector3(1f, 0.5f, 0); + Simulation.Bodies.Remove(impact.ProjectileHandle); + if (impact.ImpactedTankBodyHandle.Value >= 0) + { + //The projectile hit a tank. Hurt it! + for (int aiIndex = 0; aiIndex < aiTanks.Count; ++aiIndex) + { + ref var aiTank = ref aiTanks[aiIndex]; + if (aiTank.Controller.Tank.Body.Value == impact.ImpactedTankBodyHandle.Value) + { + --aiTank.HitPoints; + if (aiTank.HitPoints == 0) + { + ref var deathExplosion = ref explosions.Allocate(BufferPool); + deathExplosion.Position = Simulation.Bodies[aiTank.Controller.Tank.Turret].Pose.Position; + deathExplosion.Scale = 3; + deathExplosion.Age = 0; + deathExplosion.Color = new Vector3(1, 0, 0); + aiTank.Controller.Tank.Explode(Simulation, bodyProperties, BufferPool); + aiTanks.FastRemoveAt(aiIndex); + } + break; + } + } + //This loop might actually fail to find the tank- if a tank gets hit by more than one projectile in a frame, or if the player tank is hit. + //(The player tank cheats and isn't in the aiTanks list.) + //That's fine, though. + } + } + projectileImpacts.Count = 0; + } + + + void RenderControl(ref Vector2 position, float textHeight, string controlName, string controlValue, TextBuilder text, TextBatcher textBatcher, Font font) + { + text.Clear().Append(controlName).Append(": ").Append(controlValue); + textBatcher.Write(text, position, textHeight, new Vector3(1), font); + position.Y += textHeight * 1.1f; + } + + public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + { + if (playerControlActive) + { + var tankBody = new BodyReference(playerController.Tank.Body, Simulation.Bodies); + QuaternionEx.TransformUnitY(tankBody.Pose.Orientation, out var tankUp); + QuaternionEx.TransformUnitZ(tankBody.Pose.Orientation, out var tankBackward); + var backwardDirection = camera.Backward; + backwardDirection.Y = MathF.Max(backwardDirection.Y, -0.2f); + camera.Position = tankBody.Pose.Position + tankUp * 3f + tankBackward * 0.4f + backwardDirection * 8; + } + + //Draw explosions and remove old ones. + for (int i = explosions.Count - 1; i >= 0; --i) + { + ref var explosion = ref explosions[i]; + var pose = new RigidPose(explosion.Position); + //The age is measured in frames, so it's not framerate independent. That's fine for a demo. + renderer.Shapes.AddShape(new Sphere(explosion.Scale * (0.25f + MathF.Sqrt(explosion.Age))), Simulation.Shapes, pose, explosion.Color); + if (explosion.Age > 5) + { + explosions.FastRemoveAt(i); + } + ++explosion.Age; + } + + var textHeight = 16; + var position = new Vector2(32, renderer.Surface.Resolution.Y - 144); + RenderControl(ref position, textHeight, nameof(Fire), Fire.ToString(), text, renderer.TextBatcher, font); + RenderControl(ref position, textHeight, nameof(Forward), Forward.ToString(), text, renderer.TextBatcher, font); + RenderControl(ref position, textHeight, nameof(Backward), Backward.ToString(), text, renderer.TextBatcher, font); + RenderControl(ref position, textHeight, nameof(Right), Right.ToString(), text, renderer.TextBatcher, font); + RenderControl(ref position, textHeight, nameof(Left), Left.ToString(), text, renderer.TextBatcher, font); + RenderControl(ref position, textHeight, nameof(Zoom), Zoom.ToString(), text, renderer.TextBatcher, font); + RenderControl(ref position, textHeight, nameof(Brake), Brake.ToString(), text, renderer.TextBatcher, font); + RenderControl(ref position, textHeight, nameof(ToggleTank), ToggleTank.ToString(), text, renderer.TextBatcher, font); + + if (aiTanks.Count > 0) + renderer.TextBatcher.Write(text.Clear().Append("Enemy tanks remaining: ").Append(aiTanks.Count), new Vector2(32, renderer.Surface.Resolution.Y - 172), 24, new Vector3(1, 1, 1), font); + else + renderer.TextBatcher.Write(text.Clear().Append("ya did it!"), new Vector2(32, renderer.Surface.Resolution.Y - 172), 24, new Vector3(0.3f, 1, 0.3f), font); + + base.Render(renderer, camera, input, text, font); + } + } +} \ No newline at end of file From fd15efcf5048d61fac529bef6981e0f3c723c870 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 8 Feb 2022 16:26:43 -0600 Subject: [PATCH 440/947] Oops, shoo old copies --- Demos/SpecializedTests/Media/BedsheetDemo.cs | 160 ----------------- .../Media/ColosseumVideoDemo.cs | 161 ------------------ .../Media/NewtDemandingSacrificeVideoDemo.cs | 78 --------- Demos/SpecializedTests/Media/NewtVideoDemo.cs | 67 -------- .../Media/PyramidVideoDemo.cs | 84 --------- .../Media/ShrinkwrappedNewtsVideoDemo.cs | 71 -------- 6 files changed, 621 deletions(-) delete mode 100644 Demos/SpecializedTests/Media/BedsheetDemo.cs delete mode 100644 Demos/SpecializedTests/Media/ColosseumVideoDemo.cs delete mode 100644 Demos/SpecializedTests/Media/NewtDemandingSacrificeVideoDemo.cs delete mode 100644 Demos/SpecializedTests/Media/NewtVideoDemo.cs delete mode 100644 Demos/SpecializedTests/Media/PyramidVideoDemo.cs delete mode 100644 Demos/SpecializedTests/Media/ShrinkwrappedNewtsVideoDemo.cs diff --git a/Demos/SpecializedTests/Media/BedsheetDemo.cs b/Demos/SpecializedTests/Media/BedsheetDemo.cs deleted file mode 100644 index 8d271c51b..000000000 --- a/Demos/SpecializedTests/Media/BedsheetDemo.cs +++ /dev/null @@ -1,160 +0,0 @@ -using BepuPhysics; -using BepuPhysics.Collidables; -using BepuPhysics.Constraints; -using BepuUtilities; -using DemoContentLoader; -using DemoRenderer; -using DemoRenderer.UI; -using DemoUtilities; -using System; -using System.Numerics; - -namespace Demos.Demos -{ - - /// - /// Shows a few different examples of cloth-ish constraint lattices. - /// - public class BedsheetDemo : Demo - { - delegate bool KinematicDecider(int rowIndex, int columnIndex, int width, int height); - - BodyHandle[,] CreateBodyGrid(in Vector3 position, in Quaternion orientation, int width, int height, float spacing, float bodyRadius, float massPerBody, - int instanceId, CollidableProperty filters, KinematicDecider isKinematic) - { - var description = BodyDescription.CreateKinematic(orientation, Simulation.Shapes.Add(new Sphere(bodyRadius)), 0.01f); - var inverseMass = 1f / massPerBody; - BodyHandle[,] handles = new BodyHandle[height, width]; - for (int rowIndex = 0; rowIndex < height; ++rowIndex) - { - for (int columnIndex = 0; columnIndex < width; ++columnIndex) - { - description.LocalInertia.InverseMass = isKinematic(rowIndex, columnIndex, width, height) ? 0 : inverseMass; - var localPosition = new Vector3(columnIndex * spacing, rowIndex * -spacing, 0); - QuaternionEx.TransformWithoutOverlap(localPosition, orientation, out var rotatedPosition); - description.Pose.Position = rotatedPosition + position; - var handle = Simulation.Bodies.Add(description); - handles[rowIndex, columnIndex] = handle; - filters.Allocate(handle) = new ClothCollisionFilter(rowIndex, columnIndex, instanceId); - } - } - return handles; - } - - void CreateAreaConstraints(BodyHandle[,] bodyHandles, SpringSettings springSettings) - { - for (int rowIndex = 0; rowIndex < bodyHandles.GetLength(0) - 1; ++rowIndex) - { - for (int columnIndex = 0; columnIndex < bodyHandles.GetLength(1) - 1; ++columnIndex) - { - var aHandle = bodyHandles[rowIndex, columnIndex]; - var bHandle = bodyHandles[rowIndex + 1, columnIndex]; - var cHandle = bodyHandles[rowIndex, columnIndex + 1]; - var dHandle = bodyHandles[rowIndex + 1, columnIndex + 1]; - var a = new BodyReference(aHandle, Simulation.Bodies); - var b = new BodyReference(bHandle, Simulation.Bodies); - var c = new BodyReference(cHandle, Simulation.Bodies); - var d = new BodyReference(dHandle, Simulation.Bodies); - //Not worried about kinematics here- we create at most one row of kinematics in this demo. These are three body constraints that operate in a local quad, so - //there's no way for them to all be kinematic. - Simulation.Solver.Add(aHandle, bHandle, cHandle, new AreaConstraint(a.Pose.Position, b.Pose.Position, c.Pose.Position, springSettings)); - Simulation.Solver.Add(bHandle, cHandle, dHandle, new AreaConstraint(b.Pose.Position, c.Pose.Position, d.Pose.Position, springSettings)); - } - } - } - void CreateDistanceConstraints(BodyHandle[,] bodyHandles, SpringSettings springSettings) - { - void CreateConstraintBetweenBodies(BodyHandle aHandle, BodyHandle bHandle) - { - var a = new BodyReference(aHandle, Simulation.Bodies); - var b = new BodyReference(bHandle, Simulation.Bodies); - //Don't create constraints between two kinematic bodies. - if (a.LocalInertia.InverseMass > 0 || b.LocalInertia.InverseMass > 0) - { - //Note the use of a limit; the distance is allowed to go smaller. - //This helps stop the cloth from having unnatural rigidity. - var distance = Vector3.Distance(a.Pose.Position, b.Pose.Position); - Simulation.Solver.Add(aHandle, bHandle, new CenterDistanceLimit(distance * 0.15f, distance, springSettings)); - } - } - for (int rowIndex = 0; rowIndex < bodyHandles.GetLength(0); ++rowIndex) - { - for (int columnIndex = 0; columnIndex < bodyHandles.GetLength(1) - 1; ++columnIndex) - { - CreateConstraintBetweenBodies(bodyHandles[rowIndex, columnIndex], bodyHandles[rowIndex, columnIndex + 1]); - } - } - for (int rowIndex = 0; rowIndex < bodyHandles.GetLength(0) - 1; ++rowIndex) - { - for (int columnIndex = 0; columnIndex < bodyHandles.GetLength(1); ++columnIndex) - { - CreateConstraintBetweenBodies(bodyHandles[rowIndex, columnIndex], bodyHandles[rowIndex + 1, columnIndex]); - } - } - for (int rowIndex = 0; rowIndex < bodyHandles.GetLength(0) - 1; ++rowIndex) - { - for (int columnIndex = 0; columnIndex < bodyHandles.GetLength(1) - 1; ++columnIndex) - { - CreateConstraintBetweenBodies(bodyHandles[rowIndex, columnIndex], bodyHandles[rowIndex + 1, columnIndex + 1]); - CreateConstraintBetweenBodies(bodyHandles[rowIndex, columnIndex + 1], bodyHandles[rowIndex + 1, columnIndex]); - } - } - } - - RolloverInfo rolloverInfo; - - public unsafe override void Initialize(ContentArchive content, Camera camera) - { - camera.Position = new Vector3(70, 40, -80); - camera.Yaw = -MathF.PI * 0.8f; - camera.Pitch = MathF.PI * 0.1f; - - var filters = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new ClothCallbacks(filters), new DemoPoseIntegratorCallbacks(new Vector3(0, -50, 0)), new SolveDescription(8, 1)); - rolloverInfo = new RolloverInfo(); - - bool FullyDynamic(int rowIndex, int columnIndex, int width, int height) - { - return false; - } - - int clothInstanceId = 0; - var initialRotation = QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), MathF.PI * -0.5f); - - - - - Simulation.Statics.Add(new StaticDescription(new Vector3(0, 10, 0), Simulation.Shapes.Add(new Box(80, 20, 80)))); - Simulation.Statics.Add(new StaticDescription(new Vector3(-20, 22, 30), Simulation.Shapes.Add(new Box(34, 4, 14)))); - Simulation.Statics.Add(new StaticDescription(new Vector3(20, 22, 30), Simulation.Shapes.Add(new Box(34, 4, 14)))); - - - Simulation.Statics.Add(new StaticDescription(new Vector3(65.5f, 8f, 20), Simulation.Shapes.Add(new Cylinder(15, 15)))); - - - { - var position = new Vector3(96 * 1.15f * -0.5f, 30, 86 * 1.15f * -0.5f); - var handles = CreateBodyGrid(position, initialRotation, 96, 86, 1.15f, 1f, 1, clothInstanceId++, filters, FullyDynamic); - CreateDistanceConstraints(handles, new SpringSettings(20, 1)); - CreateAreaConstraints(handles, new SpringSettings(30, 1)); - } - - { - var position = new Vector3(65.5f + 56 * 0.8f * -0.5f, 25, 20 + 56 * 0.8f * -0.5f); - var handles = CreateBodyGrid(position, initialRotation, 56, 56, 0.8f, 0.65f, 1, clothInstanceId++, filters, FullyDynamic); - CreateDistanceConstraints(handles, new SpringSettings(20, 1)); - CreateAreaConstraints(handles, new SpringSettings(30, 1)); - } - - Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), Simulation.Shapes.Add(new Box(400, 1, 400)))); - - } - - public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) - { - rolloverInfo.Render(renderer, camera, input, text, font); - base.Render(renderer, camera, input, text, font); - } - - } -} diff --git a/Demos/SpecializedTests/Media/ColosseumVideoDemo.cs b/Demos/SpecializedTests/Media/ColosseumVideoDemo.cs deleted file mode 100644 index bab1a80b6..000000000 --- a/Demos/SpecializedTests/Media/ColosseumVideoDemo.cs +++ /dev/null @@ -1,161 +0,0 @@ -using BepuPhysics; -using BepuPhysics.Collidables; -using BepuUtilities; -using DemoContentLoader; -using DemoRenderer; -using DemoRenderer.UI; -using Demos.Demos.Characters; -using DemoUtilities; -using OpenTK.Input; -using System; -using System.Numerics; - -namespace Demos.SpecializedTests.Media -{ - /// - /// Version of the colosseum demo for video purposes. - /// - public class ColosseumVideoDemo : Demo - { - void CreateRingWall(Vector3 position, Box ringBoxShape, BodyDescription bodyDescription, int height, float radius) - { - var circumference = MathF.PI * 2 * radius; - var boxCountPerRing = (int)(0.9f * circumference / ringBoxShape.Length); - float increment = MathHelper.TwoPi / boxCountPerRing; - for (int ringIndex = 0; ringIndex < height; ringIndex++) - { - for (int i = 0; i < boxCountPerRing; i++) - { - var angle = ((ringIndex & 1) == 0 ? i + 0.5f : i) * increment; - bodyDescription.Pose = (position + new Vector3(-MathF.Cos(angle) * radius, (ringIndex + 0.5f) * ringBoxShape.Height, MathF.Sin(angle) * radius), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, angle)); - Simulation.Bodies.Add(bodyDescription); - } - } - } - - void CreateRingPlatform(Vector3 position, Box ringBoxShape, BodyDescription bodyDescription, float radius) - { - var innerCircumference = MathF.PI * 2 * (radius - ringBoxShape.HalfLength); - var boxCount = (int)(0.95f * innerCircumference / ringBoxShape.Height); - float increment = MathHelper.TwoPi / boxCount; - for (int i = 0; i < boxCount; i++) - { - var angle = i * increment; - bodyDescription.Pose = (position + new Vector3(-MathF.Cos(angle) * radius, ringBoxShape.HalfWidth, MathF.Sin(angle) * radius), - QuaternionEx.Concatenate(QuaternionEx.CreateFromAxisAngle(Vector3.UnitZ, MathF.PI * 0.5f), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, angle + MathF.PI * 0.5f))); - Simulation.Bodies.Add(bodyDescription); - } - } - - Vector3 CreateRing(Vector3 position, Box ringBoxShape, BodyDescription bodyDescription, float radius, int heightPerPlatformLevel, int platformLevels) - { - for (int platformIndex = 0; platformIndex < platformLevels; ++platformIndex) - { - var wallOffset = ringBoxShape.HalfLength - ringBoxShape.HalfWidth; - CreateRingWall(position, ringBoxShape, bodyDescription, heightPerPlatformLevel, radius + wallOffset); - CreateRingWall(position, ringBoxShape, bodyDescription, heightPerPlatformLevel, radius - wallOffset); - CreateRingPlatform(position + new Vector3(0, heightPerPlatformLevel * ringBoxShape.Height, 0), ringBoxShape, bodyDescription, radius); - position.Y += heightPerPlatformLevel * ringBoxShape.Height + ringBoxShape.Width; - } - return position; - } - - CharacterControllers characters; - public unsafe override void Initialize(ContentArchive content, Camera camera) - { - camera.Position = new Vector3(-30, 40, -30); - camera.Yaw = MathHelper.Pi * 3f / 4; - camera.Pitch = MathHelper.Pi * 0.2f; - - characters = new CharacterControllers(BufferPool); - Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); - - var ringBoxShape = new Box(0.5f, 1.5f, 3); - var boxDescription = BodyDescription.CreateDynamic(new Vector3(), ringBoxShape.ComputeInertia(1), Simulation.Shapes.Add(ringBoxShape), 0.01f); - - var layerPosition = new Vector3(); - const int layerCount = 10; - var innerRadius = 5f; - var heightPerPlatform = 2; - var platformsPerLayer = 1; - var ringSpacing = 0.5f; - for (int layerIndex = 0; layerIndex < layerCount; ++layerIndex) - { - var ringCount = layerCount - layerIndex; - for (int ringIndex = 0; ringIndex < ringCount; ++ringIndex) - { - CreateRing(layerPosition, ringBoxShape, boxDescription, innerRadius + ringIndex * (ringBoxShape.Length + ringSpacing) + layerIndex * (ringBoxShape.Length - ringBoxShape.Width), heightPerPlatform, platformsPerLayer); - } - layerPosition.Y += platformsPerLayer * (ringBoxShape.Height * heightPerPlatform + ringBoxShape.Width); - } - - Console.WriteLine($"box count: {Simulation.Bodies.ActiveSet.Count}"); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), Simulation.Shapes.Add(new Box(500, 1, 500)))); - - var bulletShape = new Sphere(0.5f); - bulletDescription = BodyDescription.CreateDynamic(new Vector3(), bulletShape.ComputeInertia(.1f), Simulation.Shapes.Add(bulletShape), 0.01f); - - var shootiePatootieShape = new Sphere(3f); - shootiePatootieDescription = BodyDescription.CreateDynamic(new Vector3(), shootiePatootieShape.ComputeInertia(1000), Simulation.Shapes.Add(shootiePatootieShape), 0.01f); - } - - bool characterActive; - CharacterInput character; - void CreateCharacter(Vector3 position) - { - characterActive = true; - character = new CharacterInput(characters, position, new Capsule(0.5f, 1), 0.1f, 1, 20, 100, 6, 4, MathF.PI * 0.4f); - } - - - BodyDescription bulletDescription; - BodyDescription shootiePatootieDescription; - public override void Update(Window window, Camera camera, Input input, float dt) - { - if (input != null) - { - if (input.WasPushed(Key.C)) - { - if (characterActive) - { - character.Dispose(); - characterActive = false; - } - else - { - CreateCharacter(camera.Position); - } - } - if (characterActive) - { - character.UpdateCharacterGoals(input, camera, Demo.TimestepDuration); - } - - if (input.WasPushed(Key.Z)) - { - bulletDescription.Pose.Position = camera.Position; - bulletDescription.Velocity.Linear = camera.GetRayDirection(input.MouseLocked, window.GetNormalizedMousePosition(input.MousePosition)) * 400; - Simulation.Bodies.Add(bulletDescription); - } - else if (input.WasPushed(Key.X)) - { - shootiePatootieDescription.Pose.Position = camera.Position; - shootiePatootieDescription.Velocity.Linear = camera.GetRayDirection(input.MouseLocked, window.GetNormalizedMousePosition(input.MousePosition)) * 100; - Simulation.Bodies.Add(shootiePatootieDescription); - } - } - base.Update(window, camera, input, dt); - } - - public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) - { - if (characterActive) - { - character.UpdateCameraPosition(camera, 0); - } - //text.Clear().Append("Press Z to shoot a bullet, press X to super shootie patootie!"); - //renderer.TextBatcher.Write(text, new Vector2(20, renderer.Surface.Resolution.Y - 20), 16, new Vector3(1, 1, 1), font); - base.Render(renderer, camera, input, text, font); - } - } -} diff --git a/Demos/SpecializedTests/Media/NewtDemandingSacrificeVideoDemo.cs b/Demos/SpecializedTests/Media/NewtDemandingSacrificeVideoDemo.cs deleted file mode 100644 index 87ea5cd76..000000000 --- a/Demos/SpecializedTests/Media/NewtDemandingSacrificeVideoDemo.cs +++ /dev/null @@ -1,78 +0,0 @@ -using BepuPhysics; -using BepuPhysics.Collidables; -using BepuUtilities; -using DemoContentLoader; -using DemoRenderer; -using Demos.Demos; -using DemoUtilities; -using System; -using System.Numerics; - -namespace Demos.SpecializedTests -{ - public class NewtDemandingSacrificeVideoDemo : Demo - { - CollidableProperty filters; - public unsafe override void Initialize(ContentArchive content, Camera camera) - { - camera.Position = new Vector3(-32f, 20.5f, 61f); - camera.Yaw = MathHelper.Pi * 0.3f; - camera.Pitch = MathHelper.Pi * -0.05f; - - filters = new CollidableProperty(BufferPool); - Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks(filters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(4, 1)); - - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), Simulation.Shapes.Add(new Box(1500, 1, 1500)))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, 10, 0), Simulation.Shapes.Add(new Box(70, 20, 80)))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, 7.5f, 0), Simulation.Shapes.Add(new Box(80, 15, 90)))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, 5, 0), Simulation.Shapes.Add(new Box(90, 10, 100)))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, 2.5f, 0), Simulation.Shapes.Add(new Box(100, 5, 110)))); - - //High fidelity simulation isn't super important on this one. - Simulation.Solver.VelocityIterationCount = 2; - - DemoMeshHelper.LoadModel(content, BufferPool, "Content\\newt.obj", new Vector3(30), out var mesh); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, 20, 0), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, 0), Simulation.Shapes.Add(mesh))); - } - - Random random = new Random(5); - int ragdollIndex = 0; - - BodyVelocity GetRandomizedVelocity(in Vector3 linearVelocity) - { - return new BodyVelocity { Linear = linearVelocity, Angular = new Vector3(-20) + 40 * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) }; - } - - public override void Update(Window window, Camera camera, Input input, float dt) - { - var pose = TestHelpers.CreateRandomPose(random, new BoundingBox - { - Min = new Vector3(-10, 5, 70), - Max = new Vector3(10, 15, 70) - }); - var linearVelocity = Vector3.Normalize(new Vector3(-2 + 4 * random.NextSingle(), 31 + 4 * random.NextSingle(), 50) - pose.Position) * 40; - var handles = RagdollDemo.AddRagdoll(pose.Position, pose.Orientation, ragdollIndex++, filters, Simulation); - var bodies = Simulation.Bodies; - //This could be done better, but... ... .... .......... - bodies[handles.Hips].Velocity = GetRandomizedVelocity(linearVelocity); - bodies[handles.Abdomen].Velocity = GetRandomizedVelocity(linearVelocity); - bodies[handles.Chest].Velocity = GetRandomizedVelocity(linearVelocity); - bodies[handles.Head].Velocity = GetRandomizedVelocity(linearVelocity); - bodies[handles.LeftArm.UpperArm].Velocity = GetRandomizedVelocity(linearVelocity); - bodies[handles.LeftArm.LowerArm].Velocity = GetRandomizedVelocity(linearVelocity); - bodies[handles.LeftArm.Hand].Velocity = GetRandomizedVelocity(linearVelocity); - bodies[handles.RightArm.UpperArm].Velocity = GetRandomizedVelocity(linearVelocity); - bodies[handles.RightArm.LowerArm].Velocity = GetRandomizedVelocity(linearVelocity); - bodies[handles.RightArm.Hand].Velocity = GetRandomizedVelocity(linearVelocity); - bodies[handles.LeftLeg.UpperLeg].Velocity = GetRandomizedVelocity(linearVelocity); - bodies[handles.LeftLeg.LowerLeg].Velocity = GetRandomizedVelocity(linearVelocity); - bodies[handles.LeftLeg.Foot].Velocity = GetRandomizedVelocity(linearVelocity); - bodies[handles.RightLeg.UpperLeg].Velocity = GetRandomizedVelocity(linearVelocity); - bodies[handles.RightLeg.LowerLeg].Velocity = GetRandomizedVelocity(linearVelocity); - bodies[handles.RightLeg.Foot].Velocity = GetRandomizedVelocity(linearVelocity); - - base.Update(window, camera, input, dt); - } - - } -} diff --git a/Demos/SpecializedTests/Media/NewtVideoDemo.cs b/Demos/SpecializedTests/Media/NewtVideoDemo.cs deleted file mode 100644 index d2e794dce..000000000 --- a/Demos/SpecializedTests/Media/NewtVideoDemo.cs +++ /dev/null @@ -1,67 +0,0 @@ -using BepuPhysics; -using BepuPhysics.Collidables; -using BepuPhysics.Constraints; -using BepuUtilities; -using BepuUtilities.Memory; -using DemoContentLoader; -using DemoRenderer; -using Demos.Demos; -using DemoUtilities; -using System; -using System.Numerics; - -namespace Demos.SpecializedTests -{ - public class NewtVideoDemo : Demo - { - public unsafe override void Initialize(ContentArchive content, Camera camera) - { - camera.Position = new Vector3(-5f, 5.5f, 5f); - camera.Yaw = MathHelper.Pi / 4; - camera.Pitch = MathHelper.Pi * 0.15f; - - var filters = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new DeformableCallbacks(filters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); - - var meshContent = content.Load("Content\\newt.obj"); - float cellSize = 0.1f; - DumbTetrahedralizer.Tetrahedralize(meshContent.Triangles, cellSize, BufferPool, - out var vertices, out var vertexSpatialIndices, out var cellVertexIndices, out var tetrahedraVertexIndices); - var weldSpringiness = new SpringSettings(30f, 0); - var volumeSpringiness = new SpringSettings(30f, 1); - for (int i = 0; i < 5; ++i) - { - NewtDemo.CreateDeformable(Simulation, new Vector3(i * 3, 5 + i * 1.5f, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), MathF.PI * (i * 0.55f)), 1f, cellSize, weldSpringiness, volumeSpringiness, i, filters, ref vertices, ref vertexSpatialIndices, ref cellVertexIndices, ref tetrahedraVertexIndices); - } - - BufferPool.Return(ref vertices); - vertexSpatialIndices.Dispose(BufferPool); - BufferPool.Return(ref cellVertexIndices); - BufferPool.Return(ref tetrahedraVertexIndices); - - Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 100, -.5f), 10, Simulation.Shapes, new Sphere(5))); - - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), Simulation.Shapes.Add(new Box(1500, 1, 1500)))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -1.5f, 0), Simulation.Shapes.Add(new Sphere(3)))); - - var bulletShape = new Sphere(0.5f); - bulletDescription = BodyDescription.CreateDynamic(RigidPose.Identity, bulletShape.ComputeInertia(.25f), Simulation.Shapes.Add(bulletShape), 0.01f); - - DemoMeshHelper.LoadModel(content, BufferPool, "Content\\newt.obj", new Vector3(20), out var mesh); - Simulation.Statics.Add(new StaticDescription(new Vector3(200, 0.5f, 120), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, -3 * MathHelper.PiOver4), Simulation.Shapes.Add(mesh))); - } - BodyDescription bulletDescription; - public override void Update(Window window, Camera camera, Input input, float dt) - { - if (input.WasPushed(OpenTK.Input.Key.Z)) - { - bulletDescription.Pose.Position = camera.Position; - bulletDescription.Velocity.Linear = camera.Forward * 40; - Simulation.Bodies.Add(bulletDescription); - } - base.Update(window, camera, input, dt); - } - - - } -} diff --git a/Demos/SpecializedTests/Media/PyramidVideoDemo.cs b/Demos/SpecializedTests/Media/PyramidVideoDemo.cs deleted file mode 100644 index a35211992..000000000 --- a/Demos/SpecializedTests/Media/PyramidVideoDemo.cs +++ /dev/null @@ -1,84 +0,0 @@ -using BepuPhysics; -using BepuPhysics.Collidables; -using BepuPhysics.Constraints; -using BepuUtilities; -using DemoContentLoader; -using DemoRenderer; -using DemoRenderer.UI; -using DemoUtilities; -using System; -using System.Collections.Generic; -using System.Numerics; -using System.Text; - -namespace Demos.Demos -{ - /// - /// A pyramid of boxes, because you can't have a physics engine without pyramids of boxes. - /// - public class PyramidVideoDemo : Demo - { - public unsafe override void Initialize(ContentArchive content, Camera camera) - { - camera.Position = new Vector3(-70, 8, 318); - camera.Yaw = MathHelper.Pi * 1f / 4; - camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); - - var boxShape = new Box(1, 1, 1); - var boxInertia = boxShape.ComputeInertia(1); - var boxIndex = Simulation.Shapes.Add(boxShape); - const int pyramidCount = 120; - for (int pyramidIndex = 0; pyramidIndex < pyramidCount; ++pyramidIndex) - { - const int rowCount = 20; - for (int rowIndex = 0; rowIndex < rowCount; ++rowIndex) - { - int columnCount = rowCount - rowIndex; - for (int columnIndex = 0; columnIndex < columnCount; ++columnIndex) - { - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3( - (-columnCount * 0.5f + columnIndex) * boxShape.Width, - (rowIndex + 0.5f) * boxShape.Height, - (pyramidIndex - pyramidCount * 0.5f) * (boxShape.Length + 4)), - boxInertia, boxIndex, 0.01f)); - } - } - } - - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), Simulation.Shapes.Add(new Box(2500, 1, 2500)))); - } - - //We'll randomize the size of bullets. - Random random = new Random(5); - public override void Update(Window window, Camera camera, Input input, float dt) - { - if (input != null && input.WasPushed(OpenTK.Input.Key.Z)) - { - //Create the shape that we'll launch at the pyramids when the user presses a button. - var bulletShape = new Sphere(6); - //Note that the use of radius^3 for mass can produce some pretty serious mass ratios. - //Observe what happens when a large ball sits on top of a few boxes with a fraction of the mass- - //the collision appears much squishier and less stable. For most games, if you want to maintain rigidity, you'll want to use some combination of: - //1) Limit the ratio of heavy object masses to light object masses when those heavy objects depend on the light objects. - //2) Use a shorter timestep duration and update more frequently. - //3) Use a greater number of solver iterations. - //#2 and #3 can become very expensive. In pathological cases, it can end up slower than using a quality-focused solver for the same simulation. - //Unfortunately, at the moment, bepuphysics v2 does not contain any alternative solvers, so if you can't afford to brute force the the problem away, - //the best solution is to cheat as much as possible to avoid the corner cases. - var bodyDescription = BodyDescription.CreateConvexDynamic( - new Vector3(0, 8, -500), new Vector3(0, 0, 110), 50000, Simulation.Shapes, bulletShape); - Simulation.Bodies.Add(bodyDescription); - } - base.Update(window, camera, input, dt); - } - - public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) - { - text.Clear().Append("Press Z to launch a ball!"); - renderer.TextBatcher.Write(text, new Vector2(20, renderer.Surface.Resolution.Y - 20), 16, new Vector3(1, 1, 1), font); - base.Render(renderer, camera, input, text, font); - } - - } -} diff --git a/Demos/SpecializedTests/Media/ShrinkwrappedNewtsVideoDemo.cs b/Demos/SpecializedTests/Media/ShrinkwrappedNewtsVideoDemo.cs deleted file mode 100644 index eb5e883e2..000000000 --- a/Demos/SpecializedTests/Media/ShrinkwrappedNewtsVideoDemo.cs +++ /dev/null @@ -1,71 +0,0 @@ -using BepuPhysics; -using BepuPhysics.Collidables; -using BepuPhysics.Constraints; -using BepuUtilities; -using BepuUtilities.Collections; -using DemoContentLoader; -using DemoRenderer; -using DemoUtilities; -using System; -using System.Numerics; - -namespace Demos.SpecializedTests -{ - public class ShrinkwrappedNewtsVideoDemo : Demo - { - public unsafe override void Initialize(ContentArchive content, Camera camera) - { - camera.Position = new Vector3(25f, 1.5f, 15f); - camera.Yaw = 3 * MathHelper.Pi / 4; - camera.Pitch = 0;// MathHelper.Pi * 0.15f; - - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); - - var meshContent = content.Load("Content\\newt.obj"); - - //This is actually a pretty good example of how *not* to make a convex hull shape. - //Generating it directly from a graphical data source tends to have way more surface complexity than needed, - //and it tends to have a lot of near-but-not-quite-coplanar surfaces which can make the contact manifold less stable. - //Prefer a simpler source with more distinct features, possibly created with an automated content-time tool. - var points = new QuickList(meshContent.Triangles.Length * 3, BufferPool); - for (int i = 0; i < meshContent.Triangles.Length; ++i) - { - ref var triangle = ref meshContent.Triangles[i]; - //resisting the urge to just reinterpret the memory - points.AllocateUnsafely() = triangle.A * new Vector3(1, 1.5f, 1); - points.AllocateUnsafely() = triangle.B * new Vector3(1, 1.5f, 1); - points.AllocateUnsafely() = triangle.C * new Vector3(1, 1.5f, 1); - } - - var newtHull = new ConvexHull(points.Span.Slice(points.Count), BufferPool, out _); - var bodyDescription = BodyDescription.CreateConvexDynamic(RigidPose.Identity, 1, Simulation.Shapes, newtHull); - Random random = new Random(5); - var poseBounds = new BoundingBox { Min = new Vector3(-20, 1, 5), Max = new Vector3(20, 10, 50) }; - for (int i = 0; i < 512; ++i) - { - bodyDescription.Pose = TestHelpers.CreateRandomPose(random, poseBounds); - Simulation.Bodies.Add(bodyDescription); - } - - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -30, 250), Simulation.Shapes.Add(new Box(1000, 60, 500)))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -60, 0), Simulation.Shapes.Add(new Box(1000, 1, 1000)))); - - - - DemoMeshHelper.LoadModel(content, BufferPool, "Content\\newt.obj", new Vector3(1, 1.5f, 1), out mesh); - Simulation.Statics.Add(new StaticDescription(new Vector3(30, 0, 20), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, -3 * MathHelper.PiOver4), Simulation.Shapes.Add(mesh))); - } - - Mesh mesh; - - public override void Update(Window window, Camera camera, Input input, float dt) - { - if(input.WasPushed(OpenTK.Input.Key.Z)) - { - mesh.Scale = new Vector3(30); - Simulation.Statics.Add(new StaticDescription(new Vector3(70, 0, 50), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, -3.1f * MathHelper.PiOver4), Simulation.Shapes.Add(mesh))); - } - base.Update(window, camera, input, dt); - } - } -} From 864f5251388bd5e7652e76dc0043fdf4daf7f970 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 8 Feb 2022 17:34:56 -0600 Subject: [PATCH 441/947] More video-related demo changes. --- Demos/DemoSet.cs | 4 + Demos/Demos/ColosseumDemo.cs | 2 +- .../Media/2.0/ColosseumVideoDemo.cs | 2 +- .../Media/2.4/NewtTyrannyDemo.cs | 116 ++++++++++++++++++ 4 files changed, 122 insertions(+), 2 deletions(-) create mode 100644 Demos/SpecializedTests/Media/2.4/NewtTyrannyDemo.cs diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index 0cf3a1a5e..8d2dd8e8d 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -46,6 +46,10 @@ struct Option public DemoSet() { + AddOption(); + AddOption(); + AddOption(); + AddOption(); AddOption(); AddOption(); AddOption(); diff --git a/Demos/Demos/ColosseumDemo.cs b/Demos/Demos/ColosseumDemo.cs index d6b46f3ee..5a146f9fe 100644 --- a/Demos/Demos/ColosseumDemo.cs +++ b/Demos/Demos/ColosseumDemo.cs @@ -93,7 +93,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) bulletDescription = BodyDescription.CreateDynamic(new Vector3(), bulletShape.ComputeInertia(.1f), Simulation.Shapes.Add(bulletShape), 0.01f); var shootiePatootieShape = new Sphere(3f); - shootiePatootieDescription = BodyDescription.CreateDynamic(new Vector3(), shootiePatootieShape.ComputeInertia(100), Simulation.Shapes.Add(shootiePatootieShape), 0.01f); + shootiePatootieDescription = BodyDescription.CreateDynamic(new Vector3(), shootiePatootieShape.ComputeInertia(100), new (Simulation.Shapes.Add(shootiePatootieShape), 0.1f), 0.01f); } BodyDescription bulletDescription; diff --git a/Demos/SpecializedTests/Media/2.0/ColosseumVideoDemo.cs b/Demos/SpecializedTests/Media/2.0/ColosseumVideoDemo.cs index bab1a80b6..04fd3fd6a 100644 --- a/Demos/SpecializedTests/Media/2.0/ColosseumVideoDemo.cs +++ b/Demos/SpecializedTests/Media/2.0/ColosseumVideoDemo.cs @@ -96,7 +96,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) bulletDescription = BodyDescription.CreateDynamic(new Vector3(), bulletShape.ComputeInertia(.1f), Simulation.Shapes.Add(bulletShape), 0.01f); var shootiePatootieShape = new Sphere(3f); - shootiePatootieDescription = BodyDescription.CreateDynamic(new Vector3(), shootiePatootieShape.ComputeInertia(1000), Simulation.Shapes.Add(shootiePatootieShape), 0.01f); + shootiePatootieDescription = BodyDescription.CreateDynamic(new Vector3(), shootiePatootieShape.ComputeInertia(1000), new (Simulation.Shapes.Add(shootiePatootieShape), 0.1f), 0.01f); } bool characterActive; diff --git a/Demos/SpecializedTests/Media/2.4/NewtTyrannyDemo.cs b/Demos/SpecializedTests/Media/2.4/NewtTyrannyDemo.cs new file mode 100644 index 000000000..748a22508 --- /dev/null +++ b/Demos/SpecializedTests/Media/2.4/NewtTyrannyDemo.cs @@ -0,0 +1,116 @@ +using DemoContentLoader; +using DemoRenderer; +using System; +using System.Collections.Generic; +using System.Numerics; +using System.Text; +using BepuPhysics; +using DemoRenderer.UI; +using System.IO; +using DemoUtilities; +using System.Diagnostics; +using BepuUtilities.Collections; +using BepuPhysics.Collidables; +using Demos.Demos.Characters; + +namespace Demos.Demos.Sponsors +{ + public class NewtTyrannyDemo : Demo + { + QuickList newts; + + Vector2 newtArenaMin, newtArenaMax; + Random random; + CharacterControllers characterControllers; + QuickList characterAIs; + public override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(130, 50, 130); + camera.Yaw = -MathF.PI * 0.25f; + camera.Pitch = 0.4f; + + characterControllers = new CharacterControllers(BufferPool); + Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characterControllers), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); + Simulation.Deterministic = true; + + DemoMeshHelper.LoadModel(content, BufferPool, @"Content\newt.obj", new Vector3(-10, 10, -10), out var newtMesh); + var newtShape = Simulation.Shapes.Add(newtMesh); + var newtCount = 10; + newts = new QuickList(newtCount, BufferPool); + newtArenaMin = new Vector2(-250); + newtArenaMax = new Vector2(250); + random = new Random(8); + for (int i = 0; i < newtCount; ++i) + { + ref var newt = ref newts.AllocateUnsafely(); + newt = new SponsorNewt(Simulation, newtShape, 0, newtArenaMin, newtArenaMax, random, i); + } + + const float floorSize = 520; + const float wallThickness = 200; + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -10f, 0), Simulation.Shapes.Add(new Box(floorSize, 20, floorSize)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(floorSize * -0.5f - wallThickness * 0.5f, -5, 0), Simulation.Shapes.Add(new Box(wallThickness, 30, floorSize + wallThickness * 2)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(floorSize * 0.5f + wallThickness * 0.5f, -5, 0), Simulation.Shapes.Add(new Box(wallThickness, 30, floorSize + wallThickness * 2)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5, floorSize * -0.5f - wallThickness * 0.5f), Simulation.Shapes.Add(new Box(floorSize, 30, wallThickness)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5, floorSize * 0.5f + wallThickness * 0.5f), Simulation.Shapes.Add(new Box(floorSize, 30, wallThickness)))); + + const int characterCount = 2000; + characterAIs = new QuickList(characterCount, BufferPool); + var characterCollidable = Simulation.Shapes.Add(new Capsule(0.5f, 1f)); + for (int i = 0; i < characterCount; ++i) + { + var position2D = newtArenaMin + (newtArenaMax - newtArenaMin) * new Vector2(random.NextSingle(), random.NextSingle()); + var targetPosition = 0.5f * (newtArenaMin + (newtArenaMax - newtArenaMin) * new Vector2(random.NextSingle(), random.NextSingle())); + characterAIs.AllocateUnsafely() = new SponsorCharacterAI(characterControllers, characterCollidable, new Vector3(position2D.X, 5, position2D.Y), targetPosition); + } + + const int hutCount = 120; + var hutBoxShape = new Box(0.4f, 2, 3); + var obstacleDescription = BodyDescription.CreateDynamic(new Vector3(), hutBoxShape.ComputeInertia(20), new CollidableDescription(Simulation.Shapes.Add(hutBoxShape), 0.1f), 1e-2f); + + for (int i = 0; i < hutCount; ++i) + { + var position2D = newtArenaMin + (newtArenaMax - newtArenaMin) * new Vector2(random.NextSingle(), random.NextSingle()); + ColosseumDemo.CreateRing(Simulation, new Vector3(position2D.X, 0, position2D.Y), hutBoxShape, obstacleDescription, 4 + random.NextSingle() * 8, 2, random.Next(1, 10)); + + } + + var overlordNewtShape = newtMesh; + overlordNewtShape.Scale = new Vector3(60, 60, 60); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 10, -floorSize * 0.5f - 70), Simulation.Shapes.Add(overlordNewtShape))); + + + character = new CharacterInput(characterControllers, new Vector3(-108.89504f, 28.403418f, 38.27505f), new Capsule(0.5f, 1), 0.1f, .1f, 20, 100, 6, 4, MathF.PI * 0.4f); + + Console.WriteLine($"body count: {Simulation.Bodies.ActiveSet.Count}"); + } + + CharacterInput character; + + + + double simulationTime; + public override void Update(Window window, Camera camera, Input input, float dt) + { + character.UpdateCharacterGoals(input, camera, TimestepDuration); + Simulation.Timestep(TimestepDuration, ThreadDispatcher); + character.UpdateCameraPosition(camera, -0.3f); + for (int i = 0; i < newts.Count; ++i) + { + newts[i].Update(Simulation, simulationTime, 0, newtArenaMin, newtArenaMax, random, 1f / TimestepDuration); + } + for (int i = 0; i < characterAIs.Count; ++i) + { + characterAIs[i].Update(characterControllers, Simulation, ref newts, newtArenaMin, newtArenaMax, random); + } + simulationTime += TimestepDuration; + + + if(input.WasPushed(OpenTK.Input.Key.P)) + { + Console.WriteLine($"camera position: {camera.Position}"); + } + } + + } +} From 424a10a83872c40ad924455200ec3198a87e6c36 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 8 Feb 2022 18:44:22 -0600 Subject: [PATCH 442/947] Morevideo. --- Demos/DemoSet.cs | 4 - .../Media/2.4/ExcessivePyramidVideoDemo.cs | 2 +- .../Media/2.4/RopeTwistVideoDemo.cs | 107 ++++++++++++++++++ 3 files changed, 108 insertions(+), 5 deletions(-) create mode 100644 Demos/SpecializedTests/Media/2.4/RopeTwistVideoDemo.cs diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index 8d2dd8e8d..0cf3a1a5e 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -46,10 +46,6 @@ struct Option public DemoSet() { - AddOption(); - AddOption(); - AddOption(); - AddOption(); AddOption(); AddOption(); AddOption(); diff --git a/Demos/SpecializedTests/Media/2.4/ExcessivePyramidVideoDemo.cs b/Demos/SpecializedTests/Media/2.4/ExcessivePyramidVideoDemo.cs index b1ba70499..124cabf12 100644 --- a/Demos/SpecializedTests/Media/2.4/ExcessivePyramidVideoDemo.cs +++ b/Demos/SpecializedTests/Media/2.4/ExcessivePyramidVideoDemo.cs @@ -20,7 +20,7 @@ public class ExcessivePyramidVideoDemo : Demo { public unsafe override void Initialize(ContentArchive content, Camera camera) { - camera.Position = new Vector3(-120, 16, 1045); + camera.Position = new Vector3(-120, 32, 1045); camera.Yaw = MathHelper.Pi * 1f / 4; camera.Pitch = 0; Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1), frictionCoefficient: 2), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); diff --git a/Demos/SpecializedTests/Media/2.4/RopeTwistVideoDemo.cs b/Demos/SpecializedTests/Media/2.4/RopeTwistVideoDemo.cs new file mode 100644 index 000000000..64d6174be --- /dev/null +++ b/Demos/SpecializedTests/Media/2.4/RopeTwistVideoDemo.cs @@ -0,0 +1,107 @@ +using BepuPhysics; +using BepuPhysics.Collidables; +using BepuPhysics.CollisionDetection; +using BepuPhysics.Constraints; +using DemoContentLoader; +using DemoRenderer; +using DemoRenderer.UI; +using Demos.Demos; +using DemoUtilities; +using System; +using System.Numerics; +using System.Runtime.CompilerServices; + +namespace Demos.SpecializedTests.Media; + +/// +/// Shows a bundle of ropes being tangled up by spinning weights. +/// +public class RopeTwistVideoDemo : Demo +{ + public unsafe override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(0, 20, 20); + camera.Yaw = 0; + camera.Pitch = 0; + + var filters = new CollidableProperty(); + Simulation = Simulation.Create(BufferPool, + new RopeNarrowPhaseCallbacks(filters, new PairMaterialProperties(1.0f, float.MaxValue, new SpringSettings(1200, 1))), + new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(1, 60)); + + for (int twistIndex = 0; twistIndex < 10; ++twistIndex) + { + const int ropeCount = 4; + var startLocation = new Vector3(0 + twistIndex * 30, 30, 0); + + var bigWreckingBall = new Sphere(3); + //This wrecking ball is much, much heavier. + var bigWreckingBallInertia = bigWreckingBall.ComputeInertia(10000); + var bigWreckingBallIndex = Simulation.Shapes.Add(bigWreckingBall); + const float ropeBodySpacing = -0.1f; + const float ropeBodyRadius = 0.1f; + const int ropeBodyCount = 130; + var wreckingBallPosition = startLocation - new Vector3(0, ropeBodyRadius + (ropeBodyRadius * 2 + ropeBodySpacing) * ropeBodyCount + bigWreckingBall.Radius, 0); + var description = BodyDescription.CreateDynamic(wreckingBallPosition, bigWreckingBallInertia, bigWreckingBallIndex, 0.01f); + var wreckingBallBodyHandle = Simulation.Bodies.Add(description); + var wreckingBallBody = Simulation.Bodies[wreckingBallBodyHandle]; + wreckingBallBody.Velocity.Angular = new Vector3(0, 20, 0); + filters.Allocate(wreckingBallBodyHandle) = new RopeFilter { RopeIndex = (short)(16384 + twistIndex), IndexInRope = ropeBodyCount }; + + for (int ropeIndex = 0; ropeIndex < ropeCount; ++ropeIndex) + { + var angle = ropeIndex * MathF.PI * 2 / ropeCount; + const float ropeDistributionRadius = 1f; + var horizontalOffset = ropeDistributionRadius * new Vector3(MathF.Sin(angle), 0, MathF.Cos(angle)); + var ropeStartLocation = startLocation + horizontalOffset; + + var springSettings = new SpringSettings(600, 100); + var bodyHandles = RopeStabilityDemo.BuildRopeBodies(Simulation, ropeStartLocation, ropeBodyCount, ropeBodyRadius, ropeBodySpacing, 1f, 0); + for (int i = 0; i < bodyHandles.Length; ++i) + { + filters.Allocate(bodyHandles[i]) = new RopeFilter { RopeIndex = (short)ropeIndex, IndexInRope = (short)i }; + } + + bool TryCreateConstraint(int handleIndexA, int handleIndexB) + { + if (handleIndexA >= bodyHandles.Length || handleIndexB >= bodyHandles.Length) + return false; + var maximumDistance = Vector3.Distance( + new BodyReference(bodyHandles[handleIndexA], Simulation.Bodies).Pose.Position, + new BodyReference(bodyHandles[handleIndexB], Simulation.Bodies).Pose.Position); + Simulation.Solver.Add(bodyHandles[handleIndexA], bodyHandles[handleIndexB], new DistanceLimit(default, default, .01f, maximumDistance, springSettings)); + return true; + } + const int constraintsPerBody = 1; + for (int i = 0; i < bodyHandles.Length - 1; ++i) + { + //Note that you could also create constraints which span even more links. For example, connect i and i+1, i+2, i+4, i+8 and i+16 rather than just the nearest bodies. + //That tends to make mass ratios less of an issue, but this demo is a worst case stress test. + for (int j = 1; j <= constraintsPerBody; ++j) + { + if (!TryCreateConstraint(i, i + j)) + break; + } + } + + var wreckingBallConnectionOffset = horizontalOffset + new Vector3(0, bigWreckingBall.Radius, 0); + var ropeConnectionToBall = wreckingBallBody.Pose.Position + wreckingBallConnectionOffset; + for (int i = 1; i <= constraintsPerBody; ++i) + { + var targetBodyHandleIndex = bodyHandles.Length - i; + if (targetBodyHandleIndex < 0) + break; + var maximumDistance = Vector3.Distance( + new BodyReference(bodyHandles[targetBodyHandleIndex], Simulation.Bodies).Pose.Position, + ropeConnectionToBall); + Simulation.Solver.Add(bodyHandles[targetBodyHandleIndex], wreckingBallBodyHandle, new DistanceLimit(default, wreckingBallConnectionOffset, 0.01f, maximumDistance, springSettings)); + } + + } + } + + Console.WriteLine($"body count: {Simulation.Bodies.ActiveSet.Count}"); + + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), Simulation.Shapes.Add(new Box(200, 1, 200)))); + } +} From 734dfd3dcd32b9723ab84cd5b80fa35d8a9c04ff Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 9 Feb 2022 11:30:07 -0600 Subject: [PATCH 443/947] Videodancers. --- .../Media/2.4/VideoDancerDemo.cs | 201 ++++++++++++++ .../Media/2.4/VideoPlumpDancerDemo.cs | 257 ++++++++++++++++++ 2 files changed, 458 insertions(+) create mode 100644 Demos/SpecializedTests/Media/2.4/VideoDancerDemo.cs create mode 100644 Demos/SpecializedTests/Media/2.4/VideoPlumpDancerDemo.cs diff --git a/Demos/SpecializedTests/Media/2.4/VideoDancerDemo.cs b/Demos/SpecializedTests/Media/2.4/VideoDancerDemo.cs new file mode 100644 index 000000000..ee0f40809 --- /dev/null +++ b/Demos/SpecializedTests/Media/2.4/VideoDancerDemo.cs @@ -0,0 +1,201 @@ +using BepuPhysics; +using BepuPhysics.Collidables; +using BepuPhysics.CollisionDetection; +using BepuPhysics.Constraints; +using BepuUtilities; +using BepuUtilities.Memory; +using DemoContentLoader; +using DemoRenderer; +using DemoRenderer.UI; +using Demos.Demos; +using Demos.Demos.Dancers; +using DemoUtilities; +using System; +using System.Numerics; +using System.Runtime.InteropServices; + +namespace Demos.SpecializedTests.Media +{ + /// + /// A bunch of background dancers struggle to keep up with the masterful purple prancer while wearing dresses made of out of balls connected by constraints. + /// Combined with the implementation, this provides a starting point for cosmetic cloth attached to characters. + /// + public class VideoDancerDemo : Demo + { + //This demo relies on the DemoDancers to manage all the ragdolls and their simulations. + //All this demo needs to do is make a dress out of balls and drape it onto them. + DemoDancers dancers; + static BodyHandle[,] CreateDressBodyGrid(Vector3 position, int widthInNodes, float spacing, float bodyRadius, float massPerBody, + int instanceId, Simulation simulation, CollidableProperty filters) + { + var description = BodyDescription.CreateDynamic(QuaternionEx.Identity, new BodyInertia { InverseMass = 1f / massPerBody }, simulation.Shapes.Add(new Sphere(bodyRadius)), 0.01f); + BodyHandle[,] handles = new BodyHandle[widthInNodes, widthInNodes]; + var armHoleCenter = new Vector2(DemoDancers.ArmOffsetX + 0.065f, 0); + var armHoleRadius = 0.095f; + var armHoleRadiusSquared = armHoleRadius * armHoleRadius; + var halfWidth = widthInNodes * spacing / 2; + var halfWidthSquared = halfWidth * halfWidth; + var halfWidthOffset = new Vector2(halfWidth); + for (int rowIndex = 0; rowIndex < widthInNodes; ++rowIndex) + { + for (int columnIndex = 0; columnIndex < widthInNodes; ++columnIndex) + { + var horizontalPosition = new Vector2(columnIndex, rowIndex) * spacing - halfWidthOffset; + var distanceSquared0 = Vector2.DistanceSquared(horizontalPosition, armHoleCenter); + var distanceSquared1 = Vector2.DistanceSquared(horizontalPosition, -armHoleCenter); + var centerDistanceSquared = horizontalPosition.LengthSquared(); + if (distanceSquared0 < armHoleRadiusSquared || distanceSquared1 < armHoleRadiusSquared || centerDistanceSquared > halfWidthSquared) + { + //Too close to an arm or too far from the center, don't create any bodies here. + handles[rowIndex, columnIndex] = new BodyHandle { Value = -1 }; + } + else + { + description.Pose.Position = new Vector3(horizontalPosition.X, 0, horizontalPosition.Y) + position; + var handle = simulation.Bodies.Add(description); + handles[rowIndex, columnIndex] = handle; + if (filters != null) + filters.Allocate(handle) = new ClothCollisionFilter(rowIndex, columnIndex, instanceId); + } + } + } + return handles; + } + static void CreateDistanceConstraints(BodyHandle[,] bodyHandles, SpringSettings springSettings, Simulation simulation) + { + void CreateConstraintBetweenBodies(BodyHandle aHandle, BodyHandle bHandle) + { + //Only create a constraint if bodies on both sides of the pair actually exist. + //In this demo, we use -1 in the body handle slot to represent 'no body'. + if (aHandle.Value >= 0 && bHandle.Value >= 0) + { + var a = simulation.Bodies[aHandle]; + var b = simulation.Bodies[bHandle]; + //Note the use of a limit; the distance is allowed to go smaller. + //This helps stop the cloth from having unnatural rigidity. + var distance = Vector3.Distance(a.Pose.Position, b.Pose.Position); + simulation.Solver.Add(aHandle, bHandle, new CenterDistanceLimit(distance * 0.15f, distance, springSettings)); + } + } + for (int rowIndex = 0; rowIndex < bodyHandles.GetLength(0); ++rowIndex) + { + for (int columnIndex = 0; columnIndex < bodyHandles.GetLength(1) - 1; ++columnIndex) + { + CreateConstraintBetweenBodies(bodyHandles[rowIndex, columnIndex], bodyHandles[rowIndex, columnIndex + 1]); + } + } + for (int rowIndex = 0; rowIndex < bodyHandles.GetLength(0) - 1; ++rowIndex) + { + for (int columnIndex = 0; columnIndex < bodyHandles.GetLength(1); ++columnIndex) + { + CreateConstraintBetweenBodies(bodyHandles[rowIndex, columnIndex], bodyHandles[rowIndex + 1, columnIndex]); + } + } + for (int rowIndex = 0; rowIndex < bodyHandles.GetLength(0) - 1; ++rowIndex) + { + for (int columnIndex = 0; columnIndex < bodyHandles.GetLength(1) - 1; ++columnIndex) + { + CreateConstraintBetweenBodies(bodyHandles[rowIndex, columnIndex], bodyHandles[rowIndex + 1, columnIndex + 1]); + CreateConstraintBetweenBodies(bodyHandles[rowIndex, columnIndex + 1], bodyHandles[rowIndex + 1, columnIndex]); + } + } + } + + + static void TailorDress(Simulation simulation, CollidableProperty filters, DancerBodyHandles bodyHandles, int dancerIndex, int dancerGridWidth, float levelOfDetail) + { + //The demo uses lower resolution grids on dancers further away from the main dancer. + //This is a sorta-example of level of detail. In a 'real' use case, you'd probably want to transition between levels of detail dynamically as the camera moved around. + //That's a little trickier, but doable. Going low to high, for example, requires creating bodies at interpolated positions between existing bodies, while going to a lower level of detail removes them. + levelOfDetail = MathF.Max(0f, MathF.Min(1.5f, levelOfDetail)); + var targetDressDiameter = 2.6f; + var fullDetailWidthInBodies = 40; + float spacingAtFullDetail = targetDressDiameter / fullDetailWidthInBodies; + float bodyRadius = spacingAtFullDetail / 1.75f; + var scale = MathF.Pow(2, levelOfDetail); + var widthInBodies = (int)MathF.Ceiling(fullDetailWidthInBodies / scale); + var spacing = spacingAtFullDetail * scale; + var chest = simulation.Bodies[bodyHandles.Chest]; + ref var chestShape = ref simulation.Shapes.GetShape(chest.Collidable.Shape.Index); + var topOfChestHeight = chest.Pose.Position.Y + chestShape.Radius + bodyRadius; + var bodies = CreateDressBodyGrid(new Vector3(0, topOfChestHeight, 0) + DemoDancers.GetOffsetForDancer(dancerIndex, dancerGridWidth), widthInBodies, spacing, bodyRadius, 0.01f, dancerIndex, simulation, filters); + //Create constraints that bind the cloth bodies closest to the chest, to the chest. This keeps the dress from sliding around. + //In the higher resolution simulations, the arm holes and cloth bodies can actually handle it with no help, but for lower levels of detail it can be useful. + //Also, it's very common to want to control how cloth sticks to a character. You could extend this approach to, for example, keep cloth near the body at the waist like a belt. + //This demo uses constraints to attach a subset of the cloth bodies to the chest. + //You could also either treat the bodies as kinematic and have them follow the body, or attach any constraints that would have involved the cloth body to the body instead. + //Using constraints gives you more options in configuration- the attachment doesn't have to be perfectly rigid. + //For the purposes of this demo, it's also simpler to just use some more constraints. + var midpoint = (widthInBodies * 0.5f - 0.5f); + var zRange = (chestShape.Radius * 0.65f) / spacing; + var xRange = (chestShape.Radius * 0.5f + chestShape.HalfLength) / spacing; + var minX = (int)MathF.Ceiling(midpoint - xRange); + var maxX = (int)(midpoint + xRange); + var minZ = (int)MathF.Ceiling(midpoint - zRange); + var maxZ = (int)(midpoint + zRange); + for (int z = minZ; z <= maxZ; ++z) + { + for (int x = minX; x <= maxX; ++x) + { + var clothNodeHandle = bodies[z, x]; + //When creating bodies, we set handles for bodies that don't exist to -1. + if (clothNodeHandle.Value >= 0) + { + var clothNodeBody = simulation.Bodies[clothNodeHandle]; + simulation.Solver.Add(chest.Handle, clothNodeBody.Handle, + new BallSocket + { + LocalOffsetA = QuaternionEx.Transform(clothNodeBody.Pose.Position - chest.Pose.Position, Quaternion.Conjugate(chest.Pose.Orientation)), + SpringSettings = new SpringSettings(30, 1) + }); + } + } + } + CreateDistanceConstraints(bodies, new SpringSettings(60, 1), simulation); + } + + + public unsafe override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(0, 2, 10); + camera.Yaw = 0; + camera.Pitch = 0; + + var collisionFilters = new CollidableProperty(); + Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks(collisionFilters), new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)), new SolveDescription(8, 1)); + + dancers = new DemoDancers().Initialize(40, 40, Simulation, collisionFilters, ThreadDispatcher, BufferPool, new SolveDescription(1, 4), TailorDress, new ClothCollisionFilter(0, 0, -1)); + + } + public unsafe override void Update(Window window, Camera camera, Input input, float dt) + { + dancers.UpdateTargets(Simulation); + base.Update(window, camera, input, dt); + } + + + public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + { + renderer.Shapes.AddInstances(dancers.Simulations, ThreadDispatcher); + renderer.Lines.Extract(dancers.Simulations, ThreadDispatcher); + + var resolution = renderer.Surface.Resolution; + //renderer.TextBatcher.Write(text.Clear().Append("Cosmetic simulations, like cloth, often don't need to be in a game's main simulation."), new Vector2(16, resolution.Y - 144), 16, Vector3.One, font); + //renderer.TextBatcher.Write(text.Clear().Append("Every background dancer in this demo has its own simulation. All dancers can be easily updated in parallel."), new Vector2(16, resolution.Y - 128), 16, Vector3.One, font); + //renderer.TextBatcher.Write(text.Clear().Append("Dancers further from the main dancer use sparser cloth and disable self collision for extra performance."), new Vector2(16, resolution.Y - 112), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Dancer count: ").Append(dancers.Handles.Length), new Vector2(16, resolution.Y - 80), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Total cloth body count: ").Append(dancers.BodyCount), new Vector2(16, resolution.Y - 64), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Total cloth constraint count: ").Append(dancers.ConstraintCount), new Vector2(16, resolution.Y - 48), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Total dancer execution time (ms): ").Append(dancers.ExecutionTime * 1000, 2), new Vector2(16, resolution.Y - 32), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Amortized execution time per dancer (us): ").Append(dancers.ExecutionTime * 1e6 / dancers.Handles.Length, 1), new Vector2(16, resolution.Y - 16), 16, Vector3.One, font); + + base.Render(renderer, camera, input, text, font); + } + + protected override void OnDispose() + { + dancers.Dispose(BufferPool); + + } + } +} diff --git a/Demos/SpecializedTests/Media/2.4/VideoPlumpDancerDemo.cs b/Demos/SpecializedTests/Media/2.4/VideoPlumpDancerDemo.cs new file mode 100644 index 000000000..7ac05ff5b --- /dev/null +++ b/Demos/SpecializedTests/Media/2.4/VideoPlumpDancerDemo.cs @@ -0,0 +1,257 @@ +using BepuPhysics; +using BepuPhysics.Collidables; +using BepuPhysics.CollisionDetection; +using BepuPhysics.Constraints; +using BepuUtilities; +using BepuUtilities.Memory; +using DemoContentLoader; +using DemoRenderer; +using DemoRenderer.UI; +using Demos.Demos; +using Demos.Demos.Dancers; +using DemoUtilities; +using System; +using System.Diagnostics; +using System.Numerics; +using System.Runtime.InteropServices; + +namespace Demos.SpecializedTests.Media +{ + /// + /// A bunch of somewhat overweight background dancers struggle to keep up with the masterful purple prancer. + /// Combined with the implementation, this shows an example of how cosmetic deformable physics could be applied to characters. + /// + public class VideoPlumpDancerDemo : Demo + { + //This demo relies on the DemoDancers to manage all the ragdolls and their simulations. + //All this demo needs to do is make the fatsuits. + DemoDancers dancers; + + //While creating the fatsuits, we'll precompute some stuff to make testing a little quicker. + //Could make this significantly faster still by being trickier with vectorization, but the demo launch time is already reasonable. + struct TestCapsule + { + public Vector3 Start; + public Vector3 Direction; + public float Length; + public float Radius; + } + static TestCapsule CreateTestCapsule(Simulation simulation, BodyHandle handle) + { + var body = simulation.Bodies[handle]; + Debug.Assert(body.Collidable.Shape.Type == Capsule.Id, "For the purposes of this demo, we assume that all of the bodies that are being tested are capsules."); + ref var shape = ref simulation.Shapes.GetShape(body.Collidable.Shape.Index); + var pose = body.Pose; + TestCapsule toReturn; + QuaternionEx.TransformUnitY(pose.Orientation, out toReturn.Direction); + toReturn.Start = pose.Position - toReturn.Direction * shape.HalfLength; + toReturn.Radius = shape.Radius; + toReturn.Length = shape.HalfLength * 2; + return toReturn; + + } + unsafe static void CreateBodyGrid(DancerBodyHandles bodyHandles, Int3 axisSizeInBodies, Vector3 gridMinimum, Vector3 gridMaximum, float bodyRadius, float massPerBody, + int instanceId, Simulation simulation, CollidableProperty filters) + { + var shape = new Sphere(bodyRadius); + var shapeIndex = simulation.Shapes.Add(shape); + //Note that, unlike the DancerDemo where cloth nodes cannot rotate, the deformable sub-bodies can rotate. + //That's because this demo is going to connect bodies together using Weld constraints, which control all six degrees of freedom. + //You could also use a CenterDistanceConstraint/Limit with VolumeConstraints to maintain shape, but a bunch of Weld constraints is a little simpler. + var description = BodyDescription.CreateDynamic(QuaternionEx.Identity, shape.ComputeInertia(massPerBody), shapeIndex, 0.01f); + BodyHandle[,,] handles = new BodyHandle[axisSizeInBodies.X, axisSizeInBodies.Y, axisSizeInBodies.Z]; + BodyHandle[,,] nearestHandles = new BodyHandle[axisSizeInBodies.X, axisSizeInBodies.Y, axisSizeInBodies.Z]; + var gridSpan = gridMaximum - gridMinimum; + var gridSpacing = gridSpan / new Vector3(axisSizeInBodies.X - 1, axisSizeInBodies.Y - 1, axisSizeInBodies.Z - 1); + Span testCapsules = stackalloc TestCapsule[11]; + + //DancerBodyHandles stores the head last, so we can just check the first 11 bodies that are all capsules. The head isn't going to be covered in the fatsuit, so it doesn't need to be checked anyway. + var handlesBuffer = DancerBodyHandles.AsBuffer(&bodyHandles); + for (int i = 0; i < 11; ++i) + { + testCapsules[i] = CreateTestCapsule(simulation, handlesBuffer[i]); + } + var center = (gridMinimum + gridMaximum) * 0.5f; + + for (int x = 0; x < axisSizeInBodies.X; ++x) + { + for (int y = 0; y < axisSizeInBodies.Y; ++y) + { + for (int z = 0; z < axisSizeInBodies.Z; ++z) + { + var position = gridMinimum + gridSpacing * new Vector3(x, y, z); + float minimumDistance = float.MaxValue; + int minimumIndex = 0; + for (int i = 0; i < testCapsules.Length; ++i) + { + var testCapsule = testCapsules[i]; + var distance = Vector3.Distance(position, testCapsule.Start + MathF.Max(0, MathF.Min(testCapsule.Length, Vector3.Dot(position - testCapsule.Start, testCapsule.Direction))) * testCapsule.Direction) - testCapsule.Radius; + if (distance < minimumDistance) + { + minimumDistance = distance; + minimumIndex = i; + } + } + nearestHandles[x, y, z] = handlesBuffer[minimumIndex]; + + var maximumDistanceForCreatingNodes = MathF.Max(0.1f, 0.8f - 1.5f * Vector3.Distance(position, center)); + if (minimumDistance < bodyRadius) + { + //Intersecting; don't create a body. -2 for this demo marks the body as intersecting, so we can disambiguate it from slots that are just empty due to being too far away. + handles[x, y, z] = new BodyHandle { Value = -2 }; + } + else if (minimumDistance > maximumDistanceForCreatingNodes) + { + //-1 means too far. + handles[x, y, z] = new BodyHandle { Value = -1 }; + } + else + { + //Nearby. Create and attach it to the nearest body part. + description.Pose.Position = position; + var handle = simulation.Bodies.Add(description); + handles[x, y, z] = handle; + if (filters != null) + filters.Allocate(handle) = new DeformableCollisionFilter(x, y, z, instanceId); + + var nearestHandle = handlesBuffer[minimumIndex]; + var nearestPose = simulation.Bodies[nearestHandle].Pose; + var conjugate = Quaternion.Conjugate(nearestPose.Orientation); + + } + } + } + } + for (int x = 0; x < axisSizeInBodies.X; ++x) + { + for (int y = 0; y < axisSizeInBodies.Y; ++y) + { + for (int z = 0; z < axisSizeInBodies.Z; ++z) + { + //Kind of hacky, but simple: for every node that is exposed to the air (a neighbor has a body handle flagged as -1), make sure it has a collidable. + //Anything inside doesn't need a collidable. + var handle = handles[x, y, z]; + if (handle.Value >= 0) + { + var needsAnchor = + (x != 0 && handles[x - 1, y, z].Value == -2) || + (x != handles.GetLength(0) - 1 && handles[x + 1, y, z].Value == -2) || + (y != 0 && handles[x, y - 1, z].Value == -2) || + (y != handles.GetLength(1) - 1 && handles[x, y + 1, z].Value == -2) || + (z != 0 && handles[x, y, z - 1].Value == -2) || + (z != handles.GetLength(2) - 1 && handles[x, y, z + 1].Value == -2); + var source = simulation.Bodies[handle]; + if (needsAnchor) + { + var nearestHandle = nearestHandles[x, y, z]; + var nearestPose = simulation.Bodies[nearestHandle].Pose; + var conjugate = Quaternion.Conjugate(nearestPose.Orientation); + simulation.Solver.Add(nearestHandle, handle, new Weld + { + LocalOffset = QuaternionEx.Transform(source.Pose.Position - nearestPose.Position, conjugate), + LocalOrientation = conjugate, + SpringSettings = new SpringSettings(6, 0.4f) + }); + } + var needsCollidable = + (x == 0 || handles[x - 1, y, z].Value == -1) || (x == handles.GetLength(0) - 1 || handles[x + 1, y, z].Value == -1) || + (y == 0 || handles[x, y - 1, z].Value == -1) || (y == handles.GetLength(1) - 1 || handles[x, y + 1, z].Value == -1) || + (z == 0 || handles[x, y, z - 1].Value == -1) || (z == handles.GetLength(2) - 1 || handles[x, y, z + 1].Value == -1); + if (!needsCollidable) + { + source.SetShape(default); + } + + static void TryAdd(Simulation simulation, BodyReference source, BodyHandle targetHandle) + { + if (targetHandle.Value >= 0) + { + var target = simulation.Bodies[targetHandle]; + simulation.Solver.Add(source.Handle, targetHandle, new Weld { LocalOffset = target.Pose.Position - source.Pose.Position, LocalOrientation = Quaternion.Identity, SpringSettings = new SpringSettings(6, 0.4f) }); + } + } + if (x < handles.GetLength(0) - 1) + { + TryAdd(simulation, source, handles[x + 1, y, z]); + } + if (y < handles.GetLength(1) - 1) + { + TryAdd(simulation, source, handles[x, y + 1, z]); + } + if (z < handles.GetLength(2) - 1) + { + TryAdd(simulation, source, handles[x, y, z + 1]); + } + } + } + } + } + + } + + + static void CreateFatSuit(Simulation simulation, CollidableProperty filters, DancerBodyHandles bodyHandles, int dancerIndex, int dancerGridWidth, float levelOfDetail) + { + //The demo uses lower resolution grids on dancers further away from the main dancer. + //This is a sorta-example of level of detail. In a 'real' use case, you'd probably want to transition between levels of detail dynamically as the camera moved around. + //That's a little trickier, but doable. Going low to high, for example, requires creating bodies at interpolated positions between existing bodies, while going to a lower level of detail removes them. + levelOfDetail = MathF.Max(0f, MathF.Min(0.8f, levelOfDetail)); + var suitSize = new Vector3(1, 1f, 1); + var fullDetailAxisBodyCounts = new Int3 { X = 23, Y = 23, Z = 23 }; + var scale = MathF.Pow(2, levelOfDetail); + var axisBodyCounts = new Int3 { X = (int)MathF.Ceiling(fullDetailAxisBodyCounts.X / scale), Y = (int)MathF.Ceiling(fullDetailAxisBodyCounts.Y / scale), Z = (int)MathF.Ceiling(fullDetailAxisBodyCounts.Z / scale) }; + var bodyRadius = MathF.Min(suitSize.X / axisBodyCounts.X, MathF.Min(suitSize.Y / axisBodyCounts.Y, suitSize.Z / axisBodyCounts.Z)); + + var chest = simulation.Bodies[bodyHandles.Chest]; + ref var chestShape = ref simulation.Shapes.GetShape(chest.Collidable.Shape.Index); + var topOfChestHeight = chest.Pose.Position.Y + chestShape.Radius; + var topOfChestPosition = new Vector3(0, topOfChestHeight, 0) + DemoDancers.GetOffsetForDancer(dancerIndex, dancerGridWidth); + var suitMinimum = topOfChestPosition - suitSize * new Vector3(0.5f, 1f, 0.5f); + var suitMaximum = suitMinimum + suitSize; + CreateBodyGrid(bodyHandles, axisBodyCounts, suitMinimum, suitMaximum, bodyRadius, 0.01f, dancerIndex, simulation, filters); + } + + + public unsafe override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(0, 2, 10); + camera.Yaw = 0; + camera.Pitch = 0; + + var collisionFilters = new CollidableProperty(); + Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks(collisionFilters), new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)), new SolveDescription(8, 1)); + + //Note that, because the constraints in the fat suit are quite soft, we can get away with extremely minimal solving time. There's one substep with one velocity iteration. + dancers = new DemoDancers().Initialize(32, 32, Simulation, collisionFilters, ThreadDispatcher, BufferPool, new SolveDescription(1, 1), CreateFatSuit, new DeformableCollisionFilter(0, 0, 0, -1)); + + } + public unsafe override void Update(Window window, Camera camera, Input input, float dt) + { + dancers.UpdateTargets(Simulation); + base.Update(window, camera, input, dt); + } + + public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + { + renderer.Shapes.AddInstances(dancers.Simulations, ThreadDispatcher); + renderer.Lines.Extract(dancers.Simulations, ThreadDispatcher); + + var resolution = renderer.Surface.Resolution; + //renderer.TextBatcher.Write(text.Clear().Append("Cosmetic simulations, like character blubber, often don't need to be in a game's main simulation."), new Vector2(16, resolution.Y - 144), 16, Vector3.One, font); + //renderer.TextBatcher.Write(text.Clear().Append("Every background dancer in this demo has its own simulation. All dancers can be easily updated in parallel."), new Vector2(16, resolution.Y - 128), 16, Vector3.One, font); + //renderer.TextBatcher.Write(text.Clear().Append("Dancers further from the main dancer use sparser body grids and disable self collision for extra performance."), new Vector2(16, resolution.Y - 112), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Dancer count: ").Append(dancers.Handles.Length), new Vector2(16, resolution.Y - 80), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Total deformable body count: ").Append(dancers.BodyCount), new Vector2(16, resolution.Y - 64), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Total deformable constraint count: ").Append(dancers.ConstraintCount), new Vector2(16, resolution.Y - 48), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Total dancer execution time (ms): ").Append(dancers.ExecutionTime * 1000, 2), new Vector2(16, resolution.Y - 32), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Amortized execution time per dancer (us): ").Append(dancers.ExecutionTime * 1e6 / dancers.Handles.Length, 1), new Vector2(16, resolution.Y - 16), 16, Vector3.One, font); + + base.Render(renderer, camera, input, text, font); + } + protected override void OnDispose() + { + dancers.Dispose(BufferPool); + + } + } +} From 136bcf34251c0a9b571f7b28ed28d9254420f093 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 9 Feb 2022 16:14:31 -0600 Subject: [PATCH 444/947] Allocation oopsies. --- BepuPhysics/CollisionDetection/BroadPhase.cs | 8 ++++++-- DemoRenderer/Constraints/LineExtractor.cs | 7 +++++-- DemoRenderer/ShapeDrawing/ShapesExtractor.cs | 8 ++++++-- Demos/DemoHarness.cs | 1 - Demos/Demos/Cars/CarDemo.cs | 14 +++++++------- Demos/Demos/Characters/CharacterInput.cs | 12 ++++++------ Demos/Demos/Tanks/TankDemo.cs | 16 ++++++++-------- .../SpecializedTests/Media/2.4/TankSwarmDemo.cs | 16 ++++++++-------- 8 files changed, 46 insertions(+), 36 deletions(-) diff --git a/BepuPhysics/CollisionDetection/BroadPhase.cs b/BepuPhysics/CollisionDetection/BroadPhase.cs index 73794b982..7b585a6d4 100644 --- a/BepuPhysics/CollisionDetection/BroadPhase.cs +++ b/BepuPhysics/CollisionDetection/BroadPhase.cs @@ -22,6 +22,8 @@ public unsafe partial class BroadPhase : IDisposable //TODO: static trees do not need to do nearly as much work as the active; this will change in the future. Tree.RefitAndRefineMultithreadedContext staticRefineContext; + Action executeRefitAndMarkAction, executeRefineAction; + public BroadPhase(BufferPool pool, int initialActiveLeafCapacity = 4096, int initialStaticLeafCapacity = 8192) { Pool = pool; @@ -32,6 +34,8 @@ public BroadPhase(BufferPool pool, int initialActiveLeafCapacity = 4096, int ini activeRefineContext = new Tree.RefitAndRefineMultithreadedContext(); staticRefineContext = new Tree.RefitAndRefineMultithreadedContext(); + executeRefitAndMarkAction = ExecuteRefitAndMark; + executeRefineAction = ExecuteRefine; } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -190,7 +194,7 @@ public void Update(IThreadDispatcher threadDispatcher = null) activeRefineContext.CreateRefitAndMarkJobs(ref ActiveTree, Pool, threadDispatcher); staticRefineContext.CreateRefitAndMarkJobs(ref StaticTree, Pool, threadDispatcher); remainingJobCount = activeRefineContext.RefitNodes.Count + staticRefineContext.RefitNodes.Count; - threadDispatcher.DispatchWorkers(ExecuteRefitAndMark, remainingJobCount); + threadDispatcher.DispatchWorkers(executeRefitAndMarkAction, remainingJobCount); activeRefineContext.CreateRefinementJobs(Pool, frameIndex, 1f); //TODO: for now, the inactive/static tree is simply updated like another active tree. This is enormously inefficient compared to the ideal- //by nature, static and inactive objects do not move every frame! @@ -199,7 +203,7 @@ public void Update(IThreadDispatcher threadDispatcher = null) //Since the jobs are large, reducing the refinement aggressiveness doesn't change much here. staticRefineContext.CreateRefinementJobs(Pool, frameIndex, 1f); remainingJobCount = activeRefineContext.RefinementTargets.Count + staticRefineContext.RefinementTargets.Count; - threadDispatcher.DispatchWorkers(ExecuteRefine, remainingJobCount); + threadDispatcher.DispatchWorkers(executeRefineAction, remainingJobCount); activeRefineContext.CleanUpForRefitAndRefine(Pool); staticRefineContext.CleanUpForRefitAndRefine(Pool); this.threadDispatcher = null; diff --git a/DemoRenderer/Constraints/LineExtractor.cs b/DemoRenderer/Constraints/LineExtractor.cs index 845cf1c2b..8a5686b2b 100644 --- a/DemoRenderer/Constraints/LineExtractor.cs +++ b/DemoRenderer/Constraints/LineExtractor.cs @@ -15,6 +15,7 @@ public class LineExtractor : IDisposable BufferPool pool; ParallelLooper looper; + LooperAction executeJobAction; public bool ShowConstraints = true; public bool ShowContacts; @@ -27,6 +28,8 @@ public LineExtractor(BufferPool pool, ParallelLooper looper, int initialLineCapa boundingBoxes = new BoundingBoxLineExtractor(); this.pool = pool; this.looper = looper; + executeJobAction = ExecuteJob; + } Simulation[] simulations; @@ -72,7 +75,7 @@ public void Extract(Simulation[] simulations, IThreadDispatcher threadDispatcher } this.simulations = simulations; looper.Dispatcher = threadDispatcher; - looper.For(0, constraintJobs.Count + boundingBoxJobs.Count, ExecuteJob); + looper.For(0, constraintJobs.Count + boundingBoxJobs.Count, executeJobAction); looper.Dispatcher = null; if (constraintJobs.Span.Allocated) @@ -103,7 +106,7 @@ public void Extract(Simulation simulation, IThreadDispatcher threadDispatcher = boundingBoxes.CreateJobs(simulation, 0, ref lines, ref boundingBoxJobs, pool); } looper.Dispatcher = threadDispatcher; - looper.For(0, constraintJobs.Count + boundingBoxJobs.Count, ExecuteJob); + looper.For(0, constraintJobs.Count + boundingBoxJobs.Count, executeJobAction); looper.Dispatcher = null; if (constraintJobs.Span.Allocated) diff --git a/DemoRenderer/ShapeDrawing/ShapesExtractor.cs b/DemoRenderer/ShapeDrawing/ShapesExtractor.cs index 25410b8eb..59488eb86 100644 --- a/DemoRenderer/ShapeDrawing/ShapesExtractor.cs +++ b/DemoRenderer/ShapeDrawing/ShapesExtractor.cs @@ -57,12 +57,16 @@ public class ShapesExtractor : IDisposable public MeshCache MeshCache; ParallelLooper looper; + LooperAction addShapesForJobAction; + LooperWorkerDone workerDoneAction; public ShapesExtractor(Device device, ParallelLooper looper, BufferPool pool, int initialCapacityPerShapeType = 1024) { ShapeCache = new ShapeCache(initialCapacityPerShapeType, pool); this.MeshCache = new MeshCache(device, pool); this.pool = pool; this.looper = looper; + addShapesForJobAction = AddShapesForJob; + workerDoneAction = WorkerDone; } public void ClearInstances() @@ -497,7 +501,7 @@ public void AddInstances(Simulation[] simulations, IThreadDispatcher threadDispa { CreateJobs(simulations[simulationIndex], simulationIndex, ref jobs, pool); } - looper.For(0, jobs.Count, AddShapesForJob, WorkerDone); + looper.For(0, jobs.Count, addShapesForJobAction, workerDoneAction); EndMultithreadedExecution(); this.simulations = default; } @@ -517,7 +521,7 @@ public void AddInstances(Simulation simulation, IThreadDispatcher threadDispatch this.simulation = simulation; PrepareForMultithreadedExecution(threadDispatcher); CreateJobs(simulation, 0, ref jobs, pool); - looper.For(0, jobs.Count, AddShapesForJob, WorkerDone); + looper.For(0, jobs.Count, addShapesForJobAction, workerDoneAction); EndMultithreadedExecution(); this.simulation = null; } diff --git a/Demos/DemoHarness.cs b/Demos/DemoHarness.cs index 1b2a88e97..dcf742052 100644 --- a/Demos/DemoHarness.cs +++ b/Demos/DemoHarness.cs @@ -361,7 +361,6 @@ void WriteHoldableName(string controlName, HoldableBind control) controlPosition.Y += lineSpacing; } - //Conveniently, enum strings are cached. Every (Key).ToString() returns the same reference for the same key, so no garbage worries. WriteInstantName(nameof(controls.LockMouse), controls.LockMouse); WriteHoldableName(nameof(controls.Grab), controls.Grab); WriteHoldableName(nameof(controls.GrabRotate), controls.GrabRotate); diff --git a/Demos/Demos/Cars/CarDemo.cs b/Demos/Demos/Cars/CarDemo.cs index bb94ef684..82ceb1985 100644 --- a/Demos/Demos/Cars/CarDemo.cs +++ b/Demos/Demos/Cars/CarDemo.cs @@ -210,13 +210,13 @@ public override void Render(Renderer renderer, Camera camera, Input input, TextB var textHeight = 16; var position = new Vector2(32, renderer.Surface.Resolution.Y - 128); - RenderControl(ref position, textHeight, nameof(Forward), Forward.ToString(), text, renderer.TextBatcher, font); - RenderControl(ref position, textHeight, nameof(Backward), Backward.ToString(), text, renderer.TextBatcher, font); - RenderControl(ref position, textHeight, nameof(Right), Right.ToString(), text, renderer.TextBatcher, font); - RenderControl(ref position, textHeight, nameof(Left), Left.ToString(), text, renderer.TextBatcher, font); - RenderControl(ref position, textHeight, nameof(Zoom), Zoom.ToString(), text, renderer.TextBatcher, font); - RenderControl(ref position, textHeight, nameof(Brake), Brake.ToString(), text, renderer.TextBatcher, font); - RenderControl(ref position, textHeight, nameof(ToggleCar), ToggleCar.ToString(), text, renderer.TextBatcher, font); + RenderControl(ref position, textHeight, nameof(Forward), ControlStrings.GetName(Forward), text, renderer.TextBatcher, font); + RenderControl(ref position, textHeight, nameof(Backward), ControlStrings.GetName(Backward), text, renderer.TextBatcher, font); + RenderControl(ref position, textHeight, nameof(Right), ControlStrings.GetName(Right), text, renderer.TextBatcher, font); + RenderControl(ref position, textHeight, nameof(Left), ControlStrings.GetName(Left), text, renderer.TextBatcher, font); + RenderControl(ref position, textHeight, nameof(Zoom), ControlStrings.GetName(Zoom), text, renderer.TextBatcher, font); + RenderControl(ref position, textHeight, nameof(Brake), ControlStrings.GetName(Brake), text, renderer.TextBatcher, font); + RenderControl(ref position, textHeight, nameof(ToggleCar), ControlStrings.GetName(ToggleCar), text, renderer.TextBatcher, font); base.Render(renderer, camera, input, text, font); } } diff --git a/Demos/Demos/Characters/CharacterInput.cs b/Demos/Demos/Characters/CharacterInput.cs index b844dd3ae..f3819083b 100644 --- a/Demos/Demos/Characters/CharacterInput.cs +++ b/Demos/Demos/Characters/CharacterInput.cs @@ -157,12 +157,12 @@ void RenderControl(ref Vector2 position, float textHeight, string controlName, s } public void RenderControls(Vector2 position, float textHeight, TextBatcher textBatcher, TextBuilder text, Font font) { - RenderControl(ref position, textHeight, nameof(MoveForward), MoveForward.ToString(), text, textBatcher, font); - RenderControl(ref position, textHeight, nameof(MoveBackward), MoveBackward.ToString(), text, textBatcher, font); - RenderControl(ref position, textHeight, nameof(MoveRight), MoveRight.ToString(), text, textBatcher, font); - RenderControl(ref position, textHeight, nameof(MoveLeft), MoveLeft.ToString(), text, textBatcher, font); - RenderControl(ref position, textHeight, nameof(Sprint), Sprint.ToString(), text, textBatcher, font); - RenderControl(ref position, textHeight, nameof(Jump), Jump.ToString(), text, textBatcher, font); + RenderControl(ref position, textHeight, nameof(MoveForward), ControlStrings.GetName(MoveForward), text, textBatcher, font); + RenderControl(ref position, textHeight, nameof(MoveBackward), ControlStrings.GetName(MoveBackward), text, textBatcher, font); + RenderControl(ref position, textHeight, nameof(MoveRight), ControlStrings.GetName(MoveRight), text, textBatcher, font); + RenderControl(ref position, textHeight, nameof(MoveLeft), ControlStrings.GetName(MoveLeft), text, textBatcher, font); + RenderControl(ref position, textHeight, nameof(Sprint), ControlStrings.GetName(Sprint), text, textBatcher, font); + RenderControl(ref position, textHeight, nameof(Jump), ControlStrings.GetName(Jump), text, textBatcher, font); } diff --git a/Demos/Demos/Tanks/TankDemo.cs b/Demos/Demos/Tanks/TankDemo.cs index 515f0b527..469ad668b 100644 --- a/Demos/Demos/Tanks/TankDemo.cs +++ b/Demos/Demos/Tanks/TankDemo.cs @@ -344,14 +344,14 @@ public override void Render(Renderer renderer, Camera camera, Input input, TextB var textHeight = 16; var position = new Vector2(32, renderer.Surface.Resolution.Y - 144); - RenderControl(ref position, textHeight, nameof(Fire), Fire.ToString(), text, renderer.TextBatcher, font); - RenderControl(ref position, textHeight, nameof(Forward), Forward.ToString(), text, renderer.TextBatcher, font); - RenderControl(ref position, textHeight, nameof(Backward), Backward.ToString(), text, renderer.TextBatcher, font); - RenderControl(ref position, textHeight, nameof(Right), Right.ToString(), text, renderer.TextBatcher, font); - RenderControl(ref position, textHeight, nameof(Left), Left.ToString(), text, renderer.TextBatcher, font); - RenderControl(ref position, textHeight, nameof(Zoom), Zoom.ToString(), text, renderer.TextBatcher, font); - RenderControl(ref position, textHeight, nameof(Brake), Brake.ToString(), text, renderer.TextBatcher, font); - RenderControl(ref position, textHeight, nameof(ToggleTank), ToggleTank.ToString(), text, renderer.TextBatcher, font); + RenderControl(ref position, textHeight, nameof(Fire), ControlStrings.GetName(Fire), text, renderer.TextBatcher, font); + RenderControl(ref position, textHeight, nameof(Forward), ControlStrings.GetName(Forward), text, renderer.TextBatcher, font); + RenderControl(ref position, textHeight, nameof(Backward), ControlStrings.GetName(Backward), text, renderer.TextBatcher, font); + RenderControl(ref position, textHeight, nameof(Right), ControlStrings.GetName(Right), text, renderer.TextBatcher, font); + RenderControl(ref position, textHeight, nameof(Left), ControlStrings.GetName(Left), text, renderer.TextBatcher, font); + RenderControl(ref position, textHeight, nameof(Zoom), ControlStrings.GetName(Zoom), text, renderer.TextBatcher, font); + RenderControl(ref position, textHeight, nameof(Brake), ControlStrings.GetName(Brake), text, renderer.TextBatcher, font); + RenderControl(ref position, textHeight, nameof(ToggleTank), ControlStrings.GetName(ToggleTank), text, renderer.TextBatcher, font); if (aiTanks.Count > 0) renderer.TextBatcher.Write(text.Clear().Append("Enemy tanks remaining: ").Append(aiTanks.Count), new Vector2(32, renderer.Surface.Resolution.Y - 172), 24, new Vector3(1, 1, 1), font); diff --git a/Demos/SpecializedTests/Media/2.4/TankSwarmDemo.cs b/Demos/SpecializedTests/Media/2.4/TankSwarmDemo.cs index a34460b0c..0476c9b7b 100644 --- a/Demos/SpecializedTests/Media/2.4/TankSwarmDemo.cs +++ b/Demos/SpecializedTests/Media/2.4/TankSwarmDemo.cs @@ -346,14 +346,14 @@ public override void Render(Renderer renderer, Camera camera, Input input, TextB var textHeight = 16; var position = new Vector2(32, renderer.Surface.Resolution.Y - 144); - RenderControl(ref position, textHeight, nameof(Fire), Fire.ToString(), text, renderer.TextBatcher, font); - RenderControl(ref position, textHeight, nameof(Forward), Forward.ToString(), text, renderer.TextBatcher, font); - RenderControl(ref position, textHeight, nameof(Backward), Backward.ToString(), text, renderer.TextBatcher, font); - RenderControl(ref position, textHeight, nameof(Right), Right.ToString(), text, renderer.TextBatcher, font); - RenderControl(ref position, textHeight, nameof(Left), Left.ToString(), text, renderer.TextBatcher, font); - RenderControl(ref position, textHeight, nameof(Zoom), Zoom.ToString(), text, renderer.TextBatcher, font); - RenderControl(ref position, textHeight, nameof(Brake), Brake.ToString(), text, renderer.TextBatcher, font); - RenderControl(ref position, textHeight, nameof(ToggleTank), ToggleTank.ToString(), text, renderer.TextBatcher, font); + RenderControl(ref position, textHeight, nameof(Fire), ControlStrings.GetName(Fire), text, renderer.TextBatcher, font); + RenderControl(ref position, textHeight, nameof(Forward), ControlStrings.GetName(Forward), text, renderer.TextBatcher, font); + RenderControl(ref position, textHeight, nameof(Backward), ControlStrings.GetName(Backward), text, renderer.TextBatcher, font); + RenderControl(ref position, textHeight, nameof(Right), ControlStrings.GetName(Right), text, renderer.TextBatcher, font); + RenderControl(ref position, textHeight, nameof(Left), ControlStrings.GetName(Left), text, renderer.TextBatcher, font); + RenderControl(ref position, textHeight, nameof(Zoom), ControlStrings.GetName(Zoom), text, renderer.TextBatcher, font); + RenderControl(ref position, textHeight, nameof(Brake), ControlStrings.GetName(Brake), text, renderer.TextBatcher, font); + RenderControl(ref position, textHeight, nameof(ToggleTank), ControlStrings.GetName(ToggleTank), text, renderer.TextBatcher, font); if (aiTanks.Count > 0) renderer.TextBatcher.Write(text.Clear().Append("Enemy tanks remaining: ").Append(aiTanks.Count), new Vector2(32, renderer.Surface.Resolution.Y - 172), 24, new Vector3(1, 1, 1), font); From 9a4bd50090e442d0b847353fc5cc3980d3bafaeb Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 9 Feb 2022 18:06:06 -0600 Subject: [PATCH 445/947] Re-fixed collidable overlap finder MT memory leak. --- .../CollidableOverlapFinder.cs | 30 ++++++++++--------- .../CollisionDetection/CollisionBatcher.cs | 2 +- BepuUtilities/Memory/BufferPool.cs | 3 +- 3 files changed, 19 insertions(+), 16 deletions(-) diff --git a/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs b/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs index bbf53d8f0..4a7fbd761 100644 --- a/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs +++ b/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs @@ -103,9 +103,9 @@ void Worker(int workerIndex) public override void DispatchOverlaps(float dt, IThreadDispatcher threadDispatcher = null) { - narrowPhase.Prepare(dt, threadDispatcher); - if (threadDispatcher != null) + if (threadDispatcher != null && threadDispatcher.ThreadCount > 1) { + narrowPhase.Prepare(dt, threadDispatcher); if (intertreeHandlers == null || intertreeHandlers.Length < threadDispatcher.ThreadCount) { //This initialization/resize should occur extremely rarely. @@ -128,32 +128,34 @@ public override void DispatchOverlaps(float dt, IThreadDispatcher threadDispatch intertreeTestContext.PrepareJobs(ref broadPhase.ActiveTree, ref broadPhase.StaticTree, intertreeHandlers, threadDispatcher.ThreadCount); nextJobIndex = -1; var totalJobCount = selfTestContext.JobCount + intertreeTestContext.JobCount; + threadDispatcher.DispatchWorkers(workerAction, totalJobCount); + //We dispatch over parts of the tree are not yet analyzed, but the job creation phase may have put some work into the batcher. + //If the total job count is zero, that means there's no further work to be done (implying the tree was very tiny), but we may need to flush additional jobs in worker 0. if (totalJobCount == 0) + narrowPhase.overlapWorkers[0].Batcher.Flush(); + //Any workers that we allocated resources for but did not end up using due to a lack of discovered jobs need to be cleaned up. Flushing disposes those resources. + //(this complexity could be removed if the preparation phase was aware of the job count, but that's somewhat more difficult.) + for (int i = Math.Max(1, totalJobCount); i < threadDispatcher.ThreadCount; ++i) { - //We dispatch over parts of the tree are not yet analyzed, but the job creation phase may have put some work into the batcher. - //If the total job count is zero, that means there's no further work to be done (implying the tree was very tiny), but we may need to flush. - //Flushing also disposes of any resources taken by the batcher. - for (int i = 0; i < threadDispatcher.ThreadCount; ++i) - { - narrowPhase.overlapWorkers[i].Batcher.Flush(); - } + narrowPhase.overlapWorkers[i].Batcher.Flush(); } - else +#if DEBUG + for (int i = 1; i < threadDispatcher.ThreadCount; ++i) { - threadDispatcher.DispatchWorkers(workerAction, totalJobCount); + Debug.Assert(!narrowPhase.overlapWorkers[i].Batcher.batches.Allocated, "After execution, there should be no remaining allocated collision batchers."); } - //workerAction(0); +#endif selfTestContext.CompleteSelfTest(); intertreeTestContext.CompleteTest(); } else { + narrowPhase.Prepare(dt); var selfTestHandler = new SelfOverlapHandler(broadPhase.activeLeaves, narrowPhase, 0); broadPhase.ActiveTree.GetSelfOverlaps(ref selfTestHandler); var intertreeHandler = new IntertreeOverlapHandler(broadPhase.activeLeaves, broadPhase.staticLeaves, narrowPhase, 0); broadPhase.ActiveTree.GetOverlaps(ref broadPhase.StaticTree, ref intertreeHandler); - ref var worker = ref narrowPhase.overlapWorkers[0]; - worker.Batcher.Flush(); + narrowPhase.overlapWorkers[0].Batcher.Flush(); } diff --git a/BepuPhysics/CollisionDetection/CollisionBatcher.cs b/BepuPhysics/CollisionDetection/CollisionBatcher.cs index 2892c74d0..87d16dbdf 100644 --- a/BepuPhysics/CollisionDetection/CollisionBatcher.cs +++ b/BepuPhysics/CollisionDetection/CollisionBatcher.cs @@ -46,7 +46,7 @@ public struct CollisionBatcher where TCallbacks : struct, ICollision //The streaming batcher contains batches for pending work submitted by the user. //This pending work can be top level pairs like sphere versus sphere, but it may also be subtasks of submitted work. //Consider two compound bodies colliding. The pair will decompose into a set of potentially many convex subpairs. - Buffer batches; + internal Buffer batches; //These collision tasks can then call upon some of the batcher's fixed function post processing stages. //For example, compound collisions generate multiple convex-convex manifolds which need to be reduced and combined into a single nonconvex manifold for //efficiency in constraint solving. diff --git a/BepuUtilities/Memory/BufferPool.cs b/BepuUtilities/Memory/BufferPool.cs index 0d34243c5..c29bbb038 100644 --- a/BepuUtilities/Memory/BufferPool.cs +++ b/BepuUtilities/Memory/BufferPool.cs @@ -135,7 +135,8 @@ public unsafe void Take(out Buffer buffer) idsForAllocator = new HashSet(); outstandingAllocators.Add(allocator, idsForAllocator); } - Debug.Assert(idsForAllocator.Count < (1 << 25), "Do you actually have that many allocations for this one allocator?"); + const int maximumReasonableOutstandingAllocationsForAllocator = 1 << 25; + Debug.Assert(idsForAllocator.Count < maximumReasonableOutstandingAllocationsForAllocator, "Do you actually have that many allocations for this one allocator?"); idsForAllocator.Add(slot); #endif #endif From a44d6da05c162e39e812844c9783fa3c76853c79 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 10 Feb 2022 14:18:45 -0600 Subject: [PATCH 446/947] ConstraintRemover now uses lock instead of spinlock for removing newly zero constraint count batches. Avoids Sleep(1) cornercase that can appear on extremely high batch churns and high thread counts. --- .../CollisionDetection/ConstraintRemover.cs | 12 ++++++------ BepuPhysics/CollisionDetection/NarrowPhase.cs | 13 ++++++------- .../Media/2.4/ExcessivePyramidVideoDemo.cs | 14 ++------------ 3 files changed, 14 insertions(+), 25 deletions(-) diff --git a/BepuPhysics/CollisionDetection/ConstraintRemover.cs b/BepuPhysics/CollisionDetection/ConstraintRemover.cs index b52980d38..d7cd8e16b 100644 --- a/BepuPhysics/CollisionDetection/ConstraintRemover.cs +++ b/BepuPhysics/CollisionDetection/ConstraintRemover.cs @@ -402,7 +402,7 @@ public void TryRemoveBodyFromConstrainedKinematicsAndRemoveAllConstraintsForBody } QuickList removedTypeBatches; - SpinLock removedTypeBatchLocker = new SpinLock(); + object batchRemovalLocker = new object(); public void RemoveConstraintsFromTypeBatch(int index) { var batch = batches.TypeBatches[index]; @@ -410,7 +410,6 @@ public void RemoveConstraintsFromTypeBatch(int index) ref var typeBatch = ref constraintBatch.TypeBatches[batch.TypeBatch]; var typeProcessor = solver.TypeProcessors[typeBatch.TypeId]; ref var removals = ref batches.RemovalsForTypeBatches[index]; - bool lockTaken = false; for (int i = 0; i < removals.ConstraintHandlesToRemove.Count; ++i) { var handle = removals.ConstraintHandlesToRemove[i]; @@ -423,11 +422,12 @@ public void RemoveConstraintsFromTypeBatch(int index) if (typeBatch.ConstraintCount == 0) { //This batch-typebatch needs to be removed. - //Note that we just use a spinlock here, nothing tricky- the number of typebatch/batch removals should tend to be extremely low (averaging 0), + //Note that we just use a lock here, nothing tricky- the number of typebatch/batch removals should tend to be extremely low (averaging 0), //so it's not worth doing a bunch of per worker accumulators and stuff. - removedTypeBatchLocker.Enter(ref lockTaken); - removedTypeBatches.AddUnsafely(batch); - removedTypeBatchLocker.Exit(); + lock (batchRemovalLocker) + { + removedTypeBatches.AddUnsafely(batch); + } } } } diff --git a/BepuPhysics/CollisionDetection/NarrowPhase.cs b/BepuPhysics/CollisionDetection/NarrowPhase.cs index 4e06ebafb..7a694d53c 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhase.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhase.cs @@ -202,13 +202,12 @@ public void Prepare(float dt, IThreadDispatcher threadDispatcher = null) void FlushWorkerLoop(int workerIndex) { int jobIndex; - var threadPool = threadDispatcher.GetThreadMemoryPool(workerIndex); while ((jobIndex = Interlocked.Increment(ref flushJobIndex)) < flushJobs.Count) { - ExecuteFlushJob(ref flushJobs[jobIndex], threadPool); + ExecuteFlushJob(ref flushJobs[jobIndex]); } } - void ExecuteFlushJob(ref NarrowPhaseFlushJob job, BufferPool threadPool) + void ExecuteFlushJob(ref NarrowPhaseFlushJob job) { switch (job.Type) { @@ -237,8 +236,10 @@ void ExecuteFlushJob(ref NarrowPhaseFlushJob job, BufferPool threadPool) public void Flush(IThreadDispatcher threadDispatcher = null) { var deterministic = threadDispatcher != null && Simulation.Deterministic; - OnPreflush(threadDispatcher, deterministic); //var start = Stopwatch.GetTimestamp(); + OnPreflush(threadDispatcher, deterministic); + //var end = Stopwatch.GetTimestamp(); + //Console.WriteLine($"Preflush time (us): {1e6 * (end - start) / Stopwatch.Frequency}"); flushJobs = new QuickList(128, Pool); PairCache.PrepareFlushJobs(ref flushJobs); var removalBatchJobCount = ConstraintRemover.CreateFlushJobs(deterministic); @@ -263,7 +264,7 @@ public void Flush(IThreadDispatcher threadDispatcher = null) { for (int i = 0; i < flushJobs.Count; ++i) { - ExecuteFlushJob(ref flushJobs[i], Pool); + ExecuteFlushJob(ref flushJobs[i]); } } else @@ -274,8 +275,6 @@ public void Flush(IThreadDispatcher threadDispatcher = null) //flushWorkerLoop(0); this.threadDispatcher = null; } - //var end = Stopwatch.GetTimestamp(); - //Console.WriteLine($"Flush stage 3 time (us): {1e6 * (end - start) / Stopwatch.Frequency}"); flushJobs.Dispose(Pool); PairCache.Postflush(); diff --git a/Demos/SpecializedTests/Media/2.4/ExcessivePyramidVideoDemo.cs b/Demos/SpecializedTests/Media/2.4/ExcessivePyramidVideoDemo.cs index 124cabf12..96a4d7bcc 100644 --- a/Demos/SpecializedTests/Media/2.4/ExcessivePyramidVideoDemo.cs +++ b/Demos/SpecializedTests/Media/2.4/ExcessivePyramidVideoDemo.cs @@ -56,19 +56,9 @@ public override void Update(Window window, Camera camera, Input input, float dt) ++frameCount; if (frameCount == 128 || (input != null && input.WasPushed(OpenTK.Input.Key.Z))) { - //Create the shape that we'll launch at the pyramids when the user presses a button. var bulletShape = new Sphere(6); - //Note that the use of radius^3 for mass can produce some pretty serious mass ratios. - //Observe what happens when a large ball sits on top of a few boxes with a fraction of the mass- - //the collision appears much squishier and less stable. For most games, if you want to maintain rigidity, you'll want to use some combination of: - //1) Limit the ratio of heavy object masses to light object masses when those heavy objects depend on the light objects. - //2) Use a shorter timestep duration and update more frequently. - //3) Use a greater number of solver iterations. - //#2 and #3 can become very expensive. In pathological cases, it can end up slower than using a quality-focused solver for the same simulation. - //Unfortunately, at the moment, bepuphysics v2 does not contain any alternative solvers, so if you can't afford to brute force the the problem away, - //the best solution is to cheat as much as possible to avoid the corner cases. - var bodyDescription = BodyDescription.CreateConvexDynamic( - new Vector3(0, 8, -1200), new Vector3(0, 0, 230), 5000000, Simulation.Shapes, bulletShape); + var bodyDescription = BodyDescription.CreateDynamic( + new Vector3(0, 8, -1200), new Vector3(0, 0, 230), bulletShape.ComputeInertia(5000000), new (Simulation.Shapes.Add(bulletShape), 0.1f), 0.01f); Simulation.Bodies.Add(bodyDescription); } base.Update(window, camera, input, dt); From 7307673d7ad91452daacde781133484880e2b664 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 10 Feb 2022 19:01:49 -0600 Subject: [PATCH 447/947] More demovizz, and bumped beta number again. --- BepuPhysics/BepuPhysics.csproj | 2 +- BepuUtilities/BepuUtilities.csproj | 2 +- .../RagdollTubeDemo.cs | 2 +- .../Media/2.4/ExcessivePyramidVideoDemo.cs | 10 +- .../Media/2.4/RagdollTubeVideoDemo.cs | 104 ++++++++++++++++++ 5 files changed, 108 insertions(+), 12 deletions(-) rename Demos/{SpecializedTests => Demos}/RagdollTubeDemo.cs (99%) create mode 100644 Demos/SpecializedTests/Media/2.4/RagdollTubeVideoDemo.cs diff --git a/BepuPhysics/BepuPhysics.csproj b/BepuPhysics/BepuPhysics.csproj index 0df0b65d9..ff8e78310 100644 --- a/BepuPhysics/BepuPhysics.csproj +++ b/BepuPhysics/BepuPhysics.csproj @@ -1,7 +1,7 @@  net6.0 - 2.4.0-beta11 + 2.4.0-beta12 Bepu Entertainment LLC Ross Nordby Speedy real time physics simulation library. diff --git a/BepuUtilities/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj index cf003f256..da8615f2a 100644 --- a/BepuUtilities/BepuUtilities.csproj +++ b/BepuUtilities/BepuUtilities.csproj @@ -3,7 +3,7 @@ BepuUtilities BepuUtilities net6.0 - 2.4.0-beta11 + 2.4.0-beta12 Bepu Entertainment LLC Ross Nordby Supporting utilities library for BEPUphysics v2. diff --git a/Demos/SpecializedTests/RagdollTubeDemo.cs b/Demos/Demos/RagdollTubeDemo.cs similarity index 99% rename from Demos/SpecializedTests/RagdollTubeDemo.cs rename to Demos/Demos/RagdollTubeDemo.cs index 409edbf61..98ba64d2e 100644 --- a/Demos/SpecializedTests/RagdollTubeDemo.cs +++ b/Demos/Demos/RagdollTubeDemo.cs @@ -11,7 +11,7 @@ using BepuPhysics.Constraints; using DemoRenderer.UI; -namespace Demos.SpecializedTests +namespace Demos.Demos { /// /// Subjects a bunch of unfortunate ragdolls to a tumble dry cycle. diff --git a/Demos/SpecializedTests/Media/2.4/ExcessivePyramidVideoDemo.cs b/Demos/SpecializedTests/Media/2.4/ExcessivePyramidVideoDemo.cs index 96a4d7bcc..ca491eec6 100644 --- a/Demos/SpecializedTests/Media/2.4/ExcessivePyramidVideoDemo.cs +++ b/Demos/SpecializedTests/Media/2.4/ExcessivePyramidVideoDemo.cs @@ -58,18 +58,10 @@ public override void Update(Window window, Camera camera, Input input, float dt) { var bulletShape = new Sphere(6); var bodyDescription = BodyDescription.CreateDynamic( - new Vector3(0, 8, -1200), new Vector3(0, 0, 230), bulletShape.ComputeInertia(5000000), new (Simulation.Shapes.Add(bulletShape), 0.1f), 0.01f); + new Vector3(0, 8, -1200), new Vector3(0, 0, 230), bulletShape.ComputeInertia(5000000), new(Simulation.Shapes.Add(bulletShape), 0.1f), 0.01f); Simulation.Bodies.Add(bodyDescription); } base.Update(window, camera, input, dt); } - - public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) - { - text.Clear().Append("Press Z to launch a ball!"); - renderer.TextBatcher.Write(text, new Vector2(20, renderer.Surface.Resolution.Y - 20), 16, new Vector3(1, 1, 1), font); - base.Render(renderer, camera, input, text, font); - } - } } diff --git a/Demos/SpecializedTests/Media/2.4/RagdollTubeVideoDemo.cs b/Demos/SpecializedTests/Media/2.4/RagdollTubeVideoDemo.cs new file mode 100644 index 000000000..633306c80 --- /dev/null +++ b/Demos/SpecializedTests/Media/2.4/RagdollTubeVideoDemo.cs @@ -0,0 +1,104 @@ +using BepuUtilities; +using DemoRenderer; +using BepuPhysics; +using BepuPhysics.Collidables; +using System.Numerics; +using System; +using DemoContentLoader; +using Demos.Demos; +using DemoUtilities; +using BepuPhysics.CollisionDetection; +using BepuPhysics.Constraints; +using DemoRenderer.UI; + +namespace Demos.SpecializedTests.Media +{ + /// + /// Subjects a bunch of unfortunate ragdolls to a tumble dry cycle. + /// + public class RagdollTubeVideoDemo : Demo + { + public unsafe override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(0, 9, -40); + camera.Yaw = MathHelper.Pi; + camera.Pitch = 0; + var filters = new CollidableProperty(); + //Note the lowered material stiffness compared to many of the other demos. Ragdolls aren't made of concrete. + //Increasing the maximum recovery velocity helps keep deeper contacts strong, stopping objects from interpenetrating. + //Higher friction helps the bodies clump and flop, rather than just sliding down the slope in the tube. + Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks(filters, new PairMaterialProperties(2, float.MaxValue, new SpringSettings(10, 1))), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(4, 1)); + + int ragdollIndex = 0; + var spacing = new Vector3(1.7f, 1.8f, 0.5f); + int width = 4; + int height = 4; + int length = 120; + var origin = -0.5f * spacing * new Vector3(width - 1, 0, length - 1) + new Vector3(0, 5f, 0); + for (int i = 0; i < width; ++i) + { + for (int j = 0; j < height; ++j) + { + for (int k = 0; k < length; ++k) + { + RagdollDemo.AddRagdoll(origin + spacing * new Vector3(i, j, k), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathHelper.Pi * 0.05f), ragdollIndex++, filters, Simulation); + } + } + } + + ragdollCount = ragdollIndex; + ragdollBodyCount = Simulation.Bodies.ActiveSet.Count; + ragdollConstraintCount = Simulation.Solver.CountConstraints(); + + var tubeCenter = new Vector3(0, 8, 0); + const int panelCount = 20; + const float tubeRadius = 6; + var panelShape = new Box(MathF.PI * 2 * tubeRadius / panelCount, 1, 100); + var panelShapeIndex = Simulation.Shapes.Add(panelShape); + var builder = new CompoundBuilder(BufferPool, Simulation.Shapes, panelCount + 1); + for (int i = 0; i < panelCount; ++i) + { + var rotation = QuaternionEx.CreateFromAxisAngle(Vector3.UnitZ, i * MathHelper.TwoPi / panelCount); + QuaternionEx.TransformUnitY(rotation, out var localUp); + var position = localUp * tubeRadius; + builder.AddForKinematic(panelShapeIndex, (position, rotation), 1); + } + builder.AddForKinematic(Simulation.Shapes.Add(new Box(1, 2, panelShape.Length)), new Vector3(0, tubeRadius - 1, 0), 0); + builder.BuildKinematicCompound(out var children); + var compound = new BigCompound(children, Simulation.Shapes, BufferPool); + var tubeHandle = Simulation.Bodies.Add(BodyDescription.CreateKinematic(tubeCenter, (default, new Vector3(0, 0, .25f)), Simulation.Shapes.Add(compound), 0f)); + filters[tubeHandle] = new SubgroupCollisionFilter(int.MaxValue); + builder.Dispose(); + + var staticShape = new Box(300, 1, 300); + var staticShapeIndex = Simulation.Shapes.Add(staticShape); + var staticDescription = new StaticDescription(new Vector3(0, -0.5f, 0), staticShapeIndex); + Simulation.Statics.Add(staticDescription); + + DemoMeshHelper.LoadModel(content, BufferPool, @"Content\newt.obj", new Vector3(15, 15, 15), out var newtMesh); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0.5f, 80), Quaternion.CreateFromAxisAngle(Vector3.UnitY, MathF.PI), Simulation.Shapes.Add(newtMesh))); + + } + int ragdollBodyCount; + int ragdollConstraintCount; + int ragdollCount; + + public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + { + var resolution = renderer.Surface.Resolution; + renderer.TextBatcher.Write(text.Clear().Append("Ragdoll count:"), new Vector2(16, resolution.Y - 64), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Ragdoll body count:"), new Vector2(16, resolution.Y - 48), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Ragdoll constraint count:"), new Vector2(16, resolution.Y - 32), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Collision constraint count:"), new Vector2(16, resolution.Y - 16), 16, Vector3.One, font); + const float xOffset = 192; + renderer.TextBatcher.Write(text.Clear().Append(ragdollCount), new Vector2(xOffset, resolution.Y - 64), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append(ragdollBodyCount), new Vector2(xOffset, resolution.Y - 48), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append(ragdollConstraintCount), new Vector2(xOffset, resolution.Y - 32), 16, Vector3.One, font); + var collisionConstraintCount = Simulation.Solver.CountConstraints() - ragdollConstraintCount; + renderer.TextBatcher.Write(text.Clear().Append(collisionConstraintCount), new Vector2(xOffset, resolution.Y - 16), 16, Vector3.One, font); + base.Render(renderer, camera, input, text, font); + } + } +} + + From 00585367c7977204305ed39c83433dc7ece89825 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 10 Feb 2022 21:20:02 -0600 Subject: [PATCH 448/947] Slight substepping demo tuning. --- BepuUtilities/Collections/QuickDictionary.cs | 3 +-- Demos/Demos/SubsteppingDemo.cs | 4 ++-- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/BepuUtilities/Collections/QuickDictionary.cs b/BepuUtilities/Collections/QuickDictionary.cs index a57decbfe..2d27d22a4 100644 --- a/BepuUtilities/Collections/QuickDictionary.cs +++ b/BepuUtilities/Collections/QuickDictionary.cs @@ -304,8 +304,7 @@ public void EnsureCapacity(int count, IUnmanagedMemoryPool pool) /// /// Shrinks the internal buffers to the smallest acceptable size and releases the old buffers to the pools. /// - /// Pool used for spans. - /// Element to add. + /// Pool used for spans. public void Compact(IUnmanagedMemoryPool pool) { Validate(); diff --git a/Demos/Demos/SubsteppingDemo.cs b/Demos/Demos/SubsteppingDemo.cs index c2c96b766..8d7ed7f90 100644 --- a/Demos/Demos/SubsteppingDemo.cs +++ b/Demos/Demos/SubsteppingDemo.cs @@ -89,13 +89,13 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) LocalOffsetA = offset * 0.5f, LocalOffsetB = offset * -0.5f, //Once again, the choice of high stiffness makes this potentially unstable without substepping. - SpringSettings = new SpringSettings(480, 1) + SpringSettings = new SpringSettings(640, 1) }); Simulation.Solver.Add(previousLinkHandle, linkHandle, new AngularAxisMotor { LocalAxisA = Vector3.UnitX, TargetVelocity = .25f, - Settings = new MotorSettings(float.MaxValue, 0.0001f) + Settings = new MotorSettings(float.MaxValue, 0.00001f) }); previousLinkHandle = linkHandle; } From 7067808584b2762b9e19a233b9400715eda85af8 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 12 Feb 2022 17:15:25 -0600 Subject: [PATCH 449/947] Another videodemo. --- DemoUtilities/Window.cs | 3 - .../Media/2.4/Colosseum24VideoDemo.cs | 123 ++++++++++++++++++ 2 files changed, 123 insertions(+), 3 deletions(-) create mode 100644 Demos/SpecializedTests/Media/2.4/Colosseum24VideoDemo.cs diff --git a/DemoUtilities/Window.cs b/DemoUtilities/Window.cs index c70b59c9c..899be6154 100644 --- a/DemoUtilities/Window.cs +++ b/DemoUtilities/Window.cs @@ -101,9 +101,6 @@ public Window(string title, Int2 resolution, Int2 location, WindowMode windowMod Resolution = resolution; window.Resize += (form, args) => resized = true; window.Closing += OnClosing; - - window.WindowBorder = WindowBorder.Resizable; - WindowMode = windowMode; } diff --git a/Demos/SpecializedTests/Media/2.4/Colosseum24VideoDemo.cs b/Demos/SpecializedTests/Media/2.4/Colosseum24VideoDemo.cs new file mode 100644 index 000000000..20bf4bffc --- /dev/null +++ b/Demos/SpecializedTests/Media/2.4/Colosseum24VideoDemo.cs @@ -0,0 +1,123 @@ +using BepuPhysics; +using BepuPhysics.Collidables; +using BepuPhysics.Constraints; +using BepuUtilities; +using DemoContentLoader; +using DemoRenderer; +using DemoRenderer.UI; +using DemoUtilities; +using System; +using System.Numerics; + +namespace Demos.SpecializedTests.Media +{ + /// + /// A colosseum made out of boxes that is sometimes hit by large purple hail. + /// + public class Colosseum24VideoDemo : Demo + { + public static void CreateRingWall(Simulation simulation, Vector3 position, Box ringBoxShape, BodyDescription bodyDescription, int height, float radius) + { + var circumference = MathF.PI * 2 * radius; + var boxCountPerRing = (int)(0.9f * circumference / ringBoxShape.Length); + float increment = MathHelper.TwoPi / boxCountPerRing; + for (int ringIndex = 0; ringIndex < height; ringIndex++) + { + for (int i = 0; i < boxCountPerRing; i++) + { + var angle = ((ringIndex & 1) == 0 ? i + 0.5f : i) * increment; + bodyDescription.Pose = (position + new Vector3(-MathF.Cos(angle) * radius, (ringIndex + 0.5f) * ringBoxShape.Height, MathF.Sin(angle) * radius), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, angle)); + simulation.Bodies.Add(bodyDescription); + } + } + } + + public static void CreateRingPlatform(Simulation simulation, Vector3 position, Box ringBoxShape, BodyDescription bodyDescription, float radius) + { + var innerCircumference = MathF.PI * 2 * (radius - ringBoxShape.HalfLength); + var boxCount = (int)(0.95f * innerCircumference / ringBoxShape.Height); + float increment = MathHelper.TwoPi / boxCount; + for (int i = 0; i < boxCount; i++) + { + var angle = i * increment; + bodyDescription.Pose = (position + new Vector3(-MathF.Cos(angle) * radius, ringBoxShape.HalfWidth, MathF.Sin(angle) * radius), + QuaternionEx.Concatenate(QuaternionEx.CreateFromAxisAngle(Vector3.UnitZ, MathF.PI * 0.5f), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, angle + MathF.PI * 0.5f))); + simulation.Bodies.Add(bodyDescription); + } + } + + public static Vector3 CreateRing(Simulation simulation, Vector3 position, Box ringBoxShape, BodyDescription bodyDescription, float radius, int heightPerPlatformLevel, int platformLevels) + { + for (int platformIndex = 0; platformIndex < platformLevels; ++platformIndex) + { + var wallOffset = ringBoxShape.HalfLength - ringBoxShape.HalfWidth; + CreateRingWall(simulation, position, ringBoxShape, bodyDescription, heightPerPlatformLevel, radius + wallOffset); + CreateRingWall(simulation, position, ringBoxShape, bodyDescription, heightPerPlatformLevel, radius - wallOffset); + CreateRingPlatform(simulation, position + new Vector3(0, heightPerPlatformLevel * ringBoxShape.Height, 0), ringBoxShape, bodyDescription, radius); + position.Y += heightPerPlatformLevel * ringBoxShape.Height + ringBoxShape.Width; + } + return position; + } + + public unsafe override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(-30, 40, -30); + camera.Yaw = MathHelper.Pi * 3f / 4; + camera.Pitch = MathHelper.Pi * 0.2f; + + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(90, 1), maximumRecoveryVelocity: 20), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(2, 7)); + + var ringBoxShape = new Box(0.5f, 1, 3); + var boxDescription = BodyDescription.CreateDynamic(new Vector3(), ringBoxShape.ComputeInertia(1), new(Simulation.Shapes.Add(ringBoxShape), 0.1f), -0.01f); + + //CreateRingWall(Simulation, default, ringBoxShape, boxDescription, 400, 45); + var layerPosition = new Vector3(); + const int layerCount = 1; + var innerRadius = 20f; + var heightPerPlatform = 10; + var platformsPerLayer = 30; + var ringSpacing = 0.5f; + for (int layerIndex = 0; layerIndex < layerCount; ++layerIndex) + { + var ringCount = layerCount - layerIndex; + for (int ringIndex = 0; ringIndex < ringCount; ++ringIndex) + { + CreateRing(Simulation, layerPosition, ringBoxShape, boxDescription, innerRadius + ringIndex * (ringBoxShape.Length + ringSpacing) + layerIndex * (ringBoxShape.Length - ringBoxShape.Width), heightPerPlatform, platformsPerLayer); + } + layerPosition.Y += platformsPerLayer * (ringBoxShape.Height * heightPerPlatform + ringBoxShape.Width); + } + + Console.WriteLine($"box count: {Simulation.Bodies.ActiveSet.Count}"); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), Simulation.Shapes.Add(new Box(500, 1, 500)))); + + var bulletShape = new Sphere(0.5f); + bulletDescription = BodyDescription.CreateDynamic(new Vector3(), bulletShape.ComputeInertia(.1f), Simulation.Shapes.Add(bulletShape), 0.01f); + + var shootiePatootieShape = new Sphere(3f); + shootiePatootieDescription = BodyDescription.CreateDynamic(new Vector3(), shootiePatootieShape.ComputeInertia(100), new(Simulation.Shapes.Add(shootiePatootieShape), 0.1f), 0.01f); + } + + BodyDescription bulletDescription; + BodyDescription shootiePatootieDescription; + public override void Update(Window window, Camera camera, Input input, float dt) + { + if (input != null) + { + if (input.WasPushed(OpenTK.Input.Key.Z)) + { + bulletDescription.Pose.Position = camera.Position; + bulletDescription.Velocity.Linear = camera.GetRayDirection(input.MouseLocked, window.GetNormalizedMousePosition(input.MousePosition)) * 400; + Simulation.Bodies.Add(bulletDescription); + } + else if (input.WasPushed(OpenTK.Input.Key.X)) + { + shootiePatootieDescription.Pose.Position = camera.Position; + shootiePatootieDescription.Velocity.Linear = camera.GetRayDirection(input.MouseLocked, window.GetNormalizedMousePosition(input.MousePosition)) * 100; + Simulation.Bodies.Add(shootiePatootieDescription); + } + } + base.Update(window, camera, input, dt); + } + + } +} From d72392a9f88637c131028095a520e68f35a85604 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 13 Feb 2022 17:35:24 -0600 Subject: [PATCH 450/947] Pulled min/max speculative margins into Collidable since they do nothing on statics. --- BepuPhysics/BepuPhysics.csproj | 2 +- BepuPhysics/BodySet.cs | 8 +- BepuPhysics/Collidables/BoundingBoxBatcher.cs | 6 +- BepuPhysics/Collidables/Collidable.cs | 78 +++++++------------ .../Collidables/CollidableDescription.cs | 69 ++++++++++++++-- BepuPhysics/StaticDescription.cs | 50 ++++++++++-- BepuPhysics/Statics.cs | 15 ++-- BepuUtilities/BepuUtilities.csproj | 2 +- DemoTests/PairDeterminismTests.cs | 16 ++-- Demos/Demos/Characters/CharacterInput.cs | 2 +- .../Demos/ContinuousCollisionDetectionDemo.cs | 18 ++--- Demos/Demos/Tanks/Tank.cs | 2 +- Demos/SpecializedTests/CapsuleTestDemo.cs | 4 +- Demos/SpecializedTests/ClothLatticeDemo.cs | 2 +- Demos/SpecializedTests/ConvexHullTestDemo.cs | 2 +- Demos/SpecializedTests/CylinderTestDemo.cs | 2 +- .../FountainStressTestDemo.cs | 6 +- .../SpecializedTests/MeshReductionTestDemo.cs | 10 +-- Demos/SpecializedTests/TriangleTestDemo.cs | 24 +++--- 19 files changed, 197 insertions(+), 121 deletions(-) diff --git a/BepuPhysics/BepuPhysics.csproj b/BepuPhysics/BepuPhysics.csproj index ff8e78310..f053b0374 100644 --- a/BepuPhysics/BepuPhysics.csproj +++ b/BepuPhysics/BepuPhysics.csproj @@ -1,7 +1,7 @@  net6.0 - 2.4.0-beta12 + 2.4.0-beta13 Bepu Entertainment LLC Ross Nordby Speedy real time physics simulation library. diff --git a/BepuPhysics/BodySet.cs b/BepuPhysics/BodySet.cs index 9d348ccac..f3beaa72e 100644 --- a/BepuPhysics/BodySet.cs +++ b/BepuPhysics/BodySet.cs @@ -133,10 +133,12 @@ internal void ApplyDescriptionByIndex(int index, in BodyDescription description) //Kinematics, though, can have their inertia updates skipped at runtime since the world inverse inertia should always be a bunch of zeroes, so we pre-zero it. state.Inertia.World = default; ref var collidable = ref Collidables[index]; - collidable.Continuity = description.Collidable.Continuity; //Note that we change the shape here. If the collidable transitions from shapeless->shapeful or shapeful->shapeless, the broad phase has to be notified //so that it can create/remove an entry. That's why this function isn't public. collidable.Shape = description.Collidable.Shape; + collidable.Continuity = description.Collidable.Continuity; + collidable.MinimumSpeculativeMargin = description.Collidable.MinimumSpeculativeMargin; + collidable.MaximumSpeculativeMargin = description.Collidable.MaximumSpeculativeMargin; //To avoid leaking undefined data, initialize the speculative margin to zero. //Under normal circumstances, it should not be possible for any relevant system to see an undefined speculative margin, since PredictBoundingBoxes sets the value. //However, corner cases like a body being added asleep and woken by the narrow phase can mean prediction does not run. @@ -155,8 +157,10 @@ public void GetDescription(int index, out BodyDescription description) description.Velocity = state.Motion.Velocity; description.LocalInertia = state.Inertia.Local; ref var collidable = ref Collidables[index]; - description.Collidable.Continuity = collidable.Continuity; description.Collidable.Shape = collidable.Shape; + description.Collidable.Continuity = collidable.Continuity; + description.Collidable.MinimumSpeculativeMargin = collidable.MinimumSpeculativeMargin; + description.Collidable.MaximumSpeculativeMargin = collidable.MaximumSpeculativeMargin; ref var activity = ref Activity[index]; description.Activity.SleepThreshold = activity.SleepThreshold; description.Activity.MinimumTimestepCountUnderThreshold = activity.MinimumTimestepsUnderThreshold; diff --git a/BepuPhysics/Collidables/BoundingBoxBatcher.cs b/BepuPhysics/Collidables/BoundingBoxBatcher.cs index 8c843ff16..74c863d04 100644 --- a/BepuPhysics/Collidables/BoundingBoxBatcher.cs +++ b/BepuPhysics/Collidables/BoundingBoxBatcher.cs @@ -175,8 +175,8 @@ public unsafe void ExecuteConvexBatch(ConvexShapeBatch public ContinuousDetectionMode Mode; - /// - /// Lower bound on the value of the speculative margin used by the collidable. - /// - /// 0 tends to be a good default value. Higher values can be chosen if velocity magnitude is a poor proxy for speculative margins, but these cases are rare. - /// In those cases, try to use the smallest value that still satisfies requirements to avoid creating unnecessary contact constraints. - public float MinimumSpeculativeMargin; - - /// - /// Upper bound on the value of the speculative margin used by the collidable. - /// - /// tends to be a good default value for discrete or passive mode collidables. - /// The speculative margin will increase in size proportional to velocity magnitude, so high values don't usually cause performance problems. - /// Smaller values are necessary when using continuous collision detection, since continuous collision detection will early out once it finds a time that puts the collidables within speculative margin. - /// - public float MaximumSpeculativeMargin; - /// /// If using , this defines the minimum progress that the sweep test will make when searching for the first time of impact. /// Collisions lasting less than may be missed by the sweep test. Using larger values can significantly increase the performance of sweep tests. @@ -74,8 +58,6 @@ public struct ContinuousDetection /// public bool AllowExpansionBeyondSpeculativeMargin => (uint)Mode > 0; - //Note: there's no "Discrete" property anymore. Discrete with default values of 0 and float.MaxValue would be fully equivalent to Passive with the same configuration, so better just to encourage the Passive property instead. - /// /// No sweep tests are performed. Default speculative contact generation will occur within the speculative margin. /// The collidable's bounding box will not be expanded by velocity beyond the speculative margin. @@ -83,13 +65,14 @@ public struct ContinuousDetection /// the fact that its bounding box is not expanded may cause it to miss a collision even with a non-Discrete collidable. /// Note that Discrete and Passive only differ if maximum speculative margin is restricted. /// - /// Minimum speculative margin to use for the collidable. Zero is typically a good choice. - /// Maximum speculative margin to use for the collidable. /// If using Discrete instead of Passive, this is presumably some smaller finite value to limit the number of collision pairs found during high velocity movement. /// Detection settings for the given discrete configuration. - public static ContinuousDetection Discrete(float minimumSpeculativeMargin = 0f, float maximumSpeculativeMargin = float.MaxValue) + public static ContinuousDetection Discrete { - return new() { Mode = ContinuousDetectionMode.Discrete, MinimumSpeculativeMargin = minimumSpeculativeMargin, MaximumSpeculativeMargin = maximumSpeculativeMargin }; + get + { + return new() { Mode = ContinuousDetectionMode.Discrete }; + } } /// @@ -100,20 +83,7 @@ public static ContinuousDetection Discrete(float minimumSpeculativeMargin = 0f, /// Detection settings for the passive configuration. public static ContinuousDetection Passive { - get { return new() { Mode = ContinuousDetectionMode.Passive, MinimumSpeculativeMargin = 0, MaximumSpeculativeMargin = float.MaxValue }; } - } - - /// - /// No sweep tests are performed. Default speculative contact generation will occur within the speculative margin. - /// The collidable's bounding box will be expanded by velocity. The speculative margin will grow with velocity but be bounded by the given interval. - /// This is useful when a collidable may move quickly and does not itself require continuous detection, but there exist other collidables with continuous modes that should avoid missing collisions. - /// - /// Minimum speculative margin to use for the collidable. Zero is typically a good choice. - /// Maximum speculative margin to use for the collidable. - /// Detection settings for the given passive configuration. - public static ContinuousDetection CreatePassive(float minimumSpeculativeMargin, float maximumSpeculativeMargin) - { - return new() { Mode = ContinuousDetectionMode.Passive, MinimumSpeculativeMargin = minimumSpeculativeMargin, MaximumSpeculativeMargin = maximumSpeculativeMargin }; + get { return new() { Mode = ContinuousDetectionMode.Passive }; } } /// @@ -126,16 +96,12 @@ public static ContinuousDetection CreatePassive(float minimumSpeculativeMargin, /// If the region has been refined to be smaller than SweepConvergenceThreshold, the sweep will terminate. /// Values closer to zero will converge more closely to the true time of impact, but for speculative contact generation larger values usually work fine. /// Larger values allow the sweep to terminate much earlier and can significantly improve sweep performance. - /// Minimum speculative margin to use for the collidable. Zero is typically a good choice. - /// Maximum speculative margin to use for the collidable. /// Detection settings for the given continuous configuration. - public static ContinuousDetection Continuous(float minimumSweepTimestep = 1e-3f, float sweepConvergenceThreshold = 1e-3f, float minimumSpeculativeMargin = 0f, float maximumSpeculativeMargin = float.MaxValue) + public static ContinuousDetection Continuous(float minimumSweepTimestep = 1e-3f, float sweepConvergenceThreshold = 1e-3f) { return new() { Mode = ContinuousDetectionMode.Continuous, - MinimumSpeculativeMargin = minimumSpeculativeMargin, - MaximumSpeculativeMargin = maximumSpeculativeMargin, MinimumSweepTimestep = minimumSweepTimestep, SweepConvergenceThreshold = sweepConvergenceThreshold }; @@ -143,32 +109,46 @@ public static ContinuousDetection Continuous(float minimumSweepTimestep = 1e-3f, } /// - /// Description of a collidable instance living in the broad phase and able to generate collision pairs. - /// Collidables with a ShapeIndex that points to nothing (a default constructed TypedIndex) do not actually refer to any existing Collidable. + /// Description of a collidable used by a body living in the broad phase and able to generate collision pairs. + /// Collidables with a ShapeIndex that points to nothing (a default constructed ) are not capable of colliding with anything. /// This can be used for a body which needs no collidable representation. /// public struct Collidable { + /// + /// Index of the shape used by the body. While this can be changed, any transition from shapeless->shapeful or shapeful->shapeless must be reported to the broad phase. + /// If you need to perform such a transition, consider using or ; those functions update the relevant state. + /// + public TypedIndex Shape; /// /// Continuous collision detection settings for this collidable. Includes the collision detection mode to use and tuning variables associated with those modes. /// public ContinuousDetection Continuity; - /// - /// Index of the shape used by the body. While this can be changed, any transition from shapeless->shapeful or shapeful->shapeless must be reported to the broad phase. - /// If you need to perform such a transition, consider using or ; those functions update the relevant state. + /// Lower bound on the value of the speculative margin used by the collidable. /// - public TypedIndex Shape; + /// 0 tends to be a good default value. Higher values can be chosen if velocity magnitude is a poor proxy for speculative margins, but these cases are rare. + /// In those cases, try to use the smallest value that still satisfies requirements to avoid creating unnecessary contact constraints. + public float MinimumSpeculativeMargin; + /// + /// Upper bound on the value of the speculative margin used by the collidable. + /// + /// tends to be a good default value for discrete or passive mode collidables. + /// The speculative margin will increase in size proportional to velocity magnitude, so having an unlimited maximum won't cost extra if the body isn't moving fast. + /// Smaller values can be useful for improving performance in chaotic situations where missing a collision is acceptable. When using , a speculative margin larger than the velocity magnitude will result in the sweep test being skipped, so lowering the maximum margin can help avoid ghost collisions. + /// + public float MaximumSpeculativeMargin; /// /// Automatically computed size of the margin around the surface of the shape in which contacts can be generated. These contacts will have negative depth and only contribute if the frame's velocities /// would push the shapes of a pair into overlap. - /// This is automatically set by bounding box prediction each frame, and is bound by the collidable's and values. + /// This is automatically set by bounding box prediction each frame, and is bound by the collidable's and values. /// The effective speculative margin for a collision pair can also be modified from callbacks. /// This should be positive to avoid jittering. /// It can also be used as a form of continuous collision detection, but excessively high values combined with fast motion may result in visible 'ghost collision' artifacts. /// For continuous collision detection with less chance of ghost collisions, use . - /// If using , consider setting to a smaller value to help filter ghost collisions. + /// If using , consider setting to a smaller value to help filter ghost collisions. + /// For more information, see the ContinuousCollisionDetection.md documentation. /// public float SpeculativeMargin; /// diff --git a/BepuPhysics/Collidables/CollidableDescription.cs b/BepuPhysics/Collidables/CollidableDescription.cs index a0950be1e..4a78bf1c5 100644 --- a/BepuPhysics/Collidables/CollidableDescription.cs +++ b/BepuPhysics/Collidables/CollidableDescription.cs @@ -13,24 +13,83 @@ public struct CollidableDescription /// Continuous collision detection settings used by the collidable. /// public ContinuousDetection Continuity; + /// + /// Lower bound on the value of the speculative margin used by the collidable. + /// + /// 0 tends to be a good default value. Higher values can be chosen if velocity magnitude is a poor proxy for speculative margins, but these cases are rare. + /// In those cases, try to use the smallest value that still satisfies requirements to avoid creating unnecessary contact constraints. + public float MinimumSpeculativeMargin; + /// + /// Upper bound on the value of the speculative margin used by the collidable. + /// + /// tends to be a good default value for discrete or passive mode collidables. + /// The speculative margin will increase in size proportional to velocity magnitude, so having an unlimited maximum won't cost extra if the body isn't moving fast. + /// Smaller values can be useful for improving performance in chaotic situations where missing a collision is acceptable. When using , a speculative margin larger than the velocity magnitude will result in the sweep test being skipped, so lowering the maximum margin can help avoid ghost collisions. + /// + public float MaximumSpeculativeMargin; /// /// Constructs a new collidable description. /// /// Shape used by the collidable. + /// Lower bound on the value of the speculative margin used by the collidable. + /// Upper bound on the value of the speculative margin used by the collidable. /// Continuous collision detection settings for the collidable. - public CollidableDescription(TypedIndex shape, in ContinuousDetection continuity) + public CollidableDescription(TypedIndex shape, float minimumSpeculativeMargin, float maximumSpeculativeMargin, ContinuousDetection continuity) { Shape = shape; + MinimumSpeculativeMargin = minimumSpeculativeMargin; + MaximumSpeculativeMargin = maximumSpeculativeMargin; Continuity = continuity; } /// - /// Constructs a new collidable description with . Will use a minimum speculative margin of 0 and a maximum of . + /// Constructs a new collidable description with . + /// + /// Shape used by the collidable. + /// Lower bound on the value of the speculative margin used by the collidable. + /// Upper bound on the value of the speculative margin used by the collidable. + public CollidableDescription(TypedIndex shape, float minimumSpeculativeMargin, float maximumSpeculativeMargin) + { + Shape = shape; + MinimumSpeculativeMargin = minimumSpeculativeMargin; + MaximumSpeculativeMargin = maximumSpeculativeMargin; + Continuity = ContinuousDetection.Discrete; + } + + /// + /// Constructs a new collidable description. Uses 0 for the . + /// + /// Shape used by the collidable. + /// Upper bound on the value of the speculative margin used by the collidable. + /// Continuous collision detection settings for the collidable. + public CollidableDescription(TypedIndex shape, float maximumSpeculativeMargin, ContinuousDetection continuity) + { + Shape = shape; + MinimumSpeculativeMargin = 0; + MaximumSpeculativeMargin = maximumSpeculativeMargin; + Continuity = continuity; + } + + /// + /// Constructs a new collidable description. Uses 0 for the and for the . + /// + /// Shape used by the collidable. + /// Continuous collision detection settings for the collidable. + public CollidableDescription(TypedIndex shape, ContinuousDetection continuity) + { + Shape = shape; + MinimumSpeculativeMargin = 0; + MaximumSpeculativeMargin = float.MaxValue; + Continuity = continuity; + } + + /// + /// Constructs a new collidable description with . Will use a of 0 and a of . /// /// Shape used by the collidable. - /// and are equivalent in behavior when the maximum speculative margin is since they both result in the same (unbounded) expansion of body bounding boxes in response to velocity. - public CollidableDescription(TypedIndex shape) : this(shape, ContinuousDetection.Passive) + /// and are equivalent in behavior when the is since they both result in the same (unbounded) expansion of body bounding boxes in response to velocity. + public CollidableDescription(TypedIndex shape) : this(shape, 0, float.MaxValue, ContinuousDetection.Passive) { } @@ -39,7 +98,7 @@ public CollidableDescription(TypedIndex shape) : this(shape, ContinuousDetection /// /// Shape used by the collidable. /// Maximum speculative margin to be used with the discrete continuity configuration. - public CollidableDescription(TypedIndex shape, float maximumSpeculativeMargin) : this(shape, ContinuousDetection.Discrete(0, maximumSpeculativeMargin)) + public CollidableDescription(TypedIndex shape, float maximumSpeculativeMargin) : this(shape, 0, maximumSpeculativeMargin, ContinuousDetection.Discrete) { } diff --git a/BepuPhysics/StaticDescription.cs b/BepuPhysics/StaticDescription.cs index 1fa521d95..702a061d6 100644 --- a/BepuPhysics/StaticDescription.cs +++ b/BepuPhysics/StaticDescription.cs @@ -13,19 +13,37 @@ public struct StaticDescription /// public RigidPose Pose; /// - /// Collidable properties of the static. + /// Shape of the static. /// - public CollidableDescription Collidable; + public TypedIndex Shape; + /// + /// Continuous collision detection settings for the static. + /// + public ContinuousDetection Continuity; /// /// Builds a new static description. /// /// Pose of the static collidable. - /// Collidable description for the static. - public StaticDescription(RigidPose pose, CollidableDescription collidable) + /// Shape of the static. + /// Continuous collision detection settings for the static. + public StaticDescription(RigidPose pose, TypedIndex shape, ContinuousDetection continuity) + { + Pose = pose; + Shape = shape; + Continuity = continuity; + } + + /// + /// Builds a new static description with continuity. + /// + /// Pose of the static collidable. + /// Shape of the static. + public StaticDescription(RigidPose pose, TypedIndex shape) { Pose = pose; - Collidable = collidable; + Shape = shape; + Continuity = ContinuousDetection.Discrete; } /// @@ -33,12 +51,28 @@ public StaticDescription(RigidPose pose, CollidableDescription collidable) /// /// Position of the static. /// Orientation of the static. - /// Collidable description for the static. - public StaticDescription(Vector3 position, Quaternion orientation, CollidableDescription collidable) + /// Shape of the static. + /// Continuous collision detection settings for the static. + public StaticDescription(Vector3 position, Quaternion orientation, TypedIndex shape, ContinuousDetection continuity) + { + Pose.Position = position; + Pose.Orientation = orientation; + Shape = shape; + Continuity = continuity; + } + + /// + /// Builds a new static description with continuity. + /// + /// Position of the static. + /// Orientation of the static. + /// Shape of the static. + public StaticDescription(Vector3 position, Quaternion orientation, TypedIndex shape) { Pose.Position = position; Pose.Orientation = orientation; - Collidable = collidable; + Shape = shape; + Continuity = ContinuousDetection.Discrete; } } diff --git a/BepuPhysics/Statics.cs b/BepuPhysics/Statics.cs index bdfb63134..4c4da9123 100644 --- a/BepuPhysics/Statics.cs +++ b/BepuPhysics/Statics.cs @@ -62,7 +62,6 @@ public struct Static /// Continuous collision detection settings for this collidable. Includes the collision detection mode to use and tuning variables associated with those modes. /// /// Note that statics cannot move, so there is no difference between and for them. - /// The minimum and maximum speculative margins will still constrain the margins created for each collision pair involved in the static. /// Enabling will still require that pairs associated with the static use swept continuous collision detection. public ContinuousDetection Continuity; @@ -376,11 +375,11 @@ internal void ApplyDescriptionByIndexWithoutBroadPhaseModification @@ -463,7 +462,7 @@ public unsafe void ApplyDescription(StaticHandle handle, in St { ValidateExistingHandle(handle); var index = HandleToIndex[handle.Value]; - Debug.Assert(description.Collidable.Shape.Exists, "Static collidables cannot lack a shape. Their only purpose is colliding."); + Debug.Assert(description.Shape.Exists, "Static collidables cannot lack a shape. Their only purpose is colliding."); //Wake all bodies up in the old bounds AND the new bounds. Sleeping bodies that may have been resting on the old static need to be aware of the new environment. var broadPhaseIndex = this[index].BroadPhaseIndex; AwakenBodiesInExistingBounds(broadPhaseIndex, ref filter); @@ -495,8 +494,8 @@ public void GetDescription(StaticHandle handle, out StaticDescription descriptio var index = HandleToIndex[handle.Value]; ref var collidable = ref this[index]; description.Pose = collidable.Pose; - description.Collidable.Continuity = collidable.Continuity; - description.Collidable.Shape = collidable.Shape; + description.Continuity = collidable.Continuity; + description.Shape = collidable.Shape; } /// diff --git a/BepuUtilities/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj index da8615f2a..927a3b974 100644 --- a/BepuUtilities/BepuUtilities.csproj +++ b/BepuUtilities/BepuUtilities.csproj @@ -3,7 +3,7 @@ BepuUtilities BepuUtilities net6.0 - 2.4.0-beta12 + 2.4.0-beta13 Bepu Entertainment LLC Ross Nordby Supporting utilities library for BEPUphysics v2. diff --git a/DemoTests/PairDeterminismTests.cs b/DemoTests/PairDeterminismTests.cs index 1d8da3209..6a12fa177 100644 --- a/DemoTests/PairDeterminismTests.cs +++ b/DemoTests/PairDeterminismTests.cs @@ -78,7 +78,7 @@ static void ComputeCollisions(CollisionTaskRegistry registry, Shapes shapes, Buf var index = remapIndices[i]; ref var poseA = ref posesA[index]; ref var poseB = ref posesB[index]; - batcher.Add(a.Shape, b.Shape, poseB.Position - poseA.Position, poseA.Orientation, poseB.Orientation, Math.Max(a.Continuity.MaximumSpeculativeMargin, b.Continuity.MaximumSpeculativeMargin), new PairContinuation(index)); + batcher.Add(a.Shape, b.Shape, poseB.Position - poseA.Position, poseA.Orientation, poseB.Orientation, Math.Max(a.MaximumSpeculativeMargin, b.MaximumSpeculativeMargin), new PairContinuation(index)); if (i % flushInterval == flushInterval - 1) { batcher.Flush(); @@ -239,13 +239,13 @@ public static void Test() var shapes = new Shapes(pool, 8); const float speculativeMargin = 0.1f; - var continuousDetection = ContinuousDetection.Discrete(speculativeMargin, speculativeMargin); - var sphereCollidable = new CollidableDescription(shapes.Add(sphere), continuousDetection); - var capsuleCollidable = new CollidableDescription(shapes.Add(capsule), continuousDetection); - var boxCollidable = new CollidableDescription(shapes.Add(box), continuousDetection); - var triangleCollidable = new CollidableDescription(shapes.Add(triangle), continuousDetection); - var cylinderCollidable = new CollidableDescription(shapes.Add(cylinder), continuousDetection); - var hullCollidable = new CollidableDescription(shapes.Add(convexHull), continuousDetection); + var continuousDetection = ContinuousDetection.Discrete; + var sphereCollidable = new CollidableDescription(shapes.Add(sphere), speculativeMargin, continuousDetection); + var capsuleCollidable = new CollidableDescription(shapes.Add(capsule), speculativeMargin, continuousDetection); + var boxCollidable = new CollidableDescription(shapes.Add(box), speculativeMargin, continuousDetection); + var triangleCollidable = new CollidableDescription(shapes.Add(triangle), speculativeMargin, continuousDetection); + var cylinderCollidable = new CollidableDescription(shapes.Add(cylinder), speculativeMargin, continuousDetection); + var hullCollidable = new CollidableDescription(shapes.Add(convexHull), speculativeMargin, continuousDetection); var compoundBuilder = new CompoundBuilder(pool, shapes, 6); diff --git a/Demos/Demos/Characters/CharacterInput.cs b/Demos/Demos/Characters/CharacterInput.cs index f3819083b..2679147a2 100644 --- a/Demos/Demos/Characters/CharacterInput.cs +++ b/Demos/Demos/Characters/CharacterInput.cs @@ -40,7 +40,7 @@ public CharacterInput(CharacterControllers characters, Vector3 initialPosition, //This is effectively equivalent to giving it an infinite inertia tensor- in other words, no torque will cause it to rotate. bodyHandle = characters.Simulation.Bodies.Add( BodyDescription.CreateDynamic(initialPosition, new BodyInertia { InverseMass = 1f / mass }, - new(shapeIndex, ContinuousDetection.CreatePassive(minimumSpeculativeMargin, float.MaxValue)), shape.Radius * 0.02f)); + new(shapeIndex, minimumSpeculativeMargin, float.MaxValue, ContinuousDetection.Passive), shape.Radius * 0.02f)); ref var character = ref characters.AllocateCharacter(bodyHandle); character.LocalUp = new Vector3(0, 1, 0); character.CosMaximumSlope = MathF.Cos(maximumSlope); diff --git a/Demos/Demos/ContinuousCollisionDetectionDemo.cs b/Demos/Demos/ContinuousCollisionDetectionDemo.cs index 4e766be9e..1bfa5a13a 100644 --- a/Demos/Demos/ContinuousCollisionDetectionDemo.cs +++ b/Demos/Demos/ContinuousCollisionDetectionDemo.cs @@ -19,7 +19,7 @@ public class ContinuousCollisionDetectionDemo : Demo ConstraintHandle spinnerMotorDefaultA, spinnerMotorDefaultB, spinnerMotorSweepA, spinnerMotorSweepB; RolloverInfo rolloverInfo; - ConstraintHandle BuildSpinner(Vector3 initialPosition, float rotationSpeed, ContinuousDetection continuousDetection) + ConstraintHandle BuildSpinner(Vector3 initialPosition, float rotationSpeed, float maximumSpeculativeMargin, ContinuousDetection continuousDetection) { var spinnerBase = Simulation.Bodies.Add(BodyDescription.CreateDynamic(initialPosition, new BodyInertia { InverseMass = 1e-2f }, Simulation.Shapes.Add(new Box(2, 2, 2)), 0.01f)); var bladeShape = new Box(5, 0.01f, 1); @@ -41,7 +41,7 @@ ConstraintHandle BuildSpinner(Vector3 initialPosition, float rotationSpeed, Cont //Using a restricted speculative margin by setting the maximumSpeculativeMargin to 0.2 means that collision detection won't accept distant contacts. //This pretty much eliminates ghost collisions, while the continuous sweep helps avoid missed collisions. - var spinnerBlade = Simulation.Bodies.Add(BodyDescription.CreateDynamic(initialPosition, bladeInertia, new(shapeIndex, continuousDetection), 0.01f)); + var spinnerBlade = Simulation.Bodies.Add(BodyDescription.CreateDynamic(initialPosition, bladeInertia, new(shapeIndex, maximumSpeculativeMargin, continuousDetection), 0.01f)); Simulation.Solver.Add(spinnerBase, spinnerBlade, new Hinge { LocalHingeAxisA = new Vector3(0, 0, 1), LocalHingeAxisB = new Vector3(0, 0, 1), LocalOffsetB = new Vector3(0, 0, -3), SpringSettings = new SpringSettings(30, 1) }); Simulation.Solver.Add(spinnerBase, spinnerBlade, new AngularAxisMotor { LocalAxisA = new Vector3(0, 0, 1), Settings = new MotorSettings(10, 1e-4f), TargetVelocity = rotationSpeed }); return Simulation.Solver.Add(spinnerBase, new OneBodyLinearServo { ServoSettings = ServoSettings.Default, SpringSettings = new SpringSettings(30, 1) }); @@ -70,7 +70,7 @@ public override void Initialize(ContentArchive content, Camera camera) { //The first set of boxes are forced to use very small speculative margins. They're going to tunnel into the ground, since no contacts will be created to stop it. Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(-37 + 2 * j, 100 + (i + j) * 2, -30 + i * 2), new Vector3(0, -150, 0), inertia, - new(shapeIndex, ContinuousDetection.Discrete(maximumSpeculativeMargin: 0.01f)), 0.01f)); + new(shapeIndex, 0.01f, ContinuousDetection.Discrete), 0.01f)); //The second set of boxes are not using explicit continuous collision sweeps, but have unlimited speculative margins. //This configuration is the most common one you're likely to see in the demos (and use in your own applications). @@ -93,7 +93,7 @@ public override void Initialize(ContentArchive content, Camera camera) //runs to create the actual contact manifold. That provides high quality contact positions and speculative depths. //If the ground that these boxes were smashing into was something like a mesh- which is infinitely thin- you may want to increase the sweep accuracy. Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(17 + 2 * j, 100 + (i + j) * 2, -30 + i * 2), new Vector3(0, -150, 0), inertia, - new(shapeIndex, ContinuousDetection.Continuous(1e-3f, 1e-2f, maximumSpeculativeMargin: 0.01f)), 0.01f)); + new(shapeIndex, 0.01f, ContinuousDetection.Continuous(1e-3f, 1e-2f)), 0.01f)); } } rolloverInfo = new RolloverInfo(); @@ -104,13 +104,13 @@ public override void Initialize(ContentArchive content, Camera camera) //Build a couple of spinners to ram into each other to showcase angular CCD. Note that the spin speeds are slightly different- that helps avoid //synchronization that makes the blades frequently miss each other, which sorta ruins a CCD demo. var onlySpeculativeMargin = ContinuousDetection.Passive; - spinnerMotorDefaultA = BuildSpinner(new Vector3(-20, 14, 0), 53, onlySpeculativeMargin); - spinnerMotorDefaultB = BuildSpinner(new Vector3(-10, 14, 0), 59, onlySpeculativeMargin); + spinnerMotorDefaultA = BuildSpinner(new Vector3(-20, 14, 0), 53, 0.2f, onlySpeculativeMargin); + spinnerMotorDefaultB = BuildSpinner(new Vector3(-10, 14, 0), 59, 0.2f, onlySpeculativeMargin); rolloverInfo.Add(new Vector3(-15, 14, -5), "Unlimited speculative margin"); - var continuous = ContinuousDetection.Continuous(1e-4f, 1e-4f, maximumSpeculativeMargin: 0.2f); - spinnerMotorSweepA = BuildSpinner(new Vector3(10, 14, 0), 53, continuous); - spinnerMotorSweepB = BuildSpinner(new Vector3(20, 14, 0), 59, continuous); + var continuous = ContinuousDetection.Continuous(1e-4f, 1e-4f); + spinnerMotorSweepA = BuildSpinner(new Vector3(10, 14, 0), 53, 0.2f, continuous); + spinnerMotorSweepB = BuildSpinner(new Vector3(20, 14, 0), 59, 0.2f, continuous); rolloverInfo.Add(new Vector3(15, 14, -5), "Small margin, continuous sweep"); Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5f, 0), Simulation.Shapes.Add(new Box(300, 10, 300)))); diff --git a/Demos/Demos/Tanks/Tank.cs b/Demos/Demos/Tanks/Tank.cs index 6968b2d99..9df57e1c6 100644 --- a/Demos/Demos/Tanks/Tank.cs +++ b/Demos/Demos/Tanks/Tank.cs @@ -167,7 +167,7 @@ public BodyHandle Fire(Simulation simulation, CollidableProperty(8, BufferPool); points.AllocateUnsafely() = new Vector3(0, 0, 0); @@ -90,7 +90,7 @@ public override void Initialize(ContentArchive content, Camera camera) points.AllocateUnsafely() = new Vector3(2, 2, 0); points.AllocateUnsafely() = new Vector3(2, 2, 2); var convexHull = new ConvexHull(points, BufferPool, out _); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(23, 10, 0), convexHull.ComputeInertia(1), new(Simulation.Shapes.Add(convexHull), ContinuousDetection.Discrete(10, 10)), -1)); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(23, 10, 0), convexHull.ComputeInertia(1), new(Simulation.Shapes.Add(convexHull), 10, 10, ContinuousDetection.Discrete), -1)); //var sphere = new Sphere(1.5f); //var capsule = new Capsule(1f, 1f); diff --git a/Demos/SpecializedTests/TriangleTestDemo.cs b/Demos/SpecializedTests/TriangleTestDemo.cs index 5b4074d91..996830a87 100644 --- a/Demos/SpecializedTests/TriangleTestDemo.cs +++ b/Demos/SpecializedTests/TriangleTestDemo.cs @@ -130,21 +130,21 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5, 0), Simulation.Shapes.Add(new Box(200, 5, 200)))); Simulation.Statics.Add(new StaticDescription(new Vector3(10, -2, 30), Simulation.Shapes.Add(new Box(10, 5, 10)))); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 2, 0), new BodyInertia { InverseMass = 1 }, new(Simulation.Shapes.Add(new Sphere(1.75f)), ContinuousDetection.Discrete(0.1f, 0.1f)), -1)); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 2, 0), new BodyInertia { InverseMass = 1 }, new(Simulation.Shapes.Add(new Sphere(1.75f)), 0.1f, 0.1f), -1)); var capsule = new Capsule(2, 2); - Simulation.Bodies.Add(BodyDescription.CreateDynamic((new Vector3(20, 2, 3), Quaternion.CreateFromYawPitchRoll(0f, 1.745329E-05f, 0f)), capsule.ComputeInertia(1), new(Simulation.Shapes.Add(capsule), ContinuousDetection.Discrete(0.1f, 0.1f)), -1)); + Simulation.Bodies.Add(BodyDescription.CreateDynamic((new Vector3(20, 2, 3), Quaternion.CreateFromYawPitchRoll(0f, 1.745329E-05f, 0f)), capsule.ComputeInertia(1), new(Simulation.Shapes.Add(capsule), 0.1f, 0.1f), -1)); var testBox = new Box(2, 3, 2); var testBoxInertia = testBox.ComputeInertia(1); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 2, 6), testBoxInertia, new(Simulation.Shapes.Add(testBox), ContinuousDetection.Discrete(10.1f, 10.1f)), -1)); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 2, 6), testBoxInertia, new(Simulation.Shapes.Add(testBox), 10.1f, 10.1f), -1)); var cylinder = new Cylinder(1.75f, 0.5f); var cylinderInertia = cylinder.ComputeInertia(1); //cylinderInertia.InverseInertiaTensor = default; - Simulation.Bodies.Add(BodyDescription.CreateDynamic((new Vector3(20, 2, 9), Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), MathF.PI / 2f)), cylinderInertia, new(Simulation.Shapes.Add(cylinder), ContinuousDetection.Discrete(5f, 5f)), -1)); + Simulation.Bodies.Add(BodyDescription.CreateDynamic((new Vector3(20, 2, 9), Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), MathF.PI / 2f)), cylinderInertia, new(Simulation.Shapes.Add(cylinder), 5, 5), -1)); var cylinder2 = new Cylinder(.5f, 0.5f); var cylinder2Inertia = cylinder2.ComputeInertia(1); - Simulation.Bodies.Add(BodyDescription.CreateDynamic((new Vector3(23, 2, 9), Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), 0)), cylinder2Inertia, new(Simulation.Shapes.Add(cylinder2), ContinuousDetection.Discrete(5f, 5f)), -1)); + Simulation.Bodies.Add(BodyDescription.CreateDynamic((new Vector3(23, 2, 9), Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), 0)), cylinder2Inertia, new(Simulation.Shapes.Add(cylinder2), 5, 5), -1)); var points = new QuickList(8, BufferPool); points.AllocateUnsafely() = new Vector3(0, 0, 0); points.AllocateUnsafely() = new Vector3(0, 0, 2); @@ -156,8 +156,8 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) points.AllocateUnsafely() = new Vector3(2, 2, 2); var convexHull = new ConvexHull(points, BufferPool, out _); var convexHullInertia = convexHull.ComputeInertia(1); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 2, 12), convexHullInertia, new(Simulation.Shapes.Add(convexHull), ContinuousDetection.Discrete(0.1f, 0.1f)), -1)); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(23, 2, 12), convexHullInertia, new(Simulation.Shapes.Add(convexHull), ContinuousDetection.Discrete(0.1f, 0.1f)), -1)); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 2, 12), convexHullInertia, new(Simulation.Shapes.Add(convexHull), 0.1f, 0.1f), -1)); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(23, 2, 12), convexHullInertia, new(Simulation.Shapes.Add(convexHull), 0.1f, 0.1f), -1)); CompoundBuilder builder = new CompoundBuilder(BufferPool, Simulation.Shapes, 2); builder.Add(new Box(1, 1, 1), RigidPose.Identity, 1); @@ -165,7 +165,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) builder.BuildDynamicCompound(out var children, out var compoundInertia); //compoundInertia.InverseInertiaTensor = default; var compound = new Compound(children); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 3, 14), compoundInertia, new(Simulation.Shapes.Add(compound), ContinuousDetection.Discrete(10.1f, 10.1f)), -1)); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 3, 14), compoundInertia, new(Simulation.Shapes.Add(compound), 10.1f, 10.1f), -1)); { var triangles = new QuickList(4, BufferPool); @@ -178,14 +178,14 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) triangles.AllocateUnsafely() = new Triangle { A = v0, B = v2, C = v1 }; triangles.AllocateUnsafely() = new Triangle { A = v1, B = v2, C = v3 }; var testMesh = new Mesh(triangles, Vector3.One, BufferPool); - Simulation.Statics.Add(new StaticDescription(new Vector3(30, -2.5f, 0), new CollidableDescription(Simulation.Shapes.Add(testMesh), ContinuousDetection.Discrete(10.1f, 10.1f)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(30, -2.5f, 0), Simulation.Shapes.Add(testMesh))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -2.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Triangle(v2, v0, v1)), ContinuousDetection.Discrete(10.1f, 10.1f)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -2.5f, 0), Simulation.Shapes.Add(new Triangle(v2, v0, v1)))); } DemoMeshHelper.LoadModel(content, BufferPool, "Content\\newt.obj", new Vector3(3), out var mesh); - var collidable = new CollidableDescription(Simulation.Shapes.Add(mesh), ContinuousDetection.Discrete(2f, 2f)); + var collidable = new CollidableDescription(Simulation.Shapes.Add(mesh), 2f, 2f, ContinuousDetection.Discrete); var newtInertia = mesh.ComputeClosedInertia(1); for (int i = 0; i < 5; ++i) { @@ -201,7 +201,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) triangles.AllocateUnsafely() = new Triangle { A = v0, B = v2, C = v1 }; triangles.AllocateUnsafely() = new Triangle { A = v1, B = v2, C = v3 }; var testMesh = new Mesh(triangles, Vector3.One, BufferPool); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(22, -2.5f, 0), new BodyInertia { InverseMass = 1 }, new(Simulation.Shapes.Add(testMesh), ContinuousDetection.Discrete(10.1f, 10.1f)), -1f)); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(22, -2.5f, 0), new BodyInertia { InverseMass = 1 }, new(Simulation.Shapes.Add(testMesh), 10.1f, 10.1f), -1f)); } } } From 81c0b003396926738b9c1c135d80fd86adce46c4 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 13 Feb 2022 18:20:43 -0600 Subject: [PATCH 451/947] CCD docs update. --- Documentation/ContinuousCollisionDetection.md | 24 ++++++++++--------- 1 file changed, 13 insertions(+), 11 deletions(-) diff --git a/Documentation/ContinuousCollisionDetection.md b/Documentation/ContinuousCollisionDetection.md index 2be8b43cc..b099736ac 100644 --- a/Documentation/ContinuousCollisionDetection.md +++ b/Documentation/ContinuousCollisionDetection.md @@ -10,28 +10,27 @@ See the [ContinuousCollisionDetectionDemo](../Demos/Demos/ContinuousCollisionDet # What's a speculative contact? Speculative contacts are contacts with negative depth. They're still solved, but they don't apply any forces unless the velocity is high enough that the involved collidables are expected to come into contact within the next frame. -The speculative *margin* is the maximum distance at which a collision pair will generate speculative contacts. +The speculative *margin* is the maximum distance at which a collision pair will generate speculative contacts. Bodies have configurable minimum and maximum speculative margins. The speculative margin for a body is determined from the body's velocity magnitude clamped by the specified minimum and maximum bounds. The *effective* speculative margin for a pair of bodies is the sum of both bodies' margins. Statics do not contribute anything to a pair's effective margin; they cannot move.

The ball is heading towards the ground with a high enough velocity that the velocity expanded bounding box intersects the ground's bounding box. Similarly, since the collidables in this picture are configured to have an unlimited speculative margin, a speculative contact is created. The solver will detect and push back the part of velocity which would result in penetration. In the next frame, the ball and ground are in contact. # Do I need to care about speculative margins? -Most of the time, you don't. Consider a body or static created by just specifying the collision shape like so: +Most of the time, you don't. Consider a body created by just specifying the collision shape like so: ```cs var dynamicBoxShape = new Box(1, 1, 1); Simulation.Bodies.Add(BodyDescription.CreateDynamic( new Vector3(10, 5, 0), dynamicBoxShape.ComputeInertia(1), Simulation.Shapes.Add(dynamicBoxShape), 0.01f)); -Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), Simulation.Shapes.Add(new Box(2500, 1, 2500)))); ``` -For both the dynamic and the static, this creates a `CollidableDescription` from the `TypedIndex` returned by `Simulation.Shapes.Add`. When no other information is specified, a `CollidableDescription` defaults to a `ContinuousDetection` mode of `ContinuousDetection.Passive`. See the later section for more details, but the short version is that: +This creates a `CollidableDescription` from the `TypedIndex` returned by `Simulation.Shapes.Add`. When no other information is specified, a `CollidableDescription` defaults to a `ContinuousDetection` mode of `ContinuousDetection.Passive`. See the later section for more details, but the short version is that: 1. The bounding box is expanded by the whole velocity of the body, if the collidable is associated with a body. 2. The maximum speculative margin is `float.MaxValue`. In other words, there's no upper limit. 3. No sweep tests are used. Contacts are simply created from closest features. Taken together, this makes most stuff just work. Performance stays high since speculative contacts only get created if the velocity is high enough to warrant them, and high velocity collisions tend to have robust behavior since speculative contacts get generated. For most use cases, sticking with the default is a high performance and high quality option. -Sometimes, when the details of the simulation don't matter as much, it can be useful to limit the speculative margin to reduce the number of constraints that get generated. Perhaps you have a giant building that's collapsing and tens of thousands of bodies are moving rapidly in close proximity- that'll generate a lot of speculative contacts. You could probably get a noticeable speed boost by reducing the maximum margin on the building chunks to a small nonzero value with no noticeable impact on quality. +But there are cases where further tuning can be helpful. Including spooky ghost cases. # What are ghost collisions? In the solver, a contact constraint (speculative or not) acts like a plane. As far as the solver is concerned, the contact surface has unlimited horizontal extent. This is a perfectly fine approximation when the contacts are created at a reasonable location, but it can fail when objects are moving very quickly past each other. @@ -40,12 +39,13 @@ In the solver, a contact constraint (speculative or not) acts like a plane. As f The ball smacks into the plane created by the speculative contact, sending the ball flying off to the side. That's a ghost collision. -You can mitigate ghost collisions by either using a higher `Simulation.Timestep` rate or by shrinking the maximum speculative margin on the involved bodies. To shrink the margin, instead of passing in just the shape index as your `CollidableDescription`, provide the `BodyDescription` or `StaticDescription` a full `CollidableDescription` like so: +You can mitigate ghost collisions by either using a higher `Simulation.Timestep` rate or by shrinking the maximum speculative margin on the involved bodies. To shrink the margin, instead of passing in just the shape index as your `CollidableDescription`, provide the `BodyDescription` a full `CollidableDescription` like so: ```cs -Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), - new CollidableDescription(Simulation.Shapes.Add(new Box(100, 1, 100)), ContinuousDetection.CreatePassive(0, 1)))); +var dynamicBoxShape = new Box(1, 1, 1); +Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(10, 5, 0), dynamicBoxShape.ComputeInertia(1), + new CollidableDescription(Simulation.Shapes.Add(dynamicBoxShape), 1, ContinuousDetection.Passive), 0.01f)); ``` -This still uses a 'passive' continuous collision detection mode (explained in a couple of sections) like the default, but limits the speculative margin for any pairs involving this static collidable to between 0 and 1. Collision pairs with this static cannot generate speculative contacts more than 1 unit away from the surface. +This still uses a 'passive' continuous collision detection mode (explained in a couple of sections) like the default, but limits the speculative margin for the body to between 0 and 1. Even if it's moving much faster than 1 unit per frame, no speculative contacts will be created at a greater distance than 1.

@@ -70,7 +70,7 @@ By default, both of these values are 1e-3f. Increasing them will make the search In the above picture, the sweep was configured such that it left a pretty noticable error in the time of impact, but it's still well within the speculative margin. While the speculative margin would have been too small to detect contacts had the test been performed at the T = 0 location, it finds appropriate contacts at T = 0.28. The goal is to find a rough time *close* to the time of impact such that the speculative contacts created by narrow phase testing won't cause ghost collisions. That's a pretty forgiving problem. -Overall, using `Continuous` will be pretty fast since it only uses sweeps when the velocity in a given pair is high enough to warrant it. Of course, when the sweep test does run, it's not completely free, so prefer the simpler modes if they do what you want. Especially for really complicated compound shapes or meshes. (And preferably, don't have really complicated dynamic compounds or meshes.) +Overall, using `Continuous` will be pretty fast since it only uses sweeps when the velocity in a given pair is high enough to warrant it. If the relative velocity magnitude is below the pair's effective speculative margin, no sweep will be used. Of course, when the sweep test does run, it's not completely free, so prefer the simpler modes if they do what you want. Especially for really complicated compound shapes or meshes. (And preferably, don't have really complicated dynamic compounds or meshes.) # What other configuration options exist? There are three continuous collision detection modes: @@ -80,7 +80,9 @@ There are three continuous collision detection modes: Note that, if the maximum speculative margin is set to `float.MaxValue`, there's no difference between `Discrete` and `Passive` since the bounding box will get expanded either way. -You can also set the *minimum* speculative margin to a nonzero value, though this is rarely useful. The *effective* speculative margin used in a pair is based on the velocities of the bodies clamped by the minimum and maximums from each body. If bodies aren't moving, the speculative margins will tend to be very small. Setting a nonzero minimum could make sense if you expect there to be a lot of velocity introduced in the middle of a timestep (perhaps by other constraints) that make the velocity-estimated effective speculative margin insufficient. Usually, though, just leave it at zero. +You can also set the *minimum* speculative margin to a nonzero value, though this is rarely useful. The *effective* speculative margin used in a pair is based on sum of each involved body's speculative margin. If bodies aren't moving, the speculative margins will tend to be very small. Setting a nonzero minimum could make sense if you expect there to be a lot of velocity introduced in the middle of a timestep (perhaps by other constraints) that make the velocity-estimated effective speculative margin insufficient. Usually, though, just leave it at zero. + +Sometimes, it can be useful to limit the maximum speculative margin to reduce the number of constraints that get generated. Perhaps you have a giant building that's collapsing and tens of thousands of bodies are moving rapidly in close proximity- that'll generate a lot of speculative contacts, and occasionally missing a collision won't matter much. You could probably get a noticeable speed boost by reducing the maximum margin on the building chunks to a small nonzero value with no noticeable impact on quality. # Do speculative margins have any other surprising side effects? Speculative contacts are mostly incompatible with the traditional approach to bounciness- a coefficient of restitution which sets an opposing velocity goal along a contact normal proportional to the incoming velocity. That's why you won't find a 0 to 1 `CoefficientOfRestitution` anywhere in the library. From 70bcdf05f67b3ec03c2f1953da12b3f678b0d635 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 16 Feb 2022 17:43:10 -0600 Subject: [PATCH 452/947] Fixed two holes in velocity integration through which encoded or uninitialized body indices could become visible. Bumped beta number. --- BepuPhysics/BepuPhysics.csproj | 2 +- BepuPhysics/Constraints/TypeProcessor.cs | 35 +----------------------- BepuPhysics/PoseIntegrator.cs | 28 +++++++++++++------ BepuUtilities/BepuUtilities.csproj | 2 +- 4 files changed, 22 insertions(+), 45 deletions(-) diff --git a/BepuPhysics/BepuPhysics.csproj b/BepuPhysics/BepuPhysics.csproj index f053b0374..cddd98c07 100644 --- a/BepuPhysics/BepuPhysics.csproj +++ b/BepuPhysics/BepuPhysics.csproj @@ -1,7 +1,7 @@  net6.0 - 2.4.0-beta13 + 2.4.0-beta14 Bepu Entertainment LLC Ross Nordby Speedy real time physics simulation library. diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index 8130f9b6f..20db72b28 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -1249,39 +1249,6 @@ public static unsafe void IntegratePoseAndVelocity( Vector3Wide.ConditionalSelect(integrationMask, velocity.Linear, previousVelocity.Linear, out velocity.Linear); Vector3Wide.ConditionalSelect(integrationMask, velocity.Angular, previousVelocity.Angular, out velocity.Angular); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static unsafe void IntegratePoseAndVelocity( - ref TIntegratorCallbacks integratorCallbacks, ref Vector bodyIndices, in BodyInertiaWide localInertia, float dt, - ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocityWide velocity, - int workerIndex, - out BodyInertiaWide inertia) - where TIntegratorCallbacks : struct, IPoseIntegratorCallbacks - { - //This is identical to the other IntegratePoseAndVelocity, but it avoids any masking because we know ahead of time that the entire bundle is integrating. - var dtWide = new Vector(dt); - position += velocity.Linear * dtWide; - inertia.InverseMass = localInertia.InverseMass; - if (integratorCallbacks.AngularIntegrationMode == AngularIntegrationMode.ConserveMomentum) - { - var previousOrientation = orientation; - PoseIntegration.Integrate(orientation, velocity.Angular, dtWide * new Vector(0.5f), out orientation); - PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, orientation, out inertia.InverseInertiaTensor); - PoseIntegration.IntegrateAngularVelocityConserveMomentum(previousOrientation, localInertia.InverseInertiaTensor, inertia.InverseInertiaTensor, ref velocity.Angular); - } - else if (integratorCallbacks.AngularIntegrationMode == AngularIntegrationMode.ConserveMomentumWithGyroscopicTorque) - { - PoseIntegration.Integrate(orientation, velocity.Angular, dtWide * new Vector(0.5f), out orientation); - PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, orientation, out inertia.InverseInertiaTensor); - PoseIntegration.IntegrateAngularVelocityConserveMomentumWithGyroscopicTorque(orientation, localInertia.InverseInertiaTensor, ref velocity.Angular, dtWide); - } - else - { - PoseIntegration.Integrate(orientation, velocity.Angular, dtWide * new Vector(0.5f), out orientation); - PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, orientation, out inertia.InverseInertiaTensor); - } - integratorCallbacks.IntegrateVelocity(bodyIndices, position, orientation, localInertia, new Vector(-1), workerIndex, new Vector(dt), ref velocity); - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] public static unsafe void IntegrateVelocity( @@ -1335,7 +1302,7 @@ public static unsafe void GatherAndIntegrate(Bodies.DynamicLimit))); bodies.GatherState(bodyIndices, false, out position, out orientation, out velocity, out var localInertia); - IntegratePoseAndVelocity(ref integratorCallbacks, ref bodyIndices, localInertia, dt, ref position, ref orientation, ref velocity, workerIndex, out inertia); + IntegratePoseAndVelocity(ref integratorCallbacks, ref bodyIndices, localInertia, dt, integrationMask, ref position, ref orientation, ref velocity, workerIndex, out inertia); bodies.ScatterPose(ref position, ref orientation, bodyIndices, integrationMask); bodies.ScatterInertia(ref inertia, bodyIndices, integrationMask); } diff --git a/BepuPhysics/PoseIntegrator.cs b/BepuPhysics/PoseIntegrator.cs index 6e809b46d..9b490139c 100644 --- a/BepuPhysics/PoseIntegrator.cs +++ b/BepuPhysics/PoseIntegrator.cs @@ -455,6 +455,7 @@ internal unsafe void IntegrateKinematicVelocities(Buffer bodyHandles, int b var halfDt = bundleDt * new Vector(0.5f); int* bodyIndices = stackalloc int[Vector.Count]; + var bodyIndicesSpan = new Span(bodyIndices, Vector.Count); ref var callbacks = ref Callbacks; var handleToLocation = bodies.HandleToLocation; BodyInertiaWide zeroInertia = default; @@ -470,8 +471,8 @@ internal unsafe void IntegrateKinematicVelocities(Buffer bodyHandles, int b var existingMask = BundleIndexing.CreateMaskForCountInBundle(countInBundle); var trailingMask = Vector.OnesComplement(existingMask); - var bodyIndicesVector = new Vector(new Span(bodyIndices, Vector.Count)); - bodyIndicesVector = Vector.BitwiseOr(trailingMask, bodyIndicesVector); + var bodyIndicesVector = Vector.BitwiseOr(trailingMask, new Vector(bodyIndicesSpan)); + //Slightly unfortunate sacrifice to API simplicity: //We're doing a full gather so we can use the vectorized IntegrateVelocity callback even though the amount of work we're doing is absolutely trivial. //With luck, the user sets the appropriate flag on the callbacks so this is never called in the first place. (Kinematics are generally not subject to user velocity integration!) @@ -496,6 +497,7 @@ internal unsafe void IntegrateKinematicPosesAndVelocities(Buffer bodyHandle var halfDt = bundleDt * new Vector(0.5f); int* bodyIndices = stackalloc int[Vector.Count]; + var bodyIndicesSpan = new Span(bodyIndices, Vector.Count); ref var callbacks = ref Callbacks; var handleToLocation = bodies.HandleToLocation; BodyInertiaWide zeroInertia = default; @@ -511,7 +513,7 @@ internal unsafe void IntegrateKinematicPosesAndVelocities(Buffer bodyHandle var existingMask = BundleIndexing.CreateMaskForCountInBundle(countInBundle); var trailingMask = Vector.OnesComplement(existingMask); - var bodyIndicesVector = new Vector(new Span(bodyIndices, Vector.Count)); + var bodyIndicesVector = new Vector(bodyIndicesSpan); bodyIndicesVector = Vector.BitwiseOr(trailingMask, bodyIndicesVector); bodies.GatherState(bodyIndicesVector, false, out var position, out var orientation, out var velocity, out _); //Note that we integrate pose, THEN velocity. This is executing in the context of the second (or beyond) substep, which are effectively completing the previous substep's frame. @@ -538,10 +540,10 @@ unsafe void IntegrateBundlesAfterSubstepping(ref IndexSet mergedConstrainedBodyH var bundleDt = new Vector(dt); var bundleSubstepDt = new Vector(substepDt); - Vector unconstrainedMask; - Vector bodyIndices; - int* unconstrainedMaskPointer = (int*)&unconstrainedMask; - int* bodyIndicesPointer = (int*)&bodyIndices; + int* unconstrainedMaskPointer = stackalloc int[Vector.Count]; + int* bodyIndicesPointer = stackalloc int[Vector.Count]; + var unconstrainedMaskSpan = new Span(unconstrainedMaskPointer, Vector.Count); + var bodyIndicesSpan = new Span(bodyIndicesPointer, Vector.Count); var negativeOne = new Vector(-1); ref var callbacks = ref Callbacks; ref var indexToHandle = ref bodies.ActiveSet.IndexToHandle; @@ -557,7 +559,6 @@ unsafe void IntegrateBundlesAfterSubstepping(ref IndexSet mergedConstrainedBodyH //Unconstrained bodies can optionally perform a single step for the whole timestep, or do multiple steps to match the integration behavior of constrained bodies. //Bodies that are constrained should only undergo one substep of pose integration. bool anyBodyInBundleIsUnconstrained = false; - bodyIndices = negativeOne; //Initialize bundles to -1 so that inactive lanes are consistent with the active set's storage of body references (empty lanes are -1) for (int innerIndex = 0; innerIndex < countInBundle; ++innerIndex) { var bodyIndex = bundleBaseIndex + innerIndex; @@ -575,7 +576,16 @@ unsafe void IntegrateBundlesAfterSubstepping(ref IndexSet mergedConstrainedBodyH anyBodyInBundleIsUnconstrained = true; } } - + var unconstrainedMask = new Vector(unconstrainedMaskSpan); + var bodyIndices = new Vector(bodyIndicesSpan); + if (countInBundle < Vector.Count) + { + //Set empty body index lanes to -1 so that inactive lanes are consistent with the active set's storage of body references (empty lanes are -1) + var trailingMask = BundleIndexing.CreateTrailingMaskForCountInBundle(countInBundle); + bodyIndices = Vector.BitwiseOr(bodyIndices, trailingMask); + //Empty slots should not be considered here; clear the mask slot. + unconstrainedMask = Vector.AndNot(unconstrainedMask, trailingMask); + } Vector bundleEffectiveDt; if (callbacks.AllowSubstepsForUnconstrainedBodies) diff --git a/BepuUtilities/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj index 927a3b974..16bd3c2e6 100644 --- a/BepuUtilities/BepuUtilities.csproj +++ b/BepuUtilities/BepuUtilities.csproj @@ -3,7 +3,7 @@ BepuUtilities BepuUtilities net6.0 - 2.4.0-beta13 + 2.4.0-beta14 Bepu Entertainment LLC Ross Nordby Supporting utilities library for BEPUphysics v2. From 28d65a75fbc8ea7b2c37d0446377be9c79786552 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 17 Feb 2022 15:20:44 -0600 Subject: [PATCH 453/947] Nonconvex contacts made consistent with solve order of convex contacts. --- BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs b/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs index ef9fdcf0b..fdbc7b6e7 100644 --- a/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs +++ b/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs @@ -212,11 +212,11 @@ public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in B { ref var contact = ref Unsafe.Add(ref prestepContactStart, i); ref var contactImpulse = ref Unsafe.Add(ref accumulatedImpulsesStart, i); + PenetrationLimitOneBody.Solve(inertiaA, contact.Normal, contact.Offset, contact.Depth, + positionErrorToVelocity, effectiveMassCFMScale, prestepMaterial.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref contactImpulse.Penetration, ref wsvA); Helpers.BuildOrthonormalBasis(contact.Normal, out var x, out var z); var maximumTangentImpulse = prestepMaterial.FrictionCoefficient * contactImpulse.Penetration; TangentFrictionOneBody.Solve(x, z, contact.Offset, inertiaA, maximumTangentImpulse, ref contactImpulse.Tangent, ref wsvA); - PenetrationLimitOneBody.Solve(inertiaA, contact.Normal, contact.Offset, contact.Depth, - positionErrorToVelocity, effectiveMassCFMScale, prestepMaterial.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref contactImpulse.Penetration, ref wsvA); } } @@ -277,11 +277,11 @@ public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in B ref var contact = ref Unsafe.Add(ref prestepContactStart, i); ref var contactImpulse = ref Unsafe.Add(ref accumulatedImpulsesStart, i); Vector3Wide.Subtract(contact.Offset, prestepOffsetB, out var contactOffsetB); + PenetrationLimit.Solve(inertiaA, inertiaB, contact.Normal, contact.Offset, contactOffsetB, contact.Depth, + positionErrorToVelocity, effectiveMassCFMScale, prestepMaterial.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref contactImpulse.Penetration, ref wsvA, ref wsvB); Helpers.BuildOrthonormalBasis(contact.Normal, out var x, out var z); var maximumTangentImpulse = prestepMaterial.FrictionCoefficient * contactImpulse.Penetration; TangentFriction.Solve(x, z, contact.Offset, contactOffsetB, inertiaA, inertiaB, maximumTangentImpulse, ref contactImpulse.Tangent, ref wsvA, ref wsvB); - PenetrationLimit.Solve(inertiaA, inertiaB, contact.Normal, contact.Offset, contactOffsetB, contact.Depth, - positionErrorToVelocity, effectiveMassCFMScale, prestepMaterial.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref contactImpulse.Penetration, ref wsvA, ref wsvB); } } From e483e0ac3318e9fe3924c712a207c73b54b445cf Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 17 Feb 2022 16:33:20 -0600 Subject: [PATCH 454/947] 2.4 change log. --- Documentation/changelog.md | 72 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 72 insertions(+) create mode 100644 Documentation/changelog.md diff --git a/Documentation/changelog.md b/Documentation/changelog.md new file mode 100644 index 000000000..6fa7142c9 --- /dev/null +++ b/Documentation/changelog.md @@ -0,0 +1,72 @@ +# 2.4 + +At a high level, 2.4 was a solver revamp. Data layout and access patterns were significantly changed and dispatch logic was reworked. Substepping is massively cheaper now, and even simulations without any need of substepping will still benefit significantly. Whole frame speedups in excess of 2x were not uncommon in benchmarks. Scenes that were especially difficult for the solver in 2.3 were observed to be over 3.5x faster in 2.4. + +## API Breaking Changes + +1. The library now depends on .NET 6. +1. `Simulation.Create` no longer requires an `ITimestepper`, though one can still be provided. `PositionFirstTimestepper` and `PositionLastTimestepper` no longer exist; the only built-in `ITimestepper` implementation is the `DefaultTimestepper`. This is a result of 2.4 moving entirely to an embedded substepping solver. +1. `Simulation.Create` now takes a `SolveDescription`. It can be used to configure the number of substeps, velocity iterations, and synchronized batches. There exist helper implicit casts; for example, passing an integer will simply use the value as the number of substeps, with the velocity iteration count set to 1 and `FallbackBatchThreshold` set to the default. `Solver.IterationCount` property renamed to `Solver.VelocityIterationCount`. See the [substepping documentation](Substepping.md) for more information. +1. `IPoseIntegratorCallbacks.IntegrateVelocity` now exposes multiple lanes of bodies in SIMD vectors, rather than a single body at a time. It also exposes two new properties, `IntegrateVelocityForKinematics` and `AllowSubstepsForUnconstrainedBodies`. If `IntegrateVelocityForKinematics` is false, then `IntegrateVelocity` will not include any kinematic bodies in active lanes. This is convenient when applying gravity; if only dynamics are ever invoked, then there's no need to check kinematicity prior to applying gravity. If `AllowSubstepsForUnconstrainedBodies` is true, then bodies with no constraints will have their velocities and poses integrated for every substep; if false, they will only be integrated a single time for the full frame duration. All constrained bodies are substepped if the solver is substepped. The vectorized nature of this callback is probably going to be annoying (and/or confusing) for a lot of people; sorry about that. Maintaining a scalar callback added too much overhead. You can build one on top, though! +1. When constructing a collidable description, the overload that takes a speculative margin now means the *maximum* speculative margin. 2.4 uses adaptive speculative margins that can shrink or expand according to velocity. You will usually see the same behavior, just cheaper. If you want to match the old behavior as much as possible, set both the minimum and maximum bounds to the same value. Leaving speculative margins bounds at [0, float.MaxValue] is a good default now. Note that there are some new implicit casts relevant to `BodyDescription` creation that can make things shorter, including just passing a shape index directly which defaults to passive continuity with [0, float.MaxValue] adaptive speculative margin. See [continuous collision detection documentation](ContinuousCollisionDetection.md) for more information. +1. Statics no longer have any configurable speculative margin settings and do not take a `CollidableDescription` in their constructor. They still have a `Continuity` field if a static should a static need continuous collision detection to be enabled. All information related to a static is now in one spot, stored in the `Statics.StaticsBuffer`. +1. `ContinuousDetectionSettings` renamed to `ContinuousDetection`. +1. `INarrowPhaseCallbacks.AllowContactGeneration` now exposes `ref float speculativeMargin`. Most use cases can safely ignore this completely, but if you find yourself wanting fine grained control over the speculative margin, that's now exposed. +1. `IConvexShape.ComputeInertia` now returns instead of using an out parameter. Similar changes applied to things like `Mesh.ComputeClosedInertia` and `Mesh.ComputeOpenInertia`. +1. Some callbacks that previously had `struct` generic constraints now require `unmanaged`, like `INarrowPhaseCallbacks.ConfigureContactManifold`. +1. `BodyOptimizer`/`ConstraintOptimizer` stages no longer exist, and their profiler entries have been removed. +1. `Solver.ApplyDescription` no longer requires a ref parameter. +1. `BodyInertias` and `BodyVelocities` renamed to `BodyInertiaWide` and `BodyVelocityWide` to match naming convention of other wide types. +1. `IThreadDispatcher` now takes an additional `maximumWorkerCount` parameter. Specifying a maximum worker count lower than the `IThreadDispatcher.ThreadCount` may allow the implementation to do less work. In practice, the `BepuUtilities.ThreadDispatcher` uses this to significantly reduce dispatch overhead for low job count use cases. +1. All body state used by the solver now bundled together into `BodySet.SolverStates`. Includes pose, velocity, and inertia. +1. Constraint type batches no longer have a 'projection' buffer; anything loaded from it is now recalculated on the fly. + +## Other Changes + +1. Added `CenterDistanceLimit`! Useful for clothy stuff. + +2. Added `AngularAxisGearMotor`; transforms angular motion with a multiplier, somewhat like a gear ratio. + +3. `RigidPose`, `BodyVelocity`, `CollidableDescription`, and `BodyActivityDescription` all now have helper implicit casts to optionally make configuration a little less syntax-noisy. + +4. You can now get a `BodyReference` or `StaticReference` from their respective collections by using `Simulation.Bodies[BodyHandle]` and `Simulation.Statics[StaticHandle]`. + +5. The presence of a kinematic body in a constraint batch will no longer block another constraint attached to that kinematic body from being added to the constraint batch. The velocity of kinematics in constraint batches are treated as read-only. This will improve performance on simulations where a kinematic body has a ton of constraints associated with it. + +6. Broad phase dispatches combined. The long-waiting broad phase revamp is still waiting; this just reduces dispatch related overhead slightly. + +7. Code paths dependent on trigonometric approximations have had their accuracy improved by a few orders of magnitude. `AngularHinge`, `TwistServo`, `QuaternionWide.GetAxisAngleFromQuaternion`, and orientation integration are all improved. The improvement in orientation integration in particular helps avoid contact drift and helps integration with angular momentum conservation. + +8. Constraints in the fallback batch now have sequentialized execution, rather than using a jacobi solver. This improves simulation quality in pathological cases where a single dynamic body is associated with tons of constraints, but that situation is still best avoided. Anything that ends up in the fallback batch will cost more by virtue of being executed sequentially. More information and potential future improvements here: [Fallback batch improvements · Issue #162 · bepu/bepuphysics2 (github.com)](https://github.com/bepu/bepuphysics2/issues/162). + +9. Contact constraints now solve friction last. In simulations that don't let the solver reach an equilibrium solution, you might notice that an unstable stack of bodies exhibits more tangential jitter and less penetration jitter than it used to. In a simulation that allows enough time to solve the constraints, there should be no visible difference. (This change was motivated by the fact that penetration has a corrective feedback loop via depth, while friction is open ended. Giving friction the final word slightly reduces drift.) + +10. `VolumeConstraint` had a warmstarting jacobian bug; it's fixed. Should be higher quality (and stiffer, if configured to be stiff). + +11. `Hinge` and `SwivelHinge` never made use of accumulated impulses, oops. Should be higher quality (and stiffer, if configured to be stiff). + +12. `SwivelHinge` no longer has a unguarded NaNsplode codepath. + +13. Fixed a bug in `AngularMotor` that used the wrong inertia. + +14. Guarded against a sphere-cylinder division by zero. + +15. Fixed a capsule-cylinder determinism bug. + +16. Fixed a triangle-cylinder determinism bug. + +17. Fixed capsule-cylinder contact generation bug. + +18. Fixed cylinder-cylinder contact generation bug. + +19. Box-box now has a bundle early out. + +20. All triangle-involving collision pairs now consistent in triangle degeneracy testing. + +21. All triangle-involving collision pairs now handle collisions with normals pointing nearly perpendicular to the triangle plane much more gracefully. + +22. Fixed a bunch of other tiny weird triangle collision bugs too. + +23. `MeshReduction` revamped a bit. It should catch more boundary bumps, and it no longer tries to do a quadratically catastrophic operation when the number of triangles being considered is large. At a certain (extreme) point, it now simply doesn't bother with boundary smoothing at all. + + From b61efa6e3ca0cec38c97605d0ab8ab6d61b43556 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 17 Feb 2022 16:34:52 -0600 Subject: [PATCH 455/947] Readme link. --- README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README.md b/README.md index 7a8118830..d651038c2 100644 --- a/README.md +++ b/README.md @@ -49,6 +49,8 @@ By user request, there's a [discord server](https://discord.gg/ssa2XpY). I'll be [Contributing](CONTRIBUTING.md) +[Change log](Documentation/changelog.md) + [Upgrading from v1, concept mapping](Documentation/UpgradingFromV1.md) [Packaging and Versioning](Documentation/PackagingAndVersioning.md) From 6b21adc7f30c775afcc3e8e28e8f9e3d9d221a75 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 18 Feb 2022 12:12:56 -0600 Subject: [PATCH 456/947] Readme update for 2.4. --- Documentation/images/youtubeLink24.png | Bin 0 -> 58527 bytes README.md | 4 +++- 2 files changed, 3 insertions(+), 1 deletion(-) create mode 100644 Documentation/images/youtubeLink24.png diff --git a/Documentation/images/youtubeLink24.png b/Documentation/images/youtubeLink24.png new file mode 100644 index 0000000000000000000000000000000000000000..cb82c232c098cbd783cfa2fca48bd87d517583be GIT binary patch literal 58527 zcmeGDbyHi>`aTW^C{Vn`o#HJNcQ3_?6nB>b#ft=YcP(Dr2@u?!7Iz5lPH=}{znt^= z&hr+Yf6kuCB$>%f*1l)$`?@b#5z2}(Sm>na0000>_M4kzNRY zLvU7=`3k5SCqIOLd1ER5T^sHsy5mFEP|knwwPgmY%Y8ljmRd~n@gE{nU?g&zq1Tz=QG-$2e5ij;2S7`*2t8ab zi7Ecg46j~Y=9{wVuS~mM6R*eE@}$)MxMz=E<9I1!$D`?7mJIu==~-B`H2=H)SAqXk z;C~hPUj_bGf&W$De--#&1^)k30oXb|Ye@KiuY)tpsd;ijLTa(^s>GpXh7zcAA`f5u zuyHKG4F6dj%&4RQaoV8}Ur#$tDzRD-<9=x2%Z;pivw-jl>G6=Xr3K%Ae@BCC)k8`( zU#>QgBnT8H5!*J+l=uCT5+2VV5Uil?@VB4aYOP)^?6za6;V@ygFy{K#fA1ZZLm2B# zMrdI_)xJFo_N%wFYp^^S9A8OK?ljxIT(Qjd9I^P0iPI#DVgV@L&Ji52iKt$68Zlr_ zDX}ajYR+2zBak_xx+?ZsZ`nKyLydQ}^@%~o;19wh}9EyY;=Isk|LO1!!a&p>;m-nNxC{)g)#K5G|kFfc@6Gr^ON5pIF_TV7cB;*_hmPOm1^RL&H6mxTL}qaUsHmYB%H5zh8oIk{tK%n9HX5s||CotAQEK%bJ=tE*Y_~3w5VfzK z$!U{ErzHPK(zUCU&3#uTl>PVGLe2CW>SR`);Wer{XRk%f*}U6e-9-DtjnIROM zj)2rC+m1TAXcBM z--9_vpN|@v8;f(7% zpgOwZ$mn^e$yuF=kOrj?QOD9pJaHv#p(s*~4}fF}IiYc*TpSO{M8!6|AIk)ud$@hr zd9&QUvaNnrKn(yJoY+BXnH!^C{7Y>=SZsLq2geW+5>jNjpYXVSHb`s{^<&%0SyqYm z8Mb1(Y?ccuQi!}T(rCAT`u?f!x9{1S_p7MaTqEk~qno~hO-qV4J#;$__V_VtE>vPT z+DhIhJF$JG!(Jw0KBg;~zwr3VV-})Hxi6}v=qPUZ;IVH=pwD&-?^pTTTFq~aXAYvO zECI!ytlR)LXCXH(0F_y553O$F!H%N?2FeC5ck@6Cstj&U3Ag!o`yO~ z?r8tPYNw_g-5fg!VTe?1ZIpGX=9K*WS9{D5vrGeJ2h7WCZbGlm zJodA4k`G7jh+4ARs(m`_C5?jhZq!zJzncC!$C23}#Dx&#cy$TdKv^HpUyG$==w-oQ zQu(V~WnB~vCRJ2A1_{Sn!PQw@87FD2LYmMtQO!EOPT;2H9A&mtF|XMGH(+1@BUkrtDyV$K}5l8H8q zf9ZAf+fD5%wBY#Q0z72&83&P>5~Z$D%%%ywa09f6`Fm~fyg^3t#lZ0H_)yZG@WbB` zEd!4c^VGFpi$#0}ZYLdV!eaK*e_M{GFP~fJDO1P0QBjSbk{wfF&TctwtgSLZ#yAfn zLZJ!1?|oD%iqF#!o+oDJ6Zud0qf7C+Vvo13>rWB`VgT)N2x;NrrJ@4-04m~Kc7U2s zAK+>Jt(MWHjcCJbe96nr<${|i#pAf;N4NDG_nftoFc4el)c&1|345tguad}3{U|1Q zWnaf=(QxbP!_4W+PQfBk^pniYj-rZ6o{@dwbit=~arGYsr>n5n{hK(h8NQ)^=g_#Q z))yV3n{}trALGXaCMGpdNt^qNcceE6k|jp?^4|Ja4qZA2ccgyyvWE_=-wP~3$f;%g3gai$=K7MMUhxCHV_qF#a-c_S6OK4PNWM4@*t z>WDs66~N(>pV{n6hRS>S@h|nJ3EE{t6Ldm`K7h^&J+9?+sg2^cQt?soNnHFOIg=3- zsD5~b?A}zNvDGWpd3vUi z3jpuW5G~AZ8M07O0`zBr$yv;9zt%_}^0BB&V|)rv2@DABCgYz;9@{z~vf`!JcA3>70R7z{a?5>$TaPMjJits_0J~80E59Fov ze3r%o`jymSw&zy*^rN^W24D+o9e-9Zy?l zE8#oD5b?XDq`DtFfJ%e-PMrjBpp@GWhOd#g9wttq|0-7hg!MG0S#sOEy@lKP>^ir^ zBcSwpbhD6FjEikoi$}esJ8A)1BsYm=m*v<7N05lo*rtSkOF}!;T7UX%(R=?2b4Ef~ zIQvIMWH9WVy`{mN^*u=lIWEPI#a6#gU8ooL{neAR1FXGGKlW=}oxUGz{)pAQ$4fggWfc)qEuL<@=Et9R`6DjpdD&883DjFUl+I>mRGmBQfir9^G3~9pH-S`K zxLbn3A^&cn0(3sBC@t@~|NF#?`~`Tcmr?)zb2mvVW)Mvr9f&H3Nk!rGnztROlGIm) z%YefI3?nKCIU@GQQ5ULb=-H2@lBxc8@wKAuH27!OWWLaT!kv=Yduf4;jt)T?D>mh1 z976<9CNfTW_LYk6mJtqfKJtr^NZn$jH?!Yc_KSc+Brwhwq>O0x&sySoy%0Bwf8Li< z3SIN}#MRi{_snXiql>3be7!b&RQSCP-CV+cq>QOKKf?y56gQrpI(Ve6)th2vhq__Vl3Dh*RV`^lkS zI`kb#MYb@74cx3xt_OJvO~Xn}ezgSDfUBaP#3q@8%dJBKGe&BAA^Rv2q7uZ2NrySG z9%>WLc2OFpTGJJ^CxW1lgO`-tFj=ZDBetsFja0-D%sb21(}(^=Kh<@=yl z*{%UGSW~hrjymEiQ9#L8{sMQ02o*)lonoslvcNUO7d3NV)ky3X5R<&$Uk3Z9>PInC}c(ckCU$ z5f{C*B}7=|<7st^q!u;iYM~r_Z4yO{I@PTmk#A?KaO-mM>ThCTW|fSLWUE)#)G+hohIps6SI81K_4{^ zVNOz+uEwtdZ|Za$FmBkjxjSls1*tZlMd~>{DL{C!_wOr^82XG!ZAy%y7@6yFBJE|X z7b-Xo#AIbzGWcv}+I947dxC@Mgg>2q_z-4%b(l(gB~aKMm`_7Ck3-H7RfJ0Qk{e+0N(3bzH=a!_Cvp7C;HQ z5AwBRF~02|Rl&kH+cO?r&&?5^;)!^R6U7-akkN~0o3hiKHus$rXq~YlmHIc>bxLJS zVe=vp7M&i(o4~4}N|5AS@{<{#_S^idCWM_(-DC>=_AU(t>*gi<&k6%IwSb`Dd!^yu z{c}BL+aWwN&eAFl2I*rk9}o&0#D3-k-!~4)X4PEfb2}_)xNh!Q;JKP81f{AQrm)IX z^;WR^b$sBiJ?)U`#1}y=*voArd?~YfN&9&I@lAHcmx1*lq+i$R5(CZTAw7VKm$|_S z5lXF1H~?U{63^~~12rUsWEdEMy0`M!(6Sk}7yP}4VT#)^k=8pL%GN$5GJabFUEiMo zS0T9|1mceO%&K-|xbNRj8G%&+Yi=I^B|8n)i&#Mxz^!jF9npDqJGnFCJ~zpCozHt= z$;_OpYWiiLPJcIy?~Y9Sy*NT-s1)J2D5Tc}4Hcj0u~dl-j~-?Jq5@>k+-h|b+TF`M zeZ4REbmgaA<839aTh6!5CpO&5eRgTym1nv=;Nv@Rg41LbV{I%Mm2tqg7sCN!<%w%z zPfr0%GREv^| zA%j%i_*w=B2>Mx~)U89O;nS$v*|pe%EeN^qr$U^n2}2lpLxJ4;gapU^EH4gd5JCG* z3M#nmHr9~wwn@^M=jaOyw!~|dfv17%VMEi3_t~nOh|^5Woxbbg4F?vfY_f6()H;Om zf?4BAk;2Q+2g1^{e!C%ZC+q}C=5sw-8u;-Qj|3Eos;wqm5Zc?}iS2znMO5r7;x@E3 zTd%GJ6I^fQ_8TTWn-HtY5dqE4!mN`J6krbz!T<7GPc=dVc|rf z*8@9YB|dqg zr{A4UEkZy$QS{b>%c5}|E=}*J&H5v7o7UQGB-SZU`BLq{@D*R$$vnw7QshYBmFu!Mc1m#wb#Y>wxBs(rA2Ex+9u zIQDl#RgwJMX4kOR~Ma zpueT$3C+QVhX$#wl=Lj-&%?Xf0(HL6I|U?nE6?C;5tM}rEQuvl=gHl8dEvq@7?RTc znEb%O%BHm_Lpm~84+dLvxxsQ(Ex&@YWtv~}B6)}i{_15)=Gg$p$+*z3A70fyLf83k z<=@O5oX>rgS_?wHbocG!BKD664~GT>A+b+` zVYR4e-R4%GkSld>;aw_agrT6f&(t(O)5!55YhBSGngbzn-axx-L-xj7vwx2$J7==i zBaWRXF}7aE?0O4|-p4dMgO9grJ6hB<_NQ}YcLA{xK32UB?dy&b zSDV^~nWT(U6OSF_@N|D*pb=%YdZ?`*%ar4@Td1?@>x}72eROt<7NkO=5aH8cR#lTss7&4$oVp$S=|&9s*U5pneQp&!kg`q&BFY@ONjvWa+^zS27X^K8hXdRC(UT(hnIIDKm_4RHkY&trj zpfMZjsi<7kS+FJoq7R~K=TDU9^*XO^Qx~kA>~)ZN@Icp zVfe}Lw^Nr}+&ZYUTwP?Gr6=|rI+9awyCz>A5FWx?)+jzMd?gIFoGHdCBAZJc=IsTv zPM$0Y{y2Y_w!GF96^7&cTL=&pn>ZysJLin8*Y>%kZFSu4AV!|BgxhLG7pj@}dVqy* z((t#zcIqbJG25tZDRrNDY=mWEWL3?4=+10_t$&>7B8p1-kDHx>3&z7_!S||?(mX%u zdV7f7(3VTZ8SF_!b-1Rku;QZ8VP36Od`w6e`8|Ugek1nnVrR$1Url-Wp@G=X1Ak~9 zc(da3a}vy^$_Y+OUT-vyH2L?8ndceYk8Z_RX)+kxT6gzkQa%sE=|9lHMJTE;H$eq` z*Mrih5%+~F zArY)LzChrSXRU&V_KDTx5t-xo`P9PVm3LbWShh#8$>s%T#Eg_X#n5HY6%--YjR0-; zWCzIHocOqE_EYg=h8C`W0ZzjrRHY&mWzZTbq>6S3WY zYLL3Z`$6L2Vm!$i_44dbgKCY6{ldsE}j)Coi`!&P)&=-02uV zyXP1|>Ix1mXKMqO-a@O26|^WY;%hS=x00eA9;=l#-1p2tkW;8TZbQQj)T5Ql611|3 za#%CFyAbiomO>u{ZljuD8)t1?D*94puJwEz_bDQ(K{Tn#>AuX|-{ zUx5a;mnttq{}8zC8iUKUudxYMJVvr^4UBw6n}DF~OO!VjQTFcyUzOoQH_FzpJeqn3 zu}J;H$ZyJ?{gW6rEhluGWq6_5@73J(Cn$qN-Cx^|N5!u7a7*Lb{qQ{((D0wzeVY(h z8GKefk1@WJHZ@2Z<5Zfo1l)=*bpE&oGNo}Mx*=Fc%V*9IB zF#ZP1f!$;J80!eKtkiRNDCeD>PaowcAMQXRObGEHD97whv)r*ll3A>4)WYr_|6?bC z5|bE0DLX?!4dGl65qtTSAqWs0M%tV+u$X{tLz&Oi&k#rGw{&w1B{=8oQnvq3z!Fja z1fb&Aig<5#l;RVA{T~G4gza4eyJ$BDo2%7DUkc?t*r#)Um%R>OeZwYJ+Lpz{$o6y9 zSO39@5)S)(x`~6W@U4rjgMaR1B{Vn`+Tx^)r9o%8#Ta@g@PDfD=9H}DB&P>-zF?!ve+3zbh1S*b}zq>fjZp+5Nk(V4v>opq} z*-wOkoSs_#5s#YutC7^g{(2^w>f`q>KU-^&7$|Y!b@w7H)}fa=Gs)R=VFW~wP($GV zT+vgJ-nH}a&NG1+a?Z?bkX7h;88OchvgoaygZuY`MvSsU*GYhPm!L=NWWG%40bs<} zk=trkMfw&Jv#Ej3}(bS@N6>rI-)z|=pM%Uw$4mZ}Q$J`}uqui{r#F~pDSBQOz8)$~QA zA3lu#IZ4cV$Dd)r?PbtlEyoez(jjUkPvm4X!aKSb zNLNbVzInpO{HGS78nV6J@+Nup>Up&Fpnx_{oUd!4-PWW^rgvJIxQA~$JTD6c4t6#- z7d)8X94n_(WhqH%|D!2%07b{J?hLN^_D@DxQ~1}IYurk}I=n~iF4x}N+5k(@I+;r% z$xUYWl*5QTDsq}EPj0;&N5xYlJlQf{`=Gu)Vc#;1vo(5kgJonGSKQ9F4JU4esT$8H zeT*G{q~>nqQZok%9!tZd60V~P^dm%Uh++j4g;UDmQ-i{I@BPV=tGv#Ubc*KLce|2K3-QZRJr3yD-t(7#-Lk)*7a)$ESl*k#IOghF)_XXmUN=*FLYk^7IVy&3x0WC(f1FOrnn13p1X{i!4Y zx6R74Y1)m(4;dOS3^9U3VN4*hiL%?CHzHn`wU%~jA~IRM{}?`wjjssizCH{>=fe9Z z#O|u#{kstP8kMI4Gx{@-SUR=u_N$+S;WT4``udDi_P|7~)%s_7>k6^t^6@6B+E;RN ze+|?x;u0Otdw;8|58m$^w3zBNPJr@}Ln{RVM=9zzVh7RJJ&dK0s(@$HJG{7#h%c1K zqj@|%+ix>@Hm{UQOkSK43902e#A){-O-7=7gy|J~hFTOea8j{U$LFE{w!3_Z(-Nq1 z>T@Sd*3M3LBi{3*ar>q9@u?2{+_HS4qxWoR^{eT}hVgKva$%!2QNP*op)Zgsw|5p_ z>8p3$2CJIEt2zQdvjnQ~^IFo<@7QAsxZHmbnbvR0`%mQVc$pQ|)peHcoG|X1dG#d# zDRyVNyUfc&n&4TxRrwR=FKapbJNuGvS>AbKp^A(T7`Oo;YE^NFzp6bd(+3q2dJV zWjUveRcgNT($wrfE5RaX&D?(HB~I_TH}`%&#&LmfWSrH3wUp8@c6;b%ewC^=>OYWy z)$O)&OLt3QrMg|oO5m?HcfZ_pLU7y)G_p4^iO{a%VQ78m&6$DhiJmhmJP+}gtqVZa zh_$1BRp@T;V0Ocux*I*7)Gr`;g!vbk*Bz^+AM8R{pe%45n#bh}!cEmrD~S@sqd~@T zDg9Eef+Q!KCpd(^>c{rgtR553My#8Y`uXK)8;tBhPwbg5$9bw1fgEHTeOS{x+i*n0 zHu~ZUO$2x8hI5)NMG}k%!&P~K^s@2yvL7`}h|SodRnRXZ*+_g0PD0D?SrE_B0^pt0 zfmS)0aOb4!aojH~j9%pB;OO>vjJQy)RpOsAGrn;5LHk>!t&-&clpa5zcz3r$5M!7I zU4M0%EzC`9q~whK`fp=to~HFvBtY0D#Yy;9eDyZsGQ73Ym{GBG3-<&bON{B5^v zj8dr&U=B&SWA0`5T%8q*wbppCNBx2*Ny1PmcQN8 zi=I{O=XmKcfF86}(lc0e@1rJ)AKQO&0Qr-H9w=SLXB!@3aF>TJ9)4NsC5(5{a08a% zGoD@-5y(HV3mX;j{K?}}1e)ECF46qmIW)qs5YzPtpRJnLo!$JAvXPKNoYd{=s}(N1 ziud1tg(m?p15*DF4;+&10|`F*p81w^{22PmCY@ychtBU%;~K)%VT(@k z@BtJZTzxTx%4PpFAQqMdlzL5)!6b3HVBr%HJsf#pqAAZjFlMb7qB3h1H_?_Cfr0E-N#P||x%V;TH@Mwv^6CZ5rhF;+> zYKOQ#F_j<>e}3~Esm)i(1RbqJMF$TW#A}8Ay*Xs$vhTXN*t9fq9Ipmdb-)7VjUKq+ z`cVJ+)H0A)G(KSW*l4QWA;siJw2F}K*+#!goS9h$>m&Wh$x6tl8!syDKd`|fORWh? zBy)**CnT%1g^aXGr`=2PIFPbau3XwQb=2;-AybpFTE0#%WOXTlUcu)PbNHS`=@eUI z%dy-pGND66-Y4ZIW?qzH(ofR#>*F^YH8ppM-{;l$rGq=gLk8m%zPmyW-{o|Y!cuy& zx2{fI7Umfln^FV|)aM1_*a2pt7Jmyf9a`6S@B4NacI$~qGAXmr^YCo~nxozU|LwYE zdk(i~>&KmzV2S$79z@xDAFku6&ck>sWP*<2IhDZ>rnlYD+BK@ZD!Fh66Bpwl_RZQj z?WBx)om=_P3ANyeshLU9`@^f!v-XGUB%QvA&UPTk@WRpV^PaSYTHx5yN9s=VC<_D6 z8}g@Nd8&M$Nj~8!1J(geu;qwVNfy0LKuVd(ekE4BGSTSY**u@S(P-fYnNp>B5TB8H z`yFAWwnbdjKe;@On^MF=J zpL@}rY`1c2L6-h~teM7t&iQ`^*2xBd{630R>QyVm#K)6P2x-W5qLzLfo+naYtDDy{ zFs-kQ`ovT_B!Sae&3fbZko(z?hOumCOGIEoQ}}R%YR7)m+O*>unr=JSoNDBWb2xwY z&qibLw`yd3BP($G`K~km-MS`IE}LkDa&>rVPtdr*`Hb}_f`GUoXl+A=hkfv6>*}l` zC9ONZJVZ_xZkyOntbG>O(vtYM!2|LgFzX^>DL`r|PowpkZ>~`js+XvrV<|Yzv_l`L z#zksFkw7sx?^`i4h^0u(eV2A~Xi8u<>pol4U_;c-PIF&WtPz`|?i2e6sPK%UZTopM zyuRy$M__tU=()kFq_q5W-L+;nRgw8gKWjvzKq*NBrSpACI0{j8tLY+J*F$^T(;8CF zFKqm~wO#pDM~(yFPpW#m@6U{M0JEbvtkw__8BOJwUIT2PmWkJGD9&2W?9tzs+F0I) zZmwQBL8YLb!tn{=8%522i=l34eX=M8i=KYRQ?JVFBn@JS{U3DNJ#=t60x1a@_vLhy!%8)!j(LfQ{H}yf6t18BMfg<}=AxjsAn8o# zo0qjVBU8sTluTSQL=xg5c9tUJYbm~O6Oujlf!4Xkzv3YC7f51b20}RQ5C2~E+Ln^` z*K1(&qiHA$mcV+6vRX!RSwKG~hn~#M|B~X`YD)u!q_x0P)&oIZA4Cx5i%Yt-6-!AXQ9NeE!?+7Sb zYyG;N>uma%Vo~hS`dHWlt?004|gBLWbz(U!GXXjUK}IW6JqxpojvBH-*4s2sZ^|S}j^8q~ zc75+CU&?GYj?N1cpX(`!`z6#QP}?-}p;P_Tz6j;{NR{u8=kVJHS9+1zNm_LgK`mq? z{l84|!7$jCCX3KmX4^^*=roRA!Wq+XmLj4(`vmUT9Oi}Ur;#E4BJ#Q=l;!+K++h+Q zj31ny`$5yZ2=se*_>)fUAV=tM>AUq$?q6=bMXXYVMOgz3kxgHLj%SQ#tS#Kr=ZZS+&GuSE0h1 zwaIxP!zgP%+qniZ{l$-~dKQ&y-ptX3Me=X+)p*D73FZpxw_Ohbv}p3cm-DfCjB1w8 z@3phKt&M?`FQq2#+Sjtr`);gIfxyyxsRumsu9~8haI<8jxZ1DAITXy1H zALh+43ST#ZJBkkhh-$e;B4^%+!fhtTZOA@fNMrY~K@in=^c|0Qa{jvawiz7zwaWn&)Cng$J-oqsu`vHq&xX9W&LAC$7RB9rhK3yt6 znPy7Lo&BSU7t+AvK)h`-L+T2@q71F$oBc=ZsU!g8l`# z&5iz>n!>utXYQ$f!zJFMVA}O;@m`fXv@0#bJH8el9(QWOr10iI$>lWe{XpyTC+`&$ z%7bflJ`?$Qso`pfIYnT{R2_5dZt$c4(nRw8(bdFT-+3n|$PyR5FN+DW+K@JaFnmVp zq0>i=fz5iVqGW{_t$<2hA&27&#kD^gk~lt(VVqUGu;ndNG$O~eYE#Mh8ZFV+ZA;nV zjKH}{&qEZ<&T2%B9M~@uZ2|*64cQ09!>08=_!LE5>V#P`H-u%@#G${5!!r^{2xeE7 z@(k9KM?}&xvx7Kt+E+y2%iG2S!rcee+3w}EiH4m6jaFnGW?AILP+|L92pN@7u8r^^2d>;!Tb|61;_M2o@-W$ z1lhxD9X26ZSC(MQV0%Z}=v0}Dfo!5#n%Ushl%%F6OgYa=3n)Nbpgq5OuoT0@=*vMO z|1o*opqo(eRYURQEamTC3ogsgqjw{A(Chgi0dy_!! z#}Kqp3ZSiRM^zn4opcfg0#<)+lJl!flQDESs6>1tt971Oqi!;G4^HAQLTeH|q(ZTwkPBfML!^#hVj z#`hX{HvKQmyW(x=te{=_Mn-mHo)S1ub)QLQ-(O0-`Hijt1LIW>X_>rr$LW@%SyybF z9iy4cEV>zkh{W$NSP03I-P3VxZXzOPs}^2_y=Z$u-TwKfo!i5oIv@H6%&{TJC>l9T zNbI$fjzBbSbc?{yMEv@zo=2m|dV2ZCTu1h4h{HQ$?J`}I0y&Ql$!JmBA&mO$m;K&3 zQ%D*K9}F6;`d&r`R+!%Ff-NBV`CEYq+IEhgV9!Lri5;w?krKZq_A*&Lk2C2aA`&dn zFuB4F)ph+&A*-_`_}`q$A)osiW}VZXErrXHM~|JIGPoB#qu1=LZQshzz6P@bsm^nT0>xtGk3_Fh(vh&iggoVhWMQSDIWuT)9gC< z$ODtR?C^DXxW6lwn>j30;rdL3{QRA0yX#(uUcQMH>4wC(-s2~LQm`=e455ETUC(6w zVXN+}#(KKlGwRHF2~y%g<#7NsuoRe~l+!A+jv?jsJ6)VlVKERoKE5)4-~Zd)zv1oC zyjyi&rxb4_Eb{Yb4mP*-Cl9NH#^OzDnK?EUDYv)}!qVtpb5tB0NbvC)wD~XU8o}PrF+Fi8X81yK2sOGrcjj+Y0+Ltj0o%mN)L_vkS|7+`YX%P8eXoo0iMbLUZ zLbTRs`)Gf6%jOwR6?A_|$V;RApu5~~(&-|L!kOH9sfNW3G?k2A@x3#M3k#DnBQ*$k zN6f9g!==TMhaa@#8O{fI<9){IsKy3*~PdeNfNA9c+ zqr^al-_g3G=sZm2%k;j03=3I%W#X9raM;1mTHIbzq7#-cEtUp#i?!x9Anh}rwQe6` zA%ELI6-_zT%&}VccuqcNiy|ysu7INGLTq=H zp=6Hl9Bbw-TQrk>fdFY@!+U2>7xg;14k?k&hvG-Ltqn26D?Jx&zkRUesF1+|+S$m0 zf7e*U_b`Qj6AfUb7%r#TK2ry=@~Z=4t!H3=j}>l2rlswsW<6TGG{Kkf)D$v^O+-cs zdY{ zqgTw*)K@gYz#n_ZeWQv3KwWr3?*+YgKTt7m8w=C9_-)UXQO!=J0=nd-842p#``J$}ie z9sig5sDn-b&PK*p0;1g}R+UB$zbi1zzmzF8K>=ok>)rR=#2kZPhHoc>CTQqNj}#%F z)RN2885~dVVW5W|E|q`_$#*DN+L0tf)GYK$S*-Do4nzLht64YW%dhl7RwBCP1n@-X z4&EzTc9fbWGKciQmtU8EbLhUCglfPi;d`5ScI#OZr7VIRVJkyh1+Iv2&w6#dvha+& zPr}7U>s&}p!3hpl=xfF9U=bV3l319K6i8qEbABWsIxuf}JFc_5_&y5h*N5k=8n{Vg z5k6^-?hrm^$7xbD$*jy*pq|F~5@i?+i0*t3+$<%dSQMJv^L5DOlgmtHy+4E}2q`A1 zi@S}QZAS-{Ap&^kf-hb*+<>Z7Z@M+-&h|e1P08linU=G(2L-n* z!m~*=tUwW5ur~)Asdz)sfX4lSm{CHkR^D0Jz`T)Ls!KHz!L7$IV{|4H-q8{z;i__) z^X}esG$48`c)UQ-&1@MSxDHa9wzwf?+@@uPfFBVClRkdKAy!S}9sUH3f+9T;r@!BATrA6V08E+2WgZ_N=?(VOtO z-oBTXcDujh?y!Gd{?Z$2@xnS_Uynk$?mZK9`I1pZz#CB0qA|s00e~OLD_<{fLnG{G zhEF9%z85|uWQUGixM0y|8%ZM`>DhwdAdKwjgXtC}3z%m#(~mFFQ(diixG1MOBxIy~ zwnXH2!M(W%@->F;Jq=C+zd0jB1OcgL-LvghO&i9ojeQtq`kuqGmL(ORy3>@VS1D6r z83WI^6(`@5l;RseiRDTogjt9P_ucfJX%a)Dhs>n(Zw3(FI=b}uWUINR=9&afb&IZ-ztL`Y+1KGM-VskP5-lKeg4k2sbnaZ>G5nDj8==1h>HkXZACW@t+uvTYKBFB_EuuVOXGe`oXo2{pbu81(>M zT?-4V3yURBJ#1j^Y{o0T??vV(pnnRU&!{uX+3dkfY~{;W$L{uLWEQ2;?o~0KC65Wc zjd0&9YpQW*b-G^u2-OZi_tK|NjcYMrP!_42&Y(1IFpZK~aht$1Y+CDqq-)FO6{R0$ z!Ur*fQ+2Sbpw2~x1w3z?7a8UBJ4#wUw&i*}8mPBR^MRaj2j<#xN@}nh@Mzd=XvRAu&^&r-T?CkB?` z34N2njk$g3@PfxOgy&R350mpWK-k@eZbKLE{gFMG<(FF&E;k8qRvTZ!drlc0&5Ze zf=mNHm6YCUzpi4fPHX`B06q@cAJAn%tA%OUPiAb=Lpq?jfX^iS@BHDp;(Qn&x?C4` zE3*VVa@8DJ9TC$g<(GZA{s3t&Kc#CZ$DXVD9>>$QxAnolbJMXj#MA-K$3>sI;KOBM zJXX0Gj-iG*8iDrTDSr=S!9?4#aM6xX!QgXnodxq`2p>C6--o0RMg_)z3*+G4byhfR(@k7tMpyN z`81pwHIKe?24S*Qz4ND!F3X&>O~#BXo`ICoD@15$<~UP&!@e~+hqW@R=88gL+Tb>q z%fgnF@Mi112y=>81Z|plOz>llceehK;e$GhVK}f%nLP$-p;|4KPuc#uesl$Spc&O! zYn;@2B^0}4pTlq99G_NKHwTQ5tcuwmtG8^&Qs!xvCQ{ei|D@tU?BCY2_tX!W*jTO9 z*mof+Dt0fRxrxx{`TniPPo+A~3{Flh7X)LEO$nOFAKw{IwjWyQh2{9rHx`c(YM8m# z)>|*Kn8!?dj)cV`s>?7lg9FQ(DV)@7!HbG`-?! zz1o%^24Y^D=WzqNgQTUgE;tQhm9vnlZ$`OT2d@b7BEGJjsOss6dN}S7fB;T+7=+Axw{*JriP(&VgmR${B)^%}dJ zjfoSL>O;=eqv*DSuwdIDxm@9Bq4xBM*O{U;e&lxN(nF2yhuCQ3-UFFN;&`62ZI)1h zBmtvaw;N^V_d%J6?z0`>+2_qEEwNXZ*s(3J^G|KCYX|Bl#?+9OSN0{UDB~GK4-j`A80p~REsg)_q*;#(C=jF#5U0~5 zY}tw!Far7d#{$d9{W)Ae4~?(Tl*DpankCyv#C0oQ4Z`~^JS1VZ0;Y*+<&`%#xPmIXZ(Lr2#QKbukzW!WG6b51IYisynpqd4v|Bs`y zj;H(m-}uqf!*mT}renIMhhe&Vy5s0J^O^1#)7?4K%(RI)N7uBYyZPPU-{1c6;C|on zx~}Vaow`k`UG0)F$cF{k&^No^feXmquLn=?_!{4 zGt%e#mOCNo;^URG+~T7=zALp=L3nxk&dVuqhCuc`568}kgC2K1DvC~hRB$-yfiP=# zCikk=^U>1@rz*J3)syflw1pF% z!*V|cJr%afx=beab@4gDc({CIZI;(1AS3T*$_`H{z}nL-*vdPWD(KZ@E)~T}wb`4f zj+0@)QHP>yqj+^Sd-OLJ6(7rxH(3m`Xwn9Er6O?{mkM}6JuYCVgQ>R}2@KVB)(2eT zB`ioa&yfVn6c$VcxU_rHd1*`~)f%kWdRKL|k#H-%J?jvL9((~(?ZzjE6#Klr=RgbB zsCx+==9s-QjHRXxI!@@!2hKGV-8TPopfQEZG(Hp9pyVJTg83@9kqk zkKqK94Xa1yJMK)aT*QKPshB-$>sy{$JA)fR2tvr`uPAK7LGHgQLaWj({>_Rx@gtQu zrJyuQ?OIrl0guyu&}Q@Ot?qN0+|QTGy+vu^$|QU)-9BaZ0k@wU&V9f=dyR|Fe(Ue` zPb;3yN!$9~yxUBK8|OpLcvJ_ZNHajPL*v1$^)?ctia@y+zQs?0I>ie~%BSbN^v`7H zA(ZD0hz9HKc7GPt^y8D`OtFAfs7)bn3Wdwh$cGkLlb{Rf4`KZwnE2ht^oZ0<+PU+# z+j9JNR1|D-lJ%y|vu&%Ovx~?wjlc|H%YU#p5~|-%0rO)!BAK>ChXK6)%o>Lz2YU^?}?T5_AR=;8SMh7~5?@dKz&8M|n+9J(AN<+s@t zux)nHqebp(cK*iB>y}oC=b7?V8&%ND^YWTmEgyvyIVECDDHiL%>!6irD%YaLVeFKF zNV2{B%+m53?L5GG_@us)d7P0<7E8v^_f1y_k4OlbPm%!gio1|C8B7494Q5rfF z;K{__db`N^gC~{+*eAy`n)u6H1zJQ=gRm!7bqP8|z+(GbZM0C5eLHi$JGDLb-S4l$ zYqbCEb+>!E=%(&qS4!iA-iZ3KqOJmt%Bxo|do{^kn|c#mTo=?h8iG91*Sw(+y0Y;C z@!qdEVL9MIR#d8bJY7FXT3SxWfF7$(6s-e`pE+6*aoX2$BY24}=KH%WLcCS~T2Ba4 zK^N$`6`L2vnNk-^!>$!fs?AX@8LCvL$Bz42_1oUrix#Rj_|;}PcwGFcGCJ6Y)s=jc zCAP2|%?m_Zb!X()3>12df|gaUHtV8YgbziLS`GT4l2w(1G4k;|yrm9TpJp9GmCMQA z|9cyU7MMV_WEsdOW;h^WZ$w9@^8${N`NYfOI%J~i zVXE*uE5|m6&yCS;J^J=-Ha9shU`r|;VZZm%s#1aCaY70fLKuHpHT0|Ad4{wi)wF(k zceL*pf0Fu;n3M%mV{wTtt*BveRKOQIj$|-k7rXW}SPf>tz=G}~@u+tO9j%26;V)jf zb%}55GbC<)=He&E`@&rAtmE|5`paj?x z^6YoRFiA?p9q2o%ck;2H%&53no6@VRI0pWE^Or=xb`2-aK0*_JzyRuS;->_hX-QP% z=OC(cAR!FKzojT-Xh+qL z+aO$9;4oM__v04-h~mTm8P9B>cgZ`FTt{txg@Egbzw3kx$fbVUapz(73q`|}Q6ZtW zQhPn2>96JbekG3d!slpdh)!VoFB+U{I=>at?!L1+vf&XD_)oHe?x|{mt+kXt!MrJ* zsO#RJzrO}hCk!YUASK1#t)9DKLW9>$$fHO|g8ytVOfykUKYxg>+9M41yP)#NLFVc| zpX4p!#0HsUbh0qpN~o*nR_CcU75@=sRX#KB^03~Pr@6U*uBXIG66|LO+W$v)@#%3(5&=PLdvWTK{h z*skChP`N-dy=d_jdf59tX1TB7T`W@_wG6DhV}Jw{G^`-ajgs_+V$w((H$~_bP__~y zT+s!Kk)~a_2|10;!D#rWa|wsj-=9YBGpV31Yd|;xJj!mIROYfofpcvbnI$hNY zfG{cS#Lv0~{cl8zOq3trFtxUP-avu0I^q7Q=RRnp$2oVQz?asUdeAOxfG`&TlCqd5^dB z*5GwY0LX~J7FiVmwfpSX#H6Gpr>o9@yUW8R4P)b2$i9a#5mDU{Sat!PA0c>;vhqTS zkjgK{ZUGFT!+-nmKE+`nT}Sw^R$z7~tPd!s5*V-crt&aNGeB{qEx8apfzVmm0ba~* z78C$I5%Ye+=J_-!O>6WK*NFYTQW;V$`rmJC>4jH~rd!M`kf4*ftM|_c1K=3J7mO3a z5G4+=Qd?AGC!u*HvJHNcGPfe*KV0e@eg5Znn5u%z7$=*$@HsN>Mm^mD(ZJTU*9>%kf>qr)SRRqE#=B{iy zdJUWlW0XBVpe?)X&9bP;*VbUYVFe{Kd)8YUBVhq&P}Qr=X4kP5$$(0(i!kYW}afGxdox&$tRG8*B$f{ zqNHU{9DMRgN@{AZAr)M$*R_Dh{HWyNB;xH&xl4g^tZ*gXE!i~Q1D)rrp3c~}I;;R) z4#Me{XSn&}5G8|_OW}j}nSQSEY=f+sqPEN3NZWesF>C~`Zc%J+)&2lmYKStex@K46 zy5r|R^DZD#f#y?6$Wy!FPCTZhG5w;^ero2^uU0CUq1*ExAYA;KuW$2={z1`AyF#Ph z*RtC1sj|W-;@h518NECu8!2Dovip^VG5)+uGn&e}vgS zujE7zU24OPXAYO^{tLyd48O0mgj91C_F~&}kVLioBhC3E$+uCVJY1L?5E%;86-O>T zw9?tSXWeSeU;yDCRAyLTs`^y!M*tUxTTJhy<&r8^~RG8|^7xX?eE)rowD+3%Jz z{R-k78EbdBjbW-cz_YXp$1r_iF=p3MD^xooBg=2tF&yHsdWDEX8C12N1TNpx@BRF9 zcBd2<8S0~4YhU(`&VIr&42DW=#{Vb8o)+OzKW_Rb3!@l{-K#hnlGn>O+DzYMw>>ra z+j??ImhBBO-Xd-a%o3_kUU{k}gWFXLt?z2zdjmHYCS7fK6+0eKHykI)xm3HFHIca} z+J_^*0n6#=L!i*Sq;ZjRUpp=Q+RVwW;I{>c)Cj+SqxP@-!Qz|1fm`pz6{lWS_O`Uc zLabc8M)9pj#Uk@a0iL;=AxoccmD0n5L`_g?da$B7o((M(VH}lRxRzXBbbhM_*Qals zwC?Vmv|5SxB+t5cA{cc{d~({UpwAqw+h48qQy!lBJ|6oJC|+O$!{;7v&ZcGL8R1Pn z(yV2PEr)ah%Py+=b{r(HjtWb?rk)3U7Ygzg6Nz6SBGSj0s(D;c3&}3w$g6N^T*Xg3 zxY;(o`k4^ul}k|jwALMUcsnbxf{AVaLe$Z|P+PRrkPFF^(P3{kFkoy%w;5NAf8)Pq zXX&0t_pwKLT{5>ofA7`1aTLFtx#%>Fhp+&A>5Pi%*5PN}x37+8>nu<-z+NW?W#b5` z7J^vBzqMW4m&4_bt>K#Q`H$rkB7nvz&VC)cD_0N5aJ5j1Z$3CzY8y!6J+FPbq(bO8# z-olMXA>`Ub!Ev{Y6WTZ*oVzY2Cl+4NT0vrEAY;R`Zh}ZZ71QOs=wP(#ePlRqtyN5R z&vqiok$%2F(sc?APq2jl3EW4nMZ^=r?Qpu8y)!uaspzuyA~t<P%PUV*~QrCMhe|sPx@z!p3IGyKb$>+ak zZzJ_L3;rZ$E&3v6+(CfaF07pOh0A{nZC3DNNfgeb$%9Ior7!0@;-&IeQ+}5pi*&{% zQ-)37NVh@BVRaAP+-ZwBHw!A~80-I)v<>_b5M8JmA?InH0?(Byzh;d#PoWlt_304( zj{ybvwXUwN9BCthC^&_EKuss(0>V%z}lgx>^2viuqjVq~t^>lEx&XQbkc+<9!S)lXDW-Q3NKp$YS3B**T?%a| z*TzR3&m*QwksX*&SJA;1soO~#CIv%U|J94z{R3pZMS=RrMa~kJ9i~%e?`8dA3h5`? zTZnZr14Q613c2h!+*IeCZoNmYtBCN%Yi3Pt7))!=yevjeG&Dm>a_{$6up4T z?u1-rzFIOnD}(+{8tf)hb%Rug;&r=s&FHjLpiA}+yD0_T({;G^ z#@7FqK++y&w(F%szuFVI-b`CIYMam4*2sy`!Qg-zur*y+_U^8C8~+c68L0fB8?PcR zazJLM`!*$Qz{gNzL)bAukbOjPB_*Zq;Im4J<~+myy!jqN0$VpOY`CaaaTUw~MuEWE zkC^{aib;}}U*n%3;#ss~U(>=XVqb8(SmSGrQT4fiywzTsPCJeV}G`xLjpo7in;a`X%N+#}cP9n{hVT*gwOw*(oO&v8Hnw<+BYxv^5@q zL#E;XnWPDIPHp0NJ9@ioqsx1Z7Vf5@j)K~Qi~N?>j_(*=F|B#aUL?XJmuW6<_iE7U zYi55qEY5l`IN@OaT*b&B*Ey6i z1BrnPr_VEnQiNN$Rn2rBg-aF4^d0F(Ajo($QNngiNDW>BWK6q^h*imD?(il-Z6rYu z7;!E!--$CL>W@PBzqE>)__?1}xlxGVc>JBjhq^igd%-03yk^tH{CYUE`GFCpU@7ZraJL7Ldb;X}b(14$L8+Tz`fG? zZEVV7PYby}4qm0v_vZR&p=;AN=sZx}*zIjS0D(cvMVF(LXe4t;(&!^kspu#4=iwGCT2% zVBOZq{MMPFj5FNn_62E_EFpt{Z1a7S9b%hLwurwVS2Rf&(n;MKL!a$^-xqqn^tecW zp^{(DFRXsDug7X`@lA2O3}q?&IVSx@g4wJdsmOljJDNc49og%e!=YBNq7r&>7;Z0x*m=cF#8Ki79jS^vdZsn4tKhHi;cz)E z;`L&i9*p|1on|SQw;ve=o4aEfg7#rcwP*ga0)|0HJ(*jjH4M}S9e!8KE$$_qk9wJ0 zX2{tC#|m+|n)jAKa446@BM$a6tE9r|Mo8`Gqnl>*dk!n~db|Z@PFeAzL$G_m9Qxy1 zkw>R|!L8%PhS_56f*@;A=Vh-J&;Dq|AEr-MIE&od5*XUYx@fB)0Lr1a@lMlaegxQm z(BBd29&6b78}iyl4r|`+K=!XvuSF61i|;T|S6v$BJW-Lzpn`$OE*@z_=z{K^K#)F) zd(svcFgP%v8?c0+pM~LzGUoh~>5GOKnvc&5yFe(gh$?iP17UD-)0RaYc4150 zSd{lX|B0pOC|0bKn`(HU56}jzU!m)3d2+D%j<>>Mz;O1C=;|B`ud-n11uVkKJu3X^=D8BEJyrjPTaP-I2|-!->B!z??0AGH z&>&}91I~3BP(mGHe_^>K)|$hV6n;o8*M%GYq|Gc(YGt##VEqvoEpp@?MRsjdqLyFPv05yY6qc`@g*HtJ9Si{z1WKm!6myx@d3stix#u2w(uy6exkrb(&u28a4E(+HiF1*}8QKqac!T zwUSH>ZQ<`Tc>xH&Ww=&VEReH-d(DbSQy1{XAhAQE@yg7?}i+Rz5t9P40i<%mnMO>?#A zUz=MfdUc*8!Mys11=o@A%|*X;t~gSDa+ys{8<8{>1V$x>g@jSw`bEq*#pZ%mqR-gD1L)+?me*7xNVhcd`~oj54dhxw~W-CJ^JO~{xC?1sx&xf$0PS6KZ*0JiiK7%RM%Y8Q>*!+A5vSkw z!T6^$Y9hJ;B-kqixTWua87Nzc(+BouTvmfN;bxujsmg)7m#)uE<*w-~H8ph`|DZKPG>l zdC0h&`=9iY#+*lCEc|d>@5uf7AEHhgbv5-_H+S*z4)9?(Hxv$p%at9I8kr1{pMWuG z>5(fgx@n7u4S?|c{a^kAzYQe`W3_AASPa8gOk{)5Bw7>poCt+2`d3aMAsy0)7K5k} z#!xkIi_jUzlGP-(S1Lh!2}FZnD78a`d7b(eJMq|XQ8?PNr7+4K=F|{ z3M`Sj8EWA~=?&(Kxmxkkh{V6^oXes$;SF>_i$ z@qnb>?nM5p)ALjpeHXSK!uU7!cy!8n$6&&2vc6c5XUjbOe98T2V>5Pa-`KZf zVh;VSl{a}0(Z;{caVs8gGg_I|Tc%%(8_pG2_PHba+pPAyk>0sulc2`0nH2*AtI1yE zpa}4X-#i&HH;3k!0wX)_Lrb5{^Bam+0ff3n$N@`DS3}2*!^D~gVQMfi-o>(|Et!CG5RH5*Q3^-;KmG)~@{(i%j^$LFQBCKU0wn|CexHVFJx% zV*ZCPM-s(rXZ%deow?R^>R57tcSHw~_N&y?jW;MqQ1Lj_!}4T=(gSplQz`B$zw_!Ly16a$n7e zP&)C)Wqk2~#>FRe6qA5uJ{KA-bdr=PB%*Y{Xx;U6)S6i>vnk|B!RsCg97;WMt@0;9 zuK6`l)dR9Y-RTU1k8(Ywt)gO%7Mar>uU_T;${P_j^sXw;PX4wXwxUfQd_|um>ixvZ zgHefwFwE?U0hp(^^0NHE10_V5%vv(|teZ^A%!5%U(43mv($br7q`v*?P^Mj*2W2fu z*y0N$TAMGEFf^s7-cFZqUB~weE}#x2rd6nzQ}scjWdcT&<4g&dl5YRbceC1sw(MT2 z%~fiRII*UQY$$sB?8y|Fd`5IAk+ywfx_I|}(_Xg1>C%EcSW9)EG~`HVh;(X}g>f|35V?)FL}I`^ckJ+9?8q zedFFKBsTv{)%fHcrqUd8PyPo41E{P-DU_pkWtE34`*Rk-bJ5v)mQ!#ch`~a$7&og0 zk+F?GW{a@cA&cRq5|d|=-L3JTikculZ)K17J?L+}_IweoFl3>K8h+ZyR&CK{@8AC^ z!88X%Aq5o022O$9>&rClxxj#41VB-N0H${Fuhlv(1egPBF*o94@w6i?M#_n4TAIi_ zeMV6)Dg(5*GdfEL7B@c!o;nVd`|W&RN+?>R{p)RY`-Lt{&g_v%qi`;|i`4f~zJ(9< z+pFWW_dlYH6>+hplz~`BZyqS{?T6(6ft6#AG!<#^V_4IfZF;@^HjKwDiU)5MkD>{Y@JQgNPYZSKn=t3E_?>?LCs+$u$SL^l!2saySX z$~t(2R&{g(w1`B1 zK1>JAH#EPr9P*{^gR~2St|S=xi#xHO-ibB93|VM+jF;@X-GKrZ)?D)Oa$C|w$Hf-*IkV1Oq@~JdqK2|b z4b{$!Bl#n#?vF;uZFy6jckB2i3zadS#3KsrD>F5TH{#6IZp>iE-l&k&THx2 zmHb3z{czi8prq=DzmCZ#wR2i-bRVYZT2R~fBy)bgFAM23g%PLAsnauwe8Z#Bs*n?L zTotm{X`58EThXC>=4c%1baouZ84=F-IigSEVk0QpJDTIwktm4@*^}0w-=|iXq1L@N zX~$u4@ep-jR@eNV_R(VA3c`T4LiYV_$9`fagX2!oZ6>nRin~y&M}WsejCgcDs~+`- zFVri{HmDA73Rb)1FayLJS9KE9Db5dgJ&g1_1Bp;&j~GBo4@By<564SXe^H`A3vLTr zsTMi|{MwdcckB((#LRx7LZLB+CIxGpfFP|FpcVkpe!Y15;Me)RRd{(X zwn=NUfJkX-;}Sc_c<-z{vsm%%FEWbt`8Du}1#Sj1s!&Zb@={WL`ZZ9TqKC$!vq~?m z1$$QG@Nb-)dy?JKJ46_;k7KH{Y54z1{^9L$21Zp~YMt2D)Vp?fPW&%*6VOSevXAG? zf2?9`-a?1R|9eN4QZ9f7^U+B;;Ytm-pKN}W2QoHV0bEHyxHIa+)*PEGOYKpWDP9AN zOYZBjP;TbR`qgIN_oP0cP@#ot5K5-NV4p`Jj4~qxDR7-|how_~ozHH9!iTc}+Bl`5 zNtZz^iG@<06o$*&nEy5Kk-Kl3{nQ6$nd*jQUB+7S`-r{kX_>U}s|RhjQu%XYVjXEn zY8;cfq*k?>EHQGeKc=E1GMBM--l{@}rxdddF zi1lKsF0@u`hSXc$QQVEt?V|1G3%Bf7m4Ak@G@|ST=9kEu_s~;#Ah=#yENRR0x1z2k z73%4ffdd&}$e>y-+3B;o zYicgXrSlF=zh^5k8>}BfX_y~mXiB!FM%gZ-OaVA=XA}SC8BsyQm+x%cR zI)D>L`KiCSrX{kmEwR>e7O1Yh#`D#E(;=r-oqQsdj79k@?t-b>ry=UvrLZ}aUw?(15&NMI0(0yjdA zc@_3FOl_l7qM`?5t`mE#W)R)+lmBN^V-m%e4@HG~3$$)5e$Xv4c_GT&Qz{@_9&Q{-^Hp05M)fK| z&d7793SexK9Sg^rG4=|Sz@9iMh+wS&G9&mJX8V~}uB~I>#HxEBwPuC|)|DvB@ zqbUD`jj!XZk5)li8a#@nieM+aWxa3i$qgx}8)5i|+NMq3stt(lm1T=O6jK&!SelSA z-DW+bq38)H84_7nBx3U6JbHy6?eDh@$9=un{e#O&b_iKnJgFHC9j!DJzPdRs zq{k?P9?_b?vd8)Z`EHBF$^-M?;;y`zbiz1j&qxfc@X5ezix{i8=l2i10~O;IQ5`43 z;v>V3@e~dH_~sH2_^da`^Qkz}gB3aqdf_`ZhHWG%k><_Wr#UA+aG z)S}Bnhm}TUd(-EPiV;Z^Ucq7Hi?6m58?eV-muuj~4O;SFw*Ytf7NbmK!mhfI^Zs%| zW2YA=G<#_~gtryL=fA;@@_441eZ)x{cuDLA!CzXp%ci3?nSfSl+i&7k=2wXdIm~i> z54;!%7I0n`wSzH>;esXq8oq?edh8|U!s0#NE@VI7d|=V^3caC%LRNur+5(&maICeYg%y;#(>adsc@lnKkgBGMsqS?%(sIg4vI-H_mF{B=zkGF*y|{yk&Y zU|eKdzvD$l*oj+oE37EnE!=ZQ^n|Pdj{SH0=SMENGTipUR!F5qHpLS#5`kYjJsn-4 zM<%4TjGorN&+Q_Jokuv@Zn<$UA8k&T07rH5*E1{Or`$a7M4(*AErKa`)$%l9F|&X3 zQq)^do%)gYI%Fj4(rCu`AsA<`j*U7@4f!HrM*joTC@td_@Ij9b&qjvuNPVXmEU&4Q zf%K~e$laC#64s}B-Y4D5sDSRWZBuP9FF1aOC^h`3bzd%-V{BcmyEQhFBJ_Sc0b;lT zB~>PqVEpNsZtOh8A|u;3_N@gd579H)55I<4&`dQIjILRvpIA^_9Jd@g*Dw8+s+FC# zop#~n_@>l4HTMq%Pnk!J76b?}f*ZZVvt+R|SlekiOPdVZ&ON_|ci6vP4g}oY^jQ7F z64A$@cRI=D=eY2KPg~btt<8;nb}cH46$MZGu~=5GQb#j7kTGBXEKZqGLt6zisfXo` z2%=zaTFrMxXky^->lv%`i@oL0i>t8d;{p61X|)Q|AFw9ASkcxV_$y?q%uBEnZ>`)1E1&=r}-<@h#s zeyV{_(Q*Da3zFc`D#S4Lh?6q#a?{O2$_wj2Xds_&hj~Ied;$rDCUXO&BjiaZ)Ek@t zkvmb4%B15dRSD{AYVb`k01n~{^0YnHhhtVKlig}CLGiJ-!pCb~+BZJv>)G8wmK&r~ zP3}*9*t}dkN$2_fC&C0~jma6EEkLCERL_E8=1pbRP;MVwMG%w92{p~Ldr4D?Umh`O zjF`W$#%cL&F85+UkU>h^qr}*Lu~0wgqNs3Qp^@@s` znjd12$o?1@0J-r?i#%a4cl9rvNm&snqSaTc*n_3=JB9T6YhPr3`Qt7Gt5$9D=y}!a z3rda{_dyVgN$+>FdopvH(FX8si@5grRxMRq_l^jJoZQ_Wzl`Lc6T*wH0dr9W^Nr%J ze^_$xy6bO-;Y*WQ;i{sSU)l|8Y&W5e9tZw_Lt(h-s*9xCL`HZAfIabXO%+^Wh9RDE z*8Srk)`A^0hFFIF_eK-*unkLyOyP}5Z_z0#2Bh&vC9@px& z{G?F+EV~ncbXHRj%iXd)7Tt1aJE6VWysLAKifPhri>?^WXb4yhJ_~hDrXm^DYaXKe zCTogp0NU#|dAo^`U3QNf22d&UE_n;PjHRTcRyqR)qRG!Dr15ddd0rhm@MZgKkuyM! zzf51;6Zce96rGuQa=q2eKa5%Agd$GM-Z4MnPlxJFJJ9ryIPz;Du_hTX2XK zh+U;H$i#ku@ZFk$cGdBq4<=47+`n+YC!XTX4v3}W>(u;8)a|%mxW;4Dv z(C%l7^Sc9PYkhXIMmWU1^ah7363?%XV<1*lB7=@}2eI+B=Pd;W{ztA?c3juzX8@e= zUhW35Wn3?ujwz+0(Al-WR}&cB$d_7<_@;vyWK3|XmD+i}MtSNlOSe4>$0UDa$(rT| z>Xhttb$gF%ztls|(Z*XH9baFFSloI=T?`RH?FP&Eg$bFqf|Z$MtydXvy1gq2lV3pf zImdMlM5s zRcE4r2^ZrJTc~p3dx)_M6}o9Wgeo-|=5X6cTgY3goGTJ0W3tqQyoBQ8HW^w8YC?LB zR;x0WsS*TM&THF+@|^^mw`LaQpKcUKww{I9IG#&)RIG(?>^R$~?mMLiX##Tm_P9Zm zXOd$HGL0LW=R#%lp8#Ln6jB6VReLN8q@>!Y=JvVCOOojFrrjooLyic{y-e>q<@Tmi<*V6yLCE|VI{ZO%57)SSnmr!o2dsr^ym$_b^2dTpZTW2heoNggmnII zl*lsO&u=fC$yaxqXxh-PqZdl8&IirK!kE1HdcW3?L5Z~ywuVuy_E$t{4&|!jWfFBi z9?fT+T^hH+)E;ojUscEKPTPId+iRILVG|aFBwMqrQiCR6y12+EK1DxzHIIP^aZ~GZrlP#iu9&bm3bT!W~CJu821^sFl zGgFK9+~s0}^NqV#g-l^6Jxi`ilu*4A7e#N+IO)gtkiW*c$cMDpS2* zD9NzN>ATkb0Z@Gk|Jzb3v64TYr4K2xeh7+fD3+D!qG;2xz6J-crg7n z>K<6(ycP+Fyo{oo2TCLY#Gz_JK?&j&B|qR6vt1?HBt#vCa2el znPcOdr)X+}KyOwZsyEpOx@$sk2-Dvx%_^Ymgne55Z?C4ScW(})DbewNWU8Gz{cL{e z+-3N*_^uP_I-XZ26GiP3bRrBe$<0VPGGdXGRR8h&cY_J8290Xw@|lc!oz9q<#EeX$ zY;;Vmn*h%faQsL*Q$!+I@3nJK%94xhf+>7Zk~8{Wqyv2$`?u=h{u*}(e|nqeZrhg5 zz^(sVhlJmhDL?(4py8qZSxp{?R}~2^U=*9h@%r3;vRz|p0}uEub|wS5p)<=5AlBr) zxZuAmH8hSG`}clq2cpX$ginW= zbgkp;ap2;$AboGp1~o!VSDsVPWr!55_FRH63_MMumdMiD2}A(d(D0Eh5A0UJtpvaG z=;NlyGjH^9c~0Z>*s);c=anC7wGU5SElN<&w*UQID&TJ;E3UiP6Zwqv(^z_&KNHkM z^1%d}kY)5HwxP5E=abzYTDFTnPZ02~HO6#x>n+#y{y7JJKxWLpExc9Ay)yRbS)zSd zF?@x=mj$JJQ`S=&T^cWO1#E9M`Uze4X^VjLI;H-c0GmLOJ69*EjJLnsbTm1ox*FDG z?I`;^MT3=$)|9GX0D~`@@c5IAoBG1l@&hutu3W``Saywh7@jn*Q;VdYq0>i^6py9< zI0j7UfYXy^5ZO8ypbfm*eLdCrP1Jcg%Os8SPtOiknD@;Zd9*e`zzI>2%gL{mwe@c^ zg3L11D;0R4#gO#aT}^K@SE>I}9i|Gm1P>$0r8v-ndUnlch_osn8UWD8oFwEEr( zmmvri#6L?Hn#A43`5gv|3fffMa54u*{o`Ya{>p%!BaVzTn%$vy_?k`NR|0MXg*nl6 z$K8Qmcxm_FzF6R2)~RuuYY@m|+#I%sJ`Qj9M3xSZfA9Dq+aSK)AIOj*ddk}kWVi7F z+OeJqJ(3cY&0R7!G1Lb$jCt!h!mGc1;O|5EF{a&6Qz77yC`lny9s2GT`pJ2GY@Tbs za#EehgXUdM*$_xI(TOf*!3v?n6r9LihPzecD3r3oEp`=a;vHkpG`zB<>xDN)FScx4 z1{_sUSJt~pB$LXsh^q{W@D;_C8386Hga+9-D7WUP-{L zdr~CTBL3Ly<#r@_0o)BMO=oXsS-qkxzta) za7v{f9QeH!=-ef58|SF89vCFhK~q)UV=$#F zX$*wynyY|u9b7%Y`s^YQuat~pt z-1~%10`HZN%fiXRBbCD>k6FYExlN6)_%$~9fAf~ApU=aMp=JKZF4iyZ6>jgRju<5@ zq^S;>y3ggxkOHIl8+jlzBEDHfSUwjS*4dMJq#h7-)Onw?t-gT)jN*z{JUpjEuXAv4 zyKQZ4l{5Kq@4tZ>A+!d)m`HGodgwv&rtK#RQ*DD;*`6%7nV5N`r$s>w>FDS+uE!V?L`{o%MKFmsLOj3HTq4~qj z5&n?E>s9C51?NEE6s&qx_AY$9sp2d#NtiYeqD>r+vH5Tt?S0*&lfcS_x^muzrD=n@ zD|@Zy$W7t6e*55^H`DNt?MI`At)cT{W;ll=c#<1I5`6KAv-6n7w{jtmN9+B-v}5U5 zC)i9hRTg(eglOF!ecfe@fszA?jo^AB)Lbw>tPs4HHy?=|9{eTf&C5__Juu8%3+LWtEwFkT{u!&5KV9xGd0RC}5$s?L1zV71@^o~(Vb9LwAUHBHHBS=Rc|G>4F|R6WiH-hvgO3zj(DPpvS8{w~AH) z(>n4d-9chBsukJ@29!K5B3$WW4kKab#h(P6lbpZ(RfIM-!kooHhx&j(0Ic&oG3cf9 zj$hXMp7dh>!Q3cNB|gihz$yAfv&M)X9$3?=iqAA*jS|oLX+OWet*7}6iasqX`w`r3 z69c(}`*WVZNLn;5`a&q|)SW|V4>13v*+Dbe;Z)(0al)kRaE%nr5%qN)9D~h`Xo`d# zSK{(l>o62PZWR&Jda35KIM7Je@+~11sjD0;eK6Ftc=i~}gd^+-3(Zka=&rL7lS|G= zyzHImo|q_g$rZOdH%#7U^*{ZZwr-yEp?E%PWr`~Xue)4EmNA<4s%)kKzBeiG9P86+ zSdl=n!e5zBH?^(|_Zd1CS10RC+cy(DgBC^gSVeE_Pcxak5_nm3%~PNn z2SnP+5ILRQDSK~SU0`r`8ohSxXcq|iov%V(#__`6>!n~Ye`KpSv9ymZ{YBN}jFDg3 zpD)h?$({#WkC%HBRkkk%tglEzViPW^s<4oBFKFlL|7HtAniJ$E{*|Os62LsE0G(o= znoPK0cS6kL22Ix_k0^IF~S>_@aPneze!KORS)>%J(|=qLsT#_DruOPZ1I_kBIN?^<`c zpe?gc8SD%;w7YZ&tyb62gMwHKJH2`myBzgD`7}FmP3t5tErtzjK7JT)XhwQSkuQzh)Uo@AoU|9g% zzv6bgH;-bVxml$cfEc7_su4`3xHs$JRaBETNAc%SG?3A)Cvj_CFw&3L0rA4U&KJT6 zbUdnbFm+3hblE$*`Nr>PSEt+gcx(TAh9_jv;NwRwvtOSnovsIz1(x1|F?A+xMC`Z! zIlP3~_maBD#qS(eNkOG(+)Ie_LcX}YHBR>8*c$H_L;4#GhQvW2?pge)p%FUC7f+9X zRPwsGye(KO$};->QlU=`dN|hMck(NmcbtqpjF9{}6)B%$c zRw^d3k~A#AJsuj= zn%Gg*{E!?WO*GTDDl=mxqv=1MNnfI*>0-#i?P_U6Rr3*S+eT_3glQH72(7Wbn%-{B zmA?wj_|p&ZK>wOAD}u0e9!t{^3|fn5I*ZJ{b0;duC_Kh*XeJ2 z`A0x3Sm?b}H%{+z&ap|MO-VqB@KMQ^6kw;nVC36n4r@WM6wkk|L_jJ#g)3BR8ZOxX z162C$fg5|O%PKMjCG364AFi9Zio`rz#^LHrGZmy9*KZC0#9DKOv)t({$`T$q^l2JN z7b?}_q%mwcuRC7v? zfdWKCN!mVm%9(9Kvg(U-rP~5`=;&_F;?HJivzxPE&V&1UU4rf(%{&H|uHVc&D8Od8ZP6LqNhMt#exE~zd*97Xu}L_;Nee9P|W+B_}fuYHHJH16)u8;AfO_k(rqCP0@5u4 zN{58BAkv{U2pdsEKt!cmx=T7YA)wOTB}zAJdb7{udEe)ZcZ~D(d^qEb|NX&nqrm?C z)|zY1YhLqOa~>2JC63e#1qH6`XID%&yzzIHiRO>JrM8Z|`(nd&wUVmu(5kTa`Y8Q( z^>UN!v@RLK#JA5FvUXYL(v zifQGql}|(_<}Z|7Zfp{Lcd#OIgWsoK!Zxsu>lV_=kooAW-?^vHmhejCk^I6Am%e=d^o`ng z{lO>ppB$=B=j#_Yvdrl2lj>G(X|OOw;2{mSvfpds$aAgGeT3V?#_nBjp~a|Ca@ghU zFiItfp$8I$iJE_H{^GTKm%iH`Ly^|9R~E`thh3MWI=d~B^VZPS&{KxO)Zd2ZjpXXb zynlis)EjzS*S3YGwi^4HWA)r~t_1(avc6h?D}3g*AB#f-&l*NzciVxK>n)Mx>702H zdg(>{%5XidvR@GI#g;r5jmj1p&$A37s{HN6jh#g^Hoeux$m|{IazDm@xAiiM46thh zn`L%=Lw54_Tk~?~M+e)r?X}&zJN51(C$wfV9YK{J!=MXPccuqkNJ2O$#Q(RnZ zr6n9EdfDC=Ti-KUDztbqqyvq47Hc$k|2`Mt&sSoLFMMt7adcpL&52#Xhj(;1Jk?Ma5$L#)ZzLclqbfC^`QYNFTaQF&bzK(Y?p|o-OUxcdI^zYX> z7bP6F(vLAI`53!AvNPO!%& zQ=%E4d@dx~*>{|-lClWJ7K$a=^_^a*e)-bl(caL%o_GJo;TCvw+@F^sSN$sUyf0_Z zr__w&GL+wN?15qTEJBEOxBNF6N4@@u-IN30;MxM}XFVP+VVNRxv9$}&*ZQiR*k_3z z=-up?}Pf1Y@DYG^kHCk$?0d8bxH=q)5lpi}V^Iu6F4 zwI8wVyk{6D#}I4L?h%D*dUlrwrm0`pZ=bGL?fTu>eocOuwmySAP=}t$DdOS=pTc+Y zT>bR{Yy7sMZc#k7L{W5Ryu*Wyp+(=t?!<{QYb^=qc^;Lnj%G)L3cF<|;XRS(r=0BL zKEqEf@qJ-%J=(vEawz**DV2%GBA9WEfb*}lr%KQ*Y@mfy!ZxTknZD&>&#f67I_t|} z>mvsrbxn?(&g#fNs_+j8L9dA$eIm{kV1SvK9?{&7Vd98?yrb7QYreKyUaW8D)}B55 zq`2jNF)ixi$EVEpZ7?jFFxhvGU#*@WQ_BpEsZCR2X*@|zHn|t?Mw^#*lu)3X^u%FA z!I386ipX|IHL28Ie?Z#Vm%)e0#?no%@41QJ;jDRu(8(gZ<`W- zf%zY35c~pqhjT<(zObaIGK$v_-zabxiTXIR1AG8n5gjY-sT->|K;HLs`usd%yyb`0 zRe?_D@n*`TLH}ame<t6=)X|Ss3GakV z$*Ug>a~mEJ6v+vYUs-NzIQdmM^9_$PPQ?70Xd`qaxj9DBkxOFZKU4%=l#Z^t9_78Z z=ZPDqJng47fSWkU-uvqygv~1AfF}RmhGNfe)V9QyQuJ`1Fw;doq!V-u?tU9QSALko zZp!zpGlj~_N=IpM>sgxQ=`5=B#TnnhNhI{vN!(vg1v*;e#7!cdF0Ic-jK`dM@Vi_) z5b4dj;izO0b6$24@8NzRA79Pk>s}C2DBy5MFIp#P~I90%3JCgRr&k%jr~Pc ztE0#chTVlz(X;v@5c2xiy^Yc7O|09HZfg`3d#LnOD0l;di@_U`~WKhjahP5yO6HcS08{$w_AC zp8uY!Cx`AR>dyOx&r zjX19|J{Z6aV&Dw+n@+R}tB<>(+kcJat&3xJeaO}zlehZUIDLN3I-4L}PldD= zp(lgWetxU4Ur%iPUU(SFb1xaI`k{UJN1<-V8QEnY6MGEFx;L~*bB1Yb`$Rpc-BV}B z$DSugi(A5Fbtk9J<|ZCz^>91(0O%YO58np@_$bg!s95F(llNebuFr00*hJazjC+7+p5Udr2fTtc+zQ`I zdhf1y*p}a~TI~+Js}{$s5JLf7C}5s<0?gC6faJAU9vVtsdBhYLnpT3-^Td}g;>_OA zFk#9;0YUI?$lu@pUq|%q&+UjX_&-BId%*hBX3Eve+i z|4<;8=vTLeCd|rz$Sbh4-M6B)mMzo5Gx2OO2=m)^XW*sABR>_Yc|7mNAhsuhj+krd zuJJf5R_YerZI;PO94J_Q{Oj>L=QR(myA>;2oJ9r8j=xftYM!OoB=PMH-W2@(!hRHL z^dMu~K>q&B-y@6!eXM*aJ-^Lg3dogT_gdeE5<-SWh{L7r0+<8K$EmY;6P;Z%eER`s z4K&V8{m@I}bcyZA1z$@j@$wqO3TA9rDNTA^9Ig4&vN=BDMvU3Zo#B2%-U z&X-N+x+%`d4*G?arS}DRPdLk{*Ud|w&O@^Iqb6B!tIqko$$P%jHKpU^*E(=a&1mlg zRQMTkCDd&D@=J%lvW73~K3mB39QvAzIW)7Vk@@W3EBmK! zfwiB-wRBY=cfIbxY=G5tRSZEWOz>?|^I82qTRm3OME?gmv8ozx7`whK`dz>B9X~ls zoZrWJRYyYGIcEU15T{^HIFJ_>ZEArg$rF9goGSC+!K)LuDb*FBrA~f%Gok7&|2&Ou z8sEoZ8+iH^OSEKDPfUuq^YR$m3Xl**&0c%-<6C&5LLF z_nG5*Hp;;X^D2*7QnK)AZN-$Ti08fiyK4)#dlY6GtY-a6YhBj^O@8gSe&5?~{c|sH zg?d-F>(;J<1~CQQK(+HLOETU%sGG)^nA>l3bFVYcj4D%n99)Eurq z1w`HUYTyEZPN@j(NfGyGz>oopjk}gnwnJn@yuebyxBbbtA(9`d<=p8atd@ra$dyf% zKOQaZ3k6U63k*`CG}pr-L&KC5mF<$Q&WN*LSMcamgmMs_T;*4dm45P@dy|7_%*2dL z6k}I0S}@moE80B>#o~i_xHgCl&}Y0JdjDj9!f|u5S}PGt@u=Xq2^t@vGdg1AcZy#9 zj9tCdExOA2*End%#sOHubCRCyyjhJokmi=s6yh07Kcx+cD?Z!(migQb=dneNX=SxLt8c_YcP^G^HSm{s~4?NJ?(5I4EfSk{k2tk z&5t)m%lTe>XaK>2Asf|l`(e(A9FKnV?W_g!pvEnYAsifo2Ek!H6%2n_wq^Qzd6oO( zy#_P`OD02C2F%$&&Y)?S)1FcId}BlxSLrB}dz~zo}$kLFrRf9=kjt&!Z?{Z})Ov=k)nTdMm$8fp8 z{pYqGeoYc<%{3S1(A<@FFS2hWwTXoyrBt0=US)i<`kNG4Qip(sl?@ZlkR5@ zx@Oa4_Jz|7=z36`(TQ@1_x4b;E1SGYGDba&?zP-gGnadi#cQ00&Fe7f$j0+ETVi|` z@wuKlqkGUUk6r%c$y1ljTtjwcoo7ik^xq$&+4KDCWw~WWoCS@IpOG9E@j6R1UF#1a z>13~RK|kNBbcy)It1iIVY-46-R_kW(y}vdF?Ov*?s`Pz#DojwmlF4d;#=T$4^YhnM z@`~CV(Wg)y$9$Cy4hu7>bV&U2#(iz{<$13wUj@KLacAB)HiK7C#5PxMvWRa>hdNmJ z(RrBiJ^kl)!HJ9i^3E$kRQ zR*stBQjE(_Vh!(@YnS!)q#EB#OKC*oJ?eY<04o^b`|GA+u~>jxLGZwnxZQ1hZw}MS zp<2J`0Ca4?nID!bDM5uZ37koqWAHC*hslT_K0bs%I_l zLHQ5l`IP%i{iREn+V+lR@F_zq`cmaYF|_vrr|Z4vD{HWRE7IlDGra%SD`zqt{WBym zdjoG~u7NoF`6=Urjn~F4GIIZ}EqgP2WzTQLJ_$@D`B4qm3U9?ZqVbS{;f)$#tnk!dI za}5x~#IL^bY04U@kz-D0l9(k6|Mf^^dQH{3BBJ8CBL5yK!YIs-K@K+;+rB(13GGe9 zEfP`uI5sEqE%^e|(tZ)5hkBxL{Q8C{XYSbn!OT|j~HDklGC_Px?bM+XIdHo z(;Wp*Nlj4$QYpGcWKzdVX1b%4%BoeZ7S?`TE*~5m zOi#a~Ttsz2psr4lLDYqnm9;V7W$Hy&9K|6%=)JU#d;HG4)gK2m71BOivP{pGGE%s- zU|9VGCzAMVtRG8Frd#-No5RS)Z)Ug_aYZkI-OKrjYm(tVWd`qiGCv(jsf_gk5%l^r z^N*j%s?m(+oJH;F=c(K~UF-Opm-5jdI&XsZ#5n{?Olcs;xu1XkC$D{=G|LwUb=lJS zU$W7&uh}I{a_B7-USgZ`bIA_{hLlEM*Eo*!wk#i}y4S!Ff^KkBsYPcsQ@w}!Iy&!J z(0CZ-$B%!}Io4fcU??!5J0|0#REp+K+}+(p{}_iJPLAdJu6slG>bJj+H~x9ioe=ai z#S5HgnnD=KsXvrha>g+zgccaUBgUliOTbXD*4dN?gx=-@er7*WR|vPWJwkN>2B#%ki;l5fAN!x#4~2S8{o0 z|L3yFxpaXjQN-YQkMYjH(mhYRfW50ml{GvinrbKE^ivomx5;2!RGQagce^wvTSi&n zC+h!287y=s)|#~j52%W9ZP_K3Sr5DzFSGs`m64HgocG?Pd$Qbi1lkF>t&Ki=XwtYF z5D@U_=X`ruSWwWV&RZA!xsy!3KYu)nZw8L%=cwqhjq4~}_S^KG@i`!&zK3{5?R-&O z>ET19`*9Kenazd%&ABto#oT+!eHj&{&3r09iL|o~EbiCsKf)_Mi_c{_34c~1f9VZ2 zTOE3J9ygmC?c$+}7?eHV;ZSdx7(K|k#6^)=G*Zm{qMF#_TqgrF?u^}oUax+a#4}uT zrRq*7(UT;>uoN6NNnF4V`lQtK5T5O_9Ov!x zRr-sNBv!xrr`VZpxJ2{f`E&8@A7qOO&Z?W!^(Xu5-dzGC%CUTM$O}|`RGxY~$MEBV zpDwlGVO00 z&|bPH7d_s6xg-BBg6R?`<^%3MdL#~YuO?C(QTS}(Fp~GFRgRC(C!`)#nPA&v-X2Me zn+)T4?vHg=H?b`(=4WuD?|60NlJZGbc4pvUz{b`a>}k+hE+%Brfz5K50+lX;+?(z^f>Q!i9@mwYShlMW}KTo4yVuKV|r|{s8wk8k9UGn-6pFCsVRzm(dsT~ zFws^#h3vfpa++OtKz8L%(aJh7PCL|jc1fpRCL=r#fIMN z9k@G-(hKITKeO9{T}R&s<@S&b1`v?zl=@?35%ac> zjhp`b?;5eT#3@u#@q+eVs|A(5(5|ltb*4X3Xtb>ykU<8ot3In1cv0NdgZ+w@OU`E*_i&E)^bJJ${O!+mOCKiQi_4zg1ldMfVBn2Z-~(!3Jj!7IkeOKVuS z9(@}9+|nUx0A<0tq`xE4SPMNLEz9pmP@aT8`9I%TCLryycL(o%>sI^Es)o8|zO~AjRZ0u8}Ny_y?g|hlHkF zv~+Z$ULGC%X$>(qGy7agYU=>S0FxXPqpSEIXh0t|E=qv3y4MZ@&xw+eFZJHg?(E$K z(>s*PhCBp%HAxo><@;PoxXRzLZSQ=5HV76LryDORl*k^xj9fVBI7UnGD^IOoqA{rK zyh+!dEgi5^`G!*3R!_c!iQ<8=4`Z_P(v}xi=(q0$gRE# z2Yw0uG=|Rb`NWXlP;MS*46Fd-NFiu?-%{>+Dh_Dw%$pg!&r!{&9ViKyD$CtL)wwP? zvN^^4*%HA+cak9wVpGu~M2m z@z8fU)0&ny6l32lLOu=niQJwGV-OSEvb_b|2M#jzQ}N*S%#T)mb>?ovn=mcsZCn!~ zA+iMX@n-bTWfcLT@W)d)C&*WR89EgvGXtf3yTDGb+$lc3<79;xhMsLpXF?NMxZ%IZ z(SON!j-2~+0(NjrDg35B@u4dBKr8}5QIaq%;HXAKRF zQp+BB^84geRI79Xc+Dvd>NGu66zh!8Z*JD&CSk_Y(ihJZ8O$JDgP*fk{>Ef=e7lL3 z&mJ88%2j@M)d(w0&%eBsrW+P z_#PtqH{9a_>XsnE%i$+83{^9=nan=wkt*Ets#t}2H-CS&^}u}0Pcm6XSFbeYYBAjQ z63)g?x^*ljD>b6P$cX%OvvlkuYfMDMeYge4AR~M8>Yps%hO)B3%>%V0@oLbYyrByG z+1)Mwbx>r?p>kDBN;Y7gf--Q@yZI8!By_;!-mK4Z<}pn=tTt=C98Ziq`v4Gz=2Y%Ji-7g=Pqr;j_d{_x zh`u96QPH(v->csS_a_IBmo$OHq zlQ(gTRZ-Zt>soJelope$b3Var(@HX$s-N?k5;oA?4Mqy%MYJCj{%`{QP{zETaFDCp{^eQzH|4%5wfGt}yU3)AIh|ig zMjDF2pm8DNlVa;v3J=A?*&e83bxu!?Q9sD3Mz&(JOyc;xufZ?r$}3Dd8Rsn7K3P4pYE)hgv)W2CfINOFSwl9?{tpOhueqZGm|H-rIoKG~6buUS=h{`tGZ( zto-rmM*o##8tKIIII=kkLygiydwu_tFdFIp{M!-DlPu=G5yI0jnxmD~So#o>$j@ha zG>jA!1KH1B0VQK-_jTwi%-*)Y>S=xn{D3 z#jyl+O;D>%5Zu5L2HNGpYbDUV)@`UZsbLvgR+EFy4wmpDG(0#(#Xp4n{QL&{6S+;+ zGyCQ&FEAMZX*6ptx9jlJC)mN@kPvkAb(*67!8P`m)rKBHB#rSxj`A-)1qaiLy!y+M z{Mui<&UU2m`*%JG0}0|k9KJ7JypTi>qCR`m2|3tJ9$pR^n4vTDeAyK*)UI@oY>>=Z zn%Wo8z}9Qma?v}O2b>(dyu4-A=f{D=`Q33i(>s*&AqOKH^WGJw*%54b@eb?ZYS^%J z{E}@_=pr^Ne!+B!#foXZ&Gay+B#v2p(ne5$VAH2{;g)D{u`JCpRlv!}O=@K4-_1C$ zkGH*FzkVG;$yEa(3=#)H`|&0IJP%_vk=KV#42sX7*$NE}jhNfoMV00riGzd&`$*p1 zm3+OD2dK*>l%y{@W4FN;B>b)lH1>Jo;!@^YtzjvBog{Sf z_%U(h(cXX>+8;sV0YDKlTZ+4Rc=c6{$nzKDK{G2Hw!DZkYn~A#e`oxc@~;E~kf6GE zF(%A-S6p4`v8N*!?bEu0ejf@wkz+S=2DmROr5Ez)P^y{N+9W?w(Vnmp8% z#wsf-*Db`^E_5C-#tQQAkj}XQvWCT0R#F-;QBNPUt00v+y(b}o0dOr(t*WRP$7d1z zDM$17Iy`eqjtLidx+LzjVQP>V16uro+=erAC9Ftax<7&KT)XT|N zV{R=aICuB$j(i>^yh45T&?s>!gBrKz`gy*2XpkB5D}820u8)@h+A;ezn)p`8mG|-; z7m)c7=%G`wj+?5DdD)woFplcSkM4X{9IgBwbn(NjeHs00`jh(A?3yVN!Cu}WM1vXG zv+oV$LN1<#Se5%z6r9{PwUzmDIT)AX!dQe4;ia26&AA8D*u_rw7!nqur`>m?ex9H^ zouQzTjCWq^+nPhki*#rd{SM-*mXJ5L86^@Ft=Y}8K3RprHt z%>7$S+=N16g6YnkBgkTnUMZ&>)n@vB|t@yN#~E)t)D+K`?bI3G9Se=!(tQ_Y60x zX>XbRna>#H9YJ1BW@fooMq86jVNwRsJ-9n^wEO$V%)qs?CkVtRljF^`pEoEQhq=yl zqmQuMjoupf>-jHF=^ZoD_|u{0aHLb0n4YMk_8q%KUM?yR8LbW%&UOb~ zA##puQAX;hGdm9t+2Blfc`D=JX;Y`YV0vMVQK=eJr;X%z49nMu2$$n!`}ZUa$9)P+ z{}vY5c`Q0(c$>mpf+G^kh9-vcbWwkTIoviTb1sBVc}u+Fck4-#04p>sw~1tb372R9 z1%%e@0Z&#jM3nJ%BzW9^N0aUdY;WD2c5Ib+wE2yGxea)Qp5Q08Y|T(>%u8c;nKL6t z1>+F`->Pq=lEeLwZc&JNq9rt6^XOH;^<9>!t3Y&nP}X$g2H3|vwe zF39^Z^r-Yjm#F(j!j~`4Rw-8krA{HMK3O}*s>YS0>bZu0S8fJR)vQ0GgxBcaJ%bw@97<1pXB%QaRNwwFx(Z%0?*m5x}f4CPs(Y*;;v)TA&jPO=jw*~M5+xO9=6ob0&iS0HTNeqicXxGhEpBFUMV{vdY*q)pu2+yp9U;u8 zeO&>XAHd~njjLX5gczn9a$Fh`6&ELJ@jGI)c6RDR#kJZm2F(X4ItBXd6SM8^My^BJ z`q1tG%wO1J%d~zAI@06?Vsa1&M22RwEji*tlB&Wdg5sSWU)eJ|Q14@l2e<(4yA zzk(hWy~16$^&nomE`+=0=gv?A_qTSr5J~~tVdc^_f}-_XqZ-msmBT7{k=h$c{Qhw5 zwp|n#8D(XFKivfcLRb*ERAJeyv~Ruw!Y*z$6BX;j1qRyhhpqV_NW=-)MlF)lcGq2Z zXs1+CRXy8kp$V2}2#a$Dco<42RDu6nnfJ7n%4Esax^%x}rNq2_oO69}bU;|ZX0UkX z4!y9`Y@-5`&-3JV)yI$fq6AVE>ZfO!S23?o)+?Qzoy)~et__cyLqZjXJe@b&rx0Gi zg|!Np=fm~mcM-k3qWf0XG7F5BN!+6sS1&Atibn@yQ$uB2vS}f(|HQ7q%qliNKOeA| z$04xa@4_~Vr#8~UKx@)`fDzBJ!Aen5rd2$8T6_O zJ#1iZu_;iT>eBL_cy5HXLQkeZzkIyvWtz@llhu^o1?(o)7cxBPjNaARIoM}IBUfuy zrI9ADIJrIFzWDE-wI>70IqIDn9DeZ}jyen-S*NnI{+ zC{F&aa*U~SWZM4QqK_j%?ZS3ky@f_%a+^E4)ePZYVi>`0s+%Tq~2XhZ%zA|22A?naR2TrtoU!evr^f>9obG)6xgeuqQk1d zW?iT$8})g4c#3q3jPW-UDQg@w`1tr1y5cPfZ-~ARB4c4!6v(?XQ9buwMnptpE{P|? ze$47MxR`@UXEiUc8hdPvH=IvaDp2Tw5#Cn7^O$XX0|bSpjea$FT8_gny3AK*0hC+; zY8XB}SJGbLEk%B)?FYHVzJ<=%@YvWK*ZGQ^tgd(@684|6F;%wz5aQx#BOdAbjzG8( zng7VIUuM-;{7lyVrhveZ34i+R%UoALs*K#DKwE;n3+;auKYJy&3i0!kVo|H_<5%0m z`A>I8&BMxVhJKsCo_@oP_!~!nV1IXhezyRBG$65(8HvADnyP{MpIuZvT3^+WeXo|= zR7L5#oDt1=`s0;u7`*gKBQs-p3A#AC?99IZ%WZ{miP3xOkA^U9bdFY@=0p>7V;?^4 zH@?BEuC507X=l5pUG(>E29pBME|(QP2e zTY3pWYF{fKw<5~;m4;B%Kz~2v5>6QO2uELejd3j_cE1u8O2fbKo{qp3>=S$`8yj1N zQ?brH5(MJvO=!q{ZW@cYqpY}6hqHJmz2AKr4hkx#`(5PHx>JW6V`F1&rpC+F%C#So zMS@14tA9s-g#Esl*vT70#$FfX%}sZF{7TQw>P1HTnLh)Qo{J2EN_DDxGnDp*+xab! z)hIZTRZ*|lOytXNKu;x8v9_d@?P%0#hUn7}{B|dR^y7T;9SJYnXwJdcZjThxNZT7z z+x`BvA^f7gQ9kfk6v0YY5#7DZKl>2FIXed^Y`u*-I$K)XsZhT>7lN%|C9n#z@2|v4 z`W%m!+X5^~jEZ_OP(J+g%WZou+d+IHN|*KVp#_2`LDxUpip?R6Yh0{_8Ol}C5W@q| z*{p-Rb`3pZG6U^Uwr4!^a7Ui8ItaW15qlLAeF5?KBRCqs+NA$`s}I49LD=aVgDk`n z>=_1iyuzW<)6uayy4`8on);(baMtIALD=-0lhd*D=MKvI?{i0$c8l5q$6>70_Fer* z$h6T9p1XdlnJ7=Us3J(Q7m~nHQn&sw?LJ|P-{f7I&c7$Coq-`Kc&aGl7~bH7=*WmR z&JC!?{ICMW)aw$do_VzS$Iyu%a7og2xj#cKSyFNP+HLud(6R0Ga2s+Bug!W?p6QPHJ7$&38RP_fMdKI6%q z!Uz8`j6mE`QLYc#p~}gUQ$(`Kw0apfQ?`*-o}S>hZ9PwXXD1wLRR+pO`!keQM~XJ> z*^)%ZHj3iP%F0BX7d*VTR1$=FPp4YlOmS3#IBX<_c2Wz6hskTA4PX7TO!moCS-!Y_ z!yUk&*MY9EK7@(4>AFkfY3?^lHY7Z@zDW$Z1s`Er&YCE$c7AV#6=f&ay@^Oj8PHHi z(X#KZktfg3 zt`@gD728DIRnnzpDh8SAO_QT9slF|b7&>>o!6Ph_seOroVBQkNGYA+b5zt%Fld@ov zwl*Q?a01C`efHKfJOqNCa4q*Df?clnMXp?HrkQ$}V5)V6a>hjxyQSU~?5-5noXZRT zZ?oY<=dDEfUBhD2rlr0#;1KA~B#qO!Mtx@eQ>^GnGg(+z^1P8d;mIE%NF43_BP3g>5BDYvFh=Lhj_PN{<{Ry3m$G@zW*BwlI$88iD#whXBy8tu=#|sUO z&ly@lz+Cp@W#H2Br7++=XN9#+he#0*e=G5y6gwE+(s_lTlUs&EtNl?1&-Mq*y3B&( zan?lIjgBT^&c@bR%~`05t(UbkdfP$ex7Ka_`yXP*ViBC-K-j)Zt)M&uLQ^{ zgIIkDjgBgGcxeUgM(d6a(6p|f?IF58EzM+>Kp+}l{^AZpeACp6u}=3wmkk$8oE{)a zYqs0s=>)CT9wC$fHc*5HYf_{!YS($?`aSu^xC472ps!r?z{09-TD+}k0$1@jOOnT} z!{@G_8)!>HZBtuwN5c$pxMK?V2|Rvt&SO^y?fa93Z^HsYSkpJQW>;D)pVz(v?>^CIK=xck;Szd4H5 z#!43_97%1}bH?5K*&jwWycE%AJ)w5?@;YX4zhVRjx#Gt7p!##Pl}c83TC#DVz%LQefM%yV!#U)ExO8rwt1-zEgy*E19}BUVJGQ4+bQR6k#>_mA50aX zQ23#qZF)!l5dLxa;p((sCdAgnica_(*wc<431FcH*yFOF(vG2!s8Alms;y3bF)Ob1 zRvkh>pzQt~oQQ1BU%!44QF4BCHO~KI*7Ea9n8CLrrHIG+`f>jJ@Lv^M9BVWzCp=k> z3_~rDw{S|2cY|G_a%@duperKky`R4om`7BzlA9MXR#E3x1`Z9>15c|6!t~ctL%4(M z-Q_M0Pk})|0%$+Vgsciv10! zIpEqqJ1urEOA3;CcgP?h@$Z6{qvF)tnwTNEBx!1Rwri}TGaO98;gVpZq@5uz|B-sG zheIEbC}1I~lgU!|RS#GN5RCM@h}{#*ogKAH2ZRNshH;7UI=D+-~pg=?SxMc0SyCMmGld7Sgl)R`OpJuRWKt+rpR}CuU$b7>ANu znQ|AP_}-`^;_?XRwMW}Wnw*@RIL0%^`KnOEm#q|+?Furq*Z4cbS=4lVZKI;f5EVnP zwgsufxX&oD=X^2LJC$~B+5;6pW;oeOiFgwAa^CE*=WOFmi(w0#2be^}mb`(JX;Lyo0X0Sd{8~~bpJXx2Tc{PUD+)TmqM+BSJ8uPm`k*8sC z0#36673qS7(JzWr!-QsDmqrBzY<@|ev-2MUh@viB7=;i{ZDXS;we-2ZDzT~63v@+C z=V_8u(LF^)ido3T2jA&W*XR~hS@or6C3)q73jnv{u(k}vXTV|ttquuyA1W#a2aoiR zFjDat8x|Q$Al;rZ`X0`W5{a1stb(eAnu-eAk|~8MqaC^=84-*!xJFm4B*7iXLBAZ0l(60eVE zQM$e;tx&$Wo(h{@>%M4>613WEMP5FJGo|_Cm}hfq?)3CDMHac(BX(>znb~E{ggh;Y z>U)9q7%_5}I6Iv8%J9n7%?&NH9Z?Ojj^rygf>ZZI&hQG(hjY*QzqyX&`q#=sxflh& zMDWz#{))MCs-#H8wgRK){r%E!^)Q3V zN&}9wor4UQe%UDKU?7I%OPPS4;18*CaIyh=MGl0N0ZpB^mko<5-PX12-#nNC9ff}X zZ=L&v^T`t46>ZJFEWv;kA(-hY%GUs0_l%* zWaIf$juO7tHUS4D27(iZ>VHut*yE9XoOv@<23Tq%H@6B}HISjPs3z#eZPrOZJSd!~ z@NwRnO~E@M-I$CWfD{L*iUWD~1ialFyc7H0xg-8h#q*rODD|gf@)5^?fGSMZZ39!3 z(b25RfO4_uBII&n$-e{W@O$98zTQu+GV(zeO^9AO18}Bg8;P|73%}apO5vBPhd6(I z;AXlzI?5lN2Sx~WAtJ8gz$8aGRh$ANfx=>5oYk543g%h7ND{RjZm%dD{~m)lFU!4p z^~!&Tp4aT%T>`?)FMr0&EFf?#ER_Ju0^YtnZHRYC8@1J(j0p&K0@!LlTHK`5x07p9 zV5KJDfWx`+oWgf?$y2lleFER)7kV3VPs zP-#Dr8Bp-AOfg6bzra3sL$zTgHyL?2Czb5+=T~WuWNpm!YJWxtAdYgPb)}a4r=<-C z64)&H)vZszqB!{{K9Fn{)@}sj=P3lH%sP&j<)GBB?GgB%Znx`?nsn9Pv@_uwg%UH6 z*O;ZWu$9Dv^B63h8i8fM+8D&n#+Cyh?>v$Kt+lhWvlo+`e2&3!@CXi-$j`L~QnMN( zzo2emHxMpCn`2bFKcs6S;kKri@vUn|57GX9Z;y7LtEpA}7Pmo#vVz51w{+rhwLtbq zMXjTW6Ymm^?Zmu7*5u2Um{&trrfMs@%3Ci-Mn-11&q5$b;9m^qkx={ti;Dr*sU_tc zPxT+d$|Myh`_Fiph>HCSSIfklwv%;7lRlh@%ew z5N$>u(VQAf1Tu_?1dVT{iannSh_{EHo<>R`SdJ0ABK7PDX&wu6&oBA*i zL7>j6BnFA&$JFjy1CqD}AJ9~yaXKYh!LQr_))-%Ji&KX*6a9lsE!!KUs?P@GCI-#G zlU59sztO}m{nEVemTsxUEqAfV<8Mz#z<`sSp3~5lPF4^0UI! z)WKKbEU2`YC-RbD_^`<;YHB=tgLCNb&FFN<;en^tKc+0hUJM1|Fu=Dnvf^n!qT+VG zcHS^7QL&>*Lq+C=a(jZhL~=6#5Rjudf}93b@M^&O`?D4TWYM%QA!)-{vhH0k*MNo= zSQy~VN)Y6VEQCb*p@35FTlGqpKW#nR4Mr$vMj+IdlcGp9FK`pSaoIIsF7u z5yNST+`%H&&n+y_8&fAmNX#k(JaYlP&x)=PfX`Wb$O2$nF#VDo|mn&Y;1jMw@>7v0r@&RURxDvx!zPss%LoT-%D1DgI6&jw9 za0=VGwmWLnD{RvZbSvnXT4%7nrGN;iD%d2rj1Wp5w{27-oq*;Dyh8J8v;IJGf7Bc< zA+N#d5%)RXJ1-I;TVFWm+BkBiJa$&Im>0bYw?UMY;2El?C|xnCnqOK7{*VOs8kKOT@is4&UTQx1AIAy zEG73?o4^3i_AkKTeJb`!le3-ZgDH0ts2Tx)#XN?Ma#GY72|=i<1Dgn?Ay+_s^#G7Z zjJ@r_`yJB1f~%j0C99ZkIgl8+gNr(I?t#ItYdrc@ukf8XyJTl7?X>PYz@KEcgV`x> zb%R$;pP+1JiRlD^*KDqUPw*p4FuIG|$eY6mDf0%@ur{aYYTau92y3ZflB7025Hfn0 zLfr)v9Os&2^j-j!aqrhRz^nxLt~$zGA~GctFTW;XQ{shQ;WDgp0;~6;@H$s@0;IPz zfKO<=)LnQHXEsJ3WRR|_I!a@clZFz;?o+O3;$ht^j~_n<39HxO0GQGF5=RL^!@14= zj1Vrvm2%fM+@<8;?s_T)4K!7XZP#y=^znky!0VY0>PaAJyFm;&Na>I zFDPnQ7ZfBi($fJ2Ey7+tc|N`a1_X;!`GVr*LXe=JeF<0r2w6m;P>c))@UQ|?l5F-J z@R+kde&93teWl}j{y{Dce;r~Eq)dN$IGSZiB_K6$;1M*ZXv&l1o3qO@CF?b>#`*%cBqAte8$` z2~j6go1k$|sfXJi10=FxplkE+aP1C+rC|}~K14)(X0`=NNb=}!IGp2bUpQt1Y2*Er ztiP)llj7s!^O!@gm;DFVr$sRPxn3{bjnQ|9Gy|Rov(v%j`VJ~zr-2Id5ML@~hn{7R z+W&1LWO~BVZVwp`f`zZ}2oa%ZW3vKE>bb}@;|@~`OL_VCQnmNk7fB^OW^{PLOYSZ* zWN_lpKLRZPSdYC4;oV?kVnMTITj1}VC+sT&kZYwQsDvwUxK&*YcX3z?aPWMs6Riau zrzdl?SfEWH;3-O^bq^Q{)Ogo=PAXsF7hCbHOgLe3y}5kQ*%$|?f&jHC3T1>fyq*LE z{Hk2aLM$R8YTOkRl*6$LG0FZLy(t*jnX>1QVHfnHkXW#bG&Pxj#HI|R2_nwqe?2{` zYa>yfC3{l9)-ht_3|v z;ObeicBgGoeI}w}lS4AK4u_MN_I?%kkaI>FJUDjxWP<_9I25{ueHHcw4APpKUs0dk zSN>N3eS&X@#bJG$B;oBa_Fay__CVkH)hn;PrDuy|MI0EikkGtW|DZQYiGD?S6t~fM z{?mc9=n->IysA5PVQtB<8GgBbt5OIHb*MD}T>|^4%cjVqPMiw5lR|?m(NX|M)wUuky+UKBZJtm&2VSDkeaq#Y4FYn>dY0& zfUQ% zI_r&MQ-qWIy|k3e4_>Hj7{F7}^8QqH?o4|6UlInTfXMHv^2qfoX0br7sP zxYW`)7jQtag$4q6Z9LNXuP*Vw)3XCqrN!mQgak;j&Yi&+yBF}=iy)0Itlu98uRWBl zk*1wD;EP$E;WYYg_^i$gJq~Qmk?ncK@{$Y!kF^xCMo%W|eKv>4@2v^igN^6HhW?Qi zcc@d_7)fx0TW$MD8qH&Jtv2%YL$E4U$?7=NUHEZzUm&#j0NtxX<+%7*`+Kf6K*IGN zjtB6RP)^JGH&mah-3cjv88Mw(t0aA*J9@|m;|f3O_wI%tPQ+7?p-G*r@e|D&8CR&r z+&iyxB^zBhp-{}7u~Rcd*z%8!i9n22F3_LZ)rB8JT=xf1^+NQ`6(&i;b#@UEF#%4) zs$$?CV$;Ru;$>Yo~d%%=69F=^E8l?GBYz%JNAB)f%lx36gDatM|z;h zppJ=nL=XEZLQF5*+dskyG9W;<`*}V@+6DUCQWLS^z6mlsVD|03ezj>m0_ppF+Sev}BOt~J@nj3qgNoDu#No{<`!@sxm+ zF=2iTPMF*NCrCX3aWy9EUKVB0CL&Hqgzx&S4jZ{+J6(4K@V@|Yu8xQN0gk(FW|%Zt zBYek)>*v1*CT4%FEVCb-j>pfx_@58@e`D!x?IN#SBRv7|e&FZr&~UWitRCRHF*(MB=ckzYpDo=! zea(fc(l>u}D2-@Qx!^>h@ZTNr^YCqmGBGURz4Q8p-`ri?s^HuI-8pG-DH9w7D_#R3 zHSGTk=Km80AiaEwNe{1}x9tTj%(viM|9#jDvXL4&Sj_+PujW|_-zne<_L)#ZF+k$K zhWpLs^arv2h&*r$JL%!~FATi-ei+$~#6<{8p5vi&3-)`EEkhabF8uGz6x~Hv$9tLt z6$7J`zZLVOc7EPITToT!so(p0VEHPdO1B{E?Bi#UiuWe3^D)$%-EhqZ#(sw-<@uh_ z+ad;p(}n+@Bnp+0>>5q3x51pa_#c=v`lc~7I@QKYjIJFcjIy^at{POrWLcy5&q6DU zQrIK0OCTqtQ@N0lkJXfIMoOo`$6PH~>T!$3FJ2<$+tAhyzRoDSMcRfme&f$$H;Qv- zg$XEW!vFiza(nsMsxQ~p(@-_exdjs$A^(2RyY4M(jakt}edk(QsqeHSx>QY^X;WSh zX+n;Z-`SU}abtsdbu&n+B`u)*_e;t~`9R}pcL602V9?h6FpCte`?m8#emZA~NKEdkMeG79)B8 z<{Vj5{Jpc7UkyJM{qQG{@08Kh60fTBNdJtBJoh(#aZdh~efLeaOm+@Qzqb;>Qoqt} zmp#om=Fq%?aXtpMG|tCU78p$g@*Oc8BlU#0D~YS^fIT zd@iy8O$<-<-|Ili@$)-_P|SVuWgvEvAGC`Ti8>?lf80iZ$UT>ggRmTE38?7nz=bTk z3hMcPu8EnoUw! zsdu_~jmXW?&dkZQiK^^3mL~X~E-V-_HV^_dHx zqU1KDzEB+1_f@|Es+e%qHdV3Qb#+*fBOe5HKzXc z@yw|0MNgxGV@$i$*?AHlxdoNRb2%R*+SbF#pv{3_H)e(~l>Xm-LOS64s%F}%w$VDF zi?#iL>++J?j#Y_i6uec!H}QF^teVcKt?8EQ{joT>Rqt{^POfo@_8PKCVZeZ9WNx1_ z1S^hPfq8&WbOFh=N4ac$G{Y*e4$0cOeBSY{Bl2VdR&FSPy+_x?XDI3mRL_y1NW!8GZ5LE6%!7n z0Rl=ydiv0YOIxgb`Tu;29d2E77n)5q(OayI{uoL_qYx>!m;MuS&*tkl;RPe~TcNX(OmM9dwURZct%A~NM7yWrGYBFF(U-18x#?;N+@XJloOLIMto zpyBD4wPg0EJfH)If_mY#|Esby4QlF&;`mEUWK|)8Pze@UM2LinEJ2nq5flOx%OVOQ zQ3nZ8HpSwCBnBx8$|fmQk$_?aENR4vqKFAtj4WjlGU#YANEs>!L>9r2r6+u_e(l%$ zX5PH}=G}Ad`JMm&KI8g9HINTb2QUEnbbHPOK_RLfLBerpO&o?iY+sW8aP7edj5M9P zz+HEJ;ZMK&llxqL2$}em`_c6>jkzkH3U5nfWfk}k ze4$(1babShjmi+4rz@0G%Fy+@u^ZKHG;zbnmcRCk*Vy8RbnH4wVtra;m1B6y z&5m-MNPIUGoAP-oW!+`rZA+4=sQFUnX~uBeu6}bVDHT-)wnI1x07TIn&j{Lq4Xo*O zijqQ`J!4@gM>&e6qi+4C${2n!@=_mOcQYFcUCirh1wHwM!WOaD`4TVNiPTmBvQi$V z`)96HH&$C(Y{BYyN0Mu&Eg)}nXpJ^W$p3)gRR_Ii2!ksB}5@Ah(+Pe8x~JEHRne3o(YzdYGU z4DXrIW9JpRe3~$Oyq{Y_YR-Jxl4DXT-oB!&uuC74GemY|A+Gkmi}mN#k1pm7u>Pp3nHzdT|`%jS)$lU<1)wyL&V})%K3{+S2JfOBTmnzD?&3WowUzv|IMFbW8qi#M?Vx zvbi~u4>L2MAdyb37kJ?v6_$=kg2vsC%q!1N$NSbu6Gxr=`&WDyG47#*^FzeF+*0!E z)$`)`{DSI>XUww-rhs)??ANB@uN{bPGOGT?`eYTqU|%UU+niw|?^*19llpEF8LuH* zSrdz)OaW|+XlfE9TxW@0Ar(}Bm70MFw5DzY5g2X>u7S=0lA30q7K7GMJ!GfN68J(` o_GkakE&R`EfH8uU$;JlM>M|{Fo4;KhVW){80e;{6@+dU+znF3}-2eap literal 0 HcmV?d00001 diff --git a/README.md b/README.md index d651038c2..b87a0db2a 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,7 @@ # bepuphysics v2 - +

+ +

This is the repo for the bepuphysics v2 library, a complete rewrite of the C# 3d rigid body physics engine [BEPUphysics v1](https://github.com/bepu/bepuphysics1). From 9beac0122935a1d292dc0b5f81f58f59c4e36ffe Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 18 Feb 2022 12:20:07 -0600 Subject: [PATCH 457/947] 2.4 out of beta! --- BepuPhysics/BepuPhysics.csproj | 2 +- BepuUtilities/BepuUtilities.csproj | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/BepuPhysics/BepuPhysics.csproj b/BepuPhysics/BepuPhysics.csproj index cddd98c07..159eb917a 100644 --- a/BepuPhysics/BepuPhysics.csproj +++ b/BepuPhysics/BepuPhysics.csproj @@ -1,7 +1,7 @@  net6.0 - 2.4.0-beta14 + 2.4.0 Bepu Entertainment LLC Ross Nordby Speedy real time physics simulation library. diff --git a/BepuUtilities/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj index 16bd3c2e6..1032ab299 100644 --- a/BepuUtilities/BepuUtilities.csproj +++ b/BepuUtilities/BepuUtilities.csproj @@ -3,7 +3,7 @@ BepuUtilities BepuUtilities net6.0 - 2.4.0-beta14 + 2.4.0 Bepu Entertainment LLC Ross Nordby Supporting utilities library for BEPUphysics v2. From 7b3a2e36feef2fea6bc343ea65ecc9a0921c3e40 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 18 Feb 2022 12:22:12 -0600 Subject: [PATCH 458/947] Readmeaesthetics. --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index b87a0db2a..a1b522b96 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,7 @@ # bepuphysics v2

- -

+ +

This is the repo for the bepuphysics v2 library, a complete rewrite of the C# 3d rigid body physics engine [BEPUphysics v1](https://github.com/bepu/bepuphysics1). From 64d295b9f9dc7fb58d12bd910a4fbdc3af977cdb Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 19 Feb 2022 16:06:07 -0600 Subject: [PATCH 459/947] Missing changelog items. --- Documentation/changelog.md | 54 ++++++++++++-------------------------- 1 file changed, 17 insertions(+), 37 deletions(-) diff --git a/Documentation/changelog.md b/Documentation/changelog.md index 6fa7142c9..675b8f4e6 100644 --- a/Documentation/changelog.md +++ b/Documentation/changelog.md @@ -5,68 +5,48 @@ At a high level, 2.4 was a solver revamp. Data layout and access patterns were s ## API Breaking Changes 1. The library now depends on .NET 6. -1. `Simulation.Create` no longer requires an `ITimestepper`, though one can still be provided. `PositionFirstTimestepper` and `PositionLastTimestepper` no longer exist; the only built-in `ITimestepper` implementation is the `DefaultTimestepper`. This is a result of 2.4 moving entirely to an embedded substepping solver. -1. `Simulation.Create` now takes a `SolveDescription`. It can be used to configure the number of substeps, velocity iterations, and synchronized batches. There exist helper implicit casts; for example, passing an integer will simply use the value as the number of substeps, with the velocity iteration count set to 1 and `FallbackBatchThreshold` set to the default. `Solver.IterationCount` property renamed to `Solver.VelocityIterationCount`. See the [substepping documentation](Substepping.md) for more information. -1. `IPoseIntegratorCallbacks.IntegrateVelocity` now exposes multiple lanes of bodies in SIMD vectors, rather than a single body at a time. It also exposes two new properties, `IntegrateVelocityForKinematics` and `AllowSubstepsForUnconstrainedBodies`. If `IntegrateVelocityForKinematics` is false, then `IntegrateVelocity` will not include any kinematic bodies in active lanes. This is convenient when applying gravity; if only dynamics are ever invoked, then there's no need to check kinematicity prior to applying gravity. If `AllowSubstepsForUnconstrainedBodies` is true, then bodies with no constraints will have their velocities and poses integrated for every substep; if false, they will only be integrated a single time for the full frame duration. All constrained bodies are substepped if the solver is substepped. The vectorized nature of this callback is probably going to be annoying (and/or confusing) for a lot of people; sorry about that. Maintaining a scalar callback added too much overhead. You can build one on top, though! -1. When constructing a collidable description, the overload that takes a speculative margin now means the *maximum* speculative margin. 2.4 uses adaptive speculative margins that can shrink or expand according to velocity. You will usually see the same behavior, just cheaper. If you want to match the old behavior as much as possible, set both the minimum and maximum bounds to the same value. Leaving speculative margins bounds at [0, float.MaxValue] is a good default now. Note that there are some new implicit casts relevant to `BodyDescription` creation that can make things shorter, including just passing a shape index directly which defaults to passive continuity with [0, float.MaxValue] adaptive speculative margin. See [continuous collision detection documentation](ContinuousCollisionDetection.md) for more information. -1. Statics no longer have any configurable speculative margin settings and do not take a `CollidableDescription` in their constructor. They still have a `Continuity` field if a static should a static need continuous collision detection to be enabled. All information related to a static is now in one spot, stored in the `Statics.StaticsBuffer`. -1. `ContinuousDetectionSettings` renamed to `ContinuousDetection`. -1. `INarrowPhaseCallbacks.AllowContactGeneration` now exposes `ref float speculativeMargin`. Most use cases can safely ignore this completely, but if you find yourself wanting fine grained control over the speculative margin, that's now exposed. -1. `IConvexShape.ComputeInertia` now returns instead of using an out parameter. Similar changes applied to things like `Mesh.ComputeClosedInertia` and `Mesh.ComputeOpenInertia`. -1. Some callbacks that previously had `struct` generic constraints now require `unmanaged`, like `INarrowPhaseCallbacks.ConfigureContactManifold`. -1. `BodyOptimizer`/`ConstraintOptimizer` stages no longer exist, and their profiler entries have been removed. -1. `Solver.ApplyDescription` no longer requires a ref parameter. -1. `BodyInertias` and `BodyVelocities` renamed to `BodyInertiaWide` and `BodyVelocityWide` to match naming convention of other wide types. -1. `IThreadDispatcher` now takes an additional `maximumWorkerCount` parameter. Specifying a maximum worker count lower than the `IThreadDispatcher.ThreadCount` may allow the implementation to do less work. In practice, the `BepuUtilities.ThreadDispatcher` uses this to significantly reduce dispatch overhead for low job count use cases. -1. All body state used by the solver now bundled together into `BodySet.SolverStates`. Includes pose, velocity, and inertia. -1. Constraint type batches no longer have a 'projection' buffer; anything loaded from it is now recalculated on the fly. +2. `Simulation.Create` no longer requires an `ITimestepper`, though one can still be provided. `PositionFirstTimestepper` and `PositionLastTimestepper` no longer exist; the only built-in `ITimestepper` implementation is the `DefaultTimestepper`. This is a result of 2.4 moving entirely to an embedded substepping solver. +3. `Simulation.Create` now takes a `SolveDescription`. It can be used to configure the number of substeps, velocity iterations, and synchronized batches. There exist helper implicit casts; for example, passing an integer will simply use the value as the number of substeps, with the velocity iteration count set to 1 and `FallbackBatchThreshold` set to the default. `Solver.IterationCount` property renamed to `Solver.VelocityIterationCount`. See the [substepping documentation](Substepping.md) for more information. +4. `IPoseIntegratorCallbacks.IntegrateVelocity` now exposes multiple lanes of bodies in SIMD vectors, rather than a single body at a time. It also exposes two new properties, `IntegrateVelocityForKinematics` and `AllowSubstepsForUnconstrainedBodies`. If `IntegrateVelocityForKinematics` is false, then `IntegrateVelocity` will not include any kinematic bodies in active lanes. This is convenient when applying gravity; if only dynamics are ever invoked, then there's no need to check kinematicity prior to applying gravity. If `AllowSubstepsForUnconstrainedBodies` is true, then bodies with no constraints will have their velocities and poses integrated for every substep; if false, they will only be integrated a single time for the full frame duration. All constrained bodies are substepped if the solver is substepped. The vectorized nature of this callback is probably going to be annoying (and/or confusing) for a lot of people; sorry about that. Maintaining a scalar callback added too much overhead. You can build one on top, though! +5. When constructing a collidable description, the overload that takes a speculative margin now means the *maximum* speculative margin. 2.4 uses adaptive speculative margins that can shrink or expand according to velocity. You will usually see the same behavior, just cheaper. If you want to match the old behavior as much as possible, set both the minimum and maximum bounds to the same value. Leaving speculative margins bounds at [0, float.MaxValue] is a good default now. Note that there are some new implicit casts relevant to `BodyDescription` creation that can make things shorter, including just passing a shape index directly which defaults to passive continuity with [0, float.MaxValue] adaptive speculative margin. See [continuous collision detection documentation](ContinuousCollisionDetection.md) for more information. +6. Statics no longer have any configurable speculative margin settings and do not take a `CollidableDescription` in their constructor. They still have a `Continuity` field if a static should a static need continuous collision detection to be enabled. All information related to a static is now in one spot, stored in the `Statics.StaticsBuffer`. +7. `ContinuousDetectionSettings` renamed to `ContinuousDetection`. +8. `INarrowPhaseCallbacks.AllowContactGeneration` now exposes `ref float speculativeMargin`. Most use cases can safely ignore this completely, but if you find yourself wanting fine grained control over the speculative margin, that's now exposed. +9. `IConvexShape.ComputeInertia` now returns instead of using an out parameter. Similar changes applied to things like `Mesh.ComputeClosedInertia` and `Mesh.ComputeOpenInertia`. +10. Some callbacks that previously had `struct` generic constraints now require `unmanaged`, like `INarrowPhaseCallbacks.ConfigureContactManifold`. +11. `BodyOptimizer`/`ConstraintOptimizer` stages no longer exist, and their profiler entries have been removed. +12. `Solver.ApplyDescription` no longer requires a ref parameter. +13. `BodyInertias` and `BodyVelocities` renamed to `BodyInertiaWide` and `BodyVelocityWide` to match naming convention of other wide types. +14. `IThreadDispatcher` now takes an additional `maximumWorkerCount` parameter. Specifying a maximum worker count lower than the `IThreadDispatcher.ThreadCount` may allow the implementation to do less work. In practice, the `BepuUtilities.ThreadDispatcher` uses this to significantly reduce dispatch overhead for low job count use cases. +15. All body state used by the solver now bundled together into `BodySet.SolverStates`. Includes pose, velocity, and inertia. +16. Constraint type batches no longer have a 'projection' buffer; anything loaded from it is now recalculated on the fly. +17. `RawBuffer` removed. Usages replaced with `Buffer`. ## Other Changes 1. Added `CenterDistanceLimit`! Useful for clothy stuff. - 2. Added `AngularAxisGearMotor`; transforms angular motion with a multiplier, somewhat like a gear ratio. - 3. `RigidPose`, `BodyVelocity`, `CollidableDescription`, and `BodyActivityDescription` all now have helper implicit casts to optionally make configuration a little less syntax-noisy. - 4. You can now get a `BodyReference` or `StaticReference` from their respective collections by using `Simulation.Bodies[BodyHandle]` and `Simulation.Statics[StaticHandle]`. - 5. The presence of a kinematic body in a constraint batch will no longer block another constraint attached to that kinematic body from being added to the constraint batch. The velocity of kinematics in constraint batches are treated as read-only. This will improve performance on simulations where a kinematic body has a ton of constraints associated with it. - 6. Broad phase dispatches combined. The long-waiting broad phase revamp is still waiting; this just reduces dispatch related overhead slightly. - 7. Code paths dependent on trigonometric approximations have had their accuracy improved by a few orders of magnitude. `AngularHinge`, `TwistServo`, `QuaternionWide.GetAxisAngleFromQuaternion`, and orientation integration are all improved. The improvement in orientation integration in particular helps avoid contact drift and helps integration with angular momentum conservation. - 8. Constraints in the fallback batch now have sequentialized execution, rather than using a jacobi solver. This improves simulation quality in pathological cases where a single dynamic body is associated with tons of constraints, but that situation is still best avoided. Anything that ends up in the fallback batch will cost more by virtue of being executed sequentially. More information and potential future improvements here: [Fallback batch improvements · Issue #162 · bepu/bepuphysics2 (github.com)](https://github.com/bepu/bepuphysics2/issues/162). - 9. Contact constraints now solve friction last. In simulations that don't let the solver reach an equilibrium solution, you might notice that an unstable stack of bodies exhibits more tangential jitter and less penetration jitter than it used to. In a simulation that allows enough time to solve the constraints, there should be no visible difference. (This change was motivated by the fact that penetration has a corrective feedback loop via depth, while friction is open ended. Giving friction the final word slightly reduces drift.) - 10. `VolumeConstraint` had a warmstarting jacobian bug; it's fixed. Should be higher quality (and stiffer, if configured to be stiff). - 11. `Hinge` and `SwivelHinge` never made use of accumulated impulses, oops. Should be higher quality (and stiffer, if configured to be stiff). - 12. `SwivelHinge` no longer has a unguarded NaNsplode codepath. - 13. Fixed a bug in `AngularMotor` that used the wrong inertia. - 14. Guarded against a sphere-cylinder division by zero. - 15. Fixed a capsule-cylinder determinism bug. - 16. Fixed a triangle-cylinder determinism bug. - 17. Fixed capsule-cylinder contact generation bug. - 18. Fixed cylinder-cylinder contact generation bug. - 19. Box-box now has a bundle early out. - 20. All triangle-involving collision pairs now consistent in triangle degeneracy testing. - 21. All triangle-involving collision pairs now handle collisions with normals pointing nearly perpendicular to the triangle plane much more gracefully. - 22. Fixed a bunch of other tiny weird triangle collision bugs too. - 23. `MeshReduction` revamped a bit. It should catch more boundary bumps, and it no longer tries to do a quadratically catastrophic operation when the number of triangles being considered is large. At a certain (extreme) point, it now simply doesn't bother with boundary smoothing at all. +24. `BufferPool` now allocates blocks from native memory pools rather than the managed heap. From ae20d2209c486dca145e3c19893886559b13809f Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 19 Feb 2022 16:06:58 -0600 Subject: [PATCH 460/947] Another missing change. --- Documentation/changelog.md | 1 + 1 file changed, 1 insertion(+) diff --git a/Documentation/changelog.md b/Documentation/changelog.md index 675b8f4e6..0f82662d5 100644 --- a/Documentation/changelog.md +++ b/Documentation/changelog.md @@ -21,6 +21,7 @@ At a high level, 2.4 was a solver revamp. Data layout and access patterns were s 15. All body state used by the solver now bundled together into `BodySet.SolverStates`. Includes pose, velocity, and inertia. 16. Constraint type batches no longer have a 'projection' buffer; anything loaded from it is now recalculated on the fly. 17. `RawBuffer` removed. Usages replaced with `Buffer`. +18. `BodyProperty` replaced with the more general `CollidableProperty`. ## Other Changes From 94b9c5b0607f4155ab5e2f47e00c9ddf27253953 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 19 Feb 2022 16:11:53 -0600 Subject: [PATCH 461/947] wait that happened earlier oops --- Documentation/changelog.md | 1 - 1 file changed, 1 deletion(-) diff --git a/Documentation/changelog.md b/Documentation/changelog.md index 0f82662d5..675b8f4e6 100644 --- a/Documentation/changelog.md +++ b/Documentation/changelog.md @@ -21,7 +21,6 @@ At a high level, 2.4 was a solver revamp. Data layout and access patterns were s 15. All body state used by the solver now bundled together into `BodySet.SolverStates`. Includes pose, velocity, and inertia. 16. Constraint type batches no longer have a 'projection' buffer; anything loaded from it is now recalculated on the fly. 17. `RawBuffer` removed. Usages replaced with `Buffer`. -18. `BodyProperty` replaced with the more general `CollidableProperty`. ## Other Changes From d48bf6f01ce0634c32e933a763ba792c9009087c Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 23 Feb 2022 17:13:59 -0600 Subject: [PATCH 462/947] Fixed/improved some depth refiner comments. --- .../CollisionDetection/DepthRefiner.cs | 25 +++++++++++++------ .../CollisionDetection/DepthRefiner.tt | 19 +++++++++++--- 2 files changed, 32 insertions(+), 12 deletions(-) diff --git a/BepuPhysics/CollisionDetection/DepthRefiner.cs b/BepuPhysics/CollisionDetection/DepthRefiner.cs index a55da12b9..1b9100f89 100644 --- a/BepuPhysics/CollisionDetection/DepthRefiner.cs +++ b/BepuPhysics/CollisionDetection/DepthRefiner.cs @@ -14,6 +14,19 @@ namespace BepuPhysics.CollisionDetection { + /// + /// Incrementally refines a sample direction to approach a local minimum depth between two convex bodies. + /// + /// + /// The DepthRefiner implements a Tootbird search: an incremental algorithm that takes steps towards the Tootbird. + /// The Tootbird is the origin projected on the support plane of the best(lowest depth) support direction observed so far. + /// This uses a simplex that updates with rules similar to a simplified version of GJK.The Tootbird is definitionally not inside the minkowski sum. + /// Type of the first shape. + /// SIMD type of the first shape. + /// Type providing support sampling for the first shape. + /// Type of the second shape. + /// SIMD type of the second shape. + /// Type providing support sampling for the second shape. public static class DepthRefiner where TShapeA : IConvexShape where TShapeWideA : IShapeWide @@ -88,10 +101,8 @@ static void GetNextNormal(ref Simplex simplex, in Vector3Wide support, ref Ve out Vector3Wide nextNormal) { Unsafe.SkipInit(out nextNormal); - //In the penetrating case, the search target is the closest point to the origin on the so-far-best bounding plane. - //In the separated case, it's just the origin itself. - //Termination conditions are based on the distance to the search target. In the penetrating case, we try to approach zero distance. - //The separated case makes use of the fact that the bestDepth and distance to closest point only converge when the offset and best normal align. + //The search target is the closest point to the origin on the so-far-best bounding plane, also known as the tootbird. + //(You could use the origin itself once separation is found; it would be similar to regular GJK. This implementation doesn't, but you could.) Vector3Wide.Scale(bestNormal, Vector.Max(Vector.Zero, bestDepth), out var searchTarget); var terminationEpsilon = Vector.ConditionalSelect(Vector.LessThan(bestDepth, Vector.Zero), convergenceThreshold - bestDepth, convergenceThreshold); var terminationEpsilonSquared = terminationEpsilon * terminationEpsilon; @@ -449,10 +460,8 @@ static void GetNextNormal(ref SimplexWithWitness simplex, in Vector3Wide supp out Vector3Wide nextNormal) { Unsafe.SkipInit(out nextNormal); - //In the penetrating case, the search target is the closest point to the origin on the so-far-best bounding plane. - //In the separated case, it's just the origin itself. - //Termination conditions are based on the distance to the search target. In the penetrating case, we try to approach zero distance. - //The separated case makes use of the fact that the bestDepth and distance to closest point only converge when the offset and best normal align. + //The search target is the closest point to the origin on the so-far-best bounding plane, also known as the tootbird. + //(You could use the origin itself once separation is found; it would be similar to regular GJK. This implementation doesn't, but you could.) Vector3Wide.Scale(bestNormal, Vector.Max(Vector.Zero, bestDepth), out var searchTarget); var terminationEpsilon = Vector.ConditionalSelect(Vector.LessThan(bestDepth, Vector.Zero), convergenceThreshold - bestDepth, convergenceThreshold); var terminationEpsilonSquared = terminationEpsilon * terminationEpsilon; diff --git a/BepuPhysics/CollisionDetection/DepthRefiner.tt b/BepuPhysics/CollisionDetection/DepthRefiner.tt index a8d35e745..c09c73998 100644 --- a/BepuPhysics/CollisionDetection/DepthRefiner.tt +++ b/BepuPhysics/CollisionDetection/DepthRefiner.tt @@ -50,6 +50,19 @@ namespace BepuPhysics.CollisionDetection } <#}#> + /// + /// Incrementally refines a sample direction to approach a local minimum depth between two convex bodies. + /// + /// + /// The DepthRefiner implements a Tootbird search: an incremental algorithm that takes steps towards the Tootbird. + /// The Tootbird is the origin projected on the support plane of the best(lowest depth) support direction observed so far. + /// This uses a simplex that updates with rules similar to a simplified version of GJK.The Tootbird is definitionally not inside the minkowski sum. + /// Type of the first shape. + /// SIMD type of the first shape. + /// Type providing support sampling for the first shape. + /// Type of the second shape. + /// SIMD type of the second shape. + /// Type providing support sampling for the second shape. public static class DepthRefiner where TShapeA : IConvexShape where TShapeWideA : IShapeWide @@ -144,10 +157,8 @@ namespace BepuPhysics.CollisionDetection out Vector3Wide nextNormal<#if(debug) { Write(", out DepthRefinerStep step"); }#>) { Unsafe.SkipInit(out nextNormal); - //In the penetrating case, the search target is the closest point to the origin on the so-far-best bounding plane. - //In the separated case, it's just the origin itself. - //Termination conditions are based on the distance to the search target. In the penetrating case, we try to approach zero distance. - //The separated case makes use of the fact that the bestDepth and distance to closest point only converge when the offset and best normal align. + //The search target is the closest point to the origin on the so-far-best bounding plane, also known as the tootbird. + //(You could use the origin itself once separation is found; it would be similar to regular GJK. This implementation doesn't, but you could.) Vector3Wide.Scale(bestNormal, Vector.Max(Vector.Zero, bestDepth), out var searchTarget); var terminationEpsilon = Vector.ConditionalSelect(Vector.LessThan(bestDepth, Vector.Zero), convergenceThreshold - bestDepth, convergenceThreshold); var terminationEpsilonSquared = terminationEpsilon * terminationEpsilon; From fb4c1af48e2ebe5db64e354fb5f64df01edfeefc Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 23 Feb 2022 18:02:38 -0600 Subject: [PATCH 463/947] Updated DepthRefinerTestDemo for 2.4. --- Demos/SpecializedTests/DepthRefinerTestDemo.cs | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/Demos/SpecializedTests/DepthRefinerTestDemo.cs b/Demos/SpecializedTests/DepthRefinerTestDemo.cs index ccd399640..b4d88c45a 100644 --- a/Demos/SpecializedTests/DepthRefinerTestDemo.cs +++ b/Demos/SpecializedTests/DepthRefinerTestDemo.cs @@ -17,7 +17,6 @@ //using DemoRenderer.Constraints; //using DemoRenderer.UI; //using DemoUtilities; -//using Quaternion = BepuUtilities.Quaternion; //namespace Demos.SpecializedTests //{ @@ -33,7 +32,7 @@ // camera.Position = new Vector3(0, 0, 13f); // camera.Yaw = 0; // camera.Pitch = 0; -// Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0))); +// Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)), new SolveDescription(8, 1)); // //{ // // //var shapeA = new Cylinder(1f, 2f); // // //var poseA = new RigidPose(new Vector3(12, 0.5f, 12), Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), MathF.PI * 0.5f)); @@ -140,7 +139,7 @@ // var poseA = RigidPose.Identity; // Matrix3x3Wide.ReadSlot(ref localOrientationB, 0, out var orientationBNarrow); // RigidPose poseB; -// Quaternion.CreateFromRotationMatrix(orientationBNarrow, out poseB.Orientation); +// poseB.Orientation = QuaternionEx.CreateFromRotationMatrix(orientationBNarrow); // Vector3Wide.ReadSlot(ref localOffsetB, 0, out poseB.Position); // shapeLines = MinkowskiShapeVisualizer.CreateLines( // shapeA, shapeB, poseA, poseB, 65536, @@ -150,7 +149,7 @@ // var aWide = default(ConvexHullWide); // var memoryLength = Unsafe.SizeOf() * Vector.Count; // var memory = stackalloc byte[memoryLength]; -// aWide.Initialize(new RawBuffer(memory, memoryLength)); +// aWide.Initialize(new Buffer(memory, memoryLength)); // var bWide = default(TriangleWide); // aWide.Broadcast(shapeA); // bWide.Broadcast(shapeB); @@ -197,8 +196,8 @@ // out var depthWide, out var localNormalWide, out var witnessOnA, steps, 50); -// Simulation.Statics.Add(new StaticDescription(poseA.Position + new Vector3(50, 0, 0), poseA.Orientation, new CollidableDescription(Simulation.Shapes.Add(shapeA), 0.1f))); -// Simulation.Statics.Add(new StaticDescription(localOffsetBNarrow + new Vector3(50, 0, 0), Quaternion.Identity, new CollidableDescription(Simulation.Shapes.Add(shapeB), 0.1f))); +// Simulation.Statics.Add(new StaticDescription(poseA.Position + new Vector3(50, 0, 0), poseA.Orientation, Simulation.Shapes.Add(shapeA))); +// Simulation.Statics.Add(new StaticDescription(localOffsetBNarrow + new Vector3(50, 0, 0), Quaternion.Identity, Simulation.Shapes.Add(shapeB))); // } // //{ From 9506e70c6a3705cafefeb02d58c33cbffc4ad0f3 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 25 Feb 2022 16:08:00 -0600 Subject: [PATCH 464/947] Commenttypo. --- BepuPhysics/CollisionDetection/DepthRefiner.cs | 4 ++-- BepuPhysics/CollisionDetection/DepthRefiner.tt | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/BepuPhysics/CollisionDetection/DepthRefiner.cs b/BepuPhysics/CollisionDetection/DepthRefiner.cs index 1b9100f89..155a9312a 100644 --- a/BepuPhysics/CollisionDetection/DepthRefiner.cs +++ b/BepuPhysics/CollisionDetection/DepthRefiner.cs @@ -195,7 +195,7 @@ static void GetNextNormal(ref Simplex simplex, in Vector3Wide support, ref Ve var relevantFeatures = Vector.One; - //If this is a vertex case and the sample is right on top of the origin, immediately quit. + //If this is a vertex case and the sample is right on top of the target, immediately quit. Vector3Wide.LengthSquared(targetToA, out var targetToALengthSquared); terminatedLanes = Vector.BitwiseOr(terminatedLanes, Vector.BitwiseAnd(simplexIsAVertex, Vector.LessThan(targetToALengthSquared, terminationEpsilonSquared))); @@ -558,7 +558,7 @@ static void GetNextNormal(ref SimplexWithWitness simplex, in Vector3Wide supp simplex.C.Weight = Vector.ConditionalSelect(terminatedLanes, simplex.C.Weight, Vector.Zero); simplex.WeightDenominator = Vector.ConditionalSelect(terminatedLanes, simplex.WeightDenominator, Vector.One); - //If this is a vertex case and the sample is right on top of the origin, immediately quit. + //If this is a vertex case and the sample is right on top of the target, immediately quit. Vector3Wide.LengthSquared(targetToA, out var targetToALengthSquared); terminatedLanes = Vector.BitwiseOr(terminatedLanes, Vector.BitwiseAnd(simplexIsAVertex, Vector.LessThan(targetToALengthSquared, terminationEpsilonSquared))); diff --git a/BepuPhysics/CollisionDetection/DepthRefiner.tt b/BepuPhysics/CollisionDetection/DepthRefiner.tt index c09c73998..73ebba724 100644 --- a/BepuPhysics/CollisionDetection/DepthRefiner.tt +++ b/BepuPhysics/CollisionDetection/DepthRefiner.tt @@ -278,7 +278,7 @@ namespace BepuPhysics.CollisionDetection simplex.WeightDenominator = Vector.ConditionalSelect(terminatedLanes, simplex.WeightDenominator, Vector.One); <#}#> - //If this is a vertex case and the sample is right on top of the origin, immediately quit. + //If this is a vertex case and the sample is right on top of the target, immediately quit. Vector3Wide.LengthSquared(targetToA, out var targetToALengthSquared); terminatedLanes = Vector.BitwiseOr(terminatedLanes, Vector.BitwiseAnd(simplexIsAVertex, Vector.LessThan(targetToALengthSquared, terminationEpsilonSquared))); From 995e3b80de7721f2a7c88a5c2e7945e9105a9bc2 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 10 Mar 2022 21:43:05 -0600 Subject: [PATCH 465/947] Updated other parts of the depth refiner test demo for 2.4. Still requires setting the DepthRefiner.tt debug flag to true and uncommenting the demo, though. --- .../SpecializedTests/DepthRefinerTestDemo.cs | 272 +++++++++--------- 1 file changed, 138 insertions(+), 134 deletions(-) diff --git a/Demos/SpecializedTests/DepthRefinerTestDemo.cs b/Demos/SpecializedTests/DepthRefinerTestDemo.cs index b4d88c45a..d18f58300 100644 --- a/Demos/SpecializedTests/DepthRefinerTestDemo.cs +++ b/Demos/SpecializedTests/DepthRefinerTestDemo.cs @@ -33,123 +33,54 @@ // camera.Yaw = 0; // camera.Pitch = 0; // Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)), new SolveDescription(8, 1)); -// //{ -// // //var shapeA = new Cylinder(1f, 2f); -// // //var poseA = new RigidPose(new Vector3(12, 0.5f, 12), Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), MathF.PI * 0.5f)); -// // //var shapeB = new Triangle(new Vector3(-2f, 0, -2f), new Vector3(2f, 0, -2f), new Vector3(-2f, 0, 2f)); -// // //var poseB = new RigidPose(new Vector3(12, 0, 12)); -// // var shapeA = new Cylinder(0.4f, 0.09f); -// // //var poseA = new RigidPose(new Vector3(12, 0.5f, 12), Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), MathF.PI * 0.5f)); -// // var shapeB = new Triangle(new Vector3(-0.104847f, -2.863911f, -0.8221359f), new Vector3(-0.7841263f, 1.040714f, -1.362942f), new Vector3(0.8889847f, 1.823196f, 2.185074f)); -// // //var poseB = new RigidPose(new Vector3(12, 0, 12)); -// // Matrix3x3Wide localOrientationB; -// // Vector3Wide.Broadcast(new Vector3(-0.4182778f, -0.1956204f, -0.887004f), out localOrientationB.X); -// // Vector3Wide.Broadcast(new Vector3(-0.8923031f, -0.09407896f, 0.4415249f), out localOrientationB.Y); -// // Vector3Wide.Broadcast(new Vector3(-0.1698197f, 0.9761565f, -0.1352016f), out localOrientationB.Z); -// // Vector3Wide.Broadcast(new Vector3(0.3561249f, 2.797102f, 0.4073029f), out var localOffsetB); - -// // Vector3Wide.Normalize(localOffsetB, out var initialNormal); -// // //Vector3Wide.Broadcast(new Vector3(0.9673051f, 0.07194486f, -0.2431969f), out var initialNormal); -// // //var initialDepth = new Vector(0.007193089f); - -// // var convergenceThreshold = new Vector(4e-7f); - -// // var minimumDepthThreshold = new Vector(-0.1f); - -// // //var simplex = new DepthRefiner.SimplexWithWitness(); -// // //Vector3Wide.Broadcast(new Vector3(-0.05699956f, -0.4314917f, 0.445577f), out simplex.A.Support); -// // //Vector3Wide.Broadcast(new Vector3(-0.3718189f, -0.09f, 0.1474812f), out simplex.A.SupportOnA); -// // //simplex.A.Exists = new Vector(-1); - -// // //Vector3Wide.Broadcast(new Vector3(0.3148193f, -0.4314917f, 0.2980957f), out simplex.B.Support); -// // //Vector3Wide.Broadcast(new Vector3(0f, -0.09f, 0f), out simplex.B.SupportOnA); -// // //simplex.B.Exists = new Vector(-1); - -// // //Vector3Wide.Broadcast(new Vector3(-1.175686f, 3.316494f, 0.7135266f), out simplex.C.Support); -// // //Vector3Wide.Broadcast(new Vector3(-0.2811551f, -0.09f, -0.2845203f), out simplex.C.SupportOnA); -// // //simplex.C.Exists = new Vector(-1); - -// // basePosition = default; -// // var poseA = RigidPose.Identity; -// // Matrix3x3Wide.ReadSlot(ref localOrientationB, 0, out var orientationBNarrow); -// // RigidPose poseB; -// // Quaternion.CreateFromRotationMatrix(orientationBNarrow, out poseB.Orientation); -// // Vector3Wide.ReadSlot(ref localOffsetB, 0, out poseB.Position); -// // shapeLines = MinkowskiShapeVisualizer.CreateLines( -// // shapeA, shapeB, poseA, poseB, 65536, -// // 0.01f, new Vector3(0.4f, 0.4f, 0), -// // 0.1f, new Vector3(0, 1, 0), default, basePosition, BufferPool); - -// // var aWide = default(CylinderWide); -// // var bWide = default(TriangleWide); -// // aWide.Broadcast(shapeA); -// // bWide.Broadcast(shapeB); -// // //var worldOffsetB = poseB.Position - poseA.Position; -// // //var localOrientationB = Matrix3x3.CreateFromQuaternion(Quaternion.Concatenate(poseB.Orientation, Quaternion.Conjugate(poseA.Orientation))); -// // //var localOffsetB = Quaternion.Transform(worldOffsetB, Quaternion.Conjugate(poseA.Orientation)); -// // //Vector3Wide.Broadcast(localOffsetB, out var localOffsetBWide); -// // //Matrix3x3Wide.Broadcast(localOrientationB, out var localOrientationBWide); -// // var triangleSupportFinder = default(PretransformedTriangleSupportFinder); -// // var cylinderSupportFinder = default(CylinderSupportFinder); - -// // //var initialNormal = Vector3.Normalize(localOffsetB); -// // //Vector3Wide.Broadcast(initialNormal, out var initialNormalWide); -// // steps = new List(); -// // //DepthRefiner.FindMinimumDepth( -// // // aWide, bWide, localOffsetB, localOrientationB, ref cylinderSupportFinder, ref triangleSupportFinder, ref simplex, initialNormal, initialDepth, new Vector(), convergenceThreshold, minimumDepthThreshold, -// // // out var depthWide, out var localNormalWide, out var witnessOnA, steps, 50); -// // DepthRefiner.FindMinimumDepth( -// // aWide, bWide, localOffsetB, localOrientationB, ref cylinderSupportFinder, ref triangleSupportFinder, initialNormal, new Vector(), convergenceThreshold, minimumDepthThreshold, -// // out var depthWide, out var localNormalWide, out var witnessOnA, steps, 50); - -// //} - // { -// const int pointCount = 16; -// var randomizedPoints = new QuickList(pointCount * 2, BufferPool); -// var random = new Random(5); -// for (int i = 0; i < pointCount; ++i) -// { -// randomizedPoints.AllocateUnsafely() = new Vector3(33 * random.NextSingle(), 1 * random.NextSingle(), 13 * random.NextSingle()); -// } -// var shapeA = new ConvexHull(randomizedPoints.Span.Slice(randomizedPoints.Count), BufferPool, out var hullCenter); +// //var shapeA = new Cylinder(1f, 2f); // //var poseA = new RigidPose(new Vector3(12, 0.5f, 12), Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), MathF.PI * 0.5f)); -// var shapeB = new Triangle( -// new Vector3(7.499722f, -8.201822f, -23.06599f), -// new Vector3(1.648121f, -28.36107f, 31.33404f), -// new Vector3(-9.147846f, 36.56289f, -8.268051f)); - +// //var shapeB = new Triangle(new Vector3(-2f, 0, -2f), new Vector3(2f, 0, -2f), new Vector3(-2f, 0, 2f)); +// //var poseB = new RigidPose(new Vector3(12, 0, 12)); +// var shapeA = new Cylinder(0.4f, 0.09f); +// //var poseA = new RigidPose(new Vector3(12, 0.5f, 12), Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), MathF.PI * 0.5f)); +// var shapeB = new Triangle(new Vector3(-0.104847f, -2.863911f, -0.8221359f), new Vector3(-0.7841263f, 1.040714f, -1.362942f), new Vector3(0.8889847f, 1.823196f, 2.185074f)); // //var poseB = new RigidPose(new Vector3(12, 0, 12)); // Matrix3x3Wide localOrientationB; -// Vector3Wide.Broadcast(new Vector3(0.3963324f, -0.1519237f, 0.90545f), out localOrientationB.X); -// Vector3Wide.Broadcast(new Vector3(-0.3329513f, 0.8952942f, 0.2959588f), out localOrientationB.Y); -// Vector3Wide.Broadcast(new Vector3(-0.8556075f, -0.4187689f, 0.304251f), out localOrientationB.Z); -// var localOffsetBNarrow = new Vector3(7.41188f, -34.41978f, 7.493548f); -// Vector3Wide.Broadcast(localOffsetBNarrow, out var localOffsetB); +// Vector3Wide.Broadcast(new Vector3(-0.4182778f, -0.1956204f, -0.887004f), out localOrientationB.X); +// Vector3Wide.Broadcast(new Vector3(-0.8923031f, -0.09407896f, 0.4415249f), out localOrientationB.Y); +// Vector3Wide.Broadcast(new Vector3(-0.1698197f, 0.9761565f, -0.1352016f), out localOrientationB.Z); +// Vector3Wide.Broadcast(new Vector3(0.3561249f, 2.797102f, 0.4073029f), out var localOffsetB); + +// Vector3Wide.Normalize(localOffsetB, out var initialNormal); +// //Vector3Wide.Broadcast(new Vector3(0.9673051f, 0.07194486f, -0.2431969f), out var initialNormal); +// //var initialDepth = new Vector(0.007193089f); -// //Vector3Wide.Normalize(localOffsetB, out var initialNormal); -// Vector3Wide.Broadcast(new Vector3(1f, 0, 0), out var initialNormal); -// var initialDepth = new Vector(2.674457f); +// var convergenceThreshold = new Vector(4e-7f); + +// var minimumDepthThreshold = new Vector(-0.1f); + +// //var simplex = new DepthRefiner.SimplexWithWitness(); +// //Vector3Wide.Broadcast(new Vector3(-0.05699956f, -0.4314917f, 0.445577f), out simplex.A.Support); +// //Vector3Wide.Broadcast(new Vector3(-0.3718189f, -0.09f, 0.1474812f), out simplex.A.SupportOnA); +// //simplex.A.Exists = new Vector(-1); -// var convergenceThreshold = new Vector(1e-5f * 4.187582f); +// //Vector3Wide.Broadcast(new Vector3(0.3148193f, -0.4314917f, 0.2980957f), out simplex.B.Support); +// //Vector3Wide.Broadcast(new Vector3(0f, -0.09f, 0f), out simplex.B.SupportOnA); +// //simplex.B.Exists = new Vector(-1); -// var minimumDepthThreshold = new Vector(-1f); +// //Vector3Wide.Broadcast(new Vector3(-1.175686f, 3.316494f, 0.7135266f), out simplex.C.Support); +// //Vector3Wide.Broadcast(new Vector3(-0.2811551f, -0.09f, -0.2845203f), out simplex.C.SupportOnA); +// //simplex.C.Exists = new Vector(-1); // basePosition = default; // var poseA = RigidPose.Identity; // Matrix3x3Wide.ReadSlot(ref localOrientationB, 0, out var orientationBNarrow); // RigidPose poseB; -// poseB.Orientation = QuaternionEx.CreateFromRotationMatrix(orientationBNarrow); +// QuaternionEx.CreateFromRotationMatrix(orientationBNarrow, out poseB.Orientation); // Vector3Wide.ReadSlot(ref localOffsetB, 0, out poseB.Position); -// shapeLines = MinkowskiShapeVisualizer.CreateLines( +// shapeLines = MinkowskiShapeVisualizer.CreateLines( // shapeA, shapeB, poseA, poseB, 65536, // 0.01f, new Vector3(0.4f, 0.4f, 0), // 0.1f, new Vector3(0, 1, 0), default, basePosition, BufferPool); -// var aWide = default(ConvexHullWide); -// var memoryLength = Unsafe.SizeOf() * Vector.Count; -// var memory = stackalloc byte[memoryLength]; -// aWide.Initialize(new Buffer(memory, memoryLength)); +// var aWide = default(CylinderWide); // var bWide = default(TriangleWide); // aWide.Broadcast(shapeA); // bWide.Broadcast(shapeB); @@ -159,23 +90,7 @@ // //Vector3Wide.Broadcast(localOffsetB, out var localOffsetBWide); // //Matrix3x3Wide.Broadcast(localOrientationB, out var localOrientationBWide); // var triangleSupportFinder = default(PretransformedTriangleSupportFinder); -// var convexHullSupportFinder = default(ConvexHullSupportFinder); - - -// DepthRefiner.SimplexWithWitness simplex = default; -// Vector3Wide.Broadcast(new Vector3(19.4402f, -1.873452f, 1.636454f), out simplex.A.Support); -// Vector3Wide.Broadcast(new Vector3(17.70424f, 0.2696521f, 0.8619514f), out simplex.A.SupportOnA); -// simplex.A.Exists = new Vector(-1); -// Vector3Wide.Broadcast(new Vector3(-20.94802f, 34.62053f, -9.692917f), out simplex.B.Support); -// Vector3Wide.Broadcast(new Vector3(-13.53614f, 0.2007419f, -2.199368f), out simplex.B.SupportOnA); -// simplex.B.Exists = new Vector(-1); -// Vector3Wide.Broadcast(new Vector3(-28.44774f, 42.82235f, 13.37307f), out simplex.C.Support); -// Vector3Wide.Broadcast(new Vector3(-13.53614f, 0.2007419f, -2.199368f), out simplex.C.SupportOnA); -// simplex.C.Exists = new Vector(-1); - -// //Vector3Wide.Broadcast(new Vector3(-28.44774f, 42.82235f, 13.37307f), out simplex.A.Support); -// //Vector3Wide.Broadcast(new Vector3(-13.53614f, 0.2007419f, -2.199368f), out simplex.A.SupportOnA); -// //simplex.A.Exists = new Vector(-1); +// var cylinderSupportFinder = default(CylinderSupportFinder); // //var initialNormal = Vector3.Normalize(localOffsetB); // //Vector3Wide.Broadcast(initialNormal, out var initialNormalWide); @@ -183,23 +98,112 @@ // //DepthRefiner.FindMinimumDepth( // // aWide, bWide, localOffsetB, localOrientationB, ref cylinderSupportFinder, ref triangleSupportFinder, ref simplex, initialNormal, initialDepth, new Vector(), convergenceThreshold, minimumDepthThreshold, // // out var depthWide, out var localNormalWide, out var witnessOnA, steps, 50); -// var inactiveLanes = new Vector(-1); -// Unsafe.As, int>(ref inactiveLanes) = 0; -// //DepthRefiner.FindMinimumDepth( -// // aWide, bWide, localOffsetB, localOrientationB, ref convexHullSupportFinder, ref triangleSupportFinder, ref simplex, initialNormal, initialDepth, inactiveLanes, convergenceThreshold, minimumDepthThreshold, -// // out var depthWide, out var localNormalWide, out var witnessOnA, steps, 50); - - -// //Vector3Wide.Broadcast(new Vector3(0, -1, 0), out initialNormal); -// DepthRefiner.FindMinimumDepth( -// aWide, bWide, localOffsetB, localOrientationB, ref convexHullSupportFinder, ref triangleSupportFinder, initialNormal, inactiveLanes, convergenceThreshold, minimumDepthThreshold, +// DepthRefiner.FindMinimumDepth( +// aWide, bWide, localOffsetB, localOrientationB, ref cylinderSupportFinder, ref triangleSupportFinder, initialNormal, new Vector(), convergenceThreshold, minimumDepthThreshold, // out var depthWide, out var localNormalWide, out var witnessOnA, steps, 50); - // Simulation.Statics.Add(new StaticDescription(poseA.Position + new Vector3(50, 0, 0), poseA.Orientation, Simulation.Shapes.Add(shapeA))); +// Vector3Wide.ReadFirst(localOffsetB, out var localOffsetBNarrow); // Simulation.Statics.Add(new StaticDescription(localOffsetBNarrow + new Vector3(50, 0, 0), Quaternion.Identity, Simulation.Shapes.Add(shapeB))); + // } +// //{ +// // const int pointCount = 16; +// // var randomizedPoints = new QuickList(pointCount * 2, BufferPool); +// // var random = new Random(5); +// // for (int i = 0; i < pointCount; ++i) +// // { +// // randomizedPoints.AllocateUnsafely() = new Vector3(33 * random.NextSingle(), 1 * random.NextSingle(), 13 * random.NextSingle()); +// // } +// // var shapeA = new ConvexHull(randomizedPoints.Span.Slice(randomizedPoints.Count), BufferPool, out var hullCenter); +// // //var poseA = new RigidPose(new Vector3(12, 0.5f, 12), Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), MathF.PI * 0.5f)); +// // var shapeB = new Triangle( +// // new Vector3(7.499722f, -8.201822f, -23.06599f), +// // new Vector3(1.648121f, -28.36107f, 31.33404f), +// // new Vector3(-9.147846f, 36.56289f, -8.268051f)); + +// // //var poseB = new RigidPose(new Vector3(12, 0, 12)); +// // Matrix3x3Wide localOrientationB; +// // Vector3Wide.Broadcast(new Vector3(0.3963324f, -0.1519237f, 0.90545f), out localOrientationB.X); +// // Vector3Wide.Broadcast(new Vector3(-0.3329513f, 0.8952942f, 0.2959588f), out localOrientationB.Y); +// // Vector3Wide.Broadcast(new Vector3(-0.8556075f, -0.4187689f, 0.304251f), out localOrientationB.Z); +// // var localOffsetBNarrow = new Vector3(7.41188f, -34.41978f, 7.493548f); +// // Vector3Wide.Broadcast(localOffsetBNarrow, out var localOffsetB); + +// // //Vector3Wide.Normalize(localOffsetB, out var initialNormal); +// // Vector3Wide.Broadcast(new Vector3(1f, 0, 0), out var initialNormal); +// // var initialDepth = new Vector(2.674457f); + +// // var convergenceThreshold = new Vector(1e-5f * 4.187582f); + +// // var minimumDepthThreshold = new Vector(-1f); + +// // basePosition = default; +// // var poseA = RigidPose.Identity; +// // Matrix3x3Wide.ReadSlot(ref localOrientationB, 0, out var orientationBNarrow); +// // RigidPose poseB; +// // poseB.Orientation = QuaternionEx.CreateFromRotationMatrix(orientationBNarrow); +// // Vector3Wide.ReadSlot(ref localOffsetB, 0, out poseB.Position); +// // shapeLines = MinkowskiShapeVisualizer.CreateLines( +// // shapeA, shapeB, poseA, poseB, 65536, +// // 0.01f, new Vector3(0.4f, 0.4f, 0), +// // 0.1f, new Vector3(0, 1, 0), default, basePosition, BufferPool); + +// // var aWide = default(ConvexHullWide); +// // var memoryLength = Unsafe.SizeOf() * Vector.Count; +// // var memory = stackalloc byte[memoryLength]; +// // aWide.Initialize(new Buffer(memory, memoryLength)); +// // var bWide = default(TriangleWide); +// // aWide.Broadcast(shapeA); +// // bWide.Broadcast(shapeB); +// // //var worldOffsetB = poseB.Position - poseA.Position; +// // //var localOrientationB = Matrix3x3.CreateFromQuaternion(Quaternion.Concatenate(poseB.Orientation, Quaternion.Conjugate(poseA.Orientation))); +// // //var localOffsetB = Quaternion.Transform(worldOffsetB, Quaternion.Conjugate(poseA.Orientation)); +// // //Vector3Wide.Broadcast(localOffsetB, out var localOffsetBWide); +// // //Matrix3x3Wide.Broadcast(localOrientationB, out var localOrientationBWide); +// // var triangleSupportFinder = default(PretransformedTriangleSupportFinder); +// // var convexHullSupportFinder = default(ConvexHullSupportFinder); + + +// // DepthRefiner.SimplexWithWitness simplex = default; +// // Vector3Wide.Broadcast(new Vector3(19.4402f, -1.873452f, 1.636454f), out simplex.A.Support); +// // Vector3Wide.Broadcast(new Vector3(17.70424f, 0.2696521f, 0.8619514f), out simplex.A.SupportOnA); +// // simplex.A.Exists = new Vector(-1); +// // Vector3Wide.Broadcast(new Vector3(-20.94802f, 34.62053f, -9.692917f), out simplex.B.Support); +// // Vector3Wide.Broadcast(new Vector3(-13.53614f, 0.2007419f, -2.199368f), out simplex.B.SupportOnA); +// // simplex.B.Exists = new Vector(-1); +// // Vector3Wide.Broadcast(new Vector3(-28.44774f, 42.82235f, 13.37307f), out simplex.C.Support); +// // Vector3Wide.Broadcast(new Vector3(-13.53614f, 0.2007419f, -2.199368f), out simplex.C.SupportOnA); +// // simplex.C.Exists = new Vector(-1); + +// // //Vector3Wide.Broadcast(new Vector3(-28.44774f, 42.82235f, 13.37307f), out simplex.A.Support); +// // //Vector3Wide.Broadcast(new Vector3(-13.53614f, 0.2007419f, -2.199368f), out simplex.A.SupportOnA); +// // //simplex.A.Exists = new Vector(-1); + +// // //var initialNormal = Vector3.Normalize(localOffsetB); +// // //Vector3Wide.Broadcast(initialNormal, out var initialNormalWide); +// // steps = new List(); +// // //DepthRefiner.FindMinimumDepth( +// // // aWide, bWide, localOffsetB, localOrientationB, ref cylinderSupportFinder, ref triangleSupportFinder, ref simplex, initialNormal, initialDepth, new Vector(), convergenceThreshold, minimumDepthThreshold, +// // // out var depthWide, out var localNormalWide, out var witnessOnA, steps, 50); +// // var inactiveLanes = new Vector(-1); +// // Unsafe.As, int>(ref inactiveLanes) = 0; +// // //DepthRefiner.FindMinimumDepth( +// // // aWide, bWide, localOffsetB, localOrientationB, ref convexHullSupportFinder, ref triangleSupportFinder, ref simplex, initialNormal, initialDepth, inactiveLanes, convergenceThreshold, minimumDepthThreshold, +// // // out var depthWide, out var localNormalWide, out var witnessOnA, steps, 50); + + +// // //Vector3Wide.Broadcast(new Vector3(0, -1, 0), out initialNormal); +// // DepthRefiner.FindMinimumDepth( +// // aWide, bWide, localOffsetB, localOrientationB, ref convexHullSupportFinder, ref triangleSupportFinder, initialNormal, inactiveLanes, convergenceThreshold, minimumDepthThreshold, +// // out var depthWide, out var localNormalWide, out var witnessOnA, steps, 50); + + +// // Simulation.Statics.Add(new StaticDescription(poseA.Position + new Vector3(50, 0, 0), poseA.Orientation, Simulation.Shapes.Add(shapeA))); +// // Simulation.Statics.Add(new StaticDescription(localOffsetBNarrow + new Vector3(50, 0, 0), Quaternion.Identity, Simulation.Shapes.Add(shapeB))); +// //} + // //{ // // var shapeA = new Capsule(0.5f, 1f); // // var poseA = new RigidPose(new Vector3(0, 0, 0), Quaternion.Identity); @@ -230,12 +234,12 @@ // // var aWide = default(CapsuleWide); // // var bWide = default(ConvexHullWide); // // aWide.Broadcast(shapeA); -// // BufferPool.Take(bWide.InternalAllocationSize, out var memory); +// // BufferPool.Take(bWide.InternalAllocationSize, out var memory); // // bWide.Initialize(memory.Slice(0, bWide.InternalAllocationSize)); // // bWide.Broadcast(shapeB); // // var worldOffsetB = poseB.Position - poseA.Position; // // var localOrientationB = Matrix3x3.CreateFromQuaternion(Quaternion.Concatenate(poseB.Orientation, Quaternion.Conjugate(poseA.Orientation))); -// // var localOffsetB = Quaternion.Transform(worldOffsetB, Quaternion.Conjugate(poseA.Orientation)); +// // var localOffsetB = QuaternionEx.Transform(worldOffsetB, Quaternion.Conjugate(poseA.Orientation)); // // Vector3Wide.Broadcast(localOffsetB, out var localOffsetBWide); // // Matrix3x3Wide.Broadcast(localOrientationB, out var localOrientationBWide); // // var supportFinderA = default(CapsuleSupportFinder); @@ -251,7 +255,7 @@ // // steps.Clear(); // // var worldOffsetA = poseA.Position - poseB.Position; // // var localOrientationA = Matrix3x3.CreateFromQuaternion(Quaternion.Concatenate(poseA.Orientation, Quaternion.Conjugate(poseB.Orientation))); -// // var localOffsetA = Quaternion.Transform(worldOffsetA, Quaternion.Conjugate(poseB.Orientation)); +// // var localOffsetA = QuaternionEx.Transform(worldOffsetA, Quaternion.Conjugate(poseB.Orientation)); // // Vector3Wide.Broadcast(localOffsetA, out var localOffsetAWide); // // Matrix3x3Wide.Broadcast(localOrientationA, out var localOrientationAWide); // // Vector3Wide.Broadcast(Vector3.Normalize(localOffsetA), out var initialNormalWide2); @@ -293,7 +297,7 @@ // // // Z = new Vector3(-0.1333926f, -0.9782246f, -0.1590062f) // // //}; // // //var poseB = new RigidPose(new Vector3(-0.2570486f, 1.780561f, -1.033215f), Quaternion.CreateFromAxisAngle(Vector3.Normalize(new Vector3(1, 1, 1)), MathF.PI * 0.35f)); -// // var poseB = new RigidPose(positionB, Quaternion.CreateFromRotationMatrix(localOrientationBMatrix)); +// // var poseB = new RigidPose(positionB, QuaternionEx.CreateFromRotationMatrix(localOrientationBMatrix)); // // basePosition = default; // // shapeLines = MinkowskiShapeVisualizer.CreateLines( @@ -307,7 +311,7 @@ // // bWide.Broadcast(shapeB); // // var worldOffsetB = poseB.Position - poseA.Position; // // var localOrientationB = Matrix3x3.CreateFromQuaternion(Quaternion.Concatenate(poseB.Orientation, Quaternion.Conjugate(poseA.Orientation))); -// // var localOffsetB = Quaternion.Transform(worldOffsetB, Quaternion.Conjugate(poseA.Orientation)); +// // var localOffsetB = QuaternionEx.Transform(worldOffsetB, Quaternion.Conjugate(poseA.Orientation)); // // Vector3Wide.Broadcast(localOffsetB, out var localOffsetBWide); // // Matrix3x3Wide.Broadcast(localOrientationB, out var localOrientationBWide); // // var supportFinder = default(CylinderSupportFinder); @@ -357,7 +361,7 @@ // // bWide.Broadcast(shapeB); // // var worldOffsetB = poseB.Position - poseA.Position; // // var localOrientationB = Matrix3x3.CreateFromQuaternion(Quaternion.Concatenate(poseB.Orientation, Quaternion.Conjugate(poseA.Orientation))); -// // var localOffsetB = Quaternion.Transform(worldOffsetB, Quaternion.Conjugate(poseA.Orientation)); +// // var localOffsetB = QuaternionEx.Transform(worldOffsetB, Quaternion.Conjugate(poseA.Orientation)); // // Vector3Wide.Broadcast(localOffsetB, out var localOffsetBWide); // // Matrix3x3Wide.Broadcast(localOrientationB, out var localOrientationBWide); // // var supportFinder = default(BoxSupportFinder); From 8a57038a7a3b5fe91d2ce2940a2ba7825270e046 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 11 Mar 2022 17:44:20 -0600 Subject: [PATCH 466/947] Shape pile dodecahedra. --- Demos/SpecializedTests/ShapePileTestDemo.cs | 49 +++++++++++++++++---- 1 file changed, 40 insertions(+), 9 deletions(-) diff --git a/Demos/SpecializedTests/ShapePileTestDemo.cs b/Demos/SpecializedTests/ShapePileTestDemo.cs index 6f6ef1793..7734361ee 100644 --- a/Demos/SpecializedTests/ShapePileTestDemo.cs +++ b/Demos/SpecializedTests/ShapePileTestDemo.cs @@ -21,15 +21,15 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //camera.Yaw = MathHelper.Pi ; camera.Yaw = MathHelper.Pi * 3f / 4; //camera.Pitch = MathHelper.PiOver2 * 0.999f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(4, 1)); Simulation.Deterministic = true; var sphere = new Sphere(1.5f); var capsule = new Capsule(1f, 1f); var box = new Box(1f, 3f, 2f); var cylinder = new Cylinder(1.5f, 0.3f); - const int pointCount = 32; - var points = new QuickList(pointCount, BufferPool); + var points = new QuickList(32, BufferPool); + //Boxlike point cloud. //points.Allocate(BufferPool) = new Vector3(0, 0, 0); //points.Allocate(BufferPool) = new Vector3(0, 0, 1); //points.Allocate(BufferPool) = new Vector3(0, 1, 0); @@ -38,12 +38,42 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //points.Allocate(BufferPool) = new Vector3(1, 0, 1); //points.Allocate(BufferPool) = new Vector3(1, 1, 0); //points.Allocate(BufferPool) = new Vector3(1, 1, 1); - var random = new Random(5); - for (int i = 0; i < pointCount; ++i) - { - points.AllocateUnsafely() = new Vector3(3 * random.NextSingle(), 1 * random.NextSingle(), 3 * random.NextSingle()); - //points.AllocateUnsafely() = new Vector3(0, 1, 0) + Vector3.Normalize(new Vector3(random.NextSingle() * 2 - 1, random.NextSingle() * 2 - 1, random.NextSingle() * 2 - 1)) * random.NextSingle(); - } + + //Rando pointcloud. + //var random = new Random(5); + //for (int i = 0; i < 32; ++i) + //{ + // points.Allocate(BufferPool) = new Vector3(3 * random.NextSingle(), 1 * random.NextSingle(), 3 * random.NextSingle()); + //} + + //Dodecahedron pointcloud. + points.Allocate(BufferPool) = new Vector3(-1, -1, -1); + points.Allocate(BufferPool) = new Vector3(-1, -1, 1); + points.Allocate(BufferPool) = new Vector3(-1, 1, -1); + points.Allocate(BufferPool) = new Vector3(-1, 1, 1); + points.Allocate(BufferPool) = new Vector3(1, -1, -1); + points.Allocate(BufferPool) = new Vector3(1, -1, 1); + points.Allocate(BufferPool) = new Vector3(1, 1, -1); + points.Allocate(BufferPool) = new Vector3(1, 1, 1); + + const float goldenRatio = 1.618033988749f; + const float oogr = 1f / goldenRatio; + + points.Allocate(BufferPool) = new Vector3(0, goldenRatio, oogr); + points.Allocate(BufferPool) = new Vector3(0, -goldenRatio, oogr); + points.Allocate(BufferPool) = new Vector3(0, goldenRatio, -oogr); + points.Allocate(BufferPool) = new Vector3(0, -goldenRatio, -oogr); + + points.Allocate(BufferPool) = new Vector3(oogr, 0, goldenRatio); + points.Allocate(BufferPool) = new Vector3(oogr, 0, -goldenRatio); + points.Allocate(BufferPool) = new Vector3(-oogr, 0, goldenRatio); + points.Allocate(BufferPool) = new Vector3(-oogr, 0, -goldenRatio); + + points.Allocate(BufferPool) = new Vector3(goldenRatio, oogr, 0); + points.Allocate(BufferPool) = new Vector3(goldenRatio, -oogr, 0); + points.Allocate(BufferPool) = new Vector3(-goldenRatio, oogr, 0); + points.Allocate(BufferPool) = new Vector3(-goldenRatio, -oogr, 0); + var convexHull = new ConvexHull(points.Span.Slice(points.Count), BufferPool, out _); var boxInertia = box.ComputeInertia(1); var capsuleInertia = capsule.ComputeInertia(1); @@ -97,6 +127,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) } } + //Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(new Box(500, 1, 500)))); DemoMeshHelper.CreateDeformedPlane(128, 128, (x, y) => new Vector3(x - 64, 2f * (float)(Math.Sin(x * 0.5f) * Math.Sin(y * 0.5f)), y - 64), new Vector3(4, 1, 4), BufferPool, out var mesh); Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); } From 7a0863ed02162ba5945bc228d7629a3c204e6d7e Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 15 Mar 2022 16:38:06 -0500 Subject: [PATCH 467/947] Shape generic constraints tightened. --- BepuPhysics/Collidables/BoundingBoxBatcher.cs | 4 ++-- BepuPhysics/Collidables/IShape.cs | 4 ++-- BepuPhysics/Collidables/Shapes.cs | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/BepuPhysics/Collidables/BoundingBoxBatcher.cs b/BepuPhysics/Collidables/BoundingBoxBatcher.cs index 74c863d04..f4998e6e5 100644 --- a/BepuPhysics/Collidables/BoundingBoxBatcher.cs +++ b/BepuPhysics/Collidables/BoundingBoxBatcher.cs @@ -228,8 +228,8 @@ public unsafe void ExecuteConvexBatch(ConvexShapeBatch(HomogeneousCompoundShapeBatch shapeBatch) where TShape : unmanaged, IHomogeneousCompoundShape - where TChildShape : IConvexShape - where TChildShapeWide : IShapeWide + where TChildShape : unmanaged, IConvexShape + where TChildShapeWide : unmanaged, IShapeWide { ref var batch = ref batches[shapeBatch.TypeId]; ref var activeSet = ref bodies.ActiveSet; diff --git a/BepuPhysics/Collidables/IShape.cs b/BepuPhysics/Collidables/IShape.cs index 59dfa1e4a..4636ba0a5 100644 --- a/BepuPhysics/Collidables/IShape.cs +++ b/BepuPhysics/Collidables/IShape.cs @@ -99,8 +99,8 @@ public interface ICompoundShape : IShape, IBoundsQueryableCompound /// Type of the child shapes. /// Type of the child shapes, formatted in AOSOA layout. public interface IHomogeneousCompoundShape : IShape, IBoundsQueryableCompound - where TChildShape : IConvexShape - where TChildShapeWide : IShapeWide + where TChildShape : unmanaged, IConvexShape + where TChildShapeWide : unmanaged, IShapeWide { void ComputeBounds(in Quaternion orientation, out Vector3 min, out Vector3 max); diff --git a/BepuPhysics/Collidables/Shapes.cs b/BepuPhysics/Collidables/Shapes.cs index ce8297317..caf429149 100644 --- a/BepuPhysics/Collidables/Shapes.cs +++ b/BepuPhysics/Collidables/Shapes.cs @@ -263,8 +263,8 @@ protected override void Dispose(int index, BufferPool pool) public class HomogeneousCompoundShapeBatch : ShapeBatch where TShape : unmanaged, IHomogeneousCompoundShape - where TChildShape : IConvexShape - where TChildShapeWide : IShapeWide + where TChildShape : unmanaged, IConvexShape + where TChildShapeWide : unmanaged, IShapeWide { public HomogeneousCompoundShapeBatch(BufferPool pool, int initialShapeCount) : base(pool, initialShapeCount) { From e5da026385131da2141cd4396bb58b5fe3ba6bc4 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 28 Mar 2022 21:58:15 -0500 Subject: [PATCH 468/947] Added a bit of validation to resizing to catch overflows. Avoid copying data pointlessly in MeshCache. --- BepuPhysics/Collidables/Mesh.cs | 2 +- BepuUtilities/Memory/BufferPool.cs | 1 + DemoRenderer/ShapeDrawing/MeshCache.cs | 2 +- 3 files changed, 3 insertions(+), 2 deletions(-) diff --git a/BepuPhysics/Collidables/Mesh.cs b/BepuPhysics/Collidables/Mesh.cs index d4acd04ec..fd0ef81da 100644 --- a/BepuPhysics/Collidables/Mesh.cs +++ b/BepuPhysics/Collidables/Mesh.cs @@ -72,7 +72,7 @@ public Vector3 Scale /// Scale to apply to all vertices at runtime. /// Note that the scale is not baked into the triangles or acceleration structure; the same set of triangles and acceleration structure can be used across multiple Mesh instances with different scales. /// Pool used to allocate acceleration structures. - public Mesh(Buffer triangles, in Vector3 scale, BufferPool pool) : this() + public Mesh(Buffer triangles, Vector3 scale, BufferPool pool) : this() { Triangles = triangles; Tree = new Tree(pool, triangles.Length); diff --git a/BepuUtilities/Memory/BufferPool.cs b/BepuUtilities/Memory/BufferPool.cs index c29bbb038..4aca1f864 100644 --- a/BepuUtilities/Memory/BufferPool.cs +++ b/BepuUtilities/Memory/BufferPool.cs @@ -418,6 +418,7 @@ public static int GetCapacityForCount(int count) { if (count == 0) count = 1; + Debug.Assert(BitOperations.RoundUpToPowerOf2((ulong)(count * Unsafe.SizeOf())) < int.MaxValue, "This function assumes that counts aren't going to overflow a signed 32 bit integer."); return ((int)BitOperations.RoundUpToPowerOf2((uint)(count * Unsafe.SizeOf()))) / Unsafe.SizeOf(); } diff --git a/DemoRenderer/ShapeDrawing/MeshCache.cs b/DemoRenderer/ShapeDrawing/MeshCache.cs index 53fb29f51..6357f9cf1 100644 --- a/DemoRenderer/ShapeDrawing/MeshCache.cs +++ b/DemoRenderer/ShapeDrawing/MeshCache.cs @@ -70,7 +70,7 @@ public unsafe bool Allocate(ulong id, int vertexCount, out int start, out Buffer //Didn't fit. We need to resize. var copyCount = TriangleBuffer.Capacity + vertexCount; var newSize = (int)BitOperations.RoundUpToPowerOf2((uint)copyCount); - Pool.ResizeToAtLeast(ref this.vertices, newSize, copyCount); + Pool.ResizeToAtLeast(ref this.vertices, newSize, 0); allocator.Capacity = newSize; allocator.Allocate(id, vertexCount, out longStart); start = (int)longStart; From 479ec93d0fff5d4a40266b8b71269b0e79e63a18 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 28 Mar 2022 22:00:29 -0500 Subject: [PATCH 469/947] Added DemoMeshHelper functions for dealing with giant meshes that the regular Mesh constructor's sweep build chokes on. --- Demos/DemoMeshHelper.cs | 120 ++++++++++++++++++++++++++++++++++++ Demos/Demos/Cars/CarDemo.cs | 2 +- 2 files changed, 121 insertions(+), 1 deletion(-) diff --git a/Demos/DemoMeshHelper.cs b/Demos/DemoMeshHelper.cs index 1b5adeeaf..325a0aa49 100644 --- a/Demos/DemoMeshHelper.cs +++ b/Demos/DemoMeshHelper.cs @@ -3,6 +3,8 @@ using System.Numerics; using BepuUtilities.Memory; using DemoContentLoader; +using BepuUtilities; +using BepuPhysics.Trees; namespace Demos { @@ -75,6 +77,124 @@ public static void CreateDeformedPlane(int width, int height, Func + /// Creates a bunch of nodes and associates them with leaves with absolutely no regard for where the leaves are. + ///

+ static void CreateDummyNodes(ref Tree tree, int nodeIndex, int nodeLeafCount, ref int leafCounter) + { + ref var node = ref tree.Nodes[nodeIndex]; + node.A.LeafCount = nodeLeafCount / 2; + if (node.A.LeafCount > 1) + { + node.A.Index = nodeIndex + 1; + tree.Metanodes[node.A.Index] = new Metanode { IndexInParent = 0, Parent = nodeIndex }; + CreateDummyNodes(ref tree, node.A.Index, node.A.LeafCount, ref leafCounter); + } + else + { + tree.Leaves[leafCounter] = new Leaf(nodeIndex, 0); + node.A.Index = Tree.Encode(leafCounter++); + } + node.B.LeafCount = nodeLeafCount - node.A.LeafCount; + if (node.B.LeafCount > 1) + { + node.B.Index = nodeIndex + node.A.LeafCount; + tree.Metanodes[node.B.Index] = new Metanode { IndexInParent = 1, Parent = nodeIndex }; + CreateDummyNodes(ref tree, node.B.Index, node.B.LeafCount, ref leafCounter); + } + else + { + tree.Leaves[leafCounter] = new Leaf(nodeIndex, 1); + node.B.Index = Tree.Encode(leafCounter++); + } + } + + /// + /// Takes a large number of triangles and creates a Mesh from them, but does not attempt to compute any bounds. + /// The topology of the mesh's acceleration structure is based entirely on the order of the triangles. + /// This is intended to be used with , , + /// or to provide bounds and higher quality. + /// + /// Large number of triangles to build a mesh from. + /// Scale to use for the mesh shape. + /// Buffer pool to allocate resources for the mesh. + /// Created mesh with no bounds. + /// This exists primarily as an easy example of how to work around the slow sequential default mesh building options for very large meshes, like heightmaps. + /// It is not optimized anywhere close to as much as it could be. + /// In the future, I'd like to give the Tree and Mesh much faster (and multithreaded) constructors that achieve quality and speed in one shot. + public static Mesh CreateGiantMeshFastWithoutBounds(Buffer triangles, Vector3 scaling, BufferPool pool) + { + if (triangles.Length < 128) + { + //The special logic isn't necessary for tiny meshes, and we also don't handle the corner case of leaf counts <= 2. Just use the regular constructor. + return new Mesh(triangles, scaling, pool); + } + Mesh mesh = new() + { + Triangles = triangles, + Tree = new Tree(pool, triangles.Length), + Scale = scaling + }; + mesh.Tree.NodeCount = triangles.Length - 1; + mesh.Tree.LeafCount = triangles.Length; + int leafCounter = 0; + CreateDummyNodes(ref mesh.Tree, 0, triangles.Length, ref leafCounter); + unsafe + { + for (int i = 0; i < triangles.Length; ++i) + { + ref var t = ref triangles[i]; + mesh.Tree.GetBoundsPointers(i, out var min, out var max); + *min = Vector3.Min(t.A, Vector3.Min(t.B, t.C)); + *max = Vector3.Max(t.A, Vector3.Max(t.B, t.C)); + } + } + return mesh; + } + + /// + /// Takes a very large number of triangles and turns them into a mesh by simply assuming that the input triangles are in an order that'll happen to produce an okay-ish acceleration structure. + /// If you have a large height map, you might want to use this instead of the Mesh constructor's default sweep build or insertion builder. + /// The quality is much lower than a sweep build (or even insertion build for that matter), but it can be orders of magnitude faster. + /// Consider using refinement to get the tree quality closer to the sweep builder's quality afterwards. + /// + /// Large number of triangles to build a mesh from. + /// Scale to use for the mesh shape. + /// Buffer pool to allocate resources for the mesh. + /// Created mesh. + /// This exists primarily as an easy example of how to work around the slow sequential default mesh building options for very large meshes, like heightmaps. + /// It is not optimized anywhere close to as much as it could be. + /// In the future, I'd like to give the Tree and Mesh much faster (and multithreaded) constructors that achieve quality and speed in one shot. + public static Mesh CreateGiantMeshFast(Buffer triangles, Vector3 scaling, BufferPool pool) + { + var mesh = CreateGiantMeshFastWithoutBounds(triangles, scaling, pool); + //None of the nodes actually have bounds. Give them some now. + mesh.Tree.Refit(); + return mesh; + } + + /// + /// Takes a very large number of triangles and turns them into a mesh by first creating a dummy topology and then incrementally refining it. + /// If you have a large height map, you might want to use this instead of the Mesh constructor's default sweep build or insertion builder. + /// The quality can approach at a much lower cost thanks to a more efficient algorithm and multithreading. + /// + /// Large number of triangles to build a mesh from. + /// Scale to use for the mesh shape. + /// Buffer pool to allocate resources for the mesh. + /// Created mesh. + /// This exists primarily as an easy example of how to work around the slow sequential default mesh building options for very large meshes, like heightmaps. + /// It is not optimized anywhere close to as much as it could be. + /// In the future, I'd like to give the Tree and Mesh much faster (and multithreaded) constructors that achieve quality and speed in one shot. + public static Mesh CreateGiantMeshWithRefinements(Buffer triangles, Vector3 scaling, BufferPool pool, Tree.RefitAndRefineMultithreadedContext context, IThreadDispatcher threadDispatcher, int refinementIterationCount = 8) + { + var mesh = CreateGiantMeshFastWithoutBounds(triangles, scaling, pool); + //None of the nodes actually have bounds. Give them some now. + for (int i = 0; i < refinementIterationCount; ++i) + context.RefitAndRefine(ref mesh.Tree, pool, threadDispatcher, i, 20); + return mesh; + } + } } diff --git a/Demos/Demos/Cars/CarDemo.cs b/Demos/Demos/Cars/CarDemo.cs index 82ceb1985..ba1b2643c 100644 --- a/Demos/Demos/Cars/CarDemo.cs +++ b/Demos/Demos/Cars/CarDemo.cs @@ -100,7 +100,7 @@ public override void Initialize(ContentArchive content, Camera camera) for (int i = 0; i < aiCount; ++i) { - //The AI cars are very similar, except... we handicap them a little to make the player good about themselves. + //The AI cars are very similar, except... we handicap them a little to make the player feel good about themselves. var position = min + span * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); var orientation = QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), random.NextSingle() * MathF.PI * 2); aiControllers[i].Controller = new SimpleCarController(SimpleCar.Create(Simulation, properties, (position, orientation), bodyShapeIndex, bodyInertia, 0.5f, wheelShapeIndex, wheelInertia, 2f, From 3e2cdecb505cb7bd7a68332b515fe8547e8f4e59 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 28 Mar 2022 22:16:52 -0500 Subject: [PATCH 470/947] Stinky unsafe block. --- Demos/DemoMeshHelper.cs | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/Demos/DemoMeshHelper.cs b/Demos/DemoMeshHelper.cs index 325a0aa49..dd5346742 100644 --- a/Demos/DemoMeshHelper.cs +++ b/Demos/DemoMeshHelper.cs @@ -123,7 +123,7 @@ static void CreateDummyNodes(ref Tree tree, int nodeIndex, int nodeLeafCount, re /// This exists primarily as an easy example of how to work around the slow sequential default mesh building options for very large meshes, like heightmaps. /// It is not optimized anywhere close to as much as it could be. /// In the future, I'd like to give the Tree and Mesh much faster (and multithreaded) constructors that achieve quality and speed in one shot. - public static Mesh CreateGiantMeshFastWithoutBounds(Buffer triangles, Vector3 scaling, BufferPool pool) + public unsafe static Mesh CreateGiantMeshFastWithoutBounds(Buffer triangles, Vector3 scaling, BufferPool pool) { if (triangles.Length < 128) { @@ -140,15 +140,12 @@ public static Mesh CreateGiantMeshFastWithoutBounds(Buffer triangles, mesh.Tree.LeafCount = triangles.Length; int leafCounter = 0; CreateDummyNodes(ref mesh.Tree, 0, triangles.Length, ref leafCounter); - unsafe + for (int i = 0; i < triangles.Length; ++i) { - for (int i = 0; i < triangles.Length; ++i) - { - ref var t = ref triangles[i]; - mesh.Tree.GetBoundsPointers(i, out var min, out var max); - *min = Vector3.Min(t.A, Vector3.Min(t.B, t.C)); - *max = Vector3.Max(t.A, Vector3.Max(t.B, t.C)); - } + ref var t = ref triangles[i]; + mesh.Tree.GetBoundsPointers(i, out var min, out var max); + *min = Vector3.Min(t.A, Vector3.Min(t.B, t.C)); + *max = Vector3.Max(t.A, Vector3.Max(t.B, t.C)); } return mesh; } From 5d45c69fad2d6b07390976b01c8ae4facf8385b7 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 30 Mar 2022 16:03:15 -0500 Subject: [PATCH 471/947] Workflow expanded to other branches. --- .github/workflows/dotnet-core.yml | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/.github/workflows/dotnet-core.yml b/.github/workflows/dotnet-core.yml index 2a7b78c28..c344f82eb 100644 --- a/.github/workflows/dotnet-core.yml +++ b/.github/workflows/dotnet-core.yml @@ -1,10 +1,6 @@ name: .NET Core -on: - push: - branches: [ master ] - pull_request: - branches: [ master ] +on: [push, pull_request] jobs: build: From e651aa07de91fcc895135b7a98494f6a6300f577 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 9 Apr 2022 16:38:14 -0500 Subject: [PATCH 472/947] Added ref returning enqueue and dequeue options to QuickQueue. QuickQueue no longer attempts to zero memory on remove. --- BepuUtilities/Collections/QuickQueue.cs | 138 ++++++++++++++++++------ 1 file changed, 106 insertions(+), 32 deletions(-) diff --git a/BepuUtilities/Collections/QuickQueue.cs b/BepuUtilities/Collections/QuickQueue.cs index de3cae56c..b91a59be8 100644 --- a/BepuUtilities/Collections/QuickQueue.cs +++ b/BepuUtilities/Collections/QuickQueue.cs @@ -213,55 +213,100 @@ public void Compact(IUnmanagedMemoryPool pool) } /// - /// Enqueues the element to the end of the queue, incrementing the last index. + /// Enqueues a slot on the end of the queue, incrementing the last index. + /// Does not attempt to resize; this implementation assumes the underlying buffer is large enough for the new slot. /// - /// Item to enqueue. + /// Reference to the enqueued slot. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void EnqueueUnsafely(in T element) + public ref T EnqueueUnsafely() { Validate(); ValidateUnsafeAdd(); - Span[(LastIndex = ((LastIndex + 1) & CapacityMask))] = element; ++Count; + return ref Span[(LastIndex = ((LastIndex + 1) & CapacityMask))]; } /// - /// Enqueues the element to the start of the queue, decrementing the first index. + /// Pushes a slot at the front of the queue, decrementing the first index. + /// Does not attempt to resize; this implementation assumes the underlying buffer is large enough for the new slot. /// - /// Item to enqueue. + /// Reference to the enqueued slot. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void EnqueueFirstUnsafely(in T element) + public ref T EnqueueFirstUnsafely() { Validate(); ValidateUnsafeAdd(); - Span[(FirstIndex = ((FirstIndex - 1) & CapacityMask))] = element; ++Count; + return ref Span[(FirstIndex = ((FirstIndex - 1) & CapacityMask))]; } - /// - /// Enqueues the element to the end of the queue, incrementing the last index. + /// Enqueues a slot on the end of the queue, incrementing the last index. /// - /// Item to enqueue. + /// Pool to use to resize the queue's internal buffer if necessary. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Enqueue(in T element, IUnmanagedMemoryPool pool) + public ref T Enqueue(IUnmanagedMemoryPool pool) { Validate(); if (Count == Span.Length) Resize(Span.Length * 2, pool); - EnqueueUnsafely(element); + return ref EnqueueUnsafely(); } /// - /// Enqueues the element to the start of the queue, decrementing the first index. + /// Pushes a slot at the front of the queue, decrementing the first index. /// - /// Item to enqueue. + /// Pool to use to resize the queue's internal buffer if necessary. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void EnqueueFirst(in T element, IUnmanagedMemoryPool pool) + public ref T EnqueueFirst(IUnmanagedMemoryPool pool) { Validate(); if (Count == Span.Length) Resize(Span.Length * 2, pool); - EnqueueFirstUnsafely(element); + return ref EnqueueFirstUnsafely(); + } + + /// + /// Enqueues the element to the end of the queue, incrementing the last index. + /// Does not attempt to resize; this implementation assumes the underlying buffer is large enough for the new slot. + /// + /// Item to enqueue. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void EnqueueUnsafely(in T element) + { + EnqueueUnsafely() = element; + } + + /// + /// Enqueues the element to the start of the queue, decrementing the first index. + /// Does not attempt to resize; this implementation assumes the underlying buffer is large enough for the new slot. + /// + /// Item to enqueue. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void EnqueueFirstUnsafely(in T element) + { + EnqueueFirstUnsafely() = element; + } + + /// + /// Enqueues the element to the end of the queue, incrementing the last index. + /// + /// Item to enqueue. + /// Pool to use to resize the queue's internal buffer if necessary. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void Enqueue(in T element, IUnmanagedMemoryPool pool) + { + Enqueue(pool) = element; + } + + /// + /// Enqueues the element to the start of the queue, decrementing the first index. + /// + /// Item to enqueue. + /// Pool to use to resize the queue's internal buffer if necessary. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void EnqueueFirst(in T element, IUnmanagedMemoryPool pool) + { + EnqueueFirst(pool) = element; } /// @@ -275,7 +320,7 @@ public T Dequeue() if (Count == 0) throw new InvalidOperationException("The queue is empty."); var element = Span[FirstIndex]; - DeleteFirst(); + IncrementFirst(); return element; } @@ -291,9 +336,8 @@ public T DequeueLast() if (Count == 0) throw new InvalidOperationException("The queue is empty."); var element = Span[LastIndex]; - DeleteLast(); + DecrementLast(); return element; - } /// @@ -308,7 +352,7 @@ public bool TryDequeue(out T element) if (Count > 0) { element = Span[FirstIndex]; - DeleteFirst(); + IncrementFirst(); return true; } element = default; @@ -328,24 +372,54 @@ public bool TryDequeueLast(out T element) if (Count > 0) { element = Span[LastIndex]; - DeleteLast(); + DecrementLast(); return true; } - element = default(T); + element = default; return false; + } + + /// + /// Dequeues a slot from the start of the queue, incrementing the first index and returning a reference to the slot. Does not check count before attempting to dequeue. + /// + /// Reference to the slot removed from the queue. + /// Be very careful with this function; it is easy to accidentally destroy your foot in sneaky ways. + /// Consider what happens if you call after this function- both functions will return references to the same slot. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public ref T DequeueUnsafely() + { + Validate(); + Debug.Assert(Count > 0, "Can't dequeue from an empty queue."); + ref var element = ref Span[FirstIndex]; + IncrementFirst(); + return ref element; + } + /// + /// Dequeues a slot from the end of the queue, decrementing the last index and returning a reference to the slot. Does not check count before attempting to dequeue. + /// + /// Reference to the slot removed from the queue. + /// Be very careful with this function; it is easy to accidentally destroy your foot in sneaky ways. + /// Consider what happens if you call after this function- both functions will return references to the same slot. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public ref T DequeueLastUnsafely() + { + Validate(); + Debug.Assert(Count > 0, "Can't dequeue from an empty queue."); + ref var element = ref Span[LastIndex]; + DecrementLast(); + return ref element; } + [MethodImpl(MethodImplOptions.AggressiveInlining)] - void DeleteFirst() + void IncrementFirst() { - Span[FirstIndex] = default(T); FirstIndex = (FirstIndex + 1) & CapacityMask; --Count; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - void DeleteLast() + void DecrementLast() { - Span[LastIndex] = default(T); LastIndex = (LastIndex - 1) & CapacityMask; --Count; } @@ -361,12 +435,12 @@ public void RemoveAt(int queueIndex) var arrayIndex = GetBackingArrayIndex(queueIndex); if (LastIndex == arrayIndex) { - DeleteLast(); + DecrementLast(); return; } if (FirstIndex == arrayIndex) { - DeleteFirst(); + IncrementFirst(); return; } //It's internal. @@ -383,12 +457,12 @@ public void RemoveAt(int queueIndex) (FirstIndex < LastIndex && (LastIndex - arrayIndex) < (arrayIndex - FirstIndex))) //Case 3 { Span.CopyTo(arrayIndex + 1, Span, arrayIndex, LastIndex - arrayIndex); - DeleteLast(); + DecrementLast(); } else { Span.CopyTo(FirstIndex, Span, FirstIndex + 1, arrayIndex - FirstIndex); - DeleteFirst(); + IncrementFirst(); } } @@ -420,7 +494,7 @@ public void Clear() } /// - /// Clears the queue without changing any of the values in the backing array. Be careful about using this if the queue contains reference types. + /// Clears the queue without changing any of the values in the backing array. /// [MethodImpl(MethodImplOptions.AggressiveInlining)] public void FastClear() From c61229a33537eee856cd2f0171d0c1748fd965e9 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 15 Apr 2022 15:50:10 -0500 Subject: [PATCH 473/947] Removed ReleaseStrip from Demos.GL. Added documentation on dealing with a content pipeline build error. --- DemoContentBuilder/FTL.TXT | 169 +++++++++++++++++++++++++++++++++++++ Demos.GL.sln | 60 ------------- Demos.GL/Demos.csproj | 9 +- Documentation/Building.md | 6 ++ 4 files changed, 177 insertions(+), 67 deletions(-) create mode 100644 DemoContentBuilder/FTL.TXT diff --git a/DemoContentBuilder/FTL.TXT b/DemoContentBuilder/FTL.TXT new file mode 100644 index 000000000..c406d150f --- /dev/null +++ b/DemoContentBuilder/FTL.TXT @@ -0,0 +1,169 @@ + The FreeType Project LICENSE + ---------------------------- + + 2006-Jan-27 + + Copyright 1996-2002, 2006 by + David Turner, Robert Wilhelm, and Werner Lemberg + + + +Introduction +============ + + The FreeType Project is distributed in several archive packages; + some of them may contain, in addition to the FreeType font engine, + various tools and contributions which rely on, or relate to, the + FreeType Project. + + This license applies to all files found in such packages, and + which do not fall under their own explicit license. The license + affects thus the FreeType font engine, the test programs, + documentation and makefiles, at the very least. + + This license was inspired by the BSD, Artistic, and IJG + (Independent JPEG Group) licenses, which all encourage inclusion + and use of free software in commercial and freeware products + alike. As a consequence, its main points are that: + + o We don't promise that this software works. However, we will be + interested in any kind of bug reports. (`as is' distribution) + + o You can use this software for whatever you want, in parts or + full form, without having to pay us. (`royalty-free' usage) + + o You may not pretend that you wrote this software. If you use + it, or only parts of it, in a program, you must acknowledge + somewhere in your documentation that you have used the + FreeType code. (`credits') + + We specifically permit and encourage the inclusion of this + software, with or without modifications, in commercial products. + We disclaim all warranties covering The FreeType Project and + assume no liability related to The FreeType Project. + + + Finally, many people asked us for a preferred form for a + credit/disclaimer to use in compliance with this license. We thus + encourage you to use the following text: + + """ + Portions of this software are copyright © The FreeType + Project (www.freetype.org). All rights reserved. + """ + + Please replace with the value from the FreeType version you + actually use. + + +Legal Terms +=========== + +0. Definitions +-------------- + + Throughout this license, the terms `package', `FreeType Project', + and `FreeType archive' refer to the set of files originally + distributed by the authors (David Turner, Robert Wilhelm, and + Werner Lemberg) as the `FreeType Project', be they named as alpha, + beta or final release. + + `You' refers to the licensee, or person using the project, where + `using' is a generic term including compiling the project's source + code as well as linking it to form a `program' or `executable'. + This program is referred to as `a program using the FreeType + engine'. + + This license applies to all files distributed in the original + FreeType Project, including all source code, binaries and + documentation, unless otherwise stated in the file in its + original, unmodified form as distributed in the original archive. + If you are unsure whether or not a particular file is covered by + this license, you must contact us to verify this. + + The FreeType Project is copyright (C) 1996-2000 by David Turner, + Robert Wilhelm, and Werner Lemberg. All rights reserved except as + specified below. + +1. No Warranty +-------------- + + THE FREETYPE PROJECT IS PROVIDED `AS IS' WITHOUT WARRANTY OF ANY + KIND, EITHER EXPRESS OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, + WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + PURPOSE. IN NO EVENT WILL ANY OF THE AUTHORS OR COPYRIGHT HOLDERS + BE LIABLE FOR ANY DAMAGES CAUSED BY THE USE OR THE INABILITY TO + USE, OF THE FREETYPE PROJECT. + +2. Redistribution +----------------- + + This license grants a worldwide, royalty-free, perpetual and + irrevocable right and license to use, execute, perform, compile, + display, copy, create derivative works of, distribute and + sublicense the FreeType Project (in both source and object code + forms) and derivative works thereof for any purpose; and to + authorize others to exercise some or all of the rights granted + herein, subject to the following conditions: + + o Redistribution of source code must retain this license file + (`FTL.TXT') unaltered; any additions, deletions or changes to + the original files must be clearly indicated in accompanying + documentation. The copyright notices of the unaltered, + original files must be preserved in all copies of source + files. + + o Redistribution in binary form must provide a disclaimer that + states that the software is based in part of the work of the + FreeType Team, in the distribution documentation. We also + encourage you to put an URL to the FreeType web page in your + documentation, though this isn't mandatory. + + These conditions apply to any software derived from or based on + the FreeType Project, not just the unmodified files. If you use + our work, you must acknowledge us. However, no fee need be paid + to us. + +3. Advertising +-------------- + + Neither the FreeType authors and contributors nor you shall use + the name of the other for commercial, advertising, or promotional + purposes without specific prior written permission. + + We suggest, but do not require, that you use one or more of the + following phrases to refer to this software in your documentation + or advertising materials: `FreeType Project', `FreeType Engine', + `FreeType library', or `FreeType Distribution'. + + As you have not signed this license, you are not required to + accept it. However, as the FreeType Project is copyrighted + material, only this license, or another one contracted with the + authors, grants you the right to use, distribute, and modify it. + Therefore, by using, distributing, or modifying the FreeType + Project, you indicate that you understand and accept all the terms + of this license. + +4. Contacts +----------- + + There are two mailing lists related to FreeType: + + o freetype@nongnu.org + + Discusses general use and applications of FreeType, as well as + future and wanted additions to the library and distribution. + If you are looking for support, start in this list if you + haven't found anything to help you in the documentation. + + o freetype-devel@nongnu.org + + Discusses bugs, as well as engine internals, design issues, + specific licenses, porting, etc. + + Our home page can be found at + + https://www.freetype.org + + +--- end of FTL.TXT --- diff --git a/Demos.GL.sln b/Demos.GL.sln index 49b62c2f0..ed964e8a3 100644 --- a/Demos.GL.sln +++ b/Demos.GL.sln @@ -31,10 +31,6 @@ Global ReleaseNoProfiling|ARM = ReleaseNoProfiling|ARM ReleaseNoProfiling|x64 = ReleaseNoProfiling|x64 ReleaseNoProfiling|x86 = ReleaseNoProfiling|x86 - ReleaseStrip|Any CPU = ReleaseStrip|Any CPU - ReleaseStrip|ARM = ReleaseStrip|ARM - ReleaseStrip|x64 = ReleaseStrip|x64 - ReleaseStrip|x86 = ReleaseStrip|x86 EndGlobalSection GlobalSection(ProjectConfigurationPlatforms) = postSolution {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.Debug|Any CPU.ActiveCfg = Debug|Any CPU @@ -61,14 +57,6 @@ Global {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.ReleaseNoProfiling|x64.Build.0 = Release|Any CPU {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.ReleaseNoProfiling|x86.ActiveCfg = Release|Any CPU {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.ReleaseNoProfiling|x86.Build.0 = Release|Any CPU - {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.ReleaseStrip|Any CPU.ActiveCfg = Release|Any CPU - {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.ReleaseStrip|Any CPU.Build.0 = Release|Any CPU - {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.ReleaseStrip|ARM.ActiveCfg = Release|Any CPU - {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.ReleaseStrip|ARM.Build.0 = Release|Any CPU - {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.ReleaseStrip|x64.ActiveCfg = Release|Any CPU - {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.ReleaseStrip|x64.Build.0 = Release|Any CPU - {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.ReleaseStrip|x86.ActiveCfg = Release|Any CPU - {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.ReleaseStrip|x86.Build.0 = Release|Any CPU {499C899F-CD56-476E-AFF8-85A8C29B19BF}.Debug|Any CPU.ActiveCfg = Debug|Any CPU {499C899F-CD56-476E-AFF8-85A8C29B19BF}.Debug|Any CPU.Build.0 = Debug|Any CPU {499C899F-CD56-476E-AFF8-85A8C29B19BF}.Debug|ARM.ActiveCfg = Debug|Any CPU @@ -93,14 +81,6 @@ Global {499C899F-CD56-476E-AFF8-85A8C29B19BF}.ReleaseNoProfiling|x64.Build.0 = Release|Any CPU {499C899F-CD56-476E-AFF8-85A8C29B19BF}.ReleaseNoProfiling|x86.ActiveCfg = Release|Any CPU {499C899F-CD56-476E-AFF8-85A8C29B19BF}.ReleaseNoProfiling|x86.Build.0 = Release|Any CPU - {499C899F-CD56-476E-AFF8-85A8C29B19BF}.ReleaseStrip|Any CPU.ActiveCfg = Release|Any CPU - {499C899F-CD56-476E-AFF8-85A8C29B19BF}.ReleaseStrip|Any CPU.Build.0 = Release|Any CPU - {499C899F-CD56-476E-AFF8-85A8C29B19BF}.ReleaseStrip|ARM.ActiveCfg = Release|Any CPU - {499C899F-CD56-476E-AFF8-85A8C29B19BF}.ReleaseStrip|ARM.Build.0 = Release|Any CPU - {499C899F-CD56-476E-AFF8-85A8C29B19BF}.ReleaseStrip|x64.ActiveCfg = Release|Any CPU - {499C899F-CD56-476E-AFF8-85A8C29B19BF}.ReleaseStrip|x64.Build.0 = Release|Any CPU - {499C899F-CD56-476E-AFF8-85A8C29B19BF}.ReleaseStrip|x86.ActiveCfg = Release|Any CPU - {499C899F-CD56-476E-AFF8-85A8C29B19BF}.ReleaseStrip|x86.Build.0 = Release|Any CPU {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.Debug|Any CPU.ActiveCfg = Debug|x64 {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.Debug|Any CPU.Build.0 = Debug|x64 {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.Debug|ARM.ActiveCfg = Debug|x64 @@ -125,14 +105,6 @@ Global {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.ReleaseNoProfiling|x64.Build.0 = Release|x64 {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.ReleaseNoProfiling|x86.ActiveCfg = Release|x64 {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.ReleaseNoProfiling|x86.Build.0 = Release|x64 - {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.ReleaseStrip|Any CPU.ActiveCfg = Release|x64 - {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.ReleaseStrip|Any CPU.Build.0 = Release|x64 - {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.ReleaseStrip|ARM.ActiveCfg = Release|x64 - {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.ReleaseStrip|ARM.Build.0 = Release|x64 - {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.ReleaseStrip|x64.ActiveCfg = Release|x64 - {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.ReleaseStrip|x64.Build.0 = Release|x64 - {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.ReleaseStrip|x86.ActiveCfg = Release|x64 - {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.ReleaseStrip|x86.Build.0 = Release|x64 {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.Debug|Any CPU.ActiveCfg = Debug|Any CPU {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.Debug|Any CPU.Build.0 = Debug|Any CPU {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.Debug|ARM.ActiveCfg = Debug|Any CPU @@ -157,14 +129,6 @@ Global {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.ReleaseNoProfiling|x64.Build.0 = Release|Any CPU {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.ReleaseNoProfiling|x86.ActiveCfg = Release|Any CPU {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.ReleaseNoProfiling|x86.Build.0 = Release|Any CPU - {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.ReleaseStrip|Any CPU.ActiveCfg = Release|Any CPU - {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.ReleaseStrip|Any CPU.Build.0 = Release|Any CPU - {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.ReleaseStrip|ARM.ActiveCfg = Release|Any CPU - {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.ReleaseStrip|ARM.Build.0 = Release|Any CPU - {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.ReleaseStrip|x64.ActiveCfg = Release|Any CPU - {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.ReleaseStrip|x64.Build.0 = Release|Any CPU - {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.ReleaseStrip|x86.ActiveCfg = Release|Any CPU - {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.ReleaseStrip|x86.Build.0 = Release|Any CPU {5FBC743A-8911-4DE6-B136-C0B274E1B185}.Debug|Any CPU.ActiveCfg = Debug|Any CPU {5FBC743A-8911-4DE6-B136-C0B274E1B185}.Debug|Any CPU.Build.0 = Debug|Any CPU {5FBC743A-8911-4DE6-B136-C0B274E1B185}.Debug|ARM.ActiveCfg = Debug|Any CPU @@ -189,14 +153,6 @@ Global {5FBC743A-8911-4DE6-B136-C0B274E1B185}.ReleaseNoProfiling|x64.Build.0 = ReleaseNoProfiling|Any CPU {5FBC743A-8911-4DE6-B136-C0B274E1B185}.ReleaseNoProfiling|x86.ActiveCfg = ReleaseNoProfiling|Any CPU {5FBC743A-8911-4DE6-B136-C0B274E1B185}.ReleaseNoProfiling|x86.Build.0 = ReleaseNoProfiling|Any CPU - {5FBC743A-8911-4DE6-B136-C0B274E1B185}.ReleaseStrip|Any CPU.ActiveCfg = ReleaseNoProfiling|Any CPU - {5FBC743A-8911-4DE6-B136-C0B274E1B185}.ReleaseStrip|Any CPU.Build.0 = ReleaseNoProfiling|Any CPU - {5FBC743A-8911-4DE6-B136-C0B274E1B185}.ReleaseStrip|ARM.ActiveCfg = ReleaseNoProfiling|Any CPU - {5FBC743A-8911-4DE6-B136-C0B274E1B185}.ReleaseStrip|ARM.Build.0 = ReleaseNoProfiling|Any CPU - {5FBC743A-8911-4DE6-B136-C0B274E1B185}.ReleaseStrip|x64.ActiveCfg = ReleaseNoProfiling|Any CPU - {5FBC743A-8911-4DE6-B136-C0B274E1B185}.ReleaseStrip|x64.Build.0 = ReleaseNoProfiling|Any CPU - {5FBC743A-8911-4DE6-B136-C0B274E1B185}.ReleaseStrip|x86.ActiveCfg = ReleaseNoProfiling|Any CPU - {5FBC743A-8911-4DE6-B136-C0B274E1B185}.ReleaseStrip|x86.Build.0 = ReleaseNoProfiling|Any CPU {76B75BB7-7AC8-4942-A3EA-314CB04C0B85}.Debug|Any CPU.ActiveCfg = Debug|Any CPU {76B75BB7-7AC8-4942-A3EA-314CB04C0B85}.Debug|Any CPU.Build.0 = Debug|Any CPU {76B75BB7-7AC8-4942-A3EA-314CB04C0B85}.Debug|ARM.ActiveCfg = Debug|Any CPU @@ -221,14 +177,6 @@ Global {76B75BB7-7AC8-4942-A3EA-314CB04C0B85}.ReleaseNoProfiling|x64.Build.0 = Release|Any CPU {76B75BB7-7AC8-4942-A3EA-314CB04C0B85}.ReleaseNoProfiling|x86.ActiveCfg = Release|Any CPU {76B75BB7-7AC8-4942-A3EA-314CB04C0B85}.ReleaseNoProfiling|x86.Build.0 = Release|Any CPU - {76B75BB7-7AC8-4942-A3EA-314CB04C0B85}.ReleaseStrip|Any CPU.ActiveCfg = ReleaseStrip|Any CPU - {76B75BB7-7AC8-4942-A3EA-314CB04C0B85}.ReleaseStrip|Any CPU.Build.0 = ReleaseStrip|Any CPU - {76B75BB7-7AC8-4942-A3EA-314CB04C0B85}.ReleaseStrip|ARM.ActiveCfg = ReleaseStrip|Any CPU - {76B75BB7-7AC8-4942-A3EA-314CB04C0B85}.ReleaseStrip|ARM.Build.0 = ReleaseStrip|Any CPU - {76B75BB7-7AC8-4942-A3EA-314CB04C0B85}.ReleaseStrip|x64.ActiveCfg = ReleaseStrip|Any CPU - {76B75BB7-7AC8-4942-A3EA-314CB04C0B85}.ReleaseStrip|x64.Build.0 = ReleaseStrip|Any CPU - {76B75BB7-7AC8-4942-A3EA-314CB04C0B85}.ReleaseStrip|x86.ActiveCfg = ReleaseStrip|Any CPU - {76B75BB7-7AC8-4942-A3EA-314CB04C0B85}.ReleaseStrip|x86.Build.0 = ReleaseStrip|Any CPU {85C39598-1A63-4944-B619-25F3CD76C7A2}.Debug|Any CPU.ActiveCfg = Debug|Any CPU {85C39598-1A63-4944-B619-25F3CD76C7A2}.Debug|Any CPU.Build.0 = Debug|Any CPU {85C39598-1A63-4944-B619-25F3CD76C7A2}.Debug|ARM.ActiveCfg = Debug|Any CPU @@ -253,14 +201,6 @@ Global {85C39598-1A63-4944-B619-25F3CD76C7A2}.ReleaseNoProfiling|x64.Build.0 = Release|Any CPU {85C39598-1A63-4944-B619-25F3CD76C7A2}.ReleaseNoProfiling|x86.ActiveCfg = Release|Any CPU {85C39598-1A63-4944-B619-25F3CD76C7A2}.ReleaseNoProfiling|x86.Build.0 = Release|Any CPU - {85C39598-1A63-4944-B619-25F3CD76C7A2}.ReleaseStrip|Any CPU.ActiveCfg = Release|Any CPU - {85C39598-1A63-4944-B619-25F3CD76C7A2}.ReleaseStrip|Any CPU.Build.0 = Release|Any CPU - {85C39598-1A63-4944-B619-25F3CD76C7A2}.ReleaseStrip|ARM.ActiveCfg = Release|Any CPU - {85C39598-1A63-4944-B619-25F3CD76C7A2}.ReleaseStrip|ARM.Build.0 = Release|Any CPU - {85C39598-1A63-4944-B619-25F3CD76C7A2}.ReleaseStrip|x64.ActiveCfg = Release|Any CPU - {85C39598-1A63-4944-B619-25F3CD76C7A2}.ReleaseStrip|x64.Build.0 = Release|Any CPU - {85C39598-1A63-4944-B619-25F3CD76C7A2}.ReleaseStrip|x86.ActiveCfg = Release|Any CPU - {85C39598-1A63-4944-B619-25F3CD76C7A2}.ReleaseStrip|x86.Build.0 = Release|Any CPU EndGlobalSection GlobalSection(SolutionProperties) = preSolution HideSolutionNode = FALSE diff --git a/Demos.GL/Demos.csproj b/Demos.GL/Demos.csproj index 7a83761f9..c905ce396 100644 --- a/Demos.GL/Demos.csproj +++ b/Demos.GL/Demos.csproj @@ -3,7 +3,7 @@ Exe net6.0 True - Debug;Release;ReleaseStrip + Debug;Release latest @@ -16,13 +16,8 @@ true TRACE;RELEASE - embedded true - - - - true - TRACE;RELEASE + full diff --git a/Documentation/Building.md b/Documentation/Building.md index 542c435a3..e8f23ff92 100644 --- a/Documentation/Building.md +++ b/Documentation/Building.md @@ -14,6 +14,12 @@ The libraries target .NET 6. `Demos.sln` contains all the projects necessary to build and run the demos application. The default demo renderer uses DX11, and the content pipeline's shader compiler requires the Windows SDK. There is also a Demos.GL.sln that uses OpenGL and should run on other platforms. The demos can be run from the command line (in the repo root directory) with `dotnet run --project Demos/Demos.csproj -c Release` or `dotnet run --project Demos.GL/Demos.csproj -c Release`. +The demos content pipeline uses [freetype](https://freetype.org/). On windows, the freetype.dll is included. When built elsewhere, the build will attempt to pull the dependency out of `/usr/lib`. If you try to build on windows and see an error that says: + +`Content build failed: Unable to load DLL 'freetype6' or one of its dependencies: The specified module could not be found. (0x8007007E)` + +then it's likely that freetype version used by the content builder needs the [VC++ 2013 redistributable](https://www.microsoft.com/en-us/download/details.aspx?id=40784) to be installed. + The demos applications target .NET 6. ## Build Configurations From 16f5631ee7b4380cec3d7d9e4182239d5cab24eb Mon Sep 17 00:00:00 2001 From: Paul Bartrum Date: Sun, 24 Apr 2022 01:06:18 +1200 Subject: [PATCH 474/947] Enable Source Link support. --- BepuPhysics/BepuPhysics.csproj | 8 ++++++++ BepuUtilities/BepuUtilities.csproj | 8 ++++++++ 2 files changed, 16 insertions(+) diff --git a/BepuPhysics/BepuPhysics.csproj b/BepuPhysics/BepuPhysics.csproj index 159eb917a..eada4473c 100644 --- a/BepuPhysics/BepuPhysics.csproj +++ b/BepuPhysics/BepuPhysics.csproj @@ -21,6 +21,9 @@ key.snk true + true + true + snupkg @@ -83,4 +86,9 @@ + + + + + \ No newline at end of file diff --git a/BepuUtilities/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj index 1032ab299..59b94c427 100644 --- a/BepuUtilities/BepuUtilities.csproj +++ b/BepuUtilities/BepuUtilities.csproj @@ -22,6 +22,9 @@ key.snk true + true + true + snupkg @@ -41,4 +44,9 @@ + + + + + \ No newline at end of file From bb2f2faf7a6892b00803b6a0dcf9957d760374fa Mon Sep 17 00:00:00 2001 From: Paul Bartrum Date: Sun, 24 Apr 2022 01:41:01 +1200 Subject: [PATCH 475/947] Turns out is not needed if is set. --- BepuPhysics/BepuPhysics.csproj | 1 - BepuUtilities/BepuUtilities.csproj | 1 - 2 files changed, 2 deletions(-) diff --git a/BepuPhysics/BepuPhysics.csproj b/BepuPhysics/BepuPhysics.csproj index eada4473c..8984a0038 100644 --- a/BepuPhysics/BepuPhysics.csproj +++ b/BepuPhysics/BepuPhysics.csproj @@ -21,7 +21,6 @@ key.snk true - true true snupkg diff --git a/BepuUtilities/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj index 59b94c427..d8973d9d9 100644 --- a/BepuUtilities/BepuUtilities.csproj +++ b/BepuUtilities/BepuUtilities.csproj @@ -22,7 +22,6 @@ key.snk true - true true snupkg From 44db9257ce6cf6a1e84fcdac7dc4fd6e4d24a3f5 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 23 Apr 2022 15:30:36 -0500 Subject: [PATCH 476/947] Bumped version number. --- BepuPhysics/BepuPhysics.csproj | 2 +- BepuUtilities/BepuUtilities.csproj | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/BepuPhysics/BepuPhysics.csproj b/BepuPhysics/BepuPhysics.csproj index 8984a0038..bb086760b 100644 --- a/BepuPhysics/BepuPhysics.csproj +++ b/BepuPhysics/BepuPhysics.csproj @@ -1,7 +1,7 @@  net6.0 - 2.4.0 + 2.5.0-beta.0 Bepu Entertainment LLC Ross Nordby Speedy real time physics simulation library. diff --git a/BepuUtilities/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj index d8973d9d9..e92d9a726 100644 --- a/BepuUtilities/BepuUtilities.csproj +++ b/BepuUtilities/BepuUtilities.csproj @@ -3,7 +3,7 @@ BepuUtilities BepuUtilities net6.0 - 2.4.0 + 2.5.0-beta.0 Bepu Entertainment LLC Ross Nordby Supporting utilities library for BEPUphysics v2. From 36dc91bf75746de50e6d7c9c475e6a10c29abee9 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 23 Apr 2022 16:36:11 -0500 Subject: [PATCH 477/947] Bumped version number again; debug type changed to default. --- BepuPhysics/BepuPhysics.csproj | 4 ++-- BepuUtilities/BepuUtilities.csproj | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/BepuPhysics/BepuPhysics.csproj b/BepuPhysics/BepuPhysics.csproj index bb086760b..d910ec5f9 100644 --- a/BepuPhysics/BepuPhysics.csproj +++ b/BepuPhysics/BepuPhysics.csproj @@ -1,7 +1,7 @@  net6.0 - 2.5.0-beta.0 + 2.5.0-beta.1 Bepu Entertainment LLC Ross Nordby Speedy real time physics simulation library. @@ -15,7 +15,7 @@ physics;3d;rigid body;real time;simulation True true - full + true false key.snk diff --git a/BepuUtilities/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj index e92d9a726..72c0b8888 100644 --- a/BepuUtilities/BepuUtilities.csproj +++ b/BepuUtilities/BepuUtilities.csproj @@ -3,7 +3,7 @@ BepuUtilities BepuUtilities net6.0 - 2.5.0-beta.0 + 2.5.0-beta.1 Bepu Entertainment LLC Ross Nordby Supporting utilities library for BEPUphysics v2. @@ -16,7 +16,7 @@ latest True true - full + true false key.snk From a703e197c3d5fa2a801586bf4c28fecc3fabfae5 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 2 May 2022 18:17:14 -0500 Subject: [PATCH 478/947] Added PerBodyGravityDemo. --- BepuPhysics/CollidableProperty.cs | 3 +- Demos/DemoSet.cs | 1 + Demos/Demos/PerBodyGravityDemo.cs | 162 ++++++++++++++++++++++++++++++ 3 files changed, 164 insertions(+), 2 deletions(-) create mode 100644 Demos/Demos/PerBodyGravityDemo.cs diff --git a/BepuPhysics/CollidableProperty.cs b/BepuPhysics/CollidableProperty.cs index 2d0746d56..bdffeef0a 100644 --- a/BepuPhysics/CollidableProperty.cs +++ b/BepuPhysics/CollidableProperty.cs @@ -50,8 +50,7 @@ public CollidableProperty(Simulation simulation, BufferPool pool = null) /// /// Initializes the property collection if the Bodies/Statics-less constructor was used. /// - /// Bodies collection to track. - /// Statics collection to track. + /// Simulation whose bodies and statics will be tracked. public void Initialize(Simulation simulation) { if (this.simulation != null) diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index 0cf3a1a5e..51e0620af 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -58,6 +58,7 @@ public DemoSet() AddOption(); AddOption(); AddOption(); + AddOption(); AddOption(); AddOption(); AddOption(); diff --git a/Demos/Demos/PerBodyGravityDemo.cs b/Demos/Demos/PerBodyGravityDemo.cs new file mode 100644 index 000000000..d9b5d927c --- /dev/null +++ b/Demos/Demos/PerBodyGravityDemo.cs @@ -0,0 +1,162 @@ +using BepuPhysics; +using BepuPhysics.Collidables; +using BepuPhysics.CollisionDetection; +using BepuPhysics.Constraints; +using BepuUtilities; +using DemoContentLoader; +using DemoRenderer; +using DemoRenderer.UI; +using DemoUtilities; +using System; +using System.Collections.Generic; +using System.Numerics; +using System.Runtime.CompilerServices; +using System.Text; + +namespace Demos.Demos +{ + + /// + /// Shows how to use custom velocity integration to implement per-body gravity. + /// + public class PerBodyGravityDemo : Demo + { + struct PerBodyGravityDemoCallbacks : IPoseIntegratorCallbacks + { + /// + /// Maps body handles to per-body gravity values. + /// CollidableProperty stores data aligned with the integer value of a body handle which doesn't change over the lifespan of a body in the simulation. + /// Unlike active set body indices, body handles don't move around when other bodies are removed or slept. + /// There's nothing special about the CollidableProperty; it's just a helper, feel free to use any approach that works. + /// + public CollidableProperty BodyGravities; + /// + /// Used to look up body handles using the callback-provided body indices. + /// + private Bodies bodies; + + public readonly AngularIntegrationMode AngularIntegrationMode => AngularIntegrationMode.Nonconserving; + + public readonly bool AllowSubstepsForUnconstrainedBodies => false; + + public readonly bool IntegrateVelocityForKinematics => false; + + public void Initialize(Simulation simulation) + { + //Bit awkward, but the CollidableProperty wants to know about the Simulation so it can resize things intelligently, + //and the Simulation doesn't exist when the CollidableProperty instance is created... so we let the callbacks initialize it. + BodyGravities.Initialize(simulation); + bodies = simulation.Bodies; + } + + public void PrepareForIntegration(float dt) + { + } + + public void IntegrateVelocity(Vector bodyIndices, Vector3Wide position, QuaternionWide orientation, BodyInertiaWide localInertia, Vector integrationMask, int workerIndex, Vector dt, ref BodyVelocityWide velocity) + { + //Velocity integration runs over bundles of bodies, not just one at a time. + //The reason is that all calling contexts of this function are vectorized and transitioning from AOSOA to the more familiar AOS layout is not free. + //The provided representation is the simplest and fastest one given the way integration works internally. + //Each lane in the SIMD vectors corresponds to a different body. + //In other words, velocity.Linear.Y[2] corresponds to the Y component of the linear velocity of the third body in the bundle. + + //Applications which require per-body information to be gathered dynamically are difficult to vectorize. + //Here, we write each slot in the bundle with a value gathered through a handle lookup in the BodyGravities. + //This is *not* the fastest way to gather data for a vectorized representation, but it is relatively simple. + //If you have a bunch of data that you want to gather per body, you could consider a more complex vectorized transposition, like how Bodies.GatherState works. + + //While this is more expensive than not looking up per-body data, it's a good idea to keep things in perspective: + //On a 3970x running this simulation, the full frame cost difference between this per-body lookup + //(versus something like velocity.Linear.Y += new Vector(-10) * dt) is about 50-100 microseconds. + Span gravityValues = stackalloc float[Vector.Count]; + for (int bundleSlotIndex = 0; bundleSlotIndex < Vector.Count; ++bundleSlotIndex) + { + var bodyIndex = bodyIndices[bundleSlotIndex]; + //Not every slot in the SIMD vector is guaranteed to be filled. + if (bodyIndex >= 0) + { + var bodyHandle = bodies.ActiveSet.IndexToHandle[bodyIndex]; + gravityValues[bundleSlotIndex] = BodyGravities[bodyHandle]; + } + } + //Note that the callback doesn't have to filter writes based on the integration mask, even though not every slot might be active. + //The caller is responsible for only using the active slots. + velocity.Linear.Y += new Vector(gravityValues) * dt; + } + } + + + public unsafe override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(0, 20, 80); + camera.Yaw = 0; + camera.Pitch = 0; + + //The CollidableProperty is a helper that associates body handles to whatever data you'd like to store. You don't have to use it, but it's fairly convenient. + var bodyGravities = new CollidableProperty(BufferPool); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new PerBodyGravityDemoCallbacks() { BodyGravities = bodyGravities }, new SolveDescription(4, 1)); + + Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(new Box(1000, 10, 1000)))); + + //Make bodies with different shapes, and give each shape type its own gravity so that it's visually comprehensible. + var sphereShape = new Sphere(1f); + var sphereInertia = sphereShape.ComputeInertia(1); + var sphereShapeIndex = Simulation.Shapes.Add(sphereShape); + var capsuleShape = new Capsule(1f, 1f); + var capsuleInertia = capsuleShape.ComputeInertia(1); + var capsuleShapeIndex = Simulation.Shapes.Add(capsuleShape); + var boxShape = new Box(1f, 1f, 1f); + var boxInertia = boxShape.ComputeInertia(1); + var boxShapeIndex = Simulation.Shapes.Add(boxShape); + var spacing = new Vector3(4); + const int length = 20; + for (int i = 0; i < length; ++i) + { + for (int j = 0; j < 20; ++j) + { + const int width = 20; + var origin = new Vector3(0, 40, 0) + spacing * new Vector3(length * -0.5f, 0, width * -0.5f); + for (int k = 0; k < width; ++k) + { + BodyInertia inertia; + TypedIndex shapeIndex; + float gravity; + switch ((i + k) % 3) + { + case 0: + inertia = sphereInertia; + shapeIndex = sphereShapeIndex; + gravity = -.1f; + break; + case 1: + inertia = capsuleInertia; + shapeIndex = capsuleShapeIndex; + gravity = -3f; + break; + default: + inertia = boxInertia; + shapeIndex = boxShapeIndex; + gravity = -10f; + break; + } + + var bodyHandle = Simulation.Bodies.Add(BodyDescription.CreateDynamic( + origin + new Vector3(i, j, k) * spacing, new Vector3(0, 0, 0), inertia, shapeIndex, 0.001f)); + bodyGravities.Allocate(bodyHandle) = gravity; + } + } + } + + } + + public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + { + var bottomY = renderer.Surface.Resolution.Y; + renderer.TextBatcher.Write(text.Clear().Append("The user-supplied IPoseIntegratorCallbacks.IntegrateVelocity implementation defines how body velocities change over time."), new Vector2(16, bottomY - 48), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("It's usually used to implement gravity. Here, each body's gravity is defined independently."), new Vector2(16, bottomY - 32), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Spheres fall slowly, boxes quickly, and capsules are in between."), new Vector2(16, bottomY - 16), 16, Vector3.One, font); + base.Render(renderer, camera, input, text, font); + } + } +} From 69494c59c1fee284872741bd428dfdd4ebc94353 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 2 May 2022 19:08:44 -0500 Subject: [PATCH 479/947] Fixed rather incorrect comment. --- DemoRenderer/UI/RenderableImage.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/DemoRenderer/UI/RenderableImage.cs b/DemoRenderer/UI/RenderableImage.cs index fe8dae805..79a7d0aca 100644 --- a/DemoRenderer/UI/RenderableImage.cs +++ b/DemoRenderer/UI/RenderableImage.cs @@ -10,7 +10,7 @@ namespace DemoRenderer.UI { /// - /// Runtime type containing GPU-related information necessary to render a specific font type. + /// Convenience type bundling a GPU resident texture, its view, and a CPU side buffer for uploading to the GPU. /// public class RenderableImage : IDisposable { From 415f71e292d0164aaf2dfac7e8f0c6944ff8d869 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 4 May 2022 18:25:36 -0500 Subject: [PATCH 480/947] Wording. --- Demos/Demos/PerBodyGravityDemo.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Demos/Demos/PerBodyGravityDemo.cs b/Demos/Demos/PerBodyGravityDemo.cs index d9b5d927c..02b857ce6 100644 --- a/Demos/Demos/PerBodyGravityDemo.cs +++ b/Demos/Demos/PerBodyGravityDemo.cs @@ -154,7 +154,7 @@ public override void Render(Renderer renderer, Camera camera, Input input, TextB { var bottomY = renderer.Surface.Resolution.Y; renderer.TextBatcher.Write(text.Clear().Append("The user-supplied IPoseIntegratorCallbacks.IntegrateVelocity implementation defines how body velocities change over time."), new Vector2(16, bottomY - 48), 16, Vector3.One, font); - renderer.TextBatcher.Write(text.Clear().Append("It's usually used to implement gravity. Here, each body's gravity is defined independently."), new Vector2(16, bottomY - 32), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("It's commonly used for gravity. Here, each body's gravity is defined independently."), new Vector2(16, bottomY - 32), 16, Vector3.One, font); renderer.TextBatcher.Write(text.Clear().Append("Spheres fall slowly, boxes quickly, and capsules are in between."), new Vector2(16, bottomY - 16), 16, Vector3.One, font); base.Render(renderer, camera, input, text, font); } From 6a7a65f73a64c8e19c76f210aad366125c005553 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 4 May 2022 20:16:24 -0500 Subject: [PATCH 481/947] ICollisionCallbacks.AllowCollisionTesting returning false no longer triggers memory leaks when mesh reductions are used. --- .../CollisionDetection/CollisionBatcher.cs | 36 +++++++++++++++ .../CollisionBatcherContinuations.cs | 44 ++++++++++++++++++- .../CompoundPairCollisionTask.cs | 2 +- .../ConvexCompoundCollisionTask.cs | 2 +- .../CompoundMeshReduction.cs | 4 +- .../CollisionDetection/MeshReduction.cs | 4 +- .../CollisionDetection/NonconvexReduction.cs | 2 +- Demos/Demos/CollisionQueryDemo.cs | 4 +- 8 files changed, 88 insertions(+), 10 deletions(-) diff --git a/BepuPhysics/CollisionDetection/CollisionBatcher.cs b/BepuPhysics/CollisionDetection/CollisionBatcher.cs index 87d16dbdf..c9f74c70b 100644 --- a/BepuPhysics/CollisionDetection/CollisionBatcher.cs +++ b/BepuPhysics/CollisionDetection/CollisionBatcher.cs @@ -285,6 +285,9 @@ public unsafe void Add(TShapeA shapeA, TShapeB shapeB, in Vect offsetB, orientationA, orientationB, speculativeMargin, pairId); } + /// + /// Forces any remaining partial batches to execute and disposes the batcher. + /// [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Flush() { @@ -313,6 +316,12 @@ public void Flush() CompoundMeshReductions.Dispose(Pool); } + /// + /// Reports the result of a convex collision test to the callbacks and, if necessary, to any continuations for postprocessing. + /// + /// Unless you're building custom compound collision pairs or adding new contact processing continuations, you can safely ignore this. + /// Contacts detected for the pair. + /// Continuation describing the pair and what to do with it. public unsafe void ProcessConvexResult(ref ConvexContactManifold manifold, ref PairContinuation continuation) { #if DEBUG @@ -352,5 +361,32 @@ public unsafe void ProcessConvexResult(ref ConvexContactManifold manifold, ref P } } + + /// + /// Submits a subpair whose testing was blocked by user callback as complete to any relevant continuations. + /// + /// Unless you're building custom compound collision pairs or adding new contact processing continuations, you can safely ignore this. + /// Continuation describing the pair and what to do with it. + public unsafe void ProcessUntestedConvexResult(ref PairContinuation continuation) + { + switch (continuation.Type) + { + case CollisionContinuationType.NonconvexReduction: + { + NonconvexReductions.ContributeUntestedChildToContinuation(ref continuation, ref this); + } + break; + case CollisionContinuationType.MeshReduction: + { + MeshReductions.ContributeUntestedChildToContinuation(ref continuation, ref this); + } + break; + case CollisionContinuationType.CompoundMeshReduction: + { + CompoundMeshReductions.ContributeUntestedChildToContinuation(ref continuation, ref this); + } + break; + } + } } } diff --git a/BepuPhysics/CollisionDetection/CollisionBatcherContinuations.cs b/BepuPhysics/CollisionDetection/CollisionBatcherContinuations.cs index f7710566a..5153e9b38 100644 --- a/BepuPhysics/CollisionDetection/CollisionBatcherContinuations.cs +++ b/BepuPhysics/CollisionDetection/CollisionBatcherContinuations.cs @@ -7,14 +7,43 @@ namespace BepuPhysics.CollisionDetection { + /// + /// Defines a type which includes information necessary to apply some form of post processing to a collision test result. + /// public interface ICollisionTestContinuation { + /// + /// Creates a collision test continuation with the given number of slots for subpairs. + /// + /// Number of subpair slots to include in the continuation. + /// Pool to take resources from. void Create(int slots, BufferPool pool); + /// + /// Handles what to do next when the child pair has finished execution and the resulting manifold is available. + /// + /// Type of the callbacks used in the batcher. + /// Continuation instance being considered. + /// Contact manifold for the child pair. + /// Collision batcher processing the pair. unsafe void OnChildCompleted(ref PairContinuation report, ref ConvexContactManifold manifold, ref CollisionBatcher batcher) where TCallbacks : struct, ICollisionCallbacks; - unsafe void OnChildCompletedEmpty(ref PairContinuation report, ref CollisionBatcher batcher) + /// + /// Handles what to do next when the child pair was rejected for testing, and no manifold exists. + /// + /// Type of the callbacks used in the batcher. + /// Continuation instance being considered. + /// Collision batcher processing the pair. + unsafe void OnUntestedChildCompleted(ref PairContinuation report, ref CollisionBatcher batcher) where TCallbacks : struct, ICollisionCallbacks; + + /// + /// Checks if the parent pair is complete and should be flushed. + /// + /// Type of the callbacks used in the batcher. + /// Id of the pair to attempt to flush. + /// Collision batcher processing the pair. + /// True if the pair was done and got flushed, false otherwise. unsafe bool TryFlush(int pairId, ref CollisionBatcher batcher) where TCallbacks : struct, ICollisionCallbacks; @@ -122,6 +151,19 @@ public unsafe void ContributeChildToContinuation(ref PairContinuatio } } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public unsafe void ContributeUntestedChildToContinuation(ref PairContinuation continuation, ref CollisionBatcher batcher) + where TCallbacks : struct, ICollisionCallbacks + { + ref var slot = ref Continuations[continuation.Index]; + slot.OnUntestedChildCompleted(ref continuation, ref batcher); + if (slot.TryFlush(continuation.PairId, ref batcher)) + { + //The entire continuation has completed; free the slot. + IdPool.Return(continuation.Index, batcher.Pool); + } + } + internal void Dispose(BufferPool pool) { diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairCollisionTask.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairCollisionTask.cs index df41dd863..6111052ac 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairCollisionTask.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairCollisionTask.cs @@ -116,7 +116,7 @@ public unsafe override void ExecuteBatch(ref UntypedList batch, ref } else { - continuation.OnChildCompletedEmpty(ref subpairContinuation, ref batcher); + batcher.ProcessUntestedConvexResult(ref subpairContinuation); } } } diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundCollisionTask.cs b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundCollisionTask.cs index 6bbed0a5b..15471abc7 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundCollisionTask.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundCollisionTask.cs @@ -96,7 +96,7 @@ public unsafe override void ExecuteBatch(ref UntypedList batch, ref } else { - continuation.OnChildCompletedEmpty(ref subpairContinuation, ref batcher); + batcher.ProcessUntestedConvexResult(ref subpairContinuation); } } diff --git a/BepuPhysics/CollisionDetection/CompoundMeshReduction.cs b/BepuPhysics/CollisionDetection/CompoundMeshReduction.cs index e13d5f53e..cc3769c85 100644 --- a/BepuPhysics/CollisionDetection/CompoundMeshReduction.cs +++ b/BepuPhysics/CollisionDetection/CompoundMeshReduction.cs @@ -40,9 +40,9 @@ public unsafe void OnChildCompleted(ref PairContinuation report, ref } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void OnChildCompletedEmpty(ref PairContinuation report, ref CollisionBatcher batcher) where TCallbacks : struct, ICollisionCallbacks + public void OnUntestedChildCompleted(ref PairContinuation report, ref CollisionBatcher batcher) where TCallbacks : struct, ICollisionCallbacks { - Inner.OnChildCompletedEmpty(ref report, ref batcher); + Inner.OnUntestedChildCompleted(ref report, ref batcher); } public unsafe bool TryFlush(int pairId, ref CollisionBatcher batcher) where TCallbacks : struct, ICollisionCallbacks diff --git a/BepuPhysics/CollisionDetection/MeshReduction.cs b/BepuPhysics/CollisionDetection/MeshReduction.cs index d6bd584ec..ee153e604 100644 --- a/BepuPhysics/CollisionDetection/MeshReduction.cs +++ b/BepuPhysics/CollisionDetection/MeshReduction.cs @@ -47,9 +47,9 @@ public unsafe void OnChildCompleted(ref PairContinuation report, ref } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void OnChildCompletedEmpty(ref PairContinuation report, ref CollisionBatcher batcher) where TCallbacks : struct, ICollisionCallbacks + public void OnUntestedChildCompleted(ref PairContinuation report, ref CollisionBatcher batcher) where TCallbacks : struct, ICollisionCallbacks { - Inner.OnChildCompletedEmpty(ref report, ref batcher); + Inner.OnUntestedChildCompleted(ref report, ref batcher); } [MethodImpl(MethodImplOptions.AggressiveInlining)] diff --git a/BepuPhysics/CollisionDetection/NonconvexReduction.cs b/BepuPhysics/CollisionDetection/NonconvexReduction.cs index 4a298e257..622729a9a 100644 --- a/BepuPhysics/CollisionDetection/NonconvexReduction.cs +++ b/BepuPhysics/CollisionDetection/NonconvexReduction.cs @@ -332,7 +332,7 @@ public unsafe void OnChildCompleted(ref PairContinuation report, ref } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void OnChildCompletedEmpty(ref PairContinuation report, ref CollisionBatcher batcher) where TCallbacks : struct, ICollisionCallbacks + public void OnUntestedChildCompleted(ref PairContinuation report, ref CollisionBatcher batcher) where TCallbacks : struct, ICollisionCallbacks { Children[report.ChildIndex].Manifold.Count = 0; ++CompletedChildCount; diff --git a/Demos/Demos/CollisionQueryDemo.cs b/Demos/Demos/CollisionQueryDemo.cs index 906de38dd..d7a6e72dd 100644 --- a/Demos/Demos/CollisionQueryDemo.cs +++ b/Demos/Demos/CollisionQueryDemo.cs @@ -225,9 +225,9 @@ public unsafe override void Render(Renderer renderer, Camera camera, Input input var queries = new QuickList(widthInQueries * widthInQueries, BufferPool); var querySpacing = new Vector3(3, 0, 3); var basePosition = new Vector3(0, 2, 0) - new Vector3(widthInQueries - 1) * querySpacing * 0.5f; - for (int i = 0; i < 5; ++i) + for (int i = 0; i < widthInQueries; ++i) { - for (int j = 0; j < 5; ++j) + for (int j = 0; j < widthInQueries; ++j) { queries.Allocate(BufferPool) = new Query { Box = new Box(1, 1, 1), Pose = basePosition + querySpacing * new Vector3(i, 0, j) }; } From a3d6a296b33f725436a8a8ad3bb8807e8c8ffb6c Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 4 May 2022 20:16:46 -0500 Subject: [PATCH 482/947] Version number bumped. --- BepuPhysics/BepuPhysics.csproj | 2 +- BepuUtilities/BepuUtilities.csproj | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/BepuPhysics/BepuPhysics.csproj b/BepuPhysics/BepuPhysics.csproj index d910ec5f9..d0ba4af93 100644 --- a/BepuPhysics/BepuPhysics.csproj +++ b/BepuPhysics/BepuPhysics.csproj @@ -1,7 +1,7 @@  net6.0 - 2.5.0-beta.1 + 2.5.0-beta.2 Bepu Entertainment LLC Ross Nordby Speedy real time physics simulation library. diff --git a/BepuUtilities/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj index 72c0b8888..a010e4c9f 100644 --- a/BepuUtilities/BepuUtilities.csproj +++ b/BepuUtilities/BepuUtilities.csproj @@ -3,7 +3,7 @@ BepuUtilities BepuUtilities net6.0 - 2.5.0-beta.1 + 2.5.0-beta.2 Bepu Entertainment LLC Ross Nordby Supporting utilities library for BEPUphysics v2. From 435664e1231bcd4a9158ce29b9216134d9575873 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 16 May 2022 14:22:58 -0500 Subject: [PATCH 483/947] Sponsor update! --- Demos/Content/Sponsors/bedtimeforopossum.png | Bin 0 -> 71736 bytes Demos/Demos.content | 3 ++- Demos/Demos/Sponsors/SponsorDemo.cs | 1 + 3 files changed, 3 insertions(+), 1 deletion(-) create mode 100644 Demos/Content/Sponsors/bedtimeforopossum.png diff --git a/Demos/Content/Sponsors/bedtimeforopossum.png b/Demos/Content/Sponsors/bedtimeforopossum.png new file mode 100644 index 0000000000000000000000000000000000000000..a3603b1f237dc50b7d0e0362575c77c7b46bd749 GIT binary patch literal 71736 zcmZ^L1yqz>_dN^)3_U}GfJ1kS(w)-XEsB7oNJw`|NlSyIgrI~V9ipTlib$8BG}8FL z_uO;#*?S+NwKS9naH(<8(9j4}l;w5M&@e*L&>)mBEbyHv z8IfT$G<27{a&lUCZEexe*pfVx#8fe4NW-OyWX6=C-11yH@^xBp%(}<0=`sc4PQ!X* zFB1AI>0bzloiMN9b;>sE>K_v|kU5b-Wf1aJ=jgAglMXculoN&De!CDBTodrXlGXM3 z;(i6IlfzrXT>z1tZ5bu>8vZo?0fW5ImlvhFBzG@q^Sma?eZ}&9FWPHX?TxRS(Qagg zDc%3P8`d9I`a#@uH_bIN_kCJ8QTQ3HNd8XhxFEU|=P=fJ^+y%fe)?kHzUTT+%55ap zjJo{0+&C>K0!NCRiTQEs*~`rZUb(3VsDw~b#Z)Es+e$mg2vmN*!gY4rh%A98Fq`<1 z@bl%K&%6%WEz~;dV+1FAkM*_!_wNra6y!f-LSJB=3)~FuqdlJeaZ$rK_fspv@rtVS5@|z=E8Gf!`P-C4bSy>1#e~T_~Jg>9}U15ij^^F-+VG7 z=WCw3PW|IYf#DCO^Z>LLxjP$o&ff5X9l&u_HugY6BeF*QLobsmy^Ds1L{pKM(et+W zmV+~+Hu7NrE|y;a-|v%=6opvK(hR(FUoqkL?SRWtEwpm-HGiCFhi}oe^s@ zJY-u9lw6x@ZWr5sCtm+W%ws!eH6D4 zJ?}CFCq<{K%{i;vFMSX9o7|S<`ybIN%(nXUMi5+)A1hRl7?I`O8AxJeFn#H?v2y)W zqf4Imd@y!TS2$kkhTYpVvh3^ka9iR=B+u01=p;d{cM>v$9Mg{1@&e;Ts#`WVv?~p``TdSv4_}Q){WP2LKl;MI z*+HTb1Xcd^s$xtWYbg*W#z_pif^)vpO?2~ncO(&~Eua3akG0kemaqL70x!>PzO;B& zoBSxv3*g`|sIsRF(W1j@GyYIx$;;Dz?w%xgM`Nr^o00$F4<{)(KDh||LhJ5`LFH^= zx59xZA|6Iqe2^FPQK2I_Vx<;h55B=`+Amu&uHVbDhFN$ogcr($VmZ{)KkiTtcTQnZ z#}&IlghVI)=T)04&9WJ}9K|FJ(O}*A@!91H(brcGz6(rn8$U08uiq%&ezx6iNhO^~ z!m1I4j)BFNkap!(X9#AY+frXd1tuOjDR1je$ohCmY~HEh?RW53UfVy;Xj|Kb946r3 zKQ|iYT0f~Q(J8rUbN1uA4B}m$lqHw(^PXeVr)rsxZyc_V6)hfoeQ)zNjk|Bo3i{IN zlMX(WfV`RS*OHAR-KT0eqFc5J5>Hpnn%zs9j%FWz3Ca^qM#BLSkQh`78>Kv$SXs7F zMqUdBKR^}O)9ugqCS&ZZ6Otfx*<=lKz?!yq>pr`)J=+$MHCbtBI4kUZ1Ea)os@i62 zrg?A)uc$^YlCV&>RD(XDz`e>o{lc*{jxEfw>x}$n;#u?Up{$Ix=+z$ZZ6%K;(GN}& zX}p$_8~Z+xUk?kOSNa@kkKZ?v$e0*ZiF1CsSrc)o5JT#AXwt?0bGQF$Wvy0y>%q9{ zf=2A=_Rjq;uH6I>pNE}L>!j`Wp8*p~8zfkSG?e?zZ-mFr={UjCQj?t?d`-cBA`HX73MC zFLBF~sWhHy=2ds;V&Juhi6sQG{)tE=dL?RAW0?Bs(TSC*(e>(|i2FP@dhORNe1APr z*k`Z!_~K|z|B(e>^Tm(P_xPdw6w`VcXWvS&F%>L$nwN=#=@Ko-Jp%HlGUeE#(|$3e0raF1H^P~s{EJ3S2M`T4+~7ltqBg#*r;aJ%IbdR{nAoxZpZD^V=h zDUo033fDKqyyn=b8gO~CRu&O~*z~m3!jO4?`t4J_Qg0-7%AY6ys1($f@9!icfYTI$ zCT{m3gL}rO_7BG#=xSu&}+@0zpyd zwxlT6yVx5W%b{Np<@D(}d(n2wX0@*R@NQt~K-&fN#4C=hQfpJiJMXfqjH50kG8&A*pb#{f|M#Zi}RyASX}cv6_-I(hChkhtv?GA zT!vFSVl^?~4NgOlIh35oZ2b<*CfE5(3wseeh~>eupxqepej&#XWfuN|cZM5Ob29qF zhnZE=)I218MY>*{yb~OW0@2zl{yJM@=C%K;0-iW;+&@Pv$OASixcBK3Y@Wo=cr;JrRaXZ8bIjPlhOqlCG&>Q)m|VvcD45A` zefQO~uBm(ta>$o>_(QE|WTx38eonv2NZaXSZ6|LjUpXF3K%`579VF@BuX#PlOyPr# z)B+2pL>nkbrZ&sG*YA$^BsB-IKTv5h`?GTB2_zttcJbb=Bn6qm@-th>Nn=$sU;n&( zxIz~8m`%HjTeUd%KR%WXat({tQ=|+Zrs83B1mrIv&@WS1<>2l@Q+o?t-Ew}|;E%fPmR zx0N-oIV=J%Jhh*h2)PXjO+`7@v>6ZQ`L%6!ExS@^x?J ztv&*&Tyis2KAu|0>^Y=D<@XBAGDzEwnogxu(g&BpzRfhh7nU^Q3@gKm-JgF%D;C3T z`ZCJU!bc(g{^hyP?)S#I3PZNLlNI_;>i6!y!pX0wApc_0^$oKcr1vizu6}=(D;N%? zWEpJ4!YSqp23tW{*}m1XnXF+mkVtRyb-V-;VGT0E`^i%E?CTXc61|_G@V!b*I*b2L zBt5z+<@_%C>6QjOEMgH;Cgp)QZ0yJ4%g@fb?V#Xg+mGf4+fP;JkW7>i{>K)WK!&|f zJ-8(~G$32_sEQmLicNINUezsMf9nmyU>f&kQ*Vb@lh?L|+r|XDcKvO2x%R68$1*}$ zkp)fuXAgG4QkSzF1c!K%v1v&Q`6~r1*rl64-6KR0XR~V3b* z8<(J_8mWUEHYqPcha>#Bqfj9he`vbVr6%A57HLequ(rcw1#@5gmD0G^{Z|9U9msxO z99b3+%pPt|DcuiJl91b)3#o<{Uk5N}-1=!NFwr?;7-&c#yw;%j zPqwFW8X72?eyB1gxZnkNip&I8#Q(J6v{A2Ir$UDvN~ai>Q<$*O6HPL?y|*^P>)i5{ z<#7MC|5;C+ZNHu}dtB$))+^06qpH3n7X*jGpF;@821nVgW2u~Jo&_>ZIWFn~j;q#O zc^{Hg{?ejIF&^JM5G0e`)aBAFU5aRJ32h$7eev8qp&^?GXpM&jA z#1&_F{hbjF50NC7W3a+yxsiN|QYXtKH_Y8PZvMHXG~_FBl=y=fB*2$us#bf;Zx#V& z)HzLO)0Pd_b=al~Fa$tms_L0qra%R{1azS~Sm4eu4Zr;;vd;O7=-3NK!=({$I7l5o zRB1;bp#{Y&gQ>bwbKXnwGE+ne`Dw=Wx1#}eZiyfvWByx_gIz&Y$5)C1MVbgcIxE0c z;<@?3K>=XB>jqEb85+6BEY!n-y53W;@sYl$`&b}{&F?V5<~a~U#;GS+V0_(qrmRa= zZbgmZT7%!o9%Kp0b+|twWjHHepR`VXdm`oEssh0o6&yH%;uNz5TLxa}KcnF$?-KI% zVkoqVu*%O(<0LyoQ!<~Q$&Wx_NK2aF|ge}sw zcVU2Prq!pOS+QpI03iG(o9`OhdjM2j*L#!17^yo^NcZ>fCZom6ye3daV#2D-n#WyI zR}QB0t_SM>~35yUf+h&_p;8OGDY%{w#+qgtfJbvL!%t${2>ep~KM zMP1Hy!iqWv9E#-4YhTKVe%B4}9Pr^ame$n?P$qOel1caNwjJ+Vdv2)y4)5=?3UVe&<4QLvd zu?@1!w6Xfscx}vXB-i|q<{-JsqRm7tgHMJVNBwIgA#FvRFwB<}>v}nnnV5JCfCHrI z<_|JOFF%;NkXd~!Rw3c{+L|fTY6GVk)pRX_gt03K)oxgeX3d53U2o%=h99{*S4`E= zPbvMng?V$x0lx3iw7VTsVg#BjlD;J<&becYS#RJ_E`zPu|Ln(LMt7yh+AzXU!@Sli z7?hrBzkVlb3M4hCtlW_Mq`qM^1QR!EsXu{}m)W93HQm6v zXlMGm{b+}3de@HfLkDd${69M$1$LasT#Y9j%0Ry}bYm_GbSmX?AQ#fTVXqds4N6hO z5sz(OT!2M~y@|c}#ld@2q3Ua~RvXR|dYgbD>w--{&4g+q328-(FA11OF|jzrpHFjE zCVT-HXq;-c8^D|xa&D8TG;Y%vY9WWFh=*(nojq&&gEcF^j4x~D-93u&^J?I^_N{?L8M6A$U3RUn9rt9ZZ#F}{!C%j^u4_TuuDlDkMXs>qrJDhXSbAZ4;;Rc z43qrYw*O{S%@X9nbKy7FVXr$wvCneOW;{ktXfdjvUoFryxP7S7&9^Qt;5T8|9^gNB z#ie!O=!i=2Hl^*C`|s8J=6-C?ZKupCT7vFo{2jBLy1&5vi6hWIimhg;*nTjqGU6}U zTuHZdUay+sjK96)b!;4a?Zf1DNx^1{z6otT9f$1ivstc)c0xl6ig5yQq6O=cH4PS) z{7V(B_7v-wT;mtt=F->Uq94?}1f%%leS(^g`WtZ=BM6DEr3I_b;!egf)C| z{VCr&>Al_2oCA@#xX3D}7te2vh82VFrZ3tJyEYP^XZH}EchJ)-)K13w7XIm4i++v8 z7fR75{A_p|Mg>^?e~FB!B(!dM-9e&vy-x+u!^9aH5cK|upaA(ol-hg`<5qVD-s641 z-sz>V)bk03+4kW1q}X3N53_@mQ|t|JM{mR3_wq>$3Zz%Qo{)g5K?We2pb@BZ>N%Ex z5NSG=xIA9!J1J61&=52LNMFB+z1px;BL_dnbsm(3Did9#l%_A=(l}#oN=d26(AO`o zylw=PKE&@6#2)#RMu}?^z?nY#=zfdXl?I(uY>c7%jHcAG0k;z=aC9l>sY!Ydhm8iip3XzTY0B9BU^JJFk9fIp%ONcg%XYgfohc+#1y@+fqT5^$4 zP!DnwXK$u@fLeZox}RQZJM&I10+e?WkTG7$J}c{!#v`a#-mVwOf06$sg8V%G&sH+c za{<_&<0(-_fb;QY#|#}hR#Nr~z!k*Z5d?y1L4`bgtE1h#wIY6CAs%BB%xZaKXDw6tu`*X=jW$2cUUl`qEe9{qQ*5>=dB=5w`8#fQo zC}XHRd}sKHYq!ylBU-Ts$MKj&6I=q=V(~xU&NOA*R}6m(T2>sr4H~I{R(>q}AwW*`T)WN1`sQ4!J&izA{2}U?&Y`XycN5nVuTUczH{^x-$1oftfp(V-?>>ZG6%SetCLN2H26gMTj{WF(hyh8 z29IM<`zL-Top~+x;H*f5X-BSZCC2i;HhLi=J;3#0tVoGyh5xa>D1knF{CHSwI||f| z({Q`KLPy`V4%c|4`7h>x9{EL!%*hRaT&~9%BOmo6ccb~;?9Fq+ZNz^xC^+Xe<*6Q? z#kWBY<%dfb-RFjly#ho^{31Vvg}1dlREuquilq}n z=Z6F%BfIRyJU1Q(m6C`GInNjkyhO;p%z}H(`jqCC61Aa8oqZETXT(1V9B33=dr`pO zQl(ab->D3}K)1_=rJj|_sWXa{@@4*A{6ex|l^p9S_gV0jETv=mJqYJ{J$@e6mu4?*>fY#&= zWnyrDH(sQ;xQCGHr+Q6%8sX9FJg`u|jq=jC9j!3tmo9U&E|OtdTeEGZPBTqoJ6MZo zVL=R4WJ8}p&2UE1t4re7f&u|?Gnr+r6+yC7Ze0H~ixEUCZG6&=q)K3yY-A% zA5vZm@gW*mrhIHDU{Kk%GmQo82AOWvNTeD@LYOW6b``Srt?>bIq`EwkH>Yzk6xwO@ zzMvYw3DW1y&8LEK^xo_64Vs%V{f5%aeA1lq`pR3m_QSkfTG2HW%D^ zB!YBB&vU{)Tj3hV2FFLs-c{OoxSJhK;V2gLB~_^C4c!sEqU?Wj91qBGUp(8&xN&;a z(5!Pz-bu{CC8<6#peYz=!Ld^1TwZ;DdCh}?LVc~MQ#WeXop0j4{%}Srz6Zk|`VFU# z@Nu-Bt8fVDSxrkws9$wwm$rEwi;0&Uw~0Ipns@EDP6{fYYXLOo`3P}eQCl54&^$(e2H#YnhpLymXM>~Fn!EGzEJ2^ zN~EjqJ=n%l!=ujDQnp0M5~84DM^=$z^*Z^0M4bLSy8PG`%D3>avsw4I+zuC4{dR%{ z{GJic5jb%s0f=ohx#sh?0a5|144=xp2saK>7Ft%S+|Foqn=qi^^u8nKWl-Hc=H=CB zEz46kE-RFm?)Jq1E-{!3V}y|aH%8Zlp%7W#gQ zrX)YU%o8HUF`DFU32>W2EnFQ3xfI10strXsTQCb-TK8LMeJJ z;lwd~8r-m>ur-oC(rM*5DnXVZ${VxZwpnwjJm#Y7W~;IsKhO6Qq=zklXmv4dS=`V3 z7(Ld*>R4vPXMdGlj-uD&-NNh6J_R~E(5vawF;$awSxTMn$f3la$$)Oc=J^o3$H5x^ z)*%X19cr=vf3UVU4Nu7Mya>aW5fps zn1y;w*npm|jg}vJoDPVFqbc3MtuL!vw3}GagRiljMTTI-iP)l@);?w?4 z+vB#bP07Z`Vr`#^sbCL}5w+=oX*bwwkLAmR*1ioNE8V|5=v*(2!matLVs!=o9$S~a zyywK7Jb#0Aaz+^Qi?@{cucYbzB`}pyOrwu+87a<3RkV`_eAnOt)OQb6y|YKXwD4qk z$DS4dhB2TG>-*$P;Qeb4`#B|owIe@}@cCN1b}El6*pG`5>FcI*85a`Dc$`SfSW4B? z!;0```L8|iH`^fwE$sU;Z)#o&%77jFLSioQI=G&y!s|lvh7@VK!r>End#-9k+z=qI z@JI)7zP+Wau_zc;lH@RPkp`IY_SCh%wMQ9A2<~(#s9#s5;~8|6gfIfv{SEvBrN%tF z;KPf=ya>1&WrzlOSY#o+xGXb$87XcN8*wnjH3^v}ZhTJCc`81uAZ<^wR5k1SogUF} zJ#Wiy9PuboKf|kdMPvol>)YO+?9vr(H88S{KcR@OgY!&R84C(=b*Yos;XBW^elmLb z$%>#qjThD>}gdW$wMB|^I3O6$tqxP9dtu1L1N)n#x02 zLX6am#@Mvt-df*2H)>MJYKrfMImjTYjQ#Qh%0}-JGKM3*-tIhI_;j&el%SXK4Pfra z)=nk)44i+Jg${o}!?bK?Y1ye@wwal5|RDfnr&^&|VF25CjtNy_3T!OVg)@0$8$#`jnw? z(&4%;N>rJMFIlJQyHyqM@mo_w!|BnO4@||jxf*&0d`mb(+KR`dxT5P-8KyqF)X#Gd z{Sv_^9dLQ=^jBLgs^WVCj1_^_0?_!CF{cY4WpcO>`TrPYg)2S_xZ5^*6XGbhK<15U zOYI>FdxeJtF0C<$q% z&-WKodX&6>Hvzc|2tPG(k6~pdAedZ4-;< zkNQAt0y(2E=xX` zRi<2S2h^CZwS63qWBNVJ1%-FnH){6IrEd>Eq0-JM|B{Yj(XJ_Vp@)+7!^0TDNKafq zH$$>sk^zAFD}CkTV=@jsE&3Y!o9#BexRXD5jjRjo$>fZxlP`RVVcSmivOHaDeuFO165cliUHcX3D9SA(7jF1|stgCpk!#%SA~+n6j{` zXQr$`tg?Ud67-R-avv{`22(jX+pJpC*mV^Tw!WB(gK{9VreM!mkyJQkS^GrzbLtE4 z5CCN>Et2?P*3hD!Q3QQ2u zfZ9LFSq=MvQAQVIfH#0AZ*LEJqsRQAqblohi3)lo;n)X&Ig!1gTBBE+M*JFeBr6 z-Lgyj#qD(n20w6Fa2z2nVAj%&fK+-$kt;_jvg5Sq0`oBgbouyH5wQuYKK;nP#G zpv|Le=pGA%l>B^HM6{x$+BF?kR#vbsD|tsOQnO%e{>L5rSl#G~g)z)v2PzvJ&QRT$FzJ2+-wC{KtQd3c)=X`UYfNJxGIlnA|a$J>6MKsIXm{A6EW z42`>m=*fv!XDzanP<`g;+ovM;E@Cpf>g=>4e}(+~mb-j1pE107>Dg?ED|SWPF~)95 zQjdACI9!aalYg%ZrhmP<58NfPD>xAy7_mPY-%*AnnkYvTGs}bWKDoVMtuXwrBHclY zsvAq9+N#7+8EA1rFx>F`TGf1%9|uz7jj7)I9*_SVFk6dR5W1)0K6_>768D07gz|e? z8E?4*uE$#GOPy^bVwL*GJd_U2matDi)*IstjYW$<<4-AMN|XmsAO)7En-Y|blb?ZW zQP5*{t8Zu%zeFr%SpIpR@}pgbJS#_svBK~Qnw$TkG%QfDhnol|LVzA715LDibLsWL z0#2HiDe3P!Dx>`*@^6=t=nbwwGV(+)S-K0Bk7yDZdAkw)9*-pcb~S*}Q2etjHNyDRCVuI$=)oU|%Lt>NGy?k%o?d-}u5PM^2?$HkpVZoNe|qRY z98&EQKn1={a+m(^H(VuA;nJ=+@jrLW#hUnL4J^{ zBWt>-5BFAwI31>{xdFBy9ScGUtDw+DtsBwF|7LEP=A9r^emYN{F}y%+e(*<}c~0k@ z`A!?qb2ucTh;E=>3H(6!d0M7lnZ{<;;1i0!HtIS_rU&aO9yWROS-@}bU!y1c? zpA_czlbXIh%@$W=se5~}QCZWa0eneXbpVp)_tT2FGkrT*AS?sg$Q=EE9gU=4(?B|} zWs~l|rm^6kpv7D^4F<`M`0fIiFz!wO9n2Rhd~I2)LNdns&)FYj48e2hckNM6srsB2Nf(MgcAN z>cDu3s{ZiRxK6E*iqw<+HN(I@Ilpusb9s4=hxN>zclWp#UKzUUN7e1<%2i zhjXv}QM#e^+WT$p{a`k~eW040QpIEn-XSOBG{6_51BOLTSpLRtGzlw8JXBz^{uJy{ zsQiASTo-fpElP$URz^|lucg4*CYD8;l(asl_u2+;Td&Cnr{7HFC{G-5Ul|OS$i+^S zMxr;6@*5_HEAi+0DNsf=d#sUbN}d#-rprPMh>wBc5%c2Oy z+=M~032H<^zaNJy8_LK6Xl;r_z`6JC*V4RGAG-%zGvy}YVaY)2ivhGiG^$6S4VmF3 z_fc6dQH_4HLpRgnS!uGl{3fLr^u^={{wLs4rEldLdx9EkfiPScaB<3@nJdoTnHbv; z*@2RXQG%=;3@e2}Ax2)Dzje{X3cri37*&6zQC6LkSyTdzMZ@m^MuiI35rB6Lo=62s ze)EpMa)X?0HJIV*&iNOh?plv5pfe@1L3m#u7oudUx%+%{?^8gKSHveiRnKO*sXn}f z4zW)N9m4j`#=uyk~|epI(FJO?4QPf^0#Ie4bK2HEVoEK6Pkhp#p&DxtWqkq7nql?HUh-ON6jXnQ85YMBfG z*w}LI=LKG>)ZaR&bpqB}RCOB2m3Wn&sf7kL-Nf`cqV+Mdm1 zab0|)LccNwXtqYv4A@sZz&6u?N)#0aYz@fAi5O%mMUMV*O5Kh?7Rl@YD|lDj+YeBs z4o6fX+AS4pfQs#^NZ5$>nsJMCn7+rerBk6-Uc0u3MyyN*IW{}op0iy4!OoUTdJ|PD zc0%UsCqZUE6pT}hxqZm< zt_==oA!E=Cw_bG59e0q|5IJ$KGBbv?=hZkHOSrV%2ykLn)a|dlEoBhc zF?TGP7s1L^8~_N8?(nERwZ)YS9C}neKAsbj{n*EtAWm#Z#DO0^Ymb42Uwi)c%ZUrK zLxvgzAbN_AZV~-N%>lso;ArHjRfg6_SM0sJSO~I0p<+BW%1aGNMW8tQ55&@pyC~IO zMmy>@u#Z<9kH1M_mD330(+NY^=wgkR>$2*bCof7ufE`WeyBcx_T9^qeEi2rXQO#NF zazqlvd-}h~e|QWyAu6{)ltU&P-xU-jk+&N)I_GE`Ys)DTP6Dx09)#Y>Fk^5N4zh=Y zOl%}qqOvb!A#yO{qq!s?GEZr7jlzgYpLbwH^4JI0YEb$UP+x-^gDh?;MF=uH{3^QN zQ^5A>>mMTg;f+q8lDC46XKWa;E##b}Wp}9BL7`jH4ZHJM5iLn+p!|bRi217rWPlE6 zu#>FkACL@viCoQRnrDZwyWlVO#nuvBV8+kh0o=46y=c-PaMJ7f*n6!%zq{+ zmXsl3pE1b66?WLktX~5>TzE+ecepIBFrVsY4$mtu!&stD|n=0nl0FfK}MsJZu4( zbcJZNZh*6$?|+J8UdFMivqb-Kq6j9zYyAI!SkAS9Oamlt_AQnYE0%FAvb) z-%SHHj6$eD|T z^PgW;FlPfnR<45f-3qvYxC4G(cwt(dAMaY91=)0ygy-+%ge>0KVJggG?zMohErtw9 zp;X`aMV<%mP`X3#8XOigO%Kid)h>KRDBrcMz5D*@B4Ee0{op#083CNP0I|&BC71qv zd#1U*<*u$y3R+k2tj`*4s%eYMoaE91Hzk2ydMGGCN>mT+XCEc+|EirZCFvWll_qs4 zBU|{?%M4WP9ns}5+la=Xw8`K!xW3=n)`kOuD>8{k=lLy_m9ZDP@BBGI=J8oUJVg~F zRAz#@7RhUbrvYiQ;s@LS$G16ipAhjVhIr}m1Tsix59QO^!~J>iUQ>p`9M}wsHn_T) z6K#Dj4yQ`7kk!cmMbbmHIuBkr*?0u`;GNb1W(ed; z%S?Uh)B^x?7lAO&AJIcdyISI%&;izB@*!;2N)Li+4?}o}duewP!y%9|D^(Oq43)ez!&W;LE3QN};5eFGn{WCo59-m~8e1^)c@TUeM2 zGA?w{3{rN9g|UM&3=+&@U`|TxuZ`3M5G)Z9Ndd-@m~Bw2)=FlLCoT(R8;~Z2!cVI) zBk0}VgEDygT%)lEhi0#$vYL3{#P-om zc$cH_hERciCUh52)idTdV_Sn!cy=g;WtsFL4yq|b=0S*vx9=1J!C08{so` z%<~#lNLo#-KFXMIOx%D-xb`bdGI`HaoGtTz)*=n~iAX3}DtHOSHCo8PkcB4bDW^Bn z_WBv2Z_EnxF@g&sYiX{1R$w+Hy)I3s$|h483Bs#fvE#vY42=^!MKly8YXww9QCLhR zQY{dS1qArTQ1CjRUquK_Z$`Ytd7zrl)4AHM$Y`Q8Z{)$oH4aM*4A||Z`26fCbvsA} z6{A$Qz?9BLfa0%AWDhHD;D>2FIJ(4nzz@%g8kOU2qI$4BE0MLCrqf@j&HEtb1v-H( zQLopy;NCcf-cvOy?IJ~jBu+z~)XyOfUca6T8N3XaZkO{OXSkc3E9T|AA_O4cLoK__ zsTy7m!)lrb^y>(`?cyI5UrB&}G^>w~KrBNX4b#o_YCmVvWbsfR(!_YibILra?5!>8 zkaQqHipy_Ch&$83_LUp*u}6V<*-6b5IQ++e5qL8FQ9-n9IKtYCOyPk+jRoy{piq*3 zmmA&2S!jk`gw6PFKI4z?PeFZD5cC^kke_U^c87B48bQeG$g!JDBC|lR)Il)q;>PJx z2HLZ_fq?80Gn-g@PZ>u|r3rcw*grQQ!znB3iqW!O7)yM*Dvhc7%^532Y^zC*Dx$L( zWjgkubS4X%s4}YS()17W;IU{mqVl)jdSq_XI!+P>4_3%WSq+1(4mvt6jXbL=yMgL& zi$#GCSl-f#yTIhq=nc-v(mL%Gjl`nJ){a{*DGT{2w>WmU!6q-jif^H3g26RPGc8Q~ z2xEZdy6c`FYy)$XUc49ZU_O3YAku~0#btrTyE)eT&Ww6qZMU}qj#iz+LpdP>*y;Hs zz`j}Z-dw-B>L5HEd?4^>Zh|H*2Gmn5^EcoiuqZyFVFw6^Y-ox3IJ7R2`7(SqHCoo9r{s?Ei&z$32`(S@5|s9kSKPE+TDx{BdA;+6A`V-Y$OY zH56x5VHa`*HFGk!r1XcoA&yEwyB?_x)x?5Wsxyyl|K#27Y>j zc3B2taRVUz#70=lCMcLf|K0=|jy>e#yQlO6M;M@Ra6tw7+>_IXWlV8AWXQwpe^r2A zLHSQ;DtBx`b!I7hFmu6Nr8JBY8b3s$-7>xD!UY`JzklSPAMf}CD8IqYP~vem2Z&7I zDJkzYr6+}4am^J?$fCFqjY%)QZzuto!Y-|mRm`&w; zQW#5O(v#Mv^5^_X=m4Bk~&(}xvs3w$M^!_^Xx!v2R0rgg$5H~R7({IN1*PHtH zR}6^3y%$DXNEllBzex4_*XM!4tf&`Qp?Dc33-W)5I{sYe2kKXOEgu1f;m^1J`OhOJ za05x{;Wz(#KZDx9BTc_eFZxfy8g-c}mLjYb8U_oKrMV3`H@MF0sHnEv|>VboMZsU`i!f9&&51Zv0{4CIz~ zGJ;X(TXLu!;0Kt1QcyHk`QPn`X6Exhy$>=Dj?h;W5sFLBm0^S_eC7Yl2LCz1I^ohl zztFKOHv{voLF%Z7hNgA@`_Ms?P!uC|m`DRoGcp`JyV^qRNXg%=Y>MnCkc;dAUN$E- zGhll;V$)Oy!A_$B9*mI}FZJ(+AcIGtr~ywm;KwCKpq{r!0h~^=V{d`~e)y&oX%?>v z^=JPanh-<=ziouD$7LJnbVYA{c@+z|%V<^JHa*mIQ(F8`@Za-9 z3{(uO#Y_)H)CsD6EL~&K)&nM&CNkK>T+(TP69b=;uY>Y6@D~-Trt>UTzuJ24c=OSp z2@57GM$~3@t&E~J(+d07)Q$LV56L391^w>rxpt}LYdfnb{gqeaXQh5SoJcRn!7!f%R+5V)v6HoTk>6ZmCjQ1O+lOlN$`7tv zGa_IR)A3u=X^4&wu`41NRE7$_b}$MHJ0KPdylgFKyWRy!*9EeKc9W|$YFrA1nhw{1 zUjO;^1}JyYvuzJ!f%tqjL^h~uTp$}>c7Fr3*eC+v{Syy>*_4C`!O$}=Li%;JGk5S% z=EJS#e$}^2CSaqILG)Xp-KP#!@uqOUNvv!Q0WAy#ikRA|?#%CI(PPp9q@Wv^or^^< zD$2Wi%RVv&(N77C&B8!KK@IYAd)N%2C~EteraJB=4=rL>j$o7^_-gyLDKJ2r-FlPU z%SO7Q=U+|%!dna_#?ekinz7Qmsc2$Sz}9Dtb(B|EJ-z%@g*Ib*zV{D1*v5FUnPKs9@bqR@czU8P`ObYN<& z_ZvIF$3m9e8QnXg#6I8)1|xYo63dM)a~3Q0v=Y7($&(wd{T-e22oPAwz|NfA1SAmd zOwOcGE&$tF_XaspgShmilYM$%D6lsSCgUm^L5~+EZH9wv}e~KRD{g3qJ^&3k4WnJaDMf9u}F^? zEc4rHUW>Edezh&>wh&Ii;ewK^7c+-{cxXF;!6gY9yml)BU`=TlF96$C2&^G5Hsf;gs>~4 ziCB3{`!6RcLE~g-D{d(UN0N{pHC>)=UT~-@hT_q~L0KVX)y!4%pni=%BmjE+!#OZP zP1W9)&Jh=0Wn8Z)`0QSP;UUI@o%?t&351CR%28EuA-lWSOIuWP56MM+To6e|?7eXN z7$6+>?SX3ZR}<^tX~_nRxlCVHDbzCuWPpxxb_Cw~?fKis>2S=D@f5JYvUSqv4BP{9 zlOZFUAigV&+V+RVAQp1J8(nu83J*dQCQ&TOF5vj_%!9>%6HLaI&v;pc7pEz#17riJ zxVx|6VATUK8XXst0N7fAy(SxTYH{1nirb3|S9|pv<~t2zMU)Uy?Lg&1&7ltt#2CN> zaBZ*Ab;{ikzvj}q8vz1BY8*d2(WGWgzOS1%*`o%?pmw)x_tCGACIUNJEO5-r_`GGE?%0)O1Lvre!d%eod2BDo#;hJ3ef{_g&N<^pns?`9Hulh6NOHASiXSQ zIOa2Ewx5jPk2T!LpN#N*QxPC92Vqy0j;K0W?k?Q*8O@7D4G^M6dGZbvk8hRP>l`Z= zjoh&6K)ZT>jQ5#k>(9YjU0e0`pO>#(fQcCK3?W{8M}UBLoiY3Q*)98LE+~RMzBiPC zwH8n+QmfiWrc|HMJN?VHCeNbRN!)}e_emUlPYc^lI4-8i$!WOHzkrI;A7{D&pBBFt*+#v$h z`UoYfEG{T9p~+v%A@{Z}MzGNZ99vJ0BQrxbl0XG*^}Q>j>k;<+W+*4mBu~>k#8cA! ze3b73h`SDLoiXX_(W8p!{<{zlTEHch;C{*Qpk2~=#9B&^ys@MAv51g@U^U)T^m@Y* zUn2~~!rHYlXnPWeN6p&-^|C>Gg-p!=SfnffkyOVq2t`@=QGfI0JdwVJEYJUU6n%rb z26pXtfHVCJt6aM%q9Q8CsenJ@8)wIfN(VZ0)^A_E&S8=xHIhsXl(_t@`?)A*y=^W^;1k^~yG0F7feA%3|_obSZSHV!^ zGmGGm*J%rT{7IkJ!?3S#HoKuD58~Pww64qDi^>&L-(bHET*%YH)nMEgkwY>hpq^msgYiXrV>>_+`qOZR@1z_7^Ij#T;Q^0nj^+LBmRxvox5wNF2HF|zmGjhs$Zdp z0FyY49-^v!iY24K;8k8XiVg3z7!~Bt3~`<82*E2xkMDBF z?$HMdZ zR<2eV*3c2i7@G5QWLH3~V2iK5IKdeFnRMjfGoukV@wN~TU=pF|dS9s;mhScOr>Nw6 zZO>LD5a7!iTMufIQ3iOAFa|~?OQIVSOzc?A2b0m$fwo%JgE;P9*!_7+Sj>a`CVU`V ztcPa;>|{$EHirU|KQyS<)<0@Vzjn??HuBEOD!WyX?|=RHf==AqbrgO#%xFmzGX-!b zY;v_=VlqNIA(Cx*PUkYvR^=fU!a2g#1{~NR4;rabg#ICqR^cItC*e`gjj_1=F>In5GmG<2$cP)hlFW z)GZcO+tU%T2V0V+)N1_Bm;nec$?^DN&R1hddv?5 zi)j3YHp_Zi9gvT|_%wDEJ;H5?nkHHZu6g_l^KFaXaa))~*-;lgqDUBgbV`>l4HT2Vm z`iXa>(204;jc*mAbpA0=0p1Kf@iTPy+*YD^b6@ zUwTBP>izGemQz8>?*vFIt(d(n*Gm3a9Az^>q`mSMiXyVzG#sj@HA|jvS{UVsZ3K0n z?O?*Dm9XNSw3Pa5!IT8q4Tc|++zpRpI^uXcPeBWpuq^vWgCp(I%-~1T9{HoSj2r>^ zS+-h>+rN!#U+r%!hq;RKY2u@z>MY=6cSny?1pynHKB{?JXa6nbVADb?vsN@7UDJkj z$t11$%buTa5pJ3n+oa3+pl6hZOgFXxS;BIn0qUkmASH+AB<-x+-ML-Egc!XESC|DC z5yZ*Z@a?#T;y>f=erNQ9;wwEV;k*K>3>8n$jn2xB@dagWwR8@HOfy1%Cuvf8_ZF!; zEN~BMj=fKahDmFwZKkc-VxIiTd1RqL15PF0wMu*=Pmo((6gYWY$73VSF?1GwHbLkw7>*{3veRaRse_#CaIdY=-O#gRf9Z?X5WxjbWYzSBF2}38!&lvxru=sRDZU>CZ*#4_AZ(XHaPlYdHD5az~ zgBkis<#vA9ze!rVlRq4Ec|kv5Y{%l&;V1$(L#&U%wJr>jl%wf(f<`}|t-qYV1!FeK zRW-_(UQo6?BbjG%OA;8K-{Ho(V^UA)kF#S-zW^2dugBjibMG_{ro|K#Mc5f0Lmog? zO=|%RVk?|$x7MJp3lf;q>e+u%qUnC#paZZ3TQXB46*b`FFh%nqS9LdG^EFUp6)RvP}L`shEP;C3psx2x7HHqZ|S^D8bb2$;w-?O^}U`b#XmRk$)I1l`W0dgq)B z9ANc+T8{sS>&KHQR#ewEDWQ!o@uQuy;0}c-kp+%l>Nq}K@xR^7F_=03hL*>VEn`Yx zFy~zM4<=1yxJiFmqV7qGOn!`T_!zNML6MWiXe~XD9 z!F-@vfO&07FaJ7^)qKCQsOsb_-hKnyq4gyqz%I6;&&8 z#J48YM53|>U9nsZp*cb zEFb0MC%S|S-L|4s`3}7|V?cYVQTBQN`qW`RwpIoy8hNmfeSx6=02tHXuPEPXj8we= zN=Z2tQ(^g4L3I3fSNyHMp&7I70GTI%mdQz?n|ObHwhf}{+9hP2W@V2ghco$x2-ka^ z6G(Z~+~AzOjGbIL+K&I*5C+cqP^kM!EVDw6`QdC~c^6Wx7oUlHf4|`CUjXgb^su=O z>tH$foAUJ_GzQm#yW7o%+6jEj|8@pQSAr4COU#u21q zhc>E(a4NnL3W!Hxi(qn+$nPoqmky?6BF^uemRQUbAbN z-Ji2Gi!K)<&w=PjCzbKk6ydY5#ETnOO3DanHk;e*AO9dNQ%NzVU#6)6*Y3P7KX*66 z0tkXFId&_oSv3h2yS52W{nCOGsgNObQiW3vpYSlc_zqC$D4(o`@}uKYJO1thL(LEi zi3xMT!<0lMTCR44c{Z(ZxSWeMR6^2Nd-?K?o|rGEWZ>D4VJj{Lh7MN9d28O>&`|h& znDjp!+9!cH46uR@n@jqbRUozKcMuPo_A_|q$%zn2< z8%Aqqv*vl^ofeN_pvW?oT3JBpa>x$WpR4&jWl|q)-ec`Vb?KO*s4dS2X0ywX?mAG= z>zsmk(j(1_-k<<#r_`C{o#n zGjGjNQrkG1>);aKy`^DI>hbAg{Kn&^c74V42pvDdvH${S;wg*B*R%ZTvb1lqjioPv zaK%lTH6$ksmvVjFXK8MRhoOWN2OKA1h)n~3Ww6&1v!4|5uOooTjx&!v!>@tM{xG97 zI^;BSG)>dJY`7t#bB6Fd&Ho?`_L)D0%)c!0_vO#?FP}9XjJX9*uI!55X;x9(Co$5H zOPT(Yi{&~mJ5W$90|%VoOs1FMAnTI$@~fQt*VDfnc#`^S^eP?rx_i#<8B@s4A7(sFEAKDmsn#v; zxH-Gn_>eED26}DZQvr$6SC&!`zes}Jq(RNA*uMqk&^Tj!l}P6-Hq<8BV%rEyeOtIY zwvh=wjP_5!`9Grm^N|?<& z6kft2^y%=HD=oL!f^c+mmURg;{mqqdd|AEeX#jjta~>SB`&Bn!6Q$B;fFUE3rV&}j zZ<0Ef;;^WM>K2-ML6d_SiNEAQ+og0EyDwUi57#miUK51r==!wVO41g6CR<~ zk0K@1R@SR3kHOOr2F52XTk{G{QW0opeG&~ih0EE{?1zTrYV%gZxR`C!Pu%LGtB;;p z?@oGo%)0K>L!+;L4ApgGJ3>@EMUcClc~;G|LN%^Xu+7(o(N^H zUQgDQOHt^2%jo$|`1CoeN->h(Z^Se);ZT*&t-*=!WI`@|l*xY())_PH_Sv5Rd@n1vr&KQ20;APWf-4FgG zvNNXjm+LVOT!L@>VT=Tw0jC+LzcaH1E*~bSZHmvB$tU>U(lFN3QU&fja^%o=$%~T( zfxv?#5&x<&(UxI!s}D{AhOP#R4k(=K=G){Oc*hF=XxqnRig7Ig5E-vg;Zz zMe$x`{|`1$rI!(&Jy*eAz|;i9`VMh>8@{5{Do4Fy(2j4vs=J|8@D@)2QGw}iHxplc z`!1}Hzx{+Q>pcIxK;CwR(&6EF{iJgpo~q9q4DHJhev+s^prtYg9hQBD4R@Pn%wi&y zb}Zo%I?VW!91LAeRH#6oWxYNcEMj>5`QG-MS>MlozRgxU)iv)Yc%cL}BHhAtLFX0O z!YA(*pCQlH+y52N)QI|~z_1gy+|qw)2<8+Pt-V8bbS|=VxwqsK0$gIzqdRZNt;|kE zBF`ADF-%!;7&7L97iYdNy&FFe>BDx?c_s{Me&6)TqIr219?rSq-2brIuZA}|V%s3o z9}`+H36lnxx|P$duqRsr_5^u2SDCF=3ItM-B=G!O2LH_b{*w^nJ?`6+-1D)@sgX2P zIvA_<$%UtoZ&9%+M9I9o!aTo)a*j|R_b^}xj_|K_XJ?J;r)5|+Ywdw-?kMZ^uixf~ zQI6&>C5>z2uvIRdFc`awQjHbF(vx}e-*_T^t~c`V6?)!S*RUO|59Lhx?hjF7{#K3dMiF8bl>mwCu9YjU zpp5i!$bJY+pfZuTMJ2%Z8ce?tP7#Johue8WiNfH`5DAj7@ENV(FAO-Gsw}pY5uX+n zQIGoh6#j(t_fQtG)-$z4!z&al1d<~>G0K@F(}MMa5#tLLF%H099p^JPZkSlygdYi6 z_J;~x!yI^W`jIjlxx%Bv!@27Q8JuU?8L(E#T@psg|C4!RK$LX><(&Y-m#EDvy_Lpk z{j3%RiqAZthh(-5iOha`me*cl!I-x^dG~sy8o$R6F_NPEQ|suDBEY471H9{IHu1L@ zCHyV-CR6B)7rYx^IfoszZb#&^?DfL2yAPE$Q&Z(XF00svG^gTa;98KZBxECTjF)*n zAm$HRHGf@5^(N%zJ_1L8u=<-$|3ig;x;H%u73hszCe}Am8L)bp4`oWP0`>B-8${WY zB82ANeEk$!cep4@iU8Sem*!<`9tk|Kls|N14`~Q8J#3UJcL@*UIGstvrc`Al&|gAVsi#JhZ0RW@(zLt=`0pTM%wR5K3!IRw;y*&$dJFN0y2e4p1tYr*ZNxy02%2 zqIZB{r)C2J*GiVmTfAmZ{G_j%tv?sVbyDH;HmZ9VRKutnXb>VHW{{uBAQy5>b#r@H z@t^v*bBea092J!@f!LSp`KRbqf5I4sR&j>d(&MgvLnZCLLy<#LLAyl$yjo-wF?*di zz#6T{Vvx+N`)=IwSfh4GxE3V)$U`0}K*ztoEFfe^kvNaHYVs?(T}NGYZ19zVKqLs( z*1V8lr4_%z5662)y15UA`fOdVE_hCsYZ<45=edfChUBGp0GwFYqJ>qXKr7gKzBqBz zk{HR%gH3`+K-QZUpM<=g(vbyKkHPH+K;)_zXXu!RUiA(WTKfoFVg z8|>?%Bdwxl$Zjm66sMfS2&nJYBmX1ldEu!!Qic2Q9ThV<>|Km`^JT*>Wx1kt-Os+{ zUx6Sp@;2v5%2;+(XB>|oX^XCPR>D@8s$aW`z}4rw^axfmJnjL%U#30d#0GysXC{;= zZH&Y9aL~*?Jqs?41KBEBIsoD#Yd&WdgMcf;8&4`^7HtU2iG01LK8q>p(g4}PC;~lc zYH&tGH0p_`U8MtzGdvy$LmZ!KC4yu z`5ym&F95)dD#_xTOrE4p=)M&kQwL;fF!wK2iIG0<*-GWdD-d~U9)^QlIK){Wj=BF{ zTZX6uP_MHHd;ta!_9C^mjbVRta#vM(0BwjUO&4(bwXcVW;@a~1gMuV3cGoei%K4z| z2ji5(+d#h*$OqDX>10P>>gsvNm)$7yi&szrb(}GN8Gm_7PMi0?abTi%siB;t_nI?29UHN%ET2x-Knhla8s)a*uYx2!Hq%#>;h(O+XkPp+ z1`WoDH83a7qbS`-w^rVB z3KSdC=#syG@v4SEoD21#taOs`$5uDXIK~nI|62cOG@FSu1alDOGr;p>Hg~QZ-EOq+ z1$N7aT9wgu6!Fo|*ANCcDiwSRN2|V+K>ciX-QOzt4r2N&Qzg2Df!6O0`#Ec9jP5>w zo*40QtZR2m!{1bwNk~3_AJ2w zRB@w<8i0U@siH1J;i&4>7~Q#G0-=|m9~RyGiR4xyvbT(Z3G%g=>+fzGr*g1P6!3bh z8=AbS1yd=lD6=kObAUVPJ1F7vBJkq(zqGsp`s>4&)Xi=U-Xnx*dWc*O!%e~!aTUv0 zLwAXw|7m0*hFx)a@z+WyY_C(g`MED?J7K%1`Djq0)o6OJc&oFoJ(SbDgHRC}w@I58s`bvXT)S2S9xph( zIui95$mhuH5+G+L)7$0yt*;tc@t!?_$mHdI4SkPxLoJ|b>1DK2h zLk!_`VIkKwt&(8k^K~*)k1%o6fD~wMqhWKAl0wuAz2AVl{ywk@eo8IJG+@*D`qte> znXM%{Fd~S!@sedVZYO$}Rji}z)tXI8Em>W1 zS?3{$Jf>ILPtLjYN`dLC{p2-0SVIwo^kbl{L@5fi%JSE@wxfyNQ6@8@%l=?%3n9R+ z3jf94gBDf!%f&p73M#aCgUlv3h`u+T_Z-YbfOnq?uzSncP7=S4)&RCRz2_rczNeJB zF1#=q?eC;||Kz3;K&lA3%t0<_pBVpC0E?q!P&PI*9|W6>Ex&*f-G48HlQn@ilM#7q z!Wwc|u_QCH??PjBX*EXu(H}w42CuQ7x^&DR2*ShpjG6YzHA^*2UM-6EG+=RzSiK?( z*Qx=joPdB%Q!3!mMJl~~{`an&-yLMdxr&D*=w;+)7MMP0j1G)|yd5^?U6UM#TzB~I zz)iZD#=Kn`B6b$Opy`qRc>^>BdI?*QGE8`dNkS_QXoN!MGLiEpZusY53q=VkR5 zE*jF^@*+kH7wT>Vtfo58ZA6IhbdQ`O`7%N5vHmVZ#TuH^gT3}=WtI7VZsbc`WQoX) zw1`MHOzwl?9@*JkP7q^EIKikv{Voyy^GBk=>-|gE8KI4hY@0@*_yX?NR}xR5DEZ@z zs7P)EXZV|}mS7W#KRUrv%!n2+pit2LWppSH1tSqQ541fVEss6{!@oY>9}|oyt|SOA zv61OU=;=AV@Asqhc{{NV`~_MSQ%akI?&6Fs?kg`cX6Awg&W6Y}y;HM#gIcR~r){o> z$1%nWZHl`~>s&k#qHPF6iVqX=y2JdP#6%LM;>i(nrR_wZYA@ap|2>$N1~2+UUy=WF zhas<*eChZgAR*M0X6HLAbSoBg2n6;e*4I1t70AR(cZ)6uB2CjcH_A`AtKM0QDMu$! zUhf?m*@3SwMX^hfNgz5Ql_X_Mvcf7E{Z3=x8k`K&3gE55@_y?q_a1ZrG%qh1`CnkA zzyCUsAg@(K8Nhc*CU|}gw&(oepF`6 zkDGPw1U*)mCGOS-o`Cr@uAD~T9rIKd54>^Q3ko_s`7OD>FjQ{yF4aZ z?8N5n{*p-xT*SAYW=3e{GKGHl$K)TEH`@Gmlf+r0p`%xR zF;Ek3JtPN))Sm}gDWC_clh zZsP5h2EiV1h%7nu&P}w%=CBS+Vz1)xes9bc`_T)4nw3|P6B5shg-(KiM5o`Z6>f8r zb7vUcGTLC#%{u79jU!Wu_ZM)trPpBJqMJ285h}g^3yiY&w&rCITob?hTlOxIKw)xA zh<~pCg?8qrx}w2Qs{=}cV%Ghe5R2XK8}091qk+OT#(GWgdo=rF(%YoDuti|}K1zBO zb@t9K$Q>*|xJZ)kv?cNP&(gQT>1!mz(0M zaq)`x5s=Li-`RQ!(*YGxfXeTnxN>!#Rnk7>ddd{uw`Vukm-F-eiQIa^{JipkY!!vU z0n!67(Vk?`8X$D=#Rd^z%a@{n*=C7T$NZm99?g_#9qH4ZSLUBvQxA+_=T|1%Z2;?k zmeCW!we$#D8mY6?yqEE}o8BpQd*1YKz(Lz{mCQ#K(MsJbLi#OSzX6bLFFvY7dzsX= z|LQurkfn@xyNe^9qo&K(RO_lidmRa)bONITlH8f&^RuaFR=>{HgIjxI#YxaA?t5b$ z&{?5RWo<`1ehR!EZzmw1T$=&t`(}^^klNouyaaU;km}=1@SXD;V8714)SKX!#NgG1 z?m7r{$xpcLO9I`4#eHR;rxdZp$C)sp&N>y5zuyGDx4Ke*H%AmBGM1oBjpoQU^@?ND zugh$#D<#1Lj2m=3T4Lao6=6v&YRF+hEWC0K#7&c5fN*%k5bl32B_TR5&Y=r`ihdGchT@Hfg6lXFe2=7E*;?Ry|hE= zv^%jmBzpGsQs#WS&tjYb^CWs=bm}ea0FJ>B`qz5F0NdW86Jl1tRy}abz(JE&FSq+z z$WOvfi?wN7943_x-n%ZZXYwUY?87x+Zku880^zTCkNH*rpIy%00tQ1;|W>~cgM zrLS{llsN$PbTlm16XBd215T3I_*>@le_=YnQnr$z_2CnKE8M&d>MsG@JTZtb(9zNQ zCK(X&KRj>`!V0aiC%YL*-p0ikakq{qAzaQ^{}2G=k_z!eBd8ym=ab;$%51LJSiHai zJpR|o8aR(ms_sCnFYdqXkk74GV)z(q@<`7f*piabhm1B_cHT$dN`Z+NFbbFVo>~Uw z_K}qqa-~|S9>2bpXyr=*T|5VNnEO^KvRakYrj(4n&#kOSnNk zY`qC$#zP%+)&&R*h+9SUXEmJ4W57qKF0;0kMD~T6h(%rtDExP_mK;6y0~i}fp)px2 zhJVlo?753v5Rlz9v_oU?3f^K(HDOQxYM4N{xLBeS_qBCf%+}F zY9S4dRq1o{ns|;^&D<+GTO~fqtR5|CCvFJy^hY?g8T;m3Clskk`@-!Zd1=%%Yxm zr`5sPbDMng>F#pyR*YEyrS)a;KpPO?(h6Dr1lvw>LvE%-qN!Pbr=%hz;PIR|_cHSzl%`&fXm%~5gS z2u&zdb*jLK)G8HgJ?pVsS0QjOli%`KarXy=JZ`GuCx229oBSfrBAC4nrr8mpP}rj_ zfhI9zr!vftCW;Q_8%TjA`Bxw?bwIn859+?}*OFJ;DTLcNnpH4V4RA|fz<&7o-!}ut z46NRLw1V9$vt7g;pgXL=`93Oo=e`tvmA1t7`Rf{GS)9ao_`u;c0&VX2kaU~d;8@rD zYNpJ)@rsv0+;&^}TVohgD}R6*8BCe=>!XG$S5#V+7xBTwR-6+l{$t00-G;he*Y(RSmb>{@k`7BZ|*wqd-my8&+4V| zfHbkz3p-cA>ne=zS-z`F`PK4X!YkcBm~j- z!;GCTjqsa_2!U*17?*0op~2BZY)Dv_zzzpcz!|H6${PsFs#=m)mXEq>1Y6;ZEnnHJ zks3ZeWqhdE?R*6FI0le5265oUut4gx+tc=2}cy?mL z$@c;57;#VZA$fO3FyHRV(wO~ob+*LG5$KQa8vonc_&`~TF@^5`t?&Ja}&EM*U)rHA>-jOZ+w6{+Z`Rw|> zqBcHnbEsnTnj+=l&{XifIQg-XhJs7Od#3^imRql<>{Yem%N4r(G{=6URWt*Mqi;_q zDneKugH9YR6Z=p{d>WgT-MBe>#Ea(`CeehB^|a_SJTXxMf5w$4;avR@5eITUQy7cYv1b3Hz63;4Zk6pXByBiNReQi1 zb+4+FkBJS>6nf_miOrGuJ=_#fCe63|mKy&|g@F%PlDg#z1Bzq>vJ#H71N1-oxh+9% zpP!{lKP@wm%MEZp>FUVTh1H!Uo0qdvk-S|5e4=ut_hQT|PW}SI!zaLU{%W7N z;}yNm^`g?V!`$!%3s|gi`n$*tFB~122tpjlGo8*a8)@h5(qs$jlbN$!-$BMA-YdQp z*gT{J*BSVbUhGM|2JRmw{f}Ii`xm8(@PdIzW}Y@dr6de&JA?VcuUawi9@(ZYeTm=T z6WH13@H*hruq_($_ZA2!uS5S-a}Hs26-xJVfqcEi63X#^?yNEDC?fiS(O)9ZeHJf) zF+IE^z?1h!xPll{FC@_va{)z%_Xg&s z*P!~#yO86`nBiTNQ7 y=B`zl-G z?jLjSTf=J0Eh>{KVtBQA!n<5%HuC=KB%P#KmJ?zAB+2F*)~$Gab7=%JBfAGBOK=P@Kp<(B){qzB&4O zpIA#$k*uB}MowyQzyQ9;fZ9g9uyX_g1AlF?7;4!&tW+$sfef;t>H6mR0MzLH& zmJuM|**(prp8X3R5cR<7kX*I}%r~km7x27=cVo0JOiaU&Fg|)q-HY$0n*vx92d^gC ztJ#dy_)ikjh`gx2L6_Y(DCKjA?{VvZkf*D%gT7gV-p)RfXUgQ%gCKJ?rwUo)s=nol zqk4Lr5j18uNPHF(z?vLpB6ot+sC|xH4R=b{3CTql4(NWhcEL7U8JDpP0_D!+-q(d_ zc|5ok#S!lsS*vl|!1~v!O?fr^SZWmQpW(VQpJ^x}VM{Jct;p|&aGy=Dr&*KTfGN#& z7ge=n&dQDYcWRRqR^H%&z~l2n@CeVUKDTn!>l+qZgC!=ekC*tNWoQ_$E)G{B@tU-a z3p%!8E32(G`?V!5cb49XgV8F- z*bQ2yH?iQue#n;N7|dNvh(Hba-Obq#%L<3Wxr(0&)D(KmJubHwdwMSi*6<6&gYK_BFpQnZApV=CB^H>>!XI0Zs`+0v|8_FwUpWGIPv3QPnUFZK^_|J>CZWYR0?II z$&9+|35#gTCT#6K=VH51w2`~NwLq>D3~_$$8?Bm%Z0Po~K8L*imRdVK6i-ZbB2&;Kld_5YxYl~KNo(4hE!cUzA!zQGY zen8}qz2#p`sDW22|fAnsJhzzoLQq z>^ee82Wzn2DM*s){l0HSpjKhQaI<{UBYm(mSqH!-gL}KM^gn8qS^%-eUIPqKg!QhM zE&c*5@WHFR+7(xwBO#uZ7Asj@m&xcs&?7kf)lg5}V%AiR!Qptx_QAN^wyNCAb0g@- zH}bj{>MVOOl<{NupO?N@60YJ-x;L_4Nan&p`7M5U?tk9FZEWUl%__c80~lCtA?m?J z8uIF5xCQz%Y{mzofUnssbfvm@EhJla|0ns`d5zE0q+L|0tShx423&u;$}%J|7v zZ&AfesSd%GnanZG)?AYUwNB_7uS^w9IFyp&wi6Bg6aa^RqPd?mES7dB3nw=}^JiZY#`e@*_*qf+a=Uj@eK))uEf*VtPhNrI zC~5^#Q|}4P?B7eP;X_eSYzcn@OgnRQ-RYNINSSKBD!A%uwwrFbsj^RlLnn}I%EG|Q}(1iQ7mz&Wu4n=-K-0pLxBwIgAnf_zANz$paB4}fSrg5gS%kO(#n}IQ`3KlF! zjLf#*nAWAV@w#q9_{G0GcD=mrifL6PEHzRL6CgC~_A8BeS2teeE)J8b^kuTT6|)f& z-1#K2NwzE8w}3)EM-6NABk*9>fKo5_5Lwtas%qdla69Z)jf<+hcu^#qxJ`LFd|fXT zu~pn9anAl~naj{M&0VIeY^{odljNtbyJ;I^O{_-Dz_<)%-BqyKq{ve+wPj2jxNQR0 z!VfY`F@24BZReDfoe=ctH0vn|=2m0r7=nr+f8yMWEm{8rPhx%{tgH#AmHOmqEB}u> z<>xcm$29J`Zw!@WRVOLgwe2byBk1-jer1x$CX(^v>ZA@(^(d<8PUQ>HywEfn4^X!F zDviX-!S9=!$v0kx&}TJSCKEhb9~g>^mnYuZrRu2{NvtognUw$KTF?;-`6W>l_esfk znIO5fi)VaW8TlvXX%Kctn}fK^-qWY=*5*~f!V3yn^&aY~^6`Vqk9Mt3MZ-lYK6xCh zU(TZ(g%sP@?v=X^ms-?`QLJcDxdAj(PU3YDZW0)mA6o7 zKL%0qnk_%85GczBf#RD);|yN)aaO+lG}(;*?*-`fsQIZn2H5s?47A-s+AcDQ;Yoq2Lb!yVNXpy!dnKm{zU@f{IbxMZzY$F6qDs-9&GfzNK(+ z*0P1C379fZr&1}G@5eh(!+h{l$1N$?zNPD6FjfegH^qO03ZN~PwZWVAuw^#-n>cxc z?gjh;;`Cs(+MxH@gM#3bxq7Yuvr{(yCh?MosF;c%cThu)VUt39_Mb(jjcg6b@UICh zE=4f4S_X;YCZ9hVlX{J<2N|al`wt%Ua247gOeTgaEsI^cYdSN0X(g!Dv%J#-?!Q!Y zaKBH{5aa-sfa3R-AzT(BI97g)4YD06s(h%rIW-+MedD@`{dVe2hsdJ`WE)`d6P4dg zp4C8EF9TW^v~_YK+|R`-uBovfg6w{WBSDpH`6hzlZBa48XpbzE@LUrHGa;ws`_zVU zA5-@O<))~0J*+<*OsqfLmQ`Ab@)}EPUjh88?dyQr)l$~noW;!SC{;6Jlm4C5T**)#Uve@zz<&r zp)>H4C@d|&9g2Qaqb&vD%@(BQ;}by!laXv`td;S{M<|vTgb$_5mTrR*crA>&{J5j- z3w_PM{5z?Es}W`G3x${&(XD>4*GTBO8l@6zXD7o#35P#GTi%uHHe`g1`i{z~RVR-D zfM(|nRi+>+vUERviJ@L_c}-@pwt;cg-=&A&FmWHtJoaqy)Ue-}wrEVR|Nq5a=Pc$iR}63`Ee^ zF;Fk(8y#$pQf2_BHqGD~m_W%J8$i;VKfh|9QW}o0M1;K9T5D$h!PR(^q{W`WE}RLe zcn0hduU;=JKop*FP^#eSBR zONkw^%&-f!{_s{{$!|Xhd?GfgFhKPn$I(9V`I1bJoxhB!I|NB13Ut_ozD=qwfI%~( zk%swG^O-602U}K2GvvGHImt*B`3OYDZ4uobBmHd2Sp73VI;|c7OVk241OMhnI0yHK zA}hR}Qowg@R+8BvW*|Uj$4%=6)nnJPw0g1B7ryshxu51=5E4Nk06jc^>@n;S_JOq z>ETRTtEg|4&_^)jEHtV6=egR|h8>`Qd#nMLTYBPNGYzRS3x0iH`uZvLt2{@B@;Zk? z;!;=}Q9$u~LLvjzcTm|h9@MaIM*6aAS{ZD!kHUUv5$8|=GA02u9Lfrz$I3YJ-ObMO zus472s~5SPoJy6F2p6zCAxq(^8iWuP`A{F16QdHpS5(I(ejgP4h<9G-Oppco@KTj&fU!A+^SS<<{ z^aIoD@`9bpt+<#bP!@LyET2RlJ>lhaS4>r+Z||#!h-YMbl|-Lv0ioBAFd8<|qZ#m? z6=Us#tDc63(x<+HFbU+KSfA`Ryt~fE&98ZDHU1cWUm5{WUgAUk2e}TZtn9u^uLqxBnX3D(3Lk+3BU549lh z2lUmI`sQBeB1KT|rcl{~CCbY{pF3RUc=W{;;E|^1f;twOW>DDRD5h>}?kVu`4aS-OpSv9(Z;Tj=M5RS2c=6O zpyU|bOi25YTSGK{krvSjPLvfu2;qQYf-@xyKL^bAtQJ44^9egIYaKxqRtUK8&*V=> z)sq<^0`f1=jyn+VuNEYFRWfin>A_>u58@sxfL^W!xdilH^#EsJr(Hp`l(^zY^vHVP zH^PjCmoXZFgckUlY@QIT4z$HA#65tH?1^?AQQu}_3$gT(C6zsU0|j}_qB;=Wb54=` zg|8G8+Fu~%ow$eoWE)U!szAmr%}Wn|s2_g=RD56stt4Hdc{Dv#4hvc59T-ub$w6MU zJKHV3Pmy{!azuu`Cjmc_76bjPC0n*IWyS;8iLe4Z9%?;kyesaEJsJ&6C}0P5GguSc zqX*iYW$=I3BX&0Mk&uVUkq+8^$p}$cMrM_w`B8_WDcwZ`VF>F7hg>NzL)UHr0=h%5 zXt?WsECvd=68NN`IK`e)iT0hupHCBj#O2iNKEo zdXzq>?^XcvtpP~d<^Br<*R){(fL#k#+$bQN$_GkqpuIQM_MY|x_oO}3=iNV!mGa;m zLka`^n&2#f#BoJNL7=&`61cvQuoimoC5?b{_c~PJ4T!c!&j?dR9A}!D@mEUS?L3ip z-UT@PU9dcqEk`Q2ORNQ)V0mwD*B$6G(At4e1Pv0SzXWY%9as^rEeQ^PW|J@<1b4j- z==yb_m>0Vt+*EzMIW7mAq#T^-fdzYGM?+45Rvc0kc)~6WW?ByBF*JTTGAhalxgUHW z82CVI215PUuy!Jh6aXZenP%Ysxg;rUPW#`QHI{EbGCDbW6>#n_+w&x=P2`8w5T9j= zvM$$MG5iG9 zhB&4pGc|?mdEO^7p!~2+*sXi?nH6R0R%3Y&jtoWpTAh*KwP1%p{J#XdWF%g}VTFHns?uTfRhzq(A9x?|0u=){T#h-Vzxto_B3STY?j$>S$t149z-=X zHnhM3ICY_40EVmuR*sA}y~j6ix1AvL1&2&M=r^U@{Wh0GmtVMd+OgTs1cz&XoRXBO z=oC8GY9)77{uT{&2ImDHs+2(74x`74zM)vYO+}1~9^aiy-Z8e(t4YolV08QB_z?)O zR`T!yX-~9-z?c8PvEzKU_O8lrvMLHJx89C3A@TJfxCjDds1pz)H>jAx7qe~L{IG7H zUkj{*a}_5tGE0D>5rvJSJzQhl>Szgo9wd=OR<%)#S=RuM#*y}r*YQd2K2PN3`ESZx zGOCr7jbPX{4hxFJ>e(nc6>SAyf;?6j5H~*u-467A=c$Y(#T5AbfA0<_y4ma2DIQ(u zeUn9>l)H~R_8NyN$Vu{>$W5|qly83rG#cSn>WU~9O7_xjHFvN~HKbT!@~X5{;m>&{yDqgr z^>7AgfSh$AdAp7n(ikz$RHphfVEt7f;1o1c789x-m9jN$Hwv`%MLAXDfLe;1zc$ic zsO+NWl#D(rmIjel+FbrndR1g1E~iS-nDMZn$M63ct4DT;FM8vfv34CP=JQ#F1Iu>b zZnT0rXR>zch1Dm5a4Mbhbc$2N77220IjHb3;)DR%Bi@{4_rw~@4|DHYz7n?I!>bg& zu%UTwfPYOZ47KegkOgxzf)|(8n%Ns1r_fzjT}|eX%_HVnn-A4e(`JkcE_|wl6u`cP zmz1HzaSZNma~6G(TQqi)S|dAes@ney0lMq6+z3zao%Qnv~rtv(|_fV(gIvszkZz)=D4ZxKm&D13#j=l*9_MbuU~^lV$XU0fBYnX za-~bsirytbf!6)!{$+#OeTW^wW>OYbZ$r%2V$$9aF}^fUnyUBH5d8}IImy}VzhbJr z0W*8-*h}V2!;Hse(AWXsj;5Sy(3qe?Vlja`Vkx&SNJQRhMRhg}M?eL+0lc`y^5EkQ zj8Q-!O*1*)Rrdp43r~W6uwg-mdSI7ZaUqbUFufea^DC3B0AY=Ni6r6a-M>?RU}QA_ zmQ}W2d|oGXc{l%U1#-?3Y=T=yEst&C_5bwdj`DEu;);!x#|u-W4H%wC54~vs_uj%2 zX?toh@ES|pOeb>~OdUdOpcy$f4aA{Faa4^~FZjZnfE1AN0_z^N~>{1mH62(CLC&JPK7c|5q?UQNk1vhuz8V>p4RJM&kA6`F}KBcRbbo`{s1$ z5IV=+j*(TeH*q*tMn(x`Bzt9(k}WbKd+(V_k(IKt3R&68iYO(dr185yp6~C!r{`6i z^ZC5TeO>o;Ul)Y=5=MPIY3DV4*{9iDkxnSZ?Z*Q{E0Koy;YZdgANaB1+Q%MTf|n-@ z&yqPU?R2NoNOlgGZjDK%exKNb3Wg_MyM-R9@Kg~N7jYR*&T0n7v*p|dw_XTnm`lx& zai2NQ-r*KTz2&E^y?}1%-^AurBw9fCh!)CKP3Nb_lrH+Bk*jLV6+YlwxT@CT3qi|8 z#+l$N0H6&%|AH3!3}lP@L5Q%TdPE;D!+~!zX#H~4K)b~u#QVf(uBH63ncg30av8A6 z&Ur@kRi(x{H5u5HunHs%oM^O!xCfVy!$phPH3OPh?IU5{zdD$8ms_>-QDg7GF{ftl z-HmH?>gMU24YMA1Zd2{Aj=$2F?n&fu9Sy2c25omW=mPldec={8?>bQvE1@J20ACr4 zM#Z&5w#mK3JpDGP1v;nEE2Z!w4BThxg}(eogcdJ&=XwI%oi%6Syed#T`tX$Q2Qe#X zA#A`e@;ME*ZBK(htNz->;4dc%9Vm;OVmy{V5&uolRwm@UWw>}5mm$m=F7%on9afJ6 zc5243KQ;@$@V6LAdH4madEQJ%;W2;5@QB=#$kS3~#s-4@D7~{E_*@zYF`Nf`NwSkA zOs3!?=eHCqkX>H*Q{_`lowrMRUx^|uU!skH=dWM3UrgA~3@C*B&izQT{Y;%ko|w!U zv^VW|5M&@BN5B^#o9Ub{@5~-znud6R4&+Pa!KpC1|BuBn7iVN=H*>ho(8t0+tt0fZ z(M03Od|m%Ij_*^C1#FMEV0fGR52R(UB@1aO)RL%^VhSC%7#|cNtrijUCok6v6vn#a zm@3k;G@@yV;&W_nwou|4X~KRHd~_>+mF8ql*`CAXCg?DX!GlEVxipB=^MW?2u5XxU zL&c3OK_s4$Gf^FJ+M)iuu=Ag^45-TUpXtB9 z$nWkr*W~539zqQJ_2X;7zj@n(p_VGF6@KbQxJcQx4FH{#WYT>mm%?ZgtMpLg8{WePdr|p@>m=#i%et(F* zf0@GrHutV)u%hqB1w-1qERvcD?DC^s6A`oHAd+(;hj`0MeXhahM@ zi#6cYQifb$sb7?x_e}|R7T1n0H3Gu}dTIL!aw=wQsZgKUm@3K90(GQb_g06OJGYf` z|FvppxQFYw%YT>#kD+HGoytd&KfuSkn)V1{(bvD!PU`#m2t&Zj^&bYpf~<`m5^Nh& z^L!5Ljr>q$!`j2%g6Bl_kCn+Udfd>qBG>!Qy`YZb(^0<2Q^9GG99;)Tq>Eq!aw?4Y z%ztwnmP+N2(vF@V*gPF&6!OK)C)$7-?R>$RV3Z;kOVi=o+eg;lP2N+RZ0O@@9%l3TM6o|-m$+ozD<$-Ke5YaL6;KcScplH#bNmxB&ZMZ798aT23!$iuzDFzDtrQvWw)_Wd#b@iNFIFT&m&-aTJ84)xJ+7iawG0jpU z@aY#-;Zu0llss~L43#X|lf9Rdg|8M4f7nlzL%Tw0kb-{)c~sAp?Kz(}Y3-HJqUU#~ z-S{KMl317!I1M?w%VN*0;pBH4s~nB%}r_Q}Y2#B}iPvT{)hEW2ldA zlIvXZFpsSrytvAXEXU9czIpIf8?B`&x`wWe5cQX4{SP_PLup&;8{h>RjbO!zGk#hPVyk zwAdGOP2j)SZ|wd1&|e(>m+NV~A%a3l{~@qg$hCt>YzXfVHTI{@_b-PIj94+)_Slz+ z><5sq-*tl|8kfE#Zm&|Z*o&YBo*sG>Xij&yKnhQxSKkyog4vl0$Y8K2-lJYWI(p{5 z>4WNrkU1YEcawhbUQZK?@d;t(Zbbs{Fc?%b@(WLY?0+hRgYZF%k9h=b=Hbt09UwJI z?AdN>O>@A;hjbjW`U<7cHa|*Ne}e+s5GD`e75$)alVNjzQnjmMHn(2XU~2f~^#4}K z02Wq=Wb29>L6tz|Gvi!Qh3M~uSRY(!G)3Is@F$S>t4e{^7U~`GnT4U>O2$)WHn-9Q zNjtvA{D!@IRvEx?_{V?T_)>B2lmFf1)X32iQBqIKjkekHQ4ELv-FbKbS%gr*Lb-5U z3yWQKxN0^=>DR{+N-a+Scg|Kt=V6-nt#~hcNu{;QR{N2$7pMCK-a&Rmv|xo4tikts zA}V@sr{JyemWmUsd|95s-lb;%nkSAjBvA2O!!wTPX{%fNA2#6c`j>YeLri%F|GAD= z7Fh+Ymx<|_tRg41Jf7rW7bylH9OR^l2^F0nYOBl~>J0i{fy~LO`I5IegPdqeFcW2s zbKY?&{sqFHc;3eEfqvUY-&FJP?hU;Q&umyLj$8ekEDCr|Eq0U7AlOo<CEK*pA*;}I8Pg18b zf`9x_=RzmmKc=Nrdw9a6=Okt&oMb)!&6$H5TuATVKR>LcVXUZB{|g|6)gN*<1#&&Z z<%>P~GgUk6KujQwGhUJS$#Ne-r@{5$q}iiWFP-s`-+@&lUg3*G$l-FKh2zBVOKdBi zWQ<=_6$YOMbq}IEQWXffDpV5+R;x9uOp)T--WNYIB>zQ+&EeXU7kr+>WWX5n5WB%U zSXfpL%MDf{Z(|X+-Yk?BS8#pHmzGbj+(d;1_~OWE7wZBow7!B4!w+xXuc9uAN5;}~{qluuOD`)foVkJdx0$W)q@#a2cttu?XOD;V@X}i>P^Um+cR0WNV~O_&S5WWd|3( z&+0HFHnzX%HCO;k39r@8jOI*OFh7rl-T3-^(rv2NOA=5~s&j!HAAq!m890)2DaIdM zl9gKDyk)`AsH&7~a_O3WFn$!WQ(d1$XK!j@{ZR z@F>fzx_o2v>6NVX0d*`!3dJVlW=JIQ`dbrvV<{3MX=;*gJA>!=z6l&B^lfw}*-;2n zVgWHj!|n2XncC zK63j3%wbaoOoLSKt!ls|JxUtiggl`+ZUzFr2{>TRhSU1{)TDMN0N;qVQS0{bVu<9V zGP$0A!C|Qaj^zyBcQ7zxj0|W3N=eIb;+kV2zVIF?ns>-?N3k)qLhL0yt;R%OrZZU8 z|AH%Lxl|Le$uEvbTEPYLv z!++_RM1&yU)#FNAp_1R>#yf}mmZK7K>#W?V65;g{xemKB!*hWUB~S%@En(yA?!!8w z4>>MGLs zmV9OsTxxuyG|JMS5(!Ls~*$pI^3xa6_iC_7$8eq`|B5Z^ex6me$hSWv* zu>zecmH_gAD@z7jrIR7p$h7;hRbOF<7p*7rCzBws7|CN$q@MOGA zFeqnylG&O!+=UfhX}QMQ=zJJ3imD8J!phF0iHT2{;i2pmsM_i0zKFXw)TF6F&N6nUYJ_kRD6XjRS0$tz^4~&goS%VFJ{8zpGZ)YAG z4uV1Nw}98@&?7iIPRQS>l{|o*t2ktnom8rn@Re8QUf?a8n^vcveU{j{{9nz5H-WQr zfL>f)?v;9~X*|j=5J`IG&7$T7~T;Q)}*D;9Z8qOde}5ZGGFm ztA4!fg_$q5eX)^GXdqj`jh*$(vRTrYx{gWX8-7f0lB$lwfGnFa2RlLAF3`b{PT6fr zEQM1PX<>(}mLmnVkgfMEKAaPn&KFMrm>-hxMt3fl=FZx{@ZLsgih+bsbgVzj@>j!F zn;(9-vz<0T0|(_2)!f^Y%WHVr@OG(ZzXv{5Rpg&lkO_DhNv9p~0rO-Nh;7Xu$0ewX z8*CS~NP6GQ!0C0Zcscf|k6eU|3+KwN;i}FIa2DS;svwP--*Ux`oiLlENZYrgp4+h% zl4utlu6hQk8@Vda<`fYZ;w6W~x_^x~#4vD%K_*|e)N7(vEm-)io~VQo?kKMFG|Jvw zmcAnzRp8<=zZ76*gKpkf2skG34#-*B+c!F0UX zYzp6?Z|I(&H+m-27JJj~te0OC&k_v~blXGiVg zt#60JYSnJ(9yfVs88BZ4G{LGb&0|cqH<1+FA1?kRluh&codX6Qi9SqIfMQhlIno>y z{1#w*mM($b#J4Bh=9V>A+ZAnLNplRdTOBY_CLS_p+Y)4RzL_o2-4F>1%G76~4k8W@ zxkBy*jZ#u?pmVx!tgnUfk{7vGjSBEtAD%9+e`qQbfy-!10Jh67A4`;quvF10MfUM; zw#b`S0>{%Hih8%3myh>rgF7cf$kl+kubO;WOuxbT?>^Y3bfhfOYS14OKzS4kyPSpK z0?qE7l!L#JdT30%7~w&TOU|nG3;QB}yBlYh;5JUT7O<8VFf{w*!##)WMuJWkf}91> zqfE~z#_R2BZ>bhS&VvJUD_S7V^XHX#3Gxsg)n9Z~1s+;hefeBfo+yxvR^colLy$D{ zaU2E8o;%ck3BoCs4Q{|b(5Dpt{_2XDc9b@SJGh|6uB*n=pI;pwogfb$yO}l1LN7CN zv*^?&C2A4D)t-;8895>%cDX-YOM?8E(g>36BcgP6vKyH!&-3vK{ zr7_qo0SNnoIfbh89|lY6JDG!Ar4GYwfA(_z9kqUi@!E~~eWmmqQY=%hGr4|TiR#DI zpCEH0MdsPE1k5F^SZiJ}uQxzV_~VJQvo2HsJmGMFa-QCW7?He*&ZMD~lTxO#@&0^i zCoW2rcqzvRCdDUdI&6X~&-sX>$4!A>46#vF-R=~jBg_|O;*(CJ#$WQ69}zXHZ7*dz z`qp!|MjaD(imFA%kCiqR66km`AJ%qku%l32J~Fq=G=;Hd$nR)RB_KJA%UDP~(G zn9Lx^ECeYZZ-2TPm5O^xb7>^xp_qtei_|3l%C96|5+TY+mUL~DvNBA^WOA-ej7KQ% zWL{0A*x%^iZPje2?Qfdp02hPFdHQ-~_RbuS7M*Te@vh7}kG;-HNx4OLxw^x6t${_& zrHkT|^3OPQ+g$IiDqL)~LJH^G$OEu-En!jItT(Yb)}zesZIz<{+lGS@IHZs>2B8X% zICPi8)Jg6!+<)G?fa3{);COb2?n3>(JNlAepm3CN>dN@`i-eTf37l`3Wobt@W3l9K zVSJ>_1I!%kP1wB9({e~9Zdj4(J@+Mb4?2xJqNB1he#{(Q&;^(AWquWjfKxHpoo39r zywk^oPKv2m9HZp8#m96%CZ+t$9VY95UFL5;3t2zkEp31%U^5TNbv-j}!b1RSeRAP7 z2XK;A*Ob159N@Qvqa3B#GVZJax=p%5Po7`2apZupw1$k=eutn*(2E?XZNOi}+$5I- zYwFVZj*LbD<7$cYcs1BkGB{*i#WV*b?C}v&Bp9O415W`w*zVWlW~sg z1zkau13G2LZS|ir?PSQnLu*%rt4ohg_v05RSu$3qy|Xn)%t>7D@eBWmVhe+jmq8tB zji4(leaT1*H;gPcFN8U2|Dqm3mKIKoolpp`b>_hV@s({j0kz{bnI=!ngKsx{KNA)Tgu?ueh?Uqm{!P&clOvcxN z>GgDx#sWgO0n?f%*#-t=Ec_$*S-6G=w|w;fP)d+X_!CIbP`o2X9<<^{lDqy$XV7s5 z>N-PU*)OwG0i3Vn`H)g_3ITKDFm}n|-SA{6qYXr8K{YJ6a^Zd%a+^VF-g27Z zq^aEb>6rC9e~y!muDbbRErrD4H_QzLfC$tSpst%>7)~eF@U$yI;4Nqv?WI2@ovgHO zyG1y7Q!W|cB8Vff6Q;^aTzEC(#h!Lyx+)}IuFKO>?_=_lZCZl9yJ%=e{r76Ec#?kg zuh@&?oQ^3=xdI}KZI2%-umyag=R)=L{Aa4VIw3|&G$@B^PM!D#ETdB~2F!^lr0_tU zHA65`!O|*;QRMLRqe14u;(^Qp86ua;8AYvXep{)?-Tdjf6W>S_U&8$TcPWP6XwM%so~J--EXid0vacq$c%j`Qwo*r|tQt2Tnx?dLcUDCg4yBNUIo zZX;!va`a5iCYaQDziGzepH@LAh6@D9Zb0K%hNu*~^bBPdh_jgozb_V9!qgVOK`!$< zo!0~CFbsE6Cg-d15eEAIF|1oEaGCO(mjGR0iRj^kRz2ReHdjq+3p|fd3PJmKFX2ek zb5R2iy7=Xy*hn|7#!$9UbNf$iy@Zd4hk8{3h}g2zNjD{ z+sL8z)ceEPwH8DZrW4R#DIWYpKc#hUB_0p0{EhI6dTwBQYLfV zzM6y4LC?-(QKQ{bz=@;dOz^MBfKCa?LnNvet!k4;@C~Bn)f+zj9`@SxOT_?XWQB5O zLD1tG=_^ijI0R}ekL_hIC?sB);voEJc@J4#-fS3DktY7J0W?(P!i%iDvvT1Cf>6_9 zg$M}HGxA^@(%bUWmACV@toEVnM%*Xu-a;@`(-_^qYyW;i6Z<|?QQ7|*ps5_Us14|* z5SJ-?EgxZKC65*(luK~oTI%}335mm^GYQKqW)m6&5{0EH^==&1DCH}&Lwfip7wQrM zyq&LfsK3)}8*RZSq|{89u~s`$PIN>4Em&xK*^K+&Ci?g4O?kak?2J( z>oHfU2|5m#%&~0k(0+4W-3Gvt!7Oa?WWOP{(a~DGVIUerigx*n%sd-FO$+|Umuelp zWi;swMzH|4*>vzD7q7qT2)DLB(pu>o0*t|--N{QSeaCOLiO$H02S6Nf~vh(yq zI{X70aNiq=wr+**%!)M58P`kkKHeV5=1au0a5{Z~`|$8)i#!r0B1nP=hQm}rZb3Pc zuxms_CtU#Tr|19%D%mmvxaD~g%RMcBY|dUx;7~Lbv1*<2J*)3#OL=2alUJDnwNF?` z*5dSHClrUTJAGYscs`4yL#6oDo>afs-^w-^B+7Rv z=A6QMskS^3>%xA0CD{e^TE;+bI<%V_M4!Z@Cbk15^WeZNQa7 zN5cLN++`OC2N^@#5hU^9aQ8Pk=VCm;U}}3Txn3B~N-}l)2yPA{DaYm4a!qRgmht}g z>Or;%KkF#7DD*lbOp8a*+qC2N%w6BV2$tgK+%~_bwg+crra7Xfc7a=ydOzMiC|tD( zhfny2D4Aq2Vw35bAk>CzMzF&;9v@X}7vT_{LwnU-cb+V?l;6Z*li< zBM*$*i~ZFN(4d+i3cG9|kUSDJRyH{NrwHy-wS-MLmJ;a+#B$DRXX76fLc{PkU03~p z!a4b4RlAb^WC{+7Ym&qTu+CTeaq2VaF~RR_os(Nr`!lnlb+ z%Y~|wdC2ekpBn8dhGQ~FGK$}n4e%`wAkBvDGe!SLvM>zq7X0JPnfIVfWTCgY#R z53)$Bij*?`Bj13Xz6&?vqV-klp)DZcO(5e!`sx92N^fC?>ZZH_%Xt^HjK`eQ-tA|dMUuC}CcOU9(7l_Dki4$qStlHU)hpXmeLy1*D!Qc78bmKB$>+2i(&>^S1 z9apNE$by&c~UL|r8(#N^OEFU(N#Tu_Zld;;1PB50kNUtT!Y zpNS;6O-1$8goOwZ=jeY8T^*F9ENB@%{SF5`3bq5LSNGX|~~kgB!MpZ;+>&Q zY<1QhN#^PgRPXGxpVjt!$^Ng}3s=2``-v!cAq=9-VJLUq*B7p%)kYI%ODu*Ig|u_3 zFE>Atw+dV&>%Q??X4dVh!yc6T!cQDtqQeT2?#!_1DZ6ENP}}u6@QodI;o9G0>fNS| z=i#0o!Xr_U>hatNKas`l*~8*k3Hx_a?l$vf403onB=k*(t0p5Aci%DW_b-S{=P77R zSedPYzhBU5JiXD>_xo_`_8mnfbwQr{taA(0wbd|hxc~9&3_Zw)k@VX`W%s~$fJw#> zVsPi>oc+E3wUw_HI*tJG@AwHanOp8WE zkkHbBPpWqhLaOUViizD9IKlwEpRi(HknSFFq%xK@+x?J4itIWJPHr)hE;UDtJo)-a z0QI8U>(o2Zs=(MTmkPvS-+|OegYB;GXAu;Z15qEkfF$jm%;$&+AmhAnx1d&Lzm)Vk zmJ0Uk7|A!d9|ilUkrM5YyC;xnZGqLP4{H~WPsAG8%&i(EnnnSUTL3&{2TE)^G#B7S z$W*mK^wA|jo$vTfrz61Wzo~^$>ZxI$V4?`xpx7s7#C2 zz{e$3hr^FZv5W9`%~EXZt~y=0GVT^L%%T6_{4Ihui$ksyCT;u5)t-1GkjNC?>9*Yf z^+=x<)2Krj)6JeimcpfJc_36R#p)$y6 zoR$^5=nzSQYX|YTE(hpVp*!=?81WnS`a1_AtY<=fW3aMX<7d)SmT`axU+}2MZQt{y zDP9Ih=gqngtBGNt;CEuGi|C;KloU|dqB-del#wJq7>AT5AY~=x9=j%;qgecXVINwg zno&i35s2vN=69d~9GG(_8nSnE&Qr>T!zy_g`--3Tw%O67&lzAbc^@E!^(JH{MzQ6- z3Hk*cG{kSA8ruKXwaJTxVah}1(D~-bTSmf(5bKl^xqFw%$+HkD*Crd$^_yRNynt_4 zCQxo)y7t`On0QY4`}hLYnXcrsgErBj{p5Lyl^^$usO4uD9E!%fV zggk0_S`ws)F=vP5*VuO+mEK+r;i#mz2~!bE8109U1iM^X;o29Rq!u_RJ(($Z!H_!7 zX5(Mg{MCrgR~jdB-oRMC(=FK1mvWdk!D2AR7)ve_!^2(HT6g+_iT~ z(r|5lheIUCq^h~Qh>0dbZ{+x!{k=2L7LGU%`YZivCo}!rj@pNABx~J6+|Dv)&elAn zx!F|aho$oythO(D#5rni0WyNb6wGY*wQOL-qx?)tmwRcNrFZ`c=i{OEqYzcwk$bM% z7Ms@rV6Pe;&-g%?!u|6- z{?&)#e8p_^2QXNX;Bi895Ek|MS64tzrsEA^GZpX=3QE_!X`cGoU?$D@>iqjCZEbM-4?_~Ao7U70WGn7Ge|CTK^bu2Qv7gu@U8 zbLxkL-*U;ES0qFGq@`=pH_z0xLrpl5mUf*2MW(g@BOAho=SCCoSa&o~aq~@m{XiIN zWEZwK5?QF;U7ugAxcX3&icTE1l)lm)eRTjZupyOqtX~*$gW$OF` zyJ-6ceWn~3X{os3GP;9^#Fq=z&hCJd$$X(R zP&EC-+ezp2TSYgT^p;gMl5gUuU&n^bPv)sDE+@YlMu%} z?X^gwZ_lCRchW^m*%Ts)d>%R^>jhasxslu8D`Nf`gcSxiPI(u%;eLV;$9CrW-SyOi z%tW_dU+S8+qn|rI5*@{~0@{qr&Kexc5;OZS_2BYpM8~t6Qsem*bZT)5jO&-NF_frM zZj0B_;j|0`t(gL)Jf$JDMo%<@4**AuCvmGwdZJ_EgedY}g=G(t30a zraKhqnaN@Co7bUd!!MmG zC_eI~Hm5XpwW+f-y8YuG9N27{;@ozYC53)O**Qm)yAYa@;T0U1m>T`{e5BaAu>rJ>5#cQ%#*`HC5 zW`0}lJ@A{->6LaEU?y8WJ#&}6nVG2Hy0ZxbV&z$gfV1?Eg+!s-Y>)FFU=j8HRGC4R z$5^LKIAOe^VY5VK019~x)Y%l(*Kx2S-l16pvIK|+nZedSyRhQsWHIQc|5=>)4O zz~5K+j@CQ}HbLH5*C0!xTieGlWC{urxo{rQo|EpW3GGYWaU9PLSB60DV7#ZnEvKGK zIL?UM6B56M3%%kF0;qQ~!Vj+BLFA&jdx8B37KMua6bAdpM>2TTUGLlfYXNxH2*NfX z)2ho}0>y82qQZvY+Ko-vBNu>GurPIJSmMz(#@i5L=8>^B$B%ng>KP8WP(&06Xjj?^ z3hoV>=?PB(=FG@NZqH|nWKUZjRsReOtn=xi)Zc>7IRAL1$wmA%JIWHJSqPx+PM(Zw z^RVu*!hNmK^U}IG%08=lblm?DiugWE;oF$8<%#1&;u)r1>N`PG0@4%-VbK8a6?sS#sjoHd(?etWLoVm((Yh~dHz9F_;Ec2 zdbC=6JJfRHkQ1-2)U~8X-Hb-}z7P5&;z-h8$|u1BQncU-sj>2rQ(fBBWiP0jNmW>A z2_We-O^SF9>Fz_r25^+}hAgi6PS`Fxb@%&l9Hlv7hR}!xCWy}s_<9<-8OMBJBCYNH z=5X~HP_P?Nl?+|Gea6KSd6h@pIb2spyCk5UN+b;fmP>AIrA!MczPE3aNV`oSLbs?d zg=SwcNm0co=$59`et?RG_taMh>|nV-u^Ut$5*cb>U=1+XWm+mN!}ZqhZ(5RCFP~~9 zva?Jc<Q{DCrUpxNCSFO5RRA&~jjK?=8c zATVT-!*t=(rDx~8K-%3A>7^6|ycwoRsA+l=QQe09=5Bx8Ji+9v5PIG4lq}k_HW`C( zoMYJU;iYlS<|=jTQR9Y`p<=fuWXz^MXTAUx7+N|jF-Fu@g>DSj~F$sIX znmH#By4(8WPx1HpYI8NLx8-KI>pR&|H&*`Pf3JbHLPZoG(%H z7YU|7C9A8p-Rz0I75DR|e`9E3V0S$)^X;<4p3L{xuPC(8O*TAK+%2BYogI!85Pwj@ zsLRj!=mejw0*Cn8na+2i2QQpq+`DclJ9yM5Y#Zc0J-*bgbI(i7KVOC5Xum445Kvf% z4@R<#8GX!jXg+>$Z$(KZkCk<7sIkNRb<|7WNU=4Uh;fqCep^m_;n+#rmWA==WF;qw zd-`xaw%T8tsL9&VDD(LeAxp~g7>^Z0!^p%X&)ZmmUFg9cV>a%;eSiI6d25->FyqEc zU#Xsy{SyjVw>+o(#(B$KTXhF=nm>ImpNgcQQ_8wQ&pN^*C~;yNYlrL@j)xkVG-vZP8G-Cb%2e0ztU`hixhj^zI; zUKSpb%{G5b-hI5X>My)C;z-&hrjuuT=AeJ_r}M=&mTG7`nr_0!)O;;6Qu!xf(%D{- zaieAKsbx}c6NUK`ciH(*w+=RC*UpbJS>M{Xzfk9(uO7X~-?PYPOw{qog;X%dnH%eg zl6`B)*oni>Qcq*YXl{m4h)HeLk82@*d5=s9?tVkae}zdvep1c_r6kX&d%gk@X2zWd zLe}Gz%|2_w+7U1R-X?KwO7GzOvqi}uOr&J>BIkWNCt*&&w_&!(coAw~U#oDKrYO(o z1;?A4_=*y%9&T31(7yaUEvKj8*Q3h`Gm0B6H8AMhHy+H;iB-A={SM?{e4FK%Uxm2x z@@DEMu$)?x+npGuK!T0m0wP0wnqxwZ*#*}oLjkIisq*--4YhO4O*V@(WROQ)wbVo- zEm4#Ic`q_&%6WYS_hF)g--)1n%HX5~BOs_NFK)AE?yU`EmEcV}hvb&2V05)CzW0nx zJ4qRD!<~@LXhKf{{ipEwrAlw_D$K{@1$60L3ci(vX=JE-{Mlqq8ZU@n!xeu#KV2=l z?NoY$fVaTV3)@RQ=F&gCbffU#$5*bgYa7^Ci zFYB**u6NqMVndS4B_E2{{HAXoGiEaxpWhWRtHr03f0#p~e5o%?X0jf9D;wGd;bgx{CPMK< zNc!ayBw#Ah^QqK9k`J%bgxK**i3TQ1ROCJn_QMyC-Rty6(6Qhtl$^sJSz5@>g=fTP z4O}WA6Hi~7=xG_~%7wM60$x&X08TP9-PrT*!>43*R$?4}Ckb0cC8Fky8U0aET(o)p zo^yTk<~OGzNYJmseJR-soli+;iy{5+&~!G>+6m`Kjb81#JcVV1;nUNiPD!L#D~Ha? zP_qK1$1MjMg&?!t>p!oM+;r$5R&-=o#F@{SUH=*o!pQw~WZL*vrd8Heppx`rMpX#6 zvjB;4JuG#+Bbmt?s?l;ew}XdNbeldDi zJzrrvJ=cPC&%~+C$?v9K?!bL3kS9}VwUF})D*jb?K6Dv%fMAfkzYSBiam*uqYG$@l zNg2aX>d2RW`bRkuyz*-bVDntC-*SR1BS!?F^d9K4u*yC>U-J|U_U%NDsWBx^@8otP!!F%FcD3_A3>l~_)Y+N#KnRV}47^PO3 zG!2Y3=V>s;OjVy_coPL58x=ndzlsQkBQMH0*zv{O3-5*!+`BWJn#VYH-nsRrrk}pE z!RG3LEqBtlS~HWc+QQFuuUd>>-#`1QD{!hs>ICK|6KUqpT2SDy*rE>ClM<@d)WRs} z&l8t3cWXlsVPQTs`J!(dGtbCjCI<+U(jLwl&wP_sM>NDZz1sxbDAglsR5CQoPClFl zwAYjD7dS0I#K+`*5=^`j=KH83k-Qq(<$`&gAMG_c{=&6-nUEU|TUnBk8yx68d-!AKW_e)AS49ArVusH9IN{2){2{T0~n@!#mevra( zTe{|j!hL*3`A*OSXhwIw?jlV;?W$A=Bdvn?h4S$U#WbVLCj&;n}h zM2t8(6ObQ2DPpL=}27rqpo89%iY8@LYUcul`7FugOW`=%z;pBRn3y_uOjcIp}HTU`jSI)TA{d+#jtKOlUKi*e@VoR>we@j}zIDk9TEP3`_0iY{%2fY{1EeA@n8(m#biG9E0N6|cvf$(hBW zFAa`ny4UGEsx!bF@GFE(NS!QF>q^>A0sjDvO&GDy-#eL5b~iMgO6T7Ssz65yYpFxE zmgcA`p$Ik-dDhG5D~L+k&DRt73mQ&@@ZG;00>nZ7gM9uU#!%{dv#Xt%bz9+!dlxj} zTu%MrnV|Tc%}?{pCNb5Mo$R+n*}spmQiR19nDW*=E;P6Mx{r5CvX@bmzl6o`qp*EV zI@X^ZQ$wzhKh+XuREsg8eYBQ+e(gpA?I?Au9hE=LZ>x$p1xq!j<$Nxcu9mC^>r9o*le{c=jb*o zp5UJZ^~SqNpVz$$foGrxG-Xq6qR8>NeyeqDo^X0Oy?${G9EJklos*$HYbR#%RX$-y z$YIj@>`)ecVj@JnC!G91kF?91d=^v6D$oHAFtZ1LG_-aoiHxno2aj`>UZ5rAbLk`S zC9$>wBR`29G2{J{YJpsQl8*|p`w^)L+5?~x{=BPkRw22?d8|Ck3(KNsA1^(>E4xb5 zOny|Q?#8*E2nPz;!7d8b%@c2AHbPs)im?Od>Qb`%;%8&{Rqrf6?ADz&Z60J}u5}{qG@(zGV+Pj}#t29@VGDZlj(U+swZg=N>?y#+xSEL*2 ziAV)M&vua@h?y;$YQ4TQ`b@0T5n%6B+@qbP!J%w^bIcymZWrl0;9qI&#%mWS3On%z zSMkK$ESHZV4-&bMOywX&TlBT@&-OLwiQ3NSB&`1`Pd$0Hq;NpM#GE?dJTBS4QM=J! z?kdRr_C6;LCpzd&`t{V;S@N(}_UqWrQ0&%-ItvD%I~iH(W17nNrM&KpaPTQq%Bp)G z6~T)*p~^EIa0$*wMfQL<&e@+`GkCFLvAdd^qTDB`I(eLtLnOp=Iog*?Eu<(bPnS}o zsMrHKQT_cB-0Mk0azC8SP30^`XZ)IF1x4uT!(jK08y1*cQs;OBKhApiZy6#N(M}*K zrA3C&?&YtP=QRo#`e;WcX%{^|Q@~8u4>s`6#>mi2+LLIxGfN0 z{v6Qi-(9`SJ+{cwS4Y0gk^e?Ty48wx#ZYO?rt-uQwTyd05*U&9F&V*Sv+1R=<#FfJ z*NSA-s*hN~$;0LI3Q`1FM4lBggrTyrv> zWV47-Q%IrsrdKZ6yGWEBW~I8>iszC z(bj1qotD^`qUO*!V*3-*?HfPd*mIY;{?)JM3yVXVu{C4rKLR+iB~R7BtcD-^6=Q83 zhDn^-o3&5=e?2jAgT^P532!pwUJ$$^s@xZJNkr&qK0flIosPmD{KGVvZ&YSrk8z?t z2NoqneXG?yaf_`rt3QS;Y>QXbPUONkRv_sYfod&@V^@zS3ZW_4*(%ud`FNl;_m(z9 z>mcr{o{Vvo^FWyaPAf$wCUD`W_{D?VVsdP<{C&;Nqx&tP60W%q#4P0(OkvC{4KGOqb^{QIzj;x zt(}o1&zzJ5M>ZD;TJ=mvjSws!o)DVmZNsRoXj8@BYbV)Vq&q3~IzQQ=Q=<9wg#%N@ zu7=wteNzwiiVqZu!<_aB=#2ZyU`2333gblr@~kRfcjJeHufKEh4wq;h{|>%F8BS^A z{djW)v{tFwr96Yxo*s7)GwYvE3NW!7@jx`oJ=$i}D1m%QkXLHTpj@rMWvPzPy~^ag z;_+#?C}PJNy*gd_>io9Gq%dpS0@xx;x)Vg;tZ>I&p zuL!T^G)zDxy|F51I)NUZAx<$~)q)CF@WG1#=^D(IL?qn@2gdK6Msk_kQ_FX$R z;yIWs%!prm0d6WJ@=`PB*b4}S#aYfu4sdtjTA%3m)Y%sBIkaK+@KLCGAK zRhxdvk)wkWE^4C~L(Td8z$FTSYuUGx*eWWo($(*Og>Gp1(9x?u9)p;ZkP{w%5nd%N z7Vl1ZG$BH}DB+!vs~xX!sS3fez^T+RJ>B%|px&WNEc%yGUKyV2^e)Z0>fll~0P+#4zy6XM9i ztsI3LH*@E?aSgfeSzGoOEv3BWB}QIdBauep9Jyj^&=WgVZXy&XJ=gldzh$K3c4MLDDI0KILBq+9Ax9B0^LD3`F3^t} zCwK6%9U+Rt0GRA;#o5UBCcK)AXCG?vCDEG%D6lv?V!urN5|Jgpp-OA7kTx}Ru;N-bSBXP4Z}J9ueP zj^@tVOMA0pePdI);`+v~6#4Mn&^q00-hY3;^>Xn?NzDIhyqT!-*}cnZfS;}Ab?HYn zh30M;emZ?(I&Q1DKA8>GVk4zV8TVp(u;`PBEl6(?oGZi{qmTGMEsZ62>v+Zd1=#j4 zb7Qepf{WQz;2#phh$;=A`Fhg0Q+!l&Gx=xZ`Hxfd29~dxme+hi_~jp`8&G21=?206^jckpUK7yAN3-$A3)4Iv1BWrx`ufAbEQf3 zLv{|l@pi}Uin+CO`r{A1u=ItZbK1kcnlk?#X5y3f&2ewg39+ScyytRTPHFkA=-`m` z_tlJ|Xs{mfnQ(s{^;eCgm|>Jp-0ZOq9d$N@BI>OVvN=xKm898&++TKEfLQ+1ZMzwF zk_u%<7OoggWU47|$URlS8*@2ZkX5*uwMqzqv7=Ehn`IaRlx;JH`FZQ(xI1ZF8My4& zHJJx$49$mp{7IF%lKXneBR zeU)K0=X#!pU@l?sx1ImWt0$0BTaAb-YR2yT{Q)!my@ua*K19fHlYL&vRb8H_+&6j_DdsMW?Q*8hTGT@pr(H z!AKcKKO8vB?zWm2Z5!=OzX8%c_o9L=^{vr8KR(g)yXs$2%uT!CG;}MuXqc>gKeArX z{ZrnBo#J+*M|83{S7l9b8U zw%N|55N^KV5%%~L02E-z&Wp^dRM@3QZ2UL}se9B;c`PSTE#s;XiHVq#QI_5_6p zZBu{njLM|H&HPOHJ4)|7h1w$3y<6!9*_DmPDwlnntExn@V?MMZ+_+2BBXcuE| zDU(#8J?c0dc4PVz9{dD;`*Y)G^ z0k`-Fh_t%68xa>QI<%`m22{-Gw3)Db@L2rSSZ{(6t_iFV6R60x>3(^5(ggdw^fd@D z>P>fD%U&ED&{B+OmN_wbfFXUZ`(du4#lKIBH>C*CqRC+RQqmU=v?e#ZrAEx>0#f+% zVhC>@$%ffpuK2Wf6)Dza=CfVr-{1Hhe@a6}$dgJnW$0zSK_QE^?Xv;(Neu^SgJ!`g zjF~yv7YdGVG;?A>Xv^MlF)w9UNQnXdX&P4HUj-4q$MhCr>{cHwG z@67l-<-L~Qzn*z^jZeNe1monLm1)o&dw?3NDiP=4mWtydTFL@l8BlOtoagy{Cl}Z2 zs_r1b6(96m)uvR=4gC~BB;Rx(9Sjv=}IQlpb1x!LPlCSz7H<;`3ow$(lx|F?o$Sr1FYOm>rT%NuNe9BhP zawee0UJ4ckV{4z4eV-q7>gK8df#zMmzFt8o`_>&#wOaE-BoP;S;@0E$(n*X?G9yhC z&ip-@1|0gVe!9InzW-O(c|c>`|8XB(_Pin?^D-;4XU2ug9vPKYvNB6H5!pK-SxGWN z_DrPA>@7t$nPrqwN}kWJ?*IS!-_JSEeXet#&bd!F*Kd43>-~Ox&c{YAFWB>t_2tL# z#96=yXWl$HlERHfWscAoOe@$~yG_XwUPeB44;_<9f67Aj(r2H{ z$4`VkD%tM^PnfIi#hRb{N=?%Ak<;~a{2|iEej1bKU73X(R6;ljZZEt zp>AVwX(7f7wh1}HkVEwX+E$X{DL`s0jT> zRh^-rZf5ZT7eQftEO931C&;)s)fT9m9i2c*?iR2Vrn1>zgfLWTE+*MRJU0Syo+wjz zys3?qJthu7RI8D;lh*+uRbnY}0vxC`1eQ01AYmM}pEh+nv0r@qZm{PFdF>CS97IHS zR*bM@l_Y-NZ;U*o4Yf?bxrMlc_^xiusf?)Hvd(|(Wgx@Z6x+Jl&(3u@XK*9r%zv~1 z5X{pTAf?g2VfFBOd|&zLj>as}%HCDedbQP9#a3-!IMWZ0ctqkZbrS|DV#)D&-)RgN z-D>GTfGuN%66PUUCKDO)wKSbR_evEeMyQ3V-Y!;wc{La-MT3kYw9f}eO!bfFe;mBS9{WGJjIt#_F$YBHH}By-j0HvQ zHPh`)N<9OQs6Ooj$hgm02`1y3pO@}@pNm)nf|K}=(y+D1(3@fYTPkV74XtBT0MyNn zRg@8+AONY;@zwYitUx0P!ELp`f+I-A`Zh#|urIcMS}}n7F(7KSI5*YNR-Tsei~z<5 zh6ay345}NX6fQYl5?mvQ>Kx`>UvFOf^*0zo9kE1ok;PsGbiXWG@8Af ztS|||qcJI}T&#^M)6_$Bln)$;-}m189BPAJDDQn}hgk(BvQy@OeaqK7XCu@YlDoQP zHP!IGpsyM~-_Z@652`>+{wEP3*plKf`0c``*{(ec6LR}Dl-J_J@KJU{X-B}HjtGDF z)*z~VkhQu-C2aMoFQ}t~?r>t|=vN(I`D`)sKBnOCjy>sYAPZK*V}>C(IjwUug21Lo zK}Yi5)Y;s4bhkwROoF}C^Xt<2xi9zD-(NsB@@=>cmrU+T{&=fDaUm{{=d{SCK-sIW zn#21kFtk*x7*Flxdte!IMT?6|hw!>#2AzT9e6ez;f4l0-YdWIt=TB$ECR|i{RQcso z>gjca4daR;6$-p=Oz}m&n(KCeo^Csj-UmvSKG-a$5ZB=lPGO<-?^*BR|H=He-3@mYubv61nt>|VhOTcZ-Jj#y3(KMMpzaxG@bE;OEnc# zh!eGNlRH~C^fW(F*TavDkX;;W6{!(fZcaDYzegIRo#qm@k8(y)$*+VSX-LjmDxrp$ z8n|yY?j}nM4coHkbGU(3+Xeta47m=V;^@ZZbGVa?P+9XBcfJh9en7L68K;jVuXJu7 zT^SYz(H%4KX&X@h#u!4Tub8H{u(dt)!m^df{hWg}O;um2urfHO3VYW-p*GR%hKTn+ zPa>mk>CNwhqF`}!2TAQX=(=RyyDKDL$Gt1&R8J~_3hnw;gV8q)oad(+ya!@9J5jFj zu89_tIvr2Xo#A8j(sj#~Ye^Vn}L5JOR!Zv?|NjLkGFhZ z@GWR?JA}&-g0tp>Mm|>AF-YC)GL##XPvCdjM4Mn-T1a_Q`%u~^e>{?bB^HpTvk~M5 zPMlOTX1OUxb$%OK8kbDj-luuUpkLdNpuaS)wyOJekNS{uX>kbq({=F}|7?qnz&rKM zxhjrrHKX%-!k$*6VXdkr`;di`jVw_2ntt4cQ?)NV@JXM*rqY4a>sU&3Prb%s2Iq}O zT)8K!&@|%xWYaU!TsmUco@CP0Xx%*7{$cSlGD`l`>tkH?;O((XF|l@`fw@t(C?kst znb2r;oTx5jRJgWyhvUx~SB-W#AG3;4Jg}264XO2yR7T+0H9p%``C5@8Sr+?#IKHc~ z!M?0GA@wM|kZ1?|n>mk?dk5oetP>LHvzxY9jjcry%GTxd?b^1yodo+E6!A5|=!bWJ z#_3PV;V>}T4_ZVsfAIg`W;wh2@<#j?Bcr5qhM`Q-dtLHDsZDkpBAOH7g2x12^QGg6 za_ZI?`x)n{siMm)dAKXBW2&d9*}wjXI^$Me)vY^>V0$_Ve2ifQ!RM8)+!xR?;vuyXzjyFP%!CnK41;P75UT-H~kuYl=+gI*yz+wVO)Q8-5?HKTMLP%*1Hgi-kuf z@rhhiQV-$yN^&o^S2N5&17mPH@8YlppE9~@l3l;+N{e-YPY=!4yYlNlHos;5++gj$ zvp4>;p>+L<+l=2)_<;QKBmj-Zyb0UY@8;cN>$k9+S5_Q%(*GGR$`D?Z(IR(zJ0|Wx z-%!~9z~iS?TtD}6NPEm3o7kkMuwBv2m$)#va_czzE|Fb4oxmDV;d(+;AW0;ahh#AR z6fb6ih?=!HX;-`Hf(2Cu>wr(4HaYEDzqL54#C#i_%`bs$St3pySfqfnwpZ=$#n31ZEhS3(Brg`&(L&yWt! z%$0hx(6bJT$x1$&_D;So{c=HCd^8Pwf;?!k{6R(@{uVGdM#tAno?T0WFWKUaRBW)NSK3AEx~&SZ96euvc`T`siyYrSvU|A-vdX?B;#p;e`T* zt`-$`Q~Wp&Hd67>^#i9Dn0N<*op+yZ>3A?PWRv=`r>&mdp7~Mm4wp9k^Okm0T- zenl!_OhJsyikktP=g)WjDvo?X!?8<&PHVwDDKuxvQy9HC9PuFYT@oH)b9b_U)5k1N zB3+5+%a&C~55AFo?5~-rrd@&AN>u+A=AKx6!X5CBa4KOOu0`@}oxmTpm&>`3sExv5 zQNxM|@Vyj_?R!mK?x>!JXYJ5R(!s0j<~ym?Ga!CBaUi}S-!BINrXTMvCY+n%Gyn0vi?87ee z#%k&u7P=u*d287Zh!?@IGUWzUa2%n4*7w(0B}|(t1oW07(jk`h6Y?L5GO%n{Py&j` z^&n6Wq@XiZ)+0`xfvsPT=oUC*iCva|Pal{3XxL%rPdrxp4 z*dWk46*W-@8}A{R*cHYn;+Sehn!trQvCR&k#F`BTlhkH3Yrn1%1H2@>AAdev>VYyI z&eL^@h_$nrUSPsSj7Ct)&Ts>j!WunNcP{hm^{C_A#X8{pZCu3Taow%T^XT4ng87^O zSm)=ts*`lB9Bm&sHJ2?#Rft->+H0=;dF1iYot6aT^!()qtld-xyX!_4cM*V@G|tNY zoL~=Y5M;(`&ZSX~`uocf>g;68;L9W!gS0oO*I&e0zMSx#u>g_W1?dmoA37&UN7JU;rtW-FDrtox;B2sjZ`-Y)%K?3*7@WK@lzz}i#6+MXsxr{RjK$eu8@anpGz zVgyeh zr6b_%M%@S}`asfh5uj2?b9rBqah8RK@$*Rj{TyENof!nQ@nOniM5oq$m{Om5H~sC1 zI_ss5iyuMMyNbYN871yMIJh`)vU*!H<=mG#i4Ta>qb3AdL)(Ar!HyB;iBvF{kvw@H zdl>X@p3bD~WQ>hSjyv)9V8chNpT|Eayb8mdXX)Rel3Xk#$5Ge>awl2n+8=qv-g#r=o+Z- zYNyW1|JqXC2j>b=3*%x#c&#YZ;H28sqM6<~8O=WStH%F4W;6hJ&Jwf$UT*I=C#Ap} zgjzkOtoI4!(13DaNwsGa%s2$^OFb~szAQ5Ml1EC0Jy@{oO|?AS{Q42lyI=elq=O+l z@`PJh^=z;51L>g0Yj{$}a{WP%1Foe$0p zgC%Iwh}R`yPF(}~o_1H^5m|VQaq3W0(~(mdxFJBaya^7Vau3b)>ILKF`UH&WQzgQW zL}(jkr2u;R2citgXCHI^%uQGYm*qpwqIV)HSmfwP{Tu=~~4sH&h&UNyJ-JhNa_5=3m>C0ZpA`(x*1aGB*t0YG-`&w@dMP()| zg6X_DUfC|FrRBo{d<{1Mv6g>BCp?XCNY(DQNG+|suF!w_v2#~sq-ar~Yki@u zog)pG7(OC6B$mh7Q=;Rb4ZNK8Fp|>KN-@4*fJRrxkOZGOf8{_XI|MW{(bdXZoKlW9 zg_esPU7&eoGqF+}!WulUx(VYXXDxwE=pWBP<1zUQcb-iE?p}+d8&Rw8C;!?_`|EWu zTYU*?*Ml2jrg4A2?kf}!cW+^6cSvGOr>ru-C2ewst^276T0Oi87TD;-+QM`)!O~u? z(r0_#CS0fIWv&98+!Jr70ZE6fxzWB_!^he8@Q~&1uWc1pwde(ilD3COiNYea$o_7n zh!4=W)fr0Jh4rIyX@wr8)FhpZJIyMC$1?yknQB&U5wu%8M_TzOY~$GXNoc~hoo@hl zI$5@HG21(&_XwaS8GffmqenC?!2RjPMsu^^F7%GX1_soh?0km#lH|~R`&L&(ifj6w zX?^w%51=8?Klth1i*VLYh4DUfhe+8jHU-W)!$hhcITQVUb~$$R?fI%WgLPr=lVi*=c_WXZkiVa4eFy zcvaO$ARt1dbx`EgxwweC*I2EW#Nxm_)8Ktejm>HR=$bh1F%!G-qr}oqKPr`p8y`Sy zygm)@QKYZK4%14P+C}D1liJTp-p==O_pE`1zl!2qsI!j{9i3hL`Uwf)v(h150ZbkN z-6Vr;JD?UVCvX|2Qs4myMYf15zCP0PNo@`~@}wMCH?vG6SK-4A>8u zdGY;gax@&Kj2AoR5zPeEX9W_Bh`HE9^Y^(`7c!)Vo@Ou+k$0SHb>Vh*xu0u@%NM!g z-X~rP2!FJ=5LwqJilx~w0o$&fO;R)JC$m>ozVOAO)SF}CoI#%OmKlrTgn;14kYHmo zP`#M7e1+#Gu$!tUUfp1HcrJBn=+mF;@q;Gp4yWu-W&%N z^X6tdv4!WTS$N4(;(q!v$uI?TO*iZdF+HECeacFFh=~Xnm;Fu~ZOABQAFo2uYMXoU zYxm{Hwr6g$fo*sL9Gxymuk$0sNZsePg&94!th*Tb=cCy#^%}C8*ph68M&%2EvHM1w zK9@j9Qtpj+1U9zIbLrfscv#8Ax%$0{o6czLakkyJzZ`tU_EcNUc?ec9fd$ca*Y0eG zNt;3Bu9;}?ICc7dB&#JZQl0XiGw~2~Cp*&1L7qQ}O47rp%yFGrYIO5IlB&iLH;xXn ztH{MPNVpNjuwR6L`T{yqe0e2x-ulPiynz`BXJHm})hiSD+Lp+Z>lK*_hO;i|Dj$#c z_j2@bznVmJ9p2gB971;e%nkTFI)Zl<#9pUpEK^&f+7yiVn9nNW!ik%Q9J+EH42rr# zA}^|v0J%$aIQ|~=!KT1|0^S!JzX&w z6YjAqi|lsgkwtGGuo^JlCdl=u9evYSDEai_kJ6EZ(B-(p@{(v57 z+L8X|n^pq=Da3H>_3aGw3S4-+f)@6ne6NE*#*jEKZ^n1YfI8AN=ly~Xze>|Bln=YM z92++mj&K=>$~(thZS>AOWHk_^pZms=tJP-A$u; z1_C<<)|2k zg6{pS>~zKsk&`zCBQrDZeC?`WF15z7ucJVDx`JTq1qbl?Z|G0X>V@cB5W4|91XtOM ze_JobD7`B;XI?idyF?Q=#GDkYC9ynrYGbh2r0NPt(fQ{!P!m5hdq4TNS9(c@ROk&0 zn{YQhaL!h^GrlOfW?Gj9GUPn4WW#_=4K_qL~D&fBUhSq~!BGhdIh09%lUNhO_G4t*Undy*j6Fy~q1!5rAQ zZF(@5Ho)b0#w7}BCf5S12OGGyGRRQJ_|4#XW+&*P!Kys`DKhMV{`M5b^`6o{8vhuP zF|~$)ibJ7txG96SL$+mIxAbeZOq(3m@Whjn*l)`N-eLtE_U0E@6gQ_EIU8`l(;j6) zteK#7NmC%fy+8Ic=hJb?0fO*SlHjxU=e*UF(A7I|bys*9aOm6GtZYd@X(b_bofFtJG*YCQvQVSDslumY>8iWo3K$ zuvo}o`W~6SLTGJgWD{wzQCa3}G!#EeaS%D?&mSnctqMO=`)*7wwZ}x5+JDLE>O4reYZRuG{ zoNF!2?Owvreqt!L0sJJTkbbv6&YnTaJ**~ae6nsh)wpr>ckBy}yL&pCyuGs)D0)6> z4nkysUwP%j75--w=eQRPyBK?=+;mc%3FySWd30!LuHsVr6GOe+J5a zQTOg>aFVRXt#i*EJfUyElC6&Wx#F(X$hLb|1gVHSdGI%i^xX3^U7773lH6S!DxVCc^V{}7s^?oI+Nna#NaG-m_;U46j8f|Q znQYc8m|&axUjRcuRZ%Jwh3;nD=~L4t*O0_d;&A;$?g3_P zihUt<>@*+p%(Na61{{O%QeyA}DQ#`X3d1N#4 zSRc_FD^8fXzE|(Ja1Gu0*@X6iv7-PXyXn;MN3K;WFzi=m+rR#7(?!bwWU=*+FUly} zbjo-!4}NuyvossB^t~Y)*3u9JWQVtxDSP*e!b+IY)Prw7&#?~E!C{qrW?AwfxvEPT z??`67%b5;c4}MfpokZpkRoG_IueZv0e`@SSGJLwJALq`R)fU%0#FkS&t;tGVrWp2M zb!`!PcO8=U?grzZkui8|zRu1uYxhKflyT~TAZ(K9KF-?1t~)XO z{OR%~AF8%HJ&!s`tX`jB&mXH1K>C`c6th_9V5U`+bk zSawr_ZXH7Q|7j|%){UKPtLct>dL zxWS+6i=jD3V57C2b#J<_`fBzvt1p>sAbEDep5{jiUn4j<-YZU8J@rcj)K*VP-&>_X z)2xlD1~sZrGdt}IS}xG4-unhK!122pg6#!zi&~*colI69&Y%ULr`EtJUKDq16VaKy z|Fo;YPF?>!tlZl28zcJLEyA(CC)p~06fqf@fdr%>-iOb=@A2Go zS@{2a9W2U<2g|hu0z40D`R`dd+y+qVehzcJ*8Jw^`vSFEGBvsqp3@P8NiTf=NeuC@ zGQEoZ#-m>K0q$+Ab8!;hyTu9X9hoG~3O{x&xlz%ELyE=|uxQ!Gu3k3{=xTYxvN z)NUa2wSfFHIaQ=p$YTtnDJNQVjO-C~@UBKsv0&U|nZ&zySB5w@q&z2S$*O5}(;YIP@ZE1~#GPl33y=TA~Hk@Sk-+EtwX-H9aZDfgoO1 zO3sQftn?K1T~EhzAQU^%f#0&-9hy*&E-Eppdb_RJrk=tFb--iON*fLD@#`tl>4Uy+ z*%Cm5QwIYFwE)#ny*LX@wdddFRUk=jL=zl2edxta2|3dS&vjdLlRf99q?r*L)(9c~ zF<8aA5B;HrfWrf6si)M<19h9E&rgdmjGis>4-3cBX-4|Oz+5$5$vjP>HuRgP1lT^BYYx)DORoiECu-#(=Za7P&^%LSJs7^t_Qpg(0x;7)-}+Mu0n*1jj+|K zo#h#l5pZDxZ3lnc~T`_Bt~H33dMu&Zomd?WYu1|@|cSNGkcf7W~g48}BFim=*gidh_udp@+)7=rtTv28aBQF)x9V|`S<3!>Ye2zGTpzgv* z$G6<;cFESA14`Ds54XX!V)7(Yg4_dNxtoUPD^vH8Dzp~S!8)!2Sh>d zph23U#=)X0)yr3L8K95Ic9pm;l?S4!F-AI0^#7UL{25C#kj$c}n572&VT!@CquOTy zPuG=>I2M)`;p)7nx6HU)2MTJ-)WrzR4jw_NqSFcV~ zSV!DMMEs)3QlsU$uTK&Sr$Ed8+*N^+OGqpBzn^(_WKFe6iSYAOh87_NhzPXCXEn*{ zZ|%K93dkDC<&^LFz6H81? zuVR^D`uNDESfuJlQb*D#nt2M^+j-O+ho_=q6P2Qm1;B=$PdGm+cvkPEI9~YsApQOK zQ*P{-yeqET)U(^0i4Eg7dDWI`ii$LCR z2b52`bZ~*WJhVYe!M1kV9W}lQq~eErlj1vtHj33;I3?60}`aSYi7FTpm{0`n0& zn++SD1kqFqxyNV!c_05?MtKgbKl`rZXeEaQbU{*#*F!u$04B;p+_i(*J+=rM-7?SA zeY8jA3mw|5k9Z7qS(9egVQZy=BFZX9ZzIDpXXKiIzEqzqg}t9 z+l3hn$$5o@OilzfX)PzW(DAphQgjvDGzh?%QVsZppsTw!j+M^Few zt~P?1bFx)$g3V!Q5aZWxPm;S_3+H=4=)nrK>{94e3mGoSCA?A2IKW1K8cMNH|{qI48fA@Hkr@Acb)qcXrn7R!*D)q1{w1ZjG zm*3z$F{ckKgsX4jNOK2T-ChH*S6>2%3%AF-&J?M;6b$z(tSDC;=`#HHgT+VLKi+~y z;Y+w4Q;3)>+catdP8xf%R2cStbRc<~Pn5vNBpy;IF?`y!MSX^gsl$|}*#x$y7Q((%(VGKUXY>4&CJXb(nS4Q;nQ7`)aNR z#^!htfN;k%*bivu#Vn-^V1${H6A%~RFIEiMLgPEqM6am1m0S!#_=w7Vd5o92DU#fj z2x0`7B2{&v%J$rxmfQFdi2~l@BizKa=z&9SzeU)? z=Yhsif^NK4(soy3>&3N``JVq3mHZw0aG!J_fDYPWXd?vkUf$Mk)IB%5=QX-bNAmpU|gk?LNjp!&h15R}= zHyDRLi|sZqz|RfX1S*@3eLobj^$4Zk5hFY-59GFWUGS(Vb(q1nMl)C!q(T?^CPPp%pw(Zu? z`F^9)?b3wsT9q#BW+=AVAY;gcGTc2+T2Gr2lDD&;(O18r92rjXeujZWZM}LdfJ7J< zK+yGQuo{w{*A0L{n}X-}X0{0EF5SUggVnZ1H@Ds9J|Q*{w5Q7xMsIflpKaLDy*csz z`clie8r6Rt03_`A!&nFW<{}7JJau1z=^&HAx<;yjpQmPFsXFx$_^pnGc{rMd!do$M zvF#!BYSD{0#~-q4@g6#(@;NMuZ*!l@32?ZO=@Wwe17z{EUPKO(!m#(Fh!JS9&sYE7p4hBBfKp@Grl}>z{A1aM}r(EsB@@&nW<RhK2Ug@34yL&Alzo5Lr1uv>J;UJ1ui;U+ePr{`s5TA#^ zFkU~$yOmNhl{0*>56CLLirS+P+!uYgtv=U4B#+8i>v3PCIQUv=um8V^114S;e}Xr6 zm`0g{Oydwu)>fH3LjJc}!#k2-ohyWwyNLzQhyP!FzS?*inmS&w z^NzJ!&MB+Gvfw@5f4zay*uY38P!-_NP?y>2^cL|vzYOb|WjjWe@UQpJ$KKYkQyC4f zHZs+}0VhV?2-z>Wc}d7Yt7gKkY8fUphfXWWoHj$jdRT!M5I=6{l->4M4Xpm#azWk) ztSxDLsWhNQi}pD$(M6P&(+#XVoTJFbY2vn_2(RS%3I;YWv1-A7sgpscd=DF8lI9GX zk4xc%T4GOBWxCH%yC7W=MK;tX`sB&|hoURXMYQEL3CFRV131dvXJE6%42^i20;zPYcc)n^pYs3fGk3^U9m1h~5*!U7c4mmPvppW`A zQ=JjnUcuRV17rvi3WcSh%Z{>s-+Ugz^Hyo$E$`4t=gNKy$aJ+2jDdlI4L1GT@IeOu zw+V2o;S`MXh{8{fN}nk2_8om_Qc1)t4P6t`&$-zOd+aTBrbxwLk8_Lf(U4a#pQXN) znArU{u-u2aZ&`Say4BfHhHcLq8@~TUJDsol5>nf3MTp7L;p^zawIyQ0)X36nQ!uF%7W?ybpU$zC=jXkfcg2y-1^_mdAru_6{}= zACI>w6@epLQw7}xrnYVh~Ja#VJF0+747gQL7!6jr@j3J!di?AvhQQ3&|u^HY+F z$;4dBD%a<7v9WNF$+yf(sLjp#ki~0LNcnc%1MJfqV5g~0S28eY+(UC~C3_(LEm)`I z0+v*ZVRvFnJQbo4mlHL_|N54lLP3B6e3$KpJB(~WAP!Xi#1xh(fRr&_C(Xb*9zHiJ zbDkHB?91(;iF_Kt?!<9(7jU?hg)Su2kZ(!ChD)=E&u>5)viA5-ar)l}5ED-m1c3fD zdve{AHQwfQRK$ZZbYnn@_FNfko&wTKsdkzOX+eCX@3IHWmp<3t7`IAjvcIlt zDoG=~vqt7$eno(T4L>hxO@QNHCl36^99&xp9FtYG{QvKNc*+ZElAT-l`2S7<(IlKO zQnq}xHc|CIk0)}ttbmw@;?N9>_pe}vMYX77`z;vC>i7PiA3@_w;6|i;L{EeFuiHWs zBy=3~;%|#e<^ECV{U_@S%?!+F9(+TH`kz`9`C06-a1X+_pF0iw-#hT(h*$6ty#R8h zUuq#;fc~oQ022M~Ec)@qEpMpCcc$vCOa`F@B$NFN;q=VbGGIwvAyu~HS6*lMbD4qy b(ckY1CKBb{BMk1rgMTzsbaACh7Qz1qcq9*r literal 0 HcmV?d00001 diff --git a/Demos/Demos.content b/Demos/Demos.content index 2a09360b4..c6834d717 100644 --- a/Demos/Demos.content +++ b/Demos/Demos.content @@ -24,4 +24,5 @@ Content\Sponsors\spide.png Content\Sponsors\handicat.png Content\Sponsors\raisondetre.png Content\Sponsors\ifiwereatardigrade.png -Content\Sponsors\tootbush.png \ No newline at end of file +Content\Sponsors\tootbush.png +Content\Sponsors\bedtimeforopossum.png \ No newline at end of file diff --git a/Demos/Demos/Sponsors/SponsorDemo.cs b/Demos/Demos/Sponsors/SponsorDemo.cs index 1a7f5c835..98193fe55 100644 --- a/Demos/Demos/Sponsors/SponsorDemo.cs +++ b/Demos/Demos/Sponsors/SponsorDemo.cs @@ -81,6 +81,7 @@ public override void LoadGraphicalContent(ContentArchive content, RenderSurface Add(sponsors1, "Creative-House.org", @"Content\Sponsors\handicat.png", content, surface); Add(sponsors1, "vietnt", @"Content\Sponsors\raisondetre.png", content, surface); Add(sponsors1, "demiurghg", @"Content\Sponsors\ifiwereatardigrade.png", content, surface); + Add(sponsors1, "JohnGert", @"Content\Sponsors\bedtimeforopossum.png", content, surface); //These supporters are those who gave 50 dollars a month (or historical backers of roughly equivalent or greater total contribution). //They get a larger entry, a bit more text if desired, and a physically simulated entry in this demo. From c9d0bfbe60cf78f394cff0e9696ba23cacf9e935 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 17 May 2022 16:04:40 -0500 Subject: [PATCH 484/947] Some missing comments. --- BepuPhysics/Collidables/IShape.cs | 129 +++++++++++++++++++++++++++++- 1 file changed, 125 insertions(+), 4 deletions(-) diff --git a/BepuPhysics/Collidables/IShape.cs b/BepuPhysics/Collidables/IShape.cs index 4636ba0a5..e848c5591 100644 --- a/BepuPhysics/Collidables/IShape.cs +++ b/BepuPhysics/Collidables/IShape.cs @@ -26,7 +26,8 @@ public interface IShape /// Initial capacity to allocate within the batch. /// The set of shapes to contain this batch. /// Shape batch for the shape type. - /// This is typically used internally to initialize new shape collections in response to shapes being added. It is not likely to be useful outside of the engine. + /// This is typically used internally to initialize new shape collections in response to shapes being added. It is not likely to be useful outside of the engine. + /// Ideally, this would be implemented as a static abstract, but those aren't available yet. ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches); } @@ -71,6 +72,15 @@ public interface IConvexShape : IShape /// This is because the most high frequency use of body inertia most naturally uses the inverse. BodyInertia ComputeInertia(float mass); + /// + /// Tests a ray against the shape. + /// + /// Pose of the shape during the ray test. + /// Origin of the ray to test against the shape. + /// Direction of the ray to test against the shape. + /// Distance along the ray direction to the hit point, if any, in units of the ray direction's length. In other words, hitLocation = origin + direction * t. + /// Normal of the impact surface, if any. + /// True if the ray intersected the shape, false otherwise. bool RayTest(in RigidPose pose, in Vector3 origin, in Vector3 direction, out float t, out Vector3 normal); } @@ -81,15 +91,59 @@ public interface ICompoundShape : IShape, IBoundsQueryableCompound { //Note that compound shapes have no wide GetBounds function. Compounds, by virtue of containing shapes of different types, cannot be usefully vectorized over. //Instead, their children are added to other computation batches. + /// + /// Computes the bounding box of a compound shape. + /// + /// Orientation of the compound. + /// Shape batches to look up child shape information in. + /// Minimum of the compound's bounding box. + /// Maximum of the compound's bounding box. void ComputeBounds(in Quaternion orientation, Shapes shapeBatches, out Vector3 min, out Vector3 max); + /// + /// Submits child shapes to a bounding box batcher for vectorized bounds calculation. + /// + /// This is used internally for bounding box calculation, but it is unlikely to be useful externally. + /// Batcher to accumulate children in. + /// Pose of the compound. + /// Velocity of the compound used to expand child bounds. + /// Index of the body in the active body set; used to accumulate child bounds results. void AddChildBoundsToBatcher(ref BoundingBoxBatcher batcher, in RigidPose pose, in BodyVelocity velocity, int bodyIndex); //Compound shapes may require indirections into other shape batches. This isn't wonderfully fast, but this scalar path is designed more for convenience than performance anyway. //For performance, a batched and vectorized codepath should be used. + + /// + /// Tests a ray against the shape. + /// + /// Pose of the shape during the ray test. + /// Ray to test against the shape. + /// Maximum distance along the ray, in units of the ray direction's length, that the ray will test. + /// Shape batches to look up child shapes in if necessary. + /// Callbacks called when the ray interacts with a test candidate. void RayTest(in RigidPose pose, in RayData ray, ref float maximumT, Shapes shapeBatches, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler; + + /// + /// Tests multiple rays against the shape. + /// + /// Pose of the shape during the ray test. + /// Rays to test against the shape. + /// Shape batches to look up child shapes in if necessary. + /// Callbacks called when the ray interacts with a test candidate. void RayTest(in RigidPose pose, ref RaySource rays, Shapes shapeBatches, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler; + /// + /// Gets the number of children in the compound shape. + /// int ChildCount { get; } + /// + /// Gets a child from the compound by index. + /// + /// Index of the child to look up. + /// Reference to the requested compound child. ref CompoundChild GetChild(int compoundChildIndex); + /// + /// Returns all resources used by the shape instance to the given pool. + /// + /// Pool to return shape resources to. void Dispose(BufferPool pool); } @@ -102,21 +156,67 @@ public interface IHomogeneousCompoundShape : IShap where TChildShape : unmanaged, IConvexShape where TChildShapeWide : unmanaged, IShapeWide { + /// + /// Computes the bounding box of a compound shape. + /// + /// Orientation of the compound. + /// Minimum of the compound's bounding box. + /// Maximum of the compound's bounding box. void ComputeBounds(in Quaternion orientation, out Vector3 min, out Vector3 max); + /// + /// Tests a ray against the shape. + /// + /// Pose of the shape during the ray test. + /// Ray to test against the shape. + /// Maximum distance along the ray, in units of the ray direction's length, that the ray will test. + /// Callbacks called when the ray interacts with a test candidate. void RayTest(in RigidPose pose, in RayData ray, ref float maximumT, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler; - void RayTest(in RigidPose pose, ref RaySource rays, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler; + /// + /// Tests multiple rays against the shape. + /// + /// Pose of the shape during the ray test. + /// Rays to test against the shape. + /// Callbacks called when the ray interacts with a test candidate. + void RayTest(in RigidPose pose, ref RaySource rays, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler; + /// + /// Gets the number of children in the compound shape. + /// int ChildCount { get; } + /// + /// Gets a child shape as it appears in the compound's local space. + /// + /// Index of the child in the compound parent. + /// Data representing the child. void GetLocalChild(int childIndex, out TChildShape childData); + /// + /// Gets a child shape from the compound and compounds a pose for it in the local space of the compound parent. + /// Useful for processes which require a child shape (like a triangle in a mesh) to have their center of mass at zero in the child's own local space. + /// + /// Index of the child. + /// Shape of the child. + /// Pose in the compound's local space that brings the child shape as described to the proper location in the parent compound's local space. void GetPosedLocalChild(int childIndex, out TChildShape childData, out RigidPose childPose); + /// + /// Gets a child shape as it appears in the compound's local space. + /// + /// Index of the child in the compound parent. + /// Reference to an AOSOA slot. void GetLocalChild(int childIndex, ref TChildShapeWide childData); + /// + /// Returns all resources used by the shape instance to the given pool. + /// + /// Pool to return shape resources to. void Dispose(BufferPool pool); } + /// + /// Defines a widely vectorized bundle representation of a shape. + /// + /// Scalar type of the shape. public interface IShapeWide where TShape : IShape { - /// /// Gets whether this type supports accessing its memory by lane offsets. If false, WriteSlot must be used instead of WriteFirst. /// @@ -145,13 +245,34 @@ public interface IShapeWide where TShape : IShape /// Index of the slot to put the data into. /// Source of the data to insert. void WriteSlot(int index, in TShape source); + /// + /// Broadcasts a scalar shape into a bundle containing the same shape in every lane. + /// + /// Scalar shape to broadcast. void Broadcast(in TShape shape); - + /// + /// Computes the bounds of all shapes in the bundle. + /// + /// Orientations of the shapes in the bundle. + /// Number of lanes filled in the bundle. + /// Computed maximum radius of the shapes in the bundle. + /// Computed maximum bounds expansion that can be caused by angular motion. + /// Minimum bounds of the shapes. + /// Maximum bounds of the shapes. void GetBounds(ref QuaternionWide orientations, int countInBundle, out Vector maximumRadius, out Vector maximumAngularExpansion, out Vector3Wide min, out Vector3Wide max); /// /// Gets the lower bound on the number of rays to execute in a wide fashion. Ray bundles with fewer rays will fall back to the single ray code path. /// int MinimumWideRayCount { get; } + + /// + /// Tests a ray against the shape. + /// + /// Poses of the shape bundle during the ray test. + /// Ray to test against the shape bundle. + /// Mask representing hit state in each lane. -1 means the ray in that lane hit, 0 means a miss. + /// Distance along the ray direction to the hit point, if any, in units of the ray direction's length. In other words, hitLocation = origin + direction * t. + /// Normal of the impact surface, if any. void RayTest(ref RigidPoseWide poses, ref RayWide rayWide, out Vector intersected, out Vector t, out Vector3Wide normal); } From 3162c73eafe9573ed64f02682cb5379220371fb8 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 17 May 2022 16:31:44 -0500 Subject: [PATCH 485/947] Purged a few unnecessary refs. --- BepuPhysics/Bodies.cs | 4 ++-- BepuPhysics/Collidables/CompoundHelpers.cs | 3 --- BepuPhysics/Collidables/Shapes.cs | 2 +- BepuPhysics/CollisionDetection/BroadPhase.cs | 2 +- BepuPhysics/Statics.cs | 2 +- BepuPhysics/Trees/Tree_Add.cs | 2 +- Demos/Demos/CompoundDemo.cs | 1 + Demos/SpecializedTests/IntertreeThreadingTests.cs | 6 +++--- Demos/SpecializedTests/TreeTest.cs | 4 ++-- 9 files changed, 12 insertions(+), 14 deletions(-) diff --git a/BepuPhysics/Bodies.cs b/BepuPhysics/Bodies.cs index fe602de5a..b279b5831 100644 --- a/BepuPhysics/Bodies.cs +++ b/BepuPhysics/Bodies.cs @@ -132,7 +132,7 @@ public void UpdateBounds(BodyHandle bodyHandle) ref var collidable = ref set.Collidables[location.Index]; if (collidable.Shape.Exists) { - shapes.UpdateBounds(set.SolverStates[location.Index].Motion.Pose, ref collidable.Shape, out var bodyBounds); + shapes.UpdateBounds(set.SolverStates[location.Index].Motion.Pose, collidable.Shape, out var bodyBounds); if (location.SetIndex == 0) { broadPhase.UpdateActiveBounds(collidable.BroadPhaseIndex, bodyBounds.Min, bodyBounds.Max); @@ -152,7 +152,7 @@ void AddCollidableToBroadPhase(BodyHandle bodyHandle, in RigidPose pose, in Body //Note that we have to calculate an initial bounding box for the broad phase to be able to insert it efficiently. //(In the event of batch adds, you'll want to use batched AABB calculations or just use cached values.) //Note: the min and max here are in absolute coordinates, which means this is a spot that has to be updated in the event that positions use a higher precision representation. - shapes.UpdateBounds(pose, ref collidable.Shape, out var bodyBounds); + shapes.UpdateBounds(pose, collidable.Shape, out var bodyBounds); //Note that new body collidables are always assumed to be active. collidable.BroadPhaseIndex = broadPhase.AddActive( diff --git a/BepuPhysics/Collidables/CompoundHelpers.cs b/BepuPhysics/Collidables/CompoundHelpers.cs index 136837d3d..42de8cfec 100644 --- a/BepuPhysics/Collidables/CompoundHelpers.cs +++ b/BepuPhysics/Collidables/CompoundHelpers.cs @@ -198,7 +198,6 @@ public void BuildDynamicCompound(out Buffer children, out BodyIne /// Computes a center of mass and recenters child shapes relative to it. Does not reset the accumulator. /// /// List of children created from the accumulated set. - /// Combined inertia of the compound. /// Computed center of rotation based on the poses and weights of accumulated children. public void BuildKinematicCompound(out Buffer children, out Vector3 center) { @@ -228,8 +227,6 @@ public void BuildKinematicCompound(out Buffer children, out Vecto /// Builds a buffer of compound children from the accumulated set for a kinematic compound. Does not recenter children. Does not reset the accumulator. /// /// List of children created from the accumulated set. - /// Combined inertia of the compound. - /// Computed center of rotation based on the poses and weights of accumulated children. public void BuildKinematicCompound(out Buffer children) { Pool.Take(Children.Count, out children); diff --git a/BepuPhysics/Collidables/Shapes.cs b/BepuPhysics/Collidables/Shapes.cs index caf429149..86c581ff7 100644 --- a/BepuPhysics/Collidables/Shapes.cs +++ b/BepuPhysics/Collidables/Shapes.cs @@ -383,7 +383,7 @@ public Shapes(BufferPool pool, int initialCapacityPerTypeBatch) /// Index of the shape. /// Bounding box of the specified shape with the specified pose. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateBounds(in RigidPose pose, ref TypedIndex shapeIndex, out BoundingBox bounds) + public void UpdateBounds(RigidPose pose, TypedIndex shapeIndex, out BoundingBox bounds) { //Note: the min and max here are in absolute coordinates, which means this is a spot that has to be updated in the event that positions use a higher precision representation. batches[shapeIndex.Type].ComputeBounds(shapeIndex.Index, pose, out bounds.Min, out bounds.Max); diff --git a/BepuPhysics/CollisionDetection/BroadPhase.cs b/BepuPhysics/CollisionDetection/BroadPhase.cs index 7b585a6d4..a21ee6a80 100644 --- a/BepuPhysics/CollisionDetection/BroadPhase.cs +++ b/BepuPhysics/CollisionDetection/BroadPhase.cs @@ -41,7 +41,7 @@ public BroadPhase(BufferPool pool, int initialActiveLeafCapacity = 4096, int ini [MethodImpl(MethodImplOptions.AggressiveInlining)] private static int Add(CollidableReference collidable, ref BoundingBox bounds, ref Tree tree, BufferPool pool, ref Buffer leaves) { - var leafIndex = tree.Add(ref bounds, pool); + var leafIndex = tree.Add(bounds, pool); if (leafIndex >= leaves.Length) { pool.ResizeToAtLeast(ref leaves, tree.LeafCount + 1, leaves.Length); diff --git a/BepuPhysics/Statics.cs b/BepuPhysics/Statics.cs index 4c4da9123..850ed771e 100644 --- a/BepuPhysics/Statics.cs +++ b/BepuPhysics/Statics.cs @@ -359,7 +359,7 @@ public void UpdateBounds(StaticHandle handle) { var index = HandleToIndex[handle.Value]; ref var collidable = ref this[index]; - shapes.UpdateBounds(collidable.Pose, ref collidable.Shape, out var bodyBounds); + shapes.UpdateBounds(collidable.Pose, collidable.Shape, out var bodyBounds); broadPhase.UpdateStaticBounds(collidable.BroadPhaseIndex, bodyBounds.Min, bodyBounds.Max); } diff --git a/BepuPhysics/Trees/Tree_Add.cs b/BepuPhysics/Trees/Tree_Add.cs index 9409a55db..ae463c25b 100644 --- a/BepuPhysics/Trees/Tree_Add.cs +++ b/BepuPhysics/Trees/Tree_Add.cs @@ -109,7 +109,7 @@ private static unsafe BestInsertionChoice ComputeBestInsertionChoice(ref Boundin /// Extents of the leaf bounds. /// Resource pool to use if resizing is required. /// Index of the leaf allocated in the tree's leaf array. - public unsafe int Add(ref BoundingBox bounds, BufferPool pool) + public unsafe int Add(BoundingBox bounds, BufferPool pool) { //The rest of the function assumes we have sufficient room. We don't want to deal with invalidated pointers mid-add. if (Leaves.Length == leafCount) diff --git a/Demos/Demos/CompoundDemo.cs b/Demos/Demos/CompoundDemo.cs index 8ceef2e81..685ef4f84 100644 --- a/Demos/Demos/CompoundDemo.cs +++ b/Demos/Demos/CompoundDemo.cs @@ -8,6 +8,7 @@ using BepuPhysics.Constraints; using DemoRenderer.UI; using DemoUtilities; +using BepuUtilities.Memory; namespace Demos.Demos { diff --git a/Demos/SpecializedTests/IntertreeThreadingTests.cs b/Demos/SpecializedTests/IntertreeThreadingTests.cs index ed8ba7c2a..43c33cf0e 100644 --- a/Demos/SpecializedTests/IntertreeThreadingTests.cs +++ b/Demos/SpecializedTests/IntertreeThreadingTests.cs @@ -66,20 +66,20 @@ static void TestTrees(BufferPool pool, IThreadDispatcher threadDispatcher, Rando { GetRandomLocation(random, ref aBounds, out var center); var bounds = new BoundingBox(center - aOffset, center + aOffset); - treeA.Add(ref bounds, pool); + treeA.Add(bounds, pool); } for (int i = 0; i < bCount; ++i) { GetRandomLocation(random, ref bBounds, out var center); var bounds = new BoundingBox(center - bOffset, center + bOffset); - treeB.Add(ref bounds, pool); + treeB.Add(bounds, pool); } { var indexToRemove = 1; GetBoundsForLeaf(treeB, indexToRemove, out var removedBounds); treeB.RemoveAt(indexToRemove); - treeA.Add(ref removedBounds, pool); + treeA.Add(removedBounds, pool); } var singleThreadedResults = new OverlapHandler { Pairs = new List<(int a, int b)>() }; diff --git a/Demos/SpecializedTests/TreeTest.cs b/Demos/SpecializedTests/TreeTest.cs index f9529273f..10e78b836 100644 --- a/Demos/SpecializedTests/TreeTest.cs +++ b/Demos/SpecializedTests/TreeTest.cs @@ -53,7 +53,7 @@ public static void Test() for (int i = prebuiltCount; i < leafCount; ++i) { - tree.Add(ref leafBounds[i], pool); + tree.Add(leafBounds[i], pool); } tree.Validate(); @@ -110,7 +110,7 @@ public static void Test() var indexInRemovedList = random.Next(removedLeafHandles.Count); var handleToAdd = removedLeafHandles[indexInRemovedList]; removedLeafHandles.FastRemoveAt(indexInRemovedList); - var leafIndex = tree.Add(ref leafBounds[handleToAdd], pool); + var leafIndex = tree.Add(leafBounds[handleToAdd], pool); leafIndexToHandle[leafIndex] = handleToAdd; handleToLeafIndex[handleToAdd] = leafIndex; From e846d015c07f36ee251df5e3b1acac54add2579a Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 19 May 2022 15:25:19 -0500 Subject: [PATCH 486/947] Exposed Broadphase.ActiveLeaves and StaticLeaves. --- BepuPhysics/Bodies.cs | 4 +- BepuPhysics/CollisionDetection/BroadPhase.cs | 46 +++++++++++++------ .../CollisionDetection/BroadPhase_Queries.cs | 16 +++---- .../CollidableOverlapFinder.cs | 8 ++-- BepuPhysics/CollisionDetection/RayBatchers.cs | 4 +- BepuPhysics/IslandAwakener.cs | 2 +- BepuPhysics/IslandSleeper.cs | 2 +- BepuPhysics/Simulation.cs | 10 ++-- BepuPhysics/Statics.cs | 2 +- 9 files changed, 56 insertions(+), 38 deletions(-) diff --git a/BepuPhysics/Bodies.cs b/BepuPhysics/Bodies.cs index b279b5831..85a1a5f4e 100644 --- a/BepuPhysics/Bodies.cs +++ b/BepuPhysics/Bodies.cs @@ -401,11 +401,11 @@ unsafe void UpdateForKinematicStateChange(BodyHandle handle, ref BodyMemoryLocat var mobility = currentlyKinematic ? CollidableMobility.Kinematic : CollidableMobility.Dynamic; if (location.SetIndex == 0) { - broadPhase.activeLeaves[collidable.BroadPhaseIndex] = new CollidableReference(mobility, handle); + broadPhase.ActiveLeaves[collidable.BroadPhaseIndex] = new CollidableReference(mobility, handle); } else { - broadPhase.staticLeaves[collidable.BroadPhaseIndex] = new CollidableReference(mobility, handle); + broadPhase.StaticLeaves[collidable.BroadPhaseIndex] = new CollidableReference(mobility, handle); } } ref var constraints = ref set.Constraints[location.Index]; diff --git a/BepuPhysics/CollisionDetection/BroadPhase.cs b/BepuPhysics/CollisionDetection/BroadPhase.cs index a21ee6a80..594c59d72 100644 --- a/BepuPhysics/CollisionDetection/BroadPhase.cs +++ b/BepuPhysics/CollisionDetection/BroadPhase.cs @@ -11,12 +11,30 @@ namespace BepuPhysics.CollisionDetection { + /// + /// Manages scene acceleration structures for collision detection and queries. + /// public unsafe partial class BroadPhase : IDisposable { - internal Buffer activeLeaves; - internal Buffer staticLeaves; + /// + /// Collidable references contained within the . Note that values at or beyond the .LeafCount are not defined. + /// + public Buffer ActiveLeaves; + /// + /// Collidable references contained within the . Note that values at or beyond .LeafCount are not defined. + /// + public Buffer StaticLeaves; + /// + /// Pool used by the broad phase. + /// public BufferPool Pool { get; private set; } + /// + /// Tree containing wakeful bodies. + /// public Tree ActiveTree; + /// + /// Tree containing sleeping bodies and statics. + /// public Tree StaticTree; Tree.RefitAndRefineMultithreadedContext activeRefineContext; //TODO: static trees do not need to do nearly as much work as the active; this will change in the future. @@ -29,8 +47,8 @@ public BroadPhase(BufferPool pool, int initialActiveLeafCapacity = 4096, int ini Pool = pool; ActiveTree = new Tree(pool, initialActiveLeafCapacity); StaticTree = new Tree(pool, initialStaticLeafCapacity); - pool.TakeAtLeast(initialActiveLeafCapacity, out activeLeaves); - pool.TakeAtLeast(initialStaticLeafCapacity, out staticLeaves); + pool.TakeAtLeast(initialActiveLeafCapacity, out ActiveLeaves); + pool.TakeAtLeast(initialStaticLeafCapacity, out StaticLeaves); activeRefineContext = new Tree.RefitAndRefineMultithreadedContext(); staticRefineContext = new Tree.RefitAndRefineMultithreadedContext(); @@ -66,24 +84,24 @@ public static bool RemoveAt(int index, ref Tree tree, ref Buffer @@ -252,8 +270,8 @@ void Dispose(ref Tree tree, ref Buffer leaves) /// Number of leaves to allocate space for in the static tree. public void EnsureCapacity(int activeCapacity, int staticCapacity) { - EnsureCapacity(ref ActiveTree, ref activeLeaves, activeCapacity); - EnsureCapacity(ref StaticTree, ref staticLeaves, staticCapacity); + EnsureCapacity(ref ActiveTree, ref ActiveLeaves, activeCapacity); + EnsureCapacity(ref StaticTree, ref StaticLeaves, staticCapacity); } /// @@ -263,8 +281,8 @@ public void EnsureCapacity(int activeCapacity, int staticCapacity) /// Number of leaves to allocate space for in the static tree. public void Resize(int activeCapacity, int staticCapacity) { - ResizeCapacity(ref ActiveTree, ref activeLeaves, activeCapacity); - ResizeCapacity(ref StaticTree, ref staticLeaves, staticCapacity); + ResizeCapacity(ref ActiveTree, ref ActiveLeaves, activeCapacity); + ResizeCapacity(ref StaticTree, ref StaticLeaves, staticCapacity); } /// @@ -272,8 +290,8 @@ public void Resize(int activeCapacity, int staticCapacity) /// public void Dispose() { - Dispose(ref ActiveTree, ref activeLeaves); - Dispose(ref StaticTree, ref staticLeaves); + Dispose(ref ActiveTree, ref ActiveLeaves); + Dispose(ref StaticTree, ref StaticLeaves); } } diff --git a/BepuPhysics/CollisionDetection/BroadPhase_Queries.cs b/BepuPhysics/CollisionDetection/BroadPhase_Queries.cs index 00d42d285..078980ac0 100644 --- a/BepuPhysics/CollisionDetection/BroadPhase_Queries.cs +++ b/BepuPhysics/CollisionDetection/BroadPhase_Queries.cs @@ -46,9 +46,9 @@ public unsafe void RayCast(in Vector3 origin, in Vector3 direction, TreeRay.CreateFrom(origin, direction, maximumT, id, out var rayData, out var treeRay); RayLeafTester tester; tester.LeafTester = rayTester; - tester.Leaves = activeLeaves; + tester.Leaves = ActiveLeaves; ActiveTree.RayCast(&treeRay, &rayData, ref tester); - tester.Leaves = staticLeaves; + tester.Leaves = StaticLeaves; StaticTree.RayCast(&treeRay, &rayData, ref tester); //The sweep tester probably relies on mutation to function; copy any mutations back to the original reference. rayTester = tester.LeafTester; @@ -82,9 +82,9 @@ public unsafe void Sweep(in Vector3 min, in Vector3 max, in Vector TreeRay.CreateFrom(origin, direction, maximumT, out var treeRay); SweepLeafTester tester; tester.LeafTester = sweepTester; - tester.Leaves = activeLeaves; + tester.Leaves = ActiveLeaves; ActiveTree.Sweep(expansion, origin, direction, &treeRay, ref tester); - tester.Leaves = staticLeaves; + tester.Leaves = StaticLeaves; StaticTree.Sweep(expansion, origin, direction, &treeRay, ref tester); //The sweep tester probably relies on mutation to function; copy any mutations back to the original reference. sweepTester = tester.LeafTester; @@ -126,9 +126,9 @@ public unsafe void GetOverlaps(in Vector3 min, in Vector3 ma { BoxQueryEnumerator enumerator; enumerator.Enumerator = overlapEnumerator; - enumerator.Leaves = activeLeaves; + enumerator.Leaves = ActiveLeaves; ActiveTree.GetOverlaps(min, max, ref enumerator); - enumerator.Leaves = staticLeaves; + enumerator.Leaves = StaticLeaves; StaticTree.GetOverlaps(min, max, ref enumerator); //Enumeration could have mutated the enumerator; preserve those modifications. overlapEnumerator = enumerator.Enumerator; @@ -144,9 +144,9 @@ public unsafe void GetOverlaps(in BoundingBox boundingBox, r { BoxQueryEnumerator enumerator; enumerator.Enumerator = overlapEnumerator; - enumerator.Leaves = activeLeaves; + enumerator.Leaves = ActiveLeaves; ActiveTree.GetOverlaps(boundingBox, ref enumerator); - enumerator.Leaves = staticLeaves; + enumerator.Leaves = StaticLeaves; StaticTree.GetOverlaps(boundingBox, ref enumerator); //Enumeration could have mutated the enumerator; preserve those modifications. overlapEnumerator = enumerator.Enumerator; diff --git a/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs b/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs index 4a7fbd761..e03c8067f 100644 --- a/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs +++ b/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs @@ -117,11 +117,11 @@ public override void DispatchOverlaps(float dt, IThreadDispatcher threadDispatch //would be invalid because they may get resized, invalidating the pointers. for (int i = 0; i < selfHandlers.Length; ++i) { - selfHandlers[i] = new SelfOverlapHandler(broadPhase.activeLeaves, narrowPhase, i); + selfHandlers[i] = new SelfOverlapHandler(broadPhase.ActiveLeaves, narrowPhase, i); } for (int i = 0; i < intertreeHandlers.Length; ++i) { - intertreeHandlers[i] = new IntertreeOverlapHandler(broadPhase.activeLeaves, broadPhase.staticLeaves, narrowPhase, i); + intertreeHandlers[i] = new IntertreeOverlapHandler(broadPhase.ActiveLeaves, broadPhase.StaticLeaves, narrowPhase, i); } Debug.Assert(intertreeHandlers.Length >= threadDispatcher.ThreadCount); selfTestContext.PrepareJobs(ref broadPhase.ActiveTree, selfHandlers, threadDispatcher.ThreadCount); @@ -151,9 +151,9 @@ public override void DispatchOverlaps(float dt, IThreadDispatcher threadDispatch else { narrowPhase.Prepare(dt); - var selfTestHandler = new SelfOverlapHandler(broadPhase.activeLeaves, narrowPhase, 0); + var selfTestHandler = new SelfOverlapHandler(broadPhase.ActiveLeaves, narrowPhase, 0); broadPhase.ActiveTree.GetSelfOverlaps(ref selfTestHandler); - var intertreeHandler = new IntertreeOverlapHandler(broadPhase.activeLeaves, broadPhase.staticLeaves, narrowPhase, 0); + var intertreeHandler = new IntertreeOverlapHandler(broadPhase.ActiveLeaves, broadPhase.StaticLeaves, narrowPhase, 0); broadPhase.ActiveTree.GetOverlaps(ref broadPhase.StaticTree, ref intertreeHandler); narrowPhase.overlapWorkers[0].Batcher.Flush(); diff --git a/BepuPhysics/CollisionDetection/RayBatchers.cs b/BepuPhysics/CollisionDetection/RayBatchers.cs index 85d66216b..ce5372831 100644 --- a/BepuPhysics/CollisionDetection/RayBatchers.cs +++ b/BepuPhysics/CollisionDetection/RayBatchers.cs @@ -55,8 +55,8 @@ public unsafe void TestLeaf(int leafIndex, RayData* rayData, float* maximumT) /// This should typically be chosen as the highest value which avoids spilling data out of L2 cache. public BroadPhaseRayBatcher(BufferPool pool, BroadPhase broadPhase, TRayTester rayTester, int batcherRayCapacity = 2048) { - activeTester = new LeafTester { Leaves = broadPhase.activeLeaves, RayTester = rayTester }; - staticTester = new LeafTester { Leaves = broadPhase.staticLeaves, RayTester = rayTester }; + activeTester = new LeafTester { Leaves = broadPhase.ActiveLeaves, RayTester = rayTester }; + staticTester = new LeafTester { Leaves = broadPhase.StaticLeaves, RayTester = rayTester }; this.broadPhase = broadPhase; batcher = new RayBatcher(pool, batcherRayCapacity, Math.Max(8, 2 * SpanHelper.GetContainingPowerOf2(Math.Max(broadPhase.StaticTree.LeafCount, broadPhase.ActiveTree.LeafCount)))); diff --git a/BepuPhysics/IslandAwakener.cs b/BepuPhysics/IslandAwakener.cs index 7fd304497..b3a4ec676 100644 --- a/BepuPhysics/IslandAwakener.cs +++ b/BepuPhysics/IslandAwakener.cs @@ -355,7 +355,7 @@ internal unsafe void ExecutePhaseTwoJob(int index) bounds.Min = *minPointer; bounds.Max = *maxPointer; var staticBroadPhaseIndexToRemove = broadPhaseIndex; - broadPhaseIndex = broadPhase.AddActive(broadPhase.staticLeaves[broadPhaseIndex], ref bounds); + broadPhaseIndex = broadPhase.AddActive(broadPhase.StaticLeaves[broadPhaseIndex], ref bounds); if (broadPhase.RemoveStaticAt(staticBroadPhaseIndexToRemove, out var movedLeaf)) { diff --git a/BepuPhysics/IslandSleeper.cs b/BepuPhysics/IslandSleeper.cs index 68b010ae6..b971d85a0 100644 --- a/BepuPhysics/IslandSleeper.cs +++ b/BepuPhysics/IslandSleeper.cs @@ -365,7 +365,7 @@ unsafe void Gather(int workerIndex) { //Gather the broad phase data so that the later active set removal phase can stick it into the static broad phase structures. ref var broadPhaseData = ref inactiveSetReference.BroadPhaseData[targetIndex]; - broadPhaseData.Reference = broadPhase.activeLeaves[sourceCollidable.BroadPhaseIndex]; + broadPhaseData.Reference = broadPhase.ActiveLeaves[sourceCollidable.BroadPhaseIndex]; broadPhase.GetActiveBoundsPointers(sourceCollidable.BroadPhaseIndex, out var minPtr, out var maxPtr); broadPhaseData.Bounds.Min = *minPtr; broadPhaseData.Bounds.Max = *maxPtr; diff --git a/BepuPhysics/Simulation.cs b/BepuPhysics/Simulation.cs index 53eed00e6..93076af1a 100644 --- a/BepuPhysics/Simulation.cs +++ b/BepuPhysics/Simulation.cs @@ -152,7 +152,7 @@ private static int ValidateAndCountShapefulBodies(ref BodySet bodySet, ref Tree [Conditional("DEBUG")] internal void ValidateCollidables() { - var activeShapefulBodyCount = ValidateAndCountShapefulBodies(ref Bodies.ActiveSet, ref BroadPhase.ActiveTree, ref BroadPhase.activeLeaves); + var activeShapefulBodyCount = ValidateAndCountShapefulBodies(ref Bodies.ActiveSet, ref BroadPhase.ActiveTree, ref BroadPhase.ActiveLeaves); Debug.Assert(BroadPhase.ActiveTree.LeafCount == activeShapefulBodyCount); int inactiveShapefulBodyCount = 0; @@ -162,7 +162,7 @@ internal void ValidateCollidables() ref var set = ref Bodies.Sets[setIndex]; if (set.Allocated) { - inactiveShapefulBodyCount += ValidateAndCountShapefulBodies(ref set, ref BroadPhase.StaticTree, ref BroadPhase.staticLeaves); + inactiveShapefulBodyCount += ValidateAndCountShapefulBodies(ref set, ref BroadPhase.StaticTree, ref BroadPhase.StaticLeaves); } } Debug.Assert(inactiveShapefulBodyCount + Statics.Count == BroadPhase.StaticTree.LeafCount); @@ -172,7 +172,7 @@ internal void ValidateCollidables() Debug.Assert(collidable.Shape.Exists, "All static collidables must have shapes. That's their only purpose."); Debug.Assert(collidable.BroadPhaseIndex >= 0 && collidable.BroadPhaseIndex < BroadPhase.StaticTree.LeafCount); - ref var leaf = ref BroadPhase.staticLeaves[collidable.BroadPhaseIndex]; + ref var leaf = ref BroadPhase.StaticLeaves[collidable.BroadPhaseIndex]; Debug.Assert(leaf.StaticHandle.Value == Statics.IndexToHandle[i].Value); Debug.Assert(leaf.Mobility == CollidableMobility.Static); } @@ -180,10 +180,10 @@ internal void ValidateCollidables() //Ensure there are no duplicates between the two broad phase trees. for (int i = 0; i < BroadPhase.ActiveTree.LeafCount; ++i) { - var activeLeaf = BroadPhase.activeLeaves[i]; + var activeLeaf = BroadPhase.ActiveLeaves[i]; for (int j = 0; j < BroadPhase.StaticTree.LeafCount; ++j) { - Debug.Assert(BroadPhase.staticLeaves[j].Packed != activeLeaf.Packed); + Debug.Assert(BroadPhase.StaticLeaves[j].Packed != activeLeaf.Packed); } } diff --git a/BepuPhysics/Statics.cs b/BepuPhysics/Statics.cs index 850ed771e..1d4806584 100644 --- a/BepuPhysics/Statics.cs +++ b/BepuPhysics/Statics.cs @@ -229,7 +229,7 @@ public void Dispose() [MethodImpl(MethodImplOptions.AggressiveInlining)] public bool LoopBody(int leafIndex) { - ref var leaf = ref broadPhase.staticLeaves[leafIndex]; + ref var leaf = ref broadPhase.StaticLeaves[leafIndex]; if (leaf.Mobility != CollidableMobility.Static) { if (Filter.ShouldAwaken(bodies[leaf.BodyHandle])) From d26a5c7e7122dac1d3007d5a3b0f2b2339fa9a6a Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 19 May 2022 15:55:33 -0500 Subject: [PATCH 487/947] Version bump. --- BepuPhysics/BepuPhysics.csproj | 2 +- BepuUtilities/BepuUtilities.csproj | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/BepuPhysics/BepuPhysics.csproj b/BepuPhysics/BepuPhysics.csproj index d0ba4af93..7b262fb78 100644 --- a/BepuPhysics/BepuPhysics.csproj +++ b/BepuPhysics/BepuPhysics.csproj @@ -1,7 +1,7 @@  net6.0 - 2.5.0-beta.2 + 2.5.0-beta.3 Bepu Entertainment LLC Ross Nordby Speedy real time physics simulation library. diff --git a/BepuUtilities/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj index a010e4c9f..c57cdaf16 100644 --- a/BepuUtilities/BepuUtilities.csproj +++ b/BepuUtilities/BepuUtilities.csproj @@ -3,7 +3,7 @@ BepuUtilities BepuUtilities net6.0 - 2.5.0-beta.2 + 2.5.0-beta.3 Bepu Entertainment LLC Ross Nordby Supporting utilities library for BEPUphysics v2. From 66971de28a60f84d281cb3198eb973f9a9682f0d Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 1 Jun 2022 15:05:58 -0500 Subject: [PATCH 488/947] De-oopsed CollisionQueryDemo's orientation parameter order. --- Demos/Demos/CollisionQueryDemo.cs | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/Demos/Demos/CollisionQueryDemo.cs b/Demos/Demos/CollisionQueryDemo.cs index d7a6e72dd..a4a6c47a1 100644 --- a/Demos/Demos/CollisionQueryDemo.cs +++ b/Demos/Demos/CollisionQueryDemo.cs @@ -155,7 +155,7 @@ public unsafe void AddQueryToBatch(int queryShapeType, void* queryShapeData, int shapeIndex.Type, queryShapeType, shapeData, cachedQueryShapeData, //Because we're using this as a boolean query, we use a speculative margin of 0. Don't care about negative depths. - queryPose.Position - pose.Position, queryPose.Orientation, pose.Orientation, 0, new PairContinuation(queryId)); + queryPose.Position - pose.Position, pose.Orientation, queryPose.Orientation, 0, new PairContinuation(queryId)); } broadPhaseEnumerator.References.Dispose(BufferPool); } @@ -229,7 +229,11 @@ public unsafe override void Render(Renderer renderer, Camera camera, Input input { for (int j = 0; j < widthInQueries; ++j) { - queries.Allocate(BufferPool) = new Query { Box = new Box(1, 1, 1), Pose = basePosition + querySpacing * new Vector3(i, 0, j) }; + queries.Allocate(BufferPool) = new Query + { + Box = new Box(1, 1, 1), + Pose = new RigidPose(basePosition + querySpacing * new Vector3(i, 0, j), Quaternion.CreateFromAxisAngle(Vector3.Normalize(new Vector3(i - 2.5f, 0, j - 2.5f)), i * j + 0.7457f)) + }; } } //Note that the callbacks and set are value types, so you have to be a little careful about copying. From 1c4d1c5571672517c473b1745cb7905adac7bbf1 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 5 Jun 2022 17:34:28 -0500 Subject: [PATCH 489/947] CollisionBatcher no longer deletes a batch's cached shapes until all batches complete to avoid explosions. Bumped version number. --- BepuPhysics/BepuPhysics.csproj | 2 +- BepuPhysics/Collidables/ConvexHull.cs | 7 +++++++ BepuPhysics/CollisionDetection/CollisionBatcher.cs | 11 ++++++++++- .../CollisionDetection/CollisionTaskRegistry.cs | 2 -- BepuUtilities/BepuUtilities.csproj | 2 +- 5 files changed, 19 insertions(+), 5 deletions(-) diff --git a/BepuPhysics/BepuPhysics.csproj b/BepuPhysics/BepuPhysics.csproj index 7b262fb78..e24bfacf9 100644 --- a/BepuPhysics/BepuPhysics.csproj +++ b/BepuPhysics/BepuPhysics.csproj @@ -1,7 +1,7 @@  net6.0 - 2.5.0-beta.3 + 2.5.0-beta.4 Bepu Entertainment LLC Ross Nordby Speedy real time physics simulation library. diff --git a/BepuPhysics/Collidables/ConvexHull.cs b/BepuPhysics/Collidables/ConvexHull.cs index a8d085f2e..9be1d9e94 100644 --- a/BepuPhysics/Collidables/ConvexHull.cs +++ b/BepuPhysics/Collidables/ConvexHull.cs @@ -403,6 +403,13 @@ public void WriteFirst(in ConvexHull source) [MethodImpl(MethodImplOptions.AggressiveInlining)] public void WriteSlot(int index, in ConvexHull source) { + Debug.Assert( + source.Points.Allocated && source.BoundingPlanes.Allocated && source.FaceToVertexIndicesStart.Allocated && source.FaceVertexIndices.Allocated && + (uint)source.Points.Length < 100000 && + (uint)source.BoundingPlanes.Length < 100000 && + (uint)source.FaceToVertexIndicesStart.Length < 100000 && + (uint)source.FaceVertexIndices.Length < 100000, + "If a convex hull has an extremely large (or negative) count on any of its buffers, it is very likely undefined trash data."); Hulls[index] = source; } } diff --git a/BepuPhysics/CollisionDetection/CollisionBatcher.cs b/BepuPhysics/CollisionDetection/CollisionBatcher.cs index c9f74c70b..95b71aeb0 100644 --- a/BepuPhysics/CollisionDetection/CollisionBatcher.cs +++ b/BepuPhysics/CollisionDetection/CollisionBatcher.cs @@ -300,7 +300,16 @@ public void Flush() { typeMatrix.tasks[i].ExecuteBatch(ref batch.Pairs, ref this); } - //Dispose of the batch and any associated buffers; since the flush is one pass, we won't be needing this again. + } + for (int i = minimumBatchIndex; i <= maximumBatchIndex; ++i) + { + //Disposal is deferred until execution is complete. + //Shape data could be cached in an early buffer by a task that generates child tasks. + //Those child tasks could refer to data cached in the parent task's shapes buffer. + //Deleting it would potentially explode things. + //(While internal uses of the collision batcher generally refer to the Simulation.Shapes collection, + //the collision batcher is not limited to that use case. Consider contact queries with ephemeral shapes.) + ref var batch = ref batches[i]; if (batch.Pairs.Buffer.Allocated) { Pool.Return(ref batch.Pairs.Buffer); diff --git a/BepuPhysics/CollisionDetection/CollisionTaskRegistry.cs b/BepuPhysics/CollisionDetection/CollisionTaskRegistry.cs index e388d55a8..e94706feb 100644 --- a/BepuPhysics/CollisionDetection/CollisionTaskRegistry.cs +++ b/BepuPhysics/CollisionDetection/CollisionTaskRegistry.cs @@ -63,8 +63,6 @@ public abstract class CollisionTask /// Type of the callbacks used to handle results of collision tasks. /// Batcher responsible for the invocation. /// Batch of pairs to test. - /// Continuations to invoke upon completion of a top level pair. - /// Filters to use to influence execution of the collision tasks. public abstract void ExecuteBatch(ref UntypedList batch, ref CollisionBatcher batcher) where TCallbacks : struct, ICollisionCallbacks; diff --git a/BepuUtilities/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj index c57cdaf16..b7693ca3d 100644 --- a/BepuUtilities/BepuUtilities.csproj +++ b/BepuUtilities/BepuUtilities.csproj @@ -3,7 +3,7 @@ BepuUtilities BepuUtilities net6.0 - 2.5.0-beta.3 + 2.5.0-beta.4 Bepu Entertainment LLC Ross Nordby Supporting utilities library for BEPUphysics v2. From a72692c1a1a1b82c101fea0936d70849751ff785 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 8 Jun 2022 18:28:24 -0500 Subject: [PATCH 490/947] Compound tasks are now consistent in reporting top level pair completion even if there are no subpairs. BatchedCollisionTests now includes compound combinations. --- .../CollisionDetection/CollisionBatcher.cs | 19 ++++- .../CollisionBatcherContinuations.cs | 8 +- .../CompoundPairCollisionTask.cs | 8 +- .../ConvexCompoundCollisionTask.cs | 8 +- .../SpecializedTests/BatchedCollisionTests.cs | 81 +++++++++++++++++-- 5 files changed, 109 insertions(+), 15 deletions(-) diff --git a/BepuPhysics/CollisionDetection/CollisionBatcher.cs b/BepuPhysics/CollisionDetection/CollisionBatcher.cs index 95b71aeb0..eb9793455 100644 --- a/BepuPhysics/CollisionDetection/CollisionBatcher.cs +++ b/BepuPhysics/CollisionDetection/CollisionBatcher.cs @@ -371,13 +371,29 @@ public unsafe void ProcessConvexResult(ref ConvexContactManifold manifold, ref P } } + /// + /// Reports the zero result of a convex collision test to the callbacks and, if necessary, to any continuations for postprocessing. + /// + /// Unless you're building custom compound collision pairs or adding new contact processing continuations, you can safely ignore this. + /// Continuation describing the pair and what to do with it. + public unsafe void ProcessEmptyResult(ref PairContinuation continuation) + { + Unsafe.SkipInit(out ConvexContactManifold manifold); + manifold.Count = 0; + ProcessConvexResult(ref manifold, ref continuation); + } + + /// /// Submits a subpair whose testing was blocked by user callback as complete to any relevant continuations. /// /// Unless you're building custom compound collision pairs or adding new contact processing continuations, you can safely ignore this. /// Continuation describing the pair and what to do with it. - public unsafe void ProcessUntestedConvexResult(ref PairContinuation continuation) + public unsafe void ProcessUntestedSubpairConvexResult(ref PairContinuation continuation) { + //Note that we do not call OnChildPairCompleted. A callback is only invoked if a child is actually tested. + //If a child isn't considered- because acceleration structure pruned it, or a callback said to ignore it- there is no callback report. + //That's different from the top level pair which should always report. switch (continuation.Type) { case CollisionContinuationType.NonconvexReduction: @@ -397,5 +413,6 @@ public unsafe void ProcessUntestedConvexResult(ref PairContinuation continuation break; } } + } } diff --git a/BepuPhysics/CollisionDetection/CollisionBatcherContinuations.cs b/BepuPhysics/CollisionDetection/CollisionBatcherContinuations.cs index 5153e9b38..9be27d96f 100644 --- a/BepuPhysics/CollisionDetection/CollisionBatcherContinuations.cs +++ b/BepuPhysics/CollisionDetection/CollisionBatcherContinuations.cs @@ -58,19 +58,19 @@ public enum CollisionContinuationType : byte /// /// Marks a pair as requiring no further processing before being reported to the user supplied continuations. /// - Direct, + Direct = 0, /// /// Marks a pair as part of a set of a higher (potentially multi-manifold) pair, potentially requiring contact reduction. /// - NonconvexReduction, + NonconvexReduction = 1, /// /// Marks a pair as a part of a set of mesh-convex collisions, potentially requiring mesh boundary smoothing. /// - MeshReduction, + MeshReduction = 2, /// /// Marks a pair as a part of a set of mesh-convex collisions spawned by a mesh-compound pair, potentially requiring mesh boundary smoothing. /// - CompoundMeshReduction, + CompoundMeshReduction = 3, //TODO: We don't yet support boundary smoothing for meshes or convexes. Most likely, boundary smoothed convexes won't make it into the first release of the engine at all; //they're a pretty experimental feature with limited applications. ///// diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairCollisionTask.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairCollisionTask.cs index 6111052ac..792d56c1f 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairCollisionTask.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairCollisionTask.cs @@ -63,9 +63,9 @@ public unsafe override void ExecuteBatch(ref UntypedList batch, ref { totalOverlapCountForPair += pairOverlaps[j].Count; } + ref var pair = ref pairs[pairIndex]; if (totalOverlapCountForPair > 0) { - ref var pair = ref pairs[pairIndex]; ref var continuation = ref continuationHandler.CreateContinuation(ref batcher, totalOverlapCountForPair, ref pairOverlaps, ref subpairQueries, pair, out var continuationIndex); var nextContinuationChildIndex = 0; @@ -116,11 +116,15 @@ public unsafe override void ExecuteBatch(ref UntypedList batch, ref } else { - batcher.ProcessUntestedConvexResult(ref subpairContinuation); + batcher.ProcessUntestedSubpairConvexResult(ref subpairContinuation); } } } } + else + { + batcher.ProcessEmptyResult(ref pair.Continuation); + } } diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundCollisionTask.cs b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundCollisionTask.cs index 15471abc7..7b38dede8 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundCollisionTask.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundCollisionTask.cs @@ -50,9 +50,9 @@ public unsafe override void ExecuteBatch(ref UntypedList batch, ref { ref var pairOverlaps = ref overlaps.GetOverlapsForPair(i); ref var pairQuery = ref overlaps.GetQueryForPair(i); + ref var pair = ref pairs[i]; if (pairOverlaps.Count > 0) { - ref var pair = ref pairs[i]; ref var compound = ref Unsafe.AsRef(pair.B); ref var continuation = ref continuationHandler.CreateContinuation(ref batcher, pairOverlaps.Count, pair, pairQuery, out var continuationIndex); @@ -96,11 +96,15 @@ public unsafe override void ExecuteBatch(ref UntypedList batch, ref } else { - batcher.ProcessUntestedConvexResult(ref subpairContinuation); + batcher.ProcessUntestedSubpairConvexResult(ref subpairContinuation); } } } + else + { + batcher.ProcessEmptyResult(ref pair.Continuation); + } } overlaps.Dispose(batcher.Pool); } diff --git a/Demos/SpecializedTests/BatchedCollisionTests.cs b/Demos/SpecializedTests/BatchedCollisionTests.cs index 4ec03e9f1..b5538bbee 100644 --- a/Demos/SpecializedTests/BatchedCollisionTests.cs +++ b/Demos/SpecializedTests/BatchedCollisionTests.cs @@ -67,7 +67,7 @@ unsafe static void Test(ref TA a, ref TB b, ref Buffer posesA { int count = 0; var callbacks = new TestCollisionCallbacks { Count = &count }; - TestPair(ref a, ref b, ref posesA, ref posesB, ref callbacks, pool, shapes, registry, 64); + TestPair(ref a, ref b, ref posesA, ref posesB, ref callbacks, pool, shapes, registry, 256); count = 0; var start = Stopwatch.GetTimestamp(); TestPair(ref a, ref b, ref posesA, ref posesB, ref callbacks, pool, shapes, registry, iterationCount); @@ -152,7 +152,7 @@ public static void Test() var triangle = new Triangle(new Vector3(0, 0, 0), new Vector3(1, 0, 0), new Vector3(0, 0, 1)); var cylinder = new Cylinder(0.5f, 1f); - const int pointCount = 8192; + const int pointCount = 64; var points = new QuickList(pointCount, pool); //points.Allocate(pool) = new Vector3(0, 0, 0); //points.Allocate(pool) = new Vector3(0, 0, 1); @@ -168,14 +168,59 @@ public static void Test() //points.AllocateUnsafely() = new Vector3(0, 1, 0) + Vector3.Normalize(new Vector3(random.NextSingle() * 2 - 1, random.NextSingle() * 2 - 1, random.NextSingle() * 2 - 1)) * random.NextSingle(); } + Shapes shapes = new Shapes(pool, 32); var pointsBuffer = points.Span.Slice(points.Count); ConvexHullHelper.CreateShape(pointsBuffer, pool, out _, out var convexHull); - var poseA = new RigidPose { Position = new Vector3(0, 0, 0), Orientation = Quaternion.Identity }; - var poseB = new RigidPose { Position = new Vector3(0, 1, 0), Orientation = Quaternion.Identity }; - Shapes shapes = new Shapes(pool, 32); + using var compoundBuilder = new CompoundBuilder(pool, shapes, 64); + //COMPOUND + var legShape = new Box(0.2f, 1, 0.2f); + var legInverseInertia = legShape.ComputeInertia(1f); + var legShapeIndex = shapes.Add(legShape); + var legPose0 = new RigidPose { Position = new Vector3(-1.5f, 0, -1.5f), Orientation = Quaternion.Identity }; + var legPose1 = new RigidPose { Position = new Vector3(-1.5f, 0, 1.5f), Orientation = Quaternion.Identity }; + var legPose2 = new RigidPose { Position = new Vector3(1.5f, 0, -1.5f), Orientation = Quaternion.Identity }; + var legPose3 = new RigidPose { Position = new Vector3(1.5f, 0, 1.5f), Orientation = Quaternion.Identity }; + compoundBuilder.Add(legShapeIndex, legPose0, legInverseInertia.InverseInertiaTensor, 1); + compoundBuilder.Add(legShapeIndex, legPose1, legInverseInertia.InverseInertiaTensor, 1); + compoundBuilder.Add(legShapeIndex, legPose2, legInverseInertia.InverseInertiaTensor, 1); + compoundBuilder.Add(legShapeIndex, legPose3, legInverseInertia.InverseInertiaTensor, 1); + var tableTopPose = new RigidPose { Position = new Vector3(0, 0.6f, 0), Orientation = Quaternion.Identity }; + var tableTopShape = new Box(3.2f, 0.2f, 3.2f); + compoundBuilder.Add(tableTopShape, tableTopPose, 3); + + compoundBuilder.BuildDynamicCompound(out var tableChildren, out var tableInertia, out var tableCenter); + compoundBuilder.Reset(); + var compound = new Compound(tableChildren); + + //BIGCOMPOUND + var treeCompoundBoxShape = new Box(0.5f, 1.5f, 1f); + var treeCompoundBoxShapeIndex = shapes.Add(treeCompoundBoxShape); + var childInertia = treeCompoundBoxShape.ComputeInertia(1); + for (int i = 0; i < 64; ++i) + { + RigidPose localPose; + localPose.Position = new Vector3(12, 12, 12) * (0.5f * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) - Vector3.One); + float orientationLengthSquared; + do + { + localPose.Orientation = new Quaternion(random.NextSingle(), random.NextSingle(), random.NextSingle(), random.NextSingle()); + orientationLengthSquared = QuaternionEx.LengthSquared(ref localPose.Orientation); + } + while (orientationLengthSquared < 1e-9f); + QuaternionEx.Scale(localPose.Orientation, 1f / MathF.Sqrt(orientationLengthSquared), out localPose.Orientation); + //Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), MathF.PI, out localPose.Orientation); + + compoundBuilder.Add(treeCompoundBoxShapeIndex, localPose, childInertia.InverseInertiaTensor, 1); + } + compoundBuilder.BuildDynamicCompound(out var children, out var inertia, out var center); + compoundBuilder.Reset(); + var bigCompound = new BigCompound(children, shapes, pool); + + //MESH + DemoMeshHelper.CreateDeformedPlane(8, 8, (x, y) => { return new Vector3(x * 2 - 8, 3 * MathF.Sin(x) * MathF.Sin(y), y * 2 - 8); }, Vector3.One, pool, out var mesh); - int iterationCount = 1 << 22; + int iterationCount = 1 << 20; pool.Take(iterationCount, out var posesA); pool.Take(iterationCount, out var posesB); for (int i = 0; i < iterationCount; ++i) @@ -191,21 +236,45 @@ public static void Test() Test(ref sphere, ref triangle, ref posesA, ref posesB, pool, shapes, registry, iterationCount); Test(ref sphere, ref cylinder, ref posesA, ref posesB, pool, shapes, registry, iterationCount); Test(ref sphere, ref convexHull, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref sphere, ref compound, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref sphere, ref bigCompound, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref sphere, ref mesh, ref posesA, ref posesB, pool, shapes, registry, iterationCount); Test(ref capsule, ref capsule, ref posesA, ref posesB, pool, shapes, registry, iterationCount); Test(ref capsule, ref box, ref posesA, ref posesB, pool, shapes, registry, iterationCount); Test(ref capsule, ref triangle, ref posesA, ref posesB, pool, shapes, registry, iterationCount); Test(ref capsule, ref cylinder, ref posesA, ref posesB, pool, shapes, registry, iterationCount); Test(ref capsule, ref convexHull, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref capsule, ref compound, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref capsule, ref bigCompound, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref capsule, ref mesh, ref posesA, ref posesB, pool, shapes, registry, iterationCount); Test(ref box, ref box, ref posesA, ref posesB, pool, shapes, registry, iterationCount); Test(ref box, ref triangle, ref posesA, ref posesB, pool, shapes, registry, iterationCount); Test(ref box, ref cylinder, ref posesA, ref posesB, pool, shapes, registry, iterationCount); Test(ref box, ref convexHull, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref box, ref compound, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref box, ref bigCompound, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref box, ref mesh, ref posesA, ref posesB, pool, shapes, registry, iterationCount); Test(ref triangle, ref triangle, ref posesA, ref posesB, pool, shapes, registry, iterationCount); Test(ref triangle, ref cylinder, ref posesA, ref posesB, pool, shapes, registry, iterationCount); Test(ref triangle, ref convexHull, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref triangle, ref compound, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref triangle, ref bigCompound, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref triangle, ref mesh, ref posesA, ref posesB, pool, shapes, registry, iterationCount); Test(ref cylinder, ref cylinder, ref posesA, ref posesB, pool, shapes, registry, iterationCount); Test(ref cylinder, ref convexHull, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref cylinder, ref compound, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref cylinder, ref bigCompound, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref cylinder, ref mesh, ref posesA, ref posesB, pool, shapes, registry, iterationCount); Test(ref convexHull, ref convexHull, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref convexHull, ref compound, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref convexHull, ref bigCompound, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref convexHull, ref mesh, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref compound, ref compound, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref compound, ref bigCompound, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref compound, ref mesh, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref bigCompound, ref bigCompound, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref bigCompound, ref mesh, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref mesh, ref mesh, ref posesA, ref posesB, pool, shapes, registry, iterationCount); Test(sphere, sphere, ref posesA, ref posesB, iterationCount); From aa6b0d8ae1e528bd715321fda43a15218b7d76a6 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 8 Jun 2022 23:18:11 -0500 Subject: [PATCH 491/947] PairContinuation bit allocation shifted around a little. Compound involving pairs now have a higher maximum child count (2^20), and exceeding that count now simply causes the children to be ignored rather than potentially exploding. Version bump. --- BepuPhysics/BepuPhysics.csproj | 2 +- .../CollisionBatcherContinuations.cs | 41 ++++++++++++++----- .../CompoundPairCollisionTask.cs | 8 +++- .../ConvexCompoundCollisionTask.cs | 6 ++- BepuUtilities/BepuUtilities.csproj | 2 +- .../SpecializedTests/BatchedCollisionTests.cs | 1 + 6 files changed, 46 insertions(+), 14 deletions(-) diff --git a/BepuPhysics/BepuPhysics.csproj b/BepuPhysics/BepuPhysics.csproj index e24bfacf9..98dd15a18 100644 --- a/BepuPhysics/BepuPhysics.csproj +++ b/BepuPhysics/BepuPhysics.csproj @@ -1,7 +1,7 @@  net6.0 - 2.5.0-beta.4 + 2.5.0-beta.5 Bepu Entertainment LLC Ross Nordby Speedy real time physics simulation library. diff --git a/BepuPhysics/CollisionDetection/CollisionBatcherContinuations.cs b/BepuPhysics/CollisionDetection/CollisionBatcherContinuations.cs index 9be27d96f..acade2a18 100644 --- a/BepuPhysics/CollisionDetection/CollisionBatcherContinuations.cs +++ b/BepuPhysics/CollisionDetection/CollisionBatcherContinuations.cs @@ -86,19 +86,40 @@ public struct PairContinuation public int ChildA; public int ChildB; public uint Packed; + + /// + /// Covers bits [0, 20) in the packed representation. Refers to the child pair index in a subtask generating collision task that generated this continuation. + /// + public const int ChildIndexBits = 20; + /// + /// Covers bits [20, 30) in the packed representation. Refers to the index of a subpair in a continuation processor. + /// Maximum number should be equal to the sum of the batch sizes subtask generating collision tasks, which as of this writing is 384, but we'll include a little buffer. + /// + public const int ContinuationIndexBits = 10; + /// + /// Covers bits [30, 32) in the packed representation. Refers to which continuation processor should be used for this subpair. + /// + public const int ContinuationTypeBits = 2; + + public const int ExclusiveMaximumChildIndex = 1 << ChildIndexBits; + public const int ExclusiveMaximumContinuationIndex = 1 << ContinuationIndexBits; + public const int ExclusiveMaximumContinuationType = 1 << ContinuationTypeBits; + + const int TypeShift = ChildIndexBits + ContinuationIndexBits; + const int IndexShift = ChildIndexBits; + const int IndexMask = (1 << ContinuationIndexBits) - 1; + const int ChildIndexMask = (1 << ChildIndexBits) - 1; + [MethodImpl(MethodImplOptions.AggressiveInlining)] public PairContinuation(int pairId, int childA, int childB, CollisionContinuationType continuationType, int continuationIndex, int continuationChildIndex) { PairId = pairId; ChildA = childA; ChildB = childB; - //continuationChildIndex: [0, 17] - //continuationIndex: [18, 27] - //continuationType: [28, 31] - Debug.Assert(continuationIndex < (1 << 10)); - Debug.Assert(continuationChildIndex < (1 << 18)); - Debug.Assert((int)continuationType < (1 << 4)); - Packed = (uint)(((int)continuationType << 28) | (continuationIndex << 18) | continuationChildIndex); + Debug.Assert(continuationChildIndex < ExclusiveMaximumChildIndex); + Debug.Assert(continuationIndex < ExclusiveMaximumContinuationIndex); + Debug.Assert((int)continuationType < ExclusiveMaximumContinuationType); + Packed = (uint)(((int)continuationType << TypeShift) | (continuationIndex << IndexShift) | continuationChildIndex); } public PairContinuation(int pairId) { @@ -108,9 +129,9 @@ public PairContinuation(int pairId) Packed = 0; } - public CollisionContinuationType Type { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { return (CollisionContinuationType)(Packed >> 28); } } - public int Index { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { return (int)((Packed >> 18) & ((1 << 10) - 1)); } } - public int ChildIndex { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { return (int)(Packed & ((1 << 18) - 1)); } } + public CollisionContinuationType Type { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { return (CollisionContinuationType)(Packed >> TypeShift); } } + public int Index { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { return (int)((Packed >> IndexShift) & IndexMask); } } + public int ChildIndex { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { return (int)(Packed & ChildIndexMask); } } } public struct BatcherContinuations where T : unmanaged, ICollisionTestContinuation diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairCollisionTask.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairCollisionTask.cs index 792d56c1f..f0052f682 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairCollisionTask.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairCollisionTask.cs @@ -39,7 +39,7 @@ public class CompoundPairCollisionTask(ref UntypedList batch, ref ref var pair = ref pairs[pairIndex]; if (totalOverlapCountForPair > 0) { + Debug.Assert(totalOverlapCountForPair < PairContinuation.ExclusiveMaximumChildIndex, "Are there REALLY supposed to be that many overlaps? Might need to expand the packed representation if so."); ref var continuation = ref continuationHandler.CreateContinuation(ref batcher, totalOverlapCountForPair, ref pairOverlaps, ref subpairQueries, pair, out var continuationIndex); var nextContinuationChildIndex = 0; @@ -94,6 +95,11 @@ public unsafe override void ExecuteBatch(ref UntypedList batch, ref childB = originalChildIndexB; } var continuationChildIndex = nextContinuationChildIndex++; + if (continuationChildIndex >= PairContinuation.ExclusiveMaximumChildIndex) + { + //If there are more overlaps than we can represent in the packed index, just ignore the surplus. This isn't wonderful, but it's better than an access violation. + break; + } var subpairContinuation = new PairContinuation(pair.Continuation.PairId, childA, childB, continuationHandler.CollisionContinuationType, continuationIndex, continuationChildIndex); if (batcher.Callbacks.AllowCollisionTesting(pair.Continuation.PairId, childA, childB)) diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundCollisionTask.cs b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundCollisionTask.cs index 7b38dede8..6b768df17 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundCollisionTask.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundCollisionTask.cs @@ -31,7 +31,7 @@ public class ConvexCompoundCollisionTask(ref UntypedList batch, ref if (pairOverlaps.Count > 0) { ref var compound = ref Unsafe.AsRef(pair.B); + //If there are more overlaps than we can represent in the packed index, just ignore the surplus. This isn't wonderful, but it's better than an access violation. + Debug.Assert(pairOverlaps.Count < PairContinuation.ExclusiveMaximumChildIndex, "Are there REALLY supposed to be that many overlaps? Might need to expand the packed representation if so."); + if (pairOverlaps.Count >= PairContinuation.ExclusiveMaximumChildIndex) + pairOverlaps.Count = PairContinuation.ExclusiveMaximumChildIndex - 1; ref var continuation = ref continuationHandler.CreateContinuation(ref batcher, pairOverlaps.Count, pair, pairQuery, out var continuationIndex); int nextContinuationChildIndex = 0; diff --git a/BepuUtilities/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj index b7693ca3d..aae4f2cc4 100644 --- a/BepuUtilities/BepuUtilities.csproj +++ b/BepuUtilities/BepuUtilities.csproj @@ -3,7 +3,7 @@ BepuUtilities BepuUtilities net6.0 - 2.5.0-beta.4 + 2.5.0-beta.5 Bepu Entertainment LLC Ross Nordby Supporting utilities library for BEPUphysics v2. diff --git a/Demos/SpecializedTests/BatchedCollisionTests.cs b/Demos/SpecializedTests/BatchedCollisionTests.cs index b5538bbee..f9f449d0d 100644 --- a/Demos/SpecializedTests/BatchedCollisionTests.cs +++ b/Demos/SpecializedTests/BatchedCollisionTests.cs @@ -74,6 +74,7 @@ unsafe static void Test(ref TA a, ref TB b, ref Buffer posesA var end = Stopwatch.GetTimestamp(); var time = (end - start) / (double)Stopwatch.Frequency; Console.WriteLine($"Completed {count} {typeof(TA).Name}-{typeof(TB).Name} pairs, time (ms): {1e3 * time}, time per pair (ns): {1e9 * time / *callbacks.Count}"); + //Console.WriteLine($"{typeof(TA).Name}-{typeof(TB).Name}, {1e9 * time / *callbacks.Count}"); //Console.WriteLine($"{typeof(TA).Name}-{typeof(TB).Name} {1e9 * time / *callbacks.Count}"); } From 707d4a2278dde919d743da7b9c0c4074c9e8f0b1 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 17 Jun 2022 18:30:38 -0500 Subject: [PATCH 492/947] Compound/BigCompound now have Add/RemoveAt. --- BepuPhysics/Collidables/BigCompound.cs | 38 ++++++++++++++++++++++++++ BepuPhysics/Collidables/Compound.cs | 28 +++++++++++++++++++ 2 files changed, 66 insertions(+) diff --git a/BepuPhysics/Collidables/BigCompound.cs b/BepuPhysics/Collidables/BigCompound.cs index a172c3b96..2b04a5eb1 100644 --- a/BepuPhysics/Collidables/BigCompound.cs +++ b/BepuPhysics/Collidables/BigCompound.cs @@ -154,6 +154,44 @@ public ref CompoundChild GetChild(int compoundChildIndex) return ref Children[compoundChildIndex]; } + /// + /// Adds a child to the compound. + /// + /// Child to add to the compound. + /// Pool to use to resize the compound's children buffer if necessary. + /// Shapes collection containing the compound's children. + /// This function keeps the in a valid state, but significant changes over time may degrade the tree's quality and result in reduced collision/query performance. + /// If this happens, consider calling with a refinementIndex that changes with each call (to prioritize different parts of the tree). + /// Incrementing a counter with each call would work fine. The ideal frequency of refinement depends on the kind of modifications being made, but it's likely to be rare. + public void Add(CompoundChild child, BufferPool pool, Shapes shapes) + { + pool.Resize(ref Children, Children.Length + 1, Children.Length); + Children[^1] = child; + shapes.UpdateBounds(child.LocalPose, child.ShapeIndex, out var bounds); + var childIndex = Tree.Add(bounds, pool); + Debug.Assert(childIndex == Children.Length - 1, "Adding to a tree acts like appending to the list; a newly added element should be in the last slot to match our previous child modification."); + } + + /// + /// Removes a child from the compound by index. The last child is pulled to fill the gap left by the removed child. + /// + /// Index of the child to remove from the compound. + /// Pool to use to resize the compound's children buffer if necessary. + /// This function keeps the in a valid state, but significant changes over time may degrade the tree's quality and result in reduced collision/query performance. + /// If this happens, consider calling with a refinementIndex that changes with each call (to prioritize different parts of the tree). + /// Incrementing a counter with each call would work fine. The ideal frequency of refinement depends on the kind of modifications being made, but it's likely to be rare. + public void RemoveAt(int childIndex, BufferPool pool) + { + var movedChildIndex = Tree.RemoveAt(childIndex); + if (movedChildIndex >= 0) + { + Children[childIndex] = Children[movedChildIndex]; + Debug.Assert(movedChildIndex == Children.Length - 1, "The child moved by the tree is expected to be the last one; it's pulled forward to fill the gap left by the removed child."); + } + //Shrinking the buffer takes care of 'removing' the now-empty last slot. + pool.Resize(ref Children, Children.Length - 1, Children.Length - 1); + } + unsafe struct Enumerator : IBreakableForEach where TSubpairOverlaps : ICollisionTaskSubpairOverlaps { public BufferPool Pool; diff --git a/BepuPhysics/Collidables/Compound.cs b/BepuPhysics/Collidables/Compound.cs index 8073e9473..7b8379960 100644 --- a/BepuPhysics/Collidables/Compound.cs +++ b/BepuPhysics/Collidables/Compound.cs @@ -235,6 +235,34 @@ public ref CompoundChild GetChild(int compoundChildIndex) return ref Children[compoundChildIndex]; } + /// + /// Adds a child to the compound. + /// + /// Child to add to the compound. + /// Pool to use to resize the compound's children buffer if necessary. + public void Add(CompoundChild child, BufferPool pool) + { + pool.Resize(ref Children, Children.Length + 1, Children.Length); + Children[^1] = child; + } + + /// + /// Removes a child from the compound by index. The last child is pulled to fill the gap left by the removed child. + /// + /// Index of the child to remove from the compound. + /// Pool to use to resize the compound's children buffer if necessary. + public void RemoveAt(int childIndex, BufferPool pool) + { + var lastIndex = Children.Length - 1; + if (childIndex < lastIndex) + { + Children[childIndex] = Children[lastIndex]; + } + //Shrinking the buffer takes care of 'removing' the now-empty last slot. + pool.Resize(ref Children, Children.Length - 1, Children.Length - 1); + } + + public unsafe void FindLocalOverlaps(ref Buffer pairs, BufferPool pool, Shapes shapes, ref TOverlaps overlaps) where TOverlaps : struct, ICollisionTaskOverlaps where TSubpairOverlaps : struct, ICollisionTaskSubpairOverlaps From fc2a6253eb6e3d944724d225ab3eb920d934b23b Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 18 Jun 2022 19:14:21 -0500 Subject: [PATCH 493/947] Updated BepuUtilitiesTests. Symmetric3x3 gained some operators, but didn't bother actually making them efficient. --- BepuUtilities/Matrix3x3.cs | 2 +- BepuUtilities/Symmetric3x3.cs | 257 ++++++++++++++++++- BepuUtilitiesTests/BepuUtilitiesTests.csproj | 5 +- BepuUtilitiesTests/BepuUtilitiesTests.sln | 9 +- BepuUtilitiesTests/Helper.cs | 4 +- BepuUtilitiesTests/Program.cs | 2 + BepuUtilitiesTests/SymmetricTests.cs | 108 ++++++++ 7 files changed, 371 insertions(+), 16 deletions(-) create mode 100644 BepuUtilitiesTests/SymmetricTests.cs diff --git a/BepuUtilities/Matrix3x3.cs b/BepuUtilities/Matrix3x3.cs index ed6f37c84..b8fed1c0c 100644 --- a/BepuUtilities/Matrix3x3.cs +++ b/BepuUtilities/Matrix3x3.cs @@ -77,7 +77,7 @@ public static void Subtract(in Matrix3x3 a, in Matrix3x3 b, out Matrix3x3 result unsafe static void Transpose(M* m, M* transposed) { //A weird function! Why? - //1) Missing some helpful instructions for actual SIMD accelerated transposition. + //1) Missing some helpful instructions for actual SIMD accelerated transposition. (TODO: This is no longer the case! Could improve this!) //2) Difficult to get SIMD types to generate competitive codegen due to lots of componentwise access. float m12 = m->M12; diff --git a/BepuUtilities/Symmetric3x3.cs b/BepuUtilities/Symmetric3x3.cs index 56f7e1306..4771e1496 100644 --- a/BepuUtilities/Symmetric3x3.cs +++ b/BepuUtilities/Symmetric3x3.cs @@ -37,6 +37,10 @@ public struct Symmetric3x3 /// public float ZZ; + //TODO: Worth noting that none of the implementations in here are optimized anywhere close to what's possible. + //The non-wide version of the Symmetric3x3 isn't used anywhere extremely performance sensitive. + //Could use some improvements, though. + /// /// Computes rT * m * r for a symmetric matrix m and a rotation matrix r. /// @@ -83,6 +87,18 @@ public static float Determinant(in Symmetric3x3 m) return m11 * m.XX + m21 * m.YX + m31 * m.ZX; } + /// + /// Inverts the given matix. + /// + /// Matrix to be inverted. + /// Inverted matrix. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public unsafe static Symmetric3x3 Invert(Symmetric3x3 m) + { + Invert(m, out var inverse); + return inverse; + } + /// /// Inverts the given matix. /// @@ -127,11 +143,30 @@ public static void Add(in Symmetric3x3 a, in Symmetric3x3 b, out Symmetric3x3 re } /// - /// Subtracts the components of b from a. + /// Adds the components of two matrices together. /// - /// Matrix to be subtracted from. - /// Matrix to subtract from the first matrix.. - /// Matrix with subtracted components. + /// First matrix to add. + /// Second matrix to add. + /// Matrix with components equal to the components of the two input matrices added together. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Symmetric3x3 operator +(Symmetric3x3 a, Symmetric3x3 b) + { + Symmetric3x3 result; + result.XX = a.XX + b.XX; + result.YX = a.YX + b.YX; + result.YY = a.YY + b.YY; + result.ZX = a.ZX + b.ZX; + result.ZY = a.ZY + b.ZY; + result.ZZ = a.ZZ + b.ZZ; + return result; + } + + /// + /// Subtracts the components of matrix b from matrix a. + /// + /// Matrix to be subtracted from + /// Matrix to subtract from matrix a. + /// Matrix with components equal to the components of the matrix a minus the components of matrix b. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Subtract(in Symmetric3x3 a, in Symmetric3x3 b, out Symmetric3x3 result) { @@ -143,6 +178,25 @@ public static void Subtract(in Symmetric3x3 a, in Symmetric3x3 b, out Symmetric3 result.ZZ = a.ZZ - b.ZZ; } + /// + /// Subtracts the components of matrix b from matrix a. + /// + /// Matrix to be subtracted from + /// Matrix to subtract from matrix a. + /// Matrix with components equal to the components of the matrix a minus the components of matrix b. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Symmetric3x3 operator -(Symmetric3x3 a, Symmetric3x3 b) + { + Symmetric3x3 result; + result.XX = a.XX - b.XX; + result.YX = a.YX - b.YX; + result.YY = a.YY - b.YY; + result.ZX = a.ZX - b.ZX; + result.ZY = a.ZY - b.ZY; + result.ZZ = a.ZZ - b.ZZ; + return result; + } + /// /// Adds the components of two matrices together. /// @@ -160,6 +214,44 @@ public static void Add(in Matrix3x3 a, in Symmetric3x3 b, out Matrix3x3 result) result.Z = a.Z + bZ; } + /// + /// Adds the components of two matrices together. + /// + /// First matrix to add. + /// Second matrix to add. + /// Matrix with components equal to the components of the two input matrices added together. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix3x3 operator +(Matrix3x3 a, Symmetric3x3 b) + { + Matrix3x3 result; + var bX = new Vector3(b.XX, b.YX, b.ZX); + var bY = new Vector3(b.YX, b.YY, b.ZY); + var bZ = new Vector3(b.ZX, b.ZY, b.ZZ); + result.X = a.X + bX; + result.Y = a.Y + bY; + result.Z = a.Z + bZ; + return result; + } + + /// + /// Adds the components of two matrices together. + /// + /// First matrix to add. + /// Second matrix to add. + /// Matrix with components equal to the components of the two input matrices added together. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix3x3 operator +(Symmetric3x3 a, Matrix3x3 b) + { + Matrix3x3 result; + var aX = new Vector3(a.XX, a.YX, a.ZX); + var aY = new Vector3(a.YX, a.YY, a.ZY); + var aZ = new Vector3(a.ZX, a.ZY, a.ZZ); + result.X = b.X + aX; + result.Y = b.Y + aY; + result.Z = b.Z + aZ; + return result; + } + /// /// Subtracts the components of one matrix from another. /// @@ -177,6 +269,44 @@ public static void Subtract(in Matrix3x3 a, in Symmetric3x3 b, out Matrix3x3 res result.Z = a.Z - bZ; } + /// + /// Subtracts the components of matrix b from matrix a. + /// + /// Matrix to be subtracted from + /// Matrix to subtract from matrix a. + /// Matrix with components equal to the components of the matrix a minus the components of matrix b. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix3x3 operator -(Matrix3x3 a, Symmetric3x3 b) + { + var bX = new Vector3(b.XX, b.YX, b.ZX); + var bY = new Vector3(b.YX, b.YY, b.ZY); + var bZ = new Vector3(b.ZX, b.ZY, b.ZZ); + Matrix3x3 result; + result.X = a.X - bX; + result.Y = a.Y - bY; + result.Z = a.Z - bZ; + return result; + } + + /// + /// Subtracts the components of matrix b from matrix a. + /// + /// Matrix to be subtracted from + /// Matrix to subtract from matrix a. + /// Matrix with components equal to the components of the matrix a minus the components of matrix b. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix3x3 operator -(Symmetric3x3 a, Matrix3x3 b) + { + var aX = new Vector3(a.XX, a.YX, a.ZX); + var aY = new Vector3(a.YX, a.YY, a.ZY); + var aZ = new Vector3(a.ZX, a.ZY, a.ZZ); + Matrix3x3 result; + result.X = aX - b.X; + result.Y = aY - b.Y; + result.Z = aZ - b.Z; + return result; + } + /// /// Multiplies every component in the matrix by the given scale. /// @@ -194,6 +324,25 @@ public static void Scale(in Symmetric3x3 m, float scale, out Symmetric3x3 scaled scaled.ZZ = m.ZZ * scale; } + /// + /// Multiplies every component in the matrix by the given scale. + /// + /// Matrix to be scaled. + /// Scale to apply to every component of the original matrix. + /// Scaled result. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Symmetric3x3 operator *(Symmetric3x3 m, float scale) + { + Symmetric3x3 scaled; + scaled.XX = m.XX * scale; + scaled.YX = m.YX * scale; + scaled.YY = m.YY * scale; + scaled.ZX = m.ZX * scale; + scaled.ZY = m.ZY * scale; + scaled.ZZ = m.ZZ * scale; + return scaled; + } + /// /// Multiplies the two matrices as if they were symmetric. /// @@ -216,6 +365,29 @@ public static void MultiplyWithoutOverlap(in Symmetric3x3 a, in Symmetric3x3 b, result.ZZ = azxbzx + azybzy + a.ZZ * b.ZZ; } + /// + /// Multiplies the two matrices as if they were symmetric. + /// + /// First matrix to multiply. + /// Second matrix to multiply. + /// Product of the multiplication. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Symmetric3x3 operator *(Symmetric3x3 a, Symmetric3x3 b) + { + var ayxbyx = a.YX * b.YX; + var azxbzx = a.ZX * b.ZX; + var azybzy = a.ZY * b.ZY; + Symmetric3x3 result; + result.XX = a.XX * b.XX + ayxbyx + azxbzx; + + result.YX = a.YX * b.XX + a.YY * b.YX + a.ZY * b.ZX; + result.YY = ayxbyx + a.YY * b.YY + azybzy; + + result.ZX = a.ZX * b.XX + a.ZY * b.YX + a.ZZ * b.ZX; + result.ZY = a.ZX * b.YX + a.ZY * b.YY + a.ZZ * b.ZY; + result.ZZ = azxbzx + azybzy + a.ZZ * b.ZZ; + return result; + } /// /// Multiplies the two matrices. @@ -251,6 +423,67 @@ public static void Multiply(in Matrix3x3 a, in Symmetric3x3 b, out Matrix3x3 res } } + + /// + /// Multiplies the two matrices. + /// + /// First matrix to multiply. + /// Second matrix to multiply. + /// Product of the multiplication. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix3x3 operator *(Matrix3x3 a, Symmetric3x3 b) + { + var bX = new Vector3(b.XX, b.YX, b.ZX); + var bY = new Vector3(b.YX, b.YY, b.ZY); + var bZ = new Vector3(b.ZX, b.ZY, b.ZZ); + Matrix3x3 result; + { + var x = new Vector3(a.X.X); + var y = new Vector3(a.X.Y); + var z = new Vector3(a.X.Z); + result.X = x * bX + y * bY + z * bZ; + } + + { + var x = new Vector3(a.Y.X); + var y = new Vector3(a.Y.Y); + var z = new Vector3(a.Y.Z); + result.Y = x * bX + y * bY + z * bZ; + } + + { + var x = new Vector3(a.Z.X); + var y = new Vector3(a.Z.Y); + var z = new Vector3(a.Z.Z); + result.Z = x * bX + y * bY + z * bZ; + } + return result; + } + + + + /// + /// Multiplies the two matrices. + /// + /// First matrix to multiply. + /// Second matrix to multiply. + /// Product of the multiplication. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix3x3 operator *(Symmetric3x3 a, Matrix3x3 b) + { + var aXX = new Vector3(a.XX); + var aYX = new Vector3(a.YX); + var aYY = new Vector3(a.YY); + var aZX = new Vector3(a.ZX); + var aZY = new Vector3(a.ZY); + var aZZ = new Vector3(a.ZZ); + Matrix3x3 result; + result.X = aXX * b.X + aYX * b.Y + aZX * b.Z; + result.Y = aYX * b.X + aYY * b.Y + aZY * b.Z; + result.Z = aZX * b.X + aZY * b.Y + aZZ * b.Z; + return result; + } + /// /// Transforms a vector by a symmetric matrix. /// @@ -265,6 +498,22 @@ public static void TransformWithoutOverlap(in Vector3 v, in Symmetric3x3 m, out result.Z = v.X * m.ZX + v.Y * m.ZY + v.Z * m.ZZ; } + /// + /// Transforms a vector by a symmetric matrix. + /// + /// Vector to transform. + /// Matrix to interpret as symmetric transform. + /// Result of transforming the vector by the given symmetric matrix. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3 Transform(Vector3 v, Symmetric3x3 m) + { + Vector3 result; + result.X = v.X * m.XX + v.Y * m.YX + v.Z * m.ZX; + result.Y = v.X * m.YX + v.Y * m.YY + v.Z * m.ZY; + result.Z = v.X * m.ZX + v.Y * m.ZY + v.Z * m.ZZ; + return result; + } + public override string ToString() { return $"x: {XX}, y: {YX}, {YY}, z: {ZX}, {ZY}, {ZZ}"; diff --git a/BepuUtilitiesTests/BepuUtilitiesTests.csproj b/BepuUtilitiesTests/BepuUtilitiesTests.csproj index ced0fb94e..dbf3e8363 100644 --- a/BepuUtilitiesTests/BepuUtilitiesTests.csproj +++ b/BepuUtilitiesTests/BepuUtilitiesTests.csproj @@ -1,10 +1,11 @@ - + Exe - netcoreapp2.0 + net6.0 BepuUtilitiesTests BepuUtilitiesTests + diff --git a/BepuUtilitiesTests/BepuUtilitiesTests.sln b/BepuUtilitiesTests/BepuUtilitiesTests.sln index 6b90f155d..c71e86c77 100644 --- a/BepuUtilitiesTests/BepuUtilitiesTests.sln +++ b/BepuUtilitiesTests/BepuUtilitiesTests.sln @@ -1,7 +1,7 @@  Microsoft Visual Studio Solution File, Format Version 12.00 -# Visual Studio Version 16 -VisualStudioVersion = 16.0.28803.156 +# Visual Studio Version 17 +VisualStudioVersion = 17.3.32611.2 MinimumVisualStudioVersion = 10.0.40219.1 Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "BepuUtilitiesTests", "BepuUtilitiesTests.csproj", "{E7E8D508-C3DF-4B6E-A305-D67A63862817}" EndProject @@ -11,21 +11,16 @@ Global GlobalSection(SolutionConfigurationPlatforms) = preSolution Debug|Any CPU = Debug|Any CPU Release|Any CPU = Release|Any CPU - ReleaseStrip|Any CPU = ReleaseStrip|Any CPU EndGlobalSection GlobalSection(ProjectConfigurationPlatforms) = postSolution {E7E8D508-C3DF-4B6E-A305-D67A63862817}.Debug|Any CPU.ActiveCfg = Debug|Any CPU {E7E8D508-C3DF-4B6E-A305-D67A63862817}.Debug|Any CPU.Build.0 = Debug|Any CPU {E7E8D508-C3DF-4B6E-A305-D67A63862817}.Release|Any CPU.ActiveCfg = Release|Any CPU {E7E8D508-C3DF-4B6E-A305-D67A63862817}.Release|Any CPU.Build.0 = Release|Any CPU - {E7E8D508-C3DF-4B6E-A305-D67A63862817}.ReleaseStrip|Any CPU.ActiveCfg = Release|Any CPU - {E7E8D508-C3DF-4B6E-A305-D67A63862817}.ReleaseStrip|Any CPU.Build.0 = Release|Any CPU {FB243E81-90EA-4619-A480-0C9FB760B169}.Debug|Any CPU.ActiveCfg = Debug|Any CPU {FB243E81-90EA-4619-A480-0C9FB760B169}.Debug|Any CPU.Build.0 = Debug|Any CPU {FB243E81-90EA-4619-A480-0C9FB760B169}.Release|Any CPU.ActiveCfg = Release|Any CPU {FB243E81-90EA-4619-A480-0C9FB760B169}.Release|Any CPU.Build.0 = Release|Any CPU - {FB243E81-90EA-4619-A480-0C9FB760B169}.ReleaseStrip|Any CPU.ActiveCfg = ReleaseStrip|Any CPU - {FB243E81-90EA-4619-A480-0C9FB760B169}.ReleaseStrip|Any CPU.Build.0 = ReleaseStrip|Any CPU EndGlobalSection GlobalSection(SolutionProperties) = preSolution HideSolutionNode = FALSE diff --git a/BepuUtilitiesTests/Helper.cs b/BepuUtilitiesTests/Helper.cs index 3a0fd6230..56f1bd2e2 100644 --- a/BepuUtilitiesTests/Helper.cs +++ b/BepuUtilitiesTests/Helper.cs @@ -9,10 +9,10 @@ namespace BEPUutilitiesTests { public static class Helper { - public static void Test(string testName, Func function, int benchmarkIterations = 100000000) + public static void Test(string testName, Func function, int benchmarkIterations = 100000000, int warmupIterations = 8192) { GC.Collect(); - function(10); + function(warmupIterations); var start = Stopwatch.GetTimestamp(); var accumulator = function(benchmarkIterations); var end = Stopwatch.GetTimestamp(); diff --git a/BepuUtilitiesTests/Program.cs b/BepuUtilitiesTests/Program.cs index e71945a87..a500fdfe4 100644 --- a/BepuUtilitiesTests/Program.cs +++ b/BepuUtilitiesTests/Program.cs @@ -18,6 +18,8 @@ static void Main(string[] args) Matrix3x3Tests.Test(); Console.WriteLine(); Matrix4x4Tests.Test(); + Console.WriteLine(); + SymmetricTests.Test(); } diff --git a/BepuUtilitiesTests/SymmetricTests.cs b/BepuUtilitiesTests/SymmetricTests.cs new file mode 100644 index 000000000..f18578429 --- /dev/null +++ b/BepuUtilitiesTests/SymmetricTests.cs @@ -0,0 +1,108 @@ +using BepuUtilities; +using System; +using System.Numerics; + +namespace BEPUutilitiesTests +{ + public static class SymmetricTests + { + public static float TestAddition(int iterationCount) + { + Symmetric3x3 m0 = new() { XX = 4, YY = 1, ZZ = 3, YX = 5, ZX = 6, ZY = 4 }; + Symmetric3x3 m1 = new() { XX = 1, YY = 2, ZZ = 1, YX = -1, ZX = 14, ZY = 8 }; + for (int i = 0; i < iterationCount; ++i) + { + Symmetric3x3.Add(m0, m0, out m0); + Symmetric3x3.Add(m1, m1, out m1); + Symmetric3x3.Add(m0, m1, out m0); + Symmetric3x3.Add(m0, m0, out m0); + Symmetric3x3.Add(m1, m1, out m1); + Symmetric3x3.Add(m0, m1, out m0); + Symmetric3x3.Add(m0, m0, out m0); + Symmetric3x3.Add(m1, m1, out m1); + Symmetric3x3.Add(m0, m1, out m0); + } + return m0.XX; + } + public static float TestOperatorAddition(int iterationCount) + { + Symmetric3x3 m0 = new() { XX = 4, YY = 1, ZZ = 3, YX = 5, ZX = 6, ZY = 4 }; + Symmetric3x3 m1 = new() { XX = 1, YY = 2, ZZ = 1, YX = -1, ZX = 14, ZY = 8 }; + for (int i = 0; i < iterationCount; ++i) + { + m0 = m0 + m0; + m1 = m1 + m1; + m0 = m0 + m1; + m0 = m0 + m0; + m1 = m1 + m1; + m0 = m0 + m1; + m0 = m0 + m0; + m1 = m1 + m1; + m0 = m0 + m1; + } + return m0.XX; + } + + public static float TestMultiplication(int iterationCount) + { + Symmetric3x3 m0 = new() { XX = .5f, YY = 0.75f, ZZ = 0.25f, YX = 1.1f, ZX = 1.2f, ZY = 0.3f }; + Symmetric3x3 m1; + Symmetric3x3 m2 = new() { XX = .025f, YY = 0.0575f, ZZ = 0.0425f, YX = .1f, ZX = .2f, ZY = 0.53f }; + for (int i = 0; i < iterationCount; ++i) + { + Symmetric3x3.MultiplyWithoutOverlap(m0, m2, out m1); + Symmetric3x3.MultiplyWithoutOverlap(m1, m2, out m0); + Symmetric3x3.MultiplyWithoutOverlap(m0, m1, out m2); + Symmetric3x3.MultiplyWithoutOverlap(m0, m2, out m1); + Symmetric3x3.MultiplyWithoutOverlap(m1, m2, out m0); + Symmetric3x3.MultiplyWithoutOverlap(m0, m1, out m2); + Symmetric3x3.MultiplyWithoutOverlap(m0, m2, out m1); + Symmetric3x3.MultiplyWithoutOverlap(m1, m2, out m0); + Symmetric3x3.MultiplyWithoutOverlap(m0, m1, out m2); + } + return m0.XX; + } + public static float TestOperatorMultiplication(int iterationCount) + { + Symmetric3x3 m0 = new() { XX = .5f, YY = 0.75f, ZZ = 0.25f, YX = 1.1f, ZX = 1.2f, ZY = 0.3f }; + Symmetric3x3 m1; + Symmetric3x3 m2 = new() { XX = .025f, YY = 0.0575f, ZZ = 0.0425f, YX = .1f, ZX = .2f, ZY = 0.53f }; + for (int i = 0; i < iterationCount; ++i) + { + m1 = m0 * m2; + m0 = m1 * m2; + m2 = m0 * m1; + m1 = m0 * m2; + m0 = m1 * m2; + m2 = m0 * m1; + m1 = m0 * m2; + m0 = m1 * m2; + m2 = m0 * m1; + } + return m0.XX; + } + + + public unsafe static void Test() + { + Console.WriteLine("Symmetric3x3 RESULTS:"); + Console.WriteLine($"Size: {sizeof(Symmetric3x3)}"); + const int iterationCount = 10000000; + + var a = new Matrix3x3 { X = new Vector3(1, 2, 3), Y = new Vector3(2, 3, 4), Z = new Vector3(-3, -4, -5) }; + var b = new Symmetric3x3 { XX = 1, YX = 2, ZX = 3, YY = 4, ZY = 5, ZZ = 6 }; + var c = a * b; + Symmetric3x3.Multiply(a, b, out var c2); + var d = b * a; + var b2 = new Matrix3x3 { X = new Vector3(b.XX, b.YX, b.ZX), Y = new Vector3(b.YX, b.YY, b.ZY), Z = new Vector3(b.ZX, b.ZY, b.ZZ) }; + var d2 = b2 * a; + + Helper.Test("Symmetric3x3.Add", TestAddition, iterationCount); + Helper.Test("Symmetric3x3.+", TestOperatorAddition, iterationCount); + + Helper.Test("Symmetric3x3.MultiplyWithoutOverlap", TestMultiplication, iterationCount); + Helper.Test("Symmetric3x3.*", TestOperatorMultiplication, iterationCount); + + } + } +} From 79052defff418ee9c1d9bc83af4fbeeadd498580 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 20 Jun 2022 18:25:43 -0500 Subject: [PATCH 494/947] Added compound inertia test case and fixed child orientation related bug in the CompoundBuilder. --- ...{CompoundHelpers.cs => CompoundBuilder.cs} | 128 ++++++++++----- DemoTests/InertiaTensorTests.cs | 151 +++++++++++++++--- Demos/Demos/Cars/CarDemo.cs | 2 - 3 files changed, 214 insertions(+), 67 deletions(-) rename BepuPhysics/Collidables/{CompoundHelpers.cs => CompoundBuilder.cs} (73%) diff --git a/BepuPhysics/Collidables/CompoundHelpers.cs b/BepuPhysics/Collidables/CompoundBuilder.cs similarity index 73% rename from BepuPhysics/Collidables/CompoundHelpers.cs rename to BepuPhysics/Collidables/CompoundBuilder.cs index 42de8cfec..9e0f3bd39 100644 --- a/BepuPhysics/Collidables/CompoundHelpers.cs +++ b/BepuPhysics/Collidables/CompoundBuilder.cs @@ -28,52 +28,24 @@ public struct Child /// public float Weight; /// - /// Inertia tensor associated with the child. If inertia is all zeroes, it is interpreted as infinite. + /// Inertia tensor associated with the child, including its orientation. /// public Symmetric3x3 Inertia; } public QuickList Children; - public CompoundBuilder(BufferPool pool, Shapes shapes, int builderCapacity) - { - Pool = pool; - Shapes = shapes; - Children = new QuickList(builderCapacity, Pool); - } - - /// - /// Adds a new shape to the accumulator, creating a new shape in the shapes set. The mass used to compute the inertia tensor will be based on the given weight. - /// - /// Type of the shape to add to the accumulator and the shapes set. - /// Shape to add. - /// Pose of the shape in the compound's local space. - /// Weight of the shape. If the compound is interpreted as a dynamic, this will be used as the mass and scales the inertia tensor. - /// Otherwise, it is used for recentering. - public void Add(in TShape shape, in RigidPose localPose, float weight) where TShape : unmanaged, IConvexShape - { - ref var child = ref Children.Allocate(Pool); - child.LocalPose = localPose; - child.ShapeIndex = Shapes.Add(shape); - child.Weight = weight; - var inertia = shape.ComputeInertia(weight); - Symmetric3x3.Invert(inertia.InverseInertiaTensor, out child.Inertia); - } - /// - /// Adds a new shape to the accumulator, creating a new shape in the shapes set. Inertia is assumed to be infinite. + /// Creates a compound builder. /// - /// Type of the shape to add to the accumulator and the shapes set. - /// Shape to add. - /// Pose of the shape in the compound's local space. - /// Weight of the shape. If the compound is interpreted as a dynamic, this will be used as the mass. Otherwise, it is used for recentering. - public void AddForKinematic(in TShape shape, in RigidPose localPose, float weight) where TShape : unmanaged, IConvexShape + /// Buffer pool to allocate memory from when necessary. + /// Shapes collection to access when constructing the compound children. + /// Number of children the compound builder can hold without resizing. + public CompoundBuilder(BufferPool pool, Shapes shapes, int initialBuilderCapacity) { - ref var child = ref Children.Allocate(Pool); - child.LocalPose = localPose; - child.ShapeIndex = Shapes.Add(shape); - child.Weight = weight; - child.Inertia = default; + Pool = pool; + Shapes = shapes; + Children = new QuickList(initialBuilderCapacity, Pool); } /// @@ -82,8 +54,8 @@ public void AddForKinematic(in TShape shape, in RigidPose localPose, flo /// Index of the shape to add. /// Pose of the shape in the compound's local space. /// Weight of the shape. If the compound is interpreted as a dynamic, this will be used as the mass. Otherwise, it is used for recentering. - /// Inverse inertia tensor of the shape being added. This is assumed to already be scaled as desired by the weight. - public void Add(TypedIndex shape, in RigidPose localPose, in Symmetric3x3 inverseInertia, float weight) + /// Inverse inertia tensor of the shape being added in its local space. This is assumed to already be scaled as desired by the weight. + public void Add(TypedIndex shape, in RigidPose localPose, in Symmetric3x3 localInverseInertia, float weight) { Debug.Assert(Compound.ValidateChildIndex(shape, Shapes)); ref var child = ref Children.Allocate(Pool); @@ -92,10 +64,11 @@ public void Add(TypedIndex shape, in RigidPose localPose, in Symmetric3x3 invers child.Weight = weight; //This assumes the given inertia is nonsingular. That should be a valid assumption, unless the user is trying to supply an axis-locked tensor. //For such a use case, it's best to just lock the axis after computing a 'normal' inertia. - Debug.Assert(Symmetric3x3.Determinant(inverseInertia) > 0, - "Shape inertia tensors should be invertible. If making an axis-locked compound, consider locking the axis on the completed inertia. " + + Debug.Assert(Symmetric3x3.Determinant(localInverseInertia) > 0, + "Child inertia tensors should be invertible. If making an axis-locked compound, consider locking the axis on the completed inertia. " + "If making a kinematic, consider using the overload which takes no inverse inertia."); - Symmetric3x3.Invert(inverseInertia, out child.Inertia); + PoseIntegration.RotateInverseInertia(localInverseInertia, localPose.Orientation, out var rotatedInverse); + Symmetric3x3.Invert(rotatedInverse, out child.Inertia); } /// @@ -114,8 +87,40 @@ public void AddForKinematic(TypedIndex shape, in RigidPose localPose, float weig child.Inertia = default; } + /// + /// Adds a new shape to the accumulator, creating a new shape in the shapes set. The mass used to compute the inertia tensor will be based on the given weight. + /// + /// Type of the shape to add to the accumulator and the shapes set. + /// Shape to add. + /// Pose of the shape in the compound's local space. + /// Weight of the shape. If the compound is interpreted as a dynamic, this will be used as the mass and scales the inertia tensor. + /// Otherwise, it is used for recentering. + public void Add(in TShape shape, in RigidPose localPose, float weight) where TShape : unmanaged, IConvexShape + { + Add(Shapes.Add(shape), localPose, shape.ComputeInertia(weight).InverseInertiaTensor, weight); + } + + /// + /// Adds a new shape to the accumulator, creating a new shape in the shapes set. Inertia is assumed to be infinite. + /// + /// Type of the shape to add to the accumulator and the shapes set. + /// Shape to add. + /// Pose of the shape in the compound's local space. + /// Weight of the shape. If the compound is interpreted as a dynamic, this will be used as the mass. Otherwise, it is used for recentering. + public void AddForKinematic(in TShape shape, in RigidPose localPose, float weight) where TShape : unmanaged, IConvexShape + { + AddForKinematic(Shapes.Add(shape), localPose, weight); + } + + + /// + /// Gets the contribution to an inertia tensor of a point mass at the given offset from the center of mass. + /// + /// Offset from the center of mass. + /// Mass of the point. + /// Contribution to the inertia tensor. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void GetOffsetInertiaContribution(in Vector3 offset, float mass, out Symmetric3x3 contribution) + public static void GetOffsetInertiaContribution(Vector3 offset, float mass, out Symmetric3x3 contribution) { var innerProduct = Vector3.Dot(offset, offset); contribution.XX = mass * (innerProduct - offset.X * offset.X); @@ -193,6 +198,43 @@ public void BuildDynamicCompound(out Buffer children, out BodyIne Symmetric3x3.Invert(summedInertia, out inertia.InverseInertiaTensor); } + /// + /// Computes the uninverted inertia contribution of a child. + /// + /// Pose of the child. + /// Inverse inertia tensor of the child in its local space. + /// Mass of the child. + /// Inertia contribution of the child to a compound given its relative pose. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Symmetric3x3 ComputeInertiaForChild(RigidPose pose, Symmetric3x3 inverseLocalInertia, float mass) + { + GetOffsetInertiaContribution(pose.Position, mass, out var offsetContribution); + //This assumes the given inertia is nonsingular. That should be a valid assumption, unless the user is trying to supply an axis-locked tensor. + //For such a use case, it's best to just lock the axis after computing a 'normal' inertia. + Debug.Assert(Symmetric3x3.Determinant(inverseLocalInertia) > 0, + "Child inertia tensors should be invertible. If making an axis-locked compound, consider locking the axis on the completed inertia. " + + "If making a kinematic, consider using the overload which takes no inverse inertia."); + PoseIntegration.RotateInverseInertia(inverseLocalInertia, pose.Orientation, out var rotatedInverseInertia); + Symmetric3x3.Invert(rotatedInverseInertia, out var inertia); + Symmetric3x3.Add(offsetContribution, inertia, out inertia); + return inertia; + } + + /// + /// Computes the inertia for a set of compound children based on their offsets and the provided inverse inertias. + /// + /// Children and their associated poses. + /// Inverse inertias of the children, each in the child's local space. Assumed to have already been premultiplied by the mass of the child. + /// Masses of each child in the compound. + public static void ComputeInertia(Span children, Span inverseLocalInertias, Span childMasses) + { + Symmetric3x3 summedInertia = default; + for (int i = 0; i < children.Length; ++i) + { + summedInertia += ComputeInertiaForChild(children[i].LocalPose, inverseLocalInertias[i], childMasses[i]); + } + } + /// /// Builds a buffer of compound children from the accumulated set for a kinematic compound. /// Computes a center of mass and recenters child shapes relative to it. Does not reset the accumulator. diff --git a/DemoTests/InertiaTensorTests.cs b/DemoTests/InertiaTensorTests.cs index 3e7e87718..27b1f7eae 100644 --- a/DemoTests/InertiaTensorTests.cs +++ b/DemoTests/InertiaTensorTests.cs @@ -1,5 +1,6 @@ using BepuPhysics; using BepuPhysics.Collidables; +using BepuPhysics.Trees; using BepuUtilities; using BepuUtilities.Collections; using BepuUtilities.Memory; @@ -160,8 +161,45 @@ public bool PointIsContained(ref Vector3 sampleSpacing, ref Vector3 point) } } + public static class InertiaTensorTests { + static bool ValuesAreSimilar(float a, float b) + { + var ratio = a / b; + const float ratioThreshold = 0.15f; + return MathF.Abs(a - b) < 3e-2f || (ratio < (1 + ratioThreshold) && ratio > 1f / (1 + ratioThreshold)); + } + + private static void CheckInertiaError(Symmetric3x3 numericalLocalInverseInertia, BodyInertia analyticInertia) + { + if (!ValuesAreSimilar(analyticInertia.InverseInertiaTensor.XX, numericalLocalInverseInertia.XX) || + !ValuesAreSimilar(analyticInertia.InverseInertiaTensor.YX, numericalLocalInverseInertia.YX) || + !ValuesAreSimilar(analyticInertia.InverseInertiaTensor.YY, numericalLocalInverseInertia.YY) || + !ValuesAreSimilar(analyticInertia.InverseInertiaTensor.ZX, numericalLocalInverseInertia.ZX) || + !ValuesAreSimilar(analyticInertia.InverseInertiaTensor.ZY, numericalLocalInverseInertia.ZY) || + !ValuesAreSimilar(analyticInertia.InverseInertiaTensor.ZZ, numericalLocalInverseInertia.ZZ)) + { + Assert.True(false, "Excessive error in numerical vs analytic inertia tensor."); + Console.WriteLine($"ANALYTIC INERTIA: {analyticInertia.InverseInertiaTensor} vs "); + Console.WriteLine($"NUMERICAL INERTIA: {numericalLocalInverseInertia}"); + Symmetric3x3.Subtract(analyticInertia.InverseInertiaTensor, numericalLocalInverseInertia, out var difference); + Console.WriteLine($"DIFFERENCE: {difference}"); + } + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + private static void AccumulateSampleIntoInertia(Vector3 sampleLocation, ref Symmetric3x3 numericalLocalInertia) + { + var dd = Vector3.Dot(sampleLocation, sampleLocation); + numericalLocalInertia.XX += dd - sampleLocation.X * sampleLocation.X; + numericalLocalInertia.YX += -sampleLocation.X * sampleLocation.Y; + numericalLocalInertia.YY += dd - sampleLocation.Y * sampleLocation.Y; + numericalLocalInertia.ZX += -sampleLocation.X * sampleLocation.Z; + numericalLocalInertia.ZY += -sampleLocation.Y * sampleLocation.Z; + numericalLocalInertia.ZZ += dd - sampleLocation.Z * sampleLocation.Z; + } + static void CheckInertia(ref TInertiaTester tester) where TInertiaTester : IInertiaTester { tester.ComputeBounds(out var min, out var max); @@ -183,13 +221,7 @@ static void CheckInertia(ref TInertiaTester tester) where TInert var sampleLocation = sampleMin + new Vector3(i, j, k) * sampleSpacing; if (tester.PointIsContained(ref sampleSpacing, ref sampleLocation)) { - var dd = Vector3.Dot(sampleLocation, sampleLocation); - numericalLocalInertia.XX += dd - sampleLocation.X * sampleLocation.X; - numericalLocalInertia.YX += -sampleLocation.X * sampleLocation.Y; - numericalLocalInertia.YY += dd - sampleLocation.Y * sampleLocation.Y; - numericalLocalInertia.ZX += -sampleLocation.X * sampleLocation.Z; - numericalLocalInertia.ZY += -sampleLocation.Y * sampleLocation.Z; - numericalLocalInertia.ZZ += dd - sampleLocation.Z * sampleLocation.Z; + AccumulateSampleIntoInertia(sampleLocation, ref numericalLocalInertia); ++containedSampleCount; } } @@ -199,26 +231,96 @@ static void CheckInertia(ref TInertiaTester tester) where TInert Symmetric3x3.Scale(numericalLocalInertia, mass / containedSampleCount, out numericalLocalInertia); Symmetric3x3.Invert(numericalLocalInertia, out var numericalLocalInverseInertia); tester.ComputeAnalyticInertia(mass, out var analyticInertia); - if (!ValuesAreSimilar(analyticInertia.InverseInertiaTensor.XX, numericalLocalInverseInertia.XX) || - !ValuesAreSimilar(analyticInertia.InverseInertiaTensor.YX, numericalLocalInverseInertia.YX) || - !ValuesAreSimilar(analyticInertia.InverseInertiaTensor.YY, numericalLocalInverseInertia.YY) || - !ValuesAreSimilar(analyticInertia.InverseInertiaTensor.ZX, numericalLocalInverseInertia.ZX) || - !ValuesAreSimilar(analyticInertia.InverseInertiaTensor.ZY, numericalLocalInverseInertia.ZY) || - !ValuesAreSimilar(analyticInertia.InverseInertiaTensor.ZZ, numericalLocalInverseInertia.ZZ)) + CheckInertiaError(numericalLocalInverseInertia, analyticInertia); + } + + unsafe struct HitCounter : IShapeRayHitHandler + { + public int Counter; + + public bool AllowTest(int childIndex) { - Assert.True(false, "Excessive error in numerical vs analytic inertia tensor."); - Console.WriteLine($"ANALYTIC INERTIA: {analyticInertia.InverseInertiaTensor} vs "); - Console.WriteLine($"NUMERICAL INERTIA: {numericalLocalInverseInertia}"); - Symmetric3x3.Subtract(analyticInertia.InverseInertiaTensor, numericalLocalInverseInertia, out var difference); - Console.WriteLine($"DIFFERENCE: {difference}"); + return true; + } + + public void OnRayHit(in RayData ray, ref float maximumT, float t, in Vector3 normal, int childIndex) + { + ++Counter; } } - static bool ValuesAreSimilar(float a, float b) + + public static void TestCompound(Random random, BufferPool pool) { - var ratio = a / b; - const float ratioThreshold = 0.15f; - return MathF.Abs(a - b) < 3e-2f || (ratio < (1 + ratioThreshold) && ratio > 1f / (1 + ratioThreshold)); + var shapes = new Shapes(pool, 8); + var treeCompoundBoxShape = new Box(0.5f, 1.5f, 1f); + var treeCompoundBoxShapeIndex = shapes.Add(treeCompoundBoxShape); + using var compoundBuilder = new CompoundBuilder(pool, shapes, 128); + + //This constant value isn't meaningful- it's just here to capture mass scaling bugs in implementations. + var mass = (float)Math.Sqrt(11f / MathHelper.Pi); + const int childCount = 4; + var massPerChild = mass / childCount; + var childInertia = treeCompoundBoxShape.ComputeInertia(massPerChild); + for (int i = 0; i < childCount; ++i) + { + RigidPose localPose; + localPose.Position = new Vector3(2, 4, 2) * (0.5f * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) - Vector3.One); + float orientationLengthSquared; + do + { + localPose.Orientation = new Quaternion(random.NextSingle(), random.NextSingle(), random.NextSingle(), random.NextSingle()); + orientationLengthSquared = QuaternionEx.LengthSquared(ref localPose.Orientation); + } + while (orientationLengthSquared < 1e-9f); + QuaternionEx.Scale(localPose.Orientation, 1f / MathF.Sqrt(orientationLengthSquared), out localPose.Orientation); + //localPose.Orientation = Quaternion.Identity; + //Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), MathF.PI, out localPose.Orientation); + + compoundBuilder.Add(treeCompoundBoxShapeIndex, localPose, childInertia.InverseInertiaTensor, massPerChild); + } + compoundBuilder.BuildDynamicCompound(out var children, out var analyticInertia, out var center); + var compound = new Compound(children); + + compound.ComputeBounds(Quaternion.Identity, shapes, out var min, out var max); + var span = max - min; + const int axisSampleCount = 128; + var sampleSpacing = span / axisSampleCount; + var sampleMin = min + sampleSpacing * 0.5f; + var numericalLocalInertia = new Symmetric3x3(); + + var pose = RigidPose.Identity; + var hitCounter = new HitCounter(); + float maximumT = 0.000001f; + + + for (int i = 0; i < axisSampleCount; ++i) + { + for (int j = 0; j < axisSampleCount; ++j) + { + for (int k = 0; k < axisSampleCount; ++k) + { + var sampleLocation = sampleMin + new Vector3(i, j, k) * sampleSpacing; + var previousCount = hitCounter.Counter; + compound.RayTest(pose, new RayData { Origin = sampleLocation, Direction = Vector3.UnitY }, ref maximumT, shapes, ref hitCounter); + //If the ray hit more than one shape, then we count them all. + //This matches how the analytic inertia is calculated- every shape provides its own tensor, and they're summed. + //(Notably, if you wanted non-overlapping inertia, this is counterproductive!) + for (int p = previousCount; p < hitCounter.Counter; ++p) + { + AccumulateSampleIntoInertia(sampleLocation, ref numericalLocalInertia); + } + } + } + } + + Symmetric3x3.Scale(numericalLocalInertia, mass / hitCounter.Counter, out numericalLocalInertia); + Symmetric3x3.Invert(numericalLocalInertia, out var numericalLocalInverseInertia); + CheckInertiaError(numericalLocalInverseInertia, analyticInertia); + + compound.Dispose(pool); + shapes.Dispose(); + } [Fact] @@ -277,6 +379,11 @@ public static void Test() CheckInertia(ref tester); tester.Hull.Dispose(pool); } + + for (int i = 0; i < shapeTrials; ++i) + { + TestCompound(random, pool); + } pool.Clear(); } } diff --git a/Demos/Demos/Cars/CarDemo.cs b/Demos/Demos/Cars/CarDemo.cs index ba1b2643c..a6c8eb078 100644 --- a/Demos/Demos/Cars/CarDemo.cs +++ b/Demos/Demos/Cars/CarDemo.cs @@ -131,8 +131,6 @@ public override void Initialize(ContentArchive content, Camera camera) }, new Vector3(1, 1, 1), BufferPool, out var planeMesh); Simulation.Statics.Add(new StaticDescription(new Vector3(0, -15, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), Simulation.Shapes.Add(planeMesh))); - - } bool playerControlActive = true; From f11aa64d88eba6e077ac5f663f8eb88c88121220 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 20 Jun 2022 20:00:18 -0500 Subject: [PATCH 495/947] Expanded CompoundBuilder inertia helpers. --- BepuPhysics/Collidables/CompoundBuilder.cs | 150 ++++++++++++++++++--- 1 file changed, 135 insertions(+), 15 deletions(-) diff --git a/BepuPhysics/Collidables/CompoundBuilder.cs b/BepuPhysics/Collidables/CompoundBuilder.cs index 9e0f3bd39..e7c2e9bc8 100644 --- a/BepuPhysics/Collidables/CompoundBuilder.cs +++ b/BepuPhysics/Collidables/CompoundBuilder.cs @@ -28,9 +28,9 @@ public struct Child /// public float Weight; /// - /// Inertia tensor associated with the child, including its orientation. + /// Inverse inertia tensor of the child in its local space. /// - public Symmetric3x3 Inertia; + public Symmetric3x3 LocalInverseInertia; } public QuickList Children; @@ -62,13 +62,12 @@ public void Add(TypedIndex shape, in RigidPose localPose, in Symmetric3x3 localI child.LocalPose = localPose; child.ShapeIndex = shape; child.Weight = weight; + child.LocalInverseInertia = localInverseInertia; //This assumes the given inertia is nonsingular. That should be a valid assumption, unless the user is trying to supply an axis-locked tensor. //For such a use case, it's best to just lock the axis after computing a 'normal' inertia. Debug.Assert(Symmetric3x3.Determinant(localInverseInertia) > 0, "Child inertia tensors should be invertible. If making an axis-locked compound, consider locking the axis on the completed inertia. " + "If making a kinematic, consider using the overload which takes no inverse inertia."); - PoseIntegration.RotateInverseInertia(localInverseInertia, localPose.Orientation, out var rotatedInverse); - Symmetric3x3.Invert(rotatedInverse, out child.Inertia); } /// @@ -84,7 +83,7 @@ public void AddForKinematic(TypedIndex shape, in RigidPose localPose, float weig child.LocalPose = localPose; child.ShapeIndex = shape; child.Weight = weight; - child.Inertia = default; + child.LocalInverseInertia = default; } /// @@ -158,11 +157,9 @@ public void BuildDynamicCompound(out Buffer children, out BodyIne ref var sourceChild = ref Children[i]; ref var targetChild = ref children[i]; targetChild.LocalPose.Position = sourceChild.LocalPose.Position - center; - GetOffsetInertiaContribution(targetChild.LocalPose.Position, sourceChild.Weight, out var contribution); - Symmetric3x3.Add(contribution, summedInertia, out summedInertia); - Symmetric3x3.Add(summedInertia, sourceChild.Inertia, out summedInertia); targetChild.LocalPose.Orientation = sourceChild.LocalPose.Orientation; targetChild.ShapeIndex = sourceChild.ShapeIndex; + Symmetric3x3.Add(ComputeInertiaForChild(targetChild.LocalPose, sourceChild.LocalInverseInertia, sourceChild.Weight), summedInertia, out summedInertia); } Symmetric3x3.Invert(summedInertia, out inertia.InverseInertiaTensor); } @@ -188,12 +185,9 @@ public void BuildDynamicCompound(out Buffer children, out BodyIne { ref var sourceChild = ref Children[i]; ref var targetChild = ref children[i]; - targetChild.LocalPose.Position = sourceChild.LocalPose.Position; - GetOffsetInertiaContribution(targetChild.LocalPose.Position, sourceChild.Weight, out var contribution); - Symmetric3x3.Add(contribution, summedInertia, out summedInertia); - Symmetric3x3.Add(summedInertia, sourceChild.Inertia, out summedInertia); - targetChild.LocalPose.Orientation = sourceChild.LocalPose.Orientation; + targetChild.LocalPose = sourceChild.LocalPose; targetChild.ShapeIndex = sourceChild.ShapeIndex; + Symmetric3x3.Add(ComputeInertiaForChild(sourceChild.LocalPose, sourceChild.LocalInverseInertia, sourceChild.Weight), summedInertia, out summedInertia); } Symmetric3x3.Invert(summedInertia, out inertia.InverseInertiaTensor); } @@ -221,19 +215,145 @@ public static Symmetric3x3 ComputeInertiaForChild(RigidPose pose, Symmetric3x3 i } /// - /// Computes the inertia for a set of compound children based on their offsets and the provided inverse inertias. + /// Computes the inertia for a set of compound children based on their poses and the provided inverse inertias. Does not recenter the children. /// /// Children and their associated poses. /// Inverse inertias of the children, each in the child's local space. Assumed to have already been premultiplied by the mass of the child. /// Masses of each child in the compound. - public static void ComputeInertia(Span children, Span inverseLocalInertias, Span childMasses) + /// of the compound. + public static BodyInertia ComputeInverseInertia(Span children, Span inverseLocalInertias, Span childMasses) + { + Symmetric3x3 summedInertia = default; + float massSum = 0; + for (int i = 0; i < children.Length; ++i) + { + summedInertia += ComputeInertiaForChild(children[i].LocalPose, inverseLocalInertias[i], childMasses[i]); + massSum += childMasses[i]; + } + BodyInertia inertia; + Symmetric3x3.Invert(summedInertia, out inertia.InverseInertiaTensor); + inertia.InverseMass = 1f / massSum; + return inertia; + } + + /// + /// Computes the inverse inertia for a set of compound children based on their poses and the provided inverse inertias. Does not recenter the children. + /// + /// Poses of the compound's children. + /// Inverse inertias of the children, each in the child's local space. Assumed to have already been premultiplied by the mass of the child. + /// Masses of each child in the compound. + /// of the compound. + public static BodyInertia ComputeInertia(Span childPoses, Span inverseLocalInertias, Span childMasses) + { + Symmetric3x3 summedInertia = default; + float massSum = 0; + for (int i = 0; i < childPoses.Length; ++i) + { + summedInertia += ComputeInertiaForChild(childPoses[i], inverseLocalInertias[i], childMasses[i]); + massSum += childMasses[i]; + } + BodyInertia inertia; + Symmetric3x3.Invert(summedInertia, out inertia.InverseInertiaTensor); + inertia.InverseMass = 1f / massSum; + return inertia; + } + /// + /// Computes the center of mass of a compound. + /// + /// Children of the compound. + /// Masses of the children in the compound. + /// Inverse of the sum of all child masses. + /// The compound's center of mass. + public static Vector3 ComputeCenterOfMass(Span children, Span childMasses, out float inverseMass) + { + Vector3 sum = default; + float massSum = 0; + for (int i = 0; i < children.Length; ++i) + { + sum += childMasses[i] * children[i].LocalPose.Position; + massSum += childMasses[i]; + } + inverseMass = 1f / massSum; + return sum * inverseMass; + } + /// + /// Computes the center of mass of a compound. + /// + /// Poses of the children in the compound. + /// Masses of the children in the compound. + /// Inverse of the sum of all child masses. + /// The compound's center of mass. + public static Vector3 ComputeCenterOfMass(Span childPoses, Span childMasses, out float inverseMass) + { + Vector3 sum = default; + float massSum = 0; + for (int i = 0; i < childPoses.Length; ++i) + { + sum += childMasses[i] * childPoses[i].Position; + massSum += childMasses[i]; + } + inverseMass = 1f / massSum; + return sum * inverseMass; + } + /// + /// Computes the center of mass of a compound. + /// + /// Children of the compound. + /// Masses of the children in the compound. + /// The compound's center of mass. + public static Vector3 ComputeCenterOfMass(Span children, Span childMasses) => ComputeCenterOfMass(children, childMasses, out _); + + /// + /// Computes the center of mass of a compound. + /// + /// Poses of the children in the compound. + /// Masses of the children in the compound. + /// The compound's center of mass. + public static Vector3 ComputeCenterOfMass(Span childPoses, Span childMasses) => ComputeCenterOfMass(childPoses, childMasses, out _); + + /// + /// Computes the inertia for a set of compound children based on their poses and the provided inverse inertias. Recenters the children onto the computed center of mass. + /// + /// Children and their associated poses. Center of mass will be subtracted from the child position. + /// Inverse inertias of the children, each in the child's local space. Assumed to have already been premultiplied by the mass of the child. + /// Masses of each child in the compound. + /// Computed center of mass that was subtracted from the child positions. + /// of the compound. + public static BodyInertia ComputeInverseInertia(Span children, Span inverseLocalInertias, Span childMasses, out Vector3 centerOfMass) { Symmetric3x3 summedInertia = default; + BodyInertia inertia; + centerOfMass = ComputeCenterOfMass(children, childMasses, out inertia.InverseMass); for (int i = 0; i < children.Length; ++i) { + children[i].LocalPose.Position -= centerOfMass; summedInertia += ComputeInertiaForChild(children[i].LocalPose, inverseLocalInertias[i], childMasses[i]); } + Symmetric3x3.Invert(summedInertia, out inertia.InverseInertiaTensor); + return inertia; } + /// + /// Computes the inertia for a set of compound children based on their poses and the provided inverse inertias. Recenters the children onto the computed center of mass. + /// + /// Poses of the compound's children. Center of mass will be subtracted from the child position. + /// Inverse inertias of the children, each in the child's local space. Assumed to have already been premultiplied by the mass of the child. + /// Masses of each child in the compound. + /// Computed center of mass that was subtracted from the child positions. + /// of the compound. + public static BodyInertia ComputeInverseInertia(Span childPoses, Span inverseLocalInertias, Span childMasses, out Vector3 centerOfMass) + { + Symmetric3x3 summedInertia = default; + BodyInertia inertia; + centerOfMass = ComputeCenterOfMass(childPoses, childMasses, out inertia.InverseMass); + for (int i = 0; i < childPoses.Length; ++i) + { + childPoses[i].Position -= centerOfMass; + summedInertia += ComputeInertiaForChild(childPoses[i], inverseLocalInertias[i], childMasses[i]); + } + Symmetric3x3.Invert(summedInertia, out inertia.InverseInertiaTensor); + return inertia; + } + /// /// Builds a buffer of compound children from the accumulated set for a kinematic compound. From d5301f7326c3d9f0ff4b76062ca71dedfe87a096 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 21 Jun 2022 20:32:48 -0500 Subject: [PATCH 496/947] Compound and BigCompound now have ComputeInertia. Bumped version. --- BepuPhysics/BepuPhysics.csproj | 2 +- BepuPhysics/Collidables/BigCompound.cs | 26 +++++++++++++ BepuPhysics/Collidables/Compound.cs | 31 +++++++++++++++ BepuPhysics/Collidables/CompoundBuilder.cs | 44 +++++++++++++++++++++- BepuPhysics/Collidables/Shapes.cs | 22 ++++++++++- BepuUtilities/BepuUtilities.csproj | 2 +- DemoTests/InertiaTensorTests.cs | 9 +++++ Demos/Demos/CompoundDemo.cs | 1 - 8 files changed, 132 insertions(+), 5 deletions(-) diff --git a/BepuPhysics/BepuPhysics.csproj b/BepuPhysics/BepuPhysics.csproj index 98dd15a18..ed022695f 100644 --- a/BepuPhysics/BepuPhysics.csproj +++ b/BepuPhysics/BepuPhysics.csproj @@ -1,7 +1,7 @@  net6.0 - 2.5.0-beta.5 + 2.5.0-beta.6 Bepu Entertainment LLC Ross Nordby Speedy real time physics simulation library. diff --git a/BepuPhysics/Collidables/BigCompound.cs b/BepuPhysics/Collidables/BigCompound.cs index 2b04a5eb1..441edfa0f 100644 --- a/BepuPhysics/Collidables/BigCompound.cs +++ b/BepuPhysics/Collidables/BigCompound.cs @@ -240,6 +240,32 @@ public unsafe void FindLocalOverlaps(in Vector3 min, in Vector3 max, Tree.Sweep(min, max, sweep, maximumT, ref enumerator); } + /// + /// Computes the inertia of a compound. Does not recenter the child poses. + /// + /// Masses of the children. + /// Shapes collection containing the data for the compound child shapes. + /// Inertia of the compound. + public BodyInertia ComputeInertia(Span childMasses, Shapes shapes) + { + return CompoundBuilder.ComputeInertia(Children, childMasses, shapes); + } + + /// + /// Computes the inertia of a compound. Recenters the child poses around the calculated center of mass. + /// + /// Shapes collection containing the data for the compound child shapes. + /// Masses of the children. + /// Calculated center of mass of the compound. Subtracted from all the compound child poses. + /// Inertia of the compound. + public BodyInertia ComputeInertia(Span childMasses, Shapes shapes, out Vector3 centerOfMass) + { + var bodyInertia = CompoundBuilder.ComputeInertia(Children, childMasses, shapes, out centerOfMass); + //Recentering moves the children around, so the tree needs to be updated. + Tree.Refit(); + return bodyInertia; + } + public void Dispose(BufferPool bufferPool) { bufferPool.Return(ref Children); diff --git a/BepuPhysics/Collidables/Compound.cs b/BepuPhysics/Collidables/Compound.cs index 7b8379960..5e7d8c994 100644 --- a/BepuPhysics/Collidables/Compound.cs +++ b/BepuPhysics/Collidables/Compound.cs @@ -9,9 +9,18 @@ namespace BepuPhysics.Collidables { + /// + /// Shape and pose of a child within a compound shape. + /// public struct CompoundChild { + /// + /// Index of the shape within whatever shape collection holds the compound's child shape data. + /// public TypedIndex ShapeIndex; + /// + /// Pose of the child in the compound's local space. + /// public RigidPose LocalPose; } @@ -304,7 +313,29 @@ public unsafe void FindLocalOverlaps(in Vector3 min, in Vector3 max, overlaps.Allocate(pool) = i; } } + } + /// + /// Computes the inertia of a compound. Does not recenter the child poses. + /// + /// Masses of the children. + /// Shapes collection containing the data for the compound child shapes. + /// Inertia of the compound. + public BodyInertia ComputeInertia(Span childMasses, Shapes shapes) + { + return CompoundBuilder.ComputeInertia(Children, childMasses, shapes); + } + + /// + /// Computes the inertia of a compound. Recenters the child poses around the calculated center of mass. + /// + /// Shapes collection containing the data for the compound child shapes. + /// Masses of the children. + /// Calculated center of mass of the compound. Subtracted from all the compound child poses. + /// Inertia of the compound. + public BodyInertia ComputeInertia(Span childMasses, Shapes shapes, out Vector3 centerOfMass) + { + return CompoundBuilder.ComputeInertia(Children, childMasses, shapes, out centerOfMass); } public void Dispose(BufferPool bufferPool) diff --git a/BepuPhysics/Collidables/CompoundBuilder.cs b/BepuPhysics/Collidables/CompoundBuilder.cs index e7c2e9bc8..209aa161c 100644 --- a/BepuPhysics/Collidables/CompoundBuilder.cs +++ b/BepuPhysics/Collidables/CompoundBuilder.cs @@ -243,7 +243,7 @@ public static BodyInertia ComputeInverseInertia(Span children, Sp /// Inverse inertias of the children, each in the child's local space. Assumed to have already been premultiplied by the mass of the child. /// Masses of each child in the compound. /// of the compound. - public static BodyInertia ComputeInertia(Span childPoses, Span inverseLocalInertias, Span childMasses) + public static BodyInertia ComputeInverseInertia(Span childPoses, Span inverseLocalInertias, Span childMasses) { Symmetric3x3 summedInertia = default; float massSum = 0; @@ -354,6 +354,48 @@ public static BodyInertia ComputeInverseInertia(Span childPoses, Span return inertia; } + /// + /// Computes the inertia of a compound. Does not recenter the child poses. + /// + /// Children of the compound. + /// Shapes collection containing the data for the compound child shapes. + /// Masses of the children. + /// Inertia of the compound. + public static BodyInertia ComputeInertia(Span children, Span childMasses, Shapes shapes) + { + Span localInverseInertias = stackalloc Symmetric3x3[children.Length]; + for (int i = 0; i < children.Length; ++i) + { + ref var child = ref children[i]; + if (shapes[child.ShapeIndex.Type] is IConvexShapeBatch batch) + { + localInverseInertias[i] = batch.ComputeInertia(child.ShapeIndex.Index, childMasses[i]).InverseInertiaTensor; + } + } + return ComputeInverseInertia(children, localInverseInertias, childMasses); + } + + /// + /// Computes the inertia of a compound. Recenters the child poses around the calculated center of mass. + /// + /// Children of the compound. Child local positions will have the calculated center of mass subtracted from them. + /// Shapes collection containing the data for the compound child shapes. + /// Masses of the children. + /// Calculated center of mass of the compound. Subtracted from all the compound child poses. + /// Inertia of the compound. + public static BodyInertia ComputeInertia(Span children, Span childMasses, Shapes shapes, out Vector3 centerOfMass) + { + Span localInverseInertias = stackalloc Symmetric3x3[children.Length]; + for (int i = 0; i < children.Length; ++i) + { + ref var child = ref children[i]; + if (shapes[child.ShapeIndex.Type] is IConvexShapeBatch batch) + { + localInverseInertias[i] = batch.ComputeInertia(child.ShapeIndex.Index, childMasses[i]).InverseInertiaTensor; + } + } + return ComputeInverseInertia(children, localInverseInertias, childMasses, out centerOfMass); + } /// /// Builds a buffer of compound children from the accumulated set for a kinematic compound. diff --git a/BepuPhysics/Collidables/Shapes.cs b/BepuPhysics/Collidables/Shapes.cs index 86c581ff7..53ab9a375 100644 --- a/BepuPhysics/Collidables/Shapes.cs +++ b/BepuPhysics/Collidables/Shapes.cs @@ -197,8 +197,23 @@ public override void Dispose() } } + /// + /// Defines a shape batch containing convex objects that support simple inertia calculations. + /// + /// This interface gives compounds a way to compute inertia despite not having direct typed access to the child shapes. + /// It's a layer of overhead that can usually be avoided, but it's sometimes convenient to be able to just enumerate child inertias. + public interface IConvexShapeBatch + { + /// + /// Computes the inertia of a shape. + /// + /// Index of the shape to compute the inertia of. + /// Mass to use to compute the inertia. + /// Inertia of the shape. + BodyInertia ComputeInertia(int shapeIndex, float mass); + } - public class ConvexShapeBatch : ShapeBatch + public class ConvexShapeBatch : ShapeBatch, IConvexShapeBatch where TShape : unmanaged, IConvexShape where TShapeWide : unmanaged, IShapeWide { @@ -216,6 +231,11 @@ protected override void RemoveAndDisposeChildren(int index, Shapes shapes, Buffe //And they don't have any children. } + public BodyInertia ComputeInertia(int shapeIndex, float mass) + { + return shapes[shapeIndex].ComputeInertia(mass); + } + public override void ComputeBounds(ref BoundingBoxBatcher batcher) { batcher.ExecuteConvexBatch(this); diff --git a/BepuUtilities/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj index aae4f2cc4..fabe9670b 100644 --- a/BepuUtilities/BepuUtilities.csproj +++ b/BepuUtilities/BepuUtilities.csproj @@ -3,7 +3,7 @@ BepuUtilities BepuUtilities net6.0 - 2.5.0-beta.5 + 2.5.0-beta.6 Bepu Entertainment LLC Ross Nordby Supporting utilities library for BEPUphysics v2. diff --git a/DemoTests/InertiaTensorTests.cs b/DemoTests/InertiaTensorTests.cs index 27b1f7eae..89089eb43 100644 --- a/DemoTests/InertiaTensorTests.cs +++ b/DemoTests/InertiaTensorTests.cs @@ -281,6 +281,15 @@ public static void TestCompound(Random random, BufferPool pool) } compoundBuilder.BuildDynamicCompound(out var children, out var analyticInertia, out var center); var compound = new Compound(children); + if (random.NextDouble() < 0.5) + { + //Bit hacky. Technically two codepaths, want to cover both. + Span childMasses = stackalloc float[childCount]; + for (int i = 0; i < childMasses.Length; ++i) + childMasses[i] = massPerChild; + analyticInertia = compound.ComputeInertia(childMasses, shapes, out var doubleCheckedCenter); + Assert.True(doubleCheckedCenter.Length() < 1e-4f, "The compound builder should have already recentered the children. Is there a disagreement in the center of mass calculation somewhere?"); + } compound.ComputeBounds(Quaternion.Identity, shapes, out var min, out var max); var span = max - min; diff --git a/Demos/Demos/CompoundDemo.cs b/Demos/Demos/CompoundDemo.cs index 685ef4f84..8ceef2e81 100644 --- a/Demos/Demos/CompoundDemo.cs +++ b/Demos/Demos/CompoundDemo.cs @@ -8,7 +8,6 @@ using BepuPhysics.Constraints; using DemoRenderer.UI; using DemoUtilities; -using BepuUtilities.Memory; namespace Demos.Demos { From 6c68123a914744698b87fe5f5c8b8eae0fda810e Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 5 Jul 2022 15:55:11 -0500 Subject: [PATCH 497/947] Added a FrictionDemo. --- Demos/DemoSet.cs | 1 + Demos/Demos/BouncinessDemo.cs | 12 ++++ Demos/Demos/FrictionDemo.cs | 126 ++++++++++++++++++++++++++++++++++ 3 files changed, 139 insertions(+) create mode 100644 Demos/Demos/FrictionDemo.cs diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index 51e0620af..f40e1dc29 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -64,6 +64,7 @@ public DemoSet() AddOption(); AddOption(); AddOption(); + AddOption(); AddOption(); AddOption(); AddOption(); diff --git a/Demos/Demos/BouncinessDemo.cs b/Demos/Demos/BouncinessDemo.cs index 4ec94b6c6..6aeab9e6b 100644 --- a/Demos/Demos/BouncinessDemo.cs +++ b/Demos/Demos/BouncinessDemo.cs @@ -16,8 +16,13 @@ namespace Demos.Demos /// Shows how to configure things to bounce in the absence of a coefficient of restitution. /// /// + /// /// v2 does not support traditional coefficients of restitution because it conflicts with speculative contacts. /// All contacts are, however, springs. With a little configuration, you can give objects physically reasonable bounciness. + /// + /// + /// For a similar example of friction, see the . + /// /// public class BouncinessDemo : Demo { @@ -29,6 +34,13 @@ public struct SimpleMaterial } public unsafe struct BounceCallbacks : INarrowPhaseCallbacks { + /// + /// Maps entries to their . + /// + /// + /// The narrow phase callbacks need some way to get the material data for this demo, but there's no requirement that you use the type. + /// It's just a fairly convenient and simple option. + /// public CollidableProperty CollidableMaterials; public void Initialize(Simulation simulation) diff --git a/Demos/Demos/FrictionDemo.cs b/Demos/Demos/FrictionDemo.cs new file mode 100644 index 000000000..65d7ceaed --- /dev/null +++ b/Demos/Demos/FrictionDemo.cs @@ -0,0 +1,126 @@ +using BepuPhysics; +using BepuPhysics.Collidables; +using BepuPhysics.CollisionDetection; +using BepuPhysics.Constraints; +using DemoContentLoader; +using DemoRenderer; +using DemoRenderer.UI; +using DemoUtilities; +using System; +using System.Numerics; +using System.Runtime.CompilerServices; + +namespace Demos.Demos +{ + /// + /// Shows how to configure things to slide at different rates. + /// + /// + /// For a similar example of bounciness, see the . + /// + public class FrictionDemo : Demo + { + public struct SimpleMaterial + { + public SpringSettings SpringSettings; + public float FrictionCoefficient; + public float MaximumRecoveryVelocity; + } + public unsafe struct FrictionCallbacks : INarrowPhaseCallbacks + { + /// + /// Maps entries to their . + /// + /// + /// The narrow phase callbacks need some way to get the material data for this demo, but there's no requirement that you use the type. + /// It's just a fairly convenient and simple option. + /// + public CollidableProperty CollidableMaterials; + + public void Initialize(Simulation simulation) + { + //The callbacks get created before the simulation so that they can be given to the simulation. The property needs a simulation reference, so we hand it over in the initialize. + CollidableMaterials.Initialize(simulation); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin) + { + //While the engine won't even try creating pairs between statics at all, it will ask about kinematic-kinematic pairs. + //Those pairs cannot emit constraints since both involved bodies have infinite inertia. Since most of the demos don't need + //to collect information about kinematic-kinematic pairs, we'll require that at least one of the bodies needs to be dynamic. + return a.Mobility == CollidableMobility.Dynamic || b.Mobility == CollidableMobility.Dynamic; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB) + { + return true; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public unsafe bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold + { + //For the purposes of this demo, we'll use multiplicative blending for the friction and choose spring properties according to which collidable has a higher maximum recovery velocity. + var a = CollidableMaterials[pair.A]; + var b = CollidableMaterials[pair.B]; + pairMaterial.FrictionCoefficient = a.FrictionCoefficient * b.FrictionCoefficient; + pairMaterial.MaximumRecoveryVelocity = MathF.Max(a.MaximumRecoveryVelocity, b.MaximumRecoveryVelocity); + pairMaterial.SpringSettings = pairMaterial.MaximumRecoveryVelocity == a.MaximumRecoveryVelocity ? a.SpringSettings : b.SpringSettings; + return true; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold) + { + return true; + } + + public void Dispose() + { + } + } + + public unsafe override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(0, 20, 200); + camera.Yaw = 0; + camera.Pitch = 0; + //Unlike the bounciness demo, we don't have to worry about including strong substepping to help with stiff contact constraint bounces. Friction is easier. + var collidableMaterials = new CollidableProperty(); + Simulation = Simulation.Create(BufferPool, new FrictionCallbacks() { CollidableMaterials = collidableMaterials }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0), 0, 0), new SolveDescription(4, 1)); + + //Note that the box description includes a significant sideways velocity to make the box go weee. + var shape = new Box(1, 1, 1); + var boxDescription = BodyDescription.CreateDynamic(RigidPose.Identity, new Vector3(20, 0, 0), shape.ComputeInertia(1), Simulation.Shapes.Add(shape), 1e-2f); + + int boxCount = 100; + float maximumFriction = 3; + for (int i = 0; i < 100; ++i) + { + //Drop the boxes in a line with varying friction. + boxDescription.Pose.Position = new Vector3(-80, 0.5f, i * 1.2f); + collidableMaterials.Allocate(Simulation.Bodies.Add(boxDescription)) = new SimpleMaterial + { + FrictionCoefficient = maximumFriction * i / (boxCount - 1f), + MaximumRecoveryVelocity = 2, + SpringSettings = new SpringSettings(30, 1) + }; + } + + collidableMaterials.Allocate(Simulation.Statics.Add(new StaticDescription(new Vector3(0, -15f, 0), Simulation.Shapes.Add(new Box(2500, 30, 2500))))) = + new SimpleMaterial { FrictionCoefficient = 1, MaximumRecoveryVelocity = 2, SpringSettings = new SpringSettings(30, 1) }; + } + + public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + { + var resolution = renderer.Surface.Resolution; + renderer.TextBatcher.Write(text.Clear().Append("Every contact constraint can be configured with its own material property by the INarrowPhaseCallbacks."), new Vector2(16, resolution.Y - 80), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("In this demo, a pair's coefficient of friction is the coefficients of the involved collidables multiplied together."), new Vector2(16, resolution.Y - 64), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Material state is configured ahead of time and stored in per-collidable SimpleMaterial definitions."), new Vector2(16, resolution.Y - 48), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Friction coefficients range from 0 to 3 from far to near."), new Vector2(16, resolution.Y - 32), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Notably, you don't have to do it this way; materials can change over time, or be procedural, or anything else you can express in the callback."), new Vector2(16, resolution.Y - 16), 16, Vector3.One, font); + base.Render(renderer, camera, input, text, font); + } + } +} From da873a633a49a42f4931dbaccda20d11f8bc6d82 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 23 Jul 2022 10:14:10 -0500 Subject: [PATCH 498/947] Avoided potential GL explode. --- DemoRenderer.GL/RenderSurface.cs | 1 + 1 file changed, 1 insertion(+) diff --git a/DemoRenderer.GL/RenderSurface.cs b/DemoRenderer.GL/RenderSurface.cs index 2d55f880d..7bc0d9be6 100644 --- a/DemoRenderer.GL/RenderSurface.cs +++ b/DemoRenderer.GL/RenderSurface.cs @@ -29,6 +29,7 @@ public RenderSurface(IWindowInfo window, Int2 resolution, bool fullScreen = fals { this.window = window; context = new GraphicsContext(new GraphicsMode(new ColorFormat(8, 8, 8, 8), 24, 0, 4), window, 4, 6, GraphicsContextFlags.Default); + context.MakeCurrent(window); context.LoadAll(); if (enableDeviceDebugLayer) { From 3884d88d34f854d045eebf76337a192969ac6531 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 23 Jul 2022 14:41:47 -0500 Subject: [PATCH 499/947] BufferPool resizes now avoid reallocations where possible. --- BepuUtilities/Memory/BufferPool.cs | 26 ++++-- BepuUtilitiesTests/Program.cs | 1 - BepuUtilitiesTests/Repro.cs | 126 ----------------------------- 3 files changed, 19 insertions(+), 134 deletions(-) delete mode 100644 BepuUtilitiesTests/Repro.cs diff --git a/BepuUtilities/Memory/BufferPool.cs b/BepuUtilities/Memory/BufferPool.cs index 4aca1f864..f728b1385 100644 --- a/BepuUtilities/Memory/BufferPool.cs +++ b/BepuUtilities/Memory/BufferPool.cs @@ -336,27 +336,39 @@ public unsafe void Return(ref Buffer buffer) where T : unmanaged /// Type of the buffer to resize. /// Buffer reference to resize. /// Number of elements to resize the buffer for. - /// Number of elements to copy into the new buffer from the old buffer. + /// Number of elements to copy into the new buffer from the old buffer. Contents of slots outside the copied range in the resized buffer are undefined. [MethodImpl(MethodImplOptions.AggressiveInlining)] public void ResizeToAtLeast(ref Buffer buffer, int targetSize, int copyCount) where T : unmanaged { //Only do anything if the new size is actually different from the current size. Debug.Assert(copyCount <= targetSize && copyCount <= buffer.Length, "Can't copy more elements than exist in the source or target buffers."); targetSize = GetCapacityForCount(targetSize); - if (buffer.Length != targetSize) //Note that we don't check for allocated status- for buffers, a length of 0 is the same as being unallocated. + if (!buffer.Allocated) + { + Debug.Assert(buffer.Length == 0, "If a buffer is pointing at null, then it should be default initialized and have a length of zero too."); + //This buffer is not allocated; just return a new one. No copying to be done. + TakeAtLeast(targetSize, out buffer); + } + else { - TakeAtLeast(targetSize, out Buffer newBuffer); - if (buffer.Length > 0) + var originalAllocatedSizeInBytes = 1 << (buffer.Id >> PowerPool.IdPowerShift); + var originalAllocatedSize = originalAllocatedSizeInBytes / Unsafe.SizeOf(); + Debug.Assert(originalAllocatedSize >= buffer.Length, "The original allocated capacity must be sufficient for the buffer's observed length. Did the buffer get corrupted? Is this buffer reference from uninitialized memory?"); + if (targetSize > originalAllocatedSize) { - //Don't bother copying from or re-pooling empty buffers. They're uninitialized. + //The original allocation isn't big enough to hold the new size; allocate a new buffer. + TakeAtLeast(targetSize, out Buffer newBuffer); buffer.CopyTo(0, newBuffer, 0, copyCount); ReturnUnsafely(buffer.Id); + buffer = newBuffer; } else { - Debug.Assert(copyCount == 0, "Should not be trying to copy elements from an empty span."); + //Original allocation is large enough to hold the new size; just bump the size up. + //The expectation for this function is to bump up to the next power of 2, given the 'AtLeast' suffix, so just expose the full original size. + //No need for copying. + buffer.length = originalAllocatedSize; } - buffer = newBuffer; } } diff --git a/BepuUtilitiesTests/Program.cs b/BepuUtilitiesTests/Program.cs index a500fdfe4..15dd219b5 100644 --- a/BepuUtilitiesTests/Program.cs +++ b/BepuUtilitiesTests/Program.cs @@ -9,7 +9,6 @@ static void Main(string[] args) CodeGenTests.Test(); AllocatorTests.TestChurnStability(); QuickCollectionTests.Test(); - //BoundingTests.Test(); Console.WriteLine(); AffineTests.Test(); Console.WriteLine(); diff --git a/BepuUtilitiesTests/Repro.cs b/BepuUtilitiesTests/Repro.cs deleted file mode 100644 index b23ca933b..000000000 --- a/BepuUtilitiesTests/Repro.cs +++ /dev/null @@ -1,126 +0,0 @@ -using System; -using System.Diagnostics; -using System.Numerics; -using System.Runtime.CompilerServices; -using System.Runtime.InteropServices; - -namespace Repro -{ - class Repro - { - public struct Matrix4 - { - public Vector4 X; - public Vector4 Y; - public Vector4 Z; - public Vector4 W; - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Transform(ref Vector4 v, ref Matrix4 m, out Vector4 result) - { - var x = new Vector4(v.X); - var y = new Vector4(v.Y); - var z = new Vector4(v.Z); - var w = new Vector4(v.W); - result = m.X * x + m.Y * y + m.Z * z + m.W * w; - } - } - - [StructLayout(LayoutKind.Explicit, Size = 48)] - public struct Matrix3 - { - [FieldOffset(0)] - public Vector3 X; - [FieldOffset(16)] - public Vector3 Y; - [FieldOffset(32)] - public Vector3 Z; - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Transform(ref Vector3 v, ref Matrix3 m, out Vector3 result) - { - var x = new Vector3(v.X); - var y = new Vector3(v.Y); - var z = new Vector3(v.Z); - result = m.X * x + m.Y * y + m.Z * z; - } - } - - static void Main2(string[] args) - { - const int iterationCount = 10000000; - - //MATRIX4x4 - { - var v = new Vector4(1, 2, 3, 4); - var m = new Matrix4 - { - X = new Vector4(1, 0, 0, 0), - Y = new Vector4(0, 1, 0, 0), - Z = new Vector4(0, 0, 1, 0), - W = new Vector4(0, 0, 0, 1) - }; - - //Warmup - { - Vector4 result; - Matrix4.Transform(ref v, ref m, out result); - } - Vector4 accumulator = new Vector4(); - var startTime = Stopwatch.GetTimestamp(); - for (int i = 0; i < iterationCount; ++i) - { - Vector4 result; - Matrix4.Transform(ref v, ref m, out result); - Matrix4.Transform(ref result, ref m, out result); - Matrix4.Transform(ref result, ref m, out result); - Matrix4.Transform(ref result, ref m, out result); - Matrix4.Transform(ref result, ref m, out result); - Matrix4.Transform(ref result, ref m, out result); - Matrix4.Transform(ref result, ref m, out result); - Matrix4.Transform(ref result, ref m, out result); - Matrix4.Transform(ref result, ref m, out result); - Matrix4.Transform(ref result, ref m, out result); - accumulator += result; //Avoid optimizing out the loop. - } - var endTime = Stopwatch.GetTimestamp(); - Console.WriteLine($"4x4 Time: {(endTime - startTime) / (double)Stopwatch.Frequency}"); - } - - //MATRIX3x3 - { - var v = new Vector3(1, 2, 3); - var m = new Matrix3 - { - X = new Vector3(1, 0, 0), - Y = new Vector3(0, 1, 0), - Z = new Vector3(0, 0, 1), - }; - //Warmup - { - Vector3 result; - Matrix3.Transform(ref v, ref m, out result); - } - Vector3 accumulator = new Vector3(); - var startTime = Stopwatch.GetTimestamp(); - for (int i = 0; i < iterationCount; ++i) - { - Vector3 result; - Matrix3.Transform(ref v, ref m, out result); - Matrix3.Transform(ref result, ref m, out result); - Matrix3.Transform(ref result, ref m, out result); - Matrix3.Transform(ref result, ref m, out result); - Matrix3.Transform(ref result, ref m, out result); - Matrix3.Transform(ref result, ref m, out result); - Matrix3.Transform(ref result, ref m, out result); - Matrix3.Transform(ref result, ref m, out result); - Matrix3.Transform(ref result, ref m, out result); - Matrix3.Transform(ref result, ref m, out result); - accumulator += result; //Avoid optimizing out the loop. - } - var endTime = Stopwatch.GetTimestamp(); - Console.WriteLine($"3x3 Time: {(endTime - startTime) / (double)Stopwatch.Frequency}"); - } - } - } -} From cca9d16d9d2c578994a9fdd3e3bba90d06c34ea3 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 23 Jul 2022 14:44:35 -0500 Subject: [PATCH 500/947] Bumped version number. --- BepuPhysics/BepuPhysics.csproj | 2 +- BepuUtilities/BepuUtilities.csproj | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/BepuPhysics/BepuPhysics.csproj b/BepuPhysics/BepuPhysics.csproj index ed022695f..ac5bf74fc 100644 --- a/BepuPhysics/BepuPhysics.csproj +++ b/BepuPhysics/BepuPhysics.csproj @@ -1,7 +1,7 @@  net6.0 - 2.5.0-beta.6 + 2.5.0-beta.7 Bepu Entertainment LLC Ross Nordby Speedy real time physics simulation library. diff --git a/BepuUtilities/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj index fabe9670b..4c6117aa6 100644 --- a/BepuUtilities/BepuUtilities.csproj +++ b/BepuUtilities/BepuUtilities.csproj @@ -3,7 +3,7 @@ BepuUtilities BepuUtilities net6.0 - 2.5.0-beta.6 + 2.5.0-beta.7 Bepu Entertainment LLC Ross Nordby Supporting utilities library for BEPUphysics v2. From 5061d4a738301a16365834f2a55d88adb3a72937 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 26 Jul 2022 17:28:16 -0500 Subject: [PATCH 501/947] Making a few memory layout deets explicit. --- BepuPhysics/Collidables/CollidableReference.cs | 2 ++ BepuPhysics/PoseIntegrator.cs | 6 +++--- BepuPhysics/SimulationAllocationSizes.cs | 5 ++++- 3 files changed, 9 insertions(+), 4 deletions(-) diff --git a/BepuPhysics/Collidables/CollidableReference.cs b/BepuPhysics/Collidables/CollidableReference.cs index 3ee9684d1..c9c329eab 100644 --- a/BepuPhysics/Collidables/CollidableReference.cs +++ b/BepuPhysics/Collidables/CollidableReference.cs @@ -2,6 +2,7 @@ using System; using System.Diagnostics; using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; namespace BepuPhysics.Collidables { @@ -27,6 +28,7 @@ public enum CollidableMobility /// /// Uses a bitpacked representation to refer to a body or static collidable. /// + [StructLayout(LayoutKind.Sequential, Size = 4)] public struct CollidableReference : IEquatable { /// diff --git a/BepuPhysics/PoseIntegrator.cs b/BepuPhysics/PoseIntegrator.cs index 9b490139c..84f308e12 100644 --- a/BepuPhysics/PoseIntegrator.cs +++ b/BepuPhysics/PoseIntegrator.cs @@ -26,15 +26,15 @@ public enum AngularIntegrationMode /// /// Angular velocity is directly integrated and does not change as the body pose changes. Does not conserve angular momentum. /// - Nonconserving, + Nonconserving = 0, /// /// Approximately conserves angular momentum by updating the angular velocity according to the change in orientation. Does a decent job for gyroscopes, but angular velocities will tend to drift towards a minimal inertia axis. /// - ConserveMomentum, + ConserveMomentum = 1, /// /// Approximately conserves angular momentum by including an implicit gyroscopic torque. Best option for Dzhanibekov effect simulation, but applies a damping effect that can make gyroscopes less useful. /// - ConserveMomentumWithGyroscopicTorque, + ConserveMomentumWithGyroscopicTorque = 2, } /// diff --git a/BepuPhysics/SimulationAllocationSizes.cs b/BepuPhysics/SimulationAllocationSizes.cs index f4dbf0132..8b04737c1 100644 --- a/BepuPhysics/SimulationAllocationSizes.cs +++ b/BepuPhysics/SimulationAllocationSizes.cs @@ -1,8 +1,11 @@ -namespace BepuPhysics +using System.Runtime.InteropServices; + +namespace BepuPhysics { /// /// The common set of allocation sizes for a simulation. /// + [StructLayout(LayoutKind.Sequential)] public struct SimulationAllocationSizes { /// From ce85266df3505bf958f66e76ce6e357bf279612c Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 27 Jul 2022 17:21:42 -0500 Subject: [PATCH 502/947] Layout explicitness. --- BepuPhysics/BodyDescription.cs | 2 ++ BepuPhysics/Collidables/Collidable.cs | 1 + BepuPhysics/Collidables/CollidableDescription.cs | 5 ++++- BepuPhysics/StaticDescription.cs | 2 ++ 4 files changed, 9 insertions(+), 1 deletion(-) diff --git a/BepuPhysics/BodyDescription.cs b/BepuPhysics/BodyDescription.cs index 22541db57..95c8ca5ed 100644 --- a/BepuPhysics/BodyDescription.cs +++ b/BepuPhysics/BodyDescription.cs @@ -4,12 +4,14 @@ using BepuUtilities.Memory; using System.Numerics; using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; namespace BepuPhysics { /// /// Describes the thresholds for a body going to sleep. /// + [StructLayout(LayoutKind.Sequential)] public struct BodyActivityDescription { /// diff --git a/BepuPhysics/Collidables/Collidable.cs b/BepuPhysics/Collidables/Collidable.cs index 64d551f42..a747d46a0 100644 --- a/BepuPhysics/Collidables/Collidable.cs +++ b/BepuPhysics/Collidables/Collidable.cs @@ -33,6 +33,7 @@ public enum ContinuousDetectionMode /// /// Defines how a collidable handles collisions with significant velocity. /// + [StructLayout(LayoutKind.Sequential)] public struct ContinuousDetection { /// diff --git a/BepuPhysics/Collidables/CollidableDescription.cs b/BepuPhysics/Collidables/CollidableDescription.cs index 4a78bf1c5..6302a760d 100644 --- a/BepuPhysics/Collidables/CollidableDescription.cs +++ b/BepuPhysics/Collidables/CollidableDescription.cs @@ -1,8 +1,11 @@ -namespace BepuPhysics.Collidables +using System.Runtime.InteropServices; + +namespace BepuPhysics.Collidables { /// /// Describes a collidable and how it should handle continuous collision detection. /// + [StructLayout(LayoutKind.Sequential)] public struct CollidableDescription { /// diff --git a/BepuPhysics/StaticDescription.cs b/BepuPhysics/StaticDescription.cs index 702a061d6..9086594cf 100644 --- a/BepuPhysics/StaticDescription.cs +++ b/BepuPhysics/StaticDescription.cs @@ -1,11 +1,13 @@ using BepuPhysics.Collidables; using System.Numerics; +using System.Runtime.InteropServices; namespace BepuPhysics { /// /// Describes the properties of a static object. When added to a simulation, static objects can collide but have no velocity and will not move in response to forces. /// + [StructLayout(LayoutKind.Sequential)] public struct StaticDescription { /// From 1abeac791aa836808eae8768990b434dbaad28eb Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 30 Jul 2022 13:38:30 -0500 Subject: [PATCH 503/947] Renamed "SolverState" to "BodyDynamics". --- BepuPhysics/Bodies_GatherScatter.cs | 8 ++++---- BepuPhysics/BodyProperties.cs | 2 +- BepuPhysics/BodyReference.cs | 4 ++-- BepuPhysics/BodySet.cs | 2 +- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/BepuPhysics/Bodies_GatherScatter.cs b/BepuPhysics/Bodies_GatherScatter.cs index bc7ce6b91..b99507dc0 100644 --- a/BepuPhysics/Bodies_GatherScatter.cs +++ b/BepuPhysics/Bodies_GatherScatter.cs @@ -17,7 +17,7 @@ public partial class Bodies { [MethodImpl(MethodImplOptions.AggressiveInlining)] - private static void WriteGatherInertia(int index, int bodyIndexInBundle, ref Buffer states, ref BodyInertiaWide gatheredInertias) + private static void WriteGatherInertia(int index, int bodyIndexInBundle, ref Buffer states, ref BodyInertiaWide gatheredInertias) { ref var source = ref states[index].Inertia.World; ref var targetSlot = ref GetOffsetInstance(ref gatheredInertias, bodyIndexInBundle); @@ -31,7 +31,7 @@ private static void WriteGatherInertia(int index, int bodyIndexInBundle, ref Buf } [MethodImpl(MethodImplOptions.AggressiveInlining)] - private static void WriteGatherMotionState(int index, int bodyIndexInBundle, ref Buffer states, + private static void WriteGatherMotionState(int index, int bodyIndexInBundle, ref Buffer states, ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocityWide velocity) { ref var state = ref states[index].Motion; @@ -43,7 +43,7 @@ private static void WriteGatherMotionState(int index, int bodyIndexInBundle, ref [MethodImpl(MethodImplOptions.AggressiveInlining)] - unsafe static void FallbackGatherMotionState(SolverState* states, Vector encodedBodyIndices, ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocityWide velocity) + unsafe static void FallbackGatherMotionState(BodyDynamics* states, Vector encodedBodyIndices, ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocityWide velocity) { var pPositionX = (float*)Unsafe.AsPointer(ref position.X); var pPositionY = (float*)Unsafe.AsPointer(ref position.Y); @@ -81,7 +81,7 @@ unsafe static void FallbackGatherMotionState(SolverState* states, Vector en } } [MethodImpl(MethodImplOptions.AggressiveInlining)] - unsafe static void FallbackGatherInertia(SolverState* states, Vector encodedBodyIndices, ref BodyInertiaWide inertia, int offsetInFloats) + unsafe static void FallbackGatherInertia(BodyDynamics* states, Vector encodedBodyIndices, ref BodyInertiaWide inertia, int offsetInFloats) { var pMass = (float*)Unsafe.AsPointer(ref inertia.InverseMass); var pInertiaXX = (float*)Unsafe.AsPointer(ref inertia.InverseInertiaTensor.XX); diff --git a/BepuPhysics/BodyProperties.cs b/BepuPhysics/BodyProperties.cs index 702b5d8af..832ebf398 100644 --- a/BepuPhysics/BodyProperties.cs +++ b/BepuPhysics/BodyProperties.cs @@ -289,7 +289,7 @@ public struct BodyInertias /// Note that this goes along with a change to the buffer pool's default alignment to 128 bytes. /// [StructLayout(LayoutKind.Sequential)] - public struct SolverState + public struct BodyDynamics { /// /// Pose and velocity information for the body. diff --git a/BepuPhysics/BodyReference.cs b/BepuPhysics/BodyReference.cs index 8bd062b4f..9b2b2336e 100644 --- a/BepuPhysics/BodyReference.cs +++ b/BepuPhysics/BodyReference.cs @@ -130,9 +130,9 @@ public ref MotionState MotionState } /// - /// Gets a reference to the body's solver-relevant state, including both pose, velocity, and inertia. + /// Gets a reference to the body's solver-relevant state, including pose, velocity, and inertia. /// - public ref SolverState SolverState + public ref BodyDynamics Dynamics { [MethodImpl(MethodImplOptions.NoInlining)] get diff --git a/BepuPhysics/BodySet.cs b/BepuPhysics/BodySet.cs index f3beaa72e..ad0a11594 100644 --- a/BepuPhysics/BodySet.cs +++ b/BepuPhysics/BodySet.cs @@ -38,7 +38,7 @@ public struct BodySet /// /// Stores all data involved in solving constraints for a body, including pose, velocity, and inertia. /// - public Buffer SolverStates; + public Buffer SolverStates; /// /// The collidables owned by each body in the set. Speculative margins, continuity settings, and shape indices can be changed directly. From 00d65cb3d7d6c891fbb0f24dec9e54abfbf0bc2e Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 30 Jul 2022 13:44:15 -0500 Subject: [PATCH 504/947] One more rename. --- BepuPhysics/Bodies.cs | 14 +++++++------- BepuPhysics/Bodies_GatherScatter.cs | 16 ++++++++-------- BepuPhysics/BodyReference.cs | 18 +++++++++--------- BepuPhysics/BodySet.cs | 14 +++++++------- BepuPhysics/CollisionDetection/NarrowPhase.cs | 4 ++-- .../Constraints/FourBodyTypeProcessor.cs | 4 ++-- .../Constraints/OneBodyTypeProcessor.cs | 4 ++-- .../Constraints/ThreeBodyTypeProcessor.cs | 4 ++-- .../Constraints/TwoBodyTypeProcessor.cs | 4 ++-- BepuPhysics/IslandAwakener.cs | 4 ++-- BepuPhysics/IslandSleeper.cs | 2 +- BepuPhysics/Simulation_Queries.cs | 2 +- BepuPhysics/Solver.cs | 12 ++++++------ 13 files changed, 51 insertions(+), 51 deletions(-) diff --git a/BepuPhysics/Bodies.cs b/BepuPhysics/Bodies.cs index 85a1a5f4e..30adf74cc 100644 --- a/BepuPhysics/Bodies.cs +++ b/BepuPhysics/Bodies.cs @@ -132,7 +132,7 @@ public void UpdateBounds(BodyHandle bodyHandle) ref var collidable = ref set.Collidables[location.Index]; if (collidable.Shape.Exists) { - shapes.UpdateBounds(set.SolverStates[location.Index].Motion.Pose, collidable.Shape, out var bodyBounds); + shapes.UpdateBounds(set.DynamicsState[location.Index].Motion.Pose, collidable.Shape, out var bodyBounds); if (location.SetIndex == 0) { broadPhase.UpdateActiveBounds(collidable.BroadPhaseIndex, bodyBounds.Min, bodyBounds.Max); @@ -441,8 +441,8 @@ public void SetLocalInertia(BodyHandle handle, in BodyInertia localInertia) } //Note that the HandleToLocation slot reference is still valid; it may have been updated, but handle slots don't move. ref var set = ref Sets[location.SetIndex]; - ref var inertiaReference = ref set.SolverStates[location.Index].Inertia; - ref var localInertiaReference = ref set.SolverStates[location.Index].Inertia.Local; + ref var inertiaReference = ref set.DynamicsState[location.Index].Inertia; + ref var localInertiaReference = ref set.DynamicsState[location.Index].Inertia.Local; var nowKinematic = IsKinematic(localInertia); var previouslyKinematic = IsKinematicUnsafeGCHole(ref inertiaReference.Local); inertiaReference.Local = localInertia; @@ -462,7 +462,7 @@ void UpdateForShapeChange(BodyHandle handle, int activeBodyIndex, TypedIndex old if (newShape.Exists) { //Add a collidable to the simulation for the new shape. - ref var state = ref set.SolverStates[activeBodyIndex]; + ref var state = ref set.DynamicsState[activeBodyIndex]; AddCollidableToBroadPhase(handle, state.Motion.Pose, state.Inertia.Local, ref set.Collidables[activeBodyIndex]); } else @@ -517,7 +517,7 @@ public void ApplyDescription(BodyHandle handle, in BodyDescription description) ref var collidable = ref set.Collidables[location.Index]; var oldShape = collidable.Shape; var nowKinematic = IsKinematic(description.LocalInertia); - var previouslyKinematic = IsKinematicUnsafeGCHole(ref set.SolverStates[location.Index].Inertia.Local); + var previouslyKinematic = IsKinematicUnsafeGCHole(ref set.DynamicsState[location.Index].Inertia.Local); set.ApplyDescriptionByIndex(location.Index, description); UpdateForShapeChange(handle, location.Index, oldShape, description.Collidable.Shape); UpdateForKinematicStateChange(handle, ref location, ref set, previouslyKinematic, nowKinematic); @@ -601,7 +601,7 @@ internal void ValidateMotionStates() { for (int j = 0; j < set.Count; ++j) { - ref var state = ref set.SolverStates[j]; + ref var state = ref set.DynamicsState[j]; try { state.Motion.Pose.Position.Validate(); @@ -628,7 +628,7 @@ internal void ValidateAwakeMotionStatesByHash(HashDiagnosticType type) ref var set = ref ActiveSet; for (int j = 0; j < set.Count; ++j) { - ref var state = ref set.SolverStates[j]; + ref var state = ref set.DynamicsState[j]; instance.ContributeToHash(ref hash, state.Motion.Pose.Position); instance.ContributeToHash(ref hash, state.Motion.Pose.Orientation); instance.ContributeToHash(ref hash, state.Motion.Velocity.Linear); diff --git a/BepuPhysics/Bodies_GatherScatter.cs b/BepuPhysics/Bodies_GatherScatter.cs index b99507dc0..6966f3030 100644 --- a/BepuPhysics/Bodies_GatherScatter.cs +++ b/BepuPhysics/Bodies_GatherScatter.cs @@ -270,7 +270,7 @@ public static unsafe void TransposeMotionStates(Buffer states, out public unsafe void GatherState(Vector encodedBodyIndices, bool worldInertia, out Vector3Wide position, out QuaternionWide orientation, out BodyVelocityWide velocity, out BodyInertiaWide inertia) where TAccessFilter : unmanaged, IBodyAccessFilter { - var solverStates = ActiveSet.SolverStates.Memory; + var solverStates = ActiveSet.DynamicsState.Memory; Unsafe.SkipInit(out TAccessFilter filter); if (Avx.IsSupported && Vector.Count == 8) { @@ -489,7 +489,7 @@ public unsafe void ScatterPose( { if (Avx.IsSupported && Vector.Count == 8) { - var states = ActiveSet.SolverStates.Memory; + var states = ActiveSet.DynamicsState.Memory; { var m0 = orientation.X.AsVector256(); var m1 = orientation.Y.AsVector256(); @@ -542,7 +542,7 @@ public unsafe void ScatterPose( { if (mask[innerIndex] == 0) continue; - ref var pose = ref ActiveSet.SolverStates[encodedBodyIndices[innerIndex]].Motion.Pose; + ref var pose = ref ActiveSet.DynamicsState[encodedBodyIndices[innerIndex]].Motion.Pose; pose.Position = new Vector3(position.X[innerIndex], position.Y[innerIndex], position.Z[innerIndex]); pose.Orientation = new Quaternion(orientation.X[innerIndex], orientation.Y[innerIndex], orientation.Z[innerIndex], orientation.W[innerIndex]); @@ -558,7 +558,7 @@ public unsafe void ScatterInertia( { if (Avx.IsSupported && Vector.Count == 8) { - var states = ActiveSet.SolverStates.Memory; + var states = ActiveSet.DynamicsState.Memory; { var m0 = inertia.InverseInertiaTensor.XX.AsVector256(); var m1 = inertia.InverseInertiaTensor.YX.AsVector256(); @@ -612,7 +612,7 @@ public unsafe void ScatterInertia( { if (mask[innerIndex] == 0) continue; - ref var target = ref ActiveSet.SolverStates[encodedBodyIndices[innerIndex]].Inertia.World; + ref var target = ref ActiveSet.DynamicsState[encodedBodyIndices[innerIndex]].Inertia.World; target.InverseInertiaTensor.XX = inertia.InverseInertiaTensor.XX[innerIndex]; target.InverseInertiaTensor.YX = inertia.InverseInertiaTensor.YX[innerIndex]; target.InverseInertiaTensor.YY = inertia.InverseInertiaTensor.YY[innerIndex]; @@ -674,7 +674,7 @@ public unsafe void ScatterVelocities(ref BodyVelocityWide sourceV var o6 = Avx.Shuffle(n4, n5, 2 | (3 << 2) | (2 << 4) | (3 << 6)); var indices = (uint*)Unsafe.AsPointer(ref encodedBodyIndices); - var states = ActiveSet.SolverStates.Memory; + var states = ActiveSet.DynamicsState.Memory; if (indices[0] < DynamicLimit) Sse.StoreAligned((float*)(states + indices[0]) + targetOffset, o0.GetLower()); if (indices[1] < DynamicLimit) Sse.StoreAligned((float*)(states + indices[1]) + targetOffset, o4.GetLower()); if (indices[2] < DynamicLimit) Sse.StoreAligned((float*)(states + indices[2]) + targetOffset, o2.GetLower()); @@ -716,7 +716,7 @@ public unsafe void ScatterVelocities(ref BodyVelocityWide sourceV var o7 = Avx.Shuffle(n6, n7, 2 | (3 << 2) | (2 << 4) | (3 << 6)); var indices = (uint*)Unsafe.AsPointer(ref encodedBodyIndices); - var states = ActiveSet.SolverStates.Memory; + var states = ActiveSet.DynamicsState.Memory; if (indices[0] < DynamicLimit) Avx.StoreAligned((float*)(states + indices[0]) + 8, Avx.Permute2x128(o0, o1, 0 | (2 << 4))); if (indices[1] < DynamicLimit) Avx.StoreAligned((float*)(states + indices[1]) + 8, Avx.Permute2x128(o4, o5, 0 | (2 << 4))); if (indices[2] < DynamicLimit) Avx.StoreAligned((float*)(states + indices[2]) + 8, Avx.Permute2x128(o2, o3, 0 | (2 << 4))); @@ -748,7 +748,7 @@ public unsafe void ScatterVelocities(ref BodyVelocityWide sourceV if (indices[innerIndex] >= DynamicLimit) continue; ref var sourceSlot = ref GetOffsetInstance(ref sourceVelocities, innerIndex); - ref var target = ref ActiveSet.SolverStates[indices[innerIndex]].Motion.Velocity; + ref var target = ref ActiveSet.DynamicsState[indices[innerIndex]].Motion.Velocity; target.Linear = new Vector3(sourceSlot.Linear.X[0], sourceSlot.Linear.Y[0], sourceSlot.Linear.Z[0]); target.Angular = new Vector3(sourceSlot.Angular.X[0], sourceSlot.Angular.Y[0], sourceSlot.Angular.Z[0]); } diff --git a/BepuPhysics/BodyReference.cs b/BepuPhysics/BodyReference.cs index 9b2b2336e..f782f2405 100644 --- a/BepuPhysics/BodyReference.cs +++ b/BepuPhysics/BodyReference.cs @@ -99,7 +99,7 @@ public ref BodyVelocity Velocity get { ref var location = ref MemoryLocation; - return ref Bodies.Sets[location.SetIndex].SolverStates[location.Index].Motion.Velocity; + return ref Bodies.Sets[location.SetIndex].DynamicsState[location.Index].Motion.Velocity; } } @@ -112,7 +112,7 @@ public ref RigidPose Pose get { ref var location = ref MemoryLocation; - return ref Bodies.Sets[location.SetIndex].SolverStates[location.Index].Motion.Pose; + return ref Bodies.Sets[location.SetIndex].DynamicsState[location.Index].Motion.Pose; } } @@ -125,7 +125,7 @@ public ref MotionState MotionState get { ref var location = ref MemoryLocation; - return ref Bodies.Sets[location.SetIndex].SolverStates[location.Index].Motion; + return ref Bodies.Sets[location.SetIndex].DynamicsState[location.Index].Motion; } } @@ -138,7 +138,7 @@ public ref BodyDynamics Dynamics get { ref var location = ref MemoryLocation; - return ref Bodies.Sets[location.SetIndex].SolverStates[location.Index]; + return ref Bodies.Sets[location.SetIndex].DynamicsState[location.Index]; } } @@ -164,7 +164,7 @@ public ref BodyInertia LocalInertia get { ref var location = ref MemoryLocation; - return ref Bodies.Sets[location.SetIndex].SolverStates[location.Index].Inertia.Local; + return ref Bodies.Sets[location.SetIndex].DynamicsState[location.Index].Inertia.Local; } } @@ -248,7 +248,7 @@ public void ComputeInverseInertia(out Symmetric3x3 inverseInertia) ref var set = ref Bodies.Sets[MemoryLocation.SetIndex]; //Note that inertia.World is ephemeral data packed into the same cache line for the benefit of the solver. //It should not be assumed to contain up to date information outside of the velocity integration to pose integration interval, so this computes world inertia from scratch. - ref var state = ref set.SolverStates[location.Index]; + ref var state = ref set.DynamicsState[location.Index]; PoseIntegration.RotateInverseInertia(state.Inertia.Local.InverseInertiaTensor, state.Motion.Pose.Orientation, out inverseInertia); } @@ -369,7 +369,7 @@ public static void ApplyImpulse(in Vector3 impulse, in Vector3 impulseOffset, re [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ApplyImpulse(in BodySet set, int index, in Vector3 impulse, in Vector3 impulseOffset) { - ref var state = ref set.SolverStates[index]; + ref var state = ref set.DynamicsState[index]; ApplyImpulse(impulse, impulseOffset, ref state.Inertia.Local, ref state.Motion.Pose, ref state.Motion.Velocity); } @@ -420,7 +420,7 @@ public void ApplyLinearImpulse(in Vector3 impulse) { ref var location = ref MemoryLocation; ref var set = ref Bodies.Sets[location.SetIndex]; - ref var state = ref set.SolverStates[location.Index]; + ref var state = ref set.DynamicsState[location.Index]; ApplyLinearImpulse(impulse, state.Inertia.Local.InverseMass, ref state.Motion.Velocity.Linear); } @@ -444,7 +444,7 @@ public void ApplyAngularImpulse(in Vector3 angularImpulse) { ref var location = ref MemoryLocation; ref var set = ref Bodies.Sets[location.SetIndex]; - ref var state = ref set.SolverStates[location.Index]; + ref var state = ref set.DynamicsState[location.Index]; //Note that inertia.World is ephemeral data packed into the same cache line for the benefit of the solver. //It should not be assumed to contain up to date information outside of the velocity integration to pose integration interval, so this computes world inertia from scratch. PoseIntegration.RotateInverseInertia(state.Inertia.Local.InverseInertiaTensor, state.Motion.Pose.Orientation, out var inverseInertia); diff --git a/BepuPhysics/BodySet.cs b/BepuPhysics/BodySet.cs index ad0a11594..1d8afac94 100644 --- a/BepuPhysics/BodySet.cs +++ b/BepuPhysics/BodySet.cs @@ -38,7 +38,7 @@ public struct BodySet /// /// Stores all data involved in solving constraints for a body, including pose, velocity, and inertia. /// - public Buffer SolverStates; + public Buffer DynamicsState; /// /// The collidables owned by each body in the set. Speculative margins, continuity settings, and shape indices can be changed directly. @@ -90,7 +90,7 @@ internal bool RemoveAt(int bodyIndex, out BodyHandle handle, out int movedBodyIn { movedBodyIndex = Count; //Copy the memory state of the last element down. - SolverStates[bodyIndex] = SolverStates[movedBodyIndex]; + DynamicsState[bodyIndex] = DynamicsState[movedBodyIndex]; Activity[bodyIndex] = Activity[movedBodyIndex]; Collidables[bodyIndex] = Collidables[movedBodyIndex]; //Note that the constraint list is NOT disposed before being overwritten. @@ -125,7 +125,7 @@ internal void ApplyDescriptionByIndex(int index, in BodyDescription description) description.LocalInertia.InverseInertiaTensor.ZZ * description.LocalInertia.InverseInertiaTensor.ZZ), $"Invalid body inverse inertia tensor: {description.LocalInertia.InverseInertiaTensor}"); Debug.Assert(!MathChecker.IsInvalid(description.LocalInertia.InverseMass) && description.LocalInertia.InverseMass >= 0, $"Invalid body inverse mass: {description.LocalInertia.InverseMass}"); - ref var state = ref SolverStates[index]; + ref var state = ref DynamicsState[index]; state.Motion.Pose = description.Pose; state.Motion.Velocity = description.Velocity; state.Inertia.Local = description.LocalInertia; @@ -152,7 +152,7 @@ internal void ApplyDescriptionByIndex(int index, in BodyDescription description) public void GetDescription(int index, out BodyDescription description) { - ref var state = ref SolverStates[index]; + ref var state = ref DynamicsState[index]; description.Pose = state.Motion.Pose; description.Velocity = state.Motion.Velocity; description.LocalInertia = state.Inertia.Local; @@ -235,8 +235,8 @@ internal unsafe void InternalResize(int targetBodyCapacity, BufferPool pool) //Note that we base the bundle capacities on post-resize capacity of the IndexToHandle array. This simplifies the conditions on allocation, but increases memory use. //You may want to change this in the future if memory use is concerning. targetBodyCapacity = BufferPool.GetCapacityForCount(targetBodyCapacity); - Debug.Assert(SolverStates.Length != BufferPool.GetCapacityForCount(targetBodyCapacity), "Should not try to use internal resize of the result won't change the size."); - pool.ResizeToAtLeast(ref SolverStates, targetBodyCapacity, Count); + Debug.Assert(DynamicsState.Length != BufferPool.GetCapacityForCount(targetBodyCapacity), "Should not try to use internal resize of the result won't change the size."); + pool.ResizeToAtLeast(ref DynamicsState, targetBodyCapacity, Count); pool.ResizeToAtLeast(ref IndexToHandle, targetBodyCapacity, Count); pool.ResizeToAtLeast(ref Collidables, targetBodyCapacity, Count); pool.ResizeToAtLeast(ref Activity, targetBodyCapacity, Count); @@ -258,7 +258,7 @@ public unsafe void Clear(BufferPool pool) /// Pool to return the set's top level buffers to. public void DisposeBuffers(BufferPool pool) { - pool.Return(ref SolverStates); + pool.Return(ref DynamicsState); pool.Return(ref IndexToHandle); pool.Return(ref Collidables); pool.Return(ref Activity); diff --git a/BepuPhysics/CollisionDetection/NarrowPhase.cs b/BepuPhysics/CollisionDetection/NarrowPhase.cs index 7a694d53c..b95fa4250 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhase.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhase.cs @@ -409,7 +409,7 @@ public unsafe void HandleOverlap(int workerIndex, CollidableReference a, Collida var twoBodies = bMobility != CollidableMobility.Static; ref var bodyLocationA = ref Bodies.HandleToLocation[a.BodyHandle.Value]; ref var setA = ref Bodies.Sets[bodyLocationA.SetIndex]; - ref var stateA = ref setA.SolverStates[bodyLocationA.Index]; + ref var stateA = ref setA.DynamicsState[bodyLocationA.Index]; ref var collidableA = ref setA.Collidables[bodyLocationA.Index]; float speculativeMarginB; if (twoBodies) @@ -444,7 +444,7 @@ public unsafe void HandleOverlap(int workerIndex, CollidableReference a, Collida ref var bodyLocationB = ref Bodies.HandleToLocation[b.BodyHandle.Value]; Debug.Assert(bodyLocationA.SetIndex == 0 || bodyLocationB.SetIndex == 0, "One of the two bodies must be active. Otherwise, something is busted!"); ref var setB = ref Bodies.Sets[bodyLocationB.SetIndex]; - ref var stateB = ref setB.SolverStates[bodyLocationB.Index]; + ref var stateB = ref setB.DynamicsState[bodyLocationB.Index]; ref var collidableB = ref setB.Collidables[bodyLocationB.Index]; AddBatchEntries(workerIndex, ref overlapWorker, ref pair, ref collidableA.Continuity, ref collidableB.Continuity, diff --git a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs index b358b5293..a1b9493f2 100644 --- a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs @@ -112,7 +112,7 @@ public unsafe override void WarmStart(); var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); var function = default(TConstraintFunctions); - ref var states = ref bodies.ActiveSet.SolverStates; + ref var states = ref bodies.ActiveSet.DynamicsState; for (int i = startBundle; i < exclusiveEndBundle; ++i) { ref var prestep = ref prestepBundles[i]; @@ -158,7 +158,7 @@ public unsafe override void Solve(ref TypeBatch typeBatch, Bodies bodies, float var bodyReferencesBundles = typeBatch.BodyReferences.As(); var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); var function = default(TConstraintFunctions); - ref var motionStates = ref bodies.ActiveSet.SolverStates; + ref var motionStates = ref bodies.ActiveSet.DynamicsState; for (int i = startBundle; i < exclusiveEndBundle; ++i) { ref var prestep = ref prestepBundles[i]; diff --git a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs index 489dd965a..947361fcd 100644 --- a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs @@ -89,7 +89,7 @@ public unsafe override void WarmStart>(); var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); var function = default(TConstraintFunctions); - ref var states = ref bodies.ActiveSet.SolverStates; + ref var states = ref bodies.ActiveSet.DynamicsState; for (int i = startBundle; i < exclusiveEndBundle; ++i) { ref var prestep = ref prestepBundles[i]; @@ -122,7 +122,7 @@ public unsafe override void Solve(ref TypeBatch typeBatch, Bodies bodies, float var bodyReferencesBundles = typeBatch.BodyReferences.As>(); var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); var function = default(TConstraintFunctions); - ref var motionStates = ref bodies.ActiveSet.SolverStates; + ref var motionStates = ref bodies.ActiveSet.DynamicsState; for (int i = startBundle; i < exclusiveEndBundle; ++i) { ref var prestep = ref prestepBundles[i]; diff --git a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs index 00ab98229..f4455556a 100644 --- a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs @@ -107,7 +107,7 @@ public unsafe override void WarmStart(); var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); var function = default(TConstraintFunctions); - ref var states = ref bodies.ActiveSet.SolverStates; + ref var states = ref bodies.ActiveSet.DynamicsState; for (int i = startBundle; i < exclusiveEndBundle; ++i) { ref var prestep = ref prestepBundles[i]; @@ -149,7 +149,7 @@ public unsafe override void Solve(ref TypeBatch typeBatch, Bodies bodies, float var bodyReferencesBundles = typeBatch.BodyReferences.As(); var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); var function = default(TConstraintFunctions); - ref var motionStates = ref bodies.ActiveSet.SolverStates; + ref var motionStates = ref bodies.ActiveSet.DynamicsState; for (int i = startBundle; i < exclusiveEndBundle; ++i) { ref var prestep = ref prestepBundles[i]; diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 091a0a404..2d493c9cb 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -178,7 +178,7 @@ public unsafe override void WarmStart(); var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); Unsafe.SkipInit(out TConstraintFunctions function); - ref var states = ref bodies.ActiveSet.SolverStates; + ref var states = ref bodies.ActiveSet.DynamicsState; //EarlyPrefetch(WarmStartPrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref states, startBundle, exclusiveEndBundle); for (int i = startBundle; i < exclusiveEndBundle; ++i) { @@ -215,7 +215,7 @@ public unsafe override void Solve(ref TypeBatch typeBatch, Bodies bodies, float var bodyReferencesBundles = typeBatch.BodyReferences.As(); var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); Unsafe.SkipInit(out TConstraintFunctions function); - ref var motionStates = ref bodies.ActiveSet.SolverStates; + ref var motionStates = ref bodies.ActiveSet.DynamicsState; //EarlyPrefetch(SolvePrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, startBundle, exclusiveEndBundle); for (int i = startBundle; i < exclusiveEndBundle; ++i) { diff --git a/BepuPhysics/IslandAwakener.cs b/BepuPhysics/IslandAwakener.cs index b3a4ec676..166973add 100644 --- a/BepuPhysics/IslandAwakener.cs +++ b/BepuPhysics/IslandAwakener.cs @@ -295,12 +295,12 @@ internal unsafe void ExecutePhaseOneJob(int index) ref var targetSet = ref bodies.ActiveSet; sourceSet.Collidables.CopyTo(job.SourceStart, targetSet.Collidables, job.TargetStart, job.Count); sourceSet.Constraints.CopyTo(job.SourceStart, targetSet.Constraints, job.TargetStart, job.Count); - sourceSet.SolverStates.CopyTo(job.SourceStart, targetSet.SolverStates, job.TargetStart, job.Count); + sourceSet.DynamicsState.CopyTo(job.SourceStart, targetSet.DynamicsState, job.TargetStart, job.Count); //This rescans the memory, but it should be still floating in cache ready to access. for (int i = 0; i < job.Count; ++i) { var sourceBodyIndex = i + job.SourceStart; - if (Bodies.IsKinematicUnsafeGCHole(ref sourceSet.SolverStates[sourceBodyIndex].Inertia.Local) && sourceSet.Constraints[sourceBodyIndex].Count > 0) + if (Bodies.IsKinematicUnsafeGCHole(ref sourceSet.DynamicsState[sourceBodyIndex].Inertia.Local) && sourceSet.Constraints[sourceBodyIndex].Count > 0) { bool taken = false; solver.constrainedKinematicLock.Enter(ref taken); diff --git a/BepuPhysics/IslandSleeper.cs b/BepuPhysics/IslandSleeper.cs index b971d85a0..82096f819 100644 --- a/BepuPhysics/IslandSleeper.cs +++ b/BepuPhysics/IslandSleeper.cs @@ -359,7 +359,7 @@ unsafe void Gather(int workerIndex) //Note that we are just copying the constraint list reference; we don't have to reallocate it. //Keep this in mind when removing the object from the active set. We don't want to dispose the list since we're still using it. targetSet.Constraints[targetIndex] = sourceSet.Constraints[sourceIndex]; - targetSet.SolverStates[targetIndex] = sourceSet.SolverStates[sourceIndex]; + targetSet.DynamicsState[targetIndex] = sourceSet.DynamicsState[sourceIndex]; if (sourceCollidable.Shape.Exists) { diff --git a/BepuPhysics/Simulation_Queries.cs b/BepuPhysics/Simulation_Queries.cs index 872ebdc99..189bab580 100644 --- a/BepuPhysics/Simulation_Queries.cs +++ b/BepuPhysics/Simulation_Queries.cs @@ -130,7 +130,7 @@ internal unsafe void GetPoseAndShape(CollidableReference reference, out RigidPos { ref var location = ref Bodies.HandleToLocation[reference.BodyHandle.Value]; ref var set = ref Bodies.Sets[location.SetIndex]; - pose = &(set.SolverStates.Memory + location.Index)->Motion.Pose; + pose = &(set.DynamicsState.Memory + location.Index)->Motion.Pose; shape = set.Collidables[location.Index].Shape; } } diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index 5242425f2..d8a58af88 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -376,7 +376,7 @@ internal unsafe void ValidateConstraintReferenceKinematicity() if (setIndex == 0) { //Active set references are indices. - kinematicByInertia = Bodies.IsKinematicUnsafeGCHole(ref bodies.ActiveSet.SolverStates[encodedBodyReference & Bodies.BodyReferenceMask].Inertia.Local); + kinematicByInertia = Bodies.IsKinematicUnsafeGCHole(ref bodies.ActiveSet.DynamicsState[encodedBodyReference & Bodies.BodyReferenceMask].Inertia.Local); } else { @@ -399,7 +399,7 @@ internal unsafe void ValidateConstrainedKinematicsSet() ref var set = ref bodies.ActiveSet; for (int i = 0; i < set.Count; ++i) { - if (Bodies.IsKinematicUnsafeGCHole(ref set.SolverStates[i].Inertia.Local) && set.Constraints[i].Count > 0) + if (Bodies.IsKinematicUnsafeGCHole(ref set.DynamicsState[i].Inertia.Local) && set.Constraints[i].Count > 0) { var contained = ConstrainedKinematicHandles.Contains(set.IndexToHandle[i].Value); if (!contained) @@ -486,7 +486,7 @@ internal unsafe void ValidateFallbackBodiesAreDynamic() { for (int i = 0; i < set.SequentialFallback.dynamicBodyConstraintCounts.Count; ++i) { - Debug.Assert(!Bodies.IsKinematicUnsafeGCHole(ref bodies.ActiveSet.SolverStates[set.SequentialFallback.dynamicBodyConstraintCounts.Keys[i]].Inertia.Local), + Debug.Assert(!Bodies.IsKinematicUnsafeGCHole(ref bodies.ActiveSet.DynamicsState[set.SequentialFallback.dynamicBodyConstraintCounts.Keys[i]].Inertia.Local), "All ostensibly dynamic bodies tracked by the fallback batch must actually be dynamic."); } for (int i = 0; i < bodies.ActiveSet.Count; ++i) @@ -501,7 +501,7 @@ internal unsafe void ValidateFallbackBodiesAreDynamic() } } var bodyIsInFallbackDynamicsSet = ActiveSet.SequentialFallback.dynamicBodyConstraintCounts.TryGetValue(i, out var countForBody); - if (Bodies.IsKinematicUnsafeGCHole(ref bodies.ActiveSet.SolverStates[i].Inertia.Local)) + if (Bodies.IsKinematicUnsafeGCHole(ref bodies.ActiveSet.DynamicsState[i].Inertia.Local)) { Debug.Assert(!bodyIsInFallbackDynamicsSet, "Kinematics should not be present in the dynamic bodies referenced by the fallback batch."); } @@ -740,7 +740,7 @@ internal unsafe void ValidateExistingHandles(bool activeOnly = false) var handle = bodies.ActiveSet.IndexToHandle[i]; int expectedCount = 0; int bodyReference = i; - if (Bodies.IsKinematicUnsafeGCHole(ref bodies.ActiveSet.SolverStates[i].Inertia.Local)) + if (Bodies.IsKinematicUnsafeGCHole(ref bodies.ActiveSet.DynamicsState[i].Inertia.Local)) { //Kinematic bodies may appear more than once in non-fallback batches, so we have to count how many references to expect. var constraints = bodies.ActiveSet.Constraints[i]; @@ -1060,7 +1060,7 @@ unsafe internal void GetBlockingBodyHandles(Span bodyHandles, ref Sp { //Kinematics do not block allocation in a batch; they are treated as read only by the solver. int blockingCount = 0; - var solverStates = bodies.ActiveSet.SolverStates; + var solverStates = bodies.ActiveSet.DynamicsState; for (int i = 0; i < bodyHandles.Length; ++i) { var location = bodies.HandleToLocation[bodyHandles[i].Value]; From 551788d5333da695847ec63951f081971c57c758 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 30 Jul 2022 13:59:24 -0500 Subject: [PATCH 505/947] Oops renames, plus some buffer explicitness and comments. --- BepuUtilities/Memory/Buffer.cs | 26 +++++++--- .../AngularSwivelHingeLineExtractor.cs | 4 +- .../Constraints/BallSocketLineExtractor.cs | 4 +- .../BallSocketMotorLineExtractor.cs | 4 +- .../BallSocketServoLineExtractor.cs | 4 +- .../CenterDistanceLimitLineExtractor.cs | 4 +- .../CenterDistanceLineExtractor.cs | 4 +- .../Constraints/ContactLineExtractors.cs | 28 +++++----- .../Constraints/DistanceLimitLineExtractor.cs | 4 +- .../Constraints/DistanceServoLineExtractor.cs | 4 +- .../Constraints/HingeLineExtractor.cs | 4 +- .../LinearAxisServoLineExtractor.cs | 4 +- .../OneBodyLinearServoLineExtractor.cs | 2 +- .../Constraints/PointOnLineLineExtractor.cs | 4 +- .../Constraints/SwivelHingeLineExtractor.cs | 4 +- DemoRenderer/Constraints/WeldLineExtractor.cs | 4 +- DemoRenderer/ShapeDrawing/ShapesExtractor.cs | 2 +- DemoTests/TestUtilities.cs | 2 +- .../Demos/Characters/CharacterControllers.cs | 14 ++--- Demos/Demos/ColosseumDemo.cs | 51 ++++++++++++++++++- Demos/SpecializedTests/DeterminismTest.cs | 2 +- .../FountainStressTestDemo.cs | 6 +-- Demos/SpecializedTests/TestHelpers.cs | 2 +- 23 files changed, 123 insertions(+), 64 deletions(-) diff --git a/BepuUtilities/Memory/Buffer.cs b/BepuUtilities/Memory/Buffer.cs index 353f9ec68..a3c13f011 100644 --- a/BepuUtilities/Memory/Buffer.cs +++ b/BepuUtilities/Memory/Buffer.cs @@ -14,18 +14,28 @@ namespace BepuUtilities.Memory /// Span over an unmanaged memory region. /// /// Type of the memory exposed by the span. + [StructLayout(LayoutKind.Sequential)] public unsafe struct Buffer where T : unmanaged { + /// + /// Pointer to the beginning of the memory backing this buffer. + /// public T* Memory; internal int length; //We're primarily interested in x64, so memory + length is 12 bytes. This struct would/should get padded to 16 bytes for alignment reasons anyway, //so making use of the last 4 bytes to speed up the case where the raw buffer is taken from a pool (which is basically always) is a good option. /// - /// Implementation specific identifier of the raw buffer set by its source. If taken from a BufferPool, Id represents the index in the power pool from which it was taken. + /// Implementation specific identifier of the raw buffer set by its source. If taken from a BufferPool, Id includes the index in the power pool from which it was taken. /// public int Id; + /// + /// Creates a new buffer. + /// + /// Memory to back the buffer. + /// Length of the buffer in terms of the specified type. + /// Id of the buffer. [MethodImpl(MethodImplOptions.AggressiveInlining)] public Buffer(void* memory, int length, int id = -1) { @@ -34,17 +44,17 @@ public Buffer(void* memory, int length, int id = -1) Id = id; } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static ref T Get(byte* memory, int index) - { - return ref Unsafe.Add(ref Unsafe.As(ref *memory), index); - } - + /// + /// Gets a typed reference from a byte buffer by an index in terms of the type. + /// + /// Byte buffer to interpret. + /// Index into the buffer in terms of the specified type instead of bytes. + /// Reference to the instance at the specified index. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static ref T Get(ref Buffer buffer, int index) { Debug.Assert(index >= 0 && index * Unsafe.SizeOf() < buffer.Length, "Index out of range."); - return ref Get(buffer.Memory, index); + return ref ((T*)buffer.Memory)[index]; } /// diff --git a/DemoRenderer/Constraints/AngularSwivelHingeLineExtractor.cs b/DemoRenderer/Constraints/AngularSwivelHingeLineExtractor.cs index 2e46849dd..77b55b33c 100644 --- a/DemoRenderer/Constraints/AngularSwivelHingeLineExtractor.cs +++ b/DemoRenderer/Constraints/AngularSwivelHingeLineExtractor.cs @@ -14,8 +14,8 @@ public unsafe void ExtractLines(ref AngularSwivelHingePrestepData prestepBundle, Bodies bodies, ref Vector3 tint, ref QuickList lines) { //Could do bundles of constraints at a time, but eh. - ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; - ref var poseB = ref bodies.Sets[setIndex].SolverStates[bodyIndices[1]].Motion.Pose; + ref var poseA = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[0]].Motion.Pose; + ref var poseB = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[1]].Motion.Pose; Vector3Wide.ReadFirst(prestepBundle.LocalSwivelAxisA, out var localSwivelAxisA); Vector3Wide.ReadFirst(prestepBundle.LocalHingeAxisB, out var localHingeAxisB); QuaternionEx.Transform(localSwivelAxisA, poseA.Orientation, out var swivelAxis); diff --git a/DemoRenderer/Constraints/BallSocketLineExtractor.cs b/DemoRenderer/Constraints/BallSocketLineExtractor.cs index 331eaae3b..939775d2b 100644 --- a/DemoRenderer/Constraints/BallSocketLineExtractor.cs +++ b/DemoRenderer/Constraints/BallSocketLineExtractor.cs @@ -14,8 +14,8 @@ public unsafe void ExtractLines(ref BallSocketPrestepData prestepBundle, int set Bodies bodies, ref Vector3 tint, ref QuickList lines) { //Could do bundles of constraints at a time, but eh. - ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; - ref var poseB = ref bodies.Sets[setIndex].SolverStates[bodyIndices[1]].Motion.Pose; + ref var poseA = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[0]].Motion.Pose; + ref var poseB = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[1]].Motion.Pose; Vector3Wide.ReadFirst(prestepBundle.LocalOffsetA, out var localOffsetA); Vector3Wide.ReadFirst(prestepBundle.LocalOffsetB, out var localOffsetB); QuaternionEx.Transform(localOffsetA, poseA.Orientation, out var worldOffsetA); diff --git a/DemoRenderer/Constraints/BallSocketMotorLineExtractor.cs b/DemoRenderer/Constraints/BallSocketMotorLineExtractor.cs index 2d1172e43..ccb9514f7 100644 --- a/DemoRenderer/Constraints/BallSocketMotorLineExtractor.cs +++ b/DemoRenderer/Constraints/BallSocketMotorLineExtractor.cs @@ -14,8 +14,8 @@ public unsafe void ExtractLines(ref BallSocketMotorPrestepData prestepBundle, in Bodies bodies, ref Vector3 tint, ref QuickList lines) { //Could do bundles of constraints at a time, but eh. - ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; - ref var poseB = ref bodies.Sets[setIndex].SolverStates[bodyIndices[1]].Motion.Pose; + ref var poseA = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[0]].Motion.Pose; + ref var poseB = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[1]].Motion.Pose; Vector3Wide.ReadFirst(prestepBundle.LocalOffsetB, out var localOffsetB); QuaternionEx.Transform(localOffsetB, poseB.Orientation, out var worldOffsetB); var anchor = poseB.Position + worldOffsetB; diff --git a/DemoRenderer/Constraints/BallSocketServoLineExtractor.cs b/DemoRenderer/Constraints/BallSocketServoLineExtractor.cs index c73eb8bce..9536838be 100644 --- a/DemoRenderer/Constraints/BallSocketServoLineExtractor.cs +++ b/DemoRenderer/Constraints/BallSocketServoLineExtractor.cs @@ -14,8 +14,8 @@ public unsafe void ExtractLines(ref BallSocketServoPrestepData prestepBundle, in Bodies bodies, ref Vector3 tint, ref QuickList lines) { //Could do bundles of constraints at a time, but eh. - ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; - ref var poseB = ref bodies.Sets[setIndex].SolverStates[bodyIndices[1]].Motion.Pose; + ref var poseA = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[0]].Motion.Pose; + ref var poseB = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[1]].Motion.Pose; Vector3Wide.ReadFirst(prestepBundle.LocalOffsetA, out var localOffsetA); Vector3Wide.ReadFirst(prestepBundle.LocalOffsetB, out var localOffsetB); QuaternionEx.Transform(localOffsetA, poseA.Orientation, out var worldOffsetA); diff --git a/DemoRenderer/Constraints/CenterDistanceLimitLineExtractor.cs b/DemoRenderer/Constraints/CenterDistanceLimitLineExtractor.cs index 67086ca7f..72539a275 100644 --- a/DemoRenderer/Constraints/CenterDistanceLimitLineExtractor.cs +++ b/DemoRenderer/Constraints/CenterDistanceLimitLineExtractor.cs @@ -15,8 +15,8 @@ public unsafe void ExtractLines(ref CenterDistanceLimitPrestepData prestepBundle Bodies bodies, ref Vector3 tint, ref QuickList lines) { //Could do bundles of constraints at a time, but eh. - ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; - ref var poseB = ref bodies.Sets[setIndex].SolverStates[bodyIndices[1]].Motion.Pose; + ref var poseA = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[0]].Motion.Pose; + ref var poseB = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[1]].Motion.Pose; var minimumDistance = GatherScatter.GetFirst(ref prestepBundle.MinimumDistance); var maximumDistance = GatherScatter.GetFirst(ref prestepBundle.MaximumDistance); var color = new Vector3(0.2f, 0.2f, 1f) * tint; diff --git a/DemoRenderer/Constraints/CenterDistanceLineExtractor.cs b/DemoRenderer/Constraints/CenterDistanceLineExtractor.cs index c219cf0a0..1bd003c41 100644 --- a/DemoRenderer/Constraints/CenterDistanceLineExtractor.cs +++ b/DemoRenderer/Constraints/CenterDistanceLineExtractor.cs @@ -15,8 +15,8 @@ public unsafe void ExtractLines(ref CenterDistancePrestepData prestepBundle, int Bodies bodies, ref Vector3 tint, ref QuickList lines) { //Could do bundles of constraints at a time, but eh. - ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; - ref var poseB = ref bodies.Sets[setIndex].SolverStates[bodyIndices[1]].Motion.Pose; + ref var poseA = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[0]].Motion.Pose; + ref var poseB = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[1]].Motion.Pose; var targetDistance = GatherScatter.GetFirst(ref prestepBundle.TargetDistance); var color = new Vector3(0.2f, 0.2f, 1f) * tint; var packedColor = Helpers.PackColor(color); diff --git a/DemoRenderer/Constraints/ContactLineExtractors.cs b/DemoRenderer/Constraints/ContactLineExtractors.cs index a6cc9cf77..f142b608b 100644 --- a/DemoRenderer/Constraints/ContactLineExtractors.cs +++ b/DemoRenderer/Constraints/ContactLineExtractors.cs @@ -14,7 +14,7 @@ struct Contact1OneBodyLineExtractor : IConstraintLineExtractor lines) { - ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; + ref var poseA = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[0]].Motion.Pose; ContactLines.Add(poseA, ref prestepBundle.Contact0.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact0.Depth, tint, ref lines); } } @@ -25,7 +25,7 @@ struct Contact2OneBodyLineExtractor : IConstraintLineExtractor lines) { - ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; + ref var poseA = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[0]].Motion.Pose; ContactLines.Add(poseA, ref prestepBundle.Contact0.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact0.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact1.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact1.Depth, tint, ref lines); } @@ -37,7 +37,7 @@ struct Contact3OneBodyLineExtractor : IConstraintLineExtractor lines) { - ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; + ref var poseA = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[0]].Motion.Pose; ContactLines.Add(poseA, ref prestepBundle.Contact0.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact0.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact1.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact1.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact2.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact2.Depth, tint, ref lines); @@ -50,7 +50,7 @@ struct Contact4OneBodyLineExtractor : IConstraintLineExtractor lines) { - ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; + ref var poseA = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[0]].Motion.Pose; ContactLines.Add(poseA, ref prestepBundle.Contact0.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact0.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact1.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact1.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact2.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact2.Depth, tint, ref lines); @@ -64,7 +64,7 @@ struct Contact1LineExtractor : IConstraintLineExtractor public unsafe void ExtractLines(ref Contact1PrestepData prestepBundle, int setIndex, int* bodyIndices, Bodies bodies, ref Vector3 tint, ref QuickList lines) { - ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; + ref var poseA = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[0]].Motion.Pose; ContactLines.Add(poseA, ref prestepBundle.Contact0.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact0.Depth, tint, ref lines); } } @@ -75,7 +75,7 @@ struct Contact2LineExtractor : IConstraintLineExtractor public unsafe void ExtractLines(ref Contact2PrestepData prestepBundle, int setIndex, int* bodyIndices, Bodies bodies, ref Vector3 tint, ref QuickList lines) { - ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; + ref var poseA = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[0]].Motion.Pose; ContactLines.Add(poseA, ref prestepBundle.Contact0.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact0.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact1.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact1.Depth, tint, ref lines); } @@ -87,7 +87,7 @@ struct Contact3LineExtractor : IConstraintLineExtractor public unsafe void ExtractLines(ref Contact3PrestepData prestepBundle, int setIndex, int* bodyIndices, Bodies bodies, ref Vector3 tint, ref QuickList lines) { - ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; + ref var poseA = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[0]].Motion.Pose; ContactLines.Add(poseA, ref prestepBundle.Contact0.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact0.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact1.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact1.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact2.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact2.Depth, tint, ref lines); @@ -100,7 +100,7 @@ struct Contact4LineExtractor : IConstraintLineExtractor public unsafe void ExtractLines(ref Contact4PrestepData prestepBundle, int setIndex, int* bodyIndices, Bodies bodies, ref Vector3 tint, ref QuickList lines) { - ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; + ref var poseA = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[0]].Motion.Pose; ContactLines.Add(poseA, ref prestepBundle.Contact0.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact0.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact1.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact1.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact2.OffsetA, ref prestepBundle.Normal, ref prestepBundle.Contact2.Depth, tint, ref lines); @@ -114,7 +114,7 @@ struct Contact2NonconvexOneBodyLineExtractor : IConstraintLineExtractor lines) { - ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; + ref var poseA = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[0]].Motion.Pose; ContactLines.Add(poseA, ref prestepBundle.Contact0.Offset, ref prestepBundle.Contact0.Normal, ref prestepBundle.Contact0.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact1.Offset, ref prestepBundle.Contact1.Normal, ref prestepBundle.Contact1.Depth, tint, ref lines); } @@ -126,7 +126,7 @@ struct Contact3NonconvexOneBodyLineExtractor : IConstraintLineExtractor lines) { - ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; + ref var poseA = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[0]].Motion.Pose; ContactLines.Add(poseA, ref prestepBundle.Contact0.Offset, ref prestepBundle.Contact0.Normal, ref prestepBundle.Contact0.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact1.Offset, ref prestepBundle.Contact1.Normal, ref prestepBundle.Contact1.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact2.Offset, ref prestepBundle.Contact2.Normal, ref prestepBundle.Contact2.Depth, tint, ref lines); @@ -139,7 +139,7 @@ struct Contact4NonconvexOneBodyLineExtractor : IConstraintLineExtractor lines) { - ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; + ref var poseA = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[0]].Motion.Pose; ContactLines.Add(poseA, ref prestepBundle.Contact0.Offset, ref prestepBundle.Contact0.Normal, ref prestepBundle.Contact0.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact1.Offset, ref prestepBundle.Contact1.Normal, ref prestepBundle.Contact1.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact2.Offset, ref prestepBundle.Contact2.Normal, ref prestepBundle.Contact2.Depth, tint, ref lines); @@ -153,7 +153,7 @@ struct Contact2NonconvexLineExtractor : IConstraintLineExtractor lines) { - ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; + ref var poseA = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[0]].Motion.Pose; ContactLines.Add(poseA, ref prestepBundle.Contact0.Offset, ref prestepBundle.Contact0.Normal, ref prestepBundle.Contact0.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact1.Offset, ref prestepBundle.Contact1.Normal, ref prestepBundle.Contact1.Depth, tint, ref lines); } @@ -165,7 +165,7 @@ struct Contact3NonconvexLineExtractor : IConstraintLineExtractor lines) { - ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; + ref var poseA = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[0]].Motion.Pose; ContactLines.Add(poseA, ref prestepBundle.Contact0.Offset, ref prestepBundle.Contact0.Normal, ref prestepBundle.Contact0.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact1.Offset, ref prestepBundle.Contact1.Normal, ref prestepBundle.Contact1.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact2.Offset, ref prestepBundle.Contact2.Normal, ref prestepBundle.Contact2.Depth, tint, ref lines); @@ -178,7 +178,7 @@ struct Contact4NonconvexLineExtractor : IConstraintLineExtractor lines) { - ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; + ref var poseA = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[0]].Motion.Pose; ContactLines.Add(poseA, ref prestepBundle.Contact0.Offset, ref prestepBundle.Contact0.Normal, ref prestepBundle.Contact0.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact1.Offset, ref prestepBundle.Contact1.Normal, ref prestepBundle.Contact1.Depth, tint, ref lines); ContactLines.Add(poseA, ref prestepBundle.Contact2.Offset, ref prestepBundle.Contact2.Normal, ref prestepBundle.Contact2.Depth, tint, ref lines); diff --git a/DemoRenderer/Constraints/DistanceLimitLineExtractor.cs b/DemoRenderer/Constraints/DistanceLimitLineExtractor.cs index 1a29f6556..c3804f32c 100644 --- a/DemoRenderer/Constraints/DistanceLimitLineExtractor.cs +++ b/DemoRenderer/Constraints/DistanceLimitLineExtractor.cs @@ -14,8 +14,8 @@ public unsafe void ExtractLines(ref DistanceLimitPrestepData prestepBundle, int Bodies bodies, ref Vector3 tint, ref QuickList lines) { //Could do bundles of constraints at a time, but eh. - ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; - ref var poseB = ref bodies.Sets[setIndex].SolverStates[bodyIndices[1]].Motion.Pose; + ref var poseA = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[0]].Motion.Pose; + ref var poseB = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[1]].Motion.Pose; Vector3Wide.ReadFirst(prestepBundle.LocalOffsetA, out var localOffsetA); Vector3Wide.ReadFirst(prestepBundle.LocalOffsetB, out var localOffsetB); var minimumDistance = GatherScatter.GetFirst(ref prestepBundle.MinimumDistance); diff --git a/DemoRenderer/Constraints/DistanceServoLineExtractor.cs b/DemoRenderer/Constraints/DistanceServoLineExtractor.cs index f8451107d..35ead6976 100644 --- a/DemoRenderer/Constraints/DistanceServoLineExtractor.cs +++ b/DemoRenderer/Constraints/DistanceServoLineExtractor.cs @@ -14,8 +14,8 @@ public unsafe void ExtractLines(ref DistanceServoPrestepData prestepBundle, int Bodies bodies, ref Vector3 tint, ref QuickList lines) { //Could do bundles of constraints at a time, but eh. - ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; - ref var poseB = ref bodies.Sets[setIndex].SolverStates[bodyIndices[1]].Motion.Pose; + ref var poseA = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[0]].Motion.Pose; + ref var poseB = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[1]].Motion.Pose; Vector3Wide.ReadFirst(prestepBundle.LocalOffsetA, out var localOffsetA); Vector3Wide.ReadFirst(prestepBundle.LocalOffsetB, out var localOffsetB); var targetDistance = GatherScatter.GetFirst(ref prestepBundle.TargetDistance); diff --git a/DemoRenderer/Constraints/HingeLineExtractor.cs b/DemoRenderer/Constraints/HingeLineExtractor.cs index 5e8fa50eb..f92b185c0 100644 --- a/DemoRenderer/Constraints/HingeLineExtractor.cs +++ b/DemoRenderer/Constraints/HingeLineExtractor.cs @@ -14,8 +14,8 @@ public unsafe void ExtractLines(ref HingePrestepData prestepBundle, int setIndex Bodies bodies, ref Vector3 tint, ref QuickList lines) { //Could do bundles of constraints at a time, but eh. - ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; - ref var poseB = ref bodies.Sets[setIndex].SolverStates[bodyIndices[1]].Motion.Pose; + ref var poseA = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[0]].Motion.Pose; + ref var poseB = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[1]].Motion.Pose; Vector3Wide.ReadFirst(prestepBundle.LocalHingeAxisA, out var localHingeAxisA); Vector3Wide.ReadFirst(prestepBundle.LocalOffsetA, out var localOffsetA); Vector3Wide.ReadFirst(prestepBundle.LocalHingeAxisB, out var localHingeAxisB); diff --git a/DemoRenderer/Constraints/LinearAxisServoLineExtractor.cs b/DemoRenderer/Constraints/LinearAxisServoLineExtractor.cs index 7b330c3dd..83eb272f5 100644 --- a/DemoRenderer/Constraints/LinearAxisServoLineExtractor.cs +++ b/DemoRenderer/Constraints/LinearAxisServoLineExtractor.cs @@ -14,8 +14,8 @@ public unsafe void ExtractLines(ref LinearAxisServoPrestepData prestepBundle, in Bodies bodies, ref Vector3 tint, ref QuickList lines) { //Could do bundles of constraints at a time, but eh. - ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; - ref var poseB = ref bodies.Sets[setIndex].SolverStates[bodyIndices[1]].Motion.Pose; + ref var poseA = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[0]].Motion.Pose; + ref var poseB = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[1]].Motion.Pose; Vector3Wide.ReadFirst(prestepBundle.LocalOffsetA, out var localOffsetA); Vector3Wide.ReadFirst(prestepBundle.LocalOffsetB, out var localOffsetB); Vector3Wide.ReadFirst(prestepBundle.LocalPlaneNormal, out var localPlaneNormal); diff --git a/DemoRenderer/Constraints/OneBodyLinearServoLineExtractor.cs b/DemoRenderer/Constraints/OneBodyLinearServoLineExtractor.cs index b60c469e7..119f276c2 100644 --- a/DemoRenderer/Constraints/OneBodyLinearServoLineExtractor.cs +++ b/DemoRenderer/Constraints/OneBodyLinearServoLineExtractor.cs @@ -14,7 +14,7 @@ public unsafe void ExtractLines(ref OneBodyLinearServoPrestepData prestepBundle, Bodies bodies, ref Vector3 tint, ref QuickList lines) { //Could do bundles of constraints at a time, but eh. - ref var pose = ref bodies.Sets[setIndex].SolverStates[*bodyIndices].Motion.Pose; + ref var pose = ref bodies.Sets[setIndex].DynamicsState[*bodyIndices].Motion.Pose; Vector3Wide.ReadFirst(prestepBundle.LocalOffset, out var localOffset); Vector3Wide.ReadFirst(prestepBundle.Target, out var target); QuaternionEx.Transform(localOffset, pose.Orientation, out var worldOffset); diff --git a/DemoRenderer/Constraints/PointOnLineLineExtractor.cs b/DemoRenderer/Constraints/PointOnLineLineExtractor.cs index f69e6835b..5befbff46 100644 --- a/DemoRenderer/Constraints/PointOnLineLineExtractor.cs +++ b/DemoRenderer/Constraints/PointOnLineLineExtractor.cs @@ -14,8 +14,8 @@ public unsafe void ExtractLines(ref PointOnLineServoPrestepData prestepBundle, i Bodies bodies, ref Vector3 tint, ref QuickList lines) { //Could do bundles of constraints at a time, but eh. - ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; - ref var poseB = ref bodies.Sets[setIndex].SolverStates[bodyIndices[1]].Motion.Pose; + ref var poseA = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[0]].Motion.Pose; + ref var poseB = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[1]].Motion.Pose; Vector3Wide.ReadFirst(prestepBundle.LocalOffsetA, out var localOffsetA); Vector3Wide.ReadFirst(prestepBundle.LocalOffsetB, out var localOffsetB); Vector3Wide.ReadFirst(prestepBundle.LocalDirection, out var localDirection); diff --git a/DemoRenderer/Constraints/SwivelHingeLineExtractor.cs b/DemoRenderer/Constraints/SwivelHingeLineExtractor.cs index c5c5ba0e4..127dd333d 100644 --- a/DemoRenderer/Constraints/SwivelHingeLineExtractor.cs +++ b/DemoRenderer/Constraints/SwivelHingeLineExtractor.cs @@ -14,8 +14,8 @@ public unsafe void ExtractLines(ref SwivelHingePrestepData prestepBundle, int se Bodies bodies, ref Vector3 tint, ref QuickList lines) { //Could do bundles of constraints at a time, but eh. - ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; - ref var poseB = ref bodies.Sets[setIndex].SolverStates[bodyIndices[1]].Motion.Pose; + ref var poseA = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[0]].Motion.Pose; + ref var poseB = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[1]].Motion.Pose; Vector3Wide.ReadFirst(prestepBundle.LocalSwivelAxisA, out var localSwivelAxisA); Vector3Wide.ReadFirst(prestepBundle.LocalOffsetA, out var localOffsetA); Vector3Wide.ReadFirst(prestepBundle.LocalHingeAxisB, out var localHingeAxisB); diff --git a/DemoRenderer/Constraints/WeldLineExtractor.cs b/DemoRenderer/Constraints/WeldLineExtractor.cs index 730620cd0..cbe7bce03 100644 --- a/DemoRenderer/Constraints/WeldLineExtractor.cs +++ b/DemoRenderer/Constraints/WeldLineExtractor.cs @@ -14,8 +14,8 @@ public unsafe void ExtractLines(ref WeldPrestepData prestepBundle, int setIndex, Bodies bodies, ref Vector3 tint, ref QuickList lines) { //Could do bundles of constraints at a time, but eh. - ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; - ref var poseB = ref bodies.Sets[setIndex].SolverStates[bodyIndices[1]].Motion.Pose; + ref var poseA = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[0]].Motion.Pose; + ref var poseB = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[1]].Motion.Pose; Vector3Wide.ReadFirst(prestepBundle.LocalOffset, out var localOffset); QuaternionEx.Transform(localOffset, poseA.Orientation, out var worldOffset); var bTarget = poseA.Position + worldOffset; diff --git a/DemoRenderer/ShapeDrawing/ShapesExtractor.cs b/DemoRenderer/ShapeDrawing/ShapesExtractor.cs index 59488eb86..d6e47eee5 100644 --- a/DemoRenderer/ShapeDrawing/ShapesExtractor.cs +++ b/DemoRenderer/ShapeDrawing/ShapesExtractor.cs @@ -296,7 +296,7 @@ void AddBodyShape(Shapes shapes, Bodies bodies, int setIndex, int indexInSet, re ref var activity = ref set.Activity[indexInSet]; Vector3 color; Helpers.UnpackColor((uint)HashHelper.Rehash(handle.Value), out Vector3 colorVariation); - ref var state = ref set.SolverStates[indexInSet]; + ref var state = ref set.DynamicsState[indexInSet]; if (Bodies.IsKinematic(state.Inertia.Local)) { var kinematicBase = new Vector3(0, 0.609f, 0.37f); diff --git a/DemoTests/TestUtilities.cs b/DemoTests/TestUtilities.cs index 111a638ae..b2785e625 100644 --- a/DemoTests/TestUtilities.cs +++ b/DemoTests/TestUtilities.cs @@ -45,7 +45,7 @@ public static long ComputeHash(ref Vector3 v, long constant) { for (int bodyIndex = 0; bodyIndex < set.Count; ++bodyIndex) { - ref var state = ref set.SolverStates[bodyIndex].Motion; + ref var state = ref set.DynamicsState[bodyIndex].Motion; ref var pose = ref state.Pose; ref var velocity = ref state.Velocity; var poseHash = ComputeHash(ref pose.Position, 89) + ComputeHash(ref pose.Orientation.X, 107) + ComputeHash(ref pose.Orientation.Y, 113) + ComputeHash(ref pose.Orientation.Z, 131) + ComputeHash(ref pose.Orientation.W, 149); diff --git a/Demos/Demos/Characters/CharacterControllers.cs b/Demos/Demos/Characters/CharacterControllers.cs index cddcc2647..ad8de557a 100644 --- a/Demos/Demos/Characters/CharacterControllers.cs +++ b/Demos/Demos/Characters/CharacterControllers.cs @@ -279,7 +279,7 @@ bool TryReportContacts(CollidableReference characterCollidable, Colli //Have to take into account the current potentially inactive location. ref var bodyLocation = ref Simulation.Bodies.HandleToLocation[character.BodyHandle.Value]; ref var set = ref Simulation.Bodies.Sets[bodyLocation.SetIndex]; - ref var pose = ref set.SolverStates[bodyLocation.Index].Motion.Pose; + ref var pose = ref set.DynamicsState[bodyLocation.Index].Motion.Pose; QuaternionEx.Transform(character.LocalUp, pose.Orientation, out var up); //Note that this branch is compiled out- the generic constraints force type specialization. if (manifold.Convex) @@ -594,16 +594,16 @@ void AnalyzeContactsForCharacterRegion(int start, int exclusiveEnd, int workerIn //If the character is jumping, don't create a constraint. if (supportCandidate.Depth > float.MinValue && character.TryJump) { - QuaternionEx.Transform(character.LocalUp, Simulation.Bodies.ActiveSet.SolverStates[bodyLocation.Index].Motion.Pose.Orientation, out var characterUp); + QuaternionEx.Transform(character.LocalUp, Simulation.Bodies.ActiveSet.DynamicsState[bodyLocation.Index].Motion.Pose.Orientation, out var characterUp); //Note that we assume that character orientations are constant. This isn't necessarily the case in all uses, but it's a decent approximation. - var characterUpVelocity = Vector3.Dot(Simulation.Bodies.ActiveSet.SolverStates[bodyLocation.Index].Motion.Velocity.Linear, characterUp); + var characterUpVelocity = Vector3.Dot(Simulation.Bodies.ActiveSet.DynamicsState[bodyLocation.Index].Motion.Velocity.Linear, characterUp); //We don't want the character to be able to 'superboost' by simply adding jump speed on top of horizontal motion. //Instead, jumping targets a velocity change necessary to reach character.JumpVelocity along the up axis. if (character.Support.Mobility != CollidableMobility.Static) { ref var supportingBodyLocation = ref Simulation.Bodies.HandleToLocation[character.Support.BodyHandle.Value]; Debug.Assert(supportingBodyLocation.SetIndex == 0, "If the character is active, any support should be too."); - ref var supportVelocity = ref Simulation.Bodies.ActiveSet.SolverStates[supportingBodyLocation.Index].Motion.Velocity; + ref var supportVelocity = ref Simulation.Bodies.ActiveSet.DynamicsState[supportingBodyLocation.Index].Motion.Velocity; var wxr = Vector3.Cross(supportVelocity.Angular, supportCandidate.OffsetFromSupport); var supportContactVelocity = supportVelocity.Linear + wxr; var supportUpVelocity = Vector3.Dot(supportContactVelocity, characterUp); @@ -644,7 +644,7 @@ void AnalyzeContactsForCharacterRegion(int start, int exclusiveEnd, int workerIn Matrix3x3 surfaceBasis; surfaceBasis.Y = supportCandidate.Normal; //Note negation: we're using a right handed basis where -Z is forward, +Z is backward. - QuaternionEx.Transform(character.LocalUp, Simulation.Bodies.ActiveSet.SolverStates[bodyLocation.Index].Motion.Pose.Orientation, out var up); + QuaternionEx.Transform(character.LocalUp, Simulation.Bodies.ActiveSet.DynamicsState[bodyLocation.Index].Motion.Pose.Orientation, out var up); var rayDistance = Vector3.Dot(character.ViewDirection, surfaceBasis.Y); var rayVelocity = Vector3.Dot(up, surfaceBasis.Y); Debug.Assert(rayVelocity > 0, @@ -828,10 +828,10 @@ void AnalyzeContacts(float dt, IThreadDispatcher threadDispatcher) for (int i = 0; i < workerCache.Jumps.Count; ++i) { ref var jump = ref workerCache.Jumps[i]; - activeSet.SolverStates[jump.CharacterBodyIndex].Motion.Velocity.Linear += jump.CharacterVelocityChange; + activeSet.DynamicsState[jump.CharacterBodyIndex].Motion.Velocity.Linear += jump.CharacterVelocityChange; if (jump.SupportBodyIndex >= 0) { - BodyReference.ApplyImpulse(Simulation.Bodies.ActiveSet, jump.SupportBodyIndex, jump.CharacterVelocityChange / -activeSet.SolverStates[jump.CharacterBodyIndex].Inertia.Local.InverseMass, jump.SupportImpulseOffset); + BodyReference.ApplyImpulse(Simulation.Bodies.ActiveSet, jump.SupportBodyIndex, jump.CharacterVelocityChange / -activeSet.DynamicsState[jump.CharacterBodyIndex].Inertia.Local.InverseMass, jump.SupportImpulseOffset); } } workerCache.Dispose(pool); diff --git a/Demos/Demos/ColosseumDemo.cs b/Demos/Demos/ColosseumDemo.cs index 5a146f9fe..e6b1f0df9 100644 --- a/Demos/Demos/ColosseumDemo.cs +++ b/Demos/Demos/ColosseumDemo.cs @@ -2,12 +2,15 @@ using BepuPhysics.Collidables; using BepuPhysics.Constraints; using BepuUtilities; +using BepuUtilities.Memory; using DemoContentLoader; using DemoRenderer; using DemoRenderer.UI; using DemoUtilities; using System; +using System.Diagnostics; using System.Numerics; +using System.Threading; namespace Demos.Demos { @@ -59,6 +62,8 @@ public static Vector3 CreateRing(Simulation simulation, Vector3 position, Box ri return position; } + Buffer kinematics; + public unsafe override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(-30, 40, -30); @@ -86,6 +91,22 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) layerPosition.Y += platformsPerLayer * (ringBoxShape.Height * heightPerPlatform + ringBoxShape.Width); } + const int x = 64; + const int y = 64; + const int z = 64; + BufferPool.Take(x * y * z, out kinematics); + int kinematicIndex = 0; + for (int i = 0; i < x; ++i) + { + for (int j = 0; j < y; ++j) + { + for (int k = 0; k < z; ++k) + { + kinematics[kinematicIndex++] = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(250, 10, 0) + new Vector3(i * 5, j * 5, k * 5), boxDescription.Collidable.Shape, new BodyActivityDescription(0))); + } + } + } + //Console.WriteLine($"box count: {Simulation.Bodies.ActiveSet.Count}"); Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), Simulation.Shapes.Add(new Box(500, 1, 500)))); @@ -93,11 +114,12 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) bulletDescription = BodyDescription.CreateDynamic(new Vector3(), bulletShape.ComputeInertia(.1f), Simulation.Shapes.Add(bulletShape), 0.01f); var shootiePatootieShape = new Sphere(3f); - shootiePatootieDescription = BodyDescription.CreateDynamic(new Vector3(), shootiePatootieShape.ComputeInertia(100), new (Simulation.Shapes.Add(shootiePatootieShape), 0.1f), 0.01f); + shootiePatootieDescription = BodyDescription.CreateDynamic(new Vector3(), shootiePatootieShape.ComputeInertia(100), new(Simulation.Shapes.Add(shootiePatootieShape), 0.1f), 0.01f); } BodyDescription bulletDescription; BodyDescription shootiePatootieDescription; + double time; public override void Update(Window window, Camera camera, Input input, float dt) { if (input != null) @@ -115,6 +137,33 @@ public override void Update(Window window, Camera camera, Input input, float dt) Simulation.Bodies.Add(shootiePatootieDescription); } } + var start = Stopwatch.GetTimestamp(); + time += dt; + var velocity = new Vector3((float)Math.Cos(time), 0, 0); + int startIndex = 0; + ThreadDispatcher.DispatchWorkers(workerIndex => + { + var targetKinematicsPerWorker = kinematics.Length / ThreadDispatcher.ThreadCount; + var remainder = kinematics.Length - targetKinematicsPerWorker * ThreadDispatcher.ThreadCount; + var kinematicsForWorker = workerIndex < remainder ? targetKinematicsPerWorker + 1 : targetKinematicsPerWorker; + var workerEnd = Interlocked.Add(ref startIndex, kinematicsForWorker); + for (int i = workerEnd - kinematicsForWorker; i < workerEnd; ++i) + { + var body = Simulation.Bodies[kinematics[i]]; + ref var state = ref body.Dynamics.Motion; + state.Velocity.Linear = velocity; + } + }); + //for (int i = 0; i < kinematics.Length; ++i) + //{ + // var body = Simulation.Bodies[kinematics[i]]; + // ref var state = ref body.SolverState.Motion; + // state.Velocity.Linear = velocity; + //} + var end = Stopwatch.GetTimestamp(); + var timeToUpdateTotal = (end - start) / (double)Stopwatch.Frequency; + Console.WriteLine($"Time to update (ms): {1e3 * timeToUpdateTotal}"); + Console.WriteLine($"Time to update one box (ns): {1e9 * timeToUpdateTotal / kinematics.Length}"); base.Update(window, camera, input, dt); } diff --git a/Demos/SpecializedTests/DeterminismTest.cs b/Demos/SpecializedTests/DeterminismTest.cs index ffcc30da5..a01c6a7a1 100644 --- a/Demos/SpecializedTests/DeterminismTest.cs +++ b/Demos/SpecializedTests/DeterminismTest.cs @@ -32,7 +32,7 @@ static Dictionary ExecuteSimulation(ContentArchive content, in { for (int bodyIndex = 0; bodyIndex < set.Count; ++bodyIndex) { - motionStates.Add(set.IndexToHandle[bodyIndex].Value, set.SolverStates[bodyIndex].Motion); + motionStates.Add(set.IndexToHandle[bodyIndex].Value, set.DynamicsState[bodyIndex].Motion); } } } diff --git a/Demos/SpecializedTests/FountainStressTestDemo.cs b/Demos/SpecializedTests/FountainStressTestDemo.cs index 638b10673..5007d73e2 100644 --- a/Demos/SpecializedTests/FountainStressTestDemo.cs +++ b/Demos/SpecializedTests/FountainStressTestDemo.cs @@ -297,7 +297,7 @@ public override void Update(Window window, Camera camera, Input input, float dt) 16 + 16 * (float)Math.Cos(4 * (angle + t * 0.5)), radius * (float)Math.Sin(positionAngle)); - var correction = targetLocation - set.SolverStates[bodyLocation.Index].Motion.Pose.Position; + var correction = targetLocation - set.DynamicsState[bodyLocation.Index].Motion.Pose.Position; var distance = correction.Length(); if (distance > 1e-4) { @@ -311,13 +311,13 @@ public override void Update(Window window, Camera camera, Input input, float dt) correction *= maxDisplacement / distance; } Debug.Assert(bodyLocation.SetIndex == 0); - Simulation.Bodies.ActiveSet.SolverStates[bodyLocation.Index].Motion.Velocity.Linear = correction * inverseDt; + Simulation.Bodies.ActiveSet.DynamicsState[bodyLocation.Index].Motion.Velocity.Linear = correction * inverseDt; } else { if (bodyLocation.SetIndex == 0) { - Simulation.Bodies.ActiveSet.SolverStates[bodyLocation.Index].Motion.Velocity.Linear = new Vector3(); + Simulation.Bodies.ActiveSet.DynamicsState[bodyLocation.Index].Motion.Velocity.Linear = new Vector3(); } } } diff --git a/Demos/SpecializedTests/TestHelpers.cs b/Demos/SpecializedTests/TestHelpers.cs index 5031cbf7e..bf4ff9807 100644 --- a/Demos/SpecializedTests/TestHelpers.cs +++ b/Demos/SpecializedTests/TestHelpers.cs @@ -17,7 +17,7 @@ public static float GetBodyEnergyHeuristic(Bodies bodies) float accumulated = 0; for (int index = 0; index < bodies.ActiveSet.Count; ++index) { - ref var velocity = ref bodies.ActiveSet.SolverStates[index].Motion.Velocity; + ref var velocity = ref bodies.ActiveSet.DynamicsState[index].Motion.Velocity; accumulated += Vector3.Dot(velocity.Linear, velocity.Linear); accumulated += Vector3.Dot(velocity.Angular, velocity.Angular); } From ac47100bc66e99ca50bc7a169b05d15070b24a97 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 30 Jul 2022 14:13:10 -0500 Subject: [PATCH 506/947] Colosseum demo no longer has 262144 spurious boxes, and quicklist layout more explicit. --- BepuUtilities/Collections/QuickList.cs | 2 ++ Demos/Demos/ColosseumDemo.cs | 47 -------------------------- 2 files changed, 2 insertions(+), 47 deletions(-) diff --git a/BepuUtilities/Collections/QuickList.cs b/BepuUtilities/Collections/QuickList.cs index fe6711217..dba02f1e8 100644 --- a/BepuUtilities/Collections/QuickList.cs +++ b/BepuUtilities/Collections/QuickList.cs @@ -3,6 +3,7 @@ using BepuUtilities.Memory; using System.Collections.Generic; using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; namespace BepuUtilities.Collections { @@ -21,6 +22,7 @@ namespace BepuUtilities.Collections /// it does not (and is incapable of) checking that provided memory gets returned to the same pool that it came from. /// /// Type of the elements in the list. + [StructLayout(LayoutKind.Sequential, Pack = 8)] public struct QuickList where T : unmanaged { /// diff --git a/Demos/Demos/ColosseumDemo.cs b/Demos/Demos/ColosseumDemo.cs index e6b1f0df9..112e7b844 100644 --- a/Demos/Demos/ColosseumDemo.cs +++ b/Demos/Demos/ColosseumDemo.cs @@ -62,8 +62,6 @@ public static Vector3 CreateRing(Simulation simulation, Vector3 position, Box ri return position; } - Buffer kinematics; - public unsafe override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(-30, 40, -30); @@ -90,23 +88,6 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) } layerPosition.Y += platformsPerLayer * (ringBoxShape.Height * heightPerPlatform + ringBoxShape.Width); } - - const int x = 64; - const int y = 64; - const int z = 64; - BufferPool.Take(x * y * z, out kinematics); - int kinematicIndex = 0; - for (int i = 0; i < x; ++i) - { - for (int j = 0; j < y; ++j) - { - for (int k = 0; k < z; ++k) - { - kinematics[kinematicIndex++] = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(250, 10, 0) + new Vector3(i * 5, j * 5, k * 5), boxDescription.Collidable.Shape, new BodyActivityDescription(0))); - } - } - } - //Console.WriteLine($"box count: {Simulation.Bodies.ActiveSet.Count}"); Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), Simulation.Shapes.Add(new Box(500, 1, 500)))); @@ -119,7 +100,6 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) BodyDescription bulletDescription; BodyDescription shootiePatootieDescription; - double time; public override void Update(Window window, Camera camera, Input input, float dt) { if (input != null) @@ -137,33 +117,6 @@ public override void Update(Window window, Camera camera, Input input, float dt) Simulation.Bodies.Add(shootiePatootieDescription); } } - var start = Stopwatch.GetTimestamp(); - time += dt; - var velocity = new Vector3((float)Math.Cos(time), 0, 0); - int startIndex = 0; - ThreadDispatcher.DispatchWorkers(workerIndex => - { - var targetKinematicsPerWorker = kinematics.Length / ThreadDispatcher.ThreadCount; - var remainder = kinematics.Length - targetKinematicsPerWorker * ThreadDispatcher.ThreadCount; - var kinematicsForWorker = workerIndex < remainder ? targetKinematicsPerWorker + 1 : targetKinematicsPerWorker; - var workerEnd = Interlocked.Add(ref startIndex, kinematicsForWorker); - for (int i = workerEnd - kinematicsForWorker; i < workerEnd; ++i) - { - var body = Simulation.Bodies[kinematics[i]]; - ref var state = ref body.Dynamics.Motion; - state.Velocity.Linear = velocity; - } - }); - //for (int i = 0; i < kinematics.Length; ++i) - //{ - // var body = Simulation.Bodies[kinematics[i]]; - // ref var state = ref body.SolverState.Motion; - // state.Velocity.Linear = velocity; - //} - var end = Stopwatch.GetTimestamp(); - var timeToUpdateTotal = (end - start) / (double)Stopwatch.Frequency; - Console.WriteLine($"Time to update (ms): {1e3 * timeToUpdateTotal}"); - Console.WriteLine($"Time to update one box (ns): {1e9 * timeToUpdateTotal / kinematics.Length}"); base.Update(window, camera, input, dt); } From 38caa5f4dfd9391c58e7fd832bd52bf369f6ef22 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 30 Jul 2022 17:07:31 -0500 Subject: [PATCH 507/947] Description parity. --- BepuPhysics/BodyDescription.cs | 1 + BepuPhysics/StaticReference.cs | 9 +++++++++ BepuPhysics/Statics.cs | 11 +++++++++++ 3 files changed, 21 insertions(+) diff --git a/BepuPhysics/BodyDescription.cs b/BepuPhysics/BodyDescription.cs index 95c8ca5ed..1a6edf163 100644 --- a/BepuPhysics/BodyDescription.cs +++ b/BepuPhysics/BodyDescription.cs @@ -50,6 +50,7 @@ public static implicit operator BodyActivityDescription(float sleepThreshold) /// /// Describes a body's state. /// + [StructLayout(LayoutKind.Sequential)] public struct BodyDescription { /// diff --git a/BepuPhysics/StaticReference.cs b/BepuPhysics/StaticReference.cs index 3a4ee18d9..c3b013b24 100644 --- a/BepuPhysics/StaticReference.cs +++ b/BepuPhysics/StaticReference.cs @@ -93,6 +93,15 @@ public void GetDescription(out StaticDescription description) Statics.GetDescription(Handle, out description); } + /// + /// Gets a description of the static. + /// + /// Description of the static. + public StaticDescription GetDescription() + { + return Statics.GetDescription(Handle); + } + /// /// Sets a static's properties according to a description. /// diff --git a/BepuPhysics/Statics.cs b/BepuPhysics/Statics.cs index 1d4806584..6db40b02d 100644 --- a/BepuPhysics/Statics.cs +++ b/BepuPhysics/Statics.cs @@ -498,6 +498,17 @@ public void GetDescription(StaticHandle handle, out StaticDescription descriptio description.Shape = collidable.Shape; } + /// + /// Gets the current description of the static referred to by a given handle. + /// + /// Handle of the static to look up the description of. + /// Gathered description of the handle-referenced static. + public StaticDescription GetDescription(StaticHandle handle) + { + GetDescription(handle, out var description); + return description; + } + /// /// Gets a reference to a static by its handle. /// From 2d8b8942d91d2ac1610607193da4d9b751e6634f Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 31 Jul 2022 23:22:47 -0500 Subject: [PATCH 508/947] Tiny documentation improvements. --- BepuPhysics/Collidables/Collidable.cs | 1 - BepuPhysics/Collidables/TypedIndex.cs | 3 +++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/BepuPhysics/Collidables/Collidable.cs b/BepuPhysics/Collidables/Collidable.cs index a747d46a0..2a3d040a7 100644 --- a/BepuPhysics/Collidables/Collidable.cs +++ b/BepuPhysics/Collidables/Collidable.cs @@ -66,7 +66,6 @@ public struct ContinuousDetection /// the fact that its bounding box is not expanded may cause it to miss a collision even with a non-Discrete collidable. /// Note that Discrete and Passive only differ if maximum speculative margin is restricted. /// - /// If using Discrete instead of Passive, this is presumably some smaller finite value to limit the number of collision pairs found during high velocity movement. /// Detection settings for the given discrete configuration. public static ContinuousDetection Discrete { diff --git a/BepuPhysics/Collidables/TypedIndex.cs b/BepuPhysics/Collidables/TypedIndex.cs index d8dc52e3d..13a005582 100644 --- a/BepuPhysics/Collidables/TypedIndex.cs +++ b/BepuPhysics/Collidables/TypedIndex.cs @@ -4,6 +4,9 @@ namespace BepuPhysics.Collidables { + /// + /// Represents an index with an associated type packed into a single integer. + /// public struct TypedIndex : IEquatable { /// From 79185b126dcc6ccd8353a611c476ffb7bb7f5ed4 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 3 Aug 2022 12:16:04 -0500 Subject: [PATCH 509/947] Comment oopsy. --- BepuPhysics/SimulationAllocationSizes.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/BepuPhysics/SimulationAllocationSizes.cs b/BepuPhysics/SimulationAllocationSizes.cs index 8b04737c1..1ac2f0a21 100644 --- a/BepuPhysics/SimulationAllocationSizes.cs +++ b/BepuPhysics/SimulationAllocationSizes.cs @@ -45,7 +45,7 @@ public struct SimulationAllocationSizes /// Constructs a description of simulation allocations. /// /// The number of bodies to allocate space for. - /// The number of inactive islands to allocate space for. + /// The number of statics to allocate space for. /// The number of inactive islands to allocate space for. /// Minimum number of shapes to allocate space for in each shape type batch. /// The number of constraints to allocate bookkeeping space for. This does not affect actual type batch allocation sizes, only the solver-level constraint handle storage. From c3fceec6df22e91c22cce4b9803bd49780c946ef Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 3 Aug 2022 15:46:33 -0500 Subject: [PATCH 510/947] Refactoring CompoundChild layout and some supporting stuff. --- BepuPhysics/Collidables/BigCompound.cs | 8 +- BepuPhysics/Collidables/Compound.cs | 53 ++++++++---- BepuPhysics/Collidables/CompoundBuilder.cs | 48 +++++++---- BepuPhysics/Collidables/IShape.cs | 6 +- BepuPhysics/Collidables/Shapes.cs | 49 ++++++++--- .../CompoundMeshContinuations.cs | 2 +- .../CompoundPairContinuations.cs | 4 +- .../CompoundPairOverlapFinder.cs | 2 +- .../ConvexCompoundContinuations.cs | 2 +- .../CompoundHomogeneousCompoundSweepTask.cs | 2 +- .../CompoundPairSweepOverlapFinder.cs | 2 +- .../SweepTasks/CompoundPairSweepTask.cs | 4 +- .../SweepTasks/ConvexCompoundSweepTask.cs | 2 +- BepuPhysics/Trees/Tree.cs | 86 +++++++------------ BepuPhysics/Trees/Tree_Add.cs | 8 +- BepuPhysics/Trees/Tree_BinnedRefine.cs | 2 +- BepuPhysics/Trees/Tree_CacheOptimizer.cs | 12 +-- BepuPhysics/Trees/Tree_Diagnostics.cs | 44 +++++----- BepuPhysics/Trees/Tree_IntertreeQueries.cs | 10 +-- BepuPhysics/Trees/Tree_IntertreeQueriesMT.cs | 12 +-- .../Trees/Tree_MultithreadedRefitRefine.cs | 12 +-- BepuPhysics/Trees/Tree_RayCast.cs | 8 +- BepuPhysics/Trees/Tree_RefineCommon.cs | 2 +- .../Trees/Tree_RefinementScheduling.cs | 12 +-- BepuPhysics/Trees/Tree_Refit.cs | 4 +- BepuPhysics/Trees/Tree_Remove.cs | 22 ++--- BepuPhysics/Trees/Tree_SelfQueriesMT.cs | 6 +- BepuPhysics/Trees/Tree_Sweep.cs | 8 +- BepuPhysics/Trees/Tree_SweepBuilder.cs | 4 +- BepuPhysics/Trees/Tree_VolumeQuery.cs | 8 +- 30 files changed, 242 insertions(+), 202 deletions(-) diff --git a/BepuPhysics/Collidables/BigCompound.cs b/BepuPhysics/Collidables/BigCompound.cs index 441edfa0f..d3fd823e2 100644 --- a/BepuPhysics/Collidables/BigCompound.cs +++ b/BepuPhysics/Collidables/BigCompound.cs @@ -7,12 +7,14 @@ using BepuUtilities.Collections; using BepuPhysics.Trees; using BepuPhysics.CollisionDetection.CollisionTasks; +using System.Runtime.InteropServices; namespace BepuPhysics.Collidables { /// /// Compound shape containing a bunch of shapes accessible through a tree acceleration structure. Useful for compounds with lots of children. /// + [StructLayout(LayoutKind.Sequential)] public struct BigCompound : ICompoundShape { /// @@ -48,7 +50,7 @@ public BigCompound(Buffer children, Shapes shapes, BufferPool poo pool.Return(ref leafBounds); } - public void ComputeBounds(in Quaternion orientation, Shapes shapeBatches, out Vector3 min, out Vector3 max) + public void ComputeBounds(Quaternion orientation, Shapes shapeBatches, out Vector3 min, out Vector3 max) { Compound.ComputeChildBounds(Children[0], orientation, shapeBatches, out min, out max); for (int i = 1; i < Children.Length; ++i) @@ -95,7 +97,7 @@ public unsafe void TestLeaf(int leafIndex, RayData* rayData, float* maximumT) CompoundChildShapeTester tester; tester.T = -1; tester.Normal = default; - Shapes[child.ShapeIndex.Type].RayTest(child.ShapeIndex.Index, child.LocalPose, *rayData, ref *maximumT, ref tester); + Shapes[child.ShapeIndex.Type].RayTest(child.ShapeIndex.Index, child.LocalPosition, child.LocalOrientation, *rayData, ref *maximumT, ref tester); if (tester.T >= 0) { Debug.Assert(*maximumT >= tester.T, "Whatever generated this ray hit should have obeyed the current maximumT value."); @@ -167,7 +169,7 @@ public void Add(CompoundChild child, BufferPool pool, Shapes shapes) { pool.Resize(ref Children, Children.Length + 1, Children.Length); Children[^1] = child; - shapes.UpdateBounds(child.LocalPose, child.ShapeIndex, out var bounds); + shapes.UpdateBounds(child.LocalPosition, child.LocalOrientation, child.ShapeIndex, out var bounds); var childIndex = Tree.Add(bounds, pool); Debug.Assert(childIndex == Children.Length - 1, "Adding to a tree acts like appending to the list; a newly added element should be in the last slot to match our previous child modification."); } diff --git a/BepuPhysics/Collidables/Compound.cs b/BepuPhysics/Collidables/Compound.cs index 5e7d8c994..41b9ea20a 100644 --- a/BepuPhysics/Collidables/Compound.cs +++ b/BepuPhysics/Collidables/Compound.cs @@ -6,22 +6,35 @@ using BepuUtilities; using BepuPhysics.Trees; using BepuPhysics.CollisionDetection.CollisionTasks; +using System.Runtime.InteropServices; namespace BepuPhysics.Collidables { /// /// Shape and pose of a child within a compound shape. /// + [StructLayout(LayoutKind.Sequential)] public struct CompoundChild { + /// + /// Local orientation of the child in the compound. + /// + public Quaternion LocalOrientation; + /// + /// Local position of the child in the compound. + /// + public Vector3 LocalPosition; /// /// Index of the shape within whatever shape collection holds the compound's child shape data. /// public TypedIndex ShapeIndex; + /// - /// Pose of the child in the compound's local space. + /// Reintreprets the 32 bytes of a compound child as a pose. /// - public RigidPose LocalPose; + /// Child to reinterpret. + /// Reference to the child as a pose. + public static ref RigidPose AsPose(ref CompoundChild child) => ref Unsafe.As(ref child); //TODO: This could be made a little easier with UnscopedRef. } struct CompoundChildShapeTester : IShapeRayHitHandler @@ -111,10 +124,20 @@ public static bool ValidateChildIndices(ref Buffer children, Shap } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void GetRotatedChildPose(in RigidPose localPose, in Quaternion orientation, out RigidPose rotatedChildPose) + public static void GetRotatedChildPose(in RigidPose localPose, Quaternion orientation, out RigidPose rotatedChildPose) + { + GetRotatedChildPose(localPose.Position, localPose.Orientation, orientation, out rotatedChildPose.Position, out rotatedChildPose.Orientation); + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void GetRotatedChildPose(Vector3 localPosition, Quaternion localOrientation, Quaternion orientation, out RigidPose rotatedChildPose) + { + GetRotatedChildPose(localPosition, localOrientation, orientation, out rotatedChildPose.Position, out rotatedChildPose.Orientation); + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void GetRotatedChildPose(Vector3 localPosition, Quaternion localOrientation, Quaternion parentOrientation, out Vector3 rotatedPosition, out Quaternion rotatedOrientation) { - QuaternionEx.ConcatenateWithoutOverlap(localPose.Orientation, orientation, out rotatedChildPose.Orientation); - QuaternionEx.Transform(localPose.Position, orientation, out rotatedChildPose.Position); + QuaternionEx.ConcatenateWithoutOverlap(localOrientation, parentOrientation, out rotatedOrientation); + QuaternionEx.Transform(localPosition, parentOrientation, out rotatedPosition); } [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void GetRotatedChildPose(in RigidPoseWide localPose, in QuaternionWide orientation, out Vector3Wide childPosition, out QuaternionWide childOrientation) @@ -139,12 +162,12 @@ public static void GetWorldPose(in RigidPose localPose, in RigidPose transform, [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ComputeChildBounds(in CompoundChild child, in Quaternion orientation, Shapes shapeBatches, out Vector3 childMin, out Vector3 childMax) { - GetRotatedChildPose(child.LocalPose, orientation, out var childPose); + GetRotatedChildPose(child.LocalPosition, child.LocalOrientation, orientation, out var childPose); Debug.Assert(!shapeBatches[child.ShapeIndex.Type].Compound, "All children of a compound must be convex."); shapeBatches[child.ShapeIndex.Type].ComputeBounds(child.ShapeIndex.Index, childPose, out childMin, out childMax); } - public void ComputeBounds(in Quaternion orientation, Shapes shapeBatches, out Vector3 min, out Vector3 max) + public void ComputeBounds(Quaternion orientation, Shapes shapeBatches, out Vector3 min, out Vector3 max) { ComputeChildBounds(Children[0], orientation, shapeBatches, out min, out max); for (int i = 1; i < Children.Length; ++i) @@ -167,7 +190,7 @@ public static void AddChildBoundsToBatcher(ref Buffer children, r for (int i = 0; i < children.Length; ++i) { ref var child = ref children[i]; - GetRotatedChildPose(child.LocalPose, pose.Orientation, out var childPose); + GetRotatedChildPose(child.LocalPosition, child.LocalOrientation, pose.Orientation, out var childPose); var angularContributionToChildLinear = Vector3.Cross(velocity.Angular, childPose.Position); var contributionLengthSquared = angularContributionToChildLinear.LengthSquared(); var localPoseRadiusSquared = childPose.Position.LengthSquared(); @@ -203,7 +226,7 @@ public void RayTest(in RigidPose pose, in RayData ray, ref float CompoundChildShapeTester tester; tester.T = -1; tester.Normal = default; - shapeBatches[child.ShapeIndex.Type].RayTest(child.ShapeIndex.Index, child.LocalPose, localRay, ref maximumT, ref tester); + shapeBatches[child.ShapeIndex.Type].RayTest(child.ShapeIndex.Index, CompoundChild.AsPose(ref child), localRay, ref maximumT, ref tester); if (tester.T >= 0) { Debug.Assert(maximumT >= tester.T, "Whatever generated this ray hit should have obeyed the current maximumT value."); @@ -285,9 +308,9 @@ public unsafe void FindLocalOverlaps(ref Buffer(in Vector3 min, in Vector3 max, for (int i = 0; i < Children.Length; ++i) { ref var child = ref Children[i]; - shapes[child.ShapeIndex.Type].ComputeBounds(child.ShapeIndex.Index, child.LocalPose.Orientation, out _, out _, out var childMin, out var childMax); - childMin = childMin + child.LocalPose.Position - expansion; - childMax = childMax + child.LocalPose.Position + expansion; + shapes[child.ShapeIndex.Type].ComputeBounds(child.ShapeIndex.Index, child.LocalOrientation, out _, out _, out var childMin, out var childMax); + childMin = childMin + child.LocalPosition - expansion; + childMax = childMax + child.LocalPosition + expansion; if (Tree.Intersects(childMin, childMax, &ray, out _)) { overlaps.Allocate(pool) = i; diff --git a/BepuPhysics/Collidables/CompoundBuilder.cs b/BepuPhysics/Collidables/CompoundBuilder.cs index 209aa161c..30b010731 100644 --- a/BepuPhysics/Collidables/CompoundBuilder.cs +++ b/BepuPhysics/Collidables/CompoundBuilder.cs @@ -156,10 +156,10 @@ public void BuildDynamicCompound(out Buffer children, out BodyIne { ref var sourceChild = ref Children[i]; ref var targetChild = ref children[i]; - targetChild.LocalPose.Position = sourceChild.LocalPose.Position - center; - targetChild.LocalPose.Orientation = sourceChild.LocalPose.Orientation; + targetChild.LocalPosition = sourceChild.LocalPose.Position - center; + targetChild.LocalOrientation = sourceChild.LocalPose.Orientation; targetChild.ShapeIndex = sourceChild.ShapeIndex; - Symmetric3x3.Add(ComputeInertiaForChild(targetChild.LocalPose, sourceChild.LocalInverseInertia, sourceChild.Weight), summedInertia, out summedInertia); + Symmetric3x3.Add(ComputeInertiaForChild(targetChild.LocalPosition, targetChild.LocalOrientation, sourceChild.LocalInverseInertia, sourceChild.Weight), summedInertia, out summedInertia); } Symmetric3x3.Invert(summedInertia, out inertia.InverseInertiaTensor); } @@ -185,9 +185,10 @@ public void BuildDynamicCompound(out Buffer children, out BodyIne { ref var sourceChild = ref Children[i]; ref var targetChild = ref children[i]; - targetChild.LocalPose = sourceChild.LocalPose; + targetChild.LocalPosition = sourceChild.LocalPose.Position; + targetChild.LocalOrientation = sourceChild.LocalPose.Orientation; targetChild.ShapeIndex = sourceChild.ShapeIndex; - Symmetric3x3.Add(ComputeInertiaForChild(sourceChild.LocalPose, sourceChild.LocalInverseInertia, sourceChild.Weight), summedInertia, out summedInertia); + Symmetric3x3.Add(ComputeInertiaForChild(sourceChild.LocalPose.Position, sourceChild.LocalPose.Orientation, sourceChild.LocalInverseInertia, sourceChild.Weight), summedInertia, out summedInertia); } Symmetric3x3.Invert(summedInertia, out inertia.InverseInertiaTensor); } @@ -200,15 +201,28 @@ public void BuildDynamicCompound(out Buffer children, out BodyIne /// Mass of the child. /// Inertia contribution of the child to a compound given its relative pose. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Symmetric3x3 ComputeInertiaForChild(RigidPose pose, Symmetric3x3 inverseLocalInertia, float mass) + public static Symmetric3x3 ComputeInertiaForChild(in RigidPose pose, Symmetric3x3 inverseLocalInertia, float mass) { - GetOffsetInertiaContribution(pose.Position, mass, out var offsetContribution); + return ComputeInertiaForChild(pose.Position, pose.Orientation, inverseLocalInertia, mass); + } + /// + /// Computes the uninverted inertia contribution of a child. + /// + /// Position of the child. + /// Orientation of the child. + /// Inverse inertia tensor of the child in its local space. + /// Mass of the child. + /// Inertia contribution of the child to a compound given its relative pose. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Symmetric3x3 ComputeInertiaForChild(Vector3 position, Quaternion orientation, Symmetric3x3 inverseLocalInertia, float mass) + { + GetOffsetInertiaContribution(position, mass, out var offsetContribution); //This assumes the given inertia is nonsingular. That should be a valid assumption, unless the user is trying to supply an axis-locked tensor. //For such a use case, it's best to just lock the axis after computing a 'normal' inertia. Debug.Assert(Symmetric3x3.Determinant(inverseLocalInertia) > 0, "Child inertia tensors should be invertible. If making an axis-locked compound, consider locking the axis on the completed inertia. " + "If making a kinematic, consider using the overload which takes no inverse inertia."); - PoseIntegration.RotateInverseInertia(inverseLocalInertia, pose.Orientation, out var rotatedInverseInertia); + PoseIntegration.RotateInverseInertia(inverseLocalInertia, orientation, out var rotatedInverseInertia); Symmetric3x3.Invert(rotatedInverseInertia, out var inertia); Symmetric3x3.Add(offsetContribution, inertia, out inertia); return inertia; @@ -227,7 +241,8 @@ public static BodyInertia ComputeInverseInertia(Span children, Sp float massSum = 0; for (int i = 0; i < children.Length; ++i) { - summedInertia += ComputeInertiaForChild(children[i].LocalPose, inverseLocalInertias[i], childMasses[i]); + ref var child = ref children[i]; + summedInertia += ComputeInertiaForChild(child.LocalPosition, child.LocalOrientation, inverseLocalInertias[i], childMasses[i]); massSum += childMasses[i]; } BodyInertia inertia; @@ -270,7 +285,7 @@ public static Vector3 ComputeCenterOfMass(Span children, Span children, Sp centerOfMass = ComputeCenterOfMass(children, childMasses, out inertia.InverseMass); for (int i = 0; i < children.Length; ++i) { - children[i].LocalPose.Position -= centerOfMass; - summedInertia += ComputeInertiaForChild(children[i].LocalPose, inverseLocalInertias[i], childMasses[i]); + ref var child = ref children[i]; + child.LocalPosition -= centerOfMass; + summedInertia += ComputeInertiaForChild(child.LocalPosition, child.LocalOrientation, inverseLocalInertias[i], childMasses[i]); } Symmetric3x3.Invert(summedInertia, out inertia.InverseInertiaTensor); return inertia; @@ -421,8 +437,8 @@ public void BuildKinematicCompound(out Buffer children, out Vecto { ref var sourceChild = ref Children[i]; ref var targetChild = ref children[i]; - targetChild.LocalPose.Position = sourceChild.LocalPose.Position - center; - targetChild.LocalPose.Orientation = sourceChild.LocalPose.Orientation; + targetChild.LocalPosition = sourceChild.LocalPose.Position - center; + targetChild.LocalOrientation = sourceChild.LocalPose.Orientation; targetChild.ShapeIndex = sourceChild.ShapeIndex; } } @@ -438,8 +454,8 @@ public void BuildKinematicCompound(out Buffer children) { ref var sourceChild = ref Children[i]; ref var targetChild = ref children[i]; - targetChild.LocalPose.Position = sourceChild.LocalPose.Position; - targetChild.LocalPose.Orientation = sourceChild.LocalPose.Orientation; + targetChild.LocalPosition = sourceChild.LocalPose.Position; + targetChild.LocalOrientation = sourceChild.LocalPose.Orientation; targetChild.ShapeIndex = sourceChild.ShapeIndex; } } diff --git a/BepuPhysics/Collidables/IShape.cs b/BepuPhysics/Collidables/IShape.cs index e848c5591..6901929d7 100644 --- a/BepuPhysics/Collidables/IShape.cs +++ b/BepuPhysics/Collidables/IShape.cs @@ -76,12 +76,12 @@ public interface IConvexShape : IShape /// Tests a ray against the shape. /// /// Pose of the shape during the ray test. - /// Origin of the ray to test against the shape. + /// Origin of the ray to test against the shape relative to the shape. /// Direction of the ray to test against the shape. /// Distance along the ray direction to the hit point, if any, in units of the ray direction's length. In other words, hitLocation = origin + direction * t. /// Normal of the impact surface, if any. /// True if the ray intersected the shape, false otherwise. - bool RayTest(in RigidPose pose, in Vector3 origin, in Vector3 direction, out float t, out Vector3 normal); + bool RayTest(in RigidPose pose, Vector3 origin, Vector3 direction, out float t, out Vector3 normal); } /// @@ -98,7 +98,7 @@ public interface ICompoundShape : IShape, IBoundsQueryableCompound /// Shape batches to look up child shape information in. /// Minimum of the compound's bounding box. /// Maximum of the compound's bounding box. - void ComputeBounds(in Quaternion orientation, Shapes shapeBatches, out Vector3 min, out Vector3 max); + void ComputeBounds(Quaternion orientation, Shapes shapeBatches, out Vector3 min, out Vector3 max); /// /// Submits child shapes to a bounding box batcher for vectorized bounds calculation. /// diff --git a/BepuPhysics/Collidables/Shapes.cs b/BepuPhysics/Collidables/Shapes.cs index 53ab9a375..da1751235 100644 --- a/BepuPhysics/Collidables/Shapes.cs +++ b/BepuPhysics/Collidables/Shapes.cs @@ -54,13 +54,27 @@ public void RecursivelyRemoveAndDispose(int index, Shapes shapes, BufferPool poo } public abstract void ComputeBounds(ref BoundingBoxBatcher batcher); - public abstract void ComputeBounds(int shapeIndex, in RigidPose pose, out Vector3 min, out Vector3 max); + public abstract void ComputeBounds(int shapeIndex, Quaternion orientation, out Vector3 min, out Vector3 max); + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void ComputeBounds(int shapeIndex, Vector3 position, Quaternion orientation, out Vector3 min, out Vector3 max) + { + ComputeBounds(shapeIndex, orientation, out min, out max); + min += position; + max += position; + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void ComputeBounds(int shapeIndex, RigidPose pose, out Vector3 min, out Vector3 max) + { + ComputeBounds(shapeIndex, pose.Orientation, out min, out max); + min += pose.Position; + max += pose.Position; + } internal virtual void ComputeBounds(int shapeIndex, in Quaternion orientation, out float maximumRadius, out float maximumAngularExpansion, out Vector3 min, out Vector3 max) { throw new InvalidOperationException("Nonconvex shapes are not required to have a maximum radius or angular expansion implementation. This should only ever be called on convexes."); } public abstract void RayTest(int shapeIndex, in RigidPose pose, in RayData ray, ref float maximumT, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler; - public abstract void RayTest(int shapeIndex, in RigidPose rigidPose, ref RaySource rays, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler; + public abstract void RayTest(int shapeIndex, in RigidPose pose, ref RaySource rays, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler; /// /// Gets a raw untyped pointer to a shape's data. @@ -241,11 +255,9 @@ public override void ComputeBounds(ref BoundingBoxBatcher batcher) batcher.ExecuteConvexBatch(this); } - public override void ComputeBounds(int shapeIndex, in RigidPose pose, out Vector3 min, out Vector3 max) + public override void ComputeBounds(int shapeIndex, Quaternion orientation, out Vector3 min, out Vector3 max) { - shapes[shapeIndex].ComputeBounds(pose.Orientation, out min, out max); - min += pose.Position; - max += pose.Position; + shapes[shapeIndex].ComputeBounds(orientation, out min, out max); } internal override void ComputeBounds(int shapeIndex, in Quaternion orientation, out float maximumRadius, out float angularExpansion, out Vector3 min, out Vector3 max) @@ -306,11 +318,9 @@ public override void ComputeBounds(ref BoundingBoxBatcher batcher) batcher.ExecuteHomogeneousCompoundBatch(this); } - public override void ComputeBounds(int shapeIndex, in RigidPose pose, out Vector3 min, out Vector3 max) + public override void ComputeBounds(int shapeIndex, Quaternion orientation, out Vector3 min, out Vector3 max) { - shapes[shapeIndex].ComputeBounds(pose.Orientation, out min, out max); - min += pose.Position; - max += pose.Position; + shapes[shapeIndex].ComputeBounds(orientation, out min, out max); } public override void RayTest(int shapeIndex, in RigidPose pose, in RayData ray, ref float maximumT, ref TRayHitHandler hitHandler) { @@ -353,11 +363,9 @@ public override void ComputeBounds(ref BoundingBoxBatcher batcher) batcher.ExecuteCompoundBatch(this); } - public override void ComputeBounds(int shapeIndex, in RigidPose pose, out Vector3 min, out Vector3 max) + public override void ComputeBounds(int shapeIndex, Quaternion orientation, out Vector3 min, out Vector3 max) { - shapes[shapeIndex].ComputeBounds(pose.Orientation, shapeBatches, out min, out max); - min += pose.Position; - max += pose.Position; + shapes[shapeIndex].ComputeBounds(orientation, shapeBatches, out min, out max); } public override void RayTest(int shapeIndex, in RigidPose pose, in RayData ray, ref float maximumT, ref TRayHitHandler hitHandler) @@ -408,6 +416,19 @@ public void UpdateBounds(RigidPose pose, TypedIndex shapeIndex, out BoundingBox //Note: the min and max here are in absolute coordinates, which means this is a spot that has to be updated in the event that positions use a higher precision representation. batches[shapeIndex.Type].ComputeBounds(shapeIndex.Index, pose, out bounds.Min, out bounds.Max); } + /// + /// Computes a bounding box for a single shape. + /// + /// Position of the shape. + /// Orientation of the shape. + /// Index of the shape. + /// Bounding box of the specified shape with the specified pose. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void UpdateBounds(Vector3 position, Quaternion orientation, TypedIndex shapeIndex, out BoundingBox bounds) + { + //Note: the min and max here are in absolute coordinates, which means this is a spot that has to be updated in the event that positions use a higher precision representation. + batches[shapeIndex.Type].ComputeBounds(shapeIndex.Index, position, orientation, out bounds.Min, out bounds.Max); + } [MethodImpl(MethodImplOptions.AggressiveInlining)] public ref TShape GetShape(int shapeIndex) where TShape : unmanaged, IShape diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundMeshContinuations.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundMeshContinuations.cs index 155d251b9..f1f92989f 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundMeshContinuations.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundMeshContinuations.cs @@ -52,7 +52,7 @@ public unsafe void GetChildAData(ref CollisionBatcher co { ref var compound = ref Unsafe.AsRef(pair.A); ref var compoundChild = ref compound.GetChild(childIndexA); - Compound.GetRotatedChildPose(compoundChild.LocalPose, pair.OrientationA, out childPoseA); + Compound.GetRotatedChildPose(compoundChild.LocalPosition, compoundChild.LocalOrientation, pair.OrientationA, out childPoseA); childTypeA = compoundChild.ShapeIndex.Type; collisionBatcher.Shapes[childTypeA].GetShapeData(compoundChild.ShapeIndex.Index, out childShapeDataA, out _); } diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairContinuations.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairContinuations.cs index 004d7bdac..b7860ab68 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairContinuations.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairContinuations.cs @@ -25,7 +25,7 @@ public unsafe void GetChildAData(ref CollisionBatcher co { ref var compoundA = ref Unsafe.AsRef(pair.A); ref var compoundChildA = ref compoundA.GetChild(childIndexA); - Compound.GetRotatedChildPose(compoundChildA.LocalPose, pair.OrientationA, out childPoseA); + Compound.GetRotatedChildPose(compoundChildA.LocalPosition, compoundChildA.LocalOrientation, pair.OrientationA, out childPoseA); childTypeA = compoundChildA.ShapeIndex.Type; collisionBatcher.Shapes[childTypeA].GetShapeData(compoundChildA.ShapeIndex.Index, out childShapeDataA, out _); } @@ -43,7 +43,7 @@ public unsafe void ConfigureContinuationChild( childTypeB = compoundChildB.ShapeIndex.Type; collisionBatcher.Shapes[childTypeB].GetShapeData(compoundChildB.ShapeIndex.Index, out childShapeDataB, out _); - Compound.GetRotatedChildPose(compoundChildB.LocalPose, pair.OrientationB, out childPoseB); + Compound.GetRotatedChildPose(compoundChildB.LocalPosition, compoundChildB.LocalOrientation, pair.OrientationB, out childPoseB); if (pair.FlipMask < 0) { continuationChild.ChildIndexA = childIndexB; diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlapFinder.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlapFinder.cs index ab0cdd641..f48c26f67 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlapFinder.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlapFinder.cs @@ -79,7 +79,7 @@ public unsafe void FindLocalOverlaps(ref Buffer pairs, int pai Vector3Wide.WriteFirst(subpair.Pair->AngularVelocityB, ref GatherScatter.GetOffsetInstance(ref angularVelocityB, j)); Unsafe.Add(ref Unsafe.As, float>(ref maximumAllowedExpansion), j) = subpair.Pair->MaximumExpansion; - RigidPoseWide.WriteFirst(subpair.Child->LocalPose, ref GatherScatter.GetOffsetInstance(ref localPosesA, j)); + RigidPoseWide.WriteFirst(CompoundChild.AsPose(ref *subpair.Child), ref GatherScatter.GetOffsetInstance(ref localPosesA, j)); } QuaternionWide.Conjugate(orientationB, out var toLocalB); diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundContinuations.cs b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundContinuations.cs index 9c60bf74b..a83ae1ea7 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundContinuations.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundContinuations.cs @@ -23,7 +23,7 @@ public unsafe void ConfigureContinuationChild( { ref var compoundChild = ref Unsafe.AsRef(pair.B).GetChild(childIndex); ref var continuationChild = ref continuation.Children[continuationChildIndex]; - Compound.GetRotatedChildPose(compoundChild.LocalPose, pair.OrientationB, out childPoseB); + Compound.GetRotatedChildPose(compoundChild.LocalPosition, compoundChild.LocalOrientation, pair.OrientationB, out childPoseB); childTypeB = compoundChild.ShapeIndex.Type; collisionBatcher.Shapes[childTypeB].GetShapeData(compoundChild.ShapeIndex.Index, out childShapeDataB, out _); if (pair.FlipMask < 0) diff --git a/BepuPhysics/CollisionDetection/SweepTasks/CompoundHomogeneousCompoundSweepTask.cs b/BepuPhysics/CollisionDetection/SweepTasks/CompoundHomogeneousCompoundSweepTask.cs index d9cfbdc81..586141390 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/CompoundHomogeneousCompoundSweepTask.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/CompoundHomogeneousCompoundSweepTask.cs @@ -47,7 +47,7 @@ protected unsafe override bool PreorderedTypeSweep( var task = sweepTasks.GetTask(compoundChildType, Triangle.Id); shapes[compoundChildType].GetShapeData(compoundChild.ShapeIndex.Index, out var compoundChildShapeData, out _); if (task.Sweep( - compoundChildShapeData, compoundChildType, compoundChild.LocalPose, orientationA, velocityA, + compoundChildShapeData, compoundChildType, CompoundChild.AsPose(ref compoundChild), orientationA, velocityA, Unsafe.AsPointer(ref childB), Triangle.Id, childPoseB, offsetB, orientationB, velocityB, maximumT, minimumProgression, convergenceThreshold, maximumIterationCount, out var t0Candidate, out var t1Candidate, out var hitLocationCandidate, out var hitNormalCandidate)) diff --git a/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepOverlapFinder.cs b/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepOverlapFinder.cs index dba5614d1..46299362e 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepOverlapFinder.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepOverlapFinder.cs @@ -28,7 +28,7 @@ public unsafe void FindOverlaps( { ref var child = ref compoundA.GetChild(i); BoundingBoxHelpers.GetLocalBoundingBoxForSweep( - child.ShapeIndex, shapes, child.LocalPose, orientationA, velocityA, + child.ShapeIndex, shapes, CompoundChild.AsPose(ref child), orientationA, velocityA, offsetB, orientationB, velocityB, maximumT, out var sweep, out var min, out var max); ref var childOverlaps = ref overlaps.GetOverlapsForChild(i); childOverlaps.ChildIndex = i; diff --git a/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepTask.cs b/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepTask.cs index c4967890e..36c433748 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepTask.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepTask.cs @@ -50,8 +50,8 @@ protected override unsafe bool PreorderedTypeSweep( { var task = sweepTasks.GetTask(childTypeA, childTypeB); if (task != null && task.Sweep( - childShapeDataA, childTypeA, childA.LocalPose, orientationA, velocityA, - childShapeDataB, childTypeB, childB.LocalPose, offsetB, orientationB, velocityB, + childShapeDataA, childTypeA, CompoundChild.AsPose(ref childA), orientationA, velocityA, + childShapeDataB, childTypeB, CompoundChild.AsPose(ref childB), offsetB, orientationB, velocityB, maximumT, minimumProgression, convergenceThreshold, maximumIterationCount, out var t0Candidate, out var t1Candidate, out var hitLocationCandidate, out var hitNormalCandidate)) { diff --git a/BepuPhysics/CollisionDetection/SweepTasks/ConvexCompoundSweepTask.cs b/BepuPhysics/CollisionDetection/SweepTasks/ConvexCompoundSweepTask.cs index 656a2b92d..a15add4d4 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/ConvexCompoundSweepTask.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/ConvexCompoundSweepTask.cs @@ -42,7 +42,7 @@ protected override unsafe bool PreorderedTypeSweep( var task = sweepTasks.GetTask(convex.TypeId, childType); if (task != null && task.Sweep( shapeDataA, convex.TypeId, new RigidPose() { Orientation = Quaternion.Identity }, orientationA, velocityA, - childShapeData, childType, child.LocalPose, offsetB, orientationB, velocityB, + childShapeData, childType, CompoundChild.AsPose(ref child), offsetB, orientationB, velocityB, maximumT, minimumProgression, convergenceThreshold, maximumIterationCount, out var t0Candidate, out var t1Candidate, out var hitLocationCandidate, out var hitNormalCandidate)) { diff --git a/BepuPhysics/Trees/Tree.cs b/BepuPhysics/Trees/Tree.cs index 66117334e..620ef560b 100644 --- a/BepuPhysics/Trees/Tree.cs +++ b/BepuPhysics/Trees/Tree.cs @@ -3,10 +3,11 @@ using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; - +using System.Runtime.InteropServices; namespace BepuPhysics.Trees { + [StructLayout(LayoutKind.Sequential)] public unsafe partial struct Tree { /// @@ -17,57 +18,34 @@ public unsafe partial struct Tree /// Buffer of metanodes in the tree. Metanodes contain metadata that aren't read during most query operations but are useful for bookkeeping. /// public Buffer Metanodes; - int nodeCount; - /// - /// Gets or sets the number of nodes in the tree. - /// - public int NodeCount - { - readonly get - { - return nodeCount; - } - set - { - nodeCount = value; - } - } - /// /// Buffer of leaves in the tree. /// public Buffer Leaves; - int leafCount; /// - /// Gets or sets the number of leaves in the tree. + /// Number of nodes in the tree. /// - public int LeafCount - { - readonly get - { - return leafCount; - } - set - { - leafCount = value; - } - } + public int NodeCount; + /// + /// Number of leaves in the tree. + /// + public int LeafCount; [MethodImpl(MethodImplOptions.AggressiveInlining)] int AllocateNode() { - Debug.Assert(Nodes.Length > nodeCount && Metanodes.Length > nodeCount, + Debug.Assert(Nodes.Length > NodeCount && Metanodes.Length > NodeCount, "Any attempt to allocate a node should not overrun the allocated nodes. For all operations that allocate nodes, capacity should be preallocated."); - return nodeCount++; + return NodeCount++; } [MethodImpl(MethodImplOptions.AggressiveInlining)] int AddLeaf(int nodeIndex, int childIndex) { - Debug.Assert(leafCount < Leaves.Length, + Debug.Assert(LeafCount < Leaves.Length, "Any attempt to allocate a leaf should not overrun the allocated leaves. For all operations that allocate leaves, capacity should be preallocated."); - Leaves[leafCount] = new Leaf(nodeIndex, childIndex); - return leafCount++; + Leaves[LeafCount] = new Leaf(nodeIndex, childIndex); + return LeafCount++; } /// @@ -123,19 +101,19 @@ public Tree(Span data, BufferPool pool) { if (data.Length <= 4) throw new ArgumentException($"Data is only {data.Length} bytes long; that's too small for even a header."); - leafCount = Unsafe.As(ref data[0]); - nodeCount = leafCount - 1; - var leafByteCount = leafCount * sizeof(Leaf); - var nodeByteCount = nodeCount * sizeof(Node); - var metanodeByteCount = nodeCount * sizeof(Metanode); + LeafCount = Unsafe.As(ref data[0]); + NodeCount = LeafCount - 1; + var leafByteCount = LeafCount * sizeof(Leaf); + var nodeByteCount = NodeCount * sizeof(Node); + var metanodeByteCount = NodeCount * sizeof(Metanode); const int leavesStartIndex = 4; var nodesStartIndex = leavesStartIndex + leafByteCount; var metanodesStartIndex = nodesStartIndex + nodeByteCount; if (data.Length < leavesStartIndex + leafByteCount + nodeByteCount + metanodeByteCount) - throw new ArgumentException($"Header suggested there were {leafCount} leaves, but there's not enough room in the data for that."); - pool.Take(leafCount, out Leaves); - pool.Take(nodeCount, out Nodes); - pool.Take(nodeCount, out Metanodes); + throw new ArgumentException($"Header suggested there were {LeafCount} leaves, but there's not enough room in the data for that."); + pool.Take(LeafCount, out Leaves); + pool.Take(NodeCount, out Nodes); + pool.Take(NodeCount, out Metanodes); Unsafe.CopyBlockUnaligned(ref *(byte*)Leaves.Memory, ref data[leavesStartIndex], (uint)leafByteCount); Unsafe.CopyBlockUnaligned(ref *(byte*)Nodes.Memory, ref data[nodesStartIndex], (uint)nodeByteCount); Unsafe.CopyBlockUnaligned(ref *(byte*)Metanodes.Memory, ref data[metanodesStartIndex], (uint)metanodeByteCount); @@ -182,7 +160,7 @@ public static int Encode(int index) void InitializeRoot() { //The root always exists, even if there are no children in it. Makes some bookkeeping simpler. - nodeCount = 1; + NodeCount = 1; ref var rootMetanode = ref Metanodes[0]; rootMetanode.Parent = -1; rootMetanode.IndexInParent = -1; @@ -196,27 +174,27 @@ void InitializeRoot() public void Resize(BufferPool pool, int targetLeafSlotCount) { //Note that it's not safe to resize below the size of potentially used leaves. If the user wants to go smaller, they'll need to explicitly deal with the leaves somehow first. - var leafCapacityForTarget = BufferPool.GetCapacityForCount(Math.Max(leafCount, targetLeafSlotCount)); + var leafCapacityForTarget = BufferPool.GetCapacityForCount(Math.Max(LeafCount, targetLeafSlotCount)); //Adding incrementally checks the capacity of leaves, and issues a resize if there isn't enough space. But it doesn't check nodes. //You could change that, but for now, we simply ensure that the node array has sufficient room to hold everything in the resized leaf array. - var nodeCapacityForTarget = BufferPool.GetCapacityForCount(Math.Max(nodeCount, leafCapacityForTarget - 1)); - var metanodeCapacityForTarget = BufferPool.GetCapacityForCount(Math.Max(nodeCount, leafCapacityForTarget - 1)); + var nodeCapacityForTarget = BufferPool.GetCapacityForCount(Math.Max(NodeCount, leafCapacityForTarget - 1)); + var metanodeCapacityForTarget = BufferPool.GetCapacityForCount(Math.Max(NodeCount, leafCapacityForTarget - 1)); bool wasAllocated = Leaves.Allocated; Debug.Assert(Leaves.Allocated == Nodes.Allocated); if (leafCapacityForTarget != Leaves.Length) { - pool.ResizeToAtLeast(ref Leaves, leafCapacityForTarget, leafCount); + pool.ResizeToAtLeast(ref Leaves, leafCapacityForTarget, LeafCount); } if (nodeCapacityForTarget != Nodes.Length) { - pool.ResizeToAtLeast(ref Nodes, nodeCapacityForTarget, nodeCount); + pool.ResizeToAtLeast(ref Nodes, nodeCapacityForTarget, NodeCount); } if (metanodeCapacityForTarget != Metanodes.Length) { - pool.ResizeToAtLeast(ref Metanodes, metanodeCapacityForTarget, nodeCount); + pool.ResizeToAtLeast(ref Metanodes, metanodeCapacityForTarget, NodeCount); //A node's RefineFlag must be 0, so just clear out the node set. //TODO: This won't be necessary if we get rid of refineflags as a concept. - Metanodes.Clear(nodeCount, Nodes.Length - nodeCount); + Metanodes.Clear(NodeCount, Nodes.Length - NodeCount); } if (!wasAllocated) { @@ -230,7 +208,7 @@ public void Resize(BufferPool pool, int targetLeafSlotCount) /// public void Clear() { - leafCount = 0; + LeafCount = 0; InitializeRoot(); } @@ -259,7 +237,7 @@ public void Dispose(BufferPool pool) [MethodImpl(MethodImplOptions.AggressiveInlining)] public static bool Equals(in Tree a, in Tree b) { - return a.Nodes.Memory == b.Nodes.Memory && a.nodeCount == b.nodeCount; + return a.Nodes.Memory == b.Nodes.Memory && a.NodeCount == b.NodeCount; } } diff --git a/BepuPhysics/Trees/Tree_Add.cs b/BepuPhysics/Trees/Tree_Add.cs index ae463c25b..ab78ea49d 100644 --- a/BepuPhysics/Trees/Tree_Add.cs +++ b/BepuPhysics/Trees/Tree_Add.cs @@ -112,11 +112,11 @@ private static unsafe BestInsertionChoice ComputeBestInsertionChoice(ref Boundin public unsafe int Add(BoundingBox bounds, BufferPool pool) { //The rest of the function assumes we have sufficient room. We don't want to deal with invalidated pointers mid-add. - if (Leaves.Length == leafCount) + if (Leaves.Length == LeafCount) { //Note that, while we add 1, the underlying pool will request the next higher power of 2 in bytes that can hold it. //Since we're already at capacity, that will be ~double the size. - Resize(pool, leafCount + 1); + Resize(pool, LeafCount + 1); } //Assumption: Index 0 is always the root if it exists, and an empty tree will have a 'root' with a child count of 0. @@ -130,10 +130,10 @@ public unsafe int Add(BoundingBox bounds, BufferPool pool) ref var node = ref Nodes[nodeIndex]; //This is a binary tree, so the only time a node can have less than full children is when it's the root node. //By convention, an empty tree still has a root node with no children, so we do have to handle this case. - if (leafCount < 2) + if (LeafCount < 2) { //The best slot will, at best, be tied with inserting it in a leaf node because the change in heuristic cost for filling an empty slot is zero. - return InsertLeafIntoEmptySlot(ref bounds, nodeIndex, leafCount, ref node); + return InsertLeafIntoEmptySlot(ref bounds, nodeIndex, LeafCount, ref node); } else { diff --git a/BepuPhysics/Trees/Tree_BinnedRefine.cs b/BepuPhysics/Trees/Tree_BinnedRefine.cs index 1d2a8623f..f2902ae71 100644 --- a/BepuPhysics/Trees/Tree_BinnedRefine.cs +++ b/BepuPhysics/Trees/Tree_BinnedRefine.cs @@ -513,7 +513,7 @@ unsafe void ReifyChildren(int internalNodeIndex, Node* stagingNodes, child.Index = subtreeIndex; if (subtreeIndex >= 0) { - Debug.Assert(subtreeIndex >= 0 && subtreeIndex < nodeCount); + Debug.Assert(subtreeIndex >= 0 && subtreeIndex < NodeCount); //Subtree is an internal node. Update its parent pointers. ref var metanode = ref Metanodes[subtreeIndex]; metanode.IndexInParent = i; diff --git a/BepuPhysics/Trees/Tree_CacheOptimizer.cs b/BepuPhysics/Trees/Tree_CacheOptimizer.cs index 2da4c5edf..a1d83e774 100644 --- a/BepuPhysics/Trees/Tree_CacheOptimizer.cs +++ b/BepuPhysics/Trees/Tree_CacheOptimizer.cs @@ -411,7 +411,7 @@ public unsafe bool TrySwapNodesThreadSafe(ref int aIndex, ref int bIndex) /// Will return true even if not all nodes are optimized if the reason was a target index outside of the node list bounds. public unsafe bool IncrementalCacheOptimizeThreadSafe(int nodeIndex) { - Debug.Assert(leafCount >= 2, + Debug.Assert(LeafCount >= 2, "Should only use cache optimization when there are at least two leaves. Every node has to have 2 children, and optimizing a 0 or 1 leaf tree is silly anyway."); //Multithreaded cache optimization attempts to acquire a lock on every involved node. //If any lock fails, it just abandons the entire attempt. @@ -438,7 +438,7 @@ public unsafe bool IncrementalCacheOptimizeThreadSafe(int nodeIndex) for (int i = 0; i < 2; ++i) { ref var child = ref Unsafe.Add(ref children, i); - if (targetIndex >= nodeCount) + if (targetIndex >= NodeCount) { //This attempted swap would reach beyond the allocated nodes. //That means the current node is quite a bit a lower than it should be. @@ -528,7 +528,7 @@ public unsafe bool IncrementalCacheOptimizeThreadSafe(int nodeIndex) public unsafe void IncrementalCacheOptimize(int nodeIndex) { - if (leafCount <= 2) + if (LeafCount <= 2) { //Don't bother cache optimizing if there are only two leaves. There's no work to be done, and it supplies a guarantee to the rest of the optimization logic //so that we don't have to check per-node child counts. @@ -547,7 +547,7 @@ public unsafe void IncrementalCacheOptimize(int nodeIndex) for (int i = 0; i < 2; ++i) { - if (targetIndex >= nodeCount) + if (targetIndex >= NodeCount) { //This attempted swap would reach beyond the allocated nodes. //That means the current node is quite a bit a lower than it should be. @@ -580,7 +580,7 @@ unsafe void CacheOptimize(int nodeIndex, ref int nextIndex) ref var child = ref Unsafe.Add(ref children, i); if (child.Index >= 0) { - Debug.Assert(nextIndex >= 0 && nextIndex < nodeCount, + Debug.Assert(nextIndex >= 0 && nextIndex < NodeCount, "Swap target should be within the node set. If it's not, the initial node was probably not in global optimum position."); if (child.Index != nextIndex) SwapNodes(child.Index, nextIndex); @@ -599,7 +599,7 @@ unsafe void CacheOptimize(int nodeIndex, ref int nextIndex) /// Node to begin the optimization process at. public unsafe void CacheOptimize(int nodeIndex) { - if (leafCount <= 2) + if (LeafCount <= 2) { //Don't bother cache optimizing if there are only two leaves. There's no work to be done, and it supplies a guarantee to the rest of the optimization logic //so that we don't have to check per-node child counts. diff --git a/BepuPhysics/Trees/Tree_Diagnostics.cs b/BepuPhysics/Trees/Tree_Diagnostics.cs index a4a072cc7..343a0483d 100644 --- a/BepuPhysics/Trees/Tree_Diagnostics.cs +++ b/BepuPhysics/Trees/Tree_Diagnostics.cs @@ -24,7 +24,7 @@ public unsafe float MeasureCostMetric() ref var rootChildren = ref rootNode.A; var merged = new BoundingBox { Min = new Vector3(float.MaxValue), Max = new Vector3(-float.MaxValue) }; - for (int i = 0; i < leafCount; ++i) + for (int i = 0; i < LeafCount; ++i) { ref var child = ref Unsafe.Add(ref rootChildren, i); BoundingBox.CreateMerged(child.Min, child.Max, merged.Min, merged.Max, out merged.Min, out merged.Max); @@ -34,10 +34,10 @@ public unsafe float MeasureCostMetric() const float leafCost = 1; const float internalNodeCost = 1; - if (leafCount > 2) + if (LeafCount > 2) { float totalCost = 0; - for (int i = 0; i < nodeCount; ++i) + for (int i = 0; i < NodeCount; ++i) { ref var node = ref Nodes[i]; ref var children = ref node.A; @@ -79,7 +79,7 @@ readonly unsafe void Validate(int nodeIndex, int expectedParentIndex, int expect var badMaxValue = new Vector3(float.MinValue); var mergedMin = badMinValue; //Note- using isolated vectors instead of actual BoundingBox here to avoid a compiler bug: https://github.com/dotnet/coreclr/issues/12950 var mergedMax = badMaxValue; - var childCount = Math.Min(leafCount, 2); + var childCount = Math.Min(LeafCount, 2); for (int i = 0; i < childCount; ++i) { ref var child = ref Unsafe.Add(ref children, i); @@ -88,8 +88,8 @@ readonly unsafe void Validate(int nodeIndex, int expectedParentIndex, int expect BoundingBox.CreateMerged(mergedMin, mergedMax, child.Min, child.Max, out mergedMin, out mergedMax); if (child.Index >= 0) { - if (child.Index >= nodeCount) - throw new Exception($"Implied existence of node {child} is outside of count {nodeCount}."); + if (child.Index >= NodeCount) + throw new Exception($"Implied existence of node {child} is outside of count {NodeCount}."); Validate(child.Index, nodeIndex, i, ref child.Min, ref child.Max, out int childFoundLeafCount); if (childFoundLeafCount != child.LeafCount) throw new Exception($"Bad leaf count for child {i} of node {nodeIndex}."); @@ -103,7 +103,7 @@ readonly unsafe void Validate(int nodeIndex, int expectedParentIndex, int expect throw new Exception($"Bad leaf count on {nodeIndex} child {i}, it's a leaf but leafCount is {child.LeafCount}."); } var leafIndex = Encode(child.Index); - if (leafIndex < 0 || leafIndex >= leafCount) + if (leafIndex < 0 || leafIndex >= LeafCount) throw new Exception("Bad node-contained leaf index."); if (Leaves[leafIndex].NodeIndex != nodeIndex || Leaves[leafIndex].ChildIndex != i) { @@ -111,7 +111,7 @@ readonly unsafe void Validate(int nodeIndex, int expectedParentIndex, int expect } } } - if (foundLeafCount == 0 && (leafCount > 0 || expectedParentIndex >= 0)) + if (foundLeafCount == 0 && (LeafCount > 0 || expectedParentIndex >= 0)) { //The only time foundLeafCount can be zero is if this is the root node in an empty tree. throw new Exception("Bad leaf count."); @@ -131,15 +131,15 @@ readonly unsafe void Validate(int nodeIndex, int expectedParentIndex, int expect readonly unsafe void ValidateLeafNodeIndices() { - for (int i = 0; i < leafCount; ++i) + for (int i = 0; i < LeafCount; ++i) { if (Leaves[i].NodeIndex < 0) { throw new Exception($"Leaf {i} has negative node index: {Leaves[i].NodeIndex}."); } - if (Leaves[i].NodeIndex >= nodeCount) + if (Leaves[i].NodeIndex >= NodeCount) { - throw new Exception($"Leaf {i} points to a node outside the node set, {Leaves[i].NodeIndex} >= {nodeCount}."); + throw new Exception($"Leaf {i} points to a node outside the node set, {Leaves[i].NodeIndex} >= {NodeCount}."); } } } @@ -148,7 +148,7 @@ readonly unsafe void ValidateLeaves() { ValidateLeafNodeIndices(); - for (int i = 0; i < leafCount; ++i) + for (int i = 0; i < LeafCount; ++i) { if (Encode(Unsafe.Add(ref Nodes[Leaves[i].NodeIndex].A, Leaves[i].ChildIndex).Index) != i) { @@ -159,19 +159,19 @@ readonly unsafe void ValidateLeaves() public readonly unsafe void Validate() { - if (nodeCount < 0) + if (NodeCount < 0) { - throw new Exception($"Invalid negative node count of {nodeCount}"); + throw new Exception($"Invalid negative node count of {NodeCount}"); } - else if (nodeCount > Nodes.Length) + else if (NodeCount > Nodes.Length) { - throw new Exception($"Invalid node count of {nodeCount}, larger than nodes array length {Nodes.Length}."); + throw new Exception($"Invalid node count of {NodeCount}, larger than nodes array length {Nodes.Length}."); } if (LeafCount > 0 && (Metanodes[0].Parent != -1 || Metanodes[0].IndexInParent != -1)) { throw new Exception($"Invalid parent pointers on root."); } - if ((nodeCount != 1 && leafCount < 2) || (nodeCount != LeafCount - 1 && leafCount >= 2)) + if ((NodeCount != 1 && LeafCount < 2) || (NodeCount != LeafCount - 1 && LeafCount >= 2)) { throw new Exception($"Invalid node count versus leaf count."); } @@ -179,8 +179,8 @@ public readonly unsafe void Validate() var standInBounds = new BoundingBox(); Validate(0, -1, -1, ref standInBounds.Min, ref standInBounds.Max, out int foundLeafCount); - if (foundLeafCount != leafCount) - throw new Exception($"{foundLeafCount} leaves found in tree, expected {leafCount}."); + if (foundLeafCount != LeafCount) + throw new Exception($"{foundLeafCount} leaves found in tree, expected {LeafCount}."); ValidateLeaves(); @@ -191,7 +191,7 @@ readonly unsafe int ComputeMaximumDepth(ref Node node, int currentDepth) ref var children = ref node.A; int maximum = currentDepth; int nextDepth = currentDepth + 1; - var childCount = Math.Min(leafCount, 2); + var childCount = Math.Min(LeafCount, 2); for (int i = 0; i < childCount; ++i) { ref var child = ref Unsafe.Add(ref children, i); @@ -220,7 +220,7 @@ readonly unsafe void MeasureCacheQuality(int nodeIndex, out int foundNodes, out int correctlyPositionedImmediateChildren = 0; int immediateInternalChildren = 0; int expectedChildIndex = nodeIndex + 1; - var childCount = Math.Min(leafCount, 2); + var childCount = Math.Min(LeafCount, 2); for (int i = 0; i < childCount; ++i) { ref var child = ref Unsafe.Add(ref children, i); @@ -257,7 +257,7 @@ public readonly unsafe float MeasureCacheQuality() public readonly unsafe float MeasureCacheQuality(int nodeIndex) { - if (nodeIndex < 0 || nodeIndex >= nodeCount) + if (nodeIndex < 0 || nodeIndex >= NodeCount) throw new ArgumentException("Measurement target index must be nonnegative and less than node count."); MeasureCacheQuality(nodeIndex, out int foundNodes, out float nodeScore, out int scorableNodeCount); return scorableNodeCount > 0 ? nodeScore / scorableNodeCount : 1; diff --git a/BepuPhysics/Trees/Tree_IntertreeQueries.cs b/BepuPhysics/Trees/Tree_IntertreeQueries.cs index 437cd0a73..ab08c69c9 100644 --- a/BepuPhysics/Trees/Tree_IntertreeQueries.cs +++ b/BepuPhysics/Trees/Tree_IntertreeQueries.cs @@ -135,14 +135,14 @@ private unsafe void GetOverlapsBetweenDifferentNodes(ref Node a public unsafe void GetOverlaps(ref Tree treeB, ref TOverlapHandler overlapHandler) where TOverlapHandler : struct, IOverlapHandler { - if (leafCount == 0 || treeB.leafCount == 0) + if (LeafCount == 0 || treeB.LeafCount == 0) return; - if (leafCount >= 2 && treeB.leafCount >= 2) + if (LeafCount >= 2 && treeB.LeafCount >= 2) { //Both trees have complete nodes; we can use a general case. GetOverlapsBetweenDifferentNodes(ref Nodes[0], ref treeB.Nodes[0], ref treeB, ref overlapHandler); } - else if (leafCount == 1 && treeB.leafCount >= 2) + else if (LeafCount == 1 && treeB.LeafCount >= 2) { //Tree A is degenerate; needs a special case. ref var a = ref Nodes[0]; @@ -158,7 +158,7 @@ public unsafe void GetOverlaps(ref Tree treeB, ref TOverlapHand DispatchTestForNodes(ref a.A, ref b.B, ref treeB, ref overlapHandler); } } - else if (leafCount >= 2 && treeB.leafCount == 1) + else if (LeafCount >= 2 && treeB.LeafCount == 1) { //Tree B is degenerate; needs a special case. ref var a = ref Nodes[0]; @@ -176,7 +176,7 @@ public unsafe void GetOverlaps(ref Tree treeB, ref TOverlapHand } else { - Debug.Assert(leafCount == 1 && treeB.leafCount == 1); + Debug.Assert(LeafCount == 1 && treeB.LeafCount == 1); if (Intersects(Nodes[0].A, treeB.Nodes[0].A)) { DispatchTestForNodes(ref Nodes[0].A, ref treeB.Nodes[0].A, ref treeB, ref overlapHandler); diff --git a/BepuPhysics/Trees/Tree_IntertreeQueriesMT.cs b/BepuPhysics/Trees/Tree_IntertreeQueriesMT.cs index c5fe6b7bd..896f3e1fa 100644 --- a/BepuPhysics/Trees/Tree_IntertreeQueriesMT.cs +++ b/BepuPhysics/Trees/Tree_IntertreeQueriesMT.cs @@ -42,7 +42,7 @@ public MultithreadedIntertreeTest(BufferPool pool) /// Number of threads to prepare jobs for. public unsafe void PrepareJobs(ref Tree treeA, ref Tree treeB, TOverlapHandler[] overlapHandlers, int threadCount) { - if (treeA.leafCount == 0 || treeB.leafCount == 0) + if (treeA.LeafCount == 0 || treeB.LeafCount == 0) { //If either tree has zero leaves, no intertree test is required. //Since this context has a count property for scheduling purposes that reads the jobs list, clear it to ensure no spurious jobs are executed. @@ -53,19 +53,19 @@ public unsafe void PrepareJobs(ref Tree treeA, ref Tree treeB, TOverlapHandler[] const float jobMultiplier = 1.5f; var targetJobCount = Math.Max(1, jobMultiplier * threadCount); //TODO: Not a lot of thought was put into this leaf threshold for intertree. Probably better options. - leafThreshold = (int)((treeA.leafCount + treeB.leafCount) / targetJobCount); + leafThreshold = (int)((treeA.LeafCount + treeB.LeafCount) / targetJobCount); jobs = new QuickList((int)(targetJobCount * 2), Pool); NextNodePair = -1; this.OverlapHandlers = overlapHandlers; this.TreeA = treeA; this.TreeB = treeB; //Collect jobs. - if (treeA.leafCount >= 2 && treeB.leafCount >= 2) + if (treeA.LeafCount >= 2 && treeB.LeafCount >= 2) { //Both trees have complete nodes; we can use a general case. GetJobsBetweenDifferentNodes(ref treeA.Nodes[0], ref treeB.Nodes[0], ref OverlapHandlers[0]); } - else if (treeA.leafCount == 1 && treeB.leafCount >= 2) + else if (treeA.LeafCount == 1 && treeB.LeafCount >= 2) { //Tree A is degenerate; needs a special case. ref var a = ref treeA.Nodes[0]; @@ -81,7 +81,7 @@ public unsafe void PrepareJobs(ref Tree treeA, ref Tree treeB, TOverlapHandler[] DispatchTestForNodes(ref a.A, ref b.B, ref OverlapHandlers[0]); } } - else if (treeA.leafCount >= 2 && treeB.leafCount == 1) + else if (treeA.LeafCount >= 2 && treeB.LeafCount == 1) { //Tree B is degenerate; needs a special case. ref var a = ref treeA.Nodes[0]; @@ -99,7 +99,7 @@ public unsafe void PrepareJobs(ref Tree treeA, ref Tree treeB, TOverlapHandler[] } else { - Debug.Assert(treeA.leafCount == 1 && treeB.leafCount == 1); + Debug.Assert(treeA.LeafCount == 1 && treeB.LeafCount == 1); if (Intersects(treeA.Nodes[0].A, treeB.Nodes[0].A)) { DispatchTestForNodes(ref treeA.Nodes[0].A, ref treeB.Nodes[0].A, ref OverlapHandlers[0]); diff --git a/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs b/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs index 1f99c2b34..34089d60b 100644 --- a/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs +++ b/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs @@ -43,7 +43,7 @@ public RefitAndRefineMultithreadedContext() public unsafe void CreateRefitAndMarkJobs(ref Tree tree, BufferPool pool, IThreadDispatcher threadDispatcher) { - if (tree.leafCount <= 2) + if (tree.LeafCount <= 2) { //If there are 2 or less leaves, then refit/refine doesn't do anything at all. //(The root node has no parent, so it does not have a bounding box, and the SAH won't change no matter how we swap the children of the root.) @@ -65,7 +65,7 @@ public unsafe void CreateRefitAndMarkJobs(ref Tree tree, BufferPool pool, IThrea RefinementCandidates[i] = new QuickList(estimatedRefinementCandidateCount, threadDispatcher.GetThreadMemoryPool(i)); } - int multithreadingLeafCountThreshold = Tree.leafCount / (threadDispatcher.ThreadCount * 2); + int multithreadingLeafCountThreshold = Tree.LeafCount / (threadDispatcher.ThreadCount * 2); if (multithreadingLeafCountThreshold < RefinementLeafCountThreshold) multithreadingLeafCountThreshold = RefinementLeafCountThreshold; CollectNodesForMultithreadedRefit(0, multithreadingLeafCountThreshold, ref RefitNodes, RefinementLeafCountThreshold, ref RefinementCandidates[0], @@ -76,7 +76,7 @@ public unsafe void CreateRefitAndMarkJobs(ref Tree tree, BufferPool pool, IThrea public unsafe void CreateRefinementJobs(BufferPool pool, int frameIndex, float refineAggressivenessScale = 1) { - if (Tree.leafCount <= 2) + if (Tree.LeafCount <= 2) { //If there are 2 or less leaves, then refit/refine doesn't do anything at all. //(The root node has no parent, so it does not have a bounding box, and the SAH won't change no matter how we swap the children of the root.) @@ -128,7 +128,7 @@ public unsafe void CreateRefinementJobs(BufferPool pool, int frameIndex, float r public unsafe void CleanUpForRefitAndRefine(BufferPool pool) { - if (Tree.leafCount <= 2) + if (Tree.LeafCount <= 2) { //If there are 2 or less leaves, then refit/refine doesn't do anything at all. return; @@ -170,7 +170,7 @@ unsafe void CollectNodesForMultithreadedRefit(int nodeIndex, ref var metanode = ref Tree.Metanodes[nodeIndex]; ref var children = ref node.A; Debug.Assert(metanode.RefineFlag == 0); - Debug.Assert(Tree.leafCount > 2); + Debug.Assert(Tree.LeafCount > 2); for (int i = 0; i < 2; ++i) { ref var child = ref Unsafe.Add(ref children, i); @@ -318,7 +318,7 @@ public unsafe void RefitAndMarkForWorker(int workerIndex) //The main thread already created the refinement candidate list using the worker's pool. var threadPool = threadDispatcher.GetThreadMemoryPool(workerIndex); int refitIndex; - Debug.Assert(Tree.leafCount > 2); + Debug.Assert(Tree.LeafCount > 2); while ((refitIndex = Interlocked.Increment(ref RefitNodeIndex)) < RefitNodes.Count) { ExecuteRefitAndMarkJob(threadPool, workerIndex, refitIndex); diff --git a/BepuPhysics/Trees/Tree_RayCast.cs b/BepuPhysics/Trees/Tree_RayCast.cs index e442d6151..b387a8d01 100644 --- a/BepuPhysics/Trees/Tree_RayCast.cs +++ b/BepuPhysics/Trees/Tree_RayCast.cs @@ -26,8 +26,8 @@ public unsafe static bool Intersects(in Vector3 min, in Vector3 max, TreeRay* ra internal readonly unsafe void RayCast(int nodeIndex, TreeRay* treeRay, RayData* rayData, int* stack, ref TLeafTester leafTester) where TLeafTester : IRayLeafTester { - Debug.Assert((nodeIndex >= 0 && nodeIndex < nodeCount) || (Encode(nodeIndex) >= 0 && Encode(nodeIndex) < leafCount)); - Debug.Assert(leafCount >= 2, "This implementation assumes all nodes are filled."); + Debug.Assert((nodeIndex >= 0 && nodeIndex < NodeCount) || (Encode(nodeIndex) >= 0 && Encode(nodeIndex) < LeafCount)); + Debug.Assert(LeafCount >= 2, "This implementation assumes all nodes are filled."); int stackEnd = 0; while (true) @@ -91,10 +91,10 @@ internal readonly unsafe void RayCast(int nodeIndex, TreeRay* treeR internal readonly unsafe void RayCast(TreeRay* treeRay, RayData* rayData, ref TLeafTester leafTester) where TLeafTester : IRayLeafTester { - if (leafCount == 0) + if (LeafCount == 0) return; - if (leafCount == 1) + if (LeafCount == 1) { //If the first node isn't filled, we have to use a special case. if (Intersects(Nodes[0].A.Min, Nodes[0].A.Max, treeRay, out var tA)) diff --git a/BepuPhysics/Trees/Tree_RefineCommon.cs b/BepuPhysics/Trees/Tree_RefineCommon.cs index c215fb4d9..12b15b01b 100644 --- a/BepuPhysics/Trees/Tree_RefineCommon.cs +++ b/BepuPhysics/Trees/Tree_RefineCommon.cs @@ -234,7 +234,7 @@ unsafe void ValidateStaging(Node* stagingNodes, ref QuickList subtreeNodePo var internalReferences = new QuickList(subtreeNodePointers.Count, pool); internalReferences.Add(0, pool); ValidateStaging(stagingNodes, 0, ref subtreeNodePointers, ref collectedSubtreeReferences, ref internalReferences, pool, out int foundSubtrees, out int foundLeafCount); - if (treeletParent < -1 || treeletParent >= nodeCount) + if (treeletParent < -1 || treeletParent >= NodeCount) throw new Exception("Bad treelet parent."); if (treeletIndexInParent < -1 || (treeletParent >= 0 && treeletIndexInParent >= 2)) throw new Exception("Bad treelet index in parent."); diff --git a/BepuPhysics/Trees/Tree_RefinementScheduling.cs b/BepuPhysics/Trees/Tree_RefinementScheduling.cs index 2cfe3ba01..85f2fc696 100644 --- a/BepuPhysics/Trees/Tree_RefinementScheduling.cs +++ b/BepuPhysics/Trees/Tree_RefinementScheduling.cs @@ -16,7 +16,7 @@ unsafe float RefitAndMeasure(ref NodeChild child) ref var node = ref Nodes[child.Index]; //All nodes are guaranteed to have at least 2 children. - Debug.Assert(leafCount >= 2); + Debug.Assert(LeafCount >= 2); var premetric = ComputeBoundsMetric(ref child.Min, ref child.Max); float childChange = 0; @@ -152,10 +152,10 @@ unsafe void ValidateRefineFlags(int index) readonly void GetRefitAndMarkTuning(out int maximumSubtrees, out int estimatedRefinementCandidateCount, out int refinementLeafCountThreshold) { - maximumSubtrees = (int)(Math.Sqrt(leafCount) * 3); - estimatedRefinementCandidateCount = (leafCount * 2) / maximumSubtrees; + maximumSubtrees = (int)(Math.Sqrt(LeafCount) * 3); + estimatedRefinementCandidateCount = (LeafCount * 2) / maximumSubtrees; - refinementLeafCountThreshold = Math.Min(leafCount, maximumSubtrees); + refinementLeafCountThreshold = Math.Min(LeafCount, maximumSubtrees); } readonly void GetRefineTuning(int frameIndex, int refinementCandidatesCount, float refineAggressivenessScale, float costChange, @@ -170,7 +170,7 @@ readonly void GetRefineTuning(int frameIndex, int refinementCandidatesCount, flo var refineAggressiveness = Math.Max(0, costChange * refineAggressivenessScale); float refinePortion = Math.Min(1, refineAggressiveness * 0.25f); - var targetRefinementScale = Math.Min(nodeCount, Math.Max(2, (float)Math.Ceiling(refinementCandidatesCount * refineAggressivenessScale * 0.03f)) + refinementCandidatesCount * refinePortion); + var targetRefinementScale = Math.Min(NodeCount, Math.Max(2, (float)Math.Ceiling(refinementCandidatesCount * refineAggressivenessScale * 0.03f)) + refinementCandidatesCount * refinePortion); //Note that the refinementCandidatesCount is used as a maximum instead of refinementCandidates + 1 for simplicity, since there's a chance //that the root would already be a refinementCandidate. Doesn't really have a significant effect either way. refinementPeriod = Math.Max(1, (int)(refinementCandidatesCount / targetRefinementScale)); @@ -181,7 +181,7 @@ readonly void GetRefineTuning(int frameIndex, int refinementCandidatesCount, flo public unsafe void RefitAndRefine(BufferPool pool, int frameIndex, float refineAggressivenessScale = 1) { //Don't proceed if the tree has no refitting or refinement required. This also guarantees that any nodes that do exist have two children. - if (leafCount <= 2) + if (LeafCount <= 2) return; GetRefitAndMarkTuning(out int maximumSubtrees, out int estimatedRefinementCandidateCount, out int leafCountThreshold); var refinementCandidates = new QuickList(estimatedRefinementCandidateCount, pool); diff --git a/BepuPhysics/Trees/Tree_Refit.cs b/BepuPhysics/Trees/Tree_Refit.cs index 8072c1941..fe218a37b 100644 --- a/BepuPhysics/Trees/Tree_Refit.cs +++ b/BepuPhysics/Trees/Tree_Refit.cs @@ -33,7 +33,7 @@ public unsafe readonly void RefitForNodeBoundsChange(int nodeIndex) //That would only happen if it turns out useful for other parts of the execution, though- optimizing refits at the cost of self-tests would be a terrible idea. readonly unsafe void Refit(int nodeIndex, out Vector3 min, out Vector3 max) { - Debug.Assert(leafCount >= 2); + Debug.Assert(LeafCount >= 2); ref var node = ref Nodes[nodeIndex]; ref var a = ref node.A; if (node.A.Index >= 0) @@ -54,7 +54,7 @@ readonly unsafe void Refit(int nodeIndex, out Vector3 min, out Vector3 max) public unsafe readonly void Refit() { //No point in refitting a tree with no internal nodes! - if (leafCount <= 2) + if (LeafCount <= 2) return; Refit(0, out var rootMin, out var rootMax); } diff --git a/BepuPhysics/Trees/Tree_Remove.cs b/BepuPhysics/Trees/Tree_Remove.cs index 16fe0d704..b94bbe8ac 100644 --- a/BepuPhysics/Trees/Tree_Remove.cs +++ b/BepuPhysics/Trees/Tree_Remove.cs @@ -10,18 +10,18 @@ partial struct Tree unsafe void RemoveNodeAt(int nodeIndex) { //Note that this function is a cache scrambling influence. That's okay- the cache optimization routines will take care of it later. - Debug.Assert(nodeIndex < nodeCount && nodeIndex >= 0); + Debug.Assert(nodeIndex < NodeCount && nodeIndex >= 0); //We make no guarantees here about maintaining the tree's coherency after a remove. //That's the responsibility of whoever called RemoveAt. - --nodeCount; + --NodeCount; //If the node wasn't the last node in the list, it will be replaced by the last node. - if (nodeIndex < nodeCount) + if (nodeIndex < NodeCount) { //Swap last node for removed node. ref var node = ref Nodes[nodeIndex]; - node = Nodes[nodeCount]; + node = Nodes[NodeCount]; ref var metanode = ref Metanodes[nodeIndex]; - metanode = Metanodes[nodeCount]; + metanode = Metanodes[NodeCount]; //Update the moved node's pointers: //its parent's child pointer should change, and... @@ -73,19 +73,19 @@ unsafe void RefitForRemoval(int nodeIndex) /// If leafIndex pointed at the last slot in the list, then this returns -1 since no leaf was moved. public unsafe int RemoveAt(int leafIndex) { - if (leafIndex < 0 || leafIndex >= leafCount) + if (leafIndex < 0 || leafIndex >= LeafCount) throw new ArgumentOutOfRangeException("Leaf index must be a valid index in the tree's leaf array."); //Cache the leaf being removed. var leaf = Leaves[leafIndex]; //Delete the leaf from the leaves array. - --leafCount; - if (leafIndex < leafCount) + --LeafCount; + if (leafIndex < LeafCount) { //The removed leaf was not the last leaf, so we should move the last leaf into its slot. //This can result in a form of cache scrambling, but these leaves do not need to be referenced during high performance stages. //It does somewhat reduce the performance of AABB updating, but we shouldn't bother with any form of cache optimization for this unless it becomes a proven issue. - ref var lastLeaf = ref Leaves[leafCount]; + ref var lastLeaf = ref Leaves[LeafCount]; Leaves[leafIndex] = lastLeaf; Unsafe.Add(ref Nodes[lastLeaf.NodeIndex].A, lastLeaf.ChildIndex).Index = Encode(leafIndex); } @@ -144,7 +144,7 @@ public unsafe int RemoveAt(int leafIndex) //This is the root. It cannot collapse, but if the other child is an internal node, then it will overwrite the root node. //This maintains the guarantee that any tree with at least 2 leaf nodes has every single child slot filled with a node or leaf. Debug.Assert(leaf.NodeIndex == 0, "Only the root should have a negative parent, so only the root should show up here."); - if (leafCount > 0) + if (LeafCount > 0) { //The post-removal leafCount is still positive, so there must be at least one child in the root node. //If it is an internal node, then it will be promoted into the root node's slot. @@ -187,7 +187,7 @@ public unsafe int RemoveAt(int leafIndex) } //No need to perform a RefitForRemoval here; it's the root. There is no higher bounding box. } - return leafIndex < leafCount ? leafCount : -1; + return leafIndex < LeafCount ? LeafCount : -1; } } } diff --git a/BepuPhysics/Trees/Tree_SelfQueriesMT.cs b/BepuPhysics/Trees/Tree_SelfQueriesMT.cs index 1f5f2ddf1..fc879af20 100644 --- a/BepuPhysics/Trees/Tree_SelfQueriesMT.cs +++ b/BepuPhysics/Trees/Tree_SelfQueriesMT.cs @@ -73,7 +73,7 @@ public void PrepareJobs(ref Tree tree, TOverlapHandler[] overlapHandlers, int th { //If there are not multiple children, there's no need to recurse. //This provides a guarantee that there are at least 2 children in each internal node considered by GetOverlapsInNode. - if (tree.leafCount < 2) + if (tree.LeafCount < 2) { //We clear it out to avoid keeping any old job counts. The count property is used for scheduling, so incorrect values could break the job scheduler. jobs = new QuickList(); @@ -82,13 +82,13 @@ public void PrepareJobs(ref Tree tree, TOverlapHandler[] overlapHandlers, int th Debug.Assert(overlapHandlers.Length >= threadCount); const float jobMultiplier = 1.5f; var targetJobCount = Math.Max(1, jobMultiplier * threadCount); - leafThreshold = (int)(tree.leafCount / targetJobCount); + leafThreshold = (int)(tree.LeafCount / targetJobCount); jobs = new QuickList((int)(targetJobCount * 2), Pool); NextNodePair = -1; this.OverlapHandlers = overlapHandlers; this.Tree = tree; //Collect jobs. - CollectJobsInNode(0, tree.leafCount, ref OverlapHandlers[0]); + CollectJobsInNode(0, tree.LeafCount, ref OverlapHandlers[0]); } /// diff --git a/BepuPhysics/Trees/Tree_Sweep.cs b/BepuPhysics/Trees/Tree_Sweep.cs index 758f0ca28..612c63186 100644 --- a/BepuPhysics/Trees/Tree_Sweep.cs +++ b/BepuPhysics/Trees/Tree_Sweep.cs @@ -16,8 +16,8 @@ partial struct Tree { readonly unsafe void Sweep(int nodeIndex, in Vector3 expansion, in Vector3 origin, in Vector3 direction, TreeRay* treeRay, int* stack, ref TLeafTester leafTester) where TLeafTester : ISweepLeafTester { - Debug.Assert((nodeIndex >= 0 && nodeIndex < nodeCount) || (Encode(nodeIndex) >= 0 && Encode(nodeIndex) < leafCount)); - Debug.Assert(leafCount >= 2, "This implementation assumes all nodes are filled."); + Debug.Assert((nodeIndex >= 0 && nodeIndex < NodeCount) || (Encode(nodeIndex) >= 0 && Encode(nodeIndex) < LeafCount)); + Debug.Assert(LeafCount >= 2, "This implementation assumes all nodes are filled."); int stackEnd = 0; while (true) @@ -83,10 +83,10 @@ readonly unsafe void Sweep(int nodeIndex, in Vector3 expansion, in internal readonly unsafe void Sweep(in Vector3 expansion, in Vector3 origin, in Vector3 direction, TreeRay* treeRay, ref TLeafTester sweepTester) where TLeafTester : ISweepLeafTester { - if (leafCount == 0) + if (LeafCount == 0) return; - if (leafCount == 1) + if (LeafCount == 1) { //If the first node isn't filled, we have to use a special case. if (Intersects(Nodes[0].A.Min - expansion, Nodes[0].A.Max + expansion, treeRay, out var tA)) diff --git a/BepuPhysics/Trees/Tree_SweepBuilder.cs b/BepuPhysics/Trees/Tree_SweepBuilder.cs index acf8cc044..64f69d3b0 100644 --- a/BepuPhysics/Trees/Tree_SweepBuilder.cs +++ b/BepuPhysics/Trees/Tree_SweepBuilder.cs @@ -254,14 +254,14 @@ public unsafe void SweepBuild(BufferPool pool, Buffer leafBounds) //The tree is built with an empty node at the root to make insertion work more easily. //As long as that is the case (and as long as this is not a constructor), //we must clear it out. - nodeCount = 0; + NodeCount = 0; //Guarantee that no resizes will occur during the build. if (Leaves.Length < leafBounds.Length) { Resize(pool, leafBounds.Length); } - leafCount = leafBounds.Length; + LeafCount = leafBounds.Length; pool.TakeAtLeast(leafBounds.Length, out var indexMap); diff --git a/BepuPhysics/Trees/Tree_VolumeQuery.cs b/BepuPhysics/Trees/Tree_VolumeQuery.cs index d301bf432..626f605e6 100644 --- a/BepuPhysics/Trees/Tree_VolumeQuery.cs +++ b/BepuPhysics/Trees/Tree_VolumeQuery.cs @@ -9,8 +9,8 @@ partial struct Tree { unsafe readonly void GetOverlaps(int nodeIndex, in Vector3 min, in Vector3 max, int* stack, ref TEnumerator leafEnumerator) where TEnumerator : IBreakableForEach { - Debug.Assert((nodeIndex >= 0 && nodeIndex < nodeCount) || (Encode(nodeIndex) >= 0 && Encode(nodeIndex) < leafCount)); - Debug.Assert(leafCount >= 2, "This implementation assumes all nodes are filled."); + Debug.Assert((nodeIndex >= 0 && nodeIndex < NodeCount) || (Encode(nodeIndex) >= 0 && Encode(nodeIndex) < LeafCount)); + Debug.Assert(LeafCount >= 2, "This implementation assumes all nodes are filled."); int stackEnd = 0; while (true) @@ -60,14 +60,14 @@ unsafe readonly void GetOverlaps(int nodeIndex, in Vector3 min, in [MethodImpl(MethodImplOptions.AggressiveInlining)] public readonly unsafe void GetOverlaps(in Vector3 min, in Vector3 max, ref TEnumerator leafEnumerator) where TEnumerator : IBreakableForEach { - if (leafCount > 1) + if (LeafCount > 1) { //TODO: Explicitly tracking depth in the tree during construction/refinement is practically required to guarantee correctness. //While it's exceptionally rare that any tree would have more than 256 levels, the worst case of stomping stack memory is not acceptable in the long run. var stack = stackalloc int[TraversalStackCapacity]; GetOverlaps(0, min, max, stack, ref leafEnumerator); } - else if (leafCount == 1) + else if (LeafCount == 1) { Debug.Assert(Nodes[0].A.Index < 0, "If the root only has one child, it must be a leaf."); if (BoundingBox.Intersects(min, max, Nodes[0].A.Min, Nodes[0].A.Max)) From f4889705fccd325bd7f857e3f77630526baf3b3a Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 3 Aug 2022 15:53:15 -0500 Subject: [PATCH 511/947] Purged some unnecessary ins. --- BepuPhysics/BodyProperties.cs | 4 +- BepuPhysics/BodyReference.cs | 16 +++--- BepuPhysics/BoundingBoxHelpers.cs | 14 ++--- BepuPhysics/Collidables/BigCompound.cs | 4 +- BepuPhysics/Collidables/Box.cs | 4 +- BepuPhysics/Collidables/Capsule.cs | 4 +- BepuPhysics/Collidables/Compound.cs | 6 +-- BepuPhysics/Collidables/ConvexHull.cs | 4 +- BepuPhysics/Collidables/ConvexHullHelper.cs | 4 +- BepuPhysics/Collidables/Cylinder.cs | 4 +- BepuPhysics/Collidables/IShape.cs | 4 +- BepuPhysics/Collidables/Mesh.cs | 4 +- BepuPhysics/Collidables/MeshInertiaHelper.cs | 14 ++--- BepuPhysics/Collidables/Shapes.cs | 4 +- BepuPhysics/Collidables/Sphere.cs | 4 +- BepuPhysics/Collidables/Triangle.cs | 8 +-- .../CollisionDetection/BroadPhase_Queries.cs | 8 +-- .../CollisionDetection/CollisionBatcher.cs | 16 +++--- .../ConvexCompoundOverlapFinder.cs | 2 +- .../CylinderConvexHullTester.cs | 6 +-- .../CollisionTasks/ManifoldCandidateHelper.cs | 8 +-- .../CollisionDetection/MeshReduction.cs | 2 +- .../NarrowPhaseCCDContinuations.cs | 4 +- .../CollisionDetection/NonconvexReduction.cs | 2 +- BepuPhysics/CollisionDetection/RayBatchers.cs | 2 +- .../CollisionDetection/SweepTaskRegistry.cs | 16 +++--- .../CompoundHomogeneousCompoundSweepTask.cs | 6 +-- .../CompoundPairSweepOverlapFinder.cs | 8 +-- .../SweepTasks/CompoundPairSweepTask.cs | 6 +-- .../ConvexCompoundSweepOverlapFinder.cs | 8 +-- .../SweepTasks/ConvexCompoundSweepTask.cs | 6 +-- .../ConvexHomogeneousCompoundSweepTask.cs | 6 +-- .../SweepTasks/ConvexSweepTaskCommon.cs | 38 +++++++------- BepuPhysics/Constraints/AreaConstraint.cs | 2 +- BepuPhysics/Constraints/ConstraintChecker.cs | 4 +- BepuPhysics/Constraints/DistanceLimit.cs | 2 +- BepuPhysics/Constraints/DistanceServo.cs | 4 +- BepuPhysics/Constraints/VolumeConstraint.cs | 2 +- BepuPhysics/Helpers.cs | 2 +- BepuPhysics/PoseIntegrator.cs | 6 +-- BepuPhysics/Simulation_Queries.cs | 10 ++-- BepuPhysics/Trees/RayBatcher.cs | 4 +- BepuPhysics/Trees/Tree_RayCast.cs | 4 +- BepuPhysics/Trees/Tree_Sweep.cs | 10 ++-- BepuPhysics/Trees/Tree_VolumeQuery.cs | 4 +- BepuUtilities/AffineTransform.cs | 10 ++-- BepuUtilities/BoundingBox.cs | 4 +- BepuUtilities/Matrix.cs | 22 ++++---- BepuUtilities/Matrix3x3.cs | 16 +++--- BepuUtilities/QuaternionEx.cs | 52 +++++++++---------- BepuUtilities/QuaternionWide.cs | 6 +-- BepuUtilities/Symmetric3x3.cs | 2 +- BepuUtilities/Vector3Wide.cs | 6 +-- DemoContentLoader/MeshIO.cs | 2 +- .../Constraints/BoundingBoxLineExtractor.cs | 4 +- .../Constraints/ContactLineExtractor.cs | 4 +- DemoRenderer/Constraints/LineRenderer.cs | 4 +- DemoRenderer/Helpers.cs | 4 +- DemoRenderer/ShapeDrawing/ShapesExtractor.cs | 2 +- DemoRenderer/UI/UILineBatcher.cs | 2 +- DemoRenderer/UI/UILineRenderer.cs | 2 +- DemoTests/InertiaTensorTests.cs | 2 +- Demos/DemoMeshHelper.cs | 4 +- Demos/Demos/Cars/SimpleCar.cs | 8 +-- Demos/Demos/CollisionQueryDemo.cs | 2 +- Demos/Demos/ContactEventsDemo.cs | 4 +- Demos/Demos/CustomVoxelCollidableDemo.cs | 6 +-- Demos/Demos/NewtDemo.cs | 2 +- Demos/Demos/RagdollDemo.cs | 2 +- Demos/Demos/RayCastingDemo.cs | 2 +- Demos/Demos/RopeStabilityDemo.cs | 4 +- Demos/Demos/Sponsors/SponsorCharacterAI.cs | 2 +- Demos/Demos/SweepDemo.cs | 8 +-- Demos/Demos/Tanks/Tank.cs | 6 +-- Demos/Demos/Tanks/TankController.cs | 2 +- Demos/Grabber.cs | 6 +-- Demos/RolloverInfo.cs | 2 +- Demos/SpecializedTests/CompoundBoundTests.cs | 4 +- Demos/SpecializedTests/CylinderTestDemo.cs | 2 +- .../Media/2.0/BedsheetDemo.cs | 2 +- .../2.0/NewtDemandingSacrificeVideoDemo.cs | 2 +- Demos/SpecializedTests/MinkowskiVisualizer.cs | 6 +-- Demos/SpecializedTests/TriangleRayTestDemo.cs | 2 +- 83 files changed, 261 insertions(+), 261 deletions(-) diff --git a/BepuPhysics/BodyProperties.cs b/BepuPhysics/BodyProperties.cs index 832ebf398..d5bf4072e 100644 --- a/BepuPhysics/BodyProperties.cs +++ b/BepuPhysics/BodyProperties.cs @@ -138,7 +138,7 @@ public static implicit operator RigidPose((Vector3 position, Quaternion orientat /// Pose to transform the vector with. /// Transformed vector. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Transform(in Vector3 v, in RigidPose pose, out Vector3 result) + public static void Transform(Vector3 v, in RigidPose pose, out Vector3 result) { QuaternionEx.TransformWithoutOverlap(v, pose.Orientation, out var rotated); result = rotated + pose.Position; @@ -150,7 +150,7 @@ public static void Transform(in Vector3 v, in RigidPose pose, out Vector3 result /// Pose to invert and transform the vector with. /// Transformed vector. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void TransformByInverse(in Vector3 v, in RigidPose pose, out Vector3 result) + public static void TransformByInverse(Vector3 v, in RigidPose pose, out Vector3 result) { var translated = v - pose.Position; QuaternionEx.Conjugate(pose.Orientation, out var conjugate); diff --git a/BepuPhysics/BodyReference.cs b/BepuPhysics/BodyReference.cs index f782f2405..cdd484cc9 100644 --- a/BepuPhysics/BodyReference.cs +++ b/BepuPhysics/BodyReference.cs @@ -352,7 +352,7 @@ public void UpdateBounds() /// Impulse to apply to the body. /// World space offset from the center of the body to apply the impulse at. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ApplyImpulse(in Vector3 impulse, in Vector3 impulseOffset, ref BodyInertia localInertia, ref RigidPose pose, ref BodyVelocity velocity) + public static void ApplyImpulse(Vector3 impulse, Vector3 impulseOffset, ref BodyInertia localInertia, ref RigidPose pose, ref BodyVelocity velocity) { PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, pose.Orientation, out var inverseInertiaTensor); ApplyLinearImpulse(impulse, localInertia.InverseMass, ref velocity.Linear); @@ -367,7 +367,7 @@ public static void ApplyImpulse(in Vector3 impulse, in Vector3 impulseOffset, re /// Impulse to apply to the body. /// World space offset from the center of the body to apply the impulse at. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ApplyImpulse(in BodySet set, int index, in Vector3 impulse, in Vector3 impulseOffset) + public static void ApplyImpulse(in BodySet set, int index, Vector3 impulse, Vector3 impulseOffset) { ref var state = ref set.DynamicsState[index]; ApplyImpulse(impulse, impulseOffset, ref state.Inertia.Local, ref state.Motion.Pose, ref state.Motion.Velocity); @@ -380,7 +380,7 @@ public static void ApplyImpulse(in BodySet set, int index, in Vector3 impulse, i /// Inverse inertia tensor to transform the impulse with. /// Angular velocity to be modified. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ApplyAngularImpulse(in Vector3 angularImpulse, in Symmetric3x3 inverseInertiaTensor, ref Vector3 angularVelocity) + public static void ApplyAngularImpulse(Vector3 angularImpulse, in Symmetric3x3 inverseInertiaTensor, ref Vector3 angularVelocity) { Symmetric3x3.TransformWithoutOverlap(angularImpulse, inverseInertiaTensor, out var angularVelocityChange); angularVelocity += angularVelocityChange; @@ -393,7 +393,7 @@ public static void ApplyAngularImpulse(in Vector3 angularImpulse, in Symmetric3x /// Inverse mass to transform the impulse with. /// Linear velocity to be modified. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ApplyLinearImpulse(in Vector3 impulse, float inverseMass, ref Vector3 linearVelocity) + public static void ApplyLinearImpulse(Vector3 impulse, float inverseMass, ref Vector3 linearVelocity) { linearVelocity += impulse * inverseMass; } @@ -405,7 +405,7 @@ public static void ApplyLinearImpulse(in Vector3 impulse, float inverseMass, ref /// Impulse to apply to the body. /// World space offset to apply the impulse at. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void ApplyImpulse(in Vector3 impulse, in Vector3 impulseOffset) + public void ApplyImpulse(Vector3 impulse, Vector3 impulseOffset) { ref var location = ref MemoryLocation; ApplyImpulse(Bodies.Sets[location.SetIndex], location.Index, impulse, impulseOffset); @@ -416,7 +416,7 @@ public void ApplyImpulse(in Vector3 impulse, in Vector3 impulseOffset) /// /// Impulse to apply to the velocity. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void ApplyLinearImpulse(in Vector3 impulse) + public void ApplyLinearImpulse(Vector3 impulse) { ref var location = ref MemoryLocation; ref var set = ref Bodies.Sets[location.SetIndex]; @@ -430,7 +430,7 @@ public void ApplyLinearImpulse(in Vector3 impulse) /// Offset from the body's center to /// Effective velocity of the point if it were attached to the body. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void GetVelocityForOffset(in Vector3 offset, out Vector3 velocity) + public void GetVelocityForOffset(Vector3 offset, out Vector3 velocity) { velocity = Velocity.Linear + Vector3.Cross(Velocity.Angular, offset); } @@ -440,7 +440,7 @@ public void GetVelocityForOffset(in Vector3 offset, out Vector3 velocity) /// /// Impulse to apply to the velocity. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void ApplyAngularImpulse(in Vector3 angularImpulse) + public void ApplyAngularImpulse(Vector3 angularImpulse) { ref var location = ref MemoryLocation; ref var set = ref Bodies.Sets[location.SetIndex]; diff --git a/BepuPhysics/BoundingBoxHelpers.cs b/BepuPhysics/BoundingBoxHelpers.cs index 29c599db7..0f9473b56 100644 --- a/BepuPhysics/BoundingBoxHelpers.cs +++ b/BepuPhysics/BoundingBoxHelpers.cs @@ -151,7 +151,7 @@ public static void GetBoundsExpansion(Vector3 linearVelocity, float dt, float an } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void GetBoundsExpansion(in Vector3 linearVelocity, in Vector3 angularVelocity, float dt, + public static void GetBoundsExpansion(Vector3 linearVelocity, Vector3 angularVelocity, float dt, float maximumRadius, float maximumAngularExpansion, float maximumAllowedExpansion, out Vector3 minExpansion, out Vector3 maxExpansion) { var linearDisplacement = linearVelocity * dt; @@ -166,7 +166,7 @@ public static void GetBoundsExpansion(in Vector3 linearVelocity, in Vector3 angu } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ExpandBoundingBox(ref Vector3 min, ref Vector3 max, in Vector3 linearVelocity, in Vector3 angularVelocity, float dt, + public static void ExpandBoundingBox(ref Vector3 min, ref Vector3 max, Vector3 linearVelocity, Vector3 angularVelocity, float dt, float maximumRadius, float maximumAngularExpansion, float maximumAllowedExpansion) { GetBoundsExpansion(linearVelocity, angularVelocity, dt, maximumRadius, maximumAngularExpansion, maximumAllowedExpansion, out var minExpansion, out var maxExpansion); @@ -220,7 +220,7 @@ public static unsafe void ExpandLocalBoundingBoxes(ref Vector3Wide min, ref Vect } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe static void ExpandBoundingBox(in Vector3 expansion, ref Vector3 min, ref Vector3 max) + public unsafe static void ExpandBoundingBox(Vector3 expansion, ref Vector3 min, ref Vector3 max) { var minExpansion = Vector3.Min(default, expansion); var maxExpansion = Vector3.Max(default, expansion); @@ -231,8 +231,8 @@ public unsafe static void ExpandBoundingBox(in Vector3 expansion, ref Vector3 mi /// /// Computes the bounding box of a child shape A in the local space of some other collidable B with a sweep direction representing the net linear motion. /// - public static unsafe void GetLocalBoundingBoxForSweep(TypedIndex shapeIndex, Shapes shapes, in RigidPose shapePoseLocalToA, in Quaternion orientationA, in BodyVelocity velocityA, - in Vector3 offsetB, in Quaternion orientationB, in BodyVelocity velocityB, float dt, out Vector3 sweep, out Vector3 min, out Vector3 max) + public static unsafe void GetLocalBoundingBoxForSweep(TypedIndex shapeIndex, Shapes shapes, in RigidPose shapePoseLocalToA, Quaternion orientationA, in BodyVelocity velocityA, + Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float dt, out Vector3 sweep, out Vector3 min, out Vector3 max) { //TODO: For any significant amount of B angular velocity, the resulting bounding boxes can be enormous in local space. //You should strongly consider heuristically choosing a world space path. For tree-based compounds, this would require a dedicated slow world space traversal. @@ -261,8 +261,8 @@ public static unsafe void GetLocalBoundingBoxForSweep(TypedIndex shapeIndex, Sha /// /// Computes the bounding box of shape A in the local space of some other collidable B with a sweep direction representing the net linear motion. /// - public static unsafe void GetLocalBoundingBoxForSweep(ref TConvex shape, in Quaternion orientationA, in BodyVelocity velocityA, - in Vector3 offsetB, in Quaternion orientationB, in BodyVelocity velocityB, float dt, out Vector3 sweep, out Vector3 min, out Vector3 max) where TConvex : struct, IConvexShape + public static unsafe void GetLocalBoundingBoxForSweep(ref TConvex shape, Quaternion orientationA, in BodyVelocity velocityA, + Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float dt, out Vector3 sweep, out Vector3 min, out Vector3 max) where TConvex : struct, IConvexShape { //TODO: For any significant amount of B angular velocity, the resulting bounding boxes can be enormous in local space. //You should strongly consider heuristically choosing a world space path. For tree-based compounds, this would require a dedicated slow world space traversal. diff --git a/BepuPhysics/Collidables/BigCompound.cs b/BepuPhysics/Collidables/BigCompound.cs index d3fd823e2..c0050b852 100644 --- a/BepuPhysics/Collidables/BigCompound.cs +++ b/BepuPhysics/Collidables/BigCompound.cs @@ -97,7 +97,7 @@ public unsafe void TestLeaf(int leafIndex, RayData* rayData, float* maximumT) CompoundChildShapeTester tester; tester.T = -1; tester.Normal = default; - Shapes[child.ShapeIndex.Type].RayTest(child.ShapeIndex.Index, child.LocalPosition, child.LocalOrientation, *rayData, ref *maximumT, ref tester); + Shapes[child.ShapeIndex.Type].RayTest(child.ShapeIndex.Index, CompoundChild.AsPose(ref child), *rayData, ref *maximumT, ref tester); if (tester.T >= 0) { Debug.Assert(*maximumT >= tester.T, "Whatever generated this ray hit should have obeyed the current maximumT value."); @@ -233,7 +233,7 @@ public void TestLeaf(int leafIndex, ref float maximumT) Unsafe.AsRef(Overlaps).Allocate(Pool) = leafIndex; } } - public unsafe void FindLocalOverlaps(in Vector3 min, in Vector3 max, in Vector3 sweep, float maximumT, BufferPool pool, Shapes shapes, void* overlaps) + public unsafe void FindLocalOverlaps(Vector3 min, Vector3 max, Vector3 sweep, float maximumT, BufferPool pool, Shapes shapes, void* overlaps) where TOverlaps : ICollisionTaskSubpairOverlaps { SweepLeafTester enumerator; diff --git a/BepuPhysics/Collidables/Box.cs b/BepuPhysics/Collidables/Box.cs index c8db148af..a7f989b90 100644 --- a/BepuPhysics/Collidables/Box.cs +++ b/BepuPhysics/Collidables/Box.cs @@ -52,7 +52,7 @@ public Box(float width, float height, float length) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public readonly void ComputeBounds(in Quaternion orientation, out Vector3 min, out Vector3 max) + public readonly void ComputeBounds(Quaternion orientation, out Vector3 min, out Vector3 max) { Matrix3x3.CreateFromQuaternion(orientation, out var basis); var x = HalfWidth * basis.X; @@ -69,7 +69,7 @@ public readonly void ComputeAngularExpansionData(out float maximumRadius, out fl maximumAngularExpansion = maximumRadius - Vector4.Min(new Vector4(HalfLength), Vector4.Min(new Vector4(HalfHeight), new Vector4(HalfLength))).X; } - public readonly bool RayTest(in RigidPose pose, in Vector3 origin, in Vector3 direction, out float t, out Vector3 normal) + public readonly bool RayTest(in RigidPose pose, Vector3 origin, Vector3 direction, out float t, out Vector3 normal) { var offset = origin - pose.Position; Matrix3x3.CreateFromQuaternion(pose.Orientation, out var orientation); diff --git a/BepuPhysics/Collidables/Capsule.cs b/BepuPhysics/Collidables/Capsule.cs index 254ac3cd4..a134b7252 100644 --- a/BepuPhysics/Collidables/Capsule.cs +++ b/BepuPhysics/Collidables/Capsule.cs @@ -47,7 +47,7 @@ public readonly void ComputeAngularExpansionData(out float maximumRadius, out fl } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public readonly void ComputeBounds(in Quaternion orientation, out Vector3 min, out Vector3 max) + public readonly void ComputeBounds(Quaternion orientation, out Vector3 min, out Vector3 max) { QuaternionEx.TransformUnitY(orientation, out var segmentOffset); max = Vector3.Abs(HalfLength * segmentOffset) + new Vector3(Radius); @@ -55,7 +55,7 @@ public readonly void ComputeBounds(in Quaternion orientation, out Vector3 min, o } - public readonly bool RayTest(in RigidPose pose, in Vector3 origin, in Vector3 direction, out float t, out Vector3 normal) + public readonly bool RayTest(in RigidPose pose, Vector3 origin, Vector3 direction, out float t, out Vector3 normal) { //It's convenient to work in local space, so pull the ray into the capsule's local space. Matrix3x3.CreateFromQuaternion(pose.Orientation, out var orientation); diff --git a/BepuPhysics/Collidables/Compound.cs b/BepuPhysics/Collidables/Compound.cs index 41b9ea20a..c65e82f64 100644 --- a/BepuPhysics/Collidables/Compound.cs +++ b/BepuPhysics/Collidables/Compound.cs @@ -51,7 +51,7 @@ public bool AllowTest(int childIndex) return true; } - public void OnRayHit(in RayData ray, ref float maximumT, float t, in Vector3 normal, int childIndex) + public void OnRayHit(in RayData ray, ref float maximumT, float t, Vector3 normal, int childIndex) { Debug.Assert(childIndex == 0, "Compounds can contain only convexes, so the child index is always zero."); T = t; @@ -160,7 +160,7 @@ public static void GetWorldPose(in RigidPose localPose, in RigidPose transform, } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ComputeChildBounds(in CompoundChild child, in Quaternion orientation, Shapes shapeBatches, out Vector3 childMin, out Vector3 childMax) + public static void ComputeChildBounds(in CompoundChild child, Quaternion orientation, Shapes shapeBatches, out Vector3 childMin, out Vector3 childMax) { GetRotatedChildPose(child.LocalPosition, child.LocalOrientation, orientation, out var childPose); Debug.Assert(!shapeBatches[child.ShapeIndex.Type].Compound, "All children of a compound must be convex."); @@ -319,7 +319,7 @@ public unsafe void FindLocalOverlaps(ref Buffer(in Vector3 min, in Vector3 max, in Vector3 sweep, float maximumT, BufferPool pool, Shapes shapes, void* overlapsPointer) + public unsafe void FindLocalOverlaps(Vector3 min, Vector3 max, Vector3 sweep, float maximumT, BufferPool pool, Shapes shapes, void* overlapsPointer) where TOverlaps : ICollisionTaskSubpairOverlaps { Tree.ConvertBoxToCentroidWithExtent(min, max, out var sweepOrigin, out var expansion); diff --git a/BepuPhysics/Collidables/ConvexHull.cs b/BepuPhysics/Collidables/ConvexHull.cs index 9be1d9e94..0e779a127 100644 --- a/BepuPhysics/Collidables/ConvexHull.cs +++ b/BepuPhysics/Collidables/ConvexHull.cs @@ -137,7 +137,7 @@ internal readonly void ComputeBounds(in QuaternionWide orientationWide, out Vect } } - public readonly void ComputeBounds(in Quaternion orientation, out Vector3 min, out Vector3 max) + public readonly void ComputeBounds(Quaternion orientation, out Vector3 min, out Vector3 max) { QuaternionWide.Broadcast(orientation, out var orientationWide); ComputeBounds(orientationWide, out min, out max); @@ -198,7 +198,7 @@ public readonly ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity return new ConvexHullShapeBatch(pool, initialCapacity); } - public readonly bool RayTest(in RigidPose pose, in Vector3 origin, in Vector3 direction, out float t, out Vector3 normal) + public readonly bool RayTest(in RigidPose pose, Vector3 origin, Vector3 direction, out float t, out Vector3 normal) { Matrix3x3.CreateFromQuaternion(pose.Orientation, out var orientation); var shapeToRay = origin - pose.Position; diff --git a/BepuPhysics/Collidables/ConvexHullHelper.cs b/BepuPhysics/Collidables/ConvexHullHelper.cs index 759a4f606..b32b937b9 100644 --- a/BepuPhysics/Collidables/ConvexHullHelper.cs +++ b/BepuPhysics/Collidables/ConvexHullHelper.cs @@ -240,7 +240,7 @@ static int FindNextIndexForFaceHull(Vector2 start, Vector2 previousEdgeDirection return bestIndex; } - static void ReduceFace(ref QuickList faceVertexIndices, in Vector3 faceNormal, Span points, float planeEpsilon, ref QuickList facePoints, ref Buffer allowVertex, ref QuickList reducedIndices) + static void ReduceFace(ref QuickList faceVertexIndices, Vector3 faceNormal, Span points, float planeEpsilon, ref QuickList facePoints, ref Buffer allowVertex, ref QuickList reducedIndices) { Debug.Assert(facePoints.Count == 0 && reducedIndices.Count == 0 && facePoints.Span.Length >= faceVertexIndices.Count && reducedIndices.Span.Length >= faceVertexIndices.Count); for (int i = faceVertexIndices.Count - 1; i >= 0; --i) @@ -397,7 +397,7 @@ struct EdgeToTest // public Vector3 BasisX; // public Vector3 BasisY; - // public DebugStep(EdgeEndpoints sourceEdge, ref QuickList raw, in Vector3 faceNormal, in Vector3 basisX, in Vector3 basisY) + // public DebugStep(EdgeEndpoints sourceEdge, ref QuickList raw, Vector3 faceNormal, Vector3 basisX, Vector3 basisY) // { // SourceEdge = sourceEdge; // FaceNormal = faceNormal; diff --git a/BepuPhysics/Collidables/Cylinder.cs b/BepuPhysics/Collidables/Cylinder.cs index 2ceb1b72b..9b357fa59 100644 --- a/BepuPhysics/Collidables/Cylinder.cs +++ b/BepuPhysics/Collidables/Cylinder.cs @@ -45,7 +45,7 @@ public readonly void ComputeAngularExpansionData(out float maximumRadius, out fl } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public readonly void ComputeBounds(in Quaternion orientation, out Vector3 min, out Vector3 max) + public readonly void ComputeBounds(Quaternion orientation, out Vector3 min, out Vector3 max) { //The bounding box is composed of the contribution from the axis line segment and the disc cap. //The bounding box of the disc cap can be found by sampling the extreme point in each of the three directions: @@ -73,7 +73,7 @@ public readonly void ComputeBounds(in Quaternion orientation, out Vector3 min, o } - public readonly bool RayTest(in RigidPose pose, in Vector3 origin, in Vector3 direction, out float t, out Vector3 normal) + public readonly bool RayTest(in RigidPose pose, Vector3 origin, Vector3 direction, out float t, out Vector3 normal) { //It's convenient to work in local space, so pull the ray into the cylinder's local space. Matrix3x3.CreateFromQuaternion(pose.Orientation, out var orientation); diff --git a/BepuPhysics/Collidables/IShape.cs b/BepuPhysics/Collidables/IShape.cs index 6901929d7..b86b28c24 100644 --- a/BepuPhysics/Collidables/IShape.cs +++ b/BepuPhysics/Collidables/IShape.cs @@ -50,7 +50,7 @@ public interface IConvexShape : IShape /// Orientation of the shape to use when computing the bounding box. /// Minimum corner of the bounding box. /// Maximum corner of the bounding box. - void ComputeBounds(in Quaternion orientation, out Vector3 min, out Vector3 max); + void ComputeBounds(Quaternion orientation, out Vector3 min, out Vector3 max); /// /// Computes information about how the bounding box should be expanded in response to angular velocity. @@ -162,7 +162,7 @@ public interface IHomogeneousCompoundShape : IShap /// Orientation of the compound. /// Minimum of the compound's bounding box. /// Maximum of the compound's bounding box. - void ComputeBounds(in Quaternion orientation, out Vector3 min, out Vector3 max); + void ComputeBounds(Quaternion orientation, out Vector3 min, out Vector3 max); /// /// Tests a ray against the shape. diff --git a/BepuPhysics/Collidables/Mesh.cs b/BepuPhysics/Collidables/Mesh.cs index fd0ef81da..20deb5f93 100644 --- a/BepuPhysics/Collidables/Mesh.cs +++ b/BepuPhysics/Collidables/Mesh.cs @@ -167,7 +167,7 @@ public readonly unsafe void GetLocalChild(int triangleIndex, ref TriangleWide ta Vector3Wide.WriteFirst(source.C * scale, ref target.C); } - public readonly void ComputeBounds(in Quaternion orientation, out Vector3 min, out Vector3 max) + public readonly void ComputeBounds(Quaternion orientation, out Vector3 min, out Vector3 max) { Matrix3x3.CreateFromQuaternion(orientation, out var r); min = new Vector3(float.MaxValue); @@ -294,7 +294,7 @@ public readonly unsafe void FindLocalOverlaps(ref B } } - public readonly unsafe void FindLocalOverlaps(in Vector3 min, in Vector3 max, in Vector3 sweep, float maximumT, BufferPool pool, Shapes shapes, void* overlaps) + public readonly unsafe void FindLocalOverlaps(Vector3 min, Vector3 max, Vector3 sweep, float maximumT, BufferPool pool, Shapes shapes, void* overlaps) where TOverlaps : ICollisionTaskSubpairOverlaps { var scaledMin = min * inverseScale; diff --git a/BepuPhysics/Collidables/MeshInertiaHelper.cs b/BepuPhysics/Collidables/MeshInertiaHelper.cs index 9f1a3e814..0a8a17112 100644 --- a/BepuPhysics/Collidables/MeshInertiaHelper.cs +++ b/BepuPhysics/Collidables/MeshInertiaHelper.cs @@ -27,7 +27,7 @@ public interface ITriangleSource /// public static class MeshInertiaHelper { - public static void ComputeTetrahedronContribution(in Vector3 a, in Vector3 b, in Vector3 c, float mass, out Symmetric3x3 inertiaTensor) + public static void ComputeTetrahedronContribution(Vector3 a, Vector3 b, Vector3 c, float mass, out Symmetric3x3 inertiaTensor) { //Computing the inertia of a tetrahedron requires integrating across its volume. //While it's possible to do so directly given arbitrary plane equations, it's more convenient to integrate over a normalized tetrahedron with coordinates @@ -109,7 +109,7 @@ public static void ComputeTetrahedronContribution(in Vector3 a, in Vector3 b, in /// Third vertex of the tetrahedron. /// Volume of the tetrahedron. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static float ComputeTetrahedronVolume(in Vector3 a, in Vector3 b, in Vector3 c) + public static float ComputeTetrahedronVolume(Vector3 a, Vector3 b, Vector3 c) { return (1f / 6f) * Vector3.Dot(Vector3.Cross(b, a), c); } @@ -122,7 +122,7 @@ public static float ComputeTetrahedronVolume(in Vector3 a, in Vector3 b, in Vect /// Third vertex of the tetrahedron. /// Volume of the tetrahedron. /// Inertia tensor of this tetrahedron assuming a density of 1. - public static void ComputeTetrahedronContribution(in Vector3 a, in Vector3 b, in Vector3 c, out float volume, out Symmetric3x3 inertiaTensor) + public static void ComputeTetrahedronContribution(Vector3 a, Vector3 b, Vector3 c, out float volume, out Symmetric3x3 inertiaTensor) { volume = ComputeTetrahedronVolume(a, b, c); ComputeTetrahedronContribution(a, b, c, volume, out inertiaTensor); @@ -205,7 +205,7 @@ public static void ComputeClosedCenterOfMass(ref TTriangleSourc /// Third vertex in the triangle. /// Mass of the triangle. /// Inertia tensor of the triangle. - public static void ComputeTriangleContribution(in Vector3 a, in Vector3 b, in Vector3 c, float mass, out Symmetric3x3 inertiaTensor) + public static void ComputeTriangleContribution(Vector3 a, Vector3 b, Vector3 c, float mass, out Symmetric3x3 inertiaTensor) { //This follows the same logic as the tetrahedral inertia tensor calculation, but the transform is different. //There are only two dimensions of interest, but if we wanted to express it as a 3x3 linear transform: @@ -254,7 +254,7 @@ public static void ComputeTriangleContribution(in Vector3 a, in Vector3 b, in Ve /// Second vertex in the triangle. /// Third vertex in the triangle. /// Area of the triangle. - public static float ComputeTriangleArea(in Vector3 a, in Vector3 b, in Vector3 c) + public static float ComputeTriangleArea(Vector3 a, Vector3 b, Vector3 c) { return 0.5f * Vector3.Cross(b - a, c - a).Length(); //Not exactly fast, but again, we're assuming performance is irrelevant for the mesh inertia helper. } @@ -267,7 +267,7 @@ public static float ComputeTriangleArea(in Vector3 a, in Vector3 b, in Vector3 c /// Third vertex in the triangle. /// Area of the triangle. /// Inertia tensor of the triangle assuming that the density is 1. - public static void ComputeTriangleContribution(in Vector3 a, in Vector3 b, in Vector3 c, out float area, out Symmetric3x3 inertiaTensor) + public static void ComputeTriangleContribution(Vector3 a, Vector3 b, Vector3 c, out float area, out Symmetric3x3 inertiaTensor) { area = ComputeTriangleArea(a, b, c); ComputeTriangleContribution(a, b, c, area, out inertiaTensor); @@ -344,7 +344,7 @@ public static Vector3 ComputeOpenCenterOfMass(ref TTriangleSour /// Mass associated with the inertia tensor being moved. /// Offset from the current inertia frame of reference to the new frame of reference. /// Modification to add to the inertia tensor to move it into the new reference frame. - public static void GetInertiaOffset(float mass, in Vector3 offset, out Symmetric3x3 inertiaOffset) + public static void GetInertiaOffset(float mass, Vector3 offset, out Symmetric3x3 inertiaOffset) { //Just the parallel axis theorem. var squared = offset * offset; diff --git a/BepuPhysics/Collidables/Shapes.cs b/BepuPhysics/Collidables/Shapes.cs index da1751235..efbdb6982 100644 --- a/BepuPhysics/Collidables/Shapes.cs +++ b/BepuPhysics/Collidables/Shapes.cs @@ -69,7 +69,7 @@ public void ComputeBounds(int shapeIndex, RigidPose pose, out Vector3 min, out V min += pose.Position; max += pose.Position; } - internal virtual void ComputeBounds(int shapeIndex, in Quaternion orientation, out float maximumRadius, out float maximumAngularExpansion, out Vector3 min, out Vector3 max) + internal virtual void ComputeBounds(int shapeIndex, Quaternion orientation, out float maximumRadius, out float maximumAngularExpansion, out Vector3 min, out Vector3 max) { throw new InvalidOperationException("Nonconvex shapes are not required to have a maximum radius or angular expansion implementation. This should only ever be called on convexes."); } @@ -260,7 +260,7 @@ public override void ComputeBounds(int shapeIndex, Quaternion orientation, out V shapes[shapeIndex].ComputeBounds(orientation, out min, out max); } - internal override void ComputeBounds(int shapeIndex, in Quaternion orientation, out float maximumRadius, out float angularExpansion, out Vector3 min, out Vector3 max) + internal override void ComputeBounds(int shapeIndex, Quaternion orientation, out float maximumRadius, out float angularExpansion, out Vector3 min, out Vector3 max) { ref var shape = ref shapes[shapeIndex]; shape.ComputeBounds(orientation, out min, out max); diff --git a/BepuPhysics/Collidables/Sphere.cs b/BepuPhysics/Collidables/Sphere.cs index b1217db3f..21f39c092 100644 --- a/BepuPhysics/Collidables/Sphere.cs +++ b/BepuPhysics/Collidables/Sphere.cs @@ -51,12 +51,12 @@ public readonly void ComputeAngularExpansionData(out float maximumRadius, out fl } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public readonly void ComputeBounds(in Quaternion orientation, out Vector3 min, out Vector3 max) + public readonly void ComputeBounds(Quaternion orientation, out Vector3 min, out Vector3 max) { min = new Vector3(-Radius); max = new Vector3(Radius); } - public readonly bool RayTest(in RigidPose pose, in Vector3 origin, in Vector3 direction, out float t, out Vector3 normal) + public readonly bool RayTest(in RigidPose pose, Vector3 origin, Vector3 direction, out float t, out Vector3 normal) { //Normalize the direction. Sqrts aren't *that* bad, and it both simplifies things and helps avoid numerical problems. var inverseDLength = 1f / direction.Length(); diff --git a/BepuPhysics/Collidables/Triangle.cs b/BepuPhysics/Collidables/Triangle.cs index 7e89c40c6..998b37873 100644 --- a/BepuPhysics/Collidables/Triangle.cs +++ b/BepuPhysics/Collidables/Triangle.cs @@ -31,7 +31,7 @@ public struct Triangle : IConvexShape /// First vertex of the triangle in local space. /// Second vertex of the triangle in local space. /// Third vertex of the triangle in local space. - public Triangle(in Vector3 a, in Vector3 b, in Vector3 c) + public Triangle(Vector3 a, Vector3 b, Vector3 c) { A = a; B = b; @@ -39,7 +39,7 @@ public Triangle(in Vector3 a, in Vector3 b, in Vector3 c) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public readonly void ComputeBounds(in Quaternion orientation, out Vector3 min, out Vector3 max) + public readonly void ComputeBounds(Quaternion orientation, out Vector3 min, out Vector3 max) { Matrix3x3.CreateFromQuaternion(orientation, out var basis); Matrix3x3.Transform(A, basis, out var worldA); @@ -57,7 +57,7 @@ public readonly void ComputeAngularExpansionData(out float maximumRadius, out fl } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static bool RayTest(in Vector3 a, in Vector3 b, in Vector3 c, in Vector3 origin, in Vector3 direction, out float t, out Vector3 normal) + public static bool RayTest(Vector3 a, Vector3 b, Vector3 c, Vector3 origin, Vector3 direction, out float t, out Vector3 normal) { //Note that this assumes clockwise-in-right-hand winding. Rays coming from the opposite direction pass through; triangles are one sided. var ab = b - a; @@ -95,7 +95,7 @@ public static bool RayTest(in Vector3 a, in Vector3 b, in Vector3 c, in Vector3 return true; } - public readonly bool RayTest(in RigidPose pose, in Vector3 origin, in Vector3 direction, out float t, out Vector3 normal) + public readonly bool RayTest(in RigidPose pose, Vector3 origin, Vector3 direction, out float t, out Vector3 normal) { var offset = origin - pose.Position; Matrix3x3.CreateFromQuaternion(pose.Orientation, out var orientation); diff --git a/BepuPhysics/CollisionDetection/BroadPhase_Queries.cs b/BepuPhysics/CollisionDetection/BroadPhase_Queries.cs index 078980ac0..f772751e7 100644 --- a/BepuPhysics/CollisionDetection/BroadPhase_Queries.cs +++ b/BepuPhysics/CollisionDetection/BroadPhase_Queries.cs @@ -41,7 +41,7 @@ public unsafe void TestLeaf(int leafIndex, RayData* rayData, float* maximumT) /// Maximum length of the ray traversal in units of the direction's length. /// Callback to execute on ray-leaf bounding box intersections. /// User specified id of the ray. - public unsafe void RayCast(in Vector3 origin, in Vector3 direction, float maximumT, ref TRayTester rayTester, int id = 0) where TRayTester : IBroadPhaseRayTester + public unsafe void RayCast(Vector3 origin, Vector3 direction, float maximumT, ref TRayTester rayTester, int id = 0) where TRayTester : IBroadPhaseRayTester { TreeRay.CreateFrom(origin, direction, maximumT, id, out var rayData, out var treeRay); RayLeafTester tester; @@ -76,7 +76,7 @@ public unsafe void TestLeaf(int leafIndex, ref float maximumT) /// Direction along which to sweep the bounding box. /// Maximum length of the sweep in units of the direction's length. /// Callback to execute on sweep-leaf bounding box intersections. - public unsafe void Sweep(in Vector3 min, in Vector3 max, in Vector3 direction, float maximumT, ref TSweepTester sweepTester) where TSweepTester : IBroadPhaseSweepTester + public unsafe void Sweep(Vector3 min, Vector3 max, Vector3 direction, float maximumT, ref TSweepTester sweepTester) where TSweepTester : IBroadPhaseSweepTester { Tree.ConvertBoxToCentroidWithExtent(min, max, out var origin, out var expansion); TreeRay.CreateFrom(origin, direction, maximumT, out var treeRay); @@ -98,7 +98,7 @@ public unsafe void Sweep(in Vector3 min, in Vector3 max, in Vector /// Direction along which to sweep the bounding box. /// Maximum length of the sweep in units of the direction's length. /// Callback to execute on sweep-leaf bounding box intersections. - public unsafe void Sweep(in BoundingBox boundingBox, in Vector3 direction, float maximumT, ref TSweepTester sweepTester) where TSweepTester : IBroadPhaseSweepTester + public unsafe void Sweep(in BoundingBox boundingBox, Vector3 direction, float maximumT, ref TSweepTester sweepTester) where TSweepTester : IBroadPhaseSweepTester { Sweep(boundingBox.Min, boundingBox.Max, direction, maximumT, ref sweepTester); } @@ -122,7 +122,7 @@ public bool LoopBody(int i) /// Minimum bounds of the query box. /// Maximum bounds of the query box. /// Enumerator to call for overlaps. - public unsafe void GetOverlaps(in Vector3 min, in Vector3 max, ref TOverlapEnumerator overlapEnumerator) where TOverlapEnumerator : IBreakableForEach + public unsafe void GetOverlaps(Vector3 min, Vector3 max, ref TOverlapEnumerator overlapEnumerator) where TOverlapEnumerator : IBreakableForEach { BoxQueryEnumerator enumerator; enumerator.Enumerator = overlapEnumerator; diff --git a/BepuPhysics/CollisionDetection/CollisionBatcher.cs b/BepuPhysics/CollisionDetection/CollisionBatcher.cs index eb9793455..ce7be21c4 100644 --- a/BepuPhysics/CollisionDetection/CollisionBatcher.cs +++ b/BepuPhysics/CollisionDetection/CollisionBatcher.cs @@ -86,7 +86,7 @@ unsafe ref TPair AllocatePair(ref CollisionBatch batch, ref CollisionTask } private unsafe void Add(ref CollisionTaskReference reference, int flipMask, int shapeTypeA, int shapeTypeB, void* shapeA, void* shapeB, - in Vector3 offsetB, in Quaternion orientationA, in Quaternion orientationB, in BodyVelocity velocityA, in BodyVelocity velocityB, float speculativeMargin, float maximumExpansion, + Vector3 offsetB, Quaternion orientationA, Quaternion orientationB, in BodyVelocity velocityA, in BodyVelocity velocityB, float speculativeMargin, float maximumExpansion, in PairContinuation continuation) { ref var batch = ref batches[reference.TaskIndex]; @@ -173,7 +173,7 @@ private unsafe void Add(ref CollisionTaskReference reference, int flipMask, int [MethodImpl(MethodImplOptions.AggressiveInlining)] unsafe void AddDirectly( ref CollisionTaskReference reference, int shapeTypeA, int shapeTypeB, void* shapeA, void* shapeB, - in Vector3 offsetB, in Quaternion orientationA, in Quaternion orientationB, in BodyVelocity velocityA, in BodyVelocity velocityB, float speculativeMargin, float maximumExpansion, + Vector3 offsetB, Quaternion orientationA, Quaternion orientationB, in BodyVelocity velocityA, in BodyVelocity velocityB, float speculativeMargin, float maximumExpansion, in PairContinuation pairContinuation) { if (reference.TaskIndex < 0) @@ -197,7 +197,7 @@ unsafe void AddDirectly( [MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe void AddDirectly( int shapeTypeA, int shapeTypeB, void* shapeA, void* shapeB, - in Vector3 offsetB, in Quaternion orientationA, in Quaternion orientationB, in BodyVelocity velocityA, in BodyVelocity velocityB, float speculativeMargin, float maximumExpansion, + Vector3 offsetB, Quaternion orientationA, Quaternion orientationB, in BodyVelocity velocityA, in BodyVelocity velocityB, float speculativeMargin, float maximumExpansion, in PairContinuation pairContinuation) { ref var reference = ref typeMatrix.GetTaskReference(shapeTypeA, shapeTypeB); @@ -206,7 +206,7 @@ public unsafe void AddDirectly( [MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe void AddDirectly(int shapeTypeA, int shapeTypeB, void* shapeA, void* shapeB, - in Vector3 offsetB, in Quaternion orientationA, in Quaternion orientationB, float speculativeMargin, in PairContinuation pairContinuation) + Vector3 offsetB, Quaternion orientationA, Quaternion orientationB, float speculativeMargin, in PairContinuation pairContinuation) { AddDirectly(shapeTypeA, shapeTypeB, shapeA, shapeB, offsetB, orientationA, orientationB, default, default, speculativeMargin, default, pairContinuation); } @@ -214,7 +214,7 @@ public unsafe void AddDirectly(int shapeTypeA, int shapeTypeB, void* shapeA, voi [MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe void Add(TypedIndex shapeIndexA, TypedIndex shapeIndexB, - in Vector3 offsetB, in Quaternion orientationA, in Quaternion orientationB, in BodyVelocity velocityA, in BodyVelocity velocityB, + Vector3 offsetB, Quaternion orientationA, Quaternion orientationB, in BodyVelocity velocityA, in BodyVelocity velocityB, float speculativeMargin, float maximumExpansion, in PairContinuation continuation) { @@ -226,7 +226,7 @@ public unsafe void Add(TypedIndex shapeIndexA, TypedIndex shapeIndexB, } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void Add(TypedIndex shapeIndexA, TypedIndex shapeIndexB, in Vector3 offsetB, in Quaternion orientationA, in Quaternion orientationB, + public unsafe void Add(TypedIndex shapeIndexA, TypedIndex shapeIndexB, Vector3 offsetB, Quaternion orientationA, Quaternion orientationB, float speculativeMargin, in PairContinuation continuation) { Add(shapeIndexA, shapeIndexB, offsetB, orientationA, orientationB, default, default, speculativeMargin, default, continuation); @@ -267,14 +267,14 @@ public unsafe void CacheShapeB(int shapeTypeA, int shapeTypeB, void* shapeDataB, [MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe void Add( - int shapeTypeA, int shapeTypeB, int shapeSizeA, int shapeSizeB, void* shapeA, void* shapeB, in Vector3 offsetB, in Quaternion orientationA, in Quaternion orientationB, float speculativeMargin, int pairId) + int shapeTypeA, int shapeTypeB, int shapeSizeA, int shapeSizeB, void* shapeA, void* shapeB, Vector3 offsetB, Quaternion orientationA, Quaternion orientationB, float speculativeMargin, int pairId) { ref var reference = ref typeMatrix.GetTaskReference(shapeTypeA, shapeTypeB); CacheShapes(ref reference, shapeA, shapeB, shapeSizeA, shapeSizeB, out var cachedShapeA, out var cachedShapeB); AddDirectly(ref reference, shapeTypeA, shapeTypeB, cachedShapeA, cachedShapeB, offsetB, orientationA, orientationB, default, default, speculativeMargin, default, new PairContinuation(pairId)); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void Add(TShapeA shapeA, TShapeB shapeB, in Vector3 offsetB, in Quaternion orientationA, in Quaternion orientationB, float speculativeMargin, int pairId) + public unsafe void Add(TShapeA shapeA, TShapeB shapeB, Vector3 offsetB, Quaternion orientationA, Quaternion orientationB, float speculativeMargin, int pairId) where TShapeA : struct, IShape where TShapeB : struct, IShape { //Note that the shapes are passed by copy to avoid a GC hole. This isn't optimal, but it does allow a single code path, and the underlying function is the one diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundOverlapFinder.cs b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundOverlapFinder.cs index 0677b00bb..f70657399 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundOverlapFinder.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundOverlapFinder.cs @@ -19,7 +19,7 @@ unsafe void FindLocalOverlaps(ref Buffer where TSubpairOverlaps : struct, ICollisionTaskSubpairOverlaps; - unsafe void FindLocalOverlaps(in Vector3 min, in Vector3 max, in Vector3 sweep, float maximumT, BufferPool pool, Shapes shapes, void* overlaps) + unsafe void FindLocalOverlaps(Vector3 min, Vector3 max, Vector3 sweep, float maximumT, BufferPool pool, Shapes shapes, void* overlaps) where TOverlaps : ICollisionTaskSubpairOverlaps; } public interface IConvexCompoundOverlapFinder diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CylinderConvexHullTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CylinderConvexHullTester.cs index 12afc4e92..06465570e 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CylinderConvexHullTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CylinderConvexHullTester.cs @@ -64,9 +64,9 @@ internal static bool IntersectLineCircle(in Vector2 linePosition, in Vector2 lin } [MethodImpl(MethodImplOptions.AggressiveInlining)] - static void InsertContact(in Vector3 slotSideEdgeCenter, in Vector3 slotCylinderEdgeAxis, float t, - in Vector3 hullFaceOrigin, in Vector3 slotHullFaceNormal, float inverseDepthDenominator, - in Matrix3x3 slotHullOrientation, in Vector3 slotOffsetB, int featureId, + static void InsertContact(Vector3 slotSideEdgeCenter, Vector3 slotCylinderEdgeAxis, float t, + Vector3 hullFaceOrigin, Vector3 slotHullFaceNormal, float inverseDepthDenominator, + in Matrix3x3 slotHullOrientation, Vector3 slotOffsetB, int featureId, ref Vector3Wide contactOffsetAWide, ref Vector contactDepthWide, ref Vector contactFeatureIdWide, ref Vector contactExistsWide) { //Create max contact. diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/ManifoldCandidateHelper.cs b/BepuPhysics/CollisionDetection/CollisionTasks/ManifoldCandidateHelper.cs index 5ddc41171..6359c5f9e 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/ManifoldCandidateHelper.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/ManifoldCandidateHelper.cs @@ -288,8 +288,8 @@ public static void ReduceWithoutComputingDepths(ref ManifoldCandidate candidates [MethodImpl(MethodImplOptions.AggressiveInlining)] unsafe static void PlaceCandidateInSlot(in ManifoldCandidateScalar candidate, int contactIndex, - in Vector3 faceCenterB, in Vector3 faceBX, in Vector3 faceBY, float depth, - in Matrix3x3 orientationB, in Vector3 offsetB, ref Convex4ContactManifoldWide manifoldSlot) + Vector3 faceCenterB, Vector3 faceBX, Vector3 faceBY, float depth, + in Matrix3x3 orientationB, Vector3 offsetB, ref Convex4ContactManifoldWide manifoldSlot) { var localPosition = candidate.X * faceBX + candidate.Y * faceBY + faceCenterB; Matrix3x3.Transform(localPosition, orientationB, out var position); @@ -314,8 +314,8 @@ static unsafe void RemoveCandidateAt(ManifoldCandidateScalar* candidates, float* } public unsafe static void Reduce(ManifoldCandidateScalar* candidates, int candidateCount, - in Vector3 faceNormalA, float inverseFaceNormalADotLocalNormal, in Vector3 faceCenterA, in Vector3 faceCenterB, in Vector3 tangentBX, in Vector3 tangentBY, - float epsilonScale, float minimumDepth, in Matrix3x3 rotationToWorld, in Vector3 worldOffsetB, int slotIndex, ref Convex4ContactManifoldWide manifoldWide) + Vector3 faceNormalA, float inverseFaceNormalADotLocalNormal, Vector3 faceCenterA, Vector3 faceCenterB, Vector3 tangentBX, Vector3 tangentBY, + float epsilonScale, float minimumDepth, in Matrix3x3 rotationToWorld, Vector3 worldOffsetB, int slotIndex, ref Convex4ContactManifoldWide manifoldWide) { if (candidateCount == 0) { diff --git a/BepuPhysics/CollisionDetection/MeshReduction.cs b/BepuPhysics/CollisionDetection/MeshReduction.cs index ee153e604..cd9105360 100644 --- a/BepuPhysics/CollisionDetection/MeshReduction.cs +++ b/BepuPhysics/CollisionDetection/MeshReduction.cs @@ -146,7 +146,7 @@ public TestTriangle(in Triangle triangle, int sourceChildIndex) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - private static unsafe bool ShouldBlockNormal(in TestTriangle triangle, in Vector3 meshSpaceContact, in Vector3 meshSpaceNormal) + private static unsafe bool ShouldBlockNormal(in TestTriangle triangle, Vector3 meshSpaceContact, Vector3 meshSpaceNormal) { //While we don't have a decent way to do truly scaling SIMD operations within the context of a single manifold vs triangle test, we can at least use 4-wide operations //to accelerate each individual contact test. diff --git a/BepuPhysics/CollisionDetection/NarrowPhaseCCDContinuations.cs b/BepuPhysics/CollisionDetection/NarrowPhaseCCDContinuations.cs index 54663ebdc..9366d360d 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhaseCCDContinuations.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhaseCCDContinuations.cs @@ -54,7 +54,7 @@ struct ContinuousPair public float T; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Initialize(ref CollidablePair pair, in Vector3 relativeLinearVelocity, in Vector3 angularVelocityA, in Vector3 angularVelocityB, float t) + public void Initialize(ref CollidablePair pair, Vector3 relativeLinearVelocity, Vector3 angularVelocityA, Vector3 angularVelocityB, float t) { Pair = pair; AngularA = angularVelocityA; @@ -120,7 +120,7 @@ public CCDContinuationIndex AddDiscrete(ref CollidablePair pair) return new CCDContinuationIndex((int)ConstraintGeneratorType.Discrete, index); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public CCDContinuationIndex AddContinuous(ref CollidablePair pair, in Vector3 relativeLinearVelocity, in Vector3 angularVelocityA, in Vector3 angularVelocityB, float t) + public CCDContinuationIndex AddContinuous(ref CollidablePair pair, Vector3 relativeLinearVelocity, Vector3 angularVelocityA, Vector3 angularVelocityB, float t) { continuous.Allocate(pool, out var index).Initialize(ref pair, relativeLinearVelocity, angularVelocityA, angularVelocityB, t); return new CCDContinuationIndex((int)ConstraintGeneratorType.Continuous, index); diff --git a/BepuPhysics/CollisionDetection/NonconvexReduction.cs b/BepuPhysics/CollisionDetection/NonconvexReduction.cs index 622729a9a..b5e677224 100644 --- a/BepuPhysics/CollisionDetection/NonconvexReduction.cs +++ b/BepuPhysics/CollisionDetection/NonconvexReduction.cs @@ -66,7 +66,7 @@ unsafe static void UseContact(ref QuickList remainingCandida } [MethodImpl(MethodImplOptions.AggressiveInlining)] - static float ComputeDistinctiveness(in ConvexContact candidate, in Vector3 contactNormal, in NonconvexContact reducedContact, float distanceSquaredInterpolationMin, float inverseDistanceSquaredInterpolationSpan, float depthScale) + static float ComputeDistinctiveness(in ConvexContact candidate, Vector3 contactNormal, in NonconvexContact reducedContact, float distanceSquaredInterpolationMin, float inverseDistanceSquaredInterpolationSpan, float depthScale) { //The more distant a contact is from another contact, or the more different its normal is, the more distinct it is considered. //The goal is for distinctiveness to range from around 0 to 2. The exact values aren't extremely important- we just want a rough range diff --git a/BepuPhysics/CollisionDetection/RayBatchers.cs b/BepuPhysics/CollisionDetection/RayBatchers.cs index ce5372831..d87ea00a3 100644 --- a/BepuPhysics/CollisionDetection/RayBatchers.cs +++ b/BepuPhysics/CollisionDetection/RayBatchers.cs @@ -129,7 +129,7 @@ public bool AllowTest(int childIndex) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void OnRayHit(in RayData ray, ref float maximumT, float t, in Vector3 normal, int childIndex) + public unsafe void OnRayHit(in RayData ray, ref float maximumT, float t, Vector3 normal, int childIndex) { HitHandler.OnRayHit(ray, ref maximumT, t, normal, Reference, childIndex); } diff --git a/BepuPhysics/CollisionDetection/SweepTaskRegistry.cs b/BepuPhysics/CollisionDetection/SweepTaskRegistry.cs index 43dba7e22..f840291ac 100644 --- a/BepuPhysics/CollisionDetection/SweepTaskRegistry.cs +++ b/BepuPhysics/CollisionDetection/SweepTaskRegistry.cs @@ -30,14 +30,14 @@ public abstract class SweepTask public int ShapeTypeIndexB { get; protected set; } protected abstract unsafe bool PreorderedTypeSweep( - void* shapeDataA, in RigidPose localPoseA, in Quaternion orientationA, in BodyVelocity velocityA, - void* shapeDataB, in RigidPose localPoseB, in Vector3 offsetB, in Quaternion orientationB, in BodyVelocity velocityB, float maximumT, + void* shapeDataA, in RigidPose localPoseA, Quaternion orientationA, in BodyVelocity velocityA, + void* shapeDataB, in RigidPose localPoseB, Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float maximumT, float minimumProgression, float convergenceThreshold, int maximumIterationCount, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal); public unsafe bool Sweep( - void* shapeDataA, int shapeTypeA, in RigidPose localPoseA, in Quaternion orientationA, in BodyVelocity velocityA, - void* shapeDataB, int shapeTypeB, in RigidPose localPoseB, in Vector3 offsetB, in Quaternion orientationB, in BodyVelocity velocityB, float maximumT, + void* shapeDataA, int shapeTypeA, in RigidPose localPoseA, Quaternion orientationA, in BodyVelocity velocityA, + void* shapeDataB, int shapeTypeB, in RigidPose localPoseB, Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float maximumT, float minimumProgression, float convergenceThreshold, int maximumIterationCount, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal) { @@ -66,15 +66,15 @@ public unsafe bool Sweep( } protected abstract unsafe bool PreorderedTypeSweep( - void* shapeDataA, in Quaternion orientationA, in BodyVelocity velocityA, - void* shapeDataB, in Vector3 offsetB, in Quaternion orientationB, in BodyVelocity velocityB, + void* shapeDataA, Quaternion orientationA, in BodyVelocity velocityA, + void* shapeDataB, Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float maximumT, float minimumProgression, float convergenceThreshold, int maximumIterationCount, bool flipRequired, ref TSweepFilter filter, Shapes shapes, SweepTaskRegistry sweepTasks, BufferPool pool, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal) where TSweepFilter : ISweepFilter; public unsafe bool Sweep( - void* shapeDataA, int shapeTypeA, in Quaternion orientationA, in BodyVelocity velocityA, - void* shapeDataB, int shapeTypeB, in Vector3 offsetB, in Quaternion orientationB, in BodyVelocity velocityB, + void* shapeDataA, int shapeTypeA, Quaternion orientationA, in BodyVelocity velocityA, + void* shapeDataB, int shapeTypeB, Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float maximumT, float minimumProgression, float convergenceThreshold, int maximumIterationCount, ref TSweepFilter filter, Shapes shapes, SweepTaskRegistry sweepTasks, BufferPool pool, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal) where TSweepFilter : ISweepFilter diff --git a/BepuPhysics/CollisionDetection/SweepTasks/CompoundHomogeneousCompoundSweepTask.cs b/BepuPhysics/CollisionDetection/SweepTasks/CompoundHomogeneousCompoundSweepTask.cs index 586141390..4330b951b 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/CompoundHomogeneousCompoundSweepTask.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/CompoundHomogeneousCompoundSweepTask.cs @@ -20,8 +20,8 @@ public CompoundHomogeneousCompoundSweepTask() } protected unsafe override bool PreorderedTypeSweep( - void* shapeDataA, in Quaternion orientationA, in BodyVelocity velocityA, - void* shapeDataB, in Vector3 offsetB, in Quaternion orientationB, in BodyVelocity velocityB, + void* shapeDataA, Quaternion orientationA, in BodyVelocity velocityA, + void* shapeDataB, Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float maximumT, float minimumProgression, float convergenceThreshold, int maximumIterationCount, bool flipRequired, ref TSweepFilter filter, Shapes shapes, SweepTaskRegistry sweepTasks, BufferPool pool, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal) { @@ -69,7 +69,7 @@ protected unsafe override bool PreorderedTypeSweep( return t1 < float.MaxValue; } - protected override unsafe bool PreorderedTypeSweep(void* shapeDataA, in RigidPose localPoseA, in Quaternion orientationA, in BodyVelocity velocityA, void* shapeDataB, in RigidPose localPoseB, in Vector3 offsetB, in Quaternion orientationB, in BodyVelocity velocityB, float maximumT, float minimumProgression, float convergenceThreshold, int maximumIterationCount, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal) + protected override unsafe bool PreorderedTypeSweep(void* shapeDataA, in RigidPose localPoseA, Quaternion orientationA, in BodyVelocity velocityA, void* shapeDataB, in RigidPose localPoseB, Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float maximumT, float minimumProgression, float convergenceThreshold, int maximumIterationCount, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal) { throw new NotImplementedException("Compounds and meshes can never be nested; this should never be called."); } diff --git a/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepOverlapFinder.cs b/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepOverlapFinder.cs index 46299362e..c6d1690cc 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepOverlapFinder.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepOverlapFinder.cs @@ -9,8 +9,8 @@ namespace BepuPhysics.CollisionDetection.SweepTasks //At the moment, this is basically an unused abstraction. But, if you wanted, this allows you to use a special cased overlap finder in certain cases. public interface ICompoundPairSweepOverlapFinder where TCompoundA : struct, ICompoundShape where TCompoundB : struct, IBoundsQueryableCompound { - unsafe void FindOverlaps(ref TCompoundA compoundA, in Quaternion orientationA, in BodyVelocity velocityA, - ref TCompoundB compoundB, in Vector3 offsetB, in Quaternion orientationB, in BodyVelocity velocityB, float maximumT, + unsafe void FindOverlaps(ref TCompoundA compoundA, Quaternion orientationA, in BodyVelocity velocityA, + ref TCompoundB compoundB, Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float maximumT, Shapes shapes, BufferPool pool, out CompoundPairSweepOverlaps overlaps); } @@ -19,8 +19,8 @@ public struct CompoundPairSweepOverlapFinder : ICompound where TCompoundB : struct, IBoundsQueryableCompound { public unsafe void FindOverlaps( - ref TCompoundA compoundA, in Quaternion orientationA, in BodyVelocity velocityA, - ref TCompoundB compoundB, in Vector3 offsetB, in Quaternion orientationB, in BodyVelocity velocityB, float maximumT, + ref TCompoundA compoundA, Quaternion orientationA, in BodyVelocity velocityA, + ref TCompoundB compoundB, Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float maximumT, Shapes shapes, BufferPool pool, out CompoundPairSweepOverlaps overlaps) { overlaps = new CompoundPairSweepOverlaps(pool, compoundA.ChildCount); diff --git a/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepTask.cs b/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepTask.cs index 36c433748..c080a88bc 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepTask.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepTask.cs @@ -18,8 +18,8 @@ public CompoundPairSweepTask() } protected override unsafe bool PreorderedTypeSweep( - void* shapeDataA, in Quaternion orientationA, in BodyVelocity velocityA, - void* shapeDataB, in Vector3 offsetB, in Quaternion orientationB, in BodyVelocity velocityB, + void* shapeDataA, Quaternion orientationA, in BodyVelocity velocityA, + void* shapeDataB, Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float maximumT, float minimumProgression, float convergenceThreshold, int maximumIterationCount, bool flipRequired, ref TSweepFilter filter, Shapes shapes, SweepTaskRegistry sweepTasks, BufferPool pool, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal) { @@ -72,7 +72,7 @@ protected override unsafe bool PreorderedTypeSweep( return t1 < float.MaxValue; } - protected override unsafe bool PreorderedTypeSweep(void* shapeDataA, in RigidPose localPoseA, in Quaternion orientationA, in BodyVelocity velocityA, void* shapeDataB, in RigidPose localPoseB, in Vector3 offsetB, in Quaternion orientationB, in BodyVelocity velocityB, float maximumT, float minimumProgression, float convergenceThreshold, int maximumIterationCount, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal) + protected override unsafe bool PreorderedTypeSweep(void* shapeDataA, in RigidPose localPoseA, Quaternion orientationA, in BodyVelocity velocityA, void* shapeDataB, in RigidPose localPoseB, Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float maximumT, float minimumProgression, float convergenceThreshold, int maximumIterationCount, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal) { throw new NotImplementedException("Compounds cannot be nested; this should never be called."); } diff --git a/BepuPhysics/CollisionDetection/SweepTasks/ConvexCompoundSweepOverlapFinder.cs b/BepuPhysics/CollisionDetection/SweepTasks/ConvexCompoundSweepOverlapFinder.cs index 989893607..010f1c5f9 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/ConvexCompoundSweepOverlapFinder.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/ConvexCompoundSweepOverlapFinder.cs @@ -12,16 +12,16 @@ namespace BepuPhysics.CollisionDetection.SweepTasks //At the moment, this is basically an unused abstraction. But, if you wanted, this allows you to use a special cased overlap finder in certain cases. public interface IConvexCompoundSweepOverlapFinder where TShapeA : struct, IConvexShape where TCompoundB : struct, IBoundsQueryableCompound { - unsafe void FindOverlaps(ref TShapeA shapeA, in Quaternion orientationA, in BodyVelocity velocityA, - ref TCompoundB compoundB, in Vector3 offsetB, in Quaternion orientationB, in BodyVelocity velocityB, float maximumT, + unsafe void FindOverlaps(ref TShapeA shapeA, Quaternion orientationA, in BodyVelocity velocityA, + ref TCompoundB compoundB, Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float maximumT, Shapes shapes, BufferPool pool, out ChildOverlapsCollection overlaps); } public struct ConvexCompoundSweepOverlapFinder : IConvexCompoundSweepOverlapFinder where TShapeA : struct, IConvexShape where TCompoundB : struct, IBoundsQueryableCompound { - public unsafe void FindOverlaps(ref TShapeA shapeA, in Quaternion orientationA, in BodyVelocity velocityA, - ref TCompoundB compoundB, in Vector3 offsetB, in Quaternion orientationB, in BodyVelocity velocityB, float maximumT, + public unsafe void FindOverlaps(ref TShapeA shapeA, Quaternion orientationA, in BodyVelocity velocityA, + ref TCompoundB compoundB, Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float maximumT, Shapes shapes, BufferPool pool, out ChildOverlapsCollection overlaps) { BoundingBoxHelpers.GetLocalBoundingBoxForSweep(ref shapeA, orientationA, velocityA, offsetB, orientationB, velocityB, maximumT, out var sweep, out var min, out var max); diff --git a/BepuPhysics/CollisionDetection/SweepTasks/ConvexCompoundSweepTask.cs b/BepuPhysics/CollisionDetection/SweepTasks/ConvexCompoundSweepTask.cs index a15add4d4..c2c09a35b 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/ConvexCompoundSweepTask.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/ConvexCompoundSweepTask.cs @@ -19,8 +19,8 @@ public ConvexCompoundSweepTask() } protected override unsafe bool PreorderedTypeSweep( - void* shapeDataA, in Quaternion orientationA, in BodyVelocity velocityA, - void* shapeDataB, in Vector3 offsetB, in Quaternion orientationB, in BodyVelocity velocityB, + void* shapeDataA, Quaternion orientationA, in BodyVelocity velocityA, + void* shapeDataB, Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float maximumT, float minimumProgression, float convergenceThreshold, int maximumIterationCount, bool flipRequired, ref TSweepFilter filter, Shapes shapes, SweepTaskRegistry sweepTasks, BufferPool pool, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal) { @@ -62,7 +62,7 @@ protected override unsafe bool PreorderedTypeSweep( return t1 < float.MaxValue; } - protected override unsafe bool PreorderedTypeSweep(void* shapeDataA, in RigidPose localPoseA, in Quaternion orientationA, in BodyVelocity velocityA, void* shapeDataB, in RigidPose localPoseB, in Vector3 offsetB, in Quaternion orientationB, in BodyVelocity velocityB, float maximumT, float minimumProgression, float convergenceThreshold, int maximumIterationCount, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal) + protected override unsafe bool PreorderedTypeSweep(void* shapeDataA, in RigidPose localPoseA, Quaternion orientationA, in BodyVelocity velocityA, void* shapeDataB, in RigidPose localPoseB, Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float maximumT, float minimumProgression, float convergenceThreshold, int maximumIterationCount, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal) { throw new NotImplementedException("Compounds cannot be nested; this should never be called."); } diff --git a/BepuPhysics/CollisionDetection/SweepTasks/ConvexHomogeneousCompoundSweepTask.cs b/BepuPhysics/CollisionDetection/SweepTasks/ConvexHomogeneousCompoundSweepTask.cs index d78aa244c..52ca6159a 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/ConvexHomogeneousCompoundSweepTask.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/ConvexHomogeneousCompoundSweepTask.cs @@ -24,8 +24,8 @@ public ConvexHomogeneousCompoundSweepTask() protected override unsafe bool PreorderedTypeSweep( - void* shapeDataA, in Quaternion orientationA, in BodyVelocity velocityA, - void* shapeDataB, in Vector3 offsetB, in Quaternion orientationB, in BodyVelocity velocityB, float maximumT, + void* shapeDataA, Quaternion orientationA, in BodyVelocity velocityA, + void* shapeDataB, Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float maximumT, float minimumProgression, float convergenceThreshold, int maximumIterationCount, bool flipRequired, ref TSweepFilter filter, Shapes shapes, SweepTaskRegistry sweepTasks, BufferPool pool, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal) { @@ -67,7 +67,7 @@ protected override unsafe bool PreorderedTypeSweep( return t1 < float.MaxValue; } - protected override unsafe bool PreorderedTypeSweep(void* shapeDataA, in RigidPose localPoseA, in Quaternion orientationA, in BodyVelocity velocityA, void* shapeDataB, in RigidPose localPoseB, in Vector3 offsetB, in Quaternion orientationB, in BodyVelocity velocityB, float maximumT, float minimumProgression, float convergenceThreshold, int maximumIterationCount, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal) + protected override unsafe bool PreorderedTypeSweep(void* shapeDataA, in RigidPose localPoseA, Quaternion orientationA, in BodyVelocity velocityA, void* shapeDataB, in RigidPose localPoseB, Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float maximumT, float minimumProgression, float convergenceThreshold, int maximumIterationCount, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal) { throw new NotImplementedException("Compounds can never be nested; this should never be called."); } diff --git a/BepuPhysics/CollisionDetection/SweepTasks/ConvexSweepTaskCommon.cs b/BepuPhysics/CollisionDetection/SweepTasks/ConvexSweepTaskCommon.cs index 4eb5e7aff..8cdcf0593 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/ConvexSweepTaskCommon.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/ConvexSweepTaskCommon.cs @@ -27,8 +27,8 @@ public ConvexPairSweepTask() ShapeTypeIndexB = default(TShapeB).TypeId; } protected override unsafe bool PreorderedTypeSweep( - void* shapeDataA, in RigidPose localPoseA, in Quaternion orientationA, in BodyVelocity velocityA, - void* shapeDataB, in RigidPose localPoseB, in Vector3 offsetB, in Quaternion orientationB, in BodyVelocity velocityB, float maximumT, + void* shapeDataA, in RigidPose localPoseA, Quaternion orientationA, in BodyVelocity velocityA, + void* shapeDataB, in RigidPose localPoseB, Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float maximumT, float minimumProgression, float convergenceThreshold, int maximumIterationCount, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal) { @@ -43,8 +43,8 @@ protected override unsafe bool PreorderedTypeSweep( } protected override unsafe bool PreorderedTypeSweep( - void* shapeDataA, in Quaternion orientationA, in BodyVelocity velocityA, - void* shapeDataB, in Vector3 offsetB, in Quaternion orientationB, in BodyVelocity velocityB, float maximumT, + void* shapeDataA, Quaternion orientationA, in BodyVelocity velocityA, + void* shapeDataB, Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float maximumT, float minimumProgression, float convergenceThreshold, int maximumIterationCount, bool requiresFlip, ref TSweepFilter filter, Shapes shapes, SweepTaskRegistry sweepTasks, BufferPool pool, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal) { @@ -56,7 +56,7 @@ protected override unsafe bool PreorderedTypeSweep( out t0, out t1, out hitLocation, out hitNormal); } - static bool GetSphereCastInterval(in Vector3 origin, in Vector3 direction, float radius, out float t0, out float t1) + static bool GetSphereCastInterval(Vector3 origin, Vector3 direction, float radius, out float t0, out float t1) { //Normalize the direction. Sqrts aren't *that* bad, and it both simplifies things and helps avoid numerical problems. var dLength = direction.Length(); @@ -112,22 +112,22 @@ static void GetSampleTimes(float t0, float t1, ref Vector samples) interface ISweepModifier { bool GetSphereCastInterval( - in Vector3 offsetB, in Vector3 linearVelocityB, float maximumT, float maximumRadiusA, float maximumRadiusB, - in Quaternion orientationA, in Vector3 angularVelocityA, float angularSpeedA, - in Quaternion orientationB, in Vector3 angularVelocityB, float angularSpeedB, out float t0, out float t1, out Vector3 hitNormal, out Vector3 hitLocation); + Vector3 offsetB, Vector3 linearVelocityB, float maximumT, float maximumRadiusA, float maximumRadiusB, + Quaternion orientationA, Vector3 angularVelocityA, float angularSpeedA, + Quaternion orientationB, Vector3 angularVelocityB, float angularSpeedB, out float t0, out float t1, out Vector3 hitNormal, out Vector3 hitLocation); void ConstructSamples(float t0, float t1, ref Vector3Wide linearB, ref Vector3Wide angularA, ref Vector3Wide angularB, ref Vector3Wide initialOffsetB, ref QuaternionWide initialOrientationA, ref QuaternionWide initialOrientationB, ref Vector samples, ref Vector3Wide sampleOffsetB, ref QuaternionWide sampleOrientationA, ref QuaternionWide sampleOrientationB); void GetNonlinearVelocityContribution(ref Vector3Wide normal, out Vector velocityContributionA, out Vector maximumDisplacementA, out Vector velocityContributionB, out Vector maximumDisplacementB); - void AdjustHitLocation(in Quaternion initialOrientationA, in BodyVelocity velocityA, float t0, ref Vector3 hitLocation); + void AdjustHitLocation(Quaternion initialOrientationA, in BodyVelocity velocityA, float t0, ref Vector3 hitLocation); } struct UnoffsetSweep : ISweepModifier { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void AdjustHitLocation(in Quaternion initialOrientationA, in BodyVelocity velocityA, float t0, ref Vector3 hitLocation) + public void AdjustHitLocation(Quaternion initialOrientationA, in BodyVelocity velocityA, float t0, ref Vector3 hitLocation) { hitLocation += t0 * velocityA.Linear; } @@ -150,9 +150,9 @@ public void ConstructSamples(float t0, float t1, ref Vector3Wide linearB, ref Ve [MethodImpl(MethodImplOptions.AggressiveInlining)] public bool GetSphereCastInterval( - in Vector3 offsetB, in Vector3 linearVelocityB, float maximumT, float maximumRadiusA, float maximumRadiusB, - in Quaternion orientationA, in Vector3 angularVelocityA, float angularSpeedA, - in Quaternion orientationB, in Vector3 angularVelocityB, float angularSpeedB, out float t0, out float t1, out Vector3 hitNormal, out Vector3 hitLocation) + Vector3 offsetB, Vector3 linearVelocityB, float maximumT, float maximumRadiusA, float maximumRadiusB, + Quaternion orientationA, Vector3 angularVelocityA, float angularSpeedA, + Quaternion orientationB, Vector3 angularVelocityB, float angularSpeedB, out float t0, out float t1, out Vector3 hitNormal, out Vector3 hitLocation) { var hit = ConvexPairSweepTask.GetSphereCastInterval(offsetB, linearVelocityB, maximumRadiusA + maximumRadiusB, out t0, out t1); hitLocation = offsetB + linearVelocityB * t0; @@ -184,7 +184,7 @@ struct OffsetSweep : ISweepModifier public Vector3 AngularVelocityDirectionB; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void AdjustHitLocation(in Quaternion initialOrientationA, in BodyVelocity velocityA, float t0, ref Vector3 hitLocation) + public void AdjustHitLocation(Quaternion initialOrientationA, in BodyVelocity velocityA, float t0, ref Vector3 hitLocation) { PoseIntegration.Integrate(new RigidPose { Orientation = initialOrientationA }, velocityA, t0, out var integratedPose); QuaternionEx.Transform(LocalPoseA.Position, integratedPose.Orientation, out var childOffset); @@ -218,9 +218,9 @@ public void ConstructSamples(float t0, float t1, ref Vector3Wide linearB, ref Ve [MethodImpl(MethodImplOptions.AggressiveInlining)] public bool GetSphereCastInterval( - in Vector3 offsetB, in Vector3 linearVelocityB, float maximumT, float maximumRadiusA, float maximumRadiusB, - in Quaternion orientationA, in Vector3 angularVelocityA, float angularSpeedA, - in Quaternion orientationB, in Vector3 angularVelocityB, float angularSpeedB, out float t0, out float t1, out Vector3 hitNormal, out Vector3 hitLocation) + Vector3 offsetB, Vector3 linearVelocityB, float maximumT, float maximumRadiusA, float maximumRadiusB, + Quaternion orientationA, Vector3 angularVelocityA, float angularSpeedA, + Quaternion orientationB, Vector3 angularVelocityB, float angularSpeedB, out float t0, out float t1, out Vector3 hitNormal, out Vector3 hitLocation) { //The tangent velocity magnitude doesn't change over the course of the sweep. Compute and cache it as an upper bound on the contribution from the offset. QuaternionEx.TransformWithoutOverlap(LocalPoseA.Position, orientationA, out var rA); @@ -269,8 +269,8 @@ public void GetNonlinearVelocityContribution(ref Vector3Wide normal, } static unsafe bool Sweep( - void* shapeDataA, in Quaternion orientationA, in BodyVelocity velocityA, - void* shapeDataB, in Vector3 offsetB, in Quaternion orientationB, in BodyVelocity velocityB, + void* shapeDataA, Quaternion orientationA, in BodyVelocity velocityA, + void* shapeDataB, Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float maximumT, float minimumProgression, float convergenceThreshold, int maximumIterationCount, ref TSweepModifier sweepModifier, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal) diff --git a/BepuPhysics/Constraints/AreaConstraint.cs b/BepuPhysics/Constraints/AreaConstraint.cs index 9586c7468..d52b1c47e 100644 --- a/BepuPhysics/Constraints/AreaConstraint.cs +++ b/BepuPhysics/Constraints/AreaConstraint.cs @@ -31,7 +31,7 @@ public struct AreaConstraint : IThreeBodyConstraintDescription /// Initial position of the third body. /// Spring settings to apply to the volume constraint. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public AreaConstraint(in Vector3 a, in Vector3 b, in Vector3 c, SpringSettings springSettings) + public AreaConstraint(Vector3 a, Vector3 b, Vector3 c, SpringSettings springSettings) { TargetScaledArea = Vector3.Cross(b - a, c - a).Length(); SpringSettings = springSettings; diff --git a/BepuPhysics/Constraints/ConstraintChecker.cs b/BepuPhysics/Constraints/ConstraintChecker.cs index 64c102a05..420247005 100644 --- a/BepuPhysics/Constraints/ConstraintChecker.cs +++ b/BepuPhysics/Constraints/ConstraintChecker.cs @@ -60,7 +60,7 @@ public static bool IsNonpositiveNumber(float value) return IsFiniteNumber(value) && value <= 0; } [Conditional("DEBUG")] - public static void AssertUnitLength(in Vector3 v, string typeName, string propertyName) + public static void AssertUnitLength(Vector3 v, string typeName, string propertyName) { var lengthSquared = v.LengthSquared(); if (lengthSquared > 1 + 1e-5f || lengthSquared < 1 - 1e-5f || !IsFiniteNumber(lengthSquared)) @@ -69,7 +69,7 @@ public static void AssertUnitLength(in Vector3 v, string typeName, string proper } } [Conditional("DEBUG")] - public static void AssertUnitLength(in Quaternion q, string typeName, string propertyName) + public static void AssertUnitLength(Quaternion q, string typeName, string propertyName) { var lengthSquared = q.LengthSquared(); if (lengthSquared > 1 + 1e-5f || lengthSquared < 1 - 1e-5f || !IsFiniteNumber(lengthSquared)) diff --git a/BepuPhysics/Constraints/DistanceLimit.cs b/BepuPhysics/Constraints/DistanceLimit.cs index a593f8e7e..9b450354a 100644 --- a/BepuPhysics/Constraints/DistanceLimit.cs +++ b/BepuPhysics/Constraints/DistanceLimit.cs @@ -44,7 +44,7 @@ public struct DistanceLimit : ITwoBodyConstraintDescription /// Maximum distance permitted between the point on A and the point on B. /// Spring frequency and damping parameters. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public DistanceLimit(in Vector3 localOffsetA, in Vector3 localOffsetB, float minimumDistance, float maximumDistance, in SpringSettings springSettings) + public DistanceLimit(Vector3 localOffsetA, Vector3 localOffsetB, float minimumDistance, float maximumDistance, in SpringSettings springSettings) { LocalOffsetA = localOffsetA; LocalOffsetB = localOffsetB; diff --git a/BepuPhysics/Constraints/DistanceServo.cs b/BepuPhysics/Constraints/DistanceServo.cs index 2d78f192e..4f49f1d11 100644 --- a/BepuPhysics/Constraints/DistanceServo.cs +++ b/BepuPhysics/Constraints/DistanceServo.cs @@ -44,7 +44,7 @@ public struct DistanceServo : ITwoBodyConstraintDescription /// Spring frequency and damping parameters. /// Servo control parameters. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public DistanceServo(in Vector3 localOffsetA, in Vector3 localOffsetB, float targetDistance, in SpringSettings springSettings, in ServoSettings servoSettings) + public DistanceServo(Vector3 localOffsetA, Vector3 localOffsetB, float targetDistance, in SpringSettings springSettings, in ServoSettings servoSettings) { LocalOffsetA = localOffsetA; LocalOffsetB = localOffsetB; @@ -54,7 +54,7 @@ public DistanceServo(in Vector3 localOffsetA, in Vector3 localOffsetB, float tar } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public DistanceServo(in Vector3 localOffsetA, in Vector3 localOffsetB, float targetDistance, in SpringSettings springSettings) + public DistanceServo(Vector3 localOffsetA, Vector3 localOffsetB, float targetDistance, in SpringSettings springSettings) : this(localOffsetA, localOffsetB, targetDistance, springSettings, ServoSettings.Default) { } diff --git a/BepuPhysics/Constraints/VolumeConstraint.cs b/BepuPhysics/Constraints/VolumeConstraint.cs index 4c5e1dc8f..f78a11a1c 100644 --- a/BepuPhysics/Constraints/VolumeConstraint.cs +++ b/BepuPhysics/Constraints/VolumeConstraint.cs @@ -32,7 +32,7 @@ public struct VolumeConstraint : IFourBodyConstraintDescriptionInitial position of the fourth body. /// Spring settings to apply to the volume constraint. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public VolumeConstraint(in Vector3 a, in Vector3 b, in Vector3 c, in Vector3 d, SpringSettings springSettings) + public VolumeConstraint(Vector3 a, Vector3 b, Vector3 c, Vector3 d, SpringSettings springSettings) { TargetScaledVolume = Vector3.Dot(Vector3.Cross(b - a, c - a), d - a); SpringSettings = springSettings; diff --git a/BepuPhysics/Helpers.cs b/BepuPhysics/Helpers.cs index 2f17285a9..de1a0e9c1 100644 --- a/BepuPhysics/Helpers.cs +++ b/BepuPhysics/Helpers.cs @@ -47,7 +47,7 @@ public static void FindPerpendicular(in Vector3Wide normal, out Vector3Wide perp } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void BuildOrthonormalBasis(in Vector3 normal, out Vector3 t1, out Vector3 t2) + public static void BuildOrthonormalBasis(Vector3 normal, out Vector3 t1, out Vector3 t2) { var sign = normal.Z < 0 ? -1f : 1f; diff --git a/BepuPhysics/PoseIntegrator.cs b/BepuPhysics/PoseIntegrator.cs index 84f308e12..3e9139d1b 100644 --- a/BepuPhysics/PoseIntegrator.cs +++ b/BepuPhysics/PoseIntegrator.cs @@ -100,7 +100,7 @@ void IntegrateVelocity( public static class PoseIntegration { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void RotateInverseInertia(in Symmetric3x3 localInverseInertiaTensor, in Quaternion orientation, out Symmetric3x3 rotatedInverseInertiaTensor) + public static void RotateInverseInertia(in Symmetric3x3 localInverseInertiaTensor, Quaternion orientation, out Symmetric3x3 rotatedInverseInertiaTensor) { Matrix3x3.CreateFromQuaternion(orientation, out var orientationMatrix); //I^-1 = RT * Ilocal^-1 * R @@ -111,7 +111,7 @@ public static void RotateInverseInertia(in Symmetric3x3 localInverseInertiaTenso } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Integrate(in Vector3 position, in Vector3 linearVelocity, float dt, out Vector3 integratedPosition) + public static void Integrate(Vector3 position, Vector3 linearVelocity, float dt, out Vector3 integratedPosition) { position.Validate(); linearVelocity.Validate(); @@ -120,7 +120,7 @@ public static void Integrate(in Vector3 position, in Vector3 linearVelocity, flo } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Integrate(in Quaternion orientation, in Vector3 angularVelocity, float dt, out Quaternion integratedOrientation) + public static void Integrate(Quaternion orientation, Vector3 angularVelocity, float dt, out Quaternion integratedOrientation) { orientation.ValidateOrientation(); angularVelocity.Validate(); diff --git a/BepuPhysics/Simulation_Queries.cs b/BepuPhysics/Simulation_Queries.cs index 189bab580..c54989b63 100644 --- a/BepuPhysics/Simulation_Queries.cs +++ b/BepuPhysics/Simulation_Queries.cs @@ -26,7 +26,7 @@ public interface IShapeRayHitHandler /// Distance along the ray to the impact in units of ray direction length. In other words, hitLocation = ray.Origin + ray.Direction * t. /// Surface normal at the hit location. /// Index of the hit child. For convex shapes or other types that don't have multiple children, this is always zero. - void OnRayHit(in RayData ray, ref float maximumT, float t, in Vector3 normal, int childIndex); + void OnRayHit(in RayData ray, ref float maximumT, float t, Vector3 normal, int childIndex); } /// @@ -56,7 +56,7 @@ public interface IRayHitHandler /// Surface normal at the hit location. /// Collidable hit by the ray. /// Index of the hit child. For convex shapes or other types that don't have multiple children, this is always zero. - void OnRayHit(in RayData ray, ref float maximumT, float t, in Vector3 normal, CollidableReference collidable, int childIndex); + void OnRayHit(in RayData ray, ref float maximumT, float t, Vector3 normal, CollidableReference collidable, int childIndex); } /// @@ -85,7 +85,7 @@ public interface ISweepHitHandler /// Location of the first hit detected by the sweep. /// Surface normal at the hit location. /// Collidable hit by the traversal. - void OnHit(ref float maximumT, float t, in Vector3 hitLocation, in Vector3 hitNormal, CollidableReference collidable); + void OnHit(ref float maximumT, float t, Vector3 hitLocation, Vector3 hitNormal, CollidableReference collidable); /// /// Called when a sweep test detects a hit at T = 0, meaning that no location or normal can be computed. /// @@ -109,7 +109,7 @@ public bool AllowTest(int childIndex) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void OnRayHit(in RayData ray, ref float maximumT, float t, in Vector3 normal, int childIndex) + public void OnRayHit(in RayData ray, ref float maximumT, float t, Vector3 normal, int childIndex) { HitHandler.OnRayHit(ray, ref maximumT, t, normal, Collidable, childIndex); } @@ -160,7 +160,7 @@ public unsafe void RayTest(CollidableReference collidable, RayData* rayData, flo /// Maximum length of the ray traversal in units of the direction's length. /// callbacks to execute on ray-object intersections. /// User specified id of the ray. - public unsafe void RayCast(in Vector3 origin, in Vector3 direction, float maximumT, ref THitHandler hitHandler, int id = 0) where THitHandler : IRayHitHandler + public unsafe void RayCast(Vector3 origin, Vector3 direction, float maximumT, ref THitHandler hitHandler, int id = 0) where THitHandler : IRayHitHandler { RayHitDispatcher dispatcher; dispatcher.ShapeHitHandler.HitHandler = hitHandler; diff --git a/BepuPhysics/Trees/RayBatcher.cs b/BepuPhysics/Trees/RayBatcher.cs index ad6ba3ff8..f9a490a47 100644 --- a/BepuPhysics/Trees/RayBatcher.cs +++ b/BepuPhysics/Trees/RayBatcher.cs @@ -26,7 +26,7 @@ public struct TreeRay public Vector3 InverseDirection; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void CreateFrom(in Vector3 origin, in Vector3 direction, float maximumT, out TreeRay treeRay) + public static void CreateFrom(Vector3 origin, Vector3 direction, float maximumT, out TreeRay treeRay) { //Note that this division has two odd properties: //1) If the local direction has a near zero component, it is clamped to a nonzero but extremely small value. This is a hack, but it works reasonably well. @@ -40,7 +40,7 @@ public static void CreateFrom(in Vector3 origin, in Vector3 direction, float max } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void CreateFrom(in Vector3 origin, in Vector3 direction, float maximumT, int id, out RayData rayData, out TreeRay treeRay) + public static void CreateFrom(Vector3 origin, Vector3 direction, float maximumT, int id, out RayData rayData, out TreeRay treeRay) { rayData.Origin = origin; rayData.Id = id; diff --git a/BepuPhysics/Trees/Tree_RayCast.cs b/BepuPhysics/Trees/Tree_RayCast.cs index b387a8d01..ca271f50c 100644 --- a/BepuPhysics/Trees/Tree_RayCast.cs +++ b/BepuPhysics/Trees/Tree_RayCast.cs @@ -10,7 +10,7 @@ namespace BepuPhysics.Trees partial struct Tree { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe static bool Intersects(in Vector3 min, in Vector3 max, TreeRay* ray, out float t) + public unsafe static bool Intersects(Vector3 min, Vector3 max, TreeRay* ray, out float t) { var t0 = min * ray->InverseDirection - ray->OriginOverDirection; var t1 = max * ray->InverseDirection - ray->OriginOverDirection; @@ -111,7 +111,7 @@ internal readonly unsafe void RayCast(TreeRay* treeRay, RayData* ra } } - public readonly unsafe void RayCast(in Vector3 origin, in Vector3 direction, ref float maximumT, ref TLeafTester leafTester, int id = 0) where TLeafTester : IRayLeafTester + public readonly unsafe void RayCast(Vector3 origin, Vector3 direction, ref float maximumT, ref TLeafTester leafTester, int id = 0) where TLeafTester : IRayLeafTester { TreeRay.CreateFrom(origin, direction, maximumT, id, out var rayData, out var treeRay); RayCast(&treeRay, &rayData, ref leafTester); diff --git a/BepuPhysics/Trees/Tree_Sweep.cs b/BepuPhysics/Trees/Tree_Sweep.cs index 612c63186..1838c093c 100644 --- a/BepuPhysics/Trees/Tree_Sweep.cs +++ b/BepuPhysics/Trees/Tree_Sweep.cs @@ -14,7 +14,7 @@ public interface ISweepLeafTester } partial struct Tree { - readonly unsafe void Sweep(int nodeIndex, in Vector3 expansion, in Vector3 origin, in Vector3 direction, TreeRay* treeRay, int* stack, ref TLeafTester leafTester) where TLeafTester : ISweepLeafTester + readonly unsafe void Sweep(int nodeIndex, Vector3 expansion, Vector3 origin, Vector3 direction, TreeRay* treeRay, int* stack, ref TLeafTester leafTester) where TLeafTester : ISweepLeafTester { Debug.Assert((nodeIndex >= 0 && nodeIndex < NodeCount) || (Encode(nodeIndex) >= 0 && Encode(nodeIndex) < LeafCount)); Debug.Assert(LeafCount >= 2, "This implementation assumes all nodes are filled."); @@ -81,7 +81,7 @@ readonly unsafe void Sweep(int nodeIndex, in Vector3 expansion, in } - internal readonly unsafe void Sweep(in Vector3 expansion, in Vector3 origin, in Vector3 direction, TreeRay* treeRay, ref TLeafTester sweepTester) where TLeafTester : ISweepLeafTester + internal readonly unsafe void Sweep(Vector3 expansion, Vector3 origin, Vector3 direction, TreeRay* treeRay, ref TLeafTester sweepTester) where TLeafTester : ISweepLeafTester { if (LeafCount == 0) return; @@ -104,7 +104,7 @@ internal readonly unsafe void Sweep(in Vector3 expansion, in Vector } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ConvertBoxToCentroidWithExtent(in Vector3 min, in Vector3 max, out Vector3 origin, out Vector3 expansion) + public static void ConvertBoxToCentroidWithExtent(Vector3 min, Vector3 max, out Vector3 origin, out Vector3 expansion) { var halfMin = 0.5f * min; var halfMax = 0.5f * max; @@ -112,14 +112,14 @@ public static void ConvertBoxToCentroidWithExtent(in Vector3 min, in Vector3 max origin = halfMax + halfMin; } - public readonly unsafe void Sweep(in Vector3 min, in Vector3 max, in Vector3 direction, float maximumT, ref TLeafTester sweepTester) where TLeafTester : ISweepLeafTester + public readonly unsafe void Sweep(Vector3 min, Vector3 max, Vector3 direction, float maximumT, ref TLeafTester sweepTester) where TLeafTester : ISweepLeafTester { ConvertBoxToCentroidWithExtent(min, max, out var origin, out var expansion); TreeRay.CreateFrom(origin, direction, maximumT, out var treeRay); Sweep(expansion, origin, direction, &treeRay, ref sweepTester); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public readonly unsafe void Sweep(in BoundingBox boundingBox, in Vector3 direction, float maximumT, ref TLeafTester sweepTester) where TLeafTester : ISweepLeafTester + public readonly unsafe void Sweep(in BoundingBox boundingBox, Vector3 direction, float maximumT, ref TLeafTester sweepTester) where TLeafTester : ISweepLeafTester { Sweep(boundingBox.Min, boundingBox.Max, direction, maximumT, ref sweepTester); } diff --git a/BepuPhysics/Trees/Tree_VolumeQuery.cs b/BepuPhysics/Trees/Tree_VolumeQuery.cs index 626f605e6..162b697d4 100644 --- a/BepuPhysics/Trees/Tree_VolumeQuery.cs +++ b/BepuPhysics/Trees/Tree_VolumeQuery.cs @@ -7,7 +7,7 @@ namespace BepuPhysics.Trees { partial struct Tree { - unsafe readonly void GetOverlaps(int nodeIndex, in Vector3 min, in Vector3 max, int* stack, ref TEnumerator leafEnumerator) where TEnumerator : IBreakableForEach + unsafe readonly void GetOverlaps(int nodeIndex, Vector3 min, Vector3 max, int* stack, ref TEnumerator leafEnumerator) where TEnumerator : IBreakableForEach { Debug.Assert((nodeIndex >= 0 && nodeIndex < NodeCount) || (Encode(nodeIndex) >= 0 && Encode(nodeIndex) < LeafCount)); Debug.Assert(LeafCount >= 2, "This implementation assumes all nodes are filled."); @@ -58,7 +58,7 @@ unsafe readonly void GetOverlaps(int nodeIndex, in Vector3 min, in } } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public readonly unsafe void GetOverlaps(in Vector3 min, in Vector3 max, ref TEnumerator leafEnumerator) where TEnumerator : IBreakableForEach + public readonly unsafe void GetOverlaps(Vector3 min, Vector3 max, ref TEnumerator leafEnumerator) where TEnumerator : IBreakableForEach { if (LeafCount > 1) { diff --git a/BepuUtilities/AffineTransform.cs b/BepuUtilities/AffineTransform.cs index 4065dc929..36395cf33 100644 --- a/BepuUtilities/AffineTransform.cs +++ b/BepuUtilities/AffineTransform.cs @@ -35,7 +35,7 @@ public static AffineTransform Identity /// Constructs a new affine transform. /// ///Translation to use in the transform. - public AffineTransform(in Vector3 translation) + public AffineTransform(Vector3 translation) { LinearTransform = Matrix3x3.Identity; Translation = translation; @@ -46,7 +46,7 @@ public AffineTransform(in Vector3 translation) /// ///Orientation to use as the linear transform. ///Translation to use in the transform. - public AffineTransform(in Quaternion orientation, in Vector3 translation) + public AffineTransform(Quaternion orientation, Vector3 translation) { Matrix3x3.CreateFromQuaternion(orientation, out LinearTransform); Translation = translation; @@ -58,7 +58,7 @@ public AffineTransform(in Quaternion orientation, in Vector3 translation) ///Scaling to apply in the linear transform. ///Orientation to apply in the linear transform. ///Translation to apply. - public AffineTransform(in Vector3 scaling, in Quaternion orientation, in Vector3 translation) + public AffineTransform(Vector3 scaling, Quaternion orientation, Vector3 translation) { //Create an SRT transform. Matrix3x3.CreateScale(scaling, out LinearTransform); @@ -72,7 +72,7 @@ public AffineTransform(in Vector3 scaling, in Quaternion orientation, in Vector3 /// ///The linear transform component. ///Translation component of the transform. - public AffineTransform(in Matrix3x3 linearTransform, in Vector3 translation) + public AffineTransform(in Matrix3x3 linearTransform, Vector3 translation) { LinearTransform = linearTransform; Translation = translation; @@ -86,7 +86,7 @@ public AffineTransform(in Matrix3x3 linearTransform, in Vector3 translation) ///Transform to apply. ///Transformed position. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Transform(in Vector3 position, in AffineTransform transform, out Vector3 transformed) + public static void Transform(Vector3 position, in AffineTransform transform, out Vector3 transformed) { Matrix3x3.Transform(position, transform.LinearTransform, out transformed); transformed += transform.Translation; diff --git a/BepuUtilities/BoundingBox.cs b/BepuUtilities/BoundingBox.cs index 533039c33..f7865c344 100644 --- a/BepuUtilities/BoundingBox.cs +++ b/BepuUtilities/BoundingBox.cs @@ -60,7 +60,7 @@ public static bool Intersects(in BoundingBox a, in BoundingBox b) /// Second bounding box to test. /// Whether the bounding boxes intersected. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static bool Intersects(in Vector3 minA, in Vector3 maxA, in Vector3 minB, in Vector3 maxB) + public static bool Intersects(Vector3 minA, Vector3 maxA, Vector3 minB, Vector3 maxB) { return maxA.X >= minB.X & maxA.Y >= minB.Y & maxA.Z >= minB.Z & maxB.X >= minA.X & maxB.Y >= minA.Y & maxB.Z >= minA.Z; @@ -89,7 +89,7 @@ public static unsafe float ComputeVolume(ref BoundingBox box) /// Minimum of the merged bounding box. /// Maximum of the merged bounding box. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void CreateMerged(in Vector3 minA, in Vector3 maxA, in Vector3 minB, in Vector3 maxB, out Vector3 min, out Vector3 max) + public static void CreateMerged(Vector3 minA, Vector3 maxA, Vector3 minB, Vector3 maxB, out Vector3 min, out Vector3 max) { min = Vector3.Min(minA, minB); max = Vector3.Max(maxA, maxB); diff --git a/BepuUtilities/Matrix.cs b/BepuUtilities/Matrix.cs index b9070787a..6885e5c11 100644 --- a/BepuUtilities/Matrix.cs +++ b/BepuUtilities/Matrix.cs @@ -173,7 +173,7 @@ public static void Transform(in Vector4 v, in Matrix m, out Vector4 result) /// Matrix to apply to the vector. /// Transformed vector. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Transform(in Vector3 v, in Matrix m, out Vector4 result) + public static void Transform(Vector3 v, in Matrix m, out Vector4 result) { var x = new Vector4(v.X); var y = new Vector4(v.Y); @@ -228,7 +228,7 @@ public static void Multiply(in Matrix a, in Matrix b, out Matrix result) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void CreateFromAxisAngle(in Vector3 axis, float angle, out Matrix result) + public static void CreateFromAxisAngle(Vector3 axis, float angle, out Matrix result) { //TODO: Could be better simdified. float xx = axis.X * axis.X; @@ -263,7 +263,7 @@ public static void CreateFromAxisAngle(in Vector3 axis, float angle, out Matrix } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Matrix CreateFromAxisAngle(in Vector3 axis, float angle) + public static Matrix CreateFromAxisAngle(Vector3 axis, float angle) { CreateFromAxisAngle(axis, angle, out Matrix result); return result; @@ -271,7 +271,7 @@ public static Matrix CreateFromAxisAngle(in Vector3 axis, float angle) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void CreateFromQuaternion(in Quaternion quaternion, out Matrix result) + public static void CreateFromQuaternion(Quaternion quaternion, out Matrix result) { float qX2 = quaternion.X + quaternion.X; float qY2 = quaternion.Y + quaternion.Y; @@ -313,7 +313,7 @@ public static void CreateFromQuaternion(in Quaternion quaternion, out Matrix res } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Matrix CreateFromQuaternion(in Quaternion quaternion) + public static Matrix CreateFromQuaternion(Quaternion quaternion) { CreateFromQuaternion(quaternion, out Matrix toReturn); return toReturn; @@ -533,7 +533,7 @@ public static Matrix Invert(in Matrix m) /// Target of the camera. /// Up vector of the camera. /// Look at matrix. - public static void CreateLookAt(in Vector3 position, in Vector3 target, in Vector3 upVector, out Matrix viewMatrix) + public static void CreateLookAt(Vector3 position, Vector3 target, Vector3 upVector, out Matrix viewMatrix) { Vector3 forward = target - position; CreateView(position, forward, upVector, out viewMatrix); @@ -546,7 +546,7 @@ public static void CreateLookAt(in Vector3 position, in Vector3 target, in Vecto /// Target of the camera. /// Up vector of the camera. /// Look at matrix. - public static Matrix CreateLookAt(in Vector3 position, in Vector3 target, in Vector3 upVector) + public static Matrix CreateLookAt(Vector3 position, Vector3 target, Vector3 upVector) { CreateView(position, target - position, upVector, out Matrix lookAt); return lookAt; @@ -560,7 +560,7 @@ public static Matrix CreateLookAt(in Vector3 position, in Vector3 target, in Vec /// Forward direction of the camera. /// Up vector of the camera. /// Look at matrix. - public static void CreateView(in Vector3 position, in Vector3 forward, in Vector3 upVector, out Matrix viewMatrix) + public static void CreateView(Vector3 position, Vector3 forward, Vector3 upVector, out Matrix viewMatrix) { float length = forward.Length(); var z = forward / -length; @@ -585,7 +585,7 @@ public static void CreateView(in Vector3 position, in Vector3 forward, in Vector /// Forward direction of the camera. /// Up vector of the camera. /// Look at matrix. - public static Matrix CreateView(in Vector3 position, in Vector3 forward, in Vector3 upVector) + public static Matrix CreateView(Vector3 position, Vector3 forward, Vector3 upVector) { Matrix lookat; CreateView(position, forward, upVector, out lookat); @@ -601,7 +601,7 @@ public static Matrix CreateView(in Vector3 position, in Vector3 forward, in Vect /// Position of the transform. /// 4x4 matrix representing the combined transform. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void CreateRigid(in Matrix3x3 rotation, in Vector3 position, out Matrix world) + public static void CreateRigid(in Matrix3x3 rotation, Vector3 position, out Matrix world) { world.X = new Vector4(rotation.X, 0); world.Y = new Vector4(rotation.Y, 0); @@ -616,7 +616,7 @@ public static void CreateRigid(in Matrix3x3 rotation, in Vector3 position, out M /// Position of the transform. /// 4x4 matrix representing the combined transform. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void CreateRigid(in Quaternion rotation, in Vector3 position, out Matrix world) + public static void CreateRigid(Quaternion rotation, Vector3 position, out Matrix world) { Matrix3x3.CreateFromQuaternion(rotation, out var rotationMatrix); world.X = new Vector4(rotationMatrix.X, 0); diff --git a/BepuUtilities/Matrix3x3.cs b/BepuUtilities/Matrix3x3.cs index b8fed1c0c..c2d2e7754 100644 --- a/BepuUtilities/Matrix3x3.cs +++ b/BepuUtilities/Matrix3x3.cs @@ -197,7 +197,7 @@ public unsafe static void Invert(Matrix3x3* m, Matrix3x3* inverse) /// Matrix to use as the transformation. /// Product of the transformation. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Transform(in Vector3 v, in Matrix3x3 m, out Vector3 result) + public static void Transform(Vector3 v, in Matrix3x3 m, out Vector3 result) { var x = new Vector3(v.X); var y = new Vector3(v.Y); @@ -212,7 +212,7 @@ public static void Transform(in Vector3 v, in Matrix3x3 m, out Vector3 result) /// Matrix to use as the transformation transpose. /// Product of the transformation. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void TransformTranspose(in Vector3 v, in Matrix3x3 m, out Vector3 result) + public static void TransformTranspose(Vector3 v, in Matrix3x3 m, out Vector3 result) { result = new Vector3( Vector3.Dot(v, m.X), @@ -303,7 +303,7 @@ public static void CreateFromMatrix(in Matrix matrix, out Matrix3x3 matrix3x3) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void CreateFromQuaternion(in Quaternion quaternion, out Matrix3x3 result) + public static void CreateFromQuaternion(Quaternion quaternion, out Matrix3x3 result) { float qX2 = quaternion.X + quaternion.X; float qY2 = quaternion.Y + quaternion.Y; @@ -337,7 +337,7 @@ public static void CreateFromQuaternion(in Quaternion quaternion, out Matrix3x3 } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Matrix3x3 CreateFromQuaternion(in Quaternion quaternion) + public static Matrix3x3 CreateFromQuaternion(Quaternion quaternion) { CreateFromQuaternion(quaternion, out var toReturn); return toReturn; @@ -350,7 +350,7 @@ public static Matrix3x3 CreateFromQuaternion(in Quaternion quaternion) /// Scale to represent. /// Matrix representing a scale. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void CreateScale(in Vector3 scale, out Matrix3x3 linearTransform) + public static void CreateScale(Vector3 scale, out Matrix3x3 linearTransform) { linearTransform.X = new Vector3(scale.X, 0, 0); linearTransform.Y = new Vector3(0, scale.Y, 0); @@ -364,7 +364,7 @@ public static void CreateScale(in Vector3 scale, out Matrix3x3 linearTransform) /// Angle of the rotation. /// Resulting rotation matrix. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void CreateFromAxisAngle(in Vector3 axis, float angle, out Matrix3x3 result) + public static void CreateFromAxisAngle(Vector3 axis, float angle, out Matrix3x3 result) { //TODO: Could be better simdified. float xx = axis.X * axis.X; @@ -401,7 +401,7 @@ public static void CreateFromAxisAngle(in Vector3 axis, float angle, out Matrix3 /// Angle of the rotation. /// Resulting rotation matrix. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Matrix3x3 CreateFromAxisAngle(in Vector3 axis, float angle) + public static Matrix3x3 CreateFromAxisAngle(Vector3 axis, float angle) { CreateFromAxisAngle(axis, angle, out var result); return result; @@ -414,7 +414,7 @@ public static Matrix3x3 CreateFromAxisAngle(in Vector3 axis, float angle) /// Vector to build the skew symmetric matrix from. /// Skew symmetric matrix representing the cross product. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void CreateCrossProduct(in Vector3 v, out Matrix3x3 result) + public static void CreateCrossProduct(Vector3 v, out Matrix3x3 result) { result.X.X = 0f; result.X.Y = -v.Z; diff --git a/BepuUtilities/QuaternionEx.cs b/BepuUtilities/QuaternionEx.cs index 0bf044a8f..dd3e9b49d 100644 --- a/BepuUtilities/QuaternionEx.cs +++ b/BepuUtilities/QuaternionEx.cs @@ -15,7 +15,7 @@ public static class QuaternionEx /// Second quaternion to add. /// Sum of the addition. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Add(in Quaternion a, in Quaternion b, out Quaternion result) + public static void Add(Quaternion a, Quaternion b, out Quaternion result) { result.X = a.X + b.X; result.Y = a.Y + b.Y; @@ -30,7 +30,7 @@ public static void Add(in Quaternion a, in Quaternion b, out Quaternion result) /// Amount to multiply each component of the quaternion by. /// Scaled quaternion. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Scale(in Quaternion q, float scale, out Quaternion result) + public static void Scale(Quaternion q, float scale, out Quaternion result) { result.X = q.X * scale; result.Y = q.Y * scale; @@ -47,7 +47,7 @@ public static void Scale(in Quaternion q, float scale, out Quaternion result) /// Second quaternion to concatenate. /// Product of the concatenation. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ConcatenateWithoutOverlap(in Quaternion a, in Quaternion b, out Quaternion result) + public static void ConcatenateWithoutOverlap(Quaternion a, Quaternion b, out Quaternion result) { result.X = a.W * b.X + a.X * b.W + a.Z * b.Y - a.Y * b.Z; result.Y = a.W * b.Y + a.Y * b.W + a.X * b.Z - a.Z * b.X; @@ -63,7 +63,7 @@ public static void ConcatenateWithoutOverlap(in Quaternion a, in Quaternion b, o /// Second quaternion to concatenate. /// Product of the concatenation. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Concatenate(in Quaternion a, in Quaternion b, out Quaternion result) + public static void Concatenate(Quaternion a, Quaternion b, out Quaternion result) { ConcatenateWithoutOverlap(a, b, out var temp); result = temp; @@ -78,7 +78,7 @@ public static void Concatenate(in Quaternion a, in Quaternion b, out Quaternion /// Second quaternion to multiply. /// Product of the multiplication. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Quaternion Concatenate(in Quaternion a, in Quaternion b) + public static Quaternion Concatenate(Quaternion a, Quaternion b) { ConcatenateWithoutOverlap(a, b, out var result); return result; @@ -230,7 +230,7 @@ public static float Length(ref Quaternion quaternion) /// Amount of the end point to use. /// Interpolated intermediate quaternion. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Slerp(in Quaternion start, Quaternion end, float interpolationAmount, out Quaternion result) + public static void Slerp(Quaternion start, Quaternion end, float interpolationAmount, out Quaternion result) { double cosHalfTheta = start.W * end.W + start.X * end.X + start.Y * end.Y + start.Z * end.Z; if (cosHalfTheta < 0) @@ -274,7 +274,7 @@ public static void Slerp(in Quaternion start, Quaternion end, float interpolatio /// Amount of the end point to use. /// Interpolated intermediate quaternion. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Quaternion Slerp(in Quaternion start, in Quaternion end, float interpolationAmount) + public static Quaternion Slerp(Quaternion start, Quaternion end, float interpolationAmount) { Slerp(start, end, interpolationAmount, out Quaternion toReturn); return toReturn; @@ -287,7 +287,7 @@ public static Quaternion Slerp(in Quaternion start, in Quaternion end, float int /// Quaternion to conjugate. /// Conjugated quaternion. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Conjugate(in Quaternion quaternion, out Quaternion result) + public static void Conjugate(Quaternion quaternion, out Quaternion result) { result.X = -quaternion.X; result.Y = -quaternion.Y; @@ -301,7 +301,7 @@ public static void Conjugate(in Quaternion quaternion, out Quaternion result) /// Quaternion to conjugate. /// Conjugated quaternion. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Quaternion Conjugate(in Quaternion quaternion) + public static Quaternion Conjugate(Quaternion quaternion) { Conjugate(quaternion, out Quaternion toReturn); return toReturn; @@ -315,7 +315,7 @@ public static Quaternion Conjugate(in Quaternion quaternion) /// Quaternion to invert. /// Result of the inversion. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Inverse(in Quaternion quaternion, out Quaternion result) + public static void Inverse(Quaternion quaternion, out Quaternion result) { float inverseSquaredNorm = quaternion.X * quaternion.X + quaternion.Y * quaternion.Y + quaternion.Z * quaternion.Z + quaternion.W * quaternion.W; result.X = -quaternion.X * inverseSquaredNorm; @@ -330,7 +330,7 @@ public static void Inverse(in Quaternion quaternion, out Quaternion result) /// Quaternion to invert. /// Result of the inversion. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Quaternion Inverse(in Quaternion quaternion) + public static Quaternion Inverse(Quaternion quaternion) { Inverse(quaternion, out var result); return result; @@ -343,7 +343,7 @@ public static Quaternion Inverse(in Quaternion quaternion) /// Quaternion to negate. /// Negated result. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Negate(in Quaternion a, out Quaternion b) + public static void Negate(Quaternion a, out Quaternion b) { b.X = -a.X; b.Y = -a.Y; @@ -357,7 +357,7 @@ public static void Negate(in Quaternion a, out Quaternion b) /// Quaternion to negate. /// Negated result. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Quaternion Negate(in Quaternion q) + public static Quaternion Negate(Quaternion q) { Negate(q, out var result); return result; @@ -370,7 +370,7 @@ public static Quaternion Negate(in Quaternion q) /// Rotation to apply to the vector. /// Transformed vector. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void TransformWithoutOverlap(in Vector3 v, in Quaternion rotation, out Vector3 result) + public static void TransformWithoutOverlap(Vector3 v, Quaternion rotation, out Vector3 result) { //This operation is an optimized-down version of v' = q * v * q^-1. //The expanded form would be to treat v as an 'axis only' quaternion @@ -402,7 +402,7 @@ public static void TransformWithoutOverlap(in Vector3 v, in Quaternion rotation, /// Rotation to apply to the vector. /// Transformed vector. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Transform(in Vector3 v, in Quaternion rotation, out Vector3 result) + public static void Transform(Vector3 v, Quaternion rotation, out Vector3 result) { TransformWithoutOverlap(v, rotation, out var temp); result = temp; @@ -415,7 +415,7 @@ public static void Transform(in Vector3 v, in Quaternion rotation, out Vector3 r /// Rotation to apply to the vector. /// Transformed vector. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector3 Transform(in Vector3 v, in Quaternion rotation) + public static Vector3 Transform(Vector3 v, Quaternion rotation) { TransformWithoutOverlap(v, rotation, out var toReturn); return toReturn; @@ -427,7 +427,7 @@ public static Vector3 Transform(in Vector3 v, in Quaternion rotation) /// Rotation to apply to the vector. /// Transformed vector. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void TransformUnitX(in Quaternion rotation, out Vector3 result) + public static void TransformUnitX(Quaternion rotation, out Vector3 result) { //This operation is an optimized-down version of v' = q * v * q^-1. //The expanded form would be to treat v as an 'axis only' quaternion @@ -453,7 +453,7 @@ public static void TransformUnitX(in Quaternion rotation, out Vector3 result) /// Rotation to apply to the vector. /// Transformed vector. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void TransformUnitY(in Quaternion rotation, out Vector3 result) + public static void TransformUnitY(Quaternion rotation, out Vector3 result) { //This operation is an optimized-down version of v' = q * v * q^-1. //The expanded form would be to treat v as an 'axis only' quaternion @@ -479,7 +479,7 @@ public static void TransformUnitY(in Quaternion rotation, out Vector3 result) /// Rotation to apply to the vector. /// Transformed vector. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void TransformUnitZ(in Quaternion rotation, out Vector3 result) + public static void TransformUnitZ(Quaternion rotation, out Vector3 result) { //This operation is an optimized-down version of v' = q * v * q^-1. //The expanded form would be to treat v as an 'axis only' quaternion @@ -506,7 +506,7 @@ public static void TransformUnitZ(in Quaternion rotation, out Vector3 result) /// Angle to rotate around the axis. /// Quaternion representing the axis and angle rotation. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Quaternion CreateFromAxisAngle(in Vector3 axis, float angle) + public static Quaternion CreateFromAxisAngle(Vector3 axis, float angle) { double halfAngle = angle * 0.5; double s = Math.Sin(halfAngle); @@ -525,7 +525,7 @@ public static Quaternion CreateFromAxisAngle(in Vector3 axis, float angle) /// Angle to rotate around the axis. /// Quaternion representing the axis and angle rotation. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void CreateFromAxisAngle(in Vector3 axis, float angle, out Quaternion q) + public static void CreateFromAxisAngle(Vector3 axis, float angle, out Quaternion q) { double halfAngle = angle * 0.5; double s = Math.Sin(halfAngle); @@ -589,7 +589,7 @@ public static void CreateFromYawPitchRoll(float yaw, float pitch, float roll, ou /// Quaternion to be converted. /// Angle around the axis represented by the quaternion. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static float GetAngleFromQuaternion(in Quaternion q) + public static float GetAngleFromQuaternion(Quaternion q) { float qw = Math.Abs(q.W); if (qw > 1) @@ -604,7 +604,7 @@ public static float GetAngleFromQuaternion(in Quaternion q) /// Axis represented by the quaternion. /// Angle around the axis represented by the quaternion. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void GetAxisAngleFromQuaternion(in Quaternion q, out Vector3 axis, out float angle) + public static void GetAxisAngleFromQuaternion(Quaternion q, out Vector3 axis, out float angle) { float qw = q.W; if (qw > 0) @@ -641,7 +641,7 @@ public static void GetAxisAngleFromQuaternion(in Quaternion q, out Vector3 axis, /// Second unit-length vector. /// Quaternion representing the rotation from v1 to v2. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void GetQuaternionBetweenNormalizedVectors(in Vector3 v1, in Vector3 v2, out Quaternion q) + public static void GetQuaternionBetweenNormalizedVectors(Vector3 v1, Vector3 v2, out Quaternion q) { float dot = Vector3.Dot(v1, v2); //For non-normal vectors, the multiplying the axes length squared would be necessary: @@ -682,7 +682,7 @@ public static void GetQuaternionBetweenNormalizedVectors(in Vector3 v1, in Vecto /// Ending orientation. /// Relative rotation from the start to the end orientation. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void GetRelativeRotationWithoutOverlap(in Quaternion start, in Quaternion end, out Quaternion relative) + public static void GetRelativeRotationWithoutOverlap(Quaternion start, Quaternion end, out Quaternion relative) { Conjugate(start, out var startInverse); ConcatenateWithoutOverlap(startInverse, end, out relative); @@ -697,7 +697,7 @@ public static void GetRelativeRotationWithoutOverlap(in Quaternion start, in Qua /// Basis in the original frame of reference to transform the rotation into. /// Rotation in the local space of the target basis. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void GetLocalRotationWithoutOverlap(in Quaternion rotation, in Quaternion targetBasis, out Quaternion localRotation) + public static void GetLocalRotationWithoutOverlap(Quaternion rotation, Quaternion targetBasis, out Quaternion localRotation) { Conjugate(targetBasis, out var basisInverse); ConcatenateWithoutOverlap(rotation, basisInverse, out localRotation); diff --git a/BepuUtilities/QuaternionWide.cs b/BepuUtilities/QuaternionWide.cs index c286b3324..775a871e8 100644 --- a/BepuUtilities/QuaternionWide.cs +++ b/BepuUtilities/QuaternionWide.cs @@ -12,7 +12,7 @@ public struct QuaternionWide public Vector W; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Broadcast(in Quaternion source, out QuaternionWide broadcasted) + public static void Broadcast(Quaternion source, out QuaternionWide broadcasted) { broadcasted.X = new Vector(source.X); broadcasted.Y = new Vector(source.Y); @@ -609,7 +609,7 @@ public static void ReadFirst(in QuaternionWide source, out Quaternion target) /// Quaternion to copy values from. /// Wide quaternion to place values into. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void WriteFirst(in Quaternion source, ref QuaternionWide targetSlot) + public static void WriteFirst(Quaternion source, ref QuaternionWide targetSlot) { GatherScatter.GetFirst(ref targetSlot.X) = source.X; GatherScatter.GetFirst(ref targetSlot.Y) = source.Y; @@ -624,7 +624,7 @@ public static void WriteFirst(in Quaternion source, ref QuaternionWide targetSlo /// Index of the slot to write into. /// Bundle to write the value into. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void WriteSlot(in Quaternion source, int slotIndex, ref QuaternionWide target) + public static void WriteSlot(Quaternion source, int slotIndex, ref QuaternionWide target) { WriteFirst(source, ref GatherScatter.GetOffsetInstance(ref target, slotIndex)); } diff --git a/BepuUtilities/Symmetric3x3.cs b/BepuUtilities/Symmetric3x3.cs index 4771e1496..90442add0 100644 --- a/BepuUtilities/Symmetric3x3.cs +++ b/BepuUtilities/Symmetric3x3.cs @@ -491,7 +491,7 @@ public static void Multiply(in Matrix3x3 a, in Symmetric3x3 b, out Matrix3x3 res /// Matrix to interpret as symmetric transform. /// Result of transforming the vector by the given symmetric matrix. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void TransformWithoutOverlap(in Vector3 v, in Symmetric3x3 m, out Vector3 result) + public static void TransformWithoutOverlap(Vector3 v, in Symmetric3x3 m, out Vector3 result) { result.X = v.X * m.XX + v.Y * m.YX + v.Z * m.ZX; result.Y = v.X * m.YX + v.Y * m.YY + v.Z * m.ZY; diff --git a/BepuUtilities/Vector3Wide.cs b/BepuUtilities/Vector3Wide.cs index c36c33589..4d3bca1bd 100644 --- a/BepuUtilities/Vector3Wide.cs +++ b/BepuUtilities/Vector3Wide.cs @@ -801,7 +801,7 @@ public static void ReadFirst(in Vector3Wide source, out Vector3 target) /// Vector to copy values from. /// Wide vectorto place values into. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void WriteFirst(in Vector3 source, ref Vector3Wide targetSlot) + public static void WriteFirst(Vector3 source, ref Vector3Wide targetSlot) { GatherScatter.GetFirst(ref targetSlot.X) = source.X; GatherScatter.GetFirst(ref targetSlot.Y) = source.Y; @@ -815,7 +815,7 @@ public static void WriteFirst(in Vector3 source, ref Vector3Wide targetSlot) /// Index of the slot to write into. /// Bundle to write the value into. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void WriteSlot(in Vector3 source, int slotIndex, ref Vector3Wide target) + public static void WriteSlot(Vector3 source, int slotIndex, ref Vector3Wide target) { WriteFirst(source, ref GatherScatter.GetOffsetInstance(ref target, slotIndex)); } @@ -826,7 +826,7 @@ public static void WriteSlot(in Vector3 source, int slotIndex, ref Vector3Wide t /// Source value to write to every bundle slot. /// Bundle containing the source's components in every slot. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Broadcast(in Vector3 source, out Vector3Wide broadcasted) + public static void Broadcast(Vector3 source, out Vector3Wide broadcasted) { broadcasted.X = new Vector(source.X); broadcasted.Y = new Vector(source.Y); diff --git a/DemoContentLoader/MeshIO.cs b/DemoContentLoader/MeshIO.cs index 09ce6289d..f88a61cf9 100644 --- a/DemoContentLoader/MeshIO.cs +++ b/DemoContentLoader/MeshIO.cs @@ -30,7 +30,7 @@ public static MeshContent Load(BinaryReader reader) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Write(BinaryWriter writer, in Vector3 v) + public static void Write(BinaryWriter writer, Vector3 v) { writer.Write(v.X); writer.Write(v.Y); diff --git a/DemoRenderer/Constraints/BoundingBoxLineExtractor.cs b/DemoRenderer/Constraints/BoundingBoxLineExtractor.cs index 6a672628a..e585f5685 100644 --- a/DemoRenderer/Constraints/BoundingBoxLineExtractor.cs +++ b/DemoRenderer/Constraints/BoundingBoxLineExtractor.cs @@ -24,7 +24,7 @@ internal struct ThreadJob } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void WriteBoundsLines(in Vector3 min, in Vector3 max, uint packedColor, uint packedBackgroundColor, ref LineInstance targetLines) + public static void WriteBoundsLines(Vector3 min, Vector3 max, uint packedColor, uint packedBackgroundColor, ref LineInstance targetLines) { var v001 = new Vector3(min.X, min.Y, max.Z); var v010 = new Vector3(min.X, max.Y, min.Z); @@ -46,7 +46,7 @@ public static void WriteBoundsLines(in Vector3 min, in Vector3 max, uint packedC Unsafe.Add(ref targetLines, 11) = new LineInstance(v110, max, packedColor, packedBackgroundColor); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void WriteBoundsLines(in Vector3 min, in Vector3 max, in Vector3 color, in Vector3 backgroundColor, ref LineInstance targetLines) + public static void WriteBoundsLines(Vector3 min, Vector3 max, Vector3 color, Vector3 backgroundColor, ref LineInstance targetLines) { WriteBoundsLines(min, max, Helpers.PackColor(color), Helpers.PackColor(backgroundColor), ref targetLines); } diff --git a/DemoRenderer/Constraints/ContactLineExtractor.cs b/DemoRenderer/Constraints/ContactLineExtractor.cs index d112e2f5c..118e77912 100644 --- a/DemoRenderer/Constraints/ContactLineExtractor.cs +++ b/DemoRenderer/Constraints/ContactLineExtractor.cs @@ -10,7 +10,7 @@ namespace DemoRenderer.Constraints public static class ContactLines { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void BuildOrthonormalBasis(in Vector3 normal, out Vector3 t1, out Vector3 t2) + public static void BuildOrthonormalBasis(Vector3 normal, out Vector3 t1, out Vector3 t2) { //No frisvad or friends here- just want a simple and consistent basis with only one singularity. //Could be faster if needed. @@ -26,7 +26,7 @@ public static void BuildOrthonormalBasis(in Vector3 normal, out Vector3 t1, out } public static void Add(in RigidPose poseA, ref Vector3Wide offsetAWide, ref Vector3Wide normalWide, ref Vector depthWide, - in Vector3 tint, ref QuickList lines) + Vector3 tint, ref QuickList lines) { Vector3Wide.ReadFirst(offsetAWide, out var offsetA); Vector3Wide.ReadFirst(normalWide, out var normal); diff --git a/DemoRenderer/Constraints/LineRenderer.cs b/DemoRenderer/Constraints/LineRenderer.cs index 010c4a691..f2be4040d 100644 --- a/DemoRenderer/Constraints/LineRenderer.cs +++ b/DemoRenderer/Constraints/LineRenderer.cs @@ -24,7 +24,7 @@ public struct LineInstance public uint PackedColor; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public LineInstance(in Vector3 start, in Vector3 end, in Vector3 color, in Vector3 backgroundColor) + public LineInstance(Vector3 start, Vector3 end, Vector3 color, Vector3 backgroundColor) { Start = start; PackedBackgroundColor = Helpers.PackColor(backgroundColor); @@ -32,7 +32,7 @@ public LineInstance(in Vector3 start, in Vector3 end, in Vector3 color, in Vecto PackedColor = Helpers.PackColor(color); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public LineInstance(in Vector3 start, in Vector3 end, uint packedColor, uint packedBackgroundColor) + public LineInstance(Vector3 start, Vector3 end, uint packedColor, uint packedBackgroundColor) { Start = start; PackedBackgroundColor = packedBackgroundColor; diff --git a/DemoRenderer/Helpers.cs b/DemoRenderer/Helpers.cs index 9b808c8eb..251092d2f 100644 --- a/DemoRenderer/Helpers.cs +++ b/DemoRenderer/Helpers.cs @@ -272,7 +272,7 @@ public static void UnpackOrientation(ulong packed, out Quaternion orientation) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static bool GetScreenLocation(in Vector3 position, in Matrix viewProjection, in Vector2 resolution, out Vector2 screenLocation) + public static bool GetScreenLocation(Vector3 position, in Matrix viewProjection, in Vector2 resolution, out Vector2 screenLocation) { Matrix.Transform(new Vector4(position, 1), viewProjection, out var projected); projected /= projected.W; @@ -305,7 +305,7 @@ public static float ToSRGB(float x) /// Linear input to apply the curve to. /// Transformed value. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ToSRGB(in Vector3 linear, out Vector3 srgb) + public static void ToSRGB(Vector3 linear, out Vector3 srgb) { srgb = new Vector3(ToSRGB(linear.X), ToSRGB(linear.Y), ToSRGB(linear.Z)); } diff --git a/DemoRenderer/ShapeDrawing/ShapesExtractor.cs b/DemoRenderer/ShapeDrawing/ShapesExtractor.cs index d6e47eee5..b33962fdb 100644 --- a/DemoRenderer/ShapeDrawing/ShapesExtractor.cs +++ b/DemoRenderer/ShapeDrawing/ShapesExtractor.cs @@ -79,7 +79,7 @@ private unsafe void AddCompoundChildren(ref Buffer children, Shap for (int i = 0; i < children.Length; ++i) { ref var child = ref children[i]; - Compound.GetWorldPose(child.LocalPose, pose, out var childPose); + Compound.GetWorldPose(CompoundChild.AsPose(ref child), pose, out var childPose); AddShape(shapes, child.ShapeIndex, childPose, color, ref shapeCache, pool); } } diff --git a/DemoRenderer/UI/UILineBatcher.cs b/DemoRenderer/UI/UILineBatcher.cs index 099b79132..6b421a3e6 100644 --- a/DemoRenderer/UI/UILineBatcher.cs +++ b/DemoRenderer/UI/UILineBatcher.cs @@ -30,7 +30,7 @@ public UILineBatcher(int initialCapacity = 512) lines = new UILineInstance[initialCapacity]; } - public void Draw(in Vector2 start, in Vector2 end, float radius, in Vector3 color) + public void Draw(in Vector2 start, in Vector2 end, float radius, Vector3 color) { if (LineCount == lines.Length) { diff --git a/DemoRenderer/UI/UILineRenderer.cs b/DemoRenderer/UI/UILineRenderer.cs index b94696f26..5b925a0e2 100644 --- a/DemoRenderer/UI/UILineRenderer.cs +++ b/DemoRenderer/UI/UILineRenderer.cs @@ -32,7 +32,7 @@ public struct UILineInstance public uint PackedColor; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public UILineInstance(in Vector2 start, in Vector2 end, float radius, in Vector3 color, in Vector2 screenToPackedScale) + public UILineInstance(in Vector2 start, in Vector2 end, float radius, Vector3 color, in Vector2 screenToPackedScale) { PackedStart = (uint)(start.X * screenToPackedScale.X) | ((uint)(start.Y * screenToPackedScale.Y) << 16); PackedEnd = (uint)(end.X * screenToPackedScale.X) | ((uint)(end.Y * screenToPackedScale.Y) << 16); diff --git a/DemoTests/InertiaTensorTests.cs b/DemoTests/InertiaTensorTests.cs index 89089eb43..0a4de6096 100644 --- a/DemoTests/InertiaTensorTests.cs +++ b/DemoTests/InertiaTensorTests.cs @@ -243,7 +243,7 @@ public bool AllowTest(int childIndex) return true; } - public void OnRayHit(in RayData ray, ref float maximumT, float t, in Vector3 normal, int childIndex) + public void OnRayHit(in RayData ray, ref float maximumT, float t, Vector3 normal, int childIndex) { ++Counter; } diff --git a/Demos/DemoMeshHelper.cs b/Demos/DemoMeshHelper.cs index dd5346742..6f29b81fa 100644 --- a/Demos/DemoMeshHelper.cs +++ b/Demos/DemoMeshHelper.cs @@ -10,7 +10,7 @@ namespace Demos { public static class DemoMeshHelper { - public static void LoadModel(ContentArchive content, BufferPool pool, string contentName, in Vector3 scaling, out Mesh mesh) + public static void LoadModel(ContentArchive content, BufferPool pool, string contentName, Vector3 scaling, out Mesh mesh) { var meshContent = content.Load(contentName); pool.Take(meshContent.Triangles.Length, out var triangles); @@ -21,7 +21,7 @@ public static void LoadModel(ContentArchive content, BufferPool pool, string con mesh = new Mesh(triangles, scaling, pool); } - public static void CreateFan(int triangleCount, float radius, in Vector3 scaling, BufferPool pool, out Mesh mesh) + public static void CreateFan(int triangleCount, float radius, Vector3 scaling, BufferPool pool, out Mesh mesh) { var anglePerTriangle = 2 * MathF.PI / triangleCount; pool.Take(triangleCount, out var triangles); diff --git a/Demos/Demos/Cars/SimpleCar.cs b/Demos/Demos/Cars/SimpleCar.cs index facbbefd3..5ed089f98 100644 --- a/Demos/Demos/Cars/SimpleCar.cs +++ b/Demos/Demos/Cars/SimpleCar.cs @@ -36,8 +36,8 @@ public void SetSpeed(Simulation simulation, in WheelHandles wheel, float speed, } public static WheelHandles CreateWheel(Simulation simulation, CollidableProperty properties, in RigidPose bodyPose, - TypedIndex wheelShape, BodyInertia wheelInertia, float wheelFriction, BodyHandle bodyHandle, ref SubgroupCollisionFilter bodyFilter, in Vector3 bodyToWheelSuspension, in Vector3 suspensionDirection, float suspensionLength, - in AngularHinge hingeDescription, in SpringSettings suspensionSettings, in Quaternion localWheelOrientation) + TypedIndex wheelShape, BodyInertia wheelInertia, float wheelFriction, BodyHandle bodyHandle, ref SubgroupCollisionFilter bodyFilter, Vector3 bodyToWheelSuspension, Vector3 suspensionDirection, float suspensionLength, + in AngularHinge hingeDescription, in SpringSettings suspensionSettings, Quaternion localWheelOrientation) { RigidPose wheelPose; RigidPose.Transform(bodyToWheelSuspension + suspensionDirection * suspensionLength, bodyPose, out wheelPose.Position); @@ -81,8 +81,8 @@ public static WheelHandles CreateWheel(Simulation simulation, CollidableProperty public static SimpleCar Create(Simulation simulation, CollidableProperty properties, in RigidPose pose, TypedIndex bodyShape, BodyInertia bodyInertia, float bodyFriction, TypedIndex wheelShape, BodyInertia wheelInertia, float wheelFriction, - in Vector3 bodyToFrontLeftSuspension, in Vector3 bodyToFrontRightSuspension, in Vector3 bodyToBackLeftSuspension, in Vector3 bodyToBackRightSuspension, - in Vector3 suspensionDirection, float suspensionLength, in SpringSettings suspensionSettings, in Quaternion localWheelOrientation) + Vector3 bodyToFrontLeftSuspension, Vector3 bodyToFrontRightSuspension, Vector3 bodyToBackLeftSuspension, Vector3 bodyToBackRightSuspension, + Vector3 suspensionDirection, float suspensionLength, in SpringSettings suspensionSettings, Quaternion localWheelOrientation) { SimpleCar car; car.Body = simulation.Bodies.Add(BodyDescription.CreateDynamic(pose, bodyInertia, new(bodyShape, 0.5f), 0.01f)); diff --git a/Demos/Demos/CollisionQueryDemo.cs b/Demos/Demos/CollisionQueryDemo.cs index a4a6c47a1..e2664d4d1 100644 --- a/Demos/Demos/CollisionQueryDemo.cs +++ b/Demos/Demos/CollisionQueryDemo.cs @@ -140,7 +140,7 @@ void GetPoseAndShape(CollidableReference reference, out RigidPose pose, out Type /// Pose of the query shape. /// Id to use to refer to this query when the collision batcher finishes processing it. /// Batcher to add the query's tests to. - public unsafe void AddQueryToBatch(int queryShapeType, void* queryShapeData, int queryShapeSize, in Vector3 queryBoundsMin, in Vector3 queryBoundsMax, in RigidPose queryPose, int queryId, ref CollisionBatcher batcher) + public unsafe void AddQueryToBatch(int queryShapeType, void* queryShapeData, int queryShapeSize, Vector3 queryBoundsMin, Vector3 queryBoundsMax, in RigidPose queryPose, int queryId, ref CollisionBatcher batcher) { var broadPhaseEnumerator = new BroadPhaseOverlapEnumerator { Pool = BufferPool, References = new QuickList(16, BufferPool) }; Simulation.BroadPhase.GetOverlaps(queryBoundsMin, queryBoundsMax, ref broadPhaseEnumerator); diff --git a/Demos/Demos/ContactEventsDemo.cs b/Demos/Demos/ContactEventsDemo.cs index 000e5ca64..cdfec2246 100644 --- a/Demos/Demos/ContactEventsDemo.cs +++ b/Demos/Demos/ContactEventsDemo.cs @@ -57,7 +57,7 @@ public interface IContactEventHandler /// Index of the new contact in the contact manifold. /// Index of the worker thread that fired this event. void OnContactAdded(CollidableReference eventSource, CollidablePair pair, ref TManifold contactManifold, - in Vector3 contactOffset, in Vector3 contactNormal, float depth, int featureId, int contactIndex, int workerIndex) where TManifold : unmanaged, IContactManifold + Vector3 contactOffset, Vector3 contactNormal, float depth, int featureId, int contactIndex, int workerIndex) where TManifold : unmanaged, IContactManifold { } @@ -667,7 +667,7 @@ public EventHandler(Simulation simulation, BufferPool pool) } public void OnContactAdded(CollidableReference eventSource, CollidablePair pair, ref TManifold contactManifold, - in Vector3 contactOffset, in Vector3 contactNormal, float depth, int featureId, int contactIndex, int workerIndex) where TManifold : unmanaged, IContactManifold + Vector3 contactOffset, Vector3 contactNormal, float depth, int featureId, int contactIndex, int workerIndex) where TManifold : unmanaged, IContactManifold { //Simply ignore any particles beyond the allocated space. var index = Interlocked.Increment(ref Particles.Count) - 1; diff --git a/Demos/Demos/CustomVoxelCollidableDemo.cs b/Demos/Demos/CustomVoxelCollidableDemo.cs index dd95cfec8..363eadcac 100644 --- a/Demos/Demos/CustomVoxelCollidableDemo.cs +++ b/Demos/Demos/CustomVoxelCollidableDemo.cs @@ -80,7 +80,7 @@ public readonly ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity return new HomogeneousCompoundShapeBatch(pool, initialCapacity); } - public readonly void ComputeBounds(in Quaternion orientation, out Vector3 min, out Vector3 max) + public readonly void ComputeBounds(Quaternion orientation, out Vector3 min, out Vector3 max) { Matrix3x3.CreateFromQuaternion(orientation, out var basis); min = new Vector3(float.MaxValue); @@ -226,7 +226,7 @@ public readonly unsafe void FindLocalOverlaps(ref B } } - public readonly unsafe void FindLocalOverlaps(in Vector3 min, in Vector3 max, in Vector3 sweep, float maximumT, BufferPool pool, Shapes shapes, void* overlaps) where TOverlaps : ICollisionTaskSubpairOverlaps + public readonly unsafe void FindLocalOverlaps(Vector3 min, Vector3 max, Vector3 sweep, float maximumT, BufferPool pool, Shapes shapes, void* overlaps) where TOverlaps : ICollisionTaskSubpairOverlaps { //Similar to the non-swept FindLocalOverlaps function above, this just adds the overlaps to the provided collection. //Some unfortunate loss of type information due to some language limitations around generic pointers- pretend the overlaps pointer has type TOverlaps*. @@ -333,7 +333,7 @@ public unsafe void GetChildAData(ref CollisionBatcher co { ref var compoundA = ref Unsafe.AsRef(pair.A); ref var compoundChildA = ref compoundA.GetChild(childIndexA); - Compound.GetRotatedChildPose(compoundChildA.LocalPose, pair.OrientationA, out childPoseA); + Compound.GetRotatedChildPose(CompoundChild.AsPose(ref compoundChildA), pair.OrientationA, out childPoseA); childTypeA = compoundChildA.ShapeIndex.Type; collisionBatcher.Shapes[childTypeA].GetShapeData(compoundChildA.ShapeIndex.Index, out childShapeDataA, out _); } diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index d4acc4880..d956ac457 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -667,7 +667,7 @@ private static unsafe int CreateHexahedralUniqueEdgesList(ref Buffer filters, + internal unsafe static void CreateDeformable(Simulation simulation, Vector3 position, Quaternion orientation, float density, float cellSize, in SpringSettings weldSpringiness, in SpringSettings volumeSpringiness, int instanceId, CollidableProperty filters, ref Buffer vertices, ref CellSet vertexSpatialIndices, ref Buffer cellVertexIndices, ref Buffer tetrahedraVertexIndices) { var pool = simulation.BufferPool; diff --git a/Demos/Demos/RagdollDemo.cs b/Demos/Demos/RagdollDemo.cs index de119d877..d2eb3c49f 100644 --- a/Demos/Demos/RagdollDemo.cs +++ b/Demos/Demos/RagdollDemo.cs @@ -181,7 +181,7 @@ public static void GetCapsuleForLineSegment(Vector3 start, Vector3 end, float ra orientation = crossLength > 1e-8f ? QuaternionEx.CreateFromAxisAngle(cross / crossLength, (float)Math.Asin(crossLength)) : Quaternion.Identity; } - public static Quaternion CreateBasis(in Vector3 z, in Vector3 x) + public static Quaternion CreateBasis(Vector3 z, Vector3 x) { //For ease of use, don't assume that x is perpendicular to z, nor that either input is normalized. Matrix3x3 basis; diff --git a/Demos/Demos/RayCastingDemo.cs b/Demos/Demos/RayCastingDemo.cs index 0e1cc893d..b5878394b 100644 --- a/Demos/Demos/RayCastingDemo.cs +++ b/Demos/Demos/RayCastingDemo.cs @@ -359,7 +359,7 @@ public bool AllowTest(CollidableReference collidable, int childIndex) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void OnRayHit(in RayData ray, ref float maximumT, float t, in Vector3 normal, CollidableReference collidable, int childIndex) + public void OnRayHit(in RayData ray, ref float maximumT, float t, Vector3 normal, CollidableReference collidable, int childIndex) { maximumT = t; ref var hit = ref Hits[ray.Id]; diff --git a/Demos/Demos/RopeStabilityDemo.cs b/Demos/Demos/RopeStabilityDemo.cs index 10acf6296..6fc2ee8b1 100644 --- a/Demos/Demos/RopeStabilityDemo.cs +++ b/Demos/Demos/RopeStabilityDemo.cs @@ -19,7 +19,7 @@ namespace Demos.Demos /// public class RopeStabilityDemo : Demo { - public static BodyHandle[] BuildRopeBodies(Simulation simulation, in Vector3 start, int bodyCount, float bodySize, float bodySpacing, float massPerBody, float inverseInertiaScale) + public static BodyHandle[] BuildRopeBodies(Simulation simulation, Vector3 start, int bodyCount, float bodySize, float bodySpacing, float massPerBody, float inverseInertiaScale) { BodyHandle[] handles = new BodyHandle[bodyCount + 1]; var ropeShape = new Sphere(bodySize); @@ -42,7 +42,7 @@ public static BodyHandle[] BuildRopeBodies(Simulation simulation, in Vector3 sta return handles; } - public static BodyHandle[] BuildRope(Simulation simulation, in Vector3 start, int bodyCount, float bodySize, float bodySpacing, float constraintOffsetLength, float massPerBody, float inverseInertiaScale, SpringSettings springSettings) + public static BodyHandle[] BuildRope(Simulation simulation, Vector3 start, int bodyCount, float bodySize, float bodySpacing, float constraintOffsetLength, float massPerBody, float inverseInertiaScale, SpringSettings springSettings) { var handles = BuildRopeBodies(simulation, start, bodyCount, bodySize, bodySpacing, massPerBody, inverseInertiaScale); var maximumDistance = 2 * bodySize + bodySpacing - 2 * constraintOffsetLength; diff --git a/Demos/Demos/Sponsors/SponsorCharacterAI.cs b/Demos/Demos/Sponsors/SponsorCharacterAI.cs index d10929be4..6ac467f07 100644 --- a/Demos/Demos/Sponsors/SponsorCharacterAI.cs +++ b/Demos/Demos/Sponsors/SponsorCharacterAI.cs @@ -13,7 +13,7 @@ public struct SponsorCharacterAI { BodyHandle bodyHandle; Vector2 targetLocation; - public SponsorCharacterAI(CharacterControllers characters, in CollidableDescription characterCollidable, in Vector3 initialPosition, in Vector2 targetLocation) + public SponsorCharacterAI(CharacterControllers characters, in CollidableDescription characterCollidable, Vector3 initialPosition, in Vector2 targetLocation) { bodyHandle = characters.Simulation.Bodies.Add(BodyDescription.CreateDynamic(initialPosition, new BodyInertia { InverseMass = 1f }, characterCollidable, -1f)); diff --git a/Demos/Demos/SweepDemo.cs b/Demos/Demos/SweepDemo.cs index 666db15ef..4d1b6eccf 100644 --- a/Demos/Demos/SweepDemo.cs +++ b/Demos/Demos/SweepDemo.cs @@ -111,7 +111,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) } - void DrawShape(ref TShape shape, in RigidPose pose, in Vector3 color, Shapes shapes, Renderer renderer) + void DrawShape(ref TShape shape, in RigidPose pose, Vector3 color, Shapes shapes, Renderer renderer) where TShape : struct, IShape { if (typeof(TShape) == typeof(Triangle)) @@ -133,7 +133,7 @@ void DrawShape(ref TShape shape, in RigidPose pose, in Vector3 color, Sh } unsafe void DrawSweep(TShape shape, in RigidPose pose, in BodyVelocity velocity, int steps, - float t, Renderer renderer, in Vector3 color) + float t, Renderer renderer, Vector3 color) where TShape : struct, IShape { if (steps == 1) @@ -225,7 +225,7 @@ public bool AllowTest(CollidableReference collidable, int child) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void OnHit(ref float maximumT, float t, in Vector3 hitLocation, in Vector3 hitNormal, CollidableReference collidable) + public void OnHit(ref float maximumT, float t, Vector3 hitLocation, Vector3 hitNormal, CollidableReference collidable) { //Changing the maximum T value prevents the traversal from visiting any leaf nodes more distant than that later in the traversal. //It is effectively an optimization that you can use if you only care about the time of first impact. @@ -249,7 +249,7 @@ public void OnHitAtZeroT(ref float maximumT, CollidableReference collidable) } } - void StandardTestSweep(in TA a, in TB b, ref Vector3 position, in Quaternion initialOrientationA, in Quaternion initialOrientationB, Renderer renderer) + void StandardTestSweep(in TA a, in TB b, ref Vector3 position, Quaternion initialOrientationA, Quaternion initialOrientationB, Renderer renderer) where TA : struct, IShape where TB : struct, IShape { TestSweep( diff --git a/Demos/Demos/Tanks/Tank.cs b/Demos/Demos/Tanks/Tank.cs index 9df57e1c6..be46b9c61 100644 --- a/Demos/Demos/Tanks/Tank.cs +++ b/Demos/Demos/Tanks/Tank.cs @@ -108,7 +108,7 @@ public void SetSpeed(Simulation simulation, Buffer motors, flo /// Simulation containing the tank. /// Direction to aim in. /// Swivel and pitch angles to point in the given direction. - public readonly (float targetSwivelAngle, float targetPitchAngle) ComputeTurretAngles(Simulation simulation, in Vector3 aimDirection) + public readonly (float targetSwivelAngle, float targetPitchAngle) ComputeTurretAngles(Simulation simulation, Vector3 aimDirection) { //Decompose the aim direction into target angles for the turret and barrel servos. //First, we need to compute the frame of reference and transform the aim direction into the tank's local space. @@ -182,8 +182,8 @@ public BodyHandle Fire(Simulation simulation, CollidableProperty properties, in RigidPose tankPose, in RigidPose bodyLocalPose, - TypedIndex wheelShape, BodyInertia wheelInertia, float wheelFriction, BodyHandle bodyHandle, ref SubgroupCollisionFilter bodyFilter, in Vector3 bodyToWheelSuspension, float suspensionLength, - in SpringSettings suspensionSettings, in Quaternion localWheelOrientation, + TypedIndex wheelShape, BodyInertia wheelInertia, float wheelFriction, BodyHandle bodyHandle, ref SubgroupCollisionFilter bodyFilter, Vector3 bodyToWheelSuspension, float suspensionLength, + in SpringSettings suspensionSettings, Quaternion localWheelOrientation, ref QuickList wheelHandles, ref QuickList constraints, ref QuickList motors) { RigidPose wheelPose; diff --git a/Demos/Demos/Tanks/TankController.cs b/Demos/Demos/Tanks/TankController.cs index b402606b0..f77a96dab 100644 --- a/Demos/Demos/Tanks/TankController.cs +++ b/Demos/Demos/Tanks/TankController.cs @@ -49,7 +49,7 @@ public TankController(Tank tank, /// Whether the left tread should brake. /// Whether the right tread should brake. /// Direction that the tank's barrel should point. - public void UpdateMovementAndAim(Simulation simulation, float leftTargetSpeedFraction, float rightTargetSpeedFraction, bool zoom, bool brakeLeft, bool brakeRight, in Vector3 aimDirection) + public void UpdateMovementAndAim(Simulation simulation, float leftTargetSpeedFraction, float rightTargetSpeedFraction, bool zoom, bool brakeLeft, bool brakeRight, Vector3 aimDirection) { var leftTargetSpeed = brakeLeft ? 0 : leftTargetSpeedFraction * Speed; var rightTargetSpeed = brakeRight ? 0 : rightTargetSpeedFraction * Speed; diff --git a/Demos/Grabber.cs b/Demos/Grabber.cs index ac3b9ce03..1b61f2fc6 100644 --- a/Demos/Grabber.cs +++ b/Demos/Grabber.cs @@ -38,7 +38,7 @@ public bool AllowTest(CollidableReference collidable, int childIndex) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void OnRayHit(in RayData ray, ref float maximumT, float t, in Vector3 normal, CollidableReference collidable, int childIndex) + public void OnRayHit(in RayData ray, ref float maximumT, float t, Vector3 normal, CollidableReference collidable, int childIndex) { //We are only interested in the earliest hit. This callback is executing within the traversal, so modifying maximumT informs the traversal //that it can skip any AABBs which are more distant than the new maximumT. @@ -49,7 +49,7 @@ public void OnRayHit(in RayData ray, ref float maximumT, float t, in Vector3 nor } } - readonly void CreateMotorDescription(in Vector3 target, float inverseMass, out OneBodyLinearServo linearDescription, out OneBodyAngularServo angularDescription) + readonly void CreateMotorDescription(Vector3 target, float inverseMass, out OneBodyLinearServo linearDescription, out OneBodyAngularServo angularDescription) { linearDescription = new OneBodyLinearServo { @@ -66,7 +66,7 @@ readonly void CreateMotorDescription(in Vector3 target, float inverseMass, out O }; } - public void Update(Simulation simulation, Camera camera, bool mouseLocked, bool shouldGrab, in Quaternion rotation, in Vector2 normalizedMousePosition) + public void Update(Simulation simulation, Camera camera, bool mouseLocked, bool shouldGrab, Quaternion rotation, in Vector2 normalizedMousePosition) { //On the off chance some demo modifies the kinematic state, treat that as a grab terminator. var bodyExists = body.Exists && !body.Kinematic; diff --git a/Demos/RolloverInfo.cs b/Demos/RolloverInfo.cs index 25d3db346..942e692d4 100644 --- a/Demos/RolloverInfo.cs +++ b/Demos/RolloverInfo.cs @@ -26,7 +26,7 @@ public RolloverInfo() descriptions = new List(); } - public void Add(in Vector3 position, string description, float previewOffset = -1.2f, string previewText = "Info...") + public void Add(Vector3 position, string description, float previewOffset = -1.2f, string previewText = "Info...") { this.descriptions.Add(new RolloverDescription { Position = position, Description = description, PreviewOffset = previewOffset, Preview = previewText }); } diff --git a/Demos/SpecializedTests/CompoundBoundTests.cs b/Demos/SpecializedTests/CompoundBoundTests.cs index 0eb474d3e..7d031ad2d 100644 --- a/Demos/SpecializedTests/CompoundBoundTests.cs +++ b/Demos/SpecializedTests/CompoundBoundTests.cs @@ -24,7 +24,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) } - void GetArcExpansion(in Vector3 offset, in Vector3 angularVelocity, float dt, out Vector3 minExpansion, out Vector3 maxExpansion) + void GetArcExpansion(Vector3 offset, Vector3 angularVelocity, float dt, out Vector3 minExpansion, out Vector3 maxExpansion) { //minExpansion = default; //maxExpansion = default; @@ -128,7 +128,7 @@ void GetArcExpansion(in Vector3 offset, in Vector3 angularVelocity, float dt, ou maxExpansion = maxExpansion - max; } - void GetEstimatedExpansion(in Vector3 localPoseA, in Vector3 angularVelocityA, in Vector3 offsetB, in Vector3 angularVelocityB, float dt, out Vector3 minExpansion, out Vector3 maxExpansion) + void GetEstimatedExpansion(Vector3 localPoseA, Vector3 angularVelocityA, Vector3 offsetB, Vector3 angularVelocityB, float dt, out Vector3 minExpansion, out Vector3 maxExpansion) { GetArcExpansion(localPoseA, angularVelocityA, dt, out var minExpansionA, out var maxExpansionA); GetArcExpansion(-offsetB, -angularVelocityB, dt, out var minExpansionB, out var maxExpansionB); diff --git a/Demos/SpecializedTests/CylinderTestDemo.cs b/Demos/SpecializedTests/CylinderTestDemo.cs index b832db05a..bbf039c00 100644 --- a/Demos/SpecializedTests/CylinderTestDemo.cs +++ b/Demos/SpecializedTests/CylinderTestDemo.cs @@ -13,7 +13,7 @@ namespace Demos.SpecializedTests { public class CylinderTestDemo : Demo { - private static void BruteForceSearch(in Vector3 lineOrigin, in Vector3 lineDirection, float halfLength, in Cylinder cylinder, out float closestT, out float closestDistanceSquared, out float errorMargin) + private static void BruteForceSearch(Vector3 lineOrigin, Vector3 lineDirection, float halfLength, in Cylinder cylinder, out float closestT, out float closestDistanceSquared, out float errorMargin) { const int sampleCount = 1 << 20; var inverseSampleCount = 1.0 / (sampleCount - 1); diff --git a/Demos/SpecializedTests/Media/2.0/BedsheetDemo.cs b/Demos/SpecializedTests/Media/2.0/BedsheetDemo.cs index a65fe64a7..ef88ecc9c 100644 --- a/Demos/SpecializedTests/Media/2.0/BedsheetDemo.cs +++ b/Demos/SpecializedTests/Media/2.0/BedsheetDemo.cs @@ -19,7 +19,7 @@ public class BedsheetDemo : Demo { delegate bool KinematicDecider(int rowIndex, int columnIndex, int width, int height); - BodyHandle[,] CreateBodyGrid(in Vector3 position, in Quaternion orientation, int width, int height, float spacing, float bodyRadius, float massPerBody, + BodyHandle[,] CreateBodyGrid(Vector3 position, Quaternion orientation, int width, int height, float spacing, float bodyRadius, float massPerBody, int instanceId, CollidableProperty filters, KinematicDecider isKinematic) { var description = BodyDescription.CreateKinematic(orientation, Simulation.Shapes.Add(new Sphere(bodyRadius)), 0.01f); diff --git a/Demos/SpecializedTests/Media/2.0/NewtDemandingSacrificeVideoDemo.cs b/Demos/SpecializedTests/Media/2.0/NewtDemandingSacrificeVideoDemo.cs index 5e16bd365..d5b5e2c43 100644 --- a/Demos/SpecializedTests/Media/2.0/NewtDemandingSacrificeVideoDemo.cs +++ b/Demos/SpecializedTests/Media/2.0/NewtDemandingSacrificeVideoDemo.cs @@ -38,7 +38,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Random random = new Random(5); int ragdollIndex = 0; - BodyVelocity GetRandomizedVelocity(in Vector3 linearVelocity) + BodyVelocity GetRandomizedVelocity(Vector3 linearVelocity) { return new BodyVelocity { Linear = linearVelocity, Angular = new Vector3(-20) + 40 * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) }; } diff --git a/Demos/SpecializedTests/MinkowskiVisualizer.cs b/Demos/SpecializedTests/MinkowskiVisualizer.cs index 9062deef5..b88504fd7 100644 --- a/Demos/SpecializedTests/MinkowskiVisualizer.cs +++ b/Demos/SpecializedTests/MinkowskiVisualizer.cs @@ -14,7 +14,7 @@ namespace Demos.SpecializedTests { public static class SimplexVisualizer { - public static void Draw(Renderer renderer, Buffer simplex, in Vector3 position, in Vector3 lineColor, in Vector3 backgroundColor) + public static void Draw(Renderer renderer, Buffer simplex, Vector3 position, Vector3 lineColor, Vector3 backgroundColor) { var packedLineColor = Helpers.PackColor(lineColor); var packedBackgroundColor = Helpers.PackColor(backgroundColor); @@ -60,8 +60,8 @@ static void FindSupport CreateLines( in TShapeA a, in TShapeB b, in RigidPose poseA, in RigidPose poseB, int sampleCount, - float lineLength, in Vector3 lineColor, - float originLength, in Vector3 originColor, in Vector3 backgroundColor, in Vector3 basePosition, BufferPool pool) + float lineLength, Vector3 lineColor, + float originLength, Vector3 originColor, Vector3 backgroundColor, Vector3 basePosition, BufferPool pool) where TShapeA : unmanaged, IConvexShape where TShapeWideA : unmanaged, IShapeWide where TSupportFinderA : struct, ISupportFinder diff --git a/Demos/SpecializedTests/TriangleRayTestDemo.cs b/Demos/SpecializedTests/TriangleRayTestDemo.cs index 3b89c9629..d31c12396 100644 --- a/Demos/SpecializedTests/TriangleRayTestDemo.cs +++ b/Demos/SpecializedTests/TriangleRayTestDemo.cs @@ -63,7 +63,7 @@ void GetPointOutsideTriangle(Random random, in Triangle triangle, in RigidPose p pointOutsideTriangle += pose.Position; } - void TestRay(in Triangle triangle, in RigidPose pose, in Vector3 rayOrigin, in Vector3 rayDirection, bool expectedImpact, in Vector3 pointOnTrianglePlane) + void TestRay(in Triangle triangle, in RigidPose pose, Vector3 rayOrigin, Vector3 rayDirection, bool expectedImpact, Vector3 pointOnTrianglePlane) { var hit = triangle.RayTest(pose, rayOrigin, rayDirection, out var t, out var normal); From 8f0cad80be10317ca3606642f5f9c4e6e754d3d0 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 9 Aug 2022 17:00:19 -0500 Subject: [PATCH 512/947] Purging some reflection. --- .../Constraints/AngularAxisGearMotor.cs | 1 + BepuPhysics/Constraints/AngularAxisMotor.cs | 1 + BepuPhysics/Constraints/AngularHinge.cs | 1 + BepuPhysics/Constraints/AngularMotor.cs | 1 + BepuPhysics/Constraints/AngularServo.cs | 1 + BepuPhysics/Constraints/AngularSwivelHinge.cs | 1 + BepuPhysics/Constraints/AreaConstraint.cs | 1 + BepuPhysics/Constraints/BallSocket.cs | 1 + BepuPhysics/Constraints/BallSocketMotor.cs | 1 + BepuPhysics/Constraints/BallSocketServo.cs | 1 + .../Constraints/CenterDistanceConstraint.cs | 1 + .../Constraints/CenterDistanceLimit.cs | 1 + .../Constraints/Contact/ContactConvexTypes.cs | 24 ++++++++++++------- .../Constraints/Contact/ContactConvexTypes.tt | 3 ++- .../Contact/ContactNonconvexTypes.cs | 18 +++++++++----- .../Contact/ContactNonconvexTypes.tt | 6 +++-- BepuPhysics/Constraints/DistanceLimit.cs | 1 + BepuPhysics/Constraints/DistanceServo.cs | 1 + BepuPhysics/Constraints/Hinge.cs | 1 + .../Constraints/IConstraintDescription.cs | 4 ++++ BepuPhysics/Constraints/LinearAxisLimit.cs | 1 + BepuPhysics/Constraints/LinearAxisMotor.cs | 1 + BepuPhysics/Constraints/LinearAxisServo.cs | 1 + .../Constraints/OneBodyAngularMotor.cs | 1 + .../Constraints/OneBodyAngularServo.cs | 1 + BepuPhysics/Constraints/OneBodyLinearMotor.cs | 1 + BepuPhysics/Constraints/OneBodyLinearServo.cs | 1 + BepuPhysics/Constraints/PointOnLineServo.cs | 1 + BepuPhysics/Constraints/SwingLimit.cs | 1 + BepuPhysics/Constraints/SwivelHinge.cs | 1 + BepuPhysics/Constraints/TwistLimit.cs | 1 + BepuPhysics/Constraints/TwistMotor.cs | 1 + BepuPhysics/Constraints/TwistServo.cs | 1 + BepuPhysics/Constraints/VolumeConstraint.cs | 1 + BepuPhysics/Constraints/Weld.cs | 1 + BepuPhysics/Solver.cs | 8 +++---- .../Characters/CharacterMotionConstraint.cs | 12 ++++++++-- .../Characters/CharacterMotionConstraint.tt | 6 ++++- 38 files changed, 87 insertions(+), 24 deletions(-) diff --git a/BepuPhysics/Constraints/AngularAxisGearMotor.cs b/BepuPhysics/Constraints/AngularAxisGearMotor.cs index 0569bacb1..0c6ccaa11 100644 --- a/BepuPhysics/Constraints/AngularAxisGearMotor.cs +++ b/BepuPhysics/Constraints/AngularAxisGearMotor.cs @@ -38,6 +38,7 @@ public readonly int ConstraintTypeId } public readonly Type TypeProcessorType => typeof(AngularAxisGearMotorTypeProcessor); + public readonly TypeProcessor CreateTypeProcessor() => new AngularAxisGearMotorTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { diff --git a/BepuPhysics/Constraints/AngularAxisMotor.cs b/BepuPhysics/Constraints/AngularAxisMotor.cs index fea064a6a..e60722265 100644 --- a/BepuPhysics/Constraints/AngularAxisMotor.cs +++ b/BepuPhysics/Constraints/AngularAxisMotor.cs @@ -37,6 +37,7 @@ public readonly int ConstraintTypeId } public readonly Type TypeProcessorType => typeof(AngularAxisMotorTypeProcessor); + public readonly TypeProcessor CreateTypeProcessor() => new AngularAxisMotorTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { diff --git a/BepuPhysics/Constraints/AngularHinge.cs b/BepuPhysics/Constraints/AngularHinge.cs index 50704d242..a0aef07f8 100644 --- a/BepuPhysics/Constraints/AngularHinge.cs +++ b/BepuPhysics/Constraints/AngularHinge.cs @@ -36,6 +36,7 @@ public readonly int ConstraintTypeId } public readonly Type TypeProcessorType => typeof(AngularHingeTypeProcessor); + public readonly TypeProcessor CreateTypeProcessor() => new AngularHingeTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { diff --git a/BepuPhysics/Constraints/AngularMotor.cs b/BepuPhysics/Constraints/AngularMotor.cs index 7083e7b0b..6670bd5c0 100644 --- a/BepuPhysics/Constraints/AngularMotor.cs +++ b/BepuPhysics/Constraints/AngularMotor.cs @@ -32,6 +32,7 @@ public readonly int ConstraintTypeId } public readonly Type TypeProcessorType => typeof(AngularMotorTypeProcessor); + public readonly TypeProcessor CreateTypeProcessor() => new AngularMotorTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { diff --git a/BepuPhysics/Constraints/AngularServo.cs b/BepuPhysics/Constraints/AngularServo.cs index f1a64577d..e1a8a7273 100644 --- a/BepuPhysics/Constraints/AngularServo.cs +++ b/BepuPhysics/Constraints/AngularServo.cs @@ -36,6 +36,7 @@ public readonly int ConstraintTypeId } public readonly Type TypeProcessorType => typeof(AngularServoTypeProcessor); + public readonly TypeProcessor CreateTypeProcessor() => new AngularServoTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { diff --git a/BepuPhysics/Constraints/AngularSwivelHinge.cs b/BepuPhysics/Constraints/AngularSwivelHinge.cs index bb7994769..490b59059 100644 --- a/BepuPhysics/Constraints/AngularSwivelHinge.cs +++ b/BepuPhysics/Constraints/AngularSwivelHinge.cs @@ -36,6 +36,7 @@ public readonly int ConstraintTypeId } public readonly Type TypeProcessorType => typeof(AngularSwivelHingeTypeProcessor); + public readonly TypeProcessor CreateTypeProcessor() => new AngularSwivelHingeTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { diff --git a/BepuPhysics/Constraints/AreaConstraint.cs b/BepuPhysics/Constraints/AreaConstraint.cs index d52b1c47e..28cce5e50 100644 --- a/BepuPhysics/Constraints/AreaConstraint.cs +++ b/BepuPhysics/Constraints/AreaConstraint.cs @@ -47,6 +47,7 @@ public readonly int ConstraintTypeId } public readonly Type TypeProcessorType => typeof(AreaConstraintTypeProcessor); + public readonly TypeProcessor CreateTypeProcessor() => new AreaConstraintTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { diff --git a/BepuPhysics/Constraints/BallSocket.cs b/BepuPhysics/Constraints/BallSocket.cs index ce3278e06..b46c3d3e2 100644 --- a/BepuPhysics/Constraints/BallSocket.cs +++ b/BepuPhysics/Constraints/BallSocket.cs @@ -36,6 +36,7 @@ public readonly int ConstraintTypeId } public readonly Type TypeProcessorType => typeof(BallSocketTypeProcessor); + public readonly TypeProcessor CreateTypeProcessor() => new BallSocketTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { diff --git a/BepuPhysics/Constraints/BallSocketMotor.cs b/BepuPhysics/Constraints/BallSocketMotor.cs index 6042a6d57..846ffc2dd 100644 --- a/BepuPhysics/Constraints/BallSocketMotor.cs +++ b/BepuPhysics/Constraints/BallSocketMotor.cs @@ -39,6 +39,7 @@ public readonly int ConstraintTypeId } public readonly Type TypeProcessorType => typeof(BallSocketMotorTypeProcessor); + public readonly TypeProcessor CreateTypeProcessor() => new BallSocketMotorTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { diff --git a/BepuPhysics/Constraints/BallSocketServo.cs b/BepuPhysics/Constraints/BallSocketServo.cs index b85fc516f..bc62e46f3 100644 --- a/BepuPhysics/Constraints/BallSocketServo.cs +++ b/BepuPhysics/Constraints/BallSocketServo.cs @@ -41,6 +41,7 @@ public readonly int ConstraintTypeId } public readonly Type TypeProcessorType => typeof(BallSocketServoTypeProcessor); + public readonly TypeProcessor CreateTypeProcessor() => new BallSocketServoTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { diff --git a/BepuPhysics/Constraints/CenterDistanceConstraint.cs b/BepuPhysics/Constraints/CenterDistanceConstraint.cs index ff81896bc..1fa4923d3 100644 --- a/BepuPhysics/Constraints/CenterDistanceConstraint.cs +++ b/BepuPhysics/Constraints/CenterDistanceConstraint.cs @@ -40,6 +40,7 @@ public readonly int ConstraintTypeId } public readonly Type TypeProcessorType => typeof(CenterDistanceTypeProcessor); + public readonly TypeProcessor CreateTypeProcessor() => new CenterDistanceTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { diff --git a/BepuPhysics/Constraints/CenterDistanceLimit.cs b/BepuPhysics/Constraints/CenterDistanceLimit.cs index 7694ccbd9..d916bf6b1 100644 --- a/BepuPhysics/Constraints/CenterDistanceLimit.cs +++ b/BepuPhysics/Constraints/CenterDistanceLimit.cs @@ -45,6 +45,7 @@ public readonly int ConstraintTypeId } public readonly Type TypeProcessorType => typeof(CenterDistanceLimitTypeProcessor); + public readonly TypeProcessor CreateTypeProcessor() => new CenterDistanceLimitTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { diff --git a/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs b/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs index 764325799..a450d287b 100644 --- a/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs +++ b/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs @@ -249,8 +249,9 @@ public readonly int ConstraintTypeId [MethodImpl(MethodImplOptions.AggressiveInlining)] get => Contact1OneBodyTypeProcessor.BatchTypeId; } - + public readonly Type TypeProcessorType => typeof(Contact1OneBodyTypeProcessor); + public readonly TypeProcessor CreateTypeProcessor() => new Contact1OneBodyTypeProcessor(); } @@ -396,8 +397,9 @@ public readonly int ConstraintTypeId [MethodImpl(MethodImplOptions.AggressiveInlining)] get => Contact2OneBodyTypeProcessor.BatchTypeId; } - + public readonly Type TypeProcessorType => typeof(Contact2OneBodyTypeProcessor); + public readonly TypeProcessor CreateTypeProcessor() => new Contact2OneBodyTypeProcessor(); } @@ -555,8 +557,9 @@ public readonly int ConstraintTypeId [MethodImpl(MethodImplOptions.AggressiveInlining)] get => Contact3OneBodyTypeProcessor.BatchTypeId; } - + public readonly Type TypeProcessorType => typeof(Contact3OneBodyTypeProcessor); + public readonly TypeProcessor CreateTypeProcessor() => new Contact3OneBodyTypeProcessor(); } @@ -724,8 +727,9 @@ public readonly int ConstraintTypeId [MethodImpl(MethodImplOptions.AggressiveInlining)] get => Contact4OneBodyTypeProcessor.BatchTypeId; } - + public readonly Type TypeProcessorType => typeof(Contact4OneBodyTypeProcessor); + public readonly TypeProcessor CreateTypeProcessor() => new Contact4OneBodyTypeProcessor(); } @@ -888,8 +892,9 @@ public readonly int ConstraintTypeId [MethodImpl(MethodImplOptions.AggressiveInlining)] get => Contact1TypeProcessor.BatchTypeId; } - + public readonly Type TypeProcessorType => typeof(Contact1TypeProcessor); + public readonly TypeProcessor CreateTypeProcessor() => new Contact1TypeProcessor(); } @@ -1048,8 +1053,9 @@ public readonly int ConstraintTypeId [MethodImpl(MethodImplOptions.AggressiveInlining)] get => Contact2TypeProcessor.BatchTypeId; } - + public readonly Type TypeProcessorType => typeof(Contact2TypeProcessor); + public readonly TypeProcessor CreateTypeProcessor() => new Contact2TypeProcessor(); } @@ -1220,8 +1226,9 @@ public readonly int ConstraintTypeId [MethodImpl(MethodImplOptions.AggressiveInlining)] get => Contact3TypeProcessor.BatchTypeId; } - + public readonly Type TypeProcessorType => typeof(Contact3TypeProcessor); + public readonly TypeProcessor CreateTypeProcessor() => new Contact3TypeProcessor(); } @@ -1402,8 +1409,9 @@ public readonly int ConstraintTypeId [MethodImpl(MethodImplOptions.AggressiveInlining)] get => Contact4TypeProcessor.BatchTypeId; } - + public readonly Type TypeProcessorType => typeof(Contact4TypeProcessor); + public readonly TypeProcessor CreateTypeProcessor() => new Contact4TypeProcessor(); } diff --git a/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt b/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt index fd86fbfcb..368317574 100644 --- a/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt +++ b/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt @@ -165,8 +165,9 @@ for (int i = 0; i < contactCount; ++i) [MethodImpl(MethodImplOptions.AggressiveInlining)] get => Contact<#= contactCount #><#=suffix#>TypeProcessor.BatchTypeId; } - + public readonly Type TypeProcessorType => typeof(Contact<#= contactCount #><#=suffix#>TypeProcessor); + public readonly TypeProcessor CreateTypeProcessor() => new Contact<#= contactCount #><#=suffix#>TypeProcessor(); } diff --git a/BepuPhysics/Constraints/Contact/ContactNonconvexTypes.cs b/BepuPhysics/Constraints/Contact/ContactNonconvexTypes.cs index 5b8c29ca7..1e9a435fa 100644 --- a/BepuPhysics/Constraints/Contact/ContactNonconvexTypes.cs +++ b/BepuPhysics/Constraints/Contact/ContactNonconvexTypes.cs @@ -49,8 +49,9 @@ public readonly int ConstraintTypeId [MethodImpl(MethodImplOptions.AggressiveInlining)] get => Contact2NonconvexTypeProcessor.BatchTypeId; } - + public readonly Type TypeProcessorType => typeof(Contact2NonconvexTypeProcessor); + public readonly TypeProcessor CreateTypeProcessor() => new Contact2NonconvexTypeProcessor(); } @@ -151,8 +152,9 @@ public readonly int ConstraintTypeId [MethodImpl(MethodImplOptions.AggressiveInlining)] get => Contact2NonconvexOneBodyTypeProcessor.BatchTypeId; } - + public readonly Type TypeProcessorType => typeof(Contact2NonconvexOneBodyTypeProcessor); + public readonly TypeProcessor CreateTypeProcessor() => new Contact2NonconvexOneBodyTypeProcessor(); } @@ -236,8 +238,9 @@ public readonly int ConstraintTypeId [MethodImpl(MethodImplOptions.AggressiveInlining)] get => Contact3NonconvexTypeProcessor.BatchTypeId; } - + public readonly Type TypeProcessorType => typeof(Contact3NonconvexTypeProcessor); + public readonly TypeProcessor CreateTypeProcessor() => new Contact3NonconvexTypeProcessor(); } @@ -341,8 +344,9 @@ public readonly int ConstraintTypeId [MethodImpl(MethodImplOptions.AggressiveInlining)] get => Contact3NonconvexOneBodyTypeProcessor.BatchTypeId; } - + public readonly Type TypeProcessorType => typeof(Contact3NonconvexOneBodyTypeProcessor); + public readonly TypeProcessor CreateTypeProcessor() => new Contact3NonconvexOneBodyTypeProcessor(); } @@ -428,8 +432,9 @@ public readonly int ConstraintTypeId [MethodImpl(MethodImplOptions.AggressiveInlining)] get => Contact4NonconvexTypeProcessor.BatchTypeId; } - + public readonly Type TypeProcessorType => typeof(Contact4NonconvexTypeProcessor); + public readonly TypeProcessor CreateTypeProcessor() => new Contact4NonconvexTypeProcessor(); } @@ -536,8 +541,9 @@ public readonly int ConstraintTypeId [MethodImpl(MethodImplOptions.AggressiveInlining)] get => Contact4NonconvexOneBodyTypeProcessor.BatchTypeId; } - + public readonly Type TypeProcessorType => typeof(Contact4NonconvexOneBodyTypeProcessor); + public readonly TypeProcessor CreateTypeProcessor() => new Contact4NonconvexOneBodyTypeProcessor(); } diff --git a/BepuPhysics/Constraints/Contact/ContactNonconvexTypes.tt b/BepuPhysics/Constraints/Contact/ContactNonconvexTypes.tt index 54c76a0c7..ae3318b54 100644 --- a/BepuPhysics/Constraints/Contact/ContactNonconvexTypes.tt +++ b/BepuPhysics/Constraints/Contact/ContactNonconvexTypes.tt @@ -62,8 +62,9 @@ for (int i = 0; i < contactCount ; ++i) [MethodImpl(MethodImplOptions.AggressiveInlining)] get => Contact<#= contactCount #>NonconvexTypeProcessor.BatchTypeId; } - + public readonly Type TypeProcessorType => typeof(Contact<#= contactCount #>NonconvexTypeProcessor); + public readonly TypeProcessor CreateTypeProcessor() => new Contact<#= contactCount #>NonconvexTypeProcessor(); } @@ -173,8 +174,9 @@ for (int i = 0; i < contactCount ; ++i) [MethodImpl(MethodImplOptions.AggressiveInlining)] get => Contact<#= contactCount #>NonconvexOneBodyTypeProcessor.BatchTypeId; } - + public readonly Type TypeProcessorType => typeof(Contact<#= contactCount #>NonconvexOneBodyTypeProcessor); + public readonly TypeProcessor CreateTypeProcessor() => new Contact<#= contactCount #>NonconvexOneBodyTypeProcessor(); } diff --git a/BepuPhysics/Constraints/DistanceLimit.cs b/BepuPhysics/Constraints/DistanceLimit.cs index 9b450354a..937e4ed25 100644 --- a/BepuPhysics/Constraints/DistanceLimit.cs +++ b/BepuPhysics/Constraints/DistanceLimit.cs @@ -63,6 +63,7 @@ public readonly int ConstraintTypeId } public readonly Type TypeProcessorType => typeof(DistanceLimitTypeProcessor); + public readonly TypeProcessor CreateTypeProcessor() => new DistanceLimitTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { diff --git a/BepuPhysics/Constraints/DistanceServo.cs b/BepuPhysics/Constraints/DistanceServo.cs index 4f49f1d11..0295b683b 100644 --- a/BepuPhysics/Constraints/DistanceServo.cs +++ b/BepuPhysics/Constraints/DistanceServo.cs @@ -69,6 +69,7 @@ public readonly int ConstraintTypeId } public readonly Type TypeProcessorType => typeof(DistanceServoTypeProcessor); + public readonly TypeProcessor CreateTypeProcessor() => new DistanceServoTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { diff --git a/BepuPhysics/Constraints/Hinge.cs b/BepuPhysics/Constraints/Hinge.cs index 4723aeca4..ccae6c4df 100644 --- a/BepuPhysics/Constraints/Hinge.cs +++ b/BepuPhysics/Constraints/Hinge.cs @@ -44,6 +44,7 @@ public readonly int ConstraintTypeId } public readonly Type TypeProcessorType => typeof(HingeTypeProcessor); + public readonly TypeProcessor CreateTypeProcessor() => new HingeTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { diff --git a/BepuPhysics/Constraints/IConstraintDescription.cs b/BepuPhysics/Constraints/IConstraintDescription.cs index b3a3e67c7..1c1b24131 100644 --- a/BepuPhysics/Constraints/IConstraintDescription.cs +++ b/BepuPhysics/Constraints/IConstraintDescription.cs @@ -42,6 +42,10 @@ public interface IConstraintDescription /// Gets the type of the type batch which contains described constraints. /// Type TypeProcessorType { get; } + /// + /// Creates a type processor for this constraint type. + /// + TypeProcessor CreateTypeProcessor(); } /// diff --git a/BepuPhysics/Constraints/LinearAxisLimit.cs b/BepuPhysics/Constraints/LinearAxisLimit.cs index 2ed51935d..fb8254d02 100644 --- a/BepuPhysics/Constraints/LinearAxisLimit.cs +++ b/BepuPhysics/Constraints/LinearAxisLimit.cs @@ -48,6 +48,7 @@ public readonly int ConstraintTypeId } public readonly Type TypeProcessorType => typeof(LinearAxisLimitTypeProcessor); + public readonly TypeProcessor CreateTypeProcessor() => new LinearAxisLimitTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { diff --git a/BepuPhysics/Constraints/LinearAxisMotor.cs b/BepuPhysics/Constraints/LinearAxisMotor.cs index cce43ac24..bb5367312 100644 --- a/BepuPhysics/Constraints/LinearAxisMotor.cs +++ b/BepuPhysics/Constraints/LinearAxisMotor.cs @@ -44,6 +44,7 @@ public readonly int ConstraintTypeId } public readonly Type TypeProcessorType => typeof(LinearAxisMotorTypeProcessor); + public readonly TypeProcessor CreateTypeProcessor() => new LinearAxisMotorTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { diff --git a/BepuPhysics/Constraints/LinearAxisServo.cs b/BepuPhysics/Constraints/LinearAxisServo.cs index 47d41bead..98ac5a9fd 100644 --- a/BepuPhysics/Constraints/LinearAxisServo.cs +++ b/BepuPhysics/Constraints/LinearAxisServo.cs @@ -48,6 +48,7 @@ public readonly int ConstraintTypeId } public readonly Type TypeProcessorType => typeof(LinearAxisServoTypeProcessor); + public readonly TypeProcessor CreateTypeProcessor() => new LinearAxisServoTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { diff --git a/BepuPhysics/Constraints/OneBodyAngularMotor.cs b/BepuPhysics/Constraints/OneBodyAngularMotor.cs index 636f23e44..5d89c7972 100644 --- a/BepuPhysics/Constraints/OneBodyAngularMotor.cs +++ b/BepuPhysics/Constraints/OneBodyAngularMotor.cs @@ -32,6 +32,7 @@ public readonly int ConstraintTypeId } public readonly Type TypeProcessorType => typeof(OneBodyAngularMotorTypeProcessor); + public readonly TypeProcessor CreateTypeProcessor() => new OneBodyAngularMotorTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { diff --git a/BepuPhysics/Constraints/OneBodyAngularServo.cs b/BepuPhysics/Constraints/OneBodyAngularServo.cs index aca4df12d..a11140eeb 100644 --- a/BepuPhysics/Constraints/OneBodyAngularServo.cs +++ b/BepuPhysics/Constraints/OneBodyAngularServo.cs @@ -36,6 +36,7 @@ public readonly int ConstraintTypeId } public readonly Type TypeProcessorType => typeof(OneBodyAngularServoTypeProcessor); + public readonly TypeProcessor CreateTypeProcessor() => new OneBodyAngularServoTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { diff --git a/BepuPhysics/Constraints/OneBodyLinearMotor.cs b/BepuPhysics/Constraints/OneBodyLinearMotor.cs index fe15b058f..cb525a5ee 100644 --- a/BepuPhysics/Constraints/OneBodyLinearMotor.cs +++ b/BepuPhysics/Constraints/OneBodyLinearMotor.cs @@ -36,6 +36,7 @@ public readonly int ConstraintTypeId } public readonly Type TypeProcessorType => typeof(OneBodyLinearMotorTypeProcessor); + public readonly TypeProcessor CreateTypeProcessor() => new OneBodyLinearMotorTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { diff --git a/BepuPhysics/Constraints/OneBodyLinearServo.cs b/BepuPhysics/Constraints/OneBodyLinearServo.cs index 1a72056e3..565e7c775 100644 --- a/BepuPhysics/Constraints/OneBodyLinearServo.cs +++ b/BepuPhysics/Constraints/OneBodyLinearServo.cs @@ -40,6 +40,7 @@ public readonly int ConstraintTypeId } public readonly Type TypeProcessorType => typeof(OneBodyLinearServoTypeProcessor); + public readonly TypeProcessor CreateTypeProcessor() => new OneBodyLinearServoTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { diff --git a/BepuPhysics/Constraints/PointOnLineServo.cs b/BepuPhysics/Constraints/PointOnLineServo.cs index 1812d14e8..ca252c2cb 100644 --- a/BepuPhysics/Constraints/PointOnLineServo.cs +++ b/BepuPhysics/Constraints/PointOnLineServo.cs @@ -44,6 +44,7 @@ public readonly int ConstraintTypeId } public readonly Type TypeProcessorType => typeof(PointOnLineServoTypeProcessor); + public readonly TypeProcessor CreateTypeProcessor() => new PointOnLineServoTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { diff --git a/BepuPhysics/Constraints/SwingLimit.cs b/BepuPhysics/Constraints/SwingLimit.cs index 8f0915473..cfccc39b2 100644 --- a/BepuPhysics/Constraints/SwingLimit.cs +++ b/BepuPhysics/Constraints/SwingLimit.cs @@ -45,6 +45,7 @@ public readonly int ConstraintTypeId } public readonly Type TypeProcessorType => typeof(SwingLimitTypeProcessor); + public readonly TypeProcessor CreateTypeProcessor() => new SwingLimitTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { diff --git a/BepuPhysics/Constraints/SwivelHinge.cs b/BepuPhysics/Constraints/SwivelHinge.cs index c2f0a819d..abd4eddbd 100644 --- a/BepuPhysics/Constraints/SwivelHinge.cs +++ b/BepuPhysics/Constraints/SwivelHinge.cs @@ -44,6 +44,7 @@ public readonly int ConstraintTypeId } public readonly Type TypeProcessorType => typeof(SwivelHingeTypeProcessor); + public readonly TypeProcessor CreateTypeProcessor() => new SwivelHingeTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { diff --git a/BepuPhysics/Constraints/TwistLimit.cs b/BepuPhysics/Constraints/TwistLimit.cs index 005259bae..49ace449e 100644 --- a/BepuPhysics/Constraints/TwistLimit.cs +++ b/BepuPhysics/Constraints/TwistLimit.cs @@ -46,6 +46,7 @@ public readonly int ConstraintTypeId } public readonly Type TypeProcessorType => typeof(TwistLimitTypeProcessor); + public readonly TypeProcessor CreateTypeProcessor() => new TwistLimitTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { diff --git a/BepuPhysics/Constraints/TwistMotor.cs b/BepuPhysics/Constraints/TwistMotor.cs index 243872384..533555274 100644 --- a/BepuPhysics/Constraints/TwistMotor.cs +++ b/BepuPhysics/Constraints/TwistMotor.cs @@ -40,6 +40,7 @@ public readonly int ConstraintTypeId } public readonly Type TypeProcessorType => typeof(TwistMotorTypeProcessor); + public readonly TypeProcessor CreateTypeProcessor() => new TwistMotorTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { diff --git a/BepuPhysics/Constraints/TwistServo.cs b/BepuPhysics/Constraints/TwistServo.cs index a3a0843de..97e06c093 100644 --- a/BepuPhysics/Constraints/TwistServo.cs +++ b/BepuPhysics/Constraints/TwistServo.cs @@ -46,6 +46,7 @@ public readonly int ConstraintTypeId } public readonly Type TypeProcessorType => typeof(TwistServoTypeProcessor); + public readonly TypeProcessor CreateTypeProcessor() => new TwistServoTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { diff --git a/BepuPhysics/Constraints/VolumeConstraint.cs b/BepuPhysics/Constraints/VolumeConstraint.cs index f78a11a1c..8afb2fe80 100644 --- a/BepuPhysics/Constraints/VolumeConstraint.cs +++ b/BepuPhysics/Constraints/VolumeConstraint.cs @@ -48,6 +48,7 @@ public readonly int ConstraintTypeId } public readonly Type TypeProcessorType => typeof(VolumeConstraintTypeProcessor); + public readonly TypeProcessor CreateTypeProcessor() => new VolumeConstraintTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { diff --git a/BepuPhysics/Constraints/Weld.cs b/BepuPhysics/Constraints/Weld.cs index bf7079205..7e5d0b291 100644 --- a/BepuPhysics/Constraints/Weld.cs +++ b/BepuPhysics/Constraints/Weld.cs @@ -43,6 +43,7 @@ public readonly int ConstraintTypeId } public readonly Type TypeProcessorType => typeof(WeldTypeProcessor); + public readonly TypeProcessor CreateTypeProcessor() => new WeldTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index d8a58af88..b97126568 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -294,7 +294,7 @@ protected Solver(Bodies bodies, BufferPool pool, SolveDescription solveDescripti /// is called during simuation creation and registers all the built in types. Calling manually is only necessary if custom types are used. public void Register() where TDescription : unmanaged, IConstraintDescription { - var description = default(TDescription); + Unsafe.SkipInit(out TDescription description); Debug.Assert(description.ConstraintTypeId >= 0, "Constraint type ids should never be negative. They're used for array indexing."); if (TypeProcessors == null || description.ConstraintTypeId >= TypeProcessors.Length) { @@ -304,13 +304,13 @@ public void Register() where TDescription : unmanaged, IConstraint } if (TypeProcessors[description.ConstraintTypeId] == null) { - var processor = (TypeProcessor)Activator.CreateInstance(description.TypeProcessorType); + var processor = description.CreateTypeProcessor(); TypeProcessors[description.ConstraintTypeId] = processor; processor.Initialize(description.ConstraintTypeId); } - else if (TypeProcessors[description.ConstraintTypeId].GetType() != description.TypeProcessorType) + else { - throw new ArgumentException( + Debug.Assert(TypeProcessors[description.ConstraintTypeId].GetType() == description.TypeProcessorType, $"Type processor {TypeProcessors[description.ConstraintTypeId].GetType().Name} has already been registered for this description's type id " + $"({typeof(TDescription).Name}, {default(TDescription).ConstraintTypeId}). " + $"Cannot register two types with the same type id."); diff --git a/Demos/Demos/Characters/CharacterMotionConstraint.cs b/Demos/Demos/Characters/CharacterMotionConstraint.cs index 74e9aa6bf..132564382 100644 --- a/Demos/Demos/Characters/CharacterMotionConstraint.cs +++ b/Demos/Demos/Characters/CharacterMotionConstraint.cs @@ -67,7 +67,11 @@ public struct StaticCharacterMotionConstraint : IOneBodyConstraintDescription /// Gets the TypeProcessor type that is associated with this description. /// - public readonly Type TypeProcessorType => typeof(StaticCharacterMotionTypeProcessor); + public readonly Type TypeProcessorType => typeof(StaticCharacterMotionTypeProcessor); + /// + /// Creates a type processor for this constraint type. + /// + public readonly TypeProcessor CreateTypeProcessor() => new StaticCharacterMotionTypeProcessor(); //Note that these mapping functions use a "GetOffsetInstance" function. Each CharacterMotionPrestep is a bundle of multiple constraints; //by grabbing an offset instance, we're selecting a specific slot in the bundle to modify. For simplicity and to guarantee consistency of field strides, @@ -336,7 +340,11 @@ public struct DynamicCharacterMotionConstraint : ITwoBodyConstraintDescription /// Gets the TypeProcessor type that is associated with this description. /// - public readonly Type TypeProcessorType => typeof(DynamicCharacterMotionTypeProcessor); + public readonly Type TypeProcessorType => typeof(DynamicCharacterMotionTypeProcessor); + /// + /// Creates a type processor for this constraint type. + /// + public readonly TypeProcessor CreateTypeProcessor() => new DynamicCharacterMotionTypeProcessor(); //Note that these mapping functions use a "GetOffsetInstance" function. Each CharacterMotionPrestep is a bundle of multiple constraints; //by grabbing an offset instance, we're selecting a specific slot in the bundle to modify. For simplicity and to guarantee consistency of field strides, diff --git a/Demos/Demos/Characters/CharacterMotionConstraint.tt b/Demos/Demos/Characters/CharacterMotionConstraint.tt index 1da30ad74..f045bb02b 100644 --- a/Demos/Demos/Characters/CharacterMotionConstraint.tt +++ b/Demos/Demos/Characters/CharacterMotionConstraint.tt @@ -84,7 +84,11 @@ namespace Demos.Demos.Characters /// /// Gets the TypeProcessor type that is associated with this description. /// - public readonly Type TypeProcessorType => typeof(<#=prefix#>CharacterMotionTypeProcessor); + public readonly Type TypeProcessorType => typeof(<#=prefix#>CharacterMotionTypeProcessor); + /// + /// Creates a type processor for this constraint type. + /// + public readonly TypeProcessor CreateTypeProcessor() => new <#=prefix#>CharacterMotionTypeProcessor(); //Note that these mapping functions use a "GetOffsetInstance" function. Each CharacterMotionPrestep is a bundle of multiple constraints; //by grabbing an offset instance, we're selecting a specific slot in the bundle to modify. For simplicity and to guarantee consistency of field strides, From 694fc97f31784d2ebc186207aeab9476c089b972 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 17 Aug 2022 17:32:42 -0500 Subject: [PATCH 513/947] Fixed some convex hull tester maximum contact count bounds that broke high complexity faces. --- .../CollisionTasks/BoxConvexHullTester.cs | 46 ++++++++++++++----- .../CollisionTasks/ConvexHullPairTester.cs | 4 +- .../TriangleConvexHullTester.cs | 7 +-- 3 files changed, 41 insertions(+), 16 deletions(-) diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/BoxConvexHullTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/BoxConvexHullTester.cs index 0ebbab7df..3dde6717d 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/BoxConvexHullTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/BoxConvexHullTester.cs @@ -1,5 +1,6 @@ using BepuPhysics.Collidables; using BepuUtilities; +using BepuUtilities.Memory; using System; using System.Diagnostics; using System.Numerics; @@ -80,18 +81,39 @@ public unsafe void Test(ref BoxWide a, ref ConvexHullWide b, ref Vector s Vector3Wide.Subtract(v1, boxFaceYOffset, out var v10); Vector3Wide.Add(v1, boxFaceYOffset, out var v11); - //To find the contact manifold, we'll clip the box edges against the hull face as usual, but we're dealing with potentially - //distinct convex hulls. Rather than vectorizing over the different hulls, we vectorize within each hull. Helpers.FillVectorWithLaneIndices(out var slotOffsetIndices); var boundingPlaneEpsilon = 1e-3f * epsilonScale; - //There can be no more than 8 contacts (provided there are no numerical errors); 2 per box edge. - var candidates = stackalloc ManifoldCandidateScalar[8]; + Vector3* slotHullFaceNormals = stackalloc Vector3[Vector.Count]; + Vector3* slotLocalNormals = stackalloc Vector3[Vector.Count]; + Buffer* hullVertexIndices = stackalloc Buffer[Vector.Count]; + Unsafe.SkipInit(out Vector3Wide hullFaceNormal); + int maximumFaceVertexCount = 0; + for (int slotIndex = 0; slotIndex < pairCount; ++slotIndex) + { + if (inactiveLanes[slotIndex] < 0) + continue; + ref var hull = ref b.Hulls[slotIndex]; + + ConvexHullTestHelper.PickRepresentativeFace(ref hull, slotIndex, ref localNormal, closestOnHull, slotOffsetIndices, ref boundingPlaneEpsilon, out slotHullFaceNormals[slotIndex], out slotLocalNormals[slotIndex], out var bestFaceIndex); + Vector3Wide.WriteSlot(slotHullFaceNormals[slotIndex], slotIndex, ref hullFaceNormal); + hull.GetVertexIndicesForFace(bestFaceIndex, out hullVertexIndices[slotIndex]); + var verticesInFace = hullVertexIndices[slotIndex].Length; + if (verticesInFace > maximumFaceVertexCount) + maximumFaceVertexCount = verticesInFace; + } + + //To find the contact manifold, we'll clip the box edges against the hull face as usual, but we're dealing with potentially + //distinct convex hulls. Rather than vectorizing over the different hulls, we vectorize within each hull. + //There can be no more than 8 contacts from edge intersections, but more can be generated from hull faces with many vertices. + int maximumContactCount = Math.Max(8, maximumFaceVertexCount); + var candidates = stackalloc ManifoldCandidateScalar[maximumContactCount]; for (int slotIndex = 0; slotIndex < pairCount; ++slotIndex) { if (inactiveLanes[slotIndex] < 0) continue; ref var hull = ref b.Hulls[slotIndex]; - ConvexHullTestHelper.PickRepresentativeFace(ref hull, slotIndex, ref localNormal, closestOnHull, slotOffsetIndices, ref boundingPlaneEpsilon, out var slotFaceNormal, out var slotLocalNormal, out var bestFaceIndex); + var slotFaceNormal = slotHullFaceNormals[slotIndex]; + var slotLocalNormal = slotLocalNormals[slotIndex]; //Test each face edge plane against the box face. //Note that we do not use the faceNormal x edgeOffset edge plane, but rather edgeOffset x localNormal. @@ -119,7 +141,7 @@ public unsafe void Test(ref BoxWide a, ref ConvexHullWide b, ref Vector s var edgePlaneNormalY = edgeDirectionZ * slotLocalNormalX - edgeDirectionX * slotLocalNormalZ; var edgePlaneNormalZ = edgeDirectionX * slotLocalNormalY - edgeDirectionY * slotLocalNormalX; - hull.GetVertexIndicesForFace(bestFaceIndex, out var faceVertexIndices); + var faceVertexIndices = hullVertexIndices[slotIndex]; var previousIndex = faceVertexIndices[faceVertexIndices.Length - 1]; Vector3Wide.ReadSlot(ref hull.Points[previousIndex.BundleIndex], previousIndex.InnerIndex, out var hullFaceOrigin); var previousVertex = hullFaceOrigin; @@ -237,7 +259,7 @@ public unsafe void Test(ref BoxWide a, ref ConvexHullWide b, ref Vector s var startId = (previousIndex.BundleIndex << BundleIndexing.VectorShift) + previousIndex.InnerIndex; var endId = (index.BundleIndex << BundleIndexing.VectorShift) + index.InnerIndex; var baseFeatureId = (startId ^ endId) << 8; - if (earliestExit >= latestEntry && candidateCount < 8) + if (earliestExit >= latestEntry && candidateCount < maximumContactCount) { //Create max contact. var point = hullEdgeOffset * earliestExit + previousVertex - hullFaceOrigin; @@ -248,7 +270,7 @@ public unsafe void Test(ref BoxWide a, ref ConvexHullWide b, ref Vector s candidate.FeatureId = baseFeatureId + endId; } - if (latestEntry < earliestExit && latestEntry > 0 && candidateCount < 8) + if (latestEntry < earliestExit && latestEntry > 0 && candidateCount < maximumContactCount) { //Create min contact. var point = hullEdgeOffset * latestEntry + previousVertex - hullFaceOrigin; @@ -263,7 +285,7 @@ public unsafe void Test(ref BoxWide a, ref ConvexHullWide b, ref Vector s previousIndex = index; previousVertex = vertex; } - if (candidateCount < 8) + if (candidateCount < maximumContactCount) { //Try adding the box vertex contacts. Project each vertex onto the hull face. //t = dot(boxVertex - hullFaceVertex, hullFacePlaneNormal) / dot(hullFacePlaneNormal, localNormal) @@ -299,7 +321,7 @@ public unsafe void Test(ref BoxWide a, ref ConvexHullWide b, ref Vector s candidate.Y = projectedTangentY.X; candidate.FeatureId = 0; } - if (candidateCount == 8) + if (candidateCount == maximumContactCount) goto SkipVertexCandidates; if (maximumVertexContainmentDots.Y <= 0) { @@ -308,7 +330,7 @@ public unsafe void Test(ref BoxWide a, ref ConvexHullWide b, ref Vector s candidate.Y = projectedTangentY.Y; candidate.FeatureId = 1; } - if (candidateCount == 8) + if (candidateCount == maximumContactCount) goto SkipVertexCandidates; if (maximumVertexContainmentDots.Z <= 0) { @@ -317,7 +339,7 @@ public unsafe void Test(ref BoxWide a, ref ConvexHullWide b, ref Vector s candidate.Y = projectedTangentY.Z; candidate.FeatureId = 2; } - if (candidateCount < 8 && maximumVertexContainmentDots.W <= 0) + if (candidateCount < maximumContactCount && maximumVertexContainmentDots.W <= 0) { ref var candidate = ref candidates[candidateCount++]; candidate.X = projectedTangentX.W; diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexHullPairTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexHullPairTester.cs index 2b8bce7f1..e5491b40a 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexHullPairTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexHullPairTester.cs @@ -105,7 +105,9 @@ public unsafe void Test(ref ConvexHullWide a, ref ConvexHullWide b, ref Vector.One / triangleNormalDotLocalNormal; + //Maximum number of edge-related contacts is 6. Maximum number of triangle vertex contacts is 3. Maximum number of hull vertex contacts is whatever the largest face is. int maximumContactCount = Math.Max(6, maximumFaceVertexCount); var candidates = stackalloc ManifoldCandidateScalar[maximumContactCount]; //To find the contact manifold, we'll clip the triangle edges against the hull face as usual, but we're dealing with potentially @@ -363,7 +364,7 @@ public unsafe void Test(ref TriangleWide a, ref ConvexHullWide b, ref Vector 0 && candidateCount < 6) + if (latestEntryAB < earliestExitAB && latestEntryAB > 0 && candidateCount < maximumContactCount) { //Create min contact. var point = slotTriangleAB * latestEntryAB; //Note triangle A is origin for surface basis. @@ -384,7 +385,7 @@ public unsafe void Test(ref TriangleWide a, ref ConvexHullWide b, ref Vector 0 && candidateCount < 6) + if (latestEntryBC < earliestExitBC && latestEntryBC > 0 && candidateCount < maximumContactCount) { //Create min contact. var point = slotTriangleBC * latestEntryBC + slotTriangleAB; @@ -405,7 +406,7 @@ public unsafe void Test(ref TriangleWide a, ref ConvexHullWide b, ref Vector 0 && candidateCount < 6) + if (latestEntryCA < earliestExitCA && latestEntryCA > 0 && candidateCount < maximumContactCount) { //Create min contact. var point = slotTriangleCA * latestEntryCA - slotTriangleCA; From 26b1acb881d60162c3ec82f45dbf5ecccee30764 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 17 Aug 2022 21:41:23 -0500 Subject: [PATCH 514/947] Loosened up plane epsilon in huller. Improved convex hull test demo. Still imperfect... --- BepuPhysics/Collidables/ConvexHullHelper.cs | 4 +- Demos/SpecializedTests/ConvexHullTestDemo.cs | 408 ++++++++++++++----- 2 files changed, 318 insertions(+), 94 deletions(-) diff --git a/BepuPhysics/Collidables/ConvexHullHelper.cs b/BepuPhysics/Collidables/ConvexHullHelper.cs index b32b937b9..6ab219277 100644 --- a/BepuPhysics/Collidables/ConvexHullHelper.cs +++ b/BepuPhysics/Collidables/ConvexHullHelper.cs @@ -208,7 +208,7 @@ static int FindNextIndexForFaceHull(Vector2 start, Vector2 previousEdgeDirection { Debug.Assert(candidateY <= 0, "Previous edge should include any collinear points, so this edge should not see any further collinear points beyond its start." + - "If you run into this, it implies you've found some content that violates the convex huller's assumptions, and I'd appreciate it if you reported it on github.com/bepu/bepuphysics2/issues!" + + "If you run into this, it implies you've found some content that violates the convex huller's assumptions, and I'd appreciate it if you reported it on github.com/bepu/bepuphysics2/issues!" + "A .obj or other simple demos-compatible reproduction case would help me fix it."); continue; } @@ -546,7 +546,7 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa Vector3Wide.Broadcast(initialVertex, out var initialVertexBundle); pool.Take>(pointBundles.Length, out var projectedOnX); pool.Take>(pointBundles.Length, out var projectedOnY); - var planeEpsilonNarrow = MathF.Sqrt(bestDistanceSquared) * 1e-6f; + var planeEpsilonNarrow = MathF.Sqrt(bestDistanceSquared) * 1e-4f; var planeEpsilon = new Vector(planeEpsilonNarrow); var rawFaceVertexIndices = new QuickList(pointBundles.Length * Vector.Count, pool); var initialSourceEdge = new EdgeEndpoints { A = initialIndex, B = initialIndex }; diff --git a/Demos/SpecializedTests/ConvexHullTestDemo.cs b/Demos/SpecializedTests/ConvexHullTestDemo.cs index a5bfce155..73c01bfc0 100644 --- a/Demos/SpecializedTests/ConvexHullTestDemo.cs +++ b/Demos/SpecializedTests/ConvexHullTestDemo.cs @@ -15,14 +15,246 @@ using BepuUtilities; using BepuPhysics.Constraints.Contact; using BepuPhysics.Constraints; +using Demos.Demos; +using BepuUtilities.Memory; namespace Demos.SpecializedTests { public class ConvexHullTestDemo : Demo { - QuickList points; + //Buffer points; //List debugSteps; - public override void Initialize(ContentArchive content, Camera camera) + + unsafe Buffer CreateRandomConvexHullPoints() + { + const int pointCount = 50; + BufferPool.Take(pointCount, out var points); + + var random = new Random(5); + for (int i = 0; i < pointCount; ++i) + { + points[i] = new Vector3(3 * random.NextSingle(), 1 * random.NextSingle(), 3 * random.NextSingle()); + } + + return points; + } + + unsafe Buffer CreateMeshConvexHull(MeshContent meshContent, Vector3 scale) + { + //This is actually a pretty good example of how *not* to make a convex hull shape. + //Generating it directly from a graphical data source tends to have way more surface complexity than needed, + //and it tends to have a lot of near-but-not-quite-coplanar surfaces which can make the contact manifold less stable. + //Prefer a simpler source with more distinct features, possibly created with an automated content-time tool. + BufferPool.Take(meshContent.Triangles.Length * 3, out var points); + for (int i = 0; i < meshContent.Triangles.Length; ++i) + { + ref var triangle = ref meshContent.Triangles[i]; + //resisting the urge to just reinterpret the memory + points[i * 3 + 0] = triangle.A * scale; + points[i * 3 + 1] = triangle.B * scale; + points[i * 3 + 2] = triangle.C * scale; + } + return points; + } + + unsafe Buffer CreateBoxConvexHull(float boxScale) + { + BufferPool.Take(8, out var points); + points[0] = new Vector3(0, 0, 0); + points[1] = new Vector3(0, 0, boxScale); + points[2] = new Vector3(0, boxScale, 0); + points[3] = new Vector3(0, boxScale, boxScale); + points[4] = new Vector3(boxScale, 0, 0); + points[5] = new Vector3(boxScale, 0, boxScale); + points[6] = new Vector3(boxScale, boxScale, 0); + points[7] = new Vector3(boxScale, boxScale, boxScale); + return points; + } + + //A couple of point sets from PEEL. + unsafe Buffer CreateTestConvexHull() + { + BufferPool.Take(50, out var vertices); + vertices[0] = new Vector3(-0.000000f, -0.297120f, -0.000000f); + vertices[1] = new Vector3(0.258819f, -0.297120f, 0.965926f); + vertices[2] = new Vector3(-0.000000f, -0.297120f, 1.000000f); + vertices[3] = new Vector3(0.500000f, -0.297120f, 0.866026f); + vertices[4] = new Vector3(0.707107f, -0.297120f, 0.707107f); + vertices[5] = new Vector3(0.866026f, -0.297120f, 0.500000f); + vertices[6] = new Vector3(0.965926f, -0.297120f, 0.258819f); + vertices[7] = new Vector3(1.000000f, -0.297120f, -0.000000f); + vertices[8] = new Vector3(0.965926f, -0.297120f, -0.258819f); + vertices[9] = new Vector3(0.866026f, -0.297120f, -0.500000f); + vertices[10] = new Vector3(0.707107f, -0.297120f, -0.707107f); + vertices[11] = new Vector3(0.500000f, -0.297120f, -0.866026f); + vertices[12] = new Vector3(0.258819f, -0.297120f, -0.965926f); + vertices[13] = new Vector3(-0.000000f, -0.297120f, -1.000000f); + vertices[14] = new Vector3(-0.258819f, -0.297120f, -0.965926f); + vertices[15] = new Vector3(-0.500000f, -0.297120f, -0.866025f); + vertices[16] = new Vector3(-0.707107f, -0.297120f, -0.707107f); + vertices[17] = new Vector3(-0.866026f, -0.297120f, -0.500000f); + vertices[18] = new Vector3(-0.965926f, -0.297120f, -0.258819f); + vertices[19] = new Vector3(-1.000000f, -0.297120f, 0.000000f); + vertices[20] = new Vector3(-0.965926f, -0.297120f, 0.258819f); + vertices[21] = new Vector3(-0.866025f, -0.297120f, 0.500000f); + vertices[22] = new Vector3(-0.707107f, -0.297120f, 0.707107f); + vertices[23] = new Vector3(-0.500000f, -0.297120f, 0.866026f); + vertices[24] = new Vector3(-0.258819f, -0.297120f, 0.965926f); + vertices[25] = new Vector3(-0.000000f, 0.297120f, -0.000000f); + vertices[26] = new Vector3(-0.000000f, 0.297120f, 0.537813f); + vertices[27] = new Vector3(0.139196f, 0.297120f, 0.519487f); + vertices[28] = new Vector3(0.268907f, 0.297120f, 0.465760f); + vertices[29] = new Vector3(0.380291f, 0.297120f, 0.380291f); + vertices[30] = new Vector3(0.465760f, 0.297120f, 0.268907f); + vertices[31] = new Vector3(0.519487f, 0.297120f, 0.139196f); + vertices[32] = new Vector3(0.537813f, 0.297120f, -0.000000f); + vertices[33] = new Vector3(0.519487f, 0.297120f, -0.139196f); + vertices[34] = new Vector3(0.465760f, 0.297120f, -0.268907f); + vertices[35] = new Vector3(0.380291f, 0.297120f, -0.380291f); + vertices[36] = new Vector3(0.268907f, 0.297120f, -0.465760f); + vertices[37] = new Vector3(0.139196f, 0.297120f, -0.519487f); + vertices[38] = new Vector3(-0.000000f, 0.297120f, -0.537813f); + vertices[39] = new Vector3(-0.139196f, 0.297120f, -0.519487f); + vertices[40] = new Vector3(-0.268907f, 0.297120f, -0.465760f); + vertices[41] = new Vector3(-0.380291f, 0.297120f, -0.380291f); + vertices[42] = new Vector3(-0.465760f, 0.297120f, -0.268907f); + vertices[43] = new Vector3(-0.519487f, 0.297120f, -0.139196f); + vertices[44] = new Vector3(-0.537813f, 0.297120f, 0.000000f); + vertices[45] = new Vector3(-0.519487f, 0.297120f, 0.139196f); + vertices[46] = new Vector3(-0.465760f, 0.297120f, 0.268907f); + vertices[47] = new Vector3(-0.380291f, 0.297120f, 0.380291f); + vertices[48] = new Vector3(-0.268907f, 0.297120f, 0.465760f); + vertices[49] = new Vector3(-0.139196f, 0.297120f, 0.519487f); + return vertices; + } + + unsafe Buffer CreateTestConvexHull2() + { + BufferPool.Take(120, out var vertices); + vertices[0] = new Vector3(0.153478f, 0.993671f, 0.124687f); + vertices[1] = new Vector3(0.153478f, 0.993671f, -0.117774f); + vertices[2] = new Vector3(-0.147939f, 0.993671f, -0.117774f); + vertices[3] = new Vector3(-0.147939f, 0.993671f, 0.124687f); + vertices[4] = new Vector3(0.137286f, 0.817392f, 0.586192f); + vertices[5] = new Vector3(0.333441f, 0.696161f, 0.661116f); + vertices[6] = new Vector3(0.484149f, 0.789305f, 0.417265f); + vertices[7] = new Vector3(0.287995f, 0.910536f, 0.342339f); + vertices[8] = new Vector3(0.794945f, 0.410936f, 0.484838f); + vertices[9] = new Vector3(0.916176f, 0.336012f, 0.288682f); + vertices[10] = new Vector3(0.823033f, 0.579863f, 0.137973f); + vertices[11] = new Vector3(0.701803f, 0.654787f, 0.334128f); + vertices[12] = new Vector3(0.916176f, 0.336012f, -0.281770f); + vertices[13] = new Vector3(0.794945f, 0.410936f, -0.477925f); + vertices[14] = new Vector3(0.701803f, 0.654787f, -0.327216f); + vertices[15] = new Vector3(0.823033f, 0.579863f, -0.131060f); + vertices[16] = new Vector3(0.333441f, 0.696161f, -0.654204f); + vertices[17] = new Vector3(0.137286f, 0.817392f, -0.579280f); + vertices[18] = new Vector3(0.287995f, 0.910536f, -0.335426f); + vertices[19] = new Vector3(0.484149f, 0.789305f, -0.410352f); + vertices[20] = new Vector3(-0.131747f, 0.817392f, -0.579280f); + vertices[21] = new Vector3(-0.327903f, 0.696161f, -0.654204f); + vertices[22] = new Vector3(-0.478612f, 0.789305f, -0.410352f); + vertices[23] = new Vector3(-0.282457f, 0.910536f, -0.335426f); + vertices[24] = new Vector3(-0.789408f, 0.410936f, -0.477925f); + vertices[25] = new Vector3(-0.910638f, 0.336012f, -0.281770f); + vertices[26] = new Vector3(-0.817496f, 0.579863f, -0.131060f); + vertices[27] = new Vector3(-0.696265f, 0.654787f, -0.327216f); + vertices[28] = new Vector3(-0.910638f, 0.336012f, 0.288682f); + vertices[29] = new Vector3(-0.789408f, 0.410936f, 0.484838f); + vertices[30] = new Vector3(-0.696265f, 0.654787f, 0.334128f); + vertices[31] = new Vector3(-0.817496f, 0.579863f, 0.137973f); + vertices[32] = new Vector3(-0.327903f, 0.696161f, 0.661116f); + vertices[33] = new Vector3(-0.131747f, 0.817392f, 0.586192f); + vertices[34] = new Vector3(-0.282457f, 0.910536f, 0.342339f); + vertices[35] = new Vector3(-0.478612f, 0.789305f, 0.417265f); + vertices[36] = new Vector3(0.416578f, 0.478508f, 0.795634f); + vertices[37] = new Vector3(0.341652f, 0.282353f, 0.916863f); + vertices[38] = new Vector3(0.585505f, 0.131646f, 0.823721f); + vertices[39] = new Vector3(0.660429f, 0.327801f, 0.702490f); + vertices[40] = new Vector3(0.124000f, 0.147837f, 1.000000f); + vertices[41] = new Vector3(-0.118461f, 0.147837f, 1.000000f); + vertices[42] = new Vector3(-0.118461f, -0.153580f, 1.000000f); + vertices[43] = new Vector3(0.124000f, -0.153580f, 1.000000f); + vertices[44] = new Vector3(-0.336113f, 0.282353f, 0.916863f); + vertices[45] = new Vector3(-0.411039f, 0.478508f, 0.795634f); + vertices[46] = new Vector3(-0.654891f, 0.327801f, 0.702490f); + vertices[47] = new Vector3(-0.579966f, 0.131646f, 0.823721f); + vertices[48] = new Vector3(-0.993774f, 0.118359f, -0.147252f); + vertices[49] = new Vector3(-0.993774f, -0.124103f, -0.147252f); + vertices[50] = new Vector3(-0.993774f, -0.124103f, 0.154165f); + vertices[51] = new Vector3(-0.993774f, 0.118359f, 0.154165f); + vertices[52] = new Vector3(-0.817496f, -0.585607f, 0.137973f); + vertices[53] = new Vector3(-0.696265f, -0.660531f, 0.334128f); + vertices[54] = new Vector3(-0.789408f, -0.416680f, 0.484838f); + vertices[55] = new Vector3(-0.910638f, -0.341756f, 0.288682f); + vertices[56] = new Vector3(-0.411039f, -0.484253f, 0.795634f); + vertices[57] = new Vector3(-0.336113f, -0.288097f, 0.916863f); + vertices[58] = new Vector3(-0.579966f, -0.137388f, 0.823721f); + vertices[59] = new Vector3(-0.654891f, -0.333543f, 0.702490f); + vertices[60] = new Vector3(0.341652f, -0.288097f, 0.916863f); + vertices[61] = new Vector3(0.416578f, -0.484253f, 0.795634f); + vertices[62] = new Vector3(0.660429f, -0.333543f, 0.702490f); + vertices[63] = new Vector3(0.585505f, -0.137388f, 0.823721f); + vertices[64] = new Vector3(0.333441f, -0.701905f, 0.661116f); + vertices[65] = new Vector3(0.137286f, -0.823136f, 0.586192f); + vertices[66] = new Vector3(0.287995f, -0.916278f, 0.342339f); + vertices[67] = new Vector3(0.484149f, -0.795049f, 0.417265f); + vertices[68] = new Vector3(-0.131747f, -0.823136f, 0.586192f); + vertices[69] = new Vector3(-0.327903f, -0.701905f, 0.661116f); + vertices[70] = new Vector3(-0.478612f, -0.795049f, 0.417265f); + vertices[71] = new Vector3(-0.282457f, -0.916278f, 0.342339f); + vertices[72] = new Vector3(-0.910638f, -0.341756f, -0.281770f); + vertices[73] = new Vector3(-0.789408f, -0.416680f, -0.477925f); + vertices[74] = new Vector3(-0.696265f, -0.660531f, -0.327216f); + vertices[75] = new Vector3(-0.817496f, -0.585607f, -0.131060f); + vertices[76] = new Vector3(-0.327903f, -0.701905f, -0.654204f); + vertices[77] = new Vector3(-0.131747f, -0.823136f, -0.579280f); + vertices[78] = new Vector3(-0.282457f, -0.916278f, -0.335426f); + vertices[79] = new Vector3(-0.478612f, -0.795049f, -0.410352f); + vertices[80] = new Vector3(0.153478f, -0.999415f, -0.117774f); + vertices[81] = new Vector3(0.153478f, -0.999415f, 0.124687f); + vertices[82] = new Vector3(-0.147939f, -0.999415f, 0.124687f); + vertices[83] = new Vector3(-0.147939f, -0.999415f, -0.117774f); + vertices[84] = new Vector3(0.701803f, -0.660531f, 0.334128f); + vertices[85] = new Vector3(0.823033f, -0.585607f, 0.137973f); + vertices[86] = new Vector3(0.916176f, -0.341756f, 0.288682f); + vertices[87] = new Vector3(0.794945f, -0.416680f, 0.484838f); + vertices[88] = new Vector3(0.823033f, -0.585607f, -0.131060f); + vertices[89] = new Vector3(0.701803f, -0.660531f, -0.327216f); + vertices[90] = new Vector3(0.794945f, -0.416680f, -0.477925f); + vertices[91] = new Vector3(0.916176f, -0.341756f, -0.281770f); + vertices[92] = new Vector3(0.484149f, -0.795049f, -0.410352f); + vertices[93] = new Vector3(0.287995f, -0.916278f, -0.335426f); + vertices[94] = new Vector3(0.137286f, -0.823136f, -0.579280f); + vertices[95] = new Vector3(0.333441f, -0.701905f, -0.654204f); + vertices[96] = new Vector3(-0.654891f, -0.333543f, -0.695578f); + vertices[97] = new Vector3(-0.579966f, -0.137388f, -0.816807f); + vertices[98] = new Vector3(-0.336113f, -0.288097f, -0.909951f); + vertices[99] = new Vector3(-0.411039f, -0.484253f, -0.788719f); + vertices[100] = new Vector3(-0.118461f, 0.147837f, -0.993087f); + vertices[101] = new Vector3(0.124000f, 0.147837f, -0.993087f); + vertices[102] = new Vector3(0.124000f, -0.153580f, -0.993087f); + vertices[103] = new Vector3(-0.118461f, -0.153580f, -0.993087f); + vertices[104] = new Vector3(0.585505f, -0.137388f, -0.816807f); + vertices[105] = new Vector3(0.660429f, -0.333543f, -0.695578f); + vertices[106] = new Vector3(0.416578f, -0.484253f, -0.788719f); + vertices[107] = new Vector3(0.341652f, -0.288097f, -0.909951f); + vertices[108] = new Vector3(0.999313f, -0.124103f, -0.147252f); + vertices[109] = new Vector3(0.999313f, 0.118359f, -0.147252f); + vertices[110] = new Vector3(0.999313f, 0.118359f, 0.154165f); + vertices[111] = new Vector3(0.999313f, -0.124103f, 0.154165f); + vertices[112] = new Vector3(0.660429f, 0.327801f, -0.695578f); + vertices[113] = new Vector3(0.585505f, 0.131646f, -0.816807f); + vertices[114] = new Vector3(0.341652f, 0.282353f, -0.909951f); + vertices[115] = new Vector3(0.416578f, 0.478508f, -0.788719f); + vertices[116] = new Vector3(-0.579966f, 0.131646f, -0.816807f); + vertices[117] = new Vector3(-0.654891f, 0.327801f, -0.695578f); + vertices[118] = new Vector3(-0.411039f, 0.478508f, -0.788719f); + vertices[119] = new Vector3(-0.336113f, 0.282353f, -0.909951f); + return vertices; + } + + public unsafe override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(0, -2.5f, 10); camera.Yaw = 0; @@ -30,42 +262,30 @@ public override void Initialize(ContentArchive content, Camera camera) Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); - //var meshContent = content.Load("Content\\newt.obj"); - ////This is actually a pretty good example of how *not* to make a convex hull shape. - ////Generating it directly from a graphical data source tends to have way more surface complexity than needed, - ////and it tends to have a lot of near-but-not-quite-coplanar surfaces which can make the contact manifold less stable. - ////Prefer a simpler source with more distinct features, possibly created with an automated content-time tool. - //points = new QuickList(meshContent.Triangles.Length * 3, BufferPool); - //for (int i = 0; i < meshContent.Triangles.Length; ++i) + var hullPoints = CreateTestConvexHull2(); + //var hullPoints = CreateMeshConvexHull(content.Load(@"Content\newt.obj"), new Vector3(1, 1.5f, 1f)); + var hullShape = new ConvexHull(hullPoints, BufferPool, out _); + //for (int i = 0; i < hullShape.FaceToVertexIndicesStart.Length; ++i) //{ - // ref var triangle = ref meshContent.Triangles[i]; - // //resisting the urge to just reinterpret the memory - // points.AllocateUnsafely() = triangle.A * new Vector3(1, 1.5f, 1); - // points.AllocateUnsafely() = triangle.B * new Vector3(1, 1.5f, 1); - // points.AllocateUnsafely() = triangle.C * new Vector3(1, 1.5f, 1); + // hullShape.GetVertexIndicesForFace(i, out var faceVertices); + // BundleIndexing.GetBundleIndices(i, out var normalBundleIndex, out var normalIndexInBundle); + // Vector3Wide.ReadSlot(ref hullShape.BoundingPlanes[normalBundleIndex].Normal, normalIndexInBundle, out var faceNormal); + // var offset = hullShape.BoundingPlanes[normalBundleIndex].Offset[normalIndexInBundle]; + // Console.WriteLine($"Face {i} errors:"); + // for (int j = 0; j < faceVertices.Length; ++j) + // { + // hullShape.GetPoint(faceVertices[j], out var point); + // var error = Vector3.Dot(point, faceNormal) - offset; + // Console.WriteLine($"v{j}: {error}"); + // } //} - const int pointCount = 50; - points = new QuickList(pointCount * 2, BufferPool); - //points.Allocate(BufferPool) = new Vector3(0, 0, 0); - //points.Allocate(BufferPool) = new Vector3(0, 0, 1); - //points.Allocate(BufferPool) = new Vector3(0, 1, 0); - //points.Allocate(BufferPool) = new Vector3(0, 1, 1); - //points.Allocate(BufferPool) = new Vector3(1, 0, 0); - //points.Allocate(BufferPool) = new Vector3(1, 0, 1); - //points.Allocate(BufferPool) = new Vector3(1, 1, 0); - //points.Allocate(BufferPool) = new Vector3(1, 1, 1); - var random = new Random(5); - for (int i = 0; i < pointCount; ++i) - { - points.AllocateUnsafely() = new Vector3(3 * random.NextSingle(), 1 * random.NextSingle(), 3 * random.NextSingle()); - //points.AllocateUnsafely() = new Vector3(0, 1, 0) + Vector3.Normalize(new Vector3(random.NextSingle() * 2 - 1, random.NextSingle() * 2 - 1, random.NextSingle() * 2 - 1)) * random.NextSingle(); - } + //ConvexHullHelper.ComputeHull(hullPoints, BufferPool, out var hullData, out debugSteps); + //this.points = hullPoints; - var pointsBuffer = points.Span.Slice(points.Count); - CreateShape(pointsBuffer, BufferPool, out _, out var hullShape); - //ConvexHullHelper.ComputeHull(pointsBuffer, BufferPool, out _, out debugSteps); + var boxHullPoints = CreateBoxConvexHull(2); + var boxHullShape = new ConvexHull(boxHullPoints, BufferPool, out _); Matrix3x3.CreateScale(new Vector3(5, 0.5f, 3), out var scale); var transform = Matrix3x3.CreateFromAxisAngle(Vector3.Normalize(new Vector3(3, 2, 1)), 1207) * scale; @@ -102,15 +322,17 @@ public override void Initialize(ContentArchive content, Camera camera) start = Stopwatch.GetTimestamp(); for (int i = 0; i < iterationCount; ++i) { - CreateShape(pointsBuffer, BufferPool, out _, out var perfTestShape); + CreateShape(hullPoints, BufferPool, out _, out var perfTestShape); perfTestShape.Dispose(BufferPool); } end = Stopwatch.GetTimestamp(); Console.WriteLine($"Hull computation time (us): {(end - start) * 1e6 / (iterationCount * Stopwatch.Frequency)}"); var hullShapeIndex = Simulation.Shapes.Add(hullShape); + var boxHullShapeIndex = Simulation.Shapes.Add(boxHullShape); var inertia = hullShape.ComputeInertia(1); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 0, 0), inertia, new (hullShapeIndex, 20, 20), 0.01f)); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 0, 5), inertia, new(hullShapeIndex, 20, 20), -0.01f)); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 0, 3), boxHullShape.ComputeInertia(1), new(boxHullShapeIndex, 20, 20), -0.01f)); Simulation.Statics.Add(new StaticDescription(new Vector3(-25, -5, 0), Simulation.Shapes.Add(new Sphere(2)))); Simulation.Statics.Add(new StaticDescription(new Vector3(-20, -5, 0), Simulation.Shapes.Add(new Capsule(0.5f, 2)))); @@ -119,11 +341,12 @@ public override void Initialize(ContentArchive content, Camera camera) Simulation.Statics.Add(new StaticDescription(new Vector3(-5, -5, 0), Simulation.Shapes.Add(new Cylinder(1, 1)))); Simulation.Statics.Add(new StaticDescription(new Vector3(-5, -5, 5), Simulation.Shapes.Add(new Cylinder(1, 1)))); Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5, 0), hullShapeIndex)); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5, 5), Simulation.Shapes.Add(boxHullShape))); var spacing = new Vector3(3f, 3f, 3); int width = 16; int height = 16; - int length = 16; + int length = 0; var origin = -0.5f * spacing * new Vector3(width, 0, length) + new Vector3(40, 0.2f, -40); for (int i = 0; i < width; ++i) { @@ -159,64 +382,65 @@ void TestConvexHullCreation() } } - int stepIndex = 0; + //int stepIndex = 0; - public override void Update(Window window, Camera camera, Input input, float dt) - { - //if (input.TypedCharacters.Contains('x')) - //{ - // stepIndex = Math.Max(stepIndex - 1, 0); - //} - //if (input.TypedCharacters.Contains('c')) - //{ - // stepIndex = Math.Min(stepIndex + 1, debugSteps.Count - 1); - //} - base.Update(window, camera, input, dt); - } + //public override void Update(Window window, Camera camera, Input input, float dt) + //{ + // if (input.TypedCharacters.Contains('x')) + // { + // stepIndex = Math.Max(stepIndex - 1, 0); + // } + // if (input.TypedCharacters.Contains('c')) + // { + // stepIndex = Math.Min(stepIndex + 1, debugSteps.Count - 1); + // } + // base.Update(window, camera, input, dt); + //} - public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) - { - //var step = debugSteps[stepIndex]; - //var scale = 10f; - //for (int i = 0; i < points.Count; ++i) - //{ - // var pose = new RigidPose(points[i] * scale); - // renderer.Shapes.AddShape(new Box(0.1f, 0.1f, 0.1f), Simulation.Shapes, ref pose, new Vector3(0.5f, 0.5f, 0.5f)); - // if (!step.AllowVertex[i]) - // renderer.Shapes.AddShape(new Box(0.6f, 0.25f, 0.25f), Simulation.Shapes, ref pose, new Vector3(1, 0, 0)); - //} - //for (int i = 0; i < step.Raw.Count; ++i) - //{ - // var pose = new RigidPose(points[step.Raw[i]] * scale); - // renderer.Shapes.AddShape(new Box(0.25f, 0.6f, 0.25f), Simulation.Shapes, ref pose, new Vector3(0, 0, 1)); - //} - //for (int i = 0; i < step.Reduced.Count; ++i) - //{ - // var pose = new RigidPose(points[step.Reduced[i]] * scale); - // renderer.Shapes.AddShape(new Box(0.25f, 0.25f, 0.6f), Simulation.Shapes, ref pose, new Vector3(0, 1, 0)); - //} - //for (int i = 0; i <= stepIndex; ++i) - //{ - // var pose = RigidPose.Identity; - // var oldStep = debugSteps[i]; - // for (int j = 2; j < oldStep.Reduced.Count; ++j) - // { - // renderer.Shapes.AddShape(new Triangle - // { - // A = points[oldStep.Reduced[0]] * scale, - // B = points[oldStep.Reduced[j]] * scale, - // C = points[oldStep.Reduced[j - 1]] * scale - // }, Simulation.Shapes, ref pose, new Vector3(1, 0, 1)); + //public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + //{ + // var step = debugSteps[stepIndex]; + // var scale = 10f; + // var renderOffset = new Vector3(0, 15, 0); + // for (int i = 0; i < points.Length; ++i) + // { + // var pose = new RigidPose(renderOffset + points[i] * scale); + // renderer.Shapes.AddShape(new Box(0.1f, 0.1f, 0.1f), Simulation.Shapes, pose, new Vector3(0.5f, 0.5f, 0.5f)); + // if (!step.AllowVertex[i]) + // renderer.Shapes.AddShape(new Box(0.6f, 0.25f, 0.25f), Simulation.Shapes, pose, new Vector3(1, 0, 0)); + // } + // for (int i = 0; i < step.Raw.Count; ++i) + // { + // var pose = new RigidPose(renderOffset + points[step.Raw[i]] * scale); + // renderer.Shapes.AddShape(new Box(0.25f, 0.6f, 0.25f), Simulation.Shapes, pose, new Vector3(0, 0, 1)); + // } + // for (int i = 0; i < step.Reduced.Count; ++i) + // { + // var pose = new RigidPose(renderOffset + points[step.Reduced[i]] * scale); + // renderer.Shapes.AddShape(new Box(0.25f, 0.25f, 0.6f), Simulation.Shapes, pose, new Vector3(0, 1, 0)); + // } + // for (int i = 0; i <= stepIndex; ++i) + // { + // var pose = new RigidPose(renderOffset); + // var oldStep = debugSteps[i]; + // for (int j = 2; j < oldStep.Reduced.Count; ++j) + // { + // renderer.Shapes.AddShape(new Triangle + // { + // A = points[oldStep.Reduced[0]] * scale, + // B = points[oldStep.Reduced[j]] * scale, + // C = points[oldStep.Reduced[j - 1]] * scale + // }, Simulation.Shapes, pose, new Vector3(1, 0, 1)); - // } - //} - //var edgeMidpoint = (points[step.SourceEdge.A] + points[step.SourceEdge.B]) * scale * 0.5f; - //renderer.Lines.Allocate() = new LineInstance(edgeMidpoint, edgeMidpoint + step.BasisX * scale * 0.5f, new Vector3(1, 1, 0), new Vector3()); - //renderer.Lines.Allocate() = new LineInstance(edgeMidpoint, edgeMidpoint + step.BasisY * scale * 0.5f, new Vector3(0, 1, 0), new Vector3()); - //renderer.TextBatcher.Write( - // text.Clear().Append($"Enumerate step with X and C. Current step: ").Append(stepIndex + 1).Append(" out of ").Append(debugSteps.Count), - // new Vector2(32, renderer.Surface.Resolution.Y - 140), 20, new Vector3(1), font); - base.Render(renderer, camera, input, text, font); - } + // } + // } + // var edgeMidpoint = renderOffset + (points[step.SourceEdge.A] + points[step.SourceEdge.B]) * scale * 0.5f; + // renderer.Lines.Allocate() = new LineInstance(edgeMidpoint, edgeMidpoint + step.BasisX * scale * 0.5f, new Vector3(1, 1, 0), new Vector3()); + // renderer.Lines.Allocate() = new LineInstance(edgeMidpoint, edgeMidpoint + step.BasisY * scale * 0.5f, new Vector3(0, 1, 0), new Vector3()); + // renderer.TextBatcher.Write( + // text.Clear().Append($"Enumerate step with X and C. Current step: ").Append(stepIndex + 1).Append(" out of ").Append(debugSteps.Count), + // new Vector2(32, renderer.Surface.Resolution.Y - 140), 20, new Vector3(1), font); + // base.Render(renderer, camera, input, text, font); + //} } } From 3c0fe04111ff728761fb6818b578b207c1ed2a10 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 20 Aug 2022 15:42:42 -0500 Subject: [PATCH 515/947] Enabled debug spew for convex hull helper. It's got probs. --- BepuPhysics/Collidables/ConvexHullHelper.cs | 183 +++++++++++-------- Demos/DemoSet.cs | 1 + Demos/SpecializedTests/ConvexHullTestDemo.cs | 155 ++++++++-------- 3 files changed, 191 insertions(+), 148 deletions(-) diff --git a/BepuPhysics/Collidables/ConvexHullHelper.cs b/BepuPhysics/Collidables/ConvexHullHelper.cs index 6ab219277..2341d5bee 100644 --- a/BepuPhysics/Collidables/ConvexHullHelper.cs +++ b/BepuPhysics/Collidables/ConvexHullHelper.cs @@ -4,6 +4,7 @@ using System; using System.Collections.Generic; using System.Diagnostics; +using System.IO; using System.Numerics; using System.Runtime.CompilerServices; using System.Runtime.InteropServices; @@ -101,7 +102,7 @@ static void FindExtremeFace(in Vector3Wide basisX, in Vector3Wide basisY, in Vec Vector3Wide.Dot(basisX, toCandidate, out x); //If x is negative, that means some numerical issue has resulted in a point beyond the bounding plane that generated this face request. //We'll treat it as if it's on the plane. - x = Vector.Max(Vector.Zero, x); + //x = Vector.Max(Vector.Zero, x); Vector3Wide.Dot(basisY, toCandidate, out y); var bestY = y; var bestX = x; @@ -109,31 +110,40 @@ static void FindExtremeFace(in Vector3Wide basisX, in Vector3Wide basisY, in Vec var edgeIndexA = new Vector(sourceEdgeEndpoints.A); var edgeIndexB = new Vector(sourceEdgeEndpoints.B); var pointCountBundle = new Vector(pointCount); - //Note that any slot that would have been considered coplanar with the edge triggering this test is ignored by the plane epsilon. + ////Note that any slot that would have been considered coplanar with the edge triggering this test is ignored by the plane epsilon. + //var ignoreSlot = Vector.BitwiseOr( + // Vector.BitwiseOr(Vector.GreaterThanOrEqual(indexOffsets, pointCountBundle), Vector.LessThan(bestX, planeEpsilon)), + // Vector.BitwiseOr(Vector.Equals(indexOffsets, edgeIndexA), Vector.Equals(indexOffsets, edgeIndexB))); var ignoreSlot = Vector.BitwiseOr( - Vector.BitwiseOr(Vector.GreaterThanOrEqual(indexOffsets, pointCountBundle), Vector.LessThan(bestX, planeEpsilon)), + Vector.GreaterThanOrEqual(indexOffsets, pointCountBundle), Vector.BitwiseOr(Vector.Equals(indexOffsets, edgeIndexA), Vector.Equals(indexOffsets, edgeIndexB))); bestX = Vector.ConditionalSelect(ignoreSlot, Vector.One, bestX); bestY = Vector.ConditionalSelect(ignoreSlot, new Vector(float.MinValue), bestY); + var bestIndices = indexOffsets; for (int i = 1; i < pointBundles.Length; ++i) { Vector3Wide.Subtract(pointBundles[i], basisOrigin, out toCandidate); x = ref projectedOnX[i]; y = ref projectedOnY[i]; Vector3Wide.Dot(basisX, toCandidate, out x); - x = Vector.Max(Vector.Zero, x); //Same as earlier- protect against numerical error finding points beyond the bounding plane. + //x = Vector.Max(Vector.Zero, x); //Same as earlier- protect against numerical error finding points beyond the bounding plane. Vector3Wide.Dot(basisY, toCandidate, out y); var candidateIndices = indexOffsets + new Vector(i << BundleIndexing.VectorShift); + //ignoreSlot = Vector.BitwiseOr( + // Vector.BitwiseOr(Vector.GreaterThanOrEqual(candidateIndices, pointCountBundle), Vector.LessThan(x, planeEpsilon)), + // Vector.BitwiseOr(Vector.Equals(candidateIndices, edgeIndexA), Vector.Equals(candidateIndices, edgeIndexB))); ignoreSlot = Vector.BitwiseOr( - Vector.BitwiseOr(Vector.GreaterThanOrEqual(candidateIndices, pointCountBundle), Vector.LessThan(x, planeEpsilon)), + Vector.GreaterThanOrEqual(candidateIndices, pointCountBundle), Vector.BitwiseOr(Vector.Equals(candidateIndices, edgeIndexA), Vector.Equals(candidateIndices, edgeIndexB))); var useCandidate = Vector.AndNot(Vector.GreaterThan(y * bestX, bestY * x), ignoreSlot); bestY = Vector.ConditionalSelect(useCandidate, y, bestY); bestX = Vector.ConditionalSelect(useCandidate, x, bestX); + bestIndices = Vector.ConditionalSelect(useCandidate, candidateIndices, bestIndices); } var bestYNarrow = bestY[0]; var bestXNarrow = bestX[0]; + var bestIndexNarrow = bestIndices[0]; for (int i = 1; i < Vector.Count; ++i) { var candidateNumerator = bestY[i]; @@ -142,6 +152,7 @@ static void FindExtremeFace(in Vector3Wide basisX, in Vector3Wide basisY, in Vec { bestYNarrow = candidateNumerator; bestXNarrow = candidateDenominator; + bestIndexNarrow = bestIndices[i]; } } //We now have the best index, but there may have been multiple vertices on the same plane. Capture all of them at once by doing a second pass over the results. @@ -151,9 +162,40 @@ static void FindExtremeFace(in Vector3Wide basisX, in Vector3Wide basisY, in Vec //Rotate the offset to point outward. var projectedPlaneNormalNarrow = Vector2.Normalize(new Vector2(-bestYNarrow, bestXNarrow)); Vector2Wide.Broadcast(projectedPlaneNormalNarrow, out var projectedPlaneNormal); + Vector3Wide.ReadFirst(basisX, out var basisXNarrow); + Vector3Wide.ReadFirst(basisY, out var basisYNarrow); + faceNormal = basisXNarrow * projectedPlaneNormalNarrow.X + basisYNarrow * projectedPlaneNormalNarrow.Y; + + //if (sourceEdgeEndpoints.A != sourceEdgeEndpoints.B) + //{ + // BundleIndexing.GetBundleIndices(sourceEdgeEndpoints.A, out var bundleA, out var innerA); + // BundleIndexing.GetBundleIndices(sourceEdgeEndpoints.B, out var bundleB, out var innerB); + // BundleIndexing.GetBundleIndices(bestIndexNarrow, out var bundleC, out var innerC); + // Vector3Wide.ReadSlot(ref pointBundles[bundleA], innerA, out var a); + // Vector3Wide.ReadSlot(ref pointBundles[bundleB], innerB, out var b); + // Vector3Wide.ReadSlot(ref pointBundles[bundleC], innerC, out var c); + // var faceNormalFromCross = Vector3.Normalize(Vector3.Cross(c - a, b - a)); + // var testDot = Vector3.Dot(faceNormalFromCross, faceNormal); + // var faceNormalError = faceNormal - faceNormalFromCross; + // faceNormal = faceNormalFromCross; + //} + + Vector3Wide.Broadcast(faceNormal, out var faceNormalWide); for (int i = 0; i < pointBundles.Length; ++i) { var dot = projectedOnX[i] * projectedPlaneNormal.X + projectedOnY[i] * projectedPlaneNormal.Y; + //var dot2 = Vector3Wide.Dot(pointBundles[i] - basisOrigin, faceNormalWide); + //var error = dot2 - dot; + //if (Vector.GreaterThanAny(Vector.Abs(error), planeEpsilon)) + //{ + // for (int j = 0; j < Vector.Count; ++j) + // { + // if (MathF.Abs(error[j]) > planeEpsilon[j]) + // { + // Console.WriteLine($"error: {error[j]}"); + // } + // } + //} var coplanar = Vector.LessThanOrEqual(Vector.Abs(dot), planeEpsilon); if (Vector.LessThanAny(coplanar, Vector.Zero)) { @@ -170,9 +212,9 @@ static void FindExtremeFace(in Vector3Wide basisX, in Vector3Wide basisY, in Vec } } } - Vector3Wide.ReadFirst(basisX, out var basisXNarrow); - Vector3Wide.ReadFirst(basisY, out var basisYNarrow); - faceNormal = basisXNarrow * projectedPlaneNormalNarrow.X + basisYNarrow * projectedPlaneNormalNarrow.Y; + //Vector3Wide.ReadFirst(basisX, out var basisXNarrow); + //Vector3Wide.ReadFirst(basisY, out var basisYNarrow); + //faceNormal = basisXNarrow * projectedPlaneNormalNarrow.X + basisYNarrow * projectedPlaneNormalNarrow.Y; } @@ -387,57 +429,47 @@ struct EdgeToTest public int FaceIndex; } - //public struct DebugStep - //{ - // public EdgeEndpoints SourceEdge; - // public List Raw; - // public List Reduced; - // public bool[] AllowVertex; - // public Vector3 FaceNormal; - // public Vector3 BasisX; - // public Vector3 BasisY; - - // public DebugStep(EdgeEndpoints sourceEdge, ref QuickList raw, Vector3 faceNormal, Vector3 basisX, Vector3 basisY) - // { - // SourceEdge = sourceEdge; - // FaceNormal = faceNormal; - // BasisX = basisX; - // BasisY = basisY; - // Raw = new List(); - // for (int i = 0; i < raw.Count; ++i) - // { - // Raw.Add(raw[i]); - // } - // Reduced = default; - // AllowVertex = default; - // } - - // public void AddReduced(ref QuickList reduced, ref Buffer allowVertex) - // { - // Reduced = new List(); - // for (int i = 0; i < reduced.Count; ++i) - // { - // Reduced.Add(reduced[i]); - // } - // AllowVertex = new bool[allowVertex.Length]; - // for (int i = 0; i < allowVertex.Length; ++i) - // { - // AllowVertex[i] = allowVertex[i]; - // } - // } - - - //} - ///// - ///// Computes the convex hull of a set of points. - ///// - ///// Point set to compute the convex hull of. - ///// Buffer pool to pull memory from when creating the hull. - ///// Convex hull of the input point set. - //public static void ComputeHull(Span points, BufferPool pool, out HullData hullData) - //{ - // ComputeHull(points, pool, out hullData, out _); - //} + public struct DebugStep + { + public EdgeEndpoints SourceEdge; + public List Raw; + public List Reduced; + public bool[] AllowVertex; + public Vector3 FaceNormal; + public Vector3 BasisX; + public Vector3 BasisY; + + public DebugStep(EdgeEndpoints sourceEdge, ref QuickList raw, Vector3 faceNormal, Vector3 basisX, Vector3 basisY) + { + SourceEdge = sourceEdge; + FaceNormal = faceNormal; + BasisX = basisX; + BasisY = basisY; + Raw = new List(); + for (int i = 0; i < raw.Count; ++i) + { + Raw.Add(raw[i]); + } + Reduced = default; + AllowVertex = default; + } + + public void AddReduced(ref QuickList reduced, ref Buffer allowVertex) + { + Reduced = new List(); + for (int i = 0; i < reduced.Count; ++i) + { + Reduced.Add(reduced[i]); + } + AllowVertex = new bool[allowVertex.Length]; + for (int i = 0; i < allowVertex.Length; ++i) + { + AllowVertex[i] = allowVertex[i]; + } + } + + + } /// /// Computes the convex hull of a set of points. /// @@ -446,10 +478,20 @@ struct EdgeToTest /// Convex hull of the input point set. public static void ComputeHull(Span points, BufferPool pool, out HullData hullData) { + ComputeHull(points, pool, out hullData, out _); + } + /// + /// Computes the convex hull of a set of points. + /// + /// Point set to compute the convex hull of. + /// Buffer pool to pull memory from when creating the hull. + /// Convex hull of the input point set. + public static void ComputeHull(Span points, BufferPool pool, out HullData hullData, out List steps) + { + steps = new List(); if (points.Length <= 0) { hullData = default; - //steps = new List(); return; } if (points.Length <= 3) @@ -475,7 +517,6 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa hullData.FaceStartIndices = default; hullData.FaceVertexIndices = default; } - //steps = new List(); return; } var pointBundleCount = BundleIndexing.GetBundleCount(points.Length); @@ -537,7 +578,6 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa hullData.OriginalVertexMapping[0] = 0; hullData.FaceStartIndices = default; hullData.FaceVertexIndices = default; - //steps = new List(); pool.Return(ref pointBundles); return; } @@ -546,7 +586,7 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa Vector3Wide.Broadcast(initialVertex, out var initialVertexBundle); pool.Take>(pointBundles.Length, out var projectedOnX); pool.Take>(pointBundles.Length, out var projectedOnY); - var planeEpsilonNarrow = MathF.Sqrt(bestDistanceSquared) * 1e-4f; + var planeEpsilonNarrow = MathF.Sqrt(bestDistanceSquared) * 1e-2f; var planeEpsilon = new Vector(planeEpsilonNarrow); var rawFaceVertexIndices = new QuickList(pointBundles.Length * Vector.Count, pool); var initialSourceEdge = new EdgeEndpoints { A = initialIndex, B = initialIndex }; @@ -562,12 +602,11 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa Vector3Wide.ReadFirst(initialBasisX, out var debugInitialBasisX); Vector3Wide.ReadFirst(initialBasisY, out var debugInitialBasisY); - //steps = new List(); - //var step = new DebugStep(initialSourceEdge, ref rawFaceVertexIndices, initialFaceNormal, debugInitialBasisX, debugInitialBasisY); + var step = new DebugStep(initialSourceEdge, ref rawFaceVertexIndices, initialFaceNormal, debugInitialBasisX, debugInitialBasisY); ReduceFace(ref rawFaceVertexIndices, initialFaceNormal, points, planeEpsilonNarrow, ref facePoints, ref allowVertex, ref reducedFaceIndices); - //step.AddReduced(ref reducedFaceIndices, ref allowVertex); - //steps.Add(step); + step.AddReduced(ref reducedFaceIndices, ref allowVertex); + steps.Add(step); var earlyFaceIndices = new QuickList(points.Length, pool); var earlyFaceStartIndices = new QuickList(points.Length, pool); @@ -631,7 +670,7 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa Vector3Wide.Broadcast(edgeA, out var basisOrigin); rawFaceVertexIndices.Count = 0; FindExtremeFace(basisXBundle, basisYBundle, basisOrigin, edgeToTest.Endpoints, ref pointBundles, indexOffsetBundle, points.Length, ref projectedOnX, ref projectedOnY, planeEpsilon, ref rawFaceVertexIndices, out var faceNormal); - //step = new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY); + step = new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY); reducedFaceIndices.Count = 0; facePoints.Count = 0; ReduceFace(ref rawFaceVertexIndices, faceNormal, points, planeEpsilonNarrow, ref facePoints, ref allowVertex, ref reducedFaceIndices); @@ -640,8 +679,8 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa //Degenerate face found; don't bother creating work for it. continue; } - //step.AddReduced(ref reducedFaceIndices, ref allowVertex); - //steps.Add(step); + step.AddReduced(ref reducedFaceIndices, ref allowVertex); + steps.Add(step); var newFaceIndex = earlyFaceStartIndices.Count; earlyFaceStartIndices.Allocate(pool) = earlyFaceIndices.Count; @@ -658,8 +697,8 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa if (edgeFaceCounts.GetTableIndices(ref nextEdgeToTest.Endpoints, out var tableIndex, out var elementIndex)) { ref var edgeFaceCount = ref edgeFaceCounts.Values[elementIndex]; - //Debug.Assert(edgeFaceCount == 1, - // "While we let execution continue, this is an error condition and implies overlapping triangles are being generated." + + //Debug.Assert(edgeFaceCount == 1, + // "While we let execution continue, this is an error condition and implies overlapping triangles are being generated." + // "This tends to happen when there are many near-coplanar vertices, so numerical tolerances across different faces cannot consistently agree."); ++edgeFaceCount; } diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index f40e1dc29..c32bb3879 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -46,6 +46,7 @@ struct Option public DemoSet() { + AddOption(); AddOption(); AddOption(); AddOption(); diff --git a/Demos/SpecializedTests/ConvexHullTestDemo.cs b/Demos/SpecializedTests/ConvexHullTestDemo.cs index 73c01bfc0..86fa7670b 100644 --- a/Demos/SpecializedTests/ConvexHullTestDemo.cs +++ b/Demos/SpecializedTests/ConvexHullTestDemo.cs @@ -22,8 +22,8 @@ namespace Demos.SpecializedTests { public class ConvexHullTestDemo : Demo { - //Buffer points; - //List debugSteps; + Buffer points; + List debugSteps; unsafe Buffer CreateRandomConvexHullPoints() { @@ -263,26 +263,29 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); - var hullPoints = CreateTestConvexHull2(); + var hullPoints = CreateRandomConvexHullPoints(); //var hullPoints = CreateMeshConvexHull(content.Load(@"Content\newt.obj"), new Vector3(1, 1.5f, 1f)); var hullShape = new ConvexHull(hullPoints, BufferPool, out _); - //for (int i = 0; i < hullShape.FaceToVertexIndicesStart.Length; ++i) - //{ - // hullShape.GetVertexIndicesForFace(i, out var faceVertices); - // BundleIndexing.GetBundleIndices(i, out var normalBundleIndex, out var normalIndexInBundle); - // Vector3Wide.ReadSlot(ref hullShape.BoundingPlanes[normalBundleIndex].Normal, normalIndexInBundle, out var faceNormal); - // var offset = hullShape.BoundingPlanes[normalBundleIndex].Offset[normalIndexInBundle]; - // Console.WriteLine($"Face {i} errors:"); - // for (int j = 0; j < faceVertices.Length; ++j) - // { - // hullShape.GetPoint(faceVertices[j], out var point); - // var error = Vector3.Dot(point, faceNormal) - offset; - // Console.WriteLine($"v{j}: {error}"); - // } - //} + float largestError = 0; + for (int i = 0; i < hullShape.FaceToVertexIndicesStart.Length; ++i) + { + hullShape.GetVertexIndicesForFace(i, out var faceVertices); + BundleIndexing.GetBundleIndices(i, out var normalBundleIndex, out var normalIndexInBundle); + Vector3Wide.ReadSlot(ref hullShape.BoundingPlanes[normalBundleIndex].Normal, normalIndexInBundle, out var faceNormal); + var offset = hullShape.BoundingPlanes[normalBundleIndex].Offset[normalIndexInBundle]; + Console.WriteLine($"Face {i} errors:"); + for (int j = 0; j < faceVertices.Length; ++j) + { + hullShape.GetPoint(faceVertices[j], out var point); + var error = Vector3.Dot(point, faceNormal) - offset; + Console.WriteLine($"v{j}: {error}"); + largestError = MathF.Max(MathF.Abs(error), largestError); + } + } + Console.WriteLine($"Largest error: {largestError}"); - //ConvexHullHelper.ComputeHull(hullPoints, BufferPool, out var hullData, out debugSteps); - //this.points = hullPoints; + ConvexHullHelper.ComputeHull(hullPoints, BufferPool, out var hullData, out debugSteps); + this.points = hullPoints; var boxHullPoints = CreateBoxConvexHull(2); var boxHullShape = new ConvexHull(boxHullPoints, BufferPool, out _); @@ -382,65 +385,65 @@ void TestConvexHullCreation() } } - //int stepIndex = 0; + int stepIndex = 0; - //public override void Update(Window window, Camera camera, Input input, float dt) - //{ - // if (input.TypedCharacters.Contains('x')) - // { - // stepIndex = Math.Max(stepIndex - 1, 0); - // } - // if (input.TypedCharacters.Contains('c')) - // { - // stepIndex = Math.Min(stepIndex + 1, debugSteps.Count - 1); - // } - // base.Update(window, camera, input, dt); - //} + public override void Update(Window window, Camera camera, Input input, float dt) + { + if (input.TypedCharacters.Contains('x')) + { + stepIndex = Math.Max(stepIndex - 1, 0); + } + if (input.TypedCharacters.Contains('c')) + { + stepIndex = Math.Min(stepIndex + 1, debugSteps.Count - 1); + } + base.Update(window, camera, input, dt); + } - //public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) - //{ - // var step = debugSteps[stepIndex]; - // var scale = 10f; - // var renderOffset = new Vector3(0, 15, 0); - // for (int i = 0; i < points.Length; ++i) - // { - // var pose = new RigidPose(renderOffset + points[i] * scale); - // renderer.Shapes.AddShape(new Box(0.1f, 0.1f, 0.1f), Simulation.Shapes, pose, new Vector3(0.5f, 0.5f, 0.5f)); - // if (!step.AllowVertex[i]) - // renderer.Shapes.AddShape(new Box(0.6f, 0.25f, 0.25f), Simulation.Shapes, pose, new Vector3(1, 0, 0)); - // } - // for (int i = 0; i < step.Raw.Count; ++i) - // { - // var pose = new RigidPose(renderOffset + points[step.Raw[i]] * scale); - // renderer.Shapes.AddShape(new Box(0.25f, 0.6f, 0.25f), Simulation.Shapes, pose, new Vector3(0, 0, 1)); - // } - // for (int i = 0; i < step.Reduced.Count; ++i) - // { - // var pose = new RigidPose(renderOffset + points[step.Reduced[i]] * scale); - // renderer.Shapes.AddShape(new Box(0.25f, 0.25f, 0.6f), Simulation.Shapes, pose, new Vector3(0, 1, 0)); - // } - // for (int i = 0; i <= stepIndex; ++i) - // { - // var pose = new RigidPose(renderOffset); - // var oldStep = debugSteps[i]; - // for (int j = 2; j < oldStep.Reduced.Count; ++j) - // { - // renderer.Shapes.AddShape(new Triangle - // { - // A = points[oldStep.Reduced[0]] * scale, - // B = points[oldStep.Reduced[j]] * scale, - // C = points[oldStep.Reduced[j - 1]] * scale - // }, Simulation.Shapes, pose, new Vector3(1, 0, 1)); + public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + { + var step = debugSteps[stepIndex]; + var scale = 10f; + var renderOffset = new Vector3(0, 15, 0); + for (int i = 0; i < points.Length; ++i) + { + var pose = new RigidPose(renderOffset + points[i] * scale); + renderer.Shapes.AddShape(new Box(0.1f, 0.1f, 0.1f), Simulation.Shapes, pose, new Vector3(0.5f, 0.5f, 0.5f)); + if (!step.AllowVertex[i]) + renderer.Shapes.AddShape(new Box(0.6f, 0.25f, 0.25f), Simulation.Shapes, pose, new Vector3(1, 0, 0)); + } + for (int i = 0; i < step.Raw.Count; ++i) + { + var pose = new RigidPose(renderOffset + points[step.Raw[i]] * scale); + renderer.Shapes.AddShape(new Box(0.25f, 0.6f, 0.25f), Simulation.Shapes, pose, new Vector3(0, 0, 1)); + } + for (int i = 0; i < step.Reduced.Count; ++i) + { + var pose = new RigidPose(renderOffset + points[step.Reduced[i]] * scale); + renderer.Shapes.AddShape(new Box(0.25f, 0.25f, 0.6f), Simulation.Shapes, pose, new Vector3(0, 1, 0)); + } + for (int i = 0; i <= stepIndex; ++i) + { + var pose = new RigidPose(renderOffset); + var oldStep = debugSteps[i]; + for (int j = 2; j < oldStep.Reduced.Count; ++j) + { + renderer.Shapes.AddShape(new Triangle + { + A = points[oldStep.Reduced[0]] * scale, + B = points[oldStep.Reduced[j]] * scale, + C = points[oldStep.Reduced[j - 1]] * scale + }, Simulation.Shapes, pose, new Vector3(1, 0, 1)); - // } - // } - // var edgeMidpoint = renderOffset + (points[step.SourceEdge.A] + points[step.SourceEdge.B]) * scale * 0.5f; - // renderer.Lines.Allocate() = new LineInstance(edgeMidpoint, edgeMidpoint + step.BasisX * scale * 0.5f, new Vector3(1, 1, 0), new Vector3()); - // renderer.Lines.Allocate() = new LineInstance(edgeMidpoint, edgeMidpoint + step.BasisY * scale * 0.5f, new Vector3(0, 1, 0), new Vector3()); - // renderer.TextBatcher.Write( - // text.Clear().Append($"Enumerate step with X and C. Current step: ").Append(stepIndex + 1).Append(" out of ").Append(debugSteps.Count), - // new Vector2(32, renderer.Surface.Resolution.Y - 140), 20, new Vector3(1), font); - // base.Render(renderer, camera, input, text, font); - //} + } + } + var edgeMidpoint = renderOffset + (points[step.SourceEdge.A] + points[step.SourceEdge.B]) * scale * 0.5f; + renderer.Lines.Allocate() = new LineInstance(edgeMidpoint, edgeMidpoint + step.BasisX * scale * 0.5f, new Vector3(1, 1, 0), new Vector3()); + renderer.Lines.Allocate() = new LineInstance(edgeMidpoint, edgeMidpoint + step.BasisY * scale * 0.5f, new Vector3(0, 1, 0), new Vector3()); + renderer.TextBatcher.Write( + text.Clear().Append($"Enumerate step with X and C. Current step: ").Append(stepIndex + 1).Append(" out of ").Append(debugSteps.Count), + new Vector2(32, renderer.Surface.Resolution.Y - 140), 20, new Vector3(1), font); + base.Render(renderer, camera, input, text, font); + } } } From e0f2b792b3ed944a6e8ea65979bda001facd00fe Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 21 Aug 2022 13:39:53 -0500 Subject: [PATCH 516/947] Purged some unncessary ins, fixed some busted comments. --- BepuUtilities/Collections/QuickList.cs | 40 +++++++++----------------- 1 file changed, 14 insertions(+), 26 deletions(-) diff --git a/BepuUtilities/Collections/QuickList.cs b/BepuUtilities/Collections/QuickList.cs index dba02f1e8..dd9f86ce2 100644 --- a/BepuUtilities/Collections/QuickList.cs +++ b/BepuUtilities/Collections/QuickList.cs @@ -176,7 +176,7 @@ public void Compact(IUnmanagedMemoryPool pool) /// Start index of the added range. /// Number of elements in the added range. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void AddRangeUnsafely(in Buffer span, int start, int count) + public void AddRangeUnsafely(Buffer span, int start, int count) { Validate(); ValidateUnsafeAdd(count); @@ -192,7 +192,7 @@ public void AddRangeUnsafely(in Buffer span, int start, int count) /// Number of elements in the added range. /// Pool used to obtain a new span if needed. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void AddRange(in Buffer span, int start, int count, IUnmanagedMemoryPool pool) + public void AddRange(Buffer span, int start, int count, IUnmanagedMemoryPool pool) { EnsureCapacity(Count + count, pool); AddRangeUnsafely(span, start, count); @@ -202,10 +202,8 @@ public void AddRange(in Buffer span, int start, int count, IUnmanagedMemoryPo /// Adds the elements of a buffer to the QuickList without checking capacity. /// /// Span of elements to add. - /// Start index of the added range. - /// Number of elements in the added range. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void AddRangeUnsafely(in Buffer span) + public void AddRangeUnsafely(Buffer span) { AddRangeUnsafely(span, 0, span.Length); } @@ -214,11 +212,9 @@ public void AddRangeUnsafely(in Buffer span) /// Adds the elements of a buffer to the QuickList. /// /// Span of elements to add. - /// Start index of the added range. - /// Number of elements in the added range. /// Pool used to obtain a new span if needed. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void AddRange(in Buffer span, IUnmanagedMemoryPool pool) + public void AddRange(Buffer span, IUnmanagedMemoryPool pool) { EnsureCapacity(Count + span.Length, pool); AddRangeUnsafely(span, 0, span.Length); @@ -231,7 +227,7 @@ public void AddRange(in Buffer span, IUnmanagedMemoryPool pool) /// Start index of the added range. /// Number of elements in the added range. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void AddRangeUnsafely(in Span span, int start, int count) + public void AddRangeUnsafely(Span span, int start, int count) { Validate(); ValidateUnsafeAdd(count); @@ -247,7 +243,7 @@ public void AddRangeUnsafely(in Span span, int start, int count) /// Number of elements in the added range. /// Pool used to obtain a new span if needed. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void AddRange(in Span span, int start, int count, IUnmanagedMemoryPool pool) + public void AddRange(Span span, int start, int count, IUnmanagedMemoryPool pool) { EnsureCapacity(Count + count, pool); AddRangeUnsafely(span, start, count); @@ -257,10 +253,8 @@ public void AddRange(in Span span, int start, int count, IUnmanagedMemoryPool /// Adds the elements of a span to the QuickList without checking capacity. /// /// Span of elements to add. - /// Start index of the added range. - /// Number of elements in the added range. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void AddRangeUnsafely(in Span span) + public void AddRangeUnsafely(Span span) { AddRangeUnsafely(span, 0, span.Length); } @@ -269,11 +263,9 @@ public void AddRangeUnsafely(in Span span) /// Adds the elements of a span to the QuickList. /// /// Span of elements to add. - /// Start index of the added range. - /// Number of elements in the added range. /// Pool used to obtain a new span if needed. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void AddRange(in Span span, IUnmanagedMemoryPool pool) + public void AddRange(Span span, IUnmanagedMemoryPool pool) { EnsureCapacity(Count + span.Length, pool); AddRangeUnsafely(span, 0, span.Length); @@ -286,7 +278,7 @@ public void AddRange(in Span span, IUnmanagedMemoryPool pool) /// Start index of the added range. /// Number of elements in the added range. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void AddRangeUnsafely(in ReadOnlySpan span, int start, int count) + public void AddRangeUnsafely(ReadOnlySpan span, int start, int count) { Validate(); ValidateUnsafeAdd(count); @@ -302,7 +294,7 @@ public void AddRangeUnsafely(in ReadOnlySpan span, int start, int count) /// Number of elements in the added range. /// Pool used to obtain a new span if needed. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void AddRange(in ReadOnlySpan span, int start, int count, IUnmanagedMemoryPool pool) + public void AddRange(ReadOnlySpan span, int start, int count, IUnmanagedMemoryPool pool) { EnsureCapacity(Count + count, pool); AddRangeUnsafely(span, start, count); @@ -312,10 +304,8 @@ public void AddRange(in ReadOnlySpan span, int start, int count, IUnmanagedMe /// Adds the elements of a span to the QuickList without checking capacity. /// /// Span of elements to add. - /// Start index of the added range. - /// Number of elements in the added range. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void AddRangeUnsafely(in ReadOnlySpan span) + public void AddRangeUnsafely(ReadOnlySpan span) { AddRangeUnsafely(span, 0, span.Length); } @@ -324,11 +314,9 @@ public void AddRangeUnsafely(in ReadOnlySpan span) /// Adds the elements of a span to the QuickList. /// /// Span of elements to add. - /// Start index of the added range. - /// Number of elements in the added range. /// Pool used to obtain a new span if needed. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void AddRange(in ReadOnlySpan span, IUnmanagedMemoryPool pool) + public void AddRange(ReadOnlySpan span, IUnmanagedMemoryPool pool) { EnsureCapacity(Count + span.Length, pool); AddRangeUnsafely(span, 0, span.Length); @@ -678,12 +666,12 @@ public unsafe static implicit operator Span(in QuickList list) return new Span(list.Span.Memory, list.Count); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe static implicit operator ReadOnlySpan(in QuickList list) + public unsafe static implicit operator ReadOnlySpan(QuickList list) { return new ReadOnlySpan(list.Span.Memory, list.Count); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe static implicit operator Buffer(in QuickList list) + public unsafe static implicit operator Buffer(QuickList list) { return new Buffer(list.Span.Memory, list.Count, list.Span.Id); } From c81effd987d75262cba915e0e3b1f76b84ebd189 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 21 Aug 2022 13:40:16 -0500 Subject: [PATCH 517/947] In the middle of adding face merging. --- BepuPhysics/Collidables/ConvexHullHelper.cs | 190 ++++++++++++++++---- 1 file changed, 154 insertions(+), 36 deletions(-) diff --git a/BepuPhysics/Collidables/ConvexHullHelper.cs b/BepuPhysics/Collidables/ConvexHullHelper.cs index 2341d5bee..e0129aded 100644 --- a/BepuPhysics/Collidables/ConvexHullHelper.cs +++ b/BepuPhysics/Collidables/ConvexHullHelper.cs @@ -2,6 +2,7 @@ using BepuUtilities.Collections; using BepuUtilities.Memory; using System; +using System.Collections; using System.Collections.Generic; using System.Diagnostics; using System.IO; @@ -422,6 +423,35 @@ public override string ToString() return $"({A}, {B})"; } } + + /// + /// Tracks the faces associated with a detected surface edge. + /// During hull calculation, surface edges that have only one face associated with them should have an outstanding entry in the edges to visit set. + /// Surface edges can never validly have more than two faces associated with them. Two means that an edge is 'complete', or should be. + /// Detecting a third edge implies an error condition. By tracking *which* faces were associated with an edge, we can attempt to fix the error. + /// This type of error tends to occur when there is a numerical disagreement about what vertices are coplanar with a face. + /// One iteration could find what it thinks is a complete face, and a later iteration ends up finding more vertices *including* the ones already contained in the previous face. + /// That's a recipe for excessive edge faces, but it's also a direct indicator that we should merge the involved faces for being close enough to coplanar that the error happened in the first place. + /// + struct EdgeFaceIndices + { + public int FaceA; + public int FaceB; + public bool Complete => FaceA >= 0 && FaceB >= 0; + + public EdgeFaceIndices(int initialFaceIndex) + { + FaceA = initialFaceIndex; + FaceB = -1; + } + } + + struct EarlyFace + { + public QuickList VertexIndices; + public Vector3 Normal; + } + struct EdgeToTest { public EdgeEndpoints Endpoints; @@ -480,6 +510,14 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa { ComputeHull(points, pool, out hullData, out _); } + + static void AddFace(ref QuickList faces, BufferPool pool, Vector3 normal, QuickList vertexIndices) + { + ref var face = ref faces.Allocate(pool); + face = new EarlyFace { Normal = normal, VertexIndices = new QuickList(vertexIndices.Count, pool) }; + face.VertexIndices.AddRangeUnsafely(vertexIndices); + } + /// /// Computes the convex hull of a set of points. /// @@ -586,7 +624,8 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa Vector3Wide.Broadcast(initialVertex, out var initialVertexBundle); pool.Take>(pointBundles.Length, out var projectedOnX); pool.Take>(pointBundles.Length, out var projectedOnY); - var planeEpsilonNarrow = MathF.Sqrt(bestDistanceSquared) * 1e-2f; + var planeEpsilonNarrow = MathF.Sqrt(bestDistanceSquared) * 1e-6f; + var normalCoplanarityEpsilon = 1f - 1e-5f; var planeEpsilon = new Vector(planeEpsilonNarrow); var rawFaceVertexIndices = new QuickList(pointBundles.Length * Vector.Count, pool); var initialSourceEdge = new EdgeEndpoints { A = initialIndex, B = initialIndex }; @@ -608,11 +647,11 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa step.AddReduced(ref reducedFaceIndices, ref allowVertex); steps.Add(step); - var earlyFaceIndices = new QuickList(points.Length, pool); - var earlyFaceStartIndices = new QuickList(points.Length, pool); + var faces = new QuickList(points.Length, pool); var edgesToTest = new QuickList(points.Length, pool); - var edgeFaceCounts = new QuickDictionary(points.Length, pool); + var facesForEdges = new QuickDictionary(points.Length, pool); + var facesNeedingMerge = new QuickList(32, pool); if (reducedFaceIndices.Count >= 3) { //The initial face search found an actual face! That's a bit surprising since we didn't start from an edge offset, but rather an arbitrary direction. @@ -624,11 +663,10 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa edgeToAdd.Endpoints.B = reducedFaceIndices[i]; edgeToAdd.FaceNormal = initialFaceNormal; edgeToAdd.FaceIndex = 0; - edgeFaceCounts.Add(ref edgeToAdd.Endpoints, 1, pool); + facesForEdges.Add(ref edgeToAdd.Endpoints, new EdgeFaceIndices(0), pool); } //Since an actual face was found, we go ahead and output it into the face set. - earlyFaceStartIndices.Allocate(pool) = earlyFaceIndices.Count; - earlyFaceIndices.AddRange(reducedFaceIndices.Span, 0, reducedFaceIndices.Count, pool); + AddFace(ref faces, pool, initialFaceNormal, reducedFaceIndices); } else { @@ -647,12 +685,13 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa Helpers.Swap(ref edgeToAdd.Endpoints.A, ref edgeToAdd.Endpoints.B); } + while (edgesToTest.Count > 0) { edgesToTest.Pop(out var edgeToTest); //Make sure the new edge hasn't already been filled by another traversal. - var faceCountIndex = edgeFaceCounts.IndexOf(edgeToTest.Endpoints); - if (faceCountIndex >= 0 && edgeFaceCounts.Values[faceCountIndex] >= 2) + var faceCountIndex = facesForEdges.IndexOf(edgeToTest.Endpoints); + if (faceCountIndex >= 0 && facesForEdges.Values[faceCountIndex].Complete) continue; ref var edgeA = ref points[edgeToTest.Endpoints.A]; @@ -670,10 +709,11 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa Vector3Wide.Broadcast(edgeA, out var basisOrigin); rawFaceVertexIndices.Count = 0; FindExtremeFace(basisXBundle, basisYBundle, basisOrigin, edgeToTest.Endpoints, ref pointBundles, indexOffsetBundle, points.Length, ref projectedOnX, ref projectedOnY, planeEpsilon, ref rawFaceVertexIndices, out var faceNormal); - step = new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY); reducedFaceIndices.Count = 0; facePoints.Count = 0; + facesNeedingMerge.Count = 0; ReduceFace(ref rawFaceVertexIndices, faceNormal, points, planeEpsilonNarrow, ref facePoints, ref allowVertex, ref reducedFaceIndices); + step = new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY); if (reducedFaceIndices.Count < 3) { //Degenerate face found; don't bother creating work for it. @@ -682,11 +722,79 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa step.AddReduced(ref reducedFaceIndices, ref allowVertex); steps.Add(step); - var newFaceIndex = earlyFaceStartIndices.Count; - earlyFaceStartIndices.Allocate(pool) = earlyFaceIndices.Count; - earlyFaceIndices.AddRange(reducedFaceIndices.Span, 0, reducedFaceIndices.Count, pool); - edgeFaceCounts.EnsureCapacity(edgeFaceCounts.Count + reducedFaceIndices.Count, pool); + var newFaceIndex = faces.Count; + + //This implementation bites the bullet pretty hard on numerical problems. They most frequently arise from near coplanar vertices. + //It's possible that two iterations see different subsets of 'coplanar' vertices, either causing two faces with near equal normal or + //even causing an edge to have more than two faces associated with it. + //Before making any modifications to the existing data, iterate over all the edges in the current face to check for any such edges. + var previousEndpointIndex = reducedFaceIndices.Count - 1; + for (int i = 0; i < reducedFaceIndices.Count; ++i) + { + EdgeEndpoints edgeEndpoints; + edgeEndpoints.A = previousEndpointIndex; + edgeEndpoints.B = reducedFaceIndices[i]; + previousEndpointIndex = edgeEndpoints.B; + if (facesForEdges.GetTableIndices(ref edgeEndpoints, out var tableIndex, out var elementIndex)) + { + //There is already at least one face associated with this edge. Do we need to merge? + ref var edgeFaces = ref facesForEdges.Values[elementIndex]; + Debug.Assert(edgeFaces.FaceA >= 0); + var aDot = Vector3.Dot(faces[edgeFaces.FaceA].Normal, faceNormal); + if (edgeFaces.FaceB >= 0) + { + //The edge already has two faces associated with it. This is definitely a numerical error; separate coplanar faces were generated. + //We *must* merge at least one pair of faces. + //Note that technically both faces can end up being included, even though we've already tested A and B; + //the new face could be a bridge that's close enough to both, even if they were barely too far from each other. + var bDot = Vector3.Dot(faces[edgeFaces.FaceB].Normal, faceNormal); + if (aDot >= normalCoplanarityEpsilon && bDot >= normalCoplanarityEpsilon) + { + facesNeedingMerge.Allocate(pool) = edgeFaces.FaceA; + facesNeedingMerge.Allocate(pool) = edgeFaces.FaceB; + } + else + { + //If both aren't merge candidates, then just pick the one that's closer. + facesNeedingMerge.Allocate(pool) = aDot > bDot ? edgeFaces.FaceA : edgeFaces.FaceB; + } + } + else + { + //Only one face already present. Check if it's coplanar. + if (aDot >= normalCoplanarityEpsilon) + facesNeedingMerge.Allocate(pool) = edgeFaces.FaceA; + } + } + } + + if (facesNeedingMerge.Count > 0) + { + //The new face should not be added! Instead, we should merge its vertices into an existing face. + //Remove any duplicates. If we had perfect numerical precision, duplicates would be impossible, but... this whole thing is because we don't have perfect numerical precision. + for (int i = facesNeedingMerge.Count - 1; i >= 0; --i) + { + for (int j = i - 1; j >= 0; --j) + { + if (facesNeedingMerge[j] == facesNeedingMerge[i]) + { + facesNeedingMerge.FastRemoveAt(i); + } + } + } + var mergeTargetIndex = facesNeedingMerge[0]; + + //Remove all faces other than the merge target. + + + } + else + { + //Didn't need to perform a merge; add the face. + AddFace(ref faces, pool, faceNormal, reducedFaceIndices); + } + facesForEdges.EnsureCapacity(facesForEdges.Count + reducedFaceIndices.Count, pool); for (int i = 0; i < reducedFaceIndices.Count; ++i) { EdgeToTest nextEdgeToTest; @@ -694,28 +802,28 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa nextEdgeToTest.Endpoints.B = reducedFaceIndices[i]; nextEdgeToTest.FaceNormal = faceNormal; nextEdgeToTest.FaceIndex = newFaceIndex; - if (edgeFaceCounts.GetTableIndices(ref nextEdgeToTest.Endpoints, out var tableIndex, out var elementIndex)) + if (facesForEdges.GetTableIndices(ref nextEdgeToTest.Endpoints, out var tableIndex, out var elementIndex)) { - ref var edgeFaceCount = ref edgeFaceCounts.Values[elementIndex]; - //Debug.Assert(edgeFaceCount == 1, - // "While we let execution continue, this is an error condition and implies overlapping triangles are being generated." + - // "This tends to happen when there are many near-coplanar vertices, so numerical tolerances across different faces cannot consistently agree."); - ++edgeFaceCount; + ref var edgeFaceCount = ref facesForEdges.Values[elementIndex]; + Debug.Assert(edgeFaceCount.FaceB == -1, + "While we let execution continue, this is an error condition and implies overlapping triangles are being generated." + + "This tends to happen when there are many near-coplanar vertices, so numerical tolerances across different faces cannot consistently agree."); + edgeFaceCount.FaceB = newFaceIndex; } else { //This edge is not yet claimed by any edge. Claim it for the new face and add the edge for further testing. - edgeFaceCounts.Keys[edgeFaceCounts.Count] = nextEdgeToTest.Endpoints; - edgeFaceCounts.Values[edgeFaceCounts.Count] = 1; + facesForEdges.Keys[facesForEdges.Count] = nextEdgeToTest.Endpoints; + facesForEdges.Values[facesForEdges.Count] = new EdgeFaceIndices(newFaceIndex); //Use the encoding- all indices are offset by 1 since 0 represents 'empty'. - edgeFaceCounts.Table[tableIndex] = ++edgeFaceCounts.Count; + facesForEdges.Table[tableIndex] = ++facesForEdges.Count; edgesToTest.Allocate(pool) = nextEdgeToTest; } } } edgesToTest.Dispose(pool); - edgeFaceCounts.Dispose(pool); + facesForEdges.Dispose(pool); facePoints.Dispose(pool); reducedFaceIndices.Dispose(pool); rawFaceVertexIndices.Dispose(pool); @@ -725,23 +833,28 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa pool.Return(ref pointBundles); //Create a reduced hull point set from the face vertex references. - pool.Take(earlyFaceStartIndices.Count, out hullData.FaceStartIndices); - pool.Take(earlyFaceIndices.Count, out hullData.FaceVertexIndices); - earlyFaceStartIndices.Span.CopyTo(0, hullData.FaceStartIndices, 0, earlyFaceStartIndices.Count); + int totalIndexCount = 0; + for (int i = 0; i < faces.Count; ++i) + { + totalIndexCount += faces[i].VertexIndices.Count; + } + pool.Take(faces.Count, out hullData.FaceStartIndices); + pool.Take(totalIndexCount, out hullData.FaceVertexIndices); + var nextStartIndex = 0; pool.Take(points.Length, out var originalToHullIndexMapping); var hullToOriginalIndexMapping = new QuickList(points.Length, pool); for (int i = 0; i < points.Length; ++i) { originalToHullIndexMapping[i] = -1; } - for (int i = 0; i < earlyFaceStartIndices.Count; ++i) + for (int i = 0; i < faces.Count; ++i) { - var start = earlyFaceStartIndices[i]; - var nextIndex = i + 1; - var end = earlyFaceStartIndices.Count == nextIndex ? earlyFaceIndices.Count : earlyFaceStartIndices[nextIndex]; - for (int j = start; j < end; ++j) + var source = faces[i].VertexIndices; + hullData.FaceStartIndices[i] = nextStartIndex; + //source.Span.CopyTo(0, hullData.FaceVertexIndices, nextStartIndex, source.Count); + for (int j = 0; j < source.Count; ++j) { - var originalVertexIndex = earlyFaceIndices[j]; + var originalVertexIndex = source[j]; ref var originalToHull = ref originalToHullIndexMapping[originalVertexIndex]; if (originalToHull < 0) { @@ -749,8 +862,9 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa originalToHull = hullToOriginalIndexMapping.Count; hullToOriginalIndexMapping.AllocateUnsafely() = originalVertexIndex; } - hullData.FaceVertexIndices[j] = originalToHull; + hullData.FaceVertexIndices[nextStartIndex + j] = originalToHull; } + nextStartIndex += source.Count; } pool.Take(hullToOriginalIndexMapping.Count, out hullData.OriginalVertexMapping); @@ -758,8 +872,12 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa pool.Return(ref originalToHullIndexMapping); hullToOriginalIndexMapping.Dispose(pool); - earlyFaceIndices.Dispose(pool); - earlyFaceStartIndices.Dispose(pool); + for (int i = 0; i < faces.Count; ++i) + { + faces[i].VertexIndices.Dispose(pool); + } + faces.Dispose(pool); + facesNeedingMerge.Dispose(pool); } /// From 98312a337cfacc880a26c09460be0cafa2a723b9 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 21 Aug 2022 17:06:54 -0500 Subject: [PATCH 518/947] Still in the middle of face merging. Not done yet! --- BepuPhysics/Collidables/ConvexHullHelper.cs | 190 ++++++++++++++----- Demos/SpecializedTests/ConvexHullTestDemo.cs | 4 +- 2 files changed, 147 insertions(+), 47 deletions(-) diff --git a/BepuPhysics/Collidables/ConvexHullHelper.cs b/BepuPhysics/Collidables/ConvexHullHelper.cs index e0129aded..cfe123242 100644 --- a/BepuPhysics/Collidables/ConvexHullHelper.cs +++ b/BepuPhysics/Collidables/ConvexHullHelper.cs @@ -4,6 +4,7 @@ using System; using System.Collections; using System.Collections.Generic; +using System.Data; using System.Diagnostics; using System.IO; using System.Numerics; @@ -444,11 +445,42 @@ public EdgeFaceIndices(int initialFaceIndex) FaceA = initialFaceIndex; FaceB = -1; } + + + } + + static void RemoveFaceFromEdge(int a, int b, int faceIndex, ref QuickDictionary facesForEdges) + { + EdgeEndpoints edge; + edge.A = a; + edge.B = b; + var exists = facesForEdges.GetTableIndices(ref edge, out _, out var index); + Debug.Assert(exists, "Whoa something weird happened here! A face was created, but there's a missing edge for its face?"); + ref var facesForEdge = ref facesForEdges.Values[index]; + Debug.Assert(faceIndex == facesForEdge.FaceA || faceIndex == facesForEdge.FaceB, "If you're trying to remove a face index from the edge, it better be in the edge!"); + if (facesForEdge.FaceA == faceIndex) + { + if (facesForEdge.FaceB == -1) + { + //This edge is no longer going to have any faces associated with it. + facesForEdges.FastRemove(ref edge); + } + facesForEdge.FaceA = facesForEdge.FaceB; + facesForEdge.FaceB = -1; + } + facesForEdge.FaceB = -1; + + //Note that we do *not* add the edge back into the 'edges to test' list when transitioning from 2 faces to 1 face for the edge. + //This function is invoked when an edge is being deleted because it's related to a deleted face. + //There are two possible outcomes: + //1. The completion of the merge will see a face added back to this edge, + //2. it's an orphaned internal edge that should not be tested. } struct EarlyFace { public QuickList VertexIndices; + public bool Deleted; public Vector3 Normal; } @@ -468,8 +500,9 @@ public struct DebugStep public Vector3 FaceNormal; public Vector3 BasisX; public Vector3 BasisY; + public bool Merge; - public DebugStep(EdgeEndpoints sourceEdge, ref QuickList raw, Vector3 faceNormal, Vector3 basisX, Vector3 basisY) + public DebugStep(EdgeEndpoints sourceEdge, ref QuickList raw, Vector3 faceNormal, Vector3 basisX, Vector3 basisY, ref QuickList reduced, ref Buffer allowVertex, bool merge = false) { SourceEdge = sourceEdge; FaceNormal = faceNormal; @@ -480,12 +513,6 @@ public DebugStep(EdgeEndpoints sourceEdge, ref QuickList raw, Vector3 faceN { Raw.Add(raw[i]); } - Reduced = default; - AllowVertex = default; - } - - public void AddReduced(ref QuickList reduced, ref Buffer allowVertex) - { Reduced = new List(); for (int i = 0; i < reduced.Count; ++i) { @@ -496,9 +523,8 @@ public void AddReduced(ref QuickList reduced, ref Buffer allowVertex) { AllowVertex[i] = allowVertex[i]; } + Merge = merge; } - - } /// /// Computes the convex hull of a set of points. @@ -518,6 +544,44 @@ static void AddFace(ref QuickList faces, BufferPool pool, Vector3 nor face.VertexIndices.AddRangeUnsafely(vertexIndices); } + static void AddFaceToEdgesAndTestList(BufferPool pool, + ref QuickList reducedFaceIndices, + ref QuickList edgesToTest, + ref QuickDictionary facesForEdges, + Vector3 faceNormal, int newFaceIndex) + { + facesForEdges.EnsureCapacity(facesForEdges.Count + reducedFaceIndices.Count, pool); + var previousIndex = reducedFaceIndices[reducedFaceIndices.Count - 1]; + for (int i = 0; i < reducedFaceIndices.Count; ++i) + { + EdgeEndpoints endpoints; + endpoints.A = previousIndex; + endpoints.B = reducedFaceIndices[i]; + previousIndex = endpoints.B; + if (facesForEdges.GetTableIndices(ref endpoints, out var tableIndex, out var elementIndex)) + { + ref var edgeFaceCount = ref facesForEdges.Values[elementIndex]; + Debug.Assert(edgeFaceCount.FaceB == -1, + "While we let execution continue, this is an error condition and implies overlapping triangles are being generated." + + "This tends to happen when there are many near-coplanar vertices, so numerical tolerances across different faces cannot consistently agree."); + edgeFaceCount.FaceB = newFaceIndex; + } + else + { + //This edge is not yet claimed by any edge. Claim it for the new face and add the edge for further testing. + EdgeToTest nextEdgeToTest; + nextEdgeToTest.Endpoints = endpoints; + nextEdgeToTest.FaceNormal = faceNormal; + nextEdgeToTest.FaceIndex = newFaceIndex; + facesForEdges.Keys[facesForEdges.Count] = nextEdgeToTest.Endpoints; + facesForEdges.Values[facesForEdges.Count] = new EdgeFaceIndices(newFaceIndex); + //Use the encoding- all indices are offset by 1 since 0 represents 'empty'. + facesForEdges.Table[tableIndex] = ++facesForEdges.Count; + edgesToTest.Allocate(pool) = nextEdgeToTest; + } + } + } + /// /// Computes the convex hull of a set of points. /// @@ -639,19 +703,18 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa for (int i = 0; i < points.Length; ++i) allowVertex[i] = true; + ReduceFace(ref rawFaceVertexIndices, initialFaceNormal, points, planeEpsilonNarrow, ref facePoints, ref allowVertex, ref reducedFaceIndices); + Vector3Wide.ReadFirst(initialBasisX, out var debugInitialBasisX); Vector3Wide.ReadFirst(initialBasisY, out var debugInitialBasisY); - var step = new DebugStep(initialSourceEdge, ref rawFaceVertexIndices, initialFaceNormal, debugInitialBasisX, debugInitialBasisY); - - ReduceFace(ref rawFaceVertexIndices, initialFaceNormal, points, planeEpsilonNarrow, ref facePoints, ref allowVertex, ref reducedFaceIndices); - step.AddReduced(ref reducedFaceIndices, ref allowVertex); - steps.Add(step); + steps.Add(new DebugStep(initialSourceEdge, ref rawFaceVertexIndices, initialFaceNormal, debugInitialBasisX, debugInitialBasisY, ref reducedFaceIndices, ref allowVertex)); var faces = new QuickList(points.Length, pool); var edgesToTest = new QuickList(points.Length, pool); var facesForEdges = new QuickDictionary(points.Length, pool); var facesNeedingMerge = new QuickList(32, pool); + var mergedIndices = new QuickList(32, pool); if (reducedFaceIndices.Count >= 3) { //The initial face search found an actual face! That's a bit surprising since we didn't start from an edge offset, but rather an arbitrary direction. @@ -685,6 +748,7 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa Helpers.Swap(ref edgeToAdd.Endpoints.A, ref edgeToAdd.Endpoints.B); } + int facesDeleted = 0; while (edgesToTest.Count > 0) { @@ -713,15 +777,12 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa facePoints.Count = 0; facesNeedingMerge.Count = 0; ReduceFace(ref rawFaceVertexIndices, faceNormal, points, planeEpsilonNarrow, ref facePoints, ref allowVertex, ref reducedFaceIndices); - step = new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY); if (reducedFaceIndices.Count < 3) { + steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertex)); //Degenerate face found; don't bother creating work for it. continue; } - step.AddReduced(ref reducedFaceIndices, ref allowVertex); - steps.Add(step); - var newFaceIndex = faces.Count; @@ -785,40 +846,75 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa } var mergeTargetIndex = facesNeedingMerge[0]; - //Remove all faces other than the merge target. + ref var targetFace = ref faces[mergeTargetIndex]; + //Accumulate all the vertices present in the candidate face and any other faces being merged together. + mergedIndices.Count = 0; + mergedIndices.AddRange(targetFace.VertexIndices, pool); + for (int j = 0; j < reducedFaceIndices.Count; ++j) + { + if (!mergedIndices.Contains(reducedFaceIndices[j])) + { + mergedIndices.Allocate(pool) = reducedFaceIndices[j]; + } + } + for (int i = 1; i < facesNeedingMerge.Count; ++i) + { + ref var sourceFace = ref faces[facesNeedingMerge[i]]; + for (int j = 0; j < sourceFace.VertexIndices.Count; ++j) + { + if (!mergedIndices.Contains(sourceFace.VertexIndices[j])) + { + mergedIndices.Allocate(pool) = sourceFace.VertexIndices[j]; + } + } + //Mark the face as deleted, but defer the vertex disposal for simplicity. + sourceFace.Deleted = true; + } + //For every edge of any face we're merging, remove the face from the edge lists. + for (int i = 0; i < facesNeedingMerge.Count; ++i) + { + var faceIndex = facesNeedingMerge[i]; + ref var faceToRemove = ref faces[faceIndex]; + var previousIndex = faceToRemove.VertexIndices[faceToRemove.VertexIndices.Count - 1]; + for (int j = 0; j < faceToRemove.VertexIndices.Count; ++j) + { + RemoveFaceFromEdge(previousIndex, faceToRemove.VertexIndices[j], faceIndex, ref facesForEdges); + previousIndex = faceToRemove.VertexIndices[j]; + } + //Remove any references to this face in the edges to test. + int removedEdgesToTestCount = 0; + for (int j = 0; j < edgesToTest.Count; ++j) + { + if (edgesToTest[j].FaceIndex == faceIndex) + { + ++removedEdgesToTestCount; + } + else if (removedEdgesToTestCount > 0) + { + //Scoot edges to test back. + edgesToTest[j - removedEdgesToTestCount] = edgesToTest[j]; + } + } + edgesToTest.Count -= removedEdgesToTestCount; + } + //Re-reduce the merged vertices to get the new face vertices. + facePoints.Count = 0; + targetFace.VertexIndices.Count = 0; + ReduceFace(ref mergedIndices, targetFace.Normal, points, planeEpsilonNarrow, ref facePoints, ref allowVertex, ref targetFace.VertexIndices); + AddFaceToEdgesAndTestList(pool, ref targetFace.VertexIndices, ref edgesToTest, ref facesForEdges, targetFace.Normal, mergeTargetIndex); + steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref targetFace.VertexIndices, ref allowVertex, true)); + //Note that we do *not* remove deleted faces from the faces list. That would break all the face indices that we've accumulated. + //Instead, we'll leave the the empty face there and deal with remapping stuff at the end once all faces have been processed. + facesDeleted += facesNeedingMerge.Count - 1; } else { //Didn't need to perform a merge; add the face. AddFace(ref faces, pool, faceNormal, reducedFaceIndices); - } - facesForEdges.EnsureCapacity(facesForEdges.Count + reducedFaceIndices.Count, pool); - for (int i = 0; i < reducedFaceIndices.Count; ++i) - { - EdgeToTest nextEdgeToTest; - nextEdgeToTest.Endpoints.A = reducedFaceIndices[i == 0 ? reducedFaceIndices.Count - 1 : i - 1]; - nextEdgeToTest.Endpoints.B = reducedFaceIndices[i]; - nextEdgeToTest.FaceNormal = faceNormal; - nextEdgeToTest.FaceIndex = newFaceIndex; - if (facesForEdges.GetTableIndices(ref nextEdgeToTest.Endpoints, out var tableIndex, out var elementIndex)) - { - ref var edgeFaceCount = ref facesForEdges.Values[elementIndex]; - Debug.Assert(edgeFaceCount.FaceB == -1, - "While we let execution continue, this is an error condition and implies overlapping triangles are being generated." + - "This tends to happen when there are many near-coplanar vertices, so numerical tolerances across different faces cannot consistently agree."); - edgeFaceCount.FaceB = newFaceIndex; - } - else - { - //This edge is not yet claimed by any edge. Claim it for the new face and add the edge for further testing. - facesForEdges.Keys[facesForEdges.Count] = nextEdgeToTest.Endpoints; - facesForEdges.Values[facesForEdges.Count] = new EdgeFaceIndices(newFaceIndex); - //Use the encoding- all indices are offset by 1 since 0 represents 'empty'. - facesForEdges.Table[tableIndex] = ++facesForEdges.Count; - edgesToTest.Allocate(pool) = nextEdgeToTest; - } + AddFaceToEdgesAndTestList(pool, ref reducedFaceIndices, ref edgesToTest, ref facesForEdges, faceNormal, newFaceIndex); + steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertex)); } } @@ -832,6 +928,8 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa pool.Return(ref projectedOnY); pool.Return(ref pointBundles); + Debug.Assert(facesDeleted == 0, "You still need to handle the case where faces were deleted!"); + //Create a reduced hull point set from the face vertex references. int totalIndexCount = 0; for (int i = 0; i < faces.Count; ++i) @@ -878,8 +976,10 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa } faces.Dispose(pool); facesNeedingMerge.Dispose(pool); + mergedIndices.Dispose(pool); } + /// /// Processes hull data into a runtime usable convex hull shape. Recenters the convex hull's points around its center of mass. /// diff --git a/Demos/SpecializedTests/ConvexHullTestDemo.cs b/Demos/SpecializedTests/ConvexHullTestDemo.cs index 86fa7670b..413d7111f 100644 --- a/Demos/SpecializedTests/ConvexHullTestDemo.cs +++ b/Demos/SpecializedTests/ConvexHullTestDemo.cs @@ -426,6 +426,7 @@ public override void Render(Renderer renderer, Camera camera, Input input, TextB { var pose = new RigidPose(renderOffset); var oldStep = debugSteps[i]; + var color = oldStep.Merge ? new Vector3(0.5f, 0f, 1f) : new Vector3(1, 0, 1); for (int j = 2; j < oldStep.Reduced.Count; ++j) { renderer.Shapes.AddShape(new Triangle @@ -433,8 +434,7 @@ public override void Render(Renderer renderer, Camera camera, Input input, TextB A = points[oldStep.Reduced[0]] * scale, B = points[oldStep.Reduced[j]] * scale, C = points[oldStep.Reduced[j - 1]] * scale - }, Simulation.Shapes, pose, new Vector3(1, 0, 1)); - + }, Simulation.Shapes, pose, color); } } var edgeMidpoint = renderOffset + (points[step.SourceEdge.A] + points[step.SourceEdge.B]) * scale * 0.5f; From 9474f2f50820aff9df39fbc68c835c81a7fe4ab2 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 23 Aug 2022 17:26:02 -0500 Subject: [PATCH 519/947] Iterated face merging. --- BepuPhysics/Collidables/ConvexHullHelper.cs | 244 +++++++++++--------- 1 file changed, 140 insertions(+), 104 deletions(-) diff --git a/BepuPhysics/Collidables/ConvexHullHelper.cs b/BepuPhysics/Collidables/ConvexHullHelper.cs index cfe123242..bf9f6f76c 100644 --- a/BepuPhysics/Collidables/ConvexHullHelper.cs +++ b/BepuPhysics/Collidables/ConvexHullHelper.cs @@ -748,7 +748,7 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa Helpers.Swap(ref edgeToAdd.Endpoints.A, ref edgeToAdd.Endpoints.B); } - int facesDeleted = 0; + int facesDeletedCount = 0; while (edgesToTest.Count > 0) { @@ -775,7 +775,6 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa FindExtremeFace(basisXBundle, basisYBundle, basisOrigin, edgeToTest.Endpoints, ref pointBundles, indexOffsetBundle, points.Length, ref projectedOnX, ref projectedOnY, planeEpsilon, ref rawFaceVertexIndices, out var faceNormal); reducedFaceIndices.Count = 0; facePoints.Count = 0; - facesNeedingMerge.Count = 0; ReduceFace(ref rawFaceVertexIndices, faceNormal, points, planeEpsilonNarrow, ref facePoints, ref allowVertex, ref reducedFaceIndices); if (reducedFaceIndices.Count < 3) { @@ -783,138 +782,155 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa //Degenerate face found; don't bother creating work for it. continue; } + var faceCountPriorToAdd = faces.Count; + var targetFaceIndex = faceCountPriorToAdd; - var newFaceIndex = faces.Count; - - //This implementation bites the bullet pretty hard on numerical problems. They most frequently arise from near coplanar vertices. - //It's possible that two iterations see different subsets of 'coplanar' vertices, either causing two faces with near equal normal or - //even causing an edge to have more than two faces associated with it. - //Before making any modifications to the existing data, iterate over all the edges in the current face to check for any such edges. - var previousEndpointIndex = reducedFaceIndices.Count - 1; - for (int i = 0; i < reducedFaceIndices.Count; ++i) + while (true) { - EdgeEndpoints edgeEndpoints; - edgeEndpoints.A = previousEndpointIndex; - edgeEndpoints.B = reducedFaceIndices[i]; - previousEndpointIndex = edgeEndpoints.B; - if (facesForEdges.GetTableIndices(ref edgeEndpoints, out var tableIndex, out var elementIndex)) + //This implementation bites the bullet pretty hard on numerical problems. They most frequently arise from near coplanar vertices. + //It's possible that two iterations see different subsets of 'coplanar' vertices, either causing two faces with near equal normal or + //even causing an edge to have more than two faces associated with it. + //Before making any modifications to the existing data, iterate over all the edges in the current face to check for any such edges. + + //While the usual flow here will be to reduce the vertices we just found and accept the result immediately, + //it's possible for the just-detected face to end up coplanar with an existing face because of numerical issues that prevented detecting the whole face earlier. + //Rather than using more precise math, we detect the failure and merge faces after the fact. + //Unfortunately, this can happen recursively! Merging two redundant faces could reveal a third face that also needs to be merged. + //This is uncommon, but it needs to be covered. + //So, we just stick everything into a while loop and let it iterate until it's done. + //The good news is that the merging process won't loop forever- every merge results in a face that is at least as large as any contributor, and any internal points + //that get reduced out of consideration are marked with allowVertex[whateverInteriorPointIndex] = false. + var previousEndpointIndex = reducedFaceIndices.Count - 1; + facesNeedingMerge.Count = 0; + for (int i = 0; i < reducedFaceIndices.Count; ++i) { - //There is already at least one face associated with this edge. Do we need to merge? - ref var edgeFaces = ref facesForEdges.Values[elementIndex]; - Debug.Assert(edgeFaces.FaceA >= 0); - var aDot = Vector3.Dot(faces[edgeFaces.FaceA].Normal, faceNormal); - if (edgeFaces.FaceB >= 0) + EdgeEndpoints edgeEndpoints; + edgeEndpoints.A = previousEndpointIndex; + edgeEndpoints.B = reducedFaceIndices[i]; + previousEndpointIndex = edgeEndpoints.B; + if (facesForEdges.GetTableIndices(ref edgeEndpoints, out var tableIndex, out var elementIndex)) { - //The edge already has two faces associated with it. This is definitely a numerical error; separate coplanar faces were generated. - //We *must* merge at least one pair of faces. - //Note that technically both faces can end up being included, even though we've already tested A and B; - //the new face could be a bridge that's close enough to both, even if they were barely too far from each other. - var bDot = Vector3.Dot(faces[edgeFaces.FaceB].Normal, faceNormal); - if (aDot >= normalCoplanarityEpsilon && bDot >= normalCoplanarityEpsilon) + //There is already at least one face associated with this edge. Do we need to merge? + ref var edgeFaces = ref facesForEdges.Values[elementIndex]; + Debug.Assert(edgeFaces.FaceA >= 0); + var aDot = Vector3.Dot(faces[edgeFaces.FaceA].Normal, faceNormal); + if (edgeFaces.FaceB >= 0) { - facesNeedingMerge.Allocate(pool) = edgeFaces.FaceA; - facesNeedingMerge.Allocate(pool) = edgeFaces.FaceB; + //The edge already has two faces associated with it. This is definitely a numerical error; separate coplanar faces were generated. + //We *must* merge at least one pair of faces. + //Note that technically both faces can end up being included, even though we've already tested A and B; + //the new face could be a bridge that's close enough to both, even if they were barely too far from each other. + var bDot = Vector3.Dot(faces[edgeFaces.FaceB].Normal, faceNormal); + if (aDot >= normalCoplanarityEpsilon && bDot >= normalCoplanarityEpsilon) + { + facesNeedingMerge.Allocate(pool) = edgeFaces.FaceA; + facesNeedingMerge.Allocate(pool) = edgeFaces.FaceB; + } + else + { + //If both aren't merge candidates, then just pick the one that's closer. + facesNeedingMerge.Allocate(pool) = aDot > bDot ? edgeFaces.FaceA : edgeFaces.FaceB; + } } else { - //If both aren't merge candidates, then just pick the one that's closer. - facesNeedingMerge.Allocate(pool) = aDot > bDot ? edgeFaces.FaceA : edgeFaces.FaceB; + //Only one face already present. Check if it's coplanar. + if (aDot >= normalCoplanarityEpsilon) + facesNeedingMerge.Allocate(pool) = edgeFaces.FaceA; } } - else - { - //Only one face already present. Check if it's coplanar. - if (aDot >= normalCoplanarityEpsilon) - facesNeedingMerge.Allocate(pool) = edgeFaces.FaceA; - } } - } - if (facesNeedingMerge.Count > 0) - { - //The new face should not be added! Instead, we should merge its vertices into an existing face. - //Remove any duplicates. If we had perfect numerical precision, duplicates would be impossible, but... this whole thing is because we don't have perfect numerical precision. - for (int i = facesNeedingMerge.Count - 1; i >= 0; --i) + if (facesNeedingMerge.Count > 0) { - for (int j = i - 1; j >= 0; --j) + //The new face should not be added! Instead, we should merge its vertices into an existing face. + //Remove any duplicates. If we had perfect numerical precision, duplicates would be impossible, but... this whole thing is because we don't have perfect numerical precision. + for (int i = facesNeedingMerge.Count - 1; i >= 0; --i) { - if (facesNeedingMerge[j] == facesNeedingMerge[i]) + for (int j = i - 1; j >= 0; --j) { - facesNeedingMerge.FastRemoveAt(i); + if (facesNeedingMerge[j] == facesNeedingMerge[i]) + { + facesNeedingMerge.FastRemoveAt(i); + } } } - } - var mergeTargetIndex = facesNeedingMerge[0]; + targetFaceIndex = facesNeedingMerge[0]; - ref var targetFace = ref faces[mergeTargetIndex]; - //Accumulate all the vertices present in the candidate face and any other faces being merged together. - mergedIndices.Count = 0; - mergedIndices.AddRange(targetFace.VertexIndices, pool); - for (int j = 0; j < reducedFaceIndices.Count; ++j) - { - if (!mergedIndices.Contains(reducedFaceIndices[j])) + ref var targetFace = ref faces[targetFaceIndex]; + //Accumulate all the vertices present in the candidate face and any other faces being merged together. + mergedIndices.Count = 0; + mergedIndices.AddRange(targetFace.VertexIndices, pool); + for (int j = 0; j < reducedFaceIndices.Count; ++j) { - mergedIndices.Allocate(pool) = reducedFaceIndices[j]; - } - } - for (int i = 1; i < facesNeedingMerge.Count; ++i) - { - ref var sourceFace = ref faces[facesNeedingMerge[i]]; - for (int j = 0; j < sourceFace.VertexIndices.Count; ++j) - { - if (!mergedIndices.Contains(sourceFace.VertexIndices[j])) + if (!mergedIndices.Contains(reducedFaceIndices[j])) { - mergedIndices.Allocate(pool) = sourceFace.VertexIndices[j]; + mergedIndices.Allocate(pool) = reducedFaceIndices[j]; } } - //Mark the face as deleted, but defer the vertex disposal for simplicity. - sourceFace.Deleted = true; - } - //For every edge of any face we're merging, remove the face from the edge lists. - for (int i = 0; i < facesNeedingMerge.Count; ++i) - { - var faceIndex = facesNeedingMerge[i]; - ref var faceToRemove = ref faces[faceIndex]; - var previousIndex = faceToRemove.VertexIndices[faceToRemove.VertexIndices.Count - 1]; - for (int j = 0; j < faceToRemove.VertexIndices.Count; ++j) + for (int i = 1; i < facesNeedingMerge.Count; ++i) { - RemoveFaceFromEdge(previousIndex, faceToRemove.VertexIndices[j], faceIndex, ref facesForEdges); - previousIndex = faceToRemove.VertexIndices[j]; + ref var sourceFace = ref faces[facesNeedingMerge[i]]; + for (int j = 0; j < sourceFace.VertexIndices.Count; ++j) + { + if (!mergedIndices.Contains(sourceFace.VertexIndices[j])) + { + mergedIndices.Allocate(pool) = sourceFace.VertexIndices[j]; + } + } + //Mark the face as deleted, but defer the vertex disposal for simplicity. + sourceFace.Deleted = true; } - //Remove any references to this face in the edges to test. - int removedEdgesToTestCount = 0; - for (int j = 0; j < edgesToTest.Count; ++j) + //For every edge of any face we're merging, remove the face from the edge lists. + for (int i = 0; i < facesNeedingMerge.Count; ++i) { - if (edgesToTest[j].FaceIndex == faceIndex) + var faceIndex = facesNeedingMerge[i]; + ref var faceToRemove = ref faces[faceIndex]; + var previousIndex = faceToRemove.VertexIndices[faceToRemove.VertexIndices.Count - 1]; + for (int j = 0; j < faceToRemove.VertexIndices.Count; ++j) { - ++removedEdgesToTestCount; + RemoveFaceFromEdge(previousIndex, faceToRemove.VertexIndices[j], faceIndex, ref facesForEdges); + previousIndex = faceToRemove.VertexIndices[j]; } - else if (removedEdgesToTestCount > 0) + //Remove any references to this face in the edges to test. + int removedEdgesToTestCount = 0; + for (int j = 0; j < edgesToTest.Count; ++j) { - //Scoot edges to test back. - edgesToTest[j - removedEdgesToTestCount] = edgesToTest[j]; + if (edgesToTest[j].FaceIndex == faceIndex) + { + ++removedEdgesToTestCount; + } + else if (removedEdgesToTestCount > 0) + { + //Scoot edges to test back. + edgesToTest[j - removedEdgesToTestCount] = edgesToTest[j]; + } } + edgesToTest.Count -= removedEdgesToTestCount; } - edgesToTest.Count -= removedEdgesToTestCount; + //Re-reduce the merged vertices to get the new face vertices. + facePoints.Count = 0; + targetFace.VertexIndices.Count = 0; + reducedFaceIndices.Count = 0; + ReduceFace(ref mergedIndices, targetFace.Normal, points, planeEpsilonNarrow, ref facePoints, ref allowVertex, ref reducedFaceIndices); + targetFace.VertexIndices.Count = reducedFaceIndices.Count; + reducedFaceIndices.Span.CopyTo(0, targetFace.VertexIndices.Span, 0, reducedFaceIndices.Count); + //Note that we do *not* add the face back to the edges yet; it'll go through another loop iteration to check for any more overlaps. + //Note that we do *not* remove deleted faces from the faces list. That would break all the face indices that we've accumulated. + //Instead, we'll leave the the empty face there and deal with remapping stuff at the end once all faces have been processed. + facesDeletedCount += facesNeedingMerge.Count - 1; + } + else + { + //No more merging required. + //Note that we only add the face to the faces set if we *haven't* merged into an existing face. + if (targetFaceIndex == faceCountPriorToAdd) + AddFace(ref faces, pool, faceNormal, reducedFaceIndices); + //But given that we've removed face references from edges, those need to get added back regardless. + AddFaceToEdgesAndTestList(pool, ref reducedFaceIndices, ref edgesToTest, ref facesForEdges, faceNormal, targetFaceIndex); + steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertex, faceCountPriorToAdd == targetFaceIndex)); + break; } - //Re-reduce the merged vertices to get the new face vertices. - facePoints.Count = 0; - targetFace.VertexIndices.Count = 0; - ReduceFace(ref mergedIndices, targetFace.Normal, points, planeEpsilonNarrow, ref facePoints, ref allowVertex, ref targetFace.VertexIndices); - - AddFaceToEdgesAndTestList(pool, ref targetFace.VertexIndices, ref edgesToTest, ref facesForEdges, targetFace.Normal, mergeTargetIndex); - steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref targetFace.VertexIndices, ref allowVertex, true)); - - //Note that we do *not* remove deleted faces from the faces list. That would break all the face indices that we've accumulated. - //Instead, we'll leave the the empty face there and deal with remapping stuff at the end once all faces have been processed. - facesDeleted += facesNeedingMerge.Count - 1; - } - else - { - //Didn't need to perform a merge; add the face. - AddFace(ref faces, pool, faceNormal, reducedFaceIndices); - AddFaceToEdgesAndTestList(pool, ref reducedFaceIndices, ref edgesToTest, ref facesForEdges, faceNormal, newFaceIndex); - steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertex)); } } @@ -928,7 +944,27 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa pool.Return(ref projectedOnY); pool.Return(ref pointBundles); - Debug.Assert(facesDeleted == 0, "You still need to handle the case where faces were deleted!"); + if (facesDeletedCount > 0) + { + //During execution, some faces were found to be coplanar and got merged. To avoid breaking face index references during execution, we left those triangles in place. + //Now, though, we need to get rid of any faces that are marked deleted! + int shift = 0; + for (int i = 0; i < faces.Count; ++i) + { + if (faces[i].Deleted) + { + //Removing the face from the list, so we gotta dispose it now rather than later. + faces[i].VertexIndices.Dispose(pool); + ++shift; + } + else if (shift > 0) + { + faces[i - shift] = faces[i]; + } + } + Debug.Assert(facesDeletedCount == shift); + faces.Count -= facesDeletedCount; + } //Create a reduced hull point set from the face vertex references. int totalIndexCount = 0; From d4b97d8c6fa4a64b3503053284489ca7e22d464d Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 24 Aug 2022 20:58:42 -0500 Subject: [PATCH 520/947] Fixed a vertex index counting bug, improved debug tracking. --- BepuPhysics/Collidables/ConvexHullHelper.cs | 45 +++++++++++++------ Demos/SpecializedTests/ConvexHullTestDemo.cs | 46 ++++++++++++++++---- 2 files changed, 68 insertions(+), 23 deletions(-) diff --git a/BepuPhysics/Collidables/ConvexHullHelper.cs b/BepuPhysics/Collidables/ConvexHullHelper.cs index bf9f6f76c..a9902e2ff 100644 --- a/BepuPhysics/Collidables/ConvexHullHelper.cs +++ b/BepuPhysics/Collidables/ConvexHullHelper.cs @@ -477,7 +477,7 @@ static void RemoveFaceFromEdge(int a, int b, int faceIndex, ref QuickDictionary< //2. it's an orphaned internal edge that should not be tested. } - struct EarlyFace + internal struct EarlyFace { public QuickList VertexIndices; public bool Deleted; @@ -500,9 +500,12 @@ public struct DebugStep public Vector3 FaceNormal; public Vector3 BasisX; public Vector3 BasisY; - public bool Merge; + public int MergeTarget; + public List FaceStarts; + public List FaceIndices; + public bool[] FaceDeleted; - public DebugStep(EdgeEndpoints sourceEdge, ref QuickList raw, Vector3 faceNormal, Vector3 basisX, Vector3 basisY, ref QuickList reduced, ref Buffer allowVertex, bool merge = false) + internal DebugStep(EdgeEndpoints sourceEdge, ref QuickList raw, Vector3 faceNormal, Vector3 basisX, Vector3 basisY, ref QuickList reduced, ref Buffer allowVertex, ref QuickList faces, int mergeTarget = -1) { SourceEdge = sourceEdge; FaceNormal = faceNormal; @@ -523,7 +526,18 @@ public DebugStep(EdgeEndpoints sourceEdge, ref QuickList raw, Vector3 faceN { AllowVertex[i] = allowVertex[i]; } - Merge = merge; + MergeTarget = mergeTarget; + FaceStarts = new List(faces.Count); + FaceIndices = new List(); + FaceDeleted = new bool[faces.Count]; + for (int i = 0; i < faces.Count; ++i) + { + ref var face = ref faces[i]; + FaceStarts.Add(FaceIndices.Count); + for (int j = 0; j < face.VertexIndices.Count; ++j) + FaceIndices.Add(face.VertexIndices[j]); + FaceDeleted[i] = face.Deleted; + } } } /// @@ -705,12 +719,7 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa ReduceFace(ref rawFaceVertexIndices, initialFaceNormal, points, planeEpsilonNarrow, ref facePoints, ref allowVertex, ref reducedFaceIndices); - Vector3Wide.ReadFirst(initialBasisX, out var debugInitialBasisX); - Vector3Wide.ReadFirst(initialBasisY, out var debugInitialBasisY); - steps.Add(new DebugStep(initialSourceEdge, ref rawFaceVertexIndices, initialFaceNormal, debugInitialBasisX, debugInitialBasisY, ref reducedFaceIndices, ref allowVertex)); - var faces = new QuickList(points.Length, pool); - var edgesToTest = new QuickList(points.Length, pool); var facesForEdges = new QuickDictionary(points.Length, pool); var facesNeedingMerge = new QuickList(32, pool); @@ -747,6 +756,9 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa if (Vector3.Dot(basisX, edgeToAdd.FaceNormal) > 0) Helpers.Swap(ref edgeToAdd.Endpoints.A, ref edgeToAdd.Endpoints.B); } + Vector3Wide.ReadFirst(initialBasisX, out var debugInitialBasisX); + Vector3Wide.ReadFirst(initialBasisY, out var debugInitialBasisY); + steps.Add(new DebugStep(initialSourceEdge, ref rawFaceVertexIndices, initialFaceNormal, debugInitialBasisX, debugInitialBasisY, ref reducedFaceIndices, ref allowVertex, ref faces)); int facesDeletedCount = 0; @@ -778,7 +790,7 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa ReduceFace(ref rawFaceVertexIndices, faceNormal, points, planeEpsilonNarrow, ref facePoints, ref allowVertex, ref reducedFaceIndices); if (reducedFaceIndices.Count < 3) { - steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertex)); + steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertex, ref faces)); //Degenerate face found; don't bother creating work for it. continue; } @@ -910,11 +922,8 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa } //Re-reduce the merged vertices to get the new face vertices. facePoints.Count = 0; - targetFace.VertexIndices.Count = 0; reducedFaceIndices.Count = 0; ReduceFace(ref mergedIndices, targetFace.Normal, points, planeEpsilonNarrow, ref facePoints, ref allowVertex, ref reducedFaceIndices); - targetFace.VertexIndices.Count = reducedFaceIndices.Count; - reducedFaceIndices.Span.CopyTo(0, targetFace.VertexIndices.Span, 0, reducedFaceIndices.Count); //Note that we do *not* add the face back to the edges yet; it'll go through another loop iteration to check for any more overlaps. //Note that we do *not* remove deleted faces from the faces list. That would break all the face indices that we've accumulated. //Instead, we'll leave the the empty face there and deal with remapping stuff at the end once all faces have been processed. @@ -925,10 +934,18 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa //No more merging required. //Note that we only add the face to the faces set if we *haven't* merged into an existing face. if (targetFaceIndex == faceCountPriorToAdd) + { AddFace(ref faces, pool, faceNormal, reducedFaceIndices); + } + else + { + //Update the merge targeted face's vertices with the final result. + faces[targetFaceIndex].VertexIndices.Count = reducedFaceIndices.Count; + reducedFaceIndices.Span.CopyTo(0, faces[targetFaceIndex].VertexIndices.Span, 0, reducedFaceIndices.Count); + } //But given that we've removed face references from edges, those need to get added back regardless. AddFaceToEdgesAndTestList(pool, ref reducedFaceIndices, ref edgesToTest, ref facesForEdges, faceNormal, targetFaceIndex); - steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertex, faceCountPriorToAdd == targetFaceIndex)); + steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertex, ref faces, faceCountPriorToAdd != targetFaceIndex ? targetFaceIndex : -1)); break; } } diff --git a/Demos/SpecializedTests/ConvexHullTestDemo.cs b/Demos/SpecializedTests/ConvexHullTestDemo.cs index 413d7111f..3e7ec95d7 100644 --- a/Demos/SpecializedTests/ConvexHullTestDemo.cs +++ b/Demos/SpecializedTests/ConvexHullTestDemo.cs @@ -397,9 +397,14 @@ public override void Update(Window window, Camera camera, Input input, float dt) { stepIndex = Math.Min(stepIndex + 1, debugSteps.Count - 1); } + if (input.WasPushed(OpenTK.Input.Key.P)) + { + showWireframe = !showWireframe; + } base.Update(window, camera, input, dt); } + bool showWireframe; public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) { var step = debugSteps[stepIndex]; @@ -422,19 +427,42 @@ public override void Render(Renderer renderer, Camera camera, Input input, TextB var pose = new RigidPose(renderOffset + points[step.Reduced[i]] * scale); renderer.Shapes.AddShape(new Box(0.25f, 0.25f, 0.6f), Simulation.Shapes, pose, new Vector3(0, 1, 0)); } - for (int i = 0; i <= stepIndex; ++i) + { var pose = new RigidPose(renderOffset); - var oldStep = debugSteps[i]; - var color = oldStep.Merge ? new Vector3(0.5f, 0f, 1f) : new Vector3(1, 0, 1); - for (int j = 2; j < oldStep.Reduced.Count; ++j) + for (int i = 0; i < step.FaceStarts.Count; ++i) { - renderer.Shapes.AddShape(new Triangle + if (!step.FaceDeleted[i]) { - A = points[oldStep.Reduced[0]] * scale, - B = points[oldStep.Reduced[j]] * scale, - C = points[oldStep.Reduced[j - 1]] * scale - }, Simulation.Shapes, pose, color); + var faceStart = step.FaceStarts[i]; + var faceEnd = i + 1 < step.FaceStarts.Count ? step.FaceStarts[i + 1] : step.FaceIndices.Count; + var count = faceEnd - faceStart; + var color = step.MergeTarget == i ? new Vector3(0.25f, 0.25f, 1f) : new Vector3(1, 0, 1); + if (showWireframe) + { + var previousIndex = faceEnd - 1; + for (int q = faceStart; q < faceEnd; ++q) + { + var a = points[step.FaceIndices[q]] * scale + pose.Position; + var b = points[step.FaceIndices[previousIndex]] * scale + pose.Position; + previousIndex = q; + renderer.Lines.Allocate() = new LineInstance(a, b, color, Vector3.Zero); + } + } + else + { + var ugh = step.MergeTarget == i ? new Vector3(-1, 0, 0) : new Vector3(); + for (int k = faceStart + 2; k < faceEnd; ++k) + { + renderer.Shapes.AddShape(new Triangle + { + A = points[step.FaceIndices[faceStart]] * scale + ugh, + B = points[step.FaceIndices[k]] * scale + ugh, + C = points[step.FaceIndices[k - 1]] * scale + ugh + }, Simulation.Shapes, pose, color); + } + } + } } } var edgeMidpoint = renderOffset + (points[step.SourceEdge.A] + points[step.SourceEdge.B]) * scale * 0.5f; From 2a88ac3eed576830bed4df3153f6ae6c2bbe74c5 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 25 Aug 2022 16:42:31 -0500 Subject: [PATCH 521/947] Improved visualization a little bit. --- BepuPhysics/Collidables/ConvexHullHelper.cs | 23 ++++++++++++++++---- Demos/SpecializedTests/ConvexHullTestDemo.cs | 4 ++-- 2 files changed, 21 insertions(+), 6 deletions(-) diff --git a/BepuPhysics/Collidables/ConvexHullHelper.cs b/BepuPhysics/Collidables/ConvexHullHelper.cs index a9902e2ff..7084c05b0 100644 --- a/BepuPhysics/Collidables/ConvexHullHelper.cs +++ b/BepuPhysics/Collidables/ConvexHullHelper.cs @@ -501,11 +501,12 @@ public struct DebugStep public Vector3 BasisX; public Vector3 BasisY; public int MergeTarget; + public int ModifiedFaceIndex; public List FaceStarts; public List FaceIndices; public bool[] FaceDeleted; - internal DebugStep(EdgeEndpoints sourceEdge, ref QuickList raw, Vector3 faceNormal, Vector3 basisX, Vector3 basisY, ref QuickList reduced, ref Buffer allowVertex, ref QuickList faces, int mergeTarget = -1) + internal DebugStep(EdgeEndpoints sourceEdge, ref QuickList raw, Vector3 faceNormal, Vector3 basisX, Vector3 basisY, ref QuickList reduced, ref Buffer allowVertex, ref QuickList faces, int modifiedFaceIndex, int mergeTarget = -1) { SourceEdge = sourceEdge; FaceNormal = faceNormal; @@ -527,6 +528,7 @@ internal DebugStep(EdgeEndpoints sourceEdge, ref QuickList raw, Vector3 fac AllowVertex[i] = allowVertex[i]; } MergeTarget = mergeTarget; + ModifiedFaceIndex = modifiedFaceIndex; FaceStarts = new List(faces.Count); FaceIndices = new List(); FaceDeleted = new bool[faces.Count]; @@ -758,7 +760,7 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa } Vector3Wide.ReadFirst(initialBasisX, out var debugInitialBasisX); Vector3Wide.ReadFirst(initialBasisY, out var debugInitialBasisY); - steps.Add(new DebugStep(initialSourceEdge, ref rawFaceVertexIndices, initialFaceNormal, debugInitialBasisX, debugInitialBasisY, ref reducedFaceIndices, ref allowVertex, ref faces)); + steps.Add(new DebugStep(initialSourceEdge, ref rawFaceVertexIndices, initialFaceNormal, debugInitialBasisX, debugInitialBasisY, ref reducedFaceIndices, ref allowVertex, ref faces, reducedFaceIndices.Count >= 3 ? 0 : -1)); int facesDeletedCount = 0; @@ -790,7 +792,7 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa ReduceFace(ref rawFaceVertexIndices, faceNormal, points, planeEpsilonNarrow, ref facePoints, ref allowVertex, ref reducedFaceIndices); if (reducedFaceIndices.Count < 3) { - steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertex, ref faces)); + steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertex, ref faces, -1)); //Degenerate face found; don't bother creating work for it. continue; } @@ -945,7 +947,7 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa } //But given that we've removed face references from edges, those need to get added back regardless. AddFaceToEdgesAndTestList(pool, ref reducedFaceIndices, ref edgesToTest, ref facesForEdges, faceNormal, targetFaceIndex); - steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertex, ref faces, faceCountPriorToAdd != targetFaceIndex ? targetFaceIndex : -1)); + steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertex, ref faces, targetFaceIndex, faceCountPriorToAdd != targetFaceIndex ? targetFaceIndex : -1)); break; } } @@ -961,6 +963,19 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa pool.Return(ref projectedOnY); pool.Return(ref pointBundles); + for (int i = 0; i < faces.Count; ++i) + { + for (int j = i + 1; j < faces.Count; ++j) + { + var dot = Vector3.Dot(faces[i].Normal, faces[j].Normal); + var bothFacesExist = !faces[i].Deleted && !faces[j].Deleted; + if (dot >= normalCoplanarityEpsilon && bothFacesExist) + { + Console.WriteLine($"Dot {dot} on faces {i} and {j}"); + } + } + } + if (facesDeletedCount > 0) { //During execution, some faces were found to be coplanar and got merged. To avoid breaking face index references during execution, we left those triangles in place. diff --git a/Demos/SpecializedTests/ConvexHullTestDemo.cs b/Demos/SpecializedTests/ConvexHullTestDemo.cs index 3e7ec95d7..fdf03f495 100644 --- a/Demos/SpecializedTests/ConvexHullTestDemo.cs +++ b/Demos/SpecializedTests/ConvexHullTestDemo.cs @@ -437,7 +437,7 @@ public override void Render(Renderer renderer, Camera camera, Input input, TextB var faceStart = step.FaceStarts[i]; var faceEnd = i + 1 < step.FaceStarts.Count ? step.FaceStarts[i + 1] : step.FaceIndices.Count; var count = faceEnd - faceStart; - var color = step.MergeTarget == i ? new Vector3(0.25f, 0.25f, 1f) : new Vector3(1, 0, 1); + var color = step.MergeTarget == i ? new Vector3(0.25f, 0.25f, 1f) : step.ModifiedFaceIndex == i ? new Vector3(1, 0, 0.5f) : new Vector3(1, 0, 1); if (showWireframe) { var previousIndex = faceEnd - 1; @@ -469,7 +469,7 @@ public override void Render(Renderer renderer, Camera camera, Input input, TextB renderer.Lines.Allocate() = new LineInstance(edgeMidpoint, edgeMidpoint + step.BasisX * scale * 0.5f, new Vector3(1, 1, 0), new Vector3()); renderer.Lines.Allocate() = new LineInstance(edgeMidpoint, edgeMidpoint + step.BasisY * scale * 0.5f, new Vector3(0, 1, 0), new Vector3()); renderer.TextBatcher.Write( - text.Clear().Append($"Enumerate step with X and C. Current step: ").Append(stepIndex + 1).Append(" out of ").Append(debugSteps.Count), + text.Clear().Append($"Enumerate step with X and C. Current step: ").Append(stepIndex + 1).Append(" out of ").Append(debugSteps.Count).Append(", modified face index: ").Append(step.ModifiedFaceIndex), new Vector2(32, renderer.Surface.Resolution.Y - 140), 20, new Vector3(1), font); base.Render(renderer, camera, input, text, font); } From a5cfd679b03761ea59d5241c04fbd3d6810f9664 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 26 Aug 2022 17:07:58 -0500 Subject: [PATCH 522/947] Fixed doofery in merging loop. --- BepuPhysics/Collidables/ConvexHullHelper.cs | 122 ++++++++----------- Demos/SpecializedTests/ConvexHullTestDemo.cs | 27 ++-- 2 files changed, 67 insertions(+), 82 deletions(-) diff --git a/BepuPhysics/Collidables/ConvexHullHelper.cs b/BepuPhysics/Collidables/ConvexHullHelper.cs index 7084c05b0..1b3ccaf39 100644 --- a/BepuPhysics/Collidables/ConvexHullHelper.cs +++ b/BepuPhysics/Collidables/ConvexHullHelper.cs @@ -500,13 +500,14 @@ public struct DebugStep public Vector3 FaceNormal; public Vector3 BasisX; public Vector3 BasisY; - public int MergeTarget; - public int ModifiedFaceIndex; public List FaceStarts; public List FaceIndices; public bool[] FaceDeleted; + public int[] MergedFaceIndices; + public int FaceIndex; + public Vector3[] FaceNormals; - internal DebugStep(EdgeEndpoints sourceEdge, ref QuickList raw, Vector3 faceNormal, Vector3 basisX, Vector3 basisY, ref QuickList reduced, ref Buffer allowVertex, ref QuickList faces, int modifiedFaceIndex, int mergeTarget = -1) + internal DebugStep(EdgeEndpoints sourceEdge, ref QuickList raw, Vector3 faceNormal, Vector3 basisX, Vector3 basisY, ref QuickList reduced, ref Buffer allowVertex, ref QuickList faces, Span mergedFaceIndices, int faceIndex) { SourceEdge = sourceEdge; FaceNormal = faceNormal; @@ -527,11 +528,10 @@ internal DebugStep(EdgeEndpoints sourceEdge, ref QuickList raw, Vector3 fac { AllowVertex[i] = allowVertex[i]; } - MergeTarget = mergeTarget; - ModifiedFaceIndex = modifiedFaceIndex; FaceStarts = new List(faces.Count); FaceIndices = new List(); FaceDeleted = new bool[faces.Count]; + FaceNormals = new Vector3[faces.Count]; for (int i = 0; i < faces.Count; ++i) { ref var face = ref faces[i]; @@ -539,7 +539,10 @@ internal DebugStep(EdgeEndpoints sourceEdge, ref QuickList raw, Vector3 fac for (int j = 0; j < face.VertexIndices.Count; ++j) FaceIndices.Add(face.VertexIndices[j]); FaceDeleted[i] = face.Deleted; + FaceNormals[i] = face.Normal; } + MergedFaceIndices = mergedFaceIndices.ToArray(); + FaceIndex = faceIndex; } } /// @@ -598,6 +601,12 @@ static void AddFaceToEdgesAndTestList(BufferPool pool, } } + static void AddIfNotPresent(ref QuickList list, int value, BufferPool pool) + { + if (!list.Contains(value)) + list.Allocate(pool) = value; + } + /// /// Computes the convex hull of a set of points. /// @@ -725,7 +734,6 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa var edgesToTest = new QuickList(points.Length, pool); var facesForEdges = new QuickDictionary(points.Length, pool); var facesNeedingMerge = new QuickList(32, pool); - var mergedIndices = new QuickList(32, pool); if (reducedFaceIndices.Count >= 3) { //The initial face search found an actual face! That's a bit surprising since we didn't start from an edge offset, but rather an arbitrary direction. @@ -760,7 +768,7 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa } Vector3Wide.ReadFirst(initialBasisX, out var debugInitialBasisX); Vector3Wide.ReadFirst(initialBasisY, out var debugInitialBasisY); - steps.Add(new DebugStep(initialSourceEdge, ref rawFaceVertexIndices, initialFaceNormal, debugInitialBasisX, debugInitialBasisY, ref reducedFaceIndices, ref allowVertex, ref faces, reducedFaceIndices.Count >= 3 ? 0 : -1)); + steps.Add(new DebugStep(initialSourceEdge, ref rawFaceVertexIndices, initialFaceNormal, debugInitialBasisX, debugInitialBasisY, ref reducedFaceIndices, ref allowVertex, ref faces, default, reducedFaceIndices.Count >= 3 ? 0 : -1)); int facesDeletedCount = 0; @@ -792,13 +800,13 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa ReduceFace(ref rawFaceVertexIndices, faceNormal, points, planeEpsilonNarrow, ref facePoints, ref allowVertex, ref reducedFaceIndices); if (reducedFaceIndices.Count < 3) { - steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertex, ref faces, -1)); + steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertex, ref faces, default, -1)); //Degenerate face found; don't bother creating work for it. continue; } var faceCountPriorToAdd = faces.Count; - var targetFaceIndex = faceCountPriorToAdd; + facesNeedingMerge.Count = 0; while (true) { //This implementation bites the bullet pretty hard on numerical problems. They most frequently arise from near coplanar vertices. @@ -814,8 +822,8 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa //So, we just stick everything into a while loop and let it iterate until it's done. //The good news is that the merging process won't loop forever- every merge results in a face that is at least as large as any contributor, and any internal points //that get reduced out of consideration are marked with allowVertex[whateverInteriorPointIndex] = false. + int previousFaceMergeCount = facesNeedingMerge.Count; var previousEndpointIndex = reducedFaceIndices.Count - 1; - facesNeedingMerge.Count = 0; for (int i = 0; i < reducedFaceIndices.Count; ++i) { EdgeEndpoints edgeEndpoints; @@ -837,69 +845,57 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa var bDot = Vector3.Dot(faces[edgeFaces.FaceB].Normal, faceNormal); if (aDot >= normalCoplanarityEpsilon && bDot >= normalCoplanarityEpsilon) { - facesNeedingMerge.Allocate(pool) = edgeFaces.FaceA; - facesNeedingMerge.Allocate(pool) = edgeFaces.FaceB; + AddIfNotPresent(ref facesNeedingMerge, edgeFaces.FaceA, pool); + AddIfNotPresent(ref facesNeedingMerge, edgeFaces.FaceB, pool); } else { //If both aren't merge candidates, then just pick the one that's closer. - facesNeedingMerge.Allocate(pool) = aDot > bDot ? edgeFaces.FaceA : edgeFaces.FaceB; + AddIfNotPresent(ref facesNeedingMerge, aDot > bDot ? edgeFaces.FaceA : edgeFaces.FaceB, pool); } } else { //Only one face already present. Check if it's coplanar. if (aDot >= normalCoplanarityEpsilon) - facesNeedingMerge.Allocate(pool) = edgeFaces.FaceA; + AddIfNotPresent(ref facesNeedingMerge, edgeFaces.FaceA, pool); } } } - if (facesNeedingMerge.Count > 0) + if (facesNeedingMerge.Count > previousFaceMergeCount) { - //The new face should not be added! Instead, we should merge its vertices into an existing face. - //Remove any duplicates. If we had perfect numerical precision, duplicates would be impossible, but... this whole thing is because we don't have perfect numerical precision. - for (int i = facesNeedingMerge.Count - 1; i >= 0; --i) - { - for (int j = i - 1; j >= 0; --j) - { - if (facesNeedingMerge[j] == facesNeedingMerge[i]) - { - facesNeedingMerge.FastRemoveAt(i); - } - } - } - targetFaceIndex = facesNeedingMerge[0]; - - ref var targetFace = ref faces[targetFaceIndex]; - //Accumulate all the vertices present in the candidate face and any other faces being merged together. - mergedIndices.Count = 0; - mergedIndices.AddRange(targetFace.VertexIndices, pool); - for (int j = 0; j < reducedFaceIndices.Count; ++j) - { - if (!mergedIndices.Contains(reducedFaceIndices[j])) - { - mergedIndices.Allocate(pool) = reducedFaceIndices[j]; - } - } - for (int i = 1; i < facesNeedingMerge.Count; ++i) + //This iteration has found more faces which should be merged together. + //Accumulate the vertices. + for (int i = previousFaceMergeCount; i < facesNeedingMerge.Count; ++i) { ref var sourceFace = ref faces[facesNeedingMerge[i]]; for (int j = 0; j < sourceFace.VertexIndices.Count; ++j) { - if (!mergedIndices.Contains(sourceFace.VertexIndices[j])) + if (!rawFaceVertexIndices.Contains(sourceFace.VertexIndices[j])) { - mergedIndices.Allocate(pool) = sourceFace.VertexIndices[j]; + rawFaceVertexIndices.Allocate(pool) = sourceFace.VertexIndices[j]; } } - //Mark the face as deleted, but defer the vertex disposal for simplicity. - sourceFace.Deleted = true; } - //For every edge of any face we're merging, remove the face from the edge lists. + + //Re-reduce. + reducedFaceIndices.Count = 0; + facePoints.Count = 0; + ReduceFace(ref rawFaceVertexIndices, faceNormal, points, planeEpsilonNarrow, ref facePoints, ref allowVertex, ref reducedFaceIndices); + } + else + { + //No more faces need to be merged! Go ahead and apply the changes. for (int i = 0; i < facesNeedingMerge.Count; ++i) { + //Note that we do not merge into an existing face; for simplicity, we just merge into the face we're going to append and mark the old faces as being deleted. + //Note that we do *not* remove deleted faces from the faces list. That would break all the face indices that we've accumulated. var faceIndex = facesNeedingMerge[i]; ref var faceToRemove = ref faces[faceIndex]; + faceToRemove.Deleted = true; + + //For every edge of any face we're merging, remove the face from the edge lists. var previousIndex = faceToRemove.VertexIndices[faceToRemove.VertexIndices.Count - 1]; for (int j = 0; j < faceToRemove.VertexIndices.Count; ++j) { @@ -922,32 +918,13 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa } edgesToTest.Count -= removedEdgesToTestCount; } - //Re-reduce the merged vertices to get the new face vertices. - facePoints.Count = 0; - reducedFaceIndices.Count = 0; - ReduceFace(ref mergedIndices, targetFace.Normal, points, planeEpsilonNarrow, ref facePoints, ref allowVertex, ref reducedFaceIndices); - //Note that we do *not* add the face back to the edges yet; it'll go through another loop iteration to check for any more overlaps. - //Note that we do *not* remove deleted faces from the faces list. That would break all the face indices that we've accumulated. - //Instead, we'll leave the the empty face there and deal with remapping stuff at the end once all faces have been processed. - facesDeletedCount += facesNeedingMerge.Count - 1; - } - else - { - //No more merging required. - //Note that we only add the face to the faces set if we *haven't* merged into an existing face. - if (targetFaceIndex == faceCountPriorToAdd) - { - AddFace(ref faces, pool, faceNormal, reducedFaceIndices); - } - else - { - //Update the merge targeted face's vertices with the final result. - faces[targetFaceIndex].VertexIndices.Count = reducedFaceIndices.Count; - reducedFaceIndices.Span.CopyTo(0, faces[targetFaceIndex].VertexIndices.Span, 0, reducedFaceIndices.Count); - } - //But given that we've removed face references from edges, those need to get added back regardless. - AddFaceToEdgesAndTestList(pool, ref reducedFaceIndices, ref edgesToTest, ref facesForEdges, faceNormal, targetFaceIndex); - steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertex, ref faces, targetFaceIndex, faceCountPriorToAdd != targetFaceIndex ? targetFaceIndex : -1)); + + //Retain a count of the deleted faces so the post process can handle them more easily. + facesDeletedCount += facesNeedingMerge.Count; + + AddFace(ref faces, pool, faceNormal, reducedFaceIndices); + AddFaceToEdgesAndTestList(pool, ref reducedFaceIndices, ref edgesToTest, ref facesForEdges, faceNormal, faceCountPriorToAdd); + steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertex, ref faces, facesNeedingMerge, faceCountPriorToAdd)); break; } } @@ -1044,7 +1021,6 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa } faces.Dispose(pool); facesNeedingMerge.Dispose(pool); - mergedIndices.Dispose(pool); } diff --git a/Demos/SpecializedTests/ConvexHullTestDemo.cs b/Demos/SpecializedTests/ConvexHullTestDemo.cs index fdf03f495..92c4636ff 100644 --- a/Demos/SpecializedTests/ConvexHullTestDemo.cs +++ b/Demos/SpecializedTests/ConvexHullTestDemo.cs @@ -401,10 +401,15 @@ public override void Update(Window window, Camera camera, Input input, float dt) { showWireframe = !showWireframe; } + if (input.WasPushed(OpenTK.Input.Key.U)) + { + showDeleted = !showDeleted; + } base.Update(window, camera, input, dt); } bool showWireframe; + bool showDeleted; public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) { var step = debugSteps[stepIndex]; @@ -432,33 +437,35 @@ public override void Render(Renderer renderer, Camera camera, Input input, TextB var pose = new RigidPose(renderOffset); for (int i = 0; i < step.FaceStarts.Count; ++i) { - if (!step.FaceDeleted[i]) + if (showDeleted || !step.FaceDeleted[i]) { var faceStart = step.FaceStarts[i]; var faceEnd = i + 1 < step.FaceStarts.Count ? step.FaceStarts[i + 1] : step.FaceIndices.Count; var count = faceEnd - faceStart; - var color = step.MergeTarget == i ? new Vector3(0.25f, 0.25f, 1f) : step.ModifiedFaceIndex == i ? new Vector3(1, 0, 0.5f) : new Vector3(1, 0, 1); + var color = step.FaceDeleted[i] ? new Vector3(0.25f, 0.25f, 0.25f) : step.FaceIndex == i ? new Vector3(1, 0, 0.5f) : new Vector3(1, 0, 1); + var deletionInducedScale = step.FaceDeleted[i] ? new Vector3(1.1f) : new Vector3(1f); + + var offset = step.FaceDeleted[i] ? step.FaceNormals[i] * 0.25f : new Vector3(); if (showWireframe) { var previousIndex = faceEnd - 1; for (int q = faceStart; q < faceEnd; ++q) { - var a = points[step.FaceIndices[q]] * scale + pose.Position; - var b = points[step.FaceIndices[previousIndex]] * scale + pose.Position; + var a = points[step.FaceIndices[q]] * scale + pose.Position + offset; + var b = points[step.FaceIndices[previousIndex]] * scale + pose.Position + offset; previousIndex = q; renderer.Lines.Allocate() = new LineInstance(a, b, color, Vector3.Zero); } } else { - var ugh = step.MergeTarget == i ? new Vector3(-1, 0, 0) : new Vector3(); for (int k = faceStart + 2; k < faceEnd; ++k) { renderer.Shapes.AddShape(new Triangle { - A = points[step.FaceIndices[faceStart]] * scale + ugh, - B = points[step.FaceIndices[k]] * scale + ugh, - C = points[step.FaceIndices[k - 1]] * scale + ugh + A = points[step.FaceIndices[faceStart]] * scale + offset, + B = points[step.FaceIndices[k]] * scale + offset, + C = points[step.FaceIndices[k - 1]] * scale + offset }, Simulation.Shapes, pose, color); } } @@ -469,8 +476,10 @@ public override void Render(Renderer renderer, Camera camera, Input input, TextB renderer.Lines.Allocate() = new LineInstance(edgeMidpoint, edgeMidpoint + step.BasisX * scale * 0.5f, new Vector3(1, 1, 0), new Vector3()); renderer.Lines.Allocate() = new LineInstance(edgeMidpoint, edgeMidpoint + step.BasisY * scale * 0.5f, new Vector3(0, 1, 0), new Vector3()); renderer.TextBatcher.Write( - text.Clear().Append($"Enumerate step with X and C. Current step: ").Append(stepIndex + 1).Append(" out of ").Append(debugSteps.Count).Append(", modified face index: ").Append(step.ModifiedFaceIndex), + text.Clear().Append($"Enumerate step with X and C. Current step: ").Append(stepIndex + 1).Append(" out of ").Append(debugSteps.Count), new Vector2(32, renderer.Surface.Resolution.Y - 140), 20, new Vector3(1), font); + renderer.TextBatcher.Write(text.Clear().Append($"Show wireframe: P ").Append(showWireframe ? "(on)" : "(off)"), new Vector2(32, renderer.Surface.Resolution.Y - 120), 20, new Vector3(1), font); + renderer.TextBatcher.Write(text.Clear().Append($"Show deleted: U ").Append(showDeleted ? "(on)" : "(off)"), new Vector2(32, renderer.Surface.Resolution.Y - 100), 20, new Vector3(1), font); base.Render(renderer, camera, input, text, font); } } From 3dd0e6759e83c83ad11e7c090525fb56ae1cb1dd Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 26 Aug 2022 18:14:06 -0500 Subject: [PATCH 523/947] Int2-vector2 helpers. --- BepuUtilities/Int2.cs | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/BepuUtilities/Int2.cs b/BepuUtilities/Int2.cs index 688a3ad3a..3ac6063c6 100644 --- a/BepuUtilities/Int2.cs +++ b/BepuUtilities/Int2.cs @@ -1,5 +1,6 @@ using BepuUtilities.Collections; using System; +using System.Numerics; using System.Runtime.CompilerServices; namespace BepuUtilities @@ -67,5 +68,14 @@ public bool Equals(ref Int2 a, ref Int2 b) { return a.X == b.X && a.Y == b.Y; } + + public static implicit operator Vector2(Int2 value) + { + return new Vector2(value.X, value.Y); + } + public static explicit operator Int2(Vector2 value) + { + return new Int2((int)value.X, (int)value.Y); + } } } From d821774c2953cd71859de2e193ee653dc4ee11f5 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 26 Aug 2022 18:14:22 -0500 Subject: [PATCH 524/947] Some more helper visualization. --- BepuPhysics/Collidables/ConvexHullHelper.cs | 2 +- Demos/SpecializedTests/ConvexHullTestDemo.cs | 27 +++++++++++++++++--- 2 files changed, 25 insertions(+), 4 deletions(-) diff --git a/BepuPhysics/Collidables/ConvexHullHelper.cs b/BepuPhysics/Collidables/ConvexHullHelper.cs index 1b3ccaf39..ab03b1465 100644 --- a/BepuPhysics/Collidables/ConvexHullHelper.cs +++ b/BepuPhysics/Collidables/ConvexHullHelper.cs @@ -713,7 +713,7 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa Vector3Wide.Broadcast(initialVertex, out var initialVertexBundle); pool.Take>(pointBundles.Length, out var projectedOnX); pool.Take>(pointBundles.Length, out var projectedOnY); - var planeEpsilonNarrow = MathF.Sqrt(bestDistanceSquared) * 1e-6f; + var planeEpsilonNarrow = MathF.Sqrt(bestDistanceSquared) * 1e-5f; var normalCoplanarityEpsilon = 1f - 1e-5f; var planeEpsilon = new Vector(planeEpsilonNarrow); var rawFaceVertexIndices = new QuickList(pointBundles.Length * Vector.Count, pool); diff --git a/Demos/SpecializedTests/ConvexHullTestDemo.cs b/Demos/SpecializedTests/ConvexHullTestDemo.cs index 92c4636ff..8fcb21c5e 100644 --- a/Demos/SpecializedTests/ConvexHullTestDemo.cs +++ b/Demos/SpecializedTests/ConvexHullTestDemo.cs @@ -263,8 +263,9 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); - var hullPoints = CreateRandomConvexHullPoints(); + //var hullPoints = CreateRandomConvexHullPoints(); //var hullPoints = CreateMeshConvexHull(content.Load(@"Content\newt.obj"), new Vector3(1, 1.5f, 1f)); + var hullPoints = CreateTestConvexHull(); var hullShape = new ConvexHull(hullPoints, BufferPool, out _); float largestError = 0; for (int i = 0; i < hullShape.FaceToVertexIndicesStart.Length; ++i) @@ -405,11 +406,16 @@ public override void Update(Window window, Camera camera, Input input, float dt) { showDeleted = !showDeleted; } + if (input.WasPushed(OpenTK.Input.Key.Y)) + { + showVertexIndices = !showVertexIndices; + } base.Update(window, camera, input, dt); } bool showWireframe; bool showDeleted; + bool showVertexIndices; public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) { var step = debugSteps[stepIndex]; @@ -472,14 +478,29 @@ public override void Render(Renderer renderer, Camera camera, Input input, TextB } } } + + if (showVertexIndices) + { + for (int i = 0; i < points.Length; ++i) + { + if (DemoRenderer.Helpers.GetScreenLocation(points[i] * scale + renderOffset, camera.ViewProjection, renderer.Surface.Resolution, out var location)) + { + renderer.TextBatcher.Write(text.Clear().Append(i), location, 10, new Vector3(1), font); + } + } + } + var edgeMidpoint = renderOffset + (points[step.SourceEdge.A] + points[step.SourceEdge.B]) * scale * 0.5f; renderer.Lines.Allocate() = new LineInstance(edgeMidpoint, edgeMidpoint + step.BasisX * scale * 0.5f, new Vector3(1, 1, 0), new Vector3()); renderer.Lines.Allocate() = new LineInstance(edgeMidpoint, edgeMidpoint + step.BasisY * scale * 0.5f, new Vector3(0, 1, 0), new Vector3()); renderer.TextBatcher.Write( text.Clear().Append($"Enumerate step with X and C. Current step: ").Append(stepIndex + 1).Append(" out of ").Append(debugSteps.Count), new Vector2(32, renderer.Surface.Resolution.Y - 140), 20, new Vector3(1), font); - renderer.TextBatcher.Write(text.Clear().Append($"Show wireframe: P ").Append(showWireframe ? "(on)" : "(off)"), new Vector2(32, renderer.Surface.Resolution.Y - 120), 20, new Vector3(1), font); - renderer.TextBatcher.Write(text.Clear().Append($"Show deleted: U ").Append(showDeleted ? "(on)" : "(off)"), new Vector2(32, renderer.Surface.Resolution.Y - 100), 20, new Vector3(1), font); + renderer.TextBatcher.Write(text.Clear().Append("Show wireframe: P ").Append(showWireframe ? "(on)" : "(off)"), new Vector2(32, renderer.Surface.Resolution.Y - 120), 20, new Vector3(1), font); + renderer.TextBatcher.Write(text.Clear().Append("Show deleted: U ").Append(showDeleted ? "(on)" : "(off)"), new Vector2(32, renderer.Surface.Resolution.Y - 100), 20, new Vector3(1), font); + renderer.TextBatcher.Write(text.Clear().Append("Show vertex indices: Y ").Append(showVertexIndices ? "(on)" : "(off)"), new Vector2(32, renderer.Surface.Resolution.Y - 80), 20, new Vector3(1), font); + + base.Render(renderer, camera, input, text, font); } } From eb58783aac384a17f429d8696d5c14bed8ae1114 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 27 Aug 2022 12:50:21 -0500 Subject: [PATCH 525/947] Two doofbugs fixed. --- BepuPhysics/Collidables/ConvexHullHelper.cs | 15 +++++++++------ Demos/SpecializedTests/ConvexHullTestDemo.cs | 6 +++--- 2 files changed, 12 insertions(+), 9 deletions(-) diff --git a/BepuPhysics/Collidables/ConvexHullHelper.cs b/BepuPhysics/Collidables/ConvexHullHelper.cs index ab03b1465..a637bf666 100644 --- a/BepuPhysics/Collidables/ConvexHullHelper.cs +++ b/BepuPhysics/Collidables/ConvexHullHelper.cs @@ -460,16 +460,18 @@ static void RemoveFaceFromEdge(int a, int b, int faceIndex, ref QuickDictionary< Debug.Assert(faceIndex == facesForEdge.FaceA || faceIndex == facesForEdge.FaceB, "If you're trying to remove a face index from the edge, it better be in the edge!"); if (facesForEdge.FaceA == faceIndex) { - if (facesForEdge.FaceB == -1) + facesForEdge.FaceA = facesForEdge.FaceB; + facesForEdge.FaceB = -1; + if (facesForEdge.FaceA == -1) { - //This edge is no longer going to have any faces associated with it. + //This edge no longer has any faces associated with it. facesForEdges.FastRemove(ref edge); } - facesForEdge.FaceA = facesForEdge.FaceB; + } + else + { facesForEdge.FaceB = -1; } - facesForEdge.FaceB = -1; - //Note that we do *not* add the edge back into the 'edges to test' list when transitioning from 2 faces to 1 face for the edge. //This function is invoked when an edge is being deleted because it's related to a deleted face. //There are two possible outcomes: @@ -823,13 +825,14 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa //The good news is that the merging process won't loop forever- every merge results in a face that is at least as large as any contributor, and any internal points //that get reduced out of consideration are marked with allowVertex[whateverInteriorPointIndex] = false. int previousFaceMergeCount = facesNeedingMerge.Count; - var previousEndpointIndex = reducedFaceIndices.Count - 1; + var previousEndpointIndex = reducedFaceIndices[reducedFaceIndices.Count - 1]; for (int i = 0; i < reducedFaceIndices.Count; ++i) { EdgeEndpoints edgeEndpoints; edgeEndpoints.A = previousEndpointIndex; edgeEndpoints.B = reducedFaceIndices[i]; previousEndpointIndex = edgeEndpoints.B; + if (facesForEdges.GetTableIndices(ref edgeEndpoints, out var tableIndex, out var elementIndex)) { //There is already at least one face associated with this edge. Do we need to merge? diff --git a/Demos/SpecializedTests/ConvexHullTestDemo.cs b/Demos/SpecializedTests/ConvexHullTestDemo.cs index 8fcb21c5e..a9a296e8b 100644 --- a/Demos/SpecializedTests/ConvexHullTestDemo.cs +++ b/Demos/SpecializedTests/ConvexHullTestDemo.cs @@ -71,7 +71,7 @@ unsafe Buffer CreateBoxConvexHull(float boxScale) return points; } - //A couple of point sets from PEEL. + //A couple of test point sets from PEEL: https://github.com/Pierre-Terdiman/PEEL_PhysX_Edition unsafe Buffer CreateTestConvexHull() { BufferPool.Take(50, out var vertices); @@ -264,8 +264,8 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //var hullPoints = CreateRandomConvexHullPoints(); - //var hullPoints = CreateMeshConvexHull(content.Load(@"Content\newt.obj"), new Vector3(1, 1.5f, 1f)); - var hullPoints = CreateTestConvexHull(); + var hullPoints = CreateMeshConvexHull(content.Load(@"Content\newt.obj"), new Vector3(1, 1.5f, 1f)); + //var hullPoints = CreateTestConvexHull2(); var hullShape = new ConvexHull(hullPoints, BufferPool, out _); float largestError = 0; for (int i = 0; i < hullShape.FaceToVertexIndicesStart.Length; ++i) From 41759e3b8492de62b6af1d87b6b7f236a818f17d Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 27 Aug 2022 14:37:10 -0500 Subject: [PATCH 526/947] Re-enabled anti-self-selecting conditions in extreme face finding. --- BepuPhysics/Collidables/ConvexHullHelper.cs | 19 +++++++------------ 1 file changed, 7 insertions(+), 12 deletions(-) diff --git a/BepuPhysics/Collidables/ConvexHullHelper.cs b/BepuPhysics/Collidables/ConvexHullHelper.cs index a637bf666..384af5c6e 100644 --- a/BepuPhysics/Collidables/ConvexHullHelper.cs +++ b/BepuPhysics/Collidables/ConvexHullHelper.cs @@ -103,8 +103,8 @@ static void FindExtremeFace(in Vector3Wide basisX, in Vector3Wide basisY, in Vec ref var y = ref projectedOnY[0]; Vector3Wide.Dot(basisX, toCandidate, out x); //If x is negative, that means some numerical issue has resulted in a point beyond the bounding plane that generated this face request. - //We'll treat it as if it's on the plane. - //x = Vector.Max(Vector.Zero, x); + //We'll treat it as if it's on the plane. (The reason we bother with this clamp is the sign assumption built into our angle comparison, detailed above.) + x = Vector.Max(Vector.Zero, x); Vector3Wide.Dot(basisY, toCandidate, out y); var bestY = y; var bestX = x; @@ -112,12 +112,10 @@ static void FindExtremeFace(in Vector3Wide basisX, in Vector3Wide basisY, in Vec var edgeIndexA = new Vector(sourceEdgeEndpoints.A); var edgeIndexB = new Vector(sourceEdgeEndpoints.B); var pointCountBundle = new Vector(pointCount); - ////Note that any slot that would have been considered coplanar with the edge triggering this test is ignored by the plane epsilon. - //var ignoreSlot = Vector.BitwiseOr( - // Vector.BitwiseOr(Vector.GreaterThanOrEqual(indexOffsets, pointCountBundle), Vector.LessThan(bestX, planeEpsilon)), - // Vector.BitwiseOr(Vector.Equals(indexOffsets, edgeIndexA), Vector.Equals(indexOffsets, edgeIndexB))); + //Note that any slot that would have been coplanar with the generating face *and* behind the edge (that is, a vertex almost certainly associated with the generating face) is ignored. + //Without this condition, it's possible for numerical cycles to occur where a face finds itself over and over again. var ignoreSlot = Vector.BitwiseOr( - Vector.GreaterThanOrEqual(indexOffsets, pointCountBundle), + Vector.BitwiseOr(Vector.GreaterThanOrEqual(indexOffsets, pointCountBundle), Vector.BitwiseAnd(Vector.LessThanOrEqual(bestX, planeEpsilon), Vector.LessThanOrEqual(bestY, planeEpsilon))), Vector.BitwiseOr(Vector.Equals(indexOffsets, edgeIndexA), Vector.Equals(indexOffsets, edgeIndexB))); bestX = Vector.ConditionalSelect(ignoreSlot, Vector.One, bestX); bestY = Vector.ConditionalSelect(ignoreSlot, new Vector(float.MinValue), bestY); @@ -128,15 +126,12 @@ static void FindExtremeFace(in Vector3Wide basisX, in Vector3Wide basisY, in Vec x = ref projectedOnX[i]; y = ref projectedOnY[i]; Vector3Wide.Dot(basisX, toCandidate, out x); - //x = Vector.Max(Vector.Zero, x); //Same as earlier- protect against numerical error finding points beyond the bounding plane. + x = Vector.Max(Vector.Zero, x); //Same as earlier- protect against numerical error finding points beyond the bounding plane. Vector3Wide.Dot(basisY, toCandidate, out y); var candidateIndices = indexOffsets + new Vector(i << BundleIndexing.VectorShift); - //ignoreSlot = Vector.BitwiseOr( - // Vector.BitwiseOr(Vector.GreaterThanOrEqual(candidateIndices, pointCountBundle), Vector.LessThan(x, planeEpsilon)), - // Vector.BitwiseOr(Vector.Equals(candidateIndices, edgeIndexA), Vector.Equals(candidateIndices, edgeIndexB))); ignoreSlot = Vector.BitwiseOr( - Vector.GreaterThanOrEqual(candidateIndices, pointCountBundle), + Vector.BitwiseOr(Vector.GreaterThanOrEqual(candidateIndices, pointCountBundle), Vector.BitwiseAnd(Vector.LessThanOrEqual(x, planeEpsilon), Vector.LessThanOrEqual(y, planeEpsilon))), Vector.BitwiseOr(Vector.Equals(candidateIndices, edgeIndexA), Vector.Equals(candidateIndices, edgeIndexB))); var useCandidate = Vector.AndNot(Vector.GreaterThan(y * bestX, bestY * x), ignoreSlot); bestY = Vector.ConditionalSelect(useCandidate, y, bestY); From 5747748d977ceb10e28905a9faeb430f03fac31c Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 27 Aug 2022 14:50:44 -0500 Subject: [PATCH 527/947] Stripped debug stuff. --- BepuPhysics/Collidables/ConvexHullHelper.cs | 169 ++++++------ Demos/SpecializedTests/ConvexHullTestDemo.cs | 259 ++++++++++--------- 2 files changed, 219 insertions(+), 209 deletions(-) diff --git a/BepuPhysics/Collidables/ConvexHullHelper.cs b/BepuPhysics/Collidables/ConvexHullHelper.cs index 384af5c6e..590f21f92 100644 --- a/BepuPhysics/Collidables/ConvexHullHelper.cs +++ b/BepuPhysics/Collidables/ConvexHullHelper.cs @@ -488,70 +488,6 @@ struct EdgeToTest public int FaceIndex; } - public struct DebugStep - { - public EdgeEndpoints SourceEdge; - public List Raw; - public List Reduced; - public bool[] AllowVertex; - public Vector3 FaceNormal; - public Vector3 BasisX; - public Vector3 BasisY; - public List FaceStarts; - public List FaceIndices; - public bool[] FaceDeleted; - public int[] MergedFaceIndices; - public int FaceIndex; - public Vector3[] FaceNormals; - - internal DebugStep(EdgeEndpoints sourceEdge, ref QuickList raw, Vector3 faceNormal, Vector3 basisX, Vector3 basisY, ref QuickList reduced, ref Buffer allowVertex, ref QuickList faces, Span mergedFaceIndices, int faceIndex) - { - SourceEdge = sourceEdge; - FaceNormal = faceNormal; - BasisX = basisX; - BasisY = basisY; - Raw = new List(); - for (int i = 0; i < raw.Count; ++i) - { - Raw.Add(raw[i]); - } - Reduced = new List(); - for (int i = 0; i < reduced.Count; ++i) - { - Reduced.Add(reduced[i]); - } - AllowVertex = new bool[allowVertex.Length]; - for (int i = 0; i < allowVertex.Length; ++i) - { - AllowVertex[i] = allowVertex[i]; - } - FaceStarts = new List(faces.Count); - FaceIndices = new List(); - FaceDeleted = new bool[faces.Count]; - FaceNormals = new Vector3[faces.Count]; - for (int i = 0; i < faces.Count; ++i) - { - ref var face = ref faces[i]; - FaceStarts.Add(FaceIndices.Count); - for (int j = 0; j < face.VertexIndices.Count; ++j) - FaceIndices.Add(face.VertexIndices[j]); - FaceDeleted[i] = face.Deleted; - FaceNormals[i] = face.Normal; - } - MergedFaceIndices = mergedFaceIndices.ToArray(); - FaceIndex = faceIndex; - } - } - /// - /// Computes the convex hull of a set of points. - /// - /// Point set to compute the convex hull of. - /// Buffer pool to pull memory from when creating the hull. - /// Convex hull of the input point set. - public static void ComputeHull(Span points, BufferPool pool, out HullData hullData) - { - ComputeHull(points, pool, out hullData, out _); - } static void AddFace(ref QuickList faces, BufferPool pool, Vector3 normal, QuickList vertexIndices) { @@ -604,15 +540,81 @@ static void AddIfNotPresent(ref QuickList list, int value, BufferPool pool) list.Allocate(pool) = value; } + //public struct DebugStep + //{ + // public EdgeEndpoints SourceEdge; + // public List Raw; + // public List Reduced; + // public bool[] AllowVertex; + // public Vector3 FaceNormal; + // public Vector3 BasisX; + // public Vector3 BasisY; + // public List FaceStarts; + // public List FaceIndices; + // public bool[] FaceDeleted; + // public int[] MergedFaceIndices; + // public int FaceIndex; + // public Vector3[] FaceNormals; + + // internal DebugStep(EdgeEndpoints sourceEdge, ref QuickList raw, Vector3 faceNormal, Vector3 basisX, Vector3 basisY, ref QuickList reduced, ref Buffer allowVertex, ref QuickList faces, Span mergedFaceIndices, int faceIndex) + // { + // SourceEdge = sourceEdge; + // FaceNormal = faceNormal; + // BasisX = basisX; + // BasisY = basisY; + // Raw = new List(); + // for (int i = 0; i < raw.Count; ++i) + // { + // Raw.Add(raw[i]); + // } + // Reduced = new List(); + // for (int i = 0; i < reduced.Count; ++i) + // { + // Reduced.Add(reduced[i]); + // } + // AllowVertex = new bool[allowVertex.Length]; + // for (int i = 0; i < allowVertex.Length; ++i) + // { + // AllowVertex[i] = allowVertex[i]; + // } + // FaceStarts = new List(faces.Count); + // FaceIndices = new List(); + // FaceDeleted = new bool[faces.Count]; + // FaceNormals = new Vector3[faces.Count]; + // for (int i = 0; i < faces.Count; ++i) + // { + // ref var face = ref faces[i]; + // FaceStarts.Add(FaceIndices.Count); + // for (int j = 0; j < face.VertexIndices.Count; ++j) + // FaceIndices.Add(face.VertexIndices[j]); + // FaceDeleted[i] = face.Deleted; + // FaceNormals[i] = face.Normal; + // } + // MergedFaceIndices = mergedFaceIndices.ToArray(); + // FaceIndex = faceIndex; + // } + //} + ///// + ///// Computes the convex hull of a set of points. + ///// + ///// Point set to compute the convex hull of. + ///// Buffer pool to pull memory from when creating the hull. + ///// Convex hull of the input point set. + //public static void ComputeHull(Span points, BufferPool pool, out HullData hullData) + //{ + // ComputeHull(points, pool, out hullData, out _); + //} + + /// /// Computes the convex hull of a set of points. /// /// Point set to compute the convex hull of. /// Buffer pool to pull memory from when creating the hull. /// Convex hull of the input point set. - public static void ComputeHull(Span points, BufferPool pool, out HullData hullData, out List steps) + public static void ComputeHull(Span points, BufferPool pool, out HullData hullData)//, out List steps) { - steps = new List(); + //steps = new List(); if (points.Length <= 0) { hullData = default; @@ -763,9 +765,9 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa if (Vector3.Dot(basisX, edgeToAdd.FaceNormal) > 0) Helpers.Swap(ref edgeToAdd.Endpoints.A, ref edgeToAdd.Endpoints.B); } - Vector3Wide.ReadFirst(initialBasisX, out var debugInitialBasisX); - Vector3Wide.ReadFirst(initialBasisY, out var debugInitialBasisY); - steps.Add(new DebugStep(initialSourceEdge, ref rawFaceVertexIndices, initialFaceNormal, debugInitialBasisX, debugInitialBasisY, ref reducedFaceIndices, ref allowVertex, ref faces, default, reducedFaceIndices.Count >= 3 ? 0 : -1)); + //Vector3Wide.ReadFirst(initialBasisX, out var debugInitialBasisX); + //Vector3Wide.ReadFirst(initialBasisY, out var debugInitialBasisY); + //steps.Add(new DebugStep(initialSourceEdge, ref rawFaceVertexIndices, initialFaceNormal, debugInitialBasisX, debugInitialBasisY, ref reducedFaceIndices, ref allowVertex, ref faces, default, reducedFaceIndices.Count >= 3 ? 0 : -1)); int facesDeletedCount = 0; @@ -797,7 +799,7 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa ReduceFace(ref rawFaceVertexIndices, faceNormal, points, planeEpsilonNarrow, ref facePoints, ref allowVertex, ref reducedFaceIndices); if (reducedFaceIndices.Count < 3) { - steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertex, ref faces, default, -1)); + //steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertex, ref faces, default, -1)); //Degenerate face found; don't bother creating work for it. continue; } @@ -922,7 +924,7 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa AddFace(ref faces, pool, faceNormal, reducedFaceIndices); AddFaceToEdgesAndTestList(pool, ref reducedFaceIndices, ref edgesToTest, ref facesForEdges, faceNormal, faceCountPriorToAdd); - steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertex, ref faces, facesNeedingMerge, faceCountPriorToAdd)); + //steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertex, ref faces, facesNeedingMerge, faceCountPriorToAdd)); break; } } @@ -938,18 +940,18 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa pool.Return(ref projectedOnY); pool.Return(ref pointBundles); - for (int i = 0; i < faces.Count; ++i) - { - for (int j = i + 1; j < faces.Count; ++j) - { - var dot = Vector3.Dot(faces[i].Normal, faces[j].Normal); - var bothFacesExist = !faces[i].Deleted && !faces[j].Deleted; - if (dot >= normalCoplanarityEpsilon && bothFacesExist) - { - Console.WriteLine($"Dot {dot} on faces {i} and {j}"); - } - } - } + //for (int i = 0; i < faces.Count; ++i) + //{ + // for (int j = i + 1; j < faces.Count; ++j) + // { + // var dot = Vector3.Dot(faces[i].Normal, faces[j].Normal); + // var bothFacesExist = !faces[i].Deleted && !faces[j].Deleted; + // if (dot >= normalCoplanarityEpsilon && bothFacesExist) + // { + // Console.WriteLine($"Dot {dot} on faces {i} and {j}"); + // } + // } + //} if (facesDeletedCount > 0) { @@ -992,7 +994,6 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa { var source = faces[i].VertexIndices; hullData.FaceStartIndices[i] = nextStartIndex; - //source.Span.CopyTo(0, hullData.FaceVertexIndices, nextStartIndex, source.Count); for (int j = 0; j < source.Count; ++j) { var originalVertexIndex = source[j]; diff --git a/Demos/SpecializedTests/ConvexHullTestDemo.cs b/Demos/SpecializedTests/ConvexHullTestDemo.cs index a9a296e8b..04a547b29 100644 --- a/Demos/SpecializedTests/ConvexHullTestDemo.cs +++ b/Demos/SpecializedTests/ConvexHullTestDemo.cs @@ -22,9 +22,6 @@ namespace Demos.SpecializedTests { public class ConvexHullTestDemo : Demo { - Buffer points; - List debugSteps; - unsafe Buffer CreateRandomConvexHullPoints() { const int pointCount = 50; @@ -263,9 +260,9 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); - //var hullPoints = CreateRandomConvexHullPoints(); - var hullPoints = CreateMeshConvexHull(content.Load(@"Content\newt.obj"), new Vector3(1, 1.5f, 1f)); - //var hullPoints = CreateTestConvexHull2(); + var hullPoints = CreateRandomConvexHullPoints(); + //var hullPoints = CreateMeshConvexHull(content.Load(@"Content\newt.obj"), new Vector3(1, 1.5f, 1f)); + //var hullPoints = CreateTestConvexHull(); var hullShape = new ConvexHull(hullPoints, BufferPool, out _); float largestError = 0; for (int i = 0; i < hullShape.FaceToVertexIndicesStart.Length; ++i) @@ -285,8 +282,8 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) } Console.WriteLine($"Largest error: {largestError}"); - ConvexHullHelper.ComputeHull(hullPoints, BufferPool, out var hullData, out debugSteps); - this.points = hullPoints; + //ConvexHullHelper.ComputeHull(hullPoints, BufferPool, out var hullData, out debugSteps); + //this.points = hullPoints; var boxHullPoints = CreateBoxConvexHull(2); var boxHullShape = new ConvexHull(boxHullPoints, BufferPool, out _); @@ -386,122 +383,134 @@ void TestConvexHullCreation() } } - int stepIndex = 0; - - public override void Update(Window window, Camera camera, Input input, float dt) - { - if (input.TypedCharacters.Contains('x')) - { - stepIndex = Math.Max(stepIndex - 1, 0); - } - if (input.TypedCharacters.Contains('c')) - { - stepIndex = Math.Min(stepIndex + 1, debugSteps.Count - 1); - } - if (input.WasPushed(OpenTK.Input.Key.P)) - { - showWireframe = !showWireframe; - } - if (input.WasPushed(OpenTK.Input.Key.U)) - { - showDeleted = !showDeleted; - } - if (input.WasPushed(OpenTK.Input.Key.Y)) - { - showVertexIndices = !showVertexIndices; - } - base.Update(window, camera, input, dt); - } - - bool showWireframe; - bool showDeleted; - bool showVertexIndices; - public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) - { - var step = debugSteps[stepIndex]; - var scale = 10f; - var renderOffset = new Vector3(0, 15, 0); - for (int i = 0; i < points.Length; ++i) - { - var pose = new RigidPose(renderOffset + points[i] * scale); - renderer.Shapes.AddShape(new Box(0.1f, 0.1f, 0.1f), Simulation.Shapes, pose, new Vector3(0.5f, 0.5f, 0.5f)); - if (!step.AllowVertex[i]) - renderer.Shapes.AddShape(new Box(0.6f, 0.25f, 0.25f), Simulation.Shapes, pose, new Vector3(1, 0, 0)); - } - for (int i = 0; i < step.Raw.Count; ++i) - { - var pose = new RigidPose(renderOffset + points[step.Raw[i]] * scale); - renderer.Shapes.AddShape(new Box(0.25f, 0.6f, 0.25f), Simulation.Shapes, pose, new Vector3(0, 0, 1)); - } - for (int i = 0; i < step.Reduced.Count; ++i) - { - var pose = new RigidPose(renderOffset + points[step.Reduced[i]] * scale); - renderer.Shapes.AddShape(new Box(0.25f, 0.25f, 0.6f), Simulation.Shapes, pose, new Vector3(0, 1, 0)); - } - - { - var pose = new RigidPose(renderOffset); - for (int i = 0; i < step.FaceStarts.Count; ++i) - { - if (showDeleted || !step.FaceDeleted[i]) - { - var faceStart = step.FaceStarts[i]; - var faceEnd = i + 1 < step.FaceStarts.Count ? step.FaceStarts[i + 1] : step.FaceIndices.Count; - var count = faceEnd - faceStart; - var color = step.FaceDeleted[i] ? new Vector3(0.25f, 0.25f, 0.25f) : step.FaceIndex == i ? new Vector3(1, 0, 0.5f) : new Vector3(1, 0, 1); - var deletionInducedScale = step.FaceDeleted[i] ? new Vector3(1.1f) : new Vector3(1f); - - var offset = step.FaceDeleted[i] ? step.FaceNormals[i] * 0.25f : new Vector3(); - if (showWireframe) - { - var previousIndex = faceEnd - 1; - for (int q = faceStart; q < faceEnd; ++q) - { - var a = points[step.FaceIndices[q]] * scale + pose.Position + offset; - var b = points[step.FaceIndices[previousIndex]] * scale + pose.Position + offset; - previousIndex = q; - renderer.Lines.Allocate() = new LineInstance(a, b, color, Vector3.Zero); - } - } - else - { - for (int k = faceStart + 2; k < faceEnd; ++k) - { - renderer.Shapes.AddShape(new Triangle - { - A = points[step.FaceIndices[faceStart]] * scale + offset, - B = points[step.FaceIndices[k]] * scale + offset, - C = points[step.FaceIndices[k - 1]] * scale + offset - }, Simulation.Shapes, pose, color); - } - } - } - } - } - - if (showVertexIndices) - { - for (int i = 0; i < points.Length; ++i) - { - if (DemoRenderer.Helpers.GetScreenLocation(points[i] * scale + renderOffset, camera.ViewProjection, renderer.Surface.Resolution, out var location)) - { - renderer.TextBatcher.Write(text.Clear().Append(i), location, 10, new Vector3(1), font); - } - } - } - - var edgeMidpoint = renderOffset + (points[step.SourceEdge.A] + points[step.SourceEdge.B]) * scale * 0.5f; - renderer.Lines.Allocate() = new LineInstance(edgeMidpoint, edgeMidpoint + step.BasisX * scale * 0.5f, new Vector3(1, 1, 0), new Vector3()); - renderer.Lines.Allocate() = new LineInstance(edgeMidpoint, edgeMidpoint + step.BasisY * scale * 0.5f, new Vector3(0, 1, 0), new Vector3()); - renderer.TextBatcher.Write( - text.Clear().Append($"Enumerate step with X and C. Current step: ").Append(stepIndex + 1).Append(" out of ").Append(debugSteps.Count), - new Vector2(32, renderer.Surface.Resolution.Y - 140), 20, new Vector3(1), font); - renderer.TextBatcher.Write(text.Clear().Append("Show wireframe: P ").Append(showWireframe ? "(on)" : "(off)"), new Vector2(32, renderer.Surface.Resolution.Y - 120), 20, new Vector3(1), font); - renderer.TextBatcher.Write(text.Clear().Append("Show deleted: U ").Append(showDeleted ? "(on)" : "(off)"), new Vector2(32, renderer.Surface.Resolution.Y - 100), 20, new Vector3(1), font); - renderer.TextBatcher.Write(text.Clear().Append("Show vertex indices: Y ").Append(showVertexIndices ? "(on)" : "(off)"), new Vector2(32, renderer.Surface.Resolution.Y - 80), 20, new Vector3(1), font); - - - base.Render(renderer, camera, input, text, font); - } + //Buffer points; + //List debugSteps; + + //int stepIndex = 0; + + //public override void Update(Window window, Camera camera, Input input, float dt) + //{ + // if (input.TypedCharacters.Contains('x')) + // { + // stepIndex = Math.Max(stepIndex - 1, 0); + // } + // if (input.TypedCharacters.Contains('c')) + // { + // stepIndex = Math.Min(stepIndex + 1, debugSteps.Count - 1); + // } + // if (input.WasPushed(OpenTK.Input.Key.P)) + // { + // showWireframe = !showWireframe; + // } + // if (input.WasPushed(OpenTK.Input.Key.U)) + // { + // showDeleted = !showDeleted; + // } + // if (input.WasPushed(OpenTK.Input.Key.Y)) + // { + // showVertexIndices = !showVertexIndices; + // } + // if (input.WasPushed(OpenTK.Input.Key.H)) + // { + // showFaceVertexStatuses = !showFaceVertexStatuses; + // } + // base.Update(window, camera, input, dt); + //} + + //bool showWireframe; + //bool showDeleted; + //bool showVertexIndices; + //bool showFaceVertexStatuses = true; + //public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + //{ + // var step = debugSteps[stepIndex]; + // var scale = 10f; + // var renderOffset = new Vector3(0, 15, 0); + // for (int i = 0; i < points.Length; ++i) + // { + // var pose = new RigidPose(renderOffset + points[i] * scale); + // renderer.Shapes.AddShape(new Box(0.1f, 0.1f, 0.1f), Simulation.Shapes, pose, new Vector3(0.5f, 0.5f, 0.5f)); + // if (!step.AllowVertex[i] && showFaceVertexStatuses) + // renderer.Shapes.AddShape(new Box(0.6f, 0.25f, 0.25f), Simulation.Shapes, pose, new Vector3(1, 0, 0)); + // } + // if (showFaceVertexStatuses) + // { + // for (int i = 0; i < step.Raw.Count; ++i) + // { + // var pose = new RigidPose(renderOffset + points[step.Raw[i]] * scale); + // renderer.Shapes.AddShape(new Box(0.25f, 0.6f, 0.25f), Simulation.Shapes, pose, new Vector3(0, 0, 1)); + // } + // for (int i = 0; i < step.Reduced.Count; ++i) + // { + // var pose = new RigidPose(renderOffset + points[step.Reduced[i]] * scale); + // renderer.Shapes.AddShape(new Box(0.25f, 0.25f, 0.6f), Simulation.Shapes, pose, new Vector3(0, 1, 0)); + // } + // } + + // { + // var pose = new RigidPose(renderOffset); + // for (int i = 0; i < step.FaceStarts.Count; ++i) + // { + // if (showDeleted || !step.FaceDeleted[i]) + // { + // var faceStart = step.FaceStarts[i]; + // var faceEnd = i + 1 < step.FaceStarts.Count ? step.FaceStarts[i + 1] : step.FaceIndices.Count; + // var count = faceEnd - faceStart; + // var color = step.FaceDeleted[i] ? new Vector3(0.25f, 0.25f, 0.25f) : step.FaceIndex == i ? new Vector3(1, 0, 0.5f) : new Vector3(1, 0, 1); + // var deletionInducedScale = step.FaceDeleted[i] ? new Vector3(1.1f) : new Vector3(1f); + + // var offset = step.FaceDeleted[i] ? step.FaceNormals[i] * 0.25f : new Vector3(); + // if (showWireframe) + // { + // var previousIndex = faceEnd - 1; + // for (int q = faceStart; q < faceEnd; ++q) + // { + // var a = points[step.FaceIndices[q]] * scale + pose.Position + offset; + // var b = points[step.FaceIndices[previousIndex]] * scale + pose.Position + offset; + // previousIndex = q; + // renderer.Lines.Allocate() = new LineInstance(a, b, color, Vector3.Zero); + // } + // } + // else + // { + // for (int k = faceStart + 2; k < faceEnd; ++k) + // { + // renderer.Shapes.AddShape(new Triangle + // { + // A = points[step.FaceIndices[faceStart]] * scale + offset, + // B = points[step.FaceIndices[k]] * scale + offset, + // C = points[step.FaceIndices[k - 1]] * scale + offset + // }, Simulation.Shapes, pose, color); + // } + // } + // } + // } + // } + + // if (showVertexIndices) + // { + // for (int i = 0; i < points.Length; ++i) + // { + // if (DemoRenderer.Helpers.GetScreenLocation(points[i] * scale + renderOffset, camera.ViewProjection, renderer.Surface.Resolution, out var location)) + // { + // renderer.TextBatcher.Write(text.Clear().Append(i), location, 10, new Vector3(1), font); + // } + // } + // } + + // var edgeMidpoint = renderOffset + (points[step.SourceEdge.A] + points[step.SourceEdge.B]) * scale * 0.5f; + // renderer.Lines.Allocate() = new LineInstance(edgeMidpoint, edgeMidpoint + step.BasisX * scale * 0.5f, new Vector3(1, 1, 0), new Vector3()); + // renderer.Lines.Allocate() = new LineInstance(edgeMidpoint, edgeMidpoint + step.BasisY * scale * 0.5f, new Vector3(0, 1, 0), new Vector3()); + // renderer.TextBatcher.Write( + // text.Clear().Append($"Enumerate step with X and C. Current step: ").Append(stepIndex + 1).Append(" out of ").Append(debugSteps.Count), + // new Vector2(32, renderer.Surface.Resolution.Y - 140), 20, new Vector3(1), font); + // renderer.TextBatcher.Write(text.Clear().Append("Show wireframe: P ").Append(showWireframe ? "(on)" : "(off)"), new Vector2(32, renderer.Surface.Resolution.Y - 120), 20, new Vector3(1), font); + // renderer.TextBatcher.Write(text.Clear().Append("Show deleted: U ").Append(showDeleted ? "(on)" : "(off)"), new Vector2(32, renderer.Surface.Resolution.Y - 100), 20, new Vector3(1), font); + // renderer.TextBatcher.Write(text.Clear().Append("Show vertex indices: Y ").Append(showVertexIndices ? "(on)" : "(off)"), new Vector2(32, renderer.Surface.Resolution.Y - 80), 20, new Vector3(1), font); + // renderer.TextBatcher.Write(text.Clear().Append("Show face vertex statuses: H ").Append(showFaceVertexStatuses ? "(on)" : "(off)"), new Vector2(32, renderer.Surface.Resolution.Y - 60), 20, new Vector3(1), font); + + + // base.Render(renderer, camera, input, text, font); + //} } } From f1a97d750873404cbf8d1c5bdcb0726e40ff89b7 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 27 Aug 2022 14:51:12 -0500 Subject: [PATCH 528/947] Bumped version number. --- BepuPhysics/BepuPhysics.csproj | 2 +- BepuUtilities/BepuUtilities.csproj | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/BepuPhysics/BepuPhysics.csproj b/BepuPhysics/BepuPhysics.csproj index ac5bf74fc..808f62579 100644 --- a/BepuPhysics/BepuPhysics.csproj +++ b/BepuPhysics/BepuPhysics.csproj @@ -1,7 +1,7 @@  net6.0 - 2.5.0-beta.7 + 2.5.0-beta.8 Bepu Entertainment LLC Ross Nordby Speedy real time physics simulation library. diff --git a/BepuUtilities/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj index 4c6117aa6..51d1a96ab 100644 --- a/BepuUtilities/BepuUtilities.csproj +++ b/BepuUtilities/BepuUtilities.csproj @@ -3,7 +3,7 @@ BepuUtilities BepuUtilities net6.0 - 2.5.0-beta.7 + 2.5.0-beta.8 Bepu Entertainment LLC Ross Nordby Supporting utilities library for BEPUphysics v2. From f318ff527bf30608568640fa9ac7c742614693cb Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 29 Aug 2022 13:21:17 -0500 Subject: [PATCH 529/947] Added total memory allocation grab for BufferPool, fixed doofcomments. --- BepuUtilities/Memory/BufferPool.cs | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/BepuUtilities/Memory/BufferPool.cs b/BepuUtilities/Memory/BufferPool.cs index f728b1385..68ae77474 100644 --- a/BepuUtilities/Memory/BufferPool.cs +++ b/BepuUtilities/Memory/BufferPool.cs @@ -9,9 +9,8 @@ namespace BepuUtilities.Memory { /// - /// Unmanaged memory pool that creates pinned blocks of memory for use in spans. + /// Unmanaged memory pool that suballocates from memory blocks pulled from the native heap. /// - /// This currently works by allocating large managed arrays and pinning them under the assumption that they'll end up in the large object heap. public class BufferPool : IUnmanagedMemoryPool, IDisposable { unsafe struct PowerPool @@ -252,6 +251,21 @@ public int GetCapacityForPower(int power) return pools[power].BlockCount * pools[power].BlockSize; } + /// + /// Computes the total number of bytes allocated from native memory in this buffer pool. + /// Includes allocated memory regardless of whether it currently has outstanding references. + /// + /// Total number of bytes allocated from native memory in this buffer pool. + public ulong GetTotalAllocatedByteCount() + { + ulong sum = 0; + for (int i = 0; i < pools.Length; ++i) + { + sum += (ulong)pools[i].BlockCount * (ulong)pools[i].BlockSize; + } + return sum; + } + /// /// Takes a buffer large enough to contain a number of elements of a given type. Capacity may be larger than requested. /// From a015227d641ffb75f6c2e4fe7937d4aa07a8d1d5 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 29 Aug 2022 13:52:02 -0500 Subject: [PATCH 530/947] Bumping to net7 because 2.5 won't be released before net7 does! --- BepuPhysics/BepuPhysics.csproj | 2 +- BepuUtilities/BepuUtilities.csproj | 2 +- DemoContentBuilder/DemoContentBuilder.csproj | 2 +- DemoContentLoader/DemoContentLoader.csproj | 2 +- DemoRenderer/DemoRenderer.csproj | 6 +++--- DemoTests/DemoTests.csproj | 2 +- DemoUtilities/DemoUtilities.csproj | 2 +- Demos/Demos.csproj | 6 +++--- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/BepuPhysics/BepuPhysics.csproj b/BepuPhysics/BepuPhysics.csproj index 808f62579..c7efb1b4e 100644 --- a/BepuPhysics/BepuPhysics.csproj +++ b/BepuPhysics/BepuPhysics.csproj @@ -1,6 +1,6 @@  - net6.0 + net7.0 2.5.0-beta.8 Bepu Entertainment LLC Ross Nordby diff --git a/BepuUtilities/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj index 51d1a96ab..da947baf9 100644 --- a/BepuUtilities/BepuUtilities.csproj +++ b/BepuUtilities/BepuUtilities.csproj @@ -2,7 +2,7 @@ BepuUtilities BepuUtilities - net6.0 + net7.0 2.5.0-beta.8 Bepu Entertainment LLC Ross Nordby diff --git a/DemoContentBuilder/DemoContentBuilder.csproj b/DemoContentBuilder/DemoContentBuilder.csproj index f01857a7c..5277e7c26 100644 --- a/DemoContentBuilder/DemoContentBuilder.csproj +++ b/DemoContentBuilder/DemoContentBuilder.csproj @@ -2,7 +2,7 @@ Exe - net6.0 + net7.0 x64 latest true diff --git a/DemoContentLoader/DemoContentLoader.csproj b/DemoContentLoader/DemoContentLoader.csproj index 4953d4e7e..847b2074e 100644 --- a/DemoContentLoader/DemoContentLoader.csproj +++ b/DemoContentLoader/DemoContentLoader.csproj @@ -1,7 +1,7 @@  - net6.0 + net7.0 latest True diff --git a/DemoRenderer/DemoRenderer.csproj b/DemoRenderer/DemoRenderer.csproj index 9afeb8667..a75017523 100644 --- a/DemoRenderer/DemoRenderer.csproj +++ b/DemoRenderer/DemoRenderer.csproj @@ -1,13 +1,13 @@  - net6.0 + net7.0 latest True - - + + diff --git a/DemoTests/DemoTests.csproj b/DemoTests/DemoTests.csproj index cc62ea6e8..08da411bc 100644 --- a/DemoTests/DemoTests.csproj +++ b/DemoTests/DemoTests.csproj @@ -1,7 +1,7 @@ - net6.0 + net7.0 true false latest diff --git a/DemoUtilities/DemoUtilities.csproj b/DemoUtilities/DemoUtilities.csproj index 00c05bfeb..e85afca8c 100644 --- a/DemoUtilities/DemoUtilities.csproj +++ b/DemoUtilities/DemoUtilities.csproj @@ -1,7 +1,7 @@  - net6.0 + net7.0 latest true diff --git a/Demos/Demos.csproj b/Demos/Demos.csproj index f949c6848..032d20156 100644 --- a/Demos/Demos.csproj +++ b/Demos/Demos.csproj @@ -1,7 +1,7 @@  Exe - net6.0 + net7.0 True Debug;Release latest @@ -9,8 +9,8 @@ - - + + From dd19eea9d7ad069b4e04ab3efa177e53690bdebf Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 29 Aug 2022 14:29:02 -0500 Subject: [PATCH 531/947] Fiddling with scalar integration impact. --- Demos/DemoSet.cs | 1 - .../ScalarIntegrationTestDemo.cs | 194 ++++++++++++++++++ 2 files changed, 194 insertions(+), 1 deletion(-) create mode 100644 Demos/SpecializedTests/ScalarIntegrationTestDemo.cs diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index c32bb3879..f40e1dc29 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -46,7 +46,6 @@ struct Option public DemoSet() { - AddOption(); AddOption(); AddOption(); AddOption(); diff --git a/Demos/SpecializedTests/ScalarIntegrationTestDemo.cs b/Demos/SpecializedTests/ScalarIntegrationTestDemo.cs new file mode 100644 index 000000000..fca204c7d --- /dev/null +++ b/Demos/SpecializedTests/ScalarIntegrationTestDemo.cs @@ -0,0 +1,194 @@ +using BepuPhysics.Collidables; +using BepuPhysics.Constraints; +using BepuPhysics; +using BepuUtilities; +using DemoContentLoader; +using DemoRenderer.UI; +using DemoRenderer; +using DemoUtilities; +using System; +using System.Collections.Generic; +using System.Linq; +using System.Numerics; +using System.Text; +using System.Threading.Tasks; +using BepuUtilities.Collections; + +namespace Demos.SpecializedTests +{ + public unsafe class ScalarIntegrationTestDemo : Demo + { + struct ScalarIntegrationCallbacks : IPoseIntegratorCallbacks + { + public delegate* IntegrateVelocityFunction; + + public readonly AngularIntegrationMode AngularIntegrationMode => AngularIntegrationMode.Nonconserving; + + public readonly bool AllowSubstepsForUnconstrainedBodies => false; + + public readonly bool IntegrateVelocityForKinematics => false; + + public void Initialize(Simulation simulation) + { + } + + public void PrepareForIntegration(float dt) + { + } + + public void IntegrateVelocity(Vector bodyIndices, Vector3Wide position, QuaternionWide orientation, BodyInertiaWide localInertia, Vector integrationMask, int workerIndex, Vector dt, ref BodyVelocityWide velocity) + { + //TODO: This is going to be a very bad implementation for now. Vectorized transposition would speed this up. + for (int i = 0; i < Vector.Count; ++i) + { + if (integrationMask[i] != 0) + { + Vector3Wide.ReadSlot(ref position, i, out var scalarPosition); + QuaternionWide.ReadSlot(ref orientation, i, out var scalarOrientation); + BodyInertia scalarInertia; + scalarInertia.InverseInertiaTensor.XX = localInertia.InverseInertiaTensor.XX[i]; + scalarInertia.InverseInertiaTensor.YX = localInertia.InverseInertiaTensor.YX[i]; + scalarInertia.InverseInertiaTensor.YY = localInertia.InverseInertiaTensor.YY[i]; + scalarInertia.InverseInertiaTensor.ZX = localInertia.InverseInertiaTensor.ZX[i]; + scalarInertia.InverseInertiaTensor.ZY = localInertia.InverseInertiaTensor.ZY[i]; + scalarInertia.InverseInertiaTensor.ZZ = localInertia.InverseInertiaTensor.ZZ[i]; + scalarInertia.InverseMass = localInertia.InverseMass[i]; + BodyVelocity scalarVelocity; + Vector3Wide.ReadSlot(ref velocity.Linear, i, out scalarVelocity.Linear); + Vector3Wide.ReadSlot(ref velocity.Angular, i, out scalarVelocity.Angular); + + IntegrateVelocityFunction(bodyIndices[i], scalarPosition, scalarOrientation, scalarInertia, workerIndex, dt[i], &scalarVelocity); + + Vector3Wide.WriteSlot(scalarVelocity.Linear, i, ref velocity.Linear); + Vector3Wide.WriteSlot(scalarVelocity.Angular, i, ref velocity.Angular); + } + } + } + } + + static void IntegrateVelocity(int bodyIndex, Vector3 position, Quaternion orientation, BodyInertia inertia, int workerIndex, float dt, BodyVelocity* velocity) + { + velocity->Linear += new Vector3(0, -10 / 60f, 0); + } + + public unsafe override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(-30, 10, -30); + //camera.Yaw = MathHelper.Pi ; + camera.Yaw = MathHelper.Pi * 3f / 4; + //camera.Pitch = MathHelper.PiOver2 * 0.999f; + ScalarIntegrationCallbacks callbacks = new() { IntegrateVelocityFunction = &IntegrateVelocity }; + //DemoPoseIntegratorCallbacks callbacks = new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)); + + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), callbacks, new SolveDescription(1, 4)); + + var sphere = new Sphere(1.5f); + var capsule = new Capsule(1f, 1f); + var box = new Box(1f, 3f, 2f); + var cylinder = new Cylinder(1.5f, 0.3f); + var points = new QuickList(32, BufferPool); + //Boxlike point cloud. + //points.Allocate(BufferPool) = new Vector3(0, 0, 0); + //points.Allocate(BufferPool) = new Vector3(0, 0, 1); + //points.Allocate(BufferPool) = new Vector3(0, 1, 0); + //points.Allocate(BufferPool) = new Vector3(0, 1, 1); + //points.Allocate(BufferPool) = new Vector3(1, 0, 0); + //points.Allocate(BufferPool) = new Vector3(1, 0, 1); + //points.Allocate(BufferPool) = new Vector3(1, 1, 0); + //points.Allocate(BufferPool) = new Vector3(1, 1, 1); + + //Rando pointcloud. + //var random = new Random(5); + //for (int i = 0; i < 32; ++i) + //{ + // points.Allocate(BufferPool) = new Vector3(3 * random.NextSingle(), 1 * random.NextSingle(), 3 * random.NextSingle()); + //} + + //Dodecahedron pointcloud. + points.Allocate(BufferPool) = new Vector3(-1, -1, -1); + points.Allocate(BufferPool) = new Vector3(-1, -1, 1); + points.Allocate(BufferPool) = new Vector3(-1, 1, -1); + points.Allocate(BufferPool) = new Vector3(-1, 1, 1); + points.Allocate(BufferPool) = new Vector3(1, -1, -1); + points.Allocate(BufferPool) = new Vector3(1, -1, 1); + points.Allocate(BufferPool) = new Vector3(1, 1, -1); + points.Allocate(BufferPool) = new Vector3(1, 1, 1); + + const float goldenRatio = 1.618033988749f; + const float oogr = 1f / goldenRatio; + + points.Allocate(BufferPool) = new Vector3(0, goldenRatio, oogr); + points.Allocate(BufferPool) = new Vector3(0, -goldenRatio, oogr); + points.Allocate(BufferPool) = new Vector3(0, goldenRatio, -oogr); + points.Allocate(BufferPool) = new Vector3(0, -goldenRatio, -oogr); + + points.Allocate(BufferPool) = new Vector3(oogr, 0, goldenRatio); + points.Allocate(BufferPool) = new Vector3(oogr, 0, -goldenRatio); + points.Allocate(BufferPool) = new Vector3(-oogr, 0, goldenRatio); + points.Allocate(BufferPool) = new Vector3(-oogr, 0, -goldenRatio); + + points.Allocate(BufferPool) = new Vector3(goldenRatio, oogr, 0); + points.Allocate(BufferPool) = new Vector3(goldenRatio, -oogr, 0); + points.Allocate(BufferPool) = new Vector3(-goldenRatio, oogr, 0); + points.Allocate(BufferPool) = new Vector3(-goldenRatio, -oogr, 0); + + var convexHull = new ConvexHull(points.Span.Slice(points.Count), BufferPool, out _); + var boxInertia = box.ComputeInertia(1); + var capsuleInertia = capsule.ComputeInertia(1); + var sphereInertia = sphere.ComputeInertia(1); + var cylinderInertia = cylinder.ComputeInertia(1); + var hullInertia = convexHull.ComputeInertia(1); + var boxIndex = Simulation.Shapes.Add(box); + var capsuleIndex = Simulation.Shapes.Add(capsule); + var sphereIndex = Simulation.Shapes.Add(sphere); + var cylinderIndex = Simulation.Shapes.Add(cylinder); + var hullIndex = Simulation.Shapes.Add(convexHull); + const int width = 32; + const int height = 32; + const int length = 32; + var shapeCount = 0; + for (int i = 0; i < width; ++i) + { + for (int j = 0; j < height; ++j) + { + for (int k = 0; k < length; ++k) + { + var location = new Vector3(6, 3, 6) * new Vector3(i, j, k) + new Vector3(-width * 1.5f, 5.5f, -length * 1.5f); + var bodyDescription = BodyDescription.CreateKinematic(location, new(default, ContinuousDetection.Passive), -0.01f); + var index = shapeCount++; + switch (index % 5) + { + case 0: + bodyDescription.Collidable.Shape = sphereIndex; + bodyDescription.LocalInertia = sphereInertia; + break; + case 1: + bodyDescription.Collidable.Shape = capsuleIndex; + bodyDescription.LocalInertia = capsuleInertia; + break; + case 2: + bodyDescription.Collidable.Shape = boxIndex; + bodyDescription.LocalInertia = boxInertia; + break; + case 3: + bodyDescription.Collidable.Shape = cylinderIndex; + bodyDescription.LocalInertia = cylinderInertia; + break; + case 4: + bodyDescription.Collidable.Shape = hullIndex; + bodyDescription.LocalInertia = hullInertia; + break; + } + Simulation.Bodies.Add(bodyDescription); + + } + } + } + + //Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(new Box(500, 1, 500)))); + DemoMeshHelper.CreateDeformedPlane(128, 128, (x, y) => new Vector3(x - 64, 2f * (float)(Math.Sin(x * 0.5f) * Math.Sin(y * 0.5f)), y - 64), new Vector3(4, 1, 4), BufferPool, out var mesh); + Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); + + } + } +} From 78cac6d30f03f725c3d20de036ee318d0b0c3e0e Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 29 Aug 2022 14:29:11 -0500 Subject: [PATCH 532/947] Workflow bumped to net7. --- .github/workflows/dotnet-core.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/dotnet-core.yml b/.github/workflows/dotnet-core.yml index c344f82eb..94bfc5742 100644 --- a/.github/workflows/dotnet-core.yml +++ b/.github/workflows/dotnet-core.yml @@ -11,7 +11,7 @@ jobs: - uses: actions/checkout@v2 - uses: actions/setup-dotnet@v1 with: - dotnet-version: '6.x' + dotnet-version: '7.x' - name: Install dependencies run: | dotnet restore DemoContentBuilder From 22af6054499f8e0af4313edba89a40a2f6c91bf2 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 29 Aug 2022 14:36:27 -0500 Subject: [PATCH 533/947] Obligatory oops --- .github/workflows/dotnet-core.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/dotnet-core.yml b/.github/workflows/dotnet-core.yml index 94bfc5742..86cae4581 100644 --- a/.github/workflows/dotnet-core.yml +++ b/.github/workflows/dotnet-core.yml @@ -12,6 +12,7 @@ jobs: - uses: actions/setup-dotnet@v1 with: dotnet-version: '7.x' + include-prerelease: true - name: Install dependencies run: | dotnet restore DemoContentBuilder From f29a0d2d561e629c32fbba9f6cc05be305f2a5c3 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 29 Aug 2022 16:29:33 -0500 Subject: [PATCH 534/947] Some convex hull test fiddling. --- Demos/SpecializedTests/ConvexHullTestDemo.cs | 38 +++++++++++++++++++- 1 file changed, 37 insertions(+), 1 deletion(-) diff --git a/Demos/SpecializedTests/ConvexHullTestDemo.cs b/Demos/SpecializedTests/ConvexHullTestDemo.cs index 04a547b29..370d21de8 100644 --- a/Demos/SpecializedTests/ConvexHullTestDemo.cs +++ b/Demos/SpecializedTests/ConvexHullTestDemo.cs @@ -251,6 +251,35 @@ unsafe Buffer CreateTestConvexHull2() return vertices; } + + unsafe Buffer CreateTestConvexHull3() + { + BufferPool.Take(22, out var vertices); + vertices[0] = new Vector3(-0.103558f, 1.000000f, -0.490575f); + vertices[1] = new Vector3(0.266493f, 0.659794f, -0.363751f); + vertices[2] = new Vector3(-0.245774f, 0.762636f, -0.615304f); + vertices[3] = new Vector3(0.164688f, -0.777634f, -0.365919f); + vertices[4] = new Vector3(0.503268f, -0.846406f, -0.131286f); + vertices[5] = new Vector3(0.171066f, -0.931723f, -0.140738f); + vertices[6] = new Vector3(-0.247963f, -0.738059f, -0.413146f); + vertices[7] = new Vector3(-0.319203f, -0.260078f, -0.609331f); + vertices[8] = new Vector3(0.469624f, -0.747848f, -0.286486f); + vertices[9] = new Vector3(0.398526f, -0.238233f, -0.435281f); + vertices[10] = new Vector3(0.448274f, 0.295416f, -0.246327f); + vertices[11] = new Vector3(-0.245774f, 0.762636f, 0.596521f); + vertices[12] = new Vector3(0.266493f, 0.659794f, 0.344974f); + vertices[13] = new Vector3(-0.103558f, 1.000000f, 0.471792f); + vertices[14] = new Vector3(0.171066f, -0.931723f, 0.121961f); + vertices[15] = new Vector3(0.503268f, -0.846406f, 0.112509f); + vertices[16] = new Vector3(0.164688f, -0.777634f, 0.347137f); + vertices[17] = new Vector3(-0.319203f, -0.260078f, 0.590548f); + vertices[18] = new Vector3(-0.247963f, -0.738059f, 0.394364f); + vertices[19] = new Vector3(0.469624f, -0.747848f, 0.267709f); + vertices[20] = new Vector3(0.398526f, -0.238233f, 0.416498f); + vertices[21] = new Vector3(0.448274f, 0.295411f, 0.227550f); + return vertices; + } + public unsafe override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(0, -2.5f, 10); @@ -262,7 +291,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var hullPoints = CreateRandomConvexHullPoints(); //var hullPoints = CreateMeshConvexHull(content.Load(@"Content\newt.obj"), new Vector3(1, 1.5f, 1f)); - //var hullPoints = CreateTestConvexHull(); + //var hullPoints = CreateTestConvexHull3(); var hullShape = new ConvexHull(hullPoints, BufferPool, out _); float largestError = 0; for (int i = 0; i < hullShape.FaceToVertexIndicesStart.Length; ++i) @@ -362,6 +391,13 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) } } Simulation.Statics.Add(new StaticDescription(new Vector3(0, -10, 0), Simulation.Shapes.Add(new Box(1000, 1, 1000)))); + + Random random = new Random(5); + DemoMeshHelper.CreateDeformedPlane(64, 64, (x, y) => new Vector3( + x + 8, + 2f * MathF.Sin(x * 0.125f) * MathF.Sin(y * 0.125f) + 0.1f * random.NextSingle() - 3, + y - 8), new Vector3(1, 1, 1), BufferPool, out var mesh); + Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); } void TestConvexHullCreation() From 7b3eeea883f88f8f2f8447ac6e9df848e1dd9842 Mon Sep 17 00:00:00 2001 From: Justin Britt Date: Sat, 10 Sep 2022 08:30:20 -0400 Subject: [PATCH 535/947] Added missing texture format (R8G8B8A8_UNorm_SRgb) to exception message --- DemoRenderer.GL/UI/RenderableImage.cs | 2 +- DemoRenderer/UI/RenderableImage.cs | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/DemoRenderer.GL/UI/RenderableImage.cs b/DemoRenderer.GL/UI/RenderableImage.cs index 3f364c820..e997f9190 100644 --- a/DemoRenderer.GL/UI/RenderableImage.cs +++ b/DemoRenderer.GL/UI/RenderableImage.cs @@ -26,7 +26,7 @@ public RenderableImage(Texture2DContent imageContent, bool srgb = false, string { if (imageContent.TexelSizeInBytes != 4) { - throw new ArgumentException("The renderable image assumes an R8G8B8A8_UNorm or texture."); + throw new ArgumentException("The renderable image assumes an R8G8B8A8_UNorm or R8G8B8A8_UNorm_SRgb texture."); } Debug.Assert(imageContent.MipLevels == 1, "We ignore any mip levels stored in the content; if the content pipeline output them, something's likely mismatched."); Content = imageContent; diff --git a/DemoRenderer/UI/RenderableImage.cs b/DemoRenderer/UI/RenderableImage.cs index 79a7d0aca..fc0d9fb5f 100644 --- a/DemoRenderer/UI/RenderableImage.cs +++ b/DemoRenderer/UI/RenderableImage.cs @@ -50,7 +50,7 @@ public unsafe RenderableImage(Device device, DeviceContext context, Texture2DCon { if (imageContent.TexelSizeInBytes != 4) { - throw new ArgumentException("The renderable image assumes an R8G8B8A8_UNorm or texture."); + throw new ArgumentException("The renderable image assumes an R8G8B8A8_UNorm or R8G8B8A8_UNorm_SRgb texture."); } Debug.Assert(imageContent.MipLevels == 1, "We ignore any mip levels stored in the content; if the content pipeline output them, something's likely mismatched."); Initialize(device, imageContent.Width, imageContent.Height, srgb, debugName); From cae1904799f3a607af3787a0281aca275f867173 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 10 Sep 2022 14:44:12 -0500 Subject: [PATCH 536/947] Sponsor update! --- .../ladybugwouldpreferlandvaluetax.png | Bin 0 -> 133705 bytes Demos/Demos.content | 3 ++- Demos/Demos/Sponsors/SponsorDemo.cs | 1 + 3 files changed, 3 insertions(+), 1 deletion(-) create mode 100644 Demos/Content/Sponsors/ladybugwouldpreferlandvaluetax.png diff --git a/Demos/Content/Sponsors/ladybugwouldpreferlandvaluetax.png b/Demos/Content/Sponsors/ladybugwouldpreferlandvaluetax.png new file mode 100644 index 0000000000000000000000000000000000000000..2eb6209fef31408297f92804aaf2a311672c6e91 GIT binary patch literal 133705 zcmdSBWmJ}H*98g)5+aR=N{bQ-3ML&A64D`35`wfyH!6)H4bn(ANS6pArF3_9NvE83 zKWz8=eeXBM`FX}TKOB3DJa=5zy5^c|uDKo`*(YM>@h;(^p`o3BEdEFy4eiVd{F%c! z3qMgC&bCHFLpOUVEG+v{R~HS9DbPBQTk?z`nFs%8lDMRHp?Z1`Uk@Q!B1xb4_iEp9 zsy~fb2jf&U6-3c~Q_jmLy7B0;mdA;}vv10RL}Y^Bn`y=~iJz{O3cb1d<>zPs=W8}j z1KK_!G`{E&u@FN_ZcN`GgP*n$;CE( z7bs=BXnAxVm}%Nc*%{qjV;UtfyU#J`yPmM_#)?f=EniN({$rc})VSAtNJUa>ST5L~O@6`nrMbF4*W5GO z4u7AU>{Wf>HHJ}ixpSR8A)iP_LfR73EKlN6!!J?em6v=^_|vpI=0I0@W^AbX#CrO0 ztU>k=hhiXz|N6uCF5cA|oel)F1?UFhjM8Hso6_@k7I#z*Lyh!3<(chxJ$kVWE8J6( zX*0iCKUt+KW0SEu?|9gL{g9!Bzw?U0_PX7pW!L~*3-PC~(a;E=BY)7-_)}k^q0yo} zek7=9r@0)1TNyHSc#?aeP?n|OYE#fVA^mhFMkic4kB=rd(;hYT8Z~|5EpTcgk)VG% zcvFOhI51l&DveC8dB1J$&#^%@=j++%$k(km+ITjrHFNFjAGW<7>pQNV`^nSiY?{1* zp@V}-i-wLPfQCWjhW7V=V;N)zUtdZf`uq3)@lPMnM7hl||LgaO+}y-)wF@1sbY;)| z&+DK*=ho5c_`j}>`suhF#>Extk&E4L{=MtpyJ#0~Qvc^!{{GZ11WmL;?EcxJ|NFQX zS1Pgp?@y^2-8xn~-V5IRKMxD9u=d}t{Qt985O_-TKevpQ7{m3?eya2FcHhoQ$=KlT zn5(&D*p2+JCx5sDpG|2h6R0F`u@`0$!y+eOjcNQ(CUa`eV+!F~{q5gvEv(^v{Y-BC|ECxK@vs`f>vtr3={7 zQA z`nA`p4|9IS@(vZ74qZ??`SJd;>F>$TTS9^n*%C92itXNKrfs~t_{Ui$0|mv0s}Heem*_fnj!n~9wnRAvbhU$%B#Pb z(fw`W%x<*&J+JY5n9~c#3#oBJXQZO;{Tj8OqdMH5i(@z|tvqAp5!vEc)%Ap1#Gjq8 zbR+y^Gw!6{tYSs^2`h6RP2pg22x@Q5ckIl64j3FRdwuSFdaiQzCr3gWm%WKrH}meaz?IUe z50SD1uU}`xOn$m+9IfemxMMBCU|myUHc~ERt#rwkV3{OzUF|HFl$^T+CI&sA?cu*@ zJ4U>Bu#~&fqoQ@!BcfULv8YSg##b4>{b?L!2im^2^^n8Wo6{9o>Akgi&+AN928-_L zeD^LD`NN`=p;zU&YvY0UdSEJZcneNLZsVHQQcgu{EB6`?_iE`BN%=0sQvn`1%Omwn z-Yz~`q8h_F-oKedpqI4$Os-{X>XVp7ONp=hvr+H*l0Isa+XRQOYmM6f_;k;7(X#1n zV)Ulx`ODXMu2Bfpv|xt(NqBqHegcBn)S!{#+OJP{zYG=`b2I$$X85qa(HWF6bi6xe zet+0PI6+q#0Bv$t!Zt84*Dxy4w{mZH)X{wYp8rlF$=2HT39Inp*xdq-zemayfE1rb*Ju5QOhkY#%Wz*nVtx`ur|4Jmw<)z7D!Bhs@ zreTUrUeuMKQs{K_yCEgKoU!p4Ann{CmWcb3Y)*NiwT@3G&J(VMtC?C~tIuD$!}Rh- zBJaxM%MtDcv1qY7Sz|&M{#lepDvV|ZCH-03G3UM)ZLy|bk9VFW_>DQPRb{`L`jOM4 z6m`$kH$>|bx8hS~%s*nR{5Su(|8ucBn*V%5XtY&hOE71-nv4 zND%-sV;tidYy$rWm^EFieYPqceI6Z}RxgIp|H*Iyc(`djYI*a)YS~qQaS6WtziaK1 zdm%zi0EtQnsJK*H!fq<0F$)XVe8ns_8PGvc9P8YLQl>Cf-Uf0Gc41}vp~VkGn*O(= znktVrT6*(!8@w6k-6q|!sl^ux66=0j3)ugzr%1JWa<+{CPpY_&MvLY?O)R@@l^p1_Ghl#4kTJT(GT2+?XxoR)7bOxyGy5xw zLao-1y*i@JTc-ZN>Q-mG;M?|gUn0CUyFlNEl$_i++Jy!Ud)H8Sjp?q~{`9v- zbgGMNnw9+-N||BG8n*|#e7TLUeU@?TH6izfJc=w`rHnxAJtZFHCx(^kQrn^`*1M4R z2VqByu&-FS?NWGq61fRTVK}Z7g>R-vlF4y8Y#AaM&vdl%q1AB^qohfV8~VK$KQF4( zB`RmD#U|JTGZA@rz>0N!Ty@En%Qu=?rJtv5WVKj8G@`o_m(C~pNwF*OeVzSeykS536&Mir{&aA*R3wK@ z-TC|d`r#U3uh1mJ+1|G&0K1^{VMq&igaruTsaJdr>X_chml|NUJnN!Yk$NX?-Mf&bT4!Y?=gE!^7bKkpGk9mcJQK0HAEHFCy z2H$4n04Pg2@z$Dd0PhRHC~Mw#fqR%Cju|?Zujg8C=COYf9RN0@F@N}kdqwYhj8%|^ zWfP-vx_t5;sRb5LLrzQr-Ywbixj(?f1|i9+*YC&AZ$4$8sT#9vxp|Gco-3*_T0moMe8RTePGe-=HzD_c4j>!|kC|(KnT+XPVVkNg63NOBaiXLp3FJEIE9-BSBA>1 zGowV~bdB^&rao||I1ENiO}%Wxv1ya>O&!^M_U;V|24pXylixiDf#HNbbe=g}UUO2v zC9DkN<%A~H@z;$TjJcH?zM+uq$Q2bjlGH;10&7>R56$KlSj1UWa*%BM1;Dp-M?51Z zOGh3l!`u^z9<0}#(J0!7KphmLan{?MYYQkn_Mp(TI5`3S{R~dw$k)YtfhKYx%I(^H z{YGNp%x`%IjCFZVAph>RX%>(m+w)Ltyy<#eA!Ic!oe^LRMeuGFap^*3nn6H+hdjQs z+Cs{CPrRF@Ued?xjl4R7bJ>9DV?R~fM)hixbGLwBjgh~W=Pp(y zpXjM`Ss(v)ja_-~j0wlA0X}k(ddBw8VuXG4Z0dm9vv^%t#$jZ(%j3@XN9}YUdyTu` zBtenxDDj))0V;jzU3M?fZy5yoX*w5UtNCt`r8~?t(dZ{qB0(M3GUMI+mQ0iJ>5^Ra zAGr0JJQ@*l7tO1*xd&$mc|Niq7%X^3+CE?o>yS8ijQ|A=}QsRK)2O{u#uL8cS{l9^Mn%5^vsuC?Jz z&=$kpH(2;`s?&i8XD<}(=GamBm#kC$LPAQwqrW?thl$1ej$wPew?rLd?A4|-_!n>0 zc$A6xv3y(lrksfJq^TKOy5ImG8!jC$fB~(IeWkPq9tVfJFY?f71RkH97n%nv= zK}ue@ThxJ#09GI_4-^gv^;HqS!z6)j)RE|sIglU>yx)ahz^*dRCb$UJ4}P-~xA9_c z-E-NH6K9~1SDj-g37FPSemBsVb7_0u>sLy8jJ31%8h>Y!WR?$>#Sc@5DjqH!Pz(q< z2t`a~8V8qPd_U%`LBIDOE~7{SEd|bUVb_x!1N+i6qkM{_XC-EAPA5|V{6O-dZZp}K zZpsNAf%0G(%en(ei30Mt>aNGzDXSMJ5^dtcHEbepsueP&zrKADljAY$WAIkwWz;?v zym1q{om)ET`!R>bGlaSB7;vjV;mO1Loe^6t$4Q^-8^5_Xe{yP^Y_I2aWV!hVAxF?x zaQu^TPQFAai^y<#o1qBJ!X}Bk8x5rY!{-!F&!FNjDY|C0VvYKCLkP2iJfFc9rURPN z?J^(I>w_CJEetN%62BKYb7MDPh13VFb%l`Fi=@qilCaFej=Uc~oV|LlcK&Ou57qNl zMSxcsETtc(8p#;(JRJ5-GXtSmmxGtwzrT}e$W?lrqYzD#1=&I&^bih2-8QQzf<4oh z+@Ult3p4K!EjM!c8OP7xXOX8#LjrI;ky3CKbvDKM+~?O@rrU@dzl%}Zf8S<|<#R4~ zJ~`e^@FU=wr>&UqylhhMOK#J0vm{B9Y8JuO@uwJ2&{%ssteZ*jqTa}=W;*Ns2sF6s z6240)$gAnX47>3(NcniwVd>`Ohn9NHv+6)v5u|e1nh!MpfE@NaCkRIfWmS0+Nq~#DBQ_rTAr6ipjIH2ST)O5Zw<4^|Xx=7}ks3`RhEJfTSEVOS0#R zEEc*_WqwxAGb-urtxwn-ETqn{UQ1oyMZ!PjNsN*8Qt!=$JHv#?8MMW0eK>bTflHy3 zNgmf-;2vp>VcYD8uq|@!hff5aPYLdW@?=`)d7hau(f9WR?~qc$UP_di#9}lAP~PfD z#ZaG?E7ff?&~K!?Fc&XtWh!SUwKi!ZVR7_G8wzZ1RRXMqqr!@^vJ3t@!^5`9=aCyz zVAh_Wx4I#I+)86N?(Ut=w>N&~GUp2^`IK-k@hOoO>pl$wxKzQ;%FuOv>a(a-Ho%DC zvMvaI!o^(p-TdqEz2pTcmlsWgl>kH|mk-@8pqRi%M$B^Sr58rI7ub*zLo(?Yk)9~D zobof#XhP(ZB?d$&H1t4$_2*{tHhZX&h+9Spk?+=q`zTeaYH!jHE9U_Zwk8ELA+@_( zIKp6G-fzc3oehac+M|{BjOF|tQP@D?v8nB}=QnIe?He3+mLum^i2HZ}%zN{m3;Vgm zU*UBqtO9ApWW{~3@_IG@bIh^(fSQgjoQr4`wF69i3j86v*N;Q z(x2Y3DMS@o`S9G^_~_LFEW;Fdue7Si2P*Etz28#|Ip!nfMN3T|PQk1RC;HV7$}4VR z_;j|8?*yI(%)4JgJt~@e7Q-!r)+-93I!b}zP7rlAvylda3NpO4J3>MrmJ7C#mDP8aINZv41u zaSnELT$HPLaqJn<4vXm*mqk2&Y$G?HBr@zud7nv05)$Wffct`|kqhHpgTY8x!Yvf^ z*!j3s&i1LlmiAGUH%TB%z)ae4 zVt4{aJqFp+>{t*b08}^&c-{_x^tHl|ZwdMO)d~#*+l$mO;W$7xzaG!CBllU<$(IFNn-C9FX3p_4^k*!w5QH4s&MC%0ZXU|{ik9AyqnW>U%BZ0^X#rB&o z&aunSQJHpNV(jA*QMXlKpjOyPSQ}AtwZCDEWqEm=ZlooSIvhR6GNnPu+4_I3e0mdiJ(BtEbcOF0 zAbLyAuKQ!ThakxXSNSZ8Ai7%-+imx#bloN2Jbn8xQp4^vx$k%``!f{J3iKX_<>GB0 zKXKz*aT$Bwi%^f6IZRJNn$=D60v;4Ti(hbu8eA0L8tCZ|C}_g`3SvR<-|NO)Kimc6 zamHWD;EAH(4Gle)h)JuuQbkda5cSdZi|#l`{*}^ z7zWvYHW69sP7&J5m1ld= zkvv-(Q0XGw*nfFo|V&ES;GIh{Y2u_=rcuPl8^(l!~&?BqLSV* zP+51&FuqUNf=SmFE@91?Q$|#~;IE#SUKx z`74>9enMVE8;Xs)Rh9HmUHHg)JYmutpfsNsw5zmz-SMRUjZ4HFCu9FP-(sC+;Fqtc;aJVgjRYcVe4>CH1S|3GKpo#IgG ztIt&+5^E}vSN(m2Mj=?er=>5`5L$wUF-q57WUR8r^!D1{qq+yJ$cnY%5UkVO!i9^_ zQ{t}km+#$(>Efh7Et1|tcW4{*Llq*F`CvZ&ZGn?WTAT@7xanH69qK|K6)-Q|Gcg3^ z7AM<&XL;aZk;{d1s5>$;tT(gfKEk&E%_YP{V0DKI)@00-`5bCH-K|I;EF=YY@-l9s z?(Zgpa|G>=giysC^?^%wji!S`^ZoS@e7o@Y$7vz#PsF5g;Hsq$x$c)YffU}qF(fYn ztD>Yw2^B69wn8Xy|Mdmp=@8VFP7e0qe}m243c(6&{aI9~yN3`Pf+X<0pY-|@sR4Bq zgF6kfPNX?8@;&l2w1Xt#fq@ZE-PEe)NuT1p_-_C;UWF1-s?)&ie}?nvp3^~?*gO7v z<9{EPM*^tN=gV)^{_ewne#PHr^vr+#l;e>$Xs8L1aGXNvZbtOBz*Va%x#!RRdp%^o z_rhv`qAzFWR^w%_fkd^nqV`nvH=It2I4awJw8Ot~!F10ZbZfEUp|aO=Isx_X{~&GN zu_}H2|GG2n#~HZS7pF6FH2>>(!@?a&!Yh1rY-#>`P1It-6VbN7Jq`BHCi1}wxvO87 zEw_4^shBR0QwRON*HpMmUVo317+l-AF!Bb%(gboykWdhkJb|Q)=sIP-Z>&){Mc@K) z1fR3RyJ9YHQCcEtH(RVVT+~W8I$)R%6~9-lOBC@daFM=ox@ku;NH-`!q&GvU<;jBy z%oC&Ov^#&j0PLU0FGNBC85sELlmz^{?(RXPs$X768-BcJIvC<4AiB;7fvRQ3>i55K z2VRyR5sB*93eNubcK?6y5!y@~lcD09nOhf~#1*qs_pcHJa-k-gk2?;Gn_xUk3V@=?P|KqxF z?Dt^$eg!_WzRj!=z%1NNg7g5F6@s;3J~jA1%hm>+iYndj^uD7rWjv4j7` z3wJ#zEoJV>{dkL1D1kwnMYjJJfu*^frX3$={0wA(}?y9m-Py$ug{_Jwf=j&B~1{e=qaD z$nO7G8+4oyZ-`8v^;f6$fx!3)P8yLWoP!Zpl}zPdgT(c^~XWHKhWh5DR3kvpBR=c9Hz@GF>fB#IFI2i=|mp7R=)5Y(es1??w+h7VNP zpj;%-AYFW@lyRBZc75d`$T1})qYkW@gG)%)poj0lTIHrmobwLzwa}9hQ9UsqVvf`7L$;%S zhU)Z%+{SRC?Pnu*VClTVw%5L%9GLn%)@obI{c5J=ysh-T6q?LYAn`b&YCz{|DZ;%v zLjL~OZM@-RcL5jmLBT2LNt3I4uL7x#dnQC-8FJW@<~{sVz$=qRlW{a#$7#{fBzFReE}r zcInpI*cbudo_3~sS;65e4?OZLL_>hK@zAK_+KS7y0RDe4)>{mAlbX&25sM&jndsc1 z2pIruyL{Vsuww)?i2d5;ndB8Pshp?cDt!0llXNW4fXknKgQ(lOo?vWh{&Te1MiM}2 zj7FIPT^AjRA5ZGzB_a`$EPvbTdjul&e?LJwZcxsW9Xzda*hZ)$=P_%#Mny5wQlD|S zeM++RC?Euv2iXfxwfg{)kGFR~oNDM^g=H*+ijR@*7kTpu7?iH+6%tbOJkSz%v9wh6>$B(@)D zfbKOES*#nk#+)|aFOaiA4N?FG2{1183zY{2|zzM{EjG%>rp<^Uuk8iHXGr(Z?-iqlH3k>x|JZ=vssCtylfjd{ zDf+H)FTTgiHSnA4#GM>9bhDX*Dnj@xAAC)!c5+|!vVO7Z&fgNs)$=AWseSKCb++_49_{82{Ka&SN-^Z7u+H_kixSn0k=}{+y46?gE z8-EGK8OY+?@WbOfH2azC?d^*lwvD2=tTRR+1*WKP8v5*5P~3~<>4&a^vHdJurN2FQ zhFEFlH{|LDMf@l7A!TpQkl!(Ybzhl{GA{~fp#QwutXc4xn z@F4;hmFsIHG5Lfay}+do{f+feUn&ePjxMgDE86*=76H=EpA)tYy|hNK7q6RN?HBX4f6Dn!1Lt^%X)H^ z2LYRKC9P#8%xC#j7B%Q8Ji0zSkytaLTDBwB`~Iwyaz1u4cf&IZSi?1_?SDN5B}-?i z_5HwOq>f2ZDa=yMuYJ)PP5FBYM4ARo*JJCsd=)dN#KhQb33ObF?P$VYWt=~i&>gV( zL21wr@@vR^yE-D4pkX|Q6BCQ+jely8Szw57f374N5C#ma!s+=oeK^xHuR0l9v^SI0 zgD|U=!JZITY8ikiUW!RJ2ByGT-HEA}X7;P>Uym%8AjMav?>Q5Yew52D$L=8UKai#4 z>3M|H=FwN=cj2b*{w^u+2Lh}u-!Nz@{K_d`WZBhCc0$^kkkpMqS?2Hky{+y0sYxQ+A=n3Q6A)JYq&@oB{gQrV9N7xuR?hqqiizK^I zAeVi&eiG8ArR2kbHkp7I9~RY7n#zW&oY8fwI{8Uzi4v!{?^8*P%6_5xurv$f~%ADS^e8$Dl!6fAQV3f4CzfBkfxik#TCfade8> zoJn<`oqGZIZS8vOif${=ETf+}TRH?L2b!)nU;0kiqwF1!B?rIzkV@Bo*CEe4_kmKA zN|PTkRKx@-4Db6Tvi1pqY4zJG4!CE$s=(y3y3RZ(nS1LX(U+k1D&hdyp6{r$hGyqD z76!|KT`lhQcZza)&1G}3QTKWj;~r^Ty~?%RnQ#Zg{SAn=o1-W)?_NkGJ8{FK_eT)o z^a3vYOoz9d+! zrILSqfn9UCoNXSBRZgy>0%=B3{w6s#MGUYVLuCXQj~U`?115@OI$stp9qK_wIiEe# z`^>xuD9Tk+Q~YUu&|(kGWu6kBGS58#`jJY2+i1v7EFsUR6<_@Dr>yUktnaT)7JPd) zCsfH!Yk7PvfM=u8dvUS;)2-!e_8mRvo>7ZYv49^nxt-2-K%YY4@UFu1r2Bb%v$pq{n#fd*Q6qdC?xkc zX0ieHZ6GKrxCn7Da;aicv3q~rQ$J-GUNtwQ{UYf$8PxT`ZS5zF&1QM~jp-Q*! z^P$d~Xq^>3Ir&|Qr-DS2Bzu(}$0!Ax`o-4+EEcI_OUWT5$>C|2_=s6uoWt|jpz=W+ z=srP{TnH#U6cQlpbFf&wZ@&38wIrEC6yDP^OZ_zVa?8#cxiN4p zL}f57$^ZH6d!dP;0C9*2L1WO(9CvqKt2}Fn zGOr$}3fX0oN#hGfN$O?d0FM;5F?W$N$XgJx{Ov$?k2>I{Q0l%XP1pm<@)zd&GO-); zz6&B$Uy}L=skpikhqeE^$vYAqTwuyr721>m`=PIXsFiRj4aP~F8{hS-EaIu*{&pVM zSlKl_R^Dviep9nGkQ9z@=Ul7^6@_B^Dp=0$7D+uq4)SJce+`N(Nq+bZ8b( zKw}jR>rl>iMv!SuQo`ygi5i(PI3{ze4~%vP^PayoZb+-E1ETTBqJ39%H2DxB^r2ROBsh76&d=L24FO`9+TX&kr!VDxu%T+8T zA;X*MHhi$I>IepV3xpkZ1645u4CXyD(u{-(~a4 z2_xw=M!l?J%c%S2#lW~*s;tISz8ZUUB3;4Ox2`ql*h#Vqo`^W{kq<&-LHCywg36r| z0-jmaPD;tj93W@E^S35fRuC`yqlB|4Cq@;jT=n_7g3B?OH7;~qlOSC1@l~rUGVW^y z+tVHJ5Jzt|)4?g(#ZB|A)MOz`uX{*da0uK?qg>Le+0Wm>x~N?;v5}^ytrX;ps;{!B z6G;S$1-Cgd5}$nZ>)->dsC_c#dQ#OwD*Y%!CD&|&Pm;`t#oQ@H1RGz?>ws@*f=BfM z8*x>)gDFt0TgTjbjYL8(NZo(jC0f6Q-t$v7N#+)#Se)tN`P@2g@_g3Db7Q$${1kzo z4L`x@?dJ>>q85D*9>@PB=n5oacE6Ffk45}5ReuhaUYQ5)suSh~L5rcBN1h#FX7hth z%tG3oNsnv-Xmv?J3&{zk@3KO=b2>o`ad#ppP>)wPH;6mI0 zs9*FKlHwMU4P&5R_yd|ex>VdYg^-5~#%epD>TG-p8}P7V8>yljZ7?!Eu$gzkzn;5U zUJN>m7rxbu88m3PrzBO_%7s*rj7V{iVI$*5!Oo_b3o5KCz>u)= zRRK3QfAtxwQD_tAG+PQ%=8L*1$C9M??nXv^_D5O4#bz=$$-{_i=J^|z0mkTpM5mE=?sQSoC?a#BGrmTHD3Emejrjg0w1I=$Vu9plAqKErm} z;0aWZ7CKG{@7}_@w4NbEbL4bBPWa~RmeFWsS%s$Qv?3-Ua_x^V;YN|8=(9*INhCTB zs*^3|JdmU)nrACFTVJpEgBTwn=Ls)Z1Xh&R*|s=ysIv5h%Dc0kcCl4$$zSHN zvk>ZgjmoQfL^zk$LHM$I$pv#HX4S%SB3KK_;t5diwt=g#T~F#4N=)ztF}$jOzLXeQ z&%2N#2F7ETDMS!M2vM&IC_iEwWK5IdP}Ssv5{C4mr;5FUM>AA&7gu>5 zwk*h_Lv4MLwQVGGhh8FJTHWvxh$Pl0XB{iCtq-6M*YIrx>zFCaq|fQ^Ap(1#fhI)2 z97-JA=@6rN$i9tOQDo-hFcf1ux?uUJho3k03a}zJ{JX)}_`j{D zLH!gkBSkte3(2voMl`8hS0LA0$%e6Ot@_%t^l4D21GxcZU@WY3#_q8Mnu~pTF|pRuU_3 z7iYPt6ahL8nY$alWfjD-{-XMCV4a?;ngx133(4Kd&&;cOs0wvTDbU%%p>|`08*0)! z4Wk?+w=o>{q8`3ZNtcDT&Jx{Cl|9^|_#I4QM%(Tr$?lHQs(2pknVWX5&t z2)xS62oOsLUzaH!t#Yhr-!}}Da|P>9xgw)dG-A-jN%{n}Vg@cJNb?`RV1ndGQ!x|v z44KHV9E}89!FN5n&%0oX{zCfX!MD~2Q}{CL@>u#FJ}l~Y!mhy1_9jbGWXdMIaWwp8 z)#D%|APh=;&3(-7A&qQK{^Ko$XCQ!7TVwh4EWBR#xgNXztNbZ=GRLJMXh(-B3PEiu z2&n;9IVE&m3Ec%4F3b^>W2nfd%QG&GR#l7uEX1m2TR!NqM5b2u9VoHMd7$=HKd};l zaEBK>5+8@GX}K~=LaY?85^=OXQAb)9ZH&AdnIF7ce%>omge@LnLis+If!{#7eNCl* z=5C*Q@Nepx=3G|*oM8``%ozvKfz6J6F#927rO)n&ddNi|%c|3J615 zs58x>i0$s4vsp%~nC$e+6YxT1f$EVn$b%>?v;%-`!O|hW;g-=17EqLxI&H5ficApg zz$gVPbcGQ(QKD5IDwb+VdK|1ysH2p00zCGdB6gcYimmTH9UAniBlumbTNDDq&JaAP z>0yvtYK~9y^X!a!!(lpjrz;a$U3Mc~MbCepale8DrEt_QdH5K3|HY@lP<${Spg^4X z<`G?xgY0H=-6v{VCG@Ay*PFitFd~T|(=)4idr=VOkZxqe$QPPKYCVNgCqa6dib1te zFmtq$(vXqUtn_4tA8Pg7kXs}}7Bv1UEU2VVfFrzMG;Q9CUzb#(SHN9a2pveW(pi`# zY2%G&4(U9{VKpy{h|E&dJ8!0;OezK&aDQ^RrV-c%?m@2t{zm?Nx7o_P+>(=kvsWoB}|5lJbj;I<(8aEWVk4O z2V6_u1b%(yVB%2KdE{#o1a|`K-e*bSau7Wvc=!;Ry$tEjmh43J0ML05fyf)RB*JEH zU6__O3>tV_KdW@|TE65c zo3NOWndYqlpS3~U9Uup3(AePHj8&UHg|4ow5QfnZrI~EFMQKDy09a9};ywNNDtgp5 z!2>1vE8dH*Yid;m$0!4t&jQ|uZ+j+X-`Lp;V(m!+8O9;Dr7UKeQPvf|4%?x9~PE`7~ z4{G;h6SK2WEEGfmhTE*s3_>j@LVF8rM*w~?$fWF;#)iuk+u>I>jdWgQ&%bwrBEBH6 zk|h2-xP$%};k+1ZR!V9Nzw^NN*w3Ny&GZ;|+Bx?!BAk8h=<8Ye1K zsNjS^TM%q}dXWFVWJb};rB%{bb$@%rkIKyNKqQ?zhHc*a)5!7Dp&+i3q zS_JP;@O&96q)6TNV1A*tWsx$xovx5NtB?#iyBEawC78BU>+V)ZrUu<#r`x{Dik7r* z_A@T@8cWSP0EEAP(^wxhN})-+4oOuF^de;5m(H4uDn$hf%`BLz(3B+wq;$te46N&C zZ}Gz8s$*;@MuX&h`@OhFM9U^yn{|pv_K;pyLo-EqzRRz>eM!j<$KWN?(plI%9rC{IAjRI-4D$JZPKf)}el~OOceaRQ}FX zCJ=AMb|dI6!0w-`c{VF*2IEbv8p#sm6cQIr^On9z^7hqbZoTmJ+?XST5u#n$3#NIj zuCETZ7GB=&4`%do>y8p}ujI`+`I|;lh&DRQ zYIi-%K{gay==tFSiAQgy*sIcL=n*M~$n>4PMF`DPH@?kk&_$gvyDrc zLzxp~p!;@~Zy%T9~xXm}H_kANTAMC;1k!a{2)Isn<4= z{%D4dlb_9{7w$Og2n_@&@~O}+Hg-oA5n|kptMhb-1bU@GAd>#Oo9hd9^JVwX5aJ?0};c#jGdnnhN!`W#*$$K6`B@w|!9s%PU z>o|bW0VbG6Kq-FXHuit$X?Rxuu3JcYbaQ+ORBfbgNvNrORK(TA+eAjW$TC^{yw216 zxsswdxsMXQjFMXyokI;W;8|VkS3`DzC{86VGjTiJ4R0i zLa?5ChC!)i?pt{%h%VnX3IQ74M!w<`_9TX@%%}qFx0cR7ben({NZ_mJrg=Pr{?caA zw49dR46O@rk{zI}sVj7n`wJUtEnU<%p%45z)Elu=P8MBvbX(8Zm=K9q&QcA`^>Qkh zdx5g7(#Om0t18WG+h@F$rMiMWq?FAU0U*W32J@E&e9<6a; zJ|Bp`#|1dVM+*-MDY8eAh*K3?q@~j!O}@}(%-L2!PnJuhenHcIhI|O}sZiJAp-B33 zq<(aTh5a+8tBeC)_4te>WGKKW z@Wmunnk#v#DAoct3oB&S3_&MlSIs-0i)>d@{=8YF9$hLcwp7Eg4KNk*C-1qRy zsRi?i)rnvyu~)7O4KA6c;ViYVf4wsCDmBB2YxMk2boZY~x;mnchXyx@Fkl&gRPd=D z-#+|yC6W`M@bay{K}l{K%9<asQ=ik0Bj8H4=s z*gEUaiI1x6#RDQbv3GvEx*$Nxe@K4sDZisx8O}2MlP2qNU>{&sO!>X7@)G1cK zuwFX1cR6(CFcz_JmPB9)(K_O!RfSvD^enQfIczV^_)iq*`}L)7Kpt$Jqs)adu3C$3 zP8APtE|@!R&L^NCXO|x>r%t+b$MEBN1Y?=3{Sr2fQ(jBHe@;$LyL2-74b{9Fn4O6a zlA20)1Oa}jQVO;MDpnHN?f~Bo4ftw{{S+lHKfttP1uBI!Co3g;Dhbxa8>G%NUFCMy z-U@s`4CWp04F-tTFQL!4W?nM0iyMa7zEaIOm;x?>!%6;3pLdvv;+8t(99=JJ2SXv< zMM3GamR5GfLfhng4-uKTLBg>10Y#H`pKufbNutP-{GD1DWe>U?n)*YQX2xnR&;bvv z86bg@F}{$>8UvR-gxLcb6Xl+b&|O~s(pC&QREmh0^KM$r%hCpohpX^?0VNBx=up2L zL18kYb=YJ?`!N&~=>$;UL7KiF|8RIWfTtIkA6|i^Rj(P2hs_-Q2j!T11-_qpeDi?2 zkG%+}mO+nb-pvFzNFRBFLIV9Zk4(5BuY+pv1a*tHN`Ey4YaNAG0R81HAtaUCxz|9! zIFGMy&FBL~llzzIFJvh>T=Wa`OF3(AkT24pZ-8XrYFoZ=ixPP}VR$@#$0iBn@uusk z`JC+Dt>UdIYpE#-vZsG{un6koiEZR$&-J19uGLSCvEe8R4&JSXdF$Vvj}O-LzkB9B zo>*%x3F+fij&<%N7vFSS?8p%gi=ST?5!prwU6EI5*UIUm5*hm~73j7PV#SLvGn>U| z%s>c^Nss;X8~EWJ#Jt5|k^;=S|eba@~GwvA@T(!>i0tweSN!_-{0Wj@ZnoczuMYQB#snATbu@>IMY1ngf&1MAQ)s`%b?h3h&5B4-!2!QxU*pOPLnWL9xgi^i5}HKhXU zWkz4q9#NwR08v@#QWhl!<<)puKFZ||kr5DE=l%6P;;u_}i>Xk1KN9J2UxSG8ZfOCV z0fX3wpK0Gl;hTKsn&b$a!3(<(6q-FuGUB(sq!=u~jE0;i^jwm5^hF@d2EMbtjK1w$ z(;tO(dKn(~e(GksTOM!-3O~|(`2tl|FAUqnWu;$)Tqw>7%W>z70T<3R3;N7+8vE(M z9E)d#!&Y6=_Y7L2lRnGEtpWYIt~30Grxnf3gw@#8OWS*Pho%M9g>9*z6tU7prz>qKN(1u>gQoX%Z)gv_IUsr~NJ`d$N# z>zyh@cS0x)xr%zAfBHq3;Snc==Il$1 zUw4Qal`y`=P6BaT8?DlK;>dz;^D7tU_XrY?A2-h~h&eB>$sRy*`WgQ0(+jEy0$?)C=)51lFEZyWuETM6l~AVQ zxhi|E&|;w4+41K-@=X&C-nhQC*!*DRaq@JM2+@~Ct;zab^dcPvC0lYt7L1{C-s-^0 zQQ6kqI$ZC2Y7nE8sBR_lWx-KytbQV~dIJpkc}TkHJ9>Hr^GGN@Yfx!ESw|9d>kwn< zjtnJMg^S4U3B;FTPM>!HrI_AKB_S40(-?1Ql2FL~TuQSGF5li@CD62R z%Y=?7F5~M$A}4Az#o!|9buW5Ez{%^_5CrP`+~F&zl$UGIu@{00((>WKq_>W{Vn`TE zzgNi(rB@2hge;5F2}j}wIP>k8?gDZ+?<@y<996Nxx8#|mq0JOSoSRV(-^@ZcGYSTe zLZ;UiHzA&cF}p`aJ#%YY>lphLctUSBPydol9*#F;BOmJ+b`bF!tnly?ntz!0GvWT} zfcQsrjE%I+))x43oy8Y%Fb2xZkxd&YNOdRUaBD#!{u_-sb90djx&5yyy{4e{8RY97 z4p$ZB`Z-%4FpSddjI z{~f99Ryk(cL7EmYBSBQ$7|UknoJQ$VCfg^3So!#KdLKLMet_^o^*pbE`)YfgkLCHd ziEa7F%qQjIEm1t2wD-iw4%Dz?*49lc$!UrX!Q96Rl&d&5Z-@11{5VFw!2l$(uZNm9 zGaBhi?0)O0B$>6x3nmO%kB{c-iYbiQi(t^EGZOuYPQoaI*0GY&b-k^u>$1xBY|yT< zQo2E&k-$G~ml*twSs|rVrBE?jt!FzSM9y_S!OPPnyO)$@HE9g#tA52{k{6>kwMR=# zhS;(J*;@?=r+l%YJwf839ked%ls~FyO}KAZVd{get;8_bu{lJ4oSw84T!FYFH#qK^ z0b4HcaVrmt@5YO zca0JhQ=d?APrM_IR>=Xy`c=aW1}tS1%7IK_v6vN=JBps}S8yyq6 zg+TLe&RW5@9#pn0oui<-VR+@cBj&WbZ_a^MvB}duDl|mc99O|$q3uf$_L9w1bnf${ zxo14WWkSGjk@Ku12btQ#qE;3ABLD8w;7^g=;1h78{Xbm&bySsW_dO27W}|>ew{%EI ziFCQ??vQR&Qlv{{lhPqwQc6e|AR-M0AQA$CNDBf=haign*6n%D`F!7Z48}8_KaRWZ z>sm3_oO4-m)=qU-q2$s-Q6odzefO~BC#tNrfvwC@`{^ru7GrMSFx?g8SGzq7rH_CJ zY4prDn>G**t!&nV+sjY8_p!0n?(A@s{Kq*Pu(~4?L^vjg))e4 zb%i<&+Bw2(wxZ!AjK&JgJIuHnHyXO?Vzq)|_S>*X;M~51OI<&zRm%QwH!%eosi2RJ zecjP#bWRs8#D37U-2j3HcJ2)X{Tf7S55m@Ec29^idJ+x6Z%J9?r?fKSt zz{CtXoa$j?Wzq7;;O|2QcDf-o_p4&dZG!2tS4n?BXY3DE!)FlKK1G@w6UStewq{l< zk>8Of>n7@&Vt8)n6#!qoYvc_643n9ThxDF0!r9W)xp!r zMRJ}@+SJ7viz921`5ueF#ezZDo2EeJYpB%{_=RX(CGI7D2Q;YM!}EN<4N7IV zfz~;fn$P$Sfvqu5nPNz{iiwDctfsvLe)QDa?THJm@h3pU^QeANRS-zcV<;8^SznzL z_hhEej+B^iI zf4QbKPgdXg1CM($2zxr?&QFxd@*e8Pk;{k_{ativqC?$Jq$_*9UBtjK?8@`S0`F9G z#lA8IK?S2W>Vma$s7r*pW= zO}wPK(JK+9U$>=QuDHWiZK3$rd# z3%WMnA9kiL8QUrgDW~;_Il5vSymo(c!6?%svXqKGj(Eb((`;Xz2un&zYFr=4wCY?| zxz);C6k#n9;im7|(M|o-&$`QxhZwP8Nt|RD*GMfX{-p%xnkOf`d;Y+@##b>~4cg!H ziw@(cYEXBS5KuGQ>Q6Q#wgVGL&i4-__YI=E)QvH5hE_}EapI_tpT@|CrIbw7G|b4;*6iiKm|c4&;<{m?N!{0y4@z9#d|2Ql+#2)@hzHcSH zuSb}{$Y?zG^wPD##Oqu9#9vR>%P*O=C2?#gq?obv^b zz)IzT1K;cYbGTcD9GtdgKr&y#pR!xRXFl3P;XhrB3Bcd=-!-l}LI8|2OB?5H*;J!| zwT8BcTgKlV-#bY{GY=QMuI6VvB2EnC8Xc|MS?)_&?ju}!yNpk=xR`F#pz}N#8}{_( zR^OL1RDNV_+>d|T{z8!+(_6X5tz1>v|!@Y9Uj;XBo+qw~5AxH9t*jXUw2O zDVguBilcR(@1kBM?*cs~F^btY^3*TLj=QL$7p<6uB;^XAqB@<$Vn#|^-Rhdp>%~~_ zJbKWG_!L?K8gQ z+|<>`jO|-lgTb~t`bBYt_Ji(h#l9uLiFo}ham@aTc;h4EklO9g?}1W3x`q<&g1L#% z@MT%cXZ+XpiK(IoM*}=+wrfg+(S6qQulMvM0-8%kNi~1!cd1DeWtx4lM{EskI z)T!^pt34BUR__73hFpf)6(fO)iXI%CK1|9SX-Rj4H~cX@i7lpU9tNW~)1s8!Z} zxcvCo;oCLWG8(Fb>*Khlr4*8%ePitu49?_5c)yEOt^BQ{j9BQv(1ysmnt>WSmZR)> za3F@eRRG3%0bDkY20zQ+gxIcj$+R-r#!)-rz=%J5%Jr0C=S8@PHc2$=4I zOM6m7vE7jGtH4T@jqyKCRId9IV|&yJdI^Z4X3JF${y0u0^_Z^mR?OT;E6DH5b$L}| zFcgGVK*9YnJKBGZR2EGmsa3OmWY-=dJC3T@2*Sma-JDDR{ejB6{#yb&HuCA6$C>w1 zmYPlVaD|K&r$65sWkbOkE877to?5JrU@Dk!=vxX1%IN-tqIp5RE|j zmfx0*8WcZ%5mITt%bC^wbr}WBLC^LRqQFMEXr6f&(lzzK){1ARoQV*c`R3!-f2@nQ zgQ9c!28@cjfpYV@)u>{w+KZ{T&6~OgZzG$c0Q(kU{#MH))|)H&`XyQQZQm`h@ya1n ze)a~k=epwe9-k*jF5C8qHhmcHRa&aKgAiyCuf|OpzbFFPvGc(#TlGeKsfv0DUp07m zYT7D0mrR5_b8Z=|ECv&Rt}&Ya8};?aG>@r*GVo4?48}w}>~1n-h`i_cSXzHIM0v#S ztAB)37RDFxC*D}D?hjGfZU}Ma9_Fw|FNks?f!}?qH-)mWq zP@_>ctYpHIdu`W18;~u5`1yXNer3sgM5ft$iK11|AmtW6myF+6s*Bn8f}gyZ$)K{) zH^XgKKD^?HKmiVDlvEc!&C-C=VeHEWy)6^IPIG-K@@Rq47TU*G&s`1+ZBNz#;YslY=3Nd4OpDbc(Ay2%p{)kn(bD8sJRcoDpgAw?Y}|1@=u*#K#9d&BV_4ZxBW)Uf zqlio7OiO^?YM3?Ek^9Bv*6fC@=G}!z_N^E3eAH?gmn$6n71G|^0zBmveSC-m89}{* zzA$^WX*b1bSOl{+l4E@Ks?&jFAu)&fac9oA~(9odYSZ8zI%f zUJu8xUTns*=UU1m zdD=R9RE9VZ5KWc(<@F(+u=7Hv02y__Xf0)d*Uj))n5L8&_|ODln^u>sJQ|7Ka_BnA z*I|c=n2P`90$>!-#`b2+u|VBYv7#V|Yr;Q6<0SSR=9`~ZN6M7kQq?rSsUY~h(-~a) z6&U&JMZJK@c;DeucXPc5`^HQetIq=8bCL1gKHz=2S*43xF(!j%hrjH-@;0CGCKn=}XzDb0S7YDN|y#kZR^~OA{yCX|Cv#%MM6pT}f zSb^eG?^-y|OPe3^{W~W2TneAPbxXk%SE89iw-t;CES|3eaoQi|xStT8Nf^)zeN8QT zVe{=wJ;z9~0-n)tZUo6gaI05^_C)N>tdog9HuA2_foB z2aWg#Mh54iGh*ICFM`aO-)3o6`%E-u~*C|^PYV}$17opvGL z;RL6maK*)CV}gD-$v2R$%46n**j|4LE?O4R|Mi?JzpHjDw-p?B1$}=buHhgk%1x|+ z0YB?Ke^8D1m`Fk~yfO891}tB{0~|F4+|Wy{6c2_(u}Clf9WKa38>^LI`;mjuLy3-F zm;rEPm3G_%acPgxTSq29082^w_4Ihz0aocfL2)84x!0%wa}@Z;r-X_;qkA0}ZlN19P-u^%2=$Q1lym ztV`+i!x3ixAZEe^-QSzMC@}QejHe5P0$I?3ypJD?#XGIqaLeT8^y=^rW49ytZKn{Q zIH-Uwt@!{y^g*zu#BYF9YvF2_G#gJSX0DSHyo7ZG%qgjAVTcsd^3U%RYut;}wF%si z3F;q~R+k14wW$5lz`U9)^_hsg0B~7?9M}`Kn;!w7uS31qqf&(fJd4A341d|pwy3Wh z%+@nj9v5~VRd@~othr)H7P$+V>=Q6?3SJP>OR=F`-&N4uQuqk%<1W)xE>V3No2ZeZ z==?9_nS?2ay{a#b%Dk8R>%wP%(!LJE|A1PN(hI|}DJ885XPk1ScQa_`>g5xv{WCWsB?<#`9jd0KPp(g>@!~g(ccpCVIFN|rG>>-Au z*o`^__%L$zpx^Gt-V<@w%&KMZb0kmYma_ui&~L6Bl^%A!4<#;>CDsW)?5PCLgClzv z#Fb!M3acg9{1W&$6~lbgxGNQ7T=ky!TE8B9635H7x)RdP3XVN|@zc7VM!@3eR$L7! zN&{?Yii``^ShSnXzkd5Yh?L1}KU+ z3Vu@yJKiKH0OXxqY8@W4SoBFDFkN2J5AGfEcymr)pU9SzvMZVsM~E3HbcWDF(|iSk zG;``ymG?^X;9f2gF3i({#VpuL+P5kK) z&sA-B8EA5$CEqd~hbJIZ(Qb{1K2 zaA=mh@sqK!?7NBWr1D3du)*V!C1|dvOTa+z#<9M^IjP16BlgIykj2?I_~7Xcld}A3 z;tTlfJk`&RVcu#1QuK6*w9{$`W!TETt(fNq;+MueMwN+V-CrqxU2VX(yo5#6<$YA- zHgSi7Q{~$pW@ZLk7y-b&-4dxW-_#L`O)ugF$@@G_{j~v@NBv}u3hw>`_sO#`p20Zy z0t%~)IJZTmr_>qbOd^)eiYuAizmRP&z<;@lWMJ$=W1cqRUodT)rq>c#JWzO;+5r$k zznX7rGgWl^g;<$syLymw=+doErKNGG1^YM-6CvbLHgxxBj_>dZkdC`9^q`IPFeSSS z8?W2{{0?#b>11d7b)x5#h+vV3?D;&SV_g&^L7 z;WChlZ461RS?76C&T{w9_Je}7y%(`|c&`xCn&@Lveh=}g`3kP}>jw+~c>O#8cJ{Sn zR?{6}>L&Oi#;OyUwRcI+i4O!_9qSO~K}cVa_>J&s=mtJC(v%=K`a%Gd2LxX|wX*axBFF}st@Dp~eHK&|2PQD| z>*TSgQ&13qVg34gN086VK`mn4aR`C>wbj-GoqYG{=QjsG+km(gAw~t}#O)30k2J2j zZo7VaVDq%u>Crbkp4RM}V2}|y^If2-<9+V&lQG2we;;Qvlb5n$Rx;70ckAK$j4Rx=uGOneCceUHigEwJ#Y zQuM4zJxV$!F$IdSJIXyiWXZ(|N{oGe4m+y`K zk?-)?{25lXGJ8mW@7qDJO^0ofPy5`r;M=eeYvnksz9bt&?2>()tMEh%h#`ohy4ot*b-2mMEL3kgLgAoT5&d{_J;-Q)3GzO z)KoIR;HEd)E+|KN%rgG0Q0UR5-yV?f|K-imkgK>hP~qRLF1ZN-iQ z$f3NteESQ6T-aR!KNWWu?xc8PTja!XLl`ICvzqVx z%!wH6UO6+#mdMO@2;}nsu2?1bZnf#0}8qmPlPE&Cguf{9I31Wf7+=~>~Is2239Y;*qHJ=vBg_!0O z9#GO@DY>@Ab7OQ{bZFiWT`DvA(XaaaygzJU`N_taWCZrOyGTZ10I^K6_q>u|v zMf@%zgtwnB!Q&8tbxsE_rJy}>y(r#5sPOv`UX`C^5QbK_#Mrw8R(smP03Zmef|GLB z`nS48vM!UIy+v{@qHqaqRNpgDu;N-$V6di5Fq~JJr!pMS!Z`>4bx9@fojS?uR}uoK zG<^w+BQDS0_J-{UM}Mf`A8=rmHF;<$1CNYzB09m{7^!T&#{B}sY3$j}-vsTxP#A^< zfnV0RuyT}P&&bn=^|_9@VN=o#J^h-YIJTY51H~wOLyl<}jnd|dx};NQ-IY$qA(Sk@ zDeVDA!!kq63)=Pxo-f?to1ALNj3ZPkXQ>&9^-HyIVnx93*zgR)=axC$M)&RJVm(R$ z6J`+)jkTfeD!bEmqg9` zK$(Ssdv#^SZl&oBRfQ3`Az49XGRcqjYAGL}&Sl7Q17GN?R`Oq!QJk2+7c4NKlTapp zMB$g5=%ctF4}=|81`DfuNVnFrv!XoK&_!`bo$&` zAnc1J1YaD%Q?b~bZF~5x@X|G14WMgj`!19t9|?mR1XFDdX7bxbwY>!j?Ea?gQwo2= z0(KTv4w#1PBJj==5uz306tOl0dJGW2c~u&2p=_SxF^&zZUOhvq#>^55_F4&-gqSOu zpaL>hs@2D-2lK%9V3~h_zHA!7v)+0sQ?W+fEFP z?0IhAbLuDEb#E>GhCn#J>5@BttSBWn;iO>y5D3c_=|yl6&ZB8A7uEWL0JKhe4@SzF z&n;r+mzXbm9bLLQP-%KSmLuh^=P!7lAdY@?K7J9~&&ojFWL)%r=Jru6yp4*Jv5Nhm-$t z4qE11@LB@HX=4%=d43Mn#8hYpHLH!QLM`Hrj-a%&s#Q&~+6C%Em6WkpwLzC0g$Nim z*V}_Kn>ntEU*duMF`Gs@ry-PDZ$Dz9H6Xl%qiZzmfRuEKn`z{htVhbGMX>55+Ypnr zGY{vyFUohS@HcvLGHgi=1iN}LzvBVVF4}_bCYMoF=<>S)Yumk-;~AvqT7!HV`Q*BH zN0d(B=8)umh`>MN1f9o}BtR;Uq$dD_qS>AFr<;cX+V}v5301=ujl4Vjm8ukjKr*IE z+IJOGanWKsGl*>G&Zu7-EB-Dx#=}-1qqOiJ1j8;8b9wq@1mu_t&mk06QES|!NFkFf zsN_iD6I8GlZ8)biuRLRLlrzxAuCX ziD>wFdTlK`!rJDYPGDD>E6F%0ez?GyJ#N&h0jsd}!QoYW755HIU5sxJw+qPU0+}f9 zc74G2d{Gy6WU@3fe9B`8F3Z_NNYs~C9|v*}rs5!fXf2$%kLuYzXDqtx_n7m4XY^9y zxRvv*M*H1nA1b_*laF&|Bsb(%rF8~DakXsNbn+q?3gWAbko7Z9Y)X629t3S2r7OIB z2K^LW14!N4@~+iqgW~2b$fZl23J)GM1BiPc+MN$Z1q^rpzNJ|a4x|6llh08zGc8Oc zU@JFcP9>oEFd-|1OEV)C%HfAhk=ueX7AjCcCa0fTC{&%h$kWQS z3W6A%_D2EhlWbs8oCbq`r01gC5(aDfi7;C*PCh6Cwzb=z@4(aSg%W5?h@<}xfvEeW zeyt1+b~NnZtH~l8auK!2dBE80*czAD`bER~Y}+TpV$%?)6F)pQT>%(s&A7 zMz7}yqh0ohsal!%^g8QQ$<{1xXO**~Z85*cRB zOHVGup+ zvBH#{8b3h^&}aZUgZng`r7v}Gf_{7UW$@mW1@z;k%HM#8K|Np%*9oF39(@ae!INmF z5>1#IehKb#z-#Hzejf58FtS?${+jRr*kO+Hyz_kL0tN&^pniIvq4p>EONhCTK$RHs z&Xbuqe6PWz)aVlFpCl?NO((({%O=4mlwQ6Rj|iXyqcP{Hb~%gc^%*_mJqreeE@zh= z%a>ugY?spv%-@UeqedY^BexARjTW?RcR%8qN;yj>9Nx>WJP(BZ42aY&Wp5^$xfS`m z%qWDdW1v;AFx)E+s|(zu)^v2}jIL)*0Mn zeoyziMX-A9ebMQ$h6H%lfofI}^_>-h2=h1f3l7?LP#-)AI|5XSLHVm|9Q7r*m zS-@s*D2_nf>p->j(XF(n2T$0X=mBZDqP# zi2pl8&@}aFmCpPS02KPGy3htCiV}KE0d{YE4o55mAtnr;TAE4bgs+hHR=2E214m~4 zShfjp7g0{3`|GbwQWTFeTmCMXSwyQQ(y<-)Ke)aj4hob6OKO>eo%`B< zW5hq7UJh4TQeVBZ500`-(1A-xFIu0;Mkw+$DNRh^&lJCf?&zskux^B=$m(b)`O5#E z_}}L)8HrQqf34BJ#CQp2G>uD9)| zvo7FJBl+Vg`q2b7z^1`r%GJJ5?Wls1b!l3xHdx7_2=9>xznMr9*bJ8eNXJWKp@%w9 z#QN(j0|~qzLhPCGZFJG@~vO=L3@%xKE85RG7RE#h@h|@|* zhmu)1it`nE_v${T&~O5f6TnZ(p!@&x`*D()Lq(awZ=3ANfnt*O6_#8Vu7xd6;!jc^ zAlLH$`O8Vd(VKerjD74)1{y&`gkqVZ$KZXMtY89{3Hzf?1zg%&U1d@k2PtvXn+zHlR`T4XxN!qXsuj{b@_Qlbzh$CW7`J^}qOQpP2@0->7A#W0KV(um_#UX#@Z zOV{Q~_Xa{@MoE3$d-An9mSD{60@Z87_@kLnFg0u_)DgXdJTwf>tsI0r8{ReTFMp16 z{xTL4Ngou9HGjA8{<$GOl8KZ*P%Ws9Rzeg!|310eED3(ThqZJENqmT%`}MsNy;Wc{ zluCCnakqq>ycj5wv%G}tD`_apZHR+LwF|9(`o8bldj>3Ie3_h6I^5V!0w7Pl@=}8F z9NxLk@+%1Ncz!(5)f|gFni>kfL_#bGkNP0WH8#V)5{jeXijNcY&+~VwLfOHQ)a`0V@8R4)C z=>=Vqt9~c|B{Jd4yItjkzxPjHQbSaibucm$+Oa8=4f=c3iLtG)x8-l$tof5msrSlFG8qxJ@aY6 z?j?lqf#OqsNKb>kLzyG)kyrfcu4O7i1E9HghTamwDHu?&gTrx3Wjk2|bP~v?%wpVn z%7hyw#4PF6X_sxymzC%I81g1B=y*Sj_2piwFgGq^{`=`nkw~5nK6`YpwF`3Ut?i=+ zy4=+;bB(88-`C0i@I)L={FTu**n2YYu4!E}Pw8e@NRA^@Wi%myFHGNxUxBKC*Gq(V zfx~NaTFOJXb@+*Q=+UG%Cp^|Y^5KpKC7E(71Ne}O=Zl3<1+Wx02xzNu321c;8>XT# z1(%QqeO|YK2TF+c*QiWQgVbaSa}?OnC9R_w3z0MKppK&(YXf#2g4}vVi6yE9dFTNh z%stgBrOJdF;BwYX{lK4-j0-xNGIZW(PJSmyZrt>}VB6Xk)10Y)B1TRT&QvT7AQhu~ zApJ9h>(uqf5MH~L5SWtrX7K+)$%;n@-*oSwhe?g+WLm+m`#q>4-uT3!!=cG??I}Iv z``gy6>&}DIS|-@Mx?No?Zv(>ue~J9>X_!tlQj4xO%xA$S9DLbRH&}67AoF=nWZt(9 zqwLI%vIM+{x7WEM>K@MSM~G4-XRV;Y+t%wQN4m96r@ymRaOP}VdYi2`x(*VXCm!RtW&iV}fKs&dTH$3UqkYe)ValuC)cO-5@tbjw-eeK*|HqKMh7jBpyoT@ z1Om=Az*3lWZr^H1a!x8;yvPNG?}H<$AbvvaJW&m}LG@Ls03p$XnL5A8lY{9Zxv7Jb z1P>3%Bn~8KBY$_Y!>?6DLknw>Hwk(>(G#G^aY9%7s6ozH{yQXv=Sv zX$=o@8^07?bX3*epD!;VpX|8Z0luUr@N-g5VOn*2&`tkx0W3Mf&+?zhC<1-rK2$fH z0Jao!Acr>cGr=#INbiE}AN{&bYv5x}y`t%x{H4yqyD#>oHcUxF5@;j_l>nF#LKa@V zCw>9Z1le|Hb4t3Cfy(3{1IV|%xLIn0%0tgu05*u9c4Ev$^J_v&phBDMdRh-@oOfX) z>4pbnX(KO`VjLqD%F=jw+oa6T9B=YCAhqRWJLiP%s4+ zo%{rd8DKz3WpppQCjlj8c<>G$KmXW#!QW@ENQfP`g1mTivw=F`Q5yiP*`Opd;TV4A zVcA6@#RW}Fjai9K#D`#jy_0{H7hwM$a?N6B^YgA&K1G(9{s0SY?P`H>Z^|Q>8noJ# z>*QyJM{qv!T2SmM1fuh^C76Ntjtuz0SI8c?PuBQY7OO0%!hZw*EGrgGfFLu_;gwrw z!q8KJ^c@wiVX8G10n!ZZ!liq7iOm*S!efw=X@rzI;X#D(Unm2XQ^R`%7)6|X(?n~Y zjMh_NO1Pckkk(_NHfRmJDdjaE{xm#lWQu}Iv$req^f>v*TPxceJc*;qw<@6{MMT~W zL1zG%2qe9qis3=0$9P}b;=+AM24zT#!Z^S|Blanai))|0L08Wcf60drxqZUixXP>x zQ721%cdH}7@5s3DI}rPFjqFdmM!)<0yr33{xTZ*Na@vqErB5r1e}vIL(F~BwZDRKb z16c_cP=Pq3YAYkg358{6zg|#3q2xrybZ=pBaCSlm^d?Yr8Nrs z4@LtL@66T|^m#&9_~fDDl0u6+#Ml6v%c?8RWvr47)8=+1BI0Y`^I!OpQ)K6jIud+; zPq@yu!)7=ugW?PnCXjRjXQ*}lB~Q#FnR3$_hqfS^kasf@d z921y+Zst|bRgKVrE43C}m9a)JYFg#H+Vk-g2_p~GG^6~E`4Z;@MRbT{aElsZnRri& zQewf4iD3)|MM=41Pum1nhVDsBR@UdZhkSNnMXHx0g<`0Y-a~!ZlSy!QYo?`%C15XI zpU^H6UGv#{5>^N+&|h?jLaGMeNu}tMKZip(7nRNt;;+8&-(YxDE~iy6Y8@G%^{hMg-&V}~SMIB5S&g>p*;w{i_UN}- zU`Qy53}0vmNVR+?%O0~9L_#vKgMtB1_4TbTTFR`&vu692x)Rg~9zxxJ4& zH=y{-gh5u0x!>s8vzwn)72e^LzdnIgXz~X>J|{3OUP_Xf6)rsMZ*(0(YMurV!ct}i zj5a{L*&?t}YVeq=bNgs=cV(TNHT)NkaZ(MrQ_a#g&idp9?bafdD;o*oo>O{DdLx(p2EXtKz>mBdF@$v z(yUR`3N#Y8Kb!}Dcl-VGDT~^0q(59$>em8$sZvRqiYFJa%Kh2hx5p)UIPLIIK;2I% z*UHwNw*Ls+nY+Lr)PmyN-L9}v8#6Z!`k_iqMP~PVP#l&4M)1dUDVRP-=Q&E8B(3`Tjz zGEI!tT4s}k>}O*yL7i?^fYaRwi)JfgUU95U2@o8_;KdK$_~7;jhZ#TsUp!DNiZq zMl>mg9t>ToB8LdhLESIeWBwU?0^#@xXh3z+V3B)2TwjNJIx$w<1W1&gkMYZ1v=gp>Y8oQ*k6!cvPfyM| zI&#cdq>cVj>_1&F83S%;Rw)5fcdJ1SNl>?E*X^F0YBvI8h~o2+jRw0AO^CiuyyWtl z1fvh$U$YnA7$FMi^Mnp%Jr=9wA`2D;u9@+lKM0 z7F1L^rLZjrsR_SPVv@xbKZhKAbrwD~-b_y6Ss{A}lPpbGs#*?-+BHfclqBT$uODlV z@>h3Mlh{sP8^%-dmIJ-w{QK1E)Nnt7?eZCEHx-@Ftiw;Q=^?rE`zLS?Ch)0rJbVsB z6e~c+Yk4uTa{g{zxlqll>)=#;g@Y|pP}if;t+)36zSw7EoD-8^xx60)5!+S$02H-L zCQ@@ARMjcpEM1nt-`peZjW7S6?A~fv@r@nuJ#^!>dj2Sgs{A~s9UIF1$JLTorPUDC z7`u6Wo5;F6p;4QEZsefE3WGq`Mn?j^9snz{O1vJM6CoJEJQ)f^VOqlXX58&R6YL#k z0K0%7Oy6uUySURGAcbYnb{U;{x44w$po@YJmkKmfxBimXWG|Q{OAanrw&(8UHoPze zOz4_b|6C&ksQ8B4G2(?#{c6CW>@-k3%CRg>tYfW!`^02jN0c`6ZxSOu_yf?{0D=zVqQFJMOV#Ry zi)`Wkp~10C=ahACPGX&b8+b}OFqxN=j0Ca)Sl&1)7vi*&dF9R16l|Av^K<#5Q;n*! z0aSRYOFyXnH#Zbhbh?On$B}0x%R26hrZhSY7MRam7XpJ%1!~DaAWp|O1{YjLqhJ;) zGQF`B@RVI8o{mT{COoq#9dh*#VzaxQ(1VJ-y347-zB;EF$Ar727Q9*hRMCC?L*%8u zkQFy>sP5-$K<0e{^+67fW`X2mz^@_~HZPX2{Llb~4xW+(OMyrLblkg?KBp+WaG>Up zu}j;yx(M5*m!NN$7*ITu28GbnJ8i9xFhQY@Y~N}FX-$pUh@rT?IZ~;4p>QQPj!IH7 zl7x^}lynkC6YlDL?CHyLOv54IJb#Mf5e2UCtBsMe*asrVqwGs56qcytZbcppqJ*C0 zd-?I7mH`^dQq^KmLIL1-)}tL9c?>{{npXL4C{Dd|6RGNMMSEPG16w*`8aa@|c2BQO z3O#9mMb@+E*ugyZcTkADh6;|D2gKVdoh!hfF^3gj4G&giIp+!B!7x1@_%2M5ypSj6 zM*JE9WdOJ4LR#hGU@5L+G~xu&9Qxyl_)WNMW%cN5oc}eV(pVT{ARBRO-@$Sb*T(r0 zO2GGcfig+Mou`{$h5_J(iq$86n~V-PQ$yHC`IfNyO(8!AaI4sBBFT#qNZmnS_v$>+ zze7#+&RH}-%-l%FLl-CO@5}F1YSj@oWA_~#Vb+w18>AHBOREv^$DS4Xi^jpmnFfW? z`&uhvUC_;$GMnzq00+Lg7{#kQGGkb8BZt(OjF-c#$ns1;^PXVlC?qCi z{elxN`~9oNY3wTH04aYLN&oO4ic3dO95W1-mi9+YfJmoP0DOjqMlgld3anRgsU&VT zTb+tM0cN7n9~s;BP-wuChPf9A6v#0Km|G#)FvvJ1MsnjX#{0KrAVJtB{Rn2yn`qh! zUC)zbT=Mq-FEx`h9!t?TYwtlp819~-+ zqs&>Z|2=qaiX7RXC(zQVU220}r;8txC*rJ-k_I+ejcV8Hm!RB!N$x1zDSjvW&!=X< zg8wAlDLU{RxsJEH`H1|v|3=<|*0}?Gvv5K8Kq<2SfsHxIoscs)p+^>5jb>1yx`6^< zg`Eb0Eu}Z?U-fl$tJncr@C0yr56S)CS2u&7ug1vhLUTpza_0IE1ZNvQ{e|FvPa7s^ zoUoA8EYhY@>ZOJF11mm0XC*lA9?vWNh5PmruodfH%*SB})DKvSXf$LyPVmls@;4p* zb6zkgd#-Rnd6>|Synd;%3saC>kHh`EywIIKL=0}f&LN#T#pJ{1pWbw2{Y?jbE# zSUn6K+FAV7;aFt0s{2Zz<$pK1^8<_rLZnw^wI3g(k4jQfsJki_R;EW17I(&7JC`PZ z>sT`9&JS(18>US#-7(&F*8OjI^N0mEs#6I3VeBTsJGWdpj^g{JESj|NI5-y)JByMA zV*9oeTJZDdzV$3Ujk}=@C#BqF4BT)u^2KZ8M9ca9`%M;Dhkzc^KcDY=Q zq?FksG%Ra%;PFm}S(shy;w`BEy(EWb{D!9R$aAuk{u^E?#~~yo1YWX{TE7!q-n03K z{TCQHzK5py&yD#^&iT`Dw8AipTxs0HwU!di0MLt*^Sw3#&=)!mk-%-cND;63GgLfw zB=}YM|AOKeb+qe;8eaHj&*#Zhge2|NZn0_Fp}|e`7W8utb`m?T6nA2gGs1k4+iPP= zOzP%hltKzz=hs$3eA_Ho&=m9`*c~H5eovpw{{_Ee^mbu8j5s6N?gSPH+oSX*PpoDk zK^_Hh32|WOAxx1bpW7nhz=Ho{FW@xylkONz!?}#Pd5VOZAGg8Bvcy9l5U~6pENYPp&A#B+THs}fhIb$X#1f=7#j4r?4=4tAm^K5FXDlg87Ba5N z=**T0dHUMlT2khJk1Hw?DGAz284T)Aaesr%k^wyCAz>Y;PkJs2yn$p*0CSb^Wb->8 zh6&bZTCO08@@&MTQn<*m;yhal^^nFFR!r7dQAmhj~XI z;7nY=K$sGPD{&W)9`jFLu6igMZWsVBp1JOPKr3Y9uHOOk4O8(q;x9sCx=Moi|KC?k z{qkk)QQ1Aosk>`jfE_&v{o&oy!B)5fEohTC4)RjKl$hyGxaK?#Q*}P*^e4124-@UG zJxfidVbWHU*zNmbqRL2ZoN&VizykJ3zl#47i&=JfJ>L!tp3`9f@I#bn3kCj&BELKV zql^ean)4CvRMBH2=;J6cw*Et2-(Dx3I~|Qw(Rab=+4YcyN1wIwX5i=_f_W`?g?H(2 zG5ZQ-Bu4)49ddAVC*?l9VB-vf^s-|y{y^uPA0Eq}Z&@(Td5cFX%X*{jAS*gf-;d%E zGXx0B8}In*Z$jLH<^V=aJn|A?^c22f@$_m^D9HGyZdFQO4&0s(f3t9Q7Fo`%9a~6x z>fg{)fa5(W$K+2{xJMk9@^%1(uIAfi*$^p^37z{*5#xL48W{bwG=6wXGqpMSxX11d zxY+kN&?-MX#$Wn% zHtz5hMe_z^;!D4XDz-X)A72=OVpbVBxICCIRDa0B@78X;wE^xY5P^L41IcEiy#)UM zUUgXS1V|&|^BBvfsQAGjJ*3S$P=AdJs@N}#l&Ssnxlj)ohae5P3%I-fe+XHdG=+d( zy7T91%7viae>|5Y;IHf)$|Fg!vNj#MoR;0N+XB<2R%^D{R z%CW>fZvXe|cG94SKb*x6nn{O5@)3p#QQnxvGXvnj&EA_mp#_aW>+XsNZl-E#cpOAY zi4liMbLt_h?|?3$oj0KXDu=4#NywpR&q>j(xk!criT_9gso}2(pqP2{u=d<>B`u#Z z4|^6ssIh&2pfQWMuGz5Y_hcIMWqn^pzY`#%ZTn6}o3UYjcyTcH;pY);?_Puez%XXk z3e((w3+@_ioQ(<4ZhHvh`rRhuW6|j9`>jMO=ahpwhwn9C$OpsXaqXdWu7r6&3>JVW zCuhYem8^%7@SILXqF5O1pnfgnQr+!W3jdV>thn6dNARicLsRO#_MYqAIuxhq=~y|s ztCFEVzHWfZ>{o!9vjvRIeWT!jBgV<`|fft0~z-H?=~ns z6;_@&rh?lM{g=*cldoe{8vljia5SS5tZR`^Md1@66g1I%0L$qNEt~A-!D(0-2)q?D z@x7YQmZ)Lmc}-X{h?MZ9M!2pFOY$8Gv$o(y`_Nk?#>JGdXwnSUhdDlfcS-cZ@Wmjx zf0HQSO>sEv#5YUB7P|2xr;4`JM;;Mw`SFO?&_?wc!7tgZ1&1Xt$Xk>Yb*%YdGR^}C zJ^G1IL(xdCP1N5*{Mi+f-sJ!Ho2z%aq9OAwoA?Ue{{8ET`$v%~h_u=RRThoa(3%0REFUP3dtEObsmtc0QAOV zp6rq+xP05@X&8d$@_d4%PiM-`(`g9aRtKW200F1V26^{^H-f%TF%H{n#ovta-va$P z2we4N|Lr_z*u#B$V3 z(8{X~d%SlDaHBlo(deA}36>Y+1zD&0p1--0V*9#j9#iL#ZTQOo~4eBZ*p?neN_^~=`%c9_~ zRZ*UwxBe|tY0%qpcl?_HY;3wpTKzJ=%RW}INNPH8?s>>z9jESkV(~2GopZB6Kaw31 zF3T9vr}f^yQzcp|@{Bc{(nK=OlU_dbx!r^q+0EFOTOXgk@v?lECuZ^{>BssXdxaqw zZcUtFrm$W(r@Ohe2pk>KT}?G0b)yp5m#@`G%gLrWR;Ee-gc! zm5+G?qg(fiE_i6f;(lxIReUDN<(Fm!KHSTC19sRf1l;R9sw)k&22t51M3=pX>^tPq zAYMEj^yO%`2DZh}Jc$|Igd3m%7O!|rf?eNkGHecJz9A76KQ~w;_ie8oDg>f1R+y7z z!yu8c9nD5?Ny+luD!4+59n8V1<&y6eDqZJfFGHBu)Mv55p!_+9EA&{9{hM8)o?cTo z@4MU`MRqtF$okwTr$*7HH#W}-PR6gEvyB)^0lU1ZP08ylgp=ve!62z$+e3)a zmyviCq51;$V`cP>soGoCg*20OOYaV(#3+KDLqZCS*;T8PiJMcAlzs&9Zx+VnhJT%} zD4<*5nl8*w=367wfWBl23@WH4YY3PQr_ma#VNZWC`khKl;hpMbpn&&z5 zhR42PF$VVsn0uDnbwymgm=v4}4bJ@D0#GXVtGDLeC_+8kp;G-?j~F5W=%dzMedcfX z$%w0*2f^x{FxI^KVb{18slxBk3dL8{U+WUL9rY@9<(*a;7J z3876zL3K+8CHsaNAt|YfoYCCF0+N=ZpfB`SAo5c#patoPyFSQ}daLxv?krlNZAqQ}1^x5b8QM5(VkK=~^X%;dBGKcc=n zkjg&%o8!D=G@dtKLOUB^vN`1M$-UDEr{n0(P(hRYd(e`MFJ?svS?Zdl$*V3irLiKZF2 zE5{m|z-sirS^$*MRUwMf&lO{KWJ_jNx~LFp5dXzW8$N$d*1h=s!xJ9N&m;qEdE)dB zpy`iWDN43|t0*3rh|^s$$#QFm$!hnB7vt<7eIpbIBmuDyqHt;0xwX)^p%z7v0AtJVU*q|mw}WCt_`tW_!>iL%b+PbrdpF+%DGm}mtW^ut@&(R=37nr$C& zWEAK!UM*IYXsSO-bDP~J0I3T>0XMqWXWk z0g1-fW+Sz*WTcOu;f?J^q`(%G@#5O@jsSNVj3>%^@&M)!ruqi`irUru%;X*nyAAC7 zbWXH7#nEYOzSSK@e7v*b$G?qDwTCZE7Rqjm^1c2zB7I~W0>&8ic%qC||u*?7jO~`f9moknLh%oJ=T~rUr_# zc#oGJgL}m%m8guUJ&T4fu%~FLyJaoQzRTgw6gq8+E<|ArWT(E*WIed8!UuCMZg%r` zBKEq2S6%XZljlvG-LKvwEL9@yecTD~YwKUL+6#A18Lc`oxtgrrnd~|jPN`F&5T2-Y zB|jPW3P^S3F+<|nY8speBCB_iG@Kc;jOd;DMtRb^&i0##Nn=_E&9_T;>J}MI3=LtB zG{Cxw@vj09l+e!aQ&2+;&88#JalZ;JALAKM4ARQ{2HEQKb<4#|ttsS~CR!^mrrAgd^ZKDvMA+>v-_q;J-oI zVuH{e&psMM>CqeTBJCdEQ;kTNu>*#gL}B}ko4{~Ry|8}Ev&#h{{M? z%y6p)7MwVb5mgP7=c|HB<$XiU9?Byq2Mrrn&H`x?mrAAoI(^Hx%&M)~iS+Clmazq6 z$DWl6rDecvnX40vr4r&&aA51eR(j5H!Ne``!SPebH=RmKBWb}U7+#;YfzIRLI3$VY zZ;iees0WRl8JupN-G8>U_PNgXS$KE~UZ+!?6=V8344vk@En37~`!d|Q@65vCBMlEG z%b0Upig&v27tF=cyje9apKk$`wtd%lL)5$MCcEQ?&ZNeX#hLHNf_bhJ=aVg_qVOa1 zT@JcxS+9iC=PaQ`YD}w)>zRk$m)RM5j$dSh4&8M>tKQZIGXZdSi^j%Ft?e0UXSauq zx%hWM!vE&IJzvqAaj+}tGM1=axN&kXdgO?QKS-|Z1Yo|#e=M3~-GCsmC_gtmnj{VD zcGO{gDchrdZ*OHDO;KFy0n?Oo$35#z0l4pcyJ`={TUgSMuMvKceDCO@u|OuQX~bXD zkhy#(x?tnz-~ci)ug{9I;863t1kq<+o9y2ghRe!JNGh~jF43vam|G$^#6Z|5_y#m@ zUJxDnwZ6Hp=yAzt5_c{gO$<7;mqL~k)R2Po3B%3p2 z+0w?n{hoH=)*JtmDqttW294;l>Y7)`zct-a=?H*%7KY=kU=tW~jkxKgcy-(w1@&`S z58-3Ze0b^o6SZ@#)LVo>SPYT*P=JJT{_wZDfKgJp)k%lFt? zL!@?|TEvVTly5zyF}#-*P%$~*Ztk8*Umi}+_upQ~FPtj%QU3oXGPG02!-DmF@d>yp zI5i~>?&y@JDgx2tT){WUXvlZ^3c{j~5A;qm$f+r)tJ-`_tML!-$`G}D2;TwoUrG_b zG!`_{7CUt2r5nLLl77tS6J!rgq3hsI5sJ}&5ucmLXYol$>4{WoO#s8Ew@)qCiqG&S zV+rfI_s_+YNAxNT*!p=HkDMAWB?|f~dhLaJS~HU}^+FwOq}(Zx=Ll{@ald$#t%}kI zG2={`Fh~9jg7Vl#zz&YLNU9Ipcu->jjdHvDz7xTaxJDe^DkEti!$pvHeFIBX8G0^( z2ekmjP6E3<-gjoaruKISfT$xpp*7e}T>&et^emq{iiN|I*A!)V_FjJZ>^5}?)sEpL zCSm;-XcSld7T)wTKHcn}Mz;~#6ZBWb;ubJ}K2^qM2nPHcsdqBjXBqSpR}SW#LjxlD ztmd8VOKO4i{;rXDV;t{?x?W$|R)219a;}^}6@5uIoNSomjQWL(p%l#GF~(ii08{2g zeR(Ya%5XZx_lRWVh_WE6nBN_EKPj};KhM}b7FXWS*RZv~xH5cHcXj=>G4o}Be^i=g ziyQA-0Im{4gVC9M@25t)FXw9@GXBY!H@c?D2y%Flr9$H?8b!NF?*=nE-)dg}1X2&c z)cpEWVEP8|>eeIqpeTKihNh9_OyOmnF3n>VVvHki1P{OY@z}xR1q}RUZL#wN$7h@E zPW@hYRfAjL$|hn1;0rVvY6Epb)--TXdF`bIa^GWO{dG5_|VTO|pl`x7N->hU@i&^XqhABFVws6Je9dnP$ z9GJ*xtoIq&16R0uwh#uafJ`JRT%->>F84aNxm`;jyArWuD&|~h*-^EiCi7h-K3b=v zsZk)LE7BPFg&^i8@XspGuK8VO>*}W*Iu*U}2;eTG-LJz=q)Zf9sfJkHXd$7uh$$o27fr#ZpK&RT)3Bun0 z5$)=X(&7}JAi8a7+d0>Omk}vcT+W`rHYa{DNfn|A67Fa!*RJMaE_{ zRTU&D_(#9D%fJ6;f>@zZ5VH?73r2rRsR$gA&Tiv?&j&Pb$Xs!;j47YMJo!X!(G7A$ zt=|HBN$ZofJZkqU8|p8NPDt?1R>sUYhoFi4p}35+2JG6nEi3WbIHUZ{afi~`9&Nd8 z%;kjzTWoxa=q+Y(2wbWzCH5OOILR~iw_gJ35<9#aAL|$%4cUQ(g+vMVOB3p@RGQPA z7^h@VozfGNy$uk2)l{4lIKH>dE;M8fn@$Q4?%mfAT!f{66(_UHp3GHkKonbaRsr8q zsD;|5!R;ko$C5oT&4l4uUfj6EeZJc{gR{q0gxw)RS!0!;}3g^lKn8mp?@%vY&}}xVQs0v5N6zqvUt7 zVPW@5i5EW8(3U)eQwHQ}rHFB1%%>m6P#wiUDVqi`t;LM`=+BF=vVQw{nK#%e8}!7y zru904OwVIbv1ulJi?t9!w8h|n?Ie}jiRBc09yW7Xc#rK(Y#?9=#0quUsAx)oN;(=G z>%}qAp~jVMjUoUl!r0^mObNf6%yWc7x2O#VladiGZ42Mc+4Zjk((Pn7IyJ>))4g?! zlv%|pZ|SkZ2#y$547+ZK^mC(211 zodu&x2Fvb4GLzk4w$gld)M5&LtMJ+BP;2x=m8FYp-kVeT^lZOC97LLR&YDVLDrU&` zz~$(S2eq-%R+fP2Jfp&qRsn8_G!pIJz446NimIzf zw)G^1waQv*)@sPF4O?Gx%?Es7`1}RqbWpR4fgLB8iuoy zdi|H;>C~Fxu*5?xH@cfW|57yBhZ8yg3|_;z+g}Gv*X`#rWsZkqN)cmSM{<6nKnQHl zD=15Jzq}Svup@Z3%~-3GF%){n_izHozN?D?>a*3%cGLvSGq)Da5N2sG$6D#L?cu>c z_BdoC-Btv$(dT{2cOG?=Jy6IzTF=6WN*N<=^GLFYoDr%O_>VUR5hJIfrC6!?9whUW zmuzRBGMpc=u=Y0j8p?lT(Tk0S0KTapsz2PhewRU3am%@+5n3YWkW?#der)%Jwe;0b zp1a(U1l;1#wk*Ep1}Ji>qe*RfTNTGexKzti{ZQ=A8;34)z9X>m$DXDjef+c&4)hYJ zjm--*_QLXuV4dBQ1Qw}xW5)_ca#{5}IczD^$8<$de{?`ZIBv>6#)D~GA$J4wh2nIN zmm-ZJqDT)<<|K@uKY_t%$_L~GI$POSEeWD$>XMnJYHef`k$u$0@w*mj1Y}Hs@*5zd zp$;xpysY+0%!)>EfE&MuVPMno|7cWCo%Fzg?W(!Cevl!(3{+N@Dotlzi?;koVdFF09y>{@AOHVZs=Kg#v)pg$qa%B}O^QjO z-y|$#IEPf~bJp!QusfH*w}a{QfN+JXd9#RjCEBTPw(eqb)Qc?QBrv&}=Pjsy5R1%$ z2J}2jq(Rgb%&VSxB&A<(8S$CU_&2>+Va&cv?nQU8!%Yu)bg>{`RE~e zdl2N2tV6B6`!-9iZvHEz0QKImjIZ+Gbmoy6k&t`|==BZ+%#nywRD<^7Jay=+?)em+ z#FvroYl0KphLA-hb(^&0r6Y-)9!pSsEbxROVcN^pAErqA*&La^SXy($4(h%ue__6W7~P$e)8*j zb`1a-7(&?%xW`A|M2%8uqAT%xVQhXI^_q61mt_Q{#edZNZah~imY)IBK|ctOPFkLr zr+_n)vkzGNW9NGIo_QXENjtTCscAraZUfXP9(?TG5bOcFfG8{$aHXICMVi5m28i-G ztq^sn7tct7Pz*IoZ0;OnZUIqv24S;v-ySX6oheyyf4sK?GRunBouDkG^*U3A4<-eL zz1rUWQwTr_c{3*bmmk}l5A2PhCJg@ZC4)(X2Qn|_B&K$~PXMRsRYSpwSIr8?;e2O4 z|F-jN%KY!C$F+fbuW}n>lXb@>ldM+wPs7131d+8T_^I$dz@2?XwMWY&;}DUPfO>yd z$3I|xm!WH5AzZs;4}xNQb;Cd8<2O=zs{AIm?;g5C=z{A9@5Nqtul}wE#GVmHPdvmaqCZRrN05J~K*fv+gL{~TYw?;Lqs9so)n>x?&O3060u%HW zrkz5tSCK4&6YRONFi=!_SRH`N>s_$y4^o53NF!1K2X!B*Ke{R_mk^l+i8&{p5U8AQ zevj|Z7Z|-n5G%ns)5R<*zGUEr8xlFm*qfI@!krAV9#h3<55Ycpp>HN{kn|%zoM%Qt zb)CHT;opAeBH92qq=WdXH*KA)}knw zxk0UMthO2)>tJLt!u$VTE0#ftL1BiYHNkI@LAGt3w$`;bBh4S!vF4QsF>v8^>f!UY zsvwNfRd8D_T?uALQrKA(oZ!cA5FmDrpIF>`F}f-Zi&QJaG>QSH=8J|}ID1b% zqSp)v2F4MQ-FOkfGje1$1dm(eMYIJI-G5$FkOvk5;v4lF#F)=7-7eIB_Ou}_jQf3N z7~)~O>M7lE`lQq#OE&LXnOF_A)95x=>>2kp?^WUf`JHehx(HR2J$J5{&Z#z-r?>>w zC7`3)kI)nYoP=?y!{oW<9PA*)y0)*p_FAuWpqJt&{7u~%b+LPX{>BgCfC9|}XoQPE z2l-3lDMREl99(l_+}`j;@3XKTtwVcB^*f6#8NI-roaP9{nvl+KR9CX6Z|ip8?_YQB z9K+ErA2_W)E5vJa2?fQ$)Ec!f#V{N4`%agTV)+Ot8*oV+7puyX79@e?L*x!*s^5er z`0yqN5d8SHnZ(eGriLpNI*!z*X54~qewn@(_ioc~F<`#fT3EN=oFt>G1QNVM1yaIdUZFR)T!8>cWuC0A1k zfRI+{q|i{P)K0cY#rc!=F4yn`ZUR-D0a~$0ziz8Ml7x*+56MdTJK-snp*? zeDGj`DDK&i(Xe82L1jmNt05)sdOre+*fErJ;E$oaaEp{jHz`pNsF|Gik)!@nzapqy zz-YPUH(97nTTSaLYZBwH`nQ?>jBhI=qFbFZ0L}^JIazyF%TN8?1iS(b9^eUEO0PKc zg}#1Z?J)Z;YvODPQzCa+cTzwFq@+zC}uvt{^84 z=b@Fw%fo`z!0m_8{(#Orz~``rjJO{Ax&_=cef2re^(yi1&)hl#p0&iAybemv9Wna4F4o1PMvEt`Cu>?$!K+6FqYfgQ3%Q|O^hgEVNvcx zVs$eOB063+csL5*seqw6H#{9PVBc=OB5AlXx3YMuWUtgcDd*Kvy1A9D?HDJ=7sxxu zkg+x|gRm(|>$!pQQE!F2Q?kjnG&FYSOP-w$V3=WS3l$O$4%&{4E5rYV&&uu^;BIWULLkLhHqxi6eneNpvB1-Gbu$Z%*_UCuTl;T&r zLJkx%|ISd#O>TVDD0H6h>=ii->GnUZxP^ySRp-PZ=;HEV&ev58Pt`?d@?5QF*Wvzi zdAtiQGgo1z?JMpB=-3-^vVAu!R!K7Z2gDE{f00CKIHsq_@9&|KD z_C;__or|!7wlnrOz#ozlfYpk^nEvT6s|Y2lq@#`d^tP`0w4*_*9=@D^@E(BW%CO;dR`;|A`dhB!K%+2l(Y@ z2?KjCZ>_sOEb_xaS1tsNhE&@|ts$05!8m7{ip0B9o85%`W-ZG^7VZ3&AnF(J;w%(+ z+RcgwFQqCeF1vt=+D!p|a@z$DS8ZA4J3|LP!>%`;h0@^2kjs~Ef&v$m>->5zaIpXM z*sWEFNQ58*zT!X+&rIr0Z4fr@q0>gW)vX;b9&WnF8T*W$Z{L~!W^7>};YF6D0TDO3 z5r!>k{nYCL=EtqgY3=JIGk=eNie-#23~b5dMesMnx-n-m)>8FpbXwfF=xw5r8sB%? zj44N)x$2xuvhyAEn%U2_I>k^r{VnrHT?%mzch`+omrk0$CNT;LT_NF*8)dI~@{Yft z0;n+2qPkc{>yvvn_5bd}YjQ)!r2)SlJ5EV-ib`_J#F+Ai4w>rnvH4bMKK!UY87UJ+ zeZUUy2eEIvwR> zVsW`CErv_JuvyAXf0rU5E$arOv?AcSrwL`#v-NqTrPJwn?z=TEOX;LY^hBLM6ckZc zd*K8AR?$C#KG$yOFtrMU^3Uob5`J#_>|PO1JUQzZ`wZYco55f0Xj4#l@(Rp2cYMdo zav~Ak`p``c-El3sbsgFCPthk}0M5V7@SQ94F*TACGymBc-wCMP}yGyl|Nd|5u!hzlbbMS=nzaisgJ?j*_T~&o{&!I33qt8vUKIsV-1uR%GX{5bs=kCwLXP-u-A6ENKr5SqN0g z-U1iG`c?P-pXSCCZENPz{u&;@xf9ZZ>lYomU6*!#@@h&6G2S}7h z(Vo=-b6Dc}0D<@sDsxKngFp8IAq(J6w1&{+QyZobC_$F*zBT@YRzdsSh=ZfBxxpnrqM9Z02I=tvPhprKs7rf?%eh{ZO2!g1y@0S)}_zVPn4$ ze0BVc(yz15fr{2QPd4-KT~Z-a9rS}cwYgRp)rEAjHRoZK!H;Ao_aW%xL!@euy`Xce z-CLTBk&TbJATibuc zj2A-*=70NKe3(LpW)oqIVMrTy{ORxAb+?t(Z78cgfbIHxT-~9sd>S6TN7m);ut&ov;UfA z_J1p``p&fWmZ6h3E9lHpL66Fe1qjYEE!EFRUqr)W_F0@}n(Cwci0iD%97BCHkGaBHlZQjHpz$?(ca}>^j?eElu+Dop%6e>HtNW zUxxbrf+y5aF=W=o0F{?r54wh#icN21IVfqoSqcu19@@~kpHFAK_ZCKGOQ4;kZhQqG zm%w3Zxt13{`VII7K7aW%&^|Rx1>F#$t*z%8YJx(sK5A7GEqm(S@Wv+yOPKlY6(*Sv z&CkrnHvvZu3%+CL+BZ*e|MGq#`xqUAqX+44hoA2nytdtK;=x!PK7`5&gg}SpZtLb4^OP8$qWmh3AMqTKo9VQy`8VXn* znO5CsLT`R2=AENM!RmLqqJ>I(Q46NmjK|85pNpW{4L{QpFBn;^W`(`?_2u55hx%%^ z-u?Z#aqB=y;<2;}e9)ZSXNEys2el;275is$#;;b#HGR7n^5<3k-LF3BRJrAM?33rkokr51cWtT9h4&>^8`w8vOn25+{>E&cWmZ0{vaXV3UBq;~Z=L z1!Ujh68R&}Ah4EwCCq4>z7l-RIMgK+;uL;^Gt=zl&NT9^Lv2%|91&dby z3)IBg1oOM^x>BabdnBAco(%l_`xWB2BirUGM()jCf%w0kr`&o_@Pbt=dcS1pfJsH) zGT?Lzq>cR?*I$BE`JH$w5Q3{M%u_{WLfHG zB6luX(1iSX`_a3@2DHYX$2;!dH`&@+uGySwcu&`C6gx>R>%Wyte#c5lW`Aa|hQ^jo zVfnIjopJqeWZY49lAY>TN`8r+z16w%q=RRaFDhMY`Nzuo1MXcUB_7_sO! zXljhcLsOqTAjrOT;*j;1`Hfd)glMM04<@IGFz0L|%wdqj{mp(63{^$vr#ji^-yJHA zLubdEyc`&%2h{#c%yLO32si}5Jp?Bju#KvI_c1J1VVSl{!kS}^Q==99PD?@i<8ZtWQ#R< zOLl8~db%jMUUdT&NXGA8Ti&(3@d->;Bg;n{ksW2ws$ShCtjV1K8re`rXYmW$>rEIe z){LDmHo?3}o0hI4Y*#G@EjuHqtKSAdc~Q9gaj{|2{Pr#D0Gamk-dpC6<<^37wL0&k zbgHlQFd~9y+SY47HY>B+SFJ^p#WX!g2#XAAR}*;7DYtJ8L|7@OhZTqx^*iXdFJiDSLRl>x zXt!2W~(XQuCfqbGY zl!uo8ax#Sa`kz?EQauC}Yf9E>Xprj#U$ZoPq%}W1=QgLFJp1E3@icp|Yg1ejQ9uAWo$(kB~C+3p{OD;E4n!psXXLhHfE>cL&D6V~6g zUk*O+-2M!?FyGD^=HdtaNV3#l#8^xZ66t#@{i8}xJnf)_W2j}ga+2P2w)&=LyRxRy zEp?~w-=-jOFl$06^Q!VsrPl2?tPEVLQnys_0_)R5+uAtNRK~rOv3TPIwRQR#h|1~d zC^%L8@VEZM|BM%gamp-#JCK2WEv;K-JE@p%n40gjF{ol)UQx9<4c~pVa08_?d9PJK z4OyR+)i$q|_lQ@UrfVA9JfydoW?cwTfQp(Lkf-Z~$OZ0|s@w|7o*y$T>#`x#y;;DW zpPxoa*|VghE8cT}bCq0tzD@lKs8EPX)dogSdy45+MvVvXwupDGAB7dQ|(V8Akj*^b=!KacG9YS>@vRhNExR?nc9}YW2du;^HRz8SlvGMngalPe% z$YbpwA2Mt#_Z2VD?P2xCd=K^0enn5+2D&>La^2oT%^16HvN|54JR`{P%UypL$uXI2 zxR7Yyf42kVVS(L$q4+rmf*3*z`hI5?r7{fZ)pP>SQ4jfjpf5}LIR}xKH|)=w2=%hF zqWwjq+bk>#CS@6|ev$&G7VHMfnl3oJ>YGVYE_obGR zU)$~#EaQfix99fTu9jWmC^2%M5;iU6)G05^oh5P}6P(}6r+acm#%oW%O{sq<&K=?$ z*hL>obM$>0kXL>TVFQ3ss0!feXz5x#$h*Hmom=_TDQHpeJbrfFf{y9SD-WKYw3Gb4 zc=&TP+`0AT#s+9;kM6vx2P`nG#?6(+9@y4ATeg4qil$x(ip)wQx9#DC~21EAC--$_#V8`iIA&~p2bnlm`m%kF=#n2jZD_kV? z54luu9do$Qn|{*(1w>OdBHczHKB<37%3Zs7ML+fDTlF*9gKDK_d`1>}6r?`b*}K*# z*w2YwnqDaaTw??@-oIJ&-fMA^poh9CD{YcXs8m$MB8KM2ra4r|;@kH>L0c#SJS{@f z9eXJt9wJUcBEpC}jQ~fb{rwqre=t*~+5%Sr%_r|S0mAydk-O#HDr)sN0qZwtzue`CIBH{_l--|UjSV`e{fO>G)h@^VO6`Op=AmM}P+$pDi zFvGrsW>zv4+a8XU19cegkQ8!Sx`q~;0nv2Mxq5&3i9>b@qA6)%Yubj0po0TnD^0`? z{=(;{S%csd$iFBg;e+!d*55$S7(b%8`)HEW?B`Z)Z89|WBn`U6Ns#6yQA+=ycB+Lq zeHG=P8(|;)WdFvD;aJX$7zqv_;jT}9Ze)yS2fT1RkOsqX$HuY-{D!Xo25^Q;jE>}- zrq5nyv{-?fTR~-3=s&~X0#EK9Wd=M}Es#kq`!KVSELr_#JyIL_2mfPzqItunj)=9^ zHTAuKx`7S!2;$&N``qGpH_V2NPLLDHpomvpxDQf?_}l?*U2Q9_rMtJn-9@4R66X1{qp!PL@FkpK-C=?I#e>v4WgA`BeAoFzdGTt?ddh< zV@M_$8?q11yulVSWP3WHfVS$=;U6eFcHq)n{dzN7FDf9V!aNzVhv{%y`00ZhXidv8$aT{3+@Tk5+&KJ zq?Vsx=ybkd;rbXfFls-S*%^U?FBrTBsYhdMU_}^JNP03uSv=}?Kusix? zXp=czLwpq!^K&_we|Wx zD^Lg66fY|ABPS#xo<=u)=*=Av&pagq(k=~J_cz9T1A3?=9{kKc&_j;C1@6>S=K{Y~ z^qnH9d<+`ca?9>f-#(>Fl8@1nC|T{k?}W8?9ea#uM4$+iSxdQ*xZ+GaGZFNBM)H>%UbK&dFI zyNPc|XG`l2vfthJqQRG2bxB$2=zP67pb5p@rezz5d3Pp)Nwd7^>bw0eg1$-dV%?9L z=xAZh?HmHCImDIy8Pe=pWw>q=X;2Tpp&k@@{Re(==KaL{e6H^KJ=-mKx>t5>sYNvr ze4R2Bra7iK2G|^ao$@ZF`rdk!JBNuCDzDE)yE726nDM~{x%K%sbKZ-74~=IOSv4lz zngmBZj$f7^0Zmpu-U-kxISSbb5d>Tc5+&Ua+F`UM_X16;0A~a{b+G$V9n5Rw>2eMz z_$0g3M!V194A^moW<3W{bP5%`!W5*OW0WT;`(fVng_VcGfayMJoDI}%QG{3lXN)Dp zPyIO)_6cl^SMi<8?=!~BhgTXlbdx=aN*;36&Rc%~607{mFPM`mD3ekL;N21blj@;P zgGnZEw&)Diyr95F@+w}hPwE5g7$|0_d+l%u_z9w{#0B|j};&{`5AoA7_f4_*VP!ClgCs!r-a*XbEHuC`>yvhW5w+U^bNa_eDFA5W>IUpHl z5a=7*$$y5-kSm&AY#uaTg0QoL4cHS_wv_l7=loazjU2Xr%O^L1HCIoo;p20D39@s56iUh9eSugd@ljlHLuu(?!^L+s$Z}B7qRTYBrnNTQB z%r@A{1qNz6Fdq;mcEy7wF<*D?aC^~NK4DgF%mtr_D4@zdfek{u`JJ5qOHs>ajA!^v z*q~WfD}23f=$g+D4o=nQ&NSY&4*PZ+U^ky>xP7W8#*kpru!F6z24c94g_XTJVqu4k8POSt?C5hb%sztJFKWHKpD6$?UXFPk` zFMiaI;c@)m<~*KL@?I;;U`WXGqB$v=OEBWcH=E0b_L0zcuL4k*l1s8;7XGLKiYzr< zM=@Fxpz$t`x<8hOYNO=(Gsv<0#y1yGp44GWmr8CrN3e1mz@<>CuAlI4=e~^iZw-Z| zU~R~oxf&!#-)D^4C!7*GU23Zs*{DyNLJKsqTZ(!KA1&M`R%meFpF&2{TX2P4vX#+{52-v?j#w)aH=O%6 z@Jtv49+bP!*#@jpD?L`AnpZjP+n6iJ=}Za1^FM(71@Dn{DFX3z!1|M~TJS;np?@=r zpEe2tR(zvVfJkI^X!WDWB-e!bMITO@r`1Yv zXoQ7Q*W~>+*!s`6LXsZmW%NMKMKOp-kC6~yiM&;!m&s84S@!aIbaH-#-YE3Pj*al& z%+^f9Dps=354L07>S3T+jR311q=Kfm+ zcN2;oR6PWw5tnShROK8aq^hU13|~bW2$x7LB#drq5>(b?1M(1#atw&Pui4;=^l*RB zhP_nOLP!}w49f19ZHTNpP80zJx$!G^TPcBdq?DGz(&;Q-(8JVE#UZ!xg*BRjjs*}T zteq#5%s@ZaC9n>1tXy~m9n*V|SO~`ClA0!)6klx1l=aAmXkMxRIGbb~7VYjCzq+_# zDs_Stq$7989tgzR^aPQ6derK}Zydh#ri_E^c&C%+LycTk0ftS%_p;AI%C9=OE%0{? zGDM#(eatl1+l{1sz>q+H))ZUcQC|S;)MEJ?+fh)!lqyfUskkuRFISewQ0ws#>O%YV z2{>CR%k8iZ5Tf9HG_~BIyIca@&uSY+qbEZUkqmeu{zV~2#+QDZR@K^ug-($8(Y;WU zvdGcjFFWbKoNi9h2+55gVO!ZMr2)S<6J}fQyV2zHSCuGw*;Tv>*3W}xXGtv1-0aCyCahOmI?m#&8Bx+ zUufGLRW&Z9JFD?hD^iDx_u&Y4{V=!ARQ*uCMp`K^y~s%ZI7<6qa_K?8MIq$UG=^>i#<&@tj>tfw|`Q68CUof(pU%Cc?WPyTPzrV*K$c?B;n4 z5AtCf`Yt;A+D&GijV^`IL?hE)m|1oBq@?f8vi&su+A(;!Wm7Cgfy(X6OTF7;5HI0O zaR`f3`0w2ajzyKZnMty0(X|RKZCuCw?nh#_GlSfXZfQgL>;nHp4@G zgloI74*OVMgLUAkn~W?6H781x3r{d}nJwD$2;{i@lFe<6XHK?}>zKKD zYIHR)Bn*O)rv<)}Byf5}To#>7artU;bMwhEitY$KP=~N;HOZblqKYIxMQ^lsG zp!^@m8%}zLEN|0*C3U9sbi5u29uyi9(7_z;+hLhPcSNKuI6ZWJI%eBZptzrpN*5i2 zqru=_4qsM26+8<5O#w-6o#C3r1W(Bs78$qam<>mG%DtCt0KwLg-(P%iQ98t>=Na)E zkA9Jb-7BEBoFv+fspsB;-19Putj}8?(4Wtm@fYtKyNh!tpGTFRB?;htw~)t}6T@{O z?3`kI^>z@<8&&F29{X%0>s8%92D`u)&5!JDk&R|Qt!Oh8LqLrYMfK0wnqQTv>F1$E zYp1E7W1FY>3=2~pezzk+L0EuH+Er|6!RF3fSGZAz6(oLX)JOQzJ^hD$3?kWbEy;}D z@K~1nw@$bGt725*M(Ja5t>J@vAa@l4WmwtXf{Hie&)-vuzR)sWF$e~@K^?>niH$?I zKmw;B;P%RsBHY*9X77L#7JS#+O>`AI3Yc#nkgc47=Qoe-e1Jib%yi10fY*VSVgj@m z>Oc!7?&e7o@|!=3;HMBW_;4P*)hT1sNpJyPW=&zWVv;4V#u?wtz~Mi!IM^giUyc7f z^qr-wp)02%*BriLSq%c3B#_*jEah;vsu|{Bu36@(Mydn*H+Go@D(xEJpoAIxU7wErN$f;y{nDl%?t}}*_8kcOOr8XmT(IV ziLaNHi1IwQA+U|srr^OR+)8|TA6(DVOpE0^=YN)9_aN^FUgvb zQ^kuOa%h4enZ!w4--(R=b>YiGo5Q( zxJMQP8`>~H7B+Uq_W0|8j+or6_g3xg4ukBW z4=dI0K&x_=U^ubRJC^(HZdgnwd?&9V1Av&08Eo)0zNCWpG&@g?h-=}W9Px#{`sTXV z1d>Ls$}pHKuz=aQNOyNh(GqfFi9^AMAjF=Es)wLz%iui-tQudDwI4M}mBf9X2% z4soKkIq|FNdvAE~eQ(5QN)GRM+_Wj}fKV26_Dnup2Lg^HdynBK%4aENkedie9v&JW zy2Fh_b93RN2VqcTzFWXSD_H0Fs&@86J}to=9t7;!+jg85(eL>{6j`WifY4mryi*zKk_7&Ah7Vmhi#kCQzaVW z(vCx%+mP1IqW0>7z4+g)8{Em~P>5t6Pf^|^&NuS7RGP4C5$10`V4t6@jVXRr`)|V# zAe4?=nWHm=UW#;t$km1oHRZv(Dq2)?ULgI^Y-_^2`a!+-;F_l-C zZZnH5>Q{jOTw!k2?{%@F-uu+GGw9njXYEjh$Jn7zE75&-8KxRKo3=qt8g0oC{_FCq zsh3hn_IN%>pWj%6SKjS)gzkT*nHq1eAg&=(dgpd&nMDbfX3OdK9bF&O`A>wsp7ur2WXvfBO0>IsO&7)MbL zBwyo@^DMNsEU`u&8li?`BZQ0XhZSyuE`a_l#gxR_x6W{>r5q3u7MuBk944;uYsmF6 zkg@idy<5h@TS`Uf_`Czsz~5Y7>05^$*Vx9E7MmH`Yzcd=oMj$gLbs|D1czcqjIKmo zj-;&svD>>BJ2Jzh78AEP??#&ur8%|ljp*hB`LTWaVYdXfbTJzgD7X91Ir`y$H9wma zirjE_ghvsB7se!UBlTxWQEQ{9hnt8 zca8Q$a(Zugvm0a;8a>ftq1HNu!Z390w1XLluY`!f$}XgUS(d4)<8R=2iB*vfJzZ$Cc%!zwFn%a9>NJMVnW+Lp^P0a#e!+&iL^XvrN8Bc_yiQD@B+` zNGsq4CE?Zu@CD(J^uq;^ldf87)u|{1gGxVXOEouSR8J-rR7^8|Pi!>Fx3*P4qux+D zg!@aq)(8Aevx60gMfG7z$lF)eACho>!LTJV`{+$9T@ZRhUFMM`sV7BOyLCI2+nhPz*tXKnq19`O*YTg>2Cq95AVdjnM^ z#AVm{D@obK^vMi}gGm#+Qg8C$%@pkFK^g4KHeF^Sunj2%eafGgc??xzJ~ID4uzaa>nKq<#?#UK40SxdWqKv z&NFd}rUxnpUxV4SpWyhV9||D{5NG{x%wO@d9LmvvpsCX8Es?fX5Mx2JfTc7jks6F_ zzAkmuk!pRbYF;h(Zt4#B?}B$ygO56utAC>IWI^&Er;3~w9l>`%3bin}(q9WKP9 zJkLsU&k>_AliNc{?B-1DEZYQqb2EWboF7SOxH1htwOQSmVPruOocP-GERN6 z_`6t~ws;JA0;Jaw?|aC{bqbV#e&dYE`=)~Nf(p5-}xam)Q0(7 z-;WxYed3{oyk0E-^Lbs`Qw^BgN9IxA;kw6T{KE*seGP27}rEkk&sAEnM+O2n|E3I}ZytzprHdahBne78TW7buO-&Hh(5O45^ zKKkpQY{?zJdfVD%Bz}Q_ln1|K*H4!JkSNmuiO5(`k_8dVja#bOHBiSEqFCJwbH2h( zjMqNOr}a5dj(0ny#CPoxK8Z6|KQd4J79`H=koJjwKNTkT=NUb|43~Y2hLC$s0L@hY z`-d~{ye?omB2Ro2)iXIJQ<*H*a}jbcDuL}yHxd1dO=L}~y<@g6df`%Mw$o`D0+fFc z$%{5ucJ<5OqHomPEhv<|M&u&7lbm`Hr6)$K@>Tr zR_Qk6B9rk!ftBx%5hTl)ei&{}WLJ9nmG~{e>aLfHa~$O~unVzpWDSE2UW=Rhamx06 zqdxo6^V^3qecRK4=a#~@|B5tDbLXDT%gj`iKYsjc^ZdysGWGZadmBzj*T#!H`&W2) z_Kdi_K+L_Nzp(c{78uiMpT?n{35a;r&~lJQ?*c9B5!~*|id`p>C%+5WNG}YCb(CSR zCgKUW&3u!X{kZgH=h13Zq|AE#4o$Ye|3t1z{DpEf?hYI`HAnDXN+|@TvG#aTaB$*% z<3PPtmLMPID=-0hu|=_zH^s3TBjSvN*suA{sxpmy6E&nZ`ea$2?}Xwmv_ndnUDV+8 zmpEahf}v3VzWgdCW(kEIl0yp@#plXufO{x4)=F-F42i-4WsgOFzKK{%aqIMkPX1c7 ziX!?*O&P8Y-tI=Mp z0ixSX(gUf6>d=oRY{Qby&8bN?!C(x|X|6C;pQa=`uso1!CFup6&fM#_Fxnsc&I8slg{y(GnJOoW_Y1!PFNzpcr%wH(my6aakIM4p z_o!bqU1rbG_0o-hmdgG4H^mdFIyY&pcMz-gbgf45q6Q}SESWQB>5|HMv|E_FS-qpt z{x}-&I?u>kL+Bw^g*F0SRPJ`8-w339LV-h32D=>GB=ztT!0TAFIIYxBO zkDx!qyV4Y^!j*~ahGwehevsRyI8n)`mU%F7{VqL5)yD<$LC+u9x4>SP1PS*!oW@$j z!x8tWm*J2#EL?os!WF&tbX|U%Z9rYDrR|vKdhCy6N(qN@WHXe-OIK!An`q74LApu_ zpeM{6=DJvmb~xJ$0>TNDSs@cmlK<{$v=t#&yBv zZ~N;ub09}G-*3m6Gq+tvmMP(#JUdD z#weI5s?vo_E~Q@EiTNt^?JLE}zh(P8{wE8hBGg(;gk`=r{|`^!9Zz-N|Icx7WRsP3 z%*e_nR59cnG*JH@dpzb5O;6fP^aYFJjVqK5Y9c{_`HrRuR}&3IreJwpJ|$d9@lm39 zn%Vupl z5T}`Uz*(DVpE=H(c&j6*_U_+5E_*5O;IW6}E)wUqWrL;<1wEenMhr<%Rj_|t?b!qA zw@3-bw=ZyGk|*eJ*=3YiSO5nhJn36LVV4etav$5!aw@NoqUN944y89HD3}93=G~&a zV{R5mLm2Pupfv!2!&3( zxs?S`^7j$cP!96SDtIL?=~VlumQkY9&nB2Y{Yv(M3h)jpv_U6nTs8s#Ji7SicFEs( zO{1WGk(wxwim);muI;3H{rinjk+_{hmemSjj}P|pc0F#ddBaAbg7gJJ(#E72FlP%x zeLYz7cA-QoBOQOIj8`z%n0i$}Y|C?sy~>ZtiHWsM-h-|`}VeRkiIojlCt%If{| zCHFIN#!8J5|Jg04I@4!&;L)Pp-iGVSo`b-G_TTePjLBU|X2kw$i%zp&1vBBeef%>A zzV3KV6ST%py~Ai1!=k~_c3bFMkt*dX3vuQ9RBm_AJ?@8!B&)7nVbKfUt-6(n&Z~~i zQYpQ9#<#ke_gar=%1Jjzqn6*M_BoM@&mfhYby|E`!NIt052-R8KsQy-(+dm^`6g$0 z>l?Gn{SlS9`(!@Iw6Cmm`zLHQ&zNE{N0fx70ZuJ(d{lDZs3~c(C_cdj;?Va|%d-O?{=p zD3GaQebq6RV(oMayPastpySg}JT-F!!XgNg@!cpZKJ5GC@a1e)#>1iWL5}PKaZ08Sn~#S??75GW?50*%OTcRdXkFR{&MY@_DIkWlcVG3D!3fuGtlK z;-80{sJ&40Wa?X78A7EoIZ_v}@<|uongNvf{$g!oQ+qQnQ8jmRv_p?#RyP45U1e|v z;vg9xiL>c`2K8Q3cfQYP{dgl~wc$Rw@65No%kiYEu)zC_(b}W!aU;j6Z};(!L?6HB z(%wyQ={H)3QkI_=m6lSjuw9kOyLZj)!HuNb>c3#il4M{k6N;h1^WK6rFZNf9QK_M6 zb)YDKa4=J!M+wLKalT7==)d`TJVNnUr;c-c(BThjU~*hnptJ96TPd{Sub0JG{*M5WRfZ4g}IW{Vq1nX$wPH+5`b5sFqh3Q7} zjVa!A@Hdn!>hL48f@|rdgi!IL6)#vbsM6_jh;ETUDKKr)db_|KzjKeK8&879g|Sjq zwmaq+GkH;pzr*9!kdb7sdhGVH{^2>^$l*TO4p?%Jh&l!Ra%@+a+DIJfcPvYAgllR5d;MR) zevS5IV0jNrNK5f(j#$5B-zh*zGp{dBvv9Pv18?ERa-pw=Y_g_IMq;aW zfy$5ngk&FNqQvSFJIm-X5O6JdylsOOKgRe}VtkR66;^eBxxPr;B*6T=qaI=arXc(jHKO_?*Bp zjkMeAOgtZoC6eG7>RT>*i)~3!VlX31%|+3ue{JJz~!g-9&jx zO8ffz#f#8#hnjR~;Hm{#(d~)02Jba)$WoSHP*MzrOcVKy@Bq)xbwM(fw=oHq#dEVZ z%$bs$FG<~a-}9k2VbdJQwZPoT3cJWtpC12ycRKm^!R0D^@45T2Y1mV`b8WhDI3-%U zA|=}QNeU|<0ifMq8D=`Vs6v1QYcG;iXWyB0NY`Me_IgUkb6M>pXDXp#n101#dqF`F zJ`*9ILV zQuSO%m8rv@t%u(TN47U%lRV*S$aSp+Y%zA(UEdLwBnsce4)Jp17GAOKRRPS%+l0pu zz^Zk=k$Qy4k^hFOPA~|4@2r;6z_vy~Hm!n{b{XSMwUo_LV%GdP5rdUFmCzIT$@GU1 z$dJli!p?v2-vsvTa)-I$N8McX=V(i?<9t1fUc>%)Zdmx>r>qR60cy*e?X*{C(8Hm) zyKuribB(sv3;p&Y=*73tURD)znQI1NhxV_CQ7EGrxgU?QI8vuh`IX1X8`X-+a8qPr zdQO1a$Td`0LgXCk@XJ-jg}d43PWfUyr}F19 zdQP{82x2}Ba3(7BEC-5qQA+}CmH4P;;oES?F%Zis$7xs8(oT8eK@Q*^cp`qECe)Q| zO6lICD^7%{Srw#=JLx!lCgj)VJlJ^p!ctGys8^>5TQyEFqmLM%Y@$vjK<3{;76Ug- z>O*5Y%TLowFegwz1KSuQv^6cU{2{T&8n-1K2I=!_}UtpAM4OK_&1M076FV zM3n%5vge!CLXLN}H2RoC2}FS`In#0nIs)(ITqdo{zS01OePXN9O4!5un2|UaO+n=& z%*IvPRaPwq)Yd4lXVOy53M=ryAtoVLEVnWxxc|Ssx;zH!(2+zeqfC z-r?C9NCZ5zjL)-~&VW{qI|Mo?TS+zuR+98YFY|h1In>39n_4*O&SSaM>l6o{0I=ER zIyu(AJ^WndRoDXpIrH+ixJrK-rmt^+)KvQ^R&rCX@XO%?iR!q~|IWNtc|hZ`?NY~h zR2~!Sh*uFOT7D>-I}cgh))k*48=|nm?>4L7UA%It5^np40ZkC#tMBpA)Jc$As4A@? zgu8Eowqr_`_&U|bJ`wI=4dnk{{S*HHU8V9SX896&K$p^h*S%hFY52p45ce!Q%9&J+ zQ(-D>fUdZwI^QUV@c}{4ib0~r{~u}qxnOoHdqVs}lBQ;jMB?0W5Ah&2_1>Oe6|B;l zxaTvkWWZ=J+I%M{^r2i|YxcpTh0x8ihOHC5FY@_v!J4HT$)2Hqx~;|9BYKJHhzz?O z8B0D>K1bW}OXj1QAVySf}8Wk;ebW&0QOWQRKZBvt$wYjgLEka{udf# zM|+C}fSl{$0a7alHn_wC<2{{c9^Z_-H(O^ws+)+8U9g1AiL zo7-R98l6mhcG7DOA~0y8P!JbhN$urvmrd^zGpiQ3cbbn`=xO?YA8pnI0eENoa+QZ% zZN!l%<2$pa^P;nPOO^eik|fBmJ$8|*lFO!LDa~BHYd=_eF|Ow@aU**~d1 zqwJ?I0WzsKYX$d5vD!}UNgjeqjVkz*!kvPce z<1BpYNuBFt&v)FI54L+{sZKq@RIO_D6|VYXsIO6ys&&Q z6QpS56s8j11iIcXL);ru)9PH7#aSeE{hLkECn#VP0d?e+^WKj{`FW2GL~qMDp9+_M zvdoTx7`8VT5YtZ33WXCticozP@NCA3M~Z6&$tTH*Cvjg(Nb_g4*4~#U7k0CUD)g@X ziG0*F{uh)6v4-V8S+@)ip2BL1=u+fWY%j?MPfLLj}Q|LaooQ&NR0 zipbu1f+ljTl87*&mSkTN95|3j#(RuxY+;o4IhZp$J3qk0E} z)S7<_{2CFP1+H>>QBEvDtc%$(=3w)a36c!b2m?Cmn635+6iaaY#VPWf#pD()0&Z-~ zw@pgTL9yEa>O>Ulbu7ngb6(GLm3^ekd+#p)JF&NsnPiu*m&9@%S>Y|w`HxM~s&*#;P+hqhi4nfG~$iByDS z2;I;BZx4yWDPqn#$CJK1$#zq$sSKcz6OOwfJ+|!;rjKzSZ-DRrvx4Iu0pGTbB7zon zidoPfyz+xe0FysICo>RDruN^A&BAo6y{dkeHP*!=@BSys%Kh~VwSBdb{i#AshPQ}5 z(~1K}@_^5JKhG5?z< z)LR9Svzj|KV!Egp zx+&1dPP?ZdbegoSGTj%#l1`LA&e|N)6JxOXEn-$(l3qp};)*s%#TuFfS!@Q|xLG%u>R_y$8SLb5(`_-0`$Gz6u6+RmWlvvO z-SVjl$aXR@Rs@faxbsME!WVy6*G$=Qi;RKp@W)`c)m%3Pn3A~Q%Rhz{sCZUF$0<9z z4jv0t0*c0j!BNUz8vY5pV@QhwUAe65gaL6T#YA$8giV}mKfB6GE%+OHsRwAEa^tOh zy*^+h?o&+IeUZWrR+Dbb61ILnNpv`IM7$Oy`z83ww{*3%_Z;PRUbMP|ZihKMderBc z?8gTqh6QCNfc>E7J)%4OaIA#BpN3cpHX?N1((kDM8!94zgjP}$dtseUtSTYN!dSTt zobH8&hMvQxuC#pd^<4Y$5}NjZV;F?ee4j{oyam_zJ?ENm1#GgA?y);Nh)i8-Ln9Xv zZj0LO$464vvAnNqfGAfTO}w&JMmo@*&&H*(V6XRfe(}_qy8}T}Yd^aCD;C^kn z>lm_EKiaqEr2VM!p)2)rZL>^!as%Y0{l;>J^=@@K*W1j}2#dX&bE#p~A|GPZjlJrhJnZY_|mQSczD#Ma`cRP$+bfnuCPQ zHRIKBKiUa<`3R0E7Cl=r--GApKv&mKm=LITTS4R+=0f(ltB`tgK`6Yhn3l@zNI#jtDepKET=kxLy z>A_&bU55C3t!&>eaia`nbYEw#G6T2UDH@j(pq9}R6w1;&%GNV^=J@MNo%-m_NL0e@B>^@P#pCF@5aOdss_-FJ9*3$OxRakH@ zTGcJA2hcJ2yNEhS3`~#*5GNpH#qL%w_85*QE2;xktGF)I~0` z$%nm(_cd{!K=p8-kkZ9cQ5QE3qJU0l5_qo}Ym`H_-iFAST9o&CcvRnfF1@SfcJfI; z@X)5xzKg|)Vs7M>sYO&LCN_)M5LE()vXM4v)@nafy6xiU>ekb$1Q~L@Og0px>|eE) z3f%?;3!imB=Jtz&v|Sr^w1`(Q=|ol4H6kM4nym+mT`D-y!to9W(R+^*xI?01F095P zuiFjXx-9D>xaRm_Su?@`R>q@&rJh7J?a69fnIK0oycl9TVIrWJa+tfUN7->T?^cf^MKciEe;7O_fYCw! zhapI^g@J*8wafx->Stu3I?c0hMc+FNX#xAk=D!=Iro2j)S%Xo82;ei!YOQgDxgZ@InVCj8*Zc!?uQHIe|3bdxCdRP*3F@7#eudsmnU&*xW`4R2b>BlOwrzW^=C z^9OQ-zsX7#N8V|3=9H+R(aA9F=Q^G3oixQp@SX7Av^T4TmiU!E6d(1jIE5QusZup( z>jWlz!R+L4dcs#U6ENTXV=9~87c=^XbsHVMvFoO?o5uq$K z*uZfrpSdM*XW?x~)3A2Iw2ePOhUGDXQbo%yB z0BINB=V}01xOzsl z*-f_}>+BD}_+sSxsy@^mL+u>Re9eAPxlbVP7{WeIz7ri(#$bh{oQBwz48S7sp`AfQ z9F1ft4NP(?)@t#5)pbqlae_#8wQA?lx1I#=S$66UlS6g>!Wt=S5q4dp=uo*kT;XTf zrtj(qSEpG{k2FR19(%m<#DO3TH_yWGb5^QL7?G2qFl8AYtd9@xku+aQbor<=5tYRd zilFF7i15`j_=JzHHcz{M?J`f@+))bqC4+g)vHK#n8_`}}0wC)9icg-22Ns`m-m8`B z(6zi*!X}l&NwzvBCukSH1U0<|$*LgEMNeh;utCwW@TJk>V$HWqxvCLKZ`~J!$7SaU;LOwXv zYHFf8ImGwl1uSaITgfNp9Au1E=j2tXzwWX`J3zu6t$$lWaO>|FCu;lo3i;Rv9RH~E zR(%-I8lw5T_t1n&lKGbqE6>#~O&(On0|H)3TI_lR!6~KmHJOyeu+~zDC+LFUQxNH_ zD{bG_if=L%=LVxBSBIB?^bk7SHM@=&JDrIebNE<%=|eq_&P|oT=b*>xss^Ilh)@Ck zVhOjpov6RdNp#kSGSgk97Dz9aS0l*MDrpT>vDyL|xshQ|{Y$GRGvwmA4i*KjO-?xy zHcrRre^f70Dv7IjYZbdVz{8<|Agve;Jiveta6I@6?GRfND~)feeZ8g8$ z#PIgsmIJ4}c2=v+D|%(fPF5v`^o;oL2WC${kcdds&ww_CF>MpnQW%;`qi=ceY5D#e zFliZlX@W~IdPRyO3yqq<5(S!vZWk^a(qHLE=;*{`Tw)K%MW% zdfvV|mJxN>N9S&v%llDC?G1IoC9LC^WJtxMtao$k;O26^;05p>c}m1OC^#fYHs~m5 z8o7wvWDi!qKHOZZHGa(Ff!0kC$g6dCZyHEF3H_*a{-L)xQ{|=nXmAX)RuD*EI#KPC z-1?nDMqto0csT=p9B1gt$;R>wc}VTHP0#Y`mW)#czMzN)kOC(YX1<5h(LTPHu`Kp! z4XSU%%@1y5b9LlPP!ie)v@*VA4LrSTu=QGn^$#F~RVizMYxEqsgSvFsHPx0gkMEuYhNoU4m-n-$ijJcyng#UnWV)z*0vpboF+2%UbTY1pn%XIxg zu{z9>RBreS1}4S5GYZ9)&B1m2&!4`vqP9o}d7FB&X^_6>fiuhjm&N`&0pf5&15;s` z&o^?vxj23z4tJ>xtZ7r@3W?urTDRwl<81eS68cBEic3h{D}vz6$$_Ob;1{&X>1!po z|M;aSWyKnEB0uOe_ACUxAOm;)J@fCXBni)yP#vL;bSAeO2jWashA^m6NMXStxCU3u z=W^Td8@E>>xk3&wi4Wu-a9Fzk=1A{<@TOHQ9DKr7xi6$xg;_iw1Cuj}O62?*ST&bf zSErfQt)NA}ROU#OTv4a>>4$cy0^QQ&vhR~?boQnD_jthJLVz9-CFGzed2v#}pcQoj z^S=DYID~n`G_oY>v7=z^u1bn)m(Gy)Dw>juwqH%W`*W?BI7$=nw@>foI}?#{kpi{h z^1ZFFGMO~=tRo11Ty`loDs5yboel>wK2i13DfK6ulTS(xK=38Di1R>jjzr=3xpx4u z5DXKX$^A>-{^bOdsOzy3SoL*TneVwBy4}P=KsFBu?fjIY^+Y?94V1Pb0S40|54(lt2=I=g$Z0@ELo-5ncbf?!<)0 zIeJ#Ke0L@w@TvdNMB^(E9uQ>Q1XrC!MtpdOW-j>3pSg1Q?#!yB-X{%RM#j&-;pG== z;6I#jZdupiph?(kke2CE48z#dq2xyvb9Bg0d^OvuUl-LIMz&?*=7KA>7!-sv@HYSb z6QtE%e8#_ftHSE=elGjNcNZz!dSOT@rP3;5cO8|w%Y%PdQOAJ3GX+N6-I{kCL8QyJ zJ*c$^L!FWf1^wU;cv@TZ6WqhX-3yY5!6vabLiWM> z$WKmfC$1HlFTDEs;*$9=cpbL-iTXSS)X!=J;>}T7x%^;IW0AUtW61Q^8x_%Y${0z3Mqzl;u`QL6UUN=vN;47v!qzhj7P% zdvY~^bfpy`S&noeKmK$p=XDvQmw84Uy6OP&Ja~RX+_u}F8-9$77Jg~hB~o$k9)KTS z7}^Wmg*OC9|cM9|EhTC;czL)LamozM9$=}y4y#T`l2$*^IeZ8b#JcH?~8z0WQbST@DUy2 z{r^14s!V!&Ej%T2%zjz)<;?^-6GjR?b1$=%6?=7T6s9MQO`jxy_6g_ExpiUvz8)7S zL!p@8lNfTEtS>+Y8Z#t9_56M|w5gbZSwy-nptM(i?qPcj3m9+$@MJ=(%&=e&Yn*!>9Ep-o`Gc5!3*Gy_^jZa!c{X%}QrI?Sj@Y3pW4KT#=J zEW9;YsgZaN*>I!c@B>p1v^WCP@l9+eC_w5)5%d<2@RKbh{xT!K+(J|s3Sg(|T8A}_ ze?w3mXDlh$^c{LoVt(weM7p1RRBQ!8_*&4i-g}WrgXE#(aA0nQ5N+hGUGoWJi zuJu=lGoXXWd)}o~gP>^xaUhO)Yf1C+&gC*16ll}-cVOkWpjR02CQRk#0k$)H7x}Ia zPW68F4Q0Pw)fu=-OlVmh8EoHjY%?+Q8?uaf(KXq0@p zm2hFc3ew~KDWW4G_`w*DXB5FPJAr#)m!i#X0hbGSk*a3^W;QN%AHH0p_rTfPqm3@u zgfp{BX!jLlNlC@=?Ha(ITv-^P9JSu~V>wH$N5lTMdq-i<`ELmeDst&OT2zCAyB5Na z?S~s3sE!q^8WxlEgzb7NKln$EXsa{H+UGpp=@WW&t?czi_(2DC%cUEqgHkpHLi{EZ z@9wpYdj2k~F3jJbmU9MWifwNOfJqBoJ8aJ<6u_nCpW7vm`8F7T}OEk%(oA0f5Nzk|3VPtFSOx~y!v>Vzf(tJ_joGJ~S?Xvh8 zA0qc7fBV3Dp%#kmqCt-J{H|Wa9WGO&`}W$gL2Nk*W@w~Tp!w%lAv?O>i!bb*o&R!S ziA;eI!7fnuzNt7OGvBeniR#KGV7WeJiweP-jE|_8{_xF*K@=&E0;7&yoX zdN_`%eEd>Hx@i-oMKD{$M_(PTaq1xwlq+NL6{;`@?0g#|lGb73I8_`P+}UVLyX_y% z5uYSkz8N7|Q*r6N)~g}43`@{#K3seEQhMt{{Xzy)mgmG{N!BgFltjm{J3p4v-%fuD zfrPII;0`Ok<#Oc8?cA!FC!c`3EyD2@A%zM^2@mvn{JU4gIfw*brL-9pV{P&HoMjF; zR5oTB#xE-{YA4wxa)%FoZluMp;DPw4Un$q(u_Yo3ZJI1kBzy}OkTxrdN^(zefCs%u zWur|-uiM_f#__3tVfS{8zT*+SXnZ>LI@eUMry4gG6W{0q2Vs`#(^3~DPV2mxE}1Iv z=N!jv@t>B|w;4|Dux1QiR5Q03Pdd?k+4xM$s)(3iY}$iTMb;~H>!u%1>%=SWYS7u5 z9!Gt2T#OEXoPx0@LmdqQ>GqnR!Cq?e3&%Iv_kVsz-L(d9Y3B6tI9$59hyc^<=`aM! z)hsyl^=VK#>?R4we3eej%0AUR?jY8<`*k!wGUAO^(7Ijidb-QqWY@#Ro;>^~vo zr$g=Q^w~hj&I9AZbvoahB6G`8&!>o;1y^OIpj&u7wQ=oFs6WZ1$Li2JiSLz!Lf-3# zmF6@%D#cQk5f2D|%+5L28&wO+Wlpq*PrbQybCi|ZuO)CbXXVOprnFw_-99n*cBY;N zl@sF0uHaR83h+uAAF$dNkYKZy{T8sVm6%o)iJv~7Y_LEe$cUhS+cdaeCwb?v{TQ`0 zUy!cGvG-+27ISwJkq3g>8*L=#9LZxyQ2{U_T}?*UB_jWg1h#9KZW`6WOns;HS2daF3KZYE7yF*LTf z`Vmp3LPo{8bLpg}w5^V(lQadQN&ABpN!(D(W{U8ll*wEjAPJ!_rVHC0$Xcx!4I$6H zr3{-}1w?q;`Fo*G=!zw170`yJ@FciC%ULOAB=iahhCosg2R$b1;0k^}HA>RA72OS2 zb0~dH0wm3>D*Cqh$Z{CV?kR#RVhU3AGo{=snI`7rc{}t12b_s=2b87*4b{U5R~_X;)}$xXp8j5=;8uOfDP z;RfGL!$SM|yaS`tzj37^l{ypIGFhyqRzbd|e~zL7*1m5EZDjjKj_i|H6(n8S!iZC; zN-{2=W4TxUyxC(x(@u*?q1|!*xqX++6IQZM+H&gWKI`~|nV%05r}BMQSnyEX2V_OW zm?-DJY6O{ezTBMEu@5gcZx+MA6sp>2V~shUazlDhucq*9Z|)1$izEwm5;vrqlP4* zpoSa4R(lE+I(?&uyLd3em!qc4JLH#`f38z^+E zeV5U!|EC3j;VW_`@qWm2VHMABUwi*Z_gm~~O?=oDXRchBd&Z2vS$%`(vD0vE_p^)c zGe4xLoKOEk_0oXbIM?(p^e&ihnFPP#eIC%&&dd2sI#^~mqfH2I`zcs;$C}+t&)jUY zcH1eOAw_;Wd;=zTeg`)GQG2W#%~N?kjrQA8R!RG+$qq+b=ekBK8AFjx==Vqb#HGG^ zM-eKE+6D|B6TNJQ-hdY|wP}M(rNZhdy{N%^&Xfhp&us>rrEJqwro#N!jvk*-YYnfI z-9H6ZkL@X=(H(EYtsE-NyW@;m9XOk9S!<%>mVxX1;@2Is&lEmT-AQcvI|Xcx#Lc|R z#`^%anw}*f@~GEOvb=8*2?tHVC(0o$_zEz;C#2V&1NtG8` z+{#w1Cu2QcrZPf`x*L8+P3_HVXK;&YjifWC|Mj(^c(p}qnt8j90}jlXo9v2@V5_m= z#&25$e{Q4Zo$Svi*-c8d3U?I@TrIxzWQ&Yi>+I#CaojA-K@@c@?wp{1mw*ZO5=oQP zV^%@u?)QcgXBPETbiJ9aI{WIgB)2QH-@Ct%x9R3x_B07H2%|(pj2_87DSh1(=aDkm zUwC~=S>eP=|NX`cj%ps7Dm!Zsp~>bBM~U;kQj6LWiCVQ6bJTnx_E`h>&iL=+9Iqky zO#6{|zo>$EL`AjKfXW<3;v*`_36Fhk7ifmh9Im8m|3yTm5!=+g{mGU#9pXF(D{b7*(Av@h4JCq$I%mnMVSvyEXo8&T5a;6`I0 zwYmH#H6Ydc{Cvr(HKZZTS8qZ>NNW^+G$wClk?j zhi54wiA82-%(oe#V)gO%jj{KdQ9m?w=n{Tzcc35Sx)-#^LoBAoh7!Sue+{mGf6U z$l($E^&w?3YKCd@wut>Fe1hpvZE!H;7HB}Mq3Tey)|~`0xUf!srZ%^@ZOt1dWT+5a{$AUEQl#-ZWbnQ z@svY$50{Xf@bIXlZ^slt9b z6E+C{W`I79U5!?%yW}SYa;xWP&-NMy5S{295wT~<`nhWX(|&}7$L%!2;{@?Hq#FPd z!_qEB#<8(MsBp#zDtdeHmh+Mj=fq>v%`VJ0-dLBYUj@Z1pJF95a>SuJz*&$L#9F24M#QeSf&WWYfb7_o{|_Hi?#c#kCWjjBjCb*a5wUy z(VYiG_jJXbU%uj?Ah`?@xsvDCYTUB@!GY6>fDDL7X6odqW|Y@uV(orcH1GG69!Slu z&^}%U7?&GUULi*Ne#}4ay(c45SLHYj(y{lGyLMxc&k~w#H38KrSJT8;6o9n8|Nfe> znoajLx^iAg3$vllEg9W+ygCoh+P#5cfQ!ZMKJPkx(uE;^cfYSjioZ9KoK?T|5>L8s z&$e9$pvE>G#Fg=_uSR3n!}09LWXl}4f{kg)!%4&Vjh@Ha$H?a5icT<`wPKdZX>G~- zM0Wb|?EijOJkJOO^oORWv|eQsn|P#d&KCH2mx5+!Y)en-dIz{V_=olEJ+J$zk>PiRr1ROx_%_1~Y~I zAgNow+==cb$4FM~{9?F18T2xA=~G#gXp4og9LglK=)PTaBvnH&o=1@%ZSNDe^e2=p z&V=MX<^FGBJ?kdV`}@F(CgfpoV}|@zPx}@!TOzSf9;6GGe%!ee0Cfu#8vG4u#C+Vn zzwM8gd)}09pzzOqD8b^`*)*8=nZ8vsa8P=jIr*uw<_*Q-@o0)iw?Ghw;tlBm(fl2k z3HJ0cwzL{<+R?mf$|je?y`cTx3EP(0FmmCI{fk5UCvHq%L4cQeg{Ti-K0&w8)t4g> z-|%Ys+hF6I_KHC)m+YsBcDH(6Y>rw$ZUzsLQqG}Oi?_uDgtZEZQq3T~>NDK`Wvm!+ zodu*k^^TT#-upl%32eMPgSIzFmP+LUTQ5?NT`=3BA!s3dcHT#gH9JL+>a*%<#dl0C z;~%fw6fOdGgMGIRXdN(=F)5-JX33ZCeZZL%unnDyJ!AQ6)mr`aTIYu2O??F;iy=@) zM*-x?Y;jf;Fww>LSoUjy6N znEj>FIdTaz!)Igk)^dUXZB9nF!gM8NKUb*!3>hHg$0CMd)8Th;m7dhQ9pG>}tj+B8 zh1qLm6^nA1kEqp`x4{KI^XpFXBaYlB%<7Ewc}_~7zUA13F)k6%^x)FJfidwl8?laJ zdDCY&>bkzPI}8k-<_e-x@~_H^XZMS{NjQvE=46zKd%g&hfW$#goUi2_7hWtvv6 z^dCRLDA&UoHyVTECh*lWBcM4;x(U%(RQT>TciN7Sxe%Jm50E=vzps*+Jla^4b4}~R zI^ObY-wn@kt5gp={n6tTrOmclXN~}Q@70m=qr6)PM92}awCz$c$ydioF>T+~CK>6F zmaYCECvO;uE7CrROw3qE@~q!A>@D8`+{YO{GfAEd)_>;6`;}xkhCURL;}ESq{xPNP z7A~hXH>W%Ld6*8%t3?gfsVHb;D!8Jxo%mSGpc|(w~=Kux@EyEP%?{o#z*vpP-WFUlzdS9~3&Y_rL zO9Kk{mq#GydPsz{)Pjd;dF=G~kFyEQ=)`0iiGkGt#l&TQ$j$xBO>Pz&HBUwzir^ujsvQFv=>BoJLim+5Y zjW?rhulm5_aHO=#Av@jzc6BWdTq)YB(yB~^gykGaXO}Jogg;Ve{z=PhAPU!DeuxXx-6?k(~gG1njD9D!D%R1Mj}Gm zZ_L1z0kM+_om$uiDnX6lI2%j~D3QJ;L_YGyN!h+dj{^B+cZNp)Vul1C`xP%L#$tHGaa;Bm zm+REA?`Y0e6+p=D=5_FXT?YXw>fEwp`!`l*jBf}gr023tAZJ)50v%I{m4Wdnp+*k~M z@6G#*e{HW?(V2`r|I)&1v0Qpy9iQRLX_ewG3S#7|+QzpRM9|R!(Qog%lxh(;IAMbA zSbqA@?$o&?4XCGHU(gzl20tAxwLZ^;GQ-C{!exjC_tzBO)2pRsHJoviDGb;Kg9D+G5{Qz^Yf+TY4lTD za1o?O#jz_qKw|lRQvnntmBsb2-Z!YI2@E$k7TSdcFYH9&8sTw?XA-B52a!%4LE9rR z0(21}MgW`0Q@2j6>Z#tXTJ*Xw+(G4}V%!8y{_7ymogVOr%J^}XhEg|!y*~qD3+QIg zB~&_~*oZ+}0Vj^i)zH-l(p1RH={!HW!~-W?Nz@Kx1l)QE%3?9{-lrRT7ue$dorD(X zf`h^v)~Ymr0n?YW!qDlINZsSI9feD{d&Ti*D5wpo`@e*(V2l+1&0O$oqow1ASS0MD zxI4yTRT{LIR4)oGeKB%B26tTvMXgDOIyx1b(?%ud+v2jzgKzdZ8v|?iC(p{1-pj5{ zm}6)7o`a(6Zd03X0pP8mFu9aJ^7*=>xn@zj%j*aIW$tqS9$zetoabWH4JXwPEbk2s zmev-7=&wEjB8C>Y8|sd8@8%X^%hrHvRrm@(gnG7tAT}&&Cmu?)g)fwPgcu{^L*gpY%pWONNqj^r&6;uw1a>6_e~{0$EyuhtNOCa> zwBP4gTRZb}g71J+h8P%(FO#z#dJzek~3pM zsAHM*+2FtpHUDLQa7T22fR`nRfI|g+;opbagm(=IHPovM#-}A!l$Dv+i1OXsSLGMt zBi|j(wV}~^jW*-(R`X?7BoIg~me(R!3`SaUmeA-St63;d^3>?KHz+kT`w4FMm0%Uc zB;;?1LknZ@)=t18e!V^Gry{QuWuBOg%n^81eYy5!*=OoL(MH*`qUN*Nu>Re!lrm4` z*64s=v62{7_KpvTqXxA7xd=i#IeQ*&wTKVXuQI4OUE;=V& z{ms@(5q#xoS1z7gk1XdGHs0g79e-<3XVuVr`zx za9>yNB$4yYH~aG80!NZj3H~sl3!&GFTAosA2ixy+!(d6=#AWX9D81X8=VkS}-s>Or zf0sgZ95*1a7bCwP)V{l@F!=6=e83Kaj(%l{9T_@51RIUW-7s}_tFtK+khW>LdS?%Q$hUjPQa43qn7#@k z%9@jZI`|{2(>d@qHKYB8?>mfJtDnZj z3aIiNs9#!(ZNL}zfaMQhyy8WqH%$875h0}1Mb_hJ+H36W9sldt27cn z?$s0Y7OPC!oC;Iq0~GOz4uyy740fQup(*(@7OS-BVV(3sIaDQx;5MPx2-Wx9+`vu- z6SO;@cLt__B(yz}I%cjtGV);1)`X+s*w>}^s_pBDQrz^y@_4 zyiwWT0b|U=e?D}icrioyF<@-SbDN|_P`C@V&aWv@hd(E)oKDM@w^wm7Oo8P%)X7aC ziI->e5X}1}j7sI3lFokqg#OnsuwWDUUom=Vb~X2;k!-n4_=|9uCGZ9fU*VE|4N5mo zVRxhwT)ZnkGCNHUwQAWB5jBaJvpgX6kng-Rol}I}5<=wCsVxA;WYvi)%0k55C*?MSm=(yZbp3eagKhMq-F!b<6K{)5r?zJ z1Bp78d4(Xb^PrR-I<^Oh~5my zRwI5O2zpC@8I_SguuuqC;~tZGHrq>l7c@Qj|ERhSXfF3Y&fnkOd+!-CGqNJHVbAOt z3Xz$;$<{(fRtTXol97>7A)~T|6lHHRdcRNa?bbb~bM8I&+>^&Me&6v~xZCo`elO(a zS!NX-J+QI(OkCAv*OjVQxleI4CmDWu69dDCO%-c&0z8y16NP7A+d5r9 zdXn;#C8c^-wm8Cp{NQ6PDO^U|Pm8zn6J#LtLKZ-qK#@8jTh3oc&TEd)PZ`i*muUWk zR_0Qy)@}Ms^x*5u%65}U|v+`H0aF$jqBwP~G=>sz*uF~YcTbXisq8=V-1tlW^E7b$n zvuXd@6ZIML|6c@R4ue6ogP<|I92mIdIYRohuBWQHNEuG?9pm4?A>TuI3AlSkq2;NU z(&)alK)pWirwYD|+d0bOHfuzCfvL(U5 z8PLwH)2@S1gm_S!Swfz|@H1|p9qHvZ>ePWKrHq*A7en&b*#dQk8q`d@KcE3;jhPD$Si2olS01)vNAbuj$i(R3F1Y<^G4)V?J3?bp&ek_2X!w= zhy6_oyE_yJD9JvL*rKw%1%|gJrwBNXs`AtU8b)U9o_zm8UDJ+){#RH`L~vVoy@1p5 zYklVt?65h5VVQ>Y6$wltm@~(Pdi47dad415lM5ay(&TT5!kBPDJUgvjcg#dqI%~ip ztPX=x)QcK|Y=zdW7rNMvOUo+6yjY(AAFtW02e3RrG$MHZuH1no9PV9ZtM(LTlK3b> zS_1ngPAV;gXvNZ4g)b1<##f#-gXp%&OgIj-JZUlQhf^J-7@LjPj%FX)Y~pMLy~qZf z0A(ME2tCr=JluYNI&hhVOkpKT2S^YHM1itFdAM_3{ZOtI_GoNCn6~jC&u~hqQPSf# z{DPz`7;@19e~Flo6m`wMw-<+_!aefh*Cm7Jt>*!qmE;+#yH;uWe}CFGvMMlQJ2krn z=@3*A+f)`=APBk%l=ENQn@%FO!mS^&n78ir*QzomyY)LGIh1iJ(^bFk9C8SgqR1n5 zGTmOB6Ev;vrq)iaf2@-iE?`$LfftUI@;uL$ZJd~qw7bH_GaPJu6{n%&BL}O}iw4XF zE^1*+sVbh2>d)fE3O8-(U1$NiQP4WHi^Z4E#uS`o$G|)Q0$!px;O4MlFa9Ob`gNM9 zurZS1jSrD@x(Bo*5mijK7$xQCZRx9vZN;YvUK*@3fzv0=7P{)_5I#;Tn2%aMwcg5d zzIP@hxTX+U$Sc^~N#}4%b^t$Yo##1#B7~H;?<}hNDaSALS;Yxz}U#gH_RdO&&Z66q`6_sF2OLEr6w$uK2M?kYLb zasYdXJK~55G8p9hhNdB2uZ9+RjKP@*Q->e*j}}-FIhDI<&`eLdcE1hrnypopQwA+{ z^Q*rRwBy90Z-eGV%3&{*jMQbvavYT0*3YevLn@DE4_lQnC?X&vjE1eA$dN6*;d7EK z+SJSGo02t*U1}{!^vGfQ4$H@#aRh4zlE>mP1N`0a`+$@G%cc+ts)hS z{shi>c%iW*l^3N%&MneEzi1^eoW_9G!50IGj3L1_UZcoksW}4HkV6m5*Q!lXoDw>4U-d;P1~{Pl`flHoB{7t{@a)d~|s zLR<9`bT9jGqEtA{@D)t5BqLvMJ$V17 zw{g7K@m&tPd= zTlgj9MgFORzaI1&Q&_B00C>n?Z{5fqgSH_*Hi6?3c*Ic;1wM^Vu&5@E^y_>Fxhn(= zK*uci(|WsSXZ+ZdJDQZ0h2kQ36sa^Kr8jgHv1I~pA`N;P$DA8Q1V0cVwPK{kCP=m7 zI7UDyV%n3tim>%yAws!OP;?^(qg&#MZms9MRy#g=P290>tLKEv*yw44{^7njrguX4 zynjBLz|hdEN)5Mb6kp)Cn&j^Q$aNEt7s-Jk&Kz8Pc8%O9@hYl^D3&R=4tLlU0yzUt zzq(AzbC{E^yf}U6J0BLm8tARTsbGA6axLm!FuKYaiB{l_qIxShEn}e znevPtF|4_;^8++$l(66|cy?Rgi%LWO45TsJc}+;M3H|2+$B_%fqFdO4jI&exYIw~p zMA^MQ?Zi^CAe(pZkegodpSj~k5MyWnM zg~Skbaf2eZ2tomna1o3+A1mh~Rw%3Hb%?1ogpv%2tFs0yiGCm{U_|YKVc;wA5NXlB zs_|i@;``jmM+Hy9eeCBT;E@Bog=uS-#=gl(lVnW64A<#-{*@hw2n5!uet-ZlYI_kx zhLk{t(DthUc6A&)92td|aLUhuV~j!91OM-jP?sElRGzLS8MU26Cwg}JT6aa%PzKo2 zlzi8XexI2mkITXr_3<&xcc?F0k3{Z#;yh5eKBrNW5!xH25#5VIFY^^(JKr^nt~pD~ zaC87kB<^y8hm(-RtCvZa#OpcJ0{#;v}QFZVt?2{hjw&gnz7}L z4tHm8CqWU-uA3q>$6BX6r505}64VHjN^Uu$!Pyq6*(e)Nl27aiRj~-yQ|=r=({ly{ zxBNB#Ktmt^lMLl^F{o7(en&7_p_=j#-*61dXKwy#vSE4hve?v^c&}`?@6E$r3cC`| zzEvi%HWc2HN0xgAjMatYjb}D{#7j}L04|9ZqjwddeMXzo&59Z@ER8&Am&SHw1Ja0b z54&P*t>3!sXK8~|NF>?U8;@jik)#wFTKjjf{T+T@B>whvKB^FQD7J7?c_g+*9EF8i zbRr#w@dqwT`cm<&TDrZ^r$L@p4aUJ9mVb3zd-&kncH%h$(9#XAJ6kKO<$imWvVIEp zSgY^c4&7E+!B3X*0a(w08J5mv#1K7(i1=fOQ)xjYL&oWVlq3po5qU^Jkg2#AYX4Eh z1AA-z&;XnSoh`YtP_j$dkrDJjy$Xqp8} z)Jl5CNBpPa#rdBv~RDheYd7?(=^9nx7qIJxikUaEl#o5 z?IzB$Q)1O^QVu$$?KaqZ5~(ejH0`@ZL|x5eL~(9SwX@l1%u3e{?0oWLTa7F%aI2fR z9=|9@t7V+HngE@snT7&T&22vHoc+Jw*FSIkKl;r`a>Y=BHolb1UYH8LOS_I$lY^j# z9NvwWikIAEmLQBwPjDYJd0OwkA*zRtoFLd-F?4Y=RNIBV@Ln|iDhU30vx*uuA0RbD zFhBw2Nw)X}yuCgMxd*axV0h!X+*jhph%T4pm+S*3Pj!GSj6NUSY^D+-Ri=Spe$U5B zn*Fc47Y;J~om}%14-RhTQhljN*&d3Ba2I+2=3e}Z$xChz6oRO3T9gvg`-&VZzi@+w z1?iaC^=&!x;HW-d$#U;vf0Cf2|3LwL_hDA|qtPxtu&Xf{JhQi0G+|L%iQlEoielq@ z|By>m;5bn;%Zm|Dc3E(nZ&XTrw0N8sA(j2p^O1$Ifpe2Qp79Aqh47$YAfv`H37KDO z3_4+*7c(VGH`S{0;0l(g8Sqp%&Ofmc$8^|4Y*-;1x#2o*PL2-~tAGAnR&P-wa*SlCqxzFLjp9EEUpFdwQ|Sg@=i%mh)=;NrPJ;uHr*Aj=MFS)UCV0 zLhhc>0?S&4v-}IS_roY1b>c`L#=&U77=wUJ-#3|)h=yLL%SHct1OnI*skq`|DmZ-K z5)D$cJRTM6gl6BWh6qt+`C6;seTP`_?#>KNN0*o_x~E>3>uX^Uj^W zrY+#x;o=_DL4#3&H!a&sr|I5WfhA_UbJZKp_C!mwCa>XUj?D;#AD@<^*%Y!?WMV({ zT-W;f>v%zdOsrTtCT#0L!(=Vt5B{^F*_FS5OoG_7n(IWe(ulC@kO zh2bf&W&)U8qSRD+Pu9Q%mj9dTvpg=ArS*SzO}`%gJOB#5Nz};+;TW;=EgHlWGIw4s z;+){skm<1NZvF{I7~iSTtWQwOew|5PE(n*kx+n9LGY8pRR?yC9Cl9LcQk8XBiDB8j zf=5d#UPNQet(_6(in+jWzJhg8YU0^*-yv66Wg9=>4dNu(wBscHPVt{B2-_pGz7ZJc zx4rCzvaBxT*T4?8f4XiV@XeQKF9{w=KOhL%&8tf>9_Rq)9DB6Z#e zrg+!W#G{H^vy0JUR>evULC5%%Jh6y}`Asu;)3_2ae!S|%l*RcD?_t|#ZvR<05IDjA zkLNqL|*2p^{&N1qfXx0^FIW$FMdmLT7-Q` z)D{7SK7e&pP!JTJFLat!9HRO+(!)9z%YVaLJc}O75LV? zhqb^*`O)wO2Zcs7MjS5(vFcqo0^^{&=h@UVtGoeQzK0&iAgog9as}=B9so-&*)b8R z82Au~i~LcnrLbmy;Q}=6;U;L?=~l(BkC0QW;wmlH1IK)k-?;t2$tX;LJ_t(Ii7tI? z>~ARyDLt4NV`l6em>p{dN;V31(w<`eYYY_q6nkKo=pAE^;KYy!8qJIA7;C!^L8k@Z zTidlFvUdf3|5Ob85%tg=YQYoX$nmLF$)y)mU9A&V|g*KA`rSkv&k4NYE>aw;T7CCuODn&%rhGq z1Vn4lP`$R%u_yhRef#$9_f>ezYK8%i*{bPriTJ*%vxgbr(aDcXqfLRG!>>GY{iNK% zSUXna{O6V>;LTM#1d{yyl-JInwyHO^G&C?ym%(ZM(IQRO*UgIBD5C3=Qx&KECz1$e z!fXAxN?u3s5vDB9-J+fwE-1-10x7M$E{OXEz)jRXX2zR~4c>@?O_$HpM7y`f!x_js zV<{)zo*J2)!tYc6{d=YmqZgdtMO*Y)CzcQ-o?n1pEiUOgaOYw{t)biUc^)?dGIbWl zYRa_k!rq7Qa7t*jIjBLAP}I!Y+;{ye*g0p47}a!b8(VnaMHV8bmQuyF*)hdY zPK>ecTd+vdJ*L00gs15{t`iKv)0derqiGNISOo|HhWz>e|7i(^4{6zv;Xe7gwE}qA zT2y)plCZc^nQWTsc#+&_+%pzHNT=YyQD-4f$AgD&^d&*Oub+PX^~Km)Pb2wp@+ORS zW}9O$N7244ZF!p;jmXnIh`jQeymJJ*@jH@Ac z;tkfx3O&~++dy>3qoNoxDm2DbIn@rWAx{Ma(vF@bjAn>JTc;Q7gnhI=GoR)83c~89 zWwOBH{rgwM3De<$ADH=G!{+7seXMkIz|DB(^o;T(HR-1{o_0@3h_0FZ!Y3BRzXXZd z8mAHpLe#MX=)NK=C?tf#tpWij=W50u_aLr2jMv*D@liQP251~a(=8zXy?^wg&hrYG z_QcQ(QLA7NasLffqa+uaY&6V()Rh#LB4;deMYst1>raUEBQ7lqclrrqbY54V@zU4s zQ~mwAD57jQTh{ifmYo}yBWWi?Skba1GJtU19}FL^v=Sz~nYX$F{KnblT6ijAE-W{C?<}$D6kVhT zJ<-a7tw*;wKy1UL@1T3-)cJH)K>F2H*oOPKppDP{xh58h+<_JLz{=ud5KGx(O8EuV z*H(>sHxm?uDVo&~>EO8D)rCIBfrM)YTL$YHx7bMN&4Fu4J=e)M)N z%g>|L5C`o&Fiw$Pp-Z#y$vBifTa@^CZZIcHc#9d6iA4T*@AE7qbn8x9`hVjd03NNe zcN2(H3)L;m5-0{ZG&XbpIW2#knbO*!Y7LtSK~h+4(-7mzWeI)g6M%B*j*u+{{forF zWjL-sQ6A}z5KL`DA&*CLF(4_?9sDvYm%LZ*648sP5COeNNphLQ&gir z1eX7MqK9sxovLNujTnpffH3ZBH_Mq|Ikn1XeKHpiw+$4FLM6JR22!ROhvQ@7cEF5| z4AwAf=X)0#`#VM|Wq>g7^kf?I6aqiK)(MR42dHV}dQ9}|yGxaKsp8u=Qj z953+bn9#i$;$1aS>>-osl}R#$0PoNgXZzg0>hI3+Z$YK#_Ceiv%B(mt{PSz(we#8% za$b1CQiKueCQ=)%uoPZCL9!*EJiJ&BpvhpwQDw-{TYdSO+hz63rtL={Xrh81D$N<& zM4u^)*5djENwxybIMpYuXVC;Zt8J)0x&ylFN6d$rX!nRLt2Zs^T4|! zh(B?m+iv&`vD5@x10$Pj^xp^bGz-*(NxS8-iy|viAwPqh8)vKp!gON}O|~qAz~|-a zj|@%pXNsuDJ#z?H0p7UOoo|+N_+8SOR9_^hkq9Z(1R12qkY6sIUYi95nzl4o+cxx^ zC&^UTK_PS#q6c{ciG|llF_+R9s6QL|9RgtCFdvRs=i#Cr&-XLt{O59npc`1(s11^w zD=yF&2Ob?C#Nr5a#iR+Q8i0eEQRHp}dqT<~Xt+&_H{j<9LyhJITOF#uezlHY5B`ERx++ zv1b&tTNfe%V-2Bi-oI#mpLzT-OP4<9a^8)b5_zT5Pl^yL27q^}GW7h|R|S`D)nkIO zdt2MkT_-GpvVcm^L}!JbB=(R?!$VO((eTIas&J0qEWWy7BdG0dH@kCo5gOTX-V3KIfs}<`QcAFZ#&4(KoJm$JntE&w>t!)zWyr~Gx_Rs08PnZxhcHTBj zZ+zgTT}WtZj|5v3@zG)Hk{~?}vej4set3SK-j9YcArj9@)0*>NSu?4bPcd$R24Blr zNJ>bJ>Lz3U*PlQMwOC1HRk>Q7cGApSp`XF1mdoFbg(U6r3;S-5!67BY^NA&>E77LDg5P1& zehr7SkgfycF$YK0eD`TzF$3n43;i;}x_gbiZ%LQjMG_u>^YxAHFNRx~1Y%d>={W3z z5Z$Y2hd=p^JZhECZ1-dS^Fp447m{U7eRUa@Y26vAF*y}dO zs1Wx%!Ss6xp(n@I9`f@E#qQD(5WF}cy`(#INn{nA5nYns=nUH#sUS;6+E|*rKL;EJ zz8`V|69$B)>>_f1mruTeR?UxN6xxaf_a zd3BR`k#6A-FVo|_9<(zWIJm#5rqHCYeCJ|7xL*NZXMEA|*W0 zEV;Tf43F<3VW}#@_&uKnt};c}Kf$&D>cuOV7*WqiWT`9~{hpMMJt#54%qMkc-Fqb5 z#JsrsnGPWWg?7<9^Fuw7FkA<)1KGae4SG|tcGNcPUxry$tVO1y?416qk z-n8x~f4_Hn$a{xvvM!|h!7Wm;nBsed|8h!Y+?vHp!^&FxL^=o&qZ6~&=y6|=AhAIS36bC6yG0fKbrXH_O6-oUBl5~#uuvq#6#%_gSrbMxS z8gOiCWZLPm(Us3<3DjvyCB|SeEm%O@mQyr~r$O@Xu%L({feyrtasmNDpvnlz+?SlE z2^ts}BV@0{!|jT=E*bj-CHc|(g_pB4I`26x_dSe_sTg8L9(|nh>s23&+0SCtPp!Su z&*BV~rw8@!#T==)8Rw?=KF+W#zy`liqtg>$uvD=4!EKo7yR>P6d`}14^4HGwzxroQ zRBNg2SDp-_uV?oJuaACs0LOGJ__(M~?~YnmwVNg6d@7-+q^4V1n;WpMYp&M`Ttln_ z`KWM7uQmqyY}w#CGLhw_vyL>wlutiVANYtKa|!>2Je!P)0K&C6ReIvjNe(0FG#7mB zT7$i9PhhP>Dhe(86Q5<%b@0kj#fL9at@cJ4h!kn`&Ru(rcN0f^t~r@bw9Sjm@ALX5 z*xcY(^D27>GsS@~uSCz=TO+`2Pe#uWy&H6Cbym(;O#<-Zf@{JE2GsApS#%+jZd zY{w4^)uy5Hrk%h?Mvbi~KCaj?6f}0%u0-rFTzMAIt^Qq?`p=i%ZHfxCeK+>F?%E98 z$EKck$u|HS2+C&^6H}`SO|kXAakta|d$Wx%4n}n*Q5Ru%gOYt_@kz zm31NE5U7)P+64LfunPjA<9^}r5jam@_ep?4)$?|Dc}ypPB^h`gaZ z2mhOcZU-zJGc4mi74TU~YH(2uFx#?Sw}DDeFFe`%eVs39A5Lg>Z#90L6;`jt|8&ZG z5g78zlFRL29$tBMzZS%gnQA+qL`q1Iwc%S1XEo=K2XZrK@p>O`WB$~J&#MfAc!PKL5+_K7ADf9ALmxu{v zoLdI-`4&DTd+H3;Lz*JAbxXTDtBAGYR*Ew)6)Oh#0nTZwN8(o^hq(fjfUQPh8Mk|q zgtB@V@Kvdq#jLgJ0$0`-7$vh_4}5webbbOe|NF;y89exSFLsC0{$0mllF0GNq+?8s zfb5`)=I*W7V(+T~WvlH$m|{a?+e>vrJMv3Sg~sR(Uat_6;MZ( zAY$>NN+e=i6~Hx$q|82SHF<=x;1zxDn?#ADh+QYqr3sz8h@Rr1sGsPc`uj-v7vbNn+q@$7j6xjwVVFask=$;4rPJX1^f#y6Ge5biS|D)1d-f8XmJ4l zU=y+VeSv>E4)K@Ag!$57msU3odq~9Pwq9L~hB-%-?Ua>3$w(7OYov-umzm($C)+!x zMy7QHW?m+iKR{aW*7kDe`8g*EUzpqMi&BPjr7d|jVB&Sy*l>!MD^udpynkn4?{y>H z-6M$czTfkZ-YS9tPlNZruV*s!fsDIz2pa>T;vhJQ+k2?&Z+~{ZM2H#Bw?CRa6lK+x zbQxjmS9^25KZFsCn1Z{Ny-icv-A;|z91C)mE8NfYH0G)A~ls(X^x;uuh8{C|EED zN(wgrIpt$m2k;(jR98muvU(!fZ&End6Cr5e!OrtXjqcD5dw7oM-dO=k1{v(|hlmL) zok1fow~7q+z-1>jBPuoUny;DR3=MdlxSL}<=gt>fav$J>3)HhUxLfDd`!4+1V~lD* zrMu8Dm8Xn*nli4oLtxIqU>>0A>*v~kX5!2oajGXJh>qC2DtL3MG`WGjGkCf)-}B0u z>ju^8PZXYcq5ag=6X@7ES;!QUyD=k~^`WM1p}|N}Q2&}iK@gBP#7c2r7XSU8_^Gp{ z`RY5a_K3W?edLizA@?43-DU~93_CItnj__rZ{ zTlfNKk7L8xxBP-EIop1N0C&SqL~GQO=?px)xD$m@lKwzx3+tKe@B?`A)>TQfI)V5v zTwK$fC`5SzdTaQN0MhGFANx=J)ItfW&{hQ@6$3j#zozt*+$UI)cKcC7Q+Y3M)ii9l z2|VOyAN)LY?EK6ttH?!>`gpwh1Uh5KHUwhg3muvBHrCOW{_mMWxn$TheB>LG(0c6C z4(u|9+Fmg_XRN^2j~fi`vJLLFT>ZNbcHvRfZs=9aW+7pp8k5w4UG({@=eJr{#_K8r zPnufhy{k6@Ss!ovOoUsNoo4qt^=8@9X{l>d=JV&?SL}l%?+*6TT0}dU|EHlBHGBD? zl!~qLhW+iwP?JdZ;z|_$oT=zcjKrPLW!8<0hDukTc_sK5gv6-dfPLQ}9}-39b(E1h z9OvWh7MBsjyg5E<3Ths=4R^%%z!nPh(~8i$B{*^kzH@Js&k*3-V`18{9OSFUKeZV( z=lt_S_U^%9_sLDO+!oHi~j`ZqZm~%q{SgIG}O=gG?l1*?HJ|gmaKxd z?{>d_?O|{i&hhS9Z`Po#R6$Ee!yuNKZBSs)Lf3c~dxejS>@vz_ljiH>m$83)y>PWq zwE#sQB9?VH|JX}UHhzBXmj@k%V3GC%Avjuj5JQ8>E9Mm4;;BFFAhl!X z`__86;+eA4ApOT`6esQ|}Ij?3WJkNS{#YsMlzrMC}pHDBa~wK8J2r>deNw*0)$g6x1$N>anDo>ou_XHMJp*_&(Ij*y=R zpXekSN#eDm2}fr8vG$jM5F#ou)ta-v^16RcLJc)083|&It*uaE+;G!NFN5W!5`M!F zNb`>c*T0M5|3ZJtXWZ2moTG|_&p<^k#HIW2k=)j-IdI>r*kj4N1SbSCecu>ez1$hc z_+fV0l1XlH?P(_d^(i1;sbVTWI14^Td%(O=L4<9PT`~6swxrb8nzoI^guUnPUl49G z@bL(pd-uQ!(qhegbpF$BJXeQW5K!)#f&ch*5{o>8I8DL~*!GT!#pp-mE-A$iXi=eB_09nYnZv$E^J7T{+-(ZIJ2Qh5g)3 zxD@|~B=g*V-BT{Orvi18ld7ClEGNGtu&;|a@oL`ZG8;ZUnLL?D-R`Gu(s;>9E}soq zrfSJ9pOP||VZ!8@<}F~R(+xQrv})=7!O#|XA(%xMKKwZs+5FJyLoAK*gm*gqMy^4` zPeSW2A_8<~-L+S2L`I&RkPg(@oJ<|Apr+eEW_t5gSX$GT$QM2x)trW5MxCk!nRNik zx;?4*-2cz0<(kI`XJwyflX zzw5ni{I)kyiv&EFe#13l;5NMu?*6!*qVMnjdGaVIzX2FhC-L~(om6w0OkWYmAnP}V z$3Tl9ZKDty6a`pwa53ot0Owr^+hD+mPqzzRF%*c;jGE_q2Qm|TdH4H|+f-#z-}chO zP15|UxL<_7U2?Tq`Dz!I{FtgU>ymK&90d|+=iKdhyJb8ncnic3w`VpxZNr>eV+bZ) z$kQ}^RX464fmG|(%aK+A-iztEj0m^S*Ikn2ufLfq7e0yX-H6BNly-2N_M~U2cmq-= zLHQ*St!IW{?JYPJv}@t9w`LP&Vl4B$?dU^}0O5hprV&fm;`p7EKB@D1sedj`Bnn^PY}e**QCr$0tM({n_BG$!O|(aIKd?Y7X#ikY_Z0bE)zlS9xSY8Btk z^mux05$f4?M`<6(GqPD=G~J1^GzkCmR<07yS*xlCy?r8`*M zjc5i*mxb{T`Lz}-nMspV19fJq9sj;2XV1bFYe)`0cF6Vnm2d7}^%Al^o=VAfu%KWm zP9`$d+qe>Y-3jLT7A1VS&C4eUcAQ#u56@ZyI1{X^TFX-ot0MzeCLY~?|5EsuEKbFO z$&7Ce=4tH zI$r+}OVOm4(@7B;uzhhJi^#@lQ0~`^Ruz&Z;@tA6UfvDj9wnJ%Qs?XyN9snv<5$*Ru-B2hOtuM@HQk6>w z@iT6F5CyJ-NX7n(FR)$37m-uATBoqyrpwLJ@x}UiSvboQL=;$GsC%_%-h_&JGadE- zhyq9|TF=D7Q4qo)V03$~M-rc_}OJg=~z*VZ3-_DBJwD?a{=KNay+m$UcOT z?SpE7M+Qqz{P14LYX8P*!9UH?b7;^Su9ied3_y&^$&})_^Iu9^Hg`b=HfX)OZ$tvn z=*0}Nj`w~s-@5}4hldhyl{d$~UVP%1s~^}p>oEPR-!+5q6ht_8?T!VwmdX*yz@+m2 z5tvJ%_E)I>+*LNr17y#h>`p-yBNM=_bFRX5N+Ul`3ipCn>h{bVM7<;p`MDR3tL$G>?E4wfY!P&G=1j=*MkLF`A1}b-T`zf z3_NTr_pxLOXl02+T_5^Y`-}qzr5jX&SmM_;&X9$!lU(}&vG+Osp~K(dVLLc)6)>}P zTte{)0!Fa5zy&JfbOc>`cDqRW2ata4SB^k1KqhYvsb(kRjf%Qc({BHP#F6v7Q5a3M z1B|-v5X1U~{CIm7+2e=yxR@V8)uT4xY}LwrXw0Ex(*}DVHA^)t1@gyR$H!o}a1*e6 zt3x0$T@QsOfw1t&o~97MreVPwr4s;RAz|0S`nP}lbv#Vt&9RhfOjj4R)siFOzd)u7 z>$8qwuXP&WpvifbcwGdi4U>56$D$Bi*Z?*F&Jvl1RSuU>Mx>~Ci-)e z?Rf#)e&+e9pv(L#$`-169Ghl-(^cFp)U)9WOqc5<%r3EU;YZo0{%fz!W3#PdQ2eq;o8OpbQ-(Y)YxWAX` zzWl74hP4#FYc6)1+l)=jj23&CA9`AM_~RK3uVo-=Fg6v!^bncJ5td8047{XU4_L?6 zju2Xs*Z21)Uf9mPEVzwH>Z#X=?|1t5FQWL7qWgA@z`Kp80@^xJanwlvYw`gB%ZFX6=rE#=fM^TzXhZ#S{Fa67f?Ox= zLDy=v05A|otP@qCBG_-K0%!Z-cPG`@cT9hO-HL%g-Ey2^Mit<(ct4hdFg{d{F9Ckx zg=uIO?+OP21E~WRzeCJwS%A#sLvM;0**GVYchZKxg`@w=pf#PaS#3|++XNL3vE(2q zhq^~lT-Z}SS7>%2Gr0Yew~z|7f1TMN?)MiTiavyb+hFC>hApqd+zhmc7XIw*_!&GgC`jj-=_4&&BulIEV7OsKZTNGf|g+X0A z4At+4d{@FQM7EWMTW&({ON$`?TfWj+^^jITKl8auvry~b?xC3-in5g1OgRUht{SV) zVcA{CWR69U)+~=s`ab@afoJzH8Xv7SpVjFnVIO#1eft76o!kJzB!TU3s<5icxpaE~ z(`^Bm8Q{*NX5z0sFa*n}vPIqy&^f=^359Aru>6zS*qj}LH3(k{Mfm?AB!* z{J+Fg;D%lwrh6U#2Ibn`3NuFHQ;lgl}?VF3}%+8ch8c3@a-}Uu=Bs6&O#LgkWf7 z3ma_ZpAal9;tGyyTf4D2qBM85KIa4L@+8@|POc(mw!cYyZMx<0;eH1K$b!Yxw=M@A z-hWD|dlp7cz>lqOly=uzGg?tiNuPJO(dX!w@$#CH3}EP1G6rk`ciI%u7ULbqv517VB;X7oGto9lAhN{@AtZ~HRoD3%&M=10K25!WnP-wKxl zB#>>AA!WCsHi~d*w&XW$qfn7815deHJNrzCLVR6Oi^RpWP6^!)$g#307m($_8~ZuT-ZWO*dpz^uAV72Dq#hG zp-|qbRohczh!n1$QNCmYQXJx-qrtcPv=3zQ?g-o82SQ{<=%{$Z3ro2)#BS=vm(Mp? zcIY`Tw+9-hSN6VlQfKjk>i7x-!nT&)kW6(GOhey(;=QeXGxJUQfn{^x4KV%J?q6JK zIl}o^a{sYIlDzVTI#?RK52HWLg};}HK~e^kHBK;CJ&9xzufv?&$(La68+rA!XC;u2 z#Bj%x!!@`AFO_WoyXf^jXC)dHLXZn;<3#M;=g2IDNPL{36$US3 z^XdT42a{ad5M_TR4yO~BR1}>%sO|#ZA)6nmt$rsH<0!*6n5sZHKeGF()vdLccNxX< ztErzc3D00OvN@y9KdDb|y5RWkR!$1-EyWf*KAZr1psS`rmOw@@&y1(`>&Hoasr;#M zy;Htr`W$H-_1C||eNU>1RD~3CpUE5|IuSO(3!i2F>HQVe5hDY>u zOd7Tz9KYZZ{=FjVaghqutt&L-ia+g7fPBIUEft^fq#|oU<&NcBdYjh0^L(Y>ltO2z z`SUWo?$ELvgv;Lj&G(kYBVv&odC#JkZ~Ix{S*HAadqsGV>T43zX_4+L zI}fSjC!gS6UhWd98oPDQjDGYU{xf^BXZ-6UvXOlvG4dDOP(tFas5o^Cq~CtZS}N*$ zkr@JmwO|FXSB{914kZS4CTPA~JqL^|b-vDqEYtOB!mVqh@wvp@arjprLWsyyh)ewE z`Gucnv&=g!L8YL}ATTCX;F(aHDDKBc-fIis>qS|T@4cyoQ>XEIMHZt1VNmKOh|)4( zP*19<)wo|O@PiM?Nc`tM+@Qxx|I$9)TdATZ{nWQm3uVI{YQ78lL{)^R`t)rZ$h~KU zs9k%&6_o=x(O(!S3I;0tCLLIZ!9}4aGTh)zi5LA6W0j_W6kWxE|snaJNs8!m{b`fxz6jJRZ;~} z0aA6D&O8p&9fbioox>Y!m#N_2?vc9P*+Z9?zIs;5waP@%oe)YvB77zWLB z0{>lAShr~-!`b@i3f0DYpD%l`a_m47EZ=)oB**iGLqkMh&^b)-qbZHw@Qrtn^l$P( z;dyIV{cW_+N4Y14EC0-q4~T;eMv;ummcLh1hKzUV2%tc23Dg2cvCy|ZlozsEutMn; z$dC3D$)=zT^hB+>6kHK#n9CL!l+}DMX?Zsi*^8*a(UpKDkL>_(aR}nyFf{N?S#xFc9f8R%r(2}Nx zl@}L=$yJ*^uHeawn_)}^NUMUps!x-EK8%U#fv4=Nq3$Z!`FXIqlWV;MKabSg5#l&; zm$V2stlE);zRtH^KIIu_F8l ziLyV4C6m^~)uwGIBH!3GBYfqaoA)NGA2E#*sN+g353MXD&_ooZZNM<0sa>>2LFFFm zv8uYxE9_^|dvgKpmsK4!(iD4Zjgk_^cuTTa+7pG}Be1%FYc}Hc{Byhsqftx=SLAGz zCL+iY^<9}*c)A8srQ1LUjCOrwPWr1yR8+*DKG*{(Xrk!HCrm0N?;!RQac_C@0hX5+ z#?&uwxApL-KDolQ^;v{z!2gal*;?X2#cW&}R`^x-`{-*I@(JQwTOQoM*RB8U{iEA0 znfla5H?#e%+CMJkGcB&Jy@}?>6>oo078z?g z(ktYxDN7MIX=OaMem%>uB{qzAGI2c_ADTTH)My9yb+$-O1VMMG>5ujP(Et4{QGy&Z zzGL=j*#dyis`L!VAZ<{0`cib7K&Cxpt>pt>Q0$a3wR?;5Nh0VsQ$o++wZwJ>7&;fz z7w7;;*#UHHSKTdWe}`+%27m>e{e>4*_$1e%?WI(Pa1w{^$t)`n;j z29DxJz>aci>^kx!+dkEh*MGaE%=-l7A?CXqiR^!I1U$Rkdy#Xk ze}+f*!nohT>zuWz$>nCLu`nFNOx?r*zZaA~?abK42<$doNlj&YR$@{7o!n4^Y>YSz zdt6+hrc9Ty7psehGqOyj+$K!m$Tj#LmM`X%Mk9F|HUwJ?CDa?v)iJIy^(@E*tVv-u z1Uzo2F|8i%^yQ8F>}=M4mrwVs8Wx-XaA`~+2lIzml55Ujn2|8Rb3qF%pl3Q~%@{3gXmLYnUY?0}6bnT;c%yBw~+2+Z|| zcfSt8G6f+9tHwmB`@`fYRNkF^!rgr|UzPAupcXY%U+d~`N){}h%OZSxE`c9pnB(r~1E1)O# zopAp)VbFIaV|1Z=UR|qP@*1ll@4EXG2ugZz=F4EOFUL-{TF4S5L4Hgud>#BiZha)R zio>fj<|dLKPiB$lN8s|e_c1_6dedg0IVfP%Ir8_)&4*IMpr4rKa%5{VgT$MaN6gIc z$`9_yH|=X4kS1C|Ojw z%<$uK$vL~asn&) zzMMt7dgB?vR_9{1n(T3OisY7+w=x)m>3hI16bF6>v0z8hK5)V#HtbOD-%CA*5oG}e zN_@~v(ukLj)k-+!cnc`Rav|1tbN(N&0I2fY0pC>{J)bVv@a1OWbjlGpX4bS^@Of!4 zB`v3vB=_Yf9(vFz=EoRI$KFpEMGx|A?OsTwt+Sy%ProPBMVFI35Hf5|LLX|j?47>+ zp2jNcCcy`Ja4bVDf2#h~1OvUbf4!rigK1+un@Yt<9#SV?)Lzr->-vW<`ZOl=QVdTy zODAVzn^N~BZQr_tnB(u#?$I0$!r2PT&t?Tg8Ha|RC_DdeRb z3e#hRNy-P3$daL8$3aoK5a?8cv1-^)WM1ZeDJ%Tp=H>M@%h~sEfH{-3gfSxtYp4#} z>hOpL$YFXrblEGh>J~UdU(C5*cKvgrR9wC1d{Lg}72#G|PKC1SaNz*iIRxC6n)K4w zD?ak+=@R{a`}s6RvC(poPbB74jZ62-PVsIK?^s{4ccgI0RTwYvrOKyAuwo$IFYaj; z8P!(8nlX!TJA5|kNVXzHlM?;Wsq4pj^w5qnk?jN9>R_=PwWuf0b2N7aV%WoUC|lB) z;xuP<2KhjOMgbxeQH!78CniKHh(JpFzJbwfUd&eJT5$tTtgxO{tg-n0$oEo_%i7CP zr3-}C=&W3M46MOuh$ zg}SV6{IlKdJ|1(HjQ16<5h&rY$X4kahriEa`)@94OF|1+6sy3(2HjSd7|v0l@hl3U z5bS|qzk5~uIAunCF^)Q#x&UIc5wHO9Iy3Dc&akfa34`eY9G)TQwT@A6;)CIC zhwh7Yf4iR)L9#wii^)S`q1bhWMb+5R&uF((nT6aPIgfpIg<&~DWcmuB~( zeC>dKReW!y%Ke38y@}Vu7xdbGc>g%-`z57ySVsRel#N|{zRvnXm;T`9nU{C@zZbpP zTK#n9>lmoA#&tXH#^lA$ei-hJ|MVN^Xt|0yuVT`(n?EZGLsGqE8BOdy5E=$zuRkYm z*Eul1-il4Da;J2?*)&=28gKrwHE~=7`wBzCyGvs~yMBm9&zdJfm`BeuZ=9*2ikt+x zGLeGV%qNO^gI%XcEMo6zASwjd?uEJGEF^u}mzQZ_7i78z51a(kZ<B;rKD}{$gjo z_jfXlN+lq@nAW+SOkz6C(LOm%t*-D1WHHtPo5%u}F9z#{s4mkrfc9(q+FSn_)!67! z2vzv7BBIMgfbaVc;L62z)s>&_e;&3>09S{lW5(G)5yAcrSP#Q&=LmyI* zltw^65GkcYLRv~f=?(!&>23je`@7!z-W$U|@3@@$_FikQIe#m9lL37I2ihPBO=E`6 z_dkT&7F+QH|F~g(rCIAs$hHDgtkwg>I5zx#28Ax})}Xl08YZPibx)f?)Tv=w0JG~~ zy(h31ZkEm{MVs_}Y_WI)Kdv%nmsRCA+-e|$>tZTravNafA7a-7LHH{Gh2_dnv+(IP zDm557%-%1m&g`b8p{TZ-h^hNJzUYy0eny*Oz&C6uOTQ;pwgT4Jy<)$@vUHP6@ugG{ z@ft4CU9Y6kbkS<7)hT@iH)0Mbzq|57`ZR}f2X|GUsQ<298ySzhe1SN<2-r%|=1gk5 z2Z~sQL>#nPqwf?R^6M*fhM?}sPCnfISDK3>LI2?M0G_qmxvL@`v}BlW!4n8j1B!|5 zFGijdS>AY*f#=ERB4GnSZ^W)!lF zzl~Y|Y?+O3=CPmZ`4ibedwO?gu-Ne4BR3cCVF;>QY}_Ddb*lqK0X*_<-rg~fU|FmI z*=e8=94aARFagD3RD;!us!|l7B(`9fv+haH_#XE0Z*dySbOR*xvE}dbY|c)ADq@!V z#{TbnFk?fny7dm4E7dre_Lf6n4^SOsORoubP{B~#t#p*yB%@QMjRz`*#Y7#OQJO>j z)=#hV!?~sP>2nz@-m$5tEBm~LmKXx*vkUc$BrBZVnG^TjP)x`zLQkHyWytGusM6iS?Nf^EuKNe!pjsfy;V9%;)W8&}12dw)QCv#GQz)Bl?+bqAxhrGjFfYj4JetctDO{Qj`O zogIYq209HKT;@3|!OF*S8^yL|bvda`l?*&|HUkKqqOSUCV>kX(!V?lL{Hp9*hgtWh zoBEwVHJMZkay%Az3ay*aDQZ=b9O%t8t(Ho)!(LkhQL{? zY(c>Zt9FOfHvJJ`y2od&?gu4cFioLNRzr7pU zMN8Zc^@#aoWq5de^P}((s)~n5JiM~ELntMlRUmP75n_B3)HCA2Wl?QebLhGtc<)uv z4Ou)AFFB^_+9+M#e-~aiiiF8@+8(Y)wNLyl#-383e;|rngQjvqP8p61N5vPrk{Z!^ zI-5!q)b|PuK4ljR6Mw3WyEe|wpxAFQVXIJ%vYDfK74YSiIWoVGm+hh~( z2fw_mDqHfUt8FdQ3IUlCNB%Zs|JP#9ytVT!w5yBy3MAj+3dBL9tM(ALeGI3y#x^h8 zB%<*gOzI_ayv{8C?b?{pnD-zb18$g-owkkuu{PUB-feC`B*&4jhaI$)z=xyY_GAD%9);k4vS{hi!$9Xoii2uH*00ioI zO#d0a=29y!jTIQP;&*f8-*#!GN6n*;yd2APT?nE0{?c9flY;rC+U6GgRqaq**v?~1 z$s@%ELw_+@+3muam^5N90b3c^@voK#^PFXkq`5vFH;v6}nJ+uG?iTg>p!Q>m##;px<1?r7VyQP38&Yd%&4vk+ zY7WQ&1JLx%)h^KLnRYH4WYNyKP?g!wo2EI?>05AKSmcX4{Xa)Xw>k_OO-h%F;(6MH z#1}6#(*HSpf88B-4#vI_z{G_++Wm>ND%uB}pDXXd7(7M5z1X?~APRyo35&qqIXbIa8#stT6*9(NK*EdfBP;L?SjY1O5kNTjzjugpADwao830KU zH8H?$66`mD(Ny`3GnT30z5j0;EgrxEN{^Z&^daPXH&iN+0k)JgrFzobZuf(H;!PNk zNHhBoE%pMWDU*2zXt8|2=-=(;;E)jU_DqF<4s(I0HYr7`vJ$3U32z?*F0wKB|E5T? zvS`L?JI&YrU>^K!En@zm!k^y|PvNz$3)MG-!Dsta$l<#Nyvs9%hZ_dvtk7Tz_(0hE zK@M$j%1ShcR`)mJZKet(p=U7{ZJC(*X_fg5g;CZ{Q|#^^h^f{*$Zlv_^WX1O^;!5sf26c1YdyLAvcFktnFFKMd!>15CYEw+Ec z2F(J{lLDr1aT#LJ34YCekg(jdy+QY@!*MO@M45#NvEy*JnV8txK>bct+zqc862+f2 zPCrv3LWPe3z4BDnvK&&He&J+=@q6E8ZfjGOd7(&_#fvxRHhH7$7^LP``%PeqL9{G* zz%d2w0Nn~{OZq#LhQCG2?yC3P<%Tv}Kf(47TkIzz1^a8UF2P)mY5tXxv7lvP$$oYB z3us^lvuW=j%f5GSCh{id?INL2K7tvJfh!;@gVQX1aN8Yt5#<3ePgZ}b(QSR$0(c2r zN&u*uyqgW$xtsT%6c}~m>Qp;1SZGpOkX;L;A&^L(+8R?mGLpgNi8k?1?tX#UOfQ_W zz!cyZD8PA-_w!{PQx|Sa0MLHkZE1wS7C?X`Huc8rms=n(2W9uS>odb`dW0xcg}JPoaJ6NzB#)8PaKLj3uriCD~*2~XEyXn<}PPl+Jd zg-8MH-{e4d8NRdL1zU2ysA3M5{;}l`#tK=7{C1w*-;rH`m~A|5lr{m!%n{k39O2t# z_f7e|@X;7*IY263ZAJNL!xx;L@<88dJik9*%rei5&KYzC{6cOhE22mL`qxfK14uA; zf1Paqu*6anTLIY%-WRhb%QO!e1M+Q!b9d`j>87Z-sP-*+&T19K=8T~FQ14LjrsT4C zd(RC;8E*J=GcCY1*N6w(hFy)8L0%bP3#@l%)|n%o^I#7VG6W4WG(>h6{neBF=spNJ zlJm-hC<9}42pYj5ATMvc2^uiDwPikKUK#{F-4$Ui;NIeTE9B%jU2g|eSpK$TS(y5b z6iL7OyN=<{?^xb)Yph%7R;1v&$CfuDTC}tpAw(kH0}Q~?CNA?W=u|OOoq}@U0e;=< z#CvOt*QgzM*n`Z|soN9Hz5 zv@Q=bXRgD7zOHT;7{G16)@_I$um)D3*X|M4U_?*2xOhN<7wa(#9lVs{sPX_gQL3p= zmSNBwB>?x=AZ;{EsOVtIt7`8wQ8U>d>B zK~VP0fY~#F?JH-~Z8Qv;608Nd4yz`o2<7j8$JNeb`ykxsha#$&FBS=i1QR*=G+IqK-SM#vlO@^x%cT2=v-?Q?j1=0N(Fs+51Z#RSt2i&ur z$uaVV6@*3~G2mv6&~Q34WftUjMlxiM`zJtPPE;`vhDl0Zr~1vhtDF7z2MI>X`_^%# z?|r5@k?SEin?f0vE8wCBmM{fC7Z_sYdU-E8b)oc^3*cwW=;OD3$#!^eD29D&_scW~ zhb4n_(7PJ=-?6|TF2gKY(!6=C9p=Dmc16JKDMv`de*-PxnFAJ?ZH*l1n0NRS2}vqU zjTXYJ?r$ees#a=wr83<$nvclt%Bfe;6M9r-wim?c>*9SP?zY6Hci16uRF`MbDKnWZ zC{eL9nhBc^VS1@3g z8{$*`WOwQO8mkvZc;)v8x4JAS!A?k$LN@BGHE<6kVm((^k6}Z_^Tar!cgle{<>P>x z1n&1*DU|EaSu$EybF)|i`pK*^uum+AS3F!bp|1%c3d6an$i<$^ev9=kW|3TtjZvH> ze13UUFH0y5XTa~9_i7{s5BFd;k=H>!N&K53B35LfSvpzB`rt+XqfjyROOSLYxlxEA zH#s2D1jS*Yyzs2Q+;hr46RMrPvz)wzDw~n7fJT+;=uemw=Gff?`5+!fd+Ky6@sUZ* z7RC5QAk9okTCfF)5~Cb$XR#1^m^$1{`rE6QGwcD1LImjFBfzcHW#kMF>j*Iadj-zQ z1bR?Aak;Gd>)F?>~je>hg0Dk=TCh&pV@e zInX1EwDQ{q#}(u{*d5dBdPmXe1FS!mJ3S0D*k9l(ABJxSe6NE*tLvLK<$2LbJbxKJ z$apjwVI0D7ymaAkYXy>euo|W|pD>Rp5Q&gBxC8?iE{cSB@YffRD5sH ziB>OOz6?FeV6`u8wc+zKMZG;+<`wB5ooY>;#D3lJc~8aD_wDiiAB!-j(aJ>P0m~@ zSJ~Q$orYZ;tghs{9BKC3_WNMyfYGY~%D7K9Be#9nmaJJa5z_KnKq^pc&j1G+129s{ z{^u5&w}V0Gk4tXIkg5KNavvuNiUCckqJ1W3x)03E9N7J}-r`s?f&7%~j{_G7%07SE z4r=OoMe6`YvCvM5(JOR1yX9%@k~2z|2~89l@zt7B0ar@iNvm6086Ma?_|g{?H*Uq6 zY-r|R{Ev;~glPDwM(_ol=4ev5anzf`gunQ~2m8?8i*<^#$`{T{JPewZ9H)8p_wxdp z!e^iYxkRgQd}XF+>{e^ zq8ZMdC7Q=!HnGcJl7`KpE;p0rgJ=}XGzXuxpqMt3VmiG@>IT6tld|b*4zrc#GGECT zR@*#iF=()*RRWLbN0{79T%P;a)+Egj-?JxN?FEfjc0;3dAgp502(rIXg+k5nq;$)h zJyZ#uRj3a%SpuE2V^IAsxw{}Q{N}5WB*O*c>5meY8Z*HYlUzpp*iBI@@|QvkX^YHFt;bR8k({$DQ^oAZ?Kl zi|feEXS+lB41Ohm{VPJMM?%NEP1=D)Wo5j}GXJb>pO^vL-^ut#Jh z98nAbdcKUki#U&z2GKt?JLa>j&7fvw&3}e~4Cu|I(tO3>jKC`i!l_@J@&-O5$b#sv z`_4OF+8THA^@wAxEAX4*f?p7%h|TbytRr^7-je&DtD?6c&J)Nu-I4QZvn~(15iz)6 zCZD!cdhi2O8tdPzMSVxS2pgktA}78B10v_(?f$Zt7`G{R_R6U-H(P|TCimVVr0 zbG=@~DRFi{qy0$tEc4$6E>Ir6XB-MEK*dbl+FEP)19+O>p4;8pdc}UQ%Q8=Zt|Y!) zSO^i)0zQ>S7|(qK@C{u^0jn^r)6;lr5B9k(LaKP^8)Bu7SF3I+*dWVY#TgPgS_3Kwv}pmza00;Una(wE2{vWS~Er)|&)4$XI*Y61cQ*p5HQq|7!wv^NkuXn2hRLTs7uo&uX;ZJfmpDl%< zyGSk6sZ|eBL`jsiW&pMi9{_ft=Scj(G-??^sensE)9?y_-s4M|WyRva=xyD*?jmc^ zj|ri7cVCp8OjVxe@6j=@#TMACR) zaKB@);+g6`Fm)^9w(j`>Yv9vIo>KTO)6Ou5ORe?=lIeHz(h;wX;lRLnyCv+bSQfn-Y7ZnMk;KepwC|n$ z)i1g!(Xjt^Q8?m(&}3hl@MvCzf8VJYys)RJA(ElIX?RTc>yL7IkeiWzV+rhk^h{^(n=6Pg96}e zBD_30&F>A@8(1d`0pCG#z}C~7`_hRjt0W!LvFz3s{(C;UWUUBj0bL#w>v^}T`FJ0(>9RY|7thNuh719flL1B zZ;8<7_Tm$eo_P5XP4M+*#>vyhox(0!I`bTTkVKhY1h%`AUCr=UR>HJ0J8^mlZ}>pT z6FSezMwhjvkUR~qCpbE9go>OIWMyL0gxP0VpAb|j9H9*;;E<;>A7H3Di34xs;= z+cpALuQs~?I_%Ra?;J>)=ZmETF4_~XUT?QwWI@0|pd=68DI#BM z*Cm%1s1z?aYJT{oi+gFE$h{YFwd!qna3_V0GXqNoq2&kGPNdLDW+}`7a?HToj^vMS z@H#rGoP#1v+zt1r=nBCP0n%%=!7Fea`4~M348l>N5c~+V7Rdv&(6GadBtRk%$JJr` z?jzUfK;lqlx|nE(d5CGi2l{ah)MRxHZj^a?xUIw{4zZGQ!VehjuYvdUBT`Mk6j-_m z@ECOapcq%lX#&wpQ8GZ~Yn1M3WWXVkh}(^7d~SL46`C>mH00pW`!rlw3hif2f+gF= zKi#cE^AmuftCkLZiI`*bZwjtu7rvJs80MYR3avXmzq|xb5J>-A+}4sJAE&w0hgmpQpe}X865A;OR}bz7i5O}Q zB+>sq^(|nC`0Q!6;U}FkEpF|{Xgom8EoTYPv6VNod*_2>S%~AtzM`7#&(jl7_)!d2 zEUuI3|7d#F?w`$&&bQS7CXtIk3H9_lw=%!bXe$!;?2R6`>7=og=a~QEEaoefK-l^~ zxN*xW`wK~vH?dVkDMK?krq`RhhS^VGJ9n0e!EJTBTQz3~`bMQ;j4Wm=X02Mz^HKR< zf#WL-xne|$`;-`6-XxYrgumWQJdMeLYIonL^Us2#d8)9ph1#x*&Q$+LLBmP1oc%hm zIKhU!fmV`Gf|pHv1{`~lb66Y6!Jjvpo}A(oev*RX3|S_H(!^wW_$syHM~b5GIVA7Q zr<%v;rZhSW4S_Cy^~Ze8p~}Tl4XB3}oKP3jpbbfVksu zVlYu-d7O7)%k_heaky*FbD#Ub@>Du{uiveY|WIYN$^zEksANf^A>`pKB>cB30b zM1)Y47TJ^0(IoKv&>8T2S77JHzcrEp3A9t0StU|%WEJznl9s&47MnPD01${4pDF8A zcJLMWA6O9wjKl zrs=x`z;dL;pZ+uQGvi)41gz)VR(ttYJup8Mw4;%*q7U8>at-dsA;fKn+T;1!A(nv&@lKe(PZhZ{Pq{D)TQKmU(=bm0;%Bq0MN8 zJ~wi!coEFRBaKiCA$)l_DlZ(nei2N!2tVWVhAr%eu8!gF7LkTN+gR8EV-|Uvpnb>(iR>Or*Rsfg z%HQV`z;&*6<%lI%8rY~X<74*fe}Yx+{T!`B;93F$cJ@V34dFS=@`~qVcc$3{+M*_I zZo5f&c+D&vRaWRs;I_T9lI)5Oc@SbHUIi>d_?7$<#qfkT2q0X!jzHz=p7F;Yy1y*EhQ~b3J$GS|1~- zL6RA8p$D>2A15Rab2<&hNzQU^go-8dk>3ll0KcjzKqX{au$cMM!a;^B+K%%4ffV|o zrknghHMu@osb2_8xc5cCK~= zWIXr%DPOXA)m3Aphy8PDF_54ajcSVT0?nGc0!+c=<@O-60XlP>W2=)5G-qA^rO8Y& zebzz{dpAn5IU+UB*ylHTyD{!J4`V;dwfr5Yqc4kbt@y1AA;aX{*;OHtLj~Z1hzH_j zec_woHffLFm21$vb;A zg&9)?XmB?uL8*^}+v4mHCEImRGQ;@BgHh`m{+We7RfS%Pl*X__w>D{{eSXu^fdxkcROOowZ zA6;5rU;|!l$dAC}zn{igBHEXP61-k--QJ13{CslU;5TR)KcY(pv>#H(@=eR&n^-Mj ztBJSMMo_lboU$SQQ?Mvs+_UQq`B)g$tB-42%;sQ~k0!xaT%u<_{|ch|`#;#~k@1%zU| zC=*P`Fz$JqMq_akoM!sWF1=f{Y%&*|D`6r6`?xGFn)=Y8nieW+sbG3T{zx;p#6zCc zV3ria*7Y*jq4*kCT9JIZDw?SPS7Ngz1w?S((gb>b$Di^msMF zHd89{nyRZ;ICTFPb~`Nz1yD4iAMoG@gOf_vMsij#?AulX^#lw5T$8P__33maS6i2( zKTYi4V$}o&)MGxs&n>6iU(z7U@swOk@uB`lk}l_HkIb>qGX~SCLfW@KM0)(EJc=S# zcvEHt(fINV-OoWz0~OKJ%Zw-CJb=<2n*EN{Cl+h8db1wN%0JgS*SCY6TB>y6O_qG` zq{Q=HTStF2^6uY?-RuU4Wh&%?W9ChN0FbA!eF8@_z{k!C)0-XTkN&D|T_vZA8eh6O z8ZaU8qv$>`{2k6^KTJ!r?ozsCunQ1Xt6Q%$1^=^L?U&Fe zYz#N+q1?u6$c{|lpALlTw~zE{ypwmXnKdBS?;yL7>#Gt1Pf8~fYHO==+QNk$-OYMb z(l7Jc91pocOl&1RUHex0&9AKqrkM0E^To`_ACQh`C;QLhZ~VhppHgF(b0zqagRE5p zDn+*>Ox;sk^}DswCd*Pgzxb|W*b8j)3+Dq{f>~X;*4ZN#ODCYQnt6vK0g1*tf|RCM zJqjtBLz5^(l!j~+dUZ2$vj z9LcaMVr6T#@Wn`L9_4>scIbJy1~)Hto73jQAqLJ7fvTAFikRT>HgXP z%+$7KgF4L(r>Z<%Sff)HaqdnzO@l<^>nmgM66~1LCZB93{XD$m~a6{8?!^MyP}w+uUTSR z$W=?Yk^zEM0Cv_Q^jZ9{%^=h{)GOL2^5=z2@4@$`FRPc6 z2JLH=TzBhe&l>yOMCT$;Cx~2W{yseNa3rOZFE_VVF3$U~8-YtdA23AX%$pH! z4^|6iBoOV;l{JNCVluVZgE`SbDnYlNwUgs*!*Kd~IQ2FzDrz3)qeb1*x z0r!ghd-TUNJ={}rD3=8I^zKh+XMI)7`Jmm`5S?~*H|aOp;=kyOeI2QlK6WUBMFe!9 z9PGJ!(DYO#B`QyJnone>53GEQ1SaljJ|itl_At7GKkdzrHDwVU;5&bPD~RGh6}%q4 zcjXH0Py~N(J+#Gj1ZbEc7TtMb_aR#df(cfx4@TPDdu1FzUPAyA4$?kJMEJpdFrbc0 z95<8QuN4t=79_H_s%>ZR0+E;C#cdBl+!MgccmA1$MubGRdtG(h2k;q46#(?iFYFY} zlpWtik~JDzA>-&rpU`HB^W}u0W-n*nrRlz>Bk~bp#_XC5^~6%Fa80O3Aq!&=G+*TX z`v?({w0MeTbVOlyq3E>4cQd3YM5^qG0NpQ+3H=`1jf^WOcpNYF;t!dU>YoFe6|LH1 zcdcxB+dNnPCy< z>4Q2t@*LJdZ3$pM8Jl@k0XJS;?uXR^iN%76qH41aELQCRkb~FtQWg75G<3$G`+$Wu zaEJ@{c8Ues{o^-s!U4cGl%!l@?!icn^v&EA`vH>g--yG4xw2dIVKj0ERid zegr^CJ>M3bxd6Z|z*z|}24wHF==EHJvoYRWU!?(!>h;!}qm40DNRPO+1Mm;g>iDxd zIkT#xddobxeGC;F`Ce(XsU-< z%T3*DSUjoq?oJ>0!I~R)H%F9m{hKC4yT1X{#MqBpRsRLZ zCMdUf!3q-|P?MI4rp^dYR`%LVa5sS)1d2fNyS=p_M*nbKwFY>WPHlnlMCeN+l>Ef) zxG<t|!gIY3i1tcsMX>Mr&eVtC|@Cl>GXqW0e-7ti5TW%2aNY<)akdR&C=Zi>K)n zHn)Tsv@q@W_Le)t-kVZt{B_@fV-vvN6eZ>Iv#UIA0@7|!cM@lEO@y-tcrHy50bHYC zhz)JF$d`2dp*(EewWWb&q92UDP^DRLyEG^f=q83w?LhMB5>dH+G6lS))&Q}fc6JJ- z-`ksKPwu}B@ii;($0b(nWIvG`;{O0^h%%CPZU)cglQ}M4wWNMn23`218)u$C)R39J zR9GW-Xv(<7+28dFfzmf1J4MTB4F^)MWhAEkES=_Q0yGq?h)0X4ppI1YS$G<8YkT^k zbRZWc4cN+~*gKc`@-~4dS#q2{&7@vcRB>)2} ztECA(WKf$^Gpm(49@XZjpR;SlXn`zbuDm%LAsF#s8vE_K53t0oXKFu9a5V5Y178t4 zZZI?S68R}H8f4!u={`!Q030iHhk!t}T(B{ZU1upwLEBU9t^S(e4>5$(n% zk`Li}YO&6krt;^ANYt#Dm|6+i(8QyHHzk(a&vu=TSSL<4J2RG)>NJ_xObg|oqGC*O zb7BIhz4Hox+gx`@M7XnKE{RXvY)sS^0V^a#InQU~L4vs3)$g5-G+)1^@Vw`L9$sV` zS_U&PNYQCd&)vR^(X*YB2V&03=ZJJrA^~6=P|PFF)igF@6%v2z(pl%oce35v1cX^y z5ilyXBn#8b3%<*C4_@}*C0dku&i*>93*ue>1Yjt08Z9&&7^GmDeQiAa>! znz;u<_N))(nToks*;uu^e=&BLWT#I$RzpdO2P z1DT5%GMK!sgf>(KW|Filr0=^*+bC_X^S|bw&dMSM{_Q(D5$^Jd8o$F|W9+WWw(*NJ zZg!@@vC@Z0`zu*H53QESzVlQ72{c?QRbnTZE6N89=T);YNMBsZ=e12}y=Sd?Ul7{u2+y|J4H zHn>b`VOB4Sb4+2CWWaZ5M=5y6tu@+6Fe7|&nE+1y>BT#pfc?jRt7%mj%0xQ)rmlv* z!nSOVcxI2|?ZV8lEhvlHC5vwED+STyFE>rEEn2PSTv-s(Fk4ljoDyv?gRKquF#Fi{ zyP_Cby9H!*`|R+8Jcn(%O#(vho$?;Aualrzl8N)p37bs=_rXAvTD3&-iK@CWh)Xai#^U+OiqMBLlm0a_B8-jYY2JRats*8Pw) z>CgA~F z8K&Zm8-5@yWv0U>3FwQj{b~R8R&=LJ7b-T0T4r+#Zn#4O)uYCr?i0q)^t9i|JO(JC zW@6}*PmqeXv=$($l7#{;-3~w;w7>9OCrSaXiw7i|? zESK}XMsqNf(ENy5F={V2j9q+c?{}Jl(P*KrYRlfy^qBxZ#QN4Ktn9fE$XKNfBb_gd z(BEvi=)B7+b0>pYZvXNw4#nskHLsd;_$fe7U1Ymw{+wilq3_yI?n4qgv?9JG8H)%V zOlQ*G)C*H}QeIv@#HTlv2PKUQZ2i_Y2YGNW^M`$KcH9IeiU?IzYS~!1IWREiS$3-v zC*6}m&vk4gDSWtaBzIIoRTmZQ;jU4sjkg^Pg6L$t%Y$LR0U&Sxn)|n!9A`VNZJWf{En1c}JUl|_LHn^r;_~1g=Vl;cxwqB!Sx~<~+hQ~B!F7c$;glYvUGy&iu z>H6`j=MUE%o_r9(XkDWca!fCnpuM$V#$vKBz*ha4V=h4Citan%tT_%f{s$_rER-?e zB&{qc1MgGs*vB>8d88#!9Qh0ovjG@l4R9>+##^B7tF-H`>N*Eew3R@j@q8WHhdwl4 zcq!qa{fDqg2!dw?AYdwO1Vc&LNi2Ivl(Em+e+63Iv)pxX6(&q$jw)!56vx?youur8`f?+C3T^CDeQ2YrUG7(j-j zcxl*w&rvwoe<|oEet~hr=K!Tib{u^0*bo>~Hu{sTps7NR=Ykt9Yh@qV$}QWjJzT+4 z_i>1BkI8J)8)Pp?%Qsu+ z-WTQQvBc5Bqf9*HUoz76^*<-ejr#)ljSs>G?Y=Sl(kWsRk(UCi*7@bv#@+tWK;8)}~r=|f2Y!z8`ttPkKR6!|+E zkgr7y2kyR-)CtKVy2Yc(iiEXEwov-YZ9^<+%bGdTm?EBYh-$fb1I- zqGN780BwnniNQGg+J`p!r1xxpjAb4h2Cnve?&zLx&&}PNaI(7Y?St&jiunn!>aJut zRAl!>Dg`sxX8lcqpjpx?HgEvd^v%#~TfFDCIdz`Z_otzVEkwOIE8dsQ=My35%dDz! zq0z&;VGzRNlNuVA|JY{`kF2nReCcLc`PVDw@4Bq%USLK};lZ^k4!liJ-Z1KJyLpT~y3CvM5NU&_WRH}s#AOj*G0c^im@s5R z5uXFm$=VnYlq4kLOQ|fOYU(GunIhe>Jz=mJRnx|jB!L4 zip5O4J7p)FEJOaO(zqcSRHgC==S0Z+sQbM0@;@(tb=PiyjsPJ`M8I(6SBk5z{M*w- zSMyBLF;DiW-sJfqcQN}KZ{Z+FD3=Yhg7?8wcp)h=TnvuZ=g?lE)mLF|H*+KlG~9#Z zywiSf$ZizeXdZuT$J%7H9W4EUB)md~Z=7$shoRs4G-5GzJLwcNC#*h6bEP;!9j>t) z67tOtgoxr5`D;ypP`DFZFwvF;s{;V+xJqjB?7Q3zTNPUI4GSgd_ieQ7Qz?E$pwlZ1 zj&HhmyNDBbOggr+(3Kxhh_JD%5{yomF)-x#@s1l-($K&sIWbl14x%UoL}PH!WIYq! zyumMHs@o49yO=ox)$&x*%_N*a8QFk16jKjioUi<@`Xqtm0GE9gnZ|v}ZEJ#8RMpKs ztdy>a`1sVj!CYC`tnGyxAA0eZY7czC@-0{TKUZ zgjF&X_pv+7$#2)Sqko!l>FF^3cp4(T{?F*={G_INb{VkA+%^bU=08Oqi?7(m@3#B)-CB;n_%iS5adxV! zXZXPbaM&gw%6LT^LsN+K4cO=6Cw<-{X&Kl(MY@&hUxyj31}WucrCKQ))U)|@WK%3$p<=CITE(dO#F_=>KUE9g0!d( zOn(n2+k&FSypPmf*KSB4El{rISCP;Z#pS?ji8rK?jMfh`?#a8E6o-u;Ri4}rW{~=` zAKM3%>$Emr274hLbd^~wdYon~fP_()FR&Gzxrdn?d z!>s!S_-s@t-UPLc@`ODPF!DdJF-zvj(eSqiDEWs^!40iIMa$(%M=`m%jr&(zPILQ-vm3GP`wrcE2R z=lu%7&JSCtaHO^`4}>;W^#UA4WUXb=yDg1RD>`0s1pc`ZKJE z6Z-)~*ABufyqJx>dG4mBV%c&)U)xSkFF?~!>SDU}O(2!${qkwJcGS2$%cTjA~ zC4MM=@~H=CUZ!oX95e4=~`^E^AH` zcYm!@sfs3TYeB__(HI@h?}An54!4e7Y*Y<=6a7zg8WZc->jFlD)W2QehLKLMMcpz# zAZ6sm!C@EoN9Y;4(vZR1eCVBZeCrQB7iw?E$q^M8?s_l!Ss`OPGiKgUlp2G>!cVo5 zHqpo6&^|iA{b;=SM#TbMXH48em>2KkWm zP8!^l{y;r{aRc{8iJ6pHTI2N5!%6XYqUY((im$U=Y})Va0MgskXie0}KfnHZXcGnFQKCER z^bR=S9yG!@OR8QUHYRA*fN^TtnBEhv{c2lwxTpd{q3q29+L^AT{1}9|*Ss6kmgbUt zg)+=epm$M|&cLna=t@CL=!UT{rsE1pSK3@Z#`|EuckVS_DqLMHm9qm0)}5cj@CDM^ zND6YUEY>)0_}`8+8dPs%2^6<1T-F%uc>TDJk>Qb#<6xmBbH41G~#$N+$=xStO|jL9Hdl!=2#B~!tVB<_U-bMbc|yLkVzeApD| zRyO^7EJPb+FJrJ#NN1eLJa*`-{*J@`UgF2t)yVrl%gWm(VDUWoOEfju7l2D* ziTw%dg#?>FI0o*SUjLr3EYrkb93%<^@YqR(5EP^LCD_Kb%_;R{oOAqX)jhO?ufhjy zx0`Mjd5+_K*e1s4&o~pHatS`MEI%-r`Y>V2Z?D0~#A5#kv>V3r%BDqp?zze-plU}d*6(!zysN2Poe>PYy~S%CtH2g+Khz3?>A`jN(Y*i z*Jlm6zYpg3^SU`Kod14;66Q;cU{B92&%)b?%%I^%?lCNJT@_v}N&U&`v1|s)C{2!U zA{hUIKC0p3C65e2y$LSy?J!)_czfGh*J~`|citcLbvqZlR_kypPU$@+&G{#bDq7x# zArLT7B;f?oAj+S*b%wT5pCxt&UU%^LFBD2>ey({p&(0t^+8n+XtZxIH$RgGS+(p3w ztQ#IT^BR#qk=MuV1t~(M@rq(WbEu{0P^2}_n=A<@W6_=WA%RaBBI2S3{ngDFToZET z0x(FJsD!yXgjZ#7zJmkZ*h@qu5Al)M#)v0`_pbP+Ch_edCRCWA&jS~@a_l4^jNf9? zuO$Z5@ZRNj`5*p$o#>3bgE+;tixI!5f1ZU#PUp(+gPnA}FRo8tP^aB+LK2mLe3iq# zdF(5US1!$4GQh|tI*7787{Z|e>yZi7Ki<*)s55N#{DdpogCGm^S&cCWlbY=JD03r= zuO#)fWa5>SNO0EI@0=4M=6Qo>tTwzX2x>%|UfPgF83vi)nz*Zvw%PTcp_UAN-bY@g z{k;6BZ=2*uqh80go_??2@Hr3VR`c(%GNbQ-@;Er?j12JL0Px!1F_d@SvP%I1VJeW1 zCKRk6TZrkW-tNbby$TGn2HFtT=NC-Z`RqyekG|Y6c7G^>n3tSic?QM<_VX4TU3XvG zQKf=K12mQss{*dm)qwJ##d5sF)d&JKX>kVNQs@QV-(3}rM1_j(Rwmyo&=R?8-@eR$ z(i-$oScNxObdoLWuxkBTifEW1)T4_TShE6yf#22MtJf^h4O*j-_{Tf@4o(k#ZXFvG1pJpLq=g4XBu8s!3$mY)}Ov$yd$i_JP? z>777STn&+q2e}r})n#?>0+6a?af{ zZKx*RcpiQBwnTbR`zqpo3Sn5;R8?5$MbPVrOH)uMwdCRCFQ@Im88ryu^|Oz3F+vV{ zU-A!>ob0zYr3*~j3F=l_X?v+w*6<8)kUi2?(Y1`l+C{f3rg$_IB@l63s z@Ze9DXmwEvYAahkHSr;}PswFOBN}Zv-9ixXXY}ln6 z477Z1og1Qca}>ekmyk^G9;pN-{F*6X0@9!IYIwye0uL7*D#!APmOXh!@}#H9>Kh(Y z(@slE51l%hizwhwhDmJufBEx#@Nll=bT+i3<|`_a>+>4^7u2`C{IJ#8jom_@pWEo_ zeRR3^7h10dD!EBuvY8}-C7-7Y7rfR_T(!>d&>08mp^y1k$u9bq{5**Hmrf0yzg@Xs zXow-PKiB_DY{LFeWy@VY*yOu6`bAYF^Ru$=yRDxie6okChH65#8fcqWCw`37zwNVg zI*+3mLJlmdS^3H6FZdo7^l@G;C7()fU2uZh#l&(6wu;>`fpmtAoN={B%uks&;6i*@S zRzIzZKKKNBIH~>Gw$?W|2DyCahnw1pC?qiluY48?$ z{NVZy0yZYYPRrl~&L`(Ot#3u^+C6xY5(VpH>m~Keqcgro=FCrE@&Qc}n0Q zQa+7EZI7Uqb^5kx`;lQn8X7j-*Un`)xke4 zc_4u4Z?j|P(8;$8S9XS4qB-_Wchi5oQu(5=zp8ye`28x=AOuJ=ORA4*u7T>4wmy>A zKxzfm?<2mzyt=CL$c^dbkLQPjY?X3Nt4J*);6W+yq#c`^BjD=E8_GD4$JxBG#aUd| z*LNRyOXb^}D>=|7h^W5{Yyl&Dskj{Hr#g;b!>T+b%?^(w!~%UX%e;Ve(VOA3GleV* zVzITyEym4 z@ax1};DTJOA8)YCc^40K6Ma{Jm7Pd3bG3YwS+7^fPK&JE0!XONjFbBX^4q`hxocal zq1TWYW=%4vCj|m5z;ZLsE$JAQJY67NAMq1dx5{N{jl!c z!dz(%yHleR{g(%q*X7wy{T%?C+k!tD|$;yruv6^vZ~tP}!~ ztp~@w`OZG@KWAN1Z@~zek|`scoTI)Npv$d*tkjP|k_ciXQ?xh~3Qwyjp0VbJA-b^w z8wE$Ftn*AFUbfhkHT@z$oop)4{rvRia+=0{RMQHO68@*Es|<*$>(-PobV_$CNJw|5 zN~g4Rh;$7pEiElAA{|m9B?AaZhjdGKcilbj7x!NOt21-X-fOLAJ>ju3?Kb~ea)d|B ziythDF2nBZ$rxne;e+r>BZr>7i_>a zRmJ$+=M`M0enV*s=Q%7s>o)niUfYk8SgyZL_UZfN#bI}6nh@8Qe36N`EF4{4MOWFA z-Te5-V=F_Yd|Qt=k|`U?y>(aN@mrjvB#hA8{#>nLdA#x|VXy7MYYp(z{ZU z<@WhmGu~#O0|i&uG1qR!(Yl~)^n4Txe<4qcZb-W!G=F|(eWPj)O5_^&zHJZqwh1)s zEnHIo>YKWriY8(27_p;z@+?9MC)*hn{VgNk#j?tLFRgJ^CrLTZ4G6{}Y5MAHUvYsp zmUf_Eq(ioM?e-XHTsp8t?ocFTph_0v_5mTpZCPf0X;I0(TVc<>N}$9+)4M-E(_LR@ zf7|V?$PW(ROH-j?;jORB1YN1P|y)%wNnBc{r}lg&$0Q>9vyE{OBec+RY-T4A4VwJjlU|`e`eQ6F}ccwMH7P zb%VqUs9wKLV%Mu`GX&tvI#4aWv)9b@UQaK`@DvF(D4AM9L4`nVt2?Og4R8R)9RV|C`sc_5#HUn;ymILmQyl#607k zo=Jx*mAstd6T zxjC)->$lk`cD5s{ol1;{6>#xWVLW)l0rqn=Y%91x49m80!y!Zl+aJ+>&htyaG{=Wt zT$aoaYTf`_Vnb5T3lsO<2fJAx)xv_q%k1`YScv2hUe&@`m>9}Xqb!675pgH$ZJ(!H zjJFAAE9Cm?IN~fq6RQo{;SI<#OR6uGuCqQM$RrH#WSDx6P3k_TfB8E4^biSaCOj8A zz*#eMLGO_TKqqw#kkTsG{JAd&I=VX4_PGHRQAb@I?84`2q@x><_tMV}&wS~Ja zN(b+D-o>>+rTM3Q<;jVZ(-M8u{cFFj1Bpu*uCf5`6Lyr3^rm9FN*PuYD)LY(wGsw8 zHCB}Nq4V55UCcUC4Q2wwXnuzebrY{knMmRnZ=4^F+aq6F*vbid0{DqUe^0j2@@=ia zv|wbd;s~ol!>s5ok*;p(lZ7SLVA2l$J(=-d&4CA8!KFaGH* z0^qe6n%cOfBcv*o=~HcT{D^zNDBj_)FfXd*Ot)T+1K|qma5#51J3SpA9Lt_Cjoaqjsb=#^56<4;qUPu9 zBs;LheJQMe(Omfh;4kFV@IxeVKsWvqk5m!f0j*Zsk0J*m6u*c0AyG^IgDmd06suXD ze6iOmv$Jk94^VqTcj*I+CZOVC(W+NoXLJ&@QW;tI(H<;ZQn-93ge|M{K?i zL5zohNs1bq5{YpX9=&V1`grrTby{_Kf+ejH2x#34NiIHWJ7WuKqdW^FjpmV%0;~j zH|y_O0d-M}3Au5I<`*WM4ud(4fK2Yc?Xx05xyj*TFD+8PsQ67}pD+3Lu+bTc+Opz% zQMvTcWl4^n*!R4{Sdak&nCX!FaG_0OHIm99>_%*f`Z&Wqi(#iDI zG)KFDL%^W`a1j$M8+}6{M?B!=6+h;pS3uQ7vsZJ?jy@~DWQi*|m*m1Tvo+_#O3p7* zU#2JF#6Fj%j{CTaX?foEnC7qaT#Y|umTXrE98~F5+uECN!%@Szph}DK<3f(x=rM8M z3L1vFA@tzFyS&w41hWYLd>O&NK;)V^B)P2(Fr93M_;LBBjSu#cbi+u9nSsl5+pOkd zw-WEdFOdTsaf&*Il4oE(3kX{%hs^4|HzccFeBGUkiC!PHw_62mYyq``jD;~>xS#O3&m*c(K zm(cxeN!~ARdFdO58v`!PcVOR~P-+l&+xPP}>PCcpNzWo37%^zh4+2yssa3~1fjiYP zI0oU&Md{-Bx&N&fkAq%?Nk%YsgB5sV;4q32o#f0O08ln|pJmn;RDN* z9`@F{H;wkP(RcGn6E86|^$6iKQE!*GF?v$0MJWlE zoMLDI>8hz7F5bJz%9vq9?;;T_-28F-#l|W!Gav3dTyy@oI#AWTy3tmgDPaejW!p4Y zmNgI>{Qd^pKfIHZ^+#CuO;Gc)xXtoWNWjVGT_Z0#&=AC43*!+la$Gb*!k}7jFH~OC zayuXEx2J{NL^^r3~C^KMz-x149d6Hi;Y;VX6? zz1y5VYg=G#C?xM}(eSXrF;;c+|L12Ts(`G`>aExEY5gP{KlaJGwcco&wkSJzURjkJBdPfTCls+m9wsw=DlPR~6QxyH{!2vuxKuFpV5cSFkq zQ>vkc?kP}d@&nL2Rx8uZ=7sb^j?$UUAE%O_v&O>exFV0UmIJ>^DTAFdK9J#H3N%l? z*CSCsmbuTV&x*)HGE+iKxOTo1su+H5XWT@K6Wv{INh)jE-J~WV&ruNc=j86QV{hL5 zlZv|_17nRT>r$5hN0FC-R<&s#gHFv4rB_8hwVS&9y0n6fSt>ATU69|WkLt0~si8Y= zQ+H*?@Fj3Hs6iKT_^mVALo}JM8Ci`BpPNK9Y5>I2tA{){PQLND?2Ig%!5t<9t42-z zwl*^(-S&i-g}q+XP*Ws=UJyUHZ2x+v@&Gew-e(&2tYXkNDK)EcTaKx@&z$bC$dyx) z_3tAMyq+pXIL!NRJQBh%yhr^i0s}D)L3R+&Y6ZJ1Hj;o|to=EJ+G8CHaUNJe507BW z7$Ck@24hOY+ zLyAfpFBqk#`eMsTtn*JpA}S#YIs`IedWG7<;G;T2_%*hJsP@-@pGo zeOos3=BZ>x&h09T(Lw692wRM?8ww=-;v7eQ9aI&ER?sT&et{5T_hZt(HQ1g}`wn2S z<=E42%I{l}bw0QMJ*ptoreafyfRxOwgPLEaA6!vCs>~)w@I()R;Q=?{6HD$6n)uJl zUz~R5jHLyG1`glktSn)TOy(cxlQI7)w|I61=5j_0$OgH*=O#BokK$4~vLYntKXQEMh240{|5+qBe&zJ$?-#7uUz@PT8Y4NV29%RmR z&!_{tT~~=eeU|+yFIqx>Z(NELV>zfawZQsGS`Qz&f{j!YSDcfvbm==nyhxch#?Ch5NT%xX_vs}m8gsH$G!vB4dhA|Ln zj-G^sNLJ#T=KKVSc;+$3^)hhYXlzs|#;B4wR7+2E3q{G&G}bFr(rKhWLLD3hxN6h+&QjM>|lf9XeQIp<%C^ zDvAn!bbYK~B_-naRzRMzULEUnufmL7+G}Q>#M#{ATHM^lrp?DY$WR^?S{Sh{{GaW` zKL^y9PSS1zP!%RT8cd&E8gGq9IsZMLHVFOoogo`@$LK24Ly@rOCxjwaingZ)p%&!n z43z<7w4lE!SC&%fD)W{Sy8G?b$*-zHElP*b)g&%m#r=;E%WkgfU{aq0!3UN_VRo;T ze6EWxF12wdZHc(SbhGIAMy1s*r)TLyzFlj#uJm4DI1u2f)-;ace&RgB1~Xj*bz}h4 zb0eCMtjH5D=uwVkO@aP1wv*NNi+U!9Z#{fh51l>7@3F})AqRM)h$Qat6kU>i>JSRoC6UO zzDiG_krA7 zYwWR4d`@=!Ne$=IL;n0JC@__|y1Zv!OoD0o*;}9S^B=TWbqK%7i(p7PnFl|_@g-SofG54_}a{%_+)69nRVIQe{W^{rKy@7AXy-Boxu z*$Em&M|2i#@km1s26$bu2#u-*UdV`(bY^F+YJ3o@E1qsX>7?-2!xozrkwAOJ<@D^g z{@WbC^WtZ~iYuYPIUcr*79COES@Z2XPg!sm(%$E2`)gc^T|;Xb8$;*>t7Ej&Vma3N z7ketJ%IYIXTNAJ?OP^qj;yf+JC@ z(3Yb0_L8I8tTZ%~-J$B_&Vm6Mv$T?kf*l2Q848Cr-d;A_)V;&y`Q>R|Z+tw^ za|d$iygqF^g)9~WWc{LJ~2^4Iu`|U zh=uxA&Av@Rhad_gl7&g+Ej1MH<0$D~l?~wKHGO>yBTKB#eq)`~jujUTV%*o5Uy$R| z7OsMjR2NR=uIblju|eXjt9DKu&cQ}5znQR2tZGlXpoLTSn+g(k4MZIjz^mxe&f^V9FFTxeP`Z1A6IW1#Y{-RNEY=w) zeKzyfLBUn*M9a%O?_bsp2TxCnGDut3h|eWV@Y&J{Y)d-Bv3E^}Qu>uD#NvZwnrZCd z+uj)HqhGE??mS-0F&+_(4vL;K?LTw4_<8`*a-rs;_Y!jEOv{-vP)EDtEv;JxKvytXG&7WACaC#*-P&1>7v z)L!QvvSsjX9DwualfEf*7@vc0A8SKx9c`aK(|omJ#(|?R$R(MfY!N6GuILj^taYwUHVt1 zbX5`{vE=x=bhxp)N6ChEo&+?&o=1tZV-)5M1ed9Qa>#O@h_pxyvz@K^Ng}r)HH?g^ zt-QkYtWW{Hra{BDs>=Yk&O9zIQO9y!)YZ2^%YbMFI!D^P7%L>+ue=zG|8rh1DdF4D zySqm^J+xeJne~zD@R8(XB!{dSS|VOnHe}M}RcGFgAnS+~X&@lg+2;`={m4WTpos%Kv&S=MxBgA;%b3;Ko^T zme?-JN6o%#F)7r_#5_qzHME|9g6@7P95}XgkcD2mC}h-jfqJv zkp6X7@Z|%c?M%K5u8S=kyOZymMP=_`Mzx?6Ox*MuygTDW=?!T^XUQDS<*3IaShHk# zBgBJy^AIz`lYb}h^D4$009*4_z&o>q=TEvt>8ND(WwfEa5;q>i|B2dj92s6i{y}?X zRzU$a6PG0V$ma{Q10}ksBElhkh^R6v9c8DJhOV!+JCN3qRE!m$w3&80!MB{CD8k2# zUox~Mj8D2m3T$fJC@sWJRIt3*k~aea zY!3M38PJ3cAu6xa2i(jql7|iPGKvqo?#thReUYo@OllHUM)g~-iQlOyL==c7{DaUX zZl{jAZZUm~uA64>69QF;3x6wR@W<5<3e-#9FCps29@}*cnIA*-w?v-gOY67LvPrN^ zO9kJx-t51Kjv@yu^?!fhD~2}lRUlTipNaD7Xu+COR!0@>k=vVWkKlM+TQn#?1oaP4 zc?{U+Go@({81^W1!$L^1gwW-MZnbW*fgwk`Z{NpROK&luy=paucxTP7$M&sP1MlS5 z3k^ca??HL&n=H%It|?pOb4g=blM9?H0;DNHm-(xo*!ZfvKlbWpEArxWyjNGvLJwGu z#}btVQ|C>hvX{NxKAk*XwXMQue09-)tyDUMxQ`%)tfZ?7_6Dtxe$W;zpM)Op!7qm3 zV1&hP(TiQFc4!PX|5UD3%=PH|Wq^_?-HYEO9#W3ozD=)r5fw@_gcDTa0@JfL=r}># z(A8T2E%0t&^d`D|e?@0Xd=Zt~uv8tOwla-(I8LA2P26l_xhrlmf_+R~%yreq3z)p| zABRTSS@dvp(;2dAvyKesD-%kXa$0BXj%Y#_Hq>@FC%ld@sFO1S`2?f4(*LY9?1Iib z3A`O@SSSu&4tH0zXB%fm+R>H?#L~GNkXrg*Yd3d*{av=)Q1X6iQ^r#fpLy_=UGux@ z-^_eHjkkSYf9W6+570eA(&n8hM{6;w96TApC2P;M4Mx7d-H@PA)`o}8t^>O(6}KOE zYCO%&yWk0TIld<_DwSW84Lpno-dPPy%QO754(2^E@cbhn@h}`?d{PH@AXG>qNLBcC zR=!;}#_hc=zMJ(EJs5gw?hCA7KPg#Q(HB#Oj~d;yp) zhIzlvZwV@TpP62J^|00~@}&-sl^+SO0B9YyyHbN!U3&_V9visYvVeY(8d7Bxmbnk6 zCP(CN?B@8w{$LZ;*n(R3sgxErBr3U4B!-G1)=8MM6hi{v;KpY}l?L4FGS~emJ+V{(?n56+7|Wv3$y%n;fp?1;l9^E5lc${t9FnbaGd>SF8MP zP(k7JE(~k3j_&TGCW7dWlJA~AW?rqS)6Br~=<)^wj7WM(1t6=?TBEc}*-$t(m<=+N zK*5>s4Q#72>!6gPx-mHC0}-pz!833@;0Z6h33}QR&fQFHwi0?lx7?lXwv>mt^Tod#*S!^a`XKY>HuXC~mXgGPABCvzilSW4O$VEJ5Q z=%pT4DJtaFL2dQ3j`m!m29cF!3${wUDs9cOp)j=WUu|$#)htgjN@CTfT4RqcoC&dl zIFCFj->sY1Re!?$z-~E-|KqP$U&TY<>@bY4|FFYI@ld$toSj0)7SqJh&H?7;aW;=8 z^?of2upapYJVlesdquu?ps3ygrkcq+dKOcm{oJSCGtQ{j)sDG8x4vfZ3$6M5c#16# z{dnx)ix5M`)Ai~Ns0VQaeEVw_ygJ@^hhf`b=B3B>dU3p@Qrv;+#KMC`egxa${>VLn zuOV@J(28w*4%#Y~qj*_D-#vR1p3G|KGFPc~g_uwcgF-BCO3^Pq-9d*pMdNCa)mX6p znWLLPkj~YB9Hu*iO-W-#YDx4CF_=AQ~1`KpW@oM232bh@Syh5^s~Fl1d*zuXU0ku z>%A$SuIVYBq;MrR^G#{ul38VL9C*CJ{Y}2^3z%gE)S4vVE{exRO5&OILvhyDOtj9-T@_7HIQ79G<*B!+kd;h zcupjzlPVI^14^7o7GLV;ug^tuuKKO}flSX9m?ZoiRY%jVVgn4>WxcJpKZH^_ditn+ zYeQCX11^JSNsj5aN&1VMJ4(mYZl!*ioH)iHjL48a9d0asF00cp-mw#VynRITG{N2!y z>DyXG!eO{sLATrYkZtG0M+^=g2?`N)4rbZ576c0mDPI30l1=_*KgGM|5sw8p0YT`X z`NVLhcokrfs<(Y6_kaE4p4LoausD$Pv^#~g{dlbVT+hooN>aOhr6cd?&{+%+sHfj2 zSAWlgohMnZbmCSvVVx%_mUuHmOgG=4ppnT2Q@Lsk6tkl+`Eqr7*Ni zi%UEa49>bfkz%0AhWu|M4Va5u#6!Y6>>F~-+GaXCUJUw)vi9ES5oIjwo@f zC2@Aeww)eHZAaBGsEFTRnv?v1Q>-Z{#U8FJAD+HkksQ1E1~G-68ieVsG@~L;f&oK2 zxPGxT?}!5Lko`+$J)j;ZV~06c(b1i0iEi@Mu!duTVXg9N?_Gd_Zg_kJBa>>HYE?7} znq`S*2^x@6p&@$DMAZsO zaHWQayy$c`<$AZ!46tRle-39T?ylO-IEK z4c?8Y6}CJTh+bFsLjlA?eqCHX(ofWU_B5!E%v7d=M*7HOIc0vh`_A5Y8Y_yCVKr2 z4msLQS?-nq9PeqlF{KLt`G$^#XnnvWFx(Q#sVCdDi5EMjLjH6Til>aG=`<#|=A!c| z`@9)Z=b=R*P*dYL{j%yU3F=y;QBML5=;+?aq2Hi~l@<8qKDi^nnoKPe<30tS!Uz5- zvZcy{!1GazXoE)&7L_5N+=B6V$}4$@77k*odBAy1&W}lkIMGoL{{_KJAY#gU6T=8a5qx>I_;6>kQE)IJ z66fKy2^N293Prm0cqh9fd#~c;SVj^@7BX%m^NA1;Np)A+r|3N4_7N1d*kPPU_dpSD z6z~A5hLSdo_-)W(FwJc)Y{wWOtx+dv)R)Jqf3q>?xu;|?>3%Y&|4~#}d{?s0>*D?~ zr7~!8v(WPyLe3`m>@;kRLaP`2r;+}Fo&)M@-y4~FI1vwgE7BUEk`f~JQ96fIF~>6}){`ZHOL)k8RV1dK z@h-^Fn^Hsly_rct@aeAZ*B=U>t>GZ>HHF-4e&5(aRFkAR5i#QNZ}hdVyRT<(2M73P z%a)g8Gb}Dq;?Chb%#JNvLmEpjlkD^7Wg73?uu@@1`6)rZ3i8l*`}i?%7P3F2s=I9^ zgV7LLH?G4&$o!H^^XtpvxmHGrkfww5GxQh4)IQf=R%Z%Qjkg=YnlE{vqG^AorTy_9 zU9&>B*#%In5Zl#1pTVr7)c9%wkz_}sGm7Vau98~I;2yYhP z;08SoFr4YAH-bqA3`?Q?@R>F4)oU%KXukG)M(raAX*}u*-&pFSAvD4#QXVS@xc^noND!DG7X4tt_g z6s8{>wiIN0N(mb9x2iX~4P>|sfDkWpR+iMHxGsvr(u&3s>8+cF%Ux9iblGlaOT-l| z+{rXf89Rfy%-1cP`7N5OfC0cti*(6f z1R@E%soV+;K5aPecGf(ys1XEP#VjUn1i};Y%SXtoj z!`7$ta-AybPAV>g|@Tz|vOxo#vUvwnM^`LW#;mAi?FMSO zFz8ToS0PoELTgEy{cug<$VX{2dg7O2@=IVfB#b5?jI-&BN8FRq8We&R=R!U*#ye# z@C5aWKTnKjJ*2;A#cyrn|5*1pC`W2n$0y#tVT+lP|A z8|m|+Prb(L=nW6xs%RztZ50F;c}7L?b8cfI$>%eg^Za(~|3cm1c=bPsR@-FjIbR1G zJLw_miSb9DwI>qYJ8*Ue7@fbcJ%Wc>k#KzXir4iAadH)^58r>@mFH<4gOnh0S~`M# z(2?|%F;ZQ0>H>8Y^A!B@bOns_3EametsKv1N?Y4MnV8@&@sbgc{BIvki-L-Xcs%Lt z4P)4j+*1K0`V-(9)7_2$y&LjB=ed{x>rA*>7kkYkxVjyKtsxWcanXHVhJ?hPp=@)A+b8(wCLOMxBF^N*fUfar1J3Ee08HCSerh}8j1`Z?6 zIC{&>ETLczhp(mr4JFalBm_^i5~7AGzzP`VIoNAb+}ooQ#4I$_9r%@^Sm)&^$d15< z#)Lct0|5aIrX(w+Wd#&repyYZ0u^ttP->Mr35S?%DI8?Hg+KGP7rVg}5f_bx_H0MA zp>%58D1zEJ1c@^FOZ z;6NRczoK;SsEJSbCiLUtHBr&H{S>|#BWphL?p0wN;meQeS5q=Y75 z^M>8!_nJ)1rxrev%~`@SuM8Z26qr3^*Dp5D^w|ty>Ys@mLgIrTi!!B*h#!6rL14v4 z0(VBOo(3EMQf4>=T{mx4%FXX~9aWSS_$74$@HndvFd)w{#JSTkcxzd|=nt*weOfAe z9{26%n2HG5@eWfCeIR1=3WmaCR43|c@TD_A{hD)s1@RkxBJVAX1BGiHW5ZHL7VsS! z6(LE4L7>Ie@6VA~+y!XuT4sDLF%mMoyF)Z^dZi0K$SA#}Kkr%R>~C;YXE{Iodt|Z{ z);-=vzhbVu2aWf+B*kINF4fdJnmWfwFo4U&s!9# zH(%}^>E{*!>;(%6E~mGs+JWn`{&UPEyC6$zDjGHL!OV@pyiEeo%iRxd!BAucQl0m( z9=@R1!k)zI1^~;z#z`PdIcVPn@fs2*7eGWAX@xP{$Vi4{7MxZ3y1b>5`vCg&t0$N$sNOq6Gi;w~m%SYJHbd0?%&6xl9SOK`09%TY7N~d}y;0k^^+51oS70zbo zpe}{qG-=W~%o3K~bx7O>U&TFK#`);}$tXO(q)jgD>o7fVdwcDj6@Bjix$+xZR2w*K z>Nyl~zXKQAHp3aE&xfnBN*yYcXm9I(DPJOXK< zVlmTDg6aePVeaijYWhx`iF{O9+K`iRx~v`M@D_!V4gP2V%H?Wz2S*?jX2b9bfkM|8 z&ulpJ|9x;_NmH=kch(gHoaV5`4XJ-*PhfSS$7w`7E_O2vRyM%31VnwMgGd7*nQX;C z`aYnNK-2yBj5XM?DmXOyoPM!xwEPr8R5xM16*LYyxj6i#KLMcf>>m}8tNg2Cp^^m^ z9Z)rIt`5j}T>w6l;Q2M66V*JsngHs^tRj-d1BQU;USgSyw{Rz;Pu%hXA(2nbhM)?q>S7R5&PL?j`SvFVv=mqL2=-MQg|u_=)uM2E=s)kWoD9OK z$abuG&(J46wxf&3vK~pPGV;Lt;jqV(K^7Gy@&p8nOmw0lr+ukf3&-~Go|Z65$eU-6 zt^l3bKR>&gHi{|YxpYGw3r5MdFCXW>{$*Aca4_O%Bo;z{xlZjyZvTyT!m`MG)eHnK z0ySX6bB>MRhC{In>S>?k#y2z)ndKwDCO$r=$M|q^S&87Y0VN0)Ba#SElt>O5B!cDy z{AWQf3`-|}ueKuhjD5D4cs%zFQ*l+j{E|cgn_+P2z&(l8_}qzFL>-}JBEP4UBfq7m zlwn>u6oJMCo|_ds2;31giq~l^|KC$B2@VukO2@xCOp0y-i-mzFg_&=%fmOlRNCM`? z7q*XyC0y5E@>ul58<%Koej}nb*<4F-v6MFjrIX{2;{Pdqh~S%l2y6qYXIEX?gu8Z}5!g9`4vwuWP`uZ{8j7{2;m~=jHQ7h6!H~z}{>^oUfGsOOyoM zxZGO^o Date: Sat, 10 Sep 2022 14:58:08 -0500 Subject: [PATCH 537/947] Oops 1 != 2. --- Demos/Demos/Sponsors/SponsorDemo.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Demos/Demos/Sponsors/SponsorDemo.cs b/Demos/Demos/Sponsors/SponsorDemo.cs index 2e692521e..ba882fa20 100644 --- a/Demos/Demos/Sponsors/SponsorDemo.cs +++ b/Demos/Demos/Sponsors/SponsorDemo.cs @@ -93,7 +93,7 @@ public override void LoadGraphicalContent(ContentArchive content, RenderSurface Add(sponsors2, @"A. K. D.", @"Content\Sponsors\behattedpenguin.png", content, surface); Add(sponsors2, @"Cornmaiden xoxoxo", @"Content\Sponsors\healthyostrich.png", content, surface); Add(sponsors2, @"F.", @"Content\Sponsors\tootbush.png", content, surface); - Add(sponsors1, "G. T.", @"Content\Sponsors\ladybugwouldpreferlandvaluetax.png", content, surface); + Add(sponsors2, "G. T.", @"Content\Sponsors\ladybugwouldpreferlandvaluetax.png", content, surface); //These supporters are those who gave 1000 dollars a month (or historical backers of roughly equivalent or greater total contribution). Add(sponsors3, @"Neos VR", @"Content\Sponsors\spooky.png", content, surface); From 2c8495965d918275d3f26ffc0e2323c9ff065ec7 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 14 Sep 2022 14:24:07 -0500 Subject: [PATCH 538/947] Sponsor update! --- Demos/Content/Sponsors/beaverboss.png | Bin 0 -> 90259 bytes Demos/Demos.content | 3 ++- Demos/Demos/Sponsors/SponsorDemo.cs | 1 + 3 files changed, 3 insertions(+), 1 deletion(-) create mode 100644 Demos/Content/Sponsors/beaverboss.png diff --git a/Demos/Content/Sponsors/beaverboss.png b/Demos/Content/Sponsors/beaverboss.png new file mode 100644 index 0000000000000000000000000000000000000000..425b6d277151b9acd02a93718d329b2d380cd404 GIT binary patch literal 90259 zcmeFZcR1Gl{|3wzx)3fSTS(bdMr7o&vUk~{kd=`=LnVbGGqQIXA$u!XC3|FtWEB~a zk>~Zfy1&nHJm35G&-4HHK92i7j_U66`Mlq+ah~Vvd|i*VHI>Ln7)kK(@W@n@6?F0N z2p;0$9aBaV!S9s4npefc!*{lmm)Ewlw8X>Xe&+E^Qtg;5O-Pf%`>K0kj|{UP-K1pa z&d*WkG;QXS*KY&Ui!2J*&|z#hzf!CZJu384XFGi> z{`3BwTc4~KPV8pIKf6=#E~;}OWz_1-c}X$W{D9n4+A;bJJz-Nn-pkL@XLqaL=Ph>! zEnoAQZpymT=}xvjL`=a{8AEr9`f2gJC z7bVm2rey}@!<&b6OmqzZ>yY*j#U{~A#`fI2_VtgLme?f`!__I8;3TCUzwaSq zj=N(U^*!=E`x5tF9T)G3KfKgww`I0fR7N`yI_x3d!0oU{p6rMnX(+lh7J2pTK$`m7 z55yupviqVU{4{TPi`@K=KNj`LGa&yDT=Ebz38rc%m!nFt}tErgd zld88g$lfL8)uAn!q^CnehGq#BiHcTsJ2YDfNISgjAia{9CwlOs$0pEft>VWQzt_@I zBW}|pDQm51iri?nAUxE6{il(-W37%-*n$9q#l!!v|Hub5V<=Gn{Ws8TGkW+a`O7KZ z51;(&fdgvJLlHdm zm-u;t>ED0IcD_~k-$#tzy&BXUrNxzcgX7ItSG!hK`#!{>B?f<$G_$6FQ z&%e+8f0y#VOZi`>{C`vOf0^>{>hZtw_5Zi0VDZzQzW7h+4vHjL=zhV)>c1Wwd4>5% z+s)N=zKid5Ec|ABps?3J9VW;DdRL9-@}l_P-t~v(xujQdp>YLoZp+d7I|`w|KmMOf zEtS+q+@2|aF0+0zqy2m1>pR!0udkaKm)o){hSP?e_$QW41*4#OzyE5PzW+~;nvn~h zm@qYZbL$DSggbZc)Y?SN0D8l=Bs4l8_~7?uIMJix>RA$wDEI9l1Dhtjt^YK`Sz5O9 zG-3>5|2!mC3KdRwE}oE-c78Imz)OU?l{eL!CMD3Z#`SBq^@n)fT6epwHz6b^qm3)< zOGUeX>nTy-y0^a|^zv^Mc<+>OpUh^4au_KV)!&uc=-^ngKfU1V;TKCJ&=t!=y(xXS z?z|bkr09sF4U2?(6cHKY?vuEmr*Um?#v4$?)3nnPk8s^A$dBO9?i@v*LJ!l$+ffrDvOj!dDN|~j+dw)GN4?Mi4Uu??5 zxEo$&8+G2i(f>+NzDf+|z_|D0JA5&SSaY>w*RjHnk(=kmA1bX2M2lq;5nz;qgHnaceYfo|-97Lou@5&@{61FUb0g8ynsGfdXo1AI@^&Kg zWq-pfPnj7;FdlKr(~SX7{6y?ZPlXo;Y<=4eJIS{}lpTDUjM=V#-7vhEz;7`l z)KG(lkA8$5cXClEco`mSHVHvJclzU-9tGIKJi@c~>F4OTLgee$M7YMT&s>&%cu8Ev zer8iKjE0hoc}a4FhTuRjefmY3|HflxahK+F`U^J|qJJyZCYKjwfb3jO_(6eTPpcXda(K7)CkpD{P`7e6xUO`=x^cY z8b=Ku8Ui)q`PJ;1dKQX zzprhDYrMGlb#x)(Q;-bf{&K7)VE3oaF@>?TL}8mQGq1_Gm&JYLe@jOyB<=jtlC=x^ATo&a*id%5@s{_EZYC0en=szj0kVF}dyc6S}^KIQCiFR+bu(Hv-e7}Y%frZt%2VwMk1yyllHrex5y%{ zH~5xT@V|&r7{@(AF&(`49*-3sdX#ig@}*g=`{Tq=uj`h%VcuWFru{iy*KdUJbr^)i#qwLJ+>Qa7e<9nRNz zXPfRgUa4DQKeWp<_w+e|yUBy%DGC4Gk~;zQop+C)pl3@OQPlc+BlYHKuWiM>LaF`L zH*N843U2sI!j=ZE&?+`Qoco4$F_2;Z^W#8IOZsezq*tY4zHUy~&wVYemaP9&=2b|x zn^m)_1k3{I#5tmNHq{5>wHpeNS|g6Cz7g&*oEps1r_OmfLDTT@Nu4F5+FqNUPOtf1 zIX?e7@US2u(O;}m)M1#u=Y4bVF-yj}yIKxWT-A#^Rd=1=Y;bYP3A(-va@t7|zjO2Y z52=;&D5Z%NPg64no5Ra@mP(fXfsuC9Z0BPy8uMOZ6GoL5Vv;UcUK#Z=6dq7BCgv(} znv_VC_Vb)d;L=Dsv%j+zNvys}RpB%_)lnQAbPS2>$&*1Pt-!7y`&ofUiL zbYXx_nnlvny^nk86^X63K^4PYxX2sStqy{0hnI&>VuD@wzgMY!?TV9CzzaS)={*yC z+-iC7B|)h7>A%!USv#m%`<~0hL+nMu(_amH?=JQKPL=j6GlOu8y;s?^x6C)!aZ8{t znta7h7wY@3?<%f5GoNfvGSb+OmD{ymR6(cp@nl>`AT2bl(cSX0%;xxU%;cj}?Gk67 zlpoDMC~KD$%CZzOx>_B(IWSmY&{H{*n4U0-hUKUhcSd0?thLi?oSP2yulp|LloUCf;?WSf+j%y@DnC@@eoz&G``IF* zu+v4g{3ex!KRH$FJim)CPnit#*y7%i3JPQ_9%B_-lmNbTk@8XRke9-JQoaXeq8y*? z49wT-_kYyg&=RxU*7jU@UpxebMm0|}<>qqKT~-H+D%tx)7tDGT(QFtaw&4l0TC+F$ zMaKOlH{=_Syz_CN@6Q8GKVjfg`Xo2z2Zbpasx>;i?TKY>&lf_G_E;yD@T-Q>u?<|d zBgJMy0ekKYBDUJL$Vrb~tF=;^yjspks&nll2j^xW`Lbm@rq-Pa2!1d5- zQz$ndxT1denX^UNvSUc*u0Aoz`_@)P^cjJ4laJT^zQddEKV^|TJ)Fn4yVWXerl=r` z11!xekY4qd-$nEcp`OLJI9z5YLk||4jSpF7Z+4Qlmia!KON#M&s{d}R!r^O**VHp5 z)_}o_+}Sm0?#1^=WixDF`LhIXp%-;_AQ4IYPFtj3yX%?To@R_gN6BZSgm-&p3kCa_ z2r%0C(q=ho34)dM$B0f~y3MHkA9$>XOa~q{#B%GPl+)130lz-C)s4mRUZXNov@)2w zyYG0(b^Z~JfbrE&Qk&{&9od1;la!6kO$Ck0T9t1W97%hxTPVL{oF7`ze|=p*=3xE) zozsa-t_b?NpdMn3fVkN*6C+Qu)9%PeyXv=GOM|kdDIN zV|arqq~BN_6gn?Px){FiLTrl^$=j?0GWJD^ZanU*qdHpz1#0IS2CIL}$7p#969z=; zv6VF39(r|+U8SqyRdO=3r9&c7bmmv?us3$*W`DjWm@{&P+u?bQS32Hkzg}jgGPhr1 zKlG|`<3$vsJ6A z>_)Q0o4N7z!wcUW{EW|UY?%0{eyvTX?TB`(Wh zC>>``Zko79q!^cMY<618nLN+=*ghU4J%i5>lAQ68sp1I2k`TTn4B4751&BFTxi#rE z&G6Xi{xP{=((je!*(G&Xzq$l}N|97+^#84Jk5ag`YE!GQaLeb z;nd4(1@VpnAL0ds@5u~P8S+?pnFf^Jd>G~C+F z-ulW`e`KUc=>$bwMWGI=Ux_>=&>qPeDe<#5?6qE=W@bw|@3mJ74XLD{PW?vhxnYmJ z^&JeTmUFfL*25ue4cMYQ@ek+R2J>|)eaKD|hP*AJIn5pWi^t9S7~?uR zVRdw^h24yy-23OvOD;1(rD43oLM}7dIbAZ6YlVgvhR3VP@d-@Nq{}e1J!RqZjZQS{ zE3(-W4!ZXj!kZwp$75l!%7)G1{tj-FmtLxUH&V9D{WLbQ^NnX!>6)qJxpRZ*f1n1c0@$l2owyw)}=Gs0Nv-K-% z`R*Pg{M)9*5PUXQ*k{v&&X(YARb}syVzguJ2xYx%dt2)S!IgVVTYJNg&Nc4JEe{v* z`Tbgb{Ml&Je~CEIH*K==({sLAIRcDJ1Gaj4Qoy+Uyrv+j*Q@-M}%pXJh z!Tc}jQ@Oc(r9>fH1lYsW{{HGamUeL9_7YeD&`yc*&Dd!KOPl_j8nJLa+}oDEpidNx z9N$a;w2!GY%PWgeHTqhUdWHH`$)p2T-;$m?aVa3;njCqGMS{CpWZ?+i1y4ou(Q^AC zRp@ea(R~lrZdt61#`5T$E~X(lvp5_xW!BdEt}=d?#oK6zhgsF~G%)jej`r+?f|d+|QSWj6ZQc%yV(f`hXqR-IG~(!TB; zYp#cf#e9@2Lj~W?VXlPZj+7UUv^YsnV#PDqg_tkloVnAP3NnLLebOb?q@T&z^PZA> zX?KIu?(V7lfyeJmm%V?RX+ZD8*84~^r^2lrL0?jn%G=LuQhlxfH;bM^?E}Q zaredEj3~d%!^X%OO2NOCo9#VZo$d)$5A-Op|5l$;Tlg@C($nXS%-BiJ&NUx`n?M@SmBaPrl)0{zjS(Jo_dk@p;qXSZZ{ zP5E%$#Bl1Jb8kHQ<5#o(mh|h+bRflwQRQtTpS6Y^(`Ve{6%>RlN_fG~3EqCC9Zc7O zW3_yBKOC3U809MLA9peAA$r#cYd}hNN@`RAqo%Fw>gdm5t{>E1S<7uo!eDh0 z1Q95^7jq>L>VGjDU+d2aKS6&UJN()_ZMfLZb;(wiovgO@%T{^0oX#Z_T3jyO?c1l! zU^fTgQ86ErL_bglWK-h~4_$H-rWL^=B%?*NVb-Ld0m|e6-pWmV<|6TgT5;$OvHzdn z-!hfXJC9ci%s0-y4`)(pHuwMe{>~7?_u1xOrNB?di5`k@$H@fV?W4PKlK1K(eHL>> zd=cWsIDN(B#aX*E+T!#gwg@b~6$Z;DjoQ;ppQJ5wc-?T({{G%uiR$?e2|c5}ZT1=Y z?xs=r!O$=|y5jhrc(r(()p7X^Xdz-?r}1DzjmMzaRMj_Yyj{enZc>%~@RY-3Z3W0K z!eCq~=(mmEd}+9 zENBv=s;H%JCgrwY?9LH=)5M!hE+25<{o~nAjDDss3Zzmt&CdwbwOiQQ=9vSu@mqU zev(B`AqP48LnFJYB<}FZY&#r%U?q<7A5)6y))wYS4;&@@hT+!BjaVHm<*=XgB+lFB z3}^}X8fS2&A&nH%Q5kfq>2?B_WpBDHg3(_B-~MQJ!i!&;FXSfYJkr>zgNlhNiF!mW z6dKt>wU{TNkrGI8H;1uPX!6Q;r8Yx?$x*4bm67qAYAYL%Qqpuy`S-i}qZlz}AW+$s zZOsT-bztUV8$gMCZ<=9to&BENh0QzcD0DS;CL{C|$x_Y&Q`xxv!o4rOin!;eLum8Q z`92RDVq5KeyMbKxl;z$G`N5}Sj_d<3+~Wud@x_Q2hhGODJ3i)F0}cHGVd&}x2_=o| zfF9PI63a%5f0&rhCr}hJqs{MYdLJp@QL1q2F9)*)&5>FtZgmhTwehG__w=(&uGkV| zZpuqPIxDf+K?|TDJ$W$nMYTF>sm^OnQ#0eX7XNoZdR>GfN9m%|Q5Q2?*z3}Dx+Jk7 ze>3s`$jqa5ykHviqCLBGNT=~lqo11)cOCgUS?X2G!!NuyO0zA{SA(%#1Y4hn@S=#W z6e%`o3D(S088!G;IQo5oTP@u}h+lI!DoL+HP&_+rl@_TNve~}fo7LZkr$@9GcKFo@J2Bck5#1{7t!ynrM{KvfFf`}fuODg<1STGmc zyQ$G-+qJw1g!NxYe2kjX@$Ohq?3l^ zTqRyydfNR$Ow0pYHM1P=LGm|UmZ+d%2pTPKKMCS|oGKI8XjflaxZ5XSHJq;f?6l&1 zyouA#CoIRdBb!eenBI6HZ~5(0O4~OI7744jGc-a+QcMvv_yiwO9?OFl0_2X`diAu5 z0v%=^B0&*^eFg0zM>X2lpYOcymhHeOWIN@6J^Ko+5Tx^~Ft;A*k*fF;6XU2auC}&i z+bBYkM@09_F*F;$+?p5RXv=Up&XwY>+o34!_v-;bT7)5#12j=3<%`=>v{?9<$i6nS z{bN#cdHp+pgL^6R9t7n7l(ozA z`N#iw0c29W0^Z!}&J^6P{2-8B5CTo7$3Elvd`$-_D#@Y(_$0gh@5QXh>|5PQ*Xd7N zBA_dp_CK;muJ6{jyoJ-5f5oIIUVQz(r^U6t@WMIio*YSUo>p2u`4@fC(hz=L7CLWb ztO5BF?Y%X-Wwh+=i{{cZH~Hd6PdN1ZBl6hPDw+YFQUtCgSljmsYi zST*{E(U*{zdJJ7FyZu?nBm(+B>lxhj$qZY=;Jc$O9F1?mr zrmO(MRfI29dgNmU2^88Ji@lHiz4?vew-JN)JC&*goqwRk)u1A)$6a^Q&Jm&tLhc=t zhrp=4JyETrnjpy1XEoCtEOdLcq~3uDL_+yc^6eas`&WPxnY3pX)rP}88Yq-JvVj&3 zf_|`jUF*Q!73voDbDzX5<&ZYd5w-bEHMAwsE=(We($ow{KZ3*R{)cxIGW;W^ZW^sE z0lQy9UE5i_o)<$iXj}|8fINJJXbZX}7P5UsagS-b4QLL2PPp}Sf{d*|F(vga*^TOD zv>)>H?Zg1+G@z@4=uo6H zJ9C_z`TXal=VVDQ(tn_Di(s;bwko7K}<0+Z-g#a)PAtz zPx>N0ikt>b$IJ`!g`W*>pRE%Z8m^aE1TXFwx;#E>sNm8O9HW^4!OR{E1B(xSYYjHF z2{YPqu@euVPVE{8>2>-`AD+0Zk3T7Q3jrGG%ChW5Cg!a~3I4w~C~3k@rmXn$r3bSf zPL&oj_g_(s>SM94K+sRy0Jv5zW}#qf6D?d8PU*PXf30Qn!u8`rwO6tV4Xc7KwZEON6I zU8URW4A9Y{%C)1`o?AE6UD14I|LT1OMhuByA#>NFdp|u2ovqUBq?UMbdr(g+gxNVQ zS5R93*EK#6Zq(Huxw8hBld7jk3=Os=+NARIT;9Lk>Iuk^fr>`ePst7sKazN{E91}M z{);O)1rOaTB8i@4p6uqxuBqRWKUU9(CJ_vdlV`-@YLwH-Udv^V{pS^nrN5x+xvrEf zis9-!5%fH3%!}L!uJ!%3rVmWc^*5HQiNaLlcu(Fr|K;h#Ftt2bnlN=(=tznN#DodW z@nxe9lpLSkt7o*!hXQ|=zpFs{#YKX*grO3itG4r&|0wJj706a8RS$mZ7|(+@0P(K7$%C^k3c)Ia{@G_vjjZS zB-pAdmeAl8U+BXV3^(nS?kf~B&VB)~b`A~Dn2ek-0S3Js)0lq%whLfh?~52TM|vmo z2oRB-A;586K*8yTRzT=yA1Q%?&P2zC(P5j2FEiXEBQ&@RBbJBrO!ru!Zm);o8Xsh@ z2$@n<(o)~A;Q`$Sp<8V@`5`J9FF0fVpn5zDwdZ6qQ$5m*ajyPC#B;sgyOnZ9(iIq{ zh}k=P_Q=a;PVi-+zNPvV|GdX}^7li*+QriYv^p%0QjM=rEk|hbJb~86NC+`_Qn&7u z!|N9I$qmha3cMj=$vB3h#g}e*S1V(FZV4n-g#wS2;ShqDEh2r=pO4t181W}{Z_zQU zX8+1m3{P^hlr%aWHUW50^Q7%y{}FL^Nko!P*Ag5aUhoI0N%Wv^GC zdrd{fBCAfV^;un*P*f(1-+Al)xLE3RRg@ZMnp3S@v*0uWL1vfthl@dYGhIKPht z7^Qej7G~_-D?dP?mZQ2_cY}Y666vjM0q`>YHfr61G~S!tVj2ynF5G;uNX*(7R!lqA z6xg&R#^=ZfO({WH9#*>{@RTS2#=g?kh%LCy72cgz097&o14=+X=dWXYp9X~ze!BE6Ie%PUTFU<+pu5X7 zy;g&@Plk;}#INS4p6sLk@Yl2W2XX05*VxP-@s-?sPh!`f{kT*9NI6Nu!~SX(M55q4 zqduvk^&ER9MM+O!@XWz5a zj+h4q$X!lyG1kJnQK2p8Kk)#oPw2j4yEfJEX6!g4LbLmM(nIZ_kzBv~+qzGz>z`+W zSDo+iu^E&_K_5i_EIIYv{`jw@ytFQOU3GI_a4SGtC4(4pNem)-fcO(GUAiQ=a%oZ# zK5~}2ccvG_N_y7;U`bEJs{Z*!TEe9)E`dB4qM`aCSI`yuhU+)gjaI0pg_`d}WSmM(wn zeI0>b?6-4xY+ca{s-X4yOiQi_Oz+Nt($cNmWJiG9`Dm>NU%Lp`r}h-c3Cdt-jgO1n z$pxDZHo`{_9p|613!lK{#d)L}i9iRd3CY!Xaf`93c=vIou`RUx{+D`rm@3cjW50)^ zgE?k6@ud@Opi4U~3k4wWt}Ccr=jjNdO`_p6h6!LJqJQu>N0R)VpCA`h<0jwJ&wt)? zbu@|=#F^CmB4l7c&!{o)4PX<8#IbC3s9NKWB<~8-8rOah^H)71hT#)}`O&uhhcKKD zDKBg}edwctD?RI>c){#}ABD5Z(fo!bSit8RX(`bCMVuzYGvgZH}%AG47xoz;Z!a2BSd5%8NZ`J~b#@7xRI6)T0)G3vBjB&5e(Fw>G2bd7!!fX+ z9m0*2-8bHW&m%CwN7S~T%W~|P5{qfeocP}O2Eeq`b?BPGW5FVF!K6pEH{Ly5KOGeG zj_}WB+7bQqyB3u^(W+jUy?nm1uwIkp2M>1%|L{e}-a_JnwdDu@g@vGa2&&-QNQYxu zE>9H2ubYr-@Y{JhmIkm_0%ZR{DZn?X?SyD6Gi+6q(S5lw9#>N90Q*IFRdMxnt+dPR z(#s&JN&TLWAz$H{#NCl{4s@Wa_PPmkgl&7 z0jnzGe4C!se$aUdd|N<`k5UN+@;ZJ>tB1Bs{S(TOJt*h}0YC-%4L7}g8NXk*#mMeA z;_OPT?7->C`2{_yniu=BG>-KS$@NhwjTh1O>q$Zdop(Cn8yQ4F;J)&Xkj~Vii!b}# zEz(2^&NTMZL0Daw$+$KL;pa*~n}19BOXa1jFw$^Lp%{NkVe*Za!|xWbG83S2( zFu+Lj%CPh%iJ#InctHDL800^43MiqijLI=i+Jj3kKz$#tqF_iOif||VtGzWNYFNE! zS_EfAkZ6@Q!X2Fm-;B$(*|VRzK#+VcJ?}UO>!TOBTYtJ=n!8c z$oolgIUP$3y^i7R$xGK}4tJ(0j0CoN6eIQIvq|#GI0OxU&=B!QSa~PXP{&au}<0 zWZJ%rOCj_FQW867G>Ns1rtL{!T^zm8qoZ1EPgTu`P1Oqlh2Oy6l|!8J1?CNZa2WGd|b#7X`o*f*S4>A6u(RL?~n;=il1v)p&`vdD>gaVrL5 zCy1JGdE~Y83uon{+ubqL%F(^zEZj(!N_!=68ReT?+&FX_l*=NY!^64x^3q zBV^P~7-I-xo?mjyg>mc!t9?V5N?I-T=Va!g|F*Mex?V?1m?8;ZgE;~F+*&S#GuJep zD31JFX#letUbGWWLDk-y0ib?Cw6D&~nMWs6q4gGs0H>&Ec#X=kOP=tjyiuG)x1LKs zCwC=+{)XR~2BHx6eM6`~v#*~^;`-)W5>u~958Zt2jHsF3mDf#A8F6_QPN;nO`)p^{ z$}gOm5P~VO(fjw;gb&L@1su@;j{rBkK*=-G^wT|uCKCc+n)!74B?0&SdyRUI0styw zLBt3-6X~TqvP6U=g)jc3L~g-@dx1nNJ(Kvyh?D^=naNxxLY!~AI>2)7CVNCi`utk` z%+4_wN7xrOX`ca;L@JsvqV^HWj`W~~6a6tn-|xx23#Fjbh_aW~Ie&jLiwDBHc@wpF z%jEU-2KBi?j#W_#=v3iSGIxoP^6sqzcTHzA^v$v?H7etha?(Xd#%o`p0p~i5%F)I} zfa;o$L?aDzU8V^WOf6&j{`3M(Vw*9*uM@C_aclxHHRo>JgF6VPEk;aR%S&0C`XG0b zcL2!Uyj)asE_IO9GW{Ul*r$cg5=4f$w3wXOsu47|_AtG$qo!TBBzJXb6ErNUi z$;ak7RIRcSErj~vIWS{OLfD0s1O{lk21oa9F_zkZU@a_eqmuj zbgJIRZBC;W+;ZY?STaR%1>phI(X;C9Pc*3wK}qQ78w5%2@tB60z!vCFNyU~-MprKi zB5GwHN^WdDfo>Z>WClF=KB{FF874SA>v?P5$Jr5~Vk%S!==I1(X z7Li|26EyzWbFhPv%IbJk&mhYrxrFh1VO#oZs2hqjxneVCL0U*SRP6 zmXSvHgvR-z_CaczjmfN*AWCd=2#S%pf1smKX*7~mW-25r;{)c4%agG@d*y z1e)q4@U-~@`{*w=lWhML_Y?@7D=Iih8=P-e9Z~EXbCEJhyn}Qnp|uiUvOZsuh2D*Ks&>DXs)DKF>l;oc`^yaq)6!aU+mA8!+}F?CrQEzP1_&%?!+xE^Gz z@VEC~4YdG&G%PRQnSHQ!g5Jdy1t}%0pSl?O=f$j4@cVE&rt!8yz6U7jq4cPQd~R@0 zZAxfG;)>c$+C{LIbW{Myy8Cg*4aOTl8g3!7!|;pY(`ehw&oh~qLd$d@WqHAG)pcc{ zJTX1UN&dtqoy~FQrn83BRXOtDh9qpOYu^(rGt*W+Cg;nTNE9N{PLJd4&66mNBoX3E z&u)WXWCL6@wSE>`r{I;=^8t@WY8EnuUVC+wN3Ux8{h#VPb3&J6AsV~3mIj2U1j#r# z*TFriT6yo$)R9ObRbrhUS8${B|~y6`1IFfhhNpH)jr0-KR??Teg0G{o*}Q_|VF&|>rQ zJzuF@#U7J`hC=G6aK~bt9zR1Nq=o_s?Mf^-C%?YTOC?#FN1OnwPX{EC8WjrA!(V(! zlVt0~wEp_DQR24+7+w@$zN}kQf=QkKbi`mXnR;=X3<3o(LLyY>|mYcks42huP zQp~O)SEMwmhbh6K+ob`(B-GvK1~cT3H#1C;_T9;vx5l%3d1;I;Aws>XiBJCnng765sV}ILuAhF?eM_->qV@8F;$R6pvJ_It7pX8qDz1t_i zO1h{Y1Jh1ntr@^e@iKpQ<-=$NmMu>{JjLG&?Kn~VP9`zpsTFctP)`?PD{&mZ_|Dj1 zlZ^GUlzsU9;}jQBa-NTsv!*^0;)+8tUGFvd;px6w-h)>K#X_3dko&bO?HxGliWmYc zHdlSh*WTFl&;~pUJlMTs`2Z?H4A8MokkG#LQ3@A&6F|6}rRG#e*C=k1P)bE836(m? z?5_&XQTR-j(G_7ZVVtcaNa_Zn!4}+Z__g47$h|726{f(WKRZptCw|Xeu!x1Yvw*6L zR3cA-z&{6>1rHZKrMysD|53NL7kB^8abhWO^jFGeoRaqKklvZNe=Ugssscw%O-&m~ z228b;PLS9=Edh|SA>iS_|cdZ2>m%CLaddeTR4kWc?8sLbV8IZfI# z3l~n`PvVy__L+aC6rqO5zcm|Od|76na4$;W&+P(Y#G{251!i@Xc3Vl#%A_sqte0sJ zlEDn%A4lK{QDCQkh78Xk6*n5Q$3ulq()ZW%0P0Gp^;i+QL{8b;8aGV6EP^j8n=z;v zb+@PC*O)`w&$r-ejWqND#UaOU=SP3822;&^w3_ZXr9Cp5#jIyzA;co4F9cg?TSV<0 z)!Iwx1+Mr|CCDOaOibd!YXtVy_`AVi%Wod+fJFqmc_vk*Qpw-yS|*v|^4Me8;otcn z{YOAmXJFaFo)cjcv1rBv_spvk3_@pe)1x>~uw1-NFUwBxQAQpvmOzLno$tPgxRpSP zghw<~Nt-gh@(}2WZ0mWD3a5^>lN=nb1&T#$9}nZqYW(t$slVSm;6h=$iO0Xion_R3|V%4V#TVjBnX8g8Lu@Tc1fUhP8sy`RP3XwU48)*IgUtop3+vHHobYcT|?( z`76?Wu27<{vk1*0<%JnBvOXJ-%R`}hq<4~kz7^JfdP+_Kigop7xL1eD#~jaJxNBi6uPk2KR-Zc>Jq&8*mC)`(vX9$eO+Y0{;$VQ zhXW4VepTv;LL1QAvg?no;x=1m$bunQ9-dIxRQ-$Dg}C6w-G<9!gPK*iQqBY(u9$0h zT^auojTN4lJs*5IU~hDRqQ7Ue{pM`U_Vm%85pmT+Mizp25_UAe2>SBA;BC`cABEG5Zwfc_`$xoSx9 zRQyohxan@gow4V|^`N@EaJ*levI_RE^Oa9iJjd*OMizCpV6TF_CQmMeWMeJx$V7zQ zf4!abTs>P-40mcKU)YxIO*6%-U=RZEd_$_r?e7#ri2!_hHy9r;G{T-qlG2s-^GJBw zAjR|7L2upB$=>X$mam}CHMd!!d`Xgt@QZT5$X!|UQ0n4AEQJkt&D&J`d|eHzzb8QM zi&ci%&1!XvSsB3Om^xa`c4fGzl8a!CP1{4Xu;{rkQwGvz`_2n;-^CGfixoCFOvFHWwl=PIpWp6;>PeB zS2?8s#^kwOqD=jfnpt%`RvRaJ8{$i^Vy7d}g5aMzdHExUfR`XFEwpxP{1i@V>U^kIOKsr5 z=hUb4x^!g*#)+|jR=OZNt(Nt^ybXEBsqCr#jir~;Y&$FX)T9tC8Xd9{PZSP)#WMM# zKA4D(Z(Yv@)KTWQ1ET|zU=*Qxz}}zzHJ^l0$~Ow2z~KddBr!C2ys|u;m+H&=CM6vq zI?Xl+in;j3>C_J}`U#B%Xmq^zs58m%y-83pCCdnRW@3{0e(QSD#;J_ldwm|xyQ!V1vy&d=;F912l8+<>r%Zdg1MQ6CU z+TA0IJJPe2u$vzj?I|7l9=wN!q@tB|Du4ThbK?bl;`e3c*JH$YzK@S|h`CRCjC4Y_ z*!mkDyVO@{O?3Qze}%4ARaJ^>7f&w3O|``1rwkuLw4u+i^#Wdqs@o!y#Oa4vU$~N- z_BdnVGngg9?_eOGmFRv25F(SyLZ|;+y2*R@0&SN*i;xAH{+J@}3?K(lL_Y3uy?{>N z2`%wh0^vWY7`!oT;vCX&{l4_F^Uh(mTU@b4RE2!i^j%4lF7I6&Gl-Zr*Sh>WTSbmq z=Z*cN&|}aGl_P^Ly_EMIOV=mR&aoaTYjB(@um*~{X+$~>@`P>&y(U4moK-i1Q zx!%W*)GrOFUsK5jJx^6PPsDNS$ib?UO@xj)|M4z)3h6Uo1g>6GWe*c3T)ig;3zjsK zzr5a&iHW}%Fj@svTo7UTb(^K%MV4K3NFgQ3UqZKUjm3;IHf%_S(Mr|e(>b!+OOM?p zy?%UT`UH>qMx0ax_e~2?0>L0PK1a#R938wVl}?(`uDtcY6IfjV>h{^q<&VDJrb&g~ z2ykkLS%EqSVEv*{r5{rngsy{z1|(n{R3>M=tghW5RN=c-??hJzI$Q*G`)cJAyLBrt zlVzHG%p?p!4LO|u*7}$_mtw^(@Jp&tN&V*ZJ z^4-0qTv-Q~MaYXSI)W@j;_v5g4$BZZrWfE#Q0ujpni}>}^egI%{!82RFEcIod7OK` zpIm-c@4Kg3-4E8nJ}>P$_odTt##+AB!E9RDLkdf#>{eoUW}?}bB1PC}xNmc*`M!Dy zAB=HH!NM&+8Ipw^0Wj?g-I-ND1dN~iQGdHyJvdX4Rm@QI;?K!vPrrLAiv^63)fK4` zgsie2(^+T7K2o&}GED&#se<;_nJ&{f9#{-JdI7?PkT@VmJND;x}B z+F*+P^)Xi}7EvIuY>JShx&$hspXp|7f3NlgR~+}uGhJGy)+aek8wBnD$=tVXc?0|q zKHvW5oEg79)MX8SxBBE)yF>|L6NJqrSgnjzlt>ZQJIrK33J)K0bzSOv{>)&=d7tEW zw%JDd^T!|A!fAT#qW<9ZQg>DfD|lm6ZD@d2BqajRpqR5D=o=s8DBg@@PkNTEZa<92 zrBWFWW>hyf&*{f#>AS_SKVBf`w8abkP3Qk?A+P{5LRjR(Ed>KZ!^noG!)DT-)sLEc zV^X)8_Ksp(3AfN2*h^ny_4+-dugyw-{FIVpY8<1gL_*U2{DM3p4*Ks3aL1+?Wn6=& z$HV6$U5poUz#x`v7uSR(CL}i)!NpT^>(!#Ic(98}$1LdD`GG$sYAo`Uv06`FNcoiM zyqKnbxbkds?|9*Y;-h#{wfk~?u4Fm3VWxT-Ggd-o@O>!ARj=m-rFQ%s-zYa=8)c40 z|1?>e>zd1mo{rK-oOM$$T&y-A&8f(``okgEmg@=6ry%@eP$iXcd#C`g{lA~2_y2S7 zTZ1bG_7`*-_kT#)59RYJoHvAUYPAIf0>l%Y@= zWnDX6{*59Sc4)dD5K~7mqo0KlJ>3%b41=vm)zyg_;kB4#^)_mpT5N)iEUKcNj4v`N z<})Z>b!NU2iBwj0?9+@h>(!!lC}EpV<&)7di?n5{Pah69oGG#GJI!&Ue4#pwhNj<3 zdfWJ(+^;u1omSWPiY@q&VA(+JiKU*Jun;(CRGx}wglL&YR(5s{>O8lS3cdo(B43gz z2r-q+NnNaVavVG<2h{3Ddsa_Atwy?0m6MrPgrwR18LKk)PTh0J&0I&y8pC*4C>hhj zlx(jX5`Z@#pqVeSyK~iIv;74MX~TZBpIo8@C0oBWPR77IpkiyWPhL5dxvt6SJqOwK z)$686iQHAhx@{2Bkx^CH4O|gy+`ZX^Ij@Q2M-J*tnkEldT#nuhC?s)+KThOldc`8| z2{MxReo_Gv`)ZR@w7WpvzQSBjn5YKi-;#m{$dWgO%)xp541!ak0PH)#*p=pG19JEL z?$0^m!B!a7Ah3wau<33QHQX_23rf^^Q8YLh&Oag39B0+w==QxIAL ze7p;+O0i|X^3lP{D9}gZU{$vZ3N-yMp-JB10)um~n?d=Un8Z-)?0{vM*Xrmq3&#nZ z({Sc&Fvzz_l`$!I+88!I+oTaIjrrNdGr;RzxWQuDeTSM~aR%3kKVP}x+II$915*dgdUZ1Z8(g<& zSL#G$U05rNvgDR79!x?7HxFOv=CP{MwQY-F=&y16MpF)U%u!M>hSP1#hM!|JKL9-` z^OWRkSq`(at?_O1srkS{RAA7m5f{sE6#d=1v(y!F(#-G#!&OY83GA@k+F&vMGcG=8xZHndRFxb{V0Ey=S zON`itsZKOqzF}$R?aA7qt9_P{(wwk9t$7QfrO-PKX^{ zO2fsGAozl1+XI3S5p4zSN9{yNDF*vo!NbD#;gw2f@MiNN3F8l8FuOUsEJA{zQRNW4 z7J9N5J9nJbpRMWWPo4k4&-OO^a^)Pras6*>v?{}Mz9jbRP3`^q?(nJx^p`RiAc~Y< ziNi&j6J5S(O_^GkP&MjWj_`*?<^i@y^}e zIM9QlPmY4jCH1qF+I7BzlHszy_u#Uoi zFFfg@L8I-OXU_%xRvTQYVY`Db9kL4k+6&jT>%H&vDGdHa-UM%nVJ!t&hH}^|bK-KC zsJ>u}dr?XPa%OQhABG(YYJx>9rxUc>^4`0V!DxW-cYZk{m~9+6ukv2)oNgJ%Czm?*rp)`Txa&D$!GJHUN-y0aE8 zpu;?YQ{G4gNYzGh#eo6VCGYgppLIi>YuJh|&Aa5iZs7WMJv~ zC@y{v*M?**;0Ue=4uL1{qxam<-afzoILQU%rHa_HjAf@4E1E)wrjHX0qi_p~K z7>LwQIF{ZO{g601*!=q8T+>#Y1p^^+4%jP%g*6ZNz*Yi^@dsO0+-ge4Lu|I=P0z!Z zAlKYkSKIk9nWLQ{_x8^A=jWoV8!k3u+L0dKQ;X$huVu^u zr&OiJ%E%{)75+Zvi@}+)`GB{>QzH~r;5!j&kgvx$ohsz{C4{aE$YTEhzCD6LG3)hM zpqkXzudVtpBSXGaOmFflkhWvMXvz8G=i(PZSj~-xy3)boHF2Xhtyy~_c><%tHpa`| zkdUsukjg%Jx59m?zy9`SqP-7bF1zWG*EPqsD4J&wTWt+&ROf)$Lm4VzKgf;vu_M0Q z1;1T7`_fOsT@ki(8>~RGxNyxFNGaGHv_>z}#?aE?CO8kMdz+5k3;*Mkfz(AO+Fr2| z(S&7a6klPoLOUr)>)bcA2eZ)#Zzhw@*=t{YO@a-N$=zmATgC6kdze~GsnsX*us+^h zCu`1Fq{XU?D-_uabtg4SwQuBd^J?rnh+&S)9whIs*+l0Sp34TOMjsih9C}st2Tk z)!16k?@0;99J(w}XDJuPNw3vHLjqUq1d$g2x8!J}_bdHZv;`)e2>@mRJ$jRSwr88f<*xX}(Q2d0~J z-mfZyg@CKzn8w3NM&kGZ97iBfl}kSndp2@*uhtJ}w(Ed~*n4Sxm?vtqkqy)J_uZ3A za#&b>)8i?cocbUq&x-};D=w&Sfn_}a$K-?3&T~GtODwj2<{h{i*!)oN z-7^)7viz{G71Bp+c4obuHL!IuL->=pJTf!_+23tq=V4uNvvwEvq0uv^J7p$3^KIAU zOsbsXKz~qO&3v=x6k`&e+UfyQbDQk_;TRc|$>&vlsin&E-S=3FXar3S5e5W~g9uMj zbFG-5PnP^9o#nsbTQ54n^!en$z&`~Zhieb`)4AA2@^$3zg2|_vVdGm5;_Zq9Z5sK) zfN0H^H%5@*tF*uboUv@IaF3MmIq~@iK~bsp*{nYWoBZIL6|4b5JWa8f{FVLq-Aw^7 zt?5E6NBk(L8c)jM^Dk^kM(8sT%$qD>*_Iy?i12}_CXXC<{eP&s4rr?T|Ih8pwQ_Oo zaIZb0tU}_tu957N%(6ly5wf!PUS;oHp+remB9vq+MOLy?Q7Y>H{_1&tzyI%?PUktD z^KkF?`x)=|YrW|^m3b_8Aw4BM6+-)9^lRHwTZ`e z0ah2TzdTu@&GcwK>*5^rj&dI~^&;7V3q2}+-qF`nt`bpOG@bx&mWf>9r3WzEC?56kPwi z_^f&S76WPL=*5mGrsJc%tH?3sO}X=?SmbK}xFo-fn4ax{dv$iE@z3vH@vAm(#Gy30 z*%UcG5~*;-b`)xeslw#&eWk#Ei1D2T&=8snspCG)`c;P}3zDi8bUX(vjYe~{vgk8+ z`%u`{Yp|^cY!$QKOca-Vhl>yw3|g(zzTej*;d#Z~KJoLs5AYmwa5CInu6_){Km{85 zPWkJWmqY;cXfi>dZb0*1#eP|G3q)AEx0fI5ZM;td^y*>ZBIAI4jOLf49t-DgkxT_A zK;P11@}hZnnZmEaQoihBvh>hUL@o*K8$rBxiYXTWy6R}sJJA2&xBt&y@Bc`FknqH* zYsSWm7Cpx83!ij6VuygY-5*{+lMWbereVIE)B2y^@Bhe7aJ#k?g>ijr)4O*`D7I@* zO4UgD``hH3|G*wU2hALFg&KuY{`1Az`){m(g+QJmOj&&`-&8Y*_yFxu{se3b_PcV2 z*u~TL&cx1hf~m&jPP*f&(rJ2!_{pOTuX2x$DTMgK{guFxCd_^bkc{^011Il_Rx;ha zANR50he-_^h_lXU8&*KM_61}+_w4Bx(0Dor2dCQIpdpzucUUe?v=-QRuGc&7E zK6@@VUAuY=#mjQH+EC1-!tkAllTVyr^|aHJ=F1{h1`Z92%p1=~NJ#7rg1FeejD+%9 zB7`&`99y&NqxgGtP8kJ5NCB6GpBzoy@1uEjtc`lQp?!_by)V@Wx#p+oeKsf^&wQqP zU1b>k;Fi$u(@lYqS#}*BQ)p z^P#%8H`12pJpbDt8*W6WF-YK$(dRk3Sdv;~yIyy%)oNqLool_hdSAXbCml0C;ogbpyX#*bE@XkTe-Pd z6kYClkyEWu_r(zJ-=Eiq1@%?EtP<%U+%l`En&xj=pcz%UV7SoqwU6ak$vjYi$xyKW zf?N`oluxhNFNzGR$jSu;g;L$2N!m1#OwljD*1plD4<*P4pq&%4N|ZA5Aan$Y&f0C( z%DC2ct0Z_F=`^riOu(OYZmB+?8j@M0xM@J89PET@+*}6eD+wWei+t<^TsViu0)T%@ zgWaTcd(`x-qeNy+S@siiD&FfG16W)LX)#cLgSxhQYPYc2eKLz!gqH+bNEbrGnwyq9 z;RhFbIGq$hyz2_!@(0&3#3U=+K$TI$C(_n+)9v{&b1og#QhF=;Ot)X#i*jRmgWZrMvpAbw? zebtiK^&8v*S|Hdt$1HmKPR6?{PZ?BB+aS80|NNk!Alcn^Di9P+_JDawS1}{yaXdu| zuT+Vm<{Jj)U&_oJF%vNo#{~Of`zrcwR5+6nLs&nCa~G=n&ySi?p?t~s;L3D+Mu=M0 zW0R|U<2j^BNcr}iVU%!8_|RNZbOCoJ>lQ!(x(qiZO!a*?7LwoS_wVZR{`*txD6LpG zUfU)1KnY+Ex=!-+n~>FGxW8r9C?BQFy3DK)6Iit*QR)f z5N71xrw;x0QLqO${+Q1O7SR-71UAoea5!i(FTzq(T&lOkwi17DxlZpxzmx0-SY?O; ztZ$N_piX#9t?_BMx&6;iW5?(Mt^h%B1f<6wF6c)*-_ZIOyE$Ri3ODqmBz7$j`*0ab zp0}J%|C*9H-^)S$Eluj5LZyNVPB3JP5S!P6K?el^4%|qAx#YR|fTFr?Ny7J#=1pmcPw_nkn43a;`OH4!i zNh10=Q97dqYI`z4Z4Skp0z9cC6-syd(pNXLc!bX#^StBzdB%Or>YC!6bdUczq_9`g z5o=Sab~1mFMCgXey)z*s(AOkGRltICZ!@`X+fA8DWS0>*cWm?u=!-I)Gfw~ShXFoS zybur;3&6bCzAkt?ehn9&C}OJo!7HU(15o+k@`=@bYzyP{w?qB3#{T#JcBD~;wQKyy z-C>}K2vK9H(EEv9Fx0M*fIa5Mym7mx<-!~%zv$c@ABWDQ`}3ib|9>YS?4NQez@7eHbn^&zwO_MR`{{fRLnQw}P)3^`%%u^gZ0a zFCbXU%Mviz-EBAzkp9?{TvwnRI*q57lk?l%3tE6_z4 zH_kn0>YS3n-_g|^TKMtrmk=Z%ok{om-`^f)>>8!e&voQcjkJ1_pbMqoMw zBnzx50AwrO?dn@enK&%t_$|%)e^0p2Gz-~N$Qin@RB$GyYy`YHw;>x$?&ga}&$LjN z_ItdxkE8?BJ3fT;S#K0^H#ge6zH9UORNKh^U%Wv20BNJI%LlyNdn&unihh0L&j^+Xp!j;EXqdm^T6Nm$NKW9#!If$!-FS z#=tp4&|V;sYR{bQacEl*gW+P0_g3HYGfl0si+%#k=vOEi4-dUW*u*aN%x-%o|FkeKR1MIl^pk607~!%w4*rGC88l;{@fN z?u`!+fCNX_$uSD~-B+YwO-x*1kOBzsihuqh9yu$5TB(o-OlT+bx4|GMO&z}R{ViOS z+YDU`sj#NftIfSjy96%bTl!gQ615(6+4Z?7RQ_GFt9Pe#;P-C8#538r}MB zkbNz~a=GDOEdb>oOdQYA(;(Mj{Tk%?;ta^z;!ZxJ#hnu{?7d>?B+D{*|yqKqyX5de1UYi-&tH0tX{s`;9ss0-2t%UN2AG{Y{lj z{h{1zY+F)3c zad_VC@^IojI@DGy>&OCF|JWwqC1}K6GzN*8=#4Cz(NiFpMt|}cb#ZOpys4X~tavfE z_IDePjJ@{gPtw!fPydFg|GmZ&gp+u)h^Ydz&?prYWtW%s(D>e05z6}%G|#{Vpf#y%@(JnLdN zrPWnEc4$Q$mTvzpSe=2W5)Mcm07Cpn-{oOI*E6eA} zj?GE7{sJ9-jli~-nX)4D7VHPk;Nb>io~UNq=2+sjv{HFfay&I-S=_3zZW}h8&)CH@ z72`0K`Itw4SEM|uJl890(S&_xkMh>F(2ILX@QLxxNbEI`{SMa0ets|Yq%sA zvJTwwTpTElbTjckpmFxL^3R20cL*(e=B_w~PjUk;+9BuNb%rt}FAfamI^diSIDVe+ z1GR%2RRjoH9By3%f^#jjtZVmm9H&`e-ky>HhiS>u?0JwI)U=7@``a%0!#Ol@l*4z8 zBN%#>0Su<|=ia(`j=?dZON>{0af8W!-HzoU?k*a?_h!~&1WAKn$@jl~(zX!vCPy;%_Ex6- z&I;VkLMt&Qo_(qedW|B9FgdSD;bdxn3d+Bg?F#q3Y6|LSM(H!fX9Xnk|+>7)WV9k_p>AxAdduh z{i|B28iZx_njCSa4hEMI$8&{8s$2(7-AWVd>W3K04+C0!Hv+e3`NZ}hy^fyehgbP$ z5qc#ewh*89(({RKb|d*o#977?kX1-Jv1KR`EzTc(lrwVwrKkM+wB?jK`ndsTS5rs^1AiVG2|-;+oLzgGjN|qUl#Zc z1b{tU?M9}k@QVg}NT75-^dQyq^r{mpn!c5nBKx zR4NK5vuckvLW0SqpC(YCWgJIt0Ow$UX!RkZNwPQ(j_Mvjzf-DHX$M8l?N}Au4hL0M zTFc%_BteXbpG6@&8C2Vr$i0QgFm$zgB9}{A$_6ae5R2o3I8Xx~n4K+DL+6OX!;ii{ z6Qp8h%CKnMiv-{8^|F-&`;V$z=$~B?M0Pc?)?&UUu>azr1hLrA_h>#fC;-Be~wTZk!hRXRK^58zPfJ0)kBDfGx0JD%!Vo`sBLmeBg-U$78R0a*umWwySC5XVB}45Oac$ zCIMYh;-voYWuIf-TNO=D6=emdR=NyY?8xolWnoHY41X|WFP<Iyd6<#1T62mG|PH5Rd^QGmnI)18AOj;%?3F%S1L7-g{IB0yL;POP{3_QbB z;U56qplMH(jn@sNZ1rB{-;o~)&~o#4VG#97jGj5wCZ9?{gHA@`J2hY%px17%nuF_6 z1HcTlahuzpgbF+tO;XTx6fR=re(Xx7x8dL@z80nKS8ctKG~nI6M{_P^5D_;x{LHX3 zYN~^jI8t^B*45BIS>9#bMBGb@j*YUo;*jhZA>jv{Od5zefsd9P{hilH74!^DNVu?g zqrm6PC4v6)qYR;~*!!OqMY3~I+n2YvHWj_r-c^$NTLb@82u%Mx9)#9z8$+M(%^=W_ zqOH-(k|aX$(2snGTmT&lJT<#n3Qm{I1E2bYEBh zb-WZ&D8_ubYwP{X^_!Xj+r5o#buBbG6A@FKF8g>-in*U9xYzn(^t&`F{{3CrMSlj| zbjJb;xN?St0mOIi-*4~V7;JNrt^OV38mICGNTubXNw&M zS(!0H!F1}6#HGZA(CchGj3>P4KfaL^W_ML7`J&()#Z276mx_jc9uRW@uZ1ljK(`i? zMo&ZZh=|qHnZqZg|AbbX395%?oVQbJ@7%ERZ^VqA(gD+yAon}OYRFt3+1PPz1zi@+S+5?@75pIS` z3&qO>yNhY7KtDE8s5x?;{O8pIfsXO(dA>v7q|KMPgoIX`+d*)g^~z-P+q=tWAgi1V zCXQ191^CMxI2VxL%pcNZ$RvE+ zmUd6Wev_KC5Ie>snn%I$>ZBPyrG^iqp`A)!L}-T!%^Wq6gLMw)i&B`naKy#L0R1Jk z!OkJSjwwY~((o2>HN4Xk)E&z?%)8;EGZMOA)j88-Zrc&w+82MA%`Viy-MwUu!jFOoJUY6$oeG zcB@vNPsMk`Fmpa|B^ivD154f2wiy_!CA^Wid4&3;evLHnr8_1E=qcE zrGcCG`3!cR#C|KZ6P0vGK^Nr;>j{`3baz$Sk`!D9=j>|e)E^lU$T ziv2X#di0NvR^sjw`GZE1*drMEq%GGr9A2?gaQm~yuLc$%Y;BlVZG#{B+F1SfMJYH zu{cw$0q;9nHR@3ZXrFL?XwCMLqM(+^JwTC7W=BrzAoKwi;6qDc67o2?rb2MWX zU@|r&{ct+z`97DZJ8$bRrcd8nP&f_p&S0GEBg~QC*I~b-EQ>GuhJOWce!u4dOI2i9 zBoT=;y?Mth5H3sSp%(ijz_l6XG#BUyl9yoS&|ovBh3~U*IxGw9K3Ep*_Y%ddI>DB3 zYIb&-sWX6y$wmXOW5TjGbwPutKq94f2v@|l{(&6J1`7gv`BL3t#O0v@HsQWZ+45p3 z!I*E!BCv*HO5(_O1v5-)!50Cq9_CNbRMc$5Ddkf+_N=CEH?_3bDcK zAA(L#>8O*@0g)TI-c$jf3w{DD(wUQZZ$0P&t~Lfk?@#H>cjgBL_e+U^3aJ}>ur_!U zJr8S#0MAEGuH6_UMeQ?!gj1ME1;-lqPUIN;NPi1dhpgS zg~X0KYY(Z}@8yq-qd&6btLoeJf^k#yeAV@-k+M0bSI`{z4XP_~t_eU)Rz2RpG89Kx;cJgoDLQb$^YnC)jQ zQOpeD*AhriEUG?BUg7s0_}Oe^MX(IC9>W>0b->AWvQc`xl@1ry#jRF#_m)R{ENqf* zboUDl%LnUF8X042<@L_R^}ud@6#2eDhwO`z9Ev5^89!bI;otB{!oE(2UxJu3GPG@- z@Lf9rSnc*$2pjgMpdrUngcmxkJ`}h;LQ8g!z3AW@tDOAh+7>GHsFoTIdKnW^U|m%a zOJ3U^k_JV6Wu4FGhDV<*N}lauR4$mwJK}2H^-Zltf^G2027RFG^Va(&v5rO;Kd6A9 zp4E$N3(3l(_nPO4-!OMi($Yd86an(`Q_P0k|(0gbdjyjD0B>B$H?8Q#(F{10z3w2^G zZ8Zp65)e8lDJjWw6{wmw0S&(Q}T_eUYeu{PcuZMbd57Sgiw z%_0~>xW88oGeBNRbH2%u2>Y;Kpv^?anmvJ8TX>W1W6@2W3_fw5aGseb{X(;_)^4%R z1S^kJ8hIsMcs%d&`tFmyLlGZ2MkcTAg z9U^Lynb`OP!e2T8K6h-q(X=8RhWBsf=Yn`>%^8?X@zqN@Cl_8A4xJQ#<>!Vo60AyN zZ{MwHBZ_;w+^P|FjAsS<;m>>dGf48)Ts=-5F!yX%>dGYA&XBJGx&-;5RNLX5Jf#$f~ zvqzsP2!jN}1

wmWqqgD{5rTk|QX{h4A=qk6tYA_OcJkw5+hEHItFQt*f6sY%|U{ z0^$cj`MfW4fX`UVvt7nADe>ay{FFG0BG#`RJiBL0#JIOkV<@!t>_+PDnru@E2AVq- zSn`)C9ArYbifbNnYsC|R6zPEEy4gIkO*hv`-qk&*NJhSI2WrgO!t3PIsvDQY`I2ZJ z(D{nl2P7z4z(UZyUI<|O>tsEo0&VlGu7it7LB{LW-e_|rP6ccC9-$c!*)MA6`^T+j94KKYW~ARZc(x@Y^8As}i-NR;dx3UnnvI|<%-u5}Wd8LkEvNr#e_`lzho&D_ zpV!sLVl$B-_3{XV9tB_iyvdd_d-ue>*8vPPku83qTl7$x_tA6bzxU=PtU}=LrV1#f ziSeyqG(NOL{r<(DAFl#C;Fe-NVnuJe+xLR3R-`ZQn8e8J@^B@K27%k}$w}DccUT~C zq`X=*?3Jv)3gio^Lw>w2IP}Su>jqoD-j9|~aSBfHQkJ-Y>`70a(2)U(ltwm1oB{49 zP)bw+5h;&(>ngni4(cXkKL+t}C0l$J(fnSw6s!Lk3DvBxb3Fn`mwQcNtZzs{iSJtt z#|i_Q@TS(jhd+T&b&cz*sD8;#6XusZTj#O)tmADoDA7C6ptWrjBI!6tD8i8X?EMdr zEL^vpnGjh890!J+48t6T%0A8pBfDctFc`)-Mj?KG`sf8bM$Oz#JTrAe@w5&GnMHvz zB0b-b-2R13vAg>R#K``vKK7?ml2@RX8oI8graH>?rE|2O25C4AnYI8)r3x^%!G;tcXbRB~}&C8Qk_Jq^;-d+;kft}zwes!GSD)n{zV z!s%%oN&N=OkP_Q-Ui*0+6DR~CdfjG>1hJl|@hF0bx2S5MjAP#cghMKgC_Ik^a3KiY zgby113RWaGFovZWpN^-esz3n7Ex%EYFSg$m${kQJ7A(!Ut8QMinb0hEIEHldq2YIQ z^S6zFM7F`(*XtZ!uxSO=0lj=a2UOxG{xH(2PHNAT4d3P~X_$6pI;)~7Ur?t}3!8EY z!>!5~GMcaR3q(HKD^l0t>VQXgS9=N>Q(ENnxk&T(lkfO7Gf`Tzu%4%|ruPf1gFc8R zP5KOE?^qgP>LX^NNmrtn)Dv9jsNF+@-fl)DX`Y51JTeFlP{&c6`?>p@O9Kk5KC?l7 z5$ecDN66+D-T6M=XVTMK6(81}eWWG>LZ=WZRs+nWOW_=^zj`kc&^A4TNILV5R4c>r zX7#r-Q0BblN%+`FXp16P^TOIXj}f%6!(5&E_0avp^CD-raq7YD*dsu{DCrI;8J?G}ldB^lU%iQ6GZI^j?L@38FcPt+2BLdP}KM-NBwf|?0G5;wt z@|lxpQSk2%k$EX={S8{}SKUBX-3deg9Grd2Qq0;rLz{*1_Os_sdEqyEC zI%MnIgU5AOGIf~%uREBC1)E&+a|8cC{tqF$mxY?pA*kzu8*lm+VO3^%VPs=vEWfd3 zRSfAIb8mTE!J6=nS6GlyGeLyz9lyDD&4YgA!0hHKQjRjBH8ac*wXjpap=Iq_dl=f8 z46g?0=T9@xO;5vWG>*Axs){7c@CPQTICl2;@rb$E9403?3z$Lh9pAjrRhQj&Xn}I< z%Q5;9QFQy^JNH=Eol-mGpRtJ)Do?@FTAi%~5vht|**3r$k!k{No7LhXdQ)cHTKFm4 zv@bfH{S?+CW5!d?F#?ju3SWFKEtnEx6`p5*lbn@i;#i>1V`6S8RhZpG&zEgO?6P5r5^Sf{{lfkQd?&hTHWT6{+Q) z)-c$p{=Oq%vAY#r?=ms5DZY8XkfXpD8>)TSZKh+`pqKM+Rs8pFk{X2(5vj?#Go*_V zgJ^FSa^WJ;6kTIFHr;L|`}-wP=ZAXIW*B+L(^P9YO73tMZ}1giPj=_LF>pY7?Rh*&ZqrRBnSmd7N1*QRZm)??ZMUY=tJJ%1-|}P$9*tk zNL~|&1fE6rn{{^k_7$w2iGHkvz1_}$otvR^G_qc3+8NVQ3wec zUbX{?XJa4XC?zGMHqVL89ocZgA1l6%ZWJZC#Hl!9rQ~+EEtX7AOJY0RHq{S?gi*r` z^(cF|3H9@oE6o!h+^yxYrDHn~<*Zn#XL?_@`hDMkW`CP%6@LCme(EN*>-Ds6KzKss zZdRe3zgr^i{(2ILLJiY!Hb=%mr&pFGPj>y1lFt|WVD=$I`bWICXaZ-Z3CsMJm`DM; zJo>|_U+dMlKs<$5py*!3hk8+LBx>0oFbod_>8t%74S_RM*Km-tTqtR~S0>o?;<0>GD?&{S& zN*=gyLL||h;}d3qwNUd|2EYZz+ipGF9{re}F5cKgR zMitycL-EWZ*sza#hPyV~Md^Lia<3}s6SMWbfKYNn^qwk=)N;c3SkibF3A@&R;U0h| z!5s)4+EwBYn&yL*^~lIB>5q|SqEBq=jZSx`K4Z5#qsJ|8|8U;y%#SNp|M{JQST7Vz z&Bj8H$_B1y?1EDhB4id)KS0W4C`s^f#zi!hyn4hV5ab~y z?w-+iRuDuMgn89#sb~c$sYkwYBD*HM0R)7sa2PObEf$ zhb3iBu@@hjOV2IfJI>O-Y#32NFkhmXR!G`k&Zo7@tVsCtdQBtq>j)T1AUcip; z$Pc7!GpBs=4XvWeR3DKRad6;Jc`5^!oOZo+lVs6@~n)sP$`VDe7{^Ap&CJkLGdj4BEEYeOCHqqajI zCjR%81TrZEY&Sr!8qs}IG8>|V!wd0Yy%Z%krXM*$g={=A2%hGxi_(95)or@d-;&B# znBieO^N@Kg$C4kZO*l5qfO-ysOBTkduL5wqRekBdJg^Aq1yNA=cii!UR5gAmgjLF? z?)aHn`#AIl*;LEV9XOW2b{gd!x&@g%9S0a)`JS$uagAP|ZTx$A6lqX=-vHhl=RI!; z1IMfJ9?)GOvmByfsmFpqBLiE?IE{)`LtXy_Yb!zy zm4iQ2XGI{)Fs^nnbOB247QdT!Xkz`FdvF+=PsNg*5v36{{kQi#6y0DlJqouV}FGN!Aa%R?Ur| zCix?!*(FP>9y~6kbTI@ktQc9mdqB8$Da2DtO zv!QvhZL!=XacUbrzzcd(5y+UrK5VV1p%SW;Kg0zyi2mHfGoIg~*@|a5R)=-2>M5<}u=JzbXo}CK%&VKiq5gBAF<8f#K4;-t*nB zyH-8lMz40Vnqrzo3AlzxL-9XT!U(DI_dJ`DqI?vAwWjYn!HL*xJAl6EfS4oHS>BNL zq{m|%j#r~8iX)7vePK$WbZ1%ed3hk%++d>TNJm8>i<|}UerbEl>!3`elc;+3{`z;T z;KOO-QkEQ7L5dK3J#}i%TC>z{4AO#gJD%Mm5h*6L>x{zAjDi|ZBs~8gN(|ix602c(4Y=iYh-zbf7uTOn` zAGV|5vW*rKL=M*MJdBGtlm4x-to@8(pv{vxb~=M`c=@%w|JHiHsvui(n|F@$f6C#~m_cPJ_HDcwP?XDXi9A^mEV8 z(YEOp;Z{46Bhpqu_?SV$Jx@QFzXfsHo*D{u=W=8n6KJQ9nan-?;h9OR0@AK-CrFwcfbTCV;j0{^@mEGw#Y z=Ueu${1#wREDKq&cPN{tMz}5@i$z9&P0G^gF=zbyN5&jqe(f7jeT(Lhx984XBAej` zZ%0Douum+UzD%#a{Zq(!C!y#g-*wdG(m3DNI>dJ8tF{h@6P*{V4>>%?NyEfp*{9?s zjw-_lYDe5E-Q#d+8Sxdr`Hr9K!p1d6ZLk6oxl>Q1|L$|`HagUTa$I^UlE;aNccIZ8 z_S_SKy?{}}lGJ(L4Mo0@+WJV)-ofGKN8DYK1t^SW!AogWg}@mzumPj@(;cNd%p7h= z-9drPw zaGJyD7zN=mcrUcisuF$f1Vhv zc%Qv@W@wtswx3BHle*EH#i;63BlT}bCbe;-v7DV>-){p=V-_MvtM*!r`!fIvl{b-T zm?tSde<(DSW23q$M%$^S09lPAh>upTQ(2qZS)drt1_7hQY+)L=>+m24LfS@BV0aO9 zNSv}PgH6t`00smPK~*bus})L5Hw{7)7V=ha;3i@*g`&}<1{qO$jjx{sjBlT>A}qh< zF~E((NL7h@8?P$b7Y||-idr{G5o2Pq6Y@$NczDrBw_AI zGqia7M%*E!p+?*=3WGST!Y;&H#z|8SV~sUCr@P^H3Re-#9YfP>g;NxTV+*zZvPEf8 zZ9D!@{&Yg=SNYgGt%W$7iA-31&J3aKmJ2aD*%O|$>gSJtZfIjZH=OjNDim)5$b;C9 zzQsvQGRX!PPNc39z7et63c<%TPYMgUr@fG zR*qRcBoQBj>A4?bUnJDm&-|x2Nky-JVFOJDpA*-`5{I#k zWHAQDeDD6mTb(C(_OOs&hRI*2PbOgn>NJXLV7$oF7dDUxQUpcX478WhT?MmPrF#P_ zOo+|ylt@`=X*75*$GLkW9Sf%lYqQyw=z&jv9ho;MRE{Cy2EFp1I%56`Z&dzlUmZbv zKhc2YgJBGYy6J>Q(dXJysO62tGmIXZmc$_`Gvq3C1PqD;I5)wzEmDFz2#Ty^15+q$y|l`)5meN+t1 zg>hjmG(@cL%Dt|V@q(@I-VqqWyZqVC z{V(?ei<&vxQ4EceV)<5qAy<)V5*O}9cOe-7(_v}H)*#)szPzx(Cx6(6s??ER4J_DRMU|J?7TudZf_#GGJaKgnVZx0Lx zJS-Bzr3b`k44Y6xugx8nUV~>sJn2Ft-}d8Ezksj5lb7;kmO!hve^|3{DpvELF{WxE zo_8K74W1tD`zDu-p|}(p(04{CBq(HcKh#yn$5+UjZ_&WyT>SZD$St|iK8c)Nl|onM zdpSqYK)sz<4cm`UDGq2uRgBPntXngrV5+NOnC81NKVVCCLjU!oXz=hg=pXd1C`&e= zlVl=>rCpi{QR-XkG#42#B?BKo8d}D1%!}EG6cq&qMc0xUm%E^NP8pgPm)iezcE>~TR0QB>0~irw~Cbly9GZYY=5+&e&qL3y^;FsgBVRYW(+N* zLnGdQ4W#89hcBHmd{>_TZ&Aca8Ku_nN!^EXn2}`PGTD4oM{(*A=q>|Nt+zn5YijB9 z;YQvMo``6%-2!Fr3v^ru0Vfy$)0ay>_Ad{o)4I<|X*p3&RsSo|7w-7pqO`qUl9 z(iR9DP>5rDVDj?NTJjC~X<1Zwr^8dnUVy-==%3jA0~Fxh(Rj>sCfXJoqrp8gr_N(I z%t`X)wB9H_FZz-bX!9I6)lu4Q+WXs$`PvR{b4(hlmOSTn9kIb^O5-b(jy6lvY;Reww#qj!%bVg8jyqsTWcXJm-t zBHl58{Aa%zOf9-03(f9k;?)g7F%*kO@%Wz9e?RLAIuDp2qgwmppAoa2 z6l(}}iGCW!>l}HH12A~sQMR!$Et_u7xtx9jw7r|Q#TfY;(;`V!IqqT0mYUjqzu5uF)T`5oKU;kK z^rJ&1Y13x(^~7|Dvf64F@_egrBkFlc(Vd7P?kD+Tm!3JjO}m&LMEgC;66wt@?{KOf z;N+{-o*umYA_Xe4&j8X#0V!NQ#_T@mc(Ah&fxX3oFk%`Cx02P+X&Qt_H?d?vml7;m zswAu+7wjOaYl6ZF4k1JkvZB$trJa7M_O{w*_Z!2E?&EbO;fX<~=hQod=@KGc^B?i= z-i*O>7(92eNu}`&lF8H<>CEuT%w2l1u1-$crX3wz5nz4+B$_&sXA^kws0!};K=o`m zrFvKMsRmf;sF6gA`E=Pr*W1j161WGT9LGb%KO24)Zq37`Qe7;OQ`{}+oC7*L;MR#o zAc5gBHQIvY!anAuVIrDbV%TI(!Xj`}rm_BhW$9oD-&dK(J1m`@Ve&@PG^i769svIR z_-Wq#f6&=%3Tquz00B_1FiyWiF$LfmmJ-&%yplFJwwZ?G>98GK`y>`l_?~%HlaHlg zu`Ig-lH~BEd;kdPSpv4>)~sQ&N2LSnGZRjzVvs)(*MWZ{Lbi6gc(!EltbiNlDk7Lj zU@d*d9f>GtR^B}tx}rF>M_<&IT1`8FS>~1y*+}$;X+-@!!X)8_f%5_`Qp#P9d7Um8 zuDECZEzc)-c4h>2{zXI{(iG5Bipbn3%B`q?}^M$JS?70o9P!FY&UYkTP@h3@PI6e38DFDTKLNclPtk`Ff z;Zd&k0kHk*cu)Mg#OWc@fyPRf)I+dkq<5z!7tK~`8xoR+uor2Z-N4L;GUh8Gr@?JEJ@7nTcN?^F=S&%lTA7ze+E$5(8^Bjqil)`fMlE5mbRxm zKkLV=B}S%)ZbvUAl;>!i2x0;)hjtr3C5~USa25{nwm>{22(xZ zvKva?0889MgJ5qp)wv4YPA3ehC{2HW3x79Rxb@XxBmL+`1Uf`x3g}GZ7-Qwngvpqs zc@q#2gjvPRXuK%6snHs+Lk7vLKLg0}+D`a=oe!G@K#&z7UMdT7VrFFIIT_u<5Ni{^ zXU@}?VV4kv3(~OxaN=&iZyyPD8dE^L4lD0ed4VSCU!RtVnR1rOflA{(zer zPdjG?(j2nwZuL`tg(6Wn+hQ-{55KM_Gs*hS7RetiipT3eBG4)to#URtPU70GmG&5! zc>Tyc#^{G0)i3#$nVZ_Bs7|q2YB*sV9b@XgY?3xg*>o!Cla9Xj^{1}@da-@+o-AU7)gg;=v}*PFg4EumkHn!d zrpQIpaGb*GBuslQ$f`9i)iJj50Kxq(${3bQu{Qg8a39kz-%)^=KZBe?C*IxZ2_96y z9KXm1_vjD}4x~^q;f7u&3q+=AmVLLYvmZ7;eghw-IKcl-jH!F+LE+_-wF9Ql6~!=S z6O@1W5~Ju{B-Rl`{!hZtQ$r@=6WvmRGH;bPQZP+2Z2>#koSh?^N+e?n9`H8CPit(# z2DUpLe}{rhqX*tXescSLU0U_&Hz%qE&!PLXm~vmvIdQJ4#uqsPx=Wxr?sf9setNK6u%HpV265C52k9*|8H4xnAAjft#j3Xab^m_5cjvaD%ym8PDh-v-0NP z?PS(A(ZX}q6qii-daoCcG4Tgscm?h~lcYZ|y29q%XLpkb2%T-?h9r~^$gGJKY#vXS z*Je-Q_eLx5WIWy&Jkbd;oGBK#SK!^yjv<2Mu>)cZGF0~R+H!WLh1Y6z76m~u32M9YTua` z-k~8-k}Jd*CO$j;ecB+lI8}sI7CYP0bZwv8d1ee{Fh~ueq21}UqTzRNqi;Mg6k-t6 zZn*wENg@hzaFz1So}dY|A-H(-woUWP9M~2>%jH)s9+< z9q46Q7r#MYUv)*`jvQ`kSQDFhmL?H;g)1RMI4n9Ki)b)%>FvXsh4-zkEb3S3nWNB@ z_r>gx6GIeinowuntAK`5@HN>vIgTS{@bcBFLPF9dxASEY;q%5(bEG-c63~$qM8dgJ zo@2fyAXFy$10C05KLmp)&;HaXXJ+vRK+A5Q@u=K!WmMw7*X1AV0)z++$8Uk6J8>Cj z-HrHW3ckJvVpNRwha(-&CRF~#3jZqB#A!r}KOm!dJzGJb6?x$~=d=42Ywj6 zyr_tt?0RayanIG`-}BA{ygh(&IS7PpwRu^MbShqxl&>m(w65 z9_f3!$SFNX@YmPdefm@TgW>7X4D%RkUW0kv#%T~-QOsM`cIndA8gU#m@_&(PEG=r`PGP$2 zr^pI|%fKR7Kc(Y)XHI?8;L1GgZ~%<#Ew?NjmM>q^CFM`scY0Eg+F5GnE#UtPMCZ^O*f=#CQFL~<(BBh(JCWkTKF_sxX5k3MIs=IgoAd)flbLxa zQEi;e)%*tG9Z}$9uT{PY0) zoh1!9t-X8MZ}DGwqUsY7m`d}{<+7C_s83qP~#>t!<=BlmOC1!NIKFKdiR;3_xGC;aMt7DG2Knk@qjHH z3+SEL^_`<$D_9s-c%`}18%kC<9rrK2Zw;*YCKMOz15R|4BJ5~4b>E>ixiOpx3W?!z z%N-!8P3Zkxm7^S{BiUnr>%)xsHWTS`%194oP5N*TyGIB9kEkz?hPrS6XYP@)k1_Ub z#yZHpZ-p5QMfU6=S+f--sVsxBh9p|Vpe!YcNQx|l5K^=#TUm-Im9+h?kDl-Oozr># zxX=CEede=V@9TP9uLT)MFrv&#ywX&0CCDPwJO1q&d)LIx{XHQ3pJf-po12GR7)2r_ z0PAJm`<~~OjLWy3FPdPz=*2Hit5OZyd&hr%>Ib|h-`J3@54u0ni3F&>>2PSgl*|p> zqK0*|!7uApWm4H%S$9%PVJqebHZ1D9lI?E6_*bAREWQKD{nX@_b0I)2UPCom)>nHwLozQZR>X6K-4iJS6=InK=5w?SRF@0Ta=k55l2=0 zSfy3baI?4Ec->_bvpl%^H!uc@n>GckFDZzff_ULPC~)*BNGe+0RTjfJgEWM{!~7Qh znz>ik%eELYCa69ff<2jt8EL z-qt=#ZBxDQ-$}=aLFtdd?08!X8iEC_KP6khBY_*?OB?;`;>>KOz=oZV{@(s247c6j z$&feJO2m|Xvw^CPZY^1K1EpCHtl=quZ04U=>X*Okyl(*vC~y;wmHevSM*RzSj^Hq8 z=8DBPuq8@dJ!^|R;)H!Lvwr?CG`#im2+WT#rF?F%9(10CeDtHY8L0(HxwpS%8<~Qo zcsFnzyJ3E^wI}s?MVH@4A_~z`j|)<{55aZYjvgiEr=U;}o8Mz=RlK z6%^$?S$^6fNYt?uaGE-)SP-6^kx}~vDoQ*}Y(n!M(ZPP-0$B0zlXll>jfwbm+hyqF zb>)LLVY^WnpV*$Q4-5^O!!_L21BcT#5k6`PlCfFSbUs@o4$P{1z_?syX{=g0H}y&& z$fg8_cY8~0wb zt)GH*K_QVvA;CxP-?xhtQ82Sko=V0@Vk{dFUOV#PKn<~zInSDeZFTbkA-Avjrx&e_ z$GLr^COjt zS~mZ?UDx=AGer=2bPZOUz4$g*4T$MQ66#1!z^(*}5!=m}4=x#I&zn1+3)^Oz!4#6o zn?l%Ok8fx)HcCf3So^?df=_X(3769xjxxWP+@~bL1+1DFy$}kR)LFWuPThpZD#7cct8u#Husc%e zaJ@qP&QbQrmtcY_lTM&=9~cQNM-8V7t3`-1Y;*U_631!#SnKt zat6bnQF!5Jdj5C#387xr`5j@T4i~n;q~Zq=?x`PAnY8?Su@G3bgISl60_nH4?yf*^ zn%qYUgvL+J+{9xaM*hG+Z#S#sYAhDH!XlVodeJfx|K6D?9#o$N7h*eCqq+{=*`w_k zUTG}raJYndC9g8&@Z|*WNx7@#DG2-n_~{LIPxxym_iac1>-9EFsGa}XsE+?b7XMwg zr<9Lw2*zv=oT4mJsg0>pSE#~pv=kL+q)K&beP=Vlo1N3E4E}EDb^~$K0bO04h#XMV zP=36~zCdUzx(?Bt`{?abMP4!j6Iv))IKhUI`S1Dt-NQ5@O<%DW2^keKKYMY<33sE_ zj{p3?_*q>jFC!uPCLpwYHy?pp0FiQ3UtD<|$ooCl*1=SEE%xihe-&%%2DeEAG4<2L z;edbTKMhkVmwnr34XjJh3>)b37)tC&s05U}0!A+RcT^RDeSUkY2Nezzwqt_FFCl;6ISsx1pMC$mrj||}Y`N2rykWutJSBGXm zrc=v~?dq_x$0+>i`|`2?Jp6*bX9)qjHe+UL*<;Lm&R(&xijGed%l+SK2p$f~1fUS+37J;v zzmlBx*}>m)6x^XlFRlLlB{(B7F3~vB$IoQ(`IGk?8a8Qv8`Mv&}Bv*hDxRw z0h^qVuHz-}0#3;#f*Bzt1XtP!{P>nd)I6e#NF2tQjC*SZ`zpExH5YMg)luLdWfy_8 zI2~NaK0N2i-#ZDCGKH|L_mZf%#Z#-?O(hQYLEy5EFWv#X81{#-uloI8S_#~{1zbXD z?REaN_ssue&wN0_ze(Q5m$jo{L9yt6=bKat@x%6G+qUj^f+c$OxzO@Ma488~Fa?rc z58^TeB-c-Q=PtUOPVg?s^TdZUZ)J`(3#gSs-)$0 z09M)!VT{B^?T@d$L%~|S2NH4@zcpS|W!j2JjFfCV{yqHW(NErMuy|RDv+*S{U9kF} zZj%PwoHW=$R;grw{=g2=ZaTCgUv~QAKy({EB?0BN(D)6Iqsb z+M%^|Tn9W^6vP*9zY2dE!_p+6=BKd`VJFp4X7X1Z>xatwB$!`dN!L-x*16<2Q?Ee^d?sUQHC-=J{rDzQ2+lv z`Bf0MX2g7W{vzYeT4u)?&Kks8d`iA#TJ|O2`PYtP7&$AgAL{%)r|wAbC1ACnA&|U--K0Y-jGH3y&869VlaHJHaY&VV;4TFshn$e=OH}6)-8@%RFolM&XPkPR_ub zmbEsN9cR{WMcl4aUNVfXz*zSm$r7cOj5Y+{_*&|GOFoE2c}l;HX*{ z$u4$v+2PEpjKmZ>Gd@pcZ{0;bAY2c`hi95anq06bgr0n_UL@{cDVfJWEIE%KJhIUQ zXvdQd|7WO$V-$|eb#VLyoQKbXz4+_n-D7!1E&v?ML}8l5o3=?HSxZ$;)%h=~k6q+K zVN+_Hs{NLBW8x-G17u_6bOP|rr{+XXsNi@DiGQ+)`?l1Vj5BbxuR*0*JrI)GtEC`Ic#rb+>Zow4HpVdB_V<>iM zXMb|AL(G0KRe%iTWyt1^x;h)B!m}Oll`mrjtD|K#4`;vK34*`QnT+sZ>}i< z5YE5=r!{-B;aL|Ah$il)Q5tMd3H*rHCmpsf<@ zSe`r6O-UlzPX0-M1VNp`fE>%me#?<5^^!{irg5=?^z?X;FB|n1FNuPjB2C>M0*s;V*@m?Nd;fm}Tt3nNgy z6Pv9AIC(xwTV!GP7_HHYyN+c&LPNN$5_B<2{Ay#j;0)jObFpc9+O*gpwX+h|i|a3y zhxTjhI(5Xi0AY7_J!|B9l1?tl95-?&@q6U$dAlD3qWq~JgAY|K> z%;L_u+GM+N>5_%nt2t5DzVu-K`>k0EoNPsMt03J@hi2fanJF&V)hBhbpOCO4RoJ+QYv!I^IYgB-r|7qam{-%AT!iusGnwTmhYS_= z<5Tn7ujr%x9YoBDu->Hhlnu^Zm}M4M*nts01r@4K%LG01?hc3=TOa?Z-Y=F?#^mK0 zeHP7$!7DgW_)lcuoe!XSAFw1dbsN*;(>YNcz)Dj^dMQfU=kLt&h)-1}9HLjRUM%L% zuw(5znE5S18Wfl040xR$A!94ZIPi5 zMzZi=PKl4=%oS($09(Ualzh|AxNBZ(McM~XYkQ@%a{$I7$z44+>}f{!n{F^5Qb4mR z(`=tw3hT_1u4bhtU_M`1F8{Liy>cC~EOQi20| z_{~p<3dX{IB>V0PNS&;g9|IMWUtx!@=k}LOs^Ip0(I7@K;27M!{6nvkhbJ>T?SyXF z)+s2WC$hKe8HTxlIvee+SDK)fjlPD>Wnmz3GHS~95RL)?~TCBvs zSs6=2RVhEMvX_qf_R+1&IS{I97EG3n2VZEI`FGwKgL#R%oSe;gP$VKWbYi}S+Ho4K z6ZW#=5XbfY;6GA6E?Beg6;O*ZU^d?jWnPz4Ll4(^hr6`{30Gl|^ZONGP52Fn*ma1d z{wxOEZ5Jt>??9M*-SSLPiSKz(Z{xpYuTM(eNW~iUl$7zgfxQ+5?2vv>8{NYg zB|ZRF0F7J1s3@`(Cc0hbX^251 z?a^l@ASqp9ZF9@qOECiX2$ZrvQ0YItO&QkQ>I4!Ca1#(~TLh#~tu*g~)I*2Z%@iit zx$6j#;1|XIt5iaG$z}20u+gD8x>G?bkalv6ONISQB+Y#+_+B^B(uJ$wW$A0fRBX>V0&8LTo2o3xrj`YVG= zX$X9x5_8#1nK{Gk!F?B4>MH;OO=t)h3?Z=}fG(;H);EbTFYJO-$|dYM#C(W-94!C7 z^?5QgQwj@JybS6{Xwxb4RBp|`yaXQ|MXCk!2xjssc)}?mx-(|8X|EX=q$`a*!O_%2nJAs? zO7r{;m{W!tofI2dr6o|u^+|i^EW8GKyO_SZ&e2C=!#auE9=MWw-TsIv>!T1y@{T>r zFPkdeT`j6lW7Ai_t-P9Bz;OUX#&>FS0s)iAO?blQ%1^7m2tGfQHgJL7u}^qXlk5X4 zuu%`sa+Up`g}hp$^Fl0A`XFYh#(fBIw$4VQx)h}65tAoyn5}ga+O$Hzk1+a%9pn^0 zIx&?XGlge%BbXTvKnMs2C~Uj_o^c#_b8KE*!9b#b_MxbZiL9g{2H}ri+I+ zpez7a*;AIHWY7V`Jn5AN2xXE-!nD(?)GB)ahZ*>>74gy{pMQC>Qx#~C(x2ZC?#hzE z3Zce54`&y=*8@k|zIiyb9?>aSI*g>!V5>+n$C{7Cp2PQe&7X%C$IONaV&vLHpODZg zpKz{iu{ROwp%aH<3Go1DurpLSeuSMPiK+W%YS59}0sF3Q$i1Lx)M$9qa*ePOS3qB9 z+aQCbqG08GMk?6{m>Optde^1s?zayrKyr;PEX24FdoA>#>qjnKQS(11_GAl%mBi8I z+3h($Pp9fn;X+ua1B|^NAKT5huCkxIbOwvORaiF^y$BIqIu!^u%@2r-=7FUNsd8qR zO5j8jaDBX#xDhhR#~92i7K=#fWZ1n##vJBC{;#V%%+mJsBs)FT(}rs43(dYC9p*_c%qv9CD2*fvBa#276)pBeRD9Cv6b!Z0U zJ%uQy6qvmpi-8buD)P+T#29M$)y(1*B;}qY8*s9-@jo`}Ej$HcAFlhK*=C89bPP zOk;u~(I&IPg^T~e;{s!sHw)5-dUMJ&_tu2Lw~hEDRF}vF#4o=jZM$=y<)YdY@C(ZT zSYeGnur}E~b@{@T?q^Em?1aoMe8dN5(wOTaG-KKN{W#ki>+&qP%&CB->P!)tgxMS> z^Q1#em1C(68U|rFyfVQ5{n8%yltyrvbQ`TN=v=??mZ;fvM2k0Y zc|aOUP`o9Rxi1{1!j6Btj^@ZsW8J$w)G4KmT4>Cu9mh~rFZ2NSJS%iBe9|CKH{y-m z6@LxC-XEcK>8)LmH3qQ=M*SGHcwa%t%V$By4ntD;ZtK5hN9&z1Cdh=!NCfMaQbxD2 z%?iWRp!nd%cVH#U>k<$KLS%M=jvvPS%-Xjmej=H}MBw8=3|k2AlIk5-=ZR)i5n z^eCnc-9cUeY~$j9lhHZ2EML^iCq&NE1t-Udtw8D(FByC_Eu=oDeCq9!EhBKBNidtS z`I*6b5qvSIFQ7CFVcaAegLTwxsL@F4veAuVJrt$#JUtkz&WMmP_1lyJZ1|h74|e9!EWCj}=~h zExshSF98s2?c;i<;WZm3f<3Jy9q!()Q{|ur&wNbJll`Vb#N#L#0*pjtp%jsO>WPEJ zRsX(xbKEIpm;+M7!8p%%LVAnf1d02o5h${6AQ67xou#`ZKzwH)1E^sq0TJIsW+Su@ zT7!pRNbNLBAGmJ6KhJY<>x5aA`UK9~In@#0pesnL*(ID(hS~RFAt+gdvsw*|L`wVB z%guaXyl|Mf;}*p~VzzOJAuGp}$1;)yCquoLkh?&H9cL?C6$1`02I4LyuNy`4HjT<4 z%XJ8gX@HJ(4TibOzlDqb-`qa$#zg`l(c7ik!CYHeDiV5FN>uOi;8eT}HVU=z8c79o z#@k)oOcC_Pa0%bmW*c-l?}2fUIrM%&A*S)BH2d@wgONS(`E^4lKcL@T4qwxV+mb)Dq}nJa8$i#c7Atw`J@ux8*u%7lHAbzKBbiyGO%T zXjR-o_BH8(ebRoIQjgOb>>PP9Ksk_BfbWe7YcekxW;n}1e)Mr zj$bjsjJ|}Zbb&%yc6M^vi~eqo2Nwis56Vt7TxvX1=XLkWPHZ2KMciMb_zkrW9I|h? zB}`F^Xgj-BlSC8)wx%`TUb<6a9c#3o3pntZE``FbMwZBm(mP4u1V;#JX?B}%vRbl% zNrN$b-wukBOMeO0q-UvBa!{xhf`gp*+_GRw*?0CEj7P4b36@anV&NbBZF+kGn>M5E zw>scvyL+q4dHF=DqE#v*2-e_buDXtWW;e6b11OE{Qq*FInBM2>srvP2Yj&C1A^U`}Hw)`vQp2a^z2b{(SSM z^cDHzl>}?dh?%>nc1Xe4{gQwjILt!-9@)ktu^m)pzTr#~(Em;;FjH8k!;I0yN`&Rm zru#KIP){E`wK{;hB^Q3@U15>=_vKf6!S`wBsv3!t9jGvG%QO>Be~7$YOypfYW(g0aRG7xXmlP4_WX}zWDB1qW?Lc@ zXST*{7ZLP{vCMMhY)$Y$NB(^SI-qCKzPzbL*6*J?&y5%z+AJy|$y`lU|N6LmaJmsp zyxUDFN=EC$@cWoC;ZERqeXVzS?U{@XGehb`IRZsT0i*ykO5yN{dheQ{n zX>1FX1=l_AIFxo6ZLN)7OC_WVV3Rt7vV_=Y>;Bxm61%vcWHbqDQ0$EoAfrXFmG;C# zifB)X@$udIsA<%xa|;#${`MI{FWXPGlRR!;ljWtpFC>z;A-!aEZ{uy3)766;^91Ek ziVI`Mg}qm@V-qD^k?&4Sc6!$)b#E-TCIl`xPB~$3+T4J|dshS_L_ zi4Q=%4uLm;CRZeog6PK;;z%VVJj;>KB1Cq670-zV63yKdqLl^dNffPYFMG^MpOckk z8;$394KQA**osD|P3efIw4&!qWrS&)4iQ^c1Z+~oT5Z)U-O2BIBw%ZYZ$ck2wVhT1 z2Oau6(~5S}ZFpiPH`s<8h1h=yh^S6M8}#<t^UG)(#{a0y$8mvB`eeY?Md38B_zbX zX*=f7tp1A-#~K`0twnzUSMOqzS6EosufUfhzDLY#J@;Ivv)TNK$b{Q+Om(_p%S6c* zD?zBvI4r;jDTp<^U(OQv+qS^@%_6lO;bp>~=NLPohKEX6OEO+W-H(!W17d{OXn&*g z(Z6Qg?uU~|YbYlJV0;&~UBieW(in+aJubZGYX)nq0}R@66!PriKeIC@eS z*sY5`z~;xnTIlek{BjoyBlZ12@ocMPKih#Qs*?M3QTw0G%-{o2M_88^55McLG%$Ph zt``zd=HPO!uuUDG=pBBYSbgA%{bv(oSjmMeWPgzY1`R9+-=Qw_zte`mBLCWVho~LK zD3aDWF!VzjT&p>KAm2>SVQEO>L*>TmCzUmc9=?>}M9!3R_tfZWRUQ>uV>z3!kZOyR zKViGU5)c8wMp=@(l``A4k%WjPrV7bXxM4|_rSRH~9*qJcZod$2U+r*kInIucm8RJTtx^i8CMkT766OZj8U=+EM2nHoT5X08tJI{+; z0@@5oLejFqE~75A4IC`)OC7G`CLt~0~&CV zFDw!YGO9gA&vfR7Ryi8x*>4I_5InF&g`M~pnB}Ij2~zDqIK(3~NvGaXk~8P>4MZK6 z=yC_k32>>D#Id=&Qk7TE?YuAOjYh^_GD+yS!_2JXNu#g=e1B{#_u~`jZv8@t(?@qJ zX&%*=Zao6Dr1#wK?PaD8qc%LfARJon*yoD%J-?3PIkmOcZr=wYPt7}qhw$I~<&i9r z*|3&K1eLOPhO3iENZPg+aO52O{^#onIE+&l_e07zxXaeoQtpNuk)BcsL7)I4qWl`t%XZ z{{pjnKoxK7+ht|d!O8rKt1oF7QHLI9cj$q=pUN7n2dxzjByZ!=(9}AuPAEP1Ed%xA zBvtZhYy30kz3DSOnP4l+Nd4u%HrhYY?|sja%HjO-{)wNz8qcn$pbzp5EA?I*&+0}@ z8g7CTuT@oUEq`p`=#G83N#qd3CPLk!1A-`JB2nvAsfM4p{`2xIw8fc=`N?r4?~2oX zq$b~J#B=Zy1ip1xJ^ihQGf7;4WKRqpuY;qh-2JFgeT3PeCwG%}p4>Rcw!mM2;K4>p zT9<;%<1YK2R0&+jzTr?Yn3(z z8}z!Xrxny!Ad21mE5gKwgPC0~xK&aP1p~@CDcZ0)XA0vh=cOJHf0JNhs(8uL7{mv1 z&N{YAB8%0H|0?!Y6ioJGr>|(ez7`E^#*lHy-$*5pAf$V(ELXc8E+h28#mA`)V0h0T9;^5=tyO(2-SIpVCJL=|f* z`+mWcV0Lf&bv^V>_$CSW^wi?YSDKqD`Xwn`Bren{uzel1#UIJS?Rg#lWw7Q4I8!Vy z{W@91c_|hK+Hew6*|*$pC!g`0e98))?-bS~Jzc^$4*h5vUE(0bKIrl%f>`xx`hdqK zRQ`up^JWb2`)!!ahmq7Og&#qCvLzb}wQw|o5HTOsZfPSR!WP6vc!%Z9wI*k5lN$aN z(4ZF$v(`c@BB8dD$qq3Lye4TrclJ4`x!c2}k%J7V4pLRaSo5DQl3h}V=reZt3C9dz zunIyd@Tv==D@h}pndiRucpEH-3o^iD_t<{-A1p%pUOY_9Mpe==H{sPm*4Mj>mXg;A z_{$c88mi^XsDwh5KV+t+RJ%tu!PJq@<)&h0=NK?*eYoA2hRH~~0?V1rk>3SB*J0^j z^&WF*<7mHc6bgq?W2W3Hugkst-!$l$yAg&T7#jW8X3)BY*$*KR2b`AUmWiaftN3lv z!Dj3^&k^&{@fVSu3%FQFLadpyae0RkHsGQA7=5sENrLyjD9ehD?W2k zuMv%{c7dZ_9gGWGLMel10Zp)q!0-~meC3{r`W+}KPJbAgdyBS{%=SW$xW7L11J94B zeE>EqLbg$ckUBc=Dx6(cOjxassC=0n8j;7rlgLI(1 z=e27gjg36uL_I(e4Kdo7VO5?#_2ZqOJ!o^B?wlQs>T=;A_L}X5Ex*%oV5)yASh>fS z{5JZj&1f=@K|%0`4?gj~P7st=z%ZO2NyO!I+!n$aZvip5qKTi2hw>97m?<3yy{Ts3 zb=R9o^?!&7t>?fHJImKJihd3lZwu7} zhs3IbR$!Wz!&ZL-Y@W)~>P}up?0GUk8Z;WQD{@S-)?Npk00cJHk{8*$T*hag88nJt z8!r0@i@5Lo;*Y;De2xG$Am#~qG8>jJnb3f`N$3X26RfR+u@{16u-i~AJmrggCYBln z2E$jz`)BMxZ{T{AF31B{H-~%}eTDP=XCI$A+ZZ%rhNKJzgyW4X^MIC(5KNoSS$Z*5 z$b$%Hxc-*VdvfapfSqKe-Ew>fzCZ_=0VRvNbK~evSB(#Aqo>BMRs%z#pQR#%rP7us zB&uv`33|?Mtb@k_O^9f6EBJJC>yj_ix04c?XS9>+*0HOe#im&rWFGS93> zj%2g*1yEj?5FAAq2L~#O5jEUzt#|Fm`z#}jk$l^ZKR+kmAO?V#0j%IWoTE{X@Ul$oNHP%HSoOMi5aKh|fMexIK)nr}`g(FSf+YIj~-@tURuKU_RVL01*flF9JhPvz4N2MK(1DNZDtnb<2zTTJ4r6LWr zbJs3M!=(J;VJoE3Zc<@#MJzx3)#Duip;#yZ1Z{%jM&xAD{ zgqFD7WSp&;bGeu|wKo24mOB?n@$ZqGs&YNTBTI6zMaIW(FMw19cG781OuBhz!3iEo zDn*60l!v^mB5F#$J2I5_qdz8nV9e*{p=(AvXatOzdveg+d|EX9T#)Y#yn5dG6w<{~ zkv|e60|aGdo1<({O$CG24!(ECp#JmR4~tXhJ0b5sDPTg&PmjnZs9Xa1gQr0Pq=;m| zXl@}$O*&hywsXT|Z0)eM!je=UgU?*bdq`$VoSAM0ApJ;8DQGp=3}uhc-&q^Drat$` zOMUvxZRwnE?KYfwV?@|2?cd$c{Ic8uZ_OSmV{0*S1b{?yz$Pk0l3< zT{WA6*U-`$&WtyJfMKVr?5FPf#ty@Pr@CL|1^{d?Zk!YQEye7J9xk8+0~n$c-otiK5;YlWRxv_+vV$hmea6(ZEFw)w2iO>UC7-v>QN|YIe=3PiqPQ z<}EQ%A8`^L#UO#daKH#^J_G-p9x$o86e6g1GJy?x%%`LOX-c#b3B>E1O`btO(KO4| zN!e~+kUU8zD$Tp;yF~2FmP#Xt{AgOivg?)WZ30?9dX8qWm!8n6Ef)RZe~hLaF6KhDj~<6M94h(dYFO

tipFX{J(LL!o~`f9tmjSR2%`>IWq*IFI@y{O!pkza>gEWn$LQuL zarv4kfky4ow+p;MN9R3Iz2x-`7iW*~8u20mJ`ad*QV(_s@X;O}HvQ5A@hG!2E z5e|a+^QaAF=^6|s7#d-gwc5zN>jJ_(vCLo-6urz8?t}G9$7s?Hlt+e1JqB+dME*3E z0{~Zo3(X4eyx)AvKF;Tzkzw9$i`r+I64rSB138`{Ic9bG;X?k8^JO!uLw9PwsR$TD z?76_`P2{;`=QXN&vRx{EgdohgJNE(T1Gz_X?u+mu3&8{vWl`^y6Cn0N3z(p+NBXW& zyW=R#uY+1YdZ2>p))C6hlg^h(Ppz*iaQ-;LvFO@>Zj89lI13!;!pN4PN21@L;pceW zA=3b?BC2+EZfBzISE_Kin{=ssr;zzG8R-dE-qd>3^4i9!Ba9PCa-6~A5&==uHMMm( z7>z?;a|q;-s~qw8ay5LxCC23dwkhWKaD+NP`=nVjA8=&1#Jn+cn18--8C3zdcs*e; z@XaSB<5kr}WWBMZ5g_*^u`~D7_q-BW(^4?;ayYt8djF;?E|PpmaRi8ABpxRM* zHI<(|JM?Ow;TIx1_R_e2*H_-Ga^8tA*UE@nY&&QK; z)@L*gZEH@GMV=vJ-c!PyP|A3hUzIk^WugIWHY|~pPzERuQZKVkB?){-*n`U3@YuZq z7cH8fz=`nV`91>+F!BYjV%yE11ze}VP*6v6+iA9K%<^a{`Mu|<2N{ubGGRIIv=VuC zY?vUSl1S}t{eDYW!ygO(sMHng;X;<+ujkNO#xQO^w8I`9WVsAdrJb4NKy3*@8G~fkZMm`j(*FhWLfgRTEm+f7eK#PbjKE5~h@A8?kCUTJt-C!@2!*(1RVh~- ze9`SDMrZzW}3($WfZ z&+*a-IMHZ$QgP^M=qK;1w&FLT8anNs-z<6=q=qpC5`xwD{oocd1GWtObVV(VF`?tI zM>-*ob+fq;ExtouhB*^$dr^;A8L8CXK7vK{tOZZY=TLRaH!pvxubU|TZKuZYSjC$2lX%n`lkML#OhZ?ad#M7!rqHma{or9KE{vF$!0RVg^ z@(RDU+>z8j$QK^evNwb z#3ii(r3$??JYCjJwM$)SK0I%`FP-e}yMUy<0)Y}UqcakSqsm7g0=< zie3{f&RsGzp62etT^0-uS|O5mtlPwCo)k5E4IeSrm(4hpC9%;I?C-lW#2=AdAVqc+ z3gXx^`;smwjNSpCG(XtdjSP-_zo1uf?>Vgb+`)@O$KzBQ$Yy?-nRo;uj2a|!=46zr z>`78jDXbN%meMNy%`TZr#!fLO&b)Qu%=1LtEfMm^*{AU_2c>TI1y5tXPGc)W#!1^> z$O>kCK`<;Bn`(~pHT)do>F4vE_NxS;@yh___-BFsYK_gOU^=J!{(#+t3@-j$T?8bFvnt`Le)`4afHH!VI zihyk|Ab{!mA<4@u>6~AC5VFGJvc5z9+0@OwFtqZYv{FV=ZE8-Af->P;&@tdCY?Ls$ zKTn%%v^VumH?nE6U%aEYlbXbPJ{yVch)FrKRRpPv2->(N1CjJyO^04fI)@al=^yx!vO<~5&sg?(C z0Gw{DiLDMRO%EgI&BDH2>F&3(4J`*1dqZNC3wXX% z|86@NTN`F>WICbqJi-Y(8JdW>oC0P@PGLM!i|&m~wH@GC1(s6jx9$5!RS@DJ@RZb4EP5_jGKkoDuN#0~?;M#)3=qlPy7sC>{_Hd{D5^Et~)B2xdF%%k`vL_nyk5#1&8~Bu{%+pU8p7pFbV``)B?K zs5dna2fQB`aN9Bd;cMId6UOtezsIzH`Z93wi~jiM^R0bjnMq~cgtsK2Y--hP9wVTj zv)MF2lWnl#{giO?zg|Oy;i6*(?bm3Q@BgO$aT}O|SL_wiGrLQ2;bx(WcKVZp9 z3@(-!vli=sJu{~DPtYHgc-E&N*QWS}E;3I>);6UO!If;o zLjpg)eh@q>5Bo&teTPUP8M%KTr@}d#FSzEs05Ks`WFH}YV~Vu*}L@ihPS zdfLy*$Qk>pA9>#dR22KJqj26?Whx4-vgey%iRK~In5yU#ZoxVF3tE!vWgoD=E{5-x z&GciH2SpiuOh0OdR{DiqO5N`!u$?_d?FYuws8Xpy;`qlaFMfK?S(jh9C~S)1S;Okv zYi|2%^M#w4o&c^)JTJ1RK_!51B#tvqVZ+d-9+IXE`=^qh#LxdxG9SNwA-uKkljPUs z$xGea>}mJ4HSIi|_Z{v17#F#0LbI?@Jh4p7${Xk$i?ff1X-~l`!fqeeccvD$tn~*^ zwu?$wqFiv1eu{35ez`joNqK&D*N&&M`e^+f_Gm+wyZ(MQbXulI`Hh)_OsGNpM*Zb_ z{H{w3>ig_DR}8g^H+37iuOX=WqR)NCm#=_u;%*lpNf8ioZ?5=Li@T2Qq}*GXyn7XvN4W`SXha)q zlF%V=$acIiue43y3Zv1~@|T-kPAJ9jZCz5nH?<+tm3ENBXoL0mG|QWt6!w%?H|*Li zuRj5ngrzp+TBj|e=VFz>kH33511|-{K1kk7*gSXmW>&-)Lt>Uo+`!4s;Njv!rv|Jc z-R=P>oiUxJgQ*JJcHJ{oGLL2=vs*}UP6}y}dr#I&-9jeYGDBBZv}}cB*)85dee|#b zXN=A2RJHE)a4dsPLg@$YS**UrX0kg7aF%|l)qG~%hnW3ogby;wQy)WPXGJoDFO#XF z9M79LJ4GnrWAfGoWlT*Gc)pNBWv?XJzx_P_=dXPK&LJ`?TzVC___eJ2ZYAZu>LI-S zZT|Qt_0^l%U*M|Jp@!=F*ur*DhM2>vFUa6602 zp%rJ>6_Z}T6`kgHzUajF+>$$Im=29M;r-w8OTC>8?i@x0ZOv8z?b2P#+bNRxi#kj0 zscCddCV8Q=>V~<)mEFP+A0N|Fxr&g1ST-jhe!W@myOtDL&YmJ!I-A z8r`lFN}Wcm$X|3R8>_f(3dxzW3!|%nGwEg!*su*UYnYE!xR$*)C6QmPAZb)uV)2o; zL!K$R9S_n|r72x69X9Fe0(;C{FYhaQ;Wvh51eKtUUhOnh z?Ki8}cKoZPE^+<>i-Plg-l|x;TdhjF>kGf7+bStJR8UQpr73?m>&BI1SO%7NoIa$; ze77S1M}{djCEoF1^36&i|F^X&qj+7V6q(xUDR1GKT;WU#RYSAnkI-29*KF&I#)sGv z-?iYI>Hmq-^HYg zTW{i+S^FXO#Ca+7lgYJoOJbkT40gxkT$Rku%{86)(=!jXT_4#G?C~YNx!bXJoshfR zmn5Q&34M+x>OSXe@Y{1qFEuaMKkz0AzyDq??UNjuyWm8V#p_7eEX zmf%4BFj`ZyW4qpaAX>Qanw6n5?vq?B|ad--K-DlfGP6uNYk9_my{l z7{EeQJ~0r+z_uLo7BWjyi#EXoR9!D`&pZ7s>G0?=l67|HQ|RKIQyve9JRKa!&-kVH zk*#%_rudEB6ytTsl~wxCeWPUbj1AM}N-FuAHsyX0kKJ0y+l;lZykyv9GM2{pRXJq2 zO)H8GVnSZ;{1ksTp22~18V-dXpw@M(_c3iNd=h2VfkRRCzn`$Dxyw`c-W)v%C_=%L z2I|X`v+tw~SRdG(=2Ex_W|tOBO$%7(6{(Pl7>M+%RAhpIw5r558IF1 z*bF%0jU1v_x>Hcbx!eLUt~&-htR$86PWz8{mz7Y#<;BbR`S&E^zJ?Nf>hMYNeg0(w zD-7%#*ZZIt31P$f+3k|!O^r5uU7Dt(vk9q4eRF+h_@?te%y)3ThJ|%-i|sFNtt15T{1Gy2#@|?s?RF6hBXUH<_Wv@b~CEH(@`g+-L zrKRhMaaAcMY7fn;E!IBw#(kI$K{J-6*d107o~n9cEZlE;cHsz1&v2A2AtHB4 z;Ng+U(9L%$hz(%$g@CHP6F~K<5=ghb+VZID-oAQ+VAAayl|xfPuWxiqQOw_w_DCh7 zc@yk1Mx`w-S%AAz5kXZi+Zo*^hY5adMC3GGFtzomlDbT9F~xI-4=;zq)`7kE#h;8M!9$5dE%GvNa6uQa`>rerg}-dTH;O+rYsL;WxI^%KU?und+Va_@tl%Hs)&GXTTlwfrkOoZ{1=uf=#M*{r4u= zw>}0XWLUeJA%-j>L!VdnxQ(aW6mc^uPeWWj4xFEs9e~yE!S++p!*|ic%a5DO`Bpdp zT>4XQVkf{9({|+38*vBDP7?%ZR6Lwph51%L^o0pmub&7ifa-Q(=@IJT$(2hJ2XiKd zjcw4QCl?Q>_X_4Jr4VVif%@w7j7g1s@hOwuvZ?5?>n$VM&k>p{cnxKG7>m5V2-eqo z5d7NVw*_eprYg$iQxeOq7vm+QlCW0VuzfULa#g!Kohs@NAxZx4=LWBQl9?iRkoupp zUH`Bw!^u1k%ND1oKd@BoTiwhET+h8eoB_O8HohB8RUlG3>GpRSPRDr=KNzp^Jn;)kqHwR@S%Rke;lqwX+6p(VDm zy4?Hf$?rwa%TC=TO*}pu^nAF^%dq{a9oY{qVT7~iFqJTk9_~e_3exDMmg`ulGs)>@ zL2+OdXxevC|5nZ0l+FAqWh;Bqw8tYhM4m=aL3>JbP6zPpbKl(D9(y<4Nj$z-|E8fp zcpH&PnE%KM$moVMQ9aDOyezyZDY9C!+D!<_JNDsva|Lh-Gl&@+?gt>X^19fLc-MDL z<&h0<>@-;;F0RTw<|qWh+Yi_sbFvUUYq2^+^o)Up_g~=Ze`MM2Da{s}!_tB1YtV z(Bz)4BlEGxHXul*irNN2MC8?ce)G3$x~WwW@ckkT+~b%QGoemFk^8S#Z`(y|pnd|S zP3+EoswB*X7knu;z&WS{Z&>1Z%8^#d_|-(5FyUM2tW66^L5-FR$%J{iUazHCGNkrZ zwVuZBkWP*q12Y3^TFk)O!o$=tUQ9t;GGVOL^gsT-)aF4_5yAwH+{qj*6n%dBUE+57@9O!;^Di%1fKoUU1b2)$^j~C=U3>#IburMoP;fFLY&mp z=@So1AnDuiTF|_J9Ab~x@t3#bXu%uoa&X|`j3R!OI85~c7Ys!^WNh_=ZR)*Rw_8^Z zBFVR}sBftz_pT*PgUv1#eBo`I7>|sly!{r-r|DV$yap!Gp7!xCtL{EVJkIH(kqtit zevCawXel*Ael9}jbRfBSH8ni4IoRU9bCvb%YkSd0q}@Alm(^59$A^Old)e`%QulI^ zXTJc^%nn{VAr{i=(*C>z zO}5g7+8f2o+x)LSds#h{%X=;|rFy@OD)}(x9`Qun|6}S(z@covH`8FqG8k)OX2=pM zl0C-Q#*z>!NtoV>~@}Fj4%m4}!l^JOgts=u zDfw=tY8smIMISW{IX)LG;H$w};P7(d`hi1Tuf)oCyjk)cXzY<}{LxW)$Fzdu^p$o{ zi%`9!14uNA2Fy=iIK3c>L|Ct`8qnM5q;J$OT9nxzUK=KeM6UOjifW&GesQy0o_Ekx zw;A%M2Kn2w;GFanBEK>WVc{+wW)H)WIc;p!;rSvXHsEayp zEmTR6FE*FD%@eT?ITU;za(5U`uyYucfaZ|Hw&%y-Y3(s#QSQ!m_aNrpN_77gpt^Nm z#pHoqSNeGm+Fx^6%z6*iknpJsh~oS2(avHe_MH;v`(Sc(>hweDv2}KrdE&gJdsS*FS*b17ib3?)@lcA(g|ogHlh9vX}ss4 zGt@wvh3wldBx@5bZUIBov86YFr+6>7i+mQBJ(z&KY4oG*1pM|(1}=?}oSz}ae%FeZ ze+HzH;XA8aKn72sZc79`T;5zxoQwv1tLO7!)g=*@+MCr%RKqLGfK4{H((aF5=& z8kLfYD1TE#ym`=zr{CqA&^g_|*TW#w4icu{>O4H$)3g0|=@y<JvDBupGu1%YBCH>9cNg9FuPFIIL|{tTqkoA&wzaNP|J+ZsFcFv81Ei`C>Isu$ztE zB}nNz7nSWkI^q4Qz?AQS#@i*WcKZpabz#kKIhACJktBm!p6R02doSAVs*1^i=n+NR zaGQ-Ut{zkm!F^mUi$@cIFwHy@!!rCX@YdRQ-CGNW-{U3<`seazVX1h8QXjgNY^g=FgFFFPpM>?sl>ZGF-IiqN~~qBMCuCX!wl*#x8Q`mUCoWk zl6)!*ZmxBIbYZ0EGiWYEtyw?vZ8Sf|HzIgYQ|B)jeoT9y2_ zV&u>+qvz1;z4^$hGkY#`+ZZw`w}OFAn=Sr_8Z;=bf(pf34Nc)x58z*a23`H1D#6HI zrt_HuU+ijM?l7Ptl-w-I%wv~x4(*^3C9!B8+n^vEQtpz`jsf-}6?xvK2=!(eRxez& zgJ`GiGZ1c-I@=9c#W-l$_O@5L5abGzqnu=?Tt^>*9;sSWPt zy7!r0zozJ;!@yLTXzcMbYn7qPtJPX+gm}g|mMXPeel|EdAcN1Odqt5d)|{E{Q!z+& ztF+i&JSzJGXdL!OUuK`xI~e^+@m4Jvtd2BvD?wP(=2a9mt{>=ZJY91O=b}!spcK!1 z=c6~938Z~B0ax=LFsN%bA555fVZz*P*EmH_JB)7TI6g6bgm$GV=;|%fjr$P&qrG(? zUl!(%r9}DzpjF(Na{ALo>logRTJ4lD6HF|be$rz2foVYUVxpvZn$)G{I;^$ZO(0j3 zq-Q#;*`xj-ir-h6k%_=X2Vzs!a|qfBVFrIU!*HmK<-}hu$jsAbv=EmHsQ2gfb^qpy`X|}VCud&e`^RU?n63o z6k-k41ZV{0AhWd19=t@2wmmsMhDT4`p4{R^U1@;yNO*c3Z<2e#6l~zD2(w z;1^UA5{=}pfm%>7d95dC45-40`lA|isO-~Vb8Q`P5dGt9vGL1RsWGeD9A{~Ao{+xg zo>zmao(@>jNO6|;Sr|JJwR#IZ9DKsU@xZa?n>yac#FIl{Hid0?5t$lY_M(~nfniFv zjaNKm@IIL8S72IZ)utBQAElk3Ccubd(f&%#UxZz$CLz0{*s|1OV`kTY-&5r>%O-zF z-UTS2i;W@r$3eRGCLFmn$e)EAuMLg@SE@QRRE4ghA$N|oas!4AnQER`emB}~=42X$ zj3W~CciFjCzr1erUF0xt!PiB`Z7oo38~M^bb5I6PI?p|PXoTL_g-}c>_QKtUn)#`e zTF%^&TR!H}aV*1Nm^q)5{`P(vi*1yR;S6v^4R$hA*Mc2QfCB4=cr z0+Zf|*v|u#tRY&UAz52IR=t5pZ}|zsKpcb=lZT(BLcbe;hK5+cd8oe6v#w&VreiLE z`nxoUdYwG!9WPW6&r{QH}XeY#k8=)e&=4HL?=j+ z;Dx)&;w?ox*-efjz@b@pyDmFAAWNHyHE{D*Hx%91$uSH)YFylDyG^E4f!iQUpU8!~ zFXDCrbXy4uig4i3OskU>=T9DZiaUf75@s3?@qkqFVY_2@vdGwqFkn_eX}7u1=~u-(uRt>5)bJWhT|+^86l$3W$x2)`4T ziD1R#H1$&o`8$>k4W=nXsO?FElnbi&9idFOTM8RO?1uX_mwX@Kg({&LV8C^8h{y{} z3x!d=W7HtQ2E+TeNUn_cdojVDOneZy)5V=!{B>tQJc4x)8)yPi5gBjSM572FOx{_R2mn% zdvvq`;`uQdy0#}Rc5zGL8xzN$q@)ee60b{+Z`I5C@a&>ANHLhDJKf0^A*cNL$z`(> zhE=lDl%0C-gm$T%6hz#_?o#`wFI#~QL4)bKnHkNx_{;`S9CC+s;y2MwS|l$*j?1f) z-kbOhS`PL$OugGr0xla8gT4*jQ9J`$!tZX}9kON1tD4C5eL=27O4P0&me!2+HViY% zHQam;D?Y}6E=NWAvF=Q&%m z8m;ryA4YwlOdOJ{!}J&MvV|1%+nX% z05GI|-}+9TKae=w)+0T~XmoO);E9jpfP_br!E!inlE?US_Q8qz9+2VJKPENFt>%U) zQHZ?z>-5rsnHJKiDL?Z!REU)#9Y#Z0+3QaP6B|?WcClu|08{rq5T!HJPN>NQ5KZvZ z&D*;wH-AlcK1<1mg{{=_SnD!*6C|5IfJJNn;tTY1d)D~Xl6Vf#ibS>i3UxoXNEb52 zL~lxiILXEq&}{tO3!14a{WZt0Nxv%Ht`LzcHS0f=y1J@m5{|-Bjqf24sxO@aEdf6u zdpW5I#vIdOtv&J-tiIb+`Hu%fXV@gV+U2c{hG2FRkdetSWP2`KCn);&o*6J{6o^E1 zw+ucG*0r+~Cn)*m!pv>SMzsn)j_QjBJ0l?>zWVhuGmiU8GyT&k3&?nD9Pqa!=bJt; zs$7PncLi)JKzr1kvbPRR)L?@1TxEx4StX~R5OR~86^OtJmyd5tKd3l2O0-T+)xp(mStLAjNV-0Nf zi*rVj>FSI14!~8z_{%eytJZ=z@<~mY9(4uXJZqcp6M8W}J>samgTXc1FlNLCMxG2M z=wli%LC!9R@V&;nT=~$xf|9sS9ZkN*vIj&taWQAGoP+5OtEF&DeiFco)UuU}O`S zS^{Hyr3IpYkbe&a<4mG6+R${GvIdCY`vGjYlZei;klndrU9IFdy)fi%2I|8++r>dN zYH7BRLJ-?=SL!rqk+p&}cj;(JKM=}gAl!TJtMTf6Rd+H-=!f95+Yf{k9n#?0av4zq zh5euR`jcc*>_DOSY~1j@bIG7zh7kIPJT#f;OtBInB!c`?$Rxp?ThapEl{fJ==>H7)zpy>Q2j6Z5WI4s}E-!xdNNHBsT}s|*7O6Y51i#>p|KG74MU{&4~BGN^sE zz?!{EYq#dpzB%X~FB}L2hf?`BE{*utDFtu4K@`fTBSGRxE0!q!U7T#;@l`svDDG%~ zdDF(3jcHe)S$__gLp`DKd8Y8)Z^C*a?L6Z4OE@e)sO0!)78m}AI*tNi;vCFF*$+M* z@_oc(d&6;y=bJl6e@TqDpnW;A36m*b_(?OGR~5v?g8B|q?>|jn>^=GcGco2*S+l&SfY%Z(3aG-1YauexuEGjehMp_A!uAG z!f_D5Lt;>2^DTN&8tlsZ?uw+cPiTUCb_)HW^e3a7+w=kZQ8Vvhp!J z#%q@}k2Q9;!p>8x)!K4gl@AFUvnJzl!s{a6oirviI{MVkn?@0BsMAK$i^aIHJ@m;W8iDytbh9)?!KI1DF|vK@g&z4Wsf zPWcoRtzG3i9v@Yg;-c|M=?$dq4EDDHF0cI@uTLHl0xLO9Sq46ER`mS#zhwqBD*3yu zgBS;S!ho`P)gv6>;h#JVnsa5%Mi^|{B6y5oif_LHw(2F&LQ9$(Oti{06T1&;uUOB= z8iN7nkfLho8&PK6an=Cp0o4*YPpimV8=zxYge&QH%?*C^6C{IF(zclWeBudJ!zBx1 zntYo6nb4l9r!$jogs8ETza@17EydN3l#m5L>&WZYdDQzW+Soa;9Xi2mp;eI?Cg}rP zP4gze%$WT+rYZuu%{LsMGhL$_c(EDg2V?d!&hg1>G5~rPj9l{s<_V3|1ijZI#QBEP z-J}WK z2u85aziT&koHKWU!XWfZh4nGNJ8lS%0$xEIxX-yvu;}sGa7+PR{2VYIWtz=iF!dnW zA?Dp%C`Z#v$fzQ}XE5;1(eSx1kj9sW=S|2j7sLXLODvs5Xp+V^f6Yfu&-ll0e2i{= z7dOspxs~nll5$$C^7^YFS;N%BzqloJclB(z3eG&0JLYuP2x^}BQ+qk+#OLv{vb(eX zGcZR{s`v>cGPp%RP)EzNvoJOeUjs^jb58VH}eJ+wJ@9)ca+v-+~j7+>daBqe7*A^Z)yEGdBI2?1(52V*QH8|p`| znzbN` zcns^F>A~=K_)RzP<=V`ORm$~Rgw7}?9TXdma2fKx{?*OJaAvPl!J%8Acj6##(H0%hAx+ov8c%h#U2M@?8CHe3MaC*!DfEQYpdeOB5a)OK)(`JRBS%OB_$j0hMn0^JmaO<6f)Xy*6w5tUZjIJI_U|`2nc|_Bz+! z=&X~yYyG}P7yP__3)1SnFPyCf>S9VNPW|wE_YgY&v6|2xoMN{iK`lBfC3Ww}6Hj>)MtPrOtEK_oEMP#{kS( zk(;QMAGHwu6K5ox=P&krH+D`HXaxxhR)!OX-CXzg8P_KxxG9chiyH`y*knJkK6M#x zQid+#0wQ#VM&C4o`NNKg5ORR3Q@5t9IuOPa%tv~Y!2EIP+oxOg*B1{uT#i_Z%hE24 z`7wl*@fEuihp+-&iv}8@F){gq5h=3t;Z$GraFM;OE(C?AT}M zjt!lQ7V!*+JxiG}+ouTjLB@HwyDujyw)WxiL)JO0Hh-AwRQGOX-2;Pm3^}$rLG0~8 znVZgQqVhDoH)=TOa2LNU(ptE+n1?wiR2YrKC4Tr%Nk6Gm5 zXZ4feI}Mr+&iDzE*?L_F2W98xC#kE*ct!^QasEle8Xym{E~G+1c~tjuJS*IsZ77 zoGAM;P@8o#=IR|2_ghd4G*4O-|0Tf;x9YJIxr%)dz&1>XM+?{1-+?Y8a&NZhP^pNf zc1^!_O&3Ngam283e%x0)f9s*p&$Y>?-^09D!(1Y+e*m&WXsvm7do0 z_6GTyE(@QtB$i1pt~yYkHIQaZKPXt(B*AK=FD>v)J_PsS>tvTH4^Spo8>sOs-@~F2 z88jXdCrD%KuRnd#FS~qnI*%-!p?dHUPl3g;b@&aoxV1dc@49*TaGu)%pkuzB;^Vl> z0Y~rOnN`}SNX?R?$ABDU)ql}r80W<&0A%7Fd-bg|;+eIN%!mZWdAk&{bSi{d8dkR5 zHeM6pL3I3uNoQ9HXYPH=>RESiE!(8zvUaghSH%^Bh|5zcMPT{8=nlssfAZU}f(b^U z*ZVD~58iys__*4Ng>CY|&sm)v45R?e8~|40^Aic{o^`ZxzxD%G*gANCAW`GgGrt2WG9ikb*TQs`CjP+6a{)W*{iIVjIfRjK{rT2QqZ4$( z65xQ>JU+Ov&*S-x0G8jD=5)!_bOdmf#h)T-oATHMC~+CLM*qGI9bSEWEO=}1xdw2w zEP_+{Ea2I<^YJ+`RUh`nQHN@sZy(-n!i`+us|L0L_kA*ZJDhWb4SRM$Gj<~A1%z_B zi9hZ4{13Hes1!`j2NRiHRr*#=(YoCz5FQ?!Vte#0vpz?%H!*OLCY668z5(`xz55n& zZiJX~QX;gyU`qFcvZk4{^6TrbK(zkl>R)SYDIQ_i%C8I4s!y&xn!$#AomK#gqRmy7 zYC@U;6De2SPNdxDtTquvs>|cBSRJBr-L?$JghR?l1M(Nv$K85hSy4!4#2NGb*{i!n z-^@!{wT}z|TK*Z*fgeXV)eC}1{|1_L!HEkj+Ivrf-&*)bcR*#LiL%>sl#;i8%?Ki` z3x3zfapS#U@srai+vtlKD z0pl&H85zR>N=`j_rLkS7nDvC}@`EYB1@9&_ny>*FG4?JmM$)g_xpVhNPNKHBK5fmT z-)DQ-o(9PpaUbowO*(Rv{ITxKdj{ZKi%!AN_pjp&zb4C?%>NybvNXzHm@LKVan%2K zzh>R*m~yzgr$F?wGw>AW6y^6V+@yA(jUWbsdP{DMA}3Kg1>d+US@$#U7Jj+Snk=wM z00yn!pI;KBm9?94joMPSk>aS5zYB2X(?Bk11r*ud!l&E5+mx>ktli54CT$^oO8^`% zeh%dU(1MYnPm3}dU?5k0%laDA{P)TqhcX-AO-3A1;GP-0zZ0BcisB^mw{76kG8GUO z@iw*DSONluQ;9o40O--$7t636XLTCoHnq#SR6z}(`JSACSJs=^6}r(tszp%F)3_)W zNwrFc#gYdlx0E9tzIhx=2uYH6MLbZbx*3Tqk%30+KLXo%)ycLE9h+Q16nR=gY)#Ez3tHB7KB@#paUffVzUIrj6&W38+@3-7wjrplI^PUuOA6hB z)0!XWTwX=I#&AvkpzUz7lM3+BOi7 z>%{Kujap^s;}YEfN3N)E#w&8vT;0{}t;oz-Ou_vtc-mGU`^DGo@`Q7o-F$cx>@WD zYH)wJb|^s2(JbUh{MN#vi$9XWaNf|)5Yht>*dzLb7W0{eU>M&YR(Q+X;9fq0WnwF_?e1%;2-xJNa^}`P()_+NrGN8J+%!_D|}-N97?3rKXp9ogEs4V;EcDy zUfxjhX$1rzvR(r?KdLrOkG&ZD&yt1P&dDXmjpU|Mba6^fcPv3&;eb4TzaGvAwlZS? zQil1Aq4RF}cxCd8)FnI0vE(8RBp;jp@FR;oQ!izB<0g1h-_%-)I7jn?fiaB1NKEcI6#BcksJT^mm8)Qzy#djrrok02>xh-xelc3H)cS z_q?wyuG|+_JI_722ZT@W*lsKkim;Z$%~<$Uk46)$f_KEeHeleHJs5FE&pODzcpF7|M2)U-96IZxOA4^Ssg4gq0vR!4890~ zL|*{0uibQoDJ?4hq2xKNP$LS zV7?xIn=q63)UX;m7tU@VrsX%D0zR0fH{GGztQOW?vYCa{@=Y#H5NB_kbz)EL(w7bU z>Xo1lu{lap;93`=s3o<0K$x)DPzh1LZgCV?xd&UANr$=PDGw(R;Rss&0-Jt=XdQNo zMtk^0W{To1M_=eibw5bj%5&X2U2(KlZ^zzNBKzoS@t*d6t;Q$!&0Z}>Yr8xXHPiO2 z3(`>yR4L`iGbf>=a-s>L-viLQ4ZnZ6g4^3m}GX0btCz z_Z#IvNsvi^xhp{9kH%ZgdR2#+I3r`$A~+Gf_v*h|b}ASlV;&ljY6tot4~};b@3&3J zk7|!i@N`c-Hr_E$qKCJZ=yZeH5&c0I=sa+Ssu?D>A<8u=b~*Dx0g)%H8z?O)fY#wU z8%~{hk+bz0^%QKuRhb1SSVKj9fpVMNMLYSmTpl}>ARPCps=32W&Kk-Q`vCEd$vp1K z1Db(=O_yzN<6nLT@RJbM4fuQ6dKTKhz#=C=N%b_Uo9b$=&CxENSyvEsDcJtxs}G2X z2nu?|olnF2K}5tI0K}oF$LE)+fhqJRtYwdAd9W4la+OKJq~l$-=FN$Ae0A`-u1v^T zlT4EggVLNU6nU)x7HXFS97)o<2lGM6jIZF<)B>E(vmfLS{d=Sr5D;@Vaa6mS119K?oJYj}{ly-%Hv1WlouFlHpr+ zj)Mz*PiB#h4i;N}(wfmMQ=Gqno5T92JjZ%Om|U8V`XvvL7O30TtRy7w{qOVmGW?HY zFEchT|H&>2J>S|L-w`_v4XseQ1Hg^}?^cO^if$&#Ayz&OJqVu*IeBjxOXp(+Bd+16 z$TkXDc|Q>BEHsk_buBf09J0<{`JNDNv5?B=^c~!A7F}q-6uHYbhET(P*1!o&CzJ!n z`zcU>ia^|L_CCIvEHEb;2Dve6YU$oNyaD9F;SPv7@VlGNZD?M85w{a4kZSID(s;&a zu%@THY*5V_pvHFq=!IHPKgh;Wy=4I zi0)$nfZDYupE`@;i@sDJ*u=CBeeR z!7g@UJj`zO2xWrul0Xy@r?rjX2;=+XFE8)2e)|DGaVa2^P%!;P(VEMy8|1SswDmPxqJC^v4Hrke0=Vd&3*DB+32(w20$nA=g3;$xJgaH^BCiTf` zGnd>DiN2sxC!e;?LfF)V>qi_lFK`a{ASyn$Z7R{2hpXg;RVmPEpV(21)DnStU2!fR z)FhYJf`lm$Mc&>?;6)paexGyVO($fSdbx`=gPar6QEF#Wt_ zT!g+!yr;jIGC@!;XL=I``g4n4zC*y{bqcGR<(1Og18A(T8S8dnn3mnQN0$km#6TD=XFjw#`A7zN7{|e|dx$OYIPG>c&!cr-7sjGk3%eF)2j9H3G2!VyGdw{;7wH4* zR+7#q-qXwB?sfv*#>VR@``^E-P>~&Km|vWt2UT***wJYtVV_2c%P@sF0{pkH{v)vD z6U5DD(|rTcm-SUci1m=3Hh6uZ@SIN9+xEQ8>X7^&;MRm9s?cQ}z{@xp?43cx_^SGp z{>3k7H-E!}dukX_JoUfb0`JD1%{XZpF&WXsAnCPrdWMAcj+G_9prIZ5cJIlVD%5g+ zaHi`@>ga{CUXgNleu+p}(>7QJ6Jl3`%Z!CpTLZ- zF8J1?;Qt7%vfwZG0+_2zhqdxTa?e<*3A;@Rq>X+*J|EEk1{afks2#ot3SXc4j`WxB z9xB#zaL@&t?FKC@jMd6OPw!aLBh>IkSGyI_iotpYCh^EcPWjv%Ekvp4E3D>L=7XJD3EA<%fWt0|z{Z>D#Lp z!k7=3NN0%@J^h?CL&PbY=!0oDZLwEr-Z_5F@3fpE|B@PWML>dPI7YDsqX_5!?kl3Hr&0arJLbto<)qPaDCG z$qtM_((3RL73jO`bJ?8#K2TDzsrn}R^VDx(yTDc$egKIz?wSa9vtc#IEVf?@Sv>Y=5H`GjemN|N^}8qz&`u?1NizGdfGYsYmC^j3MaD$~18Gq@jE1vSBXZ6Cdzsc}7WgAFenE}At%UE=%BaZ?tnF`+{4-5I zo{n%WKR6mOg(c3=Yo9;(iLEzw1rs(de02LoB|8q)g84=Ad7*uE_^5>rYRL^G`%X&- zn62R})_P|NP=Hv^Jb(-&4MbN5zW!-`V-T=!;l}+m5d8A zetK^COh4b|-uI6Sz}c6^@;HN56DDT78Zx1+t~T(RP--tqV71`Gv+6MMod_ zkGJgad&tet)e5%X#bUj727CE?Ma;j>`i3fUQU*%wucd>n(o3IR-5n>U%bKrWYU^;T zI{L-|zs_hSCl2%AEKK}rw338SFoANuZxc1x3&SV7s;v!i;i>7fLQ2mS`#Y#$D3uQ2 zWDC?fnA_4(3Hd35w@j8ql32)l!9+SYKLSoLIY@rhbc=w|HX8l8`xB_Bl;mGI)zzxY zMrWGzK(z^nR9*(ZAItJ4y)O_9!nABnp5T{9=xwq-eg@@#tqH>6@^;Wny$4>E4cZx0 z;Xtnw^yi$XppY@ghQQBNk$d*N0$Q6QvUWv7@9!1O>rXQ7gztiORQZ;cII3L~8a)hX6+Ylw0PsE)2L+Lq<;gZ(eexyoSV zW$O+;7e^}bKvHqK<2@+e`5RSd2p5IHNdHb%!P$?{Y=)F(J;s2xIZ)2xySgtG-GQ$X zI<^P3wU^xjM+g}C;F+w_K$zESgB#%)qR)+caa&LAHi%aj<*z4w)@_mfO)X?yEmF2fE?@7_2MgNIT*OZVLGw z7SPf<0Cr8Wfj#HHs2pxN|EA5D$;KGqysfx3O*epd2i&9*1cv?Kqa;>Bjj+(u%;wio>T8C@%pnKLwEUw30BJ zSH%^A(k|<&t5U!O=-&}6!8{6CIB`Iw!>*i_DJ&RxGLuO{B#%?>={;dtE0;Lh(q*8q)XGMcKt z>@QCnghhH%+w#`(%RFliv>DrZf4}8r!d!&htlO|l;Kb$*G=}~6-#)ws#d~QMme3_h2S^~H# zhmTad?>id4t+rsrMLv)WWOu9k2K$zu0{Yny!xBfmOX7DH5Q4UoGVPd$}f*mlL^shy~^ZPf*xdS-}*HHU{RPQlS3k@HJQx zv2DCA5Vpwx$eVT#C!m+OuV-}|7#Dnl%9Z)@;Adnbe&fnsMK6J|nU$drJ+pKmIA3|T zfMk1K>pM73bzfymr9irW*6UWC`hf{(UMLta{#6_B+e2Vw1aDhTcF<3X1P+7`Z8`Nz zI3m+FU9-H#5s4%2GgLfbgh6c}hiy%0Byahni&i;@`bCTXUnFvP1je;wN3$ajJIZxM|^1S6;P$V@^T-v#;E-OXLhy3wNasLBV}q=J z?8?%Hu0K$4TA?Mt9kq7zTOj)F5el)2&Krum<;hP00LC&}=h8WBzAQdgv~U{)Rw*$} zUzcwG&qmAl79D80W%)T#^#f!T?qV&dP2RpM-Tm(7@^OsCP##|yJN@a6r{oomtk-PVG1w*OSb zgyhP(B$G+0Wg`(DZV{kEIQ0Rm`wm@ADbB~|8X%!yv+>P@qudG&=+37(+O3W1W^~MB z(S_y^=Pl;}7WZ#+{V{ccaB2&UgcN=%putI;Vr;n?bMx*Zc;p4(4)xvPb78{Ube|IzM-mto=x??)DQ3eFK#;D^(trMK~X zSxX0vt2tf365uX~WZ0PG#-@msoz`#~8wxG~_Lsgwfgc?Yqx=9ucn;NS8I!&p)!*sP z&)UsKeN_)c*`fsKLt8@uVQ#k>9ZjIXJrlH{>v2A{_1yAXYy_C&vBmQ5LF88#Fn#aL zGvwf+O4hSiFSKwf?W^CDuC)8C-x$2)g3{IFI$r?Lz5d4sbbksV&zg$)7sFzS4L99m zbDwFgtdE9hEa>K(M2>enTR%eeEggBJ`cQzvF{Bl)2zPhB>&o_@+u|N%0l2<<4EN^+ z%I}~($GLRZqtyrjDa=`KgYTG<=;?HC%VE1t@5 zra11dKlNiwYfH0iSfKz#Tr{o({Mugm=_gEK@m)4FoSn?;RK4vz^6fUB_=k{5=ce7AK(n+LL9*i~+Ku z?OtffYno8o&vY2(Sg7 zVE}|zm3OV%GzMiLMcd4b^sxQ+A(GYHsz!pUvsB=^+*k^l5kdn^MYdTgI0I-Ook#8J zz&Qr{zcwHXXQb@1&A3cP0{}hlDBp#+PbZXF-^{(!4x|;qX=TL7fPSZr>1t*0a)e_j zGh81eFaUVd(C~?od&SP`0Z!5iUd`VIpiCHton3q)ZNpwibWCGOzy7CJK!z_Puj&kq ziF`8s?g9*-*^uTA4u>2T1XOriPUw;+3fp%6Ut~};^A%v-hNqg}7$Oo3Xt`QhEOAC> zU`}94XC7n~_&1tZ@OgpwMjQ^38n?%8bIb^cPNJ~C{IKh;g<;2xSeo+m!Hq2~1b1*p zlkF8NWgyz^+`^B14-qVVE(X>&tP7BZt8%seh8xrBbcY>tX#IJx&MCxoE`4%*I`~u8y8^Ee)F3Dv~E{9+E<+yG1Mm#V;ZZlv;jkN6u zaFnKaCII@gOhl8WVjxcs1yEb}{4%Y7fZ_sqBXg`0a?r_lxzf_lbAT7=4mIaz&@yU+ z2DY8{E~RMK4hDM-D0!X7KQ77L!}0j#i6iDQ5KDp?o5#NIl{QGvfjbi#NO=dR=jOU{!1JE2;?pGp;{@@+GxU-< zm|nECI7)_?num35bn=NkI4VP+6i5fp+>Wt)#1V^p;OVc|mbaVQcUz2KA>JeR;PXry zmU3?=q@~6@S&g<4d(Ir+{%o-&DTFJBhM6VNzkZ&Gb1T-P!d$J9VhW;iStN>Y8CVsr z2)qDanJ|yeyJyR+xu~tVk|9>z+|-&rc-!{Bbv6gWMra)j8qW5UcTRz!IR|6hzeot^ zwjt!7r@w~h^kp=_3__lE#P1&;E|cnvN>_PDf=YwgV*2u+PN1|)(6I?10v?P^dA>oM zfcpf?aMvyC)wmmP_kZ7D#G|mRfCINr{|(kO<$onR9?9SlmuMV*xe$(;rGC`bhZeCL z&Lq1Fgvc%td21r)5a=HHO0xbs3N#aczmW!3!w>V;T3)?M#e1K4dY!%>*5nyS-=8d@ zm2d0$GF#`6=DLDNfhxiG?+r>txoAFpw+-5z^)7A8(cLY1a=OenW}fLl&wOd>fc&;Z zXHhQ*Y#)Kwqza~-KY-b_~!86kJh#gpQFp+h@~!ne*N%+e^NdU z-t)h5WVn8-k|5&-6q(#7CHO!X4p7$8fY$&IvTIrxv^f6+7h+ulo6o+kcyWc=g-(mM zA8@jwslMqde8_*b30MooZ_hGmS~Y0I!Y3zBOw2R@#Pb}eb-h{mOvFJ4;q1)2ZGvyZ zyybi+JJ%_|<}zyP8+;M@@uyUnwI}WSAh6G^_L!0BNviVx)y2}jCB8SfLU3Wk{&6T0okoXB&`Q)!cHv;yZoY`^$X7Qx;F2FGB z8yvC63-p`-`F~ffln5up!^}AY5OOMMz5^ME0yLN#Pme{C8C1>*lW?;N*) zFOHR=i}2a0Y7z74ho6u`6<{Nl&lC;Uqh!k{p29dm;dNPqk@rQb@@cL?1m;w}d|Kbp z@b48+7xg$wRu!d?g|FYaFtiz02|&$G<3glPw5wTb&B{(np)}%PMrMDBmJOMcs3aNA z>rH=dqDH~32bCFQOa!jZiqKU!O5Xc^k4y>y6C8|Wyagh!--gQJwVe^G(JPRMb4bZF z^A}-d@EOMuQlXQ{rgR;ehMh30WWv?t`7~7=NKvW}E%>J&!Nf=RUlUj8!8**0zkv}P zNZk|1?Uo12Pz)NeM#tsEla8Zq7=|k{1gR0@mBSg}YGBHy5RQV+AYo+=*JGT=N^ECY z8;J&w48jiSO2{@stXzlM7AI%#nF4@T@Fxo(bp{$*bqL9i{i+7R?e~{+ z+x2XYhx1D6igq-hBFe@fSj85YRE!tz9>fl}R*Qrrl30$hHl9*&X6sTfw zx1XfMsJ~IRKr^P68Ji4{vWB0T{E_&H+?RSkEv>&r0tcT+=lBFjOe>r+HzXU~%tQ>i zL?d??y(GmY@vrr+rV2b@XAnG`yk)pXzk~q8Gd8Hrcr#fQae2=}hJ4Xc$S4;wE}xw6a}G6JbB02HZu|ob*K|0~bdChz9m3TzpvZ0mqaU_ZJh1&&!`$IP zM`LMKUbb97{z+(9M$MzWK^a%gyGGzU%taLf_lq^h08h;d!7V_>UY_^it3M}ZO=t=^ zv4vQ%B?2$>2I`#R=zbrIN&c06sfqe#rbjPmTmncS=pBf(rGXfr!jVHNm1(9N=Rw=h z9KvuNE92UNL>N@08z(ZWl3YrDe1Ur{m(KWbGih8&Gfln{;?q@NW%x<>_g(kcsP8vU zLx7jUOMamMxb8KNUEN%&0`Dz(9!IQX;%v!?;Rq&a%1g&leS&eb}HRQFl$v_ z>J-*nqb4v%W(~xC8N2z$?K#A0L8CG;F~^=A1g^U6-*b?$h@E*!|B^*8YiE_$PA-G5 z-vx6;R^I7|HbzNrrbhXAZ}`xezo#dAZ)AZ`o#yqowB!?0SFd2#dV9!>tivqXm4k(0 ziE9Uwy@e+6!k<9;ipvmxrQmd#V>orC-Y?wuAQZ65rWB``Oaq!=1j`;Ez;RJy>)TiO zT+nPy06vWFozYh3StzUAp_B0$#Y$+2z5^mZX)iUv8ffQ;b(jj07&328zkpJKL=WIE zyfpGqpu^8n%B)S@xAQL!p#L}4gvLcPo0jha2CS11iscRD)AD2m>puZup*hpwm)Ac| zlK@Q~iuQGcyBczahgr!H;NnIENrLUWaQKKgf&v-o*dk)y^t|qJDo?D(rszc{1vB~< z5{0RGlVE8?gZe`$h{d%F_?GssDgX%H*?#q@*;go1TOiYvP;V0&KH-sx=Zm%)Gv4=K z;5v&irq6x%Lk<#BLZQoG$2#e|B|;4kLoyKyvx83BL9^3`)%P$V3C&na$%0CE^rH`V zdZ4)t^v2Lf^)Mv2ez9-%4yXzW39VMoQ-Xnr@XB^5lCK4*{FlVQP+U9=d7d>M8vO!g z!H&TNXd>UN>N4%pNcsm4Wh==u=+Ul`~>Qw+*lBH0|r4gnpcn2Q2-Dn==`_@_J} z#g-TC>QI8%D!5&j^!@J&)b^8~QrQlc92kiR`N%s92xu;{0{Y1&&d%29g8*VWY^15J zQFW(Ti~L-elm>p^1?VRA9~E0*3W%V+(C`+q02VMA+Q&<4`XQygKWTT%!zM2jBPj{P z7zH(PHn_NSnV0CDhdmb96emo5YyoG3eV)6ba+>@COo1TQ-i=Eeip$gtJb)OzU;5xa z=-IBr2`M|O6>F_<(wPbE<9L9In&PkXG-m_PBmst&tnpn(1}}T0L)pFF44C`^5isVp z8#N5`#iBnfr@=C*bep3jBd)xH!k$zrYRaPasBcxEKonxm#q9?}(M{KhhzSH`dn_8L zi@o1+^}n@3BkIC3jXE?wA?;hS74+;Fag?mRzVM9MW{%H4sRXw8!R>F4Ex!m}q6$=+75BrzvN5yN3#UY*AV*OZxPaWU%$}!P1*|>*!-9=(Gc&uAkT>M?>d2?;Ry?dzStA4)U-@R88@6f7@t?DVy? z{-tQZsxhE9UAd7fdPM~wzAMzO4=*oERfsM;+x!xHV-zp(F zmz4)+LA*Y4`!7;t5r`n03A&kXT608VFY{dH!{c^(aYS9@pNUXIW21U9sV-xy|7_iL ze<_%364?%QOb-FDWL|B1HOwyE-|zeWw-jC0v5J`Yd$6!b+L{>|*n#3oXM)Rhl-p&s zBbtkqVAo@0)KezC3w!;9JoAB4j$nzrdGB99B;nribkh=Y421zni}u*iKR*uHf3Wfa z@voi7;{`mSR=E$8%`_rStfn*mg4AH08XGFN`L}Q5&gXR; z&QvDysFc9+n_|BM`_1OXvwxinVjw1yLQtwX2p~XoR6z%YRl$>kPjbN@Gki=Za2mJ9 zlW_F|K9m}(2aJ|fxB;Ds+m0ufVCUNs;s|=l|Mm5JB_NYIHl!M`XR`b1xl_PDx1q>y5<{=}CJbk>`{LKlB~4kULpWuKjR<{OJN5n-++0 z=kFvK2?QDiVA{kT-cy`t9F@4Tvmi2QW&oW))7u$I3+W7@lr7s>Fv8kv0+J!#le&B> zDOQ*}Kc%@Gkxc7hB+KAUVBP3_?r2Q!#CTCM0Wt5_AK4518H;rMH6qsL8f<2mA3lm2 zHS6?_t&Gv2ZRX1bUr%MW8%pV$-XS)W*E?BiAnoZ(C9(J1+PAMqybXv+&^2cUTDZNw zSjwlNx&zQ)=D+7&B_Fx4rSPh;ASn%`4tnfw0CV-1WRi!-iB12uA3X~I?>JYDzdt@m zoqz(*9XN&IDN8hNS8HgM_9Uu#u^Us9nwM8_Ut$9^g^J+a7Y~|OGD(<|fU8;m&D`RJ z#9lYe==1&8%ObLBz|rRJx)W+8W#q#J;wwfVRBsx#i4a=^yYX;$3F&Qf@)c16r{+q9 z&j{%fTT-{GTH9tibXY;-6_d7$5|AP7vq(a8n77W91C_O5N?b_u8?YYu4x6KEi zm1xK1@fyPUwJrJKs+Iqz!4s<%hFc7Uvs7S2&#|*$OP!Fg(&>co6rlhu_WvHkD_pRn z=n@bF&wG4JFmlA-Y!#hluwh74DYq% z`7+hhe0+t!&BAG9(QvL2&t;+y1Aczr=SDs5acXA_z(m{ne4MB2zn0mYDl43CiFNfj zwbz|N6YUSrj=dm6)t-tvHuK-so+ppQ{*vv+hJ45l`p26<1LBW$Q-uTJ&&)x-02+hp z;pEPLmjxJ5degD7$1FCut9u#6Rrz0JjY-a)|6NiBFNH}WMEm+Ps4sMaX1UGTZoEw; z-+@^$M!fM)-b{qn>7A;s^d8jK(`m5z_aND@r*@Q=N!%FX%MaM!Q5;MX;LPtrf`CzS z#mVgKHeVffk)<~6l?UIMQH%Ff9#AMu<<9};l%(x-Fu^oKk3t`~q|H``V zcq-rbA5lg)L}ZT!$|@rB2$9GUvQmoL{>;-W<+I# z-*um%U!QNU{y4ol&w1`~?fZSb?>x9k4B}4PMX^|#T9lb zV55O-bM+R#GSTlg+G56>{btl%gf(fyT8ZMSF}jVbfx>U^|s1d5eCRG94;J&lK-}-RBF81I`^C*jmsJO*PqjWn*g=_mVl691~ z{k3%PR?LBL*;yz;GIoBN=&jJ`SJR7?jCG_>5rSJ!LmP(}SGPD!gvcG9Zz;xpU7TU- zsi#WJtAg&$xW$x*SH&wk9J2sGx+)G`W5;wq1j=xOcE6lD;E5;CP?+X4hG84e<_nDs zBB#eCC|CpdzjM-r=geA&a0sC9Zvc((YEyJD4Eb78Q3`7e}_RE;AKYg!Cb^yDPE zp&LKeW(X}8e9gXBLq9z-H`3uS-Volq-{a)!tN;1Xv@u7UXJ2?b9E6zELK!^S0m2;- zCl}AlRz+^_ZJ`889q~UO^k@n3K`DTn#sY1oYrQ9)X@_y@RY}#)=?69<{K`LH{`(7r zIZUV(52_ITfu@Aa7YL4khda)cy~Z6!eOCPlWvTlvk@~wEX~bm+k*|Q;!fQG8ISE&A zZZjb?+{i`Z1j5|GYTeoQ0gxbO24teI)t%Y`=w4{cWyf>bWYIK#cOLIt@c&4Ph%Ix2Ckt& z-x5BjO?Oz%5u*HDm~K08P!-kc(Wd|guMXnV)F)4#SWh?=g@py~hb-hc`e-kCETlmVicVb-`XlWByR=u}$K=l}hM2;zGDv)FnLOa; zNV$u%!|^KKZtvhM``~0#7#i7vov_NKg~z8i6a+auvgkF6Vu06BCWzNL2G!wQXjk(A zCUTGD^|Nj6qhn(yfFRQS73207^N)a0p&FD@Xe5u94GqJz99vp+)6Cy_7xgz_%r*ps zCEg@u#R*Wr2!NmtM)AE=E)mGy9c{WHLG{9UK#j^k(A8n-QL!CfO<>}F?Y4Dhbr|py z%d`N!+;=2rB&;fht)cKSPfP|Fr;oLu9gekz>t~7ia{+BMz-0i&%k@(kU)=St@Z`mbA=hEZ%g7jfa3BBmz3!q^+#1&R0AvDN%;TQ%=N|KcNnoP_(aN zQbbvNdQe$1Rgj4N^r#9Az=y^LrcP*F2$?rEGZXl+SW;5rc;?J?$T@YDOKL zkbUZiEYk6&ZO3Y&&Jw5t2&~jZD1|TtLK6gEkIYQS9yk#X-QA!h1~(`Raoe1u(Ip84 z&K*RWOVNRL8&#%;od}>~03m>cu&~7**D2`*<;Yqta~t;Cb{CzDLi+vCJF!G_dd}dB zv0cYV|3Z@)6paekbjoOi_+D9RT}FJ%mk$uJ-vFN~-2C32q(Qq_&%v9YnQCdmt?JAo-|9qa7V#$uv^C|c`q_9>&n@H>)(znd(Y z1J$ZCzU?SR9EmG1tRX_duo{m1IiUcxqqu^VJlMl@oWYU~nbilVk{orguTEpx-Nxg+ zLM|}v+I;Wg74YtM?_fCzL6~3S`Fe@1KU?ETOKSQM(uj@krJi)L)@&D^3>n1xh5wh*GndE!=;{O8^jj z1%-_?3Rq~NZ;56liSF=bSmAr_vzw?2PY}ekLvI2irkw`9;98B9`9|rA+>{g%6vNHE z;x2C*^d2Uw_`(&KYCychUg`qZTOeSRrJN(g-?ny}W11g5d{|!?EM@at_SCz0(;_gtFH^hx)I(=m+pk~4A1sdoTsO!{GL7gOXAd% zcIN|=luk-_9z_i9vFF%7IL%)cL>+0LFaa>88s$G?pH^!uPv0miPVcIM4h0OdDj(b0 z&LeLDJGp=ZbILZ&LiIJ`36*e0$7N_}mKXxU zh7PCKs!dg{71$KS%gft;T4T}jOwS8T;|I8-gp(u#q6el&C&93os#TFum5`_PznE#b z1_6xEK(wwXS19Z64Lq{Khkzu?0!kiU8#_*`&)gB?jQ&KCP&E;lu#x!1^%T2JB%+p_ zNEBPg+m?8#3v62|cq675bT*iH9hPcsA4x%dLGg*=eaG9g>RBel*7GErN3yF#J=t?WNk3PUU)RjI3?~hYZV8Ea0R|DJGK{ZXov68ci_~rxAPvVRMdaO8=0e^D> zsu8)6S)YlGO2uWo%T^*SDs~Zk)4Vb`sf`2x+R3QIvu{liE8$vxtPHcAGw}bg~ zl2(1&HRca|Y;E&jbL(|gx)l$`v8o7qyp!wj>h*r{RUm_nND+M;4;Z&s1{vB6v-c{2K&5K#S5buk zu!q|G3P|npZH}#-^zB`=2QwgtA8vZycbVB9=qk&&A4G$sH?h$xm(I(%eqH);yCTtb zvkB*Dh)`$@N&YIW!Af7^pkv72kpnpq>iqA&bNXIbz{+B8aRwYcb0F?3Xi!z~7Ggn9 zfH8J@Yq8)vfxh8YbluF%OvlyL_0oWtP#7rYDGgt$mpA~?tT#5WQUvvyg+a<#2aR)| zzc>@Cax+}*dl%Y@_(i|p1#!X3Fs45)PIBFRP$wn-l?PlaU&a}GoT8%o91kvjm3{Tg zNGvyEYG-4*KpT?YAruuOW#hmDL6-(9l=a$WjDo|$}1sS=PUZ`Wu1MrqI@7$ zQ&+&#^tyn`9_g~#=J7-(Qc5#hUI{gz3+T@sI%B5bz;crdx z&^_zrm)A%s3@8~NjrE)QQ<8N1`d%~2D=OAO0c|rB#E0=wqoborKR=p$Malw48=jqZ zYf(TjtU}07f{5C|at)L~ah#*2S+cuG(|lk7;uD^vOxQSVgM5C+{6@Hd_0)n<8c z^`tBiF7cS*cA-WzWDgL80c(J15oe@5%RSe8yPR``Kjl}$ZKE0@UdvE&o#o~MW{SF# zb_=_L@}T4y=`mVnRZz%|lr%)Nc>Ec^s?WK1$r>8y){iqj>9#@;j!JkBZ9UB(zW7E>C0Bhtb+R|0 zG$C%wR{_4VyN1X)>f1AO?>SIe+Jjf}<19W-G6t(YP#k&hI|nY9UPWFzk4xRD3&>YW z^}{c~`ED-2c^+hCNn|D0l6GQsorr2QK_b2D2zD4KdjJZd7LcJHpT&BS3<+N*CTeJBxwZ4yxPWBgN539KmgK;ze8Xb+~k*fvt_S39s9 z=zC4gLaEy0cMC<&s7p0_6$~~68@d%-767jR-1&AmDvs_Fwv|bSpx836x}MnWOpg4h zTU;59!SN__JnXUbA>e)R$}`)~YCd@QP!be06?W8rp(VN2SXaQzM~smgbdwcjo>_cD z>%F<-r*?`3^#izH7LiAP!WdcT~F3#tRIznMZ4vw0kQS_A1{ Date: Thu, 20 Oct 2022 17:45:16 -0500 Subject: [PATCH 539/947] Adding some BDN benchmarks. One body constraints first. --- DemoBenchmarks/DemoBenchmarks.csproj | 19 +++ DemoBenchmarks/OneBodyConstraints.cs | 236 +++++++++++++++++++++++++++ DemoBenchmarks/Program.cs | 4 + Demos.sln | 26 +++ 4 files changed, 285 insertions(+) create mode 100644 DemoBenchmarks/DemoBenchmarks.csproj create mode 100644 DemoBenchmarks/OneBodyConstraints.cs create mode 100644 DemoBenchmarks/Program.cs diff --git a/DemoBenchmarks/DemoBenchmarks.csproj b/DemoBenchmarks/DemoBenchmarks.csproj new file mode 100644 index 000000000..9dd142c5c --- /dev/null +++ b/DemoBenchmarks/DemoBenchmarks.csproj @@ -0,0 +1,19 @@ + + + + Exe + net7.0 + enable + enable + + + + + + + + + + + + diff --git a/DemoBenchmarks/OneBodyConstraints.cs b/DemoBenchmarks/OneBodyConstraints.cs new file mode 100644 index 000000000..72b0f4dce --- /dev/null +++ b/DemoBenchmarks/OneBodyConstraints.cs @@ -0,0 +1,236 @@ +using BenchmarkDotNet.Attributes; +using BepuPhysics; +using BepuPhysics.Constraints; +using BepuPhysics.Constraints.Contact; +using BepuUtilities; +using System.Numerics; + +namespace DemoBenchmarks; + +public class OneBodyConstraintBenchmarks +{ + static BodyVelocityWide BenchmarkOneBodyConstraint(Vector3Wide positionA, QuaternionWide orientationA, BodyInertiaWide inertiaA, TPrestep prestep) + where TConstraintFunctions : unmanaged, IOneBodyConstraintFunctions where TPrestep : unmanaged where TAccumulatedImpulse : unmanaged + { + var functions = default(TConstraintFunctions); + var accumulatedImpulse = default(TAccumulatedImpulse); + var velocityA = default(BodyVelocityWide); + //Individual constraint iterations are often extremely cheap, so beef the benchmark up a bit. + const int iterations = 10000; + const float inverseDt = 60f; + const float dt = 1f / inverseDt; + for (int i = 0; i < iterations; ++i) + { + functions.WarmStart(positionA, orientationA, inertiaA, ref prestep, ref accumulatedImpulse, ref velocityA); + functions.Solve(positionA, orientationA, inertiaA, dt, inverseDt, ref prestep, ref accumulatedImpulse, ref velocityA); + } + return velocityA; + } + + [Benchmark] + public BodyVelocityWide Contact1OneBody() + { + var prestep = new Contact1OneBodyPrestepData + { + Contact0 = new() { Depth = Vector.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) }, + MaterialProperties = new MaterialPropertiesWide + { + FrictionCoefficient = new Vector(1f), + MaximumRecoveryVelocity = new Vector(2f), + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } + }, + Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) + }; + + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkOneBodyConstraint(new Vector3Wide(), orientation, inertia, prestep); + } + + [Benchmark] + public BodyVelocityWide Contact2OneBody() + { + var prestep = new Contact2OneBodyPrestepData + { + Contact0 = new() { Depth = Vector.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) }, + Contact1 = new() { Depth = Vector.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) }, + MaterialProperties = new MaterialPropertiesWide + { + FrictionCoefficient = new Vector(1f), + MaximumRecoveryVelocity = new Vector(2f), + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } + }, + Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) + }; + + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkOneBodyConstraint(new Vector3Wide(), orientation, inertia, prestep); + } + + [Benchmark] + public BodyVelocityWide Contact3OneBody() + { + var prestep = new Contact3OneBodyPrestepData + { + Contact0 = new() { Depth = Vector.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) }, + Contact1 = new() { Depth = Vector.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) }, + Contact2 = new() { Depth = Vector.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) }, + MaterialProperties = new MaterialPropertiesWide + { + FrictionCoefficient = new Vector(1f), + MaximumRecoveryVelocity = new Vector(2f), + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } + }, + Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) + }; + + QuaternionWide.Broadcast(Quaternion.Identity, out var orientationA); + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkOneBodyConstraint(new Vector3Wide(), orientationA, inertia, prestep); + } + [Benchmark] + public BodyVelocityWide Contact4OneBody() + { + var prestep = new Contact4OneBodyPrestepData + { + Contact0 = new() { Depth = Vector.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) }, + Contact1 = new() { Depth = Vector.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) }, + Contact2 = new() { Depth = Vector.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) }, + Contact3 = new() { Depth = Vector.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) }, + MaterialProperties = new MaterialPropertiesWide + { + FrictionCoefficient = new Vector(1f), + MaximumRecoveryVelocity = new Vector(2f), + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } + }, + Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) + }; + + QuaternionWide.Broadcast(Quaternion.Identity, out var orientationA); + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkOneBodyConstraint(new Vector3Wide(), orientationA, inertia, prestep); + } + + [Benchmark] + public BodyVelocityWide Contact2NonconvexOneBody() + { + var prestep = new Contact2NonconvexOneBodyPrestepData + { + Contact0 = new() { Depth = Vector.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) }, + Contact1 = new() { Depth = Vector.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) }, + MaterialProperties = new MaterialPropertiesWide + { + FrictionCoefficient = new Vector(1f), + MaximumRecoveryVelocity = new Vector(2f), + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } + }, + }; + + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkOneBodyConstraint, + Contact2NonconvexOneBodyPrestepData, Contact2NonconvexAccumulatedImpulses>(new Vector3Wide(), orientation, inertia, prestep); + } + + [Benchmark] + public BodyVelocityWide Contact3NonconvexOneBody() + { + var prestep = new Contact3NonconvexOneBodyPrestepData + { + Contact0 = new() { Depth = Vector.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) }, + Contact1 = new() { Depth = Vector.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) }, + Contact2 = new() { Depth = Vector.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) }, + MaterialProperties = new MaterialPropertiesWide + { + FrictionCoefficient = new Vector(1f), + MaximumRecoveryVelocity = new Vector(2f), + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } + }, + }; + + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkOneBodyConstraint, + Contact3NonconvexOneBodyPrestepData, Contact3NonconvexAccumulatedImpulses>(new Vector3Wide(), orientation, inertia, prestep); + } + + [Benchmark] + public BodyVelocityWide Contact4NonconvexOneBody() + { + var prestep = new Contact4NonconvexOneBodyPrestepData + { + Contact0 = new() { Depth = Vector.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) }, + Contact1 = new() { Depth = Vector.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) }, + Contact2 = new() { Depth = Vector.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) }, + Contact3 = new() { Depth = Vector.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) }, + MaterialProperties = new MaterialPropertiesWide + { + FrictionCoefficient = new Vector(1f), + MaximumRecoveryVelocity = new Vector(2f), + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } + }, + }; + + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkOneBodyConstraint, + Contact4NonconvexOneBodyPrestepData, Contact4NonconvexAccumulatedImpulses>(new Vector3Wide(), orientation, inertia, prestep); + } + + [Benchmark] + public BodyVelocityWide OneBodyAngularServo() + { + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var prestep = new OneBodyAngularServoPrestepData + { + ServoSettings = new() { BaseSpeed = Vector.Zero, MaximumForce = new Vector(float.MaxValue), MaximumSpeed = new Vector(float.MaxValue) }, + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) }, + TargetOrientation = orientation + }; + + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkOneBodyConstraint(new Vector3Wide(), orientation, inertia, prestep); + } + + [Benchmark] + public BodyVelocityWide OneBodyAngularMotor() + { + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var prestep = new OneBodyAngularMotorPrestepData + { + Settings = new() { Damping = Vector.One, MaximumForce = Vector.One }, + TargetVelocity = default + }; + + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkOneBodyConstraint(new Vector3Wide(), orientation, inertia, prestep); + } + + [Benchmark] + public BodyVelocityWide OneBodyLinearServo() + { + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var prestep = new OneBodyLinearServoPrestepData + { + ServoSettings = new() { BaseSpeed = Vector.Zero, MaximumForce = new Vector(float.MaxValue), MaximumSpeed = new Vector(float.MaxValue) }, + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } + }; + + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkOneBodyConstraint(new Vector3Wide(), orientation, inertia, prestep); + } + + [Benchmark] + public BodyVelocityWide OneBodyLinearMotor() + { + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var prestep = new OneBodyLinearMotorPrestepData + { + Settings = new() { Damping = Vector.One, MaximumForce = Vector.One }, + }; + + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkOneBodyConstraint(new Vector3Wide(), orientation, inertia, prestep); + } +} diff --git a/DemoBenchmarks/Program.cs b/DemoBenchmarks/Program.cs new file mode 100644 index 000000000..8cacc684b --- /dev/null +++ b/DemoBenchmarks/Program.cs @@ -0,0 +1,4 @@ +using BenchmarkDotNet.Running; +using DemoBenchmarks; + +var summary = BenchmarkRunner.Run(typeof(OneBodyConstraintBenchmarks)); \ No newline at end of file diff --git a/Demos.sln b/Demos.sln index 47c988c3c..93ca532cf 100644 --- a/Demos.sln +++ b/Demos.sln @@ -26,6 +26,8 @@ Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "BepuPhysics", "BepuPhysics\ EndProject Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "DemoTests", "DemoTests\DemoTests.csproj", "{32BABF14-6971-41F8-A556-8E0F2D8C86B2}" EndProject +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "DemoBenchmarks", "DemoBenchmarks\DemoBenchmarks.csproj", "{EA4ED604-6F12-42E6-8A0C-FC102B7D227B}" +EndProject Global GlobalSection(SolutionConfigurationPlatforms) = preSolution Debug|Any CPU = Debug|Any CPU @@ -234,6 +236,30 @@ Global {32BABF14-6971-41F8-A556-8E0F2D8C86B2}.ReleaseNoProfiling|x64.Build.0 = Release|Any CPU {32BABF14-6971-41F8-A556-8E0F2D8C86B2}.ReleaseNoProfiling|x86.ActiveCfg = Release|Any CPU {32BABF14-6971-41F8-A556-8E0F2D8C86B2}.ReleaseNoProfiling|x86.Build.0 = Release|Any CPU + {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.Debug|Any CPU.Build.0 = Debug|Any CPU + {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.Debug|ARM.ActiveCfg = Debug|Any CPU + {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.Debug|ARM.Build.0 = Debug|Any CPU + {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.Debug|x64.ActiveCfg = Debug|Any CPU + {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.Debug|x64.Build.0 = Debug|Any CPU + {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.Debug|x86.ActiveCfg = Debug|Any CPU + {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.Debug|x86.Build.0 = Debug|Any CPU + {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.Release|Any CPU.ActiveCfg = Release|Any CPU + {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.Release|Any CPU.Build.0 = Release|Any CPU + {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.Release|ARM.ActiveCfg = Release|Any CPU + {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.Release|ARM.Build.0 = Release|Any CPU + {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.Release|x64.ActiveCfg = Release|Any CPU + {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.Release|x64.Build.0 = Release|Any CPU + {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.Release|x86.ActiveCfg = Release|Any CPU + {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.Release|x86.Build.0 = Release|Any CPU + {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.ReleaseNoProfiling|Any CPU.ActiveCfg = Release|Any CPU + {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.ReleaseNoProfiling|Any CPU.Build.0 = Release|Any CPU + {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.ReleaseNoProfiling|ARM.ActiveCfg = Release|Any CPU + {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.ReleaseNoProfiling|ARM.Build.0 = Release|Any CPU + {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.ReleaseNoProfiling|x64.ActiveCfg = Release|Any CPU + {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.ReleaseNoProfiling|x64.Build.0 = Release|Any CPU + {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.ReleaseNoProfiling|x86.ActiveCfg = Release|Any CPU + {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.ReleaseNoProfiling|x86.Build.0 = Release|Any CPU EndGlobalSection GlobalSection(SolutionProperties) = preSolution HideSolutionNode = FALSE From 54da4e46a11a6c7de1801c71f6fa852357241ed7 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 20 Oct 2022 21:54:55 -0500 Subject: [PATCH 540/947] Two body constraints. --- DemoBenchmarks/OneBodyConstraints.cs | 7 + DemoBenchmarks/Program.cs | 3 +- DemoBenchmarks/TwoBodyConstraints.cs | 568 +++++++++++++++++++++++++++ 3 files changed, 577 insertions(+), 1 deletion(-) create mode 100644 DemoBenchmarks/TwoBodyConstraints.cs diff --git a/DemoBenchmarks/OneBodyConstraints.cs b/DemoBenchmarks/OneBodyConstraints.cs index 72b0f4dce..d1feb9ff7 100644 --- a/DemoBenchmarks/OneBodyConstraints.cs +++ b/DemoBenchmarks/OneBodyConstraints.cs @@ -7,6 +7,13 @@ namespace DemoBenchmarks; +///

+/// Evaluates performance of all one body constraints. +/// +/// +/// Note that all constraints operate across lanes simultaneously where T is of type . +/// The number of bundles being executed does not change if changes; if larger bundles are allowed, then more lanes end up getting solved. +/// public class OneBodyConstraintBenchmarks { static BodyVelocityWide BenchmarkOneBodyConstraint(Vector3Wide positionA, QuaternionWide orientationA, BodyInertiaWide inertiaA, TPrestep prestep) diff --git a/DemoBenchmarks/Program.cs b/DemoBenchmarks/Program.cs index 8cacc684b..f76d73d20 100644 --- a/DemoBenchmarks/Program.cs +++ b/DemoBenchmarks/Program.cs @@ -1,4 +1,5 @@ using BenchmarkDotNet.Running; using DemoBenchmarks; -var summary = BenchmarkRunner.Run(typeof(OneBodyConstraintBenchmarks)); \ No newline at end of file +var summaryOneBody = BenchmarkRunner.Run(typeof(OneBodyConstraintBenchmarks)); +var summaryTwoBody = BenchmarkRunner.Run(typeof(TwoBodyConstraintBenchmarks)); \ No newline at end of file diff --git a/DemoBenchmarks/TwoBodyConstraints.cs b/DemoBenchmarks/TwoBodyConstraints.cs new file mode 100644 index 000000000..c9a3e7631 --- /dev/null +++ b/DemoBenchmarks/TwoBodyConstraints.cs @@ -0,0 +1,568 @@ +using BenchmarkDotNet.Attributes; +using BepuPhysics; +using BepuPhysics.Constraints; +using BepuPhysics.Constraints.Contact; +using BepuUtilities; +using System.Numerics; + +namespace DemoBenchmarks; + +/// +/// Evaluates performance of all two body constraints. +/// +/// +/// Note that all constraints operate across lanes simultaneously where T is of type . +/// The number of bundles being executed does not change if changes; if larger bundles are allowed, then more lanes end up getting solved. +/// +public class TwoBodyConstraintBenchmarks +{ + static (BodyVelocityWide, BodyVelocityWide) BenchmarkTwoBodyConstraint( + Vector3Wide positionA, QuaternionWide orientationA, BodyInertiaWide inertiaA, + Vector3Wide positionB, QuaternionWide orientationB, BodyInertiaWide inertiaB, TPrestep prestep) + where TConstraintFunctions : unmanaged, ITwoBodyConstraintFunctions where TPrestep : unmanaged where TAccumulatedImpulse : unmanaged + { + var functions = default(TConstraintFunctions); + var accumulatedImpulse = default(TAccumulatedImpulse); + var velocityA = default(BodyVelocityWide); + var velocityB = default(BodyVelocityWide); + //Individual constraint iterations are often extremely cheap, so beef the benchmark up a bit. + const int iterations = 10000; + const float inverseDt = 60f; + const float dt = 1f / inverseDt; + for (int i = 0; i < iterations; ++i) + { + functions.WarmStart(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, ref prestep, ref accumulatedImpulse, ref velocityA, ref velocityB); + functions.Solve(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, dt, inverseDt, ref prestep, ref accumulatedImpulse, ref velocityA, ref velocityB); + } + return (velocityA, velocityB); + } + + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide) Contact1() + { + var prestep = new Contact1PrestepData + { + Contact0 = new ConvexContactWide { Depth = Vector.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) }, + MaterialProperties = new MaterialPropertiesWide + { + FrictionCoefficient = new Vector(1f), + MaximumRecoveryVelocity = new Vector(2f), + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } + }, + Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)), + OffsetB = Vector3Wide.Broadcast(new Vector3(2, 0, 0)) + }; + + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkTwoBodyConstraint(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); + } + + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide) Contact2() + { + var prestep = new Contact2PrestepData + { + Contact0 = new ConvexContactWide { Depth = Vector.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) }, + Contact1 = new ConvexContactWide { Depth = Vector.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) }, + MaterialProperties = new MaterialPropertiesWide + { + FrictionCoefficient = new Vector(1f), + MaximumRecoveryVelocity = new Vector(2f), + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } + }, + Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)), + OffsetB = Vector3Wide.Broadcast(new Vector3(2, 0, 0)) + }; + + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkTwoBodyConstraint(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); + } + + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide) Contact3() + { + var prestep = new Contact3PrestepData + { + Contact0 = new ConvexContactWide { Depth = Vector.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) }, + Contact1 = new ConvexContactWide { Depth = Vector.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) }, + Contact2 = new ConvexContactWide { Depth = Vector.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) }, + MaterialProperties = new MaterialPropertiesWide + { + FrictionCoefficient = new Vector(1f), + MaximumRecoveryVelocity = new Vector(2f), + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } + }, + Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)), + OffsetB = Vector3Wide.Broadcast(new Vector3(2, 0, 0)) + }; + + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkTwoBodyConstraint(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); + } + + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide) Contact4() + { + var prestep = new Contact4PrestepData + { + Contact0 = new ConvexContactWide { Depth = Vector.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) }, + Contact1 = new ConvexContactWide { Depth = Vector.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) }, + Contact2 = new ConvexContactWide { Depth = Vector.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) }, + Contact3 = new ConvexContactWide { Depth = Vector.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) }, + MaterialProperties = new MaterialPropertiesWide + { + FrictionCoefficient = new Vector(1f), + MaximumRecoveryVelocity = new Vector(2f), + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } + }, + Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)), + OffsetB = Vector3Wide.Broadcast(new Vector3(2, 0, 0)) + }; + + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkTwoBodyConstraint(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); + } + + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide) Contact2Nonconvex() + { + var prestep = new Contact2NonconvexPrestepData + { + Contact0 = new NonconvexContactPrestepData { Depth = Vector.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) }, + Contact1 = new NonconvexContactPrestepData { Depth = Vector.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 0, 1)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) }, + MaterialProperties = new MaterialPropertiesWide + { + FrictionCoefficient = new Vector(1f), + MaximumRecoveryVelocity = new Vector(2f), + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } + }, + OffsetB = Vector3Wide.Broadcast(new Vector3(2, 0, 0)) + }; + + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkTwoBodyConstraint, + Contact2NonconvexPrestepData, Contact2NonconvexAccumulatedImpulses>(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); + } + + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide) Contact3Nonconvex() + { + var prestep = new Contact3NonconvexPrestepData + { + Contact0 = new NonconvexContactPrestepData { Depth = Vector.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) }, + Contact1 = new NonconvexContactPrestepData { Depth = Vector.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 0, 1)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) }, + Contact2 = new NonconvexContactPrestepData { Depth = Vector.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 1, 0)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) }, + MaterialProperties = new MaterialPropertiesWide + { + FrictionCoefficient = new Vector(1f), + MaximumRecoveryVelocity = new Vector(2f), + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } + }, + OffsetB = Vector3Wide.Broadcast(new Vector3(2, 0, 0)) + }; + + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkTwoBodyConstraint, + Contact3NonconvexPrestepData, Contact3NonconvexAccumulatedImpulses>(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); + } + + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide) Contact4Nonconvex() + { + var prestep = new Contact4NonconvexPrestepData + { + Contact0 = new NonconvexContactPrestepData { Depth = Vector.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) }, + Contact1 = new NonconvexContactPrestepData { Depth = Vector.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 0, 1)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) }, + Contact2 = new NonconvexContactPrestepData { Depth = Vector.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 1, 0)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) }, + Contact3 = new NonconvexContactPrestepData { Depth = Vector.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 1, 1)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) }, + MaterialProperties = new MaterialPropertiesWide + { + FrictionCoefficient = new Vector(1f), + MaximumRecoveryVelocity = new Vector(2f), + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } + }, + OffsetB = Vector3Wide.Broadcast(new Vector3(2, 0, 0)) + }; + + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkTwoBodyConstraint, + Contact4NonconvexPrestepData, Contact4NonconvexAccumulatedImpulses>(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); + } + + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide) BallSocket() + { + var prestep = new BallSocketPrestepData + { + LocalOffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), + LocalOffsetB = Vector3Wide.Broadcast(new Vector3(-1, 0, 0)), + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } + }; + + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkTwoBodyConstraint(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); + } + + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide) AngularHinge() + { + var prestep = new AngularHingePrestepData + { + LocalHingeAxisA = Vector3Wide.Broadcast(new Vector3(0, 1, 0)), + LocalHingeAxisB = Vector3Wide.Broadcast(new Vector3(0, 1, 0)), + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } + }; + + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkTwoBodyConstraint(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); + } + + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide) AngularSwivelHinge() + { + var prestep = new AngularSwivelHingePrestepData + { + LocalSwivelAxisA = Vector3Wide.Broadcast(new Vector3(0, 1, 0)), + LocalHingeAxisB = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } + }; + + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkTwoBodyConstraint>(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); + } + + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide) SwingLimit() + { + var prestep = new SwingLimitPrestepData + { + AxisLocalA = Vector3Wide.Broadcast(new Vector3(0, 1, 0)), + AxisLocalB = Vector3Wide.Broadcast(new Vector3(0, 1, 0)), + MinimumDot = new Vector(0.9f), + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } + }; + + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkTwoBodyConstraint>(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); + } + + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide) TwistServo() + { + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var prestep = new TwistServoPrestepData + { + LocalBasisA = orientation, + LocalBasisB = orientation, + ServoSettings = new ServoSettingsWide { MaximumForce = new Vector(float.MaxValue), MaximumSpeed = new Vector(float.MaxValue) }, + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } + }; + + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkTwoBodyConstraint>(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); + } + + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide) TwistLimit() + { + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var prestep = new TwistLimitPrestepData + { + LocalBasisA = orientation, + LocalBasisB = orientation, + MinimumAngle = Vector.Zero, + MaximumAngle = Vector.One, + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } + }; + + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkTwoBodyConstraint>(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); + } + + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide) TwistMotor() + { + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var prestep = new TwistMotorPrestepData + { + LocalAxisA = Vector3Wide.Broadcast(new Vector3(0, 1, 0)), + LocalAxisB = Vector3Wide.Broadcast(new Vector3(0, 1, 0)), + Settings = new() { Damping = Vector.One, MaximumForce = new Vector(float.MaxValue) } + }; + + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkTwoBodyConstraint>(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); + } + + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide) AngularServo() + { + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var prestep = new AngularServoPrestepData + { + TargetRelativeRotationLocalA = orientation, + ServoSettings = new ServoSettingsWide { MaximumForce = new Vector(float.MaxValue), MaximumSpeed = new Vector(float.MaxValue) }, + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } + }; + + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkTwoBodyConstraint(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); + } + + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide) AngularMotor() + { + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var prestep = new AngularMotorPrestepData + { + Settings = new() { Damping = Vector.One, MaximumForce = new Vector(float.MaxValue) } + }; + + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkTwoBodyConstraint(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); + } + + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide) Weld() + { + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var prestep = new WeldPrestepData + { + LocalOffset = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), + LocalOrientation = orientation, + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } + }; + + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkTwoBodyConstraint(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); + } + + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide) DistanceServo() + { + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var prestep = new DistanceServoPrestepData + { + LocalOffsetA = Vector3Wide.Broadcast(new Vector3(0.5f, 0, 0)), + LocalOffsetB = Vector3Wide.Broadcast(new Vector3(-0.5f, 0, 0)), + TargetDistance = Vector.One, + ServoSettings = new ServoSettingsWide { MaximumForce = new Vector(float.MaxValue), MaximumSpeed = new Vector(float.MaxValue) }, + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } + }; + + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkTwoBodyConstraint>(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); + } + + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide) DistanceLimit() + { + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var prestep = new DistanceLimitPrestepData + { + LocalOffsetA = Vector3Wide.Broadcast(new Vector3(0.5f, 0, 0)), + LocalOffsetB = Vector3Wide.Broadcast(new Vector3(-0.5f, 0, 0)), + MinimumDistance = new Vector(0.5f), + MaximumDistance = new Vector(1.5f), + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } + }; + + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkTwoBodyConstraint>(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); + } + + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide) CenterDistanceConstraint() + { + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var prestep = new CenterDistancePrestepData + { + TargetDistance = new Vector(0.5f), + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } + }; + + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkTwoBodyConstraint>(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); + } + + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide) PointOnLineServo() + { + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var prestep = new PointOnLineServoPrestepData + { + LocalDirection = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), + LocalOffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), + LocalOffsetB = Vector3Wide.Broadcast(new Vector3(-1, 0, 0)), + ServoSettings = new ServoSettingsWide { MaximumForce = new Vector(float.MaxValue), MaximumSpeed = new Vector(float.MaxValue) }, + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } + }; + + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkTwoBodyConstraint(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); + } + + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide) LinearAxisServo() + { + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var prestep = new LinearAxisServoPrestepData + { + LocalOffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), + LocalOffsetB = Vector3Wide.Broadcast(new Vector3(-1, 0, 0)), + LocalPlaneNormal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)), + ServoSettings = new ServoSettingsWide { MaximumForce = new Vector(float.MaxValue), MaximumSpeed = new Vector(float.MaxValue) }, + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } + }; + + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkTwoBodyConstraint>(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); + } + + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide) LinearAxisMotor() + { + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var prestep = new LinearAxisMotorPrestepData + { + LocalOffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), + LocalOffsetB = Vector3Wide.Broadcast(new Vector3(-1, 0, 0)), + LocalPlaneNormal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)), + Settings = new() { Damping = Vector.One, MaximumForce = new Vector(float.MaxValue) } + }; + + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkTwoBodyConstraint>(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); + } + + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide) LinearAxisLimit() + { + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var prestep = new LinearAxisLimitPrestepData + { + LocalOffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), + LocalOffsetB = Vector3Wide.Broadcast(new Vector3(-1, 0, 0)), + LocalPlaneNormal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)), + MaximumOffset = new Vector(3), + MinimumOffset = new Vector(-3), + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } + }; + + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkTwoBodyConstraint>(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); + } + + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide) AngularAxisMotor() + { + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var prestep = new AngularAxisMotorPrestepData + { + LocalAxisA = Vector3Wide.Broadcast(new Vector3(0, 1, 0)), + Settings = new() { Damping = Vector.One, MaximumForce = new Vector(float.MaxValue) } + }; + + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkTwoBodyConstraint>(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); + } + + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide) SwivelHinge() + { + var prestep = new SwivelHingePrestepData + { + LocalSwivelAxisA = Vector3Wide.Broadcast(new Vector3(0, 1, 0)), + LocalHingeAxisB = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } + }; + + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkTwoBodyConstraint(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); + } + + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide) Hinge() + { + var prestep = new HingePrestepData + { + LocalHingeAxisA = Vector3Wide.Broadcast(new Vector3(0, 1, 0)), + LocalHingeAxisB = Vector3Wide.Broadcast(new Vector3(0, 1, 0)), + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } + }; + + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkTwoBodyConstraint(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); + } + + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide) BallSocketMotor() + { + var prestep = new BallSocketMotorPrestepData + { + Settings = new() { Damping = Vector.One, MaximumForce = new Vector(float.MaxValue) } + }; + + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkTwoBodyConstraint(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); + } + + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide) BallSocketServo() + { + var prestep = new BallSocketServoPrestepData + { + ServoSettings = new ServoSettingsWide { MaximumForce = new Vector(float.MaxValue), MaximumSpeed = new Vector(float.MaxValue) }, + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } + }; + + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkTwoBodyConstraint(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); + } + + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide) AngularAxisGearMotor() + { + var prestep = new AngularAxisGearMotorPrestepData + { + LocalAxisA = Vector3Wide.Broadcast(new Vector3(0, 1, 0)), + VelocityScale = Vector.One, + Settings = new() { Damping = Vector.One, MaximumForce = new Vector(float.MaxValue) } + }; + + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkTwoBodyConstraint>(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); + } + + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide) CenterDistanceLimit() + { + var prestep = new CenterDistanceLimitPrestepData + { + MinimumDistance = new Vector(0.2f), + MaximumDistance = new Vector(3), + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } + }; + + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkTwoBodyConstraint>(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); + } + +} From 2be716e930b12509fb4cdbdfa40e3f9f7a8223d9 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 21 Oct 2022 17:44:21 -0500 Subject: [PATCH 541/947] Three and four body constraints. --- DemoBenchmarks/FourBodyConstraints.cs | 64 ++++++++++++++++++++++++++ DemoBenchmarks/Program.cs | 4 +- DemoBenchmarks/ThreeBodyConstraints.cs | 59 ++++++++++++++++++++++++ 3 files changed, 126 insertions(+), 1 deletion(-) create mode 100644 DemoBenchmarks/FourBodyConstraints.cs create mode 100644 DemoBenchmarks/ThreeBodyConstraints.cs diff --git a/DemoBenchmarks/FourBodyConstraints.cs b/DemoBenchmarks/FourBodyConstraints.cs new file mode 100644 index 000000000..6ac9e9a70 --- /dev/null +++ b/DemoBenchmarks/FourBodyConstraints.cs @@ -0,0 +1,64 @@ +using BenchmarkDotNet.Attributes; +using BepuPhysics; +using BepuPhysics.Constraints; +using BepuPhysics.Constraints.Contact; +using BepuUtilities; +using System.Numerics; + +namespace DemoBenchmarks; + +/// +/// Evaluates performance of all four body constraints. +/// +/// +/// Note that all constraints operate across lanes simultaneously where T is of type . +/// The number of bundles being executed does not change if changes; if larger bundles are allowed, then more lanes end up getting solved. +/// +public class FourBodyConstraintBenchmarks +{ + static (BodyVelocityWide, BodyVelocityWide, BodyVelocityWide, BodyVelocityWide) BenchmarkFourBodyConstraint( + Vector3Wide positionA, QuaternionWide orientationA, BodyInertiaWide inertiaA, + Vector3Wide positionB, QuaternionWide orientationB, BodyInertiaWide inertiaB, + Vector3Wide positionC, QuaternionWide orientationC, BodyInertiaWide inertiaC, + Vector3Wide positionD, QuaternionWide orientationD, BodyInertiaWide inertiaD, TPrestep prestep) + where TConstraintFunctions : unmanaged, IFourBodyConstraintFunctions where TPrestep : unmanaged where TAccumulatedImpulse : unmanaged + { + var functions = default(TConstraintFunctions); + var accumulatedImpulse = default(TAccumulatedImpulse); + var velocityA = default(BodyVelocityWide); + var velocityB = default(BodyVelocityWide); + var velocityC = default(BodyVelocityWide); + var velocityD = default(BodyVelocityWide); + //Individual constraint iterations are often extremely cheap, so beef the benchmark up a bit. + const int iterations = 10000; + const float inverseDt = 60f; + const float dt = 1f / inverseDt; + for (int i = 0; i < iterations; ++i) + { + functions.WarmStart(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, positionD, orientationD, inertiaD, + ref prestep, ref accumulatedImpulse, ref velocityA, ref velocityB, ref velocityC, ref velocityD); + functions.Solve(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, positionD, orientationD, inertiaD, dt, inverseDt, + ref prestep, ref accumulatedImpulse, ref velocityA, ref velocityB, ref velocityC, ref velocityD); + } + return (velocityA, velocityB, velocityC, velocityD); + } + + //Not a lot of these yet! + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide, BodyVelocityWide, BodyVelocityWide) VolumeConstraint() + { + var prestep = new VolumeConstraintPrestepData + { + TargetScaledVolume = Vector.One, + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } + }; + + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkFourBodyConstraint>( + new Vector3Wide(), orientation, inertia, + Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, + Vector3Wide.Broadcast(new Vector3(0, 2, 0)), orientation, inertia, + Vector3Wide.Broadcast(new Vector3(0, 2, 0)), orientation, inertia, prestep); + } +} diff --git a/DemoBenchmarks/Program.cs b/DemoBenchmarks/Program.cs index f76d73d20..3012f288f 100644 --- a/DemoBenchmarks/Program.cs +++ b/DemoBenchmarks/Program.cs @@ -2,4 +2,6 @@ using DemoBenchmarks; var summaryOneBody = BenchmarkRunner.Run(typeof(OneBodyConstraintBenchmarks)); -var summaryTwoBody = BenchmarkRunner.Run(typeof(TwoBodyConstraintBenchmarks)); \ No newline at end of file +var summaryTwoBody = BenchmarkRunner.Run(typeof(TwoBodyConstraintBenchmarks)); +var summaryThreeBody = BenchmarkRunner.Run(typeof(ThreeBodyConstraintBenchmarks)); +var summaryFourBody = BenchmarkRunner.Run(typeof(FourBodyConstraintBenchmarks)); \ No newline at end of file diff --git a/DemoBenchmarks/ThreeBodyConstraints.cs b/DemoBenchmarks/ThreeBodyConstraints.cs new file mode 100644 index 000000000..326d9bea0 --- /dev/null +++ b/DemoBenchmarks/ThreeBodyConstraints.cs @@ -0,0 +1,59 @@ +using BenchmarkDotNet.Attributes; +using BepuPhysics; +using BepuPhysics.Constraints; +using BepuPhysics.Constraints.Contact; +using BepuUtilities; +using System.Numerics; + +namespace DemoBenchmarks; + +/// +/// Evaluates performance of all three body constraints. +/// +/// +/// Note that all constraints operate across lanes simultaneously where T is of type . +/// The number of bundles being executed does not change if changes; if larger bundles are allowed, then more lanes end up getting solved. +/// +public class ThreeBodyConstraintBenchmarks +{ + static (BodyVelocityWide, BodyVelocityWide, BodyVelocityWide) BenchmarkThreeBodyConstraint( + Vector3Wide positionA, QuaternionWide orientationA, BodyInertiaWide inertiaA, + Vector3Wide positionB, QuaternionWide orientationB, BodyInertiaWide inertiaB, + Vector3Wide positionC, QuaternionWide orientationC, BodyInertiaWide inertiaC, TPrestep prestep) + where TConstraintFunctions : unmanaged, IThreeBodyConstraintFunctions where TPrestep : unmanaged where TAccumulatedImpulse : unmanaged + { + var functions = default(TConstraintFunctions); + var accumulatedImpulse = default(TAccumulatedImpulse); + var velocityA = default(BodyVelocityWide); + var velocityB = default(BodyVelocityWide); + var velocityC = default(BodyVelocityWide); + //Individual constraint iterations are often extremely cheap, so beef the benchmark up a bit. + const int iterations = 10000; + const float inverseDt = 60f; + const float dt = 1f / inverseDt; + for (int i = 0; i < iterations; ++i) + { + functions.WarmStart(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, ref prestep, ref accumulatedImpulse, ref velocityA, ref velocityB, ref velocityC); + functions.Solve(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, dt, inverseDt, ref prestep, ref accumulatedImpulse, ref velocityA, ref velocityB, ref velocityC); + } + return (velocityA, velocityB, velocityC); + } + + //Not a lot of these yet! + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide, BodyVelocityWide) AreaConstraint() + { + var prestep = new AreaConstraintPrestepData + { + TargetScaledArea = Vector.One, + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } + }; + + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkThreeBodyConstraint>( + new Vector3Wide(), orientation, inertia, + Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, + Vector3Wide.Broadcast(new Vector3(0, 2, 0)), orientation, inertia, prestep); + } +} From 1651416c5ceed410834f3efd05ebe8d57ea26e48 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 22 Oct 2022 17:30:37 -0500 Subject: [PATCH 542/947] Convex collision testers. --- DemoBenchmarks/ConvexCollisionTesters.cs | 286 +++++++++++++++++++++++ DemoBenchmarks/DemoBenchmarks.csproj | 1 + DemoBenchmarks/Program.cs | 4 +- 3 files changed, 290 insertions(+), 1 deletion(-) create mode 100644 DemoBenchmarks/ConvexCollisionTesters.cs diff --git a/DemoBenchmarks/ConvexCollisionTesters.cs b/DemoBenchmarks/ConvexCollisionTesters.cs new file mode 100644 index 000000000..7afb47445 --- /dev/null +++ b/DemoBenchmarks/ConvexCollisionTesters.cs @@ -0,0 +1,286 @@ +using BenchmarkDotNet.Attributes; +using BepuPhysics; +using BepuPhysics.Collidables; +using BepuPhysics.CollisionDetection; +using BepuPhysics.CollisionDetection.CollisionTasks; +using BepuPhysics.Constraints; +using BepuPhysics.Constraints.Contact; +using BepuUtilities; +using BepuUtilities.Memory; +using System.Numerics; + +namespace DemoBenchmarks; + +/// +/// Evaluates performance of all multilane convex collision testers. +/// +/// +/// Note that all of these collision testers operate across lanes simultaneously where T is of type . +/// The number of bundles being executed does not change if changes; if larger bundles are allowed, then more lanes end up getting solved. +/// +public class ConvexCollisionTesters +{ + static (BodyVelocityWide, BodyVelocityWide, BodyVelocityWide, BodyVelocityWide) BenchmarkFourBodyConstraint( + Vector3Wide positionA, QuaternionWide orientationA, BodyInertiaWide inertiaA, + Vector3Wide positionB, QuaternionWide orientationB, BodyInertiaWide inertiaB, + Vector3Wide positionC, QuaternionWide orientationC, BodyInertiaWide inertiaC, + Vector3Wide positionD, QuaternionWide orientationD, BodyInertiaWide inertiaD, TPrestep prestep) + where TConstraintFunctions : unmanaged, IFourBodyConstraintFunctions where TPrestep : unmanaged where TAccumulatedImpulse : unmanaged + { + var functions = default(TConstraintFunctions); + var accumulatedImpulse = default(TAccumulatedImpulse); + var velocityA = default(BodyVelocityWide); + var velocityB = default(BodyVelocityWide); + var velocityC = default(BodyVelocityWide); + var velocityD = default(BodyVelocityWide); + //Individual constraint iterations are often extremely cheap, so beef the benchmark up a bit. + const int iterations = 10000; + const float inverseDt = 60f; + const float dt = 1f / inverseDt; + for (int i = 0; i < iterations; ++i) + { + functions.WarmStart(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, positionD, orientationD, inertiaD, + ref prestep, ref accumulatedImpulse, ref velocityA, ref velocityB, ref velocityC, ref velocityD); + functions.Solve(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, positionD, orientationD, inertiaD, dt, inverseDt, + ref prestep, ref accumulatedImpulse, ref velocityA, ref velocityB, ref velocityC, ref velocityD); + } + return (velocityA, velocityB, velocityC, velocityD); + } + + const int iterationCount = 1000; + BufferPool pool; + Buffer offsetsB; + Buffer> speculativeMargins; + Buffer orientationsA; + Buffer orientationsB; + + public static RigidPose CreateRandomPose(Random random, BoundingBox positionBounds) + { + RigidPose pose; + var span = positionBounds.Max - positionBounds.Min; + pose.Position = positionBounds.Min + span * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); + var axis = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); + var length = axis.Length(); + if (length > 0) + axis /= length; + else + axis = new Vector3(0, 1, 0); + pose.Orientation = QuaternionEx.CreateFromAxisAngle(axis, 1203f * random.NextSingle()); + return pose; + } + + [GlobalSetup] + public unsafe void Setup() + { + pool = new BufferPool(); + pool.Take(iterationCount, out offsetsB); + pool.Take(iterationCount, out speculativeMargins); + pool.Take(iterationCount, out orientationsA); + pool.Take(iterationCount, out orientationsB); + + //Fill random values for test instances. + BoundingBox bounds = new() { Min = new Vector3(0, 0, 0), Max = new Vector3(2, 2, 2) }; + Random random = new(5); + Span bundleMargins = stackalloc float[Vector.Count]; + for (int i = 0; i < iterationCount; ++i) + { + Vector3Wide offsetB = default; + QuaternionWide orientationA = default; + QuaternionWide orientationB = default; + for (int j = 0; j < Vector.Count; ++j) + { + var poseA = CreateRandomPose(random, bounds); + var poseB = CreateRandomPose(random, bounds); + Vector3Wide.WriteSlot(poseB.Position - poseA.Position, j, ref offsetB); + QuaternionWide.WriteSlot(poseA.Orientation, j, ref orientationA); + QuaternionWide.WriteSlot(poseB.Orientation, j, ref orientationB); + bundleMargins[j] = random.NextSingle() * 2; + + } + offsetsB[i] = offsetB; + orientationsA[i] = orientationA; + orientationsB[i] = orientationB; + speculativeMargins[i] = new Vector(bundleMargins); + } + + //Convex hulls are a little heavier to set up than the other shapes, so precreate one. + const int pointCount = 50; + pool.Take(pointCount, out var points); + for (int i = 0; i < pointCount; ++i) + { + points[i] = new Vector3(3 * random.NextSingle(), 1 * random.NextSingle(), 3 * random.NextSingle()); + } + var narrowHull = new ConvexHull(points, pool, out _); + pool.Take(Vector.Count, out var hullBundle); + var hull = default(ConvexHullWide); + hull.Initialize(hullBundle.As()); + hull.Broadcast(narrowHull); + Hull = hull; + } + + public void Cleanup() + { + //All outstanding allocations poof when the pool is cleared. + pool.Clear(); + } + + SphereWide Sphere => new() { Radius = new Vector(1f) }; + CapsuleWide Capsule => new() { Radius = new Vector(0.5f), HalfLength = new Vector(1f) }; + BoxWide Box => new() { HalfWidth = new Vector(1f), HalfHeight = new Vector(1f), HalfLength = new Vector(1f) }; + TriangleWide Triangle => new() { A = Vector3Wide.Broadcast(new Vector3(-1f / 3f, 0, -1f / 3f)), B = Vector3Wide.Broadcast(new Vector3(2 / 3f, 0, -1f / 3f)), C = Vector3Wide.Broadcast(new Vector3(-1f / 3f, 0, 2 / 3f)), }; + CylinderWide Cylinder => new() { Radius = new Vector(1f), HalfLength = new Vector(1f) }; + ConvexHullWide Hull { get; set; } + + + Vector TestOrientationless1Contact(TShapeA a, TShapeB b) where TTester : unmanaged, IPairTester + { + var tester = default(TTester); + Vector testSum = Vector.Zero; + for (int i = 0; i < iterationCount; ++i) + { + tester.Test(ref a, ref b, ref speculativeMargins[i], ref offsetsB[i], Vector.Count, out var manifold); + testSum += manifold.Depth + manifold.Normal.X + manifold.Normal.Y + manifold.Normal.Z + manifold.OffsetA.X + manifold.OffsetA.Y + manifold.OffsetA.Z; + } + return testSum; + } + Vector TestOrientationB1Contact(TShapeA a, TShapeB b) where TTester : unmanaged, IPairTester + { + var tester = default(TTester); + Vector testSum = Vector.Zero; + for (int i = 0; i < iterationCount; ++i) + { + tester.Test(ref a, ref b, ref speculativeMargins[i], ref offsetsB[i], ref orientationsB[i], Vector.Count, out var manifold); + testSum += manifold.Depth + manifold.Normal.X + manifold.Normal.Y + manifold.Normal.Z + manifold.OffsetA.X + manifold.OffsetA.Y + manifold.OffsetA.Z; + } + return testSum; + } + Vector Test2Contact(TShapeA a, TShapeB b) where TTester : unmanaged, IPairTester + { + var tester = default(TTester); + Vector testSum = Vector.Zero; + for (int i = 0; i < iterationCount; ++i) + { + tester.Test(ref a, ref b, ref speculativeMargins[i], ref offsetsB[i], ref orientationsA[i], ref orientationsB[i], Vector.Count, out var manifold); + testSum += manifold.Normal.X + manifold.Normal.Y + manifold.Normal.Z + + manifold.OffsetA0.X + manifold.OffsetA0.Y + manifold.OffsetA0.Z + manifold.Depth0 + + manifold.OffsetA1.X + manifold.OffsetA1.Y + manifold.OffsetA1.Z + manifold.Depth1; + } + return testSum; + } + Vector Test4Contact(TShapeA a, TShapeB b) where TTester : unmanaged, IPairTester + { + var tester = default(TTester); + Vector testSum = Vector.Zero; + for (int i = 0; i < iterationCount; ++i) + { + tester.Test(ref a, ref b, ref speculativeMargins[i], ref offsetsB[i], ref orientationsA[i], ref orientationsB[i], Vector.Count, out var manifold); + testSum += manifold.Normal.X + manifold.Normal.Y + manifold.Normal.Z + + manifold.OffsetA0.X + manifold.OffsetA0.Y + manifold.OffsetA0.Z + manifold.Depth0 + + manifold.OffsetA1.X + manifold.OffsetA1.Y + manifold.OffsetA1.Z + manifold.Depth1 + + manifold.OffsetA2.X + manifold.OffsetA2.Y + manifold.OffsetA2.Z + manifold.Depth2 + + manifold.OffsetA3.X + manifold.OffsetA3.Y + manifold.OffsetA3.Z + manifold.Depth3; + } + return testSum; + } + + [Benchmark] + public Vector SpherePairTester() + { + return TestOrientationless1Contact(Sphere, Sphere); + } + [Benchmark] + public Vector SphereCapsuleTester() + { + return TestOrientationB1Contact(Sphere, Capsule); + } + [Benchmark] + public Vector SphereBoxTester() + { + return TestOrientationB1Contact(Sphere, Box); + } + [Benchmark] + public Vector SphereTriangleTester() + { + return TestOrientationB1Contact(Sphere, Triangle); + } + [Benchmark] + public Vector SphereCylinderTester() + { + return TestOrientationB1Contact(Sphere, Cylinder); + } + [Benchmark] + public Vector SphereConvexHullTester() + { + return TestOrientationB1Contact(Sphere, Hull); + } + [Benchmark] + public Vector CapsulePairTester() + { + return Test2Contact(Capsule, Capsule); + } + [Benchmark] + public Vector CapsuleBoxTester() + { + return Test2Contact(Capsule, Box); + } + [Benchmark] + public Vector CapsuleTriangleTester() + { + return Test2Contact(Capsule, Triangle); + } + [Benchmark] + public Vector CapsuleCylinderTester() + { + return Test2Contact(Capsule, Cylinder); + } + [Benchmark] + public Vector CapsuleConvexHullTester() + { + return Test2Contact(Capsule, Hull); + } + [Benchmark] + public Vector BoxPairTester() + { + return Test4Contact(Box, Box); + } + [Benchmark] + public Vector BoxTriangleTester() + { + return Test4Contact(Box, Triangle); + } + [Benchmark] + public Vector BoxCylinderTester() + { + return Test4Contact(Box, Cylinder); + } + [Benchmark] + public Vector BoxConvexHullTester() + { + return Test4Contact(Box, Hull); + } + [Benchmark] + public Vector TrianglePairTester() + { + return Test4Contact(Triangle, Triangle); + } + [Benchmark] + public Vector TriangleCylinderTester() + { + return Test4Contact(Triangle, Cylinder); + } + [Benchmark] + public Vector TriangleConvexHullTester() + { + return Test4Contact(Triangle, Hull); + } + [Benchmark] + public Vector CylinderPairTester() + { + return Test4Contact(Cylinder, Cylinder); + } + [Benchmark] + public Vector CylinderConvexHullTester() + { + return Test4Contact(Cylinder, Hull); + } +} diff --git a/DemoBenchmarks/DemoBenchmarks.csproj b/DemoBenchmarks/DemoBenchmarks.csproj index 9dd142c5c..8d329413f 100644 --- a/DemoBenchmarks/DemoBenchmarks.csproj +++ b/DemoBenchmarks/DemoBenchmarks.csproj @@ -5,6 +5,7 @@ net7.0 enable enable + true diff --git a/DemoBenchmarks/Program.cs b/DemoBenchmarks/Program.cs index 3012f288f..2478b3a7a 100644 --- a/DemoBenchmarks/Program.cs +++ b/DemoBenchmarks/Program.cs @@ -1,7 +1,9 @@ using BenchmarkDotNet.Running; using DemoBenchmarks; + var summaryOneBody = BenchmarkRunner.Run(typeof(OneBodyConstraintBenchmarks)); var summaryTwoBody = BenchmarkRunner.Run(typeof(TwoBodyConstraintBenchmarks)); var summaryThreeBody = BenchmarkRunner.Run(typeof(ThreeBodyConstraintBenchmarks)); -var summaryFourBody = BenchmarkRunner.Run(typeof(FourBodyConstraintBenchmarks)); \ No newline at end of file +var summaryFourBody = BenchmarkRunner.Run(typeof(FourBodyConstraintBenchmarks)); +var summaryConvexCollisionTesters = BenchmarkRunner.Run(typeof(ConvexCollisionTesters)); \ No newline at end of file From a17bf448c9bd32c61484fe8fa1d9f07b9594e505 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 22 Oct 2022 18:42:13 -0500 Subject: [PATCH 543/947] Oops, missed one. --- DemoBenchmarks/ConvexCollisionTesters.cs | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/DemoBenchmarks/ConvexCollisionTesters.cs b/DemoBenchmarks/ConvexCollisionTesters.cs index 7afb47445..f1bc9b50c 100644 --- a/DemoBenchmarks/ConvexCollisionTesters.cs +++ b/DemoBenchmarks/ConvexCollisionTesters.cs @@ -283,4 +283,9 @@ public Vector CylinderConvexHullTester() { return Test4Contact(Cylinder, Hull); } + [Benchmark] + public Vector ConvexHullPairTester() + { + return Test4Contact(Hull, Hull); + } } From 426c3d4a7f73a287d4ae845daa347a38d5611e49 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 22 Oct 2022 19:46:43 -0500 Subject: [PATCH 544/947] Collision batcher tasks benchmark added. --- DemoBenchmarks/BenchmarkHelper.cs | 23 +++ DemoBenchmarks/CollisionBatcherTasks.cs | 243 +++++++++++++++++++++++ DemoBenchmarks/ConvexCollisionTesters.cs | 46 +---- DemoBenchmarks/Program.cs | 4 +- 4 files changed, 269 insertions(+), 47 deletions(-) create mode 100644 DemoBenchmarks/BenchmarkHelper.cs create mode 100644 DemoBenchmarks/CollisionBatcherTasks.cs diff --git a/DemoBenchmarks/BenchmarkHelper.cs b/DemoBenchmarks/BenchmarkHelper.cs new file mode 100644 index 000000000..21b471fb3 --- /dev/null +++ b/DemoBenchmarks/BenchmarkHelper.cs @@ -0,0 +1,23 @@ +using BepuPhysics; +using BepuUtilities; +using System.Numerics; + +namespace DemoBenchmarks; + +public static class BenchmarkHelper +{ + public static RigidPose CreateRandomPose(Random random, BoundingBox positionBounds) + { + RigidPose pose; + var span = positionBounds.Max - positionBounds.Min; + pose.Position = positionBounds.Min + span * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); + var axis = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); + var length = axis.Length(); + if (length > 0) + axis /= length; + else + axis = new Vector3(0, 1, 0); + pose.Orientation = QuaternionEx.CreateFromAxisAngle(axis, 1203f * random.NextSingle()); + return pose; + } +} diff --git a/DemoBenchmarks/CollisionBatcherTasks.cs b/DemoBenchmarks/CollisionBatcherTasks.cs new file mode 100644 index 000000000..71989a287 --- /dev/null +++ b/DemoBenchmarks/CollisionBatcherTasks.cs @@ -0,0 +1,243 @@ +using BenchmarkDotNet.Attributes; +using BepuPhysics; +using BepuPhysics.Collidables; +using BepuPhysics.CollisionDetection; +using BepuPhysics.CollisionDetection.CollisionTasks; +using BepuUtilities; +using BepuUtilities.Memory; +using System.Diagnostics; +using System.Numerics; +using static DemoBenchmarks.BenchmarkHelper; + +namespace DemoBenchmarks; + +/// +/// Evaluates performance of all collision tasks together in a collision batcher. +/// +public class CollisionBatcherTasks +{ + const int pairCount = 100000; + BufferPool pool; + struct Pair + { + public Vector3 OffsetB; + public Quaternion OrientationA; + public Quaternion OrientationB; + public BodyVelocity VelocityA; + public BodyVelocity VelocityB; + public float SpeculativeMargin; + public TypedIndex A; + public TypedIndex B; + } + Buffer pairs; + CollisionTaskRegistry taskRegistry; + Shapes shapes; + + + public static void CreateDeformedPlane(int width, int height, Func deformer, Vector3 scaling, BufferPool pool, out Mesh mesh) + { + pool.Take(width * height, out var vertices); + for (int i = 0; i < width; ++i) + { + for (int j = 0; j < height; ++j) + { + vertices[width * j + i] = deformer(i, j); + } + } + + var quadWidth = width - 1; + var quadHeight = height - 1; + var triangleCount = quadWidth * quadHeight * 2; + pool.Take(triangleCount, out var triangles); + + for (int i = 0; i < quadWidth; ++i) + { + for (int j = 0; j < quadHeight; ++j) + { + var triangleIndex = (j * quadWidth + i) * 2; + ref var triangle0 = ref triangles[triangleIndex]; + ref var v00 = ref vertices[width * j + i]; + ref var v01 = ref vertices[width * j + i + 1]; + ref var v10 = ref vertices[width * (j + 1) + i]; + ref var v11 = ref vertices[width * (j + 1) + i + 1]; + triangle0.A = v00; + triangle0.B = v01; + triangle0.C = v10; + ref var triangle1 = ref triangles[triangleIndex + 1]; + triangle1.A = v01; + triangle1.B = v11; + triangle1.C = v10; + } + } + pool.Return(ref vertices); + mesh = new Mesh(triangles, scaling, pool); + } + + [GlobalSetup] + public unsafe void Setup() + { + pool = new BufferPool(); + pool.Take(pairCount, out pairs); + taskRegistry = DefaultTypes.CreateDefaultCollisionTaskRegistry(); + shapes = new Shapes(pool, 1); + + var sphere = shapes.Add(new Sphere(1)); + var capsule = shapes.Add(new Capsule(0.5f, 1)); + var box = shapes.Add(new Box(2, 2, 2)); + var triangle = shapes.Add(new Triangle(new Vector3(0, 0, 0), new Vector3(1, 0, 0), new Vector3(0, 0, 1))); + var cylinder = shapes.Add(new Cylinder(0.5f, 1)); + + Random random = new(5); + const int pointCount = 50; + pool.Take(pointCount, out var points); + for (int i = 0; i < pointCount; ++i) + { + points[i] = new Vector3(3 * random.NextSingle(), 1 * random.NextSingle(), 3 * random.NextSingle()); + } + var hullShape = new ConvexHull(points, pool, out _); + var hull = shapes.Add(hullShape); + + CompoundBuilder builder = new(pool, shapes, 64); + BoundingBox compoundBounds = new() { Min = new Vector3(0, 0, 0), Max = new Vector3(4, 4, 4) }; + builder.AddForKinematic(sphere, CreateRandomPose(random, compoundBounds), 1); + builder.AddForKinematic(capsule, CreateRandomPose(random, compoundBounds), 1); + builder.AddForKinematic(box, CreateRandomPose(random, compoundBounds), 1); + builder.AddForKinematic(triangle, CreateRandomPose(random, compoundBounds), 1); + builder.AddForKinematic(cylinder, CreateRandomPose(random, compoundBounds), 1); + builder.AddForKinematic(hull, CreateRandomPose(random, compoundBounds), 1); + builder.BuildKinematicCompound(out var children, out _); + var compound = shapes.Add(new Compound(children)); + builder.Reset(); + + Span shapeIndices = stackalloc TypedIndex[9]; + shapeIndices[0] = sphere; + shapeIndices[1] = capsule; + shapeIndices[2] = box; + shapeIndices[3] = triangle; + shapeIndices[4] = cylinder; + shapeIndices[5] = hull; + + BoundingBox bigCompoundBounds = new() { Min = new Vector3(0, 0, 0), Max = new Vector3(16, 16, 16) }; + for (int i = 0; i < 64; ++i) + { + builder.AddForKinematic(shapeIndices[random.Next(6)], CreateRandomPose(random, bigCompoundBounds), 1); + } + builder.BuildKinematicCompound(out var bigChildren, out _); + var bigCompound = shapes.Add(new BigCompound(bigChildren, shapes, pool)); + + CreateDeformedPlane(16, 16, (x, y) => { return new Vector3(x * 2 - 8, 3 * MathF.Sin(x) * MathF.Sin(y), y * 2 - 8); }, Vector3.One, pool, out var meshShape); + var mesh = shapes.Add(meshShape); + + shapeIndices[6] = compound; + shapeIndices[7] = bigCompound; + shapeIndices[8] = mesh; + + Span shapeRelativeProbabilities = stackalloc float[9]; + shapeRelativeProbabilities[0] = 1; + shapeRelativeProbabilities[1] = 1; + shapeRelativeProbabilities[2] = 1; + shapeRelativeProbabilities[3] = 1; + shapeRelativeProbabilities[4] = 1; + shapeRelativeProbabilities[5] = 1; + + shapeRelativeProbabilities[6] = 0.2f; + shapeRelativeProbabilities[7] = 0.2f; + shapeRelativeProbabilities[7] = 0.2f; + + var sum = 0f; + Span cumulative = stackalloc float[9]; + for (int i = 0; i < shapeRelativeProbabilities.Length; ++i) + { + cumulative[i] = sum; + sum += shapeRelativeProbabilities[i]; + } + var inverseSum = 1f / sum; + for (int i = 0; i < shapeRelativeProbabilities.Length; ++i) + cumulative[i] *= inverseSum; + + TypedIndex GetRandomShapeTypeIndex(Span cumulative) + { + var r = random.NextSingle(); + for (int i = 0; i < cumulative.Length; ++i) + { + if (r < cumulative[i]) + return new TypedIndex(i, 0); //there's only one shape per type, sooo. + } + Debug.Fail("hey whatnow"); + return default; + } + + //Fill random values for pair tests. + BoundingBox bounds = new() { Min = new Vector3(0, 0, 0), Max = new Vector3(2, 2, 2) }; + for (int i = 0; i < pairCount; ++i) + { + var poseA = CreateRandomPose(random, bounds); + var poseB = CreateRandomPose(random, bounds); + ref var pair = ref pairs[i]; + pair = new Pair + { + OffsetB = poseB.Position - poseA.Position, + OrientationA = poseA.Orientation, + OrientationB = poseB.Orientation, + VelocityA = new BodyVelocity + { + Linear = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) * 2 - Vector3.One, + Angular = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) * 2 - Vector3.One + }, + VelocityB = new BodyVelocity + { + Linear = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) * 2 - Vector3.One, + Angular = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) * 2 - Vector3.One + }, + SpeculativeMargin = random.NextSingle(), + A = GetRandomShapeTypeIndex(cumulative), + B = GetRandomShapeTypeIndex(cumulative) + }; + } + } + + public void Cleanup() + { + //All outstanding allocations poof when the pool is cleared. + pool.Clear(); + } + + + struct Callbacks : ICollisionCallbacks + { + public Vector3 ResultSum; + public bool AllowCollisionTesting(int pairId, int childA, int childB) + { + return true; + } + + public void OnChildPairCompleted(int pairId, int childA, int childB, ref ConvexContactManifold manifold) + { + } + + public void OnPairCompleted(int pairId, ref TManifold manifold) where TManifold : unmanaged, IContactManifold + { + var count = manifold.Count; + for (int i = 0; i < count; ++i) + { + ResultSum += manifold.GetNormal(ref manifold, i) + manifold.GetOffset(ref manifold, i) + new Vector3(manifold.GetDepth(ref manifold, i)); + } + } + } + + [Benchmark] + public unsafe Vector3 Benchmark() + { + CollisionBatcher batcher = new(pool, shapes, taskRegistry, 1 / 60f, new Callbacks()); + + for (int i = 0; i < pairCount; ++i) + { + ref var pair = ref pairs[i]; + shapes[pair.A.Type].GetShapeData(pair.A.Index, out var aData, out _); + shapes[pair.B.Type].GetShapeData(pair.B.Index, out var bData, out _); + batcher.AddDirectly(pair.A.Type, pair.B.Type, aData, bData, pair.OffsetB, pair.OrientationA, pair.OrientationB, pair.VelocityA, pair.VelocityB, pair.SpeculativeMargin, float.MaxValue, new PairContinuation(i)); + } + batcher.Flush(); + return batcher.Callbacks.ResultSum; + } +} diff --git a/DemoBenchmarks/ConvexCollisionTesters.cs b/DemoBenchmarks/ConvexCollisionTesters.cs index f1bc9b50c..f828c16ca 100644 --- a/DemoBenchmarks/ConvexCollisionTesters.cs +++ b/DemoBenchmarks/ConvexCollisionTesters.cs @@ -1,13 +1,11 @@ using BenchmarkDotNet.Attributes; -using BepuPhysics; using BepuPhysics.Collidables; using BepuPhysics.CollisionDetection; using BepuPhysics.CollisionDetection.CollisionTasks; -using BepuPhysics.Constraints; -using BepuPhysics.Constraints.Contact; using BepuUtilities; using BepuUtilities.Memory; using System.Numerics; +using static DemoBenchmarks.BenchmarkHelper; namespace DemoBenchmarks; @@ -20,33 +18,6 @@ namespace DemoBenchmarks; /// public class ConvexCollisionTesters { - static (BodyVelocityWide, BodyVelocityWide, BodyVelocityWide, BodyVelocityWide) BenchmarkFourBodyConstraint( - Vector3Wide positionA, QuaternionWide orientationA, BodyInertiaWide inertiaA, - Vector3Wide positionB, QuaternionWide orientationB, BodyInertiaWide inertiaB, - Vector3Wide positionC, QuaternionWide orientationC, BodyInertiaWide inertiaC, - Vector3Wide positionD, QuaternionWide orientationD, BodyInertiaWide inertiaD, TPrestep prestep) - where TConstraintFunctions : unmanaged, IFourBodyConstraintFunctions where TPrestep : unmanaged where TAccumulatedImpulse : unmanaged - { - var functions = default(TConstraintFunctions); - var accumulatedImpulse = default(TAccumulatedImpulse); - var velocityA = default(BodyVelocityWide); - var velocityB = default(BodyVelocityWide); - var velocityC = default(BodyVelocityWide); - var velocityD = default(BodyVelocityWide); - //Individual constraint iterations are often extremely cheap, so beef the benchmark up a bit. - const int iterations = 10000; - const float inverseDt = 60f; - const float dt = 1f / inverseDt; - for (int i = 0; i < iterations; ++i) - { - functions.WarmStart(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, positionD, orientationD, inertiaD, - ref prestep, ref accumulatedImpulse, ref velocityA, ref velocityB, ref velocityC, ref velocityD); - functions.Solve(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, positionD, orientationD, inertiaD, dt, inverseDt, - ref prestep, ref accumulatedImpulse, ref velocityA, ref velocityB, ref velocityC, ref velocityD); - } - return (velocityA, velocityB, velocityC, velocityD); - } - const int iterationCount = 1000; BufferPool pool; Buffer offsetsB; @@ -54,21 +25,6 @@ public class ConvexCollisionTesters Buffer orientationsA; Buffer orientationsB; - public static RigidPose CreateRandomPose(Random random, BoundingBox positionBounds) - { - RigidPose pose; - var span = positionBounds.Max - positionBounds.Min; - pose.Position = positionBounds.Min + span * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); - var axis = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); - var length = axis.Length(); - if (length > 0) - axis /= length; - else - axis = new Vector3(0, 1, 0); - pose.Orientation = QuaternionEx.CreateFromAxisAngle(axis, 1203f * random.NextSingle()); - return pose; - } - [GlobalSetup] public unsafe void Setup() { diff --git a/DemoBenchmarks/Program.cs b/DemoBenchmarks/Program.cs index 2478b3a7a..72d033649 100644 --- a/DemoBenchmarks/Program.cs +++ b/DemoBenchmarks/Program.cs @@ -1,9 +1,9 @@ using BenchmarkDotNet.Running; using DemoBenchmarks; - var summaryOneBody = BenchmarkRunner.Run(typeof(OneBodyConstraintBenchmarks)); var summaryTwoBody = BenchmarkRunner.Run(typeof(TwoBodyConstraintBenchmarks)); var summaryThreeBody = BenchmarkRunner.Run(typeof(ThreeBodyConstraintBenchmarks)); var summaryFourBody = BenchmarkRunner.Run(typeof(FourBodyConstraintBenchmarks)); -var summaryConvexCollisionTesters = BenchmarkRunner.Run(typeof(ConvexCollisionTesters)); \ No newline at end of file +var summaryConvexCollisionTesters = BenchmarkRunner.Run(typeof(ConvexCollisionTesters)); +var summaryCollisionBatcherTasks = BenchmarkRunner.Run(typeof(CollisionBatcherTasks)); From 4f4be5d4b0eb216bdb22d11d76aafbdd1bde2c31 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 22 Oct 2022 21:01:37 -0500 Subject: [PATCH 545/947] Added sweeps. --- DemoBenchmarks/BenchmarkHelper.cs | 83 ++++++++ DemoBenchmarks/CollisionBatcherTasks.cs | 51 +---- DemoBenchmarks/Program.cs | 13 +- DemoBenchmarks/Sweeps.cs | 239 ++++++++++++++++++++++++ 4 files changed, 331 insertions(+), 55 deletions(-) create mode 100644 DemoBenchmarks/Sweeps.cs diff --git a/DemoBenchmarks/BenchmarkHelper.cs b/DemoBenchmarks/BenchmarkHelper.cs index 21b471fb3..b8fc30ddb 100644 --- a/DemoBenchmarks/BenchmarkHelper.cs +++ b/DemoBenchmarks/BenchmarkHelper.cs @@ -1,5 +1,7 @@ using BepuPhysics; +using BepuPhysics.Collidables; using BepuUtilities; +using BepuUtilities.Memory; using System.Numerics; namespace DemoBenchmarks; @@ -20,4 +22,85 @@ public static RigidPose CreateRandomPose(Random random, BoundingBox positionBoun pose.Orientation = QuaternionEx.CreateFromAxisAngle(axis, 1203f * random.NextSingle()); return pose; } + + public static void CreateDeformedPlane(int width, int height, Func deformer, Vector3 scaling, BufferPool pool, out Mesh mesh) + { + pool.Take(width * height, out var vertices); + for (int i = 0; i < width; ++i) + { + for (int j = 0; j < height; ++j) + { + vertices[width * j + i] = deformer(i, j); + } + } + + var quadWidth = width - 1; + var quadHeight = height - 1; + var triangleCount = quadWidth * quadHeight * 2; + pool.Take(triangleCount, out var triangles); + + for (int i = 0; i < quadWidth; ++i) + { + for (int j = 0; j < quadHeight; ++j) + { + var triangleIndex = (j * quadWidth + i) * 2; + ref var triangle0 = ref triangles[triangleIndex]; + ref var v00 = ref vertices[width * j + i]; + ref var v01 = ref vertices[width * j + i + 1]; + ref var v10 = ref vertices[width * (j + 1) + i]; + ref var v11 = ref vertices[width * (j + 1) + i + 1]; + triangle0.A = v00; + triangle0.B = v01; + triangle0.C = v10; + ref var triangle1 = ref triangles[triangleIndex + 1]; + triangle1.A = v01; + triangle1.B = v11; + triangle1.C = v10; + } + } + pool.Return(ref vertices); + mesh = new Mesh(triangles, scaling, pool); + } + + public static void CreateShapes(Random random, BufferPool pool, Shapes shapes) + { + var sphere = shapes.Add(new Sphere(1)); + var capsule = shapes.Add(new Capsule(0.5f, 1)); + var box = shapes.Add(new Box(2, 2, 2)); + var triangle = shapes.Add(new Triangle(new Vector3(0, 0, 0), new Vector3(1, 0, 0), new Vector3(0, 0, 1))); + var cylinder = shapes.Add(new Cylinder(0.5f, 1)); + + const int hullPointCount = 50; + pool.Take(hullPointCount, out var points); + for (int i = 0; i < hullPointCount; ++i) + { + points[i] = new Vector3(3 * random.NextSingle(), 1 * random.NextSingle(), 3 * random.NextSingle()); + } + var hullShape = new ConvexHull(points, pool, out _); + var hull = shapes.Add(hullShape); + + CompoundBuilder builder = new(pool, shapes, 64); + BoundingBox compoundBounds = new() { Min = new Vector3(0, 0, 0), Max = new Vector3(4, 4, 4) }; + builder.AddForKinematic(sphere, CreateRandomPose(random, compoundBounds), 1); + builder.AddForKinematic(capsule, CreateRandomPose(random, compoundBounds), 1); + builder.AddForKinematic(box, CreateRandomPose(random, compoundBounds), 1); + builder.AddForKinematic(triangle, CreateRandomPose(random, compoundBounds), 1); + builder.AddForKinematic(cylinder, CreateRandomPose(random, compoundBounds), 1); + builder.AddForKinematic(hull, CreateRandomPose(random, compoundBounds), 1); + builder.BuildKinematicCompound(out var children, out _); + var compound = shapes.Add(new Compound(children)); + builder.Reset(); + + BoundingBox bigCompoundBounds = new() { Min = new Vector3(0, 0, 0), Max = new Vector3(16, 16, 16) }; + for (int i = 0; i < 64; ++i) + { + builder.AddForKinematic(new TypedIndex(random.Next(6), 0), CreateRandomPose(random, bigCompoundBounds), 1); + } + builder.BuildKinematicCompound(out var bigChildren, out _); + var bigCompound = shapes.Add(new BigCompound(bigChildren, shapes, pool)); + + CreateDeformedPlane(16, 16, (x, y) => { return new Vector3(x * 2 - 8, 3 * MathF.Sin(x) * MathF.Sin(y), y * 2 - 8); }, Vector3.One, pool, out var meshShape); + var mesh = shapes.Add(meshShape); + } + } diff --git a/DemoBenchmarks/CollisionBatcherTasks.cs b/DemoBenchmarks/CollisionBatcherTasks.cs index 71989a287..f05d46cd1 100644 --- a/DemoBenchmarks/CollisionBatcherTasks.cs +++ b/DemoBenchmarks/CollisionBatcherTasks.cs @@ -81,56 +81,8 @@ public unsafe void Setup() taskRegistry = DefaultTypes.CreateDefaultCollisionTaskRegistry(); shapes = new Shapes(pool, 1); - var sphere = shapes.Add(new Sphere(1)); - var capsule = shapes.Add(new Capsule(0.5f, 1)); - var box = shapes.Add(new Box(2, 2, 2)); - var triangle = shapes.Add(new Triangle(new Vector3(0, 0, 0), new Vector3(1, 0, 0), new Vector3(0, 0, 1))); - var cylinder = shapes.Add(new Cylinder(0.5f, 1)); - Random random = new(5); - const int pointCount = 50; - pool.Take(pointCount, out var points); - for (int i = 0; i < pointCount; ++i) - { - points[i] = new Vector3(3 * random.NextSingle(), 1 * random.NextSingle(), 3 * random.NextSingle()); - } - var hullShape = new ConvexHull(points, pool, out _); - var hull = shapes.Add(hullShape); - - CompoundBuilder builder = new(pool, shapes, 64); - BoundingBox compoundBounds = new() { Min = new Vector3(0, 0, 0), Max = new Vector3(4, 4, 4) }; - builder.AddForKinematic(sphere, CreateRandomPose(random, compoundBounds), 1); - builder.AddForKinematic(capsule, CreateRandomPose(random, compoundBounds), 1); - builder.AddForKinematic(box, CreateRandomPose(random, compoundBounds), 1); - builder.AddForKinematic(triangle, CreateRandomPose(random, compoundBounds), 1); - builder.AddForKinematic(cylinder, CreateRandomPose(random, compoundBounds), 1); - builder.AddForKinematic(hull, CreateRandomPose(random, compoundBounds), 1); - builder.BuildKinematicCompound(out var children, out _); - var compound = shapes.Add(new Compound(children)); - builder.Reset(); - - Span shapeIndices = stackalloc TypedIndex[9]; - shapeIndices[0] = sphere; - shapeIndices[1] = capsule; - shapeIndices[2] = box; - shapeIndices[3] = triangle; - shapeIndices[4] = cylinder; - shapeIndices[5] = hull; - - BoundingBox bigCompoundBounds = new() { Min = new Vector3(0, 0, 0), Max = new Vector3(16, 16, 16) }; - for (int i = 0; i < 64; ++i) - { - builder.AddForKinematic(shapeIndices[random.Next(6)], CreateRandomPose(random, bigCompoundBounds), 1); - } - builder.BuildKinematicCompound(out var bigChildren, out _); - var bigCompound = shapes.Add(new BigCompound(bigChildren, shapes, pool)); - - CreateDeformedPlane(16, 16, (x, y) => { return new Vector3(x * 2 - 8, 3 * MathF.Sin(x) * MathF.Sin(y), y * 2 - 8); }, Vector3.One, pool, out var meshShape); - var mesh = shapes.Add(meshShape); - - shapeIndices[6] = compound; - shapeIndices[7] = bigCompound; - shapeIndices[8] = mesh; + CreateShapes(random, pool, shapes); Span shapeRelativeProbabilities = stackalloc float[9]; shapeRelativeProbabilities[0] = 1; @@ -196,6 +148,7 @@ TypedIndex GetRandomShapeTypeIndex(Span cumulative) } } + [GlobalCleanup] public void Cleanup() { //All outstanding allocations poof when the pool is cleared. diff --git a/DemoBenchmarks/Program.cs b/DemoBenchmarks/Program.cs index 72d033649..52378c459 100644 --- a/DemoBenchmarks/Program.cs +++ b/DemoBenchmarks/Program.cs @@ -1,9 +1,10 @@ using BenchmarkDotNet.Running; using DemoBenchmarks; -var summaryOneBody = BenchmarkRunner.Run(typeof(OneBodyConstraintBenchmarks)); -var summaryTwoBody = BenchmarkRunner.Run(typeof(TwoBodyConstraintBenchmarks)); -var summaryThreeBody = BenchmarkRunner.Run(typeof(ThreeBodyConstraintBenchmarks)); -var summaryFourBody = BenchmarkRunner.Run(typeof(FourBodyConstraintBenchmarks)); -var summaryConvexCollisionTesters = BenchmarkRunner.Run(typeof(ConvexCollisionTesters)); -var summaryCollisionBatcherTasks = BenchmarkRunner.Run(typeof(CollisionBatcherTasks)); +BenchmarkRunner.Run(typeof(OneBodyConstraintBenchmarks)); +BenchmarkRunner.Run(typeof(TwoBodyConstraintBenchmarks)); +BenchmarkRunner.Run(typeof(ThreeBodyConstraintBenchmarks)); +BenchmarkRunner.Run(typeof(FourBodyConstraintBenchmarks)); +BenchmarkRunner.Run(typeof(ConvexCollisionTesters)); +BenchmarkRunner.Run(typeof(CollisionBatcherTasks)); +BenchmarkRunner.Run(typeof(Sweeps)); diff --git a/DemoBenchmarks/Sweeps.cs b/DemoBenchmarks/Sweeps.cs new file mode 100644 index 000000000..d3d846027 --- /dev/null +++ b/DemoBenchmarks/Sweeps.cs @@ -0,0 +1,239 @@ +using BenchmarkDotNet.Attributes; +using BepuPhysics; +using BepuPhysics.Collidables; +using BepuPhysics.CollisionDetection; +using BepuPhysics.CollisionDetection.CollisionTasks; +using BepuUtilities; +using BepuUtilities.Memory; +using System.Diagnostics; +using System.Numerics; +using System.Runtime.CompilerServices; +using static DemoBenchmarks.BenchmarkHelper; + +namespace DemoBenchmarks; + +/// +/// Evaluates performance of all sweep tests. +/// +public class Sweeps +{ + const int iterationCount = 100; + BufferPool pool; + struct Pair + { + public Vector3 OffsetB; + public Quaternion OrientationA; + public Quaternion OrientationB; + public BodyVelocity VelocityA; + public BodyVelocity VelocityB; + public float SpeculativeMargin; + public TypedIndex A; + public TypedIndex B; + } + Buffer pairs; + SweepTaskRegistry taskRegistry; + Shapes shapes; + + + [GlobalSetup] + public unsafe void Setup() + { + pool = new BufferPool(); + pool.Take(iterationCount, out pairs); + taskRegistry = DefaultTypes.CreateDefaultSweepTaskRegistry(); + shapes = new Shapes(pool, 1); + Random random = new(5); + CreateShapes(random, pool, shapes); + + Span shapeRelativeProbabilities = stackalloc float[9]; + shapeRelativeProbabilities[0] = 1; + shapeRelativeProbabilities[1] = 1; + shapeRelativeProbabilities[2] = 1; + shapeRelativeProbabilities[3] = 1; + shapeRelativeProbabilities[4] = 1; + shapeRelativeProbabilities[5] = 1; + + shapeRelativeProbabilities[6] = 0.2f; + shapeRelativeProbabilities[7] = 0.2f; + shapeRelativeProbabilities[7] = 0.2f; + + var sum = 0f; + Span cumulative = stackalloc float[9]; + for (int i = 0; i < shapeRelativeProbabilities.Length; ++i) + { + cumulative[i] = sum; + sum += shapeRelativeProbabilities[i]; + } + var inverseSum = 1f / sum; + for (int i = 0; i < shapeRelativeProbabilities.Length; ++i) + cumulative[i] *= inverseSum; + + TypedIndex GetRandomShapeTypeIndex(Span cumulative) + { + var r = random.NextSingle(); + for (int i = 0; i < cumulative.Length; ++i) + { + if (r < cumulative[i]) + return new TypedIndex(i, 0); //there's only one shape per type, sooo. + } + Debug.Fail("hey whatnow"); + return default; + } + + //Fill random values for pair tests. + BoundingBox bounds = new() { Min = new Vector3(0, 0, 0), Max = new Vector3(2, 2, 2) }; + for (int i = 0; i < iterationCount; ++i) + { + var poseA = CreateRandomPose(random, bounds); + var poseB = CreateRandomPose(random, bounds); + ref var pair = ref pairs[i]; + pair = new Pair + { + OffsetB = poseB.Position - poseA.Position, + OrientationA = poseA.Orientation, + OrientationB = poseB.Orientation, + VelocityA = new BodyVelocity + { + Linear = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) * 2 - Vector3.One, + Angular = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) * 2 - Vector3.One + }, + VelocityB = new BodyVelocity + { + Linear = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) * 2 - Vector3.One, + Angular = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) * 2 - Vector3.One + }, + SpeculativeMargin = random.NextSingle(), + A = GetRandomShapeTypeIndex(cumulative), + B = GetRandomShapeTypeIndex(cumulative) + }; + } + } + + [GlobalCleanup] + public void Cleanup() + { + //All outstanding allocations poof when the pool is cleared. + pool.Clear(); + } + + struct Filter : ISweepFilter + { + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool AllowTest(int childA, int childB) + { + return true; + } + } + + unsafe Vector3 Test() where TA : unmanaged, IShape where TB : unmanaged, IShape + { + var task = taskRegistry.GetTask(); + var aType = default(TA).TypeId; + var bType = default(TB).TypeId; + shapes[aType].GetShapeData(0, out var aData, out _); + shapes[bType].GetShapeData(0, out var bData, out _); + var filter = default(Filter); + Vector3 resultSum = Vector3.Zero; + for (int i = 0; i < iterationCount; ++i) + { + ref var pair = ref pairs[i]; + var hit = task.Sweep( + aData, aType, pair.OrientationA, pair.VelocityA, + bData, bType, pair.OffsetB, pair.OrientationB, pair.VelocityB, + 0.1f, 1e-3f, 1e-3f, 15, ref filter, shapes, taskRegistry, pool, out var t0, out var t1, out var hitLocation, out var hitNormal); + if (hit) + resultSum += new Vector3(t0) + new Vector3(t1) + hitLocation + hitNormal; + } + return resultSum; + } + + [Benchmark] + public void SphereSphere() => Test(); + [Benchmark] + public void SphereCapsule() => Test(); + [Benchmark] + public void SphereBox() => Test(); + [Benchmark] + public void SphereTriangle() => Test(); + [Benchmark] + public void SphereCylinder() => Test(); + [Benchmark] + public void SphereConvexHull() => Test(); + [Benchmark] + public void SphereCompound() => Test(); + [Benchmark] + public void SphereBigCompound() => Test(); + [Benchmark] + public void SphereMesh() => Test(); + [Benchmark] + public void CapsuleCapsule() => Test(); + [Benchmark] + public void CapsuleBox() => Test(); + [Benchmark] + public void CapsuleTriangle() => Test(); + [Benchmark] + public void CapsuleCylinder() => Test(); + [Benchmark] + public void CapsuleConvexHull() => Test(); + [Benchmark] + public void CapsuleCompound() => Test(); + [Benchmark] + public void CapsuleBigCompound() => Test(); + [Benchmark] + public void CapsuleMesh() => Test(); + [Benchmark] + public void BoxBox() => Test(); + [Benchmark] + public void BoxTriangle() => Test(); + [Benchmark] + public void BoxCylinder() => Test(); + [Benchmark] + public void BoxConvexHull() => Test(); + [Benchmark] + public void BoxCompound() => Test(); + [Benchmark] + public void BoxBigCompound() => Test(); + [Benchmark] + public void BoxMesh() => Test(); + [Benchmark] + public void TriangleTriangle() => Test(); + [Benchmark] + public void TriangleCylinder() => Test(); + [Benchmark] + public void TriangleConvexHull() => Test(); + [Benchmark] + public void TriangleCompound() => Test(); + [Benchmark] + public void TriangleBigCompound() => Test(); + [Benchmark] + public void TriangleMesh() => Test(); + [Benchmark] + public void CylinderCylinder() => Test(); + [Benchmark] + public void CylinderConvexHull() => Test(); + [Benchmark] + public void CylinderCompound() => Test(); + [Benchmark] + public void CylinderBigCompound() => Test(); + [Benchmark] + public void CylinderMesh() => Test(); + [Benchmark] + public void ConvexHullConvexHull() => Test(); + [Benchmark] + public void ConvexHullCompound() => Test(); + [Benchmark] + public void ConvexHullBigCompound() => Test(); + [Benchmark] + public void ConvexHullMesh() => Test(); + [Benchmark] + public void CompoundCompound() => Test(); + [Benchmark] + public void CompoundBigCompound() => Test(); + [Benchmark] + public void CompoundMesh() => Test(); + [Benchmark] + public void BigCompoundBigCompound() => Test(); + [Benchmark] + public void BigCompoundMesh() => Test(); + //No mesh-mesh! +} From 9a74ff8cf37619b35b47567258a41415f9c5d4dd Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 22 Oct 2022 21:02:40 -0500 Subject: [PATCH 546/947] Shrank per run counts. --- DemoBenchmarks/ConvexCollisionTesters.cs | 2 +- DemoBenchmarks/FourBodyConstraints.cs | 2 +- DemoBenchmarks/OneBodyConstraints.cs | 2 +- DemoBenchmarks/ThreeBodyConstraints.cs | 2 +- DemoBenchmarks/TwoBodyConstraints.cs | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/DemoBenchmarks/ConvexCollisionTesters.cs b/DemoBenchmarks/ConvexCollisionTesters.cs index f828c16ca..dfb70905e 100644 --- a/DemoBenchmarks/ConvexCollisionTesters.cs +++ b/DemoBenchmarks/ConvexCollisionTesters.cs @@ -18,7 +18,7 @@ namespace DemoBenchmarks; /// public class ConvexCollisionTesters { - const int iterationCount = 1000; + const int iterationCount = 100; BufferPool pool; Buffer offsetsB; Buffer> speculativeMargins; diff --git a/DemoBenchmarks/FourBodyConstraints.cs b/DemoBenchmarks/FourBodyConstraints.cs index 6ac9e9a70..237d9d074 100644 --- a/DemoBenchmarks/FourBodyConstraints.cs +++ b/DemoBenchmarks/FourBodyConstraints.cs @@ -30,7 +30,7 @@ public class FourBodyConstraintBenchmarks var velocityC = default(BodyVelocityWide); var velocityD = default(BodyVelocityWide); //Individual constraint iterations are often extremely cheap, so beef the benchmark up a bit. - const int iterations = 10000; + const int iterations = 1000; const float inverseDt = 60f; const float dt = 1f / inverseDt; for (int i = 0; i < iterations; ++i) diff --git a/DemoBenchmarks/OneBodyConstraints.cs b/DemoBenchmarks/OneBodyConstraints.cs index d1feb9ff7..edd4d0f01 100644 --- a/DemoBenchmarks/OneBodyConstraints.cs +++ b/DemoBenchmarks/OneBodyConstraints.cs @@ -23,7 +23,7 @@ static BodyVelocityWide BenchmarkOneBodyConstraint Date: Mon, 24 Oct 2022 15:26:20 -0500 Subject: [PATCH 547/947] Shape ray tests. --- DemoBenchmarks/BenchmarkHelper.cs | 21 +++-- DemoBenchmarks/CollisionBatcherTasks.cs | 40 --------- DemoBenchmarks/Program.cs | 1 + DemoBenchmarks/ShapeRayTests.cs | 108 ++++++++++++++++++++++++ 4 files changed, 124 insertions(+), 46 deletions(-) create mode 100644 DemoBenchmarks/ShapeRayTests.cs diff --git a/DemoBenchmarks/BenchmarkHelper.cs b/DemoBenchmarks/BenchmarkHelper.cs index b8fc30ddb..322d277c1 100644 --- a/DemoBenchmarks/BenchmarkHelper.cs +++ b/DemoBenchmarks/BenchmarkHelper.cs @@ -8,18 +8,27 @@ namespace DemoBenchmarks; public static class BenchmarkHelper { - public static RigidPose CreateRandomPose(Random random, BoundingBox positionBounds) + public static Vector3 CreateRandomPosition(Random random, BoundingBox positionBounds) { - RigidPose pose; var span = positionBounds.Max - positionBounds.Min; - pose.Position = positionBounds.Min + span * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); - var axis = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); + return positionBounds.Min + span * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); + } + public static Vector3 CreateRandomDirection(Random random) + { + //This is a biased sampling, but that doesn't matter. + var axis = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) * 2 - Vector3.One; var length = axis.Length(); - if (length > 0) + if (length > 1e-10f) axis /= length; else axis = new Vector3(0, 1, 0); - pose.Orientation = QuaternionEx.CreateFromAxisAngle(axis, 1203f * random.NextSingle()); + return axis; + } + public static RigidPose CreateRandomPose(Random random, BoundingBox positionBounds) + { + RigidPose pose; + pose.Position = CreateRandomPosition(random, positionBounds); + pose.Orientation = QuaternionEx.CreateFromAxisAngle(CreateRandomDirection(random), 1203f * random.NextSingle()); return pose; } diff --git a/DemoBenchmarks/CollisionBatcherTasks.cs b/DemoBenchmarks/CollisionBatcherTasks.cs index f05d46cd1..6cb4fc0d5 100644 --- a/DemoBenchmarks/CollisionBatcherTasks.cs +++ b/DemoBenchmarks/CollisionBatcherTasks.cs @@ -33,46 +33,6 @@ struct Pair CollisionTaskRegistry taskRegistry; Shapes shapes; - - public static void CreateDeformedPlane(int width, int height, Func deformer, Vector3 scaling, BufferPool pool, out Mesh mesh) - { - pool.Take(width * height, out var vertices); - for (int i = 0; i < width; ++i) - { - for (int j = 0; j < height; ++j) - { - vertices[width * j + i] = deformer(i, j); - } - } - - var quadWidth = width - 1; - var quadHeight = height - 1; - var triangleCount = quadWidth * quadHeight * 2; - pool.Take(triangleCount, out var triangles); - - for (int i = 0; i < quadWidth; ++i) - { - for (int j = 0; j < quadHeight; ++j) - { - var triangleIndex = (j * quadWidth + i) * 2; - ref var triangle0 = ref triangles[triangleIndex]; - ref var v00 = ref vertices[width * j + i]; - ref var v01 = ref vertices[width * j + i + 1]; - ref var v10 = ref vertices[width * (j + 1) + i]; - ref var v11 = ref vertices[width * (j + 1) + i + 1]; - triangle0.A = v00; - triangle0.B = v01; - triangle0.C = v10; - ref var triangle1 = ref triangles[triangleIndex + 1]; - triangle1.A = v01; - triangle1.B = v11; - triangle1.C = v10; - } - } - pool.Return(ref vertices); - mesh = new Mesh(triangles, scaling, pool); - } - [GlobalSetup] public unsafe void Setup() { diff --git a/DemoBenchmarks/Program.cs b/DemoBenchmarks/Program.cs index 52378c459..d56d9e0d0 100644 --- a/DemoBenchmarks/Program.cs +++ b/DemoBenchmarks/Program.cs @@ -8,3 +8,4 @@ BenchmarkRunner.Run(typeof(ConvexCollisionTesters)); BenchmarkRunner.Run(typeof(CollisionBatcherTasks)); BenchmarkRunner.Run(typeof(Sweeps)); +BenchmarkRunner.Run(typeof(ShapeRayTests)); diff --git a/DemoBenchmarks/ShapeRayTests.cs b/DemoBenchmarks/ShapeRayTests.cs new file mode 100644 index 000000000..c7fa12dc1 --- /dev/null +++ b/DemoBenchmarks/ShapeRayTests.cs @@ -0,0 +1,108 @@ +using BenchmarkDotNet.Attributes; +using BepuPhysics; +using BepuPhysics.Collidables; +using BepuPhysics.CollisionDetection; +using BepuPhysics.CollisionDetection.CollisionTasks; +using BepuPhysics.Trees; +using BepuUtilities; +using BepuUtilities.Memory; +using System.Diagnostics; +using System.Numerics; +using static DemoBenchmarks.BenchmarkHelper; + +namespace DemoBenchmarks; + +/// +/// Evaluates performance of shape ray tests. +/// +public class ShapeRayTests +{ + const int iterationCount = 100; + BufferPool pool; + + struct Iteration + { + public RigidPose Pose; + public RayData Ray; + } + + Buffer iterations; + Shapes shapes; + + + [GlobalSetup] + public unsafe void Setup() + { + pool = new BufferPool(); + pool.Take(iterationCount, out iterations); + shapes = new Shapes(pool, 1); + + Random random = new(5); + CreateShapes(random, pool, shapes); + + //Fill random values for pair tests. + BoundingBox bounds = new() { Min = new Vector3(0, 0, 0), Max = new Vector3(2, 2, 2) }; + for (int i = 0; i < iterationCount; ++i) + { + iterations[i] = new() + { + Pose = CreateRandomPose(random, bounds), + Ray = new RayData { Origin = CreateRandomPosition(random, bounds), Direction = CreateRandomDirection(random), Id = i } + }; + } + } + + [GlobalCleanup] + public void Cleanup() + { + //All outstanding allocations poof when the pool is cleared. + pool.Clear(); + } + + + struct HitHandler : IShapeRayHitHandler + { + public Vector3 ResultSum; + + public bool AllowTest(int childIndex) + { + return true; + } + + public void OnRayHit(in RayData ray, ref float maximumT, float t, Vector3 normal, int childIndex) + { + ResultSum += new Vector3(t) + normal; + } + } + + unsafe Vector3 Test() where TShape : unmanaged, IShape + { + var hitHandler = new HitHandler(); + for (int i = 0; i < iterationCount; ++i) + { + ref var iteration = ref iterations[i]; + float maximumT = float.MaxValue; + shapes[default(TShape).TypeId].RayTest(0, iteration.Pose, iteration.Ray, ref maximumT, ref hitHandler); + } + return hitHandler.ResultSum; + } + + [Benchmark] + public unsafe Vector3 RaySphere() => Test(); + [Benchmark] + public unsafe Vector3 RayCapsule() => Test(); + [Benchmark] + public unsafe Vector3 RayBox() => Test(); + [Benchmark] + public unsafe Vector3 RayTriangle() => Test(); + [Benchmark] + public unsafe Vector3 RayCylinder() => Test(); + [Benchmark] + public unsafe Vector3 RayConvexHull() => Test(); + [Benchmark] + public unsafe Vector3 RayCompound() => Test(); + [Benchmark] + public unsafe Vector3 RayBigCompound() => Test(); + [Benchmark] + public unsafe Vector3 RayMesh() => Test(); +} From 96f6f4667abe305cf5d4af70cf03cbcb7901da7a Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 24 Oct 2022 17:42:47 -0500 Subject: [PATCH 548/947] Gather scatter benchmarks. --- DemoBenchmarks/GatherScatter.cs | 151 ++++++++++++++++++++++++++++++++ DemoBenchmarks/Program.cs | 1 + 2 files changed, 152 insertions(+) create mode 100644 DemoBenchmarks/GatherScatter.cs diff --git a/DemoBenchmarks/GatherScatter.cs b/DemoBenchmarks/GatherScatter.cs new file mode 100644 index 000000000..dc8eeac8b --- /dev/null +++ b/DemoBenchmarks/GatherScatter.cs @@ -0,0 +1,151 @@ +using BenchmarkDotNet.Attributes; +using BepuPhysics; +using BepuPhysics.Collidables; +using BepuPhysics.CollisionDetection; +using BepuPhysics.CollisionDetection.CollisionTasks; +using BepuPhysics.Constraints; +using BepuPhysics.Trees; +using BepuUtilities; +using BepuUtilities.Memory; +using System.Diagnostics; +using System.Numerics; +using static DemoBenchmarks.BenchmarkHelper; + +namespace DemoBenchmarks; + +/// +/// Evaluates performance of scatter/gather operations used by constraints to pull body data. +/// +public class GatherScatter +{ + unsafe struct NarrowPhaseCallbacks : INarrowPhaseCallbacks + { + public void Initialize(Simulation simulation) { } + public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin) => a.Mobility == CollidableMobility.Dynamic || b.Mobility == CollidableMobility.Dynamic; + public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB) => true; + public unsafe bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold + { + pairMaterial = default; + return true; + } + public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold) { return true; } + public void Dispose() { } + } + + public struct PoseIntegratorCallbacks : IPoseIntegratorCallbacks + { + public void Initialize(Simulation simulation) { } + public readonly AngularIntegrationMode AngularIntegrationMode => AngularIntegrationMode.Nonconserving; + public readonly bool AllowSubstepsForUnconstrainedBodies => false; + public readonly bool IntegrateVelocityForKinematics => false; + public void PrepareForIntegration(float dt) { } + public void IntegrateVelocity(Vector bodyIndices, Vector3Wide position, QuaternionWide orientation, BodyInertiaWide localInertia, Vector integrationMask, int workerIndex, Vector dt, ref BodyVelocityWide velocity) { } + + } + + + const int iterationCount = 1000; + const int bodyCount = 1000; + BufferPool pool; + + + Buffer> bodyIndices; + Buffer poses; + Buffer velocities; + Buffer inertias; + Simulation simulation; + + + [GlobalSetup] + public unsafe void Setup() + { + pool = new BufferPool(); + pool.Take(iterationCount, out bodyIndices); + pool.Take(iterationCount, out poses); + pool.Take(iterationCount, out velocities); + pool.Take(iterationCount, out inertias); + simulation = Simulation.Create(pool, new NarrowPhaseCallbacks(), new PoseIntegratorCallbacks(), new SolveDescription(1, 1)); + + Random random = new(5); + + //Fill random values for pair tests. + BoundingBox bounds = new() { Min = new Vector3(0, 0, 0), Max = new Vector3(50, 50, 50) }; + for (int i = 0; i < bodyCount; ++i) + { + simulation.Bodies.Add(BodyDescription.CreateDynamic( + CreateRandomPose(random, bounds), + new BodyInertia { InverseMass = 1, InverseInertiaTensor = new Symmetric3x3 { XX = 1, YY = 1, ZZ = 1 } }, + default, new BodyActivityDescription(-0.01f))); + } + + Span bodyIndicesBundle = stackalloc int[Vector.Count]; + for (int i = 0; i < iterationCount; ++i) + { + for (int j = 0; j < Vector.Count; ++j) + { + bodyIndicesBundle[j] = random.Next(0, bodyCount); + } + bodyIndices[i] = new Vector(bodyIndicesBundle); + + simulation.Bodies.GatherState(bodyIndices[i], true, out poses[i].Position, out poses[i].Orientation, out velocities[i], out inertias[i]); + } + } + + [GlobalCleanup] + public void Cleanup() + { + //All outstanding allocations poof when the pool is cleared. + pool.Clear(); + } + + + + [Benchmark] + public unsafe Vector GatherState() + { + Vector sum = default; + for (int i = 0; i < iterationCount; ++i) + { + simulation.Bodies.GatherState(bodyIndices[i], true, out var position, out var orientation, out var velocity, out var inertia); + sum += position.X + position.Y + position.Z + + velocity.Linear.X + velocity.Linear.Y + velocity.Linear.Z + + velocity.Angular.X + velocity.Angular.Y + velocity.Angular.Z + + orientation.X + orientation.Y + orientation.Z + orientation.Z + + inertia.InverseInertiaTensor.XX + inertia.InverseInertiaTensor.YX + inertia.InverseInertiaTensor.YY + + inertia.InverseInertiaTensor.ZX + inertia.InverseInertiaTensor.ZY + inertia.InverseInertiaTensor.ZZ + inertia.InverseMass; + } + return sum; + } + + + [Benchmark] + public unsafe void ScatterPose() + { + var mask = new Vector(-1); + for (int i = 0; i < iterationCount; ++i) + { + ref var pose = ref poses[i]; + simulation.Bodies.ScatterPose(ref pose.Position, ref pose.Orientation, bodyIndices[i], mask); + } + } + + [Benchmark] + public unsafe void ScatterInertia() + { + var mask = new Vector(-1); + for (int i = 0; i < iterationCount; ++i) + { + simulation.Bodies.ScatterInertia(ref inertias[i], bodyIndices[i], mask); + } + } + [Benchmark] + public unsafe void ScatterVelocities() + { + for (int i = 0; i < iterationCount; ++i) + { + simulation.Bodies.ScatterVelocities(ref velocities[i], ref bodyIndices[i]); + } + } + + +} diff --git a/DemoBenchmarks/Program.cs b/DemoBenchmarks/Program.cs index d56d9e0d0..71aaaf366 100644 --- a/DemoBenchmarks/Program.cs +++ b/DemoBenchmarks/Program.cs @@ -9,3 +9,4 @@ BenchmarkRunner.Run(typeof(CollisionBatcherTasks)); BenchmarkRunner.Run(typeof(Sweeps)); BenchmarkRunner.Run(typeof(ShapeRayTests)); +BenchmarkRunner.Run(typeof(GatherScatter)); From 844b717672c351b9464aaeede45573d95b3f41aa Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 24 Oct 2022 18:20:49 -0500 Subject: [PATCH 549/947] RagdollTubeBenchmark. --- DemoBenchmarks/GatherScatter.cs | 3 - DemoBenchmarks/Program.cs | 2 + DemoBenchmarks/RagdollTubeBenchmark.cs | 594 +++++++++++++++++++++++++ 3 files changed, 596 insertions(+), 3 deletions(-) create mode 100644 DemoBenchmarks/RagdollTubeBenchmark.cs diff --git a/DemoBenchmarks/GatherScatter.cs b/DemoBenchmarks/GatherScatter.cs index dc8eeac8b..5c52ad050 100644 --- a/DemoBenchmarks/GatherScatter.cs +++ b/DemoBenchmarks/GatherScatter.cs @@ -2,12 +2,9 @@ using BepuPhysics; using BepuPhysics.Collidables; using BepuPhysics.CollisionDetection; -using BepuPhysics.CollisionDetection.CollisionTasks; using BepuPhysics.Constraints; -using BepuPhysics.Trees; using BepuUtilities; using BepuUtilities.Memory; -using System.Diagnostics; using System.Numerics; using static DemoBenchmarks.BenchmarkHelper; diff --git a/DemoBenchmarks/Program.cs b/DemoBenchmarks/Program.cs index 71aaaf366..226ea8019 100644 --- a/DemoBenchmarks/Program.cs +++ b/DemoBenchmarks/Program.cs @@ -10,3 +10,5 @@ BenchmarkRunner.Run(typeof(Sweeps)); BenchmarkRunner.Run(typeof(ShapeRayTests)); BenchmarkRunner.Run(typeof(GatherScatter)); +BenchmarkRunner.Run(typeof(RagdollTubeBenchmark)); + diff --git a/DemoBenchmarks/RagdollTubeBenchmark.cs b/DemoBenchmarks/RagdollTubeBenchmark.cs new file mode 100644 index 000000000..1bf8f61e6 --- /dev/null +++ b/DemoBenchmarks/RagdollTubeBenchmark.cs @@ -0,0 +1,594 @@ +using BenchmarkDotNet.Attributes; +using BepuPhysics; +using BepuPhysics.Collidables; +using BepuPhysics.CollisionDetection; +using BepuPhysics.Constraints; +using BepuUtilities; +using BepuUtilities.Memory; +using System.Diagnostics; +using System.Numerics; +using System.Runtime.CompilerServices; + +namespace DemoBenchmarks; + +/// +/// Evaluates performance of scatter/gather operations used by constraints to pull body data. +/// +public class RagdollTubeBenchmark +{ + public struct SubgroupCollisionFilter + { + public ushort SubgroupMembership; + public ushort CollidableSubgroups; + public int GroupId; + public SubgroupCollisionFilter(int groupId) + { + GroupId = groupId; + SubgroupMembership = ushort.MaxValue; + CollidableSubgroups = ushort.MaxValue; + } + public SubgroupCollisionFilter(int groupId, int subgroupId) + { + GroupId = groupId; + Debug.Assert(subgroupId >= 0 && subgroupId < 16, "The subgroup field is a ushort; it can only hold 16 distinct subgroups."); + SubgroupMembership = (ushort)(1 << subgroupId); + CollidableSubgroups = ushort.MaxValue; + } + public void DisableCollision(int subgroupId) + { + Debug.Assert(subgroupId >= 0 && subgroupId < 16, "The subgroup field is a ushort; it can only hold 16 distinct subgroups."); + CollidableSubgroups ^= (ushort)(1 << subgroupId); + } + public static void DisableCollision(ref SubgroupCollisionFilter filterA, ref SubgroupCollisionFilter filterB) + { + filterA.CollidableSubgroups &= (ushort)~filterB.SubgroupMembership; + filterB.CollidableSubgroups &= (ushort)~filterA.SubgroupMembership; + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static bool AllowCollision(in SubgroupCollisionFilter a, in SubgroupCollisionFilter b) + { + return a.GroupId != b.GroupId || (a.CollidableSubgroups & b.SubgroupMembership) > 0; + } + + } + + struct SubgroupFilteredCallbacks : INarrowPhaseCallbacks + { + public CollidableProperty CollisionFilters; + public PairMaterialProperties Material; + public SubgroupFilteredCallbacks(CollidableProperty filters) + { + CollisionFilters = filters; + Material = new PairMaterialProperties(1, 2, new SpringSettings(30, 1)); + } + public SubgroupFilteredCallbacks(CollidableProperty filters, PairMaterialProperties material) + { + CollisionFilters = filters; + Material = material; + } + public void Initialize(Simulation simulation) + { + CollisionFilters.Initialize(simulation); + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin) + { + if (b.Mobility != CollidableMobility.Static) + { + return SubgroupCollisionFilter.AllowCollision(CollisionFilters[a.BodyHandle], CollisionFilters[b.BodyHandle]); + } + return a.Mobility == CollidableMobility.Dynamic || b.Mobility == CollidableMobility.Dynamic; + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB) + { + return true; + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public unsafe bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold + { + pairMaterial = Material; + return true; + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public unsafe bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold) + { + return true; + } + public void Dispose() + { + CollisionFilters.Dispose(); + } + } + + public struct DemoPoseIntegratorCallbacks : IPoseIntegratorCallbacks + { + public Vector3 Gravity; + public float LinearDamping; + public float AngularDamping; + public readonly AngularIntegrationMode AngularIntegrationMode => AngularIntegrationMode.Nonconserving; + public readonly bool AllowSubstepsForUnconstrainedBodies => false; + public readonly bool IntegrateVelocityForKinematics => false; + public void Initialize(Simulation simulation) { } + public DemoPoseIntegratorCallbacks(Vector3 gravity, float linearDamping = .03f, float angularDamping = .03f) : this() + { + Gravity = gravity; + LinearDamping = linearDamping; + AngularDamping = angularDamping; + } + + Vector3Wide gravityWideDt; + Vector linearDampingDt; + Vector angularDampingDt; + + public void PrepareForIntegration(float dt) + { + linearDampingDt = new Vector(MathF.Pow(MathHelper.Clamp(1 - LinearDamping, 0, 1), dt)); + angularDampingDt = new Vector(MathF.Pow(MathHelper.Clamp(1 - AngularDamping, 0, 1), dt)); + gravityWideDt = Vector3Wide.Broadcast(Gravity * dt); + } + public void IntegrateVelocity(Vector bodyIndices, Vector3Wide position, QuaternionWide orientation, BodyInertiaWide localInertia, Vector integrationMask, int workerIndex, Vector dt, ref BodyVelocityWide velocity) + { + velocity.Linear = (velocity.Linear + gravityWideDt) * linearDampingDt; + velocity.Angular = velocity.Angular * angularDampingDt; + } + } + static BodyHandle AddBody(TShape shape, float mass, in RigidPose pose, Simulation simulation) where TShape : unmanaged, IConvexShape + { + //Note that this always registers a new shape instance. You could be more clever/efficient and share shapes, but the goal here is to show the most basic option. + //Also, the cost of registering different shapes isn't that high for tiny implicit shapes. + var shapeIndex = simulation.Shapes.Add(shape); + var description = BodyDescription.CreateDynamic(pose, shape.ComputeInertia(mass), shapeIndex, 0.01f); + return simulation.Bodies.Add(description); + } + + static RigidPose GetWorldPose(Vector3 localPosition, Quaternion localOrientation, RigidPose ragdollPose) + { + RigidPose worldPose; + RigidPose.Transform(localPosition, ragdollPose, out worldPose.Position); + QuaternionEx.ConcatenateWithoutOverlap(localOrientation, ragdollPose.Orientation, out worldPose.Orientation); + return worldPose; + } + public static void GetCapsuleForLineSegment(Vector3 start, Vector3 end, float radius, out Capsule capsule, out Vector3 position, out Quaternion orientation) + { + position = 0.5f * (start + end); + + var offset = end - start; + capsule.HalfLength = 0.5f * offset.Length(); + capsule.Radius = radius; + //The capsule shape's length is along its local Y axis, so get the shortest rotation from Y to the current orientation. + var cross = Vector3.Cross(offset / capsule.Length, new Vector3(0, 1, 0)); + var crossLength = cross.Length(); + orientation = crossLength > 1e-8f ? QuaternionEx.CreateFromAxisAngle(cross / crossLength, (float)Math.Asin(crossLength)) : Quaternion.Identity; + } + + public static Quaternion CreateBasis(Vector3 z, Vector3 x) + { + //For ease of use, don't assume that x is perpendicular to z, nor that either input is normalized. + Matrix3x3 basis; + basis.Z = Vector3.Normalize(z); + basis.Y = Vector3.Normalize(Vector3.Cross(basis.Z, x)); + basis.X = Vector3.Cross(basis.Y, basis.Z); + QuaternionEx.CreateFromRotationMatrix(basis, out var toReturn); + return toReturn; + } + + static AngularMotor BuildAngularMotor() + { + //By default, these motors use nonzero softness (inverse damping) to damp the relative motion between ragdoll pieces. + //If you set the damping to 0 and then set the maximum force to some finite value (75 works reasonably well), the ragdolls act more like action figures. + //You could also replace the AngularMotors with AngularServos and provide actual relative orientation goals for physics-driven animation. + return new AngularMotor { TargetVelocityLocalA = new Vector3(), Settings = new MotorSettings(float.MaxValue, 0.01f) }; + } + + static RagdollArmHandles AddArm(float sign, Vector3 localShoulder, RigidPose localChestPose, BodyHandle chestHandle, ref SubgroupCollisionFilter chestMask, + int limbBaseBitIndex, int ragdollIndex, RigidPose ragdollPose, CollidableProperty filters, SpringSettings constraintSpringSettings, Simulation simulation) + { + RagdollArmHandles handles; + var localElbow = localShoulder + new Vector3(sign * 0.45f, 0, 0); + var localWrist = localElbow + new Vector3(sign * 0.45f, 0, 0); + var handPosition = localWrist + new Vector3(sign * 0.1f, 0, 0); + GetCapsuleForLineSegment(localShoulder, localElbow, 0.1f, out var upperArmShape, out var upperArmPosition, out var upperArmOrientation); + handles.UpperArm = AddBody(upperArmShape, 5, GetWorldPose(upperArmPosition, upperArmOrientation, ragdollPose), simulation); + GetCapsuleForLineSegment(localElbow, localWrist, 0.09f, out var lowerArmShape, out var lowerArmPosition, out var lowerArmOrientation); + handles.LowerArm = AddBody(lowerArmShape, 5, GetWorldPose(lowerArmPosition, lowerArmOrientation, ragdollPose), simulation); + handles.Hand = AddBody(new Box(0.2f, 0.1f, 0.2f), 2, GetWorldPose(handPosition, Quaternion.Identity, ragdollPose), simulation); + + //Create joints between limb pieces. + //Chest-Upper Arm + simulation.Solver.Add(chestHandle, handles.UpperArm, new BallSocket + { + LocalOffsetA = QuaternionEx.Transform(localShoulder - localChestPose.Position, QuaternionEx.Conjugate(localChestPose.Orientation)), + LocalOffsetB = QuaternionEx.Transform(localShoulder - upperArmPosition, QuaternionEx.Conjugate(upperArmOrientation)), + SpringSettings = constraintSpringSettings + }); + simulation.Solver.Add(chestHandle, handles.UpperArm, new SwingLimit + { + AxisLocalA = QuaternionEx.Transform(Vector3.Normalize(new Vector3(sign, 0, 1)), QuaternionEx.Conjugate(localChestPose.Orientation)), + AxisLocalB = QuaternionEx.Transform(new Vector3(sign, 0, 0), QuaternionEx.Conjugate(upperArmOrientation)), + MaximumSwingAngle = MathHelper.Pi * 0.56f, + SpringSettings = constraintSpringSettings + }); + simulation.Solver.Add(chestHandle, handles.UpperArm, new TwistLimit + { + LocalBasisA = QuaternionEx.Concatenate(CreateBasis(new Vector3(1, 0, 0), new Vector3(0, 0, -1)), QuaternionEx.Conjugate(localChestPose.Orientation)), + LocalBasisB = QuaternionEx.Concatenate(CreateBasis(new Vector3(1, 0, 0), new Vector3(0, 0, -1)), QuaternionEx.Conjugate(upperArmOrientation)), + MinimumAngle = MathHelper.Pi * -0.55f, + MaximumAngle = MathHelper.Pi * 0.55f, + SpringSettings = constraintSpringSettings + }); + simulation.Solver.Add(chestHandle, handles.UpperArm, BuildAngularMotor()); + + //Upper Arm-Lower Arm + simulation.Solver.Add(handles.UpperArm, handles.LowerArm, new SwivelHinge + { + LocalOffsetA = QuaternionEx.Transform(localElbow - upperArmPosition, QuaternionEx.Conjugate(upperArmOrientation)), + LocalSwivelAxisA = new Vector3(1, 0, 0), + LocalOffsetB = QuaternionEx.Transform(localElbow - lowerArmPosition, QuaternionEx.Conjugate(lowerArmOrientation)), + LocalHingeAxisB = new Vector3(0, 1, 0), + SpringSettings = constraintSpringSettings + }); + simulation.Solver.Add(handles.UpperArm, handles.LowerArm, new SwingLimit + { + AxisLocalA = new Vector3(0, 1, 0), + AxisLocalB = new Vector3(sign, 0, 0), + MaximumSwingAngle = MathHelper.PiOver2, + SpringSettings = constraintSpringSettings + }); + simulation.Solver.Add(handles.UpperArm, handles.LowerArm, new TwistLimit + { + LocalBasisA = QuaternionEx.Concatenate(CreateBasis(new Vector3(1, 0, 0), new Vector3(0, 0, -1)), QuaternionEx.Conjugate(upperArmOrientation)), + LocalBasisB = QuaternionEx.Concatenate(CreateBasis(new Vector3(1, 0, 0), new Vector3(0, 0, -1)), QuaternionEx.Conjugate(lowerArmOrientation)), + MinimumAngle = MathHelper.Pi * -0.55f, + MaximumAngle = MathHelper.Pi * 0.55f, + SpringSettings = constraintSpringSettings + }); + simulation.Solver.Add(handles.UpperArm, handles.LowerArm, BuildAngularMotor()); + + //Lower Arm-Hand + simulation.Solver.Add(handles.LowerArm, handles.Hand, new BallSocket + { + LocalOffsetA = QuaternionEx.Transform(localWrist - lowerArmPosition, QuaternionEx.Conjugate(lowerArmOrientation)), + LocalOffsetB = localWrist - handPosition, + SpringSettings = constraintSpringSettings + }); + simulation.Solver.Add(handles.LowerArm, handles.Hand, new SwingLimit + { + AxisLocalA = QuaternionEx.Transform(new Vector3(sign, 0, 0), QuaternionEx.Conjugate(lowerArmOrientation)), + AxisLocalB = new Vector3(sign, 0, 0), + MaximumSwingAngle = MathHelper.PiOver2, + SpringSettings = constraintSpringSettings + }); + simulation.Solver.Add(handles.LowerArm, handles.Hand, new TwistServo + { + LocalBasisA = QuaternionEx.Concatenate(CreateBasis(new Vector3(1, 0, 0), new Vector3(0, 0, 1)), QuaternionEx.Conjugate(lowerArmOrientation)), + LocalBasisB = CreateBasis(new Vector3(1, 0, 0), new Vector3(0, 0, 1)), + TargetAngle = 0, + SpringSettings = constraintSpringSettings, + ServoSettings = new ServoSettings(float.MaxValue, 0, float.MaxValue) + }); + simulation.Solver.Add(handles.LowerArm, handles.Hand, BuildAngularMotor()); + + //Disable collisions between connected ragdoll pieces. + var upperArmLocalIndex = limbBaseBitIndex; + var lowerArmLocalIndex = limbBaseBitIndex + 1; + var handLocalIndex = limbBaseBitIndex + 2; + ref var upperArmFilter = ref filters.Allocate(handles.UpperArm); + ref var lowerArmFilter = ref filters.Allocate(handles.LowerArm); + ref var handFilter = ref filters.Allocate(handles.Hand); + upperArmFilter = new SubgroupCollisionFilter(ragdollIndex, upperArmLocalIndex); + lowerArmFilter = new SubgroupCollisionFilter(ragdollIndex, lowerArmLocalIndex); + handFilter = new SubgroupCollisionFilter(ragdollIndex, handLocalIndex); + SubgroupCollisionFilter.DisableCollision(ref chestMask, ref upperArmFilter); + SubgroupCollisionFilter.DisableCollision(ref upperArmFilter, ref lowerArmFilter); + SubgroupCollisionFilter.DisableCollision(ref lowerArmFilter, ref handFilter); + + return handles; + } + + static RagdollLegHandles AddLeg(Vector3 localHip, RigidPose localHipsPose, BodyHandle hipsHandle, ref SubgroupCollisionFilter hipsFilter, + int limbBaseBitIndex, int ragdollIndex, RigidPose ragdollPose, CollidableProperty filters, SpringSettings constraintSpringSettings, Simulation simulation) + { + RagdollLegHandles handles; + var localKnee = localHip - new Vector3(0, 0.5f, 0); + var localAnkle = localKnee - new Vector3(0, 0.5f, 0); + var localFoot = localAnkle + new Vector3(0, -0.075f, 0.05f); + GetCapsuleForLineSegment(localHip, localKnee, 0.12f, out var upperLegShape, out var upperLegPosition, out var upperLegOrientation); + handles.UpperLeg = AddBody(upperLegShape, 5, GetWorldPose(upperLegPosition, upperLegOrientation, ragdollPose), simulation); + GetCapsuleForLineSegment(localKnee, localAnkle, 0.11f, out var lowerLegShape, out var lowerLegPosition, out var lowerLegOrientation); + handles.LowerLeg = AddBody(lowerLegShape, 5, GetWorldPose(lowerLegPosition, lowerLegOrientation, ragdollPose), simulation); + handles.Foot = AddBody(new Box(0.2f, 0.15f, 0.3f), 2, GetWorldPose(localFoot, Quaternion.Identity, ragdollPose), simulation); + + //Create joints between limb pieces. + //Hips-Upper Leg + simulation.Solver.Add(hipsHandle, handles.UpperLeg, new BallSocket + { + LocalOffsetA = QuaternionEx.Transform(localHip - localHipsPose.Position, QuaternionEx.Conjugate(localHipsPose.Orientation)), + LocalOffsetB = QuaternionEx.Transform(localHip - upperLegPosition, QuaternionEx.Conjugate(upperLegOrientation)), + SpringSettings = constraintSpringSettings + }); + simulation.Solver.Add(hipsHandle, handles.UpperLeg, new SwingLimit + { + AxisLocalA = QuaternionEx.Transform(Vector3.Normalize(new Vector3(Math.Sign(localHip.X), -1, 0)), QuaternionEx.Conjugate(localHipsPose.Orientation)), + AxisLocalB = QuaternionEx.Transform(new Vector3(0, -1, 0), QuaternionEx.Conjugate(upperLegOrientation)), + MaximumSwingAngle = MathHelper.PiOver2, + SpringSettings = constraintSpringSettings + }); + simulation.Solver.Add(hipsHandle, handles.UpperLeg, new TwistLimit + { + LocalBasisA = QuaternionEx.Concatenate(CreateBasis(new Vector3(0, -1, 0), new Vector3(0, 0, 1)), QuaternionEx.Conjugate(localHipsPose.Orientation)), + LocalBasisB = QuaternionEx.Concatenate(CreateBasis(new Vector3(0, -1, 0), new Vector3(0, 0, 1)), QuaternionEx.Conjugate(upperLegOrientation)), + MinimumAngle = localHip.X < 0 ? MathHelper.Pi * -0.05f : MathHelper.Pi * -0.55f, + MaximumAngle = localHip.X < 0 ? MathHelper.Pi * 0.55f : MathHelper.Pi * 0.05f, + SpringSettings = constraintSpringSettings + }); + simulation.Solver.Add(hipsHandle, handles.UpperLeg, BuildAngularMotor()); + + //Upper Leg-Lower Leg + simulation.Solver.Add(handles.UpperLeg, handles.LowerLeg, new Hinge + { + LocalHingeAxisA = QuaternionEx.Transform(new Vector3(1, 0, 0), QuaternionEx.Conjugate(upperLegOrientation)), + LocalOffsetA = QuaternionEx.Transform(localKnee - upperLegPosition, QuaternionEx.Conjugate(upperLegOrientation)), + LocalHingeAxisB = QuaternionEx.Transform(new Vector3(1, 0, 0), QuaternionEx.Conjugate(lowerLegOrientation)), + LocalOffsetB = QuaternionEx.Transform(localKnee - lowerLegPosition, QuaternionEx.Conjugate(lowerLegOrientation)), + SpringSettings = constraintSpringSettings + }); + simulation.Solver.Add(handles.UpperLeg, handles.LowerLeg, new SwingLimit + { + AxisLocalA = QuaternionEx.Transform(new Vector3(0, 0, 1), QuaternionEx.Conjugate(upperLegOrientation)), + AxisLocalB = QuaternionEx.Transform(new Vector3(0, 1, 0), QuaternionEx.Conjugate(lowerLegOrientation)), + MaximumSwingAngle = MathHelper.PiOver2, + SpringSettings = constraintSpringSettings + }); + simulation.Solver.Add(handles.UpperLeg, handles.LowerLeg, BuildAngularMotor()); + + //Lower Leg-Foot + simulation.Solver.Add(handles.LowerLeg, handles.Foot, new BallSocket + { + LocalOffsetA = QuaternionEx.Transform(localAnkle - lowerLegPosition, QuaternionEx.Conjugate(lowerLegOrientation)), + LocalOffsetB = localAnkle - localFoot, + SpringSettings = constraintSpringSettings + }); + simulation.Solver.Add(handles.LowerLeg, handles.Foot, new SwingLimit + { + AxisLocalA = QuaternionEx.Transform(new Vector3(0, 1, 0), QuaternionEx.Conjugate(lowerLegOrientation)), + AxisLocalB = new Vector3(0, 1, 0), + MaximumSwingAngle = 1, + SpringSettings = constraintSpringSettings + }); + simulation.Solver.Add(handles.LowerLeg, handles.Foot, new TwistServo + { + LocalBasisA = QuaternionEx.Concatenate(CreateBasis(new Vector3(0, 1, 0), new Vector3(0, 0, 1)), QuaternionEx.Conjugate(lowerLegOrientation)), + LocalBasisB = CreateBasis(new Vector3(0, 1, 0), new Vector3(0, 0, 1)), + TargetAngle = 0, + SpringSettings = constraintSpringSettings, + ServoSettings = new ServoSettings(float.MaxValue, 0, float.MaxValue) + }); + simulation.Solver.Add(handles.LowerLeg, handles.Foot, BuildAngularMotor()); + + //Disable collisions between connected ragdoll pieces. + var upperLegLocalIndex = limbBaseBitIndex; + var lowerLegLocalIndex = limbBaseBitIndex + 1; + var footLocalIndex = limbBaseBitIndex + 2; + ref var upperLegFilter = ref filters.Allocate(handles.UpperLeg); + ref var lowerLegFilter = ref filters.Allocate(handles.LowerLeg); + ref var footFilter = ref filters.Allocate(handles.Foot); + upperLegFilter = new SubgroupCollisionFilter(ragdollIndex, upperLegLocalIndex); + lowerLegFilter = new SubgroupCollisionFilter(ragdollIndex, lowerLegLocalIndex); + footFilter = new SubgroupCollisionFilter(ragdollIndex, footLocalIndex); + SubgroupCollisionFilter.DisableCollision(ref hipsFilter, ref upperLegFilter); + SubgroupCollisionFilter.DisableCollision(ref upperLegFilter, ref lowerLegFilter); + SubgroupCollisionFilter.DisableCollision(ref lowerLegFilter, ref footFilter); + return handles; + } + + public struct RagdollArmHandles + { + public BodyHandle UpperArm; + public BodyHandle LowerArm; + public BodyHandle Hand; + } + public struct RagdollLegHandles + { + public BodyHandle UpperLeg; + public BodyHandle LowerLeg; + public BodyHandle Foot; + } + public struct RagdollHandles + { + public BodyHandle Head; + public BodyHandle Chest; + public BodyHandle Abdomen; + public BodyHandle Hips; + public RagdollArmHandles LeftArm; + public RagdollArmHandles RightArm; + public RagdollLegHandles LeftLeg; + public RagdollLegHandles RightLeg; + } + + public static RagdollHandles AddRagdoll(Vector3 position, Quaternion orientation, int ragdollIndex, CollidableProperty collisionFilters, Simulation simulation) + { + var ragdollPose = new RigidPose { Position = position, Orientation = orientation }; + var horizontalOrientation = QuaternionEx.CreateFromAxisAngle(new Vector3(0, 0, 1), MathHelper.PiOver2); + RagdollHandles handles; + var hipsPose = new RigidPose { Position = new Vector3(0, 1.1f, 0), Orientation = horizontalOrientation }; + handles.Hips = AddBody(new Capsule(0.17f, 0.25f), 8, GetWorldPose(hipsPose.Position, hipsPose.Orientation, ragdollPose), simulation); + var abdomenPose = new RigidPose { Position = new Vector3(0, 1.3f, 0), Orientation = horizontalOrientation }; + handles.Abdomen = AddBody(new Capsule(0.17f, 0.22f), 7, GetWorldPose(abdomenPose.Position, abdomenPose.Orientation, ragdollPose), simulation); + var chestPose = new RigidPose { Position = new Vector3(0, 1.6f, 0), Orientation = horizontalOrientation }; + handles.Chest = AddBody(new Capsule(0.21f, 0.3f), 10, GetWorldPose(chestPose.Position, chestPose.Orientation, ragdollPose), simulation); + var headPose = new RigidPose { Position = new Vector3(0, 2.05f, 0), Orientation = Quaternion.Identity }; + handles.Head = AddBody(new Sphere(0.2f), 5, GetWorldPose(headPose.Position, headPose.Orientation, ragdollPose), simulation); + + //Attach constraints between torso pieces. + var springSettings = new SpringSettings(15f, 1f); + var lowerSpine = (hipsPose.Position + abdomenPose.Position) * 0.5f; + //Hips-Abdomen + simulation.Solver.Add(handles.Hips, handles.Abdomen, new BallSocket + { + LocalOffsetA = QuaternionEx.Transform(lowerSpine - hipsPose.Position, QuaternionEx.Conjugate(hipsPose.Orientation)), + LocalOffsetB = QuaternionEx.Transform(lowerSpine - abdomenPose.Position, QuaternionEx.Conjugate(abdomenPose.Orientation)), + SpringSettings = springSettings + }); + simulation.Solver.Add(handles.Hips, handles.Abdomen, new SwingLimit + { + AxisLocalA = QuaternionEx.Transform(new Vector3(0, 1, 0), QuaternionEx.Conjugate(hipsPose.Orientation)), + AxisLocalB = QuaternionEx.Transform(new Vector3(0, 1, 0), QuaternionEx.Conjugate(abdomenPose.Orientation)), + MaximumSwingAngle = MathHelper.Pi * 0.27f, + SpringSettings = springSettings + }); + simulation.Solver.Add(handles.Hips, handles.Abdomen, new TwistLimit + { + LocalBasisA = QuaternionEx.Concatenate(CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)), QuaternionEx.Conjugate(hipsPose.Orientation)), + LocalBasisB = QuaternionEx.Concatenate(CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)), QuaternionEx.Conjugate(abdomenPose.Orientation)), + MinimumAngle = MathHelper.Pi * -0.2f, + MaximumAngle = MathHelper.Pi * 0.2f, + SpringSettings = springSettings + }); + simulation.Solver.Add(handles.Hips, handles.Abdomen, BuildAngularMotor()); + + //Abdomen-Chest + var upperSpine = (abdomenPose.Position + chestPose.Position) * 0.5f; + simulation.Solver.Add(handles.Abdomen, handles.Chest, new BallSocket + { + LocalOffsetA = QuaternionEx.Transform(upperSpine - abdomenPose.Position, QuaternionEx.Conjugate(abdomenPose.Orientation)), + LocalOffsetB = QuaternionEx.Transform(upperSpine - chestPose.Position, QuaternionEx.Conjugate(chestPose.Orientation)), + SpringSettings = springSettings + }); + simulation.Solver.Add(handles.Abdomen, handles.Chest, new SwingLimit + { + AxisLocalA = QuaternionEx.Transform(new Vector3(0, 1, 0), QuaternionEx.Conjugate(abdomenPose.Orientation)), + AxisLocalB = QuaternionEx.Transform(new Vector3(0, 1, 0), QuaternionEx.Conjugate(chestPose.Orientation)), + MaximumSwingAngle = MathHelper.Pi * 0.27f, + SpringSettings = springSettings + }); + simulation.Solver.Add(handles.Abdomen, handles.Chest, new TwistLimit + { + LocalBasisA = QuaternionEx.Concatenate(CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)), QuaternionEx.Conjugate(abdomenPose.Orientation)), + LocalBasisB = QuaternionEx.Concatenate(CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)), QuaternionEx.Conjugate(chestPose.Orientation)), + MinimumAngle = MathHelper.Pi * -0.2f, + MaximumAngle = MathHelper.Pi * 0.2f, + SpringSettings = springSettings + }); + simulation.Solver.Add(handles.Abdomen, handles.Chest, BuildAngularMotor()); + + //Chest-Head + var neck = (headPose.Position + chestPose.Position) * 0.5f; + simulation.Solver.Add(handles.Chest, handles.Head, new BallSocket + { + LocalOffsetA = QuaternionEx.Transform(neck - chestPose.Position, QuaternionEx.Conjugate(chestPose.Orientation)), + LocalOffsetB = neck - headPose.Position, + SpringSettings = springSettings + }); + simulation.Solver.Add(handles.Chest, handles.Head, new SwingLimit + { + AxisLocalA = QuaternionEx.Transform(new Vector3(0, 1, 0), QuaternionEx.Conjugate(chestPose.Orientation)), + AxisLocalB = new Vector3(0, 1, 0), + MaximumSwingAngle = MathHelper.PiOver2 * 0.9f, + SpringSettings = springSettings + }); + simulation.Solver.Add(handles.Chest, handles.Head, new TwistLimit + { + LocalBasisA = QuaternionEx.Concatenate(CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)), QuaternionEx.Conjugate(chestPose.Orientation)), + LocalBasisB = QuaternionEx.Concatenate(CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)), QuaternionEx.Conjugate(headPose.Orientation)), + MinimumAngle = MathHelper.Pi * -0.5f, + MaximumAngle = MathHelper.Pi * 0.5f, + SpringSettings = springSettings + }); + simulation.Solver.Add(handles.Chest, handles.Head, BuildAngularMotor()); + + var hipsLocalIndex = 0; + var abdomenLocalIndex = 1; + var chestLocalIndex = 2; + var headLocalIndex = 3; + ref var hipsFilter = ref collisionFilters.Allocate(handles.Hips); + ref var abdomenFilter = ref collisionFilters.Allocate(handles.Abdomen); + ref var chestFilter = ref collisionFilters.Allocate(handles.Chest); + ref var headFilter = ref collisionFilters.Allocate(handles.Head); + hipsFilter = new SubgroupCollisionFilter(ragdollIndex, hipsLocalIndex); + abdomenFilter = new SubgroupCollisionFilter(ragdollIndex, abdomenLocalIndex); + chestFilter = new SubgroupCollisionFilter(ragdollIndex, chestLocalIndex); + headFilter = new SubgroupCollisionFilter(ragdollIndex, headLocalIndex); + //Disable collisions in the torso and head. + SubgroupCollisionFilter.DisableCollision(ref hipsFilter, ref abdomenFilter); + SubgroupCollisionFilter.DisableCollision(ref abdomenFilter, ref chestFilter); + SubgroupCollisionFilter.DisableCollision(ref chestFilter, ref headFilter); + + //Build all the limbs. Setting the masks is delayed until after the limbs have been created and have disabled collisions with the chest/hips. + handles.RightArm = AddArm(1, chestPose.Position + new Vector3(0.4f, 0.1f, 0), chestPose, handles.Chest, ref chestFilter, 4, ragdollIndex, ragdollPose, collisionFilters, springSettings, simulation); + handles.LeftArm = AddArm(-1, chestPose.Position + new Vector3(-0.4f, 0.1f, 0), chestPose, handles.Chest, ref chestFilter, 7, ragdollIndex, ragdollPose, collisionFilters, springSettings, simulation); + handles.RightLeg = AddLeg(hipsPose.Position + new Vector3(-0.17f, -0.2f, 0), hipsPose, handles.Hips, ref hipsFilter, 10, ragdollIndex, ragdollPose, collisionFilters, springSettings, simulation); + handles.LeftLeg = AddLeg(hipsPose.Position + new Vector3(0.17f, -0.2f, 0), hipsPose, handles.Hips, ref hipsFilter, 13, ragdollIndex, ragdollPose, collisionFilters, springSettings, simulation); + return handles; + } + + const int timestepCount = 384; + BufferPool BufferPool; + Simulation Simulation; + + [IterationSetup] + public unsafe void Initialize() + { + var filters = new CollidableProperty(); + BufferPool = new BufferPool(); + Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks(filters, new PairMaterialProperties(2, float.MaxValue, new SpringSettings(10, 1))), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(4, 1)); + + int ragdollIndex = 0; + var spacing = new Vector3(1.7f, 1.8f, 0.5f); + int width = 4; + int height = 4; + int length = 2; + var origin = -0.5f * spacing * new Vector3(width - 1, 0, length - 1) + new Vector3(0, 5f, 0); + for (int i = 0; i < width; ++i) + { + for (int j = 0; j < height; ++j) + { + for (int k = 0; k < length; ++k) + { + AddRagdoll(origin + spacing * new Vector3(i, j, k), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathHelper.Pi * 0.05f), ragdollIndex++, filters, Simulation); + } + } + } + + var tubeCenter = new Vector3(0, 8, 0); + const int panelCount = 20; + const float tubeRadius = 6; + var panelShape = new Box(MathF.PI * 2 * tubeRadius / panelCount, 1, 80); + var panelShapeIndex = Simulation.Shapes.Add(panelShape); + var builder = new CompoundBuilder(BufferPool, Simulation.Shapes, panelCount + 1); + for (int i = 0; i < panelCount; ++i) + { + var rotation = QuaternionEx.CreateFromAxisAngle(Vector3.UnitZ, i * MathHelper.TwoPi / panelCount); + QuaternionEx.TransformUnitY(rotation, out var localUp); + var position = localUp * tubeRadius; + builder.AddForKinematic(panelShapeIndex, (position, rotation), 1); + } + builder.AddForKinematic(Simulation.Shapes.Add(new Box(1, 2, panelShape.Length)), new Vector3(0, tubeRadius - 1, 0), 0); + builder.BuildKinematicCompound(out var children); + var compound = new BigCompound(children, Simulation.Shapes, BufferPool); + var tubeHandle = Simulation.Bodies.Add(BodyDescription.CreateKinematic(tubeCenter, (default, new Vector3(0, 0, .25f)), Simulation.Shapes.Add(compound), 0f)); + filters[tubeHandle] = new SubgroupCollisionFilter(int.MaxValue); + builder.Dispose(); + + var staticShape = new Box(300, 1, 300); + var staticShapeIndex = Simulation.Shapes.Add(staticShape); + var staticDescription = new StaticDescription(new Vector3(0, -0.5f, 0), staticShapeIndex); + Simulation.Statics.Add(staticDescription); + } + + [IterationCleanup] + public void CleanUp() + { + BufferPool.Clear(); + BufferPool = null; + } + + [Benchmark] + public unsafe void Benchmark() + { + for (int i = 0; i < timestepCount; ++i) + { + Simulation.Timestep(1 / 60f); + } + } +} From 4ca89ac0c45708702d2d2719903d1bff8ec817e2 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 24 Oct 2022 21:58:48 -0500 Subject: [PATCH 550/947] ShapePileBenchmark. --- DemoBenchmarks/Program.cs | 1 + DemoBenchmarks/RagdollTubeBenchmark.cs | 2 +- DemoBenchmarks/ShapePileBenchmark.cs | 231 +++++++++++++++++++++++++ DemoBenchmarks/Sweeps.cs | 4 +- 4 files changed, 235 insertions(+), 3 deletions(-) create mode 100644 DemoBenchmarks/ShapePileBenchmark.cs diff --git a/DemoBenchmarks/Program.cs b/DemoBenchmarks/Program.cs index 226ea8019..d64f5ffdc 100644 --- a/DemoBenchmarks/Program.cs +++ b/DemoBenchmarks/Program.cs @@ -11,4 +11,5 @@ BenchmarkRunner.Run(typeof(ShapeRayTests)); BenchmarkRunner.Run(typeof(GatherScatter)); BenchmarkRunner.Run(typeof(RagdollTubeBenchmark)); +BenchmarkRunner.Run(typeof(ShapePileBenchmark)); diff --git a/DemoBenchmarks/RagdollTubeBenchmark.cs b/DemoBenchmarks/RagdollTubeBenchmark.cs index 1bf8f61e6..f81a8247b 100644 --- a/DemoBenchmarks/RagdollTubeBenchmark.cs +++ b/DemoBenchmarks/RagdollTubeBenchmark.cs @@ -12,7 +12,7 @@ namespace DemoBenchmarks; /// -/// Evaluates performance of scatter/gather operations used by constraints to pull body data. +/// Evaluates performance of a simulation similar to the first timesteps of the RagdollTubeDemo. /// public class RagdollTubeBenchmark { diff --git a/DemoBenchmarks/ShapePileBenchmark.cs b/DemoBenchmarks/ShapePileBenchmark.cs new file mode 100644 index 000000000..bac950da2 --- /dev/null +++ b/DemoBenchmarks/ShapePileBenchmark.cs @@ -0,0 +1,231 @@ +using BenchmarkDotNet.Attributes; +using BepuPhysics; +using BepuPhysics.Collidables; +using BepuPhysics.CollisionDetection; +using BepuPhysics.Constraints; +using BepuUtilities; +using BepuUtilities.Collections; +using BepuUtilities.Memory; +using System.Numerics; +using System.Runtime.CompilerServices; + +namespace DemoBenchmarks; + +/// +/// Evaluates performance of a simulation similar to the first timesteps of the ShapePileTestDemo. +/// +public class ShapePileBenchmark +{ + public struct DemoPoseIntegratorCallbacks : IPoseIntegratorCallbacks + { + public Vector3 Gravity; + public float LinearDamping; + public float AngularDamping; + public readonly AngularIntegrationMode AngularIntegrationMode => AngularIntegrationMode.Nonconserving; + public readonly bool AllowSubstepsForUnconstrainedBodies => false; + public readonly bool IntegrateVelocityForKinematics => false; + public void Initialize(Simulation simulation) { } + public DemoPoseIntegratorCallbacks(Vector3 gravity, float linearDamping = .03f, float angularDamping = .03f) : this() + { + Gravity = gravity; + LinearDamping = linearDamping; + AngularDamping = angularDamping; + } + Vector3Wide gravityWideDt; + Vector linearDampingDt; + Vector angularDampingDt; + public void PrepareForIntegration(float dt) + { + linearDampingDt = new Vector(MathF.Pow(MathHelper.Clamp(1 - LinearDamping, 0, 1), dt)); + angularDampingDt = new Vector(MathF.Pow(MathHelper.Clamp(1 - AngularDamping, 0, 1), dt)); + gravityWideDt = Vector3Wide.Broadcast(Gravity * dt); + } + public void IntegrateVelocity(Vector bodyIndices, Vector3Wide position, QuaternionWide orientation, BodyInertiaWide localInertia, Vector integrationMask, int workerIndex, Vector dt, ref BodyVelocityWide velocity) + { + velocity.Linear = (velocity.Linear + gravityWideDt) * linearDampingDt; + velocity.Angular = velocity.Angular * angularDampingDt; + } + } + public unsafe struct DemoNarrowPhaseCallbacks : INarrowPhaseCallbacks + { + public SpringSettings ContactSpringiness; + public float MaximumRecoveryVelocity; + public float FrictionCoefficient; + public DemoNarrowPhaseCallbacks(SpringSettings contactSpringiness, float maximumRecoveryVelocity = 2f, float frictionCoefficient = 1f) + { + ContactSpringiness = contactSpringiness; + MaximumRecoveryVelocity = maximumRecoveryVelocity; + FrictionCoefficient = frictionCoefficient; + } + public void Initialize(Simulation simulation) + { + if (ContactSpringiness.AngularFrequency == 0 && ContactSpringiness.TwiceDampingRatio == 0) + { + ContactSpringiness = new(30, 1); + MaximumRecoveryVelocity = 2f; + FrictionCoefficient = 1f; + } + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin) + { + return a.Mobility == CollidableMobility.Dynamic || b.Mobility == CollidableMobility.Dynamic; + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB) + { + return true; + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public unsafe bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold + { + pairMaterial.FrictionCoefficient = FrictionCoefficient; + pairMaterial.MaximumRecoveryVelocity = MaximumRecoveryVelocity; + pairMaterial.SpringSettings = ContactSpringiness; + return true; + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold) + { + return true; + } + public void Dispose() + { + } + } + + + const int timestepCount = 512; + BufferPool BufferPool; + Simulation Simulation; + + [IterationSetup] + public unsafe void Initialize() + { + BufferPool = new BufferPool(); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(4, 1)); + Simulation.Deterministic = true; + + var sphere = new Sphere(1.5f); + var capsule = new Capsule(1f, 1f); + var box = new Box(1f, 3f, 2f); + var cylinder = new Cylinder(1.5f, 0.3f); + var points = new QuickList(32, BufferPool); + //Boxlike point cloud. + //points.Allocate(BufferPool) = new Vector3(0, 0, 0); + //points.Allocate(BufferPool) = new Vector3(0, 0, 1); + //points.Allocate(BufferPool) = new Vector3(0, 1, 0); + //points.Allocate(BufferPool) = new Vector3(0, 1, 1); + //points.Allocate(BufferPool) = new Vector3(1, 0, 0); + //points.Allocate(BufferPool) = new Vector3(1, 0, 1); + //points.Allocate(BufferPool) = new Vector3(1, 1, 0); + //points.Allocate(BufferPool) = new Vector3(1, 1, 1); + + //Rando pointcloud. + //var random = new Random(5); + //for (int i = 0; i < 32; ++i) + //{ + // points.Allocate(BufferPool) = new Vector3(3 * random.NextSingle(), 1 * random.NextSingle(), 3 * random.NextSingle()); + //} + + //Dodecahedron pointcloud. + points.Allocate(BufferPool) = new Vector3(-1, -1, -1); + points.Allocate(BufferPool) = new Vector3(-1, -1, 1); + points.Allocate(BufferPool) = new Vector3(-1, 1, -1); + points.Allocate(BufferPool) = new Vector3(-1, 1, 1); + points.Allocate(BufferPool) = new Vector3(1, -1, -1); + points.Allocate(BufferPool) = new Vector3(1, -1, 1); + points.Allocate(BufferPool) = new Vector3(1, 1, -1); + points.Allocate(BufferPool) = new Vector3(1, 1, 1); + + const float goldenRatio = 1.618033988749f; + const float oogr = 1f / goldenRatio; + + points.Allocate(BufferPool) = new Vector3(0, goldenRatio, oogr); + points.Allocate(BufferPool) = new Vector3(0, -goldenRatio, oogr); + points.Allocate(BufferPool) = new Vector3(0, goldenRatio, -oogr); + points.Allocate(BufferPool) = new Vector3(0, -goldenRatio, -oogr); + + points.Allocate(BufferPool) = new Vector3(oogr, 0, goldenRatio); + points.Allocate(BufferPool) = new Vector3(oogr, 0, -goldenRatio); + points.Allocate(BufferPool) = new Vector3(-oogr, 0, goldenRatio); + points.Allocate(BufferPool) = new Vector3(-oogr, 0, -goldenRatio); + + points.Allocate(BufferPool) = new Vector3(goldenRatio, oogr, 0); + points.Allocate(BufferPool) = new Vector3(goldenRatio, -oogr, 0); + points.Allocate(BufferPool) = new Vector3(-goldenRatio, oogr, 0); + points.Allocate(BufferPool) = new Vector3(-goldenRatio, -oogr, 0); + + var convexHull = new ConvexHull(points.Span.Slice(points.Count), BufferPool, out _); + var boxInertia = box.ComputeInertia(1); + var capsuleInertia = capsule.ComputeInertia(1); + var sphereInertia = sphere.ComputeInertia(1); + var cylinderInertia = cylinder.ComputeInertia(1); + var hullInertia = convexHull.ComputeInertia(1); + var boxIndex = Simulation.Shapes.Add(box); + var capsuleIndex = Simulation.Shapes.Add(capsule); + var sphereIndex = Simulation.Shapes.Add(sphere); + var cylinderIndex = Simulation.Shapes.Add(cylinder); + var hullIndex = Simulation.Shapes.Add(convexHull); + const int width = 8; + const int height = 8; + const int length = 8; + var shapeCount = 0; + for (int i = 0; i < width; ++i) + { + for (int j = 0; j < height; ++j) + { + for (int k = 0; k < length; ++k) + { + var location = new Vector3(6, 3, 6) * new Vector3(i, j, k) + new Vector3(-width * 1.5f, 5.5f, -length * 1.5f); + var bodyDescription = BodyDescription.CreateKinematic(location, new(default, ContinuousDetection.Passive), 0.01f); + var index = shapeCount++; + switch (index % 5) + { + case 0: + bodyDescription.Collidable.Shape = sphereIndex; + bodyDescription.LocalInertia = sphereInertia; + break; + case 1: + bodyDescription.Collidable.Shape = capsuleIndex; + bodyDescription.LocalInertia = capsuleInertia; + break; + case 2: + bodyDescription.Collidable.Shape = boxIndex; + bodyDescription.LocalInertia = boxInertia; + break; + case 3: + bodyDescription.Collidable.Shape = cylinderIndex; + bodyDescription.LocalInertia = cylinderInertia; + break; + case 4: + bodyDescription.Collidable.Shape = hullIndex; + bodyDescription.LocalInertia = hullInertia; + break; + } + Simulation.Bodies.Add(bodyDescription); + } + } + } + + //Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(new Box(500, 1, 500)))); + BenchmarkHelper.CreateDeformedPlane(128, 128, (x, y) => new Vector3(x - 64, 2f * (float)(Math.Sin(x * 0.5f) * Math.Sin(y * 0.5f)), y - 64), new Vector3(4, 1, 4), BufferPool, out var mesh); + Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); + } + + [IterationCleanup] + public void CleanUp() + { + BufferPool.Clear(); + BufferPool = null; + } + + [Benchmark] + public unsafe void Benchmark() + { + for (int i = 0; i < timestepCount; ++i) + { + Simulation.Timestep(1 / 60f); + } + } +} diff --git a/DemoBenchmarks/Sweeps.cs b/DemoBenchmarks/Sweeps.cs index d3d846027..b1086f96a 100644 --- a/DemoBenchmarks/Sweeps.cs +++ b/DemoBenchmarks/Sweeps.cs @@ -95,12 +95,12 @@ TypedIndex GetRandomShapeTypeIndex(Span cumulative) VelocityA = new BodyVelocity { Linear = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) * 2 - Vector3.One, - Angular = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) * 2 - Vector3.One + Angular = 0.1f * (new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) * 2 - Vector3.One) }, VelocityB = new BodyVelocity { Linear = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) * 2 - Vector3.One, - Angular = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) * 2 - Vector3.One + Angular = 0.1f * (new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) * 2 - Vector3.One) }, SpeculativeMargin = random.NextSingle(), A = GetRandomShapeTypeIndex(cumulative), From 3bb26ac0c8144f956a404733fafcde1db437178e Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 26 Oct 2022 14:00:15 -0500 Subject: [PATCH 551/947] XML errors. --- BepuPhysics/BodyDescription.cs | 2 +- BepuPhysics/CollisionDetection/SweepTaskRegistry.cs | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/BepuPhysics/BodyDescription.cs b/BepuPhysics/BodyDescription.cs index 1a6edf163..c2ef06324 100644 --- a/BepuPhysics/BodyDescription.cs +++ b/BepuPhysics/BodyDescription.cs @@ -39,7 +39,7 @@ public BodyActivityDescription(float sleepThreshold, byte minimumTimestepCountUn /// /// Creates a body activity description. Uses a of 32. /// - /// Threshold of squared velocity under which the body is allowed to go to sleep. This is compared against dot(linearVelocity, linearVelocity) + dot(angularVelocity, angularVelocity). + /// Threshold of squared velocity under which the body is allowed to go to sleep. This is compared against dot(linearVelocity, linearVelocity) + dot(angularVelocity, angularVelocity). /// Note that the body is not guaranteed to go to sleep immediately after meeting this minimum. public static implicit operator BodyActivityDescription(float sleepThreshold) { diff --git a/BepuPhysics/CollisionDetection/SweepTaskRegistry.cs b/BepuPhysics/CollisionDetection/SweepTaskRegistry.cs index f840291ac..6fc3482a1 100644 --- a/BepuPhysics/CollisionDetection/SweepTaskRegistry.cs +++ b/BepuPhysics/CollisionDetection/SweepTaskRegistry.cs @@ -13,7 +13,7 @@ public interface ISweepFilter /// Checks whether a swept test should be performed for children of swept shapes. /// /// Index of the child belonging to collidable A. - /// Index of the child belonging to collidable B. + /// Index of the child belonging to collidable B. /// True if testing should proceed, false otherwise. bool AllowTest(int childA, int childB); } From d312fcf2c9d6c002874c33c5e1696887350184ef Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 26 Oct 2022 14:01:49 -0500 Subject: [PATCH 552/947] Tried a Vector256 fallback. Not maximally optimized; includes extra shuffles. Around 60% slower than the AVX dedicated codepath as is. Going to revisit once I have an actual arm device to test. --- BepuPhysics/Bodies_GatherScatter.cs | 233 +++++++++++++++++++++++++++- 1 file changed, 231 insertions(+), 2 deletions(-) diff --git a/BepuPhysics/Bodies_GatherScatter.cs b/BepuPhysics/Bodies_GatherScatter.cs index 6966f3030..5c6ee0cee 100644 --- a/BepuPhysics/Bodies_GatherScatter.cs +++ b/BepuPhysics/Bodies_GatherScatter.cs @@ -10,6 +10,7 @@ using static BepuUtilities.GatherScatter; using System.Runtime.Intrinsics.X86; using System.Runtime.Intrinsics; +using System.Runtime.Intrinsics.Arm; namespace BepuPhysics { @@ -111,7 +112,7 @@ unsafe static void FallbackGatherInertia(BodyDynamics* states, Vector encod public const int KinematicFlagIndex = 30; public const int KinematicMask = 1 << KinematicFlagIndex; /// - /// Constraint body references greater than a given unsigned value are either kinematic (1<<30 set) or correspond to an empty lane (1<<31 set). + /// Constraint body references greater than a given unsigned value are either kinematic (bit 30 set) or correspond to an empty lane (bit 31 set). /// public const uint DynamicLimit = KinematicMask; public const uint BodyReferenceMetadataMask = (1u << DoesntExistFlagIndex) | KinematicMask; @@ -272,7 +273,235 @@ public unsafe void GatherState(Vector encodedBodyIndices, bo { var solverStates = ActiveSet.DynamicsState.Memory; Unsafe.SkipInit(out TAccessFilter filter); - if (Avx.IsSupported && Vector.Count == 8) + if (Vector256.IsHardwareAccelerated && Vector.Count == 8) + { + var bodyIndices0 = encodedBodyIndices[0]; + var empty0 = bodyIndices0 < 0; + var s0 = (float*)(solverStates + (bodyIndices0 & BodyReferenceMask)); + var bodyIndices1 = encodedBodyIndices[1]; + var empty1 = bodyIndices1 < 0; + var s1 = (float*)(solverStates + (bodyIndices1 & BodyReferenceMask)); + var bodyIndices2 = encodedBodyIndices[2]; + var empty2 = bodyIndices2 < 0; + var s2 = (float*)(solverStates + (bodyIndices2 & BodyReferenceMask)); + var bodyIndices3 = encodedBodyIndices[3]; + var empty3 = bodyIndices3 < 0; + var s3 = (float*)(solverStates + (bodyIndices3 & BodyReferenceMask)); + var bodyIndices4 = encodedBodyIndices[4]; + var empty4 = bodyIndices4 < 0; + var s4 = (float*)(solverStates + (bodyIndices4 & BodyReferenceMask)); + var bodyIndices5 = encodedBodyIndices[5]; + var empty5 = bodyIndices5 < 0; + var s5 = (float*)(solverStates + (bodyIndices5 & BodyReferenceMask)); + var bodyIndices6 = encodedBodyIndices[6]; + var empty6 = bodyIndices6 < 0; + var s6 = (float*)(solverStates + (bodyIndices6 & BodyReferenceMask)); + var bodyIndices7 = encodedBodyIndices[7]; + var empty7 = bodyIndices7 < 0; + var s7 = (float*)(solverStates + (bodyIndices7 & BodyReferenceMask)); + + //for (int i = 0; i < 8; ++i) + //{ + // s0[i] = i; + // s1[i] = i + 100; + // s2[i] = i + 200; + // s3[i] = i + 300; + // s4[i] = i + 400; + // s5[i] = i + 500; + // s6[i] = i + 600; + // s7[i] = i + 700; + //} + + { + //Load every body for the first half of the motion state. + //Note that buffers are allocated on cache line boundaries, so we can use aligned loads for all that matters. + var m0 = empty0 ? Vector256.Zero : Vector256.LoadAligned(s0); + var m1 = empty1 ? Vector256.Zero : Vector256.LoadAligned(s1); + var m2 = empty2 ? Vector256.Zero : Vector256.LoadAligned(s2); + var m3 = empty3 ? Vector256.Zero : Vector256.LoadAligned(s3); + var m4 = empty4 ? Vector256.Zero : Vector256.LoadAligned(s4); + var m5 = empty5 ? Vector256.Zero : Vector256.LoadAligned(s5); + var m6 = empty6 ? Vector256.Zero : Vector256.LoadAligned(s6); + var m7 = empty7 ? Vector256.Zero : Vector256.LoadAligned(s7); + + //m0 = Vector256.Create(00f, 01f, 02f, 03f, 04f, 05f, 06f, 07f); + //m1 = Vector256.Create(10f, 11f, 12f, 13f, 14f, 15f, 16f, 17f); + //m2 = Vector256.Create(20f, 21f, 22f, 23f, 24f, 25f, 26f, 27f); + //m3 = Vector256.Create(30f, 31f, 32f, 33f, 34f, 35f, 36f, 37f); + //m4 = Vector256.Create(40f, 41f, 42f, 43f, 44f, 45f, 46f, 47f); + //m5 = Vector256.Create(50f, 51f, 52f, 53f, 54f, 55f, 56f, 57f); + //m6 = Vector256.Create(60f, 61f, 62f, 63f, 64f, 65f, 66f, 67f); + //m7 = Vector256.Create(70f, 71f, 72f, 73f, 74f, 75f, 76f, 77f); + + //Shuffle indices of one vector into 10325476, then blend with its neighbor to get interleaved 00224466 and 11335577. + var interleaveBlendMask = Vector256.Create(-1, 0, -1, 0, -1, 0, -1, 0).AsSingle(); + var shuffledM0 = Vector256.Shuffle(m0, Vector256.Create(1, 0, 3, 2, 5, 4, 7, 6)); + var evens01 = Vector256.ConditionalSelect(interleaveBlendMask, m1, shuffledM0); + var odds01 = Vector256.ConditionalSelect(interleaveBlendMask, shuffledM0, m1); + var shuffledM2 = Vector256.Shuffle(m2, Vector256.Create(1, 0, 3, 2, 5, 4, 7, 6)); + var evens23 = Vector256.ConditionalSelect(interleaveBlendMask, m3, shuffledM2); + var odds23 = Vector256.ConditionalSelect(interleaveBlendMask, shuffledM2, m3); + var shuffledM4 = Vector256.Shuffle(m4, Vector256.Create(1, 0, 3, 2, 5, 4, 7, 6)); + var evens45 = Vector256.ConditionalSelect(interleaveBlendMask, m5, shuffledM4); + var odds45 = Vector256.ConditionalSelect(interleaveBlendMask, shuffledM4, m5); + var shuffledM6 = Vector256.Shuffle(m6, Vector256.Create(1, 0, 3, 2, 5, 4, 7, 6)); + var evens67 = Vector256.ConditionalSelect(interleaveBlendMask, m7, shuffledM6); + var odds67 = Vector256.ConditionalSelect(interleaveBlendMask, shuffledM6, m7); + + //We have a couple of vectors of 00224466, we want 00004444 and 22226666. Likewise, 11335577 -> 11115555 and 33337777. + //Same idea as before: shuffle one of the two contributors and blend. + var interleaveBlendMask2 = Vector256.Create(0, 0, -1, -1, 0, 0, -1, -1).AsSingle(); + var shuffledEvens01 = Vector256.Shuffle(evens01, Vector256.Create(2, 3, 0, 1, 6, 7, 4, 5)); + var v22226666from1032 = Vector256.ConditionalSelect(interleaveBlendMask2, evens23, shuffledEvens01); + var v00004444from3210 = Vector256.ConditionalSelect(interleaveBlendMask2, shuffledEvens01, evens23); + var shuffledEvens45 = Vector256.Shuffle(evens45, Vector256.Create(2, 3, 0, 1, 6, 7, 4, 5)); + var v22226666from5476 = Vector256.ConditionalSelect(interleaveBlendMask2, evens67, shuffledEvens45); + var v00004444from7654 = Vector256.ConditionalSelect(interleaveBlendMask2, shuffledEvens45, evens67); + + var shuffledOdds01 = Vector256.Shuffle(odds01, Vector256.Create(2, 3, 0, 1, 6, 7, 4, 5)); + var v33337777from1032 = Vector256.ConditionalSelect(interleaveBlendMask2, odds23, shuffledOdds01); + var v11115555from3210 = Vector256.ConditionalSelect(interleaveBlendMask2, shuffledOdds01, odds23); + var shuffledOdds45 = Vector256.Shuffle(odds45, Vector256.Create(2, 3, 0, 1, 6, 7, 4, 5)); + var v33337777from4567 = Vector256.ConditionalSelect(interleaveBlendMask2, odds67, shuffledOdds45); + var v11115555from6745 = Vector256.ConditionalSelect(interleaveBlendMask2, shuffledOdds45, odds67); + + if (filter.GatherOrientation) + { + var v0from32107654 = Vector256.WithUpper(v00004444from3210, Vector256.GetLower(v00004444from7654)); + var v1from23016745 = Vector256.WithUpper(v11115555from3210, Vector256.GetLower(v11115555from6745)); + var v2from10325476 = Vector256.WithUpper(v22226666from1032, Vector256.GetLower(v22226666from5476)); + var v3from01234567 = Vector256.WithUpper(v33337777from1032, Vector256.GetLower(v33337777from4567)); + orientation.X = Vector256.Shuffle(v0from32107654, Vector256.Create(3, 2, 1, 0, 7, 6, 5, 4)).AsVector(); + orientation.Y = Vector256.Shuffle(v1from23016745, Vector256.Create(2, 3, 0, 1, 6, 7, 4, 5)).AsVector(); + orientation.Z = Vector256.Shuffle(v2from10325476, Vector256.Create(1, 0, 3, 2, 5, 4, 7, 6)).AsVector(); + orientation.W = v3from01234567.AsVector(); + } + else + { + Unsafe.SkipInit(out orientation); + } + if (filter.GatherPosition) + { + var v4from32107654 = Vector256.WithLower(v00004444from7654, Vector256.GetUpper(v00004444from3210)); + var v5from23016745 = Vector256.WithLower(v11115555from6745, Vector256.GetUpper(v11115555from3210)); + var v6from10325476 = Vector256.WithLower(v22226666from5476, Vector256.GetUpper(v22226666from1032)); + //var v7from10324567 = Vector256.WithLower(v33337777from4567, Vector256.GetUpper(v33337777from1032); + position.X = Vector256.Shuffle(v4from32107654, Vector256.Create(3, 2, 1, 0, 7, 6, 5, 4)).AsVector(); + position.Y = Vector256.Shuffle(v5from23016745, Vector256.Create(2, 3, 0, 1, 6, 7, 4, 5)).AsVector(); + position.Z = Vector256.Shuffle(v6from10325476, Vector256.Create(1, 0, 3, 2, 5, 4, 7, 6)).AsVector(); + } + else + { + Unsafe.SkipInit(out position); + } + } + + { + //Second half. + var m0 = empty0 ? Vector256.Zero : Avx.LoadAlignedVector256(s0 + 8); + var m1 = empty1 ? Vector256.Zero : Avx.LoadAlignedVector256(s1 + 8); + var m2 = empty2 ? Vector256.Zero : Avx.LoadAlignedVector256(s2 + 8); + var m3 = empty3 ? Vector256.Zero : Avx.LoadAlignedVector256(s3 + 8); + var m4 = empty4 ? Vector256.Zero : Avx.LoadAlignedVector256(s4 + 8); + var m5 = empty5 ? Vector256.Zero : Avx.LoadAlignedVector256(s5 + 8); + var m6 = empty6 ? Vector256.Zero : Avx.LoadAlignedVector256(s6 + 8); + var m7 = empty7 ? Vector256.Zero : Avx.LoadAlignedVector256(s7 + 8); + + var n0 = Avx.UnpackLow(m0, m1); + var n1 = Avx.UnpackLow(m2, m3); + var n2 = Avx.UnpackLow(m4, m5); + var n3 = Avx.UnpackLow(m6, m7); + var n4 = Avx.UnpackHigh(m0, m1); + var n5 = Avx.UnpackHigh(m2, m3); + var n6 = Avx.UnpackHigh(m4, m5); + var n7 = Avx.UnpackHigh(m6, m7); + + var o0 = Avx.Shuffle(n0, n1, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o1 = Avx.Shuffle(n2, n3, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o2 = Avx.Shuffle(n4, n5, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o3 = Avx.Shuffle(n6, n7, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o4 = Avx.Shuffle(n0, n1, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o5 = Avx.Shuffle(n2, n3, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + + if (filter.AccessLinearVelocity) + { + velocity.Linear.X = Avx.Permute2x128(o0, o1, 0 | (2 << 4)).AsVector(); + velocity.Linear.Y = Avx.Permute2x128(o4, o5, 0 | (2 << 4)).AsVector(); + velocity.Linear.Z = Avx.Permute2x128(o2, o3, 0 | (2 << 4)).AsVector(); + } + else + { + Unsafe.SkipInit(out velocity.Linear); + } + if (filter.AccessAngularVelocity) + { + velocity.Angular.X = Avx.Permute2x128(o0, o1, 1 | (3 << 4)).AsVector(); + velocity.Angular.Y = Avx.Permute2x128(o4, o5, 1 | (3 << 4)).AsVector(); + velocity.Angular.Z = Avx.Permute2x128(o2, o3, 1 | (3 << 4)).AsVector(); + } + else + { + Unsafe.SkipInit(out velocity.Angular); + } + } + + { + var offsetInFloats = worldInertia ? 24 : 16; + + //Load every inertia vector. + //Assuming here that most bodies are dynamic, so it's not worth the extra work extracting a dynamic-only mask to avoid loading some kinematic zeroes. + var m0 = empty0 ? Vector256.Zero : Avx.LoadAlignedVector256(s0 + offsetInFloats); + var m1 = empty1 ? Vector256.Zero : Avx.LoadAlignedVector256(s1 + offsetInFloats); + var m2 = empty2 ? Vector256.Zero : Avx.LoadAlignedVector256(s2 + offsetInFloats); + var m3 = empty3 ? Vector256.Zero : Avx.LoadAlignedVector256(s3 + offsetInFloats); + var m4 = empty4 ? Vector256.Zero : Avx.LoadAlignedVector256(s4 + offsetInFloats); + var m5 = empty5 ? Vector256.Zero : Avx.LoadAlignedVector256(s5 + offsetInFloats); + var m6 = empty6 ? Vector256.Zero : Avx.LoadAlignedVector256(s6 + offsetInFloats); + var m7 = empty7 ? Vector256.Zero : Avx.LoadAlignedVector256(s7 + offsetInFloats); + + var n0 = Avx.UnpackLow(m0, m1); + var n1 = Avx.UnpackLow(m2, m3); + var n2 = Avx.UnpackLow(m4, m5); + var n3 = Avx.UnpackLow(m6, m7); + var n4 = Avx.UnpackHigh(m0, m1); + var n5 = Avx.UnpackHigh(m2, m3); + var n6 = Avx.UnpackHigh(m4, m5); + var n7 = Avx.UnpackHigh(m6, m7); + + var o0 = Avx.Shuffle(n0, n1, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o1 = Avx.Shuffle(n2, n3, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o2 = Avx.Shuffle(n4, n5, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o3 = Avx.Shuffle(n6, n7, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o4 = Avx.Shuffle(n0, n1, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o5 = Avx.Shuffle(n2, n3, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o6 = Avx.Shuffle(n4, n5, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o7 = Avx.Shuffle(n6, n7, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + + if (filter.GatherInertiaTensor) + { + inertia.InverseInertiaTensor.XX = Avx.Permute2x128(o0, o1, 0 | (2 << 4)).AsVector(); + inertia.InverseInertiaTensor.YX = Avx.Permute2x128(o4, o5, 0 | (2 << 4)).AsVector(); + inertia.InverseInertiaTensor.YY = Avx.Permute2x128(o2, o3, 0 | (2 << 4)).AsVector(); + inertia.InverseInertiaTensor.ZX = Avx.Permute2x128(o6, o7, 0 | (2 << 4)).AsVector(); + inertia.InverseInertiaTensor.ZY = Avx.Permute2x128(o0, o1, 1 | (3 << 4)).AsVector(); + inertia.InverseInertiaTensor.ZZ = Avx.Permute2x128(o4, o5, 1 | (3 << 4)).AsVector(); + } + else + { + Unsafe.SkipInit(out inertia.InverseInertiaTensor); + } + if (filter.GatherMass) + { + inertia.InverseMass = Avx.Permute2x128(o2, o3, 1 | (3 << 4)).AsVector(); + } + else + { + Unsafe.SkipInit(out inertia.InverseMass); + } + } + + } + else if (Avx.IsSupported && Vector.Count == 8) { var bodyIndices0 = encodedBodyIndices[0]; var empty0 = bodyIndices0 < 0; From d08374139c514220249355974a52607362ee3e3f Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 26 Oct 2022 14:03:36 -0500 Subject: [PATCH 553/947] Revert. --- BepuPhysics/Bodies_GatherScatter.cs | 230 +--------------------------- 1 file changed, 1 insertion(+), 229 deletions(-) diff --git a/BepuPhysics/Bodies_GatherScatter.cs b/BepuPhysics/Bodies_GatherScatter.cs index 5c6ee0cee..0b57f5947 100644 --- a/BepuPhysics/Bodies_GatherScatter.cs +++ b/BepuPhysics/Bodies_GatherScatter.cs @@ -273,235 +273,7 @@ public unsafe void GatherState(Vector encodedBodyIndices, bo { var solverStates = ActiveSet.DynamicsState.Memory; Unsafe.SkipInit(out TAccessFilter filter); - if (Vector256.IsHardwareAccelerated && Vector.Count == 8) - { - var bodyIndices0 = encodedBodyIndices[0]; - var empty0 = bodyIndices0 < 0; - var s0 = (float*)(solverStates + (bodyIndices0 & BodyReferenceMask)); - var bodyIndices1 = encodedBodyIndices[1]; - var empty1 = bodyIndices1 < 0; - var s1 = (float*)(solverStates + (bodyIndices1 & BodyReferenceMask)); - var bodyIndices2 = encodedBodyIndices[2]; - var empty2 = bodyIndices2 < 0; - var s2 = (float*)(solverStates + (bodyIndices2 & BodyReferenceMask)); - var bodyIndices3 = encodedBodyIndices[3]; - var empty3 = bodyIndices3 < 0; - var s3 = (float*)(solverStates + (bodyIndices3 & BodyReferenceMask)); - var bodyIndices4 = encodedBodyIndices[4]; - var empty4 = bodyIndices4 < 0; - var s4 = (float*)(solverStates + (bodyIndices4 & BodyReferenceMask)); - var bodyIndices5 = encodedBodyIndices[5]; - var empty5 = bodyIndices5 < 0; - var s5 = (float*)(solverStates + (bodyIndices5 & BodyReferenceMask)); - var bodyIndices6 = encodedBodyIndices[6]; - var empty6 = bodyIndices6 < 0; - var s6 = (float*)(solverStates + (bodyIndices6 & BodyReferenceMask)); - var bodyIndices7 = encodedBodyIndices[7]; - var empty7 = bodyIndices7 < 0; - var s7 = (float*)(solverStates + (bodyIndices7 & BodyReferenceMask)); - - //for (int i = 0; i < 8; ++i) - //{ - // s0[i] = i; - // s1[i] = i + 100; - // s2[i] = i + 200; - // s3[i] = i + 300; - // s4[i] = i + 400; - // s5[i] = i + 500; - // s6[i] = i + 600; - // s7[i] = i + 700; - //} - - { - //Load every body for the first half of the motion state. - //Note that buffers are allocated on cache line boundaries, so we can use aligned loads for all that matters. - var m0 = empty0 ? Vector256.Zero : Vector256.LoadAligned(s0); - var m1 = empty1 ? Vector256.Zero : Vector256.LoadAligned(s1); - var m2 = empty2 ? Vector256.Zero : Vector256.LoadAligned(s2); - var m3 = empty3 ? Vector256.Zero : Vector256.LoadAligned(s3); - var m4 = empty4 ? Vector256.Zero : Vector256.LoadAligned(s4); - var m5 = empty5 ? Vector256.Zero : Vector256.LoadAligned(s5); - var m6 = empty6 ? Vector256.Zero : Vector256.LoadAligned(s6); - var m7 = empty7 ? Vector256.Zero : Vector256.LoadAligned(s7); - - //m0 = Vector256.Create(00f, 01f, 02f, 03f, 04f, 05f, 06f, 07f); - //m1 = Vector256.Create(10f, 11f, 12f, 13f, 14f, 15f, 16f, 17f); - //m2 = Vector256.Create(20f, 21f, 22f, 23f, 24f, 25f, 26f, 27f); - //m3 = Vector256.Create(30f, 31f, 32f, 33f, 34f, 35f, 36f, 37f); - //m4 = Vector256.Create(40f, 41f, 42f, 43f, 44f, 45f, 46f, 47f); - //m5 = Vector256.Create(50f, 51f, 52f, 53f, 54f, 55f, 56f, 57f); - //m6 = Vector256.Create(60f, 61f, 62f, 63f, 64f, 65f, 66f, 67f); - //m7 = Vector256.Create(70f, 71f, 72f, 73f, 74f, 75f, 76f, 77f); - - //Shuffle indices of one vector into 10325476, then blend with its neighbor to get interleaved 00224466 and 11335577. - var interleaveBlendMask = Vector256.Create(-1, 0, -1, 0, -1, 0, -1, 0).AsSingle(); - var shuffledM0 = Vector256.Shuffle(m0, Vector256.Create(1, 0, 3, 2, 5, 4, 7, 6)); - var evens01 = Vector256.ConditionalSelect(interleaveBlendMask, m1, shuffledM0); - var odds01 = Vector256.ConditionalSelect(interleaveBlendMask, shuffledM0, m1); - var shuffledM2 = Vector256.Shuffle(m2, Vector256.Create(1, 0, 3, 2, 5, 4, 7, 6)); - var evens23 = Vector256.ConditionalSelect(interleaveBlendMask, m3, shuffledM2); - var odds23 = Vector256.ConditionalSelect(interleaveBlendMask, shuffledM2, m3); - var shuffledM4 = Vector256.Shuffle(m4, Vector256.Create(1, 0, 3, 2, 5, 4, 7, 6)); - var evens45 = Vector256.ConditionalSelect(interleaveBlendMask, m5, shuffledM4); - var odds45 = Vector256.ConditionalSelect(interleaveBlendMask, shuffledM4, m5); - var shuffledM6 = Vector256.Shuffle(m6, Vector256.Create(1, 0, 3, 2, 5, 4, 7, 6)); - var evens67 = Vector256.ConditionalSelect(interleaveBlendMask, m7, shuffledM6); - var odds67 = Vector256.ConditionalSelect(interleaveBlendMask, shuffledM6, m7); - - //We have a couple of vectors of 00224466, we want 00004444 and 22226666. Likewise, 11335577 -> 11115555 and 33337777. - //Same idea as before: shuffle one of the two contributors and blend. - var interleaveBlendMask2 = Vector256.Create(0, 0, -1, -1, 0, 0, -1, -1).AsSingle(); - var shuffledEvens01 = Vector256.Shuffle(evens01, Vector256.Create(2, 3, 0, 1, 6, 7, 4, 5)); - var v22226666from1032 = Vector256.ConditionalSelect(interleaveBlendMask2, evens23, shuffledEvens01); - var v00004444from3210 = Vector256.ConditionalSelect(interleaveBlendMask2, shuffledEvens01, evens23); - var shuffledEvens45 = Vector256.Shuffle(evens45, Vector256.Create(2, 3, 0, 1, 6, 7, 4, 5)); - var v22226666from5476 = Vector256.ConditionalSelect(interleaveBlendMask2, evens67, shuffledEvens45); - var v00004444from7654 = Vector256.ConditionalSelect(interleaveBlendMask2, shuffledEvens45, evens67); - - var shuffledOdds01 = Vector256.Shuffle(odds01, Vector256.Create(2, 3, 0, 1, 6, 7, 4, 5)); - var v33337777from1032 = Vector256.ConditionalSelect(interleaveBlendMask2, odds23, shuffledOdds01); - var v11115555from3210 = Vector256.ConditionalSelect(interleaveBlendMask2, shuffledOdds01, odds23); - var shuffledOdds45 = Vector256.Shuffle(odds45, Vector256.Create(2, 3, 0, 1, 6, 7, 4, 5)); - var v33337777from4567 = Vector256.ConditionalSelect(interleaveBlendMask2, odds67, shuffledOdds45); - var v11115555from6745 = Vector256.ConditionalSelect(interleaveBlendMask2, shuffledOdds45, odds67); - - if (filter.GatherOrientation) - { - var v0from32107654 = Vector256.WithUpper(v00004444from3210, Vector256.GetLower(v00004444from7654)); - var v1from23016745 = Vector256.WithUpper(v11115555from3210, Vector256.GetLower(v11115555from6745)); - var v2from10325476 = Vector256.WithUpper(v22226666from1032, Vector256.GetLower(v22226666from5476)); - var v3from01234567 = Vector256.WithUpper(v33337777from1032, Vector256.GetLower(v33337777from4567)); - orientation.X = Vector256.Shuffle(v0from32107654, Vector256.Create(3, 2, 1, 0, 7, 6, 5, 4)).AsVector(); - orientation.Y = Vector256.Shuffle(v1from23016745, Vector256.Create(2, 3, 0, 1, 6, 7, 4, 5)).AsVector(); - orientation.Z = Vector256.Shuffle(v2from10325476, Vector256.Create(1, 0, 3, 2, 5, 4, 7, 6)).AsVector(); - orientation.W = v3from01234567.AsVector(); - } - else - { - Unsafe.SkipInit(out orientation); - } - if (filter.GatherPosition) - { - var v4from32107654 = Vector256.WithLower(v00004444from7654, Vector256.GetUpper(v00004444from3210)); - var v5from23016745 = Vector256.WithLower(v11115555from6745, Vector256.GetUpper(v11115555from3210)); - var v6from10325476 = Vector256.WithLower(v22226666from5476, Vector256.GetUpper(v22226666from1032)); - //var v7from10324567 = Vector256.WithLower(v33337777from4567, Vector256.GetUpper(v33337777from1032); - position.X = Vector256.Shuffle(v4from32107654, Vector256.Create(3, 2, 1, 0, 7, 6, 5, 4)).AsVector(); - position.Y = Vector256.Shuffle(v5from23016745, Vector256.Create(2, 3, 0, 1, 6, 7, 4, 5)).AsVector(); - position.Z = Vector256.Shuffle(v6from10325476, Vector256.Create(1, 0, 3, 2, 5, 4, 7, 6)).AsVector(); - } - else - { - Unsafe.SkipInit(out position); - } - } - - { - //Second half. - var m0 = empty0 ? Vector256.Zero : Avx.LoadAlignedVector256(s0 + 8); - var m1 = empty1 ? Vector256.Zero : Avx.LoadAlignedVector256(s1 + 8); - var m2 = empty2 ? Vector256.Zero : Avx.LoadAlignedVector256(s2 + 8); - var m3 = empty3 ? Vector256.Zero : Avx.LoadAlignedVector256(s3 + 8); - var m4 = empty4 ? Vector256.Zero : Avx.LoadAlignedVector256(s4 + 8); - var m5 = empty5 ? Vector256.Zero : Avx.LoadAlignedVector256(s5 + 8); - var m6 = empty6 ? Vector256.Zero : Avx.LoadAlignedVector256(s6 + 8); - var m7 = empty7 ? Vector256.Zero : Avx.LoadAlignedVector256(s7 + 8); - - var n0 = Avx.UnpackLow(m0, m1); - var n1 = Avx.UnpackLow(m2, m3); - var n2 = Avx.UnpackLow(m4, m5); - var n3 = Avx.UnpackLow(m6, m7); - var n4 = Avx.UnpackHigh(m0, m1); - var n5 = Avx.UnpackHigh(m2, m3); - var n6 = Avx.UnpackHigh(m4, m5); - var n7 = Avx.UnpackHigh(m6, m7); - - var o0 = Avx.Shuffle(n0, n1, 0 | (1 << 2) | (0 << 4) | (1 << 6)); - var o1 = Avx.Shuffle(n2, n3, 0 | (1 << 2) | (0 << 4) | (1 << 6)); - var o2 = Avx.Shuffle(n4, n5, 0 | (1 << 2) | (0 << 4) | (1 << 6)); - var o3 = Avx.Shuffle(n6, n7, 0 | (1 << 2) | (0 << 4) | (1 << 6)); - var o4 = Avx.Shuffle(n0, n1, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - var o5 = Avx.Shuffle(n2, n3, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - - if (filter.AccessLinearVelocity) - { - velocity.Linear.X = Avx.Permute2x128(o0, o1, 0 | (2 << 4)).AsVector(); - velocity.Linear.Y = Avx.Permute2x128(o4, o5, 0 | (2 << 4)).AsVector(); - velocity.Linear.Z = Avx.Permute2x128(o2, o3, 0 | (2 << 4)).AsVector(); - } - else - { - Unsafe.SkipInit(out velocity.Linear); - } - if (filter.AccessAngularVelocity) - { - velocity.Angular.X = Avx.Permute2x128(o0, o1, 1 | (3 << 4)).AsVector(); - velocity.Angular.Y = Avx.Permute2x128(o4, o5, 1 | (3 << 4)).AsVector(); - velocity.Angular.Z = Avx.Permute2x128(o2, o3, 1 | (3 << 4)).AsVector(); - } - else - { - Unsafe.SkipInit(out velocity.Angular); - } - } - - { - var offsetInFloats = worldInertia ? 24 : 16; - - //Load every inertia vector. - //Assuming here that most bodies are dynamic, so it's not worth the extra work extracting a dynamic-only mask to avoid loading some kinematic zeroes. - var m0 = empty0 ? Vector256.Zero : Avx.LoadAlignedVector256(s0 + offsetInFloats); - var m1 = empty1 ? Vector256.Zero : Avx.LoadAlignedVector256(s1 + offsetInFloats); - var m2 = empty2 ? Vector256.Zero : Avx.LoadAlignedVector256(s2 + offsetInFloats); - var m3 = empty3 ? Vector256.Zero : Avx.LoadAlignedVector256(s3 + offsetInFloats); - var m4 = empty4 ? Vector256.Zero : Avx.LoadAlignedVector256(s4 + offsetInFloats); - var m5 = empty5 ? Vector256.Zero : Avx.LoadAlignedVector256(s5 + offsetInFloats); - var m6 = empty6 ? Vector256.Zero : Avx.LoadAlignedVector256(s6 + offsetInFloats); - var m7 = empty7 ? Vector256.Zero : Avx.LoadAlignedVector256(s7 + offsetInFloats); - - var n0 = Avx.UnpackLow(m0, m1); - var n1 = Avx.UnpackLow(m2, m3); - var n2 = Avx.UnpackLow(m4, m5); - var n3 = Avx.UnpackLow(m6, m7); - var n4 = Avx.UnpackHigh(m0, m1); - var n5 = Avx.UnpackHigh(m2, m3); - var n6 = Avx.UnpackHigh(m4, m5); - var n7 = Avx.UnpackHigh(m6, m7); - - var o0 = Avx.Shuffle(n0, n1, 0 | (1 << 2) | (0 << 4) | (1 << 6)); - var o1 = Avx.Shuffle(n2, n3, 0 | (1 << 2) | (0 << 4) | (1 << 6)); - var o2 = Avx.Shuffle(n4, n5, 0 | (1 << 2) | (0 << 4) | (1 << 6)); - var o3 = Avx.Shuffle(n6, n7, 0 | (1 << 2) | (0 << 4) | (1 << 6)); - var o4 = Avx.Shuffle(n0, n1, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - var o5 = Avx.Shuffle(n2, n3, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - var o6 = Avx.Shuffle(n4, n5, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - var o7 = Avx.Shuffle(n6, n7, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - - if (filter.GatherInertiaTensor) - { - inertia.InverseInertiaTensor.XX = Avx.Permute2x128(o0, o1, 0 | (2 << 4)).AsVector(); - inertia.InverseInertiaTensor.YX = Avx.Permute2x128(o4, o5, 0 | (2 << 4)).AsVector(); - inertia.InverseInertiaTensor.YY = Avx.Permute2x128(o2, o3, 0 | (2 << 4)).AsVector(); - inertia.InverseInertiaTensor.ZX = Avx.Permute2x128(o6, o7, 0 | (2 << 4)).AsVector(); - inertia.InverseInertiaTensor.ZY = Avx.Permute2x128(o0, o1, 1 | (3 << 4)).AsVector(); - inertia.InverseInertiaTensor.ZZ = Avx.Permute2x128(o4, o5, 1 | (3 << 4)).AsVector(); - } - else - { - Unsafe.SkipInit(out inertia.InverseInertiaTensor); - } - if (filter.GatherMass) - { - inertia.InverseMass = Avx.Permute2x128(o2, o3, 1 | (3 << 4)).AsVector(); - } - else - { - Unsafe.SkipInit(out inertia.InverseMass); - } - } - - } - else if (Avx.IsSupported && Vector.Count == 8) + if (Avx.IsSupported && Vector.Count == 8) { var bodyIndices0 = encodedBodyIndices[0]; var empty0 = bodyIndices0 < 0; From e9af2eb2a2e9455710bdaaf866626b5218312e3e Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 26 Oct 2022 14:12:44 -0500 Subject: [PATCH 554/947] Bumped version number. --- BepuPhysics/BepuPhysics.csproj | 2 +- BepuUtilities/BepuUtilities.csproj | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/BepuPhysics/BepuPhysics.csproj b/BepuPhysics/BepuPhysics.csproj index c7efb1b4e..98424711c 100644 --- a/BepuPhysics/BepuPhysics.csproj +++ b/BepuPhysics/BepuPhysics.csproj @@ -1,7 +1,7 @@  net7.0 - 2.5.0-beta.8 + 2.5.0-beta.9 Bepu Entertainment LLC Ross Nordby Speedy real time physics simulation library. diff --git a/BepuUtilities/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj index da947baf9..af406f064 100644 --- a/BepuUtilities/BepuUtilities.csproj +++ b/BepuUtilities/BepuUtilities.csproj @@ -3,7 +3,7 @@ BepuUtilities BepuUtilities net7.0 - 2.5.0-beta.8 + 2.5.0-beta.9 Bepu Entertainment LLC Ross Nordby Supporting utilities library for BEPUphysics v2. From 2373f40e6e6ef9bb5c84aa2d7295c46b59b9cc0f Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 26 Oct 2022 15:35:15 -0500 Subject: [PATCH 555/947] Benchmarks readme. --- DemoBenchmarks/README.md | 29 +++++++++++++++++++++++++++++ 1 file changed, 29 insertions(+) create mode 100644 DemoBenchmarks/README.md diff --git a/DemoBenchmarks/README.md b/DemoBenchmarks/README.md new file mode 100644 index 000000000..d56c99e10 --- /dev/null +++ b/DemoBenchmarks/README.md @@ -0,0 +1,29 @@ +# Benchmarks + +This project contains a variety of performance tests for different pieces of the library. + +The tests sometimes cover different scopes. Most are microbenchmarks. Benchmarks in the following classes cover specific codepaths, like a single collision pair tester: + +[ConvexCollisionTesters](ConvexCollisionTesters.cs): Contact manifold generating convex collision tests, one benchmark per type pair. + +[OneBodyConstraints](OneBodyConstraints.cs): One body constraints, one benchmark per type. + +[TwoBodyConstraints](TwoBodyConstraints.cs): Two body constraints, one benchmark per type. + +[ThreeBodyConstraints](ThreeBodyConstraints.cs): Three body constraints, one benchmark per type. + +[FourBodyConstraints](FourBodyConstraints.cs): Four body constraints, one benchmark per type. + +[ShapeRayTests](ShapeRayTests.cs): Shape-ray tests, one benchmark per type. + +[Sweeps](Sweeps.cs): Linear + angular sweep tests for shape pairs, one benchmark per type pair. + +[GatherScatter](GatherScatter.cs): Tests SIMD body data gathering/scattering for constraint solving. One benchmark for each gather/scatter operation. + +Other tests cover more codepaths at once: + +[CollisionBatcherTasks](CollisionBatcherTasks.cs): A single benchmark testing all shape pair types (including nonconvex pairs) in the `CollisionBatcher`. Pairs are dynamically batched and dispatched across a loop. + +[RagdollTubeBenchmark](RagdollTubeBenchmark.cs): Simpler version of the `RagdollTubeDemo`, testing a bunch of ragdolls in a tumblertube. One execution of the benchmark covers many timesteps of simulation. This covers several collision pairs, many constraint types, and the rest of the simulation code gluing things together. + +[ShapePileBenchmark](ShapePileBenchmark.cs): Another full simulation benchmark, this time a simpler version of the `ShapePileTestDemo`. Compared to the RagdollTubeDemo, this focuses less on constraints, but includes more collision types. From 423fc03f31215634153c9094d707c0a75dc07771 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 27 Oct 2022 14:12:22 -0500 Subject: [PATCH 556/947] Demos.GL works again. --- DemoRenderer.GL/DemoRenderer.csproj | 6 +++--- DemoRenderer.GL/ShapeDrawing/ShapesExtractor.cs | 6 ++++-- Demos.GL/Demos.csproj | 6 +++--- 3 files changed, 10 insertions(+), 8 deletions(-) diff --git a/DemoRenderer.GL/DemoRenderer.csproj b/DemoRenderer.GL/DemoRenderer.csproj index 8f42e7fa0..74a20afbf 100644 --- a/DemoRenderer.GL/DemoRenderer.csproj +++ b/DemoRenderer.GL/DemoRenderer.csproj @@ -1,13 +1,13 @@  - net6.0 + net7.0 latest True - - + + diff --git a/DemoRenderer.GL/ShapeDrawing/ShapesExtractor.cs b/DemoRenderer.GL/ShapeDrawing/ShapesExtractor.cs index 450515338..72d444212 100644 --- a/DemoRenderer.GL/ShapeDrawing/ShapesExtractor.cs +++ b/DemoRenderer.GL/ShapeDrawing/ShapesExtractor.cs @@ -73,7 +73,9 @@ private unsafe void AddCompoundChildren(ref Buffer children, Shap for (int i = 0; i < children.Length; ++i) { ref var child = ref children[i]; - Compound.GetWorldPose(child.LocalPose, pose, out var childPose); + RigidPose childPose; + Compound.GetRotatedChildPose(child.LocalPosition, child.LocalOrientation, pose.Orientation, out childPose.Position, out childPose.Orientation); + childPose.Position += pose.Position; AddShape(shapes, child.ShapeIndex, childPose, color, ref shapeCache, pool); } } @@ -290,7 +292,7 @@ void AddBodyShape(Shapes shapes, Bodies bodies, int setIndex, int indexInSet, re ref var activity = ref set.Activity[indexInSet]; Vector3 color; Helpers.UnpackColor((uint)HashHelper.Rehash(handle.Value), out Vector3 colorVariation); - ref var state = ref set.SolverStates[indexInSet]; + ref var state = ref set.DynamicsState[indexInSet]; if (Bodies.IsKinematic(state.Inertia.Local)) { var kinematicBase = new Vector3(0, 0.609f, 0.37f); diff --git a/Demos.GL/Demos.csproj b/Demos.GL/Demos.csproj index c905ce396..75af1539d 100644 --- a/Demos.GL/Demos.csproj +++ b/Demos.GL/Demos.csproj @@ -1,7 +1,7 @@  Exe - net6.0 + net7.0 True Debug;Release latest @@ -9,8 +9,8 @@ - - + + From 810eaa5a1ddd987b37680b270c633242dc754e92 Mon Sep 17 00:00:00 2001 From: Kunal Pathak Date: Fri, 28 Oct 2022 08:10:32 -0700 Subject: [PATCH 557/947] Ability to pass command line args to BDN - This change uses `BenchmarkSwitcher` to pass the arguments to BDN so the `--filter` or `--help` can work - It also updates the name of certain benchmarks to easily identify them in the report. --- DemoBenchmarks/CollisionBatcherTasks.cs | 2 +- DemoBenchmarks/DemoBenchmarks.csproj | 1 - DemoBenchmarks/Program.cs | 19 +++++++------------ DemoBenchmarks/RagdollTubeBenchmark.cs | 2 +- DemoBenchmarks/ShapePileBenchmark.cs | 2 +- 5 files changed, 10 insertions(+), 16 deletions(-) diff --git a/DemoBenchmarks/CollisionBatcherTasks.cs b/DemoBenchmarks/CollisionBatcherTasks.cs index 6cb4fc0d5..ae7bb3973 100644 --- a/DemoBenchmarks/CollisionBatcherTasks.cs +++ b/DemoBenchmarks/CollisionBatcherTasks.cs @@ -139,7 +139,7 @@ public void OnPairCompleted(int pairId, ref TManifold manifold) where } [Benchmark] - public unsafe Vector3 Benchmark() + public unsafe Vector3 CollisionBatcherTasksBenchmark() { CollisionBatcher batcher = new(pool, shapes, taskRegistry, 1 / 60f, new Callbacks()); diff --git a/DemoBenchmarks/DemoBenchmarks.csproj b/DemoBenchmarks/DemoBenchmarks.csproj index 8d329413f..3fb0c0067 100644 --- a/DemoBenchmarks/DemoBenchmarks.csproj +++ b/DemoBenchmarks/DemoBenchmarks.csproj @@ -4,7 +4,6 @@ Exe net7.0 enable - enable true diff --git a/DemoBenchmarks/Program.cs b/DemoBenchmarks/Program.cs index d64f5ffdc..59e457681 100644 --- a/DemoBenchmarks/Program.cs +++ b/DemoBenchmarks/Program.cs @@ -1,15 +1,10 @@ using BenchmarkDotNet.Running; using DemoBenchmarks; -BenchmarkRunner.Run(typeof(OneBodyConstraintBenchmarks)); -BenchmarkRunner.Run(typeof(TwoBodyConstraintBenchmarks)); -BenchmarkRunner.Run(typeof(ThreeBodyConstraintBenchmarks)); -BenchmarkRunner.Run(typeof(FourBodyConstraintBenchmarks)); -BenchmarkRunner.Run(typeof(ConvexCollisionTesters)); -BenchmarkRunner.Run(typeof(CollisionBatcherTasks)); -BenchmarkRunner.Run(typeof(Sweeps)); -BenchmarkRunner.Run(typeof(ShapeRayTests)); -BenchmarkRunner.Run(typeof(GatherScatter)); -BenchmarkRunner.Run(typeof(RagdollTubeBenchmark)); -BenchmarkRunner.Run(typeof(ShapePileBenchmark)); - +public class BepuPhysics2Benchmarks +{ + public static void Main(string[] args) + { + BenchmarkSwitcher.FromAssembly(typeof(BepuPhysics2Benchmarks).Assembly).Run(args); + } +} diff --git a/DemoBenchmarks/RagdollTubeBenchmark.cs b/DemoBenchmarks/RagdollTubeBenchmark.cs index f81a8247b..303444f56 100644 --- a/DemoBenchmarks/RagdollTubeBenchmark.cs +++ b/DemoBenchmarks/RagdollTubeBenchmark.cs @@ -584,7 +584,7 @@ public void CleanUp() } [Benchmark] - public unsafe void Benchmark() + public unsafe void RagdollTubeBenchmarks() { for (int i = 0; i < timestepCount; ++i) { diff --git a/DemoBenchmarks/ShapePileBenchmark.cs b/DemoBenchmarks/ShapePileBenchmark.cs index bac950da2..f5501147a 100644 --- a/DemoBenchmarks/ShapePileBenchmark.cs +++ b/DemoBenchmarks/ShapePileBenchmark.cs @@ -221,7 +221,7 @@ public void CleanUp() } [Benchmark] - public unsafe void Benchmark() + public unsafe void ShapePileBenchmarks() { for (int i = 0; i < timestepCount; ++i) { From a4c37f76d98ef8d06db4f9878f1e67272ba7a5ea Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 28 Oct 2022 20:16:54 -0500 Subject: [PATCH 558/947] Trimming and splitting and merging benchmarks for speedier speed. --- ...s.cs => CollisionBatcherTaskBenchmarks.cs} | 14 +- ....cs => ConvexCollisionTesterBenchmarks.cs} | 3 +- DemoBenchmarks/DemoBenchmarks.csproj | 4 +- ...nts.cs => FourBodyConstraintBenchmarks.cs} | 0 ...rScatter.cs => GatherScatterBenchmarks.cs} | 50 +-- .../GroupedCollisionTesterBenchmarks.cs | 76 +++++ .../OneBodyConstraintsBenchmarks.cs | 114 +++++++ ...cs => OneBodyConstraintsBenchmarksDeep.cs} | 96 +----- DemoBenchmarks/README.md | 42 ++- DemoBenchmarks/ShapePileBenchmark.cs | 2 +- DemoBenchmarks/ShapeRayBenchmarks.cs | 41 +++ ...eRayTests.cs => ShapeRayBenchmarksDeep.cs} | 4 +- .../{Sweeps.cs => SweepBenchmarks.cs} | 215 +++++++------ DemoBenchmarks/SweepBenchmarksDeep.cs | 106 +++++++ ...ts.cs => ThreeBodyConstraintBenchmarks.cs} | 0 ...nts.cs => TwoBodyConstraintsBenchmarks.cs} | 285 +---------------- .../TwoBodyConstraintsBenchmarksDeep.cs | 298 ++++++++++++++++++ 17 files changed, 844 insertions(+), 506 deletions(-) rename DemoBenchmarks/{CollisionBatcherTasks.cs => CollisionBatcherTaskBenchmarks.cs} (94%) rename DemoBenchmarks/{ConvexCollisionTesters.cs => ConvexCollisionTesterBenchmarks.cs} (99%) rename DemoBenchmarks/{FourBodyConstraints.cs => FourBodyConstraintBenchmarks.cs} (100%) rename DemoBenchmarks/{GatherScatter.cs => GatherScatterBenchmarks.cs} (82%) create mode 100644 DemoBenchmarks/GroupedCollisionTesterBenchmarks.cs create mode 100644 DemoBenchmarks/OneBodyConstraintsBenchmarks.cs rename DemoBenchmarks/{OneBodyConstraints.cs => OneBodyConstraintsBenchmarksDeep.cs} (59%) create mode 100644 DemoBenchmarks/ShapeRayBenchmarks.cs rename DemoBenchmarks/{ShapeRayTests.cs => ShapeRayBenchmarksDeep.cs} (95%) rename DemoBenchmarks/{Sweeps.cs => SweepBenchmarks.cs} (53%) create mode 100644 DemoBenchmarks/SweepBenchmarksDeep.cs rename DemoBenchmarks/{ThreeBodyConstraints.cs => ThreeBodyConstraintBenchmarks.cs} (100%) rename DemoBenchmarks/{TwoBodyConstraints.cs => TwoBodyConstraintsBenchmarks.cs} (50%) create mode 100644 DemoBenchmarks/TwoBodyConstraintsBenchmarksDeep.cs diff --git a/DemoBenchmarks/CollisionBatcherTasks.cs b/DemoBenchmarks/CollisionBatcherTaskBenchmarks.cs similarity index 94% rename from DemoBenchmarks/CollisionBatcherTasks.cs rename to DemoBenchmarks/CollisionBatcherTaskBenchmarks.cs index ae7bb3973..a02563836 100644 --- a/DemoBenchmarks/CollisionBatcherTasks.cs +++ b/DemoBenchmarks/CollisionBatcherTaskBenchmarks.cs @@ -14,9 +14,9 @@ namespace DemoBenchmarks; /// /// Evaluates performance of all collision tasks together in a collision batcher. /// -public class CollisionBatcherTasks +public class CollisionBatcherTaskBenchmarks { - const int pairCount = 100000; + const int pairCount = 10000; BufferPool pool; struct Pair { @@ -49,12 +49,12 @@ public unsafe void Setup() shapeRelativeProbabilities[1] = 1; shapeRelativeProbabilities[2] = 1; shapeRelativeProbabilities[3] = 1; - shapeRelativeProbabilities[4] = 1; - shapeRelativeProbabilities[5] = 1; + shapeRelativeProbabilities[4] = 0.4f; + shapeRelativeProbabilities[5] = 0.35f; - shapeRelativeProbabilities[6] = 0.2f; - shapeRelativeProbabilities[7] = 0.2f; - shapeRelativeProbabilities[7] = 0.2f; + shapeRelativeProbabilities[6] = 0.1f; + shapeRelativeProbabilities[7] = 0.1f; + shapeRelativeProbabilities[8] = 0.1f; var sum = 0f; Span cumulative = stackalloc float[9]; diff --git a/DemoBenchmarks/ConvexCollisionTesters.cs b/DemoBenchmarks/ConvexCollisionTesterBenchmarks.cs similarity index 99% rename from DemoBenchmarks/ConvexCollisionTesters.cs rename to DemoBenchmarks/ConvexCollisionTesterBenchmarks.cs index dfb70905e..67ea43aed 100644 --- a/DemoBenchmarks/ConvexCollisionTesters.cs +++ b/DemoBenchmarks/ConvexCollisionTesterBenchmarks.cs @@ -16,7 +16,7 @@ namespace DemoBenchmarks; /// Note that all of these collision testers operate across lanes simultaneously where T is of type . /// The number of bundles being executed does not change if changes; if larger bundles are allowed, then more lanes end up getting solved. /// -public class ConvexCollisionTesters +public class ConvexCollisionTesterBenchmarks { const int iterationCount = 100; BufferPool pool; @@ -74,6 +74,7 @@ public unsafe void Setup() Hull = hull; } + [GlobalCleanup] public void Cleanup() { //All outstanding allocations poof when the pool is cleared. diff --git a/DemoBenchmarks/DemoBenchmarks.csproj b/DemoBenchmarks/DemoBenchmarks.csproj index 3fb0c0067..3cc15ee2c 100644 --- a/DemoBenchmarks/DemoBenchmarks.csproj +++ b/DemoBenchmarks/DemoBenchmarks.csproj @@ -1,10 +1,12 @@ - + Exe net7.0 enable true + -f *CollisionBatcherTaskBenchmarks.* *GroupedCollisionTesterBenchmarks.* *GatherScatterBenchmarks.* *OneBodyConstraintBenchmarks.* *TwoBodyConstraintBenchmarks.* *ThreeBodyConstraintBenchmarks.* *FourBodyConstraintBenchmarks.* *SweepBenchmarks.* *ShapeRayBenchmarks.* *ShapePileBenchmark.* *RagdollTubeBenchmark.* --join + diff --git a/DemoBenchmarks/FourBodyConstraints.cs b/DemoBenchmarks/FourBodyConstraintBenchmarks.cs similarity index 100% rename from DemoBenchmarks/FourBodyConstraints.cs rename to DemoBenchmarks/FourBodyConstraintBenchmarks.cs diff --git a/DemoBenchmarks/GatherScatter.cs b/DemoBenchmarks/GatherScatterBenchmarks.cs similarity index 82% rename from DemoBenchmarks/GatherScatter.cs rename to DemoBenchmarks/GatherScatterBenchmarks.cs index 5c52ad050..8647eca8a 100644 --- a/DemoBenchmarks/GatherScatter.cs +++ b/DemoBenchmarks/GatherScatterBenchmarks.cs @@ -13,7 +13,7 @@ namespace DemoBenchmarks; /// /// Evaluates performance of scatter/gather operations used by constraints to pull body data. /// -public class GatherScatter +public class GatherScatterBenchmarks { unsafe struct NarrowPhaseCallbacks : INarrowPhaseCallbacks { @@ -115,34 +115,48 @@ public unsafe Vector GatherState() } + //Scattering operations are extremely similar. Breaking them out could be useful in corner cases if a regression is observed, but usually doing them all in one benchmark should suffice. [Benchmark] - public unsafe void ScatterPose() + public unsafe void ScatterState() { var mask = new Vector(-1); for (int i = 0; i < iterationCount; ++i) { ref var pose = ref poses[i]; simulation.Bodies.ScatterPose(ref pose.Position, ref pose.Orientation, bodyIndices[i], mask); - } - } - - [Benchmark] - public unsafe void ScatterInertia() - { - var mask = new Vector(-1); - for (int i = 0; i < iterationCount; ++i) - { simulation.Bodies.ScatterInertia(ref inertias[i], bodyIndices[i], mask); - } - } - [Benchmark] - public unsafe void ScatterVelocities() - { - for (int i = 0; i < iterationCount; ++i) - { simulation.Bodies.ScatterVelocities(ref velocities[i], ref bodyIndices[i]); } } + //[Benchmark] + //public unsafe void ScatterPose() + //{ + // var mask = new Vector(-1); + // for (int i = 0; i < iterationCount; ++i) + // { + // ref var pose = ref poses[i]; + // simulation.Bodies.ScatterPose(ref pose.Position, ref pose.Orientation, bodyIndices[i], mask); + // } + //} + + //[Benchmark] + //public unsafe void ScatterInertia() + //{ + // var mask = new Vector(-1); + // for (int i = 0; i < iterationCount; ++i) + // { + // simulation.Bodies.ScatterInertia(ref inertias[i], bodyIndices[i], mask); + // } + //} + //[Benchmark] + //public unsafe void ScatterVelocities() + //{ + // for (int i = 0; i < iterationCount; ++i) + // { + // simulation.Bodies.ScatterVelocities(ref velocities[i], ref bodyIndices[i]); + // } + //} + } diff --git a/DemoBenchmarks/GroupedCollisionTesterBenchmarks.cs b/DemoBenchmarks/GroupedCollisionTesterBenchmarks.cs new file mode 100644 index 000000000..d8979aada --- /dev/null +++ b/DemoBenchmarks/GroupedCollisionTesterBenchmarks.cs @@ -0,0 +1,76 @@ +using BenchmarkDotNet.Attributes; +using BepuPhysics.Collidables; +using BepuPhysics.CollisionDetection; +using BepuPhysics.CollisionDetection.CollisionTasks; +using BepuUtilities; +using BepuUtilities.Memory; +using System.Numerics; +using static DemoBenchmarks.BenchmarkHelper; + +namespace DemoBenchmarks; + +/// +/// Evaluates performance of all multilane convex collision testers, but grouped together by approximate runtime for quicker top-level benchmarking. +/// To check performance of individual pairs, use the . +/// +/// +/// Note that all of these collision testers operate across lanes simultaneously where T is of type . +/// The number of bundles being executed does not change if changes; if larger bundles are allowed, then more lanes end up getting solved. +/// +public class GroupedCollisionTesterBenchmarks +{ + ConvexCollisionTesterBenchmarks benchmarks; + + [GlobalSetup] + public unsafe void Setup() + { + benchmarks = new ConvexCollisionTesterBenchmarks(); + benchmarks.Setup(); + } + + [GlobalCleanup] + public void Cleanup() + { + //All outstanding allocations poof when the pool is cleared. + benchmarks.Cleanup(); + } + + [Benchmark] + public Vector CheapCollisionBenchmarks() + { + return + benchmarks.SpherePairTester() + + benchmarks.SphereCapsuleTester() + + benchmarks.SphereBoxTester() + + benchmarks.SphereTriangleTester() + + benchmarks.SphereCylinderTester() + + benchmarks.CapsulePairTester(); + } + + [Benchmark] + public Vector ModerateCostCollisionBenchmarks() + { + return + benchmarks.CapsuleBoxTester() + + benchmarks.CapsuleTriangleTester() + + benchmarks.CapsuleCylinderTester() + + benchmarks.BoxPairTester() + + benchmarks.BoxTriangleTester() + + benchmarks.TrianglePairTester(); + } + + [Benchmark] + public Vector ExpensiveCollisionBenchmarks() + { + return + benchmarks.SphereConvexHullTester() + + benchmarks.CapsuleConvexHullTester() + + benchmarks.BoxConvexHullTester() + + benchmarks.TriangleConvexHullTester() + + benchmarks.CylinderConvexHullTester() + + benchmarks.ConvexHullPairTester() + + benchmarks.BoxCylinderTester() + + benchmarks.TriangleCylinderTester() + + benchmarks.CylinderPairTester(); + } +} diff --git a/DemoBenchmarks/OneBodyConstraintsBenchmarks.cs b/DemoBenchmarks/OneBodyConstraintsBenchmarks.cs new file mode 100644 index 000000000..27050f0a1 --- /dev/null +++ b/DemoBenchmarks/OneBodyConstraintsBenchmarks.cs @@ -0,0 +1,114 @@ +using BenchmarkDotNet.Attributes; +using BepuPhysics; +using BepuPhysics.Constraints; +using BepuPhysics.Constraints.Contact; +using BepuUtilities; +using System.Numerics; + +namespace DemoBenchmarks; + +/// +/// Evaluates performance of a representative subset of one body constraints. Excluded types are benchmarked in . +/// +/// +/// Note that all constraints operate across lanes simultaneously where T is of type . +/// The number of bundles being executed does not change if changes; if larger bundles are allowed, then more lanes end up getting solved. +/// +public class OneBodyConstraintBenchmarks +{ + public static BodyVelocityWide BenchmarkOneBodyConstraint(Vector3Wide positionA, QuaternionWide orientationA, BodyInertiaWide inertiaA, TPrestep prestep) + where TConstraintFunctions : unmanaged, IOneBodyConstraintFunctions where TPrestep : unmanaged where TAccumulatedImpulse : unmanaged + { + var functions = default(TConstraintFunctions); + var accumulatedImpulse = default(TAccumulatedImpulse); + var velocityA = default(BodyVelocityWide); + //Individual constraint iterations are often extremely cheap, so beef the benchmark up a bit. + const int iterations = 1000; + const float inverseDt = 60f; + const float dt = 1f / inverseDt; + for (int i = 0; i < iterations; ++i) + { + functions.WarmStart(positionA, orientationA, inertiaA, ref prestep, ref accumulatedImpulse, ref velocityA); + functions.Solve(positionA, orientationA, inertiaA, dt, inverseDt, ref prestep, ref accumulatedImpulse, ref velocityA); + } + return velocityA; + } + + //Contact constraints for a given bodycount/convexity are very similar. Trimming out the submaximal contact count benchmarks should usually be fine without losing important coverage. + + [Benchmark] + public BodyVelocityWide Contact4OneBody() + { + var prestep = new Contact4OneBodyPrestepData + { + Contact0 = new() { Depth = Vector.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) }, + Contact1 = new() { Depth = Vector.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) }, + Contact2 = new() { Depth = Vector.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) }, + Contact3 = new() { Depth = Vector.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) }, + MaterialProperties = new MaterialPropertiesWide + { + FrictionCoefficient = new Vector(1f), + MaximumRecoveryVelocity = new Vector(2f), + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } + }, + Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) + }; + + QuaternionWide.Broadcast(Quaternion.Identity, out var orientationA); + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkOneBodyConstraint(new Vector3Wide(), orientationA, inertia, prestep); + } + + [Benchmark] + public BodyVelocityWide Contact4NonconvexOneBody() + { + var prestep = new Contact4NonconvexOneBodyPrestepData + { + Contact0 = new() { Depth = Vector.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) }, + Contact1 = new() { Depth = Vector.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) }, + Contact2 = new() { Depth = Vector.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) }, + Contact3 = new() { Depth = Vector.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) }, + MaterialProperties = new MaterialPropertiesWide + { + FrictionCoefficient = new Vector(1f), + MaximumRecoveryVelocity = new Vector(2f), + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } + }, + }; + + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkOneBodyConstraint, + Contact4NonconvexOneBodyPrestepData, Contact4NonconvexAccumulatedImpulses>(new Vector3Wide(), orientation, inertia, prestep); + } + + //Servos and motors tend to be fairly similar. They're not *identical*, though, so we'll use one servo and one motor. The others will be in the deep tests. + + [Benchmark] + public BodyVelocityWide OneBodyAngularServo() + { + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var prestep = new OneBodyAngularServoPrestepData + { + ServoSettings = new() { BaseSpeed = Vector.Zero, MaximumForce = new Vector(float.MaxValue), MaximumSpeed = new Vector(float.MaxValue) }, + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) }, + TargetOrientation = orientation + }; + + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkOneBodyConstraint(new Vector3Wide(), orientation, inertia, prestep); + } + + [Benchmark] + public BodyVelocityWide OneBodyLinearMotor() + { + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var prestep = new OneBodyLinearMotorPrestepData + { + Settings = new() { Damping = Vector.One, MaximumForce = Vector.One }, + }; + + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkOneBodyConstraint(new Vector3Wide(), orientation, inertia, prestep); + } +} diff --git a/DemoBenchmarks/OneBodyConstraints.cs b/DemoBenchmarks/OneBodyConstraintsBenchmarksDeep.cs similarity index 59% rename from DemoBenchmarks/OneBodyConstraints.cs rename to DemoBenchmarks/OneBodyConstraintsBenchmarksDeep.cs index edd4d0f01..2f1b99ead 100644 --- a/DemoBenchmarks/OneBodyConstraints.cs +++ b/DemoBenchmarks/OneBodyConstraintsBenchmarksDeep.cs @@ -4,36 +4,19 @@ using BepuPhysics.Constraints.Contact; using BepuUtilities; using System.Numerics; +using static DemoBenchmarks.OneBodyConstraintBenchmarks; namespace DemoBenchmarks; /// -/// Evaluates performance of all one body constraints. +/// Evaluates performance of all one body constraints excluded from /// /// /// Note that all constraints operate across lanes simultaneously where T is of type . /// The number of bundles being executed does not change if changes; if larger bundles are allowed, then more lanes end up getting solved. /// -public class OneBodyConstraintBenchmarks +public class OneBodyConstraintBenchmarksDeep { - static BodyVelocityWide BenchmarkOneBodyConstraint(Vector3Wide positionA, QuaternionWide orientationA, BodyInertiaWide inertiaA, TPrestep prestep) - where TConstraintFunctions : unmanaged, IOneBodyConstraintFunctions where TPrestep : unmanaged where TAccumulatedImpulse : unmanaged - { - var functions = default(TConstraintFunctions); - var accumulatedImpulse = default(TAccumulatedImpulse); - var velocityA = default(BodyVelocityWide); - //Individual constraint iterations are often extremely cheap, so beef the benchmark up a bit. - const int iterations = 1000; - const float inverseDt = 60f; - const float dt = 1f / inverseDt; - for (int i = 0; i < iterations; ++i) - { - functions.WarmStart(positionA, orientationA, inertiaA, ref prestep, ref accumulatedImpulse, ref velocityA); - functions.Solve(positionA, orientationA, inertiaA, dt, inverseDt, ref prestep, ref accumulatedImpulse, ref velocityA); - } - return velocityA; - } - [Benchmark] public BodyVelocityWide Contact1OneBody() { @@ -96,28 +79,6 @@ public BodyVelocityWide Contact3OneBody() var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; return BenchmarkOneBodyConstraint(new Vector3Wide(), orientationA, inertia, prestep); } - [Benchmark] - public BodyVelocityWide Contact4OneBody() - { - var prestep = new Contact4OneBodyPrestepData - { - Contact0 = new() { Depth = Vector.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) }, - Contact1 = new() { Depth = Vector.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) }, - Contact2 = new() { Depth = Vector.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) }, - Contact3 = new() { Depth = Vector.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) }, - MaterialProperties = new MaterialPropertiesWide - { - FrictionCoefficient = new Vector(1f), - MaximumRecoveryVelocity = new Vector(2f), - SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } - }, - Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) - }; - - QuaternionWide.Broadcast(Quaternion.Identity, out var orientationA); - var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; - return BenchmarkOneBodyConstraint(new Vector3Wide(), orientationA, inertia, prestep); - } [Benchmark] public BodyVelocityWide Contact2NonconvexOneBody() @@ -162,44 +123,6 @@ public BodyVelocityWide Contact3NonconvexOneBody() Contact3NonconvexOneBodyPrestepData, Contact3NonconvexAccumulatedImpulses>(new Vector3Wide(), orientation, inertia, prestep); } - [Benchmark] - public BodyVelocityWide Contact4NonconvexOneBody() - { - var prestep = new Contact4NonconvexOneBodyPrestepData - { - Contact0 = new() { Depth = Vector.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) }, - Contact1 = new() { Depth = Vector.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) }, - Contact2 = new() { Depth = Vector.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) }, - Contact3 = new() { Depth = Vector.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) }, - MaterialProperties = new MaterialPropertiesWide - { - FrictionCoefficient = new Vector(1f), - MaximumRecoveryVelocity = new Vector(2f), - SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } - }, - }; - - QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); - var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; - return BenchmarkOneBodyConstraint, - Contact4NonconvexOneBodyPrestepData, Contact4NonconvexAccumulatedImpulses>(new Vector3Wide(), orientation, inertia, prestep); - } - - [Benchmark] - public BodyVelocityWide OneBodyAngularServo() - { - QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); - var prestep = new OneBodyAngularServoPrestepData - { - ServoSettings = new() { BaseSpeed = Vector.Zero, MaximumForce = new Vector(float.MaxValue), MaximumSpeed = new Vector(float.MaxValue) }, - SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) }, - TargetOrientation = orientation - }; - - var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; - return BenchmarkOneBodyConstraint(new Vector3Wide(), orientation, inertia, prestep); - } - [Benchmark] public BodyVelocityWide OneBodyAngularMotor() { @@ -227,17 +150,4 @@ public BodyVelocityWide OneBodyLinearServo() var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; return BenchmarkOneBodyConstraint(new Vector3Wide(), orientation, inertia, prestep); } - - [Benchmark] - public BodyVelocityWide OneBodyLinearMotor() - { - QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); - var prestep = new OneBodyLinearMotorPrestepData - { - Settings = new() { Damping = Vector.One, MaximumForce = Vector.One }, - }; - - var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; - return BenchmarkOneBodyConstraint(new Vector3Wide(), orientation, inertia, prestep); - } } diff --git a/DemoBenchmarks/README.md b/DemoBenchmarks/README.md index d56c99e10..37df8f4b9 100644 --- a/DemoBenchmarks/README.md +++ b/DemoBenchmarks/README.md @@ -2,27 +2,51 @@ This project contains a variety of performance tests for different pieces of the library. +For a partial but pretty high coverage set of benchmarks, consider running with a filter of: +``` +-f *CollisionBatcherTaskBenchmarks.* *GroupedCollisionTesterBenchmarks.* *GatherScatterBenchmarks.* *OneBodyConstraintBenchmarks.* *TwoBodyConstraintBenchmarks.* *ThreeBodyConstraintBenchmarks.* *FourBodyConstraintBenchmarks.* *SweepBenchmarks.* *ShapeRayBenchmarks.* *ShapePileBenchmark.* *RagdollTubeBenchmark.* +``` + +For a longer complete set of benchmarkers, consider running with a filter of: +``` +-f *CollisionBatcherTaskBenchmarks.* *ConvexCollisionTesterBenchmarks.* *GatherScatterBenchmarks.* *OneBodyConstraintBenchmarks.* *OneBodyConstraintBenchmarksDeep.* *TwoBodyConstraintBenchmarks.* *TwoBodyConstraintBenchmarksDeep.* *ThreeBodyConstraintBenchmarks.* *FourBodyConstraintBenchmarks.* *SweepBenchmarks.* *SweepBenchmarksDeep.* *ShapeRayBenchmarksDeep.* *ShapePileBenchmark.* *RagdollTubeBenchmark.* +``` + The tests sometimes cover different scopes. Most are microbenchmarks. Benchmarks in the following classes cover specific codepaths, like a single collision pair tester: -[ConvexCollisionTesters](ConvexCollisionTesters.cs): Contact manifold generating convex collision tests, one benchmark per type pair. +[ConvexCollisionTesterBenchmarks](ConvexCollisionTesterBenchmarks.cs): Contact manifold generating convex collision tests, one benchmark per type pair. + +[OneBodyConstraintBenchmarks](OneBodyConstraintBenchmarks.cs): One body constraints, one benchmark per type. + +[TwoBodyConstraintBenchmarks](TwoBodyConstraintBenchmarks.cs): Two body constraints, one benchmark per type. + +[ThreeBodyConstraintBenchmarks](ThreeBodyConstraintBenchmarks.cs): Three body constraints, one benchmark per type. -[OneBodyConstraints](OneBodyConstraints.cs): One body constraints, one benchmark per type. +[FourBodyConstraintBenchmarks](FourBodyConstraintBenchmarks.cs): Four body constraints, one benchmark per type. -[TwoBodyConstraints](TwoBodyConstraints.cs): Two body constraints, one benchmark per type. +[ShapeRayBenchmarksDeep](ShapeRayBenchmarksDeep.cs): Shape-ray tests, one benchmark per type. -[ThreeBodyConstraints](ThreeBodyConstraints.cs): Three body constraints, one benchmark per type. +[SweepBenchmarks](SweepBenchmarks.cs): Linear + angular sweep tests for shape pairs, one benchmark per type pair. -[FourBodyConstraints](FourBodyConstraints.cs): Four body constraints, one benchmark per type. +[GatherScatterBenchmarks](GatherScatterBenchmarks.cs): Tests SIMD body data gathering/scattering for constraint solving. One benchmark for gather and one for scatter. -[ShapeRayTests](ShapeRayTests.cs): Shape-ray tests, one benchmark per type. +Some microbenchmarks are excluded from the above because they're reasonably well covered by other benchmarks. The excluded codepaths are found in the classes suffixed with `Deep`: -[Sweeps](Sweeps.cs): Linear + angular sweep tests for shape pairs, one benchmark per type pair. +[OneBodyConstraintBenchmarksDeep](OneBodyConstraintBenchmarksDeep.cs): One body constraints excluded from `OneBodyConstraintBenchmarks`. -[GatherScatter](GatherScatter.cs): Tests SIMD body data gathering/scattering for constraint solving. One benchmark for each gather/scatter operation. +[TwoBodyConstraintBenchmarksDeep](TwoBodyConstraintBenchmarksDeep.cs): Two body constraints excluded from `TwoBodyConstraintBenchmarks`. + +[SweepBenchmarksDeep](SweepBenchmarksDeep.cs): Sweep tests excluded from `SweepBenchmarks`. + +[ShapeRayBenchmarksDeep](ShapeRayBenchmarksDeep.cs): Individual shape-ray tests that make up the `ShapeRayBenchmarks`. Other tests cover more codepaths at once: -[CollisionBatcherTasks](CollisionBatcherTasks.cs): A single benchmark testing all shape pair types (including nonconvex pairs) in the `CollisionBatcher`. Pairs are dynamically batched and dispatched across a loop. +[ShapeRayBenchmarks](ShapeRayBenchmarks.cs): One benchmark for all convex ray tests, another for all compound ray tests. + +[GroupedCollisionTesterBenchmarks](GroupedCollisionTesterBenchmarks.cs): A few benchmarks testing all convex shape pair types directly (with no `CollisionBatcher` involvement), but with type pairs grouped together by approximate cost. + +[CollisionBatcherTaskBenchmarks](CollisionBatcherTaskBenchmarks.cs): A single benchmark testing all shape pair types (including nonconvex pairs) in the `CollisionBatcher`. Pairs are dynamically batched and dispatched across a loop. [RagdollTubeBenchmark](RagdollTubeBenchmark.cs): Simpler version of the `RagdollTubeDemo`, testing a bunch of ragdolls in a tumblertube. One execution of the benchmark covers many timesteps of simulation. This covers several collision pairs, many constraint types, and the rest of the simulation code gluing things together. diff --git a/DemoBenchmarks/ShapePileBenchmark.cs b/DemoBenchmarks/ShapePileBenchmark.cs index f5501147a..fcd4745c7 100644 --- a/DemoBenchmarks/ShapePileBenchmark.cs +++ b/DemoBenchmarks/ShapePileBenchmark.cs @@ -168,7 +168,7 @@ public unsafe void Initialize() var cylinderIndex = Simulation.Shapes.Add(cylinder); var hullIndex = Simulation.Shapes.Add(convexHull); const int width = 8; - const int height = 8; + const int height = 4; const int length = 8; var shapeCount = 0; for (int i = 0; i < width; ++i) diff --git a/DemoBenchmarks/ShapeRayBenchmarks.cs b/DemoBenchmarks/ShapeRayBenchmarks.cs new file mode 100644 index 000000000..85b1522df --- /dev/null +++ b/DemoBenchmarks/ShapeRayBenchmarks.cs @@ -0,0 +1,41 @@ +using BenchmarkDotNet.Attributes; +namespace DemoBenchmarks; + +/// +/// Evaluates performance of shape ray tests. Performs groups of types in single benchmarks: all convexes in one, compounds in the other. +/// +public class ShapeRayBenchmarks +{ + ShapeRayBenchmarksDeep deep; + [GlobalSetup] + public void Setup() + { + deep = new ShapeRayBenchmarksDeep(); + deep.Setup(); + } + + [GlobalCleanup] + public void Cleanup() + { + deep.Cleanup(); + } + + [Benchmark] + public void ConvexRayTests() + { + deep.RaySphere(); + deep.RayCapsule(); + deep.RayBox(); + deep.RayTriangle(); + deep.RayCylinder(); + deep.RayConvexHull(); + } + + [Benchmark] + public void CompoundRayTests() + { + deep.RayCompound(); + deep.RayBigCompound(); + deep.RayMesh(); + } +} diff --git a/DemoBenchmarks/ShapeRayTests.cs b/DemoBenchmarks/ShapeRayBenchmarksDeep.cs similarity index 95% rename from DemoBenchmarks/ShapeRayTests.cs rename to DemoBenchmarks/ShapeRayBenchmarksDeep.cs index c7fa12dc1..9fe5025f0 100644 --- a/DemoBenchmarks/ShapeRayTests.cs +++ b/DemoBenchmarks/ShapeRayBenchmarksDeep.cs @@ -13,9 +13,9 @@ namespace DemoBenchmarks; /// -/// Evaluates performance of shape ray tests. +/// Evaluates performance of shape ray tests. Each benchmark covers a different shape type. /// -public class ShapeRayTests +public class ShapeRayBenchmarksDeep { const int iterationCount = 100; BufferPool pool; diff --git a/DemoBenchmarks/Sweeps.cs b/DemoBenchmarks/SweepBenchmarks.cs similarity index 53% rename from DemoBenchmarks/Sweeps.cs rename to DemoBenchmarks/SweepBenchmarks.cs index b1086f96a..29e2af687 100644 --- a/DemoBenchmarks/Sweeps.cs +++ b/DemoBenchmarks/SweepBenchmarks.cs @@ -12,12 +12,9 @@ namespace DemoBenchmarks; -/// -/// Evaluates performance of all sweep tests. -/// -public class Sweeps +public class Sweeper { - const int iterationCount = 100; + const int iterationCount = 10; BufferPool pool; struct Pair { @@ -34,9 +31,7 @@ struct Pair SweepTaskRegistry taskRegistry; Shapes shapes; - - [GlobalSetup] - public unsafe void Setup() + public Sweeper() { pool = new BufferPool(); pool.Take(iterationCount, out pairs); @@ -109,7 +104,6 @@ TypedIndex GetRandomShapeTypeIndex(Span cumulative) } } - [GlobalCleanup] public void Cleanup() { //All outstanding allocations poof when the pool is cleared. @@ -125,7 +119,7 @@ public bool AllowTest(int childA, int childB) } } - unsafe Vector3 Test() where TA : unmanaged, IShape where TB : unmanaged, IShape + public unsafe Vector3 Test() where TA : unmanaged, IShape where TB : unmanaged, IShape { var task = taskRegistry.GetTask(); var aType = default(TA).TypeId; @@ -146,94 +140,119 @@ unsafe Vector3 Test() where TA : unmanaged, IShape where TB : unmanaged, } return resultSum; } +} - [Benchmark] - public void SphereSphere() => Test(); - [Benchmark] - public void SphereCapsule() => Test(); - [Benchmark] - public void SphereBox() => Test(); - [Benchmark] - public void SphereTriangle() => Test(); - [Benchmark] - public void SphereCylinder() => Test(); - [Benchmark] - public void SphereConvexHull() => Test(); - [Benchmark] - public void SphereCompound() => Test(); - [Benchmark] - public void SphereBigCompound() => Test(); - [Benchmark] - public void SphereMesh() => Test(); - [Benchmark] - public void CapsuleCapsule() => Test(); - [Benchmark] - public void CapsuleBox() => Test(); - [Benchmark] - public void CapsuleTriangle() => Test(); - [Benchmark] - public void CapsuleCylinder() => Test(); - [Benchmark] - public void CapsuleConvexHull() => Test(); - [Benchmark] - public void CapsuleCompound() => Test(); - [Benchmark] - public void CapsuleBigCompound() => Test(); - [Benchmark] - public void CapsuleMesh() => Test(); - [Benchmark] - public void BoxBox() => Test(); - [Benchmark] - public void BoxTriangle() => Test(); - [Benchmark] - public void BoxCylinder() => Test(); - [Benchmark] - public void BoxConvexHull() => Test(); - [Benchmark] - public void BoxCompound() => Test(); - [Benchmark] - public void BoxBigCompound() => Test(); - [Benchmark] - public void BoxMesh() => Test(); - [Benchmark] - public void TriangleTriangle() => Test(); - [Benchmark] - public void TriangleCylinder() => Test(); - [Benchmark] - public void TriangleConvexHull() => Test(); - [Benchmark] - public void TriangleCompound() => Test(); - [Benchmark] - public void TriangleBigCompound() => Test(); - [Benchmark] - public void TriangleMesh() => Test(); - [Benchmark] - public void CylinderCylinder() => Test(); - [Benchmark] - public void CylinderConvexHull() => Test(); - [Benchmark] - public void CylinderCompound() => Test(); - [Benchmark] - public void CylinderBigCompound() => Test(); - [Benchmark] - public void CylinderMesh() => Test(); - [Benchmark] - public void ConvexHullConvexHull() => Test(); - [Benchmark] - public void ConvexHullCompound() => Test(); - [Benchmark] - public void ConvexHullBigCompound() => Test(); - [Benchmark] - public void ConvexHullMesh() => Test(); - [Benchmark] - public void CompoundCompound() => Test(); - [Benchmark] - public void CompoundBigCompound() => Test(); - [Benchmark] - public void CompoundMesh() => Test(); - [Benchmark] - public void BigCompoundBigCompound() => Test(); - [Benchmark] - public void BigCompoundMesh() => Test(); +/// +/// Evaluates performance of a representative subset of sweep tests. +/// +public class SweepBenchmarks +{ + Sweeper sweeper; + [GlobalSetup] + public unsafe void Setup() + { + sweeper = new Sweeper(); + } + + [GlobalCleanup] + public void Cleanup() + { + sweeper.Cleanup(); + } + + + //Some (commented) benchmarks are punted to SweepBenchmarksDeep: + //-Convex-compound tests are pretty similar to each other. Box-compound/bigcompound/mesh is preserved, but the rest are commented. Likewise, bigcompound-bigcompound is the only compound-compound pair that's tested. + //-Multiple sweeps use GJK for the distance tester. While the support functions used vary, those are relatively low value compared to the GJK outer loop. If we really need to, we can cover the support functions separately. + //Most sphere tests aren't super interesting, so we exclude some of them. + + //[Benchmark] + //public void SphereSphere() => sweeper.Test(); + [Benchmark] + public void SphereCapsule() => sweeper.Test(); + //[Benchmark] + //public void SphereBox() => sweeper.Test(); + [Benchmark] + public void SphereTriangle() => sweeper.Test(); + //[Benchmark] + //public void SphereCylinder() => sweeper.Test(); + //[Benchmark] + //public void SphereConvexHull() => Test(); //GJK + //[Benchmark] + //public void SphereCompound() => Test(); + //[Benchmark] + //public void SphereBigCompound() => Test(); + //[Benchmark] + //public void SphereMesh() => Test(); + [Benchmark] + public void CapsuleCapsule() => sweeper.Test(); + [Benchmark] + public void CapsuleBox() => sweeper.Test(); + //[Benchmark] + //public void CapsuleTriangle() => Test(); //GJK + [Benchmark] + public void CapsuleCylinder() => sweeper.Test(); //GJK + //[Benchmark] + //public void CapsuleConvexHull() => Test(); //GJK + //[Benchmark] + //public void CapsuleCompound() => Test(); + //[Benchmark] + //public void CapsuleBigCompound() => Test(); + //[Benchmark] + //public void CapsuleMesh() => Test(); + //[Benchmark] + //public void BoxBox() => Test(); //GJK + //[Benchmark] + //public void BoxTriangle() => Test(); //GJK + //[Benchmark] + //public void BoxCylinder() => Test(); //GJK + //[Benchmark] + //public void BoxConvexHull() => Test(); //GJK + [Benchmark] + public void BoxCompound() => sweeper.Test(); + //[Benchmark] + //public void BoxBigCompound() => Test(); //Well covered by bigcompound-bigcompound. + [Benchmark] + public void BoxMesh() => sweeper.Test(); + //[Benchmark] + //public void TriangleTriangle() => Test(); //GJK + //[Benchmark] + //public void TriangleCylinder() => Test(); //GJK + //[Benchmark] + //public void TriangleConvexHull() => Test(); //GJK + //[Benchmark] + //public void TriangleCompound() => Test(); + //[Benchmark] + //public void TriangleBigCompound() => Test(); + //[Benchmark] + //public void TriangleMesh() => Test(); + //[Benchmark] + //public void CylinderCylinder() => Test(); //GJK + //[Benchmark] + //public void CylinderConvexHull() => Test(); //GJK + //[Benchmark] + //public void CylinderCompound() => Test(); + //[Benchmark] + //public void CylinderBigCompound() => Test(); + //[Benchmark] + //public void CylinderMesh() => Test(); + //[Benchmark] + //public void ConvexHullConvexHull() => Test(); //GJK + //[Benchmark] + //public void ConvexHullCompound() => Test(); + //[Benchmark] + //public void ConvexHullBigCompound() => Test(); + //[Benchmark] + //public void ConvexHullMesh() => Test(); + //[Benchmark] + //public void CompoundCompound() => Test(); + //[Benchmark] + //public void CompoundBigCompound() => Test(); + //[Benchmark] + //public void CompoundMesh() => Test(); + [Benchmark] + public void BigCompoundBigCompound() => sweeper.Test(); + //[Benchmark] + //public void BigCompoundMesh() => Test(); //No mesh-mesh! } diff --git a/DemoBenchmarks/SweepBenchmarksDeep.cs b/DemoBenchmarks/SweepBenchmarksDeep.cs new file mode 100644 index 000000000..96eb1ea61 --- /dev/null +++ b/DemoBenchmarks/SweepBenchmarksDeep.cs @@ -0,0 +1,106 @@ +using BenchmarkDotNet.Attributes; +using BepuPhysics; +using BepuPhysics.Collidables; +using BepuPhysics.CollisionDetection; +using BepuPhysics.CollisionDetection.CollisionTasks; +using BepuUtilities; +using BepuUtilities.Memory; +using System.Diagnostics; +using System.Numerics; +using System.Runtime.CompilerServices; +using static DemoBenchmarks.BenchmarkHelper; + +namespace DemoBenchmarks; + +/// +/// Evaluates performance of all sweep tests excluded from . +/// +public class SweepBenchmarksDeep +{ + Sweeper sweeper; + [GlobalSetup] + public unsafe void Setup() + { + sweeper = new Sweeper(); + } + + [GlobalCleanup] + public void Cleanup() + { + sweeper.Cleanup(); + } + + [Benchmark] + public void SphereSphere() => sweeper.Test(); + [Benchmark] + public void SphereBox() => sweeper.Test(); + [Benchmark] + public void SphereCylinder() => sweeper.Test(); + [Benchmark] + public void SphereConvexHull() => sweeper.Test(); //GJK + [Benchmark] + public void SphereCompound() => sweeper.Test(); + [Benchmark] + public void SphereBigCompound() => sweeper.Test(); + [Benchmark] + public void SphereMesh() => sweeper.Test(); + [Benchmark] + public void CapsuleTriangle() => sweeper.Test(); //GJK + [Benchmark] + public void CapsuleConvexHull() => sweeper.Test(); //GJK + [Benchmark] + public void CapsuleCompound() => sweeper.Test(); + [Benchmark] + public void CapsuleBigCompound() => sweeper.Test(); + [Benchmark] + public void CapsuleMesh() => sweeper.Test(); + [Benchmark] + public void BoxBox() => sweeper.Test(); //GJK + [Benchmark] + public void BoxTriangle() => sweeper.Test(); //GJK + [Benchmark] + public void BoxCylinder() => sweeper.Test(); //GJK + [Benchmark] + public void BoxConvexHull() => sweeper.Test(); //GJK + [Benchmark] + public void BoxBigCompound() => sweeper.Test(); //Well covered by bigcompound-bigcompound. + [Benchmark] + public void TriangleTriangle() => sweeper.Test(); //GJK + [Benchmark] + public void TriangleCylinder() => sweeper.Test(); //GJK + [Benchmark] + public void TriangleConvexHull() => sweeper.Test(); //GJK + [Benchmark] + public void TriangleCompound() => sweeper.Test(); + [Benchmark] + public void TriangleBigCompound() => sweeper.Test(); + [Benchmark] + public void TriangleMesh() => sweeper.Test(); + [Benchmark] + public void CylinderCylinder() => sweeper.Test(); //GJK + [Benchmark] + public void CylinderConvexHull() => sweeper.Test(); //GJK + [Benchmark] + public void CylinderCompound() => sweeper.Test(); + [Benchmark] + public void CylinderBigCompound() => sweeper.Test(); + [Benchmark] + public void CylinderMesh() => sweeper.Test(); + [Benchmark] + public void ConvexHullConvexHull() => sweeper.Test(); //GJK + [Benchmark] + public void ConvexHullCompound() => sweeper.Test(); + [Benchmark] + public void ConvexHullBigCompound() => sweeper.Test(); + [Benchmark] + public void ConvexHullMesh() => sweeper.Test(); + [Benchmark] + public void CompoundCompound() => sweeper.Test(); + [Benchmark] + public void CompoundBigCompound() => sweeper.Test(); + [Benchmark] + public void CompoundMesh() => sweeper.Test(); + [Benchmark] + public void BigCompoundMesh() => sweeper.Test(); + //No mesh-mesh! +} diff --git a/DemoBenchmarks/ThreeBodyConstraints.cs b/DemoBenchmarks/ThreeBodyConstraintBenchmarks.cs similarity index 100% rename from DemoBenchmarks/ThreeBodyConstraints.cs rename to DemoBenchmarks/ThreeBodyConstraintBenchmarks.cs diff --git a/DemoBenchmarks/TwoBodyConstraints.cs b/DemoBenchmarks/TwoBodyConstraintsBenchmarks.cs similarity index 50% rename from DemoBenchmarks/TwoBodyConstraints.cs rename to DemoBenchmarks/TwoBodyConstraintsBenchmarks.cs index 60d78f023..3e133e5c4 100644 --- a/DemoBenchmarks/TwoBodyConstraints.cs +++ b/DemoBenchmarks/TwoBodyConstraintsBenchmarks.cs @@ -8,7 +8,7 @@ namespace DemoBenchmarks; /// -/// Evaluates performance of all two body constraints. +/// Evaluates performance of a representative subset of two body constraints. Excluded types are benchmarked in . /// /// /// Note that all constraints operate across lanes simultaneously where T is of type . @@ -16,7 +16,7 @@ namespace DemoBenchmarks; /// public class TwoBodyConstraintBenchmarks { - static (BodyVelocityWide, BodyVelocityWide) BenchmarkTwoBodyConstraint( + public static (BodyVelocityWide, BodyVelocityWide) BenchmarkTwoBodyConstraint( Vector3Wide positionA, QuaternionWide orientationA, BodyInertiaWide inertiaA, Vector3Wide positionB, QuaternionWide orientationB, BodyInertiaWide inertiaB, TPrestep prestep) where TConstraintFunctions : unmanaged, ITwoBodyConstraintFunctions where TPrestep : unmanaged where TAccumulatedImpulse : unmanaged @@ -37,71 +37,7 @@ public class TwoBodyConstraintBenchmarks return (velocityA, velocityB); } - [Benchmark] - public (BodyVelocityWide, BodyVelocityWide) Contact1() - { - var prestep = new Contact1PrestepData - { - Contact0 = new ConvexContactWide { Depth = Vector.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) }, - MaterialProperties = new MaterialPropertiesWide - { - FrictionCoefficient = new Vector(1f), - MaximumRecoveryVelocity = new Vector(2f), - SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } - }, - Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)), - OffsetB = Vector3Wide.Broadcast(new Vector3(2, 0, 0)) - }; - - QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); - var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; - return BenchmarkTwoBodyConstraint(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); - } - - [Benchmark] - public (BodyVelocityWide, BodyVelocityWide) Contact2() - { - var prestep = new Contact2PrestepData - { - Contact0 = new ConvexContactWide { Depth = Vector.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) }, - Contact1 = new ConvexContactWide { Depth = Vector.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) }, - MaterialProperties = new MaterialPropertiesWide - { - FrictionCoefficient = new Vector(1f), - MaximumRecoveryVelocity = new Vector(2f), - SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } - }, - Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)), - OffsetB = Vector3Wide.Broadcast(new Vector3(2, 0, 0)) - }; - - QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); - var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; - return BenchmarkTwoBodyConstraint(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); - } - - [Benchmark] - public (BodyVelocityWide, BodyVelocityWide) Contact3() - { - var prestep = new Contact3PrestepData - { - Contact0 = new ConvexContactWide { Depth = Vector.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) }, - Contact1 = new ConvexContactWide { Depth = Vector.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) }, - Contact2 = new ConvexContactWide { Depth = Vector.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) }, - MaterialProperties = new MaterialPropertiesWide - { - FrictionCoefficient = new Vector(1f), - MaximumRecoveryVelocity = new Vector(2f), - SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } - }, - Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)), - OffsetB = Vector3Wide.Broadcast(new Vector3(2, 0, 0)) - }; - - QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); - var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; - return BenchmarkTwoBodyConstraint(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); - } + //Contact constraints for a given bodycount/convexity are very similar. Trimming out the submaximal contact count benchmarks should usually be fine without losing important coverage. [Benchmark] public (BodyVelocityWide, BodyVelocityWide) Contact4() @@ -127,51 +63,6 @@ public class TwoBodyConstraintBenchmarks return BenchmarkTwoBodyConstraint(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); } - [Benchmark] - public (BodyVelocityWide, BodyVelocityWide) Contact2Nonconvex() - { - var prestep = new Contact2NonconvexPrestepData - { - Contact0 = new NonconvexContactPrestepData { Depth = Vector.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) }, - Contact1 = new NonconvexContactPrestepData { Depth = Vector.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 0, 1)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) }, - MaterialProperties = new MaterialPropertiesWide - { - FrictionCoefficient = new Vector(1f), - MaximumRecoveryVelocity = new Vector(2f), - SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } - }, - OffsetB = Vector3Wide.Broadcast(new Vector3(2, 0, 0)) - }; - - QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); - var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; - return BenchmarkTwoBodyConstraint, - Contact2NonconvexPrestepData, Contact2NonconvexAccumulatedImpulses>(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); - } - - [Benchmark] - public (BodyVelocityWide, BodyVelocityWide) Contact3Nonconvex() - { - var prestep = new Contact3NonconvexPrestepData - { - Contact0 = new NonconvexContactPrestepData { Depth = Vector.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) }, - Contact1 = new NonconvexContactPrestepData { Depth = Vector.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 0, 1)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) }, - Contact2 = new NonconvexContactPrestepData { Depth = Vector.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 1, 0)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) }, - MaterialProperties = new MaterialPropertiesWide - { - FrictionCoefficient = new Vector(1f), - MaximumRecoveryVelocity = new Vector(2f), - SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } - }, - OffsetB = Vector3Wide.Broadcast(new Vector3(2, 0, 0)) - }; - - QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); - var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; - return BenchmarkTwoBodyConstraint, - Contact3NonconvexPrestepData, Contact3NonconvexAccumulatedImpulses>(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); - } - [Benchmark] public (BodyVelocityWide, BodyVelocityWide) Contact4Nonconvex() { @@ -273,37 +164,7 @@ public class TwoBodyConstraintBenchmarks return BenchmarkTwoBodyConstraint>(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); } - [Benchmark] - public (BodyVelocityWide, BodyVelocityWide) TwistLimit() - { - QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); - var prestep = new TwistLimitPrestepData - { - LocalBasisA = orientation, - LocalBasisB = orientation, - MinimumAngle = Vector.Zero, - MaximumAngle = Vector.One, - SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } - }; - - var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; - return BenchmarkTwoBodyConstraint>(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); - } - - [Benchmark] - public (BodyVelocityWide, BodyVelocityWide) TwistMotor() - { - QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); - var prestep = new TwistMotorPrestepData - { - LocalAxisA = Vector3Wide.Broadcast(new Vector3(0, 1, 0)), - LocalAxisB = Vector3Wide.Broadcast(new Vector3(0, 1, 0)), - Settings = new() { Damping = Vector.One, MaximumForce = new Vector(float.MaxValue) } - }; - - var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; - return BenchmarkTwoBodyConstraint>(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); - } + //TwistLimit and TwistMotor are in the deep tests. [Benchmark] public (BodyVelocityWide, BodyVelocityWide) AngularServo() @@ -320,18 +181,7 @@ public class TwoBodyConstraintBenchmarks return BenchmarkTwoBodyConstraint(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); } - [Benchmark] - public (BodyVelocityWide, BodyVelocityWide) AngularMotor() - { - QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); - var prestep = new AngularMotorPrestepData - { - Settings = new() { Damping = Vector.One, MaximumForce = new Vector(float.MaxValue) } - }; - - var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; - return BenchmarkTwoBodyConstraint(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); - } + //Angular motor is in the deep tests. [Benchmark] public (BodyVelocityWide, BodyVelocityWide) Weld() @@ -365,22 +215,7 @@ public class TwoBodyConstraintBenchmarks return BenchmarkTwoBodyConstraint>(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); } - [Benchmark] - public (BodyVelocityWide, BodyVelocityWide) DistanceLimit() - { - QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); - var prestep = new DistanceLimitPrestepData - { - LocalOffsetA = Vector3Wide.Broadcast(new Vector3(0.5f, 0, 0)), - LocalOffsetB = Vector3Wide.Broadcast(new Vector3(-0.5f, 0, 0)), - MinimumDistance = new Vector(0.5f), - MaximumDistance = new Vector(1.5f), - SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } - }; - - var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; - return BenchmarkTwoBodyConstraint>(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); - } + //DistanceLimit is in the deep tests. [Benchmark] public (BodyVelocityWide, BodyVelocityWide) CenterDistanceConstraint() @@ -430,53 +265,7 @@ public class TwoBodyConstraintBenchmarks return BenchmarkTwoBodyConstraint>(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); } - [Benchmark] - public (BodyVelocityWide, BodyVelocityWide) LinearAxisMotor() - { - QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); - var prestep = new LinearAxisMotorPrestepData - { - LocalOffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), - LocalOffsetB = Vector3Wide.Broadcast(new Vector3(-1, 0, 0)), - LocalPlaneNormal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)), - Settings = new() { Damping = Vector.One, MaximumForce = new Vector(float.MaxValue) } - }; - - var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; - return BenchmarkTwoBodyConstraint>(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); - } - - [Benchmark] - public (BodyVelocityWide, BodyVelocityWide) LinearAxisLimit() - { - QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); - var prestep = new LinearAxisLimitPrestepData - { - LocalOffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), - LocalOffsetB = Vector3Wide.Broadcast(new Vector3(-1, 0, 0)), - LocalPlaneNormal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)), - MaximumOffset = new Vector(3), - MinimumOffset = new Vector(-3), - SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } - }; - - var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; - return BenchmarkTwoBodyConstraint>(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); - } - - [Benchmark] - public (BodyVelocityWide, BodyVelocityWide) AngularAxisMotor() - { - QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); - var prestep = new AngularAxisMotorPrestepData - { - LocalAxisA = Vector3Wide.Broadcast(new Vector3(0, 1, 0)), - Settings = new() { Damping = Vector.One, MaximumForce = new Vector(float.MaxValue) } - }; - - var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; - return BenchmarkTwoBodyConstraint>(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); - } + //LinearAxisMotor, LinearAxisLimit, and AngularAxisMotor are in deep tests. [Benchmark] public (BodyVelocityWide, BodyVelocityWide) SwivelHinge() @@ -507,62 +296,6 @@ public class TwoBodyConstraintBenchmarks var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; return BenchmarkTwoBodyConstraint(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); } - - [Benchmark] - public (BodyVelocityWide, BodyVelocityWide) BallSocketMotor() - { - var prestep = new BallSocketMotorPrestepData - { - Settings = new() { Damping = Vector.One, MaximumForce = new Vector(float.MaxValue) } - }; - - QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); - var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; - return BenchmarkTwoBodyConstraint(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); - } - - [Benchmark] - public (BodyVelocityWide, BodyVelocityWide) BallSocketServo() - { - var prestep = new BallSocketServoPrestepData - { - ServoSettings = new ServoSettingsWide { MaximumForce = new Vector(float.MaxValue), MaximumSpeed = new Vector(float.MaxValue) }, - SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } - }; - - QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); - var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; - return BenchmarkTwoBodyConstraint(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); - } - - [Benchmark] - public (BodyVelocityWide, BodyVelocityWide) AngularAxisGearMotor() - { - var prestep = new AngularAxisGearMotorPrestepData - { - LocalAxisA = Vector3Wide.Broadcast(new Vector3(0, 1, 0)), - VelocityScale = Vector.One, - Settings = new() { Damping = Vector.One, MaximumForce = new Vector(float.MaxValue) } - }; - - QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); - var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; - return BenchmarkTwoBodyConstraint>(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); - } - - [Benchmark] - public (BodyVelocityWide, BodyVelocityWide) CenterDistanceLimit() - { - var prestep = new CenterDistanceLimitPrestepData - { - MinimumDistance = new Vector(0.2f), - MaximumDistance = new Vector(3), - SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } - }; - - QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); - var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; - return BenchmarkTwoBodyConstraint>(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); - } - + + //BallSocketServo, BallSocketMotor, AngularAxisGearMotor, and CenterDistanceLimit are in the deep tests. } diff --git a/DemoBenchmarks/TwoBodyConstraintsBenchmarksDeep.cs b/DemoBenchmarks/TwoBodyConstraintsBenchmarksDeep.cs new file mode 100644 index 000000000..ce8966699 --- /dev/null +++ b/DemoBenchmarks/TwoBodyConstraintsBenchmarksDeep.cs @@ -0,0 +1,298 @@ +using BenchmarkDotNet.Attributes; +using BepuPhysics; +using BepuPhysics.Constraints; +using BepuPhysics.Constraints.Contact; +using BepuUtilities; +using System.Numerics; +using static DemoBenchmarks.TwoBodyConstraintBenchmarks; + +namespace DemoBenchmarks; + +/// +/// Evaluates performance of all two body constraints excluded from . +/// +/// +/// Note that all constraints operate across lanes simultaneously where T is of type . +/// The number of bundles being executed does not change if changes; if larger bundles are allowed, then more lanes end up getting solved. +/// +public class TwoBodyConstraintBenchmarksDeep +{ + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide) Contact1() + { + var prestep = new Contact1PrestepData + { + Contact0 = new ConvexContactWide { Depth = Vector.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) }, + MaterialProperties = new MaterialPropertiesWide + { + FrictionCoefficient = new Vector(1f), + MaximumRecoveryVelocity = new Vector(2f), + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } + }, + Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)), + OffsetB = Vector3Wide.Broadcast(new Vector3(2, 0, 0)) + }; + + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkTwoBodyConstraint(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); + } + + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide) Contact2() + { + var prestep = new Contact2PrestepData + { + Contact0 = new ConvexContactWide { Depth = Vector.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) }, + Contact1 = new ConvexContactWide { Depth = Vector.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) }, + MaterialProperties = new MaterialPropertiesWide + { + FrictionCoefficient = new Vector(1f), + MaximumRecoveryVelocity = new Vector(2f), + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } + }, + Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)), + OffsetB = Vector3Wide.Broadcast(new Vector3(2, 0, 0)) + }; + + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkTwoBodyConstraint(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); + } + + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide) Contact3() + { + var prestep = new Contact3PrestepData + { + Contact0 = new ConvexContactWide { Depth = Vector.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) }, + Contact1 = new ConvexContactWide { Depth = Vector.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) }, + Contact2 = new ConvexContactWide { Depth = Vector.Zero, OffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)) }, + MaterialProperties = new MaterialPropertiesWide + { + FrictionCoefficient = new Vector(1f), + MaximumRecoveryVelocity = new Vector(2f), + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } + }, + Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)), + OffsetB = Vector3Wide.Broadcast(new Vector3(2, 0, 0)) + }; + + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkTwoBodyConstraint(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); + } + + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide) Contact2Nonconvex() + { + var prestep = new Contact2NonconvexPrestepData + { + Contact0 = new NonconvexContactPrestepData { Depth = Vector.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) }, + Contact1 = new NonconvexContactPrestepData { Depth = Vector.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 0, 1)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) }, + MaterialProperties = new MaterialPropertiesWide + { + FrictionCoefficient = new Vector(1f), + MaximumRecoveryVelocity = new Vector(2f), + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } + }, + OffsetB = Vector3Wide.Broadcast(new Vector3(2, 0, 0)) + }; + + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkTwoBodyConstraint, + Contact2NonconvexPrestepData, Contact2NonconvexAccumulatedImpulses>(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); + } + + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide) Contact3Nonconvex() + { + var prestep = new Contact3NonconvexPrestepData + { + Contact0 = new NonconvexContactPrestepData { Depth = Vector.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) }, + Contact1 = new NonconvexContactPrestepData { Depth = Vector.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 0, 1)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) }, + Contact2 = new NonconvexContactPrestepData { Depth = Vector.Zero, Offset = Vector3Wide.Broadcast(new Vector3(1, 1, 0)), Normal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)) }, + MaterialProperties = new MaterialPropertiesWide + { + FrictionCoefficient = new Vector(1f), + MaximumRecoveryVelocity = new Vector(2f), + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } + }, + OffsetB = Vector3Wide.Broadcast(new Vector3(2, 0, 0)) + }; + + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkTwoBodyConstraint, + Contact3NonconvexPrestepData, Contact3NonconvexAccumulatedImpulses>(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); + } + + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide) TwistLimit() + { + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var prestep = new TwistLimitPrestepData + { + LocalBasisA = orientation, + LocalBasisB = orientation, + MinimumAngle = Vector.Zero, + MaximumAngle = Vector.One, + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } + }; + + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkTwoBodyConstraint>(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); + } + + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide) TwistMotor() + { + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var prestep = new TwistMotorPrestepData + { + LocalAxisA = Vector3Wide.Broadcast(new Vector3(0, 1, 0)), + LocalAxisB = Vector3Wide.Broadcast(new Vector3(0, 1, 0)), + Settings = new() { Damping = Vector.One, MaximumForce = new Vector(float.MaxValue) } + }; + + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkTwoBodyConstraint>(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); + } + + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide) AngularMotor() + { + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var prestep = new AngularMotorPrestepData + { + Settings = new() { Damping = Vector.One, MaximumForce = new Vector(float.MaxValue) } + }; + + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkTwoBodyConstraint(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); + } + + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide) DistanceLimit() + { + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var prestep = new DistanceLimitPrestepData + { + LocalOffsetA = Vector3Wide.Broadcast(new Vector3(0.5f, 0, 0)), + LocalOffsetB = Vector3Wide.Broadcast(new Vector3(-0.5f, 0, 0)), + MinimumDistance = new Vector(0.5f), + MaximumDistance = new Vector(1.5f), + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } + }; + + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkTwoBodyConstraint>(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); + } + + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide) LinearAxisMotor() + { + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var prestep = new LinearAxisMotorPrestepData + { + LocalOffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), + LocalOffsetB = Vector3Wide.Broadcast(new Vector3(-1, 0, 0)), + LocalPlaneNormal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)), + Settings = new() { Damping = Vector.One, MaximumForce = new Vector(float.MaxValue) } + }; + + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkTwoBodyConstraint>(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); + } + + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide) LinearAxisLimit() + { + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var prestep = new LinearAxisLimitPrestepData + { + LocalOffsetA = Vector3Wide.Broadcast(new Vector3(1, 0, 0)), + LocalOffsetB = Vector3Wide.Broadcast(new Vector3(-1, 0, 0)), + LocalPlaneNormal = Vector3Wide.Broadcast(new Vector3(0, 1, 0)), + MaximumOffset = new Vector(3), + MinimumOffset = new Vector(-3), + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } + }; + + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkTwoBodyConstraint>(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); + } + + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide) AngularAxisMotor() + { + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var prestep = new AngularAxisMotorPrestepData + { + LocalAxisA = Vector3Wide.Broadcast(new Vector3(0, 1, 0)), + Settings = new() { Damping = Vector.One, MaximumForce = new Vector(float.MaxValue) } + }; + + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkTwoBodyConstraint>(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); + } + + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide) BallSocketServo() + { + var prestep = new BallSocketServoPrestepData + { + ServoSettings = new ServoSettingsWide { MaximumForce = new Vector(float.MaxValue), MaximumSpeed = new Vector(float.MaxValue) }, + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } + }; + + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkTwoBodyConstraint(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); + } + + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide) BallSocketMotor() + { + var prestep = new BallSocketMotorPrestepData + { + Settings = new() { Damping = Vector.One, MaximumForce = new Vector(float.MaxValue) } + }; + + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkTwoBodyConstraint(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); + } + + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide) AngularAxisGearMotor() + { + var prestep = new AngularAxisGearMotorPrestepData + { + LocalAxisA = Vector3Wide.Broadcast(new Vector3(0, 1, 0)), + VelocityScale = Vector.One, + Settings = new() { Damping = Vector.One, MaximumForce = new Vector(float.MaxValue) } + }; + + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkTwoBodyConstraint>(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); + } + + [Benchmark] + public (BodyVelocityWide, BodyVelocityWide) CenterDistanceLimit() + { + var prestep = new CenterDistanceLimitPrestepData + { + MinimumDistance = new Vector(0.2f), + MaximumDistance = new Vector(3), + SpringSettings = new() { TwiceDampingRatio = new Vector(2), AngularFrequency = new Vector(20 * MathF.PI) } + }; + + QuaternionWide.Broadcast(Quaternion.Identity, out var orientation); + var inertia = new BodyInertiaWide { InverseInertiaTensor = new Symmetric3x3Wide { XX = Vector.One, YY = Vector.One, ZZ = Vector.One }, InverseMass = Vector.One }; + return BenchmarkTwoBodyConstraint>(new Vector3Wide(), orientation, inertia, Vector3Wide.Broadcast(new Vector3(2, 0, 0)), orientation, inertia, prestep); + } + +} From d9b1eea65b55f43e61b39de51f34db3a0d2cf5a1 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 28 Oct 2022 20:21:17 -0500 Subject: [PATCH 559/947] Obligatory oopses. --- ...yConstraintsBenchmarks.cs => OneBodyConstraintBenchmarks.cs} | 0 ...intsBenchmarksDeep.cs => OneBodyConstraintBenchmarksDeep.cs} | 0 DemoBenchmarks/README.md | 2 +- ...yConstraintsBenchmarks.cs => TwoBodyConstraintBenchmarks.cs} | 0 ...intsBenchmarksDeep.cs => TwoBodyConstraintBenchmarksDeep.cs} | 0 5 files changed, 1 insertion(+), 1 deletion(-) rename DemoBenchmarks/{OneBodyConstraintsBenchmarks.cs => OneBodyConstraintBenchmarks.cs} (100%) rename DemoBenchmarks/{OneBodyConstraintsBenchmarksDeep.cs => OneBodyConstraintBenchmarksDeep.cs} (100%) rename DemoBenchmarks/{TwoBodyConstraintsBenchmarks.cs => TwoBodyConstraintBenchmarks.cs} (100%) rename DemoBenchmarks/{TwoBodyConstraintsBenchmarksDeep.cs => TwoBodyConstraintBenchmarksDeep.cs} (100%) diff --git a/DemoBenchmarks/OneBodyConstraintsBenchmarks.cs b/DemoBenchmarks/OneBodyConstraintBenchmarks.cs similarity index 100% rename from DemoBenchmarks/OneBodyConstraintsBenchmarks.cs rename to DemoBenchmarks/OneBodyConstraintBenchmarks.cs diff --git a/DemoBenchmarks/OneBodyConstraintsBenchmarksDeep.cs b/DemoBenchmarks/OneBodyConstraintBenchmarksDeep.cs similarity index 100% rename from DemoBenchmarks/OneBodyConstraintsBenchmarksDeep.cs rename to DemoBenchmarks/OneBodyConstraintBenchmarksDeep.cs diff --git a/DemoBenchmarks/README.md b/DemoBenchmarks/README.md index 37df8f4b9..0a3a4834a 100644 --- a/DemoBenchmarks/README.md +++ b/DemoBenchmarks/README.md @@ -7,7 +7,7 @@ For a partial but pretty high coverage set of benchmarks, consider running with -f *CollisionBatcherTaskBenchmarks.* *GroupedCollisionTesterBenchmarks.* *GatherScatterBenchmarks.* *OneBodyConstraintBenchmarks.* *TwoBodyConstraintBenchmarks.* *ThreeBodyConstraintBenchmarks.* *FourBodyConstraintBenchmarks.* *SweepBenchmarks.* *ShapeRayBenchmarks.* *ShapePileBenchmark.* *RagdollTubeBenchmark.* ``` -For a longer complete set of benchmarkers, consider running with a filter of: +For a longer complete set of benchmarks, consider running with a filter of: ``` -f *CollisionBatcherTaskBenchmarks.* *ConvexCollisionTesterBenchmarks.* *GatherScatterBenchmarks.* *OneBodyConstraintBenchmarks.* *OneBodyConstraintBenchmarksDeep.* *TwoBodyConstraintBenchmarks.* *TwoBodyConstraintBenchmarksDeep.* *ThreeBodyConstraintBenchmarks.* *FourBodyConstraintBenchmarks.* *SweepBenchmarks.* *SweepBenchmarksDeep.* *ShapeRayBenchmarksDeep.* *ShapePileBenchmark.* *RagdollTubeBenchmark.* ``` diff --git a/DemoBenchmarks/TwoBodyConstraintsBenchmarks.cs b/DemoBenchmarks/TwoBodyConstraintBenchmarks.cs similarity index 100% rename from DemoBenchmarks/TwoBodyConstraintsBenchmarks.cs rename to DemoBenchmarks/TwoBodyConstraintBenchmarks.cs diff --git a/DemoBenchmarks/TwoBodyConstraintsBenchmarksDeep.cs b/DemoBenchmarks/TwoBodyConstraintBenchmarksDeep.cs similarity index 100% rename from DemoBenchmarks/TwoBodyConstraintsBenchmarksDeep.cs rename to DemoBenchmarks/TwoBodyConstraintBenchmarksDeep.cs From b91313c46db6afa67716ac823e8e50b57fca929a Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 19 Nov 2022 18:10:43 -0600 Subject: [PATCH 560/947] Added a Buffer helperconstructor to match Quick* collections. --- BepuUtilities/Memory/Buffer.cs | 20 +++++++++++++++++++- 1 file changed, 19 insertions(+), 1 deletion(-) diff --git a/BepuUtilities/Memory/Buffer.cs b/BepuUtilities/Memory/Buffer.cs index a3c13f011..7734fa070 100644 --- a/BepuUtilities/Memory/Buffer.cs +++ b/BepuUtilities/Memory/Buffer.cs @@ -36,7 +36,6 @@ public unsafe struct Buffer where T : unmanaged /// Memory to back the buffer. /// Length of the buffer in terms of the specified type. /// Id of the buffer. - [MethodImpl(MethodImplOptions.AggressiveInlining)] public Buffer(void* memory, int length, int id = -1) { Memory = (T*)memory; @@ -44,6 +43,25 @@ public Buffer(void* memory, int length, int id = -1) Id = id; } + /// + /// Allocates a new buffer from a pool. + /// + /// Length of the buffer in terms of elements of type T + /// Pool to allocate from. + public Buffer(int length, IUnmanagedMemoryPool pool) + { + pool.Take(length, out this); + } + + /// + /// Returns a buffer to a pool. This should only be used if the specified pool is the same as the one used to allocate the buffer. + /// + /// Pool to return the buffer to. + public void Dispose(IUnmanagedMemoryPool pool) + { + pool.Return(ref this); + } + /// /// Gets a typed reference from a byte buffer by an index in terms of the type. /// From 5353c20b02f2382b8ce5bb8c82d4810df26676e3 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 17 Nov 2022 15:09:56 -0600 Subject: [PATCH 561/947] Some documentation. --- .../CollisionTaskRegistry.cs | 64 ++++++++++++++++++- 1 file changed, 62 insertions(+), 2 deletions(-) diff --git a/BepuPhysics/CollisionDetection/CollisionTaskRegistry.cs b/BepuPhysics/CollisionDetection/CollisionTaskRegistry.cs index e94706feb..ee9879789 100644 --- a/BepuPhysics/CollisionDetection/CollisionTaskRegistry.cs +++ b/BepuPhysics/CollisionDetection/CollisionTaskRegistry.cs @@ -5,9 +5,18 @@ namespace BepuPhysics.CollisionDetection { + /// + /// Callbacks invoked by a . + /// public interface ICollisionCallbacks { //TODO: In the future, continuations will need to be able to take typed collision caches. The PairCache will store cached separating axes for hull-hull acceleration and similar things. + /// + /// Called when a pair submitted to a collision batcher has finished collision detection. + /// + /// Type of the contact manifold generated by collision detection. + /// Id of the pair that completed. + /// Contact manifold generated by collision testing. unsafe void OnPairCompleted(int pairId, ref TManifold manifold) where TManifold : unmanaged, IContactManifold; /// @@ -29,7 +38,9 @@ public interface ICollisionCallbacks bool AllowCollisionTesting(int pairId, int childA, int childB); } - + /// + /// Parent type of tasks which handle collision tests between batches of shapes of a particular type. + /// public abstract class CollisionTask { /// @@ -68,6 +79,9 @@ public abstract void ExecuteBatch(ref UntypedList batch, ref Collisi } + /// + /// Describes the data requirements for a collision pair type in a . + /// public enum CollisionTaskPairType { /// @@ -93,20 +107,43 @@ public enum CollisionTaskPairType } + /// + /// Metadata about a collision task. + /// public struct CollisionTaskReference { + /// + /// Index of the task in the registry. + /// public int TaskIndex; + /// + /// Number of pairs to accumulate in a batch before dispatching tests. + /// public int BatchSize; + /// + /// The type id that is expected to come first in the collision pair. + /// public int ExpectedFirstTypeId; + /// + /// Data requirements for the collision pair type in a . + /// public CollisionTaskPairType PairType; } + /// + /// Registry of collision tasks used to handle various shape pair types. + /// public class CollisionTaskRegistry { CollisionTaskReference[][] topLevelMatrix; internal CollisionTask[] tasks; int count; - + + /// + /// Gets the collision task associated with a task index. + /// + /// Task index to look up. + /// Task associated with the task index. public CollisionTask this[int taskIndex] { [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -116,6 +153,10 @@ public CollisionTask this[int taskIndex] } } + /// + /// Creates a new collision task registry. + /// + /// Initial number of shape types to allocate space for in the registry. public CollisionTaskRegistry(int initialShapeCount = 9) { ResizeMatrix(initialShapeCount); @@ -135,6 +176,11 @@ void ResizeMatrix(int newSize) } } + /// + /// Registers a collision task. + /// + /// Task to register. + /// Index of the task in the registry. public int Register(CollisionTask task) { //Some tasks can generate tasks. Note that this can only be one level deep; nesting compounds is not allowed. @@ -206,11 +252,25 @@ public int Register(CollisionTask task) return index; } + + /// + /// Gets metadata about the task associated with a shape type pair. + /// + /// Type index of the first shape. + /// Type index of the second shape. + /// Reference to the metadata for the task. [MethodImpl(MethodImplOptions.AggressiveInlining)] public ref CollisionTaskReference GetTaskReference(int topLevelTypeA, int topLevelTypeB) { return ref topLevelMatrix[topLevelTypeA][topLevelTypeB]; } + + /// + /// Gets metadata about the task associated with a shape type pair. + /// + /// Type of the first shape. + /// Type of the second shape. + /// Reference to the metadata for the task. [MethodImpl(MethodImplOptions.AggressiveInlining)] public ref CollisionTaskReference GetTaskReference() where TShapeA : unmanaged, IShape From b5a21f6780027c432c301115bb3c37fb269c70e9 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 19 Nov 2022 18:06:01 -0600 Subject: [PATCH 562/947] PairCache no longer preserves worker caches across multiple timesteps. Massive simplification of related code and very tiny speedup. --- .../CollisionDetection/ConstraintCache.cs | 101 ------- .../ContactConstraintAccessor.cs | 101 +++---- .../CollisionDetection/FreshnessChecker.cs | 12 +- .../CollisionDetection/InactiveSetBuilder.cs | 154 +--------- .../NarrowPhaseCCDContinuations.cs | 5 +- .../NarrowPhaseConstraintUpdate.cs | 66 ++--- .../NarrowPhasePendingConstraintAdds.cs | 19 +- .../CollisionDetection/NarrowPhasePreflush.cs | 3 +- BepuPhysics/CollisionDetection/PairCache.cs | 263 +++++++----------- .../CollisionDetection/PairCacheIndex.cs | 92 ------ .../CollisionDetection/PairCache_Activity.cs | 88 +----- .../CollisionDetection/WorkerPairCache.cs | 173 +----------- BepuPhysics/DefaultTypes.cs | 32 +-- BepuPhysics/IslandAwakener.cs | 82 +----- BepuPhysics/IslandSleeper.cs | 2 +- Demos/SpecializedTests/ShapePileTestDemo.cs | 2 +- 16 files changed, 235 insertions(+), 960 deletions(-) delete mode 100644 BepuPhysics/CollisionDetection/ConstraintCache.cs delete mode 100644 BepuPhysics/CollisionDetection/PairCacheIndex.cs diff --git a/BepuPhysics/CollisionDetection/ConstraintCache.cs b/BepuPhysics/CollisionDetection/ConstraintCache.cs deleted file mode 100644 index 1f4b7bc9c..000000000 --- a/BepuPhysics/CollisionDetection/ConstraintCache.cs +++ /dev/null @@ -1,101 +0,0 @@ -using System; -using System.Collections.Generic; -using System.Text; - -namespace BepuPhysics.CollisionDetection -{ - public interface IPairCacheEntry - { - /// - /// Gets the cache's type id. - /// Note that this is not the same as a constraint type id or other type ids; it only refers to the type of the caches for storage within the PairCache's structures. - /// - int CacheTypeId { get; } - } - public struct ConstraintCache1 : IPairCacheEntry - { - public int ConstraintHandle; - public int FeatureId0; - - public int CacheTypeId => 0; - } - public struct ConstraintCache2 : IPairCacheEntry - { - public int ConstraintHandle; - public int FeatureId0; - public int FeatureId1; - - public int CacheTypeId => 1; - } - public struct ConstraintCache3 : IPairCacheEntry - { - public int ConstraintHandle; - public int FeatureId0; - public int FeatureId1; - public int FeatureId2; - - public int CacheTypeId => 2; - } - public struct ConstraintCache4 : IPairCacheEntry - { - public int ConstraintHandle; - public int FeatureId0; - public int FeatureId1; - public int FeatureId2; - public int FeatureId3; - - public int CacheTypeId => 3; - } - - public struct ConstraintCache5 : IPairCacheEntry - { - public int ConstraintHandle; - public int FeatureId0; - public int FeatureId1; - public int FeatureId2; - public int FeatureId3; - public int FeatureId4; - - public int CacheTypeId => 4; - } - public struct ConstraintCache6 : IPairCacheEntry - { - public int ConstraintHandle; - public int FeatureId0; - public int FeatureId1; - public int FeatureId2; - public int FeatureId3; - public int FeatureId4; - public int FeatureId5; - - public int CacheTypeId => 5; - } - public struct ConstraintCache7 : IPairCacheEntry - { - public int ConstraintHandle; - public int FeatureId0; - public int FeatureId1; - public int FeatureId2; - public int FeatureId3; - public int FeatureId4; - public int FeatureId5; - public int FeatureId6; - - public int CacheTypeId => 6; - } - public struct ConstraintCache8 : IPairCacheEntry - { - public int ConstraintHandle; - public int FeatureId0; - public int FeatureId1; - public int FeatureId2; - public int FeatureId3; - public int FeatureId4; - public int FeatureId5; - public int FeatureId6; - public int FeatureId7; - - public int CacheTypeId => 7; - } - -} diff --git a/BepuPhysics/CollisionDetection/ContactConstraintAccessor.cs b/BepuPhysics/CollisionDetection/ContactConstraintAccessor.cs index 7e1b9c85e..c551334ec 100644 --- a/BepuPhysics/CollisionDetection/ContactConstraintAccessor.cs +++ b/BepuPhysics/CollisionDetection/ContactConstraintAccessor.cs @@ -92,11 +92,10 @@ public abstract void FlushWithSpeculativeBatches(ref UntypedList lis public abstract void FlushSequentially(ref UntypedList list, int narrowPhaseConstraintTypeId, Simulation simulation, PairCache pairCache) where TCallbacks : struct, INarrowPhaseCallbacks; - public abstract unsafe void UpdateConstraintForManifold( + public abstract unsafe void UpdateConstraintForManifold( NarrowPhase narrowPhase, int manifoldTypeAsConstraintType, int workerIndex, - ref CollidablePair pair, ref TContactManifold manifoldPointer, ref TCollisionCache collisionCache, ref PairMaterialProperties material, TCallBodyHandles bodyHandles) - where TCallbacks : struct, INarrowPhaseCallbacks - where TCollisionCache : unmanaged, IPairCacheEntry; + ref CollidablePair pair, ref TContactManifold manifoldPointer, ref PairMaterialProperties material, TCallBodyHandles bodyHandles) + where TCallbacks : struct, INarrowPhaseCallbacks; /// /// Extracts references to data from a contact constraint of the accessor's type. @@ -144,11 +143,10 @@ public void ExtractContactPrestepAndImpulses(ConstraintHandle constr } //Note that the vast majority of the 'work' done by these accessor implementations is just type definitions used to call back into some other functions that need that type knowledge. - public abstract class ContactConstraintAccessor : ContactConstraintAccessor + public abstract class ContactConstraintAccessor : ContactConstraintAccessor where TBodyHandles : unmanaged where TConstraintDescription : unmanaged, IConstraintDescription where TContactImpulses : unmanaged - where TConstraintCache : unmanaged, IPairCacheEntry where TPrestepData : unmanaged { protected ContactConstraintAccessor() @@ -183,10 +181,6 @@ protected ContactConstraintAccessor() Debug.Assert(ContactCount * 3 * Unsafe.SizeOf>() == Unsafe.SizeOf(), "The layout of nonconvex accumulated impulses seems to have changed; the assumptions of impulse gather/scatter are probably no longer valid."); } - //Note that this test has to special case count == 1; 1 contact manifolds have no feature ids. - Debug.Assert(Unsafe.SizeOf() == sizeof(int) * (1 + ContactCount) && - default(TConstraintCache).CacheTypeId == ContactCount - 1, - "The type of the constraint cache should hold as many contacts as the contact impulses requires."); AccumulatedImpulseBundleStrideInBytes = Unsafe.SizeOf(); ConstraintTypeId = default(TConstraintDescription).ConstraintTypeId; } @@ -213,29 +207,28 @@ public override void FlushWithSpeculativeBatches(ref UntypedList lis } [MethodImpl(MethodImplOptions.AggressiveInlining)] - protected static unsafe void UpdateConstraint( + protected static unsafe void UpdateConstraint( NarrowPhase narrowPhase, int manifoldTypeAsConstraintType, int workerIndex, - ref CollidablePair pair, ref TConstraintCache constraintCache, ref TCollisionCache collisionCache, ref TConstraintDescription description, TCallBodyHandles bodyHandles) - where TCallbacks : struct, INarrowPhaseCallbacks where TCollisionCache : unmanaged, IPairCacheEntry + ref CollidablePair pair, ref ConstraintCache constraintCache, int newContactCount, ref TConstraintDescription description, TCallBodyHandles bodyHandles) + where TCallbacks : struct, INarrowPhaseCallbacks { //Note that we let the user pass in a body handles type to a generic function, rather than requiring that the top level abstract class define the type. //That allows a type inconsistency, but it's easy to catch. Debug.Assert(typeof(TCallBodyHandles) == typeof(TBodyHandles), "Don't call an update with inconsistent body handle types."); - narrowPhase.UpdateConstraint( - workerIndex, ref pair, manifoldTypeAsConstraintType, ref constraintCache, ref collisionCache, ref description, Unsafe.As(ref bodyHandles)); + narrowPhase.UpdateConstraint( + workerIndex, pair, manifoldTypeAsConstraintType, ref constraintCache, newContactCount, ref description, Unsafe.As(ref bodyHandles)); } - protected static void CopyContactData(ref ConvexContactManifold manifold, out TConstraintCache constraintCache, out TConstraintDescription description) + protected static void CopyContactData(ref ConvexContactManifold manifold, out ConstraintCache constraintCache, out TConstraintDescription description) { //TODO: Unnecessary zero inits. Unsafe.SkipInit would help here once available. Could also hack away with pointers. constraintCache = default; description = default; //This should be a compilation time constant provided an inlined constant property. - var contactCount = constraintCache.CacheTypeId + 1; - Debug.Assert(contactCount == manifold.Count, "Relying on generic specialization; should be the same value!"); + var contactCount = manifold.Count; //Contact data comes first in the constraint description memory layout. ref var targetContacts = ref Unsafe.As(ref description); - ref var targetFeatureIds = ref Unsafe.Add(ref Unsafe.As(ref constraintCache), 1); + ref var targetFeatureIds = ref constraintCache.FeatureId0; for (int i = 0; i < contactCount; ++i) { ref var sourceContact = ref Unsafe.Add(ref manifold.Contact0, i); @@ -245,12 +238,11 @@ protected static void CopyContactData(ref ConvexContactManifold manifold, out TC targetContact.PenetrationDepth = sourceContact.Depth; } } - protected static void CopyContactData(ref NonconvexContactManifold manifold, ref TConstraintCache constraintCache, ref NonconvexConstraintContactData targetContacts) + protected static void CopyContactData(ref NonconvexContactManifold manifold, ref ConstraintCache constraintCache, ref NonconvexConstraintContactData targetContacts) { //TODO: Check codegen. This should be a compilation time constant. If it's not, just use the ContactCount that we cached. - var contactCount = constraintCache.CacheTypeId + 1; - Debug.Assert(contactCount == manifold.Count, "Relying on generic specialization; should be the same value!"); - ref var targetFeatureIds = ref Unsafe.Add(ref Unsafe.As(ref constraintCache), 1); + var contactCount = manifold.Count; + ref var targetFeatureIds = ref constraintCache.FeatureId0; for (int i = 0; i < contactCount; ++i) { ref var sourceContact = ref Unsafe.Add(ref manifold.Contact0, i); @@ -261,30 +253,28 @@ protected static void CopyContactData(ref NonconvexContactManifold manifold, ref targetContact.PenetrationDepth = sourceContact.Depth; } } - protected static void CopyContactData(ref NonconvexContactManifold manifold, ref TConstraintCache constraintCache, ref ConstraintContactData targetContacts) + protected static void CopyContactData(ref NonconvexContactManifold manifold, ref ConstraintCache constraintCache, ref ConstraintContactData targetContacts) { - Debug.Assert(manifold.Count == 1, "Nonconvex manifolds used to create convex constraints must only have one contact."); //TODO: Check codegen. This should be a compilation time constant. If it's not, just use the ContactCount that we cached. - var contactCount = constraintCache.CacheTypeId + 1; + var contactCount = manifold.Count; Debug.Assert(contactCount == manifold.Count, "Relying on generic specialization; should be the same value!"); - Unsafe.Add(ref Unsafe.As(ref constraintCache), 1) = manifold.Contact0.FeatureId; + constraintCache.FeatureId0 = manifold.Contact0.FeatureId; targetContacts.OffsetA = manifold.Contact0.Offset; targetContacts.PenetrationDepth = manifold.Contact0.Depth; } } - public class ConvexOneBodyAccessor : - ContactConstraintAccessor + public class ConvexOneBodyAccessor : + ContactConstraintAccessor where TConstraintDescription : unmanaged, IConvexOneBodyContactConstraintDescription where TContactImpulses : unmanaged - where TConstraintCache : unmanaged, IPairCacheEntry where TPrestepData : unmanaged, IConvexContactPrestep where TAccumulatedImpulses : unmanaged, IConvexContactAccumulatedImpulses { - public override void UpdateConstraintForManifold( + public override void UpdateConstraintForManifold( NarrowPhase narrowPhase, int manifoldTypeAsConstraintType, int workerIndex, - ref CollidablePair pair, ref TContactManifold manifoldPointer, ref TCollisionCache collisionCache, ref PairMaterialProperties material, TCallBodyHandles bodyHandles) + ref CollidablePair pair, ref TContactManifold manifoldPointer, ref PairMaterialProperties material, TCallBodyHandles bodyHandles) { Debug.Assert(typeof(TCallBodyHandles) == typeof(int)); if (typeof(TContactManifold) == typeof(ConvexContactManifold)) @@ -292,18 +282,18 @@ public override void UpdateConstraintForManifold(ref manifoldPointer); CopyContactData(ref manifold, out var constraintCache, out var description); description.CopyManifoldWideProperties(ref manifold.Normal, ref material); - UpdateConstraint(narrowPhase, manifoldTypeAsConstraintType, workerIndex, ref pair, ref constraintCache, ref collisionCache, ref description, bodyHandles); + UpdateConstraint(narrowPhase, manifoldTypeAsConstraintType, workerIndex, ref pair, ref constraintCache, manifold.Count, ref description, bodyHandles); } else { Debug.Assert(typeof(TContactManifold) == typeof(NonconvexContactManifold)); ref var manifold = ref Unsafe.As(ref manifoldPointer); Debug.Assert(manifold.Count == 1, "Nonconvex manifolds should only result in convex constraints when the contact count is 1."); - Unsafe.SkipInit(out TConstraintCache constraintCache); + Unsafe.SkipInit(out ConstraintCache constraintCache); Unsafe.SkipInit(out TConstraintDescription description); CopyContactData(ref manifold, ref constraintCache, ref description.GetFirstContact(ref description)); description.CopyManifoldWideProperties(ref manifold.Contact0.Normal, ref material); - UpdateConstraint(narrowPhase, manifoldTypeAsConstraintType, workerIndex, ref pair, ref constraintCache, ref collisionCache, ref description, bodyHandles); + UpdateConstraint(narrowPhase, manifoldTypeAsConstraintType, workerIndex, ref pair, ref constraintCache, manifold.Count, ref description, bodyHandles); } } @@ -333,17 +323,16 @@ public override void ExtractContactPrestepAndImpulses(in ConstraintL } } - public class ConvexTwoBodyAccessor : - ContactConstraintAccessor + public class ConvexTwoBodyAccessor : + ContactConstraintAccessor where TConstraintDescription : unmanaged, IConvexTwoBodyContactConstraintDescription where TContactImpulses : unmanaged - where TConstraintCache : unmanaged, IPairCacheEntry where TPrestepData : unmanaged, ITwoBodyConvexContactPrestep where TAccumulatedImpulses : unmanaged, IConvexContactAccumulatedImpulses { - public override void UpdateConstraintForManifold( + public override void UpdateConstraintForManifold( NarrowPhase narrowPhase, int manifoldTypeAsConstraintType, int workerIndex, - ref CollidablePair pair, ref TContactManifold manifoldPointer, ref TCollisionCache collisionCache, ref PairMaterialProperties material, TCallBodyHandles bodyHandles) + ref CollidablePair pair, ref TContactManifold manifoldPointer, ref PairMaterialProperties material, TCallBodyHandles bodyHandles) { Debug.Assert(typeof(TCallBodyHandles) == typeof(TwoBodyHandles)); if (typeof(TContactManifold) == typeof(ConvexContactManifold)) @@ -351,18 +340,18 @@ public override void UpdateConstraintForManifold(ref manifoldPointer); CopyContactData(ref manifold, out var constraintCache, out var description); description.CopyManifoldWideProperties(ref manifold.OffsetB, ref manifold.Normal, ref material); - UpdateConstraint(narrowPhase, manifoldTypeAsConstraintType, workerIndex, ref pair, ref constraintCache, ref collisionCache, ref description, bodyHandles); + UpdateConstraint(narrowPhase, manifoldTypeAsConstraintType, workerIndex, ref pair, ref constraintCache, manifold.Count, ref description, bodyHandles); } else { Debug.Assert(typeof(TContactManifold) == typeof(NonconvexContactManifold)); ref var manifold = ref Unsafe.As(ref manifoldPointer); Debug.Assert(manifold.Count == 1, "Nonconvex manifolds should only result in convex constraints when the contact count is 1."); - Unsafe.SkipInit(out TConstraintCache constraintCache); + Unsafe.SkipInit(out ConstraintCache constraintCache); Unsafe.SkipInit(out TConstraintDescription description); CopyContactData(ref manifold, ref constraintCache, ref description.GetFirstContact(ref description)); description.CopyManifoldWideProperties(ref manifold.OffsetB, ref manifold.Contact0.Normal, ref material); - UpdateConstraint(narrowPhase, manifoldTypeAsConstraintType, workerIndex, ref pair, ref constraintCache, ref collisionCache, ref description, bodyHandles); + UpdateConstraint(narrowPhase, manifoldTypeAsConstraintType, workerIndex, ref pair, ref constraintCache, manifold.Count, ref description, bodyHandles); } } public override void ExtractContactData(in ConstraintLocation constraintLocation, Solver solver, ref TExtractor extractor) @@ -402,25 +391,24 @@ public override void ExtractContactPrestepAndImpulses(in ConstraintL } } - public class NonconvexOneBodyAccessor : - ContactConstraintAccessor + public class NonconvexOneBodyAccessor : + ContactConstraintAccessor where TConstraintDescription : unmanaged, INonconvexOneBodyContactConstraintDescription where TContactImpulses : unmanaged - where TConstraintCache : unmanaged, IPairCacheEntry where TPrestepData : unmanaged, INonconvexContactPrestep where TAccumulatedImpulses : unmanaged, INonconvexContactAccumulatedImpulses { - public override void UpdateConstraintForManifold( + public override void UpdateConstraintForManifold( NarrowPhase narrowPhase, int manifoldTypeAsConstraintType, int workerIndex, - ref CollidablePair pair, ref TContactManifold manifoldPointer, ref TCollisionCache collisionCache, ref PairMaterialProperties material, TCallBodyHandles bodyHandles) + ref CollidablePair pair, ref TContactManifold manifoldPointer, ref PairMaterialProperties material, TCallBodyHandles bodyHandles) { Debug.Assert(typeof(TCallBodyHandles) == typeof(int)); ref var manifold = ref Unsafe.As(ref manifoldPointer); - Unsafe.SkipInit(out TConstraintCache constraintCache); + Unsafe.SkipInit(out ConstraintCache constraintCache); Unsafe.SkipInit(out TConstraintDescription description); CopyContactData(ref manifold, ref constraintCache, ref description.GetFirstContact(ref description)); description.CopyManifoldWideProperties(ref material); - UpdateConstraint(narrowPhase, manifoldTypeAsConstraintType, workerIndex, ref pair, ref constraintCache, ref collisionCache, ref description, bodyHandles); + UpdateConstraint(narrowPhase, manifoldTypeAsConstraintType, workerIndex, ref pair, ref constraintCache, manifold.Count, ref description, bodyHandles); } public override void ExtractContactData(in ConstraintLocation constraintLocation, Solver solver, ref TExtractor extractor) @@ -449,25 +437,24 @@ public override void ExtractContactPrestepAndImpulses(in ConstraintL } } - public class NonconvexTwoBodyAccessor : - ContactConstraintAccessor + public class NonconvexTwoBodyAccessor : + ContactConstraintAccessor where TConstraintDescription : unmanaged, INonconvexTwoBodyContactConstraintDescription where TContactImpulses : unmanaged - where TConstraintCache : unmanaged, IPairCacheEntry where TPrestepData : unmanaged, ITwoBodyNonconvexContactPrestep where TAccumulatedImpulses : unmanaged, INonconvexContactAccumulatedImpulses { - public override void UpdateConstraintForManifold( + public override void UpdateConstraintForManifold( NarrowPhase narrowPhase, int manifoldTypeAsConstraintType, int workerIndex, - ref CollidablePair pair, ref TContactManifold manifoldPointer, ref TCollisionCache collisionCache, ref PairMaterialProperties material, TCallBodyHandles bodyHandles) + ref CollidablePair pair, ref TContactManifold manifoldPointer, ref PairMaterialProperties material, TCallBodyHandles bodyHandles) { Debug.Assert(typeof(TCallBodyHandles) == typeof(TwoBodyHandles)); ref var manifold = ref Unsafe.As(ref manifoldPointer); - Unsafe.SkipInit(out TConstraintCache constraintCache); + Unsafe.SkipInit(out ConstraintCache constraintCache); Unsafe.SkipInit(out TConstraintDescription description); CopyContactData(ref manifold, ref constraintCache, ref description.GetFirstContact(ref description)); description.CopyManifoldWideProperties(ref manifold.OffsetB, ref material); - UpdateConstraint(narrowPhase, manifoldTypeAsConstraintType, workerIndex, ref pair, ref constraintCache, ref collisionCache, ref description, bodyHandles); + UpdateConstraint(narrowPhase, manifoldTypeAsConstraintType, workerIndex, ref pair, ref constraintCache, manifold.Count, ref description, bodyHandles); } public override void ExtractContactData(in ConstraintLocation constraintLocation, Solver solver, ref TExtractor extractor) diff --git a/BepuPhysics/CollisionDetection/FreshnessChecker.cs b/BepuPhysics/CollisionDetection/FreshnessChecker.cs index 28e46c0c4..bbe6cf9e6 100644 --- a/BepuPhysics/CollisionDetection/FreshnessChecker.cs +++ b/BepuPhysics/CollisionDetection/FreshnessChecker.cs @@ -1,4 +1,5 @@ -using BepuUtilities.Collections; +using BepuUtilities; +using BepuUtilities.Collections; using BepuUtilities.Memory; using System; using System.Diagnostics; @@ -11,6 +12,7 @@ internal class FreshnessChecker int freshnessJobCount; PairCache pairCache; ConstraintRemover constraintRemover; + internal IThreadDispatcher cachedDispatcher; public FreshnessChecker(NarrowPhase narrowPhase) { @@ -18,8 +20,9 @@ public FreshnessChecker(NarrowPhase narrowPhase) constraintRemover = narrowPhase.ConstraintRemover; } - public void CreateJobs(int threadCount, ref QuickList jobs, BufferPool pool, int mappingCount) + public void CreateJobs(IThreadDispatcher dispatcher, int threadCount, ref QuickList jobs, BufferPool pool, int mappingCount) { + cachedDispatcher = dispatcher; if (mappingCount > 0) { if (threadCount > 1) @@ -158,11 +161,10 @@ unsafe void EnqueueStaleRemoval(int workerIndex, int pairIndex) { //Note that we have to grab the *old* handle, because the current frame's set of constraint caches do not contain this pair. //If they DID contain this pair, then it wouldn't be stale! - Debug.Assert(pairCache.Mapping.Values[pairIndex].ConstraintCache.Exists, "This implementation currently assumes that all pairs have constraint caches."); var constraintHandle = pairCache.GetOldConstraintHandle(pairIndex); constraintRemover.EnqueueRemoval(workerIndex, constraintHandle); - ref var cache = ref pairCache.NextWorkerCaches[workerIndex]; - cache.PendingRemoves.Add(pairCache.Mapping.Keys[pairIndex], cache.pool); + ref var pendingChanges = ref pairCache.WorkerPendingChanges[workerIndex]; + pendingChanges.PendingRemoves.Add(pairCache.Mapping.Keys[pairIndex], cachedDispatcher == null ? pairCache.pool : cachedDispatcher.GetThreadMemoryPool(workerIndex)); } } } diff --git a/BepuPhysics/CollisionDetection/InactiveSetBuilder.cs b/BepuPhysics/CollisionDetection/InactiveSetBuilder.cs index 319311382..dc1bd818d 100644 --- a/BepuPhysics/CollisionDetection/InactiveSetBuilder.cs +++ b/BepuPhysics/CollisionDetection/InactiveSetBuilder.cs @@ -9,151 +9,39 @@ namespace BepuPhysics.CollisionDetection internal struct SleepingPair { public CollidablePair Pair; - public TypedIndex ConstraintCache; - public TypedIndex CollisionCache; + public ConstraintCache Cache; } - internal struct SleepingCache - { - public int TypeId; - public UntypedList List; - } internal struct SleepingSet { - public bool Allocated { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { return Pairs.Span.Allocated; } } + public bool Allocated => Pairs.Span.Allocated; - public Buffer ConstraintCaches; - public Buffer CollisionCaches; public QuickList Pairs; public void Dispose(BufferPool pool) { - //Note that we use allocation status as an early terminator here. Didn't want to store the extra bytes for counts for no reason. - //This does require a clear over the unfilled slots in the inactive set builder, though. - for (int i = 0; i < ConstraintCaches.Length; ++i) - { - ref var cache = ref ConstraintCaches[i]; - if (cache.List.Buffer.Allocated) - pool.Return(ref cache.List.Buffer); - else - break; - } - pool.Return(ref ConstraintCaches); - //Remember, collision caches are not guaranteed to exist. If none are found during set construction, nothing is allocated for them. - //This just saves a little bit of extra space for the inactive set. - if (CollisionCaches.Allocated) - { - for (int i = 0; i < CollisionCaches.Length; ++i) - { - ref var cache = ref CollisionCaches[i]; - if (cache.List.Buffer.Allocated) - pool.Return(ref cache.List.Buffer); - else - break; - } - pool.Return(ref CollisionCaches); - } Pairs.Dispose(pool); } } internal struct SleepingSetBuilder { - public Buffer ConstraintCaches; - public Buffer CollisionCaches; public QuickList Pairs; - public int InitialCapacityPerCache; - public SleepingSetBuilder(BufferPool pool, int initialPairCapacity, int initialCapacityPerCache) + public SleepingSetBuilder(BufferPool pool, int initialPairCapacity) { - pool.TakeAtLeast(PairCache.CollisionConstraintTypeCount, out ConstraintCaches); - pool.TakeAtLeast(PairCache.CollisionTypeCount, out CollisionCaches); - //Original values are used to test for existence; have to clear to avoid undefined values. - ConstraintCaches.Clear(0, ConstraintCaches.Length); - CollisionCaches.Clear(0, CollisionCaches.Length); Pairs = new QuickList(initialPairCapacity, pool); - InitialCapacityPerCache = initialCapacityPerCache; } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - unsafe TypedIndex CopyToBuilderCache(ref Buffer sourceCaches, ref Buffer targetCaches, int typeId, int sourceByteIndex, BufferPool pool) - { - ref var sourceCache = ref sourceCaches[typeId]; - ref var targetCache = ref targetCaches[typeId]; - var targetByteIndex = targetCache.Allocate(sourceCache.ElementSizeInBytes, InitialCapacityPerCache, pool); - Unsafe.CopyBlockUnaligned(targetCache.Buffer.Memory + targetByteIndex, sourceCache.Buffer.Memory + sourceByteIndex, (uint)sourceCache.ElementSizeInBytes); - return new TypedIndex(typeId, targetByteIndex); - } - - public int Add(ref ArrayList pairCaches, BufferPool pool, ref CollidablePair pair, ref CollidablePairPointers sourcePointers) + public int Add(BufferPool pool, CollidablePair pair, in ConstraintCache cache) { var pairIndex = Pairs.Count; - Pairs.EnsureCapacity(Pairs.Count + 1, pool); - ref var entry = ref Pairs.AllocateUnsafely(); + ref var entry = ref Pairs.Allocate(pool); entry.Pair = pair; - Debug.Assert(sourcePointers.ConstraintCache.Exists); - var workerIndex = sourcePointers.ConstraintCache.Cache; - ref var workerCache = ref pairCaches[workerIndex]; - Debug.Assert(!sourcePointers.CollisionDetectionCache.Exists || sourcePointers.CollisionDetectionCache.Cache == workerIndex); - entry.ConstraintCache = CopyToBuilderCache(ref workerCache.constraintCaches, ref ConstraintCaches, - sourcePointers.ConstraintCache.Type, sourcePointers.ConstraintCache.Index, pool); - if (sourcePointers.CollisionDetectionCache.Exists) - { - entry.CollisionCache = CopyToBuilderCache(ref workerCache.collisionCaches, ref CollisionCaches, - sourcePointers.CollisionDetectionCache.Type, sourcePointers.CollisionDetectionCache.Index, pool); - } - else - { - entry.CollisionCache = new TypedIndex(); - } + entry.Cache = cache; return pairIndex; } - - unsafe void CopyExistingLists(ref Buffer sourceCaches, BufferPool pool, out Buffer inactiveCaches, out Buffer typeRemap) - { - int sourceTypeCount = 0; - for (int i = 0; i < sourceCaches.Length; ++i) - { - if (sourceCaches[i].Count > 0) - { - ++sourceTypeCount; - } - } - //Note that collision caches are not guaranteed to exist, so there may be no need to allocate room to store them. - if (sourceTypeCount > 0) - { - pool.TakeAtLeast(sourceTypeCount, out inactiveCaches); - int index = 0; - pool.TakeAtLeast(sourceCaches.Length, out typeRemap); - for (int i = 0; i < sourceCaches.Length; ++i) - { - ref var sourceList = ref sourceCaches[i]; - if (sourceList.Count > 0) - { - ref var inactiveCache = ref inactiveCaches[index]; - inactiveCache.TypeId = i; - inactiveCache.List = new UntypedList(sourceList.ElementSizeInBytes, sourceList.Count, pool); - inactiveCache.List.ByteCount = sourceList.ByteCount; - inactiveCache.List.Count = sourceList.Count; - Unsafe.CopyBlockUnaligned(inactiveCache.List.Buffer.Memory, sourceList.Buffer.Memory, (uint)sourceList.ByteCount); - typeRemap[i] = index; //Note that unfilled mapping slots won't be accessed; this is only used for pointing pairs to the proper packed locations. - ++index; - - //Clear for the next usage. - sourceList.ByteCount = 0; - sourceList.Count = 0; - } - } - //The inactive set's disposal uses allocation status as a loop terminator. Go ahead and clear any empty slots to avoid corrupt allocation state. - inactiveCaches.Clear(index, inactiveCaches.Length - index); - } - else - { - typeRemap = new Buffer(); - inactiveCaches = new Buffer(); - } - } public void FinalizeSet(BufferPool pool, out SleepingSet set) { //Repackage the gathered caches into a smaller format for longer term storage. @@ -164,24 +52,8 @@ public void FinalizeSet(BufferPool pool, out SleepingSet set) if (Pairs.Count > 0) { - CopyExistingLists(ref ConstraintCaches, pool, out set.ConstraintCaches, out var constraintTypeRemap); - CopyExistingLists(ref CollisionCaches, pool, out set.CollisionCaches, out var collisionTypeRemap); - Debug.Assert(set.ConstraintCaches.Length > 0, - "While there may be no collision caches, pair mapping entries only exist for constraintful pairs."); - set.Pairs = new QuickList(Pairs.Count, pool); - for (int i = 0; i < Pairs.Count; ++i) - { - ref var sourcePair = ref Pairs[i]; - ref var remappedPair = ref set.Pairs.AllocateUnsafely(); - remappedPair.Pair = sourcePair.Pair; - remappedPair.ConstraintCache = new TypedIndex(constraintTypeRemap[sourcePair.ConstraintCache.Type], sourcePair.ConstraintCache.Index); - remappedPair.CollisionCache = sourcePair.CollisionCache.Exists ? - new TypedIndex(collisionTypeRemap[sourcePair.CollisionCache.Type], sourcePair.CollisionCache.Index) : new TypedIndex(); - } - pool.Return(ref constraintTypeRemap); - if (collisionTypeRemap.Allocated) - pool.Return(ref collisionTypeRemap); + set.Pairs.AddRangeUnsafely(Pairs.Span, 0, Pairs.Count); Pairs.Count = 0; } else @@ -193,18 +65,6 @@ public void FinalizeSet(BufferPool pool, out SleepingSet set) public void Dispose(BufferPool pool) { - for (int i = 0; i < ConstraintCaches.Length; ++i) - { - if (ConstraintCaches[i].Buffer.Allocated) - pool.Return(ref ConstraintCaches[i].Buffer); - } - pool.Return(ref ConstraintCaches); - for (int i = 0; i < CollisionCaches.Length; ++i) - { - if (CollisionCaches[i].Buffer.Allocated) - pool.Return(ref CollisionCaches[i].Buffer); - } - pool.Return(ref CollisionCaches); Pairs.Dispose(pool); } } diff --git a/BepuPhysics/CollisionDetection/NarrowPhaseCCDContinuations.cs b/BepuPhysics/CollisionDetection/NarrowPhaseCCDContinuations.cs index 9366d360d..06bc39b65 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhaseCCDContinuations.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhaseCCDContinuations.cs @@ -129,7 +129,6 @@ public CCDContinuationIndex AddContinuous(ref CollidablePair pair, Vector3 relat [MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe void OnPairCompleted(int pairId, ref TManifold manifoldReference) where TManifold : unmanaged, IContactManifold { - var todoTestCollisionCache = default(EmptyCollisionCache); CCDContinuationIndex continuationId = new CCDContinuationIndex(pairId); Debug.Assert(continuationId.Exists); var continuationIndex = continuationId.Index; @@ -150,7 +149,7 @@ public unsafe void OnPairCompleted(int pairId, ref TManifold manifold { //Direct has no need for accumulating multiple reports; we can immediately dispatch. ref var continuation = ref discrete.Caches[continuationIndex]; - narrowPhase.UpdateConstraintsForPair(workerIndex, ref continuation.Pair, ref manifoldReference, ref todoTestCollisionCache); + narrowPhase.UpdateConstraintsForPair(workerIndex, continuation.Pair, ref manifoldReference); discrete.Return(continuationIndex, pool); } break; @@ -184,7 +183,7 @@ public unsafe void OnPairCompleted(int pairId, ref TManifold manifold contact.Depth -= velocityAtContact * continuation.T; } } - narrowPhase.UpdateConstraintsForPair(workerIndex, ref continuation.Pair, ref manifoldReference, ref todoTestCollisionCache); + narrowPhase.UpdateConstraintsForPair(workerIndex, continuation.Pair, ref manifoldReference); continuous.Return(continuationIndex, pool); } break; diff --git a/BepuPhysics/CollisionDetection/NarrowPhaseConstraintUpdate.cs b/BepuPhysics/CollisionDetection/NarrowPhaseConstraintUpdate.cs index dd0b8ea48..7d8a29a0e 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhaseConstraintUpdate.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhaseConstraintUpdate.cs @@ -20,14 +20,6 @@ public struct TwoBodyHandles public int B; } - /// - /// Special type for collision pairs that do not need to store any supplementary information. - /// - struct EmptyCollisionCache : IPairCacheEntry - { - public int CacheTypeId => -1; - } - public struct ContactImpulses1 { public float Impulse0; @@ -146,7 +138,7 @@ private unsafe void RedistributeImpulses( [MethodImpl(MethodImplOptions.AggressiveInlining)] unsafe void RequestAddConstraint(int workerIndex, int manifoldConstraintType, - ref CollidablePair pair, PairCacheIndex constraintCacheIndex, ref TContactImpulses newImpulses, + CollidablePair pair, PairCacheChangeIndex pairCacheChange, ref TContactImpulses newImpulses, ref TDescription description, TBodyHandles bodyHandles) where TBodyHandles : unmanaged where TDescription : unmanaged, IConstraintDescription { //Note that this branch is (was?) JIT constant. @@ -154,29 +146,22 @@ unsafe void RequestAddConstraint(i { throw new InvalidOperationException("Invalid body handles type; the narrow phase should only use TwoBodyHandles or int."); } - AddConstraint(workerIndex, manifoldConstraintType, ref pair, constraintCacheIndex, ref newImpulses, bodyHandles, ref description); + AddConstraint(workerIndex, manifoldConstraintType, pair, pairCacheChange, ref newImpulses, bodyHandles, ref description); } - public unsafe void UpdateConstraint(int workerIndex, ref CollidablePair pair, - int manifoldTypeAsConstraintType, ref TConstraintCache newConstraintCache, ref TCollisionCache collisionCache, - ref TDescription description, TBodyHandles bodyHandles) + public unsafe void UpdateConstraint(int workerIndex, CollidablePair pair, + int manifoldTypeAsConstraintType, ref ConstraintCache newConstraintCache, int newContactCount, ref TDescription description, TBodyHandles bodyHandles) where TBodyHandles : unmanaged - where TConstraintCache : unmanaged, IPairCacheEntry - where TCollisionCache : unmanaged, IPairCacheEntry where TDescription : unmanaged, IConstraintDescription where TContactImpulses : unmanaged { - var index = PairCache.IndexOf(ref pair); + var index = PairCache.IndexOf(pair); if (index >= 0) { //The previous frame had a constraint for this pair. - ref var pointers = ref PairCache.GetPointers(index); - Debug.Assert(pointers.ConstraintCache.Exists, "If a pair was persisted in the narrow phase, there should be a constraint associated with it."); - - var constraintCacheIndex = pointers.ConstraintCache; - var oldConstraintCachePointer = PairCache.GetOldConstraintCachePointer(index); - var constraintHandle = *(ConstraintHandle*)oldConstraintCachePointer; - var constraintReference = Solver.GetConstraintReference(constraintHandle); + ref var cache = ref PairCache.GetCache(index); + var oldConstraintHandle = cache.ConstraintHandle; + var constraintReference = Solver.GetConstraintReference(oldConstraintHandle); Debug.Assert( constraintReference.typeBatchPointer != null && constraintReference.IndexInTypeBatch >= 0 && @@ -194,17 +179,15 @@ public unsafe void UpdateConstraint(ref newConstraintCache), 1), ref newImpulses); + accessor.ContactCount, (int*)Unsafe.AsPointer(ref cache) + 1, oldImpulses, newContactCount, ref newConstraintCache.FeatureId0, ref newImpulses); if (manifoldTypeAsConstraintType == constraintReference.TypeBatch.TypeId) { //Since the old constraint is the same type, we aren't going to remove the old constraint and add a new one. That means no deferred process is going //to update the constraint cache's constraint handle. The good news is that we already have a valid constraint handle from the pre-existing constraint. - //It's exactly the same type, so we can just overwrite its properties without worry. - //Note that we rely on the constraint handle being stored in the first 4 bytes of the constraint cache. - Unsafe.As(ref newConstraintCache) = constraintHandle; - PairCache.Update(workerIndex, index, ref pointers, ref collisionCache, ref newConstraintCache); + //It's exactly the same type, so we can just write the handle. + newConstraintCache.ConstraintHandle = oldConstraintHandle; + PairCache.Update(index, newConstraintCache); //There exists a constraint and it has the same type as the manifold. Directly apply the new description and impulses. Solver.ApplyDescriptionWithoutWaking(constraintReference, description); accessor.ScatterNewImpulses(ref constraintReference, ref newImpulses); @@ -214,19 +197,19 @@ public unsafe void UpdateConstraint(int contactCount) } //TODO: If you end up changing the NarrowPhasePendingConstraintAdds and PairCache hardcoded type handling, you should change this too. This is getting silly. - unsafe void UpdateConstraintForManifold( - int workerIndex, ref CollidablePair pair, ref TContactManifold manifold, ref TCollisionCache collisionCache, ref PairMaterialProperties material, TBodyHandles bodyHandles) - where TCollisionCache : unmanaged, IPairCacheEntry + unsafe void UpdateConstraintForManifold( + int workerIndex, ref CollidablePair pair, ref TContactManifold manifold, ref PairMaterialProperties material, TBodyHandles bodyHandles) { //Note that this function has two responsibilities: //1) Create the description of the constraint that should represent the new manifold. @@ -308,12 +290,10 @@ unsafe void UpdateConstraintForManifold(int workerIndex, ref CollidablePair pair, ref TContactManifold manifold, ref TCollisionCache collisionCache) - where TCollisionCache : unmanaged, IPairCacheEntry - where TContactManifold : unmanaged, IContactManifold + public unsafe void UpdateConstraintsForPair(int workerIndex, CollidablePair pair, ref TContactManifold manifold) where TContactManifold : unmanaged, IContactManifold { //Note that we do not check for the pair being between two statics before reporting it. The assumption is that, if the initial broadphase pair filter allowed such a pair //to reach this point, the user probably wants to receive some information about the resulting contact manifold. @@ -333,13 +313,13 @@ public unsafe void UpdateConstraintsForPair(i //Two bodies. Debug.Assert(pair.A.Mobility != CollidableMobility.Static && pair.B.Mobility != CollidableMobility.Static); var bodyHandles = new TwoBodyHandles { A = pair.A.BodyHandle.Value, B = pair.B.BodyHandle.Value }; - UpdateConstraintForManifold(workerIndex, ref pair, ref manifold, ref collisionCache, ref pairMaterial, bodyHandles); + UpdateConstraintForManifold(workerIndex, ref pair, ref manifold, ref pairMaterial, bodyHandles); } else { //One of the two collidables is static. Debug.Assert(pair.A.Mobility != CollidableMobility.Static && pair.B.Mobility == CollidableMobility.Static); - UpdateConstraintForManifold(workerIndex, ref pair, ref manifold, ref collisionCache, ref pairMaterial, pair.A.BodyHandle.Value); + UpdateConstraintForManifold(workerIndex, ref pair, ref manifold, ref pairMaterial, pair.A.BodyHandle.Value); } //In the event that there are no contacts in the new manifold, the pair is left in a stale state. It will be removed by the stale removal post process. } diff --git a/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs b/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs index 5b0f9c4b4..59fbd0f87 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs @@ -22,11 +22,12 @@ public partial class NarrowPhase public struct PendingConstraintAddCache { BufferPool pool; - struct PendingConstraint where TBodyHandles : unmanaged where TDescription : unmanaged, IConstraintDescription + [StructLayout(LayoutKind.Sequential)] + unsafe struct PendingConstraint where TBodyHandles : unmanaged where TDescription : unmanaged, IConstraintDescription { //Note the memory ordering. Collidable pair comes first; deterministic flushes rely the memory layout to sort pending constraints. public CollidablePair Pair; - public PairCacheIndex ConstraintCacheIndex; + public PairCacheChangeIndex PairCacheChange; public TBodyHandles BodyHandles; public TDescription ConstraintDescription; public TContactImpulses Impulses; @@ -47,14 +48,14 @@ public PendingConstraintAddCache(BufferPool pool, int minimumConstraintCountPerC } public unsafe void AddConstraint(int manifoldConstraintType, - ref CollidablePair pair, PairCacheIndex constraintCacheIndex, TBodyHandles bodyHandles, ref TDescription constraintDescription, ref TContactImpulses impulses) + CollidablePair pair, PairCacheChangeIndex pairCacheChange, TBodyHandles bodyHandles, ref TDescription constraintDescription, ref TContactImpulses impulses) where TBodyHandles : unmanaged where TDescription : unmanaged, IConstraintDescription { ref var cache = ref pendingConstraintsByType[manifoldConstraintType]; var byteIndex = cache.Allocate>(minimumConstraintCountPerCache, pool); ref var pendingAdd = ref Unsafe.AsRef>(cache.Buffer.Memory + byteIndex); pendingAdd.Pair = pair; - pendingAdd.ConstraintCacheIndex = constraintCacheIndex; + pendingAdd.PairCacheChange = pairCacheChange; pendingAdd.BodyHandles = bodyHandles; pendingAdd.ConstraintDescription = constraintDescription; pendingAdd.Impulses = impulses; @@ -74,7 +75,7 @@ public static unsafe void SequentialAddToSimulation(Unsafe.AsPointer(ref add.BodyHandles), typeof(TBodyHandles) == typeof(TwoBodyHandles) ? 2 : 1), add.ConstraintDescription); - pairCache.CompleteConstraintAdd(simulation.NarrowPhase, simulation.Solver, ref add.Impulses, add.ConstraintCacheIndex, handle, ref add.Pair); + pairCache.CompleteConstraintAdd(simulation.NarrowPhase, simulation.Solver, ref add.Impulses, add.PairCacheChange, handle, ref add.Pair); } } } @@ -140,7 +141,7 @@ static unsafe void AddToSimulationSpeculative(int workerIndex, int manifoldConstraintType, ref CollidablePair pair, - PairCacheIndex constraintCacheIndex, ref TContactImpulses impulses, TBodyHandles bodyHandles, ref TDescription constraintDescription) + unsafe void AddConstraint(int workerIndex, int manifoldConstraintType, CollidablePair pair, + PairCacheChangeIndex pairCacheChange, ref TContactImpulses impulses, TBodyHandles bodyHandles, ref TDescription constraintDescription) where TBodyHandles : unmanaged where TDescription : unmanaged, IConstraintDescription { - overlapWorkers[workerIndex].PendingConstraints.AddConstraint(manifoldConstraintType, ref pair, constraintCacheIndex, bodyHandles, ref constraintDescription, ref impulses); + overlapWorkers[workerIndex].PendingConstraints.AddConstraint(manifoldConstraintType, pair, pairCacheChange, bodyHandles, ref constraintDescription, ref impulses); } diff --git a/BepuPhysics/CollisionDetection/NarrowPhasePreflush.cs b/BepuPhysics/CollisionDetection/NarrowPhasePreflush.cs index b9036b3e3..4e59ff186 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhasePreflush.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhasePreflush.cs @@ -358,10 +358,11 @@ protected override void OnPreflush(IThreadDispatcher threadDispatcher, bool dete //var job = new PreflushJob { Type = PreflushJobType.NondeterministicConstraintAdd, WorkerCount = threadCount }; //ExecutePreflushJob(0, ref job); } - FreshnessChecker.CreateJobs(threadCount, ref preflushJobs, Pool, originalPairCacheMappingCount); + FreshnessChecker.CreateJobs(threadDispatcher, threadCount, ref preflushJobs, Pool, originalPairCacheMappingCount); //start = Stopwatch.GetTimestamp(); preflushJobIndex = -1; threadDispatcher.DispatchWorkers(preflushWorkerLoop, preflushJobs.Count); + FreshnessChecker.cachedDispatcher = null; //preflushWorkerLoop(0); //end = Stopwatch.GetTimestamp(); //Console.WriteLine($"Preflush phase 3 time (us): {1e6 * (end - start) / Stopwatch.Frequency}"); diff --git a/BepuPhysics/CollisionDetection/PairCache.cs b/BepuPhysics/CollisionDetection/PairCache.cs index af74b0555..e980ef015 100644 --- a/BepuPhysics/CollisionDetection/PairCache.cs +++ b/BepuPhysics/CollisionDetection/PairCache.cs @@ -2,19 +2,14 @@ using BepuUtilities.Collections; using BepuUtilities.Memory; using BepuPhysics.Collidables; -using BepuPhysics.Constraints; -using BepuPhysics.Constraints.Contact; using System; -using System.Collections.Generic; using System.Diagnostics; using System.Runtime.CompilerServices; using System.Runtime.InteropServices; -using System.Text; -using static BepuPhysics.CollisionDetection.WorkerPairCache; namespace BepuPhysics.CollisionDetection { - using OverlapMapping = QuickDictionary; + using OverlapMapping = QuickDictionary; [StructLayout(LayoutKind.Explicit, Size = 8)] public struct CollidablePair @@ -56,38 +51,52 @@ public int Hash(ref CollidablePair item) } } - public struct CollidablePairPointers + /// + /// Refers to a change in a . + /// + public struct PairCacheChangeIndex { /// - /// A narrowphase-specific type and index into the pair cache's constraint data set. Collision pairs which have no associated constraint, either - /// because no contacts were generated or because the constraint was filtered, will have a nonexistent ConstraintCache. + /// Index of the storing the pending change, if any. If -1, then this pair cache change refers to a change directly to the mapping. /// - public PairCacheIndex ConstraintCache; + public int WorkerIndex; /// - /// A narrowphase-specific type and index into a batch of custom data for the pair. Many types do not use any supplementary data, but some make use of temporal coherence - /// to accelerate contact generation. + /// Index of the change in the cache. For pending changes, refers to the index within the pending cache; for a direct mapping changes, refers to the pair index. /// - public PairCacheIndex CollisionDetectionCache; + public int Index; + + /// + /// Gets whether this change is in the + /// + public bool IsPending => WorkerIndex >= 0; } - internal struct ArrayList + /// + /// Stores information about a contact constraint from the previous timestep. + /// + [StructLayout(LayoutKind.Sequential)] + public struct ConstraintCache { - public T[] Values; - public int Count; - public ref T this[int index] { get { return ref Values[index]; } } - public bool Allocated { get { return Values != null; } } - public ArrayList(int initialCapacity) - { - Values = new T[initialCapacity]; - Count = 0; - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - internal ref T AllocateUnsafely() - { - Debug.Assert(Count < Values.Length); - return ref Values[Count++]; - } + /// + /// Handle of the contact constraint associated with this cache. + /// + public ConstraintHandle ConstraintHandle; + /// + /// Feature id of the first contact in the constraint associated with this cache. + /// + public int FeatureId0; + /// + /// Feature id of the second contact in the constraint associated with this cache. + /// + public int FeatureId1; + /// + /// Feature id of the third contact in the constraint associated with this cache. + /// + public int FeatureId2; + /// + /// Feature id of the fourth contact in the constraint associated with this cache. + /// + public int FeatureId3; } public partial class PairCache @@ -103,18 +112,15 @@ public partial class PairCache /// Choice of data type is a balancing act between the memory bandwidth of the post analysis and the frequency of false sharing. /// internal Buffer PairFreshness; - BufferPool pool; + internal BufferPool pool; int minimumPendingSize; int minimumPerTypeCapacity; int previousPendingSize; - //While the current worker caches are read from, the next caches are written to. - //The worker pair caches contain a reference to a buffer pool, which is a reference type. That makes WorkerPairCache non-blittable, so in the interest of not being - //super duper gross, we don't use the untyped buffer pools to store it. - //Given that the size of the arrays here will be small and almost never change, this isn't a significant issue. - ArrayList workerCaches; - internal ArrayList NextWorkerCaches; + //While the current worker caches are read from, changes to the cache are accumulated. + internal Buffer WorkerPendingChanges; + internal IThreadDispatcher cachedDispatcher; public PairCache(BufferPool pool, int initialSetCapacity, int minimumMappingSize, int minimumPendingSize, int minimumPerTypeCapacity) @@ -128,56 +134,22 @@ public PairCache(BufferPool pool, int initialSetCapacity, int minimumMappingSize public void Prepare(IThreadDispatcher threadDispatcher = null) { - int maximumConstraintTypeCount = 0, maximumCollisionTypeCount = 0; - for (int i = 0; i < workerCaches.Count; ++i) - { - workerCaches[i].GetMaximumCacheTypeCounts(out var collision, out var constraint); - if (collision > maximumCollisionTypeCount) - maximumCollisionTypeCount = collision; - if (constraint > maximumConstraintTypeCount) - maximumConstraintTypeCount = constraint; - } - var minimumSizesPerConstraintType = new QuickList(maximumConstraintTypeCount, pool); - var minimumSizesPerCollisionType = new QuickList(maximumCollisionTypeCount, pool); - //Since the minimum size accumulation builds the minimum size incrementally, bad data within the array can corrupt the result- we must clear it. - minimumSizesPerConstraintType.Span.Clear(0, minimumSizesPerConstraintType.Span.Length); - minimumSizesPerCollisionType.Span.Clear(0, minimumSizesPerCollisionType.Span.Length); - for (int i = 0; i < workerCaches.Count; ++i) - { - workerCaches[i].AccumulateMinimumSizes(ref minimumSizesPerConstraintType, ref minimumSizesPerCollisionType); - } - var threadCount = threadDispatcher != null ? threadDispatcher.ThreadCount : 1; - //Ensure that the new worker pair caches can hold all workers. - if (!NextWorkerCaches.Allocated || NextWorkerCaches.Values.Length < threadCount) - { - //The next worker caches should never need to be disposed here. The flush should have taken care of it. -#if DEBUG - for (int i = 0; i < NextWorkerCaches.Count; ++i) - Debug.Assert(NextWorkerCaches[i].Equals(default(WorkerPairCache))); -#endif - Array.Resize(ref NextWorkerCaches.Values, threadCount); - NextWorkerCaches.Count = threadCount; - } - //Note that we have not initialized the workerCaches from the previous frame. In the event that this is the first frame and there are no previous worker caches, - //there will be no pointers into the caches, and removal analysis loops over the count which defaults to zero- so it's safe. - NextWorkerCaches.Count = threadCount; + cachedDispatcher = threadDispatcher; var pendingSize = Math.Max(minimumPendingSize, previousPendingSize); + pool.Take(threadCount, out WorkerPendingChanges); if (threadDispatcher != null) { for (int i = 0; i < threadCount; ++i) { - NextWorkerCaches[i] = new WorkerPairCache(i, threadDispatcher.GetThreadMemoryPool(i), ref minimumSizesPerConstraintType, ref minimumSizesPerCollisionType, - pendingSize, minimumPerTypeCapacity); + WorkerPendingChanges[i] = new WorkerPendingPairChanges(threadDispatcher.GetThreadMemoryPool(i), pendingSize); } } else { - NextWorkerCaches[0] = new WorkerPairCache(0, pool, ref minimumSizesPerConstraintType, ref minimumSizesPerCollisionType, pendingSize, minimumPerTypeCapacity); + WorkerPendingChanges[0] = new WorkerPendingPairChanges(pool, pendingSize); } - minimumSizesPerConstraintType.Dispose(pool); - minimumSizesPerCollisionType.Dispose(pool); //Create the pair freshness array for the existing overlaps. pool.TakeAtLeast(Mapping.Count, out PairFreshness); @@ -213,21 +185,15 @@ internal void ResizeConstraintToPairMappingCapacity(Solver solver, int targetCap /// public void PrepareFlushJobs(ref QuickList jobs) { - //Get rid of the now-unused worker caches. - for (int i = 0; i < workerCaches.Count; ++i) - { - workerCaches[i].Dispose(); - } - //The freshness cache should have already been used in order to generate the constraint removal requests and the PendingRemoves that we handle in a moment; dispose it now. pool.Return(ref PairFreshness); //Ensure the overlap mapping size is sufficient up front. This requires scanning all the pending sizes. int largestIntermediateSize = Mapping.Count; var newMappingSize = Mapping.Count; - for (int i = 0; i < NextWorkerCaches.Count; ++i) + for (int i = 0; i < WorkerPendingChanges.Length; ++i) { - ref var cache = ref NextWorkerCaches[i]; + ref var cache = ref WorkerPendingChanges[i]; //Removes occur first, so this cache can only result in a larger mapping if there are more adds than removes. newMappingSize += cache.PendingAdds.Count - cache.PendingRemoves.Count; if (newMappingSize > largestIntermediateSize) @@ -244,9 +210,9 @@ public unsafe void FlushMappingChanges() //Note that this phase accesses no shared memory- it's all pair cache local, and no pool accesses are made. //That means we could run it as a job alongside solver constraint removal. That's good, because adding and removing to the hash tables isn't terribly fast. //(On the order of 10-100 nanoseconds per operation, so in pathological cases, it can start showing up in profiles.) - for (int i = 0; i < NextWorkerCaches.Count; ++i) + for (int i = 0; i < WorkerPendingChanges.Length; ++i) { - ref var cache = ref NextWorkerCaches[i]; + ref var cache = ref WorkerPendingChanges[i]; //Walk backwards on the off chance that a swap can be avoided. for (int j = cache.PendingRemoves.Count - 1; j >= 0; --j) @@ -257,7 +223,7 @@ public unsafe void FlushMappingChanges() for (int j = 0; j < cache.PendingAdds.Count; ++j) { ref var pending = ref cache.PendingAdds[j]; - var added = Mapping.AddUnsafely(ref pending.Pair, pending.Pointers); + var added = Mapping.AddUnsafely(pending.Pair, pending.Cache); Debug.Assert(added); } } @@ -267,37 +233,39 @@ public void Postflush() //This bookkeeping and disposal phase is trivially cheap compared to the cost of updating the mapping table, so we do it sequentially. //The fact that we access the per-worker pools here would prevent easy multithreading anyway; the other threads may use them. int largestPendingSize = 0; - for (int i = 0; i < NextWorkerCaches.Count; ++i) + for (int i = 0; i < WorkerPendingChanges.Length; ++i) { - ref var cache = ref NextWorkerCaches[i]; - if (cache.PendingAdds.Count > largestPendingSize) + ref var pendingChanges = ref WorkerPendingChanges[i]; + if (pendingChanges.PendingAdds.Count > largestPendingSize) + { + largestPendingSize = pendingChanges.PendingAdds.Count; + } + if (pendingChanges.PendingRemoves.Count > largestPendingSize) { - largestPendingSize = cache.PendingAdds.Count; + largestPendingSize = pendingChanges.PendingRemoves.Count; } - if (cache.PendingRemoves.Count > largestPendingSize) + } + if (WorkerPendingChanges.Length > 1) + { + for (int i = 0; i < WorkerPendingChanges.Length; ++i) { - largestPendingSize = cache.PendingRemoves.Count; + WorkerPendingChanges[i].Dispose(cachedDispatcher.GetThreadMemoryPool(i)); } - cache.PendingAdds.Dispose(cache.pool); - cache.PendingRemoves.Dispose(cache.pool); + } + else + { + WorkerPendingChanges[0].Dispose(pool); } previousPendingSize = largestPendingSize; - //Swap references. - var temp = workerCaches; - workerCaches = NextWorkerCaches; - NextWorkerCaches = temp; + pool.Return(ref WorkerPendingChanges); + cachedDispatcher = null; } internal void Clear() { - for (int i = 0; i < workerCaches.Count; ++i) - { - workerCaches[i].Dispose(); - } - workerCaches.Count = 0; for (int i = 1; i < SleepingSets.Length; ++i) { if (SleepingSets[i].Allocated) @@ -305,33 +273,15 @@ internal void Clear() SleepingSets[i].Dispose(pool); } } -#if DEBUG - if (NextWorkerCaches.Allocated) - { - for (int i = 0; i < NextWorkerCaches.Count; ++i) - { - Debug.Assert(NextWorkerCaches[i].Equals(default(WorkerPairCache)), "Outside of the execution of the narrow phase, the 'next' caches should not be allocated."); - } - } -#endif + + Debug.Assert(!WorkerPendingChanges.Allocated); } public void Dispose() { - for (int i = 0; i < workerCaches.Count; ++i) - { - workerCaches[i].Dispose(); - } //Note that we do not need to dispose the worker cache arrays themselves- they were just arrays pulled out of a passthrough pool. -#if DEBUG - if (NextWorkerCaches.Allocated) - { - for (int i = 0; i < NextWorkerCaches.Count; ++i) - { - Debug.Assert(NextWorkerCaches[i].Equals(default(WorkerPairCache)), "Outside of the execution of the narrow phase, the 'next' caches should not be allocated."); - } - } -#endif + Debug.Assert(!WorkerPendingChanges.Allocated); + Mapping.Dispose(pool); for (int i = 1; i < SleepingSets.Length; ++i) { @@ -348,55 +298,42 @@ public void Dispose() [MethodImpl(MethodImplOptions.AggressiveInlining)] - public int IndexOf(ref CollidablePair pair) + public int IndexOf(CollidablePair pair) { - return Mapping.IndexOf(ref pair); + return Mapping.IndexOf(pair); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref CollidablePairPointers GetPointers(int index) + public ref ConstraintCache GetCache(int index) { return ref Mapping.Values[index]; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - internal unsafe PairCacheIndex Add(int workerIndex, ref CollidablePair pair, - ref TCollisionCache collisionCache, ref TConstraintCache constraintCache) - where TConstraintCache : IPairCacheEntry - where TCollisionCache : IPairCacheEntry + internal unsafe PairCacheChangeIndex Add(int workerIndex, CollidablePair pair, in ConstraintCache constraintCache) { - //Note that we do not have to set any freshness bytes here; using this path means there exists no previous overlap to remove anyway. - return NextWorkerCaches[workerIndex].Add(ref pair, ref collisionCache, ref constraintCache); + //Note that we do not have to set any freshness bytes here; using this path means there exists no previous overlap to remove anyway. + return new PairCacheChangeIndex { WorkerIndex = workerIndex, Index = WorkerPendingChanges[workerIndex].Add(cachedDispatcher == null ? pool : cachedDispatcher.GetThreadMemoryPool(workerIndex), pair, constraintCache) }; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - internal unsafe void Update(int workerIndex, int pairIndex, ref CollidablePairPointers pointers, - ref TCollisionCache collisionCache, ref TConstraintCache constraintCache) - where TConstraintCache : IPairCacheEntry - where TCollisionCache : IPairCacheEntry + internal unsafe PairCacheChangeIndex Update(int pairIndex, in ConstraintCache cache) { //We're updating an existing pair, so we should prevent this pair from being removed. PairFreshness[pairIndex] = 0xFF; - NextWorkerCaches[workerIndex].Update(ref pointers, ref collisionCache, ref constraintCache); + Mapping.Values[pairIndex] = cache; + return new PairCacheChangeIndex { WorkerIndex = -1, Index = pairIndex }; } - + //4 convex one body, 4 convex two body, 7 nonconvex one body, 7 convex two body. public const int CollisionConstraintTypeCount = 22; public const int CollisionTypeCount = 16; - [MethodImpl(MethodImplOptions.AggressiveInlining)] - internal unsafe void* GetOldConstraintCachePointer(int pairIndex) - { - ref var constraintCacheIndex = ref Mapping.Values[pairIndex].ConstraintCache; - return workerCaches[constraintCacheIndex.Cache].GetConstraintCachePointer(constraintCacheIndex); - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] internal unsafe ConstraintHandle GetOldConstraintHandle(int pairIndex) { - ref var constraintCacheIndex = ref Mapping.Values[pairIndex].ConstraintCache; - return *(ConstraintHandle*)workerCaches[constraintCacheIndex.Cache].GetConstraintCachePointer(constraintCacheIndex); + return Mapping.Values[pairIndex].ConstraintHandle; } /// @@ -406,16 +343,21 @@ internal unsafe ConstraintHandle GetOldConstraintHandle(int pairIndex) /// Narrow phase that triggered the constraint add. /// Solver containing the constraint to set the impulses of. /// Warm starting impulses to apply to the contact constraint. - /// Index of the constraint cache to update. + /// Index of the change associated with this constraint in the . /// Constraint handle associated with the constraint cache being updated. /// Collidable pair associated with the new constraint. [MethodImpl(MethodImplOptions.AggressiveInlining)] - internal unsafe void CompleteConstraintAdd(NarrowPhase narrowPhase, Solver solver, ref TContactImpulses impulses, PairCacheIndex constraintCacheIndex, + internal unsafe void CompleteConstraintAdd(NarrowPhase narrowPhase, Solver solver, ref TContactImpulses impulses, PairCacheChangeIndex pairCacheChangeIndex, ConstraintHandle constraintHandle, ref CollidablePair pair) { - //Note that the update is being directed to the *next* worker caches. We have not yet performed the flush that swaps references. - //Note that this assumes that the constraint handle is stored in the first 4 bytes of the constraint cache. - *(ConstraintHandle*)NextWorkerCaches[constraintCacheIndex.Cache].GetConstraintCachePointer(constraintCacheIndex) = constraintHandle; + if (pairCacheChangeIndex.IsPending) + { + WorkerPendingChanges[pairCacheChangeIndex.WorkerIndex].PendingAdds[pairCacheChangeIndex.Index].Cache.ConstraintHandle = constraintHandle; + } + else + { + Mapping.Values[pairCacheChangeIndex.Index].ConstraintHandle = constraintHandle; + } var reference = solver.GetConstraintReference(constraintHandle); Debug.Assert(reference.IndexInTypeBatch >= 0 && reference.IndexInTypeBatch < reference.TypeBatch.ConstraintCount); narrowPhase.contactConstraintAccessors[reference.TypeBatch.TypeId].ScatterNewImpulses(ref reference, ref impulses); @@ -424,18 +366,5 @@ internal unsafe void CompleteConstraintAdd(NarrowPhase narrowP ConstraintHandleToPair[constraintHandle.Value].Pair = pair; } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe ref TConstraintCache GetConstraintCache(PairCacheIndex constraintCacheIndex) - { - //Note that these refer to the previous workerCaches, not the nextWorkerCaches. We read from these caches during the narrowphase to redistribute impulses. - return ref Unsafe.AsRef(workerCaches[constraintCacheIndex.Cache].GetConstraintCachePointer(constraintCacheIndex)); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe ref TCollisionData GetCollisionData(PairCacheIndex index) where TCollisionData : struct, IPairCacheEntry - { - return ref Unsafe.AsRef(workerCaches[index.Cache].GetCollisionCachePointer(index)); - } - } } diff --git a/BepuPhysics/CollisionDetection/PairCacheIndex.cs b/BepuPhysics/CollisionDetection/PairCacheIndex.cs deleted file mode 100644 index 4ee900abb..000000000 --- a/BepuPhysics/CollisionDetection/PairCacheIndex.cs +++ /dev/null @@ -1,92 +0,0 @@ -using System.Diagnostics; -using System.Runtime.CompilerServices; - -namespace BepuPhysics.CollisionDetection -{ - /// - /// Packed indirection to data associated with a pair cache entry. - /// - public struct PairCacheIndex - { - internal ulong packed; - - /// - /// Gets whether this index actually refers to anything. The Type and Index should only be used if this is true. - /// - public bool Exists - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - get { return (packed & (1UL << 63)) > 0; } - } - - /// - /// Gets whether this index refers to an active cache entry. If false, the entry exists in an inactive set. - /// - public bool Active - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - get { return (packed & (1UL << 62)) > 0; } - } - - - /// - /// Gets the index of the cache that owns the entry. - /// - public int Cache - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - get { return (int)(packed >> 38) & 0x3FFFFF; } //24 bits - } - - /// - /// Gets the type index of the object. - /// - public int Type - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - get { return (int)(packed >> 30) & 0xFF; } //8 bits - } - - /// - /// Gets the index of the object within the type specific list. - /// - public int Index - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - get { return (int)(packed & 0x3FFF_FFFF); } //30 bits - } - - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public PairCacheIndex(int cache, int type, int index) - { - Debug.Assert(cache >= 0 && cache < (1 << 24), "Do you really have that many threads, or is the index corrupt?"); - Debug.Assert(type >= 0 && type < (1 << 8), "Do you really have that many type indices, or is the index corrupt?"); - //Note the inclusion of a set bit in the most significant 2 bits. - //The MSB encodes that the index was explicitly constructed, so it is a 'real' reference. - //A default constructed PairCacheIndex will have a 0 in the MSB, so we can use the default constructor for empty references. - //The second most significant bit sets the active flag. This constructor is used only by active references. - packed = (ulong)((3L << 62) | ((long)cache << 38) | ((long)type << 30) | (long)index); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static PairCacheIndex CreateInactiveReference(int cache, int type, int index) - { - Debug.Assert(cache >= 0 && cache < (1 << 24), "Do you really have that many sets, or is the index corrupt?"); - Debug.Assert(type >= 0 && type < (1 << 8), "Do you really have that many type indices, or is the index corrupt?"); - //Note the inclusion of a set bit in the most significant 2 bits. - //The MSB encodes that the index was explicitly constructed, so it is a 'real' reference. - //A default constructed PairCacheIndex will have a 0 in the MSB, so we can use the default constructor for empty references. - //The second most significant bit is left unset. This function creates only inactive references.. - PairCacheIndex toReturn; - toReturn.packed = (ulong)((1L << 63) | ((long)cache << 38) | ((long)type << 30) | (long)index); - return toReturn; - } - - public override string ToString() - { - return $"{{{Cache}, {Type}, {Index}}}"; - } - - } -} diff --git a/BepuPhysics/CollisionDetection/PairCache_Activity.cs b/BepuPhysics/CollisionDetection/PairCache_Activity.cs index 0d0d3c9c2..932382f50 100644 --- a/BepuPhysics/CollisionDetection/PairCache_Activity.cs +++ b/BepuPhysics/CollisionDetection/PairCache_Activity.cs @@ -40,33 +40,6 @@ internal void ResizeSetsCapacity(int setsCapacity, int potentiallyAllocatedCount } } - [Conditional("DEBUG")] - internal unsafe void ValidateConstraintHandleToPairMapping() - { - ValidateConstraintHandleToPairMapping(ref workerCaches, false); - } - [Conditional("DEBUG")] - internal unsafe void ValidateConstraintHandleToPairMappingInProgress(bool ignoreStale) - { - ValidateConstraintHandleToPairMapping(ref NextWorkerCaches, ignoreStale); - } - - [Conditional("DEBUG")] - internal unsafe void ValidateConstraintHandleToPairMapping(ref ArrayList caches, bool ignoreStale) - { - for (int i = 0; i < Mapping.Count; ++i) - { - if (!ignoreStale || PairFreshness[i] > 0) - { - var existingCache = Mapping.Values[i].ConstraintCache; - var existingHandle = *(int*)(caches[existingCache.Cache].constraintCaches[existingCache.Type].Buffer.Memory + existingCache.Index); - Debug.Assert(existingCache.Active, "The overlap mapping should only contain references to constraints which are active."); - ref var pairLocation = ref ConstraintHandleToPair[existingHandle]; - Debug.Assert(new CollidablePairComparer().Equals(ref ConstraintHandleToPair[existingHandle].Pair, ref Mapping.Keys[i]), - "The overlap mapping and handle mapping should match."); - } - } - } [Conditional("DEBUG")] internal unsafe void ValidateHandleCountInMapping(ConstraintHandle constraintHandle, int expectedCount) @@ -74,9 +47,8 @@ internal unsafe void ValidateHandleCountInMapping(ConstraintHandle constraintHan int count = 0; for (int i = 0; i < Mapping.Count; ++i) { - var existingCache = Mapping.Values[i].ConstraintCache; - var existingHandle = *(int*)(workerCaches[existingCache.Cache].constraintCaches[existingCache.Type].Buffer.Memory + existingCache.Index); - if (existingHandle == constraintHandle.Value) + var existingCache = Mapping.Values[i]; + if (existingCache.ConstraintHandle == constraintHandle) { ++count; Debug.Assert(count <= expectedCount && count <= 1, "Expected count violated."); @@ -102,11 +74,9 @@ internal unsafe void SleepTypeBatchPairs(ref SleepingSetBuilder builder, int set var handle = typeBatch.IndexToHandle[indexInTypeBatch]; ref var pairLocation = ref ConstraintHandleToPair[handle.Value]; Mapping.GetTableIndices(ref pairLocation.Pair, out var tableIndex, out var elementIndex); - ref var cacheLocations = ref Mapping.Values[elementIndex]; - Debug.Assert(cacheLocations.ConstraintCache.Exists); - + ref var cache = ref Mapping.Values[elementIndex]; pairLocation.InactiveSetIndex = setIndex; - pairLocation.InactivePairIndex = builder.Add(ref workerCaches, pool, ref Mapping.Keys[elementIndex], ref cacheLocations); + pairLocation.InactivePairIndex = builder.Add(pool, Mapping.Keys[elementIndex], cache); //Now that any existing cache data has been moved into the inactive set, we should remove the overlap from the overlap mapping. Mapping.FastRemove(tableIndex, elementIndex); @@ -117,64 +87,16 @@ internal unsafe void SleepTypeBatchPairs(ref SleepingSetBuilder builder, int set builder.FinalizeSet(pool, out SleepingSets[setIndex]); } - internal ref WorkerPairCache GetCacheForAwakening() - { - //Note that the target location for the set depends on whether the awakening is being executed from within the context of the narrow phase. - //Either way, we need to put the data into the most recently updated cache. If this is happening inside the narrow phase, that is the NextWorkerCaches, - //because we haven't yet flipped the buffers. If it's outside of the narrow phase, then it's the current workerCaches. - //We can distinguish between the two by checking whether the NextWorkerCaches are allocated. They don't exist outside of the narrowphase's execution. - - //Also note that we only deal with one worker cache. Wake ups just dump new caches into the first thread. This works out since - //the actual pair cache modification is locally sequential right now. - if (NextWorkerCaches.Allocated && NextWorkerCaches.Count > 0 && NextWorkerCaches[0].collisionCaches.Allocated) - return ref NextWorkerCaches[0]; - if (workerCaches.Allocated) - return ref workerCaches[0]; - //No caches exist yet; this must be an external call taking place before the first update. Lazily initialize one worker cache. - workerCaches = new ArrayList(1); - var constraints = new QuickList(1, pool); - var collisions = new QuickList(1, pool); - workerCaches.AllocateUnsafely() = new WorkerPairCache(0, pool, ref constraints, ref collisions, 0); - constraints.Dispose(pool); - collisions.Dispose(pool); - return ref workerCaches[0]; - } - - private unsafe PairCacheIndex CopyCacheForAwakening(ref Buffer inactiveCaches, ref Buffer activeCaches, TypedIndex sourceCacheIndex) - { - ref var sourceCache = ref inactiveCaches[sourceCacheIndex.Type]; - //Note that the sourceCacheIndex.Type refers to the index of the type in a packed list, not the noncontiguous type id. - //The unpacked active caches use the noncontiguous type id, so the sourceCache.TypeId is now referenced rather than the sourceCacheIndex.Type. - ref var targetCache = ref activeCaches[sourceCache.TypeId]; - var targetByteIndex = targetCache.Allocate(sourceCache.List.ElementSizeInBytes, sourceCache.List.Count, pool); - Unsafe.CopyBlockUnaligned(targetCache.Buffer.Memory + targetByteIndex, sourceCache.List.Buffer.Memory + sourceCacheIndex.Index, (uint)sourceCache.List.ElementSizeInBytes); - //Note that the cache chosen for activated entries is always the first one, so the cache index is simply 0. - return new PairCacheIndex(0, sourceCache.TypeId, targetByteIndex); - } internal unsafe void AwakenSet(int setIndex) { ref var sleepingSet = ref SleepingSets[setIndex]; //If there are no pairs, there is no need for an inactive set, so it's not guaranteed to be allocated. if (sleepingSet.Allocated) { - ref var activeSet = ref GetCacheForAwakening(); - //For simplicity, awakening simply walks the pairs list in the sleeping set. - //By construction of the inactive set, the cache accesses will be highly cache coherent, so the fact that it doesn't do bulk copies isn't that bad. - //(we COULD make it do bulk copies, but only bother with that if there is any reason to.) for (int i = 0; i < sleepingSet.Pairs.Count; ++i) { ref var pair = ref sleepingSet.Pairs[i]; - CollidablePairPointers pointers; - pointers.ConstraintCache = CopyCacheForAwakening(ref sleepingSet.ConstraintCaches, ref activeSet.constraintCaches, pair.ConstraintCache); - if (pair.CollisionCache.Exists) - { - pointers.CollisionDetectionCache = CopyCacheForAwakening(ref sleepingSet.CollisionCaches, ref activeSet.collisionCaches, pair.CollisionCache); - } - else - { - pointers.CollisionDetectionCache = new PairCacheIndex(); - } - Mapping.AddUnsafely(ref pair.Pair, pointers); + Mapping.AddUnsafely(pair.Pair, pair.Cache); } } } diff --git a/BepuPhysics/CollisionDetection/WorkerPairCache.cs b/BepuPhysics/CollisionDetection/WorkerPairCache.cs index b19afa95d..e262dcbaa 100644 --- a/BepuPhysics/CollisionDetection/WorkerPairCache.cs +++ b/BepuPhysics/CollisionDetection/WorkerPairCache.cs @@ -6,30 +6,15 @@ namespace BepuPhysics.CollisionDetection { - - /// - /// The cached pair data created by a single worker during the last execution of narrow phase pair processing. + /// Contains the pending pair cache changes created by a single worker during the last execution of narrow phase pair processing. /// - public struct WorkerPairCache + public struct WorkerPendingPairChanges { - public struct PreallocationSizes - { - public int ElementCount; - public int ElementSizeInBytes; - } - internal BufferPool pool; //note that this reference makes the entire worker pair cache nonblittable. That's why the pair cache uses managed arrays to store the worker caches. - int minimumPerTypeCapacity; - int workerIndex; - //Note that the per-type batches are untyped. - //The caller will have the necessary type knowledge to interpret the buffer. - internal Buffer constraintCaches; - internal Buffer collisionCaches; - public struct PendingAdd { public CollidablePair Pair; - public CollidablePairPointers Pointers; + public ConstraintCache Cache; } /// @@ -41,162 +26,28 @@ public struct PendingAdd /// public QuickList PendingRemoves; - public WorkerPairCache(int workerIndex, BufferPool pool, - ref QuickList minimumSizesPerConstraintType, - ref QuickList minimumSizesPerCollisionType, - int pendingCapacity, int minimumPerTypeCapacity = 128) + public WorkerPendingPairChanges(BufferPool pool, int pendingCapacity) { - this.workerIndex = workerIndex; - this.pool = pool; - this.minimumPerTypeCapacity = minimumPerTypeCapacity; - const float previousCountMultiplier = 1.25f; - pool.TakeAtLeast(PairCache.CollisionConstraintTypeCount, out constraintCaches); - pool.TakeAtLeast(PairCache.CollisionTypeCount, out collisionCaches); - for (int i = 0; i < minimumSizesPerConstraintType.Count; ++i) - { - ref var sizes = ref minimumSizesPerConstraintType[i]; - if (sizes.ElementCount > 0) - constraintCaches[i] = new UntypedList(sizes.ElementSizeInBytes, Math.Max(minimumPerTypeCapacity, (int)(previousCountMultiplier * sizes.ElementCount)), pool); - else - constraintCaches[i] = new UntypedList(); - } - //Clear out the remainder of slots to avoid invalid data. - constraintCaches.Clear(minimumSizesPerConstraintType.Count, constraintCaches.Length - minimumSizesPerConstraintType.Count); - for (int i = 0; i < minimumSizesPerCollisionType.Count; ++i) - { - ref var sizes = ref minimumSizesPerCollisionType[i]; - if (sizes.ElementCount > 0) - collisionCaches[i] = new UntypedList(sizes.ElementSizeInBytes, Math.Max(minimumPerTypeCapacity, (int)(previousCountMultiplier * sizes.ElementCount)), pool); - else - collisionCaches[i] = new UntypedList(); - } - //Clear out the remainder of slots to avoid invalid data. - collisionCaches.Clear(minimumSizesPerCollisionType.Count, collisionCaches.Length - minimumSizesPerCollisionType.Count); - PendingAdds = new QuickList(pendingCapacity, pool); PendingRemoves = new QuickList(pendingCapacity, pool); } - public void GetMaximumCacheTypeCounts(out int collision, out int constraint) - { - collision = 0; - constraint = 0; - for (int i = collisionCaches.Length - 1; i >= 0; --i) - { - if (collisionCaches[i].Count > 0) - { - collision = i + 1; - break; - } - } - for (int i = constraintCaches.Length - 1; i >= 0; --i) - { - if (constraintCaches[i].Count > 0) - { - constraint = i + 1; - break; - } - } - } - - public void AccumulateMinimumSizes( - ref QuickList minimumSizesPerConstraintType, - ref QuickList minimumSizesPerCollisionType) - { - //Note that the count is expanded only as a constraint or cache of a given type is encountered. - for (int i = 0; i < constraintCaches.Length; ++i) - { - ref var constraintCache = ref constraintCaches[i]; - if (constraintCache.Count > 0) - { - if (i >= minimumSizesPerConstraintType.Count) - { - minimumSizesPerConstraintType.Count = i + 1; - } - ref var sizes = ref minimumSizesPerConstraintType[i]; - sizes.ElementCount = Math.Max(sizes.ElementCount, constraintCache.Count); - //Technically this element size assignment may occur multiple times, but it's also simple and a one time process. - Debug.Assert(sizes.ElementSizeInBytes == 0 || sizes.ElementSizeInBytes == constraintCache.ElementSizeInBytes, "Either this size hasn't been initialized, or it should match."); - sizes.ElementSizeInBytes = constraintCache.ElementSizeInBytes; - } - } - for (int i = collisionCaches.Length - 1; i >= 0; --i) - { - ref var collisionCache = ref collisionCaches[i]; - if (collisionCache.Count > 0) - { - if (i >= minimumSizesPerCollisionType.Count) - { - minimumSizesPerCollisionType.Count = i + 1; - } - ref var sizes = ref minimumSizesPerCollisionType[i]; - sizes.ElementCount = Math.Max(sizes.ElementCount, collisionCache.Count); - //Technically this element size assignment may occur multiple times, but it's also simple and a one time process. - Debug.Assert(sizes.ElementSizeInBytes == 0 || sizes.ElementSizeInBytes == collisionCache.ElementSizeInBytes, "Either this size hasn't been initialized, or it should match."); - sizes.ElementSizeInBytes = collisionCache.ElementSizeInBytes; - } - } - } - - //Note that we have no-collision-data overloads. The vast majority of types don't actually have any collision data cached. [MethodImpl(MethodImplOptions.AggressiveInlining)] - private unsafe void WorkerCacheAdd(ref TCollision collisionCache, ref TConstraint constraintCache, out CollidablePairPointers pointers) - where TCollision : IPairCacheEntry where TConstraint : IPairCacheEntry + public unsafe int Add(BufferPool pool, CollidablePair pair, in ConstraintCache cache) { - pointers.ConstraintCache = new PairCacheIndex(workerIndex, constraintCache.CacheTypeId, constraintCaches[constraintCache.CacheTypeId].Add(ref constraintCache, minimumPerTypeCapacity, pool)); - - if (typeof(TCollision) == typeof(EmptyCollisionCache)) - pointers.CollisionDetectionCache = new PairCacheIndex(); - else - pointers.CollisionDetectionCache = new PairCacheIndex(workerIndex, collisionCache.CacheTypeId, collisionCaches[collisionCache.CacheTypeId].Add(ref collisionCache, minimumPerTypeCapacity, pool)); - - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public PairCacheIndex Add(ref CollidablePair pair, ref TCollision collisionCache, ref TConstraint constraintCache) - where TCollision : IPairCacheEntry where TConstraint : IPairCacheEntry - { - PendingAdd pendingAdd; - WorkerCacheAdd(ref collisionCache, ref constraintCache, out pendingAdd.Pointers); + int index = PendingAdds.Count; + ref var pendingAdd = ref PendingAdds.Allocate(pool); pendingAdd.Pair = pair; - PendingAdds.Add(pendingAdd, pool); - return pendingAdd.Pointers.ConstraintCache; - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Update(ref CollidablePairPointers pointers, ref TCollision collisionCache, ref TConstraint constraintCache) - where TCollision : IPairCacheEntry where TConstraint : IPairCacheEntry - { - WorkerCacheAdd(ref collisionCache, ref constraintCache, out pointers); + pendingAdd.Cache = cache; + return index; } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - internal unsafe void* GetConstraintCachePointer(PairCacheIndex constraintCacheIndex) - { - return constraintCaches[constraintCacheIndex.Type].Buffer.Memory + constraintCacheIndex.Index; - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - internal unsafe void* GetCollisionCachePointer(PairCacheIndex collisionCacheIndex) - { - return collisionCaches[collisionCacheIndex.Type].Buffer.Memory + collisionCacheIndex.Index; - } - public void Dispose() + public void Dispose(BufferPool pool) { - for (int i = 0; i < constraintCaches.Length; ++i) - { - if (constraintCaches[i].Buffer.Allocated) - pool.Return(ref constraintCaches[i].Buffer); - } - pool.Return(ref constraintCaches); - for (int i = 0; i < collisionCaches.Length; ++i) - { - if (collisionCaches[i].Buffer.Allocated) - pool.Return(ref collisionCaches[i].Buffer); - } - pool.Return(ref collisionCaches); - this = new WorkerPairCache(); - //note that the pending collections are not disposed here; they are disposed upon flushing immediately after the narrow phase completes. + PendingAdds.Dispose(pool); + PendingRemoves.Dispose(pool); } } } diff --git a/BepuPhysics/DefaultTypes.cs b/BepuPhysics/DefaultTypes.cs index 73b673dce..1a7223e04 100644 --- a/BepuPhysics/DefaultTypes.cs +++ b/BepuPhysics/DefaultTypes.cs @@ -64,22 +64,22 @@ public static void RegisterDefaults(Solver solver, NarrowPhase narrowPhase) solver.Register(); solver.Register(); - narrowPhase.RegisterContactConstraintAccessor(new NonconvexTwoBodyAccessor()); - narrowPhase.RegisterContactConstraintAccessor(new NonconvexTwoBodyAccessor()); - narrowPhase.RegisterContactConstraintAccessor(new NonconvexTwoBodyAccessor()); - - narrowPhase.RegisterContactConstraintAccessor(new NonconvexOneBodyAccessor()); - narrowPhase.RegisterContactConstraintAccessor(new NonconvexOneBodyAccessor()); - narrowPhase.RegisterContactConstraintAccessor(new NonconvexOneBodyAccessor()); - - narrowPhase.RegisterContactConstraintAccessor(new ConvexTwoBodyAccessor()); - narrowPhase.RegisterContactConstraintAccessor(new ConvexTwoBodyAccessor()); - narrowPhase.RegisterContactConstraintAccessor(new ConvexTwoBodyAccessor()); - narrowPhase.RegisterContactConstraintAccessor(new ConvexTwoBodyAccessor()); - narrowPhase.RegisterContactConstraintAccessor(new ConvexOneBodyAccessor()); - narrowPhase.RegisterContactConstraintAccessor(new ConvexOneBodyAccessor()); - narrowPhase.RegisterContactConstraintAccessor(new ConvexOneBodyAccessor()); - narrowPhase.RegisterContactConstraintAccessor(new ConvexOneBodyAccessor()); + narrowPhase.RegisterContactConstraintAccessor(new NonconvexTwoBodyAccessor()); + narrowPhase.RegisterContactConstraintAccessor(new NonconvexTwoBodyAccessor()); + narrowPhase.RegisterContactConstraintAccessor(new NonconvexTwoBodyAccessor()); + + narrowPhase.RegisterContactConstraintAccessor(new NonconvexOneBodyAccessor()); + narrowPhase.RegisterContactConstraintAccessor(new NonconvexOneBodyAccessor()); + narrowPhase.RegisterContactConstraintAccessor(new NonconvexOneBodyAccessor()); + + narrowPhase.RegisterContactConstraintAccessor(new ConvexTwoBodyAccessor()); + narrowPhase.RegisterContactConstraintAccessor(new ConvexTwoBodyAccessor()); + narrowPhase.RegisterContactConstraintAccessor(new ConvexTwoBodyAccessor()); + narrowPhase.RegisterContactConstraintAccessor(new ConvexTwoBodyAccessor()); + narrowPhase.RegisterContactConstraintAccessor(new ConvexOneBodyAccessor()); + narrowPhase.RegisterContactConstraintAccessor(new ConvexOneBodyAccessor()); + narrowPhase.RegisterContactConstraintAccessor(new ConvexOneBodyAccessor()); + narrowPhase.RegisterContactConstraintAccessor(new ConvexOneBodyAccessor()); } diff --git a/BepuPhysics/IslandAwakener.cs b/BepuPhysics/IslandAwakener.cs index 166973add..fec45ae25 100644 --- a/BepuPhysics/IslandAwakener.cs +++ b/BepuPhysics/IslandAwakener.cs @@ -448,41 +448,9 @@ void ValidateUniqueSets(ref QuickList setIndices) } - //This is getting into the realm of Fizzbuzz Enterprise. - interface ITypeCount + struct TypeAllocationSizes { - void Add(T other) where T : ITypeCount; - } - struct ConstraintCount : ITypeCount - { - public int Count; - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Add(T other) where T : ITypeCount - { - Debug.Assert(typeof(T) == typeof(ConstraintCount)); - Count += Unsafe.As(ref other).Count; - } - } - struct PairCacheCount : ITypeCount - { - public int ElementSizeInBytes; - public int ByteCount; - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Add(T other) where T : ITypeCount - { - Debug.Assert(typeof(T) == typeof(PairCacheCount)); - ref var pairCacheOther = ref Unsafe.As(ref other); - Debug.Assert(ElementSizeInBytes == 0 || ElementSizeInBytes == pairCacheOther.ElementSizeInBytes); - ElementSizeInBytes = pairCacheOther.ElementSizeInBytes; - ByteCount += pairCacheOther.ByteCount; - } - } - - struct TypeAllocationSizes where T : unmanaged, ITypeCount - { - public Buffer TypeCounts; + public Buffer TypeCounts; public int HighestOccupiedTypeIndex; public TypeAllocationSizes(BufferPool pool, int maximumTypeCount) { @@ -491,9 +459,9 @@ public TypeAllocationSizes(BufferPool pool, int maximumTypeCount) HighestOccupiedTypeIndex = 0; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Add(int typeId, T typeCount) + public void Add(int typeId, int typeCount) { - TypeCounts[typeId].Add(typeCount); + TypeCounts[typeId] += typeCount; if (typeId > HighestOccupiedTypeIndex) HighestOccupiedTypeIndex = typeId; } @@ -546,25 +514,12 @@ unsafe internal (int phaseOneJobCount, int phaseTwoJobCount) PrepareJobs(ref Qui } //We accumulated indices above; add one to get the capacity requirement. ++highestRequiredTypeCapacity; - pool.Take>(highestNewBatchCount, out var constraintCountPerTypePerBatch); + pool.Take(highestNewBatchCount, out var constraintCountPerTypePerBatch); for (int batchIndex = 0; batchIndex < highestNewBatchCount; ++batchIndex) { - constraintCountPerTypePerBatch[batchIndex] = new TypeAllocationSizes(pool, highestRequiredTypeCapacity); + constraintCountPerTypePerBatch[batchIndex] = new TypeAllocationSizes(pool, highestRequiredTypeCapacity); } - var narrowPhaseConstraintCaches = new TypeAllocationSizes(pool, PairCache.CollisionConstraintTypeCount); - var narrowPhaseCollisionCaches = new TypeAllocationSizes(pool, PairCache.CollisionTypeCount); - void AccumulatePairCacheTypeCounts(ref Buffer sourceTypeCaches, ref TypeAllocationSizes counts) - { - for (int j = 0; j < sourceTypeCaches.Length; ++j) - { - ref var sourceCache = ref sourceTypeCaches[j]; - if (sourceCache.List.Buffer.Allocated) - counts.Add(sourceCache.TypeId, new PairCacheCount { ByteCount = sourceCache.List.ByteCount, ElementSizeInBytes = sourceCache.List.ElementSizeInBytes }); - else - break; //Encountering an unallocated slot is a termination condition. Used instead of explicitly storing cache counts, which are only rarely useful. - } - } int newPairCount = 0; for (int i = 0; i < setIndices.Count; ++i) { @@ -577,14 +532,12 @@ void AccumulatePairCacheTypeCounts(ref Buffer sourceTypeCaches, r for (int typeBatchIndex = 0; typeBatchIndex < batch.TypeBatches.Count; ++typeBatchIndex) { ref var typeBatch = ref batch.TypeBatches[typeBatchIndex]; - constraintCountPerType.Add(typeBatch.TypeId, new ConstraintCount { Count = typeBatch.ConstraintCount }); + constraintCountPerType.Add(typeBatch.TypeId, typeBatch.ConstraintCount); } } ref var sourceSet = ref pairCache.SleepingSets[setIndex]; newPairCount += sourceSet.Pairs.Count; - AccumulatePairCacheTypeCounts(ref sourceSet.ConstraintCaches, ref narrowPhaseConstraintCaches); - AccumulatePairCacheTypeCounts(ref sourceSet.CollisionCaches, ref narrowPhaseCollisionCaches); } //We now know how many new bodies, constraint batch entries, and pair cache entries are going to be added. @@ -613,7 +566,7 @@ void AccumulatePairCacheTypeCounts(ref Buffer sourceTypeCaches, r solver.batchReferencedHandles[batchIndex].EnsureCapacity(bodies.HandlePool.HighestPossiblyClaimedId + 1, pool); for (int typeId = 0; typeId <= constraintCountPerType.HighestOccupiedTypeIndex; ++typeId) { - var countForType = constraintCountPerType.TypeCounts[typeId].Count; + var countForType = constraintCountPerType.TypeCounts[typeId]; //The fallback batch must allocate a worst case scenario assuming that every new constraint needs its own bundle. //It's difficult to be more conservative ahead of time; we don't know which existing partial bundles will be able to accept the new constraints. //Fallback batches should tend to be rarely used and relatively small, and the extra memory won't be touched, so this isn't a major concern. @@ -633,24 +586,7 @@ void AccumulatePairCacheTypeCounts(ref Buffer sourceTypeCaches, r constraintCountPerType.Dispose(pool); } pool.Return(ref constraintCountPerTypePerBatch); - //and narrow phase pair caches. - ref var targetPairCache = ref pairCache.GetCacheForAwakening(); - void EnsurePairCacheTypeCapacities(ref TypeAllocationSizes cacheSizes, ref Buffer targetCaches, BufferPool cachePool) - { - for (int typeIndex = 0; typeIndex <= cacheSizes.HighestOccupiedTypeIndex; ++typeIndex) - { - ref var pairCacheCount = ref cacheSizes.TypeCounts[typeIndex]; - if (pairCacheCount.ByteCount > 0) - { - ref var targetSubCache = ref targetCaches[typeIndex]; - targetSubCache.EnsureCapacityInBytes(pairCacheCount.ElementSizeInBytes, targetSubCache.ByteCount + pairCacheCount.ByteCount, cachePool); - } - } - } - EnsurePairCacheTypeCapacities(ref narrowPhaseConstraintCaches, ref targetPairCache.constraintCaches, targetPairCache.pool); - EnsurePairCacheTypeCapacities(ref narrowPhaseCollisionCaches, ref targetPairCache.collisionCaches, targetPairCache.pool); - narrowPhaseConstraintCaches.Dispose(pool); - narrowPhaseCollisionCaches.Dispose(pool); + //and the narrow phase pair cache. pairCache.Mapping.EnsureCapacity(pairCache.Mapping.Count + newPairCount, pool); phaseOneJobs = new QuickList(Math.Max(32, highestNewBatchCount + 1), pool); diff --git a/BepuPhysics/IslandSleeper.cs b/BepuPhysics/IslandSleeper.cs index 82096f819..539cba248 100644 --- a/BepuPhysics/IslandSleeper.cs +++ b/BepuPhysics/IslandSleeper.cs @@ -497,7 +497,7 @@ void ExecuteRemoval(ref RemovalJob job) largestBodyCount = setCount; } //We just arbitrarily guess a few pairs per body. It might be wrong, but that's fine- it'll resize if needed. Just don't want to constantly resize. - var setBuilder = new SleepingSetBuilder(pool, largestBodyCount * 4, largestBodyCount); + var setBuilder = new SleepingSetBuilder(pool, largestBodyCount * 4); for (int setReferenceIndex = 0; setReferenceIndex < newInactiveSets.Count; ++setReferenceIndex) { pairCache.SleepTypeBatchPairs(ref setBuilder, newInactiveSets[setReferenceIndex].Index, solver); diff --git a/Demos/SpecializedTests/ShapePileTestDemo.cs b/Demos/SpecializedTests/ShapePileTestDemo.cs index 7734361ee..cdbe4ad9e 100644 --- a/Demos/SpecializedTests/ShapePileTestDemo.cs +++ b/Demos/SpecializedTests/ShapePileTestDemo.cs @@ -95,7 +95,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) { for (int k = 0; k < length; ++k) { - var location = new Vector3(6, 3, 6) * new Vector3(i, j, k) + new Vector3(-width * 1.5f, 5.5f, -length * 1.5f); + var location = new Vector3(6, 3, 6) * new Vector3(i, j, k) + new Vector3(-width * 3, 5.5f, -length * 3); var bodyDescription = BodyDescription.CreateKinematic(location, new(default, ContinuousDetection.Passive), 0.01f); var index = shapeCount++; switch (index % 5) From d7a961245e24cc809d64bfb2150840fdcbb8212f Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 19 Nov 2022 18:58:03 -0600 Subject: [PATCH 563/947] Fixed raybatcher memory leak. --- BepuPhysics/Trees/RayBatcher.cs | 31 +++++++++++++++---------------- 1 file changed, 15 insertions(+), 16 deletions(-) diff --git a/BepuPhysics/Trees/RayBatcher.cs b/BepuPhysics/Trees/RayBatcher.cs index f9a490a47..4fd59086c 100644 --- a/BepuPhysics/Trees/RayBatcher.cs +++ b/BepuPhysics/Trees/RayBatcher.cs @@ -170,7 +170,22 @@ public RayBatcher(BufferPool pool, int rayCapacity = 2048, int treeDepthForPreal ResizeRayStacks(rayCapacity, treeDepthForPreallocation); stackPointer = stackPointerA0 = stackPointerB = stackPointerA1 = 0; + } + /// + /// Disposes all the resources backing the ray batcher. + /// + public void Dispose() + { + pool.ReturnUnsafely(rayIndicesA0.Id); + pool.ReturnUnsafely(rayIndicesB.Id); + pool.ReturnUnsafely(rayIndicesA1.Id); + pool.ReturnUnsafely(stack.Id); + pool.ReturnUnsafely(fallbackStack.Id); + pool.ReturnUnsafely(batchOriginalRays.Id); + pool.ReturnUnsafely(batchRays.Id); + //Easier to catch bugs if the references get cleared. + this = default; } void ResizeRayStacks(int rayCapacity, int treeDepthForPreallocation) @@ -547,21 +562,5 @@ public void ResetRays() batchRayCount = 0; } - /// - /// Disposes all the resources backing the ray batcher. - /// - public void Dispose() - { - pool.ReturnUnsafely(rayIndicesA0.Id); - pool.ReturnUnsafely(rayIndicesB.Id); - pool.ReturnUnsafely(rayIndicesA1.Id); - pool.ReturnUnsafely(stack.Id); - pool.ReturnUnsafely(batchOriginalRays.Id); - pool.ReturnUnsafely(batchRays.Id); - //Easier to catch bugs if the references get cleared. - this = default; - } - - } } From 9d2da28aa073e33c1abc2170e1be8a317692e618 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 20 Nov 2022 16:16:42 -0600 Subject: [PATCH 564/947] ArenaPool. --- BepuUtilities/Memory/ArenaPool.cs | 263 ++++++++++++++++++++++++++++++ 1 file changed, 263 insertions(+) create mode 100644 BepuUtilities/Memory/ArenaPool.cs diff --git a/BepuUtilities/Memory/ArenaPool.cs b/BepuUtilities/Memory/ArenaPool.cs new file mode 100644 index 000000000..71226b481 --- /dev/null +++ b/BepuUtilities/Memory/ArenaPool.cs @@ -0,0 +1,263 @@ +using BepuUtilities.Collections; +using System; +using System.Collections.Generic; +using System.Diagnostics; +using System.Runtime.CompilerServices; + +namespace BepuUtilities.Memory; + +/// +/// Arena allocator built to serve a single thread. Pulls resources from a central buffer pool when necessary. +/// +public class ArenaPool : IUnmanagedMemoryPool +{ + /// + /// Gets the central pool backing this arena allocator. Resources are pulled from this pool when the thread pool's blocks are depleted. + /// + public IUnmanagedMemoryPool Pool { get; set; } + /// + /// Gets the locker object used to protect accesses to the cetnral buffer pool. + /// + public object Locker { get; private set; } + /// + /// Default capacity within blocks allocated by the pool. + /// + public int DefaultBlockCapacity { get; set; } + + struct Block + { + public int Count; + public Buffer Data; + + public Block(Buffer data) + { + Data = data; + Count = 0; + } + + public bool TryAllocate(int sizeInBytes, out Buffer allocation) + { + //Following the pattern set by the bufferpool, use beefy alignment: + var startLocation = (Count + 127) & (~127); + var newCount = startLocation + sizeInBytes; + if (Data.Length >= newCount) + { + allocation = Data.Slice(startLocation, sizeInBytes); + Count = newCount; + return true; + } + allocation = default; + return false; + } + } +#if DEBUG + internal HashSet outstandingIds; +#if LEAKDEBUG + internal Dictionary> outstandingAllocators; +#endif +#endif + + QuickList blocks; + + /// + /// Creates a new arena thread pool. + /// + /// Central pool to allocate blocks from + /// Locker object used to protect accesses to the central pool. If no locker is specified, the pool reference is used. + public ArenaPool(IUnmanagedMemoryPool pool, object locker = null) + { + Pool = pool; + Locker = locker == null ? pool : locker; + blocks = new QuickList(8, pool); + //We check for allocated blocks before allocating one, so we need to clear the memory up front. + blocks.Span.Clear(0, blocks.Span.Length); +#if DEBUG + outstandingIds = new HashSet(); +#if LEAKDEBUG + outstandingAllocators = new Dictionary>(); +#endif +#endif + } + + /// + /// Ensures that there is a given minimum amount of continguous space available in the pool. This does not acquire a lock to read from the internal pool. + /// + /// Size of the block to use. If negative, will be used instead. + public void EnsurePreallocatedSpaceUnsafely(int sizeInBytes = -1) + { + if (sizeInBytes < 0) + sizeInBytes = DefaultBlockCapacity; + if (blocks.Count > 0) + { + ref var block = ref blocks[blocks.Count - 1]; + if (block.Data.Length - block.Count >= sizeInBytes) + { + //There's enough room already; no need to allocate. + return; + } + } + //We need a new block. + Pool.Take(sizeInBytes, out var data); + blocks.Allocate(Pool) = new Block(data); + + } + + /// + /// Clears all block allocations from the pool. The pool can still be used. + /// + public void Clear() + { +#if DEBUG + outstandingIds.Clear(); +#if LEAKDEBUG + outstandingAllocators.Clear(); +#endif +#endif + for (int i = 0; i < blocks.Count; ++i) + { + Pool.Return(ref blocks[i].Data); + } + blocks.Count = 0; + } + + /// + /// Clears all allocations from the pool, including any used by the pool itself. The pool can no longer be used. + /// + public void Dispose() + { + Clear(); + blocks.Dispose(Pool); + } + + const int maximumBitsForBlock = 1; + const int maximumBitsForIndices = 31 - maximumBitsForBlock; + const int maximumBitsForIndex = maximumBitsForIndices / 2; + const int countInBlockMask = (1 << maximumBitsForIndex) - 1; + const int previousCountInBlockMask = countInBlockMask << maximumBitsForIndex; + const int blockMask = ~(countInBlockMask | previousCountInBlockMask); + const int lowerBlockMask = (1 << maximumBitsForBlock) - 1; + + /// + public unsafe void TakeAtLeast(int count, out Buffer buffer) where T : unmanaged + { + var sizeInBytes = Unsafe.SizeOf() * count; + var blockIndex = blocks.Count - 1; + int previousCount = blocks[blockIndex].Count; + if (!blocks[blockIndex].TryAllocate(sizeInBytes, out Buffer allocation)) + { + //No room; need a new block. + previousCount = 0; + var newBlockCapacityInBytes = Math.Max(DefaultBlockCapacity, sizeInBytes); + //Check to see if there's already a block allocated that we can use. + if (blocks.Span.Length > blocks.Count && blocks.Span[blocks.Count].Data.Length >= sizeInBytes) + { + ++blocks.Count; + Debug.Assert(blocks[blocks.Count - 1].Count == 0 && blocks[blocks.Count - 1].Data.Memory != null); + } + else + { + Buffer blockData; + lock (Locker) + { + Pool.Take(newBlockCapacityInBytes, out blockData); + if (blocks.Span.Length == blocks.Count) + blocks.EnsureCapacity(blocks.Count * 2, Pool); + } + blocks.AllocateUnsafely() = new Block(blockData); + } + blockIndex = blocks.Count - 1; + var succeeded = blocks[blockIndex].TryAllocate(sizeInBytes, out allocation); + Debug.Assert(succeeded, "We just allocated that block, it should hold everything requested!"); + } + buffer = allocation.As(); + var newCount = previousCount + sizeInBytes; + var bitpackedBlockIndex = lowerBlockMask & blockIndex; + var bitpackedStartIndex = countInBlockMask & newCount; + var bitpackedPreviousIndex = countInBlockMask & previousCount; + buffer.Id = (bitpackedBlockIndex << maximumBitsForIndices) | (bitpackedPreviousIndex << maximumBitsForIndex) | bitpackedStartIndex; + if (blockIndex >= (1 << maximumBitsForBlock) || previousCount >= (1 << maximumBitsForIndex) || newCount >= (1 << maximumBitsForIndex)) + { + //Bit index 31 being set is code for 'unrepresentable'. + buffer.Id |= 1 << 31; + } +#if DEBUG + if (buffer.Id >= 0) //Don't include unrepresentable ids in the tracked set. + { + const int maximumOutstandingCount = 1 << 26; + Debug.Assert(outstandingIds.Count < maximumOutstandingCount, + $"Do you actually truly really need to have {maximumOutstandingCount} allocations taken from this pool, or is this a memory leak?"); + Debug.Assert(outstandingIds.Add(buffer.Id), "Should not be able to request the same slot twice."); +#if LEAKDEBUG + var allocator = new StackTrace().ToString(); + if (!outstandingAllocators.TryGetValue(allocator, out var idsForAllocator)) + { + idsForAllocator = new HashSet(); + outstandingAllocators.Add(allocator, idsForAllocator); + } + const int maximumReasonableOutstandingAllocationsForAllocator = 1 << 25; + Debug.Assert(idsForAllocator.Count < maximumReasonableOutstandingAllocationsForAllocator, "Do you actually have that many allocations for this one allocator?"); + idsForAllocator.Add(buffer.Id); +#endif + } +#endif + } + /// + public void Take(int count, out Buffer buffer) where T : unmanaged + { + TakeAtLeast(count, out buffer); + } + + /// + /// Unlike a , the will not generally free up space in response to calls to . + /// If the deallocated buffer is the last allocated buffer for a given block, the pool may choose to bump the allocation pointer back, but it is not guaranteed. + public void Return(ref Buffer buffer) where T : unmanaged + { + if (buffer.Id >= 0) + { + //This was a representable id. +#if DEBUG + Debug.Assert(outstandingIds.Remove(buffer.Id), + "This buffer id must have been taken from the pool previously."); +#if LEAKDEBUG + bool found = false; + foreach (var pair in outstandingAllocators) + { + if (pair.Value.Remove(buffer.Id)) + { + found = true; + if (pair.Value.Count == 0) + { + outstandingAllocators.Remove(pair.Key); + break; + } + } + } + Debug.Assert(found, "Allocator set must contain the buffer id."); +#endif +#endif + //Was it the most recently allocated buffer (in that block) such that we can pop it off like a stack? + var blockIndex = (buffer.Id & blockMask) >> maximumBitsForIndices; + var countInBlock = buffer.Id & countInBlockMask; + var previousCountInBlock = (buffer.Id & countInBlockMask) >> maximumBitsForIndex; + ref var block = ref blocks[blockIndex]; + if (block.Count == countInBlock) + { + //Deallocating is as simple as just resetting to the previous value. + block.Count = previousCountInBlock; + while (blocks[blocks.Count - 1].Count == 0) + { + //Push the block count as far as it can go. + --blocks.Count; + } + } + } + buffer = default; + } + + /// + public int GetCapacityForCount(int count) where T : unmanaged + { + return count; + } + +} From 3b2a944fa2bcd37d4901ed839401afacbf1a3ab3 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 20 Nov 2022 16:35:03 -0600 Subject: [PATCH 565/947] WorkerBufferPools. --- BepuUtilities/Memory/ArenaPool.cs | 16 ++-- BepuUtilities/Memory/WorkerBufferPools.cs | 107 ++++++++++++++++++++++ 2 files changed, 117 insertions(+), 6 deletions(-) create mode 100644 BepuUtilities/Memory/WorkerBufferPools.cs diff --git a/BepuUtilities/Memory/ArenaPool.cs b/BepuUtilities/Memory/ArenaPool.cs index 71226b481..42e2e6db3 100644 --- a/BepuUtilities/Memory/ArenaPool.cs +++ b/BepuUtilities/Memory/ArenaPool.cs @@ -9,10 +9,10 @@ namespace BepuUtilities.Memory; /// /// Arena allocator built to serve a single thread. Pulls resources from a central buffer pool when necessary. /// -public class ArenaPool : IUnmanagedMemoryPool +public class ArenaPool : IUnmanagedMemoryPool, IDisposable { /// - /// Gets the central pool backing this arena allocator. Resources are pulled from this pool when the thread pool's blocks are depleted. + /// Gets or sets the central pool backing this arena allocator. Resources are pulled from this pool when the thread pool's blocks are depleted. /// public IUnmanagedMemoryPool Pool { get; set; } /// @@ -20,7 +20,7 @@ public class ArenaPool : IUnmanagedMemoryPool /// public object Locker { get; private set; } /// - /// Default capacity within blocks allocated by the pool. + /// Gets or sets the default capacity within blocks allocated by the pool. /// public int DefaultBlockCapacity { get; set; } @@ -64,10 +64,11 @@ public bool TryAllocate(int sizeInBytes, out Buffer allocation) /// /// Central pool to allocate blocks from /// Locker object used to protect accesses to the central pool. If no locker is specified, the pool reference is used. - public ArenaPool(IUnmanagedMemoryPool pool, object locker = null) + public ArenaPool(IUnmanagedMemoryPool pool, int defaultBlockCapacity = 16384, object locker = null) { Pool = pool; Locker = locker == null ? pool : locker; + DefaultBlockCapacity = defaultBlockCapacity; blocks = new QuickList(8, pool); //We check for allocated blocks before allocating one, so we need to clear the memory up front. blocks.Span.Clear(0, blocks.Span.Length); @@ -125,8 +126,11 @@ public void Clear() /// public void Dispose() { - Clear(); - blocks.Dispose(Pool); + if (blocks.Span.Allocated) + { + Clear(); + blocks.Dispose(Pool); + } } const int maximumBitsForBlock = 1; diff --git a/BepuUtilities/Memory/WorkerBufferPools.cs b/BepuUtilities/Memory/WorkerBufferPools.cs new file mode 100644 index 000000000..f58f4d672 --- /dev/null +++ b/BepuUtilities/Memory/WorkerBufferPools.cs @@ -0,0 +1,107 @@ +using System; + +namespace BepuUtilities.Memory; + +/// +/// Collection of arena pools used by worker threads. +/// +public class WorkerBufferPools : IDisposable +{ + ArenaPool[] pools; + /// + /// Central pool from which subpools allocate. + /// + public BufferPool Pool { get; private set; } + /// + /// Locker used by subpools to control access to the central pool. + /// + public object Locker { get; private set; } + + /// + /// Gets the pool associated with this worker. + /// + /// Worker index of the pool to look up. + /// Pool associated with the given worker. + public ArenaPool this[int workerIndex] => pools[workerIndex]; + + /// + /// Gets or sets the default block capacity for any newly created arena subpools. + /// + public int DefaultBlockCapacity { get; set; } + + + /// + /// Creates a new set of worker pools. + /// + /// Central pool from which worker pools allocate from. + /// Initial number of workers to allocate space for. + /// Default block capacity in thread pools. + public WorkerBufferPools(BufferPool pool, int initialWorkerCount, int defaultBlockCapacity = 16384) + { + Pool = pool; + Locker = new object(); + pools = new ArenaPool[initialWorkerCount]; + DefaultBlockCapacity = defaultBlockCapacity; + for (int i = 0; i < pools.Length; ++i) + { + pools[i] = new ArenaPool(pool, defaultBlockCapacity, Locker); + } + } + + /// + /// Preallocates for a given number of workers. + /// + /// Number of workers to preallocate for. + /// Capacity to preallocate in the specified worker pools. + public void Preallocate(int workerCount, int preallocationSize) + { + if (workerCount > pools.Length) + { + var oldSize = pools.Length; + Array.Resize(ref pools, workerCount); + for (int i = oldSize; i < pools.Length; ++i) + { + pools[i] = new ArenaPool(Pool, DefaultBlockCapacity); + } + } + for (int i = 0; i < workerCount; ++i) + { + pools[i].EnsurePreallocatedSpaceUnsafely(preallocationSize); + } + } + + /// + /// Preallocates space in all worker pools. + /// + /// Capacity to preallocate in all worker pools. + public void Preallocate(int preallocationSize) + { + Preallocate(pools.Length, preallocationSize); + } + + /// + /// Clears all allocations from worker arena pools. Pools can still be used after being cleared. + /// + public void Clear() + { + for (int i = 0; i < pools.Length; ++i) + { + pools[i].Clear(); + } + } + + + /// + /// Disposes all worker arena pools. Pools cannot be used after being disposed. + /// + public void Dispose() + { + for (int i = 0; i < pools.Length; ++i) + { + pools[i].Dispose(); + } + } + + + +} From ed18cae7b839a9bddfed359e1c05a51b1ef4f8c5 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 20 Nov 2022 17:01:42 -0600 Subject: [PATCH 566/947] ArenaPool deoopsed. ContactEventsDemo now uses it instead of ThreadDispatcher pools. --- BepuUtilities/Memory/ArenaPool.cs | 10 +++++----- Demos/Demos/ContactEventsDemo.cs | 13 +++++++++++-- Demos/Demos/RayCastingDemo.cs | 2 +- 3 files changed, 17 insertions(+), 8 deletions(-) diff --git a/BepuUtilities/Memory/ArenaPool.cs b/BepuUtilities/Memory/ArenaPool.cs index 42e2e6db3..6969191b3 100644 --- a/BepuUtilities/Memory/ArenaPool.cs +++ b/BepuUtilities/Memory/ArenaPool.cs @@ -146,11 +146,9 @@ public unsafe void TakeAtLeast(int count, out Buffer buffer) where T : unm { var sizeInBytes = Unsafe.SizeOf() * count; var blockIndex = blocks.Count - 1; - int previousCount = blocks[blockIndex].Count; - if (!blocks[blockIndex].TryAllocate(sizeInBytes, out Buffer allocation)) + if (blocks.Count == 0 || !blocks[blockIndex].TryAllocate(sizeInBytes, out Buffer allocation)) { //No room; need a new block. - previousCount = 0; var newBlockCapacityInBytes = Math.Max(DefaultBlockCapacity, sizeInBytes); //Check to see if there's already a block allocated that we can use. if (blocks.Span.Length > blocks.Count && blocks.Span[blocks.Count].Data.Length >= sizeInBytes) @@ -160,6 +158,7 @@ public unsafe void TakeAtLeast(int count, out Buffer buffer) where T : unm } else { + //Need a new block. Buffer blockData; lock (Locker) { @@ -174,7 +173,8 @@ public unsafe void TakeAtLeast(int count, out Buffer buffer) where T : unm Debug.Assert(succeeded, "We just allocated that block, it should hold everything requested!"); } buffer = allocation.As(); - var newCount = previousCount + sizeInBytes; + var newCount = blocks[blockIndex].Count; + var previousCount = newCount - sizeInBytes; var bitpackedBlockIndex = lowerBlockMask & blockIndex; var bitpackedStartIndex = countInBlockMask & newCount; var bitpackedPreviousIndex = countInBlockMask & previousCount; @@ -248,7 +248,7 @@ public void Return(ref Buffer buffer) where T : unmanaged { //Deallocating is as simple as just resetting to the previous value. block.Count = previousCountInBlock; - while (blocks[blocks.Count - 1].Count == 0) + while (blocks.Count > 0 && blocks[blocks.Count - 1].Count == 0) { //Push the block count as far as it can go. --blocks.Count; diff --git a/Demos/Demos/ContactEventsDemo.cs b/Demos/Demos/ContactEventsDemo.cs index cdfec2246..b6afc48af 100644 --- a/Demos/Demos/ContactEventsDemo.cs +++ b/Demos/Demos/ContactEventsDemo.cs @@ -172,6 +172,7 @@ struct PreviousCollision Simulation simulation; IThreadDispatcher threadDispatcher; + WorkerBufferPools threadPools; BufferPool pool; //We'll use a handle->index mapping in a CollidableProperty to point at our contiguously stored listeners (in the later listeners array). @@ -215,6 +216,11 @@ public ContactEvents(IThreadDispatcher threadDispatcher = null, BufferPool pool listeners = new Listener[initialListenerCapacity]; } + IUnmanagedMemoryPool GetPoolForWorker(int workerIndex) + { + return threadDispatcher == null ? pool : threadPools[workerIndex]; + } + /// /// Initializes the contact events system with a simulation. /// @@ -226,6 +232,7 @@ public void Initialize(Simulation simulation) this.simulation = simulation; if (pool == null) pool = simulation.BufferPool; + threadPools = threadDispatcher != null ? new WorkerBufferPools(pool, threadDispatcher.ThreadCount) : null; simulation.Timestepper.BeforeCollisionDetection += SetFreshnessForCurrentActivityStatus; listenerIndices = new CollidableProperty(simulation, pool); pendingWorkerAdds = new QuickList[threadDispatcher == null ? 1 : threadDispatcher.ThreadCount]; @@ -469,7 +476,7 @@ void HandleManifoldForCollidable(int workerIndex, CollidableReference //There was no collision previously. ref var addsforWorker = ref pendingWorkerAdds[workerIndex]; //EnsureCapacity will create the list if it doesn't already exist. - addsforWorker.EnsureCapacity(Math.Max(addsforWorker.Count + 1, 64), threadDispatcher != null ? threadDispatcher.GetThreadMemoryPool(workerIndex) : pool); + addsforWorker.EnsureCapacity(Math.Max(addsforWorker.Count + 1, 64), GetPoolForWorker(workerIndex)); ref var pendingAdd = ref addsforWorker.AllocateUnsafely(); pendingAdd.ListenerIndex = listenerIndex; pendingAdd.Collision.Collidable = other; @@ -571,10 +578,11 @@ public void Flush() collisions.AllocateUnsafely() = pendingAdds[j].Collision; } if (pendingAdds.Span.Allocated) - pendingAdds.Dispose(threadDispatcher == null ? pool : threadDispatcher.GetThreadMemoryPool(i)); + pendingAdds.Dispose(GetPoolForWorker(i)); //We rely on zeroing out the count for lazy initialization. pendingAdds = default; } + threadPools?.Clear(); } public void Dispose() @@ -585,6 +593,7 @@ public void Dispose() staticListenerFlags.Dispose(pool); listenerIndices.Dispose(); simulation.Timestepper.BeforeCollisionDetection -= SetFreshnessForCurrentActivityStatus; + threadPools?.Dispose(); for (int i = 0; i < pendingWorkerAdds.Length; ++i) { Debug.Assert(!pendingWorkerAdds[i].Span.Allocated, "The pending worker adds should have been disposed by the previous flush."); diff --git a/Demos/Demos/RayCastingDemo.cs b/Demos/Demos/RayCastingDemo.cs index b5878394b..fcc87e905 100644 --- a/Demos/Demos/RayCastingDemo.cs +++ b/Demos/Demos/RayCastingDemo.cs @@ -65,7 +65,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) camera.Pitch = MathHelper.Pi * 0.1f; Simulation = Simulation.Create(BufferPool, new NoCollisionCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); - + var sphere = new Sphere(0.5f); var capsule = new Capsule(0, 0.5f); From 4d87f41f56dbc321c8cde4219ff2889b40dde7f9 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 20 Dec 2022 18:58:33 -0600 Subject: [PATCH 567/947] Edge fallback in capsule-triangle improved, version bumped. --- BepuPhysics/BepuPhysics.csproj | 2 +- .../CollisionTasks/CapsuleTriangleTester.cs | 18 +++++++++++------- BepuUtilities/BepuUtilities.csproj | 2 +- 3 files changed, 13 insertions(+), 9 deletions(-) diff --git a/BepuPhysics/BepuPhysics.csproj b/BepuPhysics/BepuPhysics.csproj index 98424711c..753dafe0c 100644 --- a/BepuPhysics/BepuPhysics.csproj +++ b/BepuPhysics/BepuPhysics.csproj @@ -1,7 +1,7 @@  net7.0 - 2.5.0-beta.9 + 2.5.0-beta.10 Bepu Entertainment LLC Ross Nordby Speedy real time physics simulation library. diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleTriangleTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleTriangleTester.cs index e120c16e9..e12e5f337 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleTriangleTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleTriangleTester.cs @@ -55,22 +55,26 @@ public static void TestEdge(in TriangleWide triangle, in Vector3Wide triangleNor Vector3Wide.Subtract(closestPointOnCapsule, closestPointOnEdge, out normal); Vector3Wide.LengthSquared(normal, out var normalLengthSquared); - //In the event that the normal has zero length due to the capsule internal line segment touching the edge, use the cross product of the edge and axis. - Vector3Wide.CrossWithoutOverlap(edgeOffset, capsuleAxis, out var fallbackNormal); + //In the event that the normal has zero length due to the capsule internal line segment touching the edge, use the calibrated cross product of the edge and axis. + Vector3Wide.CrossWithoutOverlap(capsuleAxis, edgeOffset, out var fallbackNormal); + //Fallback calibration can use the fact that dot(fallbackNormal, capsuleCenter) >= 0, because capsuleCenter is the offset from the center of the triangle to the center of the capsule. + Vector3Wide.Dot(fallbackNormal, capsuleCenter, out var calibrationDot); + Vector3Wide.ConditionallyNegate(Vector.LessThan(calibrationDot, Vector.Zero), ref fallbackNormal); Vector3Wide.LengthSquared(fallbackNormal, out var fallbackNormalLengthSquared); - var useFallbackNormal = Vector.LessThan(normalLengthSquared, new Vector(1e-15f)); + var useFallbackNormal = Vector.LessThan(normalLengthSquared, new Vector(1e-13f)); Vector3Wide.ConditionalSelect(useFallbackNormal, fallbackNormal, normal, out normal); normalLengthSquared = Vector.ConditionalSelect(useFallbackNormal, fallbackNormalLengthSquared, normalLengthSquared); //Unfortunately, if the edge and axis are parallel, the cross product will ALSO be zero, so we need another fallback. We'll use the edge plane normal. //Unless the triangle is degenerate, this can't be zero length. Vector3Wide.CrossWithoutOverlap(triangleNormal, edgeOffset, out var secondFallbackNormal); Vector3Wide.LengthSquared(fallbackNormal, out var secondFallbackNormalLengthSquared); - var useSecondFallbackNormal = Vector.LessThan(normalLengthSquared, new Vector(1e-15f)); + var useSecondFallbackNormal = Vector.LessThan(normalLengthSquared, new Vector(1e-13f)); Vector3Wide.ConditionalSelect(useSecondFallbackNormal, secondFallbackNormal, normal, out normal); normalLengthSquared = Vector.ConditionalSelect(useSecondFallbackNormal, secondFallbackNormalLengthSquared, normalLengthSquared); - //Note that we DO NOT 'calibrate' the normal here! The edge winding should avoid the need for any calibration, - //and attempting to calibrate this normal can actually result in normals pointing into the triangle face. - //That can cause backfaces to generate contacts incorrectly. + //Note that we do not do additional normal calibration here! + //Closest points contacts should not need it, + //first fallback normals do their own calibration, and + //second fallback does not need calibration by edge winding. Vector3Wide.Scale(normal, Vector.One / Vector.SquareRoot(normalLengthSquared), out normal); //Note that the normal between the closest points is not necessarily perpendicular to both the edge and capsule axis due to clamping, so to compute depth diff --git a/BepuUtilities/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj index af406f064..01e197cd6 100644 --- a/BepuUtilities/BepuUtilities.csproj +++ b/BepuUtilities/BepuUtilities.csproj @@ -3,7 +3,7 @@ BepuUtilities BepuUtilities net7.0 - 2.5.0-beta.9 + 2.5.0-beta.10 Bepu Entertainment LLC Ross Nordby Supporting utilities library for BEPUphysics v2. From 074d784d14980f0b9dfa96949e0057109f0cecc4 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 10 Jan 2023 17:59:55 -0600 Subject: [PATCH 568/947] Fixed an oopsy in MeshCache; old meshes now actually get removed. --- DemoRenderer/ShapeDrawing/MeshCache.cs | 1 + 1 file changed, 1 insertion(+) diff --git a/DemoRenderer/ShapeDrawing/MeshCache.cs b/DemoRenderer/ShapeDrawing/MeshCache.cs index 6357f9cf1..ab357dae4 100644 --- a/DemoRenderer/ShapeDrawing/MeshCache.cs +++ b/DemoRenderer/ShapeDrawing/MeshCache.cs @@ -56,6 +56,7 @@ public unsafe bool TryGetExistingMesh(ulong id, out int start, out Buffer vertices) { + requestedIds.Add(id, Pool); if (TryGetExistingMesh(id, out start, out vertices)) { return false; From 15547f977952b9cb274a0fc19060a03d60784681 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 11 Feb 2023 18:42:33 -0600 Subject: [PATCH 569/947] Fixed raytesting bugs in capsule and cylinder for rays parallel to the internal line segment and originating inside the shape. Improved RayTesting to capture those cases. --- BepuPhysics/Collidables/Capsule.cs | 28 ++++++++------ BepuPhysics/Collidables/Cylinder.cs | 19 ++++------ Demos/SpecializedTests/RayTesting.cs | 56 +++++++++++++++++++--------- 3 files changed, 63 insertions(+), 40 deletions(-) diff --git a/BepuPhysics/Collidables/Capsule.cs b/BepuPhysics/Collidables/Capsule.cs index a134b7252..38865fdd1 100644 --- a/BepuPhysics/Collidables/Capsule.cs +++ b/BepuPhysics/Collidables/Capsule.cs @@ -69,8 +69,7 @@ public readonly bool RayTest(in RigidPose pose, Vector3 origin, Vector3 directio //Move the origin up to the earliest possible impact time. This isn't necessary for math reasons, but it does help avoid some numerical problems. var tOffset = -Vector3.Dot(o, d) - (HalfLength + Radius); - if (tOffset < 0) - tOffset = 0; + tOffset = float.Max(0, tOffset); o += d * tOffset; var oh = new Vector3(o.X, 0, o.Z); var dh = new Vector3(d.X, 0, d.Z); @@ -97,7 +96,7 @@ public readonly bool RayTest(in RigidPose pose, Vector3 origin, Vector3 directio normal = new Vector3(); return false; } - t = (-b - (float)Math.Sqrt(discriminant)) / a; + t = (-b - MathF.Sqrt(discriminant)) / a; if (t < -tOffset) t = -tOffset; var cylinderHitLocation = o + d * t; @@ -121,7 +120,11 @@ public readonly bool RayTest(in RigidPose pose, Vector3 origin, Vector3 directio else { //The ray is parallel to the axis; the impact is on a spherical cap or nothing. - sphereY = d.Y > 0 ? -HalfLength : HalfLength; + //Note that the sphere cap is nudged forward to match the origin of the ray. + //This is just a simple way to capture the case where the ray starts inside the capsule, but too far to up/down to hit the cap chosen by d.Y. + sphereY = d.Y > 0 ? + float.Max(float.Min(HalfLength, o.Y), -HalfLength) : + float.Min(float.Max(-HalfLength, o.Y), HalfLength); } var os = o - new Vector3(0, sphereY, 0); @@ -144,9 +147,8 @@ public readonly bool RayTest(in RigidPose pose, Vector3 origin, Vector3 directio normal = new Vector3(); return false; } - t = -capB - (float)Math.Sqrt(capDiscriminant); - if (t < -tOffset) - t = -tOffset; + t = -capB - MathF.Sqrt(capDiscriminant); + t = float.Max(t, -tOffset); normal = (os + d * t) / Radius; t = (t + tOffset) * inverseDLength; Matrix3x3.Transform(normal, orientation, out normal); @@ -284,10 +286,14 @@ public void RayTest(ref RigidPoseWide pose, ref RayWide ray, out Vector int var useCylinder = Vector.BitwiseAnd(Vector.GreaterThanOrEqual(cylinderHitLocation.Y, -HalfLength), Vector.LessThanOrEqual(cylinderHitLocation.Y, HalfLength)); //Intersect the spherical cap for any lane which ended up not using the cylinder. - Vector sphereY = Vector.ConditionalSelect( - Vector.BitwiseOr( - Vector.BitwiseAnd(Vector.GreaterThan(cylinderHitLocation.Y, HalfLength), rayIsntParallel), - Vector.AndNot(Vector.LessThanOrEqual(d.Y, Vector.Zero), rayIsntParallel)), HalfLength, -HalfLength); + //Note that the sphere cap is nudged forward in the parallel case to match the origin of the ray. + //This is just a simple way to capture the case where the ray starts inside the capsule, but too far to up/down to hit the cap chosen by d.Y. + var negatedHalfLength = -HalfLength; + var parallelSphereY = Vector.ConditionalSelect(Vector.LessThan(d.Y, Vector.Zero), + Vector.Max(negatedHalfLength, Vector.Min(o.Y, HalfLength)), + Vector.Min(HalfLength, Vector.Max(o.Y, negatedHalfLength))); + var nonParallelSphereY = Vector.ConditionalSelect(Vector.GreaterThan(cylinderHitLocation.Y, HalfLength), HalfLength, negatedHalfLength); + Vector sphereY = Vector.ConditionalSelect(rayIsntParallel, nonParallelSphereY, parallelSphereY); o.Y -= sphereY; Vector3Wide.Dot(o, d, out var capB); diff --git a/BepuPhysics/Collidables/Cylinder.cs b/BepuPhysics/Collidables/Cylinder.cs index 9b357fa59..6dee60c4c 100644 --- a/BepuPhysics/Collidables/Cylinder.cs +++ b/BepuPhysics/Collidables/Cylinder.cs @@ -86,9 +86,7 @@ public readonly bool RayTest(in RigidPose pose, Vector3 origin, Vector3 directio d *= inverseDLength; //Move the origin up to the earliest possible impact time. This isn't necessary for math reasons, but it does help avoid some numerical problems. - var tOffset = -Vector3.Dot(o, d) - (HalfLength + Radius); - if (tOffset < 0) - tOffset = 0; + var tOffset = float.Max(0, -Vector3.Dot(o, d) - (HalfLength + Radius)); o += d * tOffset; var oh = new Vector3(o.X, 0, o.Z); var dh = new Vector3(d.X, 0, d.Z); @@ -115,9 +113,8 @@ public readonly bool RayTest(in RigidPose pose, Vector3 origin, Vector3 directio normal = new Vector3(); return false; } - t = (-b - (float)Math.Sqrt(discriminant)) / a; - if (t < -tOffset) - t = -tOffset; + t = (-b - MathF.Sqrt(discriminant)) / a; + t = float.Max(t, -tOffset); var cylinderHitLocation = o + d * t; if (cylinderHitLocation.Y < -HalfLength) { @@ -144,9 +141,9 @@ public readonly bool RayTest(in RigidPose pose, Vector3 origin, Vector3 directio //Intersect the ray with the plane anchored at discY with normal equal to (0,1,0). //t = dot(rayOrigin - (0,discY,0), (0,1,0)) / dot(rayDirection, (0,1,0) - if (o.Y * d.Y >= 0) + if (float.Abs(o.Y) > HalfLength && o.Y * d.Y >= 0) { - //The ray can only hit the disc if the direction points toward the cylinder. + //The ray can only hit the disc if the ray is inside the cylinder or the direction points toward the cylinder. t = 0; normal = new Vector3(); return false; @@ -291,15 +288,15 @@ public void RayTest(ref RigidPoseWide pose, ref RayWide ray, out Vector int //Intersect the ray with the plane anchored at discY with normal equal to (0,1,0). //t = dot(rayOrigin - (0,discY,0), (0,1,0)) / dot(rayDirection, (0,1,0) - //The ray can only hit the disc if the direction points toward the cylinder. - var rayPointsTowardDisc = Vector.LessThan(o.Y * d.Y, Vector.Zero); + //The ray can only hit the disc if the ray is inside the cylinder or the direction points toward the cylinder. + var withinDiscsOrRayPointsTowardDisc = Vector.BitwiseOr(Vector.LessThanOrEqual(Vector.Abs(o.Y), HalfLength), Vector.LessThan(o.Y * d.Y, Vector.Zero)); var capT = (discY - o.Y) / d.Y; var hitLocationX = o.X + d.X * capT; var hitLocationZ = o.Z + d.Z * capT; var capHitWithinRadius = Vector.LessThanOrEqual(hitLocationX * hitLocationX + hitLocationZ * hitLocationZ, radiusSquared); - var hitCap = Vector.BitwiseAnd(rayPointsTowardDisc, capHitWithinRadius); + var hitCap = Vector.BitwiseAnd(withinDiscsOrRayPointsTowardDisc, capHitWithinRadius); t = (tOffset + Vector.ConditionalSelect(useCylinder, cylinderT, Vector.ConditionalSelect(hitCap, capT, Vector.Zero))) * inverseDLength; var capUsesUpwardFacingNormal = Vector.LessThan(d.Y, Vector.Zero); diff --git a/Demos/SpecializedTests/RayTesting.cs b/Demos/SpecializedTests/RayTesting.cs index 43423d2fb..a3f4ad173 100644 --- a/Demos/SpecializedTests/RayTesting.cs +++ b/Demos/SpecializedTests/RayTesting.cs @@ -263,29 +263,49 @@ public static class RayTesting { internal static void GetUnitDirection(Random random, out Vector3 direction) { - //Not much cleverness involved here. This does not produce a uniform distribution over the the unit sphere. - float length; - do + var directionSelector = random.NextSingle(); + //Occasionally choose to use an axis-aligned direction. These are often special cases that could fail. + const float axisAlignedProbability = 0.2f; + if (directionSelector < axisAlignedProbability / 3) + direction = new Vector3(random.NextSingle() < 0.5f ? -1 : 1, 0, 0); + else if (directionSelector < axisAlignedProbability * 2 / 3) + direction = new Vector3(0, random.NextSingle() < 0.5f ? -1 : 1, 0); + else if (directionSelector < axisAlignedProbability) + direction = new Vector3(0, 0, random.NextSingle() < 0.5f ? -1 : 1); + else { - direction = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) * new Vector3(2) - new Vector3(1); - length = direction.Length(); - } while (length < 1e-7f); - direction /= length; + //Not much cleverness involved here. This does not produce a uniform distribution over the the unit sphere. + float length; + do + { + direction = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) * new Vector3(2) - new Vector3(1); + length = direction.Length(); + } while (length < 1e-7f); + direction /= length; + } } static void GetUnitQuaternion(Random random, out Quaternion orientation) { - //Not much cleverness involved here. This does not produce a uniform distribution over the the unit sphere. - float length; - do + var identitySelector = random.NextSingle(); + if (identitySelector < 0.5) { - orientation = new Quaternion( - random.NextSingle() * 2 - 1, - random.NextSingle() * 2 - 1, - random.NextSingle() * 2 - 1, - random.NextSingle() * 2 - 1); - length = orientation.Length(); - } while (length < 1e-7f); - Unsafe.As(ref orientation) /= length; + //Combined with choosing ray directions that are often axis-aligned, identity orientation can help reveal special case failures. + orientation = Quaternion.Identity; + } + else + { + float length; + do + { + orientation = new Quaternion( + random.NextSingle() * 2 - 1, + random.NextSingle() * 2 - 1, + random.NextSingle() * 2 - 1, + random.NextSingle() * 2 - 1); + length = orientation.Length(); + } while (length < 1e-7f); + Unsafe.As(ref orientation) /= length; + } } static void GetPointOnPlane(Random random, float centralExclusion, float span, ref Vector3 anchor, ref Vector3 normal, out Vector3 point) { From c20484e2885bab438d40d8262295621cc1e62943 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 11 Feb 2023 18:43:09 -0600 Subject: [PATCH 570/947] Version bump. --- BepuPhysics/BepuPhysics.csproj | 2 +- BepuUtilities/BepuUtilities.csproj | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/BepuPhysics/BepuPhysics.csproj b/BepuPhysics/BepuPhysics.csproj index 753dafe0c..d868fd070 100644 --- a/BepuPhysics/BepuPhysics.csproj +++ b/BepuPhysics/BepuPhysics.csproj @@ -1,7 +1,7 @@  net7.0 - 2.5.0-beta.10 + 2.5.0-beta.11 Bepu Entertainment LLC Ross Nordby Speedy real time physics simulation library. diff --git a/BepuUtilities/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj index 01e197cd6..80ff901c7 100644 --- a/BepuUtilities/BepuUtilities.csproj +++ b/BepuUtilities/BepuUtilities.csproj @@ -3,7 +3,7 @@ BepuUtilities BepuUtilities net7.0 - 2.5.0-beta.10 + 2.5.0-beta.11 Bepu Entertainment LLC Ross Nordby Supporting utilities library for BEPUphysics v2. From 25ec18243cb1858149b2c42afb2c086882e45d8b Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 16 Feb 2023 23:21:37 -0600 Subject: [PATCH 571/947] Initial slightly stinky CollisionTrackingDemo. --- Demos/DemoSet.cs | 1 + Demos/Demos/CollisionTrackingDemo.cs | 530 +++++++++++++++++++++++++++ 2 files changed, 531 insertions(+) create mode 100644 Demos/Demos/CollisionTrackingDemo.cs diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index f40e1dc29..94637674e 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -69,6 +69,7 @@ public DemoSet() AddOption(); AddOption(); AddOption(); + AddOption(); AddOption(); AddOption(); AddOption(); diff --git a/Demos/Demos/CollisionTrackingDemo.cs b/Demos/Demos/CollisionTrackingDemo.cs new file mode 100644 index 000000000..cf72bc4a5 --- /dev/null +++ b/Demos/Demos/CollisionTrackingDemo.cs @@ -0,0 +1,530 @@ +using System; +using System.Numerics; +using BepuUtilities; +using BepuPhysics; +using DemoContentLoader; +using DemoRenderer; +using BepuPhysics.Collidables; +using DemoUtilities; +using DemoRenderer.UI; +using BepuPhysics.CollisionDetection; +using BepuUtilities.Collections; +using BepuUtilities.Memory; +using System.Runtime.CompilerServices; +using BepuPhysics.Constraints; +using System.Diagnostics; +using System.Runtime.InteropServices; + +namespace Demos.Demos; + +/// +/// Shows how to store out collision information provided in for later analysis, +/// and how to use it to implement something like collision events without imposing the weird control flow that events imply. +/// +/// +/// This implementation focuses more on simplicity than performance. +/// It's not too slow, but if you run into bottlenecks, be advised that you can make it faster! +/// +public class CollisionTrackingDemo : Demo +{ + /// + /// Refers to a collidable, or a child within a collidable. + /// + /// This contact tracker allows the children of compounds to be tracked, so it's not enough to only store a . + /// That would only permit top-level pair analysis. + public struct ContactSource : IEqualityComparerRef, IEquatable + { + /// + /// Collidable associated with this listener. + /// + public CollidableReference Collidable; + /// + /// Child index within the collidable associated with this listener, if any. -1 if the listener is associated with the entire collidable. + /// + public int ChildIndex; + + public bool Equals(ref ContactSource a, ref ContactSource b) => a.Collidable == b.Collidable && a.ChildIndex == b.ChildIndex; + public int Hash(ref ContactSource item) => (int)(item.Collidable.Packed ^ uint.RotateLeft((uint)item.ChildIndex, 16)); + public bool Equals(ContactSource other) => Equals(ref this, ref other); + public override bool Equals(object obj) => obj is ContactSource source && Equals(source); + public override int GetHashCode() => Hash(ref this); + public static bool operator ==(ContactSource left, ContactSource right) => left.Equals(right); + public static bool operator !=(ContactSource left, ContactSource right) => !(left == right); + + public static implicit operator ContactSource(CollidableReference collidable) => new() { Collidable = collidable, ChildIndex = -1 }; + } + + /// + /// Stores contacts generated by the narrow phase for a pair. + /// + [StructLayout(LayoutKind.Explicit)] + public struct PairCollision + { + /// + /// The other collidable (and if applicable, child) associated with this pair. + /// + [FieldOffset(0)] + public ContactSource Other; + /// + /// Stores whether the contact manifold held in this instance is convex or nonconvex. + /// If this is true, then only is populated with valid values and contains undefined values. + /// If this is false, then only is populated with valid values and contains undefined values. + /// + [FieldOffset(8)] + public bool IsConvex; + /// + /// Stores whether is stored in the first slot of the collision pair. True if it is, false if it isn't. + /// This matters for interpreting the contact data: by convention, the contact offset is relative to collidable A, and the normal points from B to A. + /// + [FieldOffset(9)] + public bool OtherIsPairA; + /// + /// Contacts generated by the narrow phase if is true. Otherwise, contains undefined values. + /// + [FieldOffset(12)] + public ConvexContactManifold Convex; + /// + /// Contacts generated by the narrow phase if is false. Otherwise, contains undefined values. + /// + [FieldOffset(12)] + public NonconvexContactManifold Nonconvex; + } + + + + /// + /// Tracks collisions for a set of collidables (or their children). + /// + public class CollisionTracker + { + IThreadDispatcher dispatcher; + BufferPool pool; + + public CollisionTracker(BufferPool pool, IThreadDispatcher dispatcher) + { + this.pool = pool; + this.dispatcher = dispatcher; + Tracked = new QuickDictionary(16, pool); + } + + public struct TrackedContacts + { + /// + /// Holds all contacts reported about a tracked object by narrow phase callbacks for the timestep before the most recent timestep. + /// + public QuickList PreviousCollisions; + /// + /// Holds all contacts reported about a tracked object by narrow phase callbacks for the most recent timestep. + /// + public QuickList Collisions; + + public TrackedContacts(BufferPool pool) + { + //For simplicity, we'll initialize all the contact storage with a fixed size. It'll grow over time if more collisions per tracked object are detected. + //Some kind of compaction would be easy to add- just iterate over all tracked objects and call Compact on these lists. + //(Or just part of them, incrementally! Probably better to avoid excess resizes!) + Collisions = new QuickList(8, pool); + PreviousCollisions = new QuickList(8, pool); + } + + public void Dispose(BufferPool pool) + { + PreviousCollisions.Dispose(pool); + Collisions.Dispose(pool); + } + } + + /// + /// Maps tracked objects to the contacts associated with them. + /// + public QuickDictionary Tracked; + + /// + /// Adds a collidable (or its child) to the collision tracker. Collisions associated with it will be included for queries. + /// + /// Collidable (or child) to track. + /// Triggered if the collidable or child is already present. + public void Track(ContactSource target) + { + if (Tracked.ContainsKey(target)) + throw new ArgumentException("Object already tracked."); + Tracked.Add(target, new TrackedContacts(pool), pool); + } + + /// + /// Removes a listener from the tracker. + /// + /// Collidable (or child) to untrack. + /// Triggered if the collidable or child is not present. + public void Untrack(ContactSource target) + { + if (!Tracked.ContainsKey(target)) + throw new ArgumentException("Object is not tracked."); + Tracked.GetTableIndices(ref target, out var tableIndex, out var elementIndex); + ref var contacts = ref Tracked.Values[elementIndex]; + contacts.PreviousCollisions.Dispose(pool); + contacts.Collisions.Dispose(pool); + Tracked.FastRemove(tableIndex, elementIndex); + } + + //The INarrowPhaseCallbacks are invoked from multiple threads and are performance sensitive. + //We want to do as little as possible, so rather than locking and trying to modify the final storage directly, + //just create per-worker caches that get flushed at the end. + + /// + /// Stores a single tracked result in a worker cache to later be flushed. + /// + struct WorkerPairContacts + { + public ContactSource Self; + public PairCollision Contacts; + } + + /// + /// Stores contacts for listeners reported by callbacks to a particular thread. + /// + Buffer> workerCaches; + + public void PrepareForNextTimestep() + { + workerCaches = new Buffer>(dispatcher.ThreadCount, pool); + for (int i = 0; i < workerCaches.Length; ++i) + { + workerCaches[i] = new QuickList(512, dispatcher.GetThreadMemoryPool(i)); + } + } + + /// + /// Flushes all collisions found in the previous timestep into efficient storage for queries. + /// + public void Flush() + { + //Flip the caches for each listener and clear out the now-current set for accumulation. + for (int i = 0; i < Tracked.Count; ++i) + { + ref var listenerContacts = ref Tracked.Values[i]; + BepuPhysics.Helpers.Swap(ref listenerContacts.PreviousCollisions, ref listenerContacts.Collisions); + listenerContacts.Collisions.Count = 0; + } + + //Flush the worker caches into the main storage and dispose the caches. + for (int i = 0; i < workerCaches.Length; ++i) + { + ref var cache = ref workerCaches[i]; + for (int j = 0; j < cache.Count; ++j) + { + ref var entry = ref cache[j]; + Tracked.GetTableIndices(ref entry.Self, out _, out var elementIndex); + Tracked.Values[elementIndex].Collisions.Add(entry.Contacts, pool); + } + //The worker cache memory must be returned to the thread pool, not the main pool! + cache.Dispose(dispatcher.GetThreadMemoryPool(i)); + } + workerCaches.Dispose(pool); + } + + private void ReportContacts(int workerIndex, ContactSource self, ContactSource other, bool otherIsA, ref TManifold contacts) where TManifold : unmanaged, IContactManifold + { + if (Tracked.ContainsKey(self)) + { + //A is a listener, add it. + ref var pairContacts = ref workerCaches[workerIndex].Allocate(dispatcher.GetThreadMemoryPool(workerIndex)); + pairContacts.Self = self; + pairContacts.Contacts.Other = other; + pairContacts.Contacts.OtherIsPairA = otherIsA; + if (typeof(TManifold) == typeof(ConvexContactManifold)) //This is a JIT-time constant. + { + pairContacts.Contacts.IsConvex = true; + pairContacts.Contacts.Convex = Unsafe.As(ref contacts); + } + else + { + pairContacts.Contacts.IsConvex = false; + pairContacts.Contacts.Nonconvex = Unsafe.As(ref contacts); + } + } + } + + /// + /// Notifies the tracker of a collision between collidable children. + /// + /// First collidable in the pair. + /// Child index associated with the first collidable in the pair. + /// Second collidable in the pair. + /// Child index associated with the second collidable in the pair. + /// Index of the worker invoking this callback. + /// Contacts reported in the callback. + public void ReportChildContacts(CollidableReference collidableA, int childIndexA, CollidableReference collidableB, int childIndexB, int workerIndex, ref ConvexContactManifold contacts) + { + Debug.Assert(workerCaches.Allocated, "The worker caches must be allocated in order to report contacts. Make sure PrepareForNextTimestep was called."); + var a = new ContactSource { Collidable = collidableA, ChildIndex = childIndexA }; + var b = new ContactSource { Collidable = collidableB, ChildIndex = childIndexB }; + ReportContacts(workerIndex, a, b, false, ref contacts); + ReportContacts(workerIndex, b, a, true, ref contacts); + } + + + /// + /// Notifies the tracker of a collision between collidables. + /// + /// Type of the manifold being reported. + /// First collidable in the pair. + /// Second collidable in the pair. + /// Index of the worker invoking this callback. + /// Contacts reported in the callback. + public void ReportContacts(CollidableReference collidableA, CollidableReference collidableB, int workerIndex, ref TManifold contacts) where TManifold : unmanaged, IContactManifold + { + Debug.Assert(workerCaches.Allocated, "The worker caches must be allocated in order to report contacts. Make sure PrepareForNextTimestep was called."); + Debug.Assert(workerCaches.Allocated, "The worker caches must be allocated in order to report contacts. Make sure PrepareForNextTimestep was called."); + var a = new ContactSource { Collidable = collidableA, ChildIndex = -1 }; + var b = new ContactSource { Collidable = collidableB, ChildIndex = -1 }; + ReportContacts(workerIndex, a, b, false, ref contacts); + ReportContacts(workerIndex, b, a, true, ref contacts); + } + + } + + /// + /// Callbacks invoked by the simulation's narrow phase. + /// In this demo, we'll collect all contact data associated with tracked objects for later processing. + /// + public unsafe struct CollisionTrackingCallbacks : INarrowPhaseCallbacks + { + CollisionTracker collisionTracker; + public CollisionTrackingCallbacks(CollisionTracker collisionTracker) + { + this.collisionTracker = collisionTracker; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin) + { + return true; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB) + { + return true; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public unsafe bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold + { + pairMaterial.FrictionCoefficient = 1f; + pairMaterial.MaximumRecoveryVelocity = 2f; + pairMaterial.SpringSettings = new SpringSettings(30, 1); + collisionTracker.ReportContacts(pair.A, pair.B, workerIndex, ref manifold); + return true; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold) + { + collisionTracker.ReportChildContacts(pair.A, childIndexA, pair.B, childIndexB, workerIndex, ref manifold); + return true; + } + + public void Initialize(Simulation simulation) + { + } + + public void Dispose() + { + } + + } + + /// + /// Tracks a particle created by a collision. + /// + struct ContactResponseParticle + { + public Vector3 Position; + public float Age; + public Vector3 Normal; + } + QuickList particles; + + CollisionTracker collisionTracker; + + + public override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(0, 8, -20); + camera.Yaw = MathHelper.Pi; + + particles = new QuickList(8, BufferPool); + + collisionTracker = new CollisionTracker(BufferPool, ThreadDispatcher); + Simulation = Simulation.Create(BufferPool, new CollisionTrackingCallbacks(collisionTracker), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); + + + var listenedBody1 = Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 5, 0), 1, Simulation.Shapes, new Box(1, 2, 3))); + collisionTracker.Track(Simulation.Bodies[listenedBody1].CollidableReference); + + var listenedBody2 = Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0.5f, 10, 0), 1, Simulation.Shapes, new Capsule(0.25f, 0.7f))); + collisionTracker.Track(Simulation.Bodies[listenedBody2].CollidableReference); + + + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), Simulation.Shapes.Add(new Box(30, 1, 30)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 3, 15), Simulation.Shapes.Add(new Box(30, 5, 1)))); + } + + static bool IsTouching(ref TManifold contacts) where TManifold : unmanaged, IContactManifold + { + for (int i = 0; i < contacts.Count; ++i) + { + //This may look a bit odd: it's a legacy issue from before static abstracts existed. Expect it to change soon! + if (contacts.GetDepth(ref contacts, i) >= 0) + { + return true; + } + } + return false; + } + + static bool IsTouching(ref PairCollision collision) + { + return collision.IsConvex ? IsTouching(ref collision.Convex) : IsTouching(ref collision.Nonconvex); + } + + void AddParticle(Vector3 contactOffset, Vector3 contactNormal, CollidableReference firstCollidableInPair) + { + ref var particle = ref particles.Allocate(BufferPool); + + //Contact data is calibrated according to the order of the pair, so using A's position is important. + particle.Position = contactOffset + (firstCollidableInPair.Mobility == CollidableMobility.Static ? + new StaticReference(firstCollidableInPair.StaticHandle, Simulation.Statics).Pose.Position : + new BodyReference(firstCollidableInPair.BodyHandle, Simulation.Bodies).Pose.Position); + particle.Age = 0; + particle.Normal = contactNormal; + } + + bool PreviousContainsFeatureId(ref PairCollision pair, int featureId) + { + if (pair.IsConvex) + { + for (int i = 0; i < pair.Convex.Count; ++i) + { + if (pair.Convex.GetFeatureId(i) == featureId) + return true; + } + } + else + { + for (int i = 0; i < pair.Nonconvex.Count; ++i) + { + if (pair.Nonconvex.GetFeatureId(i) == featureId) + return true; + } + } + return false; + } + + public override void Update(Window window, Camera camera, Input input, float dt) + { + //base.Update includes a call to the Simulation.Timestep. We want to prepare the worker caches and flush them after the timestep completes. + //(These could be hooked onto events exposed by the simulation.Timestepper, like the CollisionEvents demo does, but this demo tries to be a little less magic. + collisionTracker.PrepareForNextTimestep(); + base.Update(window, camera, input, dt); + collisionTracker.Flush(); + + //Now analyze the contacts we've collected. + for (int trackedIndex = 0; trackedIndex < collisionTracker.Tracked.Count; ++trackedIndex) + { + //We do a somewhat questionable previous test here. All contacts are stored in lists, so we enumerate all previous collisions. + //For objects with relatively few collisions, this is perfectly fine (and actually near optimal). + //But if a single object has thousands of collisions, this is going to scale pretty poorly. + //The demo doesn't worry about this for simplicity reasons, but you could use a dictionary or other more suitable structure. + ref var others = ref collisionTracker.Tracked.Values[trackedIndex]; + var self = collisionTracker.Tracked.Keys[trackedIndex]; + for (int otherIndex = 0; otherIndex < others.Collisions.Count; ++otherIndex) + { + int indexInPrevious = -1; + ref var other = ref others.Collisions[otherIndex]; + for (int k = 0; k < others.PreviousCollisions.Count; ++k) + { + if (others.PreviousCollisions[k].Other == others.Collisions[otherIndex].Other) + { + indexInPrevious = k; + break; + } + } + //TODO: This is pretty stinky. + if (indexInPrevious >= 0) + { + //There exists a previous collision. + ref var previous = ref others.PreviousCollisions[indexInPrevious]; + if (other.IsConvex) + { + for (int i = 0; i < other.Convex.Count; ++i) + { + if (!PreviousContainsFeatureId(ref previous, other.Convex.GetFeatureId(i))) + AddParticle(other.Convex.GetOffset(ref other.Convex, i), other.Convex.Normal, other.OtherIsPairA ? other.Other.Collidable : self.Collidable); + } + } + else + { + for (int i = 0; i < other.Nonconvex.Count; ++i) + { + if (!PreviousContainsFeatureId(ref previous, other.Convex.GetFeatureId(i))) + AddParticle(other.Nonconvex.GetOffset(ref other.Nonconvex, i), other.Nonconvex.GetNormal(ref other.Nonconvex, i), other.OtherIsPairA ? other.Other.Collidable : self.Collidable); + } + } + } + else + { + //No previous collision, so all contacts are new. + if (other.IsConvex) + { + for (int i = 0; i < other.Convex.Count; ++i) + { + AddParticle(other.Convex.GetOffset(ref other.Convex, i), other.Convex.Normal, other.OtherIsPairA ? other.Other.Collidable : self.Collidable); + } + } + else + { + for (int i = 0; i < other.Nonconvex.Count; ++i) + { + AddParticle(other.Nonconvex.GetOffset(ref other.Nonconvex, i), other.Nonconvex.GetNormal(ref other.Nonconvex, i), other.OtherIsPairA ? other.Other.Collidable : self.Collidable); + } + } + } + } + } + + + //Age and scoot the particles we created for new contacts for the animation. + for (int i = particles.Count - 1; i >= 0; --i) + { + ref var particle = ref particles[i]; + particle.Age += dt; + if (particle.Age > 0.7325f) + { + particles.FastRemoveAt(i); + } + else + { + particle.Position += particle.Normal * (2 * dt); + } + + } + } + + public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + { + for (int i = particles.Count - 1; i >= 0; --i) + { + ref var particle = ref particles[i]; + var radius = particle.Age * (particle.Age * (0.135f - 2.7f * particle.Age) + 1.35f); + var pose = new RigidPose(particle.Position); + renderer.Shapes.AddShape(new Sphere(radius), Simulation.Shapes, pose, new Vector3(0, 1, 0)); + } + + var resolution = renderer.Surface.Resolution; + renderer.TextBatcher.Write(text.Clear().Append("TBA!"), new Vector2(16, resolution.Y - 80), 16, Vector3.One, font); + + base.Render(renderer, camera, input, text, font); + } +} From 2e900c4de72ccba1f6042236284a827f00a09eca Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 17 Feb 2023 15:36:21 -0600 Subject: [PATCH 572/947] Some cleanup; no more analysis-time IsConvex testing. --- Demos/Demos/CollisionTrackingDemo.cs | 107 +++++++++------------------ 1 file changed, 35 insertions(+), 72 deletions(-) diff --git a/Demos/Demos/CollisionTrackingDemo.cs b/Demos/Demos/CollisionTrackingDemo.cs index cf72bc4a5..5034ebd1a 100644 --- a/Demos/Demos/CollisionTrackingDemo.cs +++ b/Demos/Demos/CollisionTrackingDemo.cs @@ -54,6 +54,7 @@ public struct ContactSource : IEqualityComparerRef, IEquatable new() { Collidable = collidable, ChildIndex = -1 }; } + /// /// Stores contacts generated by the narrow phase for a pair. /// @@ -66,28 +67,18 @@ public struct PairCollision [FieldOffset(0)] public ContactSource Other; /// - /// Stores whether the contact manifold held in this instance is convex or nonconvex. - /// If this is true, then only is populated with valid values and contains undefined values. - /// If this is false, then only is populated with valid values and contains undefined values. - /// - [FieldOffset(8)] - public bool IsConvex; - /// /// Stores whether is stored in the first slot of the collision pair. True if it is, false if it isn't. /// This matters for interpreting the contact data: by convention, the contact offset is relative to collidable A, and the normal points from B to A. /// - [FieldOffset(9)] + [FieldOffset(8)] public bool OtherIsPairA; /// - /// Contacts generated by the narrow phase if is true. Otherwise, contains undefined values. - /// - [FieldOffset(12)] - public ConvexContactManifold Convex; - /// - /// Contacts generated by the narrow phase if is false. Otherwise, contains undefined values. + /// Contacts generated by the narrow phase. /// + /// We're making use of the type here because it's able to store both nonconvex manifolds and convex manifolds. + /// Within the callbacks, we convert convex manifolds into nonconvex manifolds by rearranging data; that way, post-analysis doesn't need to worry about the manifold type. [FieldOffset(12)] - public NonconvexContactManifold Nonconvex; + public NonconvexContactManifold Contacts; } @@ -177,7 +168,7 @@ public void Untrack(ContactSource target) struct WorkerPairContacts { public ContactSource Self; - public PairCollision Contacts; + public PairCollision Collision; } /// @@ -215,7 +206,7 @@ public void Flush() { ref var entry = ref cache[j]; Tracked.GetTableIndices(ref entry.Self, out _, out var elementIndex); - Tracked.Values[elementIndex].Collisions.Add(entry.Contacts, pool); + Tracked.Values[elementIndex].Collisions.Add(entry.Collision, pool); } //The worker cache memory must be returned to the thread pool, not the main pool! cache.Dispose(dispatcher.GetThreadMemoryPool(i)); @@ -223,24 +214,31 @@ public void Flush() workerCaches.Dispose(pool); } - private void ReportContacts(int workerIndex, ContactSource self, ContactSource other, bool otherIsA, ref TManifold contacts) where TManifold : unmanaged, IContactManifold + private void ReportContacts(int workerIndex, ContactSource self, ContactSource other, bool otherIsA, ref TContacts contacts) where TContacts : unmanaged, IContactManifold { if (Tracked.ContainsKey(self)) { //A is a listener, add it. ref var pairContacts = ref workerCaches[workerIndex].Allocate(dispatcher.GetThreadMemoryPool(workerIndex)); pairContacts.Self = self; - pairContacts.Contacts.Other = other; - pairContacts.Contacts.OtherIsPairA = otherIsA; - if (typeof(TManifold) == typeof(ConvexContactManifold)) //This is a JIT-time constant. + pairContacts.Collision.Other = other; + pairContacts.Collision.OtherIsPairA = otherIsA; + if (typeof(TContacts) == typeof(ConvexContactManifold)) //This is a JIT-time constant. { - pairContacts.Contacts.IsConvex = true; - pairContacts.Contacts.Convex = Unsafe.As(ref contacts); + //The pairContacts representation just uses a NonconvexContactManifold for simplicity- we can just rearrange convex contacts to fit. + //(Without doing this, the post-analysis would constantly have to check an "IsConvex" flag and so forth. It would be quite stinky.) + ref var convex = ref Unsafe.As(ref contacts); + for (int i = 0; i < contacts.Count; ++i) + { + ref var targetContact = ref Unsafe.Add(ref pairContacts.Collision.Contacts.Contact0, i); + convex.GetContact(i, out targetContact.Offset, out targetContact.Normal, out targetContact.Depth, out targetContact.FeatureId); + } + pairContacts.Collision.Contacts.Count = contacts.Count; + pairContacts.Collision.Contacts.OffsetB = convex.OffsetB; } else { - pairContacts.Contacts.IsConvex = false; - pairContacts.Contacts.Nonconvex = Unsafe.As(ref contacts); + pairContacts.Collision.Contacts = Unsafe.As(ref contacts); } } } @@ -371,7 +369,7 @@ public override void Initialize(ContentArchive content, Camera camera) Simulation.Statics.Add(new StaticDescription(new Vector3(0, 3, 15), Simulation.Shapes.Add(new Box(30, 5, 1)))); } - static bool IsTouching(ref TManifold contacts) where TManifold : unmanaged, IContactManifold + static bool IsTouching(NonconvexContactManifold contacts) { for (int i = 0; i < contacts.Count; ++i) { @@ -384,11 +382,6 @@ static bool IsTouching(ref TManifold contacts) where TManifold : unma return false; } - static bool IsTouching(ref PairCollision collision) - { - return collision.IsConvex ? IsTouching(ref collision.Convex) : IsTouching(ref collision.Nonconvex); - } - void AddParticle(Vector3 contactOffset, Vector3 contactNormal, CollidableReference firstCollidableInPair) { ref var particle = ref particles.Allocate(BufferPool); @@ -401,23 +394,12 @@ void AddParticle(Vector3 contactOffset, Vector3 contactNormal, CollidableReferen particle.Normal = contactNormal; } - bool PreviousContainsFeatureId(ref PairCollision pair, int featureId) + bool PreviousContainsTouchingFeatureId(ref PairCollision pair, int featureId) { - if (pair.IsConvex) + for (int i = 0; i < pair.Contacts.Count; ++i) { - for (int i = 0; i < pair.Convex.Count; ++i) - { - if (pair.Convex.GetFeatureId(i) == featureId) - return true; - } - } - else - { - for (int i = 0; i < pair.Nonconvex.Count; ++i) - { - if (pair.Nonconvex.GetFeatureId(i) == featureId) - return true; - } + if (pair.Contacts.GetFeatureId(i) == featureId && pair.Contacts.GetDepth(ref pair.Contacts, i) >= 0) + return true; } return false; } @@ -451,50 +433,31 @@ public override void Update(Window window, Camera camera, Input input, float dt) break; } } - //TODO: This is pretty stinky. if (indexInPrevious >= 0) { //There exists a previous collision. ref var previous = ref others.PreviousCollisions[indexInPrevious]; - if (other.IsConvex) - { - for (int i = 0; i < other.Convex.Count; ++i) - { - if (!PreviousContainsFeatureId(ref previous, other.Convex.GetFeatureId(i))) - AddParticle(other.Convex.GetOffset(ref other.Convex, i), other.Convex.Normal, other.OtherIsPairA ? other.Other.Collidable : self.Collidable); - } - } - else + for (int i = 0; i < other.Contacts.Count; ++i) { - for (int i = 0; i < other.Nonconvex.Count; ++i) + if (other.Contacts.GetDepth(ref other.Contacts, i) >= 0) { - if (!PreviousContainsFeatureId(ref previous, other.Convex.GetFeatureId(i))) - AddParticle(other.Nonconvex.GetOffset(ref other.Nonconvex, i), other.Nonconvex.GetNormal(ref other.Nonconvex, i), other.OtherIsPairA ? other.Other.Collidable : self.Collidable); + //This contact is touching. Does there exist a contact with the same feature id that was touching in the previous timestep? + if (!PreviousContainsTouchingFeatureId(ref previous, other.Contacts.GetFeatureId(i))) + AddParticle(other.Contacts.GetOffset(ref other.Contacts, i), other.Contacts.GetNormal(ref other.Contacts, i), other.OtherIsPairA ? other.Other.Collidable : self.Collidable); } } } else { //No previous collision, so all contacts are new. - if (other.IsConvex) + for (int i = 0; i < other.Contacts.Count; ++i) { - for (int i = 0; i < other.Convex.Count; ++i) - { - AddParticle(other.Convex.GetOffset(ref other.Convex, i), other.Convex.Normal, other.OtherIsPairA ? other.Other.Collidable : self.Collidable); - } - } - else - { - for (int i = 0; i < other.Nonconvex.Count; ++i) - { - AddParticle(other.Nonconvex.GetOffset(ref other.Nonconvex, i), other.Nonconvex.GetNormal(ref other.Nonconvex, i), other.OtherIsPairA ? other.Other.Collidable : self.Collidable); - } + AddParticle(other.Contacts.GetOffset(ref other.Contacts, i), other.Contacts.GetNormal(ref other.Contacts, i), other.OtherIsPairA ? other.Other.Collidable : self.Collidable); } } } } - //Age and scoot the particles we created for new contacts for the animation. for (int i = particles.Count - 1; i >= 0; --i) { From e9852ee1bcdc73bf89c83b1d377e5e49e7400a23 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 17 Feb 2023 15:36:36 -0600 Subject: [PATCH 573/947] SolverContactEnumerationDemo now contains its relevant types to protect the namespace a bit. --- Demos/Demos/SolverContactEnumerationDemo.cs | 168 ++++++++++---------- 1 file changed, 84 insertions(+), 84 deletions(-) diff --git a/Demos/Demos/SolverContactEnumerationDemo.cs b/Demos/Demos/SolverContactEnumerationDemo.cs index 7fd8729f3..45d325354 100644 --- a/Demos/Demos/SolverContactEnumerationDemo.cs +++ b/Demos/Demos/SolverContactEnumerationDemo.cs @@ -13,7 +13,13 @@ using System; using System.Numerics; -namespace Demos.Demos +namespace Demos.Demos; + + +/// +/// Shows how to enumerate contact constraints in the solver using contact accessors and . +/// +public class SolverContactEnumerationDemo : Demo { /// /// Stores the easy to read data associated with a single contact extracted from solver contact constraints. @@ -186,104 +192,98 @@ public void Dispose() } } - /// - /// Shows how to enumerate contact constraints in the solver using contact accessors and ISolverContactDataExtractor. - /// - public class SolverContactEnumerationDemo : Demo + BodyHandle sensorBodyHandle; + public override void Initialize(ContentArchive content, Camera camera) { - BodyHandle sensorBodyHandle; - public override void Initialize(ContentArchive content, Camera camera) - { - camera.Position = new Vector3(0, 5, 25); - camera.Yaw = 0; - camera.Pitch = 0; + camera.Position = new Vector3(0, 5, 25); + camera.Yaw = 0; + camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); - //Drop a pyramid on top of the sensor so there are more contacts to look at. - var boxShape = new Box(1, 1, 1); - var boxInertia = boxShape.ComputeInertia(1); - var boxIndex = Simulation.Shapes.Add(boxShape); + //Drop a pyramid on top of the sensor so there are more contacts to look at. + var boxShape = new Box(1, 1, 1); + var boxInertia = boxShape.ComputeInertia(1); + var boxIndex = Simulation.Shapes.Add(boxShape); - const int rowCount = 20; - for (int rowIndex = 0; rowIndex < rowCount; ++rowIndex) + const int rowCount = 20; + for (int rowIndex = 0; rowIndex < rowCount; ++rowIndex) + { + int columnCount = rowCount - rowIndex; + for (int columnIndex = 0; columnIndex < columnCount; ++columnIndex) { - int columnCount = rowCount - rowIndex; - for (int columnIndex = 0; columnIndex < columnCount; ++columnIndex) - { - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3( - (-columnCount * 0.5f + columnIndex) * boxShape.Width, - (rowIndex + 0.5f) * boxShape.Height + 10, 0), - boxInertia, boxIndex, 0.01f)); - } + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3( + (-columnCount * 0.5f + columnIndex) * boxShape.Width, + (rowIndex + 0.5f) * boxShape.Height + 10, 0), + boxInertia, boxIndex, 0.01f)); } + } - sensorBodyHandle = Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 0, 1), 10, Simulation.Shapes, new Box(4, 2, 6))); + sensorBodyHandle = Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 0, 1), 10, Simulation.Shapes, new Box(4, 2, 6))); - //Put a mesh under the sensor so that nonconvex contacts are shown. - const int planeWidth = 128; - const int planeHeight = 128; - DemoMeshHelper.CreateDeformedPlane(planeWidth, planeHeight, - (int x, int y) => - { - return new Vector3(x - planeWidth / 2, 1 * MathF.Cos(x / 2f) * MathF.Sin(y / 2f), y - planeHeight / 2); - }, new Vector3(2, 1, 2), BufferPool, out var planeMesh); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -2, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), Simulation.Shapes.Add(planeMesh))); - } + //Put a mesh under the sensor so that nonconvex contacts are shown. + const int planeWidth = 128; + const int planeHeight = 128; + DemoMeshHelper.CreateDeformedPlane(planeWidth, planeHeight, + (int x, int y) => + { + return new Vector3(x - planeWidth / 2, 1 * MathF.Cos(x / 2f) * MathF.Sin(y / 2f), y - planeHeight / 2); + }, new Vector3(2, 1, 2), BufferPool, out var planeMesh); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -2, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), Simulation.Shapes.Add(planeMesh))); + } - public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + { + var sensorBody = Simulation.Bodies[sensorBodyHandle]; + var extractor = new SolverContactDataExtractor(BufferPool, sensorBody.Constraints.Count); + //The basic idea behind the contact extractor is to submit it to a narrow phase contact accessor that is able to understand the solver's layout, + //which will then call the contact extractor's relevant callbacks for the type of constraint encountered. + //Here, we'll enumerate over all the constraints currently affecting the sensor body, attempting to extract contact data from each one. + //If there are constraints that aren't contact constraints, they'll just get skipped. + for (int i = 0; i < sensorBody.Constraints.Count; ++i) { - var sensorBody = Simulation.Bodies[sensorBodyHandle]; - var extractor = new SolverContactDataExtractor(BufferPool, sensorBody.Constraints.Count); - //The basic idea behind the contact extractor is to submit it to a narrow phase contact accessor that is able to understand the solver's layout, - //which will then call the contact extractor's relevant callbacks for the type of constraint encountered. - //Here, we'll enumerate over all the constraints currently affecting the sensor body, attempting to extract contact data from each one. - //If there are constraints that aren't contact constraints, they'll just get skipped. - for (int i = 0; i < sensorBody.Constraints.Count; ++i) - { - Simulation.NarrowPhase.TryExtractSolverContactData(sensorBody.Constraints[i].ConnectingConstraintHandle, ref extractor); - } - //We now have extracted contact data. Let's analyze it! - //For the purposes of the demo, we'll draw debug shapes at the contacts to represent the different properties. - int sensorContactCount = 0; - for (int manifoldIndex = 0; manifoldIndex < extractor.Constraints.Count; ++manifoldIndex) + Simulation.NarrowPhase.TryExtractSolverContactData(sensorBody.Constraints[i].ConnectingConstraintHandle, ref extractor); + } + //We now have extracted contact data. Let's analyze it! + //For the purposes of the demo, we'll draw debug shapes at the contacts to represent the different properties. + int sensorContactCount = 0; + for (int manifoldIndex = 0; manifoldIndex < extractor.Constraints.Count; ++manifoldIndex) + { + ref var constraintContacts = ref extractor.Constraints[manifoldIndex]; + var bodyA = Simulation.Bodies[constraintContacts.BodyA]; + sensorContactCount += constraintContacts.Contacts.Count; + for (int contactIndex = 0; contactIndex < constraintContacts.Contacts.Count; ++contactIndex) { - ref var constraintContacts = ref extractor.Constraints[manifoldIndex]; - var bodyA = Simulation.Bodies[constraintContacts.BodyA]; - sensorContactCount += constraintContacts.Contacts.Count; - for (int contactIndex = 0; contactIndex < constraintContacts.Contacts.Count; ++contactIndex) - { - ref var contact = ref constraintContacts.Contacts[contactIndex]; - var contactPosition = contact.OffsetA + bodyA.Pose.Position; - Matrix3x3 basisPose; - //We want to visualize both friction and penetration impulses, so a cylinder is a good choice- radius and length. - //The length will be oriented along the contact normal, while the radius will expand along the tangent directions. - BepuPhysics.Helpers.BuildOrthonormalBasis(contact.Normal, out basisPose.X, out basisPose.Z); - basisPose.Y = contact.Normal; - var baseLength = 0.2f; - var baseRadius = 0.1f; - //We'll make purely speculative contacts (negative depth) a fixed minimum size and a different color. - var contactVisualShape = contact.Depth < 0 ? - new Cylinder(baseRadius, baseLength) : - new Cylinder(baseRadius + MathF.Min(5, contact.FrictionImpulseMagnitude * 0.1f), baseLength + MathF.Min(5, contact.PenetrationImpulse * 0.3f)); - RigidPose contactVisualPose; - contactVisualPose.Position = contactPosition + contact.Normal * contactVisualShape.HalfLength; - QuaternionEx.CreateFromRotationMatrix(basisPose, out contactVisualPose.Orientation); - renderer.Shapes.AddShape(contactVisualShape, Simulation.Shapes, contactVisualPose, contact.Depth < 0 ? new Vector3(0, 0, 1) : new Vector3(0, 1, 0)); - } + ref var contact = ref constraintContacts.Contacts[contactIndex]; + var contactPosition = contact.OffsetA + bodyA.Pose.Position; + Matrix3x3 basisPose; + //We want to visualize both friction and penetration impulses, so a cylinder is a good choice- radius and length. + //The length will be oriented along the contact normal, while the radius will expand along the tangent directions. + BepuPhysics.Helpers.BuildOrthonormalBasis(contact.Normal, out basisPose.X, out basisPose.Z); + basisPose.Y = contact.Normal; + var baseLength = 0.2f; + var baseRadius = 0.1f; + //We'll make purely speculative contacts (negative depth) a fixed minimum size and a different color. + var contactVisualShape = contact.Depth < 0 ? + new Cylinder(baseRadius, baseLength) : + new Cylinder(baseRadius + MathF.Min(5, contact.FrictionImpulseMagnitude * 0.1f), baseLength + MathF.Min(5, contact.PenetrationImpulse * 0.3f)); + RigidPose contactVisualPose; + contactVisualPose.Position = contactPosition + contact.Normal * contactVisualShape.HalfLength; + QuaternionEx.CreateFromRotationMatrix(basisPose, out contactVisualPose.Orientation); + renderer.Shapes.AddShape(contactVisualShape, Simulation.Shapes, contactVisualPose, contact.Depth < 0 ? new Vector3(0, 0, 1) : new Vector3(0, 1, 0)); } + } - extractor.Dispose(); + extractor.Dispose(); - var resolution = renderer.Surface.Resolution; - renderer.TextBatcher.Write(text.Clear().Append("The solver stores data in an optimized array-of-structures-of-arrays format that makes it difficult to directly read."), new Vector2(16, resolution.Y - 96), 16, Vector3.One, font); - renderer.TextBatcher.Write(text.Clear().Append("This demo implements an ISolverContactDataExtractor that pulls data out of the solver and puts it into a simpler format."), new Vector2(16, resolution.Y - 80), 16, Vector3.One, font); - renderer.TextBatcher.Write(text.Clear().Append("Pulling data this way can sometimes be more convenient than tracking contacts from INarrowPhaseCallbacks."), new Vector2(16, resolution.Y - 64), 16, Vector3.One, font); - renderer.TextBatcher.Write(text.Clear().Append("Contacts associated with the large box are visualized. The size of a contact corresponds to the contact's force."), new Vector2(16, resolution.Y - 48), 16, Vector3.One, font); - renderer.TextBatcher.Write(text.Clear().Append("Sensor manifold constraint count: ").Append(extractor.Constraints.Count).Append(", contact count: ").Append(sensorContactCount), new Vector2(16, resolution.Y - 20), 20, Vector3.One, font); - base.Render(renderer, camera, input, text, font); - } + var resolution = renderer.Surface.Resolution; + renderer.TextBatcher.Write(text.Clear().Append("The solver stores data in an optimized array-of-structures-of-arrays format that makes it difficult to directly read."), new Vector2(16, resolution.Y - 96), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("This demo implements an ISolverContactDataExtractor that pulls data out of the solver and puts it into a simpler format."), new Vector2(16, resolution.Y - 80), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Pulling data this way can sometimes be more convenient than tracking contacts from INarrowPhaseCallbacks."), new Vector2(16, resolution.Y - 64), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Contacts associated with the large box are visualized. The size of a contact corresponds to the contact's force."), new Vector2(16, resolution.Y - 48), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Sensor manifold constraint count: ").Append(extractor.Constraints.Count).Append(", contact count: ").Append(sensorContactCount), new Vector2(16, resolution.Y - 20), 20, Vector3.One, font); + base.Render(renderer, camera, input, text, font); } } From 5b58d209c05d1340c738e8f7f05da78b3fca70fa Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 19 Feb 2023 20:09:33 -0600 Subject: [PATCH 574/947] Fiddling toward proper sleeping management. --- Demos/Demos/CollisionTrackingDemo.cs | 154 ++++++++++++++++++++------- 1 file changed, 113 insertions(+), 41 deletions(-) diff --git a/Demos/Demos/CollisionTrackingDemo.cs b/Demos/Demos/CollisionTrackingDemo.cs index 5034ebd1a..c9fded9c8 100644 --- a/Demos/Demos/CollisionTrackingDemo.cs +++ b/Demos/Demos/CollisionTrackingDemo.cs @@ -23,7 +23,7 @@ namespace Demos.Demos; /// /// /// This implementation focuses more on simplicity than performance. -/// It's not too slow, but if you run into bottlenecks, be advised that you can make it faster! +/// It's not too slow, but if you run into bottlenecks with tens of thousands of bodies with tracked collisions, be advised that you can make it faster! /// public class CollisionTrackingDemo : Demo { @@ -54,31 +54,40 @@ public struct ContactSource : IEqualityComparerRef, IEquatable new() { Collidable = collidable, ChildIndex = -1 }; } - /// /// Stores contacts generated by the narrow phase for a pair. /// - [StructLayout(LayoutKind.Explicit)] public struct PairCollision { /// - /// The other collidable (and if applicable, child) associated with this pair. - /// - [FieldOffset(0)] - public ContactSource Other; - /// - /// Stores whether is stored in the first slot of the collision pair. True if it is, false if it isn't. + /// Stores whether the 'other' collidable (the one that isn't the current tracked object) is stored in the first slot of the collision pair. True if it is, false if it isn't. /// This matters for interpreting the contact data: by convention, the contact offset is relative to collidable A, and the normal points from B to A. /// - [FieldOffset(8)] public bool OtherIsPairA; /// - /// Contacts generated by the narrow phase. + /// True if the current front buffer is , otherwise the front buffer is . + /// + public bool AIsFrontBuffer; + + /// + /// One of two contact buffers in the collision. This may be either the front or back buffer depending on the value of . + /// + public NonconvexContactManifold A; + /// + /// One of two contact buffers in the collision. This may be either the front or back buffer depending on the value of . + /// + public NonconvexContactManifold B; + + /// + /// Gets the contacts generated by the narrow phase in the latest update affecting this pair. /// /// We're making use of the type here because it's able to store both nonconvex manifolds and convex manifolds. /// Within the callbacks, we convert convex manifolds into nonconvex manifolds by rearranging data; that way, post-analysis doesn't need to worry about the manifold type. - [FieldOffset(12)] - public NonconvexContactManifold Contacts; + public static ref NonconvexContactManifold GetContacts(ref PairCollision pair) => ref pair.AIsFrontBuffer ? ref pair.A : ref pair.B; + /// + /// Gets the contacts generated by the narrow phase in the update before last affecting this pair. + /// + public static ref NonconvexContactManifold GetPreviousContacts(ref PairCollision pair) => ref pair.AIsFrontBuffer ? ref pair.B : ref pair.A; } @@ -90,45 +99,80 @@ public class CollisionTracker { IThreadDispatcher dispatcher; BufferPool pool; + Simulation simulation; public CollisionTracker(BufferPool pool, IThreadDispatcher dispatcher) { this.pool = pool; this.dispatcher = dispatcher; - Tracked = new QuickDictionary(16, pool); + Tracked = new QuickDictionary(16, pool); } - public struct TrackedContacts + public void Initialize(Simulation simulation) + { + //The CollisionTracker is created before the Simulation in this demo because we're passing it in as a part of the INarrowPhaseCallbacks. + //So, we let the callbacks initialize the simulation reference! + this.simulation = simulation; + } + + /// + /// Defines how a tracked object should handle pair tracking when it (or the other collidable in the pair) is sleeping/static. + /// + /// + /// This complexity arises from the fact that collision pairs between sleeping bodies do not invoke narrow phase callbacks, because they are never examined. + /// The broad phase will only ever emit pair tests for pairs that contain at least one wakeful body. + /// This enumeration defines the ways in which the collision tracker can handle this situation. + /// + public enum SleepBehavior { /// - /// Holds all contacts reported about a tracked object by narrow phase callbacks for the timestep before the most recent timestep. + /// If the tracked object is a sleeping body, all of its associated pairs are considered paused in time and will not be flipped/cleared. + /// Pairs with no wakeful bodies will then still have tracked contacts even though sleeping pairs do not invoke narrow phase callbacks. + /// + /// Any contacts detected between the sleeping body and other wakeful bodies will not be reported unless those contacts cause the sleeping body to wake up. + /// + /// Static bodies + /// + /// This is very cheap and simple. /// - public QuickList PreviousCollisions; + PauseTrackingForSleepingBodies, /// - /// Holds all contacts reported about a tracked object by narrow phase callbacks for the most recent timestep. + /// Every frame, regardless of whether the tracked object is awake or static, the collision tracker will flip the previous/current buffers and clear the newly-current buffer before flushing new contacts. + /// If the tracked collidable is static or sleeping, the incoming contacts will only contain contacts found from wakeful bodies interacting with the tracked object. + /// If neither collidable in a pair is awake, no new contacts will be reported, and so the tracker will contain no contacts for the pair even if it had contacts prior to sleeping. + /// + /// This is very cheap and simple. /// - public QuickList Collisions; + AlwaysClear, + } + - public TrackedContacts(BufferPool pool) + public struct TrackedPairs + { + /// + /// Holds all pairs associated with a tracked object. Keys are the 'other' collidable in the pair- whatever collidable isn't the tracked object. + /// + public QuickDictionary Pairs; + + public TrackedPairs(BufferPool pool) { //For simplicity, we'll initialize all the contact storage with a fixed size. It'll grow over time if more collisions per tracked object are detected. - //Some kind of compaction would be easy to add- just iterate over all tracked objects and call Compact on these lists. + //Some kind of compaction would be easy to add- just iterate over all tracked objects and call Compact. //(Or just part of them, incrementally! Probably better to avoid excess resizes!) - Collisions = new QuickList(8, pool); - PreviousCollisions = new QuickList(8, pool); + Pairs = new QuickDictionary(16, pool); } + public void Dispose(BufferPool pool) { - PreviousCollisions.Dispose(pool); - Collisions.Dispose(pool); + Pairs.Dispose(pool); } } /// /// Maps tracked objects to the contacts associated with them. /// - public QuickDictionary Tracked; + public QuickDictionary Tracked; /// /// Adds a collidable (or its child) to the collision tracker. Collisions associated with it will be included for queries. @@ -139,7 +183,7 @@ public void Track(ContactSource target) { if (Tracked.ContainsKey(target)) throw new ArgumentException("Object already tracked."); - Tracked.Add(target, new TrackedContacts(pool), pool); + Tracked.Add(target, new TrackedPairs(pool), pool); } /// @@ -152,9 +196,7 @@ public void Untrack(ContactSource target) if (!Tracked.ContainsKey(target)) throw new ArgumentException("Object is not tracked."); Tracked.GetTableIndices(ref target, out var tableIndex, out var elementIndex); - ref var contacts = ref Tracked.Values[elementIndex]; - contacts.PreviousCollisions.Dispose(pool); - contacts.Collisions.Dispose(pool); + Tracked.Values[elementIndex].Dispose(pool); Tracked.FastRemove(tableIndex, elementIndex); } @@ -168,7 +210,9 @@ public void Untrack(ContactSource target) struct WorkerPairContacts { public ContactSource Self; - public PairCollision Collision; + public ContactSource Other; + public bool OtherIsAInPair; + public NonconvexContactManifold Contacts; } /// @@ -193,9 +237,35 @@ public void Flush() //Flip the caches for each listener and clear out the now-current set for accumulation. for (int i = 0; i < Tracked.Count; ++i) { - ref var listenerContacts = ref Tracked.Values[i]; - BepuPhysics.Helpers.Swap(ref listenerContacts.PreviousCollisions, ref listenerContacts.Collisions); - listenerContacts.Collisions.Count = 0; + ref var pairs = ref Tracked.Values[i].Pairs; + var collidable = Tracked.Keys[i].Collidable; + if (collidable.Mobility != CollidableMobility.Static && simulation.Bodies[collidable.BodyHandle].Awake) + { + //There is no need to consider the sleeping state of the other body when the tracked object is awake; flip all pairs and clear the new front buffer. + for (int j = 0; j < pairs.Count; ++j) + { + ref var pair = ref pairs.Values[j]; + pair.AIsFrontBuffer = !pair.AIsFrontBuffer; + PairCollision.GetContacts(ref pair).Count = 0; + } + } + else + { + //The tracked object is either sleeping or static. + //Any pair involving a wakeful body must be flipped and cleared. Any pair involving a sleeping body or a static doesn't. + //(Note that the broad phase will never report a pair containing only sleeping bodies or statics; those both live in the static tree, and it is not tested with itself.) + for (int j = 0; j < pairs.Count; ++j) + { + var other = pairs.Keys[j]; + if (other.Collidable.Mobility != CollidableMobility.Static && simulation.Bodies[other.Collidable.BodyHandle].Awake) + { + //Wakeful body! Flip and clear. + ref var pair = ref pairs.Values[j]; + pair.AIsFrontBuffer = !pair.AIsFrontBuffer; + PairCollision.GetContacts(ref pair).Count = 0; + } + } + } } //Flush the worker caches into the main storage and dispose the caches. @@ -206,7 +276,7 @@ public void Flush() { ref var entry = ref cache[j]; Tracked.GetTableIndices(ref entry.Self, out _, out var elementIndex); - Tracked.Values[elementIndex].Collisions.Add(entry.Collision, pool); + Tracked.Values[elementIndex].Pairs.Add(entry.Collision., pool); } //The worker cache memory must be returned to the thread pool, not the main pool! cache.Dispose(dispatcher.GetThreadMemoryPool(i)); @@ -221,8 +291,9 @@ private void ReportContacts(int workerIndex, ContactSource self, Cont //A is a listener, add it. ref var pairContacts = ref workerCaches[workerIndex].Allocate(dispatcher.GetThreadMemoryPool(workerIndex)); pairContacts.Self = self; - pairContacts.Collision.Other = other; - pairContacts.Collision.OtherIsPairA = otherIsA; + pairContacts.Other = other; + pairContacts.OtherIsAInPair = otherIsA; + PairCollision.GetContacts if (typeof(TContacts) == typeof(ConvexContactManifold)) //This is a JIT-time constant. { //The pairContacts representation just uses a NonconvexContactManifold for simplicity- we can just rearrange convex contacts to fit. @@ -230,15 +301,15 @@ private void ReportContacts(int workerIndex, ContactSource self, Cont ref var convex = ref Unsafe.As(ref contacts); for (int i = 0; i < contacts.Count; ++i) { - ref var targetContact = ref Unsafe.Add(ref pairContacts.Collision.Contacts.Contact0, i); + ref var targetContact = ref Unsafe.Add(ref pairContacts.Contacts.Contact0, i); convex.GetContact(i, out targetContact.Offset, out targetContact.Normal, out targetContact.Depth, out targetContact.FeatureId); } - pairContacts.Collision.Contacts.Count = contacts.Count; - pairContacts.Collision.Contacts.OffsetB = convex.OffsetB; + pairContacts.Contacts.Count = contacts.Count; + pairContacts.Contacts.OffsetB = convex.OffsetB; } else { - pairContacts.Collision.Contacts = Unsafe.As(ref contacts); + pairContacts.Contacts = Unsafe.As(ref contacts); } } } @@ -325,6 +396,7 @@ public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int c public void Initialize(Simulation simulation) { + collisionTracker.Initialize(simulation); } public void Dispose() From 2e182a743c7a4e0bd679d71d434955b2ef117d15 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 23 Feb 2023 15:53:45 -0600 Subject: [PATCH 575/947] Sleeping stuff works; collisiontrackingdemo done except for polish/feature pass that will come later. --- Demos/Demos/CollisionTrackingDemo.cs | 231 ++++++++++++--------------- 1 file changed, 101 insertions(+), 130 deletions(-) diff --git a/Demos/Demos/CollisionTrackingDemo.cs b/Demos/Demos/CollisionTrackingDemo.cs index c9fded9c8..8ce019432 100644 --- a/Demos/Demos/CollisionTrackingDemo.cs +++ b/Demos/Demos/CollisionTrackingDemo.cs @@ -63,31 +63,12 @@ public struct PairCollision /// Stores whether the 'other' collidable (the one that isn't the current tracked object) is stored in the first slot of the collision pair. True if it is, false if it isn't. /// This matters for interpreting the contact data: by convention, the contact offset is relative to collidable A, and the normal points from B to A. /// - public bool OtherIsPairA; - /// - /// True if the current front buffer is , otherwise the front buffer is . - /// - public bool AIsFrontBuffer; - - /// - /// One of two contact buffers in the collision. This may be either the front or back buffer depending on the value of . - /// - public NonconvexContactManifold A; - /// - /// One of two contact buffers in the collision. This may be either the front or back buffer depending on the value of . - /// - public NonconvexContactManifold B; + public bool OtherIsAInPair; /// /// Gets the contacts generated by the narrow phase in the latest update affecting this pair. /// - /// We're making use of the type here because it's able to store both nonconvex manifolds and convex manifolds. - /// Within the callbacks, we convert convex manifolds into nonconvex manifolds by rearranging data; that way, post-analysis doesn't need to worry about the manifold type. - public static ref NonconvexContactManifold GetContacts(ref PairCollision pair) => ref pair.AIsFrontBuffer ? ref pair.A : ref pair.B; - /// - /// Gets the contacts generated by the narrow phase in the update before last affecting this pair. - /// - public static ref NonconvexContactManifold GetPreviousContacts(ref PairCollision pair) => ref pair.AIsFrontBuffer ? ref pair.B : ref pair.A; + public NonconvexContactManifold Contacts; } @@ -95,7 +76,7 @@ public struct PairCollision /// /// Tracks collisions for a set of collidables (or their children). /// - public class CollisionTracker + public class CollisionTracker : IDisposable { IThreadDispatcher dispatcher; BufferPool pool; @@ -113,46 +94,44 @@ public void Initialize(Simulation simulation) //The CollisionTracker is created before the Simulation in this demo because we're passing it in as a part of the INarrowPhaseCallbacks. //So, we let the callbacks initialize the simulation reference! this.simulation = simulation; + //Attach the callbacks to the simulation so that we can monitor the sleeping state of bodies *after* the sleeper has run, but before the narrow phase runs. + //This ensures we have an informative view of the sleeping state of bodies. + //(If you sample before the sleeper, you could miss out on something going to sleep; if you sample after the narrow phase, + //you could miss out on the fact that a body was asleep beforehand and wouldn't have reported anything in the narrow phase.) + this.simulation.Timestepper.BeforeCollisionDetection += PrepareForNextTimestep; + this.simulation.Timestepper.CollisionsDetected += Flush; } /// - /// Defines how a tracked object should handle pair tracking when it (or the other collidable in the pair) is sleeping/static. + /// Disposes the collision tracker's internal allocations and detaches its callbacks from the simulation. /// - /// - /// This complexity arises from the fact that collision pairs between sleeping bodies do not invoke narrow phase callbacks, because they are never examined. - /// The broad phase will only ever emit pair tests for pairs that contain at least one wakeful body. - /// This enumeration defines the ways in which the collision tracker can handle this situation. - /// - public enum SleepBehavior + public void Dispose() { - /// - /// If the tracked object is a sleeping body, all of its associated pairs are considered paused in time and will not be flipped/cleared. - /// Pairs with no wakeful bodies will then still have tracked contacts even though sleeping pairs do not invoke narrow phase callbacks. - /// - /// Any contacts detected between the sleeping body and other wakeful bodies will not be reported unless those contacts cause the sleeping body to wake up. - /// - /// Static bodies - /// - /// This is very cheap and simple. - /// - PauseTrackingForSleepingBodies, - /// - /// Every frame, regardless of whether the tracked object is awake or static, the collision tracker will flip the previous/current buffers and clear the newly-current buffer before flushing new contacts. - /// If the tracked collidable is static or sleeping, the incoming contacts will only contain contacts found from wakeful bodies interacting with the tracked object. - /// If neither collidable in a pair is awake, no new contacts will be reported, and so the tracker will contain no contacts for the pair even if it had contacts prior to sleeping. - /// - /// This is very cheap and simple. - /// - AlwaysClear, + if (Tracked.Keys.Allocated) + { + for (int i = 0; i < Tracked.Count; ++i) + { + Tracked.Values[i].Pairs.Dispose(pool); + } + Tracked.Dispose(pool); + simulation.Timestepper.BeforeCollisionDetection -= PrepareForNextTimestep; + simulation.Timestepper.CollisionsDetected -= Flush; + } } public struct TrackedPairs { /// - /// Holds all pairs associated with a tracked object. Keys are the 'other' collidable in the pair- whatever collidable isn't the tracked object. + /// Holds all pairs associated with a tracked object as found in the most recent update to this pair. + /// Keys are the 'other' collidable in the pair- whatever collidable isn't the tracked object. /// public QuickDictionary Pairs; + /// + /// Holds all pairs associated with a tracked object as found in the update prior to the most recent one. + /// Keys are the 'other' collidable in the pair- whatever collidable isn't the tracked object. + /// + public QuickDictionary PreviousPairs; public TrackedPairs(BufferPool pool) { @@ -160,12 +139,14 @@ public TrackedPairs(BufferPool pool) //Some kind of compaction would be easy to add- just iterate over all tracked objects and call Compact. //(Or just part of them, incrementally! Probably better to avoid excess resizes!) Pairs = new QuickDictionary(16, pool); + PreviousPairs = new QuickDictionary(16, pool); } public void Dispose(BufferPool pool) { Pairs.Dispose(pool); + PreviousPairs.Dispose(pool); } } @@ -211,8 +192,7 @@ struct WorkerPairContacts { public ContactSource Self; public ContactSource Other; - public bool OtherIsAInPair; - public NonconvexContactManifold Contacts; + public PairCollision Collision; } /// @@ -220,54 +200,67 @@ struct WorkerPairContacts /// Buffer> workerCaches; - public void PrepareForNextTimestep() - { - workerCaches = new Buffer>(dispatcher.ThreadCount, pool); - for (int i = 0; i < workerCaches.Length; ++i) - { - workerCaches[i] = new QuickList(512, dispatcher.GetThreadMemoryPool(i)); - } - } - /// - /// Flushes all collisions found in the previous timestep into efficient storage for queries. + /// Prepares the collision tracker for the next timestep by swapping/clearing pairs that expect updates in the coming narrow phase step and preparing the per-worker contact caches. /// - public void Flush() + void PrepareForNextTimestep(float dt, IThreadDispatcher dispatcher) { //Flip the caches for each listener and clear out the now-current set for accumulation. + //Note that we have to do some special stuff in the case of sleeping. + //This complexity arises from the fact that collision pairs between sleeping bodies do not invoke narrow phase callbacks, because they are never examined. + //The broad phase will only ever emit pair tests for pairs that contain at least one wakeful body. + + //Note that this function is attached to simulation.Timestepper.BeforeCollisionDetection. + //That fires *after* the sleeper runs and before the narrowphase, so we get an accurate picture of what pairs will end up providing contacts during the upcoming tests. for (int i = 0; i < Tracked.Count; ++i) { - ref var pairs = ref Tracked.Values[i].Pairs; + ref var tracked = ref Tracked.Values[i]; var collidable = Tracked.Keys[i].Collidable; if (collidable.Mobility != CollidableMobility.Static && simulation.Bodies[collidable.BodyHandle].Awake) - { - //There is no need to consider the sleeping state of the other body when the tracked object is awake; flip all pairs and clear the new front buffer. - for (int j = 0; j < pairs.Count; ++j) - { - ref var pair = ref pairs.Values[j]; - pair.AIsFrontBuffer = !pair.AIsFrontBuffer; - PairCollision.GetContacts(ref pair).Count = 0; - } + { + //There is no need to consider the sleeping state of the other body when the tracked object is awake; flip the buffers and clear the new front buffer. + BepuPhysics.Helpers.Swap(ref tracked.Pairs, ref tracked.PreviousPairs); + tracked.Pairs.FastClear(); } else { //The tracked object is either sleeping or static. - //Any pair involving a wakeful body must be flipped and cleared. Any pair involving a sleeping body or a static doesn't. - //(Note that the broad phase will never report a pair containing only sleeping bodies or statics; those both live in the static tree, and it is not tested with itself.) - for (int j = 0; j < pairs.Count; ++j) + //Any pair involving a wakeful body must be flipped and cleared. + //Any pair that involves no wakeful bodies should preserve the old values, because pairs with no wakeful bodies did not receive new contacts in the narrow phase. + + // NOTE: While this is not a zero cost operation, you could make the choice to ignore reported contacts for sleeping tracked bodies, + // and then just don't touch any of the contact data for sleeping tracked bodies during this phase! + // That would technically miss out on reported contacts that don't result in waking up the shape. + // + // You could also make the behavior around sleeping different on a per tracked object level. + // For example, you could include extra data you want in the TrackedPairs type to decide how to handle sleeping. + // This demo just does the most frequently desired thing with no configuration for simplicity's sake. + for (int j = tracked.Pairs.Count - 1; j >= 0; --j) { - var other = pairs.Keys[j]; + var other = tracked.Pairs.Keys[j]; if (other.Collidable.Mobility != CollidableMobility.Static && simulation.Bodies[other.Collidable.BodyHandle].Awake) { - //Wakeful body! Flip and clear. - ref var pair = ref pairs.Values[j]; - pair.AIsFrontBuffer = !pair.AIsFrontBuffer; - PairCollision.GetContacts(ref pair).Count = 0; + //Wakeful body! Push the current values into the previous buffer and remove the entry from the current. + tracked.PreviousPairs.AddAndReplace(other, tracked.Pairs.Values[j], pool); + tracked.Pairs.FastRemove(other); } + //If this is a sleeping body or static, then the pair has no wakeful bodies and "preserving the old values" means doing nothing. } } } + workerCaches = new Buffer>(dispatcher.ThreadCount, pool); + for (int i = 0; i < workerCaches.Length; ++i) + { + workerCaches[i] = new QuickList(512, dispatcher.GetThreadMemoryPool(i)); + } + } + + /// + /// Flushes all collisions found in the previous timestep into efficient storage for queries. + /// + void Flush(float dt, IThreadDispatcher threadDispatcher) + { //Flush the worker caches into the main storage and dispose the caches. for (int i = 0; i < workerCaches.Length; ++i) { @@ -276,7 +269,7 @@ public void Flush() { ref var entry = ref cache[j]; Tracked.GetTableIndices(ref entry.Self, out _, out var elementIndex); - Tracked.Values[elementIndex].Pairs.Add(entry.Collision., pool); + Tracked.Values[elementIndex].Pairs.Add(entry.Other, entry.Collision, pool); } //The worker cache memory must be returned to the thread pool, not the main pool! cache.Dispose(dispatcher.GetThreadMemoryPool(i)); @@ -292,8 +285,7 @@ private void ReportContacts(int workerIndex, ContactSource self, Cont ref var pairContacts = ref workerCaches[workerIndex].Allocate(dispatcher.GetThreadMemoryPool(workerIndex)); pairContacts.Self = self; pairContacts.Other = other; - pairContacts.OtherIsAInPair = otherIsA; - PairCollision.GetContacts + pairContacts.Collision.OtherIsAInPair = otherIsA; if (typeof(TContacts) == typeof(ConvexContactManifold)) //This is a JIT-time constant. { //The pairContacts representation just uses a NonconvexContactManifold for simplicity- we can just rearrange convex contacts to fit. @@ -301,15 +293,15 @@ private void ReportContacts(int workerIndex, ContactSource self, Cont ref var convex = ref Unsafe.As(ref contacts); for (int i = 0; i < contacts.Count; ++i) { - ref var targetContact = ref Unsafe.Add(ref pairContacts.Contacts.Contact0, i); + ref var targetContact = ref Unsafe.Add(ref pairContacts.Collision.Contacts.Contact0, i); convex.GetContact(i, out targetContact.Offset, out targetContact.Normal, out targetContact.Depth, out targetContact.FeatureId); } - pairContacts.Contacts.Count = contacts.Count; - pairContacts.Contacts.OffsetB = convex.OffsetB; + pairContacts.Collision.Contacts.Count = contacts.Count; + pairContacts.Collision.Contacts.OffsetB = convex.OffsetB; } else { - pairContacts.Contacts = Unsafe.As(ref contacts); + pairContacts.Collision.Contacts = Unsafe.As(ref contacts); } } } @@ -441,23 +433,9 @@ public override void Initialize(ContentArchive content, Camera camera) Simulation.Statics.Add(new StaticDescription(new Vector3(0, 3, 15), Simulation.Shapes.Add(new Box(30, 5, 1)))); } - static bool IsTouching(NonconvexContactManifold contacts) - { - for (int i = 0; i < contacts.Count; ++i) - { - //This may look a bit odd: it's a legacy issue from before static abstracts existed. Expect it to change soon! - if (contacts.GetDepth(ref contacts, i) >= 0) - { - return true; - } - } - return false; - } - void AddParticle(Vector3 contactOffset, Vector3 contactNormal, CollidableReference firstCollidableInPair) { ref var particle = ref particles.Allocate(BufferPool); - //Contact data is calibrated according to the order of the pair, so using A's position is important. particle.Position = contactOffset + (firstCollidableInPair.Mobility == CollidableMobility.Static ? new StaticReference(firstCollidableInPair.StaticHandle, Simulation.Statics).Pose.Position : @@ -478,53 +456,42 @@ bool PreviousContainsTouchingFeatureId(ref PairCollision pair, int featureId) public override void Update(Window window, Camera camera, Input input, float dt) { - //base.Update includes a call to the Simulation.Timestep. We want to prepare the worker caches and flush them after the timestep completes. - //(These could be hooked onto events exposed by the simulation.Timestepper, like the CollisionEvents demo does, but this demo tries to be a little less magic. - collisionTracker.PrepareForNextTimestep(); + //base.Update includes a call to the Simulation.Timestep. base.Update(window, camera, input, dt); - collisionTracker.Flush(); - //Now analyze the contacts we've collected. + //Now analyze the contacts we've collected. We'll match the ContactEventsDemo's OnContactAdded behavior. + //Note that you could implement any query, including queries that span multiple contact manifolds and children. + //Having all of the contact state accessible at once tends to provide a lot more flexibility than events, + //and it tends to be easier to reason about the control flow. + //(The analysis is exactly where you do it, not inside some other thread's execution in the middle of the physics engine!) for (int trackedIndex = 0; trackedIndex < collisionTracker.Tracked.Count; ++trackedIndex) { - //We do a somewhat questionable previous test here. All contacts are stored in lists, so we enumerate all previous collisions. - //For objects with relatively few collisions, this is perfectly fine (and actually near optimal). - //But if a single object has thousands of collisions, this is going to scale pretty poorly. - //The demo doesn't worry about this for simplicity reasons, but you could use a dictionary or other more suitable structure. - ref var others = ref collisionTracker.Tracked.Values[trackedIndex]; + ref var collisions = ref collisionTracker.Tracked.Values[trackedIndex]; var self = collisionTracker.Tracked.Keys[trackedIndex]; - for (int otherIndex = 0; otherIndex < others.Collisions.Count; ++otherIndex) + for (int pairIndex = 0; pairIndex < collisions.Pairs.Count; ++pairIndex) { - int indexInPrevious = -1; - ref var other = ref others.Collisions[otherIndex]; - for (int k = 0; k < others.PreviousCollisions.Count; ++k) - { - if (others.PreviousCollisions[k].Other == others.Collisions[otherIndex].Other) - { - indexInPrevious = k; - break; - } - } - if (indexInPrevious >= 0) + ref var pair = ref collisions.Pairs.Values[pairIndex]; + var other = collisions.Pairs.Keys[pairIndex]; + if (collisions.PreviousPairs.GetTableIndices(ref other, out _, out int otherIndexInPrevious)) { //There exists a previous collision. - ref var previous = ref others.PreviousCollisions[indexInPrevious]; - for (int i = 0; i < other.Contacts.Count; ++i) + ref var previous = ref collisions.PreviousPairs.Values[otherIndexInPrevious]; + for (int i = 0; i < pair.Contacts.Count; ++i) { - if (other.Contacts.GetDepth(ref other.Contacts, i) >= 0) + if (pair.Contacts.GetDepth(ref pair.Contacts, i) >= 0) { //This contact is touching. Does there exist a contact with the same feature id that was touching in the previous timestep? - if (!PreviousContainsTouchingFeatureId(ref previous, other.Contacts.GetFeatureId(i))) - AddParticle(other.Contacts.GetOffset(ref other.Contacts, i), other.Contacts.GetNormal(ref other.Contacts, i), other.OtherIsPairA ? other.Other.Collidable : self.Collidable); + if (!PreviousContainsTouchingFeatureId(ref previous, pair.Contacts.GetFeatureId(i))) + AddParticle(pair.Contacts.GetOffset(ref pair.Contacts, i), pair.Contacts.GetNormal(ref pair.Contacts, i), pair.OtherIsAInPair ? other.Collidable : self.Collidable); } } } else { //No previous collision, so all contacts are new. - for (int i = 0; i < other.Contacts.Count; ++i) + for (int i = 0; i < pair.Contacts.Count; ++i) { - AddParticle(other.Contacts.GetOffset(ref other.Contacts, i), other.Contacts.GetNormal(ref other.Contacts, i), other.OtherIsPairA ? other.Other.Collidable : self.Collidable); + AddParticle(pair.Contacts.GetOffset(ref pair.Contacts, i), pair.Contacts.GetNormal(ref pair.Contacts, i), pair.OtherIsAInPair ? other.Collidable : self.Collidable); } } } @@ -558,7 +525,11 @@ public override void Render(Renderer renderer, Camera camera, Input input, TextB } var resolution = renderer.Surface.Resolution; - renderer.TextBatcher.Write(text.Clear().Append("TBA!"), new Vector2(16, resolution.Y - 80), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Collision events aren't the only way to interact with contacts!"), new Vector2(16, resolution.Y - 80), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("This demo collects contacts in the INarrowPhaseCallbacks implementation and hands them off to a CollisionTracker type."), new Vector2(16, resolution.Y - 64), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Tracked collidables keep the current and previous collision state around."), new Vector2(16, resolution.Y - 48), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("This collision state can be queried after the fact to serve many of the same use cases as collision events,"), new Vector2(16, resolution.Y - 32), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("but in a more flexible way that doesn't force the user to think about potentially confusing execution contexts and control flows."), new Vector2(16, resolution.Y - 16), 16, Vector3.One, font); base.Render(renderer, camera, input, text, font); } From ee72e14c25a3520300b4b4fdf175370cf6e254d8 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 30 Aug 2022 17:36:50 -0500 Subject: [PATCH 576/947] First pass at self test options. Worse than original! --- BepuPhysics/Trees/Tree_SelfQueries.cs | 300 +++++++++++++++++- Demos/DemoSet.cs | 2 + Demos/SpecializedTests/ShapePileTestDemo.cs | 42 +-- .../SpecializedTests/TreeFiddlingTestDemo.cs | 71 +++++ 4 files changed, 394 insertions(+), 21 deletions(-) create mode 100644 Demos/SpecializedTests/TreeFiddlingTestDemo.cs diff --git a/BepuPhysics/Trees/Tree_SelfQueries.cs b/BepuPhysics/Trees/Tree_SelfQueries.cs index bf2c4dcbf..93a5d9bc0 100644 --- a/BepuPhysics/Trees/Tree_SelfQueries.cs +++ b/BepuPhysics/Trees/Tree_SelfQueries.cs @@ -1,6 +1,12 @@ -using BepuUtilities; +using BepuPhysics.CollisionDetection.CollisionTasks; +using BepuUtilities; +using BepuUtilities.Collections; +using BepuUtilities.Memory; +using System; +using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; namespace BepuPhysics.Trees { @@ -147,5 +153,297 @@ public unsafe void GetSelfOverlaps(ref TOverlapHandler results) GetOverlapsInNode(ref Nodes[0], ref results); } + struct StackEntry + { + public int A; + public int B; + } + + unsafe void GetOverlapsWithLeaf(ref TOverlapHandler results, NodeChild leaf, int nodeToTest, ref QuickList stack, BufferPool pool) where TOverlapHandler : IOverlapHandler + { + var leafIndex = Encode(leaf.Index); + Debug.Assert(stack.Count == 0); + while (true) + { + ref var node = ref Nodes[nodeToTest]; + var a = Intersects(leaf, node.A); + var b = Intersects(leaf, node.B); + var aIsInternal = node.A.Index >= 0; + var bIsInternal = node.B.Index >= 0; + var intersectedInternalA = a && aIsInternal; + var intersectedInternalB = b && bIsInternal; + if (intersectedInternalA && intersectedInternalB) + { + nodeToTest = node.A.Index; + stack.Allocate(pool) = node.B.Index; + } + else + { + if (a && !aIsInternal) + { + results.Handle(leafIndex, Encode(node.A.Index)); + } + if (b && !bIsInternal) + { + results.Handle(leafIndex, Encode(node.B.Index)); + } + if (intersectedInternalA || intersectedInternalB) + { + nodeToTest = intersectedInternalA ? node.A.Index : node.B.Index; + } + else + { + //The current traversal step doesn't offer a next step; pull from the stack. + if (!stack.TryPop(out nodeToTest)) + { + //Nothing left to test against this leaf! Done! + break; + } + } + } + } + } + + public unsafe void GetSelfOverlaps2(ref TOverlapHandler results, BufferPool pool) where TOverlapHandler : IOverlapHandler + { + //If there are less than two leaves, there can't be any overlap. + //This provides a guarantee that there are at least 2 children in each internal node considered by GetOverlapsInNode. + if (LeafCount < 2) + return; + + QuickList stack = new(Math.Min(1024, Math.Max(0, NodeCount / 4)), pool); + QuickList leafStack = new(Math.Min(512, Math.Max(0, NodeCount / 16)), pool); + StackEntry nextTest = default; + + while (true) + { + if (nextTest.A == nextTest.B) + { + //Self test. + //Possible sources of further stack entries in a self test: + //1) Child A and B are both internal and their bounds intersect. + //2) Child A is internal. + //3) Child B is internal. + //We can also spawn leaf-subtree tests if: + //1) Child A and B have intersecting bounds, and only one of them is a leaf. + //We can spawn direct leaf tests if: + //1) Child A and B have intersecting bounds, and both are leaves. + //Note that we never need to push an entry with differing A and B indices to the stack *from a self test*. There can only be one such entry created from any self test, and it's always visited next. + //Non-self tests will generate *only* results with differing A and B indices. + ref var node = ref Nodes[nextTest.A]; + var abIntersect = Intersects(node.A, node.B); + var aIsInternal = node.A.Index >= 0; + var bIsInternal = node.B.Index >= 0; + if (abIntersect && aIsInternal && bIsInternal) + { + //Both children internal and their bounds intersect. + //Their intersection should be the next visited. + nextTest.A = node.A.Index; + nextTest.B = node.B.Index; + + //The two child self tests get pushed to the stack. + ref var stackA = ref stack.Allocate(pool); + stackA.A = node.A.Index; + stackA.B = node.A.Index; + ref var stackB = ref stack.Allocate(pool); + stackB.A = node.B.Index; + stackB.B = node.B.Index; + } + else + { + if (abIntersect) + { + Debug.Assert(!aIsInternal || !bIsInternal, "Just in case you shuffle logic around in the future: this clause assumes at least one leaf."); + //The children have overlapping bounds, but at least one is a leaf. + //Note that this path cannot generate any stack entries. + //At least one child is a leaf, so there is at most one self-test. + //The a-b test involves at least one leaf as well, which means it will use the GetOverlapsWithLeaf path. + //So the one possible self-test will be put into the nextTest. + if (aIsInternal || bIsInternal) + { + //One's a leaf, one's internal. + if (aIsInternal) + { + nextTest.A = node.A.Index; + nextTest.B = node.A.Index; + GetOverlapsWithLeaf(ref results, node.B, node.A.Index, ref leafStack, pool); + } + else + { + nextTest.A = node.B.Index; + nextTest.B = node.B.Index; + GetOverlapsWithLeaf(ref results, node.A, node.B.Index, ref leafStack, pool); + } + } + else + { + //Both children are leaves. They have overlapping bounds, so... + results.Handle(Encode(node.A.Index), Encode(node.B.Index)); + + //If both children are leaves, then there's no other source for the next visit, so pop. + if (!stack.TryPop(out nextTest)) + break; + } + } + else + { + //No a-b test, but possibly self tests. + if (aIsInternal && bIsInternal) + { + nextTest.A = node.A.Index; + nextTest.B = node.A.Index; + ref var stackB = ref stack.Allocate(pool); + stackB.A = node.B.Index; + stackB.B = node.B.Index; + } + else if (aIsInternal || bIsInternal) + { + var nextSelfTestIndex = aIsInternal ? node.A.Index : node.B.Index; + nextTest.A = nextSelfTestIndex; + nextTest.B = nextSelfTestIndex; + } + else + { + //There was no A-B test and both children are leaves. No new tests available, so grab from the stack. + if (!stack.TryPop(out nextTest)) + break; + } + } + } + } + else + { + //Not a self test! + //Possible sources of stack entry: + //1) AA intersection, both internal + //2) AB intersection, both internal + //3) BA intersection, both internal + //4) BB intersection, both internal + ref var n0 = ref Nodes[nextTest.A]; + ref var n1 = ref Nodes[nextTest.B]; + var aaIntersects = Intersects(n0.A, n1.A); + var abIntersects = Intersects(n0.A, n1.B); + var baIntersects = Intersects(n0.B, n1.A); + var bbIntersects = Intersects(n0.B, n1.B); + var n0AIsInternal = n0.A.Index >= 0; + var n0BIsInternal = n0.B.Index >= 0; + var n1AIsInternal = n1.A.Index >= 0; + var n1BIsInternal = n1.B.Index >= 0; + //The first test which generates a stack candidate gets the nextTest; the rest get pushed to the stack. + int previousStackGeneratorCount = 0; + if (aaIntersects) + { + if (n0AIsInternal && n1AIsInternal) + { + ++previousStackGeneratorCount; + nextTest.A = n0.A.Index; + nextTest.B = n1.A.Index; + } + else + { + //At least one is a leaf. + if (n0AIsInternal) + GetOverlapsWithLeaf(ref results, n1.A, n0.A.Index, ref leafStack, pool); + else if (n1AIsInternal) + GetOverlapsWithLeaf(ref results, n0.A, n1.A.Index, ref leafStack, pool); + else //Both are leaves. + results.Handle(Encode(n0.A.Index), Encode(n1.A.Index)); + } + } + if (abIntersects) + { + if (n0AIsInternal && n1BIsInternal) + { + if (previousStackGeneratorCount++ == 0) + { + nextTest.A = n0.A.Index; + nextTest.B = n1.B.Index; + } + else + { + ref var stackEntry = ref stack.Allocate(pool); + stackEntry.A = n0.A.Index; + stackEntry.B = n1.B.Index; + } + } + else + { + //At least one is a leaf. + if (n0AIsInternal) + GetOverlapsWithLeaf(ref results, n1.B, n0.A.Index, ref leafStack, pool); + else if (n1BIsInternal) + GetOverlapsWithLeaf(ref results, n0.A, n1.B.Index, ref leafStack, pool); + else //Both are leaves. + results.Handle(Encode(n0.A.Index), Encode(n1.B.Index)); + } + } + if (baIntersects) + { + if (n0BIsInternal && n1AIsInternal) + { + if (previousStackGeneratorCount++ == 0) + { + nextTest.A = n0.B.Index; + nextTest.B = n1.A.Index; + } + else + { + ref var stackEntry = ref stack.Allocate(pool); + stackEntry.A = n0.B.Index; + stackEntry.B = n1.A.Index; + } + } + else + { + //At least one is a leaf. + if (n0BIsInternal) + GetOverlapsWithLeaf(ref results, n1.A, n0.B.Index, ref leafStack, pool); + else if (n1AIsInternal) + GetOverlapsWithLeaf(ref results, n0.B, n1.A.Index, ref leafStack, pool); + else //Both are leaves. + results.Handle(Encode(n0.B.Index), Encode(n1.A.Index)); + } + } + if (bbIntersects) + { + if (n0BIsInternal && n1BIsInternal) + { + if (previousStackGeneratorCount++ == 0) + { + nextTest.A = n0.B.Index; + nextTest.B = n1.B.Index; + } + else + { + ref var stackEntry = ref stack.Allocate(pool); + stackEntry.A = n0.B.Index; + stackEntry.B = n1.B.Index; + } + } + else + { + //At least one is a leaf. + if (n0BIsInternal) + GetOverlapsWithLeaf(ref results, n1.B, n0.B.Index, ref leafStack, pool); + else if (n1BIsInternal) + GetOverlapsWithLeaf(ref results, n0.B, n1.B.Index, ref leafStack, pool); + else //Both are leaves. + results.Handle(Encode(n0.B.Index), Encode(n1.B.Index)); + } + } + if (previousStackGeneratorCount == 0) + { + //None of the candidates generated a next step, so grab from the stack. + if (!stack.TryPop(out nextTest)) + break; + } + } + + } + leafStack.Dispose(pool); + stack.Dispose(pool); + + } + } } diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index 94637674e..ac9406bde 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -46,6 +46,8 @@ struct Option public DemoSet() { + AddOption(); + AddOption(); AddOption(); AddOption(); AddOption(); diff --git a/Demos/SpecializedTests/ShapePileTestDemo.cs b/Demos/SpecializedTests/ShapePileTestDemo.cs index cdbe4ad9e..acdf05c98 100644 --- a/Demos/SpecializedTests/ShapePileTestDemo.cs +++ b/Demos/SpecializedTests/ShapePileTestDemo.cs @@ -23,6 +23,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //camera.Pitch = MathHelper.PiOver2 * 0.999f; Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(4, 1)); Simulation.Deterministic = true; + //Simulation.Deterministic = true; var sphere = new Sphere(1.5f); var capsule = new Capsule(1f, 1f); @@ -85,9 +86,9 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var sphereIndex = Simulation.Shapes.Add(sphere); var cylinderIndex = Simulation.Shapes.Add(cylinder); var hullIndex = Simulation.Shapes.Add(convexHull); - const int width = 8; + const int width = 16; const int height = 16; - const int length = 8; + const int length = 16; var shapeCount = 0; for (int i = 0; i < width; ++i) { @@ -96,27 +97,28 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) for (int k = 0; k < length; ++k) { var location = new Vector3(6, 3, 6) * new Vector3(i, j, k) + new Vector3(-width * 3, 5.5f, -length * 3); - var bodyDescription = BodyDescription.CreateKinematic(location, new(default, ContinuousDetection.Passive), 0.01f); + var bodyDescription = BodyDescription.CreateKinematic(location, new(default, ContinuousDetection.Passive), -0.01f); var index = shapeCount++; switch (index % 5) { - case 0: - bodyDescription.Collidable.Shape = sphereIndex; - bodyDescription.LocalInertia = sphereInertia; - break; - case 1: - bodyDescription.Collidable.Shape = capsuleIndex; - bodyDescription.LocalInertia = capsuleInertia; - break; - case 2: - bodyDescription.Collidable.Shape = boxIndex; - bodyDescription.LocalInertia = boxInertia; - break; - case 3: - bodyDescription.Collidable.Shape = cylinderIndex; - bodyDescription.LocalInertia = cylinderInertia; - break; - case 4: + //case 0: + // bodyDescription.Collidable.Shape = sphereIndex; + // bodyDescription.LocalInertia = sphereInertia; + // break; + //case 1: + // bodyDescription.Collidable.Shape = capsuleIndex; + // bodyDescription.LocalInertia = capsuleInertia; + // break; + //case 2: + // bodyDescription.Collidable.Shape = boxIndex; + // bodyDescription.LocalInertia = boxInertia; + // break; + //case 3: + // bodyDescription.Collidable.Shape = cylinderIndex; + // bodyDescription.LocalInertia = cylinderInertia; + // break; + //case 4: + default: bodyDescription.Collidable.Shape = hullIndex; bodyDescription.LocalInertia = hullInertia; break; diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs new file mode 100644 index 000000000..1ec7fd31d --- /dev/null +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -0,0 +1,71 @@ +using BepuUtilities; +using BepuUtilities.Collections; +using BepuUtilities.Memory; +using BepuPhysics.CollisionDetection; +using System; +using System.Collections.Generic; +using System.Diagnostics; +using System.Numerics; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; +using System.Text; +using BepuPhysics.Trees; +using DemoContentLoader; +using DemoRenderer; +using BepuPhysics; +using BepuPhysics.Constraints; + +namespace Demos.SpecializedTests +{ + public class TreeFiddlingTestDemo : Demo + { + struct OverlapHandler : IOverlapHandler + { + public int OverlapCount; + public int OverlapSum; + public int OverlapHash; + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void Handle(int indexA, int indexB) + { + ++OverlapCount; + OverlapSum += indexA + indexB; + OverlapHash = indexA * OverlapCount ^ indexB * OverlapCount * OverlapCount; + } + } + + public override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(-10, 3, -10); + camera.Yaw = MathHelper.Pi * 3f / 4; + camera.Pitch = 0; + + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(4, 1)); + var width = 128; + var height = 128; + var scale = new Vector3(1, 1, 1); + DemoMeshHelper.CreateDeformedPlane(width, height, (x, y) => new Vector3(x - width * scale.X * 0.5f, 2f * (float)(Math.Sin(x * 0.5f) * Math.Sin(y * 0.5f)), y - height * scale.Y * 0.5f), scale, BufferPool, out var mesh); + Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); + + int testCount = 1000; + var overlapHandler0 = new OverlapHandler(); + var startTime0 = Stopwatch.GetTimestamp(); + for (int i = 0; i < testCount; ++i) + mesh.Tree.GetSelfOverlaps(ref overlapHandler0); + var endTime0 = Stopwatch.GetTimestamp(); + var overlapHandler1 = new OverlapHandler(); + var startTime1 = Stopwatch.GetTimestamp(); + for (int i = 0; i < testCount; ++i) + mesh.Tree.GetSelfOverlaps2(ref overlapHandler1, BufferPool); + var endTime1 = Stopwatch.GetTimestamp(); + + Console.WriteLine($"Original time per execution (ms): {(endTime0 - startTime0) * 1e3 / Stopwatch.Frequency}"); + Console.WriteLine($"Revamped time per execution (ms): {(endTime1 - startTime1) * 1e3 / Stopwatch.Frequency}"); + + Console.WriteLine($"Original count: {overlapHandler0.OverlapCount}, sum {overlapHandler0.OverlapSum}, hash {overlapHandler0.OverlapHash}"); + Console.WriteLine($"Revamped count: {overlapHandler1.OverlapCount}, sum {overlapHandler1.OverlapSum}, hash {overlapHandler1.OverlapHash}"); + } + + + } +} From e084a8627fe2cf4e7286dce24fa583412b7cbd84 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 31 Aug 2022 16:19:58 -0500 Subject: [PATCH 577/947] Bounding box intersect is now significantly less very bad. --- BepuPhysics/Trees/Tree_IntertreeQueries.cs | 18 ++--- BepuPhysics/Trees/Tree_IntertreeQueriesMT.cs | 18 ++--- BepuPhysics/Trees/Tree_SelfQueries.cs | 68 ++++++++++++---- BepuPhysics/Trees/Tree_SelfQueriesMT.cs | 10 +-- BepuUtilities/BoundingBox.cs | 77 ++++++++++++++++--- .../SpecializedTests/TreeFiddlingTestDemo.cs | 25 +++--- 6 files changed, 153 insertions(+), 63 deletions(-) diff --git a/BepuPhysics/Trees/Tree_IntertreeQueries.cs b/BepuPhysics/Trees/Tree_IntertreeQueries.cs index ab08c69c9..4e0a535a2 100644 --- a/BepuPhysics/Trees/Tree_IntertreeQueries.cs +++ b/BepuPhysics/Trees/Tree_IntertreeQueries.cs @@ -109,10 +109,10 @@ private unsafe void GetOverlapsBetweenDifferentNodes(ref Node a ref var ab = ref a.B; ref var ba = ref b.A; ref var bb = ref b.B; - var aaIntersects = Intersects(aa, ba); - var abIntersects = Intersects(aa, bb); - var baIntersects = Intersects(ab, ba); - var bbIntersects = Intersects(ab, bb); + var aaIntersects = BoundingBox.IntersectsUnsafe(aa, ba); + var abIntersects = BoundingBox.IntersectsUnsafe(aa, bb); + var baIntersects = BoundingBox.IntersectsUnsafe(ab, ba); + var bbIntersects = BoundingBox.IntersectsUnsafe(ab, bb); if (aaIntersects) { @@ -147,8 +147,8 @@ public unsafe void GetOverlaps(ref Tree treeB, ref TOverlapHand //Tree A is degenerate; needs a special case. ref var a = ref Nodes[0]; ref var b = ref treeB.Nodes[0]; - var aaIntersects = Intersects(a.A, b.A); - var abIntersects = Intersects(a.A, b.B); + var aaIntersects = BoundingBox.IntersectsUnsafe(a.A, b.A); + var abIntersects = BoundingBox.IntersectsUnsafe(a.A, b.B); if (aaIntersects) { DispatchTestForNodes(ref a.A, ref b.A, ref treeB, ref overlapHandler); @@ -163,8 +163,8 @@ public unsafe void GetOverlaps(ref Tree treeB, ref TOverlapHand //Tree B is degenerate; needs a special case. ref var a = ref Nodes[0]; ref var b = ref treeB.Nodes[0]; - var aaIntersects = Intersects(a.A, b.A); - var baIntersects = Intersects(a.B, b.A); + var aaIntersects = BoundingBox.IntersectsUnsafe(a.A, b.A); + var baIntersects = BoundingBox.IntersectsUnsafe(a.B, b.A); if (aaIntersects) { DispatchTestForNodes(ref a.A, ref b.A, ref treeB, ref overlapHandler); @@ -177,7 +177,7 @@ public unsafe void GetOverlaps(ref Tree treeB, ref TOverlapHand else { Debug.Assert(LeafCount == 1 && treeB.LeafCount == 1); - if (Intersects(Nodes[0].A, treeB.Nodes[0].A)) + if (BoundingBox.IntersectsUnsafe(Nodes[0].A, treeB.Nodes[0].A)) { DispatchTestForNodes(ref Nodes[0].A, ref treeB.Nodes[0].A, ref treeB, ref overlapHandler); } diff --git a/BepuPhysics/Trees/Tree_IntertreeQueriesMT.cs b/BepuPhysics/Trees/Tree_IntertreeQueriesMT.cs index 896f3e1fa..e1b018f54 100644 --- a/BepuPhysics/Trees/Tree_IntertreeQueriesMT.cs +++ b/BepuPhysics/Trees/Tree_IntertreeQueriesMT.cs @@ -70,8 +70,8 @@ public unsafe void PrepareJobs(ref Tree treeA, ref Tree treeB, TOverlapHandler[] //Tree A is degenerate; needs a special case. ref var a = ref treeA.Nodes[0]; ref var b = ref treeB.Nodes[0]; - var aaIntersects = Intersects(a.A, b.A); - var abIntersects = Intersects(a.A, b.B); + var aaIntersects = BoundingBox.IntersectsUnsafe(a.A, b.A); + var abIntersects = BoundingBox.IntersectsUnsafe(a.A, b.B); if (aaIntersects) { DispatchTestForNodes(ref a.A, ref b.A, ref OverlapHandlers[0]); @@ -86,8 +86,8 @@ public unsafe void PrepareJobs(ref Tree treeA, ref Tree treeB, TOverlapHandler[] //Tree B is degenerate; needs a special case. ref var a = ref treeA.Nodes[0]; ref var b = ref treeB.Nodes[0]; - var aaIntersects = Intersects(a.A, b.A); - var baIntersects = Intersects(a.B, b.A); + var aaIntersects = BoundingBox.IntersectsUnsafe(a.A, b.A); + var baIntersects = BoundingBox.IntersectsUnsafe(a.B, b.A); if (aaIntersects) { DispatchTestForNodes(ref a.A, ref b.A, ref OverlapHandlers[0]); @@ -100,7 +100,7 @@ public unsafe void PrepareJobs(ref Tree treeA, ref Tree treeB, TOverlapHandler[] else { Debug.Assert(treeA.LeafCount == 1 && treeB.LeafCount == 1); - if (Intersects(treeA.Nodes[0].A, treeB.Nodes[0].A)) + if (BoundingBox.IntersectsUnsafe(treeA.Nodes[0].A, treeB.Nodes[0].A)) { DispatchTestForNodes(ref treeA.Nodes[0].A, ref treeB.Nodes[0].A, ref OverlapHandlers[0]); } @@ -251,10 +251,10 @@ unsafe void GetJobsBetweenDifferentNodes(ref Node a, ref Node b, ref TOverlapHan ref var ab = ref a.B; ref var ba = ref b.A; ref var bb = ref b.B; - var aaIntersects = Intersects(aa, ba); - var abIntersects = Intersects(aa, bb); - var baIntersects = Intersects(ab, ba); - var bbIntersects = Intersects(ab, bb); + var aaIntersects = BoundingBox.IntersectsUnsafe(aa, ba); + var abIntersects = BoundingBox.IntersectsUnsafe(aa, bb); + var baIntersects = BoundingBox.IntersectsUnsafe(ab, ba); + var bbIntersects = BoundingBox.IntersectsUnsafe(ab, bb); if (aaIntersects) { diff --git a/BepuPhysics/Trees/Tree_SelfQueries.cs b/BepuPhysics/Trees/Tree_SelfQueries.cs index 93a5d9bc0..39363f6ae 100644 --- a/BepuPhysics/Trees/Tree_SelfQueries.cs +++ b/BepuPhysics/Trees/Tree_SelfQueries.cs @@ -7,6 +7,8 @@ using System.Numerics; using System.Runtime.CompilerServices; using System.Runtime.InteropServices; +using System.Runtime.Intrinsics; +using System.Runtime.Intrinsics.X86; namespace BepuPhysics.Trees { @@ -86,11 +88,43 @@ unsafe void DispatchTestForNodes(ref NodeChild a, ref NodeChild } } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - static bool Intersects(in NodeChild a, in NodeChild b) - { - return BoundingBox.Intersects(a.Min, a.Max, b.Min, b.Max); - } + + ///// + ///// Intersects two node children. + ///// The referenced node children must not be in unpinned managed memory. + ///// + ///// First child to compare. + ///// Second child to compare. + ///// True if the children overlap, false otherwise. + //[MethodImpl(MethodImplOptions.AggressiveInlining)] + //unsafe static bool Intersects(in NodeChild childA, in NodeChild childB) + //{ + // return BoundingBox.Intersects(childA, childB); + // //if (Vector256.IsHardwareAccelerated && Avx.IsSupported) + // //{ + // // var a = Vector256.LoadUnsafe(ref Unsafe.AsRef(childA.Min.X)); + // // var b = Vector256.LoadUnsafe(ref Unsafe.AsRef(childB.Min.X)); + // // var min = Avx.Permute2x128(a, b, (0) | (2 << 4)); //(aMin, aMax) (bMin, bMax) -> (aMin, bMin) + // // var max = Avx.Permute2x128(a, b, (3) | (1 << 4)); //(aMin, aMax) (bMin, bMax) -> (bMax, aMax) + // // var noIntersection = Vector256.LessThan(max, min); + // // return (Vector256.ExtractMostSignificantBits(noIntersection) & 0b1110111) == 0; + // //} + // //else + // //if (Vector128.IsHardwareAccelerated) + // //{ + // // //THIS IS A POTENTIAL GC HOLE IF CHILDREN ARE PASSED FROM UNPINNED MANAGED MEMORY + // // var aMin = Vector128.LoadUnsafe(ref Unsafe.AsRef(childA.Min.X)); + // // var aMax = Vector128.LoadUnsafe(ref Unsafe.AsRef(childA.Max.X)); + // // var bMin = Vector128.LoadUnsafe(ref Unsafe.AsRef(childB.Min.X)); + // // var bMax = Vector128.LoadUnsafe(ref Unsafe.AsRef(childB.Max.X)); + // // var noIntersectionOnAxes = Vector128.LessThan(aMax, bMin) | Vector128.LessThan(bMax, aMin); + // // return (Vector128.ExtractMostSignificantBits(noIntersectionOnAxes) & 0b111) == 0; + // //} + // //else + // { + // return BoundingBox.Intersects(childA.Min, childA.Max, childB.Min, childB.Max); + // } + //} unsafe void GetOverlapsBetweenDifferentNodes(ref Node a, ref Node b, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler { @@ -99,10 +133,10 @@ unsafe void GetOverlapsBetweenDifferentNodes(ref Node a, ref No ref var ab = ref a.B; ref var ba = ref b.A; ref var bb = ref b.B; - var aaIntersects = Intersects(aa, ba); - var abIntersects = Intersects(aa, bb); - var baIntersects = Intersects(ab, ba); - var bbIntersects = Intersects(ab, bb); + var aaIntersects = BoundingBox.IntersectsUnsafe(aa, ba); + var abIntersects = BoundingBox.IntersectsUnsafe(aa, bb); + var baIntersects = BoundingBox.IntersectsUnsafe(ab, ba); + var bbIntersects = BoundingBox.IntersectsUnsafe(ab, bb); if (aaIntersects) { @@ -128,7 +162,7 @@ unsafe void GetOverlapsInNode(ref Node node, ref TOverlapHandle ref var a = ref node.A; ref var b = ref node.B; - var ab = Intersects(a, b); + var ab = BoundingBox.IntersectsUnsafe(a, b); if (a.Index >= 0) GetOverlapsInNode(ref Nodes[a.Index], ref results); @@ -166,8 +200,8 @@ unsafe void GetOverlapsWithLeaf(ref TOverlapHandler results, No while (true) { ref var node = ref Nodes[nodeToTest]; - var a = Intersects(leaf, node.A); - var b = Intersects(leaf, node.B); + var a = BoundingBox.IntersectsUnsafe(leaf, node.A); + var b = BoundingBox.IntersectsUnsafe(leaf, node.B); var aIsInternal = node.A.Index >= 0; var bIsInternal = node.B.Index >= 0; var intersectedInternalA = a && aIsInternal; @@ -231,7 +265,7 @@ public unsafe void GetSelfOverlaps2(ref TOverlapHandler results //Note that we never need to push an entry with differing A and B indices to the stack *from a self test*. There can only be one such entry created from any self test, and it's always visited next. //Non-self tests will generate *only* results with differing A and B indices. ref var node = ref Nodes[nextTest.A]; - var abIntersect = Intersects(node.A, node.B); + var abIntersect = BoundingBox.IntersectsUnsafe(node.A, node.B); var aIsInternal = node.A.Index >= 0; var bIsInternal = node.B.Index >= 0; if (abIntersect && aIsInternal && bIsInternal) @@ -321,10 +355,10 @@ public unsafe void GetSelfOverlaps2(ref TOverlapHandler results //4) BB intersection, both internal ref var n0 = ref Nodes[nextTest.A]; ref var n1 = ref Nodes[nextTest.B]; - var aaIntersects = Intersects(n0.A, n1.A); - var abIntersects = Intersects(n0.A, n1.B); - var baIntersects = Intersects(n0.B, n1.A); - var bbIntersects = Intersects(n0.B, n1.B); + var aaIntersects = BoundingBox.IntersectsUnsafe(n0.A, n1.A); + var abIntersects = BoundingBox.IntersectsUnsafe(n0.A, n1.B); + var baIntersects = BoundingBox.IntersectsUnsafe(n0.B, n1.A); + var bbIntersects = BoundingBox.IntersectsUnsafe(n0.B, n1.B); var n0AIsInternal = n0.A.Index >= 0; var n0BIsInternal = n0.B.Index >= 0; var n1AIsInternal = n1.A.Index >= 0; diff --git a/BepuPhysics/Trees/Tree_SelfQueriesMT.cs b/BepuPhysics/Trees/Tree_SelfQueriesMT.cs index fc879af20..ba3b3153a 100644 --- a/BepuPhysics/Trees/Tree_SelfQueriesMT.cs +++ b/BepuPhysics/Trees/Tree_SelfQueriesMT.cs @@ -229,10 +229,10 @@ unsafe void GetJobsBetweenDifferentNodes(ref Node a, ref Node b, ref TOverlapHan ref var ab = ref a.B; ref var ba = ref b.A; ref var bb = ref b.B; - var aaIntersects = Intersects(aa, ba); - var abIntersects = Intersects(aa, bb); - var baIntersects = Intersects(ab, ba); - var bbIntersects = Intersects(ab, bb); + var aaIntersects = BoundingBox.IntersectsUnsafe(aa, ba); + var abIntersects = BoundingBox.IntersectsUnsafe(aa, bb); + var baIntersects = BoundingBox.IntersectsUnsafe(ab, ba); + var bbIntersects = BoundingBox.IntersectsUnsafe(ab, bb); if (aaIntersects) { @@ -265,7 +265,7 @@ unsafe void CollectJobsInNode(int nodeIndex, int leafCount, ref TOverlapHandler ref var a = ref node.A; ref var b = ref node.B; - var ab = Intersects(a, b); + var ab = BoundingBox.IntersectsUnsafe(a, b); if (a.Index >= 0) CollectJobsInNode(a.Index, a.LeafCount, ref results); diff --git a/BepuUtilities/BoundingBox.cs b/BepuUtilities/BoundingBox.cs index f7865c344..53a9744fd 100644 --- a/BepuUtilities/BoundingBox.cs +++ b/BepuUtilities/BoundingBox.cs @@ -1,9 +1,12 @@ using System; using System.Collections.Generic; +using System.Diagnostics; using System.Linq; using System.Numerics; using System.Runtime.CompilerServices; using System.Runtime.InteropServices; +using System.Runtime.Intrinsics; +using System.Runtime.Intrinsics.X86; using System.Text; namespace BepuUtilities @@ -12,7 +15,7 @@ namespace BepuUtilities /// Provides XNA-like axis-aligned bounding box functionality. /// //NOTE: The explicit size avoids https://github.com/dotnet/coreclr/issues/12950 - [StructLayout(LayoutKind.Explicit, Size = 32)] + [StructLayout(LayoutKind.Explicit, Size = 32)] public struct BoundingBox { /// @@ -39,6 +42,50 @@ public BoundingBox(Vector3 min, Vector3 max) this.Max = max; } + /// + /// Checks if two structures with memory layouts equivalent to the intersect. + /// The referenced values must not be in unpinned managed memory. + /// + /// First child to compare. + /// Second child to compare. + /// True if the children overlap, false otherwise. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public unsafe static bool IntersectsUnsafe(in TA childA, in TB childB) where TA : unmanaged where TB : unmanaged + { + //This is a weird function. We're directly interpreting the memory of an incoming type as a vector where we assume the min/max layout matches the BoundingBox. + //Happens to be convenient! + Debug.Assert(Unsafe.SizeOf() == 32 && Unsafe.SizeOf() == 32); + //AVX codepath is not helpful in my tests. + //if (Vector256.IsHardwareAccelerated && Avx.IsSupported) + //{ + // var a = Vector256.LoadUnsafe(ref Unsafe.As(ref Unsafe.AsRef(childA))); + // var b = Vector256.LoadUnsafe(ref Unsafe.As(ref Unsafe.AsRef(childB))); + // var min = Avx.Permute2x128(a, b, (0) | (2 << 4)); //(aMin, aMax) (bMin, bMax) -> (aMin, bMin) + // var max = Avx.Permute2x128(a, b, (3) | (1 << 4)); //(aMin, aMax) (bMin, bMax) -> (bMax, aMax) + // var noIntersection = Vector256.LessThan(max, min); + // return (Vector256.ExtractMostSignificantBits(noIntersection) & 0b1110111) == 0; + //} + //else + if (Vector128.IsHardwareAccelerated) + { + //THIS IS A POTENTIAL GC HOLE IF CHILDREN ARE PASSED FROM UNPINNED MANAGED MEMORY + ref var a = ref Unsafe.As(ref Unsafe.AsRef(childA)); + ref var b = ref Unsafe.As(ref Unsafe.AsRef(childB)); + var aMin = Vector128.LoadUnsafe(ref a); + var aMax = Vector128.LoadUnsafe(ref Unsafe.Add(ref a, 4)); + var bMin = Vector128.LoadUnsafe(ref b); + var bMax = Vector128.LoadUnsafe(ref Unsafe.Add(ref b, 4)); + var noIntersectionOnAxes = Vector128.LessThan(aMax, bMin) | Vector128.LessThan(bMax, aMin); + return (Vector128.ExtractMostSignificantBits(noIntersectionOnAxes) & 0b111) == 0; + } + else + { + var a = (float*)Unsafe.AsPointer(ref Unsafe.AsRef(childA)); + var b = (float*)Unsafe.AsPointer(ref Unsafe.AsRef(childB)); + return a[4] >= b[0] & a[5] >= b[1] & a[6] >= b[2] & + b[4] >= a[0] & b[5] >= a[1] & b[6] >= a[2]; + } + } /// /// Determines if a bounding box intersects another bounding box. @@ -46,22 +93,30 @@ public BoundingBox(Vector3 min, Vector3 max) /// First bounding box to test. /// Second bounding box to test. /// Whether the bounding boxes intersected. + /// When possible, prefer using the variant for slightly better performance. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static bool Intersects(in BoundingBox a, in BoundingBox b) + public unsafe static bool Intersects(BoundingBox a, BoundingBox b) { - return Intersects(a.Min, a.Max, b.Min, b.Max); + return IntersectsUnsafe(a, b); } - //TODO: At some point in the past, intersection was found to be faster with non-short circuiting operators. - //While that does make some sense (the branches aren't really valuable relative to their cost), it's still questionable enough that it should be reevaluated on a modern compiler. + /// /// Determines if a bounding box intersects another bounding box. /// - /// First bounding box to test. - /// Second bounding box to test. + /// Minimum bounds of bounding box A. + /// Maximum bounds of bounding box A. + /// Minimum bounds of bounding box B. + /// Maximum bounds of bounding box B. /// Whether the bounding boxes intersected. + /// When possible, prefer using the variant for slightly better performance. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static bool Intersects(Vector3 minA, Vector3 maxA, Vector3 minB, Vector3 maxB) { + if (Vector128.IsHardwareAccelerated) + { + var noIntersectionOnAxes = Vector128.LessThan(maxA.AsVector128(), minB.AsVector128()) | Vector128.LessThan(maxB.AsVector128(), minA.AsVector128()); + return (Vector128.ExtractMostSignificantBits(noIntersectionOnAxes) & 0b111) == 0; + } return maxA.X >= minB.X & maxA.Y >= minB.Y & maxA.Z >= minB.Z & maxB.X >= minA.X & maxB.Y >= minA.Y & maxB.Z >= minA.Z; } @@ -142,14 +197,14 @@ public ContainmentType Contains(ref BoundingBox boundingBox) /// /// Points to enclose with a bounding box. /// Bounding box which contains the list of points. - public static BoundingBox CreateFromPoints(IList points) + public static BoundingBox CreateFromPoints(ReadOnlySpan points) { BoundingBox aabb; - if (points.Count == 0) + if (points.Length == 0) throw new Exception("Cannot construct a bounding box from an empty list."); aabb.Min = points[0]; aabb.Max = aabb.Min; - for (int i = points.Count - 1; i >= 1; i--) + for (int i = points.Length - 1; i >= 1; i--) { aabb.Min = Vector3.Min(points[i], aabb.Min); aabb.Max = Vector3.Max(points[i], aabb.Max); @@ -179,7 +234,7 @@ public static void CreateFromSphere(ref BoundingSphere boundingSphere, out Bound /// String representation of the bounding box. public override string ToString() { - return $"({Min.ToString()}, {Max.ToString()})"; + return $"({Min}, {Max})"; } } diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index 1ec7fd31d..d78249edf 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -48,22 +48,23 @@ public override void Initialize(ContentArchive content, Camera camera) Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); int testCount = 1000; - var overlapHandler0 = new OverlapHandler(); - var startTime0 = Stopwatch.GetTimestamp(); + var overlapHandlerNew = new OverlapHandler(); + var startTimeNew = Stopwatch.GetTimestamp(); for (int i = 0; i < testCount; ++i) - mesh.Tree.GetSelfOverlaps(ref overlapHandler0); - var endTime0 = Stopwatch.GetTimestamp(); - var overlapHandler1 = new OverlapHandler(); - var startTime1 = Stopwatch.GetTimestamp(); + mesh.Tree.GetSelfOverlaps2(ref overlapHandlerNew, BufferPool); + var endTimeNew = Stopwatch.GetTimestamp(); + + var overlapHandlerOld = new OverlapHandler(); + var startTimeOld = Stopwatch.GetTimestamp(); for (int i = 0; i < testCount; ++i) - mesh.Tree.GetSelfOverlaps2(ref overlapHandler1, BufferPool); - var endTime1 = Stopwatch.GetTimestamp(); + mesh.Tree.GetSelfOverlaps(ref overlapHandlerOld); + var endTimeOld = Stopwatch.GetTimestamp(); - Console.WriteLine($"Original time per execution (ms): {(endTime0 - startTime0) * 1e3 / Stopwatch.Frequency}"); - Console.WriteLine($"Revamped time per execution (ms): {(endTime1 - startTime1) * 1e3 / Stopwatch.Frequency}"); + Console.WriteLine($"Revamped time per execution (ms): {(endTimeNew - startTimeNew) * 1e3 / (testCount * Stopwatch.Frequency)}"); + Console.WriteLine($"Original time per execution (ms): {(endTimeOld - startTimeOld) * 1e3 / (testCount * Stopwatch.Frequency)}"); - Console.WriteLine($"Original count: {overlapHandler0.OverlapCount}, sum {overlapHandler0.OverlapSum}, hash {overlapHandler0.OverlapHash}"); - Console.WriteLine($"Revamped count: {overlapHandler1.OverlapCount}, sum {overlapHandler1.OverlapSum}, hash {overlapHandler1.OverlapHash}"); + Console.WriteLine($"Revamped count: {overlapHandlerNew.OverlapCount}, sum {overlapHandlerNew.OverlapSum}, hash {overlapHandlerNew.OverlapHash}"); + Console.WriteLine($"Original count: {overlapHandlerOld.OverlapCount}, sum {overlapHandlerOld.OverlapSum}, hash {overlapHandlerOld.OverlapHash}"); } From a21b6e4ea285c380112a50e2501724e26f17d56f Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 31 Aug 2022 16:52:36 -0500 Subject: [PATCH 578/947] IntersectsUnsafe replaces Intersects most places. --- BepuPhysics/Collidables/Compound.cs | 1 + BepuPhysics/Trees/Tree_IntertreeQueries.cs | 32 ++++++++++---------- BepuPhysics/Trees/Tree_IntertreeQueriesMT.cs | 22 +++++++------- BepuPhysics/Trees/Tree_SelfQueries.cs | 18 +++++------ BepuPhysics/Trees/Tree_SelfQueriesMT.cs | 22 +++++++------- BepuPhysics/Trees/Tree_VolumeQuery.cs | 16 +++++----- 6 files changed, 56 insertions(+), 55 deletions(-) diff --git a/BepuPhysics/Collidables/Compound.cs b/BepuPhysics/Collidables/Compound.cs index c65e82f64..58c1ccdb2 100644 --- a/BepuPhysics/Collidables/Compound.cs +++ b/BepuPhysics/Collidables/Compound.cs @@ -308,6 +308,7 @@ public unsafe void FindLocalOverlaps(ref Buffer(int leafIndex, ref Vector3 leafMin, ref Vector3 leafMax, int nodeIndex, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler + unsafe void DispatchTestForNodeAgainstLeaf(int leafIndex, ref NodeChild leafChild, int nodeIndex, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler { if (nodeIndex < 0) { @@ -17,10 +17,10 @@ unsafe void DispatchTestForNodeAgainstLeaf(int leafIndex, ref V } else { - TestNodeAgainstLeaf(nodeIndex, leafIndex, ref leafMin, ref leafMax, ref results); + TestNodeAgainstLeaf(nodeIndex, leafIndex, ref leafChild, ref results); } } - private unsafe void TestNodeAgainstLeaf(int nodeIndex, int leafIndex, ref Vector3 leafMin, ref Vector3 leafMax, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler + private unsafe void TestNodeAgainstLeaf(int nodeIndex, int leafIndex, ref NodeChild leafChild, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler { ref var node = ref Nodes[nodeIndex]; ref var a = ref node.A; @@ -30,19 +30,19 @@ private unsafe void TestNodeAgainstLeaf(int nodeIndex, int leaf //Reloading that in the event of eviction would require more work than keeping the derived data on the stack. //TODO: this is some pretty questionable microtuning. It's not often that the post-leaf-found recursion will be long enough to evict L1. Definitely test it. var bIndex = b.Index; - var aIntersects = BoundingBox.Intersects(leafMin, leafMax, a.Min, a.Max); - var bIntersects = BoundingBox.Intersects(leafMin, leafMax, b.Min, b.Max); + var aIntersects = BoundingBox.IntersectsUnsafe(leafChild, a); + var bIntersects = BoundingBox.IntersectsUnsafe(leafChild, b); if (aIntersects) { - DispatchTestForNodeAgainstLeaf(leafIndex, ref leafMin, ref leafMax, a.Index, ref results); + DispatchTestForNodeAgainstLeaf(leafIndex, ref leafChild, a.Index, ref results); } if (bIntersects) { - DispatchTestForNodeAgainstLeaf(leafIndex, ref leafMin, ref leafMax, bIndex, ref results); + DispatchTestForNodeAgainstLeaf(leafIndex, ref leafChild, bIndex, ref results); } } - unsafe void DispatchTestForLeafAgainstNode(int leafIndex, ref Vector3 leafMin, ref Vector3 leafMax, int nodeIndex, ref Tree treeB, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler + unsafe void DispatchTestForLeafAgainstNode(int leafIndex, ref NodeChild leafChild, int nodeIndex, ref Tree treeB, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler { if (nodeIndex < 0) { @@ -50,10 +50,10 @@ unsafe void DispatchTestForLeafAgainstNode(int leafIndex, ref V } else { - TestLeafAgainstNode(leafIndex, ref leafMin, ref leafMax, nodeIndex, ref treeB, ref results); + TestLeafAgainstNode(leafIndex, ref leafChild, nodeIndex, ref treeB, ref results); } } - unsafe void TestLeafAgainstNode(int leafIndex, ref Vector3 leafMin, ref Vector3 leafMax, int nodeIndex, ref Tree treeB, ref TOverlapHandler results) + unsafe void TestLeafAgainstNode(int leafIndex, ref NodeChild leafChild, int nodeIndex, ref Tree treeB, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler { ref var node = ref treeB.Nodes[nodeIndex]; @@ -64,15 +64,15 @@ unsafe void TestLeafAgainstNode(int leafIndex, ref Vector3 leaf //Reloading that in the event of eviction would require more work than keeping the derived data on the stack. //TODO: this is some pretty questionable microtuning. It's not often that the post-leaf-found recursion will be long enough to evict L1. Definitely test it. var bIndex = b.Index; - var aIntersects = BoundingBox.Intersects(leafMin, leafMax, a.Min, a.Max); - var bIntersects = BoundingBox.Intersects(leafMin, leafMax, b.Min, b.Max); + var aIntersects = BoundingBox.IntersectsUnsafe(leafChild, a); + var bIntersects = BoundingBox.IntersectsUnsafe(leafChild, b); if (aIntersects) { - DispatchTestForLeafAgainstNode(leafIndex, ref leafMin, ref leafMax, a.Index, ref treeB, ref results); + DispatchTestForLeafAgainstNode(leafIndex, ref leafChild, a.Index, ref treeB, ref results); } if (bIntersects) { - DispatchTestForLeafAgainstNode(leafIndex, ref leafMin, ref leafMax, bIndex, ref treeB, ref results); + DispatchTestForLeafAgainstNode(leafIndex, ref leafChild, bIndex, ref treeB, ref results); } } @@ -88,13 +88,13 @@ unsafe void DispatchTestForNodes(ref NodeChild a, ref NodeChild else { //leaf B versus node A. Note that we have to maintain order; treeB nodes always should be in the second slot. - TestNodeAgainstLeaf(a.Index, Encode(b.Index), ref b.Min, ref b.Max, ref results); + TestNodeAgainstLeaf(a.Index, Encode(b.Index), ref b, ref results); } } else if (b.Index >= 0) { //leaf A versus node B. Note that we have to maintain order; treeB nodes always should be in the second slot. - TestLeafAgainstNode(Encode(a.Index), ref a.Min, ref a.Max, b.Index, ref treeB, ref results); + TestLeafAgainstNode(Encode(a.Index), ref a, b.Index, ref treeB, ref results); } else { diff --git a/BepuPhysics/Trees/Tree_IntertreeQueriesMT.cs b/BepuPhysics/Trees/Tree_IntertreeQueriesMT.cs index e1b018f54..de22b26ef 100644 --- a/BepuPhysics/Trees/Tree_IntertreeQueriesMT.cs +++ b/BepuPhysics/Trees/Tree_IntertreeQueriesMT.cs @@ -134,7 +134,7 @@ public unsafe void ExecuteJob(int jobIndex, int workerIndex) var leafIndex = Encode(overlap.B); ref var leaf = ref TreeB.Leaves[leafIndex]; ref var childOwningLeaf = ref Unsafe.Add(ref TreeB.Nodes[leaf.NodeIndex].A, leaf.ChildIndex); - TreeA.TestNodeAgainstLeaf(overlap.A, leafIndex, ref childOwningLeaf.Min, ref childOwningLeaf.Max, ref OverlapHandlers[workerIndex]); + TreeA.TestNodeAgainstLeaf(overlap.A, leafIndex, ref childOwningLeaf, ref OverlapHandlers[workerIndex]); } } else @@ -143,7 +143,7 @@ public unsafe void ExecuteJob(int jobIndex, int workerIndex) var leafIndex = Encode(overlap.A); ref var leaf = ref TreeA.Leaves[leafIndex]; ref var childOwningLeaf = ref Unsafe.Add(ref TreeA.Nodes[leaf.NodeIndex].A, leaf.ChildIndex); - TreeA.TestLeafAgainstNode(leafIndex, ref childOwningLeaf.Min, ref childOwningLeaf.Max, overlap.B, ref TreeB, ref OverlapHandlers[workerIndex]); + TreeA.TestLeafAgainstNode(leafIndex, ref childOwningLeaf, overlap.B, ref TreeB, ref OverlapHandlers[workerIndex]); //NOTE THAT WE DO NOT HANDLE THE CASE THAT BOTH A AND B ARE LEAVES HERE. //The collection routine should take care of that, since it has more convenient access to bounding boxes and because a single test isn't worth an atomic increment. @@ -164,7 +164,7 @@ public unsafe void PairTest(int workerIndex) } } - unsafe void DispatchTestForLeaf(ref Tree nodeOwner, int leafIndex, ref Vector3 leafMin, ref Vector3 leafMax, int nodeIndex, int nodeLeafCount, ref TOverlapHandler results) + unsafe void DispatchTestForLeaf(ref Tree nodeOwner, int leafIndex, ref NodeChild leafChild, int nodeIndex, int nodeLeafCount, ref TOverlapHandler results) { if (nodeIndex < 0) { @@ -185,11 +185,11 @@ unsafe void DispatchTestForLeaf(ref Tree nodeOwner, int leafIndex, ref Vector3 l jobs.Add(new Job { A = Encode(leafIndex), B = nodeIndex }, Pool); } else - TestLeafAgainstNode(ref nodeOwner, leafIndex, ref leafMin, ref leafMax, nodeIndex, ref results); + TestLeafAgainstNode(ref nodeOwner, leafIndex, ref leafChild, nodeIndex, ref results); } } - unsafe void TestLeafAgainstNode(ref Tree nodeOwner, int leafIndex, ref Vector3 leafMin, ref Vector3 leafMax, int nodeIndex, ref TOverlapHandler results) + unsafe void TestLeafAgainstNode(ref Tree nodeOwner, int leafIndex, ref NodeChild leafChild, int nodeIndex, ref TOverlapHandler results) { ref var node = ref nodeOwner.Nodes[nodeIndex]; ref var a = ref node.A; @@ -200,15 +200,15 @@ unsafe void TestLeafAgainstNode(ref Tree nodeOwner, int leafIndex, ref Vector3 l //TODO: this is some pretty questionable microtuning. It's not often that the post-leaf-found recursion will be long enough to evict L1. Definitely test it. var bIndex = b.Index; var bLeafCount = b.LeafCount; - var aIntersects = BoundingBox.Intersects(leafMin, leafMax, a.Min, a.Max); - var bIntersects = BoundingBox.Intersects(leafMin, leafMax, b.Min, b.Max); + var aIntersects = BoundingBox.IntersectsUnsafe(leafChild, a); + var bIntersects = BoundingBox.IntersectsUnsafe(leafChild, b); if (aIntersects) { - DispatchTestForLeaf(ref nodeOwner, leafIndex, ref leafMin, ref leafMax, a.Index, a.LeafCount, ref results); + DispatchTestForLeaf(ref nodeOwner, leafIndex, ref leafChild, a.Index, a.LeafCount, ref results); } if (bIntersects) { - DispatchTestForLeaf(ref nodeOwner, leafIndex, ref leafMin, ref leafMax, bIndex, bLeafCount, ref results); + DispatchTestForLeaf(ref nodeOwner, leafIndex, ref leafChild, bIndex, bLeafCount, ref results); } } @@ -228,13 +228,13 @@ unsafe void DispatchTestForNodes(ref NodeChild a, ref NodeChild b, ref TOverlapH else { //leaf B versus node A. - TestLeafAgainstNode(ref TreeA, Encode(b.Index), ref b.Min, ref b.Max, a.Index, ref results); + TestLeafAgainstNode(ref TreeA, Encode(b.Index), ref b, a.Index, ref results); } } else if (b.Index >= 0) { //leaf A versus node B. - TestLeafAgainstNode(ref TreeB, Encode(a.Index), ref a.Min, ref a.Max, b.Index, ref results); + TestLeafAgainstNode(ref TreeB, Encode(a.Index), ref a, b.Index, ref results); } else { diff --git a/BepuPhysics/Trees/Tree_SelfQueries.cs b/BepuPhysics/Trees/Tree_SelfQueries.cs index 39363f6ae..8ce4667b9 100644 --- a/BepuPhysics/Trees/Tree_SelfQueries.cs +++ b/BepuPhysics/Trees/Tree_SelfQueries.cs @@ -27,7 +27,7 @@ partial struct Tree //Note that all of these implementations make use of a fully generic handler. It could be dumping to a list, or it could be directly processing the results- at this //level of abstraction we don't know or care. It's up to the user to use a handler which maximizes performance if they want it. We'll be using this in the broad phase. - unsafe void DispatchTestForLeaf(int leafIndex, ref Vector3 leafMin, ref Vector3 leafMax, int nodeIndex, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler + unsafe void DispatchTestForLeaf(int leafIndex, ref NodeChild leafChild, int nodeIndex, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler { if (nodeIndex < 0) { @@ -35,10 +35,10 @@ unsafe void DispatchTestForLeaf(int leafIndex, ref Vector3 leaf } else { - TestLeafAgainstNode(leafIndex, ref leafMin, ref leafMax, nodeIndex, ref results); + TestLeafAgainstNode(leafIndex, ref leafChild, nodeIndex, ref results); } } - unsafe void TestLeafAgainstNode(int leafIndex, ref Vector3 leafMin, ref Vector3 leafMax, int nodeIndex, ref TOverlapHandler results) + unsafe void TestLeafAgainstNode(int leafIndex, ref NodeChild leafChild, int nodeIndex, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler { ref var node = ref Nodes[nodeIndex]; @@ -49,15 +49,15 @@ unsafe void TestLeafAgainstNode(int leafIndex, ref Vector3 leaf //Reloading that in the event of eviction would require more work than keeping the derived data on the stack. //TODO: this is some pretty questionable microtuning. It's not often that the post-leaf-found recursion will be long enough to evict L1. Definitely test it. var bIndex = b.Index; - var aIntersects = BoundingBox.Intersects(leafMin, leafMax, a.Min, a.Max); - var bIntersects = BoundingBox.Intersects(leafMin, leafMax, b.Min, b.Max); + var aIntersects = BoundingBox.IntersectsUnsafe(leafChild, a); + var bIntersects = BoundingBox.IntersectsUnsafe(leafChild, b); if (aIntersects) { - DispatchTestForLeaf(leafIndex, ref leafMin, ref leafMax, a.Index, ref results); + DispatchTestForLeaf(leafIndex, ref leafChild, a.Index, ref results); } if (bIntersects) { - DispatchTestForLeaf(leafIndex, ref leafMin, ref leafMax, bIndex, ref results); + DispatchTestForLeaf(leafIndex, ref leafChild, bIndex, ref results); } } @@ -73,13 +73,13 @@ unsafe void DispatchTestForNodes(ref NodeChild a, ref NodeChild else { //leaf B versus node A. - TestLeafAgainstNode(Encode(b.Index), ref b.Min, ref b.Max, a.Index, ref results); + TestLeafAgainstNode(Encode(b.Index), ref b, a.Index, ref results); } } else if (b.Index >= 0) { //leaf A versus node B. - TestLeafAgainstNode(Encode(a.Index), ref a.Min, ref a.Max, b.Index, ref results); + TestLeafAgainstNode(Encode(a.Index), ref a, b.Index, ref results); } else { diff --git a/BepuPhysics/Trees/Tree_SelfQueriesMT.cs b/BepuPhysics/Trees/Tree_SelfQueriesMT.cs index ba3b3153a..d08ead631 100644 --- a/BepuPhysics/Trees/Tree_SelfQueriesMT.cs +++ b/BepuPhysics/Trees/Tree_SelfQueriesMT.cs @@ -122,7 +122,7 @@ public unsafe void ExecuteJob(int jobIndex, int workerIndex) var leafIndex = Encode(overlap.B); ref var leaf = ref Tree.Leaves[leafIndex]; ref var childOwningLeaf = ref Unsafe.Add(ref Tree.Nodes[leaf.NodeIndex].A, leaf.ChildIndex); - Tree.TestLeafAgainstNode(leafIndex, ref childOwningLeaf.Min, ref childOwningLeaf.Max, overlap.A, ref OverlapHandlers[workerIndex]); + Tree.TestLeafAgainstNode(leafIndex, ref childOwningLeaf, overlap.A, ref OverlapHandlers[workerIndex]); } } else @@ -131,7 +131,7 @@ public unsafe void ExecuteJob(int jobIndex, int workerIndex) var leafIndex = Encode(overlap.A); ref var leaf = ref Tree.Leaves[leafIndex]; ref var childOwningLeaf = ref Unsafe.Add(ref Tree.Nodes[leaf.NodeIndex].A, leaf.ChildIndex); - Tree.TestLeafAgainstNode(leafIndex, ref childOwningLeaf.Min, ref childOwningLeaf.Max, overlap.B, ref OverlapHandlers[workerIndex]); + Tree.TestLeafAgainstNode(leafIndex, ref childOwningLeaf, overlap.B, ref OverlapHandlers[workerIndex]); //NOTE THAT WE DO NOT HANDLE THE CASE THAT BOTH A AND B ARE LEAVES HERE. //The collection routine should take care of that, since it has more convenient access to bounding boxes and because a single test isn't worth an atomic increment. @@ -152,7 +152,7 @@ public unsafe void PairTest(int workerIndex) } } - unsafe void DispatchTestForLeaf(int leafIndex, ref Vector3 leafMin, ref Vector3 leafMax, int nodeIndex, int nodeLeafCount, ref TOverlapHandler results) + unsafe void DispatchTestForLeaf(int leafIndex, ref NodeChild leafChild, int nodeIndex, int nodeLeafCount, ref TOverlapHandler results) { if (nodeIndex < 0) { @@ -163,11 +163,11 @@ unsafe void DispatchTestForLeaf(int leafIndex, ref Vector3 leafMin, ref Vector3 if (nodeLeafCount <= leafThreshold) jobs.Add(new Job { A = Encode(leafIndex), B = nodeIndex }, Pool); else - TestLeafAgainstNode(leafIndex, ref leafMin, ref leafMax, nodeIndex, ref results); + TestLeafAgainstNode(leafIndex, ref leafChild, nodeIndex, ref results); } } - unsafe void TestLeafAgainstNode(int leafIndex, ref Vector3 leafMin, ref Vector3 leafMax, int nodeIndex, ref TOverlapHandler results) + unsafe void TestLeafAgainstNode(int leafIndex, ref NodeChild leafChild, int nodeIndex, ref TOverlapHandler results) { ref var node = ref Tree.Nodes[nodeIndex]; ref var a = ref node.A; @@ -178,15 +178,15 @@ unsafe void TestLeafAgainstNode(int leafIndex, ref Vector3 leafMin, ref Vector3 //TODO: this is some pretty questionable microtuning. It's not often that the post-leaf-found recursion will be long enough to evict L1. Definitely test it. var bIndex = b.Index; var bLeafCount = b.LeafCount; - var aIntersects = BoundingBox.Intersects(leafMin, leafMax, a.Min, a.Max); - var bIntersects = BoundingBox.Intersects(leafMin, leafMax, b.Min, b.Max); + var aIntersects = BoundingBox.IntersectsUnsafe(leafChild, a); + var bIntersects = BoundingBox.IntersectsUnsafe(leafChild, b); if (aIntersects) { - DispatchTestForLeaf(leafIndex, ref leafMin, ref leafMax, a.Index, a.LeafCount, ref results); + DispatchTestForLeaf(leafIndex, ref leafChild, a.Index, a.LeafCount, ref results); } if (bIntersects) { - DispatchTestForLeaf(leafIndex, ref leafMin, ref leafMax, bIndex, bLeafCount, ref results); + DispatchTestForLeaf(leafIndex, ref leafChild, bIndex, bLeafCount, ref results); } } @@ -206,13 +206,13 @@ unsafe void DispatchTestForNodes(ref NodeChild a, ref NodeChild b, ref TOverlapH else { //leaf B versus node A. - TestLeafAgainstNode(Encode(b.Index), ref b.Min, ref b.Max, a.Index, ref results); + TestLeafAgainstNode(Encode(b.Index), ref b, a.Index, ref results); } } else if (b.Index >= 0) { //leaf A versus node B. - TestLeafAgainstNode(Encode(a.Index), ref a.Min, ref a.Max, b.Index, ref results); + TestLeafAgainstNode(Encode(a.Index), ref a, b.Index, ref results); } else { diff --git a/BepuPhysics/Trees/Tree_VolumeQuery.cs b/BepuPhysics/Trees/Tree_VolumeQuery.cs index 162b697d4..8a9b3b416 100644 --- a/BepuPhysics/Trees/Tree_VolumeQuery.cs +++ b/BepuPhysics/Trees/Tree_VolumeQuery.cs @@ -7,7 +7,7 @@ namespace BepuPhysics.Trees { partial struct Tree { - unsafe readonly void GetOverlaps(int nodeIndex, Vector3 min, Vector3 max, int* stack, ref TEnumerator leafEnumerator) where TEnumerator : IBreakableForEach + unsafe readonly void GetOverlaps(int nodeIndex, BoundingBox boundingBox, int* stack, ref TEnumerator leafEnumerator) where TEnumerator : IBreakableForEach { Debug.Assert((nodeIndex >= 0 && nodeIndex < NodeCount) || (Encode(nodeIndex) >= 0 && Encode(nodeIndex) < LeafCount)); Debug.Assert(LeafCount >= 2, "This implementation assumes all nodes are filled."); @@ -29,8 +29,8 @@ unsafe readonly void GetOverlaps(int nodeIndex, Vector3 min, Vector else { ref var node = ref Nodes[nodeIndex]; - var aIntersected = BoundingBox.Intersects(node.A.Min, node.A.Max, min, max); - var bIntersected = BoundingBox.Intersects(node.B.Min, node.B.Max, min, max); + var aIntersected = BoundingBox.IntersectsUnsafe(node.A, boundingBox); + var bIntersected = BoundingBox.IntersectsUnsafe(node.B, boundingBox); if (aIntersected) { @@ -58,19 +58,19 @@ unsafe readonly void GetOverlaps(int nodeIndex, Vector3 min, Vector } } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public readonly unsafe void GetOverlaps(Vector3 min, Vector3 max, ref TEnumerator leafEnumerator) where TEnumerator : IBreakableForEach + public readonly unsafe void GetOverlaps(BoundingBox boundingBox, ref TEnumerator leafEnumerator) where TEnumerator : IBreakableForEach { if (LeafCount > 1) { //TODO: Explicitly tracking depth in the tree during construction/refinement is practically required to guarantee correctness. //While it's exceptionally rare that any tree would have more than 256 levels, the worst case of stomping stack memory is not acceptable in the long run. var stack = stackalloc int[TraversalStackCapacity]; - GetOverlaps(0, min, max, stack, ref leafEnumerator); + GetOverlaps(0, boundingBox, stack, ref leafEnumerator); } else if (LeafCount == 1) { Debug.Assert(Nodes[0].A.Index < 0, "If the root only has one child, it must be a leaf."); - if (BoundingBox.Intersects(min, max, Nodes[0].A.Min, Nodes[0].A.Max)) + if (BoundingBox.IntersectsUnsafe(boundingBox, Nodes[0].A)) { leafEnumerator.LoopBody(Encode(Nodes[0].A.Index)); } @@ -80,9 +80,9 @@ public readonly unsafe void GetOverlaps(Vector3 min, Vector3 max, r } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public readonly unsafe void GetOverlaps(in BoundingBox boundingBox, ref TEnumerator leafEnumerator) where TEnumerator : IBreakableForEach + public readonly unsafe void GetOverlaps(Vector3 min, Vector3 max, ref TEnumerator leafEnumerator) where TEnumerator : IBreakableForEach { - GetOverlaps(boundingBox.Min, boundingBox.Max, ref leafEnumerator); + GetOverlaps(new BoundingBox(min, max), ref leafEnumerator); } From c1afc94f8bd80a09af9b650e4a742fbf509fa487 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 31 Aug 2022 18:47:05 -0500 Subject: [PATCH 579/947] Another variant, still slower due to pushing way too much stuff into intermediates. --- BepuPhysics/Trees/Tree_SelfQueries.cs | 338 +++++++++++++++--- .../SpecializedTests/TreeFiddlingTestDemo.cs | 8 + 2 files changed, 288 insertions(+), 58 deletions(-) diff --git a/BepuPhysics/Trees/Tree_SelfQueries.cs b/BepuPhysics/Trees/Tree_SelfQueries.cs index 8ce4667b9..bb6621c72 100644 --- a/BepuPhysics/Trees/Tree_SelfQueries.cs +++ b/BepuPhysics/Trees/Tree_SelfQueries.cs @@ -1,10 +1,12 @@ using BepuPhysics.CollisionDetection.CollisionTasks; +using BepuPhysics.Constraints.Contact; using BepuUtilities; using BepuUtilities.Collections; using BepuUtilities.Memory; using System; using System.Diagnostics; using System.Numerics; +using System.Reflection.Metadata.Ecma335; using System.Runtime.CompilerServices; using System.Runtime.InteropServices; using System.Runtime.Intrinsics; @@ -88,44 +90,6 @@ unsafe void DispatchTestForNodes(ref NodeChild a, ref NodeChild } } - - ///// - ///// Intersects two node children. - ///// The referenced node children must not be in unpinned managed memory. - ///// - ///// First child to compare. - ///// Second child to compare. - ///// True if the children overlap, false otherwise. - //[MethodImpl(MethodImplOptions.AggressiveInlining)] - //unsafe static bool Intersects(in NodeChild childA, in NodeChild childB) - //{ - // return BoundingBox.Intersects(childA, childB); - // //if (Vector256.IsHardwareAccelerated && Avx.IsSupported) - // //{ - // // var a = Vector256.LoadUnsafe(ref Unsafe.AsRef(childA.Min.X)); - // // var b = Vector256.LoadUnsafe(ref Unsafe.AsRef(childB.Min.X)); - // // var min = Avx.Permute2x128(a, b, (0) | (2 << 4)); //(aMin, aMax) (bMin, bMax) -> (aMin, bMin) - // // var max = Avx.Permute2x128(a, b, (3) | (1 << 4)); //(aMin, aMax) (bMin, bMax) -> (bMax, aMax) - // // var noIntersection = Vector256.LessThan(max, min); - // // return (Vector256.ExtractMostSignificantBits(noIntersection) & 0b1110111) == 0; - // //} - // //else - // //if (Vector128.IsHardwareAccelerated) - // //{ - // // //THIS IS A POTENTIAL GC HOLE IF CHILDREN ARE PASSED FROM UNPINNED MANAGED MEMORY - // // var aMin = Vector128.LoadUnsafe(ref Unsafe.AsRef(childA.Min.X)); - // // var aMax = Vector128.LoadUnsafe(ref Unsafe.AsRef(childA.Max.X)); - // // var bMin = Vector128.LoadUnsafe(ref Unsafe.AsRef(childB.Min.X)); - // // var bMax = Vector128.LoadUnsafe(ref Unsafe.AsRef(childB.Max.X)); - // // var noIntersectionOnAxes = Vector128.LessThan(aMax, bMin) | Vector128.LessThan(bMax, aMin); - // // return (Vector128.ExtractMostSignificantBits(noIntersectionOnAxes) & 0b111) == 0; - // //} - // //else - // { - // return BoundingBox.Intersects(childA.Min, childA.Max, childB.Min, childB.Max); - // } - //} - unsafe void GetOverlapsBetweenDifferentNodes(ref Node a, ref Node b, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler { //There are no shared children, so test them all. @@ -193,7 +157,7 @@ struct StackEntry public int B; } - unsafe void GetOverlapsWithLeaf(ref TOverlapHandler results, NodeChild leaf, int nodeToTest, ref QuickList stack, BufferPool pool) where TOverlapHandler : IOverlapHandler + unsafe void GetOverlapsWithLeaf(ref TOverlapHandler results, NodeChild leaf, int nodeToTest, ref QuickList stack) where TOverlapHandler : IOverlapHandler { var leafIndex = Encode(leaf.Index); Debug.Assert(stack.Count == 0); @@ -209,7 +173,7 @@ unsafe void GetOverlapsWithLeaf(ref TOverlapHandler results, No if (intersectedInternalA && intersectedInternalB) { nodeToTest = node.A.Index; - stack.Allocate(pool) = node.B.Index; + stack.AllocateUnsafely() = node.B.Index; } else { @@ -245,8 +209,8 @@ public unsafe void GetSelfOverlaps2(ref TOverlapHandler results if (LeafCount < 2) return; - QuickList stack = new(Math.Min(1024, Math.Max(0, NodeCount / 4)), pool); - QuickList leafStack = new(Math.Min(512, Math.Max(0, NodeCount / 16)), pool); + QuickList stack = new(NodeCount, pool); + QuickList leafStack = new(NodeCount, pool); StackEntry nextTest = default; while (true) @@ -276,10 +240,10 @@ public unsafe void GetSelfOverlaps2(ref TOverlapHandler results nextTest.B = node.B.Index; //The two child self tests get pushed to the stack. - ref var stackA = ref stack.Allocate(pool); + ref var stackA = ref stack.AllocateUnsafely(); stackA.A = node.A.Index; stackA.B = node.A.Index; - ref var stackB = ref stack.Allocate(pool); + ref var stackB = ref stack.AllocateUnsafely(); stackB.A = node.B.Index; stackB.B = node.B.Index; } @@ -300,13 +264,13 @@ public unsafe void GetSelfOverlaps2(ref TOverlapHandler results { nextTest.A = node.A.Index; nextTest.B = node.A.Index; - GetOverlapsWithLeaf(ref results, node.B, node.A.Index, ref leafStack, pool); + GetOverlapsWithLeaf(ref results, node.B, node.A.Index, ref leafStack); } else { nextTest.A = node.B.Index; nextTest.B = node.B.Index; - GetOverlapsWithLeaf(ref results, node.A, node.B.Index, ref leafStack, pool); + GetOverlapsWithLeaf(ref results, node.A, node.B.Index, ref leafStack); } } else @@ -326,7 +290,7 @@ public unsafe void GetSelfOverlaps2(ref TOverlapHandler results { nextTest.A = node.A.Index; nextTest.B = node.A.Index; - ref var stackB = ref stack.Allocate(pool); + ref var stackB = ref stack.AllocateUnsafely(); stackB.A = node.B.Index; stackB.B = node.B.Index; } @@ -377,9 +341,9 @@ public unsafe void GetSelfOverlaps2(ref TOverlapHandler results { //At least one is a leaf. if (n0AIsInternal) - GetOverlapsWithLeaf(ref results, n1.A, n0.A.Index, ref leafStack, pool); + GetOverlapsWithLeaf(ref results, n1.A, n0.A.Index, ref leafStack); else if (n1AIsInternal) - GetOverlapsWithLeaf(ref results, n0.A, n1.A.Index, ref leafStack, pool); + GetOverlapsWithLeaf(ref results, n0.A, n1.A.Index, ref leafStack); else //Both are leaves. results.Handle(Encode(n0.A.Index), Encode(n1.A.Index)); } @@ -395,7 +359,7 @@ public unsafe void GetSelfOverlaps2(ref TOverlapHandler results } else { - ref var stackEntry = ref stack.Allocate(pool); + ref var stackEntry = ref stack.AllocateUnsafely(); stackEntry.A = n0.A.Index; stackEntry.B = n1.B.Index; } @@ -404,9 +368,9 @@ public unsafe void GetSelfOverlaps2(ref TOverlapHandler results { //At least one is a leaf. if (n0AIsInternal) - GetOverlapsWithLeaf(ref results, n1.B, n0.A.Index, ref leafStack, pool); + GetOverlapsWithLeaf(ref results, n1.B, n0.A.Index, ref leafStack); else if (n1BIsInternal) - GetOverlapsWithLeaf(ref results, n0.A, n1.B.Index, ref leafStack, pool); + GetOverlapsWithLeaf(ref results, n0.A, n1.B.Index, ref leafStack); else //Both are leaves. results.Handle(Encode(n0.A.Index), Encode(n1.B.Index)); } @@ -422,7 +386,7 @@ public unsafe void GetSelfOverlaps2(ref TOverlapHandler results } else { - ref var stackEntry = ref stack.Allocate(pool); + ref var stackEntry = ref stack.AllocateUnsafely(); stackEntry.A = n0.B.Index; stackEntry.B = n1.A.Index; } @@ -431,9 +395,9 @@ public unsafe void GetSelfOverlaps2(ref TOverlapHandler results { //At least one is a leaf. if (n0BIsInternal) - GetOverlapsWithLeaf(ref results, n1.A, n0.B.Index, ref leafStack, pool); + GetOverlapsWithLeaf(ref results, n1.A, n0.B.Index, ref leafStack); else if (n1AIsInternal) - GetOverlapsWithLeaf(ref results, n0.B, n1.A.Index, ref leafStack, pool); + GetOverlapsWithLeaf(ref results, n0.B, n1.A.Index, ref leafStack); else //Both are leaves. results.Handle(Encode(n0.B.Index), Encode(n1.A.Index)); } @@ -449,7 +413,7 @@ public unsafe void GetSelfOverlaps2(ref TOverlapHandler results } else { - ref var stackEntry = ref stack.Allocate(pool); + ref var stackEntry = ref stack.AllocateUnsafely(); stackEntry.A = n0.B.Index; stackEntry.B = n1.B.Index; } @@ -458,9 +422,9 @@ public unsafe void GetSelfOverlaps2(ref TOverlapHandler results { //At least one is a leaf. if (n0BIsInternal) - GetOverlapsWithLeaf(ref results, n1.B, n0.B.Index, ref leafStack, pool); + GetOverlapsWithLeaf(ref results, n1.B, n0.B.Index, ref leafStack); else if (n1BIsInternal) - GetOverlapsWithLeaf(ref results, n0.B, n1.B.Index, ref leafStack, pool); + GetOverlapsWithLeaf(ref results, n0.B, n1.B.Index, ref leafStack); else //Both are leaves. results.Handle(Encode(n0.B.Index), Encode(n1.B.Index)); } @@ -479,5 +443,263 @@ public unsafe void GetSelfOverlaps2(ref TOverlapHandler results } + struct LeafNodeStackEntry + { + public uint EncodedLeafParentIndex; + public int NodeIndex; + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public LeafNodeStackEntry(int leafParentIndex, bool leafChildIsB, int nodeIndex) + { + EncodedLeafParentIndex = (uint)leafParentIndex; + if (leafChildIsB) + EncodedLeafParentIndex |= 1u << 31; + NodeIndex = nodeIndex; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public ref NodeChild GetChild(ref Tree tree) + { + var parentNodeIndex = EncodedLeafParentIndex & 0x7FFF_FFFF; + var leafIsChildB = EncodedLeafParentIndex >= 0x8000_0000; + ref var parent = ref tree.Nodes[parentNodeIndex]; + return ref leafIsChildB ? ref parent.B : ref parent.A; + } + } + + + public unsafe void GetSelfOverlapsContiguousPrepass(ref TOverlapHandler results, BufferPool pool) where TOverlapHandler : IOverlapHandler + { + //If there are less than two leaves, there can't be any overlap. + //This provides a guarantee that there are at least 2 children in each internal node considered by GetOverlapsInNode. + if (LeafCount < 2) + return; + + //A recursive self test will at some point visit all nodes with certainty. Instead of framing it as a recursive test at all, do a prepass that's just a contiguous iteration. + QuickList crossoverStack = new(NodeCount, pool); + QuickList leafNodeStack = new(NodeCount, pool); + for (int i = 0; i < NodeCount; ++i) + { + ref var node = ref Nodes[i]; + var a = node.A.Index; + var b = node.B.Index; + var aIsInternal = a >= 0; + var bIsInternal = b >= 0; + if (aIsInternal && bIsInternal) + { + if (BoundingBox.IntersectsUnsafe(node.A, node.B)) + { + ref var stackEntry = ref crossoverStack.AllocateUnsafely(); + stackEntry.A = a; + stackEntry.B = b; + } + } + else if (aIsInternal || bIsInternal) + { + //One is a leaf, one is internal. + leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(i, aIsInternal, aIsInternal ? a : b); + } + else + { + //Both are leaves. + results.Handle(Encode(a), Encode(b)); + } + } + + //Now, we need to complete all crossovers and leaf-node tests. Note that leaf-node tests can only generate leaf-node or leaf-leaf tests, they never produce crossovers. + //In contrast, crossovers can generate leaf-node tests. So do crossovers first so we'll have every leaf-node test in the stack ready to go. + if (crossoverStack.TryPop(out var nextCrossover)) + { + while (true) + { + //Possible sources of stack entry: + //1) AA intersection, both internal + //2) AB intersection, both internal + //3) BA intersection, both internal + //4) BB intersection, both internal + var parent0 = nextCrossover.A; + var parent1 = nextCrossover.B; + ref var n0 = ref Nodes[parent0]; + ref var n1 = ref Nodes[parent1]; + var aaIntersects = BoundingBox.IntersectsUnsafe(n0.A, n1.A); + var abIntersects = BoundingBox.IntersectsUnsafe(n0.A, n1.B); + var baIntersects = BoundingBox.IntersectsUnsafe(n0.B, n1.A); + var bbIntersects = BoundingBox.IntersectsUnsafe(n0.B, n1.B); + var n0A = n0.A.Index; + var n0B = n0.B.Index; + var n1A = n1.A.Index; + var n1B = n1.B.Index; + var n0AIsInternal = n0A >= 0; + var n0BIsInternal = n0B >= 0; + var n1AIsInternal = n1A >= 0; + var n1BIsInternal = n1B >= 0; + //The first test which generates a stack candidate gets the nextTest; the rest get pushed to the stack. + int previousStackGeneratorCount = 0; + if (aaIntersects) + { + if (n0AIsInternal && n1AIsInternal) + { + ++previousStackGeneratorCount; + nextCrossover.A = n0A; + nextCrossover.B = n1A; + } + else + { + //At least one is a leaf. + if (n0AIsInternal) + leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent1, false, n0A); + else if (n1AIsInternal) + leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent0, false, n1A); + else //Both are leaves. + results.Handle(Encode(n0A), Encode(n1A)); + } + } + if (abIntersects) + { + if (n0AIsInternal && n1BIsInternal) + { + if (previousStackGeneratorCount++ == 0) + { + nextCrossover.A = n0A; + nextCrossover.B = n1B; + } + else + { + ref var stackEntry = ref crossoverStack.AllocateUnsafely(); + stackEntry.A = n0A; + stackEntry.B = n1B; + } + } + else + { + //At least one is a leaf. + if (n0AIsInternal) + leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent1, true, n0A); + else if (n1BIsInternal) + leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent0, false, n1B); + else //Both are leaves. + results.Handle(Encode(n0A), Encode(n1B)); + } + } + if (baIntersects) + { + if (n0BIsInternal && n1AIsInternal) + { + if (previousStackGeneratorCount++ == 0) + { + nextCrossover.A = n0B; + nextCrossover.B = n1A; + } + else + { + ref var stackEntry = ref crossoverStack.AllocateUnsafely(); + stackEntry.A = n0B; + stackEntry.B = n1A; + } + } + else + { + //At least one is a leaf. + if (n0BIsInternal) + leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent1, false, n0B); + else if (n1AIsInternal) + leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent0, true, n1A); + else //Both are leaves. + results.Handle(Encode(n0B), Encode(n1A)); + } + } + if (bbIntersects) + { + if (n0BIsInternal && n1BIsInternal) + { + if (previousStackGeneratorCount++ == 0) + { + nextCrossover.A = n0B; + nextCrossover.B = n1B; + } + else + { + ref var stackEntry = ref crossoverStack.AllocateUnsafely(); + stackEntry.A = n0B; + stackEntry.B = n1B; + } + } + else + { + //At least one is a leaf. + if (n0BIsInternal) + leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent1, true, n0B); + else if (n1BIsInternal) + leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent0, true, n1B); + else //Both are leaves. + results.Handle(Encode(n0B), Encode(n1B)); + } + } + if (previousStackGeneratorCount == 0) + { + //None of the candidates generated a next step, so grab from the stack. + if (!crossoverStack.TryPop(out nextCrossover)) + break; + } + } + } + crossoverStack.Dispose(pool); + QuickList stack = new(NodeCount, pool); + + for (int i = 0; i < leafNodeStack.Count; ++i) + { + var leafNodeTarget = leafNodeStack[i]; + ref var leafChild = ref leafNodeTarget.GetChild(ref this); + var leafIndex = Encode(leafChild.Index); + var nodeToTest = leafNodeTarget.NodeIndex; + Debug.Assert(stack.Count == 0); + while (true) + { + //Now process all the leaf nodes! + ref var node = ref Nodes[nodeToTest]; + var a = node.A.Index; + var b = node.B.Index; + var aIntersected = BoundingBox.IntersectsUnsafe(leafChild, node.A); + var bIntersected = BoundingBox.IntersectsUnsafe(leafChild, node.B); + var aIsInternal = a >= 0; + var bIsInternal = b >= 0; + var intersectedInternalA = aIntersected && aIsInternal; + var intersectedInternalB = bIntersected && bIsInternal; + if (intersectedInternalA && intersectedInternalB) + { + nodeToTest = a; + stack.AllocateUnsafely() = b; + } + else + { + if (aIntersected && !aIsInternal) + { + results.Handle(leafIndex, Encode(a)); + } + if (bIntersected && !bIsInternal) + { + results.Handle(leafIndex, Encode(b)); + } + if (intersectedInternalA || intersectedInternalB) + { + nodeToTest = intersectedInternalA ? a : b; + } + else + { + //The current traversal step doesn't offer a next step; pull from the stack. + if (!stack.TryPop(out nodeToTest)) + { + //Nothing left to test against this leaf! Done! + break; + } + } + } + } + } + + leafNodeStack.Dispose(pool); + stack.Dispose(pool); + } + } } diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index d78249edf..ebeb2c94d 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -48,6 +48,12 @@ public override void Initialize(ContentArchive content, Camera camera) Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); int testCount = 1000; + var overlapHandlerPre = new OverlapHandler(); + var startTimePre = Stopwatch.GetTimestamp(); + for (int i = 0; i < testCount; ++i) + mesh.Tree.GetSelfOverlapsContiguousPrepass(ref overlapHandlerPre, BufferPool); + var endTimePre = Stopwatch.GetTimestamp(); + var overlapHandlerNew = new OverlapHandler(); var startTimeNew = Stopwatch.GetTimestamp(); for (int i = 0; i < testCount; ++i) @@ -60,9 +66,11 @@ public override void Initialize(ContentArchive content, Camera camera) mesh.Tree.GetSelfOverlaps(ref overlapHandlerOld); var endTimeOld = Stopwatch.GetTimestamp(); + Console.WriteLine($"CPrepass time per execution (ms): {(endTimePre - startTimePre) * 1e3 / (testCount * Stopwatch.Frequency)}"); Console.WriteLine($"Revamped time per execution (ms): {(endTimeNew - startTimeNew) * 1e3 / (testCount * Stopwatch.Frequency)}"); Console.WriteLine($"Original time per execution (ms): {(endTimeOld - startTimeOld) * 1e3 / (testCount * Stopwatch.Frequency)}"); + Console.WriteLine($"Revamped count: {overlapHandlerPre.OverlapCount}, sum {overlapHandlerPre.OverlapSum}, hash {overlapHandlerPre.OverlapHash}"); Console.WriteLine($"Revamped count: {overlapHandlerNew.OverlapCount}, sum {overlapHandlerNew.OverlapSum}, hash {overlapHandlerNew.OverlapHash}"); Console.WriteLine($"Original count: {overlapHandlerOld.OverlapCount}, sum {overlapHandlerOld.OverlapSum}, hash {overlapHandlerOld.OverlapHash}"); } From 640b2e8a09e80c688c82c18fe34f8189784fbd98 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 1 Sep 2022 19:17:00 -0500 Subject: [PATCH 580/947] Simplify 2. --- BepuPhysics/Trees/Tree_SelfQueries.cs | 98 +++++++++------------------ 1 file changed, 32 insertions(+), 66 deletions(-) diff --git a/BepuPhysics/Trees/Tree_SelfQueries.cs b/BepuPhysics/Trees/Tree_SelfQueries.cs index bb6621c72..f8ef086f4 100644 --- a/BepuPhysics/Trees/Tree_SelfQueries.cs +++ b/BepuPhysics/Trees/Tree_SelfQueries.cs @@ -232,82 +232,48 @@ public unsafe void GetSelfOverlaps2(ref TOverlapHandler results var abIntersect = BoundingBox.IntersectsUnsafe(node.A, node.B); var aIsInternal = node.A.Index >= 0; var bIsInternal = node.B.Index >= 0; - if (abIntersect && aIsInternal && bIsInternal) + + if (aIsInternal) { - //Both children internal and their bounds intersect. - //Their intersection should be the next visited. nextTest.A = node.A.Index; - nextTest.B = node.B.Index; + nextTest.B = node.A.Index; - //The two child self tests get pushed to the stack. - ref var stackA = ref stack.AllocateUnsafely(); - stackA.A = node.A.Index; - stackA.B = node.A.Index; - ref var stackB = ref stack.AllocateUnsafely(); - stackB.A = node.B.Index; - stackB.B = node.B.Index; - } - else - { - if (abIntersect) + if (bIsInternal) { - Debug.Assert(!aIsInternal || !bIsInternal, "Just in case you shuffle logic around in the future: this clause assumes at least one leaf."); - //The children have overlapping bounds, but at least one is a leaf. - //Note that this path cannot generate any stack entries. - //At least one child is a leaf, so there is at most one self-test. - //The a-b test involves at least one leaf as well, which means it will use the GetOverlapsWithLeaf path. - //So the one possible self-test will be put into the nextTest. - if (aIsInternal || bIsInternal) - { - //One's a leaf, one's internal. - if (aIsInternal) - { - nextTest.A = node.A.Index; - nextTest.B = node.A.Index; - GetOverlapsWithLeaf(ref results, node.B, node.A.Index, ref leafStack); - } - else - { - nextTest.A = node.B.Index; - nextTest.B = node.B.Index; - GetOverlapsWithLeaf(ref results, node.A, node.B.Index, ref leafStack); - } - } - else - { - //Both children are leaves. They have overlapping bounds, so... - results.Handle(Encode(node.A.Index), Encode(node.B.Index)); + ref var stackB = ref stack.AllocateUnsafely(); + stackB.A = node.B.Index; + stackB.B = node.B.Index; - //If both children are leaves, then there's no other source for the next visit, so pop. - if (!stack.TryPop(out nextTest)) - break; + if (abIntersect) + { + ref var stackEntry = ref stack.AllocateUnsafely(); + stackEntry.A = node.A.Index; + stackEntry.B = node.B.Index; } } - else + else if (abIntersect) { - //No a-b test, but possibly self tests. - if (aIsInternal && bIsInternal) - { - nextTest.A = node.A.Index; - nextTest.B = node.A.Index; - ref var stackB = ref stack.AllocateUnsafely(); - stackB.A = node.B.Index; - stackB.B = node.B.Index; - } - else if (aIsInternal || bIsInternal) - { - var nextSelfTestIndex = aIsInternal ? node.A.Index : node.B.Index; - nextTest.A = nextSelfTestIndex; - nextTest.B = nextSelfTestIndex; - } - else - { - //There was no A-B test and both children are leaves. No new tests available, so grab from the stack. - if (!stack.TryPop(out nextTest)) - break; - } + GetOverlapsWithLeaf(ref results, node.B, node.A.Index, ref leafStack); } } + else if (bIsInternal) + { + nextTest.A = node.B.Index; + nextTest.B = node.B.Index; + + if (abIntersect) + { + GetOverlapsWithLeaf(ref results, node.A, node.B.Index, ref leafStack); + } + } + else if (abIntersect) + { + //Both children are leaves. They have overlapping bounds, so... + results.Handle(Encode(node.A.Index), Encode(node.B.Index)); + //No new tests available, so grab from the stack. + if (!stack.TryPop(out nextTest)) + break; + } } else { From 366dce5b61ac25402bb6e6c4b7af70a8e1a0623f Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 2 Sep 2022 17:52:24 -0500 Subject: [PATCH 581/947] Another even worse implementation. --- BepuPhysics/Trees/Tree_SelfQueries.cs | 333 +++++++++++++++++++++++++- 1 file changed, 325 insertions(+), 8 deletions(-) diff --git a/BepuPhysics/Trees/Tree_SelfQueries.cs b/BepuPhysics/Trees/Tree_SelfQueries.cs index f8ef086f4..ef13b2665 100644 --- a/BepuPhysics/Trees/Tree_SelfQueries.cs +++ b/BepuPhysics/Trees/Tree_SelfQueries.cs @@ -11,6 +11,7 @@ using System.Runtime.InteropServices; using System.Runtime.Intrinsics; using System.Runtime.Intrinsics.X86; +using System.Security.Cryptography; namespace BepuPhysics.Trees { @@ -151,12 +152,6 @@ public unsafe void GetSelfOverlaps(ref TOverlapHandler results) GetOverlapsInNode(ref Nodes[0], ref results); } - struct StackEntry - { - public int A; - public int B; - } - unsafe void GetOverlapsWithLeaf(ref TOverlapHandler results, NodeChild leaf, int nodeToTest, ref QuickList stack) where TOverlapHandler : IOverlapHandler { var leafIndex = Encode(leaf.Index); @@ -201,6 +196,12 @@ unsafe void GetOverlapsWithLeaf(ref TOverlapHandler results, No } } } + struct StackEntry + { + public int A; + public int B; + } + public unsafe void GetSelfOverlaps2(ref TOverlapHandler results, BufferPool pool) where TOverlapHandler : IOverlapHandler { @@ -409,6 +410,323 @@ public unsafe void GetSelfOverlaps2(ref TOverlapHandler results } + enum StackEntryType + { + SelfTest, + Crossover, + Leaf, + } + struct ComboStackEntry + { + public int EncodedA; + public int B; + + public StackEntryType Type => EncodedA < 0 ? StackEntryType.Leaf : EncodedA == B ? StackEntryType.SelfTest : StackEntryType.Crossover; + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static ComboStackEntry CreateForLeafTest(int leafParentIndex, bool leafChildIsB, int nodeIndex) + { + ComboStackEntry entry; + //Upper bit flags this stack entry as a leaf versus node test. + entry.EncodedA = leafParentIndex | (1 << 31); + if (leafChildIsB) + entry.EncodedA |= 1 << 30; + entry.B = nodeIndex; + return entry; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static ComboStackEntry CreateForSelfTest(int nodeIndex) + { + ComboStackEntry entry; + entry.EncodedA = nodeIndex; + entry.B = nodeIndex; + return entry; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static ComboStackEntry CreateCrossover(int a, int b) + { + ComboStackEntry entry; + entry.EncodedA = a; + entry.B = b; + return entry; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public ref NodeChild GetLeafChild(ref Tree tree) + { + var parentNodeIndex = EncodedA & 0x3FFF_FFFF; + var leafIsChildB = (EncodedA & (1 << 30)) != 0; + ref var parent = ref tree.Nodes[parentNodeIndex]; + return ref leafIsChildB ? ref parent.B : ref parent.A; + } + } + + public unsafe void GetSelfOverlaps3(ref TOverlapHandler results, BufferPool pool) where TOverlapHandler : IOverlapHandler + { + //If there are less than two leaves, there can't be any overlap. + //This provides a guarantee that there are at least 2 children in each internal node considered by GetOverlapsInNode. + if (LeafCount < 2) + return; + + QuickList stack = new(NodeCount, pool); + QuickList leafStack = new(NodeCount, pool); + ComboStackEntry nextTest = default; + + while (true) + { + switch (nextTest.Type) + { + case StackEntryType.SelfTest: + { + //Self test. + //Possible sources of further stack entries in a self test: + //1) Child A and B are both internal and their bounds intersect. + //2) Child A is internal. + //3) Child B is internal. + //We can also spawn leaf-subtree tests if: + //1) Child A and B have intersecting bounds, and only one of them is a leaf. + //We can spawn direct leaf tests if: + //1) Child A and B have intersecting bounds, and both are leaves. + //Note that we never need to push an entry with differing A and B indices to the stack *from a self test*. There can only be one such entry created from any self test, and it's always visited next. + //Non-self tests will generate *only* results with differing A and B indices. + ref var node = ref Nodes[nextTest.B]; + //GetOverlapsInNode(ref node, ref results); + //if (!stack.TryPop(out nextTest)) + //{ + // goto Terminate; + //} + //break; + var abIntersect = BoundingBox.IntersectsUnsafe(node.A, node.B); + var aIsInternal = node.A.Index >= 0; + var bIsInternal = node.B.Index >= 0; + + if (aIsInternal) + { + if (bIsInternal) + { + stack.AllocateUnsafely() = ComboStackEntry.CreateForSelfTest(node.B.Index); + if (abIntersect) + { + stack.AllocateUnsafely() = ComboStackEntry.CreateCrossover(node.A.Index, node.B.Index); + } + } + else if (abIntersect) + { + stack.AllocateUnsafely() = ComboStackEntry.CreateForLeafTest(nextTest.B, true, node.A.Index); + } + nextTest = ComboStackEntry.CreateForSelfTest(node.A.Index); + } + else if (bIsInternal) + { + if (abIntersect) + { + stack.AllocateUnsafely() = ComboStackEntry.CreateForLeafTest(nextTest.EncodedA, false, node.B.Index); + } + nextTest = ComboStackEntry.CreateForSelfTest(node.B.Index); + } + else if (abIntersect) + { + //Both children are leaves. They have overlapping bounds, so... + results.Handle(Encode(node.A.Index), Encode(node.B.Index)); + //No new tests available, so grab from the stack. + if (!stack.TryPop(out nextTest)) + { + goto Terminate; + } + } + } + break; + case StackEntryType.Crossover: + { + //Not a self test! + //Possible sources of stack entry: + //1) AA intersection, both internal + //2) AB intersection, both internal + //3) BA intersection, both internal + //4) BB intersection, both internal + var n0Index = nextTest.EncodedA; + var n1Index = nextTest.B; + ref var n0 = ref Nodes[nextTest.EncodedA]; + ref var n1 = ref Nodes[nextTest.B]; + + //GetOverlapsBetweenDifferentNodes(ref n0, ref n1, ref results); + //if (!stack.TryPop(out nextTest)) + //{ + // goto Terminate; + //} + //break; + var aaIntersects = BoundingBox.IntersectsUnsafe(n0.A, n1.A); + var abIntersects = BoundingBox.IntersectsUnsafe(n0.A, n1.B); + var baIntersects = BoundingBox.IntersectsUnsafe(n0.B, n1.A); + var bbIntersects = BoundingBox.IntersectsUnsafe(n0.B, n1.B); + var n0AIsInternal = n0.A.Index >= 0; + var n0BIsInternal = n0.B.Index >= 0; + var n1AIsInternal = n1.A.Index >= 0; + var n1BIsInternal = n1.B.Index >= 0; + //The first test which generates a stack candidate gets the nextTest; the rest get pushed to the stack. + int previousStackGeneratorCount = 0; + if (aaIntersects) + { + if (n0AIsInternal && n1AIsInternal) + { + ++previousStackGeneratorCount; + nextTest = ComboStackEntry.CreateCrossover(n0.A.Index, n1.A.Index); + } + else + { + //At least one is a leaf. + if (n0AIsInternal) + stack.AllocateUnsafely() = ComboStackEntry.CreateForLeafTest(n1Index, false, n0.A.Index); + else if (n1AIsInternal) + stack.AllocateUnsafely() = ComboStackEntry.CreateForLeafTest(n0Index, false, n1.A.Index); + else //Both are leaves. + results.Handle(Encode(n0.A.Index), Encode(n1.A.Index)); + } + } + if (abIntersects) + { + if (n0AIsInternal && n1BIsInternal) + { + if (previousStackGeneratorCount++ == 0) + { + nextTest = ComboStackEntry.CreateCrossover(n0.A.Index, n1.B.Index); + } + else + { + stack.AllocateUnsafely() = ComboStackEntry.CreateCrossover(n0.A.Index, n1.B.Index); + } + } + else + { + //At least one is a leaf. + if (n0AIsInternal) + stack.AllocateUnsafely() = ComboStackEntry.CreateForLeafTest(n1Index, true, n0.A.Index); + else if (n1BIsInternal) + stack.AllocateUnsafely() = ComboStackEntry.CreateForLeafTest(n0Index, false, n1.B.Index); + else //Both are leaves. + results.Handle(Encode(n0.A.Index), Encode(n1.B.Index)); + } + } + if (baIntersects) + { + if (n0BIsInternal && n1AIsInternal) + { + if (previousStackGeneratorCount++ == 0) + { + nextTest = ComboStackEntry.CreateCrossover(n0.B.Index, n1.A.Index); + } + else + { + stack.AllocateUnsafely() = ComboStackEntry.CreateCrossover(n0.B.Index, n1.A.Index); + } + } + else + { + //At least one is a leaf. + if (n0BIsInternal) + stack.AllocateUnsafely() = ComboStackEntry.CreateForLeafTest(n1Index, false, n0.B.Index); + else if (n1AIsInternal) + stack.AllocateUnsafely() = ComboStackEntry.CreateForLeafTest(n0Index, true, n1.A.Index); + else //Both are leaves. + results.Handle(Encode(n0.B.Index), Encode(n1.A.Index)); + } + } + if (bbIntersects) + { + if (n0BIsInternal && n1BIsInternal) + { + if (previousStackGeneratorCount++ == 0) + { + nextTest = ComboStackEntry.CreateCrossover(n0.B.Index, n1.B.Index); + } + else + { + stack.AllocateUnsafely() = ComboStackEntry.CreateCrossover(n0.B.Index, n1.B.Index); + } + } + else + { + //At least one is a leaf. + if (n0BIsInternal) + stack.AllocateUnsafely() = ComboStackEntry.CreateForLeafTest(n1Index, true, n0.B.Index); + else if (n1BIsInternal) + stack.AllocateUnsafely() = ComboStackEntry.CreateForLeafTest(n0Index, true, n1.B.Index); + else //Both are leaves. + results.Handle(Encode(n0.B.Index), Encode(n1.B.Index)); + } + } + if (previousStackGeneratorCount == 0) + { + //None of the candidates generated a next step, so grab from the stack. + if (!stack.TryPop(out nextTest)) + { + goto Terminate; + } + } + } + break; + default: + { + //Leaf-node test. Swap over to our own local stack. Equivalent visitation order. + ref var leafChild = ref nextTest.GetLeafChild(ref this); + var nodeToTest = nextTest.B; + //DispatchTestForLeaf(Encode(leafChild.Index), ref leafChild, nodeToTest, ref results); + //GetOverlapsWithLeaf(ref results, leafChild, nodeToTest, ref leafStack); + var leafIndex = Encode(leafChild.Index); + Debug.Assert(leafStack.Count == 0); + while (true) + { + ref var node = ref Nodes[nodeToTest]; + var aIntersects = BoundingBox.IntersectsUnsafe(leafChild, node.A); + var bIntersects = BoundingBox.IntersectsUnsafe(leafChild, node.B); + var aIsInternal = node.A.Index >= 0; + var bIsInternal = node.B.Index >= 0; + var intersectedInternalA = aIntersects && aIsInternal; + var intersectedInternalB = bIntersects && bIsInternal; + + if (aIntersects && !aIsInternal) + { + results.Handle(leafIndex, Encode(node.A.Index)); + } + if (bIntersects && !bIsInternal) + { + results.Handle(leafIndex, Encode(node.B.Index)); + } + + if (intersectedInternalA) + { + nodeToTest = node.A.Index; + if (intersectedInternalB) + { + leafStack.AllocateUnsafely() = node.B.Index; + } + } + else if (intersectedInternalB) + { + nodeToTest = node.B.Index; + } + else if (!leafStack.TryPop(out nodeToTest)) + { + //Nothing left to test against this leaf! Done! + break; + } + } + if (!stack.TryPop(out nextTest)) + { + goto Terminate; + } + } + break; + } + } + Terminate: + leafStack.Dispose(pool); + stack.Dispose(pool); + + } + struct LeafNodeStackEntry { public uint EncodedLeafParentIndex; @@ -427,7 +745,7 @@ public LeafNodeStackEntry(int leafParentIndex, bool leafChildIsB, int nodeIndex) public ref NodeChild GetChild(ref Tree tree) { var parentNodeIndex = EncodedLeafParentIndex & 0x7FFF_FFFF; - var leafIsChildB = EncodedLeafParentIndex >= 0x8000_0000; + var leafIsChildB = EncodedLeafParentIndex > 0x7FFF_FFFF; ref var parent = ref tree.Nodes[parentNodeIndex]; return ref leafIsChildB ? ref parent.B : ref parent.A; } @@ -662,7 +980,6 @@ public unsafe void GetSelfOverlapsContiguousPrepass(ref TOverla } } } - leafNodeStack.Dispose(pool); stack.Dispose(pool); } From 4fb4995565e6eaffd1877fc573ec3d102a6e3335 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 2 Sep 2022 19:11:28 -0500 Subject: [PATCH 582/947] infinite fiddleworks. --- BepuPhysics/Trees/Tree_SelfQueries.cs | 303 ++++++++++++++++++ .../SpecializedTests/TreeFiddlingTestDemo.cs | 63 +++- 2 files changed, 351 insertions(+), 15 deletions(-) diff --git a/BepuPhysics/Trees/Tree_SelfQueries.cs b/BepuPhysics/Trees/Tree_SelfQueries.cs index ef13b2665..aee5d7790 100644 --- a/BepuPhysics/Trees/Tree_SelfQueries.cs +++ b/BepuPhysics/Trees/Tree_SelfQueries.cs @@ -984,5 +984,308 @@ public unsafe void GetSelfOverlapsContiguousPrepass(ref TOverla stack.Dispose(pool); } + public unsafe void GetSelfOverlapsPrepassWithRecursion(ref TOverlapHandler results, BufferPool pool) where TOverlapHandler : IOverlapHandler + { + //If there are less than two leaves, there can't be any overlap. + //This provides a guarantee that there are at least 2 children in each internal node considered by GetOverlapsInNode. + if (LeafCount < 2) + return; + + var leafStack = new QuickList(NodeCount, pool); + //A recursive self test will at some point visit all nodes with certainty. Instead of framing it as a recursive test at all, do a prepass that's just a contiguous iteration. + for (int i = 0; i < NodeCount; ++i) + { + ref var node = ref Nodes[i]; + var a = node.A.Index; + var b = node.B.Index; + var aIsInternal = a >= 0; + var bIsInternal = b >= 0; + if (aIsInternal && bIsInternal) + { + if (BoundingBox.IntersectsUnsafe(node.A, node.B)) + { + GetOverlapsBetweenDifferentNodes(ref Nodes[a], ref Nodes[b], ref results); + } + } + else if (aIsInternal || bIsInternal) + { + //One is a leaf, one is internal. + GetOverlapsWithLeaf(ref results, aIsInternal ? node.B : node.A, aIsInternal ? a : b, ref leafStack); + } + else + { + //Both are leaves. + results.Handle(Encode(a), Encode(b)); + } + } + leafStack.Dispose(pool); + } + + + + + ref struct SelfTest where TOverlapHandler : IOverlapHandler + { + public Buffer Nodes; + public ref TOverlapHandler results; + + unsafe void DispatchTestForLeaf(int leafIndex, ref NodeChild leafChild, int nodeIndex) + { + if (nodeIndex < 0) + { + results.Handle(leafIndex, Encode(nodeIndex)); + } + else + { + TestLeafAgainstNode(leafIndex, ref leafChild, nodeIndex); + } + } + unsafe void TestLeafAgainstNode(int leafIndex, ref NodeChild leafChild, int nodeIndex) + { + ref var node = ref Nodes[nodeIndex]; + ref var a = ref node.A; + ref var b = ref node.B; + //Despite recursion, leafBounds should remain in L1- it'll be used all the way down the recursion from here. + //However, while we likely loaded child B when we loaded child A, there's no guarantee that it will stick around. + //Reloading that in the event of eviction would require more work than keeping the derived data on the stack. + //TODO: this is some pretty questionable microtuning. It's not often that the post-leaf-found recursion will be long enough to evict L1. Definitely test it. + var bIndex = b.Index; + var aIntersects = BoundingBox.IntersectsUnsafe(leafChild, a); + var bIntersects = BoundingBox.IntersectsUnsafe(leafChild, b); + if (aIntersects) + { + DispatchTestForLeaf(leafIndex, ref leafChild, a.Index); + } + if (bIntersects) + { + DispatchTestForLeaf(leafIndex, ref leafChild, bIndex); + } + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + unsafe void DispatchTestForNodes(ref NodeChild a, ref NodeChild b) + { + if (a.Index >= 0) + { + if (b.Index >= 0) + { + GetOverlapsBetweenDifferentNodes(ref Nodes[a.Index], ref Nodes[b.Index]); + } + else + { + //leaf B versus node A. + TestLeafAgainstNode(Encode(b.Index), ref b, a.Index); + } + } + else if (b.Index >= 0) + { + //leaf A versus node B. + TestLeafAgainstNode(Encode(a.Index), ref a, b.Index); + } + else + { + //Two leaves. + results.Handle(Encode(a.Index), Encode(b.Index)); + } + } + + unsafe void GetOverlapsBetweenDifferentNodes(ref Node a, ref Node b) + { + //There are no shared children, so test them all. + ref var aa = ref a.A; + ref var ab = ref a.B; + ref var ba = ref b.A; + ref var bb = ref b.B; + var aaIntersects = BoundingBox.IntersectsUnsafe(aa, ba); + var abIntersects = BoundingBox.IntersectsUnsafe(aa, bb); + var baIntersects = BoundingBox.IntersectsUnsafe(ab, ba); + var bbIntersects = BoundingBox.IntersectsUnsafe(ab, bb); + + if (aaIntersects) + { + DispatchTestForNodes(ref aa, ref ba); + } + if (abIntersects) + { + DispatchTestForNodes(ref aa, ref bb); + } + if (baIntersects) + { + DispatchTestForNodes(ref ab, ref ba); + } + if (bbIntersects) + { + DispatchTestForNodes(ref ab, ref bb); + } + } + + public unsafe void GetOverlapsInNode(ref Node node) + { + + ref var a = ref node.A; + ref var b = ref node.B; + + var ab = BoundingBox.IntersectsUnsafe(a, b); + + if (a.Index >= 0) + GetOverlapsInNode(ref Nodes[a.Index]); + if (b.Index >= 0) + GetOverlapsInNode(ref Nodes[b.Index]); + + //Test all different nodes. + if (ab) + { + DispatchTestForNodes(ref a, ref b); + } + } + } + public unsafe void GetSelfOverlaps4(ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler + { + //If there are less than two leaves, there can't be any overlap. + //This provides a guarantee that there are at least 2 children in each internal node considered by GetOverlapsInNode. + if (LeafCount < 2) + return; + + SelfTest hmm; + hmm.Nodes = Nodes; + hmm.results = ref results; + hmm.GetOverlapsInNode(ref Nodes[0]); + } + + ref struct SelfTest5 where TOverlapHandler : IOverlapHandler + { + public Buffer Nodes; + public ref TOverlapHandler Results; + //When using a leaf-node test. + public int LeafIndex; + public ref NodeChild LeafChild; + + unsafe void DispatchTestForLeaf(int nodeIndex) + { + if (nodeIndex < 0) + { + Results.Handle(LeafIndex, Encode(nodeIndex)); + } + else + { + TestLeafAgainstNode(nodeIndex); + } + } + unsafe void TestLeafAgainstNode(int nodeIndex) + { + ref var node = ref Nodes[nodeIndex]; + ref var a = ref node.A; + ref var b = ref node.B; + //Despite recursion, leafBounds should remain in L1- it'll be used all the way down the recursion from here. + //However, while we likely loaded child B when we loaded child A, there's no guarantee that it will stick around. + //Reloading that in the event of eviction would require more work than keeping the derived data on the stack. + //TODO: this is some pretty questionable microtuning. It's not often that the post-leaf-found recursion will be long enough to evict L1. Definitely test it. + var bIndex = b.Index; + var aIntersects = BoundingBox.IntersectsUnsafe(LeafChild, a); + var bIntersects = BoundingBox.IntersectsUnsafe(LeafChild, b); + if (aIntersects) + { + DispatchTestForLeaf(a.Index); + } + if (bIntersects) + { + DispatchTestForLeaf(bIndex); + } + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + unsafe void DispatchTestForNodes(ref NodeChild a, ref NodeChild b) + { + if (a.Index >= 0) + { + if (b.Index >= 0) + { + GetOverlapsBetweenDifferentNodes(ref Nodes[a.Index], ref Nodes[b.Index]); + } + else + { + //leaf B versus node A. + LeafIndex = Encode(b.Index); + LeafChild = ref b; + TestLeafAgainstNode(a.Index); + } + } + else if (b.Index >= 0) + { + //leaf A versus node B. + LeafIndex = Encode(a.Index); + LeafChild = ref a; + TestLeafAgainstNode(b.Index); + } + else + { + //Two leaves. + Results.Handle(Encode(a.Index), Encode(b.Index)); + } + } + + unsafe void GetOverlapsBetweenDifferentNodes(ref Node a, ref Node b) + { + //There are no shared children, so test them all. + ref var aa = ref a.A; + ref var ab = ref a.B; + ref var ba = ref b.A; + ref var bb = ref b.B; + var aaIntersects = BoundingBox.IntersectsUnsafe(aa, ba); + var abIntersects = BoundingBox.IntersectsUnsafe(aa, bb); + var baIntersects = BoundingBox.IntersectsUnsafe(ab, ba); + var bbIntersects = BoundingBox.IntersectsUnsafe(ab, bb); + + if (aaIntersects) + { + DispatchTestForNodes(ref aa, ref ba); + } + if (abIntersects) + { + DispatchTestForNodes(ref aa, ref bb); + } + if (baIntersects) + { + DispatchTestForNodes(ref ab, ref ba); + } + if (bbIntersects) + { + DispatchTestForNodes(ref ab, ref bb); + } + } + + public unsafe void GetOverlapsInNode(ref Node node) + { + + ref var a = ref node.A; + ref var b = ref node.B; + + var ab = BoundingBox.IntersectsUnsafe(a, b); + + if (a.Index >= 0) + GetOverlapsInNode(ref Nodes[a.Index]); + if (b.Index >= 0) + GetOverlapsInNode(ref Nodes[b.Index]); + + //Test all different nodes. + if (ab) + { + DispatchTestForNodes(ref a, ref b); + } + } + } + public unsafe void GetSelfOverlaps5(ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler + { + //If there are less than two leaves, there can't be any overlap. + //This provides a guarantee that there are at least 2 children in each internal node considered by GetOverlapsInNode. + if (LeafCount < 2) + return; + + SelfTest5 hmm = default; + hmm.Nodes = Nodes; + hmm.Results = ref results; + hmm.GetOverlapsInNode(ref Nodes[0]); + } + } } diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index ebeb2c94d..6045666fe 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -30,7 +30,7 @@ public void Handle(int indexA, int indexB) { ++OverlapCount; OverlapSum += indexA + indexB; - OverlapHash = indexA * OverlapCount ^ indexB * OverlapCount * OverlapCount; + OverlapHash += (indexA + (indexB * OverlapCount)) * OverlapCount; } } @@ -48,31 +48,64 @@ public override void Initialize(ContentArchive content, Camera camera) Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); int testCount = 1000; - var overlapHandlerPre = new OverlapHandler(); - var startTimePre = Stopwatch.GetTimestamp(); + //var overlapHandlerLoopWithRecursion = new OverlapHandler(); + //var startTimeLoopRecursion = Stopwatch.GetTimestamp(); + //for (int i = 0; i < testCount; ++i) + // mesh.Tree.GetSelfOverlapsPrepassWithRecursion(ref overlapHandlerLoopWithRecursion, BufferPool); + //var endTimeLoopRecursion = Stopwatch.GetTimestamp(); + //Console.WriteLine($"LRecurse time per execution (ms): {(endTimeLoopRecursion - startTimeLoopRecursion) * 1e3 / (testCount * Stopwatch.Frequency)}"); + //Console.WriteLine($"LRecurse count: {overlapHandlerLoopWithRecursion.OverlapCount}, sum {overlapHandlerLoopWithRecursion.OverlapSum}, hash {overlapHandlerLoopWithRecursion.OverlapHash}"); + + //var overlapHandlerPre = new OverlapHandler(); + //var startTimePre = Stopwatch.GetTimestamp(); + //for (int i = 0; i < testCount; ++i) + // mesh.Tree.GetSelfOverlapsContiguousPrepass(ref overlapHandlerPre, BufferPool); + //var endTimePre = Stopwatch.GetTimestamp(); + //Console.WriteLine($"CPrepass time per execution (ms): {(endTimePre - startTimePre) * 1e3 / (testCount * Stopwatch.Frequency)}"); + //Console.WriteLine($"CPrepass count: {overlapHandlerPre.OverlapCount}, sum {overlapHandlerPre.OverlapSum}, hash {overlapHandlerPre.OverlapHash}"); + + //var overlapHandler2 = new OverlapHandler(); + //var startTime2 = Stopwatch.GetTimestamp(); + //for (int i = 0; i < testCount; ++i) + // mesh.Tree.GetSelfOverlaps2(ref overlapHandler2, BufferPool); + //var endTime2 = Stopwatch.GetTimestamp(); + //Console.WriteLine($"Revamp 2 time per execution (ms): {(endTime2 - startTime2) * 1e3 / (testCount * Stopwatch.Frequency)}"); + //Console.WriteLine($"Revamp 2 count: {overlapHandler2.OverlapCount}, sum {overlapHandler2.OverlapSum}, hash {overlapHandler2.OverlapHash}"); + + //var overlapHandler3 = new OverlapHandler(); + //var startTime3 = Stopwatch.GetTimestamp(); + //for (int i = 0; i < testCount; ++i) + // mesh.Tree.GetSelfOverlaps3(ref overlapHandler3, BufferPool); + //var endTime3 = Stopwatch.GetTimestamp(); + //Console.WriteLine($"Revamp 3 time per execution (ms): {(endTime3 - startTime3) * 1e3 / (testCount * Stopwatch.Frequency)}"); + //Console.WriteLine($"Revamp 3 count: {overlapHandler3.OverlapCount}, sum {overlapHandler3.OverlapSum}, hash {overlapHandler3.OverlapHash}"); + + var overlapHandler4 = new OverlapHandler(); + var startTime4 = Stopwatch.GetTimestamp(); for (int i = 0; i < testCount; ++i) - mesh.Tree.GetSelfOverlapsContiguousPrepass(ref overlapHandlerPre, BufferPool); - var endTimePre = Stopwatch.GetTimestamp(); + mesh.Tree.GetSelfOverlaps4(ref overlapHandler4); + var endTime4 = Stopwatch.GetTimestamp(); + Console.WriteLine($"Revamp 4 time per execution (ms): {(endTime4 - startTime4) * 1e3 / (testCount * Stopwatch.Frequency)}"); + Console.WriteLine($"Revamp 4 count: {overlapHandler4.OverlapCount}, sum {overlapHandler4.OverlapSum}, hash {overlapHandler4.OverlapHash}"); - var overlapHandlerNew = new OverlapHandler(); - var startTimeNew = Stopwatch.GetTimestamp(); + var overlapHandler5 = new OverlapHandler(); + var startTime5 = Stopwatch.GetTimestamp(); for (int i = 0; i < testCount; ++i) - mesh.Tree.GetSelfOverlaps2(ref overlapHandlerNew, BufferPool); - var endTimeNew = Stopwatch.GetTimestamp(); + mesh.Tree.GetSelfOverlaps5(ref overlapHandler4); + var endTime5 = Stopwatch.GetTimestamp(); + Console.WriteLine($"Revamp 5 time per execution (ms): {(endTime5 - startTime5) * 1e3 / (testCount * Stopwatch.Frequency)}"); + Console.WriteLine($"Revamp 5 count: {overlapHandler4.OverlapCount}, sum {overlapHandler4.OverlapSum}, hash {overlapHandler5.OverlapHash}"); var overlapHandlerOld = new OverlapHandler(); var startTimeOld = Stopwatch.GetTimestamp(); for (int i = 0; i < testCount; ++i) mesh.Tree.GetSelfOverlaps(ref overlapHandlerOld); var endTimeOld = Stopwatch.GetTimestamp(); - - Console.WriteLine($"CPrepass time per execution (ms): {(endTimePre - startTimePre) * 1e3 / (testCount * Stopwatch.Frequency)}"); - Console.WriteLine($"Revamped time per execution (ms): {(endTimeNew - startTimeNew) * 1e3 / (testCount * Stopwatch.Frequency)}"); Console.WriteLine($"Original time per execution (ms): {(endTimeOld - startTimeOld) * 1e3 / (testCount * Stopwatch.Frequency)}"); - - Console.WriteLine($"Revamped count: {overlapHandlerPre.OverlapCount}, sum {overlapHandlerPre.OverlapSum}, hash {overlapHandlerPre.OverlapHash}"); - Console.WriteLine($"Revamped count: {overlapHandlerNew.OverlapCount}, sum {overlapHandlerNew.OverlapSum}, hash {overlapHandlerNew.OverlapHash}"); Console.WriteLine($"Original count: {overlapHandlerOld.OverlapCount}, sum {overlapHandlerOld.OverlapSum}, hash {overlapHandlerOld.OverlapHash}"); + + + } From 112738c724e2f5a35e59affb642c1416ea5cef29 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 2 Sep 2022 20:02:04 -0500 Subject: [PATCH 583/947] Peculiar! --- BepuPhysics/Trees/Tree_SelfQueries.cs | 87 ++++++++++--------- .../SpecializedTests/TreeFiddlingTestDemo.cs | 10 +-- 2 files changed, 52 insertions(+), 45 deletions(-) diff --git a/BepuPhysics/Trees/Tree_SelfQueries.cs b/BepuPhysics/Trees/Tree_SelfQueries.cs index aee5d7790..9e2418089 100644 --- a/BepuPhysics/Trees/Tree_SelfQueries.cs +++ b/BepuPhysics/Trees/Tree_SelfQueries.cs @@ -1152,44 +1152,51 @@ public unsafe void GetSelfOverlaps4(ref TOverlapHandler results hmm.GetOverlapsInNode(ref Nodes[0]); } - ref struct SelfTest5 where TOverlapHandler : IOverlapHandler + struct SelfTest5 where TOverlapHandler : IOverlapHandler { public Buffer Nodes; - public ref TOverlapHandler Results; + public TOverlapHandler Results; //When using a leaf-node test. - public int LeafIndex; - public ref NodeChild LeafChild; + public QuickList LeafStack; - unsafe void DispatchTestForLeaf(int nodeIndex) + unsafe void TestLeafAgainstNode(in NodeChild leafChild, int nodeToTest) { - if (nodeIndex < 0) - { - Results.Handle(LeafIndex, Encode(nodeIndex)); - } - else - { - TestLeafAgainstNode(nodeIndex); - } - } - unsafe void TestLeafAgainstNode(int nodeIndex) - { - ref var node = ref Nodes[nodeIndex]; - ref var a = ref node.A; - ref var b = ref node.B; - //Despite recursion, leafBounds should remain in L1- it'll be used all the way down the recursion from here. - //However, while we likely loaded child B when we loaded child A, there's no guarantee that it will stick around. - //Reloading that in the event of eviction would require more work than keeping the derived data on the stack. - //TODO: this is some pretty questionable microtuning. It's not often that the post-leaf-found recursion will be long enough to evict L1. Definitely test it. - var bIndex = b.Index; - var aIntersects = BoundingBox.IntersectsUnsafe(LeafChild, a); - var bIntersects = BoundingBox.IntersectsUnsafe(LeafChild, b); - if (aIntersects) - { - DispatchTestForLeaf(a.Index); - } - if (bIntersects) + var leafIndex = Encode(leafChild.Index); + Debug.Assert(LeafStack.Count == 0); + while (true) { - DispatchTestForLeaf(bIndex); + ref var node = ref Nodes[nodeToTest]; + var aIntersects = BoundingBox.IntersectsUnsafe(leafChild, node.A); + var bIntersects = BoundingBox.IntersectsUnsafe(leafChild, node.B); + var aIsInternal = node.A.Index >= 0; + var bIsInternal = node.B.Index >= 0; + + if (aIntersects && !aIsInternal) + { + Results.Handle(leafIndex, Encode(node.A.Index)); + } + if (bIntersects && !bIsInternal) + { + Results.Handle(leafIndex, Encode(node.B.Index)); + } + + if (aIntersects && aIsInternal) + { + nodeToTest = node.A.Index; + if (bIntersects && bIsInternal) + { + LeafStack.AllocateUnsafely() = node.B.Index; + } + } + else if (bIntersects && bIsInternal) + { + nodeToTest = node.B.Index; + } + else if (!LeafStack.TryPop(out nodeToTest)) + { + //Nothing left to test against this leaf! Done! + break; + } } } @@ -1205,17 +1212,13 @@ unsafe void DispatchTestForNodes(ref NodeChild a, ref NodeChild b) else { //leaf B versus node A. - LeafIndex = Encode(b.Index); - LeafChild = ref b; - TestLeafAgainstNode(a.Index); + TestLeafAgainstNode(b, a.Index); } } else if (b.Index >= 0) { //leaf A versus node B. - LeafIndex = Encode(a.Index); - LeafChild = ref a; - TestLeafAgainstNode(b.Index); + TestLeafAgainstNode(a, b.Index); } else { @@ -1274,7 +1277,7 @@ public unsafe void GetOverlapsInNode(ref Node node) } } } - public unsafe void GetSelfOverlaps5(ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler + public unsafe void GetSelfOverlaps5(ref TOverlapHandler results, BufferPool pool) where TOverlapHandler : IOverlapHandler { //If there are less than two leaves, there can't be any overlap. //This provides a guarantee that there are at least 2 children in each internal node considered by GetOverlapsInNode. @@ -1283,8 +1286,12 @@ public unsafe void GetSelfOverlaps5(ref TOverlapHandler results SelfTest5 hmm = default; hmm.Nodes = Nodes; - hmm.Results = ref results; + hmm.Results = results; + hmm.LeafStack = new QuickList(NodeCount, pool); hmm.GetOverlapsInNode(ref Nodes[0]); + + results = hmm.Results; + hmm.LeafStack.Dispose(pool); } } diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index 6045666fe..3390cd37c 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -41,13 +41,13 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0; Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(4, 1)); - var width = 128; - var height = 128; + var width = 512; + var height = 512; var scale = new Vector3(1, 1, 1); DemoMeshHelper.CreateDeformedPlane(width, height, (x, y) => new Vector3(x - width * scale.X * 0.5f, 2f * (float)(Math.Sin(x * 0.5f) * Math.Sin(y * 0.5f)), y - height * scale.Y * 0.5f), scale, BufferPool, out var mesh); Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); - int testCount = 1000; + int testCount = 100; //var overlapHandlerLoopWithRecursion = new OverlapHandler(); //var startTimeLoopRecursion = Stopwatch.GetTimestamp(); //for (int i = 0; i < testCount; ++i) @@ -91,10 +91,10 @@ public override void Initialize(ContentArchive content, Camera camera) var overlapHandler5 = new OverlapHandler(); var startTime5 = Stopwatch.GetTimestamp(); for (int i = 0; i < testCount; ++i) - mesh.Tree.GetSelfOverlaps5(ref overlapHandler4); + mesh.Tree.GetSelfOverlaps5(ref overlapHandler5, BufferPool); var endTime5 = Stopwatch.GetTimestamp(); Console.WriteLine($"Revamp 5 time per execution (ms): {(endTime5 - startTime5) * 1e3 / (testCount * Stopwatch.Frequency)}"); - Console.WriteLine($"Revamp 5 count: {overlapHandler4.OverlapCount}, sum {overlapHandler4.OverlapSum}, hash {overlapHandler5.OverlapHash}"); + Console.WriteLine($"Revamp 5 count: {overlapHandler5.OverlapCount}, sum {overlapHandler5.OverlapSum}, hash {overlapHandler5.OverlapHash}"); var overlapHandlerOld = new OverlapHandler(); var startTimeOld = Stopwatch.GetTimestamp(); From 861c1d56c678a8ac48eaa0224f8b7fa665edbad7 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 4 Sep 2022 14:15:17 -0500 Subject: [PATCH 584/947] Increased test difficulty. --- Demos/Demos.csproj | 2 +- Demos/SpecializedTests/CacheBlaster.cs | 2 +- .../SpecializedTests/TreeFiddlingTestDemo.cs | 86 ++++++------------- 3 files changed, 28 insertions(+), 62 deletions(-) diff --git a/Demos/Demos.csproj b/Demos/Demos.csproj index 032d20156..9bb89d121 100644 --- a/Demos/Demos.csproj +++ b/Demos/Demos.csproj @@ -5,7 +5,7 @@ True Debug;Release latest - + false diff --git a/Demos/SpecializedTests/CacheBlaster.cs b/Demos/SpecializedTests/CacheBlaster.cs index 628445c21..13aa378d7 100644 --- a/Demos/SpecializedTests/CacheBlaster.cs +++ b/Demos/SpecializedTests/CacheBlaster.cs @@ -11,7 +11,7 @@ namespace Demos.SpecializedTests { public static class CacheBlaster { - const int byteCount = (1 << 24); //16.7MB is bigger than most desktop last level caches. You'll want to pick something higher if you're running this on some ginormo xeon. + const int byteCount = 1 << 28; const int intCount = byteCount / 4; static int vectorCount = intCount / Vector.Count; static int vectorMask = vectorCount - 1; diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index 3390cd37c..fbbd82c68 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -14,6 +14,7 @@ using DemoRenderer; using BepuPhysics; using BepuPhysics.Constraints; +using BepuPhysics.Collidables; namespace Demos.SpecializedTests { @@ -41,73 +42,38 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0; Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(4, 1)); - var width = 512; - var height = 512; + var width = 768; + var height = 768; var scale = new Vector3(1, 1, 1); DemoMeshHelper.CreateDeformedPlane(width, height, (x, y) => new Vector3(x - width * scale.X * 0.5f, 2f * (float)(Math.Sin(x * 0.5f) * Math.Sin(y * 0.5f)), y - height * scale.Y * 0.5f), scale, BufferPool, out var mesh); Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); - int testCount = 100; - //var overlapHandlerLoopWithRecursion = new OverlapHandler(); - //var startTimeLoopRecursion = Stopwatch.GetTimestamp(); - //for (int i = 0; i < testCount; ++i) - // mesh.Tree.GetSelfOverlapsPrepassWithRecursion(ref overlapHandlerLoopWithRecursion, BufferPool); - //var endTimeLoopRecursion = Stopwatch.GetTimestamp(); - //Console.WriteLine($"LRecurse time per execution (ms): {(endTimeLoopRecursion - startTimeLoopRecursion) * 1e3 / (testCount * Stopwatch.Frequency)}"); - //Console.WriteLine($"LRecurse count: {overlapHandlerLoopWithRecursion.OverlapCount}, sum {overlapHandlerLoopWithRecursion.OverlapSum}, hash {overlapHandlerLoopWithRecursion.OverlapHash}"); - - //var overlapHandlerPre = new OverlapHandler(); - //var startTimePre = Stopwatch.GetTimestamp(); - //for (int i = 0; i < testCount; ++i) - // mesh.Tree.GetSelfOverlapsContiguousPrepass(ref overlapHandlerPre, BufferPool); - //var endTimePre = Stopwatch.GetTimestamp(); - //Console.WriteLine($"CPrepass time per execution (ms): {(endTimePre - startTimePre) * 1e3 / (testCount * Stopwatch.Frequency)}"); - //Console.WriteLine($"CPrepass count: {overlapHandlerPre.OverlapCount}, sum {overlapHandlerPre.OverlapSum}, hash {overlapHandlerPre.OverlapHash}"); - - //var overlapHandler2 = new OverlapHandler(); - //var startTime2 = Stopwatch.GetTimestamp(); - //for (int i = 0; i < testCount; ++i) - // mesh.Tree.GetSelfOverlaps2(ref overlapHandler2, BufferPool); - //var endTime2 = Stopwatch.GetTimestamp(); - //Console.WriteLine($"Revamp 2 time per execution (ms): {(endTime2 - startTime2) * 1e3 / (testCount * Stopwatch.Frequency)}"); - //Console.WriteLine($"Revamp 2 count: {overlapHandler2.OverlapCount}, sum {overlapHandler2.OverlapSum}, hash {overlapHandler2.OverlapHash}"); - - //var overlapHandler3 = new OverlapHandler(); - //var startTime3 = Stopwatch.GetTimestamp(); - //for (int i = 0; i < testCount; ++i) - // mesh.Tree.GetSelfOverlaps3(ref overlapHandler3, BufferPool); - //var endTime3 = Stopwatch.GetTimestamp(); - //Console.WriteLine($"Revamp 3 time per execution (ms): {(endTime3 - startTime3) * 1e3 / (testCount * Stopwatch.Frequency)}"); - //Console.WriteLine($"Revamp 3 count: {overlapHandler3.OverlapCount}, sum {overlapHandler3.OverlapSum}, hash {overlapHandler3.OverlapHash}"); - - var overlapHandler4 = new OverlapHandler(); - var startTime4 = Stopwatch.GetTimestamp(); - for (int i = 0; i < testCount; ++i) - mesh.Tree.GetSelfOverlaps4(ref overlapHandler4); - var endTime4 = Stopwatch.GetTimestamp(); - Console.WriteLine($"Revamp 4 time per execution (ms): {(endTime4 - startTime4) * 1e3 / (testCount * Stopwatch.Frequency)}"); - Console.WriteLine($"Revamp 4 count: {overlapHandler4.OverlapCount}, sum {overlapHandler4.OverlapSum}, hash {overlapHandler4.OverlapHash}"); + //Test((ref OverlapHandler handler) => mesh.Tree.GetSelfOverlapsPrepassWithRecursion(ref handler, BufferPool), "LRecurse"); + //Test((ref OverlapHandler handler) => mesh.Tree.GetSelfOverlapsContiguousPrepass(ref handler, BufferPool), "Prepass"); + //Test((ref OverlapHandler handler) => mesh.Tree.GetSelfOverlaps2(ref handler, BufferPool), "Revamp 2"); + //Test((ref OverlapHandler handler) => mesh.Tree.GetSelfOverlaps3(ref handler, BufferPool), "Revamp 3"); + Test((ref OverlapHandler handler) => mesh.Tree.GetSelfOverlaps4(ref handler), "Revamp 4"); + //Test((ref OverlapHandler handler) => mesh.Tree.GetSelfOverlaps5(ref handler, BufferPool), "Revamp 5"); + Test((ref OverlapHandler handler) => mesh.Tree.GetSelfOverlaps(ref handler), "Original"); + } - var overlapHandler5 = new OverlapHandler(); - var startTime5 = Stopwatch.GetTimestamp(); - for (int i = 0; i < testCount; ++i) - mesh.Tree.GetSelfOverlaps5(ref overlapHandler5, BufferPool); - var endTime5 = Stopwatch.GetTimestamp(); - Console.WriteLine($"Revamp 5 time per execution (ms): {(endTime5 - startTime5) * 1e3 / (testCount * Stopwatch.Frequency)}"); - Console.WriteLine($"Revamp 5 count: {overlapHandler5.OverlapCount}, sum {overlapHandler5.OverlapSum}, hash {overlapHandler5.OverlapHash}"); + delegate void TestFunction(ref OverlapHandler handler); - var overlapHandlerOld = new OverlapHandler(); - var startTimeOld = Stopwatch.GetTimestamp(); + static void Test(TestFunction function, string name) + { + var overlapHandler = new OverlapHandler(); + long accumulatedTime = 0; + const int testCount = 16; for (int i = 0; i < testCount; ++i) - mesh.Tree.GetSelfOverlaps(ref overlapHandlerOld); - var endTimeOld = Stopwatch.GetTimestamp(); - Console.WriteLine($"Original time per execution (ms): {(endTimeOld - startTimeOld) * 1e3 / (testCount * Stopwatch.Frequency)}"); - Console.WriteLine($"Original count: {overlapHandlerOld.OverlapCount}, sum {overlapHandlerOld.OverlapSum}, hash {overlapHandlerOld.OverlapHash}"); - - - + { + var startTime = Stopwatch.GetTimestamp(); + function(ref overlapHandler); + var endTime = Stopwatch.GetTimestamp(); + accumulatedTime += endTime - startTime; + CacheBlaster.Blast(); + } + Console.WriteLine($"{name} time per execution (ms): {(accumulatedTime) * 1e3 / (testCount * Stopwatch.Frequency)}"); + Console.WriteLine($"{name} count: {overlapHandler.OverlapCount}, sum {overlapHandler.OverlapSum}, hash {overlapHandler.OverlapHash}"); } - - } } From 111704e266504ea771e3c94529e4a3e97b20250a Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 4 Sep 2022 15:27:42 -0500 Subject: [PATCH 585/947] More fiddling. Diminishing returns to any small changes in ST mode. Would require significant reshuffle to avoid or hide branch mispredicts. --- BepuPhysics/Trees/Tree_SelfQueries.cs | 21 +++++++++++++++++++ .../SpecializedTests/TreeFiddlingTestDemo.cs | 2 +- 2 files changed, 22 insertions(+), 1 deletion(-) diff --git a/BepuPhysics/Trees/Tree_SelfQueries.cs b/BepuPhysics/Trees/Tree_SelfQueries.cs index 9e2418089..038a0676a 100644 --- a/BepuPhysics/Trees/Tree_SelfQueries.cs +++ b/BepuPhysics/Trees/Tree_SelfQueries.cs @@ -194,6 +194,27 @@ unsafe void GetOverlapsWithLeaf(ref TOverlapHandler results, No } } } + + //ref var node = ref Nodes[nodeToTest]; + //var a = BoundingBox.IntersectsUnsafe(leaf, node.A); + //var b = BoundingBox.IntersectsUnsafe(leaf, node.B); + //var aIsInternal = node.A.Index >= 0; + //var bIsInternal = node.B.Index >= 0; + //var intersectedInternalA = a && aIsInternal; + //var intersectedInternalB = b && bIsInternal; + //if (a && !aIsInternal) + // results.Handle(leafIndex, Encode(node.A.Index)); + //if (b && !bIsInternal) + // results.Handle(leafIndex, Encode(node.B.Index)); + + //if (intersectedInternalA && intersectedInternalB) + // stack.AllocateUnsafely() = node.B.Index; + //else if (intersectedInternalA || intersectedInternalB) + // nodeToTest = intersectedInternalA ? node.A.Index : node.B.Index; + //else if (!stack.TryPop(out nodeToTest)) + // break; + + } } struct StackEntry diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index fbbd82c68..b57229fff 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -48,7 +48,7 @@ public override void Initialize(ContentArchive content, Camera camera) DemoMeshHelper.CreateDeformedPlane(width, height, (x, y) => new Vector3(x - width * scale.X * 0.5f, 2f * (float)(Math.Sin(x * 0.5f) * Math.Sin(y * 0.5f)), y - height * scale.Y * 0.5f), scale, BufferPool, out var mesh); Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); - //Test((ref OverlapHandler handler) => mesh.Tree.GetSelfOverlapsPrepassWithRecursion(ref handler, BufferPool), "LRecurse"); + Test((ref OverlapHandler handler) => mesh.Tree.GetSelfOverlapsPrepassWithRecursion(ref handler, BufferPool), "LRecurse"); //Test((ref OverlapHandler handler) => mesh.Tree.GetSelfOverlapsContiguousPrepass(ref handler, BufferPool), "Prepass"); //Test((ref OverlapHandler handler) => mesh.Tree.GetSelfOverlaps2(ref handler, BufferPool), "Revamp 2"); //Test((ref OverlapHandler handler) => mesh.Tree.GetSelfOverlaps3(ref handler, BufferPool), "Revamp 3"); From 56f04c1943da44c2f4fb0d723630a11e71b39a83 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 4 Sep 2022 16:06:25 -0500 Subject: [PATCH 586/947] Purgin'! --- BepuPhysics/Trees/Tree_SelfQueries.cs | 1095 ++--------------- .../SpecializedTests/TreeFiddlingTestDemo.cs | 7 +- 2 files changed, 135 insertions(+), 967 deletions(-) diff --git a/BepuPhysics/Trees/Tree_SelfQueries.cs b/BepuPhysics/Trees/Tree_SelfQueries.cs index 038a0676a..a9830e489 100644 --- a/BepuPhysics/Trees/Tree_SelfQueries.cs +++ b/BepuPhysics/Trees/Tree_SelfQueries.cs @@ -223,531 +223,6 @@ struct StackEntry public int B; } - - public unsafe void GetSelfOverlaps2(ref TOverlapHandler results, BufferPool pool) where TOverlapHandler : IOverlapHandler - { - //If there are less than two leaves, there can't be any overlap. - //This provides a guarantee that there are at least 2 children in each internal node considered by GetOverlapsInNode. - if (LeafCount < 2) - return; - - QuickList stack = new(NodeCount, pool); - QuickList leafStack = new(NodeCount, pool); - StackEntry nextTest = default; - - while (true) - { - if (nextTest.A == nextTest.B) - { - //Self test. - //Possible sources of further stack entries in a self test: - //1) Child A and B are both internal and their bounds intersect. - //2) Child A is internal. - //3) Child B is internal. - //We can also spawn leaf-subtree tests if: - //1) Child A and B have intersecting bounds, and only one of them is a leaf. - //We can spawn direct leaf tests if: - //1) Child A and B have intersecting bounds, and both are leaves. - //Note that we never need to push an entry with differing A and B indices to the stack *from a self test*. There can only be one such entry created from any self test, and it's always visited next. - //Non-self tests will generate *only* results with differing A and B indices. - ref var node = ref Nodes[nextTest.A]; - var abIntersect = BoundingBox.IntersectsUnsafe(node.A, node.B); - var aIsInternal = node.A.Index >= 0; - var bIsInternal = node.B.Index >= 0; - - if (aIsInternal) - { - nextTest.A = node.A.Index; - nextTest.B = node.A.Index; - - if (bIsInternal) - { - ref var stackB = ref stack.AllocateUnsafely(); - stackB.A = node.B.Index; - stackB.B = node.B.Index; - - if (abIntersect) - { - ref var stackEntry = ref stack.AllocateUnsafely(); - stackEntry.A = node.A.Index; - stackEntry.B = node.B.Index; - } - } - else if (abIntersect) - { - GetOverlapsWithLeaf(ref results, node.B, node.A.Index, ref leafStack); - } - } - else if (bIsInternal) - { - nextTest.A = node.B.Index; - nextTest.B = node.B.Index; - - if (abIntersect) - { - GetOverlapsWithLeaf(ref results, node.A, node.B.Index, ref leafStack); - } - } - else if (abIntersect) - { - //Both children are leaves. They have overlapping bounds, so... - results.Handle(Encode(node.A.Index), Encode(node.B.Index)); - //No new tests available, so grab from the stack. - if (!stack.TryPop(out nextTest)) - break; - } - } - else - { - //Not a self test! - //Possible sources of stack entry: - //1) AA intersection, both internal - //2) AB intersection, both internal - //3) BA intersection, both internal - //4) BB intersection, both internal - ref var n0 = ref Nodes[nextTest.A]; - ref var n1 = ref Nodes[nextTest.B]; - var aaIntersects = BoundingBox.IntersectsUnsafe(n0.A, n1.A); - var abIntersects = BoundingBox.IntersectsUnsafe(n0.A, n1.B); - var baIntersects = BoundingBox.IntersectsUnsafe(n0.B, n1.A); - var bbIntersects = BoundingBox.IntersectsUnsafe(n0.B, n1.B); - var n0AIsInternal = n0.A.Index >= 0; - var n0BIsInternal = n0.B.Index >= 0; - var n1AIsInternal = n1.A.Index >= 0; - var n1BIsInternal = n1.B.Index >= 0; - //The first test which generates a stack candidate gets the nextTest; the rest get pushed to the stack. - int previousStackGeneratorCount = 0; - if (aaIntersects) - { - if (n0AIsInternal && n1AIsInternal) - { - ++previousStackGeneratorCount; - nextTest.A = n0.A.Index; - nextTest.B = n1.A.Index; - } - else - { - //At least one is a leaf. - if (n0AIsInternal) - GetOverlapsWithLeaf(ref results, n1.A, n0.A.Index, ref leafStack); - else if (n1AIsInternal) - GetOverlapsWithLeaf(ref results, n0.A, n1.A.Index, ref leafStack); - else //Both are leaves. - results.Handle(Encode(n0.A.Index), Encode(n1.A.Index)); - } - } - if (abIntersects) - { - if (n0AIsInternal && n1BIsInternal) - { - if (previousStackGeneratorCount++ == 0) - { - nextTest.A = n0.A.Index; - nextTest.B = n1.B.Index; - } - else - { - ref var stackEntry = ref stack.AllocateUnsafely(); - stackEntry.A = n0.A.Index; - stackEntry.B = n1.B.Index; - } - } - else - { - //At least one is a leaf. - if (n0AIsInternal) - GetOverlapsWithLeaf(ref results, n1.B, n0.A.Index, ref leafStack); - else if (n1BIsInternal) - GetOverlapsWithLeaf(ref results, n0.A, n1.B.Index, ref leafStack); - else //Both are leaves. - results.Handle(Encode(n0.A.Index), Encode(n1.B.Index)); - } - } - if (baIntersects) - { - if (n0BIsInternal && n1AIsInternal) - { - if (previousStackGeneratorCount++ == 0) - { - nextTest.A = n0.B.Index; - nextTest.B = n1.A.Index; - } - else - { - ref var stackEntry = ref stack.AllocateUnsafely(); - stackEntry.A = n0.B.Index; - stackEntry.B = n1.A.Index; - } - } - else - { - //At least one is a leaf. - if (n0BIsInternal) - GetOverlapsWithLeaf(ref results, n1.A, n0.B.Index, ref leafStack); - else if (n1AIsInternal) - GetOverlapsWithLeaf(ref results, n0.B, n1.A.Index, ref leafStack); - else //Both are leaves. - results.Handle(Encode(n0.B.Index), Encode(n1.A.Index)); - } - } - if (bbIntersects) - { - if (n0BIsInternal && n1BIsInternal) - { - if (previousStackGeneratorCount++ == 0) - { - nextTest.A = n0.B.Index; - nextTest.B = n1.B.Index; - } - else - { - ref var stackEntry = ref stack.AllocateUnsafely(); - stackEntry.A = n0.B.Index; - stackEntry.B = n1.B.Index; - } - } - else - { - //At least one is a leaf. - if (n0BIsInternal) - GetOverlapsWithLeaf(ref results, n1.B, n0.B.Index, ref leafStack); - else if (n1BIsInternal) - GetOverlapsWithLeaf(ref results, n0.B, n1.B.Index, ref leafStack); - else //Both are leaves. - results.Handle(Encode(n0.B.Index), Encode(n1.B.Index)); - } - } - if (previousStackGeneratorCount == 0) - { - //None of the candidates generated a next step, so grab from the stack. - if (!stack.TryPop(out nextTest)) - break; - } - } - - } - leafStack.Dispose(pool); - stack.Dispose(pool); - - } - - enum StackEntryType - { - SelfTest, - Crossover, - Leaf, - } - struct ComboStackEntry - { - public int EncodedA; - public int B; - - public StackEntryType Type => EncodedA < 0 ? StackEntryType.Leaf : EncodedA == B ? StackEntryType.SelfTest : StackEntryType.Crossover; - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static ComboStackEntry CreateForLeafTest(int leafParentIndex, bool leafChildIsB, int nodeIndex) - { - ComboStackEntry entry; - //Upper bit flags this stack entry as a leaf versus node test. - entry.EncodedA = leafParentIndex | (1 << 31); - if (leafChildIsB) - entry.EncodedA |= 1 << 30; - entry.B = nodeIndex; - return entry; - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static ComboStackEntry CreateForSelfTest(int nodeIndex) - { - ComboStackEntry entry; - entry.EncodedA = nodeIndex; - entry.B = nodeIndex; - return entry; - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static ComboStackEntry CreateCrossover(int a, int b) - { - ComboStackEntry entry; - entry.EncodedA = a; - entry.B = b; - return entry; - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref NodeChild GetLeafChild(ref Tree tree) - { - var parentNodeIndex = EncodedA & 0x3FFF_FFFF; - var leafIsChildB = (EncodedA & (1 << 30)) != 0; - ref var parent = ref tree.Nodes[parentNodeIndex]; - return ref leafIsChildB ? ref parent.B : ref parent.A; - } - } - - public unsafe void GetSelfOverlaps3(ref TOverlapHandler results, BufferPool pool) where TOverlapHandler : IOverlapHandler - { - //If there are less than two leaves, there can't be any overlap. - //This provides a guarantee that there are at least 2 children in each internal node considered by GetOverlapsInNode. - if (LeafCount < 2) - return; - - QuickList stack = new(NodeCount, pool); - QuickList leafStack = new(NodeCount, pool); - ComboStackEntry nextTest = default; - - while (true) - { - switch (nextTest.Type) - { - case StackEntryType.SelfTest: - { - //Self test. - //Possible sources of further stack entries in a self test: - //1) Child A and B are both internal and their bounds intersect. - //2) Child A is internal. - //3) Child B is internal. - //We can also spawn leaf-subtree tests if: - //1) Child A and B have intersecting bounds, and only one of them is a leaf. - //We can spawn direct leaf tests if: - //1) Child A and B have intersecting bounds, and both are leaves. - //Note that we never need to push an entry with differing A and B indices to the stack *from a self test*. There can only be one such entry created from any self test, and it's always visited next. - //Non-self tests will generate *only* results with differing A and B indices. - ref var node = ref Nodes[nextTest.B]; - //GetOverlapsInNode(ref node, ref results); - //if (!stack.TryPop(out nextTest)) - //{ - // goto Terminate; - //} - //break; - var abIntersect = BoundingBox.IntersectsUnsafe(node.A, node.B); - var aIsInternal = node.A.Index >= 0; - var bIsInternal = node.B.Index >= 0; - - if (aIsInternal) - { - if (bIsInternal) - { - stack.AllocateUnsafely() = ComboStackEntry.CreateForSelfTest(node.B.Index); - if (abIntersect) - { - stack.AllocateUnsafely() = ComboStackEntry.CreateCrossover(node.A.Index, node.B.Index); - } - } - else if (abIntersect) - { - stack.AllocateUnsafely() = ComboStackEntry.CreateForLeafTest(nextTest.B, true, node.A.Index); - } - nextTest = ComboStackEntry.CreateForSelfTest(node.A.Index); - } - else if (bIsInternal) - { - if (abIntersect) - { - stack.AllocateUnsafely() = ComboStackEntry.CreateForLeafTest(nextTest.EncodedA, false, node.B.Index); - } - nextTest = ComboStackEntry.CreateForSelfTest(node.B.Index); - } - else if (abIntersect) - { - //Both children are leaves. They have overlapping bounds, so... - results.Handle(Encode(node.A.Index), Encode(node.B.Index)); - //No new tests available, so grab from the stack. - if (!stack.TryPop(out nextTest)) - { - goto Terminate; - } - } - } - break; - case StackEntryType.Crossover: - { - //Not a self test! - //Possible sources of stack entry: - //1) AA intersection, both internal - //2) AB intersection, both internal - //3) BA intersection, both internal - //4) BB intersection, both internal - var n0Index = nextTest.EncodedA; - var n1Index = nextTest.B; - ref var n0 = ref Nodes[nextTest.EncodedA]; - ref var n1 = ref Nodes[nextTest.B]; - - //GetOverlapsBetweenDifferentNodes(ref n0, ref n1, ref results); - //if (!stack.TryPop(out nextTest)) - //{ - // goto Terminate; - //} - //break; - var aaIntersects = BoundingBox.IntersectsUnsafe(n0.A, n1.A); - var abIntersects = BoundingBox.IntersectsUnsafe(n0.A, n1.B); - var baIntersects = BoundingBox.IntersectsUnsafe(n0.B, n1.A); - var bbIntersects = BoundingBox.IntersectsUnsafe(n0.B, n1.B); - var n0AIsInternal = n0.A.Index >= 0; - var n0BIsInternal = n0.B.Index >= 0; - var n1AIsInternal = n1.A.Index >= 0; - var n1BIsInternal = n1.B.Index >= 0; - //The first test which generates a stack candidate gets the nextTest; the rest get pushed to the stack. - int previousStackGeneratorCount = 0; - if (aaIntersects) - { - if (n0AIsInternal && n1AIsInternal) - { - ++previousStackGeneratorCount; - nextTest = ComboStackEntry.CreateCrossover(n0.A.Index, n1.A.Index); - } - else - { - //At least one is a leaf. - if (n0AIsInternal) - stack.AllocateUnsafely() = ComboStackEntry.CreateForLeafTest(n1Index, false, n0.A.Index); - else if (n1AIsInternal) - stack.AllocateUnsafely() = ComboStackEntry.CreateForLeafTest(n0Index, false, n1.A.Index); - else //Both are leaves. - results.Handle(Encode(n0.A.Index), Encode(n1.A.Index)); - } - } - if (abIntersects) - { - if (n0AIsInternal && n1BIsInternal) - { - if (previousStackGeneratorCount++ == 0) - { - nextTest = ComboStackEntry.CreateCrossover(n0.A.Index, n1.B.Index); - } - else - { - stack.AllocateUnsafely() = ComboStackEntry.CreateCrossover(n0.A.Index, n1.B.Index); - } - } - else - { - //At least one is a leaf. - if (n0AIsInternal) - stack.AllocateUnsafely() = ComboStackEntry.CreateForLeafTest(n1Index, true, n0.A.Index); - else if (n1BIsInternal) - stack.AllocateUnsafely() = ComboStackEntry.CreateForLeafTest(n0Index, false, n1.B.Index); - else //Both are leaves. - results.Handle(Encode(n0.A.Index), Encode(n1.B.Index)); - } - } - if (baIntersects) - { - if (n0BIsInternal && n1AIsInternal) - { - if (previousStackGeneratorCount++ == 0) - { - nextTest = ComboStackEntry.CreateCrossover(n0.B.Index, n1.A.Index); - } - else - { - stack.AllocateUnsafely() = ComboStackEntry.CreateCrossover(n0.B.Index, n1.A.Index); - } - } - else - { - //At least one is a leaf. - if (n0BIsInternal) - stack.AllocateUnsafely() = ComboStackEntry.CreateForLeafTest(n1Index, false, n0.B.Index); - else if (n1AIsInternal) - stack.AllocateUnsafely() = ComboStackEntry.CreateForLeafTest(n0Index, true, n1.A.Index); - else //Both are leaves. - results.Handle(Encode(n0.B.Index), Encode(n1.A.Index)); - } - } - if (bbIntersects) - { - if (n0BIsInternal && n1BIsInternal) - { - if (previousStackGeneratorCount++ == 0) - { - nextTest = ComboStackEntry.CreateCrossover(n0.B.Index, n1.B.Index); - } - else - { - stack.AllocateUnsafely() = ComboStackEntry.CreateCrossover(n0.B.Index, n1.B.Index); - } - } - else - { - //At least one is a leaf. - if (n0BIsInternal) - stack.AllocateUnsafely() = ComboStackEntry.CreateForLeafTest(n1Index, true, n0.B.Index); - else if (n1BIsInternal) - stack.AllocateUnsafely() = ComboStackEntry.CreateForLeafTest(n0Index, true, n1.B.Index); - else //Both are leaves. - results.Handle(Encode(n0.B.Index), Encode(n1.B.Index)); - } - } - if (previousStackGeneratorCount == 0) - { - //None of the candidates generated a next step, so grab from the stack. - if (!stack.TryPop(out nextTest)) - { - goto Terminate; - } - } - } - break; - default: - { - //Leaf-node test. Swap over to our own local stack. Equivalent visitation order. - ref var leafChild = ref nextTest.GetLeafChild(ref this); - var nodeToTest = nextTest.B; - //DispatchTestForLeaf(Encode(leafChild.Index), ref leafChild, nodeToTest, ref results); - //GetOverlapsWithLeaf(ref results, leafChild, nodeToTest, ref leafStack); - var leafIndex = Encode(leafChild.Index); - Debug.Assert(leafStack.Count == 0); - while (true) - { - ref var node = ref Nodes[nodeToTest]; - var aIntersects = BoundingBox.IntersectsUnsafe(leafChild, node.A); - var bIntersects = BoundingBox.IntersectsUnsafe(leafChild, node.B); - var aIsInternal = node.A.Index >= 0; - var bIsInternal = node.B.Index >= 0; - var intersectedInternalA = aIntersects && aIsInternal; - var intersectedInternalB = bIntersects && bIsInternal; - - if (aIntersects && !aIsInternal) - { - results.Handle(leafIndex, Encode(node.A.Index)); - } - if (bIntersects && !bIsInternal) - { - results.Handle(leafIndex, Encode(node.B.Index)); - } - - if (intersectedInternalA) - { - nodeToTest = node.A.Index; - if (intersectedInternalB) - { - leafStack.AllocateUnsafely() = node.B.Index; - } - } - else if (intersectedInternalB) - { - nodeToTest = node.B.Index; - } - else if (!leafStack.TryPop(out nodeToTest)) - { - //Nothing left to test against this leaf! Done! - break; - } - } - if (!stack.TryPop(out nextTest)) - { - goto Terminate; - } - } - break; - } - } - Terminate: - leafStack.Dispose(pool); - stack.Dispose(pool); - - } - struct LeafNodeStackEntry { public uint EncodedLeafParentIndex; @@ -817,135 +292,140 @@ public unsafe void GetSelfOverlapsContiguousPrepass(ref TOverla { while (true) { - //Possible sources of stack entry: - //1) AA intersection, both internal - //2) AB intersection, both internal - //3) BA intersection, both internal - //4) BB intersection, both internal - var parent0 = nextCrossover.A; - var parent1 = nextCrossover.B; - ref var n0 = ref Nodes[parent0]; - ref var n1 = ref Nodes[parent1]; - var aaIntersects = BoundingBox.IntersectsUnsafe(n0.A, n1.A); - var abIntersects = BoundingBox.IntersectsUnsafe(n0.A, n1.B); - var baIntersects = BoundingBox.IntersectsUnsafe(n0.B, n1.A); - var bbIntersects = BoundingBox.IntersectsUnsafe(n0.B, n1.B); - var n0A = n0.A.Index; - var n0B = n0.B.Index; - var n1A = n1.A.Index; - var n1B = n1.B.Index; - var n0AIsInternal = n0A >= 0; - var n0BIsInternal = n0B >= 0; - var n1AIsInternal = n1A >= 0; - var n1BIsInternal = n1B >= 0; - //The first test which generates a stack candidate gets the nextTest; the rest get pushed to the stack. - int previousStackGeneratorCount = 0; - if (aaIntersects) - { - if (n0AIsInternal && n1AIsInternal) - { - ++previousStackGeneratorCount; - nextCrossover.A = n0A; - nextCrossover.B = n1A; - } - else - { - //At least one is a leaf. - if (n0AIsInternal) - leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent1, false, n0A); - else if (n1AIsInternal) - leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent0, false, n1A); - else //Both are leaves. - results.Handle(Encode(n0A), Encode(n1A)); - } - } - if (abIntersects) - { - if (n0AIsInternal && n1BIsInternal) - { - if (previousStackGeneratorCount++ == 0) - { - nextCrossover.A = n0A; - nextCrossover.B = n1B; - } - else - { - ref var stackEntry = ref crossoverStack.AllocateUnsafely(); - stackEntry.A = n0A; - stackEntry.B = n1B; - } - } - else - { - //At least one is a leaf. - if (n0AIsInternal) - leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent1, true, n0A); - else if (n1BIsInternal) - leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent0, false, n1B); - else //Both are leaves. - results.Handle(Encode(n0A), Encode(n1B)); - } - } - if (baIntersects) - { - if (n0BIsInternal && n1AIsInternal) - { - if (previousStackGeneratorCount++ == 0) - { - nextCrossover.A = n0B; - nextCrossover.B = n1A; - } - else - { - ref var stackEntry = ref crossoverStack.AllocateUnsafely(); - stackEntry.A = n0B; - stackEntry.B = n1A; - } - } - else - { - //At least one is a leaf. - if (n0BIsInternal) - leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent1, false, n0B); - else if (n1AIsInternal) - leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent0, true, n1A); - else //Both are leaves. - results.Handle(Encode(n0B), Encode(n1A)); - } - } - if (bbIntersects) - { - if (n0BIsInternal && n1BIsInternal) - { - if (previousStackGeneratorCount++ == 0) - { - nextCrossover.A = n0B; - nextCrossover.B = n1B; - } - else - { - ref var stackEntry = ref crossoverStack.AllocateUnsafely(); - stackEntry.A = n0B; - stackEntry.B = n1B; - } - } - else - { - //At least one is a leaf. - if (n0BIsInternal) - leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent1, true, n0B); - else if (n1BIsInternal) - leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent0, true, n1B); - else //Both are leaves. - results.Handle(Encode(n0B), Encode(n1B)); - } - } - if (previousStackGeneratorCount == 0) - { - //None of the candidates generated a next step, so grab from the stack. - if (!crossoverStack.TryPop(out nextCrossover)) - break; - } + GetOverlapsBetweenDifferentNodes(ref Nodes[nextCrossover.A], ref Nodes[nextCrossover.B], ref results); + //None of the candidates generated a next step, so grab from the stack. + if (!crossoverStack.TryPop(out nextCrossover)) + break; + + ////Possible sources of stack entry: + ////1) AA intersection, both internal + ////2) AB intersection, both internal + ////3) BA intersection, both internal + ////4) BB intersection, both internal + //var parent0 = nextCrossover.A; + //var parent1 = nextCrossover.B; + //ref var n0 = ref Nodes[parent0]; + //ref var n1 = ref Nodes[parent1]; + //var aaIntersects = BoundingBox.IntersectsUnsafe(n0.A, n1.A); + //var abIntersects = BoundingBox.IntersectsUnsafe(n0.A, n1.B); + //var baIntersects = BoundingBox.IntersectsUnsafe(n0.B, n1.A); + //var bbIntersects = BoundingBox.IntersectsUnsafe(n0.B, n1.B); + //var n0A = n0.A.Index; + //var n0B = n0.B.Index; + //var n1A = n1.A.Index; + //var n1B = n1.B.Index; + //var n0AIsInternal = n0A >= 0; + //var n0BIsInternal = n0B >= 0; + //var n1AIsInternal = n1A >= 0; + //var n1BIsInternal = n1B >= 0; + ////The first test which generates a stack candidate gets the nextTest; the rest get pushed to the stack. + //int previousStackGeneratorCount = 0; + //if (aaIntersects) + //{ + // if (n0AIsInternal && n1AIsInternal) + // { + // ++previousStackGeneratorCount; + // nextCrossover.A = n0A; + // nextCrossover.B = n1A; + // } + // else + // { + // //At least one is a leaf. + // if (n0AIsInternal) + // leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent1, false, n0A); + // else if (n1AIsInternal) + // leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent0, false, n1A); + // else //Both are leaves. + // results.Handle(Encode(n0A), Encode(n1A)); + // } + //} + //if (abIntersects) + //{ + // if (n0AIsInternal && n1BIsInternal) + // { + // if (previousStackGeneratorCount++ == 0) + // { + // nextCrossover.A = n0A; + // nextCrossover.B = n1B; + // } + // else + // { + // ref var stackEntry = ref crossoverStack.AllocateUnsafely(); + // stackEntry.A = n0A; + // stackEntry.B = n1B; + // } + // } + // else + // { + // //At least one is a leaf. + // if (n0AIsInternal) + // leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent1, true, n0A); + // else if (n1BIsInternal) + // leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent0, false, n1B); + // else //Both are leaves. + // results.Handle(Encode(n0A), Encode(n1B)); + // } + //} + //if (baIntersects) + //{ + // if (n0BIsInternal && n1AIsInternal) + // { + // if (previousStackGeneratorCount++ == 0) + // { + // nextCrossover.A = n0B; + // nextCrossover.B = n1A; + // } + // else + // { + // ref var stackEntry = ref crossoverStack.AllocateUnsafely(); + // stackEntry.A = n0B; + // stackEntry.B = n1A; + // } + // } + // else + // { + // //At least one is a leaf. + // if (n0BIsInternal) + // leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent1, false, n0B); + // else if (n1AIsInternal) + // leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent0, true, n1A); + // else //Both are leaves. + // results.Handle(Encode(n0B), Encode(n1A)); + // } + //} + //if (bbIntersects) + //{ + // if (n0BIsInternal && n1BIsInternal) + // { + // if (previousStackGeneratorCount++ == 0) + // { + // nextCrossover.A = n0B; + // nextCrossover.B = n1B; + // } + // else + // { + // ref var stackEntry = ref crossoverStack.AllocateUnsafely(); + // stackEntry.A = n0B; + // stackEntry.B = n1B; + // } + // } + // else + // { + // //At least one is a leaf. + // if (n0BIsInternal) + // leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent1, true, n0B); + // else if (n1BIsInternal) + // leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent0, true, n1B); + // else //Both are leaves. + // results.Handle(Encode(n0B), Encode(n1B)); + // } + //} + //if (previousStackGeneratorCount == 0) + //{ + // //None of the candidates generated a next step, so grab from the stack. + // if (!crossoverStack.TryPop(out nextCrossover)) + // break; + //} } } crossoverStack.Dispose(pool); @@ -1005,315 +485,8 @@ public unsafe void GetSelfOverlapsContiguousPrepass(ref TOverla stack.Dispose(pool); } - public unsafe void GetSelfOverlapsPrepassWithRecursion(ref TOverlapHandler results, BufferPool pool) where TOverlapHandler : IOverlapHandler - { - //If there are less than two leaves, there can't be any overlap. - //This provides a guarantee that there are at least 2 children in each internal node considered by GetOverlapsInNode. - if (LeafCount < 2) - return; - - var leafStack = new QuickList(NodeCount, pool); - //A recursive self test will at some point visit all nodes with certainty. Instead of framing it as a recursive test at all, do a prepass that's just a contiguous iteration. - for (int i = 0; i < NodeCount; ++i) - { - ref var node = ref Nodes[i]; - var a = node.A.Index; - var b = node.B.Index; - var aIsInternal = a >= 0; - var bIsInternal = b >= 0; - if (aIsInternal && bIsInternal) - { - if (BoundingBox.IntersectsUnsafe(node.A, node.B)) - { - GetOverlapsBetweenDifferentNodes(ref Nodes[a], ref Nodes[b], ref results); - } - } - else if (aIsInternal || bIsInternal) - { - //One is a leaf, one is internal. - GetOverlapsWithLeaf(ref results, aIsInternal ? node.B : node.A, aIsInternal ? a : b, ref leafStack); - } - else - { - //Both are leaves. - results.Handle(Encode(a), Encode(b)); - } - } - leafStack.Dispose(pool); - } - - - - - ref struct SelfTest where TOverlapHandler : IOverlapHandler - { - public Buffer Nodes; - public ref TOverlapHandler results; - - unsafe void DispatchTestForLeaf(int leafIndex, ref NodeChild leafChild, int nodeIndex) - { - if (nodeIndex < 0) - { - results.Handle(leafIndex, Encode(nodeIndex)); - } - else - { - TestLeafAgainstNode(leafIndex, ref leafChild, nodeIndex); - } - } - unsafe void TestLeafAgainstNode(int leafIndex, ref NodeChild leafChild, int nodeIndex) - { - ref var node = ref Nodes[nodeIndex]; - ref var a = ref node.A; - ref var b = ref node.B; - //Despite recursion, leafBounds should remain in L1- it'll be used all the way down the recursion from here. - //However, while we likely loaded child B when we loaded child A, there's no guarantee that it will stick around. - //Reloading that in the event of eviction would require more work than keeping the derived data on the stack. - //TODO: this is some pretty questionable microtuning. It's not often that the post-leaf-found recursion will be long enough to evict L1. Definitely test it. - var bIndex = b.Index; - var aIntersects = BoundingBox.IntersectsUnsafe(leafChild, a); - var bIntersects = BoundingBox.IntersectsUnsafe(leafChild, b); - if (aIntersects) - { - DispatchTestForLeaf(leafIndex, ref leafChild, a.Index); - } - if (bIntersects) - { - DispatchTestForLeaf(leafIndex, ref leafChild, bIndex); - } - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - unsafe void DispatchTestForNodes(ref NodeChild a, ref NodeChild b) - { - if (a.Index >= 0) - { - if (b.Index >= 0) - { - GetOverlapsBetweenDifferentNodes(ref Nodes[a.Index], ref Nodes[b.Index]); - } - else - { - //leaf B versus node A. - TestLeafAgainstNode(Encode(b.Index), ref b, a.Index); - } - } - else if (b.Index >= 0) - { - //leaf A versus node B. - TestLeafAgainstNode(Encode(a.Index), ref a, b.Index); - } - else - { - //Two leaves. - results.Handle(Encode(a.Index), Encode(b.Index)); - } - } - - unsafe void GetOverlapsBetweenDifferentNodes(ref Node a, ref Node b) - { - //There are no shared children, so test them all. - ref var aa = ref a.A; - ref var ab = ref a.B; - ref var ba = ref b.A; - ref var bb = ref b.B; - var aaIntersects = BoundingBox.IntersectsUnsafe(aa, ba); - var abIntersects = BoundingBox.IntersectsUnsafe(aa, bb); - var baIntersects = BoundingBox.IntersectsUnsafe(ab, ba); - var bbIntersects = BoundingBox.IntersectsUnsafe(ab, bb); - - if (aaIntersects) - { - DispatchTestForNodes(ref aa, ref ba); - } - if (abIntersects) - { - DispatchTestForNodes(ref aa, ref bb); - } - if (baIntersects) - { - DispatchTestForNodes(ref ab, ref ba); - } - if (bbIntersects) - { - DispatchTestForNodes(ref ab, ref bb); - } - } - - public unsafe void GetOverlapsInNode(ref Node node) - { - - ref var a = ref node.A; - ref var b = ref node.B; - - var ab = BoundingBox.IntersectsUnsafe(a, b); - - if (a.Index >= 0) - GetOverlapsInNode(ref Nodes[a.Index]); - if (b.Index >= 0) - GetOverlapsInNode(ref Nodes[b.Index]); - - //Test all different nodes. - if (ab) - { - DispatchTestForNodes(ref a, ref b); - } - } - } - public unsafe void GetSelfOverlaps4(ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler - { - //If there are less than two leaves, there can't be any overlap. - //This provides a guarantee that there are at least 2 children in each internal node considered by GetOverlapsInNode. - if (LeafCount < 2) - return; - - SelfTest hmm; - hmm.Nodes = Nodes; - hmm.results = ref results; - hmm.GetOverlapsInNode(ref Nodes[0]); - } - - struct SelfTest5 where TOverlapHandler : IOverlapHandler - { - public Buffer Nodes; - public TOverlapHandler Results; - //When using a leaf-node test. - public QuickList LeafStack; - - unsafe void TestLeafAgainstNode(in NodeChild leafChild, int nodeToTest) - { - var leafIndex = Encode(leafChild.Index); - Debug.Assert(LeafStack.Count == 0); - while (true) - { - ref var node = ref Nodes[nodeToTest]; - var aIntersects = BoundingBox.IntersectsUnsafe(leafChild, node.A); - var bIntersects = BoundingBox.IntersectsUnsafe(leafChild, node.B); - var aIsInternal = node.A.Index >= 0; - var bIsInternal = node.B.Index >= 0; - - if (aIntersects && !aIsInternal) - { - Results.Handle(leafIndex, Encode(node.A.Index)); - } - if (bIntersects && !bIsInternal) - { - Results.Handle(leafIndex, Encode(node.B.Index)); - } - - if (aIntersects && aIsInternal) - { - nodeToTest = node.A.Index; - if (bIntersects && bIsInternal) - { - LeafStack.AllocateUnsafely() = node.B.Index; - } - } - else if (bIntersects && bIsInternal) - { - nodeToTest = node.B.Index; - } - else if (!LeafStack.TryPop(out nodeToTest)) - { - //Nothing left to test against this leaf! Done! - break; - } - } - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - unsafe void DispatchTestForNodes(ref NodeChild a, ref NodeChild b) - { - if (a.Index >= 0) - { - if (b.Index >= 0) - { - GetOverlapsBetweenDifferentNodes(ref Nodes[a.Index], ref Nodes[b.Index]); - } - else - { - //leaf B versus node A. - TestLeafAgainstNode(b, a.Index); - } - } - else if (b.Index >= 0) - { - //leaf A versus node B. - TestLeafAgainstNode(a, b.Index); - } - else - { - //Two leaves. - Results.Handle(Encode(a.Index), Encode(b.Index)); - } - } - - unsafe void GetOverlapsBetweenDifferentNodes(ref Node a, ref Node b) - { - //There are no shared children, so test them all. - ref var aa = ref a.A; - ref var ab = ref a.B; - ref var ba = ref b.A; - ref var bb = ref b.B; - var aaIntersects = BoundingBox.IntersectsUnsafe(aa, ba); - var abIntersects = BoundingBox.IntersectsUnsafe(aa, bb); - var baIntersects = BoundingBox.IntersectsUnsafe(ab, ba); - var bbIntersects = BoundingBox.IntersectsUnsafe(ab, bb); - - if (aaIntersects) - { - DispatchTestForNodes(ref aa, ref ba); - } - if (abIntersects) - { - DispatchTestForNodes(ref aa, ref bb); - } - if (baIntersects) - { - DispatchTestForNodes(ref ab, ref ba); - } - if (bbIntersects) - { - DispatchTestForNodes(ref ab, ref bb); - } - } - - public unsafe void GetOverlapsInNode(ref Node node) - { - - ref var a = ref node.A; - ref var b = ref node.B; - var ab = BoundingBox.IntersectsUnsafe(a, b); - if (a.Index >= 0) - GetOverlapsInNode(ref Nodes[a.Index]); - if (b.Index >= 0) - GetOverlapsInNode(ref Nodes[b.Index]); - - //Test all different nodes. - if (ab) - { - DispatchTestForNodes(ref a, ref b); - } - } - } - public unsafe void GetSelfOverlaps5(ref TOverlapHandler results, BufferPool pool) where TOverlapHandler : IOverlapHandler - { - //If there are less than two leaves, there can't be any overlap. - //This provides a guarantee that there are at least 2 children in each internal node considered by GetOverlapsInNode. - if (LeafCount < 2) - return; - - SelfTest5 hmm = default; - hmm.Nodes = Nodes; - hmm.Results = results; - hmm.LeafStack = new QuickList(NodeCount, pool); - hmm.GetOverlapsInNode(ref Nodes[0]); - - results = hmm.Results; - hmm.LeafStack.Dispose(pool); - } } } diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index b57229fff..e8389710d 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -48,12 +48,7 @@ public override void Initialize(ContentArchive content, Camera camera) DemoMeshHelper.CreateDeformedPlane(width, height, (x, y) => new Vector3(x - width * scale.X * 0.5f, 2f * (float)(Math.Sin(x * 0.5f) * Math.Sin(y * 0.5f)), y - height * scale.Y * 0.5f), scale, BufferPool, out var mesh); Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); - Test((ref OverlapHandler handler) => mesh.Tree.GetSelfOverlapsPrepassWithRecursion(ref handler, BufferPool), "LRecurse"); - //Test((ref OverlapHandler handler) => mesh.Tree.GetSelfOverlapsContiguousPrepass(ref handler, BufferPool), "Prepass"); - //Test((ref OverlapHandler handler) => mesh.Tree.GetSelfOverlaps2(ref handler, BufferPool), "Revamp 2"); - //Test((ref OverlapHandler handler) => mesh.Tree.GetSelfOverlaps3(ref handler, BufferPool), "Revamp 3"); - Test((ref OverlapHandler handler) => mesh.Tree.GetSelfOverlaps4(ref handler), "Revamp 4"); - //Test((ref OverlapHandler handler) => mesh.Tree.GetSelfOverlaps5(ref handler, BufferPool), "Revamp 5"); + Test((ref OverlapHandler handler) => mesh.Tree.GetSelfOverlapsContiguousPrepass(ref handler, BufferPool), "Prepass"); Test((ref OverlapHandler handler) => mesh.Tree.GetSelfOverlaps(ref handler), "Original"); } From 835b8b878b9d7c19c1419130210c5729ab1ef935 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 4 Sep 2022 20:56:25 -0500 Subject: [PATCH 587/947] Maybe a vectorized option! --- BepuPhysics/Trees/Tree_SelfQueries.cs | 503 ++++++++++++++++++-------- 1 file changed, 362 insertions(+), 141 deletions(-) diff --git a/BepuPhysics/Trees/Tree_SelfQueries.cs b/BepuPhysics/Trees/Tree_SelfQueries.cs index a9830e489..ed489dbb7 100644 --- a/BepuPhysics/Trees/Tree_SelfQueries.cs +++ b/BepuPhysics/Trees/Tree_SelfQueries.cs @@ -247,6 +247,56 @@ public ref NodeChild GetChild(ref Tree tree) } } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Transpose( + Vector256 m0, Vector256 m1, Vector256 m2, Vector256 m3, Vector256 m4, Vector256 m5, Vector256 m6, Vector256 m7, + out Vector256 t0, out Vector256 t1, out Vector256 t2, out Vector256 t3, out Vector256 t4, out Vector256 t5, out Vector256 t6, out Vector256 t7) + { + + if (Avx.IsSupported) + { + var n0 = Avx.UnpackLow(m0, m1); + var n1 = Avx.UnpackLow(m2, m3); + var n2 = Avx.UnpackLow(m4, m5); + var n3 = Avx.UnpackLow(m6, m7); + var n4 = Avx.UnpackHigh(m0, m1); + var n5 = Avx.UnpackHigh(m2, m3); + var n6 = Avx.UnpackHigh(m4, m5); + var n7 = Avx.UnpackHigh(m6, m7); + + var o0 = Avx.Shuffle(n0, n1, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o1 = Avx.Shuffle(n2, n3, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o2 = Avx.Shuffle(n4, n5, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o3 = Avx.Shuffle(n6, n7, 0 | (1 << 2) | (0 << 4) | (1 << 6)); + var o4 = Avx.Shuffle(n0, n1, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o5 = Avx.Shuffle(n2, n3, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o6 = Avx.Shuffle(n4, n5, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + var o7 = Avx.Shuffle(n6, n7, 2 | (3 << 2) | (2 << 4) | (3 << 6)); + + t0 = Avx.Permute2x128(o0, o1, 0 | (2 << 4)); + t1 = Avx.Permute2x128(o4, o5, 0 | (2 << 4)); + t2 = Avx.Permute2x128(o2, o3, 0 | (2 << 4)); + t3 = Avx.Permute2x128(o6, o7, 0 | (2 << 4)); + + t4 = Avx.Permute2x128(o0, o1, 1 | (3 << 4)); + t5 = Avx.Permute2x128(o4, o5, 1 | (3 << 4)); + t6 = Avx.Permute2x128(o2, o3, 1 | (3 << 4)); + t7 = Avx.Permute2x128(o6, o7, 1 | (3 << 4)); + } + else + { + throw new NotSupportedException("No fallback exists! This should never be visible!"); + } + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector256 Intersects( + Vector256 minAX, Vector256 minAY, Vector256 minAZ, Vector256 maxAX, Vector256 maxAY, Vector256 maxAZ, + Vector256 minBX, Vector256 minBY, Vector256 minBZ, Vector256 maxBX, Vector256 maxBY, Vector256 maxBZ) + { + return ( + Vector256.GreaterThanOrEqual(maxAX, minBX) & Vector256.GreaterThanOrEqual(maxAY, minBY) & Vector256.GreaterThanOrEqual(maxAZ, minBZ) & + Vector256.GreaterThanOrEqual(maxBX, minAX) & Vector256.GreaterThanOrEqual(maxBY, minAY) & Vector256.GreaterThanOrEqual(maxBZ, minBZ)).As(); + } public unsafe void GetSelfOverlapsContiguousPrepass(ref TOverlapHandler results, BufferPool pool) where TOverlapHandler : IOverlapHandler { @@ -256,7 +306,9 @@ public unsafe void GetSelfOverlapsContiguousPrepass(ref TOverla return; //A recursive self test will at some point visit all nodes with certainty. Instead of framing it as a recursive test at all, do a prepass that's just a contiguous iteration. - QuickList crossoverStack = new(NodeCount, pool); + pool.Take(NodeCount, out var crossoverStackA); + pool.Take(NodeCount, out var crossoverStackB); + int crossoverStackCount = 0; QuickList leafNodeStack = new(NodeCount, pool); for (int i = 0; i < NodeCount; ++i) { @@ -269,9 +321,9 @@ public unsafe void GetSelfOverlapsContiguousPrepass(ref TOverla { if (BoundingBox.IntersectsUnsafe(node.A, node.B)) { - ref var stackEntry = ref crossoverStack.AllocateUnsafely(); - stackEntry.A = a; - stackEntry.B = b; + crossoverStackA[crossoverStackCount] = a; + crossoverStackB[crossoverStackCount] = b; + ++crossoverStackCount; } } else if (aIsInternal || bIsInternal) @@ -288,147 +340,316 @@ public unsafe void GetSelfOverlapsContiguousPrepass(ref TOverla //Now, we need to complete all crossovers and leaf-node tests. Note that leaf-node tests can only generate leaf-node or leaf-leaf tests, they never produce crossovers. //In contrast, crossovers can generate leaf-node tests. So do crossovers first so we'll have every leaf-node test in the stack ready to go. - if (crossoverStack.TryPop(out var nextCrossover)) + if (crossoverStackCount > 0) { - while (true) + if (Vector256.IsHardwareAccelerated) { - GetOverlapsBetweenDifferentNodes(ref Nodes[nextCrossover.A], ref Nodes[nextCrossover.B], ref results); - //None of the candidates generated a next step, so grab from the stack. - if (!crossoverStack.TryPop(out nextCrossover)) - break; - - ////Possible sources of stack entry: - ////1) AA intersection, both internal - ////2) AB intersection, both internal - ////3) BA intersection, both internal - ////4) BB intersection, both internal - //var parent0 = nextCrossover.A; - //var parent1 = nextCrossover.B; - //ref var n0 = ref Nodes[parent0]; - //ref var n1 = ref Nodes[parent1]; - //var aaIntersects = BoundingBox.IntersectsUnsafe(n0.A, n1.A); - //var abIntersects = BoundingBox.IntersectsUnsafe(n0.A, n1.B); - //var baIntersects = BoundingBox.IntersectsUnsafe(n0.B, n1.A); - //var bbIntersects = BoundingBox.IntersectsUnsafe(n0.B, n1.B); - //var n0A = n0.A.Index; - //var n0B = n0.B.Index; - //var n1A = n1.A.Index; - //var n1B = n1.B.Index; - //var n0AIsInternal = n0A >= 0; - //var n0BIsInternal = n0B >= 0; - //var n1AIsInternal = n1A >= 0; - //var n1BIsInternal = n1B >= 0; - ////The first test which generates a stack candidate gets the nextTest; the rest get pushed to the stack. - //int previousStackGeneratorCount = 0; - //if (aaIntersects) - //{ - // if (n0AIsInternal && n1AIsInternal) - // { - // ++previousStackGeneratorCount; - // nextCrossover.A = n0A; - // nextCrossover.B = n1A; - // } - // else - // { - // //At least one is a leaf. - // if (n0AIsInternal) - // leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent1, false, n0A); - // else if (n1AIsInternal) - // leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent0, false, n1A); - // else //Both are leaves. - // results.Handle(Encode(n0A), Encode(n1A)); - // } - //} - //if (abIntersects) - //{ - // if (n0AIsInternal && n1BIsInternal) - // { - // if (previousStackGeneratorCount++ == 0) - // { - // nextCrossover.A = n0A; - // nextCrossover.B = n1B; - // } - // else - // { - // ref var stackEntry = ref crossoverStack.AllocateUnsafely(); - // stackEntry.A = n0A; - // stackEntry.B = n1B; - // } - // } - // else - // { - // //At least one is a leaf. - // if (n0AIsInternal) - // leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent1, true, n0A); - // else if (n1BIsInternal) - // leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent0, false, n1B); - // else //Both are leaves. - // results.Handle(Encode(n0A), Encode(n1B)); - // } - //} - //if (baIntersects) - //{ - // if (n0BIsInternal && n1AIsInternal) - // { - // if (previousStackGeneratorCount++ == 0) - // { - // nextCrossover.A = n0B; - // nextCrossover.B = n1A; - // } - // else - // { - // ref var stackEntry = ref crossoverStack.AllocateUnsafely(); - // stackEntry.A = n0B; - // stackEntry.B = n1A; - // } - // } - // else - // { - // //At least one is a leaf. - // if (n0BIsInternal) - // leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent1, false, n0B); - // else if (n1AIsInternal) - // leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent0, true, n1A); - // else //Both are leaves. - // results.Handle(Encode(n0B), Encode(n1A)); - // } - //} - //if (bbIntersects) - //{ - // if (n0BIsInternal && n1BIsInternal) - // { - // if (previousStackGeneratorCount++ == 0) - // { - // nextCrossover.A = n0B; - // nextCrossover.B = n1B; - // } - // else - // { - // ref var stackEntry = ref crossoverStack.AllocateUnsafely(); - // stackEntry.A = n0B; - // stackEntry.B = n1B; - // } - // } - // else - // { - // //At least one is a leaf. - // if (n0BIsInternal) - // leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent1, true, n0B); - // else if (n1BIsInternal) - // leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent0, true, n1B); - // else //Both are leaves. - // results.Handle(Encode(n0B), Encode(n1B)); - // } - //} - //if (previousStackGeneratorCount == 0) - //{ - // //None of the candidates generated a next step, so grab from the stack. - // if (!crossoverStack.TryPop(out nextCrossover)) - // break; - //} + //In a given iteration, the maximum number of leaf-leaf intersections is 8 lanes * 4 child combinations per lane. + var toReportA = stackalloc int[32]; + var toReportB = stackalloc int[32]; + + while (crossoverStackCount > 0) + { + //Note that we do not have a 'nextCrossover' entry held over from the previous iteration. Instead, we directly yoink off the stack. Serves the same purpose. + //Pick the appropriate location to load from in the stack. + var nextLaneCount = 8 < crossoverStackCount ? 8 : crossoverStackCount; + var startIndex = crossoverStackCount - nextLaneCount; + var nextCrossover0 = crossoverStackA.Memory + startIndex; + var nextCrossover1 = crossoverStackB.Memory + startIndex; + + //Each crossover execution loads two nodes, each having two children, for each lane. + //Gather all of it and transpose it for vectorized operations. + //Note that we're not actually getting much benefit on the actual intersection test here; + //the goal of going wide is to reduce the burden of branch misprediction by brute forcing it with vectorization. + var n0Pointer0 = (float*)(Nodes.Memory + nextCrossover0[0]); + var n0Pointer1 = (float*)(Nodes.Memory + nextCrossover0[1]); + var n0Pointer2 = (float*)(Nodes.Memory + nextCrossover0[2]); + var n0Pointer3 = (float*)(Nodes.Memory + nextCrossover0[3]); + var n0Pointer4 = (float*)(Nodes.Memory + nextCrossover0[4]); + var n0Pointer5 = (float*)(Nodes.Memory + nextCrossover0[5]); + var n0Pointer6 = (float*)(Nodes.Memory + nextCrossover0[6]); + var n0Pointer7 = (float*)(Nodes.Memory + nextCrossover0[7]); + + var n0A0 = Vector256.Load(n0Pointer0); + var n0A1 = 1 < nextLaneCount ? Vector256.Load(n0Pointer1) : Vector256.AllBitsSet; + var n0A2 = 2 < nextLaneCount ? Vector256.Load(n0Pointer2) : Vector256.AllBitsSet; + var n0A3 = 3 < nextLaneCount ? Vector256.Load(n0Pointer3) : Vector256.AllBitsSet; + var n0A4 = 4 < nextLaneCount ? Vector256.Load(n0Pointer4) : Vector256.AllBitsSet; + var n0A5 = 5 < nextLaneCount ? Vector256.Load(n0Pointer5) : Vector256.AllBitsSet; + var n0A6 = 6 < nextLaneCount ? Vector256.Load(n0Pointer6) : Vector256.AllBitsSet; + var n0A7 = 7 < nextLaneCount ? Vector256.Load(n0Pointer7) : Vector256.AllBitsSet; + + Transpose(n0A0, n0A1, n0A2, n0A3, n0A4, n0A5, n0A6, n0A7, + out var n0AMinX, out var n0AMinY, out var n0AMinZ, out var n0AIndex, + out var n0AMaxX, out var n0AMaxY, out var n0AMaxZ, out _); + + var n0B0 = Vector256.Load(n0Pointer0 + 8); + var n0B1 = 1 < nextLaneCount ? Vector256.Load(n0Pointer1 + 8) : Vector256.AllBitsSet; + var n0B2 = 2 < nextLaneCount ? Vector256.Load(n0Pointer2 + 8) : Vector256.AllBitsSet; + var n0B3 = 3 < nextLaneCount ? Vector256.Load(n0Pointer3 + 8) : Vector256.AllBitsSet; + var n0B4 = 4 < nextLaneCount ? Vector256.Load(n0Pointer4 + 8) : Vector256.AllBitsSet; + var n0B5 = 5 < nextLaneCount ? Vector256.Load(n0Pointer5 + 8) : Vector256.AllBitsSet; + var n0B6 = 6 < nextLaneCount ? Vector256.Load(n0Pointer6 + 8) : Vector256.AllBitsSet; + var n0B7 = 7 < nextLaneCount ? Vector256.Load(n0Pointer7 + 8) : Vector256.AllBitsSet; + + Transpose(n0B0, n0B1, n0B2, n0B3, n0B4, n0B5, n0A6, n0B7, + out var n0BMinX, out var n0BMinY, out var n0BMinZ, out var n0BIndex, + out var n0BMaxX, out var n0BMaxY, out var n0BMaxZ, out _); + + var n1Pointer0 = (float*)(Nodes.Memory + nextCrossover1[0]); + var n1Pointer1 = (float*)(Nodes.Memory + nextCrossover1[1]); + var n1Pointer2 = (float*)(Nodes.Memory + nextCrossover1[2]); + var n1Pointer3 = (float*)(Nodes.Memory + nextCrossover1[3]); + var n1Pointer4 = (float*)(Nodes.Memory + nextCrossover1[4]); + var n1Pointer5 = (float*)(Nodes.Memory + nextCrossover1[5]); + var n1Pointer6 = (float*)(Nodes.Memory + nextCrossover1[6]); + var n1Pointer7 = (float*)(Nodes.Memory + nextCrossover1[7]); + + var n1A0 = Vector256.Load(n1Pointer0); + var n1A1 = 1 < nextLaneCount ? Vector256.Load(n1Pointer1) : Vector256.AllBitsSet; + var n1A2 = 2 < nextLaneCount ? Vector256.Load(n1Pointer2) : Vector256.AllBitsSet; + var n1A3 = 3 < nextLaneCount ? Vector256.Load(n1Pointer3) : Vector256.AllBitsSet; + var n1A4 = 4 < nextLaneCount ? Vector256.Load(n1Pointer4) : Vector256.AllBitsSet; + var n1A5 = 5 < nextLaneCount ? Vector256.Load(n1Pointer5) : Vector256.AllBitsSet; + var n1A6 = 6 < nextLaneCount ? Vector256.Load(n1Pointer6) : Vector256.AllBitsSet; + var n1A7 = 7 < nextLaneCount ? Vector256.Load(n1Pointer7) : Vector256.AllBitsSet; + + Transpose(n1A0, n1A1, n1A2, n1A3, n1A4, n1A5, n1A6, n1A7, + out var n1AMinX, out var n1AMinY, out var n1AMinZ, out var n1AIndex, + out var n1AMaxX, out var n1AMaxY, out var n1AMaxZ, out _); + + var n1B0 = Vector256.Load(n1Pointer0 + 8); + var n1B1 = 1 < nextLaneCount ? Vector256.Load(n1Pointer1 + 8) : Vector256.AllBitsSet; + var n1B2 = 2 < nextLaneCount ? Vector256.Load(n1Pointer2 + 8) : Vector256.AllBitsSet; + var n1B3 = 3 < nextLaneCount ? Vector256.Load(n1Pointer3 + 8) : Vector256.AllBitsSet; + var n1B4 = 4 < nextLaneCount ? Vector256.Load(n1Pointer4 + 8) : Vector256.AllBitsSet; + var n1B5 = 5 < nextLaneCount ? Vector256.Load(n1Pointer5 + 8) : Vector256.AllBitsSet; + var n1B6 = 6 < nextLaneCount ? Vector256.Load(n1Pointer6 + 8) : Vector256.AllBitsSet; + var n1B7 = 7 < nextLaneCount ? Vector256.Load(n1Pointer7 + 8) : Vector256.AllBitsSet; + + Transpose(n1B0, n1B1, n1B2, n1B3, n1B4, n1B5, n1B6, n1B7, + out var n1BMinX, out var n1BMinY, out var n1BMinZ, out var n1BIndex, + out var n1BMaxX, out var n1BMaxY, out var n1BMaxZ, out _); + + var aaIntersects = Intersects( + n0AMinX, n0AMinY, n0AMinZ, n0AMaxX, n0AMaxY, n0AMaxZ, + n1AMinX, n1AMinY, n1AMinZ, n1AMaxX, n1AMaxY, n1AMaxZ); + var abIntersects = Intersects( + n0AMinX, n0AMinY, n0AMinZ, n0AMaxX, n0AMaxY, n0AMaxZ, + n1BMinX, n1BMinY, n1BMinZ, n1BMaxX, n1BMaxY, n1BMaxZ); + var baIntersects = Intersects( + n0BMinX, n0BMinY, n0BMinZ, n0BMaxX, n0BMaxY, n0BMaxZ, + n1AMinX, n1AMinY, n1AMinZ, n1AMaxX, n1AMaxY, n1AMaxZ); + var bbIntersects = Intersects( + n0BMinX, n0BMinY, n0BMinZ, n0BMaxX, n0BMaxY, n0BMaxZ, + n1BMinX, n1BMinY, n1BMinZ, n1BMaxX, n1BMaxY, n1BMaxZ); + + var n0AIsInternal = Vector256.GreaterThanOrEqual(n0AIndex.As(), Vector256.Zero); + var n0BIsInternal = Vector256.GreaterThanOrEqual(n0BIndex.As(), Vector256.Zero); + var n1AIsInternal = Vector256.GreaterThanOrEqual(n1AIndex.As(), Vector256.Zero); + var n1BIsInternal = Vector256.GreaterThanOrEqual(n1BIndex.As(), Vector256.Zero); + + var reportAA = Vector256.AndNot(Vector256.AndNot(aaIntersects, n0AIsInternal), n1AIsInternal); + var reportAB = Vector256.AndNot(Vector256.AndNot(abIntersects, n0AIsInternal), n1BIsInternal); + var reportBA = Vector256.AndNot(Vector256.AndNot(baIntersects, n0BIsInternal), n1AIsInternal); + var reportBB = Vector256.AndNot(Vector256.AndNot(bbIntersects, n0BIsInternal), n1BIsInternal); + var reportAnyPair = reportAA | reportAB | reportBA | reportBB; + if (Vector256.LessThanAny(reportAnyPair, Vector256.Zero)) + { + //At least one leaf-leaf test is reported. + //For each report vector, encode the indices, left pack for reported lanes, and store into the toReport buffers. + var aaBitMask = Vector256.ExtractMostSignificantBits(reportAA); + var reportCountAA = BitOperations.PopCount(aaBitMask); + var abBitMask = Vector256.ExtractMostSignificantBits(reportBA); + var reportCountAB = BitOperations.PopCount(abBitMask); + var baBitMask = Vector256.ExtractMostSignificantBits(reportBA); + var reportCountBA = BitOperations.PopCount(baBitMask); + var bbBitMask = Vector256.ExtractMostSignificantBits(reportBB); + var reportCountBB = BitOperations.PopCount(bbBitMask); + + var reportCount = 0; + //Reporting itself is sequentialized; exposing the vectorized context to the callback is grossbad. + for (int i = 0; i < reportCount; ++i) + { + results.Handle(toReportA[i], toReportB[i]); + } + } + + var pushNode0AVersusLeaf1A = aaIntersects & Vector256.AndNot(n0AIsInternal, n1AIsInternal); + var pushNode0AVersusLeaf1B = abIntersects & Vector256.AndNot(n0AIsInternal, n1BIsInternal); + var pushNode0BVersusLeaf1A = baIntersects & Vector256.AndNot(n0BIsInternal, n1AIsInternal); + var pushNode0BVersusLeaf1B = bbIntersects & Vector256.AndNot(n0BIsInternal, n1BIsInternal); + var pushAnyNode0VersusLeaf1 = pushNode0AVersusLeaf1A | pushNode0AVersusLeaf1B | pushNode0BVersusLeaf1A | pushNode0BVersusLeaf1B; + + var pushLeaf0AVersusNode1A = aaIntersects & Vector256.AndNot(n1AIsInternal, n0AIsInternal); + var pushLeaf0AVersusNode1B = abIntersects & Vector256.AndNot(n1BIsInternal, n0AIsInternal); + var pushLeaf0BVersusNode1A = baIntersects & Vector256.AndNot(n1AIsInternal, n0BIsInternal); + var pushLeaf0BVersusNode1B = bbIntersects & Vector256.AndNot(n1BIsInternal, n0BIsInternal); + var pushAnyLeaf0VersusNode1 = pushLeaf0AVersusNode1A | pushLeaf0AVersusNode1B | pushLeaf0BVersusNode1A | pushLeaf0BVersusNode1B; + + if (Vector256.LessThanAny(pushAnyNode0VersusLeaf1 | pushAnyLeaf0VersusNode1, Vector256.Zero)) + { + //At least one node-leaf push is needed. + } + + var aaWantsToPushCrossover = aaIntersects & n0AIsInternal & n1AIsInternal; + var abWantsToPushCrossover = abIntersects & n0AIsInternal & n1BIsInternal; + var baWantsToPushCrossover = baIntersects & n0BIsInternal & n1AIsInternal; + var bbWantsToPushCrossover = bbIntersects & n0BIsInternal & n1BIsInternal; + var pushAnyCrossover = aaWantsToPushCrossover | abWantsToPushCrossover | baWantsToPushCrossover | bbWantsToPushCrossover; + if (Vector256.LessThanAny(pushAnyCrossover, Vector256.Zero)) + { + //At least one push for crossovers. + //Similar to leaf-leaf reporting; left pack the indices and store them into the push buffers. + } + + } + + } + //while (true) + //{ + + // GetOverlapsBetweenDifferentNodes(ref Nodes[nextCrossover.A], ref Nodes[nextCrossover.B], ref results); + // //None of the candidates generated a next step, so grab from the stack. + // if (!crossoverStack.TryPop(out nextCrossover)) + // break; + + + + // ////Possible sources of stack entry: + // ////1) AA intersection, both internal + // ////2) AB intersection, both internal + // ////3) BA intersection, both internal + // ////4) BB intersection, both internal + // //var parent0 = nextCrossover.A; + // //var parent1 = nextCrossover.B; + // //ref var n0 = ref Nodes[parent0]; + // //ref var n1 = ref Nodes[parent1]; + // //var aaIntersects = BoundingBox.IntersectsUnsafe(n0.A, n1.A); + // //var abIntersects = BoundingBox.IntersectsUnsafe(n0.A, n1.B); + // //var baIntersects = BoundingBox.IntersectsUnsafe(n0.B, n1.A); + // //var bbIntersects = BoundingBox.IntersectsUnsafe(n0.B, n1.B); + // //var n0A = n0.A.Index; + // //var n0B = n0.B.Index; + // //var n1A = n1.A.Index; + // //var n1B = n1.B.Index; + // //var n0AIsInternal = n0A >= 0; + // //var n0BIsInternal = n0B >= 0; + // //var n1AIsInternal = n1A >= 0; + // //var n1BIsInternal = n1B >= 0; + // ////The first test which generates a stack candidate gets the nextTest; the rest get pushed to the stack. + // //int previousStackGeneratorCount = 0; + // //if (aaIntersects) + // //{ + // // if (n0AIsInternal && n1AIsInternal) + // // { + // // ++previousStackGeneratorCount; + // // nextCrossover.A = n0A; + // // nextCrossover.B = n1A; + // // } + // // else + // // { + // // //At least one is a leaf. + // // if (n0AIsInternal) + // // leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent1, false, n0A); + // // else if (n1AIsInternal) + // // leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent0, false, n1A); + // // else //Both are leaves. + // // results.Handle(Encode(n0A), Encode(n1A)); + // // } + // //} + // //if (abIntersects) + // //{ + // // if (n0AIsInternal && n1BIsInternal) + // // { + // // if (previousStackGeneratorCount++ == 0) + // // { + // // nextCrossover.A = n0A; + // // nextCrossover.B = n1B; + // // } + // // else + // // { + // // ref var stackEntry = ref crossoverStack.AllocateUnsafely(); + // // stackEntry.A = n0A; + // // stackEntry.B = n1B; + // // } + // // } + // // else + // // { + // // //At least one is a leaf. + // // if (n0AIsInternal) + // // leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent1, true, n0A); + // // else if (n1BIsInternal) + // // leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent0, false, n1B); + // // else //Both are leaves. + // // results.Handle(Encode(n0A), Encode(n1B)); + // // } + // //} + // //if (baIntersects) + // //{ + // // if (n0BIsInternal && n1AIsInternal) + // // { + // // if (previousStackGeneratorCount++ == 0) + // // { + // // nextCrossover.A = n0B; + // // nextCrossover.B = n1A; + // // } + // // else + // // { + // // ref var stackEntry = ref crossoverStack.AllocateUnsafely(); + // // stackEntry.A = n0B; + // // stackEntry.B = n1A; + // // } + // // } + // // else + // // { + // // //At least one is a leaf. + // // if (n0BIsInternal) + // // leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent1, false, n0B); + // // else if (n1AIsInternal) + // // leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent0, true, n1A); + // // else //Both are leaves. + // // results.Handle(Encode(n0B), Encode(n1A)); + // // } + // //} + // //if (bbIntersects) + // //{ + // // if (n0BIsInternal && n1BIsInternal) + // // { + // // if (previousStackGeneratorCount++ == 0) + // // { + // // nextCrossover.A = n0B; + // // nextCrossover.B = n1B; + // // } + // // else + // // { + // // ref var stackEntry = ref crossoverStack.AllocateUnsafely(); + // // stackEntry.A = n0B; + // // stackEntry.B = n1B; + // // } + // // } + // // else + // // { + // // //At least one is a leaf. + // // if (n0BIsInternal) + // // leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent1, true, n0B); + // // else if (n1BIsInternal) + // // leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent0, true, n1B); + // // else //Both are leaves. + // // results.Handle(Encode(n0B), Encode(n1B)); + // // } + // //} + // //if (previousStackGeneratorCount == 0) + // //{ + // // //None of the candidates generated a next step, so grab from the stack. + // // if (!crossoverStack.TryPop(out nextCrossover)) + // // break; + // //} + //} } - crossoverStack.Dispose(pool); + pool.Return(ref crossoverStackA); + pool.Return(ref crossoverStackB); QuickList stack = new(NodeCount, pool); for (int i = 0; i < leafNodeStack.Count; ++i) From c95ae50d8c7366e624e8af13adbb35ccaab1a724 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 6 Sep 2022 19:57:02 -0500 Subject: [PATCH 588/947] Bugs, but VECTORIZED bugs. --- BepuPhysics/Trees/Tree_SelfQueries.cs | 340 +++++++----------- Demos/Program.cs | 4 +- .../SpecializedTests/TreeFiddlingTestDemo.cs | 6 +- 3 files changed, 145 insertions(+), 205 deletions(-) diff --git a/BepuPhysics/Trees/Tree_SelfQueries.cs b/BepuPhysics/Trees/Tree_SelfQueries.cs index ed489dbb7..c0d9a8f72 100644 --- a/BepuPhysics/Trees/Tree_SelfQueries.cs +++ b/BepuPhysics/Trees/Tree_SelfQueries.cs @@ -217,34 +217,22 @@ unsafe void GetOverlapsWithLeaf(ref TOverlapHandler results, No } } - struct StackEntry + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + static uint EncodeParentIndex(int leafParentIndex, bool childIsB) { - public int A; - public int B; + var encoded = (uint)leafParentIndex; + if (childIsB) + encoded |= 1u << 31; + return encoded; } - - struct LeafNodeStackEntry + [MethodImpl(MethodImplOptions.AggressiveInlining)] + static ref NodeChild GetLeafChild(ref Tree tree, uint encodedLeafParentIndex) { - public uint EncodedLeafParentIndex; - public int NodeIndex; - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public LeafNodeStackEntry(int leafParentIndex, bool leafChildIsB, int nodeIndex) - { - EncodedLeafParentIndex = (uint)leafParentIndex; - if (leafChildIsB) - EncodedLeafParentIndex |= 1u << 31; - NodeIndex = nodeIndex; - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref NodeChild GetChild(ref Tree tree) - { - var parentNodeIndex = EncodedLeafParentIndex & 0x7FFF_FFFF; - var leafIsChildB = EncodedLeafParentIndex > 0x7FFF_FFFF; - ref var parent = ref tree.Nodes[parentNodeIndex]; - return ref leafIsChildB ? ref parent.B : ref parent.A; - } + var parentNodeIndex = encodedLeafParentIndex & 0x7FFF_FFFF; + var leafIsChildB = encodedLeafParentIndex > 0x7FFF_FFFF; + ref var parent = ref tree.Nodes[parentNodeIndex]; + return ref leafIsChildB ? ref parent.B : ref parent.A; } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -295,7 +283,44 @@ public static Vector256 Intersects( { return ( Vector256.GreaterThanOrEqual(maxAX, minBX) & Vector256.GreaterThanOrEqual(maxAY, minBY) & Vector256.GreaterThanOrEqual(maxAZ, minBZ) & - Vector256.GreaterThanOrEqual(maxBX, minAX) & Vector256.GreaterThanOrEqual(maxBY, minAY) & Vector256.GreaterThanOrEqual(maxBZ, minBZ)).As(); + Vector256.GreaterThanOrEqual(maxBX, minAX) & Vector256.GreaterThanOrEqual(maxBY, minAY) & Vector256.GreaterThanOrEqual(maxBZ, minAZ)).As(); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector256 GetLeftPackMask(Vector256 mask, out int count) + { + if (!Avx2.IsSupported || !Bmi2.X64.IsSupported) throw new NotSupportedException("No fallback exists! This should never be visible!"); + + var bitmask = Vector256.ExtractMostSignificantBits(mask); + //From https://stackoverflow.com/a/36951611, courtesy of Peter Cordes. + //pdep/pext are apparently quite slow pre-Zen3, unfortunately. This is just a proof of correctness. + ulong expanded_mask = Bmi2.X64.ParallelBitDeposit(bitmask, 0x0101010101010101); // unpack each bit to a byte + expanded_mask *= 0xFF; // mask |= mask<<1 | mask<<2 | ... | mask<<7; + // ABC... -> AAAAAAAABBBBBBBBCCCCCCCC...: replicate each bit to fill its byte + + ulong identity_indices = 0x0706050403020100; // the identity shuffle for vpermps, packed to one index per byte + ulong wanted_indices = Bmi2.X64.ParallelBitExtract(identity_indices, expanded_mask); + + count = BitOperations.PopCount(bitmask); + return Avx2.ConvertToVector256Int32(Vector128.CreateScalarUnsafe(wanted_indices).As()); + + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector256 Encode(Vector256 index) + { + return Vector256.AllBitsSet - index; + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector256 Encode(Vector256 index) + { + return Vector256.AllBitsSet - index.AsInt32(); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector256 EncodeLeafChildBForStack(Vector256 parentIndex) + { + return parentIndex | Vector256.Create(1 << 31); } public unsafe void GetSelfOverlapsContiguousPrepass(ref TOverlapHandler results, BufferPool pool) where TOverlapHandler : IOverlapHandler @@ -306,10 +331,14 @@ public unsafe void GetSelfOverlapsContiguousPrepass(ref TOverla return; //A recursive self test will at some point visit all nodes with certainty. Instead of framing it as a recursive test at all, do a prepass that's just a contiguous iteration. - pool.Take(NodeCount, out var crossoverStackA); - pool.Take(NodeCount, out var crossoverStackB); + //Include a little buffer to avoid overruns by vectorized operations. + var stackSize = 1 + ((NodeCount + 7) / 8) * 8; + pool.Take(stackSize, out var crossoverStackA); + pool.Take(stackSize, out var crossoverStackB); + pool.Take(stackSize, out var nodeLeafStackA); + pool.Take(stackSize, out var nodeLeafStackB); int crossoverStackCount = 0; - QuickList leafNodeStack = new(NodeCount, pool); + int nodeLeafStackCount = 0; for (int i = 0; i < NodeCount; ++i) { ref var node = ref Nodes[i]; @@ -329,7 +358,9 @@ public unsafe void GetSelfOverlapsContiguousPrepass(ref TOverla else if (aIsInternal || bIsInternal) { //One is a leaf, one is internal. - leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(i, aIsInternal, aIsInternal ? a : b); + nodeLeafStackA[nodeLeafStackCount] = (uint)i; + nodeLeafStackB[nodeLeafStackCount] = (uint)i | (1u << 31); + ++nodeLeafStackCount; } else { @@ -346,7 +377,7 @@ public unsafe void GetSelfOverlapsContiguousPrepass(ref TOverla { //In a given iteration, the maximum number of leaf-leaf intersections is 8 lanes * 4 child combinations per lane. var toReportA = stackalloc int[32]; - var toReportB = stackalloc int[32]; + var toReportB = stackalloc int[32]; while (crossoverStackCount > 0) { @@ -356,6 +387,7 @@ public unsafe void GetSelfOverlapsContiguousPrepass(ref TOverla var startIndex = crossoverStackCount - nextLaneCount; var nextCrossover0 = crossoverStackA.Memory + startIndex; var nextCrossover1 = crossoverStackB.Memory + startIndex; + crossoverStackCount = startIndex; //Each crossover execution loads two nodes, each having two children, for each lane. //Gather all of it and transpose it for vectorized operations. @@ -458,16 +490,30 @@ public unsafe void GetSelfOverlapsContiguousPrepass(ref TOverla { //At least one leaf-leaf test is reported. //For each report vector, encode the indices, left pack for reported lanes, and store into the toReport buffers. - var aaBitMask = Vector256.ExtractMostSignificantBits(reportAA); - var reportCountAA = BitOperations.PopCount(aaBitMask); - var abBitMask = Vector256.ExtractMostSignificantBits(reportBA); - var reportCountAB = BitOperations.PopCount(abBitMask); - var baBitMask = Vector256.ExtractMostSignificantBits(reportBA); - var reportCountBA = BitOperations.PopCount(baBitMask); - var bbBitMask = Vector256.ExtractMostSignificantBits(reportBB); - var reportCountBB = BitOperations.PopCount(bbBitMask); - + //TODO: The chance that this is actually faster than brute force in the worst case seems a wee bit low given that we're *still iterating at the end*. + var aaShuffle = GetLeftPackMask(reportAA, out int aaCount); + var abShuffle = GetLeftPackMask(reportAB, out int abCount); + var baShuffle = GetLeftPackMask(reportBA, out int baCount); + var bbShuffle = GetLeftPackMask(reportBB, out int bbCount); + + var encodedN0A = Encode(n0AIndex); + var encodedN0B = Encode(n0BIndex); + var encodedN1A = Encode(n1AIndex); + var encodedN1B = Encode(n1BIndex); var reportCount = 0; + Vector256.Store(Vector256.Shuffle(encodedN0A, aaShuffle), toReportA); + Vector256.Store(Vector256.Shuffle(encodedN1A, aaShuffle), toReportB); + reportCount += aaCount; + Vector256.Store(Vector256.Shuffle(encodedN0A, abShuffle), toReportA + reportCount); + Vector256.Store(Vector256.Shuffle(encodedN1B, abShuffle), toReportB + reportCount); + reportCount += abCount; + Vector256.Store(Vector256.Shuffle(encodedN0B, baShuffle), toReportA + reportCount); + Vector256.Store(Vector256.Shuffle(encodedN1A, baShuffle), toReportB + reportCount); + reportCount += baCount; + Vector256.Store(Vector256.Shuffle(encodedN0B, bbShuffle), toReportA + reportCount); + Vector256.Store(Vector256.Shuffle(encodedN1B, bbShuffle), toReportB + reportCount); + reportCount += bbCount; + //Reporting itself is sequentialized; exposing the vectorized context to the callback is grossbad. for (int i = 0; i < reportCount; ++i) { @@ -475,21 +521,37 @@ public unsafe void GetSelfOverlapsContiguousPrepass(ref TOverla } } - var pushNode0AVersusLeaf1A = aaIntersects & Vector256.AndNot(n0AIsInternal, n1AIsInternal); - var pushNode0AVersusLeaf1B = abIntersects & Vector256.AndNot(n0AIsInternal, n1BIsInternal); - var pushNode0BVersusLeaf1A = baIntersects & Vector256.AndNot(n0BIsInternal, n1AIsInternal); - var pushNode0BVersusLeaf1B = bbIntersects & Vector256.AndNot(n0BIsInternal, n1BIsInternal); - var pushAnyNode0VersusLeaf1 = pushNode0AVersusLeaf1A | pushNode0AVersusLeaf1B | pushNode0BVersusLeaf1A | pushNode0BVersusLeaf1B; - - var pushLeaf0AVersusNode1A = aaIntersects & Vector256.AndNot(n1AIsInternal, n0AIsInternal); - var pushLeaf0AVersusNode1B = abIntersects & Vector256.AndNot(n1BIsInternal, n0AIsInternal); - var pushLeaf0BVersusNode1A = baIntersects & Vector256.AndNot(n1AIsInternal, n0BIsInternal); - var pushLeaf0BVersusNode1B = bbIntersects & Vector256.AndNot(n1BIsInternal, n0BIsInternal); - var pushAnyLeaf0VersusNode1 = pushLeaf0AVersusNode1A | pushLeaf0AVersusNode1B | pushLeaf0BVersusNode1A | pushLeaf0BVersusNode1B; + var pushNodeLeaf0AVersus1A = aaIntersects & (n0AIsInternal ^ n1AIsInternal); + var pushNodeLeaf0AVersus1B = abIntersects & (n0AIsInternal ^ n1BIsInternal); + var pushNodeLeaf0BVersus1A = baIntersects & (n0BIsInternal ^ n1AIsInternal); + var pushNodeLeaf0BVersus1B = bbIntersects & (n0BIsInternal ^ n1BIsInternal); + var pushAnyNodeLeaf = pushNodeLeaf0AVersus1A | pushNodeLeaf0AVersus1B | pushNodeLeaf0BVersus1A | pushNodeLeaf0BVersus1B; - if (Vector256.LessThanAny(pushAnyNode0VersusLeaf1 | pushAnyLeaf0VersusNode1, Vector256.Zero)) + if (Vector256.LessThanAny(pushAnyNodeLeaf, Vector256.Zero)) { //At least one node-leaf push is needed. + var shuffle0A1A = GetLeftPackMask(pushNodeLeaf0AVersus1A, out int count0A1A); + var shuffle0A1B = GetLeftPackMask(pushNodeLeaf0AVersus1B, out int count0A1B); + var shuffle0B1A = GetLeftPackMask(pushNodeLeaf0BVersus1A, out int count0B1A); + var shuffle0B1B = GetLeftPackMask(pushNodeLeaf0BVersus1B, out int count0B1B); + + var encodedForStack0A = Vector256.Load(nextCrossover0); + var encodedForStack0B = EncodeLeafChildBForStack(encodedForStack0A); + var encodedForStack1A = Vector256.Load(nextCrossover1); + var encodedForStack1B = EncodeLeafChildBForStack(encodedForStack1A); + Vector256.Store(Vector256.Shuffle(encodedForStack0A, shuffle0A1A), (int*)nodeLeafStackA.Memory + nodeLeafStackCount); + Vector256.Store(Vector256.Shuffle(encodedForStack1A, shuffle0A1A), (int*)nodeLeafStackB.Memory + nodeLeafStackCount); + nodeLeafStackCount += count0A1A; + Vector256.Store(Vector256.Shuffle(encodedForStack0A, shuffle0A1B), (int*)nodeLeafStackA.Memory + nodeLeafStackCount); + Vector256.Store(Vector256.Shuffle(encodedForStack1B, shuffle0A1B), (int*)nodeLeafStackB.Memory + nodeLeafStackCount); + nodeLeafStackCount += count0A1B; + Vector256.Store(Vector256.Shuffle(encodedForStack0B, shuffle0B1A), (int*)nodeLeafStackA.Memory + nodeLeafStackCount); + Vector256.Store(Vector256.Shuffle(encodedForStack1A, shuffle0B1A), (int*)nodeLeafStackB.Memory + nodeLeafStackCount); + nodeLeafStackCount += count0B1A; + Vector256.Store(Vector256.Shuffle(encodedForStack0B, shuffle0B1B), (int*)nodeLeafStackA.Memory + nodeLeafStackCount); + Vector256.Store(Vector256.Shuffle(encodedForStack1B, shuffle0B1B), (int*)nodeLeafStackB.Memory + nodeLeafStackCount); + nodeLeafStackCount += count0B1B; + } var aaWantsToPushCrossover = aaIntersects & n0AIsInternal & n1AIsInternal; @@ -500,164 +562,39 @@ public unsafe void GetSelfOverlapsContiguousPrepass(ref TOverla if (Vector256.LessThanAny(pushAnyCrossover, Vector256.Zero)) { //At least one push for crossovers. - //Similar to leaf-leaf reporting; left pack the indices and store them into the push buffers. + var aaShuffle = GetLeftPackMask(aaWantsToPushCrossover, out int aaCount); + var abShuffle = GetLeftPackMask(abWantsToPushCrossover, out int abCount); + var baShuffle = GetLeftPackMask(baWantsToPushCrossover, out int baCount); + var bbShuffle = GetLeftPackMask(bbWantsToPushCrossover, out int bbCount); + + Vector256.Store(Vector256.Shuffle(n0AIndex.AsInt32(), aaShuffle), crossoverStackA.Memory + crossoverStackCount); + Vector256.Store(Vector256.Shuffle(n1AIndex.AsInt32(), aaShuffle), crossoverStackB.Memory + crossoverStackCount); + crossoverStackCount += aaCount; + Vector256.Store(Vector256.Shuffle(n0AIndex.AsInt32(), abShuffle), crossoverStackA.Memory + crossoverStackCount); + Vector256.Store(Vector256.Shuffle(n1BIndex.AsInt32(), abShuffle), crossoverStackB.Memory + crossoverStackCount); + crossoverStackCount += abCount; + Vector256.Store(Vector256.Shuffle(n0BIndex.AsInt32(), baShuffle), crossoverStackA.Memory + crossoverStackCount); + Vector256.Store(Vector256.Shuffle(n1AIndex.AsInt32(), baShuffle), crossoverStackB.Memory + crossoverStackCount); + crossoverStackCount += baCount; + Vector256.Store(Vector256.Shuffle(n0BIndex.AsInt32(), bbShuffle), crossoverStackA.Memory + crossoverStackCount); + Vector256.Store(Vector256.Shuffle(n1BIndex.AsInt32(), bbShuffle), crossoverStackB.Memory + crossoverStackCount); + crossoverStackCount += bbCount; } - } - - } - //while (true) - //{ - - // GetOverlapsBetweenDifferentNodes(ref Nodes[nextCrossover.A], ref Nodes[nextCrossover.B], ref results); - // //None of the candidates generated a next step, so grab from the stack. - // if (!crossoverStack.TryPop(out nextCrossover)) - // break; - - - - // ////Possible sources of stack entry: - // ////1) AA intersection, both internal - // ////2) AB intersection, both internal - // ////3) BA intersection, both internal - // ////4) BB intersection, both internal - // //var parent0 = nextCrossover.A; - // //var parent1 = nextCrossover.B; - // //ref var n0 = ref Nodes[parent0]; - // //ref var n1 = ref Nodes[parent1]; - // //var aaIntersects = BoundingBox.IntersectsUnsafe(n0.A, n1.A); - // //var abIntersects = BoundingBox.IntersectsUnsafe(n0.A, n1.B); - // //var baIntersects = BoundingBox.IntersectsUnsafe(n0.B, n1.A); - // //var bbIntersects = BoundingBox.IntersectsUnsafe(n0.B, n1.B); - // //var n0A = n0.A.Index; - // //var n0B = n0.B.Index; - // //var n1A = n1.A.Index; - // //var n1B = n1.B.Index; - // //var n0AIsInternal = n0A >= 0; - // //var n0BIsInternal = n0B >= 0; - // //var n1AIsInternal = n1A >= 0; - // //var n1BIsInternal = n1B >= 0; - // ////The first test which generates a stack candidate gets the nextTest; the rest get pushed to the stack. - // //int previousStackGeneratorCount = 0; - // //if (aaIntersects) - // //{ - // // if (n0AIsInternal && n1AIsInternal) - // // { - // // ++previousStackGeneratorCount; - // // nextCrossover.A = n0A; - // // nextCrossover.B = n1A; - // // } - // // else - // // { - // // //At least one is a leaf. - // // if (n0AIsInternal) - // // leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent1, false, n0A); - // // else if (n1AIsInternal) - // // leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent0, false, n1A); - // // else //Both are leaves. - // // results.Handle(Encode(n0A), Encode(n1A)); - // // } - // //} - // //if (abIntersects) - // //{ - // // if (n0AIsInternal && n1BIsInternal) - // // { - // // if (previousStackGeneratorCount++ == 0) - // // { - // // nextCrossover.A = n0A; - // // nextCrossover.B = n1B; - // // } - // // else - // // { - // // ref var stackEntry = ref crossoverStack.AllocateUnsafely(); - // // stackEntry.A = n0A; - // // stackEntry.B = n1B; - // // } - // // } - // // else - // // { - // // //At least one is a leaf. - // // if (n0AIsInternal) - // // leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent1, true, n0A); - // // else if (n1BIsInternal) - // // leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent0, false, n1B); - // // else //Both are leaves. - // // results.Handle(Encode(n0A), Encode(n1B)); - // // } - // //} - // //if (baIntersects) - // //{ - // // if (n0BIsInternal && n1AIsInternal) - // // { - // // if (previousStackGeneratorCount++ == 0) - // // { - // // nextCrossover.A = n0B; - // // nextCrossover.B = n1A; - // // } - // // else - // // { - // // ref var stackEntry = ref crossoverStack.AllocateUnsafely(); - // // stackEntry.A = n0B; - // // stackEntry.B = n1A; - // // } - // // } - // // else - // // { - // // //At least one is a leaf. - // // if (n0BIsInternal) - // // leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent1, false, n0B); - // // else if (n1AIsInternal) - // // leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent0, true, n1A); - // // else //Both are leaves. - // // results.Handle(Encode(n0B), Encode(n1A)); - // // } - // //} - // //if (bbIntersects) - // //{ - // // if (n0BIsInternal && n1BIsInternal) - // // { - // // if (previousStackGeneratorCount++ == 0) - // // { - // // nextCrossover.A = n0B; - // // nextCrossover.B = n1B; - // // } - // // else - // // { - // // ref var stackEntry = ref crossoverStack.AllocateUnsafely(); - // // stackEntry.A = n0B; - // // stackEntry.B = n1B; - // // } - // // } - // // else - // // { - // // //At least one is a leaf. - // // if (n0BIsInternal) - // // leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent1, true, n0B); - // // else if (n1BIsInternal) - // // leafNodeStack.AllocateUnsafely() = new LeafNodeStackEntry(parent0, true, n1B); - // // else //Both are leaves. - // // results.Handle(Encode(n0B), Encode(n1B)); - // // } - // //} - // //if (previousStackGeneratorCount == 0) - // //{ - // // //None of the candidates generated a next step, so grab from the stack. - // // if (!crossoverStack.TryPop(out nextCrossover)) - // // break; - // //} - //} } pool.Return(ref crossoverStackA); pool.Return(ref crossoverStackB); QuickList stack = new(NodeCount, pool); - for (int i = 0; i < leafNodeStack.Count; ++i) + for (int i = 0; i < nodeLeafStackCount; ++i) { - var leafNodeTarget = leafNodeStack[i]; - ref var leafChild = ref leafNodeTarget.GetChild(ref this); + ref var childA = ref GetLeafChild(ref this, nodeLeafStackA[i]); + ref var childB = ref GetLeafChild(ref this, nodeLeafStackB[i]); + Debug.Assert((childA.Index < 0) ^ (childB.Index < 0), "One and only one of the two children must be a leaf."); + ref var leafChild = ref childA.Index < 0 ? ref childA : ref childB; + var nodeToTest = childA.Index < 0 ? childB.Index : childA.Index; var leafIndex = Encode(leafChild.Index); - var nodeToTest = leafNodeTarget.NodeIndex; Debug.Assert(stack.Count == 0); while (true) { @@ -702,7 +639,8 @@ public unsafe void GetSelfOverlapsContiguousPrepass(ref TOverla } } } - leafNodeStack.Dispose(pool); + pool.Return(ref nodeLeafStackA); + pool.Return(ref nodeLeafStackB); stack.Dispose(pool); } diff --git a/Demos/Program.cs b/Demos/Program.cs index f72a43955..f2013db84 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -1,7 +1,9 @@ -using BepuUtilities; +using BepuPhysics.Trees; +using BepuUtilities; using DemoContentLoader; using DemoUtilities; using OpenTK; +using System.Runtime.Intrinsics; namespace Demos { diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index e8389710d..dcc3ea7e5 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -42,8 +42,8 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0; Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(4, 1)); - var width = 768; - var height = 768; + var width = 2; + var height = 9; var scale = new Vector3(1, 1, 1); DemoMeshHelper.CreateDeformedPlane(width, height, (x, y) => new Vector3(x - width * scale.X * 0.5f, 2f * (float)(Math.Sin(x * 0.5f) * Math.Sin(y * 0.5f)), y - height * scale.Y * 0.5f), scale, BufferPool, out var mesh); Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); @@ -61,7 +61,7 @@ static void Test(TestFunction function, string name) const int testCount = 16; for (int i = 0; i < testCount; ++i) { - var startTime = Stopwatch.GetTimestamp(); + var startTime = Stopwatch.GetTimestamp(); function(ref overlapHandler); var endTime = Stopwatch.GetTimestamp(); accumulatedTime += endTime - startTime; From 283fbfa57473343e38774b196a221fe6f9128397 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 8 Sep 2022 19:32:32 -0500 Subject: [PATCH 589/947] Extremely bad performance as you might expect! --- BepuPhysics/Trees/Tree_SelfQueries.cs | 23 ++++++---- .../SpecializedTests/TreeFiddlingTestDemo.cs | 46 +++++++++++++++++-- 2 files changed, 56 insertions(+), 13 deletions(-) diff --git a/BepuPhysics/Trees/Tree_SelfQueries.cs b/BepuPhysics/Trees/Tree_SelfQueries.cs index c0d9a8f72..3e72feb78 100644 --- a/BepuPhysics/Trees/Tree_SelfQueries.cs +++ b/BepuPhysics/Trees/Tree_SelfQueries.cs @@ -283,7 +283,7 @@ public static Vector256 Intersects( { return ( Vector256.GreaterThanOrEqual(maxAX, minBX) & Vector256.GreaterThanOrEqual(maxAY, minBY) & Vector256.GreaterThanOrEqual(maxAZ, minBZ) & - Vector256.GreaterThanOrEqual(maxBX, minAX) & Vector256.GreaterThanOrEqual(maxBY, minAY) & Vector256.GreaterThanOrEqual(maxBZ, minAZ)).As(); + Vector256.GreaterThanOrEqual(maxBX, minAX) & Vector256.GreaterThanOrEqual(maxBY, minAY) & Vector256.GreaterThanOrEqual(maxBZ, minAZ)).AsInt32(); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -302,7 +302,7 @@ public static Vector256 GetLeftPackMask(Vector256 mask, out int count) ulong wanted_indices = Bmi2.X64.ParallelBitExtract(identity_indices, expanded_mask); count = BitOperations.PopCount(bitmask); - return Avx2.ConvertToVector256Int32(Vector128.CreateScalarUnsafe(wanted_indices).As()); + return Avx2.ConvertToVector256Int32(Vector128.CreateScalarUnsafe(wanted_indices).AsByte()); } @@ -332,7 +332,7 @@ public unsafe void GetSelfOverlapsContiguousPrepass(ref TOverla //A recursive self test will at some point visit all nodes with certainty. Instead of framing it as a recursive test at all, do a prepass that's just a contiguous iteration. //Include a little buffer to avoid overruns by vectorized operations. - var stackSize = 1 + ((NodeCount + 7) / 8) * 8; + var stackSize = ((NodeCount + 7) / 8 + 1) * 8; pool.Take(stackSize, out var crossoverStackA); pool.Take(stackSize, out var crossoverStackB); pool.Take(stackSize, out var nodeLeafStackA); @@ -381,6 +381,7 @@ public unsafe void GetSelfOverlapsContiguousPrepass(ref TOverla while (crossoverStackCount > 0) { + //Console.WriteLine($"START iteration, crossoverStack: {crossoverStackCount}, nodeLeafStack: {nodeLeafStackCount}"); //Note that we do not have a 'nextCrossover' entry held over from the previous iteration. Instead, we directly yoink off the stack. Serves the same purpose. //Pick the appropriate location to load from in the stack. var nextLaneCount = 8 < crossoverStackCount ? 8 : crossoverStackCount; @@ -424,7 +425,7 @@ public unsafe void GetSelfOverlapsContiguousPrepass(ref TOverla var n0B6 = 6 < nextLaneCount ? Vector256.Load(n0Pointer6 + 8) : Vector256.AllBitsSet; var n0B7 = 7 < nextLaneCount ? Vector256.Load(n0Pointer7 + 8) : Vector256.AllBitsSet; - Transpose(n0B0, n0B1, n0B2, n0B3, n0B4, n0B5, n0A6, n0B7, + Transpose(n0B0, n0B1, n0B2, n0B3, n0B4, n0B5, n0B6, n0B7, out var n0BMinX, out var n0BMinY, out var n0BMinZ, out var n0BIndex, out var n0BMaxX, out var n0BMaxY, out var n0BMaxZ, out _); @@ -476,10 +477,10 @@ public unsafe void GetSelfOverlapsContiguousPrepass(ref TOverla n0BMinX, n0BMinY, n0BMinZ, n0BMaxX, n0BMaxY, n0BMaxZ, n1BMinX, n1BMinY, n1BMinZ, n1BMaxX, n1BMaxY, n1BMaxZ); - var n0AIsInternal = Vector256.GreaterThanOrEqual(n0AIndex.As(), Vector256.Zero); - var n0BIsInternal = Vector256.GreaterThanOrEqual(n0BIndex.As(), Vector256.Zero); - var n1AIsInternal = Vector256.GreaterThanOrEqual(n1AIndex.As(), Vector256.Zero); - var n1BIsInternal = Vector256.GreaterThanOrEqual(n1BIndex.As(), Vector256.Zero); + var n0AIsInternal = Vector256.GreaterThanOrEqual(n0AIndex.AsInt32(), Vector256.Zero); + var n0BIsInternal = Vector256.GreaterThanOrEqual(n0BIndex.AsInt32(), Vector256.Zero); + var n1AIsInternal = Vector256.GreaterThanOrEqual(n1AIndex.AsInt32(), Vector256.Zero); + var n1BIsInternal = Vector256.GreaterThanOrEqual(n1BIndex.AsInt32(), Vector256.Zero); var reportAA = Vector256.AndNot(Vector256.AndNot(aaIntersects, n0AIsInternal), n1AIsInternal); var reportAB = Vector256.AndNot(Vector256.AndNot(abIntersects, n0AIsInternal), n1BIsInternal); @@ -517,8 +518,10 @@ public unsafe void GetSelfOverlapsContiguousPrepass(ref TOverla //Reporting itself is sequentialized; exposing the vectorized context to the callback is grossbad. for (int i = 0; i < reportCount; ++i) { + //Console.WriteLine($"reporting: {toReportA[i]}, {toReportB[i]}"); results.Handle(toReportA[i], toReportB[i]); } + //Console.WriteLine($"total leaf-leaf iteration: {reportCount}"); } var pushNodeLeaf0AVersus1A = aaIntersects & (n0AIsInternal ^ n1AIsInternal); @@ -552,6 +555,7 @@ public unsafe void GetSelfOverlapsContiguousPrepass(ref TOverla Vector256.Store(Vector256.Shuffle(encodedForStack1B, shuffle0B1B), (int*)nodeLeafStackB.Memory + nodeLeafStackCount); nodeLeafStackCount += count0B1B; + //Console.WriteLine($"nodeleaf for iteration: {count0A1A + count0A1B + count0B1A + count0B1B}, new stack count {nodeLeafStackCount}"); } var aaWantsToPushCrossover = aaIntersects & n0AIsInternal & n1AIsInternal; @@ -579,10 +583,13 @@ public unsafe void GetSelfOverlapsContiguousPrepass(ref TOverla Vector256.Store(Vector256.Shuffle(n0BIndex.AsInt32(), bbShuffle), crossoverStackA.Memory + crossoverStackCount); Vector256.Store(Vector256.Shuffle(n1BIndex.AsInt32(), bbShuffle), crossoverStackB.Memory + crossoverStackCount); crossoverStackCount += bbCount; + + //Console.WriteLine($"crossover count for iteration: {aaCount + abCount + baCount + bbCount}, new stack count {crossoverStackCount}"); } } } } + //Console.WriteLine("End crossovers"); pool.Return(ref crossoverStackA); pool.Return(ref crossoverStackB); QuickList stack = new(NodeCount, pool); diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index dcc3ea7e5..0dae87675 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -20,15 +20,48 @@ namespace Demos.SpecializedTests { public class TreeFiddlingTestDemo : Demo { + struct Pair : IEquatable + { + public int A; + public int B; + + public Pair(int a, int b) + { + A = a; + B = b; + } + + public bool Equals(Pair other) + { + return (A == other.A && B == other.B) || (A == other.B && B == other.A); + } + + public override bool Equals(object obj) + { + return obj is Pair pair && Equals(pair); + } + public override int GetHashCode() + { + return A.GetHashCode() + B.GetHashCode(); + } + public override string ToString() + { + return $"{A}, {B}"; + } + } struct OverlapHandler : IOverlapHandler { public int OverlapCount; public int OverlapSum; public int OverlapHash; + public int TreeLeafCount; + + //public HashSet Set; [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Handle(int indexA, int indexB) { + //Debug.Assert(indexA >= 0 && indexB >= 0 && indexA < TreeLeafCount && indexB < TreeLeafCount && Set.Add(new Pair(indexA, indexB))); ++OverlapCount; OverlapSum += indexA + indexB; OverlapHash += (indexA + (indexB * OverlapCount)) * OverlapCount; @@ -42,21 +75,23 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0; Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(4, 1)); - var width = 2; - var height = 9; + var width = 768; + var height = 768; var scale = new Vector3(1, 1, 1); DemoMeshHelper.CreateDeformedPlane(width, height, (x, y) => new Vector3(x - width * scale.X * 0.5f, 2f * (float)(Math.Sin(x * 0.5f) * Math.Sin(y * 0.5f)), y - height * scale.Y * 0.5f), scale, BufferPool, out var mesh); Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); - Test((ref OverlapHandler handler) => mesh.Tree.GetSelfOverlapsContiguousPrepass(ref handler, BufferPool), "Prepass"); - Test((ref OverlapHandler handler) => mesh.Tree.GetSelfOverlaps(ref handler), "Original"); + Test((ref OverlapHandler handler) => mesh.Tree.GetSelfOverlapsContiguousPrepass(ref handler, BufferPool), mesh.Tree.LeafCount, "Prepass"); + Test((ref OverlapHandler handler) => mesh.Tree.GetSelfOverlaps(ref handler), mesh.Tree.LeafCount, "Original"); } delegate void TestFunction(ref OverlapHandler handler); - static void Test(TestFunction function, string name) + static void Test(TestFunction function, int leafCount, string name) { var overlapHandler = new OverlapHandler(); + overlapHandler.TreeLeafCount = leafCount; + //overlapHandler.Set = new HashSet(); long accumulatedTime = 0; const int testCount = 16; for (int i = 0; i < testCount; ++i) @@ -65,6 +100,7 @@ static void Test(TestFunction function, string name) function(ref overlapHandler); var endTime = Stopwatch.GetTimestamp(); accumulatedTime += endTime - startTime; + //overlapHandler.Set.Clear(); CacheBlaster.Blast(); } Console.WriteLine($"{name} time per execution (ms): {(accumulatedTime) * 1e3 / (testCount * Stopwatch.Frequency)}"); From 09de083c86310ea06ae38b466bfbc0582fd3d63e Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 9 Sep 2022 17:43:04 -0500 Subject: [PATCH 590/947] Vector256.Shuffle apparently doesn't do well with nonconstant shuffle masks. --- BepuPhysics/Trees/Tree_SelfQueries.cs | 48 +++++++++++++-------------- 1 file changed, 24 insertions(+), 24 deletions(-) diff --git a/BepuPhysics/Trees/Tree_SelfQueries.cs b/BepuPhysics/Trees/Tree_SelfQueries.cs index 3e72feb78..4730114f8 100644 --- a/BepuPhysics/Trees/Tree_SelfQueries.cs +++ b/BepuPhysics/Trees/Tree_SelfQueries.cs @@ -502,17 +502,17 @@ public unsafe void GetSelfOverlapsContiguousPrepass(ref TOverla var encodedN1A = Encode(n1AIndex); var encodedN1B = Encode(n1BIndex); var reportCount = 0; - Vector256.Store(Vector256.Shuffle(encodedN0A, aaShuffle), toReportA); - Vector256.Store(Vector256.Shuffle(encodedN1A, aaShuffle), toReportB); + Vector256.Store(Avx2.PermuteVar8x32(encodedN0A, aaShuffle), toReportA); + Vector256.Store(Avx2.PermuteVar8x32(encodedN1A, aaShuffle), toReportB); reportCount += aaCount; - Vector256.Store(Vector256.Shuffle(encodedN0A, abShuffle), toReportA + reportCount); - Vector256.Store(Vector256.Shuffle(encodedN1B, abShuffle), toReportB + reportCount); + Vector256.Store(Avx2.PermuteVar8x32(encodedN0A, abShuffle), toReportA + reportCount); + Vector256.Store(Avx2.PermuteVar8x32(encodedN1B, abShuffle), toReportB + reportCount); reportCount += abCount; - Vector256.Store(Vector256.Shuffle(encodedN0B, baShuffle), toReportA + reportCount); - Vector256.Store(Vector256.Shuffle(encodedN1A, baShuffle), toReportB + reportCount); + Vector256.Store(Avx2.PermuteVar8x32(encodedN0B, baShuffle), toReportA + reportCount); + Vector256.Store(Avx2.PermuteVar8x32(encodedN1A, baShuffle), toReportB + reportCount); reportCount += baCount; - Vector256.Store(Vector256.Shuffle(encodedN0B, bbShuffle), toReportA + reportCount); - Vector256.Store(Vector256.Shuffle(encodedN1B, bbShuffle), toReportB + reportCount); + Vector256.Store(Avx2.PermuteVar8x32(encodedN0B, bbShuffle), toReportA + reportCount); + Vector256.Store(Avx2.PermuteVar8x32(encodedN1B, bbShuffle), toReportB + reportCount); reportCount += bbCount; //Reporting itself is sequentialized; exposing the vectorized context to the callback is grossbad. @@ -542,17 +542,17 @@ public unsafe void GetSelfOverlapsContiguousPrepass(ref TOverla var encodedForStack0B = EncodeLeafChildBForStack(encodedForStack0A); var encodedForStack1A = Vector256.Load(nextCrossover1); var encodedForStack1B = EncodeLeafChildBForStack(encodedForStack1A); - Vector256.Store(Vector256.Shuffle(encodedForStack0A, shuffle0A1A), (int*)nodeLeafStackA.Memory + nodeLeafStackCount); - Vector256.Store(Vector256.Shuffle(encodedForStack1A, shuffle0A1A), (int*)nodeLeafStackB.Memory + nodeLeafStackCount); + Vector256.Store(Avx2.PermuteVar8x32(encodedForStack0A, shuffle0A1A), (int*)nodeLeafStackA.Memory + nodeLeafStackCount); + Vector256.Store(Avx2.PermuteVar8x32(encodedForStack1A, shuffle0A1A), (int*)nodeLeafStackB.Memory + nodeLeafStackCount); nodeLeafStackCount += count0A1A; - Vector256.Store(Vector256.Shuffle(encodedForStack0A, shuffle0A1B), (int*)nodeLeafStackA.Memory + nodeLeafStackCount); - Vector256.Store(Vector256.Shuffle(encodedForStack1B, shuffle0A1B), (int*)nodeLeafStackB.Memory + nodeLeafStackCount); + Vector256.Store(Avx2.PermuteVar8x32(encodedForStack0A, shuffle0A1B), (int*)nodeLeafStackA.Memory + nodeLeafStackCount); + Vector256.Store(Avx2.PermuteVar8x32(encodedForStack1B, shuffle0A1B), (int*)nodeLeafStackB.Memory + nodeLeafStackCount); nodeLeafStackCount += count0A1B; - Vector256.Store(Vector256.Shuffle(encodedForStack0B, shuffle0B1A), (int*)nodeLeafStackA.Memory + nodeLeafStackCount); - Vector256.Store(Vector256.Shuffle(encodedForStack1A, shuffle0B1A), (int*)nodeLeafStackB.Memory + nodeLeafStackCount); + Vector256.Store(Avx2.PermuteVar8x32(encodedForStack0B, shuffle0B1A), (int*)nodeLeafStackA.Memory + nodeLeafStackCount); + Vector256.Store(Avx2.PermuteVar8x32(encodedForStack1A, shuffle0B1A), (int*)nodeLeafStackB.Memory + nodeLeafStackCount); nodeLeafStackCount += count0B1A; - Vector256.Store(Vector256.Shuffle(encodedForStack0B, shuffle0B1B), (int*)nodeLeafStackA.Memory + nodeLeafStackCount); - Vector256.Store(Vector256.Shuffle(encodedForStack1B, shuffle0B1B), (int*)nodeLeafStackB.Memory + nodeLeafStackCount); + Vector256.Store(Avx2.PermuteVar8x32(encodedForStack0B, shuffle0B1B), (int*)nodeLeafStackA.Memory + nodeLeafStackCount); + Vector256.Store(Avx2.PermuteVar8x32(encodedForStack1B, shuffle0B1B), (int*)nodeLeafStackB.Memory + nodeLeafStackCount); nodeLeafStackCount += count0B1B; //Console.WriteLine($"nodeleaf for iteration: {count0A1A + count0A1B + count0B1A + count0B1B}, new stack count {nodeLeafStackCount}"); @@ -571,17 +571,17 @@ public unsafe void GetSelfOverlapsContiguousPrepass(ref TOverla var baShuffle = GetLeftPackMask(baWantsToPushCrossover, out int baCount); var bbShuffle = GetLeftPackMask(bbWantsToPushCrossover, out int bbCount); - Vector256.Store(Vector256.Shuffle(n0AIndex.AsInt32(), aaShuffle), crossoverStackA.Memory + crossoverStackCount); - Vector256.Store(Vector256.Shuffle(n1AIndex.AsInt32(), aaShuffle), crossoverStackB.Memory + crossoverStackCount); + Vector256.Store(Avx2.PermuteVar8x32(n0AIndex.AsInt32(), aaShuffle), crossoverStackA.Memory + crossoverStackCount); + Vector256.Store(Avx2.PermuteVar8x32(n1AIndex.AsInt32(), aaShuffle), crossoverStackB.Memory + crossoverStackCount); crossoverStackCount += aaCount; - Vector256.Store(Vector256.Shuffle(n0AIndex.AsInt32(), abShuffle), crossoverStackA.Memory + crossoverStackCount); - Vector256.Store(Vector256.Shuffle(n1BIndex.AsInt32(), abShuffle), crossoverStackB.Memory + crossoverStackCount); + Vector256.Store(Avx2.PermuteVar8x32(n0AIndex.AsInt32(), abShuffle), crossoverStackA.Memory + crossoverStackCount); + Vector256.Store(Avx2.PermuteVar8x32(n1BIndex.AsInt32(), abShuffle), crossoverStackB.Memory + crossoverStackCount); crossoverStackCount += abCount; - Vector256.Store(Vector256.Shuffle(n0BIndex.AsInt32(), baShuffle), crossoverStackA.Memory + crossoverStackCount); - Vector256.Store(Vector256.Shuffle(n1AIndex.AsInt32(), baShuffle), crossoverStackB.Memory + crossoverStackCount); + Vector256.Store(Avx2.PermuteVar8x32(n0BIndex.AsInt32(), baShuffle), crossoverStackA.Memory + crossoverStackCount); + Vector256.Store(Avx2.PermuteVar8x32(n1AIndex.AsInt32(), baShuffle), crossoverStackB.Memory + crossoverStackCount); crossoverStackCount += baCount; - Vector256.Store(Vector256.Shuffle(n0BIndex.AsInt32(), bbShuffle), crossoverStackA.Memory + crossoverStackCount); - Vector256.Store(Vector256.Shuffle(n1BIndex.AsInt32(), bbShuffle), crossoverStackB.Memory + crossoverStackCount); + Vector256.Store(Avx2.PermuteVar8x32(n0BIndex.AsInt32(), bbShuffle), crossoverStackA.Memory + crossoverStackCount); + Vector256.Store(Avx2.PermuteVar8x32(n1BIndex.AsInt32(), bbShuffle), crossoverStackB.Memory + crossoverStackCount); crossoverStackCount += bbCount; //Console.WriteLine($"crossover count for iteration: {aaCount + abCount + baCount + bbCount}, new stack count {crossoverStackCount}"); From a767b67370fac86652b851611e046dcf7f8dd642 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 9 Sep 2022 20:29:32 -0500 Subject: [PATCH 591/947] Only like 1.7x slower now! --- BepuPhysics/Trees/Tree_SelfQueries.cs | 82 ++++++++++++++++++++++++--- 1 file changed, 73 insertions(+), 9 deletions(-) diff --git a/BepuPhysics/Trees/Tree_SelfQueries.cs b/BepuPhysics/Trees/Tree_SelfQueries.cs index 4730114f8..76f49e768 100644 --- a/BepuPhysics/Trees/Tree_SelfQueries.cs +++ b/BepuPhysics/Trees/Tree_SelfQueries.cs @@ -286,23 +286,87 @@ public static Vector256 Intersects( Vector256.GreaterThanOrEqual(maxBX, minAX) & Vector256.GreaterThanOrEqual(maxBY, minAY) & Vector256.GreaterThanOrEqual(maxBZ, minAZ)).AsInt32(); } + //public static void CreateLeftPackLookupTable8() + //{ + // var lookupTable = new byte[768 + 1]; //last write will be the last 3 bytes plus one, so add a little buffer. + // for (int i = 0; i < 256; ++i) + // { + // var accumulator = 0; + // int index = 0; + // for (int j = 0; j < 8; ++j) + // { + // if ((i & (1 << j)) != 0) + // { + // accumulator |= j << (index * 3); + // ++index; + // } + // } + // Unsafe.As(ref lookupTable[i * 3]) = accumulator; + // } + // Console.WriteLine("new byte[] { "); + // for (int i = 0; i < 768; ++i) + // { + // Console.Write($"{lookupTable[i]}, "); + // if (i % 32 == 31) + // Console.WriteLine(); + // } + // Console.WriteLine("}"); + + //} + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Vector256 GetLeftPackMask(Vector256 mask, out int count) { if (!Avx2.IsSupported || !Bmi2.X64.IsSupported) throw new NotSupportedException("No fallback exists! This should never be visible!"); var bitmask = Vector256.ExtractMostSignificantBits(mask); - //From https://stackoverflow.com/a/36951611, courtesy of Peter Cordes. - //pdep/pext are apparently quite slow pre-Zen3, unfortunately. This is just a proof of correctness. - ulong expanded_mask = Bmi2.X64.ParallelBitDeposit(bitmask, 0x0101010101010101); // unpack each bit to a byte - expanded_mask *= 0xFF; // mask |= mask<<1 | mask<<2 | ... | mask<<7; - // ABC... -> AAAAAAAABBBBBBBBCCCCCCCC...: replicate each bit to fill its byte - - ulong identity_indices = 0x0706050403020100; // the identity shuffle for vpermps, packed to one index per byte - ulong wanted_indices = Bmi2.X64.ParallelBitExtract(identity_indices, expanded_mask); + //This depends upon an optimization that preallocates the constant array in fixed memory. Bit of a turbohack that won't work on other runtimes. + //The lookup table includes one entry for each of the 256 possible bitmasks. Each lane requires 3 bits to define the source for a shuffle mask. + //3 bits, 8 lanes, 256 bitmasks means only 768 bytes. + ReadOnlySpan lookupTable = new byte[] { + 0, 0, 0, 0, 0, 0, 1, 0, 0, 8, 0, 0, 2, 0, 0, 16, 0, 0, 17, 0, 0, 136, 0, 0, 3, 0, 0, 24, 0, 0, 25, 0, + 0, 200, 0, 0, 26, 0, 0, 208, 0, 0, 209, 0, 0, 136, 6, 0, 4, 0, 0, 32, 0, 0, 33, 0, 0, 8, 1, 0, 34, 0, 0, 16, + 1, 0, 17, 1, 0, 136, 8, 0, 35, 0, 0, 24, 1, 0, 25, 1, 0, 200, 8, 0, 26, 1, 0, 208, 8, 0, 209, 8, 0, 136, 70, 0, + 5, 0, 0, 40, 0, 0, 41, 0, 0, 72, 1, 0, 42, 0, 0, 80, 1, 0, 81, 1, 0, 136, 10, 0, 43, 0, 0, 88, 1, 0, 89, 1, + 0, 200, 10, 0, 90, 1, 0, 208, 10, 0, 209, 10, 0, 136, 86, 0, 44, 0, 0, 96, 1, 0, 97, 1, 0, 8, 11, 0, 98, 1, 0, 16, + 11, 0, 17, 11, 0, 136, 88, 0, 99, 1, 0, 24, 11, 0, 25, 11, 0, 200, 88, 0, 26, 11, 0, 208, 88, 0, 209, 88, 0, 136, 198, 2, + 6, 0, 0, 48, 0, 0, 49, 0, 0, 136, 1, 0, 50, 0, 0, 144, 1, 0, 145, 1, 0, 136, 12, 0, 51, 0, 0, 152, 1, 0, 153, 1, + 0, 200, 12, 0, 154, 1, 0, 208, 12, 0, 209, 12, 0, 136, 102, 0, 52, 0, 0, 160, 1, 0, 161, 1, 0, 8, 13, 0, 162, 1, 0, 16, + 13, 0, 17, 13, 0, 136, 104, 0, 163, 1, 0, 24, 13, 0, 25, 13, 0, 200, 104, 0, 26, 13, 0, 208, 104, 0, 209, 104, 0, 136, 70, 3, + 53, 0, 0, 168, 1, 0, 169, 1, 0, 72, 13, 0, 170, 1, 0, 80, 13, 0, 81, 13, 0, 136, 106, 0, 171, 1, 0, 88, 13, 0, 89, 13, + 0, 200, 106, 0, 90, 13, 0, 208, 106, 0, 209, 106, 0, 136, 86, 3, 172, 1, 0, 96, 13, 0, 97, 13, 0, 8, 107, 0, 98, 13, 0, 16, + 107, 0, 17, 107, 0, 136, 88, 3, 99, 13, 0, 24, 107, 0, 25, 107, 0, 200, 88, 3, 26, 107, 0, 208, 88, 3, 209, 88, 3, 136, 198, 26, + 7, 0, 0, 56, 0, 0, 57, 0, 0, 200, 1, 0, 58, 0, 0, 208, 1, 0, 209, 1, 0, 136, 14, 0, 59, 0, 0, 216, 1, 0, 217, 1, + 0, 200, 14, 0, 218, 1, 0, 208, 14, 0, 209, 14, 0, 136, 118, 0, 60, 0, 0, 224, 1, 0, 225, 1, 0, 8, 15, 0, 226, 1, 0, 16, + 15, 0, 17, 15, 0, 136, 120, 0, 227, 1, 0, 24, 15, 0, 25, 15, 0, 200, 120, 0, 26, 15, 0, 208, 120, 0, 209, 120, 0, 136, 198, 3, + 61, 0, 0, 232, 1, 0, 233, 1, 0, 72, 15, 0, 234, 1, 0, 80, 15, 0, 81, 15, 0, 136, 122, 0, 235, 1, 0, 88, 15, 0, 89, 15, + 0, 200, 122, 0, 90, 15, 0, 208, 122, 0, 209, 122, 0, 136, 214, 3, 236, 1, 0, 96, 15, 0, 97, 15, 0, 8, 123, 0, 98, 15, 0, 16, + 123, 0, 17, 123, 0, 136, 216, 3, 99, 15, 0, 24, 123, 0, 25, 123, 0, 200, 216, 3, 26, 123, 0, 208, 216, 3, 209, 216, 3, 136, 198, 30, + 62, 0, 0, 240, 1, 0, 241, 1, 0, 136, 15, 0, 242, 1, 0, 144, 15, 0, 145, 15, 0, 136, 124, 0, 243, 1, 0, 152, 15, 0, 153, 15, + 0, 200, 124, 0, 154, 15, 0, 208, 124, 0, 209, 124, 0, 136, 230, 3, 244, 1, 0, 160, 15, 0, 161, 15, 0, 8, 125, 0, 162, 15, 0, 16, + 125, 0, 17, 125, 0, 136, 232, 3, 163, 15, 0, 24, 125, 0, 25, 125, 0, 200, 232, 3, 26, 125, 0, 208, 232, 3, 209, 232, 3, 136, 70, 31, + 245, 1, 0, 168, 15, 0, 169, 15, 0, 72, 125, 0, 170, 15, 0, 80, 125, 0, 81, 125, 0, 136, 234, 3, 171, 15, 0, 88, 125, 0, 89, 125, + 0, 200, 234, 3, 90, 125, 0, 208, 234, 3, 209, 234, 3, 136, 86, 31, 172, 15, 0, 96, 125, 0, 97, 125, 0, 8, 235, 3, 98, 125, 0, 16, + 235, 3, 17, 235, 3, 136, 88, 31, 99, 125, 0, 24, 235, 3, 25, 235, 3, 200, 88, 31, 26, 235, 3, 208, 88, 31, 209, 88, 31, 136, 198, 250 }; + var encodedLeftPackMask = Unsafe.As(ref Unsafe.Add(ref Unsafe.AsRef(lookupTable[0]), bitmask * 3)); count = BitOperations.PopCount(bitmask); - return Avx2.ConvertToVector256Int32(Vector128.CreateScalarUnsafe(wanted_indices).AsByte()); + //Broadcast, variable shift. + return Avx2.ShiftRightLogicalVariable(Vector256.Create(encodedLeftPackMask), Vector256.Create(0u, 3, 6, 9, 12, 15, 18, 21)); + + + //var bitmask = Vector256.ExtractMostSignificantBits(mask); + ////From https://stackoverflow.com/a/36951611, courtesy of Peter Cordes. + ////pdep/pext are apparently quite slow pre-Zen3, unfortunately. This is just a proof of correctness. + //ulong expanded_mask = Bmi2.X64.ParallelBitDeposit(bitmask, 0x0101010101010101); // unpack each bit to a byte + //expanded_mask *= 0xFF; // mask |= mask<<1 | mask<<2 | ... | mask<<7; + // // ABC... -> AAAAAAAABBBBBBBBCCCCCCCC...: replicate each bit to fill its byte + + //ulong identity_indices = 0x0706050403020100; // the identity shuffle for vpermps, packed to one index per byte + //ulong wanted_indices = Bmi2.X64.ParallelBitExtract(identity_indices, expanded_mask); + + //count = BitOperations.PopCount(bitmask); + //return Avx2.ConvertToVector256Int32(Vector128.CreateScalarUnsafe(wanted_indices).AsByte()); } From cbe328c9f89f0e185b3b7d9f758cb8237a1bcf62 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 9 Sep 2022 20:49:08 -0500 Subject: [PATCH 592/947] This is a doomed endeavor but I'm doing it anyway. --- BepuPhysics/Trees/Tree_SelfQueries.cs | 176 ++++++++++++++++++++++---- 1 file changed, 151 insertions(+), 25 deletions(-) diff --git a/BepuPhysics/Trees/Tree_SelfQueries.cs b/BepuPhysics/Trees/Tree_SelfQueries.cs index 76f49e768..419089cd2 100644 --- a/BepuPhysics/Trees/Tree_SelfQueries.cs +++ b/BepuPhysics/Trees/Tree_SelfQueries.cs @@ -442,6 +442,7 @@ public unsafe void GetSelfOverlapsContiguousPrepass(ref TOverla //In a given iteration, the maximum number of leaf-leaf intersections is 8 lanes * 4 child combinations per lane. var toReportA = stackalloc int[32]; var toReportB = stackalloc int[32]; + var shouldReport = stackalloc int[32]; while (crossoverStackCount > 0) { @@ -553,39 +554,164 @@ public unsafe void GetSelfOverlapsContiguousPrepass(ref TOverla var reportAnyPair = reportAA | reportAB | reportBA | reportBB; if (Vector256.LessThanAny(reportAnyPair, Vector256.Zero)) { - //At least one leaf-leaf test is reported. - //For each report vector, encode the indices, left pack for reported lanes, and store into the toReport buffers. - //TODO: The chance that this is actually faster than brute force in the worst case seems a wee bit low given that we're *still iterating at the end*. - var aaShuffle = GetLeftPackMask(reportAA, out int aaCount); - var abShuffle = GetLeftPackMask(reportAB, out int abCount); - var baShuffle = GetLeftPackMask(reportBA, out int baCount); - var bbShuffle = GetLeftPackMask(reportBB, out int bbCount); + ////At least one leaf-leaf test is reported. + ////For each report vector, encode the indices, left pack for reported lanes, and store into the toReport buffers. + ////TODO: The chance that this is actually faster than brute force in the worst case seems a wee bit low given that we're *still iterating at the end*. + //var aaShuffle = GetLeftPackMask(reportAA, out int aaCount); + //var abShuffle = GetLeftPackMask(reportAB, out int abCount); + //var baShuffle = GetLeftPackMask(reportBA, out int baCount); + //var bbShuffle = GetLeftPackMask(reportBB, out int bbCount); + + //var encodedN0A = Encode(n0AIndex); + //var encodedN0B = Encode(n0BIndex); + //var encodedN1A = Encode(n1AIndex); + //var encodedN1B = Encode(n1BIndex); + //var reportCount = 0; + //Vector256.Store(Avx2.PermuteVar8x32(encodedN0A, aaShuffle), toReportA); + //Vector256.Store(Avx2.PermuteVar8x32(encodedN1A, aaShuffle), toReportB); + //reportCount += aaCount; + //Vector256.Store(Avx2.PermuteVar8x32(encodedN0A, abShuffle), toReportA + reportCount); + //Vector256.Store(Avx2.PermuteVar8x32(encodedN1B, abShuffle), toReportB + reportCount); + //reportCount += abCount; + //Vector256.Store(Avx2.PermuteVar8x32(encodedN0B, baShuffle), toReportA + reportCount); + //Vector256.Store(Avx2.PermuteVar8x32(encodedN1A, baShuffle), toReportB + reportCount); + //reportCount += baCount; + //Vector256.Store(Avx2.PermuteVar8x32(encodedN0B, bbShuffle), toReportA + reportCount); + //Vector256.Store(Avx2.PermuteVar8x32(encodedN1B, bbShuffle), toReportB + reportCount); + //reportCount += bbCount; + + ////Reporting itself is sequentialized; exposing the vectorized context to the callback is grossbad. + //for (int i = 0; i < reportCount; ++i) + //{ + // //Console.WriteLine($"reporting: {toReportA[i]}, {toReportB[i]}"); + // results.Handle(toReportA[i], toReportB[i]); + //} + ////Console.WriteLine($"total leaf-leaf iteration: {reportCount}"); + var encodedN0A = Encode(n0AIndex); var encodedN0B = Encode(n0BIndex); var encodedN1A = Encode(n1AIndex); var encodedN1B = Encode(n1BIndex); var reportCount = 0; - Vector256.Store(Avx2.PermuteVar8x32(encodedN0A, aaShuffle), toReportA); - Vector256.Store(Avx2.PermuteVar8x32(encodedN1A, aaShuffle), toReportB); - reportCount += aaCount; - Vector256.Store(Avx2.PermuteVar8x32(encodedN0A, abShuffle), toReportA + reportCount); - Vector256.Store(Avx2.PermuteVar8x32(encodedN1B, abShuffle), toReportB + reportCount); - reportCount += abCount; - Vector256.Store(Avx2.PermuteVar8x32(encodedN0B, baShuffle), toReportA + reportCount); - Vector256.Store(Avx2.PermuteVar8x32(encodedN1A, baShuffle), toReportB + reportCount); - reportCount += baCount; - Vector256.Store(Avx2.PermuteVar8x32(encodedN0B, bbShuffle), toReportA + reportCount); - Vector256.Store(Avx2.PermuteVar8x32(encodedN1B, bbShuffle), toReportB + reportCount); - reportCount += bbCount; - - //Reporting itself is sequentialized; exposing the vectorized context to the callback is grossbad. - for (int i = 0; i < reportCount; ++i) + if (Vector256.LessThanAny(reportAA, Vector256.Zero)) + { + var aaShuffle = GetLeftPackMask(reportAA, out int aaCount); + Vector256.Store(Avx2.PermuteVar8x32(encodedN0A, aaShuffle), toReportA); + Vector256.Store(Avx2.PermuteVar8x32(encodedN1A, aaShuffle), toReportB); + reportCount += aaCount; + } + if (Vector256.LessThanAny(reportAB, Vector256.Zero)) + { + var abShuffle = GetLeftPackMask(reportAB, out int abCount); + Vector256.Store(Avx2.PermuteVar8x32(encodedN0A, abShuffle), toReportA + reportCount); + Vector256.Store(Avx2.PermuteVar8x32(encodedN1B, abShuffle), toReportB + reportCount); + reportCount += abCount; + } + if (Vector256.LessThanAny(reportBA, Vector256.Zero)) + { + var baShuffle = GetLeftPackMask(reportBA, out int baCount); + Vector256.Store(Avx2.PermuteVar8x32(encodedN0B, baShuffle), toReportA + reportCount); + Vector256.Store(Avx2.PermuteVar8x32(encodedN1A, baShuffle), toReportB + reportCount); + reportCount += baCount; + } + if (Vector256.LessThanAny(reportBB, Vector256.Zero)) { - //Console.WriteLine($"reporting: {toReportA[i]}, {toReportB[i]}"); - results.Handle(toReportA[i], toReportB[i]); + var bbShuffle = GetLeftPackMask(reportBB, out int bbCount); + Vector256.Store(Avx2.PermuteVar8x32(encodedN0B, bbShuffle), toReportA + reportCount); + Vector256.Store(Avx2.PermuteVar8x32(encodedN1B, bbShuffle), toReportB + reportCount); + reportCount += bbCount; } - //Console.WriteLine($"total leaf-leaf iteration: {reportCount}"); + + ////Reporting itself is sequentialized; exposing the vectorized context to the callback is grossbad. + //for (int i = 0; i < reportCount; ++i) + //{ + // //Console.WriteLine($"reporting: {toReportA[i]}, {toReportB[i]}"); + // results.Handle(toReportA[i], toReportB[i]); + //} + ////Console.WriteLine($"total leaf-leaf iteration: {reportCount}"); + + //var encodedN0A = Encode(n0AIndex); + //var encodedN0B = Encode(n0BIndex); + //var encodedN1A = Encode(n1AIndex); + //var encodedN1B = Encode(n1BIndex); + //var shouldReportAB = shouldReport + 8; + //var shouldReportBA = shouldReport + 16; + //var shouldReportBB = shouldReport + 24; + //Vector256.Store(reportAA, shouldReport); + //Vector256.Store(reportAB, shouldReportAB); + //Vector256.Store(reportBA, shouldReportBA); + //Vector256.Store(reportBB, shouldReportBB); + //var encodedMemoryN0B = toReportA + 8; + //var encodedMemoryN1A = toReportA + 16; + //var encodedMemoryN1B = toReportA + 24; + //Vector256.Store(encodedN0A, toReportA); + //Vector256.Store(encodedN0B, encodedMemoryN0B); + //Vector256.Store(encodedN1A, encodedMemoryN1A); + //Vector256.Store(encodedN1B, encodedMemoryN1B); + //for (int i = 0; i < 8; ++i) + //{ + // if (shouldReport[i] < 0) + // results.Handle(toReportA[i], encodedMemoryN1A[i]); + // if (shouldReportAB[i] < 0) + // results.Handle(toReportA[i], encodedMemoryN1B[i]); + // if (shouldReportBA[i] < 0) + // results.Handle(encodedMemoryN0B[i], encodedMemoryN1A[i]); + // if (shouldReportBB[i] < 0) + // results.Handle(encodedMemoryN0B[i], encodedMemoryN1B[i]); + //} + + //var encodedN0A = Encode(n0AIndex); + //var encodedN0B = Encode(n0BIndex); + //var encodedN1A = Encode(n1AIndex); + //var encodedN1B = Encode(n1BIndex); + //var encodedMemoryN0B = toReportA + 8; + //var encodedMemoryN1A = toReportA + 16; + //var encodedMemoryN1B = toReportA + 24; + //Vector256.Store(encodedN0A, toReportA); + //Vector256.Store(encodedN0B, encodedMemoryN0B); + //Vector256.Store(encodedN1A, encodedMemoryN1A); + //Vector256.Store(encodedN1B, encodedMemoryN1B); + + //if (Vector256.LessThanAny(reportAA, Vector256.Zero)) + //{ + // Vector256.Store(reportAA, shouldReport); + // for (int i = 0; i < 8; ++i) + // { + // if (shouldReport[i] < 0) + // results.Handle(toReportA[i], encodedMemoryN1A[i]); + // } + //} + //if (Vector256.LessThanAny(reportAB, Vector256.Zero)) + //{ + // var shouldReportAB = shouldReport + 8; + // Vector256.Store(reportAB, shouldReportAB); + // for (int i = 0; i < 8; ++i) + // { + // if (shouldReportAB[i] < 0) + // results.Handle(toReportA[i], encodedMemoryN1B[i]); + // } + //} + //if (Vector256.LessThanAny(reportBA, Vector256.Zero)) + //{ + // var shouldReportBA = shouldReport + 16; + // Vector256.Store(reportBA, shouldReportBA); + // for (int i = 0; i < 8; ++i) + // { + // if (shouldReportBA[i] < 0) + // results.Handle(encodedMemoryN0B[i], encodedMemoryN1A[i]); + // } + //} + //if (Vector256.LessThanAny(reportBB, Vector256.Zero)) + //{ + // var shouldReportBB = shouldReport + 24; + // Vector256.Store(reportBB, shouldReportBB); + // for (int i = 0; i < 8; ++i) + // { + // if (shouldReportBB[i] < 0) + // results.Handle(encodedMemoryN0B[i], encodedMemoryN1B[i]); + // } + //} } var pushNodeLeaf0AVersus1A = aaIntersects & (n0AIsInternal ^ n1AIsInternal); From f11b15c4fd9a9ec20624ea25529ba352a7573548 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 9 Sep 2022 21:11:33 -0500 Subject: [PATCH 593/947] only 66% slower! and not much fruit left to pick. --- BepuPhysics/Trees/Tree_SelfQueries.cs | 216 ++++++-------------------- 1 file changed, 45 insertions(+), 171 deletions(-) diff --git a/BepuPhysics/Trees/Tree_SelfQueries.cs b/BepuPhysics/Trees/Tree_SelfQueries.cs index 419089cd2..3087485db 100644 --- a/BepuPhysics/Trees/Tree_SelfQueries.cs +++ b/BepuPhysics/Trees/Tree_SelfQueries.cs @@ -481,19 +481,6 @@ public unsafe void GetSelfOverlapsContiguousPrepass(ref TOverla out var n0AMinX, out var n0AMinY, out var n0AMinZ, out var n0AIndex, out var n0AMaxX, out var n0AMaxY, out var n0AMaxZ, out _); - var n0B0 = Vector256.Load(n0Pointer0 + 8); - var n0B1 = 1 < nextLaneCount ? Vector256.Load(n0Pointer1 + 8) : Vector256.AllBitsSet; - var n0B2 = 2 < nextLaneCount ? Vector256.Load(n0Pointer2 + 8) : Vector256.AllBitsSet; - var n0B3 = 3 < nextLaneCount ? Vector256.Load(n0Pointer3 + 8) : Vector256.AllBitsSet; - var n0B4 = 4 < nextLaneCount ? Vector256.Load(n0Pointer4 + 8) : Vector256.AllBitsSet; - var n0B5 = 5 < nextLaneCount ? Vector256.Load(n0Pointer5 + 8) : Vector256.AllBitsSet; - var n0B6 = 6 < nextLaneCount ? Vector256.Load(n0Pointer6 + 8) : Vector256.AllBitsSet; - var n0B7 = 7 < nextLaneCount ? Vector256.Load(n0Pointer7 + 8) : Vector256.AllBitsSet; - - Transpose(n0B0, n0B1, n0B2, n0B3, n0B4, n0B5, n0B6, n0B7, - out var n0BMinX, out var n0BMinY, out var n0BMinZ, out var n0BIndex, - out var n0BMaxX, out var n0BMaxY, out var n0BMaxZ, out _); - var n1Pointer0 = (float*)(Nodes.Memory + nextCrossover1[0]); var n1Pointer1 = (float*)(Nodes.Memory + nextCrossover1[1]); var n1Pointer2 = (float*)(Nodes.Memory + nextCrossover1[2]); @@ -516,6 +503,27 @@ public unsafe void GetSelfOverlapsContiguousPrepass(ref TOverla out var n1AMinX, out var n1AMinY, out var n1AMinZ, out var n1AIndex, out var n1AMaxX, out var n1AMaxY, out var n1AMaxZ, out _); + var aaIntersects = Intersects( + n0AMinX, n0AMinY, n0AMinZ, n0AMaxX, n0AMaxY, n0AMaxZ, + n1AMinX, n1AMinY, n1AMinZ, n1AMaxX, n1AMaxY, n1AMaxZ); + + var n0B0 = Vector256.Load(n0Pointer0 + 8); + var n0B1 = 1 < nextLaneCount ? Vector256.Load(n0Pointer1 + 8) : Vector256.AllBitsSet; + var n0B2 = 2 < nextLaneCount ? Vector256.Load(n0Pointer2 + 8) : Vector256.AllBitsSet; + var n0B3 = 3 < nextLaneCount ? Vector256.Load(n0Pointer3 + 8) : Vector256.AllBitsSet; + var n0B4 = 4 < nextLaneCount ? Vector256.Load(n0Pointer4 + 8) : Vector256.AllBitsSet; + var n0B5 = 5 < nextLaneCount ? Vector256.Load(n0Pointer5 + 8) : Vector256.AllBitsSet; + var n0B6 = 6 < nextLaneCount ? Vector256.Load(n0Pointer6 + 8) : Vector256.AllBitsSet; + var n0B7 = 7 < nextLaneCount ? Vector256.Load(n0Pointer7 + 8) : Vector256.AllBitsSet; + + Transpose(n0B0, n0B1, n0B2, n0B3, n0B4, n0B5, n0B6, n0B7, + out var n0BMinX, out var n0BMinY, out var n0BMinZ, out var n0BIndex, + out var n0BMaxX, out var n0BMaxY, out var n0BMaxZ, out _); + + var baIntersects = Intersects( + n0BMinX, n0BMinY, n0BMinZ, n0BMaxX, n0BMaxY, n0BMaxZ, + n1AMinX, n1AMinY, n1AMinZ, n1AMaxX, n1AMaxY, n1AMaxZ); + var n1B0 = Vector256.Load(n1Pointer0 + 8); var n1B1 = 1 < nextLaneCount ? Vector256.Load(n1Pointer1 + 8) : Vector256.AllBitsSet; var n1B2 = 2 < nextLaneCount ? Vector256.Load(n1Pointer2 + 8) : Vector256.AllBitsSet; @@ -529,15 +537,9 @@ public unsafe void GetSelfOverlapsContiguousPrepass(ref TOverla out var n1BMinX, out var n1BMinY, out var n1BMinZ, out var n1BIndex, out var n1BMaxX, out var n1BMaxY, out var n1BMaxZ, out _); - var aaIntersects = Intersects( - n0AMinX, n0AMinY, n0AMinZ, n0AMaxX, n0AMaxY, n0AMaxZ, - n1AMinX, n1AMinY, n1AMinZ, n1AMaxX, n1AMaxY, n1AMaxZ); var abIntersects = Intersects( n0AMinX, n0AMinY, n0AMinZ, n0AMaxX, n0AMaxY, n0AMaxZ, n1BMinX, n1BMinY, n1BMinZ, n1BMaxX, n1BMaxY, n1BMaxZ); - var baIntersects = Intersects( - n0BMinX, n0BMinY, n0BMinZ, n0BMaxX, n0BMaxY, n0BMaxZ, - n1AMinX, n1AMinY, n1AMinZ, n1AMaxX, n1AMaxY, n1AMaxZ); var bbIntersects = Intersects( n0BMinX, n0BMinY, n0BMinZ, n0BMaxX, n0BMaxY, n0BMaxZ, n1BMinX, n1BMinY, n1BMinZ, n1BMaxX, n1BMaxY, n1BMaxZ); @@ -554,164 +556,38 @@ public unsafe void GetSelfOverlapsContiguousPrepass(ref TOverla var reportAnyPair = reportAA | reportAB | reportBA | reportBB; if (Vector256.LessThanAny(reportAnyPair, Vector256.Zero)) { - ////At least one leaf-leaf test is reported. - ////For each report vector, encode the indices, left pack for reported lanes, and store into the toReport buffers. - ////TODO: The chance that this is actually faster than brute force in the worst case seems a wee bit low given that we're *still iterating at the end*. - //var aaShuffle = GetLeftPackMask(reportAA, out int aaCount); - //var abShuffle = GetLeftPackMask(reportAB, out int abCount); - //var baShuffle = GetLeftPackMask(reportBA, out int baCount); - //var bbShuffle = GetLeftPackMask(reportBB, out int bbCount); - - //var encodedN0A = Encode(n0AIndex); - //var encodedN0B = Encode(n0BIndex); - //var encodedN1A = Encode(n1AIndex); - //var encodedN1B = Encode(n1BIndex); - //var reportCount = 0; - //Vector256.Store(Avx2.PermuteVar8x32(encodedN0A, aaShuffle), toReportA); - //Vector256.Store(Avx2.PermuteVar8x32(encodedN1A, aaShuffle), toReportB); - //reportCount += aaCount; - //Vector256.Store(Avx2.PermuteVar8x32(encodedN0A, abShuffle), toReportA + reportCount); - //Vector256.Store(Avx2.PermuteVar8x32(encodedN1B, abShuffle), toReportB + reportCount); - //reportCount += abCount; - //Vector256.Store(Avx2.PermuteVar8x32(encodedN0B, baShuffle), toReportA + reportCount); - //Vector256.Store(Avx2.PermuteVar8x32(encodedN1A, baShuffle), toReportB + reportCount); - //reportCount += baCount; - //Vector256.Store(Avx2.PermuteVar8x32(encodedN0B, bbShuffle), toReportA + reportCount); - //Vector256.Store(Avx2.PermuteVar8x32(encodedN1B, bbShuffle), toReportB + reportCount); - //reportCount += bbCount; - - ////Reporting itself is sequentialized; exposing the vectorized context to the callback is grossbad. - //for (int i = 0; i < reportCount; ++i) - //{ - // //Console.WriteLine($"reporting: {toReportA[i]}, {toReportB[i]}"); - // results.Handle(toReportA[i], toReportB[i]); - //} - ////Console.WriteLine($"total leaf-leaf iteration: {reportCount}"); - + //At least one leaf-leaf test is reported. + //For each report vector, encode the indices, left pack for reported lanes, and store into the toReport buffers. + var aaShuffle = GetLeftPackMask(reportAA, out int aaCount); + var abShuffle = GetLeftPackMask(reportAB, out int abCount); + var baShuffle = GetLeftPackMask(reportBA, out int baCount); + var bbShuffle = GetLeftPackMask(reportBB, out int bbCount); var encodedN0A = Encode(n0AIndex); var encodedN0B = Encode(n0BIndex); var encodedN1A = Encode(n1AIndex); var encodedN1B = Encode(n1BIndex); var reportCount = 0; - if (Vector256.LessThanAny(reportAA, Vector256.Zero)) - { - var aaShuffle = GetLeftPackMask(reportAA, out int aaCount); - Vector256.Store(Avx2.PermuteVar8x32(encodedN0A, aaShuffle), toReportA); - Vector256.Store(Avx2.PermuteVar8x32(encodedN1A, aaShuffle), toReportB); - reportCount += aaCount; - } - if (Vector256.LessThanAny(reportAB, Vector256.Zero)) + Vector256.Store(Avx2.PermuteVar8x32(encodedN0A, aaShuffle), toReportA); + Vector256.Store(Avx2.PermuteVar8x32(encodedN1A, aaShuffle), toReportB); + reportCount += aaCount; + Vector256.Store(Avx2.PermuteVar8x32(encodedN0A, abShuffle), toReportA + reportCount); + Vector256.Store(Avx2.PermuteVar8x32(encodedN1B, abShuffle), toReportB + reportCount); + reportCount += abCount; + Vector256.Store(Avx2.PermuteVar8x32(encodedN0B, baShuffle), toReportA + reportCount); + Vector256.Store(Avx2.PermuteVar8x32(encodedN1A, baShuffle), toReportB + reportCount); + reportCount += baCount; + Vector256.Store(Avx2.PermuteVar8x32(encodedN0B, bbShuffle), toReportA + reportCount); + Vector256.Store(Avx2.PermuteVar8x32(encodedN1B, bbShuffle), toReportB + reportCount); + reportCount += bbCount; + + //Reporting itself is sequentialized; exposing the vectorized context to the callback is grossbad. + for (int i = 0; i < reportCount; ++i) { - var abShuffle = GetLeftPackMask(reportAB, out int abCount); - Vector256.Store(Avx2.PermuteVar8x32(encodedN0A, abShuffle), toReportA + reportCount); - Vector256.Store(Avx2.PermuteVar8x32(encodedN1B, abShuffle), toReportB + reportCount); - reportCount += abCount; + //Console.WriteLine($"reporting: {toReportA[i]}, {toReportB[i]}"); + results.Handle(toReportA[i], toReportB[i]); } - if (Vector256.LessThanAny(reportBA, Vector256.Zero)) - { - var baShuffle = GetLeftPackMask(reportBA, out int baCount); - Vector256.Store(Avx2.PermuteVar8x32(encodedN0B, baShuffle), toReportA + reportCount); - Vector256.Store(Avx2.PermuteVar8x32(encodedN1A, baShuffle), toReportB + reportCount); - reportCount += baCount; - } - if (Vector256.LessThanAny(reportBB, Vector256.Zero)) - { - var bbShuffle = GetLeftPackMask(reportBB, out int bbCount); - Vector256.Store(Avx2.PermuteVar8x32(encodedN0B, bbShuffle), toReportA + reportCount); - Vector256.Store(Avx2.PermuteVar8x32(encodedN1B, bbShuffle), toReportB + reportCount); - reportCount += bbCount; - } - - ////Reporting itself is sequentialized; exposing the vectorized context to the callback is grossbad. - //for (int i = 0; i < reportCount; ++i) - //{ - // //Console.WriteLine($"reporting: {toReportA[i]}, {toReportB[i]}"); - // results.Handle(toReportA[i], toReportB[i]); - //} - ////Console.WriteLine($"total leaf-leaf iteration: {reportCount}"); - - //var encodedN0A = Encode(n0AIndex); - //var encodedN0B = Encode(n0BIndex); - //var encodedN1A = Encode(n1AIndex); - //var encodedN1B = Encode(n1BIndex); - //var shouldReportAB = shouldReport + 8; - //var shouldReportBA = shouldReport + 16; - //var shouldReportBB = shouldReport + 24; - //Vector256.Store(reportAA, shouldReport); - //Vector256.Store(reportAB, shouldReportAB); - //Vector256.Store(reportBA, shouldReportBA); - //Vector256.Store(reportBB, shouldReportBB); - //var encodedMemoryN0B = toReportA + 8; - //var encodedMemoryN1A = toReportA + 16; - //var encodedMemoryN1B = toReportA + 24; - //Vector256.Store(encodedN0A, toReportA); - //Vector256.Store(encodedN0B, encodedMemoryN0B); - //Vector256.Store(encodedN1A, encodedMemoryN1A); - //Vector256.Store(encodedN1B, encodedMemoryN1B); - //for (int i = 0; i < 8; ++i) - //{ - // if (shouldReport[i] < 0) - // results.Handle(toReportA[i], encodedMemoryN1A[i]); - // if (shouldReportAB[i] < 0) - // results.Handle(toReportA[i], encodedMemoryN1B[i]); - // if (shouldReportBA[i] < 0) - // results.Handle(encodedMemoryN0B[i], encodedMemoryN1A[i]); - // if (shouldReportBB[i] < 0) - // results.Handle(encodedMemoryN0B[i], encodedMemoryN1B[i]); - //} - - //var encodedN0A = Encode(n0AIndex); - //var encodedN0B = Encode(n0BIndex); - //var encodedN1A = Encode(n1AIndex); - //var encodedN1B = Encode(n1BIndex); - //var encodedMemoryN0B = toReportA + 8; - //var encodedMemoryN1A = toReportA + 16; - //var encodedMemoryN1B = toReportA + 24; - //Vector256.Store(encodedN0A, toReportA); - //Vector256.Store(encodedN0B, encodedMemoryN0B); - //Vector256.Store(encodedN1A, encodedMemoryN1A); - //Vector256.Store(encodedN1B, encodedMemoryN1B); - - //if (Vector256.LessThanAny(reportAA, Vector256.Zero)) - //{ - // Vector256.Store(reportAA, shouldReport); - // for (int i = 0; i < 8; ++i) - // { - // if (shouldReport[i] < 0) - // results.Handle(toReportA[i], encodedMemoryN1A[i]); - // } - //} - //if (Vector256.LessThanAny(reportAB, Vector256.Zero)) - //{ - // var shouldReportAB = shouldReport + 8; - // Vector256.Store(reportAB, shouldReportAB); - // for (int i = 0; i < 8; ++i) - // { - // if (shouldReportAB[i] < 0) - // results.Handle(toReportA[i], encodedMemoryN1B[i]); - // } - //} - //if (Vector256.LessThanAny(reportBA, Vector256.Zero)) - //{ - // var shouldReportBA = shouldReport + 16; - // Vector256.Store(reportBA, shouldReportBA); - // for (int i = 0; i < 8; ++i) - // { - // if (shouldReportBA[i] < 0) - // results.Handle(encodedMemoryN0B[i], encodedMemoryN1A[i]); - // } - //} - //if (Vector256.LessThanAny(reportBB, Vector256.Zero)) - //{ - // var shouldReportBB = shouldReport + 24; - // Vector256.Store(reportBB, shouldReportBB); - // for (int i = 0; i < 8; ++i) - // { - // if (shouldReportBB[i] < 0) - // results.Handle(encodedMemoryN0B[i], encodedMemoryN1B[i]); - // } - //} + //Console.WriteLine($"total leaf-leaf iteration: {reportCount}"); } var pushNodeLeaf0AVersus1A = aaIntersects & (n0AIsInternal ^ n1AIsInternal); @@ -744,8 +620,6 @@ public unsafe void GetSelfOverlapsContiguousPrepass(ref TOverla Vector256.Store(Avx2.PermuteVar8x32(encodedForStack0B, shuffle0B1B), (int*)nodeLeafStackA.Memory + nodeLeafStackCount); Vector256.Store(Avx2.PermuteVar8x32(encodedForStack1B, shuffle0B1B), (int*)nodeLeafStackB.Memory + nodeLeafStackCount); nodeLeafStackCount += count0B1B; - - //Console.WriteLine($"nodeleaf for iteration: {count0A1A + count0A1B + count0B1A + count0B1B}, new stack count {nodeLeafStackCount}"); } var aaWantsToPushCrossover = aaIntersects & n0AIsInternal & n1AIsInternal; From 573740a3ea1aeaee450682feebaa7eaa05267f33 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 10 Sep 2022 15:44:09 -0500 Subject: [PATCH 594/947] Another very poor option! --- BepuPhysics/Trees/Tree_SelfQueries.cs | 424 ++++---------------------- BepuUtilities/BoundingBox.cs | 14 +- 2 files changed, 63 insertions(+), 375 deletions(-) diff --git a/BepuPhysics/Trees/Tree_SelfQueries.cs b/BepuPhysics/Trees/Tree_SelfQueries.cs index 3087485db..7584206a8 100644 --- a/BepuPhysics/Trees/Tree_SelfQueries.cs +++ b/BepuPhysics/Trees/Tree_SelfQueries.cs @@ -235,158 +235,6 @@ static ref NodeChild GetLeafChild(ref Tree tree, uint encodedLeafParentIndex) return ref leafIsChildB ? ref parent.B : ref parent.A; } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Transpose( - Vector256 m0, Vector256 m1, Vector256 m2, Vector256 m3, Vector256 m4, Vector256 m5, Vector256 m6, Vector256 m7, - out Vector256 t0, out Vector256 t1, out Vector256 t2, out Vector256 t3, out Vector256 t4, out Vector256 t5, out Vector256 t6, out Vector256 t7) - { - - if (Avx.IsSupported) - { - var n0 = Avx.UnpackLow(m0, m1); - var n1 = Avx.UnpackLow(m2, m3); - var n2 = Avx.UnpackLow(m4, m5); - var n3 = Avx.UnpackLow(m6, m7); - var n4 = Avx.UnpackHigh(m0, m1); - var n5 = Avx.UnpackHigh(m2, m3); - var n6 = Avx.UnpackHigh(m4, m5); - var n7 = Avx.UnpackHigh(m6, m7); - - var o0 = Avx.Shuffle(n0, n1, 0 | (1 << 2) | (0 << 4) | (1 << 6)); - var o1 = Avx.Shuffle(n2, n3, 0 | (1 << 2) | (0 << 4) | (1 << 6)); - var o2 = Avx.Shuffle(n4, n5, 0 | (1 << 2) | (0 << 4) | (1 << 6)); - var o3 = Avx.Shuffle(n6, n7, 0 | (1 << 2) | (0 << 4) | (1 << 6)); - var o4 = Avx.Shuffle(n0, n1, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - var o5 = Avx.Shuffle(n2, n3, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - var o6 = Avx.Shuffle(n4, n5, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - var o7 = Avx.Shuffle(n6, n7, 2 | (3 << 2) | (2 << 4) | (3 << 6)); - - t0 = Avx.Permute2x128(o0, o1, 0 | (2 << 4)); - t1 = Avx.Permute2x128(o4, o5, 0 | (2 << 4)); - t2 = Avx.Permute2x128(o2, o3, 0 | (2 << 4)); - t3 = Avx.Permute2x128(o6, o7, 0 | (2 << 4)); - - t4 = Avx.Permute2x128(o0, o1, 1 | (3 << 4)); - t5 = Avx.Permute2x128(o4, o5, 1 | (3 << 4)); - t6 = Avx.Permute2x128(o2, o3, 1 | (3 << 4)); - t7 = Avx.Permute2x128(o6, o7, 1 | (3 << 4)); - } - else - { - throw new NotSupportedException("No fallback exists! This should never be visible!"); - } - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector256 Intersects( - Vector256 minAX, Vector256 minAY, Vector256 minAZ, Vector256 maxAX, Vector256 maxAY, Vector256 maxAZ, - Vector256 minBX, Vector256 minBY, Vector256 minBZ, Vector256 maxBX, Vector256 maxBY, Vector256 maxBZ) - { - return ( - Vector256.GreaterThanOrEqual(maxAX, minBX) & Vector256.GreaterThanOrEqual(maxAY, minBY) & Vector256.GreaterThanOrEqual(maxAZ, minBZ) & - Vector256.GreaterThanOrEqual(maxBX, minAX) & Vector256.GreaterThanOrEqual(maxBY, minAY) & Vector256.GreaterThanOrEqual(maxBZ, minAZ)).AsInt32(); - } - - //public static void CreateLeftPackLookupTable8() - //{ - // var lookupTable = new byte[768 + 1]; //last write will be the last 3 bytes plus one, so add a little buffer. - // for (int i = 0; i < 256; ++i) - // { - // var accumulator = 0; - // int index = 0; - // for (int j = 0; j < 8; ++j) - // { - // if ((i & (1 << j)) != 0) - // { - // accumulator |= j << (index * 3); - // ++index; - // } - // } - // Unsafe.As(ref lookupTable[i * 3]) = accumulator; - // } - // Console.WriteLine("new byte[] { "); - // for (int i = 0; i < 768; ++i) - // { - // Console.Write($"{lookupTable[i]}, "); - // if (i % 32 == 31) - // Console.WriteLine(); - // } - // Console.WriteLine("}"); - - //} - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector256 GetLeftPackMask(Vector256 mask, out int count) - { - if (!Avx2.IsSupported || !Bmi2.X64.IsSupported) throw new NotSupportedException("No fallback exists! This should never be visible!"); - - var bitmask = Vector256.ExtractMostSignificantBits(mask); - //This depends upon an optimization that preallocates the constant array in fixed memory. Bit of a turbohack that won't work on other runtimes. - //The lookup table includes one entry for each of the 256 possible bitmasks. Each lane requires 3 bits to define the source for a shuffle mask. - //3 bits, 8 lanes, 256 bitmasks means only 768 bytes. - ReadOnlySpan lookupTable = new byte[] { - 0, 0, 0, 0, 0, 0, 1, 0, 0, 8, 0, 0, 2, 0, 0, 16, 0, 0, 17, 0, 0, 136, 0, 0, 3, 0, 0, 24, 0, 0, 25, 0, - 0, 200, 0, 0, 26, 0, 0, 208, 0, 0, 209, 0, 0, 136, 6, 0, 4, 0, 0, 32, 0, 0, 33, 0, 0, 8, 1, 0, 34, 0, 0, 16, - 1, 0, 17, 1, 0, 136, 8, 0, 35, 0, 0, 24, 1, 0, 25, 1, 0, 200, 8, 0, 26, 1, 0, 208, 8, 0, 209, 8, 0, 136, 70, 0, - 5, 0, 0, 40, 0, 0, 41, 0, 0, 72, 1, 0, 42, 0, 0, 80, 1, 0, 81, 1, 0, 136, 10, 0, 43, 0, 0, 88, 1, 0, 89, 1, - 0, 200, 10, 0, 90, 1, 0, 208, 10, 0, 209, 10, 0, 136, 86, 0, 44, 0, 0, 96, 1, 0, 97, 1, 0, 8, 11, 0, 98, 1, 0, 16, - 11, 0, 17, 11, 0, 136, 88, 0, 99, 1, 0, 24, 11, 0, 25, 11, 0, 200, 88, 0, 26, 11, 0, 208, 88, 0, 209, 88, 0, 136, 198, 2, - 6, 0, 0, 48, 0, 0, 49, 0, 0, 136, 1, 0, 50, 0, 0, 144, 1, 0, 145, 1, 0, 136, 12, 0, 51, 0, 0, 152, 1, 0, 153, 1, - 0, 200, 12, 0, 154, 1, 0, 208, 12, 0, 209, 12, 0, 136, 102, 0, 52, 0, 0, 160, 1, 0, 161, 1, 0, 8, 13, 0, 162, 1, 0, 16, - 13, 0, 17, 13, 0, 136, 104, 0, 163, 1, 0, 24, 13, 0, 25, 13, 0, 200, 104, 0, 26, 13, 0, 208, 104, 0, 209, 104, 0, 136, 70, 3, - 53, 0, 0, 168, 1, 0, 169, 1, 0, 72, 13, 0, 170, 1, 0, 80, 13, 0, 81, 13, 0, 136, 106, 0, 171, 1, 0, 88, 13, 0, 89, 13, - 0, 200, 106, 0, 90, 13, 0, 208, 106, 0, 209, 106, 0, 136, 86, 3, 172, 1, 0, 96, 13, 0, 97, 13, 0, 8, 107, 0, 98, 13, 0, 16, - 107, 0, 17, 107, 0, 136, 88, 3, 99, 13, 0, 24, 107, 0, 25, 107, 0, 200, 88, 3, 26, 107, 0, 208, 88, 3, 209, 88, 3, 136, 198, 26, - 7, 0, 0, 56, 0, 0, 57, 0, 0, 200, 1, 0, 58, 0, 0, 208, 1, 0, 209, 1, 0, 136, 14, 0, 59, 0, 0, 216, 1, 0, 217, 1, - 0, 200, 14, 0, 218, 1, 0, 208, 14, 0, 209, 14, 0, 136, 118, 0, 60, 0, 0, 224, 1, 0, 225, 1, 0, 8, 15, 0, 226, 1, 0, 16, - 15, 0, 17, 15, 0, 136, 120, 0, 227, 1, 0, 24, 15, 0, 25, 15, 0, 200, 120, 0, 26, 15, 0, 208, 120, 0, 209, 120, 0, 136, 198, 3, - 61, 0, 0, 232, 1, 0, 233, 1, 0, 72, 15, 0, 234, 1, 0, 80, 15, 0, 81, 15, 0, 136, 122, 0, 235, 1, 0, 88, 15, 0, 89, 15, - 0, 200, 122, 0, 90, 15, 0, 208, 122, 0, 209, 122, 0, 136, 214, 3, 236, 1, 0, 96, 15, 0, 97, 15, 0, 8, 123, 0, 98, 15, 0, 16, - 123, 0, 17, 123, 0, 136, 216, 3, 99, 15, 0, 24, 123, 0, 25, 123, 0, 200, 216, 3, 26, 123, 0, 208, 216, 3, 209, 216, 3, 136, 198, 30, - 62, 0, 0, 240, 1, 0, 241, 1, 0, 136, 15, 0, 242, 1, 0, 144, 15, 0, 145, 15, 0, 136, 124, 0, 243, 1, 0, 152, 15, 0, 153, 15, - 0, 200, 124, 0, 154, 15, 0, 208, 124, 0, 209, 124, 0, 136, 230, 3, 244, 1, 0, 160, 15, 0, 161, 15, 0, 8, 125, 0, 162, 15, 0, 16, - 125, 0, 17, 125, 0, 136, 232, 3, 163, 15, 0, 24, 125, 0, 25, 125, 0, 200, 232, 3, 26, 125, 0, 208, 232, 3, 209, 232, 3, 136, 70, 31, - 245, 1, 0, 168, 15, 0, 169, 15, 0, 72, 125, 0, 170, 15, 0, 80, 125, 0, 81, 125, 0, 136, 234, 3, 171, 15, 0, 88, 125, 0, 89, 125, - 0, 200, 234, 3, 90, 125, 0, 208, 234, 3, 209, 234, 3, 136, 86, 31, 172, 15, 0, 96, 125, 0, 97, 125, 0, 8, 235, 3, 98, 125, 0, 16, - 235, 3, 17, 235, 3, 136, 88, 31, 99, 125, 0, 24, 235, 3, 25, 235, 3, 200, 88, 31, 26, 235, 3, 208, 88, 31, 209, 88, 31, 136, 198, 250 }; - var encodedLeftPackMask = Unsafe.As(ref Unsafe.Add(ref Unsafe.AsRef(lookupTable[0]), bitmask * 3)); - - count = BitOperations.PopCount(bitmask); - //Broadcast, variable shift. - return Avx2.ShiftRightLogicalVariable(Vector256.Create(encodedLeftPackMask), Vector256.Create(0u, 3, 6, 9, 12, 15, 18, 21)); - - - //var bitmask = Vector256.ExtractMostSignificantBits(mask); - ////From https://stackoverflow.com/a/36951611, courtesy of Peter Cordes. - ////pdep/pext are apparently quite slow pre-Zen3, unfortunately. This is just a proof of correctness. - //ulong expanded_mask = Bmi2.X64.ParallelBitDeposit(bitmask, 0x0101010101010101); // unpack each bit to a byte - //expanded_mask *= 0xFF; // mask |= mask<<1 | mask<<2 | ... | mask<<7; - // // ABC... -> AAAAAAAABBBBBBBBCCCCCCCC...: replicate each bit to fill its byte - - //ulong identity_indices = 0x0706050403020100; // the identity shuffle for vpermps, packed to one index per byte - //ulong wanted_indices = Bmi2.X64.ParallelBitExtract(identity_indices, expanded_mask); - - //count = BitOperations.PopCount(bitmask); - //return Avx2.ConvertToVector256Int32(Vector128.CreateScalarUnsafe(wanted_indices).AsByte()); - - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector256 Encode(Vector256 index) - { - return Vector256.AllBitsSet - index; - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector256 Encode(Vector256 index) - { - return Vector256.AllBitsSet - index.AsInt32(); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector256 EncodeLeafChildBForStack(Vector256 parentIndex) - { - return parentIndex | Vector256.Create(1 << 31); - } - public unsafe void GetSelfOverlapsContiguousPrepass(ref TOverlapHandler results, BufferPool pool) where TOverlapHandler : IOverlapHandler { //If there are less than two leaves, there can't be any overlap. @@ -435,225 +283,65 @@ public unsafe void GetSelfOverlapsContiguousPrepass(ref TOverla //Now, we need to complete all crossovers and leaf-node tests. Note that leaf-node tests can only generate leaf-node or leaf-leaf tests, they never produce crossovers. //In contrast, crossovers can generate leaf-node tests. So do crossovers first so we'll have every leaf-node test in the stack ready to go. - if (crossoverStackCount > 0) + while (crossoverStackCount > 0) { - if (Vector256.IsHardwareAccelerated) - { - //In a given iteration, the maximum number of leaf-leaf intersections is 8 lanes * 4 child combinations per lane. - var toReportA = stackalloc int[32]; - var toReportB = stackalloc int[32]; - var shouldReport = stackalloc int[32]; - - while (crossoverStackCount > 0) - { - //Console.WriteLine($"START iteration, crossoverStack: {crossoverStackCount}, nodeLeafStack: {nodeLeafStackCount}"); - //Note that we do not have a 'nextCrossover' entry held over from the previous iteration. Instead, we directly yoink off the stack. Serves the same purpose. - //Pick the appropriate location to load from in the stack. - var nextLaneCount = 8 < crossoverStackCount ? 8 : crossoverStackCount; - var startIndex = crossoverStackCount - nextLaneCount; - var nextCrossover0 = crossoverStackA.Memory + startIndex; - var nextCrossover1 = crossoverStackB.Memory + startIndex; - crossoverStackCount = startIndex; - - //Each crossover execution loads two nodes, each having two children, for each lane. - //Gather all of it and transpose it for vectorized operations. - //Note that we're not actually getting much benefit on the actual intersection test here; - //the goal of going wide is to reduce the burden of branch misprediction by brute forcing it with vectorization. - var n0Pointer0 = (float*)(Nodes.Memory + nextCrossover0[0]); - var n0Pointer1 = (float*)(Nodes.Memory + nextCrossover0[1]); - var n0Pointer2 = (float*)(Nodes.Memory + nextCrossover0[2]); - var n0Pointer3 = (float*)(Nodes.Memory + nextCrossover0[3]); - var n0Pointer4 = (float*)(Nodes.Memory + nextCrossover0[4]); - var n0Pointer5 = (float*)(Nodes.Memory + nextCrossover0[5]); - var n0Pointer6 = (float*)(Nodes.Memory + nextCrossover0[6]); - var n0Pointer7 = (float*)(Nodes.Memory + nextCrossover0[7]); - - var n0A0 = Vector256.Load(n0Pointer0); - var n0A1 = 1 < nextLaneCount ? Vector256.Load(n0Pointer1) : Vector256.AllBitsSet; - var n0A2 = 2 < nextLaneCount ? Vector256.Load(n0Pointer2) : Vector256.AllBitsSet; - var n0A3 = 3 < nextLaneCount ? Vector256.Load(n0Pointer3) : Vector256.AllBitsSet; - var n0A4 = 4 < nextLaneCount ? Vector256.Load(n0Pointer4) : Vector256.AllBitsSet; - var n0A5 = 5 < nextLaneCount ? Vector256.Load(n0Pointer5) : Vector256.AllBitsSet; - var n0A6 = 6 < nextLaneCount ? Vector256.Load(n0Pointer6) : Vector256.AllBitsSet; - var n0A7 = 7 < nextLaneCount ? Vector256.Load(n0Pointer7) : Vector256.AllBitsSet; - - Transpose(n0A0, n0A1, n0A2, n0A3, n0A4, n0A5, n0A6, n0A7, - out var n0AMinX, out var n0AMinY, out var n0AMinZ, out var n0AIndex, - out var n0AMaxX, out var n0AMaxY, out var n0AMaxZ, out _); - - var n1Pointer0 = (float*)(Nodes.Memory + nextCrossover1[0]); - var n1Pointer1 = (float*)(Nodes.Memory + nextCrossover1[1]); - var n1Pointer2 = (float*)(Nodes.Memory + nextCrossover1[2]); - var n1Pointer3 = (float*)(Nodes.Memory + nextCrossover1[3]); - var n1Pointer4 = (float*)(Nodes.Memory + nextCrossover1[4]); - var n1Pointer5 = (float*)(Nodes.Memory + nextCrossover1[5]); - var n1Pointer6 = (float*)(Nodes.Memory + nextCrossover1[6]); - var n1Pointer7 = (float*)(Nodes.Memory + nextCrossover1[7]); - - var n1A0 = Vector256.Load(n1Pointer0); - var n1A1 = 1 < nextLaneCount ? Vector256.Load(n1Pointer1) : Vector256.AllBitsSet; - var n1A2 = 2 < nextLaneCount ? Vector256.Load(n1Pointer2) : Vector256.AllBitsSet; - var n1A3 = 3 < nextLaneCount ? Vector256.Load(n1Pointer3) : Vector256.AllBitsSet; - var n1A4 = 4 < nextLaneCount ? Vector256.Load(n1Pointer4) : Vector256.AllBitsSet; - var n1A5 = 5 < nextLaneCount ? Vector256.Load(n1Pointer5) : Vector256.AllBitsSet; - var n1A6 = 6 < nextLaneCount ? Vector256.Load(n1Pointer6) : Vector256.AllBitsSet; - var n1A7 = 7 < nextLaneCount ? Vector256.Load(n1Pointer7) : Vector256.AllBitsSet; - - Transpose(n1A0, n1A1, n1A2, n1A3, n1A4, n1A5, n1A6, n1A7, - out var n1AMinX, out var n1AMinY, out var n1AMinZ, out var n1AIndex, - out var n1AMaxX, out var n1AMaxY, out var n1AMaxZ, out _); - - var aaIntersects = Intersects( - n0AMinX, n0AMinY, n0AMinZ, n0AMaxX, n0AMaxY, n0AMaxZ, - n1AMinX, n1AMinY, n1AMinZ, n1AMaxX, n1AMaxY, n1AMaxZ); - - var n0B0 = Vector256.Load(n0Pointer0 + 8); - var n0B1 = 1 < nextLaneCount ? Vector256.Load(n0Pointer1 + 8) : Vector256.AllBitsSet; - var n0B2 = 2 < nextLaneCount ? Vector256.Load(n0Pointer2 + 8) : Vector256.AllBitsSet; - var n0B3 = 3 < nextLaneCount ? Vector256.Load(n0Pointer3 + 8) : Vector256.AllBitsSet; - var n0B4 = 4 < nextLaneCount ? Vector256.Load(n0Pointer4 + 8) : Vector256.AllBitsSet; - var n0B5 = 5 < nextLaneCount ? Vector256.Load(n0Pointer5 + 8) : Vector256.AllBitsSet; - var n0B6 = 6 < nextLaneCount ? Vector256.Load(n0Pointer6 + 8) : Vector256.AllBitsSet; - var n0B7 = 7 < nextLaneCount ? Vector256.Load(n0Pointer7 + 8) : Vector256.AllBitsSet; - - Transpose(n0B0, n0B1, n0B2, n0B3, n0B4, n0B5, n0B6, n0B7, - out var n0BMinX, out var n0BMinY, out var n0BMinZ, out var n0BIndex, - out var n0BMaxX, out var n0BMaxY, out var n0BMaxZ, out _); - - var baIntersects = Intersects( - n0BMinX, n0BMinY, n0BMinZ, n0BMaxX, n0BMaxY, n0BMaxZ, - n1AMinX, n1AMinY, n1AMinZ, n1AMaxX, n1AMaxY, n1AMaxZ); - - var n1B0 = Vector256.Load(n1Pointer0 + 8); - var n1B1 = 1 < nextLaneCount ? Vector256.Load(n1Pointer1 + 8) : Vector256.AllBitsSet; - var n1B2 = 2 < nextLaneCount ? Vector256.Load(n1Pointer2 + 8) : Vector256.AllBitsSet; - var n1B3 = 3 < nextLaneCount ? Vector256.Load(n1Pointer3 + 8) : Vector256.AllBitsSet; - var n1B4 = 4 < nextLaneCount ? Vector256.Load(n1Pointer4 + 8) : Vector256.AllBitsSet; - var n1B5 = 5 < nextLaneCount ? Vector256.Load(n1Pointer5 + 8) : Vector256.AllBitsSet; - var n1B6 = 6 < nextLaneCount ? Vector256.Load(n1Pointer6 + 8) : Vector256.AllBitsSet; - var n1B7 = 7 < nextLaneCount ? Vector256.Load(n1Pointer7 + 8) : Vector256.AllBitsSet; - - Transpose(n1B0, n1B1, n1B2, n1B3, n1B4, n1B5, n1B6, n1B7, - out var n1BMinX, out var n1BMinY, out var n1BMinZ, out var n1BIndex, - out var n1BMaxX, out var n1BMaxY, out var n1BMaxZ, out _); - - var abIntersects = Intersects( - n0AMinX, n0AMinY, n0AMinZ, n0AMaxX, n0AMaxY, n0AMaxZ, - n1BMinX, n1BMinY, n1BMinZ, n1BMaxX, n1BMaxY, n1BMaxZ); - var bbIntersects = Intersects( - n0BMinX, n0BMinY, n0BMinZ, n0BMaxX, n0BMaxY, n0BMaxZ, - n1BMinX, n1BMinY, n1BMinZ, n1BMaxX, n1BMaxY, n1BMaxZ); - - var n0AIsInternal = Vector256.GreaterThanOrEqual(n0AIndex.AsInt32(), Vector256.Zero); - var n0BIsInternal = Vector256.GreaterThanOrEqual(n0BIndex.AsInt32(), Vector256.Zero); - var n1AIsInternal = Vector256.GreaterThanOrEqual(n1AIndex.AsInt32(), Vector256.Zero); - var n1BIsInternal = Vector256.GreaterThanOrEqual(n1BIndex.AsInt32(), Vector256.Zero); - - var reportAA = Vector256.AndNot(Vector256.AndNot(aaIntersects, n0AIsInternal), n1AIsInternal); - var reportAB = Vector256.AndNot(Vector256.AndNot(abIntersects, n0AIsInternal), n1BIsInternal); - var reportBA = Vector256.AndNot(Vector256.AndNot(baIntersects, n0BIsInternal), n1AIsInternal); - var reportBB = Vector256.AndNot(Vector256.AndNot(bbIntersects, n0BIsInternal), n1BIsInternal); - var reportAnyPair = reportAA | reportAB | reportBA | reportBB; - if (Vector256.LessThanAny(reportAnyPair, Vector256.Zero)) - { - //At least one leaf-leaf test is reported. - //For each report vector, encode the indices, left pack for reported lanes, and store into the toReport buffers. - var aaShuffle = GetLeftPackMask(reportAA, out int aaCount); - var abShuffle = GetLeftPackMask(reportAB, out int abCount); - var baShuffle = GetLeftPackMask(reportBA, out int baCount); - var bbShuffle = GetLeftPackMask(reportBB, out int bbCount); - - var encodedN0A = Encode(n0AIndex); - var encodedN0B = Encode(n0BIndex); - var encodedN1A = Encode(n1AIndex); - var encodedN1B = Encode(n1BIndex); - var reportCount = 0; - Vector256.Store(Avx2.PermuteVar8x32(encodedN0A, aaShuffle), toReportA); - Vector256.Store(Avx2.PermuteVar8x32(encodedN1A, aaShuffle), toReportB); - reportCount += aaCount; - Vector256.Store(Avx2.PermuteVar8x32(encodedN0A, abShuffle), toReportA + reportCount); - Vector256.Store(Avx2.PermuteVar8x32(encodedN1B, abShuffle), toReportB + reportCount); - reportCount += abCount; - Vector256.Store(Avx2.PermuteVar8x32(encodedN0B, baShuffle), toReportA + reportCount); - Vector256.Store(Avx2.PermuteVar8x32(encodedN1A, baShuffle), toReportB + reportCount); - reportCount += baCount; - Vector256.Store(Avx2.PermuteVar8x32(encodedN0B, bbShuffle), toReportA + reportCount); - Vector256.Store(Avx2.PermuteVar8x32(encodedN1B, bbShuffle), toReportB + reportCount); - reportCount += bbCount; - - //Reporting itself is sequentialized; exposing the vectorized context to the callback is grossbad. - for (int i = 0; i < reportCount; ++i) - { - //Console.WriteLine($"reporting: {toReportA[i]}, {toReportB[i]}"); - results.Handle(toReportA[i], toReportB[i]); - } - //Console.WriteLine($"total leaf-leaf iteration: {reportCount}"); - } - - var pushNodeLeaf0AVersus1A = aaIntersects & (n0AIsInternal ^ n1AIsInternal); - var pushNodeLeaf0AVersus1B = abIntersects & (n0AIsInternal ^ n1BIsInternal); - var pushNodeLeaf0BVersus1A = baIntersects & (n0BIsInternal ^ n1AIsInternal); - var pushNodeLeaf0BVersus1B = bbIntersects & (n0BIsInternal ^ n1BIsInternal); - var pushAnyNodeLeaf = pushNodeLeaf0AVersus1A | pushNodeLeaf0AVersus1B | pushNodeLeaf0BVersus1A | pushNodeLeaf0BVersus1B; - - if (Vector256.LessThanAny(pushAnyNodeLeaf, Vector256.Zero)) - { - //At least one node-leaf push is needed. - var shuffle0A1A = GetLeftPackMask(pushNodeLeaf0AVersus1A, out int count0A1A); - var shuffle0A1B = GetLeftPackMask(pushNodeLeaf0AVersus1B, out int count0A1B); - var shuffle0B1A = GetLeftPackMask(pushNodeLeaf0BVersus1A, out int count0B1A); - var shuffle0B1B = GetLeftPackMask(pushNodeLeaf0BVersus1B, out int count0B1B); - - var encodedForStack0A = Vector256.Load(nextCrossover0); - var encodedForStack0B = EncodeLeafChildBForStack(encodedForStack0A); - var encodedForStack1A = Vector256.Load(nextCrossover1); - var encodedForStack1B = EncodeLeafChildBForStack(encodedForStack1A); - Vector256.Store(Avx2.PermuteVar8x32(encodedForStack0A, shuffle0A1A), (int*)nodeLeafStackA.Memory + nodeLeafStackCount); - Vector256.Store(Avx2.PermuteVar8x32(encodedForStack1A, shuffle0A1A), (int*)nodeLeafStackB.Memory + nodeLeafStackCount); - nodeLeafStackCount += count0A1A; - Vector256.Store(Avx2.PermuteVar8x32(encodedForStack0A, shuffle0A1B), (int*)nodeLeafStackA.Memory + nodeLeafStackCount); - Vector256.Store(Avx2.PermuteVar8x32(encodedForStack1B, shuffle0A1B), (int*)nodeLeafStackB.Memory + nodeLeafStackCount); - nodeLeafStackCount += count0A1B; - Vector256.Store(Avx2.PermuteVar8x32(encodedForStack0B, shuffle0B1A), (int*)nodeLeafStackA.Memory + nodeLeafStackCount); - Vector256.Store(Avx2.PermuteVar8x32(encodedForStack1A, shuffle0B1A), (int*)nodeLeafStackB.Memory + nodeLeafStackCount); - nodeLeafStackCount += count0B1A; - Vector256.Store(Avx2.PermuteVar8x32(encodedForStack0B, shuffle0B1B), (int*)nodeLeafStackA.Memory + nodeLeafStackCount); - Vector256.Store(Avx2.PermuteVar8x32(encodedForStack1B, shuffle0B1B), (int*)nodeLeafStackB.Memory + nodeLeafStackCount); - nodeLeafStackCount += count0B1B; - } - - var aaWantsToPushCrossover = aaIntersects & n0AIsInternal & n1AIsInternal; - var abWantsToPushCrossover = abIntersects & n0AIsInternal & n1BIsInternal; - var baWantsToPushCrossover = baIntersects & n0BIsInternal & n1AIsInternal; - var bbWantsToPushCrossover = bbIntersects & n0BIsInternal & n1BIsInternal; - var pushAnyCrossover = aaWantsToPushCrossover | abWantsToPushCrossover | baWantsToPushCrossover | bbWantsToPushCrossover; - if (Vector256.LessThanAny(pushAnyCrossover, Vector256.Zero)) - { - //At least one push for crossovers. - var aaShuffle = GetLeftPackMask(aaWantsToPushCrossover, out int aaCount); - var abShuffle = GetLeftPackMask(abWantsToPushCrossover, out int abCount); - var baShuffle = GetLeftPackMask(baWantsToPushCrossover, out int baCount); - var bbShuffle = GetLeftPackMask(bbWantsToPushCrossover, out int bbCount); - - Vector256.Store(Avx2.PermuteVar8x32(n0AIndex.AsInt32(), aaShuffle), crossoverStackA.Memory + crossoverStackCount); - Vector256.Store(Avx2.PermuteVar8x32(n1AIndex.AsInt32(), aaShuffle), crossoverStackB.Memory + crossoverStackCount); - crossoverStackCount += aaCount; - Vector256.Store(Avx2.PermuteVar8x32(n0AIndex.AsInt32(), abShuffle), crossoverStackA.Memory + crossoverStackCount); - Vector256.Store(Avx2.PermuteVar8x32(n1BIndex.AsInt32(), abShuffle), crossoverStackB.Memory + crossoverStackCount); - crossoverStackCount += abCount; - Vector256.Store(Avx2.PermuteVar8x32(n0BIndex.AsInt32(), baShuffle), crossoverStackA.Memory + crossoverStackCount); - Vector256.Store(Avx2.PermuteVar8x32(n1AIndex.AsInt32(), baShuffle), crossoverStackB.Memory + crossoverStackCount); - crossoverStackCount += baCount; - Vector256.Store(Avx2.PermuteVar8x32(n0BIndex.AsInt32(), bbShuffle), crossoverStackA.Memory + crossoverStackCount); - Vector256.Store(Avx2.PermuteVar8x32(n1BIndex.AsInt32(), bbShuffle), crossoverStackB.Memory + crossoverStackCount); - crossoverStackCount += bbCount; - - //Console.WriteLine($"crossover count for iteration: {aaCount + abCount + baCount + bbCount}, new stack count {crossoverStackCount}"); - } - } - } + var toVisitIndex = --crossoverStackCount; + var index0 = crossoverStackA[toVisitIndex]; + var index1 = crossoverStackB[toVisitIndex]; + ref var n0 = ref Nodes[index0]; + ref var n1 = ref Nodes[index1]; + + var aaIntersects = BoundingBox.IntersectsUnsafe(n0.A, n1.A); + var abIntersects = BoundingBox.IntersectsUnsafe(n0.A, n1.B); + var baIntersects = BoundingBox.IntersectsUnsafe(n0.B, n1.A); + var bbIntersects = BoundingBox.IntersectsUnsafe(n0.B, n1.B); + + var n0AIndex = n0.A.Index; + var n0BIndex = n0.B.Index; + var n1AIndex = n1.A.Index; + var n1BIndex = n1.B.Index; + var n0AInternal = n0AIndex >= 0; + var n0BInternal = n0BIndex >= 0; + var n1AInternal = n1AIndex >= 0; + var n1BInternal = n1BIndex >= 0; + + var aaLeafLeaf = aaIntersects & !(n0AInternal | n1AInternal); + var abLeafLeaf = abIntersects & !(n0AInternal | n1BInternal); + var baLeafLeaf = baIntersects & !(n0BInternal | n1AInternal); + var bbLeafLeaf = bbIntersects & !(n0BInternal | n1BInternal); + + var aaNodeLeaf = aaIntersects & (n0AInternal ^ n1AInternal); + var abNodeLeaf = abIntersects & (n0AInternal ^ n1BInternal); + var baNodeLeaf = baIntersects & (n0BInternal ^ n1AInternal); + var bbNodeLeaf = bbIntersects & (n0BInternal ^ n1BInternal); + + var aaPushCrossover = aaIntersects & n0AInternal & n1AInternal; + var abPushCrossover = abIntersects & n0AInternal & n1BInternal; + var baPushCrossover = baIntersects & n0BInternal & n1AInternal; + var bbPushCrossover = bbIntersects & n0BInternal & n1BInternal; + + if (aaLeafLeaf) results.Handle(Encode(n0AIndex), Encode(n1AIndex)); + if (abLeafLeaf) results.Handle(Encode(n0AIndex), Encode(n1BIndex)); + if (baLeafLeaf) results.Handle(Encode(n0BIndex), Encode(n1AIndex)); + if (bbLeafLeaf) results.Handle(Encode(n0BIndex), Encode(n1BIndex)); + + if (aaNodeLeaf) { nodeLeafStackA[nodeLeafStackCount] = (uint)index0; nodeLeafStackB[nodeLeafStackCount] = (uint)index1; ++nodeLeafStackCount; } + if (abNodeLeaf) { nodeLeafStackA[nodeLeafStackCount] = (uint)index0; nodeLeafStackB[nodeLeafStackCount] = (uint)index1 | (1u << 31); ++nodeLeafStackCount; } + if (baNodeLeaf) { nodeLeafStackA[nodeLeafStackCount] = (uint)index0 | (1u << 31); nodeLeafStackB[nodeLeafStackCount] = (uint)index1; ++nodeLeafStackCount; } + if (bbNodeLeaf) { nodeLeafStackA[nodeLeafStackCount] = (uint)index0 | (1u << 31); nodeLeafStackB[nodeLeafStackCount] = (uint)index1 | (1u << 31); ++nodeLeafStackCount; } + + if (aaPushCrossover) { crossoverStackA[crossoverStackCount] = n0AIndex; crossoverStackB[crossoverStackCount] = n1AIndex; ++crossoverStackCount; } + if (abPushCrossover) { crossoverStackA[crossoverStackCount] = n0AIndex; crossoverStackB[crossoverStackCount] = n1BIndex; ++crossoverStackCount; } + if (baPushCrossover) { crossoverStackA[crossoverStackCount] = n0BIndex; crossoverStackB[crossoverStackCount] = n1AIndex; ++crossoverStackCount; } + if (bbPushCrossover) { crossoverStackA[crossoverStackCount] = n0BIndex; crossoverStackB[crossoverStackCount] = n1BIndex; ++crossoverStackCount; } + + //var leafleafCount = (aaLeafLeaf ? 1 : 0) + (abLeafLeaf ? 1 : 0) + (baLeafLeaf ? 1 : 0) + (bbLeafLeaf ? 1 : 0); + //var nodeLeafCount = (aaNodeLeaf ? 1 : 0) + (abNodeLeaf ? 1 : 0) + (baNodeLeaf ? 1 : 0) + (bbNodeLeaf ? 1 : 0); + //var crossoverCount = (aaPushCrossover ? 1 : 0) + (abPushCrossover ? 1 : 0) + (baPushCrossover ? 1 : 0) + (bbPushCrossover ? 1 : 0); + //Console.WriteLine($"new stackcounts: nodeleaf {nodeLeafStackCount}, crossover {crossoverStackCount}; new leafleaf {leafleafCount}, nodeleaf {nodeLeafCount}, crossover {crossoverCount}"); } //Console.WriteLine("End crossovers"); + pool.Return(ref crossoverStackA); pool.Return(ref crossoverStackB); QuickList stack = new(NodeCount, pool); diff --git a/BepuUtilities/BoundingBox.cs b/BepuUtilities/BoundingBox.cs index 53a9744fd..701870b80 100644 --- a/BepuUtilities/BoundingBox.cs +++ b/BepuUtilities/BoundingBox.cs @@ -46,11 +46,11 @@ public BoundingBox(Vector3 min, Vector3 max) /// Checks if two structures with memory layouts equivalent to the intersect. /// The referenced values must not be in unpinned managed memory. /// - /// First child to compare. - /// Second child to compare. + /// First bounding box to compare. + /// Second bounding box to compare. /// True if the children overlap, false otherwise. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe static bool IntersectsUnsafe(in TA childA, in TB childB) where TA : unmanaged where TB : unmanaged + public unsafe static bool IntersectsUnsafe(in TA boundingBoxA, in TB boundingBoxB) where TA : unmanaged where TB : unmanaged { //This is a weird function. We're directly interpreting the memory of an incoming type as a vector where we assume the min/max layout matches the BoundingBox. //Happens to be convenient! @@ -69,8 +69,8 @@ public unsafe static bool IntersectsUnsafe(in TA childA, in TB childB) w if (Vector128.IsHardwareAccelerated) { //THIS IS A POTENTIAL GC HOLE IF CHILDREN ARE PASSED FROM UNPINNED MANAGED MEMORY - ref var a = ref Unsafe.As(ref Unsafe.AsRef(childA)); - ref var b = ref Unsafe.As(ref Unsafe.AsRef(childB)); + ref var a = ref Unsafe.As(ref Unsafe.AsRef(boundingBoxA)); + ref var b = ref Unsafe.As(ref Unsafe.AsRef(boundingBoxB)); var aMin = Vector128.LoadUnsafe(ref a); var aMax = Vector128.LoadUnsafe(ref Unsafe.Add(ref a, 4)); var bMin = Vector128.LoadUnsafe(ref b); @@ -80,8 +80,8 @@ public unsafe static bool IntersectsUnsafe(in TA childA, in TB childB) w } else { - var a = (float*)Unsafe.AsPointer(ref Unsafe.AsRef(childA)); - var b = (float*)Unsafe.AsPointer(ref Unsafe.AsRef(childB)); + var a = (float*)Unsafe.AsPointer(ref Unsafe.AsRef(boundingBoxA)); + var b = (float*)Unsafe.AsPointer(ref Unsafe.AsRef(boundingBoxB)); return a[4] >= b[0] & a[5] >= b[1] & a[6] >= b[2] & b[4] >= a[0] & b[5] >= a[1] & b[6] >= a[2]; } From 9402d3adb36562008a62f067e6ebf5924e297170 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 10 Sep 2022 18:00:42 -0500 Subject: [PATCH 595/947] Want another low-quality vectorized implementation? I've got you covered. --- BepuPhysics/Trees/Tree_SelfQueries.cs | 142 ++++++++++++++++++-------- 1 file changed, 100 insertions(+), 42 deletions(-) diff --git a/BepuPhysics/Trees/Tree_SelfQueries.cs b/BepuPhysics/Trees/Tree_SelfQueries.cs index 7584206a8..a895951e3 100644 --- a/BepuPhysics/Trees/Tree_SelfQueries.cs +++ b/BepuPhysics/Trees/Tree_SelfQueries.cs @@ -227,6 +227,11 @@ static uint EncodeParentIndex(int leafParentIndex, bool childIsB) return encoded; } [MethodImpl(MethodImplOptions.AggressiveInlining)] + static Vector128 Encode(Vector128 indices) + { + return Vector128.AllBitsSet - indices; + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] static ref NodeChild GetLeafChild(ref Tree tree, uint encodedLeafParentIndex) { var parentNodeIndex = encodedLeafParentIndex & 0x7FFF_FFFF; @@ -235,6 +240,64 @@ static ref NodeChild GetLeafChild(ref Tree tree, uint encodedLeafParentIndex) return ref leafIsChildB ? ref parent.B : ref parent.A; } + //[MethodImpl(MethodImplOptions.AggressiveInlining)] + //static void LeftPack(Vector128 mask, Vector128 a, Vector128 b, out Vector128 packedA, out Vector128 packedB, out int count) + //{ + // var bitmask = Vector128.ExtractMostSignificantBits(mask); + // count = BitOperations.PopCount(bitmask); + // switch (bitmask) + // { + // // 0000 requires no shuffle. + // // 0001 requires no shuffle. + // case 0b0010: packedA = Vector128.Shuffle(a, Vector128.Create(1, 0, 0, 0)); packedB = Vector128.Shuffle(b, Vector128.Create(1, 0, 0, 0)); break; + // // 0011 requires no shuffle. + // case 0b0100: packedA = Vector128.Shuffle(a, Vector128.Create(2, 0, 0, 0)); packedB = Vector128.Shuffle(b, Vector128.Create(2, 0, 0, 0)); break; + // case 0b0101: packedA = Vector128.Shuffle(a, Vector128.Create(0, 2, 0, 0)); packedB = Vector128.Shuffle(b, Vector128.Create(0, 2, 0, 0)); break; + // case 0b0110: packedA = Vector128.Shuffle(a, Vector128.Create(1, 2, 0, 0)); packedB = Vector128.Shuffle(b, Vector128.Create(1, 2, 0, 0)); break; + // // 0111 requires no shuffle. + // case 0b1000: packedA = Vector128.Shuffle(a, Vector128.Create(3, 0, 0, 0)); packedB = Vector128.Shuffle(b, Vector128.Create(3, 0, 0, 0)); break; + // case 0b1001: packedA = Vector128.Shuffle(a, Vector128.Create(0, 3, 0, 0)); packedB = Vector128.Shuffle(b, Vector128.Create(0, 3, 0, 0)); break; + // case 0b1010: packedA = Vector128.Shuffle(a, Vector128.Create(1, 3, 0, 0)); packedB = Vector128.Shuffle(b, Vector128.Create(1, 3, 0, 0)); break; + // case 0b1011: packedA = Vector128.Shuffle(a, Vector128.Create(0, 1, 3, 0)); packedB = Vector128.Shuffle(b, Vector128.Create(0, 1, 3, 0)); break; + // case 0b1100: packedA = Vector128.Shuffle(a, Vector128.Create(2, 3, 0, 0)); packedB = Vector128.Shuffle(b, Vector128.Create(2, 3, 0, 0)); break; + // case 0b1101: packedA = Vector128.Shuffle(a, Vector128.Create(0, 2, 3, 0)); packedB = Vector128.Shuffle(b, Vector128.Create(0, 2, 3, 0)); break; + // case 0b1110: packedA = Vector128.Shuffle(a, Vector128.Create(1, 2, 3, 0)); packedB = Vector128.Shuffle(b, Vector128.Create(1, 2, 3, 0)); break; + // // 1111 requires no shuffle. + // default: packedA = a; packedB = b; break; + // } + + //} + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector128 GetLeftPackMask(Vector128 mask, out int count) + { + if (!Avx2.IsSupported) throw new NotSupportedException("No fallback exists! This should never be visible!"); + + var bitmask = Vector128.ExtractMostSignificantBits(mask); + //This depends upon an optimization that preallocates the constant array in fixed memory. Bit of a turbohack that won't work on other runtimes. + //The lookup table includes one entry for each of the 256 possible bitmasks. Each lane requires 3 bits to define the source for a shuffle mask. + //3 bits, 8 lanes, 256 bitmasks means only 768 bytes. + ReadOnlySpan lookupTable = new byte[] { + //0000 0001 0010 0011 0100 0101 0110 0111 + 0b1110_0100, 0b1110_0100, 0b1110_0101, 0b1110_0100, 0b1110_0110, 0b1110_1000, 0b1110_1001, 0b1110_0100, + //1000 1001 1010 1011 1100 1101 1110 1111 + 0b1110_0111, 0b1110_1100, 0b1110_1101, 0b1111_0100, 0b1110_1110, 0b1111_1000, 0b1111_1001, 0b1110_0100 }; + var encodedLeftPackMask = Unsafe.Add(ref Unsafe.AsRef(lookupTable[0]), bitmask); + + count = BitOperations.PopCount(bitmask); + //Broadcast, variable shift. + return Avx2.ShiftRightLogicalVariable(Vector128.Create((int)encodedLeftPackMask), Vector128.Create(0u, 2, 4, 6)); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + static void LeftPack(Vector128 mask, Vector128 a, Vector128 b, out Vector128 packedA, out Vector128 packedB, out int count) + { + var permuteMask = GetLeftPackMask(mask, out count); + packedA = Avx.PermuteVar(a.AsSingle(), permuteMask).AsInt32(); + packedB = Avx.PermuteVar(b.AsSingle(), permuteMask).AsInt32(); + + } + public unsafe void GetSelfOverlapsContiguousPrepass(ref TOverlapHandler results, BufferPool pool) where TOverlapHandler : IOverlapHandler { //If there are less than two leaves, there can't be any overlap. @@ -296,48 +359,43 @@ public unsafe void GetSelfOverlapsContiguousPrepass(ref TOverla var baIntersects = BoundingBox.IntersectsUnsafe(n0.B, n1.A); var bbIntersects = BoundingBox.IntersectsUnsafe(n0.B, n1.B); - var n0AIndex = n0.A.Index; - var n0BIndex = n0.B.Index; - var n1AIndex = n1.A.Index; - var n1BIndex = n1.B.Index; - var n0AInternal = n0AIndex >= 0; - var n0BInternal = n0BIndex >= 0; - var n1AInternal = n1AIndex >= 0; - var n1BInternal = n1BIndex >= 0; - - var aaLeafLeaf = aaIntersects & !(n0AInternal | n1AInternal); - var abLeafLeaf = abIntersects & !(n0AInternal | n1BInternal); - var baLeafLeaf = baIntersects & !(n0BInternal | n1AInternal); - var bbLeafLeaf = bbIntersects & !(n0BInternal | n1BInternal); - - var aaNodeLeaf = aaIntersects & (n0AInternal ^ n1AInternal); - var abNodeLeaf = abIntersects & (n0AInternal ^ n1BInternal); - var baNodeLeaf = baIntersects & (n0BInternal ^ n1AInternal); - var bbNodeLeaf = bbIntersects & (n0BInternal ^ n1BInternal); - - var aaPushCrossover = aaIntersects & n0AInternal & n1AInternal; - var abPushCrossover = abIntersects & n0AInternal & n1BInternal; - var baPushCrossover = baIntersects & n0BInternal & n1AInternal; - var bbPushCrossover = bbIntersects & n0BInternal & n1BInternal; - - if (aaLeafLeaf) results.Handle(Encode(n0AIndex), Encode(n1AIndex)); - if (abLeafLeaf) results.Handle(Encode(n0AIndex), Encode(n1BIndex)); - if (baLeafLeaf) results.Handle(Encode(n0BIndex), Encode(n1AIndex)); - if (bbLeafLeaf) results.Handle(Encode(n0BIndex), Encode(n1BIndex)); - - if (aaNodeLeaf) { nodeLeafStackA[nodeLeafStackCount] = (uint)index0; nodeLeafStackB[nodeLeafStackCount] = (uint)index1; ++nodeLeafStackCount; } - if (abNodeLeaf) { nodeLeafStackA[nodeLeafStackCount] = (uint)index0; nodeLeafStackB[nodeLeafStackCount] = (uint)index1 | (1u << 31); ++nodeLeafStackCount; } - if (baNodeLeaf) { nodeLeafStackA[nodeLeafStackCount] = (uint)index0 | (1u << 31); nodeLeafStackB[nodeLeafStackCount] = (uint)index1; ++nodeLeafStackCount; } - if (bbNodeLeaf) { nodeLeafStackA[nodeLeafStackCount] = (uint)index0 | (1u << 31); nodeLeafStackB[nodeLeafStackCount] = (uint)index1 | (1u << 31); ++nodeLeafStackCount; } - - if (aaPushCrossover) { crossoverStackA[crossoverStackCount] = n0AIndex; crossoverStackB[crossoverStackCount] = n1AIndex; ++crossoverStackCount; } - if (abPushCrossover) { crossoverStackA[crossoverStackCount] = n0AIndex; crossoverStackB[crossoverStackCount] = n1BIndex; ++crossoverStackCount; } - if (baPushCrossover) { crossoverStackA[crossoverStackCount] = n0BIndex; crossoverStackB[crossoverStackCount] = n1AIndex; ++crossoverStackCount; } - if (bbPushCrossover) { crossoverStackA[crossoverStackCount] = n0BIndex; crossoverStackB[crossoverStackCount] = n1BIndex; ++crossoverStackCount; } - - //var leafleafCount = (aaLeafLeaf ? 1 : 0) + (abLeafLeaf ? 1 : 0) + (baLeafLeaf ? 1 : 0) + (bbLeafLeaf ? 1 : 0); - //var nodeLeafCount = (aaNodeLeaf ? 1 : 0) + (abNodeLeaf ? 1 : 0) + (baNodeLeaf ? 1 : 0) + (bbNodeLeaf ? 1 : 0); - //var crossoverCount = (aaPushCrossover ? 1 : 0) + (abPushCrossover ? 1 : 0) + (baPushCrossover ? 1 : 0) + (bbPushCrossover ? 1 : 0); + var intersects = Vector128.Create(aaIntersects ? -1 : 0, abIntersects ? -1 : 0, baIntersects ? -1 : 0, bbIntersects ? -1 : 0); + var indices = Vector128.Create(n0.A.Index, n0.B.Index, n1.A.Index, n1.B.Index); + var n0Indices = Vector128.Shuffle(indices, Vector128.Create(0, 0, 1, 1)); + var n1Indices = Vector128.Shuffle(indices, Vector128.Create(2, 3, 2, 3)); + var n0Internal = Vector128.GreaterThan(n0Indices, Vector128.Zero); + var n1Internal = Vector128.GreaterThan(n1Indices, Vector128.Zero); + + var leafLeaf = Vector128.AndNot(Vector128.AndNot(intersects, n0Internal), n1Internal); + var nodeLeaf = intersects & (n0Internal ^ n1Internal); + var crossover = intersects & n0Internal & n1Internal; + + LeftPack(leafLeaf, Encode(n0Indices), Encode(n1Indices), out var leafleafToPush0, out var leafleafToPush1, out var leafLeafCount); + var parentEncoded0 = Vector128.BitwiseOr(Vector128.Create(index0), Vector128.Create(0, 0, 1 << 31, 1 << 31)); + var parentEncoded1 = Vector128.BitwiseOr(Vector128.Create(index1), Vector128.Create(0, 1 << 31, 0, 1 << 31)); + LeftPack(nodeLeaf, parentEncoded0, parentEncoded1, out var nodeLeafToPush0, out var nodeLeafToPush1, out var nodeLeafCount); + LeftPack(crossover, n0Indices, n1Indices, out var crossoverToPush0, out var crossoverToPush1, out var crossoverCount); + + if (leafLeafCount > 0) + { + results.Handle(leafleafToPush0[0], leafleafToPush1[0]); + if (leafLeafCount > 1) results.Handle(leafleafToPush0[1], leafleafToPush1[1]); + if (leafLeafCount > 2) results.Handle(leafleafToPush0[2], leafleafToPush1[2]); + if (leafLeafCount > 3) results.Handle(leafleafToPush0[3], leafleafToPush1[3]); + } + if (nodeLeafCount > 0) + { + Vector128.Store(nodeLeafToPush0.AsUInt32(), nodeLeafStackA.Memory + nodeLeafStackCount); + Vector128.Store(nodeLeafToPush1.AsUInt32(), nodeLeafStackB.Memory + nodeLeafStackCount); + nodeLeafStackCount += nodeLeafCount; + } + if (crossoverCount > 0) + { + Vector128.Store(crossoverToPush0, crossoverStackA.Memory + crossoverStackCount); + Vector128.Store(crossoverToPush1, crossoverStackB.Memory + crossoverStackCount); + crossoverStackCount += crossoverCount; + } + //Console.WriteLine($"new stackcounts: nodeleaf {nodeLeafStackCount}, crossover {crossoverStackCount}; new leafleaf {leafleafCount}, nodeleaf {nodeLeafCount}, crossover {crossoverCount}"); } //Console.WriteLine("End crossovers"); From 49d5f8780a193300c150aa3c2e0d71f44188ddfe Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 10 Sep 2022 19:15:25 -0500 Subject: [PATCH 596/947] Slight simplification. --- BepuPhysics/Trees/Tree_SelfQueries.cs | 44 ++++++++------------------- 1 file changed, 13 insertions(+), 31 deletions(-) diff --git a/BepuPhysics/Trees/Tree_SelfQueries.cs b/BepuPhysics/Trees/Tree_SelfQueries.cs index a895951e3..dc163831f 100644 --- a/BepuPhysics/Trees/Tree_SelfQueries.cs +++ b/BepuPhysics/Trees/Tree_SelfQueries.cs @@ -410,50 +410,32 @@ public unsafe void GetSelfOverlapsContiguousPrepass(ref TOverla ref var childB = ref GetLeafChild(ref this, nodeLeafStackB[i]); Debug.Assert((childA.Index < 0) ^ (childB.Index < 0), "One and only one of the two children must be a leaf."); ref var leafChild = ref childA.Index < 0 ? ref childA : ref childB; - var nodeToTest = childA.Index < 0 ? childB.Index : childA.Index; + stack.AllocateUnsafely() = childA.Index < 0 ? childB.Index : childA.Index; var leafIndex = Encode(leafChild.Index); Debug.Assert(stack.Count == 0); - while (true) + while (stack.TryPop(out var nodeToTest)) { - //Now process all the leaf nodes! ref var node = ref Nodes[nodeToTest]; var a = node.A.Index; var b = node.B.Index; var aIntersected = BoundingBox.IntersectsUnsafe(leafChild, node.A); var bIntersected = BoundingBox.IntersectsUnsafe(leafChild, node.B); - var aIsInternal = a >= 0; - var bIsInternal = b >= 0; - var intersectedInternalA = aIntersected && aIsInternal; - var intersectedInternalB = bIntersected && bIsInternal; - if (intersectedInternalA && intersectedInternalB) + + if (bIntersected) { - nodeToTest = a; - stack.AllocateUnsafely() = b; + if (b >= 0) + stack.AllocateUnsafely() = b; + else + results.Handle(leafIndex, Encode(b)); } - else + if (aIntersected) { - if (aIntersected && !aIsInternal) - { - results.Handle(leafIndex, Encode(a)); - } - if (bIntersected && !bIsInternal) - { - results.Handle(leafIndex, Encode(b)); - } - if (intersectedInternalA || intersectedInternalB) - { - nodeToTest = intersectedInternalA ? a : b; - } + if (a >= 0) + stack.AllocateUnsafely() = a; else - { - //The current traversal step doesn't offer a next step; pull from the stack. - if (!stack.TryPop(out nodeToTest)) - { - //Nothing left to test against this leaf! Done! - break; - } - } + results.Handle(leafIndex, Encode(a)); } + } } pool.Return(ref nodeLeafStackA); From 5f2db845d212245ce39030e090e01c1c41c503da Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 10 Sep 2022 23:11:29 -0500 Subject: [PATCH 597/947] Some alternatives refits. Pretty low value. --- BepuPhysics/Trees/Tree_Refit.cs | 75 ++++++++++++++++++- BepuUtilities/BoundingBox.cs | 71 ++++++++++++++++++ .../SpecializedTests/TreeFiddlingTestDemo.cs | 32 +++++++- 3 files changed, 174 insertions(+), 4 deletions(-) diff --git a/BepuPhysics/Trees/Tree_Refit.cs b/BepuPhysics/Trees/Tree_Refit.cs index fe218a37b..3b067fb6c 100644 --- a/BepuPhysics/Trees/Tree_Refit.cs +++ b/BepuPhysics/Trees/Tree_Refit.cs @@ -3,6 +3,8 @@ using System.Linq; using System.Numerics; using System.Runtime.CompilerServices; +using System.Runtime.Intrinsics; +using System.Runtime.Intrinsics.X86; namespace BepuPhysics.Trees { @@ -47,7 +49,6 @@ readonly unsafe void Refit(int nodeIndex, out Vector3 min, out Vector3 max) } BoundingBox.CreateMerged(a.Min, a.Max, b.Min, b.Max, out min, out max); } - /// /// Updates the bounding boxes of all internal nodes in the tree. /// @@ -59,6 +60,78 @@ public unsafe readonly void Refit() Refit(0, out var rootMin, out var rootMax); } + readonly unsafe void Refit2(ref NodeChild childInParent) + { + Debug.Assert(LeafCount >= 2); + ref var node = ref Nodes[childInParent.Index]; + ref var a = ref node.A; + if (node.A.Index >= 0) + { + Refit2(ref a); + } + ref var b = ref node.B; + if (b.Index >= 0) + { + Refit2(ref b); + } + BoundingBox.CreateMergedUnsafeWithPreservation(a, b, out childInParent); + } + /// + /// Updates the bounding boxes of all internal nodes in the tree. + /// + public unsafe readonly void Refit2() + { + //No point in refitting a tree with no internal nodes! + if (LeafCount <= 2) + return; + NodeChild stub = default; + Refit2(ref stub); + } + + readonly unsafe void Refit3(ref NodeChild childInParent, out Vector4 min, out Vector4 max) + { + Debug.Assert(LeafCount >= 2); + ref var node = ref Nodes[childInParent.Index]; + ref var a = ref node.A; + Vector4 aMin, aMax; + if (node.A.Index >= 0) + { + Refit3(ref a, out aMin, out aMax); + } + else + { + aMin = Unsafe.As(ref a.Min); + aMax = Unsafe.As(ref a.Max); + } + ref var b = ref node.B; + Vector4 bMin, bMax; + if (b.Index >= 0) + { + Refit3(ref b, out bMin, out bMax); + } + else + { + bMin = Unsafe.As(ref b.Min); + bMax = Unsafe.As(ref b.Max); + } + min = Vector4.Max(aMin, bMin); + max = Vector4.Max(aMax, bMax); + childInParent.Min.AsVector128(); + childInParent.Max.AsVector128(); + } + + /// + /// Updates the bounding boxes of all internal nodes in the tree. + /// + public unsafe readonly void Refit3() + { + //No point in refitting a tree with no internal nodes! + if (LeafCount <= 2) + return; + NodeChild stub = default; + Refit3(ref stub, out var min, out var max); + } + } diff --git a/BepuUtilities/BoundingBox.cs b/BepuUtilities/BoundingBox.cs index 701870b80..b1f2af1e2 100644 --- a/BepuUtilities/BoundingBox.cs +++ b/BepuUtilities/BoundingBox.cs @@ -162,6 +162,77 @@ public static void CreateMerged(in BoundingBox a, in BoundingBox b, out Bounding CreateMerged(a.Min, a.Max, b.Min, b.Max, out merged.Min, out merged.Max); } + /// + /// Merges two structures with memory layouts equivalent to the . + /// The referenced values must not be in unpinned managed memory. + /// Any data in the empty slots is preserved. + /// + /// First bounding box to compare. + /// Second bounding box to compare. + /// Merged bounding box. + /// Type of the first bounding box-like parameter. + /// Type of the second bounding box-like parameter. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public unsafe static void CreateMergedUnsafeWithPreservation(in TA boundingBoxA, in TB boundingBoxB, out TA merged) where TA : unmanaged where TB : unmanaged + { + if (Sse41.IsSupported) + { + Unsafe.SkipInit(out merged); + ref var resultMin = ref Unsafe.As>(ref merged); + ref var resultMax = ref Unsafe.Add(ref Unsafe.As>(ref merged), 1); + resultMin = Sse41.Blend(Vector128.Min( + Unsafe.As>(ref Unsafe.AsRef(boundingBoxA)), + Unsafe.As>(ref Unsafe.AsRef(boundingBoxB))), resultMin, 0b111111); + resultMax = Sse41.Blend(Vector128.Max( + Unsafe.Add(ref Unsafe.As>(ref Unsafe.AsRef(boundingBoxA)), 1), + Unsafe.Add(ref Unsafe.As>(ref Unsafe.AsRef(boundingBoxB)), 1)), resultMax, 0b111111); + } + else + { + ref var a = ref Unsafe.As(ref Unsafe.AsRef(boundingBoxA)); + ref var b = ref Unsafe.As(ref Unsafe.AsRef(boundingBoxB)); + Unsafe.SkipInit(out merged); + ref var result = ref Unsafe.As(ref Unsafe.AsRef(merged)); + result.Min = Vector3.Min(a.Min, b.Min); + result.Max = Vector3.Max(a.Max, b.Max); + } + } + /// + /// Merges two structures with memory layouts equivalent to the . + /// The referenced values must not be in unpinned managed memory. + /// Any data in the empty slots is not preserved. + /// + /// First bounding box to compare. + /// Second bounding box to compare. + /// Merged bounding box. + /// Type of the first bounding box-like parameter. + /// Type of the second bounding box-like parameter. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public unsafe static void CreateMergedUnsafe(in TA boundingBoxA, in TB boundingBoxB, out TA merged) where TA : unmanaged where TB : unmanaged + { + if (Vector128.IsHardwareAccelerated) + { + Unsafe.SkipInit(out merged); + ref var resultMin = ref Unsafe.As>(ref merged); + ref var resultMax = ref Unsafe.Add(ref Unsafe.As>(ref merged), 1); + resultMin = Vector128.Min( + Unsafe.As>(ref Unsafe.AsRef(boundingBoxA)), + Unsafe.As>(ref Unsafe.AsRef(boundingBoxB))); + resultMax = Vector128.Max( + Unsafe.Add(ref Unsafe.As>(ref Unsafe.AsRef(boundingBoxA)), 1), + Unsafe.Add(ref Unsafe.As>(ref Unsafe.AsRef(boundingBoxB)), 1)); + } + else + { + ref var a = ref Unsafe.As(ref Unsafe.AsRef(boundingBoxA)); + ref var b = ref Unsafe.As(ref Unsafe.AsRef(boundingBoxB)); + Unsafe.SkipInit(out merged); + ref var result = ref Unsafe.As(ref Unsafe.AsRef(merged)); + result.Min = Vector3.Min(a.Min, b.Min); + result.Max = Vector3.Max(a.Max, b.Max); + } + } + /// /// Determines if a bounding box intersects a bounding sphere. /// diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index 0dae87675..7f62a8880 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -81,13 +81,18 @@ public override void Initialize(ContentArchive content, Camera camera) DemoMeshHelper.CreateDeformedPlane(width, height, (x, y) => new Vector3(x - width * scale.X * 0.5f, 2f * (float)(Math.Sin(x * 0.5f) * Math.Sin(y * 0.5f)), y - height * scale.Y * 0.5f), scale, BufferPool, out var mesh); Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); - Test((ref OverlapHandler handler) => mesh.Tree.GetSelfOverlapsContiguousPrepass(ref handler, BufferPool), mesh.Tree.LeafCount, "Prepass"); - Test((ref OverlapHandler handler) => mesh.Tree.GetSelfOverlaps(ref handler), mesh.Tree.LeafCount, "Original"); + Console.WriteLine($"node count: {mesh.Tree.NodeCount}"); + RefitTest(() => mesh.Tree.Refit3(), "refit3", ref mesh.Tree); + RefitTest(() => mesh.Tree.Refit2(), "refit2", ref mesh.Tree); + RefitTest(() => mesh.Tree.Refit(), "Original", ref mesh.Tree); + + //SelfTest((ref OverlapHandler handler) => mesh.Tree.GetSelfOverlapsContiguousPrepass(ref handler, BufferPool), mesh.Tree.LeafCount, "Prepass"); + //SelfTest((ref OverlapHandler handler) => mesh.Tree.GetSelfOverlaps(ref handler), mesh.Tree.LeafCount, "Original"); } delegate void TestFunction(ref OverlapHandler handler); - static void Test(TestFunction function, int leafCount, string name) + static void SelfTest(TestFunction function, int leafCount, string name) { var overlapHandler = new OverlapHandler(); overlapHandler.TreeLeafCount = leafCount; @@ -106,5 +111,26 @@ static void Test(TestFunction function, int leafCount, string name) Console.WriteLine($"{name} time per execution (ms): {(accumulatedTime) * 1e3 / (testCount * Stopwatch.Frequency)}"); Console.WriteLine($"{name} count: {overlapHandler.OverlapCount}, sum {overlapHandler.OverlapSum}, hash {overlapHandler.OverlapHash}"); } + + + static void RefitTest(Action function, string name, ref Tree tree) + { + long accumulatedTime = 0; + const int testCount = 16; + for (int i = 0; i < testCount; ++i) + { + var startTime = Stopwatch.GetTimestamp(); + function(); + var endTime = Stopwatch.GetTimestamp(); + accumulatedTime += endTime - startTime; + //overlapHandler.Set.Clear(); + CacheBlaster.Blast(); + } + Console.WriteLine($"{name} time per execution (ms): {(accumulatedTime) * 1e3 / (testCount * Stopwatch.Frequency)}"); + + var sum = tree.Nodes[0].A.Min * 5 + tree.Nodes[0].A.Max * 7 + tree.Nodes[0].B.Min * 13 + tree.Nodes[0].B.Max * 17; + var hash = Unsafe.As(ref sum.X) * 31 + Unsafe.As(ref sum.Y) * 37 + Unsafe.As(ref sum.Z) * 41; + Console.WriteLine($"{name} bounds 0 hash: {hash}, A ({tree.Nodes[0].A.Min}, {tree.Nodes[0].B.Max}), B ({tree.Nodes[0].B.Min}, {tree.Nodes[0].B.Max})"); + } } } From 67406a06e550d25a46c45451efb326010fcb168b Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 14 Sep 2022 14:21:41 -0500 Subject: [PATCH 598/947] Plebt. --- BepuPhysics/Trees/Tree_Refit.cs | 46 --------------------------------- 1 file changed, 46 deletions(-) diff --git a/BepuPhysics/Trees/Tree_Refit.cs b/BepuPhysics/Trees/Tree_Refit.cs index 3b067fb6c..a402b441b 100644 --- a/BepuPhysics/Trees/Tree_Refit.cs +++ b/BepuPhysics/Trees/Tree_Refit.cs @@ -88,51 +88,5 @@ public unsafe readonly void Refit2() Refit2(ref stub); } - readonly unsafe void Refit3(ref NodeChild childInParent, out Vector4 min, out Vector4 max) - { - Debug.Assert(LeafCount >= 2); - ref var node = ref Nodes[childInParent.Index]; - ref var a = ref node.A; - Vector4 aMin, aMax; - if (node.A.Index >= 0) - { - Refit3(ref a, out aMin, out aMax); - } - else - { - aMin = Unsafe.As(ref a.Min); - aMax = Unsafe.As(ref a.Max); - } - ref var b = ref node.B; - Vector4 bMin, bMax; - if (b.Index >= 0) - { - Refit3(ref b, out bMin, out bMax); - } - else - { - bMin = Unsafe.As(ref b.Min); - bMax = Unsafe.As(ref b.Max); - } - min = Vector4.Max(aMin, bMin); - max = Vector4.Max(aMax, bMax); - childInParent.Min.AsVector128(); - childInParent.Max.AsVector128(); - } - - /// - /// Updates the bounding boxes of all internal nodes in the tree. - /// - public unsafe readonly void Refit3() - { - //No point in refitting a tree with no internal nodes! - if (LeafCount <= 2) - return; - NodeChild stub = default; - Refit3(ref stub, out var min, out var max); - } - - - } } From f0a2a1e0aa9eb0141f05629e2dbcbb38e8652517 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 14 Sep 2022 18:59:22 -0500 Subject: [PATCH 599/947] Preppin binned benches. --- .../SpecializedTests/TreeFiddlingTestDemo.cs | 38 ++++++++++++++++++- 1 file changed, 37 insertions(+), 1 deletion(-) diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index 7f62a8880..555bc8d1b 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -82,7 +82,17 @@ public override void Initialize(ContentArchive content, Camera camera) Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); Console.WriteLine($"node count: {mesh.Tree.NodeCount}"); - RefitTest(() => mesh.Tree.Refit3(), "refit3", ref mesh.Tree); + + QuickList subtreeReferences = new QuickList(mesh.Tree.LeafCount, BufferPool); + QuickList treeletInternalNodes = new QuickList(mesh.Tree.LeafCount, BufferPool); + Tree.CreateBinnedResources(BufferPool, mesh.Tree.LeafCount, out var binnedResourcesBuffer, out var binnedResources); + BinnedTest(() => + { + subtreeReferences.Count = 0; + treeletInternalNodes.Count = 0; + mesh.Tree.BinnedRefine(0, ref subtreeReferences, mesh.Tree.LeafCount, ref treeletInternalNodes, ref binnedResources, BufferPool); + }, "Original", ref mesh.Tree); + RefitTest(() => mesh.Tree.Refit2(), "refit2", ref mesh.Tree); RefitTest(() => mesh.Tree.Refit(), "Original", ref mesh.Tree); @@ -132,5 +142,31 @@ static void RefitTest(Action function, string name, ref Tree tree) var hash = Unsafe.As(ref sum.X) * 31 + Unsafe.As(ref sum.Y) * 37 + Unsafe.As(ref sum.Z) * 41; Console.WriteLine($"{name} bounds 0 hash: {hash}, A ({tree.Nodes[0].A.Min}, {tree.Nodes[0].B.Max}), B ({tree.Nodes[0].B.Min}, {tree.Nodes[0].B.Max})"); } + + static void BinnedTest(Action function, string name, ref Tree tree) + { + long accumulatedTime = 0; + const int testCount = 16; + for (int i = 0; i < testCount; ++i) + { + var startTime = Stopwatch.GetTimestamp(); + function(); + var endTime = Stopwatch.GetTimestamp(); + accumulatedTime += endTime - startTime; + //overlapHandler.Set.Clear(); + CacheBlaster.Blast(); + } + Console.WriteLine($"{name} time per execution (ms): {(accumulatedTime) * 1e3 / (testCount * Stopwatch.Frequency)}"); + + ulong accumulator = 0; + for (int i = 0; i < 1000; ++i) + { + var index = (int)(((ulong)i * 941083987 + accumulator * 797003413) % (ulong)tree.NodeCount); + var localSum = tree.Nodes[index].A.Min * 5 + tree.Nodes[index].A.Max * 7 + tree.Nodes[index].B.Min * 13 + tree.Nodes[index].B.Max * 17; + var hash = Unsafe.As(ref localSum.X) * 31 + Unsafe.As(ref localSum.Y) * 37 + Unsafe.As(ref localSum.Z) * 41; + accumulator = ((accumulator << 7) | (accumulator >> (64 - 7))) + (ulong)hash; + } + Console.WriteLine($"{name} bounds hash: {accumulator}, A ({tree.Nodes[0].A.Min}, {tree.Nodes[0].B.Max}), B ({tree.Nodes[0].B.Min}, {tree.Nodes[0].B.Max})"); + } } } From e8db6b91d974cdd89496ccba4cfef239020db720 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 15 Sep 2022 19:42:01 -0500 Subject: [PATCH 600/947] In the middle of binned builder revamp. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 132 ++++++++++++++++++ BepuUtilities/BoundingBox.cs | 22 ++- BepuUtilities/Int4.cs | 71 ++++++++++ .../SpecializedTests/TreeFiddlingTestDemo.cs | 34 +++-- 4 files changed, 247 insertions(+), 12 deletions(-) create mode 100644 BepuPhysics/Trees/Tree_BinnedBuilder.cs create mode 100644 BepuUtilities/Int4.cs diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs new file mode 100644 index 000000000..8df4bbd9a --- /dev/null +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -0,0 +1,132 @@ +using BepuUtilities; +using BepuUtilities.Collections; +using BepuUtilities.Memory; +using System; +using System.Diagnostics; +using System.Linq; +using System.Numerics; +using System.Runtime.CompilerServices; +using System.Runtime.Intrinsics; + +namespace BepuPhysics.Trees +{ + partial struct Tree + { + [MethodImpl(MethodImplOptions.AggressiveInlining)] + static unsafe Int4 Truncate(Vector4 v) + { + Int4 discrete; + if (Vector128.IsHardwareAccelerated) + { + Vector128.Store(Vector128.ConvertToInt32(v.AsVector128()), (int*)&discrete); + } + else + { + discrete.X = (int)v.X; + discrete.Y = (int)v.Y; + discrete.Z = (int)v.Z; + discrete.W = (int)v.W; + } + return discrete; + } + static unsafe void BinnedBuilderInternal(Buffer indices, Buffer boundingBoxes, Buffer binBoundingBoxes, Buffer nodes) + { + var centroidMin = new Vector4(float.MaxValue); + var centroidMax = new Vector4(float.MinValue); + var leafCount = indices.Length; + for (int i = 0; i < leafCount; ++i) + { + ref var box = ref boundingBoxes[i]; + //Note that centroids never bother scaling by 0.5. It's fine as long as we're consistent. + var centroid = box.Min + box.Max; + centroidMin = Vector4.Min(centroidMin, centroid); + centroidMax = Vector4.Max(centroidMax, centroid); + } + var binCount = Math.Min(MaximumBinCount, Math.Max((int)(leafCount * 0.25f), 4)); + Debug.Assert(binBoundingBoxes.Length >= binCount); + var centroidSpan = centroidMax - centroidMin; + var offsetToBinIndex = centroidSpan / binCount; + + var zeroedUpperSpan = centroidSpan.AsVector128().WithElement(3, 0); + if (Vector128.Dot(zeroedUpperSpan, zeroedUpperSpan) <= 1e-12f) + { + //This node is completely degenerate; there is no 'good' ordering of the children. Pick a split in the middle and shrug. + //This shouldn't happen unless something is badly wrong with the input; no point in optimizing it. + var midpoint = indices.Length / 2; + var secondCount = indices.Length - midpoint; + //Still have to compute the child bounding boxes, because the centroid bounds span being zero doesn't imply that the full bounds are zero. + for (int i = 0; i < midpoint; ++i) + { + + } + //nodes[0] = + BinnedBuilderInternal(indices.Slice(midpoint), boundingBoxes.Slice(midpoint), binBoundingBoxes, nodes.Slice(midpoint - 1)); + BinnedBuilderInternal(indices.Slice(midpoint, secondCount), boundingBoxes.Slice(midpoint, secondCount), binBoundingBoxes, nodes.Slice(secondCount - 1)); + } + + for (int i = 0; i < binCount; ++i) + { + ref var box = ref binBoundingBoxes[i]; + box.Min = new Vector4(float.MaxValue); + box.Max = new Vector4(float.MinValue); + } + + var maximumBinIndex = new Vector4(binCount - 1); + for (int i = 0; i < leafCount; ++i) + { + ref var box = ref boundingBoxes[i]; + var centroid = box.Min + box.Max; + var binIndicesForLeafContinuous = Vector4.Min(maximumBinIndex, (centroid - centroidMin) * offsetToBinIndex); + //Note that we don't store out any of the indices into per-bin lists here. We only *really* want two final groups for the children, + //and we can easily compute those by performing another scan. It requires recomputing the bin indices, but that's really not much of a concern. + var binIndicesForLeaf = Truncate(binIndicesForLeafContinuous); + ref var xBounds = ref binBoundingBoxes[binIndicesForLeaf.X]; + ref var yBounds = ref binBoundingBoxes[binIndicesForLeaf.Y]; + ref var zBounds = ref binBoundingBoxes[binIndicesForLeaf.Z]; + xBounds.Min = Vector4.Min(xBounds.Min, box.Min); + xBounds.Max = Vector4.Max(xBounds.Max, box.Max); + yBounds.Min = Vector4.Min(yBounds.Min, box.Min); + yBounds.Max = Vector4.Max(yBounds.Max, box.Max); + zBounds.Min = Vector4.Min(zBounds.Min, box.Min); + zBounds.Max = Vector4.Max(zBounds.Max, box.Max); + } + + //Identify the split index by examining the SAH of very split option. + var leftBounds = binBoundingBoxes[0]; + Debug.Assert(leftBounds.Min.X > float.MinValue && leftBounds.Min.Y > float.MinValue && leftBounds.Min.Z > float.MinValue, "Bin 0 should have been updated "); + for (int i = 1; i < binCount; ++i) + { + + } + } + + public static unsafe void BinnedBuilder(Buffer indices, Buffer boundingBoxes, Buffer nodes, BufferPool pool) + { + var leafCount = indices.Length; + Debug.Assert(boundingBoxes.Length >= leafCount, "The bounding boxes provided must cover the range of indices provided."); + Debug.Assert(nodes.Length > leafCount - 1, "The output nodes must be able to contain the nodes created for the leaves."); + if (leafCount == 0) + return; + if (leafCount == 1) + { + //If there's only one leaf, the tree has a special format: the root node has only one child. + ref var root = ref nodes[0]; + root.A.Min = boundingBoxes[0].Min; + root.A.Index = Encode(indices[0]); + root.A.Max = boundingBoxes[0].Max; + root.A.LeafCount = 1; + root.B = default; + return; + } + boundingBoxes = boundingBoxes.Slice(indices.Length); + nodes = nodes.Slice(leafCount - 1); + + var binBoundsMemory = stackalloc BoundingBox4[MaximumBinCount]; + var binBounds = new Buffer(binBoundsMemory, MaximumBinCount); + + //While we could avoid a recursive implementation, the overhead is low compared to the per-iteration cost. + BinnedBuilderInternal(indices, boundingBoxes.As(), binBounds, nodes); + } + + } +} diff --git a/BepuUtilities/BoundingBox.cs b/BepuUtilities/BoundingBox.cs index b1f2af1e2..508434108 100644 --- a/BepuUtilities/BoundingBox.cs +++ b/BepuUtilities/BoundingBox.cs @@ -12,9 +12,27 @@ namespace BepuUtilities { /// - /// Provides XNA-like axis-aligned bounding box functionality. + /// Represents a bounding box as two values to to avoid complexity associated with a 's empty SIMD lane. + /// + [StructLayout(LayoutKind.Explicit, Size = 32)] + public struct BoundingBox4 + { + /// + /// Location with the lowest X, Y, and Z coordinates in the axis-aligned bounding box. W lane is undefined. + /// + [FieldOffset(0)] + public Vector4 Min; + + /// + /// Location with the highest X, Y, and Z coordinates in the axis-aligned bounding box. W lane is undefined. + /// + [FieldOffset(16)] + public Vector4 Max; + } + + /// + /// Provides simple axis-aligned bounding box functionality. /// - //NOTE: The explicit size avoids https://github.com/dotnet/coreclr/issues/12950 [StructLayout(LayoutKind.Explicit, Size = 32)] public struct BoundingBox { diff --git a/BepuUtilities/Int4.cs b/BepuUtilities/Int4.cs new file mode 100644 index 000000000..ee8487acf --- /dev/null +++ b/BepuUtilities/Int4.cs @@ -0,0 +1,71 @@ +using BepuUtilities.Collections; +using System; +using System.Numerics; +using System.Runtime.CompilerServices; + +namespace BepuUtilities +{ + /// + /// A set of 4 integers, useful for spatial hashing. + /// + public struct Int4 : IEquatable, IEqualityComparerRef + { + public int X; + public int Y; + public int Z; + public int W; + + public unsafe override int GetHashCode() + { + const ulong p1 = 961748927UL; + const ulong p2 = 899809343UL; + const ulong p3 = 715225741UL; + const ulong p4 = 472882027UL; + var hash64 = (ulong)X * unchecked(p1 * p2 * p3) + (ulong)Y * (p2 * p3) + (ulong)Z * p3 + (ulong)W * p4; + return (int)(hash64 ^ (hash64 >> 32)); + } + + public override bool Equals(object obj) + { + return Equals((Int4)obj); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool Equals(Int4 other) + { + return other.X == X && other.Y == Y && other.Z == Z && other.W == W; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static bool operator ==(Int4 lhs, Int4 rhs) + { + return lhs.X == rhs.X && lhs.Y == rhs.Y && lhs.Z == rhs.Z && lhs.W == rhs.W; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static bool operator !=(Int4 lhs, Int4 rhs) + { + return lhs.X != rhs.X || lhs.Y != rhs.Y || lhs.Z != rhs.Z || lhs.W != rhs.W; + } + + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public override string ToString() + { + return $"{{{X}, {Y}, {Z}, {W}}}"; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public int Hash(ref Int4 item) + { + return item.GetHashCode(); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool Equals(ref Int4 a, ref Int4 b) + { + return a.X == b.X && a.Y == b.Y && a.Z == b.Z && a.W == b.W; + } + } + +} diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index 555bc8d1b..b9722f5aa 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -83,18 +83,32 @@ public override void Initialize(ContentArchive content, Camera camera) Console.WriteLine($"node count: {mesh.Tree.NodeCount}"); - QuickList subtreeReferences = new QuickList(mesh.Tree.LeafCount, BufferPool); - QuickList treeletInternalNodes = new QuickList(mesh.Tree.LeafCount, BufferPool); - Tree.CreateBinnedResources(BufferPool, mesh.Tree.LeafCount, out var binnedResourcesBuffer, out var binnedResources); + BufferPool.Take(mesh.Triangles.Length, out var leafBounds); + BufferPool.Take(mesh.Triangles.Length, out var leafIndices); + for (int i = 0; i < mesh.Triangles.Length; ++i) + { + ref var t = ref mesh.Triangles[i]; + ref var bounds = ref leafBounds[i]; + bounds.Min = Vector3.Min(t.A, Vector3.Min(t.B, t.C)); + bounds.Max = Vector3.Max(t.A, Vector3.Max(t.B, t.C)); + leafIndices[i] = i; + } BinnedTest(() => { - subtreeReferences.Count = 0; - treeletInternalNodes.Count = 0; - mesh.Tree.BinnedRefine(0, ref subtreeReferences, mesh.Tree.LeafCount, ref treeletInternalNodes, ref binnedResources, BufferPool); - }, "Original", ref mesh.Tree); - - RefitTest(() => mesh.Tree.Refit2(), "refit2", ref mesh.Tree); - RefitTest(() => mesh.Tree.Refit(), "Original", ref mesh.Tree); + Tree.BinnedBuilder(leafIndices, leafBounds, mesh.Tree.Nodes, BufferPool); + }, "Revamp", ref mesh.Tree); + //QuickList subtreeReferences = new QuickList(mesh.Tree.LeafCount, BufferPool); + //QuickList treeletInternalNodes = new QuickList(mesh.Tree.LeafCount, BufferPool); + //Tree.CreateBinnedResources(BufferPool, mesh.Tree.LeafCount, out var binnedResourcesBuffer, out var binnedResources); + //BinnedTest(() => + //{ + // subtreeReferences.Count = 0; + // treeletInternalNodes.Count = 0; + // mesh.Tree.BinnedRefine(0, ref subtreeReferences, mesh.Tree.LeafCount, ref treeletInternalNodes, ref binnedResources, BufferPool); + //}, "Original", ref mesh.Tree); + + //RefitTest(() => mesh.Tree.Refit2(), "refit2", ref mesh.Tree); + //RefitTest(() => mesh.Tree.Refit(), "Original", ref mesh.Tree); //SelfTest((ref OverlapHandler handler) => mesh.Tree.GetSelfOverlapsContiguousPrepass(ref handler, BufferPool), mesh.Tree.LeafCount, "Prepass"); //SelfTest((ref OverlapHandler handler) => mesh.Tree.GetSelfOverlaps(ref handler), mesh.Tree.LeafCount, "Original"); From faead1797444699bbea950dcb7c3663aabc67270 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 16 Sep 2022 18:13:23 -0500 Subject: [PATCH 601/947] Single pass of new binned builder done. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 321 ++++++++++++++++-- BepuUtilities/BoundingBox.cs | 11 + .../SpecializedTests/TreeFiddlingTestDemo.cs | 3 +- 3 files changed, 313 insertions(+), 22 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 8df4bbd9a..35404072f 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -1,12 +1,15 @@ -using BepuUtilities; +using BepuPhysics.Constraints; +using BepuUtilities; using BepuUtilities.Collections; using BepuUtilities.Memory; using System; using System.Diagnostics; using System.Linq; +using System.Net; using System.Numerics; using System.Runtime.CompilerServices; using System.Runtime.Intrinsics; +using System.Threading.Tasks.Sources; namespace BepuPhysics.Trees { @@ -28,8 +31,64 @@ static unsafe Int4 Truncate(Vector4 v) discrete.W = (int)v.W; } return discrete; + + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + static void BuildParentNode(BoundingBox4 a, BoundingBox4 b, Buffer nodes, int parentNodeIndex, int firstChildCount, int secondChildCount, out int aIndex, out int bIndex) + { + ref var parentNode = ref nodes[0]; + aIndex = parentNodeIndex + 1; + bIndex = parentNodeIndex + firstChildCount;//parentNodeIndex + 1 + (firstChildCount - 1) + parentNode.A = Unsafe.As(ref a); + parentNode.B = Unsafe.As(ref b); + parentNode.A.Index = aIndex; + parentNode.A.LeafCount = firstChildCount; + parentNode.B.Index = bIndex; + parentNode.B.LeafCount = secondChildCount; + } + + struct Bins + { + public Buffer BinBoundingBoxesX; + public Buffer BinBoundingBoxesY; + public Buffer BinBoundingBoxesZ; + public Buffer BinBoundingBoxesScanX; + public Buffer BinBoundingBoxesScanY; + public Buffer BinBoundingBoxesScanZ; + + public Buffer BinLeafCountsX; + public Buffer BinLeafCountsY; + public Buffer BinLeafCountsZ; } - static unsafe void BinnedBuilderInternal(Buffer indices, Buffer boundingBoxes, Buffer binBoundingBoxes, Buffer nodes) + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + internal static float ComputeBoundsMetric(BoundingBox4 bounds) + { + return ComputeBoundsMetric(bounds.Min, bounds.Max); + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + internal static float ComputeBoundsMetric(Vector4 min, Vector4 max) + { + //Note that we just use the SAH. While we are primarily interested in volume queries for the purposes of collision detection, the topological difference + //between a volume heuristic and surface area heuristic isn't huge. There is, however, one big annoying issue that volume heuristics run into: + //all bounding boxes with one extent equal to zero have zero cost. Surface area approaches avoid this hole simply. + if (Vector128.IsHardwareAccelerated) + { + var span = max - min; + var shuffled = Vector128.Shuffle(span.AsVector128(), Vector128.Create(1, 2, 0, 0)); + var zeroedUpper = shuffled.WithElement(3, 0); + return Vector128.Dot(span.AsVector128(), zeroedUpper); + } + else + { + var offset = max - min; + //Note that this is merely proportional to surface area. Being scaled by a constant factor is irrelevant. + return offset.X * offset.Y + offset.Y * offset.Z + offset.Z * offset.X; + } + } + + static unsafe void BinnedBuilderInternal(Buffer indices, Buffer boundingBoxes, Buffer nodes, int parentNodeIndex, in Bins bins) { var centroidMin = new Vector4(float.MaxValue); var centroidMax = new Vector4(float.MinValue); @@ -43,32 +102,58 @@ static unsafe void BinnedBuilderInternal(Buffer indices, Buffer= binCount); + Debug.Assert(bins.BinBoundingBoxesX.Length >= binCount); + Debug.Assert(bins.BinBoundingBoxesY.Length >= binCount); + Debug.Assert(bins.BinBoundingBoxesZ.Length >= binCount); var centroidSpan = centroidMax - centroidMin; - var offsetToBinIndex = centroidSpan / binCount; + var offsetToBinIndex = new Vector4(binCount) / centroidSpan; + //Avoid letting NaNs into the offsetToBinIndex scale. + var axisIsDegenerate = Vector128.LessThanOrEqual(centroidSpan.AsVector128(), Vector128.Create(1e-12f)); + offsetToBinIndex = Vector128.ConditionalSelect(axisIsDegenerate, Vector128.Zero, offsetToBinIndex.AsVector128()).AsVector4(); - var zeroedUpperSpan = centroidSpan.AsVector128().WithElement(3, 0); - if (Vector128.Dot(zeroedUpperSpan, zeroedUpperSpan) <= 1e-12f) + if ((Vector128.ExtractMostSignificantBits(axisIsDegenerate) & 0b111) == 0b111) { //This node is completely degenerate; there is no 'good' ordering of the children. Pick a split in the middle and shrug. //This shouldn't happen unless something is badly wrong with the input; no point in optimizing it. var midpoint = indices.Length / 2; var secondCount = indices.Length - midpoint; //Still have to compute the child bounding boxes, because the centroid bounds span being zero doesn't imply that the full bounds are zero. + BoundingBox4 boundsA, boundsB; + boundsA.Min = new Vector4(float.MaxValue); + boundsA.Max = new Vector4(float.MinValue); + boundsB.Min = new Vector4(float.MaxValue); + boundsB.Max = new Vector4(float.MinValue); for (int i = 0; i < midpoint; ++i) { - + ref var bounds = ref boundingBoxes[i]; + boundsA.Min = Vector4.Min(bounds.Min, boundsA.Min); + boundsA.Max = Vector4.Max(bounds.Max, boundsA.Max); } - //nodes[0] = - BinnedBuilderInternal(indices.Slice(midpoint), boundingBoxes.Slice(midpoint), binBoundingBoxes, nodes.Slice(midpoint - 1)); - BinnedBuilderInternal(indices.Slice(midpoint, secondCount), boundingBoxes.Slice(midpoint, secondCount), binBoundingBoxes, nodes.Slice(secondCount - 1)); + for (int i = midpoint; i < indices.Length; ++i) + { + ref var bounds = ref boundingBoxes[i]; + boundsB.Min = Vector4.Min(bounds.Min, boundsB.Min); + boundsB.Max = Vector4.Max(bounds.Max, boundsB.Max); + } + BuildParentNode(boundsA, boundsB, nodes, parentNodeIndex, midpoint, secondCount, out var aIndex, out var bIndex); + BinnedBuilderInternal(indices.Slice(midpoint), boundingBoxes.Slice(midpoint), nodes.Slice(1, midpoint - 1), aIndex, bins); + BinnedBuilderInternal(indices.Slice(midpoint, secondCount), boundingBoxes.Slice(midpoint, secondCount), nodes.Slice(midpoint, secondCount - 1), bIndex, bins); } for (int i = 0; i < binCount; ++i) { - ref var box = ref binBoundingBoxes[i]; - box.Min = new Vector4(float.MaxValue); - box.Max = new Vector4(float.MinValue); + ref var boxX = ref bins.BinBoundingBoxesX[i]; + ref var boxY = ref bins.BinBoundingBoxesY[i]; + ref var boxZ = ref bins.BinBoundingBoxesZ[i]; + boxX.Min = new Vector4(float.MaxValue); + boxX.Max = new Vector4(float.MinValue); + boxY.Min = new Vector4(float.MaxValue); + boxY.Max = new Vector4(float.MinValue); + boxZ.Min = new Vector4(float.MaxValue); + boxZ.Max = new Vector4(float.MinValue); + bins.BinLeafCountsX[i] = 0; + bins.BinLeafCountsY[i] = 0; + bins.BinLeafCountsZ[i] = 0; } var maximumBinIndex = new Vector4(binCount - 1); @@ -80,24 +165,207 @@ static unsafe void BinnedBuilderInternal(Buffer indices, Buffer float.MinValue && leftBounds.Min.Y > float.MinValue && leftBounds.Min.Z > float.MinValue, "Bin 0 should have been updated "); + //Premerge from left to right so we have a sorta-summed area table to cheaply look up all possible child A bounds as we scan. + bins.BinBoundingBoxesScanX[0] = bins.BinBoundingBoxesX[0]; + bins.BinBoundingBoxesScanY[0] = bins.BinBoundingBoxesY[0]; + bins.BinBoundingBoxesScanZ[0] = bins.BinBoundingBoxesZ[0]; for (int i = 1; i < binCount; ++i) { + var previousIndex = i - 1; + ref var xBounds = ref bins.BinBoundingBoxesX[i]; + ref var yBounds = ref bins.BinBoundingBoxesY[i]; + ref var zBounds = ref bins.BinBoundingBoxesZ[i]; + ref var xScanBounds = ref bins.BinBoundingBoxesScanX[i]; + ref var yScanBounds = ref bins.BinBoundingBoxesScanY[i]; + ref var zScanBounds = ref bins.BinBoundingBoxesScanZ[i]; + ref var xPreviousScanBounds = ref bins.BinBoundingBoxesScanX[previousIndex]; + ref var yPreviousScanBounds = ref bins.BinBoundingBoxesScanY[previousIndex]; + ref var zPreviousScanBounds = ref bins.BinBoundingBoxesScanZ[previousIndex]; + xScanBounds.Min = Vector4.Min(xBounds.Min, xPreviousScanBounds.Min); + xScanBounds.Max = Vector4.Max(xBounds.Max, xPreviousScanBounds.Max); + yScanBounds.Min = Vector4.Min(yBounds.Min, yPreviousScanBounds.Min); + yScanBounds.Max = Vector4.Max(yBounds.Max, yPreviousScanBounds.Max); + zScanBounds.Min = Vector4.Min(zBounds.Min, zPreviousScanBounds.Min); + zScanBounds.Max = Vector4.Max(zBounds.Max, zPreviousScanBounds.Max); + } + var leftBoundsX = bins.BinBoundingBoxesX[0]; + var leftBoundsY = bins.BinBoundingBoxesY[0]; + var leftBoundsZ = bins.BinBoundingBoxesZ[0]; + Debug.Assert( + leftBoundsX.Min.X > float.MinValue && leftBoundsX.Min.Y > float.MinValue && leftBoundsX.Min.Z > float.MinValue && + leftBoundsY.Min.X > float.MinValue && leftBoundsY.Min.Y > float.MinValue && leftBoundsY.Min.Z > float.MinValue && + leftBoundsZ.Min.X > float.MinValue && leftBoundsZ.Min.Y > float.MinValue && leftBoundsZ.Min.Z > float.MinValue, + "Bin 0 should have been updated in all cases because it is aligned with the minimum bin, and the centroid span isn't degenerate."); + float bestX = float.MaxValue, bestY = float.MaxValue, bestZ = float.MaxValue; + int bestSplitX = 1, bestSplitY = 1, bestSplitZ = 1; + //The split index is going to end up in child B. + var lastBinIndex = binCount - 1; + BoundingBox4 accumulatedBoundingBoxBX, accumulatedBoundingBoxBY, accumulatedBoundingBoxBZ; + accumulatedBoundingBoxBX = bins.BinBoundingBoxesX[lastBinIndex]; + accumulatedBoundingBoxBY = bins.BinBoundingBoxesY[lastBinIndex]; + accumulatedBoundingBoxBZ = bins.BinBoundingBoxesZ[lastBinIndex]; + BoundingBox4 bestBoundingBoxBX, bestBoundingBoxBY, bestBoundingBoxBZ; + bestBoundingBoxBX = bins.BinBoundingBoxesX[lastBinIndex]; + bestBoundingBoxBY = bins.BinBoundingBoxesY[lastBinIndex]; + bestBoundingBoxBZ = bins.BinBoundingBoxesZ[lastBinIndex]; + int accumulatedLeafCountBX = bins.BinLeafCountsX[lastBinIndex]; + int accumulatedLeafCountBY = bins.BinLeafCountsY[lastBinIndex]; + int accumulatedLeafCountBZ = bins.BinLeafCountsZ[lastBinIndex]; + for (int splitIndexCandidate = lastBinIndex; splitIndexCandidate >= 1; --splitIndexCandidate) + { + var previousIndex = splitIndexCandidate - 1; + var leafCountAX = leafCount - accumulatedLeafCountBX; + var leafCountAY = leafCount - accumulatedLeafCountBY; + var leafCountAZ = leafCount - accumulatedLeafCountBZ; + var sahX = ComputeBoundsMetric(bins.BinBoundingBoxesScanX[previousIndex]) * leafCountAX + ComputeBoundsMetric(accumulatedBoundingBoxBX) * accumulatedLeafCountBX; + var sahY = ComputeBoundsMetric(bins.BinBoundingBoxesScanY[previousIndex]) * leafCountAY + ComputeBoundsMetric(accumulatedBoundingBoxBY) * accumulatedLeafCountBY; + var sahZ = ComputeBoundsMetric(bins.BinBoundingBoxesScanZ[previousIndex]) * leafCountAZ + ComputeBoundsMetric(accumulatedBoundingBoxBZ) * accumulatedLeafCountBZ; + if (sahX < bestX) + { + bestX = sahX; + bestSplitX = splitIndexCandidate; + bestBoundingBoxBX = accumulatedBoundingBoxBX; + } + if (sahY < bestY) + { + bestY = sahY; + bestSplitY = splitIndexCandidate; + bestBoundingBoxBY = accumulatedBoundingBoxBY; + } + if (sahZ < bestZ) + { + bestZ = sahZ; + bestSplitZ = splitIndexCandidate; + bestBoundingBoxBZ = accumulatedBoundingBoxBZ; + } + ref var xBounds = ref bins.BinBoundingBoxesX[splitIndexCandidate]; + ref var yBounds = ref bins.BinBoundingBoxesY[splitIndexCandidate]; + ref var zBounds = ref bins.BinBoundingBoxesZ[splitIndexCandidate]; + accumulatedBoundingBoxBX.Min = Vector4.Min(xBounds.Min, accumulatedBoundingBoxBX.Min); + accumulatedBoundingBoxBX.Max = Vector4.Max(xBounds.Max, accumulatedBoundingBoxBX.Max); + accumulatedBoundingBoxBY.Min = Vector4.Min(yBounds.Min, accumulatedBoundingBoxBY.Min); + accumulatedBoundingBoxBY.Max = Vector4.Max(yBounds.Max, accumulatedBoundingBoxBY.Max); + accumulatedBoundingBoxBZ.Min = Vector4.Min(zBounds.Min, accumulatedBoundingBoxBZ.Min); + accumulatedBoundingBoxBZ.Max = Vector4.Max(zBounds.Max, accumulatedBoundingBoxBZ.Max); + accumulatedLeafCountBX += bins.BinLeafCountsX[splitIndexCandidate]; + accumulatedLeafCountBY += bins.BinLeafCountsY[splitIndexCandidate]; + accumulatedLeafCountBZ += bins.BinLeafCountsZ[splitIndexCandidate]; + } + + //Choose the best SAH from all axes and split the indices/bounds into two halves for the children to operate on. + int splitIndex; + BoundingBox4 bestboundsA, bestboundsB; + var bCount = 0; + var aCount = 0; + if (bestX < bestY && bestX < bestZ) + { + splitIndex = bestSplitX; + bestboundsA = bins.BinBoundingBoxesScanX[bestSplitX - 1]; + bestboundsB = bestBoundingBoxBX; + //Now we have the split index between bins. Go back through and sort the indices and bounds into two halves. + while(aCount + bCount < leafCount) + { + ref var box = ref boundingBoxes[aCount]; + var centroid = box.Min + box.Max; + var binIndicesForLeafContinuous = Vector4.Min(maximumBinIndex, (centroid - centroidMin) * offsetToBinIndex); + //Note that we don't store out any of the indices into per-bin lists here. We only *really* want two final groups for the children, + //and we can easily compute those by performing another scan. It requires recomputing the bin indices, but that's really not much of a concern. + var binIndex = (int)binIndicesForLeafContinuous.X; + if (binIndex >= splitIndex) + { + //Belongs to B. Swap it. + var targetIndex = leafCount - bCount - 1; + Helpers.Swap(ref indices[targetIndex], ref indices[aCount]); + Helpers.Swap(ref boundingBoxes[targetIndex], ref boundingBoxes[aCount]); + ++bCount; + //(Note that we still need to examine what we just swapped into the slot! It may belong to B too!) + } + else + { + //Belongs to A, no movement necessary. + ++aCount; + } + } + } + else if (bestY < bestZ) + { + splitIndex = bestSplitY; + bestboundsA = bins.BinBoundingBoxesScanY[bestSplitY - 1]; + bestboundsB = bestBoundingBoxBY; + while (aCount + bCount < leafCount) + { + ref var box = ref boundingBoxes[aCount]; + var centroid = box.Min + box.Max; + var binIndicesForLeafContinuous = Vector4.Min(maximumBinIndex, (centroid - centroidMin) * offsetToBinIndex); + //Note that we don't store out any of the indices into per-bin lists here. We only *really* want two final groups for the children, + //and we can easily compute those by performing another scan. It requires recomputing the bin indices, but that's really not much of a concern. + var binIndex = (int)binIndicesForLeafContinuous.Y; + if (binIndex >= splitIndex) + { + //Belongs to B. Swap it. + var targetIndex = leafCount - bCount - 1; + Helpers.Swap(ref indices[targetIndex], ref indices[aCount]); + Helpers.Swap(ref boundingBoxes[targetIndex], ref boundingBoxes[aCount]); + ++bCount; + //(Note that we still need to examine what we just swapped into the slot! It may belong to B too!) + } + else + { + //Belongs to A, no movement necessary. + ++aCount; + } + } } + else + { + splitIndex = bestSplitZ; + bestboundsA = bins.BinBoundingBoxesScanZ[bestSplitZ - 1]; + bestboundsB = bestBoundingBoxBZ; + while (aCount + bCount < leafCount) + { + ref var box = ref boundingBoxes[aCount]; + var centroid = box.Min + box.Max; + var binIndicesForLeafContinuous = Vector4.Min(maximumBinIndex, (centroid - centroidMin) * offsetToBinIndex); + //Note that we don't store out any of the indices into per-bin lists here. We only *really* want two final groups for the children, + //and we can easily compute those by performing another scan. It requires recomputing the bin indices, but that's really not much of a concern. + var binIndex = (int)binIndicesForLeafContinuous.Z; + if (binIndex >= splitIndex) + { + //Belongs to B. Swap it. + var targetIndex = leafCount - bCount - 1; + Helpers.Swap(ref indices[targetIndex], ref indices[aCount]); + Helpers.Swap(ref boundingBoxes[targetIndex], ref boundingBoxes[aCount]); + ++bCount; + //(Note that we still need to examine what we just swapped into the slot! It may belong to B too!) + } + else + { + //Belongs to A, no movement necessary. + ++aCount; + } + } + } + + { + BuildParentNode(bestboundsA, bestboundsB, nodes, parentNodeIndex, leafCount - bCount, bCount, out var aIndex, out var bIndex); + } + } public static unsafe void BinnedBuilder(Buffer indices, Buffer boundingBoxes, Buffer nodes, BufferPool pool) @@ -121,11 +389,22 @@ public static unsafe void BinnedBuilder(Buffer indices, Buffer boundingBoxes = boundingBoxes.Slice(indices.Length); nodes = nodes.Slice(leafCount - 1); - var binBoundsMemory = stackalloc BoundingBox4[MaximumBinCount]; - var binBounds = new Buffer(binBoundsMemory, MaximumBinCount); + var binBoundsMemory = stackalloc BoundingBox4[MaximumBinCount * 6]; + Bins bins; + bins.BinBoundingBoxesX = new Buffer(binBoundsMemory, MaximumBinCount); + bins.BinBoundingBoxesY = new Buffer(binBoundsMemory + MaximumBinCount, MaximumBinCount); + bins.BinBoundingBoxesZ = new Buffer(binBoundsMemory + MaximumBinCount * 2, MaximumBinCount); + bins.BinBoundingBoxesScanX = new Buffer(binBoundsMemory + MaximumBinCount * 3, MaximumBinCount); + bins.BinBoundingBoxesScanY = new Buffer(binBoundsMemory + MaximumBinCount * 4, MaximumBinCount); + bins.BinBoundingBoxesScanZ = new Buffer(binBoundsMemory + MaximumBinCount * 5, MaximumBinCount); + + var binLeafCountsMemory = stackalloc int[MaximumBinCount * 3]; + bins.BinLeafCountsX = new Buffer(binLeafCountsMemory, MaximumBinCount); + bins.BinLeafCountsY = new Buffer(binLeafCountsMemory + MaximumBinCount, MaximumBinCount); + bins.BinLeafCountsZ = new Buffer(binLeafCountsMemory + MaximumBinCount * 2, MaximumBinCount); //While we could avoid a recursive implementation, the overhead is low compared to the per-iteration cost. - BinnedBuilderInternal(indices, boundingBoxes.As(), binBounds, nodes); + BinnedBuilderInternal(indices, boundingBoxes.As(), nodes, 0, bins); } } diff --git a/BepuUtilities/BoundingBox.cs b/BepuUtilities/BoundingBox.cs index 508434108..6f45045c5 100644 --- a/BepuUtilities/BoundingBox.cs +++ b/BepuUtilities/BoundingBox.cs @@ -28,6 +28,17 @@ public struct BoundingBox4 /// [FieldOffset(16)] public Vector4 Max; + + + /// + /// Creates a string representation of the bounding box. + /// + /// String representation of the bounding box. + public override string ToString() + { + return $"({Unsafe.As(ref Min)}, {Unsafe.As(ref Max)})"; + } + } /// diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index b9722f5aa..2ec031b1c 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -78,7 +78,8 @@ public override void Initialize(ContentArchive content, Camera camera) var width = 768; var height = 768; var scale = new Vector3(1, 1, 1); - DemoMeshHelper.CreateDeformedPlane(width, height, (x, y) => new Vector3(x - width * scale.X * 0.5f, 2f * (float)(Math.Sin(x * 0.5f) * Math.Sin(y * 0.5f)), y - height * scale.Y * 0.5f), scale, BufferPool, out var mesh); + //DemoMeshHelper.CreateDeformedPlane(width, height, (x, y) => new Vector3(x - width * scale.X * 0.5f, 2f * (float)(Math.Sin(x * 0.5f) * Math.Sin(y * 0.5f)), y - height * scale.Y * 0.5f), scale, BufferPool, out var mesh); + DemoMeshHelper.CreateDeformedPlane(width, height, (x, y) => new Vector3(x - width * scale.X * 0.5f, 0, y - height * scale.Y * 0.5f), scale, BufferPool, out var mesh); Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); Console.WriteLine($"node count: {mesh.Tree.NodeCount}"); From 68e8c340fbaedb94d2b021d96b1f6a7b47b96abe Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 16 Sep 2022 19:10:34 -0500 Subject: [PATCH 602/947] BinnedRefine works, though it doesn't yet manage metanodes or use a reasonable termination condition. And still does triple axis SAH testing. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 39 ++++++++++++------- .../SpecializedTests/TreeFiddlingTestDemo.cs | 25 ++++++------ 2 files changed, 38 insertions(+), 26 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 35404072f..64dcc27d2 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -35,17 +35,17 @@ static unsafe Int4 Truncate(Vector4 v) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - static void BuildParentNode(BoundingBox4 a, BoundingBox4 b, Buffer nodes, int parentNodeIndex, int firstChildCount, int secondChildCount, out int aIndex, out int bIndex) + static void BuildParentNode(BoundingBox4 a, BoundingBox4 b, Buffer nodes, Buffer indices, int parentNodeIndex, int aCount, int bCount, out int aIndex, out int bIndex) { ref var parentNode = ref nodes[0]; - aIndex = parentNodeIndex + 1; - bIndex = parentNodeIndex + firstChildCount;//parentNodeIndex + 1 + (firstChildCount - 1) + aIndex = aCount == 1 ? indices[0] : parentNodeIndex + 1; + bIndex = bCount == 1 ? indices[^1] : parentNodeIndex + aCount;//parentNodeIndex + 1 + (aCount - 1) parentNode.A = Unsafe.As(ref a); parentNode.B = Unsafe.As(ref b); parentNode.A.Index = aIndex; - parentNode.A.LeafCount = firstChildCount; + parentNode.A.LeafCount = aCount; parentNode.B.Index = bIndex; - parentNode.B.LeafCount = secondChildCount; + parentNode.B.LeafCount = bCount; } struct Bins @@ -93,6 +93,7 @@ static unsafe void BinnedBuilderInternal(Buffer indices, Buffer indices, Buffer 1) + BinnedBuilderInternal(indices.Slice(countA), boundingBoxes.Slice(countA), nodes.Slice(1, countA - 1), aIndex, bins); + if (countB > 1) + BinnedBuilderInternal(indices.Slice(countA, countB), boundingBoxes.Slice(countA, countB), nodes.Slice(countA, countB - 1), bIndex, bins); + return; } for (int i = 0; i < binCount; ++i) @@ -279,7 +283,7 @@ static unsafe void BinnedBuilderInternal(Buffer indices, Buffer indices, Buffer 1) + BinnedBuilderInternal(indices.Slice(aCount), boundingBoxes.Slice(aCount), nodes.Slice(1, aCount - 1), aIndex, bins); + if (bCount > 1) + BinnedBuilderInternal(indices.Slice(aCount, bCount), boundingBoxes.Slice(aCount, bCount), nodes.Slice(aCount, bCount - 1), bIndex, bins); } } @@ -372,7 +381,7 @@ public static unsafe void BinnedBuilder(Buffer indices, Buffer { var leafCount = indices.Length; Debug.Assert(boundingBoxes.Length >= leafCount, "The bounding boxes provided must cover the range of indices provided."); - Debug.Assert(nodes.Length > leafCount - 1, "The output nodes must be able to contain the nodes created for the leaves."); + Debug.Assert(nodes.Length >= leafCount - 1, "The output nodes must be able to contain the nodes created for the leaves."); if (leafCount == 0) return; if (leafCount == 1) diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index 2ec031b1c..9de73061a 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -78,8 +78,8 @@ public override void Initialize(ContentArchive content, Camera camera) var width = 768; var height = 768; var scale = new Vector3(1, 1, 1); - //DemoMeshHelper.CreateDeformedPlane(width, height, (x, y) => new Vector3(x - width * scale.X * 0.5f, 2f * (float)(Math.Sin(x * 0.5f) * Math.Sin(y * 0.5f)), y - height * scale.Y * 0.5f), scale, BufferPool, out var mesh); - DemoMeshHelper.CreateDeformedPlane(width, height, (x, y) => new Vector3(x - width * scale.X * 0.5f, 0, y - height * scale.Y * 0.5f), scale, BufferPool, out var mesh); + DemoMeshHelper.CreateDeformedPlane(width, height, (x, y) => new Vector3(x - width * scale.X * 0.5f, 2f * (float)(Math.Sin(x * 0.5f) * Math.Sin(y * 0.5f)), y - height * scale.Y * 0.5f), scale, BufferPool, out var mesh); + //DemoMeshHelper.CreateDeformedPlane(width, height, (x, y) => new Vector3(x - width * scale.X * 0.5f, 0, y - height * scale.Y * 0.5f), scale, BufferPool, out var mesh); Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); Console.WriteLine($"node count: {mesh.Tree.NodeCount}"); @@ -98,15 +98,18 @@ public override void Initialize(ContentArchive content, Camera camera) { Tree.BinnedBuilder(leafIndices, leafBounds, mesh.Tree.Nodes, BufferPool); }, "Revamp", ref mesh.Tree); - //QuickList subtreeReferences = new QuickList(mesh.Tree.LeafCount, BufferPool); - //QuickList treeletInternalNodes = new QuickList(mesh.Tree.LeafCount, BufferPool); - //Tree.CreateBinnedResources(BufferPool, mesh.Tree.LeafCount, out var binnedResourcesBuffer, out var binnedResources); - //BinnedTest(() => - //{ - // subtreeReferences.Count = 0; - // treeletInternalNodes.Count = 0; - // mesh.Tree.BinnedRefine(0, ref subtreeReferences, mesh.Tree.LeafCount, ref treeletInternalNodes, ref binnedResources, BufferPool); - //}, "Original", ref mesh.Tree); + + DemoMeshHelper.CreateDeformedPlane(width, height, (x, y) => new Vector3(x - width * scale.X * 0.5f, 2f * (float)(Math.Sin(x * 0.5f) * Math.Sin(y * 0.5f)), y - height * scale.Y * 0.5f), scale, BufferPool, out var mesh2); + + QuickList subtreeReferences = new QuickList(mesh2.Tree.LeafCount, BufferPool); + QuickList treeletInternalNodes = new QuickList(mesh2.Tree.LeafCount, BufferPool); + Tree.CreateBinnedResources(BufferPool, mesh.Tree.LeafCount, out var binnedResourcesBuffer, out var binnedResources); + BinnedTest(() => + { + subtreeReferences.Count = 0; + treeletInternalNodes.Count = 0; + mesh2.Tree.BinnedRefine(0, ref subtreeReferences, mesh2.Tree.LeafCount, ref treeletInternalNodes, ref binnedResources, BufferPool); + }, "Original", ref mesh.Tree); //RefitTest(() => mesh.Tree.Refit2(), "refit2", ref mesh.Tree); //RefitTest(() => mesh.Tree.Refit(), "Original", ref mesh.Tree); From 8d3060f2b5788baa9b7f200e8e2304a0cfae75cf Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 17 Sep 2022 15:53:28 -0500 Subject: [PATCH 603/947] Test refactor + SAH output. --- .../SpecializedTests/TreeFiddlingTestDemo.cs | 69 +++++++++++++++++-- 1 file changed, 64 insertions(+), 5 deletions(-) diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index 9de73061a..9416b52de 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -15,6 +15,8 @@ using BepuPhysics; using BepuPhysics.Constraints; using BepuPhysics.Collidables; +using static System.Formats.Asn1.AsnWriter; +using static OpenTK.Graphics.OpenGL.GL; namespace Demos.SpecializedTests { @@ -68,6 +70,54 @@ public void Handle(int indexA, int indexB) } } + Buffer CreateDeformedPlaneTriangles(int width, int height, Vector3 scale) + { + Vector3 Deform(int x, int y) => new Vector3(x - width * scale.X * 0.5f, 2f * (float)(Math.Sin(x * 0.5f) * Math.Sin(y * 0.5f)), y - height * scale.Y * 0.5f); + BufferPool.Take(width * height, out var vertices); + for (int i = 0; i < width; ++i) + { + for (int j = 0; j < height; ++j) + { + vertices[width * j + i] = Deform(i, j); + } + } + + var quadWidth = width - 1; + var quadHeight = height - 1; + var triangleCount = quadWidth * quadHeight * 2; + BufferPool.Take(triangleCount, out var triangles); + + for (int i = 0; i < quadWidth; ++i) + { + for (int j = 0; j < quadHeight; ++j) + { + var triangleIndex = (j * quadWidth + i) * 2; + ref var triangle0 = ref triangles[triangleIndex]; + ref var v00 = ref vertices[width * j + i]; + ref var v01 = ref vertices[width * j + i + 1]; + ref var v10 = ref vertices[width * (j + 1) + i]; + ref var v11 = ref vertices[width * (j + 1) + i + 1]; + triangle0.A = v00; + triangle0.B = v01; + triangle0.C = v10; + ref var triangle1 = ref triangles[triangleIndex + 1]; + triangle1.A = v01; + triangle1.B = v11; + triangle1.C = v10; + } + } + BufferPool.Return(ref vertices); + //Scramble the heck out of its triangles. + var random = new Random(5); + for (int index = 0; index < triangles.Length; ++index) + { + ref var a = ref triangles[index]; + ref var b = ref triangles[random.Next(index + 1, triangles.Length)]; + BepuPhysics.Helpers.Swap(ref a, ref b); + } + return triangles; + } + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(-10, 3, -10); @@ -78,10 +128,16 @@ public override void Initialize(ContentArchive content, Camera camera) var width = 768; var height = 768; var scale = new Vector3(1, 1, 1); - DemoMeshHelper.CreateDeformedPlane(width, height, (x, y) => new Vector3(x - width * scale.X * 0.5f, 2f * (float)(Math.Sin(x * 0.5f) * Math.Sin(y * 0.5f)), y - height * scale.Y * 0.5f), scale, BufferPool, out var mesh); + + //DemoMeshHelper.CreateDeformedPlane(width, height, (x, y) => new Vector3(x - width * scale.X * 0.5f, 2f * (float)(Math.Sin(x * 0.5f) * Math.Sin(y * 0.5f)), y - height * scale.Y * 0.5f), scale, BufferPool, out var mesh); //DemoMeshHelper.CreateDeformedPlane(width, height, (x, y) => new Vector3(x - width * scale.X * 0.5f, 0, y - height * scale.Y * 0.5f), scale, BufferPool, out var mesh); + + //Create a wiggly mesh. + var triangles = CreateDeformedPlaneTriangles(width, height, scale); + var mesh = new Mesh(triangles, Vector3.One, BufferPool); Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); + Console.WriteLine($"node count: {mesh.Tree.NodeCount}"); BufferPool.Take(mesh.Triangles.Length, out var leafBounds); @@ -99,11 +155,12 @@ public override void Initialize(ContentArchive content, Camera camera) Tree.BinnedBuilder(leafIndices, leafBounds, mesh.Tree.Nodes, BufferPool); }, "Revamp", ref mesh.Tree); - DemoMeshHelper.CreateDeformedPlane(width, height, (x, y) => new Vector3(x - width * scale.X * 0.5f, 2f * (float)(Math.Sin(x * 0.5f) * Math.Sin(y * 0.5f)), y - height * scale.Y * 0.5f), scale, BufferPool, out var mesh2); - QuickList subtreeReferences = new QuickList(mesh2.Tree.LeafCount, BufferPool); - QuickList treeletInternalNodes = new QuickList(mesh2.Tree.LeafCount, BufferPool); - Tree.CreateBinnedResources(BufferPool, mesh.Tree.LeafCount, out var binnedResourcesBuffer, out var binnedResources); + var mesh2 = new Mesh(triangles, Vector3.One, BufferPool); + + QuickList subtreeReferences = new(mesh2.Tree.LeafCount, BufferPool); + QuickList treeletInternalNodes = new(mesh2.Tree.LeafCount, BufferPool); + Tree.CreateBinnedResources(BufferPool, mesh2.Tree.LeafCount, out var binnedResourcesBuffer, out var binnedResources); BinnedTest(() => { subtreeReferences.Count = 0; @@ -185,6 +242,8 @@ static void BinnedTest(Action function, string name, ref Tree tree) accumulator = ((accumulator << 7) | (accumulator >> (64 - 7))) + (ulong)hash; } Console.WriteLine($"{name} bounds hash: {accumulator}, A ({tree.Nodes[0].A.Min}, {tree.Nodes[0].B.Max}), B ({tree.Nodes[0].B.Min}, {tree.Nodes[0].B.Max})"); + var sah = tree.MeasureCostMetric(); + Console.WriteLine($"SAH: {sah}"); } } } From 56b7bae97ae8a72b0fba702deacb403562c1e169 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 17 Sep 2022 16:49:31 -0500 Subject: [PATCH 604/947] Revamped binned builder now initializes metanodes. Test now has nasty geometry. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 44 ++++++++++--------- .../SpecializedTests/TreeFiddlingTestDemo.cs | 39 +++++++++++----- 2 files changed, 53 insertions(+), 30 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 64dcc27d2..0c13b2386 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -35,17 +35,21 @@ static unsafe Int4 Truncate(Vector4 v) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - static void BuildParentNode(BoundingBox4 a, BoundingBox4 b, Buffer nodes, Buffer indices, int parentNodeIndex, int aCount, int bCount, out int aIndex, out int bIndex) + static void BuildNode(BoundingBox4 a, BoundingBox4 b, Buffer nodes, Buffer metanodes, Buffer indices, int nodeIndex, int parentNodeIndex, int childIndexInParent, int aCount, int bCount, out int aIndex, out int bIndex) { - ref var parentNode = ref nodes[0]; - aIndex = aCount == 1 ? indices[0] : parentNodeIndex + 1; - bIndex = bCount == 1 ? indices[^1] : parentNodeIndex + aCount;//parentNodeIndex + 1 + (aCount - 1) - parentNode.A = Unsafe.As(ref a); - parentNode.B = Unsafe.As(ref b); - parentNode.A.Index = aIndex; - parentNode.A.LeafCount = aCount; - parentNode.B.Index = bIndex; - parentNode.B.LeafCount = bCount; + ref var metanode = ref metanodes[0]; + metanode.Parent = parentNodeIndex; + metanode.IndexInParent = childIndexInParent; + metanode.RefineFlag = 0; + ref var node = ref nodes[0]; + aIndex = aCount == 1 ? indices[0] : nodeIndex + 1; + bIndex = bCount == 1 ? indices[^1] : nodeIndex + aCount;//parentNodeIndex + 1 + (aCount - 1) + node.A = Unsafe.As(ref a); + node.B = Unsafe.As(ref b); + node.A.Index = aIndex; + node.A.LeafCount = aCount; + node.B.Index = bIndex; + node.B.LeafCount = bCount; } struct Bins @@ -88,7 +92,7 @@ internal static float ComputeBoundsMetric(Vector4 min, Vector4 max) } } - static unsafe void BinnedBuilderInternal(Buffer indices, Buffer boundingBoxes, Buffer nodes, int parentNodeIndex, in Bins bins) + static unsafe void BinnedBuilderInternal(Buffer indices, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, in Bins bins) { var centroidMin = new Vector4(float.MaxValue); var centroidMax = new Vector4(float.MinValue); @@ -102,7 +106,7 @@ static unsafe void BinnedBuilderInternal(Buffer indices, Buffer= binCount); Debug.Assert(bins.BinBoundingBoxesY.Length >= binCount); Debug.Assert(bins.BinBoundingBoxesZ.Length >= binCount); @@ -136,11 +140,11 @@ static unsafe void BinnedBuilderInternal(Buffer indices, Buffer 1) - BinnedBuilderInternal(indices.Slice(countA), boundingBoxes.Slice(countA), nodes.Slice(1, countA - 1), aIndex, bins); + BinnedBuilderInternal(indices.Slice(countA), boundingBoxes.Slice(countA), nodes.Slice(1, countA - 1), metanodes.Slice(1, countA - 1), aIndex, nodeIndex, 0, bins); if (countB > 1) - BinnedBuilderInternal(indices.Slice(countA, countB), boundingBoxes.Slice(countA, countB), nodes.Slice(countA, countB - 1), bIndex, bins); + BinnedBuilderInternal(indices.Slice(countA, countB), boundingBoxes.Slice(countA, countB), nodes.Slice(countA, countB - 1), metanodes.Slice(countA, countB - 1), bIndex, nodeIndex, 1, bins); return; } @@ -368,16 +372,16 @@ static unsafe void BinnedBuilderInternal(Buffer indices, Buffer 1) - BinnedBuilderInternal(indices.Slice(aCount), boundingBoxes.Slice(aCount), nodes.Slice(1, aCount - 1), aIndex, bins); + BinnedBuilderInternal(indices.Slice(aCount), boundingBoxes.Slice(aCount), nodes.Slice(1, aCount - 1), metanodes.Slice(1, aCount - 1), aIndex, nodeIndex, 0, bins); if (bCount > 1) - BinnedBuilderInternal(indices.Slice(aCount, bCount), boundingBoxes.Slice(aCount, bCount), nodes.Slice(aCount, bCount - 1), bIndex, bins); + BinnedBuilderInternal(indices.Slice(aCount, bCount), boundingBoxes.Slice(aCount, bCount), nodes.Slice(aCount, bCount - 1), metanodes.Slice(aCount, bCount - 1), bIndex, nodeIndex, 1, bins); } } - public static unsafe void BinnedBuilder(Buffer indices, Buffer boundingBoxes, Buffer nodes, BufferPool pool) + public static unsafe void BinnedBuilder(Buffer indices, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, BufferPool pool) { var leafCount = indices.Length; Debug.Assert(boundingBoxes.Length >= leafCount, "The bounding boxes provided must cover the range of indices provided."); @@ -413,7 +417,7 @@ public static unsafe void BinnedBuilder(Buffer indices, Buffer bins.BinLeafCountsZ = new Buffer(binLeafCountsMemory + MaximumBinCount * 2, MaximumBinCount); //While we could avoid a recursive implementation, the overhead is low compared to the per-iteration cost. - BinnedBuilderInternal(indices, boundingBoxes.As(), nodes, 0, bins); + BinnedBuilderInternal(indices, boundingBoxes.As(), nodes, metanodes, 0, -1, -1, bins); } } diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index 9416b52de..0216cee17 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -118,6 +118,24 @@ Buffer CreateDeformedPlaneTriangles(int width, int height, Vector3 sca return triangles; } + + Buffer CreateRandomSoupTriangles(BoundingBox bounds, int triangleCount, float minimumSize, float maximumSize) + { + Random random = new Random(5); + BufferPool.Take(triangleCount, out var triangles); + for (int i = 0; i < triangleCount; ++i) + { + var startPoint = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) * (bounds.Max - bounds.Min) + bounds.Min; + var uniform = random.NextSingle(); + var size = MathF.Pow(uniform, 200) * (maximumSize - minimumSize) + minimumSize; + ref var triangle = ref triangles[i]; + triangle.A = startPoint + (2 * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) - Vector3.One) * size; + triangle.B = startPoint + (2 * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) - Vector3.One) * size; + triangle.C = startPoint + (2 * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) - Vector3.One) * size; + } + return triangles; + } + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(-10, 3, -10); @@ -125,20 +143,22 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0; Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(4, 1)); - var width = 768; - var height = 768; - var scale = new Vector3(1, 1, 1); + //Create a mesh. + var width = 256; + var height = 256; + var scale = new Vector3(1, 1, 1); //DemoMeshHelper.CreateDeformedPlane(width, height, (x, y) => new Vector3(x - width * scale.X * 0.5f, 2f * (float)(Math.Sin(x * 0.5f) * Math.Sin(y * 0.5f)), y - height * scale.Y * 0.5f), scale, BufferPool, out var mesh); //DemoMeshHelper.CreateDeformedPlane(width, height, (x, y) => new Vector3(x - width * scale.X * 0.5f, 0, y - height * scale.Y * 0.5f), scale, BufferPool, out var mesh); - //Create a wiggly mesh. - var triangles = CreateDeformedPlaneTriangles(width, height, scale); + //var triangles = CreateDeformedPlaneTriangles(width, height, scale); + var triangles = CreateRandomSoupTriangles(new BoundingBox(new(width / -2f, scale.Y * -2, height / -2f), new(width / 2f, scale.Y * 2, height / 2f)), (width - 1) * (height - 1) * 2, 0.5f, 50f); var mesh = new Mesh(triangles, Vector3.One, BufferPool); Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); Console.WriteLine($"node count: {mesh.Tree.NodeCount}"); + Console.WriteLine($"sweep SAH: {mesh.Tree.MeasureCostMetric()}, cache quality: {mesh.Tree.MeasureCacheQuality()}"); BufferPool.Take(mesh.Triangles.Length, out var leafBounds); BufferPool.Take(mesh.Triangles.Length, out var leafIndices); @@ -148,11 +168,11 @@ public override void Initialize(ContentArchive content, Camera camera) ref var bounds = ref leafBounds[i]; bounds.Min = Vector3.Min(t.A, Vector3.Min(t.B, t.C)); bounds.Max = Vector3.Max(t.A, Vector3.Max(t.B, t.C)); - leafIndices[i] = i; + leafIndices[i] = Tree.Encode(i); } BinnedTest(() => { - Tree.BinnedBuilder(leafIndices, leafBounds, mesh.Tree.Nodes, BufferPool); + Tree.BinnedBuilder(leafIndices, leafBounds, mesh.Tree.Nodes, mesh.Tree.Metanodes, BufferPool); }, "Revamp", ref mesh.Tree); @@ -166,7 +186,7 @@ public override void Initialize(ContentArchive content, Camera camera) subtreeReferences.Count = 0; treeletInternalNodes.Count = 0; mesh2.Tree.BinnedRefine(0, ref subtreeReferences, mesh2.Tree.LeafCount, ref treeletInternalNodes, ref binnedResources, BufferPool); - }, "Original", ref mesh.Tree); + }, "Original", ref mesh2.Tree); //RefitTest(() => mesh.Tree.Refit2(), "refit2", ref mesh.Tree); //RefitTest(() => mesh.Tree.Refit(), "Original", ref mesh.Tree); @@ -242,8 +262,7 @@ static void BinnedTest(Action function, string name, ref Tree tree) accumulator = ((accumulator << 7) | (accumulator >> (64 - 7))) + (ulong)hash; } Console.WriteLine($"{name} bounds hash: {accumulator}, A ({tree.Nodes[0].A.Min}, {tree.Nodes[0].B.Max}), B ({tree.Nodes[0].B.Min}, {tree.Nodes[0].B.Max})"); - var sah = tree.MeasureCostMetric(); - Console.WriteLine($"SAH: {sah}"); + Console.WriteLine($"SAH: {tree.MeasureCostMetric()}, cache quality: {tree.MeasureCacheQuality()}"); } } } From 2c2fb690f6214346ad7deaaf6f1afb6539a11547 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 17 Sep 2022 22:15:31 -0500 Subject: [PATCH 605/947] Nastified test case a bit more. Fixed off-by-one issue and added a microsweep builder. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 232 ++++++++++++++++-- .../SpecializedTests/TreeFiddlingTestDemo.cs | 47 +++- 2 files changed, 242 insertions(+), 37 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 0c13b2386..ceab37be4 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -15,6 +15,7 @@ namespace BepuPhysics.Trees { partial struct Tree { + const int MaximumBinCountRevamp = 128; [MethodImpl(MethodImplOptions.AggressiveInlining)] static unsafe Int4 Truncate(Vector4 v) { @@ -92,6 +93,173 @@ internal static float ComputeBoundsMetric(Vector4 min, Vector4 max) } } + struct BoundsComparerX : IComparerRef + { + public int Compare(ref BoundingBox4 a, ref BoundingBox4 b) + { + return (a.Min + a.Max).X.CompareTo((b.Min + b.Max).X); + } + } + struct BoundsComparerY : IComparerRef + { + public int Compare(ref BoundingBox4 a, ref BoundingBox4 b) + { + return (a.Min + a.Max).Y.CompareTo((b.Min + b.Max).Y); + } + } + struct BoundsComparerZ : IComparerRef + { + public int Compare(ref BoundingBox4 a, ref BoundingBox4 b) + { + return (a.Min + a.Max).Z.CompareTo((b.Min + b.Max).Z); + } + } + static unsafe void MicroSweepForBinnedBuilder(Vector4 centroidMin, Vector4 centroidMax, Buffer indices, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, Bins bins) + { + //This is a very small scale sweep build. + var leafCount = indices.Length; + if (leafCount == 2) + { + BuildNode(boundingBoxes[0], boundingBoxes[1], nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, 1, 1, out _, out _); + return; + } + var centroidSpan = centroidMax - centroidMin; + if (centroidSpan.X > centroidSpan.Y && centroidSpan.X > centroidSpan.Z) + { + var comparer = new BoundsComparerX(); + QuickSort.Sort(ref boundingBoxes[0], ref indices[0], 0, leafCount - 1, ref comparer); + } + else if (centroidSpan.Y > centroidSpan.Z) + { + var comparer = new BoundsComparerY(); + QuickSort.Sort(ref boundingBoxes[0], ref indices[0], 0, leafCount - 1, ref comparer); + } + else + { + var comparer = new BoundsComparerZ(); + QuickSort.Sort(ref boundingBoxes[0], ref indices[0], 0, leafCount - 1, ref comparer); + } + + Debug.Assert(leafCount <= MaximumBinCountRevamp, "We're reusing the bin resources under the assumption that this is only ever called when there are less leaves than maximum bins."); + //Identify the split index by examining the SAH of very split option. + //Premerge from left to right so we have a sorta-summed area table to cheaply look up all possible child A bounds as we scan. + bins.BinBoundingBoxesScanX[0] = boundingBoxes[0]; + for (int i = 1; i < leafCount; ++i) + { + ref var previousScanBounds = ref bins.BinBoundingBoxesScanX[i - 1]; + ref var scanBounds = ref bins.BinBoundingBoxesScanX[i]; + ref var bounds = ref boundingBoxes[i]; + scanBounds.Min = Vector4.Min(bounds.Min, previousScanBounds.Min); + scanBounds.Max = Vector4.Max(bounds.Max, previousScanBounds.Max); + } + + float bestSAH = float.MaxValue; + int bestSplit = 1; + //The split index is going to end up in child B. + var lastLeafIndex = leafCount - 1; + BoundingBox4 accumulatedBoundingBoxB = boundingBoxes[lastLeafIndex]; + Unsafe.SkipInit(out BoundingBox4 bestBoundsB); + int accumulatedLeafCountB = 1; + for (int splitIndexCandidate = lastLeafIndex; splitIndexCandidate >= 1; --splitIndexCandidate) + { + var previousIndex = splitIndexCandidate - 1; + var leafCountA = leafCount - accumulatedLeafCountB; + var sahCandidate = ComputeBoundsMetric(bins.BinBoundingBoxesScanX[previousIndex]) * leafCountA + ComputeBoundsMetric(accumulatedBoundingBoxB) * accumulatedLeafCountB; + if (sahCandidate < bestSAH) + { + bestSAH = sahCandidate; + bestSplit = splitIndexCandidate; + bestBoundsB = accumulatedBoundingBoxB; + } + ref var bounds = ref boundingBoxes[splitIndexCandidate - 1]; + accumulatedBoundingBoxB.Min = Vector4.Min(bounds.Min, accumulatedBoundingBoxB.Min); + accumulatedBoundingBoxB.Max = Vector4.Max(bounds.Max, accumulatedBoundingBoxB.Max); + ++accumulatedLeafCountB; + } + + var bestBoundsA = bins.BinBoundingBoxesScanX[bestSplit - 1]; + + var aCount = bestSplit; + var bCount = leafCount - bestSplit; + { + //if (leafCount == 2) + //{ + // Debug.Assert(bestBoundsA.Min == boundingBoxes[0].Min); + // Debug.Assert(bestBoundsA.Max == boundingBoxes[0].Max); + // Debug.Assert(bestBoundsB.Min == boundingBoxes[1].Min); + // Debug.Assert(bestBoundsB.Max == boundingBoxes[1].Max); + //} + //for (int i = 0; i < leafCount; ++i) + //{ + // var bounds = i < bestSplit ? bestBoundsA : bestBoundsB; + // var containedA = Vector128.LessThanOrEqual(boundingBoxes[i].Max.AsVector128(), bounds.Max.AsVector128()) & Vector128.GreaterThanOrEqual(boundingBoxes[i].Min.AsVector128(), bounds.Min.AsVector128()); + // var mask = containedA.ExtractMostSignificantBits() & 0b111; + // Debug.Assert(mask == 0b111, "All children must be contained within their parent."); + //} + //BoundingBox4 debugBoundsA = boundingBoxes[0]; + //for (int i = 1; i < aCount; ++i) + //{ + // ref var bounds = ref boundingBoxes[i]; + // debugBoundsA.Min = Vector4.Min(debugBoundsA.Min, bounds.Min); + // debugBoundsA.Max = Vector4.Max(debugBoundsA.Max, bounds.Max); + //} + //BoundingBox4 debugBoundsB = boundingBoxes[aCount]; + //for (int i = aCount + 1; i < leafCount; ++i) + //{ + // ref var bounds = ref boundingBoxes[i]; + // debugBoundsB.Min = Vector4.Min(debugBoundsB.Min, bounds.Min); + // debugBoundsB.Max = Vector4.Max(debugBoundsB.Max, bounds.Max); + //} + //Debug.Assert(bestBoundsA.Min == debugBoundsA.Min); + //Debug.Assert(bestBoundsA.Max == debugBoundsA.Max); + //Debug.Assert(bestBoundsB.Min == debugBoundsB.Min); + //Debug.Assert(bestBoundsB.Max == debugBoundsB.Max); + BuildNode(bestBoundsA, bestBoundsB, nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, aCount, bCount, out var aIndex, out var bIndex); + if (aCount > 1) + { + var aBounds = boundingBoxes.Slice(aCount); + BoundingBox4 centroidBoundsA = aBounds[0]; + for (int i = 1; i < aCount; ++i) + { + ref var bounds = ref aBounds[i]; + centroidBoundsA.Min = Vector4.Min(centroidBoundsA.Min, bounds.Min); + centroidBoundsA.Max = Vector4.Max(centroidBoundsA.Max, bounds.Max); + } + MicroSweepForBinnedBuilder(centroidBoundsA.Min, centroidBoundsA.Max, indices.Slice(aCount), aBounds, nodes.Slice(1, aCount - 1), metanodes.Slice(1, aCount - 1), aIndex, nodeIndex, 0, bins); + } + if (bCount > 1) + { + var bBounds = boundingBoxes.Slice(aCount, bCount); + BoundingBox4 centroidBoundsB = bBounds[0]; + for (int i = 0; i < bCount; ++i) + { + ref var bounds = ref bBounds[i]; + centroidBoundsB.Min = Vector4.Min(centroidBoundsB.Min, bounds.Min); + centroidBoundsB.Max = Vector4.Max(centroidBoundsB.Max, bounds.Max); + } + MicroSweepForBinnedBuilder(centroidBoundsB.Min, centroidBoundsB.Max, indices.Slice(aCount, bCount), bBounds, nodes.Slice(aCount, bCount - 1), metanodes.Slice(aCount, bCount - 1), bIndex, nodeIndex, 1, bins); + } + } + } + + //static void ValidateNode(int nodeIndex, Buffer nodes, out Vector3 min, out Vector3 max) + //{ + // if (nodes[nodeIndex].A.Index >= 0) + // { + // ValidateNode(nodes[nodeIndex].A.Index, nodes, out var aMin, out var aMax); + // Debug.Assert(nodes[nodeIndex].A.Min == aMin); + // Debug.Assert(nodes[nodeIndex].A.Max == aMax); + // } + // if (nodes[nodeIndex].B.Index >= 0) + // { + // ValidateNode(nodes[nodeIndex].B.Index, nodes, out var bMin, out var bMax); + // Debug.Assert(nodes[nodeIndex].B.Min == bMin); + // Debug.Assert(nodes[nodeIndex].B.Max == bMax); + // } + // min = Vector3.Min(nodes[nodeIndex].A.Min, nodes[nodeIndex].A.Min); + // max = Vector3.Max(nodes[nodeIndex].B.Max, nodes[nodeIndex].B.Max); + //} + static unsafe void BinnedBuilderInternal(Buffer indices, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, in Bins bins) { var centroidMin = new Vector4(float.MaxValue); @@ -106,16 +274,8 @@ static unsafe void BinnedBuilderInternal(Buffer indices, Buffer= binCount); - Debug.Assert(bins.BinBoundingBoxesY.Length >= binCount); - Debug.Assert(bins.BinBoundingBoxesZ.Length >= binCount); var centroidSpan = centroidMax - centroidMin; - var offsetToBinIndex = new Vector4(binCount) / centroidSpan; - //Avoid letting NaNs into the offsetToBinIndex scale. var axisIsDegenerate = Vector128.LessThanOrEqual(centroidSpan.AsVector128(), Vector128.Create(1e-12f)); - offsetToBinIndex = Vector128.ConditionalSelect(axisIsDegenerate, Vector128.Zero, offsetToBinIndex.AsVector128()).AsVector4(); - if ((Vector128.ExtractMostSignificantBits(axisIsDegenerate) & 0b111) == 0b111) { //This node is completely degenerate; there is no 'good' ordering of the children. Pick a split in the middle and shrug. @@ -147,6 +307,26 @@ static unsafe void BinnedBuilderInternal(Buffer indices, Buffer= binCount); + Debug.Assert(bins.BinBoundingBoxesY.Length >= binCount); + Debug.Assert(bins.BinBoundingBoxesZ.Length >= binCount); + + var offsetToBinIndex = new Vector4(binCount) / centroidSpan; + //Avoid letting NaNs into the offsetToBinIndex scale. + offsetToBinIndex = Vector128.ConditionalSelect(axisIsDegenerate, Vector128.Zero, offsetToBinIndex.AsVector128()).AsVector4(); + for (int i = 0; i < binCount; ++i) { @@ -262,18 +442,18 @@ static unsafe void BinnedBuilderInternal(Buffer indices, Buffer indices, Buffer //If there's only one leaf, the tree has a special format: the root node has only one child. ref var root = ref nodes[0]; root.A.Min = boundingBoxes[0].Min; - root.A.Index = Encode(indices[0]); + root.A.Index = indices[0]; //Node that we assume the indices are already encoded. This function works with subtree refinements as well which can manage either leaves or internals. root.A.Max = boundingBoxes[0].Max; root.A.LeafCount = 1; root.B = default; @@ -402,19 +582,19 @@ public static unsafe void BinnedBuilder(Buffer indices, Buffer boundingBoxes = boundingBoxes.Slice(indices.Length); nodes = nodes.Slice(leafCount - 1); - var binBoundsMemory = stackalloc BoundingBox4[MaximumBinCount * 6]; + var binBoundsMemory = stackalloc BoundingBox4[MaximumBinCountRevamp * 6]; Bins bins; - bins.BinBoundingBoxesX = new Buffer(binBoundsMemory, MaximumBinCount); - bins.BinBoundingBoxesY = new Buffer(binBoundsMemory + MaximumBinCount, MaximumBinCount); - bins.BinBoundingBoxesZ = new Buffer(binBoundsMemory + MaximumBinCount * 2, MaximumBinCount); - bins.BinBoundingBoxesScanX = new Buffer(binBoundsMemory + MaximumBinCount * 3, MaximumBinCount); - bins.BinBoundingBoxesScanY = new Buffer(binBoundsMemory + MaximumBinCount * 4, MaximumBinCount); - bins.BinBoundingBoxesScanZ = new Buffer(binBoundsMemory + MaximumBinCount * 5, MaximumBinCount); + bins.BinBoundingBoxesX = new Buffer(binBoundsMemory, MaximumBinCountRevamp); + bins.BinBoundingBoxesY = new Buffer(binBoundsMemory + MaximumBinCountRevamp, MaximumBinCountRevamp); + bins.BinBoundingBoxesZ = new Buffer(binBoundsMemory + MaximumBinCountRevamp * 2, MaximumBinCountRevamp); + bins.BinBoundingBoxesScanX = new Buffer(binBoundsMemory + MaximumBinCountRevamp * 3, MaximumBinCountRevamp); + bins.BinBoundingBoxesScanY = new Buffer(binBoundsMemory + MaximumBinCountRevamp * 4, MaximumBinCountRevamp); + bins.BinBoundingBoxesScanZ = new Buffer(binBoundsMemory + MaximumBinCountRevamp * 5, MaximumBinCountRevamp); - var binLeafCountsMemory = stackalloc int[MaximumBinCount * 3]; - bins.BinLeafCountsX = new Buffer(binLeafCountsMemory, MaximumBinCount); - bins.BinLeafCountsY = new Buffer(binLeafCountsMemory + MaximumBinCount, MaximumBinCount); - bins.BinLeafCountsZ = new Buffer(binLeafCountsMemory + MaximumBinCount * 2, MaximumBinCount); + var binLeafCountsMemory = stackalloc int[MaximumBinCountRevamp * 3]; + bins.BinLeafCountsX = new Buffer(binLeafCountsMemory, MaximumBinCountRevamp); + bins.BinLeafCountsY = new Buffer(binLeafCountsMemory + MaximumBinCountRevamp, MaximumBinCountRevamp); + bins.BinLeafCountsZ = new Buffer(binLeafCountsMemory + MaximumBinCountRevamp * 2, MaximumBinCountRevamp); //While we could avoid a recursive implementation, the overhead is low compared to the per-iteration cost. BinnedBuilderInternal(indices, boundingBoxes.As(), nodes, metanodes, 0, -1, -1, bins); diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index 0216cee17..6278dcc78 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -17,6 +17,7 @@ using BepuPhysics.Collidables; using static System.Formats.Asn1.AsnWriter; using static OpenTK.Graphics.OpenGL.GL; +using System.Xml.Linq; namespace Demos.SpecializedTests { @@ -109,7 +110,7 @@ Buffer CreateDeformedPlaneTriangles(int width, int height, Vector3 sca BufferPool.Return(ref vertices); //Scramble the heck out of its triangles. var random = new Random(5); - for (int index = 0; index < triangles.Length; ++index) + for (int index = 0; index < triangles.Length - 1; ++index) { ref var a = ref triangles[index]; ref var b = ref triangles[random.Next(index + 1, triangles.Length)]; @@ -125,13 +126,24 @@ Buffer CreateRandomSoupTriangles(BoundingBox bounds, int triangleCount BufferPool.Take(triangleCount, out var triangles); for (int i = 0; i < triangleCount; ++i) { - var startPoint = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) * (bounds.Max - bounds.Min) + bounds.Min; - var uniform = random.NextSingle(); - var size = MathF.Pow(uniform, 200) * (maximumSize - minimumSize) + minimumSize; + var startPoint = new Vector3(random.NextSingle() * random.NextSingle(), random.NextSingle(), random.NextSingle() * random.NextSingle()) * (bounds.Max - bounds.Min) + bounds.Min; + var size = new Vector3(MathF.Pow(random.NextSingle(), 200), MathF.Pow(random.NextSingle(), 200), MathF.Pow(random.NextSingle(), 200)) * (maximumSize - minimumSize) + new Vector3(minimumSize); + ref var triangle = ref triangles[i]; - triangle.A = startPoint + (2 * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) - Vector3.One) * size; - triangle.B = startPoint + (2 * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) - Vector3.One) * size; - triangle.C = startPoint + (2 * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) - Vector3.One) * size; + triangle.A = (2 * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) - Vector3.One) * size; + triangle.B = (2 * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) - Vector3.One) * size; + triangle.C = (2 * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) - Vector3.One) * size; + + if (random.NextSingle() < 0.75f) + { + var rotation = Quaternion.CreateFromAxisAngle(Vector3.Normalize(new Vector3(0.0001f) + new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle())), random.NextSingle() * MathF.PI * 2); + triangle.A = Vector3.Transform(triangle.A, rotation); + triangle.B = Vector3.Transform(triangle.B, rotation); + triangle.C = Vector3.Transform(triangle.C, rotation); + } + triangle.A += startPoint; + triangle.B += startPoint; + triangle.C += startPoint; } return triangles; } @@ -145,20 +157,20 @@ public override void Initialize(ContentArchive content, Camera camera) Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(4, 1)); //Create a mesh. - var width = 256; - var height = 256; + var width = 768; + var height = 768; var scale = new Vector3(1, 1, 1); //DemoMeshHelper.CreateDeformedPlane(width, height, (x, y) => new Vector3(x - width * scale.X * 0.5f, 2f * (float)(Math.Sin(x * 0.5f) * Math.Sin(y * 0.5f)), y - height * scale.Y * 0.5f), scale, BufferPool, out var mesh); //DemoMeshHelper.CreateDeformedPlane(width, height, (x, y) => new Vector3(x - width * scale.X * 0.5f, 0, y - height * scale.Y * 0.5f), scale, BufferPool, out var mesh); //var triangles = CreateDeformedPlaneTriangles(width, height, scale); - var triangles = CreateRandomSoupTriangles(new BoundingBox(new(width / -2f, scale.Y * -2, height / -2f), new(width / 2f, scale.Y * 2, height / 2f)), (width - 1) * (height - 1) * 2, 0.5f, 50f); + var triangles = CreateRandomSoupTriangles(new BoundingBox(new(width / -2f, scale.Y * -2, height / -2f), new(width / 2f, scale.Y * 2, height / 2f)), (width - 1) * (height - 1) * 2, 0.5f, 100f); var mesh = new Mesh(triangles, Vector3.One, BufferPool); - Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); Console.WriteLine($"node count: {mesh.Tree.NodeCount}"); Console.WriteLine($"sweep SAH: {mesh.Tree.MeasureCostMetric()}, cache quality: {mesh.Tree.MeasureCacheQuality()}"); + Console.WriteLine($"sweep bounds: A ({mesh.Tree.Nodes[0].A.Min}, {mesh.Tree.Nodes[0].B.Max}), B ({mesh.Tree.Nodes[0].B.Min}, {mesh.Tree.Nodes[0].B.Max})"); BufferPool.Take(mesh.Triangles.Length, out var leafBounds); BufferPool.Take(mesh.Triangles.Length, out var leafIndices); @@ -173,8 +185,19 @@ public override void Initialize(ContentArchive content, Camera camera) BinnedTest(() => { Tree.BinnedBuilder(leafIndices, leafBounds, mesh.Tree.Nodes, mesh.Tree.Metanodes, BufferPool); + for (int i = 0; i < mesh.Tree.NodeCount; ++i) + { + ref var node = ref mesh.Tree.Nodes[i]; + ref var a = ref node.A; + ref var b = ref node.B; + if (a.Index < 0) + mesh.Tree.Leaves[Tree.Encode(a.Index)] = new Leaf(i, 0); + if (b.Index < 0) + mesh.Tree.Leaves[Tree.Encode(b.Index)] = new Leaf(i, 1); + } }, "Revamp", ref mesh.Tree); + Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); var mesh2 = new Mesh(triangles, Vector3.One, BufferPool); @@ -193,6 +216,8 @@ public override void Initialize(ContentArchive content, Camera camera) //SelfTest((ref OverlapHandler handler) => mesh.Tree.GetSelfOverlapsContiguousPrepass(ref handler, BufferPool), mesh.Tree.LeafCount, "Prepass"); //SelfTest((ref OverlapHandler handler) => mesh.Tree.GetSelfOverlaps(ref handler), mesh.Tree.LeafCount, "Original"); + + Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 10, 0), 1, Simulation.Shapes, new Sphere(0.5f))); } delegate void TestFunction(ref OverlapHandler handler); From 273461034d9ed2fa495ad0d49945c409bd53312b Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 18 Sep 2022 17:42:42 -0500 Subject: [PATCH 606/947] Slight sort improvement. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 16 ++++---------- .../SpecializedTests/TreeFiddlingTestDemo.cs | 22 +++++++++---------- 2 files changed, 15 insertions(+), 23 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index ceab37be4..42dbd1e6a 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -9,6 +9,7 @@ using System.Numerics; using System.Runtime.CompilerServices; using System.Runtime.Intrinsics; +using System.Runtime.Intrinsics.X86; using System.Threading.Tasks.Sources; namespace BepuPhysics.Trees @@ -95,24 +96,15 @@ internal static float ComputeBoundsMetric(Vector4 min, Vector4 max) struct BoundsComparerX : IComparerRef { - public int Compare(ref BoundingBox4 a, ref BoundingBox4 b) - { - return (a.Min + a.Max).X.CompareTo((b.Min + b.Max).X); - } + public int Compare(ref BoundingBox4 a, ref BoundingBox4 b) => (a.Min.X + a.Max.X) > (b.Min.X + b.Max.X) ? -1 : 1; } struct BoundsComparerY : IComparerRef { - public int Compare(ref BoundingBox4 a, ref BoundingBox4 b) - { - return (a.Min + a.Max).Y.CompareTo((b.Min + b.Max).Y); - } + public int Compare(ref BoundingBox4 a, ref BoundingBox4 b) => (a.Min.Y + a.Max.Y) > (b.Min.Y + b.Max.Y) ? -1 : 1; } struct BoundsComparerZ : IComparerRef { - public int Compare(ref BoundingBox4 a, ref BoundingBox4 b) - { - return (a.Min + a.Max).Z.CompareTo((b.Min + b.Max).Z); - } + public int Compare(ref BoundingBox4 a, ref BoundingBox4 b) => (a.Min.Z + a.Max.Z) > (b.Min.Z + b.Max.Z) ? -1 : 1; } static unsafe void MicroSweepForBinnedBuilder(Vector4 centroidMin, Vector4 centroidMax, Buffer indices, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, Bins bins) { diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index 6278dcc78..299dcbd6e 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -199,17 +199,17 @@ public override void Initialize(ContentArchive content, Camera camera) Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); - var mesh2 = new Mesh(triangles, Vector3.One, BufferPool); - - QuickList subtreeReferences = new(mesh2.Tree.LeafCount, BufferPool); - QuickList treeletInternalNodes = new(mesh2.Tree.LeafCount, BufferPool); - Tree.CreateBinnedResources(BufferPool, mesh2.Tree.LeafCount, out var binnedResourcesBuffer, out var binnedResources); - BinnedTest(() => - { - subtreeReferences.Count = 0; - treeletInternalNodes.Count = 0; - mesh2.Tree.BinnedRefine(0, ref subtreeReferences, mesh2.Tree.LeafCount, ref treeletInternalNodes, ref binnedResources, BufferPool); - }, "Original", ref mesh2.Tree); + //var mesh2 = new Mesh(triangles, Vector3.One, BufferPool); + + //QuickList subtreeReferences = new(mesh2.Tree.LeafCount, BufferPool); + //QuickList treeletInternalNodes = new(mesh2.Tree.LeafCount, BufferPool); + //Tree.CreateBinnedResources(BufferPool, mesh2.Tree.LeafCount, out var binnedResourcesBuffer, out var binnedResources); + //BinnedTest(() => + //{ + // subtreeReferences.Count = 0; + // treeletInternalNodes.Count = 0; + // mesh2.Tree.BinnedRefine(0, ref subtreeReferences, mesh2.Tree.LeafCount, ref treeletInternalNodes, ref binnedResources, BufferPool); + //}, "Original", ref mesh2.Tree); //RefitTest(() => mesh.Tree.Refit2(), "refit2", ref mesh.Tree); //RefitTest(() => mesh.Tree.Refit(), "Original", ref mesh.Tree); From 8a172ed6bc7841990d122f4854e65891e2618886 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 18 Sep 2022 18:12:29 -0500 Subject: [PATCH 607/947] Purged pessimisation. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 19 ++++--------------- 1 file changed, 4 insertions(+), 15 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 42dbd1e6a..d0f2d9586 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -68,30 +68,19 @@ struct Bins public Buffer BinLeafCountsZ; } - [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static float ComputeBoundsMetric(BoundingBox4 bounds) { return ComputeBoundsMetric(bounds.Min, bounds.Max); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static float ComputeBoundsMetric(Vector4 min, Vector4 max) { //Note that we just use the SAH. While we are primarily interested in volume queries for the purposes of collision detection, the topological difference //between a volume heuristic and surface area heuristic isn't huge. There is, however, one big annoying issue that volume heuristics run into: //all bounding boxes with one extent equal to zero have zero cost. Surface area approaches avoid this hole simply. - if (Vector128.IsHardwareAccelerated) - { - var span = max - min; - var shuffled = Vector128.Shuffle(span.AsVector128(), Vector128.Create(1, 2, 0, 0)); - var zeroedUpper = shuffled.WithElement(3, 0); - return Vector128.Dot(span.AsVector128(), zeroedUpper); - } - else - { - var offset = max - min; - //Note that this is merely proportional to surface area. Being scaled by a constant factor is irrelevant. - return offset.X * offset.Y + offset.Y * offset.Z + offset.Z * offset.X; - } + var offset = max - min; + //Note that this is merely proportional to surface area. Being scaled by a constant factor is irrelevant. + return offset.X * offset.Y + offset.Y * offset.Z + offset.Z * offset.X; + } struct BoundsComparerX : IComparerRef From 902626a40a55c95087922d8b4462b7b8827b59a0 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 18 Sep 2022 19:18:03 -0500 Subject: [PATCH 608/947] Single axis variant. Faster but worse SAH. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 404 ++++++++++++++++++ .../SpecializedTests/TreeFiddlingTestDemo.cs | 15 + 2 files changed, 419 insertions(+) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index d0f2d9586..80785d2ef 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -581,5 +581,409 @@ public static unsafe void BinnedBuilder(Buffer indices, Buffer BinnedBuilderInternal(indices, boundingBoxes.As(), nodes, metanodes, 0, -1, -1, bins); } + + static unsafe void MicroSweepForBinnedBuilderSingleAxis(Vector4 centroidMin, Vector4 centroidMax, Buffer indices, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, BinsSingleAxis bins) + { + //This is a very small scale sweep build. + var leafCount = indices.Length; + if (leafCount == 2) + { + BuildNode(boundingBoxes[0], boundingBoxes[1], nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, 1, 1, out _, out _); + return; + } + var centroidSpan = centroidMax - centroidMin; + if (centroidSpan.X > centroidSpan.Y && centroidSpan.X > centroidSpan.Z) + { + var comparer = new BoundsComparerX(); + QuickSort.Sort(ref boundingBoxes[0], ref indices[0], 0, leafCount - 1, ref comparer); + } + else if (centroidSpan.Y > centroidSpan.Z) + { + var comparer = new BoundsComparerY(); + QuickSort.Sort(ref boundingBoxes[0], ref indices[0], 0, leafCount - 1, ref comparer); + } + else + { + var comparer = new BoundsComparerZ(); + QuickSort.Sort(ref boundingBoxes[0], ref indices[0], 0, leafCount - 1, ref comparer); + } + + Debug.Assert(leafCount <= MaximumBinCountRevamp, "We're reusing the bin resources under the assumption that this is only ever called when there are less leaves than maximum bins."); + //Identify the split index by examining the SAH of very split option. + //Premerge from left to right so we have a sorta-summed area table to cheaply look up all possible child A bounds as we scan. + bins.BinBoundingBoxesScan[0] = boundingBoxes[0]; + for (int i = 1; i < leafCount; ++i) + { + ref var previousScanBounds = ref bins.BinBoundingBoxesScan[i - 1]; + ref var scanBounds = ref bins.BinBoundingBoxesScan[i]; + ref var bounds = ref boundingBoxes[i]; + scanBounds.Min = Vector4.Min(bounds.Min, previousScanBounds.Min); + scanBounds.Max = Vector4.Max(bounds.Max, previousScanBounds.Max); + } + + float bestSAH = float.MaxValue; + int bestSplit = 1; + //The split index is going to end up in child B. + var lastLeafIndex = leafCount - 1; + BoundingBox4 accumulatedBoundingBoxB = boundingBoxes[lastLeafIndex]; + Unsafe.SkipInit(out BoundingBox4 bestBoundsB); + int accumulatedLeafCountB = 1; + for (int splitIndexCandidate = lastLeafIndex; splitIndexCandidate >= 1; --splitIndexCandidate) + { + var previousIndex = splitIndexCandidate - 1; + var leafCountA = leafCount - accumulatedLeafCountB; + var sahCandidate = ComputeBoundsMetric(bins.BinBoundingBoxesScan[previousIndex]) * leafCountA + ComputeBoundsMetric(accumulatedBoundingBoxB) * accumulatedLeafCountB; + if (sahCandidate < bestSAH) + { + bestSAH = sahCandidate; + bestSplit = splitIndexCandidate; + bestBoundsB = accumulatedBoundingBoxB; + } + ref var bounds = ref boundingBoxes[splitIndexCandidate - 1]; + accumulatedBoundingBoxB.Min = Vector4.Min(bounds.Min, accumulatedBoundingBoxB.Min); + accumulatedBoundingBoxB.Max = Vector4.Max(bounds.Max, accumulatedBoundingBoxB.Max); + ++accumulatedLeafCountB; + } + + var bestBoundsA = bins.BinBoundingBoxesScan[bestSplit - 1]; + + var aCount = bestSplit; + var bCount = leafCount - bestSplit; + { + //if (leafCount == 2) + //{ + // Debug.Assert(bestBoundsA.Min == boundingBoxes[0].Min); + // Debug.Assert(bestBoundsA.Max == boundingBoxes[0].Max); + // Debug.Assert(bestBoundsB.Min == boundingBoxes[1].Min); + // Debug.Assert(bestBoundsB.Max == boundingBoxes[1].Max); + //} + //for (int i = 0; i < leafCount; ++i) + //{ + // var bounds = i < bestSplit ? bestBoundsA : bestBoundsB; + // var containedA = Vector128.LessThanOrEqual(boundingBoxes[i].Max.AsVector128(), bounds.Max.AsVector128()) & Vector128.GreaterThanOrEqual(boundingBoxes[i].Min.AsVector128(), bounds.Min.AsVector128()); + // var mask = containedA.ExtractMostSignificantBits() & 0b111; + // Debug.Assert(mask == 0b111, "All children must be contained within their parent."); + //} + //BoundingBox4 debugBoundsA = boundingBoxes[0]; + //for (int i = 1; i < aCount; ++i) + //{ + // ref var bounds = ref boundingBoxes[i]; + // debugBoundsA.Min = Vector4.Min(debugBoundsA.Min, bounds.Min); + // debugBoundsA.Max = Vector4.Max(debugBoundsA.Max, bounds.Max); + //} + //BoundingBox4 debugBoundsB = boundingBoxes[aCount]; + //for (int i = aCount + 1; i < leafCount; ++i) + //{ + // ref var bounds = ref boundingBoxes[i]; + // debugBoundsB.Min = Vector4.Min(debugBoundsB.Min, bounds.Min); + // debugBoundsB.Max = Vector4.Max(debugBoundsB.Max, bounds.Max); + //} + //Debug.Assert(bestBoundsA.Min == debugBoundsA.Min); + //Debug.Assert(bestBoundsA.Max == debugBoundsA.Max); + //Debug.Assert(bestBoundsB.Min == debugBoundsB.Min); + //Debug.Assert(bestBoundsB.Max == debugBoundsB.Max); + BuildNode(bestBoundsA, bestBoundsB, nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, aCount, bCount, out var aIndex, out var bIndex); + if (aCount > 1) + { + var aBounds = boundingBoxes.Slice(aCount); + BoundingBox4 centroidBoundsA = aBounds[0]; + for (int i = 1; i < aCount; ++i) + { + ref var bounds = ref aBounds[i]; + centroidBoundsA.Min = Vector4.Min(centroidBoundsA.Min, bounds.Min); + centroidBoundsA.Max = Vector4.Max(centroidBoundsA.Max, bounds.Max); + } + MicroSweepForBinnedBuilderSingleAxis(centroidBoundsA.Min, centroidBoundsA.Max, indices.Slice(aCount), aBounds, nodes.Slice(1, aCount - 1), metanodes.Slice(1, aCount - 1), aIndex, nodeIndex, 0, bins); + } + if (bCount > 1) + { + var bBounds = boundingBoxes.Slice(aCount, bCount); + BoundingBox4 centroidBoundsB = bBounds[0]; + for (int i = 0; i < bCount; ++i) + { + ref var bounds = ref bBounds[i]; + centroidBoundsB.Min = Vector4.Min(centroidBoundsB.Min, bounds.Min); + centroidBoundsB.Max = Vector4.Max(centroidBoundsB.Max, bounds.Max); + } + MicroSweepForBinnedBuilderSingleAxis(centroidBoundsB.Min, centroidBoundsB.Max, indices.Slice(aCount, bCount), bBounds, nodes.Slice(aCount, bCount - 1), metanodes.Slice(aCount, bCount - 1), bIndex, nodeIndex, 1, bins); + } + } + } + + static unsafe void BinnedBuilderInternalSingleAxis(Buffer indices, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, in BinsSingleAxis bins) + { + var centroidMin = new Vector4(float.MaxValue); + var centroidMax = new Vector4(float.MinValue); + var leafCount = indices.Length; + + for (int i = 0; i < leafCount; ++i) + { + ref var box = ref boundingBoxes[i]; + //Note that centroids never bother scaling by 0.5. It's fine as long as we're consistent. + var centroid = box.Min + box.Max; + centroidMin = Vector4.Min(centroidMin, centroid); + centroidMax = Vector4.Max(centroidMax, centroid); + } + var centroidSpan = centroidMax - centroidMin; + var axisIsDegenerate = Vector128.LessThanOrEqual(centroidSpan.AsVector128(), Vector128.Create(1e-12f)); + if ((Vector128.ExtractMostSignificantBits(axisIsDegenerate) & 0b111) == 0b111) + { + //This node is completely degenerate; there is no 'good' ordering of the children. Pick a split in the middle and shrug. + //This shouldn't happen unless something is badly wrong with the input; no point in optimizing it. + var countA = indices.Length / 2; + var countB = indices.Length - countA; + //Still have to compute the child bounding boxes, because the centroid bounds span being zero doesn't imply that the full bounds are zero. + BoundingBox4 boundsA, boundsB; + boundsA.Min = new Vector4(float.MaxValue); + boundsA.Max = new Vector4(float.MinValue); + boundsB.Min = new Vector4(float.MaxValue); + boundsB.Max = new Vector4(float.MinValue); + for (int i = 0; i < countA; ++i) + { + ref var bounds = ref boundingBoxes[i]; + boundsA.Min = Vector4.Min(bounds.Min, boundsA.Min); + boundsA.Max = Vector4.Max(bounds.Max, boundsA.Max); + } + for (int i = countA; i < indices.Length; ++i) + { + ref var bounds = ref boundingBoxes[i]; + boundsB.Min = Vector4.Min(bounds.Min, boundsB.Min); + boundsB.Max = Vector4.Max(bounds.Max, boundsB.Max); + } + BuildNode(boundsA, boundsB, nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, countA, countB, out var aIndex, out var bIndex); + if (countA > 1) + BinnedBuilderInternalSingleAxis(indices.Slice(countA), boundingBoxes.Slice(countA), nodes.Slice(1, countA - 1), metanodes.Slice(1, countA - 1), aIndex, nodeIndex, 0, bins); + if (countB > 1) + BinnedBuilderInternalSingleAxis(indices.Slice(countA, countB), boundingBoxes.Slice(countA, countB), nodes.Slice(countA, countB - 1), metanodes.Slice(countA, countB - 1), bIndex, nodeIndex, 1, bins); + return; + } + if (leafCount == 2) + { + BuildNode(boundingBoxes[0], boundingBoxes[1], nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, 1, 1, out _, out _); + return; + } + + if (leafCount < 8) + { + MicroSweepForBinnedBuilderSingleAxis(centroidMin, centroidMax, indices, boundingBoxes, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, bins); + return; + } + + var binCount = Math.Min(MaximumBinCountRevamp, Math.Max((int)(leafCount * 0.1f), 4)); + Debug.Assert(bins.BinBoundingBoxes.Length >= binCount); + var offsetToBinIndex = new Vector4(binCount) / centroidSpan; + //Avoid letting NaNs into the offsetToBinIndex scale. + offsetToBinIndex = Vector128.ConditionalSelect(axisIsDegenerate, Vector128.Zero, offsetToBinIndex.AsVector128()).AsVector4(); + BoundingBox4 bestBoundsA, bestBoundsB; + int aCount, bCount; + //Use generic specialization to avoid branching on every bin selection. + if (centroidSpan.X > centroidSpan.Y && centroidSpan.X > centroidSpan.Z) + { + var indexer = new BinIndexerX { Min = centroidMin.X, MaximumIndex = binCount - 1, Scale = offsetToBinIndex.X }; + ComputeBinnedSplit(indices, boundingBoxes, binCount, bins, indexer, out bestBoundsA, out bestBoundsB, out aCount, out bCount); + } + else if (centroidSpan.Y > centroidSpan.Z) + { + var indexer = new BinIndexerY { Min = centroidMin.Y, MaximumIndex = binCount - 1, Scale = offsetToBinIndex.Y }; + ComputeBinnedSplit(indices, boundingBoxes, binCount, bins, indexer, out bestBoundsA, out bestBoundsB, out aCount, out bCount); + } + else + { + var indexer = new BinIndexerZ { Min = centroidMin.Z, MaximumIndex = binCount - 1, Scale = offsetToBinIndex.Z }; + ComputeBinnedSplit(indices, boundingBoxes, binCount, bins, indexer, out bestBoundsA, out bestBoundsB, out aCount, out bCount); + } + + + { + Debug.Assert(aCount + bCount == leafCount); + BuildNode(bestBoundsA, bestBoundsB, nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, aCount, bCount, out var aIndex, out var bIndex); + if (aCount > 1) + BinnedBuilderInternalSingleAxis(indices.Slice(aCount), boundingBoxes.Slice(aCount), nodes.Slice(1, aCount - 1), metanodes.Slice(1, aCount - 1), aIndex, nodeIndex, 0, bins); + if (bCount > 1) + BinnedBuilderInternalSingleAxis(indices.Slice(aCount, bCount), boundingBoxes.Slice(aCount, bCount), nodes.Slice(aCount, bCount - 1), metanodes.Slice(aCount, bCount - 1), bIndex, nodeIndex, 1, bins); + } + } + + interface IBinIndexer + { + int ComputeBinIndex(Vector4 centroid); + } + + struct BinIndexerX : IBinIndexer + { + public float Min; + public float Scale; + public float MaximumIndex; + public int ComputeBinIndex(Vector4 centroid) + { + return (int)MathF.Min(MaximumIndex, ((centroid.X - Min) * Scale)); + } + } + struct BinIndexerY : IBinIndexer + { + public float Min; + public float Scale; + public float MaximumIndex; + public int ComputeBinIndex(Vector4 centroid) + { + return (int)MathF.Min(MaximumIndex, ((centroid.Y - Min) * Scale)); + } + } + struct BinIndexerZ : IBinIndexer + { + public float Min; + public float Scale; + public float MaximumIndex; + public int ComputeBinIndex(Vector4 centroid) + { + return (int)MathF.Min(MaximumIndex, ((centroid.Z - Min) * Scale)); + } + } + + private static unsafe void ComputeBinnedSplit( + Buffer indices, Buffer boundingBoxes, int binCount, in BinsSingleAxis bins, TBinIndexer indexer, + out BoundingBox4 bestboundsA, out BoundingBox4 bestboundsB, out int aCount, out int bCount) + where TBinIndexer : unmanaged, IBinIndexer + { + var leafCount = indices.Length; + + + + for (int i = 0; i < binCount; ++i) + { + ref var boxX = ref bins.BinBoundingBoxes[i]; + boxX.Min = new Vector4(float.MaxValue); + boxX.Max = new Vector4(float.MinValue); + bins.BinLeafCounts[i] = 0; + } + + var maximumBinIndex = new Vector4(binCount - 1); + for (int i = 0; i < leafCount; ++i) + { + ref var box = ref boundingBoxes[i]; + var centroid = box.Min + box.Max; + var binIndexForLeaf = indexer.ComputeBinIndex(centroid); + //Note that we don't store out any of the indices into per-bin lists here. We only *really* want two final groups for the children, + //and we can easily compute those by performing another scan. It requires recomputing the bin indices, but that's really not much of a concern. + ref var xBounds = ref bins.BinBoundingBoxes[binIndexForLeaf]; + xBounds.Min = Vector4.Min(xBounds.Min, box.Min); + xBounds.Max = Vector4.Max(xBounds.Max, box.Max); + ++bins.BinLeafCounts[binIndexForLeaf]; + } + + //Identify the split index by examining the SAH of very split option. + //Premerge from left to right so we have a sorta-summed area table to cheaply look up all possible child A bounds as we scan. + bins.BinBoundingBoxesScan[0] = bins.BinBoundingBoxes[0]; + for (int i = 1; i < binCount; ++i) + { + var previousIndex = i - 1; + ref var xBounds = ref bins.BinBoundingBoxes[i]; + ref var xScanBounds = ref bins.BinBoundingBoxesScan[i]; + ref var xPreviousScanBounds = ref bins.BinBoundingBoxesScan[previousIndex]; + xScanBounds.Min = Vector4.Min(xBounds.Min, xPreviousScanBounds.Min); + xScanBounds.Max = Vector4.Max(xBounds.Max, xPreviousScanBounds.Max); + } + var leftBoundsX = bins.BinBoundingBoxes[0]; + Debug.Assert( + leftBoundsX.Min.X > float.MinValue && leftBoundsX.Min.Y > float.MinValue && leftBoundsX.Min.Z > float.MinValue, + "Bin 0 should have been updated in all cases because it is aligned with the minimum bin, and the centroid span isn't degenerate."); + + float bestSAH = float.MaxValue; + int bestSplit = 1; + //The split index is going to end up in child B. + var lastBinIndex = binCount - 1; + BoundingBox4 accumulatedBoundingBoxB; + accumulatedBoundingBoxB = bins.BinBoundingBoxes[lastBinIndex]; + BoundingBox4 bestBoundingBoxB; + bestBoundingBoxB = bins.BinBoundingBoxes[lastBinIndex]; + int accumulatedLeafCountB = bins.BinLeafCounts[lastBinIndex]; + for (int splitIndexCandidate = lastBinIndex; splitIndexCandidate >= 1; --splitIndexCandidate) + { + var previousIndex = splitIndexCandidate - 1; + var leafCountA = leafCount - accumulatedLeafCountB; + var sahCandidate = ComputeBoundsMetric(bins.BinBoundingBoxesScan[previousIndex]) * leafCountA + ComputeBoundsMetric(accumulatedBoundingBoxB) * accumulatedLeafCountB; + if (sahCandidate < bestSAH) + { + bestSAH = sahCandidate; + bestSplit = splitIndexCandidate; + bestBoundingBoxB = accumulatedBoundingBoxB; + } + ref var xBounds = ref bins.BinBoundingBoxes[previousIndex]; + accumulatedBoundingBoxB.Min = Vector4.Min(xBounds.Min, accumulatedBoundingBoxB.Min); + accumulatedBoundingBoxB.Max = Vector4.Max(xBounds.Max, accumulatedBoundingBoxB.Max); + accumulatedLeafCountB += bins.BinLeafCounts[previousIndex]; + } + + //Choose the best SAH from all axes and split the indices/bounds into two halves for the children to operate on. + int splitIndex; + bCount = 0; + aCount = 0; + splitIndex = bestSplit; + bestboundsA = bins.BinBoundingBoxesScan[bestSplit - 1]; + bestboundsB = bestBoundingBoxB; + //Now we have the split index between bins. Go back through and sort the indices and bounds into two halves. + while (aCount + bCount < leafCount) + { + ref var box = ref boundingBoxes[aCount]; + var centroid = box.Min + box.Max; + //Note that we don't store out any of the indices into per-bin lists here. We only *really* want two final groups for the children, + //and we can easily compute those by performing another scan. It requires recomputing the bin indices, but that's really not much of a concern. + var binIndex = indexer.ComputeBinIndex(centroid); + if (binIndex >= splitIndex) + { + //Belongs to B. Swap it. + var targetIndex = leafCount - bCount - 1; + Helpers.Swap(ref indices[targetIndex], ref indices[aCount]); + Helpers.Swap(ref boundingBoxes[targetIndex], ref boundingBoxes[aCount]); + ++bCount; + //(Note that we still need to examine what we just swapped into the slot! It may belong to B too!) + } + else + { + //Belongs to A, no movement necessary. + ++aCount; + } + } + } + + struct BinsSingleAxis + { + public Buffer BinBoundingBoxes; + public Buffer BinBoundingBoxesScan; + public Buffer BinLeafCounts; + } + public static unsafe void BinnedBuilderSingleAxis(Buffer indices, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, BufferPool pool) + { + var leafCount = indices.Length; + Debug.Assert(boundingBoxes.Length >= leafCount, "The bounding boxes provided must cover the range of indices provided."); + Debug.Assert(nodes.Length >= leafCount - 1, "The output nodes must be able to contain the nodes created for the leaves."); + if (leafCount == 0) + return; + if (leafCount == 1) + { + //If there's only one leaf, the tree has a special format: the root node has only one child. + ref var root = ref nodes[0]; + root.A.Min = boundingBoxes[0].Min; + root.A.Index = indices[0]; //Node that we assume the indices are already encoded. This function works with subtree refinements as well which can manage either leaves or internals. + root.A.Max = boundingBoxes[0].Max; + root.A.LeafCount = 1; + root.B = default; + return; + } + boundingBoxes = boundingBoxes.Slice(indices.Length); + nodes = nodes.Slice(leafCount - 1); + + var binBoundsMemory = stackalloc BoundingBox4[MaximumBinCountRevamp * 2]; + BinsSingleAxis bins; + bins.BinBoundingBoxes = new Buffer(binBoundsMemory, MaximumBinCountRevamp); + bins.BinBoundingBoxesScan = new Buffer(binBoundsMemory + MaximumBinCountRevamp, MaximumBinCountRevamp); + + var binLeafCountsMemory = stackalloc int[MaximumBinCountRevamp]; + bins.BinLeafCounts = new Buffer(binLeafCountsMemory, MaximumBinCountRevamp); + + //While we could avoid a recursive implementation, the overhead is low compared to the per-iteration cost. + BinnedBuilderInternalSingleAxis(indices, boundingBoxes.As(), nodes, metanodes, 0, -1, -1, bins); + } + } } diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index 299dcbd6e..99bcec10c 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -199,6 +199,21 @@ public override void Initialize(ContentArchive content, Camera camera) Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); + BinnedTest(() => + { + Tree.BinnedBuilderSingleAxis(leafIndices, leafBounds, mesh.Tree.Nodes, mesh.Tree.Metanodes, BufferPool); + for (int i = 0; i < mesh.Tree.NodeCount; ++i) + { + ref var node = ref mesh.Tree.Nodes[i]; + ref var a = ref node.A; + ref var b = ref node.B; + if (a.Index < 0) + mesh.Tree.Leaves[Tree.Encode(a.Index)] = new Leaf(i, 0); + if (b.Index < 0) + mesh.Tree.Leaves[Tree.Encode(b.Index)] = new Leaf(i, 1); + } + }, "Revamp Single Axis", ref mesh.Tree); + //var mesh2 = new Mesh(triangles, Vector3.One, BufferPool); //QuickList subtreeReferences = new(mesh2.Tree.LeafCount, BufferPool); From f313a82172b57eb77d75e3b8f408f7c9ee7a6e2d Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 18 Sep 2022 20:06:47 -0500 Subject: [PATCH 609/947] Simpler branchy, faster? Forced higher bin counts can make single axis better than multiaxis at same performance. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 111 ++++-------------- .../SpecializedTests/TreeFiddlingTestDemo.cs | 48 ++++---- 2 files changed, 50 insertions(+), 109 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 80785d2ef..0bdd07e9e 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -16,7 +16,7 @@ namespace BepuPhysics.Trees { partial struct Tree { - const int MaximumBinCountRevamp = 128; + const int MaximumBinCountRevamp = 64; [MethodImpl(MethodImplOptions.AggressiveInlining)] static unsafe Int4 Truncate(Vector4 v) { @@ -294,7 +294,7 @@ static unsafe void BinnedBuilderInternal(Buffer indices, Buffer indices, Buffer centroidSpan.Y && centroidSpan.X > centroidSpan.Z; + var useY = centroidSpan.Y > centroidSpan.Z; + + var binCount = Math.Min(MaximumBinCountRevamp, Math.Max((int)(leafCount * 0.1f), 16)); Debug.Assert(bins.BinBoundingBoxes.Length >= binCount); + var offsetToBinIndex = new Vector4(binCount) / centroidSpan; //Avoid letting NaNs into the offsetToBinIndex scale. offsetToBinIndex = Vector128.ConditionalSelect(axisIsDegenerate, Vector128.Zero, offsetToBinIndex.AsVector128()).AsVector4(); - BoundingBox4 bestBoundsA, bestBoundsB; - int aCount, bCount; - //Use generic specialization to avoid branching on every bin selection. - if (centroidSpan.X > centroidSpan.Y && centroidSpan.X > centroidSpan.Z) - { - var indexer = new BinIndexerX { Min = centroidMin.X, MaximumIndex = binCount - 1, Scale = offsetToBinIndex.X }; - ComputeBinnedSplit(indices, boundingBoxes, binCount, bins, indexer, out bestBoundsA, out bestBoundsB, out aCount, out bCount); - } - else if (centroidSpan.Y > centroidSpan.Z) - { - var indexer = new BinIndexerY { Min = centroidMin.Y, MaximumIndex = binCount - 1, Scale = offsetToBinIndex.Y }; - ComputeBinnedSplit(indices, boundingBoxes, binCount, bins, indexer, out bestBoundsA, out bestBoundsB, out aCount, out bCount); - } - else - { - var indexer = new BinIndexerZ { Min = centroidMin.Z, MaximumIndex = binCount - 1, Scale = offsetToBinIndex.Z }; - ComputeBinnedSplit(indices, boundingBoxes, binCount, bins, indexer, out bestBoundsA, out bestBoundsB, out aCount, out bCount); - } - - - { - Debug.Assert(aCount + bCount == leafCount); - BuildNode(bestBoundsA, bestBoundsB, nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, aCount, bCount, out var aIndex, out var bIndex); - if (aCount > 1) - BinnedBuilderInternalSingleAxis(indices.Slice(aCount), boundingBoxes.Slice(aCount), nodes.Slice(1, aCount - 1), metanodes.Slice(1, aCount - 1), aIndex, nodeIndex, 0, bins); - if (bCount > 1) - BinnedBuilderInternalSingleAxis(indices.Slice(aCount, bCount), boundingBoxes.Slice(aCount, bCount), nodes.Slice(aCount, bCount - 1), metanodes.Slice(aCount, bCount - 1), bIndex, nodeIndex, 1, bins); - } - } - - interface IBinIndexer - { - int ComputeBinIndex(Vector4 centroid); - } - - struct BinIndexerX : IBinIndexer - { - public float Min; - public float Scale; - public float MaximumIndex; - public int ComputeBinIndex(Vector4 centroid) - { - return (int)MathF.Min(MaximumIndex, ((centroid.X - Min) * Scale)); - } - } - struct BinIndexerY : IBinIndexer - { - public float Min; - public float Scale; - public float MaximumIndex; - public int ComputeBinIndex(Vector4 centroid) - { - return (int)MathF.Min(MaximumIndex, ((centroid.Y - Min) * Scale)); - } - } - struct BinIndexerZ : IBinIndexer - { - public float Min; - public float Scale; - public float MaximumIndex; - public int ComputeBinIndex(Vector4 centroid) - { - return (int)MathF.Min(MaximumIndex, ((centroid.Z - Min) * Scale)); - } - } - - private static unsafe void ComputeBinnedSplit( - Buffer indices, Buffer boundingBoxes, int binCount, in BinsSingleAxis bins, TBinIndexer indexer, - out BoundingBox4 bestboundsA, out BoundingBox4 bestboundsB, out int aCount, out int bCount) - where TBinIndexer : unmanaged, IBinIndexer - { - var leafCount = indices.Length; - for (int i = 0; i < binCount; ++i) @@ -862,9 +793,10 @@ private static unsafe void ComputeBinnedSplit( { ref var box = ref boundingBoxes[i]; var centroid = box.Min + box.Max; - var binIndexForLeaf = indexer.ComputeBinIndex(centroid); + var binIndicesForLeafContinuous = Vector4.Min(maximumBinIndex, (centroid - centroidMin) * offsetToBinIndex); //Note that we don't store out any of the indices into per-bin lists here. We only *really* want two final groups for the children, //and we can easily compute those by performing another scan. It requires recomputing the bin indices, but that's really not much of a concern. + var binIndexForLeaf = (int)(useX ? binIndicesForLeafContinuous.X : useY ? binIndicesForLeafContinuous.Y : binIndicesForLeafContinuous.Z); ref var xBounds = ref bins.BinBoundingBoxes[binIndexForLeaf]; xBounds.Min = Vector4.Min(xBounds.Min, box.Min); xBounds.Max = Vector4.Max(xBounds.Max, box.Max); @@ -915,20 +847,20 @@ private static unsafe void ComputeBinnedSplit( } //Choose the best SAH from all axes and split the indices/bounds into two halves for the children to operate on. - int splitIndex; - bCount = 0; - aCount = 0; - splitIndex = bestSplit; - bestboundsA = bins.BinBoundingBoxesScan[bestSplit - 1]; - bestboundsB = bestBoundingBoxB; + var bCount = 0; + var aCount = 0; + var splitIndex = bestSplit; + var bestboundsA = bins.BinBoundingBoxesScan[bestSplit - 1]; + var bestboundsB = bestBoundingBoxB; //Now we have the split index between bins. Go back through and sort the indices and bounds into two halves. while (aCount + bCount < leafCount) { ref var box = ref boundingBoxes[aCount]; var centroid = box.Min + box.Max; + var binIndicesForLeafContinuous = Vector4.Min(maximumBinIndex, (centroid - centroidMin) * offsetToBinIndex); //Note that we don't store out any of the indices into per-bin lists here. We only *really* want two final groups for the children, //and we can easily compute those by performing another scan. It requires recomputing the bin indices, but that's really not much of a concern. - var binIndex = indexer.ComputeBinIndex(centroid); + var binIndex = (int)(useX ? binIndicesForLeafContinuous.X : useY ? binIndicesForLeafContinuous.Y : binIndicesForLeafContinuous.Z); if (binIndex >= splitIndex) { //Belongs to B. Swap it. @@ -944,6 +876,15 @@ private static unsafe void ComputeBinnedSplit( ++aCount; } } + + { + Debug.Assert(aCount + bCount == leafCount); + BuildNode(bestboundsA, bestboundsB, nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, aCount, bCount, out var aIndex, out var bIndex); + if (aCount > 1) + BinnedBuilderInternalSingleAxis(indices.Slice(aCount), boundingBoxes.Slice(aCount), nodes.Slice(1, aCount - 1), metanodes.Slice(1, aCount - 1), aIndex, nodeIndex, 0, bins); + if (bCount > 1) + BinnedBuilderInternalSingleAxis(indices.Slice(aCount, bCount), boundingBoxes.Slice(aCount, bCount), nodes.Slice(aCount, bCount - 1), metanodes.Slice(aCount, bCount - 1), bIndex, nodeIndex, 1, bins); + } } struct BinsSingleAxis diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index 99bcec10c..0d0db971a 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -182,20 +182,20 @@ public override void Initialize(ContentArchive content, Camera camera) bounds.Max = Vector3.Max(t.A, Vector3.Max(t.B, t.C)); leafIndices[i] = Tree.Encode(i); } - BinnedTest(() => - { - Tree.BinnedBuilder(leafIndices, leafBounds, mesh.Tree.Nodes, mesh.Tree.Metanodes, BufferPool); - for (int i = 0; i < mesh.Tree.NodeCount; ++i) - { - ref var node = ref mesh.Tree.Nodes[i]; - ref var a = ref node.A; - ref var b = ref node.B; - if (a.Index < 0) - mesh.Tree.Leaves[Tree.Encode(a.Index)] = new Leaf(i, 0); - if (b.Index < 0) - mesh.Tree.Leaves[Tree.Encode(b.Index)] = new Leaf(i, 1); - } - }, "Revamp", ref mesh.Tree); + //BinnedTest(() => + //{ + // Tree.BinnedBuilder(leafIndices, leafBounds, mesh.Tree.Nodes, mesh.Tree.Metanodes, BufferPool); + // for (int i = 0; i < mesh.Tree.NodeCount; ++i) + // { + // ref var node = ref mesh.Tree.Nodes[i]; + // ref var a = ref node.A; + // ref var b = ref node.B; + // if (a.Index < 0) + // mesh.Tree.Leaves[Tree.Encode(a.Index)] = new Leaf(i, 0); + // if (b.Index < 0) + // mesh.Tree.Leaves[Tree.Encode(b.Index)] = new Leaf(i, 1); + // } + //}, "Revamp", ref mesh.Tree); Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); @@ -214,17 +214,17 @@ public override void Initialize(ContentArchive content, Camera camera) } }, "Revamp Single Axis", ref mesh.Tree); - //var mesh2 = new Mesh(triangles, Vector3.One, BufferPool); + var mesh2 = new Mesh(triangles, Vector3.One, BufferPool); - //QuickList subtreeReferences = new(mesh2.Tree.LeafCount, BufferPool); - //QuickList treeletInternalNodes = new(mesh2.Tree.LeafCount, BufferPool); - //Tree.CreateBinnedResources(BufferPool, mesh2.Tree.LeafCount, out var binnedResourcesBuffer, out var binnedResources); - //BinnedTest(() => - //{ - // subtreeReferences.Count = 0; - // treeletInternalNodes.Count = 0; - // mesh2.Tree.BinnedRefine(0, ref subtreeReferences, mesh2.Tree.LeafCount, ref treeletInternalNodes, ref binnedResources, BufferPool); - //}, "Original", ref mesh2.Tree); + QuickList subtreeReferences = new(mesh2.Tree.LeafCount, BufferPool); + QuickList treeletInternalNodes = new(mesh2.Tree.LeafCount, BufferPool); + Tree.CreateBinnedResources(BufferPool, mesh2.Tree.LeafCount, out var binnedResourcesBuffer, out var binnedResources); + BinnedTest(() => + { + subtreeReferences.Count = 0; + treeletInternalNodes.Count = 0; + mesh2.Tree.BinnedRefine(0, ref subtreeReferences, mesh2.Tree.LeafCount, ref treeletInternalNodes, ref binnedResources, BufferPool); + }, "Original", ref mesh2.Tree); //RefitTest(() => mesh.Tree.Refit2(), "refit2", ref mesh.Tree); //RefitTest(() => mesh.Tree.Refit(), "Original", ref mesh.Tree); From a1a3ca5fc8ab9656c3c7ccb0117e6fc60a68083d Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 20 Sep 2022 16:50:32 -0500 Subject: [PATCH 610/947] Purging multiaxis. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 492 +----------------- .../SpecializedTests/TreeFiddlingTestDemo.cs | 41 +- 2 files changed, 25 insertions(+), 508 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 0bdd07e9e..94c7d6941 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -95,133 +95,6 @@ struct BoundsComparerZ : IComparerRef { public int Compare(ref BoundingBox4 a, ref BoundingBox4 b) => (a.Min.Z + a.Max.Z) > (b.Min.Z + b.Max.Z) ? -1 : 1; } - static unsafe void MicroSweepForBinnedBuilder(Vector4 centroidMin, Vector4 centroidMax, Buffer indices, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, Bins bins) - { - //This is a very small scale sweep build. - var leafCount = indices.Length; - if (leafCount == 2) - { - BuildNode(boundingBoxes[0], boundingBoxes[1], nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, 1, 1, out _, out _); - return; - } - var centroidSpan = centroidMax - centroidMin; - if (centroidSpan.X > centroidSpan.Y && centroidSpan.X > centroidSpan.Z) - { - var comparer = new BoundsComparerX(); - QuickSort.Sort(ref boundingBoxes[0], ref indices[0], 0, leafCount - 1, ref comparer); - } - else if (centroidSpan.Y > centroidSpan.Z) - { - var comparer = new BoundsComparerY(); - QuickSort.Sort(ref boundingBoxes[0], ref indices[0], 0, leafCount - 1, ref comparer); - } - else - { - var comparer = new BoundsComparerZ(); - QuickSort.Sort(ref boundingBoxes[0], ref indices[0], 0, leafCount - 1, ref comparer); - } - - Debug.Assert(leafCount <= MaximumBinCountRevamp, "We're reusing the bin resources under the assumption that this is only ever called when there are less leaves than maximum bins."); - //Identify the split index by examining the SAH of very split option. - //Premerge from left to right so we have a sorta-summed area table to cheaply look up all possible child A bounds as we scan. - bins.BinBoundingBoxesScanX[0] = boundingBoxes[0]; - for (int i = 1; i < leafCount; ++i) - { - ref var previousScanBounds = ref bins.BinBoundingBoxesScanX[i - 1]; - ref var scanBounds = ref bins.BinBoundingBoxesScanX[i]; - ref var bounds = ref boundingBoxes[i]; - scanBounds.Min = Vector4.Min(bounds.Min, previousScanBounds.Min); - scanBounds.Max = Vector4.Max(bounds.Max, previousScanBounds.Max); - } - - float bestSAH = float.MaxValue; - int bestSplit = 1; - //The split index is going to end up in child B. - var lastLeafIndex = leafCount - 1; - BoundingBox4 accumulatedBoundingBoxB = boundingBoxes[lastLeafIndex]; - Unsafe.SkipInit(out BoundingBox4 bestBoundsB); - int accumulatedLeafCountB = 1; - for (int splitIndexCandidate = lastLeafIndex; splitIndexCandidate >= 1; --splitIndexCandidate) - { - var previousIndex = splitIndexCandidate - 1; - var leafCountA = leafCount - accumulatedLeafCountB; - var sahCandidate = ComputeBoundsMetric(bins.BinBoundingBoxesScanX[previousIndex]) * leafCountA + ComputeBoundsMetric(accumulatedBoundingBoxB) * accumulatedLeafCountB; - if (sahCandidate < bestSAH) - { - bestSAH = sahCandidate; - bestSplit = splitIndexCandidate; - bestBoundsB = accumulatedBoundingBoxB; - } - ref var bounds = ref boundingBoxes[splitIndexCandidate - 1]; - accumulatedBoundingBoxB.Min = Vector4.Min(bounds.Min, accumulatedBoundingBoxB.Min); - accumulatedBoundingBoxB.Max = Vector4.Max(bounds.Max, accumulatedBoundingBoxB.Max); - ++accumulatedLeafCountB; - } - - var bestBoundsA = bins.BinBoundingBoxesScanX[bestSplit - 1]; - - var aCount = bestSplit; - var bCount = leafCount - bestSplit; - { - //if (leafCount == 2) - //{ - // Debug.Assert(bestBoundsA.Min == boundingBoxes[0].Min); - // Debug.Assert(bestBoundsA.Max == boundingBoxes[0].Max); - // Debug.Assert(bestBoundsB.Min == boundingBoxes[1].Min); - // Debug.Assert(bestBoundsB.Max == boundingBoxes[1].Max); - //} - //for (int i = 0; i < leafCount; ++i) - //{ - // var bounds = i < bestSplit ? bestBoundsA : bestBoundsB; - // var containedA = Vector128.LessThanOrEqual(boundingBoxes[i].Max.AsVector128(), bounds.Max.AsVector128()) & Vector128.GreaterThanOrEqual(boundingBoxes[i].Min.AsVector128(), bounds.Min.AsVector128()); - // var mask = containedA.ExtractMostSignificantBits() & 0b111; - // Debug.Assert(mask == 0b111, "All children must be contained within their parent."); - //} - //BoundingBox4 debugBoundsA = boundingBoxes[0]; - //for (int i = 1; i < aCount; ++i) - //{ - // ref var bounds = ref boundingBoxes[i]; - // debugBoundsA.Min = Vector4.Min(debugBoundsA.Min, bounds.Min); - // debugBoundsA.Max = Vector4.Max(debugBoundsA.Max, bounds.Max); - //} - //BoundingBox4 debugBoundsB = boundingBoxes[aCount]; - //for (int i = aCount + 1; i < leafCount; ++i) - //{ - // ref var bounds = ref boundingBoxes[i]; - // debugBoundsB.Min = Vector4.Min(debugBoundsB.Min, bounds.Min); - // debugBoundsB.Max = Vector4.Max(debugBoundsB.Max, bounds.Max); - //} - //Debug.Assert(bestBoundsA.Min == debugBoundsA.Min); - //Debug.Assert(bestBoundsA.Max == debugBoundsA.Max); - //Debug.Assert(bestBoundsB.Min == debugBoundsB.Min); - //Debug.Assert(bestBoundsB.Max == debugBoundsB.Max); - BuildNode(bestBoundsA, bestBoundsB, nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, aCount, bCount, out var aIndex, out var bIndex); - if (aCount > 1) - { - var aBounds = boundingBoxes.Slice(aCount); - BoundingBox4 centroidBoundsA = aBounds[0]; - for (int i = 1; i < aCount; ++i) - { - ref var bounds = ref aBounds[i]; - centroidBoundsA.Min = Vector4.Min(centroidBoundsA.Min, bounds.Min); - centroidBoundsA.Max = Vector4.Max(centroidBoundsA.Max, bounds.Max); - } - MicroSweepForBinnedBuilder(centroidBoundsA.Min, centroidBoundsA.Max, indices.Slice(aCount), aBounds, nodes.Slice(1, aCount - 1), metanodes.Slice(1, aCount - 1), aIndex, nodeIndex, 0, bins); - } - if (bCount > 1) - { - var bBounds = boundingBoxes.Slice(aCount, bCount); - BoundingBox4 centroidBoundsB = bBounds[0]; - for (int i = 0; i < bCount; ++i) - { - ref var bounds = ref bBounds[i]; - centroidBoundsB.Min = Vector4.Min(centroidBoundsB.Min, bounds.Min); - centroidBoundsB.Max = Vector4.Max(centroidBoundsB.Max, bounds.Max); - } - MicroSweepForBinnedBuilder(centroidBoundsB.Min, centroidBoundsB.Max, indices.Slice(aCount, bCount), bBounds, nodes.Slice(aCount, bCount - 1), metanodes.Slice(aCount, bCount - 1), bIndex, nodeIndex, 1, bins); - } - } - } //static void ValidateNode(int nodeIndex, Buffer nodes, out Vector3 min, out Vector3 max) //{ @@ -241,348 +114,7 @@ static unsafe void MicroSweepForBinnedBuilder(Vector4 centroidMin, Vector4 centr // max = Vector3.Max(nodes[nodeIndex].B.Max, nodes[nodeIndex].B.Max); //} - static unsafe void BinnedBuilderInternal(Buffer indices, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, in Bins bins) - { - var centroidMin = new Vector4(float.MaxValue); - var centroidMax = new Vector4(float.MinValue); - var leafCount = indices.Length; - - for (int i = 0; i < leafCount; ++i) - { - ref var box = ref boundingBoxes[i]; - //Note that centroids never bother scaling by 0.5. It's fine as long as we're consistent. - var centroid = box.Min + box.Max; - centroidMin = Vector4.Min(centroidMin, centroid); - centroidMax = Vector4.Max(centroidMax, centroid); - } - var centroidSpan = centroidMax - centroidMin; - var axisIsDegenerate = Vector128.LessThanOrEqual(centroidSpan.AsVector128(), Vector128.Create(1e-12f)); - if ((Vector128.ExtractMostSignificantBits(axisIsDegenerate) & 0b111) == 0b111) - { - //This node is completely degenerate; there is no 'good' ordering of the children. Pick a split in the middle and shrug. - //This shouldn't happen unless something is badly wrong with the input; no point in optimizing it. - var countA = indices.Length / 2; - var countB = indices.Length - countA; - //Still have to compute the child bounding boxes, because the centroid bounds span being zero doesn't imply that the full bounds are zero. - BoundingBox4 boundsA, boundsB; - boundsA.Min = new Vector4(float.MaxValue); - boundsA.Max = new Vector4(float.MinValue); - boundsB.Min = new Vector4(float.MaxValue); - boundsB.Max = new Vector4(float.MinValue); - for (int i = 0; i < countA; ++i) - { - ref var bounds = ref boundingBoxes[i]; - boundsA.Min = Vector4.Min(bounds.Min, boundsA.Min); - boundsA.Max = Vector4.Max(bounds.Max, boundsA.Max); - } - for (int i = countA; i < indices.Length; ++i) - { - ref var bounds = ref boundingBoxes[i]; - boundsB.Min = Vector4.Min(bounds.Min, boundsB.Min); - boundsB.Max = Vector4.Max(bounds.Max, boundsB.Max); - } - BuildNode(boundsA, boundsB, nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, countA, countB, out var aIndex, out var bIndex); - if (countA > 1) - BinnedBuilderInternal(indices.Slice(countA), boundingBoxes.Slice(countA), nodes.Slice(1, countA - 1), metanodes.Slice(1, countA - 1), aIndex, nodeIndex, 0, bins); - if (countB > 1) - BinnedBuilderInternal(indices.Slice(countA, countB), boundingBoxes.Slice(countA, countB), nodes.Slice(countA, countB - 1), metanodes.Slice(countA, countB - 1), bIndex, nodeIndex, 1, bins); - return; - } - if (leafCount == 2) - { - BuildNode(boundingBoxes[0], boundingBoxes[1], nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, 1, 1, out _, out _); - return; - } - - if (leafCount <= 8) - { - MicroSweepForBinnedBuilder(centroidMin, centroidMax, indices, boundingBoxes, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, bins); - return; - } - var binCount = Math.Min(MaximumBinCountRevamp, Math.Max((int)(leafCount * 0.1f), 4)); - Debug.Assert(bins.BinBoundingBoxesX.Length >= binCount); - Debug.Assert(bins.BinBoundingBoxesY.Length >= binCount); - Debug.Assert(bins.BinBoundingBoxesZ.Length >= binCount); - - var offsetToBinIndex = new Vector4(binCount) / centroidSpan; - //Avoid letting NaNs into the offsetToBinIndex scale. - offsetToBinIndex = Vector128.ConditionalSelect(axisIsDegenerate, Vector128.Zero, offsetToBinIndex.AsVector128()).AsVector4(); - - - for (int i = 0; i < binCount; ++i) - { - ref var boxX = ref bins.BinBoundingBoxesX[i]; - ref var boxY = ref bins.BinBoundingBoxesY[i]; - ref var boxZ = ref bins.BinBoundingBoxesZ[i]; - boxX.Min = new Vector4(float.MaxValue); - boxX.Max = new Vector4(float.MinValue); - boxY.Min = new Vector4(float.MaxValue); - boxY.Max = new Vector4(float.MinValue); - boxZ.Min = new Vector4(float.MaxValue); - boxZ.Max = new Vector4(float.MinValue); - bins.BinLeafCountsX[i] = 0; - bins.BinLeafCountsY[i] = 0; - bins.BinLeafCountsZ[i] = 0; - } - - var maximumBinIndex = new Vector4(binCount - 1); - for (int i = 0; i < leafCount; ++i) - { - ref var box = ref boundingBoxes[i]; - var centroid = box.Min + box.Max; - var binIndicesForLeafContinuous = Vector4.Min(maximumBinIndex, (centroid - centroidMin) * offsetToBinIndex); - //Note that we don't store out any of the indices into per-bin lists here. We only *really* want two final groups for the children, - //and we can easily compute those by performing another scan. It requires recomputing the bin indices, but that's really not much of a concern. - var binIndicesForLeaf = Truncate(binIndicesForLeafContinuous); - ref var xBounds = ref bins.BinBoundingBoxesX[binIndicesForLeaf.X]; - ref var yBounds = ref bins.BinBoundingBoxesY[binIndicesForLeaf.Y]; - ref var zBounds = ref bins.BinBoundingBoxesZ[binIndicesForLeaf.Z]; - xBounds.Min = Vector4.Min(xBounds.Min, box.Min); - xBounds.Max = Vector4.Max(xBounds.Max, box.Max); - yBounds.Min = Vector4.Min(yBounds.Min, box.Min); - yBounds.Max = Vector4.Max(yBounds.Max, box.Max); - zBounds.Min = Vector4.Min(zBounds.Min, box.Min); - zBounds.Max = Vector4.Max(zBounds.Max, box.Max); - ++bins.BinLeafCountsX[binIndicesForLeaf.X]; - ++bins.BinLeafCountsY[binIndicesForLeaf.Y]; - ++bins.BinLeafCountsZ[binIndicesForLeaf.Z]; - } - - //Identify the split index by examining the SAH of very split option. - //Premerge from left to right so we have a sorta-summed area table to cheaply look up all possible child A bounds as we scan. - bins.BinBoundingBoxesScanX[0] = bins.BinBoundingBoxesX[0]; - bins.BinBoundingBoxesScanY[0] = bins.BinBoundingBoxesY[0]; - bins.BinBoundingBoxesScanZ[0] = bins.BinBoundingBoxesZ[0]; - for (int i = 1; i < binCount; ++i) - { - var previousIndex = i - 1; - ref var xBounds = ref bins.BinBoundingBoxesX[i]; - ref var yBounds = ref bins.BinBoundingBoxesY[i]; - ref var zBounds = ref bins.BinBoundingBoxesZ[i]; - ref var xScanBounds = ref bins.BinBoundingBoxesScanX[i]; - ref var yScanBounds = ref bins.BinBoundingBoxesScanY[i]; - ref var zScanBounds = ref bins.BinBoundingBoxesScanZ[i]; - ref var xPreviousScanBounds = ref bins.BinBoundingBoxesScanX[previousIndex]; - ref var yPreviousScanBounds = ref bins.BinBoundingBoxesScanY[previousIndex]; - ref var zPreviousScanBounds = ref bins.BinBoundingBoxesScanZ[previousIndex]; - xScanBounds.Min = Vector4.Min(xBounds.Min, xPreviousScanBounds.Min); - xScanBounds.Max = Vector4.Max(xBounds.Max, xPreviousScanBounds.Max); - yScanBounds.Min = Vector4.Min(yBounds.Min, yPreviousScanBounds.Min); - yScanBounds.Max = Vector4.Max(yBounds.Max, yPreviousScanBounds.Max); - zScanBounds.Min = Vector4.Min(zBounds.Min, zPreviousScanBounds.Min); - zScanBounds.Max = Vector4.Max(zBounds.Max, zPreviousScanBounds.Max); - } - var leftBoundsX = bins.BinBoundingBoxesX[0]; - var leftBoundsY = bins.BinBoundingBoxesY[0]; - var leftBoundsZ = bins.BinBoundingBoxesZ[0]; - Debug.Assert( - leftBoundsX.Min.X > float.MinValue && leftBoundsX.Min.Y > float.MinValue && leftBoundsX.Min.Z > float.MinValue && - leftBoundsY.Min.X > float.MinValue && leftBoundsY.Min.Y > float.MinValue && leftBoundsY.Min.Z > float.MinValue && - leftBoundsZ.Min.X > float.MinValue && leftBoundsZ.Min.Y > float.MinValue && leftBoundsZ.Min.Z > float.MinValue, - "Bin 0 should have been updated in all cases because it is aligned with the minimum bin, and the centroid span isn't degenerate."); - - float bestX = float.MaxValue, bestY = float.MaxValue, bestZ = float.MaxValue; - int bestSplitX = 1, bestSplitY = 1, bestSplitZ = 1; - //The split index is going to end up in child B. - var lastBinIndex = binCount - 1; - BoundingBox4 accumulatedBoundingBoxBX, accumulatedBoundingBoxBY, accumulatedBoundingBoxBZ; - accumulatedBoundingBoxBX = bins.BinBoundingBoxesX[lastBinIndex]; - accumulatedBoundingBoxBY = bins.BinBoundingBoxesY[lastBinIndex]; - accumulatedBoundingBoxBZ = bins.BinBoundingBoxesZ[lastBinIndex]; - BoundingBox4 bestBoundingBoxBX, bestBoundingBoxBY, bestBoundingBoxBZ; - bestBoundingBoxBX = bins.BinBoundingBoxesX[lastBinIndex]; - bestBoundingBoxBY = bins.BinBoundingBoxesY[lastBinIndex]; - bestBoundingBoxBZ = bins.BinBoundingBoxesZ[lastBinIndex]; - int accumulatedLeafCountBX = bins.BinLeafCountsX[lastBinIndex]; - int accumulatedLeafCountBY = bins.BinLeafCountsY[lastBinIndex]; - int accumulatedLeafCountBZ = bins.BinLeafCountsZ[lastBinIndex]; - for (int splitIndexCandidate = lastBinIndex; splitIndexCandidate >= 1; --splitIndexCandidate) - { - var previousIndex = splitIndexCandidate - 1; - var leafCountAX = leafCount - accumulatedLeafCountBX; - var leafCountAY = leafCount - accumulatedLeafCountBY; - var leafCountAZ = leafCount - accumulatedLeafCountBZ; - var sahX = ComputeBoundsMetric(bins.BinBoundingBoxesScanX[previousIndex]) * leafCountAX + ComputeBoundsMetric(accumulatedBoundingBoxBX) * accumulatedLeafCountBX; - var sahY = ComputeBoundsMetric(bins.BinBoundingBoxesScanY[previousIndex]) * leafCountAY + ComputeBoundsMetric(accumulatedBoundingBoxBY) * accumulatedLeafCountBY; - var sahZ = ComputeBoundsMetric(bins.BinBoundingBoxesScanZ[previousIndex]) * leafCountAZ + ComputeBoundsMetric(accumulatedBoundingBoxBZ) * accumulatedLeafCountBZ; - if (sahX < bestX) - { - bestX = sahX; - bestSplitX = splitIndexCandidate; - bestBoundingBoxBX = accumulatedBoundingBoxBX; - } - if (sahY < bestY) - { - bestY = sahY; - bestSplitY = splitIndexCandidate; - bestBoundingBoxBY = accumulatedBoundingBoxBY; - } - if (sahZ < bestZ) - { - bestZ = sahZ; - bestSplitZ = splitIndexCandidate; - bestBoundingBoxBZ = accumulatedBoundingBoxBZ; - } - ref var xBounds = ref bins.BinBoundingBoxesX[previousIndex]; - ref var yBounds = ref bins.BinBoundingBoxesY[previousIndex]; - ref var zBounds = ref bins.BinBoundingBoxesZ[previousIndex]; - accumulatedBoundingBoxBX.Min = Vector4.Min(xBounds.Min, accumulatedBoundingBoxBX.Min); - accumulatedBoundingBoxBX.Max = Vector4.Max(xBounds.Max, accumulatedBoundingBoxBX.Max); - accumulatedBoundingBoxBY.Min = Vector4.Min(yBounds.Min, accumulatedBoundingBoxBY.Min); - accumulatedBoundingBoxBY.Max = Vector4.Max(yBounds.Max, accumulatedBoundingBoxBY.Max); - accumulatedBoundingBoxBZ.Min = Vector4.Min(zBounds.Min, accumulatedBoundingBoxBZ.Min); - accumulatedBoundingBoxBZ.Max = Vector4.Max(zBounds.Max, accumulatedBoundingBoxBZ.Max); - accumulatedLeafCountBX += bins.BinLeafCountsX[previousIndex]; - accumulatedLeafCountBY += bins.BinLeafCountsY[previousIndex]; - accumulatedLeafCountBZ += bins.BinLeafCountsZ[previousIndex]; - } - - //Choose the best SAH from all axes and split the indices/bounds into two halves for the children to operate on. - int splitIndex; - BoundingBox4 bestboundsA, bestboundsB; - var bCount = 0; - var aCount = 0; - if (bestX < bestY && bestX < bestZ) - { - splitIndex = bestSplitX; - bestboundsA = bins.BinBoundingBoxesScanX[bestSplitX - 1]; - bestboundsB = bestBoundingBoxBX; - //Now we have the split index between bins. Go back through and sort the indices and bounds into two halves. - while (aCount + bCount < leafCount) - { - ref var box = ref boundingBoxes[aCount]; - var centroid = box.Min + box.Max; - var binIndicesForLeafContinuous = Vector4.Min(maximumBinIndex, (centroid - centroidMin) * offsetToBinIndex); - //Note that we don't store out any of the indices into per-bin lists here. We only *really* want two final groups for the children, - //and we can easily compute those by performing another scan. It requires recomputing the bin indices, but that's really not much of a concern. - var binIndex = (int)binIndicesForLeafContinuous.X; - if (binIndex >= splitIndex) - { - //Belongs to B. Swap it. - var targetIndex = leafCount - bCount - 1; - Helpers.Swap(ref indices[targetIndex], ref indices[aCount]); - Helpers.Swap(ref boundingBoxes[targetIndex], ref boundingBoxes[aCount]); - ++bCount; - //(Note that we still need to examine what we just swapped into the slot! It may belong to B too!) - } - else - { - //Belongs to A, no movement necessary. - ++aCount; - } - } - } - else if (bestY < bestZ) - { - splitIndex = bestSplitY; - bestboundsA = bins.BinBoundingBoxesScanY[bestSplitY - 1]; - bestboundsB = bestBoundingBoxBY; - while (aCount + bCount < leafCount) - { - ref var box = ref boundingBoxes[aCount]; - var centroid = box.Min + box.Max; - var binIndicesForLeafContinuous = Vector4.Min(maximumBinIndex, (centroid - centroidMin) * offsetToBinIndex); - //Note that we don't store out any of the indices into per-bin lists here. We only *really* want two final groups for the children, - //and we can easily compute those by performing another scan. It requires recomputing the bin indices, but that's really not much of a concern. - var binIndex = (int)binIndicesForLeafContinuous.Y; - if (binIndex >= splitIndex) - { - //Belongs to B. Swap it. - var targetIndex = leafCount - bCount - 1; - Helpers.Swap(ref indices[targetIndex], ref indices[aCount]); - Helpers.Swap(ref boundingBoxes[targetIndex], ref boundingBoxes[aCount]); - ++bCount; - //(Note that we still need to examine what we just swapped into the slot! It may belong to B too!) - } - else - { - //Belongs to A, no movement necessary. - ++aCount; - } - } - } - else - { - splitIndex = bestSplitZ; - bestboundsA = bins.BinBoundingBoxesScanZ[bestSplitZ - 1]; - bestboundsB = bestBoundingBoxBZ; - while (aCount + bCount < leafCount) - { - ref var box = ref boundingBoxes[aCount]; - var centroid = box.Min + box.Max; - var binIndicesForLeafContinuous = Vector4.Min(maximumBinIndex, (centroid - centroidMin) * offsetToBinIndex); - //Note that we don't store out any of the indices into per-bin lists here. We only *really* want two final groups for the children, - //and we can easily compute those by performing another scan. It requires recomputing the bin indices, but that's really not much of a concern. - var binIndex = (int)binIndicesForLeafContinuous.Z; - if (binIndex >= splitIndex) - { - //Belongs to B. Swap it. - var targetIndex = leafCount - bCount - 1; - Helpers.Swap(ref indices[targetIndex], ref indices[aCount]); - Helpers.Swap(ref boundingBoxes[targetIndex], ref boundingBoxes[aCount]); - ++bCount; - //(Note that we still need to examine what we just swapped into the slot! It may belong to B too!) - } - else - { - //Belongs to A, no movement necessary. - ++aCount; - } - } - } - - { - Debug.Assert(aCount + bCount == leafCount); - BuildNode(bestboundsA, bestboundsB, nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, aCount, bCount, out var aIndex, out var bIndex); - if (aCount > 1) - BinnedBuilderInternal(indices.Slice(aCount), boundingBoxes.Slice(aCount), nodes.Slice(1, aCount - 1), metanodes.Slice(1, aCount - 1), aIndex, nodeIndex, 0, bins); - if (bCount > 1) - BinnedBuilderInternal(indices.Slice(aCount, bCount), boundingBoxes.Slice(aCount, bCount), nodes.Slice(aCount, bCount - 1), metanodes.Slice(aCount, bCount - 1), bIndex, nodeIndex, 1, bins); - } - - } - - public static unsafe void BinnedBuilder(Buffer indices, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, BufferPool pool) - { - var leafCount = indices.Length; - Debug.Assert(boundingBoxes.Length >= leafCount, "The bounding boxes provided must cover the range of indices provided."); - Debug.Assert(nodes.Length >= leafCount - 1, "The output nodes must be able to contain the nodes created for the leaves."); - if (leafCount == 0) - return; - if (leafCount == 1) - { - //If there's only one leaf, the tree has a special format: the root node has only one child. - ref var root = ref nodes[0]; - root.A.Min = boundingBoxes[0].Min; - root.A.Index = indices[0]; //Node that we assume the indices are already encoded. This function works with subtree refinements as well which can manage either leaves or internals. - root.A.Max = boundingBoxes[0].Max; - root.A.LeafCount = 1; - root.B = default; - return; - } - boundingBoxes = boundingBoxes.Slice(indices.Length); - nodes = nodes.Slice(leafCount - 1); - - var binBoundsMemory = stackalloc BoundingBox4[MaximumBinCountRevamp * 6]; - Bins bins; - bins.BinBoundingBoxesX = new Buffer(binBoundsMemory, MaximumBinCountRevamp); - bins.BinBoundingBoxesY = new Buffer(binBoundsMemory + MaximumBinCountRevamp, MaximumBinCountRevamp); - bins.BinBoundingBoxesZ = new Buffer(binBoundsMemory + MaximumBinCountRevamp * 2, MaximumBinCountRevamp); - bins.BinBoundingBoxesScanX = new Buffer(binBoundsMemory + MaximumBinCountRevamp * 3, MaximumBinCountRevamp); - bins.BinBoundingBoxesScanY = new Buffer(binBoundsMemory + MaximumBinCountRevamp * 4, MaximumBinCountRevamp); - bins.BinBoundingBoxesScanZ = new Buffer(binBoundsMemory + MaximumBinCountRevamp * 5, MaximumBinCountRevamp); - - var binLeafCountsMemory = stackalloc int[MaximumBinCountRevamp * 3]; - bins.BinLeafCountsX = new Buffer(binLeafCountsMemory, MaximumBinCountRevamp); - bins.BinLeafCountsY = new Buffer(binLeafCountsMemory + MaximumBinCountRevamp, MaximumBinCountRevamp); - bins.BinLeafCountsZ = new Buffer(binLeafCountsMemory + MaximumBinCountRevamp * 2, MaximumBinCountRevamp); - - //While we could avoid a recursive implementation, the overhead is low compared to the per-iteration cost. - BinnedBuilderInternal(indices, boundingBoxes.As(), nodes, metanodes, 0, -1, -1, bins); - } - - - static unsafe void MicroSweepForBinnedBuilderSingleAxis(Vector4 centroidMin, Vector4 centroidMax, Buffer indices, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, BinsSingleAxis bins) + static unsafe void MicroSweepForBinnedBuilder(Vector4 centroidMin, Vector4 centroidMax, Buffer indices, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, BinsSingleAxis bins) { //This is a very small scale sweep build. var leafCount = indices.Length; @@ -693,7 +225,7 @@ static unsafe void MicroSweepForBinnedBuilderSingleAxis(Vector4 centroidMin, Vec centroidBoundsA.Min = Vector4.Min(centroidBoundsA.Min, bounds.Min); centroidBoundsA.Max = Vector4.Max(centroidBoundsA.Max, bounds.Max); } - MicroSweepForBinnedBuilderSingleAxis(centroidBoundsA.Min, centroidBoundsA.Max, indices.Slice(aCount), aBounds, nodes.Slice(1, aCount - 1), metanodes.Slice(1, aCount - 1), aIndex, nodeIndex, 0, bins); + MicroSweepForBinnedBuilder(centroidBoundsA.Min, centroidBoundsA.Max, indices.Slice(aCount), aBounds, nodes.Slice(1, aCount - 1), metanodes.Slice(1, aCount - 1), aIndex, nodeIndex, 0, bins); } if (bCount > 1) { @@ -705,12 +237,12 @@ static unsafe void MicroSweepForBinnedBuilderSingleAxis(Vector4 centroidMin, Vec centroidBoundsB.Min = Vector4.Min(centroidBoundsB.Min, bounds.Min); centroidBoundsB.Max = Vector4.Max(centroidBoundsB.Max, bounds.Max); } - MicroSweepForBinnedBuilderSingleAxis(centroidBoundsB.Min, centroidBoundsB.Max, indices.Slice(aCount, bCount), bBounds, nodes.Slice(aCount, bCount - 1), metanodes.Slice(aCount, bCount - 1), bIndex, nodeIndex, 1, bins); + MicroSweepForBinnedBuilder(centroidBoundsB.Min, centroidBoundsB.Max, indices.Slice(aCount, bCount), bBounds, nodes.Slice(aCount, bCount - 1), metanodes.Slice(aCount, bCount - 1), bIndex, nodeIndex, 1, bins); } } } - static unsafe void BinnedBuilderInternalSingleAxis(Buffer indices, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, in BinsSingleAxis bins) + static unsafe void BinnedBuilderInternal(Buffer indices, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, in BinsSingleAxis bins) { var centroidMin = new Vector4(float.MaxValue); var centroidMax = new Vector4(float.MinValue); @@ -752,9 +284,9 @@ static unsafe void BinnedBuilderInternalSingleAxis(Buffer indices, Buffer 1) - BinnedBuilderInternalSingleAxis(indices.Slice(countA), boundingBoxes.Slice(countA), nodes.Slice(1, countA - 1), metanodes.Slice(1, countA - 1), aIndex, nodeIndex, 0, bins); + BinnedBuilderInternal(indices.Slice(countA), boundingBoxes.Slice(countA), nodes.Slice(1, countA - 1), metanodes.Slice(1, countA - 1), aIndex, nodeIndex, 0, bins); if (countB > 1) - BinnedBuilderInternalSingleAxis(indices.Slice(countA, countB), boundingBoxes.Slice(countA, countB), nodes.Slice(countA, countB - 1), metanodes.Slice(countA, countB - 1), bIndex, nodeIndex, 1, bins); + BinnedBuilderInternal(indices.Slice(countA, countB), boundingBoxes.Slice(countA, countB), nodes.Slice(countA, countB - 1), metanodes.Slice(countA, countB - 1), bIndex, nodeIndex, 1, bins); return; } if (leafCount == 2) @@ -765,14 +297,14 @@ static unsafe void BinnedBuilderInternalSingleAxis(Buffer indices, Buffer centroidSpan.Y && centroidSpan.X > centroidSpan.Z; var useY = centroidSpan.Y > centroidSpan.Z; - var binCount = Math.Min(MaximumBinCountRevamp, Math.Max((int)(leafCount * 0.1f), 16)); + var binCount = Math.Min(MaximumBinCountRevamp, Math.Max((int)(leafCount * .1f), 16)); Debug.Assert(bins.BinBoundingBoxes.Length >= binCount); var offsetToBinIndex = new Vector4(binCount) / centroidSpan; @@ -881,9 +413,9 @@ static unsafe void BinnedBuilderInternalSingleAxis(Buffer indices, Buffer 1) - BinnedBuilderInternalSingleAxis(indices.Slice(aCount), boundingBoxes.Slice(aCount), nodes.Slice(1, aCount - 1), metanodes.Slice(1, aCount - 1), aIndex, nodeIndex, 0, bins); + BinnedBuilderInternal(indices.Slice(aCount), boundingBoxes.Slice(aCount), nodes.Slice(1, aCount - 1), metanodes.Slice(1, aCount - 1), aIndex, nodeIndex, 0, bins); if (bCount > 1) - BinnedBuilderInternalSingleAxis(indices.Slice(aCount, bCount), boundingBoxes.Slice(aCount, bCount), nodes.Slice(aCount, bCount - 1), metanodes.Slice(aCount, bCount - 1), bIndex, nodeIndex, 1, bins); + BinnedBuilderInternal(indices.Slice(aCount, bCount), boundingBoxes.Slice(aCount, bCount), nodes.Slice(aCount, bCount - 1), metanodes.Slice(aCount, bCount - 1), bIndex, nodeIndex, 1, bins); } } @@ -893,7 +425,7 @@ struct BinsSingleAxis public Buffer BinBoundingBoxesScan; public Buffer BinLeafCounts; } - public static unsafe void BinnedBuilderSingleAxis(Buffer indices, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, BufferPool pool) + public static unsafe void BinnedBuilder(Buffer indices, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, BufferPool pool) { var leafCount = indices.Length; Debug.Assert(boundingBoxes.Length >= leafCount, "The bounding boxes provided must cover the range of indices provided."); @@ -923,7 +455,7 @@ public static unsafe void BinnedBuilderSingleAxis(Buffer indices, Buffer(binLeafCountsMemory, MaximumBinCountRevamp); //While we could avoid a recursive implementation, the overhead is low compared to the per-iteration cost. - BinnedBuilderInternalSingleAxis(indices, boundingBoxes.As(), nodes, metanodes, 0, -1, -1, bins); + BinnedBuilderInternal(indices, boundingBoxes.As(), nodes, metanodes, 0, -1, -1, bins); } } diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index 0d0db971a..3c426f520 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -167,6 +167,7 @@ public override void Initialize(ContentArchive content, Camera camera) var triangles = CreateRandomSoupTriangles(new BoundingBox(new(width / -2f, scale.Y * -2, height / -2f), new(width / 2f, scale.Y * 2, height / 2f)), (width - 1) * (height - 1) * 2, 0.5f, 100f); var mesh = new Mesh(triangles, Vector3.One, BufferPool); + Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); Console.WriteLine($"node count: {mesh.Tree.NodeCount}"); Console.WriteLine($"sweep SAH: {mesh.Tree.MeasureCostMetric()}, cache quality: {mesh.Tree.MeasureCacheQuality()}"); @@ -181,27 +182,11 @@ public override void Initialize(ContentArchive content, Camera camera) bounds.Min = Vector3.Min(t.A, Vector3.Min(t.B, t.C)); bounds.Max = Vector3.Max(t.A, Vector3.Max(t.B, t.C)); leafIndices[i] = Tree.Encode(i); - } - //BinnedTest(() => - //{ - // Tree.BinnedBuilder(leafIndices, leafBounds, mesh.Tree.Nodes, mesh.Tree.Metanodes, BufferPool); - // for (int i = 0; i < mesh.Tree.NodeCount; ++i) - // { - // ref var node = ref mesh.Tree.Nodes[i]; - // ref var a = ref node.A; - // ref var b = ref node.B; - // if (a.Index < 0) - // mesh.Tree.Leaves[Tree.Encode(a.Index)] = new Leaf(i, 0); - // if (b.Index < 0) - // mesh.Tree.Leaves[Tree.Encode(b.Index)] = new Leaf(i, 1); - // } - //}, "Revamp", ref mesh.Tree); - - Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); + } BinnedTest(() => { - Tree.BinnedBuilderSingleAxis(leafIndices, leafBounds, mesh.Tree.Nodes, mesh.Tree.Metanodes, BufferPool); + Tree.BinnedBuilder(leafIndices, leafBounds, mesh.Tree.Nodes, mesh.Tree.Metanodes, BufferPool); for (int i = 0; i < mesh.Tree.NodeCount; ++i) { ref var node = ref mesh.Tree.Nodes[i]; @@ -214,17 +199,17 @@ public override void Initialize(ContentArchive content, Camera camera) } }, "Revamp Single Axis", ref mesh.Tree); - var mesh2 = new Mesh(triangles, Vector3.One, BufferPool); + //var mesh2 = new Mesh(triangles, Vector3.One, BufferPool); - QuickList subtreeReferences = new(mesh2.Tree.LeafCount, BufferPool); - QuickList treeletInternalNodes = new(mesh2.Tree.LeafCount, BufferPool); - Tree.CreateBinnedResources(BufferPool, mesh2.Tree.LeafCount, out var binnedResourcesBuffer, out var binnedResources); - BinnedTest(() => - { - subtreeReferences.Count = 0; - treeletInternalNodes.Count = 0; - mesh2.Tree.BinnedRefine(0, ref subtreeReferences, mesh2.Tree.LeafCount, ref treeletInternalNodes, ref binnedResources, BufferPool); - }, "Original", ref mesh2.Tree); + //QuickList subtreeReferences = new(mesh2.Tree.LeafCount, BufferPool); + //QuickList treeletInternalNodes = new(mesh2.Tree.LeafCount, BufferPool); + //Tree.CreateBinnedResources(BufferPool, mesh2.Tree.LeafCount, out var binnedResourcesBuffer, out var binnedResources); + //BinnedTest(() => + //{ + // subtreeReferences.Count = 0; + // treeletInternalNodes.Count = 0; + // mesh2.Tree.BinnedRefine(0, ref subtreeReferences, mesh2.Tree.LeafCount, ref treeletInternalNodes, ref binnedResources, BufferPool); + //}, "Original", ref mesh2.Tree); //RefitTest(() => mesh.Tree.Refit2(), "refit2", ref mesh.Tree); //RefitTest(() => mesh.Tree.Refit(), "Original", ref mesh.Tree); From 5e8e2a5fe20d048752c6b9d905bdab4e89a3fc6f Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 20 Sep 2022 19:27:08 -0500 Subject: [PATCH 611/947] Some microoptimizations. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 89 ++++++++++++------- .../SpecializedTests/TreeFiddlingTestDemo.cs | 3 +- 2 files changed, 57 insertions(+), 35 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 94c7d6941..a872ab394 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -9,6 +9,7 @@ using System.Numerics; using System.Runtime.CompilerServices; using System.Runtime.Intrinsics; +using System.Runtime.Intrinsics.Arm; using System.Runtime.Intrinsics.X86; using System.Threading.Tasks.Sources; @@ -54,20 +55,6 @@ static void BuildNode(BoundingBox4 a, BoundingBox4 b, Buffer nodes, Buffer node.B.LeafCount = bCount; } - struct Bins - { - public Buffer BinBoundingBoxesX; - public Buffer BinBoundingBoxesY; - public Buffer BinBoundingBoxesZ; - public Buffer BinBoundingBoxesScanX; - public Buffer BinBoundingBoxesScanY; - public Buffer BinBoundingBoxesScanZ; - - public Buffer BinLeafCountsX; - public Buffer BinLeafCountsY; - public Buffer BinLeafCountsZ; - } - internal static float ComputeBoundsMetric(BoundingBox4 bounds) { return ComputeBoundsMetric(bounds.Min, bounds.Max); @@ -82,6 +69,12 @@ internal static float ComputeBoundsMetric(Vector4 min, Vector4 max) return offset.X * offset.Y + offset.Y * offset.Z + offset.Z * offset.X; } + struct Bins + { + public Buffer BinBoundingBoxes; + public Buffer BinBoundingBoxesScan; + public Buffer BinLeafCounts; + } struct BoundsComparerX : IComparerRef { @@ -114,7 +107,7 @@ struct BoundsComparerZ : IComparerRef // max = Vector3.Max(nodes[nodeIndex].B.Max, nodes[nodeIndex].B.Max); //} - static unsafe void MicroSweepForBinnedBuilder(Vector4 centroidMin, Vector4 centroidMax, Buffer indices, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, BinsSingleAxis bins) + static unsafe void MicroSweepForBinnedBuilder(Vector4 centroidMin, Vector4 centroidMax, Buffer indices, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, Bins bins) { //This is a very small scale sweep build. var leafCount = indices.Length; @@ -242,7 +235,32 @@ static unsafe void MicroSweepForBinnedBuilder(Vector4 centroidMin, Vector4 centr } } - static unsafe void BinnedBuilderInternal(Buffer indices, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, in BinsSingleAxis bins) + [MethodImpl(MethodImplOptions.AggressiveInlining)] + private static unsafe int ComputeBinIndex(Vector4 centroidMin, bool useX, bool useY, Vector128 permuteMask, int axisIndex, Vector4 offsetToBinIndex, Vector4 maximumBinIndex, in BoundingBox4 box) + { + var centroid = box.Min + box.Max; + var binIndicesForLeafContinuous = Vector4.Min(maximumBinIndex, (centroid - centroidMin) * offsetToBinIndex); + //Note that we don't store out any of the indices into per-bin lists here. We only *really* want two final groups for the children, + //and we can easily compute those by performing another scan. It requires recomputing the bin indices, but that's really not much of a concern. + int binIndex; + //To extract the desired lane, we need to use a variable shuffle mask. At the time of writing, the Vector128 cross platform shuffle did not like variable masks. + if (Avx.IsSupported) + { + binIndex = (int)Vector128.ToScalar(Avx.PermuteVar(binIndicesForLeafContinuous.AsVector128(), permuteMask)); + } + else if (Vector128.IsHardwareAccelerated) + { + binIndex = (int)Vector128.GetElement(binIndicesForLeafContinuous.AsVector128(), axisIndex); + } + else + { + binIndex = (int)(useX ? binIndicesForLeafContinuous.X : useY ? binIndicesForLeafContinuous.Y : binIndicesForLeafContinuous.Z); + } + + return binIndex; + } + + static unsafe void BinnedBuilderInternal(Buffer indices, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, in Bins bins) { var centroidMin = new Vector4(float.MaxValue); var centroidMax = new Vector4(float.MinValue); @@ -303,6 +321,9 @@ static unsafe void BinnedBuilderInternal(Buffer indices, Buffer centroidSpan.Y && centroidSpan.X > centroidSpan.Z; var useY = centroidSpan.Y > centroidSpan.Z; + //These will be used conditionally based on what hardware acceleration is available. Pretty minor detail. + var permuteMask = Vector128.Create(useX ? 0 : useY ? 1 : 2, 0, 0, 0); + var axisIndex = useX ? 0 : useY ? 1 : 2; var binCount = Math.Min(MaximumBinCountRevamp, Math.Max((int)(leafCount * .1f), 16)); Debug.Assert(bins.BinBoundingBoxes.Length >= binCount); @@ -324,15 +345,13 @@ static unsafe void BinnedBuilderInternal(Buffer indices, Buffer indices, Buffer= splitIndex) { //Belongs to B. Swap it. var targetIndex = leafCount - bCount - 1; Helpers.Swap(ref indices[targetIndex], ref indices[aCount]); - Helpers.Swap(ref boundingBoxes[targetIndex], ref boundingBoxes[aCount]); + if (Vector256.IsHardwareAccelerated) + { + var targetMemory = (byte*)(boundingBoxes.Memory + targetIndex); + var aCountMemory = (byte*)(boundingBoxes.Memory + aCount); + var targetVector = Vector256.Load(targetMemory); + var aCountVector = Vector256.Load(aCountMemory); + Vector256.Store(aCountVector, targetMemory); + Vector256.Store(targetVector, aCountMemory); + } + else + { + Helpers.Swap(ref boundingBoxes[targetIndex], ref boundingBoxes[aCount]); + } ++bCount; //(Note that we still need to examine what we just swapped into the slot! It may belong to B too!) } @@ -419,12 +446,6 @@ static unsafe void BinnedBuilderInternal(Buffer indices, Buffer BinBoundingBoxes; - public Buffer BinBoundingBoxesScan; - public Buffer BinLeafCounts; - } public static unsafe void BinnedBuilder(Buffer indices, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, BufferPool pool) { var leafCount = indices.Length; @@ -447,7 +468,7 @@ public static unsafe void BinnedBuilder(Buffer indices, Buffer nodes = nodes.Slice(leafCount - 1); var binBoundsMemory = stackalloc BoundingBox4[MaximumBinCountRevamp * 2]; - BinsSingleAxis bins; + Bins bins; bins.BinBoundingBoxes = new Buffer(binBoundsMemory, MaximumBinCountRevamp); bins.BinBoundingBoxesScan = new Buffer(binBoundsMemory + MaximumBinCountRevamp, MaximumBinCountRevamp); diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index 3c426f520..265c830bb 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -182,7 +182,7 @@ public override void Initialize(ContentArchive content, Camera camera) bounds.Min = Vector3.Min(t.A, Vector3.Min(t.B, t.C)); bounds.Max = Vector3.Max(t.A, Vector3.Max(t.B, t.C)); leafIndices[i] = Tree.Encode(i); - } + } BinnedTest(() => { @@ -199,6 +199,7 @@ public override void Initialize(ContentArchive content, Camera camera) } }, "Revamp Single Axis", ref mesh.Tree); + //var mesh2 = new Mesh(triangles, Vector3.One, BufferPool); //QuickList subtreeReferences = new(mesh2.Tree.LeafCount, BufferPool); From 47c82c71872156217aa0b7ef4d78888d2821693e Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 20 Sep 2022 19:58:51 -0500 Subject: [PATCH 612/947] I refuse. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 30 +++++++++++++++++++++---- 1 file changed, 26 insertions(+), 4 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index a872ab394..026620689 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -1,4 +1,5 @@ -using BepuPhysics.Constraints; +using BepuPhysics.Collidables; +using BepuPhysics.Constraints; using BepuUtilities; using BepuUtilities.Collections; using BepuUtilities.Memory; @@ -341,13 +342,34 @@ static unsafe void BinnedBuilderInternal(Buffer indices, Buffer Date: Tue, 20 Sep 2022 19:59:26 -0500 Subject: [PATCH 613/947] Refusal complete. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 21 +-------------------- 1 file changed, 1 insertion(+), 20 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 026620689..08b2fe077 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -344,29 +344,10 @@ static unsafe void BinnedBuilderInternal(Buffer indices, Buffer Date: Tue, 20 Sep 2022 20:47:17 -0500 Subject: [PATCH 614/947] Oops. --- BepuUtilities/Collections/MSBRadixSort.cs | 2 -- 1 file changed, 2 deletions(-) diff --git a/BepuUtilities/Collections/MSBRadixSort.cs b/BepuUtilities/Collections/MSBRadixSort.cs index 1925b141c..36c2e9e2d 100644 --- a/BepuUtilities/Collections/MSBRadixSort.cs +++ b/BepuUtilities/Collections/MSBRadixSort.cs @@ -155,9 +155,7 @@ public unsafe static void SortU32(ref int keys, ref T values, int keyCount, i const int mask = bucketCount - 1; //This stackalloc isn't actually super fast- the default behavior is to zero out the range. But we actually want it to be zeroed, so that's okay. var bucketCounts = stackalloc int[bucketCount]; -#if RELEASESTRIP Unsafe.InitBlockUnaligned(bucketCounts, 0, sizeof(int) * bucketCount); -#endif #if DEBUG for (int i = 0; i < bucketCount; ++i) { From 878783dabc7c649291b9880155f960bd842b2faa Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 21 Sep 2022 15:29:44 -0500 Subject: [PATCH 615/947] Preparing for some sortfun. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 84 +++++++++++++++++++++++-- 1 file changed, 78 insertions(+), 6 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 08b2fe077..381cd9c9f 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -1,5 +1,6 @@ using BepuPhysics.Collidables; using BepuPhysics.Constraints; +using BepuPhysics.Constraints.Contact; using BepuUtilities; using BepuUtilities.Collections; using BepuUtilities.Memory; @@ -9,6 +10,7 @@ using System.Net; using System.Numerics; using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; using System.Runtime.Intrinsics; using System.Runtime.Intrinsics.Arm; using System.Runtime.Intrinsics.X86; @@ -75,6 +77,7 @@ struct Bins public Buffer BinBoundingBoxes; public Buffer BinBoundingBoxesScan; public Buffer BinLeafCounts; + } struct BoundsComparerX : IComparerRef @@ -90,6 +93,7 @@ struct BoundsComparerZ : IComparerRef public int Compare(ref BoundingBox4 a, ref BoundingBox4 b) => (a.Min.Z + a.Max.Z) > (b.Min.Z + b.Max.Z) ? -1 : 1; } + //static void ValidateNode(int nodeIndex, Buffer nodes, out Vector3 min, out Vector3 max) //{ // if (nodes[nodeIndex].A.Index >= 0) @@ -108,6 +112,21 @@ struct BoundsComparerZ : IComparerRef // max = Vector3.Max(nodes[nodeIndex].B.Max, nodes[nodeIndex].B.Max); //} + [StructLayout(LayoutKind.Sequential)] + struct BoundsSortable + { + public float Key; + public int Index; + } + + struct AxisComparer : IComparerRef + { + public int Compare(ref BoundsSortable a, ref BoundsSortable b) + { + return a.Key > b.Key ? -1 : 1; + } + } + static unsafe void MicroSweepForBinnedBuilder(Vector4 centroidMin, Vector4 centroidMax, Buffer indices, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, Bins bins) { //This is a very small scale sweep build. @@ -118,22 +137,75 @@ static unsafe void MicroSweepForBinnedBuilder(Vector4 centroidMin, Vector4 centr return; } var centroidSpan = centroidMax - centroidMin; + + Debug.Assert(bins.BinBoundingBoxes.Length >= leafCount); + //Repurpose the bins memory so we don't need to allocate any extra. The bins aren't in use right now anyway. + var sortables = new Buffer(bins.BinBoundingBoxes.Memory, leafCount); + var boundingBoxCache = bins.BinBoundingBoxesScan; + var indicesCache = new Buffer((int*)((BoundsSortable*)bins.BinBoundingBoxes.Memory + leafCount), leafCount); + + //Store the bounds/indices into temporary memory so that we can shuffle them trivially once we're done with the sort. + //Doing this as a prepass means we don't have to worry about doing heavy swaps in the sort. + boundingBoxes.CopyTo(0, boundingBoxCache, 0, leafCount); + indices.CopyTo(0, indicesCache, 0, leafCount); + //Compute the axis centroids up front to avoid having to recompute them during a sort. if (centroidSpan.X > centroidSpan.Y && centroidSpan.X > centroidSpan.Z) { - var comparer = new BoundsComparerX(); - QuickSort.Sort(ref boundingBoxes[0], ref indices[0], 0, leafCount - 1, ref comparer); + for (int i = 0; i < leafCount; ++i) + { + ref var bounds = ref boundingBoxes[i]; + ref var target = ref sortables[i]; + target.Key = (bounds.Min + bounds.Max).X; + target.Index = i; + } } else if (centroidSpan.Y > centroidSpan.Z) { - var comparer = new BoundsComparerY(); - QuickSort.Sort(ref boundingBoxes[0], ref indices[0], 0, leafCount - 1, ref comparer); + for (int i = 0; i < leafCount; ++i) + { + ref var bounds = ref boundingBoxes[i]; + ref var target = ref sortables[i]; + target.Key = (bounds.Min + bounds.Max).Y; + target.Index = i; + } } else { - var comparer = new BoundsComparerZ(); - QuickSort.Sort(ref boundingBoxes[0], ref indices[0], 0, leafCount - 1, ref comparer); + for (int i = 0; i < leafCount; ++i) + { + ref var bounds = ref boundingBoxes[i]; + ref var target = ref sortables[i]; + target.Key = (bounds.Min + bounds.Max).Z; + target.Index = i; + } + } + var comparer2 = new AxisComparer(); + QuickSort.Sort(ref sortables[0], 0, leafCount - 1, ref comparer2); + + //Now that we know the target indices, copy things back. + for (int i =0; i < leafCount; ++i) + { + var sourceIndex = sortables[i].Index; + boundingBoxes[i] = boundingBoxCache[sourceIndex]; + indices[i] = indicesCache[sourceIndex]; } + //if (centroidSpan.X > centroidSpan.Y && centroidSpan.X > centroidSpan.Z) + //{ + // var comparer = new BoundsComparerX(); + // QuickSort.Sort(ref boundingBoxes[0], ref indices[0], 0, leafCount - 1, ref comparer); + //} + //else if (centroidSpan.Y > centroidSpan.Z) + //{ + // var comparer = new BoundsComparerY(); + // QuickSort.Sort(ref boundingBoxes[0], ref indices[0], 0, leafCount - 1, ref comparer); + //} + //else + //{ + // var comparer = new BoundsComparerZ(); + // QuickSort.Sort(ref boundingBoxes[0], ref indices[0], 0, leafCount - 1, ref comparer); + //} + Debug.Assert(leafCount <= MaximumBinCountRevamp, "We're reusing the bin resources under the assumption that this is only ever called when there are less leaves than maximum bins."); //Identify the split index by examining the SAH of very split option. //Premerge from left to right so we have a sorta-summed area table to cheaply look up all possible child A bounds as we scan. From b617ec2695d7cd82ea7c98f7e5b9bab423a0cfa2 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 21 Sep 2022 19:24:47 -0500 Subject: [PATCH 616/947] Simple vectorsort for microsweep. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 100 ++++++++++++++---- BepuUtilities/Collections/SpeculativeSorts.cs | 53 ++++++++++ 2 files changed, 135 insertions(+), 18 deletions(-) create mode 100644 BepuUtilities/Collections/SpeculativeSorts.cs diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 381cd9c9f..2e43f348b 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -138,11 +138,14 @@ static unsafe void MicroSweepForBinnedBuilder(Vector4 centroidMin, Vector4 centr } var centroidSpan = centroidMax - centroidMin; - Debug.Assert(bins.BinBoundingBoxes.Length >= leafCount); //Repurpose the bins memory so we don't need to allocate any extra. The bins aren't in use right now anyway. - var sortables = new Buffer(bins.BinBoundingBoxes.Memory, leafCount); + //TODO: this assumes Vector256 widths! + var paddedKeyCount = ((leafCount + 7) / 8) * 8; + Debug.Assert(Unsafe.SizeOf() * bins.BinBoundingBoxes.Length >= (paddedKeyCount + 2 * leafCount) * Unsafe.SizeOf()); + var keys = new Buffer(bins.BinBoundingBoxes.Memory, paddedKeyCount); + var indicesCache = new Buffer((int*)keys.Memory + paddedKeyCount, leafCount); + var targetIndices = new Buffer(indicesCache.Memory + leafCount, leafCount); var boundingBoxCache = bins.BinBoundingBoxesScan; - var indicesCache = new Buffer((int*)((BoundsSortable*)bins.BinBoundingBoxes.Memory + leafCount), leafCount); //Store the bounds/indices into temporary memory so that we can shuffle them trivially once we're done with the sort. //Doing this as a prepass means we don't have to worry about doing heavy swaps in the sort. @@ -154,9 +157,7 @@ static unsafe void MicroSweepForBinnedBuilder(Vector4 centroidMin, Vector4 centr for (int i = 0; i < leafCount; ++i) { ref var bounds = ref boundingBoxes[i]; - ref var target = ref sortables[i]; - target.Key = (bounds.Min + bounds.Max).X; - target.Index = i; + keys[i] = (bounds.Min + bounds.Max).X; } } else if (centroidSpan.Y > centroidSpan.Z) @@ -164,9 +165,7 @@ static unsafe void MicroSweepForBinnedBuilder(Vector4 centroidMin, Vector4 centr for (int i = 0; i < leafCount; ++i) { ref var bounds = ref boundingBoxes[i]; - ref var target = ref sortables[i]; - target.Key = (bounds.Min + bounds.Max).Y; - target.Index = i; + keys[i] = (bounds.Min + bounds.Max).Y; } } else @@ -174,21 +173,86 @@ static unsafe void MicroSweepForBinnedBuilder(Vector4 centroidMin, Vector4 centr for (int i = 0; i < leafCount; ++i) { ref var bounds = ref boundingBoxes[i]; - ref var target = ref sortables[i]; - target.Key = (bounds.Min + bounds.Max).Z; - target.Index = i; + keys[i] = (bounds.Min + bounds.Max).Z; } } - var comparer2 = new AxisComparer(); - QuickSort.Sort(ref sortables[0], 0, leafCount - 1, ref comparer2); + for (int i = leafCount; i < paddedKeyCount; ++i) + { + keys[i] = float.MaxValue; + } + SpeculativeSorts.VectorCountingSort(keys, targetIndices); //Now that we know the target indices, copy things back. - for (int i =0; i < leafCount; ++i) + for (int i = 0; i < leafCount; ++i) { - var sourceIndex = sortables[i].Index; - boundingBoxes[i] = boundingBoxCache[sourceIndex]; - indices[i] = indicesCache[sourceIndex]; + var targetIndex = targetIndices[i]; + boundingBoxes[targetIndex] = boundingBoxCache[i]; + indices[targetIndex] = indicesCache[i]; } + //var previousValue = float.MinValue; + //for (int i = 0; i < leafCount; ++i) + //{ + // var centroid = boundingBoxes[i].Min + boundingBoxes[i].Max; + // var axis = centroidSpan.X > centroidSpan.Y && centroidSpan.X > centroidSpan.Z ? centroid.X : centroidSpan.Y > centroidSpan.Z ? centroid.Y : centroid.Z; + // if (axis < previousValue) + // Console.WriteLine("BAD"); + // Console.WriteLine($"centroid {i}: {axis}"); + //} + + //----------------- + + ////Repurpose the bins memory so we don't need to allocate any extra. The bins aren't in use right now anyway. + //var sortables = new Buffer(bins.BinBoundingBoxes.Memory, leafCount); + //var boundingBoxCache = bins.BinBoundingBoxesScan; + //var indicesCache = new Buffer((int*)((BoundsSortable*)bins.BinBoundingBoxes.Memory + leafCount), leafCount); + + ////Store the bounds/indices into temporary memory so that we can shuffle them trivially once we're done with the sort. + ////Doing this as a prepass means we don't have to worry about doing heavy swaps in the sort. + //boundingBoxes.CopyTo(0, boundingBoxCache, 0, leafCount); + //indices.CopyTo(0, indicesCache, 0, leafCount); + ////Compute the axis centroids up front to avoid having to recompute them during a sort. + //if (centroidSpan.X > centroidSpan.Y && centroidSpan.X > centroidSpan.Z) + //{ + // for (int i = 0; i < leafCount; ++i) + // { + // ref var bounds = ref boundingBoxes[i]; + // ref var target = ref sortables[i]; + // target.Key = (bounds.Min + bounds.Max).X; + // target.Index = i; + // } + //} + //else if (centroidSpan.Y > centroidSpan.Z) + //{ + // for (int i = 0; i < leafCount; ++i) + // { + // ref var bounds = ref boundingBoxes[i]; + // ref var target = ref sortables[i]; + // target.Key = (bounds.Min + bounds.Max).Y; + // target.Index = i; + // } + //} + //else + //{ + // for (int i = 0; i < leafCount; ++i) + // { + // ref var bounds = ref boundingBoxes[i]; + // ref var target = ref sortables[i]; + // target.Key = (bounds.Min + bounds.Max).Z; + // target.Index = i; + // } + //} + //var comparer2 = new AxisComparer(); + //QuickSort.Sort(ref sortables[0], 0, leafCount - 1, ref comparer2); + + ////Now that we know the target indices, copy things back. + //for (int i = 0; i < leafCount; ++i) + //{ + // var sourceIndex = sortables[i].Index; + // boundingBoxes[i] = boundingBoxCache[sourceIndex]; + // indices[i] = indicesCache[sourceIndex]; + //} + + //----------------- //if (centroidSpan.X > centroidSpan.Y && centroidSpan.X > centroidSpan.Z) //{ diff --git a/BepuUtilities/Collections/SpeculativeSorts.cs b/BepuUtilities/Collections/SpeculativeSorts.cs new file mode 100644 index 000000000..f78645397 --- /dev/null +++ b/BepuUtilities/Collections/SpeculativeSorts.cs @@ -0,0 +1,53 @@ +using BepuUtilities.Memory; +using System; +using System.Collections.Generic; +using System.Diagnostics; +using System.Linq; +using System.Numerics; +using System.Runtime.Intrinsics; +using System.Runtime.Intrinsics.X86; +using System.Text; +using System.Threading.Tasks; + +namespace BepuUtilities.Collections +{ + public static class SpeculativeSorts + { + public unsafe static void VectorCountingSort(Buffer paddedKeys, Buffer newIndices) + { + if (Avx2.IsSupported) + { + Debug.Assert(paddedKeys.length >= newIndices.length && paddedKeys.length % 8 == 0, "This implementation assumes the keys are pre-padded."); + var elementCount = newIndices.length; + var indexOffsets = Vector256.Create(0, 1, 2, 3, 4, 5, 6, 7); + for (int i = 0; i < elementCount; ++i) + { + //Broadcast the current value and test it against all other values. + //Count all values smaller than the current value, and all equal values that are in a lower index. + var previousCount = 0; + var value = Vector256.Create(paddedKeys[i]); + var currentIndex = Vector256.Create(i); + + //TODO: There's no reason to include the full loop complexity for bundles that come after the current index. + for (int j = 0; j < paddedKeys.Length; j += 8) + { + var testVector = Vector256.Load(paddedKeys.Memory + j); + var vectorIndices = indexOffsets + Vector256.Create(j); + var indexIsLesser = Vector256.LessThan(vectorIndices, currentIndex); + var slotIsEqual = Vector256.Equals(testVector, value); + var slotIsLesser = Vector256.LessThan(testVector, value); + var slotPrecedesValue = Vector256.BitwiseOr(slotIsLesser, Vector256.BitwiseAnd(slotIsEqual, indexIsLesser.AsSingle())); + + var slotPrecedesValueMask = Avx.MoveMask(slotPrecedesValue.AsSingle()); + + var slotsPrecedingValueCount = BitOperations.PopCount((uint)slotPrecedesValueMask); + + previousCount += slotsPrecedingValueCount; + } + newIndices[i] = previousCount; + } + } + + } + } +} From c615499debf47daedf95ee31a36567e4364ea7a3 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 21 Sep 2022 21:11:05 -0500 Subject: [PATCH 617/947] Vector counting sort, plus another half done variant. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 8 +- BepuUtilities/Collections/SpeculativeSorts.cs | 171 ++++++++++++++++-- 2 files changed, 162 insertions(+), 17 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 2e43f348b..4935df9d0 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -157,7 +157,7 @@ static unsafe void MicroSweepForBinnedBuilder(Vector4 centroidMin, Vector4 centr for (int i = 0; i < leafCount; ++i) { ref var bounds = ref boundingBoxes[i]; - keys[i] = (bounds.Min + bounds.Max).X; + keys[i] = bounds.Min.X + bounds.Max.X; } } else if (centroidSpan.Y > centroidSpan.Z) @@ -165,7 +165,7 @@ static unsafe void MicroSweepForBinnedBuilder(Vector4 centroidMin, Vector4 centr for (int i = 0; i < leafCount; ++i) { ref var bounds = ref boundingBoxes[i]; - keys[i] = (bounds.Min + bounds.Max).Y; + keys[i] = bounds.Min.Y + bounds.Max.Y; } } else @@ -173,14 +173,14 @@ static unsafe void MicroSweepForBinnedBuilder(Vector4 centroidMin, Vector4 centr for (int i = 0; i < leafCount; ++i) { ref var bounds = ref boundingBoxes[i]; - keys[i] = (bounds.Min + bounds.Max).Z; + keys[i] = bounds.Min.Z + bounds.Max.Z; } } for (int i = leafCount; i < paddedKeyCount; ++i) { keys[i] = float.MaxValue; } - SpeculativeSorts.VectorCountingSort(keys, targetIndices); + VectorizedSorts.VectorCountingSort(keys, targetIndices); //Now that we know the target indices, copy things back. for (int i = 0; i < leafCount; ++i) diff --git a/BepuUtilities/Collections/SpeculativeSorts.cs b/BepuUtilities/Collections/SpeculativeSorts.cs index f78645397..f7db0e370 100644 --- a/BepuUtilities/Collections/SpeculativeSorts.cs +++ b/BepuUtilities/Collections/SpeculativeSorts.cs @@ -11,14 +11,24 @@ namespace BepuUtilities.Collections { - public static class SpeculativeSorts + /// + /// Specialized sorts that can go fast sometimes, but often come with special requirements. + /// + public static class VectorizedSorts { - public unsafe static void VectorCountingSort(Buffer paddedKeys, Buffer newIndices) + /// + /// Computes the mapping from current index to sorted index for all elements in the paddedKeys. The paddedKeys buffer is not actually sorted. + /// + /// Padded keys to compute sorted indices for. This buffer's contents will not be mutated. + /// The buffer must be at least as long as the targetIndices buffer and must be padded to be divisible by vector length. + /// Vector length is 8 elements if or 4 elements if only . + /// Buffer to be filled with the target indices each slot should be moved to in order to sort the buffer. + public unsafe static void VectorCountingSort(Buffer paddedKeys, Buffer targetIndices) { - if (Avx2.IsSupported) + if (Vector256.IsHardwareAccelerated) { - Debug.Assert(paddedKeys.length >= newIndices.length && paddedKeys.length % 8 == 0, "This implementation assumes the keys are pre-padded."); - var elementCount = newIndices.length; + Debug.Assert(paddedKeys.length >= targetIndices.length && paddedKeys.length % 8 == 0, "This implementation assumes the keys are pre-padded."); + var elementCount = targetIndices.length; var indexOffsets = Vector256.Create(0, 1, 2, 3, 4, 5, 6, 7); for (int i = 0; i < elementCount; ++i) { @@ -28,26 +38,161 @@ public unsafe static void VectorCountingSort(Buffer paddedKeys, Buffer= targetIndices.length && paddedKeys.length % 4 == 0, "This implementation assumes the keys are pre-padded."); + var elementCount = targetIndices.length; + var indexOffsets = Vector128.Create(0, 1, 2, 3); + for (int i = 0; i < elementCount; ++i) + { + //Broadcast the current value and test it against all other values. + //Count all values smaller than the current value, and all equal values that are in a lower index. + var previousCount = 0; + var value = Vector128.Create(paddedKeys[i]); + var currentIndex = Vector128.Create(i); - var slotsPrecedingValueCount = BitOperations.PopCount((uint)slotPrecedesValueMask); + int testedSlots; + for (testedSlots = 0; testedSlots < i; testedSlots += 4) + { + var testVector = Vector128.Load(paddedKeys.Memory + testedSlots); + var vectorIndices = indexOffsets + Vector128.Create(testedSlots); + var indexIsLesser = Vector128.LessThan(vectorIndices, currentIndex); + var slotIsEqual = Vector128.Equals(testVector, value); + var slotIsLesser = Vector128.LessThan(testVector, value); + var slotPrecedesValue = Vector128.BitwiseOr(slotIsLesser, Vector128.BitwiseAnd(slotIsEqual, indexIsLesser.AsSingle())); + var slotPrecedesValueMask = Vector128.ExtractMostSignificantBits(slotPrecedesValue.AsSingle()); + var slotsPrecedingValueCount = BitOperations.PopCount(slotPrecedesValueMask); + + previousCount += slotsPrecedingValueCount; + } + + //For any slots after the query slot, there will be no 'lesser index but equal' entries, so the loop is simpler. + for (; testedSlots < paddedKeys.Length; testedSlots += 4) + { + var testVector = Vector128.Load(paddedKeys.Memory + testedSlots); + var slotPrecedesValue = Vector128.LessThan(testVector, value); + var slotPrecedesValueMask = Vector128.ExtractMostSignificantBits(slotPrecedesValue.AsSingle()); + var slotsPrecedingValueCount = BitOperations.PopCount(slotPrecedesValueMask); previousCount += slotsPrecedingValueCount; } - newIndices[i] = previousCount; + targetIndices[i] = previousCount; } } + else + { + //Shrug! Nonvector fallback. Really shouldn't call a function called VectorCountingSort if you don't have any vectorization. + for (int i = 0; i < targetIndices.Length; ++i) + { + targetIndices[i] = i; + } + var comparer = new PrimitiveComparer(); + QuickSort.Sort(ref paddedKeys[0], ref targetIndices[0], 0, targetIndices.Length - 1, ref comparer); + } + + } + + /// + /// Computes the mapping from current index to sorted index for all elements in the paddedKeys. The paddedKeys buffer is not actually sorted. + /// + /// Padded keys to compute sorted indices for. This buffer's contents will not be mutated. + /// The buffer must be at least as long as the targetIndices buffer and must be padded to be divisible by vector length. + /// Vector length is 8 elements if or 4 elements if only . + /// Buffer to be filled with the target indices each slot should be moved to in order to sort the buffer. + public unsafe static void VectorCountingSortTranspose(Buffer paddedKeys, Buffer targetIndices) + { + if (Vector256.IsHardwareAccelerated) + { + Debug.Assert(paddedKeys.length >= targetIndices.length && paddedKeys.length % 8 == 0, "This implementation assumes the keys are pre-padded."); + var elementCount = targetIndices.length; + var indexOffsets = Vector256.Create(0, 1, 2, 3, 4, 5, 6, 7); + for (int i = 0; i < elementCount; i += 8) + { + //Grab a bundle of values and test them against every other key. + var values = Vector256.Load(paddedKeys.Memory + i); + var counts = Vector256.Zero; + var slotIndex = Vector256.Create(i) + indexOffsets; + + var endOfEqualityTesting = i + 8; + //The first loop tests all vectors up to the start of the current bundle. + //These bundles do not require testing to see if the index is lesser; it definitely is. + for (int j = 0; j < i; ++j) + { + var testVector = Vector256.Create(paddedKeys[j]); + var slotPrecedesValue = Vector256.LessThanOrEqual(testVector, values); + + } + + int testedSlots; + for (testedSlots = 0; testedSlots < i; testedSlots += 8) + { + var testVector = Vector256.Load(paddedKeys.Memory + testedSlots); + var vectorIndices = indexOffsets + Vector256.Create(testedSlots); + var indexIsLesser = Vector256.LessThan(vectorIndices, currentIndex); + var slotIsEqual = Vector256.Equals(testVector, value); + var slotIsLesser = Vector256.LessThan(testVector, value); + var slotPrecedesValue = Vector256.BitwiseOr(slotIsLesser, Vector256.BitwiseAnd(slotIsEqual, indexIsLesser.AsSingle())); + var slotPrecedesValueMask = Vector256.ExtractMostSignificantBits(slotPrecedesValue.AsSingle()); + var slotsPrecedingValueCount = BitOperations.PopCount(slotPrecedesValueMask); + + previousCount += slotsPrecedingValueCount; + } + + //For any slots after the query slot, there will be no 'lesser index but equal' entries, so the loop is simpler. + for (; testedSlots < paddedKeys.Length; testedSlots += 8) + { + var testVector = Vector256.Load(paddedKeys.Memory + testedSlots); + var slotPrecedesValue = Vector256.LessThan(testVector, value); + var slotPrecedesValueMask = Vector256.ExtractMostSignificantBits(slotPrecedesValue.AsSingle()); + var slotsPrecedingValueCount = BitOperations.PopCount(slotPrecedesValueMask); + + previousCount += slotsPrecedingValueCount; + } + targetIndices[i] = previousCount; + } + } + else if (Vector128.IsHardwareAccelerated) + { + + } + else + { + //Shrug! Nonvector fallback. Really shouldn't call a function called VectorCountingSort if you don't have any vectorization. + for (int i = 0; i < targetIndices.Length; ++i) + { + targetIndices[i] = i; + } + var comparer = new PrimitiveComparer(); + QuickSort.Sort(ref paddedKeys[0], ref targetIndices[0], 0, targetIndices.Length - 1, ref comparer); + } } } -} From e759cecfbb484c46104cca9d99432fca4359c760 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 22 Sep 2022 14:43:56 -0500 Subject: [PATCH 618/947] Transpose variant complete. Very similar. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 7 +- BepuUtilities/Collections/SpeculativeSorts.cs | 167 ++++++++++++------ 2 files changed, 114 insertions(+), 60 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 4935df9d0..8062fa89a 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -141,10 +141,10 @@ static unsafe void MicroSweepForBinnedBuilder(Vector4 centroidMin, Vector4 centr //Repurpose the bins memory so we don't need to allocate any extra. The bins aren't in use right now anyway. //TODO: this assumes Vector256 widths! var paddedKeyCount = ((leafCount + 7) / 8) * 8; - Debug.Assert(Unsafe.SizeOf() * bins.BinBoundingBoxes.Length >= (paddedKeyCount + 2 * leafCount) * Unsafe.SizeOf()); + Debug.Assert(Unsafe.SizeOf() * bins.BinBoundingBoxes.Length >= (paddedKeyCount * 2 + leafCount) * Unsafe.SizeOf()); var keys = new Buffer(bins.BinBoundingBoxes.Memory, paddedKeyCount); - var indicesCache = new Buffer((int*)keys.Memory + paddedKeyCount, leafCount); - var targetIndices = new Buffer(indicesCache.Memory + leafCount, leafCount); + var targetIndices = new Buffer(keys.Memory + paddedKeyCount, leafCount); + var indicesCache = new Buffer(targetIndices.Memory + paddedKeyCount, leafCount); var boundingBoxCache = bins.BinBoundingBoxesScan; //Store the bounds/indices into temporary memory so that we can shuffle them trivially once we're done with the sort. @@ -181,6 +181,7 @@ static unsafe void MicroSweepForBinnedBuilder(Vector4 centroidMin, Vector4 centr keys[i] = float.MaxValue; } VectorizedSorts.VectorCountingSort(keys, targetIndices); + //VectorizedSorts.VectorCountingSortTranspose(keys, targetIndices, leafCount); //Now that we know the target indices, copy things back. for (int i = 0; i < leafCount; ++i) diff --git a/BepuUtilities/Collections/SpeculativeSorts.cs b/BepuUtilities/Collections/SpeculativeSorts.cs index f7db0e370..b57528b3a 100644 --- a/BepuUtilities/Collections/SpeculativeSorts.cs +++ b/BepuUtilities/Collections/SpeculativeSorts.cs @@ -17,12 +17,14 @@ namespace BepuUtilities.Collections public static class VectorizedSorts { /// - /// Computes the mapping from current index to sorted index for all elements in the paddedKeys. The paddedKeys buffer is not actually sorted. + /// Computes the mapping from current index to sorted index for all elements in the paddedKeys. The paddedKeys buffer is not actually sorted. + /// This function requires that or . /// /// Padded keys to compute sorted indices for. This buffer's contents will not be mutated. /// The buffer must be at least as long as the targetIndices buffer and must be padded to be divisible by vector length. /// Vector length is 8 elements if or 4 elements if only . /// Buffer to be filled with the target indices each slot should be moved to in order to sort the buffer. + /// This function requires that or . public unsafe static void VectorCountingSort(Buffer paddedKeys, Buffer targetIndices) { if (Vector256.IsHardwareAccelerated) @@ -38,11 +40,22 @@ public unsafe static void VectorCountingSort(Buffer paddedKeys, Buffer paddedKeys, Buffer paddedKeys, Buffer paddedKeys, Buffer paddedKeys, Buffer(); - QuickSort.Sort(ref paddedKeys[0], ref targetIndices[0], 0, targetIndices.Length - 1, ref comparer); + throw new NotSupportedException("This sort assumes that Vector256.IsHardwareAccelerated or Vector128.IsHardwareAccelerated. It is the caller's responsibility to guarantee this."); } } @@ -123,76 +141,111 @@ public unsafe static void VectorCountingSort(Buffer paddedKeys, Buffer /// Computes the mapping from current index to sorted index for all elements in the paddedKeys. The paddedKeys buffer is not actually sorted. + /// This function requires that or . /// /// Padded keys to compute sorted indices for. This buffer's contents will not be mutated. /// The buffer must be at least as long as the targetIndices buffer and must be padded to be divisible by vector length. /// Vector length is 8 elements if or 4 elements if only . - /// Buffer to be filled with the target indices each slot should be moved to in order to sort the buffer. - public unsafe static void VectorCountingSortTranspose(Buffer paddedKeys, Buffer targetIndices) + /// Buffer to be filled with the target indices each slot should be moved to in order to sort the buffer. + /// The buffer must be padded to be divisible by vector length. + /// Vector length is 8 elements if or 4 elements if only . + /// Number of elements + public unsafe static void VectorCountingSortTranspose(Buffer paddedKeys, Buffer paddedTargetIndices, int elementCount) { if (Vector256.IsHardwareAccelerated) { - Debug.Assert(paddedKeys.length >= targetIndices.length && paddedKeys.length % 8 == 0, "This implementation assumes the keys are pre-padded."); - var elementCount = targetIndices.length; + Debug.Assert(paddedKeys.length == paddedTargetIndices.length && paddedKeys.length % 8 == 0 && paddedKeys.Length >= elementCount, + "This implementation assumes the keys and target indices are pre-padded, and can contain the element count."); var indexOffsets = Vector256.Create(0, 1, 2, 3, 4, 5, 6, 7); for (int i = 0; i < elementCount; i += 8) { //Grab a bundle of values and test them against every other key. var values = Vector256.Load(paddedKeys.Memory + i); var counts = Vector256.Zero; - var slotIndex = Vector256.Create(i) + indexOffsets; - var endOfEqualityTesting = i + 8; + var oneMask = Vector256.Create(1); //The first loop tests all vectors up to the start of the current bundle. //These bundles do not require testing to see if the index is lesser; it definitely is. for (int j = 0; j < i; ++j) { var testVector = Vector256.Create(paddedKeys[j]); var slotPrecedesValue = Vector256.LessThanOrEqual(testVector, values); - + counts += Vector256.BitwiseAnd(slotPrecedesValue.AsInt32(), oneMask); } - int testedSlots; - for (testedSlots = 0; testedSlots < i; testedSlots += 8) + //For indices that overlap the current bundle, we need to check their relative position. + var endOfEqualityTesting = Math.Min(i + 8, elementCount); + var slotIndex = Vector256.Create(i) + indexOffsets; + for (int j = i; j < endOfEqualityTesting; ++j) { - var testVector = Vector256.Load(paddedKeys.Memory + testedSlots); - var vectorIndices = indexOffsets + Vector256.Create(testedSlots); - var indexIsLesser = Vector256.LessThan(vectorIndices, currentIndex); - var slotIsEqual = Vector256.Equals(testVector, value); - var slotIsLesser = Vector256.LessThan(testVector, value); - var slotPrecedesValue = Vector256.BitwiseOr(slotIsLesser, Vector256.BitwiseAnd(slotIsEqual, indexIsLesser.AsSingle())); - var slotPrecedesValueMask = Vector256.ExtractMostSignificantBits(slotPrecedesValue.AsSingle()); - var slotsPrecedingValueCount = BitOperations.PopCount(slotPrecedesValueMask); - - previousCount += slotsPrecedingValueCount; + var testVector = Vector256.Create(paddedKeys[j]); + var slotIsLesser = Vector256.LessThan(testVector, values); + var slotIndexIsLesser = Vector256.LessThan(Vector256.Create(j), slotIndex); + var slotIsEqual = Vector256.Equals(testVector, values); + var slotPrecedesValue = Vector256.BitwiseOr(slotIsLesser, Vector256.BitwiseAnd(slotIsEqual, slotIndexIsLesser.AsSingle())); + counts += Vector256.BitwiseAnd(slotPrecedesValue.AsInt32(), oneMask); } - //For any slots after the query slot, there will be no 'lesser index but equal' entries, so the loop is simpler. - for (; testedSlots < paddedKeys.Length; testedSlots += 8) + //For indices that come after the current bundle, the relative position is known. + for (int j = endOfEqualityTesting; j < elementCount; ++j) { - var testVector = Vector256.Load(paddedKeys.Memory + testedSlots); - var slotPrecedesValue = Vector256.LessThan(testVector, value); - var slotPrecedesValueMask = Vector256.ExtractMostSignificantBits(slotPrecedesValue.AsSingle()); - var slotsPrecedingValueCount = BitOperations.PopCount(slotPrecedesValueMask); - - previousCount += slotsPrecedingValueCount; + var testVector = Vector256.Create(paddedKeys[j]); + var slotPrecedesValue = Vector256.LessThan(testVector, values); + counts += Vector256.BitwiseAnd(slotPrecedesValue.AsInt32(), oneMask); } - targetIndices[i] = previousCount; + + Vector256.Store(counts, paddedTargetIndices.Memory + i); } } else if (Vector128.IsHardwareAccelerated) { + Debug.Assert(paddedKeys.length == paddedTargetIndices.length && paddedKeys.length % 4 == 0 && paddedKeys.Length >= elementCount, + "This implementation assumes the keys and target indices are pre-padded, and can contain the element count."); + var indexOffsets = Vector128.Create(0, 1, 2, 3); + for (int i = 0; i < elementCount; i += 4) + { + //Grab a bundle of values and test them against every other key. + var values = Vector128.Load(paddedKeys.Memory + i); + var counts = Vector128.Zero; + var oneMask = Vector128.Create(1); + //The first loop tests all vectors up to the start of the current bundle. + //These bundles do not require testing to see if the index is lesser; it definitely is. + for (int j = 0; j < i; ++j) + { + var testVector = Vector128.Create(paddedKeys[j]); + var slotPrecedesValue = Vector128.LessThanOrEqual(testVector, values); + counts += Vector128.BitwiseAnd(slotPrecedesValue.AsInt32(), oneMask); + } + + //For indices that overlap the current bundle, we need to check their relative position. + var endOfEqualityTesting = Math.Min(i + 4, elementCount); + var slotIndex = Vector128.Create(i) + indexOffsets; + for (int j = i; j < endOfEqualityTesting; ++j) + { + var testVector = Vector128.Create(paddedKeys[j]); + var slotIsLesser = Vector128.LessThan(testVector, values); + var slotIndexIsLesser = Vector128.LessThan(Vector128.Create(j), slotIndex); + var slotIsEqual = Vector128.Equals(testVector, values); + var slotPrecedesValue = Vector128.BitwiseOr(slotIsLesser, Vector128.BitwiseAnd(slotIsEqual, slotIndexIsLesser.AsSingle())); + counts += Vector128.BitwiseAnd(slotPrecedesValue.AsInt32(), oneMask); + } + + //For indices that come after the current bundle, the relative position is known. + for (int j = endOfEqualityTesting; j < elementCount; ++j) + { + var testVector = Vector128.Create(paddedKeys[j]); + var slotPrecedesValue = Vector128.LessThan(testVector, values); + counts += Vector128.BitwiseAnd(slotPrecedesValue.AsInt32(), oneMask); + } + + Vector128.Store(counts, paddedTargetIndices.Memory + i); + } } else { - //Shrug! Nonvector fallback. Really shouldn't call a function called VectorCountingSort if you don't have any vectorization. - for (int i = 0; i < targetIndices.Length; ++i) - { - targetIndices[i] = i; - } - var comparer = new PrimitiveComparer(); - QuickSort.Sort(ref paddedKeys[0], ref targetIndices[0], 0, targetIndices.Length - 1, ref comparer); + throw new NotSupportedException("This sort assumes that Vector256.IsHardwareAccelerated or Vector128.IsHardwareAccelerated. It is the caller's responsibility to guarantee this."); } } } +} From 1d8bfc59ec2c11717ce5e9b3036e18006f27b0f7 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 22 Sep 2022 16:37:20 -0500 Subject: [PATCH 619/947] Some tuning. Exposed tuning variables. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 38 ++++++++++++------- BepuUtilities/Collections/SpeculativeSorts.cs | 4 +- 2 files changed, 27 insertions(+), 15 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 8062fa89a..9f97f77ff 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -20,7 +20,6 @@ namespace BepuPhysics.Trees { partial struct Tree { - const int MaximumBinCountRevamp = 64; [MethodImpl(MethodImplOptions.AggressiveInlining)] static unsafe Int4 Truncate(Vector4 v) { @@ -78,6 +77,11 @@ struct Bins public Buffer BinBoundingBoxesScan; public Buffer BinLeafCounts; + public int MinimumBinCount; + public int MaximumBinCount; + public float LeafToBinMultiplier; + public int MicrosweepThreshold; + } struct BoundsComparerX : IComparerRef @@ -143,7 +147,7 @@ static unsafe void MicroSweepForBinnedBuilder(Vector4 centroidMin, Vector4 centr var paddedKeyCount = ((leafCount + 7) / 8) * 8; Debug.Assert(Unsafe.SizeOf() * bins.BinBoundingBoxes.Length >= (paddedKeyCount * 2 + leafCount) * Unsafe.SizeOf()); var keys = new Buffer(bins.BinBoundingBoxes.Memory, paddedKeyCount); - var targetIndices = new Buffer(keys.Memory + paddedKeyCount, leafCount); + var targetIndices = new Buffer(keys.Memory + paddedKeyCount, paddedKeyCount); var indicesCache = new Buffer(targetIndices.Memory + paddedKeyCount, leafCount); var boundingBoxCache = bins.BinBoundingBoxesScan; @@ -180,8 +184,8 @@ static unsafe void MicroSweepForBinnedBuilder(Vector4 centroidMin, Vector4 centr { keys[i] = float.MaxValue; } - VectorizedSorts.VectorCountingSort(keys, targetIndices); - //VectorizedSorts.VectorCountingSortTranspose(keys, targetIndices, leafCount); + //VectorizedSorts.VectorCountingSort(keys, targetIndices); + VectorizedSorts.VectorCountingSortTranspose(keys, targetIndices, leafCount); //Now that we know the target indices, copy things back. for (int i = 0; i < leafCount; ++i) @@ -271,7 +275,7 @@ static unsafe void MicroSweepForBinnedBuilder(Vector4 centroidMin, Vector4 centr // QuickSort.Sort(ref boundingBoxes[0], ref indices[0], 0, leafCount - 1, ref comparer); //} - Debug.Assert(leafCount <= MaximumBinCountRevamp, "We're reusing the bin resources under the assumption that this is only ever called when there are less leaves than maximum bins."); + Debug.Assert(leafCount <= bins.MaximumBinCount || leafCount < bins.MicrosweepThreshold, "We're reusing the bin resources under the assumption that this is only ever called when there are less leaves than maximum bins."); //Identify the split index by examining the SAH of very split option. //Premerge from left to right so we have a sorta-summed area table to cheaply look up all possible child A bounds as we scan. bins.BinBoundingBoxesScan[0] = boundingBoxes[0]; @@ -451,7 +455,7 @@ static unsafe void BinnedBuilderInternal(Buffer indices, Buffer indices, Buffer= binCount); var offsetToBinIndex = new Vector4(binCount) / centroidSpan; @@ -586,7 +590,8 @@ static unsafe void BinnedBuilderInternal(Buffer indices, Buffer indices, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, BufferPool pool) + public static unsafe void BinnedBuilder(Buffer indices, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, BufferPool pool, + int minimumBinCount = 16, int maximumBinCount = 64, float leafToBinMultiplier = 1 / 16f, int microsweepThreshold = 64) { var leafCount = indices.Length; Debug.Assert(boundingBoxes.Length >= leafCount, "The bounding boxes provided must cover the range of indices provided."); @@ -607,13 +612,20 @@ public static unsafe void BinnedBuilder(Buffer indices, Buffer boundingBoxes = boundingBoxes.Slice(indices.Length); nodes = nodes.Slice(leafCount - 1); - var binBoundsMemory = stackalloc BoundingBox4[MaximumBinCountRevamp * 2]; + //The microsweep uses the same resources as the bin allocations, so expand to hold whichever is larger. + var allocatedBinCount = Math.Max(maximumBinCount, microsweepThreshold); + var binBoundsMemory = stackalloc BoundingBox4[allocatedBinCount * 2]; Bins bins; - bins.BinBoundingBoxes = new Buffer(binBoundsMemory, MaximumBinCountRevamp); - bins.BinBoundingBoxesScan = new Buffer(binBoundsMemory + MaximumBinCountRevamp, MaximumBinCountRevamp); + bins.BinBoundingBoxes = new Buffer(binBoundsMemory, allocatedBinCount); + bins.BinBoundingBoxesScan = new Buffer(binBoundsMemory + allocatedBinCount, allocatedBinCount); + + var binLeafCountsMemory = stackalloc int[allocatedBinCount]; + bins.BinLeafCounts = new Buffer(binLeafCountsMemory, allocatedBinCount); - var binLeafCountsMemory = stackalloc int[MaximumBinCountRevamp]; - bins.BinLeafCounts = new Buffer(binLeafCountsMemory, MaximumBinCountRevamp); + bins.MinimumBinCount = minimumBinCount; + bins.MaximumBinCount = maximumBinCount; + bins.LeafToBinMultiplier = leafToBinMultiplier; + bins.MicrosweepThreshold = microsweepThreshold; //While we could avoid a recursive implementation, the overhead is low compared to the per-iteration cost. BinnedBuilderInternal(indices, boundingBoxes.As(), nodes, metanodes, 0, -1, -1, bins); diff --git a/BepuUtilities/Collections/SpeculativeSorts.cs b/BepuUtilities/Collections/SpeculativeSorts.cs index b57528b3a..e3cccaad0 100644 --- a/BepuUtilities/Collections/SpeculativeSorts.cs +++ b/BepuUtilities/Collections/SpeculativeSorts.cs @@ -17,7 +17,7 @@ namespace BepuUtilities.Collections public static class VectorizedSorts { /// - /// Computes the mapping from current index to sorted index for all elements in the paddedKeys. The paddedKeys buffer is not actually sorted. + /// Computes the mapping from current index to ascending sorted index for all elements in the paddedKeys. The paddedKeys buffer is not actually sorted. /// This function requires that or . /// /// Padded keys to compute sorted indices for. This buffer's contents will not be mutated. @@ -140,7 +140,7 @@ public unsafe static void VectorCountingSort(Buffer paddedKeys, Buffer - /// Computes the mapping from current index to sorted index for all elements in the paddedKeys. The paddedKeys buffer is not actually sorted. + /// Computes the mapping from current index to ascending sorted index for all elements in the paddedKeys. The paddedKeys buffer is not actually sorted. /// This function requires that or . /// /// Padded keys to compute sorted indices for. This buffer's contents will not be mutated. From 48ef43173f000be63c1442b34fc36e070eeaaa1b Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 22 Sep 2022 17:00:03 -0500 Subject: [PATCH 620/947] Cleanup and option collapse. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 312 +++++------------- BepuUtilities/Collections/SpeculativeSorts.cs | 251 -------------- BepuUtilities/Collections/VectorizedSorts.cs | 123 +++++++ .../SpecializedTests/TreeFiddlingTestDemo.cs | 26 +- 4 files changed, 222 insertions(+), 490 deletions(-) delete mode 100644 BepuUtilities/Collections/SpeculativeSorts.cs create mode 100644 BepuUtilities/Collections/VectorizedSorts.cs diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 9f97f77ff..94376e22d 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -20,24 +20,6 @@ namespace BepuPhysics.Trees { partial struct Tree { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - static unsafe Int4 Truncate(Vector4 v) - { - Int4 discrete; - if (Vector128.IsHardwareAccelerated) - { - Vector128.Store(Vector128.ConvertToInt32(v.AsVector128()), (int*)&discrete); - } - else - { - discrete.X = (int)v.X; - discrete.Y = (int)v.Y; - discrete.Z = (int)v.Z; - discrete.W = (int)v.W; - } - return discrete; - - } [MethodImpl(MethodImplOptions.AggressiveInlining)] static void BuildNode(BoundingBox4 a, BoundingBox4 b, Buffer nodes, Buffer metanodes, Buffer indices, int nodeIndex, int parentNodeIndex, int childIndexInParent, int aCount, int bCount, out int aIndex, out int bIndex) @@ -97,40 +79,6 @@ struct BoundsComparerZ : IComparerRef public int Compare(ref BoundingBox4 a, ref BoundingBox4 b) => (a.Min.Z + a.Max.Z) > (b.Min.Z + b.Max.Z) ? -1 : 1; } - - //static void ValidateNode(int nodeIndex, Buffer nodes, out Vector3 min, out Vector3 max) - //{ - // if (nodes[nodeIndex].A.Index >= 0) - // { - // ValidateNode(nodes[nodeIndex].A.Index, nodes, out var aMin, out var aMax); - // Debug.Assert(nodes[nodeIndex].A.Min == aMin); - // Debug.Assert(nodes[nodeIndex].A.Max == aMax); - // } - // if (nodes[nodeIndex].B.Index >= 0) - // { - // ValidateNode(nodes[nodeIndex].B.Index, nodes, out var bMin, out var bMax); - // Debug.Assert(nodes[nodeIndex].B.Min == bMin); - // Debug.Assert(nodes[nodeIndex].B.Max == bMax); - // } - // min = Vector3.Min(nodes[nodeIndex].A.Min, nodes[nodeIndex].A.Min); - // max = Vector3.Max(nodes[nodeIndex].B.Max, nodes[nodeIndex].B.Max); - //} - - [StructLayout(LayoutKind.Sequential)] - struct BoundsSortable - { - public float Key; - public int Index; - } - - struct AxisComparer : IComparerRef - { - public int Compare(ref BoundsSortable a, ref BoundsSortable b) - { - return a.Key > b.Key ? -1 : 1; - } - } - static unsafe void MicroSweepForBinnedBuilder(Vector4 centroidMin, Vector4 centroidMax, Buffer indices, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, Bins bins) { //This is a very small scale sweep build. @@ -142,138 +90,79 @@ static unsafe void MicroSweepForBinnedBuilder(Vector4 centroidMin, Vector4 centr } var centroidSpan = centroidMax - centroidMin; - //Repurpose the bins memory so we don't need to allocate any extra. The bins aren't in use right now anyway. - //TODO: this assumes Vector256 widths! - var paddedKeyCount = ((leafCount + 7) / 8) * 8; - Debug.Assert(Unsafe.SizeOf() * bins.BinBoundingBoxes.Length >= (paddedKeyCount * 2 + leafCount) * Unsafe.SizeOf()); - var keys = new Buffer(bins.BinBoundingBoxes.Memory, paddedKeyCount); - var targetIndices = new Buffer(keys.Memory + paddedKeyCount, paddedKeyCount); - var indicesCache = new Buffer(targetIndices.Memory + paddedKeyCount, leafCount); - var boundingBoxCache = bins.BinBoundingBoxesScan; - - //Store the bounds/indices into temporary memory so that we can shuffle them trivially once we're done with the sort. - //Doing this as a prepass means we don't have to worry about doing heavy swaps in the sort. - boundingBoxes.CopyTo(0, boundingBoxCache, 0, leafCount); - indices.CopyTo(0, indicesCache, 0, leafCount); - //Compute the axis centroids up front to avoid having to recompute them during a sort. - if (centroidSpan.X > centroidSpan.Y && centroidSpan.X > centroidSpan.Z) + if (Vector256.IsHardwareAccelerated || Vector128.IsHardwareAccelerated) { - for (int i = 0; i < leafCount; ++i) + //Repurpose the bins memory so we don't need to allocate any extra. The bins aren't in use right now anyway. + int paddedKeyCount = Vector256.IsHardwareAccelerated ? ((leafCount + 7) / 8) * 8 : ((leafCount + 3) / 4) * 4; + + Debug.Assert(Unsafe.SizeOf() * bins.BinBoundingBoxes.Length >= (paddedKeyCount * 2 + leafCount) * Unsafe.SizeOf()); + var keys = new Buffer(bins.BinBoundingBoxes.Memory, paddedKeyCount); + var targetIndices = new Buffer(keys.Memory + paddedKeyCount, paddedKeyCount); + var indicesCache = new Buffer(targetIndices.Memory + paddedKeyCount, leafCount); + var boundingBoxCache = bins.BinBoundingBoxesScan; + + //Store the bounds/indices into temporary memory so that we can shuffle them trivially once we're done with the sort. + //Doing this as a prepass means we don't have to worry about doing heavy swaps in the sort. + boundingBoxes.CopyTo(0, boundingBoxCache, 0, leafCount); + indices.CopyTo(0, indicesCache, 0, leafCount); + //Compute the axis centroids up front to avoid having to recompute them during a sort. + if (centroidSpan.X > centroidSpan.Y && centroidSpan.X > centroidSpan.Z) { - ref var bounds = ref boundingBoxes[i]; - keys[i] = bounds.Min.X + bounds.Max.X; + for (int i = 0; i < leafCount; ++i) + { + ref var bounds = ref boundingBoxes[i]; + keys[i] = bounds.Min.X + bounds.Max.X; + } } - } - else if (centroidSpan.Y > centroidSpan.Z) - { + else if (centroidSpan.Y > centroidSpan.Z) + { + for (int i = 0; i < leafCount; ++i) + { + ref var bounds = ref boundingBoxes[i]; + keys[i] = bounds.Min.Y + bounds.Max.Y; + } + } + else + { + for (int i = 0; i < leafCount; ++i) + { + ref var bounds = ref boundingBoxes[i]; + keys[i] = bounds.Min.Z + bounds.Max.Z; + } + } + for (int i = leafCount; i < paddedKeyCount; ++i) + { + keys[i] = float.MaxValue; + } + VectorizedSorts.VectorCountingSort(keys, targetIndices, leafCount); + + //Now that we know the target indices, copy things back. for (int i = 0; i < leafCount; ++i) { - ref var bounds = ref boundingBoxes[i]; - keys[i] = bounds.Min.Y + bounds.Max.Y; + var targetIndex = targetIndices[i]; + boundingBoxes[targetIndex] = boundingBoxCache[i]; + indices[targetIndex] = indicesCache[i]; } } else { - for (int i = 0; i < leafCount; ++i) + //No vectorization supported. Fall back to poopymode! + if (centroidSpan.X > centroidSpan.Y && centroidSpan.X > centroidSpan.Z) { - ref var bounds = ref boundingBoxes[i]; - keys[i] = bounds.Min.Z + bounds.Max.Z; + var comparer = new BoundsComparerX(); + QuickSort.Sort(ref boundingBoxes[0], ref indices[0], 0, leafCount - 1, ref comparer); + } + else if (centroidSpan.Y > centroidSpan.Z) + { + var comparer = new BoundsComparerY(); + QuickSort.Sort(ref boundingBoxes[0], ref indices[0], 0, leafCount - 1, ref comparer); + } + else + { + var comparer = new BoundsComparerZ(); + QuickSort.Sort(ref boundingBoxes[0], ref indices[0], 0, leafCount - 1, ref comparer); } } - for (int i = leafCount; i < paddedKeyCount; ++i) - { - keys[i] = float.MaxValue; - } - //VectorizedSorts.VectorCountingSort(keys, targetIndices); - VectorizedSorts.VectorCountingSortTranspose(keys, targetIndices, leafCount); - - //Now that we know the target indices, copy things back. - for (int i = 0; i < leafCount; ++i) - { - var targetIndex = targetIndices[i]; - boundingBoxes[targetIndex] = boundingBoxCache[i]; - indices[targetIndex] = indicesCache[i]; - } - //var previousValue = float.MinValue; - //for (int i = 0; i < leafCount; ++i) - //{ - // var centroid = boundingBoxes[i].Min + boundingBoxes[i].Max; - // var axis = centroidSpan.X > centroidSpan.Y && centroidSpan.X > centroidSpan.Z ? centroid.X : centroidSpan.Y > centroidSpan.Z ? centroid.Y : centroid.Z; - // if (axis < previousValue) - // Console.WriteLine("BAD"); - // Console.WriteLine($"centroid {i}: {axis}"); - //} - - //----------------- - - ////Repurpose the bins memory so we don't need to allocate any extra. The bins aren't in use right now anyway. - //var sortables = new Buffer(bins.BinBoundingBoxes.Memory, leafCount); - //var boundingBoxCache = bins.BinBoundingBoxesScan; - //var indicesCache = new Buffer((int*)((BoundsSortable*)bins.BinBoundingBoxes.Memory + leafCount), leafCount); - - ////Store the bounds/indices into temporary memory so that we can shuffle them trivially once we're done with the sort. - ////Doing this as a prepass means we don't have to worry about doing heavy swaps in the sort. - //boundingBoxes.CopyTo(0, boundingBoxCache, 0, leafCount); - //indices.CopyTo(0, indicesCache, 0, leafCount); - ////Compute the axis centroids up front to avoid having to recompute them during a sort. - //if (centroidSpan.X > centroidSpan.Y && centroidSpan.X > centroidSpan.Z) - //{ - // for (int i = 0; i < leafCount; ++i) - // { - // ref var bounds = ref boundingBoxes[i]; - // ref var target = ref sortables[i]; - // target.Key = (bounds.Min + bounds.Max).X; - // target.Index = i; - // } - //} - //else if (centroidSpan.Y > centroidSpan.Z) - //{ - // for (int i = 0; i < leafCount; ++i) - // { - // ref var bounds = ref boundingBoxes[i]; - // ref var target = ref sortables[i]; - // target.Key = (bounds.Min + bounds.Max).Y; - // target.Index = i; - // } - //} - //else - //{ - // for (int i = 0; i < leafCount; ++i) - // { - // ref var bounds = ref boundingBoxes[i]; - // ref var target = ref sortables[i]; - // target.Key = (bounds.Min + bounds.Max).Z; - // target.Index = i; - // } - //} - //var comparer2 = new AxisComparer(); - //QuickSort.Sort(ref sortables[0], 0, leafCount - 1, ref comparer2); - - ////Now that we know the target indices, copy things back. - //for (int i = 0; i < leafCount; ++i) - //{ - // var sourceIndex = sortables[i].Index; - // boundingBoxes[i] = boundingBoxCache[sourceIndex]; - // indices[i] = indicesCache[sourceIndex]; - //} - - //----------------- - - //if (centroidSpan.X > centroidSpan.Y && centroidSpan.X > centroidSpan.Z) - //{ - // var comparer = new BoundsComparerX(); - // QuickSort.Sort(ref boundingBoxes[0], ref indices[0], 0, leafCount - 1, ref comparer); - //} - //else if (centroidSpan.Y > centroidSpan.Z) - //{ - // var comparer = new BoundsComparerY(); - // QuickSort.Sort(ref boundingBoxes[0], ref indices[0], 0, leafCount - 1, ref comparer); - //} - //else - //{ - // var comparer = new BoundsComparerZ(); - // QuickSort.Sort(ref boundingBoxes[0], ref indices[0], 0, leafCount - 1, ref comparer); - //} Debug.Assert(leafCount <= bins.MaximumBinCount || leafCount < bins.MicrosweepThreshold, "We're reusing the bin resources under the assumption that this is only ever called when there are less leaves than maximum bins."); //Identify the split index by examining the SAH of very split option. @@ -316,65 +205,33 @@ static unsafe void MicroSweepForBinnedBuilder(Vector4 centroidMin, Vector4 centr var aCount = bestSplit; var bCount = leafCount - bestSplit; + + BuildNode(bestBoundsA, bestBoundsB, nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, aCount, bCount, out var aIndex, out var bIndex); + if (aCount > 1) { - //if (leafCount == 2) - //{ - // Debug.Assert(bestBoundsA.Min == boundingBoxes[0].Min); - // Debug.Assert(bestBoundsA.Max == boundingBoxes[0].Max); - // Debug.Assert(bestBoundsB.Min == boundingBoxes[1].Min); - // Debug.Assert(bestBoundsB.Max == boundingBoxes[1].Max); - //} - //for (int i = 0; i < leafCount; ++i) - //{ - // var bounds = i < bestSplit ? bestBoundsA : bestBoundsB; - // var containedA = Vector128.LessThanOrEqual(boundingBoxes[i].Max.AsVector128(), bounds.Max.AsVector128()) & Vector128.GreaterThanOrEqual(boundingBoxes[i].Min.AsVector128(), bounds.Min.AsVector128()); - // var mask = containedA.ExtractMostSignificantBits() & 0b111; - // Debug.Assert(mask == 0b111, "All children must be contained within their parent."); - //} - //BoundingBox4 debugBoundsA = boundingBoxes[0]; - //for (int i = 1; i < aCount; ++i) - //{ - // ref var bounds = ref boundingBoxes[i]; - // debugBoundsA.Min = Vector4.Min(debugBoundsA.Min, bounds.Min); - // debugBoundsA.Max = Vector4.Max(debugBoundsA.Max, bounds.Max); - //} - //BoundingBox4 debugBoundsB = boundingBoxes[aCount]; - //for (int i = aCount + 1; i < leafCount; ++i) - //{ - // ref var bounds = ref boundingBoxes[i]; - // debugBoundsB.Min = Vector4.Min(debugBoundsB.Min, bounds.Min); - // debugBoundsB.Max = Vector4.Max(debugBoundsB.Max, bounds.Max); - //} - //Debug.Assert(bestBoundsA.Min == debugBoundsA.Min); - //Debug.Assert(bestBoundsA.Max == debugBoundsA.Max); - //Debug.Assert(bestBoundsB.Min == debugBoundsB.Min); - //Debug.Assert(bestBoundsB.Max == debugBoundsB.Max); - BuildNode(bestBoundsA, bestBoundsB, nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, aCount, bCount, out var aIndex, out var bIndex); - if (aCount > 1) + var aBounds = boundingBoxes.Slice(aCount); + BoundingBox4 centroidBoundsA = aBounds[0]; + for (int i = 1; i < aCount; ++i) { - var aBounds = boundingBoxes.Slice(aCount); - BoundingBox4 centroidBoundsA = aBounds[0]; - for (int i = 1; i < aCount; ++i) - { - ref var bounds = ref aBounds[i]; - centroidBoundsA.Min = Vector4.Min(centroidBoundsA.Min, bounds.Min); - centroidBoundsA.Max = Vector4.Max(centroidBoundsA.Max, bounds.Max); - } - MicroSweepForBinnedBuilder(centroidBoundsA.Min, centroidBoundsA.Max, indices.Slice(aCount), aBounds, nodes.Slice(1, aCount - 1), metanodes.Slice(1, aCount - 1), aIndex, nodeIndex, 0, bins); + ref var bounds = ref aBounds[i]; + centroidBoundsA.Min = Vector4.Min(centroidBoundsA.Min, bounds.Min); + centroidBoundsA.Max = Vector4.Max(centroidBoundsA.Max, bounds.Max); } - if (bCount > 1) + MicroSweepForBinnedBuilder(centroidBoundsA.Min, centroidBoundsA.Max, indices.Slice(aCount), aBounds, nodes.Slice(1, aCount - 1), metanodes.Slice(1, aCount - 1), aIndex, nodeIndex, 0, bins); + } + if (bCount > 1) + { + var bBounds = boundingBoxes.Slice(aCount, bCount); + BoundingBox4 centroidBoundsB = bBounds[0]; + for (int i = 0; i < bCount; ++i) { - var bBounds = boundingBoxes.Slice(aCount, bCount); - BoundingBox4 centroidBoundsB = bBounds[0]; - for (int i = 0; i < bCount; ++i) - { - ref var bounds = ref bBounds[i]; - centroidBoundsB.Min = Vector4.Min(centroidBoundsB.Min, bounds.Min); - centroidBoundsB.Max = Vector4.Max(centroidBoundsB.Max, bounds.Max); - } - MicroSweepForBinnedBuilder(centroidBoundsB.Min, centroidBoundsB.Max, indices.Slice(aCount, bCount), bBounds, nodes.Slice(aCount, bCount - 1), metanodes.Slice(aCount, bCount - 1), bIndex, nodeIndex, 1, bins); + ref var bounds = ref bBounds[i]; + centroidBoundsB.Min = Vector4.Min(centroidBoundsB.Min, bounds.Min); + centroidBoundsB.Max = Vector4.Max(centroidBoundsB.Max, bounds.Max); } + MicroSweepForBinnedBuilder(centroidBoundsB.Min, centroidBoundsB.Max, indices.Slice(aCount, bCount), bBounds, nodes.Slice(aCount, bCount - 1), metanodes.Slice(aCount, bCount - 1), bIndex, nodeIndex, 1, bins); } + } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -614,7 +471,10 @@ public static unsafe void BinnedBuilder(Buffer indices, Buffer //The microsweep uses the same resources as the bin allocations, so expand to hold whichever is larger. var allocatedBinCount = Math.Max(maximumBinCount, microsweepThreshold); - var binBoundsMemory = stackalloc BoundingBox4[allocatedBinCount * 2]; + var binBoundsMemory = stackalloc BoundingBox4[allocatedBinCount * 2 + 1]; + //Should be basically irrelevant, but just in case it's not on some platform, align the allocation. + binBoundsMemory = (BoundingBox4*)(((ulong)binBoundsMemory + 31ul) & (~31ul)); + Bins bins; bins.BinBoundingBoxes = new Buffer(binBoundsMemory, allocatedBinCount); bins.BinBoundingBoxesScan = new Buffer(binBoundsMemory + allocatedBinCount, allocatedBinCount); diff --git a/BepuUtilities/Collections/SpeculativeSorts.cs b/BepuUtilities/Collections/SpeculativeSorts.cs deleted file mode 100644 index e3cccaad0..000000000 --- a/BepuUtilities/Collections/SpeculativeSorts.cs +++ /dev/null @@ -1,251 +0,0 @@ -using BepuUtilities.Memory; -using System; -using System.Collections.Generic; -using System.Diagnostics; -using System.Linq; -using System.Numerics; -using System.Runtime.Intrinsics; -using System.Runtime.Intrinsics.X86; -using System.Text; -using System.Threading.Tasks; - -namespace BepuUtilities.Collections -{ - /// - /// Specialized sorts that can go fast sometimes, but often come with special requirements. - /// - public static class VectorizedSorts - { - /// - /// Computes the mapping from current index to ascending sorted index for all elements in the paddedKeys. The paddedKeys buffer is not actually sorted. - /// This function requires that or . - /// - /// Padded keys to compute sorted indices for. This buffer's contents will not be mutated. - /// The buffer must be at least as long as the targetIndices buffer and must be padded to be divisible by vector length. - /// Vector length is 8 elements if or 4 elements if only . - /// Buffer to be filled with the target indices each slot should be moved to in order to sort the buffer. - /// This function requires that or . - public unsafe static void VectorCountingSort(Buffer paddedKeys, Buffer targetIndices) - { - if (Vector256.IsHardwareAccelerated) - { - Debug.Assert(paddedKeys.length >= targetIndices.length && paddedKeys.length % 8 == 0, "This implementation assumes the keys are pre-padded."); - var elementCount = targetIndices.length; - var indexOffsets = Vector256.Create(0, 1, 2, 3, 4, 5, 6, 7); - for (int i = 0; i < elementCount; ++i) - { - //Broadcast the current value and test it against all other values. - //Count all values smaller than the current value, and all equal values that are in a lower index. - var previousCount = 0; - var value = Vector256.Create(paddedKeys[i]); - var currentIndex = Vector256.Create(i); - - //Bundles that occur before the current value index do not need to have their relative position tested. - var currentBundleStartIndex = (i / 8) * 8; - for (int j = 0; j < currentBundleStartIndex; j += 8) - { - var testVector = Vector256.Load(paddedKeys.Memory + j); - var slotPrecedesValue = Vector256.LessThanOrEqual(testVector, value); - var slotPrecedesValueMask = Vector256.ExtractMostSignificantBits(slotPrecedesValue.AsSingle()); - var slotsPrecedingValueCount = BitOperations.PopCount(slotPrecedesValueMask); - - previousCount += slotsPrecedingValueCount; - } - - //Bundles which overlap the current value index require relative position testing. - { - var testVector = Vector256.Load(paddedKeys.Memory + currentBundleStartIndex); - var vectorIndices = indexOffsets + Vector256.Create(currentBundleStartIndex); - var indexIsLesser = Vector256.LessThan(vectorIndices, currentIndex); - var slotIsEqual = Vector256.Equals(testVector, value); - var slotIsLesser = Vector256.LessThan(testVector, value); - var slotPrecedesValue = Vector256.BitwiseOr(slotIsLesser, Vector256.BitwiseAnd(slotIsEqual, indexIsLesser.AsSingle())); - var slotPrecedesValueMask = Vector256.ExtractMostSignificantBits(slotPrecedesValue.AsSingle()); - var slotsPrecedingValueCount = BitOperations.PopCount(slotPrecedesValueMask); - - previousCount += slotsPrecedingValueCount; - } - - //For any slots after the query slot, there will be no 'lesser index but equal' entries, so the loop is simpler again. - for (int j = currentBundleStartIndex + 8; j < paddedKeys.Length; j += 8) - { - var testVector = Vector256.Load(paddedKeys.Memory + j); - var slotPrecedesValue = Vector256.LessThan(testVector, value); - var slotPrecedesValueMask = Vector256.ExtractMostSignificantBits(slotPrecedesValue.AsSingle()); - var slotsPrecedingValueCount = BitOperations.PopCount(slotPrecedesValueMask); - - previousCount += slotsPrecedingValueCount; - } - targetIndices[i] = previousCount; - } - } - else if (Vector128.IsHardwareAccelerated) - { - Debug.Assert(paddedKeys.length >= targetIndices.length && paddedKeys.length % 4 == 0, "This implementation assumes the keys are pre-padded."); - var elementCount = targetIndices.length; - var indexOffsets = Vector128.Create(0, 1, 2, 3); - for (int i = 0; i < elementCount; ++i) - { - //Broadcast the current value and test it against all other values. - //Count all values smaller than the current value, and all equal values that are in a lower index. - var previousCount = 0; - var value = Vector128.Create(paddedKeys[i]); - var currentIndex = Vector128.Create(i); - - //Bundles that occur before the current value index do not need to have their relative position tested. - var currentBundleStartIndex = (i / 4) * 4; - for (int j = 0; j < currentBundleStartIndex; j += 4) - { - var testVector = Vector128.Load(paddedKeys.Memory + j); - var slotPrecedesValue = Vector128.LessThanOrEqual(testVector, value); - var slotPrecedesValueMask = Vector128.ExtractMostSignificantBits(slotPrecedesValue.AsSingle()); - var slotsPrecedingValueCount = BitOperations.PopCount(slotPrecedesValueMask); - - previousCount += slotsPrecedingValueCount; - } - - //Bundles which overlap the current value index require relative position testing. - { - var testVector = Vector128.Load(paddedKeys.Memory + currentBundleStartIndex); - var vectorIndices = indexOffsets + Vector128.Create(currentBundleStartIndex); - var indexIsLesser = Vector128.LessThan(vectorIndices, currentIndex); - var slotIsEqual = Vector128.Equals(testVector, value); - var slotIsLesser = Vector128.LessThan(testVector, value); - var slotPrecedesValue = Vector128.BitwiseOr(slotIsLesser, Vector128.BitwiseAnd(slotIsEqual, indexIsLesser.AsSingle())); - var slotPrecedesValueMask = Vector128.ExtractMostSignificantBits(slotPrecedesValue.AsSingle()); - var slotsPrecedingValueCount = BitOperations.PopCount(slotPrecedesValueMask); - - previousCount += slotsPrecedingValueCount; - } - - //For any slots after the query slot, there will be no 'lesser index but equal' entries, so the loop is simpler again. - for (int j = currentBundleStartIndex + 4; j < paddedKeys.Length; j += 4) - { - var testVector = Vector128.Load(paddedKeys.Memory + j); - var slotPrecedesValue = Vector128.LessThan(testVector, value); - var slotPrecedesValueMask = Vector128.ExtractMostSignificantBits(slotPrecedesValue.AsSingle()); - var slotsPrecedingValueCount = BitOperations.PopCount(slotPrecedesValueMask); - - previousCount += slotsPrecedingValueCount; - } - targetIndices[i] = previousCount; - } - } - else - { - throw new NotSupportedException("This sort assumes that Vector256.IsHardwareAccelerated or Vector128.IsHardwareAccelerated. It is the caller's responsibility to guarantee this."); - } - - } - - - /// - /// Computes the mapping from current index to ascending sorted index for all elements in the paddedKeys. The paddedKeys buffer is not actually sorted. - /// This function requires that or . - /// - /// Padded keys to compute sorted indices for. This buffer's contents will not be mutated. - /// The buffer must be at least as long as the targetIndices buffer and must be padded to be divisible by vector length. - /// Vector length is 8 elements if or 4 elements if only . - /// Buffer to be filled with the target indices each slot should be moved to in order to sort the buffer. - /// The buffer must be padded to be divisible by vector length. - /// Vector length is 8 elements if or 4 elements if only . - /// Number of elements - public unsafe static void VectorCountingSortTranspose(Buffer paddedKeys, Buffer paddedTargetIndices, int elementCount) - { - if (Vector256.IsHardwareAccelerated) - { - Debug.Assert(paddedKeys.length == paddedTargetIndices.length && paddedKeys.length % 8 == 0 && paddedKeys.Length >= elementCount, - "This implementation assumes the keys and target indices are pre-padded, and can contain the element count."); - var indexOffsets = Vector256.Create(0, 1, 2, 3, 4, 5, 6, 7); - for (int i = 0; i < elementCount; i += 8) - { - //Grab a bundle of values and test them against every other key. - var values = Vector256.Load(paddedKeys.Memory + i); - var counts = Vector256.Zero; - - var oneMask = Vector256.Create(1); - //The first loop tests all vectors up to the start of the current bundle. - //These bundles do not require testing to see if the index is lesser; it definitely is. - for (int j = 0; j < i; ++j) - { - var testVector = Vector256.Create(paddedKeys[j]); - var slotPrecedesValue = Vector256.LessThanOrEqual(testVector, values); - counts += Vector256.BitwiseAnd(slotPrecedesValue.AsInt32(), oneMask); - } - - //For indices that overlap the current bundle, we need to check their relative position. - var endOfEqualityTesting = Math.Min(i + 8, elementCount); - var slotIndex = Vector256.Create(i) + indexOffsets; - for (int j = i; j < endOfEqualityTesting; ++j) - { - var testVector = Vector256.Create(paddedKeys[j]); - var slotIsLesser = Vector256.LessThan(testVector, values); - var slotIndexIsLesser = Vector256.LessThan(Vector256.Create(j), slotIndex); - var slotIsEqual = Vector256.Equals(testVector, values); - var slotPrecedesValue = Vector256.BitwiseOr(slotIsLesser, Vector256.BitwiseAnd(slotIsEqual, slotIndexIsLesser.AsSingle())); - counts += Vector256.BitwiseAnd(slotPrecedesValue.AsInt32(), oneMask); - } - - //For indices that come after the current bundle, the relative position is known. - for (int j = endOfEqualityTesting; j < elementCount; ++j) - { - var testVector = Vector256.Create(paddedKeys[j]); - var slotPrecedesValue = Vector256.LessThan(testVector, values); - counts += Vector256.BitwiseAnd(slotPrecedesValue.AsInt32(), oneMask); - } - - Vector256.Store(counts, paddedTargetIndices.Memory + i); - } - } - else if (Vector128.IsHardwareAccelerated) - { - Debug.Assert(paddedKeys.length == paddedTargetIndices.length && paddedKeys.length % 4 == 0 && paddedKeys.Length >= elementCount, - "This implementation assumes the keys and target indices are pre-padded, and can contain the element count."); - var indexOffsets = Vector128.Create(0, 1, 2, 3); - for (int i = 0; i < elementCount; i += 4) - { - //Grab a bundle of values and test them against every other key. - var values = Vector128.Load(paddedKeys.Memory + i); - var counts = Vector128.Zero; - - var oneMask = Vector128.Create(1); - //The first loop tests all vectors up to the start of the current bundle. - //These bundles do not require testing to see if the index is lesser; it definitely is. - for (int j = 0; j < i; ++j) - { - var testVector = Vector128.Create(paddedKeys[j]); - var slotPrecedesValue = Vector128.LessThanOrEqual(testVector, values); - counts += Vector128.BitwiseAnd(slotPrecedesValue.AsInt32(), oneMask); - } - - //For indices that overlap the current bundle, we need to check their relative position. - var endOfEqualityTesting = Math.Min(i + 4, elementCount); - var slotIndex = Vector128.Create(i) + indexOffsets; - for (int j = i; j < endOfEqualityTesting; ++j) - { - var testVector = Vector128.Create(paddedKeys[j]); - var slotIsLesser = Vector128.LessThan(testVector, values); - var slotIndexIsLesser = Vector128.LessThan(Vector128.Create(j), slotIndex); - var slotIsEqual = Vector128.Equals(testVector, values); - var slotPrecedesValue = Vector128.BitwiseOr(slotIsLesser, Vector128.BitwiseAnd(slotIsEqual, slotIndexIsLesser.AsSingle())); - counts += Vector128.BitwiseAnd(slotPrecedesValue.AsInt32(), oneMask); - } - - //For indices that come after the current bundle, the relative position is known. - for (int j = endOfEqualityTesting; j < elementCount; ++j) - { - var testVector = Vector128.Create(paddedKeys[j]); - var slotPrecedesValue = Vector128.LessThan(testVector, values); - counts += Vector128.BitwiseAnd(slotPrecedesValue.AsInt32(), oneMask); - } - - Vector128.Store(counts, paddedTargetIndices.Memory + i); - } - } - else - { - throw new NotSupportedException("This sort assumes that Vector256.IsHardwareAccelerated or Vector128.IsHardwareAccelerated. It is the caller's responsibility to guarantee this."); - } - } - } -} diff --git a/BepuUtilities/Collections/VectorizedSorts.cs b/BepuUtilities/Collections/VectorizedSorts.cs new file mode 100644 index 000000000..e38c79139 --- /dev/null +++ b/BepuUtilities/Collections/VectorizedSorts.cs @@ -0,0 +1,123 @@ +using BepuUtilities.Memory; +using System; +using System.Diagnostics; +using System.Numerics; +using System.Runtime.Intrinsics; + +namespace BepuUtilities.Collections +{ + /// + /// Specialized sorts that can go fast sometimes, but often come with special requirements. + /// + public static class VectorizedSorts + { + /// + /// Computes the mapping from current index to ascending sorted index for all elements in the paddedKeys. The paddedKeys buffer is not actually sorted. + /// This function requires that or . + /// + /// Padded keys to compute sorted indices for. This buffer's contents will not be mutated. + /// The buffer must be at least as long as the targetIndices buffer and must be padded to be divisible by vector length. + /// Vector length is 8 elements if or 4 elements if only . + /// Buffer to be filled with the target indices each slot should be moved to in order to sort the buffer. + /// The buffer must be padded to be divisible by vector length. + /// Vector length is 8 elements if or 4 elements if only . + /// Number of actual elements in the paddedKeys buffer. + public unsafe static void VectorCountingSort(Buffer paddedKeys, Buffer paddedTargetIndices, int elementCount) + { + if (Vector256.IsHardwareAccelerated) + { + Debug.Assert(paddedKeys.length == paddedTargetIndices.length && paddedKeys.length % 8 == 0 && paddedKeys.Length >= elementCount, + "This implementation assumes the keys and target indices are pre-padded, and can contain the element count."); + var indexOffsets = Vector256.Create(0, 1, 2, 3, 4, 5, 6, 7); + for (int i = 0; i < elementCount; i += 8) + { + //Grab a bundle of values and test them against every other key. + var values = Vector256.Load(paddedKeys.Memory + i); + var counts = Vector256.Zero; + + var oneMask = Vector256.Create(1); + //The first loop tests all vectors up to the start of the current bundle. + //These bundles do not require testing to see if the index is lesser; it definitely is. + for (int j = 0; j < i; ++j) + { + var testVector = Vector256.Create(paddedKeys[j]); + var slotPrecedesValue = Vector256.LessThanOrEqual(testVector, values); + counts += Vector256.BitwiseAnd(slotPrecedesValue.AsInt32(), oneMask); + } + + //For indices that overlap the current bundle, we need to check their relative position. + var endOfEqualityTesting = Math.Min(i + 8, elementCount); + var slotIndex = Vector256.Create(i) + indexOffsets; + for (int j = i; j < endOfEqualityTesting; ++j) + { + var testVector = Vector256.Create(paddedKeys[j]); + var slotIsLesser = Vector256.LessThan(testVector, values); + var slotIndexIsLesser = Vector256.LessThan(Vector256.Create(j), slotIndex); + var slotIsEqual = Vector256.Equals(testVector, values); + var slotPrecedesValue = Vector256.BitwiseOr(slotIsLesser, Vector256.BitwiseAnd(slotIsEqual, slotIndexIsLesser.AsSingle())); + counts += Vector256.BitwiseAnd(slotPrecedesValue.AsInt32(), oneMask); + } + + //For indices that come after the current bundle, the relative position is known. + for (int j = endOfEqualityTesting; j < elementCount; ++j) + { + var testVector = Vector256.Create(paddedKeys[j]); + var slotPrecedesValue = Vector256.LessThan(testVector, values); + counts += Vector256.BitwiseAnd(slotPrecedesValue.AsInt32(), oneMask); + } + + Vector256.Store(counts, paddedTargetIndices.Memory + i); + } + } + else if (Vector128.IsHardwareAccelerated) + { + Debug.Assert(paddedKeys.length == paddedTargetIndices.length && paddedKeys.length % 4 == 0 && paddedKeys.Length >= elementCount, + "This implementation assumes the keys and target indices are pre-padded, and can contain the element count."); + var indexOffsets = Vector128.Create(0, 1, 2, 3); + for (int i = 0; i < elementCount; i += 4) + { + //Grab a bundle of values and test them against every other key. + var values = Vector128.Load(paddedKeys.Memory + i); + var counts = Vector128.Zero; + + var oneMask = Vector128.Create(1); + //The first loop tests all vectors up to the start of the current bundle. + //These bundles do not require testing to see if the index is lesser; it definitely is. + for (int j = 0; j < i; ++j) + { + var testVector = Vector128.Create(paddedKeys[j]); + var slotPrecedesValue = Vector128.LessThanOrEqual(testVector, values); + counts += Vector128.BitwiseAnd(slotPrecedesValue.AsInt32(), oneMask); + } + + //For indices that overlap the current bundle, we need to check their relative position. + var endOfEqualityTesting = Math.Min(i + 4, elementCount); + var slotIndex = Vector128.Create(i) + indexOffsets; + for (int j = i; j < endOfEqualityTesting; ++j) + { + var testVector = Vector128.Create(paddedKeys[j]); + var slotIsLesser = Vector128.LessThan(testVector, values); + var slotIndexIsLesser = Vector128.LessThan(Vector128.Create(j), slotIndex); + var slotIsEqual = Vector128.Equals(testVector, values); + var slotPrecedesValue = Vector128.BitwiseOr(slotIsLesser, Vector128.BitwiseAnd(slotIsEqual, slotIndexIsLesser.AsSingle())); + counts += Vector128.BitwiseAnd(slotPrecedesValue.AsInt32(), oneMask); + } + + //For indices that come after the current bundle, the relative position is known. + for (int j = endOfEqualityTesting; j < elementCount; ++j) + { + var testVector = Vector128.Create(paddedKeys[j]); + var slotPrecedesValue = Vector128.LessThan(testVector, values); + counts += Vector128.BitwiseAnd(slotPrecedesValue.AsInt32(), oneMask); + } + + Vector128.Store(counts, paddedTargetIndices.Memory + i); + } + } + else + { + throw new NotSupportedException("This sort assumes that Vector256.IsHardwareAccelerated or Vector128.IsHardwareAccelerated. It is the caller's responsibility to guarantee this."); + } + } + } +} diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index 265c830bb..06c45817e 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -157,8 +157,8 @@ public override void Initialize(ContentArchive content, Camera camera) Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(4, 1)); //Create a mesh. - var width = 768; - var height = 768; + var width = 128; + var height = 16; var scale = new Vector3(1, 1, 1); //DemoMeshHelper.CreateDeformedPlane(width, height, (x, y) => new Vector3(x - width * scale.X * 0.5f, 2f * (float)(Math.Sin(x * 0.5f) * Math.Sin(y * 0.5f)), y - height * scale.Y * 0.5f), scale, BufferPool, out var mesh); //DemoMeshHelper.CreateDeformedPlane(width, height, (x, y) => new Vector3(x - width * scale.X * 0.5f, 0, y - height * scale.Y * 0.5f), scale, BufferPool, out var mesh); @@ -200,17 +200,17 @@ public override void Initialize(ContentArchive content, Camera camera) }, "Revamp Single Axis", ref mesh.Tree); - //var mesh2 = new Mesh(triangles, Vector3.One, BufferPool); + var mesh2 = new Mesh(triangles, Vector3.One, BufferPool); - //QuickList subtreeReferences = new(mesh2.Tree.LeafCount, BufferPool); - //QuickList treeletInternalNodes = new(mesh2.Tree.LeafCount, BufferPool); - //Tree.CreateBinnedResources(BufferPool, mesh2.Tree.LeafCount, out var binnedResourcesBuffer, out var binnedResources); - //BinnedTest(() => - //{ - // subtreeReferences.Count = 0; - // treeletInternalNodes.Count = 0; - // mesh2.Tree.BinnedRefine(0, ref subtreeReferences, mesh2.Tree.LeafCount, ref treeletInternalNodes, ref binnedResources, BufferPool); - //}, "Original", ref mesh2.Tree); + QuickList subtreeReferences = new(mesh2.Tree.LeafCount, BufferPool); + QuickList treeletInternalNodes = new(mesh2.Tree.LeafCount, BufferPool); + Tree.CreateBinnedResources(BufferPool, mesh2.Tree.LeafCount, out var binnedResourcesBuffer, out var binnedResources); + BinnedTest(() => + { + subtreeReferences.Count = 0; + treeletInternalNodes.Count = 0; + mesh2.Tree.BinnedRefine(0, ref subtreeReferences, mesh2.Tree.LeafCount, ref treeletInternalNodes, ref binnedResources, BufferPool); + }, "Original", ref mesh2.Tree); //RefitTest(() => mesh.Tree.Refit2(), "refit2", ref mesh.Tree); //RefitTest(() => mesh.Tree.Refit(), "Original", ref mesh.Tree); @@ -267,7 +267,7 @@ static void RefitTest(Action function, string name, ref Tree tree) static void BinnedTest(Action function, string name, ref Tree tree) { long accumulatedTime = 0; - const int testCount = 16; + const int testCount = 256; for (int i = 0; i < testCount; ++i) { var startTime = Stopwatch.GetTimestamp(); From 05ec654391d7ff4c597a4b55070a0e38ef12d9b8 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 22 Sep 2022 20:30:00 -0500 Subject: [PATCH 621/947] In the middle of giving the builder the ability to handle non unit leaf counts properly (for refinement). --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 340 +++++++++++++++++------- 1 file changed, 240 insertions(+), 100 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 94376e22d..221892516 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -22,21 +22,25 @@ partial struct Tree { [MethodImpl(MethodImplOptions.AggressiveInlining)] - static void BuildNode(BoundingBox4 a, BoundingBox4 b, Buffer nodes, Buffer metanodes, Buffer indices, int nodeIndex, int parentNodeIndex, int childIndexInParent, int aCount, int bCount, out int aIndex, out int bIndex) + static void BuildNode( + BoundingBox4 a, BoundingBox4 b, + int leafCountA, int leafCountB, + Buffer nodes, Buffer metanodes, Buffer indices, + int nodeIndex, int parentNodeIndex, int childIndexInParent, int subtreeCountA, int subtreeCountB, out int aIndex, out int bIndex) { ref var metanode = ref metanodes[0]; metanode.Parent = parentNodeIndex; metanode.IndexInParent = childIndexInParent; metanode.RefineFlag = 0; ref var node = ref nodes[0]; - aIndex = aCount == 1 ? indices[0] : nodeIndex + 1; - bIndex = bCount == 1 ? indices[^1] : nodeIndex + aCount;//parentNodeIndex + 1 + (aCount - 1) + aIndex = subtreeCountA == 1 ? indices[0] : nodeIndex + 1; + bIndex = subtreeCountB == 1 ? indices[^1] : nodeIndex + subtreeCountA;//parentNodeIndex + 1 + (subtreeCountA - 1) node.A = Unsafe.As(ref a); node.B = Unsafe.As(ref b); node.A.Index = aIndex; - node.A.LeafCount = aCount; + node.A.LeafCount = leafCountA; node.B.Index = bIndex; - node.B.LeafCount = bCount; + node.B.LeafCount = leafCountB; } internal static float ComputeBoundsMetric(BoundingBox4 bounds) @@ -53,11 +57,46 @@ internal static float ComputeBoundsMetric(Vector4 min, Vector4 max) return offset.X * offset.Y + offset.Y * offset.Z + offset.Z * offset.X; } + + interface ILeafCountBuffer where T : unmanaged, ILeafCountBuffer + { + int this[int index] { get; set; } + + T Slice(int startIndex, int count); + + + } + + /// + /// An implicit buffer where every slot contains a 1. + /// + struct UnitLeafCount : ILeafCountBuffer + { + public int this[int index] { get => 1; set { } } + + public UnitLeafCount Slice(int startIndex, int count) + { + return this; + } + } + + /// + /// Leaf counts buffer with actual values. + /// + struct LeafCountBuffer : ILeafCountBuffer + { + public Buffer LeafCounts; + public int this[int index] { get => LeafCounts[index]; set => LeafCounts[index] = value; } + + public LeafCountBuffer Slice(int startIndex, int count) => new() { LeafCounts = LeafCounts.Slice(startIndex, count) }; + } + struct Bins { public Buffer BinBoundingBoxes; public Buffer BinBoundingBoxesScan; public Buffer BinLeafCounts; + public Buffer BinLeafCountsScan; public int MinimumBinCount; public int MaximumBinCount; @@ -79,13 +118,14 @@ struct BoundsComparerZ : IComparerRef public int Compare(ref BoundingBox4 a, ref BoundingBox4 b) => (a.Min.Z + a.Max.Z) > (b.Min.Z + b.Max.Z) ? -1 : 1; } - static unsafe void MicroSweepForBinnedBuilder(Vector4 centroidMin, Vector4 centroidMax, Buffer indices, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, Bins bins) + static unsafe void MicroSweepForBinnedBuilder(Vector4 centroidMin, Vector4 centroidMax, Buffer indices, TLeafCounts leafCounts, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, Bins bins) + where TLeafCounts : unmanaged, ILeafCountBuffer { //This is a very small scale sweep build. - var leafCount = indices.Length; - if (leafCount == 2) + var subtreeCount = indices.Length; + if (subtreeCount == 2) { - BuildNode(boundingBoxes[0], boundingBoxes[1], nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, 1, 1, out _, out _); + BuildNode(boundingBoxes[0], boundingBoxes[1], leafCounts[0], leafCounts[1], nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, 1, 1, out _, out _); return; } var centroidSpan = centroidMax - centroidMin; @@ -93,22 +133,17 @@ static unsafe void MicroSweepForBinnedBuilder(Vector4 centroidMin, Vector4 centr if (Vector256.IsHardwareAccelerated || Vector128.IsHardwareAccelerated) { //Repurpose the bins memory so we don't need to allocate any extra. The bins aren't in use right now anyway. - int paddedKeyCount = Vector256.IsHardwareAccelerated ? ((leafCount + 7) / 8) * 8 : ((leafCount + 3) / 4) * 4; + int paddedKeyCount = Vector256.IsHardwareAccelerated ? ((subtreeCount + 7) / 8) * 8 : ((subtreeCount + 3) / 4) * 4; - Debug.Assert(Unsafe.SizeOf() * bins.BinBoundingBoxes.Length >= (paddedKeyCount * 2 + leafCount) * Unsafe.SizeOf()); + Debug.Assert(Unsafe.SizeOf() * bins.BinBoundingBoxes.Length >= (paddedKeyCount * 2 + subtreeCount) * Unsafe.SizeOf(), + "The bins should preallocate enough space to handle the needs of microsweeps. They reuse the same allocations."); var keys = new Buffer(bins.BinBoundingBoxes.Memory, paddedKeyCount); var targetIndices = new Buffer(keys.Memory + paddedKeyCount, paddedKeyCount); - var indicesCache = new Buffer(targetIndices.Memory + paddedKeyCount, leafCount); - var boundingBoxCache = bins.BinBoundingBoxesScan; - //Store the bounds/indices into temporary memory so that we can shuffle them trivially once we're done with the sort. - //Doing this as a prepass means we don't have to worry about doing heavy swaps in the sort. - boundingBoxes.CopyTo(0, boundingBoxCache, 0, leafCount); - indices.CopyTo(0, indicesCache, 0, leafCount); //Compute the axis centroids up front to avoid having to recompute them during a sort. if (centroidSpan.X > centroidSpan.Y && centroidSpan.X > centroidSpan.Z) { - for (int i = 0; i < leafCount; ++i) + for (int i = 0; i < subtreeCount; ++i) { ref var bounds = ref boundingBoxes[i]; keys[i] = bounds.Min.X + bounds.Max.X; @@ -116,7 +151,7 @@ static unsafe void MicroSweepForBinnedBuilder(Vector4 centroidMin, Vector4 centr } else if (centroidSpan.Y > centroidSpan.Z) { - for (int i = 0; i < leafCount; ++i) + for (int i = 0; i < subtreeCount; ++i) { ref var bounds = ref boundingBoxes[i]; keys[i] = bounds.Min.Y + bounds.Max.Y; @@ -124,112 +159,187 @@ static unsafe void MicroSweepForBinnedBuilder(Vector4 centroidMin, Vector4 centr } else { - for (int i = 0; i < leafCount; ++i) + for (int i = 0; i < subtreeCount; ++i) { ref var bounds = ref boundingBoxes[i]; keys[i] = bounds.Min.Z + bounds.Max.Z; } } - for (int i = leafCount; i < paddedKeyCount; ++i) + for (int i = subtreeCount; i < paddedKeyCount; ++i) { keys[i] = float.MaxValue; } - VectorizedSorts.VectorCountingSort(keys, targetIndices, leafCount); + VectorizedSorts.VectorCountingSort(keys, targetIndices, subtreeCount); //Now that we know the target indices, copy things back. - for (int i = 0; i < leafCount; ++i) + //Have to copy things into a temporary cache to avoid overwrites since we didn't do any shuffling during the sort. + //Note that we can now reuse the keys memory. + if (typeof(TLeafCounts) != typeof(LeafCountBuffer)) { - var targetIndex = targetIndices[i]; - boundingBoxes[targetIndex] = boundingBoxCache[i]; - indices[targetIndex] = indicesCache[i]; + //There aren't any leaf counts that we need to copy; they're all just 1 anyway. + Debug.Assert(typeof(TLeafCounts) == typeof(UnitLeafCount)); + var indicesCache = new Buffer(bins.BinBoundingBoxes.Memory, subtreeCount); + var boundingBoxCache = bins.BinBoundingBoxesScan; + boundingBoxes.CopyTo(0, boundingBoxCache, 0, subtreeCount); + indices.CopyTo(0, indicesCache, 0, subtreeCount); + for (int i = 0; i < subtreeCount; ++i) + { + var targetIndex = targetIndices[i]; + boundingBoxes[targetIndex] = boundingBoxCache[i]; + indices[targetIndex] = indicesCache[i]; + } + } + else + { + //There are actual leaf counts we need to worry about on top of the rest! + var indicesCache = new Buffer(bins.BinBoundingBoxes.Memory, subtreeCount); + var leafCountCache = new Buffer(targetIndices.Memory + subtreeCount, subtreeCount); + var boundingBoxCache = bins.BinBoundingBoxesScan; + boundingBoxes.CopyTo(0, boundingBoxCache, 0, subtreeCount); + indices.CopyTo(0, indicesCache, 0, subtreeCount); + var leafCountBuffer = Unsafe.As(ref leafCounts).LeafCounts; + leafCountBuffer.CopyTo(0, leafCountCache, 0, subtreeCount); + for (int i = 0; i < subtreeCount; ++i) + { + var targetIndex = targetIndices[i]; + boundingBoxes[targetIndex] = boundingBoxCache[i]; + indices[targetIndex] = indicesCache[i]; + leafCountBuffer[targetIndex] = leafCountCache[i]; + } } } else { //No vectorization supported. Fall back to poopymode! - if (centroidSpan.X > centroidSpan.Y && centroidSpan.X > centroidSpan.Z) + if (typeof(TLeafCounts) != typeof(LeafCountBuffer)) { - var comparer = new BoundsComparerX(); - QuickSort.Sort(ref boundingBoxes[0], ref indices[0], 0, leafCount - 1, ref comparer); - } - else if (centroidSpan.Y > centroidSpan.Z) - { - var comparer = new BoundsComparerY(); - QuickSort.Sort(ref boundingBoxes[0], ref indices[0], 0, leafCount - 1, ref comparer); + if (centroidSpan.X > centroidSpan.Y && centroidSpan.X > centroidSpan.Z) + { + var comparer = new BoundsComparerX(); + QuickSort.Sort(ref boundingBoxes[0], ref indices[0], 0, subtreeCount - 1, ref comparer); + } + else if (centroidSpan.Y > centroidSpan.Z) + { + var comparer = new BoundsComparerY(); + QuickSort.Sort(ref boundingBoxes[0], ref indices[0], 0, subtreeCount - 1, ref comparer); + } + else + { + var comparer = new BoundsComparerZ(); + QuickSort.Sort(ref boundingBoxes[0], ref indices[0], 0, subtreeCount - 1, ref comparer); + } } else { - var comparer = new BoundsComparerZ(); - QuickSort.Sort(ref boundingBoxes[0], ref indices[0], 0, leafCount - 1, ref comparer); + //There are leaf counts that we need to sort alongside the rest. This is a pretty low value codepath, so we'll just create a targetIndices buffer. + var targetIndices = new Buffer(bins.BinBoundingBoxes.Memory, subtreeCount); + for (int i = 0; i < subtreeCount; ++i) + { + targetIndices[i] = i; + } + if (centroidSpan.X > centroidSpan.Y && centroidSpan.X > centroidSpan.Z) + { + var comparer = new BoundsComparerX(); + QuickSort.Sort(ref boundingBoxes[0], ref targetIndices[0], 0, subtreeCount - 1, ref comparer); + } + else if (centroidSpan.Y > centroidSpan.Z) + { + var comparer = new BoundsComparerY(); + QuickSort.Sort(ref boundingBoxes[0], ref targetIndices[0], 0, subtreeCount - 1, ref comparer); + } + else + { + var comparer = new BoundsComparerZ(); + QuickSort.Sort(ref boundingBoxes[0], ref targetIndices[0], 0, subtreeCount - 1, ref comparer); + } + //Apply the swaps to indices and leaf counts. + var indicesCache = new Buffer(targetIndices.Memory + subtreeCount, subtreeCount); + var leafCountCache = new Buffer(indicesCache.Memory + subtreeCount, subtreeCount); + indices.CopyTo(0, indicesCache, 0, subtreeCount); + var leafCountBuffer = Unsafe.As(ref leafCounts).LeafCounts; + leafCountBuffer.CopyTo(0, leafCountCache, 0, subtreeCount); + for (int i = 0; i < subtreeCount; ++i) + { + var targetIndex = targetIndices[i]; + leafCountBuffer[targetIndex] = leafCountCache[i]; + indices[targetIndex] = indicesCache[i]; + } } } - Debug.Assert(leafCount <= bins.MaximumBinCount || leafCount < bins.MicrosweepThreshold, "We're reusing the bin resources under the assumption that this is only ever called when there are less leaves than maximum bins."); + Debug.Assert(subtreeCount <= bins.MaximumBinCount || subtreeCount < bins.MicrosweepThreshold, "We're reusing the bin resources under the assumption that this is only ever called when there are less leaves than maximum bins."); //Identify the split index by examining the SAH of very split option. //Premerge from left to right so we have a sorta-summed area table to cheaply look up all possible child A bounds as we scan. bins.BinBoundingBoxesScan[0] = boundingBoxes[0]; - for (int i = 1; i < leafCount; ++i) + if (typeof(TLeafCounts) == typeof(LeafCountBuffer)) + bins.BinLeafCountsScan[0] = leafCounts[0]; + for (int i = 1; i < subtreeCount; ++i) { - ref var previousScanBounds = ref bins.BinBoundingBoxesScan[i - 1]; + var previousIndex = i - 1; + ref var previousScanBounds = ref bins.BinBoundingBoxesScan[previousIndex]; ref var scanBounds = ref bins.BinBoundingBoxesScan[i]; ref var bounds = ref boundingBoxes[i]; scanBounds.Min = Vector4.Min(bounds.Min, previousScanBounds.Min); scanBounds.Max = Vector4.Max(bounds.Max, previousScanBounds.Max); + if (typeof(TLeafCounts) == typeof(LeafCountBuffer)) + bins.BinLeafCountsScan[i] = leafCounts[i] + bins.BinLeafCountsScan[previousIndex]; } float bestSAH = float.MaxValue; int bestSplit = 1; //The split index is going to end up in child B. - var lastLeafIndex = leafCount - 1; - BoundingBox4 accumulatedBoundingBoxB = boundingBoxes[lastLeafIndex]; + var lastSubtreeIndex = subtreeCount - 1; + BoundingBox4 accumulatedBoundingBoxB = boundingBoxes[lastSubtreeIndex]; Unsafe.SkipInit(out BoundingBox4 bestBoundsB); int accumulatedLeafCountB = 1; - for (int splitIndexCandidate = lastLeafIndex; splitIndexCandidate >= 1; --splitIndexCandidate) + for (int splitIndexCandidate = lastSubtreeIndex; splitIndexCandidate >= 1; --splitIndexCandidate) { var previousIndex = splitIndexCandidate - 1; - var leafCountA = leafCount - accumulatedLeafCountB; - var sahCandidate = ComputeBoundsMetric(bins.BinBoundingBoxesScan[previousIndex]) * leafCountA + ComputeBoundsMetric(accumulatedBoundingBoxB) * accumulatedLeafCountB; + var sahCandidate = + ComputeBoundsMetric(bins.BinBoundingBoxesScan[previousIndex]) * (typeof(TLeafCounts) == typeof(UnitLeafCount) ? splitIndexCandidate : bins.BinLeafCountsScan[previousIndex]) + + ComputeBoundsMetric(accumulatedBoundingBoxB) * accumulatedLeafCountB; if (sahCandidate < bestSAH) { bestSAH = sahCandidate; bestSplit = splitIndexCandidate; bestBoundsB = accumulatedBoundingBoxB; } - ref var bounds = ref boundingBoxes[splitIndexCandidate - 1]; + ref var bounds = ref boundingBoxes[previousIndex]; accumulatedBoundingBoxB.Min = Vector4.Min(bounds.Min, accumulatedBoundingBoxB.Min); accumulatedBoundingBoxB.Max = Vector4.Max(bounds.Max, accumulatedBoundingBoxB.Max); - ++accumulatedLeafCountB; + accumulatedLeafCountB += leafCounts[previousIndex]; } var bestBoundsA = bins.BinBoundingBoxesScan[bestSplit - 1]; + var subtreeCountA = bestSplit; + var subtreeCountB = subtreeCount - bestSplit; + var bestLeafCountA = typeof(TLeafCounts) == typeof(UnitLeafCount) ? subtreeCountA : bins.BinLeafCountsScan[bestSplit - 1]; + var bestLeafCountB = typeof(TLeafCounts) == typeof(UnitLeafCount) ? subtreeCountB : bins.BinLeafCountsScan[subtreeCount - 1] - bestLeafCountA; - var aCount = bestSplit; - var bCount = leafCount - bestSplit; - - BuildNode(bestBoundsA, bestBoundsB, nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, aCount, bCount, out var aIndex, out var bIndex); - if (aCount > 1) + BuildNode(bestBoundsA, bestBoundsB, bestLeafCountA, bestLeafCountB, nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, subtreeCountA, subtreeCountB, out var aIndex, out var bIndex); + if (subtreeCountA > 1) { - var aBounds = boundingBoxes.Slice(aCount); + var aBounds = boundingBoxes.Slice(subtreeCountA); BoundingBox4 centroidBoundsA = aBounds[0]; - for (int i = 1; i < aCount; ++i) + for (int i = 1; i < subtreeCountA; ++i) { ref var bounds = ref aBounds[i]; centroidBoundsA.Min = Vector4.Min(centroidBoundsA.Min, bounds.Min); centroidBoundsA.Max = Vector4.Max(centroidBoundsA.Max, bounds.Max); } - MicroSweepForBinnedBuilder(centroidBoundsA.Min, centroidBoundsA.Max, indices.Slice(aCount), aBounds, nodes.Slice(1, aCount - 1), metanodes.Slice(1, aCount - 1), aIndex, nodeIndex, 0, bins); + MicroSweepForBinnedBuilder(centroidBoundsA.Min, centroidBoundsA.Max, indices.Slice(subtreeCountA), leafCounts.Slice(0, subtreeCountA), aBounds, nodes.Slice(1, subtreeCountA - 1), metanodes.Slice(1, subtreeCountA - 1), aIndex, nodeIndex, 0, bins); } - if (bCount > 1) + if (subtreeCountB > 1) { - var bBounds = boundingBoxes.Slice(aCount, bCount); + var bBounds = boundingBoxes.Slice(subtreeCountA, subtreeCountB); BoundingBox4 centroidBoundsB = bBounds[0]; - for (int i = 0; i < bCount; ++i) + for (int i = 0; i < subtreeCountB; ++i) { ref var bounds = ref bBounds[i]; centroidBoundsB.Min = Vector4.Min(centroidBoundsB.Min, bounds.Min); centroidBoundsB.Max = Vector4.Max(centroidBoundsB.Max, bounds.Max); } - MicroSweepForBinnedBuilder(centroidBoundsB.Min, centroidBoundsB.Max, indices.Slice(aCount, bCount), bBounds, nodes.Slice(aCount, bCount - 1), metanodes.Slice(aCount, bCount - 1), bIndex, nodeIndex, 1, bins); + MicroSweepForBinnedBuilder(centroidBoundsB.Min, centroidBoundsB.Max, indices.Slice(subtreeCountA, subtreeCountB), leafCounts.Slice(subtreeCountA, subtreeCountB), bBounds, nodes.Slice(subtreeCountA, subtreeCountB - 1), metanodes.Slice(subtreeCountA, subtreeCountB - 1), bIndex, nodeIndex, 1, bins); } } @@ -259,13 +369,20 @@ private static unsafe int ComputeBinIndex(Vector4 centroidMin, bool useX, bool u return binIndex; } - static unsafe void BinnedBuilderInternal(Buffer indices, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, in Bins bins) + static unsafe void BinnedBuilderInternal(Buffer indices, TLeafCounts leafCounts, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, + int nodeIndex, int parentNodeIndex, int childIndexInParent, in Bins bins) + where TLeafCounts : unmanaged, ILeafCountBuffer { + var subtreeCount = indices.Length; + if (subtreeCount == 2) + { + BuildNode(boundingBoxes[0], boundingBoxes[1], leafCounts[0], leafCounts[1], nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, 1, 1, out _, out _); + return; + } var centroidMin = new Vector4(float.MaxValue); var centroidMax = new Vector4(float.MinValue); - var leafCount = indices.Length; - for (int i = 0; i < leafCount; ++i) + for (int i = 0; i < subtreeCount; ++i) { ref var box = ref boundingBoxes[i]; //Note that centroids never bother scaling by 0.5. It's fine as long as we're consistent. @@ -279,42 +396,40 @@ static unsafe void BinnedBuilderInternal(Buffer indices, Buffer 1) - BinnedBuilderInternal(indices.Slice(countA), boundingBoxes.Slice(countA), nodes.Slice(1, countA - 1), metanodes.Slice(1, countA - 1), aIndex, nodeIndex, 0, bins); - if (countB > 1) - BinnedBuilderInternal(indices.Slice(countA, countB), boundingBoxes.Slice(countA, countB), nodes.Slice(countA, countB - 1), metanodes.Slice(countA, countB - 1), bIndex, nodeIndex, 1, bins); - return; - } - if (leafCount == 2) - { - BuildNode(boundingBoxes[0], boundingBoxes[1], nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, 1, 1, out _, out _); + BuildNode(boundsA, boundsB, degenerateLeafCountA, degenerateLeafCountB, nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, degenerateSubtreeCountA, degenerateSubtreeCountB, out var aIndex, out var bIndex); + if (degenerateSubtreeCountA > 1) + BinnedBuilderInternal(indices.Slice(degenerateSubtreeCountA), leafCounts.Slice(0, degenerateSubtreeCountA), boundingBoxes.Slice(degenerateSubtreeCountA), nodes.Slice(1, degenerateSubtreeCountA - 1), metanodes.Slice(1, degenerateSubtreeCountA - 1), aIndex, nodeIndex, 0, bins); + if (degenerateSubtreeCountB > 1) + BinnedBuilderInternal(indices.Slice(degenerateSubtreeCountA, degenerateSubtreeCountB), leafCounts.Slice(degenerateSubtreeCountA, degenerateSubtreeCountB), boundingBoxes.Slice(degenerateSubtreeCountA, degenerateSubtreeCountB), nodes.Slice(degenerateSubtreeCountA, degenerateSubtreeCountB - 1), metanodes.Slice(degenerateSubtreeCountA, degenerateSubtreeCountB - 1), bIndex, nodeIndex, 1, bins); return; } - if (leafCount <= bins.MicrosweepThreshold) + if (subtreeCount <= bins.MicrosweepThreshold) { - MicroSweepForBinnedBuilder(centroidMin, centroidMax, indices, boundingBoxes, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, bins); + MicroSweepForBinnedBuilder(centroidMin, centroidMax, indices, leafCounts, boundingBoxes, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, bins); return; } @@ -324,7 +439,7 @@ static unsafe void BinnedBuilderInternal(Buffer indices, Buffer= binCount); var offsetToBinIndex = new Vector4(binCount) / centroidSpan; @@ -345,19 +460,21 @@ static unsafe void BinnedBuilderInternal(Buffer indices, Buffer indices, Buffer indices, Buffer= 1; --splitIndexCandidate) { var previousIndex = splitIndexCandidate - 1; - var leafCountA = leafCount - accumulatedLeafCountB; - var sahCandidate = ComputeBoundsMetric(bins.BinBoundingBoxesScan[previousIndex]) * leafCountA + ComputeBoundsMetric(accumulatedBoundingBoxB) * accumulatedLeafCountB; + var sahCandidate = + ComputeBoundsMetric(bins.BinBoundingBoxesScan[previousIndex]) * (typeof(TLeafCounts) == typeof(UnitLeafCount) ? splitIndexCandidate : bins.BinLeafCountsScan[previousIndex]) + + ComputeBoundsMetric(accumulatedBoundingBoxB) * accumulatedLeafCountB; + if (sahCandidate < bestSAH) { bestSAH = sahCandidate; @@ -399,25 +521,32 @@ static unsafe void BinnedBuilderInternal(Buffer indices, Buffer= splitIndex) { //Belongs to B. Swap it. - var targetIndex = leafCount - bCount - 1; - Helpers.Swap(ref indices[targetIndex], ref indices[aCount]); + var targetIndex = subtreeCount - subtreeCountB - 1; + Helpers.Swap(ref indices[targetIndex], ref indices[subtreeCountA]); + if (typeof(TLeafCounts) == typeof(LeafCountBuffer)) + { + var tempTarget = leafCounts[targetIndex]; + var tempACount = leafCounts[subtreeCountA]; + leafCounts[subtreeCountA] = tempTarget; + leafCounts[targetIndex] = tempACount; + } if (Vector256.IsHardwareAccelerated) { var targetMemory = (byte*)(boundingBoxes.Memory + targetIndex); - var aCountMemory = (byte*)(boundingBoxes.Memory + aCount); + var aCountMemory = (byte*)(boundingBoxes.Memory + subtreeCountA); var targetVector = Vector256.Load(targetMemory); var aCountVector = Vector256.Load(aCountMemory); Vector256.Store(aCountVector, targetMemory); @@ -425,25 +554,28 @@ static unsafe void BinnedBuilderInternal(Buffer indices, Buffer 1) - BinnedBuilderInternal(indices.Slice(aCount), boundingBoxes.Slice(aCount), nodes.Slice(1, aCount - 1), metanodes.Slice(1, aCount - 1), aIndex, nodeIndex, 0, bins); - if (bCount > 1) - BinnedBuilderInternal(indices.Slice(aCount, bCount), boundingBoxes.Slice(aCount, bCount), nodes.Slice(aCount, bCount - 1), metanodes.Slice(aCount, bCount - 1), bIndex, nodeIndex, 1, bins); + Debug.Assert(subtreeCountA + subtreeCountB == subtreeCount); + BuildNode(bestboundsA, bestboundsB, leafCountA, leafCountB, nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, subtreeCountA, subtreeCountB, out var aIndex, out var bIndex); + if (subtreeCountA > 1) + BinnedBuilderInternal(indices.Slice(subtreeCountA), leafCounts.Slice(0, subtreeCountA), boundingBoxes.Slice(subtreeCountA), nodes.Slice(1, subtreeCountA - 1), metanodes.Slice(1, subtreeCountA - 1), aIndex, nodeIndex, 0, bins); + if (subtreeCountB > 1) + BinnedBuilderInternal(indices.Slice(subtreeCountA, subtreeCountB), leafCounts.Slice(subtreeCountA, subtreeCountB), boundingBoxes.Slice(subtreeCountA, subtreeCountB), nodes.Slice(subtreeCountA, subtreeCountB - 1), metanodes.Slice(subtreeCountA, subtreeCountB - 1), bIndex, nodeIndex, 1, bins); } } @@ -469,6 +601,10 @@ public static unsafe void BinnedBuilder(Buffer indices, Buffer boundingBoxes = boundingBoxes.Slice(indices.Length); nodes = nodes.Slice(leafCount - 1); + //Don't let the user pick values that will just cause an explosion. + Debug.Assert(minimumBinCount >= 2 && maximumBinCount >= 2, "At least two bins are required. In release mode, this will be clamped up to 2, but where did lower values come from?"); + minimumBinCount = Math.Max(2, minimumBinCount); + maximumBinCount = Math.Max(2, maximumBinCount); //The microsweep uses the same resources as the bin allocations, so expand to hold whichever is larger. var allocatedBinCount = Math.Max(maximumBinCount, microsweepThreshold); var binBoundsMemory = stackalloc BoundingBox4[allocatedBinCount * 2 + 1]; @@ -479,16 +615,20 @@ public static unsafe void BinnedBuilder(Buffer indices, Buffer bins.BinBoundingBoxes = new Buffer(binBoundsMemory, allocatedBinCount); bins.BinBoundingBoxesScan = new Buffer(binBoundsMemory + allocatedBinCount, allocatedBinCount); - var binLeafCountsMemory = stackalloc int[allocatedBinCount]; + var binLeafCountsMemory = stackalloc int[allocatedBinCount * 2]; bins.BinLeafCounts = new Buffer(binLeafCountsMemory, allocatedBinCount); + //This leaf count scan is actually only used when we're running a refinement but allocating an extra handful of bytes is pretty whocaresville. + bins.BinLeafCountsScan = new Buffer(binLeafCountsMemory + allocatedBinCount, allocatedBinCount); bins.MinimumBinCount = minimumBinCount; bins.MaximumBinCount = maximumBinCount; bins.LeafToBinMultiplier = leafToBinMultiplier; bins.MicrosweepThreshold = microsweepThreshold; + var leafCounts = new UnitLeafCount(); + //While we could avoid a recursive implementation, the overhead is low compared to the per-iteration cost. - BinnedBuilderInternal(indices, boundingBoxes.As(), nodes, metanodes, 0, -1, -1, bins); + BinnedBuilderInternal(indices, leafCounts, boundingBoxes.As(), nodes, metanodes, 0, -1, -1, bins); } } From da4d812c5a7af221980cc3309eb01ae26eaec70f Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 22 Sep 2022 21:13:50 -0500 Subject: [PATCH 622/947] Got rid of unnecessary preallocation. In theory, nonunit leaf counts are now supported, but probably not actually. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 35 +++++++++---------- .../SpecializedTests/TreeFiddlingTestDemo.cs | 6 ++-- 2 files changed, 20 insertions(+), 21 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 221892516..b0fcf7db0 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -96,7 +96,6 @@ struct Bins public Buffer BinBoundingBoxes; public Buffer BinBoundingBoxesScan; public Buffer BinLeafCounts; - public Buffer BinLeafCountsScan; public int MinimumBinCount; public int MaximumBinCount; @@ -271,8 +270,7 @@ static unsafe void MicroSweepForBinnedBuilder(Vector4 centroidMin, //Identify the split index by examining the SAH of very split option. //Premerge from left to right so we have a sorta-summed area table to cheaply look up all possible child A bounds as we scan. bins.BinBoundingBoxesScan[0] = boundingBoxes[0]; - if (typeof(TLeafCounts) == typeof(LeafCountBuffer)) - bins.BinLeafCountsScan[0] = leafCounts[0]; + int totalLeafCount = typeof(TLeafCounts) == typeof(LeafCountBuffer) ? leafCounts[0] : subtreeCount; for (int i = 1; i < subtreeCount; ++i) { var previousIndex = i - 1; @@ -282,7 +280,7 @@ static unsafe void MicroSweepForBinnedBuilder(Vector4 centroidMin, scanBounds.Min = Vector4.Min(bounds.Min, previousScanBounds.Min); scanBounds.Max = Vector4.Max(bounds.Max, previousScanBounds.Max); if (typeof(TLeafCounts) == typeof(LeafCountBuffer)) - bins.BinLeafCountsScan[i] = leafCounts[i] + bins.BinLeafCountsScan[previousIndex]; + totalLeafCount += leafCounts[i]; } float bestSAH = float.MaxValue; @@ -292,17 +290,20 @@ static unsafe void MicroSweepForBinnedBuilder(Vector4 centroidMin, BoundingBox4 accumulatedBoundingBoxB = boundingBoxes[lastSubtreeIndex]; Unsafe.SkipInit(out BoundingBox4 bestBoundsB); int accumulatedLeafCountB = 1; + int bestLeafCountB = 0; for (int splitIndexCandidate = lastSubtreeIndex; splitIndexCandidate >= 1; --splitIndexCandidate) { var previousIndex = splitIndexCandidate - 1; var sahCandidate = - ComputeBoundsMetric(bins.BinBoundingBoxesScan[previousIndex]) * (typeof(TLeafCounts) == typeof(UnitLeafCount) ? splitIndexCandidate : bins.BinLeafCountsScan[previousIndex]) + + ComputeBoundsMetric(bins.BinBoundingBoxesScan[previousIndex]) * (totalLeafCount - accumulatedLeafCountB) + ComputeBoundsMetric(accumulatedBoundingBoxB) * accumulatedLeafCountB; if (sahCandidate < bestSAH) { bestSAH = sahCandidate; bestSplit = splitIndexCandidate; bestBoundsB = accumulatedBoundingBoxB; + if (typeof(TLeafCounts) == typeof(LeafCountBuffer)) + bestLeafCountB = accumulatedLeafCountB; } ref var bounds = ref boundingBoxes[previousIndex]; accumulatedBoundingBoxB.Min = Vector4.Min(bounds.Min, accumulatedBoundingBoxB.Min); @@ -313,8 +314,9 @@ static unsafe void MicroSweepForBinnedBuilder(Vector4 centroidMin, var bestBoundsA = bins.BinBoundingBoxesScan[bestSplit - 1]; var subtreeCountA = bestSplit; var subtreeCountB = subtreeCount - bestSplit; - var bestLeafCountA = typeof(TLeafCounts) == typeof(UnitLeafCount) ? subtreeCountA : bins.BinLeafCountsScan[bestSplit - 1]; - var bestLeafCountB = typeof(TLeafCounts) == typeof(UnitLeafCount) ? subtreeCountB : bins.BinLeafCountsScan[subtreeCount - 1] - bestLeafCountA; + var bestLeafCountA = typeof(TLeafCounts) == typeof(UnitLeafCount) ? subtreeCountA : totalLeafCount - bestLeafCountB; + if (typeof(TLeafCounts) == typeof(UnitLeafCount)) + bestLeafCountB = subtreeCountB; BuildNode(bestBoundsA, bestBoundsB, bestLeafCountA, bestLeafCountB, nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, subtreeCountA, subtreeCountB, out var aIndex, out var bIndex); if (subtreeCountA > 1) @@ -473,8 +475,7 @@ static unsafe void BinnedBuilderInternal(Buffer indices, TLeaf //Identify the split index by examining the SAH of very split option. //Premerge from left to right so we have a sorta-summed area table to cheaply look up all possible child A bounds as we scan. bins.BinBoundingBoxesScan[0] = bins.BinBoundingBoxes[0]; - if (typeof(TLeafCounts) == typeof(LeafCountBuffer)) - bins.BinLeafCountsScan[0] = bins.BinLeafCounts[0]; + int totalLeafCount = typeof(TLeafCounts) == typeof(LeafCountBuffer) ? bins.BinLeafCounts[0] : subtreeCount; for (int i = 1; i < binCount; ++i) { var previousIndex = i - 1; @@ -484,7 +485,7 @@ static unsafe void BinnedBuilderInternal(Buffer indices, TLeaf xScanBounds.Min = Vector4.Min(xBounds.Min, xPreviousScanBounds.Min); xScanBounds.Max = Vector4.Max(xBounds.Max, xPreviousScanBounds.Max); if (typeof(TLeafCounts) == typeof(LeafCountBuffer)) - bins.BinLeafCountsScan[i] = bins.BinLeafCounts[i] + bins.BinLeafCountsScan[previousIndex]; + totalLeafCount += bins.BinLeafCounts[i]; } var leftBoundsX = bins.BinBoundingBoxes[0]; Debug.Assert( @@ -500,19 +501,19 @@ static unsafe void BinnedBuilderInternal(Buffer indices, TLeaf BoundingBox4 bestBoundingBoxB; bestBoundingBoxB = bins.BinBoundingBoxes[lastBinIndex]; int accumulatedLeafCountB = bins.BinLeafCounts[lastBinIndex]; - var totalLeafCount = typeof(TLeafCounts) == typeof(LeafCountBuffer) ? bins.BinLeafCountsScan[binCount - 1] : subtreeCount; + int bestLeafCountB = 0; for (int splitIndexCandidate = lastBinIndex; splitIndexCandidate >= 1; --splitIndexCandidate) { var previousIndex = splitIndexCandidate - 1; - var sahCandidate = - ComputeBoundsMetric(bins.BinBoundingBoxesScan[previousIndex]) * (typeof(TLeafCounts) == typeof(UnitLeafCount) ? splitIndexCandidate : bins.BinLeafCountsScan[previousIndex]) + - ComputeBoundsMetric(accumulatedBoundingBoxB) * accumulatedLeafCountB; + var sahCandidate = ComputeBoundsMetric(bins.BinBoundingBoxesScan[previousIndex]) * (totalLeafCount - accumulatedLeafCountB) + ComputeBoundsMetric(accumulatedBoundingBoxB) * accumulatedLeafCountB; if (sahCandidate < bestSAH) { bestSAH = sahCandidate; bestSplit = splitIndexCandidate; bestBoundingBoxB = accumulatedBoundingBoxB; + if (typeof(TLeafCounts) == typeof(LeafCountBuffer)) + bestLeafCountB = accumulatedLeafCountB; } ref var xBounds = ref bins.BinBoundingBoxes[previousIndex]; accumulatedBoundingBoxB.Min = Vector4.Min(xBounds.Min, accumulatedBoundingBoxB.Min); @@ -566,8 +567,8 @@ static unsafe void BinnedBuilderInternal(Buffer indices, TLeaf } } - var leafCountA = typeof(TLeafCounts) == typeof(UnitLeafCount) ? subtreeCountA : bins.BinLeafCountsScan[bestSplit - 1]; - var leafCountB = typeof(TLeafCounts) == typeof(UnitLeafCount) ? subtreeCountB : bins.BinLeafCountsScan[binCount - 1] - leafCountA; + var leafCountB = typeof(TLeafCounts) == typeof(UnitLeafCount) ? subtreeCountB : bestLeafCountB; + var leafCountA = typeof(TLeafCounts) == typeof(UnitLeafCount) ? subtreeCountA : totalLeafCount - leafCountB; { Debug.Assert(subtreeCountA + subtreeCountB == subtreeCount); @@ -617,8 +618,6 @@ public static unsafe void BinnedBuilder(Buffer indices, Buffer var binLeafCountsMemory = stackalloc int[allocatedBinCount * 2]; bins.BinLeafCounts = new Buffer(binLeafCountsMemory, allocatedBinCount); - //This leaf count scan is actually only used when we're running a refinement but allocating an extra handful of bytes is pretty whocaresville. - bins.BinLeafCountsScan = new Buffer(binLeafCountsMemory + allocatedBinCount, allocatedBinCount); bins.MinimumBinCount = minimumBinCount; bins.MaximumBinCount = maximumBinCount; diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index 06c45817e..696f3eec3 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -157,8 +157,8 @@ public override void Initialize(ContentArchive content, Camera camera) Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(4, 1)); //Create a mesh. - var width = 128; - var height = 16; + var width = 768; + var height = 768; var scale = new Vector3(1, 1, 1); //DemoMeshHelper.CreateDeformedPlane(width, height, (x, y) => new Vector3(x - width * scale.X * 0.5f, 2f * (float)(Math.Sin(x * 0.5f) * Math.Sin(y * 0.5f)), y - height * scale.Y * 0.5f), scale, BufferPool, out var mesh); //DemoMeshHelper.CreateDeformedPlane(width, height, (x, y) => new Vector3(x - width * scale.X * 0.5f, 0, y - height * scale.Y * 0.5f), scale, BufferPool, out var mesh); @@ -267,7 +267,7 @@ static void RefitTest(Action function, string name, ref Tree tree) static void BinnedTest(Action function, string name, ref Tree tree) { long accumulatedTime = 0; - const int testCount = 256; + const int testCount = 16; for (int i = 0; i < testCount; ++i) { var startTime = Stopwatch.GetTimestamp(); From 1509b4c43a952ff168cd0adde7a14a09ebeea696 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 22 Sep 2022 21:16:04 -0500 Subject: [PATCH 623/947] Tinycleanup. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 33 ++++--------------------- 1 file changed, 5 insertions(+), 28 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index b0fcf7db0..556cbb140 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -43,10 +43,7 @@ static void BuildNode( node.B.LeafCount = leafCountB; } - internal static float ComputeBoundsMetric(BoundingBox4 bounds) - { - return ComputeBoundsMetric(bounds.Min, bounds.Max); - } + internal static float ComputeBoundsMetric(BoundingBox4 bounds) => ComputeBoundsMetric(bounds.Min, bounds.Max); internal static float ComputeBoundsMetric(Vector4 min, Vector4 max) { //Note that we just use the SAH. While we are primarily interested in volume queries for the purposes of collision detection, the topological difference @@ -61,10 +58,7 @@ internal static float ComputeBoundsMetric(Vector4 min, Vector4 max) interface ILeafCountBuffer where T : unmanaged, ILeafCountBuffer { int this[int index] { get; set; } - T Slice(int startIndex, int count); - - } /// @@ -73,11 +67,7 @@ interface ILeafCountBuffer where T : unmanaged, ILeafCountBuffer struct UnitLeafCount : ILeafCountBuffer { public int this[int index] { get => 1; set { } } - - public UnitLeafCount Slice(int startIndex, int count) - { - return this; - } + public UnitLeafCount Slice(int startIndex, int count) => this; } /// @@ -87,7 +77,6 @@ struct LeafCountBuffer : ILeafCountBuffer { public Buffer LeafCounts; public int this[int index] { get => LeafCounts[index]; set => LeafCounts[index] = value; } - public LeafCountBuffer Slice(int startIndex, int count) => new() { LeafCounts = LeafCounts.Slice(startIndex, count) }; } @@ -96,26 +85,15 @@ struct Bins public Buffer BinBoundingBoxes; public Buffer BinBoundingBoxesScan; public Buffer BinLeafCounts; - public int MinimumBinCount; public int MaximumBinCount; public float LeafToBinMultiplier; public int MicrosweepThreshold; - } - struct BoundsComparerX : IComparerRef - { - public int Compare(ref BoundingBox4 a, ref BoundingBox4 b) => (a.Min.X + a.Max.X) > (b.Min.X + b.Max.X) ? -1 : 1; - } - struct BoundsComparerY : IComparerRef - { - public int Compare(ref BoundingBox4 a, ref BoundingBox4 b) => (a.Min.Y + a.Max.Y) > (b.Min.Y + b.Max.Y) ? -1 : 1; - } - struct BoundsComparerZ : IComparerRef - { - public int Compare(ref BoundingBox4 a, ref BoundingBox4 b) => (a.Min.Z + a.Max.Z) > (b.Min.Z + b.Max.Z) ? -1 : 1; - } + struct BoundsComparerX : IComparerRef { public int Compare(ref BoundingBox4 a, ref BoundingBox4 b) => (a.Min.X + a.Max.X) > (b.Min.X + b.Max.X) ? -1 : 1; } + struct BoundsComparerY : IComparerRef { public int Compare(ref BoundingBox4 a, ref BoundingBox4 b) => (a.Min.Y + a.Max.Y) > (b.Min.Y + b.Max.Y) ? -1 : 1; } + struct BoundsComparerZ : IComparerRef { public int Compare(ref BoundingBox4 a, ref BoundingBox4 b) => (a.Min.Z + a.Max.Z) > (b.Min.Z + b.Max.Z) ? -1 : 1; } static unsafe void MicroSweepForBinnedBuilder(Vector4 centroidMin, Vector4 centroidMax, Buffer indices, TLeafCounts leafCounts, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, Bins bins) where TLeafCounts : unmanaged, ILeafCountBuffer @@ -629,6 +607,5 @@ public static unsafe void BinnedBuilder(Buffer indices, Buffer //While we could avoid a recursive implementation, the overhead is low compared to the per-iteration cost. BinnedBuilderInternal(indices, leafCounts, boundingBoxes.As(), nodes, metanodes, 0, -1, -1, bins); } - } } From 4a8fdf1e9e41b7663444dcb0d04aa8c9286c6304 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 22 Sep 2022 21:20:52 -0500 Subject: [PATCH 624/947] More tinycleanup. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 39 +++++++------------------ 1 file changed, 11 insertions(+), 28 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 556cbb140..2bf5719fd 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -321,7 +321,6 @@ static unsafe void MicroSweepForBinnedBuilder(Vector4 centroidMin, } MicroSweepForBinnedBuilder(centroidBoundsB.Min, centroidBoundsB.Max, indices.Slice(subtreeCountA, subtreeCountB), leafCounts.Slice(subtreeCountA, subtreeCountB), bBounds, nodes.Slice(subtreeCountA, subtreeCountB - 1), metanodes.Slice(subtreeCountA, subtreeCountB - 1), bIndex, nodeIndex, 1, bins); } - } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -331,22 +330,13 @@ private static unsafe int ComputeBinIndex(Vector4 centroidMin, bool useX, bool u var binIndicesForLeafContinuous = Vector4.Min(maximumBinIndex, (centroid - centroidMin) * offsetToBinIndex); //Note that we don't store out any of the indices into per-bin lists here. We only *really* want two final groups for the children, //and we can easily compute those by performing another scan. It requires recomputing the bin indices, but that's really not much of a concern. - int binIndex; //To extract the desired lane, we need to use a variable shuffle mask. At the time of writing, the Vector128 cross platform shuffle did not like variable masks. if (Avx.IsSupported) - { - binIndex = (int)Vector128.ToScalar(Avx.PermuteVar(binIndicesForLeafContinuous.AsVector128(), permuteMask)); - } + return (int)Vector128.ToScalar(Avx.PermuteVar(binIndicesForLeafContinuous.AsVector128(), permuteMask)); else if (Vector128.IsHardwareAccelerated) - { - binIndex = (int)Vector128.GetElement(binIndicesForLeafContinuous.AsVector128(), axisIndex); - } + return (int)Vector128.GetElement(binIndicesForLeafContinuous.AsVector128(), axisIndex); else - { - binIndex = (int)(useX ? binIndicesForLeafContinuous.X : useY ? binIndicesForLeafContinuous.Y : binIndicesForLeafContinuous.Z); - } - - return binIndex; + return (int)(useX ? binIndicesForLeafContinuous.X : useY ? binIndicesForLeafContinuous.Y : binIndicesForLeafContinuous.Z); } static unsafe void BinnedBuilderInternal(Buffer indices, TLeafCounts leafCounts, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, @@ -426,7 +416,6 @@ static unsafe void BinnedBuilderInternal(Buffer indices, TLeaf //Avoid letting NaNs into the offsetToBinIndex scale. offsetToBinIndex = Vector128.ConditionalSelect(axisIsDegenerate, Vector128.Zero, offsetToBinIndex.AsVector128()).AsVector4(); - for (int i = 0; i < binCount; ++i) { ref var boxX = ref bins.BinBoundingBoxes[i]; @@ -435,11 +424,9 @@ static unsafe void BinnedBuilderInternal(Buffer indices, TLeaf bins.BinLeafCounts[i] = 0; } - //Unfortunately, unrolling has a slight benefit here. - var maximumBinIndex = new Vector4(binCount - 1); - //Note that we don't store out any of the indices into per-bin lists here. We only *really* want two final groups for the children, //and we can easily compute those by performing another scan. It requires recomputing the bin indices, but that's really not much of a concern. + var maximumBinIndex = new Vector4(binCount - 1); for (int i = 0; i < subtreeCount; ++i) { ref var box = ref boundingBoxes[i]; @@ -544,18 +531,14 @@ static unsafe void BinnedBuilderInternal(Buffer indices, TLeaf ++subtreeCountA; } } - var leafCountB = typeof(TLeafCounts) == typeof(UnitLeafCount) ? subtreeCountB : bestLeafCountB; var leafCountA = typeof(TLeafCounts) == typeof(UnitLeafCount) ? subtreeCountA : totalLeafCount - leafCountB; - - { - Debug.Assert(subtreeCountA + subtreeCountB == subtreeCount); - BuildNode(bestboundsA, bestboundsB, leafCountA, leafCountB, nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, subtreeCountA, subtreeCountB, out var aIndex, out var bIndex); - if (subtreeCountA > 1) - BinnedBuilderInternal(indices.Slice(subtreeCountA), leafCounts.Slice(0, subtreeCountA), boundingBoxes.Slice(subtreeCountA), nodes.Slice(1, subtreeCountA - 1), metanodes.Slice(1, subtreeCountA - 1), aIndex, nodeIndex, 0, bins); - if (subtreeCountB > 1) - BinnedBuilderInternal(indices.Slice(subtreeCountA, subtreeCountB), leafCounts.Slice(subtreeCountA, subtreeCountB), boundingBoxes.Slice(subtreeCountA, subtreeCountB), nodes.Slice(subtreeCountA, subtreeCountB - 1), metanodes.Slice(subtreeCountA, subtreeCountB - 1), bIndex, nodeIndex, 1, bins); - } + Debug.Assert(subtreeCountA + subtreeCountB == subtreeCount); + BuildNode(bestboundsA, bestboundsB, leafCountA, leafCountB, nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, subtreeCountA, subtreeCountB, out var nodeChildIndexA, out var nodeChildIndexB); + if (subtreeCountA > 1) + BinnedBuilderInternal(indices.Slice(subtreeCountA), leafCounts.Slice(0, subtreeCountA), boundingBoxes.Slice(subtreeCountA), nodes.Slice(1, subtreeCountA - 1), metanodes.Slice(1, subtreeCountA - 1), nodeChildIndexA, nodeIndex, 0, bins); + if (subtreeCountB > 1) + BinnedBuilderInternal(indices.Slice(subtreeCountA, subtreeCountB), leafCounts.Slice(subtreeCountA, subtreeCountB), boundingBoxes.Slice(subtreeCountA, subtreeCountB), nodes.Slice(subtreeCountA, subtreeCountB - 1), metanodes.Slice(subtreeCountA, subtreeCountB - 1), nodeChildIndexB, nodeIndex, 1, bins); } public static unsafe void BinnedBuilder(Buffer indices, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, BufferPool pool, @@ -594,7 +577,7 @@ public static unsafe void BinnedBuilder(Buffer indices, Buffer bins.BinBoundingBoxes = new Buffer(binBoundsMemory, allocatedBinCount); bins.BinBoundingBoxesScan = new Buffer(binBoundsMemory + allocatedBinCount, allocatedBinCount); - var binLeafCountsMemory = stackalloc int[allocatedBinCount * 2]; + var binLeafCountsMemory = stackalloc int[allocatedBinCount]; bins.BinLeafCounts = new Buffer(binLeafCountsMemory, allocatedBinCount); bins.MinimumBinCount = minimumBinCount; From 3fb4dbc2d9a67530d291cf3af67ee67a4b317819 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 15 Oct 2022 17:30:34 -0500 Subject: [PATCH 625/947] Binned builder now writes to leaf data. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 91 +++++++++++++------ .../SpecializedTests/TreeFiddlingTestDemo.cs | 12 +-- 2 files changed, 62 insertions(+), 41 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 2bf5719fd..9071b4d89 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -20,21 +20,50 @@ namespace BepuPhysics.Trees { partial struct Tree { + struct LeavesHandledInPostPass { } [MethodImpl(MethodImplOptions.AggressiveInlining)] - static void BuildNode( + static void BuildNode( BoundingBox4 a, BoundingBox4 b, int leafCountA, int leafCountB, Buffer nodes, Buffer metanodes, Buffer indices, - int nodeIndex, int parentNodeIndex, int childIndexInParent, int subtreeCountA, int subtreeCountB, out int aIndex, out int bIndex) + int nodeIndex, int parentNodeIndex, int childIndexInParent, int subtreeCountA, int subtreeCountB, ref TLeaves leaves, out int aIndex, out int bIndex) + where TLeaves : unmanaged { + Debug.Assert(typeof(TLeaves) == typeof(LeavesHandledInPostPass) || typeof(TLeaves) == typeof(Buffer), "While we didn't bother with an interface here, we assume one of two types only."); ref var metanode = ref metanodes[0]; metanode.Parent = parentNodeIndex; metanode.IndexInParent = childIndexInParent; metanode.RefineFlag = 0; ref var node = ref nodes[0]; - aIndex = subtreeCountA == 1 ? indices[0] : nodeIndex + 1; - bIndex = subtreeCountB == 1 ? indices[^1] : nodeIndex + subtreeCountA;//parentNodeIndex + 1 + (subtreeCountA - 1) + if (subtreeCountA == 1) + { + aIndex = indices[0]; + if (typeof(TLeaves) == typeof(Buffer)) + { + Debug.Assert(aIndex < 0, "During building, any subtreeCount of 1 should imply a leaf."); + //This is a leaf node, and this is a direct builder execution, so write to the leaf data. + Unsafe.As>(ref leaves)[Encode(aIndex)] = new Leaf(nodeIndex, 0); + } + } + else + { + aIndex = nodeIndex + 1; + } + if (subtreeCountB == 1) + { + bIndex = indices[^1]; + if (typeof(TLeaves) == typeof(Buffer)) + { + Debug.Assert(bIndex < 0, "During building, any subtreeCount of 1 should imply a leaf."); + //This is a leaf node, and this is a direct builder execution, so write to the leaf data. + Unsafe.As>(ref leaves)[Encode(bIndex)] = new Leaf(nodeIndex, 1); + } + } + else + { + bIndex = nodeIndex + subtreeCountA; //parentNodeIndex + 1 + (subtreeCountA - 1) + } node.A = Unsafe.As(ref a); node.B = Unsafe.As(ref b); node.A.Index = aIndex; @@ -95,14 +124,16 @@ struct BoundsComparerX : IComparerRef { public int Compare(ref Bou struct BoundsComparerY : IComparerRef { public int Compare(ref BoundingBox4 a, ref BoundingBox4 b) => (a.Min.Y + a.Max.Y) > (b.Min.Y + b.Max.Y) ? -1 : 1; } struct BoundsComparerZ : IComparerRef { public int Compare(ref BoundingBox4 a, ref BoundingBox4 b) => (a.Min.Z + a.Max.Z) > (b.Min.Z + b.Max.Z) ? -1 : 1; } - static unsafe void MicroSweepForBinnedBuilder(Vector4 centroidMin, Vector4 centroidMax, Buffer indices, TLeafCounts leafCounts, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, Bins bins) - where TLeafCounts : unmanaged, ILeafCountBuffer + static unsafe void MicroSweepForBinnedBuilder( + Vector4 centroidMin, Vector4 centroidMax, Buffer indices, TLeafCounts leafCounts, ref TLeaves leaves, + Buffer boundingBoxes, Buffer nodes, Buffer metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, Bins bins) + where TLeafCounts : unmanaged, ILeafCountBuffer where TLeaves : unmanaged { //This is a very small scale sweep build. var subtreeCount = indices.Length; if (subtreeCount == 2) { - BuildNode(boundingBoxes[0], boundingBoxes[1], leafCounts[0], leafCounts[1], nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, 1, 1, out _, out _); + BuildNode(boundingBoxes[0], boundingBoxes[1], leafCounts[0], leafCounts[1], nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, 1, 1, ref leaves, out _, out _); return; } var centroidSpan = centroidMax - centroidMin; @@ -296,7 +327,7 @@ static unsafe void MicroSweepForBinnedBuilder(Vector4 centroidMin, if (typeof(TLeafCounts) == typeof(UnitLeafCount)) bestLeafCountB = subtreeCountB; - BuildNode(bestBoundsA, bestBoundsB, bestLeafCountA, bestLeafCountB, nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, subtreeCountA, subtreeCountB, out var aIndex, out var bIndex); + BuildNode(bestBoundsA, bestBoundsB, bestLeafCountA, bestLeafCountB, nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, subtreeCountA, subtreeCountB, ref leaves, out var aIndex, out var bIndex); if (subtreeCountA > 1) { var aBounds = boundingBoxes.Slice(subtreeCountA); @@ -307,7 +338,7 @@ static unsafe void MicroSweepForBinnedBuilder(Vector4 centroidMin, centroidBoundsA.Min = Vector4.Min(centroidBoundsA.Min, bounds.Min); centroidBoundsA.Max = Vector4.Max(centroidBoundsA.Max, bounds.Max); } - MicroSweepForBinnedBuilder(centroidBoundsA.Min, centroidBoundsA.Max, indices.Slice(subtreeCountA), leafCounts.Slice(0, subtreeCountA), aBounds, nodes.Slice(1, subtreeCountA - 1), metanodes.Slice(1, subtreeCountA - 1), aIndex, nodeIndex, 0, bins); + MicroSweepForBinnedBuilder(centroidBoundsA.Min, centroidBoundsA.Max, indices.Slice(subtreeCountA), leafCounts.Slice(0, subtreeCountA), ref leaves, aBounds, nodes.Slice(1, subtreeCountA - 1), metanodes.Slice(1, subtreeCountA - 1), aIndex, nodeIndex, 0, bins); } if (subtreeCountB > 1) { @@ -319,7 +350,7 @@ static unsafe void MicroSweepForBinnedBuilder(Vector4 centroidMin, centroidBoundsB.Min = Vector4.Min(centroidBoundsB.Min, bounds.Min); centroidBoundsB.Max = Vector4.Max(centroidBoundsB.Max, bounds.Max); } - MicroSweepForBinnedBuilder(centroidBoundsB.Min, centroidBoundsB.Max, indices.Slice(subtreeCountA, subtreeCountB), leafCounts.Slice(subtreeCountA, subtreeCountB), bBounds, nodes.Slice(subtreeCountA, subtreeCountB - 1), metanodes.Slice(subtreeCountA, subtreeCountB - 1), bIndex, nodeIndex, 1, bins); + MicroSweepForBinnedBuilder(centroidBoundsB.Min, centroidBoundsB.Max, indices.Slice(subtreeCountA, subtreeCountB), leafCounts.Slice(subtreeCountA, subtreeCountB), ref leaves, bBounds, nodes.Slice(subtreeCountA, subtreeCountB - 1), metanodes.Slice(subtreeCountA, subtreeCountB - 1), bIndex, nodeIndex, 1, bins); } } @@ -339,14 +370,14 @@ private static unsafe int ComputeBinIndex(Vector4 centroidMin, bool useX, bool u return (int)(useX ? binIndicesForLeafContinuous.X : useY ? binIndicesForLeafContinuous.Y : binIndicesForLeafContinuous.Z); } - static unsafe void BinnedBuilderInternal(Buffer indices, TLeafCounts leafCounts, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, + static unsafe void BinnedBuilderInternal(Buffer indices, TLeafCounts leafCounts, ref TLeaves leaves, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, in Bins bins) - where TLeafCounts : unmanaged, ILeafCountBuffer + where TLeafCounts : unmanaged, ILeafCountBuffer where TLeaves : unmanaged { var subtreeCount = indices.Length; if (subtreeCount == 2) { - BuildNode(boundingBoxes[0], boundingBoxes[1], leafCounts[0], leafCounts[1], nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, 1, 1, out _, out _); + BuildNode(boundingBoxes[0], boundingBoxes[1], leafCounts[0], leafCounts[1], nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, 1, 1, ref leaves, out _, out _); return; } var centroidMin = new Vector4(float.MaxValue); @@ -389,17 +420,17 @@ static unsafe void BinnedBuilderInternal(Buffer indices, TLeaf boundsB.Max = Vector4.Max(bounds.Max, boundsB.Max); degenerateLeafCountB += leafCounts[i]; } - BuildNode(boundsA, boundsB, degenerateLeafCountA, degenerateLeafCountB, nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, degenerateSubtreeCountA, degenerateSubtreeCountB, out var aIndex, out var bIndex); + BuildNode(boundsA, boundsB, degenerateLeafCountA, degenerateLeafCountB, nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, degenerateSubtreeCountA, degenerateSubtreeCountB, ref leaves, out var aIndex, out var bIndex); if (degenerateSubtreeCountA > 1) - BinnedBuilderInternal(indices.Slice(degenerateSubtreeCountA), leafCounts.Slice(0, degenerateSubtreeCountA), boundingBoxes.Slice(degenerateSubtreeCountA), nodes.Slice(1, degenerateSubtreeCountA - 1), metanodes.Slice(1, degenerateSubtreeCountA - 1), aIndex, nodeIndex, 0, bins); + BinnedBuilderInternal(indices.Slice(degenerateSubtreeCountA), leafCounts.Slice(0, degenerateSubtreeCountA), ref leaves, boundingBoxes.Slice(degenerateSubtreeCountA), nodes.Slice(1, degenerateSubtreeCountA - 1), metanodes.Slice(1, degenerateSubtreeCountA - 1), aIndex, nodeIndex, 0, bins); if (degenerateSubtreeCountB > 1) - BinnedBuilderInternal(indices.Slice(degenerateSubtreeCountA, degenerateSubtreeCountB), leafCounts.Slice(degenerateSubtreeCountA, degenerateSubtreeCountB), boundingBoxes.Slice(degenerateSubtreeCountA, degenerateSubtreeCountB), nodes.Slice(degenerateSubtreeCountA, degenerateSubtreeCountB - 1), metanodes.Slice(degenerateSubtreeCountA, degenerateSubtreeCountB - 1), bIndex, nodeIndex, 1, bins); + BinnedBuilderInternal(indices.Slice(degenerateSubtreeCountA, degenerateSubtreeCountB), leafCounts.Slice(degenerateSubtreeCountA, degenerateSubtreeCountB), ref leaves, boundingBoxes.Slice(degenerateSubtreeCountA, degenerateSubtreeCountB), nodes.Slice(degenerateSubtreeCountA, degenerateSubtreeCountB - 1), metanodes.Slice(degenerateSubtreeCountA, degenerateSubtreeCountB - 1), bIndex, nodeIndex, 1, bins); return; } if (subtreeCount <= bins.MicrosweepThreshold) { - MicroSweepForBinnedBuilder(centroidMin, centroidMax, indices, leafCounts, boundingBoxes, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, bins); + MicroSweepForBinnedBuilder(centroidMin, centroidMax, indices, leafCounts, ref leaves, boundingBoxes, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, bins); return; } @@ -534,34 +565,34 @@ static unsafe void BinnedBuilderInternal(Buffer indices, TLeaf var leafCountB = typeof(TLeafCounts) == typeof(UnitLeafCount) ? subtreeCountB : bestLeafCountB; var leafCountA = typeof(TLeafCounts) == typeof(UnitLeafCount) ? subtreeCountA : totalLeafCount - leafCountB; Debug.Assert(subtreeCountA + subtreeCountB == subtreeCount); - BuildNode(bestboundsA, bestboundsB, leafCountA, leafCountB, nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, subtreeCountA, subtreeCountB, out var nodeChildIndexA, out var nodeChildIndexB); + BuildNode(bestboundsA, bestboundsB, leafCountA, leafCountB, nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, subtreeCountA, subtreeCountB, ref leaves, out var nodeChildIndexA, out var nodeChildIndexB); if (subtreeCountA > 1) - BinnedBuilderInternal(indices.Slice(subtreeCountA), leafCounts.Slice(0, subtreeCountA), boundingBoxes.Slice(subtreeCountA), nodes.Slice(1, subtreeCountA - 1), metanodes.Slice(1, subtreeCountA - 1), nodeChildIndexA, nodeIndex, 0, bins); + BinnedBuilderInternal(indices.Slice(subtreeCountA), leafCounts.Slice(0, subtreeCountA), ref leaves, boundingBoxes.Slice(subtreeCountA), nodes.Slice(1, subtreeCountA - 1), metanodes.Slice(1, subtreeCountA - 1), nodeChildIndexA, nodeIndex, 0, bins); if (subtreeCountB > 1) - BinnedBuilderInternal(indices.Slice(subtreeCountA, subtreeCountB), leafCounts.Slice(subtreeCountA, subtreeCountB), boundingBoxes.Slice(subtreeCountA, subtreeCountB), nodes.Slice(subtreeCountA, subtreeCountB - 1), metanodes.Slice(subtreeCountA, subtreeCountB - 1), nodeChildIndexB, nodeIndex, 1, bins); + BinnedBuilderInternal(indices.Slice(subtreeCountA, subtreeCountB), leafCounts.Slice(subtreeCountA, subtreeCountB), ref leaves, boundingBoxes.Slice(subtreeCountA, subtreeCountB), nodes.Slice(subtreeCountA, subtreeCountB - 1), metanodes.Slice(subtreeCountA, subtreeCountB - 1), nodeChildIndexB, nodeIndex, 1, bins); } - public static unsafe void BinnedBuilder(Buffer indices, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, BufferPool pool, + public static unsafe void BinnedBuilder(Buffer encodedLeafIndices, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, Buffer leaves, BufferPool pool, int minimumBinCount = 16, int maximumBinCount = 64, float leafToBinMultiplier = 1 / 16f, int microsweepThreshold = 64) { - var leafCount = indices.Length; - Debug.Assert(boundingBoxes.Length >= leafCount, "The bounding boxes provided must cover the range of indices provided."); - Debug.Assert(nodes.Length >= leafCount - 1, "The output nodes must be able to contain the nodes created for the leaves."); - if (leafCount == 0) + var subtreeCount = encodedLeafIndices.Length; + Debug.Assert(boundingBoxes.Length >= subtreeCount, "The bounding boxes provided must cover the range of indices provided."); + Debug.Assert(nodes.Length >= subtreeCount - 1, "The output nodes must be able to contain the nodes created for the leaves."); + if (subtreeCount == 0) return; - if (leafCount == 1) + if (subtreeCount == 1) { //If there's only one leaf, the tree has a special format: the root node has only one child. ref var root = ref nodes[0]; root.A.Min = boundingBoxes[0].Min; - root.A.Index = indices[0]; //Node that we assume the indices are already encoded. This function works with subtree refinements as well which can manage either leaves or internals. + root.A.Index = encodedLeafIndices[0]; //Node that we assume the indices are already encoded. This function works with subtree refinements as well which can manage either leaves or internals. root.A.Max = boundingBoxes[0].Max; root.A.LeafCount = 1; root.B = default; return; } - boundingBoxes = boundingBoxes.Slice(indices.Length); - nodes = nodes.Slice(leafCount - 1); + boundingBoxes = boundingBoxes.Slice(encodedLeafIndices.Length); + nodes = nodes.Slice(subtreeCount - 1); //Don't let the user pick values that will just cause an explosion. Debug.Assert(minimumBinCount >= 2 && maximumBinCount >= 2, "At least two bins are required. In release mode, this will be clamped up to 2, but where did lower values come from?"); @@ -588,7 +619,7 @@ public static unsafe void BinnedBuilder(Buffer indices, Buffer var leafCounts = new UnitLeafCount(); //While we could avoid a recursive implementation, the overhead is low compared to the per-iteration cost. - BinnedBuilderInternal(indices, leafCounts, boundingBoxes.As(), nodes, metanodes, 0, -1, -1, bins); + BinnedBuilderInternal(encodedLeafIndices, leafCounts, ref leaves, boundingBoxes.As(), nodes, metanodes, 0, -1, -1, bins); } } } diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index 696f3eec3..018cab8a5 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -186,17 +186,7 @@ public override void Initialize(ContentArchive content, Camera camera) BinnedTest(() => { - Tree.BinnedBuilder(leafIndices, leafBounds, mesh.Tree.Nodes, mesh.Tree.Metanodes, BufferPool); - for (int i = 0; i < mesh.Tree.NodeCount; ++i) - { - ref var node = ref mesh.Tree.Nodes[i]; - ref var a = ref node.A; - ref var b = ref node.B; - if (a.Index < 0) - mesh.Tree.Leaves[Tree.Encode(a.Index)] = new Leaf(i, 0); - if (b.Index < 0) - mesh.Tree.Leaves[Tree.Encode(b.Index)] = new Leaf(i, 1); - } + Tree.BinnedBuilder(leafIndices, leafBounds, mesh.Tree.Nodes, mesh.Tree.Metanodes, mesh.Tree.Leaves, BufferPool); }, "Revamp Single Axis", ref mesh.Tree); From fe9e2629d41ff0ae62f681a22218b0f99164f56b Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 27 Oct 2022 19:26:22 -0500 Subject: [PATCH 626/947] IThreadDispatcher now passes along a context pointer to workers. --- BepuPhysics/BatchCompressor.cs | 6 ++--- BepuPhysics/CollisionDetection/BroadPhase.cs | 10 ++++---- .../CollidableOverlapFinder.cs | 6 ++--- BepuPhysics/CollisionDetection/NarrowPhase.cs | 8 +++---- .../CollisionDetection/NarrowPhasePreflush.cs | 6 ++--- BepuPhysics/IslandAwakener.cs | 10 ++++---- BepuPhysics/IslandSleeper.cs | 20 ++++++++-------- BepuPhysics/PoseIntegrator.cs | 10 ++++---- BepuPhysics/Solver_Solve.cs | 12 +++++----- BepuPhysics/Trees/Tree_IntertreeQueriesMT.cs | 3 ++- .../Trees/Tree_MultithreadedRefitRefine.cs | 10 ++++---- BepuPhysics/Trees/Tree_SelfQueriesMT.cs | 2 +- BepuUtilities/IThreadDispatcher.cs | 24 +++++++++++++++++-- BepuUtilities/ThreadDispatcher.cs | 14 +++++++---- DemoRenderer/ParallelLooper.cs | 9 +++---- .../Demos/Characters/CharacterControllers.cs | 10 ++++---- Demos/Demos/RayCastingDemo.cs | 8 +++---- .../IntertreeThreadingTests.cs | 2 +- Demos/SpecializedTests/TreeTest.cs | 4 ++-- Demos/SpecializedTests/VolumeQueryTests.cs | 8 +++---- 20 files changed, 104 insertions(+), 78 deletions(-) diff --git a/BepuPhysics/BatchCompressor.cs b/BepuPhysics/BatchCompressor.cs index 2f9d01a06..db1e09a2c 100644 --- a/BepuPhysics/BatchCompressor.cs +++ b/BepuPhysics/BatchCompressor.cs @@ -13,7 +13,7 @@ namespace BepuPhysics /// /// Handles the movement of constraints from higher indexed batches into lower indexed batches to avoid accumulating a bunch of unnecessary ConstraintBatches. /// - public class BatchCompressor + public unsafe class BatchCompressor { //We want to keep removes as fast as possible. So, when removing constraints, no attempt is made to pull constraints from higher constraint batches into the revealed slot. //Over time, this could result in lots of extra constraint batches that ruin multithreading performance. @@ -97,7 +97,7 @@ struct AnalysisRegion QuickList analysisJobs; - Action analysisWorkerDelegate; + ThreadDispatcherWorker analysisWorkerDelegate; public BatchCompressor(Solver solver, Bodies bodies, float targetCandidateFraction = 0.005f, float maximumCompressionFraction = 0.0005f) { this.Solver = solver; @@ -109,7 +109,7 @@ public BatchCompressor(Solver solver, Bodies bodies, float targetCandidateFracti - void AnalysisWorker(int workerIndex) + unsafe void AnalysisWorker(int workerIndex, void* context) { int jobIndex; while ((jobIndex = Interlocked.Increment(ref analysisJobIndex)) < analysisJobs.Count) diff --git a/BepuPhysics/CollisionDetection/BroadPhase.cs b/BepuPhysics/CollisionDetection/BroadPhase.cs index 594c59d72..fa2454784 100644 --- a/BepuPhysics/CollisionDetection/BroadPhase.cs +++ b/BepuPhysics/CollisionDetection/BroadPhase.cs @@ -40,7 +40,7 @@ public unsafe partial class BroadPhase : IDisposable //TODO: static trees do not need to do nearly as much work as the active; this will change in the future. Tree.RefitAndRefineMultithreadedContext staticRefineContext; - Action executeRefitAndMarkAction, executeRefineAction; + ThreadDispatcherWorker executeRefitAndMarkAction, executeRefineAction; public BroadPhase(BufferPool pool, int initialActiveLeafCapacity = 4096, int initialStaticLeafCapacity = 8192) { @@ -153,7 +153,7 @@ public void UpdateStaticBounds(int broadPhaseIndex, Vector3 min, Vector3 max) int frameIndex; int remainingJobCount; IThreadDispatcher threadDispatcher; - void ExecuteRefitAndMark(int workerIndex) + void ExecuteRefitAndMark(int workerIndex, void* context) { var threadPool = threadDispatcher.GetThreadMemoryPool(workerIndex); while (true) @@ -174,7 +174,7 @@ void ExecuteRefitAndMark(int workerIndex) } } - void ExecuteRefine(int workerIndex) + void ExecuteRefine(int workerIndex, void* context) { var threadPool = threadDispatcher.GetThreadMemoryPool(workerIndex); var maximumSubtrees = Math.Max(activeRefineContext.MaximumSubtrees, staticRefineContext.MaximumSubtrees); @@ -212,7 +212,7 @@ public void Update(IThreadDispatcher threadDispatcher = null) activeRefineContext.CreateRefitAndMarkJobs(ref ActiveTree, Pool, threadDispatcher); staticRefineContext.CreateRefitAndMarkJobs(ref StaticTree, Pool, threadDispatcher); remainingJobCount = activeRefineContext.RefitNodes.Count + staticRefineContext.RefitNodes.Count; - threadDispatcher.DispatchWorkers(executeRefitAndMarkAction, remainingJobCount); + threadDispatcher.DispatchWorkers(executeRefitAndMarkAction, null, remainingJobCount); activeRefineContext.CreateRefinementJobs(Pool, frameIndex, 1f); //TODO: for now, the inactive/static tree is simply updated like another active tree. This is enormously inefficient compared to the ideal- //by nature, static and inactive objects do not move every frame! @@ -221,7 +221,7 @@ public void Update(IThreadDispatcher threadDispatcher = null) //Since the jobs are large, reducing the refinement aggressiveness doesn't change much here. staticRefineContext.CreateRefinementJobs(Pool, frameIndex, 1f); remainingJobCount = activeRefineContext.RefinementTargets.Count + staticRefineContext.RefinementTargets.Count; - threadDispatcher.DispatchWorkers(executeRefineAction, remainingJobCount); + threadDispatcher.DispatchWorkers(executeRefineAction, null, remainingJobCount); activeRefineContext.CleanUpForRefitAndRefine(Pool); staticRefineContext.CleanUpForRefitAndRefine(Pool); this.threadDispatcher = null; diff --git a/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs b/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs index e03c8067f..487705c4a 100644 --- a/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs +++ b/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs @@ -16,7 +16,7 @@ public abstract class CollidableOverlapFinder //The overlap finder requires type knowledge about the narrow phase that the broad phase lacks. Don't really want to infect the broad phase with a bunch of narrow phase dependent //generic parameters, so instead we just explicitly create a type-aware overlap finder to help the broad phase. - public class CollidableOverlapFinder : CollidableOverlapFinder where TCallbacks : struct, INarrowPhaseCallbacks + public unsafe class CollidableOverlapFinder : CollidableOverlapFinder where TCallbacks : struct, INarrowPhaseCallbacks { struct SelfOverlapHandler : IOverlapHandler { @@ -62,7 +62,7 @@ public void Handle(int indexA, int indexB) BroadPhase broadPhase; SelfOverlapHandler[] selfHandlers; IntertreeOverlapHandler[] intertreeHandlers; - Action workerAction; + ThreadDispatcherWorker workerAction; int nextJobIndex; public CollidableOverlapFinder(NarrowPhase narrowPhase, BroadPhase broadPhase) { @@ -73,7 +73,7 @@ public CollidableOverlapFinder(NarrowPhase narrowPhase, BroadPhase b workerAction = Worker; } - void Worker(int workerIndex) + void Worker(int workerIndex, void* context) { Debug.Assert(workerIndex >= 0 && workerIndex < intertreeHandlers.Length && workerIndex < selfHandlers.Length); diff --git a/BepuPhysics/CollisionDetection/NarrowPhase.cs b/BepuPhysics/CollisionDetection/NarrowPhase.cs index b95fa4250..101b0c899 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhase.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhase.cs @@ -81,7 +81,7 @@ public struct NarrowPhaseFlushJob public int Index; } - public abstract class NarrowPhase + public unsafe abstract class NarrowPhase { public Simulation Simulation; public BufferPool Pool; @@ -198,8 +198,8 @@ public void Prepare(float dt, IThreadDispatcher threadDispatcher = null) int flushJobIndex; QuickList flushJobs; IThreadDispatcher threadDispatcher; - Action flushWorkerLoop; - void FlushWorkerLoop(int workerIndex) + ThreadDispatcherWorker flushWorkerLoop; + void FlushWorkerLoop(int workerIndex, void* context) { int jobIndex; while ((jobIndex = Interlocked.Increment(ref flushJobIndex)) < flushJobs.Count) @@ -330,7 +330,7 @@ public static void SortCollidableReferencesForPair(CollidableReference a, Collid /// Turns broad phase overlaps into contact manifolds and uses them to manage constraints in the solver. /// /// Type of the callbacks to use. - public partial class NarrowPhase : NarrowPhase where TCallbacks : struct, INarrowPhaseCallbacks + public unsafe partial class NarrowPhase : NarrowPhase where TCallbacks : struct, INarrowPhaseCallbacks { public TCallbacks Callbacks; public struct OverlapWorker diff --git a/BepuPhysics/CollisionDetection/NarrowPhasePreflush.cs b/BepuPhysics/CollisionDetection/NarrowPhasePreflush.cs index 4e59ff186..08f31e821 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhasePreflush.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhasePreflush.cs @@ -91,7 +91,7 @@ internal struct PreflushJob public int JobIndex; } - public partial class NarrowPhase + public unsafe partial class NarrowPhase { public struct SortConstraintTarget { @@ -109,8 +109,8 @@ public struct SortConstraintTarget int preflushJobIndex; QuickList preflushJobs; - Action preflushWorkerLoop; - void PreflushWorkerLoop(int workerIndex) + ThreadDispatcherWorker preflushWorkerLoop; + void PreflushWorkerLoop(int workerIndex, void* context) { int jobIndex; while ((jobIndex = Interlocked.Increment(ref preflushJobIndex)) < preflushJobs.Count) diff --git a/BepuPhysics/IslandAwakener.cs b/BepuPhysics/IslandAwakener.cs index fec45ae25..339d4c49b 100644 --- a/BepuPhysics/IslandAwakener.cs +++ b/BepuPhysics/IslandAwakener.cs @@ -16,7 +16,7 @@ namespace BepuPhysics /// /// Provides functionality for efficiently waking up sleeping bodies. /// - public class IslandAwakener + public unsafe class IslandAwakener { Solver solver; Statics statics; @@ -135,9 +135,9 @@ public void AwakenSets(ref QuickList setIndices, IThreadDispatcher threadDi int jobIndex; int jobCount; //TODO: once again, we repeat this worker pattern. We've done this, what, seven times? It wouldn't even be that difficult to centralize it. We'll get around to that at some point. - Action phaseOneWorkerDelegate; - Action phaseTwoWorkerDelegate; - internal void PhaseOneWorker(int workerIndex) + ThreadDispatcherWorker phaseOneWorkerDelegate; + ThreadDispatcherWorker phaseTwoWorkerDelegate; + internal void PhaseOneWorker(int workerIndex, void* context) { while (true) { @@ -147,7 +147,7 @@ internal void PhaseOneWorker(int workerIndex) ExecutePhaseOneJob(index); } } - internal void PhaseTwoWorker(int workerIndex) + internal void PhaseTwoWorker(int workerIndex, void* context) { while (true) { diff --git a/BepuPhysics/IslandSleeper.cs b/BepuPhysics/IslandSleeper.cs index 539cba248..54917ae23 100644 --- a/BepuPhysics/IslandSleeper.cs +++ b/BepuPhysics/IslandSleeper.cs @@ -12,7 +12,7 @@ namespace BepuPhysics { - public class IslandSleeper + public unsafe class IslandSleeper { IdPool setIdPool; Bodies bodies; @@ -324,16 +324,16 @@ void FindIslands(int workerIndex, BufferPool threadPool) FindIslands(workerIndex, threadPool, ref predicate); } } - Action findIslandsDelegate; + ThreadDispatcherWorker findIslandsDelegate; bool forceSleep; - void FindIslands(int workerIndex) + void FindIslands(int workerIndex, void* context) { //The only reason we separate this out is to make it easier for the main pool to be passed in if there is only a single thread. FindIslands(workerIndex, threadDispatcher.GetThreadMemoryPool(workerIndex)); } - Action gatherDelegate; - unsafe void Gather(int workerIndex) + ThreadDispatcherWorker gatherDelegate; + unsafe void Gather(int workerIndex, void* context) { while (true) { @@ -389,9 +389,9 @@ unsafe void Gather(int workerIndex) } int typeBatchConstraintRemovalJobCount; - Action typeBatchConstraintRemovalDelegate; + ThreadDispatcherWorker typeBatchConstraintRemovalDelegate; - void TypeBatchConstraintRemoval(int workerIndex) + void TypeBatchConstraintRemoval(int workerIndex, void* context) { while (true) { @@ -507,8 +507,8 @@ void ExecuteRemoval(ref RemovalJob job) break; } } - Action executeRemovalWorkDelegate; - void ExecuteRemovalWork(int workerIndex) + ThreadDispatcherWorker executeRemovalWorkDelegate; + void ExecuteRemovalWork(int workerIndex, void* context) { while (true) { @@ -797,7 +797,7 @@ void DisposeWorkerTraversalResults() } else { - Gather(0); + Gather(0, null); } DisposeWorkerTraversalResults(); gatheringJobs.Dispose(pool); diff --git a/BepuPhysics/PoseIntegrator.cs b/BepuPhysics/PoseIntegrator.cs index 3e9139d1b..b9e7abd92 100644 --- a/BepuPhysics/PoseIntegrator.cs +++ b/BepuPhysics/PoseIntegrator.cs @@ -265,7 +265,7 @@ public static unsafe void Integrate(in RigidPose pose, in BodyVelocity velocity, /// /// Handles body integration work that isn't bundled into the solver's execution. Predicts bounding boxes, integrates velocity and poses for unconstrained bodies, and does final post-substepping pose integration for constrained bodies. /// - public class PoseIntegrator : IPoseIntegrator where TCallbacks : IPoseIntegratorCallbacks + public unsafe class PoseIntegrator : IPoseIntegrator where TCallbacks : IPoseIntegratorCallbacks { Bodies bodies; Shapes shapes; @@ -273,7 +273,7 @@ public class PoseIntegrator : IPoseIntegrator where TCallbacks : IPo public TCallbacks Callbacks; - Action predictBoundingBoxesWorker; + ThreadDispatcherWorker predictBoundingBoxesWorker; public PoseIntegrator(Bodies bodies, Shapes shapes, BroadPhase broadPhase, TCallbacks callbacks) { this.bodies = bodies; @@ -395,7 +395,7 @@ bool TryGetJob(int maximumJobInterval, out int start, out int exclusiveEnd) return true; } - void PredictBoundingBoxesWorker(int workerIndex) + void PredictBoundingBoxesWorker(int workerIndex, void* context) { var boundingBoxUpdater = new BoundingBoxBatcher(bodies, shapes, broadPhase, threadDispatcher.GetThreadMemoryPool(workerIndex), cachedDt); var bundleCount = BundleIndexing.GetBundleCount(bodies.ActiveSet.Count); @@ -690,9 +690,9 @@ unsafe void IntegrateBundlesAfterSubstepping(ref IndexSet mergedConstrainedBodyH } } - Action integrateAfterSubsteppingWorker; + ThreadDispatcherWorker integrateAfterSubsteppingWorker; IndexSet constrainedBodies; - private void IntegrateAfterSubsteppingWorker(int workerIndex) + private void IntegrateAfterSubsteppingWorker(int workerIndex, void* context) { var bundleCount = BundleIndexing.GetBundleCount(bodies.ActiveSet.Count); var substepDt = cachedDt / substepCount; diff --git a/BepuPhysics/Solver_Solve.cs b/BepuPhysics/Solver_Solve.cs index 947fd4f06..8fc664102 100644 --- a/BepuPhysics/Solver_Solve.cs +++ b/BepuPhysics/Solver_Solve.cs @@ -115,7 +115,7 @@ protected struct SubstepMultithreadingContext /// Handles integration-aware substepped solving. /// /// Type of integration callbacks being used during the substepped solve. - public class Solver : Solver where TIntegrationCallbacks : struct, IPoseIntegratorCallbacks + public unsafe class Solver : Solver where TIntegrationCallbacks : struct, IPoseIntegratorCallbacks { /* There are two significant sources of complexity here: @@ -458,8 +458,8 @@ protected static int GetUniformlyDistributedStart(int workerIndex, int blockCoun return offset + blocksPerWorker * workerIndex + Math.Min(remainder, workerIndex); } - Action solveWorker; - void SolveWorker(int workerIndex) + ThreadDispatcherWorker solveWorker; + void SolveWorker(int workerIndex, void* context) { //The solver has two codepaths: one thread, acting as an orchestrator, and the others, just waiting to be used. //There is no requirement that a worker thread above index 0 actually runs at all for a given dispatch. @@ -754,7 +754,7 @@ int GetVelocityIterationCountForSubstepIndex(int substepIndex) return VelocityIterationCount; } //Buffer> debugStageWorkBlocksCompleted; - protected void ExecuteMultithreaded(float dt, IThreadDispatcher threadDispatcher, Action workDelegate) + protected void ExecuteMultithreaded(float dt, IThreadDispatcher threadDispatcher, ThreadDispatcherWorker workDelegate) { var workerCount = substepContext.WorkerCount = threadDispatcher.ThreadCount; substepContext.Dt = dt; @@ -1046,7 +1046,7 @@ unsafe bool ComputeIntegrationResponsibilitiesForConstraintRegion return mergedFlagBundles != 0; } - void ConstraintIntegrationResponsibilitiesWorker(int workerIndex) + void ConstraintIntegrationResponsibilitiesWorker(int workerIndex, void* context) { int jobIndex; while ((jobIndex = Interlocked.Increment(ref nextConstraintIntegrationResponsibilityJobIndex) - 1) < integrationResponsibilityPrepassJobs.Count) @@ -1069,7 +1069,7 @@ void ConstraintIntegrationResponsibilitiesWorker(int workerIndex) /// Type batches with no integration responsibilities can use a codepath with no integration checks at all. /// Buffer> coarseBatchIntegrationResponsibilities; - Action constraintIntegrationResponsibilitiesWorker; + ThreadDispatcherWorker constraintIntegrationResponsibilitiesWorker; IndexSet mergedConstrainedBodyHandles; public override unsafe IndexSet PrepareConstraintIntegrationResponsibilities(IThreadDispatcher threadDispatcher = null) diff --git a/BepuPhysics/Trees/Tree_IntertreeQueriesMT.cs b/BepuPhysics/Trees/Tree_IntertreeQueriesMT.cs index de22b26ef..0de1c95a5 100644 --- a/BepuPhysics/Trees/Tree_IntertreeQueriesMT.cs +++ b/BepuPhysics/Trees/Tree_IntertreeQueriesMT.cs @@ -153,7 +153,8 @@ public unsafe void ExecuteJob(int jobIndex, int workerIndex) /// Executes a single worker of the multithreaded self test. /// /// Index of the worker executing this set of tests. - public unsafe void PairTest(int workerIndex) + /// Context of the dispatch, if any. + public unsafe void PairTest(int workerIndex, void* context) { Debug.Assert(workerIndex >= 0 && workerIndex < OverlapHandlers.Length); int nextNodePairIndex; diff --git a/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs b/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs index 34089d60b..e4529c212 100644 --- a/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs +++ b/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs @@ -15,7 +15,7 @@ partial struct Tree /// /// Caches input and output for the multithreaded execution of a tree's refit and refinement operations. /// - public class RefitAndRefineMultithreadedContext + public unsafe class RefitAndRefineMultithreadedContext { Tree Tree; @@ -25,12 +25,12 @@ public class RefitAndRefineMultithreadedContext int RefinementLeafCountThreshold; Buffer> RefinementCandidates; - Action RefitAndMarkAction; + ThreadDispatcherWorker RefitAndMarkAction; int RefineIndex; public QuickList RefinementTargets; public int MaximumSubtrees; - Action RefineAction; + ThreadDispatcherWorker RefineAction; IThreadDispatcher threadDispatcher; @@ -310,7 +310,7 @@ public unsafe void ExecuteRefitAndMarkJob(BufferPool threadPool, int workerIndex } } } - public unsafe void RefitAndMarkForWorker(int workerIndex) + public unsafe void RefitAndMarkForWorker(int workerIndex, void* context) { if (RefitNodes.Count == 0) return; @@ -334,7 +334,7 @@ public unsafe void ExecuteRefineJob(ref QuickList subtreeReferences, ref Qu treeletInternalNodes.Count = 0; } - public unsafe void RefineForWorker(int workerIndex) + public unsafe void RefineForWorker(int workerIndex, void* context) { if (RefinementTargets.Count == 0) return; diff --git a/BepuPhysics/Trees/Tree_SelfQueriesMT.cs b/BepuPhysics/Trees/Tree_SelfQueriesMT.cs index d08ead631..b767ee256 100644 --- a/BepuPhysics/Trees/Tree_SelfQueriesMT.cs +++ b/BepuPhysics/Trees/Tree_SelfQueriesMT.cs @@ -141,7 +141,7 @@ public unsafe void ExecuteJob(int jobIndex, int workerIndex) /// Executes a single worker of the multithreaded self test. /// /// Index of the worker executing this set of tests. - public unsafe void PairTest(int workerIndex) + public unsafe void PairTest(int workerIndex, void* context) { Debug.Assert(workerIndex >= 0 && workerIndex < OverlapHandlers.Length); int nextNodePairIndex; diff --git a/BepuUtilities/IThreadDispatcher.cs b/BepuUtilities/IThreadDispatcher.cs index eb8305d02..96e9f04d5 100644 --- a/BepuUtilities/IThreadDispatcher.cs +++ b/BepuUtilities/IThreadDispatcher.cs @@ -1,8 +1,17 @@ using BepuUtilities.Memory; using System; +using System.Runtime.CompilerServices; namespace BepuUtilities { + /// + /// Function to be invoked on a worker thread in an . Provides the unmanaged context of the dispatch. + /// + /// Index of the worker in the dispatcher executing this function. + /// Pointer to the context of this dispatch, if any. + /// + public unsafe delegate void ThreadDispatcherWorker(int workerIndex, void* context); + /// /// Provides multithreading dispatch primitives, a thread count, and per thread resource pools for the simulation to use. /// @@ -14,7 +23,7 @@ namespace BepuUtilities /// This is important when a user wants to share some other thread pool, but doesn't have the time to guarantee extremely high performance and high quality /// load balancing. Instead of worrying about that, they can just wrap whatever implementation they happen to have and it'll probably work fine. /// - public interface IThreadDispatcher + public unsafe interface IThreadDispatcher { /// /// Gets the number of workers available in the thread dispatcher. @@ -28,8 +37,19 @@ public interface IThreadDispatcher /// Dispatches all the available workers. /// /// Delegate to be invoked on every worker. + /// Pointer to the context to passed to workers, if any. + /// Maximum number of workers to dispatch. + void DispatchWorkers(ThreadDispatcherWorker workerBody, void* context, int maximumWorkerCount = int.MaxValue); + + /// + /// Dispatches all the available workers with a null context. + /// + /// Delegate to be invoked on every worker. /// Maximum number of workers to dispatch. - void DispatchWorkers(Action workerBody, int maximumWorkerCount = int.MaxValue); + void DispatchWorkers(ThreadDispatcherWorker workerBody, int maximumWorkerCount = int.MaxValue) + { + DispatchWorkers(workerBody, null, maximumWorkerCount); + } /// /// Gets the memory pool associated with a given worker index. It is guaranteed that no other workers will share the same pool for the duration of the worker's execution. diff --git a/BepuUtilities/ThreadDispatcher.cs b/BepuUtilities/ThreadDispatcher.cs index c0a6ad652..41253b1ce 100644 --- a/BepuUtilities/ThreadDispatcher.cs +++ b/BepuUtilities/ThreadDispatcher.cs @@ -1,5 +1,6 @@ using System; using System.Diagnostics; +using System.Runtime.CompilerServices; using System.Threading; using BepuUtilities.Memory; @@ -8,7 +9,7 @@ namespace BepuUtilities /// /// Provides a implementation. Not reentrant. /// - public class ThreadDispatcher : IThreadDispatcher, IDisposable + public unsafe class ThreadDispatcher : IThreadDispatcher, IDisposable { int threadCount; /// @@ -52,7 +53,7 @@ public ThreadDispatcher(int threadCount, int threadPoolBlockAllocationSize = 163 void DispatchThread(int workerIndex) { Debug.Assert(workerBody != null); - workerBody(workerIndex); + workerBody(workerIndex, context); if (Interlocked.Decrement(ref remainingWorkerCounter) == -1) { @@ -60,7 +61,8 @@ void DispatchThread(int workerIndex) } } - volatile Action workerBody; + volatile ThreadDispatcherWorker workerBody; + volatile void* context; int remainingWorkerCounter; void WorkerLoop(object untypedSignal) @@ -88,21 +90,23 @@ void SignalThreads(int maximumWorkerCount) } } - public void DispatchWorkers(Action workerBody, int maximumWorkerCount = int.MaxValue) + public void DispatchWorkers(ThreadDispatcherWorker workerBody, void* context = null, int maximumWorkerCount = int.MaxValue) { if (maximumWorkerCount > 1) { Debug.Assert(this.workerBody == null); this.workerBody = workerBody; + this.context = context; SignalThreads(maximumWorkerCount); //Calling thread does work. No reason to spin up another worker and block this one! DispatchThread(0); finished.WaitOne(); this.workerBody = null; + this.context = null; } else if (maximumWorkerCount == 1) { - workerBody(0); + workerBody(0, context); } } diff --git a/DemoRenderer/ParallelLooper.cs b/DemoRenderer/ParallelLooper.cs index 8b5743e22..b1a279b32 100644 --- a/DemoRenderer/ParallelLooper.cs +++ b/DemoRenderer/ParallelLooper.cs @@ -1,6 +1,7 @@ using BepuUtilities; using System; using System.Collections.Generic; +using System.Runtime.CompilerServices; using System.Text; using System.Threading; @@ -24,9 +25,9 @@ namespace DemoRenderer /// /// This helps avoid some unnecessary allocations associated with the TPL implementation. While a little garbage from the renderer in the demos isn't exactly a catastrophe, /// having zero allocations under normal execution makes it easier to notice when the physics simulation itself is allocating inappropriately. - public class ParallelLooper + public unsafe class ParallelLooper { - Action dispatcherWorker; + ThreadDispatcherWorker dispatcherWorker; /// /// Gets or sets the dispatcher used by the looper. @@ -38,7 +39,7 @@ public ParallelLooper() dispatcherWorker = Worker; } - void Worker(int workerIndex) + void Worker(int workerIndex, void* context) { while (true) { @@ -78,7 +79,7 @@ public void For(int start, int exclusiveEnd, LooperAction workAction, LooperWork this.end = exclusiveEnd; this.iteration = workAction; this.workerDone = workerDone; - Dispatcher.DispatchWorkers(dispatcherWorker, exclusiveEnd - start); + Dispatcher.DispatchWorkers(dispatcherWorker, null, exclusiveEnd - start); this.iteration = null; this.workerDone = workerDone; } diff --git a/Demos/Demos/Characters/CharacterControllers.cs b/Demos/Demos/Characters/CharacterControllers.cs index ad8de557a..b99e5d334 100644 --- a/Demos/Demos/Characters/CharacterControllers.cs +++ b/Demos/Demos/Characters/CharacterControllers.cs @@ -82,7 +82,7 @@ public struct CharacterController /// /// System that manages all the characters in a simulation. Responsible for updating movement constraints based on character goals and contact states. /// - public class CharacterControllers : IDisposable + public unsafe class CharacterControllers : IDisposable { /// /// Gets the simulation to which this set of chracters belongs. @@ -433,8 +433,8 @@ unsafe void ExpandBoundingBoxes(int start, int count) } int boundingBoxExpansionJobIndex; - Action expandBoundingBoxesWorker; - void ExpandBoundingBoxesWorker(int workerIndex) + ThreadDispatcherWorker expandBoundingBoxesWorker; + unsafe void ExpandBoundingBoxesWorker(int workerIndex, void* context) { while (true) { @@ -735,8 +735,8 @@ struct AnalyzeContactsJob int analysisJobIndex; int analysisJobCount; Buffer jobs; - Action analyzeContactsWorker; - void AnalyzeContactsWorker(int workerIndex) + ThreadDispatcherWorker analyzeContactsWorker; + unsafe void AnalyzeContactsWorker(int workerIndex, void* context) { int jobIndex; while ((jobIndex = Interlocked.Increment(ref analysisJobIndex)) < analysisJobCount) diff --git a/Demos/Demos/RayCastingDemo.cs b/Demos/Demos/RayCastingDemo.cs index fcc87e905..8adcd3bfc 100644 --- a/Demos/Demos/RayCastingDemo.cs +++ b/Demos/Demos/RayCastingDemo.cs @@ -242,7 +242,7 @@ struct RayHit public CollidableReference Collidable; public bool Hit; } - class IntersectionAlgorithm + unsafe class IntersectionAlgorithm { public string Name; public int IntersectionCount; @@ -250,7 +250,7 @@ class IntersectionAlgorithm public TimingsRingBuffer Timings; Func worker; - Action internalWorker; + ThreadDispatcherWorker internalWorker; public int JobIndex; public IntersectionAlgorithm(string name, Func worker, @@ -263,7 +263,7 @@ public IntersectionAlgorithm(string name, Func pool.Take(largestRayCount, out Results); } - void ExecuteWorker(int workerIndex) + unsafe void ExecuteWorker(int workerIndex, void* context) { var intersectionCount = worker(workerIndex, this); Interlocked.Add(ref IntersectionCount, intersectionCount); @@ -286,7 +286,7 @@ public void Execute(ref QuickList rays, IThreadDispatcher dispatcher) } else { - internalWorker(0); + internalWorker(0, null); } var stop = Stopwatch.GetTimestamp(); Timings.Add((stop - start) / (double)Stopwatch.Frequency); diff --git a/Demos/SpecializedTests/IntertreeThreadingTests.cs b/Demos/SpecializedTests/IntertreeThreadingTests.cs index 43c33cf0e..0f999de04 100644 --- a/Demos/SpecializedTests/IntertreeThreadingTests.cs +++ b/Demos/SpecializedTests/IntertreeThreadingTests.cs @@ -51,7 +51,7 @@ static void SortPairs(List<(int a, int b)> pairs) pairs.Sort(comparison); } - static void TestTrees(BufferPool pool, IThreadDispatcher threadDispatcher, Random random) + unsafe static void TestTrees(BufferPool pool, IThreadDispatcher threadDispatcher, Random random) { var treeA = new Tree(pool, 1); var treeB = new Tree(pool, 1); diff --git a/Demos/SpecializedTests/TreeTest.cs b/Demos/SpecializedTests/TreeTest.cs index 10e78b836..ed1675d29 100644 --- a/Demos/SpecializedTests/TreeTest.cs +++ b/Demos/SpecializedTests/TreeTest.cs @@ -13,7 +13,7 @@ namespace Demos.SpecializedTests { - public static class TreeTest + public unsafe static class TreeTest { public static void Test() { @@ -72,7 +72,7 @@ public static void Test() var refineContext = new Tree.RefitAndRefineMultithreadedContext(); var selfTestContext = new Tree.MultithreadedSelfTest(pool); var overlapHandlers = new OverlapHandler[threadDispatcher.ThreadCount]; - Action pairTestAction = selfTestContext.PairTest; + ThreadDispatcherWorker pairTestAction = selfTestContext.PairTest; var removedLeafHandles = new QuickList(leafCount, pool); for (int i = 0; i < iterations; ++i) { diff --git a/Demos/SpecializedTests/VolumeQueryTests.cs b/Demos/SpecializedTests/VolumeQueryTests.cs index 4e9c65199..451a2b61d 100644 --- a/Demos/SpecializedTests/VolumeQueryTests.cs +++ b/Demos/SpecializedTests/VolumeQueryTests.cs @@ -130,14 +130,14 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) QuickList queryBoxes; - class BoxQueryAlgorithm + unsafe class BoxQueryAlgorithm { public string Name; public int IntersectionCount; public TimingsRingBuffer Timings; Func worker; - Action internalWorker; + ThreadDispatcherWorker internalWorker; public int JobIndex; public BoxQueryAlgorithm(string name, BufferPool pool, Func worker, int timingSampleCount = 16) @@ -148,7 +148,7 @@ public BoxQueryAlgorithm(string name, BufferPool pool, Func boxes, IThreadDispatcher dispatch } else { - internalWorker(0); + internalWorker(0, null); } var stop = Stopwatch.GetTimestamp(); Timings.Add((stop - start) / (double)Stopwatch.Frequency); From 1d39fcf9fdcb89b82cbcc2be917f0376e396c9b4 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 27 Oct 2022 22:45:57 -0500 Subject: [PATCH 627/947] Preparing for inline multithreaded binned building. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 172 ++++++++++++++---------- 1 file changed, 102 insertions(+), 70 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 9071b4d89..a60e1954e 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -109,7 +109,7 @@ struct LeafCountBuffer : ILeafCountBuffer public LeafCountBuffer Slice(int startIndex, int count) => new() { LeafCounts = LeafCounts.Slice(startIndex, count) }; } - struct Bins + struct Context where TThreading : unmanaged { public Buffer BinBoundingBoxes; public Buffer BinBoundingBoxesScan; @@ -118,16 +118,18 @@ struct Bins public int MaximumBinCount; public float LeafToBinMultiplier; public int MicrosweepThreshold; + public TThreading Threading; } struct BoundsComparerX : IComparerRef { public int Compare(ref BoundingBox4 a, ref BoundingBox4 b) => (a.Min.X + a.Max.X) > (b.Min.X + b.Max.X) ? -1 : 1; } struct BoundsComparerY : IComparerRef { public int Compare(ref BoundingBox4 a, ref BoundingBox4 b) => (a.Min.Y + a.Max.Y) > (b.Min.Y + b.Max.Y) ? -1 : 1; } struct BoundsComparerZ : IComparerRef { public int Compare(ref BoundingBox4 a, ref BoundingBox4 b) => (a.Min.Z + a.Max.Z) > (b.Min.Z + b.Max.Z) ? -1 : 1; } - static unsafe void MicroSweepForBinnedBuilder( + static unsafe void MicroSweepForBinnedBuilder( Vector4 centroidMin, Vector4 centroidMax, Buffer indices, TLeafCounts leafCounts, ref TLeaves leaves, - Buffer boundingBoxes, Buffer nodes, Buffer metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, Bins bins) + Buffer boundingBoxes, Buffer nodes, Buffer metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, Context* context) where TLeafCounts : unmanaged, ILeafCountBuffer where TLeaves : unmanaged + where TThreading : unmanaged { //This is a very small scale sweep build. var subtreeCount = indices.Length; @@ -143,9 +145,9 @@ static unsafe void MicroSweepForBinnedBuilder( //Repurpose the bins memory so we don't need to allocate any extra. The bins aren't in use right now anyway. int paddedKeyCount = Vector256.IsHardwareAccelerated ? ((subtreeCount + 7) / 8) * 8 : ((subtreeCount + 3) / 4) * 4; - Debug.Assert(Unsafe.SizeOf() * bins.BinBoundingBoxes.Length >= (paddedKeyCount * 2 + subtreeCount) * Unsafe.SizeOf(), + Debug.Assert(Unsafe.SizeOf() * context->BinBoundingBoxes.Length >= (paddedKeyCount * 2 + subtreeCount) * Unsafe.SizeOf(), "The bins should preallocate enough space to handle the needs of microsweeps. They reuse the same allocations."); - var keys = new Buffer(bins.BinBoundingBoxes.Memory, paddedKeyCount); + var keys = new Buffer(context->BinBoundingBoxes.Memory, paddedKeyCount); var targetIndices = new Buffer(keys.Memory + paddedKeyCount, paddedKeyCount); //Compute the axis centroids up front to avoid having to recompute them during a sort. @@ -186,8 +188,8 @@ static unsafe void MicroSweepForBinnedBuilder( { //There aren't any leaf counts that we need to copy; they're all just 1 anyway. Debug.Assert(typeof(TLeafCounts) == typeof(UnitLeafCount)); - var indicesCache = new Buffer(bins.BinBoundingBoxes.Memory, subtreeCount); - var boundingBoxCache = bins.BinBoundingBoxesScan; + var indicesCache = new Buffer(context->BinBoundingBoxes.Memory, subtreeCount); + var boundingBoxCache = context->BinBoundingBoxesScan; boundingBoxes.CopyTo(0, boundingBoxCache, 0, subtreeCount); indices.CopyTo(0, indicesCache, 0, subtreeCount); for (int i = 0; i < subtreeCount; ++i) @@ -200,9 +202,9 @@ static unsafe void MicroSweepForBinnedBuilder( else { //There are actual leaf counts we need to worry about on top of the rest! - var indicesCache = new Buffer(bins.BinBoundingBoxes.Memory, subtreeCount); + var indicesCache = new Buffer(context->BinBoundingBoxes.Memory, subtreeCount); var leafCountCache = new Buffer(targetIndices.Memory + subtreeCount, subtreeCount); - var boundingBoxCache = bins.BinBoundingBoxesScan; + var boundingBoxCache = context->BinBoundingBoxesScan; boundingBoxes.CopyTo(0, boundingBoxCache, 0, subtreeCount); indices.CopyTo(0, indicesCache, 0, subtreeCount); var leafCountBuffer = Unsafe.As(ref leafCounts).LeafCounts; @@ -240,7 +242,7 @@ static unsafe void MicroSweepForBinnedBuilder( else { //There are leaf counts that we need to sort alongside the rest. This is a pretty low value codepath, so we'll just create a targetIndices buffer. - var targetIndices = new Buffer(bins.BinBoundingBoxes.Memory, subtreeCount); + var targetIndices = new Buffer(context->BinBoundingBoxes.Memory, subtreeCount); for (int i = 0; i < subtreeCount; ++i) { targetIndices[i] = i; @@ -275,16 +277,16 @@ static unsafe void MicroSweepForBinnedBuilder( } } - Debug.Assert(subtreeCount <= bins.MaximumBinCount || subtreeCount < bins.MicrosweepThreshold, "We're reusing the bin resources under the assumption that this is only ever called when there are less leaves than maximum bins."); + Debug.Assert(subtreeCount <= context->MaximumBinCount || subtreeCount < context->MicrosweepThreshold, "We're reusing the bin resources under the assumption that this is only ever called when there are less leaves than maximum bins."); //Identify the split index by examining the SAH of very split option. //Premerge from left to right so we have a sorta-summed area table to cheaply look up all possible child A bounds as we scan. - bins.BinBoundingBoxesScan[0] = boundingBoxes[0]; + context->BinBoundingBoxesScan[0] = boundingBoxes[0]; int totalLeafCount = typeof(TLeafCounts) == typeof(LeafCountBuffer) ? leafCounts[0] : subtreeCount; for (int i = 1; i < subtreeCount; ++i) { var previousIndex = i - 1; - ref var previousScanBounds = ref bins.BinBoundingBoxesScan[previousIndex]; - ref var scanBounds = ref bins.BinBoundingBoxesScan[i]; + ref var previousScanBounds = ref context->BinBoundingBoxesScan[previousIndex]; + ref var scanBounds = ref context->BinBoundingBoxesScan[i]; ref var bounds = ref boundingBoxes[i]; scanBounds.Min = Vector4.Min(bounds.Min, previousScanBounds.Min); scanBounds.Max = Vector4.Max(bounds.Max, previousScanBounds.Max); @@ -304,7 +306,7 @@ static unsafe void MicroSweepForBinnedBuilder( { var previousIndex = splitIndexCandidate - 1; var sahCandidate = - ComputeBoundsMetric(bins.BinBoundingBoxesScan[previousIndex]) * (totalLeafCount - accumulatedLeafCountB) + + ComputeBoundsMetric(context->BinBoundingBoxesScan[previousIndex]) * (totalLeafCount - accumulatedLeafCountB) + ComputeBoundsMetric(accumulatedBoundingBoxB) * accumulatedLeafCountB; if (sahCandidate < bestSAH) { @@ -320,7 +322,7 @@ static unsafe void MicroSweepForBinnedBuilder( accumulatedLeafCountB += leafCounts[previousIndex]; } - var bestBoundsA = bins.BinBoundingBoxesScan[bestSplit - 1]; + var bestBoundsA = context->BinBoundingBoxesScan[bestSplit - 1]; var subtreeCountA = bestSplit; var subtreeCountB = subtreeCount - bestSplit; var bestLeafCountA = typeof(TLeafCounts) == typeof(UnitLeafCount) ? subtreeCountA : totalLeafCount - bestLeafCountB; @@ -338,7 +340,7 @@ static unsafe void MicroSweepForBinnedBuilder( centroidBoundsA.Min = Vector4.Min(centroidBoundsA.Min, bounds.Min); centroidBoundsA.Max = Vector4.Max(centroidBoundsA.Max, bounds.Max); } - MicroSweepForBinnedBuilder(centroidBoundsA.Min, centroidBoundsA.Max, indices.Slice(subtreeCountA), leafCounts.Slice(0, subtreeCountA), ref leaves, aBounds, nodes.Slice(1, subtreeCountA - 1), metanodes.Slice(1, subtreeCountA - 1), aIndex, nodeIndex, 0, bins); + MicroSweepForBinnedBuilder(centroidBoundsA.Min, centroidBoundsA.Max, indices.Slice(subtreeCountA), leafCounts.Slice(0, subtreeCountA), ref leaves, aBounds, nodes.Slice(1, subtreeCountA - 1), metanodes.Slice(1, subtreeCountA - 1), aIndex, nodeIndex, 0, context); } if (subtreeCountB > 1) { @@ -350,7 +352,7 @@ static unsafe void MicroSweepForBinnedBuilder( centroidBoundsB.Min = Vector4.Min(centroidBoundsB.Min, bounds.Min); centroidBoundsB.Max = Vector4.Max(centroidBoundsB.Max, bounds.Max); } - MicroSweepForBinnedBuilder(centroidBoundsB.Min, centroidBoundsB.Max, indices.Slice(subtreeCountA, subtreeCountB), leafCounts.Slice(subtreeCountA, subtreeCountB), ref leaves, bBounds, nodes.Slice(subtreeCountA, subtreeCountB - 1), metanodes.Slice(subtreeCountA, subtreeCountB - 1), bIndex, nodeIndex, 1, bins); + MicroSweepForBinnedBuilder(centroidBoundsB.Min, centroidBoundsB.Max, indices.Slice(subtreeCountA, subtreeCountB), leafCounts.Slice(subtreeCountA, subtreeCountB), ref leaves, bBounds, nodes.Slice(subtreeCountA, subtreeCountB - 1), metanodes.Slice(subtreeCountA, subtreeCountB - 1), bIndex, nodeIndex, 1, context); } } @@ -370,9 +372,15 @@ private static unsafe int ComputeBinIndex(Vector4 centroidMin, bool useX, bool u return (int)(useX ? binIndicesForLeafContinuous.X : useY ? binIndicesForLeafContinuous.Y : binIndicesForLeafContinuous.Z); } - static unsafe void BinnedBuilderInternal(Buffer indices, TLeafCounts leafCounts, ref TLeaves leaves, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, - int nodeIndex, int parentNodeIndex, int childIndexInParent, in Bins bins) - where TLeafCounts : unmanaged, ILeafCountBuffer where TLeaves : unmanaged + struct SingleThreaded { } + + const int SubtreesPerThreadForCentroidPrepass = 2048; + const int SubtreesPerThreadForBinning = 512; + const int SubtreesPerThreadForNodeJob = 512; + + static unsafe void BinnedBuilderInternal(Buffer indices, TLeafCounts leafCounts, ref TLeaves leaves, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, + int nodeIndex, int parentNodeIndex, int childIndexInParent, Context* context) + where TLeafCounts : unmanaged, ILeafCountBuffer where TLeaves : unmanaged where TThreading : unmanaged { var subtreeCount = indices.Length; if (subtreeCount == 2) @@ -383,13 +391,21 @@ static unsafe void BinnedBuilderInternal(Buffer indic var centroidMin = new Vector4(float.MaxValue); var centroidMax = new Vector4(float.MinValue); - for (int i = 0; i < subtreeCount; ++i) + + if (typeof(TThreading) == typeof(SingleThreaded) || subtreeCount < SubtreesPerThreadForCentroidPrepass) { - ref var box = ref boundingBoxes[i]; - //Note that centroids never bother scaling by 0.5. It's fine as long as we're consistent. - var centroid = box.Min + box.Max; - centroidMin = Vector4.Min(centroidMin, centroid); - centroidMax = Vector4.Max(centroidMax, centroid); + for (int i = 0; i < subtreeCount; ++i) + { + ref var box = ref boundingBoxes[i]; + //Note that centroids never bother scaling by 0.5. It's fine as long as we're consistent. + var centroid = box.Min + box.Max; + centroidMin = Vector4.Min(centroidMin, centroid); + centroidMax = Vector4.Max(centroidMax, centroid); + } + } + else + { + //Use the multithreaded path. } var centroidSpan = centroidMax - centroidMin; var axisIsDegenerate = Vector128.LessThanOrEqual(centroidSpan.AsVector128(), Vector128.Create(1e-12f)); @@ -422,15 +438,16 @@ static unsafe void BinnedBuilderInternal(Buffer indic } BuildNode(boundsA, boundsB, degenerateLeafCountA, degenerateLeafCountB, nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, degenerateSubtreeCountA, degenerateSubtreeCountB, ref leaves, out var aIndex, out var bIndex); if (degenerateSubtreeCountA > 1) - BinnedBuilderInternal(indices.Slice(degenerateSubtreeCountA), leafCounts.Slice(0, degenerateSubtreeCountA), ref leaves, boundingBoxes.Slice(degenerateSubtreeCountA), nodes.Slice(1, degenerateSubtreeCountA - 1), metanodes.Slice(1, degenerateSubtreeCountA - 1), aIndex, nodeIndex, 0, bins); + BinnedBuilderInternal(indices.Slice(degenerateSubtreeCountA), leafCounts.Slice(0, degenerateSubtreeCountA), ref leaves, boundingBoxes.Slice(degenerateSubtreeCountA), nodes.Slice(1, degenerateSubtreeCountA - 1), metanodes.Slice(1, degenerateSubtreeCountA - 1), aIndex, nodeIndex, 0, context); if (degenerateSubtreeCountB > 1) - BinnedBuilderInternal(indices.Slice(degenerateSubtreeCountA, degenerateSubtreeCountB), leafCounts.Slice(degenerateSubtreeCountA, degenerateSubtreeCountB), ref leaves, boundingBoxes.Slice(degenerateSubtreeCountA, degenerateSubtreeCountB), nodes.Slice(degenerateSubtreeCountA, degenerateSubtreeCountB - 1), metanodes.Slice(degenerateSubtreeCountA, degenerateSubtreeCountB - 1), bIndex, nodeIndex, 1, bins); + BinnedBuilderInternal(indices.Slice(degenerateSubtreeCountA, degenerateSubtreeCountB), leafCounts.Slice(degenerateSubtreeCountA, degenerateSubtreeCountB), ref leaves, boundingBoxes.Slice(degenerateSubtreeCountA, degenerateSubtreeCountB), nodes.Slice(degenerateSubtreeCountA, degenerateSubtreeCountB - 1), metanodes.Slice(degenerateSubtreeCountA, degenerateSubtreeCountB - 1), bIndex, nodeIndex, 1, context); return; } - if (subtreeCount <= bins.MicrosweepThreshold) + //Note that we don't bother even trying to internally multithread microsweeps. They *should* be small, and should only show up deeper in the recursion process. + if (subtreeCount <= context->MicrosweepThreshold) { - MicroSweepForBinnedBuilder(centroidMin, centroidMax, indices, leafCounts, ref leaves, boundingBoxes, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, bins); + MicroSweepForBinnedBuilder(centroidMin, centroidMax, indices, leafCounts, ref leaves, boundingBoxes, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, context); return; } @@ -440,8 +457,8 @@ static unsafe void BinnedBuilderInternal(Buffer indic var permuteMask = Vector128.Create(useX ? 0 : useY ? 1 : 2, 0, 0, 0); var axisIndex = useX ? 0 : useY ? 1 : 2; - var binCount = Math.Min(bins.MaximumBinCount, Math.Max((int)(subtreeCount * bins.LeafToBinMultiplier), bins.MinimumBinCount)); - Debug.Assert(bins.BinBoundingBoxes.Length >= binCount); + var binCount = Math.Min(context->MaximumBinCount, Math.Max((int)(subtreeCount * context->LeafToBinMultiplier), context->MinimumBinCount)); + Debug.Assert(context->BinBoundingBoxes.Length >= binCount); var offsetToBinIndex = new Vector4(binCount) / centroidSpan; //Avoid letting NaNs into the offsetToBinIndex scale. @@ -449,41 +466,48 @@ static unsafe void BinnedBuilderInternal(Buffer indic for (int i = 0; i < binCount; ++i) { - ref var boxX = ref bins.BinBoundingBoxes[i]; + ref var boxX = ref context->BinBoundingBoxes[i]; boxX.Min = new Vector4(float.MaxValue); boxX.Max = new Vector4(float.MinValue); - bins.BinLeafCounts[i] = 0; + context->BinLeafCounts[i] = 0; } - //Note that we don't store out any of the indices into per-bin lists here. We only *really* want two final groups for the children, - //and we can easily compute those by performing another scan. It requires recomputing the bin indices, but that's really not much of a concern. var maximumBinIndex = new Vector4(binCount - 1); - for (int i = 0; i < subtreeCount; ++i) + if (typeof(TThreading) == typeof(SingleThreaded) || subtreeCount < SubtreesPerThreadForBinning) { - ref var box = ref boundingBoxes[i]; - var binIndex = ComputeBinIndex(centroidMin, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, box); - ref var xBounds = ref bins.BinBoundingBoxes[binIndex]; - xBounds.Min = Vector4.Min(xBounds.Min, box.Min); - xBounds.Max = Vector4.Max(xBounds.Max, box.Max); - bins.BinLeafCounts[binIndex] += leafCounts[i]; + //Note that we don't store out any of the indices into per-bin lists here. We only *really* want two final groups for the children, + //and we can easily compute those by performing another scan. It requires recomputing the bin indices, but that's really not much of a concern. + for (int i = 0; i < subtreeCount; ++i) + { + ref var box = ref boundingBoxes[i]; + var binIndex = ComputeBinIndex(centroidMin, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, box); + ref var xBounds = ref context->BinBoundingBoxes[binIndex]; + xBounds.Min = Vector4.Min(xBounds.Min, box.Min); + xBounds.Max = Vector4.Max(xBounds.Max, box.Max); + context->BinLeafCounts[binIndex] += leafCounts[i]; + } + } + else + { + //Multithreading binning! } //Identify the split index by examining the SAH of very split option. //Premerge from left to right so we have a sorta-summed area table to cheaply look up all possible child A bounds as we scan. - bins.BinBoundingBoxesScan[0] = bins.BinBoundingBoxes[0]; - int totalLeafCount = typeof(TLeafCounts) == typeof(LeafCountBuffer) ? bins.BinLeafCounts[0] : subtreeCount; + context->BinBoundingBoxesScan[0] = context->BinBoundingBoxes[0]; + int totalLeafCount = typeof(TLeafCounts) == typeof(LeafCountBuffer) ? context->BinLeafCounts[0] : subtreeCount; for (int i = 1; i < binCount; ++i) { var previousIndex = i - 1; - ref var xBounds = ref bins.BinBoundingBoxes[i]; - ref var xScanBounds = ref bins.BinBoundingBoxesScan[i]; - ref var xPreviousScanBounds = ref bins.BinBoundingBoxesScan[previousIndex]; + ref var xBounds = ref context->BinBoundingBoxes[i]; + ref var xScanBounds = ref context->BinBoundingBoxesScan[i]; + ref var xPreviousScanBounds = ref context->BinBoundingBoxesScan[previousIndex]; xScanBounds.Min = Vector4.Min(xBounds.Min, xPreviousScanBounds.Min); xScanBounds.Max = Vector4.Max(xBounds.Max, xPreviousScanBounds.Max); if (typeof(TLeafCounts) == typeof(LeafCountBuffer)) - totalLeafCount += bins.BinLeafCounts[i]; + totalLeafCount += context->BinLeafCounts[i]; } - var leftBoundsX = bins.BinBoundingBoxes[0]; + var leftBoundsX = context->BinBoundingBoxes[0]; Debug.Assert( leftBoundsX.Min.X > float.MinValue && leftBoundsX.Min.Y > float.MinValue && leftBoundsX.Min.Z > float.MinValue, "Bin 0 should have been updated in all cases because it is aligned with the minimum bin, and the centroid span isn't degenerate."); @@ -493,15 +517,15 @@ static unsafe void BinnedBuilderInternal(Buffer indic //The split index is going to end up in child B. var lastBinIndex = binCount - 1; BoundingBox4 accumulatedBoundingBoxB; - accumulatedBoundingBoxB = bins.BinBoundingBoxes[lastBinIndex]; + accumulatedBoundingBoxB = context->BinBoundingBoxes[lastBinIndex]; BoundingBox4 bestBoundingBoxB; - bestBoundingBoxB = bins.BinBoundingBoxes[lastBinIndex]; - int accumulatedLeafCountB = bins.BinLeafCounts[lastBinIndex]; + bestBoundingBoxB = context->BinBoundingBoxes[lastBinIndex]; + int accumulatedLeafCountB = context->BinLeafCounts[lastBinIndex]; int bestLeafCountB = 0; for (int splitIndexCandidate = lastBinIndex; splitIndexCandidate >= 1; --splitIndexCandidate) { var previousIndex = splitIndexCandidate - 1; - var sahCandidate = ComputeBoundsMetric(bins.BinBoundingBoxesScan[previousIndex]) * (totalLeafCount - accumulatedLeafCountB) + ComputeBoundsMetric(accumulatedBoundingBoxB) * accumulatedLeafCountB; + var sahCandidate = ComputeBoundsMetric(context->BinBoundingBoxesScan[previousIndex]) * (totalLeafCount - accumulatedLeafCountB) + ComputeBoundsMetric(accumulatedBoundingBoxB) * accumulatedLeafCountB; if (sahCandidate < bestSAH) { @@ -511,17 +535,17 @@ static unsafe void BinnedBuilderInternal(Buffer indic if (typeof(TLeafCounts) == typeof(LeafCountBuffer)) bestLeafCountB = accumulatedLeafCountB; } - ref var xBounds = ref bins.BinBoundingBoxes[previousIndex]; + ref var xBounds = ref context->BinBoundingBoxes[previousIndex]; accumulatedBoundingBoxB.Min = Vector4.Min(xBounds.Min, accumulatedBoundingBoxB.Min); accumulatedBoundingBoxB.Max = Vector4.Max(xBounds.Max, accumulatedBoundingBoxB.Max); - accumulatedLeafCountB += bins.BinLeafCounts[previousIndex]; + accumulatedLeafCountB += context->BinLeafCounts[previousIndex]; } //Choose the best SAH from all axes and split the indices/bounds into two halves for the children to operate on. var subtreeCountB = 0; var subtreeCountA = 0; var splitIndex = bestSplit; - var bestboundsA = bins.BinBoundingBoxesScan[bestSplit - 1]; + var bestboundsA = context->BinBoundingBoxesScan[bestSplit - 1]; var bestboundsB = bestBoundingBoxB; //Now we have the split index between bins. Go back through and sort the indices and bounds into two halves. while (subtreeCountA + subtreeCountB < subtreeCount) @@ -566,10 +590,17 @@ static unsafe void BinnedBuilderInternal(Buffer indic var leafCountA = typeof(TLeafCounts) == typeof(UnitLeafCount) ? subtreeCountA : totalLeafCount - leafCountB; Debug.Assert(subtreeCountA + subtreeCountB == subtreeCount); BuildNode(bestboundsA, bestboundsB, leafCountA, leafCountB, nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, subtreeCountA, subtreeCountB, ref leaves, out var nodeChildIndexA, out var nodeChildIndexB); + + var shouldPushBOntoMultithreadedQueue = typeof(TThreading) != typeof(SingleThreaded) && subtreeCountA >= SubtreesPerThreadForNodeJob && subtreeCountB >= SubtreesPerThreadForNodeJob; + if (shouldPushBOntoMultithreadedQueue) + { + //Both of the children are large. Push child B onto the multithreaded execution stack so it can run at the same time as child A (potentially). + Debug.Assert(SubtreesPerThreadForNodeJob > 1, "The job threshold for a new node should be large enough that there's no need for a subtreeCountB > 1 test."); + } if (subtreeCountA > 1) - BinnedBuilderInternal(indices.Slice(subtreeCountA), leafCounts.Slice(0, subtreeCountA), ref leaves, boundingBoxes.Slice(subtreeCountA), nodes.Slice(1, subtreeCountA - 1), metanodes.Slice(1, subtreeCountA - 1), nodeChildIndexA, nodeIndex, 0, bins); - if (subtreeCountB > 1) - BinnedBuilderInternal(indices.Slice(subtreeCountA, subtreeCountB), leafCounts.Slice(subtreeCountA, subtreeCountB), ref leaves, boundingBoxes.Slice(subtreeCountA, subtreeCountB), nodes.Slice(subtreeCountA, subtreeCountB - 1), metanodes.Slice(subtreeCountA, subtreeCountB - 1), nodeChildIndexB, nodeIndex, 1, bins); + BinnedBuilderInternal(indices.Slice(subtreeCountA), leafCounts.Slice(0, subtreeCountA), ref leaves, boundingBoxes.Slice(subtreeCountA), nodes.Slice(1, subtreeCountA - 1), metanodes.Slice(1, subtreeCountA - 1), nodeChildIndexA, nodeIndex, 0, context); + if (!shouldPushBOntoMultithreadedQueue && subtreeCountB > 1) + BinnedBuilderInternal(indices.Slice(subtreeCountA, subtreeCountB), leafCounts.Slice(subtreeCountA, subtreeCountB), ref leaves, boundingBoxes.Slice(subtreeCountA, subtreeCountB), nodes.Slice(subtreeCountA, subtreeCountB - 1), metanodes.Slice(subtreeCountA, subtreeCountB - 1), nodeChildIndexB, nodeIndex, 1, context); } public static unsafe void BinnedBuilder(Buffer encodedLeafIndices, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, Buffer leaves, BufferPool pool, @@ -604,22 +635,23 @@ public static unsafe void BinnedBuilder(Buffer encodedLeafIndices, Buffer(binBoundsMemory, allocatedBinCount); - bins.BinBoundingBoxesScan = new Buffer(binBoundsMemory + allocatedBinCount, allocatedBinCount); + Context context; + context.BinBoundingBoxes = new Buffer(binBoundsMemory, allocatedBinCount); + context.BinBoundingBoxesScan = new Buffer(binBoundsMemory + allocatedBinCount, allocatedBinCount); var binLeafCountsMemory = stackalloc int[allocatedBinCount]; - bins.BinLeafCounts = new Buffer(binLeafCountsMemory, allocatedBinCount); + context.BinLeafCounts = new Buffer(binLeafCountsMemory, allocatedBinCount); - bins.MinimumBinCount = minimumBinCount; - bins.MaximumBinCount = maximumBinCount; - bins.LeafToBinMultiplier = leafToBinMultiplier; - bins.MicrosweepThreshold = microsweepThreshold; + context.MinimumBinCount = minimumBinCount; + context.MaximumBinCount = maximumBinCount; + context.LeafToBinMultiplier = leafToBinMultiplier; + context.MicrosweepThreshold = microsweepThreshold; + context.Threading = default; var leafCounts = new UnitLeafCount(); //While we could avoid a recursive implementation, the overhead is low compared to the per-iteration cost. - BinnedBuilderInternal(encodedLeafIndices, leafCounts, ref leaves, boundingBoxes.As(), nodes, metanodes, 0, -1, -1, bins); + BinnedBuilderInternal(encodedLeafIndices, leafCounts, ref leaves, boundingBoxes.As(), nodes, metanodes, 0, -1, -1, &context); } } } From 0991d28cde8709e4d41c9cae990e1fcd076c81b7 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 3 Nov 2022 20:23:01 -0500 Subject: [PATCH 628/947] Fiddling with beginnings of a job queue. --- BepuUtilities/IThreadDispatcher.cs | 8 + BepuUtilities/JobQueue.cs | 268 ++++++++++++++++++ BepuUtilities/Memory/SpanHelper.cs | 6 +- BepuUtilities/ThreadDispatcher.cs | 30 +- .../SpecializedTests/TreeFiddlingTestDemo.cs | 3 +- 5 files changed, 308 insertions(+), 7 deletions(-) create mode 100644 BepuUtilities/JobQueue.cs diff --git a/BepuUtilities/IThreadDispatcher.cs b/BepuUtilities/IThreadDispatcher.cs index 96e9f04d5..5a4b30a8f 100644 --- a/BepuUtilities/IThreadDispatcher.cs +++ b/BepuUtilities/IThreadDispatcher.cs @@ -33,6 +33,14 @@ public unsafe interface IThreadDispatcher /// int ThreadCount { get; } + /// + /// Dispatches all the available workers. + /// + /// Function pointer to be invoked on every worker. Matches the signature of the . + /// Pointer to the context to passed to workers, if any. + /// Maximum number of workers to dispatch. + void DispatchWorkers(delegate* workerBody, void* context, int maximumWorkerCount = int.MaxValue); + /// /// Dispatches all the available workers. /// diff --git a/BepuUtilities/JobQueue.cs b/BepuUtilities/JobQueue.cs new file mode 100644 index 000000000..14110f83c --- /dev/null +++ b/BepuUtilities/JobQueue.cs @@ -0,0 +1,268 @@ +using System; +using System.Collections.Generic; +using System.Diagnostics; +using System.Linq; +using System.Numerics; +using System.Text; +using System.Threading; +using System.Threading.Tasks; +using BepuUtilities.Memory; + +namespace BepuUtilities; + +/// +/// Refers to a job within a . +/// +public struct JobHandle +{ + internal long JobIndex; +} + +/// +/// Description of one task within a job to be submitted to a . +/// +public unsafe struct TaskDescription +{ + /// + /// Function to be executed by the task. Takes as arguments the , pointer, and executing worker index. + /// + public delegate* Function; + /// + /// Context to be passed into the . + /// + public void* Context; + /// + /// Identifier of this task within the job. + /// + public int TaskId; +} + + + +public unsafe struct JobQueue +{ + internal struct Task + { + public delegate* Function; + public void* Context; + /// + /// Identifier of this task within the job. + /// + public int TaskId; + /// + /// Index of the job to which this task belongs. + /// + public int JobIndex; + } + struct Job + { + public int TaskStart; + public int TaskEnd; + public int CompletionCounter; + /// + /// Because we use a ring buffer, we will eventually reuse slots. + /// Jobs are given long identifiers to track them over time. This version will store the number of times the queue has wrapped around the jobs buffer before creating this task. + /// + public int Version; + } + + public enum DequeueResult + { + /// + /// A task was successfully dequeued. + /// + Success, + /// + /// The dequeue attempt was contested. + /// + Contested, + /// + /// The job queue was empty, but may have more tasks in the future. + /// + Empty, + /// + /// The job queue has been terminated and all threads seeking work should stop. + /// + Stop + } + + + + Buffer jobs; + + Buffer tasks; + + int jobMask, jobShift; + + //TODO: Careful about false sharing. + long taskIndex; + long allocatedTaskIndex; + long writtenTaskIndex; + + volatile int locker; + + /// + /// Constructs a new job queue. + /// + /// Buffer pool to allocate resources from. + /// Maximum number of jobs to allocate space for. If more jobs exist at any one moment, attempts to create new jobs will have to stall until space is available. Rounds up to the nearest equal or larger power of 2. + /// Maximum number of tasks to allocate space for. Tasks are groupings of jobs that can be waited on. + public JobQueue(BufferPool pool, int maximumJobCapacity = 1024, int maximumTaskCapacity = 256) + { + maximumJobCapacity = (int)BitOperations.RoundUpToPowerOf2((uint)maximumJobCapacity); + pool.Take(maximumJobCapacity, out jobs); + pool.Take(maximumJobCapacity, out tasks); + tasks.Clear(0, tasks.Length); + var jobMask = jobs.length - 1; + var jobShift = BitOperations.TrailingZeroCount(jobs.length); + } + + public void Dispose(BufferPool pool) + { + pool.Return(ref jobs); + pool.Return(ref tasks); + } + + public DequeueResult TryDequeue(out delegate* function, out void* context, out int taskId) + { + function = default; + context = default; + taskId = default; + if (Interlocked.CompareExchange(ref locker, 1, 0) != 0) + return DequeueResult.Contested; + try + { + //We have the lock. + var nextTaskIndex = taskIndex; + if (nextTaskIndex >= writtenTaskIndex) + return DequeueResult.Empty; + var task = tasks[(int)(nextTaskIndex & jobMask)]; + if (task.JobIndex < 0) + return DequeueResult.Stop; + //There's an actual job! + function = task.Function; + context = task.Context; + taskId = task.TaskId; + ++taskIndex; + return DequeueResult.Success; + } + finally + { + locker = 0; + } + } + + /// + /// Checks whether all tasks composing a job have completed. + /// + /// Job to check for completion. + /// True if the job has completed, false otherwise. + public bool TaskIsComplete(JobHandle jobHandle) + { + var version = jobHandle.JobIndex >> jobShift; + ref var job = ref jobs[(int)(jobHandle.JobIndex & jobMask)]; + return job.Version == version && job.CompletionCounter == 0; + } + + /// + /// Tries to appends a task to the job queue if the ring buffer is uncontested. Will return false if contested or if there is no room left. + /// + /// Tasks composing the job. + /// Handle representing the created job if the enqueue succeeded, invalid handle otherwise. + public bool TryEnqueueJob(Span tasks, out JobHandle jobHandle) + { + jobHandle.JobIndex = -1; + if (Interlocked.CompareExchange(ref locker, 1, 0) != 0) + return false; + try + { + //We have the lock. + Debug.Assert(this.tasks[(int)writtenTaskIndex].JobIndex >= 0, "No more jobs should be written after a stop command."); + var taskStartIndex = allocatedTaskIndex; + var taskEndIndex = taskStartIndex + tasks.Length; + allocatedTaskIndex = taskEndIndex; + var longLength = (long)this.tasks.length; + if (taskEndIndex - taskIndex > longLength) + { + //We've run out of space in the ring buffer. If we tried to write, we'd overwrite jobs that haven't yet been completed. + return false; + } + //We can actually write the jobs. + Debug.Assert(BitOperations.IsPow2(jobs.Length)); + long mask = longLength - 1; + //jobHandle.JobIndex = ; //TODO: Not done! + var jobIndex = (int)(jobHandle.JobIndex & mask); + for (int i = 0; i < tasks.Length; ++i) + { + var taskIndex = (int)((i + taskStartIndex) & mask); + var sourceJob = tasks[i]; + ref var targetJob = ref this.tasks[taskIndex]; + targetJob.Function = sourceJob.Function; + targetJob.Context = sourceJob.Context; + targetJob.TaskId = sourceJob.TaskId; + targetJob.JobIndex = jobIndex; + } + Interlocked.Exchange(ref writtenTaskIndex, taskEndIndex); //This is not assuming 64 bits. Mildly goofy considering the rest. + return true; + } + finally + { + locker = 0; + } + } + + + ///// + ///// Tries to enqueues the stop command. + ///// + ///// True if the stop could be pushed onto the queue, false otherwise. + //public bool TryEnqueueStop() + //{ + // Span stopJob = stackalloc TaskDescription[1]; + // stopJob[0] = new TaskDescription { Function = null, TaskId = -1 }; + // return TryEnqueueJob(stopJob, out _); + //} + + + /// + /// Submits a set of tasks representing a for loop over the given indices and returns when all loop iterations are complete. + /// + /// Function to execute on each iteration of the loop. + /// Context pointer to pass into each iteration of the loop. + /// Inclusive start index of the loop range. + /// Exclusive end index of the loop range. + /// Index of the currently executing worker. + /// This function attempts to minimize completion latency. The calling thread will not be used to execute any task unrelated to the for loop job. + /// This prevents other parallel jobs from making the total execution time of this for loop longer than if the calling thread simply executed the entire loop itself. + public void For(delegate* function, void* context, int inclusiveStartIndex, int exclusiveEndIndex, int workerIndex) + { + var taskCount = exclusiveEndIndex - inclusiveStartIndex; + if (taskCount <= 0) + return; + if (taskCount > 1) + { + //Note that we only submit tasks to the queue for tasks beyond the first. The current thread is responsible for at least task 0. + Span taskDescriptions = stackalloc TaskDescription[taskCount - 1]; + for (int i = 0; i < taskDescriptions.Length; ++i) + { + taskDescriptions[i] = new TaskDescription { Function = function, Context = context, TaskId = i + 1 + inclusiveStartIndex }; + } + SpinWait waiter = new SpinWait(); + while (!TryEnqueueJob(taskDescriptions, out var jobHandle)) + { + waiter.SpinOnce(-1); //TODO: We're biting the bullet on yields/sleep(0) here. May not be ideal for the use case; investigate. + } + } + //Tasks [1, count) are submitted to the queue and may now be executing on other workers. + //The thread calling the for loop should not relinquish its timeslice. It should immediately begin working on task 0. + function(inclusiveStartIndex, context, workerIndex); + + if (taskCount > 1) + { + //Task 0 is done; this thread should seek out other work. Critically, this thread should pull *only* tasks which were related to the for loop job. + //If we were to take other tasks, + } + + } + +} diff --git a/BepuUtilities/Memory/SpanHelper.cs b/BepuUtilities/Memory/SpanHelper.cs index d74ad0196..80d32e4ae 100644 --- a/BepuUtilities/Memory/SpanHelper.cs +++ b/BepuUtilities/Memory/SpanHelper.cs @@ -17,7 +17,7 @@ internal static void ValidatePower(int power) { Debug.Assert(power >= 0 && power <= MaximumSpanSizePower, $"Power must be from 0 to {MaximumSpanSizePower}, inclusive."); } - + /// /// Computes the lowest integer N such that 2^N >= i. /// @@ -28,7 +28,7 @@ public static int GetContainingPowerOf2(int i) { var unsigned = i == 0 ? 1u : (uint)i; return 32 - BitOperations.LeadingZeroCount(unsigned - 1); - + } /// @@ -60,7 +60,7 @@ public static bool IsPrimitive() /// /// Tests if a type is primitive. Slow path; unspecialized compilation. /// - /// Type to check for primitiveness. + /// Type to check for primitiveness. /// True if the type is one of the primitive types, false otherwise. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static bool IsPrimitive(Type type) diff --git a/BepuUtilities/ThreadDispatcher.cs b/BepuUtilities/ThreadDispatcher.cs index 41253b1ce..73bdae073 100644 --- a/BepuUtilities/ThreadDispatcher.cs +++ b/BepuUtilities/ThreadDispatcher.cs @@ -52,8 +52,11 @@ public ThreadDispatcher(int threadCount, int threadPoolBlockAllocationSize = 163 void DispatchThread(int workerIndex) { - Debug.Assert(workerBody != null); - workerBody(workerIndex, context); + Debug.Assert(workerBody != null ^ workerBodyPointer != null); + if (workerBody != null) + workerBody(workerIndex, context); + else + workerBodyPointer(workerIndex, context); if (Interlocked.Decrement(ref remainingWorkerCounter) == -1) { @@ -62,6 +65,7 @@ void DispatchThread(int workerIndex) } volatile ThreadDispatcherWorker workerBody; + volatile delegate* workerBodyPointer; volatile void* context; int remainingWorkerCounter; @@ -90,11 +94,31 @@ void SignalThreads(int maximumWorkerCount) } } + public void DispatchWorkers(delegate* workerBody, void* context = null, int maximumWorkerCount = int.MaxValue) + { + if (maximumWorkerCount > 1) + { + Debug.Assert(this.workerBody == null && this.workerBodyPointer == null); + workerBodyPointer = workerBody; + this.context = context; + SignalThreads(maximumWorkerCount); + //Calling thread does work. No reason to spin up another worker and block this one! + DispatchThread(0); + finished.WaitOne(); + workerBodyPointer = null; + this.context = null; + } + else if (maximumWorkerCount == 1) + { + workerBody(0, context); + } + } + public void DispatchWorkers(ThreadDispatcherWorker workerBody, void* context = null, int maximumWorkerCount = int.MaxValue) { if (maximumWorkerCount > 1) { - Debug.Assert(this.workerBody == null); + Debug.Assert(this.workerBody == null && this.workerBodyPointer == null); this.workerBody = workerBody; this.context = context; SignalThreads(maximumWorkerCount); diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index 018cab8a5..1d6d1a4d1 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -18,10 +18,11 @@ using static System.Formats.Asn1.AsnWriter; using static OpenTK.Graphics.OpenGL.GL; using System.Xml.Linq; +using System.Threading; namespace Demos.SpecializedTests { - public class TreeFiddlingTestDemo : Demo + public unsafe class TreeFiddlingTestDemo : Demo { struct Pair : IEquatable { From a0de53b277b3eb29f6417f36b7f3a5984196e489 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 5 Nov 2022 20:59:30 -0500 Subject: [PATCH 629/947] JobQueue messy, but closer to existing. --- BepuUtilities/JobQueue.cs | 518 +++++++++++++++++++++++++++++++------- 1 file changed, 421 insertions(+), 97 deletions(-) diff --git a/BepuUtilities/JobQueue.cs b/BepuUtilities/JobQueue.cs index 14110f83c..e50083766 100644 --- a/BepuUtilities/JobQueue.cs +++ b/BepuUtilities/JobQueue.cs @@ -1,8 +1,13 @@ using System; using System.Collections.Generic; +using System.ComponentModel; using System.Diagnostics; using System.Linq; using System.Numerics; +using System.Reflection.Metadata; +using System.Reflection.Metadata.Ecma335; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; using System.Text; using System.Threading; using System.Threading.Tasks; @@ -11,17 +16,49 @@ namespace BepuUtilities; /// -/// Refers to a job within a . +/// Refers to a continuation within a . /// -public struct JobHandle +public struct ContinuationHandle { - internal long JobIndex; + uint index; + uint encodedVersion; + + internal ContinuationHandle(uint index, int version) + { + this.index = index; + encodedVersion = (uint)version | 1u << 31; + } + + internal uint Index + { + get + { + Debug.Assert(Initialized, "If you're trying to pull a continuation id from a continuation handle, it should have been initialized."); + return index; + } + } + + internal int Version + { + get + { + Debug.Assert(Initialized, "If you're trying to pull a continuation id from a continuation handle, it should have been initialized."); + return (int)(encodedVersion & ((1u << 31) - 1)); + } + } + + public static ContinuationHandle Null => default; + + /// + /// Gets whether this handle was ever initialized. This does not guarantee that the job handle is active in the JobQueue that it was allocated from. + /// + public bool Initialized => encodedVersion >= 1 << 31; } /// /// Description of one task within a job to be submitted to a . /// -public unsafe struct TaskDescription +public unsafe struct Task { /// /// Function to be executed by the task. Takes as arguments the , pointer, and executing worker index. @@ -38,33 +75,8 @@ public unsafe struct TaskDescription } - public unsafe struct JobQueue { - internal struct Task - { - public delegate* Function; - public void* Context; - /// - /// Identifier of this task within the job. - /// - public int TaskId; - /// - /// Index of the job to which this task belongs. - /// - public int JobIndex; - } - struct Job - { - public int TaskStart; - public int TaskEnd; - public int CompletionCounter; - /// - /// Because we use a ring buffer, we will eventually reuse slots. - /// Jobs are given long identifiers to track them over time. This version will store the number of times the queue has wrapped around the jobs buffer before creating this task. - /// - public int Version; - } public enum DequeueResult { @@ -85,42 +97,101 @@ public enum DequeueResult /// Stop } + /// + /// Describes the result of a continuation allocation attempt. + /// + public enum ContinuationAllocationResult + { + /// + /// The continuation was successfully allocated. + /// + Success, + /// + /// The continuation was blocked by concurrent access. + /// + Contested, + /// + /// The job queue's continuation buffer is full and can't hold the continuation. + /// + Full + } - Buffer jobs; - Buffer tasks; - int jobMask, jobShift; + int taskMask, taskShift; //TODO: Careful about false sharing. long taskIndex; long allocatedTaskIndex; long writtenTaskIndex; - volatile int locker; + volatile int taskLocker; + + + internal struct JobQueueContinuations + { + public Buffer Continuations; + public IdPool IndexPool; + public int ContinuationCount; + public int Locker; + + /// + /// Retrieves a pointer to the continuation data for . + /// + /// Handle to look up the associated continuation for. + /// Pointer to the continuation backing the given handle. + /// This should not be used if the continuation handle is not known to be valid. The data pointed to by the data could become invalidated if the continuation completes. + public Continuation* GetContinuation(ContinuationHandle continuationHandle) + { + Debug.Assert(continuationHandle.Initialized, "This continuation handle was never initialized."); + Debug.Assert(continuationHandle.Index >= Continuations.length, "This continuation refers to an invalid index."); + if (continuationHandle.Index >= Continuations.length || !continuationHandle.Initialized) + return null; + var continuation = Continuations.Memory + continuationHandle.Index; + Debug.Assert(continuation->Version == continuationHandle.Version, "This continuation no longer refers to an active continuation."); + if (continuation->Version != continuationHandle.Version) + return null; + return Continuations.Memory + continuationHandle.Index; + } + + } + /// + /// Holds the job queue's continuations data in unmanaged memory just in case the queue itself is in unpinned memory. + /// + Buffer continuationsContainer; /// /// Constructs a new job queue. /// /// Buffer pool to allocate resources from. - /// Maximum number of jobs to allocate space for. If more jobs exist at any one moment, attempts to create new jobs will have to stall until space is available. Rounds up to the nearest equal or larger power of 2. - /// Maximum number of tasks to allocate space for. Tasks are groupings of jobs that can be waited on. - public JobQueue(BufferPool pool, int maximumJobCapacity = 1024, int maximumTaskCapacity = 256) + /// Maximum number of tasks to allocate space for. Tasks are individual chunks of scheduled work. Rounded up to the nearest power of 2. + /// Maximum number of continuations to allocate space for. If more continuations exist at any one moment, attempts to create new jobs will have to stall until space is available. + public JobQueue(BufferPool pool, int maximumTaskCapacity = 1024, int maximumContinuationCapacity = 256) { - maximumJobCapacity = (int)BitOperations.RoundUpToPowerOf2((uint)maximumJobCapacity); - pool.Take(maximumJobCapacity, out jobs); - pool.Take(maximumJobCapacity, out tasks); + maximumTaskCapacity = (int)BitOperations.RoundUpToPowerOf2((uint)maximumTaskCapacity); + pool.Take(1, out continuationsContainer); + ref var continuations = ref continuationsContainer[0]; + pool.Take(maximumContinuationCapacity, out continuations.Continuations); + continuations.Continuations.Clear(0, continuations.Continuations.Length); + continuations.IndexPool = new IdPool(maximumContinuationCapacity, pool); + continuations.ContinuationCount = 0; + continuations.Locker = 0; + + pool.Take(maximumTaskCapacity, out tasks); tasks.Clear(0, tasks.Length); - var jobMask = jobs.length - 1; - var jobShift = BitOperations.TrailingZeroCount(jobs.length); + taskMask = tasks.length - 1; + taskShift = BitOperations.TrailingZeroCount(tasks.length); + taskLocker = 0; } public void Dispose(BufferPool pool) { - pool.Return(ref jobs); + continuationsContainer[0].IndexPool.Dispose(pool); + pool.Return(ref continuationsContainer[0].Continuations); pool.Return(ref tasks); + pool.Return(ref continuationsContainer); } public DequeueResult TryDequeue(out delegate* function, out void* context, out int taskId) @@ -128,7 +199,7 @@ public DequeueResult TryDequeue(out delegate* function, o function = default; context = default; taskId = default; - if (Interlocked.CompareExchange(ref locker, 1, 0) != 0) + if (Interlocked.CompareExchange(ref taskLocker, 1, 0) != 0) return DequeueResult.Contested; try { @@ -136,8 +207,8 @@ public DequeueResult TryDequeue(out delegate* function, o var nextTaskIndex = taskIndex; if (nextTaskIndex >= writtenTaskIndex) return DequeueResult.Empty; - var task = tasks[(int)(nextTaskIndex & jobMask)]; - if (task.JobIndex < 0) + var task = tasks[(int)(nextTaskIndex & taskMask)]; + if (task.Function == null) return DequeueResult.Stop; //There's an actual job! function = task.Function; @@ -148,36 +219,87 @@ public DequeueResult TryDequeue(out delegate* function, o } finally { - locker = 0; + taskLocker = 0; } } + public DequeueResult TryDequeueAndRun(int workerIndex) + { + var result = TryDequeue(out var function, out var context, out var taskId); + if (result == DequeueResult.Success) + function(taskId, context, workerIndex); + return result; + } + + public void DequeueAndRun(int workerIndex) + { + SpinWait waiter = new SpinWait(); + while (TryDequeueAndRun(workerIndex) != DequeueResult.Success) + { + waiter.SpinOnce(); + } + } + + /// - /// Checks whether all tasks composing a job have completed. + /// Checks whether all tasks composing a job, as reported to the continuation, have completed. /// - /// Job to check for completion. + /// Job to check for completion. /// True if the job has completed, false otherwise. - public bool TaskIsComplete(JobHandle jobHandle) + public bool IsComplete(ContinuationHandle continuationHandle) { - var version = jobHandle.JobIndex >> jobShift; - ref var job = ref jobs[(int)(jobHandle.JobIndex & jobMask)]; - return job.Version == version && job.CompletionCounter == 0; + Debug.Assert(continuationHandle.Initialized, "This continuation handle was never initialized."); + Debug.Assert(continuationHandle.Index >= continuationsContainer[0].Continuations.length, "This continuation refers to an invalid index."); + ref var continuationSet = ref continuationsContainer[0]; + if (continuationHandle.Index >= continuationSet.Continuations.length || !continuationHandle.Initialized) + return false; + ref var continuation = ref continuationSet.Continuations[continuationHandle.Index]; + return continuation.Version > continuationHandle.Version || continuation.RemainingTaskCounter == 0; + } + /// + /// Retrieves a pointer to the continuation data for . + /// + /// Handle to look up the associated continuation for. + /// Pointer to the continuation backing the given handle. + /// This should not be used if the continuation handle is not known to be valid. The data pointed to by the data could become invalidated if the continuation completes. + public Continuation* GetContinuation(ContinuationHandle continuationHandle) + { + return continuationsContainer[0].GetContinuation(continuationHandle); } + /// - /// Tries to appends a task to the job queue if the ring buffer is uncontested. Will return false if contested or if there is no room left. + /// Describes the result of a task enqueue attempt. + /// + public enum EnqueueTasksResult + { + /// + /// The tasks were successfully enqueued. + /// + Success, + /// + /// The enqueue attempt was blocked by concurrent access. + /// + Contested, + /// + /// The enqueue attempt was blocked because no space remained in the tasks buffer. + /// + Full, + } + + /// + /// Tries to appends a set of tasks to the job queue if the ring buffer is uncontested. /// /// Tasks composing the job. - /// Handle representing the created job if the enqueue succeeded, invalid handle otherwise. - public bool TryEnqueueJob(Span tasks, out JobHandle jobHandle) + /// Result of the enqueue attempt. + EnqueueTasksResult TryEnqueueTasks(Span tasks) { - jobHandle.JobIndex = -1; - if (Interlocked.CompareExchange(ref locker, 1, 0) != 0) - return false; + if (Interlocked.CompareExchange(ref taskLocker, 1, 0) != 0) + return EnqueueTasksResult.Contested; try { //We have the lock. - Debug.Assert(this.tasks[(int)writtenTaskIndex].JobIndex >= 0, "No more jobs should be written after a stop command."); + Debug.Assert(writtenTaskIndex > 0 && this.tasks[(int)((writtenTaskIndex - 1) & taskMask)].Function != null, "No more jobs should be written after a stop command."); var taskStartIndex = allocatedTaskIndex; var taskEndIndex = taskStartIndex + tasks.Length; allocatedTaskIndex = taskEndIndex; @@ -185,45 +307,225 @@ public bool TryEnqueueJob(Span tasks, out JobHandle jobHandle) if (taskEndIndex - taskIndex > longLength) { //We've run out of space in the ring buffer. If we tried to write, we'd overwrite jobs that haven't yet been completed. - return false; + return EnqueueTasksResult.Full; } //We can actually write the jobs. - Debug.Assert(BitOperations.IsPow2(jobs.Length)); - long mask = longLength - 1; - //jobHandle.JobIndex = ; //TODO: Not done! - var jobIndex = (int)(jobHandle.JobIndex & mask); + Debug.Assert(BitOperations.IsPow2(this.tasks.Length)); + var wrappedStartIndex = taskStartIndex & taskMask; + var wrappedEndIndex = taskEndIndex & taskMask; + //if (wrappedEndIndex > wrappedStartIndex) + //{ + // //We can just copy the whole task block as one blob. + // Unsafe.CopyBlockUnaligned(ref *(byte*)(this.tasks.Memory + taskStartIndex), ref Unsafe.As(ref MemoryMarshal.GetReference(tasks), Unsafe.SizeOf() * tasks.Length); + //} + //else + //{ + // //Copy the task block as two blobs. + // Unsafe.CopyBlockUnaligned(ref *(byte*)(this.tasks.Memory + taskStartIndex), ref Unsafe.As(ref MemoryMarshal.GetReference(tasks), Unsafe.SizeOf() * tasks.Length); + //} for (int i = 0; i < tasks.Length; ++i) { var taskIndex = (int)((i + taskStartIndex) & mask); - var sourceJob = tasks[i]; - ref var targetJob = ref this.tasks[taskIndex]; - targetJob.Function = sourceJob.Function; - targetJob.Context = sourceJob.Context; - targetJob.TaskId = sourceJob.TaskId; - targetJob.JobIndex = jobIndex; + this.tasks[taskIndex] = tasks[i]; } Interlocked.Exchange(ref writtenTaskIndex, taskEndIndex); //This is not assuming 64 bits. Mildly goofy considering the rest. - return true; + return EnqueueTasksResult.Success; } finally { - locker = 0; + taskLocker = 0; + } + } + + /// + /// Appends a task to the job queue if the ring buffer is uncontested. + /// + /// Tasks composing the job. + void EnqueueTasks(Span tasks) + { + SpinWait waiter = new SpinWait(); + while (TryEnqueueTasks(tasks) != EnqueueTasksResult.Success) + { + waiter.SpinOnce(-1); + } + } + /// + /// Tries to enqueues the stop command. + /// + /// True if the stop could be pushed onto the queue, false otherwise. + public EnqueueTasksResult TryEnqueueStop() + { + Span stopJob = stackalloc Task[1]; + stopJob[0] = new Task { Function = null }; + return TryEnqueueTasks(stopJob); + } + + /// + /// Enqueues the stop command. + /// + public void EnqueueStop() + { + Span stopJob = stackalloc Task[1]; + stopJob[0] = new Task { Function = null }; + SpinWait waiter = new SpinWait(); + while (TryEnqueueTasks(stopJob) != EnqueueTasksResult.Success) + { + waiter.SpinOnce(-1); } } + /// + /// Wraps a task for easier use with continuations. + /// + public struct WrappedTaskContext + { + /// + /// Function to be invoked by this wrapped tsak. + /// + public delegate* Function; + /// + /// Context to be passed to this wrapped task. + /// + public void* Context; + /// + /// Handle of the continuation associated with this wrapped task. + /// + public ContinuationHandle Continuation; + /// + /// Set of continuations in the job queue. + /// + internal JobQueueContinuations* Continuations; + } + + + public void CreateCompletionWrappedTasks(ContinuationHandle continuationHandle, Span tasks, WrappedTaskContext* wrappedTaskContexts, Span wrappedTasks) + { + var count = Math.Min(tasks.Length, wrappedTasks.Length); + Debug.Assert(tasks.Length == wrappedTasks.Length, "This is probably a bug!"); + for (int i = 0; i < count; ++i) + { + ref var sourceTask = ref tasks[i]; + var wrappedContext = wrappedTaskContexts + i; + ref var targetTask = ref wrappedTasks[i]; + wrappedContext->Function = sourceTask.Function; + wrappedContext->Context = sourceTask.Context; + wrappedContext->Continuation = continuationHandle; + wrappedContext->Continuations = continuationsContainer.Memory; + targetTask.Function = &RunAndMarkAsComplete; + targetTask.Context = wrappedContext; + targetTask.TaskId = sourceTask.TaskId; + } + } - ///// - ///// Tries to enqueues the stop command. - ///// - ///// True if the stop could be pushed onto the queue, false otherwise. - //public bool TryEnqueueStop() - //{ - // Span stopJob = stackalloc TaskDescription[1]; - // stopJob[0] = new TaskDescription { Function = null, TaskId = -1 }; - // return TryEnqueueJob(stopJob, out _); - //} + + + static void RunAndMarkAsComplete(int taskId, void* wrapperContextPointer, int workerIndex) + { + var wrapperContext = (WrappedTaskContext*)wrapperContextPointer; + wrapperContext->Function(taskId, wrapperContext->Context, workerIndex); + var continuationHandle = wrapperContext->Continuation; + var continuations = wrapperContext->Continuations; + var continuation = continuations->GetContinuation(continuationHandle); + var counter = Interlocked.Decrement(ref continuation->RemainingTaskCounter); + if (counter == 0) + { + //This entire job has completed. + if (continuation->OnCompleted != null) + { + continuation->OnCompleted(continuation->UserId, continuation->OnCompletedContext, workerIndex); + } + //Free this continuation slot. + var waiter = new SpinWait(); + while (true) + { + if (Interlocked.CompareExchange(ref continuations->Locker, 1, 0) != 0) + { + waiter.SpinOnce(-1); + } + else + { + //We have the lock. + continuations->IndexPool.ReturnUnsafely((int)continuationHandle.Index); + --continuations->ContinuationCount; + continuations->Locker = 0; + } + } + } + } + /// + /// Stores data relevant to tracking task completion and reporting completion for a job. + /// + public struct Continuation + { + /// + /// Function to call upon completion of the job, if any. + /// + public delegate* OnCompleted; + /// + /// Context to pass to the completion function, if any. + /// + public void* OnCompletedContext; + internal JobQueueContinuations* Continuations; + /// + /// Id provided by the user to identify this job. + /// + public ulong UserId; + /// + /// Version of this continuation. + /// + public int Version; + /// + /// Number of tasks not yet reported as complete in the job. + /// + public int RemainingTaskCounter; + } + + + public ContinuationAllocationResult TryAllocateContinuation(int taskCount, ulong userContinuationId, delegate* onCompleted, void* onCompletedContext, out ContinuationHandle continuationHandle) + { + continuationHandle = default; + ref var continuations = ref continuationsContainer[0]; + if (Interlocked.CompareExchange(ref continuations.Locker, 1, 0) != 0) + return ContinuationAllocationResult.Contested; + try + { + //We have the lock. + Debug.Assert(continuations.ContinuationCount <= continuations.Continuations.length); + if (continuations.ContinuationCount >= continuations.Continuations.length) + { + //No room. + return ContinuationAllocationResult.Full; + } + var index = continuations.IndexPool.Take(); + ref var continuation = ref continuations.Continuations[index]; + var newVersion = continuation.Version + 1; + continuation.OnCompletedContext = onCompletedContext; + continuation.OnCompleted = onCompleted; + continuation.UserId = userContinuationId; + continuation.Version = newVersion; + continuation.RemainingTaskCounter = taskCount; + continuationHandle = new ContinuationHandle((uint)index, newVersion); + return ContinuationAllocationResult.Success; + } + finally + { + continuations.Locker = 0; + } + } + + public ContinuationHandle AllocateContinuation(int taskCount, ulong userContinuationId = 0, delegate* onCompleted = null, void* onCompletedContext = null) + { + SpinWait waiter = new SpinWait(); + ContinuationHandle handle; + while (TryAllocateContinuation(taskCount, userContinuationId, onCompleted, onCompletedContext, out handle) != ContinuationAllocationResult.Success) + { + waiter.SpinOnce(-1); + } + return handle; + } + /// /// Submits a set of tasks representing a for loop over the given indices and returns when all loop iterations are complete. /// @@ -232,23 +534,27 @@ public bool TryEnqueueJob(Span tasks, out JobHandle jobHandle) /// Inclusive start index of the loop range. /// Exclusive end index of the loop range. /// Index of the currently executing worker. - /// This function attempts to minimize completion latency. The calling thread will not be used to execute any task unrelated to the for loop job. - /// This prevents other parallel jobs from making the total execution time of this for loop longer than if the calling thread simply executed the entire loop itself. public void For(delegate* function, void* context, int inclusiveStartIndex, int exclusiveEndIndex, int workerIndex) { - var taskCount = exclusiveEndIndex - inclusiveStartIndex; - if (taskCount <= 0) + var iterationCount = exclusiveEndIndex - inclusiveStartIndex; + if (iterationCount <= 0) return; - if (taskCount > 1) + ContinuationHandle continuationHandle = default; + if (iterationCount > 1) { //Note that we only submit tasks to the queue for tasks beyond the first. The current thread is responsible for at least task 0. - Span taskDescriptions = stackalloc TaskDescription[taskCount - 1]; - for (int i = 0; i < taskDescriptions.Length; ++i) + var taskCount = iterationCount - 1; + WrappedTaskContext* wrappedContexts = stackalloc WrappedTaskContext[taskCount]; + Span tasks = stackalloc Task[taskCount]; + continuationHandle = AllocateContinuation(taskCount); + for (int i = 0; i < tasks.Length; ++i) { - taskDescriptions[i] = new TaskDescription { Function = function, Context = context, TaskId = i + 1 + inclusiveStartIndex }; + var wrappedTaskContext = wrappedContexts + i; + *wrappedTaskContext = new WrappedTaskContext { Function = function, Context = context, Continuation = continuationHandle, Continuations = continuationsContainer.Memory }; + tasks[i] = new Task { Function = &RunAndMarkAsComplete, Context = wrappedTaskContext, TaskId = i + 1 + inclusiveStartIndex }; } - SpinWait waiter = new SpinWait(); - while (!TryEnqueueJob(taskDescriptions, out var jobHandle)) + var waiter = new SpinWait(); + while (TryEnqueueTasks(tasks) != EnqueueTasksResult.Success) { waiter.SpinOnce(-1); //TODO: We're biting the bullet on yields/sleep(0) here. May not be ideal for the use case; investigate. } @@ -257,12 +563,30 @@ public void For(delegate* function, void* context, int in //The thread calling the for loop should not relinquish its timeslice. It should immediately begin working on task 0. function(inclusiveStartIndex, context, workerIndex); - if (taskCount > 1) + if (iterationCount > 1) { - //Task 0 is done; this thread should seek out other work. Critically, this thread should pull *only* tasks which were related to the for loop job. - //If we were to take other tasks, + //Task 0 is done; this thread should seek out other work until the job is complete. + var waiter = new SpinWait(); + while (!IsComplete(continuationHandle)) + { + //Note that we don't handle the DequeueResult.Stop case; if the job isn't complete yet, there's no way to hit a stop unless we enqueued this job after a stop. + //Enqueuing after a stop is an error condition and is debug checked for in TryEnqueueJob. + var dequeueResult = TryDequeue(out var fillerJob, out var fillerContext, out var fillerTaskId); + if (dequeueResult == DequeueResult.Stop) + { + Debug.Assert(dequeueResult != DequeueResult.Stop, "Did you enqueue this for loop *after* some thread enqueued a stop command? That's illegal!"); + return; + } + if (dequeueResult == DequeueResult.Success) + { + fillerJob(fillerTaskId, fillerContext, workerIndex); + waiter.Reset(); + } + else + { + waiter.SpinOnce(-1); + } + } } - } - } From 3dfa5fb833883df7495552da786163a097d208bf Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 7 Nov 2022 13:10:34 -0600 Subject: [PATCH 630/947] Block copies, visibility corrections. --- BepuUtilities/JobQueue.cs | 53 +++++++++++++++++++++++---------------- 1 file changed, 32 insertions(+), 21 deletions(-) diff --git a/BepuUtilities/JobQueue.cs b/BepuUtilities/JobQueue.cs index e50083766..5fc5adce1 100644 --- a/BepuUtilities/JobQueue.cs +++ b/BepuUtilities/JobQueue.cs @@ -292,8 +292,10 @@ public enum EnqueueTasksResult /// /// Tasks composing the job. /// Result of the enqueue attempt. - EnqueueTasksResult TryEnqueueTasks(Span tasks) + public EnqueueTasksResult TryEnqueueTasks(Span tasks) { + if (tasks.Length == 0) + return EnqueueTasksResult.Success; if (Interlocked.CompareExchange(ref taskLocker, 1, 0) != 0) return EnqueueTasksResult.Contested; try @@ -311,23 +313,28 @@ EnqueueTasksResult TryEnqueueTasks(Span tasks) } //We can actually write the jobs. Debug.Assert(BitOperations.IsPow2(this.tasks.Length)); - var wrappedStartIndex = taskStartIndex & taskMask; - var wrappedEndIndex = taskEndIndex & taskMask; - //if (wrappedEndIndex > wrappedStartIndex) - //{ - // //We can just copy the whole task block as one blob. - // Unsafe.CopyBlockUnaligned(ref *(byte*)(this.tasks.Memory + taskStartIndex), ref Unsafe.As(ref MemoryMarshal.GetReference(tasks), Unsafe.SizeOf() * tasks.Length); - //} - //else - //{ - // //Copy the task block as two blobs. - // Unsafe.CopyBlockUnaligned(ref *(byte*)(this.tasks.Memory + taskStartIndex), ref Unsafe.As(ref MemoryMarshal.GetReference(tasks), Unsafe.SizeOf() * tasks.Length); - //} - for (int i = 0; i < tasks.Length; ++i) + var wrappedInclusiveStartIndex = (int)(taskStartIndex & taskMask); + var wrappedInclusiveEndIndex = (int)(taskEndIndex & taskMask); + if (wrappedInclusiveEndIndex > wrappedInclusiveStartIndex) + { + //We can just copy the whole task block as one blob. + Unsafe.CopyBlockUnaligned(ref *(byte*)(this.tasks.Memory + taskStartIndex), ref Unsafe.As(ref MemoryMarshal.GetReference(tasks)), (uint)(Unsafe.SizeOf() * tasks.Length)); + } + else { - var taskIndex = (int)((i + taskStartIndex) & mask); - this.tasks[taskIndex] = tasks[i]; + //Copy the task block as two blobs. + ref var startTask = ref tasks[0]; + var firstRegionCount = this.tasks.length - wrappedInclusiveStartIndex; + ref var secondBlobStartTask = ref tasks[firstRegionCount]; + var secondRegionCount = tasks.Length - firstRegionCount; + Unsafe.CopyBlockUnaligned(ref *(byte*)(this.tasks.Memory + taskStartIndex), ref Unsafe.As(ref startTask), (uint)(Unsafe.SizeOf() * firstRegionCount)); + Unsafe.CopyBlockUnaligned(ref *(byte*)this.tasks.Memory, ref Unsafe.As(ref secondBlobStartTask), (uint)(Unsafe.SizeOf() * secondRegionCount)); } + //for (int i = 0; i < tasks.Length; ++i) + //{ + // var taskIndex = (int)((i + taskStartIndex) & taskMask); + // this.tasks[taskIndex] = tasks[i]; + //} Interlocked.Exchange(ref writtenTaskIndex, taskEndIndex); //This is not assuming 64 bits. Mildly goofy considering the rest. return EnqueueTasksResult.Success; } @@ -341,7 +348,7 @@ EnqueueTasksResult TryEnqueueTasks(Span tasks) /// Appends a task to the job queue if the ring buffer is uncontested. /// /// Tasks composing the job. - void EnqueueTasks(Span tasks) + public void EnqueueTasks(Span tasks) { SpinWait waiter = new SpinWait(); while (TryEnqueueTasks(tasks) != EnqueueTasksResult.Success) @@ -367,7 +374,7 @@ public void EnqueueStop() { Span stopJob = stackalloc Task[1]; stopJob[0] = new Task { Function = null }; - SpinWait waiter = new SpinWait(); + var waiter = new SpinWait(); while (TryEnqueueTasks(stopJob) != EnqueueTasksResult.Success) { waiter.SpinOnce(-1); @@ -397,7 +404,13 @@ public struct WrappedTaskContext internal JobQueueContinuations* Continuations; } - + /// + /// Wraps a set of tasks in continuation tasks that will report their completion. + /// + /// Handle of the continuation to report to. + /// Tasks to wrap. + /// Contexts to be used for the wrapped tasks. This memory must persist until the wrapped tasks complete. + /// Span to hold the tasks created by this function. public void CreateCompletionWrappedTasks(ContinuationHandle continuationHandle, Span tasks, WrappedTaskContext* wrappedTaskContexts, Span wrappedTasks) { var count = Math.Min(tasks.Length, wrappedTasks.Length); @@ -417,8 +430,6 @@ public void CreateCompletionWrappedTasks(ContinuationHandle continuationHandle, } } - - static void RunAndMarkAsComplete(int taskId, void* wrapperContextPointer, int workerIndex) { var wrapperContext = (WrappedTaskContext*)wrapperContextPointer; From 043d58c3b076085f389866da3224f677c194c84d Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 7 Nov 2022 13:41:35 -0600 Subject: [PATCH 631/947] Cleanup. --- BepuUtilities/{JobQueue.cs => TaskQueue.cs} | 376 +++++++++++--------- 1 file changed, 216 insertions(+), 160 deletions(-) rename BepuUtilities/{JobQueue.cs => TaskQueue.cs} (65%) diff --git a/BepuUtilities/JobQueue.cs b/BepuUtilities/TaskQueue.cs similarity index 65% rename from BepuUtilities/JobQueue.cs rename to BepuUtilities/TaskQueue.cs index 5fc5adce1..ebc810385 100644 --- a/BepuUtilities/JobQueue.cs +++ b/BepuUtilities/TaskQueue.cs @@ -2,6 +2,7 @@ using System.Collections.Generic; using System.ComponentModel; using System.Diagnostics; +using System.Diagnostics.CodeAnalysis; using System.Linq; using System.Numerics; using System.Reflection.Metadata; @@ -16,9 +17,70 @@ namespace BepuUtilities; /// -/// Refers to a continuation within a . +/// Description of one task within a job to be submitted to a . /// -public struct ContinuationHandle +public unsafe struct Task +{ + /// + /// Function to be executed by the task. Takes as arguments the , pointer, and executing worker index. + /// + public delegate* Function; + /// + /// Context to be passed into the . + /// + public void* Context; + /// + /// Identifier of this task within the job. + /// + public int TaskId; +} + +/// +/// Describes the result status of a dequeue attempt. +/// +public enum DequeueTaskResult +{ + /// + /// A task was successfully dequeued. + /// + Success, + /// + /// The dequeue attempt was contested. + /// + Contested, + /// + /// The queue was empty, but may have more tasks in the future. + /// + Empty, + /// + /// The queue has been terminated and all threads seeking work should stop. + /// + Stop +} + +/// +/// Describes the result of a task enqueue attempt. +/// +public enum EnqueueTaskResult +{ + /// + /// The tasks were successfully enqueued. + /// + Success, + /// + /// The enqueue attempt was blocked by concurrent access. + /// + Contested, + /// + /// The enqueue attempt was blocked because no space remained in the tasks buffer. + /// + Full, +} + +/// +/// Refers to a continuation within a . +/// +public struct ContinuationHandle : IEquatable { uint index; uint encodedVersion; @@ -47,77 +109,107 @@ internal int Version } } + /// + /// Gets a null continuation handle. + /// public static ContinuationHandle Null => default; /// - /// Gets whether this handle was ever initialized. This does not guarantee that the job handle is active in the JobQueue that it was allocated from. + /// Gets whether this handle was ever initialized. This does not guarantee that the job handle is active in the that it was allocated from. /// - public bool Initialized => encodedVersion >= 1 << 31; + public bool Initialized => encodedVersion >= 1u << 31; + + public bool Equals(ContinuationHandle other) => other.index == index && other.encodedVersion == encodedVersion; + + public override bool Equals([NotNullWhen(true)] object obj) => obj is ContinuationHandle handle && Equals(handle); + + public override int GetHashCode() => (int)(index ^ (encodedVersion << 24)); + + public static bool operator ==(ContinuationHandle left, ContinuationHandle right) => left.Equals(right); + + public static bool operator !=(ContinuationHandle left, ContinuationHandle right) => !(left == right); } /// -/// Description of one task within a job to be submitted to a . +/// Describes the result of a continuation allocation attempt. /// -public unsafe struct Task +public enum AllocateTaskContinuationResult { /// - /// Function to be executed by the task. Takes as arguments the , pointer, and executing worker index. + /// The continuation was successfully allocated. /// - public delegate* Function; + Success, /// - /// Context to be passed into the . + /// The continuation was blocked by concurrent access. /// - public void* Context; + Contested, /// - /// Identifier of this task within the job. + /// The queue's continuation buffer is full and can't hold the continuation. /// - public int TaskId; + Full } - -public unsafe struct JobQueue +internal unsafe struct TaskQueueContinuations { + public Buffer Continuations; + public IdPool IndexPool; + public int ContinuationCount; + public int Locker; - public enum DequeueResult - { - /// - /// A task was successfully dequeued. - /// - Success, - /// - /// The dequeue attempt was contested. - /// - Contested, - /// - /// The job queue was empty, but may have more tasks in the future. - /// - Empty, - /// - /// The job queue has been terminated and all threads seeking work should stop. - /// - Stop - } /// - /// Describes the result of a continuation allocation attempt. + /// Retrieves a pointer to the continuation data for . /// - public enum ContinuationAllocationResult + /// Handle to look up the associated continuation for. + /// Pointer to the continuation backing the given handle. + /// This should not be used if the continuation handle is not known to be valid. The data pointed to by the data could become invalidated if the continuation completes. + public TaskContinuation* GetContinuation(ContinuationHandle continuationHandle) { - /// - /// The continuation was successfully allocated. - /// - Success, - /// - /// The continuation was blocked by concurrent access. - /// - Contested, - /// - /// The job queue's continuation buffer is full and can't hold the continuation. - /// - Full + Debug.Assert(continuationHandle.Initialized, "This continuation handle was never initialized."); + Debug.Assert(continuationHandle.Index >= Continuations.length, "This continuation refers to an invalid index."); + if (continuationHandle.Index >= Continuations.length || !continuationHandle.Initialized) + return null; + var continuation = Continuations.Memory + continuationHandle.Index; + Debug.Assert(continuation->Version == continuationHandle.Version, "This continuation no longer refers to an active continuation."); + if (continuation->Version != continuationHandle.Version) + return null; + return Continuations.Memory + continuationHandle.Index; } +} +/// +/// Stores data relevant to tracking task completion and reporting completion for a job. +/// +public unsafe struct TaskContinuation +{ + /// + /// Function to call upon completion of the job, if any. + /// + public delegate* OnCompleted; + /// + /// Context to pass to the completion function, if any. + /// + public void* OnCompletedContext; + internal TaskQueueContinuations* Continuations; + /// + /// Id provided by the user to identify this job. + /// + public ulong UserId; + /// + /// Version of this continuation. + /// + public int Version; + /// + /// Number of tasks not yet reported as complete in the job. + /// + public int RemainingTaskCounter; +} +/// +/// Multithreaded task queue +/// +public unsafe struct TaskQueue +{ Buffer tasks; int taskMask, taskShift; @@ -129,46 +221,18 @@ public enum ContinuationAllocationResult volatile int taskLocker; - - internal struct JobQueueContinuations - { - public Buffer Continuations; - public IdPool IndexPool; - public int ContinuationCount; - public int Locker; - - /// - /// Retrieves a pointer to the continuation data for . - /// - /// Handle to look up the associated continuation for. - /// Pointer to the continuation backing the given handle. - /// This should not be used if the continuation handle is not known to be valid. The data pointed to by the data could become invalidated if the continuation completes. - public Continuation* GetContinuation(ContinuationHandle continuationHandle) - { - Debug.Assert(continuationHandle.Initialized, "This continuation handle was never initialized."); - Debug.Assert(continuationHandle.Index >= Continuations.length, "This continuation refers to an invalid index."); - if (continuationHandle.Index >= Continuations.length || !continuationHandle.Initialized) - return null; - var continuation = Continuations.Memory + continuationHandle.Index; - Debug.Assert(continuation->Version == continuationHandle.Version, "This continuation no longer refers to an active continuation."); - if (continuation->Version != continuationHandle.Version) - return null; - return Continuations.Memory + continuationHandle.Index; - } - - } /// - /// Holds the job queue's continuations data in unmanaged memory just in case the queue itself is in unpinned memory. + /// Holds the task queue's continuations data in unmanaged memory just in case the queue itself is in unpinned memory. /// - Buffer continuationsContainer; + Buffer continuationsContainer; /// - /// Constructs a new job queue. + /// Constructs a new task queue. /// /// Buffer pool to allocate resources from. /// Maximum number of tasks to allocate space for. Tasks are individual chunks of scheduled work. Rounded up to the nearest power of 2. /// Maximum number of continuations to allocate space for. If more continuations exist at any one moment, attempts to create new jobs will have to stall until space is available. - public JobQueue(BufferPool pool, int maximumTaskCapacity = 1024, int maximumContinuationCapacity = 256) + public TaskQueue(BufferPool pool, int maximumTaskCapacity = 1024, int maximumContinuationCapacity = 256) { maximumTaskCapacity = (int)BitOperations.RoundUpToPowerOf2((uint)maximumTaskCapacity); pool.Take(1, out continuationsContainer); @@ -186,6 +250,10 @@ public JobQueue(BufferPool pool, int maximumTaskCapacity = 1024, int maximumCont taskLocker = 0; } + /// + /// Returns unmanaged resources held by the to a pool. + /// + /// Buffer pool to return resources to. public void Dispose(BufferPool pool) { continuationsContainer[0].IndexPool.Dispose(pool); @@ -194,28 +262,35 @@ public void Dispose(BufferPool pool) pool.Return(ref continuationsContainer); } - public DequeueResult TryDequeue(out delegate* function, out void* context, out int taskId) + /// + /// Attempts to dequeue a task. + /// + /// Function associated with the dequeued task, if any. + /// Context pointer associated with the dequeued task, if any. + /// Task id associated with the dequeued task, if any. + /// Result status of the dequeue attempt. + public DequeueTaskResult TryDequeue(out delegate* function, out void* context, out int taskId) { function = default; context = default; taskId = default; if (Interlocked.CompareExchange(ref taskLocker, 1, 0) != 0) - return DequeueResult.Contested; + return DequeueTaskResult.Contested; try { //We have the lock. var nextTaskIndex = taskIndex; if (nextTaskIndex >= writtenTaskIndex) - return DequeueResult.Empty; + return DequeueTaskResult.Empty; var task = tasks[(int)(nextTaskIndex & taskMask)]; if (task.Function == null) - return DequeueResult.Stop; + return DequeueTaskResult.Stop; //There's an actual job! function = task.Function; context = task.Context; taskId = task.TaskId; ++taskIndex; - return DequeueResult.Success; + return DequeueTaskResult.Success; } finally { @@ -223,24 +298,38 @@ public DequeueResult TryDequeue(out delegate* function, o } } - public DequeueResult TryDequeueAndRun(int workerIndex) + /// + /// Attempts to dequeue a task and run it. + /// + /// Index of the worker to pass into the task function. + /// Result status of the dequeue attempt. + public DequeueTaskResult TryDequeueAndRun(int workerIndex) { var result = TryDequeue(out var function, out var context, out var taskId); - if (result == DequeueResult.Success) + if (result == DequeueTaskResult.Success) function(taskId, context, workerIndex); return result; } - public void DequeueAndRun(int workerIndex) + /// + /// Dequeues a task from the queue and runs it. + /// + /// Currently executing worker index to be provided to the function. + /// True if a task was dequeued and run, false if a stop command was enountered. + public bool DequeueAndRun(int workerIndex) { - SpinWait waiter = new SpinWait(); - while (TryDequeueAndRun(workerIndex) != DequeueResult.Success) + var waiter = new SpinWait(); + while (true) { + var result = TryDequeueAndRun(workerIndex); + if (result == DequeueTaskResult.Stop) + return false; + if (result == DequeueTaskResult.Success) + return true; waiter.SpinOnce(); } } - /// /// Checks whether all tasks composing a job, as reported to the continuation, have completed. /// @@ -262,42 +351,22 @@ public bool IsComplete(ContinuationHandle continuationHandle) /// Handle to look up the associated continuation for. /// Pointer to the continuation backing the given handle. /// This should not be used if the continuation handle is not known to be valid. The data pointed to by the data could become invalidated if the continuation completes. - public Continuation* GetContinuation(ContinuationHandle continuationHandle) + public TaskContinuation* GetContinuation(ContinuationHandle continuationHandle) { return continuationsContainer[0].GetContinuation(continuationHandle); } - - /// - /// Describes the result of a task enqueue attempt. - /// - public enum EnqueueTasksResult - { - /// - /// The tasks were successfully enqueued. - /// - Success, - /// - /// The enqueue attempt was blocked by concurrent access. - /// - Contested, - /// - /// The enqueue attempt was blocked because no space remained in the tasks buffer. - /// - Full, - } - /// - /// Tries to appends a set of tasks to the job queue if the ring buffer is uncontested. + /// Tries to appends a set of tasks to the task queue if the ring buffer is uncontested. /// /// Tasks composing the job. /// Result of the enqueue attempt. - public EnqueueTasksResult TryEnqueueTasks(Span tasks) + public EnqueueTaskResult TryEnqueueTasks(Span tasks) { if (tasks.Length == 0) - return EnqueueTasksResult.Success; + return EnqueueTaskResult.Success; if (Interlocked.CompareExchange(ref taskLocker, 1, 0) != 0) - return EnqueueTasksResult.Contested; + return EnqueueTaskResult.Contested; try { //We have the lock. @@ -309,7 +378,7 @@ public EnqueueTasksResult TryEnqueueTasks(Span tasks) if (taskEndIndex - taskIndex > longLength) { //We've run out of space in the ring buffer. If we tried to write, we'd overwrite jobs that haven't yet been completed. - return EnqueueTasksResult.Full; + return EnqueueTaskResult.Full; } //We can actually write the jobs. Debug.Assert(BitOperations.IsPow2(this.tasks.Length)); @@ -336,7 +405,7 @@ public EnqueueTasksResult TryEnqueueTasks(Span tasks) // this.tasks[taskIndex] = tasks[i]; //} Interlocked.Exchange(ref writtenTaskIndex, taskEndIndex); //This is not assuming 64 bits. Mildly goofy considering the rest. - return EnqueueTasksResult.Success; + return EnqueueTaskResult.Success; } finally { @@ -345,13 +414,13 @@ public EnqueueTasksResult TryEnqueueTasks(Span tasks) } /// - /// Appends a task to the job queue if the ring buffer is uncontested. + /// Appends a task to the queue if the ring buffer is uncontested. /// /// Tasks composing the job. public void EnqueueTasks(Span tasks) { SpinWait waiter = new SpinWait(); - while (TryEnqueueTasks(tasks) != EnqueueTasksResult.Success) + while (TryEnqueueTasks(tasks) != EnqueueTaskResult.Success) { waiter.SpinOnce(-1); } @@ -360,7 +429,7 @@ public void EnqueueTasks(Span tasks) /// Tries to enqueues the stop command. /// /// True if the stop could be pushed onto the queue, false otherwise. - public EnqueueTasksResult TryEnqueueStop() + public EnqueueTaskResult TryEnqueueStop() { Span stopJob = stackalloc Task[1]; stopJob[0] = new Task { Function = null }; @@ -375,7 +444,7 @@ public void EnqueueStop() Span stopJob = stackalloc Task[1]; stopJob[0] = new Task { Function = null }; var waiter = new SpinWait(); - while (TryEnqueueTasks(stopJob) != EnqueueTasksResult.Success) + while (TryEnqueueTasks(stopJob) != EnqueueTaskResult.Success) { waiter.SpinOnce(-1); } @@ -399,9 +468,9 @@ public struct WrappedTaskContext /// public ContinuationHandle Continuation; /// - /// Set of continuations in the job queue. + /// Set of continuations in the queue. /// - internal JobQueueContinuations* Continuations; + internal TaskQueueContinuations* Continuations; } /// @@ -464,42 +533,21 @@ static void RunAndMarkAsComplete(int taskId, void* wrapperContextPointer, int wo } } - /// - /// Stores data relevant to tracking task completion and reporting completion for a job. + /// Attempts to allocate a continuation for a set of tasks. /// - public struct Continuation - { - /// - /// Function to call upon completion of the job, if any. - /// - public delegate* OnCompleted; - /// - /// Context to pass to the completion function, if any. - /// - public void* OnCompletedContext; - internal JobQueueContinuations* Continuations; - /// - /// Id provided by the user to identify this job. - /// - public ulong UserId; - /// - /// Version of this continuation. - /// - public int Version; - /// - /// Number of tasks not yet reported as complete in the job. - /// - public int RemainingTaskCounter; - } - - - public ContinuationAllocationResult TryAllocateContinuation(int taskCount, ulong userContinuationId, delegate* onCompleted, void* onCompletedContext, out ContinuationHandle continuationHandle) + /// Number of tasks associated with the continuation. + /// User id to associate with the handle. + /// Function to execute upon completing all associated tasks. + /// Context pointer to pass into the completion function. + /// Handle of the continuation if allocation is successful. + /// Result status of the continuation allocation attempt. + public AllocateTaskContinuationResult TryAllocateContinuation(int taskCount, ulong userContinuationId, delegate* onCompleted, void* onCompletedContext, out ContinuationHandle continuationHandle) { continuationHandle = default; ref var continuations = ref continuationsContainer[0]; if (Interlocked.CompareExchange(ref continuations.Locker, 1, 0) != 0) - return ContinuationAllocationResult.Contested; + return AllocateTaskContinuationResult.Contested; try { //We have the lock. @@ -507,7 +555,7 @@ public ContinuationAllocationResult TryAllocateContinuation(int taskCount, ulong if (continuations.ContinuationCount >= continuations.Continuations.length) { //No room. - return ContinuationAllocationResult.Full; + return AllocateTaskContinuationResult.Full; } var index = continuations.IndexPool.Take(); ref var continuation = ref continuations.Continuations[index]; @@ -518,7 +566,7 @@ public ContinuationAllocationResult TryAllocateContinuation(int taskCount, ulong continuation.Version = newVersion; continuation.RemainingTaskCounter = taskCount; continuationHandle = new ContinuationHandle((uint)index, newVersion); - return ContinuationAllocationResult.Success; + return AllocateTaskContinuationResult.Success; } finally { @@ -526,11 +574,19 @@ public ContinuationAllocationResult TryAllocateContinuation(int taskCount, ulong } } + /// + /// Allocates a continuation for a set of tasks. + /// + /// Number of tasks associated with the continuation. + /// User id to associate with the handle. + /// Function to execute upon completing all associated tasks. + /// Context pointer to pass into the completion function. + /// Handle of the allocated continuation. public ContinuationHandle AllocateContinuation(int taskCount, ulong userContinuationId = 0, delegate* onCompleted = null, void* onCompletedContext = null) { SpinWait waiter = new SpinWait(); ContinuationHandle handle; - while (TryAllocateContinuation(taskCount, userContinuationId, onCompleted, onCompletedContext, out handle) != ContinuationAllocationResult.Success) + while (TryAllocateContinuation(taskCount, userContinuationId, onCompleted, onCompletedContext, out handle) != AllocateTaskContinuationResult.Success) { waiter.SpinOnce(-1); } @@ -565,7 +621,7 @@ public void For(delegate* function, void* context, int in tasks[i] = new Task { Function = &RunAndMarkAsComplete, Context = wrappedTaskContext, TaskId = i + 1 + inclusiveStartIndex }; } var waiter = new SpinWait(); - while (TryEnqueueTasks(tasks) != EnqueueTasksResult.Success) + while (TryEnqueueTasks(tasks) != EnqueueTaskResult.Success) { waiter.SpinOnce(-1); //TODO: We're biting the bullet on yields/sleep(0) here. May not be ideal for the use case; investigate. } @@ -583,12 +639,12 @@ public void For(delegate* function, void* context, int in //Note that we don't handle the DequeueResult.Stop case; if the job isn't complete yet, there's no way to hit a stop unless we enqueued this job after a stop. //Enqueuing after a stop is an error condition and is debug checked for in TryEnqueueJob. var dequeueResult = TryDequeue(out var fillerJob, out var fillerContext, out var fillerTaskId); - if (dequeueResult == DequeueResult.Stop) + if (dequeueResult == DequeueTaskResult.Stop) { - Debug.Assert(dequeueResult != DequeueResult.Stop, "Did you enqueue this for loop *after* some thread enqueued a stop command? That's illegal!"); + Debug.Assert(dequeueResult != DequeueTaskResult.Stop, "Did you enqueue this for loop *after* some thread enqueued a stop command? That's illegal!"); return; } - if (dequeueResult == DequeueResult.Success) + if (dequeueResult == DequeueTaskResult.Success) { fillerJob(fillerTaskId, fillerContext, workerIndex); waiter.Reset(); From f6d10ee185c44d7e2cfe4ae1ac7df29d15b5fac9 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 7 Nov 2022 13:54:06 -0600 Subject: [PATCH 632/947] Multigoofs fixed. --- BepuUtilities/TaskQueue.cs | 7 ++- Demos/DemoSet.cs | 1 + Demos/SpecializedTests/TaskQueueTestDemo.cs | 69 +++++++++++++++++++++ 3 files changed, 74 insertions(+), 3 deletions(-) create mode 100644 Demos/SpecializedTests/TaskQueueTestDemo.cs diff --git a/BepuUtilities/TaskQueue.cs b/BepuUtilities/TaskQueue.cs index ebc810385..3a3581a48 100644 --- a/BepuUtilities/TaskQueue.cs +++ b/BepuUtilities/TaskQueue.cs @@ -165,7 +165,7 @@ internal unsafe struct TaskQueueContinuations public TaskContinuation* GetContinuation(ContinuationHandle continuationHandle) { Debug.Assert(continuationHandle.Initialized, "This continuation handle was never initialized."); - Debug.Assert(continuationHandle.Index >= Continuations.length, "This continuation refers to an invalid index."); + Debug.Assert(continuationHandle.Index < Continuations.length, "This continuation refers to an invalid index."); if (continuationHandle.Index >= Continuations.length || !continuationHandle.Initialized) return null; var continuation = Continuations.Memory + continuationHandle.Index; @@ -338,7 +338,7 @@ public bool DequeueAndRun(int workerIndex) public bool IsComplete(ContinuationHandle continuationHandle) { Debug.Assert(continuationHandle.Initialized, "This continuation handle was never initialized."); - Debug.Assert(continuationHandle.Index >= continuationsContainer[0].Continuations.length, "This continuation refers to an invalid index."); + Debug.Assert(continuationHandle.Index < continuationsContainer[0].Continuations.length, "This continuation refers to an invalid index."); ref var continuationSet = ref continuationsContainer[0]; if (continuationHandle.Index >= continuationSet.Continuations.length || !continuationHandle.Initialized) return false; @@ -370,7 +370,7 @@ public EnqueueTaskResult TryEnqueueTasks(Span tasks) try { //We have the lock. - Debug.Assert(writtenTaskIndex > 0 && this.tasks[(int)((writtenTaskIndex - 1) & taskMask)].Function != null, "No more jobs should be written after a stop command."); + Debug.Assert(writtenTaskIndex == 0 || this.tasks[(int)((writtenTaskIndex - 1) & taskMask)].Function != null, "No more jobs should be written after a stop command."); var taskStartIndex = allocatedTaskIndex; var taskEndIndex = taskStartIndex + tasks.Length; allocatedTaskIndex = taskEndIndex; @@ -528,6 +528,7 @@ static void RunAndMarkAsComplete(int taskId, void* wrapperContextPointer, int wo continuations->IndexPool.ReturnUnsafely((int)continuationHandle.Index); --continuations->ContinuationCount; continuations->Locker = 0; + break; } } } diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index ac9406bde..9a542f8b3 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -46,6 +46,7 @@ struct Option public DemoSet() { + AddOption(); AddOption(); AddOption(); AddOption(); diff --git a/Demos/SpecializedTests/TaskQueueTestDemo.cs b/Demos/SpecializedTests/TaskQueueTestDemo.cs new file mode 100644 index 000000000..e1e576d24 --- /dev/null +++ b/Demos/SpecializedTests/TaskQueueTestDemo.cs @@ -0,0 +1,69 @@ +using BepuUtilities; +using BepuUtilities.Collections; +using BepuUtilities.Memory; +using BepuPhysics.CollisionDetection; +using System; +using System.Collections.Generic; +using System.Diagnostics; +using System.Numerics; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; +using System.Text; +using BepuPhysics.Trees; +using DemoContentLoader; +using DemoRenderer; +using BepuPhysics; +using BepuPhysics.Constraints; +using BepuPhysics.Collidables; +using static System.Formats.Asn1.AsnWriter; +using static OpenTK.Graphics.OpenGL.GL; +using System.Xml.Linq; +using System.Threading; + +namespace Demos.SpecializedTests +{ + public unsafe class TaskQueueTestDemo : Demo + { + static void Test(int taskId, void* context, int workerIndex) + { + + } + + public override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(-10, 3, -10); + camera.Yaw = MathHelper.Pi * 3f / 4; + camera.Pitch = 0; + + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(4, 1)); + + var taskQueue = new TaskQueue(BufferPool); + + taskQueue.For(&Test, null, 0, 100, 0); + + taskQueue.Dispose(BufferPool); + + } + + delegate int TestFunction(); + + static void Test(TestFunction function, string name) + { + long accumulatedTime = 0; + const int testCount = 16; + int accumulator = 0; + for (int i = 0; i < testCount; ++i) + { + var startTime = Stopwatch.GetTimestamp(); + accumulator += function(); + var endTime = Stopwatch.GetTimestamp(); + accumulatedTime += endTime - startTime; + //overlapHandler.Set.Clear(); + CacheBlaster.Blast(); + } + Console.WriteLine($"{name} time per execution (ms): {(accumulatedTime) * 1e3 / (testCount * Stopwatch.Frequency)}, acc: {accumulator}"); + } + + + } +} From b21666ae469c4088f5b6f2e8faa58cbc01105621 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 7 Nov 2022 16:16:06 -0600 Subject: [PATCH 633/947] Test demo improved; added fallback to forloop for full queue. --- BepuUtilities/TaskQueue.cs | 40 +++++++- Demos/SpecializedTests/TaskQueueTestDemo.cs | 107 ++++++++++++-------- 2 files changed, 104 insertions(+), 43 deletions(-) diff --git a/BepuUtilities/TaskQueue.cs b/BepuUtilities/TaskQueue.cs index 3a3581a48..f65ed633c 100644 --- a/BepuUtilities/TaskQueue.cs +++ b/BepuUtilities/TaskQueue.cs @@ -417,6 +417,7 @@ public EnqueueTaskResult TryEnqueueTasks(Span tasks) /// Appends a task to the queue if the ring buffer is uncontested. /// /// Tasks composing the job. + /// Note that this will spin until task submission succeeds. If something is blocking task submission, such as insufficient room in the tasks buffer and there are no workers consuming tasks, this will block forever. public void EnqueueTasks(Span tasks) { SpinWait waiter = new SpinWait(); @@ -439,6 +440,7 @@ public EnqueueTaskResult TryEnqueueStop() /// /// Enqueues the stop command. /// + /// Note that this will spin until task submission succeeds. If something is blocking task submission, such as insufficient room in the tasks buffer and there are no workers consuming tasks, this will block forever. public void EnqueueStop() { Span stopJob = stackalloc Task[1]; @@ -583,9 +585,10 @@ public AllocateTaskContinuationResult TryAllocateContinuation(int taskCount, ulo /// Function to execute upon completing all associated tasks. /// Context pointer to pass into the completion function. /// Handle of the allocated continuation. + /// Note that this will spin until allocation succeeds. If something is blocking allocation, such as insufficient room in the continuations buffer and there are no workers consuming tasks, this will block forever. public ContinuationHandle AllocateContinuation(int taskCount, ulong userContinuationId = 0, delegate* onCompleted = null, void* onCompletedContext = null) { - SpinWait waiter = new SpinWait(); + var waiter = new SpinWait(); ContinuationHandle handle; while (TryAllocateContinuation(taskCount, userContinuationId, onCompleted, onCompletedContext, out handle) != AllocateTaskContinuationResult.Success) { @@ -594,6 +597,26 @@ public ContinuationHandle AllocateContinuation(int taskCount, ulong userContinua return handle; } + /// + /// Enqueues a for loop onto the task queue and returns. + /// + /// Function to execute on each iteration of the loop. + /// Context pointer to pass into each task execution. + /// Inclusive start index of the loop range. + /// Exclusive end index of the loop range. + /// This function will not attempt to run any iterations of the loop itself. It only pushes the loop tasks onto the queue. + /// Note that this will spin until loop task submission succeeds. If something is blocking task submission, such as insufficient room in the tasks buffer and there are no workers consuming tasks, this will block forever. + public void EnqueueFor(delegate* function, void* context, int inclusiveStartIndex, int exclusiveEndIndex) + { + var taskCount = exclusiveEndIndex - inclusiveStartIndex; + Span tasks = stackalloc Task[taskCount]; + for (int i = 0; i < tasks.Length; ++i) + { + tasks[i] = new Task { Function = function, Context = context, TaskId = i + inclusiveStartIndex }; + } + EnqueueTasks(tasks); + } + /// /// Submits a set of tasks representing a for loop over the given indices and returns when all loop iterations are complete. /// @@ -622,9 +645,20 @@ public void For(delegate* function, void* context, int in tasks[i] = new Task { Function = &RunAndMarkAsComplete, Context = wrappedTaskContext, TaskId = i + 1 + inclusiveStartIndex }; } var waiter = new SpinWait(); - while (TryEnqueueTasks(tasks) != EnqueueTaskResult.Success) + EnqueueTaskResult result; + while ((result = TryEnqueueTasks(tasks)) != EnqueueTaskResult.Success) { - waiter.SpinOnce(-1); //TODO: We're biting the bullet on yields/sleep(0) here. May not be ideal for the use case; investigate. + if (result == EnqueueTaskResult.Full) + { + //If the task buffer is full, just execute the task locally. Clearly there's enough work for other threads to keep running productively. + var task = tasks[0]; + task.Function(task.TaskId, task.Context, workerIndex); + tasks = tasks.Slice(1); + } + else + { + waiter.SpinOnce(-1); //TODO: We're biting the bullet on yields/sleep(0) here. May not be ideal for the use case; investigate + } } } //Tasks [1, count) are submitted to the queue and may now be executing on other workers. diff --git a/Demos/SpecializedTests/TaskQueueTestDemo.cs b/Demos/SpecializedTests/TaskQueueTestDemo.cs index e1e576d24..fa3372f99 100644 --- a/Demos/SpecializedTests/TaskQueueTestDemo.cs +++ b/Demos/SpecializedTests/TaskQueueTestDemo.cs @@ -1,69 +1,96 @@ using BepuUtilities; -using BepuUtilities.Collections; -using BepuUtilities.Memory; -using BepuPhysics.CollisionDetection; using System; -using System.Collections.Generic; using System.Diagnostics; using System.Numerics; -using System.Runtime.CompilerServices; -using System.Runtime.InteropServices; -using System.Text; -using BepuPhysics.Trees; using DemoContentLoader; using DemoRenderer; using BepuPhysics; using BepuPhysics.Constraints; -using BepuPhysics.Collidables; -using static System.Formats.Asn1.AsnWriter; -using static OpenTK.Graphics.OpenGL.GL; -using System.Xml.Linq; using System.Threading; -namespace Demos.SpecializedTests +namespace Demos.SpecializedTests; + +public unsafe class TaskQueueTestDemo : Demo { - public unsafe class TaskQueueTestDemo : Demo + static void Test(int taskId, void* context, int workerIndex) { - static void Test(int taskId, void* context, int workerIndex) + int sum = 0; + for (int i = 0; i < 100000; ++i) { - + sum = (sum ^ i) * i; } + var typedContext = (Context*)context; + Interlocked.Add(ref typedContext->Sum, sum); + } - public override void Initialize(ContentArchive content, Camera camera) - { - camera.Position = new Vector3(-10, 3, -10); - camera.Yaw = MathHelper.Pi * 3f / 4; - camera.Pitch = 0; + static void DispatcherBody(int workerIndex, void* context) + { + var taskQueue = (TaskQueue*)context; + while (taskQueue->DequeueAndRun(workerIndex)) ; + } - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(4, 1)); + struct Context + { + public int Sum; + } - var taskQueue = new TaskQueue(BufferPool); + public override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(-10, 3, -10); + camera.Yaw = MathHelper.Pi * 3f / 4; + camera.Pitch = 0; + + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(4, 1)); - taskQueue.For(&Test, null, 0, 100, 0); - taskQueue.Dispose(BufferPool); - } - delegate int TestFunction(); + int iterationCount = 10; + int tasksPerIteration = 16; + Test(() => + { + var context = new Context { }; + var taskQueue = new TaskQueue(BufferPool); + for (int i = 0; i < iterationCount; ++i) + taskQueue.EnqueueFor(&Test, &context, 0, tasksPerIteration); + taskQueue.EnqueueStop(); + ThreadDispatcher.DispatchWorkers(&DispatcherBody, &taskQueue); + + taskQueue.Dispose(BufferPool); + return context.Sum; + }, "MT"); - static void Test(TestFunction function, string name) + + Test(() => { - long accumulatedTime = 0; - const int testCount = 16; - int accumulator = 0; - for (int i = 0; i < testCount; ++i) + var testContext = new Context { }; + for (int i = 0; i < iterationCount * tasksPerIteration; ++i) { - var startTime = Stopwatch.GetTimestamp(); - accumulator += function(); - var endTime = Stopwatch.GetTimestamp(); - accumulatedTime += endTime - startTime; - //overlapHandler.Set.Clear(); - CacheBlaster.Blast(); + Test(0, &testContext, 0); } - Console.WriteLine($"{name} time per execution (ms): {(accumulatedTime) * 1e3 / (testCount * Stopwatch.Frequency)}, acc: {accumulator}"); - } + return testContext.Sum; + }, "ST"); + } + delegate int TestFunction(); + + static void Test(TestFunction function, string name) + { + long accumulatedTime = 0; + const int testCount = 16; + int accumulator = 0; + for (int i = 0; i < testCount; ++i) + { + var startTime = Stopwatch.GetTimestamp(); + accumulator += function(); + var endTime = Stopwatch.GetTimestamp(); + accumulatedTime += endTime - startTime; + //overlapHandler.Set.Clear(); + CacheBlaster.Blast(); + } + Console.WriteLine($"{name} time per execution (ms): {(accumulatedTime) * 1e3 / (testCount * Stopwatch.Frequency)}, acc: {accumulator}"); } + + } From 10e260392ea2ec377e3bb20027b8a9507a6bff6f Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 7 Nov 2022 17:26:17 -0600 Subject: [PATCH 634/947] All enqueue-related blocking calls now will attempt to execute tasks if the buffer is full. --- BepuUtilities/TaskQueue.cs | 84 +++++++++++++++------ Demos/SpecializedTests/TaskQueueTestDemo.cs | 4 +- 2 files changed, 64 insertions(+), 24 deletions(-) diff --git a/BepuUtilities/TaskQueue.cs b/BepuUtilities/TaskQueue.cs index f65ed633c..915babed3 100644 --- a/BepuUtilities/TaskQueue.cs +++ b/BepuUtilities/TaskQueue.cs @@ -231,7 +231,7 @@ public unsafe struct TaskQueue /// /// Buffer pool to allocate resources from. /// Maximum number of tasks to allocate space for. Tasks are individual chunks of scheduled work. Rounded up to the nearest power of 2. - /// Maximum number of continuations to allocate space for. If more continuations exist at any one moment, attempts to create new jobs will have to stall until space is available. + /// Maximum number of continuations to allocate space for. If more continuations exist at any one moment, attempts to create new continuations may have to stall until space is available. public TaskQueue(BufferPool pool, int maximumTaskCapacity = 1024, int maximumContinuationCapacity = 256) { maximumTaskCapacity = (int)BitOperations.RoundUpToPowerOf2((uint)maximumTaskCapacity); @@ -315,7 +315,7 @@ public DequeueTaskResult TryDequeueAndRun(int workerIndex) /// Dequeues a task from the queue and runs it. /// /// Currently executing worker index to be provided to the function. - /// True if a task was dequeued and run, false if a stop command was enountered. + /// True if a task was dequeued and run, false if a stop command was encountered. public bool DequeueAndRun(int workerIndex) { var waiter = new SpinWait(); @@ -414,16 +414,32 @@ public EnqueueTaskResult TryEnqueueTasks(Span tasks) } /// - /// Appends a task to the queue if the ring buffer is uncontested. + /// Appends a set of tasks to the queue. /// /// Tasks composing the job. - /// Note that this will spin until task submission succeeds. If something is blocking task submission, such as insufficient room in the tasks buffer and there are no workers consuming tasks, this will block forever. - public void EnqueueTasks(Span tasks) + /// Worker index to pass to inline-executed tasks if the task buffer is full. + /// Note that this will keep trying until task submission succeeds. + /// If the task queue is full, this will opt to run some tasks inline while waiting for room. + public void EnqueueTasks(Span tasks, int workerIndex) { - SpinWait waiter = new SpinWait(); - while (TryEnqueueTasks(tasks) != EnqueueTaskResult.Success) + var waiter = new SpinWait(); + EnqueueTaskResult result; + while ((result = TryEnqueueTasks(tasks)) != EnqueueTaskResult.Success) { - waiter.SpinOnce(-1); + if (result == EnqueueTaskResult.Full) + { + //Couldn't enqueue the tasks because the task buffer is full. + //Clearly there's plenty of work available to execute, so go ahead and try to run one task inline. + var task = tasks[0]; + task.Function(task.TaskId, task.Context, workerIndex); + if (tasks.Length == 1) + break; + tasks = tasks[1..]; + } + else + { + waiter.SpinOnce(-1); + } } } /// @@ -440,15 +456,26 @@ public EnqueueTaskResult TryEnqueueStop() /// /// Enqueues the stop command. /// - /// Note that this will spin until task submission succeeds. If something is blocking task submission, such as insufficient room in the tasks buffer and there are no workers consuming tasks, this will block forever. - public void EnqueueStop() + /// Worker index to pass to any inline executed tasks if the task buffer is full. + /// Note that this will keep trying until task submission succeeds. + /// If the task buffer is full, this may attempt to dequeue tasks to run inline. + public void EnqueueStop(int workerIndex) { Span stopJob = stackalloc Task[1]; stopJob[0] = new Task { Function = null }; var waiter = new SpinWait(); - while (TryEnqueueTasks(stopJob) != EnqueueTaskResult.Success) + EnqueueTaskResult result; + while ((result = TryEnqueueTasks(stopJob)) != EnqueueTaskResult.Success) { - waiter.SpinOnce(-1); + if (result == EnqueueTaskResult.Full) + { + var dequeueResult = TryDequeueAndRun(workerIndex); + Debug.Assert(dequeueResult != DequeueTaskResult.Stop, "We're trying to enqueue a stop, we shouldn't have found one already present!"); + } + else + { + waiter.SpinOnce(-1); + } } } @@ -581,18 +608,28 @@ public AllocateTaskContinuationResult TryAllocateContinuation(int taskCount, ulo /// Allocates a continuation for a set of tasks. /// /// Number of tasks associated with the continuation. + /// Worker index to pass to any inline executed tasks if the continuations buffer is full. /// User id to associate with the handle. /// Function to execute upon completing all associated tasks. /// Context pointer to pass into the completion function. /// Handle of the allocated continuation. - /// Note that this will spin until allocation succeeds. If something is blocking allocation, such as insufficient room in the continuations buffer and there are no workers consuming tasks, this will block forever. - public ContinuationHandle AllocateContinuation(int taskCount, ulong userContinuationId = 0, delegate* onCompleted = null, void* onCompletedContext = null) + /// Note that this will keep trying until allocation succeeds. If something is blocking allocation, such as insufficient room in the continuations buffer and there are no workers consuming tasks, this will block forever. + public ContinuationHandle AllocateContinuation(int taskCount, int workerIndex, ulong userContinuationId = 0, delegate* onCompleted = null, void* onCompletedContext = null) { var waiter = new SpinWait(); ContinuationHandle handle; - while (TryAllocateContinuation(taskCount, userContinuationId, onCompleted, onCompletedContext, out handle) != AllocateTaskContinuationResult.Success) + AllocateTaskContinuationResult result; + while ((result = TryAllocateContinuation(taskCount, userContinuationId, onCompleted, onCompletedContext, out handle)) != AllocateTaskContinuationResult.Success) { - waiter.SpinOnce(-1); + if (result == AllocateTaskContinuationResult.Full) + { + var dequeueResult = TryDequeueAndRun(workerIndex); + Debug.Assert(dequeueResult != DequeueTaskResult.Stop, "We're trying to allocate a continuation, we shouldn't have run into a stop command!"); + } + else + { + waiter.SpinOnce(-1); + } } return handle; } @@ -604,9 +641,10 @@ public ContinuationHandle AllocateContinuation(int taskCount, ulong userContinua /// Context pointer to pass into each task execution. /// Inclusive start index of the loop range. /// Exclusive end index of the loop range. - /// This function will not attempt to run any iterations of the loop itself. It only pushes the loop tasks onto the queue. - /// Note that this will spin until loop task submission succeeds. If something is blocking task submission, such as insufficient room in the tasks buffer and there are no workers consuming tasks, this will block forever. - public void EnqueueFor(delegate* function, void* context, int inclusiveStartIndex, int exclusiveEndIndex) + /// Worker index to pass to any inline-executed task if the task queue is full. + /// This function will not usually attempt to run any iterations of the loop itself. It tries to push the loop tasks onto the queue. + /// If the task queue is full, this will opt to run the tasks inline while waiting for room. + public void EnqueueFor(delegate* function, void* context, int inclusiveStartIndex, int exclusiveEndIndex, int workerIndex) { var taskCount = exclusiveEndIndex - inclusiveStartIndex; Span tasks = stackalloc Task[taskCount]; @@ -614,7 +652,7 @@ public void EnqueueFor(delegate* function, void* context, { tasks[i] = new Task { Function = function, Context = context, TaskId = i + inclusiveStartIndex }; } - EnqueueTasks(tasks); + EnqueueTasks(tasks, workerIndex); } /// @@ -637,7 +675,7 @@ public void For(delegate* function, void* context, int in var taskCount = iterationCount - 1; WrappedTaskContext* wrappedContexts = stackalloc WrappedTaskContext[taskCount]; Span tasks = stackalloc Task[taskCount]; - continuationHandle = AllocateContinuation(taskCount); + continuationHandle = AllocateContinuation(taskCount, workerIndex); for (int i = 0; i < tasks.Length; ++i) { var wrappedTaskContext = wrappedContexts + i; @@ -653,7 +691,9 @@ public void For(delegate* function, void* context, int in //If the task buffer is full, just execute the task locally. Clearly there's enough work for other threads to keep running productively. var task = tasks[0]; task.Function(task.TaskId, task.Context, workerIndex); - tasks = tasks.Slice(1); + if (tasks.Length == 1) + break; + tasks = tasks[1..]; } else { diff --git a/Demos/SpecializedTests/TaskQueueTestDemo.cs b/Demos/SpecializedTests/TaskQueueTestDemo.cs index fa3372f99..db1a703fb 100644 --- a/Demos/SpecializedTests/TaskQueueTestDemo.cs +++ b/Demos/SpecializedTests/TaskQueueTestDemo.cs @@ -52,8 +52,8 @@ public override void Initialize(ContentArchive content, Camera camera) var context = new Context { }; var taskQueue = new TaskQueue(BufferPool); for (int i = 0; i < iterationCount; ++i) - taskQueue.EnqueueFor(&Test, &context, 0, tasksPerIteration); - taskQueue.EnqueueStop(); + taskQueue.EnqueueFor(&Test, &context, 0, tasksPerIteration, 0); + taskQueue.EnqueueStop(0); ThreadDispatcher.DispatchWorkers(&DispatcherBody, &taskQueue); taskQueue.Dispose(BufferPool); From 889c3b4a9ae0004c3e9472d56a62c2f44ea107c9 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 7 Nov 2022 18:27:13 -0600 Subject: [PATCH 635/947] Filling out task queue. Some performance oddities. --- BepuUtilities/TaskQueue.cs | 218 +++++++++++++++----- Demos/SpecializedTests/TaskQueueTestDemo.cs | 28 +-- 2 files changed, 177 insertions(+), 69 deletions(-) diff --git a/BepuUtilities/TaskQueue.cs b/BepuUtilities/TaskQueue.cs index 915babed3..18c608f88 100644 --- a/BepuUtilities/TaskQueue.cs +++ b/BepuUtilities/TaskQueue.cs @@ -3,6 +3,7 @@ using System.ComponentModel; using System.Diagnostics; using System.Diagnostics.CodeAnalysis; +using System.Dynamic; using System.Linq; using System.Numerics; using System.Reflection.Metadata; @@ -176,6 +177,29 @@ internal unsafe struct TaskQueueContinuations } } +/// +/// Wraps a task for easier use with continuations. +/// +public unsafe struct WrappedTaskContext +{ + /// + /// Function to be invoked by this wrapped tsak. + /// + public delegate* Function; + /// + /// Context to be passed to this wrapped task. + /// + public void* Context; + /// + /// Handle of the continuation associated with this wrapped task. + /// + public ContinuationHandle Continuation; + /// + /// Set of continuations in the queue. + /// + internal TaskQueueContinuations* Continuations; +} + /// /// Stores data relevant to tracking task completion and reporting completion for a job. /// @@ -238,16 +262,31 @@ public TaskQueue(BufferPool pool, int maximumTaskCapacity = 1024, int maximumCon pool.Take(1, out continuationsContainer); ref var continuations = ref continuationsContainer[0]; pool.Take(maximumContinuationCapacity, out continuations.Continuations); - continuations.Continuations.Clear(0, continuations.Continuations.Length); continuations.IndexPool = new IdPool(maximumContinuationCapacity, pool); continuations.ContinuationCount = 0; continuations.Locker = 0; pool.Take(maximumTaskCapacity, out tasks); - tasks.Clear(0, tasks.Length); taskMask = tasks.length - 1; taskShift = BitOperations.TrailingZeroCount(tasks.length); taskLocker = 0; + Reset(); + } + + /// + /// Returns the task queue to a fresh state without reallocating. + /// + public void Reset() + { + taskIndex = 0; + allocatedTaskIndex = 0; + writtenTaskIndex = 0; + Debug.Assert(taskLocker == 0, "There appears to be a thread actively working still. That's invalid."); + + ref var continuations = ref continuationsContainer[0]; + continuations.Continuations.Clear(0, continuations.Continuations.Length); + continuations.ContinuationCount = 0; + Debug.Assert(continuations.Locker == 0, "There appears to be a thread actively working still. That's invalid."); } /// @@ -262,6 +301,41 @@ public void Dispose(BufferPool pool) pool.Return(ref continuationsContainer); } + /// + /// Gets the queue's capacity for tasks. + /// + public int TaskCapacity => tasks.length; + /// + /// Gets the queue's capacity for continuations. + /// + public int ContinuationCapacity => continuationsContainer[0].Continuations.length; + /// + /// Gets the number of tasks active in the queue. + /// + /// Does not take a lock; if other threads are modifying the task values, the reported count may be invalid. + public int UnsafeTaskCount => (int)(writtenTaskIndex - taskIndex); + /// + /// Gets the number of tasks active in the queue. + /// + public int TaskCount + { + get + { + var waiter = new SpinWait(); + while (Interlocked.CompareExchange(ref taskLocker, 1, 0) != 0) + { + waiter.SpinOnce(); + } + var result = (int)(writtenTaskIndex - taskIndex); + taskLocker = 0; + return result; + } + } + /// + /// Gets the number of continuations active in the queue. + /// + public int ContinuationCount => continuationsContainer[0].ContinuationCount; + /// /// Attempts to dequeue a task. /// @@ -356,6 +430,59 @@ public bool IsComplete(ContinuationHandle continuationHandle) return continuationsContainer[0].GetContinuation(continuationHandle); } + EnqueueTaskResult TryEnqueueTasksUnsafelyInternal(Span tasks, out long taskEndIndex) + { + Debug.Assert(tasks.Length > 0, "Probably shouldn't be trying to enqueue zero tasks."); + Debug.Assert(writtenTaskIndex == 0 || this.tasks[(int)((writtenTaskIndex - 1) & taskMask)].Function != null, "No more jobs should be written after a stop command."); + var taskStartIndex = allocatedTaskIndex; + taskEndIndex = taskStartIndex + tasks.Length; + if (taskEndIndex - taskIndex > this.tasks.length) + { + //We've run out of space in the ring buffer. If we tried to write, we'd overwrite jobs that haven't yet been completed. + return EnqueueTaskResult.Full; + } + allocatedTaskIndex = taskEndIndex; + Debug.Assert(BitOperations.IsPow2(this.tasks.Length)); + var wrappedInclusiveStartIndex = (int)(taskStartIndex & taskMask); + var wrappedInclusiveEndIndex = (int)(taskEndIndex & taskMask); + if (wrappedInclusiveEndIndex > wrappedInclusiveStartIndex) + { + //We can just copy the whole task block as one blob. + Unsafe.CopyBlockUnaligned(ref *(byte*)(this.tasks.Memory + taskStartIndex), ref Unsafe.As(ref MemoryMarshal.GetReference(tasks)), (uint)(Unsafe.SizeOf() * tasks.Length)); + } + else + { + //Copy the task block as two blobs. + ref var startTask = ref tasks[0]; + var firstRegionCount = this.tasks.length - wrappedInclusiveStartIndex; + ref var secondBlobStartTask = ref tasks[firstRegionCount]; + var secondRegionCount = tasks.Length - firstRegionCount; + Unsafe.CopyBlockUnaligned(ref *(byte*)(this.tasks.Memory + taskStartIndex), ref Unsafe.As(ref startTask), (uint)(Unsafe.SizeOf() * firstRegionCount)); + Unsafe.CopyBlockUnaligned(ref *(byte*)this.tasks.Memory, ref Unsafe.As(ref secondBlobStartTask), (uint)(Unsafe.SizeOf() * secondRegionCount)); + } + //for (int i = 0; i < tasks.Length; ++i) + //{ + // var taskIndex = (int)((i + taskStartIndex) & taskMask); + // this.tasks[taskIndex] = tasks[i]; + //} + return EnqueueTaskResult.Success; + } + /// + /// Tries to appends a set of tasks to the task queue. Does not acquire a lock; cannot return . + /// + /// Tasks composing the job. + /// Result of the enqueue attempt. + /// This must not be used while other threads could be performing task enqueues or task dequeues. + public EnqueueTaskResult TryEnqueueTasksUnsafely(Span tasks) + { + EnqueueTaskResult result; + if ((result = TryEnqueueTasksUnsafelyInternal(tasks, out var taskEndIndex)) == EnqueueTaskResult.Success) + { + writtenTaskIndex = taskEndIndex; + } + return result; + } + /// /// Tries to appends a set of tasks to the task queue if the ring buffer is uncontested. /// @@ -370,42 +497,12 @@ public EnqueueTaskResult TryEnqueueTasks(Span tasks) try { //We have the lock. - Debug.Assert(writtenTaskIndex == 0 || this.tasks[(int)((writtenTaskIndex - 1) & taskMask)].Function != null, "No more jobs should be written after a stop command."); - var taskStartIndex = allocatedTaskIndex; - var taskEndIndex = taskStartIndex + tasks.Length; - allocatedTaskIndex = taskEndIndex; - var longLength = (long)this.tasks.length; - if (taskEndIndex - taskIndex > longLength) - { - //We've run out of space in the ring buffer. If we tried to write, we'd overwrite jobs that haven't yet been completed. - return EnqueueTaskResult.Full; - } - //We can actually write the jobs. - Debug.Assert(BitOperations.IsPow2(this.tasks.Length)); - var wrappedInclusiveStartIndex = (int)(taskStartIndex & taskMask); - var wrappedInclusiveEndIndex = (int)(taskEndIndex & taskMask); - if (wrappedInclusiveEndIndex > wrappedInclusiveStartIndex) - { - //We can just copy the whole task block as one blob. - Unsafe.CopyBlockUnaligned(ref *(byte*)(this.tasks.Memory + taskStartIndex), ref Unsafe.As(ref MemoryMarshal.GetReference(tasks)), (uint)(Unsafe.SizeOf() * tasks.Length)); - } - else + EnqueueTaskResult result; + if ((result = TryEnqueueTasksUnsafelyInternal(tasks, out var taskEndIndex)) == EnqueueTaskResult.Success) { - //Copy the task block as two blobs. - ref var startTask = ref tasks[0]; - var firstRegionCount = this.tasks.length - wrappedInclusiveStartIndex; - ref var secondBlobStartTask = ref tasks[firstRegionCount]; - var secondRegionCount = tasks.Length - firstRegionCount; - Unsafe.CopyBlockUnaligned(ref *(byte*)(this.tasks.Memory + taskStartIndex), ref Unsafe.As(ref startTask), (uint)(Unsafe.SizeOf() * firstRegionCount)); - Unsafe.CopyBlockUnaligned(ref *(byte*)this.tasks.Memory, ref Unsafe.As(ref secondBlobStartTask), (uint)(Unsafe.SizeOf() * secondRegionCount)); + Interlocked.Exchange(ref writtenTaskIndex, taskEndIndex); //This is not assuming 64 bits. Mildly goofy considering the rest. } - //for (int i = 0; i < tasks.Length; ++i) - //{ - // var taskIndex = (int)((i + taskStartIndex) & taskMask); - // this.tasks[taskIndex] = tasks[i]; - //} - Interlocked.Exchange(ref writtenTaskIndex, taskEndIndex); //This is not assuming 64 bits. Mildly goofy considering the rest. - return EnqueueTaskResult.Success; + return result; } finally { @@ -445,7 +542,7 @@ public void EnqueueTasks(Span tasks, int workerIndex) /// /// Tries to enqueues the stop command. /// - /// True if the stop could be pushed onto the queue, false otherwise. + /// Result status of the enqueue attempt. public EnqueueTaskResult TryEnqueueStop() { Span stopJob = stackalloc Task[1]; @@ -480,26 +577,15 @@ public void EnqueueStop(int workerIndex) } /// - /// Wraps a task for easier use with continuations. + /// Tries to enqueues the stop command. Does not take a lock; cannot return . /// - public struct WrappedTaskContext + /// Result status of the enqueue attempt. + /// This must not be used while other threads could be performing task enqueues or task dequeues. + public EnqueueTaskResult TryEnqueueStopUnsafely() { - /// - /// Function to be invoked by this wrapped tsak. - /// - public delegate* Function; - /// - /// Context to be passed to this wrapped task. - /// - public void* Context; - /// - /// Handle of the continuation associated with this wrapped task. - /// - public ContinuationHandle Continuation; - /// - /// Set of continuations in the queue. - /// - internal TaskQueueContinuations* Continuations; + Span stopJob = stackalloc Task[1]; + stopJob[0] = new Task { Function = null }; + return TryEnqueueTasksUnsafely(stopJob); } /// @@ -635,7 +721,27 @@ public ContinuationHandle AllocateContinuation(int taskCount, int workerIndex, u } /// - /// Enqueues a for loop onto the task queue and returns. + /// Enqueues a for loop onto the task queue. Does not take a lock; cannot return . + /// + /// Function to execute on each iteration of the loop. + /// Context pointer to pass into each task execution. + /// Inclusive start index of the loop range. + /// Exclusive end index of the loop range. + /// Status result of the enqueue operation. + /// This must not be used while other threads could be performing task enqueues or task dequeues. + public EnqueueTaskResult TryEnqueueForUnsafely(delegate* function, void* context, int inclusiveStartIndex, int exclusiveEndIndex) + { + var taskCount = exclusiveEndIndex - inclusiveStartIndex; + Span tasks = stackalloc Task[taskCount]; + for (int i = 0; i < tasks.Length; ++i) + { + tasks[i] = new Task { Function = function, Context = context, TaskId = i + inclusiveStartIndex }; + } + return TryEnqueueTasksUnsafely(tasks); + } + + /// + /// Enqueues a for loop onto the task queue. /// /// Function to execute on each iteration of the loop. /// Context pointer to pass into each task execution. diff --git a/Demos/SpecializedTests/TaskQueueTestDemo.cs b/Demos/SpecializedTests/TaskQueueTestDemo.cs index db1a703fb..5d84235f3 100644 --- a/Demos/SpecializedTests/TaskQueueTestDemo.cs +++ b/Demos/SpecializedTests/TaskQueueTestDemo.cs @@ -15,12 +15,13 @@ public unsafe class TaskQueueTestDemo : Demo static void Test(int taskId, void* context, int workerIndex) { int sum = 0; - for (int i = 0; i < 100000; ++i) + for (int i = 0; i < 1000; ++i) { sum = (sum ^ i) * i; } var typedContext = (Context*)context; - Interlocked.Add(ref typedContext->Sum, sum); + if (sum == int.MaxValue) + Interlocked.Add(ref typedContext->Sum, sum); } static void DispatcherBody(int workerIndex, void* context) @@ -45,21 +46,21 @@ public override void Initialize(ContentArchive content, Camera camera) - int iterationCount = 10; - int tasksPerIteration = 16; + int iterationCount = 128; + int tasksPerIteration = 4; + var taskQueue = new TaskQueue(BufferPool); + var taskQueuePointer = &taskQueue; Test(() => { var context = new Context { }; - var taskQueue = new TaskQueue(BufferPool); for (int i = 0; i < iterationCount; ++i) - taskQueue.EnqueueFor(&Test, &context, 0, tasksPerIteration, 0); - taskQueue.EnqueueStop(0); - ThreadDispatcher.DispatchWorkers(&DispatcherBody, &taskQueue); - - taskQueue.Dispose(BufferPool); + taskQueuePointer->TryEnqueueForUnsafely(&Test, &context, 0, tasksPerIteration); + taskQueuePointer->TryEnqueueStopUnsafely(); + ThreadDispatcher.DispatchWorkers(&DispatcherBody, taskQueuePointer); return context.Sum; - }, "MT"); + }, "MT", () => taskQueuePointer->Reset()); + taskQueue.Dispose(BufferPool); Test(() => { @@ -75,16 +76,17 @@ public override void Initialize(ContentArchive content, Camera camera) delegate int TestFunction(); - static void Test(TestFunction function, string name) + static void Test(TestFunction function, string name, Action reset = null) { long accumulatedTime = 0; - const int testCount = 16; + const int testCount = 128; int accumulator = 0; for (int i = 0; i < testCount; ++i) { var startTime = Stopwatch.GetTimestamp(); accumulator += function(); var endTime = Stopwatch.GetTimestamp(); + reset?.Invoke(); accumulatedTime += endTime - startTime; //overlapHandler.Set.Clear(); CacheBlaster.Blast(); From f348777452186029597a3903c1b1d1438c6446cb Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 7 Nov 2022 18:37:13 -0600 Subject: [PATCH 636/947] Got rid of accidental evil Sleep(1). --- BepuUtilities/TaskQueue.cs | 4 ++-- Demos/SpecializedTests/TaskQueueTestDemo.cs | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/BepuUtilities/TaskQueue.cs b/BepuUtilities/TaskQueue.cs index 18c608f88..466ede1b7 100644 --- a/BepuUtilities/TaskQueue.cs +++ b/BepuUtilities/TaskQueue.cs @@ -324,7 +324,7 @@ public int TaskCount var waiter = new SpinWait(); while (Interlocked.CompareExchange(ref taskLocker, 1, 0) != 0) { - waiter.SpinOnce(); + waiter.SpinOnce(-1); } var result = (int)(writtenTaskIndex - taskIndex); taskLocker = 0; @@ -400,7 +400,7 @@ public bool DequeueAndRun(int workerIndex) return false; if (result == DequeueTaskResult.Success) return true; - waiter.SpinOnce(); + waiter.SpinOnce(-1); } } diff --git a/Demos/SpecializedTests/TaskQueueTestDemo.cs b/Demos/SpecializedTests/TaskQueueTestDemo.cs index 5d84235f3..128c3b6c6 100644 --- a/Demos/SpecializedTests/TaskQueueTestDemo.cs +++ b/Demos/SpecializedTests/TaskQueueTestDemo.cs @@ -15,7 +15,7 @@ public unsafe class TaskQueueTestDemo : Demo static void Test(int taskId, void* context, int workerIndex) { int sum = 0; - for (int i = 0; i < 1000; ++i) + for (int i = 0; i < 100000; ++i) { sum = (sum ^ i) * i; } @@ -79,7 +79,7 @@ public override void Initialize(ContentArchive content, Camera camera) static void Test(TestFunction function, string name, Action reset = null) { long accumulatedTime = 0; - const int testCount = 128; + const int testCount = 64; int accumulator = 0; for (int i = 0; i < testCount; ++i) { @@ -89,7 +89,7 @@ static void Test(TestFunction function, string name, Action reset = null) reset?.Invoke(); accumulatedTime += endTime - startTime; //overlapHandler.Set.Clear(); - CacheBlaster.Blast(); + //CacheBlaster.Blast(); } Console.WriteLine($"{name} time per execution (ms): {(accumulatedTime) * 1e3 / (testCount * Stopwatch.Frequency)}, acc: {accumulator}"); } From 8a68ad0e434472c04f2e027a11fd4a0f1f4e69e4 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 8 Nov 2022 12:54:39 -0600 Subject: [PATCH 637/947] Dequeues are now lockless. --- BepuUtilities/TaskQueue.cs | 63 +++++++++------------ Demos/SpecializedTests/TaskQueueTestDemo.cs | 8 +-- 2 files changed, 32 insertions(+), 39 deletions(-) diff --git a/BepuUtilities/TaskQueue.cs b/BepuUtilities/TaskQueue.cs index 466ede1b7..8de456f24 100644 --- a/BepuUtilities/TaskQueue.cs +++ b/BepuUtilities/TaskQueue.cs @@ -46,10 +46,6 @@ public enum DequeueTaskResult /// Success, /// - /// The dequeue attempt was contested. - /// - Contested, - /// /// The queue was empty, but may have more tasks in the future. /// Empty, @@ -348,28 +344,37 @@ public DequeueTaskResult TryDequeue(out delegate* functio function = default; context = default; taskId = default; - if (Interlocked.CompareExchange(ref taskLocker, 1, 0) != 0) - return DequeueTaskResult.Contested; - try + while (true) { - //We have the lock. - var nextTaskIndex = taskIndex; - if (nextTaskIndex >= writtenTaskIndex) + long nextTaskIndex, sampledWrittenTaskIndex; + //Note that there is no lock taken. We sample the currently visible values and treat the dequeue as a transaction. + //If the transaction fails, we don't make any changes and try again. + if (Environment.Is64BitProcess) //This branch is compile time (at least where I've tested it). + { + //It's fine if we don't get a consistent view of the task index and written task index. Worst case scenario, this will claim that the queue is empty where a lock wouldn't. + nextTaskIndex = Volatile.Read(ref taskIndex); + sampledWrittenTaskIndex = Volatile.Read(ref writtenTaskIndex); + } + else + { + //Interlocked reads for 32 bit systems. + nextTaskIndex = Interlocked.Read(ref taskIndex); + sampledWrittenTaskIndex = Interlocked.Read(ref writtenTaskIndex); + } + if (nextTaskIndex >= sampledWrittenTaskIndex) return DequeueTaskResult.Empty; var task = tasks[(int)(nextTaskIndex & taskMask)]; if (task.Function == null) return DequeueTaskResult.Stop; + //Unlike enqueues, a dequeue has a fixed contention window on a single value. There's not much point in using a spinwait when there's no reason to expect our next attempt to be blocked. + if (Interlocked.CompareExchange(ref taskIndex, nextTaskIndex + 1, nextTaskIndex) != nextTaskIndex) + continue; //There's an actual job! function = task.Function; context = task.Context; taskId = task.TaskId; - ++taskIndex; return DequeueTaskResult.Success; } - finally - { - taskLocker = 0; - } } /// @@ -385,25 +390,6 @@ public DequeueTaskResult TryDequeueAndRun(int workerIndex) return result; } - /// - /// Dequeues a task from the queue and runs it. - /// - /// Currently executing worker index to be provided to the function. - /// True if a task was dequeued and run, false if a stop command was encountered. - public bool DequeueAndRun(int workerIndex) - { - var waiter = new SpinWait(); - while (true) - { - var result = TryDequeueAndRun(workerIndex); - if (result == DequeueTaskResult.Stop) - return false; - if (result == DequeueTaskResult.Success) - return true; - waiter.SpinOnce(-1); - } - } - /// /// Checks whether all tasks composing a job, as reported to the continuation, have completed. /// @@ -500,7 +486,14 @@ public EnqueueTaskResult TryEnqueueTasks(Span tasks) EnqueueTaskResult result; if ((result = TryEnqueueTasksUnsafelyInternal(tasks, out var taskEndIndex)) == EnqueueTaskResult.Success) { - Interlocked.Exchange(ref writtenTaskIndex, taskEndIndex); //This is not assuming 64 bits. Mildly goofy considering the rest. + if (Environment.Is64BitProcess) + { + Volatile.Write(ref writtenTaskIndex, taskEndIndex); + } + else + { + Interlocked.Exchange(ref writtenTaskIndex, taskEndIndex); + } } return result; } diff --git a/Demos/SpecializedTests/TaskQueueTestDemo.cs b/Demos/SpecializedTests/TaskQueueTestDemo.cs index 128c3b6c6..3bb068d95 100644 --- a/Demos/SpecializedTests/TaskQueueTestDemo.cs +++ b/Demos/SpecializedTests/TaskQueueTestDemo.cs @@ -20,14 +20,14 @@ static void Test(int taskId, void* context, int workerIndex) sum = (sum ^ i) * i; } var typedContext = (Context*)context; - if (sum == int.MaxValue) - Interlocked.Add(ref typedContext->Sum, sum); + //if (sum == int.MaxValue) + Interlocked.Add(ref typedContext->Sum, sum); } static void DispatcherBody(int workerIndex, void* context) { var taskQueue = (TaskQueue*)context; - while (taskQueue->DequeueAndRun(workerIndex)) ; + while (taskQueue->TryDequeueAndRun(workerIndex) != DequeueTaskResult.Stop) ; } struct Context @@ -79,7 +79,7 @@ public override void Initialize(ContentArchive content, Camera camera) static void Test(TestFunction function, string name, Action reset = null) { long accumulatedTime = 0; - const int testCount = 64; + const int testCount = 128; int accumulator = 0; for (int i = 0; i < testCount; ++i) { From 07baae5d678d388e3dd6bdf7473c43eb3c9d8625 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 8 Nov 2022 16:56:26 -0600 Subject: [PATCH 638/947] RunTasks, cleanup. --- BepuUtilities/TaskQueue.cs | 249 +++++++++++--------- Demos/SpecializedTests/TaskQueueTestDemo.cs | 54 ++++- 2 files changed, 178 insertions(+), 125 deletions(-) diff --git a/BepuUtilities/TaskQueue.cs b/BepuUtilities/TaskQueue.cs index 8de456f24..04ffa09fa 100644 --- a/BepuUtilities/TaskQueue.cs +++ b/BepuUtilities/TaskQueue.cs @@ -23,9 +23,9 @@ namespace BepuUtilities; public unsafe struct Task { /// - /// Function to be executed by the task. Takes as arguments the , pointer, and executing worker index. + /// Function to be executed by the task. Takes as arguments the , pointer, and executing worker index. /// - public delegate* Function; + public delegate* Function; /// /// Context to be passed into the . /// @@ -33,7 +33,22 @@ public unsafe struct Task /// /// Identifier of this task within the job. /// - public int TaskId; + public long Id; + + /// + /// Creates a new task. + /// + /// Function to be executed by the task. Takes as arguments the , pointer, and executing worker index. + /// Context pointer to pass to the . + /// Id of this task to be passed into the . + public Task(delegate* function, void* context = null, long taskId = 0) + { + Function = function; + Context = context; + Id = taskId; + } + + public static implicit operator Task(delegate* function) => new Task(function); } /// @@ -181,7 +196,7 @@ public unsafe struct WrappedTaskContext /// /// Function to be invoked by this wrapped tsak. /// - public delegate* Function; + public delegate* Function; /// /// Context to be passed to this wrapped task. /// @@ -202,19 +217,11 @@ public unsafe struct WrappedTaskContext public unsafe struct TaskContinuation { /// - /// Function to call upon completion of the job, if any. - /// - public delegate* OnCompleted; - /// - /// Context to pass to the completion function, if any. + /// Task to run upon completion of the associated task. /// - public void* OnCompletedContext; + public Task OnCompleted; internal TaskQueueContinuations* Continuations; /// - /// Id provided by the user to identify this job. - /// - public ulong UserId; - /// /// Version of this continuation. /// public int Version; @@ -335,15 +342,11 @@ public int TaskCount /// /// Attempts to dequeue a task. /// - /// Function associated with the dequeued task, if any. - /// Context pointer associated with the dequeued task, if any. - /// Task id associated with the dequeued task, if any. + /// Dequeued task, if any. /// Result status of the dequeue attempt. - public DequeueTaskResult TryDequeue(out delegate* function, out void* context, out int taskId) + public DequeueTaskResult TryDequeue(out Task task) { - function = default; - context = default; - taskId = default; + task = default; while (true) { long nextTaskIndex, sampledWrittenTaskIndex; @@ -363,16 +366,14 @@ public DequeueTaskResult TryDequeue(out delegate* functio } if (nextTaskIndex >= sampledWrittenTaskIndex) return DequeueTaskResult.Empty; - var task = tasks[(int)(nextTaskIndex & taskMask)]; - if (task.Function == null) + var taskCandidate = tasks[(int)(nextTaskIndex & taskMask)]; + if (taskCandidate.Function == null) return DequeueTaskResult.Stop; //Unlike enqueues, a dequeue has a fixed contention window on a single value. There's not much point in using a spinwait when there's no reason to expect our next attempt to be blocked. if (Interlocked.CompareExchange(ref taskIndex, nextTaskIndex + 1, nextTaskIndex) != nextTaskIndex) continue; //There's an actual job! - function = task.Function; - context = task.Context; - taskId = task.TaskId; + task = taskCandidate; return DequeueTaskResult.Success; } } @@ -384,9 +385,9 @@ public DequeueTaskResult TryDequeue(out delegate* functio /// Result status of the dequeue attempt. public DequeueTaskResult TryDequeueAndRun(int workerIndex) { - var result = TryDequeue(out var function, out var context, out var taskId); + var result = TryDequeue(out var task); if (result == DequeueTaskResult.Success) - function(taskId, context, workerIndex); + task.Function(task.Id, task.Context, workerIndex); return result; } @@ -521,7 +522,7 @@ public void EnqueueTasks(Span tasks, int workerIndex) //Couldn't enqueue the tasks because the task buffer is full. //Clearly there's plenty of work available to execute, so go ahead and try to run one task inline. var task = tasks[0]; - task.Function(task.TaskId, task.Context, workerIndex); + task.Function(task.Id, task.Context, workerIndex); if (tasks.Length == 1) break; tasks = tasks[1..]; @@ -532,6 +533,85 @@ public void EnqueueTasks(Span tasks, int workerIndex) } } } + + /// + /// Appends a set of tasks to the queue and returns when all tasks are complete. + /// + /// Tasks composing the job. + /// Worker index to pass to inline-executed tasks if the task buffer is full. + /// Note that this will keep working until all tasks are run. + /// If the task queue is full, this will opt to run some tasks inline while waiting for room. + public void RunTasks(Span tasks, int workerIndex) + { + if (tasks.Length == 0) + return; + ContinuationHandle continuationHandle = default; + if (tasks.Length > 1) + { + //Note that we only submit tasks to the queue for tasks beyond the first. The current thread is responsible for at least task 0. + var taskCount = tasks.Length - 1; + WrappedTaskContext* wrappedContexts = stackalloc WrappedTaskContext[taskCount]; + Span tasksToEnqueue = stackalloc Task[taskCount]; + continuationHandle = AllocateContinuation(taskCount, workerIndex); + for (int i = 0; i < tasksToEnqueue.Length; ++i) + { + var wrappedTaskContext = wrappedContexts + i; + var sourceTask = tasks[i + 1]; + *wrappedTaskContext = new WrappedTaskContext { Function = sourceTask.Function, Context = sourceTask.Context, Continuation = continuationHandle, Continuations = continuationsContainer.Memory }; + tasksToEnqueue[i] = new Task { Function = &RunAndMarkAsComplete, Context = wrappedTaskContext, Id = sourceTask.Id }; + } + var waiter = new SpinWait(); + EnqueueTaskResult result; + while ((result = TryEnqueueTasks(tasksToEnqueue)) != EnqueueTaskResult.Success) + { + if (result == EnqueueTaskResult.Full) + { + //If the task buffer is full, just execute the task locally. Clearly there's enough work for other threads to keep running productively. + var task = tasksToEnqueue[0]; + task.Function(task.Id, task.Context, workerIndex); + if (tasksToEnqueue.Length == 1) + break; + tasksToEnqueue = tasksToEnqueue[1..]; + } + else + { + waiter.SpinOnce(-1); //TODO: We're biting the bullet on yields/sleep(0) here. May not be ideal for the use case; investigate + } + } + } + //Tasks [1, count) are submitted to the queue and may now be executing on other workers. + //The thread calling the for loop should not relinquish its timeslice. It should immediately begin working on task 0. + var task0 = tasks[0]; + task0.Function(task0.Id, task0.Context, workerIndex); + + if (tasks.Length > 1) + { + //Task 0 is done; this thread should seek out other work until the job is complete. + var waiter = new SpinWait(); + Debug.Assert(continuationHandle.Initialized, "This codepath should only run if the continuation was allocated earlier."); + while (!IsComplete(continuationHandle)) + { + //Note that we don't handle the DequeueResult.Stop case; if the job isn't complete yet, there's no way to hit a stop unless we enqueued this job after a stop. + //Enqueuing after a stop is an error condition and is debug checked for in TryEnqueueJob. + var dequeueResult = TryDequeue(out var fillerTask); + if (dequeueResult == DequeueTaskResult.Stop) + { + Debug.Assert(dequeueResult != DequeueTaskResult.Stop, "Did you enqueue these tasks *after* some thread enqueued a stop command? That's illegal!"); + return; + } + if (dequeueResult == DequeueTaskResult.Success) + { + fillerTask.Function(fillerTask.Id, fillerTask.Context, workerIndex); + waiter.Reset(); + } + else + { + waiter.SpinOnce(-1); + } + } + } + } + /// /// Tries to enqueues the stop command. /// @@ -603,11 +683,11 @@ public void CreateCompletionWrappedTasks(ContinuationHandle continuationHandle, wrappedContext->Continuations = continuationsContainer.Memory; targetTask.Function = &RunAndMarkAsComplete; targetTask.Context = wrappedContext; - targetTask.TaskId = sourceTask.TaskId; + targetTask.Id = sourceTask.Id; } } - static void RunAndMarkAsComplete(int taskId, void* wrapperContextPointer, int workerIndex) + static void RunAndMarkAsComplete(long taskId, void* wrapperContextPointer, int workerIndex) { var wrapperContext = (WrappedTaskContext*)wrapperContextPointer; wrapperContext->Function(taskId, wrapperContext->Context, workerIndex); @@ -618,9 +698,9 @@ static void RunAndMarkAsComplete(int taskId, void* wrapperContextPointer, int wo if (counter == 0) { //This entire job has completed. - if (continuation->OnCompleted != null) + if (continuation->OnCompleted.Function != null) { - continuation->OnCompleted(continuation->UserId, continuation->OnCompletedContext, workerIndex); + continuation->OnCompleted.Function(continuation->OnCompleted.Id, continuation->OnCompleted.Context, workerIndex); } //Free this continuation slot. var waiter = new SpinWait(); @@ -646,12 +726,10 @@ static void RunAndMarkAsComplete(int taskId, void* wrapperContextPointer, int wo /// Attempts to allocate a continuation for a set of tasks. /// /// Number of tasks associated with the continuation. - /// User id to associate with the handle. - /// Function to execute upon completing all associated tasks. - /// Context pointer to pass into the completion function. + /// Function to execute upon completing all associated tasks, if any. Any task with a null will not be executed. /// Handle of the continuation if allocation is successful. /// Result status of the continuation allocation attempt. - public AllocateTaskContinuationResult TryAllocateContinuation(int taskCount, ulong userContinuationId, delegate* onCompleted, void* onCompletedContext, out ContinuationHandle continuationHandle) + public AllocateTaskContinuationResult TryAllocateContinuation(int taskCount, out ContinuationHandle continuationHandle, Task onCompleted = default) { continuationHandle = default; ref var continuations = ref continuationsContainer[0]; @@ -669,9 +747,7 @@ public AllocateTaskContinuationResult TryAllocateContinuation(int taskCount, ulo var index = continuations.IndexPool.Take(); ref var continuation = ref continuations.Continuations[index]; var newVersion = continuation.Version + 1; - continuation.OnCompletedContext = onCompletedContext; continuation.OnCompleted = onCompleted; - continuation.UserId = userContinuationId; continuation.Version = newVersion; continuation.RemainingTaskCounter = taskCount; continuationHandle = new ContinuationHandle((uint)index, newVersion); @@ -688,17 +764,15 @@ public AllocateTaskContinuationResult TryAllocateContinuation(int taskCount, ulo /// /// Number of tasks associated with the continuation. /// Worker index to pass to any inline executed tasks if the continuations buffer is full. - /// User id to associate with the handle. - /// Function to execute upon completing all associated tasks. - /// Context pointer to pass into the completion function. + /// Task to execute upon completing all associated tasks, if any. Any task with a null will not be executed. /// Handle of the allocated continuation. /// Note that this will keep trying until allocation succeeds. If something is blocking allocation, such as insufficient room in the continuations buffer and there are no workers consuming tasks, this will block forever. - public ContinuationHandle AllocateContinuation(int taskCount, int workerIndex, ulong userContinuationId = 0, delegate* onCompleted = null, void* onCompletedContext = null) + public ContinuationHandle AllocateContinuation(int taskCount, int workerIndex, Task onCompleted = default) { var waiter = new SpinWait(); ContinuationHandle handle; AllocateTaskContinuationResult result; - while ((result = TryAllocateContinuation(taskCount, userContinuationId, onCompleted, onCompletedContext, out handle)) != AllocateTaskContinuationResult.Success) + while ((result = TryAllocateContinuation(taskCount, out handle, onCompleted)) != AllocateTaskContinuationResult.Success) { if (result == AllocateTaskContinuationResult.Full) { @@ -719,16 +793,15 @@ public ContinuationHandle AllocateContinuation(int taskCount, int workerIndex, u /// Function to execute on each iteration of the loop. /// Context pointer to pass into each task execution. /// Inclusive start index of the loop range. - /// Exclusive end index of the loop range. + /// Number of iterations to perform. /// Status result of the enqueue operation. /// This must not be used while other threads could be performing task enqueues or task dequeues. - public EnqueueTaskResult TryEnqueueForUnsafely(delegate* function, void* context, int inclusiveStartIndex, int exclusiveEndIndex) + public EnqueueTaskResult TryEnqueueForUnsafely(delegate* function, void* context, int inclusiveStartIndex, int iterationCount) { - var taskCount = exclusiveEndIndex - inclusiveStartIndex; - Span tasks = stackalloc Task[taskCount]; + Span tasks = stackalloc Task[iterationCount]; for (int i = 0; i < tasks.Length; ++i) { - tasks[i] = new Task { Function = function, Context = context, TaskId = i + inclusiveStartIndex }; + tasks[i] = new Task { Function = function, Context = context, Id = i + inclusiveStartIndex }; } return TryEnqueueTasksUnsafely(tasks); } @@ -739,17 +812,16 @@ public EnqueueTaskResult TryEnqueueForUnsafely(delegate* /// Function to execute on each iteration of the loop. /// Context pointer to pass into each task execution. /// Inclusive start index of the loop range. - /// Exclusive end index of the loop range. + /// Number of iterations to perform. /// Worker index to pass to any inline-executed task if the task queue is full. /// This function will not usually attempt to run any iterations of the loop itself. It tries to push the loop tasks onto the queue. /// If the task queue is full, this will opt to run the tasks inline while waiting for room. - public void EnqueueFor(delegate* function, void* context, int inclusiveStartIndex, int exclusiveEndIndex, int workerIndex) + public void EnqueueFor(delegate* function, void* context, int inclusiveStartIndex, int iterationCount, int workerIndex) { - var taskCount = exclusiveEndIndex - inclusiveStartIndex; - Span tasks = stackalloc Task[taskCount]; + Span tasks = stackalloc Task[iterationCount]; for (int i = 0; i < tasks.Length; ++i) { - tasks[i] = new Task { Function = function, Context = context, TaskId = i + inclusiveStartIndex }; + tasks[i] = new Task { Function = function, Context = context, Id = i + inclusiveStartIndex }; } EnqueueTasks(tasks, workerIndex); } @@ -760,74 +832,17 @@ public void EnqueueFor(delegate* function, void* context, /// Function to execute on each iteration of the loop. /// Context pointer to pass into each iteration of the loop. /// Inclusive start index of the loop range. - /// Exclusive end index of the loop range. + /// Number of iterations to perform. /// Index of the currently executing worker. - public void For(delegate* function, void* context, int inclusiveStartIndex, int exclusiveEndIndex, int workerIndex) + public void For(delegate* function, void* context, int inclusiveStartIndex, int iterationCount, int workerIndex) { - var iterationCount = exclusiveEndIndex - inclusiveStartIndex; if (iterationCount <= 0) return; - ContinuationHandle continuationHandle = default; - if (iterationCount > 1) - { - //Note that we only submit tasks to the queue for tasks beyond the first. The current thread is responsible for at least task 0. - var taskCount = iterationCount - 1; - WrappedTaskContext* wrappedContexts = stackalloc WrappedTaskContext[taskCount]; - Span tasks = stackalloc Task[taskCount]; - continuationHandle = AllocateContinuation(taskCount, workerIndex); - for (int i = 0; i < tasks.Length; ++i) - { - var wrappedTaskContext = wrappedContexts + i; - *wrappedTaskContext = new WrappedTaskContext { Function = function, Context = context, Continuation = continuationHandle, Continuations = continuationsContainer.Memory }; - tasks[i] = new Task { Function = &RunAndMarkAsComplete, Context = wrappedTaskContext, TaskId = i + 1 + inclusiveStartIndex }; - } - var waiter = new SpinWait(); - EnqueueTaskResult result; - while ((result = TryEnqueueTasks(tasks)) != EnqueueTaskResult.Success) - { - if (result == EnqueueTaskResult.Full) - { - //If the task buffer is full, just execute the task locally. Clearly there's enough work for other threads to keep running productively. - var task = tasks[0]; - task.Function(task.TaskId, task.Context, workerIndex); - if (tasks.Length == 1) - break; - tasks = tasks[1..]; - } - else - { - waiter.SpinOnce(-1); //TODO: We're biting the bullet on yields/sleep(0) here. May not be ideal for the use case; investigate - } - } - } - //Tasks [1, count) are submitted to the queue and may now be executing on other workers. - //The thread calling the for loop should not relinquish its timeslice. It should immediately begin working on task 0. - function(inclusiveStartIndex, context, workerIndex); - - if (iterationCount > 1) + Span tasks = stackalloc Task[iterationCount]; + for (int i = 0; i < tasks.Length; ++i) { - //Task 0 is done; this thread should seek out other work until the job is complete. - var waiter = new SpinWait(); - while (!IsComplete(continuationHandle)) - { - //Note that we don't handle the DequeueResult.Stop case; if the job isn't complete yet, there's no way to hit a stop unless we enqueued this job after a stop. - //Enqueuing after a stop is an error condition and is debug checked for in TryEnqueueJob. - var dequeueResult = TryDequeue(out var fillerJob, out var fillerContext, out var fillerTaskId); - if (dequeueResult == DequeueTaskResult.Stop) - { - Debug.Assert(dequeueResult != DequeueTaskResult.Stop, "Did you enqueue this for loop *after* some thread enqueued a stop command? That's illegal!"); - return; - } - if (dequeueResult == DequeueTaskResult.Success) - { - fillerJob(fillerTaskId, fillerContext, workerIndex); - waiter.Reset(); - } - else - { - waiter.SpinOnce(-1); - } - } + tasks[i] = new Task(function, context, inclusiveStartIndex + i); } + RunTasks(tasks, workerIndex); } } diff --git a/Demos/SpecializedTests/TaskQueueTestDemo.cs b/Demos/SpecializedTests/TaskQueueTestDemo.cs index 3bb068d95..8f383c7a7 100644 --- a/Demos/SpecializedTests/TaskQueueTestDemo.cs +++ b/Demos/SpecializedTests/TaskQueueTestDemo.cs @@ -12,15 +12,51 @@ namespace Demos.SpecializedTests; public unsafe class TaskQueueTestDemo : Demo { - static void Test(int taskId, void* context, int workerIndex) + static int DoSomeWork(int iterations, int sum) { - int sum = 0; - for (int i = 0; i < 100000; ++i) + for (int i = 0; i < iterations; ++i) { sum = (sum ^ i) * i; } + return sum; + } + static void DynamicallyEnqueuedTest(long taskId, void* context, int workerIndex) + { + int sum = 0; + DoSomeWork(10000, sum); + Interlocked.Add(ref ((Context*)context)->Sum, sum); + } + static void Test(long taskId, void* context, int workerIndex) + { + int sum = 0; + DoSomeWork(100000, sum); + var typedContext = (Context*)context; + if ((taskId & 7) == 0) + { + const int subtaskCount = 8; + //Span tasks = stackalloc Task[subtaskCount]; + //for (int i = 0; i < tasks.Length; ++i) + //{ + // tasks[i] = new Task { Function = &DynamicallyEnqueuedTest, Context = context, TaskId = taskId }; + //} + //typedContext->Queue->EnqueueTasks(tasks, workerIndex); + typedContext->Queue->For(&DynamicallyEnqueuedTest, context, 0, subtaskCount, workerIndex); + } + Interlocked.Add(ref typedContext->Sum, sum); + } + static void STTest(long taskId, void* context, int workerIndex) + { + int sum = 0; + DoSomeWork(100000, sum); var typedContext = (Context*)context; - //if (sum == int.MaxValue) + if ((taskId & 7) == 0) + { + const int subtaskCount = 8; + for (int i = 0; i < subtaskCount; ++i) + { + DynamicallyEnqueuedTest(taskId, context, workerIndex); + } + } Interlocked.Add(ref typedContext->Sum, sum); } @@ -32,6 +68,7 @@ static void DispatcherBody(int workerIndex, void* context) struct Context { + public TaskQueue* Queue; public int Sum; } @@ -52,10 +89,11 @@ public override void Initialize(ContentArchive content, Camera camera) var taskQueuePointer = &taskQueue; Test(() => { - var context = new Context { }; + var context = new Context { Queue = taskQueuePointer }; for (int i = 0; i < iterationCount; ++i) - taskQueuePointer->TryEnqueueForUnsafely(&Test, &context, 0, tasksPerIteration); - taskQueuePointer->TryEnqueueStopUnsafely(); + taskQueuePointer->TryEnqueueForUnsafely(&Test, &context, i * tasksPerIteration, tasksPerIteration); + //taskQueuePointer->TryEnqueueStopUnsafely(); + //taskQueuePointer->EnqueueTasks() ThreadDispatcher.DispatchWorkers(&DispatcherBody, taskQueuePointer); return context.Sum; }, "MT", () => taskQueuePointer->Reset()); @@ -67,7 +105,7 @@ public override void Initialize(ContentArchive content, Camera camera) var testContext = new Context { }; for (int i = 0; i < iterationCount * tasksPerIteration; ++i) { - Test(0, &testContext, 0); + STTest(0, &testContext, 0); } return testContext.Sum; }, "ST"); From 8ac327d95e65058b49b50320183300d4cd9e7c1a Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 8 Nov 2022 17:12:30 -0600 Subject: [PATCH 639/947] ContinuationHandles now more autonomous. --- BepuUtilities/TaskQueue.cs | 79 +++++++++++++++++++------------------- 1 file changed, 40 insertions(+), 39 deletions(-) diff --git a/BepuUtilities/TaskQueue.cs b/BepuUtilities/TaskQueue.cs index 04ffa09fa..d938395d7 100644 --- a/BepuUtilities/TaskQueue.cs +++ b/BepuUtilities/TaskQueue.cs @@ -92,15 +92,20 @@ public enum EnqueueTaskResult /// /// Refers to a continuation within a . /// -public struct ContinuationHandle : IEquatable +public unsafe struct ContinuationHandle : IEquatable { uint index; uint encodedVersion; + /// + /// Set of continuations in the queue. + /// + internal TaskQueueContinuations* Continuations; - internal ContinuationHandle(uint index, int version) + internal ContinuationHandle(uint index, int version, TaskQueueContinuations* continuations) { this.index = index; encodedVersion = (uint)version | 1u << 31; + Continuations = continuations; } internal uint Index @@ -121,6 +126,18 @@ internal int Version } } + /// + /// Gets whether the tasks associated with this continuation have completed. + /// + public bool Completed => Continuations->IsComplete(this); + + /// + /// Retrieves a pointer to the continuation data for . + /// + /// Pointer to the continuation backing the given handle. + /// This should not be used if the continuation handle is not known to be valid. The data pointed to by the data could become invalidated if the continuation completes. + public TaskContinuation* Continuation => Continuations->GetContinuation(this); + /// /// Gets a null continuation handle. /// @@ -186,6 +203,21 @@ internal unsafe struct TaskQueueContinuations return null; return Continuations.Memory + continuationHandle.Index; } + + /// + /// Checks whether all tasks composing a job, as reported to the continuation, have completed. + /// + /// Job to check for completion. + /// True if the job has completed, false otherwise. + public bool IsComplete(ContinuationHandle continuationHandle) + { + Debug.Assert(continuationHandle.Initialized, "This continuation handle was never initialized."); + Debug.Assert(continuationHandle.Index < Continuations.length, "This continuation refers to an invalid index."); + if (continuationHandle.Index >= Continuations.length || !continuationHandle.Initialized) + return false; + ref var continuation = ref Continuations[continuationHandle.Index]; + return continuation.Version > continuationHandle.Version || continuation.RemainingTaskCounter == 0; + } } /// @@ -205,10 +237,6 @@ public unsafe struct WrappedTaskContext /// Handle of the continuation associated with this wrapped task. /// public ContinuationHandle Continuation; - /// - /// Set of continuations in the queue. - /// - internal TaskQueueContinuations* Continuations; } /// @@ -220,7 +248,6 @@ public unsafe struct TaskContinuation /// Task to run upon completion of the associated task. /// public Task OnCompleted; - internal TaskQueueContinuations* Continuations; /// /// Version of this continuation. /// @@ -391,32 +418,6 @@ public DequeueTaskResult TryDequeueAndRun(int workerIndex) return result; } - /// - /// Checks whether all tasks composing a job, as reported to the continuation, have completed. - /// - /// Job to check for completion. - /// True if the job has completed, false otherwise. - public bool IsComplete(ContinuationHandle continuationHandle) - { - Debug.Assert(continuationHandle.Initialized, "This continuation handle was never initialized."); - Debug.Assert(continuationHandle.Index < continuationsContainer[0].Continuations.length, "This continuation refers to an invalid index."); - ref var continuationSet = ref continuationsContainer[0]; - if (continuationHandle.Index >= continuationSet.Continuations.length || !continuationHandle.Initialized) - return false; - ref var continuation = ref continuationSet.Continuations[continuationHandle.Index]; - return continuation.Version > continuationHandle.Version || continuation.RemainingTaskCounter == 0; - } - /// - /// Retrieves a pointer to the continuation data for . - /// - /// Handle to look up the associated continuation for. - /// Pointer to the continuation backing the given handle. - /// This should not be used if the continuation handle is not known to be valid. The data pointed to by the data could become invalidated if the continuation completes. - public TaskContinuation* GetContinuation(ContinuationHandle continuationHandle) - { - return continuationsContainer[0].GetContinuation(continuationHandle); - } - EnqueueTaskResult TryEnqueueTasksUnsafelyInternal(Span tasks, out long taskEndIndex) { Debug.Assert(tasks.Length > 0, "Probably shouldn't be trying to enqueue zero tasks."); @@ -557,7 +558,8 @@ public void RunTasks(Span tasks, int workerIndex) { var wrappedTaskContext = wrappedContexts + i; var sourceTask = tasks[i + 1]; - *wrappedTaskContext = new WrappedTaskContext { Function = sourceTask.Function, Context = sourceTask.Context, Continuation = continuationHandle, Continuations = continuationsContainer.Memory }; + Debug.Assert(continuationHandle.Continuations == continuationsContainer.Memory); + *wrappedTaskContext = new WrappedTaskContext { Function = sourceTask.Function, Context = sourceTask.Context, Continuation = continuationHandle }; tasksToEnqueue[i] = new Task { Function = &RunAndMarkAsComplete, Context = wrappedTaskContext, Id = sourceTask.Id }; } var waiter = new SpinWait(); @@ -589,7 +591,7 @@ public void RunTasks(Span tasks, int workerIndex) //Task 0 is done; this thread should seek out other work until the job is complete. var waiter = new SpinWait(); Debug.Assert(continuationHandle.Initialized, "This codepath should only run if the continuation was allocated earlier."); - while (!IsComplete(continuationHandle)) + while (!continuationHandle.Completed) { //Note that we don't handle the DequeueResult.Stop case; if the job isn't complete yet, there's no way to hit a stop unless we enqueued this job after a stop. //Enqueuing after a stop is an error condition and is debug checked for in TryEnqueueJob. @@ -680,7 +682,6 @@ public void CreateCompletionWrappedTasks(ContinuationHandle continuationHandle, wrappedContext->Function = sourceTask.Function; wrappedContext->Context = sourceTask.Context; wrappedContext->Continuation = continuationHandle; - wrappedContext->Continuations = continuationsContainer.Memory; targetTask.Function = &RunAndMarkAsComplete; targetTask.Context = wrappedContext; targetTask.Id = sourceTask.Id; @@ -692,8 +693,8 @@ static void RunAndMarkAsComplete(long taskId, void* wrapperContextPointer, int w var wrapperContext = (WrappedTaskContext*)wrapperContextPointer; wrapperContext->Function(taskId, wrapperContext->Context, workerIndex); var continuationHandle = wrapperContext->Continuation; - var continuations = wrapperContext->Continuations; - var continuation = continuations->GetContinuation(continuationHandle); + var continuations = continuationHandle.Continuations; + var continuation = continuationHandle.Continuation; var counter = Interlocked.Decrement(ref continuation->RemainingTaskCounter); if (counter == 0) { @@ -750,7 +751,7 @@ public AllocateTaskContinuationResult TryAllocateContinuation(int taskCount, out continuation.OnCompleted = onCompleted; continuation.Version = newVersion; continuation.RemainingTaskCounter = taskCount; - continuationHandle = new ContinuationHandle((uint)index, newVersion); + continuationHandle = new ContinuationHandle((uint)index, newVersion, continuationsContainer.Memory); return AllocateTaskContinuationResult.Success; } finally From 5ae6a98be6c01b0c5168fdb3f542f878fd5a0f46 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 8 Nov 2022 18:56:04 -0600 Subject: [PATCH 640/947] Continuation task wrapping removed. Tasks now just optionally have a continuation that is carried through. --- BepuUtilities/TaskQueue.cs | 161 +++++++++++++++++-------------------- 1 file changed, 72 insertions(+), 89 deletions(-) diff --git a/BepuUtilities/TaskQueue.cs b/BepuUtilities/TaskQueue.cs index d938395d7..1d8da2672 100644 --- a/BepuUtilities/TaskQueue.cs +++ b/BepuUtilities/TaskQueue.cs @@ -34,6 +34,10 @@ public unsafe struct Task /// Identifier of this task within the job. /// public long Id; + /// + /// Continuation to be notified after this job completes, if any. + /// + public ContinuationHandle Continuation; /// /// Creates a new task. @@ -41,14 +45,27 @@ public unsafe struct Task /// Function to be executed by the task. Takes as arguments the , pointer, and executing worker index. /// Context pointer to pass to the . /// Id of this task to be passed into the . - public Task(delegate* function, void* context = null, long taskId = 0) + /// Continuation to notify after the completion of this task, if any. + public Task(delegate* function, void* context = null, long taskId = 0, ContinuationHandle continuation = default) { Function = function; Context = context; Id = taskId; + Continuation = continuation; } public static implicit operator Task(delegate* function) => new Task(function); + + /// + /// Runs the task and, if necessary, notifies the associated continuation of its completion. + /// + /// Worker index to pass to the function. + public void Run(int workerIndex) + { + Function(Id, Context, workerIndex); + if (Continuation.Initialized) + Continuation.NotifyTaskCompleted(workerIndex); + } } /// @@ -144,11 +161,54 @@ internal int Version public static ContinuationHandle Null => default; /// - /// Gets whether this handle was ever initialized. This does not guarantee that the job handle is active in the that it was allocated from. + /// Gets whether the continuation associated with this handle was allocated and has outstanding jobs. + /// + public bool Exists + { + get + { + if (!Initialized || Continuations == null) + return false; + ref var continuation = ref Continuations->Continuations[Index]; + return continuation.Version == Version && continuation.RemainingTaskCounter > 0; + } + } + + /// + /// Gets whether this handle ever represented an allocated handle. This does not guarantee that the job handle is active in the that it was allocated from. /// public bool Initialized => encodedVersion >= 1u << 31; - public bool Equals(ContinuationHandle other) => other.index == index && other.encodedVersion == encodedVersion; + /// + /// Notifies the continuation that one task was completed. + /// + /// Worker index to pass to the continuation's delegate, if any. + public void NotifyTaskCompleted(int workerIndex) + { + var continuation = Continuation; + var counter = Interlocked.Decrement(ref continuation->RemainingTaskCounter); + Debug.Assert(counter >= 0, "The counter should not go negative. Was notify called too many times?"); + if (counter == 0) + { + //This entire job has completed. + if (continuation->OnCompleted.Function != null) + { + continuation->OnCompleted.Function(continuation->OnCompleted.Id, continuation->OnCompleted.Context, workerIndex); + } + //Free this continuation slot. + var waiter = new SpinWait(); + while (Interlocked.CompareExchange(ref Continuations->Locker, 1, 0) != 0) + { + waiter.SpinOnce(-1); + } + //We have the lock. + Continuations->IndexPool.ReturnUnsafely((int)Index); + --Continuations->ContinuationCount; + Continuations->Locker = 0; + } + } + + public bool Equals(ContinuationHandle other) => other.index == index && other.encodedVersion == encodedVersion && other.Continuations == Continuations; public override bool Equals([NotNullWhen(true)] object obj) => obj is ContinuationHandle handle && Equals(handle); @@ -220,25 +280,6 @@ public bool IsComplete(ContinuationHandle continuationHandle) } } -/// -/// Wraps a task for easier use with continuations. -/// -public unsafe struct WrappedTaskContext -{ - /// - /// Function to be invoked by this wrapped tsak. - /// - public delegate* Function; - /// - /// Context to be passed to this wrapped task. - /// - public void* Context; - /// - /// Handle of the continuation associated with this wrapped task. - /// - public ContinuationHandle Continuation; -} - /// /// Stores data relevant to tracking task completion and reporting completion for a job. /// @@ -414,7 +455,9 @@ public DequeueTaskResult TryDequeueAndRun(int workerIndex) { var result = TryDequeue(out var task); if (result == DequeueTaskResult.Success) - task.Function(task.Id, task.Context, workerIndex); + { + task.Run(workerIndex); + } return result; } @@ -538,7 +581,7 @@ public void EnqueueTasks(Span tasks, int workerIndex) /// /// Appends a set of tasks to the queue and returns when all tasks are complete. /// - /// Tasks composing the job. + /// Tasks composing the job. A continuation will be assigned internally; no continuation should be present on any of the provided tasks. /// Worker index to pass to inline-executed tasks if the task buffer is full. /// Note that this will keep working until all tasks are run. /// If the task queue is full, this will opt to run some tasks inline while waiting for room. @@ -551,16 +594,15 @@ public void RunTasks(Span tasks, int workerIndex) { //Note that we only submit tasks to the queue for tasks beyond the first. The current thread is responsible for at least task 0. var taskCount = tasks.Length - 1; - WrappedTaskContext* wrappedContexts = stackalloc WrappedTaskContext[taskCount]; Span tasksToEnqueue = stackalloc Task[taskCount]; continuationHandle = AllocateContinuation(taskCount, workerIndex); for (int i = 0; i < tasksToEnqueue.Length; ++i) { - var wrappedTaskContext = wrappedContexts + i; - var sourceTask = tasks[i + 1]; + var task = tasks[i + 1]; Debug.Assert(continuationHandle.Continuations == continuationsContainer.Memory); - *wrappedTaskContext = new WrappedTaskContext { Function = sourceTask.Function, Context = sourceTask.Context, Continuation = continuationHandle }; - tasksToEnqueue[i] = new Task { Function = &RunAndMarkAsComplete, Context = wrappedTaskContext, Id = sourceTask.Id }; + Debug.Assert(!task.Continuation.Initialized, $"None of the source tasks should have continuations when provided to {nameof(RunTasks)}."); + task.Continuation = continuationHandle; + tasksToEnqueue[i] = task; } var waiter = new SpinWait(); EnqueueTaskResult result; @@ -584,6 +626,7 @@ public void RunTasks(Span tasks, int workerIndex) //Tasks [1, count) are submitted to the queue and may now be executing on other workers. //The thread calling the for loop should not relinquish its timeslice. It should immediately begin working on task 0. var task0 = tasks[0]; + Debug.Assert(!task0.Continuation.Initialized, $"None of the source tasks should have continuations when provided to {nameof(RunTasks)}."); task0.Function(task0.Id, task0.Context, workerIndex); if (tasks.Length > 1) @@ -663,66 +706,6 @@ public EnqueueTaskResult TryEnqueueStopUnsafely() return TryEnqueueTasksUnsafely(stopJob); } - /// - /// Wraps a set of tasks in continuation tasks that will report their completion. - /// - /// Handle of the continuation to report to. - /// Tasks to wrap. - /// Contexts to be used for the wrapped tasks. This memory must persist until the wrapped tasks complete. - /// Span to hold the tasks created by this function. - public void CreateCompletionWrappedTasks(ContinuationHandle continuationHandle, Span tasks, WrappedTaskContext* wrappedTaskContexts, Span wrappedTasks) - { - var count = Math.Min(tasks.Length, wrappedTasks.Length); - Debug.Assert(tasks.Length == wrappedTasks.Length, "This is probably a bug!"); - for (int i = 0; i < count; ++i) - { - ref var sourceTask = ref tasks[i]; - var wrappedContext = wrappedTaskContexts + i; - ref var targetTask = ref wrappedTasks[i]; - wrappedContext->Function = sourceTask.Function; - wrappedContext->Context = sourceTask.Context; - wrappedContext->Continuation = continuationHandle; - targetTask.Function = &RunAndMarkAsComplete; - targetTask.Context = wrappedContext; - targetTask.Id = sourceTask.Id; - } - } - - static void RunAndMarkAsComplete(long taskId, void* wrapperContextPointer, int workerIndex) - { - var wrapperContext = (WrappedTaskContext*)wrapperContextPointer; - wrapperContext->Function(taskId, wrapperContext->Context, workerIndex); - var continuationHandle = wrapperContext->Continuation; - var continuations = continuationHandle.Continuations; - var continuation = continuationHandle.Continuation; - var counter = Interlocked.Decrement(ref continuation->RemainingTaskCounter); - if (counter == 0) - { - //This entire job has completed. - if (continuation->OnCompleted.Function != null) - { - continuation->OnCompleted.Function(continuation->OnCompleted.Id, continuation->OnCompleted.Context, workerIndex); - } - //Free this continuation slot. - var waiter = new SpinWait(); - while (true) - { - if (Interlocked.CompareExchange(ref continuations->Locker, 1, 0) != 0) - { - waiter.SpinOnce(-1); - } - else - { - //We have the lock. - continuations->IndexPool.ReturnUnsafely((int)continuationHandle.Index); - --continuations->ContinuationCount; - continuations->Locker = 0; - break; - } - } - } - } - /// /// Attempts to allocate a continuation for a set of tasks. /// From 257cce00036fd1c882841d7c5ef873e75f84bcaf Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 8 Nov 2022 19:44:07 -0600 Subject: [PATCH 641/947] Refactoring and helper. --- BepuUtilities/TaskQueue.cs | 172 ++++++++++---------- Demos/SpecializedTests/TaskQueueTestDemo.cs | 49 +++--- 2 files changed, 112 insertions(+), 109 deletions(-) diff --git a/BepuUtilities/TaskQueue.cs b/BepuUtilities/TaskQueue.cs index 1d8da2672..066c33c44 100644 --- a/BepuUtilities/TaskQueue.cs +++ b/BepuUtilities/TaskQueue.cs @@ -407,6 +407,71 @@ public int TaskCount /// public int ContinuationCount => continuationsContainer[0].ContinuationCount; + /// + /// Attempts to allocate a continuation for a set of tasks. + /// + /// Number of tasks associated with the continuation. + /// Function to execute upon completing all associated tasks, if any. Any task with a null will not be executed. + /// Handle of the continuation if allocation is successful. + /// Result status of the continuation allocation attempt. + public AllocateTaskContinuationResult TryAllocateContinuation(int taskCount, out ContinuationHandle continuationHandle, Task onCompleted = default) + { + continuationHandle = default; + ref var continuations = ref continuationsContainer[0]; + if (Interlocked.CompareExchange(ref continuations.Locker, 1, 0) != 0) + return AllocateTaskContinuationResult.Contested; + try + { + //We have the lock. + Debug.Assert(continuations.ContinuationCount <= continuations.Continuations.length); + if (continuations.ContinuationCount >= continuations.Continuations.length) + { + //No room. + return AllocateTaskContinuationResult.Full; + } + var index = continuations.IndexPool.Take(); + ref var continuation = ref continuations.Continuations[index]; + var newVersion = continuation.Version + 1; + continuation.OnCompleted = onCompleted; + continuation.Version = newVersion; + continuation.RemainingTaskCounter = taskCount; + continuationHandle = new ContinuationHandle((uint)index, newVersion, continuationsContainer.Memory); + return AllocateTaskContinuationResult.Success; + } + finally + { + continuations.Locker = 0; + } + } + + /// + /// Allocates a continuation for a set of tasks. + /// + /// Number of tasks associated with the continuation. + /// Worker index to pass to any inline executed tasks if the continuations buffer is full. + /// Task to execute upon completing all associated tasks, if any. Any task with a null will not be executed. + /// Handle of the allocated continuation. + /// Note that this will keep trying until allocation succeeds. If something is blocking allocation, such as insufficient room in the continuations buffer and there are no workers consuming tasks, this will block forever. + public ContinuationHandle AllocateContinuation(int taskCount, int workerIndex, Task onCompleted = default) + { + var waiter = new SpinWait(); + ContinuationHandle handle; + AllocateTaskContinuationResult result; + while ((result = TryAllocateContinuation(taskCount, out handle, onCompleted)) != AllocateTaskContinuationResult.Success) + { + if (result == AllocateTaskContinuationResult.Full) + { + var dequeueResult = TryDequeueAndRun(workerIndex); + Debug.Assert(dequeueResult != DequeueTaskResult.Stop, "We're trying to allocate a continuation, we shouldn't have run into a stop command!"); + } + else + { + waiter.SpinOnce(-1); + } + } + return handle; + } + /// /// Attempts to dequeue a task. /// @@ -578,6 +643,29 @@ public void EnqueueTasks(Span tasks, int workerIndex) } } + /// + /// Appends a set of tasks to the queue. Creates a continuation for the tasks. + /// + /// Tasks composing the job. A continuation will be assigned internally; no continuation should be present on any of the provided tasks. + /// Worker index to pass to inline-executed tasks if the task buffer is full. + /// Task to run upon completion of all the submitted tasks, if any. + /// Handle of the continuation created for these tasks. + /// Note that this will keep trying until task submission succeeds. + /// If the task queue is full, this will opt to run some tasks inline while waiting for room. + public ContinuationHandle AllocateContinuationAndEnqueueTasks(Span tasks, int workerIndex, Task onComplete = default) + { + var continuationHandle = AllocateContinuation(tasks.Length, workerIndex); + for (int i = 0; i < tasks.Length; ++i) + { + ref var task = ref tasks[i]; + Debug.Assert(!task.Continuation.Initialized, "This function creates a continuation for the tasks"); + task.Continuation = continuationHandle; + } + EnqueueTasks(tasks, workerIndex); + return continuationHandle; + } + + /// /// Appends a set of tasks to the queue and returns when all tasks are complete. /// @@ -604,24 +692,7 @@ public void RunTasks(Span tasks, int workerIndex) task.Continuation = continuationHandle; tasksToEnqueue[i] = task; } - var waiter = new SpinWait(); - EnqueueTaskResult result; - while ((result = TryEnqueueTasks(tasksToEnqueue)) != EnqueueTaskResult.Success) - { - if (result == EnqueueTaskResult.Full) - { - //If the task buffer is full, just execute the task locally. Clearly there's enough work for other threads to keep running productively. - var task = tasksToEnqueue[0]; - task.Function(task.Id, task.Context, workerIndex); - if (tasksToEnqueue.Length == 1) - break; - tasksToEnqueue = tasksToEnqueue[1..]; - } - else - { - waiter.SpinOnce(-1); //TODO: We're biting the bullet on yields/sleep(0) here. May not be ideal for the use case; investigate - } - } + EnqueueTasks(tasksToEnqueue, workerIndex); } //Tasks [1, count) are submitted to the queue and may now be executing on other workers. //The thread calling the for loop should not relinquish its timeslice. It should immediately begin working on task 0. @@ -706,71 +777,6 @@ public EnqueueTaskResult TryEnqueueStopUnsafely() return TryEnqueueTasksUnsafely(stopJob); } - /// - /// Attempts to allocate a continuation for a set of tasks. - /// - /// Number of tasks associated with the continuation. - /// Function to execute upon completing all associated tasks, if any. Any task with a null will not be executed. - /// Handle of the continuation if allocation is successful. - /// Result status of the continuation allocation attempt. - public AllocateTaskContinuationResult TryAllocateContinuation(int taskCount, out ContinuationHandle continuationHandle, Task onCompleted = default) - { - continuationHandle = default; - ref var continuations = ref continuationsContainer[0]; - if (Interlocked.CompareExchange(ref continuations.Locker, 1, 0) != 0) - return AllocateTaskContinuationResult.Contested; - try - { - //We have the lock. - Debug.Assert(continuations.ContinuationCount <= continuations.Continuations.length); - if (continuations.ContinuationCount >= continuations.Continuations.length) - { - //No room. - return AllocateTaskContinuationResult.Full; - } - var index = continuations.IndexPool.Take(); - ref var continuation = ref continuations.Continuations[index]; - var newVersion = continuation.Version + 1; - continuation.OnCompleted = onCompleted; - continuation.Version = newVersion; - continuation.RemainingTaskCounter = taskCount; - continuationHandle = new ContinuationHandle((uint)index, newVersion, continuationsContainer.Memory); - return AllocateTaskContinuationResult.Success; - } - finally - { - continuations.Locker = 0; - } - } - - /// - /// Allocates a continuation for a set of tasks. - /// - /// Number of tasks associated with the continuation. - /// Worker index to pass to any inline executed tasks if the continuations buffer is full. - /// Task to execute upon completing all associated tasks, if any. Any task with a null will not be executed. - /// Handle of the allocated continuation. - /// Note that this will keep trying until allocation succeeds. If something is blocking allocation, such as insufficient room in the continuations buffer and there are no workers consuming tasks, this will block forever. - public ContinuationHandle AllocateContinuation(int taskCount, int workerIndex, Task onCompleted = default) - { - var waiter = new SpinWait(); - ContinuationHandle handle; - AllocateTaskContinuationResult result; - while ((result = TryAllocateContinuation(taskCount, out handle, onCompleted)) != AllocateTaskContinuationResult.Success) - { - if (result == AllocateTaskContinuationResult.Full) - { - var dequeueResult = TryDequeueAndRun(workerIndex); - Debug.Assert(dequeueResult != DequeueTaskResult.Stop, "We're trying to allocate a continuation, we shouldn't have run into a stop command!"); - } - else - { - waiter.SpinOnce(-1); - } - } - return handle; - } - /// /// Enqueues a for loop onto the task queue. Does not take a lock; cannot return . /// diff --git a/Demos/SpecializedTests/TaskQueueTestDemo.cs b/Demos/SpecializedTests/TaskQueueTestDemo.cs index 8f383c7a7..1f7160ebb 100644 --- a/Demos/SpecializedTests/TaskQueueTestDemo.cs +++ b/Demos/SpecializedTests/TaskQueueTestDemo.cs @@ -22,41 +22,38 @@ static int DoSomeWork(int iterations, int sum) } static void DynamicallyEnqueuedTest(long taskId, void* context, int workerIndex) { - int sum = 0; - DoSomeWork(10000, sum); + var sum = DoSomeWork(10000, 0); Interlocked.Add(ref ((Context*)context)->Sum, sum); } static void Test(long taskId, void* context, int workerIndex) { - int sum = 0; - DoSomeWork(100000, sum); + var sum = DoSomeWork(100000, 0); var typedContext = (Context*)context; - if ((taskId & 7) == 0) - { - const int subtaskCount = 8; - //Span tasks = stackalloc Task[subtaskCount]; - //for (int i = 0; i < tasks.Length; ++i) - //{ - // tasks[i] = new Task { Function = &DynamicallyEnqueuedTest, Context = context, TaskId = taskId }; - //} - //typedContext->Queue->EnqueueTasks(tasks, workerIndex); - typedContext->Queue->For(&DynamicallyEnqueuedTest, context, 0, subtaskCount, workerIndex); - } + //if ((taskId & 7) == 0) + //{ + // const int subtaskCount = 8; + // //Span tasks = stackalloc Task[subtaskCount]; + // //for (int i = 0; i < tasks.Length; ++i) + // //{ + // // tasks[i] = new Task { Function = &DynamicallyEnqueuedTest, Context = context, TaskId = taskId }; + // //} + // //typedContext->Queue->EnqueueTasks(tasks, workerIndex); + // typedContext->Queue->For(&DynamicallyEnqueuedTest, context, 0, subtaskCount, workerIndex); + //} Interlocked.Add(ref typedContext->Sum, sum); } static void STTest(long taskId, void* context, int workerIndex) { - int sum = 0; - DoSomeWork(100000, sum); + var sum = DoSomeWork(100000, 0); var typedContext = (Context*)context; - if ((taskId & 7) == 0) - { - const int subtaskCount = 8; - for (int i = 0; i < subtaskCount; ++i) - { - DynamicallyEnqueuedTest(taskId, context, workerIndex); - } - } + //if ((taskId & 7) == 0) + //{ + // const int subtaskCount = 8; + // for (int i = 0; i < subtaskCount; ++i) + // { + // DynamicallyEnqueuedTest(taskId, context, workerIndex); + // } + //} Interlocked.Add(ref typedContext->Sum, sum); } @@ -92,7 +89,7 @@ public override void Initialize(ContentArchive content, Camera camera) var context = new Context { Queue = taskQueuePointer }; for (int i = 0; i < iterationCount; ++i) taskQueuePointer->TryEnqueueForUnsafely(&Test, &context, i * tasksPerIteration, tasksPerIteration); - //taskQueuePointer->TryEnqueueStopUnsafely(); + taskQueuePointer->TryEnqueueStopUnsafely(); //taskQueuePointer->EnqueueTasks() ThreadDispatcher.DispatchWorkers(&DispatcherBody, taskQueuePointer); return context.Sum; From be6e8fde3defed4fc3826f46a5888cc7c707042e Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 8 Nov 2022 19:47:27 -0600 Subject: [PATCH 642/947] A bit more terminological consistency. --- BepuUtilities/TaskQueue.cs | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/BepuUtilities/TaskQueue.cs b/BepuUtilities/TaskQueue.cs index 066c33c44..8c082fd0e 100644 --- a/BepuUtilities/TaskQueue.cs +++ b/BepuUtilities/TaskQueue.cs @@ -18,7 +18,7 @@ namespace BepuUtilities; /// -/// Description of one task within a job to be submitted to a . +/// Description of a task to be submitted to a . /// public unsafe struct Task { @@ -31,11 +31,11 @@ public unsafe struct Task /// public void* Context; /// - /// Identifier of this task within the job. + /// User-provided identifier of this task. /// public long Id; /// - /// Continuation to be notified after this job completes, if any. + /// Continuation to be notified after this task completes, if any. /// public ContinuationHandle Continuation; @@ -161,7 +161,7 @@ internal int Version public static ContinuationHandle Null => default; /// - /// Gets whether the continuation associated with this handle was allocated and has outstanding jobs. + /// Gets whether the continuation associated with this handle was allocated and has outstanding tasks. /// public bool Exists { @@ -175,7 +175,7 @@ public bool Exists } /// - /// Gets whether this handle ever represented an allocated handle. This does not guarantee that the job handle is active in the that it was allocated from. + /// Gets whether this handle ever represented an allocated handle. This does not guarantee that the continuation's associated tasks are active in the that it was allocated from. /// public bool Initialized => encodedVersion >= 1u << 31; @@ -265,10 +265,10 @@ internal unsafe struct TaskQueueContinuations } /// - /// Checks whether all tasks composing a job, as reported to the continuation, have completed. + /// Checks whether all tasks associated with this continuation have completed. /// - /// Job to check for completion. - /// True if the job has completed, false otherwise. + /// Continuation to check for completion. + /// True if all tasks associated with a continuation have completed, false otherwise. public bool IsComplete(ContinuationHandle continuationHandle) { Debug.Assert(continuationHandle.Initialized, "This continuation handle was never initialized."); @@ -281,7 +281,7 @@ public bool IsComplete(ContinuationHandle continuationHandle) } /// -/// Stores data relevant to tracking task completion and reporting completion for a job. +/// Stores data relevant to tracking task completion and reporting completion for a continuation. /// public unsafe struct TaskContinuation { @@ -294,7 +294,7 @@ public unsafe struct TaskContinuation /// public int Version; /// - /// Number of tasks not yet reported as complete in the job. + /// Number of tasks not yet reported as complete in the continuation. /// public int RemainingTaskCounter; } From 8e7c07ca2cbca653b34618d3746aa62f6f955f41 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 8 Nov 2022 20:18:22 -0600 Subject: [PATCH 643/947] Fixed some bugs; dynamic enqueues now complete, but bustedness remains. --- BepuUtilities/TaskQueue.cs | 14 ++--- Demos/SpecializedTests/TaskQueueTestDemo.cs | 59 ++++++++++++--------- 2 files changed, 41 insertions(+), 32 deletions(-) diff --git a/BepuUtilities/TaskQueue.cs b/BepuUtilities/TaskQueue.cs index 8c082fd0e..b98680966 100644 --- a/BepuUtilities/TaskQueue.cs +++ b/BepuUtilities/TaskQueue.cs @@ -631,7 +631,7 @@ public void EnqueueTasks(Span tasks, int workerIndex) //Couldn't enqueue the tasks because the task buffer is full. //Clearly there's plenty of work available to execute, so go ahead and try to run one task inline. var task = tasks[0]; - task.Function(task.Id, task.Context, workerIndex); + task.Run(workerIndex); if (tasks.Length == 1) break; tasks = tasks[1..]; @@ -717,7 +717,7 @@ public void RunTasks(Span tasks, int workerIndex) } if (dequeueResult == DequeueTaskResult.Success) { - fillerTask.Function(fillerTask.Id, fillerTask.Context, workerIndex); + fillerTask.Run(workerIndex); waiter.Reset(); } else @@ -784,14 +784,15 @@ public EnqueueTaskResult TryEnqueueStopUnsafely() /// Context pointer to pass into each task execution. /// Inclusive start index of the loop range. /// Number of iterations to perform. + /// Continuation associated with the loop tasks, if any. /// Status result of the enqueue operation. /// This must not be used while other threads could be performing task enqueues or task dequeues. - public EnqueueTaskResult TryEnqueueForUnsafely(delegate* function, void* context, int inclusiveStartIndex, int iterationCount) + public EnqueueTaskResult TryEnqueueForUnsafely(delegate* function, void* context, int inclusiveStartIndex, int iterationCount, ContinuationHandle continuation = default) { Span tasks = stackalloc Task[iterationCount]; for (int i = 0; i < tasks.Length; ++i) { - tasks[i] = new Task { Function = function, Context = context, Id = i + inclusiveStartIndex }; + tasks[i] = new Task { Function = function, Context = context, Id = i + inclusiveStartIndex, Continuation = continuation }; } return TryEnqueueTasksUnsafely(tasks); } @@ -804,14 +805,15 @@ public EnqueueTaskResult TryEnqueueForUnsafely(delegate* /// Inclusive start index of the loop range. /// Number of iterations to perform. /// Worker index to pass to any inline-executed task if the task queue is full. + /// Continuation associated with the loop tasks, if any. /// This function will not usually attempt to run any iterations of the loop itself. It tries to push the loop tasks onto the queue. /// If the task queue is full, this will opt to run the tasks inline while waiting for room. - public void EnqueueFor(delegate* function, void* context, int inclusiveStartIndex, int iterationCount, int workerIndex) + public void EnqueueFor(delegate* function, void* context, int inclusiveStartIndex, int iterationCount, int workerIndex, ContinuationHandle continuation = default) { Span tasks = stackalloc Task[iterationCount]; for (int i = 0; i < tasks.Length; ++i) { - tasks[i] = new Task { Function = function, Context = context, Id = i + inclusiveStartIndex }; + tasks[i] = new Task { Function = function, Context = context, Id = i + inclusiveStartIndex, Continuation = continuation }; } EnqueueTasks(tasks, workerIndex); } diff --git a/Demos/SpecializedTests/TaskQueueTestDemo.cs b/Demos/SpecializedTests/TaskQueueTestDemo.cs index 1f7160ebb..a78be68fc 100644 --- a/Demos/SpecializedTests/TaskQueueTestDemo.cs +++ b/Demos/SpecializedTests/TaskQueueTestDemo.cs @@ -22,38 +22,38 @@ static int DoSomeWork(int iterations, int sum) } static void DynamicallyEnqueuedTest(long taskId, void* context, int workerIndex) { - var sum = DoSomeWork(10000, 0); + var sum = DoSomeWork(100, 0); Interlocked.Add(ref ((Context*)context)->Sum, sum); } static void Test(long taskId, void* context, int workerIndex) { - var sum = DoSomeWork(100000, 0); + var sum = DoSomeWork(100, 0); var typedContext = (Context*)context; - //if ((taskId & 7) == 0) - //{ - // const int subtaskCount = 8; - // //Span tasks = stackalloc Task[subtaskCount]; - // //for (int i = 0; i < tasks.Length; ++i) - // //{ - // // tasks[i] = new Task { Function = &DynamicallyEnqueuedTest, Context = context, TaskId = taskId }; - // //} - // //typedContext->Queue->EnqueueTasks(tasks, workerIndex); - // typedContext->Queue->For(&DynamicallyEnqueuedTest, context, 0, subtaskCount, workerIndex); - //} + if ((taskId & 7) == 0) + { + const int subtaskCount = 8; + //Span tasks = stackalloc Task[subtaskCount]; + //for (int i = 0; i < tasks.Length; ++i) + //{ + // tasks[i] = new Task { Function = &DynamicallyEnqueuedTest, Context = context, TaskId = taskId }; + //} + //typedContext->Queue->EnqueueTasks(tasks, workerIndex); + typedContext->Queue->For(&DynamicallyEnqueuedTest, context, 0, subtaskCount, workerIndex); + } Interlocked.Add(ref typedContext->Sum, sum); } static void STTest(long taskId, void* context, int workerIndex) { - var sum = DoSomeWork(100000, 0); + var sum = DoSomeWork(100, 0); var typedContext = (Context*)context; - //if ((taskId & 7) == 0) - //{ - // const int subtaskCount = 8; - // for (int i = 0; i < subtaskCount; ++i) - // { - // DynamicallyEnqueuedTest(taskId, context, workerIndex); - // } - //} + if ((taskId & 7) == 0) + { + const int subtaskCount = 8; + for (int i = 0; i < subtaskCount; ++i) + { + DynamicallyEnqueuedTest(i, context, workerIndex); + } + } Interlocked.Add(ref typedContext->Sum, sum); } @@ -69,6 +69,12 @@ struct Context public int Sum; } + static void IssueStop(long id, void* context, int workerIndex) + { + var typedContext = (Context*)context; + typedContext->Queue->EnqueueStop(workerIndex); + } + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(-10, 3, -10); @@ -80,16 +86,17 @@ public override void Initialize(ContentArchive content, Camera camera) - int iterationCount = 128; - int tasksPerIteration = 4; + int iterationCount = 1; + int tasksPerIteration = 2; var taskQueue = new TaskQueue(BufferPool); var taskQueuePointer = &taskQueue; Test(() => { var context = new Context { Queue = taskQueuePointer }; + var continuation = taskQueuePointer->AllocateContinuation(iterationCount * tasksPerIteration, 0, new Task(&IssueStop, &context)); for (int i = 0; i < iterationCount; ++i) - taskQueuePointer->TryEnqueueForUnsafely(&Test, &context, i * tasksPerIteration, tasksPerIteration); - taskQueuePointer->TryEnqueueStopUnsafely(); + taskQueuePointer->TryEnqueueForUnsafely(&Test, &context, i * tasksPerIteration, tasksPerIteration, continuation); + //taskQueuePointer->TryEnqueueStopUnsafely(); //taskQueuePointer->EnqueueTasks() ThreadDispatcher.DispatchWorkers(&DispatcherBody, taskQueuePointer); return context.Sum; From 161555260faf0898202c7fade2c41ba7b3807f20 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 9 Nov 2022 13:06:14 -0600 Subject: [PATCH 644/947] Fixed tests. Queue actually wasn't bugged! --- Demos/SpecializedTests/TaskQueueTestDemo.cs | 25 ++++++++++----------- 1 file changed, 12 insertions(+), 13 deletions(-) diff --git a/Demos/SpecializedTests/TaskQueueTestDemo.cs b/Demos/SpecializedTests/TaskQueueTestDemo.cs index a78be68fc..f658ae773 100644 --- a/Demos/SpecializedTests/TaskQueueTestDemo.cs +++ b/Demos/SpecializedTests/TaskQueueTestDemo.cs @@ -20,31 +20,27 @@ static int DoSomeWork(int iterations, int sum) } return sum; } + static int counter = 0; static void DynamicallyEnqueuedTest(long taskId, void* context, int workerIndex) { - var sum = DoSomeWork(100, 0); + var sum = DoSomeWork(10000, 0); + //Console.WriteLine($"Dynamically enqueued {Interlocked.Increment(ref counter)}"); Interlocked.Add(ref ((Context*)context)->Sum, sum); } static void Test(long taskId, void* context, int workerIndex) { - var sum = DoSomeWork(100, 0); + var sum = DoSomeWork(100000, 0); var typedContext = (Context*)context; if ((taskId & 7) == 0) { const int subtaskCount = 8; - //Span tasks = stackalloc Task[subtaskCount]; - //for (int i = 0; i < tasks.Length; ++i) - //{ - // tasks[i] = new Task { Function = &DynamicallyEnqueuedTest, Context = context, TaskId = taskId }; - //} - //typedContext->Queue->EnqueueTasks(tasks, workerIndex); typedContext->Queue->For(&DynamicallyEnqueuedTest, context, 0, subtaskCount, workerIndex); } Interlocked.Add(ref typedContext->Sum, sum); } static void STTest(long taskId, void* context, int workerIndex) { - var sum = DoSomeWork(100, 0); + var sum = DoSomeWork(100000, 0); var typedContext = (Context*)context; if ((taskId & 7) == 0) { @@ -86,8 +82,8 @@ public override void Initialize(ContentArchive content, Camera camera) - int iterationCount = 1; - int tasksPerIteration = 2; + int iterationCount = 4; + int tasksPerIteration = 64; var taskQueue = new TaskQueue(BufferPool); var taskQueuePointer = &taskQueue; Test(() => @@ -107,9 +103,12 @@ public override void Initialize(ContentArchive content, Camera camera) Test(() => { var testContext = new Context { }; - for (int i = 0; i < iterationCount * tasksPerIteration; ++i) + for (int i = 0; i < iterationCount; ++i) { - STTest(0, &testContext, 0); + for (int j = 0; j < tasksPerIteration; ++j) + { + STTest(i * tasksPerIteration + j, &testContext, 0); + } } return testContext.Sum; }, "ST"); From 3bb688736483d1b33a0f304bdf9c139f56ca200e Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 9 Nov 2022 14:20:09 -0600 Subject: [PATCH 645/947] Multithreaded prepass. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 124 ++++++++++++++++---- Demos/DemoSet.cs | 2 +- Demos/SpecializedTests/TaskQueueTestDemo.cs | 2 - 3 files changed, 102 insertions(+), 26 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index a60e1954e..3eb1ca553 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -374,12 +374,100 @@ private static unsafe int ComputeBinIndex(Vector4 centroidMin, bool useX, bool u struct SingleThreaded { } + struct MultithreadBinnedBuildContext + { + public TaskQueue Queue; + public Buffer WorkerPrepassBounds; + public Buffer Bounds; + /// + /// Maximum number of tasks any one job submission should create. + /// If you have far more tasks than there are workers, adding more tasks just adds overhead without additional workstealing advantages. + /// + public int MaximumTaskCountPerSubmission; + public int SlotsPerTaskBase; + public int SlotRemainder; + public bool LessTasksThanWorkers; + + public void GetSlotInterval(long taskId, out int start, out int count) + { + var remainderedTaskCount = Math.Min(SlotRemainder, taskId); + var earlySlotCount = (SlotsPerTaskBase + 1) * remainderedTaskCount; + var lateSlotCount = SlotsPerTaskBase * (taskId - remainderedTaskCount); + start = (int)(earlySlotCount + lateSlotCount); + count = taskId > SlotRemainder ? SlotsPerTaskBase : SlotsPerTaskBase + 1; + + } + } + const int SubtreesPerThreadForCentroidPrepass = 2048; const int SubtreesPerThreadForBinning = 512; const int SubtreesPerThreadForNodeJob = 512; + [MethodImpl(MethodImplOptions.AggressiveInlining)] + static BoundingBox4 ComputeCentroidBounds(Buffer bounds) + { + BoundingBox4 centroidBounds; + centroidBounds.Min = new Vector4(float.MaxValue); + centroidBounds.Max = new Vector4(float.MinValue); + for (int i = 0; i < bounds.Length; ++i) + { + ref var box = ref bounds[i]; + //Note that centroids never bother scaling by 0.5. It's fine as long as we're consistent. + var centroid = box.Min + box.Max; + centroidBounds.Min = Vector4.Min(centroidBounds.Min, centroid); + centroidBounds.Max = Vector4.Max(centroidBounds.Max, centroid); + } + return centroidBounds; + } + + unsafe static void CentroidPrepassWorker(long taskId, void* untypedContext, int workerIndex) + { + ref var context = ref *(MultithreadBinnedBuildContext*)untypedContext; + context.GetSlotInterval(taskId, out var start, out var count); + var centroidBounds = ComputeCentroidBounds(context.Bounds.Slice(start, count)); + if (context.LessTasksThanWorkers) + { + //There were less tasks than workers; directly write into the slot without bothering to merge. + context.WorkerPrepassBounds[(int)taskId] = centroidBounds; + } + else + { + ref var workerBounds = ref context.WorkerPrepassBounds[workerIndex]; + workerBounds.Min = Vector4.Min(workerBounds.Min, centroidBounds.Min); + workerBounds.Max = Vector4.Max(workerBounds.Max, centroidBounds.Max); + } + } + + unsafe static BoundingBox4 MultithreadedCentroidPrepass(MultithreadBinnedBuildContext* context, int subtreeCount, int workerIndex) + { + int taskCount = Math.Min(context->MaximumTaskCountPerSubmission, (subtreeCount + SubtreesPerThreadForCentroidPrepass - 1) / SubtreesPerThreadForCentroidPrepass); + //Don't bother initializing more slots than we have tasks. Note that this requires special handling on the task level; + //if we have less tasks than workers, then the task needs to distinguish that fact. + var initializationCount = Math.Min(context->WorkerPrepassBounds.Length, taskCount); + context->LessTasksThanWorkers = initializationCount < context->WorkerPrepassBounds.Length; + for (int i = 0; i < initializationCount; ++i) + { + ref var workerBounds = ref context->WorkerPrepassBounds[i]; + workerBounds.Min = new Vector4(float.MaxValue); + workerBounds.Max = new Vector4(float.MinValue); + } + context->SlotsPerTaskBase = subtreeCount / taskCount; + context->SlotRemainder = subtreeCount - context->SlotsPerTaskBase * taskCount; + context->Queue.For(&CentroidPrepassWorker, context, 0, taskCount, workerIndex); + + var centroidBounds = context->WorkerPrepassBounds[0]; + for (int i = 1; i < initializationCount; ++i) + { + ref var workerBounds = ref context->WorkerPrepassBounds[i]; + centroidBounds.Min = Vector4.Min(workerBounds.Min, centroidBounds.Min); + centroidBounds.Max = Vector4.Max(workerBounds.Max, centroidBounds.Max); + } + return centroidBounds; + + } + static unsafe void BinnedBuilderInternal(Buffer indices, TLeafCounts leafCounts, ref TLeaves leaves, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, - int nodeIndex, int parentNodeIndex, int childIndexInParent, Context* context) + int nodeIndex, int parentNodeIndex, int childIndexInParent, Context* context, int workerIndex) where TLeafCounts : unmanaged, ILeafCountBuffer where TLeaves : unmanaged where TThreading : unmanaged { var subtreeCount = indices.Length; @@ -388,26 +476,16 @@ static unsafe void BinnedBuilderInternal(Buffe BuildNode(boundingBoxes[0], boundingBoxes[1], leafCounts[0], leafCounts[1], nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, 1, 1, ref leaves, out _, out _); return; } - var centroidMin = new Vector4(float.MaxValue); - var centroidMax = new Vector4(float.MinValue); - - + BoundingBox4 centroidBounds; if (typeof(TThreading) == typeof(SingleThreaded) || subtreeCount < SubtreesPerThreadForCentroidPrepass) { - for (int i = 0; i < subtreeCount; ++i) - { - ref var box = ref boundingBoxes[i]; - //Note that centroids never bother scaling by 0.5. It's fine as long as we're consistent. - var centroid = box.Min + box.Max; - centroidMin = Vector4.Min(centroidMin, centroid); - centroidMax = Vector4.Max(centroidMax, centroid); - } + centroidBounds = ComputeCentroidBounds(boundingBoxes); } else { - //Use the multithreaded path. + centroidBounds = MultithreadedCentroidPrepass((MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As(ref context->Threading)), subtreeCount, workerIndex); } - var centroidSpan = centroidMax - centroidMin; + var centroidSpan = centroidBounds.Max - centroidBounds.Min; var axisIsDegenerate = Vector128.LessThanOrEqual(centroidSpan.AsVector128(), Vector128.Create(1e-12f)); if ((Vector128.ExtractMostSignificantBits(axisIsDegenerate) & 0b111) == 0b111) { @@ -438,16 +516,16 @@ static unsafe void BinnedBuilderInternal(Buffe } BuildNode(boundsA, boundsB, degenerateLeafCountA, degenerateLeafCountB, nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, degenerateSubtreeCountA, degenerateSubtreeCountB, ref leaves, out var aIndex, out var bIndex); if (degenerateSubtreeCountA > 1) - BinnedBuilderInternal(indices.Slice(degenerateSubtreeCountA), leafCounts.Slice(0, degenerateSubtreeCountA), ref leaves, boundingBoxes.Slice(degenerateSubtreeCountA), nodes.Slice(1, degenerateSubtreeCountA - 1), metanodes.Slice(1, degenerateSubtreeCountA - 1), aIndex, nodeIndex, 0, context); + BinnedBuilderInternal(indices.Slice(degenerateSubtreeCountA), leafCounts.Slice(0, degenerateSubtreeCountA), ref leaves, boundingBoxes.Slice(degenerateSubtreeCountA), nodes.Slice(1, degenerateSubtreeCountA - 1), metanodes.Slice(1, degenerateSubtreeCountA - 1), aIndex, nodeIndex, 0, context, workerIndex); if (degenerateSubtreeCountB > 1) - BinnedBuilderInternal(indices.Slice(degenerateSubtreeCountA, degenerateSubtreeCountB), leafCounts.Slice(degenerateSubtreeCountA, degenerateSubtreeCountB), ref leaves, boundingBoxes.Slice(degenerateSubtreeCountA, degenerateSubtreeCountB), nodes.Slice(degenerateSubtreeCountA, degenerateSubtreeCountB - 1), metanodes.Slice(degenerateSubtreeCountA, degenerateSubtreeCountB - 1), bIndex, nodeIndex, 1, context); + BinnedBuilderInternal(indices.Slice(degenerateSubtreeCountA, degenerateSubtreeCountB), leafCounts.Slice(degenerateSubtreeCountA, degenerateSubtreeCountB), ref leaves, boundingBoxes.Slice(degenerateSubtreeCountA, degenerateSubtreeCountB), nodes.Slice(degenerateSubtreeCountA, degenerateSubtreeCountB - 1), metanodes.Slice(degenerateSubtreeCountA, degenerateSubtreeCountB - 1), bIndex, nodeIndex, 1, context, workerIndex); return; } //Note that we don't bother even trying to internally multithread microsweeps. They *should* be small, and should only show up deeper in the recursion process. if (subtreeCount <= context->MicrosweepThreshold) { - MicroSweepForBinnedBuilder(centroidMin, centroidMax, indices, leafCounts, ref leaves, boundingBoxes, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, context); + MicroSweepForBinnedBuilder(centroidBounds.Min, centroidBounds.Max, indices, leafCounts, ref leaves, boundingBoxes, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, context); return; } @@ -480,7 +558,7 @@ static unsafe void BinnedBuilderInternal(Buffe for (int i = 0; i < subtreeCount; ++i) { ref var box = ref boundingBoxes[i]; - var binIndex = ComputeBinIndex(centroidMin, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, box); + var binIndex = ComputeBinIndex(centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, box); ref var xBounds = ref context->BinBoundingBoxes[binIndex]; xBounds.Min = Vector4.Min(xBounds.Min, box.Min); xBounds.Max = Vector4.Max(xBounds.Max, box.Max); @@ -551,7 +629,7 @@ static unsafe void BinnedBuilderInternal(Buffe while (subtreeCountA + subtreeCountB < subtreeCount) { ref var box = ref boundingBoxes[subtreeCountA]; - var binIndex = ComputeBinIndex(centroidMin, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, box); + var binIndex = ComputeBinIndex(centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, box); if (binIndex >= splitIndex) { //Belongs to B. Swap it. @@ -598,9 +676,9 @@ static unsafe void BinnedBuilderInternal(Buffe Debug.Assert(SubtreesPerThreadForNodeJob > 1, "The job threshold for a new node should be large enough that there's no need for a subtreeCountB > 1 test."); } if (subtreeCountA > 1) - BinnedBuilderInternal(indices.Slice(subtreeCountA), leafCounts.Slice(0, subtreeCountA), ref leaves, boundingBoxes.Slice(subtreeCountA), nodes.Slice(1, subtreeCountA - 1), metanodes.Slice(1, subtreeCountA - 1), nodeChildIndexA, nodeIndex, 0, context); + BinnedBuilderInternal(indices.Slice(subtreeCountA), leafCounts.Slice(0, subtreeCountA), ref leaves, boundingBoxes.Slice(subtreeCountA), nodes.Slice(1, subtreeCountA - 1), metanodes.Slice(1, subtreeCountA - 1), nodeChildIndexA, nodeIndex, 0, context, workerIndex); if (!shouldPushBOntoMultithreadedQueue && subtreeCountB > 1) - BinnedBuilderInternal(indices.Slice(subtreeCountA, subtreeCountB), leafCounts.Slice(subtreeCountA, subtreeCountB), ref leaves, boundingBoxes.Slice(subtreeCountA, subtreeCountB), nodes.Slice(subtreeCountA, subtreeCountB - 1), metanodes.Slice(subtreeCountA, subtreeCountB - 1), nodeChildIndexB, nodeIndex, 1, context); + BinnedBuilderInternal(indices.Slice(subtreeCountA, subtreeCountB), leafCounts.Slice(subtreeCountA, subtreeCountB), ref leaves, boundingBoxes.Slice(subtreeCountA, subtreeCountB), nodes.Slice(subtreeCountA, subtreeCountB - 1), metanodes.Slice(subtreeCountA, subtreeCountB - 1), nodeChildIndexB, nodeIndex, 1, context, workerIndex); } public static unsafe void BinnedBuilder(Buffer encodedLeafIndices, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, Buffer leaves, BufferPool pool, @@ -651,7 +729,7 @@ public static unsafe void BinnedBuilder(Buffer encodedLeafIndices, Buffer(), nodes, metanodes, 0, -1, -1, &context); + BinnedBuilderInternal(encodedLeafIndices, leafCounts, ref leaves, boundingBoxes.As(), nodes, metanodes, 0, -1, -1, &context, 0); } } } diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index 9a542f8b3..fed59c61f 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -46,8 +46,8 @@ struct Option public DemoSet() { - AddOption(); AddOption(); + AddOption(); AddOption(); AddOption(); AddOption(); diff --git a/Demos/SpecializedTests/TaskQueueTestDemo.cs b/Demos/SpecializedTests/TaskQueueTestDemo.cs index f658ae773..f0675ef5b 100644 --- a/Demos/SpecializedTests/TaskQueueTestDemo.cs +++ b/Demos/SpecializedTests/TaskQueueTestDemo.cs @@ -20,11 +20,9 @@ static int DoSomeWork(int iterations, int sum) } return sum; } - static int counter = 0; static void DynamicallyEnqueuedTest(long taskId, void* context, int workerIndex) { var sum = DoSomeWork(10000, 0); - //Console.WriteLine($"Dynamically enqueued {Interlocked.Increment(ref counter)}"); Interlocked.Add(ref ((Context*)context)->Sum, sum); } static void Test(long taskId, void* context, int workerIndex) From 11974f63761bfc73ebf2fb92430f1b2a509795ad Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 9 Nov 2022 20:21:09 -0600 Subject: [PATCH 646/947] In the middle of binning MT. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 141 +++++++++++++++++++----- 1 file changed, 112 insertions(+), 29 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 3eb1ca553..b42885f1c 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -374,20 +374,30 @@ private static unsafe int ComputeBinIndex(Vector4 centroidMin, bool useX, bool u struct SingleThreaded { } - struct MultithreadBinnedBuildContext + /// + /// Stores resources required by a worker to dispatch and manage multithreaded work. + /// + unsafe struct BinnedBuildWorkerContext { - public TaskQueue Queue; + /// + /// Stores per-worker prepass bounds accumulated over multiple tasks. If there are less tasks than workers, then only the lower contiguous region of these bounds are used. + /// public Buffer WorkerPrepassBounds; - public Buffer Bounds; + /// - /// Maximum number of tasks any one job submission should create. - /// If you have far more tasks than there are workers, adding more tasks just adds overhead without additional workstealing advantages. + /// Bounds associated with the node that invoked this multithreaded job. /// - public int MaximumTaskCountPerSubmission; + public Buffer Bounds; public int SlotsPerTaskBase; public int SlotRemainder; public bool LessTasksThanWorkers; + public BinnedBuildWorkerContext(Buffer workerPrepassBounds) + { + //We let the caller preallocate the bounds to avoid an individual allocation for every worker. + WorkerPrepassBounds = workerPrepassBounds; + } + public void GetSlotInterval(long taskId, out int start, out int count) { var remainderedTaskCount = Math.Min(SlotRemainder, taskId); @@ -395,10 +405,33 @@ public void GetSlotInterval(long taskId, out int start, out int count) var lateSlotCount = SlotsPerTaskBase * (taskId - remainderedTaskCount); start = (int)(earlySlotCount + lateSlotCount); count = taskId > SlotRemainder ? SlotsPerTaskBase : SlotsPerTaskBase + 1; + } + } + unsafe struct MultithreadBinnedBuildContext + { + public TaskQueue* Queue; + /// + /// Maximum number of tasks any one job submission should create. + /// If you have far more tasks than there are workers, adding more tasks just adds overhead without additional workstealing advantages. + /// + public int MaximumTaskCountPerSubmission; + public Buffer Workers; + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public int SetupTaskCounts(int slotCount, int slotsPerTaskTarget, int workerIndex) + { + Debug.Assert(BitOperations.IsPow2(slotsPerTaskTarget), "This mask trick requires power of 2 task size targets."); + var mask = slotsPerTaskTarget - 1; + int taskCount = Math.Min(MaximumTaskCountPerSubmission, (slotCount + mask) & mask); + ref var worker = ref Workers[workerIndex]; + worker.SlotsPerTaskBase = slotCount / taskCount; + worker.SlotRemainder = slotCount - taskCount * worker.SlotsPerTaskBase; + worker.LessTasksThanWorkers = taskCount < Workers.Length; + return taskCount; } } + //These should be powers of 2 for maskhack reasons. const int SubtreesPerThreadForCentroidPrepass = 2048; const int SubtreesPerThreadForBinning = 512; const int SubtreesPerThreadForNodeJob = 512; @@ -422,7 +455,7 @@ static BoundingBox4 ComputeCentroidBounds(Buffer bounds) unsafe static void CentroidPrepassWorker(long taskId, void* untypedContext, int workerIndex) { - ref var context = ref *(MultithreadBinnedBuildContext*)untypedContext; + ref var context = ref *(BinnedBuildWorkerContext*)untypedContext; context.GetSlotInterval(taskId, out var start, out var count); var centroidBounds = ComputeCentroidBounds(context.Bounds.Slice(start, count)); if (context.LessTasksThanWorkers) @@ -440,30 +473,88 @@ unsafe static void CentroidPrepassWorker(long taskId, void* untypedContext, int unsafe static BoundingBox4 MultithreadedCentroidPrepass(MultithreadBinnedBuildContext* context, int subtreeCount, int workerIndex) { - int taskCount = Math.Min(context->MaximumTaskCountPerSubmission, (subtreeCount + SubtreesPerThreadForCentroidPrepass - 1) / SubtreesPerThreadForCentroidPrepass); + var taskCount = context->SetupTaskCounts(subtreeCount, SubtreesPerThreadForCentroidPrepass, workerIndex); //Don't bother initializing more slots than we have tasks. Note that this requires special handling on the task level; //if we have less tasks than workers, then the task needs to distinguish that fact. - var initializationCount = Math.Min(context->WorkerPrepassBounds.Length, taskCount); - context->LessTasksThanWorkers = initializationCount < context->WorkerPrepassBounds.Length; + var initializationCount = Math.Min(context->Workers.Length, taskCount); + ref var worker = ref context->Workers[workerIndex]; for (int i = 0; i < initializationCount; ++i) { - ref var workerBounds = ref context->WorkerPrepassBounds[i]; + ref var workerBounds = ref worker.WorkerPrepassBounds[i]; workerBounds.Min = new Vector4(float.MaxValue); workerBounds.Max = new Vector4(float.MinValue); } - context->SlotsPerTaskBase = subtreeCount / taskCount; - context->SlotRemainder = subtreeCount - context->SlotsPerTaskBase * taskCount; - context->Queue.For(&CentroidPrepassWorker, context, 0, taskCount, workerIndex); + context->Queue->For(&CentroidPrepassWorker, context, 0, taskCount, workerIndex); - var centroidBounds = context->WorkerPrepassBounds[0]; + var centroidBounds = worker.WorkerPrepassBounds[0]; for (int i = 1; i < initializationCount; ++i) { - ref var workerBounds = ref context->WorkerPrepassBounds[i]; + ref var workerBounds = ref worker.WorkerPrepassBounds[i]; centroidBounds.Min = Vector4.Min(workerBounds.Min, centroidBounds.Min); centroidBounds.Max = Vector4.Max(workerBounds.Max, centroidBounds.Max); } return centroidBounds; + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + static void BinSubtrees(BoundingBox4 centroidBounds, + bool useX, bool useY, Vector128 permuteMask, int axisIndex, Vector4 offsetToBinIndex, Vector4 maximumBinIndex, + Buffer bounds, TLeafCounts leafCounts, + Buffer binBoundingBoxes, Buffer binLeafCounts) where TLeafCounts : unmanaged, ILeafCountBuffer + { + //Note that we don't store out any of the indices into per-bin lists here. We only *really* want two final groups for the children, + //and we can easily compute those by performing another scan. It requires recomputing the bin indices, but that's really not much of a concern. + for (int i = 0; i < bounds.Length; ++i) + { + ref var box = ref bounds[i]; + var binIndex = ComputeBinIndex(centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, box); + ref var xBounds = ref binBoundingBoxes[binIndex]; + xBounds.Min = Vector4.Min(xBounds.Min, box.Min); + xBounds.Max = Vector4.Max(xBounds.Max, box.Max); + binLeafCounts[binIndex] += leafCounts[i]; + } + } + unsafe static void BinSubtreesWorker(long taskId, void* untypedContext, int workerIndex) where TLeafCounts : unmanaged, ILeafCountBuffer + { + ref var context = ref *(BinnedBuildWorkerContext*)untypedContext; + context.GetSlotInterval(taskId, out var start, out var count); + //BinSubtrees(context.Bounds.Slice(start, count)); + //if (context.LessTasksThanWorkers) + //{ + // //There were less tasks than workers; directly write into the slot without bothering to merge. + // context.WorkerPrepassBounds[(int)taskId] = centroidBounds; + //} + //else + //{ + // ref var workerBounds = ref context.WorkerPrepassBounds[workerIndex]; + // workerBounds.Min = Vector4.Min(workerBounds.Min, centroidBounds.Min); + // workerBounds.Max = Vector4.Max(workerBounds.Max, centroidBounds.Max); + //} + } + + unsafe static void MultithreadedBinSubtrees(MultithreadBinnedBuildContext* context, int subtreeCount, int workerIndex) where TLeafCounts : unmanaged, ILeafCountBuffer + { + var taskCount = context->SetupTaskCounts(subtreeCount, SubtreesPerThreadForBinning, workerIndex); + //Don't bother initializing more slots than we have tasks. Note that this requires special handling on the task level; + //if we have less tasks than workers, then the task needs to distinguish that fact. + var initializationCount = Math.Min(context->Workers.Length, taskCount); + ref var worker = ref context->Workers[workerIndex]; + for (int i = 0; i < initializationCount; ++i) + { + ref var workerBounds = ref worker.WorkerPrepassBounds[i]; + workerBounds.Min = new Vector4(float.MaxValue); + workerBounds.Max = new Vector4(float.MinValue); + } + context->Queue->For(&BinSubtreesWorker, context, 0, taskCount, workerIndex); + + //var centroidBounds = worker.WorkerPrepassBounds[0]; + //for (int i = 1; i < initializationCount; ++i) + //{ + // ref var workerBounds = ref worker.WorkerPrepassBounds[i]; + // centroidBounds.Min = Vector4.Min(workerBounds.Min, centroidBounds.Min); + // centroidBounds.Max = Vector4.Max(workerBounds.Max, centroidBounds.Max); + //} + //return centroidBounds; } static unsafe void BinnedBuilderInternal(Buffer indices, TLeafCounts leafCounts, ref TLeaves leaves, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, @@ -483,7 +574,8 @@ static unsafe void BinnedBuilderInternal(Buffe } else { - centroidBounds = MultithreadedCentroidPrepass((MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As(ref context->Threading)), subtreeCount, workerIndex); + centroidBounds = MultithreadedCentroidPrepass( + (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As(ref context->Threading)), subtreeCount, workerIndex); } var centroidSpan = centroidBounds.Max - centroidBounds.Min; var axisIsDegenerate = Vector128.LessThanOrEqual(centroidSpan.AsVector128(), Vector128.Create(1e-12f)); @@ -553,21 +645,12 @@ static unsafe void BinnedBuilderInternal(Buffe var maximumBinIndex = new Vector4(binCount - 1); if (typeof(TThreading) == typeof(SingleThreaded) || subtreeCount < SubtreesPerThreadForBinning) { - //Note that we don't store out any of the indices into per-bin lists here. We only *really* want two final groups for the children, - //and we can easily compute those by performing another scan. It requires recomputing the bin indices, but that's really not much of a concern. - for (int i = 0; i < subtreeCount; ++i) - { - ref var box = ref boundingBoxes[i]; - var binIndex = ComputeBinIndex(centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, box); - ref var xBounds = ref context->BinBoundingBoxes[binIndex]; - xBounds.Min = Vector4.Min(xBounds.Min, box.Min); - xBounds.Max = Vector4.Max(xBounds.Max, box.Max); - context->BinLeafCounts[binIndex] += leafCounts[i]; - } + BinSubtrees(centroidBounds, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, boundingBoxes, leafCounts, context->BinBoundingBoxes, context->BinLeafCounts); } else { - //Multithreading binning! + MultithreadedBinSubtrees( + (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As(ref context->Threading)), subtreeCount, workerIndex); } //Identify the split index by examining the SAH of very split option. From c5d00adb453a31b9cc67297e7b5fa1c2692e11ef Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 11 Nov 2022 16:22:49 -0600 Subject: [PATCH 647/947] MT binning mostly implemented, some stragglerbits. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 172 ++++++++++++++++-------- 1 file changed, 115 insertions(+), 57 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index b42885f1c..1db540028 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -5,6 +5,7 @@ using BepuUtilities.Collections; using BepuUtilities.Memory; using System; +using System.ComponentModel; using System.Diagnostics; using System.Linq; using System.Net; @@ -374,28 +375,49 @@ private static unsafe int ComputeBinIndex(Vector4 centroidMin, bool useX, bool u struct SingleThreaded { } + struct BinSubtreesWorkerContext + { + public Buffer BinBoundingBoxes; + public Buffer BinLeafCounts; + } + /// /// Stores resources required by a worker to dispatch and manage multithreaded work. /// - unsafe struct BinnedBuildWorkerContext + unsafe struct BinnedBuildWorkerContext where TLeafCounts : unmanaged, ILeafCountBuffer { /// /// Stores per-worker prepass bounds accumulated over multiple tasks. If there are less tasks than workers, then only the lower contiguous region of these bounds are used. /// - public Buffer WorkerPrepassBounds; + public Buffer PrepassWorkers; + public Buffer BinSubtreesWorkers; + + public int BinCount; + public Vector4 CentroidBoundsMin; + public bool UseX, UseY; + public Vector128 PermuteMask; + public int AxisIndex; + public Vector4 OffsetToBinIndex; + public Vector4 MaximumBinIndex; /// /// Bounds associated with the node that invoked this multithreaded job. /// public Buffer Bounds; + /// + /// Leaf counts associated with the node that invoked this multithreaded job. + /// + public TLeafCounts LeafCounts; + public int SlotsPerTaskBase; public int SlotRemainder; - public bool LessTasksThanWorkers; + public bool TaskCountFitsInWorkerCount; - public BinnedBuildWorkerContext(Buffer workerPrepassBounds) + public BinnedBuildWorkerContext(Buffer prepassWorkers, Buffer binSubtreesWorkers) { //We let the caller preallocate the bounds to avoid an individual allocation for every worker. - WorkerPrepassBounds = workerPrepassBounds; + PrepassWorkers = prepassWorkers; + BinSubtreesWorkers = binSubtreesWorkers; } public void GetSlotInterval(long taskId, out int start, out int count) @@ -407,7 +429,7 @@ public void GetSlotInterval(long taskId, out int start, out int count) count = taskId > SlotRemainder ? SlotsPerTaskBase : SlotsPerTaskBase + 1; } } - unsafe struct MultithreadBinnedBuildContext + unsafe struct MultithreadBinnedBuildContext where TLeafCounts : unmanaged, ILeafCountBuffer { public TaskQueue* Queue; /// @@ -415,7 +437,8 @@ unsafe struct MultithreadBinnedBuildContext /// If you have far more tasks than there are workers, adding more tasks just adds overhead without additional workstealing advantages. /// public int MaximumTaskCountPerSubmission; - public Buffer Workers; + public Buffer> Workers; + [MethodImpl(MethodImplOptions.AggressiveInlining)] public int SetupTaskCounts(int slotCount, int slotsPerTaskTarget, int workerIndex) @@ -426,7 +449,7 @@ public int SetupTaskCounts(int slotCount, int slotsPerTaskTarget, int workerInde ref var worker = ref Workers[workerIndex]; worker.SlotsPerTaskBase = slotCount / taskCount; worker.SlotRemainder = slotCount - taskCount * worker.SlotsPerTaskBase; - worker.LessTasksThanWorkers = taskCount < Workers.Length; + worker.TaskCountFitsInWorkerCount = taskCount <= Workers.Length; return taskCount; } } @@ -453,43 +476,47 @@ static BoundingBox4 ComputeCentroidBounds(Buffer bounds) return centroidBounds; } - unsafe static void CentroidPrepassWorker(long taskId, void* untypedContext, int workerIndex) + unsafe static void CentroidPrepassWorker(long taskId, void* untypedContext, int workerIndex) where TLeafCounts : unmanaged, ILeafCountBuffer { - ref var context = ref *(BinnedBuildWorkerContext*)untypedContext; + ref var context = ref *(BinnedBuildWorkerContext*)untypedContext; context.GetSlotInterval(taskId, out var start, out var count); var centroidBounds = ComputeCentroidBounds(context.Bounds.Slice(start, count)); - if (context.LessTasksThanWorkers) + if (context.TaskCountFitsInWorkerCount) { //There were less tasks than workers; directly write into the slot without bothering to merge. - context.WorkerPrepassBounds[(int)taskId] = centroidBounds; + context.PrepassWorkers[(int)taskId] = centroidBounds; } else { - ref var workerBounds = ref context.WorkerPrepassBounds[workerIndex]; + ref var workerBounds = ref context.PrepassWorkers[workerIndex]; workerBounds.Min = Vector4.Min(workerBounds.Min, centroidBounds.Min); workerBounds.Max = Vector4.Max(workerBounds.Max, centroidBounds.Max); } } - unsafe static BoundingBox4 MultithreadedCentroidPrepass(MultithreadBinnedBuildContext* context, int subtreeCount, int workerIndex) + unsafe static BoundingBox4 MultithreadedCentroidPrepass(MultithreadBinnedBuildContext* context, int subtreeCount, int workerIndex) where TLeafCounts : unmanaged, ILeafCountBuffer { var taskCount = context->SetupTaskCounts(subtreeCount, SubtreesPerThreadForCentroidPrepass, workerIndex); //Don't bother initializing more slots than we have tasks. Note that this requires special handling on the task level; //if we have less tasks than workers, then the task needs to distinguish that fact. - var initializationCount = Math.Min(context->Workers.Length, taskCount); + var activeWorkerCount = Math.Min(context->Workers.Length, taskCount); ref var worker = ref context->Workers[workerIndex]; - for (int i = 0; i < initializationCount; ++i) + if (taskCount > context->Workers.Length) { - ref var workerBounds = ref worker.WorkerPrepassBounds[i]; - workerBounds.Min = new Vector4(float.MaxValue); - workerBounds.Max = new Vector4(float.MinValue); + //Potentially multiple tasks per worker; we must preinitialize slots. + for (int i = 0; i < activeWorkerCount; ++i) + { + ref var workerBounds = ref worker.PrepassWorkers[i]; + workerBounds.Min = new Vector4(float.MaxValue); + workerBounds.Max = new Vector4(float.MinValue); + } } - context->Queue->For(&CentroidPrepassWorker, context, 0, taskCount, workerIndex); + context->Queue->For(&CentroidPrepassWorker, context, 0, taskCount, workerIndex); - var centroidBounds = worker.WorkerPrepassBounds[0]; - for (int i = 1; i < initializationCount; ++i) + var centroidBounds = worker.PrepassWorkers[0]; + for (int i = 1; i < activeWorkerCount; ++i) { - ref var workerBounds = ref worker.WorkerPrepassBounds[i]; + ref var workerBounds = ref worker.PrepassWorkers[i]; centroidBounds.Min = Vector4.Min(workerBounds.Min, centroidBounds.Min); centroidBounds.Max = Vector4.Max(workerBounds.Max, centroidBounds.Max); } @@ -497,7 +524,7 @@ unsafe static BoundingBox4 MultithreadedCentroidPrepass(MultithreadBinnedBuildCo } [MethodImpl(MethodImplOptions.AggressiveInlining)] - static void BinSubtrees(BoundingBox4 centroidBounds, + static void BinSubtrees(Vector4 centroidBoundsMin, bool useX, bool useY, Vector128 permuteMask, int axisIndex, Vector4 offsetToBinIndex, Vector4 maximumBinIndex, Buffer bounds, TLeafCounts leafCounts, Buffer binBoundingBoxes, Buffer binLeafCounts) where TLeafCounts : unmanaged, ILeafCountBuffer @@ -507,7 +534,7 @@ static void BinSubtrees(BoundingBox4 centroidBounds, for (int i = 0; i < bounds.Length; ++i) { ref var box = ref bounds[i]; - var binIndex = ComputeBinIndex(centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, box); + var binIndex = ComputeBinIndex(centroidBoundsMin, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, box); ref var xBounds = ref binBoundingBoxes[binIndex]; xBounds.Min = Vector4.Min(xBounds.Min, box.Min); xBounds.Max = Vector4.Max(xBounds.Max, box.Max); @@ -516,45 +543,75 @@ static void BinSubtrees(BoundingBox4 centroidBounds, } unsafe static void BinSubtreesWorker(long taskId, void* untypedContext, int workerIndex) where TLeafCounts : unmanaged, ILeafCountBuffer { - ref var context = ref *(BinnedBuildWorkerContext*)untypedContext; + ref var context = ref *(BinnedBuildWorkerContext*)untypedContext; + //Note that if we have more workers than tasks, we use the task id to index into the caches (and initialize the data here rather then before dispatching). + ref var worker = ref context.BinSubtreesWorkers[context.TaskCountFitsInWorkerCount ? (int)taskId : workerIndex]; + if (context.TaskCountFitsInWorkerCount) + { + for (int i = 0; i < context.BinCount; ++i) + { + ref var binBounds = ref worker.BinBoundingBoxes[i]; + binBounds.Min = new Vector4(float.MaxValue); + binBounds.Max = new Vector4(float.MinValue); + worker.BinLeafCounts[i] = 0; + } + } context.GetSlotInterval(taskId, out var start, out var count); - //BinSubtrees(context.Bounds.Slice(start, count)); - //if (context.LessTasksThanWorkers) - //{ - // //There were less tasks than workers; directly write into the slot without bothering to merge. - // context.WorkerPrepassBounds[(int)taskId] = centroidBounds; - //} - //else - //{ - // ref var workerBounds = ref context.WorkerPrepassBounds[workerIndex]; - // workerBounds.Min = Vector4.Min(workerBounds.Min, centroidBounds.Min); - // workerBounds.Max = Vector4.Max(workerBounds.Max, centroidBounds.Max); - //} + BinSubtrees(context.CentroidBoundsMin, context.UseX, context.UseY, context.PermuteMask, context.AxisIndex, context.OffsetToBinIndex, context.MaximumBinIndex, + context.Bounds.Slice(start, count), context.LeafCounts.Slice(start, count), worker.BinBoundingBoxes, worker.BinLeafCounts); } - unsafe static void MultithreadedBinSubtrees(MultithreadBinnedBuildContext* context, int subtreeCount, int workerIndex) where TLeafCounts : unmanaged, ILeafCountBuffer + unsafe static void MultithreadedBinSubtrees(MultithreadBinnedBuildContext* context, + Vector4 centroidBoundsMin, bool useX, bool useY, Vector128 permuteMask, int axisIndex, Vector4 offsetToBinIndex, Vector4 maximumBinIndex, + Buffer bounds, TLeafCounts leafCounts, + Buffer binBoundingBoxes, Buffer binLeafCounts, int binCount, int workerIndex) where TLeafCounts : unmanaged, ILeafCountBuffer { - var taskCount = context->SetupTaskCounts(subtreeCount, SubtreesPerThreadForBinning, workerIndex); + var taskCount = context->SetupTaskCounts(bounds.Length, SubtreesPerThreadForBinning, workerIndex); //Don't bother initializing more slots than we have tasks. Note that this requires special handling on the task level; //if we have less tasks than workers, then the task needs to distinguish that fact. - var initializationCount = Math.Min(context->Workers.Length, taskCount); + var activeWorkerCount = Math.Min(context->Workers.Length, taskCount); ref var worker = ref context->Workers[workerIndex]; - for (int i = 0; i < initializationCount; ++i) + if (!worker.TaskCountFitsInWorkerCount) { - ref var workerBounds = ref worker.WorkerPrepassBounds[i]; - workerBounds.Min = new Vector4(float.MaxValue); - workerBounds.Max = new Vector4(float.MinValue); + //If there are more tasks than workers, then we need to preinitialize all the worker caches. + for (int cacheIndex = 0; cacheIndex < activeWorkerCount; ++cacheIndex) + { + ref var cache = ref worker.BinSubtreesWorkers[cacheIndex]; + for (int i = 0; i < binCount; ++i) + { + ref var binBounds = ref cache.BinBoundingBoxes[i]; + binBounds.Min = new Vector4(float.MaxValue); + binBounds.Max = new Vector4(float.MinValue); + cache.BinLeafCounts[i] = 0; + } + } } + worker.BinCount = binCount; + worker.CentroidBoundsMin = centroidBoundsMin; + worker.UseX = useX; + worker.UseY = useY; + worker.PermuteMask = permuteMask; + worker.AxisIndex = axisIndex; + worker.OffsetToBinIndex = offsetToBinIndex; + worker.MaximumBinIndex = maximumBinIndex; + context->Queue->For(&BinSubtreesWorker, context, 0, taskCount, workerIndex); - //var centroidBounds = worker.WorkerPrepassBounds[0]; - //for (int i = 1; i < initializationCount; ++i) - //{ - // ref var workerBounds = ref worker.WorkerPrepassBounds[i]; - // centroidBounds.Min = Vector4.Min(workerBounds.Min, centroidBounds.Min); - // centroidBounds.Max = Vector4.Max(workerBounds.Max, centroidBounds.Max); - //} - //return centroidBounds; + //Unless the number of threads and bins is really huge, there's no value in attempting to multithread the final compression. + //(Parallel reduction is an option, but even then... I suspect the single threaded version will be faster. And it's way simpler.) + ref var cache0 = ref worker.BinSubtreesWorkers[0]; + for (int cacheIndex = 1; cacheIndex < activeWorkerCount; ++cacheIndex) + { + ref var cache = ref worker.BinSubtreesWorkers[cacheIndex]; + for (int binIndex = 0; binIndex < binCount; ++binIndex) + { + ref var b0 = ref cache0.BinBoundingBoxes[binIndex]; + ref var bi = ref cache.BinBoundingBoxes[binIndex]; + b0.Min = Vector4.Min(b0.Min, bi.Min); + b0.Max = Vector4.Min(b0.Max, bi.Max); + cache0.BinLeafCounts[binIndex] += cache.BinLeafCounts[binIndex]; + } + } } static unsafe void BinnedBuilderInternal(Buffer indices, TLeafCounts leafCounts, ref TLeaves leaves, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, @@ -575,7 +632,7 @@ static unsafe void BinnedBuilderInternal(Buffe else { centroidBounds = MultithreadedCentroidPrepass( - (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As(ref context->Threading)), subtreeCount, workerIndex); + (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As>(ref context->Threading)), subtreeCount, workerIndex); } var centroidSpan = centroidBounds.Max - centroidBounds.Min; var axisIsDegenerate = Vector128.LessThanOrEqual(centroidSpan.AsVector128(), Vector128.Create(1e-12f)); @@ -645,12 +702,13 @@ static unsafe void BinnedBuilderInternal(Buffe var maximumBinIndex = new Vector4(binCount - 1); if (typeof(TThreading) == typeof(SingleThreaded) || subtreeCount < SubtreesPerThreadForBinning) { - BinSubtrees(centroidBounds, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, boundingBoxes, leafCounts, context->BinBoundingBoxes, context->BinLeafCounts); + BinSubtrees(centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, boundingBoxes, leafCounts, context->BinBoundingBoxes, context->BinLeafCounts); } else { - MultithreadedBinSubtrees( - (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As(ref context->Threading)), subtreeCount, workerIndex); + MultithreadedBinSubtrees( + (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As>(ref context->Threading)), + centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, boundingBoxes, leafCounts, context->BinBoundingBoxes, context->BinLeafCounts, binCount, workerIndex); } //Identify the split index by examining the SAH of very split option. From d1ade64e1355f70902a0fef7a094081664982f63 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 11 Nov 2022 17:28:49 -0600 Subject: [PATCH 648/947] Bins now grabbed in a threading compatible way. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 120 ++++++++++++++---------- 1 file changed, 73 insertions(+), 47 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 1db540028..34e95fcde 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -110,11 +110,14 @@ struct LeafCountBuffer : ILeafCountBuffer public LeafCountBuffer Slice(int startIndex, int count) => new() { LeafCounts = LeafCounts.Slice(startIndex, count) }; } - struct Context where TThreading : unmanaged + interface IBinnedBuilderThreading + { + void GetBins(int workerIndex, out Buffer binBoundingBoxes, out Buffer binBoundingBoxesScan, out Buffer binLeafCounts); + } + + + struct Context where TThreading : unmanaged, IBinnedBuilderThreading { - public Buffer BinBoundingBoxes; - public Buffer BinBoundingBoxesScan; - public Buffer BinLeafCounts; public int MinimumBinCount; public int MaximumBinCount; public float LeafToBinMultiplier; @@ -128,9 +131,9 @@ struct BoundsComparerZ : IComparerRef { public int Compare(ref Bou static unsafe void MicroSweepForBinnedBuilder( Vector4 centroidMin, Vector4 centroidMax, Buffer indices, TLeafCounts leafCounts, ref TLeaves leaves, - Buffer boundingBoxes, Buffer nodes, Buffer metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, Context* context) + Buffer boundingBoxes, Buffer nodes, Buffer metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, Context* context, int workerIndex) where TLeafCounts : unmanaged, ILeafCountBuffer where TLeaves : unmanaged - where TThreading : unmanaged + where TThreading : unmanaged, IBinnedBuilderThreading { //This is a very small scale sweep build. var subtreeCount = indices.Length; @@ -140,15 +143,16 @@ static unsafe void MicroSweepForBinnedBuilder( return; } var centroidSpan = centroidMax - centroidMin; + context->Threading.GetBins(workerIndex, out var binBoundingBoxes, out var binBoundingBoxesScan, out var binLeafCounts); if (Vector256.IsHardwareAccelerated || Vector128.IsHardwareAccelerated) { //Repurpose the bins memory so we don't need to allocate any extra. The bins aren't in use right now anyway. int paddedKeyCount = Vector256.IsHardwareAccelerated ? ((subtreeCount + 7) / 8) * 8 : ((subtreeCount + 3) / 4) * 4; - Debug.Assert(Unsafe.SizeOf() * context->BinBoundingBoxes.Length >= (paddedKeyCount * 2 + subtreeCount) * Unsafe.SizeOf(), + Debug.Assert(Unsafe.SizeOf() * binBoundingBoxes.Length >= (paddedKeyCount * 2 + subtreeCount) * Unsafe.SizeOf(), "The bins should preallocate enough space to handle the needs of microsweeps. They reuse the same allocations."); - var keys = new Buffer(context->BinBoundingBoxes.Memory, paddedKeyCount); + var keys = new Buffer(binBoundingBoxes.Memory, paddedKeyCount); var targetIndices = new Buffer(keys.Memory + paddedKeyCount, paddedKeyCount); //Compute the axis centroids up front to avoid having to recompute them during a sort. @@ -189,8 +193,8 @@ static unsafe void MicroSweepForBinnedBuilder( { //There aren't any leaf counts that we need to copy; they're all just 1 anyway. Debug.Assert(typeof(TLeafCounts) == typeof(UnitLeafCount)); - var indicesCache = new Buffer(context->BinBoundingBoxes.Memory, subtreeCount); - var boundingBoxCache = context->BinBoundingBoxesScan; + var indicesCache = new Buffer(binBoundingBoxes.Memory, subtreeCount); + var boundingBoxCache = binBoundingBoxesScan; boundingBoxes.CopyTo(0, boundingBoxCache, 0, subtreeCount); indices.CopyTo(0, indicesCache, 0, subtreeCount); for (int i = 0; i < subtreeCount; ++i) @@ -203,9 +207,9 @@ static unsafe void MicroSweepForBinnedBuilder( else { //There are actual leaf counts we need to worry about on top of the rest! - var indicesCache = new Buffer(context->BinBoundingBoxes.Memory, subtreeCount); + var indicesCache = new Buffer(binBoundingBoxes.Memory, subtreeCount); var leafCountCache = new Buffer(targetIndices.Memory + subtreeCount, subtreeCount); - var boundingBoxCache = context->BinBoundingBoxesScan; + var boundingBoxCache = binBoundingBoxesScan; boundingBoxes.CopyTo(0, boundingBoxCache, 0, subtreeCount); indices.CopyTo(0, indicesCache, 0, subtreeCount); var leafCountBuffer = Unsafe.As(ref leafCounts).LeafCounts; @@ -243,7 +247,7 @@ static unsafe void MicroSweepForBinnedBuilder( else { //There are leaf counts that we need to sort alongside the rest. This is a pretty low value codepath, so we'll just create a targetIndices buffer. - var targetIndices = new Buffer(context->BinBoundingBoxes.Memory, subtreeCount); + var targetIndices = new Buffer(binBoundingBoxes.Memory, subtreeCount); for (int i = 0; i < subtreeCount; ++i) { targetIndices[i] = i; @@ -281,13 +285,13 @@ static unsafe void MicroSweepForBinnedBuilder( Debug.Assert(subtreeCount <= context->MaximumBinCount || subtreeCount < context->MicrosweepThreshold, "We're reusing the bin resources under the assumption that this is only ever called when there are less leaves than maximum bins."); //Identify the split index by examining the SAH of very split option. //Premerge from left to right so we have a sorta-summed area table to cheaply look up all possible child A bounds as we scan. - context->BinBoundingBoxesScan[0] = boundingBoxes[0]; + binBoundingBoxesScan[0] = boundingBoxes[0]; int totalLeafCount = typeof(TLeafCounts) == typeof(LeafCountBuffer) ? leafCounts[0] : subtreeCount; for (int i = 1; i < subtreeCount; ++i) { var previousIndex = i - 1; - ref var previousScanBounds = ref context->BinBoundingBoxesScan[previousIndex]; - ref var scanBounds = ref context->BinBoundingBoxesScan[i]; + ref var previousScanBounds = ref binBoundingBoxesScan[previousIndex]; + ref var scanBounds = ref binBoundingBoxesScan[i]; ref var bounds = ref boundingBoxes[i]; scanBounds.Min = Vector4.Min(bounds.Min, previousScanBounds.Min); scanBounds.Max = Vector4.Max(bounds.Max, previousScanBounds.Max); @@ -307,7 +311,7 @@ static unsafe void MicroSweepForBinnedBuilder( { var previousIndex = splitIndexCandidate - 1; var sahCandidate = - ComputeBoundsMetric(context->BinBoundingBoxesScan[previousIndex]) * (totalLeafCount - accumulatedLeafCountB) + + ComputeBoundsMetric(binBoundingBoxesScan[previousIndex]) * (totalLeafCount - accumulatedLeafCountB) + ComputeBoundsMetric(accumulatedBoundingBoxB) * accumulatedLeafCountB; if (sahCandidate < bestSAH) { @@ -323,7 +327,7 @@ static unsafe void MicroSweepForBinnedBuilder( accumulatedLeafCountB += leafCounts[previousIndex]; } - var bestBoundsA = context->BinBoundingBoxesScan[bestSplit - 1]; + var bestBoundsA = binBoundingBoxesScan[bestSplit - 1]; var subtreeCountA = bestSplit; var subtreeCountB = subtreeCount - bestSplit; var bestLeafCountA = typeof(TLeafCounts) == typeof(UnitLeafCount) ? subtreeCountA : totalLeafCount - bestLeafCountB; @@ -341,7 +345,7 @@ static unsafe void MicroSweepForBinnedBuilder( centroidBoundsA.Min = Vector4.Min(centroidBoundsA.Min, bounds.Min); centroidBoundsA.Max = Vector4.Max(centroidBoundsA.Max, bounds.Max); } - MicroSweepForBinnedBuilder(centroidBoundsA.Min, centroidBoundsA.Max, indices.Slice(subtreeCountA), leafCounts.Slice(0, subtreeCountA), ref leaves, aBounds, nodes.Slice(1, subtreeCountA - 1), metanodes.Slice(1, subtreeCountA - 1), aIndex, nodeIndex, 0, context); + MicroSweepForBinnedBuilder(centroidBoundsA.Min, centroidBoundsA.Max, indices.Slice(subtreeCountA), leafCounts.Slice(0, subtreeCountA), ref leaves, aBounds, nodes.Slice(1, subtreeCountA - 1), metanodes.Slice(1, subtreeCountA - 1), aIndex, nodeIndex, 0, context, workerIndex); } if (subtreeCountB > 1) { @@ -353,7 +357,7 @@ static unsafe void MicroSweepForBinnedBuilder( centroidBoundsB.Min = Vector4.Min(centroidBoundsB.Min, bounds.Min); centroidBoundsB.Max = Vector4.Max(centroidBoundsB.Max, bounds.Max); } - MicroSweepForBinnedBuilder(centroidBoundsB.Min, centroidBoundsB.Max, indices.Slice(subtreeCountA, subtreeCountB), leafCounts.Slice(subtreeCountA, subtreeCountB), ref leaves, bBounds, nodes.Slice(subtreeCountA, subtreeCountB - 1), metanodes.Slice(subtreeCountA, subtreeCountB - 1), bIndex, nodeIndex, 1, context); + MicroSweepForBinnedBuilder(centroidBoundsB.Min, centroidBoundsB.Max, indices.Slice(subtreeCountA, subtreeCountB), leafCounts.Slice(subtreeCountA, subtreeCountB), ref leaves, bBounds, nodes.Slice(subtreeCountA, subtreeCountB - 1), metanodes.Slice(subtreeCountA, subtreeCountB - 1), bIndex, nodeIndex, 1, context, workerIndex); } } @@ -373,7 +377,19 @@ private static unsafe int ComputeBinIndex(Vector4 centroidMin, bool useX, bool u return (int)(useX ? binIndicesForLeafContinuous.X : useY ? binIndicesForLeafContinuous.Y : binIndicesForLeafContinuous.Z); } - struct SingleThreaded { } + struct SingleThreaded : IBinnedBuilderThreading + { + public Buffer BinBoundingBoxes; + public Buffer BinBoundingBoxesScan; + public Buffer BinLeafCounts; + + public void GetBins(int workerIndex, out Buffer binBoundingBoxes, out Buffer binBoundingBoxesScan, out Buffer binLeafCounts) + { + binBoundingBoxes = BinBoundingBoxes; + binBoundingBoxesScan = BinBoundingBoxesScan; + binLeafCounts = BinLeafCounts; + } + } struct BinSubtreesWorkerContext { @@ -391,6 +407,7 @@ unsafe struct BinnedBuildWorkerContext where TLeafCounts : unmanage /// public Buffer PrepassWorkers; public Buffer BinSubtreesWorkers; + public Buffer BinBoundingBoxesScan; public int BinCount; public Vector4 CentroidBoundsMin; @@ -429,7 +446,7 @@ public void GetSlotInterval(long taskId, out int start, out int count) count = taskId > SlotRemainder ? SlotsPerTaskBase : SlotsPerTaskBase + 1; } } - unsafe struct MultithreadBinnedBuildContext where TLeafCounts : unmanaged, ILeafCountBuffer + unsafe struct MultithreadBinnedBuildContext : IBinnedBuilderThreading where TLeafCounts : unmanaged, ILeafCountBuffer { public TaskQueue* Queue; /// @@ -452,6 +469,15 @@ public int SetupTaskCounts(int slotCount, int slotsPerTaskTarget, int workerInde worker.TaskCountFitsInWorkerCount = taskCount <= Workers.Length; return taskCount; } + + public void GetBins(int workerIndex, out Buffer binBoundingBoxes, out Buffer binBoundingBoxesScan, out Buffer binLeafCounts) + { + ref var worker = ref Workers[workerIndex]; + ref var cache0 = ref worker.BinSubtreesWorkers[0]; + binBoundingBoxes = cache0.BinBoundingBoxes; + binBoundingBoxesScan = worker.BinBoundingBoxesScan; + binLeafCounts = cache0.BinLeafCounts; + } } //These should be powers of 2 for maskhack reasons. @@ -616,7 +642,7 @@ unsafe static void MultithreadedBinSubtrees(MultithreadBinnedBuildC static unsafe void BinnedBuilderInternal(Buffer indices, TLeafCounts leafCounts, ref TLeaves leaves, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, Context* context, int workerIndex) - where TLeafCounts : unmanaged, ILeafCountBuffer where TLeaves : unmanaged where TThreading : unmanaged + where TLeafCounts : unmanaged, ILeafCountBuffer where TLeaves : unmanaged where TThreading : unmanaged, IBinnedBuilderThreading { var subtreeCount = indices.Length; if (subtreeCount == 2) @@ -674,7 +700,7 @@ static unsafe void BinnedBuilderInternal(Buffe //Note that we don't bother even trying to internally multithread microsweeps. They *should* be small, and should only show up deeper in the recursion process. if (subtreeCount <= context->MicrosweepThreshold) { - MicroSweepForBinnedBuilder(centroidBounds.Min, centroidBounds.Max, indices, leafCounts, ref leaves, boundingBoxes, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, context); + MicroSweepForBinnedBuilder(centroidBounds.Min, centroidBounds.Max, indices, leafCounts, ref leaves, boundingBoxes, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, context, workerIndex); return; } @@ -685,7 +711,8 @@ static unsafe void BinnedBuilderInternal(Buffe var axisIndex = useX ? 0 : useY ? 1 : 2; var binCount = Math.Min(context->MaximumBinCount, Math.Max((int)(subtreeCount * context->LeafToBinMultiplier), context->MinimumBinCount)); - Debug.Assert(context->BinBoundingBoxes.Length >= binCount); + context->Threading.GetBins(workerIndex, out var binBoundingBoxes, out var binBoundingBoxesScan, out var binLeafCounts); + Debug.Assert(binBoundingBoxes.Length >= binCount); var offsetToBinIndex = new Vector4(binCount) / centroidSpan; //Avoid letting NaNs into the offsetToBinIndex scale. @@ -693,40 +720,40 @@ static unsafe void BinnedBuilderInternal(Buffe for (int i = 0; i < binCount; ++i) { - ref var boxX = ref context->BinBoundingBoxes[i]; + ref var boxX = ref binBoundingBoxes[i]; boxX.Min = new Vector4(float.MaxValue); boxX.Max = new Vector4(float.MinValue); - context->BinLeafCounts[i] = 0; + binLeafCounts[i] = 0; } var maximumBinIndex = new Vector4(binCount - 1); if (typeof(TThreading) == typeof(SingleThreaded) || subtreeCount < SubtreesPerThreadForBinning) { - BinSubtrees(centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, boundingBoxes, leafCounts, context->BinBoundingBoxes, context->BinLeafCounts); + BinSubtrees(centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, boundingBoxes, leafCounts, binBoundingBoxes, binLeafCounts); } else { MultithreadedBinSubtrees( (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As>(ref context->Threading)), - centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, boundingBoxes, leafCounts, context->BinBoundingBoxes, context->BinLeafCounts, binCount, workerIndex); + centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, boundingBoxes, leafCounts, binBoundingBoxes, binLeafCounts, binCount, workerIndex); } //Identify the split index by examining the SAH of very split option. //Premerge from left to right so we have a sorta-summed area table to cheaply look up all possible child A bounds as we scan. - context->BinBoundingBoxesScan[0] = context->BinBoundingBoxes[0]; - int totalLeafCount = typeof(TLeafCounts) == typeof(LeafCountBuffer) ? context->BinLeafCounts[0] : subtreeCount; + binBoundingBoxesScan[0] = binBoundingBoxes[0]; + int totalLeafCount = typeof(TLeafCounts) == typeof(LeafCountBuffer) ? binLeafCounts[0] : subtreeCount; for (int i = 1; i < binCount; ++i) { var previousIndex = i - 1; - ref var xBounds = ref context->BinBoundingBoxes[i]; - ref var xScanBounds = ref context->BinBoundingBoxesScan[i]; - ref var xPreviousScanBounds = ref context->BinBoundingBoxesScan[previousIndex]; + ref var xBounds = ref binBoundingBoxes[i]; + ref var xScanBounds = ref binBoundingBoxesScan[i]; + ref var xPreviousScanBounds = ref binBoundingBoxesScan[previousIndex]; xScanBounds.Min = Vector4.Min(xBounds.Min, xPreviousScanBounds.Min); xScanBounds.Max = Vector4.Max(xBounds.Max, xPreviousScanBounds.Max); if (typeof(TLeafCounts) == typeof(LeafCountBuffer)) - totalLeafCount += context->BinLeafCounts[i]; + totalLeafCount += binLeafCounts[i]; } - var leftBoundsX = context->BinBoundingBoxes[0]; + var leftBoundsX = binBoundingBoxes[0]; Debug.Assert( leftBoundsX.Min.X > float.MinValue && leftBoundsX.Min.Y > float.MinValue && leftBoundsX.Min.Z > float.MinValue, "Bin 0 should have been updated in all cases because it is aligned with the minimum bin, and the centroid span isn't degenerate."); @@ -736,15 +763,15 @@ static unsafe void BinnedBuilderInternal(Buffe //The split index is going to end up in child B. var lastBinIndex = binCount - 1; BoundingBox4 accumulatedBoundingBoxB; - accumulatedBoundingBoxB = context->BinBoundingBoxes[lastBinIndex]; + accumulatedBoundingBoxB = binBoundingBoxes[lastBinIndex]; BoundingBox4 bestBoundingBoxB; - bestBoundingBoxB = context->BinBoundingBoxes[lastBinIndex]; - int accumulatedLeafCountB = context->BinLeafCounts[lastBinIndex]; + bestBoundingBoxB = binBoundingBoxes[lastBinIndex]; + int accumulatedLeafCountB = binLeafCounts[lastBinIndex]; int bestLeafCountB = 0; for (int splitIndexCandidate = lastBinIndex; splitIndexCandidate >= 1; --splitIndexCandidate) { var previousIndex = splitIndexCandidate - 1; - var sahCandidate = ComputeBoundsMetric(context->BinBoundingBoxesScan[previousIndex]) * (totalLeafCount - accumulatedLeafCountB) + ComputeBoundsMetric(accumulatedBoundingBoxB) * accumulatedLeafCountB; + var sahCandidate = ComputeBoundsMetric(binBoundingBoxesScan[previousIndex]) * (totalLeafCount - accumulatedLeafCountB) + ComputeBoundsMetric(accumulatedBoundingBoxB) * accumulatedLeafCountB; if (sahCandidate < bestSAH) { @@ -754,17 +781,17 @@ static unsafe void BinnedBuilderInternal(Buffe if (typeof(TLeafCounts) == typeof(LeafCountBuffer)) bestLeafCountB = accumulatedLeafCountB; } - ref var xBounds = ref context->BinBoundingBoxes[previousIndex]; + ref var xBounds = ref binBoundingBoxes[previousIndex]; accumulatedBoundingBoxB.Min = Vector4.Min(xBounds.Min, accumulatedBoundingBoxB.Min); accumulatedBoundingBoxB.Max = Vector4.Max(xBounds.Max, accumulatedBoundingBoxB.Max); - accumulatedLeafCountB += context->BinLeafCounts[previousIndex]; + accumulatedLeafCountB += binLeafCounts[previousIndex]; } //Choose the best SAH from all axes and split the indices/bounds into two halves for the children to operate on. var subtreeCountB = 0; var subtreeCountA = 0; var splitIndex = bestSplit; - var bestboundsA = context->BinBoundingBoxesScan[bestSplit - 1]; + var bestboundsA = binBoundingBoxesScan[bestSplit - 1]; var bestboundsB = bestBoundingBoxB; //Now we have the split index between bins. Go back through and sort the indices and bounds into two halves. while (subtreeCountA + subtreeCountB < subtreeCount) @@ -855,17 +882,16 @@ public static unsafe void BinnedBuilder(Buffer encodedLeafIndices, Buffer context; - context.BinBoundingBoxes = new Buffer(binBoundsMemory, allocatedBinCount); - context.BinBoundingBoxesScan = new Buffer(binBoundsMemory + allocatedBinCount, allocatedBinCount); + context.Threading.BinBoundingBoxes = new Buffer(binBoundsMemory, allocatedBinCount); + context.Threading.BinBoundingBoxesScan = new Buffer(binBoundsMemory + allocatedBinCount, allocatedBinCount); var binLeafCountsMemory = stackalloc int[allocatedBinCount]; - context.BinLeafCounts = new Buffer(binLeafCountsMemory, allocatedBinCount); + context.Threading.BinLeafCounts = new Buffer(binLeafCountsMemory, allocatedBinCount); context.MinimumBinCount = minimumBinCount; context.MaximumBinCount = maximumBinCount; context.LeafToBinMultiplier = leafToBinMultiplier; context.MicrosweepThreshold = microsweepThreshold; - context.Threading = default; var leafCounts = new UnitLeafCount(); From 22dfe87ad0fe1c657f1ae2b1b6d493d1944d3a1e Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 12 Nov 2022 14:53:56 -0600 Subject: [PATCH 649/947] TaskQueue.WaitForCompletion. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 18 +++++- BepuUtilities/TaskQueue.cs | 66 ++++++++++++--------- Demos/DemoSet.cs | 2 +- Demos/SpecializedTests/TaskQueueTestDemo.cs | 3 +- 4 files changed, 56 insertions(+), 33 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 34e95fcde..0a965e95a 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -589,8 +589,7 @@ unsafe static void BinSubtreesWorker(long taskId, void* untypedCont unsafe static void MultithreadedBinSubtrees(MultithreadBinnedBuildContext* context, Vector4 centroidBoundsMin, bool useX, bool useY, Vector128 permuteMask, int axisIndex, Vector4 offsetToBinIndex, Vector4 maximumBinIndex, - Buffer bounds, TLeafCounts leafCounts, - Buffer binBoundingBoxes, Buffer binLeafCounts, int binCount, int workerIndex) where TLeafCounts : unmanaged, ILeafCountBuffer + Buffer bounds, TLeafCounts leafCounts, int binCount, int workerIndex) where TLeafCounts : unmanaged, ILeafCountBuffer { var taskCount = context->SetupTaskCounts(bounds.Length, SubtreesPerThreadForBinning, workerIndex); //Don't bother initializing more slots than we have tasks. Note that this requires special handling on the task level; @@ -625,6 +624,7 @@ unsafe static void MultithreadedBinSubtrees(MultithreadBinnedBuildC //Unless the number of threads and bins is really huge, there's no value in attempting to multithread the final compression. //(Parallel reduction is an option, but even then... I suspect the single threaded version will be faster. And it's way simpler.) + //Merge everything into cache 0. That's the source of bins for the worker that invoked this dispatch. ref var cache0 = ref worker.BinSubtreesWorkers[0]; for (int cacheIndex = 1; cacheIndex < activeWorkerCount; ++cacheIndex) { @@ -735,7 +735,7 @@ static unsafe void BinnedBuilderInternal(Buffe { MultithreadedBinSubtrees( (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As>(ref context->Threading)), - centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, boundingBoxes, leafCounts, binBoundingBoxes, binLeafCounts, binCount, workerIndex); + centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, boundingBoxes, leafCounts, binCount, workerIndex); } //Identify the split index by examining the SAH of very split option. @@ -838,15 +838,27 @@ static unsafe void BinnedBuilderInternal(Buffe BuildNode(bestboundsA, bestboundsB, leafCountA, leafCountB, nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, subtreeCountA, subtreeCountB, ref leaves, out var nodeChildIndexA, out var nodeChildIndexB); var shouldPushBOntoMultithreadedQueue = typeof(TThreading) != typeof(SingleThreaded) && subtreeCountA >= SubtreesPerThreadForNodeJob && subtreeCountB >= SubtreesPerThreadForNodeJob; + ContinuationHandle nodeBContinuation = default; if (shouldPushBOntoMultithreadedQueue) { //Both of the children are large. Push child B onto the multithreaded execution stack so it can run at the same time as child A (potentially). Debug.Assert(SubtreesPerThreadForNodeJob > 1, "The job threshold for a new node should be large enough that there's no need for a subtreeCountB > 1 test."); + ref var threading = ref Unsafe.As>(ref context->Threading); + //var task = new Task(&BinnedBuilderNodeWorker, &context->, ) + //nodeBContinuation = threading.Queue->AllocateContinuationAndEnqueueTasks(new Span(&task, 1), workerIndex); } if (subtreeCountA > 1) BinnedBuilderInternal(indices.Slice(subtreeCountA), leafCounts.Slice(0, subtreeCountA), ref leaves, boundingBoxes.Slice(subtreeCountA), nodes.Slice(1, subtreeCountA - 1), metanodes.Slice(1, subtreeCountA - 1), nodeChildIndexA, nodeIndex, 0, context, workerIndex); if (!shouldPushBOntoMultithreadedQueue && subtreeCountB > 1) BinnedBuilderInternal(indices.Slice(subtreeCountA, subtreeCountB), leafCounts.Slice(subtreeCountA, subtreeCountB), ref leaves, boundingBoxes.Slice(subtreeCountA, subtreeCountB), nodes.Slice(subtreeCountA, subtreeCountB - 1), metanodes.Slice(subtreeCountA, subtreeCountB - 1), nodeChildIndexB, nodeIndex, 1, context, workerIndex); + if (shouldPushBOntoMultithreadedQueue) + { + //We want to keep the stack at this level alive until the memory we allocated for the node push completes. + //Note that WaitForCompletion will execute pending work; this isn't just busywaiting the current thread. + //(This was just slightly easier than + Debug.Assert(nodeBContinuation.Initialized); + Unsafe.As>(ref context->Threading).Queue->WaitForCompletion(nodeBContinuation, workerIndex); + } } public static unsafe void BinnedBuilder(Buffer encodedLeafIndices, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, Buffer leaves, BufferPool pool, diff --git a/BepuUtilities/TaskQueue.cs b/BepuUtilities/TaskQueue.cs index b98680966..b2b73273e 100644 --- a/BepuUtilities/TaskQueue.cs +++ b/BepuUtilities/TaskQueue.cs @@ -31,13 +31,13 @@ public unsafe struct Task /// public void* Context; /// - /// User-provided identifier of this task. - /// - public long Id; - /// /// Continuation to be notified after this task completes, if any. /// public ContinuationHandle Continuation; + /// + /// User-provided identifier of this task. + /// + public long Id; /// /// Creates a new task. @@ -50,8 +50,8 @@ public Task(delegate* function, void* context = null, lo { Function = function; Context = context; - Id = taskId; Continuation = continuation; + Id = taskId; } public static implicit operator Task(delegate* function) => new Task(function); @@ -499,7 +499,7 @@ public DequeueTaskResult TryDequeue(out Task task) } if (nextTaskIndex >= sampledWrittenTaskIndex) return DequeueTaskResult.Empty; - var taskCandidate = tasks[(int)(nextTaskIndex & taskMask)]; + ref var taskCandidate = ref tasks[(int)(nextTaskIndex & taskMask)]; if (taskCandidate.Function == null) return DequeueTaskResult.Stop; //Unlike enqueues, a dequeue has a fixed contention window on a single value. There's not much point in using a spinwait when there's no reason to expect our next attempt to be blocked. @@ -665,6 +665,37 @@ public ContinuationHandle AllocateContinuationAndEnqueueTasks(Span tasks, return continuationHandle; } + /// + /// Waits for a continuation to be completed. + /// + /// Instead of spinning the entire time, this may dequeue and execute pending tasks to fill the gap. + /// Continuation to wait on. + /// Currently executing worker index to pass to any tasks used to fill the wait period. + public void WaitForCompletion(ContinuationHandle continuation, int workerIndex) + { + var waiter = new SpinWait(); + Debug.Assert(continuation.Initialized, "This codepath should only run if the continuation was allocated earlier."); + while (!continuation.Completed) + { + //Note that we don't handle the DequeueResult.Stop case; if the job isn't complete yet, there's no way to hit a stop unless we enqueued this job after a stop. + //Enqueuing after a stop is an error condition and is debug checked for in TryEnqueueJob. + var dequeueResult = TryDequeue(out var fillerTask); + if (dequeueResult == DequeueTaskResult.Stop) + { + Debug.Assert(dequeueResult != DequeueTaskResult.Stop, "Did you enqueue these tasks *after* some thread enqueued a stop command? That's illegal!"); + return; + } + if (dequeueResult == DequeueTaskResult.Success) + { + fillerTask.Run(workerIndex); + waiter.Reset(); + } + else + { + waiter.SpinOnce(-1); + } + } + } /// /// Appends a set of tasks to the queue and returns when all tasks are complete. @@ -703,28 +734,7 @@ public void RunTasks(Span tasks, int workerIndex) if (tasks.Length > 1) { //Task 0 is done; this thread should seek out other work until the job is complete. - var waiter = new SpinWait(); - Debug.Assert(continuationHandle.Initialized, "This codepath should only run if the continuation was allocated earlier."); - while (!continuationHandle.Completed) - { - //Note that we don't handle the DequeueResult.Stop case; if the job isn't complete yet, there's no way to hit a stop unless we enqueued this job after a stop. - //Enqueuing after a stop is an error condition and is debug checked for in TryEnqueueJob. - var dequeueResult = TryDequeue(out var fillerTask); - if (dequeueResult == DequeueTaskResult.Stop) - { - Debug.Assert(dequeueResult != DequeueTaskResult.Stop, "Did you enqueue these tasks *after* some thread enqueued a stop command? That's illegal!"); - return; - } - if (dequeueResult == DequeueTaskResult.Success) - { - fillerTask.Run(workerIndex); - waiter.Reset(); - } - else - { - waiter.SpinOnce(-1); - } - } + WaitForCompletion(continuationHandle, workerIndex); } } diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index fed59c61f..9a542f8b3 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -46,8 +46,8 @@ struct Option public DemoSet() { - AddOption(); AddOption(); + AddOption(); AddOption(); AddOption(); AddOption(); diff --git a/Demos/SpecializedTests/TaskQueueTestDemo.cs b/Demos/SpecializedTests/TaskQueueTestDemo.cs index f0675ef5b..b5bb3e469 100644 --- a/Demos/SpecializedTests/TaskQueueTestDemo.cs +++ b/Demos/SpecializedTests/TaskQueueTestDemo.cs @@ -7,6 +7,7 @@ using BepuPhysics; using BepuPhysics.Constraints; using System.Threading; +using System.Runtime.CompilerServices; namespace Demos.SpecializedTests; @@ -77,7 +78,7 @@ public override void Initialize(ContentArchive content, Camera camera) Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(4, 1)); - + Console.WriteLine($"Task size: {Unsafe.SizeOf()}"); int iterationCount = 4; From c3e40ed84994e585f4accd0b883c3200f1e9ef9d Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 12 Nov 2022 16:33:07 -0600 Subject: [PATCH 650/947] Partial refactor. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 58 +++++++++++++++++-------- Demos/DemoSet.cs | 2 +- 2 files changed, 40 insertions(+), 20 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 0a965e95a..6d5037848 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -116,12 +116,23 @@ interface IBinnedBuilderThreading } - struct Context where TThreading : unmanaged, IBinnedBuilderThreading + struct Context + where TLeafCounts : unmanaged, ILeafCountBuffer + where TLeaves : unmanaged + where TThreading : unmanaged, IBinnedBuilderThreading { public int MinimumBinCount; public int MaximumBinCount; public float LeafToBinMultiplier; public int MicrosweepThreshold; + + public Buffer Indices; + public TLeafCounts LeafCounts; + public TLeaves Leaves; + public Buffer BoundingBoxes; + public Buffer Nodes; + public Buffer Metanodes; + public TThreading Threading; } @@ -131,9 +142,8 @@ struct BoundsComparerZ : IComparerRef { public int Compare(ref Bou static unsafe void MicroSweepForBinnedBuilder( Vector4 centroidMin, Vector4 centroidMax, Buffer indices, TLeafCounts leafCounts, ref TLeaves leaves, - Buffer boundingBoxes, Buffer nodes, Buffer metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, Context* context, int workerIndex) - where TLeafCounts : unmanaged, ILeafCountBuffer where TLeaves : unmanaged - where TThreading : unmanaged, IBinnedBuilderThreading + Buffer boundingBoxes, Buffer nodes, Buffer metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, Context* context, int workerIndex) + where TLeafCounts : unmanaged, ILeafCountBuffer where TLeaves : unmanaged where TThreading : unmanaged, IBinnedBuilderThreading { //This is a very small scale sweep build. var subtreeCount = indices.Length; @@ -640,14 +650,19 @@ unsafe static void MultithreadedBinSubtrees(MultithreadBinnedBuildC } } - static unsafe void BinnedBuilderInternal(Buffer indices, TLeafCounts leafCounts, ref TLeaves leaves, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, - int nodeIndex, int parentNodeIndex, int childIndexInParent, Context* context, int workerIndex) + static unsafe void BinnedBuilderInternal( + int subtreeRegionStartIndex, int nodeIndex, int subtreeCount, int parentNodeIndex, int childIndexInParent, Context* context, int workerIndex) where TLeafCounts : unmanaged, ILeafCountBuffer where TLeaves : unmanaged where TThreading : unmanaged, IBinnedBuilderThreading { - var subtreeCount = indices.Length; + var indices = context->Indices.Slice(subtreeRegionStartIndex, subtreeCount); + var boundingBoxes = context->BoundingBoxes.Slice(subtreeRegionStartIndex, subtreeCount); + var leafCounts = context->LeafCounts.Slice(subtreeRegionStartIndex, subtreeCount); + var nodeCount = subtreeCount - 1; + var nodes = context->Nodes.Slice(nodeIndex, nodeCount); + var metanodes = context->Metanodes.Slice(nodeIndex, nodeCount); if (subtreeCount == 2) { - BuildNode(boundingBoxes[0], boundingBoxes[1], leafCounts[0], leafCounts[1], nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, 1, 1, ref leaves, out _, out _); + BuildNode(boundingBoxes[0], boundingBoxes[1], leafCounts[0], leafCounts[1], nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, 1, 1, ref context->Leaves, out _, out _); return; } BoundingBox4 centroidBounds; @@ -689,18 +704,18 @@ static unsafe void BinnedBuilderInternal(Buffe boundsB.Max = Vector4.Max(bounds.Max, boundsB.Max); degenerateLeafCountB += leafCounts[i]; } - BuildNode(boundsA, boundsB, degenerateLeafCountA, degenerateLeafCountB, nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, degenerateSubtreeCountA, degenerateSubtreeCountB, ref leaves, out var aIndex, out var bIndex); + BuildNode(boundsA, boundsB, degenerateLeafCountA, degenerateLeafCountB, nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, degenerateSubtreeCountA, degenerateSubtreeCountB, ref context->Leaves, out var aIndex, out var bIndex); if (degenerateSubtreeCountA > 1) - BinnedBuilderInternal(indices.Slice(degenerateSubtreeCountA), leafCounts.Slice(0, degenerateSubtreeCountA), ref leaves, boundingBoxes.Slice(degenerateSubtreeCountA), nodes.Slice(1, degenerateSubtreeCountA - 1), metanodes.Slice(1, degenerateSubtreeCountA - 1), aIndex, nodeIndex, 0, context, workerIndex); + BinnedBuilderInternal(subtreeRegionStartIndex, aIndex, degenerateSubtreeCountA, nodeIndex, 0, context, workerIndex); if (degenerateSubtreeCountB > 1) - BinnedBuilderInternal(indices.Slice(degenerateSubtreeCountA, degenerateSubtreeCountB), leafCounts.Slice(degenerateSubtreeCountA, degenerateSubtreeCountB), ref leaves, boundingBoxes.Slice(degenerateSubtreeCountA, degenerateSubtreeCountB), nodes.Slice(degenerateSubtreeCountA, degenerateSubtreeCountB - 1), metanodes.Slice(degenerateSubtreeCountA, degenerateSubtreeCountB - 1), bIndex, nodeIndex, 1, context, workerIndex); + BinnedBuilderInternal(subtreeRegionStartIndex + degenerateSubtreeCountA, bIndex, degenerateSubtreeCountB, nodeIndex, 1, context, workerIndex); return; } //Note that we don't bother even trying to internally multithread microsweeps. They *should* be small, and should only show up deeper in the recursion process. if (subtreeCount <= context->MicrosweepThreshold) { - MicroSweepForBinnedBuilder(centroidBounds.Min, centroidBounds.Max, indices, leafCounts, ref leaves, boundingBoxes, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, context, workerIndex); + MicroSweepForBinnedBuilder(centroidBounds.Min, centroidBounds.Max, indices, leafCounts, ref context->Leaves, boundingBoxes, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, context, workerIndex); return; } @@ -835,7 +850,7 @@ static unsafe void BinnedBuilderInternal(Buffe var leafCountB = typeof(TLeafCounts) == typeof(UnitLeafCount) ? subtreeCountB : bestLeafCountB; var leafCountA = typeof(TLeafCounts) == typeof(UnitLeafCount) ? subtreeCountA : totalLeafCount - leafCountB; Debug.Assert(subtreeCountA + subtreeCountB == subtreeCount); - BuildNode(bestboundsA, bestboundsB, leafCountA, leafCountB, nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, subtreeCountA, subtreeCountB, ref leaves, out var nodeChildIndexA, out var nodeChildIndexB); + BuildNode(bestboundsA, bestboundsB, leafCountA, leafCountB, nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, subtreeCountA, subtreeCountB, ref context->Leaves, out var nodeChildIndexA, out var nodeChildIndexB); var shouldPushBOntoMultithreadedQueue = typeof(TThreading) != typeof(SingleThreaded) && subtreeCountA >= SubtreesPerThreadForNodeJob && subtreeCountB >= SubtreesPerThreadForNodeJob; ContinuationHandle nodeBContinuation = default; @@ -848,14 +863,14 @@ static unsafe void BinnedBuilderInternal(Buffe //nodeBContinuation = threading.Queue->AllocateContinuationAndEnqueueTasks(new Span(&task, 1), workerIndex); } if (subtreeCountA > 1) - BinnedBuilderInternal(indices.Slice(subtreeCountA), leafCounts.Slice(0, subtreeCountA), ref leaves, boundingBoxes.Slice(subtreeCountA), nodes.Slice(1, subtreeCountA - 1), metanodes.Slice(1, subtreeCountA - 1), nodeChildIndexA, nodeIndex, 0, context, workerIndex); + BinnedBuilderInternal(subtreeRegionStartIndex, nodeChildIndexA, subtreeCountA, nodeIndex, 0, context, workerIndex); if (!shouldPushBOntoMultithreadedQueue && subtreeCountB > 1) - BinnedBuilderInternal(indices.Slice(subtreeCountA, subtreeCountB), leafCounts.Slice(subtreeCountA, subtreeCountB), ref leaves, boundingBoxes.Slice(subtreeCountA, subtreeCountB), nodes.Slice(subtreeCountA, subtreeCountB - 1), metanodes.Slice(subtreeCountA, subtreeCountB - 1), nodeChildIndexB, nodeIndex, 1, context, workerIndex); + BinnedBuilderInternal(subtreeRegionStartIndex + subtreeCountA, nodeChildIndexB, subtreeCountB, nodeIndex, 1, context, workerIndex); if (shouldPushBOntoMultithreadedQueue) { //We want to keep the stack at this level alive until the memory we allocated for the node push completes. //Note that WaitForCompletion will execute pending work; this isn't just busywaiting the current thread. - //(This was just slightly easier than + //(This was just slightly easier than managing heap allocations or doing other interthread communication.) Debug.Assert(nodeBContinuation.Initialized); Unsafe.As>(ref context->Threading).Queue->WaitForCompletion(nodeBContinuation, workerIndex); } @@ -893,7 +908,7 @@ public static unsafe void BinnedBuilder(Buffer encodedLeafIndices, Buffer context; + Context, SingleThreaded> context; context.Threading.BinBoundingBoxes = new Buffer(binBoundsMemory, allocatedBinCount); context.Threading.BinBoundingBoxesScan = new Buffer(binBoundsMemory + allocatedBinCount, allocatedBinCount); @@ -905,10 +920,15 @@ public static unsafe void BinnedBuilder(Buffer encodedLeafIndices, Buffer(); + context.Nodes = nodes; + context.Metanodes = metanodes; //While we could avoid a recursive implementation, the overhead is low compared to the per-iteration cost. - BinnedBuilderInternal(encodedLeafIndices, leafCounts, ref leaves, boundingBoxes.As(), nodes, metanodes, 0, -1, -1, &context, 0); + BinnedBuilderInternal(0, 0, subtreeCount, -1, -1, &context, 0); } } } diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index 9a542f8b3..fed59c61f 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -46,8 +46,8 @@ struct Option public DemoSet() { - AddOption(); AddOption(); + AddOption(); AddOption(); AddOption(); AddOption(); From 08061ea293612ca82002efddc856b38899870a16 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 12 Nov 2022 17:09:58 -0600 Subject: [PATCH 651/947] Node push. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 28 +++++++++++++++++++++++-- 1 file changed, 26 insertions(+), 2 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 6d5037848..65ec2dc1e 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -650,6 +650,24 @@ unsafe static void MultithreadedBinSubtrees(MultithreadBinnedBuildC } } + unsafe struct NodePushTaskContext + where TLeafCounts : unmanaged, ILeafCountBuffer where TLeaves : unmanaged where TThreading : unmanaged, IBinnedBuilderThreading + { + public Context* Context; + public int NodeIndex; + public int ParentNodeIndex; + //Subtree region start index and subtree count are both encoded into the task id. + } + unsafe static void BinnedBuilderNodeWorker(long taskId, void* context, int workerIndex) + where TLeafCounts : unmanaged, ILeafCountBuffer where TLeaves : unmanaged where TThreading : unmanaged, IBinnedBuilderThreading + { + var subtreeRegionStartIndex = (int)taskId; + var subtreeCount = (int)(taskId >> 32); + var wrappedContext = (NodePushTaskContext*)context; + //Note that child index is always 1 because we only ever push child B. + BinnedBuilderInternal(subtreeRegionStartIndex, wrappedContext->NodeIndex, subtreeCount, wrappedContext->ParentNodeIndex, 1, wrappedContext->Context, workerIndex); + } + static unsafe void BinnedBuilderInternal( int subtreeRegionStartIndex, int nodeIndex, int subtreeCount, int parentNodeIndex, int childIndexInParent, Context* context, int workerIndex) where TLeafCounts : unmanaged, ILeafCountBuffer where TLeaves : unmanaged where TThreading : unmanaged, IBinnedBuilderThreading @@ -859,8 +877,14 @@ static unsafe void BinnedBuilderInternal( //Both of the children are large. Push child B onto the multithreaded execution stack so it can run at the same time as child A (potentially). Debug.Assert(SubtreesPerThreadForNodeJob > 1, "The job threshold for a new node should be large enough that there's no need for a subtreeCountB > 1 test."); ref var threading = ref Unsafe.As>(ref context->Threading); - //var task = new Task(&BinnedBuilderNodeWorker, &context->, ) - //nodeBContinuation = threading.Queue->AllocateContinuationAndEnqueueTasks(new Span(&task, 1), workerIndex); + //Allocate the parameters to send to the worker on the local stack. Note that we have to preserve the stack for this to work; see the later WaitForCompletion. + NodePushTaskContext wrappedContext; + wrappedContext.Context = context; + wrappedContext.NodeIndex = nodeChildIndexB; + wrappedContext.ParentNodeIndex = nodeIndex; + //Note that we use the task id to store subtree start and subtree count. Don't have to do that, but no reason not to use it. + var task = new Task(&BinnedBuilderNodeWorker, &wrappedContext, (long)(subtreeRegionStartIndex + subtreeCountA) | ((long)subtreeCountB << 32)); + nodeBContinuation = threading.Queue->AllocateContinuationAndEnqueueTasks(new Span(&task, 1), workerIndex); } if (subtreeCountA > 1) BinnedBuilderInternal(subtreeRegionStartIndex, nodeChildIndexA, subtreeCountA, nodeIndex, 0, context, workerIndex); From 4b7e1ea009b66b2f59ce2170b53cb909cb46eca2 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 13 Nov 2022 22:11:23 -0600 Subject: [PATCH 652/947] Refactoring; fixed test performance bug. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 76 ++++++++++++------- .../SpecializedTests/TreeFiddlingTestDemo.cs | 35 ++++++--- 2 files changed, 71 insertions(+), 40 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 65ec2dc1e..3b79f9b66 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -134,6 +134,23 @@ struct Context public Buffer Metanodes; public TThreading Threading; + + public Context(int minimumBinCount, int maximumBinCount, float leafToBinMultiplier, int microsweepThreshold, + Buffer indices, TLeafCounts leafCounts, TLeaves leaves, Buffer boundingBoxes, + Buffer nodes, Buffer metanodes, TThreading threading) + { + MinimumBinCount = minimumBinCount; + MaximumBinCount = maximumBinCount; + LeafToBinMultiplier = leafToBinMultiplier; + MicrosweepThreshold = microsweepThreshold; + Indices = indices; + LeafCounts = leafCounts; + Leaves = leaves; + BoundingBoxes = boundingBoxes; + Nodes = nodes; + Metanodes = metanodes; + Threading = threading; + } } struct BoundsComparerX : IComparerRef { public int Compare(ref BoundingBox4 a, ref BoundingBox4 b) => (a.Min.X + a.Max.X) > (b.Min.X + b.Max.X) ? -1 : 1; } @@ -894,14 +911,18 @@ static unsafe void BinnedBuilderInternal( { //We want to keep the stack at this level alive until the memory we allocated for the node push completes. //Note that WaitForCompletion will execute pending work; this isn't just busywaiting the current thread. - //(This was just slightly easier than managing heap allocations or doing other interthread communication.) + //In addition to letting us use the local stack to store some arguments for the other thread, this wait means that all children have completed when this function returns. + //That makes knowing when to stop the queue easier. Debug.Assert(nodeBContinuation.Initialized); Unsafe.As>(ref context->Threading).Queue->WaitForCompletion(nodeBContinuation, workerIndex); } } - public static unsafe void BinnedBuilder(Buffer encodedLeafIndices, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, Buffer leaves, BufferPool pool, - int minimumBinCount = 16, int maximumBinCount = 64, float leafToBinMultiplier = 1 / 16f, int microsweepThreshold = 64) + + + + static unsafe void BinnedBuilderInternal(Buffer encodedLeafIndices, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, Buffer leaves, + IThreadDispatcher dispatcher, TaskQueue* taskQueue, int workerCount, int minimumBinCount, int maximumBinCount, float leafToBinMultiplier, int microsweepThreshold) { var subtreeCount = encodedLeafIndices.Length; Debug.Assert(boundingBoxes.Length >= subtreeCount, "The bounding boxes provided must cover the range of indices provided."); @@ -928,31 +949,30 @@ public static unsafe void BinnedBuilder(Buffer encodedLeafIndices, Buffer, SingleThreaded> context; - context.Threading.BinBoundingBoxes = new Buffer(binBoundsMemory, allocatedBinCount); - context.Threading.BinBoundingBoxesScan = new Buffer(binBoundsMemory + allocatedBinCount, allocatedBinCount); - - var binLeafCountsMemory = stackalloc int[allocatedBinCount]; - context.Threading.BinLeafCounts = new Buffer(binLeafCountsMemory, allocatedBinCount); - - context.MinimumBinCount = minimumBinCount; - context.MaximumBinCount = maximumBinCount; - context.LeafToBinMultiplier = leafToBinMultiplier; - context.MicrosweepThreshold = microsweepThreshold; - - context.Indices = encodedLeafIndices; - context.LeafCounts = new UnitLeafCount(); - context.Leaves = leaves; - context.BoundingBoxes = boundingBoxes.As(); - context.Nodes = nodes; - context.Metanodes = metanodes; - - //While we could avoid a recursive implementation, the overhead is low compared to the per-iteration cost. - BinnedBuilderInternal(0, 0, subtreeCount, -1, -1, &context, 0); + + if (dispatcher == null && taskQueue == null) + { + //Use the single threaded path. + var binBoundsMemory = stackalloc BoundingBox4[allocatedBinCount * 2 + 1]; + //Should be basically irrelevant, but just in case it's not on some platform, align the allocation. + binBoundsMemory = (BoundingBox4*)(((ulong)binBoundsMemory + 31ul) & (~31ul)); + + var binLeafCountsMemory = stackalloc int[allocatedBinCount]; + SingleThreaded threading; + threading.BinBoundingBoxes = new Buffer(binBoundsMemory, allocatedBinCount); + threading.BinBoundingBoxesScan = new Buffer(binBoundsMemory + allocatedBinCount, allocatedBinCount); + threading.BinLeafCounts = new Buffer(binLeafCountsMemory, allocatedBinCount); + var context = new Context, SingleThreaded>( + minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold, encodedLeafIndices, + new UnitLeafCount(), leaves, boundingBoxes.As(), nodes, metanodes, threading); + BinnedBuilderInternal(0, 0, subtreeCount, -1, -1, &context, 0); + } + } + + public static unsafe void BinnedBuilder(Buffer encodedLeafIndices, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, Buffer leaves, + int minimumBinCount = 16, int maximumBinCount = 64, float leafToBinMultiplier = 1 / 16f, int microsweepThreshold = 64) + { + BinnedBuilderInternal(encodedLeafIndices, boundingBoxes, nodes, metanodes, leaves, null, null, 0, minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold); } } } diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index 1d6d1a4d1..5c38b199a 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -176,19 +176,29 @@ public override void Initialize(ContentArchive content, Camera camera) BufferPool.Take(mesh.Triangles.Length, out var leafBounds); BufferPool.Take(mesh.Triangles.Length, out var leafIndices); - for (int i = 0; i < mesh.Triangles.Length; ++i) + + Action setup = () => { - ref var t = ref mesh.Triangles[i]; - ref var bounds = ref leafBounds[i]; - bounds.Min = Vector3.Min(t.A, Vector3.Min(t.B, t.C)); - bounds.Max = Vector3.Max(t.A, Vector3.Max(t.B, t.C)); - leafIndices[i] = Tree.Encode(i); - } + for (int i = 0; i < mesh.Triangles.Length; ++i) + { + ref var t = ref mesh.Triangles[i]; + ref var bounds = ref leafBounds[i]; + bounds.Min = Vector3.Min(t.A, Vector3.Min(t.B, t.C)); + bounds.Max = Vector3.Max(t.A, Vector3.Max(t.B, t.C)); + leafIndices[i] = Tree.Encode(i); + } + }; - BinnedTest(() => + BinnedTest(setup, () => { - Tree.BinnedBuilder(leafIndices, leafBounds, mesh.Tree.Nodes, mesh.Tree.Metanodes, mesh.Tree.Leaves, BufferPool); - }, "Revamp Single Axis", ref mesh.Tree); + Tree.BinnedBuilder(leafIndices, leafBounds, mesh.Tree.Nodes, mesh.Tree.Metanodes, mesh.Tree.Leaves); + }, "Revamp Single Axis MT", ref mesh.Tree); + + BinnedTest(setup, () => + { + Tree.BinnedBuilder(leafIndices, leafBounds, mesh.Tree.Nodes, mesh.Tree.Metanodes, mesh.Tree.Leaves); + }, "Revamp Single Axis ST", ref mesh.Tree); + var mesh2 = new Mesh(triangles, Vector3.One, BufferPool); @@ -196,7 +206,7 @@ public override void Initialize(ContentArchive content, Camera camera) QuickList subtreeReferences = new(mesh2.Tree.LeafCount, BufferPool); QuickList treeletInternalNodes = new(mesh2.Tree.LeafCount, BufferPool); Tree.CreateBinnedResources(BufferPool, mesh2.Tree.LeafCount, out var binnedResourcesBuffer, out var binnedResources); - BinnedTest(() => + BinnedTest(null, () => { subtreeReferences.Count = 0; treeletInternalNodes.Count = 0; @@ -255,12 +265,13 @@ static void RefitTest(Action function, string name, ref Tree tree) Console.WriteLine($"{name} bounds 0 hash: {hash}, A ({tree.Nodes[0].A.Min}, {tree.Nodes[0].B.Max}), B ({tree.Nodes[0].B.Min}, {tree.Nodes[0].B.Max})"); } - static void BinnedTest(Action function, string name, ref Tree tree) + static void BinnedTest(Action setup, Action function, string name, ref Tree tree) { long accumulatedTime = 0; const int testCount = 16; for (int i = 0; i < testCount; ++i) { + setup?.Invoke(); var startTime = Stopwatch.GetTimestamp(); function(); var endTime = Stopwatch.GetTimestamp(); From a8fa91bb645583fe00b1744d33d6ebb275b6530a Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 15 Nov 2022 14:35:41 -0600 Subject: [PATCH 653/947] Olden test fairified. --- .../SpecializedTests/TreeFiddlingTestDemo.cs | 28 +++++++++++-------- 1 file changed, 17 insertions(+), 11 deletions(-) diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index 5c38b199a..b3450db8e 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -189,10 +189,10 @@ public override void Initialize(ContentArchive content, Camera camera) } }; - BinnedTest(setup, () => - { - Tree.BinnedBuilder(leafIndices, leafBounds, mesh.Tree.Nodes, mesh.Tree.Metanodes, mesh.Tree.Leaves); - }, "Revamp Single Axis MT", ref mesh.Tree); + //BinnedTest(setup, () => + //{ + // Tree.BinnedBuilder(leafIndices, leafBounds, mesh.Tree.Nodes, mesh.Tree.Metanodes, mesh.Tree.Leaves); + //}, "Revamp Single Axis MT", ref mesh.Tree); BinnedTest(setup, () => { @@ -201,17 +201,23 @@ public override void Initialize(ContentArchive content, Camera camera) - var mesh2 = new Mesh(triangles, Vector3.One, BufferPool); + Mesh mesh2 = default; + Mesh* mesh2Pointer = &mesh2; - QuickList subtreeReferences = new(mesh2.Tree.LeafCount, BufferPool); - QuickList treeletInternalNodes = new(mesh2.Tree.LeafCount, BufferPool); - Tree.CreateBinnedResources(BufferPool, mesh2.Tree.LeafCount, out var binnedResourcesBuffer, out var binnedResources); - BinnedTest(null, () => + QuickList subtreeReferences = new(triangles.Length, BufferPool); + QuickList treeletInternalNodes = new(triangles.Length, BufferPool); + Tree.CreateBinnedResources(BufferPool, triangles.Length, out var binnedResourcesBuffer, out var binnedResources); + BinnedTest(() => + { + if (mesh2Pointer->Tree.Leaves.Allocated) + mesh2Pointer->Tree.Dispose(BufferPool); + *mesh2Pointer = DemoMeshHelper.CreateGiantMeshFast(triangles, Vector3.One, BufferPool); + }, () => { subtreeReferences.Count = 0; treeletInternalNodes.Count = 0; - mesh2.Tree.BinnedRefine(0, ref subtreeReferences, mesh2.Tree.LeafCount, ref treeletInternalNodes, ref binnedResources, BufferPool); - }, "Original", ref mesh2.Tree); + mesh2Pointer->Tree.BinnedRefine(0, ref subtreeReferences, mesh2Pointer->Tree.LeafCount, ref treeletInternalNodes, ref binnedResources, BufferPool); + }, "Original", ref mesh2Pointer->Tree); //RefitTest(() => mesh.Tree.Refit2(), "refit2", ref mesh.Tree); //RefitTest(() => mesh.Tree.Refit(), "Original", ref mesh.Tree); From b4cfbd1b20a1daa070f7e287297091ca7036185b Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 15 Nov 2022 22:10:04 -0600 Subject: [PATCH 654/947] MT build. Doesn't work. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 135 ++++++++++++++++-- BepuUtilities/TaskQueue.cs | 40 ++++-- Demos/Demo.cs | 2 +- .../SpecializedTests/TreeFiddlingTestDemo.cs | 15 +- 4 files changed, 154 insertions(+), 38 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 3b79f9b66..293a12226 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -15,6 +15,7 @@ using System.Runtime.Intrinsics; using System.Runtime.Intrinsics.Arm; using System.Runtime.Intrinsics.X86; +using System.Threading; using System.Threading.Tasks.Sources; namespace BepuPhysics.Trees @@ -457,11 +458,39 @@ unsafe struct BinnedBuildWorkerContext where TLeafCounts : unmanage public int SlotRemainder; public bool TaskCountFitsInWorkerCount; - public BinnedBuildWorkerContext(Buffer prepassWorkers, Buffer binSubtreesWorkers) + public static int ComputeRequiredSizeInBytes(int workerCount, int binCount) { - //We let the caller preallocate the bounds to avoid an individual allocation for every worker. - PrepassWorkers = prepassWorkers; - BinSubtreesWorkers = binSubtreesWorkers; + //Align the leaf counts allocation size so that the bounding box allocations stay aligned despite allocation interleaving. + var binLeafCountsSize = (binCount * Unsafe.SizeOf() + Unsafe.SizeOf() - 1) / Unsafe.SizeOf(); + var binSubtreesSize = binCount * Unsafe.SizeOf() + binLeafCountsSize; + return workerCount * (Unsafe.SizeOf() + Unsafe.SizeOf() + binSubtreesSize) + binCount * Unsafe.SizeOf(); + } + + static Buffer Suballocate(Buffer buffer, ref int start, int count) where T : unmanaged + { + var size = count * Unsafe.SizeOf(); + var previousStart = start; + start += size; + return buffer.Slice(previousStart, size).As(); + } + static Buffer SuballocateAligned(Buffer buffer, ref int start, int count, int alignmentMask) where T : unmanaged + { + Debug.Assert(BitOperations.IsPow2(alignmentMask + 1)); + start = (start + alignmentMask) & alignmentMask; + return Suballocate(buffer, ref start, count); + } + + public BinnedBuildWorkerContext(Buffer allocation, ref int allocationStart, int workerCount, int binCount) + { + PrepassWorkers = Suballocate(allocation, ref allocationStart, workerCount); + BinBoundingBoxesScan = Suballocate(allocation, ref allocationStart, binCount); + BinSubtreesWorkers = Suballocate(allocation, ref allocationStart, workerCount); + for (int i = 0; i < workerCount; ++i) + { + ref var binSubtreesWorker = ref BinSubtreesWorkers[i]; + binSubtreesWorker.BinBoundingBoxes = Suballocate(allocation, ref allocationStart, binCount); + binSubtreesWorker.BinLeafCounts = SuballocateAligned(allocation, ref allocationStart, binCount, Unsafe.SizeOf() - 1); + } } public void GetSlotInterval(long taskId, out int start, out int count) @@ -682,10 +711,10 @@ unsafe static void BinnedBuilderNodeWorker(lon var subtreeCount = (int)(taskId >> 32); var wrappedContext = (NodePushTaskContext*)context; //Note that child index is always 1 because we only ever push child B. - BinnedBuilderInternal(subtreeRegionStartIndex, wrappedContext->NodeIndex, subtreeCount, wrappedContext->ParentNodeIndex, 1, wrappedContext->Context, workerIndex); + BinnedBuildNode(subtreeRegionStartIndex, wrappedContext->NodeIndex, subtreeCount, wrappedContext->ParentNodeIndex, 1, wrappedContext->Context, workerIndex); } - static unsafe void BinnedBuilderInternal( + static unsafe void BinnedBuildNode( int subtreeRegionStartIndex, int nodeIndex, int subtreeCount, int parentNodeIndex, int childIndexInParent, Context* context, int workerIndex) where TLeafCounts : unmanaged, ILeafCountBuffer where TLeaves : unmanaged where TThreading : unmanaged, IBinnedBuilderThreading { @@ -741,9 +770,9 @@ static unsafe void BinnedBuilderInternal( } BuildNode(boundsA, boundsB, degenerateLeafCountA, degenerateLeafCountB, nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, degenerateSubtreeCountA, degenerateSubtreeCountB, ref context->Leaves, out var aIndex, out var bIndex); if (degenerateSubtreeCountA > 1) - BinnedBuilderInternal(subtreeRegionStartIndex, aIndex, degenerateSubtreeCountA, nodeIndex, 0, context, workerIndex); + BinnedBuildNode(subtreeRegionStartIndex, aIndex, degenerateSubtreeCountA, nodeIndex, 0, context, workerIndex); if (degenerateSubtreeCountB > 1) - BinnedBuilderInternal(subtreeRegionStartIndex + degenerateSubtreeCountA, bIndex, degenerateSubtreeCountB, nodeIndex, 1, context, workerIndex); + BinnedBuildNode(subtreeRegionStartIndex + degenerateSubtreeCountA, bIndex, degenerateSubtreeCountB, nodeIndex, 1, context, workerIndex); return; } @@ -901,12 +930,12 @@ static unsafe void BinnedBuilderInternal( wrappedContext.ParentNodeIndex = nodeIndex; //Note that we use the task id to store subtree start and subtree count. Don't have to do that, but no reason not to use it. var task = new Task(&BinnedBuilderNodeWorker, &wrappedContext, (long)(subtreeRegionStartIndex + subtreeCountA) | ((long)subtreeCountB << 32)); - nodeBContinuation = threading.Queue->AllocateContinuationAndEnqueueTasks(new Span(&task, 1), workerIndex); + nodeBContinuation = threading.Queue->AllocateContinuationAndEnqueue(new Span(&task, 1), workerIndex); } if (subtreeCountA > 1) - BinnedBuilderInternal(subtreeRegionStartIndex, nodeChildIndexA, subtreeCountA, nodeIndex, 0, context, workerIndex); + BinnedBuildNode(subtreeRegionStartIndex, nodeChildIndexA, subtreeCountA, nodeIndex, 0, context, workerIndex); if (!shouldPushBOntoMultithreadedQueue && subtreeCountB > 1) - BinnedBuilderInternal(subtreeRegionStartIndex + subtreeCountA, nodeChildIndexB, subtreeCountB, nodeIndex, 1, context, workerIndex); + BinnedBuildNode(subtreeRegionStartIndex + subtreeCountA, nodeChildIndexB, subtreeCountB, nodeIndex, 1, context, workerIndex); if (shouldPushBOntoMultithreadedQueue) { //We want to keep the stack at this level alive until the memory we allocated for the node push completes. @@ -922,7 +951,7 @@ static unsafe void BinnedBuilderInternal( static unsafe void BinnedBuilderInternal(Buffer encodedLeafIndices, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, Buffer leaves, - IThreadDispatcher dispatcher, TaskQueue* taskQueue, int workerCount, int minimumBinCount, int maximumBinCount, float leafToBinMultiplier, int microsweepThreshold) + IThreadDispatcher dispatcher, TaskQueue* taskQueuePointer, int workerCount, BufferPool pool, int minimumBinCount, int maximumBinCount, float leafToBinMultiplier, int microsweepThreshold) { var subtreeCount = encodedLeafIndices.Length; Debug.Assert(boundingBoxes.Length >= subtreeCount, "The bounding boxes provided must cover the range of indices provided."); @@ -950,7 +979,7 @@ static unsafe void BinnedBuilderInternal(Buffer encodedLeafIndices, Buffer< //The microsweep uses the same resources as the bin allocations, so expand to hold whichever is larger. var allocatedBinCount = Math.Max(maximumBinCount, microsweepThreshold); - if (dispatcher == null && taskQueue == null) + if (dispatcher == null && taskQueuePointer == null) { //Use the single threaded path. var binBoundsMemory = stackalloc BoundingBox4[allocatedBinCount * 2 + 1]; @@ -965,14 +994,90 @@ static unsafe void BinnedBuilderInternal(Buffer encodedLeafIndices, Buffer< var context = new Context, SingleThreaded>( minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold, encodedLeafIndices, new UnitLeafCount(), leaves, boundingBoxes.As(), nodes, metanodes, threading); - BinnedBuilderInternal(0, 0, subtreeCount, -1, -1, &context, 0); + BinnedBuildNode(0, 0, subtreeCount, -1, -1, &context, 0); + } + else if (dispatcher != null) + { + //There's a task queue; we should use a multithreaded dispatch. + //While we could allocate on the stack with reasonable safety in the single threaded path, that's not very reasonable for the multithreaded path. + //Each worker thread could be given a node job which executes asynchronously with respect to other node jobs. + //Those node jobs could spawn multithreaded work that other workers assist with. + //So, each worker working on a node job needs to have its own set of worker caches for other workers to help it with the node job, giving an allocation count + //proportional to workerCount * workerCount * binCount. Not a big problem for small bin counts on a quad core system, but how about a 64 core system with 128 threads and 64 bins? + //128 * 128 * 64 > 1e6, and that's before taking into account the size of the bins being allocated! + pool.Take(BinnedBuildWorkerContext.ComputeRequiredSizeInBytes(workerCount, allocatedBinCount), out var workerAllocation); + + BinnedBuildWorkerContext* workerContextsPointer = stackalloc BinnedBuildWorkerContext[workerCount]; + var workerContexts = new Buffer>(workerContextsPointer, workerCount); + + int allocatedByteCount = 0; + for (int i = 0; i < workerCount; ++i) + { + workerContexts[i] = new BinnedBuildWorkerContext(workerAllocation, ref allocatedByteCount, workerCount, allocatedBinCount); + } + + TaskQueue taskQueue = default; + bool createdTaskQueueLocally = taskQueuePointer == null; + if (taskQueuePointer == null) + { + taskQueue = new TaskQueue(pool); + taskQueuePointer = &taskQueue; + } + var threading = new MultithreadBinnedBuildContext + { + MaximumTaskCountPerSubmission = workerCount * 2, + Queue = taskQueuePointer, + Workers = workerContexts + }; + var context = new Context, MultithreadBinnedBuildContext>( + minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold, encodedLeafIndices, + new UnitLeafCount(), leaves, boundingBoxes.As(), nodes, metanodes, threading); + + taskQueuePointer->TryEnqueueUnsafely(new Task(&BinnedBuilderWorkerEntry>, &context)); + dispatcher.DispatchWorkers(&BinnedBuilderWorkerFunction>, taskQueuePointer); + + if (createdTaskQueueLocally) + taskQueue.Dispose(pool); + + + } + } + + public static unsafe void BinnedBuilder(Buffer encodedLeafIndices, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, Buffer leaves, + int minimumBinCount = 16, int maximumBinCount = 64, float leafToBinMultiplier = 1 / 16f, int microsweepThreshold = 64) + { + BinnedBuilderInternal(encodedLeafIndices, boundingBoxes, nodes, metanodes, leaves, null, null, 0, null, minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold); + } + + unsafe static void BinnedBuilderWorkerEntry(long taskId, void* untypedContext, int workerIndex) + where TLeafCounts : unmanaged, ILeafCountBuffer + where TLeaves : unmanaged + { + var context = (Context>*)untypedContext; + BinnedBuildNode(0, 0, context->Indices.Length, -1, -1, context, workerIndex); + //Once the entry point returns, all workers should stop because it won't return unless both nodes are done. + context->Threading.Queue->EnqueueStop(workerIndex); + + } + + unsafe static void BinnedBuilderWorkerFunction(int workerIndex, void* untypedContext) + where TLeafCounts : unmanaged, ILeafCountBuffer + where TLeaves : unmanaged + { + var taskQueue = (TaskQueue*)untypedContext; + DequeueTaskResult dequeueTaskResult; + var waiter = new SpinWait(); + while ((dequeueTaskResult = taskQueue->TryDequeueAndRun(workerIndex)) != DequeueTaskResult.Stop) + { + waiter.SpinOnce(-1); } } public static unsafe void BinnedBuilder(Buffer encodedLeafIndices, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, Buffer leaves, + IThreadDispatcher threadDispatcher, BufferPool pool, int minimumBinCount = 16, int maximumBinCount = 64, float leafToBinMultiplier = 1 / 16f, int microsweepThreshold = 64) { - BinnedBuilderInternal(encodedLeafIndices, boundingBoxes, nodes, metanodes, leaves, null, null, 0, minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold); + BinnedBuilderInternal(encodedLeafIndices, boundingBoxes, nodes, metanodes, leaves, threadDispatcher, null, threadDispatcher.ThreadCount, pool, minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold); } } } diff --git a/BepuUtilities/TaskQueue.cs b/BepuUtilities/TaskQueue.cs index b2b73273e..1e429c0de 100644 --- a/BepuUtilities/TaskQueue.cs +++ b/BepuUtilities/TaskQueue.cs @@ -526,7 +526,7 @@ public DequeueTaskResult TryDequeueAndRun(int workerIndex) return result; } - EnqueueTaskResult TryEnqueueTasksUnsafelyInternal(Span tasks, out long taskEndIndex) + EnqueueTaskResult TryEnqueueUnsafelyInternal(Span tasks, out long taskEndIndex) { Debug.Assert(tasks.Length > 0, "Probably shouldn't be trying to enqueue zero tasks."); Debug.Assert(writtenTaskIndex == 0 || this.tasks[(int)((writtenTaskIndex - 1) & taskMask)].Function != null, "No more jobs should be written after a stop command."); @@ -569,22 +569,32 @@ EnqueueTaskResult TryEnqueueTasksUnsafelyInternal(Span tasks, out long tas /// Tasks composing the job. /// Result of the enqueue attempt. /// This must not be used while other threads could be performing task enqueues or task dequeues. - public EnqueueTaskResult TryEnqueueTasksUnsafely(Span tasks) + public EnqueueTaskResult TryEnqueueUnsafely(Span tasks) { EnqueueTaskResult result; - if ((result = TryEnqueueTasksUnsafelyInternal(tasks, out var taskEndIndex)) == EnqueueTaskResult.Success) + if ((result = TryEnqueueUnsafelyInternal(tasks, out var taskEndIndex)) == EnqueueTaskResult.Success) { writtenTaskIndex = taskEndIndex; } return result; } + /// + /// Tries to appends a task to the task queue. Does not acquire a lock; cannot return . + /// + /// Task to enqueue + /// Result of the enqueue attempt. + /// This must not be used while other threads could be performing task enqueues or task dequeues. + public EnqueueTaskResult TryEnqueueUnsafely(Task task) + { + return TryEnqueueUnsafely(new Span(&task, 1)); + } /// /// Tries to appends a set of tasks to the task queue if the ring buffer is uncontested. /// /// Tasks composing the job. /// Result of the enqueue attempt. - public EnqueueTaskResult TryEnqueueTasks(Span tasks) + public EnqueueTaskResult TryEnqueue(Span tasks) { if (tasks.Length == 0) return EnqueueTaskResult.Success; @@ -594,7 +604,7 @@ public EnqueueTaskResult TryEnqueueTasks(Span tasks) { //We have the lock. EnqueueTaskResult result; - if ((result = TryEnqueueTasksUnsafelyInternal(tasks, out var taskEndIndex)) == EnqueueTaskResult.Success) + if ((result = TryEnqueueUnsafelyInternal(tasks, out var taskEndIndex)) == EnqueueTaskResult.Success) { if (Environment.Is64BitProcess) { @@ -620,11 +630,11 @@ public EnqueueTaskResult TryEnqueueTasks(Span tasks) /// Worker index to pass to inline-executed tasks if the task buffer is full. /// Note that this will keep trying until task submission succeeds. /// If the task queue is full, this will opt to run some tasks inline while waiting for room. - public void EnqueueTasks(Span tasks, int workerIndex) + public void Enqueue(Span tasks, int workerIndex) { var waiter = new SpinWait(); EnqueueTaskResult result; - while ((result = TryEnqueueTasks(tasks)) != EnqueueTaskResult.Success) + while ((result = TryEnqueue(tasks)) != EnqueueTaskResult.Success) { if (result == EnqueueTaskResult.Full) { @@ -652,7 +662,7 @@ public void EnqueueTasks(Span tasks, int workerIndex) /// Handle of the continuation created for these tasks. /// Note that this will keep trying until task submission succeeds. /// If the task queue is full, this will opt to run some tasks inline while waiting for room. - public ContinuationHandle AllocateContinuationAndEnqueueTasks(Span tasks, int workerIndex, Task onComplete = default) + public ContinuationHandle AllocateContinuationAndEnqueue(Span tasks, int workerIndex, Task onComplete = default) { var continuationHandle = AllocateContinuation(tasks.Length, workerIndex); for (int i = 0; i < tasks.Length; ++i) @@ -661,7 +671,7 @@ public ContinuationHandle AllocateContinuationAndEnqueueTasks(Span tasks, Debug.Assert(!task.Continuation.Initialized, "This function creates a continuation for the tasks"); task.Continuation = continuationHandle; } - EnqueueTasks(tasks, workerIndex); + Enqueue(tasks, workerIndex); return continuationHandle; } @@ -723,7 +733,7 @@ public void RunTasks(Span tasks, int workerIndex) task.Continuation = continuationHandle; tasksToEnqueue[i] = task; } - EnqueueTasks(tasksToEnqueue, workerIndex); + Enqueue(tasksToEnqueue, workerIndex); } //Tasks [1, count) are submitted to the queue and may now be executing on other workers. //The thread calling the for loop should not relinquish its timeslice. It should immediately begin working on task 0. @@ -746,7 +756,7 @@ public EnqueueTaskResult TryEnqueueStop() { Span stopJob = stackalloc Task[1]; stopJob[0] = new Task { Function = null }; - return TryEnqueueTasks(stopJob); + return TryEnqueue(stopJob); } /// @@ -761,7 +771,7 @@ public void EnqueueStop(int workerIndex) stopJob[0] = new Task { Function = null }; var waiter = new SpinWait(); EnqueueTaskResult result; - while ((result = TryEnqueueTasks(stopJob)) != EnqueueTaskResult.Success) + while ((result = TryEnqueue(stopJob)) != EnqueueTaskResult.Success) { if (result == EnqueueTaskResult.Full) { @@ -784,7 +794,7 @@ public EnqueueTaskResult TryEnqueueStopUnsafely() { Span stopJob = stackalloc Task[1]; stopJob[0] = new Task { Function = null }; - return TryEnqueueTasksUnsafely(stopJob); + return TryEnqueueUnsafely(stopJob); } /// @@ -804,7 +814,7 @@ public EnqueueTaskResult TryEnqueueForUnsafely(delegate* { tasks[i] = new Task { Function = function, Context = context, Id = i + inclusiveStartIndex, Continuation = continuation }; } - return TryEnqueueTasksUnsafely(tasks); + return TryEnqueueUnsafely(tasks); } /// @@ -825,7 +835,7 @@ public void EnqueueFor(delegate* function, void* context { tasks[i] = new Task { Function = function, Context = context, Id = i + inclusiveStartIndex, Continuation = continuation }; } - EnqueueTasks(tasks, workerIndex); + Enqueue(tasks, workerIndex); } /// diff --git a/Demos/Demo.cs b/Demos/Demo.cs index 690a83f81..34d90bbdf 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -43,7 +43,7 @@ protected Demo() //there won't be enough memory bandwidth to even feed half the physical cores. Using all 128 logical cores would just add overhead. //It may be worth using something like hwloc or CPUID to extract extra information to reason about. - var targetThreadCount = Math.Max(1, Environment.ProcessorCount > 4 ? Environment.ProcessorCount - 2 : Environment.ProcessorCount - 1); + var targetThreadCount = 2;// Math.Max(1, Environment.ProcessorCount > 4 ? Environment.ProcessorCount - 2 : Environment.ProcessorCount - 1); ThreadDispatcher = new ThreadDispatcher(targetThreadCount); } diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index b3450db8e..ad502ebc0 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -166,13 +166,14 @@ public override void Initialize(ContentArchive content, Camera camera) //var triangles = CreateDeformedPlaneTriangles(width, height, scale); var triangles = CreateRandomSoupTriangles(new BoundingBox(new(width / -2f, scale.Y * -2, height / -2f), new(width / 2f, scale.Y * 2, height / 2f)), (width - 1) * (height - 1) * 2, 0.5f, 100f); - var mesh = new Mesh(triangles, Vector3.One, BufferPool); + //var mesh = new Mesh(triangles, Vector3.One, BufferPool); + var mesh = DemoMeshHelper.CreateGiantMeshFast(triangles, Vector3.One, BufferPool); Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); Console.WriteLine($"node count: {mesh.Tree.NodeCount}"); - Console.WriteLine($"sweep SAH: {mesh.Tree.MeasureCostMetric()}, cache quality: {mesh.Tree.MeasureCacheQuality()}"); - Console.WriteLine($"sweep bounds: A ({mesh.Tree.Nodes[0].A.Min}, {mesh.Tree.Nodes[0].B.Max}), B ({mesh.Tree.Nodes[0].B.Min}, {mesh.Tree.Nodes[0].B.Max})"); + Console.WriteLine($"initial SAH: {mesh.Tree.MeasureCostMetric()}, cache quality: {mesh.Tree.MeasureCacheQuality()}"); + Console.WriteLine($"initial bounds: A ({mesh.Tree.Nodes[0].A.Min}, {mesh.Tree.Nodes[0].B.Max}), B ({mesh.Tree.Nodes[0].B.Min}, {mesh.Tree.Nodes[0].B.Max})"); BufferPool.Take(mesh.Triangles.Length, out var leafBounds); BufferPool.Take(mesh.Triangles.Length, out var leafIndices); @@ -189,10 +190,10 @@ public override void Initialize(ContentArchive content, Camera camera) } }; - //BinnedTest(setup, () => - //{ - // Tree.BinnedBuilder(leafIndices, leafBounds, mesh.Tree.Nodes, mesh.Tree.Metanodes, mesh.Tree.Leaves); - //}, "Revamp Single Axis MT", ref mesh.Tree); + BinnedTest(setup, () => + { + Tree.BinnedBuilder(leafIndices, leafBounds, mesh.Tree.Nodes, mesh.Tree.Metanodes, mesh.Tree.Leaves, ThreadDispatcher, BufferPool); + }, "Revamp Single Axis MT", ref mesh.Tree); BinnedTest(setup, () => { From adcdbf4d7f9dfd76437066b065b81f4e731a70e6 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 16 Nov 2022 19:49:45 -0600 Subject: [PATCH 655/947] Some refactoring, plus oops there's a major structural issue with the worker caches that breaks everything. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 112 +++++++++++++----------- 1 file changed, 59 insertions(+), 53 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 293a12226..bb5c1917d 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -445,27 +445,22 @@ unsafe struct BinnedBuildWorkerContext where TLeafCounts : unmanage public Vector4 OffsetToBinIndex; public Vector4 MaximumBinIndex; + public int SubtreeStartIndex; + public int SubtreeCount; + + public int SlotsPerTaskBase; + public int SlotRemainder; + public bool TaskCountFitsInWorkerCount; + /// - /// Bounds associated with the node that invoked this multithreaded job. + /// Bounds for all subtrees in the builder. Cached in the worker to simplify accesses. /// public Buffer Bounds; /// - /// Leaf counts associated with the node that invoked this multithreaded job. + /// Leaf counts for all subtrees in the builder. Cached in the worker to simplify accesses. /// public TLeafCounts LeafCounts; - public int SlotsPerTaskBase; - public int SlotRemainder; - public bool TaskCountFitsInWorkerCount; - - public static int ComputeRequiredSizeInBytes(int workerCount, int binCount) - { - //Align the leaf counts allocation size so that the bounding box allocations stay aligned despite allocation interleaving. - var binLeafCountsSize = (binCount * Unsafe.SizeOf() + Unsafe.SizeOf() - 1) / Unsafe.SizeOf(); - var binSubtreesSize = binCount * Unsafe.SizeOf() + binLeafCountsSize; - return workerCount * (Unsafe.SizeOf() + Unsafe.SizeOf() + binSubtreesSize) + binCount * Unsafe.SizeOf(); - } - static Buffer Suballocate(Buffer buffer, ref int start, int count) where T : unmanaged { var size = count * Unsafe.SizeOf(); @@ -476,11 +471,11 @@ static Buffer Suballocate(Buffer buffer, ref int start, int count) w static Buffer SuballocateAligned(Buffer buffer, ref int start, int count, int alignmentMask) where T : unmanaged { Debug.Assert(BitOperations.IsPow2(alignmentMask + 1)); - start = (start + alignmentMask) & alignmentMask; + start = (start + alignmentMask) & (~alignmentMask); return Suballocate(buffer, ref start, count); } - public BinnedBuildWorkerContext(Buffer allocation, ref int allocationStart, int workerCount, int binCount) + public BinnedBuildWorkerContext(Buffer allocation, ref int allocationStart, int workerCount, int binCount, Buffer bounds, TLeafCounts leafCounts) { PrepassWorkers = Suballocate(allocation, ref allocationStart, workerCount); BinBoundingBoxesScan = Suballocate(allocation, ref allocationStart, binCount); @@ -490,7 +485,30 @@ public BinnedBuildWorkerContext(Buffer allocation, ref int allocationStart ref var binSubtreesWorker = ref BinSubtreesWorkers[i]; binSubtreesWorker.BinBoundingBoxes = Suballocate(allocation, ref allocationStart, binCount); binSubtreesWorker.BinLeafCounts = SuballocateAligned(allocation, ref allocationStart, binCount, Unsafe.SizeOf() - 1); + //binSubtreesWorker.BinLeafCounts = Suballocate(allocation, ref allocationStart, binCount); } + Bounds = bounds; + LeafCounts = leafCounts; + } + public static int ComputeRequiredSizeInBytes(int workerCount, int binCount) + { + //Align the leaf counts allocation size so that the bounding box allocations stay aligned despite allocation interleaving. + var binLeafCountsSize = ((binCount * Unsafe.SizeOf() + Unsafe.SizeOf() - 1) / Unsafe.SizeOf()) * Unsafe.SizeOf(); + var binSubtreesSize = binCount * Unsafe.SizeOf() + binLeafCountsSize; + return workerCount * (Unsafe.SizeOf() + Unsafe.SizeOf() + binSubtreesSize) + binCount * Unsafe.SizeOf(); + } + + public int SetupTaskCounts(int subtreeStartIndex, int slotCount, int slotsPerTaskTarget, int maximumTaskCountPerSubmission) + { + Debug.Assert(BitOperations.IsPow2(slotsPerTaskTarget), "This mask trick requires power of 2 task size targets."); + var mask = slotsPerTaskTarget - 1; + int taskCount = Math.Min(maximumTaskCountPerSubmission, (slotCount + mask) & mask); + SubtreeStartIndex = subtreeStartIndex; + SubtreeCount = slotCount; + SlotsPerTaskBase = slotCount / taskCount; + SlotRemainder = slotCount - taskCount * SlotsPerTaskBase; + TaskCountFitsInWorkerCount = taskCount <= PrepassWorkers.Length; + return taskCount; } public void GetSlotInterval(long taskId, out int start, out int count) @@ -498,8 +516,8 @@ public void GetSlotInterval(long taskId, out int start, out int count) var remainderedTaskCount = Math.Min(SlotRemainder, taskId); var earlySlotCount = (SlotsPerTaskBase + 1) * remainderedTaskCount; var lateSlotCount = SlotsPerTaskBase * (taskId - remainderedTaskCount); - start = (int)(earlySlotCount + lateSlotCount); - count = taskId > SlotRemainder ? SlotsPerTaskBase : SlotsPerTaskBase + 1; + start = SubtreeStartIndex + (int)(earlySlotCount + lateSlotCount); + count = taskId >= SlotRemainder ? SlotsPerTaskBase : SlotsPerTaskBase + 1; } } unsafe struct MultithreadBinnedBuildContext : IBinnedBuilderThreading where TLeafCounts : unmanaged, ILeafCountBuffer @@ -512,20 +530,6 @@ unsafe struct MultithreadBinnedBuildContext : IBinnedBuilderThreadi public int MaximumTaskCountPerSubmission; public Buffer> Workers; - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public int SetupTaskCounts(int slotCount, int slotsPerTaskTarget, int workerIndex) - { - Debug.Assert(BitOperations.IsPow2(slotsPerTaskTarget), "This mask trick requires power of 2 task size targets."); - var mask = slotsPerTaskTarget - 1; - int taskCount = Math.Min(MaximumTaskCountPerSubmission, (slotCount + mask) & mask); - ref var worker = ref Workers[workerIndex]; - worker.SlotsPerTaskBase = slotCount / taskCount; - worker.SlotRemainder = slotCount - taskCount * worker.SlotsPerTaskBase; - worker.TaskCountFitsInWorkerCount = taskCount <= Workers.Length; - return taskCount; - } - public void GetBins(int workerIndex, out Buffer binBoundingBoxes, out Buffer binBoundingBoxesScan, out Buffer binLeafCounts) { ref var worker = ref Workers[workerIndex]; @@ -576,13 +580,14 @@ unsafe static void CentroidPrepassWorker(long taskId, void* untyped } } - unsafe static BoundingBox4 MultithreadedCentroidPrepass(MultithreadBinnedBuildContext* context, int subtreeCount, int workerIndex) where TLeafCounts : unmanaged, ILeafCountBuffer + unsafe static BoundingBox4 MultithreadedCentroidPrepass(MultithreadBinnedBuildContext* context, int subtreeStartIndex, int subtreeCount, int workerIndex) + where TLeafCounts : unmanaged, ILeafCountBuffer { - var taskCount = context->SetupTaskCounts(subtreeCount, SubtreesPerThreadForCentroidPrepass, workerIndex); + ref var worker = ref context->Workers[workerIndex]; + var taskCount = worker.SetupTaskCounts(subtreeStartIndex, subtreeCount, SubtreesPerThreadForCentroidPrepass, context->MaximumTaskCountPerSubmission); //Don't bother initializing more slots than we have tasks. Note that this requires special handling on the task level; //if we have less tasks than workers, then the task needs to distinguish that fact. var activeWorkerCount = Math.Min(context->Workers.Length, taskCount); - ref var worker = ref context->Workers[workerIndex]; if (taskCount > context->Workers.Length) { //Potentially multiple tasks per worker; we must preinitialize slots. @@ -593,7 +598,7 @@ unsafe static BoundingBox4 MultithreadedCentroidPrepass(Multithread workerBounds.Max = new Vector4(float.MinValue); } } - context->Queue->For(&CentroidPrepassWorker, context, 0, taskCount, workerIndex); + context->Queue->For(&CentroidPrepassWorker, Unsafe.AsPointer(ref worker), 0, taskCount, workerIndex); var centroidBounds = worker.PrepassWorkers[0]; for (int i = 1; i < activeWorkerCount; ++i) @@ -645,13 +650,13 @@ unsafe static void BinSubtreesWorker(long taskId, void* untypedCont unsafe static void MultithreadedBinSubtrees(MultithreadBinnedBuildContext* context, Vector4 centroidBoundsMin, bool useX, bool useY, Vector128 permuteMask, int axisIndex, Vector4 offsetToBinIndex, Vector4 maximumBinIndex, - Buffer bounds, TLeafCounts leafCounts, int binCount, int workerIndex) where TLeafCounts : unmanaged, ILeafCountBuffer + int subtreeStartIndex, int subtreeCount, int binCount, int workerIndex) where TLeafCounts : unmanaged, ILeafCountBuffer { - var taskCount = context->SetupTaskCounts(bounds.Length, SubtreesPerThreadForBinning, workerIndex); + ref var worker = ref context->Workers[workerIndex]; + var taskCount = worker.SetupTaskCounts(subtreeStartIndex, subtreeCount, SubtreesPerThreadForBinning, context->MaximumTaskCountPerSubmission); //Don't bother initializing more slots than we have tasks. Note that this requires special handling on the task level; //if we have less tasks than workers, then the task needs to distinguish that fact. var activeWorkerCount = Math.Min(context->Workers.Length, taskCount); - ref var worker = ref context->Workers[workerIndex]; if (!worker.TaskCountFitsInWorkerCount) { //If there are more tasks than workers, then we need to preinitialize all the worker caches. @@ -676,7 +681,7 @@ unsafe static void MultithreadedBinSubtrees(MultithreadBinnedBuildC worker.OffsetToBinIndex = offsetToBinIndex; worker.MaximumBinIndex = maximumBinIndex; - context->Queue->For(&BinSubtreesWorker, context, 0, taskCount, workerIndex); + context->Queue->For(&BinSubtreesWorker, Unsafe.AsPointer(ref worker), 0, taskCount, workerIndex); //Unless the number of threads and bins is really huge, there's no value in attempting to multithread the final compression. //(Parallel reduction is an option, but even then... I suspect the single threaded version will be faster. And it's way simpler.) @@ -709,9 +714,9 @@ unsafe static void BinnedBuilderNodeWorker(lon { var subtreeRegionStartIndex = (int)taskId; var subtreeCount = (int)(taskId >> 32); - var wrappedContext = (NodePushTaskContext*)context; + var nodePushContext = (NodePushTaskContext*)context; //Note that child index is always 1 because we only ever push child B. - BinnedBuildNode(subtreeRegionStartIndex, wrappedContext->NodeIndex, subtreeCount, wrappedContext->ParentNodeIndex, 1, wrappedContext->Context, workerIndex); + BinnedBuildNode(subtreeRegionStartIndex, nodePushContext->NodeIndex, subtreeCount, nodePushContext->ParentNodeIndex, 1, nodePushContext->Context, workerIndex); } static unsafe void BinnedBuildNode( @@ -737,7 +742,7 @@ static unsafe void BinnedBuildNode( else { centroidBounds = MultithreadedCentroidPrepass( - (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As>(ref context->Threading)), subtreeCount, workerIndex); + (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As>(ref context->Threading)), subtreeRegionStartIndex, subtreeCount, workerIndex); } var centroidSpan = centroidBounds.Max - centroidBounds.Min; var axisIsDegenerate = Vector128.LessThanOrEqual(centroidSpan.AsVector128(), Vector128.Create(1e-12f)); @@ -814,7 +819,7 @@ static unsafe void BinnedBuildNode( { MultithreadedBinSubtrees( (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As>(ref context->Threading)), - centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, boundingBoxes, leafCounts, binCount, workerIndex); + centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, subtreeRegionStartIndex, subtreeCount, binCount, workerIndex); } //Identify the split index by examining the SAH of very split option. @@ -924,12 +929,12 @@ static unsafe void BinnedBuildNode( Debug.Assert(SubtreesPerThreadForNodeJob > 1, "The job threshold for a new node should be large enough that there's no need for a subtreeCountB > 1 test."); ref var threading = ref Unsafe.As>(ref context->Threading); //Allocate the parameters to send to the worker on the local stack. Note that we have to preserve the stack for this to work; see the later WaitForCompletion. - NodePushTaskContext wrappedContext; - wrappedContext.Context = context; - wrappedContext.NodeIndex = nodeChildIndexB; - wrappedContext.ParentNodeIndex = nodeIndex; + NodePushTaskContext nodePushContext; + nodePushContext.Context = context; + nodePushContext.NodeIndex = nodeChildIndexB; + nodePushContext.ParentNodeIndex = nodeIndex; //Note that we use the task id to store subtree start and subtree count. Don't have to do that, but no reason not to use it. - var task = new Task(&BinnedBuilderNodeWorker, &wrappedContext, (long)(subtreeRegionStartIndex + subtreeCountA) | ((long)subtreeCountB << 32)); + var task = new Task(&BinnedBuilderNodeWorker, &nodePushContext, (long)(subtreeRegionStartIndex + subtreeCountA) | ((long)subtreeCountB << 32)); nodeBContinuation = threading.Queue->AllocateContinuationAndEnqueue(new Span(&task, 1), workerIndex); } if (subtreeCountA > 1) @@ -1005,15 +1010,16 @@ static unsafe void BinnedBuilderInternal(Buffer encodedLeafIndices, Buffer< //So, each worker working on a node job needs to have its own set of worker caches for other workers to help it with the node job, giving an allocation count //proportional to workerCount * workerCount * binCount. Not a big problem for small bin counts on a quad core system, but how about a 64 core system with 128 threads and 64 bins? //128 * 128 * 64 > 1e6, and that's before taking into account the size of the bins being allocated! - pool.Take(BinnedBuildWorkerContext.ComputeRequiredSizeInBytes(workerCount, allocatedBinCount), out var workerAllocation); + pool.Take(BinnedBuildWorkerContext.ComputeRequiredSizeInBytes(workerCount, allocatedBinCount) * workerCount, out var workerAllocation); BinnedBuildWorkerContext* workerContextsPointer = stackalloc BinnedBuildWorkerContext[workerCount]; + var leafCounts = new UnitLeafCount(); var workerContexts = new Buffer>(workerContextsPointer, workerCount); int allocatedByteCount = 0; for (int i = 0; i < workerCount; ++i) { - workerContexts[i] = new BinnedBuildWorkerContext(workerAllocation, ref allocatedByteCount, workerCount, allocatedBinCount); + workerContexts[i] = new BinnedBuildWorkerContext(workerAllocation, ref allocatedByteCount, workerCount, allocatedBinCount, boundingBoxes.As(), leafCounts); } TaskQueue taskQueue = default; @@ -1027,11 +1033,11 @@ static unsafe void BinnedBuilderInternal(Buffer encodedLeafIndices, Buffer< { MaximumTaskCountPerSubmission = workerCount * 2, Queue = taskQueuePointer, - Workers = workerContexts + Workers = workerContexts, }; var context = new Context, MultithreadBinnedBuildContext>( minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold, encodedLeafIndices, - new UnitLeafCount(), leaves, boundingBoxes.As(), nodes, metanodes, threading); + leafCounts, leaves, boundingBoxes.As(), nodes, metanodes, threading); taskQueuePointer->TryEnqueueUnsafely(new Task(&BinnedBuilderWorkerEntry>, &context)); dispatcher.DispatchWorkers(&BinnedBuilderWorkerFunction>, taskQueuePointer); From 17509009383492609f7897eaeef9bfa9d3f3b80c Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 24 Nov 2022 16:31:41 -0600 Subject: [PATCH 656/947] Refactored for arena pools. Clears are not yet applied where they should be. --- BepuPhysics/BatchCompressor.cs | 10 +-- BepuPhysics/CollidableProperty.cs | 6 +- BepuPhysics/Collidables/BigCompound.cs | 22 +++--- BepuPhysics/Collidables/BoundingBoxBatcher.cs | 8 +- BepuPhysics/Collidables/Box.cs | 2 +- BepuPhysics/Collidables/Capsule.cs | 2 +- BepuPhysics/Collidables/Compound.cs | 12 +-- BepuPhysics/Collidables/ConvexHull.cs | 6 +- BepuPhysics/Collidables/ConvexHullHelper.cs | 22 +++--- BepuPhysics/Collidables/Cylinder.cs | 2 +- BepuPhysics/Collidables/IShape.cs | 6 +- BepuPhysics/Collidables/Mesh.cs | 16 ++-- BepuPhysics/Collidables/Shapes.cs | 42 +++++------ BepuPhysics/Collidables/Sphere.cs | 2 +- BepuPhysics/Collidables/Triangle.cs | 2 +- BepuPhysics/CollisionDetection/BroadPhase.cs | 4 +- .../CollisionDetection/CollisionBatcher.cs | 4 +- .../CollisionBatcherContinuations.cs | 6 +- .../CompoundPairCollisionTask.cs | 2 +- .../CompoundPairOverlapFinder.cs | 2 +- .../CollisionTasks/CompoundPairOverlaps.cs | 10 +-- .../ConvexCompoundOverlapFinder.cs | 8 +- .../ConvexCompoundTaskOverlaps.cs | 8 +- .../CollisionTasks/MeshPairOverlapFinder.cs | 2 +- .../CompoundMeshReduction.cs | 2 +- .../CollisionDetection/ConstraintRemover.cs | 12 +-- .../CollisionDetection/FreshnessChecker.cs | 2 +- .../CollisionDetection/MeshReduction.cs | 6 +- BepuPhysics/CollisionDetection/NarrowPhase.cs | 8 +- .../NarrowPhaseCCDContinuations.cs | 12 +-- .../NarrowPhasePendingConstraintAdds.cs | 4 +- .../CollisionDetection/NonconvexReduction.cs | 4 +- BepuPhysics/CollisionDetection/PairCache.cs | 10 +-- BepuPhysics/CollisionDetection/RayBatchers.cs | 5 +- .../CollisionDetection/SweepTaskRegistry.cs | 4 +- .../CompoundHomogeneousCompoundSweepTask.cs | 2 +- .../CompoundPairSweepOverlapFinder.cs | 4 +- .../SweepTasks/CompoundPairSweepOverlaps.cs | 4 +- .../SweepTasks/CompoundPairSweepTask.cs | 2 +- .../ConvexCompoundSweepOverlapFinder.cs | 4 +- .../SweepTasks/ConvexCompoundSweepTask.cs | 2 +- .../ConvexHomogeneousCompoundSweepTask.cs | 2 +- .../SweepTasks/ConvexSweepTaskCommon.cs | 2 +- BepuPhysics/CollisionDetection/UntypedList.cs | 10 +-- .../CollisionDetection/WorkerPairCache.cs | 6 +- BepuPhysics/IslandScaffold.cs | 16 ++-- BepuPhysics/IslandSleeper.cs | 18 ++--- BepuPhysics/PoseIntegrator.cs | 2 +- BepuPhysics/SequentialFallbackBatch.cs | 12 +-- BepuPhysics/Trees/RayBatcher.cs | 18 ++--- BepuPhysics/Trees/Tree.cs | 8 +- BepuPhysics/Trees/Tree_Add.cs | 2 +- BepuPhysics/Trees/Tree_BinnedRefine.cs | 4 +- .../Trees/Tree_MultithreadedRefitRefine.cs | 16 ++-- .../Trees/Tree_RefinementScheduling.cs | 6 +- BepuPhysics/Trees/Tree_SweepBuilder.cs | 16 ++-- BepuUtilities/Collections/IndexSet.cs | 18 ++--- BepuUtilities/IThreadDispatcher.cs | 14 ++-- BepuUtilities/Memory/ArenaPool.cs | 74 ++++++++++++++++--- BepuUtilities/Memory/BufferPool.cs | 44 ++++------- BepuUtilities/Memory/IUnmanagedMemoryPool.cs | 34 ++++++++- BepuUtilities/Memory/WorkerBufferPools.cs | 10 ++- BepuUtilities/ThreadDispatcher.cs | 23 ++---- DemoRenderer/ShapeDrawing/ShapesExtractor.cs | 21 +++--- Demos/Demo.cs | 4 +- Demos/DemoSet.cs | 4 +- Demos/Demos/CustomVoxelCollidableDemo.cs | 10 +-- Demos/Demos/RayCastingDemo.cs | 2 +- Demos/Demos/SimpleSelfContainedDemo.cs | 2 +- .../IntertreeThreadingTests.cs | 2 +- .../PyramidAwakenerTestDemo.cs | 2 +- Demos/SpecializedTests/TreeTest.cs | 2 +- 72 files changed, 381 insertions(+), 316 deletions(-) diff --git a/BepuPhysics/BatchCompressor.cs b/BepuPhysics/BatchCompressor.cs index db1e09a2c..54d758168 100644 --- a/BepuPhysics/BatchCompressor.cs +++ b/BepuPhysics/BatchCompressor.cs @@ -114,7 +114,7 @@ unsafe void AnalysisWorker(int workerIndex, void* context) int jobIndex; while ((jobIndex = Interlocked.Increment(ref analysisJobIndex)) < analysisJobs.Count) { - DoJob(ref analysisJobs[jobIndex], workerIndex, threadDispatcher.GetThreadMemoryPool(workerIndex)); + DoJob(ref analysisJobs[jobIndex], workerIndex, threadDispatcher.WorkerPools[workerIndex]); } } @@ -128,7 +128,7 @@ unsafe void AnalysisWorker(int workerIndex, void* context) [MethodImpl(MethodImplOptions.AggressiveInlining)] private unsafe void TryToFindBetterBatchForConstraint( - BufferPool pool, ref QuickList compressions, ref TypeBatch typeBatch, int* bodyHandles, ref ActiveConstraintDynamicBodyHandleCollector handleAccumulator, TypeProcessor typeProcessor, int constraintIndex) + IUnmanagedMemoryPool pool, ref QuickList compressions, ref TypeBatch typeBatch, int* bodyHandles, ref ActiveConstraintDynamicBodyHandleCollector handleAccumulator, TypeProcessor typeProcessor, int constraintIndex) { handleAccumulator.Count = 0; Solver.EnumerateConnectedRawBodyReferences(ref typeBatch, constraintIndex, ref handleAccumulator); @@ -145,7 +145,7 @@ private unsafe void TryToFindBetterBatchForConstraint( } - unsafe void DoJob(ref AnalysisRegion region, int workerIndex, BufferPool pool) + unsafe void DoJob(ref AnalysisRegion region, int workerIndex, IUnmanagedMemoryPool pool) { ref var compressions = ref this.workerCompressions[workerIndex]; ref var batch = ref Solver.ActiveSet.Batches[nextBatchIndex]; @@ -246,7 +246,7 @@ public void Compress(BufferPool pool, IThreadDispatcher threadDispatcher = null, for (int i = 0; i < workerCount; ++i) { //Be careful: the jobs may require resizes on the compression count list. That requires the use of per-worker pools. - workerCompressions[i] = new QuickList(Math.Max(8, maximumCompressionCount), threadDispatcher == null ? pool : threadDispatcher.GetThreadMemoryPool(i)); + workerCompressions[i] = new QuickList(Math.Max(8, maximumCompressionCount), threadDispatcher == null ? pool : threadDispatcher.WorkerPools[i]); } //In any given compression attempt, we only optimize over one ConstraintBatch. @@ -404,7 +404,7 @@ public void Compress(BufferPool pool, IThreadDispatcher threadDispatcher = null, for (int i = 0; i < workerCount; ++i) { //Be careful: the jobs may require resizes on the compression count list. That requires the use of per-worker pools. - workerCompressions[i].Dispose((threadDispatcher == null ? pool : threadDispatcher.GetThreadMemoryPool(i))); + workerCompressions[i].Dispose((threadDispatcher == null ? pool : threadDispatcher.WorkerPools[i])); } pool.Return(ref workerCompressions); } diff --git a/BepuPhysics/CollidableProperty.cs b/BepuPhysics/CollidableProperty.cs index bdffeef0a..7b4867c4a 100644 --- a/BepuPhysics/CollidableProperty.cs +++ b/BepuPhysics/CollidableProperty.cs @@ -19,7 +19,7 @@ public class CollidableProperty : IDisposable where T : unmanaged //Bodies and statics each have 'handle spaces', like namespaces. A body and static can have the same integer valued handle. //So, we need to have two different buffers for data. Simulation simulation; - BufferPool pool; + IUnmanagedMemoryPool pool; Buffer bodyData; Buffer staticData; @@ -29,7 +29,7 @@ public class CollidableProperty : IDisposable where T : unmanaged /// Constructs a new collection to store handle-aligned body properties. Assumes the Initialize function will be called later to provide the Bodies collection. /// /// Pool from which to pull internal resources. If null, uses the later Initialize-provided Bodies pool. - public CollidableProperty(BufferPool pool = null) + public CollidableProperty(IUnmanagedMemoryPool pool = null) { this.pool = pool; } @@ -39,7 +39,7 @@ public CollidableProperty(BufferPool pool = null) /// /// Simulation to track. /// Pool from which to pull internal resources. If null, uses the Simulation pool. - public CollidableProperty(Simulation simulation, BufferPool pool = null) + public CollidableProperty(Simulation simulation, IUnmanagedMemoryPool pool = null) { this.simulation = simulation; this.pool = pool == null ? simulation.BufferPool : pool; diff --git a/BepuPhysics/Collidables/BigCompound.cs b/BepuPhysics/Collidables/BigCompound.cs index c0050b852..9cfc3018b 100644 --- a/BepuPhysics/Collidables/BigCompound.cs +++ b/BepuPhysics/Collidables/BigCompound.cs @@ -33,7 +33,7 @@ public struct BigCompound : ICompoundShape /// Shapes set in which child shapes are allocated. /// Pool to use to allocate acceleration structures. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public BigCompound(Buffer children, Shapes shapes, BufferPool pool) + public BigCompound(Buffer children, Shapes shapes, IUnmanagedMemoryPool pool) { Debug.Assert(children.Length > 0, "Compounds must have a nonzero number of children."); Debug.Assert(Compound.ValidateChildIndices(ref children, shapes), "Children must all be convex."); @@ -139,7 +139,7 @@ public unsafe void RayTest(in RigidPose pose, ref RaySource rays } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapes) + public ShapeBatch CreateShapeBatch(IUnmanagedMemoryPool pool, int initialCapacity, Shapes shapes) { return new CompoundShapeBatch(pool, initialCapacity, shapes); } @@ -163,9 +163,9 @@ public ref CompoundChild GetChild(int compoundChildIndex) /// Pool to use to resize the compound's children buffer if necessary. /// Shapes collection containing the compound's children. /// This function keeps the in a valid state, but significant changes over time may degrade the tree's quality and result in reduced collision/query performance. - /// If this happens, consider calling with a refinementIndex that changes with each call (to prioritize different parts of the tree). + /// If this happens, consider calling with a refinementIndex that changes with each call (to prioritize different parts of the tree). /// Incrementing a counter with each call would work fine. The ideal frequency of refinement depends on the kind of modifications being made, but it's likely to be rare. - public void Add(CompoundChild child, BufferPool pool, Shapes shapes) + public void Add(CompoundChild child, IUnmanagedMemoryPool pool, Shapes shapes) { pool.Resize(ref Children, Children.Length + 1, Children.Length); Children[^1] = child; @@ -180,9 +180,9 @@ public void Add(CompoundChild child, BufferPool pool, Shapes shapes) /// Index of the child to remove from the compound. /// Pool to use to resize the compound's children buffer if necessary. /// This function keeps the in a valid state, but significant changes over time may degrade the tree's quality and result in reduced collision/query performance. - /// If this happens, consider calling with a refinementIndex that changes with each call (to prioritize different parts of the tree). + /// If this happens, consider calling with a refinementIndex that changes with each call (to prioritize different parts of the tree). /// Incrementing a counter with each call would work fine. The ideal frequency of refinement depends on the kind of modifications being made, but it's likely to be rare. - public void RemoveAt(int childIndex, BufferPool pool) + public void RemoveAt(int childIndex, IUnmanagedMemoryPool pool) { var movedChildIndex = Tree.RemoveAt(childIndex); if (movedChildIndex >= 0) @@ -196,7 +196,7 @@ public void RemoveAt(int childIndex, BufferPool pool) unsafe struct Enumerator : IBreakableForEach where TSubpairOverlaps : ICollisionTaskSubpairOverlaps { - public BufferPool Pool; + public IUnmanagedMemoryPool Pool; public void* Overlaps; [MethodImpl(MethodImplOptions.AggressiveInlining)] public bool LoopBody(int i) @@ -206,7 +206,7 @@ public bool LoopBody(int i) } } - public unsafe void FindLocalOverlaps(ref Buffer pairs, BufferPool pool, Shapes shapes, ref TOverlaps overlaps) + public unsafe void FindLocalOverlaps(ref Buffer pairs, IUnmanagedMemoryPool pool, Shapes shapes, ref TOverlaps overlaps) where TOverlaps : struct, ICollisionTaskOverlaps where TSubpairOverlaps : struct, ICollisionTaskSubpairOverlaps { @@ -225,7 +225,7 @@ public unsafe void FindLocalOverlaps(ref Buffer : ISweepLeafTester where TOverlaps : ICollisionTaskSubpairOverlaps { - public BufferPool Pool; + public IUnmanagedMemoryPool Pool; public void* Overlaps; [MethodImpl(MethodImplOptions.AggressiveInlining)] public void TestLeaf(int leafIndex, ref float maximumT) @@ -233,7 +233,7 @@ public void TestLeaf(int leafIndex, ref float maximumT) Unsafe.AsRef(Overlaps).Allocate(Pool) = leafIndex; } } - public unsafe void FindLocalOverlaps(Vector3 min, Vector3 max, Vector3 sweep, float maximumT, BufferPool pool, Shapes shapes, void* overlaps) + public unsafe void FindLocalOverlaps(Vector3 min, Vector3 max, Vector3 sweep, float maximumT, IUnmanagedMemoryPool pool, Shapes shapes, void* overlaps) where TOverlaps : ICollisionTaskSubpairOverlaps { SweepLeafTester enumerator; @@ -268,7 +268,7 @@ public BodyInertia ComputeInertia(Span childMasses, Shapes shapes, out Ve return bodyInertia; } - public void Dispose(BufferPool bufferPool) + public void Dispose(IUnmanagedMemoryPool bufferPool) { bufferPool.Return(ref Children); Tree.Dispose(bufferPool); diff --git a/BepuPhysics/Collidables/BoundingBoxBatcher.cs b/BepuPhysics/Collidables/BoundingBoxBatcher.cs index f4998e6e5..4ee8270c8 100644 --- a/BepuPhysics/Collidables/BoundingBoxBatcher.cs +++ b/BepuPhysics/Collidables/BoundingBoxBatcher.cs @@ -82,7 +82,7 @@ public struct BoundingBoxBatch public int Count; public bool Allocated => ShapeIndices.Allocated; - public BoundingBoxBatch(BufferPool pool, int initialCapacity) + public BoundingBoxBatch(IUnmanagedMemoryPool pool, int initialCapacity) { pool.Take(initialCapacity, out ShapeIndices); pool.Take(initialCapacity, out Continuations); @@ -99,7 +99,7 @@ internal void Add(int shapeIndex, RigidPose pose, BodyVelocity velocity, BoundsC Count++; } - public void Dispose(BufferPool pool) + public void Dispose(IUnmanagedMemoryPool pool) { if (Allocated) { @@ -114,7 +114,7 @@ public void Dispose(BufferPool pool) public struct BoundingBoxBatcher { - internal BufferPool pool; + internal IUnmanagedMemoryPool pool; internal Shapes shapes; internal Bodies bodies; internal BroadPhase broadPhase; @@ -129,7 +129,7 @@ public struct BoundingBoxBatcher /// public const int CollidablesPerFlush = 16; - public unsafe BoundingBoxBatcher(Bodies bodies, Shapes shapes, BroadPhase broadPhase, BufferPool pool, float dt) + public unsafe BoundingBoxBatcher(Bodies bodies, Shapes shapes, BroadPhase broadPhase, IUnmanagedMemoryPool pool, float dt) { this.bodies = bodies; this.shapes = shapes; diff --git a/BepuPhysics/Collidables/Box.cs b/BepuPhysics/Collidables/Box.cs index a7f989b90..320c4e0bd 100644 --- a/BepuPhysics/Collidables/Box.cs +++ b/BepuPhysics/Collidables/Box.cs @@ -162,7 +162,7 @@ public readonly BodyInertia ComputeInertia(float mass) return inertia; } - public readonly ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches) + public readonly ShapeBatch CreateShapeBatch(IUnmanagedMemoryPool pool, int initialCapacity, Shapes shapeBatches) { return new ConvexShapeBatch(pool, initialCapacity); } diff --git a/BepuPhysics/Collidables/Capsule.cs b/BepuPhysics/Collidables/Capsule.cs index 38865fdd1..abf95e507 100644 --- a/BepuPhysics/Collidables/Capsule.cs +++ b/BepuPhysics/Collidables/Capsule.cs @@ -179,7 +179,7 @@ public readonly BodyInertia ComputeInertia(float mass) return inertia; } - public readonly ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches) + public readonly ShapeBatch CreateShapeBatch(IUnmanagedMemoryPool pool, int initialCapacity, Shapes shapeBatches) { return new ConvexShapeBatch(pool, initialCapacity); } diff --git a/BepuPhysics/Collidables/Compound.cs b/BepuPhysics/Collidables/Compound.cs index 58c1ccdb2..4129544a5 100644 --- a/BepuPhysics/Collidables/Compound.cs +++ b/BepuPhysics/Collidables/Compound.cs @@ -250,7 +250,7 @@ public unsafe void RayTest(in RigidPose pose, ref RaySource rays } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapes) + public ShapeBatch CreateShapeBatch(IUnmanagedMemoryPool pool, int initialCapacity, Shapes shapes) { return new CompoundShapeBatch(pool, initialCapacity, shapes); } @@ -272,7 +272,7 @@ public ref CompoundChild GetChild(int compoundChildIndex) /// /// Child to add to the compound. /// Pool to use to resize the compound's children buffer if necessary. - public void Add(CompoundChild child, BufferPool pool) + public void Add(CompoundChild child, IUnmanagedMemoryPool pool) { pool.Resize(ref Children, Children.Length + 1, Children.Length); Children[^1] = child; @@ -283,7 +283,7 @@ public void Add(CompoundChild child, BufferPool pool) /// /// Index of the child to remove from the compound. /// Pool to use to resize the compound's children buffer if necessary. - public void RemoveAt(int childIndex, BufferPool pool) + public void RemoveAt(int childIndex, IUnmanagedMemoryPool pool) { var lastIndex = Children.Length - 1; if (childIndex < lastIndex) @@ -295,7 +295,7 @@ public void RemoveAt(int childIndex, BufferPool pool) } - public unsafe void FindLocalOverlaps(ref Buffer pairs, BufferPool pool, Shapes shapes, ref TOverlaps overlaps) + public unsafe void FindLocalOverlaps(ref Buffer pairs, IUnmanagedMemoryPool pool, Shapes shapes, ref TOverlaps overlaps) where TOverlaps : struct, ICollisionTaskOverlaps where TSubpairOverlaps : struct, ICollisionTaskSubpairOverlaps { @@ -320,7 +320,7 @@ public unsafe void FindLocalOverlaps(ref Buffer(Vector3 min, Vector3 max, Vector3 sweep, float maximumT, BufferPool pool, Shapes shapes, void* overlapsPointer) + public unsafe void FindLocalOverlaps(Vector3 min, Vector3 max, Vector3 sweep, float maximumT, IUnmanagedMemoryPool pool, Shapes shapes, void* overlapsPointer) where TOverlaps : ICollisionTaskSubpairOverlaps { Tree.ConvertBoxToCentroidWithExtent(min, max, out var sweepOrigin, out var expansion); @@ -362,7 +362,7 @@ public BodyInertia ComputeInertia(Span childMasses, Shapes shapes, out Ve return CompoundBuilder.ComputeInertia(Children, childMasses, shapes, out centerOfMass); } - public void Dispose(BufferPool bufferPool) + public void Dispose(IUnmanagedMemoryPool bufferPool) { bufferPool.Return(ref Children); } diff --git a/BepuPhysics/Collidables/ConvexHull.cs b/BepuPhysics/Collidables/ConvexHull.cs index 0e779a127..8c2084bcc 100644 --- a/BepuPhysics/Collidables/ConvexHull.cs +++ b/BepuPhysics/Collidables/ConvexHull.cs @@ -58,7 +58,7 @@ public struct ConvexHull : IConvexShape /// Points to compute the convex hull of. /// Pool in which to allocate the convex hull and any temporary resources needed to compute the hull. /// Computed center of the convex hull before the hull was recentered. - public ConvexHull(Span points, BufferPool pool, out Vector3 center) + public ConvexHull(Span points, IUnmanagedMemoryPool pool, out Vector3 center) { ConvexHullHelper.CreateShape(points, pool, out center, out this); } @@ -193,7 +193,7 @@ public readonly BodyInertia ComputeInertia(float mass) return inertia; } - public readonly ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches) + public readonly ShapeBatch CreateShapeBatch(IUnmanagedMemoryPool pool, int initialCapacity, Shapes shapeBatches) { return new ConvexHullShapeBatch(pool, initialCapacity); } @@ -269,7 +269,7 @@ public readonly bool RayTest(in RigidPose pose, Vector3 origin, Vector3 directio return true; } } - public void Dispose(BufferPool bufferPool) + public void Dispose(IUnmanagedMemoryPool bufferPool) { bufferPool.Return(ref Points); bufferPool.Return(ref BoundingPlanes); diff --git a/BepuPhysics/Collidables/ConvexHullHelper.cs b/BepuPhysics/Collidables/ConvexHullHelper.cs index 590f21f92..fa2c89e77 100644 --- a/BepuPhysics/Collidables/ConvexHullHelper.cs +++ b/BepuPhysics/Collidables/ConvexHullHelper.cs @@ -72,7 +72,7 @@ public void GetFace(int faceIndex, out HullFace face) face.OriginalVertexMapping = OriginalVertexMapping; } - public void Dispose(BufferPool pool) + public void Dispose(IUnmanagedMemoryPool pool) { pool.Return(ref OriginalVertexMapping); pool.Return(ref FaceVertexIndices); @@ -489,14 +489,14 @@ struct EdgeToTest } - static void AddFace(ref QuickList faces, BufferPool pool, Vector3 normal, QuickList vertexIndices) + static void AddFace(ref QuickList faces, IUnmanagedMemoryPool pool, Vector3 normal, QuickList vertexIndices) { ref var face = ref faces.Allocate(pool); face = new EarlyFace { Normal = normal, VertexIndices = new QuickList(vertexIndices.Count, pool) }; face.VertexIndices.AddRangeUnsafely(vertexIndices); } - static void AddFaceToEdgesAndTestList(BufferPool pool, + static void AddFaceToEdgesAndTestList(IUnmanagedMemoryPool pool, ref QuickList reducedFaceIndices, ref QuickList edgesToTest, ref QuickDictionary facesForEdges, @@ -534,7 +534,7 @@ static void AddFaceToEdgesAndTestList(BufferPool pool, } } - static void AddIfNotPresent(ref QuickList list, int value, BufferPool pool) + static void AddIfNotPresent(ref QuickList list, int value, IUnmanagedMemoryPool pool) { if (!list.Contains(value)) list.Allocate(pool) = value; @@ -600,7 +600,7 @@ static void AddIfNotPresent(ref QuickList list, int value, BufferPool pool) ///// Point set to compute the convex hull of. ///// Buffer pool to pull memory from when creating the hull. ///// Convex hull of the input point set. - //public static void ComputeHull(Span points, BufferPool pool, out HullData hullData) + //public static void ComputeHull(Span points, IUnmanagedMemoryPool pool, out HullData hullData) //{ // ComputeHull(points, pool, out hullData, out _); //} @@ -612,7 +612,7 @@ static void AddIfNotPresent(ref QuickList list, int value, BufferPool pool) /// Point set to compute the convex hull of. /// Buffer pool to pull memory from when creating the hull. /// Convex hull of the input point set. - public static void ComputeHull(Span points, BufferPool pool, out HullData hullData)//, out List steps) + public static void ComputeHull(Span points, IUnmanagedMemoryPool pool, out HullData hullData)//, out List steps) { //steps = new List(); if (points.Length <= 0) @@ -1031,7 +1031,7 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa /// Pool used to allocate resources for the hullShape. /// Convex hull shape created from the input data. /// Computed center of mass of the convex hull before its points were recentered onto the origin. - public static void CreateShape(Span points, HullData hullData, BufferPool pool, out Vector3 center, out ConvexHull hullShape) + public static void CreateShape(Span points, HullData hullData, IUnmanagedMemoryPool pool, out Vector3 center, out ConvexHull hullShape) { hullShape = default; if (hullData.OriginalVertexMapping.Length < 3) @@ -1146,7 +1146,7 @@ public static void CreateShape(Span points, HullData hullData, BufferPo /// Intermediate hull data that got processed into the convex hull. /// Computed center of mass of the convex hull before its points were recentered onto the origin. /// Convex hull shape of the input point set. - public static void CreateShape(Span points, BufferPool pool, out HullData hullData, out Vector3 center, out ConvexHull convexHull) + public static void CreateShape(Span points, IUnmanagedMemoryPool pool, out HullData hullData, out Vector3 center, out ConvexHull convexHull) { ComputeHull(points, pool, out hullData); CreateShape(points, hullData, pool, out center, out convexHull); @@ -1159,7 +1159,7 @@ public static void CreateShape(Span points, BufferPool pool, out HullDa /// Buffer pool used for temporary allocations and the output data structures. /// Computed center of mass of the convex hull before its points were recentered onto the origin. /// Convex hull shape of the input point set. - public static void CreateShape(Span points, BufferPool pool, out Vector3 center, out ConvexHull convexHull) + public static void CreateShape(Span points, IUnmanagedMemoryPool pool, out Vector3 center, out ConvexHull convexHull) { ComputeHull(points, pool, out var hullData); CreateShape(points, hullData, pool, out center, out convexHull); @@ -1224,7 +1224,7 @@ public static void CreateTransformedCopy(in ConvexHull source, in Matrix3x3 tran /// Transform to apply to the hull points. /// Pool from which to allocate the new hull's points and bounding planes buffers. /// Target convex hull to copy into. FaceVertexIndices and FaceToVertexIndicesStart buffers are reused from the source. - public static void CreateTransformedShallowCopy(in ConvexHull source, in Matrix3x3 transform, BufferPool pool, out ConvexHull target) + public static void CreateTransformedShallowCopy(in ConvexHull source, in Matrix3x3 transform, IUnmanagedMemoryPool pool, out ConvexHull target) { pool.Take(source.Points.Length, out target.Points); pool.Take(source.BoundingPlanes.Length, out target.BoundingPlanes); @@ -1240,7 +1240,7 @@ public static void CreateTransformedShallowCopy(in ConvexHull source, in Matrix3 /// Transform to apply to the hull points. /// Pool from which to allocate the new hull's buffers. /// Target convex hull to copy into. - public static void CreateTransformedCopy(in ConvexHull source, in Matrix3x3 transform, BufferPool pool, out ConvexHull target) + public static void CreateTransformedCopy(in ConvexHull source, in Matrix3x3 transform, IUnmanagedMemoryPool pool, out ConvexHull target) { pool.Take(source.Points.Length, out target.Points); pool.Take(source.BoundingPlanes.Length, out target.BoundingPlanes); diff --git a/BepuPhysics/Collidables/Cylinder.cs b/BepuPhysics/Collidables/Cylinder.cs index 6dee60c4c..287d9970c 100644 --- a/BepuPhysics/Collidables/Cylinder.cs +++ b/BepuPhysics/Collidables/Cylinder.cs @@ -176,7 +176,7 @@ public readonly BodyInertia ComputeInertia(float mass) return inertia; } - public readonly ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches) + public readonly ShapeBatch CreateShapeBatch(IUnmanagedMemoryPool pool, int initialCapacity, Shapes shapeBatches) { return new ConvexShapeBatch(pool, initialCapacity); } diff --git a/BepuPhysics/Collidables/IShape.cs b/BepuPhysics/Collidables/IShape.cs index b86b28c24..64d4b4c56 100644 --- a/BepuPhysics/Collidables/IShape.cs +++ b/BepuPhysics/Collidables/IShape.cs @@ -28,7 +28,7 @@ public interface IShape /// Shape batch for the shape type. /// This is typically used internally to initialize new shape collections in response to shapes being added. It is not likely to be useful outside of the engine. /// Ideally, this would be implemented as a static abstract, but those aren't available yet. - ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches); + ShapeBatch CreateShapeBatch(IUnmanagedMemoryPool pool, int initialCapacity, Shapes shapeBatches); } //Note that the following bounds functions require only an orientation because the effect of the position on the bounding box is the same for all shapes. @@ -144,7 +144,7 @@ public interface ICompoundShape : IShape, IBoundsQueryableCompound /// Returns all resources used by the shape instance to the given pool. /// /// Pool to return shape resources to. - void Dispose(BufferPool pool); + void Dispose(IUnmanagedMemoryPool pool); } /// @@ -208,7 +208,7 @@ public interface IHomogeneousCompoundShape : IShap /// Returns all resources used by the shape instance to the given pool. /// /// Pool to return shape resources to. - void Dispose(BufferPool pool); + void Dispose(IUnmanagedMemoryPool pool); } /// diff --git a/BepuPhysics/Collidables/Mesh.cs b/BepuPhysics/Collidables/Mesh.cs index 20deb5f93..61bb76677 100644 --- a/BepuPhysics/Collidables/Mesh.cs +++ b/BepuPhysics/Collidables/Mesh.cs @@ -11,7 +11,7 @@ namespace BepuPhysics.Collidables { public unsafe struct ShapeTreeOverlapEnumerator : IBreakableForEach where TSubpairOverlaps : ICollisionTaskSubpairOverlaps { - public BufferPool Pool; + public IUnmanagedMemoryPool Pool; public void* Overlaps; [MethodImpl(MethodImplOptions.AggressiveInlining)] public bool LoopBody(int i) @@ -22,7 +22,7 @@ public bool LoopBody(int i) } public unsafe struct ShapeTreeSweepLeafTester : ISweepLeafTester where TOverlaps : ICollisionTaskSubpairOverlaps { - public BufferPool Pool; + public IUnmanagedMemoryPool Pool; public void* Overlaps; [MethodImpl(MethodImplOptions.AggressiveInlining)] public void TestLeaf(int leafIndex, ref float maximumT) @@ -72,7 +72,7 @@ public Vector3 Scale /// Scale to apply to all vertices at runtime. /// Note that the scale is not baked into the triangles or acceleration structure; the same set of triangles and acceleration structure can be used across multiple Mesh instances with different scales. /// Pool used to allocate acceleration structures. - public Mesh(Buffer triangles, Vector3 scale, BufferPool pool) : this() + public Mesh(Buffer triangles, Vector3 scale, IUnmanagedMemoryPool pool) : this() { Triangles = triangles; Tree = new Tree(pool, triangles.Length); @@ -94,7 +94,7 @@ public Mesh(Buffer triangles, Vector3 scale, BufferPool pool) : this() /// /// Data to load the mesh from. /// Pool to create the mesh with. - public unsafe Mesh(Span data, BufferPool pool) + public unsafe Mesh(Span data, IUnmanagedMemoryPool pool) { if (data.Length < 16) throw new ArgumentException("Data is not large enough to contain a header."); @@ -192,7 +192,7 @@ public readonly void ComputeBounds(Quaternion orientation, out Vector3 min, out } } - public readonly ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches) + public readonly ShapeBatch CreateShapeBatch(IUnmanagedMemoryPool pool, int initialCapacity, Shapes shapeBatches) { return new HomogeneousCompoundShapeBatch(pool, initialCapacity); } @@ -273,7 +273,7 @@ public readonly unsafe void RayTest(in RigidPose pose, ref RaySo hitHandler = leafTester.HitHandler; } - public readonly unsafe void FindLocalOverlaps(ref Buffer pairs, BufferPool pool, Shapes shapes, ref TOverlaps overlaps) + public readonly unsafe void FindLocalOverlaps(ref Buffer pairs, IUnmanagedMemoryPool pool, Shapes shapes, ref TOverlaps overlaps) where TOverlaps : struct, ICollisionTaskOverlaps where TSubpairOverlaps : struct, ICollisionTaskSubpairOverlaps { @@ -294,7 +294,7 @@ public readonly unsafe void FindLocalOverlaps(ref B } } - public readonly unsafe void FindLocalOverlaps(Vector3 min, Vector3 max, Vector3 sweep, float maximumT, BufferPool pool, Shapes shapes, void* overlaps) + public readonly unsafe void FindLocalOverlaps(Vector3 min, Vector3 max, Vector3 sweep, float maximumT, IUnmanagedMemoryPool pool, Shapes shapes, void* overlaps) where TOverlaps : ICollisionTaskSubpairOverlaps { var scaledMin = min * inverseScale; @@ -470,7 +470,7 @@ public readonly Vector3 ComputeOpenCenterOfMass() /// Returns the mesh's resources to a buffer pool. /// /// Pool to return the mesh's resources to. - public void Dispose(BufferPool bufferPool) + public void Dispose(IUnmanagedMemoryPool bufferPool) { bufferPool.Return(ref Triangles); Tree.Dispose(bufferPool); diff --git a/BepuPhysics/Collidables/Shapes.cs b/BepuPhysics/Collidables/Shapes.cs index efbdb6982..f990d1f2a 100644 --- a/BepuPhysics/Collidables/Shapes.cs +++ b/BepuPhysics/Collidables/Shapes.cs @@ -18,7 +18,7 @@ public abstract class ShapeBatch /// Gets the number of shapes that the batch can currently hold without resizing. /// public int Capacity { get { return shapesData.Length / shapeDataSize; } } - protected BufferPool pool; + protected IUnmanagedMemoryPool pool; protected IdPool idPool; /// /// Gets the type id of the shape type in this batch. @@ -33,21 +33,21 @@ public abstract class ShapeBatch /// public int ShapeDataSize { get { return shapeDataSize; } } - protected abstract void Dispose(int index, BufferPool pool); - protected abstract void RemoveAndDisposeChildren(int index, Shapes shapes, BufferPool pool); + protected abstract void Dispose(int index, IUnmanagedMemoryPool pool); + protected abstract void RemoveAndDisposeChildren(int index, Shapes shapes, IUnmanagedMemoryPool pool); public void Remove(int index) { idPool.Return(index, pool); } - public void RemoveAndDispose(int index, BufferPool pool) + public void RemoveAndDispose(int index, IUnmanagedMemoryPool pool) { Dispose(index, pool); Remove(index); } - public void RecursivelyRemoveAndDispose(int index, Shapes shapes, BufferPool pool) + public void RecursivelyRemoveAndDispose(int index, Shapes shapes, IUnmanagedMemoryPool pool) { RemoveAndDisposeChildren(index, shapes, pool); RemoveAndDispose(index, pool); @@ -133,7 +133,7 @@ public abstract class ShapeBatch : ShapeBatch where TShape : unmanaged, /// Reference to the shape at the given index. public ref TShape this[int shapeIndex] { get { return ref shapes[shapeIndex]; } } - protected ShapeBatch(BufferPool pool, int initialShapeCount) + protected ShapeBatch(IUnmanagedMemoryPool pool, int initialShapeCount) { this.pool = pool; TypeId = default(TShape).TypeId; @@ -231,16 +231,16 @@ public class ConvexShapeBatch : ShapeBatch, IConvexS where TShape : unmanaged, IConvexShape where TShapeWide : unmanaged, IShapeWide { - public ConvexShapeBatch(BufferPool pool, int initialShapeCount) : base(pool, initialShapeCount) + public ConvexShapeBatch(IUnmanagedMemoryPool pool, int initialShapeCount) : base(pool, initialShapeCount) { } - protected override void Dispose(int index, BufferPool pool) + protected override void Dispose(int index, IUnmanagedMemoryPool pool) { //Most convex shapes with an associated Wide type doesn't have any internal resources to dispose. } - protected override void RemoveAndDisposeChildren(int index, Shapes shapes, BufferPool pool) + protected override void RemoveAndDisposeChildren(int index, Shapes shapes, IUnmanagedMemoryPool pool) { //And they don't have any children. } @@ -283,11 +283,11 @@ public unsafe override void RayTest(int index, in RigidPose pose public class ConvexHullShapeBatch : ConvexShapeBatch { - public ConvexHullShapeBatch(BufferPool pool, int initialShapeCount) : base(pool, initialShapeCount) + public ConvexHullShapeBatch(IUnmanagedMemoryPool pool, int initialShapeCount) : base(pool, initialShapeCount) { } - protected override void Dispose(int index, BufferPool pool) + protected override void Dispose(int index, IUnmanagedMemoryPool pool) { shapes[index].Dispose(pool); } @@ -298,17 +298,17 @@ public class HomogeneousCompoundShapeBatch where TChildShape : unmanaged, IConvexShape where TChildShapeWide : unmanaged, IShapeWide { - public HomogeneousCompoundShapeBatch(BufferPool pool, int initialShapeCount) : base(pool, initialShapeCount) + public HomogeneousCompoundShapeBatch(IUnmanagedMemoryPool pool, int initialShapeCount) : base(pool, initialShapeCount) { Compound = true; } - protected override void Dispose(int index, BufferPool pool) + protected override void Dispose(int index, IUnmanagedMemoryPool pool) { shapes[index].Dispose(pool); } - protected override void RemoveAndDisposeChildren(int index, Shapes shapes, BufferPool pool) + protected override void RemoveAndDisposeChildren(int index, Shapes shapes, IUnmanagedMemoryPool pool) { //Meshes and other single-type containers don't have any shape-registered children. } @@ -337,18 +337,18 @@ public class CompoundShapeBatch : ShapeBatch where TShape : unma { Shapes shapeBatches; - public CompoundShapeBatch(BufferPool pool, int initialShapeCount, Shapes shapeBatches) : base(pool, initialShapeCount) + public CompoundShapeBatch(IUnmanagedMemoryPool pool, int initialShapeCount, Shapes shapeBatches) : base(pool, initialShapeCount) { this.shapeBatches = shapeBatches; Compound = true; } - protected override void Dispose(int index, BufferPool pool) + protected override void Dispose(int index, IUnmanagedMemoryPool pool) { shapes[index].Dispose(pool); } - protected override void RemoveAndDisposeChildren(int index, Shapes shapes, BufferPool pool) + protected override void RemoveAndDisposeChildren(int index, Shapes shapes, IUnmanagedMemoryPool pool) { ref var shape = ref this.shapes[index]; for (int i = 0; i < shape.ChildCount; ++i) @@ -393,10 +393,10 @@ public class Shapes public int InitialCapacityPerTypeBatch { get; set; } public ShapeBatch this[int typeIndex] => batches[typeIndex]; - BufferPool pool; + IUnmanagedMemoryPool pool; - public Shapes(BufferPool pool, int initialCapacityPerTypeBatch) + public Shapes(IUnmanagedMemoryPool pool, int initialCapacityPerTypeBatch) { InitialCapacityPerTypeBatch = initialCapacityPerTypeBatch; //This list pretty much will never resize unless something really strange happens, and since batches use virtual calls, we have to allow storage of reference types. @@ -467,7 +467,7 @@ public TypedIndex Add(in TShape shape) where TShape : unmanaged, IShape /// /// Index of the shape to remove. /// Pool to return all shape resources to. - public void RecursivelyRemoveAndDispose(TypedIndex shapeIndex, BufferPool pool) + public void RecursivelyRemoveAndDispose(TypedIndex shapeIndex, IUnmanagedMemoryPool pool) { if (shapeIndex.Exists) { @@ -481,7 +481,7 @@ public void RecursivelyRemoveAndDispose(TypedIndex shapeIndex, BufferPool pool) /// /// Index of the shape to remove. /// Pool to return all shape resources to. - public void RemoveAndDispose(TypedIndex shapeIndex, BufferPool pool) + public void RemoveAndDispose(TypedIndex shapeIndex, IUnmanagedMemoryPool pool) { if (shapeIndex.Exists) { diff --git a/BepuPhysics/Collidables/Sphere.cs b/BepuPhysics/Collidables/Sphere.cs index 21f39c092..3e2678125 100644 --- a/BepuPhysics/Collidables/Sphere.cs +++ b/BepuPhysics/Collidables/Sphere.cs @@ -108,7 +108,7 @@ public readonly BodyInertia ComputeInertia(float mass) return inertia; } - public readonly ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapes) + public readonly ShapeBatch CreateShapeBatch(IUnmanagedMemoryPool pool, int initialCapacity, Shapes shapes) { return new ConvexShapeBatch(pool, initialCapacity); } diff --git a/BepuPhysics/Collidables/Triangle.cs b/BepuPhysics/Collidables/Triangle.cs index 998b37873..b91f3693b 100644 --- a/BepuPhysics/Collidables/Triangle.cs +++ b/BepuPhysics/Collidables/Triangle.cs @@ -118,7 +118,7 @@ public readonly BodyInertia ComputeInertia(float mass) return inertia; } - public readonly ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches) + public readonly ShapeBatch CreateShapeBatch(IUnmanagedMemoryPool pool, int initialCapacity, Shapes shapeBatches) { return new ConvexShapeBatch(pool, initialCapacity); } diff --git a/BepuPhysics/CollisionDetection/BroadPhase.cs b/BepuPhysics/CollisionDetection/BroadPhase.cs index fa2454784..4cfc35ada 100644 --- a/BepuPhysics/CollisionDetection/BroadPhase.cs +++ b/BepuPhysics/CollisionDetection/BroadPhase.cs @@ -155,7 +155,7 @@ public void UpdateStaticBounds(int broadPhaseIndex, Vector3 min, Vector3 max) IThreadDispatcher threadDispatcher; void ExecuteRefitAndMark(int workerIndex, void* context) { - var threadPool = threadDispatcher.GetThreadMemoryPool(workerIndex); + var threadPool = threadDispatcher.WorkerPools[workerIndex]; while (true) { var jobIndex = Interlocked.Decrement(ref remainingJobCount); @@ -176,7 +176,7 @@ void ExecuteRefitAndMark(int workerIndex, void* context) void ExecuteRefine(int workerIndex, void* context) { - var threadPool = threadDispatcher.GetThreadMemoryPool(workerIndex); + var threadPool = threadDispatcher.WorkerPools[workerIndex]; var maximumSubtrees = Math.Max(activeRefineContext.MaximumSubtrees, staticRefineContext.MaximumSubtrees); var subtreeReferences = new QuickList(maximumSubtrees, threadPool); var treeletInternalNodes = new QuickList(maximumSubtrees, threadPool); diff --git a/BepuPhysics/CollisionDetection/CollisionBatcher.cs b/BepuPhysics/CollisionDetection/CollisionBatcher.cs index ce7be21c4..e6b3169c7 100644 --- a/BepuPhysics/CollisionDetection/CollisionBatcher.cs +++ b/BepuPhysics/CollisionDetection/CollisionBatcher.cs @@ -33,7 +33,7 @@ struct CollisionBatch public struct CollisionBatcher where TCallbacks : struct, ICollisionCallbacks { - public BufferPool Pool; + public IUnmanagedMemoryPool Pool; public Shapes Shapes; CollisionTaskRegistry typeMatrix; public TCallbacks Callbacks; @@ -54,7 +54,7 @@ public struct CollisionBatcher where TCallbacks : struct, ICollision public BatcherContinuations MeshReductions; public BatcherContinuations CompoundMeshReductions; - public unsafe CollisionBatcher(BufferPool pool, Shapes shapes, CollisionTaskRegistry collisionTypeMatrix, float dt, TCallbacks callbacks) + public unsafe CollisionBatcher(IUnmanagedMemoryPool pool, Shapes shapes, CollisionTaskRegistry collisionTypeMatrix, float dt, TCallbacks callbacks) { Pool = pool; Shapes = shapes; diff --git a/BepuPhysics/CollisionDetection/CollisionBatcherContinuations.cs b/BepuPhysics/CollisionDetection/CollisionBatcherContinuations.cs index acade2a18..e074cf798 100644 --- a/BepuPhysics/CollisionDetection/CollisionBatcherContinuations.cs +++ b/BepuPhysics/CollisionDetection/CollisionBatcherContinuations.cs @@ -17,7 +17,7 @@ public interface ICollisionTestContinuation /// /// Number of subpair slots to include in the continuation. /// Pool to take resources from. - void Create(int slots, BufferPool pool); + void Create(int slots, IUnmanagedMemoryPool pool); /// /// Handles what to do next when the child pair has finished execution and the resulting manifold is available. @@ -140,7 +140,7 @@ public struct BatcherContinuations where T : unmanaged, ICollisionTestContinu public IdPool IdPool; const int InitialCapacity = 64; - public ref T CreateContinuation(int slotsInContinuation, BufferPool pool, out int index) + public ref T CreateContinuation(int slotsInContinuation, IUnmanagedMemoryPool pool, out int index) { if (!Continuations.Allocated) { @@ -186,7 +186,7 @@ public unsafe void ContributeUntestedChildToContinuation(ref PairCon } - internal void Dispose(BufferPool pool) + internal void Dispose(IUnmanagedMemoryPool pool) { if (Continuations.Allocated) { diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairCollisionTask.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairCollisionTask.cs index f0052f682..9bced1c14 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairCollisionTask.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairCollisionTask.cs @@ -11,7 +11,7 @@ namespace BepuPhysics.CollisionDetection.CollisionTasks { public interface ICompoundPairOverlapFinder { - void FindLocalOverlaps(ref Buffer pairs, int pairCount, BufferPool pool, Shapes shapes, float dt, out CompoundPairOverlaps overlaps); + void FindLocalOverlaps(ref Buffer pairs, int pairCount, IUnmanagedMemoryPool pool, Shapes shapes, float dt, out CompoundPairOverlaps overlaps); } public unsafe interface ICompoundPairContinuationHandler where TContinuation : struct, ICollisionTestContinuation diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlapFinder.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlapFinder.cs index f48c26f67..53dbb2c87 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlapFinder.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlapFinder.cs @@ -20,7 +20,7 @@ public struct CompoundPairOverlapFinder : ICompoundPairO where TCompoundB : struct, IBoundsQueryableCompound { - public unsafe void FindLocalOverlaps(ref Buffer pairs, int pairCount, BufferPool pool, Shapes shapes, float dt, out CompoundPairOverlaps overlaps) + public unsafe void FindLocalOverlaps(ref Buffer pairs, int pairCount, IUnmanagedMemoryPool pool, Shapes shapes, float dt, out CompoundPairOverlaps overlaps) { var totalCompoundChildCount = 0; for (int i = 0; i < pairCount; ++i) diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlaps.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlaps.cs index 268d22f86..0782e8cd8 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlaps.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlaps.cs @@ -12,7 +12,7 @@ namespace BepuPhysics.CollisionDetection.CollisionTasks { public interface ICollisionTaskSubpairOverlaps { - ref int Allocate(BufferPool pool); + ref int Allocate(IUnmanagedMemoryPool pool); } public interface ICollisionTaskOverlaps where TSubpairOverlaps : struct, ICollisionTaskSubpairOverlaps @@ -27,7 +27,7 @@ public unsafe struct ChildOverlapsCollection : ICollisionTaskSubpairOverlaps public int ChildIndex; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe ref int Allocate(BufferPool pool) + public unsafe ref int Allocate(IUnmanagedMemoryPool pool) { if (Overlaps.Length == Count) { @@ -37,7 +37,7 @@ public unsafe ref int Allocate(BufferPool pool) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Dispose(BufferPool pool) + public void Dispose(IUnmanagedMemoryPool pool) { if (Overlaps.Allocated) pool.Return(ref Overlaps); @@ -51,7 +51,7 @@ public struct CompoundPairOverlaps : ICollisionTaskOverlaps pairRegions; int pairCount; int subpairCount; - public CompoundPairOverlaps(BufferPool pool, int pairCapacity, int subpairCapacity) + public CompoundPairOverlaps(IUnmanagedMemoryPool pool, int pairCapacity, int subpairCapacity) { this.pairCount = 0; this.subpairCount = 0; @@ -84,7 +84,7 @@ public void GetPairOverlaps(int pairIndex, out Buffer p this.pairQueries.Slice(region.start, region.count, out pairQueries); } - public void Dispose(BufferPool pool) + public void Dispose(IUnmanagedMemoryPool pool) { for (int i = 0; i < subpairCount; ++i) { diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundOverlapFinder.cs b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundOverlapFinder.cs index f70657399..e89898e02 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundOverlapFinder.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundOverlapFinder.cs @@ -15,16 +15,16 @@ public unsafe struct OverlapQueryForPair public unsafe interface IBoundsQueryableCompound { - unsafe void FindLocalOverlaps(ref Buffer pairs, BufferPool pool, Shapes shapes, ref TOverlaps overlaps) + unsafe void FindLocalOverlaps(ref Buffer pairs, IUnmanagedMemoryPool pool, Shapes shapes, ref TOverlaps overlaps) where TOverlaps : struct, ICollisionTaskOverlaps where TSubpairOverlaps : struct, ICollisionTaskSubpairOverlaps; - unsafe void FindLocalOverlaps(Vector3 min, Vector3 max, Vector3 sweep, float maximumT, BufferPool pool, Shapes shapes, void* overlaps) + unsafe void FindLocalOverlaps(Vector3 min, Vector3 max, Vector3 sweep, float maximumT, IUnmanagedMemoryPool pool, Shapes shapes, void* overlaps) where TOverlaps : ICollisionTaskSubpairOverlaps; } public interface IConvexCompoundOverlapFinder { - void FindLocalOverlaps(ref Buffer pairs, int pairCount, BufferPool pool, Shapes shapes, float dt, out ConvexCompoundTaskOverlaps overlaps); + void FindLocalOverlaps(ref Buffer pairs, int pairCount, IUnmanagedMemoryPool pool, Shapes shapes, float dt, out ConvexCompoundTaskOverlaps overlaps); } public struct ConvexCompoundOverlapFinder : IConvexCompoundOverlapFinder @@ -32,7 +32,7 @@ public struct ConvexCompoundOverlapFinder : ICo where TConvexWide : struct, IShapeWide where TCompound : struct, IBoundsQueryableCompound { - public unsafe void FindLocalOverlaps(ref Buffer pairs, int pairCount, BufferPool pool, Shapes shapes, float dt, out ConvexCompoundTaskOverlaps overlaps) + public unsafe void FindLocalOverlaps(ref Buffer pairs, int pairCount, IUnmanagedMemoryPool pool, Shapes shapes, float dt, out ConvexCompoundTaskOverlaps overlaps) { overlaps = new ConvexCompoundTaskOverlaps(pool, pairCount); ref var pairsToTest = ref overlaps.subpairQueries; diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundTaskOverlaps.cs b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundTaskOverlaps.cs index d814cc6e0..65f1f62f2 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundTaskOverlaps.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundTaskOverlaps.cs @@ -10,7 +10,7 @@ public unsafe struct ConvexCompoundOverlaps : ICollisionTaskSubpairOverlaps public int Count; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe ref int Allocate(BufferPool pool) + public unsafe ref int Allocate(IUnmanagedMemoryPool pool) { if (Overlaps.Length == Count) { @@ -20,7 +20,7 @@ public unsafe ref int Allocate(BufferPool pool) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Dispose(BufferPool pool) + public void Dispose(IUnmanagedMemoryPool pool) { if (Overlaps.Allocated) pool.Return(ref Overlaps); @@ -31,7 +31,7 @@ public struct ConvexCompoundTaskOverlaps : ICollisionTaskOverlaps overlaps; internal Buffer subpairQueries; - public ConvexCompoundTaskOverlaps(BufferPool pool, int pairCount) + public ConvexCompoundTaskOverlaps(IUnmanagedMemoryPool pool, int pairCount) { pool.Take(pairCount, out overlaps); pool.Take(pairCount, out subpairQueries); @@ -50,7 +50,7 @@ public ref OverlapQueryForPair GetQueryForPair(int pairIndex) return ref subpairQueries[pairIndex]; } - public void Dispose(BufferPool pool) + public void Dispose(IUnmanagedMemoryPool pool) { for (int i = 0; i < overlaps.Length; ++i) { diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/MeshPairOverlapFinder.cs b/BepuPhysics/CollisionDetection/CollisionTasks/MeshPairOverlapFinder.cs index 73f356c1a..85e051a1a 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/MeshPairOverlapFinder.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/MeshPairOverlapFinder.cs @@ -15,7 +15,7 @@ public struct MeshPairOverlapFinder : ICompoundPairOverlapFinder where TMeshB : struct, IHomogeneousCompoundShape { - public unsafe void FindLocalOverlaps(ref Buffer pairs, int pairCount, BufferPool pool, Shapes shapes, float dt, out CompoundPairOverlaps overlaps) + public unsafe void FindLocalOverlaps(ref Buffer pairs, int pairCount, IUnmanagedMemoryPool pool, Shapes shapes, float dt, out CompoundPairOverlaps overlaps) { var totalCompoundChildCount = 0; for (int i = 0; i < pairCount; ++i) diff --git a/BepuPhysics/CollisionDetection/CompoundMeshReduction.cs b/BepuPhysics/CollisionDetection/CompoundMeshReduction.cs index cc3769c85..d420ac5bf 100644 --- a/BepuPhysics/CollisionDetection/CompoundMeshReduction.cs +++ b/BepuPhysics/CollisionDetection/CompoundMeshReduction.cs @@ -27,7 +27,7 @@ public unsafe struct CompoundMeshReduction : ICollisionTestContinuation public Mesh* Mesh; //TODO: This is not flexible with respect to different mesh types. Not a problem right now, but it will be in the future. - public void Create(int childManifoldCount, BufferPool pool) + public void Create(int childManifoldCount, IUnmanagedMemoryPool pool) { Inner.Create(childManifoldCount, pool); } diff --git a/BepuPhysics/CollisionDetection/ConstraintRemover.cs b/BepuPhysics/CollisionDetection/ConstraintRemover.cs index d7cd8e16b..b77ce6efc 100644 --- a/BepuPhysics/CollisionDetection/ConstraintRemover.cs +++ b/BepuPhysics/CollisionDetection/ConstraintRemover.cs @@ -50,7 +50,7 @@ struct RemovalCache //Storing this stuff by constraint batch is an option, but it would require more prefiltering that isn't free. int minimumCapacityPerBatch; - public RemovalCache(BufferPool pool, int batchCapacity, int minimumCapacityPerBatch) + public RemovalCache(IUnmanagedMemoryPool pool, int batchCapacity, int minimumCapacityPerBatch) { this.minimumCapacityPerBatch = minimumCapacityPerBatch; @@ -71,7 +71,7 @@ public int IndexOf(TypeBatchIndex typeBatchIndex) } return -1; } - public int AllocateSpaceForTargets(TypeBatchIndex typeBatchIndex, int constraintHandleCount, int perBodyRemovalCount, BufferPool pool) + public int AllocateSpaceForTargets(TypeBatchIndex typeBatchIndex, int constraintHandleCount, int perBodyRemovalCount, IUnmanagedMemoryPool pool) { var index = IndexOf(typeBatchIndex); if (index >= 0) @@ -95,7 +95,7 @@ public int AllocateSpaceForTargets(TypeBatchIndex typeBatchIndex, int constraint return index; } - public void Dispose(BufferPool pool) + public void Dispose(IUnmanagedMemoryPool pool) { pool.Return(ref TypeBatches); for (int i = 0; i < BatchCount; ++i) @@ -113,10 +113,10 @@ public void Dispose(BufferPool pool) struct WorkerCache { - internal BufferPool pool; + internal IUnmanagedMemoryPool pool; public RemovalCache Removals; - public WorkerCache(BufferPool pool, int batchCapacity, int minimumCapacityPerBatch) + public WorkerCache(IUnmanagedMemoryPool pool, int batchCapacity, int minimumCapacityPerBatch) { this.pool = pool; Debug.Assert(minimumCapacityPerBatch > 0); @@ -200,7 +200,7 @@ public void Prepare(IThreadDispatcher dispatcher) for (int i = 0; i < threadCount; ++i) { //Note the use of per-thread pools. It is possible for the workers to resize the collections. - workerCaches[i] = new WorkerCache(dispatcher.GetThreadMemoryPool(i), batchCapacity, capacityPerBatch); + workerCaches[i] = new WorkerCache(dispatcher.WorkerPools[i], batchCapacity, capacityPerBatch); } } else diff --git a/BepuPhysics/CollisionDetection/FreshnessChecker.cs b/BepuPhysics/CollisionDetection/FreshnessChecker.cs index bbe6cf9e6..152b7e994 100644 --- a/BepuPhysics/CollisionDetection/FreshnessChecker.cs +++ b/BepuPhysics/CollisionDetection/FreshnessChecker.cs @@ -164,7 +164,7 @@ unsafe void EnqueueStaleRemoval(int workerIndex, int pairIndex) var constraintHandle = pairCache.GetOldConstraintHandle(pairIndex); constraintRemover.EnqueueRemoval(workerIndex, constraintHandle); ref var pendingChanges = ref pairCache.WorkerPendingChanges[workerIndex]; - pendingChanges.PendingRemoves.Add(pairCache.Mapping.Keys[pairIndex], cachedDispatcher == null ? pairCache.pool : cachedDispatcher.GetThreadMemoryPool(workerIndex)); + pendingChanges.PendingRemoves.Add(pairCache.Mapping.Keys[pairIndex], cachedDispatcher == null ? pairCache.pool : cachedDispatcher.WorkerPools[workerIndex]); } } } diff --git a/BepuPhysics/CollisionDetection/MeshReduction.cs b/BepuPhysics/CollisionDetection/MeshReduction.cs index cd9105360..9bd3a2b83 100644 --- a/BepuPhysics/CollisionDetection/MeshReduction.cs +++ b/BepuPhysics/CollisionDetection/MeshReduction.cs @@ -34,7 +34,7 @@ public unsafe struct MeshReduction : ICollisionTestContinuation public void* Mesh; //TODO: This is not flexible with respect to different mesh types. Not a problem right now, but it will be in the future. - public void Create(int childManifoldCount, BufferPool pool) + public void Create(int childManifoldCount, IUnmanagedMemoryPool pool) { Inner.Create(childManifoldCount, pool); } @@ -286,7 +286,7 @@ static void TryApplyBlockToTriangle(ref TestTriangle triangle, Buffer { public QuickList List; - public BufferPool Pool; + public IUnmanagedMemoryPool Pool; public bool LoopBody(int i) { List.Allocate(Pool) = i; @@ -295,7 +295,7 @@ public bool LoopBody(int i) } public unsafe static void ReduceManifolds(ref Buffer continuationTriangles, ref Buffer continuationChildren, int start, int count, - bool requiresFlip, in BoundingBox queryBounds, in Matrix3x3 meshOrientation, in Matrix3x3 meshInverseOrientation, Mesh* mesh, BufferPool pool) + bool requiresFlip, in BoundingBox queryBounds, in Matrix3x3 meshOrientation, in Matrix3x3 meshInverseOrientation, Mesh* mesh, IUnmanagedMemoryPool pool) { //Before handing responsibility off to the nonconvex reduction, make sure that no contacts create nasty 'bumps' at the border of triangles. //Bumps can occur when an isolated triangle test detects a contact pointing outward, like when a box hits the side. This is fine when the triangle truly is isolated, diff --git a/BepuPhysics/CollisionDetection/NarrowPhase.cs b/BepuPhysics/CollisionDetection/NarrowPhase.cs index 101b0c899..5a00dd9a0 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhase.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhase.cs @@ -339,7 +339,7 @@ public struct OverlapWorker public PendingConstraintAddCache PendingConstraints; public QuickList PendingSetAwakenings; - public OverlapWorker(int workerIndex, BufferPool pool, NarrowPhase narrowPhase) + public OverlapWorker(int workerIndex, IUnmanagedMemoryPool pool, NarrowPhase narrowPhase) { Batcher = new CollisionBatcher(pool, narrowPhase.Shapes, narrowPhase.CollisionTaskRegistry, narrowPhase.timestepDuration, new CollisionCallbacks(workerIndex, pool, narrowPhase)); @@ -351,7 +351,7 @@ public OverlapWorker(int workerIndex, BufferPool pool, NarrowPhase n internal OverlapWorker[] overlapWorkers; public NarrowPhase(Simulation simulation, CollisionTaskRegistry collisionTaskRegistry, SweepTaskRegistry sweepTaskRegistry, TCallbacks callbacks, - int initialSetCapacity, int minimumMappingSize = 2048, int minimumPendingSize = 128, int minimumPerTypeCapacity = 128) + int initialSetCapacity, int minimumMappingSize = 2048, int minimumPendingSize = 128) : base() { Simulation = simulation; @@ -364,7 +364,7 @@ public NarrowPhase(Simulation simulation, CollisionTaskRegistry collisionTaskReg Callbacks = callbacks; CollisionTaskRegistry = collisionTaskRegistry; SweepTaskRegistry = sweepTaskRegistry; - PairCache = new PairCache(simulation.BufferPool, initialSetCapacity, minimumMappingSize, minimumPendingSize, minimumPerTypeCapacity); + PairCache = new PairCache(simulation.BufferPool, initialSetCapacity, minimumMappingSize, minimumPendingSize); FreshnessChecker = new FreshnessChecker(this); preflushWorkerLoop = PreflushWorkerLoop; } @@ -378,7 +378,7 @@ protected override void OnPrepare(IThreadDispatcher threadDispatcher) Array.Resize(ref overlapWorkers, threadCount); for (int i = 0; i < threadCount; ++i) { - overlapWorkers[i] = new OverlapWorker(i, threadDispatcher != null ? threadDispatcher.GetThreadMemoryPool(i) : Pool, this); + overlapWorkers[i] = new OverlapWorker(i, threadDispatcher != null ? threadDispatcher.WorkerPools[i] : Pool, this); } } diff --git a/BepuPhysics/CollisionDetection/NarrowPhaseCCDContinuations.cs b/BepuPhysics/CollisionDetection/NarrowPhaseCCDContinuations.cs index 06bc39b65..52d91ead7 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhaseCCDContinuations.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhaseCCDContinuations.cs @@ -30,7 +30,7 @@ public enum ConstraintGeneratorType public struct CollisionCallbacks : ICollisionCallbacks { int workerIndex; - BufferPool pool; + IUnmanagedMemoryPool pool; NarrowPhase narrowPhase; @@ -70,14 +70,14 @@ struct ContinuationCache where T : unmanaged public IdPool Ids; public Buffer Caches; - public ContinuationCache(BufferPool pool) + public ContinuationCache(IUnmanagedMemoryPool pool) { Ids = new IdPool(32, pool); pool.TakeAtLeast(128, out Caches); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref T Allocate(BufferPool pool, out int index) + public ref T Allocate(IUnmanagedMemoryPool pool, out int index) { index = Ids.Take(); if (Caches.Length <= index) @@ -88,13 +88,13 @@ public ref T Allocate(BufferPool pool, out int index) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Return(int index, BufferPool pool) + public void Return(int index, IUnmanagedMemoryPool pool) { Ids.Return(index, pool); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Dispose(BufferPool pool) + public void Dispose(IUnmanagedMemoryPool pool) { Ids.Dispose(pool); pool.Return(ref Caches); @@ -104,7 +104,7 @@ public void Dispose(BufferPool pool) ContinuationCache discrete; ContinuationCache continuous; - public CollisionCallbacks(int workerIndex, BufferPool pool, NarrowPhase narrowPhase) + public CollisionCallbacks(int workerIndex, IUnmanagedMemoryPool pool, NarrowPhase narrowPhase) { this.pool = pool; this.workerIndex = workerIndex; diff --git a/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs b/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs index 59fbd0f87..b2804595e 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs @@ -21,7 +21,7 @@ public partial class NarrowPhase { public struct PendingConstraintAddCache { - BufferPool pool; + IUnmanagedMemoryPool pool; [StructLayout(LayoutKind.Sequential)] unsafe struct PendingConstraint where TBodyHandles : unmanaged where TDescription : unmanaged, IConstraintDescription { @@ -37,7 +37,7 @@ unsafe struct PendingConstraint wh internal Buffer> speculativeBatchIndices; int minimumConstraintCountPerCache; - public PendingConstraintAddCache(BufferPool pool, int minimumConstraintCountPerCache = 128) + public PendingConstraintAddCache(IUnmanagedMemoryPool pool, int minimumConstraintCountPerCache = 128) { this.pool = pool; pool.TakeAtLeast(PairCache.CollisionConstraintTypeCount, out pendingConstraintsByType); diff --git a/BepuPhysics/CollisionDetection/NonconvexReduction.cs b/BepuPhysics/CollisionDetection/NonconvexReduction.cs index b5e677224..1f878e4fa 100644 --- a/BepuPhysics/CollisionDetection/NonconvexReduction.cs +++ b/BepuPhysics/CollisionDetection/NonconvexReduction.cs @@ -32,7 +32,7 @@ public struct NonconvexReduction : ICollisionTestContinuation public int CompletedChildCount; public Buffer Children; - public void Create(int childManifoldCount, BufferPool pool) + public void Create(int childManifoldCount, IUnmanagedMemoryPool pool) { ChildCount = childManifoldCount; CompletedChildCount = 0; @@ -102,7 +102,7 @@ static float ComputeDistinctiveness(in ConvexContact candidate, Vector3 contactN return distinctiveness; } - unsafe void ChooseMostDistinct(NonconvexContactManifold* manifold, BufferPool pool) + unsafe void ChooseMostDistinct(NonconvexContactManifold* manifold, IUnmanagedMemoryPool pool) { //The end goal of contact reduction is to choose a reasonably stable subset of contacts which offer the greatest degree of constraint. //Computing a globally optimal solution to this would be pretty expensive for any given scoring mechanism, so we'll make some simplifications: diff --git a/BepuPhysics/CollisionDetection/PairCache.cs b/BepuPhysics/CollisionDetection/PairCache.cs index e980ef015..25c4ea8ce 100644 --- a/BepuPhysics/CollisionDetection/PairCache.cs +++ b/BepuPhysics/CollisionDetection/PairCache.cs @@ -114,7 +114,6 @@ public partial class PairCache internal Buffer PairFreshness; internal BufferPool pool; int minimumPendingSize; - int minimumPerTypeCapacity; int previousPendingSize; @@ -123,10 +122,9 @@ public partial class PairCache internal IThreadDispatcher cachedDispatcher; - public PairCache(BufferPool pool, int initialSetCapacity, int minimumMappingSize, int minimumPendingSize, int minimumPerTypeCapacity) + public PairCache(BufferPool pool, int initialSetCapacity, int minimumMappingSize, int minimumPendingSize) { this.minimumPendingSize = minimumPendingSize; - this.minimumPerTypeCapacity = minimumPerTypeCapacity; this.pool = pool; Mapping = new OverlapMapping(minimumMappingSize, pool); ResizeSetsCapacity(initialSetCapacity, 0); @@ -143,7 +141,7 @@ public void Prepare(IThreadDispatcher threadDispatcher = null) { for (int i = 0; i < threadCount; ++i) { - WorkerPendingChanges[i] = new WorkerPendingPairChanges(threadDispatcher.GetThreadMemoryPool(i), pendingSize); + WorkerPendingChanges[i] = new WorkerPendingPairChanges(threadDispatcher.WorkerPools[i], pendingSize); } } else @@ -249,7 +247,7 @@ public void Postflush() { for (int i = 0; i < WorkerPendingChanges.Length; ++i) { - WorkerPendingChanges[i].Dispose(cachedDispatcher.GetThreadMemoryPool(i)); + WorkerPendingChanges[i].Dispose(cachedDispatcher.WorkerPools[i]); } } else @@ -313,7 +311,7 @@ public ref ConstraintCache GetCache(int index) internal unsafe PairCacheChangeIndex Add(int workerIndex, CollidablePair pair, in ConstraintCache constraintCache) { //Note that we do not have to set any freshness bytes here; using this path means there exists no previous overlap to remove anyway. - return new PairCacheChangeIndex { WorkerIndex = workerIndex, Index = WorkerPendingChanges[workerIndex].Add(cachedDispatcher == null ? pool : cachedDispatcher.GetThreadMemoryPool(workerIndex), pair, constraintCache) }; + return new PairCacheChangeIndex { WorkerIndex = workerIndex, Index = WorkerPendingChanges[workerIndex].Add(cachedDispatcher == null ? pool : cachedDispatcher.WorkerPools[workerIndex], pair, constraintCache) }; } [MethodImpl(MethodImplOptions.AggressiveInlining)] diff --git a/BepuPhysics/CollisionDetection/RayBatchers.cs b/BepuPhysics/CollisionDetection/RayBatchers.cs index d87ea00a3..67facdb85 100644 --- a/BepuPhysics/CollisionDetection/RayBatchers.cs +++ b/BepuPhysics/CollisionDetection/RayBatchers.cs @@ -50,10 +50,11 @@ public unsafe void TestLeaf(int leafIndex, RayData* rayData, float* maximumT) /// Constructs a ray batcher for the broad phase and initializes its backing resources. /// /// Pool to pull resources from. + /// Broad phase to be tested. /// Ray tester used to test leaves found by the broad phase tree traversals. /// Maximum number of rays to execute in each traversal. /// This should typically be chosen as the highest value which avoids spilling data out of L2 cache. - public BroadPhaseRayBatcher(BufferPool pool, BroadPhase broadPhase, TRayTester rayTester, int batcherRayCapacity = 2048) + public BroadPhaseRayBatcher(IUnmanagedMemoryPool pool, BroadPhase broadPhase, TRayTester rayTester, int batcherRayCapacity = 2048) { activeTester = new LeafTester { Leaves = broadPhase.ActiveLeaves, RayTester = rayTester }; staticTester = new LeafTester { Leaves = broadPhase.StaticLeaves, RayTester = rayTester }; @@ -162,7 +163,7 @@ public unsafe void RayTest(CollidableReference reference, RayData* rayData, floa BroadPhaseRayBatcher batcher; - public SimulationRayBatcher(BufferPool pool, Simulation simulation, TRayHitHandler hitHandler, int batcherRayCapacity = 2048) + public SimulationRayBatcher(IUnmanagedMemoryPool pool, Simulation simulation, TRayHitHandler hitHandler, int batcherRayCapacity = 2048) { Dispatcher dispatcher = default; dispatcher.Simulation = simulation; diff --git a/BepuPhysics/CollisionDetection/SweepTaskRegistry.cs b/BepuPhysics/CollisionDetection/SweepTaskRegistry.cs index 6fc3482a1..4f16b354c 100644 --- a/BepuPhysics/CollisionDetection/SweepTaskRegistry.cs +++ b/BepuPhysics/CollisionDetection/SweepTaskRegistry.cs @@ -69,14 +69,14 @@ protected abstract unsafe bool PreorderedTypeSweep( void* shapeDataA, Quaternion orientationA, in BodyVelocity velocityA, void* shapeDataB, Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float maximumT, float minimumProgression, float convergenceThreshold, int maximumIterationCount, - bool flipRequired, ref TSweepFilter filter, Shapes shapes, SweepTaskRegistry sweepTasks, BufferPool pool, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal) + bool flipRequired, ref TSweepFilter filter, Shapes shapes, SweepTaskRegistry sweepTasks, IUnmanagedMemoryPool pool, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal) where TSweepFilter : ISweepFilter; public unsafe bool Sweep( void* shapeDataA, int shapeTypeA, Quaternion orientationA, in BodyVelocity velocityA, void* shapeDataB, int shapeTypeB, Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float maximumT, float minimumProgression, float convergenceThreshold, int maximumIterationCount, - ref TSweepFilter filter, Shapes shapes, SweepTaskRegistry sweepTasks, BufferPool pool, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal) + ref TSweepFilter filter, Shapes shapes, SweepTaskRegistry sweepTasks, IUnmanagedMemoryPool pool, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal) where TSweepFilter : ISweepFilter { Debug.Assert((shapeTypeA == ShapeTypeIndexA && shapeTypeB == ShapeTypeIndexB) || (shapeTypeA == ShapeTypeIndexB && shapeTypeB == ShapeTypeIndexA), diff --git a/BepuPhysics/CollisionDetection/SweepTasks/CompoundHomogeneousCompoundSweepTask.cs b/BepuPhysics/CollisionDetection/SweepTasks/CompoundHomogeneousCompoundSweepTask.cs index 4330b951b..de5e587c3 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/CompoundHomogeneousCompoundSweepTask.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/CompoundHomogeneousCompoundSweepTask.cs @@ -23,7 +23,7 @@ protected unsafe override bool PreorderedTypeSweep( void* shapeDataA, Quaternion orientationA, in BodyVelocity velocityA, void* shapeDataB, Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float maximumT, float minimumProgression, float convergenceThreshold, int maximumIterationCount, - bool flipRequired, ref TSweepFilter filter, Shapes shapes, SweepTaskRegistry sweepTasks, BufferPool pool, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal) + bool flipRequired, ref TSweepFilter filter, Shapes shapes, SweepTaskRegistry sweepTasks, IUnmanagedMemoryPool pool, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal) { ref var compoundB = ref Unsafe.AsRef(shapeDataB); TOverlapFinder overlapFinder = default; diff --git a/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepOverlapFinder.cs b/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepOverlapFinder.cs index c6d1690cc..a7108362f 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepOverlapFinder.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepOverlapFinder.cs @@ -11,7 +11,7 @@ public interface ICompoundPairSweepOverlapFinder where T { unsafe void FindOverlaps(ref TCompoundA compoundA, Quaternion orientationA, in BodyVelocity velocityA, ref TCompoundB compoundB, Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float maximumT, - Shapes shapes, BufferPool pool, out CompoundPairSweepOverlaps overlaps); + Shapes shapes, IUnmanagedMemoryPool pool, out CompoundPairSweepOverlaps overlaps); } public struct CompoundPairSweepOverlapFinder : ICompoundPairSweepOverlapFinder @@ -21,7 +21,7 @@ public struct CompoundPairSweepOverlapFinder : ICompound public unsafe void FindOverlaps( ref TCompoundA compoundA, Quaternion orientationA, in BodyVelocity velocityA, ref TCompoundB compoundB, Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float maximumT, - Shapes shapes, BufferPool pool, out CompoundPairSweepOverlaps overlaps) + Shapes shapes, IUnmanagedMemoryPool pool, out CompoundPairSweepOverlaps overlaps) { overlaps = new CompoundPairSweepOverlaps(pool, compoundA.ChildCount); for (int i = 0; i < compoundA.ChildCount; ++i) diff --git a/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepOverlaps.cs b/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepOverlaps.cs index a8110a34a..bd5cd28bb 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepOverlaps.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepOverlaps.cs @@ -8,7 +8,7 @@ public struct CompoundPairSweepOverlaps { Buffer childOverlaps; public readonly int ChildCount; - public CompoundPairSweepOverlaps(BufferPool pool, int childCount) + public CompoundPairSweepOverlaps(IUnmanagedMemoryPool pool, int childCount) { ChildCount = childCount; pool.Take(childCount, out childOverlaps); @@ -22,7 +22,7 @@ public ref ChildOverlapsCollection GetOverlapsForChild(int pairIndex) return ref childOverlaps[pairIndex]; } - public void Dispose(BufferPool pool) + public void Dispose(IUnmanagedMemoryPool pool) { for (int i = 0; i < ChildCount; ++i) { diff --git a/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepTask.cs b/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepTask.cs index c080a88bc..75de54c31 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepTask.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepTask.cs @@ -21,7 +21,7 @@ protected override unsafe bool PreorderedTypeSweep( void* shapeDataA, Quaternion orientationA, in BodyVelocity velocityA, void* shapeDataB, Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float maximumT, float minimumProgression, float convergenceThreshold, int maximumIterationCount, - bool flipRequired, ref TSweepFilter filter, Shapes shapes, SweepTaskRegistry sweepTasks, BufferPool pool, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal) + bool flipRequired, ref TSweepFilter filter, Shapes shapes, SweepTaskRegistry sweepTasks, IUnmanagedMemoryPool pool, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal) { ref var a = ref Unsafe.AsRef(shapeDataA); ref var b = ref Unsafe.AsRef(shapeDataB); diff --git a/BepuPhysics/CollisionDetection/SweepTasks/ConvexCompoundSweepOverlapFinder.cs b/BepuPhysics/CollisionDetection/SweepTasks/ConvexCompoundSweepOverlapFinder.cs index 010f1c5f9..a090ddd06 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/ConvexCompoundSweepOverlapFinder.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/ConvexCompoundSweepOverlapFinder.cs @@ -14,7 +14,7 @@ public interface IConvexCompoundSweepOverlapFinder where TS { unsafe void FindOverlaps(ref TShapeA shapeA, Quaternion orientationA, in BodyVelocity velocityA, ref TCompoundB compoundB, Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float maximumT, - Shapes shapes, BufferPool pool, out ChildOverlapsCollection overlaps); + Shapes shapes, IUnmanagedMemoryPool pool, out ChildOverlapsCollection overlaps); } public struct ConvexCompoundSweepOverlapFinder : IConvexCompoundSweepOverlapFinder @@ -22,7 +22,7 @@ public struct ConvexCompoundSweepOverlapFinder : IConvexCom { public unsafe void FindOverlaps(ref TShapeA shapeA, Quaternion orientationA, in BodyVelocity velocityA, ref TCompoundB compoundB, Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float maximumT, - Shapes shapes, BufferPool pool, out ChildOverlapsCollection overlaps) + Shapes shapes, IUnmanagedMemoryPool pool, out ChildOverlapsCollection overlaps) { BoundingBoxHelpers.GetLocalBoundingBoxForSweep(ref shapeA, orientationA, velocityA, offsetB, orientationB, velocityB, maximumT, out var sweep, out var min, out var max); diff --git a/BepuPhysics/CollisionDetection/SweepTasks/ConvexCompoundSweepTask.cs b/BepuPhysics/CollisionDetection/SweepTasks/ConvexCompoundSweepTask.cs index c2c09a35b..c4c22bef5 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/ConvexCompoundSweepTask.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/ConvexCompoundSweepTask.cs @@ -22,7 +22,7 @@ protected override unsafe bool PreorderedTypeSweep( void* shapeDataA, Quaternion orientationA, in BodyVelocity velocityA, void* shapeDataB, Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float maximumT, float minimumProgression, float convergenceThreshold, int maximumIterationCount, - bool flipRequired, ref TSweepFilter filter, Shapes shapes, SweepTaskRegistry sweepTasks, BufferPool pool, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal) + bool flipRequired, ref TSweepFilter filter, Shapes shapes, SweepTaskRegistry sweepTasks, IUnmanagedMemoryPool pool, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal) { ref var convex = ref Unsafe.AsRef(shapeDataA); ref var compound = ref Unsafe.AsRef(shapeDataB); diff --git a/BepuPhysics/CollisionDetection/SweepTasks/ConvexHomogeneousCompoundSweepTask.cs b/BepuPhysics/CollisionDetection/SweepTasks/ConvexHomogeneousCompoundSweepTask.cs index 52ca6159a..34959b82b 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/ConvexHomogeneousCompoundSweepTask.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/ConvexHomogeneousCompoundSweepTask.cs @@ -27,7 +27,7 @@ protected override unsafe bool PreorderedTypeSweep( void* shapeDataA, Quaternion orientationA, in BodyVelocity velocityA, void* shapeDataB, Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float maximumT, float minimumProgression, float convergenceThreshold, int maximumIterationCount, - bool flipRequired, ref TSweepFilter filter, Shapes shapes, SweepTaskRegistry sweepTasks, BufferPool pool, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal) + bool flipRequired, ref TSweepFilter filter, Shapes shapes, SweepTaskRegistry sweepTasks, IUnmanagedMemoryPool pool, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal) { ref var compound = ref Unsafe.AsRef(shapeDataB); t0 = float.MaxValue; diff --git a/BepuPhysics/CollisionDetection/SweepTasks/ConvexSweepTaskCommon.cs b/BepuPhysics/CollisionDetection/SweepTasks/ConvexSweepTaskCommon.cs index 8cdcf0593..bf868f3d4 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/ConvexSweepTaskCommon.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/ConvexSweepTaskCommon.cs @@ -46,7 +46,7 @@ protected override unsafe bool PreorderedTypeSweep( void* shapeDataA, Quaternion orientationA, in BodyVelocity velocityA, void* shapeDataB, Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float maximumT, float minimumProgression, float convergenceThreshold, int maximumIterationCount, - bool requiresFlip, ref TSweepFilter filter, Shapes shapes, SweepTaskRegistry sweepTasks, BufferPool pool, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal) + bool requiresFlip, ref TSweepFilter filter, Shapes shapes, SweepTaskRegistry sweepTasks, IUnmanagedMemoryPool pool, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal) { UnoffsetSweep sweepModifier = default; return Sweep( diff --git a/BepuPhysics/CollisionDetection/UntypedList.cs b/BepuPhysics/CollisionDetection/UntypedList.cs index 2dc7476bc..645e9b800 100644 --- a/BepuPhysics/CollisionDetection/UntypedList.cs +++ b/BepuPhysics/CollisionDetection/UntypedList.cs @@ -14,7 +14,7 @@ public struct UntypedList [MethodImpl(MethodImplOptions.AggressiveInlining)] - public UntypedList(int elementSizeInBytes, int initialCapacityInElements, BufferPool pool) + public UntypedList(int elementSizeInBytes, int initialCapacityInElements, IUnmanagedMemoryPool pool) { pool.TakeAtLeast(initialCapacityInElements * elementSizeInBytes, out Buffer); Count = 0; @@ -23,7 +23,7 @@ public UntypedList(int elementSizeInBytes, int initialCapacityInElements, Buffer } [MethodImpl(MethodImplOptions.AggressiveInlining)] - internal void EnsureCapacityInBytes(int elementSizeInBytes, int targetCapacityInBytes, BufferPool pool) + internal void EnsureCapacityInBytes(int elementSizeInBytes, int targetCapacityInBytes, IUnmanagedMemoryPool pool) { //EnsureCapacity is basically a secondary constructor, but it can be used on already-existing caches. It has the same required output. Debug.Assert(ElementSizeInBytes == 0 || ElementSizeInBytes == elementSizeInBytes, @@ -91,7 +91,7 @@ public unsafe ref T AllocateUnsafely() /// Pool to pull allocations from. /// Index of the element in bytes within the list's buffer. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe int Allocate(int elementSizeInBytes, int minimumElementCount, BufferPool pool) + public unsafe int Allocate(int elementSizeInBytes, int minimumElementCount, IUnmanagedMemoryPool pool) { var newSize = ByteCount + elementSizeInBytes; if (!Buffer.Allocated) @@ -123,14 +123,14 @@ public unsafe int Allocate(int elementSizeInBytes, int minimumElementCount, Buff } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe int Allocate(int minimumElementCount, BufferPool pool) + public unsafe int Allocate(int minimumElementCount, IUnmanagedMemoryPool pool) { var elementSizeInBytes = Unsafe.SizeOf(); return Allocate(elementSizeInBytes, minimumElementCount, pool); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe int Add(ref T data, int minimumCount, BufferPool pool) + public unsafe int Add(ref T data, int minimumCount, IUnmanagedMemoryPool pool) { var byteIndex = Allocate(minimumCount, pool); GetFromBytes(byteIndex) = data; diff --git a/BepuPhysics/CollisionDetection/WorkerPairCache.cs b/BepuPhysics/CollisionDetection/WorkerPairCache.cs index e262dcbaa..f0f99ff86 100644 --- a/BepuPhysics/CollisionDetection/WorkerPairCache.cs +++ b/BepuPhysics/CollisionDetection/WorkerPairCache.cs @@ -26,7 +26,7 @@ public struct PendingAdd /// public QuickList PendingRemoves; - public WorkerPendingPairChanges(BufferPool pool, int pendingCapacity) + public WorkerPendingPairChanges(IUnmanagedMemoryPool pool, int pendingCapacity) { PendingAdds = new QuickList(pendingCapacity, pool); PendingRemoves = new QuickList(pendingCapacity, pool); @@ -34,7 +34,7 @@ public WorkerPendingPairChanges(BufferPool pool, int pendingCapacity) [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe int Add(BufferPool pool, CollidablePair pair, in ConstraintCache cache) + public unsafe int Add(IUnmanagedMemoryPool pool, CollidablePair pair, in ConstraintCache cache) { int index = PendingAdds.Count; ref var pendingAdd = ref PendingAdds.Allocate(pool); @@ -44,7 +44,7 @@ public unsafe int Add(BufferPool pool, CollidablePair pair, in ConstraintCache c } - public void Dispose(BufferPool pool) + public void Dispose(IUnmanagedMemoryPool pool) { PendingAdds.Dispose(pool); PendingRemoves.Dispose(pool); diff --git a/BepuPhysics/IslandScaffold.cs b/BepuPhysics/IslandScaffold.cs index a2e2a8ef7..659f6d412 100644 --- a/BepuPhysics/IslandScaffold.cs +++ b/BepuPhysics/IslandScaffold.cs @@ -22,7 +22,7 @@ internal struct IslandScaffoldTypeBatch public int TypeId; public QuickList Handles; - public IslandScaffoldTypeBatch(BufferPool pool, int typeId, int initialTypeBatchSize) + public IslandScaffoldTypeBatch(IUnmanagedMemoryPool pool, int typeId, int initialTypeBatchSize) { TypeId = typeId; Handles = new QuickList(initialTypeBatchSize, pool); @@ -37,7 +37,7 @@ internal struct IslandScaffoldConstraintBatch //Note that we use *indices* during island construction, not handles. This protobatch doesn't have to deal with memory moves in between adds, so indices are fine. public IndexSet ReferencedBodyIndices; - public unsafe IslandScaffoldConstraintBatch(Solver solver, BufferPool pool, int batchIndex) + public unsafe IslandScaffoldConstraintBatch(Solver solver, IUnmanagedMemoryPool pool, int batchIndex) { pool.TakeAtLeast(solver.TypeProcessors.Length, out TypeIdToIndex); Unsafe.InitBlockUnaligned(TypeIdToIndex.Memory, 0xFF, (uint)(TypeIdToIndex.Length * sizeof(int))); @@ -46,7 +46,7 @@ public unsafe IslandScaffoldConstraintBatch(Solver solver, BufferPool pool, int } [MethodImpl(MethodImplOptions.AggressiveInlining)] - ref IslandScaffoldTypeBatch GetOrCreateTypeBatch(int typeId, Solver solver, BufferPool pool) + ref IslandScaffoldTypeBatch GetOrCreateTypeBatch(int typeId, Solver solver, IUnmanagedMemoryPool pool) { ref var idMap = ref TypeIdToIndex[typeId]; if (idMap == -1) @@ -76,7 +76,7 @@ internal void Validate(Solver solver) } } - public unsafe bool TryAdd(ConstraintHandle constraintHandle, Span dynamicBodyIndices, int typeId, int batchIndex, Solver solver, BufferPool pool, ref SequentialFallbackBatch fallbackBatch) + public unsafe bool TryAdd(ConstraintHandle constraintHandle, Span dynamicBodyIndices, int typeId, int batchIndex, Solver solver, IUnmanagedMemoryPool pool, ref SequentialFallbackBatch fallbackBatch) { if (batchIndex == solver.FallbackBatchThreshold || ReferencedBodyIndices.CanFit(dynamicBodyIndices)) { @@ -105,7 +105,7 @@ public unsafe bool TryAdd(ConstraintHandle constraintHandle, Span dynamicBo return false; } - public void Dispose(BufferPool pool) + public void Dispose(IUnmanagedMemoryPool pool) { for (int i = 0; i < TypeBatches.Count; ++i) { @@ -128,7 +128,7 @@ internal struct IslandScaffold public QuickList Protobatches; public SequentialFallbackBatch FallbackBatch; - public IslandScaffold(ref QuickList bodyIndices, ref QuickList constraintHandles, Solver solver, BufferPool pool) : this() + public IslandScaffold(ref QuickList bodyIndices, ref QuickList constraintHandles, Solver solver, IUnmanagedMemoryPool pool) : this() { Debug.Assert(bodyIndices.Count > 0, "Don't be tryin' to create islands with no bodies in them! That don't make no sense."); //Create a copy of the body indices with just enough space to hold the island's indices. The original list will continue to be reused in the caller. @@ -151,7 +151,7 @@ public void Validate(Solver solver) } } - unsafe void AddConstraint(ConstraintHandle constraintHandle, Solver solver, BufferPool pool) + unsafe void AddConstraint(ConstraintHandle constraintHandle, Solver solver, IUnmanagedMemoryPool pool) { var typeId = solver.HandleToConstraint[constraintHandle.Value].TypeId; var typeProcessor = solver.TypeProcessors[typeId]; @@ -178,7 +178,7 @@ unsafe void AddConstraint(ConstraintHandle constraintHandle, Solver solver, Buff Debug.Assert(addedSuccessfully, "If we created a new batch for a constraint, then it must successfully add."); } - internal void Dispose(BufferPool pool) + internal void Dispose(IUnmanagedMemoryPool pool) { BodyIndices.Dispose(pool); for (int k = 0; k < Protobatches.Count; ++k) diff --git a/BepuPhysics/IslandSleeper.cs b/BepuPhysics/IslandSleeper.cs index 54917ae23..3330c9588 100644 --- a/BepuPhysics/IslandSleeper.cs +++ b/BepuPhysics/IslandSleeper.cs @@ -63,7 +63,7 @@ internal void ReturnSetId(int id) struct ConstraintBodyEnumerator : IForEach { public QuickList ConstraintBodyIndices; - public BufferPool Pool; + public IUnmanagedMemoryPool Pool; public int SourceIndex; public void LoopBody(int bodyIndex) { @@ -113,7 +113,7 @@ public bool Matches(ref int bodyIndex) [MethodImpl(MethodImplOptions.AggressiveInlining)] static bool PushBody(int bodyIndex, ref IndexSet consideredBodies, ref QuickList bodyIndices, ref QuickList visitationStack, - BufferPool pool, ref TTraversalPredicate predicate) where TTraversalPredicate : IPredicate + IUnmanagedMemoryPool pool, ref TTraversalPredicate predicate) where TTraversalPredicate : IPredicate { if (!consideredBodies.Contains(bodyIndex)) { @@ -137,7 +137,7 @@ bool EnqueueUnvisitedNeighbors(int bodyIndex, ref IndexSet consideredBodies, ref IndexSet consideredConstraints, ref QuickList visitationStack, ref ConstraintBodyEnumerator bodyEnumerator, - BufferPool pool, ref TTraversalPredicate predicate) where TTraversalPredicate : IPredicate + IUnmanagedMemoryPool pool, ref TTraversalPredicate predicate) where TTraversalPredicate : IPredicate { bodyEnumerator.SourceIndex = bodyIndex; ref var list = ref bodies.ActiveSet.Constraints[bodyIndex]; @@ -173,7 +173,7 @@ bool EnqueueUnvisitedNeighbors(int bodyIndex, /// List to fill with constraint handles traversed during island collection. /// True if the simulation graph was traversed without ever finding a body that made the predicate return false. False if any body failed the predicate. /// The bodyIndices and constraintHandles lists will contain all traversed predicate-passing bodies and constraints. - public bool CollectIsland(BufferPool pool, int startingActiveBodyIndex, ref TTraversalPredicate predicate, + public bool CollectIsland(IUnmanagedMemoryPool pool, int startingActiveBodyIndex, ref TTraversalPredicate predicate, ref QuickList bodyIndices, ref QuickList constraintHandles) where TTraversalPredicate : IPredicate { Debug.Assert(startingActiveBodyIndex >= 0 && startingActiveBodyIndex < bodies.ActiveSet.Count); @@ -237,7 +237,7 @@ struct WorkerTraversalResults public IndexSet TraversedBodies; public QuickList Islands; - internal void Dispose(BufferPool pool) + internal void Dispose(IUnmanagedMemoryPool pool) { for (int islandIndex = 0; islandIndex < Islands.Count; ++islandIndex) { @@ -269,7 +269,7 @@ struct GatheringJob QuickList gatheringJobs; - void FindIslands(int workerIndex, BufferPool threadPool, ref TPredicate predicate) where TPredicate : IPredicate + void FindIslands(int workerIndex, IUnmanagedMemoryPool threadPool, ref TPredicate predicate) where TPredicate : IPredicate { Debug.Assert(workerTraversalResults.Allocated && workerTraversalResults.Length > workerIndex); ref var results = ref workerTraversalResults[workerIndex]; @@ -310,7 +310,7 @@ void FindIslands(int workerIndex, BufferPool threadPool, ref TPredic constraintHandles.Dispose(threadPool); results.TraversedBodies = traversalTest.PreviouslyTraversedBodies; } - void FindIslands(int workerIndex, BufferPool threadPool) + void FindIslands(int workerIndex, IUnmanagedMemoryPool threadPool) { //This if is handled externally to push the code specialization early. if (forceSleep) @@ -329,7 +329,7 @@ void FindIslands(int workerIndex, BufferPool threadPool) void FindIslands(int workerIndex, void* context) { //The only reason we separate this out is to make it easier for the main pool to be passed in if there is only a single thread. - FindIslands(workerIndex, threadDispatcher.GetThreadMemoryPool(workerIndex)); + FindIslands(workerIndex, threadDispatcher.WorkerPools[workerIndex]); } ThreadDispatcherWorker gatherDelegate; @@ -648,7 +648,7 @@ void DisposeWorkerTraversalResults() //The source of traversal worker resources is a per-thread pool. for (int workerIndex = 0; workerIndex < workerTraversalThreadCount; ++workerIndex) { - workerTraversalResults[workerIndex].Dispose(threadDispatcher.GetThreadMemoryPool(workerIndex)); + workerTraversalResults[workerIndex].Dispose(threadDispatcher.WorkerPools[workerIndex]); } } else diff --git a/BepuPhysics/PoseIntegrator.cs b/BepuPhysics/PoseIntegrator.cs index b9e7abd92..650fe4ef2 100644 --- a/BepuPhysics/PoseIntegrator.cs +++ b/BepuPhysics/PoseIntegrator.cs @@ -397,7 +397,7 @@ bool TryGetJob(int maximumJobInterval, out int start, out int exclusiveEnd) void PredictBoundingBoxesWorker(int workerIndex, void* context) { - var boundingBoxUpdater = new BoundingBoxBatcher(bodies, shapes, broadPhase, threadDispatcher.GetThreadMemoryPool(workerIndex), cachedDt); + var boundingBoxUpdater = new BoundingBoxBatcher(bodies, shapes, broadPhase, threadDispatcher.WorkerPools[workerIndex], cachedDt); var bundleCount = BundleIndexing.GetBundleCount(bodies.ActiveSet.Count); while (TryGetJob(bundleCount, out var start, out var exclusiveEnd)) { diff --git a/BepuPhysics/SequentialFallbackBatch.cs b/BepuPhysics/SequentialFallbackBatch.cs index 4b4a7aa6e..482e28611 100644 --- a/BepuPhysics/SequentialFallbackBatch.cs +++ b/BepuPhysics/SequentialFallbackBatch.cs @@ -54,7 +54,7 @@ public struct SequentialFallbackBatch [MethodImpl(MethodImplOptions.AggressiveInlining)] unsafe void Allocate(Span dynamicBodyHandles, Bodies bodies, - BufferPool pool, TBodyReferenceGetter bodyReferenceGetter, int minimumBodyCapacity) + IUnmanagedMemoryPool pool, TBodyReferenceGetter bodyReferenceGetter, int minimumBodyCapacity) where TBodyReferenceGetter : struct, IBodyReferenceGetter { EnsureCapacity(Math.Max(dynamicBodyConstraintCounts.Count + dynamicBodyHandles.Length, minimumBodyCapacity), pool); @@ -84,7 +84,7 @@ internal unsafe void AllocateForActive(Span dynamicBodyHandles, Bodi [MethodImpl(MethodImplOptions.AggressiveInlining)] internal void AllocateForInactive(Span dynamicBodyHandles, Bodies bodies, - BufferPool pool, int minimumBodyCapacity = 8) + IUnmanagedMemoryPool pool, int minimumBodyCapacity = 8) { Allocate(dynamicBodyHandles, bodies, pool, new InactiveSetGetter(), minimumBodyCapacity); } @@ -239,7 +239,7 @@ internal void UpdateForBodyMemorySwap(int a, int b) Helpers.Swap(ref dynamicBodyConstraintCounts.Values[indexA], ref dynamicBodyConstraintCounts.Values[indexB]); } - internal static void CreateFrom(ref SequentialFallbackBatch sourceBatch, BufferPool pool, out SequentialFallbackBatch targetBatch) + internal static void CreateFrom(ref SequentialFallbackBatch sourceBatch, IUnmanagedMemoryPool pool, out SequentialFallbackBatch targetBatch) { //Copy over non-buffer state. This copies buffer references pointlessly, but that doesn't matter. targetBatch.dynamicBodyConstraintCounts = sourceBatch.dynamicBodyConstraintCounts; @@ -251,7 +251,7 @@ internal static void CreateFrom(ref SequentialFallbackBatch sourceBatch, BufferP sourceBatch.dynamicBodyConstraintCounts.Table.CopyTo(0, targetBatch.dynamicBodyConstraintCounts.Table, 0, sourceBatch.dynamicBodyConstraintCounts.TableMask + 1); } - internal void EnsureCapacity(int bodyCapacity, BufferPool pool) + internal void EnsureCapacity(int bodyCapacity, IUnmanagedMemoryPool pool) { if (dynamicBodyConstraintCounts.Keys.Allocated) { @@ -265,7 +265,7 @@ internal void EnsureCapacity(int bodyCapacity, BufferPool pool) } - public void Compact(BufferPool pool) + public void Compact(IUnmanagedMemoryPool pool) { if (dynamicBodyConstraintCounts.Keys.Allocated) { @@ -274,7 +274,7 @@ public void Compact(BufferPool pool) } - public void Dispose(BufferPool pool) + public void Dispose(IUnmanagedMemoryPool pool) { if (dynamicBodyConstraintCounts.Keys.Allocated) { diff --git a/BepuPhysics/Trees/RayBatcher.cs b/BepuPhysics/Trees/RayBatcher.cs index 4fd59086c..ff39f62f4 100644 --- a/BepuPhysics/Trees/RayBatcher.cs +++ b/BepuPhysics/Trees/RayBatcher.cs @@ -138,7 +138,7 @@ struct StackEntry } int stackPointer; Buffer stack; - BufferPool pool; + IUnmanagedMemoryPool pool; int batchRayCount; Buffer batchRays; Buffer batchOriginalRays; @@ -157,7 +157,7 @@ struct StackEntry /// Maximum number of rays to execute in each traversal. /// This should typically be chosen as the highest value which avoids spilling data out of L2 cache. /// Tree depth to preallocate ray stack space for. If a traversal finds nodes deeper than this, a dynamic resize will be triggered. - public RayBatcher(BufferPool pool, int rayCapacity = 2048, int treeDepthForPreallocation = 24) : this() + public RayBatcher(IUnmanagedMemoryPool pool, int rayCapacity = 2048, int treeDepthForPreallocation = 24) : this() { this.pool = pool; batchRayCount = 0; @@ -177,14 +177,14 @@ public RayBatcher(BufferPool pool, int rayCapacity = 2048, int treeDepthForPreal /// public void Dispose() { - pool.ReturnUnsafely(rayIndicesA0.Id); - pool.ReturnUnsafely(rayIndicesB.Id); - pool.ReturnUnsafely(rayIndicesA1.Id); - pool.ReturnUnsafely(stack.Id); - pool.ReturnUnsafely(fallbackStack.Id); - pool.ReturnUnsafely(batchOriginalRays.Id); - pool.ReturnUnsafely(batchRays.Id); + pool.Return(ref rayIndicesA0); + pool.Return(ref rayIndicesB); + pool.Return(ref rayIndicesA1); + pool.Return(ref stack); //Easier to catch bugs if the references get cleared. + pool.Return(ref fallbackStack); + pool.Return(ref batchOriginalRays); + pool.Return(ref batchRays); this = default; } diff --git a/BepuPhysics/Trees/Tree.cs b/BepuPhysics/Trees/Tree.cs index 620ef560b..dba3b9a31 100644 --- a/BepuPhysics/Trees/Tree.cs +++ b/BepuPhysics/Trees/Tree.cs @@ -83,7 +83,7 @@ public unsafe readonly void UpdateBounds(int leafIndex, Vector3 min, Vector3 max /// /// Buffer pool to use to allocate resources in the tree. /// Initial number of leaves to allocate room for. - public unsafe Tree(BufferPool pool, int initialLeafCapacity = 4096) : this() + public unsafe Tree(IUnmanagedMemoryPool pool, int initialLeafCapacity = 4096) : this() { if (initialLeafCapacity <= 0) throw new ArgumentException("Initial leaf capacity must be positive."); @@ -97,7 +97,7 @@ public unsafe Tree(BufferPool pool, int initialLeafCapacity = 4096) : this() /// /// Data to load into the tree. /// Pool to use to create the tree. - public Tree(Span data, BufferPool pool) + public Tree(Span data, IUnmanagedMemoryPool pool) { if (data.Length <= 4) throw new ArgumentException($"Data is only {data.Length} bytes long; that's too small for even a header."); @@ -171,7 +171,7 @@ void InitializeRoot() /// /// Pool from which to take and return resources. /// The desired number of available leaf slots. - public void Resize(BufferPool pool, int targetLeafSlotCount) + public void Resize(IUnmanagedMemoryPool pool, int targetLeafSlotCount) { //Note that it's not safe to resize below the size of potentially used leaves. If the user wants to go smaller, they'll need to explicitly deal with the leaves somehow first. var leafCapacityForTarget = BufferPool.GetCapacityForCount(Math.Max(LeafCount, targetLeafSlotCount)); @@ -217,7 +217,7 @@ public void Clear() /// /// Pool to return resources to. /// Disposed trees can be reused if EnsureCapacity or Resize is used to rehydrate them. - public void Dispose(BufferPool pool) + public void Dispose(IUnmanagedMemoryPool pool) { Debug.Assert(Nodes.Allocated == Leaves.Allocated && Nodes.Allocated == Metanodes.Allocated, "Nodes and leaves should have consistent lifetimes."); if (Nodes.Allocated) diff --git a/BepuPhysics/Trees/Tree_Add.cs b/BepuPhysics/Trees/Tree_Add.cs index ab78ea49d..92f1adbb5 100644 --- a/BepuPhysics/Trees/Tree_Add.cs +++ b/BepuPhysics/Trees/Tree_Add.cs @@ -109,7 +109,7 @@ private static unsafe BestInsertionChoice ComputeBestInsertionChoice(ref Boundin /// Extents of the leaf bounds. /// Resource pool to use if resizing is required. /// Index of the leaf allocated in the tree's leaf array. - public unsafe int Add(BoundingBox bounds, BufferPool pool) + public unsafe int Add(BoundingBox bounds, IUnmanagedMemoryPool pool) { //The rest of the function assumes we have sufficient room. We don't want to deal with invalidated pointers mid-add. if (Leaves.Length == LeafCount) diff --git a/BepuPhysics/Trees/Tree_BinnedRefine.cs b/BepuPhysics/Trees/Tree_BinnedRefine.cs index f2902ae71..a8fd5b2b4 100644 --- a/BepuPhysics/Trees/Tree_BinnedRefine.cs +++ b/BepuPhysics/Trees/Tree_BinnedRefine.cs @@ -80,7 +80,7 @@ partial struct Tree return toReturn; } - public static unsafe void CreateBinnedResources(BufferPool bufferPool, int maximumSubtreeCount, out Buffer buffer, out BinnedResources resources) + public static unsafe void CreateBinnedResources(IUnmanagedMemoryPool bufferPool, int maximumSubtreeCount, out Buffer buffer, out BinnedResources resources) { //TODO: This is a holdover from the pre-BufferPool tree design. It's pretty ugly. While some preallocation is useful (there's no reason to suffer the overhead of //pulling things out of the BufferPool over and over and over again), the degree to which this preallocates has a negative impact on L1 cache for subtree refines. @@ -578,7 +578,7 @@ unsafe void ReifyStagingNodes(int treeletRootIndex, Node* stagingNodes, public unsafe void BinnedRefine(int nodeIndex, ref QuickList subtreeReferences, int maximumSubtrees, ref QuickList treeletInternalNodes, - ref BinnedResources resources, BufferPool pool) + ref BinnedResources resources, IUnmanagedMemoryPool pool) { Debug.Assert(subtreeReferences.Count == 0, "The subtree references list should be empty since it's about to get filled."); Debug.Assert(subtreeReferences.Span.Length >= maximumSubtrees, "Subtree references list should have a backing array large enough to hold all possible subtrees."); diff --git a/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs b/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs index e4529c212..257d922ca 100644 --- a/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs +++ b/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs @@ -62,14 +62,14 @@ public unsafe void CreateRefitAndMarkJobs(ref Tree tree, BufferPool pool, IThrea //Note that we haven't rigorously guaranteed a refinement count maximum, so it's possible that the workers will need to resize the per-thread refinement candidate lists. for (int i = 0; i < threadDispatcher.ThreadCount; ++i) { - RefinementCandidates[i] = new QuickList(estimatedRefinementCandidateCount, threadDispatcher.GetThreadMemoryPool(i)); + RefinementCandidates[i] = new QuickList(estimatedRefinementCandidateCount, threadDispatcher.WorkerPools[i]); } int multithreadingLeafCountThreshold = Tree.LeafCount / (threadDispatcher.ThreadCount * 2); if (multithreadingLeafCountThreshold < RefinementLeafCountThreshold) multithreadingLeafCountThreshold = RefinementLeafCountThreshold; CollectNodesForMultithreadedRefit(0, multithreadingLeafCountThreshold, ref RefitNodes, RefinementLeafCountThreshold, ref RefinementCandidates[0], - pool, threadDispatcher.GetThreadMemoryPool(0)); + pool, threadDispatcher.WorkerPools[0]); RefitNodeIndex = -1; } @@ -143,7 +143,7 @@ public unsafe void CleanUpForRefitAndRefine(BufferPool pool) for (int i = 0; i < threadDispatcher.ThreadCount; ++i) { //Note the use of the thread memory pool. Each thread allocated their own memory for the list since resizes were possible. - RefinementCandidates[i].Dispose(threadDispatcher.GetThreadMemoryPool(i)); + RefinementCandidates[i].Dispose(threadDispatcher.WorkerPools[i]); } pool.Return(ref RefinementCandidates); RefitNodes.Dispose(pool); @@ -164,7 +164,7 @@ public unsafe void RefitAndRefine(ref Tree tree, BufferPool pool, IThreadDispatc unsafe void CollectNodesForMultithreadedRefit(int nodeIndex, int multithreadingLeafCountThreshold, ref QuickList refitAndMarkTargets, - int refinementLeafCountThreshold, ref QuickList refinementCandidates, BufferPool pool, BufferPool threadPool) + int refinementLeafCountThreshold, ref QuickList refinementCandidates, BufferPool pool, IUnmanagedMemoryPool threadPool) { ref var node = ref Tree.Nodes[nodeIndex]; ref var metanode = ref Tree.Metanodes[nodeIndex]; @@ -201,7 +201,7 @@ unsafe void CollectNodesForMultithreadedRefit(int nodeIndex, } } - public unsafe void ExecuteRefitAndMarkJob(BufferPool threadPool, int workerIndex, int refitIndex) + public unsafe void ExecuteRefitAndMarkJob(IUnmanagedMemoryPool threadPool, int workerIndex, int refitIndex) { var nodeIndex = RefitNodes[refitIndex]; bool shouldUseMark; @@ -316,7 +316,7 @@ public unsafe void RefitAndMarkForWorker(int workerIndex, void* context) return; //Since resizes may occur, we have to use the thread's buffer pool. //The main thread already created the refinement candidate list using the worker's pool. - var threadPool = threadDispatcher.GetThreadMemoryPool(workerIndex); + var threadPool = threadDispatcher.WorkerPools[workerIndex]; int refitIndex; Debug.Assert(Tree.LeafCount > 2); while ((refitIndex = Interlocked.Increment(ref RefitNodeIndex)) < RefitNodes.Count) @@ -327,7 +327,7 @@ public unsafe void RefitAndMarkForWorker(int workerIndex, void* context) } - public unsafe void ExecuteRefineJob(ref QuickList subtreeReferences, ref QuickList treeletInternalNodes, ref BinnedResources resources, BufferPool threadPool, int refineIndex) + public unsafe void ExecuteRefineJob(ref QuickList subtreeReferences, ref QuickList treeletInternalNodes, ref BinnedResources resources, IUnmanagedMemoryPool threadPool, int refineIndex) { Tree.BinnedRefine(RefinementTargets[refineIndex], ref subtreeReferences, MaximumSubtrees, ref treeletInternalNodes, ref resources, threadPool); subtreeReferences.Count = 0; @@ -338,7 +338,7 @@ public unsafe void RefineForWorker(int workerIndex, void* context) { if (RefinementTargets.Count == 0) return; - var threadPool = threadDispatcher.GetThreadMemoryPool(workerIndex); + var threadPool = threadDispatcher.WorkerPools[workerIndex]; var subtreeCountEstimate = (int)BitOperations.RoundUpToPowerOf2((uint)MaximumSubtrees); var subtreeReferences = new QuickList(subtreeCountEstimate, threadPool); var treeletInternalNodes = new QuickList(subtreeCountEstimate, threadPool); diff --git a/BepuPhysics/Trees/Tree_RefinementScheduling.cs b/BepuPhysics/Trees/Tree_RefinementScheduling.cs index 85f2fc696..751abb14c 100644 --- a/BepuPhysics/Trees/Tree_RefinementScheduling.cs +++ b/BepuPhysics/Trees/Tree_RefinementScheduling.cs @@ -37,7 +37,7 @@ unsafe float RefitAndMeasure(ref NodeChild child) } - unsafe float RefitAndMark(ref NodeChild child, int leafCountThreshold, ref QuickList refinementCandidates, BufferPool pool) + unsafe float RefitAndMark(ref NodeChild child, int leafCountThreshold, ref QuickList refinementCandidates, IUnmanagedMemoryPool pool) { Debug.Assert(leafCountThreshold > 1); @@ -87,7 +87,7 @@ unsafe float RefitAndMark(ref NodeChild child, int leafCountThreshold, ref Quick } - unsafe float RefitAndMark(int leafCountThreshold, ref QuickList refinementCandidates, BufferPool pool) + unsafe float RefitAndMark(int leafCountThreshold, ref QuickList refinementCandidates, IUnmanagedMemoryPool pool) { Debug.Assert(LeafCount > 2, "There's no reason to refit a tree with 2 or less elements. Nothing would happen."); @@ -178,7 +178,7 @@ readonly void GetRefineTuning(int frameIndex, int refinementCandidatesCount, flo targetRefinementCount = Math.Min(refinementCandidatesCount, (int)targetRefinementScale); } - public unsafe void RefitAndRefine(BufferPool pool, int frameIndex, float refineAggressivenessScale = 1) + public unsafe void RefitAndRefine(IUnmanagedMemoryPool pool, int frameIndex, float refineAggressivenessScale = 1) { //Don't proceed if the tree has no refitting or refinement required. This also guarantees that any nodes that do exist have two children. if (LeafCount <= 2) diff --git a/BepuPhysics/Trees/Tree_SweepBuilder.cs b/BepuPhysics/Trees/Tree_SweepBuilder.cs index 64f69d3b0..b7c67df64 100644 --- a/BepuPhysics/Trees/Tree_SweepBuilder.cs +++ b/BepuPhysics/Trees/Tree_SweepBuilder.cs @@ -245,7 +245,7 @@ unsafe int CreateSweepBuilderNode(int parentIndex, int indexInParent, } - public unsafe void SweepBuild(BufferPool pool, Buffer leafBounds) + public unsafe void SweepBuild(IUnmanagedMemoryPool pool, Buffer leafBounds) { if (leafBounds.Length <= 0) throw new ArgumentException("Length must be positive."); @@ -302,13 +302,13 @@ public unsafe void SweepBuild(BufferPool pool, Buffer leafBounds) //Return resources. - pool.ReturnUnsafely(centroidsX.Id); - pool.ReturnUnsafely(centroidsY.Id); - pool.ReturnUnsafely(centroidsZ.Id); - pool.ReturnUnsafely(indexMap.Id); - pool.ReturnUnsafely(indexMapX.Id); - pool.ReturnUnsafely(indexMapY.Id); - pool.ReturnUnsafely(indexMapZ.Id); + pool.Return(ref centroidsX); + pool.Return(ref centroidsY); + pool.Return(ref centroidsZ); + pool.Return(ref indexMap); + pool.Return(ref indexMapX); + pool.Return(ref indexMapY); + pool.Return(ref indexMapZ); pool.Return(ref merged); } diff --git a/BepuUtilities/Collections/IndexSet.cs b/BepuUtilities/Collections/IndexSet.cs index e272d1582..391e440a5 100644 --- a/BepuUtilities/Collections/IndexSet.cs +++ b/BepuUtilities/Collections/IndexSet.cs @@ -31,7 +31,7 @@ public static int GetBundleCapacity(int count) return (count + mask) >> shift; } - public IndexSet(BufferPool pool, int initialCapacity) + public IndexSet(IUnmanagedMemoryPool pool, int initialCapacity) { //Remember; the bundles are 64 flags wide. A default of 128 supports up to 8192 indices without needing resizing... Flags = new Buffer(); @@ -39,13 +39,13 @@ public IndexSet(BufferPool pool, int initialCapacity) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - void InternalResize(BufferPool pool, int capacity) + void InternalResize(IUnmanagedMemoryPool pool, int capacity) { InternalResizeForBundleCount(pool, GetBundleCapacity(capacity)); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - void InternalResizeForBundleCount(BufferPool pool, int bundleCapacity) + void InternalResizeForBundleCount(IUnmanagedMemoryPool pool, int bundleCapacity) { var copyRegionLength = Math.Min(bundleCapacity, Flags.Length); pool.ResizeToAtLeast(ref Flags, bundleCapacity, copyRegionLength); @@ -107,7 +107,7 @@ public void SetUnsafely(int index) /// This is functionally identical to the Add method, but it doesn't include the same debug assertions. Just a way to make intent clear so that the assert can catch errors. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Set(int index, BufferPool pool) + public void Set(int index, IUnmanagedMemoryPool pool) { var bundleIndex = index >> shift; if (bundleIndex >= Flags.Length) @@ -148,7 +148,7 @@ public void AddUnsafely(int index) /// Index to add. /// Pool to use to resize the set if necessary. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Add(int index, BufferPool pool) + public void Add(int index, IUnmanagedMemoryPool pool) { var bundleIndex = index >> shift; if (bundleIndex >= Flags.Length) @@ -173,7 +173,7 @@ public void Clear() Flags.Clear(0, Flags.Length); } - public void EnsureCapacity(int indexCapacity, BufferPool pool) + public void EnsureCapacity(int indexCapacity, IUnmanagedMemoryPool pool) { if ((Flags.Length << shift) < indexCapacity) { @@ -182,7 +182,7 @@ public void EnsureCapacity(int indexCapacity, BufferPool pool) } //While we expose a compaction and resize, using it requires care. It would be a mistake to, for example, shrink beyond the current bodies indices size. - public void Compact(int indexCapacity, BufferPool pool) + public void Compact(int indexCapacity, IUnmanagedMemoryPool pool) { var desiredBundleCount = BufferPool.GetCapacityForCount(GetBundleCapacity(indexCapacity)); if (Flags.Length > desiredBundleCount) @@ -190,7 +190,7 @@ public void Compact(int indexCapacity, BufferPool pool) InternalResizeForBundleCount(pool, desiredBundleCount); } } - public void Resize(int indexCapacity, BufferPool pool) + public void Resize(int indexCapacity, IUnmanagedMemoryPool pool) { var desiredBundleCount = BufferPool.GetCapacityForCount(GetBundleCapacity(indexCapacity)); if (Flags.Length != desiredBundleCount) @@ -203,7 +203,7 @@ public void Resize(int indexCapacity, BufferPool pool) /// /// The instance can be reused after a Dispose if EnsureCapacity or Resize is called. /// That's a little meaningless given that the instance is a value type, but hey, you don't have to new another one, that's something. - public void Dispose(BufferPool pool) + public void Dispose(IUnmanagedMemoryPool pool) { Debug.Assert(Flags.Length > 0, "Cannot double-dispose."); pool.Return(ref Flags); diff --git a/BepuUtilities/IThreadDispatcher.cs b/BepuUtilities/IThreadDispatcher.cs index 5a4b30a8f..2bf4ffbaa 100644 --- a/BepuUtilities/IThreadDispatcher.cs +++ b/BepuUtilities/IThreadDispatcher.cs @@ -60,13 +60,15 @@ void DispatchWorkers(ThreadDispatcherWorker workerBody, int maximumWorkerCount = } /// - /// Gets the memory pool associated with a given worker index. It is guaranteed that no other workers will share the same pool for the duration of the worker's execution. + /// Gets the set of memory pools associated with thread workers. /// - /// All usages of the memory pool within the simulation are guaranteed to return thread pool memory before the function returns. In other words, + /// All usages of these worker pools within the simulation are guaranteed to return thread pool memory before the function returns. In other words, /// thread memory pools are used for strictly ephemeral memory, and it will never be held by the simulation outside the scope of a function that - /// takes the IThreadDispatcher as input. - /// Index of the worker to grab the pool for. - /// The memory pool for the specified worker index. - BufferPool GetThreadMemoryPool(int workerIndex); + /// takes the IThreadDispatcher as input. + /// + /// Further, the simulation will often flush these pools. They should not be relied upon to store long term data; + /// consider creating another instance, using other instances, or using other instances. + /// The set of memory pools associated with the dispatcher's thread workers. + WorkerBufferPools WorkerPools { get; } } } diff --git a/BepuUtilities/Memory/ArenaPool.cs b/BepuUtilities/Memory/ArenaPool.cs index 6969191b3..d2239a09a 100644 --- a/BepuUtilities/Memory/ArenaPool.cs +++ b/BepuUtilities/Memory/ArenaPool.cs @@ -2,6 +2,7 @@ using System; using System.Collections.Generic; using System.Diagnostics; +using System.Numerics; using System.Runtime.CompilerServices; namespace BepuUtilities.Memory; @@ -9,6 +10,8 @@ namespace BepuUtilities.Memory; /// /// Arena allocator built to serve a single thread. Pulls resources from a central buffer pool when necessary. /// +/// Returns to this pool are not guaranteed to free memory because it does not carry enough information about allocations to do so. +/// To free up memory after use, the arena pool as a whole must be cleared using . public class ArenaPool : IUnmanagedMemoryPool, IDisposable { /// @@ -38,6 +41,7 @@ public Block(Buffer data) public bool TryAllocate(int sizeInBytes, out Buffer allocation) { //Following the pattern set by the bufferpool, use beefy alignment: + //TODO: This is somewhat dumb and we should change it! var startLocation = (Count + 127) & (~127); var newCount = startLocation + sizeInBytes; if (Data.Length >= newCount) @@ -63,6 +67,7 @@ public bool TryAllocate(int sizeInBytes, out Buffer allocation) /// Creates a new arena thread pool. /// /// Central pool to allocate blocks from + /// Number of bytes to allocate for a single block if an allocation request does not need more. /// Locker object used to protect accesses to the central pool. If no locker is specified, the pool reference is used. public ArenaPool(IUnmanagedMemoryPool pool, int defaultBlockCapacity = 16384, object locker = null) { @@ -149,7 +154,7 @@ public unsafe void TakeAtLeast(int count, out Buffer buffer) where T : unm if (blocks.Count == 0 || !blocks[blockIndex].TryAllocate(sizeInBytes, out Buffer allocation)) { //No room; need a new block. - var newBlockCapacityInBytes = Math.Max(DefaultBlockCapacity, sizeInBytes); + var newBlockCapacityInBytes = int.Max(DefaultBlockCapacity, sizeInBytes); //Check to see if there's already a block allocated that we can use. if (blocks.Span.Length > blocks.Count && blocks.Span[blocks.Count].Data.Length >= sizeInBytes) { @@ -160,13 +165,23 @@ public unsafe void TakeAtLeast(int count, out Buffer buffer) where T : unm { //Need a new block. Buffer blockData; + bool resizedBlockList = false; lock (Locker) { Pool.Take(newBlockCapacityInBytes, out blockData); if (blocks.Span.Length == blocks.Count) + { blocks.EnsureCapacity(blocks.Count * 2, Pool); + resizedBlockList = true; + } + } + if (resizedBlockList) + { + //We check for allocated blocks before allocating one, so we need to clear the memory up front. + blocks.Span.Clear(blocks.Count, blocks.Span.length - blocks.Count); } blocks.AllocateUnsafely() = new Block(blockData); + //Console.WriteLine($"allocated a new block for {sizeInBytes} size: {blocks.Count}"); } blockIndex = blocks.Count - 1; var succeeded = blocks[blockIndex].TryAllocate(sizeInBytes, out allocation); @@ -211,22 +226,23 @@ public void Take(int count, out Buffer buffer) where T : unmanaged TakeAtLeast(count, out buffer); } + /// - /// Unlike a , the will not generally free up space in response to calls to . + /// Unlike a , the will not generally free up space in response to calls to . /// If the deallocated buffer is the last allocated buffer for a given block, the pool may choose to bump the allocation pointer back, but it is not guaranteed. - public void Return(ref Buffer buffer) where T : unmanaged + public void ReturnUnsafely(int id) { - if (buffer.Id >= 0) + if (id >= 0) { //This was a representable id. #if DEBUG - Debug.Assert(outstandingIds.Remove(buffer.Id), + Debug.Assert(outstandingIds.Remove(id), "This buffer id must have been taken from the pool previously."); #if LEAKDEBUG bool found = false; foreach (var pair in outstandingAllocators) { - if (pair.Value.Remove(buffer.Id)) + if (pair.Value.Remove(id)) { found = true; if (pair.Value.Count == 0) @@ -240,9 +256,10 @@ public void Return(ref Buffer buffer) where T : unmanaged #endif #endif //Was it the most recently allocated buffer (in that block) such that we can pop it off like a stack? - var blockIndex = (buffer.Id & blockMask) >> maximumBitsForIndices; - var countInBlock = buffer.Id & countInBlockMask; - var previousCountInBlock = (buffer.Id & countInBlockMask) >> maximumBitsForIndex; + var blockIndex = (id & blockMask) >> maximumBitsForIndices; + Debug.Assert(blockIndex < blocks.Count, "Invalid id; the encoded block index doesn't fit in this pool."); + var countInBlock = id & countInBlockMask; + var previousCountInBlock = (id >> maximumBitsForIndex) & countInBlockMask; ref var block = ref blocks[blockIndex]; if (block.Count == countInBlock) { @@ -252,9 +269,18 @@ public void Return(ref Buffer buffer) where T : unmanaged { //Push the block count as far as it can go. --blocks.Count; + //Console.WriteLine($"destroyed a block: {blocks.Count}"); } } } + } + + /// + /// Unlike a , the will not generally free up space in response to calls to . + /// If the deallocated buffer is the last allocated buffer for a given block, the pool may choose to bump the allocation pointer back, but it is not guaranteed. + public void Return(ref Buffer buffer) where T : unmanaged + { + ReturnUnsafely(buffer.Id); buffer = default; } @@ -264,4 +290,34 @@ public int GetCapacityForCount(int count) where T : unmanaged return count; } + /// + public void ResizeToAtLeast(ref Buffer buffer, int targetSize, int copyCount) where T : unmanaged + { + //Only do anything if the new size is actually different from the current size. + Debug.Assert(copyCount <= targetSize && copyCount <= buffer.Length, "Can't copy more elements than exist in the source or target buffers."); + targetSize = GetCapacityForCount(targetSize); + if (!buffer.Allocated) + { + Debug.Assert(buffer.Length == 0, "If a buffer is pointing at null, then it should be default initialized and have a length of zero too."); + //This buffer is not allocated; just return a new one. No copying to be done. + TakeAtLeast(targetSize, out buffer); + } + else + { + //Unlike the BufferPool, we can't rely on the Buffer id to tell us if there was spare room beyond the buffer, so we can't incrementally resize. We have to reallocate. + //(Alignment could be different per allocation, too, so we'd need to know something about the *next* allocation to know whether we can resize.) + TakeAtLeast(targetSize, out Buffer newBuffer); + buffer.CopyTo(0, newBuffer, 0, copyCount); + ReturnUnsafely(buffer.Id); + buffer = newBuffer; + } + } + + /// + public void Resize(ref Buffer buffer, int targetSize, int copyCount) where T : unmanaged => ResizeToAtLeast(ref buffer, targetSize, copyCount); + + + + + } diff --git a/BepuUtilities/Memory/BufferPool.cs b/BepuUtilities/Memory/BufferPool.cs index 68ae77474..fa5bec10f 100644 --- a/BepuUtilities/Memory/BufferPool.cs +++ b/BepuUtilities/Memory/BufferPool.cs @@ -36,6 +36,11 @@ unsafe struct PowerPool public int BlockCount; internal const int IdPowerShift = 26; + /// + /// Byte alignment to enforce for all block allocations within the buffer pool. + /// + /// Since this only applies at the level of blocks, we can use a pretty beefy value without much concern. + public const int BlockAlignment = 128; public PowerPool(int power, int minimumBlockSize, int expectedPooledCount) { @@ -76,8 +81,7 @@ void AllocateBlock(int blockIndex) //Suballocations from the block will always occur on pow2 boundaries, so the only way for a suballocation to violate this alignment is if an individual //suballocation is smaller than the alignment- in which case it doesn't require the alignment to be that wide. Also, since the alignment and //suballocations are both pow2 sized, they won't drift out of sync. - //We pick 128 bytes to allow alignment with cache line pairs: https://www.intel.com/content/dam/www/public/us/en/documents/manuals/64-ia-32-architectures-optimization-manual.pdf#page=162 - Blocks[blockIndex] = (byte*)NativeMemory.AlignedAlloc((nuint)BlockSize, 128); + Blocks[blockIndex] = (byte*)NativeMemory.AlignedAlloc((nuint)BlockSize, BlockAlignment); BlockCount = blockIndex + 1; } @@ -314,13 +318,7 @@ internal static void DecomposeId(int bufferId, out int powerIndex, out int slotI slotIndex = bufferId & ((1 << PowerPool.IdPowerShift) - 1); } - /// - /// Returns a buffer to the pool by id. - /// - /// Id of the buffer to return to the pool. - /// Typed buffer pools zero out the passed-in buffer by convention. - /// This costs very little and avoids a wide variety of bugs (either directly or by forcing fast failure). For consistency, BufferPool.Return does the same thing. - /// This "Unsafe" overload should be used only in cases where there's a reason to bypass the clear; the naming is intended to dissuade casual use. + /// [MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe void ReturnUnsafely(int id) { @@ -328,10 +326,7 @@ public unsafe void ReturnUnsafely(int id) pools[powerIndex].Return(slotIndex); } - /// - /// Returns a buffer to the pool. - /// - /// Buffer to return to the pool. + /// [MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe void Return(ref Buffer buffer) where T : unmanaged { @@ -343,15 +338,7 @@ public unsafe void Return(ref Buffer buffer) where T : unmanaged buffer = default; } - /// - /// Resizes a typed buffer to the smallest size available in the pool which contains the target size. Copies a subset of elements into the new buffer. - /// Final buffer size is at least as large as the target size and may be larger. - /// - /// Type of the buffer to resize. - /// Buffer reference to resize. - /// Number of elements to resize the buffer for. - /// Number of elements to copy into the new buffer from the old buffer. Contents of slots outside the copied range in the resized buffer are undefined. - [MethodImpl(MethodImplOptions.AggressiveInlining)] + /// public void ResizeToAtLeast(ref Buffer buffer, int targetSize, int copyCount) where T : unmanaged { //Only do anything if the new size is actually different from the current size. @@ -386,21 +373,16 @@ public void ResizeToAtLeast(ref Buffer buffer, int targetSize, int copyCou } } - - /// - /// Resizes a buffer to the target size. Copies a subset of elements into the new buffer. - /// - /// Type of the buffer to resize. - /// Buffer reference to resize. - /// Number of elements to resize the buffer for. - /// Number of elements to copy into the new buffer from the old buffer. - [MethodImpl(MethodImplOptions.AggressiveInlining)] + /// public void Resize(ref Buffer buffer, int targetSize, int copyCount) where T : unmanaged { ResizeToAtLeast(ref buffer, targetSize, copyCount); buffer.length = targetSize; } + /// + /// Issues debug assertions that all pools are empty. + /// [Conditional("DEBUG")] public void AssertEmpty() { diff --git a/BepuUtilities/Memory/IUnmanagedMemoryPool.cs b/BepuUtilities/Memory/IUnmanagedMemoryPool.cs index 28543700f..354feb1e5 100644 --- a/BepuUtilities/Memory/IUnmanagedMemoryPool.cs +++ b/BepuUtilities/Memory/IUnmanagedMemoryPool.cs @@ -1,4 +1,6 @@ -namespace BepuUtilities.Memory +using System.Runtime.CompilerServices; + +namespace BepuUtilities.Memory { /// /// Defines a type that is capable of rapidly serving requests for allocation and deallocation of unmanaged memory. @@ -23,7 +25,7 @@ public interface IUnmanagedMemoryPool /// Returns a buffer to the pool. /// /// Type of the buffer's elements. - /// Buffer to return to the pool. + /// Buffer to return to the pool. The reference will be cleared. void Return(ref Buffer buffer) where T : unmanaged; /// /// Gets the capacity of a buffer that would be returned by the pool if a given element count was requested from TakeAtLeast. @@ -32,5 +34,33 @@ public interface IUnmanagedMemoryPool /// Number of elements to request. /// Capacity of a buffer that would be returned if the given element count was requested. int GetCapacityForCount(int count) where T : unmanaged; + + /// + /// Returns a buffer to the pool by id. + /// + /// Id of the buffer to return to the pool. + /// Pools zero out the passed-in buffer by convention. This costs very little and avoids a wide variety of bugs (either directly or by forcing fast failure). + /// This "Unsafe" overload should be used only in cases where there's a reason to bypass the clear; the naming is intended to dissuade casual use. + void ReturnUnsafely(int id); + + /// + /// Resizes a typed buffer to the smallest size available in the pool which contains the target size. Copies a subset of elements into the new buffer. + /// Final buffer size is at least as large as the target size and may be larger. + /// + /// Type of the buffer to resize. + /// Buffer reference to resize. + /// Number of elements to resize the buffer for. + /// Number of elements to copy into the new buffer from the old buffer. Contents of slots outside the copied range in the resized buffer are undefined. + void ResizeToAtLeast(ref Buffer buffer, int targetSize, int copyCount) where T : unmanaged; + + /// + /// Resizes a buffer to the target size. Copies a subset of elements into the new buffer. + /// + /// Type of the buffer to resize. + /// Buffer reference to resize. + /// Number of elements to resize the buffer for. + /// Number of elements to copy into the new buffer from the old buffer. + void Resize(ref Buffer buffer, int targetSize, int copyCount) where T : unmanaged; + } } \ No newline at end of file diff --git a/BepuUtilities/Memory/WorkerBufferPools.cs b/BepuUtilities/Memory/WorkerBufferPools.cs index f58f4d672..54d16e118 100644 --- a/BepuUtilities/Memory/WorkerBufferPools.cs +++ b/BepuUtilities/Memory/WorkerBufferPools.cs @@ -4,14 +4,17 @@ namespace BepuUtilities.Memory; /// /// Collection of arena pools used by worker threads. -/// +/// +/// Returns to an are not guaranteed to free memory because it does not carry enough information about allocations to do so. +/// To free up memory after use, the arena pool as a whole must be cleared using . +/// Use to clear all pools together. public class WorkerBufferPools : IDisposable { ArenaPool[] pools; /// /// Central pool from which subpools allocate. /// - public BufferPool Pool { get; private set; } + public IUnmanagedMemoryPool Pool { get; private set; } /// /// Locker used by subpools to control access to the central pool. /// @@ -36,7 +39,7 @@ public class WorkerBufferPools : IDisposable /// Central pool from which worker pools allocate from. /// Initial number of workers to allocate space for. /// Default block capacity in thread pools. - public WorkerBufferPools(BufferPool pool, int initialWorkerCount, int defaultBlockCapacity = 16384) + public WorkerBufferPools(IUnmanagedMemoryPool pool, int initialWorkerCount, int defaultBlockCapacity = 16384) { Pool = pool; Locker = new object(); @@ -90,7 +93,6 @@ public void Clear() } } - /// /// Disposes all worker arena pools. Pools cannot be used after being disposed. /// diff --git a/BepuUtilities/ThreadDispatcher.cs b/BepuUtilities/ThreadDispatcher.cs index 73bdae073..2556a4162 100644 --- a/BepuUtilities/ThreadDispatcher.cs +++ b/BepuUtilities/ThreadDispatcher.cs @@ -25,14 +25,16 @@ struct Worker Worker[] workers; AutoResetEvent finished; - BufferPool[] bufferPools; + /// + public WorkerBufferPools WorkerPools { get; private set; } /// /// Creates a new thread dispatcher with the given number of threads. /// /// Number of threads to dispatch on each invocation. + /// Pool to allocate blocks from for use in per-thread arena pools. /// Size of memory blocks to allocate for thread pools. - public ThreadDispatcher(int threadCount, int threadPoolBlockAllocationSize = 16384) + public ThreadDispatcher(int threadCount, IUnmanagedMemoryPool pool, int threadPoolBlockAllocationSize = 16384) { this.threadCount = threadCount; workers = new Worker[threadCount - 1]; @@ -43,11 +45,7 @@ public ThreadDispatcher(int threadCount, int threadPoolBlockAllocationSize = 163 workers[i].Thread.Start((workers[i].Signal, i + 1)); } finished = new AutoResetEvent(false); - bufferPools = new BufferPool[threadCount]; - for (int i = 0; i < bufferPools.Length; ++i) - { - bufferPools[i] = new BufferPool(threadPoolBlockAllocationSize); - } + WorkerPools = new WorkerBufferPools(pool, threadCount, threadPoolBlockAllocationSize); } void DispatchThread(int workerIndex) @@ -94,6 +92,7 @@ void SignalThreads(int maximumWorkerCount) } } + /// public void DispatchWorkers(delegate* workerBody, void* context = null, int maximumWorkerCount = int.MaxValue) { if (maximumWorkerCount > 1) @@ -114,6 +113,7 @@ public void DispatchWorkers(delegate* workerBody, void* contex } } + /// public void DispatchWorkers(ThreadDispatcherWorker workerBody, void* context = null, int maximumWorkerCount = int.MaxValue) { if (maximumWorkerCount > 1) @@ -145,10 +145,7 @@ public void Dispose() { disposed = true; SignalThreads(threadCount); - for (int i = 0; i < bufferPools.Length; ++i) - { - bufferPools[i].Clear(); - } + WorkerPools.Dispose(); foreach (var worker in workers) { worker.Thread.Join(); @@ -157,10 +154,6 @@ public void Dispose() } } - public BufferPool GetThreadMemoryPool(int workerIndex) - { - return bufferPools[workerIndex]; - } } } diff --git a/DemoRenderer/ShapeDrawing/ShapesExtractor.cs b/DemoRenderer/ShapeDrawing/ShapesExtractor.cs index b33962fdb..e9d307c27 100644 --- a/DemoRenderer/ShapeDrawing/ShapesExtractor.cs +++ b/DemoRenderer/ShapeDrawing/ShapesExtractor.cs @@ -20,7 +20,7 @@ public struct ShapeCache internal QuickList Triangles; internal QuickList Meshes; - public ShapeCache(int initialCapacityPerShapeType, BufferPool pool) + public ShapeCache(int initialCapacityPerShapeType, IUnmanagedMemoryPool pool) { Spheres = new QuickList(initialCapacityPerShapeType, pool); Capsules = new QuickList(initialCapacityPerShapeType, pool); @@ -38,7 +38,7 @@ public void Clear() Triangles.Count = 0; Meshes.Count = 0; } - public void Dispose(BufferPool pool) + public void Dispose(IUnmanagedMemoryPool pool) { Spheres.Dispose(pool); Capsules.Dispose(pool); @@ -74,7 +74,7 @@ public void ClearInstances() ShapeCache.Clear(); } - private unsafe void AddCompoundChildren(ref Buffer children, Shapes shapes, RigidPose pose, Vector3 color, ref ShapeCache shapeCache, BufferPool pool) + private unsafe void AddCompoundChildren(ref Buffer children, Shapes shapes, RigidPose pose, Vector3 color, ref ShapeCache shapeCache, IUnmanagedMemoryPool pool) { for (int i = 0; i < children.Length; ++i) { @@ -85,7 +85,7 @@ private unsafe void AddCompoundChildren(ref Buffer children, Shap } [MethodImpl(MethodImplOptions.AggressiveInlining)] - unsafe void AddShape(void* shapeData, int shapeType, Shapes shapes, RigidPose pose, Vector3 color, ref ShapeCache shapeCache, BufferPool pool) + unsafe void AddShape(void* shapeData, int shapeType, Shapes shapes, RigidPose pose, Vector3 color, ref ShapeCache shapeCache, IUnmanagedMemoryPool pool) { //TODO: This should likely be swapped over to a registration-based virtualized table approach to more easily support custom shape extractors- //generic terrain windows and examples like voxel grids would benefit. @@ -259,7 +259,7 @@ public unsafe void AddShape(void* shapeData, int shapeType, Shapes shapes, Rigid } [MethodImpl(MethodImplOptions.AggressiveInlining)] - unsafe void AddShape(Shapes shapes, TypedIndex shapeIndex, RigidPose pose, Vector3 color, ref ShapeCache shapeCache, BufferPool pool) + unsafe void AddShape(Shapes shapes, TypedIndex shapeIndex, RigidPose pose, Vector3 color, ref ShapeCache shapeCache, IUnmanagedMemoryPool pool) { if (shapeIndex.Exists) { @@ -284,7 +284,7 @@ public unsafe void AddShape(TShape shape, Shapes shapes, RigidPose pose, } [MethodImpl(MethodImplOptions.AggressiveInlining)] - void AddBodyShape(Shapes shapes, Bodies bodies, int setIndex, int indexInSet, ref ShapeCache shapeCache, BufferPool pool) + void AddBodyShape(Shapes shapes, Bodies bodies, int setIndex, int indexInSet, ref ShapeCache shapeCache, IUnmanagedMemoryPool pool) { ref var set = ref bodies.Sets[setIndex]; var handle = set.IndexToHandle[indexInSet]; @@ -328,7 +328,7 @@ void AddBodyShape(Shapes shapes, Bodies bodies, int setIndex, int indexInSet, re } [MethodImpl(MethodImplOptions.AggressiveInlining)] - void AddStaticShape(Shapes shapes, Statics statics, int index, ref ShapeCache shapeCache, BufferPool pool) + void AddStaticShape(Shapes shapes, Statics statics, int index, ref ShapeCache shapeCache, IUnmanagedMemoryPool pool) { var handle = statics.IndexToHandle[index]; //Statics don't have any activity states. Just some simple variation on a central static color. @@ -361,7 +361,7 @@ void PrepareForMultithreadedExecution(IThreadDispatcher threadDispatcher) pool.Take(threadDispatcher.ThreadCount, out workerCaches); for (int i = 0; i < workerCaches.Length; ++i) { - workerCaches[i] = new ShapeCache(128, threadDispatcher.GetThreadMemoryPool(i)); + workerCaches[i] = new ShapeCache(128, threadDispatcher.WorkerPools[i]); } } @@ -370,8 +370,9 @@ void EndMultithreadedExecution() jobs.Dispose(pool); for (int i = 0; i < workerCaches.Length; ++i) { - workerCaches[i].Dispose(looper.Dispatcher.GetThreadMemoryPool(i)); + workerCaches[i].Dispose(looper.Dispatcher.WorkerPools[i]); } + looper.Dispatcher.WorkerPools.Clear(); looper.Dispatcher = null; pool.Return(ref workerCaches); } @@ -417,7 +418,7 @@ void AddShapesForJob(int jobIndex, int workerIndex) { var job = jobs[jobIndex]; var simulation = simulations == null ? this.simulation : this.simulations[job.SimulationIndex]; - var pool = looper.Dispatcher.GetThreadMemoryPool(workerIndex); + var pool = looper.Dispatcher.WorkerPools[workerIndex]; if (job.SetIndex >= 0) { diff --git a/Demos/Demo.cs b/Demos/Demo.cs index 34d90bbdf..d382a11d4 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -43,8 +43,8 @@ protected Demo() //there won't be enough memory bandwidth to even feed half the physical cores. Using all 128 logical cores would just add overhead. //It may be worth using something like hwloc or CPUID to extract extra information to reason about. - var targetThreadCount = 2;// Math.Max(1, Environment.ProcessorCount > 4 ? Environment.ProcessorCount - 2 : Environment.ProcessorCount - 1); - ThreadDispatcher = new ThreadDispatcher(targetThreadCount); + var targetThreadCount = Math.Max(1, Environment.ProcessorCount > 4 ? Environment.ProcessorCount - 2 : Environment.ProcessorCount - 1); + ThreadDispatcher = new ThreadDispatcher(targetThreadCount, BufferPool); } public virtual void LoadGraphicalContent(ContentArchive content, RenderSurface surface) diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index fed59c61f..323bc1af0 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -46,8 +46,8 @@ struct Option public DemoSet() { - AddOption(); - AddOption(); + //AddOption(); + //AddOption(); AddOption(); AddOption(); AddOption(); diff --git a/Demos/Demos/CustomVoxelCollidableDemo.cs b/Demos/Demos/CustomVoxelCollidableDemo.cs index 363eadcac..059ea66d1 100644 --- a/Demos/Demos/CustomVoxelCollidableDemo.cs +++ b/Demos/Demos/CustomVoxelCollidableDemo.cs @@ -49,7 +49,7 @@ struct Voxels : IHomogeneousCompoundShape public readonly int ChildCount => VoxelIndices.Count; - public Voxels(QuickList voxelIndices, Vector3 voxelSize, BufferPool pool) + public Voxels(QuickList voxelIndices, Vector3 voxelSize, IUnmanagedMemoryPool pool) { VoxelIndices = voxelIndices; VoxelSize = voxelSize; @@ -70,7 +70,7 @@ public Voxels(QuickList voxelIndices, Vector3 voxelSize, BufferPool poo pool.Return(ref bounds); } - public readonly ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches) + public readonly ShapeBatch CreateShapeBatch(IUnmanagedMemoryPool pool, int initialCapacity, Shapes shapeBatches) { //Shapes types are responsible for informing the shape system how to create a batch for them. //Convex shapes will return a ConvexShapeBatch, compound shapes a CompoundShapeBatch, @@ -206,7 +206,7 @@ public readonly void GetLocalChild(int childIndex, ref BoxWide shapeWide) [MethodImpl(MethodImplOptions.AggressiveInlining)] - public readonly unsafe void FindLocalOverlaps(ref Buffer pairs, BufferPool pool, Shapes shapes, ref TOverlaps overlaps) + public readonly unsafe void FindLocalOverlaps(ref Buffer pairs, IUnmanagedMemoryPool pool, Shapes shapes, ref TOverlaps overlaps) where TOverlaps : struct, ICollisionTaskOverlaps where TSubpairOverlaps : struct, ICollisionTaskSubpairOverlaps { @@ -226,7 +226,7 @@ public readonly unsafe void FindLocalOverlaps(ref B } } - public readonly unsafe void FindLocalOverlaps(Vector3 min, Vector3 max, Vector3 sweep, float maximumT, BufferPool pool, Shapes shapes, void* overlaps) where TOverlaps : ICollisionTaskSubpairOverlaps + public readonly unsafe void FindLocalOverlaps(Vector3 min, Vector3 max, Vector3 sweep, float maximumT, IUnmanagedMemoryPool pool, Shapes shapes, void* overlaps) where TOverlaps : ICollisionTaskSubpairOverlaps { //Similar to the non-swept FindLocalOverlaps function above, this just adds the overlaps to the provided collection. //Some unfortunate loss of type information due to some language limitations around generic pointers- pretend the overlaps pointer has type TOverlaps*. @@ -237,7 +237,7 @@ public readonly unsafe void FindLocalOverlaps(Vector3 min, Vector3 ma Tree.Sweep(min, max, sweep, maximumT, ref enumerator); } - public void Dispose(BufferPool pool) + public void Dispose(IUnmanagedMemoryPool pool) { Tree.Dispose(pool); VoxelIndices.Dispose(pool); diff --git a/Demos/Demos/RayCastingDemo.cs b/Demos/Demos/RayCastingDemo.cs index 8adcd3bfc..8ca864e9a 100644 --- a/Demos/Demos/RayCastingDemo.cs +++ b/Demos/Demos/RayCastingDemo.cs @@ -299,7 +299,7 @@ unsafe int BatchedWorker(int workerIndex, IntersectionAlgorithm algorithm) { int intersectionCount = 0; var hitHandler = new HitHandler { Hits = algorithm.Results, IntersectionCount = &intersectionCount }; - var batcher = new SimulationRayBatcher(ThreadDispatcher.GetThreadMemoryPool(workerIndex), Simulation, hitHandler, 2048); + var batcher = new SimulationRayBatcher(ThreadDispatcher.WorkerPools[workerIndex], Simulation, hitHandler, 2048); int claimedIndex; while ((claimedIndex = Interlocked.Increment(ref algorithm.JobIndex)) < jobs.Length) { diff --git a/Demos/Demos/SimpleSelfContainedDemo.cs b/Demos/Demos/SimpleSelfContainedDemo.cs index 9c11c1b68..fc0e0854d 100644 --- a/Demos/Demos/SimpleSelfContainedDemo.cs +++ b/Demos/Demos/SimpleSelfContainedDemo.cs @@ -224,7 +224,7 @@ public static void Run() simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), simulation.Shapes.Add(new Box(500, 1, 500)))); //Any IThreadDispatcher implementation can be used for multithreading. Here, we use the BepuUtilities.ThreadDispatcher implementation. - var threadDispatcher = new ThreadDispatcher(Environment.ProcessorCount); + var threadDispatcher = new ThreadDispatcher(Environment.ProcessorCount, bufferPool); //Now take 100 time steps! for (int i = 0; i < 100; ++i) diff --git a/Demos/SpecializedTests/IntertreeThreadingTests.cs b/Demos/SpecializedTests/IntertreeThreadingTests.cs index 0f999de04..4a7687407 100644 --- a/Demos/SpecializedTests/IntertreeThreadingTests.cs +++ b/Demos/SpecializedTests/IntertreeThreadingTests.cs @@ -181,7 +181,7 @@ public static void Test() { var random = new Random(5); var pool = new BufferPool(); - var threadDispatcher = new ThreadDispatcher(Environment.ProcessorCount); + var threadDispatcher = new ThreadDispatcher(Environment.ProcessorCount, pool); for (int i = 0; i < 1000; ++i) { TestTrees(pool, threadDispatcher, random); diff --git a/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs b/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs index 349d779b3..2d06fc5df 100644 --- a/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs +++ b/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs @@ -69,7 +69,7 @@ public override void Update(Window window, Camera camera, Input input, float dt) Simulation.Dispose(); BufferPool.Clear(); for (int i = 0; i < ThreadDispatcher.ThreadCount; ++i) - ThreadDispatcher.GetThreadMemoryPool(i).Clear(); + ThreadDispatcher.WorkerPools[i].Clear(); Initialize(null, camera); } base.Update(window, camera, input, dt); diff --git a/Demos/SpecializedTests/TreeTest.cs b/Demos/SpecializedTests/TreeTest.cs index ed1675d29..3d4c81a92 100644 --- a/Demos/SpecializedTests/TreeTest.cs +++ b/Demos/SpecializedTests/TreeTest.cs @@ -68,7 +68,7 @@ public static void Test() const int iterations = 100000; const int maximumChangesPerIteration = 20; - var threadDispatcher = new ThreadDispatcher(Environment.ProcessorCount); + var threadDispatcher = new ThreadDispatcher(Environment.ProcessorCount, pool); var refineContext = new Tree.RefitAndRefineMultithreadedContext(); var selfTestContext = new Tree.MultithreadedSelfTest(pool); var overlapHandlers = new OverlapHandler[threadDispatcher.ThreadCount]; From 0daf5db0b1f91404ef05b43030e8df7daf2f88ec Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 24 Nov 2022 20:17:34 -0600 Subject: [PATCH 657/947] Fixed bugs. Thread dispatcher must maintain its own buffer pool to avoid narrow phase flush stomping issues. Introduced some intermediate clears. --- BepuPhysics/BatchCompressor.cs | 10 ++--- BepuPhysics/CollisionDetection/BroadPhase.cs | 1 + BepuPhysics/IslandSleeper.cs | 4 +- BepuUtilities/Memory/ArenaPool.cs | 16 +++++--- BepuUtilities/Memory/BufferPool.cs | 14 ++++++- BepuUtilities/Memory/IUnmanagedMemoryPool.cs | 10 ++++- BepuUtilities/Memory/WorkerBufferPools.cs | 41 ++++++++++++------- BepuUtilities/ThreadDispatcher.cs | 8 ++-- Demos/Demo.cs | 4 +- Demos/DemoSet.cs | 1 + Demos/Demos/ContactEventsDemo.cs | 8 +++- Demos/Demos/SimpleSelfContainedDemo.cs | 2 +- .../IntertreeThreadingTests.cs | 2 +- Demos/SpecializedTests/TreeTest.cs | 2 +- 14 files changed, 82 insertions(+), 41 deletions(-) diff --git a/BepuPhysics/BatchCompressor.cs b/BepuPhysics/BatchCompressor.cs index 54d758168..9340721b0 100644 --- a/BepuPhysics/BatchCompressor.cs +++ b/BepuPhysics/BatchCompressor.cs @@ -401,11 +401,11 @@ public void Compress(BufferPool pool, IThreadDispatcher threadDispatcher = null, //var applyTime = 1e6 * (applyEnd - applyStart) / Stopwatch.Frequency; //Console.WriteLine($"Apply time (us): {applyTime}, per applied: {applyTime / compressionsApplied}, (maximum: {maximumCompressionCount})"); - for (int i = 0; i < workerCount; ++i) - { - //Be careful: the jobs may require resizes on the compression count list. That requires the use of per-worker pools. - workerCompressions[i].Dispose((threadDispatcher == null ? pool : threadDispatcher.WorkerPools[i])); - } + if (threadDispatcher == null) + workerCompressions[0].Dispose(pool); + else + threadDispatcher.WorkerPools.Clear(); //Ephemeral worker pools, don't have to be careful about deallocating individual things! + pool.Return(ref workerCompressions); } diff --git a/BepuPhysics/CollisionDetection/BroadPhase.cs b/BepuPhysics/CollisionDetection/BroadPhase.cs index 4cfc35ada..73bc8975d 100644 --- a/BepuPhysics/CollisionDetection/BroadPhase.cs +++ b/BepuPhysics/CollisionDetection/BroadPhase.cs @@ -224,6 +224,7 @@ public void Update(IThreadDispatcher threadDispatcher = null) threadDispatcher.DispatchWorkers(executeRefineAction, null, remainingJobCount); activeRefineContext.CleanUpForRefitAndRefine(Pool); staticRefineContext.CleanUpForRefitAndRefine(Pool); + threadDispatcher.WorkerPools.Clear(); this.threadDispatcher = null; } else diff --git a/BepuPhysics/IslandSleeper.cs b/BepuPhysics/IslandSleeper.cs index 3330c9588..7361db395 100644 --- a/BepuPhysics/IslandSleeper.cs +++ b/BepuPhysics/IslandSleeper.cs @@ -274,8 +274,8 @@ void FindIslands(int workerIndex, IUnmanagedMemoryPool threadPool, r Debug.Assert(workerTraversalResults.Allocated && workerTraversalResults.Length > workerIndex); ref var results = ref workerTraversalResults[workerIndex]; results.Islands = new QuickList(64, threadPool); - var bodyIndices = new QuickList(Math.Min(InitialIslandBodyCapacity, bodies.ActiveSet.Count), threadPool); - var constraintHandles = new QuickList(Math.Min(InitialIslandConstraintCapacity, solver.HandlePool.HighestPossiblyClaimedId + 1), threadPool); + var bodyIndices = new QuickList(int.Min(InitialIslandBodyCapacity, bodies.ActiveSet.Count), threadPool); + var constraintHandles = new QuickList(int.Max(8, int.Min(InitialIslandConstraintCapacity, solver.HandlePool.HighestPossiblyClaimedId + 1)), threadPool); TraversalTest traversalTest; traversalTest.Predicate = predicate; diff --git a/BepuUtilities/Memory/ArenaPool.cs b/BepuUtilities/Memory/ArenaPool.cs index d2239a09a..69f548f91 100644 --- a/BepuUtilities/Memory/ArenaPool.cs +++ b/BepuUtilities/Memory/ArenaPool.cs @@ -11,8 +11,8 @@ namespace BepuUtilities.Memory; /// Arena allocator built to serve a single thread. Pulls resources from a central buffer pool when necessary. /// /// Returns to this pool are not guaranteed to free memory because it does not carry enough information about allocations to do so. -/// To free up memory after use, the arena pool as a whole must be cleared using . -public class ArenaPool : IUnmanagedMemoryPool, IDisposable +/// To free up memory after use, the arena pool as a whole must be cleared using or . +public class ArenaPool : IUnmanagedMemoryPool { /// /// Gets or sets the central pool backing this arena allocator. Resources are pulled from this pool when the thread pool's blocks are depleted. @@ -89,7 +89,7 @@ public ArenaPool(IUnmanagedMemoryPool pool, int defaultBlockCapacity = 16384, ob /// Ensures that there is a given minimum amount of continguous space available in the pool. This does not acquire a lock to read from the internal pool. /// /// Size of the block to use. If negative, will be used instead. - public void EnsurePreallocatedSpaceUnsafely(int sizeInBytes = -1) + public void EnsureCapacityUnsafely(int sizeInBytes = -1) { if (sizeInBytes < 0) sizeInBytes = DefaultBlockCapacity; @@ -119,10 +119,13 @@ public void Clear() outstandingAllocators.Clear(); #endif #endif - for (int i = 0; i < blocks.Count; ++i) + int index = 0; + while (index < blocks.Span.length && blocks.Span[index].Data.Allocated) { - Pool.Return(ref blocks[i].Data); + Pool.ReturnUnsafely(blocks.Span[index].Data.Id); + ++index; } + blocks.Span.Clear(0, blocks.Span.Length); blocks.Count = 0; } @@ -149,6 +152,7 @@ public void Dispose() /// public unsafe void TakeAtLeast(int count, out Buffer buffer) where T : unmanaged { + count = int.Max(1, count); var sizeInBytes = Unsafe.SizeOf() * count; var blockIndex = blocks.Count - 1; if (blocks.Count == 0 || !blocks[blockIndex].TryAllocate(sizeInBytes, out Buffer allocation)) @@ -224,6 +228,7 @@ public unsafe void TakeAtLeast(int count, out Buffer buffer) where T : unm public void Take(int count, out Buffer buffer) where T : unmanaged { TakeAtLeast(count, out buffer); + buffer.length = count; } @@ -295,7 +300,6 @@ public void ResizeToAtLeast(ref Buffer buffer, int targetSize, int copyCou { //Only do anything if the new size is actually different from the current size. Debug.Assert(copyCount <= targetSize && copyCount <= buffer.Length, "Can't copy more elements than exist in the source or target buffers."); - targetSize = GetCapacityForCount(targetSize); if (!buffer.Allocated) { Debug.Assert(buffer.Length == 0, "If a buffer is pointing at null, then it should be default initialized and have a length of zero too."); diff --git a/BepuUtilities/Memory/BufferPool.cs b/BepuUtilities/Memory/BufferPool.cs index fa5bec10f..c09ed59b4 100644 --- a/BepuUtilities/Memory/BufferPool.cs +++ b/BepuUtilities/Memory/BufferPool.cs @@ -11,7 +11,7 @@ namespace BepuUtilities.Memory /// /// Unmanaged memory pool that suballocates from memory blocks pulled from the native heap. /// - public class BufferPool : IUnmanagedMemoryPool, IDisposable + public class BufferPool : IUnmanagedMemoryPool { unsafe struct PowerPool { @@ -270,6 +270,7 @@ public ulong GetTotalAllocatedByteCount() return sum; } + bool DEBUGLOCK; /// /// Takes a buffer large enough to contain a number of elements of a given type. Capacity may be larger than requested. /// @@ -279,11 +280,13 @@ public ulong GetTotalAllocatedByteCount() [MethodImpl(MethodImplOptions.AggressiveInlining)] public void TakeAtLeast(int count, out Buffer buffer) where T : unmanaged { + Debug.Assert(!DEBUGLOCK); DEBUGLOCK = true; //Avoid returning a zero length span because 1 byte / Unsafe.SizeOf() happens to be zero. if (count == 0) count = 1; TakeForPower(SpanHelper.GetContainingPowerOf2(count * Unsafe.SizeOf()), out var rawBuffer); buffer = rawBuffer.As(); + Debug.Assert(DEBUGLOCK); DEBUGLOCK = false; } /// @@ -322,8 +325,10 @@ internal static void DecomposeId(int bufferId, out int powerIndex, out int slotI [MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe void ReturnUnsafely(int id) { + Debug.Assert(!DEBUGLOCK); DEBUGLOCK = true; DecomposeId(id, out var powerIndex, out var slotIndex); pools[powerIndex].Return(slotIndex); + Debug.Assert(DEBUGLOCK); DEBUGLOCK = false; } /// @@ -406,7 +411,8 @@ public void AssertEmpty() } /// - /// Unpins and drops reference to all memory. Any outstanding buffers will be invalidated silently. + /// Returns all allocations in the pool to sources. Any outstanding buffers will be invalidated silently. + /// The pool will remain in a usable state after clearing. /// public void Clear() { @@ -416,6 +422,10 @@ public void Clear() } } + /// + /// Returns all allocations in the pool to sources. Any outstanding buffers will be invalidated silently. + /// Equivalent to for . + /// void IDisposable.Dispose() { Clear(); diff --git a/BepuUtilities/Memory/IUnmanagedMemoryPool.cs b/BepuUtilities/Memory/IUnmanagedMemoryPool.cs index 354feb1e5..891a572e7 100644 --- a/BepuUtilities/Memory/IUnmanagedMemoryPool.cs +++ b/BepuUtilities/Memory/IUnmanagedMemoryPool.cs @@ -1,11 +1,12 @@ -using System.Runtime.CompilerServices; +using System; +using System.Runtime.CompilerServices; namespace BepuUtilities.Memory { /// /// Defines a type that is capable of rapidly serving requests for allocation and deallocation of unmanaged memory. /// - public interface IUnmanagedMemoryPool + public interface IUnmanagedMemoryPool : IDisposable { /// /// Takes a buffer large enough to contain a number of elements of a given type. Capacity may be larger than requested. @@ -62,5 +63,10 @@ public interface IUnmanagedMemoryPool /// Number of elements to copy into the new buffer from the old buffer. void Resize(ref Buffer buffer, int targetSize, int copyCount) where T : unmanaged; + /// + /// Returns all allocations in the pool to sources. Any outstanding buffers will be invalidated silently. + /// The pool will remain in a usable state after clearing. + /// + void Clear(); } } \ No newline at end of file diff --git a/BepuUtilities/Memory/WorkerBufferPools.cs b/BepuUtilities/Memory/WorkerBufferPools.cs index 54d16e118..e105db854 100644 --- a/BepuUtilities/Memory/WorkerBufferPools.cs +++ b/BepuUtilities/Memory/WorkerBufferPools.cs @@ -7,16 +7,22 @@ namespace BepuUtilities.Memory; /// /// Returns to an are not guaranteed to free memory because it does not carry enough information about allocations to do so. /// To free up memory after use, the arena pool as a whole must be cleared using . -/// Use to clear all pools together. +/// Use to clear all pools together. public class WorkerBufferPools : IDisposable { ArenaPool[] pools; /// - /// Central pool from which subpools allocate. + /// Internal pool from which arena pools are allocated from. /// + /// This pool will be accessed from multiple threads through the arena pools. Accessing it while workers are using the worker pools without using the could lead to race conditions. + /// If no external pool was provided to the constructor, will dispose the internally created . If an external pool was provided, calling will not dispose it. public IUnmanagedMemoryPool Pool { get; private set; } /// - /// Locker used by subpools to control access to the central pool. + /// True if the backing pool was created by the and should be disposed with it, false otherwise. + /// + bool poolIsLocallyOwned; + /// + /// Locker used by subpools to control access to the backing pool. /// public object Locker { get; private set; } @@ -36,27 +42,29 @@ public class WorkerBufferPools : IDisposable /// /// Creates a new set of worker pools. /// - /// Central pool from which worker pools allocate from. + /// Central pool from which worker pools allocate from. If null, a will be created. The created pool will be disposed by , but if an external pool is provided, calling will not dispose it. /// Initial number of workers to allocate space for. /// Default block capacity in thread pools. - public WorkerBufferPools(IUnmanagedMemoryPool pool, int initialWorkerCount, int defaultBlockCapacity = 16384) + public WorkerBufferPools(int initialWorkerCount, IUnmanagedMemoryPool backingPool = null, int defaultBlockCapacity = 16384) { - Pool = pool; + poolIsLocallyOwned = backingPool == null; + Pool = poolIsLocallyOwned ? new BufferPool() : backingPool; Locker = new object(); pools = new ArenaPool[initialWorkerCount]; DefaultBlockCapacity = defaultBlockCapacity; for (int i = 0; i < pools.Length; ++i) { - pools[i] = new ArenaPool(pool, defaultBlockCapacity, Locker); + pools[i] = new ArenaPool(Pool, defaultBlockCapacity, Locker); } } /// - /// Preallocates for a given number of workers. + /// Ensures that at least the given amount of memory is available in the given number of worker pools. /// /// Number of workers to preallocate for. /// Capacity to preallocate in the specified worker pools. - public void Preallocate(int workerCount, int preallocationSize) + /// This does not take any locks and should not be called if any other threads may be using any of the involved pools. + public void EnsureCapacity(int workerCount, int preallocationSize) { if (workerCount > pools.Length) { @@ -64,27 +72,29 @@ public void Preallocate(int workerCount, int preallocationSize) Array.Resize(ref pools, workerCount); for (int i = oldSize; i < pools.Length; ++i) { - pools[i] = new ArenaPool(Pool, DefaultBlockCapacity); + pools[i] = new ArenaPool(Pool, DefaultBlockCapacity, Locker); } } for (int i = 0; i < workerCount; ++i) { - pools[i].EnsurePreallocatedSpaceUnsafely(preallocationSize); + pools[i].EnsureCapacityUnsafely(preallocationSize); } } /// - /// Preallocates space in all worker pools. + /// Ensures that at least the given amount of memory is available in all worker pools. /// /// Capacity to preallocate in all worker pools. - public void Preallocate(int preallocationSize) + /// This does not take any locks and should not be called if any other threads may be using any of the involved pools. + public void EnsureCapacity(int preallocationSize) { - Preallocate(pools.Length, preallocationSize); + EnsureCapacity(pools.Length, preallocationSize); } /// /// Clears all allocations from worker arena pools. Pools can still be used after being cleared. /// + /// This does not take any locks and should not be called if any other threads may be using any of the involved pools. public void Clear() { for (int i = 0; i < pools.Length; ++i) @@ -96,12 +106,15 @@ public void Clear() /// /// Disposes all worker arena pools. Pools cannot be used after being disposed. /// + /// This will dispose the only if no external pool was provided to the constructor. public void Dispose() { for (int i = 0; i < pools.Length; ++i) { pools[i].Dispose(); } + if (poolIsLocallyOwned) + Pool.Dispose(); } diff --git a/BepuUtilities/ThreadDispatcher.cs b/BepuUtilities/ThreadDispatcher.cs index 2556a4162..012cd3209 100644 --- a/BepuUtilities/ThreadDispatcher.cs +++ b/BepuUtilities/ThreadDispatcher.cs @@ -32,9 +32,9 @@ struct Worker /// Creates a new thread dispatcher with the given number of threads. /// /// Number of threads to dispatch on each invocation. - /// Pool to allocate blocks from for use in per-thread arena pools. + /// Central pool from which worker pools allocate from. If null, a buffer pool will be created. /// Size of memory blocks to allocate for thread pools. - public ThreadDispatcher(int threadCount, IUnmanagedMemoryPool pool, int threadPoolBlockAllocationSize = 16384) + public ThreadDispatcher(int threadCount, IUnmanagedMemoryPool backingPool = null, int threadPoolBlockAllocationSize = 16384) { this.threadCount = threadCount; workers = new Worker[threadCount - 1]; @@ -45,7 +45,7 @@ public ThreadDispatcher(int threadCount, IUnmanagedMemoryPool pool, int threadPo workers[i].Thread.Start((workers[i].Signal, i + 1)); } finished = new AutoResetEvent(false); - WorkerPools = new WorkerBufferPools(pool, threadCount, threadPoolBlockAllocationSize); + WorkerPools = new WorkerBufferPools(threadCount, backingPool, threadPoolBlockAllocationSize); } void DispatchThread(int workerIndex) @@ -145,12 +145,12 @@ public void Dispose() { disposed = true; SignalThreads(threadCount); - WorkerPools.Dispose(); foreach (var worker in workers) { worker.Thread.Join(); worker.Signal.Dispose(); } + WorkerPools.Dispose(); } } diff --git a/Demos/Demo.cs b/Demos/Demo.cs index d382a11d4..b150f4693 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -44,7 +44,7 @@ protected Demo() //It may be worth using something like hwloc or CPUID to extract extra information to reason about. var targetThreadCount = Math.Max(1, Environment.ProcessorCount > 4 ? Environment.ProcessorCount - 2 : Environment.ProcessorCount - 1); - ThreadDispatcher = new ThreadDispatcher(targetThreadCount, BufferPool); + ThreadDispatcher = new ThreadDispatcher(targetThreadCount); } public virtual void LoadGraphicalContent(ContentArchive content, RenderSurface surface) @@ -105,8 +105,8 @@ public void Dispose() disposed = true; OnDispose(); Simulation.Dispose(); - BufferPool.Clear(); ThreadDispatcher.Dispose(); + BufferPool.Clear(); } } diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index 323bc1af0..46748bf4c 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -46,6 +46,7 @@ struct Option public DemoSet() { + AddOption(); //AddOption(); //AddOption(); AddOption(); diff --git a/Demos/Demos/ContactEventsDemo.cs b/Demos/Demos/ContactEventsDemo.cs index b6afc48af..1b79144aa 100644 --- a/Demos/Demos/ContactEventsDemo.cs +++ b/Demos/Demos/ContactEventsDemo.cs @@ -232,7 +232,7 @@ public void Initialize(Simulation simulation) this.simulation = simulation; if (pool == null) pool = simulation.BufferPool; - threadPools = threadDispatcher != null ? new WorkerBufferPools(pool, threadDispatcher.ThreadCount) : null; + threadPools = threadDispatcher != null ? new WorkerBufferPools(threadDispatcher.ThreadCount) : null; simulation.Timestepper.BeforeCollisionDetection += SetFreshnessForCurrentActivityStatus; listenerIndices = new CollidableProperty(simulation, pool); pendingWorkerAdds = new QuickList[threadDispatcher == null ? 1 : threadDispatcher.ThreadCount]; @@ -774,5 +774,11 @@ public override void Render(Renderer renderer, Camera camera, Input input, TextB base.Render(renderer, camera, input, text, font); } + + protected override void OnDispose() + { + base.OnDispose(); + events.Dispose(); + } } } diff --git a/Demos/Demos/SimpleSelfContainedDemo.cs b/Demos/Demos/SimpleSelfContainedDemo.cs index fc0e0854d..9c11c1b68 100644 --- a/Demos/Demos/SimpleSelfContainedDemo.cs +++ b/Demos/Demos/SimpleSelfContainedDemo.cs @@ -224,7 +224,7 @@ public static void Run() simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), simulation.Shapes.Add(new Box(500, 1, 500)))); //Any IThreadDispatcher implementation can be used for multithreading. Here, we use the BepuUtilities.ThreadDispatcher implementation. - var threadDispatcher = new ThreadDispatcher(Environment.ProcessorCount, bufferPool); + var threadDispatcher = new ThreadDispatcher(Environment.ProcessorCount); //Now take 100 time steps! for (int i = 0; i < 100; ++i) diff --git a/Demos/SpecializedTests/IntertreeThreadingTests.cs b/Demos/SpecializedTests/IntertreeThreadingTests.cs index 4a7687407..0f999de04 100644 --- a/Demos/SpecializedTests/IntertreeThreadingTests.cs +++ b/Demos/SpecializedTests/IntertreeThreadingTests.cs @@ -181,7 +181,7 @@ public static void Test() { var random = new Random(5); var pool = new BufferPool(); - var threadDispatcher = new ThreadDispatcher(Environment.ProcessorCount, pool); + var threadDispatcher = new ThreadDispatcher(Environment.ProcessorCount); for (int i = 0; i < 1000; ++i) { TestTrees(pool, threadDispatcher, random); diff --git a/Demos/SpecializedTests/TreeTest.cs b/Demos/SpecializedTests/TreeTest.cs index 3d4c81a92..ed1675d29 100644 --- a/Demos/SpecializedTests/TreeTest.cs +++ b/Demos/SpecializedTests/TreeTest.cs @@ -68,7 +68,7 @@ public static void Test() const int iterations = 100000; const int maximumChangesPerIteration = 20; - var threadDispatcher = new ThreadDispatcher(Environment.ProcessorCount, pool); + var threadDispatcher = new ThreadDispatcher(Environment.ProcessorCount); var refineContext = new Tree.RefitAndRefineMultithreadedContext(); var selfTestContext = new Tree.MultithreadedSelfTest(pool); var overlapHandlers = new OverlapHandler[threadDispatcher.ThreadCount]; From e1a20511abbe3c2a72c9599382251c394ec6c525 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 25 Nov 2022 14:02:30 -0600 Subject: [PATCH 658/947] Works, but massive overallocation on small thread counts. --- BepuUtilities/Memory/ArenaPool.cs | 15 ++- BepuUtilities/Memory/IUnmanagedMemoryPool.cs | 7 ++ Demos/SpecializedTests/HeadlessDemo.cs | 106 +++++++++++-------- Demos/SpecializedTests/ShapePileTestDemo.cs | 37 ++++--- 4 files changed, 99 insertions(+), 66 deletions(-) diff --git a/BepuUtilities/Memory/ArenaPool.cs b/BepuUtilities/Memory/ArenaPool.cs index 69f548f91..2b25eca31 100644 --- a/BepuUtilities/Memory/ArenaPool.cs +++ b/BepuUtilities/Memory/ArenaPool.cs @@ -321,7 +321,20 @@ public void ResizeToAtLeast(ref Buffer buffer, int targetSize, int copyCou public void Resize(ref Buffer buffer, int targetSize, int copyCount) where T : unmanaged => ResizeToAtLeast(ref buffer, targetSize, copyCount); - + /// + /// Computes the total number of bytes allocated from native memory in this buffer pool. + /// Includes allocated memory regardless of whether it currently has outstanding references. + /// + /// Total number of bytes allocated from native memory in this buffer pool. + public ulong GetTotalAllocatedByteCount() + { + ulong sum = 0; + for (int i = 0; i < blocks.Span.Length; ++i) + { + sum += (ulong)blocks.Span[i].Data.Length; + } + return sum; + } } diff --git a/BepuUtilities/Memory/IUnmanagedMemoryPool.cs b/BepuUtilities/Memory/IUnmanagedMemoryPool.cs index 891a572e7..a2babe566 100644 --- a/BepuUtilities/Memory/IUnmanagedMemoryPool.cs +++ b/BepuUtilities/Memory/IUnmanagedMemoryPool.cs @@ -68,5 +68,12 @@ public interface IUnmanagedMemoryPool : IDisposable /// The pool will remain in a usable state after clearing. /// void Clear(); + + /// + /// Computes the total number of bytes allocated from the memory source in this pool. + /// Includes allocated memory regardless of whether it currently has outstanding references. + /// + /// Total number of bytes allocated from the backing memory source in this pool. + ulong GetTotalAllocatedByteCount(); } } \ No newline at end of file diff --git a/Demos/SpecializedTests/HeadlessDemo.cs b/Demos/SpecializedTests/HeadlessDemo.cs index d9932db95..07f54e7e4 100644 --- a/Demos/SpecializedTests/HeadlessDemo.cs +++ b/Demos/SpecializedTests/HeadlessDemo.cs @@ -1,5 +1,6 @@ using BepuPhysics.Collidables; using BepuPhysics.CollisionDetection; +using BepuUtilities.Memory; using DemoContentLoader; using DemoUtilities; using System; @@ -7,59 +8,72 @@ using System.Diagnostics; using System.Text; -namespace Demos.SpecializedTests +namespace Demos.SpecializedTests; + +static class HeadlessTest { - static class HeadlessTest + public static void Test(ContentArchive content, int runCount, int warmUpFrames, int frameCount) where T : Demo, new() { - public static void Test(ContentArchive content, int runCount, int warmUpFrames, int frameCount) where T : Demo, new() + var runFrameTimes = new double[runCount]; + ulong maximumMemoryUsedInMainPool = 0; + ulong maximumMemoryUsedInThreadPools = 0; + ulong maximumMemoryUsedInPools = 0; + for (int runIndex = 0; runIndex < runCount; ++runIndex) { - var runFrameTimes = new double[runCount]; - for (int runIndex = 0; runIndex < runCount; ++runIndex) + var demo = new T(); + demo.Initialize(content, new DemoRenderer.Camera(1, 1, 1, 1)); + GC.Collect(3, GCCollectionMode.Forced, true, true); + for (int i = 0; i < warmUpFrames; ++i) { - var demo = new T(); - demo.Initialize(content, new DemoRenderer.Camera(1, 1, 1, 1)); - GC.Collect(3, GCCollectionMode.Forced, true, true); - for (int i = 0; i < warmUpFrames; ++i) - { - demo.Update(null, null, null, Demo.TimestepDuration); - } - Console.WriteLine($"Warmup {runIndex} complete"); - double time = 0; - int largestOverlapCount = 0; - Console.Write("Completed frames: "); - for (int i = 0; i < frameCount; ++i) - { - //CacheBlaster.Blast(); - var start = Stopwatch.GetTimestamp(); - demo.Update(null, null, null, Demo.TimestepDuration); - var end = Stopwatch.GetTimestamp(); - time += (end - start) / (double)Stopwatch.Frequency; - if (i % 32 == 0) - Console.Write($"{i}, "); - } - Console.WriteLine(); - var frameTime = time / frameCount; - Console.WriteLine($"Time per frame (ms): {1e3 * frameTime}, maximum overlap count: {largestOverlapCount}"); - runFrameTimes[runIndex] = frameTime; - demo.Dispose(); + demo.Update(null, null, null, Demo.TimestepDuration); } - var min = double.MaxValue; - var max = double.MinValue; - var sum = 0.0; - var sumOfSquares = 0.0; - for (int runIndex = 0; runIndex < runCount; ++runIndex) + Console.WriteLine($"Warmup {runIndex} complete"); + double time = 0; + int largestOverlapCount = 0; + Console.Write("Completed frames: "); + for (int i = 0; i < frameCount; ++i) { - var time = runFrameTimes[runIndex]; - min = Math.Min(time, min); - max = Math.Max(time, max); - sum += time; - sumOfSquares += time * time; + //CacheBlaster.Blast(); + var start = Stopwatch.GetTimestamp(); + demo.Update(null, null, null, Demo.TimestepDuration); + var end = Stopwatch.GetTimestamp(); + time += (end - start) / (double)Stopwatch.Frequency; + if (i % 32 == 0) + Console.Write($"{i}, "); + var mainPoolSize = demo.BufferPool.GetTotalAllocatedByteCount(); + var threadPoolSize = demo.ThreadDispatcher.WorkerPools.Pool.GetTotalAllocatedByteCount(); + var totalPoolSize = mainPoolSize + threadPoolSize; + if (totalPoolSize > maximumMemoryUsedInPools) + { + maximumMemoryUsedInPools = totalPoolSize; + maximumMemoryUsedInMainPool = mainPoolSize; + maximumMemoryUsedInThreadPools = threadPoolSize; + } + } - var average = sum / runCount; - var stdDev = Math.Sqrt(sumOfSquares / runCount - average * average); - Console.WriteLine($"Average (ms): {average * 1e3}"); - Console.WriteLine($"Min, max (ms): {min * 1e3}, {max * 1e3}"); - Console.WriteLine($"Std Dev (ms): {stdDev * 1e3}"); + Console.WriteLine(); + var frameTime = time / frameCount; + Console.WriteLine($"Time per frame (ms): {1e3 * frameTime}, maximum overlap count: {largestOverlapCount}"); + runFrameTimes[runIndex] = frameTime; + demo.Dispose(); + } + var min = double.MaxValue; + var max = double.MinValue; + var sum = 0.0; + var sumOfSquares = 0.0; + for (int runIndex = 0; runIndex < runCount; ++runIndex) + { + var time = runFrameTimes[runIndex]; + min = Math.Min(time, min); + max = Math.Max(time, max); + sum += time; + sumOfSquares += time * time; } + var average = sum / runCount; + var stdDev = Math.Sqrt(sumOfSquares / runCount - average * average); + Console.WriteLine($"Average (ms): {average * 1e3}"); + Console.WriteLine($"Min, max (ms): {min * 1e3}, {max * 1e3}"); + Console.WriteLine($"Std Dev (ms): {stdDev * 1e3}"); + Console.WriteLine($"Maximum memory used in pools: {maximumMemoryUsedInPools} (main: {maximumMemoryUsedInMainPool}, threads: {maximumMemoryUsedInThreadPools})"); } } diff --git a/Demos/SpecializedTests/ShapePileTestDemo.cs b/Demos/SpecializedTests/ShapePileTestDemo.cs index acdf05c98..b4ef43d7f 100644 --- a/Demos/SpecializedTests/ShapePileTestDemo.cs +++ b/Demos/SpecializedTests/ShapePileTestDemo.cs @@ -23,7 +23,6 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //camera.Pitch = MathHelper.PiOver2 * 0.999f; Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(4, 1)); Simulation.Deterministic = true; - //Simulation.Deterministic = true; var sphere = new Sphere(1.5f); var capsule = new Capsule(1f, 1f); @@ -97,27 +96,27 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) for (int k = 0; k < length; ++k) { var location = new Vector3(6, 3, 6) * new Vector3(i, j, k) + new Vector3(-width * 3, 5.5f, -length * 3); - var bodyDescription = BodyDescription.CreateKinematic(location, new(default, ContinuousDetection.Passive), -0.01f); + var bodyDescription = BodyDescription.CreateKinematic(location, new(default, ContinuousDetection.Passive), 0.01f); var index = shapeCount++; switch (index % 5) { - //case 0: - // bodyDescription.Collidable.Shape = sphereIndex; - // bodyDescription.LocalInertia = sphereInertia; - // break; - //case 1: - // bodyDescription.Collidable.Shape = capsuleIndex; - // bodyDescription.LocalInertia = capsuleInertia; - // break; - //case 2: - // bodyDescription.Collidable.Shape = boxIndex; - // bodyDescription.LocalInertia = boxInertia; - // break; - //case 3: - // bodyDescription.Collidable.Shape = cylinderIndex; - // bodyDescription.LocalInertia = cylinderInertia; - // break; - //case 4: + case 0: + bodyDescription.Collidable.Shape = sphereIndex; + bodyDescription.LocalInertia = sphereInertia; + break; + case 1: + bodyDescription.Collidable.Shape = capsuleIndex; + bodyDescription.LocalInertia = capsuleInertia; + break; + case 2: + bodyDescription.Collidable.Shape = boxIndex; + bodyDescription.LocalInertia = boxInertia; + break; + case 3: + bodyDescription.Collidable.Shape = cylinderIndex; + bodyDescription.LocalInertia = cylinderInertia; + break; + case 4: default: bodyDescription.Collidable.Shape = hullIndex; bodyDescription.LocalInertia = hullInertia; From 99b372d367ab0e888cfa791d51f82b5342ec013a Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 26 Nov 2022 11:53:33 -0600 Subject: [PATCH 659/947] Reverted arena pools. May return later! --- BepuPhysics/BatchCompressor.cs | 9 +- BepuPhysics/CollisionDetection/BroadPhase.cs | 1 - BepuUtilities/Memory/ArenaPool.cs | 340 ------------------- BepuUtilities/Memory/WorkerBufferPools.cs | 84 ++--- BepuUtilities/ThreadDispatcher.cs | 5 +- DemoRenderer/ShapeDrawing/ShapesExtractor.cs | 1 - Demos/Demos/ContactEventsDemo.cs | 6 +- Demos/Program.cs | 2 + Demos/SpecializedTests/HeadlessDemo.cs | 2 +- 9 files changed, 33 insertions(+), 417 deletions(-) delete mode 100644 BepuUtilities/Memory/ArenaPool.cs diff --git a/BepuPhysics/BatchCompressor.cs b/BepuPhysics/BatchCompressor.cs index 9340721b0..ac51b0616 100644 --- a/BepuPhysics/BatchCompressor.cs +++ b/BepuPhysics/BatchCompressor.cs @@ -404,8 +404,13 @@ public void Compress(BufferPool pool, IThreadDispatcher threadDispatcher = null, if (threadDispatcher == null) workerCompressions[0].Dispose(pool); else - threadDispatcher.WorkerPools.Clear(); //Ephemeral worker pools, don't have to be careful about deallocating individual things! - + { + for (int i = 0; i < workerCount; ++i) + { + workerCompressions[i].Dispose(threadDispatcher.WorkerPools[i]); + } + } + pool.Return(ref workerCompressions); } diff --git a/BepuPhysics/CollisionDetection/BroadPhase.cs b/BepuPhysics/CollisionDetection/BroadPhase.cs index 73bc8975d..4cfc35ada 100644 --- a/BepuPhysics/CollisionDetection/BroadPhase.cs +++ b/BepuPhysics/CollisionDetection/BroadPhase.cs @@ -224,7 +224,6 @@ public void Update(IThreadDispatcher threadDispatcher = null) threadDispatcher.DispatchWorkers(executeRefineAction, null, remainingJobCount); activeRefineContext.CleanUpForRefitAndRefine(Pool); staticRefineContext.CleanUpForRefitAndRefine(Pool); - threadDispatcher.WorkerPools.Clear(); this.threadDispatcher = null; } else diff --git a/BepuUtilities/Memory/ArenaPool.cs b/BepuUtilities/Memory/ArenaPool.cs deleted file mode 100644 index 2b25eca31..000000000 --- a/BepuUtilities/Memory/ArenaPool.cs +++ /dev/null @@ -1,340 +0,0 @@ -using BepuUtilities.Collections; -using System; -using System.Collections.Generic; -using System.Diagnostics; -using System.Numerics; -using System.Runtime.CompilerServices; - -namespace BepuUtilities.Memory; - -/// -/// Arena allocator built to serve a single thread. Pulls resources from a central buffer pool when necessary. -/// -/// Returns to this pool are not guaranteed to free memory because it does not carry enough information about allocations to do so. -/// To free up memory after use, the arena pool as a whole must be cleared using or . -public class ArenaPool : IUnmanagedMemoryPool -{ - /// - /// Gets or sets the central pool backing this arena allocator. Resources are pulled from this pool when the thread pool's blocks are depleted. - /// - public IUnmanagedMemoryPool Pool { get; set; } - /// - /// Gets the locker object used to protect accesses to the cetnral buffer pool. - /// - public object Locker { get; private set; } - /// - /// Gets or sets the default capacity within blocks allocated by the pool. - /// - public int DefaultBlockCapacity { get; set; } - - struct Block - { - public int Count; - public Buffer Data; - - public Block(Buffer data) - { - Data = data; - Count = 0; - } - - public bool TryAllocate(int sizeInBytes, out Buffer allocation) - { - //Following the pattern set by the bufferpool, use beefy alignment: - //TODO: This is somewhat dumb and we should change it! - var startLocation = (Count + 127) & (~127); - var newCount = startLocation + sizeInBytes; - if (Data.Length >= newCount) - { - allocation = Data.Slice(startLocation, sizeInBytes); - Count = newCount; - return true; - } - allocation = default; - return false; - } - } -#if DEBUG - internal HashSet outstandingIds; -#if LEAKDEBUG - internal Dictionary> outstandingAllocators; -#endif -#endif - - QuickList blocks; - - /// - /// Creates a new arena thread pool. - /// - /// Central pool to allocate blocks from - /// Number of bytes to allocate for a single block if an allocation request does not need more. - /// Locker object used to protect accesses to the central pool. If no locker is specified, the pool reference is used. - public ArenaPool(IUnmanagedMemoryPool pool, int defaultBlockCapacity = 16384, object locker = null) - { - Pool = pool; - Locker = locker == null ? pool : locker; - DefaultBlockCapacity = defaultBlockCapacity; - blocks = new QuickList(8, pool); - //We check for allocated blocks before allocating one, so we need to clear the memory up front. - blocks.Span.Clear(0, blocks.Span.Length); -#if DEBUG - outstandingIds = new HashSet(); -#if LEAKDEBUG - outstandingAllocators = new Dictionary>(); -#endif -#endif - } - - /// - /// Ensures that there is a given minimum amount of continguous space available in the pool. This does not acquire a lock to read from the internal pool. - /// - /// Size of the block to use. If negative, will be used instead. - public void EnsureCapacityUnsafely(int sizeInBytes = -1) - { - if (sizeInBytes < 0) - sizeInBytes = DefaultBlockCapacity; - if (blocks.Count > 0) - { - ref var block = ref blocks[blocks.Count - 1]; - if (block.Data.Length - block.Count >= sizeInBytes) - { - //There's enough room already; no need to allocate. - return; - } - } - //We need a new block. - Pool.Take(sizeInBytes, out var data); - blocks.Allocate(Pool) = new Block(data); - - } - - /// - /// Clears all block allocations from the pool. The pool can still be used. - /// - public void Clear() - { -#if DEBUG - outstandingIds.Clear(); -#if LEAKDEBUG - outstandingAllocators.Clear(); -#endif -#endif - int index = 0; - while (index < blocks.Span.length && blocks.Span[index].Data.Allocated) - { - Pool.ReturnUnsafely(blocks.Span[index].Data.Id); - ++index; - } - blocks.Span.Clear(0, blocks.Span.Length); - blocks.Count = 0; - } - - /// - /// Clears all allocations from the pool, including any used by the pool itself. The pool can no longer be used. - /// - public void Dispose() - { - if (blocks.Span.Allocated) - { - Clear(); - blocks.Dispose(Pool); - } - } - - const int maximumBitsForBlock = 1; - const int maximumBitsForIndices = 31 - maximumBitsForBlock; - const int maximumBitsForIndex = maximumBitsForIndices / 2; - const int countInBlockMask = (1 << maximumBitsForIndex) - 1; - const int previousCountInBlockMask = countInBlockMask << maximumBitsForIndex; - const int blockMask = ~(countInBlockMask | previousCountInBlockMask); - const int lowerBlockMask = (1 << maximumBitsForBlock) - 1; - - /// - public unsafe void TakeAtLeast(int count, out Buffer buffer) where T : unmanaged - { - count = int.Max(1, count); - var sizeInBytes = Unsafe.SizeOf() * count; - var blockIndex = blocks.Count - 1; - if (blocks.Count == 0 || !blocks[blockIndex].TryAllocate(sizeInBytes, out Buffer allocation)) - { - //No room; need a new block. - var newBlockCapacityInBytes = int.Max(DefaultBlockCapacity, sizeInBytes); - //Check to see if there's already a block allocated that we can use. - if (blocks.Span.Length > blocks.Count && blocks.Span[blocks.Count].Data.Length >= sizeInBytes) - { - ++blocks.Count; - Debug.Assert(blocks[blocks.Count - 1].Count == 0 && blocks[blocks.Count - 1].Data.Memory != null); - } - else - { - //Need a new block. - Buffer blockData; - bool resizedBlockList = false; - lock (Locker) - { - Pool.Take(newBlockCapacityInBytes, out blockData); - if (blocks.Span.Length == blocks.Count) - { - blocks.EnsureCapacity(blocks.Count * 2, Pool); - resizedBlockList = true; - } - } - if (resizedBlockList) - { - //We check for allocated blocks before allocating one, so we need to clear the memory up front. - blocks.Span.Clear(blocks.Count, blocks.Span.length - blocks.Count); - } - blocks.AllocateUnsafely() = new Block(blockData); - //Console.WriteLine($"allocated a new block for {sizeInBytes} size: {blocks.Count}"); - } - blockIndex = blocks.Count - 1; - var succeeded = blocks[blockIndex].TryAllocate(sizeInBytes, out allocation); - Debug.Assert(succeeded, "We just allocated that block, it should hold everything requested!"); - } - buffer = allocation.As(); - var newCount = blocks[blockIndex].Count; - var previousCount = newCount - sizeInBytes; - var bitpackedBlockIndex = lowerBlockMask & blockIndex; - var bitpackedStartIndex = countInBlockMask & newCount; - var bitpackedPreviousIndex = countInBlockMask & previousCount; - buffer.Id = (bitpackedBlockIndex << maximumBitsForIndices) | (bitpackedPreviousIndex << maximumBitsForIndex) | bitpackedStartIndex; - if (blockIndex >= (1 << maximumBitsForBlock) || previousCount >= (1 << maximumBitsForIndex) || newCount >= (1 << maximumBitsForIndex)) - { - //Bit index 31 being set is code for 'unrepresentable'. - buffer.Id |= 1 << 31; - } -#if DEBUG - if (buffer.Id >= 0) //Don't include unrepresentable ids in the tracked set. - { - const int maximumOutstandingCount = 1 << 26; - Debug.Assert(outstandingIds.Count < maximumOutstandingCount, - $"Do you actually truly really need to have {maximumOutstandingCount} allocations taken from this pool, or is this a memory leak?"); - Debug.Assert(outstandingIds.Add(buffer.Id), "Should not be able to request the same slot twice."); -#if LEAKDEBUG - var allocator = new StackTrace().ToString(); - if (!outstandingAllocators.TryGetValue(allocator, out var idsForAllocator)) - { - idsForAllocator = new HashSet(); - outstandingAllocators.Add(allocator, idsForAllocator); - } - const int maximumReasonableOutstandingAllocationsForAllocator = 1 << 25; - Debug.Assert(idsForAllocator.Count < maximumReasonableOutstandingAllocationsForAllocator, "Do you actually have that many allocations for this one allocator?"); - idsForAllocator.Add(buffer.Id); -#endif - } -#endif - } - /// - public void Take(int count, out Buffer buffer) where T : unmanaged - { - TakeAtLeast(count, out buffer); - buffer.length = count; - } - - - /// - /// Unlike a , the will not generally free up space in response to calls to . - /// If the deallocated buffer is the last allocated buffer for a given block, the pool may choose to bump the allocation pointer back, but it is not guaranteed. - public void ReturnUnsafely(int id) - { - if (id >= 0) - { - //This was a representable id. -#if DEBUG - Debug.Assert(outstandingIds.Remove(id), - "This buffer id must have been taken from the pool previously."); -#if LEAKDEBUG - bool found = false; - foreach (var pair in outstandingAllocators) - { - if (pair.Value.Remove(id)) - { - found = true; - if (pair.Value.Count == 0) - { - outstandingAllocators.Remove(pair.Key); - break; - } - } - } - Debug.Assert(found, "Allocator set must contain the buffer id."); -#endif -#endif - //Was it the most recently allocated buffer (in that block) such that we can pop it off like a stack? - var blockIndex = (id & blockMask) >> maximumBitsForIndices; - Debug.Assert(blockIndex < blocks.Count, "Invalid id; the encoded block index doesn't fit in this pool."); - var countInBlock = id & countInBlockMask; - var previousCountInBlock = (id >> maximumBitsForIndex) & countInBlockMask; - ref var block = ref blocks[blockIndex]; - if (block.Count == countInBlock) - { - //Deallocating is as simple as just resetting to the previous value. - block.Count = previousCountInBlock; - while (blocks.Count > 0 && blocks[blocks.Count - 1].Count == 0) - { - //Push the block count as far as it can go. - --blocks.Count; - //Console.WriteLine($"destroyed a block: {blocks.Count}"); - } - } - } - } - - /// - /// Unlike a , the will not generally free up space in response to calls to . - /// If the deallocated buffer is the last allocated buffer for a given block, the pool may choose to bump the allocation pointer back, but it is not guaranteed. - public void Return(ref Buffer buffer) where T : unmanaged - { - ReturnUnsafely(buffer.Id); - buffer = default; - } - - /// - public int GetCapacityForCount(int count) where T : unmanaged - { - return count; - } - - /// - public void ResizeToAtLeast(ref Buffer buffer, int targetSize, int copyCount) where T : unmanaged - { - //Only do anything if the new size is actually different from the current size. - Debug.Assert(copyCount <= targetSize && copyCount <= buffer.Length, "Can't copy more elements than exist in the source or target buffers."); - if (!buffer.Allocated) - { - Debug.Assert(buffer.Length == 0, "If a buffer is pointing at null, then it should be default initialized and have a length of zero too."); - //This buffer is not allocated; just return a new one. No copying to be done. - TakeAtLeast(targetSize, out buffer); - } - else - { - //Unlike the BufferPool, we can't rely on the Buffer id to tell us if there was spare room beyond the buffer, so we can't incrementally resize. We have to reallocate. - //(Alignment could be different per allocation, too, so we'd need to know something about the *next* allocation to know whether we can resize.) - TakeAtLeast(targetSize, out Buffer newBuffer); - buffer.CopyTo(0, newBuffer, 0, copyCount); - ReturnUnsafely(buffer.Id); - buffer = newBuffer; - } - } - - /// - public void Resize(ref Buffer buffer, int targetSize, int copyCount) where T : unmanaged => ResizeToAtLeast(ref buffer, targetSize, copyCount); - - - /// - /// Computes the total number of bytes allocated from native memory in this buffer pool. - /// Includes allocated memory regardless of whether it currently has outstanding references. - /// - /// Total number of bytes allocated from native memory in this buffer pool. - public ulong GetTotalAllocatedByteCount() - { - ulong sum = 0; - for (int i = 0; i < blocks.Span.Length; ++i) - { - sum += (ulong)blocks.Span[i].Data.Length; - } - return sum; - } - - -} diff --git a/BepuUtilities/Memory/WorkerBufferPools.cs b/BepuUtilities/Memory/WorkerBufferPools.cs index e105db854..5beda8a7d 100644 --- a/BepuUtilities/Memory/WorkerBufferPools.cs +++ b/BepuUtilities/Memory/WorkerBufferPools.cs @@ -3,35 +3,20 @@ namespace BepuUtilities.Memory; /// -/// Collection of arena pools used by worker threads. +/// Collection of pools used by worker threads. /// -/// Returns to an are not guaranteed to free memory because it does not carry enough information about allocations to do so. -/// To free up memory after use, the arena pool as a whole must be cleared using . -/// Use to clear all pools together. public class WorkerBufferPools : IDisposable { - ArenaPool[] pools; - /// - /// Internal pool from which arena pools are allocated from. - /// - /// This pool will be accessed from multiple threads through the arena pools. Accessing it while workers are using the worker pools without using the could lead to race conditions. - /// If no external pool was provided to the constructor, will dispose the internally created . If an external pool was provided, calling will not dispose it. - public IUnmanagedMemoryPool Pool { get; private set; } - /// - /// True if the backing pool was created by the and should be disposed with it, false otherwise. - /// - bool poolIsLocallyOwned; + BufferPool[] pools; + /// - /// Locker used by subpools to control access to the backing pool. - /// - public object Locker { get; private set; } /// /// Gets the pool associated with this worker. /// /// Worker index of the pool to look up. /// Pool associated with the given worker. - public ArenaPool this[int workerIndex] => pools[workerIndex]; + public BufferPool this[int workerIndex] => pools[workerIndex]; /// /// Gets or sets the default block capacity for any newly created arena subpools. @@ -42,60 +27,34 @@ public class WorkerBufferPools : IDisposable /// /// Creates a new set of worker pools. /// - /// Central pool from which worker pools allocate from. If null, a will be created. The created pool will be disposed by , but if an external pool is provided, calling will not dispose it. - /// Initial number of workers to allocate space for. + /// Initial number of workers to allocate space for. /// Default block capacity in thread pools. - public WorkerBufferPools(int initialWorkerCount, IUnmanagedMemoryPool backingPool = null, int defaultBlockCapacity = 16384) + public WorkerBufferPools(int initialWorkerCount, int defaultBlockCapacity = 16384) { - poolIsLocallyOwned = backingPool == null; - Pool = poolIsLocallyOwned ? new BufferPool() : backingPool; - Locker = new object(); - pools = new ArenaPool[initialWorkerCount]; + pools = new BufferPool[initialWorkerCount]; DefaultBlockCapacity = defaultBlockCapacity; for (int i = 0; i < pools.Length; ++i) { - pools[i] = new ArenaPool(Pool, defaultBlockCapacity, Locker); + pools[i] = new BufferPool(defaultBlockCapacity); } } /// - /// Ensures that at least the given amount of memory is available in the given number of worker pools. + /// Clears all allocations from worker pools. Pools can still be used after being cleared. /// - /// Number of workers to preallocate for. - /// Capacity to preallocate in the specified worker pools. /// This does not take any locks and should not be called if any other threads may be using any of the involved pools. - public void EnsureCapacity(int workerCount, int preallocationSize) + public void Clear() { - if (workerCount > pools.Length) - { - var oldSize = pools.Length; - Array.Resize(ref pools, workerCount); - for (int i = oldSize; i < pools.Length; ++i) - { - pools[i] = new ArenaPool(Pool, DefaultBlockCapacity, Locker); - } - } - for (int i = 0; i < workerCount; ++i) + for (int i = 0; i < pools.Length; ++i) { - pools[i].EnsureCapacityUnsafely(preallocationSize); + pools[i].Clear(); } } /// - /// Ensures that at least the given amount of memory is available in all worker pools. - /// - /// Capacity to preallocate in all worker pools. - /// This does not take any locks and should not be called if any other threads may be using any of the involved pools. - public void EnsureCapacity(int preallocationSize) - { - EnsureCapacity(pools.Length, preallocationSize); - } - - /// - /// Clears all allocations from worker arena pools. Pools can still be used after being cleared. + /// Disposes all worker pools. Pools cannot be used after being disposed. /// - /// This does not take any locks and should not be called if any other threads may be using any of the involved pools. - public void Clear() + public void Dispose() { for (int i = 0; i < pools.Length; ++i) { @@ -104,19 +63,16 @@ public void Clear() } /// - /// Disposes all worker arena pools. Pools cannot be used after being disposed. + /// Gets the total number of bytes allocated from native memory by all worker pools. Includes memory that is not currently in use by external allocators. /// - /// This will dispose the only if no external pool was provided to the constructor. - public void Dispose() + /// Total number of bytes allocated from native memory by all worker pools. Includes memory that is not currently in use by external allocators. + public ulong GetTotalAllocatedByteCount() { + ulong sum = 0; for (int i = 0; i < pools.Length; ++i) { - pools[i].Dispose(); + sum += pools[i].GetTotalAllocatedByteCount(); } - if (poolIsLocallyOwned) - Pool.Dispose(); + return sum; } - - - } diff --git a/BepuUtilities/ThreadDispatcher.cs b/BepuUtilities/ThreadDispatcher.cs index 012cd3209..e6a8cff7f 100644 --- a/BepuUtilities/ThreadDispatcher.cs +++ b/BepuUtilities/ThreadDispatcher.cs @@ -32,9 +32,8 @@ struct Worker /// Creates a new thread dispatcher with the given number of threads. /// /// Number of threads to dispatch on each invocation. - /// Central pool from which worker pools allocate from. If null, a buffer pool will be created. /// Size of memory blocks to allocate for thread pools. - public ThreadDispatcher(int threadCount, IUnmanagedMemoryPool backingPool = null, int threadPoolBlockAllocationSize = 16384) + public ThreadDispatcher(int threadCount, int threadPoolBlockAllocationSize = 16384) { this.threadCount = threadCount; workers = new Worker[threadCount - 1]; @@ -45,7 +44,7 @@ public ThreadDispatcher(int threadCount, IUnmanagedMemoryPool backingPool = null workers[i].Thread.Start((workers[i].Signal, i + 1)); } finished = new AutoResetEvent(false); - WorkerPools = new WorkerBufferPools(threadCount, backingPool, threadPoolBlockAllocationSize); + WorkerPools = new WorkerBufferPools(threadCount, threadPoolBlockAllocationSize); } void DispatchThread(int workerIndex) diff --git a/DemoRenderer/ShapeDrawing/ShapesExtractor.cs b/DemoRenderer/ShapeDrawing/ShapesExtractor.cs index e9d307c27..66348fb2f 100644 --- a/DemoRenderer/ShapeDrawing/ShapesExtractor.cs +++ b/DemoRenderer/ShapeDrawing/ShapesExtractor.cs @@ -372,7 +372,6 @@ void EndMultithreadedExecution() { workerCaches[i].Dispose(looper.Dispatcher.WorkerPools[i]); } - looper.Dispatcher.WorkerPools.Clear(); looper.Dispatcher = null; pool.Return(ref workerCaches); } diff --git a/Demos/Demos/ContactEventsDemo.cs b/Demos/Demos/ContactEventsDemo.cs index 1b79144aa..7d8e1686e 100644 --- a/Demos/Demos/ContactEventsDemo.cs +++ b/Demos/Demos/ContactEventsDemo.cs @@ -172,7 +172,6 @@ struct PreviousCollision Simulation simulation; IThreadDispatcher threadDispatcher; - WorkerBufferPools threadPools; BufferPool pool; //We'll use a handle->index mapping in a CollidableProperty to point at our contiguously stored listeners (in the later listeners array). @@ -218,7 +217,7 @@ public ContactEvents(IThreadDispatcher threadDispatcher = null, BufferPool pool IUnmanagedMemoryPool GetPoolForWorker(int workerIndex) { - return threadDispatcher == null ? pool : threadPools[workerIndex]; + return threadDispatcher == null ? pool : threadDispatcher.WorkerPools[workerIndex]; } /// @@ -232,7 +231,6 @@ public void Initialize(Simulation simulation) this.simulation = simulation; if (pool == null) pool = simulation.BufferPool; - threadPools = threadDispatcher != null ? new WorkerBufferPools(threadDispatcher.ThreadCount) : null; simulation.Timestepper.BeforeCollisionDetection += SetFreshnessForCurrentActivityStatus; listenerIndices = new CollidableProperty(simulation, pool); pendingWorkerAdds = new QuickList[threadDispatcher == null ? 1 : threadDispatcher.ThreadCount]; @@ -582,7 +580,6 @@ public void Flush() //We rely on zeroing out the count for lazy initialization. pendingAdds = default; } - threadPools?.Clear(); } public void Dispose() @@ -593,7 +590,6 @@ public void Dispose() staticListenerFlags.Dispose(pool); listenerIndices.Dispose(); simulation.Timestepper.BeforeCollisionDetection -= SetFreshnessForCurrentActivityStatus; - threadPools?.Dispose(); for (int i = 0; i < pendingWorkerAdds.Length; ++i) { Debug.Assert(!pendingWorkerAdds[i].Span.Allocated, "The pending worker adds should have been disposed by the previous flush."); diff --git a/Demos/Program.cs b/Demos/Program.cs index f2013db84..08a89de9c 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -1,6 +1,7 @@ using BepuPhysics.Trees; using BepuUtilities; using DemoContentLoader; +using Demos.SpecializedTests; using DemoUtilities; using OpenTK; using System.Runtime.Intrinsics; @@ -19,6 +20,7 @@ static void Main() { content = ContentArchive.Load(stream); } + //HeadlessTest.Test(content, 4, 32, 512); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); diff --git a/Demos/SpecializedTests/HeadlessDemo.cs b/Demos/SpecializedTests/HeadlessDemo.cs index 07f54e7e4..572a2d05c 100644 --- a/Demos/SpecializedTests/HeadlessDemo.cs +++ b/Demos/SpecializedTests/HeadlessDemo.cs @@ -41,7 +41,7 @@ static class HeadlessTest if (i % 32 == 0) Console.Write($"{i}, "); var mainPoolSize = demo.BufferPool.GetTotalAllocatedByteCount(); - var threadPoolSize = demo.ThreadDispatcher.WorkerPools.Pool.GetTotalAllocatedByteCount(); + var threadPoolSize = demo.ThreadDispatcher.WorkerPools.GetTotalAllocatedByteCount(); var totalPoolSize = mainPoolSize + threadPoolSize; if (totalPoolSize > maximumMemoryUsedInPools) { From a3d0313ce2aad2a35cffed94416fd65211add675 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 26 Nov 2022 12:08:22 -0600 Subject: [PATCH 660/947] Removed some debug. --- BepuUtilities/Memory/BufferPool.cs | 5 ----- Demos/Program.cs | 2 +- 2 files changed, 1 insertion(+), 6 deletions(-) diff --git a/BepuUtilities/Memory/BufferPool.cs b/BepuUtilities/Memory/BufferPool.cs index c09ed59b4..f6485bb6c 100644 --- a/BepuUtilities/Memory/BufferPool.cs +++ b/BepuUtilities/Memory/BufferPool.cs @@ -270,7 +270,6 @@ public ulong GetTotalAllocatedByteCount() return sum; } - bool DEBUGLOCK; /// /// Takes a buffer large enough to contain a number of elements of a given type. Capacity may be larger than requested. /// @@ -280,13 +279,11 @@ public ulong GetTotalAllocatedByteCount() [MethodImpl(MethodImplOptions.AggressiveInlining)] public void TakeAtLeast(int count, out Buffer buffer) where T : unmanaged { - Debug.Assert(!DEBUGLOCK); DEBUGLOCK = true; //Avoid returning a zero length span because 1 byte / Unsafe.SizeOf() happens to be zero. if (count == 0) count = 1; TakeForPower(SpanHelper.GetContainingPowerOf2(count * Unsafe.SizeOf()), out var rawBuffer); buffer = rawBuffer.As(); - Debug.Assert(DEBUGLOCK); DEBUGLOCK = false; } /// @@ -325,10 +322,8 @@ internal static void DecomposeId(int bufferId, out int powerIndex, out int slotI [MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe void ReturnUnsafely(int id) { - Debug.Assert(!DEBUGLOCK); DEBUGLOCK = true; DecomposeId(id, out var powerIndex, out var slotIndex); pools[powerIndex].Return(slotIndex); - Debug.Assert(DEBUGLOCK); DEBUGLOCK = false; } /// diff --git a/Demos/Program.cs b/Demos/Program.cs index 08a89de9c..c31a73c7f 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -20,7 +20,7 @@ static void Main() { content = ContentArchive.Load(stream); } - //HeadlessTest.Test(content, 4, 32, 512); + HeadlessTest.Test(content, 4, 32, 512); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); From d38f8181c55c1a923f11344ddea2d015f0781758 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 26 Nov 2022 12:33:42 -0600 Subject: [PATCH 661/947] Fart! Interfacizing bufferpools still carries a slight overhead. Something is hitting the pools too frequently to make this change yet. --- BepuPhysics/BatchCompressor.cs | 4 +- BepuPhysics/CollidableProperty.cs | 6 +-- BepuPhysics/Collidables/BigCompound.cs | 22 +++++----- BepuPhysics/Collidables/BoundingBoxBatcher.cs | 8 ++-- BepuPhysics/Collidables/Box.cs | 2 +- BepuPhysics/Collidables/Capsule.cs | 2 +- BepuPhysics/Collidables/Compound.cs | 12 +++--- BepuPhysics/Collidables/ConvexHull.cs | 6 +-- BepuPhysics/Collidables/ConvexHullHelper.cs | 22 +++++----- BepuPhysics/Collidables/Cylinder.cs | 2 +- BepuPhysics/Collidables/IShape.cs | 6 +-- BepuPhysics/Collidables/Mesh.cs | 16 +++---- BepuPhysics/Collidables/Shapes.cs | 42 +++++++++---------- BepuPhysics/Collidables/Sphere.cs | 2 +- BepuPhysics/Collidables/Triangle.cs | 2 +- .../CollisionDetection/CollisionBatcher.cs | 4 +- .../CollisionBatcherContinuations.cs | 6 +-- .../CompoundPairCollisionTask.cs | 2 +- .../CompoundPairOverlapFinder.cs | 2 +- .../CollisionTasks/CompoundPairOverlaps.cs | 10 ++--- .../ConvexCompoundOverlapFinder.cs | 8 ++-- .../ConvexCompoundTaskOverlaps.cs | 8 ++-- .../CollisionTasks/MeshPairOverlapFinder.cs | 2 +- .../CompoundMeshReduction.cs | 2 +- .../CollisionDetection/ConstraintRemover.cs | 10 ++--- .../CollisionDetection/MeshReduction.cs | 6 +-- BepuPhysics/CollisionDetection/NarrowPhase.cs | 2 +- .../NarrowPhaseCCDContinuations.cs | 12 +++--- .../NarrowPhasePendingConstraintAdds.cs | 4 +- .../CollisionDetection/NonconvexReduction.cs | 4 +- BepuPhysics/CollisionDetection/RayBatchers.cs | 4 +- .../CollisionDetection/SweepTaskRegistry.cs | 4 +- .../CompoundHomogeneousCompoundSweepTask.cs | 2 +- .../CompoundPairSweepOverlapFinder.cs | 4 +- .../SweepTasks/CompoundPairSweepOverlaps.cs | 4 +- .../SweepTasks/CompoundPairSweepTask.cs | 2 +- .../ConvexCompoundSweepOverlapFinder.cs | 4 +- .../SweepTasks/ConvexCompoundSweepTask.cs | 2 +- .../ConvexHomogeneousCompoundSweepTask.cs | 2 +- .../SweepTasks/ConvexSweepTaskCommon.cs | 2 +- BepuPhysics/CollisionDetection/UntypedList.cs | 10 ++--- .../CollisionDetection/WorkerPairCache.cs | 6 +-- BepuPhysics/IslandScaffold.cs | 16 +++---- BepuPhysics/IslandSleeper.cs | 14 +++---- BepuPhysics/SequentialFallbackBatch.cs | 12 +++--- BepuPhysics/Trees/RayBatcher.cs | 4 +- BepuPhysics/Trees/Tree.cs | 8 ++-- BepuPhysics/Trees/Tree_Add.cs | 2 +- BepuPhysics/Trees/Tree_BinnedRefine.cs | 4 +- .../Trees/Tree_MultithreadedRefitRefine.cs | 6 +-- .../Trees/Tree_RefinementScheduling.cs | 6 +-- BepuPhysics/Trees/Tree_SweepBuilder.cs | 2 +- Demos/Demos/ContactEventsDemo.cs | 2 +- Demos/Demos/CustomVoxelCollidableDemo.cs | 10 ++--- 54 files changed, 184 insertions(+), 184 deletions(-) diff --git a/BepuPhysics/BatchCompressor.cs b/BepuPhysics/BatchCompressor.cs index ac51b0616..5513b2f3d 100644 --- a/BepuPhysics/BatchCompressor.cs +++ b/BepuPhysics/BatchCompressor.cs @@ -128,7 +128,7 @@ unsafe void AnalysisWorker(int workerIndex, void* context) [MethodImpl(MethodImplOptions.AggressiveInlining)] private unsafe void TryToFindBetterBatchForConstraint( - IUnmanagedMemoryPool pool, ref QuickList compressions, ref TypeBatch typeBatch, int* bodyHandles, ref ActiveConstraintDynamicBodyHandleCollector handleAccumulator, TypeProcessor typeProcessor, int constraintIndex) + BufferPool pool, ref QuickList compressions, ref TypeBatch typeBatch, int* bodyHandles, ref ActiveConstraintDynamicBodyHandleCollector handleAccumulator, TypeProcessor typeProcessor, int constraintIndex) { handleAccumulator.Count = 0; Solver.EnumerateConnectedRawBodyReferences(ref typeBatch, constraintIndex, ref handleAccumulator); @@ -145,7 +145,7 @@ private unsafe void TryToFindBetterBatchForConstraint( } - unsafe void DoJob(ref AnalysisRegion region, int workerIndex, IUnmanagedMemoryPool pool) + unsafe void DoJob(ref AnalysisRegion region, int workerIndex, BufferPool pool) { ref var compressions = ref this.workerCompressions[workerIndex]; ref var batch = ref Solver.ActiveSet.Batches[nextBatchIndex]; diff --git a/BepuPhysics/CollidableProperty.cs b/BepuPhysics/CollidableProperty.cs index 7b4867c4a..bdffeef0a 100644 --- a/BepuPhysics/CollidableProperty.cs +++ b/BepuPhysics/CollidableProperty.cs @@ -19,7 +19,7 @@ public class CollidableProperty : IDisposable where T : unmanaged //Bodies and statics each have 'handle spaces', like namespaces. A body and static can have the same integer valued handle. //So, we need to have two different buffers for data. Simulation simulation; - IUnmanagedMemoryPool pool; + BufferPool pool; Buffer bodyData; Buffer staticData; @@ -29,7 +29,7 @@ public class CollidableProperty : IDisposable where T : unmanaged /// Constructs a new collection to store handle-aligned body properties. Assumes the Initialize function will be called later to provide the Bodies collection. /// /// Pool from which to pull internal resources. If null, uses the later Initialize-provided Bodies pool. - public CollidableProperty(IUnmanagedMemoryPool pool = null) + public CollidableProperty(BufferPool pool = null) { this.pool = pool; } @@ -39,7 +39,7 @@ public CollidableProperty(IUnmanagedMemoryPool pool = null) /// /// Simulation to track. /// Pool from which to pull internal resources. If null, uses the Simulation pool. - public CollidableProperty(Simulation simulation, IUnmanagedMemoryPool pool = null) + public CollidableProperty(Simulation simulation, BufferPool pool = null) { this.simulation = simulation; this.pool = pool == null ? simulation.BufferPool : pool; diff --git a/BepuPhysics/Collidables/BigCompound.cs b/BepuPhysics/Collidables/BigCompound.cs index 9cfc3018b..c0050b852 100644 --- a/BepuPhysics/Collidables/BigCompound.cs +++ b/BepuPhysics/Collidables/BigCompound.cs @@ -33,7 +33,7 @@ public struct BigCompound : ICompoundShape /// Shapes set in which child shapes are allocated. /// Pool to use to allocate acceleration structures. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public BigCompound(Buffer children, Shapes shapes, IUnmanagedMemoryPool pool) + public BigCompound(Buffer children, Shapes shapes, BufferPool pool) { Debug.Assert(children.Length > 0, "Compounds must have a nonzero number of children."); Debug.Assert(Compound.ValidateChildIndices(ref children, shapes), "Children must all be convex."); @@ -139,7 +139,7 @@ public unsafe void RayTest(in RigidPose pose, ref RaySource rays } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ShapeBatch CreateShapeBatch(IUnmanagedMemoryPool pool, int initialCapacity, Shapes shapes) + public ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapes) { return new CompoundShapeBatch(pool, initialCapacity, shapes); } @@ -163,9 +163,9 @@ public ref CompoundChild GetChild(int compoundChildIndex) /// Pool to use to resize the compound's children buffer if necessary. /// Shapes collection containing the compound's children. /// This function keeps the in a valid state, but significant changes over time may degrade the tree's quality and result in reduced collision/query performance. - /// If this happens, consider calling with a refinementIndex that changes with each call (to prioritize different parts of the tree). + /// If this happens, consider calling with a refinementIndex that changes with each call (to prioritize different parts of the tree). /// Incrementing a counter with each call would work fine. The ideal frequency of refinement depends on the kind of modifications being made, but it's likely to be rare. - public void Add(CompoundChild child, IUnmanagedMemoryPool pool, Shapes shapes) + public void Add(CompoundChild child, BufferPool pool, Shapes shapes) { pool.Resize(ref Children, Children.Length + 1, Children.Length); Children[^1] = child; @@ -180,9 +180,9 @@ public void Add(CompoundChild child, IUnmanagedMemoryPool pool, Shapes shapes) /// Index of the child to remove from the compound. /// Pool to use to resize the compound's children buffer if necessary. /// This function keeps the in a valid state, but significant changes over time may degrade the tree's quality and result in reduced collision/query performance. - /// If this happens, consider calling with a refinementIndex that changes with each call (to prioritize different parts of the tree). + /// If this happens, consider calling with a refinementIndex that changes with each call (to prioritize different parts of the tree). /// Incrementing a counter with each call would work fine. The ideal frequency of refinement depends on the kind of modifications being made, but it's likely to be rare. - public void RemoveAt(int childIndex, IUnmanagedMemoryPool pool) + public void RemoveAt(int childIndex, BufferPool pool) { var movedChildIndex = Tree.RemoveAt(childIndex); if (movedChildIndex >= 0) @@ -196,7 +196,7 @@ public void RemoveAt(int childIndex, IUnmanagedMemoryPool pool) unsafe struct Enumerator : IBreakableForEach where TSubpairOverlaps : ICollisionTaskSubpairOverlaps { - public IUnmanagedMemoryPool Pool; + public BufferPool Pool; public void* Overlaps; [MethodImpl(MethodImplOptions.AggressiveInlining)] public bool LoopBody(int i) @@ -206,7 +206,7 @@ public bool LoopBody(int i) } } - public unsafe void FindLocalOverlaps(ref Buffer pairs, IUnmanagedMemoryPool pool, Shapes shapes, ref TOverlaps overlaps) + public unsafe void FindLocalOverlaps(ref Buffer pairs, BufferPool pool, Shapes shapes, ref TOverlaps overlaps) where TOverlaps : struct, ICollisionTaskOverlaps where TSubpairOverlaps : struct, ICollisionTaskSubpairOverlaps { @@ -225,7 +225,7 @@ public unsafe void FindLocalOverlaps(ref Buffer : ISweepLeafTester where TOverlaps : ICollisionTaskSubpairOverlaps { - public IUnmanagedMemoryPool Pool; + public BufferPool Pool; public void* Overlaps; [MethodImpl(MethodImplOptions.AggressiveInlining)] public void TestLeaf(int leafIndex, ref float maximumT) @@ -233,7 +233,7 @@ public void TestLeaf(int leafIndex, ref float maximumT) Unsafe.AsRef(Overlaps).Allocate(Pool) = leafIndex; } } - public unsafe void FindLocalOverlaps(Vector3 min, Vector3 max, Vector3 sweep, float maximumT, IUnmanagedMemoryPool pool, Shapes shapes, void* overlaps) + public unsafe void FindLocalOverlaps(Vector3 min, Vector3 max, Vector3 sweep, float maximumT, BufferPool pool, Shapes shapes, void* overlaps) where TOverlaps : ICollisionTaskSubpairOverlaps { SweepLeafTester enumerator; @@ -268,7 +268,7 @@ public BodyInertia ComputeInertia(Span childMasses, Shapes shapes, out Ve return bodyInertia; } - public void Dispose(IUnmanagedMemoryPool bufferPool) + public void Dispose(BufferPool bufferPool) { bufferPool.Return(ref Children); Tree.Dispose(bufferPool); diff --git a/BepuPhysics/Collidables/BoundingBoxBatcher.cs b/BepuPhysics/Collidables/BoundingBoxBatcher.cs index 4ee8270c8..f4998e6e5 100644 --- a/BepuPhysics/Collidables/BoundingBoxBatcher.cs +++ b/BepuPhysics/Collidables/BoundingBoxBatcher.cs @@ -82,7 +82,7 @@ public struct BoundingBoxBatch public int Count; public bool Allocated => ShapeIndices.Allocated; - public BoundingBoxBatch(IUnmanagedMemoryPool pool, int initialCapacity) + public BoundingBoxBatch(BufferPool pool, int initialCapacity) { pool.Take(initialCapacity, out ShapeIndices); pool.Take(initialCapacity, out Continuations); @@ -99,7 +99,7 @@ internal void Add(int shapeIndex, RigidPose pose, BodyVelocity velocity, BoundsC Count++; } - public void Dispose(IUnmanagedMemoryPool pool) + public void Dispose(BufferPool pool) { if (Allocated) { @@ -114,7 +114,7 @@ public void Dispose(IUnmanagedMemoryPool pool) public struct BoundingBoxBatcher { - internal IUnmanagedMemoryPool pool; + internal BufferPool pool; internal Shapes shapes; internal Bodies bodies; internal BroadPhase broadPhase; @@ -129,7 +129,7 @@ public struct BoundingBoxBatcher /// public const int CollidablesPerFlush = 16; - public unsafe BoundingBoxBatcher(Bodies bodies, Shapes shapes, BroadPhase broadPhase, IUnmanagedMemoryPool pool, float dt) + public unsafe BoundingBoxBatcher(Bodies bodies, Shapes shapes, BroadPhase broadPhase, BufferPool pool, float dt) { this.bodies = bodies; this.shapes = shapes; diff --git a/BepuPhysics/Collidables/Box.cs b/BepuPhysics/Collidables/Box.cs index 320c4e0bd..a7f989b90 100644 --- a/BepuPhysics/Collidables/Box.cs +++ b/BepuPhysics/Collidables/Box.cs @@ -162,7 +162,7 @@ public readonly BodyInertia ComputeInertia(float mass) return inertia; } - public readonly ShapeBatch CreateShapeBatch(IUnmanagedMemoryPool pool, int initialCapacity, Shapes shapeBatches) + public readonly ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches) { return new ConvexShapeBatch(pool, initialCapacity); } diff --git a/BepuPhysics/Collidables/Capsule.cs b/BepuPhysics/Collidables/Capsule.cs index abf95e507..38865fdd1 100644 --- a/BepuPhysics/Collidables/Capsule.cs +++ b/BepuPhysics/Collidables/Capsule.cs @@ -179,7 +179,7 @@ public readonly BodyInertia ComputeInertia(float mass) return inertia; } - public readonly ShapeBatch CreateShapeBatch(IUnmanagedMemoryPool pool, int initialCapacity, Shapes shapeBatches) + public readonly ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches) { return new ConvexShapeBatch(pool, initialCapacity); } diff --git a/BepuPhysics/Collidables/Compound.cs b/BepuPhysics/Collidables/Compound.cs index 4129544a5..58c1ccdb2 100644 --- a/BepuPhysics/Collidables/Compound.cs +++ b/BepuPhysics/Collidables/Compound.cs @@ -250,7 +250,7 @@ public unsafe void RayTest(in RigidPose pose, ref RaySource rays } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ShapeBatch CreateShapeBatch(IUnmanagedMemoryPool pool, int initialCapacity, Shapes shapes) + public ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapes) { return new CompoundShapeBatch(pool, initialCapacity, shapes); } @@ -272,7 +272,7 @@ public ref CompoundChild GetChild(int compoundChildIndex) /// /// Child to add to the compound. /// Pool to use to resize the compound's children buffer if necessary. - public void Add(CompoundChild child, IUnmanagedMemoryPool pool) + public void Add(CompoundChild child, BufferPool pool) { pool.Resize(ref Children, Children.Length + 1, Children.Length); Children[^1] = child; @@ -283,7 +283,7 @@ public void Add(CompoundChild child, IUnmanagedMemoryPool pool) /// /// Index of the child to remove from the compound. /// Pool to use to resize the compound's children buffer if necessary. - public void RemoveAt(int childIndex, IUnmanagedMemoryPool pool) + public void RemoveAt(int childIndex, BufferPool pool) { var lastIndex = Children.Length - 1; if (childIndex < lastIndex) @@ -295,7 +295,7 @@ public void RemoveAt(int childIndex, IUnmanagedMemoryPool pool) } - public unsafe void FindLocalOverlaps(ref Buffer pairs, IUnmanagedMemoryPool pool, Shapes shapes, ref TOverlaps overlaps) + public unsafe void FindLocalOverlaps(ref Buffer pairs, BufferPool pool, Shapes shapes, ref TOverlaps overlaps) where TOverlaps : struct, ICollisionTaskOverlaps where TSubpairOverlaps : struct, ICollisionTaskSubpairOverlaps { @@ -320,7 +320,7 @@ public unsafe void FindLocalOverlaps(ref Buffer(Vector3 min, Vector3 max, Vector3 sweep, float maximumT, IUnmanagedMemoryPool pool, Shapes shapes, void* overlapsPointer) + public unsafe void FindLocalOverlaps(Vector3 min, Vector3 max, Vector3 sweep, float maximumT, BufferPool pool, Shapes shapes, void* overlapsPointer) where TOverlaps : ICollisionTaskSubpairOverlaps { Tree.ConvertBoxToCentroidWithExtent(min, max, out var sweepOrigin, out var expansion); @@ -362,7 +362,7 @@ public BodyInertia ComputeInertia(Span childMasses, Shapes shapes, out Ve return CompoundBuilder.ComputeInertia(Children, childMasses, shapes, out centerOfMass); } - public void Dispose(IUnmanagedMemoryPool bufferPool) + public void Dispose(BufferPool bufferPool) { bufferPool.Return(ref Children); } diff --git a/BepuPhysics/Collidables/ConvexHull.cs b/BepuPhysics/Collidables/ConvexHull.cs index 8c2084bcc..0e779a127 100644 --- a/BepuPhysics/Collidables/ConvexHull.cs +++ b/BepuPhysics/Collidables/ConvexHull.cs @@ -58,7 +58,7 @@ public struct ConvexHull : IConvexShape /// Points to compute the convex hull of. /// Pool in which to allocate the convex hull and any temporary resources needed to compute the hull. /// Computed center of the convex hull before the hull was recentered. - public ConvexHull(Span points, IUnmanagedMemoryPool pool, out Vector3 center) + public ConvexHull(Span points, BufferPool pool, out Vector3 center) { ConvexHullHelper.CreateShape(points, pool, out center, out this); } @@ -193,7 +193,7 @@ public readonly BodyInertia ComputeInertia(float mass) return inertia; } - public readonly ShapeBatch CreateShapeBatch(IUnmanagedMemoryPool pool, int initialCapacity, Shapes shapeBatches) + public readonly ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches) { return new ConvexHullShapeBatch(pool, initialCapacity); } @@ -269,7 +269,7 @@ public readonly bool RayTest(in RigidPose pose, Vector3 origin, Vector3 directio return true; } } - public void Dispose(IUnmanagedMemoryPool bufferPool) + public void Dispose(BufferPool bufferPool) { bufferPool.Return(ref Points); bufferPool.Return(ref BoundingPlanes); diff --git a/BepuPhysics/Collidables/ConvexHullHelper.cs b/BepuPhysics/Collidables/ConvexHullHelper.cs index fa2c89e77..590f21f92 100644 --- a/BepuPhysics/Collidables/ConvexHullHelper.cs +++ b/BepuPhysics/Collidables/ConvexHullHelper.cs @@ -72,7 +72,7 @@ public void GetFace(int faceIndex, out HullFace face) face.OriginalVertexMapping = OriginalVertexMapping; } - public void Dispose(IUnmanagedMemoryPool pool) + public void Dispose(BufferPool pool) { pool.Return(ref OriginalVertexMapping); pool.Return(ref FaceVertexIndices); @@ -489,14 +489,14 @@ struct EdgeToTest } - static void AddFace(ref QuickList faces, IUnmanagedMemoryPool pool, Vector3 normal, QuickList vertexIndices) + static void AddFace(ref QuickList faces, BufferPool pool, Vector3 normal, QuickList vertexIndices) { ref var face = ref faces.Allocate(pool); face = new EarlyFace { Normal = normal, VertexIndices = new QuickList(vertexIndices.Count, pool) }; face.VertexIndices.AddRangeUnsafely(vertexIndices); } - static void AddFaceToEdgesAndTestList(IUnmanagedMemoryPool pool, + static void AddFaceToEdgesAndTestList(BufferPool pool, ref QuickList reducedFaceIndices, ref QuickList edgesToTest, ref QuickDictionary facesForEdges, @@ -534,7 +534,7 @@ static void AddFaceToEdgesAndTestList(IUnmanagedMemoryPool pool, } } - static void AddIfNotPresent(ref QuickList list, int value, IUnmanagedMemoryPool pool) + static void AddIfNotPresent(ref QuickList list, int value, BufferPool pool) { if (!list.Contains(value)) list.Allocate(pool) = value; @@ -600,7 +600,7 @@ static void AddIfNotPresent(ref QuickList list, int value, IUnmanagedMemory ///// Point set to compute the convex hull of. ///// Buffer pool to pull memory from when creating the hull. ///// Convex hull of the input point set. - //public static void ComputeHull(Span points, IUnmanagedMemoryPool pool, out HullData hullData) + //public static void ComputeHull(Span points, BufferPool pool, out HullData hullData) //{ // ComputeHull(points, pool, out hullData, out _); //} @@ -612,7 +612,7 @@ static void AddIfNotPresent(ref QuickList list, int value, IUnmanagedMemory /// Point set to compute the convex hull of. /// Buffer pool to pull memory from when creating the hull. /// Convex hull of the input point set. - public static void ComputeHull(Span points, IUnmanagedMemoryPool pool, out HullData hullData)//, out List steps) + public static void ComputeHull(Span points, BufferPool pool, out HullData hullData)//, out List steps) { //steps = new List(); if (points.Length <= 0) @@ -1031,7 +1031,7 @@ public static void ComputeHull(Span points, IUnmanagedMemoryPool pool, /// Pool used to allocate resources for the hullShape. /// Convex hull shape created from the input data. /// Computed center of mass of the convex hull before its points were recentered onto the origin. - public static void CreateShape(Span points, HullData hullData, IUnmanagedMemoryPool pool, out Vector3 center, out ConvexHull hullShape) + public static void CreateShape(Span points, HullData hullData, BufferPool pool, out Vector3 center, out ConvexHull hullShape) { hullShape = default; if (hullData.OriginalVertexMapping.Length < 3) @@ -1146,7 +1146,7 @@ public static void CreateShape(Span points, HullData hullData, IUnmanag /// Intermediate hull data that got processed into the convex hull. /// Computed center of mass of the convex hull before its points were recentered onto the origin. /// Convex hull shape of the input point set. - public static void CreateShape(Span points, IUnmanagedMemoryPool pool, out HullData hullData, out Vector3 center, out ConvexHull convexHull) + public static void CreateShape(Span points, BufferPool pool, out HullData hullData, out Vector3 center, out ConvexHull convexHull) { ComputeHull(points, pool, out hullData); CreateShape(points, hullData, pool, out center, out convexHull); @@ -1159,7 +1159,7 @@ public static void CreateShape(Span points, IUnmanagedMemoryPool pool, /// Buffer pool used for temporary allocations and the output data structures. /// Computed center of mass of the convex hull before its points were recentered onto the origin. /// Convex hull shape of the input point set. - public static void CreateShape(Span points, IUnmanagedMemoryPool pool, out Vector3 center, out ConvexHull convexHull) + public static void CreateShape(Span points, BufferPool pool, out Vector3 center, out ConvexHull convexHull) { ComputeHull(points, pool, out var hullData); CreateShape(points, hullData, pool, out center, out convexHull); @@ -1224,7 +1224,7 @@ public static void CreateTransformedCopy(in ConvexHull source, in Matrix3x3 tran /// Transform to apply to the hull points. /// Pool from which to allocate the new hull's points and bounding planes buffers. /// Target convex hull to copy into. FaceVertexIndices and FaceToVertexIndicesStart buffers are reused from the source. - public static void CreateTransformedShallowCopy(in ConvexHull source, in Matrix3x3 transform, IUnmanagedMemoryPool pool, out ConvexHull target) + public static void CreateTransformedShallowCopy(in ConvexHull source, in Matrix3x3 transform, BufferPool pool, out ConvexHull target) { pool.Take(source.Points.Length, out target.Points); pool.Take(source.BoundingPlanes.Length, out target.BoundingPlanes); @@ -1240,7 +1240,7 @@ public static void CreateTransformedShallowCopy(in ConvexHull source, in Matrix3 /// Transform to apply to the hull points. /// Pool from which to allocate the new hull's buffers. /// Target convex hull to copy into. - public static void CreateTransformedCopy(in ConvexHull source, in Matrix3x3 transform, IUnmanagedMemoryPool pool, out ConvexHull target) + public static void CreateTransformedCopy(in ConvexHull source, in Matrix3x3 transform, BufferPool pool, out ConvexHull target) { pool.Take(source.Points.Length, out target.Points); pool.Take(source.BoundingPlanes.Length, out target.BoundingPlanes); diff --git a/BepuPhysics/Collidables/Cylinder.cs b/BepuPhysics/Collidables/Cylinder.cs index 287d9970c..6dee60c4c 100644 --- a/BepuPhysics/Collidables/Cylinder.cs +++ b/BepuPhysics/Collidables/Cylinder.cs @@ -176,7 +176,7 @@ public readonly BodyInertia ComputeInertia(float mass) return inertia; } - public readonly ShapeBatch CreateShapeBatch(IUnmanagedMemoryPool pool, int initialCapacity, Shapes shapeBatches) + public readonly ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches) { return new ConvexShapeBatch(pool, initialCapacity); } diff --git a/BepuPhysics/Collidables/IShape.cs b/BepuPhysics/Collidables/IShape.cs index 64d4b4c56..b86b28c24 100644 --- a/BepuPhysics/Collidables/IShape.cs +++ b/BepuPhysics/Collidables/IShape.cs @@ -28,7 +28,7 @@ public interface IShape /// Shape batch for the shape type. /// This is typically used internally to initialize new shape collections in response to shapes being added. It is not likely to be useful outside of the engine. /// Ideally, this would be implemented as a static abstract, but those aren't available yet. - ShapeBatch CreateShapeBatch(IUnmanagedMemoryPool pool, int initialCapacity, Shapes shapeBatches); + ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches); } //Note that the following bounds functions require only an orientation because the effect of the position on the bounding box is the same for all shapes. @@ -144,7 +144,7 @@ public interface ICompoundShape : IShape, IBoundsQueryableCompound /// Returns all resources used by the shape instance to the given pool. /// /// Pool to return shape resources to. - void Dispose(IUnmanagedMemoryPool pool); + void Dispose(BufferPool pool); } /// @@ -208,7 +208,7 @@ public interface IHomogeneousCompoundShape : IShap /// Returns all resources used by the shape instance to the given pool. /// /// Pool to return shape resources to. - void Dispose(IUnmanagedMemoryPool pool); + void Dispose(BufferPool pool); } /// diff --git a/BepuPhysics/Collidables/Mesh.cs b/BepuPhysics/Collidables/Mesh.cs index 61bb76677..20deb5f93 100644 --- a/BepuPhysics/Collidables/Mesh.cs +++ b/BepuPhysics/Collidables/Mesh.cs @@ -11,7 +11,7 @@ namespace BepuPhysics.Collidables { public unsafe struct ShapeTreeOverlapEnumerator : IBreakableForEach where TSubpairOverlaps : ICollisionTaskSubpairOverlaps { - public IUnmanagedMemoryPool Pool; + public BufferPool Pool; public void* Overlaps; [MethodImpl(MethodImplOptions.AggressiveInlining)] public bool LoopBody(int i) @@ -22,7 +22,7 @@ public bool LoopBody(int i) } public unsafe struct ShapeTreeSweepLeafTester : ISweepLeafTester where TOverlaps : ICollisionTaskSubpairOverlaps { - public IUnmanagedMemoryPool Pool; + public BufferPool Pool; public void* Overlaps; [MethodImpl(MethodImplOptions.AggressiveInlining)] public void TestLeaf(int leafIndex, ref float maximumT) @@ -72,7 +72,7 @@ public Vector3 Scale /// Scale to apply to all vertices at runtime. /// Note that the scale is not baked into the triangles or acceleration structure; the same set of triangles and acceleration structure can be used across multiple Mesh instances with different scales. /// Pool used to allocate acceleration structures. - public Mesh(Buffer triangles, Vector3 scale, IUnmanagedMemoryPool pool) : this() + public Mesh(Buffer triangles, Vector3 scale, BufferPool pool) : this() { Triangles = triangles; Tree = new Tree(pool, triangles.Length); @@ -94,7 +94,7 @@ public Mesh(Buffer triangles, Vector3 scale, IUnmanagedMemoryPool pool /// /// Data to load the mesh from. /// Pool to create the mesh with. - public unsafe Mesh(Span data, IUnmanagedMemoryPool pool) + public unsafe Mesh(Span data, BufferPool pool) { if (data.Length < 16) throw new ArgumentException("Data is not large enough to contain a header."); @@ -192,7 +192,7 @@ public readonly void ComputeBounds(Quaternion orientation, out Vector3 min, out } } - public readonly ShapeBatch CreateShapeBatch(IUnmanagedMemoryPool pool, int initialCapacity, Shapes shapeBatches) + public readonly ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches) { return new HomogeneousCompoundShapeBatch(pool, initialCapacity); } @@ -273,7 +273,7 @@ public readonly unsafe void RayTest(in RigidPose pose, ref RaySo hitHandler = leafTester.HitHandler; } - public readonly unsafe void FindLocalOverlaps(ref Buffer pairs, IUnmanagedMemoryPool pool, Shapes shapes, ref TOverlaps overlaps) + public readonly unsafe void FindLocalOverlaps(ref Buffer pairs, BufferPool pool, Shapes shapes, ref TOverlaps overlaps) where TOverlaps : struct, ICollisionTaskOverlaps where TSubpairOverlaps : struct, ICollisionTaskSubpairOverlaps { @@ -294,7 +294,7 @@ public readonly unsafe void FindLocalOverlaps(ref B } } - public readonly unsafe void FindLocalOverlaps(Vector3 min, Vector3 max, Vector3 sweep, float maximumT, IUnmanagedMemoryPool pool, Shapes shapes, void* overlaps) + public readonly unsafe void FindLocalOverlaps(Vector3 min, Vector3 max, Vector3 sweep, float maximumT, BufferPool pool, Shapes shapes, void* overlaps) where TOverlaps : ICollisionTaskSubpairOverlaps { var scaledMin = min * inverseScale; @@ -470,7 +470,7 @@ public readonly Vector3 ComputeOpenCenterOfMass() /// Returns the mesh's resources to a buffer pool. /// /// Pool to return the mesh's resources to. - public void Dispose(IUnmanagedMemoryPool bufferPool) + public void Dispose(BufferPool bufferPool) { bufferPool.Return(ref Triangles); Tree.Dispose(bufferPool); diff --git a/BepuPhysics/Collidables/Shapes.cs b/BepuPhysics/Collidables/Shapes.cs index f990d1f2a..efbdb6982 100644 --- a/BepuPhysics/Collidables/Shapes.cs +++ b/BepuPhysics/Collidables/Shapes.cs @@ -18,7 +18,7 @@ public abstract class ShapeBatch /// Gets the number of shapes that the batch can currently hold without resizing. /// public int Capacity { get { return shapesData.Length / shapeDataSize; } } - protected IUnmanagedMemoryPool pool; + protected BufferPool pool; protected IdPool idPool; /// /// Gets the type id of the shape type in this batch. @@ -33,21 +33,21 @@ public abstract class ShapeBatch /// public int ShapeDataSize { get { return shapeDataSize; } } - protected abstract void Dispose(int index, IUnmanagedMemoryPool pool); - protected abstract void RemoveAndDisposeChildren(int index, Shapes shapes, IUnmanagedMemoryPool pool); + protected abstract void Dispose(int index, BufferPool pool); + protected abstract void RemoveAndDisposeChildren(int index, Shapes shapes, BufferPool pool); public void Remove(int index) { idPool.Return(index, pool); } - public void RemoveAndDispose(int index, IUnmanagedMemoryPool pool) + public void RemoveAndDispose(int index, BufferPool pool) { Dispose(index, pool); Remove(index); } - public void RecursivelyRemoveAndDispose(int index, Shapes shapes, IUnmanagedMemoryPool pool) + public void RecursivelyRemoveAndDispose(int index, Shapes shapes, BufferPool pool) { RemoveAndDisposeChildren(index, shapes, pool); RemoveAndDispose(index, pool); @@ -133,7 +133,7 @@ public abstract class ShapeBatch : ShapeBatch where TShape : unmanaged, /// Reference to the shape at the given index. public ref TShape this[int shapeIndex] { get { return ref shapes[shapeIndex]; } } - protected ShapeBatch(IUnmanagedMemoryPool pool, int initialShapeCount) + protected ShapeBatch(BufferPool pool, int initialShapeCount) { this.pool = pool; TypeId = default(TShape).TypeId; @@ -231,16 +231,16 @@ public class ConvexShapeBatch : ShapeBatch, IConvexS where TShape : unmanaged, IConvexShape where TShapeWide : unmanaged, IShapeWide { - public ConvexShapeBatch(IUnmanagedMemoryPool pool, int initialShapeCount) : base(pool, initialShapeCount) + public ConvexShapeBatch(BufferPool pool, int initialShapeCount) : base(pool, initialShapeCount) { } - protected override void Dispose(int index, IUnmanagedMemoryPool pool) + protected override void Dispose(int index, BufferPool pool) { //Most convex shapes with an associated Wide type doesn't have any internal resources to dispose. } - protected override void RemoveAndDisposeChildren(int index, Shapes shapes, IUnmanagedMemoryPool pool) + protected override void RemoveAndDisposeChildren(int index, Shapes shapes, BufferPool pool) { //And they don't have any children. } @@ -283,11 +283,11 @@ public unsafe override void RayTest(int index, in RigidPose pose public class ConvexHullShapeBatch : ConvexShapeBatch { - public ConvexHullShapeBatch(IUnmanagedMemoryPool pool, int initialShapeCount) : base(pool, initialShapeCount) + public ConvexHullShapeBatch(BufferPool pool, int initialShapeCount) : base(pool, initialShapeCount) { } - protected override void Dispose(int index, IUnmanagedMemoryPool pool) + protected override void Dispose(int index, BufferPool pool) { shapes[index].Dispose(pool); } @@ -298,17 +298,17 @@ public class HomogeneousCompoundShapeBatch where TChildShape : unmanaged, IConvexShape where TChildShapeWide : unmanaged, IShapeWide { - public HomogeneousCompoundShapeBatch(IUnmanagedMemoryPool pool, int initialShapeCount) : base(pool, initialShapeCount) + public HomogeneousCompoundShapeBatch(BufferPool pool, int initialShapeCount) : base(pool, initialShapeCount) { Compound = true; } - protected override void Dispose(int index, IUnmanagedMemoryPool pool) + protected override void Dispose(int index, BufferPool pool) { shapes[index].Dispose(pool); } - protected override void RemoveAndDisposeChildren(int index, Shapes shapes, IUnmanagedMemoryPool pool) + protected override void RemoveAndDisposeChildren(int index, Shapes shapes, BufferPool pool) { //Meshes and other single-type containers don't have any shape-registered children. } @@ -337,18 +337,18 @@ public class CompoundShapeBatch : ShapeBatch where TShape : unma { Shapes shapeBatches; - public CompoundShapeBatch(IUnmanagedMemoryPool pool, int initialShapeCount, Shapes shapeBatches) : base(pool, initialShapeCount) + public CompoundShapeBatch(BufferPool pool, int initialShapeCount, Shapes shapeBatches) : base(pool, initialShapeCount) { this.shapeBatches = shapeBatches; Compound = true; } - protected override void Dispose(int index, IUnmanagedMemoryPool pool) + protected override void Dispose(int index, BufferPool pool) { shapes[index].Dispose(pool); } - protected override void RemoveAndDisposeChildren(int index, Shapes shapes, IUnmanagedMemoryPool pool) + protected override void RemoveAndDisposeChildren(int index, Shapes shapes, BufferPool pool) { ref var shape = ref this.shapes[index]; for (int i = 0; i < shape.ChildCount; ++i) @@ -393,10 +393,10 @@ public class Shapes public int InitialCapacityPerTypeBatch { get; set; } public ShapeBatch this[int typeIndex] => batches[typeIndex]; - IUnmanagedMemoryPool pool; + BufferPool pool; - public Shapes(IUnmanagedMemoryPool pool, int initialCapacityPerTypeBatch) + public Shapes(BufferPool pool, int initialCapacityPerTypeBatch) { InitialCapacityPerTypeBatch = initialCapacityPerTypeBatch; //This list pretty much will never resize unless something really strange happens, and since batches use virtual calls, we have to allow storage of reference types. @@ -467,7 +467,7 @@ public TypedIndex Add(in TShape shape) where TShape : unmanaged, IShape /// /// Index of the shape to remove. /// Pool to return all shape resources to. - public void RecursivelyRemoveAndDispose(TypedIndex shapeIndex, IUnmanagedMemoryPool pool) + public void RecursivelyRemoveAndDispose(TypedIndex shapeIndex, BufferPool pool) { if (shapeIndex.Exists) { @@ -481,7 +481,7 @@ public void RecursivelyRemoveAndDispose(TypedIndex shapeIndex, IUnmanagedMemoryP /// /// Index of the shape to remove. /// Pool to return all shape resources to. - public void RemoveAndDispose(TypedIndex shapeIndex, IUnmanagedMemoryPool pool) + public void RemoveAndDispose(TypedIndex shapeIndex, BufferPool pool) { if (shapeIndex.Exists) { diff --git a/BepuPhysics/Collidables/Sphere.cs b/BepuPhysics/Collidables/Sphere.cs index 3e2678125..21f39c092 100644 --- a/BepuPhysics/Collidables/Sphere.cs +++ b/BepuPhysics/Collidables/Sphere.cs @@ -108,7 +108,7 @@ public readonly BodyInertia ComputeInertia(float mass) return inertia; } - public readonly ShapeBatch CreateShapeBatch(IUnmanagedMemoryPool pool, int initialCapacity, Shapes shapes) + public readonly ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapes) { return new ConvexShapeBatch(pool, initialCapacity); } diff --git a/BepuPhysics/Collidables/Triangle.cs b/BepuPhysics/Collidables/Triangle.cs index b91f3693b..998b37873 100644 --- a/BepuPhysics/Collidables/Triangle.cs +++ b/BepuPhysics/Collidables/Triangle.cs @@ -118,7 +118,7 @@ public readonly BodyInertia ComputeInertia(float mass) return inertia; } - public readonly ShapeBatch CreateShapeBatch(IUnmanagedMemoryPool pool, int initialCapacity, Shapes shapeBatches) + public readonly ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches) { return new ConvexShapeBatch(pool, initialCapacity); } diff --git a/BepuPhysics/CollisionDetection/CollisionBatcher.cs b/BepuPhysics/CollisionDetection/CollisionBatcher.cs index e6b3169c7..ce7be21c4 100644 --- a/BepuPhysics/CollisionDetection/CollisionBatcher.cs +++ b/BepuPhysics/CollisionDetection/CollisionBatcher.cs @@ -33,7 +33,7 @@ struct CollisionBatch public struct CollisionBatcher where TCallbacks : struct, ICollisionCallbacks { - public IUnmanagedMemoryPool Pool; + public BufferPool Pool; public Shapes Shapes; CollisionTaskRegistry typeMatrix; public TCallbacks Callbacks; @@ -54,7 +54,7 @@ public struct CollisionBatcher where TCallbacks : struct, ICollision public BatcherContinuations MeshReductions; public BatcherContinuations CompoundMeshReductions; - public unsafe CollisionBatcher(IUnmanagedMemoryPool pool, Shapes shapes, CollisionTaskRegistry collisionTypeMatrix, float dt, TCallbacks callbacks) + public unsafe CollisionBatcher(BufferPool pool, Shapes shapes, CollisionTaskRegistry collisionTypeMatrix, float dt, TCallbacks callbacks) { Pool = pool; Shapes = shapes; diff --git a/BepuPhysics/CollisionDetection/CollisionBatcherContinuations.cs b/BepuPhysics/CollisionDetection/CollisionBatcherContinuations.cs index e074cf798..acade2a18 100644 --- a/BepuPhysics/CollisionDetection/CollisionBatcherContinuations.cs +++ b/BepuPhysics/CollisionDetection/CollisionBatcherContinuations.cs @@ -17,7 +17,7 @@ public interface ICollisionTestContinuation /// /// Number of subpair slots to include in the continuation. /// Pool to take resources from. - void Create(int slots, IUnmanagedMemoryPool pool); + void Create(int slots, BufferPool pool); /// /// Handles what to do next when the child pair has finished execution and the resulting manifold is available. @@ -140,7 +140,7 @@ public struct BatcherContinuations where T : unmanaged, ICollisionTestContinu public IdPool IdPool; const int InitialCapacity = 64; - public ref T CreateContinuation(int slotsInContinuation, IUnmanagedMemoryPool pool, out int index) + public ref T CreateContinuation(int slotsInContinuation, BufferPool pool, out int index) { if (!Continuations.Allocated) { @@ -186,7 +186,7 @@ public unsafe void ContributeUntestedChildToContinuation(ref PairCon } - internal void Dispose(IUnmanagedMemoryPool pool) + internal void Dispose(BufferPool pool) { if (Continuations.Allocated) { diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairCollisionTask.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairCollisionTask.cs index 9bced1c14..f0052f682 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairCollisionTask.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairCollisionTask.cs @@ -11,7 +11,7 @@ namespace BepuPhysics.CollisionDetection.CollisionTasks { public interface ICompoundPairOverlapFinder { - void FindLocalOverlaps(ref Buffer pairs, int pairCount, IUnmanagedMemoryPool pool, Shapes shapes, float dt, out CompoundPairOverlaps overlaps); + void FindLocalOverlaps(ref Buffer pairs, int pairCount, BufferPool pool, Shapes shapes, float dt, out CompoundPairOverlaps overlaps); } public unsafe interface ICompoundPairContinuationHandler where TContinuation : struct, ICollisionTestContinuation diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlapFinder.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlapFinder.cs index 53dbb2c87..f48c26f67 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlapFinder.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlapFinder.cs @@ -20,7 +20,7 @@ public struct CompoundPairOverlapFinder : ICompoundPairO where TCompoundB : struct, IBoundsQueryableCompound { - public unsafe void FindLocalOverlaps(ref Buffer pairs, int pairCount, IUnmanagedMemoryPool pool, Shapes shapes, float dt, out CompoundPairOverlaps overlaps) + public unsafe void FindLocalOverlaps(ref Buffer pairs, int pairCount, BufferPool pool, Shapes shapes, float dt, out CompoundPairOverlaps overlaps) { var totalCompoundChildCount = 0; for (int i = 0; i < pairCount; ++i) diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlaps.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlaps.cs index 0782e8cd8..268d22f86 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlaps.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlaps.cs @@ -12,7 +12,7 @@ namespace BepuPhysics.CollisionDetection.CollisionTasks { public interface ICollisionTaskSubpairOverlaps { - ref int Allocate(IUnmanagedMemoryPool pool); + ref int Allocate(BufferPool pool); } public interface ICollisionTaskOverlaps where TSubpairOverlaps : struct, ICollisionTaskSubpairOverlaps @@ -27,7 +27,7 @@ public unsafe struct ChildOverlapsCollection : ICollisionTaskSubpairOverlaps public int ChildIndex; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe ref int Allocate(IUnmanagedMemoryPool pool) + public unsafe ref int Allocate(BufferPool pool) { if (Overlaps.Length == Count) { @@ -37,7 +37,7 @@ public unsafe ref int Allocate(IUnmanagedMemoryPool pool) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Dispose(IUnmanagedMemoryPool pool) + public void Dispose(BufferPool pool) { if (Overlaps.Allocated) pool.Return(ref Overlaps); @@ -51,7 +51,7 @@ public struct CompoundPairOverlaps : ICollisionTaskOverlaps pairRegions; int pairCount; int subpairCount; - public CompoundPairOverlaps(IUnmanagedMemoryPool pool, int pairCapacity, int subpairCapacity) + public CompoundPairOverlaps(BufferPool pool, int pairCapacity, int subpairCapacity) { this.pairCount = 0; this.subpairCount = 0; @@ -84,7 +84,7 @@ public void GetPairOverlaps(int pairIndex, out Buffer p this.pairQueries.Slice(region.start, region.count, out pairQueries); } - public void Dispose(IUnmanagedMemoryPool pool) + public void Dispose(BufferPool pool) { for (int i = 0; i < subpairCount; ++i) { diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundOverlapFinder.cs b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundOverlapFinder.cs index e89898e02..f70657399 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundOverlapFinder.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundOverlapFinder.cs @@ -15,16 +15,16 @@ public unsafe struct OverlapQueryForPair public unsafe interface IBoundsQueryableCompound { - unsafe void FindLocalOverlaps(ref Buffer pairs, IUnmanagedMemoryPool pool, Shapes shapes, ref TOverlaps overlaps) + unsafe void FindLocalOverlaps(ref Buffer pairs, BufferPool pool, Shapes shapes, ref TOverlaps overlaps) where TOverlaps : struct, ICollisionTaskOverlaps where TSubpairOverlaps : struct, ICollisionTaskSubpairOverlaps; - unsafe void FindLocalOverlaps(Vector3 min, Vector3 max, Vector3 sweep, float maximumT, IUnmanagedMemoryPool pool, Shapes shapes, void* overlaps) + unsafe void FindLocalOverlaps(Vector3 min, Vector3 max, Vector3 sweep, float maximumT, BufferPool pool, Shapes shapes, void* overlaps) where TOverlaps : ICollisionTaskSubpairOverlaps; } public interface IConvexCompoundOverlapFinder { - void FindLocalOverlaps(ref Buffer pairs, int pairCount, IUnmanagedMemoryPool pool, Shapes shapes, float dt, out ConvexCompoundTaskOverlaps overlaps); + void FindLocalOverlaps(ref Buffer pairs, int pairCount, BufferPool pool, Shapes shapes, float dt, out ConvexCompoundTaskOverlaps overlaps); } public struct ConvexCompoundOverlapFinder : IConvexCompoundOverlapFinder @@ -32,7 +32,7 @@ public struct ConvexCompoundOverlapFinder : ICo where TConvexWide : struct, IShapeWide where TCompound : struct, IBoundsQueryableCompound { - public unsafe void FindLocalOverlaps(ref Buffer pairs, int pairCount, IUnmanagedMemoryPool pool, Shapes shapes, float dt, out ConvexCompoundTaskOverlaps overlaps) + public unsafe void FindLocalOverlaps(ref Buffer pairs, int pairCount, BufferPool pool, Shapes shapes, float dt, out ConvexCompoundTaskOverlaps overlaps) { overlaps = new ConvexCompoundTaskOverlaps(pool, pairCount); ref var pairsToTest = ref overlaps.subpairQueries; diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundTaskOverlaps.cs b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundTaskOverlaps.cs index 65f1f62f2..d814cc6e0 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundTaskOverlaps.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundTaskOverlaps.cs @@ -10,7 +10,7 @@ public unsafe struct ConvexCompoundOverlaps : ICollisionTaskSubpairOverlaps public int Count; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe ref int Allocate(IUnmanagedMemoryPool pool) + public unsafe ref int Allocate(BufferPool pool) { if (Overlaps.Length == Count) { @@ -20,7 +20,7 @@ public unsafe ref int Allocate(IUnmanagedMemoryPool pool) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Dispose(IUnmanagedMemoryPool pool) + public void Dispose(BufferPool pool) { if (Overlaps.Allocated) pool.Return(ref Overlaps); @@ -31,7 +31,7 @@ public struct ConvexCompoundTaskOverlaps : ICollisionTaskOverlaps overlaps; internal Buffer subpairQueries; - public ConvexCompoundTaskOverlaps(IUnmanagedMemoryPool pool, int pairCount) + public ConvexCompoundTaskOverlaps(BufferPool pool, int pairCount) { pool.Take(pairCount, out overlaps); pool.Take(pairCount, out subpairQueries); @@ -50,7 +50,7 @@ public ref OverlapQueryForPair GetQueryForPair(int pairIndex) return ref subpairQueries[pairIndex]; } - public void Dispose(IUnmanagedMemoryPool pool) + public void Dispose(BufferPool pool) { for (int i = 0; i < overlaps.Length; ++i) { diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/MeshPairOverlapFinder.cs b/BepuPhysics/CollisionDetection/CollisionTasks/MeshPairOverlapFinder.cs index 85e051a1a..73f356c1a 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/MeshPairOverlapFinder.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/MeshPairOverlapFinder.cs @@ -15,7 +15,7 @@ public struct MeshPairOverlapFinder : ICompoundPairOverlapFinder where TMeshB : struct, IHomogeneousCompoundShape { - public unsafe void FindLocalOverlaps(ref Buffer pairs, int pairCount, IUnmanagedMemoryPool pool, Shapes shapes, float dt, out CompoundPairOverlaps overlaps) + public unsafe void FindLocalOverlaps(ref Buffer pairs, int pairCount, BufferPool pool, Shapes shapes, float dt, out CompoundPairOverlaps overlaps) { var totalCompoundChildCount = 0; for (int i = 0; i < pairCount; ++i) diff --git a/BepuPhysics/CollisionDetection/CompoundMeshReduction.cs b/BepuPhysics/CollisionDetection/CompoundMeshReduction.cs index d420ac5bf..cc3769c85 100644 --- a/BepuPhysics/CollisionDetection/CompoundMeshReduction.cs +++ b/BepuPhysics/CollisionDetection/CompoundMeshReduction.cs @@ -27,7 +27,7 @@ public unsafe struct CompoundMeshReduction : ICollisionTestContinuation public Mesh* Mesh; //TODO: This is not flexible with respect to different mesh types. Not a problem right now, but it will be in the future. - public void Create(int childManifoldCount, IUnmanagedMemoryPool pool) + public void Create(int childManifoldCount, BufferPool pool) { Inner.Create(childManifoldCount, pool); } diff --git a/BepuPhysics/CollisionDetection/ConstraintRemover.cs b/BepuPhysics/CollisionDetection/ConstraintRemover.cs index b77ce6efc..44b2f6cff 100644 --- a/BepuPhysics/CollisionDetection/ConstraintRemover.cs +++ b/BepuPhysics/CollisionDetection/ConstraintRemover.cs @@ -50,7 +50,7 @@ struct RemovalCache //Storing this stuff by constraint batch is an option, but it would require more prefiltering that isn't free. int minimumCapacityPerBatch; - public RemovalCache(IUnmanagedMemoryPool pool, int batchCapacity, int minimumCapacityPerBatch) + public RemovalCache(BufferPool pool, int batchCapacity, int minimumCapacityPerBatch) { this.minimumCapacityPerBatch = minimumCapacityPerBatch; @@ -71,7 +71,7 @@ public int IndexOf(TypeBatchIndex typeBatchIndex) } return -1; } - public int AllocateSpaceForTargets(TypeBatchIndex typeBatchIndex, int constraintHandleCount, int perBodyRemovalCount, IUnmanagedMemoryPool pool) + public int AllocateSpaceForTargets(TypeBatchIndex typeBatchIndex, int constraintHandleCount, int perBodyRemovalCount, BufferPool pool) { var index = IndexOf(typeBatchIndex); if (index >= 0) @@ -95,7 +95,7 @@ public int AllocateSpaceForTargets(TypeBatchIndex typeBatchIndex, int constraint return index; } - public void Dispose(IUnmanagedMemoryPool pool) + public void Dispose(BufferPool pool) { pool.Return(ref TypeBatches); for (int i = 0; i < BatchCount; ++i) @@ -113,10 +113,10 @@ public void Dispose(IUnmanagedMemoryPool pool) struct WorkerCache { - internal IUnmanagedMemoryPool pool; + internal BufferPool pool; public RemovalCache Removals; - public WorkerCache(IUnmanagedMemoryPool pool, int batchCapacity, int minimumCapacityPerBatch) + public WorkerCache(BufferPool pool, int batchCapacity, int minimumCapacityPerBatch) { this.pool = pool; Debug.Assert(minimumCapacityPerBatch > 0); diff --git a/BepuPhysics/CollisionDetection/MeshReduction.cs b/BepuPhysics/CollisionDetection/MeshReduction.cs index 9bd3a2b83..cd9105360 100644 --- a/BepuPhysics/CollisionDetection/MeshReduction.cs +++ b/BepuPhysics/CollisionDetection/MeshReduction.cs @@ -34,7 +34,7 @@ public unsafe struct MeshReduction : ICollisionTestContinuation public void* Mesh; //TODO: This is not flexible with respect to different mesh types. Not a problem right now, but it will be in the future. - public void Create(int childManifoldCount, IUnmanagedMemoryPool pool) + public void Create(int childManifoldCount, BufferPool pool) { Inner.Create(childManifoldCount, pool); } @@ -286,7 +286,7 @@ static void TryApplyBlockToTriangle(ref TestTriangle triangle, Buffer { public QuickList List; - public IUnmanagedMemoryPool Pool; + public BufferPool Pool; public bool LoopBody(int i) { List.Allocate(Pool) = i; @@ -295,7 +295,7 @@ public bool LoopBody(int i) } public unsafe static void ReduceManifolds(ref Buffer continuationTriangles, ref Buffer continuationChildren, int start, int count, - bool requiresFlip, in BoundingBox queryBounds, in Matrix3x3 meshOrientation, in Matrix3x3 meshInverseOrientation, Mesh* mesh, IUnmanagedMemoryPool pool) + bool requiresFlip, in BoundingBox queryBounds, in Matrix3x3 meshOrientation, in Matrix3x3 meshInverseOrientation, Mesh* mesh, BufferPool pool) { //Before handing responsibility off to the nonconvex reduction, make sure that no contacts create nasty 'bumps' at the border of triangles. //Bumps can occur when an isolated triangle test detects a contact pointing outward, like when a box hits the side. This is fine when the triangle truly is isolated, diff --git a/BepuPhysics/CollisionDetection/NarrowPhase.cs b/BepuPhysics/CollisionDetection/NarrowPhase.cs index 5a00dd9a0..d7d9e3631 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhase.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhase.cs @@ -339,7 +339,7 @@ public struct OverlapWorker public PendingConstraintAddCache PendingConstraints; public QuickList PendingSetAwakenings; - public OverlapWorker(int workerIndex, IUnmanagedMemoryPool pool, NarrowPhase narrowPhase) + public OverlapWorker(int workerIndex, BufferPool pool, NarrowPhase narrowPhase) { Batcher = new CollisionBatcher(pool, narrowPhase.Shapes, narrowPhase.CollisionTaskRegistry, narrowPhase.timestepDuration, new CollisionCallbacks(workerIndex, pool, narrowPhase)); diff --git a/BepuPhysics/CollisionDetection/NarrowPhaseCCDContinuations.cs b/BepuPhysics/CollisionDetection/NarrowPhaseCCDContinuations.cs index 52d91ead7..06bc39b65 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhaseCCDContinuations.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhaseCCDContinuations.cs @@ -30,7 +30,7 @@ public enum ConstraintGeneratorType public struct CollisionCallbacks : ICollisionCallbacks { int workerIndex; - IUnmanagedMemoryPool pool; + BufferPool pool; NarrowPhase narrowPhase; @@ -70,14 +70,14 @@ struct ContinuationCache where T : unmanaged public IdPool Ids; public Buffer Caches; - public ContinuationCache(IUnmanagedMemoryPool pool) + public ContinuationCache(BufferPool pool) { Ids = new IdPool(32, pool); pool.TakeAtLeast(128, out Caches); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref T Allocate(IUnmanagedMemoryPool pool, out int index) + public ref T Allocate(BufferPool pool, out int index) { index = Ids.Take(); if (Caches.Length <= index) @@ -88,13 +88,13 @@ public ref T Allocate(IUnmanagedMemoryPool pool, out int index) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Return(int index, IUnmanagedMemoryPool pool) + public void Return(int index, BufferPool pool) { Ids.Return(index, pool); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Dispose(IUnmanagedMemoryPool pool) + public void Dispose(BufferPool pool) { Ids.Dispose(pool); pool.Return(ref Caches); @@ -104,7 +104,7 @@ public void Dispose(IUnmanagedMemoryPool pool) ContinuationCache discrete; ContinuationCache continuous; - public CollisionCallbacks(int workerIndex, IUnmanagedMemoryPool pool, NarrowPhase narrowPhase) + public CollisionCallbacks(int workerIndex, BufferPool pool, NarrowPhase narrowPhase) { this.pool = pool; this.workerIndex = workerIndex; diff --git a/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs b/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs index b2804595e..59fbd0f87 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs @@ -21,7 +21,7 @@ public partial class NarrowPhase { public struct PendingConstraintAddCache { - IUnmanagedMemoryPool pool; + BufferPool pool; [StructLayout(LayoutKind.Sequential)] unsafe struct PendingConstraint where TBodyHandles : unmanaged where TDescription : unmanaged, IConstraintDescription { @@ -37,7 +37,7 @@ unsafe struct PendingConstraint wh internal Buffer> speculativeBatchIndices; int minimumConstraintCountPerCache; - public PendingConstraintAddCache(IUnmanagedMemoryPool pool, int minimumConstraintCountPerCache = 128) + public PendingConstraintAddCache(BufferPool pool, int minimumConstraintCountPerCache = 128) { this.pool = pool; pool.TakeAtLeast(PairCache.CollisionConstraintTypeCount, out pendingConstraintsByType); diff --git a/BepuPhysics/CollisionDetection/NonconvexReduction.cs b/BepuPhysics/CollisionDetection/NonconvexReduction.cs index 1f878e4fa..b5e677224 100644 --- a/BepuPhysics/CollisionDetection/NonconvexReduction.cs +++ b/BepuPhysics/CollisionDetection/NonconvexReduction.cs @@ -32,7 +32,7 @@ public struct NonconvexReduction : ICollisionTestContinuation public int CompletedChildCount; public Buffer Children; - public void Create(int childManifoldCount, IUnmanagedMemoryPool pool) + public void Create(int childManifoldCount, BufferPool pool) { ChildCount = childManifoldCount; CompletedChildCount = 0; @@ -102,7 +102,7 @@ static float ComputeDistinctiveness(in ConvexContact candidate, Vector3 contactN return distinctiveness; } - unsafe void ChooseMostDistinct(NonconvexContactManifold* manifold, IUnmanagedMemoryPool pool) + unsafe void ChooseMostDistinct(NonconvexContactManifold* manifold, BufferPool pool) { //The end goal of contact reduction is to choose a reasonably stable subset of contacts which offer the greatest degree of constraint. //Computing a globally optimal solution to this would be pretty expensive for any given scoring mechanism, so we'll make some simplifications: diff --git a/BepuPhysics/CollisionDetection/RayBatchers.cs b/BepuPhysics/CollisionDetection/RayBatchers.cs index 67facdb85..3752aa898 100644 --- a/BepuPhysics/CollisionDetection/RayBatchers.cs +++ b/BepuPhysics/CollisionDetection/RayBatchers.cs @@ -54,7 +54,7 @@ public unsafe void TestLeaf(int leafIndex, RayData* rayData, float* maximumT) /// Ray tester used to test leaves found by the broad phase tree traversals. /// Maximum number of rays to execute in each traversal. /// This should typically be chosen as the highest value which avoids spilling data out of L2 cache. - public BroadPhaseRayBatcher(IUnmanagedMemoryPool pool, BroadPhase broadPhase, TRayTester rayTester, int batcherRayCapacity = 2048) + public BroadPhaseRayBatcher(BufferPool pool, BroadPhase broadPhase, TRayTester rayTester, int batcherRayCapacity = 2048) { activeTester = new LeafTester { Leaves = broadPhase.ActiveLeaves, RayTester = rayTester }; staticTester = new LeafTester { Leaves = broadPhase.StaticLeaves, RayTester = rayTester }; @@ -163,7 +163,7 @@ public unsafe void RayTest(CollidableReference reference, RayData* rayData, floa BroadPhaseRayBatcher batcher; - public SimulationRayBatcher(IUnmanagedMemoryPool pool, Simulation simulation, TRayHitHandler hitHandler, int batcherRayCapacity = 2048) + public SimulationRayBatcher(BufferPool pool, Simulation simulation, TRayHitHandler hitHandler, int batcherRayCapacity = 2048) { Dispatcher dispatcher = default; dispatcher.Simulation = simulation; diff --git a/BepuPhysics/CollisionDetection/SweepTaskRegistry.cs b/BepuPhysics/CollisionDetection/SweepTaskRegistry.cs index 4f16b354c..6fc3482a1 100644 --- a/BepuPhysics/CollisionDetection/SweepTaskRegistry.cs +++ b/BepuPhysics/CollisionDetection/SweepTaskRegistry.cs @@ -69,14 +69,14 @@ protected abstract unsafe bool PreorderedTypeSweep( void* shapeDataA, Quaternion orientationA, in BodyVelocity velocityA, void* shapeDataB, Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float maximumT, float minimumProgression, float convergenceThreshold, int maximumIterationCount, - bool flipRequired, ref TSweepFilter filter, Shapes shapes, SweepTaskRegistry sweepTasks, IUnmanagedMemoryPool pool, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal) + bool flipRequired, ref TSweepFilter filter, Shapes shapes, SweepTaskRegistry sweepTasks, BufferPool pool, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal) where TSweepFilter : ISweepFilter; public unsafe bool Sweep( void* shapeDataA, int shapeTypeA, Quaternion orientationA, in BodyVelocity velocityA, void* shapeDataB, int shapeTypeB, Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float maximumT, float minimumProgression, float convergenceThreshold, int maximumIterationCount, - ref TSweepFilter filter, Shapes shapes, SweepTaskRegistry sweepTasks, IUnmanagedMemoryPool pool, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal) + ref TSweepFilter filter, Shapes shapes, SweepTaskRegistry sweepTasks, BufferPool pool, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal) where TSweepFilter : ISweepFilter { Debug.Assert((shapeTypeA == ShapeTypeIndexA && shapeTypeB == ShapeTypeIndexB) || (shapeTypeA == ShapeTypeIndexB && shapeTypeB == ShapeTypeIndexA), diff --git a/BepuPhysics/CollisionDetection/SweepTasks/CompoundHomogeneousCompoundSweepTask.cs b/BepuPhysics/CollisionDetection/SweepTasks/CompoundHomogeneousCompoundSweepTask.cs index de5e587c3..4330b951b 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/CompoundHomogeneousCompoundSweepTask.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/CompoundHomogeneousCompoundSweepTask.cs @@ -23,7 +23,7 @@ protected unsafe override bool PreorderedTypeSweep( void* shapeDataA, Quaternion orientationA, in BodyVelocity velocityA, void* shapeDataB, Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float maximumT, float minimumProgression, float convergenceThreshold, int maximumIterationCount, - bool flipRequired, ref TSweepFilter filter, Shapes shapes, SweepTaskRegistry sweepTasks, IUnmanagedMemoryPool pool, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal) + bool flipRequired, ref TSweepFilter filter, Shapes shapes, SweepTaskRegistry sweepTasks, BufferPool pool, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal) { ref var compoundB = ref Unsafe.AsRef(shapeDataB); TOverlapFinder overlapFinder = default; diff --git a/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepOverlapFinder.cs b/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepOverlapFinder.cs index a7108362f..c6d1690cc 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepOverlapFinder.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepOverlapFinder.cs @@ -11,7 +11,7 @@ public interface ICompoundPairSweepOverlapFinder where T { unsafe void FindOverlaps(ref TCompoundA compoundA, Quaternion orientationA, in BodyVelocity velocityA, ref TCompoundB compoundB, Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float maximumT, - Shapes shapes, IUnmanagedMemoryPool pool, out CompoundPairSweepOverlaps overlaps); + Shapes shapes, BufferPool pool, out CompoundPairSweepOverlaps overlaps); } public struct CompoundPairSweepOverlapFinder : ICompoundPairSweepOverlapFinder @@ -21,7 +21,7 @@ public struct CompoundPairSweepOverlapFinder : ICompound public unsafe void FindOverlaps( ref TCompoundA compoundA, Quaternion orientationA, in BodyVelocity velocityA, ref TCompoundB compoundB, Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float maximumT, - Shapes shapes, IUnmanagedMemoryPool pool, out CompoundPairSweepOverlaps overlaps) + Shapes shapes, BufferPool pool, out CompoundPairSweepOverlaps overlaps) { overlaps = new CompoundPairSweepOverlaps(pool, compoundA.ChildCount); for (int i = 0; i < compoundA.ChildCount; ++i) diff --git a/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepOverlaps.cs b/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepOverlaps.cs index bd5cd28bb..a8110a34a 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepOverlaps.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepOverlaps.cs @@ -8,7 +8,7 @@ public struct CompoundPairSweepOverlaps { Buffer childOverlaps; public readonly int ChildCount; - public CompoundPairSweepOverlaps(IUnmanagedMemoryPool pool, int childCount) + public CompoundPairSweepOverlaps(BufferPool pool, int childCount) { ChildCount = childCount; pool.Take(childCount, out childOverlaps); @@ -22,7 +22,7 @@ public ref ChildOverlapsCollection GetOverlapsForChild(int pairIndex) return ref childOverlaps[pairIndex]; } - public void Dispose(IUnmanagedMemoryPool pool) + public void Dispose(BufferPool pool) { for (int i = 0; i < ChildCount; ++i) { diff --git a/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepTask.cs b/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepTask.cs index 75de54c31..c080a88bc 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepTask.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepTask.cs @@ -21,7 +21,7 @@ protected override unsafe bool PreorderedTypeSweep( void* shapeDataA, Quaternion orientationA, in BodyVelocity velocityA, void* shapeDataB, Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float maximumT, float minimumProgression, float convergenceThreshold, int maximumIterationCount, - bool flipRequired, ref TSweepFilter filter, Shapes shapes, SweepTaskRegistry sweepTasks, IUnmanagedMemoryPool pool, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal) + bool flipRequired, ref TSweepFilter filter, Shapes shapes, SweepTaskRegistry sweepTasks, BufferPool pool, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal) { ref var a = ref Unsafe.AsRef(shapeDataA); ref var b = ref Unsafe.AsRef(shapeDataB); diff --git a/BepuPhysics/CollisionDetection/SweepTasks/ConvexCompoundSweepOverlapFinder.cs b/BepuPhysics/CollisionDetection/SweepTasks/ConvexCompoundSweepOverlapFinder.cs index a090ddd06..010f1c5f9 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/ConvexCompoundSweepOverlapFinder.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/ConvexCompoundSweepOverlapFinder.cs @@ -14,7 +14,7 @@ public interface IConvexCompoundSweepOverlapFinder where TS { unsafe void FindOverlaps(ref TShapeA shapeA, Quaternion orientationA, in BodyVelocity velocityA, ref TCompoundB compoundB, Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float maximumT, - Shapes shapes, IUnmanagedMemoryPool pool, out ChildOverlapsCollection overlaps); + Shapes shapes, BufferPool pool, out ChildOverlapsCollection overlaps); } public struct ConvexCompoundSweepOverlapFinder : IConvexCompoundSweepOverlapFinder @@ -22,7 +22,7 @@ public struct ConvexCompoundSweepOverlapFinder : IConvexCom { public unsafe void FindOverlaps(ref TShapeA shapeA, Quaternion orientationA, in BodyVelocity velocityA, ref TCompoundB compoundB, Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float maximumT, - Shapes shapes, IUnmanagedMemoryPool pool, out ChildOverlapsCollection overlaps) + Shapes shapes, BufferPool pool, out ChildOverlapsCollection overlaps) { BoundingBoxHelpers.GetLocalBoundingBoxForSweep(ref shapeA, orientationA, velocityA, offsetB, orientationB, velocityB, maximumT, out var sweep, out var min, out var max); diff --git a/BepuPhysics/CollisionDetection/SweepTasks/ConvexCompoundSweepTask.cs b/BepuPhysics/CollisionDetection/SweepTasks/ConvexCompoundSweepTask.cs index c4c22bef5..c2c09a35b 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/ConvexCompoundSweepTask.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/ConvexCompoundSweepTask.cs @@ -22,7 +22,7 @@ protected override unsafe bool PreorderedTypeSweep( void* shapeDataA, Quaternion orientationA, in BodyVelocity velocityA, void* shapeDataB, Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float maximumT, float minimumProgression, float convergenceThreshold, int maximumIterationCount, - bool flipRequired, ref TSweepFilter filter, Shapes shapes, SweepTaskRegistry sweepTasks, IUnmanagedMemoryPool pool, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal) + bool flipRequired, ref TSweepFilter filter, Shapes shapes, SweepTaskRegistry sweepTasks, BufferPool pool, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal) { ref var convex = ref Unsafe.AsRef(shapeDataA); ref var compound = ref Unsafe.AsRef(shapeDataB); diff --git a/BepuPhysics/CollisionDetection/SweepTasks/ConvexHomogeneousCompoundSweepTask.cs b/BepuPhysics/CollisionDetection/SweepTasks/ConvexHomogeneousCompoundSweepTask.cs index 34959b82b..52ca6159a 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/ConvexHomogeneousCompoundSweepTask.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/ConvexHomogeneousCompoundSweepTask.cs @@ -27,7 +27,7 @@ protected override unsafe bool PreorderedTypeSweep( void* shapeDataA, Quaternion orientationA, in BodyVelocity velocityA, void* shapeDataB, Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float maximumT, float minimumProgression, float convergenceThreshold, int maximumIterationCount, - bool flipRequired, ref TSweepFilter filter, Shapes shapes, SweepTaskRegistry sweepTasks, IUnmanagedMemoryPool pool, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal) + bool flipRequired, ref TSweepFilter filter, Shapes shapes, SweepTaskRegistry sweepTasks, BufferPool pool, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal) { ref var compound = ref Unsafe.AsRef(shapeDataB); t0 = float.MaxValue; diff --git a/BepuPhysics/CollisionDetection/SweepTasks/ConvexSweepTaskCommon.cs b/BepuPhysics/CollisionDetection/SweepTasks/ConvexSweepTaskCommon.cs index bf868f3d4..8cdcf0593 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/ConvexSweepTaskCommon.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/ConvexSweepTaskCommon.cs @@ -46,7 +46,7 @@ protected override unsafe bool PreorderedTypeSweep( void* shapeDataA, Quaternion orientationA, in BodyVelocity velocityA, void* shapeDataB, Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float maximumT, float minimumProgression, float convergenceThreshold, int maximumIterationCount, - bool requiresFlip, ref TSweepFilter filter, Shapes shapes, SweepTaskRegistry sweepTasks, IUnmanagedMemoryPool pool, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal) + bool requiresFlip, ref TSweepFilter filter, Shapes shapes, SweepTaskRegistry sweepTasks, BufferPool pool, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal) { UnoffsetSweep sweepModifier = default; return Sweep( diff --git a/BepuPhysics/CollisionDetection/UntypedList.cs b/BepuPhysics/CollisionDetection/UntypedList.cs index 645e9b800..2dc7476bc 100644 --- a/BepuPhysics/CollisionDetection/UntypedList.cs +++ b/BepuPhysics/CollisionDetection/UntypedList.cs @@ -14,7 +14,7 @@ public struct UntypedList [MethodImpl(MethodImplOptions.AggressiveInlining)] - public UntypedList(int elementSizeInBytes, int initialCapacityInElements, IUnmanagedMemoryPool pool) + public UntypedList(int elementSizeInBytes, int initialCapacityInElements, BufferPool pool) { pool.TakeAtLeast(initialCapacityInElements * elementSizeInBytes, out Buffer); Count = 0; @@ -23,7 +23,7 @@ public UntypedList(int elementSizeInBytes, int initialCapacityInElements, IUnman } [MethodImpl(MethodImplOptions.AggressiveInlining)] - internal void EnsureCapacityInBytes(int elementSizeInBytes, int targetCapacityInBytes, IUnmanagedMemoryPool pool) + internal void EnsureCapacityInBytes(int elementSizeInBytes, int targetCapacityInBytes, BufferPool pool) { //EnsureCapacity is basically a secondary constructor, but it can be used on already-existing caches. It has the same required output. Debug.Assert(ElementSizeInBytes == 0 || ElementSizeInBytes == elementSizeInBytes, @@ -91,7 +91,7 @@ public unsafe ref T AllocateUnsafely() /// Pool to pull allocations from. /// Index of the element in bytes within the list's buffer. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe int Allocate(int elementSizeInBytes, int minimumElementCount, IUnmanagedMemoryPool pool) + public unsafe int Allocate(int elementSizeInBytes, int minimumElementCount, BufferPool pool) { var newSize = ByteCount + elementSizeInBytes; if (!Buffer.Allocated) @@ -123,14 +123,14 @@ public unsafe int Allocate(int elementSizeInBytes, int minimumElementCount, IUnm } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe int Allocate(int minimumElementCount, IUnmanagedMemoryPool pool) + public unsafe int Allocate(int minimumElementCount, BufferPool pool) { var elementSizeInBytes = Unsafe.SizeOf(); return Allocate(elementSizeInBytes, minimumElementCount, pool); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe int Add(ref T data, int minimumCount, IUnmanagedMemoryPool pool) + public unsafe int Add(ref T data, int minimumCount, BufferPool pool) { var byteIndex = Allocate(minimumCount, pool); GetFromBytes(byteIndex) = data; diff --git a/BepuPhysics/CollisionDetection/WorkerPairCache.cs b/BepuPhysics/CollisionDetection/WorkerPairCache.cs index f0f99ff86..e262dcbaa 100644 --- a/BepuPhysics/CollisionDetection/WorkerPairCache.cs +++ b/BepuPhysics/CollisionDetection/WorkerPairCache.cs @@ -26,7 +26,7 @@ public struct PendingAdd /// public QuickList PendingRemoves; - public WorkerPendingPairChanges(IUnmanagedMemoryPool pool, int pendingCapacity) + public WorkerPendingPairChanges(BufferPool pool, int pendingCapacity) { PendingAdds = new QuickList(pendingCapacity, pool); PendingRemoves = new QuickList(pendingCapacity, pool); @@ -34,7 +34,7 @@ public WorkerPendingPairChanges(IUnmanagedMemoryPool pool, int pendingCapacity) [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe int Add(IUnmanagedMemoryPool pool, CollidablePair pair, in ConstraintCache cache) + public unsafe int Add(BufferPool pool, CollidablePair pair, in ConstraintCache cache) { int index = PendingAdds.Count; ref var pendingAdd = ref PendingAdds.Allocate(pool); @@ -44,7 +44,7 @@ public unsafe int Add(IUnmanagedMemoryPool pool, CollidablePair pair, in Constra } - public void Dispose(IUnmanagedMemoryPool pool) + public void Dispose(BufferPool pool) { PendingAdds.Dispose(pool); PendingRemoves.Dispose(pool); diff --git a/BepuPhysics/IslandScaffold.cs b/BepuPhysics/IslandScaffold.cs index 659f6d412..a2e2a8ef7 100644 --- a/BepuPhysics/IslandScaffold.cs +++ b/BepuPhysics/IslandScaffold.cs @@ -22,7 +22,7 @@ internal struct IslandScaffoldTypeBatch public int TypeId; public QuickList Handles; - public IslandScaffoldTypeBatch(IUnmanagedMemoryPool pool, int typeId, int initialTypeBatchSize) + public IslandScaffoldTypeBatch(BufferPool pool, int typeId, int initialTypeBatchSize) { TypeId = typeId; Handles = new QuickList(initialTypeBatchSize, pool); @@ -37,7 +37,7 @@ internal struct IslandScaffoldConstraintBatch //Note that we use *indices* during island construction, not handles. This protobatch doesn't have to deal with memory moves in between adds, so indices are fine. public IndexSet ReferencedBodyIndices; - public unsafe IslandScaffoldConstraintBatch(Solver solver, IUnmanagedMemoryPool pool, int batchIndex) + public unsafe IslandScaffoldConstraintBatch(Solver solver, BufferPool pool, int batchIndex) { pool.TakeAtLeast(solver.TypeProcessors.Length, out TypeIdToIndex); Unsafe.InitBlockUnaligned(TypeIdToIndex.Memory, 0xFF, (uint)(TypeIdToIndex.Length * sizeof(int))); @@ -46,7 +46,7 @@ public unsafe IslandScaffoldConstraintBatch(Solver solver, IUnmanagedMemoryPool } [MethodImpl(MethodImplOptions.AggressiveInlining)] - ref IslandScaffoldTypeBatch GetOrCreateTypeBatch(int typeId, Solver solver, IUnmanagedMemoryPool pool) + ref IslandScaffoldTypeBatch GetOrCreateTypeBatch(int typeId, Solver solver, BufferPool pool) { ref var idMap = ref TypeIdToIndex[typeId]; if (idMap == -1) @@ -76,7 +76,7 @@ internal void Validate(Solver solver) } } - public unsafe bool TryAdd(ConstraintHandle constraintHandle, Span dynamicBodyIndices, int typeId, int batchIndex, Solver solver, IUnmanagedMemoryPool pool, ref SequentialFallbackBatch fallbackBatch) + public unsafe bool TryAdd(ConstraintHandle constraintHandle, Span dynamicBodyIndices, int typeId, int batchIndex, Solver solver, BufferPool pool, ref SequentialFallbackBatch fallbackBatch) { if (batchIndex == solver.FallbackBatchThreshold || ReferencedBodyIndices.CanFit(dynamicBodyIndices)) { @@ -105,7 +105,7 @@ public unsafe bool TryAdd(ConstraintHandle constraintHandle, Span dynamicBo return false; } - public void Dispose(IUnmanagedMemoryPool pool) + public void Dispose(BufferPool pool) { for (int i = 0; i < TypeBatches.Count; ++i) { @@ -128,7 +128,7 @@ internal struct IslandScaffold public QuickList Protobatches; public SequentialFallbackBatch FallbackBatch; - public IslandScaffold(ref QuickList bodyIndices, ref QuickList constraintHandles, Solver solver, IUnmanagedMemoryPool pool) : this() + public IslandScaffold(ref QuickList bodyIndices, ref QuickList constraintHandles, Solver solver, BufferPool pool) : this() { Debug.Assert(bodyIndices.Count > 0, "Don't be tryin' to create islands with no bodies in them! That don't make no sense."); //Create a copy of the body indices with just enough space to hold the island's indices. The original list will continue to be reused in the caller. @@ -151,7 +151,7 @@ public void Validate(Solver solver) } } - unsafe void AddConstraint(ConstraintHandle constraintHandle, Solver solver, IUnmanagedMemoryPool pool) + unsafe void AddConstraint(ConstraintHandle constraintHandle, Solver solver, BufferPool pool) { var typeId = solver.HandleToConstraint[constraintHandle.Value].TypeId; var typeProcessor = solver.TypeProcessors[typeId]; @@ -178,7 +178,7 @@ unsafe void AddConstraint(ConstraintHandle constraintHandle, Solver solver, IUnm Debug.Assert(addedSuccessfully, "If we created a new batch for a constraint, then it must successfully add."); } - internal void Dispose(IUnmanagedMemoryPool pool) + internal void Dispose(BufferPool pool) { BodyIndices.Dispose(pool); for (int k = 0; k < Protobatches.Count; ++k) diff --git a/BepuPhysics/IslandSleeper.cs b/BepuPhysics/IslandSleeper.cs index 7361db395..653a791c7 100644 --- a/BepuPhysics/IslandSleeper.cs +++ b/BepuPhysics/IslandSleeper.cs @@ -63,7 +63,7 @@ internal void ReturnSetId(int id) struct ConstraintBodyEnumerator : IForEach { public QuickList ConstraintBodyIndices; - public IUnmanagedMemoryPool Pool; + public BufferPool Pool; public int SourceIndex; public void LoopBody(int bodyIndex) { @@ -113,7 +113,7 @@ public bool Matches(ref int bodyIndex) [MethodImpl(MethodImplOptions.AggressiveInlining)] static bool PushBody(int bodyIndex, ref IndexSet consideredBodies, ref QuickList bodyIndices, ref QuickList visitationStack, - IUnmanagedMemoryPool pool, ref TTraversalPredicate predicate) where TTraversalPredicate : IPredicate + BufferPool pool, ref TTraversalPredicate predicate) where TTraversalPredicate : IPredicate { if (!consideredBodies.Contains(bodyIndex)) { @@ -137,7 +137,7 @@ bool EnqueueUnvisitedNeighbors(int bodyIndex, ref IndexSet consideredBodies, ref IndexSet consideredConstraints, ref QuickList visitationStack, ref ConstraintBodyEnumerator bodyEnumerator, - IUnmanagedMemoryPool pool, ref TTraversalPredicate predicate) where TTraversalPredicate : IPredicate + BufferPool pool, ref TTraversalPredicate predicate) where TTraversalPredicate : IPredicate { bodyEnumerator.SourceIndex = bodyIndex; ref var list = ref bodies.ActiveSet.Constraints[bodyIndex]; @@ -173,7 +173,7 @@ bool EnqueueUnvisitedNeighbors(int bodyIndex, /// List to fill with constraint handles traversed during island collection. /// True if the simulation graph was traversed without ever finding a body that made the predicate return false. False if any body failed the predicate. /// The bodyIndices and constraintHandles lists will contain all traversed predicate-passing bodies and constraints. - public bool CollectIsland(IUnmanagedMemoryPool pool, int startingActiveBodyIndex, ref TTraversalPredicate predicate, + public bool CollectIsland(BufferPool pool, int startingActiveBodyIndex, ref TTraversalPredicate predicate, ref QuickList bodyIndices, ref QuickList constraintHandles) where TTraversalPredicate : IPredicate { Debug.Assert(startingActiveBodyIndex >= 0 && startingActiveBodyIndex < bodies.ActiveSet.Count); @@ -237,7 +237,7 @@ struct WorkerTraversalResults public IndexSet TraversedBodies; public QuickList Islands; - internal void Dispose(IUnmanagedMemoryPool pool) + internal void Dispose(BufferPool pool) { for (int islandIndex = 0; islandIndex < Islands.Count; ++islandIndex) { @@ -269,7 +269,7 @@ struct GatheringJob QuickList gatheringJobs; - void FindIslands(int workerIndex, IUnmanagedMemoryPool threadPool, ref TPredicate predicate) where TPredicate : IPredicate + void FindIslands(int workerIndex, BufferPool threadPool, ref TPredicate predicate) where TPredicate : IPredicate { Debug.Assert(workerTraversalResults.Allocated && workerTraversalResults.Length > workerIndex); ref var results = ref workerTraversalResults[workerIndex]; @@ -310,7 +310,7 @@ void FindIslands(int workerIndex, IUnmanagedMemoryPool threadPool, r constraintHandles.Dispose(threadPool); results.TraversedBodies = traversalTest.PreviouslyTraversedBodies; } - void FindIslands(int workerIndex, IUnmanagedMemoryPool threadPool) + void FindIslands(int workerIndex, BufferPool threadPool) { //This if is handled externally to push the code specialization early. if (forceSleep) diff --git a/BepuPhysics/SequentialFallbackBatch.cs b/BepuPhysics/SequentialFallbackBatch.cs index 482e28611..4b4a7aa6e 100644 --- a/BepuPhysics/SequentialFallbackBatch.cs +++ b/BepuPhysics/SequentialFallbackBatch.cs @@ -54,7 +54,7 @@ public struct SequentialFallbackBatch [MethodImpl(MethodImplOptions.AggressiveInlining)] unsafe void Allocate(Span dynamicBodyHandles, Bodies bodies, - IUnmanagedMemoryPool pool, TBodyReferenceGetter bodyReferenceGetter, int minimumBodyCapacity) + BufferPool pool, TBodyReferenceGetter bodyReferenceGetter, int minimumBodyCapacity) where TBodyReferenceGetter : struct, IBodyReferenceGetter { EnsureCapacity(Math.Max(dynamicBodyConstraintCounts.Count + dynamicBodyHandles.Length, minimumBodyCapacity), pool); @@ -84,7 +84,7 @@ internal unsafe void AllocateForActive(Span dynamicBodyHandles, Bodi [MethodImpl(MethodImplOptions.AggressiveInlining)] internal void AllocateForInactive(Span dynamicBodyHandles, Bodies bodies, - IUnmanagedMemoryPool pool, int minimumBodyCapacity = 8) + BufferPool pool, int minimumBodyCapacity = 8) { Allocate(dynamicBodyHandles, bodies, pool, new InactiveSetGetter(), minimumBodyCapacity); } @@ -239,7 +239,7 @@ internal void UpdateForBodyMemorySwap(int a, int b) Helpers.Swap(ref dynamicBodyConstraintCounts.Values[indexA], ref dynamicBodyConstraintCounts.Values[indexB]); } - internal static void CreateFrom(ref SequentialFallbackBatch sourceBatch, IUnmanagedMemoryPool pool, out SequentialFallbackBatch targetBatch) + internal static void CreateFrom(ref SequentialFallbackBatch sourceBatch, BufferPool pool, out SequentialFallbackBatch targetBatch) { //Copy over non-buffer state. This copies buffer references pointlessly, but that doesn't matter. targetBatch.dynamicBodyConstraintCounts = sourceBatch.dynamicBodyConstraintCounts; @@ -251,7 +251,7 @@ internal static void CreateFrom(ref SequentialFallbackBatch sourceBatch, IUnmana sourceBatch.dynamicBodyConstraintCounts.Table.CopyTo(0, targetBatch.dynamicBodyConstraintCounts.Table, 0, sourceBatch.dynamicBodyConstraintCounts.TableMask + 1); } - internal void EnsureCapacity(int bodyCapacity, IUnmanagedMemoryPool pool) + internal void EnsureCapacity(int bodyCapacity, BufferPool pool) { if (dynamicBodyConstraintCounts.Keys.Allocated) { @@ -265,7 +265,7 @@ internal void EnsureCapacity(int bodyCapacity, IUnmanagedMemoryPool pool) } - public void Compact(IUnmanagedMemoryPool pool) + public void Compact(BufferPool pool) { if (dynamicBodyConstraintCounts.Keys.Allocated) { @@ -274,7 +274,7 @@ public void Compact(IUnmanagedMemoryPool pool) } - public void Dispose(IUnmanagedMemoryPool pool) + public void Dispose(BufferPool pool) { if (dynamicBodyConstraintCounts.Keys.Allocated) { diff --git a/BepuPhysics/Trees/RayBatcher.cs b/BepuPhysics/Trees/RayBatcher.cs index ff39f62f4..48940d975 100644 --- a/BepuPhysics/Trees/RayBatcher.cs +++ b/BepuPhysics/Trees/RayBatcher.cs @@ -138,7 +138,7 @@ struct StackEntry } int stackPointer; Buffer stack; - IUnmanagedMemoryPool pool; + BufferPool pool; int batchRayCount; Buffer batchRays; Buffer batchOriginalRays; @@ -157,7 +157,7 @@ struct StackEntry /// Maximum number of rays to execute in each traversal. /// This should typically be chosen as the highest value which avoids spilling data out of L2 cache. /// Tree depth to preallocate ray stack space for. If a traversal finds nodes deeper than this, a dynamic resize will be triggered. - public RayBatcher(IUnmanagedMemoryPool pool, int rayCapacity = 2048, int treeDepthForPreallocation = 24) : this() + public RayBatcher(BufferPool pool, int rayCapacity = 2048, int treeDepthForPreallocation = 24) : this() { this.pool = pool; batchRayCount = 0; diff --git a/BepuPhysics/Trees/Tree.cs b/BepuPhysics/Trees/Tree.cs index dba3b9a31..620ef560b 100644 --- a/BepuPhysics/Trees/Tree.cs +++ b/BepuPhysics/Trees/Tree.cs @@ -83,7 +83,7 @@ public unsafe readonly void UpdateBounds(int leafIndex, Vector3 min, Vector3 max /// /// Buffer pool to use to allocate resources in the tree. /// Initial number of leaves to allocate room for. - public unsafe Tree(IUnmanagedMemoryPool pool, int initialLeafCapacity = 4096) : this() + public unsafe Tree(BufferPool pool, int initialLeafCapacity = 4096) : this() { if (initialLeafCapacity <= 0) throw new ArgumentException("Initial leaf capacity must be positive."); @@ -97,7 +97,7 @@ public unsafe Tree(IUnmanagedMemoryPool pool, int initialLeafCapacity = 4096) : /// /// Data to load into the tree. /// Pool to use to create the tree. - public Tree(Span data, IUnmanagedMemoryPool pool) + public Tree(Span data, BufferPool pool) { if (data.Length <= 4) throw new ArgumentException($"Data is only {data.Length} bytes long; that's too small for even a header."); @@ -171,7 +171,7 @@ void InitializeRoot() /// /// Pool from which to take and return resources. /// The desired number of available leaf slots. - public void Resize(IUnmanagedMemoryPool pool, int targetLeafSlotCount) + public void Resize(BufferPool pool, int targetLeafSlotCount) { //Note that it's not safe to resize below the size of potentially used leaves. If the user wants to go smaller, they'll need to explicitly deal with the leaves somehow first. var leafCapacityForTarget = BufferPool.GetCapacityForCount(Math.Max(LeafCount, targetLeafSlotCount)); @@ -217,7 +217,7 @@ public void Clear() /// /// Pool to return resources to. /// Disposed trees can be reused if EnsureCapacity or Resize is used to rehydrate them. - public void Dispose(IUnmanagedMemoryPool pool) + public void Dispose(BufferPool pool) { Debug.Assert(Nodes.Allocated == Leaves.Allocated && Nodes.Allocated == Metanodes.Allocated, "Nodes and leaves should have consistent lifetimes."); if (Nodes.Allocated) diff --git a/BepuPhysics/Trees/Tree_Add.cs b/BepuPhysics/Trees/Tree_Add.cs index 92f1adbb5..ab78ea49d 100644 --- a/BepuPhysics/Trees/Tree_Add.cs +++ b/BepuPhysics/Trees/Tree_Add.cs @@ -109,7 +109,7 @@ private static unsafe BestInsertionChoice ComputeBestInsertionChoice(ref Boundin /// Extents of the leaf bounds. /// Resource pool to use if resizing is required. /// Index of the leaf allocated in the tree's leaf array. - public unsafe int Add(BoundingBox bounds, IUnmanagedMemoryPool pool) + public unsafe int Add(BoundingBox bounds, BufferPool pool) { //The rest of the function assumes we have sufficient room. We don't want to deal with invalidated pointers mid-add. if (Leaves.Length == LeafCount) diff --git a/BepuPhysics/Trees/Tree_BinnedRefine.cs b/BepuPhysics/Trees/Tree_BinnedRefine.cs index a8fd5b2b4..f2902ae71 100644 --- a/BepuPhysics/Trees/Tree_BinnedRefine.cs +++ b/BepuPhysics/Trees/Tree_BinnedRefine.cs @@ -80,7 +80,7 @@ partial struct Tree return toReturn; } - public static unsafe void CreateBinnedResources(IUnmanagedMemoryPool bufferPool, int maximumSubtreeCount, out Buffer buffer, out BinnedResources resources) + public static unsafe void CreateBinnedResources(BufferPool bufferPool, int maximumSubtreeCount, out Buffer buffer, out BinnedResources resources) { //TODO: This is a holdover from the pre-BufferPool tree design. It's pretty ugly. While some preallocation is useful (there's no reason to suffer the overhead of //pulling things out of the BufferPool over and over and over again), the degree to which this preallocates has a negative impact on L1 cache for subtree refines. @@ -578,7 +578,7 @@ unsafe void ReifyStagingNodes(int treeletRootIndex, Node* stagingNodes, public unsafe void BinnedRefine(int nodeIndex, ref QuickList subtreeReferences, int maximumSubtrees, ref QuickList treeletInternalNodes, - ref BinnedResources resources, IUnmanagedMemoryPool pool) + ref BinnedResources resources, BufferPool pool) { Debug.Assert(subtreeReferences.Count == 0, "The subtree references list should be empty since it's about to get filled."); Debug.Assert(subtreeReferences.Span.Length >= maximumSubtrees, "Subtree references list should have a backing array large enough to hold all possible subtrees."); diff --git a/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs b/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs index 257d922ca..bd1e0f1e8 100644 --- a/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs +++ b/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs @@ -164,7 +164,7 @@ public unsafe void RefitAndRefine(ref Tree tree, BufferPool pool, IThreadDispatc unsafe void CollectNodesForMultithreadedRefit(int nodeIndex, int multithreadingLeafCountThreshold, ref QuickList refitAndMarkTargets, - int refinementLeafCountThreshold, ref QuickList refinementCandidates, BufferPool pool, IUnmanagedMemoryPool threadPool) + int refinementLeafCountThreshold, ref QuickList refinementCandidates, BufferPool pool, BufferPool threadPool) { ref var node = ref Tree.Nodes[nodeIndex]; ref var metanode = ref Tree.Metanodes[nodeIndex]; @@ -201,7 +201,7 @@ unsafe void CollectNodesForMultithreadedRefit(int nodeIndex, } } - public unsafe void ExecuteRefitAndMarkJob(IUnmanagedMemoryPool threadPool, int workerIndex, int refitIndex) + public unsafe void ExecuteRefitAndMarkJob(BufferPool threadPool, int workerIndex, int refitIndex) { var nodeIndex = RefitNodes[refitIndex]; bool shouldUseMark; @@ -327,7 +327,7 @@ public unsafe void RefitAndMarkForWorker(int workerIndex, void* context) } - public unsafe void ExecuteRefineJob(ref QuickList subtreeReferences, ref QuickList treeletInternalNodes, ref BinnedResources resources, IUnmanagedMemoryPool threadPool, int refineIndex) + public unsafe void ExecuteRefineJob(ref QuickList subtreeReferences, ref QuickList treeletInternalNodes, ref BinnedResources resources, BufferPool threadPool, int refineIndex) { Tree.BinnedRefine(RefinementTargets[refineIndex], ref subtreeReferences, MaximumSubtrees, ref treeletInternalNodes, ref resources, threadPool); subtreeReferences.Count = 0; diff --git a/BepuPhysics/Trees/Tree_RefinementScheduling.cs b/BepuPhysics/Trees/Tree_RefinementScheduling.cs index 751abb14c..85f2fc696 100644 --- a/BepuPhysics/Trees/Tree_RefinementScheduling.cs +++ b/BepuPhysics/Trees/Tree_RefinementScheduling.cs @@ -37,7 +37,7 @@ unsafe float RefitAndMeasure(ref NodeChild child) } - unsafe float RefitAndMark(ref NodeChild child, int leafCountThreshold, ref QuickList refinementCandidates, IUnmanagedMemoryPool pool) + unsafe float RefitAndMark(ref NodeChild child, int leafCountThreshold, ref QuickList refinementCandidates, BufferPool pool) { Debug.Assert(leafCountThreshold > 1); @@ -87,7 +87,7 @@ unsafe float RefitAndMark(ref NodeChild child, int leafCountThreshold, ref Quick } - unsafe float RefitAndMark(int leafCountThreshold, ref QuickList refinementCandidates, IUnmanagedMemoryPool pool) + unsafe float RefitAndMark(int leafCountThreshold, ref QuickList refinementCandidates, BufferPool pool) { Debug.Assert(LeafCount > 2, "There's no reason to refit a tree with 2 or less elements. Nothing would happen."); @@ -178,7 +178,7 @@ readonly void GetRefineTuning(int frameIndex, int refinementCandidatesCount, flo targetRefinementCount = Math.Min(refinementCandidatesCount, (int)targetRefinementScale); } - public unsafe void RefitAndRefine(IUnmanagedMemoryPool pool, int frameIndex, float refineAggressivenessScale = 1) + public unsafe void RefitAndRefine(BufferPool pool, int frameIndex, float refineAggressivenessScale = 1) { //Don't proceed if the tree has no refitting or refinement required. This also guarantees that any nodes that do exist have two children. if (LeafCount <= 2) diff --git a/BepuPhysics/Trees/Tree_SweepBuilder.cs b/BepuPhysics/Trees/Tree_SweepBuilder.cs index b7c67df64..98f34db67 100644 --- a/BepuPhysics/Trees/Tree_SweepBuilder.cs +++ b/BepuPhysics/Trees/Tree_SweepBuilder.cs @@ -245,7 +245,7 @@ unsafe int CreateSweepBuilderNode(int parentIndex, int indexInParent, } - public unsafe void SweepBuild(IUnmanagedMemoryPool pool, Buffer leafBounds) + public unsafe void SweepBuild(BufferPool pool, Buffer leafBounds) { if (leafBounds.Length <= 0) throw new ArgumentException("Length must be positive."); diff --git a/Demos/Demos/ContactEventsDemo.cs b/Demos/Demos/ContactEventsDemo.cs index 7d8e1686e..f9b3772ac 100644 --- a/Demos/Demos/ContactEventsDemo.cs +++ b/Demos/Demos/ContactEventsDemo.cs @@ -215,7 +215,7 @@ public ContactEvents(IThreadDispatcher threadDispatcher = null, BufferPool pool listeners = new Listener[initialListenerCapacity]; } - IUnmanagedMemoryPool GetPoolForWorker(int workerIndex) + BufferPool GetPoolForWorker(int workerIndex) { return threadDispatcher == null ? pool : threadDispatcher.WorkerPools[workerIndex]; } diff --git a/Demos/Demos/CustomVoxelCollidableDemo.cs b/Demos/Demos/CustomVoxelCollidableDemo.cs index 059ea66d1..363eadcac 100644 --- a/Demos/Demos/CustomVoxelCollidableDemo.cs +++ b/Demos/Demos/CustomVoxelCollidableDemo.cs @@ -49,7 +49,7 @@ struct Voxels : IHomogeneousCompoundShape public readonly int ChildCount => VoxelIndices.Count; - public Voxels(QuickList voxelIndices, Vector3 voxelSize, IUnmanagedMemoryPool pool) + public Voxels(QuickList voxelIndices, Vector3 voxelSize, BufferPool pool) { VoxelIndices = voxelIndices; VoxelSize = voxelSize; @@ -70,7 +70,7 @@ public Voxels(QuickList voxelIndices, Vector3 voxelSize, IUnmanagedMemo pool.Return(ref bounds); } - public readonly ShapeBatch CreateShapeBatch(IUnmanagedMemoryPool pool, int initialCapacity, Shapes shapeBatches) + public readonly ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches) { //Shapes types are responsible for informing the shape system how to create a batch for them. //Convex shapes will return a ConvexShapeBatch, compound shapes a CompoundShapeBatch, @@ -206,7 +206,7 @@ public readonly void GetLocalChild(int childIndex, ref BoxWide shapeWide) [MethodImpl(MethodImplOptions.AggressiveInlining)] - public readonly unsafe void FindLocalOverlaps(ref Buffer pairs, IUnmanagedMemoryPool pool, Shapes shapes, ref TOverlaps overlaps) + public readonly unsafe void FindLocalOverlaps(ref Buffer pairs, BufferPool pool, Shapes shapes, ref TOverlaps overlaps) where TOverlaps : struct, ICollisionTaskOverlaps where TSubpairOverlaps : struct, ICollisionTaskSubpairOverlaps { @@ -226,7 +226,7 @@ public readonly unsafe void FindLocalOverlaps(ref B } } - public readonly unsafe void FindLocalOverlaps(Vector3 min, Vector3 max, Vector3 sweep, float maximumT, IUnmanagedMemoryPool pool, Shapes shapes, void* overlaps) where TOverlaps : ICollisionTaskSubpairOverlaps + public readonly unsafe void FindLocalOverlaps(Vector3 min, Vector3 max, Vector3 sweep, float maximumT, BufferPool pool, Shapes shapes, void* overlaps) where TOverlaps : ICollisionTaskSubpairOverlaps { //Similar to the non-swept FindLocalOverlaps function above, this just adds the overlaps to the provided collection. //Some unfortunate loss of type information due to some language limitations around generic pointers- pretend the overlaps pointer has type TOverlaps*. @@ -237,7 +237,7 @@ public readonly unsafe void FindLocalOverlaps(Vector3 min, Vector3 ma Tree.Sweep(min, max, sweep, maximumT, ref enumerator); } - public void Dispose(IUnmanagedMemoryPool pool) + public void Dispose(BufferPool pool) { Tree.Dispose(pool); VoxelIndices.Dispose(pool); From 2ae41c2daa892dffbf5a73801185509034f95ed3 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 26 Nov 2022 12:35:07 -0600 Subject: [PATCH 662/947] Removed incorrect note. --- BepuUtilities/IThreadDispatcher.cs | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/BepuUtilities/IThreadDispatcher.cs b/BepuUtilities/IThreadDispatcher.cs index 2bf4ffbaa..d0c1f0d49 100644 --- a/BepuUtilities/IThreadDispatcher.cs +++ b/BepuUtilities/IThreadDispatcher.cs @@ -64,11 +64,7 @@ void DispatchWorkers(ThreadDispatcherWorker workerBody, int maximumWorkerCount = /// /// All usages of these worker pools within the simulation are guaranteed to return thread pool memory before the function returns. In other words, /// thread memory pools are used for strictly ephemeral memory, and it will never be held by the simulation outside the scope of a function that - /// takes the IThreadDispatcher as input. - /// - /// Further, the simulation will often flush these pools. They should not be relied upon to store long term data; - /// consider creating another instance, using other instances, or using other instances. - /// The set of memory pools associated with the dispatcher's thread workers. + /// takes the IThreadDispatcher as input. WorkerBufferPools WorkerPools { get; } } } From 011d4a581bb524c9c602b66c9fc65877263576ed Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 28 Nov 2022 21:41:31 -0600 Subject: [PATCH 663/947] Flailing a bit with thread dispatcher. Contextful worker bodies no longer required. --- BepuPhysics/BatchCompressor.cs | 4 +- .../CollidableOverlapFinder.cs | 4 +- BepuPhysics/CollisionDetection/NarrowPhase.cs | 4 +- .../CollisionDetection/NarrowPhasePreflush.cs | 4 +- BepuPhysics/IslandAwakener.cs | 8 +- BepuPhysics/IslandSleeper.cs | 18 ++--- BepuPhysics/PoseIntegrator.cs | 8 +- BepuPhysics/Solver_Solve.cs | 10 +-- BepuPhysics/Trees/Tree_IntertreeQueriesMT.cs | 3 +- .../Trees/Tree_MultithreadedRefitRefine.cs | 8 +- BepuPhysics/Trees/Tree_SelfQueriesMT.cs | 2 +- BepuUtilities/IThreadDispatcher.cs | 5 +- BepuUtilities/ThreadDispatcher.cs | 73 +++++++++++++++---- Demos/DemoSet.cs | 2 +- .../Demos/Characters/CharacterControllers.cs | 8 +- Demos/Demos/RayCastingDemo.cs | 6 +- Demos/Program.cs | 3 +- Demos/SpecializedTests/TreeTest.cs | 2 +- Demos/SpecializedTests/VolumeQueryTests.cs | 6 +- 19 files changed, 109 insertions(+), 69 deletions(-) diff --git a/BepuPhysics/BatchCompressor.cs b/BepuPhysics/BatchCompressor.cs index 5513b2f3d..ba3ee0067 100644 --- a/BepuPhysics/BatchCompressor.cs +++ b/BepuPhysics/BatchCompressor.cs @@ -97,7 +97,7 @@ struct AnalysisRegion QuickList analysisJobs; - ThreadDispatcherWorker analysisWorkerDelegate; + Action analysisWorkerDelegate; public BatchCompressor(Solver solver, Bodies bodies, float targetCandidateFraction = 0.005f, float maximumCompressionFraction = 0.0005f) { this.Solver = solver; @@ -109,7 +109,7 @@ public BatchCompressor(Solver solver, Bodies bodies, float targetCandidateFracti - unsafe void AnalysisWorker(int workerIndex, void* context) + unsafe void AnalysisWorker(int workerIndex) { int jobIndex; while ((jobIndex = Interlocked.Increment(ref analysisJobIndex)) < analysisJobs.Count) diff --git a/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs b/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs index 487705c4a..6f99cac36 100644 --- a/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs +++ b/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs @@ -62,7 +62,7 @@ public void Handle(int indexA, int indexB) BroadPhase broadPhase; SelfOverlapHandler[] selfHandlers; IntertreeOverlapHandler[] intertreeHandlers; - ThreadDispatcherWorker workerAction; + Action workerAction; int nextJobIndex; public CollidableOverlapFinder(NarrowPhase narrowPhase, BroadPhase broadPhase) { @@ -73,7 +73,7 @@ public CollidableOverlapFinder(NarrowPhase narrowPhase, BroadPhase b workerAction = Worker; } - void Worker(int workerIndex, void* context) + void Worker(int workerIndex) { Debug.Assert(workerIndex >= 0 && workerIndex < intertreeHandlers.Length && workerIndex < selfHandlers.Length); diff --git a/BepuPhysics/CollisionDetection/NarrowPhase.cs b/BepuPhysics/CollisionDetection/NarrowPhase.cs index d7d9e3631..c880b1ae6 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhase.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhase.cs @@ -198,8 +198,8 @@ public void Prepare(float dt, IThreadDispatcher threadDispatcher = null) int flushJobIndex; QuickList flushJobs; IThreadDispatcher threadDispatcher; - ThreadDispatcherWorker flushWorkerLoop; - void FlushWorkerLoop(int workerIndex, void* context) + Action flushWorkerLoop; + void FlushWorkerLoop(int workerIndex) { int jobIndex; while ((jobIndex = Interlocked.Increment(ref flushJobIndex)) < flushJobs.Count) diff --git a/BepuPhysics/CollisionDetection/NarrowPhasePreflush.cs b/BepuPhysics/CollisionDetection/NarrowPhasePreflush.cs index 08f31e821..3a88493a2 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhasePreflush.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhasePreflush.cs @@ -109,8 +109,8 @@ public struct SortConstraintTarget int preflushJobIndex; QuickList preflushJobs; - ThreadDispatcherWorker preflushWorkerLoop; - void PreflushWorkerLoop(int workerIndex, void* context) + Action preflushWorkerLoop; + void PreflushWorkerLoop(int workerIndex) { int jobIndex; while ((jobIndex = Interlocked.Increment(ref preflushJobIndex)) < preflushJobs.Count) diff --git a/BepuPhysics/IslandAwakener.cs b/BepuPhysics/IslandAwakener.cs index 339d4c49b..a71c7dcf1 100644 --- a/BepuPhysics/IslandAwakener.cs +++ b/BepuPhysics/IslandAwakener.cs @@ -135,9 +135,9 @@ public void AwakenSets(ref QuickList setIndices, IThreadDispatcher threadDi int jobIndex; int jobCount; //TODO: once again, we repeat this worker pattern. We've done this, what, seven times? It wouldn't even be that difficult to centralize it. We'll get around to that at some point. - ThreadDispatcherWorker phaseOneWorkerDelegate; - ThreadDispatcherWorker phaseTwoWorkerDelegate; - internal void PhaseOneWorker(int workerIndex, void* context) + Action phaseOneWorkerDelegate; + Action phaseTwoWorkerDelegate; + internal void PhaseOneWorker(int workerIndex) { while (true) { @@ -147,7 +147,7 @@ internal void PhaseOneWorker(int workerIndex, void* context) ExecutePhaseOneJob(index); } } - internal void PhaseTwoWorker(int workerIndex, void* context) + internal void PhaseTwoWorker(int workerIndex) { while (true) { diff --git a/BepuPhysics/IslandSleeper.cs b/BepuPhysics/IslandSleeper.cs index 653a791c7..824d1bc7c 100644 --- a/BepuPhysics/IslandSleeper.cs +++ b/BepuPhysics/IslandSleeper.cs @@ -324,16 +324,16 @@ void FindIslands(int workerIndex, BufferPool threadPool) FindIslands(workerIndex, threadPool, ref predicate); } } - ThreadDispatcherWorker findIslandsDelegate; + Action findIslandsDelegate; bool forceSleep; - void FindIslands(int workerIndex, void* context) + void FindIslands(int workerIndex) { //The only reason we separate this out is to make it easier for the main pool to be passed in if there is only a single thread. FindIslands(workerIndex, threadDispatcher.WorkerPools[workerIndex]); } - ThreadDispatcherWorker gatherDelegate; - unsafe void Gather(int workerIndex, void* context) + Action gatherDelegate; + unsafe void Gather(int workerIndex) { while (true) { @@ -389,9 +389,9 @@ unsafe void Gather(int workerIndex, void* context) } int typeBatchConstraintRemovalJobCount; - ThreadDispatcherWorker typeBatchConstraintRemovalDelegate; + Action typeBatchConstraintRemovalDelegate; - void TypeBatchConstraintRemoval(int workerIndex, void* context) + void TypeBatchConstraintRemoval(int workerIndex) { while (true) { @@ -507,8 +507,8 @@ void ExecuteRemoval(ref RemovalJob job) break; } } - ThreadDispatcherWorker executeRemovalWorkDelegate; - void ExecuteRemovalWork(int workerIndex, void* context) + Action executeRemovalWorkDelegate; + void ExecuteRemovalWork(int workerIndex) { while (true) { @@ -797,7 +797,7 @@ void DisposeWorkerTraversalResults() } else { - Gather(0, null); + Gather(0); } DisposeWorkerTraversalResults(); gatheringJobs.Dispose(pool); diff --git a/BepuPhysics/PoseIntegrator.cs b/BepuPhysics/PoseIntegrator.cs index 650fe4ef2..0fde1a8a3 100644 --- a/BepuPhysics/PoseIntegrator.cs +++ b/BepuPhysics/PoseIntegrator.cs @@ -273,7 +273,7 @@ public unsafe class PoseIntegrator : IPoseIntegrator where TCallback public TCallbacks Callbacks; - ThreadDispatcherWorker predictBoundingBoxesWorker; + Action predictBoundingBoxesWorker; public PoseIntegrator(Bodies bodies, Shapes shapes, BroadPhase broadPhase, TCallbacks callbacks) { this.bodies = bodies; @@ -395,7 +395,7 @@ bool TryGetJob(int maximumJobInterval, out int start, out int exclusiveEnd) return true; } - void PredictBoundingBoxesWorker(int workerIndex, void* context) + void PredictBoundingBoxesWorker(int workerIndex) { var boundingBoxUpdater = new BoundingBoxBatcher(bodies, shapes, broadPhase, threadDispatcher.WorkerPools[workerIndex], cachedDt); var bundleCount = BundleIndexing.GetBundleCount(bodies.ActiveSet.Count); @@ -690,9 +690,9 @@ unsafe void IntegrateBundlesAfterSubstepping(ref IndexSet mergedConstrainedBodyH } } - ThreadDispatcherWorker integrateAfterSubsteppingWorker; + Action integrateAfterSubsteppingWorker; IndexSet constrainedBodies; - private void IntegrateAfterSubsteppingWorker(int workerIndex, void* context) + private void IntegrateAfterSubsteppingWorker(int workerIndex) { var bundleCount = BundleIndexing.GetBundleCount(bodies.ActiveSet.Count); var substepDt = cachedDt / substepCount; diff --git a/BepuPhysics/Solver_Solve.cs b/BepuPhysics/Solver_Solve.cs index 8fc664102..08d0d0d94 100644 --- a/BepuPhysics/Solver_Solve.cs +++ b/BepuPhysics/Solver_Solve.cs @@ -458,8 +458,8 @@ protected static int GetUniformlyDistributedStart(int workerIndex, int blockCoun return offset + blocksPerWorker * workerIndex + Math.Min(remainder, workerIndex); } - ThreadDispatcherWorker solveWorker; - void SolveWorker(int workerIndex, void* context) + Action solveWorker; + void SolveWorker(int workerIndex) { //The solver has two codepaths: one thread, acting as an orchestrator, and the others, just waiting to be used. //There is no requirement that a worker thread above index 0 actually runs at all for a given dispatch. @@ -754,7 +754,7 @@ int GetVelocityIterationCountForSubstepIndex(int substepIndex) return VelocityIterationCount; } //Buffer> debugStageWorkBlocksCompleted; - protected void ExecuteMultithreaded(float dt, IThreadDispatcher threadDispatcher, ThreadDispatcherWorker workDelegate) + protected void ExecuteMultithreaded(float dt, IThreadDispatcher threadDispatcher, Action workDelegate) { var workerCount = substepContext.WorkerCount = threadDispatcher.ThreadCount; substepContext.Dt = dt; @@ -1046,7 +1046,7 @@ unsafe bool ComputeIntegrationResponsibilitiesForConstraintRegion return mergedFlagBundles != 0; } - void ConstraintIntegrationResponsibilitiesWorker(int workerIndex, void* context) + void ConstraintIntegrationResponsibilitiesWorker(int workerIndex) { int jobIndex; while ((jobIndex = Interlocked.Increment(ref nextConstraintIntegrationResponsibilityJobIndex) - 1) < integrationResponsibilityPrepassJobs.Count) @@ -1069,7 +1069,7 @@ void ConstraintIntegrationResponsibilitiesWorker(int workerIndex, void* context) /// Type batches with no integration responsibilities can use a codepath with no integration checks at all. /// Buffer> coarseBatchIntegrationResponsibilities; - ThreadDispatcherWorker constraintIntegrationResponsibilitiesWorker; + Action constraintIntegrationResponsibilitiesWorker; IndexSet mergedConstrainedBodyHandles; public override unsafe IndexSet PrepareConstraintIntegrationResponsibilities(IThreadDispatcher threadDispatcher = null) diff --git a/BepuPhysics/Trees/Tree_IntertreeQueriesMT.cs b/BepuPhysics/Trees/Tree_IntertreeQueriesMT.cs index 0de1c95a5..de22b26ef 100644 --- a/BepuPhysics/Trees/Tree_IntertreeQueriesMT.cs +++ b/BepuPhysics/Trees/Tree_IntertreeQueriesMT.cs @@ -153,8 +153,7 @@ public unsafe void ExecuteJob(int jobIndex, int workerIndex) /// Executes a single worker of the multithreaded self test. /// /// Index of the worker executing this set of tests. - /// Context of the dispatch, if any. - public unsafe void PairTest(int workerIndex, void* context) + public unsafe void PairTest(int workerIndex) { Debug.Assert(workerIndex >= 0 && workerIndex < OverlapHandlers.Length); int nextNodePairIndex; diff --git a/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs b/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs index bd1e0f1e8..174951505 100644 --- a/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs +++ b/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs @@ -25,12 +25,12 @@ public unsafe class RefitAndRefineMultithreadedContext int RefinementLeafCountThreshold; Buffer> RefinementCandidates; - ThreadDispatcherWorker RefitAndMarkAction; + Action RefitAndMarkAction; int RefineIndex; public QuickList RefinementTargets; public int MaximumSubtrees; - ThreadDispatcherWorker RefineAction; + Action RefineAction; IThreadDispatcher threadDispatcher; @@ -310,7 +310,7 @@ public unsafe void ExecuteRefitAndMarkJob(BufferPool threadPool, int workerIndex } } } - public unsafe void RefitAndMarkForWorker(int workerIndex, void* context) + public unsafe void RefitAndMarkForWorker(int workerIndex) { if (RefitNodes.Count == 0) return; @@ -334,7 +334,7 @@ public unsafe void ExecuteRefineJob(ref QuickList subtreeReferences, ref Qu treeletInternalNodes.Count = 0; } - public unsafe void RefineForWorker(int workerIndex, void* context) + public unsafe void RefineForWorker(int workerIndex) { if (RefinementTargets.Count == 0) return; diff --git a/BepuPhysics/Trees/Tree_SelfQueriesMT.cs b/BepuPhysics/Trees/Tree_SelfQueriesMT.cs index b767ee256..d08ead631 100644 --- a/BepuPhysics/Trees/Tree_SelfQueriesMT.cs +++ b/BepuPhysics/Trees/Tree_SelfQueriesMT.cs @@ -141,7 +141,7 @@ public unsafe void ExecuteJob(int jobIndex, int workerIndex) /// Executes a single worker of the multithreaded self test. /// /// Index of the worker executing this set of tests. - public unsafe void PairTest(int workerIndex, void* context) + public unsafe void PairTest(int workerIndex) { Debug.Assert(workerIndex >= 0 && workerIndex < OverlapHandlers.Length); int nextNodePairIndex; diff --git a/BepuUtilities/IThreadDispatcher.cs b/BepuUtilities/IThreadDispatcher.cs index d0c1f0d49..b632e9905 100644 --- a/BepuUtilities/IThreadDispatcher.cs +++ b/BepuUtilities/IThreadDispatcher.cs @@ -54,10 +54,7 @@ public unsafe interface IThreadDispatcher /// /// Delegate to be invoked on every worker. /// Maximum number of workers to dispatch. - void DispatchWorkers(ThreadDispatcherWorker workerBody, int maximumWorkerCount = int.MaxValue) - { - DispatchWorkers(workerBody, null, maximumWorkerCount); - } + void DispatchWorkers(Action workerBody, int maximumWorkerCount = int.MaxValue); /// /// Gets the set of memory pools associated with thread workers. diff --git a/BepuUtilities/ThreadDispatcher.cs b/BepuUtilities/ThreadDispatcher.cs index e6a8cff7f..8f1c6ccdc 100644 --- a/BepuUtilities/ThreadDispatcher.cs +++ b/BepuUtilities/ThreadDispatcher.cs @@ -49,11 +49,12 @@ public ThreadDispatcher(int threadCount, int threadPoolBlockAllocationSize = 163 void DispatchThread(int workerIndex) { - Debug.Assert(workerBody != null ^ workerBodyPointer != null); - if (workerBody != null) - workerBody(workerIndex, context); - else - workerBodyPointer(workerIndex, context); + switch (workerType) + { + case WorkerType.Managed: managedWorker(workerIndex); break; + case WorkerType.ManagedWithUnmanagedContext: managedWithContextWorker(workerIndex, context); break; + case WorkerType.Unmanaged: unmanagedWorker(workerIndex, context); break; + } if (Interlocked.Decrement(ref remainingWorkerCounter) == -1) { @@ -61,9 +62,29 @@ void DispatchThread(int workerIndex) } } - volatile ThreadDispatcherWorker workerBody; - volatile delegate* workerBodyPointer; + enum WorkerType + { + /// + /// Action{int} + /// + Managed, + /// + /// + /// + ManagedWithUnmanagedContext, + /// + /// Includes unmanaged context by default because these jobs can only be static functions and *some* context is probably required. + /// delegate*{int, void*, void} + /// + Unmanaged, + } + + volatile WorkerType workerType; + volatile Action managedWorker; + volatile ThreadDispatcherWorker managedWithContextWorker; + volatile delegate* unmanagedWorker; volatile void* context; + int remainingWorkerCounter; void WorkerLoop(object untypedSignal) @@ -92,18 +113,19 @@ void SignalThreads(int maximumWorkerCount) } /// - public void DispatchWorkers(delegate* workerBody, void* context = null, int maximumWorkerCount = int.MaxValue) + public void DispatchWorkers(delegate* workerBody, void* context, int maximumWorkerCount = int.MaxValue) { if (maximumWorkerCount > 1) { - Debug.Assert(this.workerBody == null && this.workerBodyPointer == null); - workerBodyPointer = workerBody; + Debug.Assert(this.managedWorker == null && this.managedWithContextWorker == null && this.unmanagedWorker == null && this.context == null); + workerType = WorkerType.Unmanaged; + unmanagedWorker = workerBody; this.context = context; SignalThreads(maximumWorkerCount); //Calling thread does work. No reason to spin up another worker and block this one! DispatchThread(0); finished.WaitOne(); - workerBodyPointer = null; + unmanagedWorker = null; this.context = null; } else if (maximumWorkerCount == 1) @@ -113,18 +135,19 @@ public void DispatchWorkers(delegate* workerBody, void* contex } /// - public void DispatchWorkers(ThreadDispatcherWorker workerBody, void* context = null, int maximumWorkerCount = int.MaxValue) + public void DispatchWorkers(ThreadDispatcherWorker workerBody, void* context, int maximumWorkerCount = int.MaxValue) { if (maximumWorkerCount > 1) { - Debug.Assert(this.workerBody == null && this.workerBodyPointer == null); - this.workerBody = workerBody; + Debug.Assert(this.managedWorker == null && this.managedWithContextWorker == null && this.unmanagedWorker == null && this.context == null); + workerType = WorkerType.ManagedWithUnmanagedContext; + this.managedWithContextWorker = workerBody; this.context = context; SignalThreads(maximumWorkerCount); //Calling thread does work. No reason to spin up another worker and block this one! DispatchThread(0); finished.WaitOne(); - this.workerBody = null; + this.managedWithContextWorker = null; this.context = null; } else if (maximumWorkerCount == 1) @@ -133,6 +156,26 @@ public void DispatchWorkers(ThreadDispatcherWorker workerBody, void* context = n } } + /// + public void DispatchWorkers(Action workerBody, int maximumWorkerCount = int.MaxValue) + { + if (maximumWorkerCount > 1) + { + Debug.Assert(this.managedWorker == null && this.managedWithContextWorker == null && this.unmanagedWorker == null && this.context == null); + workerType = WorkerType.ManagedWithUnmanagedContext; + this.managedWorker = workerBody; + SignalThreads(maximumWorkerCount); + //Calling thread does work. No reason to spin up another worker and block this one! + DispatchThread(0); + finished.WaitOne(); + this.managedWorker = null; + } + else if (maximumWorkerCount == 1) + { + workerBody(0); + } + } + volatile bool disposed; /// diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index 46748bf4c..dfffb7775 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -46,7 +46,7 @@ struct Option public DemoSet() { - AddOption(); + //AddOption(); //AddOption(); //AddOption(); AddOption(); diff --git a/Demos/Demos/Characters/CharacterControllers.cs b/Demos/Demos/Characters/CharacterControllers.cs index b99e5d334..bd1596fe6 100644 --- a/Demos/Demos/Characters/CharacterControllers.cs +++ b/Demos/Demos/Characters/CharacterControllers.cs @@ -433,8 +433,8 @@ unsafe void ExpandBoundingBoxes(int start, int count) } int boundingBoxExpansionJobIndex; - ThreadDispatcherWorker expandBoundingBoxesWorker; - unsafe void ExpandBoundingBoxesWorker(int workerIndex, void* context) + Action expandBoundingBoxesWorker; + unsafe void ExpandBoundingBoxesWorker(int workerIndex) { while (true) { @@ -735,8 +735,8 @@ struct AnalyzeContactsJob int analysisJobIndex; int analysisJobCount; Buffer jobs; - ThreadDispatcherWorker analyzeContactsWorker; - unsafe void AnalyzeContactsWorker(int workerIndex, void* context) + Action analyzeContactsWorker; + unsafe void AnalyzeContactsWorker(int workerIndex) { int jobIndex; while ((jobIndex = Interlocked.Increment(ref analysisJobIndex)) < analysisJobCount) diff --git a/Demos/Demos/RayCastingDemo.cs b/Demos/Demos/RayCastingDemo.cs index 8ca864e9a..e62fba392 100644 --- a/Demos/Demos/RayCastingDemo.cs +++ b/Demos/Demos/RayCastingDemo.cs @@ -250,7 +250,7 @@ unsafe class IntersectionAlgorithm public TimingsRingBuffer Timings; Func worker; - ThreadDispatcherWorker internalWorker; + Action internalWorker; public int JobIndex; public IntersectionAlgorithm(string name, Func worker, @@ -263,7 +263,7 @@ public IntersectionAlgorithm(string name, Func pool.Take(largestRayCount, out Results); } - unsafe void ExecuteWorker(int workerIndex, void* context) + unsafe void ExecuteWorker(int workerIndex) { var intersectionCount = worker(workerIndex, this); Interlocked.Add(ref IntersectionCount, intersectionCount); @@ -286,7 +286,7 @@ public void Execute(ref QuickList rays, IThreadDispatcher dispatcher) } else { - internalWorker(0, null); + internalWorker(0); } var stop = Stopwatch.GetTimestamp(); Timings.Add((stop - start) / (double)Stopwatch.Frequency); diff --git a/Demos/Program.cs b/Demos/Program.cs index c31a73c7f..72ceababf 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -1,6 +1,7 @@ using BepuPhysics.Trees; using BepuUtilities; using DemoContentLoader; +using Demos.Demos; using Demos.SpecializedTests; using DemoUtilities; using OpenTK; @@ -20,7 +21,7 @@ static void Main() { content = ContentArchive.Load(stream); } - HeadlessTest.Test(content, 4, 32, 512); + //HeadlessTest.Test(content, 4, 32, 512); var demo = new DemoHarness(loop, content); loop.Run(demo); loop.Dispose(); diff --git a/Demos/SpecializedTests/TreeTest.cs b/Demos/SpecializedTests/TreeTest.cs index ed1675d29..33f54ccbc 100644 --- a/Demos/SpecializedTests/TreeTest.cs +++ b/Demos/SpecializedTests/TreeTest.cs @@ -72,7 +72,7 @@ public static void Test() var refineContext = new Tree.RefitAndRefineMultithreadedContext(); var selfTestContext = new Tree.MultithreadedSelfTest(pool); var overlapHandlers = new OverlapHandler[threadDispatcher.ThreadCount]; - ThreadDispatcherWorker pairTestAction = selfTestContext.PairTest; + Action pairTestAction = selfTestContext.PairTest; var removedLeafHandles = new QuickList(leafCount, pool); for (int i = 0; i < iterations; ++i) { diff --git a/Demos/SpecializedTests/VolumeQueryTests.cs b/Demos/SpecializedTests/VolumeQueryTests.cs index 451a2b61d..6ae34526a 100644 --- a/Demos/SpecializedTests/VolumeQueryTests.cs +++ b/Demos/SpecializedTests/VolumeQueryTests.cs @@ -137,7 +137,7 @@ unsafe class BoxQueryAlgorithm public TimingsRingBuffer Timings; Func worker; - ThreadDispatcherWorker internalWorker; + Action internalWorker; public int JobIndex; public BoxQueryAlgorithm(string name, BufferPool pool, Func worker, int timingSampleCount = 16) @@ -148,7 +148,7 @@ public BoxQueryAlgorithm(string name, BufferPool pool, Func boxes, IThreadDispatcher dispatch } else { - internalWorker(0, null); + internalWorker(0); } var stop = Stopwatch.GetTimestamp(); Timings.Add((stop - start) / (double)Stopwatch.Frequency); From c01c611f96706651b85b80596fe3b47eac712b13 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 28 Nov 2022 21:52:10 -0600 Subject: [PATCH 664/947] Doesn't build. Revamping binned builder resource acquisition. Still need to get managed references in, or... make the pools unmanaged, oi. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 110 +++++++++++++++--------- 1 file changed, 70 insertions(+), 40 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index bb5c1917d..8084dbabf 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -432,9 +432,26 @@ unsafe struct BinnedBuildWorkerContext where TLeafCounts : unmanage { /// /// Stores per-worker prepass bounds accumulated over multiple tasks. If there are less tasks than workers, then only the lower contiguous region of these bounds are used. + /// This allocation is ephemeral; it is allocated from the current worker when needed. + /// Note that the allocation occurs on the loop dispatching thread: the workers that help with the loop do not have to allocate anything themselves. /// public Buffer PrepassWorkers; + /// + /// Bins associated with any workers that end up contributing to this worker's dispatch of a binning loop. If there are less tasks than workers, then only the lower contiguous region of these bounds are used. + /// This allocation is ephemeral; it is allocated from the current worker when needed. + /// Note that the allocation occurs on the loop dispatching thread: the workers that help with the loop do not have to allocate anything themselves. + /// public Buffer BinSubtreesWorkers; + /// + /// Whether a given worker contributed to the subtree binning process. If this worker did not contribute, there's no reason to merge its bins. + /// This allocation is ephemeral; it is allocated from the current worker when needed. + /// Note that the allocation occurs on the loop dispatching thread: the workers that help with the loop do not have to allocate anything themselves. + /// + public Buffer WorkerHelpedWithBinning; + + /// + /// Bins associated with this worker for use in the SAH scan. This will persist across the build. + /// public Buffer BinBoundingBoxesScan; public int BinCount; @@ -445,6 +462,8 @@ unsafe struct BinnedBuildWorkerContext where TLeafCounts : unmanage public Vector4 OffsetToBinIndex; public Vector4 MaximumBinIndex; + public int WorkerCount; + public int SubtreeStartIndex; public int SubtreeCount; @@ -468,34 +487,35 @@ static Buffer Suballocate(Buffer buffer, ref int start, int count) w start += size; return buffer.Slice(previousStart, size).As(); } - static Buffer SuballocateAligned(Buffer buffer, ref int start, int count, int alignmentMask) where T : unmanaged - { - Debug.Assert(BitOperations.IsPow2(alignmentMask + 1)); - start = (start + alignmentMask) & (~alignmentMask); - return Suballocate(buffer, ref start, count); - } - public BinnedBuildWorkerContext(Buffer allocation, ref int allocationStart, int workerCount, int binCount, Buffer bounds, TLeafCounts leafCounts) + public void AllocatePrepassWorkers(BufferPool pool, int taskCount) => pool.Take(Unsafe.SizeOf() * int.Min(WorkerCount, taskCount), out PrepassWorkers); + public void DisposePrepassWorkers(BufferPool pool) => pool.Return(ref PrepassWorkers); + public void AllocateBinSubtreesWorkers(BufferPool pool, int taskCount) { - PrepassWorkers = Suballocate(allocation, ref allocationStart, workerCount); - BinBoundingBoxesScan = Suballocate(allocation, ref allocationStart, binCount); - BinSubtreesWorkers = Suballocate(allocation, ref allocationStart, workerCount); - for (int i = 0; i < workerCount; ++i) + var effectiveWorkerCount = int.Min(WorkerCount, taskCount); + //Pull one allocation from the pool instead of 1 + workerCount * 2. Slight reduction in overhead. Note that this means we only need to return one buffer of the associated id at the end! + var allocationSize = (sizeof(BinSubtreesWorkerContext) + (sizeof(BoundingBox4) + sizeof(int)) * BinCount + sizeof(bool) * WorkerCount) * effectiveWorkerCount; + pool.Take(allocationSize, out var allocation); + int start = 0; + BinSubtreesWorkers = Suballocate(allocation, ref start, effectiveWorkerCount); + for (int i = 0; i < effectiveWorkerCount; ++i) { - ref var binSubtreesWorker = ref BinSubtreesWorkers[i]; - binSubtreesWorker.BinBoundingBoxes = Suballocate(allocation, ref allocationStart, binCount); - binSubtreesWorker.BinLeafCounts = SuballocateAligned(allocation, ref allocationStart, binCount, Unsafe.SizeOf() - 1); - //binSubtreesWorker.BinLeafCounts = Suballocate(allocation, ref allocationStart, binCount); + ref var worker = ref BinSubtreesWorkers[i]; + worker.BinBoundingBoxes = Suballocate(allocation, ref start, BinCount); + worker.BinLeafCounts = Suballocate(allocation, ref start, BinCount); } - Bounds = bounds; - LeafCounts = leafCounts; + WorkerHelpedWithBinning = Suballocate(allocation, ref start, effectiveWorkerCount); + WorkerHelpedWithBinning.Clear(0, effectiveWorkerCount); } - public static int ComputeRequiredSizeInBytes(int workerCount, int binCount) + public void DisposeBinSubtreesWorkers(BufferPool pool) => pool.Return(ref BinSubtreesWorkers); //Only need to return the main buffer because all the other allocations share the same id! + + + public BinnedBuildWorkerContext(Buffer bounds, Buffer binBoundingBoxesScan, TLeafCounts leafCounts, int workerCount) { - //Align the leaf counts allocation size so that the bounding box allocations stay aligned despite allocation interleaving. - var binLeafCountsSize = ((binCount * Unsafe.SizeOf() + Unsafe.SizeOf() - 1) / Unsafe.SizeOf()) * Unsafe.SizeOf(); - var binSubtreesSize = binCount * Unsafe.SizeOf() + binLeafCountsSize; - return workerCount * (Unsafe.SizeOf() + Unsafe.SizeOf() + binSubtreesSize) + binCount * Unsafe.SizeOf(); + WorkerCount = workerCount; + BinBoundingBoxesScan = binBoundingBoxesScan; + Bounds = bounds; + LeafCounts = leafCounts; } public int SetupTaskCounts(int subtreeStartIndex, int slotCount, int slotsPerTaskTarget, int maximumTaskCountPerSubmission) @@ -580,11 +600,12 @@ unsafe static void CentroidPrepassWorker(long taskId, void* untyped } } - unsafe static BoundingBox4 MultithreadedCentroidPrepass(MultithreadBinnedBuildContext* context, int subtreeStartIndex, int subtreeCount, int workerIndex) + unsafe static BoundingBox4 MultithreadedCentroidPrepass(MultithreadBinnedBuildContext* context, int subtreeStartIndex, int subtreeCount, int workerIndex, BufferPool workerPool) where TLeafCounts : unmanaged, ILeafCountBuffer { ref var worker = ref context->Workers[workerIndex]; var taskCount = worker.SetupTaskCounts(subtreeStartIndex, subtreeCount, SubtreesPerThreadForCentroidPrepass, context->MaximumTaskCountPerSubmission); + worker.AllocatePrepassWorkers(workerPool, taskCount); //Don't bother initializing more slots than we have tasks. Note that this requires special handling on the task level; //if we have less tasks than workers, then the task needs to distinguish that fact. var activeWorkerCount = Math.Min(context->Workers.Length, taskCount); @@ -607,6 +628,7 @@ unsafe static BoundingBox4 MultithreadedCentroidPrepass(Multithread centroidBounds.Min = Vector4.Min(workerBounds.Min, centroidBounds.Min); centroidBounds.Max = Vector4.Max(workerBounds.Max, centroidBounds.Max); } + worker.DisposePrepassWorkers(workerPool); return centroidBounds; } @@ -632,7 +654,9 @@ unsafe static void BinSubtreesWorker(long taskId, void* untypedCont { ref var context = ref *(BinnedBuildWorkerContext*)untypedContext; //Note that if we have more workers than tasks, we use the task id to index into the caches (and initialize the data here rather then before dispatching). - ref var worker = ref context.BinSubtreesWorkers[context.TaskCountFitsInWorkerCount ? (int)taskId : workerIndex]; + var effectiveWorkerIndex = context.TaskCountFitsInWorkerCount ? (int)taskId : workerIndex; + ref var worker = ref context.BinSubtreesWorkers[effectiveWorkerIndex]; + context.WorkerHelpedWithBinning[effectiveWorkerIndex] = true; if (context.TaskCountFitsInWorkerCount) { for (int i = 0; i < context.BinCount; ++i) @@ -650,10 +674,11 @@ unsafe static void BinSubtreesWorker(long taskId, void* untypedCont unsafe static void MultithreadedBinSubtrees(MultithreadBinnedBuildContext* context, Vector4 centroidBoundsMin, bool useX, bool useY, Vector128 permuteMask, int axisIndex, Vector4 offsetToBinIndex, Vector4 maximumBinIndex, - int subtreeStartIndex, int subtreeCount, int binCount, int workerIndex) where TLeafCounts : unmanaged, ILeafCountBuffer + int subtreeStartIndex, int subtreeCount, int binCount, int workerIndex, BufferPool workerPool) where TLeafCounts : unmanaged, ILeafCountBuffer { ref var worker = ref context->Workers[workerIndex]; var taskCount = worker.SetupTaskCounts(subtreeStartIndex, subtreeCount, SubtreesPerThreadForBinning, context->MaximumTaskCountPerSubmission); + worker.AllocateBinSubtreesWorkers(workerPool, taskCount); //Don't bother initializing more slots than we have tasks. Note that this requires special handling on the task level; //if we have less tasks than workers, then the task needs to distinguish that fact. var activeWorkerCount = Math.Min(context->Workers.Length, taskCount); @@ -689,16 +714,21 @@ unsafe static void MultithreadedBinSubtrees(MultithreadBinnedBuildC ref var cache0 = ref worker.BinSubtreesWorkers[0]; for (int cacheIndex = 1; cacheIndex < activeWorkerCount; ++cacheIndex) { - ref var cache = ref worker.BinSubtreesWorkers[cacheIndex]; - for (int binIndex = 0; binIndex < binCount; ++binIndex) + //Only bother merging from workers that actually did anything. + if (worker.WorkerHelpedWithBinning[cacheIndex]) { - ref var b0 = ref cache0.BinBoundingBoxes[binIndex]; - ref var bi = ref cache.BinBoundingBoxes[binIndex]; - b0.Min = Vector4.Min(b0.Min, bi.Min); - b0.Max = Vector4.Min(b0.Max, bi.Max); - cache0.BinLeafCounts[binIndex] += cache.BinLeafCounts[binIndex]; + ref var cache = ref worker.BinSubtreesWorkers[cacheIndex]; + for (int binIndex = 0; binIndex < binCount; ++binIndex) + { + ref var b0 = ref cache0.BinBoundingBoxes[binIndex]; + ref var bi = ref cache.BinBoundingBoxes[binIndex]; + b0.Min = Vector4.Min(b0.Min, bi.Min); + b0.Max = Vector4.Min(b0.Max, bi.Max); + cache0.BinLeafCounts[binIndex] += cache.BinLeafCounts[binIndex]; + } } } + worker.DisposeBinSubtreesWorkers(workerPool); } unsafe struct NodePushTaskContext @@ -742,7 +772,7 @@ static unsafe void BinnedBuildNode( else { centroidBounds = MultithreadedCentroidPrepass( - (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As>(ref context->Threading)), subtreeRegionStartIndex, subtreeCount, workerIndex); + (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As>(ref context->Threading)), subtreeRegionStartIndex, subtreeCount, workerIndex, workerPool); } var centroidSpan = centroidBounds.Max - centroidBounds.Min; var axisIsDegenerate = Vector128.LessThanOrEqual(centroidSpan.AsVector128(), Vector128.Create(1e-12f)); @@ -819,7 +849,7 @@ static unsafe void BinnedBuildNode( { MultithreadedBinSubtrees( (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As>(ref context->Threading)), - centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, subtreeRegionStartIndex, subtreeCount, binCount, workerIndex); + centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, subtreeRegionStartIndex, subtreeCount, binCount, workerIndex, workerPool); } //Identify the split index by examining the SAH of very split option. @@ -1007,19 +1037,17 @@ static unsafe void BinnedBuilderInternal(Buffer encodedLeafIndices, Buffer< //While we could allocate on the stack with reasonable safety in the single threaded path, that's not very reasonable for the multithreaded path. //Each worker thread could be given a node job which executes asynchronously with respect to other node jobs. //Those node jobs could spawn multithreaded work that other workers assist with. - //So, each worker working on a node job needs to have its own set of worker caches for other workers to help it with the node job, giving an allocation count - //proportional to workerCount * workerCount * binCount. Not a big problem for small bin counts on a quad core system, but how about a 64 core system with 128 threads and 64 bins? - //128 * 128 * 64 > 1e6, and that's before taking into account the size of the bins being allocated! - pool.Take(BinnedBuildWorkerContext.ComputeRequiredSizeInBytes(workerCount, allocatedBinCount) * workerCount, out var workerAllocation); + //Each of those jobs needs its own context for those workers, and the number of jobs is not 1:1 with the workers. + //We'll handle such dispatch-required allocations from worker pools. Here, we just preallocate stuff for the first level across all workers. + pool.Take(maximumBinCount * workerCount, out var workerBinsForScan); BinnedBuildWorkerContext* workerContextsPointer = stackalloc BinnedBuildWorkerContext[workerCount]; var leafCounts = new UnitLeafCount(); var workerContexts = new Buffer>(workerContextsPointer, workerCount); - int allocatedByteCount = 0; for (int i = 0; i < workerCount; ++i) { - workerContexts[i] = new BinnedBuildWorkerContext(workerAllocation, ref allocatedByteCount, workerCount, allocatedBinCount, boundingBoxes.As(), leafCounts); + workerContexts[i] = new BinnedBuildWorkerContext(boundingBoxes.As(), workerBinsForScan.Slice(i * maximumBinCount, (i + 1) * maximumBinCount), leafCounts, workerCount); } TaskQueue taskQueue = default; @@ -1042,8 +1070,10 @@ static unsafe void BinnedBuilderInternal(Buffer encodedLeafIndices, Buffer< taskQueuePointer->TryEnqueueUnsafely(new Task(&BinnedBuilderWorkerEntry>, &context)); dispatcher.DispatchWorkers(&BinnedBuilderWorkerFunction>, taskQueuePointer); + if (createdTaskQueueLocally) taskQueue.Dispose(pool); + pool.Return(ref workerBinsForScan); } From c9a59fbbe5336861f3d22888767c0950bf48b5a8 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 29 Nov 2022 17:20:01 -0600 Subject: [PATCH 665/947] More musical chairs with threaddispatcher. TaskQueue now suppliles dispatcher to all tasks; gives a way for unmanaged delegates to grab bufferpool. --- BepuPhysics/CollisionDetection/BroadPhase.cs | 10 +-- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 53 ++++++------- BepuUtilities/IThreadDispatcher.cs | 25 +++--- BepuUtilities/TaskQueue.cs | 81 ++++++++++++-------- BepuUtilities/ThreadDispatcher.cs | 76 +++++++----------- DemoRenderer/ParallelLooper.cs | 6 +- Demos/DemoSet.cs | 2 +- Demos/SpecializedTests/TaskQueueTestDemo.cs | 26 +++---- 8 files changed, 141 insertions(+), 138 deletions(-) diff --git a/BepuPhysics/CollisionDetection/BroadPhase.cs b/BepuPhysics/CollisionDetection/BroadPhase.cs index 4cfc35ada..06c535030 100644 --- a/BepuPhysics/CollisionDetection/BroadPhase.cs +++ b/BepuPhysics/CollisionDetection/BroadPhase.cs @@ -40,7 +40,7 @@ public unsafe partial class BroadPhase : IDisposable //TODO: static trees do not need to do nearly as much work as the active; this will change in the future. Tree.RefitAndRefineMultithreadedContext staticRefineContext; - ThreadDispatcherWorker executeRefitAndMarkAction, executeRefineAction; + Action executeRefitAndMarkAction, executeRefineAction; public BroadPhase(BufferPool pool, int initialActiveLeafCapacity = 4096, int initialStaticLeafCapacity = 8192) { @@ -153,7 +153,7 @@ public void UpdateStaticBounds(int broadPhaseIndex, Vector3 min, Vector3 max) int frameIndex; int remainingJobCount; IThreadDispatcher threadDispatcher; - void ExecuteRefitAndMark(int workerIndex, void* context) + void ExecuteRefitAndMark(int workerIndex) { var threadPool = threadDispatcher.WorkerPools[workerIndex]; while (true) @@ -174,7 +174,7 @@ void ExecuteRefitAndMark(int workerIndex, void* context) } } - void ExecuteRefine(int workerIndex, void* context) + void ExecuteRefine(int workerIndex) { var threadPool = threadDispatcher.WorkerPools[workerIndex]; var maximumSubtrees = Math.Max(activeRefineContext.MaximumSubtrees, staticRefineContext.MaximumSubtrees); @@ -212,7 +212,7 @@ public void Update(IThreadDispatcher threadDispatcher = null) activeRefineContext.CreateRefitAndMarkJobs(ref ActiveTree, Pool, threadDispatcher); staticRefineContext.CreateRefitAndMarkJobs(ref StaticTree, Pool, threadDispatcher); remainingJobCount = activeRefineContext.RefitNodes.Count + staticRefineContext.RefitNodes.Count; - threadDispatcher.DispatchWorkers(executeRefitAndMarkAction, null, remainingJobCount); + threadDispatcher.DispatchWorkers(executeRefitAndMarkAction, remainingJobCount); activeRefineContext.CreateRefinementJobs(Pool, frameIndex, 1f); //TODO: for now, the inactive/static tree is simply updated like another active tree. This is enormously inefficient compared to the ideal- //by nature, static and inactive objects do not move every frame! @@ -221,7 +221,7 @@ public void Update(IThreadDispatcher threadDispatcher = null) //Since the jobs are large, reducing the refinement aggressiveness doesn't change much here. staticRefineContext.CreateRefinementJobs(Pool, frameIndex, 1f); remainingJobCount = activeRefineContext.RefinementTargets.Count + staticRefineContext.RefinementTargets.Count; - threadDispatcher.DispatchWorkers(executeRefineAction, null, remainingJobCount); + threadDispatcher.DispatchWorkers(executeRefineAction, remainingJobCount); activeRefineContext.CleanUpForRefitAndRefine(Pool); staticRefineContext.CleanUpForRefitAndRefine(Pool); this.threadDispatcher = null; diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 8084dbabf..9fdfdd9bd 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -582,7 +582,7 @@ static BoundingBox4 ComputeCentroidBounds(Buffer bounds) return centroidBounds; } - unsafe static void CentroidPrepassWorker(long taskId, void* untypedContext, int workerIndex) where TLeafCounts : unmanaged, ILeafCountBuffer + unsafe static void CentroidPrepassWorker(long taskId, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) where TLeafCounts : unmanaged, ILeafCountBuffer { ref var context = ref *(BinnedBuildWorkerContext*)untypedContext; context.GetSlotInterval(taskId, out var start, out var count); @@ -600,11 +600,12 @@ unsafe static void CentroidPrepassWorker(long taskId, void* untyped } } - unsafe static BoundingBox4 MultithreadedCentroidPrepass(MultithreadBinnedBuildContext* context, int subtreeStartIndex, int subtreeCount, int workerIndex, BufferPool workerPool) + unsafe static BoundingBox4 MultithreadedCentroidPrepass(MultithreadBinnedBuildContext* context, int subtreeStartIndex, int subtreeCount, int workerIndex, IThreadDispatcher dispatcher) where TLeafCounts : unmanaged, ILeafCountBuffer { ref var worker = ref context->Workers[workerIndex]; var taskCount = worker.SetupTaskCounts(subtreeStartIndex, subtreeCount, SubtreesPerThreadForCentroidPrepass, context->MaximumTaskCountPerSubmission); + var workerPool = dispatcher.WorkerPools[workerIndex]; worker.AllocatePrepassWorkers(workerPool, taskCount); //Don't bother initializing more slots than we have tasks. Note that this requires special handling on the task level; //if we have less tasks than workers, then the task needs to distinguish that fact. @@ -619,7 +620,7 @@ unsafe static BoundingBox4 MultithreadedCentroidPrepass(Multithread workerBounds.Max = new Vector4(float.MinValue); } } - context->Queue->For(&CentroidPrepassWorker, Unsafe.AsPointer(ref worker), 0, taskCount, workerIndex); + context->Queue->For(&CentroidPrepassWorker, Unsafe.AsPointer(ref worker), 0, taskCount, workerIndex, dispatcher); var centroidBounds = worker.PrepassWorkers[0]; for (int i = 1; i < activeWorkerCount; ++i) @@ -650,7 +651,7 @@ static void BinSubtrees(Vector4 centroidBoundsMin, binLeafCounts[binIndex] += leafCounts[i]; } } - unsafe static void BinSubtreesWorker(long taskId, void* untypedContext, int workerIndex) where TLeafCounts : unmanaged, ILeafCountBuffer + unsafe static void BinSubtreesWorker(long taskId, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) where TLeafCounts : unmanaged, ILeafCountBuffer { ref var context = ref *(BinnedBuildWorkerContext*)untypedContext; //Note that if we have more workers than tasks, we use the task id to index into the caches (and initialize the data here rather then before dispatching). @@ -674,10 +675,11 @@ unsafe static void BinSubtreesWorker(long taskId, void* untypedCont unsafe static void MultithreadedBinSubtrees(MultithreadBinnedBuildContext* context, Vector4 centroidBoundsMin, bool useX, bool useY, Vector128 permuteMask, int axisIndex, Vector4 offsetToBinIndex, Vector4 maximumBinIndex, - int subtreeStartIndex, int subtreeCount, int binCount, int workerIndex, BufferPool workerPool) where TLeafCounts : unmanaged, ILeafCountBuffer + int subtreeStartIndex, int subtreeCount, int binCount, int workerIndex, IThreadDispatcher dispatcher) where TLeafCounts : unmanaged, ILeafCountBuffer { ref var worker = ref context->Workers[workerIndex]; var taskCount = worker.SetupTaskCounts(subtreeStartIndex, subtreeCount, SubtreesPerThreadForBinning, context->MaximumTaskCountPerSubmission); + var workerPool = dispatcher.WorkerPools[workerIndex]; worker.AllocateBinSubtreesWorkers(workerPool, taskCount); //Don't bother initializing more slots than we have tasks. Note that this requires special handling on the task level; //if we have less tasks than workers, then the task needs to distinguish that fact. @@ -706,7 +708,7 @@ unsafe static void MultithreadedBinSubtrees(MultithreadBinnedBuildC worker.OffsetToBinIndex = offsetToBinIndex; worker.MaximumBinIndex = maximumBinIndex; - context->Queue->For(&BinSubtreesWorker, Unsafe.AsPointer(ref worker), 0, taskCount, workerIndex); + context->Queue->For(&BinSubtreesWorker, Unsafe.AsPointer(ref worker), 0, taskCount, workerIndex, dispatcher); //Unless the number of threads and bins is really huge, there's no value in attempting to multithread the final compression. //(Parallel reduction is an option, but even then... I suspect the single threaded version will be faster. And it's way simpler.) @@ -739,18 +741,18 @@ unsafe struct NodePushTaskContext public int ParentNodeIndex; //Subtree region start index and subtree count are both encoded into the task id. } - unsafe static void BinnedBuilderNodeWorker(long taskId, void* context, int workerIndex) + unsafe static void BinnedBuilderNodeWorker(long taskId, void* context, int workerIndex, IThreadDispatcher dispatcher) where TLeafCounts : unmanaged, ILeafCountBuffer where TLeaves : unmanaged where TThreading : unmanaged, IBinnedBuilderThreading { var subtreeRegionStartIndex = (int)taskId; var subtreeCount = (int)(taskId >> 32); var nodePushContext = (NodePushTaskContext*)context; //Note that child index is always 1 because we only ever push child B. - BinnedBuildNode(subtreeRegionStartIndex, nodePushContext->NodeIndex, subtreeCount, nodePushContext->ParentNodeIndex, 1, nodePushContext->Context, workerIndex); + BinnedBuildNode(subtreeRegionStartIndex, nodePushContext->NodeIndex, subtreeCount, nodePushContext->ParentNodeIndex, 1, nodePushContext->Context, workerIndex, dispatcher); } static unsafe void BinnedBuildNode( - int subtreeRegionStartIndex, int nodeIndex, int subtreeCount, int parentNodeIndex, int childIndexInParent, Context* context, int workerIndex) + int subtreeRegionStartIndex, int nodeIndex, int subtreeCount, int parentNodeIndex, int childIndexInParent, Context* context, int workerIndex, IThreadDispatcher dispatcher) where TLeafCounts : unmanaged, ILeafCountBuffer where TLeaves : unmanaged where TThreading : unmanaged, IBinnedBuilderThreading { var indices = context->Indices.Slice(subtreeRegionStartIndex, subtreeCount); @@ -772,7 +774,7 @@ static unsafe void BinnedBuildNode( else { centroidBounds = MultithreadedCentroidPrepass( - (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As>(ref context->Threading)), subtreeRegionStartIndex, subtreeCount, workerIndex, workerPool); + (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As>(ref context->Threading)), subtreeRegionStartIndex, subtreeCount, workerIndex, dispatcher); } var centroidSpan = centroidBounds.Max - centroidBounds.Min; var axisIsDegenerate = Vector128.LessThanOrEqual(centroidSpan.AsVector128(), Vector128.Create(1e-12f)); @@ -805,9 +807,9 @@ static unsafe void BinnedBuildNode( } BuildNode(boundsA, boundsB, degenerateLeafCountA, degenerateLeafCountB, nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, degenerateSubtreeCountA, degenerateSubtreeCountB, ref context->Leaves, out var aIndex, out var bIndex); if (degenerateSubtreeCountA > 1) - BinnedBuildNode(subtreeRegionStartIndex, aIndex, degenerateSubtreeCountA, nodeIndex, 0, context, workerIndex); + BinnedBuildNode(subtreeRegionStartIndex, aIndex, degenerateSubtreeCountA, nodeIndex, 0, context, workerIndex, dispatcher); if (degenerateSubtreeCountB > 1) - BinnedBuildNode(subtreeRegionStartIndex + degenerateSubtreeCountA, bIndex, degenerateSubtreeCountB, nodeIndex, 1, context, workerIndex); + BinnedBuildNode(subtreeRegionStartIndex + degenerateSubtreeCountA, bIndex, degenerateSubtreeCountB, nodeIndex, 1, context, workerIndex, dispatcher); return; } @@ -849,7 +851,7 @@ static unsafe void BinnedBuildNode( { MultithreadedBinSubtrees( (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As>(ref context->Threading)), - centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, subtreeRegionStartIndex, subtreeCount, binCount, workerIndex, workerPool); + centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, subtreeRegionStartIndex, subtreeCount, binCount, workerIndex, dispatcher); } //Identify the split index by examining the SAH of very split option. @@ -965,12 +967,12 @@ static unsafe void BinnedBuildNode( nodePushContext.ParentNodeIndex = nodeIndex; //Note that we use the task id to store subtree start and subtree count. Don't have to do that, but no reason not to use it. var task = new Task(&BinnedBuilderNodeWorker, &nodePushContext, (long)(subtreeRegionStartIndex + subtreeCountA) | ((long)subtreeCountB << 32)); - nodeBContinuation = threading.Queue->AllocateContinuationAndEnqueue(new Span(&task, 1), workerIndex); + nodeBContinuation = threading.Queue->AllocateContinuationAndEnqueue(new Span(&task, 1), workerIndex, dispatcher); } if (subtreeCountA > 1) - BinnedBuildNode(subtreeRegionStartIndex, nodeChildIndexA, subtreeCountA, nodeIndex, 0, context, workerIndex); + BinnedBuildNode(subtreeRegionStartIndex, nodeChildIndexA, subtreeCountA, nodeIndex, 0, context, workerIndex, dispatcher); if (!shouldPushBOntoMultithreadedQueue && subtreeCountB > 1) - BinnedBuildNode(subtreeRegionStartIndex + subtreeCountA, nodeChildIndexB, subtreeCountB, nodeIndex, 1, context, workerIndex); + BinnedBuildNode(subtreeRegionStartIndex + subtreeCountA, nodeChildIndexB, subtreeCountB, nodeIndex, 1, context, workerIndex, dispatcher); if (shouldPushBOntoMultithreadedQueue) { //We want to keep the stack at this level alive until the memory we allocated for the node push completes. @@ -978,7 +980,7 @@ static unsafe void BinnedBuildNode( //In addition to letting us use the local stack to store some arguments for the other thread, this wait means that all children have completed when this function returns. //That makes knowing when to stop the queue easier. Debug.Assert(nodeBContinuation.Initialized); - Unsafe.As>(ref context->Threading).Queue->WaitForCompletion(nodeBContinuation, workerIndex); + Unsafe.As>(ref context->Threading).Queue->WaitForCompletion(nodeBContinuation, workerIndex, dispatcher); } } @@ -1029,7 +1031,7 @@ static unsafe void BinnedBuilderInternal(Buffer encodedLeafIndices, Buffer< var context = new Context, SingleThreaded>( minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold, encodedLeafIndices, new UnitLeafCount(), leaves, boundingBoxes.As(), nodes, metanodes, threading); - BinnedBuildNode(0, 0, subtreeCount, -1, -1, &context, 0); + BinnedBuildNode(0, 0, subtreeCount, -1, -1, &context, 0, null); } else if (dispatcher != null) { @@ -1068,7 +1070,7 @@ static unsafe void BinnedBuilderInternal(Buffer encodedLeafIndices, Buffer< leafCounts, leaves, boundingBoxes.As(), nodes, metanodes, threading); taskQueuePointer->TryEnqueueUnsafely(new Task(&BinnedBuilderWorkerEntry>, &context)); - dispatcher.DispatchWorkers(&BinnedBuilderWorkerFunction>, taskQueuePointer); + dispatcher.DispatchWorkers(&BinnedBuilderWorkerFunction>, unmanagedContext: taskQueuePointer); if (createdTaskQueueLocally) @@ -1085,25 +1087,24 @@ public static unsafe void BinnedBuilder(Buffer encodedLeafIndices, Buffer(long taskId, void* untypedContext, int workerIndex) + unsafe static void BinnedBuilderWorkerEntry(long taskId, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) where TLeafCounts : unmanaged, ILeafCountBuffer where TLeaves : unmanaged { var context = (Context>*)untypedContext; - BinnedBuildNode(0, 0, context->Indices.Length, -1, -1, context, workerIndex); + BinnedBuildNode(0, 0, context->Indices.Length, -1, -1, context, workerIndex, dispatcher); //Once the entry point returns, all workers should stop because it won't return unless both nodes are done. - context->Threading.Queue->EnqueueStop(workerIndex); - + context->Threading.Queue->EnqueueStop(workerIndex, dispatcher); } - unsafe static void BinnedBuilderWorkerFunction(int workerIndex, void* untypedContext) + unsafe static void BinnedBuilderWorkerFunction(int workerIndex, IThreadDispatcher dispatcher) where TLeafCounts : unmanaged, ILeafCountBuffer where TLeaves : unmanaged { - var taskQueue = (TaskQueue*)untypedContext; + var taskQueue = (TaskQueue*)dispatcher.UnmanagedContext; DequeueTaskResult dequeueTaskResult; var waiter = new SpinWait(); - while ((dequeueTaskResult = taskQueue->TryDequeueAndRun(workerIndex)) != DequeueTaskResult.Stop) + while ((dequeueTaskResult = taskQueue->TryDequeueAndRun(workerIndex, dispatcher)) != DequeueTaskResult.Stop) { waiter.SpinOnce(-1); } diff --git a/BepuUtilities/IThreadDispatcher.cs b/BepuUtilities/IThreadDispatcher.cs index b632e9905..5746b85c4 100644 --- a/BepuUtilities/IThreadDispatcher.cs +++ b/BepuUtilities/IThreadDispatcher.cs @@ -34,27 +34,34 @@ public unsafe interface IThreadDispatcher int ThreadCount { get; } /// - /// Dispatches all the available workers. + /// Gets the unmanaged context associated with the current dispatch, if any. /// - /// Function pointer to be invoked on every worker. Matches the signature of the . - /// Pointer to the context to passed to workers, if any. - /// Maximum number of workers to dispatch. - void DispatchWorkers(delegate* workerBody, void* context, int maximumWorkerCount = int.MaxValue); + /// This is intended to help pass information to workers, especially those defined with function pointers that can't just include extra state in closures. + public void* UnmanagedContext { get; } + + /// + /// Gets the managed context associated with the current dispatch, if any. + /// + /// This is intended to help pass information to workers, especially those defined with function pointers that can't just include extra state in closures. + public object ManagedContext { get; } /// /// Dispatches all the available workers. /// - /// Delegate to be invoked on every worker. - /// Pointer to the context to passed to workers, if any. + /// Function pointer to be invoked on every worker. Matches the signature of the . /// Maximum number of workers to dispatch. - void DispatchWorkers(ThreadDispatcherWorker workerBody, void* context, int maximumWorkerCount = int.MaxValue); + /// Unmanaged context of the dispatch. + /// Managed context of the dispatch. + void DispatchWorkers(delegate* workerBody, int maximumWorkerCount = int.MaxValue, void* unmanagedContext = null, object managedContext = null); /// /// Dispatches all the available workers with a null context. /// /// Delegate to be invoked on every worker. /// Maximum number of workers to dispatch. - void DispatchWorkers(Action workerBody, int maximumWorkerCount = int.MaxValue); + /// Unmanaged context of the dispatch. + /// Managed context of the dispatch. + void DispatchWorkers(Action workerBody, int maximumWorkerCount = int.MaxValue, void* unmanagedContext = null, object managedContext = null); /// /// Gets the set of memory pools associated with thread workers. diff --git a/BepuUtilities/TaskQueue.cs b/BepuUtilities/TaskQueue.cs index 1e429c0de..2e7a98f8a 100644 --- a/BepuUtilities/TaskQueue.cs +++ b/BepuUtilities/TaskQueue.cs @@ -25,7 +25,7 @@ public unsafe struct Task /// /// Function to be executed by the task. Takes as arguments the , pointer, and executing worker index. /// - public delegate* Function; + public delegate* Function; /// /// Context to be passed into the . /// @@ -42,11 +42,11 @@ public unsafe struct Task /// /// Creates a new task. /// - /// Function to be executed by the task. Takes as arguments the , pointer, and executing worker index. + /// Function to be executed by the task. Takes as arguments the , pointer, executing worker index, and executing . /// Context pointer to pass to the . /// Id of this task to be passed into the . /// Continuation to notify after the completion of this task, if any. - public Task(delegate* function, void* context = null, long taskId = 0, ContinuationHandle continuation = default) + public Task(delegate* function, void* context = null, long taskId = 0, ContinuationHandle continuation = default) { Function = function; Context = context; @@ -54,17 +54,22 @@ public Task(delegate* function, void* context = null, lo Id = taskId; } - public static implicit operator Task(delegate* function) => new Task(function); + /// + /// Creates a task from a function. + /// + /// Function to turn into a task. + public static implicit operator Task(delegate* function) => new(function); /// /// Runs the task and, if necessary, notifies the associated continuation of its completion. /// - /// Worker index to pass to the function. - public void Run(int workerIndex) + /// Worker index to pass to the function. + /// Dispatcher running this task. + public void Run(int workerIndex, IThreadDispatcher dispatcher) { - Function(Id, Context, workerIndex); + Function(Id, Context, workerIndex, dispatcher); if (Continuation.Initialized) - Continuation.NotifyTaskCompleted(workerIndex); + Continuation.NotifyTaskCompleted(workerIndex, dispatcher); } } @@ -183,7 +188,8 @@ public bool Exists /// Notifies the continuation that one task was completed. /// /// Worker index to pass to the continuation's delegate, if any. - public void NotifyTaskCompleted(int workerIndex) + /// Dispatcher to pass to the continuation's delegate, if any. + public void NotifyTaskCompleted(int workerIndex, IThreadDispatcher dispatcher) { var continuation = Continuation; var counter = Interlocked.Decrement(ref continuation->RemainingTaskCounter); @@ -193,7 +199,7 @@ public void NotifyTaskCompleted(int workerIndex) //This entire job has completed. if (continuation->OnCompleted.Function != null) { - continuation->OnCompleted.Function(continuation->OnCompleted.Id, continuation->OnCompleted.Context, workerIndex); + continuation->OnCompleted.Function(continuation->OnCompleted.Id, continuation->OnCompleted.Context, workerIndex, dispatcher); } //Free this continuation slot. var waiter = new SpinWait(); @@ -449,10 +455,11 @@ public AllocateTaskContinuationResult TryAllocateContinuation(int taskCount, out /// /// Number of tasks associated with the continuation. /// Worker index to pass to any inline executed tasks if the continuations buffer is full. + /// Dispatcher to pass to inline executed tasks if the continuations buffer is full. /// Task to execute upon completing all associated tasks, if any. Any task with a null will not be executed. /// Handle of the allocated continuation. /// Note that this will keep trying until allocation succeeds. If something is blocking allocation, such as insufficient room in the continuations buffer and there are no workers consuming tasks, this will block forever. - public ContinuationHandle AllocateContinuation(int taskCount, int workerIndex, Task onCompleted = default) + public ContinuationHandle AllocateContinuation(int taskCount, int workerIndex, IThreadDispatcher dispatcher, Task onCompleted = default) { var waiter = new SpinWait(); ContinuationHandle handle; @@ -461,7 +468,7 @@ public ContinuationHandle AllocateContinuation(int taskCount, int workerIndex, T { if (result == AllocateTaskContinuationResult.Full) { - var dequeueResult = TryDequeueAndRun(workerIndex); + var dequeueResult = TryDequeueAndRun(workerIndex, dispatcher); Debug.Assert(dequeueResult != DequeueTaskResult.Stop, "We're trying to allocate a continuation, we shouldn't have run into a stop command!"); } else @@ -515,13 +522,14 @@ public DequeueTaskResult TryDequeue(out Task task) /// Attempts to dequeue a task and run it. /// /// Index of the worker to pass into the task function. + /// Thread dispatcher running this task queue. /// Result status of the dequeue attempt. - public DequeueTaskResult TryDequeueAndRun(int workerIndex) + public DequeueTaskResult TryDequeueAndRun(int workerIndex, IThreadDispatcher dispatcher) { var result = TryDequeue(out var task); if (result == DequeueTaskResult.Success) { - task.Run(workerIndex); + task.Run(workerIndex, dispatcher); } return result; } @@ -628,9 +636,10 @@ public EnqueueTaskResult TryEnqueue(Span tasks) /// /// Tasks composing the job. /// Worker index to pass to inline-executed tasks if the task buffer is full. + /// Dispatcher to pass to inline-executed tasks if the task buffer is full. /// Note that this will keep trying until task submission succeeds. /// If the task queue is full, this will opt to run some tasks inline while waiting for room. - public void Enqueue(Span tasks, int workerIndex) + public void Enqueue(Span tasks, int workerIndex, IThreadDispatcher dispatcher) { var waiter = new SpinWait(); EnqueueTaskResult result; @@ -641,7 +650,7 @@ public void Enqueue(Span tasks, int workerIndex) //Couldn't enqueue the tasks because the task buffer is full. //Clearly there's plenty of work available to execute, so go ahead and try to run one task inline. var task = tasks[0]; - task.Run(workerIndex); + task.Run(workerIndex, dispatcher); if (tasks.Length == 1) break; tasks = tasks[1..]; @@ -658,20 +667,21 @@ public void Enqueue(Span tasks, int workerIndex) /// /// Tasks composing the job. A continuation will be assigned internally; no continuation should be present on any of the provided tasks. /// Worker index to pass to inline-executed tasks if the task buffer is full. + /// Dispatcher to pass to inline-executed tasks if the task buffer is full. /// Task to run upon completion of all the submitted tasks, if any. /// Handle of the continuation created for these tasks. /// Note that this will keep trying until task submission succeeds. /// If the task queue is full, this will opt to run some tasks inline while waiting for room. - public ContinuationHandle AllocateContinuationAndEnqueue(Span tasks, int workerIndex, Task onComplete = default) + public ContinuationHandle AllocateContinuationAndEnqueue(Span tasks, int workerIndex, IThreadDispatcher dispatcher, Task onComplete = default) { - var continuationHandle = AllocateContinuation(tasks.Length, workerIndex); + var continuationHandle = AllocateContinuation(tasks.Length, workerIndex, dispatcher); for (int i = 0; i < tasks.Length; ++i) { ref var task = ref tasks[i]; Debug.Assert(!task.Continuation.Initialized, "This function creates a continuation for the tasks"); task.Continuation = continuationHandle; } - Enqueue(tasks, workerIndex); + Enqueue(tasks, workerIndex, dispatcher); return continuationHandle; } @@ -681,7 +691,8 @@ public ContinuationHandle AllocateContinuationAndEnqueue(Span tasks, int w /// Instead of spinning the entire time, this may dequeue and execute pending tasks to fill the gap. /// Continuation to wait on. /// Currently executing worker index to pass to any tasks used to fill the wait period. - public void WaitForCompletion(ContinuationHandle continuation, int workerIndex) + /// Currently executing dispatcher to pass to any tasks used to fill the wait period. + public void WaitForCompletion(ContinuationHandle continuation, int workerIndex, IThreadDispatcher dispatcher) { var waiter = new SpinWait(); Debug.Assert(continuation.Initialized, "This codepath should only run if the continuation was allocated earlier."); @@ -697,7 +708,7 @@ public void WaitForCompletion(ContinuationHandle continuation, int workerIndex) } if (dequeueResult == DequeueTaskResult.Success) { - fillerTask.Run(workerIndex); + fillerTask.Run(workerIndex, dispatcher); waiter.Reset(); } else @@ -712,9 +723,10 @@ public void WaitForCompletion(ContinuationHandle continuation, int workerIndex) /// /// Tasks composing the job. A continuation will be assigned internally; no continuation should be present on any of the provided tasks. /// Worker index to pass to inline-executed tasks if the task buffer is full. + /// Dispatcher to pass to inline-executed tasks if the task buffer is full. /// Note that this will keep working until all tasks are run. /// If the task queue is full, this will opt to run some tasks inline while waiting for room. - public void RunTasks(Span tasks, int workerIndex) + public void RunTasks(Span tasks, int workerIndex, IThreadDispatcher dispatcher) { if (tasks.Length == 0) return; @@ -724,7 +736,7 @@ public void RunTasks(Span tasks, int workerIndex) //Note that we only submit tasks to the queue for tasks beyond the first. The current thread is responsible for at least task 0. var taskCount = tasks.Length - 1; Span tasksToEnqueue = stackalloc Task[taskCount]; - continuationHandle = AllocateContinuation(taskCount, workerIndex); + continuationHandle = AllocateContinuation(taskCount, workerIndex, dispatcher); for (int i = 0; i < tasksToEnqueue.Length; ++i) { var task = tasks[i + 1]; @@ -733,18 +745,18 @@ public void RunTasks(Span tasks, int workerIndex) task.Continuation = continuationHandle; tasksToEnqueue[i] = task; } - Enqueue(tasksToEnqueue, workerIndex); + Enqueue(tasksToEnqueue, workerIndex, dispatcher); } //Tasks [1, count) are submitted to the queue and may now be executing on other workers. //The thread calling the for loop should not relinquish its timeslice. It should immediately begin working on task 0. var task0 = tasks[0]; Debug.Assert(!task0.Continuation.Initialized, $"None of the source tasks should have continuations when provided to {nameof(RunTasks)}."); - task0.Function(task0.Id, task0.Context, workerIndex); + task0.Function(task0.Id, task0.Context, workerIndex, dispatcher); if (tasks.Length > 1) { //Task 0 is done; this thread should seek out other work until the job is complete. - WaitForCompletion(continuationHandle, workerIndex); + WaitForCompletion(continuationHandle, workerIndex, dispatcher); } } @@ -763,9 +775,10 @@ public EnqueueTaskResult TryEnqueueStop() /// Enqueues the stop command. /// /// Worker index to pass to any inline executed tasks if the task buffer is full. + /// Dispatcher to pass to any inline executed tasks if the task buffer is full. /// Note that this will keep trying until task submission succeeds. /// If the task buffer is full, this may attempt to dequeue tasks to run inline. - public void EnqueueStop(int workerIndex) + public void EnqueueStop(int workerIndex, IThreadDispatcher dispatcher) { Span stopJob = stackalloc Task[1]; stopJob[0] = new Task { Function = null }; @@ -775,7 +788,7 @@ public void EnqueueStop(int workerIndex) { if (result == EnqueueTaskResult.Full) { - var dequeueResult = TryDequeueAndRun(workerIndex); + var dequeueResult = TryDequeueAndRun(workerIndex, dispatcher); Debug.Assert(dequeueResult != DequeueTaskResult.Stop, "We're trying to enqueue a stop, we shouldn't have found one already present!"); } else @@ -807,7 +820,7 @@ public EnqueueTaskResult TryEnqueueStopUnsafely() /// Continuation associated with the loop tasks, if any. /// Status result of the enqueue operation. /// This must not be used while other threads could be performing task enqueues or task dequeues. - public EnqueueTaskResult TryEnqueueForUnsafely(delegate* function, void* context, int inclusiveStartIndex, int iterationCount, ContinuationHandle continuation = default) + public EnqueueTaskResult TryEnqueueForUnsafely(delegate* function, void* context, int inclusiveStartIndex, int iterationCount, ContinuationHandle continuation = default) { Span tasks = stackalloc Task[iterationCount]; for (int i = 0; i < tasks.Length; ++i) @@ -825,17 +838,18 @@ public EnqueueTaskResult TryEnqueueForUnsafely(delegate* /// Inclusive start index of the loop range. /// Number of iterations to perform. /// Worker index to pass to any inline-executed task if the task queue is full. + /// Dispatcher to pass to any inline-executed task if the task queue is full. /// Continuation associated with the loop tasks, if any. /// This function will not usually attempt to run any iterations of the loop itself. It tries to push the loop tasks onto the queue. /// If the task queue is full, this will opt to run the tasks inline while waiting for room. - public void EnqueueFor(delegate* function, void* context, int inclusiveStartIndex, int iterationCount, int workerIndex, ContinuationHandle continuation = default) + public void EnqueueFor(delegate* function, void* context, int inclusiveStartIndex, int iterationCount, int workerIndex, IThreadDispatcher dispatcher, ContinuationHandle continuation = default) { Span tasks = stackalloc Task[iterationCount]; for (int i = 0; i < tasks.Length; ++i) { tasks[i] = new Task { Function = function, Context = context, Id = i + inclusiveStartIndex, Continuation = continuation }; } - Enqueue(tasks, workerIndex); + Enqueue(tasks, workerIndex, dispatcher); } /// @@ -846,7 +860,8 @@ public void EnqueueFor(delegate* function, void* context /// Inclusive start index of the loop range. /// Number of iterations to perform. /// Index of the currently executing worker. - public void For(delegate* function, void* context, int inclusiveStartIndex, int iterationCount, int workerIndex) + /// Currently executing thread dispatcher. + public void For(delegate* function, void* context, int inclusiveStartIndex, int iterationCount, int workerIndex, IThreadDispatcher dispatcher) { if (iterationCount <= 0) return; @@ -855,6 +870,6 @@ public void For(delegate* function, void* context, int i { tasks[i] = new Task(function, context, inclusiveStartIndex + i); } - RunTasks(tasks, workerIndex); + RunTasks(tasks, workerIndex, dispatcher); } } diff --git a/BepuUtilities/ThreadDispatcher.cs b/BepuUtilities/ThreadDispatcher.cs index 8f1c6ccdc..5c33d8487 100644 --- a/BepuUtilities/ThreadDispatcher.cs +++ b/BepuUtilities/ThreadDispatcher.cs @@ -27,6 +27,10 @@ struct Worker /// public WorkerBufferPools WorkerPools { get; private set; } + /// + public void* UnmanagedContext => unmanagedContext; + /// + public object ManagedContext => managedWorker; /// /// Creates a new thread dispatcher with the given number of threads. @@ -52,8 +56,7 @@ void DispatchThread(int workerIndex) switch (workerType) { case WorkerType.Managed: managedWorker(workerIndex); break; - case WorkerType.ManagedWithUnmanagedContext: managedWithContextWorker(workerIndex, context); break; - case WorkerType.Unmanaged: unmanagedWorker(workerIndex, context); break; + case WorkerType.Unmanaged: unmanagedWorker(workerIndex, this); break; } if (Interlocked.Decrement(ref remainingWorkerCounter) == -1) @@ -64,26 +67,16 @@ void DispatchThread(int workerIndex) enum WorkerType { - /// - /// Action{int} - /// + //We've gone back and forth many times on how many types exist. 2 at the moment, but will it be 4 again next week? Who knows! Managed, - /// - /// - /// - ManagedWithUnmanagedContext, - /// - /// Includes unmanaged context by default because these jobs can only be static functions and *some* context is probably required. - /// delegate*{int, void*, void} - /// Unmanaged, } volatile WorkerType workerType; volatile Action managedWorker; - volatile ThreadDispatcherWorker managedWithContextWorker; - volatile delegate* unmanagedWorker; - volatile void* context; + volatile delegate* unmanagedWorker; + volatile void* unmanagedContext; + volatile object managedContext; int remainingWorkerCounter; @@ -113,56 +106,40 @@ void SignalThreads(int maximumWorkerCount) } /// - public void DispatchWorkers(delegate* workerBody, void* context, int maximumWorkerCount = int.MaxValue) + public void DispatchWorkers(delegate* workerBody, int maximumWorkerCount = int.MaxValue, void* unmanagedContext = null, object managedContext = null) { + Debug.Assert(this.managedWorker == null && this.unmanagedWorker == null && this.managedContext == null && this.unmanagedContext == null); + this.unmanagedContext = unmanagedContext; + this.managedContext = managedContext; if (maximumWorkerCount > 1) { - Debug.Assert(this.managedWorker == null && this.managedWithContextWorker == null && this.unmanagedWorker == null && this.context == null); workerType = WorkerType.Unmanaged; - unmanagedWorker = workerBody; - this.context = context; + this.unmanagedWorker = workerBody; SignalThreads(maximumWorkerCount); //Calling thread does work. No reason to spin up another worker and block this one! DispatchThread(0); finished.WaitOne(); - unmanagedWorker = null; - this.context = null; + this.unmanagedWorker = null; } else if (maximumWorkerCount == 1) { - workerBody(0, context); + workerBody(0, this); } + this.unmanagedContext = null; + this.managedContext = null; } + //While we *could* pass in the IThreadDispatcher for the managed side of things, it is typically best to just expect closures. Simplifies some stuff. + //(The fact that we supply context at all is a bit of a shrug.) /// - public void DispatchWorkers(ThreadDispatcherWorker workerBody, void* context, int maximumWorkerCount = int.MaxValue) + public void DispatchWorkers(Action workerBody, int maximumWorkerCount = int.MaxValue, void* unmanagedContext = null, object managedContext = null) { + Debug.Assert(this.managedWorker == null && this.unmanagedWorker == null && this.managedContext == null && this.unmanagedContext == null); + this.unmanagedContext = unmanagedContext; + this.managedContext = managedContext; if (maximumWorkerCount > 1) { - Debug.Assert(this.managedWorker == null && this.managedWithContextWorker == null && this.unmanagedWorker == null && this.context == null); - workerType = WorkerType.ManagedWithUnmanagedContext; - this.managedWithContextWorker = workerBody; - this.context = context; - SignalThreads(maximumWorkerCount); - //Calling thread does work. No reason to spin up another worker and block this one! - DispatchThread(0); - finished.WaitOne(); - this.managedWithContextWorker = null; - this.context = null; - } - else if (maximumWorkerCount == 1) - { - workerBody(0, context); - } - } - - /// - public void DispatchWorkers(Action workerBody, int maximumWorkerCount = int.MaxValue) - { - if (maximumWorkerCount > 1) - { - Debug.Assert(this.managedWorker == null && this.managedWithContextWorker == null && this.unmanagedWorker == null && this.context == null); - workerType = WorkerType.ManagedWithUnmanagedContext; + workerType = WorkerType.Managed; this.managedWorker = workerBody; SignalThreads(maximumWorkerCount); //Calling thread does work. No reason to spin up another worker and block this one! @@ -174,8 +151,11 @@ public void DispatchWorkers(Action workerBody, int maximumWorkerCount = int { workerBody(0); } + this.unmanagedContext = null; + this.managedContext = null; } + volatile bool disposed; /// diff --git a/DemoRenderer/ParallelLooper.cs b/DemoRenderer/ParallelLooper.cs index b1a279b32..6d6f787d4 100644 --- a/DemoRenderer/ParallelLooper.cs +++ b/DemoRenderer/ParallelLooper.cs @@ -27,7 +27,7 @@ namespace DemoRenderer /// having zero allocations under normal execution makes it easier to notice when the physics simulation itself is allocating inappropriately. public unsafe class ParallelLooper { - ThreadDispatcherWorker dispatcherWorker; + Action dispatcherWorker; /// /// Gets or sets the dispatcher used by the looper. @@ -39,7 +39,7 @@ public ParallelLooper() dispatcherWorker = Worker; } - void Worker(int workerIndex, void* context) + void Worker(int workerIndex) { while (true) { @@ -79,7 +79,7 @@ public void For(int start, int exclusiveEnd, LooperAction workAction, LooperWork this.end = exclusiveEnd; this.iteration = workAction; this.workerDone = workerDone; - Dispatcher.DispatchWorkers(dispatcherWorker, null, exclusiveEnd - start); + Dispatcher.DispatchWorkers(dispatcherWorker, exclusiveEnd - start); this.iteration = null; this.workerDone = workerDone; } diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index dfffb7775..a80e8370a 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -48,7 +48,7 @@ public DemoSet() { //AddOption(); //AddOption(); - //AddOption(); + AddOption(); AddOption(); AddOption(); AddOption(); diff --git a/Demos/SpecializedTests/TaskQueueTestDemo.cs b/Demos/SpecializedTests/TaskQueueTestDemo.cs index b5bb3e469..4b3c269b4 100644 --- a/Demos/SpecializedTests/TaskQueueTestDemo.cs +++ b/Demos/SpecializedTests/TaskQueueTestDemo.cs @@ -21,23 +21,23 @@ static int DoSomeWork(int iterations, int sum) } return sum; } - static void DynamicallyEnqueuedTest(long taskId, void* context, int workerIndex) + static void DynamicallyEnqueuedTest(long taskId, void* context, int workerIndex, IThreadDispatcher dispatcher) { var sum = DoSomeWork(10000, 0); Interlocked.Add(ref ((Context*)context)->Sum, sum); } - static void Test(long taskId, void* context, int workerIndex) + static void Test(long taskId, void* context, int workerIndex, IThreadDispatcher dispatcher) { var sum = DoSomeWork(100000, 0); var typedContext = (Context*)context; if ((taskId & 7) == 0) { const int subtaskCount = 8; - typedContext->Queue->For(&DynamicallyEnqueuedTest, context, 0, subtaskCount, workerIndex); + typedContext->Queue->For(&DynamicallyEnqueuedTest, context, 0, subtaskCount, workerIndex, dispatcher); } Interlocked.Add(ref typedContext->Sum, sum); } - static void STTest(long taskId, void* context, int workerIndex) + static void STTest(long taskId, void* context, int workerIndex, IThreadDispatcher dispatcher) { var sum = DoSomeWork(100000, 0); var typedContext = (Context*)context; @@ -46,16 +46,16 @@ static void STTest(long taskId, void* context, int workerIndex) const int subtaskCount = 8; for (int i = 0; i < subtaskCount; ++i) { - DynamicallyEnqueuedTest(i, context, workerIndex); + DynamicallyEnqueuedTest(i, context, workerIndex, dispatcher); } } Interlocked.Add(ref typedContext->Sum, sum); } - static void DispatcherBody(int workerIndex, void* context) + static void DispatcherBody(int workerIndex, IThreadDispatcher dispatcher) { - var taskQueue = (TaskQueue*)context; - while (taskQueue->TryDequeueAndRun(workerIndex) != DequeueTaskResult.Stop) ; + var taskQueue = (TaskQueue*)dispatcher.UnmanagedContext; + while (taskQueue->TryDequeueAndRun(workerIndex, dispatcher) != DequeueTaskResult.Stop) ; } struct Context @@ -64,10 +64,10 @@ struct Context public int Sum; } - static void IssueStop(long id, void* context, int workerIndex) + static void IssueStop(long id, void* context, int workerIndex, IThreadDispatcher dispatcher) { var typedContext = (Context*)context; - typedContext->Queue->EnqueueStop(workerIndex); + typedContext->Queue->EnqueueStop(workerIndex, dispatcher); } public override void Initialize(ContentArchive content, Camera camera) @@ -88,12 +88,12 @@ public override void Initialize(ContentArchive content, Camera camera) Test(() => { var context = new Context { Queue = taskQueuePointer }; - var continuation = taskQueuePointer->AllocateContinuation(iterationCount * tasksPerIteration, 0, new Task(&IssueStop, &context)); + var continuation = taskQueuePointer->AllocateContinuation(iterationCount * tasksPerIteration, 0, ThreadDispatcher, new Task(&IssueStop, &context)); for (int i = 0; i < iterationCount; ++i) taskQueuePointer->TryEnqueueForUnsafely(&Test, &context, i * tasksPerIteration, tasksPerIteration, continuation); //taskQueuePointer->TryEnqueueStopUnsafely(); //taskQueuePointer->EnqueueTasks() - ThreadDispatcher.DispatchWorkers(&DispatcherBody, taskQueuePointer); + ThreadDispatcher.DispatchWorkers(&DispatcherBody, unmanagedContext: taskQueuePointer); return context.Sum; }, "MT", () => taskQueuePointer->Reset()); @@ -106,7 +106,7 @@ public override void Initialize(ContentArchive content, Camera camera) { for (int j = 0; j < tasksPerIteration; ++j) { - STTest(i * tasksPerIteration + j, &testContext, 0); + STTest(i * tasksPerIteration + j, &testContext, 0, ThreadDispatcher); } } return testContext.Sum; From 72a5e9b98aac838b6e54bc2e9be99fe8b8551718 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 29 Nov 2022 18:37:14 -0600 Subject: [PATCH 666/947] Superminor pad. --- BepuUtilities/ThreadDispatcher.cs | 19 ++++++++++++++++--- 1 file changed, 16 insertions(+), 3 deletions(-) diff --git a/BepuUtilities/ThreadDispatcher.cs b/BepuUtilities/ThreadDispatcher.cs index 5c33d8487..133e75acc 100644 --- a/BepuUtilities/ThreadDispatcher.cs +++ b/BepuUtilities/ThreadDispatcher.cs @@ -1,6 +1,7 @@ using System; using System.Diagnostics; using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; using System.Threading; using BepuUtilities.Memory; @@ -59,7 +60,7 @@ void DispatchThread(int workerIndex) case WorkerType.Unmanaged: unmanagedWorker(workerIndex, this); break; } - if (Interlocked.Decrement(ref remainingWorkerCounter) == -1) + if (Interlocked.Decrement(ref remainingWorkerCounter.Value) == -1) { finished.Set(); } @@ -78,7 +79,19 @@ enum WorkerType volatile void* unmanagedContext; volatile object managedContext; - int remainingWorkerCounter; + //We'd like to avoid the thread readonly values above being adjacent to the thread readwrite counter. + //If they were in the same cache line, it would cause a bit of extra contention for no reason. + //(It's not *that* big of a deal since the counter is only touched once per worker, but padding this also costs nothing.) + //In a class, we don't control layout, so wrap the counter in a beefy struct. + //128B padding is used for the sake of architectures that might try prefetching cache line pairs and running into sync problems. + [StructLayout(LayoutKind.Explicit, Size = 256)] + struct Counter + { + [FieldOffset(128)] + public int Value; + } + + Counter remainingWorkerCounter; void WorkerLoop(object untypedSignal) { @@ -98,7 +111,7 @@ void SignalThreads(int maximumWorkerCount) //So if we want 4 total executing threads, we should signal 3 workers. int maximumWorkersToSignal = maximumWorkerCount - 1; var workersToSignal = maximumWorkersToSignal < workers.Length ? maximumWorkersToSignal : workers.Length; - remainingWorkerCounter = workersToSignal; + remainingWorkerCounter.Value = workersToSignal; for (int i = 0; i < workersToSignal; ++i) { workers[i].Signal.Set(); From 99b4c89b13a4c8b69673f3167a74bb493b0abb9a Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 3 Dec 2022 16:13:24 -0600 Subject: [PATCH 667/947] Padded mutable task queue counters. --- BepuUtilities/TaskQueue.cs | 67 ++++++++++++--------- Demos/SpecializedTests/TaskQueueTestDemo.cs | 13 ++++ 2 files changed, 52 insertions(+), 28 deletions(-) diff --git a/BepuUtilities/TaskQueue.cs b/BepuUtilities/TaskQueue.cs index 2e7a98f8a..5a75574a6 100644 --- a/BepuUtilities/TaskQueue.cs +++ b/BepuUtilities/TaskQueue.cs @@ -315,12 +315,23 @@ public unsafe struct TaskQueue int taskMask, taskShift; - //TODO: Careful about false sharing. - long taskIndex; - long allocatedTaskIndex; - long writtenTaskIndex; + [StructLayout(LayoutKind.Explicit, Size = 540)] + struct Padded + { + //WrittenTaskIndex and TaskIndex are referenced by dequeues, so keep them together. + //They're *also* referenced by enqueues, but enqueues are rarer so there's a good chance the local thread can avoid a direct contest. + [FieldOffset(128)] + public long WrittenTaskIndex; + [FieldOffset(136)] + public long TaskIndex; + //AllocatedTaskIndex and TaskLocker are only touched by enqueues, so we shove them off into their own padded region. + [FieldOffset(400)] + public long AllocatedTaskIndex; + [FieldOffset(408)] + public volatile int TaskLocker; + } - volatile int taskLocker; + Padded paddedCounters; /// /// Holds the task queue's continuations data in unmanaged memory just in case the queue itself is in unpinned memory. @@ -346,7 +357,7 @@ public TaskQueue(BufferPool pool, int maximumTaskCapacity = 1024, int maximumCon pool.Take(maximumTaskCapacity, out tasks); taskMask = tasks.length - 1; taskShift = BitOperations.TrailingZeroCount(tasks.length); - taskLocker = 0; + paddedCounters.TaskLocker = 0; Reset(); } @@ -355,10 +366,10 @@ public TaskQueue(BufferPool pool, int maximumTaskCapacity = 1024, int maximumCon /// public void Reset() { - taskIndex = 0; - allocatedTaskIndex = 0; - writtenTaskIndex = 0; - Debug.Assert(taskLocker == 0, "There appears to be a thread actively working still. That's invalid."); + paddedCounters.TaskIndex = 0; + paddedCounters.AllocatedTaskIndex = 0; + paddedCounters.WrittenTaskIndex = 0; + Debug.Assert(paddedCounters.TaskLocker == 0, "There appears to be a thread actively working still. That's invalid."); ref var continuations = ref continuationsContainer[0]; continuations.Continuations.Clear(0, continuations.Continuations.Length); @@ -390,7 +401,7 @@ public void Dispose(BufferPool pool) /// Gets the number of tasks active in the queue. /// /// Does not take a lock; if other threads are modifying the task values, the reported count may be invalid. - public int UnsafeTaskCount => (int)(writtenTaskIndex - taskIndex); + public int UnsafeTaskCount => (int)(paddedCounters.WrittenTaskIndex - paddedCounters.TaskIndex); /// /// Gets the number of tasks active in the queue. /// @@ -399,12 +410,12 @@ public int TaskCount get { var waiter = new SpinWait(); - while (Interlocked.CompareExchange(ref taskLocker, 1, 0) != 0) + while (Interlocked.CompareExchange(ref paddedCounters.TaskLocker, 1, 0) != 0) { waiter.SpinOnce(-1); } - var result = (int)(writtenTaskIndex - taskIndex); - taskLocker = 0; + var result = (int)(paddedCounters.WrittenTaskIndex - paddedCounters.TaskIndex); + paddedCounters.TaskLocker = 0; return result; } } @@ -495,14 +506,14 @@ public DequeueTaskResult TryDequeue(out Task task) if (Environment.Is64BitProcess) //This branch is compile time (at least where I've tested it). { //It's fine if we don't get a consistent view of the task index and written task index. Worst case scenario, this will claim that the queue is empty where a lock wouldn't. - nextTaskIndex = Volatile.Read(ref taskIndex); - sampledWrittenTaskIndex = Volatile.Read(ref writtenTaskIndex); + nextTaskIndex = Volatile.Read(ref paddedCounters.TaskIndex); + sampledWrittenTaskIndex = Volatile.Read(ref paddedCounters.WrittenTaskIndex); } else { //Interlocked reads for 32 bit systems. - nextTaskIndex = Interlocked.Read(ref taskIndex); - sampledWrittenTaskIndex = Interlocked.Read(ref writtenTaskIndex); + nextTaskIndex = Interlocked.Read(ref paddedCounters.TaskIndex); + sampledWrittenTaskIndex = Interlocked.Read(ref paddedCounters.WrittenTaskIndex); } if (nextTaskIndex >= sampledWrittenTaskIndex) return DequeueTaskResult.Empty; @@ -510,7 +521,7 @@ public DequeueTaskResult TryDequeue(out Task task) if (taskCandidate.Function == null) return DequeueTaskResult.Stop; //Unlike enqueues, a dequeue has a fixed contention window on a single value. There's not much point in using a spinwait when there's no reason to expect our next attempt to be blocked. - if (Interlocked.CompareExchange(ref taskIndex, nextTaskIndex + 1, nextTaskIndex) != nextTaskIndex) + if (Interlocked.CompareExchange(ref paddedCounters.TaskIndex, nextTaskIndex + 1, nextTaskIndex) != nextTaskIndex) continue; //There's an actual job! task = taskCandidate; @@ -537,15 +548,15 @@ public DequeueTaskResult TryDequeueAndRun(int workerIndex, IThreadDispatcher dis EnqueueTaskResult TryEnqueueUnsafelyInternal(Span tasks, out long taskEndIndex) { Debug.Assert(tasks.Length > 0, "Probably shouldn't be trying to enqueue zero tasks."); - Debug.Assert(writtenTaskIndex == 0 || this.tasks[(int)((writtenTaskIndex - 1) & taskMask)].Function != null, "No more jobs should be written after a stop command."); - var taskStartIndex = allocatedTaskIndex; + Debug.Assert(paddedCounters.WrittenTaskIndex == 0 || this.tasks[(int)((paddedCounters.WrittenTaskIndex - 1) & taskMask)].Function != null, "No more jobs should be written after a stop command."); + var taskStartIndex = paddedCounters.AllocatedTaskIndex; taskEndIndex = taskStartIndex + tasks.Length; - if (taskEndIndex - taskIndex > this.tasks.length) + if (taskEndIndex - paddedCounters.TaskIndex > this.tasks.length) { //We've run out of space in the ring buffer. If we tried to write, we'd overwrite jobs that haven't yet been completed. return EnqueueTaskResult.Full; } - allocatedTaskIndex = taskEndIndex; + paddedCounters.AllocatedTaskIndex = taskEndIndex; Debug.Assert(BitOperations.IsPow2(this.tasks.Length)); var wrappedInclusiveStartIndex = (int)(taskStartIndex & taskMask); var wrappedInclusiveEndIndex = (int)(taskEndIndex & taskMask); @@ -582,7 +593,7 @@ public EnqueueTaskResult TryEnqueueUnsafely(Span tasks) EnqueueTaskResult result; if ((result = TryEnqueueUnsafelyInternal(tasks, out var taskEndIndex)) == EnqueueTaskResult.Success) { - writtenTaskIndex = taskEndIndex; + paddedCounters.WrittenTaskIndex = taskEndIndex; } return result; } @@ -606,7 +617,7 @@ public EnqueueTaskResult TryEnqueue(Span tasks) { if (tasks.Length == 0) return EnqueueTaskResult.Success; - if (Interlocked.CompareExchange(ref taskLocker, 1, 0) != 0) + if (Interlocked.CompareExchange(ref paddedCounters.TaskLocker, 1, 0) != 0) return EnqueueTaskResult.Contested; try { @@ -616,18 +627,18 @@ public EnqueueTaskResult TryEnqueue(Span tasks) { if (Environment.Is64BitProcess) { - Volatile.Write(ref writtenTaskIndex, taskEndIndex); + Volatile.Write(ref paddedCounters.WrittenTaskIndex, taskEndIndex); } else { - Interlocked.Exchange(ref writtenTaskIndex, taskEndIndex); + Interlocked.Exchange(ref paddedCounters.WrittenTaskIndex, taskEndIndex); } } return result; } finally { - taskLocker = 0; + paddedCounters.TaskLocker = 0; } } diff --git a/Demos/SpecializedTests/TaskQueueTestDemo.cs b/Demos/SpecializedTests/TaskQueueTestDemo.cs index 4b3c269b4..df997d804 100644 --- a/Demos/SpecializedTests/TaskQueueTestDemo.cs +++ b/Demos/SpecializedTests/TaskQueueTestDemo.cs @@ -70,6 +70,11 @@ static void IssueStop(long id, void* context, int workerIndex, IThreadDispatcher typedContext->Queue->EnqueueStop(workerIndex, dispatcher); } + static void EmptyDispatch(int workerIndex, IThreadDispatcher dispatcher) + { + + } + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(-10, 3, -10); @@ -85,6 +90,14 @@ public override void Initialize(ContentArchive content, Camera camera) int tasksPerIteration = 64; var taskQueue = new TaskQueue(BufferPool); var taskQueuePointer = &taskQueue; + + //Test(() => + //{ + // for (int i = 0; i < 1024; ++i) + // ThreadDispatcher.DispatchWorkers(&EmptyDispatch); + // return 0; + //}, "Dispatch"); + Test(() => { var context = new Context { Queue = taskQueuePointer }; From 2289d19b0f59574d097c87977a98497a92fd5647 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 3 Dec 2022 18:52:37 -0600 Subject: [PATCH 668/947] Binned builder taking steps towards working, but not there yet! --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 94 +++++++++++-------- Demos/Demo.cs | 2 +- Demos/DemoSet.cs | 2 +- .../SpecializedTests/TreeFiddlingTestDemo.cs | 4 +- 4 files changed, 60 insertions(+), 42 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 9fdfdd9bd..bad6a34d9 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -450,9 +450,23 @@ unsafe struct BinnedBuildWorkerContext where TLeafCounts : unmanage public Buffer WorkerHelpedWithBinning; /// - /// Bins associated with this worker for use in the SAH scan. This will persist across the build. + /// Bins associated with this worker for the duration of a node. This allocation will persist across the build. + /// + /// This is technically redundant with the storage used for workers and ends up involving an extra bin scan on a multithreaded test, + /// but the cost associated with doing so is... low. The complexity cost of trying to use the memory allocated for workers is not low. + /// + public Buffer BinBoundingBoxes; + /// + /// Bins associated with this worker for use in the SAH scan. This allocation will persist across the build. /// public Buffer BinBoundingBoxesScan; + /// + /// Bin leaf counts associated with this worker for the duration of a node. This allocation will persist across the build. + /// + /// This is technically redundant with the storage used for workers and ends up involving an extra bin scan on a multithreaded test, + /// but the cost associated with doing so is... low. The complexity cost of trying to use the memory allocated for workers is not low. + /// + public Buffer BinLeafCounts; public int BinCount; public Vector4 CentroidBoundsMin; @@ -488,7 +502,7 @@ static Buffer Suballocate(Buffer buffer, ref int start, int count) w return buffer.Slice(previousStart, size).As(); } - public void AllocatePrepassWorkers(BufferPool pool, int taskCount) => pool.Take(Unsafe.SizeOf() * int.Min(WorkerCount, taskCount), out PrepassWorkers); + public void AllocatePrepassWorkers(BufferPool pool, int taskCount) => pool.Take(int.Min(WorkerCount, taskCount), out PrepassWorkers); public void DisposePrepassWorkers(BufferPool pool) => pool.Return(ref PrepassWorkers); public void AllocateBinSubtreesWorkers(BufferPool pool, int taskCount) { @@ -510,30 +524,31 @@ public void AllocateBinSubtreesWorkers(BufferPool pool, int taskCount) public void DisposeBinSubtreesWorkers(BufferPool pool) => pool.Return(ref BinSubtreesWorkers); //Only need to return the main buffer because all the other allocations share the same id! - public BinnedBuildWorkerContext(Buffer bounds, Buffer binBoundingBoxesScan, TLeafCounts leafCounts, int workerCount) + public BinnedBuildWorkerContext(Buffer bounds, Buffer binAllocationBuffer, ref int binStart, int binCount, TLeafCounts leafCounts, int workerCount) { WorkerCount = workerCount; - BinBoundingBoxesScan = binBoundingBoxesScan; + BinBoundingBoxes = Suballocate(binAllocationBuffer, ref binStart, binCount); + BinBoundingBoxesScan = Suballocate(binAllocationBuffer, ref binStart, binCount); + BinLeafCounts = Suballocate(binAllocationBuffer, ref binStart, binCount); Bounds = bounds; LeafCounts = leafCounts; } public int SetupTaskCounts(int subtreeStartIndex, int slotCount, int slotsPerTaskTarget, int maximumTaskCountPerSubmission) { - Debug.Assert(BitOperations.IsPow2(slotsPerTaskTarget), "This mask trick requires power of 2 task size targets."); - var mask = slotsPerTaskTarget - 1; - int taskCount = Math.Min(maximumTaskCountPerSubmission, (slotCount + mask) & mask); + Debug.Assert(BitOperations.IsPow2(slotsPerTaskTarget), "Ideally, this gets inlined and the division becomes a shift. Can't do that if the count isn't a power of 2."); + int taskCount = int.Min(maximumTaskCountPerSubmission, (slotCount + slotsPerTaskTarget - 1) / slotsPerTaskTarget); SubtreeStartIndex = subtreeStartIndex; SubtreeCount = slotCount; SlotsPerTaskBase = slotCount / taskCount; SlotRemainder = slotCount - taskCount * SlotsPerTaskBase; - TaskCountFitsInWorkerCount = taskCount <= PrepassWorkers.Length; + TaskCountFitsInWorkerCount = taskCount <= WorkerCount; return taskCount; } public void GetSlotInterval(long taskId, out int start, out int count) { - var remainderedTaskCount = Math.Min(SlotRemainder, taskId); + var remainderedTaskCount = int.Min(SlotRemainder, (int)taskId); var earlySlotCount = (SlotsPerTaskBase + 1) * remainderedTaskCount; var lateSlotCount = SlotsPerTaskBase * (taskId - remainderedTaskCount); start = SubtreeStartIndex + (int)(earlySlotCount + lateSlotCount); @@ -553,10 +568,9 @@ unsafe struct MultithreadBinnedBuildContext : IBinnedBuilderThreadi public void GetBins(int workerIndex, out Buffer binBoundingBoxes, out Buffer binBoundingBoxesScan, out Buffer binLeafCounts) { ref var worker = ref Workers[workerIndex]; - ref var cache0 = ref worker.BinSubtreesWorkers[0]; - binBoundingBoxes = cache0.BinBoundingBoxes; + binBoundingBoxes = worker.BinBoundingBoxes; binBoundingBoxesScan = worker.BinBoundingBoxesScan; - binLeafCounts = cache0.BinLeafCounts; + binLeafCounts = worker.BinLeafCounts; } } @@ -609,7 +623,7 @@ unsafe static BoundingBox4 MultithreadedCentroidPrepass(Multithread worker.AllocatePrepassWorkers(workerPool, taskCount); //Don't bother initializing more slots than we have tasks. Note that this requires special handling on the task level; //if we have less tasks than workers, then the task needs to distinguish that fact. - var activeWorkerCount = Math.Min(context->Workers.Length, taskCount); + var activeWorkerCount = int.Min(context->Workers.Length, taskCount); if (taskCount > context->Workers.Length) { //Potentially multiple tasks per worker; we must preinitialize slots. @@ -680,10 +694,20 @@ unsafe static void MultithreadedBinSubtrees(MultithreadBinnedBuildC ref var worker = ref context->Workers[workerIndex]; var taskCount = worker.SetupTaskCounts(subtreeStartIndex, subtreeCount, SubtreesPerThreadForBinning, context->MaximumTaskCountPerSubmission); var workerPool = dispatcher.WorkerPools[workerIndex]; + + worker.BinCount = binCount; + worker.CentroidBoundsMin = centroidBoundsMin; + worker.UseX = useX; + worker.UseY = useY; + worker.PermuteMask = permuteMask; + worker.AxisIndex = axisIndex; + worker.OffsetToBinIndex = offsetToBinIndex; + worker.MaximumBinIndex = maximumBinIndex; + worker.AllocateBinSubtreesWorkers(workerPool, taskCount); //Don't bother initializing more slots than we have tasks. Note that this requires special handling on the task level; //if we have less tasks than workers, then the task needs to distinguish that fact. - var activeWorkerCount = Math.Min(context->Workers.Length, taskCount); + var activeWorkerCount = int.Min(context->Workers.Length, taskCount); if (!worker.TaskCountFitsInWorkerCount) { //If there are more tasks than workers, then we need to preinitialize all the worker caches. @@ -699,21 +723,18 @@ unsafe static void MultithreadedBinSubtrees(MultithreadBinnedBuildC } } } - worker.BinCount = binCount; - worker.CentroidBoundsMin = centroidBoundsMin; - worker.UseX = useX; - worker.UseY = useY; - worker.PermuteMask = permuteMask; - worker.AxisIndex = axisIndex; - worker.OffsetToBinIndex = offsetToBinIndex; - worker.MaximumBinIndex = maximumBinIndex; context->Queue->For(&BinSubtreesWorker, Unsafe.AsPointer(ref worker), 0, taskCount, workerIndex, dispatcher); //Unless the number of threads and bins is really huge, there's no value in attempting to multithread the final compression. //(Parallel reduction is an option, but even then... I suspect the single threaded version will be faster. And it's way simpler.) - //Merge everything into cache 0. That's the source of bins for the worker that invoked this dispatch. + //Note that we have a separate merging target from the caches; that just makes resource management easier. + //We can dispose the worker stuff immediately after this merge. + //(Consider what happens in the case where the single threaded path is used: you need an allocation! would you allocate a bunch of multithreaded workers for it? + //That's not an irrelevant case, either. *Most* nodes will be too small to warrant internal multithreading.) ref var cache0 = ref worker.BinSubtreesWorkers[0]; + cache0.BinBoundingBoxes.CopyTo(0, worker.BinBoundingBoxes, 0, cache0.BinBoundingBoxes.Length); + cache0.BinLeafCounts.CopyTo(0, worker.BinLeafCounts, 0, cache0.BinLeafCounts.Length); for (int cacheIndex = 1; cacheIndex < activeWorkerCount; ++cacheIndex) { //Only bother merging from workers that actually did anything. @@ -722,7 +743,7 @@ unsafe static void MultithreadedBinSubtrees(MultithreadBinnedBuildC ref var cache = ref worker.BinSubtreesWorkers[cacheIndex]; for (int binIndex = 0; binIndex < binCount; ++binIndex) { - ref var b0 = ref cache0.BinBoundingBoxes[binIndex]; + ref var b0 = ref worker.BinBoundingBoxes[binIndex]; ref var bi = ref cache.BinBoundingBoxes[binIndex]; b0.Min = Vector4.Min(b0.Min, bi.Min); b0.Max = Vector4.Min(b0.Max, bi.Max); @@ -826,14 +847,15 @@ static unsafe void BinnedBuildNode( var permuteMask = Vector128.Create(useX ? 0 : useY ? 1 : 2, 0, 0, 0); var axisIndex = useX ? 0 : useY ? 1 : 2; - var binCount = Math.Min(context->MaximumBinCount, Math.Max((int)(subtreeCount * context->LeafToBinMultiplier), context->MinimumBinCount)); - context->Threading.GetBins(workerIndex, out var binBoundingBoxes, out var binBoundingBoxesScan, out var binLeafCounts); - Debug.Assert(binBoundingBoxes.Length >= binCount); + var binCount = int.Min(context->MaximumBinCount, int.Max((int)(subtreeCount * context->LeafToBinMultiplier), context->MinimumBinCount)); var offsetToBinIndex = new Vector4(binCount) / centroidSpan; //Avoid letting NaNs into the offsetToBinIndex scale. offsetToBinIndex = Vector128.ConditionalSelect(axisIsDegenerate, Vector128.Zero, offsetToBinIndex.AsVector128()).AsVector4(); + var maximumBinIndex = new Vector4(binCount - 1); + context->Threading.GetBins(workerIndex, out var binBoundingBoxes, out var binBoundingBoxesScan, out var binLeafCounts); + Debug.Assert(binBoundingBoxes.Length >= binCount); for (int i = 0; i < binCount; ++i) { ref var boxX = ref binBoundingBoxes[i]; @@ -841,8 +863,6 @@ static unsafe void BinnedBuildNode( boxX.Max = new Vector4(float.MinValue); binLeafCounts[i] = 0; } - - var maximumBinIndex = new Vector4(binCount - 1); if (typeof(TThreading) == typeof(SingleThreaded) || subtreeCount < SubtreesPerThreadForBinning) { BinSubtrees(centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, boundingBoxes, leafCounts, binBoundingBoxes, binLeafCounts); @@ -1011,10 +1031,10 @@ static unsafe void BinnedBuilderInternal(Buffer encodedLeafIndices, Buffer< //Don't let the user pick values that will just cause an explosion. Debug.Assert(minimumBinCount >= 2 && maximumBinCount >= 2, "At least two bins are required. In release mode, this will be clamped up to 2, but where did lower values come from?"); - minimumBinCount = Math.Max(2, minimumBinCount); - maximumBinCount = Math.Max(2, maximumBinCount); + minimumBinCount = int.Max(2, minimumBinCount); + maximumBinCount = int.Max(2, maximumBinCount); //The microsweep uses the same resources as the bin allocations, so expand to hold whichever is larger. - var allocatedBinCount = Math.Max(maximumBinCount, microsweepThreshold); + var allocatedBinCount = int.Max(maximumBinCount, microsweepThreshold); if (dispatcher == null && taskQueuePointer == null) { @@ -1041,15 +1061,16 @@ static unsafe void BinnedBuilderInternal(Buffer encodedLeafIndices, Buffer< //Those node jobs could spawn multithreaded work that other workers assist with. //Each of those jobs needs its own context for those workers, and the number of jobs is not 1:1 with the workers. //We'll handle such dispatch-required allocations from worker pools. Here, we just preallocate stuff for the first level across all workers. - pool.Take(maximumBinCount * workerCount, out var workerBinsForScan); + pool.Take(maximumBinCount * workerCount * (sizeof(BoundingBox4) * 2 + sizeof(int)), out var workerBinsAllocation); BinnedBuildWorkerContext* workerContextsPointer = stackalloc BinnedBuildWorkerContext[workerCount]; var leafCounts = new UnitLeafCount(); var workerContexts = new Buffer>(workerContextsPointer, workerCount); + int binAllocationStart = 0; for (int i = 0; i < workerCount; ++i) { - workerContexts[i] = new BinnedBuildWorkerContext(boundingBoxes.As(), workerBinsForScan.Slice(i * maximumBinCount, (i + 1) * maximumBinCount), leafCounts, workerCount); + workerContexts[i] = new BinnedBuildWorkerContext(boundingBoxes.As(), workerBinsAllocation, ref binAllocationStart, maximumBinCount, leafCounts, workerCount); } TaskQueue taskQueue = default; @@ -1072,12 +1093,9 @@ static unsafe void BinnedBuilderInternal(Buffer encodedLeafIndices, Buffer< taskQueuePointer->TryEnqueueUnsafely(new Task(&BinnedBuilderWorkerEntry>, &context)); dispatcher.DispatchWorkers(&BinnedBuilderWorkerFunction>, unmanagedContext: taskQueuePointer); - if (createdTaskQueueLocally) taskQueue.Dispose(pool); - pool.Return(ref workerBinsForScan); - - + pool.Return(ref workerBinsAllocation); } } diff --git a/Demos/Demo.cs b/Demos/Demo.cs index b150f4693..264961f51 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -43,7 +43,7 @@ protected Demo() //there won't be enough memory bandwidth to even feed half the physical cores. Using all 128 logical cores would just add overhead. //It may be worth using something like hwloc or CPUID to extract extra information to reason about. - var targetThreadCount = Math.Max(1, Environment.ProcessorCount > 4 ? Environment.ProcessorCount - 2 : Environment.ProcessorCount - 1); + var targetThreadCount = 2;// int.Max(1, Environment.ProcessorCount > 4 ? Environment.ProcessorCount - 2 : Environment.ProcessorCount - 1); ThreadDispatcher = new ThreadDispatcher(targetThreadCount); } diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index a80e8370a..a59471775 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -47,7 +47,7 @@ struct Option public DemoSet() { //AddOption(); - //AddOption(); + AddOption(); AddOption(); AddOption(); AddOption(); diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index ad502ebc0..3a99617dd 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -158,8 +158,8 @@ public override void Initialize(ContentArchive content, Camera camera) Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(4, 1)); //Create a mesh. - var width = 768; - var height = 768; + var width = 32;// 768; + var height = 16;// 768; var scale = new Vector3(1, 1, 1); //DemoMeshHelper.CreateDeformedPlane(width, height, (x, y) => new Vector3(x - width * scale.X * 0.5f, 2f * (float)(Math.Sin(x * 0.5f) * Math.Sin(y * 0.5f)), y - height * scale.Y * 0.5f), scale, BufferPool, out var mesh); //DemoMeshHelper.CreateDeformedPlane(width, height, (x, y) => new Vector3(x - width * scale.X * 0.5f, 0, y - height * scale.Y * 0.5f), scale, BufferPool, out var mesh); From 9fe197fcd1aaf42fd7032abe87ea7a2d3cf4468c Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 5 Dec 2022 17:44:04 -0600 Subject: [PATCH 669/947] Dedoofused allocations. Still broken more generally. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 301 ++++++++++-------- .../SpecializedTests/TreeFiddlingTestDemo.cs | 4 +- 2 files changed, 168 insertions(+), 137 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index bad6a34d9..fe668d1a6 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -419,10 +419,12 @@ public void GetBins(int workerIndex, out Buffer binBoundingBoxes, } } - struct BinSubtreesWorkerContext + static Buffer Suballocate(Buffer buffer, ref int start, int count) where T : unmanaged { - public Buffer BinBoundingBoxes; - public Buffer BinLeafCounts; + var size = count * Unsafe.SizeOf(); + var previousStart = start; + start += size; + return buffer.Slice(previousStart, size).As(); } /// @@ -430,25 +432,6 @@ struct BinSubtreesWorkerContext /// unsafe struct BinnedBuildWorkerContext where TLeafCounts : unmanaged, ILeafCountBuffer { - /// - /// Stores per-worker prepass bounds accumulated over multiple tasks. If there are less tasks than workers, then only the lower contiguous region of these bounds are used. - /// This allocation is ephemeral; it is allocated from the current worker when needed. - /// Note that the allocation occurs on the loop dispatching thread: the workers that help with the loop do not have to allocate anything themselves. - /// - public Buffer PrepassWorkers; - /// - /// Bins associated with any workers that end up contributing to this worker's dispatch of a binning loop. If there are less tasks than workers, then only the lower contiguous region of these bounds are used. - /// This allocation is ephemeral; it is allocated from the current worker when needed. - /// Note that the allocation occurs on the loop dispatching thread: the workers that help with the loop do not have to allocate anything themselves. - /// - public Buffer BinSubtreesWorkers; - /// - /// Whether a given worker contributed to the subtree binning process. If this worker did not contribute, there's no reason to merge its bins. - /// This allocation is ephemeral; it is allocated from the current worker when needed. - /// Note that the allocation occurs on the loop dispatching thread: the workers that help with the loop do not have to allocate anything themselves. - /// - public Buffer WorkerHelpedWithBinning; - /// /// Bins associated with this worker for the duration of a node. This allocation will persist across the build. /// @@ -468,23 +451,6 @@ unsafe struct BinnedBuildWorkerContext where TLeafCounts : unmanage /// public Buffer BinLeafCounts; - public int BinCount; - public Vector4 CentroidBoundsMin; - public bool UseX, UseY; - public Vector128 PermuteMask; - public int AxisIndex; - public Vector4 OffsetToBinIndex; - public Vector4 MaximumBinIndex; - - public int WorkerCount; - - public int SubtreeStartIndex; - public int SubtreeCount; - - public int SlotsPerTaskBase; - public int SlotRemainder; - public bool TaskCountFitsInWorkerCount; - /// /// Bounds for all subtrees in the builder. Cached in the worker to simplify accesses. /// @@ -494,66 +460,14 @@ unsafe struct BinnedBuildWorkerContext where TLeafCounts : unmanage /// public TLeafCounts LeafCounts; - static Buffer Suballocate(Buffer buffer, ref int start, int count) where T : unmanaged - { - var size = count * Unsafe.SizeOf(); - var previousStart = start; - start += size; - return buffer.Slice(previousStart, size).As(); - } - - public void AllocatePrepassWorkers(BufferPool pool, int taskCount) => pool.Take(int.Min(WorkerCount, taskCount), out PrepassWorkers); - public void DisposePrepassWorkers(BufferPool pool) => pool.Return(ref PrepassWorkers); - public void AllocateBinSubtreesWorkers(BufferPool pool, int taskCount) - { - var effectiveWorkerCount = int.Min(WorkerCount, taskCount); - //Pull one allocation from the pool instead of 1 + workerCount * 2. Slight reduction in overhead. Note that this means we only need to return one buffer of the associated id at the end! - var allocationSize = (sizeof(BinSubtreesWorkerContext) + (sizeof(BoundingBox4) + sizeof(int)) * BinCount + sizeof(bool) * WorkerCount) * effectiveWorkerCount; - pool.Take(allocationSize, out var allocation); - int start = 0; - BinSubtreesWorkers = Suballocate(allocation, ref start, effectiveWorkerCount); - for (int i = 0; i < effectiveWorkerCount; ++i) - { - ref var worker = ref BinSubtreesWorkers[i]; - worker.BinBoundingBoxes = Suballocate(allocation, ref start, BinCount); - worker.BinLeafCounts = Suballocate(allocation, ref start, BinCount); - } - WorkerHelpedWithBinning = Suballocate(allocation, ref start, effectiveWorkerCount); - WorkerHelpedWithBinning.Clear(0, effectiveWorkerCount); - } - public void DisposeBinSubtreesWorkers(BufferPool pool) => pool.Return(ref BinSubtreesWorkers); //Only need to return the main buffer because all the other allocations share the same id! - - - public BinnedBuildWorkerContext(Buffer bounds, Buffer binAllocationBuffer, ref int binStart, int binCount, TLeafCounts leafCounts, int workerCount) + public BinnedBuildWorkerContext(Buffer bounds, Buffer binAllocationBuffer, ref int binStart, int binCount, TLeafCounts leafCounts) { - WorkerCount = workerCount; BinBoundingBoxes = Suballocate(binAllocationBuffer, ref binStart, binCount); BinBoundingBoxesScan = Suballocate(binAllocationBuffer, ref binStart, binCount); BinLeafCounts = Suballocate(binAllocationBuffer, ref binStart, binCount); Bounds = bounds; LeafCounts = leafCounts; } - - public int SetupTaskCounts(int subtreeStartIndex, int slotCount, int slotsPerTaskTarget, int maximumTaskCountPerSubmission) - { - Debug.Assert(BitOperations.IsPow2(slotsPerTaskTarget), "Ideally, this gets inlined and the division becomes a shift. Can't do that if the count isn't a power of 2."); - int taskCount = int.Min(maximumTaskCountPerSubmission, (slotCount + slotsPerTaskTarget - 1) / slotsPerTaskTarget); - SubtreeStartIndex = subtreeStartIndex; - SubtreeCount = slotCount; - SlotsPerTaskBase = slotCount / taskCount; - SlotRemainder = slotCount - taskCount * SlotsPerTaskBase; - TaskCountFitsInWorkerCount = taskCount <= WorkerCount; - return taskCount; - } - - public void GetSlotInterval(long taskId, out int start, out int count) - { - var remainderedTaskCount = int.Min(SlotRemainder, (int)taskId); - var earlySlotCount = (SlotsPerTaskBase + 1) * remainderedTaskCount; - var lateSlotCount = SlotsPerTaskBase * (taskId - remainderedTaskCount); - start = SubtreeStartIndex + (int)(earlySlotCount + lateSlotCount); - count = taskId >= SlotRemainder ? SlotsPerTaskBase : SlotsPerTaskBase + 1; - } } unsafe struct MultithreadBinnedBuildContext : IBinnedBuilderThreading where TLeafCounts : unmanaged, ILeafCountBuffer { @@ -575,9 +489,9 @@ public void GetBins(int workerIndex, out Buffer binBoundingBoxes, } //These should be powers of 2 for maskhack reasons. - const int SubtreesPerThreadForCentroidPrepass = 2048; - const int SubtreesPerThreadForBinning = 512; - const int SubtreesPerThreadForNodeJob = 512; + const int SubtreesPerThreadForCentroidPrepass = 2;//2048; + const int SubtreesPerThreadForBinning = 2;//512; + const int SubtreesPerThreadForNodeJob = 2;//512; [MethodImpl(MethodImplOptions.AggressiveInlining)] static BoundingBox4 ComputeCentroidBounds(Buffer bounds) @@ -596,12 +510,68 @@ static BoundingBox4 ComputeCentroidBounds(Buffer bounds) return centroidBounds; } - unsafe static void CentroidPrepassWorker(long taskId, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) where TLeafCounts : unmanaged, ILeafCountBuffer + struct SharedTaskData + { + public int WorkerCount; + public int TaskCount; + + public int SubtreeStartIndex; + public int SubtreeCount; + + public int SlotsPerTaskBase; + public int SlotRemainder; + public bool TaskCountFitsInWorkerCount; + + public SharedTaskData(int workerCount, int subtreeStartIndex, int slotCount, int slotsPerTaskTarget, int maximumTaskCountPerSubmission) + { + WorkerCount = workerCount; + Debug.Assert(BitOperations.IsPow2(slotsPerTaskTarget), "Ideally, this gets inlined and the division becomes a shift. Can't do that if the count isn't a power of 2."); + TaskCount = int.Min(maximumTaskCountPerSubmission, (slotCount + slotsPerTaskTarget - 1) / slotsPerTaskTarget); + SubtreeStartIndex = subtreeStartIndex; + SubtreeCount = slotCount; + SlotsPerTaskBase = slotCount / TaskCount; + SlotRemainder = slotCount - TaskCount * SlotsPerTaskBase; + TaskCountFitsInWorkerCount = TaskCount <= WorkerCount; + } + + public void GetSlotInterval(long taskId, out int start, out int count) + { + var remainderedTaskCount = int.Min(SlotRemainder, (int)taskId); + var earlySlotCount = (SlotsPerTaskBase + 1) * remainderedTaskCount; + var lateSlotCount = SlotsPerTaskBase * (taskId - remainderedTaskCount); + start = SubtreeStartIndex + (int)(earlySlotCount + lateSlotCount); + count = taskId >= SlotRemainder ? SlotsPerTaskBase : SlotsPerTaskBase + 1; + } + } + unsafe struct CentroidPrepassTaskContext + { + public SharedTaskData TaskData; + /// + /// Stores per-worker prepass bounds accumulated over multiple tasks. If there are less tasks than workers, then only the lower contiguous region of these bounds are used. + /// This allocation is ephemeral; it is allocated from the current worker when needed. + /// Note that the allocation occurs on the loop dispatching thread: the workers that help with the loop do not have to allocate anything themselves. + /// + public Buffer PrepassWorkers; + /// + /// Buffer containing all bounds in the tree. + /// + public Buffer Bounds; + + public CentroidPrepassTaskContext(BufferPool pool, SharedTaskData taskData, Buffer bounds) + { + TaskData = taskData; + pool.Take(int.Min(taskData.WorkerCount, taskData.TaskCount), out PrepassWorkers); + Bounds = bounds; + } + + public void Dispose(BufferPool pool) => pool.Return(ref PrepassWorkers); + } + unsafe static void CentroidPrepassWorker(long taskId, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) { - ref var context = ref *(BinnedBuildWorkerContext*)untypedContext; - context.GetSlotInterval(taskId, out var start, out var count); + ref var context = ref *(CentroidPrepassTaskContext*)untypedContext; + context.TaskData.GetSlotInterval(taskId, out var start, out var count); var centroidBounds = ComputeCentroidBounds(context.Bounds.Slice(start, count)); - if (context.TaskCountFitsInWorkerCount) + if (context.TaskData.TaskCountFitsInWorkerCount) { //There were less tasks than workers; directly write into the slot without bothering to merge. context.PrepassWorkers[(int)taskId] = centroidBounds; @@ -618,35 +588,106 @@ unsafe static BoundingBox4 MultithreadedCentroidPrepass(Multithread where TLeafCounts : unmanaged, ILeafCountBuffer { ref var worker = ref context->Workers[workerIndex]; - var taskCount = worker.SetupTaskCounts(subtreeStartIndex, subtreeCount, SubtreesPerThreadForCentroidPrepass, context->MaximumTaskCountPerSubmission); var workerPool = dispatcher.WorkerPools[workerIndex]; - worker.AllocatePrepassWorkers(workerPool, taskCount); + var taskContext = new CentroidPrepassTaskContext(workerPool, new SharedTaskData(context->Workers.Length, subtreeStartIndex, subtreeCount, SubtreesPerThreadForCentroidPrepass, context->MaximumTaskCountPerSubmission), worker.Bounds); + var taskCount = taskContext.TaskData.TaskCount; //Don't bother initializing more slots than we have tasks. Note that this requires special handling on the task level; //if we have less tasks than workers, then the task needs to distinguish that fact. - var activeWorkerCount = int.Min(context->Workers.Length, taskCount); - if (taskCount > context->Workers.Length) + var activeWorkerCount = int.Min(taskContext.TaskData.WorkerCount, taskCount); + if (taskCount > taskContext.TaskData.WorkerCount) { //Potentially multiple tasks per worker; we must preinitialize slots. for (int i = 0; i < activeWorkerCount; ++i) { - ref var workerBounds = ref worker.PrepassWorkers[i]; + ref var workerBounds = ref taskContext.PrepassWorkers[i]; workerBounds.Min = new Vector4(float.MaxValue); workerBounds.Max = new Vector4(float.MinValue); } } - context->Queue->For(&CentroidPrepassWorker, Unsafe.AsPointer(ref worker), 0, taskCount, workerIndex, dispatcher); + context->Queue->For(&CentroidPrepassWorker, &taskContext, 0, taskCount, workerIndex, dispatcher); - var centroidBounds = worker.PrepassWorkers[0]; + var centroidBounds = taskContext.PrepassWorkers[0]; for (int i = 1; i < activeWorkerCount; ++i) { - ref var workerBounds = ref worker.PrepassWorkers[i]; + ref var workerBounds = ref taskContext.PrepassWorkers[i]; centroidBounds.Min = Vector4.Min(workerBounds.Min, centroidBounds.Min); centroidBounds.Max = Vector4.Max(workerBounds.Max, centroidBounds.Max); } - worker.DisposePrepassWorkers(workerPool); + taskContext.Dispose(workerPool); return centroidBounds; } + struct BinSubtreesWorkerContext + { + public Buffer BinBoundingBoxes; + public Buffer BinLeafCounts; + } + unsafe struct BinSubtreesTaskContext where TLeafCounts : unmanaged, ILeafCountBuffer + { + public SharedTaskData TaskData; + /// + /// Bins associated with any workers that end up contributing to this worker's dispatch of a binning loop. If there are less tasks than workers, then only the lower contiguous region of these bounds are used. + /// This allocation is ephemeral; it is allocated from the current worker when needed. + /// Note that the allocation occurs on the loop dispatching thread: the workers that help with the loop do not have to allocate anything themselves. + /// + public Buffer BinSubtreesWorkers; + /// + /// Whether a given worker contributed to the subtree binning process. If this worker did not contribute, there's no reason to merge its bins. + /// This allocation is ephemeral; it is allocated from the current worker when needed. + /// Note that the allocation occurs on the loop dispatching thread: the workers that help with the loop do not have to allocate anything themselves. + /// + public Buffer WorkerHelpedWithBinning; + + /// + /// Buffer containing all bounds in the tree. + /// + public Buffer Bounds; + /// + /// Buffer containing all leaf counts in the tree. + /// + public TLeafCounts LeafCounts; + + public int BinCount; + public bool UseX, UseY; + public Vector128 PermuteMask; + public int AxisIndex; + public Vector4 CentroidBoundsMin; + public Vector4 OffsetToBinIndex; + public Vector4 MaximumBinIndex; + + public BinSubtreesTaskContext(BufferPool pool, SharedTaskData taskData, Buffer bounds, TLeafCounts leafCounts, + int binCount, bool useX, bool useY, Vector128 permuteMask, int axisIndex, + Vector4 centroidBoundsMin, Vector4 offsetToBinIndex, Vector4 maximumBinIndex) + { + TaskData = taskData; + Bounds = bounds; + LeafCounts = leafCounts; + BinCount = binCount; + UseX = useX; + UseY = useY; + PermuteMask = permuteMask; + AxisIndex = axisIndex; + CentroidBoundsMin = centroidBoundsMin; + OffsetToBinIndex = offsetToBinIndex; + MaximumBinIndex = maximumBinIndex; + var effectiveWorkerCount = int.Min(taskData.WorkerCount, taskData.TaskCount); + //Pull one allocation from the pool instead of 1 + workerCount * 2. Slight reduction in overhead. Note that this means we only need to return one buffer of the associated id at the end! + var allocationSize = (sizeof(BinSubtreesWorkerContext) + (sizeof(BoundingBox4) + sizeof(int)) * binCount + sizeof(bool) * taskData.WorkerCount) * effectiveWorkerCount; + pool.Take(allocationSize, out var allocation); + int start = 0; + BinSubtreesWorkers = Suballocate(allocation, ref start, effectiveWorkerCount); + for (int i = 0; i < effectiveWorkerCount; ++i) + { + ref var worker = ref BinSubtreesWorkers[i]; + worker.BinBoundingBoxes = Suballocate(allocation, ref start, BinCount); + worker.BinLeafCounts = Suballocate(allocation, ref start, BinCount); + } + WorkerHelpedWithBinning = Suballocate(allocation, ref start, effectiveWorkerCount); + WorkerHelpedWithBinning.Clear(0, effectiveWorkerCount); + } + public void Dispose(BufferPool pool) => pool.Return(ref BinSubtreesWorkers); //Only need to return the main buffer because all the other allocations share the same id! + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] static void BinSubtrees(Vector4 centroidBoundsMin, bool useX, bool useY, Vector128 permuteMask, int axisIndex, Vector4 offsetToBinIndex, Vector4 maximumBinIndex, @@ -667,12 +708,12 @@ static void BinSubtrees(Vector4 centroidBoundsMin, } unsafe static void BinSubtreesWorker(long taskId, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) where TLeafCounts : unmanaged, ILeafCountBuffer { - ref var context = ref *(BinnedBuildWorkerContext*)untypedContext; + ref var context = ref *(BinSubtreesTaskContext*)untypedContext; //Note that if we have more workers than tasks, we use the task id to index into the caches (and initialize the data here rather then before dispatching). - var effectiveWorkerIndex = context.TaskCountFitsInWorkerCount ? (int)taskId : workerIndex; + var effectiveWorkerIndex = context.TaskData.TaskCountFitsInWorkerCount ? (int)taskId : workerIndex; ref var worker = ref context.BinSubtreesWorkers[effectiveWorkerIndex]; context.WorkerHelpedWithBinning[effectiveWorkerIndex] = true; - if (context.TaskCountFitsInWorkerCount) + if (context.TaskData.TaskCountFitsInWorkerCount) { for (int i = 0; i < context.BinCount; ++i) { @@ -682,7 +723,7 @@ unsafe static void BinSubtreesWorker(long taskId, void* untypedCont worker.BinLeafCounts[i] = 0; } } - context.GetSlotInterval(taskId, out var start, out var count); + context.TaskData.GetSlotInterval(taskId, out var start, out var count); BinSubtrees(context.CentroidBoundsMin, context.UseX, context.UseY, context.PermuteMask, context.AxisIndex, context.OffsetToBinIndex, context.MaximumBinIndex, context.Bounds.Slice(start, count), context.LeafCounts.Slice(start, count), worker.BinBoundingBoxes, worker.BinLeafCounts); } @@ -692,28 +733,21 @@ unsafe static void MultithreadedBinSubtrees(MultithreadBinnedBuildC int subtreeStartIndex, int subtreeCount, int binCount, int workerIndex, IThreadDispatcher dispatcher) where TLeafCounts : unmanaged, ILeafCountBuffer { ref var worker = ref context->Workers[workerIndex]; - var taskCount = worker.SetupTaskCounts(subtreeStartIndex, subtreeCount, SubtreesPerThreadForBinning, context->MaximumTaskCountPerSubmission); var workerPool = dispatcher.WorkerPools[workerIndex]; + var taskContext = new BinSubtreesTaskContext( + workerPool, + new SharedTaskData(context->Workers.Length, subtreeStartIndex, subtreeCount, SubtreesPerThreadForBinning, context->MaximumTaskCountPerSubmission), + worker.Bounds, worker.LeafCounts, binCount, useX, useY, permuteMask, axisIndex, centroidBoundsMin, offsetToBinIndex, maximumBinIndex); - worker.BinCount = binCount; - worker.CentroidBoundsMin = centroidBoundsMin; - worker.UseX = useX; - worker.UseY = useY; - worker.PermuteMask = permuteMask; - worker.AxisIndex = axisIndex; - worker.OffsetToBinIndex = offsetToBinIndex; - worker.MaximumBinIndex = maximumBinIndex; - - worker.AllocateBinSubtreesWorkers(workerPool, taskCount); //Don't bother initializing more slots than we have tasks. Note that this requires special handling on the task level; //if we have less tasks than workers, then the task needs to distinguish that fact. - var activeWorkerCount = int.Min(context->Workers.Length, taskCount); - if (!worker.TaskCountFitsInWorkerCount) + var activeWorkerCount = int.Min(context->Workers.Length, taskContext.TaskData.TaskCount); + if (!taskContext.TaskData.TaskCountFitsInWorkerCount) { //If there are more tasks than workers, then we need to preinitialize all the worker caches. for (int cacheIndex = 0; cacheIndex < activeWorkerCount; ++cacheIndex) { - ref var cache = ref worker.BinSubtreesWorkers[cacheIndex]; + ref var cache = ref taskContext.BinSubtreesWorkers[cacheIndex]; for (int i = 0; i < binCount; ++i) { ref var binBounds = ref cache.BinBoundingBoxes[i]; @@ -724,7 +758,7 @@ unsafe static void MultithreadedBinSubtrees(MultithreadBinnedBuildC } } - context->Queue->For(&BinSubtreesWorker, Unsafe.AsPointer(ref worker), 0, taskCount, workerIndex, dispatcher); + context->Queue->For(&BinSubtreesWorker, &taskContext, 0, taskContext.TaskData.TaskCount, workerIndex, dispatcher); //Unless the number of threads and bins is really huge, there's no value in attempting to multithread the final compression. //(Parallel reduction is an option, but even then... I suspect the single threaded version will be faster. And it's way simpler.) @@ -732,15 +766,15 @@ unsafe static void MultithreadedBinSubtrees(MultithreadBinnedBuildC //We can dispose the worker stuff immediately after this merge. //(Consider what happens in the case where the single threaded path is used: you need an allocation! would you allocate a bunch of multithreaded workers for it? //That's not an irrelevant case, either. *Most* nodes will be too small to warrant internal multithreading.) - ref var cache0 = ref worker.BinSubtreesWorkers[0]; + ref var cache0 = ref taskContext.BinSubtreesWorkers[0]; cache0.BinBoundingBoxes.CopyTo(0, worker.BinBoundingBoxes, 0, cache0.BinBoundingBoxes.Length); cache0.BinLeafCounts.CopyTo(0, worker.BinLeafCounts, 0, cache0.BinLeafCounts.Length); for (int cacheIndex = 1; cacheIndex < activeWorkerCount; ++cacheIndex) { //Only bother merging from workers that actually did anything. - if (worker.WorkerHelpedWithBinning[cacheIndex]) + if (taskContext.WorkerHelpedWithBinning[cacheIndex]) { - ref var cache = ref worker.BinSubtreesWorkers[cacheIndex]; + ref var cache = ref taskContext.BinSubtreesWorkers[cacheIndex]; for (int binIndex = 0; binIndex < binCount; ++binIndex) { ref var b0 = ref worker.BinBoundingBoxes[binIndex]; @@ -751,7 +785,7 @@ unsafe static void MultithreadedBinSubtrees(MultithreadBinnedBuildC } } } - worker.DisposeBinSubtreesWorkers(workerPool); + taskContext.Dispose(workerPool); } unsafe struct NodePushTaskContext @@ -1004,9 +1038,6 @@ static unsafe void BinnedBuildNode( } } - - - static unsafe void BinnedBuilderInternal(Buffer encodedLeafIndices, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, Buffer leaves, IThreadDispatcher dispatcher, TaskQueue* taskQueuePointer, int workerCount, BufferPool pool, int minimumBinCount, int maximumBinCount, float leafToBinMultiplier, int microsweepThreshold) { @@ -1070,7 +1101,7 @@ static unsafe void BinnedBuilderInternal(Buffer encodedLeafIndices, Buffer< int binAllocationStart = 0; for (int i = 0; i < workerCount; ++i) { - workerContexts[i] = new BinnedBuildWorkerContext(boundingBoxes.As(), workerBinsAllocation, ref binAllocationStart, maximumBinCount, leafCounts, workerCount); + workerContexts[i] = new BinnedBuildWorkerContext(boundingBoxes.As(), workerBinsAllocation, ref binAllocationStart, maximumBinCount, leafCounts); } TaskQueue taskQueue = default; diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index 3a99617dd..656d4d163 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -158,8 +158,8 @@ public override void Initialize(ContentArchive content, Camera camera) Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(4, 1)); //Create a mesh. - var width = 32;// 768; - var height = 16;// 768; + var width = 8;// 768; + var height = 8;// 768; var scale = new Vector3(1, 1, 1); //DemoMeshHelper.CreateDeformedPlane(width, height, (x, y) => new Vector3(x - width * scale.X * 0.5f, 2f * (float)(Math.Sin(x * 0.5f) * Math.Sin(y * 0.5f)), y - height * scale.Y * 0.5f), scale, BufferPool, out var mesh); //DemoMeshHelper.CreateDeformedPlane(width, height, (x, y) => new Vector3(x - width * scale.X * 0.5f, 0, y - height * scale.Y * 0.5f), scale, BufferPool, out var mesh); From dcad4db00fa139b83f9d06675a663c40f25fbbc6 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 5 Dec 2022 17:54:01 -0600 Subject: [PATCH 670/947] Couple of doofbugs in merge. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index fe668d1a6..b360573ba 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -780,8 +780,8 @@ unsafe static void MultithreadedBinSubtrees(MultithreadBinnedBuildC ref var b0 = ref worker.BinBoundingBoxes[binIndex]; ref var bi = ref cache.BinBoundingBoxes[binIndex]; b0.Min = Vector4.Min(b0.Min, bi.Min); - b0.Max = Vector4.Min(b0.Max, bi.Max); - cache0.BinLeafCounts[binIndex] += cache.BinLeafCounts[binIndex]; + b0.Max = Vector4.Max(b0.Max, bi.Max); + worker.BinLeafCounts[binIndex] += cache.BinLeafCounts[binIndex]; } } } From 2a8361fe919fea4f67ebd6249211aa7a88dcda6f Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 6 Dec 2022 20:38:59 -0600 Subject: [PATCH 671/947] Task demo slightly enfancied. --- Demos/SpecializedTests/TaskQueueTestDemo.cs | 46 +++++++++++++++++++-- 1 file changed, 42 insertions(+), 4 deletions(-) diff --git a/Demos/SpecializedTests/TaskQueueTestDemo.cs b/Demos/SpecializedTests/TaskQueueTestDemo.cs index df997d804..50ff71537 100644 --- a/Demos/SpecializedTests/TaskQueueTestDemo.cs +++ b/Demos/SpecializedTests/TaskQueueTestDemo.cs @@ -8,6 +8,7 @@ using BepuPhysics.Constraints; using System.Threading; using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; namespace Demos.SpecializedTests; @@ -21,10 +22,38 @@ static int DoSomeWork(int iterations, int sum) } return sum; } - static void DynamicallyEnqueuedTest(long taskId, void* context, int workerIndex, IThreadDispatcher dispatcher) + + //Try different context layouts to make sure the task queue isn't mixing and matching tasks somehow. + [StructLayout(LayoutKind.Explicit)] + struct DynamicContext1 + { + [FieldOffset(0)] + public long Pad; + [FieldOffset(8)] + public Context* Context; + } + + static void DynamicallyEnqueuedTest1(long taskId, void* context, int workerIndex, IThreadDispatcher dispatcher) + { + var sum = DoSomeWork(10000, 0); + Interlocked.Add(ref ((DynamicContext1*)context)->Context->Sum, sum); + } + + [StructLayout(LayoutKind.Explicit)] + struct DynamicContext2 + { + [FieldOffset(0)] + public long Pad1; + [FieldOffset(8)] + public long Pad2; + [FieldOffset(16)] + public Context* Context; + } + + static void DynamicallyEnqueuedTest2(long taskId, void* context, int workerIndex, IThreadDispatcher dispatcher) { var sum = DoSomeWork(10000, 0); - Interlocked.Add(ref ((Context*)context)->Sum, sum); + Interlocked.Add(ref ((DynamicContext2*)context)->Context->Sum, sum); } static void Test(long taskId, void* context, int workerIndex, IThreadDispatcher dispatcher) { @@ -33,7 +62,10 @@ static void Test(long taskId, void* context, int workerIndex, IThreadDispatcher if ((taskId & 7) == 0) { const int subtaskCount = 8; - typedContext->Queue->For(&DynamicallyEnqueuedTest, context, 0, subtaskCount, workerIndex, dispatcher); + var context1 = new DynamicContext1 { Context = typedContext }; + typedContext->Queue->For(&DynamicallyEnqueuedTest1, &context1, 0, subtaskCount, workerIndex, dispatcher); + var context2 = new DynamicContext2 { Context = typedContext }; + typedContext->Queue->For(&DynamicallyEnqueuedTest2, &context2, 0, subtaskCount, workerIndex, dispatcher); } Interlocked.Add(ref typedContext->Sum, sum); } @@ -44,9 +76,15 @@ static void STTest(long taskId, void* context, int workerIndex, IThreadDispatche if ((taskId & 7) == 0) { const int subtaskCount = 8; + var context1 = new DynamicContext1 { Context = typedContext }; + for (int i = 0; i < subtaskCount; ++i) + { + DynamicallyEnqueuedTest1(i, &context1, workerIndex, dispatcher); + } + var context2 = new DynamicContext2 { Context = typedContext }; for (int i = 0; i < subtaskCount; ++i) { - DynamicallyEnqueuedTest(i, context, workerIndex, dispatcher); + DynamicallyEnqueuedTest2(i, &context2, workerIndex, dispatcher); } } Interlocked.Add(ref typedContext->Sum, sum); From a1f14fc6769b4a41ab49e63e1fa8313c9faa85b3 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 7 Dec 2022 21:06:18 -0600 Subject: [PATCH 672/947] Fixed some severe TaskQueue bugs. --- BepuUtilities/TaskQueue.cs | 15 +++++++++------ Demos/Demo.cs | 2 +- Demos/SpecializedTests/TaskQueueTestDemo.cs | 4 +++- 3 files changed, 13 insertions(+), 8 deletions(-) diff --git a/BepuUtilities/TaskQueue.cs b/BepuUtilities/TaskQueue.cs index 5a75574a6..d756c074e 100644 --- a/BepuUtilities/TaskQueue.cs +++ b/BepuUtilities/TaskQueue.cs @@ -67,6 +67,7 @@ public Task(delegate* function, void* /// Dispatcher running this task. public void Run(int workerIndex, IThreadDispatcher dispatcher) { + Debug.Assert(!Continuation.Completed); Function(Id, Context, workerIndex, dispatcher); if (Continuation.Initialized) Continuation.NotifyTaskCompleted(workerIndex, dispatcher); @@ -249,7 +250,7 @@ internal unsafe struct TaskQueueContinuations public Buffer Continuations; public IdPool IndexPool; public int ContinuationCount; - public int Locker; + public volatile int Locker; /// /// Retrieves a pointer to the continuation data for . @@ -441,12 +442,13 @@ public AllocateTaskContinuationResult TryAllocateContinuation(int taskCount, out { //We have the lock. Debug.Assert(continuations.ContinuationCount <= continuations.Continuations.length); - if (continuations.ContinuationCount >= continuations.Continuations.length) + if (continuations.ContinuationCount == continuations.Continuations.length) { //No room. return AllocateTaskContinuationResult.Full; } var index = continuations.IndexPool.Take(); + ++continuations.ContinuationCount; ref var continuation = ref continuations.Continuations[index]; var newVersion = continuation.Version + 1; continuation.OnCompleted = onCompleted; @@ -559,11 +561,12 @@ EnqueueTaskResult TryEnqueueUnsafelyInternal(Span tasks, out long taskEndI paddedCounters.AllocatedTaskIndex = taskEndIndex; Debug.Assert(BitOperations.IsPow2(this.tasks.Length)); var wrappedInclusiveStartIndex = (int)(taskStartIndex & taskMask); - var wrappedInclusiveEndIndex = (int)(taskEndIndex & taskMask); - if (wrappedInclusiveEndIndex > wrappedInclusiveStartIndex) + //Note - 1; the taskEndIndex is exclusive. Working with the inclusive index means we don't have to worry about an end index of tasks.Length being wrapped to 0. + var wrappedInclusiveEndIndex = (int)((taskEndIndex - 1) & taskMask); + if (wrappedInclusiveEndIndex >= wrappedInclusiveStartIndex) { //We can just copy the whole task block as one blob. - Unsafe.CopyBlockUnaligned(ref *(byte*)(this.tasks.Memory + taskStartIndex), ref Unsafe.As(ref MemoryMarshal.GetReference(tasks)), (uint)(Unsafe.SizeOf() * tasks.Length)); + Unsafe.CopyBlockUnaligned(ref *(byte*)(this.tasks.Memory + wrappedInclusiveStartIndex), ref Unsafe.As(ref MemoryMarshal.GetReference(tasks)), (uint)(Unsafe.SizeOf() * tasks.Length)); } else { @@ -572,7 +575,7 @@ EnqueueTaskResult TryEnqueueUnsafelyInternal(Span tasks, out long taskEndI var firstRegionCount = this.tasks.length - wrappedInclusiveStartIndex; ref var secondBlobStartTask = ref tasks[firstRegionCount]; var secondRegionCount = tasks.Length - firstRegionCount; - Unsafe.CopyBlockUnaligned(ref *(byte*)(this.tasks.Memory + taskStartIndex), ref Unsafe.As(ref startTask), (uint)(Unsafe.SizeOf() * firstRegionCount)); + Unsafe.CopyBlockUnaligned(ref *(byte*)(this.tasks.Memory + wrappedInclusiveStartIndex), ref Unsafe.As(ref startTask), (uint)(Unsafe.SizeOf() * firstRegionCount)); Unsafe.CopyBlockUnaligned(ref *(byte*)this.tasks.Memory, ref Unsafe.As(ref secondBlobStartTask), (uint)(Unsafe.SizeOf() * secondRegionCount)); } //for (int i = 0; i < tasks.Length; ++i) diff --git a/Demos/Demo.cs b/Demos/Demo.cs index 264961f51..c65faca66 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -43,7 +43,7 @@ protected Demo() //there won't be enough memory bandwidth to even feed half the physical cores. Using all 128 logical cores would just add overhead. //It may be worth using something like hwloc or CPUID to extract extra information to reason about. - var targetThreadCount = 2;// int.Max(1, Environment.ProcessorCount > 4 ? Environment.ProcessorCount - 2 : Environment.ProcessorCount - 1); + var targetThreadCount = int.Max(1, Environment.ProcessorCount > 4 ? Environment.ProcessorCount - 2 : Environment.ProcessorCount - 1); ThreadDispatcher = new ThreadDispatcher(targetThreadCount); } diff --git a/Demos/SpecializedTests/TaskQueueTestDemo.cs b/Demos/SpecializedTests/TaskQueueTestDemo.cs index 50ff71537..062e8693e 100644 --- a/Demos/SpecializedTests/TaskQueueTestDemo.cs +++ b/Demos/SpecializedTests/TaskQueueTestDemo.cs @@ -9,6 +9,7 @@ using System.Threading; using System.Runtime.CompilerServices; using System.Runtime.InteropServices; +using System.Reflection.Metadata; namespace Demos.SpecializedTests; @@ -123,7 +124,6 @@ public override void Initialize(ContentArchive content, Camera camera) Console.WriteLine($"Task size: {Unsafe.SizeOf()}"); - int iterationCount = 4; int tasksPerIteration = 64; var taskQueue = new TaskQueue(BufferPool); @@ -141,7 +141,9 @@ public override void Initialize(ContentArchive content, Camera camera) var context = new Context { Queue = taskQueuePointer }; var continuation = taskQueuePointer->AllocateContinuation(iterationCount * tasksPerIteration, 0, ThreadDispatcher, new Task(&IssueStop, &context)); for (int i = 0; i < iterationCount; ++i) + { taskQueuePointer->TryEnqueueForUnsafely(&Test, &context, i * tasksPerIteration, tasksPerIteration, continuation); + } //taskQueuePointer->TryEnqueueStopUnsafely(); //taskQueuePointer->EnqueueTasks() ThreadDispatcher.DispatchWorkers(&DispatcherBody, unmanagedContext: taskQueuePointer); From c23468b5f73e83f634955a9b12288586e3fca041 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 9 Dec 2022 19:56:58 -0600 Subject: [PATCH 673/947] ParallelTaskStack, for comparison with TaskQueue. TaskQueue still has some bugs when the task count is limited. ParallelTaskStack is very naive at the moment, but it does work, so that's something. --- BepuUtilities/ParallelTaskStack.cs | 860 ++++++++++++++++++++ BepuUtilities/TaskQueue.cs | 4 +- Demos/SpecializedTests/TaskQueueTestDemo.cs | 99 ++- 3 files changed, 935 insertions(+), 28 deletions(-) create mode 100644 BepuUtilities/ParallelTaskStack.cs diff --git a/BepuUtilities/ParallelTaskStack.cs b/BepuUtilities/ParallelTaskStack.cs new file mode 100644 index 000000000..ce5649b27 --- /dev/null +++ b/BepuUtilities/ParallelTaskStack.cs @@ -0,0 +1,860 @@ +using System; +using System.Diagnostics; +using System.Diagnostics.CodeAnalysis; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; +using System.Threading; +using BepuUtilities.Collections; +using BepuUtilities.Memory; + +namespace BepuUtilities.TestStack; + +/// +/// Description of a task to be submitted to a . +/// +public unsafe struct Task +{ + /// + /// Function to be executed by the task. Takes as arguments the , pointer, and executing worker index. + /// + public delegate* Function; + /// + /// Context to be passed into the . + /// + public void* Context; + /// + /// Continuation to be notified after this task completes, if any. + /// + public ContinuationHandle Continuation; + /// + /// User-provided identifier of this task. + /// + public long Id; + + /// + /// Creates a new task. + /// + /// Function to be executed by the task. Takes as arguments the , pointer, executing worker index, and executing . + /// Context pointer to pass to the . + /// Id of this task to be passed into the . + /// Continuation to notify after the completion of this task, if any. + public Task(delegate* function, void* context = null, long taskId = 0, ContinuationHandle continuation = default) + { + Function = function; + Context = context; + Continuation = continuation; + Id = taskId; + } + + /// + /// Creates a task from a function. + /// + /// Function to turn into a task. + public static implicit operator Task(delegate* function) => new(function); + + /// + /// Runs the task and, if necessary, notifies the associated continuation of its completion. + /// + /// Worker index to pass to the function. + /// Dispatcher running this task. + public void Run(int workerIndex, IThreadDispatcher dispatcher) + { + Debug.Assert(!Continuation.Completed); + Function(Id, Context, workerIndex, dispatcher); + if (Continuation.Initialized) + Continuation.NotifyTaskCompleted(workerIndex, dispatcher); + } +} + +/// +/// Describes the result status of a pop attempt. +/// +public enum PopTaskResult +{ + /// + /// A task was successfully popped. + /// + Success, + /// + /// The stack was empty, but may have more tasks in the future. + /// + Empty, + /// + /// The stack has been terminated and all threads seeking work should stop. + /// + Stop +} + +/// +/// Refers to a continuation within a . +/// +public unsafe struct ContinuationHandle : IEquatable +{ + uint index; + uint encodedVersion; + /// + /// Source worker task stack. + /// + internal WorkerTaskStack* WorkerStack; + + internal ContinuationHandle(uint index, int version, WorkerTaskStack* workerStack) + { + this.index = index; + encodedVersion = (uint)version | 1u << 31; + WorkerStack = workerStack; + } + + internal uint Index + { + get + { + Debug.Assert(Initialized, "If you're trying to pull a continuation id from a continuation handle, it should have been initialized."); + return index; + } + } + + internal int Version + { + get + { + Debug.Assert(Initialized, "If you're trying to pull a continuation id from a continuation handle, it should have been initialized."); + return (int)(encodedVersion & ((1u << 31) - 1)); + } + } + + /// + /// Gets whether the tasks associated with this continuation have completed. + /// + public bool Completed => WorkerStack->IsComplete(this); + + /// + /// Retrieves a pointer to the continuation data for . + /// + /// Pointer to the continuation backing the given handle. + /// This should not be used if the continuation handle is not known to be valid. The data pointed to by the data could become invalidated if the continuation completes. + public TaskContinuation* Continuation => WorkerStack->GetContinuation(this); + + /// + /// Gets a null continuation handle. + /// + public static ContinuationHandle Null => default; + + /// + /// Gets whether the continuation associated with this handle was allocated and has outstanding tasks. + /// + public bool Exists + { + get + { + if (!Initialized || WorkerStack == null) + return false; + ref var continuation = ref WorkerStack->Continuations[Index]; + return continuation.Version == Version && continuation.RemainingTaskCounter > 0; + } + } + + /// + /// Gets whether this handle ever represented an allocated handle. This does not guarantee that the continuation's associated tasks are active in the that it was allocated from. + /// + public bool Initialized => encodedVersion >= 1u << 31; + + /// + /// Notifies the continuation that one task was completed. + /// + /// Worker index to pass to the continuation's delegate, if any. + /// Dispatcher to pass to the continuation's delegate, if any. + public void NotifyTaskCompleted(int workerIndex, IThreadDispatcher dispatcher) + { + var continuation = Continuation; + Debug.Assert(!Completed); + var counter = Interlocked.Decrement(ref continuation->RemainingTaskCounter); + Debug.Assert(counter >= 0, "The counter should not go negative. Was notify called too many times?"); + if (counter == 0) + { + //This entire job has completed. + if (continuation->OnCompleted.Function != null) + { + continuation->OnCompleted.Function(continuation->OnCompleted.Id, continuation->OnCompleted.Context, workerIndex, dispatcher); + } + //Free this continuation slot. + var waiter = new SpinWait(); + while (Interlocked.CompareExchange(ref WorkerStack->ContinuationLocker, 1, 0) != 0) + { + waiter.SpinOnce(-1); + } + //We have the lock. + WorkerStack->ContinuationIndexPool.ReturnUnsafely((int)Index); + --WorkerStack->ContinuationCount; + WorkerStack->ContinuationLocker = 0; + } + } + + public bool Equals(ContinuationHandle other) => other.index == index && other.encodedVersion == encodedVersion && other.WorkerStack == WorkerStack; + + public override bool Equals([NotNullWhen(true)] object obj) => obj is ContinuationHandle handle && Equals(handle); + + public override int GetHashCode() => (int)(index ^ (encodedVersion << 24)); + + public static bool operator ==(ContinuationHandle left, ContinuationHandle right) => left.Equals(right); + + public static bool operator !=(ContinuationHandle left, ContinuationHandle right) => !(left == right); +} + +internal unsafe struct WorkerTaskStack +{ + public QuickList Tasks; + public Buffer Continuations; + public IdPool ContinuationIndexPool; + public int ContinuationCount; + public int WorkerIndex; + public volatile int TaskLocker; + public volatile int ContinuationLocker; + + public WorkerTaskStack(int workerIndex, IThreadDispatcher dispatcher, int initialTaskCapacity = 64, int initialContinuationCapacity = 16) + { + var threadPool = dispatcher.WorkerPools[workerIndex]; + WorkerIndex = workerIndex; + Tasks = new QuickList(initialTaskCapacity, threadPool); + threadPool.Take(initialContinuationCapacity, out Continuations); +#if DEBUG + //While you shouldn't *need* to clear continuations, it can be useful for debug purposes. + Continuations.Clear(0, Continuations.length); + Tasks.Span.Clear(0, Tasks.Span.length); +#endif + ContinuationIndexPool = new IdPool(initialContinuationCapacity, threadPool); + } + + public void Dispose(BufferPool threadPool) + { + Tasks.Dispose(threadPool); + threadPool.Return(ref Continuations); + ContinuationIndexPool.Dispose(threadPool); + } + + /// + /// Attempts to pop a task from the stack. + /// + /// Task popped from the stack, if any. + /// True if a task was available to pop, false otherwise. + internal bool TryPop(out Task task) + { + //Quick early out to avoid lock expense. + if (Tasks.Count == 0) + { + task = default; + return false; + } + var waiter = new SpinWait(); + //Unlike the pushes, we only return false if there is no task. Just simplifies things a bit. Don't want to have 'try' with two different meanings. + //(There's some argument for there not being *any* try-because-contested functions. Bit of a holdover from how things used to work.) + while (true) + { + if (Interlocked.CompareExchange(ref TaskLocker, 1, 0) != 0) + { + waiter.SpinOnce(); + continue; + } + try + { + //We have the lock. + return Tasks.TryPop(out task); + } + finally + { + TaskLocker = 0; + } + } + } + + /// + /// Pushes a set of tasks onto the stack. Does not acquire a lock. + /// + /// Tasks composing the job. + /// Dispatcher used to pull thread allocations if necessary. + /// If the worker associated with this stack might be active, this function can only be called by the worker. + internal void PushUnsafely(Span tasks, IThreadDispatcher dispatcher) + { + Debug.Assert(tasks.Length > 0, "Probably shouldn't be trying to push zero tasks."); + var newTaskCount = Tasks.Count + tasks.Length; + + if (Tasks.Span.length < newTaskCount) + { + //Doing a resize within a lock is *really* not great. This is something to be avoided via preallocation whenever possible. + //BUT: + //1. It should be trivial to make these resizes effectively never happen. + //2. In the cases where it happens anyway, suffering the pain of a resize in a lock is the lesser of two evils. + //In the case where we *didn't* resize, we'd have to either stall on the allocation attempt or cycle on other tasks until continuations are freed up. + //But there's no guarantee that running tasks will actually free up continuations on net- they could make more! + //So having the fallback plan of expanding storage for more continuations avoids deadlock prone options. + //Note that *ONLY THE OWNING WORKER* can ever validly perform this resize! + //We have to use the thread's buffer pool, and only the thread can validly access that pool. + Tasks.Resize(int.Max(newTaskCount, Tasks.Span.length * 2), dispatcher.WorkerPools[WorkerIndex]); +#if DEBUG + //While you shouldn't *need* to clear tasks, it can be useful for debug purposes. + Tasks.Span.Clear(Tasks.Count, Tasks.Span.length - Tasks.Count); +#endif + } + + Tasks.AddRangeUnsafely(tasks); + } + /// + /// Pushes a task onto the task stack. Does not acquire a lock. + /// + /// Task to push + /// Dispatcher used to pull thread allocations if necessary. + /// This must not be used while other threads could be performing task pushes or pops. + public void PushUnsafely(Task task, IThreadDispatcher dispatcher) + { + PushUnsafely(new Span(&task, 1), dispatcher); + } + + /// + /// Tries to push a set of tasks to the task stack if the stack is uncontested. + /// + /// Tasks composing the job. + /// Dispatcher used to pull thread allocations if necessary. + /// True if the push succeeded, false if the push was contested. + /// This must only be called from the associated worker if the worker is potentially active. + public bool TryPush(Span tasks, IThreadDispatcher dispatcher) + { + if (Interlocked.CompareExchange(ref TaskLocker, 1, 0) != 0) + return false; + try + { + //We have the lock. + PushUnsafely(tasks, dispatcher); + return true; + } + finally + { + TaskLocker = 0; + } + } + + /// + /// Push a set of tasks to the task stack. + /// + /// Tasks composing the job. + /// Dispatcher used to pull thread allocations if necessary. + /// This must only be called from the associated worker if the worker is potentially active. + public void Push(Span tasks, IThreadDispatcher dispatcher) + { + var waiter = new SpinWait(); + while (!TryPush(tasks, dispatcher)) + { + waiter.SpinOnce(-1); + } + } + + + /// + /// Attempts to allocate a continuation for a set of tasks. + /// + /// Number of tasks associated with the continuation. + /// Dispatcher from which to pull a buffer pool if needed for resizing. + /// Function to execute upon completing all associated tasks, if any. Any task with a null will not be executed. + /// Handle of the continuation if allocation is successful. + /// True if the continuation was allocated, false if the attempt was contested.. + public bool TryAllocateContinuation(int taskCount, IThreadDispatcher dispatcher, out ContinuationHandle continuationHandle, Task onCompleted = default) + { + continuationHandle = default; + if (Interlocked.CompareExchange(ref ContinuationLocker, 1, 0) != 0) + return false; + try + { + //We have the lock. + if (ContinuationCount == Continuations.length) + { + //Doing a resize within a lock is *really* not great. This is something to be avoided via preallocation whenever possible. + //BUT: + //1. It should be trivial to make these resizes effectively never happen. + //2. In the cases where it happens anyway, suffering the pain of a resize in a lock is the lesser of two evils. + //In the case where we *didn't* resize, we'd have to either stall on the allocation attempt or cycle on other tasks until continuations are freed up. + //But there's no guarantee that running tasks will actually free up continuations on net- they could make more! + //So having the fallback plan of expanding storage for more continuations avoids deadlock prone options. + //Note that *ONLY THE OWNING WORKER* can ever validly perform this resize! + //We have to use the thread's buffer pool, and only the thread can validly access that pool. + var workerPool = dispatcher.WorkerPools[WorkerIndex]; + workerPool.ResizeToAtLeast(ref Continuations, ContinuationCount * 2, ContinuationCount); + ContinuationIndexPool.Resize(Continuations.length, workerPool); +#if DEBUG + //While you shouldn't *need* to clear continuations, it can be useful for debug purposes. + Continuations.Clear(ContinuationCount, Continuations.length - ContinuationCount); +#endif + } + var index = ContinuationIndexPool.Take(); + ++ContinuationCount; + ref var continuation = ref Continuations[index]; + //Note that the version number could be based on undefined data initially. That's actually fine; all we care about is whether it is different. + //Note mask to leave a valid bit for encoding in the handle. + var newVersion = (continuation.Version + 1) & (~(1 << 31)); + continuation.OnCompleted = onCompleted; + continuation.Version = newVersion; + continuation.RemainingTaskCounter = taskCount; + continuationHandle = new ContinuationHandle((uint)index, newVersion, (WorkerTaskStack*)Unsafe.AsPointer(ref this)); + return true; + } + finally + { + ContinuationLocker = 0; + } + } + + + /// + /// Allocates a continuation for a set of tasks. + /// + /// Number of tasks associated with the continuation. + /// Dispatcher from which to pull any thread allocations if necessary. + /// Task to execute upon completing all associated tasks, if any. Any task with a null will not be executed. + /// Handle of the allocated continuation. + /// Note that this will keep trying until allocation succeeds. If something is blocking allocation, such as insufficient room in the continuations buffer and there are no workers consuming tasks, this will block forever. + public ContinuationHandle AllocateContinuation(int taskCount, IThreadDispatcher dispatcher, Task onCompleted = default) + { + var waiter = new SpinWait(); + ContinuationHandle handle; + while (!TryAllocateContinuation(taskCount, dispatcher, out handle, onCompleted)) + { + waiter.SpinOnce(-1); + } + return handle; + } + + /// + /// Retrieves a pointer to the continuation data for . + /// + /// Handle to look up the associated continuation for. + /// Pointer to the continuation backing the given handle. + /// This should not be used if the continuation handle is not known to be valid. The data pointed to by the data could become invalidated if the continuation completes. + public TaskContinuation* GetContinuation(ContinuationHandle continuationHandle) + { + Debug.Assert(continuationHandle.Initialized, "This continuation handle was never initialized."); + Debug.Assert(continuationHandle.Index < Continuations.length, "This continuation refers to an invalid index."); + if (continuationHandle.Index >= Continuations.length || !continuationHandle.Initialized) + return null; + var continuation = Continuations.Memory + continuationHandle.Index; + Debug.Assert(continuation->Version == continuationHandle.Version, "This continuation no longer refers to an active continuation."); + if (continuation->Version != continuationHandle.Version) + return null; + return Continuations.Memory + continuationHandle.Index; + } + + /// + /// Checks whether all tasks associated with this continuation have completed. + /// + /// Continuation to check for completion. + /// True if all tasks associated with a continuation have completed, false otherwise. + public bool IsComplete(ContinuationHandle continuationHandle) + { + Debug.Assert(continuationHandle.Initialized, "This continuation handle was never initialized."); + Debug.Assert(continuationHandle.Index < Continuations.length, "This continuation refers to an invalid index."); + if (continuationHandle.Index >= Continuations.length || !continuationHandle.Initialized) + return false; + ref var continuation = ref Continuations[continuationHandle.Index]; + return continuation.Version > continuationHandle.Version || continuation.RemainingTaskCounter == 0; + } + + internal void Reset() + { + Tasks.Count = 0; + Debug.Assert(TaskLocker == 0, "There appears to be a thread actively working still. That's invalid."); +#if DEBUG + //While you shouldn't *need* to clear continuations, it can be useful for debug purposes. + Continuations.Clear(0, Continuations.length); +#endif + ContinuationCount = 0; + Debug.Assert(ContinuationLocker == 0, "There appears to be a thread actively working still. That's invalid."); + } +} + +/// +/// Stores data relevant to tracking task completion and reporting completion for a continuation. +/// +public unsafe struct TaskContinuation +{ + /// + /// Task to run upon completion of the associated task. + /// + public Task OnCompleted; + /// + /// Version of this continuation. + /// + public int Version; + /// + /// Number of tasks not yet reported as complete in the continuation. + /// + public int RemainingTaskCounter; +} + + +/// +/// Manages task stacks for parallel workers. Supports work stealing between workers. +/// +public unsafe struct ParallelTaskStack +{ + Buffer workerStacks; + + [StructLayout(LayoutKind.Explicit, Size = 256 + 16)] + struct StopPad + { + [FieldOffset(128)] + public volatile bool Stop; + } + StopPad padded; + + /// + /// Constructs a new parallel task stack. + /// + /// Buffer pool to allocate non-thread allocated resources from. + /// Thread dispatcher to pull thread pools from for thread allocations. + /// Number of workers to allocate space for. + /// Initial number of tasks to allocate space for in each worker. Tasks are individual chunks of scheduled work.. + /// Initial number of continuations to allocate space for in each worker. + public ParallelTaskStack(BufferPool pool, IThreadDispatcher dispatcher, int workerCount, int initialWorkerTaskCapacity = 64, int initialWorkerContinuationCapacity = 16) + { + pool.Take(workerCount, out workerStacks); + for (int i = 0; i < workerCount; ++i) + { + workerStacks[i] = new WorkerTaskStack(i, dispatcher, initialWorkerTaskCapacity, initialWorkerContinuationCapacity); + } + Reset(); + } + + /// + /// Returns the stack to a fresh state without reallocating. + /// + public void Reset() + { + for (int i = 0; i < workerStacks.Length; ++i) + { + workerStacks[i].Reset(); + } + padded.Stop = false; + } + + /// + /// Returns unmanaged resources held by the to a pool. + /// + /// Buffer pool to return resources to. + public void Dispose(BufferPool pool) + { + for (int i = 0; i < workerStacks.Length; ++i) + { + workerStacks[i].Reset(); + } + pool.Return(ref workerStacks); + } + + /// + /// Gets the approximate number of active tasks. This is not guaranteed to actually measure the true number of tasks at any one point in time; it checks each worker in sequence, and the task counts could vary arbitrarily as the checks proceed. + /// + public int ApproximateTaskCount + { + get + { + int sum = 0; + for (int i = 0; i < workerStacks.Length; ++i) + { + sum += workerStacks[i].Tasks.Count; + } + return sum; + } + } + /// + /// Gets the approximate number of active continuations. This is not guaranteed to actually measure the true number of continuations at any one point in time; it checks each worker in sequence, and the continuation counts could vary arbitrarily as the checks proceed. + /// + public int ApproximateContinuationCount + { + get + { + int sum = 0; + for (int i = 0; i < workerStacks.Length; ++i) + { + sum += workerStacks[i].ContinuationCount; + } + return sum; + } + } + + + /// + /// Attempts to allocate a continuation for a set of tasks. + /// + /// Number of tasks associated with the continuation. + /// Worker index to allocate the continuation on. + /// Dispatcher to use for any per-thread allocations if necessary. + /// Function to execute upon completing all associated tasks, if any. Any task with a null will not be executed. + /// Handle of the continuation if allocation is successful. + /// True if the allocation succeeded, false if it was contested.. + public bool TryAllocateContinuation(int taskCount, int workerIndex, IThreadDispatcher dispatcher, out ContinuationHandle continuationHandle, Task onCompleted = default) + { + return workerStacks[workerIndex].TryAllocateContinuation(taskCount, dispatcher, out continuationHandle, onCompleted); + } + + /// + /// Allocates a continuation for a set of tasks. + /// + /// Number of tasks associated with the continuation. + /// Worker index to allocate the continuation on. + /// Dispatcher to use for any per-thread allocations if necessary. + /// Task to execute upon completing all associated tasks, if any. Any task with a null will not be executed. + /// Handle of the allocated continuation. + public ContinuationHandle AllocateContinuation(int taskCount, int workerIndex, IThreadDispatcher dispatcher, Task onCompleted = default) + { + return workerStacks[workerIndex].AllocateContinuation(taskCount, dispatcher, onCompleted); + } + + /// + /// Attempts to pop a task. + /// + /// Index of the worker executing this function. + /// Popped task, if any. + /// Result status of the pop attempt. + public PopTaskResult TryPop(int workerIndex, out Task task) + { + //Walk through the worker stacks looking for work. Start with the current worker. + int peekedWorkerIndex = workerIndex; + for (int i = 0; i < workerStacks.length; ++i) + { + if (workerStacks[peekedWorkerIndex].TryPop(out task)) + { + return PopTaskResult.Success; + } + ++peekedWorkerIndex; + if (peekedWorkerIndex >= workerStacks.length) + peekedWorkerIndex -= workerStacks.length; + } + //No worker had anything! + task = default; + return padded.Stop ? PopTaskResult.Stop : PopTaskResult.Empty; + } + + /// + /// Attempts to pop a task and run it. + /// + /// Index of the worker to pass into the task function. + /// Thread dispatcher running this task stack. + /// Result status of the pop attempt. + public PopTaskResult TryPopAndRun(int workerIndex, IThreadDispatcher dispatcher) + { + var result = TryPop(workerIndex, out var task); + if (result == PopTaskResult.Success) + { + task.Run(workerIndex, dispatcher); + } + return result; + } + + /// + /// Pushes a set of tasks onto a worker's task stack. Does not acquire a lock. + /// + /// Tasks composing the job. + /// Thread dispatcher to allocate thread data from if necessary. + /// Index of the worker stack to push the tasks onto. + /// This must not be used while other threads could be performing task pushes or pops that could affect the specified worker. + public void PushUnsafely(Span tasks, int workerIndex, IThreadDispatcher dispatcher) + { + workerStacks[workerIndex].PushUnsafely(tasks, dispatcher); + } + /// + /// Pushes a task onto a worker's task stack. Does not acquire a lock. + /// + /// Task to push. + /// Thread dispatcher to allocate thread data from if necessary. + /// Index of the worker stack to push the tasks onto. + /// This must not be used while other threads could be performing task pushes or pops that could affect the specified worker. + public unsafe void PushUnsafely(Task task, int workerIndex, IThreadDispatcher dispatcher) + { + workerStacks[workerIndex].PushUnsafely(new Span(&task, 1), dispatcher); + } + + /// + /// Tries to pushes a set of tasks onto a worker's task stack. + /// + /// Tasks composing the job. + /// Thread dispatcher to allocate thread data from if necessary. + /// Index of the worker stack to push the tasks onto. + /// True if the push succeeded, false if it was contested. + public bool TryPush(Span tasks, int workerIndex, IThreadDispatcher dispatcher) + { + return workerStacks[workerIndex].TryPush(tasks, dispatcher); + } + + /// + /// Pushes a set of tasks onto a worker's task stack. + /// + /// Tasks composing the job. + /// Thread dispatcher to allocate thread data from if necessary. + /// Index of the worker stack to push the tasks onto. + public void Push(Span tasks, int workerIndex, IThreadDispatcher dispatcher) + { + workerStacks[workerIndex].Push(tasks, dispatcher); + } + + /// + /// Pushes a set of tasks to the stack with a created continuation. + /// + /// Tasks composing the job. A continuation will be assigned internally; no continuation should be present on any of the provided tasks. + /// Thread dispatcher to allocate thread data from if necessary. + /// Index of the worker stack to push the tasks onto. + /// Task to run upon completion of all the submitted tasks, if any. + /// Handle of the continuation created for these tasks. + /// Note that this will keep trying until task submission succeeds. + public ContinuationHandle AllocateContinuationAndPush(Span tasks, int workerIndex, IThreadDispatcher dispatcher, Task onComplete = default) + { + var continuationHandle = AllocateContinuation(tasks.Length, workerIndex, dispatcher, onComplete); + for (int i = 0; i < tasks.Length; ++i) + { + ref var task = ref tasks[i]; + Debug.Assert(!task.Continuation.Initialized, "This function creates a continuation for the tasks"); + task.Continuation = continuationHandle; + } + Push(tasks, workerIndex, dispatcher); + return continuationHandle; + } + + /// + /// Waits for a continuation to be completed. + /// + /// Instead of spinning the entire time, this may pop and execute pending tasks to fill the gap. + /// Continuation to wait on. + /// Thread dispatcher to allocate thread data from if necessary. + /// Index of the executing worker. + public void WaitForCompletion(ContinuationHandle continuation, int workerIndex, IThreadDispatcher dispatcher) + { + var waiter = new SpinWait(); + Debug.Assert(continuation.Initialized, "This codepath should only run if the continuation was allocated earlier."); + while (!continuation.Completed) + { + var result = TryPop(workerIndex, out var fillerTask); + if (result == PopTaskResult.Stop) + { + return; + } + if (result == PopTaskResult.Success) + { + fillerTask.Run(workerIndex, dispatcher); + waiter.Reset(); + } + else + { + waiter.SpinOnce(-1); + } + } + } + + /// + /// Pushes a set of tasks to the worker stack and returns when all tasks are complete. + /// + /// Tasks composing the job. A continuation will be assigned internally; no continuation should be present on any of the provided tasks. + /// Index of the worker executing this function. + /// Thread dispatcher to allocate thread data from if necessary. + /// Note that this will keep working until all tasks are run. It may execute tasks unrelated to the requested tasks while waiting on other workers to complete constituent tasks. + public void RunTasks(Span tasks, int workerIndex, IThreadDispatcher dispatcher) + { + if (tasks.Length == 0) + return; + ContinuationHandle continuationHandle = default; + if (tasks.Length > 1) + { + //Note that we only submit tasks to the stack for tasks beyond the first. The current thread is responsible for at least task 0. + var taskCount = tasks.Length - 1; + Span tasksToPush = stackalloc Task[taskCount]; + ref var worker = ref workerStacks[workerIndex]; + continuationHandle = worker.AllocateContinuation(taskCount, dispatcher); + for (int i = 0; i < tasksToPush.Length; ++i) + { + var task = tasks[i + 1]; + Debug.Assert(continuationHandle.WorkerStack == workerStacks.Memory + workerIndex); + Debug.Assert(!task.Continuation.Initialized, $"None of the source tasks should have continuations when provided to {nameof(RunTasks)}."); + task.Continuation = continuationHandle; + tasksToPush[i] = task; + } + worker.Push(tasksToPush, dispatcher); + } + //Tasks [1, count) are submitted to the stack and may now be executing on other workers. + //The thread calling the for loop should not relinquish its timeslice. It should immediately begin working on task 0. + var task0 = tasks[0]; + Debug.Assert(!task0.Continuation.Initialized, $"None of the source tasks should have continuations when provided to {nameof(RunTasks)}."); + task0.Function(task0.Id, task0.Context, workerIndex, dispatcher); + + if (tasks.Length > 1) + { + //Task 0 is done; this thread should seek out other work until the job is complete. + WaitForCompletion(continuationHandle, workerIndex, dispatcher); + } + } + + /// + /// Requests that all workers stop. The next time a worker runs out of tasks to run, if it sees a stop command, it will be reported. + /// + public void RequestStop() + { + padded.Stop = true; + } + + + /// + /// Pushes a for loop onto the task stack. Does not take a lock. + /// + /// Function to execute on each iteration of the loop. + /// Context pointer to pass into each task execution. + /// Inclusive start index of the loop range. + /// Number of iterations to perform. + /// Thread dispatcher to allocate thread data from if necessary. + /// Index of the worker stack to push the tasks onto. + /// Continuation associated with the loop tasks, if any. + /// This must not be used while other threads could be performing task pushes or pops that could affect the specified worker. + public void PushForUnsafely(delegate* function, void* context, int inclusiveStartIndex, int iterationCount, int workerIndex, IThreadDispatcher dispatcher, ContinuationHandle continuation = default) + { + Span tasks = stackalloc Task[iterationCount]; + for (int i = 0; i < tasks.Length; ++i) + { + tasks[i] = new Task { Function = function, Context = context, Id = i + inclusiveStartIndex, Continuation = continuation }; + } + PushUnsafely(tasks, workerIndex, dispatcher); + } + + /// + /// Pushes a for loop onto the task stack. + /// + /// Function to execute on each iteration of the loop. + /// Context pointer to pass into each task execution. + /// Inclusive start index of the loop range. + /// Number of iterations to perform. + /// Thread dispatcher to allocate thread data from if necessary. + /// Index of the worker stack to push the tasks onto. + /// Continuation associated with the loop tasks, if any. + /// This function will not usually attempt to run any iterations of the loop itself. It tries to push the loop tasks onto the stack. + /// If the task stack is full, this will opt to run the tasks inline while waiting for room. + public void PushFor(delegate* function, void* context, int inclusiveStartIndex, int iterationCount, int workerIndex, IThreadDispatcher dispatcher, ContinuationHandle continuation = default) + { + Span tasks = stackalloc Task[iterationCount]; + for (int i = 0; i < tasks.Length; ++i) + { + tasks[i] = new Task { Function = function, Context = context, Id = i + inclusiveStartIndex, Continuation = continuation }; + } + Push(tasks, workerIndex, dispatcher); + } + + /// + /// Submits a set of tasks representing a for loop over the given indices and returns when all loop iterations are complete. + /// + /// Function to execute on each iteration of the loop. + /// Context pointer to pass into each iteration of the loop. + /// Inclusive start index of the loop range. + /// Number of iterations to perform. + /// Thread dispatcher to allocate thread data from if necessary. + /// Index of the worker stack to push the tasks onto. + public void For(delegate* function, void* context, int inclusiveStartIndex, int iterationCount, int workerIndex, IThreadDispatcher dispatcher) + { + if (iterationCount <= 0) + return; + Span tasks = stackalloc Task[iterationCount]; + for (int i = 0; i < tasks.Length; ++i) + { + tasks[i] = new Task(function, context, inclusiveStartIndex + i); + } + RunTasks(tasks, workerIndex, dispatcher); + } +} diff --git a/BepuUtilities/TaskQueue.cs b/BepuUtilities/TaskQueue.cs index d756c074e..4625cdae7 100644 --- a/BepuUtilities/TaskQueue.cs +++ b/BepuUtilities/TaskQueue.cs @@ -15,7 +15,7 @@ using System.Threading.Tasks; using BepuUtilities.Memory; -namespace BepuUtilities; +namespace BepuUtilities.TestQueue; /// /// Description of a task to be submitted to a . @@ -193,6 +193,7 @@ public bool Exists public void NotifyTaskCompleted(int workerIndex, IThreadDispatcher dispatcher) { var continuation = Continuation; + Debug.Assert(!Completed); var counter = Interlocked.Decrement(ref continuation->RemainingTaskCounter); Debug.Assert(counter >= 0, "The counter should not go negative. Was notify called too many times?"); if (counter == 0) @@ -527,6 +528,7 @@ public DequeueTaskResult TryDequeue(out Task task) continue; //There's an actual job! task = taskCandidate; + Debug.Assert(!task.Continuation.Completed); return DequeueTaskResult.Success; } } diff --git a/Demos/SpecializedTests/TaskQueueTestDemo.cs b/Demos/SpecializedTests/TaskQueueTestDemo.cs index 062e8693e..549303a34 100644 --- a/Demos/SpecializedTests/TaskQueueTestDemo.cs +++ b/Demos/SpecializedTests/TaskQueueTestDemo.cs @@ -10,9 +10,14 @@ using System.Runtime.CompilerServices; using System.Runtime.InteropServices; using System.Reflection.Metadata; +using BepuUtilities.TestQueue; +using BepuUtilities.TestStack; +using TaskQ = BepuUtilities.TestQueue.Task; +using TaskS = BepuUtilities.TestStack.Task; namespace Demos.SpecializedTests; + public unsafe class TaskQueueTestDemo : Demo { static int DoSomeWork(int iterations, int sum) @@ -56,17 +61,27 @@ static void DynamicallyEnqueuedTest2(long taskId, void* context, int workerIndex var sum = DoSomeWork(10000, 0); Interlocked.Add(ref ((DynamicContext2*)context)->Context->Sum, sum); } - static void Test(long taskId, void* context, int workerIndex, IThreadDispatcher dispatcher) + static void Test(long taskId, void* context, int workerIndex, IThreadDispatcher dispatcher) where T : unmanaged { var sum = DoSomeWork(100000, 0); var typedContext = (Context*)context; - if ((taskId & 7) == 0) + //if ((taskId & 7) == 0) { const int subtaskCount = 8; var context1 = new DynamicContext1 { Context = typedContext }; - typedContext->Queue->For(&DynamicallyEnqueuedTest1, &context1, 0, subtaskCount, workerIndex, dispatcher); var context2 = new DynamicContext2 { Context = typedContext }; - typedContext->Queue->For(&DynamicallyEnqueuedTest2, &context2, 0, subtaskCount, workerIndex, dispatcher); + if (typeof(T) == typeof(TaskQueue)) + { + var queue = (TaskQueue*)typedContext->TaskPile; + queue->For(&DynamicallyEnqueuedTest1, &context1, 0, subtaskCount, workerIndex, dispatcher); + queue->For(&DynamicallyEnqueuedTest2, &context2, 0, subtaskCount, workerIndex, dispatcher); + } + else + { + var stack = (ParallelTaskStack*)typedContext->TaskPile; + stack->For(&DynamicallyEnqueuedTest1, &context1, 0, subtaskCount, workerIndex, dispatcher); + stack->For(&DynamicallyEnqueuedTest2, &context2, 0, subtaskCount, workerIndex, dispatcher); + } } Interlocked.Add(ref typedContext->Sum, sum); } @@ -74,7 +89,7 @@ static void STTest(long taskId, void* context, int workerIndex, IThreadDispatche { var sum = DoSomeWork(100000, 0); var typedContext = (Context*)context; - if ((taskId & 7) == 0) + //if ((taskId & 7) == 0) { const int subtaskCount = 8; var context1 = new DynamicContext1 { Context = typedContext }; @@ -91,22 +106,34 @@ static void STTest(long taskId, void* context, int workerIndex, IThreadDispatche Interlocked.Add(ref typedContext->Sum, sum); } - static void DispatcherBody(int workerIndex, IThreadDispatcher dispatcher) + static void DispatcherBody(int workerIndex, IThreadDispatcher dispatcher) where T : unmanaged { - var taskQueue = (TaskQueue*)dispatcher.UnmanagedContext; - while (taskQueue->TryDequeueAndRun(workerIndex, dispatcher) != DequeueTaskResult.Stop) ; + if (typeof(T) == typeof(TaskQueue)) + { + var taskQueue = (TaskQueue*)dispatcher.UnmanagedContext; + while (taskQueue->TryDequeueAndRun(workerIndex, dispatcher) != DequeueTaskResult.Stop) ; + } + else + { + var taskStack = (ParallelTaskStack*)dispatcher.UnmanagedContext; + while (taskStack->TryPopAndRun(workerIndex, dispatcher) != PopTaskResult.Stop) ; + } } struct Context { - public TaskQueue* Queue; + public void* TaskPile; public int Sum; } - static void IssueStop(long id, void* context, int workerIndex, IThreadDispatcher dispatcher) + static void IssueStop(long id, void* context, int workerIndex, IThreadDispatcher dispatcher) where T : unmanaged { var typedContext = (Context*)context; - typedContext->Queue->EnqueueStop(workerIndex, dispatcher); + if (typeof(T) == typeof(TaskQueue)) + ((TaskQueue*)typedContext->TaskPile)->EnqueueStop(workerIndex, dispatcher); + else + ((ParallelTaskStack*)typedContext->TaskPile)->RequestStop(); + } static void EmptyDispatch(int workerIndex, IThreadDispatcher dispatcher) @@ -122,12 +149,10 @@ public override void Initialize(ContentArchive content, Camera camera) Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(4, 1)); - Console.WriteLine($"Task size: {Unsafe.SizeOf()}"); + Console.WriteLine($"Task size: {Unsafe.SizeOf()}, {Unsafe.SizeOf()}"); int iterationCount = 4; int tasksPerIteration = 64; - var taskQueue = new TaskQueue(BufferPool); - var taskQueuePointer = &taskQueue; //Test(() => //{ @@ -136,34 +161,54 @@ public override void Initialize(ContentArchive content, Camera camera) // return 0; //}, "Dispatch"); + var taskStack = new ParallelTaskStack(BufferPool, ThreadDispatcher, ThreadDispatcher.ThreadCount); + var taskStackPointer = &taskStack; + Test(() => { - var context = new Context { Queue = taskQueuePointer }; - var continuation = taskQueuePointer->AllocateContinuation(iterationCount * tasksPerIteration, 0, ThreadDispatcher, new Task(&IssueStop, &context)); + var context = new Context { TaskPile = taskStackPointer }; + var continuation = taskStackPointer->AllocateContinuation(iterationCount * tasksPerIteration, 0, ThreadDispatcher, new TaskS(&IssueStop, &context)); for (int i = 0; i < iterationCount; ++i) { - taskQueuePointer->TryEnqueueForUnsafely(&Test, &context, i * tasksPerIteration, tasksPerIteration, continuation); + taskStackPointer->PushForUnsafely(&Test, &context, i * tasksPerIteration, tasksPerIteration, 0, ThreadDispatcher, continuation); } //taskQueuePointer->TryEnqueueStopUnsafely(); //taskQueuePointer->EnqueueTasks() - ThreadDispatcher.DispatchWorkers(&DispatcherBody, unmanagedContext: taskQueuePointer); + ThreadDispatcher.DispatchWorkers(&DispatcherBody, unmanagedContext: taskStackPointer); return context.Sum; - }, "MT", () => taskQueuePointer->Reset()); + }, "MT", () => taskStackPointer->Reset()); - taskQueue.Dispose(BufferPool); + var taskQueue = new TaskQueue(BufferPool, maximumTaskCapacity: 1 << 19, maximumContinuationCapacity: 1 << 19); + var taskQueuePointer = &taskQueue; Test(() => { - var testContext = new Context { }; + var context = new Context { TaskPile = taskQueuePointer }; + var continuation = taskQueuePointer->AllocateContinuation(iterationCount * tasksPerIteration, 0, ThreadDispatcher, new TaskQ(&IssueStop, &context)); for (int i = 0; i < iterationCount; ++i) { - for (int j = 0; j < tasksPerIteration; ++j) - { - STTest(i * tasksPerIteration + j, &testContext, 0, ThreadDispatcher); - } + taskQueuePointer->TryEnqueueForUnsafely(&Test, &context, i * tasksPerIteration, tasksPerIteration, continuation); } - return testContext.Sum; - }, "ST"); + //taskQueuePointer->TryEnqueueStopUnsafely(); + //taskQueuePointer->EnqueueTasks() + ThreadDispatcher.DispatchWorkers(&DispatcherBody, unmanagedContext: taskQueuePointer); + return context.Sum; + }, "MT", () => taskQueuePointer->Reset()); + + taskQueue.Dispose(BufferPool); + + //Test(() => + //{ + // var testContext = new Context { }; + // for (int i = 0; i < iterationCount; ++i) + // { + // for (int j = 0; j < tasksPerIteration; ++j) + // { + // STTest(i * tasksPerIteration + j, &testContext, 0, ThreadDispatcher); + // } + // } + // return testContext.Sum; + //}, "ST"); } From 1b4a88a0edd8e56f76a8cdfcae8d3cebce51d3e1 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 10 Dec 2022 17:02:55 -0600 Subject: [PATCH 674/947] Added masks; not very helpful. --- BepuUtilities/ParallelTaskStack.cs | 98 ++++++++++++++++++--- Demos/SpecializedTests/TaskQueueTestDemo.cs | 15 +++- 2 files changed, 98 insertions(+), 15 deletions(-) diff --git a/BepuUtilities/ParallelTaskStack.cs b/BepuUtilities/ParallelTaskStack.cs index ce5649b27..73737efbb 100644 --- a/BepuUtilities/ParallelTaskStack.cs +++ b/BepuUtilities/ParallelTaskStack.cs @@ -1,6 +1,8 @@ using System; +using System.ComponentModel; using System.Diagnostics; using System.Diagnostics.CodeAnalysis; +using System.Numerics; using System.Runtime.CompilerServices; using System.Runtime.InteropServices; using System.Threading; @@ -207,11 +209,13 @@ internal unsafe struct WorkerTaskStack public IdPool ContinuationIndexPool; public int ContinuationCount; public int WorkerIndex; + public Buffer Masks; public volatile int TaskLocker; public volatile int ContinuationLocker; - public WorkerTaskStack(int workerIndex, IThreadDispatcher dispatcher, int initialTaskCapacity = 64, int initialContinuationCapacity = 16) + public WorkerTaskStack(int workerIndex, IThreadDispatcher dispatcher, Buffer masks, int initialTaskCapacity = 64, int initialContinuationCapacity = 16) { + Masks = masks; var threadPool = dispatcher.WorkerPools[workerIndex]; WorkerIndex = workerIndex; Tasks = new QuickList(initialTaskCapacity, threadPool); @@ -257,11 +261,23 @@ internal bool TryPop(out Task task) try { //We have the lock. - return Tasks.TryPop(out task); + if (Tasks.TryPop(out task)) + { + if (Tasks.Count == 0) + { + //Just transitioned from taskful to taskless; notify the bitmask. + var maskIndex = WorkerIndex / 64; + var indexInMask = WorkerIndex - maskIndex * 64; + Interlocked.And(ref Masks[maskIndex], ~(1ul << indexInMask)); + } + return true; + } + return false; } finally { TaskLocker = 0; + } } } @@ -294,8 +310,15 @@ internal void PushUnsafely(Span tasks, IThreadDispatcher dispatcher) Tasks.Span.Clear(Tasks.Count, Tasks.Span.length - Tasks.Count); #endif } - Tasks.AddRangeUnsafely(tasks); + + if (Tasks.Count == tasks.Length) + { + //Just transitioned from taskless to taskful; notify the bitmask. + var maskIndex = WorkerIndex / 64; + var indexInMask = WorkerIndex - maskIndex * 64; + Interlocked.Or(ref Masks[maskIndex], 1ul << indexInMask); + } } /// /// Pushes a task onto the task stack. Does not acquire a lock. @@ -493,6 +516,7 @@ public unsafe struct TaskContinuation public unsafe struct ParallelTaskStack { Buffer workerStacks; + Buffer workerHasTaskMask; [StructLayout(LayoutKind.Explicit, Size = 256 + 16)] struct StopPad @@ -513,9 +537,11 @@ struct StopPad public ParallelTaskStack(BufferPool pool, IThreadDispatcher dispatcher, int workerCount, int initialWorkerTaskCapacity = 64, int initialWorkerContinuationCapacity = 16) { pool.Take(workerCount, out workerStacks); + pool.Take((workerCount + 63) / 64, out workerHasTaskMask); + workerHasTaskMask.Clear(0, workerHasTaskMask.Length); for (int i = 0; i < workerCount; ++i) { - workerStacks[i] = new WorkerTaskStack(i, dispatcher, initialWorkerTaskCapacity, initialWorkerContinuationCapacity); + workerStacks[i] = new WorkerTaskStack(i, dispatcher, workerHasTaskMask, initialWorkerTaskCapacity, initialWorkerContinuationCapacity); } Reset(); } @@ -530,6 +556,7 @@ public void Reset() workerStacks[i].Reset(); } padded.Stop = false; + workerHasTaskMask.Clear(0, workerHasTaskMask.Length); } /// @@ -543,6 +570,7 @@ public void Dispose(BufferPool pool) workerStacks[i].Reset(); } pool.Return(ref workerStacks); + pool.Return(ref workerHasTaskMask); } /// @@ -604,6 +632,9 @@ public ContinuationHandle AllocateContinuation(int taskCount, int workerIndex, I return workerStacks[workerIndex].AllocateContinuation(taskCount, dispatcher, onCompleted); } + public static int PopFailed; + public static int StealRequired; + public static int NoStealRequired; /// /// Attempts to pop a task. /// @@ -612,18 +643,61 @@ public ContinuationHandle AllocateContinuation(int taskCount, int workerIndex, I /// Result status of the pop attempt. public PopTaskResult TryPop(int workerIndex, out Task task) { - //Walk through the worker stacks looking for work. Start with the current worker. - int peekedWorkerIndex = workerIndex; - for (int i = 0; i < workerStacks.length; ++i) + //Try the local worker first. + if (workerStacks[workerIndex].TryPop(out task)) { - if (workerStacks[peekedWorkerIndex].TryPop(out task)) - { + Interlocked.Increment(ref NoStealRequired); + return PopTaskResult.Success; + } + Interlocked.Increment(ref StealRequired); + //There was no task available locally, so go for a steal. + //We want to distribute steals so that not *every* thread seeks the same source. + //So check all worker slots as a ring. + //The first time we check the local long, we should mask out all earlier slots to ensure the order. + var startIndex = workerIndex / 64; + var indexInMask = workerIndex - startIndex * 64; + var mask = workerHasTaskMask[startIndex]; + var earlyMask = mask; + mask &= ~((1ul << indexInMask) - 1); + var setBitIndex = BitOperations.TrailingZeroCount(mask); + if (setBitIndex < 64) + { + if (workerStacks[startIndex * 64 + setBitIndex].TryPop(out task)) return PopTaskResult.Success; + } + + //No task available in the local chunk, so just keep going. + for (int i = 1; i <= workerHasTaskMask.length; ++i) + { + var maskIndex = startIndex + i; + if (maskIndex >= workerHasTaskMask.length) + maskIndex -= workerHasTaskMask.length; + //No more need for masking out early bits; just find the first task. + mask = workerHasTaskMask[maskIndex]; + setBitIndex = BitOperations.TrailingZeroCount(mask); + if (setBitIndex < 64) + { + if (workerStacks[maskIndex * 64 + setBitIndex].TryPop(out task)) + return PopTaskResult.Success; } - ++peekedWorkerIndex; - if (peekedWorkerIndex >= workerStacks.length) - peekedWorkerIndex -= workerStacks.length; } + + Interlocked.Increment(ref PopFailed); + task = default; + return padded.Stop ? PopTaskResult.Stop : PopTaskResult.Empty; + + ////Walk through the worker stacks looking for work. Start with the current worker. + //int peekedWorkerIndex = workerIndex; + //for (int i = 0; i < workerStacks.length; ++i) + //{ + // if (workerStacks[peekedWorkerIndex].TryPop(out task)) + // { + // return PopTaskResult.Success; + // } + // ++peekedWorkerIndex; + // if (peekedWorkerIndex >= workerStacks.length) + // peekedWorkerIndex -= workerStacks.length; + //} //No worker had anything! task = default; return padded.Stop ? PopTaskResult.Stop : PopTaskResult.Empty; diff --git a/Demos/SpecializedTests/TaskQueueTestDemo.cs b/Demos/SpecializedTests/TaskQueueTestDemo.cs index 549303a34..d6b831674 100644 --- a/Demos/SpecializedTests/TaskQueueTestDemo.cs +++ b/Demos/SpecializedTests/TaskQueueTestDemo.cs @@ -151,7 +151,7 @@ public override void Initialize(ContentArchive content, Camera camera) Console.WriteLine($"Task size: {Unsafe.SizeOf()}, {Unsafe.SizeOf()}"); - int iterationCount = 4; + int iterationCount = 64; int tasksPerIteration = 64; //Test(() => @@ -170,13 +170,22 @@ public override void Initialize(ContentArchive content, Camera camera) var continuation = taskStackPointer->AllocateContinuation(iterationCount * tasksPerIteration, 0, ThreadDispatcher, new TaskS(&IssueStop, &context)); for (int i = 0; i < iterationCount; ++i) { - taskStackPointer->PushForUnsafely(&Test, &context, i * tasksPerIteration, tasksPerIteration, 0, ThreadDispatcher, continuation); + taskStackPointer->PushForUnsafely(&Test, &context, i * tasksPerIteration, tasksPerIteration, i % ThreadDispatcher.ThreadCount, ThreadDispatcher, continuation); } //taskQueuePointer->TryEnqueueStopUnsafely(); //taskQueuePointer->EnqueueTasks() ThreadDispatcher.DispatchWorkers(&DispatcherBody, unmanagedContext: taskStackPointer); return context.Sum; - }, "MT", () => taskStackPointer->Reset()); + }, "MT Parallel Stack", () => taskStackPointer->Reset()); + + var noSteal = ParallelTaskStack.NoStealRequired; + var stealRequired = ParallelTaskStack.StealRequired; + var stealFail = ParallelTaskStack.PopFailed; + var steal = stealRequired - stealFail; + + Console.WriteLine($"Nosteal, steal succeed, steal fail: {noSteal}, {steal}, {stealFail} ({steal / (double)(stealRequired + noSteal)})"); + + taskStack.Dispose(BufferPool); var taskQueue = new TaskQueue(BufferPool, maximumTaskCapacity: 1 << 19, maximumContinuationCapacity: 1 << 19); var taskQueuePointer = &taskQueue; From d82467706d21f6e62713b5d879acfa97f508d338 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 10 Dec 2022 17:47:41 -0600 Subject: [PATCH 675/947] Not great! --- BepuUtilities/ParallelTaskStack.cs | 29 ++++++++++++++++++++++++++--- 1 file changed, 26 insertions(+), 3 deletions(-) diff --git a/BepuUtilities/ParallelTaskStack.cs b/BepuUtilities/ParallelTaskStack.cs index 73737efbb..60fab2214 100644 --- a/BepuUtilities/ParallelTaskStack.cs +++ b/BepuUtilities/ParallelTaskStack.cs @@ -646,10 +646,31 @@ public PopTaskResult TryPop(int workerIndex, out Task task) //Try the local worker first. if (workerStacks[workerIndex].TryPop(out task)) { - Interlocked.Increment(ref NoStealRequired); + //Interlocked.Increment(ref NoStealRequired); return PopTaskResult.Success; } - Interlocked.Increment(ref StealRequired); + //Interlocked.Increment(ref StealRequired); + + //for (int i = 0; i < workerHasTaskMask.length; ++i) + //{ + // //No more need for masking out early bits; just find the first task. + // var mask = workerHasTaskMask[i]; + // int setBitIndex; + + // while ((setBitIndex = BitOperations.TrailingZeroCount(mask)) < 64) + // { + // if (workerStacks[i * 64 + setBitIndex].TryPop(out task)) + // return PopTaskResult.Success; + // //This stack didn't have anything for us, mask it out. + // mask &= ~(1ul << setBitIndex); + // } + //} + ////Interlocked.Increment(ref PopFailed); + //task = default; + //return padded.Stop ? PopTaskResult.Stop : PopTaskResult.Empty; + + + //There was no task available locally, so go for a steal. //We want to distribute steals so that not *every* thread seeks the same source. //So check all worker slots as a ring. @@ -682,10 +703,12 @@ public PopTaskResult TryPop(int workerIndex, out Task task) } } - Interlocked.Increment(ref PopFailed); + //Interlocked.Increment(ref PopFailed); task = default; return padded.Stop ? PopTaskResult.Stop : PopTaskResult.Empty; + + ////Walk through the worker stacks looking for work. Start with the current worker. //int peekedWorkerIndex = workerIndex; //for (int i = 0; i < workerStacks.length; ++i) From 0ce149b73d515d598cf5c18a2121f633ca69f6f5 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 10 Dec 2022 20:47:36 -0600 Subject: [PATCH 676/947] More bad implementations! --- BepuUtilities/ParallelTaskStack.cs | 2 +- BepuUtilities/TaskStack.cs | 738 ++++++++++++++++++++ Demos/SpecializedTests/TaskQueueTestDemo.cs | 68 +- 3 files changed, 792 insertions(+), 16 deletions(-) create mode 100644 BepuUtilities/TaskStack.cs diff --git a/BepuUtilities/ParallelTaskStack.cs b/BepuUtilities/ParallelTaskStack.cs index 60fab2214..679091316 100644 --- a/BepuUtilities/ParallelTaskStack.cs +++ b/BepuUtilities/ParallelTaskStack.cs @@ -9,7 +9,7 @@ using BepuUtilities.Collections; using BepuUtilities.Memory; -namespace BepuUtilities.TestStack; +namespace BepuUtilities.TestParallelStack; /// /// Description of a task to be submitted to a . diff --git a/BepuUtilities/TaskStack.cs b/BepuUtilities/TaskStack.cs new file mode 100644 index 000000000..7e77c6af2 --- /dev/null +++ b/BepuUtilities/TaskStack.cs @@ -0,0 +1,738 @@ +using System; +using System.ComponentModel; +using System.Diagnostics; +using System.Diagnostics.CodeAnalysis; +using System.Numerics; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; +using System.Threading; +using BepuUtilities.Collections; +using BepuUtilities.Memory; + +namespace BepuUtilities.TestStack; + +/// +/// Description of a task to be submitted to a . +/// +public unsafe struct Task +{ + /// + /// Function to be executed by the task. Takes as arguments the , pointer, and executing worker index. + /// + public delegate* Function; + /// + /// Context to be passed into the . + /// + public void* Context; + /// + /// Continuation to be notified after this task completes, if any. + /// + public ContinuationHandle Continuation; + /// + /// User-provided identifier of this task. + /// + public long Id; + + /// + /// Creates a new task. + /// + /// Function to be executed by the task. Takes as arguments the , pointer, executing worker index, and executing . + /// Context pointer to pass to the . + /// Id of this task to be passed into the . + /// Continuation to notify after the completion of this task, if any. + public Task(delegate* function, void* context = null, long taskId = 0, ContinuationHandle continuation = default) + { + Function = function; + Context = context; + Continuation = continuation; + Id = taskId; + } + + /// + /// Creates a task from a function. + /// + /// Function to turn into a task. + public static implicit operator Task(delegate* function) => new(function); + + /// + /// Runs the task and, if necessary, notifies the associated continuation of its completion. + /// + /// Worker index to pass to the function. + /// Dispatcher running this task. + public void Run(int workerIndex, IThreadDispatcher dispatcher) + { + Debug.Assert(!Continuation.Completed); + Function(Id, Context, workerIndex, dispatcher); + if (Continuation.Initialized) + Continuation.NotifyTaskCompleted(workerIndex, dispatcher); + } +} + +/// +/// Describes the result status of a pop attempt. +/// +public enum PopTaskResult +{ + /// + /// A task was successfully popped. + /// + Success, + /// + /// The pop was contested. + /// + Contested, + /// + /// The stack was empty, but may have more tasks in the future. + /// + Empty, + /// + /// The stack has been terminated and all threads seeking work should stop. + /// + Stop +} +/// +/// Describes the result of a task enqueue attempt. +/// +public enum PushTaskResult +{ + /// + /// The tasks were successfully pushed. + /// + Success, + /// + /// The push attempt was blocked by concurrent access. + /// + Contested, + /// + /// The push attempt was blocked because no space remained in the tasks buffer. + /// + Full, +} + + +/// +/// Refers to a continuation within a . +/// +public unsafe struct ContinuationHandle : IEquatable +{ + uint index; + uint encodedVersion; + /// + /// Source worker task stack. + /// + internal TaskStackContinuations* Continuations; + + internal ContinuationHandle(uint index, int version, TaskStackContinuations* continuations) + { + this.index = index; + encodedVersion = (uint)version | 1u << 31; + Continuations = continuations; + } + + internal uint Index + { + get + { + Debug.Assert(Initialized, "If you're trying to pull a continuation id from a continuation handle, it should have been initialized."); + return index; + } + } + + internal int Version + { + get + { + Debug.Assert(Initialized, "If you're trying to pull a continuation id from a continuation handle, it should have been initialized."); + return (int)(encodedVersion & ((1u << 31) - 1)); + } + } + + /// + /// Gets whether the tasks associated with this continuation have completed. + /// + public bool Completed => Continuations->IsComplete(this); + + /// + /// Retrieves a pointer to the continuation data for . + /// + /// Pointer to the continuation backing the given handle. + /// This should not be used if the continuation handle is not known to be valid. The data pointed to by the data could become invalidated if the continuation completes. + public TaskContinuation* Continuation => Continuations->GetContinuation(this); + + /// + /// Gets a null continuation handle. + /// + public static ContinuationHandle Null => default; + + /// + /// Gets whether the continuation associated with this handle was allocated and has outstanding tasks. + /// + public bool Exists + { + get + { + if (!Initialized || Continuations == null) + return false; + ref var continuation = ref Continuations->Continuations[Index]; + return continuation.Version == Version && continuation.RemainingTaskCounter > 0; + } + } + + /// + /// Gets whether this handle ever represented an allocated handle. This does not guarantee that the continuation's associated tasks are active in the that it was allocated from. + /// + public bool Initialized => encodedVersion >= 1u << 31; + + /// + /// Notifies the continuation that one task was completed. + /// + /// Worker index to pass to the continuation's delegate, if any. + /// Dispatcher to pass to the continuation's delegate, if any. + public void NotifyTaskCompleted(int workerIndex, IThreadDispatcher dispatcher) + { + var continuation = Continuation; + Debug.Assert(!Completed); + var counter = Interlocked.Decrement(ref continuation->RemainingTaskCounter); + Debug.Assert(counter >= 0, "The counter should not go negative. Was notify called too many times?"); + if (counter == 0) + { + //This entire job has completed. + if (continuation->OnCompleted.Function != null) + { + continuation->OnCompleted.Function(continuation->OnCompleted.Id, continuation->OnCompleted.Context, workerIndex, dispatcher); + } + //Free this continuation slot. + var waiter = new SpinWait(); + while (Interlocked.CompareExchange(ref Continuations->ContinuationLocker, 1, 0) != 0) + { + waiter.SpinOnce(-1); + } + //We have the lock. + Continuations->ContinuationIndexPool.ReturnUnsafely((int)Index); + --Continuations->ContinuationCount; + Continuations->ContinuationLocker = 0; + } + } + + public bool Equals(ContinuationHandle other) => other.index == index && other.encodedVersion == encodedVersion && other.Continuations == Continuations; + + public override bool Equals([NotNullWhen(true)] object obj) => obj is ContinuationHandle handle && Equals(handle); + + public override int GetHashCode() => (int)(index ^ (encodedVersion << 24)); + + public static bool operator ==(ContinuationHandle left, ContinuationHandle right) => left.Equals(right); + + public static bool operator !=(ContinuationHandle left, ContinuationHandle right) => !(left == right); +} + +/// +/// Stores data relevant to tracking task completion and reporting completion for a continuation. +/// +public unsafe struct TaskContinuation +{ + /// + /// Task to run upon completion of the associated task. + /// + public Task OnCompleted; + /// + /// Version of this continuation. + /// + public int Version; + /// + /// Number of tasks not yet reported as complete in the continuation. + /// + public int RemainingTaskCounter; +} + +unsafe struct TaskStackContinuations +{ + public Buffer Continuations; + public IdPool ContinuationIndexPool; + public int ContinuationCount; + public volatile int ContinuationLocker; + + public TaskStackContinuations(BufferPool pool, int maximumCapacity) + { + pool.Take(maximumCapacity, out Continuations); + ContinuationIndexPool = new IdPool(maximumCapacity, pool); + } + + public void Dispose(BufferPool pool) + { + pool.Return(ref Continuations); + ContinuationIndexPool.Dispose(pool); + } + + /// + /// Retrieves a pointer to the continuation data for . + /// + /// Handle to look up the associated continuation for. + /// Pointer to the continuation backing the given handle. + /// This should not be used if the continuation handle is not known to be valid. The data pointed to by the data could become invalidated if the continuation completes. + public TaskContinuation* GetContinuation(ContinuationHandle continuationHandle) + { + Debug.Assert(continuationHandle.Initialized, "This continuation handle was never initialized."); + Debug.Assert(continuationHandle.Index < Continuations.length, "This continuation refers to an invalid index."); + if (continuationHandle.Index >= Continuations.length || !continuationHandle.Initialized) + return null; + var continuation = Continuations.Memory + continuationHandle.Index; + Debug.Assert(continuation->Version == continuationHandle.Version, "This continuation no longer refers to an active continuation."); + if (continuation->Version != continuationHandle.Version) + return null; + return Continuations.Memory + continuationHandle.Index; + } + + /// + /// Checks whether all tasks associated with this continuation have completed. + /// + /// Continuation to check for completion. + /// True if all tasks associated with a continuation have completed, false otherwise. + public bool IsComplete(ContinuationHandle continuationHandle) + { + Debug.Assert(continuationHandle.Initialized, "This continuation handle was never initialized."); + Debug.Assert(continuationHandle.Index < Continuations.length, "This continuation refers to an invalid index."); + if (continuationHandle.Index >= Continuations.length || !continuationHandle.Initialized) + return false; + ref var continuation = ref Continuations[continuationHandle.Index]; + return continuation.Version > continuationHandle.Version || continuation.RemainingTaskCounter == 0; + } +} + +/// +/// Manages task stacks for parallel workers. Supports work stealing between workers. +/// +public unsafe struct TaskStack +{ + [StructLayout(LayoutKind.Explicit, Size = 256 + 16)] + struct StopPad + { + [FieldOffset(128)] + public volatile bool Stop; + } + StopPad padded; + + + Buffer continuations; + public QuickList Tasks; + public volatile int TaskLocker; + + /// + /// Constructs a new parallel task stack. + /// + /// Buffer pool to allocate non-thread allocated resources from. + /// Thread dispatcher calling this TaskStack. + /// Maximum number of tasks to allocate space for in each worker. Tasks are individual chunks of scheduled work.. + /// Maximum number of continuations to allocate space for in each worker. + public TaskStack(BufferPool pool, IThreadDispatcher dispatcher, int maximumTaskCapacity = 512, int maximumContinuationCapacity = 512) + { + Tasks = new QuickList(maximumTaskCapacity, pool); + pool.Take(1, out continuations); + continuations[0] = new TaskStackContinuations(pool, maximumContinuationCapacity); +#if DEBUG + //While you shouldn't *need* to clear continuations, it can be useful for debug purposes. + continuations[0].Continuations.Clear(0, continuations[0].Continuations.length); + Tasks.Span.Clear(0, Tasks.Span.length); +#endif + Reset(); + } + + /// + /// Returns unmanaged resources held by the to a pool. + /// + /// Buffer pool to return resources to. + public void Dispose(BufferPool pool) + { + Tasks.Dispose(pool); + continuations[0].Dispose(pool); + } + + /// + /// Returns the stack to a fresh state without reallocating. + /// + public void Reset() + { + Tasks.Count = 0; + Debug.Assert(TaskLocker == 0, "There appears to be a thread actively working still. That's invalid."); +#if DEBUG + //While you shouldn't *need* to clear continuations, it can be useful for debug purposes. + continuations[0].Continuations.Clear(0, continuations[0].Continuations.length); +#endif + continuations[0].ContinuationCount = 0; + Debug.Assert(continuations[0].ContinuationLocker == 0, "There appears to be a thread actively working still. That's invalid."); + padded.Stop = false; + } + + /// + /// Gets the number of active tasks. + /// + public int TaskCount + { + get + { + return Tasks.Count; + } + } + /// + /// Gets the number of active continuations. + /// + public int ContinuationCount + { + get + { + return continuations[0].ContinuationCount; + } + } + + /// + /// Attempts to pop a task from the stack. + /// + /// Task popped from the stack, if any. + /// True if a task was available to pop, false otherwise. + public PopTaskResult TryPop(out Task task) + { + //Quick early out to avoid lock expense. + if (Tasks.Count == 0) + { + task = default; + return padded.Stop ? PopTaskResult.Stop : PopTaskResult.Empty; + } + if (Interlocked.CompareExchange(ref TaskLocker, 1, 0) != 0) + { + task = default; + return PopTaskResult.Contested; + } + try + { + //We have the lock. + if (Tasks.TryPop(out task)) + { + return PopTaskResult.Success; + } + return PopTaskResult.Empty; + } + finally + { + TaskLocker = 0; + } + } + + /// + /// Pushes a set of tasks onto the stack. Does not acquire a lock. + /// + /// Tasks composing the job. + /// True if there was room to complete the push, false otherwise. + public bool TryPushUnsafely(Span tasks) + { + Debug.Assert(tasks.Length > 0, "Probably shouldn't be trying to push zero tasks."); + if (tasks.Length + Tasks.Count > Tasks.Span.Length) + { + return false; + } + Tasks.AddRangeUnsafely(tasks); + return true; + } + + /// + /// Pushes a task onto the task stack. Does not acquire a lock. + /// + /// Task to push + /// True if there was room to complete the push, false otherwise. + public unsafe bool TryPushUnsafely(Task task) + { + return TryPushUnsafely(new Span(&task, 1)); + } + + /// + /// Tries to push a set of tasks to the task stack if the stack is uncontested. + /// + /// Tasks composing the job. + /// Status of the push attempt. + public PushTaskResult TryPush(Span tasks) + { + if (Interlocked.CompareExchange(ref TaskLocker, 1, 0) != 0) + return PushTaskResult.Contested; + try + { + //We have the lock. + return TryPushUnsafely(tasks) ? PushTaskResult.Success : PushTaskResult.Full; + } + finally + { + TaskLocker = 0; + } + } + + /// + /// Push a set of tasks to the task stack. + /// + /// Tasks composing the job. + /// Worker index of the currently executing thread. + /// Currently executing thread dispatcher. + /// Runs filler tasks if the push is blocked by a lack of capacity. + public void Push(Span tasks, int workerIndex, IThreadDispatcher dispatcher) + { + var waiter = new SpinWait(); + PushTaskResult result; + while ((result = TryPush(tasks)) != PushTaskResult.Success) + { + if (result == PushTaskResult.Contested) + waiter.SpinOnce(-1); + else if (result == PushTaskResult.Full) + { + waiter.Reset(); + while (true) + { + var popResult = TryPopAndRun(workerIndex, dispatcher); + Debug.Assert(popResult != PopTaskResult.Stop, "Should not try to push onto a stopped stack."); + if (popResult == PopTaskResult.Stop) return; + if (popResult != PopTaskResult.Contested) break; + waiter.SpinOnce(-1); + } + waiter.Reset(); + } + } + } + + /// + /// Attempts to allocate a continuation for a set of tasks. + /// + /// Number of tasks associated with the continuation. + /// Function to execute upon completing all associated tasks, if any. Any task with a null will not be executed. + /// Handle of the continuation if allocation is successful. + /// True if the continuation was allocated, false if the attempt was contested.. + public bool TryAllocateContinuation(int taskCount, out ContinuationHandle continuationHandle, Task onCompleted = default) + { + continuationHandle = default; + ref var continuations = ref this.continuations[0]; + if (Interlocked.CompareExchange(ref continuations.ContinuationLocker, 1, 0) != 0) + return false; + try + { + //We have the lock. + var index = continuations.ContinuationIndexPool.Take(); + ++continuations.ContinuationCount; + ref var continuation = ref continuations.Continuations[index]; + //Note that the version number could be based on undefined data initially. That's actually fine; all we care about is whether it is different. + //Note mask to leave a valid bit for encoding in the handle. + var newVersion = (continuation.Version + 1) & (~(1 << 31)); + continuation.OnCompleted = onCompleted; + continuation.Version = newVersion; + continuation.RemainingTaskCounter = taskCount; + continuationHandle = new ContinuationHandle((uint)index, newVersion, this.continuations.Memory); + return true; + } + finally + { + continuations.ContinuationLocker = 0; + } + } + + /// + /// Allocates a continuation for a set of tasks. + /// + /// Number of tasks associated with the continuation. + /// Index of the worker to pass into the task function. + /// Thread dispatcher running this task stack. + /// Task to execute upon completing all associated tasks, if any. Any task with a null will not be executed. + /// Handle of the allocated continuation. + /// Note that this will keep trying until allocation succeeds. + public ContinuationHandle AllocateContinuation(int taskCount, int workerIndex, IThreadDispatcher dispatcher, Task onCompleted = default) + { + var waiter = new SpinWait(); + ContinuationHandle handle; + while (!TryAllocateContinuation(taskCount, out handle, onCompleted)) + { + var result = TryPopAndRun(workerIndex, dispatcher); + Debug.Assert(result != PopTaskResult.Stop, "Should not attempt to allocate a continuation after a stop command."); + if (result == PopTaskResult.Stop) + return default; + if (result == PopTaskResult.Success) + waiter.Reset(); + else + waiter.SpinOnce(-1); + } + return handle; + } + + + /// + /// Attempts to pop a task and run it. + /// + /// Index of the worker to pass into the task function. + /// Thread dispatcher running this task stack. + /// Result status of the pop attempt. + public PopTaskResult TryPopAndRun(int workerIndex, IThreadDispatcher dispatcher) + { + var result = TryPop(out var task); + if (result == PopTaskResult.Success) + { + task.Run(workerIndex, dispatcher); + } + return result; + } + + /// + /// Pushes a set of tasks to the stack with a created continuation. + /// + /// Tasks composing the job. A continuation will be assigned internally; no continuation should be present on any of the provided tasks. + /// Thread dispatcher calling this TaskStack. + /// Index of the currently executing worker. + /// Task to run upon completion of all the submitted tasks, if any. + /// Handle of the continuation created for these tasks. + /// Note that this will keep trying until task submission succeeds. + public ContinuationHandle AllocateContinuationAndPush(Span tasks, int workerIndex, IThreadDispatcher dispatcher, Task onComplete = default) + { + var continuationHandle = AllocateContinuation(tasks.Length, workerIndex, dispatcher, onComplete); + for (int i = 0; i < tasks.Length; ++i) + { + ref var task = ref tasks[i]; + Debug.Assert(!task.Continuation.Initialized, "This function creates a continuation for the tasks"); + task.Continuation = continuationHandle; + } + Push(tasks, workerIndex, dispatcher); + return continuationHandle; + } + + /// + /// Waits for a continuation to be completed. + /// + /// Instead of spinning the entire time, this may pop and execute pending tasks to fill the gap. + /// Continuation to wait on. + /// Thread dispatcher calling this TaskStack. + /// Index of the executing worker. + public void WaitForCompletion(ContinuationHandle continuation, int workerIndex, IThreadDispatcher dispatcher) + { + var waiter = new SpinWait(); + Debug.Assert(continuation.Initialized, "This codepath should only run if the continuation was allocated earlier."); + while (!continuation.Completed) + { + var result = TryPop(out var fillerTask); + if (result == PopTaskResult.Stop) + { + return; + } + if (result == PopTaskResult.Success) + { + fillerTask.Run(workerIndex, dispatcher); + waiter.Reset(); + } + else + { + waiter.SpinOnce(-1); + } + } + } + + /// + /// Pushes a set of tasks to the worker stack and returns when all tasks are complete. + /// + /// Tasks composing the job. A continuation will be assigned internally; no continuation should be present on any of the provided tasks. + /// Index of the worker executing this function. + /// Thread dispatcher calling this TaskStack. + /// Note that this will keep working until all tasks are run. It may execute tasks unrelated to the requested tasks while waiting on other workers to complete constituent tasks. + public void RunTasks(Span tasks, int workerIndex, IThreadDispatcher dispatcher) + { + if (tasks.Length == 0) + return; + ContinuationHandle continuationHandle = default; + if (tasks.Length > 1) + { + //Note that we only submit tasks to the stack for tasks beyond the first. The current thread is responsible for at least task 0. + var taskCount = tasks.Length - 1; + Span tasksToPush = stackalloc Task[taskCount]; + continuationHandle = AllocateContinuation(taskCount, workerIndex, dispatcher); + for (int i = 0; i < tasksToPush.Length; ++i) + { + var task = tasks[i + 1]; + Debug.Assert(continuationHandle.Continuations == this.continuations.Memory); + Debug.Assert(!task.Continuation.Initialized, $"None of the source tasks should have continuations when provided to {nameof(RunTasks)}."); + task.Continuation = continuationHandle; + tasksToPush[i] = task; + } + Push(tasksToPush, workerIndex, dispatcher); + } + //Tasks [1, count) are submitted to the stack and may now be executing on other workers. + //The thread calling the for loop should not relinquish its timeslice. It should immediately begin working on task 0. + var task0 = tasks[0]; + Debug.Assert(!task0.Continuation.Initialized, $"None of the source tasks should have continuations when provided to {nameof(RunTasks)}."); + task0.Function(task0.Id, task0.Context, workerIndex, dispatcher); + + if (tasks.Length > 1) + { + //Task 0 is done; this thread should seek out other work until the job is complete. + WaitForCompletion(continuationHandle, workerIndex, dispatcher); + } + } + + /// + /// Requests that all workers stop. The next time a worker runs out of tasks to run, if it sees a stop command, it will be reported. + /// + public void RequestStop() + { + padded.Stop = true; + } + + + /// + /// Pushes a for loop onto the task stack. Does not take a lock. + /// + /// Function to execute on each iteration of the loop. + /// Context pointer to pass into each task execution. + /// Inclusive start index of the loop range. + /// Number of iterations to perform. + /// Continuation associated with the loop tasks, if any. + /// True if there was enough room on the stack to push, false otherwise. + /// This must not be used while other threads could be performing task pushes or pops that could affect the specified worker. + public bool TryPushForUnsafely(delegate* function, void* context, int inclusiveStartIndex, int iterationCount, ContinuationHandle continuation = default) + { + Span tasks = stackalloc Task[iterationCount]; + for (int i = 0; i < tasks.Length; ++i) + { + tasks[i] = new Task { Function = function, Context = context, Id = i + inclusiveStartIndex, Continuation = continuation }; + } + return TryPushUnsafely(tasks); + } + + /// + /// Pushes a for loop onto the task stack. + /// + /// Function to execute on each iteration of the loop. + /// Context pointer to pass into each task execution. + /// Inclusive start index of the loop range. + /// Number of iterations to perform. + /// Thread dispatcher calling this TaskStack. + /// Index of the worker stack to push the tasks onto. + /// Continuation associated with the loop tasks, if any. + /// This function will not usually attempt to run any iterations of the loop itself. It tries to push the loop tasks onto the stack. + /// If the task stack is full, this will opt to run the tasks inline while waiting for room. + public void PushFor(delegate* function, void* context, int inclusiveStartIndex, int iterationCount, int workerIndex, IThreadDispatcher dispatcher, ContinuationHandle continuation = default) + { + Span tasks = stackalloc Task[iterationCount]; + for (int i = 0; i < tasks.Length; ++i) + { + tasks[i] = new Task { Function = function, Context = context, Id = i + inclusiveStartIndex, Continuation = continuation }; + } + Push(tasks, workerIndex, dispatcher); + } + + /// + /// Submits a set of tasks representing a for loop over the given indices and returns when all loop iterations are complete. + /// + /// Function to execute on each iteration of the loop. + /// Context pointer to pass into each iteration of the loop. + /// Inclusive start index of the loop range. + /// Number of iterations to perform. + /// Thread dispatcher calling this TaskStack. + /// Index of the worker stack to push the tasks onto. + public void For(delegate* function, void* context, int inclusiveStartIndex, int iterationCount, int workerIndex, IThreadDispatcher dispatcher) + { + if (iterationCount <= 0) + return; + Span tasks = stackalloc Task[iterationCount]; + for (int i = 0; i < tasks.Length; ++i) + { + tasks[i] = new Task(function, context, inclusiveStartIndex + i); + } + RunTasks(tasks, workerIndex, dispatcher); + } +} diff --git a/Demos/SpecializedTests/TaskQueueTestDemo.cs b/Demos/SpecializedTests/TaskQueueTestDemo.cs index d6b831674..a2f340aba 100644 --- a/Demos/SpecializedTests/TaskQueueTestDemo.cs +++ b/Demos/SpecializedTests/TaskQueueTestDemo.cs @@ -11,9 +11,9 @@ using System.Runtime.InteropServices; using System.Reflection.Metadata; using BepuUtilities.TestQueue; -using BepuUtilities.TestStack; +using BepuUtilities.TestParallelStack; using TaskQ = BepuUtilities.TestQueue.Task; -using TaskS = BepuUtilities.TestStack.Task; +using BepuUtilities.TestStack; namespace Demos.SpecializedTests; @@ -76,12 +76,18 @@ static void Test(long taskId, void* context, int workerIndex, IThreadDispatch queue->For(&DynamicallyEnqueuedTest1, &context1, 0, subtaskCount, workerIndex, dispatcher); queue->For(&DynamicallyEnqueuedTest2, &context2, 0, subtaskCount, workerIndex, dispatcher); } - else + else if(typeof(T) == typeof(ParallelTaskStack)) { var stack = (ParallelTaskStack*)typedContext->TaskPile; stack->For(&DynamicallyEnqueuedTest1, &context1, 0, subtaskCount, workerIndex, dispatcher); stack->For(&DynamicallyEnqueuedTest2, &context2, 0, subtaskCount, workerIndex, dispatcher); } + else if (typeof(T) == typeof(TaskStack)) + { + var stack = (TaskStack*)typedContext->TaskPile; + stack->For(&DynamicallyEnqueuedTest1, &context1, 0, subtaskCount, workerIndex, dispatcher); + stack->For(&DynamicallyEnqueuedTest2, &context2, 0, subtaskCount, workerIndex, dispatcher); + } } Interlocked.Add(ref typedContext->Sum, sum); } @@ -113,10 +119,15 @@ static void DispatcherBody(int workerIndex, IThreadDispatcher dispatcher) whe var taskQueue = (TaskQueue*)dispatcher.UnmanagedContext; while (taskQueue->TryDequeueAndRun(workerIndex, dispatcher) != DequeueTaskResult.Stop) ; } - else + else if (typeof(T) == typeof(ParallelTaskStack)) { var taskStack = (ParallelTaskStack*)dispatcher.UnmanagedContext; - while (taskStack->TryPopAndRun(workerIndex, dispatcher) != PopTaskResult.Stop) ; + while (taskStack->TryPopAndRun(workerIndex, dispatcher) != BepuUtilities.TestParallelStack.PopTaskResult.Stop) ; + } + else if (typeof(T) == typeof(TaskStack)) + { + var taskStack = (TaskStack*)dispatcher.UnmanagedContext; + while (taskStack->TryPopAndRun(workerIndex, dispatcher) != BepuUtilities.TestStack.PopTaskResult.Stop) ; } } @@ -131,8 +142,10 @@ static void IssueStop(long id, void* context, int workerIndex, IThreadDispatc var typedContext = (Context*)context; if (typeof(T) == typeof(TaskQueue)) ((TaskQueue*)typedContext->TaskPile)->EnqueueStop(workerIndex, dispatcher); - else + else if (typeof(T) == typeof(ParallelTaskStack)) ((ParallelTaskStack*)typedContext->TaskPile)->RequestStop(); + else if (typeof(T) == typeof(TaskStack)) + ((TaskStack*)typedContext->TaskPile)->RequestStop(); } @@ -149,9 +162,9 @@ public override void Initialize(ContentArchive content, Camera camera) Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(4, 1)); - Console.WriteLine($"Task size: {Unsafe.SizeOf()}, {Unsafe.SizeOf()}"); + //Console.WriteLine($"Task size: {Unsafe.SizeOf()}, {Unsafe.SizeOf()}"); - int iterationCount = 64; + int iterationCount = 4; int tasksPerIteration = 64; //Test(() => @@ -161,22 +174,45 @@ public override void Initialize(ContentArchive content, Camera camera) // return 0; //}, "Dispatch"); - var taskStack = new ParallelTaskStack(BufferPool, ThreadDispatcher, ThreadDispatcher.ThreadCount); + //TASK STACK + var taskStack = new TaskStack(BufferPool, ThreadDispatcher); var taskStackPointer = &taskStack; Test(() => { var context = new Context { TaskPile = taskStackPointer }; - var continuation = taskStackPointer->AllocateContinuation(iterationCount * tasksPerIteration, 0, ThreadDispatcher, new TaskS(&IssueStop, &context)); + var continuation = taskStackPointer->AllocateContinuation(iterationCount * tasksPerIteration, 0, ThreadDispatcher, new BepuUtilities.TestStack.Task(&IssueStop, &context)); + for (int i = 0; i < iterationCount; ++i) + { + taskStackPointer->TryPushForUnsafely(&Test, &context, i * tasksPerIteration, tasksPerIteration, continuation); + } + //taskQueuePointer->TryEnqueueStopUnsafely(); + //taskQueuePointer->EnqueueTasks() + ThreadDispatcher.DispatchWorkers(&DispatcherBody, unmanagedContext: taskStackPointer); + return context.Sum; + }, "MT Stack", () => taskStackPointer->Reset()); + + + taskStack.Dispose(BufferPool); + + + //PARALLEL TASK STACK + var parallelTaskStack = new ParallelTaskStack(BufferPool, ThreadDispatcher, ThreadDispatcher.ThreadCount); + var parallelTaskStackPointer = ¶llelTaskStack; + + Test(() => + { + var context = new Context { TaskPile = parallelTaskStackPointer }; + var continuation = parallelTaskStackPointer->AllocateContinuation(iterationCount * tasksPerIteration, 0, ThreadDispatcher, new BepuUtilities.TestParallelStack.Task(&IssueStop, &context)); for (int i = 0; i < iterationCount; ++i) { - taskStackPointer->PushForUnsafely(&Test, &context, i * tasksPerIteration, tasksPerIteration, i % ThreadDispatcher.ThreadCount, ThreadDispatcher, continuation); + parallelTaskStackPointer->PushForUnsafely(&Test, &context, i * tasksPerIteration, tasksPerIteration, i % ThreadDispatcher.ThreadCount, ThreadDispatcher, continuation); } //taskQueuePointer->TryEnqueueStopUnsafely(); //taskQueuePointer->EnqueueTasks() - ThreadDispatcher.DispatchWorkers(&DispatcherBody, unmanagedContext: taskStackPointer); + ThreadDispatcher.DispatchWorkers(&DispatcherBody, unmanagedContext: parallelTaskStackPointer); return context.Sum; - }, "MT Parallel Stack", () => taskStackPointer->Reset()); + }, "MT Parallel Stack", () => parallelTaskStackPointer->Reset()); var noSteal = ParallelTaskStack.NoStealRequired; var stealRequired = ParallelTaskStack.StealRequired; @@ -185,8 +221,10 @@ public override void Initialize(ContentArchive content, Camera camera) Console.WriteLine($"Nosteal, steal succeed, steal fail: {noSteal}, {steal}, {stealFail} ({steal / (double)(stealRequired + noSteal)})"); - taskStack.Dispose(BufferPool); + parallelTaskStack.Dispose(BufferPool); + + //TASK QUEUE var taskQueue = new TaskQueue(BufferPool, maximumTaskCapacity: 1 << 19, maximumContinuationCapacity: 1 << 19); var taskQueuePointer = &taskQueue; @@ -226,7 +264,7 @@ public override void Initialize(ContentArchive content, Camera camera) static void Test(TestFunction function, string name, Action reset = null) { long accumulatedTime = 0; - const int testCount = 128; + const int testCount = 1024; int accumulator = 0; for (int i = 0; i < testCount; ++i) { From 4e7f9eb91c8d56b5bd074279f15177552a2e4e51 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 14 Jan 2023 22:57:11 -0600 Subject: [PATCH 677/947] In the middle of adding/testing LinkedTaskStack. Still busted. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 7 + BepuUtilities/LinkedTaskStack.cs | 904 ++++++++++++++++++ Demos/Demo.cs | 2 +- Demos/DemoSet.cs | 2 +- Demos/SpecializedTests/TaskQueueTestDemo.cs | 36 + .../SpecializedTests/TreeFiddlingTestDemo.cs | 4 +- 6 files changed, 951 insertions(+), 4 deletions(-) create mode 100644 BepuUtilities/LinkedTaskStack.cs diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index b360573ba..15cdba4ee 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -4,6 +4,7 @@ using BepuUtilities; using BepuUtilities.Collections; using BepuUtilities.Memory; +using BepuUtilities.TestQueue; using System; using System.ComponentModel; using System.Diagnostics; @@ -561,6 +562,7 @@ public CentroidPrepassTaskContext(BufferPool pool, SharedTaskData taskData, Buff { TaskData = taskData; pool.Take(int.Min(taskData.WorkerCount, taskData.TaskCount), out PrepassWorkers); + Debug.Assert(PrepassWorkers.Length >= 2); Bounds = bounds; } @@ -569,6 +571,7 @@ public CentroidPrepassTaskContext(BufferPool pool, SharedTaskData taskData, Buff unsafe static void CentroidPrepassWorker(long taskId, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) { ref var context = ref *(CentroidPrepassTaskContext*)untypedContext; + Debug.Assert(context.TaskData.WorkerCount > 1 && context.TaskData.TaskCount > 1 && context.TaskData.WorkerCount < 100); context.TaskData.GetSlotInterval(taskId, out var start, out var count); var centroidBounds = ComputeCentroidBounds(context.Bounds.Slice(start, count)); if (context.TaskData.TaskCountFitsInWorkerCount) @@ -590,6 +593,8 @@ unsafe static BoundingBox4 MultithreadedCentroidPrepass(Multithread ref var worker = ref context->Workers[workerIndex]; var workerPool = dispatcher.WorkerPools[workerIndex]; var taskContext = new CentroidPrepassTaskContext(workerPool, new SharedTaskData(context->Workers.Length, subtreeStartIndex, subtreeCount, SubtreesPerThreadForCentroidPrepass, context->MaximumTaskCountPerSubmission), worker.Bounds); + Debug.Assert(taskContext.TaskData.TaskCount > 1, "This codepath shouldn't be used if there's only one task!"); + Debug.Assert(taskContext.TaskData.WorkerCount > 1 && taskContext.TaskData.WorkerCount < 100); var taskCount = taskContext.TaskData.TaskCount; //Don't bother initializing more slots than we have tasks. Note that this requires special handling on the task level; //if we have less tasks than workers, then the task needs to distinguish that fact. @@ -604,6 +609,7 @@ unsafe static BoundingBox4 MultithreadedCentroidPrepass(Multithread workerBounds.Max = new Vector4(float.MinValue); } } + Debug.Assert(taskContext.TaskData.TaskCount > 0 && taskContext.TaskData.WorkerCount > 0); context->Queue->For(&CentroidPrepassWorker, &taskContext, 0, taskCount, workerIndex, dispatcher); var centroidBounds = taskContext.PrepassWorkers[0]; @@ -709,6 +715,7 @@ static void BinSubtrees(Vector4 centroidBoundsMin, unsafe static void BinSubtreesWorker(long taskId, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) where TLeafCounts : unmanaged, ILeafCountBuffer { ref var context = ref *(BinSubtreesTaskContext*)untypedContext; + Debug.Assert(context.TaskData.WorkerCount > 1 && context.TaskData.TaskCount > 1 && context.TaskData.WorkerCount < 100); //Note that if we have more workers than tasks, we use the task id to index into the caches (and initialize the data here rather then before dispatching). var effectiveWorkerIndex = context.TaskData.TaskCountFitsInWorkerCount ? (int)taskId : workerIndex; ref var worker = ref context.BinSubtreesWorkers[effectiveWorkerIndex]; diff --git a/BepuUtilities/LinkedTaskStack.cs b/BepuUtilities/LinkedTaskStack.cs new file mode 100644 index 000000000..6f384c858 --- /dev/null +++ b/BepuUtilities/LinkedTaskStack.cs @@ -0,0 +1,904 @@ +using System; +using System.ComponentModel; +using System.Diagnostics; +using System.Diagnostics.CodeAnalysis; +using System.Numerics; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; +using System.Threading; +using BepuUtilities.Collections; +using BepuUtilities.Memory; + +namespace BepuUtilities.TestLinkedTaskStack; + +/// +/// Description of a task to be submitted to a . +/// +public unsafe struct Task +{ + /// + /// Function to be executed by the task. Takes as arguments the , pointer, and executing worker index. + /// + public delegate* Function; + /// + /// Context to be passed into the . + /// + public void* Context; + /// + /// Continuation to be notified after this task completes, if any. + /// + public ContinuationHandle Continuation; + /// + /// User-provided identifier of this task. + /// + public long Id; + + /// + /// Creates a new task. + /// + /// Function to be executed by the task. Takes as arguments the , pointer, executing worker index, and executing . + /// Context pointer to pass to the . + /// Id of this task to be passed into the . + /// Continuation to notify after the completion of this task, if any. + public Task(delegate* function, void* context = null, long taskId = 0, ContinuationHandle continuation = default) + { + Function = function; + Context = context; + Continuation = continuation; + Id = taskId; + } + + /// + /// Creates a task from a function. + /// + /// Function to turn into a task. + public static implicit operator Task(delegate* function) => new(function); + + /// + /// Runs the task and, if necessary, notifies the associated continuation of its completion. + /// + /// Worker index to pass to the function. + /// Dispatcher running this task. + public void Run(int workerIndex, IThreadDispatcher dispatcher) + { + Debug.Assert(!Continuation.Completed && (Function != null)); + Function(Id, Context, workerIndex, dispatcher); + if (Continuation.Initialized) + Continuation.NotifyTaskCompleted(workerIndex, dispatcher); + } +} + +/// +/// Describes the result status of a pop attempt. +/// +public enum PopTaskResult +{ + /// + /// A task was successfully popped. + /// + Success, + /// + /// The stack was empty, but may have more tasks in the future. + /// + Empty, + /// + /// The stack has been terminated and all threads seeking work should stop. + /// + Stop +} + +/// +/// Refers to a continuation within a . +/// +public unsafe struct ContinuationHandle : IEquatable +{ + uint index; + uint encodedVersion; + /// + /// Source worker task stack. + /// + internal Worker* Worker; + + internal ContinuationHandle(uint index, int version, Worker* workerStack) + { + this.index = index; + encodedVersion = (uint)version | 1u << 31; + Worker = workerStack; + } + + internal uint Index + { + get + { + Debug.Assert(Initialized, "If you're trying to pull a continuation id from a continuation handle, it should have been initialized."); + return index; + } + } + + internal int Version + { + get + { + Debug.Assert(Initialized, "If you're trying to pull a continuation id from a continuation handle, it should have been initialized."); + return (int)(encodedVersion & ((1u << 31) - 1)); + } + } + + /// + /// Gets whether the tasks associated with this continuation have completed. If the continuation has not been initialized, this will always return false. + /// + public bool Completed + { + get + { + Debug.Assert(Initialized == (Worker != null)); + return Initialized && Worker->IsComplete(this); + } + } + + /// + /// Retrieves a pointer to the continuation data for . + /// + /// Pointer to the continuation backing the given handle. + /// This should not be used if the continuation handle is not known to be valid. The data pointed to by the data could become invalidated if the continuation completes. + public TaskContinuation* Continuation => Worker->GetContinuation(this); + + /// + /// Gets a null continuation handle. + /// + public static ContinuationHandle Null => default; + + /// + /// Gets whether the continuation associated with this handle was allocated and has outstanding tasks. + /// + public bool Exists + { + get + { + if (!Initialized || Worker == null) + return false; + ref var continuation = ref Worker->Continuations[Index]; + return continuation.Version == Version && continuation.RemainingTaskCounter > 0; + } + } + + /// + /// Gets whether this handle ever represented an allocated handle. This does not guarantee that the continuation's associated tasks are active in the that it was allocated from. + /// + public bool Initialized => encodedVersion >= 1u << 31; + + /// + /// Notifies the continuation that one task was completed. + /// + /// Worker index to pass to the continuation's delegate, if any. + /// Dispatcher to pass to the continuation's delegate, if any. + public void NotifyTaskCompleted(int workerIndex, IThreadDispatcher dispatcher) + { + var continuation = Continuation; + Debug.Assert(!Completed); + var counter = Interlocked.Decrement(ref continuation->RemainingTaskCounter); + Debug.Assert(counter >= 0, "The counter should not go negative. Was notify called too many times?"); + if (counter == 0) + { + //This entire job has completed. + if (continuation->OnCompleted.Function != null) + { + continuation->OnCompleted.Function(continuation->OnCompleted.Id, continuation->OnCompleted.Context, workerIndex, dispatcher); + } + //Free this continuation slot. + var waiter = new SpinWait(); + while (Interlocked.CompareExchange(ref Worker->ContinuationLocker, 1, 0) != 0) + { + waiter.SpinOnce(-1); + } + //We have the lock. + Worker->ContinuationIndexPool.ReturnUnsafely((int)Index); + --Worker->ContinuationCount; + Worker->ContinuationLocker = 0; + } + } + + public bool Equals(ContinuationHandle other) => other.index == index && other.encodedVersion == encodedVersion && other.Worker == Worker; + + public override bool Equals([NotNullWhen(true)] object obj) => obj is ContinuationHandle handle && Equals(handle); + + public override int GetHashCode() => (int)(index ^ (encodedVersion << 24)); + + public static bool operator ==(ContinuationHandle left, ContinuationHandle right) => left.Equals(right); + + public static bool operator !=(ContinuationHandle left, ContinuationHandle right) => !(left == right); +} + +[StructLayout(LayoutKind.Explicit, Size = 280)] +internal unsafe struct Job +{ + [FieldOffset(0)] + public Buffer Tasks; + [FieldOffset(16)] + public Job* Previous; + + [FieldOffset(152)] + public int Counter; + + public static Job* Create(Span sourceTasks, BufferPool pool) + { + //Note that the job and the buffer of tasks are allocated together as one block. + //This ensures we only need to perform one + var sizeToAllocate = sizeof(Job) + sourceTasks.Length * sizeof(Task); + pool.Take(sizeToAllocate, out var rawBuffer); + Job* job = (Job*)rawBuffer.Memory; + job->Tasks = new Buffer(rawBuffer.Memory + sizeof(Job), sourceTasks.Length, rawBuffer.Id); + sourceTasks.CopyTo(job->Tasks); + job->Counter = sourceTasks.Length; + job->Previous = null; + for (int i = 0; i < job->Tasks.Length; ++i) + { + Debug.Assert(job->Tasks[i].Function != null); + } + return job; + } + + + /// + /// Attempts to pop a task from the job. + /// + /// Task popped from the job, if any. + /// True if a task was available to pop, false otherwise. + internal bool TryPop(out Task task) + { + for (int i = 0; i < Tasks.Length; ++i) + { + Debug.Assert(Tasks[i].Function != null); + } + var newCount = Interlocked.Decrement(ref Counter); + if (newCount >= 0) + { + task = Tasks[newCount]; + Debug.Assert(task.Function != null); + return true; + } + task = default; + return false; + } + + public void Dispose(BufferPool pool) + { + //The instance is allocated from the same memory as the tasks buffer, so disposing it returns the Job memory too. + var id = Tasks.Id; + this = default; + pool.ReturnUnsafely(id); + } +} + +internal unsafe struct Worker +{ + //The worker needs to track allocations made over the course of its lifetime so they can be disposed later. + public QuickList AllocatedJobs; + + public Buffer Continuations; + public IdPool ContinuationIndexPool; + public int ContinuationCount; + public int WorkerIndex; + public volatile int ContinuationLocker; + + + [Conditional("DEBUG")] + public void ValidateTasks() + { + for (int i = 0; i < AllocatedJobs.Count; ++i) + { + var job = (Job*)AllocatedJobs[i]; + for (int j = 0; j < job->Tasks.length; ++j) + { + Debug.Assert(job->Tasks[j].Function != null); + } + } + } + + public Worker(int workerIndex, IThreadDispatcher dispatcher, int initialJobCapacity = 64, int initialContinuationCapacity = 16) + { + var threadPool = dispatcher.WorkerPools[workerIndex]; + WorkerIndex = workerIndex; + AllocatedJobs = new QuickList(initialJobCapacity, threadPool); + threadPool.Take(initialContinuationCapacity, out Continuations); +#if DEBUG + //While you shouldn't *need* to clear continuations, it can be useful for debug purposes. + Continuations.Clear(0, Continuations.length); +#endif + ContinuationIndexPool = new IdPool(initialContinuationCapacity, threadPool); + } + + public void Dispose(BufferPool threadPool) + { + ValidateTasks(); + Console.WriteLine($"Dispose {WorkerIndex}"); + for (int i = 0; i < AllocatedJobs.Count; ++i) + { + ((Job*)AllocatedJobs[i])->Dispose(threadPool); + } + AllocatedJobs.Dispose(threadPool); + threadPool.Return(ref Continuations); + ContinuationIndexPool.Dispose(threadPool); + } + + internal void Reset(BufferPool threadPool) + { + ValidateTasks(); + Console.WriteLine($"Reset {WorkerIndex}"); + for (int i = 0; i < AllocatedJobs.Count; ++i) + { + ((Job*)AllocatedJobs[i])->Dispose(threadPool); + } + AllocatedJobs.Count = 0; +#if DEBUG + //While you shouldn't *need* to clear continuations, it can be useful for debug purposes. + Continuations.Clear(0, Continuations.length); +#endif + ContinuationCount = 0; + Debug.Assert(ContinuationLocker == 0, "There appears to be a thread actively working still. That's invalid."); + } + + /// + /// Pushes a set of tasks onto the stack. + /// + /// Tasks composing the job. + /// Dispatcher used to pull thread allocations if necessary. + /// If the worker associated with this stack might be active, this function can only be called by the worker. + internal Job* AllocateJob(Span tasks, IThreadDispatcher dispatcher) + { + Console.WriteLine($"AllocateJob {WorkerIndex}"); + ValidateTasks(); + Debug.Assert(tasks.Length > 0, "Probably shouldn't be trying to push zero tasks."); + var threadPool = dispatcher.WorkerPools[WorkerIndex]; + //Note that we allocate jobs on the heap directly; it's safe to resize the AllocatedJobs list because it's just storing pointers. + var job = Job.Create(tasks, threadPool); + AllocatedJobs.Allocate(threadPool) = (nuint)job; + return job; + } + + + /// + /// Attempts to allocate a continuation for a set of tasks. + /// + /// Number of tasks associated with the continuation. + /// Dispatcher from which to pull a buffer pool if needed for resizing. + /// Function to execute upon completing all associated tasks, if any. Any task with a null will not be executed. + /// Handle of the continuation if allocation is successful. + /// True if the continuation was allocated, false if the attempt was contested.. + public bool TryAllocateContinuation(int taskCount, IThreadDispatcher dispatcher, out ContinuationHandle continuationHandle, Task onCompleted = default) + { + ValidateTasks(); + continuationHandle = default; + if (Interlocked.CompareExchange(ref ContinuationLocker, 1, 0) != 0) + return false; + try + { + //We have the lock. + if (ContinuationCount == Continuations.length) + { + //Doing a resize within a lock is *really* not great. This is something to be avoided via preallocation whenever possible. + //BUT: + //1. It should be trivial to make these resizes effectively never happen. + //2. In the cases where it happens anyway, suffering the pain of a resize in a lock is the lesser of two evils. + //In the case where we *didn't* resize, we'd have to either stall on the allocation attempt or cycle on other tasks until continuations are freed up. + //But there's no guarantee that running tasks will actually free up continuations on net- they could make more! + //So having the fallback plan of expanding storage for more continuations avoids deadlock prone options. + //Note that *ONLY THE OWNING WORKER* can ever validly perform this resize! + //We have to use the thread's buffer pool, and only the thread can validly access that pool. + var workerPool = dispatcher.WorkerPools[WorkerIndex]; + workerPool.ResizeToAtLeast(ref Continuations, ContinuationCount * 2, ContinuationCount); + ContinuationIndexPool.Resize(Continuations.length, workerPool); +#if DEBUG + //While you shouldn't *need* to clear continuations, it can be useful for debug purposes. + Continuations.Clear(ContinuationCount, Continuations.length - ContinuationCount); +#endif + } + var index = ContinuationIndexPool.Take(); + ++ContinuationCount; + ref var continuation = ref Continuations[index]; + //Note that the version number could be based on undefined data initially. That's actually fine; all we care about is whether it is different. + //Note mask to leave a valid bit for encoding in the handle. + var newVersion = (continuation.Version + 1) & (~(1 << 31)); + continuation.OnCompleted = onCompleted; + continuation.Version = newVersion; + continuation.RemainingTaskCounter = taskCount; + continuationHandle = new ContinuationHandle((uint)index, newVersion, (Worker*)Unsafe.AsPointer(ref this)); + return true; + } + finally + { + ContinuationLocker = 0; + } + } + + + /// + /// Allocates a continuation for a set of tasks. + /// + /// Number of tasks associated with the continuation. + /// Dispatcher from which to pull any thread allocations if necessary. + /// Task to execute upon completing all associated tasks, if any. Any task with a null will not be executed. + /// Handle of the allocated continuation. + /// Note that this will keep trying until allocation succeeds. If something is blocking allocation, such as insufficient room in the continuations buffer and there are no workers consuming tasks, this will block forever. + public ContinuationHandle AllocateContinuation(int taskCount, IThreadDispatcher dispatcher, Task onCompleted = default) + { + var waiter = new SpinWait(); + ContinuationHandle handle; + while (!TryAllocateContinuation(taskCount, dispatcher, out handle, onCompleted)) + { + waiter.SpinOnce(-1); + } + return handle; + } + + /// + /// Retrieves a pointer to the continuation data for . + /// + /// Handle to look up the associated continuation for. + /// Pointer to the continuation backing the given handle. + /// This should not be used if the continuation handle is not known to be valid. The data pointed to by the data could become invalidated if the continuation completes. + public TaskContinuation* GetContinuation(ContinuationHandle continuationHandle) + { + ValidateTasks(); + Debug.Assert(continuationHandle.Initialized, "This continuation handle was never initialized."); + Debug.Assert(continuationHandle.Index < Continuations.length, "This continuation refers to an invalid index."); + if (continuationHandle.Index >= Continuations.length || !continuationHandle.Initialized) + return null; + var continuation = Continuations.Memory + continuationHandle.Index; + Debug.Assert(continuation->Version == continuationHandle.Version, "This continuation no longer refers to an active continuation."); + if (continuation->Version != continuationHandle.Version) + return null; + return Continuations.Memory + continuationHandle.Index; + } + + /// + /// Checks whether all tasks associated with this continuation have completed. + /// + /// Continuation to check for completion. + /// True if all tasks associated with a continuation have completed, false otherwise. + public bool IsComplete(ContinuationHandle continuationHandle) + { + ValidateTasks(); + Debug.Assert(continuationHandle.Initialized, "This continuation handle was never initialized."); + Debug.Assert(continuationHandle.Index < Continuations.length, "This continuation refers to an invalid index."); + if (continuationHandle.Index >= Continuations.length || !continuationHandle.Initialized) + return false; + ref var continuation = ref Continuations[continuationHandle.Index]; + return continuation.Version > continuationHandle.Version || continuation.RemainingTaskCounter == 0; + } + +} + +/// +/// Stores data relevant to tracking task completion and reporting completion for a continuation. +/// +public unsafe struct TaskContinuation +{ + /// + /// Task to run upon completion of the associated task. + /// + public Task OnCompleted; + /// + /// Version of this continuation. + /// + public int Version; + /// + /// Number of tasks not yet reported as complete in the continuation. + /// + public int RemainingTaskCounter; +} + + +/// +/// Manages a linked stack of tasks. +/// +public unsafe struct LinkedTaskStack +{ + Buffer workers; + + [StructLayout(LayoutKind.Explicit, Size = 256 + 16)] + struct StopPad + { + [FieldOffset(128)] + public volatile bool Stop; + } + StopPad padded; + + /// + /// Most recently pushed job on the stack. May be null if the stack is empty. + /// + Job* head; + + /// + /// Constructs a new parallel task stack. + /// + /// Buffer pool to allocate non-thread allocated resources from. + /// Thread dispatcher to pull thread pools from for thread allocations. + /// Number of workers to allocate space for. + /// Initial number of jobs (groups of tasks submitted together) to allocate space for in each worker. + /// Initial number of continuations to allocate space for in each worker. + public LinkedTaskStack(BufferPool pool, IThreadDispatcher dispatcher, int workerCount, int initialWorkerJobCapacity = 64, int initialWorkerContinuationCapacity = 16) + { + pool.Take(workerCount, out workers); + for (int i = 0; i < workerCount; ++i) + { + workers[i] = new Worker(i, dispatcher, initialWorkerJobCapacity, initialWorkerContinuationCapacity); + } + Reset(dispatcher); + } + + /// + /// Returns the stack to a fresh state without reallocating. + /// + /// Dispatcher whose thread pools should be used to return any thread allocated resources. + public void Reset(IThreadDispatcher dispatcher) + { + for (int i = 0; i < workers.Length; ++i) + { + workers[i].Reset(dispatcher.WorkerPools[workers[i].WorkerIndex]); + } + padded.Stop = false; + } + + /// + /// Returns unmanaged resources held by the to a pool. + /// + /// Buffer pool to return resources to. + /// Dispatcher whose thread pools should be used to return any thread allocated resources. + public void Dispose(BufferPool pool, IThreadDispatcher dispatcher) + { + for (int i = 0; i < workers.Length; ++i) + { + workers[i].Dispose(dispatcher.WorkerPools[workers[i].WorkerIndex]); + } + pool.Return(ref workers); + } + + /// + /// Gets the approximate number of active tasks. This is not guaranteed to actually measure the true number of tasks at any one point in time. + /// + public int ApproximateTaskCount + { + get + { + int sum = 0; + var job = this.head; + while (true) + { + if (job == null) + break; + sum += int.Max(0, job->Counter); + job = job->Previous; + } + return sum; + } + } + /// + /// Gets the approximate number of active continuations. This is not guaranteed to actually measure the true number of continuations at any one point in time; it checks each worker in sequence, and the continuation counts could vary arbitrarily as the checks proceed. + /// + public int ApproximateContinuationCount + { + get + { + int sum = 0; + for (int i = 0; i < workers.Length; ++i) + { + sum += workers[i].ContinuationCount; + } + return sum; + } + } + + + /// + /// Attempts to allocate a continuation for a set of tasks. + /// + /// Number of tasks associated with the continuation. + /// Worker index to allocate the continuation on. + /// Dispatcher to use for any per-thread allocations if necessary. + /// Function to execute upon completing all associated tasks, if any. Any task with a null will not be executed. + /// Handle of the continuation if allocation is successful. + /// True if the allocation succeeded, false if it was contested.. + public bool TryAllocateContinuation(int taskCount, int workerIndex, IThreadDispatcher dispatcher, out ContinuationHandle continuationHandle, Task onCompleted = default) + { + return workers[workerIndex].TryAllocateContinuation(taskCount, dispatcher, out continuationHandle, onCompleted); + } + + /// + /// Allocates a continuation for a set of tasks. + /// + /// Number of tasks associated with the continuation. + /// Worker index to allocate the continuation on. + /// Dispatcher to use for any per-thread allocations if necessary. + /// Task to execute upon completing all associated tasks, if any. Any task with a null will not be executed. + /// Handle of the allocated continuation. + public ContinuationHandle AllocateContinuation(int taskCount, int workerIndex, IThreadDispatcher dispatcher, Task onCompleted = default) + { + Console.WriteLine($"AllocateContinuation {workerIndex}"); + return workers[workerIndex].AllocateContinuation(taskCount, dispatcher, onCompleted); + } + + /// + /// Attempts to pop a task. + /// + /// Popped task, if any. + /// Result status of the pop attempt. + public PopTaskResult TryPop(out Task task) + { + //Note that this implementation does not need to lock against anything. We just follow the pointer. + int counter = 0; + Console.WriteLine("TryPop"); + while (true) + { + var job = head; + if (job == null) + { + //There is no job to pop from. + task = default; + return padded.Stop ? PopTaskResult.Stop : PopTaskResult.Empty; + } + //The sampled head exists. Try to pop a task from it. + if (job->TryPop(out task)) + { + Debug.Assert(task.Function != null); + return PopTaskResult.Success; + } + else + { + task = default; + return padded.Stop ? PopTaskResult.Stop : PopTaskResult.Empty; + //There was no task available in this job, which means the sampled job should be removed from the stack. + //Note that other threads might be doing the same thing; we must use an interlocked operation to try to swap the head. + var previousHead = head; + var scusy = Interlocked.CompareExchange(ref Unsafe.AsRef(head), (ulong)job->Previous, (ulong)job); + ++counter; + if (counter > 100000) + Console.WriteLine($"ughh {counter}"); + + } + } + } + + /// + /// Attempts to pop a task and run it. + /// + /// Index of the worker to pass into the task function. + /// Thread dispatcher running this task stack. + /// Result status of the pop attempt. + public PopTaskResult TryPopAndRun(int workerIndex, IThreadDispatcher dispatcher) + { + Console.WriteLine($"TryPopAndRun {workerIndex}"); + workers[workerIndex].ValidateTasks(); + var result = TryPop(out var task); + if (result == PopTaskResult.Success) + { + task.Run(workerIndex, dispatcher); + } + workers[workerIndex].ValidateTasks(); + return result; + } + + /// + /// Pushes a set of tasks onto the task stack. This function is not thread safe. + /// + /// Tasks composing the job. + /// Thread dispatcher to allocate thread data from if necessary. + /// Index of the worker stack to push the tasks onto. + /// This must not be used while other threads could be performing task pushes or pops that could affect the specified worker. + public void PushUnsafely(Span tasks, int workerIndex, IThreadDispatcher dispatcher) + { + Console.WriteLine($"PushUnsafely {workerIndex}"); + workers[workerIndex].ValidateTasks(); + var job = workers[workerIndex].AllocateJob(tasks, dispatcher); + job->Previous = head; + head = job; + workers[workerIndex].ValidateTasks(); + } + /// + /// Pushes a task onto the task stack. This function is not thread safe. + /// + /// Task to push. + /// Thread dispatcher to allocate thread data from if necessary. + /// Index of the worker stack to push the tasks onto. + /// This must not be used while other threads could be performing task pushes or pops that could affect the specified worker. + public unsafe void PushUnsafely(Task task, int workerIndex, IThreadDispatcher dispatcher) + { + PushUnsafely(new Span(&task, 1), workerIndex, dispatcher); + } + + /// + /// Pushes a set of tasks onto the task stack. + /// + /// Tasks composing the job. + /// Thread dispatcher to allocate thread data from if necessary. + /// Index of the worker stack to push the tasks onto. + /// True if the push succeeded, false if it was contested. + public void Push(Span tasks, int workerIndex, IThreadDispatcher dispatcher) + { + Console.WriteLine($"Push {workerIndex}"); + workers[workerIndex].ValidateTasks(); + var job = workers[workerIndex].AllocateJob(tasks, dispatcher); + //Note that we assign a previous pointer *before* doing the "official" swap because we don't want to risk a pop seeing a null previous pointer. + //(There's a brief gap between the swap and the assignment. This doesn't actually matter much; it would just result in a false empty.) + job->Previous = head; + job->Previous = (Job*)Interlocked.Exchange(ref Unsafe.AsRef(head), (nuint)job); + workers[workerIndex].ValidateTasks(); + } + + + /// + /// Pushes a set of tasks to the stack with a created continuation. + /// + /// Tasks composing the job. A continuation will be assigned internally; no continuation should be present on any of the provided tasks. + /// Thread dispatcher to allocate thread data from if necessary. + /// Index of the worker stack to push the tasks onto. + /// Task to run upon completion of all the submitted tasks, if any. + /// Handle of the continuation created for these tasks. + /// Note that this will keep trying until task submission succeeds. + public ContinuationHandle AllocateContinuationAndPush(Span tasks, int workerIndex, IThreadDispatcher dispatcher, Task onComplete = default) + { + Console.WriteLine($"AllocateContinuationAndPush {workerIndex}"); + workers[workerIndex].ValidateTasks(); + var continuationHandle = AllocateContinuation(tasks.Length, workerIndex, dispatcher, onComplete); + for (int i = 0; i < tasks.Length; ++i) + { + ref var task = ref tasks[i]; + Debug.Assert(!task.Continuation.Initialized, "This function creates a continuation for the tasks"); + task.Continuation = continuationHandle; + } + Push(tasks, workerIndex, dispatcher); + workers[workerIndex].ValidateTasks(); + return continuationHandle; + } + + /// + /// Waits for a continuation to be completed. + /// + /// Instead of spinning the entire time, this may pop and execute pending tasks to fill the gap. + /// Continuation to wait on. + /// Thread dispatcher to allocate thread data from if necessary. + /// Index of the executing worker. + public void WaitForCompletion(ContinuationHandle continuation, int workerIndex, IThreadDispatcher dispatcher) + { + workers[workerIndex].ValidateTasks(); + Console.WriteLine($"WaitForCompletion {workerIndex}"); + var waiter = new SpinWait(); + Debug.Assert(continuation.Initialized, "This codepath should only run if the continuation was allocated earlier."); + while (!continuation.Completed) + { + var result = TryPop(out var fillerTask); + if (result == PopTaskResult.Stop) + { + return; + } + if (result == PopTaskResult.Success) + { + fillerTask.Run(workerIndex, dispatcher); + waiter.Reset(); + } + else + { + waiter.SpinOnce(-1); + } + } + workers[workerIndex].ValidateTasks(); + } + + /// + /// Pushes a set of tasks to the worker stack and returns when all tasks are complete. + /// + /// Tasks composing the job. A continuation will be assigned internally; no continuation should be present on any of the provided tasks. + /// Index of the worker executing this function. + /// Thread dispatcher to allocate thread data from if necessary. + /// Note that this will keep working until all tasks are run. It may execute tasks unrelated to the requested tasks while waiting on other workers to complete constituent tasks. + public void RunTasks(Span tasks, int workerIndex, IThreadDispatcher dispatcher) + { + Console.WriteLine($"RunTasks {workerIndex}"); + workers[workerIndex].ValidateTasks(); + if (tasks.Length == 0) + return; + ContinuationHandle continuationHandle = default; + if (tasks.Length > 1) + { + //Note that we only submit tasks to the stack for tasks beyond the first. The current thread is responsible for at least task 0. + var taskCount = tasks.Length - 1; + Span tasksToPush = stackalloc Task[taskCount]; + ref var worker = ref workers[workerIndex]; + continuationHandle = worker.AllocateContinuation(taskCount, dispatcher); + for (int i = 0; i < tasksToPush.Length; ++i) + { + var task = tasks[i + 1]; + Debug.Assert(continuationHandle.Worker == workers.Memory + workerIndex); + Debug.Assert(!task.Continuation.Initialized, $"None of the source tasks should have continuations when provided to {nameof(RunTasks)}."); + task.Continuation = continuationHandle; + tasksToPush[i] = task; + } + Push(tasksToPush, workerIndex, dispatcher); + } + //Tasks [1, count) are submitted to the stack and may now be executing on other workers. + //The thread calling the for loop should not relinquish its timeslice. It should immediately begin working on task 0. + var task0 = tasks[0]; + Debug.Assert(!task0.Continuation.Initialized, $"None of the source tasks should have continuations when provided to {nameof(RunTasks)}."); + task0.Function(task0.Id, task0.Context, workerIndex, dispatcher); + + if (tasks.Length > 1) + { + //Task 0 is done; this thread should seek out other work until the job is complete. + WaitForCompletion(continuationHandle, workerIndex, dispatcher); + } + workers[workerIndex].ValidateTasks(); + } + + /// + /// Requests that all workers stop. The next time a worker runs out of tasks to run, if it sees a stop command, it will be reported. + /// + public void RequestStop() + { + padded.Stop = true; + } + + + /// + /// Pushes a for loop onto the task stack. Does not take a lock. + /// + /// Function to execute on each iteration of the loop. + /// Context pointer to pass into each task execution. + /// Inclusive start index of the loop range. + /// Number of iterations to perform. + /// Thread dispatcher to allocate thread data from if necessary. + /// Index of the worker stack to push the tasks onto. + /// Continuation associated with the loop tasks, if any. + /// This must not be used while other threads could be performing task pushes or pops that could affect the specified worker. + public void PushForUnsafely(delegate* function, void* context, int inclusiveStartIndex, int iterationCount, int workerIndex, IThreadDispatcher dispatcher, ContinuationHandle continuation = default) + { + Span tasks = stackalloc Task[iterationCount]; + for (int i = 0; i < tasks.Length; ++i) + { + tasks[i] = new Task { Function = function, Context = context, Id = i + inclusiveStartIndex, Continuation = continuation }; + } + PushUnsafely(tasks, workerIndex, dispatcher); + } + + /// + /// Pushes a for loop onto the task stack. + /// + /// Function to execute on each iteration of the loop. + /// Context pointer to pass into each task execution. + /// Inclusive start index of the loop range. + /// Number of iterations to perform. + /// Thread dispatcher to allocate thread data from if necessary. + /// Index of the worker stack to push the tasks onto. + /// Continuation associated with the loop tasks, if any. + /// This function will not usually attempt to run any iterations of the loop itself. It tries to push the loop tasks onto the stack. + /// If the task stack is full, this will opt to run the tasks inline while waiting for room. + public void PushFor(delegate* function, void* context, int inclusiveStartIndex, int iterationCount, int workerIndex, IThreadDispatcher dispatcher, ContinuationHandle continuation = default) + { + Span tasks = stackalloc Task[iterationCount]; + for (int i = 0; i < tasks.Length; ++i) + { + tasks[i] = new Task { Function = function, Context = context, Id = i + inclusiveStartIndex, Continuation = continuation }; + } + Push(tasks, workerIndex, dispatcher); + } + + /// + /// Submits a set of tasks representing a for loop over the given indices and returns when all loop iterations are complete. + /// + /// Function to execute on each iteration of the loop. + /// Context pointer to pass into each iteration of the loop. + /// Inclusive start index of the loop range. + /// Number of iterations to perform. + /// Thread dispatcher to allocate thread data from if necessary. + /// Index of the worker stack to push the tasks onto. + public void For(delegate* function, void* context, int inclusiveStartIndex, int iterationCount, int workerIndex, IThreadDispatcher dispatcher) + { + if (iterationCount <= 0) + return; + Span tasks = stackalloc Task[iterationCount]; + for (int i = 0; i < tasks.Length; ++i) + { + tasks[i] = new Task(function, context, inclusiveStartIndex + i); + } + RunTasks(tasks, workerIndex, dispatcher); + } +} diff --git a/Demos/Demo.cs b/Demos/Demo.cs index c65faca66..264961f51 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -43,7 +43,7 @@ protected Demo() //there won't be enough memory bandwidth to even feed half the physical cores. Using all 128 logical cores would just add overhead. //It may be worth using something like hwloc or CPUID to extract extra information to reason about. - var targetThreadCount = int.Max(1, Environment.ProcessorCount > 4 ? Environment.ProcessorCount - 2 : Environment.ProcessorCount - 1); + var targetThreadCount = 2;// int.Max(1, Environment.ProcessorCount > 4 ? Environment.ProcessorCount - 2 : Environment.ProcessorCount - 1); ThreadDispatcher = new ThreadDispatcher(targetThreadCount); } diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index a59471775..a80e8370a 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -47,7 +47,7 @@ struct Option public DemoSet() { //AddOption(); - AddOption(); + //AddOption(); AddOption(); AddOption(); AddOption(); diff --git a/Demos/SpecializedTests/TaskQueueTestDemo.cs b/Demos/SpecializedTests/TaskQueueTestDemo.cs index a2f340aba..32d37a033 100644 --- a/Demos/SpecializedTests/TaskQueueTestDemo.cs +++ b/Demos/SpecializedTests/TaskQueueTestDemo.cs @@ -14,6 +14,7 @@ using BepuUtilities.TestParallelStack; using TaskQ = BepuUtilities.TestQueue.Task; using BepuUtilities.TestStack; +using BepuUtilities.TestLinkedTaskStack; namespace Demos.SpecializedTests; @@ -88,6 +89,12 @@ static void Test(long taskId, void* context, int workerIndex, IThreadDispatch stack->For(&DynamicallyEnqueuedTest1, &context1, 0, subtaskCount, workerIndex, dispatcher); stack->For(&DynamicallyEnqueuedTest2, &context2, 0, subtaskCount, workerIndex, dispatcher); } + else if (typeof(T) == typeof(LinkedTaskStack)) + { + var stack = (LinkedTaskStack*)typedContext->TaskPile; + stack->For(&DynamicallyEnqueuedTest1, &context1, 0, subtaskCount, workerIndex, dispatcher); + stack->For(&DynamicallyEnqueuedTest2, &context2, 0, subtaskCount, workerIndex, dispatcher); + } } Interlocked.Add(ref typedContext->Sum, sum); } @@ -129,6 +136,11 @@ static void DispatcherBody(int workerIndex, IThreadDispatcher dispatcher) whe var taskStack = (TaskStack*)dispatcher.UnmanagedContext; while (taskStack->TryPopAndRun(workerIndex, dispatcher) != BepuUtilities.TestStack.PopTaskResult.Stop) ; } + else if (typeof(T) == typeof(LinkedTaskStack)) + { + var taskStack = (LinkedTaskStack*)dispatcher.UnmanagedContext; + while (taskStack->TryPopAndRun(workerIndex, dispatcher) != BepuUtilities.TestLinkedTaskStack.PopTaskResult.Stop) ; + } } struct Context @@ -146,6 +158,8 @@ static void IssueStop(long id, void* context, int workerIndex, IThreadDispatc ((ParallelTaskStack*)typedContext->TaskPile)->RequestStop(); else if (typeof(T) == typeof(TaskStack)) ((TaskStack*)typedContext->TaskPile)->RequestStop(); + else if (typeof(T) == typeof(LinkedTaskStack)) + ((LinkedTaskStack*)typedContext->TaskPile)->RequestStop(); } @@ -174,6 +188,28 @@ public override void Initialize(ContentArchive content, Camera camera) // return 0; //}, "Dispatch"); + //LINKED TASK STACK + var linkedTaskStack = new LinkedTaskStack(BufferPool, ThreadDispatcher, ThreadDispatcher.ThreadCount); + var linkedTaskStackPointer = &linkedTaskStack; + + Test(() => + { + var context = new Context { TaskPile = linkedTaskStackPointer }; + var continuation = linkedTaskStackPointer->AllocateContinuation(iterationCount * tasksPerIteration, 0, ThreadDispatcher, new BepuUtilities.TestLinkedTaskStack.Task(&IssueStop, &context)); + for (int i = 0; i < iterationCount; ++i) + { + linkedTaskStackPointer->PushForUnsafely(&Test, &context, i * tasksPerIteration, tasksPerIteration, 0, ThreadDispatcher, continuation); + } + //taskQueuePointer->TryEnqueueStopUnsafely(); + //taskQueuePointer->EnqueueTasks() + ThreadDispatcher.DispatchWorkers(&DispatcherBody, unmanagedContext: linkedTaskStackPointer); + return context.Sum; + }, "MT Linked Stack", () => linkedTaskStackPointer->Reset(ThreadDispatcher)); + + + linkedTaskStack.Dispose(BufferPool, ThreadDispatcher); + + //TASK STACK var taskStack = new TaskStack(BufferPool, ThreadDispatcher); var taskStackPointer = &taskStack; diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index 656d4d163..fcf537722 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -158,8 +158,8 @@ public override void Initialize(ContentArchive content, Camera camera) Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(4, 1)); //Create a mesh. - var width = 8;// 768; - var height = 8;// 768; + var width = 64;// 768; + var height = 64;// 768; var scale = new Vector3(1, 1, 1); //DemoMeshHelper.CreateDeformedPlane(width, height, (x, y) => new Vector3(x - width * scale.X * 0.5f, 2f * (float)(Math.Sin(x * 0.5f) * Math.Sin(y * 0.5f)), y - height * scale.Y * 0.5f), scale, BufferPool, out var mesh); //DemoMeshHelper.CreateDeformedPlane(width, height, (x, y) => new Vector3(x - width * scale.X * 0.5f, 0, y - height * scale.Y * 0.5f), scale, BufferPool, out var mesh); From bcc7024ad3c25b1ae5dd14616e0997feb0b1f814 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 28 Jan 2023 22:47:25 -0600 Subject: [PATCH 678/947] Fixed LinkedTaskStack. --- BepuUtilities/LinkedTaskStack.cs | 71 +++++++-------------- Demos/Demo.cs | 2 +- Demos/SpecializedTests/TaskQueueTestDemo.cs | 35 +++++----- 3 files changed, 43 insertions(+), 65 deletions(-) diff --git a/BepuUtilities/LinkedTaskStack.cs b/BepuUtilities/LinkedTaskStack.cs index 6f384c858..3253e51ae 100644 --- a/BepuUtilities/LinkedTaskStack.cs +++ b/BepuUtilities/LinkedTaskStack.cs @@ -310,8 +310,6 @@ public Worker(int workerIndex, IThreadDispatcher dispatcher, int initialJobCapac public void Dispose(BufferPool threadPool) { - ValidateTasks(); - Console.WriteLine($"Dispose {WorkerIndex}"); for (int i = 0; i < AllocatedJobs.Count; ++i) { ((Job*)AllocatedJobs[i])->Dispose(threadPool); @@ -323,8 +321,6 @@ public void Dispose(BufferPool threadPool) internal void Reset(BufferPool threadPool) { - ValidateTasks(); - Console.WriteLine($"Reset {WorkerIndex}"); for (int i = 0; i < AllocatedJobs.Count; ++i) { ((Job*)AllocatedJobs[i])->Dispose(threadPool); @@ -346,8 +342,6 @@ internal void Reset(BufferPool threadPool) /// If the worker associated with this stack might be active, this function can only be called by the worker. internal Job* AllocateJob(Span tasks, IThreadDispatcher dispatcher) { - Console.WriteLine($"AllocateJob {WorkerIndex}"); - ValidateTasks(); Debug.Assert(tasks.Length > 0, "Probably shouldn't be trying to push zero tasks."); var threadPool = dispatcher.WorkerPools[WorkerIndex]; //Note that we allocate jobs on the heap directly; it's safe to resize the AllocatedJobs list because it's just storing pointers. @@ -367,7 +361,6 @@ internal void Reset(BufferPool threadPool) /// True if the continuation was allocated, false if the attempt was contested.. public bool TryAllocateContinuation(int taskCount, IThreadDispatcher dispatcher, out ContinuationHandle continuationHandle, Task onCompleted = default) { - ValidateTasks(); continuationHandle = default; if (Interlocked.CompareExchange(ref ContinuationLocker, 1, 0) != 0) return false; @@ -439,7 +432,6 @@ public ContinuationHandle AllocateContinuation(int taskCount, IThreadDispatcher /// This should not be used if the continuation handle is not known to be valid. The data pointed to by the data could become invalidated if the continuation completes. public TaskContinuation* GetContinuation(ContinuationHandle continuationHandle) { - ValidateTasks(); Debug.Assert(continuationHandle.Initialized, "This continuation handle was never initialized."); Debug.Assert(continuationHandle.Index < Continuations.length, "This continuation refers to an invalid index."); if (continuationHandle.Index >= Continuations.length || !continuationHandle.Initialized) @@ -458,7 +450,6 @@ public ContinuationHandle AllocateContinuation(int taskCount, IThreadDispatcher /// True if all tasks associated with a continuation have completed, false otherwise. public bool IsComplete(ContinuationHandle continuationHandle) { - ValidateTasks(); Debug.Assert(continuationHandle.Initialized, "This continuation handle was never initialized."); Debug.Assert(continuationHandle.Index < Continuations.length, "This continuation refers to an invalid index."); if (continuationHandle.Index >= Continuations.length || !continuationHandle.Initialized) @@ -507,7 +498,8 @@ struct StopPad /// /// Most recently pushed job on the stack. May be null if the stack is empty. /// - Job* head; + /// Pointers and generics don't play well, alas. + volatile nuint head; /// /// Constructs a new parallel task stack. @@ -538,6 +530,7 @@ public void Reset(IThreadDispatcher dispatcher) workers[i].Reset(dispatcher.WorkerPools[workers[i].WorkerIndex]); } padded.Stop = false; + head = (nuint)null; } /// @@ -562,7 +555,7 @@ public int ApproximateTaskCount get { int sum = 0; - var job = this.head; + var job = (Job*)this.head; while (true) { if (job == null) @@ -614,7 +607,6 @@ public bool TryAllocateContinuation(int taskCount, int workerIndex, IThreadDispa /// Handle of the allocated continuation. public ContinuationHandle AllocateContinuation(int taskCount, int workerIndex, IThreadDispatcher dispatcher, Task onCompleted = default) { - Console.WriteLine($"AllocateContinuation {workerIndex}"); return workers[workerIndex].AllocateContinuation(taskCount, dispatcher, onCompleted); } @@ -626,11 +618,9 @@ public ContinuationHandle AllocateContinuation(int taskCount, int workerIndex, I public PopTaskResult TryPop(out Task task) { //Note that this implementation does not need to lock against anything. We just follow the pointer. - int counter = 0; - Console.WriteLine("TryPop"); while (true) { - var job = head; + var job = (Job*)head; if (job == null) { //There is no job to pop from. @@ -645,17 +635,13 @@ public PopTaskResult TryPop(out Task task) } else { - task = default; - return padded.Stop ? PopTaskResult.Stop : PopTaskResult.Empty; //There was no task available in this job, which means the sampled job should be removed from the stack. //Note that other threads might be doing the same thing; we must use an interlocked operation to try to swap the head. - var previousHead = head; - var scusy = Interlocked.CompareExchange(ref Unsafe.AsRef(head), (ulong)job->Previous, (ulong)job); - ++counter; - if (counter > 100000) - Console.WriteLine($"ughh {counter}"); - + //If this fails, the head has changed before we could remove it and the current empty job will persists in the stack until some other dequeue finds it. + //That's okay. + Interlocked.CompareExchange(ref head, (nuint)job->Previous, (nuint)job); } + } } @@ -667,14 +653,11 @@ public PopTaskResult TryPop(out Task task) /// Result status of the pop attempt. public PopTaskResult TryPopAndRun(int workerIndex, IThreadDispatcher dispatcher) { - Console.WriteLine($"TryPopAndRun {workerIndex}"); - workers[workerIndex].ValidateTasks(); var result = TryPop(out var task); if (result == PopTaskResult.Success) { task.Run(workerIndex, dispatcher); } - workers[workerIndex].ValidateTasks(); return result; } @@ -687,12 +670,9 @@ public PopTaskResult TryPopAndRun(int workerIndex, IThreadDispatcher dispatcher) /// This must not be used while other threads could be performing task pushes or pops that could affect the specified worker. public void PushUnsafely(Span tasks, int workerIndex, IThreadDispatcher dispatcher) { - Console.WriteLine($"PushUnsafely {workerIndex}"); - workers[workerIndex].ValidateTasks(); - var job = workers[workerIndex].AllocateJob(tasks, dispatcher); - job->Previous = head; - head = job; - workers[workerIndex].ValidateTasks(); + Job* job = workers[workerIndex].AllocateJob(tasks, dispatcher); + job->Previous = (Job*)head; + head = (nuint)job; } /// /// Pushes a task onto the task stack. This function is not thread safe. @@ -715,14 +695,16 @@ public unsafe void PushUnsafely(Task task, int workerIndex, IThreadDispatcher di /// True if the push succeeded, false if it was contested. public void Push(Span tasks, int workerIndex, IThreadDispatcher dispatcher) { - Console.WriteLine($"Push {workerIndex}"); - workers[workerIndex].ValidateTasks(); - var job = workers[workerIndex].AllocateJob(tasks, dispatcher); - //Note that we assign a previous pointer *before* doing the "official" swap because we don't want to risk a pop seeing a null previous pointer. - //(There's a brief gap between the swap and the assignment. This doesn't actually matter much; it would just result in a false empty.) - job->Previous = head; - job->Previous = (Job*)Interlocked.Exchange(ref Unsafe.AsRef(head), (nuint)job); - workers[workerIndex].ValidateTasks(); + Job* job = workers[workerIndex].AllocateJob(tasks, dispatcher); + + while (true) + { + //Pre-set the previous pointer so that it's visible when the job is swapped in. + //Note that if the head pointer changes between the first set attempt and the swap, the previous pointer will be wrong and we must try again. + job->Previous = (Job*)head; + if ((nuint)job->Previous == Interlocked.CompareExchange(ref head, (nuint)job, (nuint)job->Previous)) + break; + } } @@ -737,8 +719,6 @@ public void Push(Span tasks, int workerIndex, IThreadDispatcher dispatcher /// Note that this will keep trying until task submission succeeds. public ContinuationHandle AllocateContinuationAndPush(Span tasks, int workerIndex, IThreadDispatcher dispatcher, Task onComplete = default) { - Console.WriteLine($"AllocateContinuationAndPush {workerIndex}"); - workers[workerIndex].ValidateTasks(); var continuationHandle = AllocateContinuation(tasks.Length, workerIndex, dispatcher, onComplete); for (int i = 0; i < tasks.Length; ++i) { @@ -747,7 +727,6 @@ public ContinuationHandle AllocateContinuationAndPush(Span tasks, int work task.Continuation = continuationHandle; } Push(tasks, workerIndex, dispatcher); - workers[workerIndex].ValidateTasks(); return continuationHandle; } @@ -760,8 +739,6 @@ public ContinuationHandle AllocateContinuationAndPush(Span tasks, int work /// Index of the executing worker. public void WaitForCompletion(ContinuationHandle continuation, int workerIndex, IThreadDispatcher dispatcher) { - workers[workerIndex].ValidateTasks(); - Console.WriteLine($"WaitForCompletion {workerIndex}"); var waiter = new SpinWait(); Debug.Assert(continuation.Initialized, "This codepath should only run if the continuation was allocated earlier."); while (!continuation.Completed) @@ -781,7 +758,6 @@ public void WaitForCompletion(ContinuationHandle continuation, int workerIndex, waiter.SpinOnce(-1); } } - workers[workerIndex].ValidateTasks(); } /// @@ -793,8 +769,6 @@ public void WaitForCompletion(ContinuationHandle continuation, int workerIndex, /// Note that this will keep working until all tasks are run. It may execute tasks unrelated to the requested tasks while waiting on other workers to complete constituent tasks. public void RunTasks(Span tasks, int workerIndex, IThreadDispatcher dispatcher) { - Console.WriteLine($"RunTasks {workerIndex}"); - workers[workerIndex].ValidateTasks(); if (tasks.Length == 0) return; ContinuationHandle continuationHandle = default; @@ -826,7 +800,6 @@ public void RunTasks(Span tasks, int workerIndex, IThreadDispatcher dispat //Task 0 is done; this thread should seek out other work until the job is complete. WaitForCompletion(continuationHandle, workerIndex, dispatcher); } - workers[workerIndex].ValidateTasks(); } /// diff --git a/Demos/Demo.cs b/Demos/Demo.cs index 264961f51..c65faca66 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -43,7 +43,7 @@ protected Demo() //there won't be enough memory bandwidth to even feed half the physical cores. Using all 128 logical cores would just add overhead. //It may be worth using something like hwloc or CPUID to extract extra information to reason about. - var targetThreadCount = 2;// int.Max(1, Environment.ProcessorCount > 4 ? Environment.ProcessorCount - 2 : Environment.ProcessorCount - 1); + var targetThreadCount = int.Max(1, Environment.ProcessorCount > 4 ? Environment.ProcessorCount - 2 : Environment.ProcessorCount - 1); ThreadDispatcher = new ThreadDispatcher(targetThreadCount); } diff --git a/Demos/SpecializedTests/TaskQueueTestDemo.cs b/Demos/SpecializedTests/TaskQueueTestDemo.cs index 32d37a033..caaa6014c 100644 --- a/Demos/SpecializedTests/TaskQueueTestDemo.cs +++ b/Demos/SpecializedTests/TaskQueueTestDemo.cs @@ -77,7 +77,7 @@ static void Test(long taskId, void* context, int workerIndex, IThreadDispatch queue->For(&DynamicallyEnqueuedTest1, &context1, 0, subtaskCount, workerIndex, dispatcher); queue->For(&DynamicallyEnqueuedTest2, &context2, 0, subtaskCount, workerIndex, dispatcher); } - else if(typeof(T) == typeof(ParallelTaskStack)) + else if (typeof(T) == typeof(ParallelTaskStack)) { var stack = (ParallelTaskStack*)typedContext->TaskPile; stack->For(&DynamicallyEnqueuedTest1, &context1, 0, subtaskCount, workerIndex, dispatcher); @@ -121,6 +121,8 @@ static void STTest(long taskId, void* context, int workerIndex, IThreadDispatche static void DispatcherBody(int workerIndex, IThreadDispatcher dispatcher) where T : unmanaged { + //if (workerIndex > 1) + // return; if (typeof(T) == typeof(TaskQueue)) { var taskQueue = (TaskQueue*)dispatcher.UnmanagedContext; @@ -189,25 +191,28 @@ public override void Initialize(ContentArchive content, Camera camera) //}, "Dispatch"); //LINKED TASK STACK - var linkedTaskStack = new LinkedTaskStack(BufferPool, ThreadDispatcher, ThreadDispatcher.ThreadCount); - var linkedTaskStackPointer = &linkedTaskStack; - Test(() => + for (int i = 0; i < 1000; ++i) { - var context = new Context { TaskPile = linkedTaskStackPointer }; - var continuation = linkedTaskStackPointer->AllocateContinuation(iterationCount * tasksPerIteration, 0, ThreadDispatcher, new BepuUtilities.TestLinkedTaskStack.Task(&IssueStop, &context)); - for (int i = 0; i < iterationCount; ++i) + var linkedTaskStack = new LinkedTaskStack(BufferPool, ThreadDispatcher, ThreadDispatcher.ThreadCount); + var linkedTaskStackPointer = &linkedTaskStack; + Test(() => { - linkedTaskStackPointer->PushForUnsafely(&Test, &context, i * tasksPerIteration, tasksPerIteration, 0, ThreadDispatcher, continuation); - } - //taskQueuePointer->TryEnqueueStopUnsafely(); - //taskQueuePointer->EnqueueTasks() - ThreadDispatcher.DispatchWorkers(&DispatcherBody, unmanagedContext: linkedTaskStackPointer); - return context.Sum; - }, "MT Linked Stack", () => linkedTaskStackPointer->Reset(ThreadDispatcher)); + var context = new Context { TaskPile = linkedTaskStackPointer }; + var continuation = linkedTaskStackPointer->AllocateContinuation(iterationCount * tasksPerIteration, 0, ThreadDispatcher, new BepuUtilities.TestLinkedTaskStack.Task(&IssueStop, &context)); + for (int i = 0; i < iterationCount; ++i) + { + linkedTaskStackPointer->PushForUnsafely(&Test, &context, i * tasksPerIteration, tasksPerIteration, 0, ThreadDispatcher, continuation); + } + //taskQueuePointer->TryEnqueueStopUnsafely(); + //taskQueuePointer->EnqueueTasks() + ThreadDispatcher.DispatchWorkers(&DispatcherBody, unmanagedContext: linkedTaskStackPointer); + return context.Sum; + }, "MT Linked Stack", () => linkedTaskStackPointer->Reset(ThreadDispatcher)); + linkedTaskStack.Dispose(BufferPool, ThreadDispatcher); + } - linkedTaskStack.Dispose(BufferPool, ThreadDispatcher); //TASK STACK From 4f0331bb237ec71b9357d492b02f0c39ce210d13 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 2 Feb 2023 19:02:03 -0600 Subject: [PATCH 679/947] Alas badness! --- BepuUtilities/LinkedTaskStack.cs | 40 ++++--- Demos/SpecializedTests/TaskQueueTestDemo.cs | 123 ++++++++++---------- 2 files changed, 90 insertions(+), 73 deletions(-) diff --git a/BepuUtilities/LinkedTaskStack.cs b/BepuUtilities/LinkedTaskStack.cs index 3253e51ae..c9dc8607d 100644 --- a/BepuUtilities/LinkedTaskStack.cs +++ b/BepuUtilities/LinkedTaskStack.cs @@ -12,7 +12,7 @@ namespace BepuUtilities.TestLinkedTaskStack; /// -/// Description of a task to be submitted to a . +/// Description of a task to be submitted to a . /// public unsafe struct Task { @@ -231,10 +231,6 @@ internal unsafe struct Job sourceTasks.CopyTo(job->Tasks); job->Counter = sourceTasks.Length; job->Previous = null; - for (int i = 0; i < job->Tasks.Length; ++i) - { - Debug.Assert(job->Tasks[i].Function != null); - } return job; } @@ -246,19 +242,37 @@ internal unsafe struct Job /// True if a task was available to pop, false otherwise. internal bool TryPop(out Task task) { - for (int i = 0; i < Tasks.Length; ++i) - { - Debug.Assert(Tasks[i].Function != null); - } - var newCount = Interlocked.Decrement(ref Counter); - if (newCount >= 0) + //Note that we can't pre-decrement to claim the task: + //we can't return until after the task is copied into the caller's memory (because the counter is used to dispose the job), + //and we can't know what task to copy until after we've claimed it. + //So attempt a claim, and then compare exchange. + var count = Counter; + while (count > 0) { + var newCount = count - 1; task = Tasks[newCount]; - Debug.Assert(task.Function != null); - return true; + var preexchangeCount = Interlocked.CompareExchange(ref Counter, newCount, count); + if (count == preexchangeCount) + { + //The claim succeeded. + Debug.Assert(task.Function != null); + return true; + } + //the claim didn't succeed. Try again. + count = preexchangeCount; } task = default; return false; + + //var newCount = Interlocked.Decrement(ref Counter); + //if (newCount >= 0) + //{ + // task = Tasks[newCount]; + // Debug.Assert(task.Function != null); + // return true; + //} + //task = default; + //return false; } public void Dispose(BufferPool pool) diff --git a/Demos/SpecializedTests/TaskQueueTestDemo.cs b/Demos/SpecializedTests/TaskQueueTestDemo.cs index caaa6014c..4c8c8ca1d 100644 --- a/Demos/SpecializedTests/TaskQueueTestDemo.cs +++ b/Demos/SpecializedTests/TaskQueueTestDemo.cs @@ -42,7 +42,7 @@ struct DynamicContext1 static void DynamicallyEnqueuedTest1(long taskId, void* context, int workerIndex, IThreadDispatcher dispatcher) { - var sum = DoSomeWork(10000, 0); + var sum = DoSomeWork(100, 0); Interlocked.Add(ref ((DynamicContext1*)context)->Context->Sum, sum); } @@ -59,12 +59,12 @@ struct DynamicContext2 static void DynamicallyEnqueuedTest2(long taskId, void* context, int workerIndex, IThreadDispatcher dispatcher) { - var sum = DoSomeWork(10000, 0); + var sum = DoSomeWork(100, 0); Interlocked.Add(ref ((DynamicContext2*)context)->Context->Sum, sum); } static void Test(long taskId, void* context, int workerIndex, IThreadDispatcher dispatcher) where T : unmanaged { - var sum = DoSomeWork(100000, 0); + var sum = DoSomeWork(100, 0); var typedContext = (Context*)context; //if ((taskId & 7) == 0) { @@ -100,7 +100,7 @@ static void Test(long taskId, void* context, int workerIndex, IThreadDispatch } static void STTest(long taskId, void* context, int workerIndex, IThreadDispatcher dispatcher) { - var sum = DoSomeWork(100000, 0); + var sum = DoSomeWork(100, 0); var typedContext = (Context*)context; //if ((taskId & 7) == 0) { @@ -192,7 +192,7 @@ public override void Initialize(ContentArchive content, Camera camera) //LINKED TASK STACK - for (int i = 0; i < 1000; ++i) + for (int i = 0; i < 10; ++i) { var linkedTaskStack = new LinkedTaskStack(BufferPool, ThreadDispatcher, ThreadDispatcher.ThreadCount); var linkedTaskStackPointer = &linkedTaskStack; @@ -215,75 +215,78 @@ public override void Initialize(ContentArchive content, Camera camera) - //TASK STACK - var taskStack = new TaskStack(BufferPool, ThreadDispatcher); - var taskStackPointer = &taskStack; - - Test(() => - { - var context = new Context { TaskPile = taskStackPointer }; - var continuation = taskStackPointer->AllocateContinuation(iterationCount * tasksPerIteration, 0, ThreadDispatcher, new BepuUtilities.TestStack.Task(&IssueStop, &context)); - for (int i = 0; i < iterationCount; ++i) - { - taskStackPointer->TryPushForUnsafely(&Test, &context, i * tasksPerIteration, tasksPerIteration, continuation); - } - //taskQueuePointer->TryEnqueueStopUnsafely(); - //taskQueuePointer->EnqueueTasks() - ThreadDispatcher.DispatchWorkers(&DispatcherBody, unmanagedContext: taskStackPointer); - return context.Sum; - }, "MT Stack", () => taskStackPointer->Reset()); + ////TASK STACK + //var taskStack = new TaskStack(BufferPool, ThreadDispatcher); + //var taskStackPointer = &taskStack; + //Test(() => + //{ + // var context = new Context { TaskPile = taskStackPointer }; + // var continuation = taskStackPointer->AllocateContinuation(iterationCount * tasksPerIteration, 0, ThreadDispatcher, new BepuUtilities.TestStack.Task(&IssueStop, &context)); + // for (int i = 0; i < iterationCount; ++i) + // { + // taskStackPointer->TryPushForUnsafely(&Test, &context, i * tasksPerIteration, tasksPerIteration, continuation); + // } + // //taskQueuePointer->TryEnqueueStopUnsafely(); + // //taskQueuePointer->EnqueueTasks() + // ThreadDispatcher.DispatchWorkers(&DispatcherBody, unmanagedContext: taskStackPointer); + // return context.Sum; + //}, "MT Stack", () => taskStackPointer->Reset()); - taskStack.Dispose(BufferPool); + //taskStack.Dispose(BufferPool); - //PARALLEL TASK STACK - var parallelTaskStack = new ParallelTaskStack(BufferPool, ThreadDispatcher, ThreadDispatcher.ThreadCount); - var parallelTaskStackPointer = ¶llelTaskStack; - Test(() => - { - var context = new Context { TaskPile = parallelTaskStackPointer }; - var continuation = parallelTaskStackPointer->AllocateContinuation(iterationCount * tasksPerIteration, 0, ThreadDispatcher, new BepuUtilities.TestParallelStack.Task(&IssueStop, &context)); - for (int i = 0; i < iterationCount; ++i) - { - parallelTaskStackPointer->PushForUnsafely(&Test, &context, i * tasksPerIteration, tasksPerIteration, i % ThreadDispatcher.ThreadCount, ThreadDispatcher, continuation); - } - //taskQueuePointer->TryEnqueueStopUnsafely(); - //taskQueuePointer->EnqueueTasks() - ThreadDispatcher.DispatchWorkers(&DispatcherBody, unmanagedContext: parallelTaskStackPointer); - return context.Sum; - }, "MT Parallel Stack", () => parallelTaskStackPointer->Reset()); + ////PARALLEL TASK STACK + //var parallelTaskStack = new ParallelTaskStack(BufferPool, ThreadDispatcher, ThreadDispatcher.ThreadCount); + //var parallelTaskStackPointer = ¶llelTaskStack; - var noSteal = ParallelTaskStack.NoStealRequired; - var stealRequired = ParallelTaskStack.StealRequired; - var stealFail = ParallelTaskStack.PopFailed; - var steal = stealRequired - stealFail; + //Test(() => + //{ + // var context = new Context { TaskPile = parallelTaskStackPointer }; + // var continuation = parallelTaskStackPointer->AllocateContinuation(iterationCount * tasksPerIteration, 0, ThreadDispatcher, new BepuUtilities.TestParallelStack.Task(&IssueStop, &context)); + // for (int i = 0; i < iterationCount; ++i) + // { + // parallelTaskStackPointer->PushForUnsafely(&Test, &context, i * tasksPerIteration, tasksPerIteration, i % ThreadDispatcher.ThreadCount, ThreadDispatcher, continuation); + // } + // //taskQueuePointer->TryEnqueueStopUnsafely(); + // //taskQueuePointer->EnqueueTasks() + // ThreadDispatcher.DispatchWorkers(&DispatcherBody, unmanagedContext: parallelTaskStackPointer); + // return context.Sum; + //}, "MT Parallel Stack", () => parallelTaskStackPointer->Reset()); - Console.WriteLine($"Nosteal, steal succeed, steal fail: {noSteal}, {steal}, {stealFail} ({steal / (double)(stealRequired + noSteal)})"); + //var noSteal = ParallelTaskStack.NoStealRequired; + //var stealRequired = ParallelTaskStack.StealRequired; + //var stealFail = ParallelTaskStack.PopFailed; + //var steal = stealRequired - stealFail; - parallelTaskStack.Dispose(BufferPool); + //Console.WriteLine($"Nosteal, steal succeed, steal fail: {noSteal}, {steal}, {stealFail} ({steal / (double)(stealRequired + noSteal)})"); + //parallelTaskStack.Dispose(BufferPool); - //TASK QUEUE - var taskQueue = new TaskQueue(BufferPool, maximumTaskCapacity: 1 << 19, maximumContinuationCapacity: 1 << 19); - var taskQueuePointer = &taskQueue; - Test(() => + for (int i = 0; i < 10; ++i) { - var context = new Context { TaskPile = taskQueuePointer }; - var continuation = taskQueuePointer->AllocateContinuation(iterationCount * tasksPerIteration, 0, ThreadDispatcher, new TaskQ(&IssueStop, &context)); - for (int i = 0; i < iterationCount; ++i) + //TASK QUEUE + var taskQueue = new TaskQueue(BufferPool, maximumTaskCapacity: 1 << 19, maximumContinuationCapacity: 1 << 19); + var taskQueuePointer = &taskQueue; + + Test(() => { - taskQueuePointer->TryEnqueueForUnsafely(&Test, &context, i * tasksPerIteration, tasksPerIteration, continuation); - } - //taskQueuePointer->TryEnqueueStopUnsafely(); - //taskQueuePointer->EnqueueTasks() - ThreadDispatcher.DispatchWorkers(&DispatcherBody, unmanagedContext: taskQueuePointer); - return context.Sum; - }, "MT", () => taskQueuePointer->Reset()); + var context = new Context { TaskPile = taskQueuePointer }; + var continuation = taskQueuePointer->AllocateContinuation(iterationCount * tasksPerIteration, 0, ThreadDispatcher, new TaskQ(&IssueStop, &context)); + for (int i = 0; i < iterationCount; ++i) + { + taskQueuePointer->TryEnqueueForUnsafely(&Test, &context, i * tasksPerIteration, tasksPerIteration, continuation); + } + //taskQueuePointer->TryEnqueueStopUnsafely(); + //taskQueuePointer->EnqueueTasks() + ThreadDispatcher.DispatchWorkers(&DispatcherBody, unmanagedContext: taskQueuePointer); + return context.Sum; + }, "MT", () => taskQueuePointer->Reset()); - taskQueue.Dispose(BufferPool); + taskQueue.Dispose(BufferPool); + } //Test(() => //{ From 49ad90ba72d0d5694fcb0e53295dda1efe191786 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 2 Feb 2023 19:02:41 -0600 Subject: [PATCH 680/947] Revert badness. --- BepuUtilities/LinkedTaskStack.cs | 31 +++++-------------------------- 1 file changed, 5 insertions(+), 26 deletions(-) diff --git a/BepuUtilities/LinkedTaskStack.cs b/BepuUtilities/LinkedTaskStack.cs index c9dc8607d..b3c3fc7dc 100644 --- a/BepuUtilities/LinkedTaskStack.cs +++ b/BepuUtilities/LinkedTaskStack.cs @@ -242,37 +242,16 @@ internal unsafe struct Job /// True if a task was available to pop, false otherwise. internal bool TryPop(out Task task) { - //Note that we can't pre-decrement to claim the task: - //we can't return until after the task is copied into the caller's memory (because the counter is used to dispose the job), - //and we can't know what task to copy until after we've claimed it. - //So attempt a claim, and then compare exchange. - var count = Counter; - while (count > 0) + + var newCount = Interlocked.Decrement(ref Counter); + if (newCount >= 0) { - var newCount = count - 1; task = Tasks[newCount]; - var preexchangeCount = Interlocked.CompareExchange(ref Counter, newCount, count); - if (count == preexchangeCount) - { - //The claim succeeded. - Debug.Assert(task.Function != null); - return true; - } - //the claim didn't succeed. Try again. - count = preexchangeCount; + Debug.Assert(task.Function != null); + return true; } task = default; return false; - - //var newCount = Interlocked.Decrement(ref Counter); - //if (newCount >= 0) - //{ - // task = Tasks[newCount]; - // Debug.Assert(task.Function != null); - // return true; - //} - //task = default; - //return false; } public void Dispose(BufferPool pool) From 0f54043188663d0a94ee422fdf6731b4ce71a0f5 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 5 Feb 2023 20:15:26 -0600 Subject: [PATCH 681/947] Fixed/simplified/enspeedened LinkedTaskStack continuation management, binned builder test now passes. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 42 +-- BepuUtilities/LinkedTaskStack.cs | 284 ++++++------------ Demos/DemoSet.cs | 4 +- .../SpecializedTests/TreeFiddlingTestDemo.cs | 119 ++++---- 4 files changed, 169 insertions(+), 280 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 15cdba4ee..6b13264f4 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -4,7 +4,7 @@ using BepuUtilities; using BepuUtilities.Collections; using BepuUtilities.Memory; -using BepuUtilities.TestQueue; +using BepuUtilities.TestLinkedTaskStack; using System; using System.ComponentModel; using System.Diagnostics; @@ -472,7 +472,7 @@ public BinnedBuildWorkerContext(Buffer bounds, Buffer binAll } unsafe struct MultithreadBinnedBuildContext : IBinnedBuilderThreading where TLeafCounts : unmanaged, ILeafCountBuffer { - public TaskQueue* Queue; + public LinkedTaskStack* TaskStack; /// /// Maximum number of tasks any one job submission should create. /// If you have far more tasks than there are workers, adding more tasks just adds overhead without additional workstealing advantages. @@ -610,7 +610,7 @@ unsafe static BoundingBox4 MultithreadedCentroidPrepass(Multithread } } Debug.Assert(taskContext.TaskData.TaskCount > 0 && taskContext.TaskData.WorkerCount > 0); - context->Queue->For(&CentroidPrepassWorker, &taskContext, 0, taskCount, workerIndex, dispatcher); + context->TaskStack->For(&CentroidPrepassWorker, &taskContext, 0, taskCount, workerIndex, dispatcher); var centroidBounds = taskContext.PrepassWorkers[0]; for (int i = 1; i < activeWorkerCount; ++i) @@ -765,7 +765,7 @@ unsafe static void MultithreadedBinSubtrees(MultithreadBinnedBuildC } } - context->Queue->For(&BinSubtreesWorker, &taskContext, 0, taskContext.TaskData.TaskCount, workerIndex, dispatcher); + context->TaskStack->For(&BinSubtreesWorker, &taskContext, 0, taskContext.TaskData.TaskCount, workerIndex, dispatcher); //Unless the number of threads and bins is really huge, there's no value in attempting to multithread the final compression. //(Parallel reduction is an option, but even then... I suspect the single threaded version will be faster. And it's way simpler.) @@ -1028,7 +1028,7 @@ static unsafe void BinnedBuildNode( nodePushContext.ParentNodeIndex = nodeIndex; //Note that we use the task id to store subtree start and subtree count. Don't have to do that, but no reason not to use it. var task = new Task(&BinnedBuilderNodeWorker, &nodePushContext, (long)(subtreeRegionStartIndex + subtreeCountA) | ((long)subtreeCountB << 32)); - nodeBContinuation = threading.Queue->AllocateContinuationAndEnqueue(new Span(&task, 1), workerIndex, dispatcher); + nodeBContinuation = threading.TaskStack->AllocateContinuationAndPush(new Span(&task, 1), workerIndex, dispatcher); } if (subtreeCountA > 1) BinnedBuildNode(subtreeRegionStartIndex, nodeChildIndexA, subtreeCountA, nodeIndex, 0, context, workerIndex, dispatcher); @@ -1041,12 +1041,12 @@ static unsafe void BinnedBuildNode( //In addition to letting us use the local stack to store some arguments for the other thread, this wait means that all children have completed when this function returns. //That makes knowing when to stop the queue easier. Debug.Assert(nodeBContinuation.Initialized); - Unsafe.As>(ref context->Threading).Queue->WaitForCompletion(nodeBContinuation, workerIndex, dispatcher); + Unsafe.As>(ref context->Threading).TaskStack->WaitForCompletion(nodeBContinuation, workerIndex, dispatcher); } } static unsafe void BinnedBuilderInternal(Buffer encodedLeafIndices, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, Buffer leaves, - IThreadDispatcher dispatcher, TaskQueue* taskQueuePointer, int workerCount, BufferPool pool, int minimumBinCount, int maximumBinCount, float leafToBinMultiplier, int microsweepThreshold) + IThreadDispatcher dispatcher, LinkedTaskStack* taskStackPointer, int workerCount, BufferPool pool, int minimumBinCount, int maximumBinCount, float leafToBinMultiplier, int microsweepThreshold) { var subtreeCount = encodedLeafIndices.Length; Debug.Assert(boundingBoxes.Length >= subtreeCount, "The bounding boxes provided must cover the range of indices provided."); @@ -1074,7 +1074,7 @@ static unsafe void BinnedBuilderInternal(Buffer encodedLeafIndices, Buffer< //The microsweep uses the same resources as the bin allocations, so expand to hold whichever is larger. var allocatedBinCount = int.Max(maximumBinCount, microsweepThreshold); - if (dispatcher == null && taskQueuePointer == null) + if (dispatcher == null && taskStackPointer == null) { //Use the single threaded path. var binBoundsMemory = stackalloc BoundingBox4[allocatedBinCount * 2 + 1]; @@ -1111,28 +1111,28 @@ static unsafe void BinnedBuilderInternal(Buffer encodedLeafIndices, Buffer< workerContexts[i] = new BinnedBuildWorkerContext(boundingBoxes.As(), workerBinsAllocation, ref binAllocationStart, maximumBinCount, leafCounts); } - TaskQueue taskQueue = default; - bool createdTaskQueueLocally = taskQueuePointer == null; - if (taskQueuePointer == null) + LinkedTaskStack taskStack = default; + bool createdTaskQueueLocally = taskStackPointer == null; + if (taskStackPointer == null) { - taskQueue = new TaskQueue(pool); - taskQueuePointer = &taskQueue; + taskStack = new LinkedTaskStack(pool, dispatcher, dispatcher.ThreadCount); + taskStackPointer = &taskStack; } var threading = new MultithreadBinnedBuildContext { MaximumTaskCountPerSubmission = workerCount * 2, - Queue = taskQueuePointer, + TaskStack = taskStackPointer, Workers = workerContexts, }; var context = new Context, MultithreadBinnedBuildContext>( minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold, encodedLeafIndices, leafCounts, leaves, boundingBoxes.As(), nodes, metanodes, threading); - taskQueuePointer->TryEnqueueUnsafely(new Task(&BinnedBuilderWorkerEntry>, &context)); - dispatcher.DispatchWorkers(&BinnedBuilderWorkerFunction>, unmanagedContext: taskQueuePointer); + taskStackPointer->PushUnsafely(new Task(&BinnedBuilderWorkerEntry>, &context), 0, dispatcher); + dispatcher.DispatchWorkers(&BinnedBuilderWorkerFunction>, unmanagedContext: taskStackPointer); if (createdTaskQueueLocally) - taskQueue.Dispose(pool); + taskStack.Dispose(pool, dispatcher); pool.Return(ref workerBinsAllocation); } } @@ -1150,17 +1150,17 @@ unsafe static void BinnedBuilderWorkerEntry(long taskId, v var context = (Context>*)untypedContext; BinnedBuildNode(0, 0, context->Indices.Length, -1, -1, context, workerIndex, dispatcher); //Once the entry point returns, all workers should stop because it won't return unless both nodes are done. - context->Threading.Queue->EnqueueStop(workerIndex, dispatcher); + context->Threading.TaskStack->RequestStop(); } unsafe static void BinnedBuilderWorkerFunction(int workerIndex, IThreadDispatcher dispatcher) where TLeafCounts : unmanaged, ILeafCountBuffer where TLeaves : unmanaged { - var taskQueue = (TaskQueue*)dispatcher.UnmanagedContext; - DequeueTaskResult dequeueTaskResult; + var taskQueue = (LinkedTaskStack*)dispatcher.UnmanagedContext; + PopTaskResult popTaskResult; var waiter = new SpinWait(); - while ((dequeueTaskResult = taskQueue->TryDequeueAndRun(workerIndex, dispatcher)) != DequeueTaskResult.Stop) + while ((popTaskResult = taskQueue->TryPopAndRun(workerIndex, dispatcher)) != PopTaskResult.Stop) { waiter.SpinOnce(-1); } diff --git a/BepuUtilities/LinkedTaskStack.cs b/BepuUtilities/LinkedTaskStack.cs index b3c3fc7dc..b5858110c 100644 --- a/BepuUtilities/LinkedTaskStack.cs +++ b/BepuUtilities/LinkedTaskStack.cs @@ -92,36 +92,13 @@ public enum PopTaskResult /// public unsafe struct ContinuationHandle : IEquatable { - uint index; - uint encodedVersion; - /// - /// Source worker task stack. - /// - internal Worker* Worker; - - internal ContinuationHandle(uint index, int version, Worker* workerStack) - { - this.index = index; - encodedVersion = (uint)version | 1u << 31; - Worker = workerStack; - } - - internal uint Index - { - get - { - Debug.Assert(Initialized, "If you're trying to pull a continuation id from a continuation handle, it should have been initialized."); - return index; - } - } + //This is a bit odd. We're presenting this pointer as a handle, even though it's not. + //Hiding the implementation detail makes it a little easier to change later if we need to. + TaskContinuation* continuation; - internal int Version + internal ContinuationHandle(TaskContinuation* continuation) { - get - { - Debug.Assert(Initialized, "If you're trying to pull a continuation id from a continuation handle, it should have been initialized."); - return (int)(encodedVersion & ((1u << 31) - 1)); - } + this.continuation = continuation; } /// @@ -131,8 +108,7 @@ public bool Completed { get { - Debug.Assert(Initialized == (Worker != null)); - return Initialized && Worker->IsComplete(this); + return Initialized && continuation->RemainingTaskCounter <= 0; } } @@ -141,31 +117,17 @@ public bool Completed /// /// Pointer to the continuation backing the given handle. /// This should not be used if the continuation handle is not known to be valid. The data pointed to by the data could become invalidated if the continuation completes. - public TaskContinuation* Continuation => Worker->GetContinuation(this); + public TaskContinuation* Continuation => continuation; /// /// Gets a null continuation handle. /// public static ContinuationHandle Null => default; - /// - /// Gets whether the continuation associated with this handle was allocated and has outstanding tasks. - /// - public bool Exists - { - get - { - if (!Initialized || Worker == null) - return false; - ref var continuation = ref Worker->Continuations[Index]; - return continuation.Version == Version && continuation.RemainingTaskCounter > 0; - } - } - /// /// Gets whether this handle ever represented an allocated handle. This does not guarantee that the continuation's associated tasks are active in the that it was allocated from. /// - public bool Initialized => encodedVersion >= 1u << 31; + public bool Initialized => continuation != null; /// /// Notifies the continuation that one task was completed. @@ -185,24 +147,14 @@ public void NotifyTaskCompleted(int workerIndex, IThreadDispatcher dispatcher) { continuation->OnCompleted.Function(continuation->OnCompleted.Id, continuation->OnCompleted.Context, workerIndex, dispatcher); } - //Free this continuation slot. - var waiter = new SpinWait(); - while (Interlocked.CompareExchange(ref Worker->ContinuationLocker, 1, 0) != 0) - { - waiter.SpinOnce(-1); - } - //We have the lock. - Worker->ContinuationIndexPool.ReturnUnsafely((int)Index); - --Worker->ContinuationCount; - Worker->ContinuationLocker = 0; } } - public bool Equals(ContinuationHandle other) => other.index == index && other.encodedVersion == encodedVersion && other.Worker == Worker; + public bool Equals(ContinuationHandle other) => other.continuation == continuation; public override bool Equals([NotNullWhen(true)] object obj) => obj is ContinuationHandle handle && Equals(handle); - public override int GetHashCode() => (int)(index ^ (encodedVersion << 24)); + public override int GetHashCode() => (int)continuation; public static bool operator ==(ContinuationHandle left, ContinuationHandle right) => left.Equals(right); @@ -263,17 +215,55 @@ public void Dispose(BufferPool pool) } } +/// +/// Stores a block of task continuations that maintains a pointer to previous blocks. +/// +internal unsafe struct ContinuationBlock +{ + public ContinuationBlock* Previous; + + public int Count; + public Buffer Continuations; + + public static ContinuationBlock* Create(int continuationCapacity, BufferPool pool) + { + pool.Take(sizeof(TaskContinuation) * continuationCapacity + sizeof(ContinuationBlock), out var rawBuffer); + ContinuationBlock* block = (ContinuationBlock*)rawBuffer.Memory; + block->Continuations = new Buffer(rawBuffer.Memory + sizeof(ContinuationBlock), continuationCapacity, rawBuffer.Id); + block->Count = 0; + block->Previous = null; + return block; + } + + public bool TryAllocateContinuation(out TaskContinuation* continuation) + { + if (Count < Continuations.length) + { + continuation = Continuations.Memory + (Count++); + return true; + } + continuation = null; + return false; + } + + public void Dispose(BufferPool pool) + { + var id = Continuations.Id; + pool.ReturnUnsafely(id); + if (Previous != null) + Previous->Dispose(pool); + this = default; + } +} + + internal unsafe struct Worker { //The worker needs to track allocations made over the course of its lifetime so they can be disposed later. public QuickList AllocatedJobs; - public Buffer Continuations; - public IdPool ContinuationIndexPool; - public int ContinuationCount; + public ContinuationBlock* ContinuationHead; public int WorkerIndex; - public volatile int ContinuationLocker; - [Conditional("DEBUG")] public void ValidateTasks() @@ -288,17 +278,13 @@ public void ValidateTasks() } } - public Worker(int workerIndex, IThreadDispatcher dispatcher, int initialJobCapacity = 64, int initialContinuationCapacity = 16) + + public Worker(int workerIndex, IThreadDispatcher dispatcher, int initialJobCapacity = 128, int continuationBlockCapacity = 128) { var threadPool = dispatcher.WorkerPools[workerIndex]; WorkerIndex = workerIndex; AllocatedJobs = new QuickList(initialJobCapacity, threadPool); - threadPool.Take(initialContinuationCapacity, out Continuations); -#if DEBUG - //While you shouldn't *need* to clear continuations, it can be useful for debug purposes. - Continuations.Clear(0, Continuations.length); -#endif - ContinuationIndexPool = new IdPool(initialContinuationCapacity, threadPool); + ContinuationHead = ContinuationBlock.Create(continuationBlockCapacity, threadPool); } public void Dispose(BufferPool threadPool) @@ -308,8 +294,7 @@ public void Dispose(BufferPool threadPool) ((Job*)AllocatedJobs[i])->Dispose(threadPool); } AllocatedJobs.Dispose(threadPool); - threadPool.Return(ref Continuations); - ContinuationIndexPool.Dispose(threadPool); + ContinuationHead->Dispose(threadPool); } internal void Reset(BufferPool threadPool) @@ -319,12 +304,9 @@ internal void Reset(BufferPool threadPool) ((Job*)AllocatedJobs[i])->Dispose(threadPool); } AllocatedJobs.Count = 0; -#if DEBUG - //While you shouldn't *need* to clear continuations, it can be useful for debug purposes. - Continuations.Clear(0, Continuations.length); -#endif - ContinuationCount = 0; - Debug.Assert(ContinuationLocker == 0, "There appears to be a thread actively working still. That's invalid."); + var capacity = ContinuationHead->Continuations.length; + ContinuationHead->Dispose(threadPool); + ContinuationHead = ContinuationBlock.Create(capacity, threadPool); } /// @@ -345,112 +327,28 @@ internal void Reset(BufferPool threadPool) /// - /// Attempts to allocate a continuation for a set of tasks. + /// Allocates a continuation for a set of tasks. /// /// Number of tasks associated with the continuation. /// Dispatcher from which to pull a buffer pool if needed for resizing. /// Function to execute upon completing all associated tasks, if any. Any task with a null will not be executed. - /// Handle of the continuation if allocation is successful. - /// True if the continuation was allocated, false if the attempt was contested.. - public bool TryAllocateContinuation(int taskCount, IThreadDispatcher dispatcher, out ContinuationHandle continuationHandle, Task onCompleted = default) - { - continuationHandle = default; - if (Interlocked.CompareExchange(ref ContinuationLocker, 1, 0) != 0) - return false; - try - { - //We have the lock. - if (ContinuationCount == Continuations.length) - { - //Doing a resize within a lock is *really* not great. This is something to be avoided via preallocation whenever possible. - //BUT: - //1. It should be trivial to make these resizes effectively never happen. - //2. In the cases where it happens anyway, suffering the pain of a resize in a lock is the lesser of two evils. - //In the case where we *didn't* resize, we'd have to either stall on the allocation attempt or cycle on other tasks until continuations are freed up. - //But there's no guarantee that running tasks will actually free up continuations on net- they could make more! - //So having the fallback plan of expanding storage for more continuations avoids deadlock prone options. - //Note that *ONLY THE OWNING WORKER* can ever validly perform this resize! - //We have to use the thread's buffer pool, and only the thread can validly access that pool. - var workerPool = dispatcher.WorkerPools[WorkerIndex]; - workerPool.ResizeToAtLeast(ref Continuations, ContinuationCount * 2, ContinuationCount); - ContinuationIndexPool.Resize(Continuations.length, workerPool); -#if DEBUG - //While you shouldn't *need* to clear continuations, it can be useful for debug purposes. - Continuations.Clear(ContinuationCount, Continuations.length - ContinuationCount); -#endif - } - var index = ContinuationIndexPool.Take(); - ++ContinuationCount; - ref var continuation = ref Continuations[index]; - //Note that the version number could be based on undefined data initially. That's actually fine; all we care about is whether it is different. - //Note mask to leave a valid bit for encoding in the handle. - var newVersion = (continuation.Version + 1) & (~(1 << 31)); - continuation.OnCompleted = onCompleted; - continuation.Version = newVersion; - continuation.RemainingTaskCounter = taskCount; - continuationHandle = new ContinuationHandle((uint)index, newVersion, (Worker*)Unsafe.AsPointer(ref this)); - return true; - } - finally - { - ContinuationLocker = 0; - } - } - - - /// - /// Allocates a continuation for a set of tasks. - /// - /// Number of tasks associated with the continuation. - /// Dispatcher from which to pull any thread allocations if necessary. - /// Task to execute upon completing all associated tasks, if any. Any task with a null will not be executed. - /// Handle of the allocated continuation. - /// Note that this will keep trying until allocation succeeds. If something is blocking allocation, such as insufficient room in the continuations buffer and there are no workers consuming tasks, this will block forever. + /// Handle of the continuation. public ContinuationHandle AllocateContinuation(int taskCount, IThreadDispatcher dispatcher, Task onCompleted = default) { - var waiter = new SpinWait(); - ContinuationHandle handle; - while (!TryAllocateContinuation(taskCount, dispatcher, out handle, onCompleted)) + if (!ContinuationHead->TryAllocateContinuation(out TaskContinuation* continuation)) { - waiter.SpinOnce(-1); + //Couldn't allocate; need to allocate a new block. + //(The reason for the linked list style allocation is that resizing a buffer- and returning the old buffer- opens up a potential race condition.) + var newBlock = ContinuationBlock.Create(ContinuationHead->Continuations.length, dispatcher.WorkerPools[WorkerIndex]); + newBlock->Previous = ContinuationHead; + ContinuationHead = newBlock; + var allocated = ContinuationHead->TryAllocateContinuation(out continuation); + Debug.Assert(allocated, "Just created that block! Is the capacity wrong?"); } - return handle; + continuation->OnCompleted = onCompleted; + continuation->RemainingTaskCounter = taskCount; + return new ContinuationHandle(continuation); } - - /// - /// Retrieves a pointer to the continuation data for . - /// - /// Handle to look up the associated continuation for. - /// Pointer to the continuation backing the given handle. - /// This should not be used if the continuation handle is not known to be valid. The data pointed to by the data could become invalidated if the continuation completes. - public TaskContinuation* GetContinuation(ContinuationHandle continuationHandle) - { - Debug.Assert(continuationHandle.Initialized, "This continuation handle was never initialized."); - Debug.Assert(continuationHandle.Index < Continuations.length, "This continuation refers to an invalid index."); - if (continuationHandle.Index >= Continuations.length || !continuationHandle.Initialized) - return null; - var continuation = Continuations.Memory + continuationHandle.Index; - Debug.Assert(continuation->Version == continuationHandle.Version, "This continuation no longer refers to an active continuation."); - if (continuation->Version != continuationHandle.Version) - return null; - return Continuations.Memory + continuationHandle.Index; - } - - /// - /// Checks whether all tasks associated with this continuation have completed. - /// - /// Continuation to check for completion. - /// True if all tasks associated with a continuation have completed, false otherwise. - public bool IsComplete(ContinuationHandle continuationHandle) - { - Debug.Assert(continuationHandle.Initialized, "This continuation handle was never initialized."); - Debug.Assert(continuationHandle.Index < Continuations.length, "This continuation refers to an invalid index."); - if (continuationHandle.Index >= Continuations.length || !continuationHandle.Initialized) - return false; - ref var continuation = ref Continuations[continuationHandle.Index]; - return continuation.Version > continuationHandle.Version || continuation.RemainingTaskCounter == 0; - } - } /// @@ -463,10 +361,6 @@ public unsafe struct TaskContinuation /// public Task OnCompleted; /// - /// Version of this continuation. - /// - public int Version; - /// /// Number of tasks not yet reported as complete in the continuation. /// public int RemainingTaskCounter; @@ -501,13 +395,13 @@ struct StopPad /// Thread dispatcher to pull thread pools from for thread allocations. /// Number of workers to allocate space for. /// Initial number of jobs (groups of tasks submitted together) to allocate space for in each worker. - /// Initial number of continuations to allocate space for in each worker. - public LinkedTaskStack(BufferPool pool, IThreadDispatcher dispatcher, int workerCount, int initialWorkerJobCapacity = 64, int initialWorkerContinuationCapacity = 16) + /// Number of slots to allocate in each block of continuations in each worker. + public LinkedTaskStack(BufferPool pool, IThreadDispatcher dispatcher, int workerCount, int initialWorkerJobCapacity = 128, int continuationBlockCapacity = 128) { pool.Take(workerCount, out workers); for (int i = 0; i < workerCount; ++i) { - workers[i] = new Worker(i, dispatcher, initialWorkerJobCapacity, initialWorkerContinuationCapacity); + workers[i] = new Worker(i, dispatcher, initialWorkerJobCapacity, continuationBlockCapacity); } Reset(dispatcher); } @@ -569,7 +463,12 @@ public int ApproximateContinuationCount int sum = 0; for (int i = 0; i < workers.Length; ++i) { - sum += workers[i].ContinuationCount; + var block = workers[i].ContinuationHead; + while (block != null) + { + sum += block->Count; + block = block->Previous; + } } return sum; } @@ -583,26 +482,12 @@ public int ApproximateContinuationCount /// Worker index to allocate the continuation on. /// Dispatcher to use for any per-thread allocations if necessary. /// Function to execute upon completing all associated tasks, if any. Any task with a null will not be executed. - /// Handle of the continuation if allocation is successful. - /// True if the allocation succeeded, false if it was contested.. - public bool TryAllocateContinuation(int taskCount, int workerIndex, IThreadDispatcher dispatcher, out ContinuationHandle continuationHandle, Task onCompleted = default) - { - return workers[workerIndex].TryAllocateContinuation(taskCount, dispatcher, out continuationHandle, onCompleted); - } - - /// - /// Allocates a continuation for a set of tasks. - /// - /// Number of tasks associated with the continuation. - /// Worker index to allocate the continuation on. - /// Dispatcher to use for any per-thread allocations if necessary. - /// Task to execute upon completing all associated tasks, if any. Any task with a null will not be executed. - /// Handle of the allocated continuation. + /// Handle of the continuation. public ContinuationHandle AllocateContinuation(int taskCount, int workerIndex, IThreadDispatcher dispatcher, Task onCompleted = default) { return workers[workerIndex].AllocateContinuation(taskCount, dispatcher, onCompleted); } - + /// /// Attempts to pop a task. /// @@ -775,7 +660,6 @@ public void RunTasks(Span tasks, int workerIndex, IThreadDispatcher dispat for (int i = 0; i < tasksToPush.Length; ++i) { var task = tasks[i + 1]; - Debug.Assert(continuationHandle.Worker == workers.Memory + workerIndex); Debug.Assert(!task.Continuation.Initialized, $"None of the source tasks should have continuations when provided to {nameof(RunTasks)}."); task.Continuation = continuationHandle; tasksToPush[i] = task; diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index a80e8370a..d2433f7df 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -47,8 +47,8 @@ struct Option public DemoSet() { //AddOption(); - //AddOption(); - AddOption(); + AddOption(); + //AddOption(); AddOption(); AddOption(); AddOption(); diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index fcf537722..af0dc6e47 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -155,78 +155,83 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(4, 1)); + for (int i = 0; i < 128; ++i) + { + BufferPool.Clear(); + ThreadDispatcher.WorkerPools.Clear(); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(4, 1)); - //Create a mesh. - var width = 64;// 768; - var height = 64;// 768; - var scale = new Vector3(1, 1, 1); - //DemoMeshHelper.CreateDeformedPlane(width, height, (x, y) => new Vector3(x - width * scale.X * 0.5f, 2f * (float)(Math.Sin(x * 0.5f) * Math.Sin(y * 0.5f)), y - height * scale.Y * 0.5f), scale, BufferPool, out var mesh); - //DemoMeshHelper.CreateDeformedPlane(width, height, (x, y) => new Vector3(x - width * scale.X * 0.5f, 0, y - height * scale.Y * 0.5f), scale, BufferPool, out var mesh); + //Create a mesh. + var width = 1024; + var height = 1024; + var scale = new Vector3(1, 1, 1); + //DemoMeshHelper.CreateDeformedPlane(width, height, (x, y) => new Vector3(x - width * scale.X * 0.5f, 2f * (float)(Math.Sin(x * 0.5f) * Math.Sin(y * 0.5f)), y - height * scale.Y * 0.5f), scale, BufferPool, out var mesh); + //DemoMeshHelper.CreateDeformedPlane(width, height, (x, y) => new Vector3(x - width * scale.X * 0.5f, 0, y - height * scale.Y * 0.5f), scale, BufferPool, out var mesh); - //var triangles = CreateDeformedPlaneTriangles(width, height, scale); - var triangles = CreateRandomSoupTriangles(new BoundingBox(new(width / -2f, scale.Y * -2, height / -2f), new(width / 2f, scale.Y * 2, height / 2f)), (width - 1) * (height - 1) * 2, 0.5f, 100f); - //var mesh = new Mesh(triangles, Vector3.One, BufferPool); - var mesh = DemoMeshHelper.CreateGiantMeshFast(triangles, Vector3.One, BufferPool); + //var triangles = CreateDeformedPlaneTriangles(width, height, scale); + var triangles = CreateRandomSoupTriangles(new BoundingBox(new(width / -2f, scale.Y * -2, height / -2f), new(width / 2f, scale.Y * 2, height / 2f)), (width - 1) * (height - 1) * 2, 0.5f, 100f); + //var mesh = new Mesh(triangles, Vector3.One, BufferPool); + var mesh = DemoMeshHelper.CreateGiantMeshFast(triangles, Vector3.One, BufferPool); - Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); + Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); - Console.WriteLine($"node count: {mesh.Tree.NodeCount}"); - Console.WriteLine($"initial SAH: {mesh.Tree.MeasureCostMetric()}, cache quality: {mesh.Tree.MeasureCacheQuality()}"); - Console.WriteLine($"initial bounds: A ({mesh.Tree.Nodes[0].A.Min}, {mesh.Tree.Nodes[0].B.Max}), B ({mesh.Tree.Nodes[0].B.Min}, {mesh.Tree.Nodes[0].B.Max})"); + Console.WriteLine($"node count: {mesh.Tree.NodeCount}"); + Console.WriteLine($"initial SAH: {mesh.Tree.MeasureCostMetric()}, cache quality: {mesh.Tree.MeasureCacheQuality()}"); + Console.WriteLine($"initial bounds: A ({mesh.Tree.Nodes[0].A.Min}, {mesh.Tree.Nodes[0].B.Max}), B ({mesh.Tree.Nodes[0].B.Min}, {mesh.Tree.Nodes[0].B.Max})"); - BufferPool.Take(mesh.Triangles.Length, out var leafBounds); - BufferPool.Take(mesh.Triangles.Length, out var leafIndices); + BufferPool.Take(mesh.Triangles.Length, out var leafBounds); + BufferPool.Take(mesh.Triangles.Length, out var leafIndices); - Action setup = () => - { - for (int i = 0; i < mesh.Triangles.Length; ++i) + Action setup = () => { - ref var t = ref mesh.Triangles[i]; - ref var bounds = ref leafBounds[i]; - bounds.Min = Vector3.Min(t.A, Vector3.Min(t.B, t.C)); - bounds.Max = Vector3.Max(t.A, Vector3.Max(t.B, t.C)); - leafIndices[i] = Tree.Encode(i); - } - }; - - BinnedTest(setup, () => - { - Tree.BinnedBuilder(leafIndices, leafBounds, mesh.Tree.Nodes, mesh.Tree.Metanodes, mesh.Tree.Leaves, ThreadDispatcher, BufferPool); - }, "Revamp Single Axis MT", ref mesh.Tree); + for (int i = 0; i < mesh.Triangles.Length; ++i) + { + ref var t = ref mesh.Triangles[i]; + ref var bounds = ref leafBounds[i]; + bounds.Min = Vector3.Min(t.A, Vector3.Min(t.B, t.C)); + bounds.Max = Vector3.Max(t.A, Vector3.Max(t.B, t.C)); + leafIndices[i] = Tree.Encode(i); + } + }; + + BinnedTest(setup, () => + { + Tree.BinnedBuilder(leafIndices, leafBounds, mesh.Tree.Nodes, mesh.Tree.Metanodes, mesh.Tree.Leaves, ThreadDispatcher, BufferPool); + }, "Revamp Single Axis MT", ref mesh.Tree); - BinnedTest(setup, () => - { - Tree.BinnedBuilder(leafIndices, leafBounds, mesh.Tree.Nodes, mesh.Tree.Metanodes, mesh.Tree.Leaves); - }, "Revamp Single Axis ST", ref mesh.Tree); + //BinnedTest(setup, () => + //{ + // Tree.BinnedBuilder(leafIndices, leafBounds, mesh.Tree.Nodes, mesh.Tree.Metanodes, mesh.Tree.Leaves); + //}, "Revamp Single Axis ST", ref mesh.Tree); - Mesh mesh2 = default; - Mesh* mesh2Pointer = &mesh2; + //Mesh mesh2 = default; + //Mesh* mesh2Pointer = &mesh2; - QuickList subtreeReferences = new(triangles.Length, BufferPool); - QuickList treeletInternalNodes = new(triangles.Length, BufferPool); - Tree.CreateBinnedResources(BufferPool, triangles.Length, out var binnedResourcesBuffer, out var binnedResources); - BinnedTest(() => - { - if (mesh2Pointer->Tree.Leaves.Allocated) - mesh2Pointer->Tree.Dispose(BufferPool); - *mesh2Pointer = DemoMeshHelper.CreateGiantMeshFast(triangles, Vector3.One, BufferPool); - }, () => - { - subtreeReferences.Count = 0; - treeletInternalNodes.Count = 0; - mesh2Pointer->Tree.BinnedRefine(0, ref subtreeReferences, mesh2Pointer->Tree.LeafCount, ref treeletInternalNodes, ref binnedResources, BufferPool); - }, "Original", ref mesh2Pointer->Tree); + //QuickList subtreeReferences = new(triangles.Length, BufferPool); + //QuickList treeletInternalNodes = new(triangles.Length, BufferPool); + //Tree.CreateBinnedResources(BufferPool, triangles.Length, out var binnedResourcesBuffer, out var binnedResources); + //BinnedTest(() => + //{ + // if (mesh2Pointer->Tree.Leaves.Allocated) + // mesh2Pointer->Tree.Dispose(BufferPool); + // *mesh2Pointer = DemoMeshHelper.CreateGiantMeshFast(triangles, Vector3.One, BufferPool); + //}, () => + //{ + // subtreeReferences.Count = 0; + // treeletInternalNodes.Count = 0; + // mesh2Pointer->Tree.BinnedRefine(0, ref subtreeReferences, mesh2Pointer->Tree.LeafCount, ref treeletInternalNodes, ref binnedResources, BufferPool); + //}, "Original", ref mesh2Pointer->Tree); - //RefitTest(() => mesh.Tree.Refit2(), "refit2", ref mesh.Tree); - //RefitTest(() => mesh.Tree.Refit(), "Original", ref mesh.Tree); + //RefitTest(() => mesh.Tree.Refit2(), "refit2", ref mesh.Tree); + //RefitTest(() => mesh.Tree.Refit(), "Original", ref mesh.Tree); - //SelfTest((ref OverlapHandler handler) => mesh.Tree.GetSelfOverlapsContiguousPrepass(ref handler, BufferPool), mesh.Tree.LeafCount, "Prepass"); - //SelfTest((ref OverlapHandler handler) => mesh.Tree.GetSelfOverlaps(ref handler), mesh.Tree.LeafCount, "Original"); + //SelfTest((ref OverlapHandler handler) => mesh.Tree.GetSelfOverlapsContiguousPrepass(ref handler, BufferPool), mesh.Tree.LeafCount, "Prepass"); + //SelfTest((ref OverlapHandler handler) => mesh.Tree.GetSelfOverlaps(ref handler), mesh.Tree.LeafCount, "Original"); - Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 10, 0), 1, Simulation.Shapes, new Sphere(0.5f))); + Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 10, 0), 1, Simulation.Shapes, new Sphere(0.5f))); + } } delegate void TestFunction(ref OverlapHandler handler); From 5f28434dd9bc7cb00b31f167b132a0f4fb2fdb18 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 23 Feb 2023 17:28:36 -0600 Subject: [PATCH 682/947] Updates for workerpools refactor. --- Demos/Demos/CollisionTrackingDemo.cs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Demos/Demos/CollisionTrackingDemo.cs b/Demos/Demos/CollisionTrackingDemo.cs index 8ce019432..dd326b000 100644 --- a/Demos/Demos/CollisionTrackingDemo.cs +++ b/Demos/Demos/CollisionTrackingDemo.cs @@ -252,7 +252,7 @@ void PrepareForNextTimestep(float dt, IThreadDispatcher dispatcher) workerCaches = new Buffer>(dispatcher.ThreadCount, pool); for (int i = 0; i < workerCaches.Length; ++i) { - workerCaches[i] = new QuickList(512, dispatcher.GetThreadMemoryPool(i)); + workerCaches[i] = new QuickList(512, dispatcher.WorkerPools[i]); } } @@ -272,7 +272,7 @@ void Flush(float dt, IThreadDispatcher threadDispatcher) Tracked.Values[elementIndex].Pairs.Add(entry.Other, entry.Collision, pool); } //The worker cache memory must be returned to the thread pool, not the main pool! - cache.Dispose(dispatcher.GetThreadMemoryPool(i)); + cache.Dispose(dispatcher.WorkerPools[i]); } workerCaches.Dispose(pool); } @@ -282,7 +282,7 @@ private void ReportContacts(int workerIndex, ContactSource self, Cont if (Tracked.ContainsKey(self)) { //A is a listener, add it. - ref var pairContacts = ref workerCaches[workerIndex].Allocate(dispatcher.GetThreadMemoryPool(workerIndex)); + ref var pairContacts = ref workerCaches[workerIndex].Allocate(dispatcher.WorkerPools[workerIndex]); pairContacts.Self = self; pairContacts.Other = other; pairContacts.Collision.OtherIsAInPair = otherIsA; From 06402677170accae113b84a01b9fc4d1ad9c2a0a Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 23 Feb 2023 19:38:32 -0600 Subject: [PATCH 683/947] Tuning. Seem to be hitting a bottleneck. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 6 +++--- Demos/SpecializedTests/TreeFiddlingTestDemo.cs | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 6b13264f4..75fcbc98d 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -490,9 +490,9 @@ public void GetBins(int workerIndex, out Buffer binBoundingBoxes, } //These should be powers of 2 for maskhack reasons. - const int SubtreesPerThreadForCentroidPrepass = 2;//2048; - const int SubtreesPerThreadForBinning = 2;//512; - const int SubtreesPerThreadForNodeJob = 2;//512; + const int SubtreesPerThreadForCentroidPrepass = 262144; + const int SubtreesPerThreadForBinning = 262144; + const int SubtreesPerThreadForNodeJob = 1024; [MethodImpl(MethodImplOptions.AggressiveInlining)] static BoundingBox4 ComputeCentroidBounds(Buffer bounds) diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index af0dc6e47..e0bcf49e8 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -155,7 +155,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = 0; - for (int i = 0; i < 128; ++i) + for (int i = 0; i < 2; ++i) { BufferPool.Clear(); ThreadDispatcher.WorkerPools.Clear(); @@ -280,7 +280,7 @@ static void RefitTest(Action function, string name, ref Tree tree) static void BinnedTest(Action setup, Action function, string name, ref Tree tree) { long accumulatedTime = 0; - const int testCount = 16; + const int testCount = 64; for (int i = 0; i < testCount; ++i) { setup?.Invoke(); From 16a8e4743e900b0fda66bfaafdffa151a97de6ea Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 23 Feb 2023 20:22:04 -0600 Subject: [PATCH 684/947] Fixed worker preallocation capacity. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 75fcbc98d..033eaac28 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -311,7 +311,7 @@ static unsafe void MicroSweepForBinnedBuilder( } } - Debug.Assert(subtreeCount <= context->MaximumBinCount || subtreeCount < context->MicrosweepThreshold, "We're reusing the bin resources under the assumption that this is only ever called when there are less leaves than maximum bins."); + Debug.Assert(subtreeCount <= context->MaximumBinCount || subtreeCount <= context->MicrosweepThreshold, "We're reusing the bin resources under the assumption that this is only ever called when there are less leaves than maximum bins."); //Identify the split index by examining the SAH of very split option. //Premerge from left to right so we have a sorta-summed area table to cheaply look up all possible child A bounds as we scan. binBoundingBoxesScan[0] = boundingBoxes[0]; @@ -461,11 +461,11 @@ unsafe struct BinnedBuildWorkerContext where TLeafCounts : unmanage /// public TLeafCounts LeafCounts; - public BinnedBuildWorkerContext(Buffer bounds, Buffer binAllocationBuffer, ref int binStart, int binCount, TLeafCounts leafCounts) + public BinnedBuildWorkerContext(Buffer bounds, Buffer binAllocationBuffer, ref int binStart, int binCapacity, TLeafCounts leafCounts) { - BinBoundingBoxes = Suballocate(binAllocationBuffer, ref binStart, binCount); - BinBoundingBoxesScan = Suballocate(binAllocationBuffer, ref binStart, binCount); - BinLeafCounts = Suballocate(binAllocationBuffer, ref binStart, binCount); + BinBoundingBoxes = Suballocate(binAllocationBuffer, ref binStart, binCapacity); + BinBoundingBoxesScan = Suballocate(binAllocationBuffer, ref binStart, binCapacity); + BinLeafCounts = Suballocate(binAllocationBuffer, ref binStart, binCapacity); Bounds = bounds; LeafCounts = leafCounts; } @@ -1099,7 +1099,7 @@ static unsafe void BinnedBuilderInternal(Buffer encodedLeafIndices, Buffer< //Those node jobs could spawn multithreaded work that other workers assist with. //Each of those jobs needs its own context for those workers, and the number of jobs is not 1:1 with the workers. //We'll handle such dispatch-required allocations from worker pools. Here, we just preallocate stuff for the first level across all workers. - pool.Take(maximumBinCount * workerCount * (sizeof(BoundingBox4) * 2 + sizeof(int)), out var workerBinsAllocation); + pool.Take(allocatedBinCount * workerCount * (sizeof(BoundingBox4) * 2 + sizeof(int)), out var workerBinsAllocation); BinnedBuildWorkerContext* workerContextsPointer = stackalloc BinnedBuildWorkerContext[workerCount]; var leafCounts = new UnitLeafCount(); @@ -1108,7 +1108,7 @@ static unsafe void BinnedBuilderInternal(Buffer encodedLeafIndices, Buffer< int binAllocationStart = 0; for (int i = 0; i < workerCount; ++i) { - workerContexts[i] = new BinnedBuildWorkerContext(boundingBoxes.As(), workerBinsAllocation, ref binAllocationStart, maximumBinCount, leafCounts); + workerContexts[i] = new BinnedBuildWorkerContext(boundingBoxes.As(), workerBinsAllocation, ref binAllocationStart, allocatedBinCount, leafCounts); } LinkedTaskStack taskStack = default; From d060f18cf84abc860d5b308938109af5e5619f65 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 6 Mar 2023 18:56:40 -0600 Subject: [PATCH 685/947] Refit2 -> Refit. Collapsing branch for simplicity. --- BepuPhysics/Trees/Tree_BinnedRefine.cs | 1 - BepuPhysics/Trees/Tree_Refit.cs | 40 +++---------------- Demos/DemoSet.cs | 4 +- .../SpecializedTests/TreeFiddlingTestDemo.cs | 3 +- 4 files changed, 8 insertions(+), 40 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedRefine.cs b/BepuPhysics/Trees/Tree_BinnedRefine.cs index f2902ae71..b239a2143 100644 --- a/BepuPhysics/Trees/Tree_BinnedRefine.cs +++ b/BepuPhysics/Trees/Tree_BinnedRefine.cs @@ -78,7 +78,6 @@ partial struct Tree var toReturn = Memory + memoryAllocated; memoryAllocated = newSize; return toReturn; - } public static unsafe void CreateBinnedResources(BufferPool bufferPool, int maximumSubtreeCount, out Buffer buffer, out BinnedResources resources) { diff --git a/BepuPhysics/Trees/Tree_Refit.cs b/BepuPhysics/Trees/Tree_Refit.cs index a402b441b..6cb3f2fdf 100644 --- a/BepuPhysics/Trees/Tree_Refit.cs +++ b/BepuPhysics/Trees/Tree_Refit.cs @@ -30,62 +30,32 @@ public unsafe readonly void RefitForNodeBoundsChange(int nodeIndex) } } - //TODO: Recursive approach is a bit silly. Our earlier nonrecursive implementations weren't great, but we could do better. - //This is especially true if we end up changing the memory layout. If we go back to a contiguous array per level, refit becomes trivial. - //That would only happen if it turns out useful for other parts of the execution, though- optimizing refits at the cost of self-tests would be a terrible idea. - readonly unsafe void Refit(int nodeIndex, out Vector3 min, out Vector3 max) - { - Debug.Assert(LeafCount >= 2); - ref var node = ref Nodes[nodeIndex]; - ref var a = ref node.A; - if (node.A.Index >= 0) - { - Refit(a.Index, out a.Min, out a.Max); - } - ref var b = ref node.B; - if (b.Index >= 0) - { - Refit(b.Index, out b.Min, out b.Max); - } - BoundingBox.CreateMerged(a.Min, a.Max, b.Min, b.Max, out min, out max); - } - /// - /// Updates the bounding boxes of all internal nodes in the tree. - /// - public unsafe readonly void Refit() - { - //No point in refitting a tree with no internal nodes! - if (LeafCount <= 2) - return; - Refit(0, out var rootMin, out var rootMax); - } - - readonly unsafe void Refit2(ref NodeChild childInParent) + readonly unsafe void Refit(ref NodeChild childInParent) { Debug.Assert(LeafCount >= 2); ref var node = ref Nodes[childInParent.Index]; ref var a = ref node.A; if (node.A.Index >= 0) { - Refit2(ref a); + Refit(ref a); } ref var b = ref node.B; if (b.Index >= 0) { - Refit2(ref b); + Refit(ref b); } BoundingBox.CreateMergedUnsafeWithPreservation(a, b, out childInParent); } /// /// Updates the bounding boxes of all internal nodes in the tree. /// - public unsafe readonly void Refit2() + public unsafe readonly void Refit() { //No point in refitting a tree with no internal nodes! if (LeafCount <= 2) return; NodeChild stub = default; - Refit2(ref stub); + Refit(ref stub); } } diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index d2433f7df..f1f34eeab 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -47,9 +47,9 @@ struct Option public DemoSet() { //AddOption(); - AddOption(); + //AddOption(); //AddOption(); - AddOption(); + //AddOption(); AddOption(); AddOption(); AddOption(); diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index e0bcf49e8..eb57832c8 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -224,8 +224,7 @@ public override void Initialize(ContentArchive content, Camera camera) // mesh2Pointer->Tree.BinnedRefine(0, ref subtreeReferences, mesh2Pointer->Tree.LeafCount, ref treeletInternalNodes, ref binnedResources, BufferPool); //}, "Original", ref mesh2Pointer->Tree); - //RefitTest(() => mesh.Tree.Refit2(), "refit2", ref mesh.Tree); - //RefitTest(() => mesh.Tree.Refit(), "Original", ref mesh.Tree); + RefitTest(() => mesh.Tree.Refit(), "Refit", ref mesh.Tree); //SelfTest((ref OverlapHandler handler) => mesh.Tree.GetSelfOverlapsContiguousPrepass(ref handler, BufferPool), mesh.Tree.LeafCount, "Prepass"); //SelfTest((ref OverlapHandler handler) => mesh.Tree.GetSelfOverlaps(ref handler), mesh.Tree.LeafCount, "Original"); From bf675799077cb8730b90d730887313b0c80ddf7e Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 6 Mar 2023 19:07:47 -0600 Subject: [PATCH 686/947] CollisionTrackingDemo now handles single threaded case correctly, oops. --- Demos/Demos/CollisionTrackingDemo.cs | 57 +++++++++++++++++++--------- 1 file changed, 40 insertions(+), 17 deletions(-) diff --git a/Demos/Demos/CollisionTrackingDemo.cs b/Demos/Demos/CollisionTrackingDemo.cs index dd326b000..168d650a2 100644 --- a/Demos/Demos/CollisionTrackingDemo.cs +++ b/Demos/Demos/CollisionTrackingDemo.cs @@ -82,10 +82,9 @@ public class CollisionTracker : IDisposable BufferPool pool; Simulation simulation; - public CollisionTracker(BufferPool pool, IThreadDispatcher dispatcher) + public CollisionTracker(BufferPool pool) { this.pool = pool; - this.dispatcher = dispatcher; Tracked = new QuickDictionary(16, pool); } @@ -249,32 +248,56 @@ void PrepareForNextTimestep(float dt, IThreadDispatcher dispatcher) } } - workerCaches = new Buffer>(dispatcher.ThreadCount, pool); - for (int i = 0; i < workerCaches.Length; ++i) + const int initialWorkerCapacity = 512; + if (dispatcher != null) { - workerCaches[i] = new QuickList(512, dispatcher.WorkerPools[i]); + //Cache the dispatcher so ReportContacts can use the buffers in the narrowphase callbacks. + workerCaches = new Buffer>(dispatcher.ThreadCount, pool); + for (int i = 0; i < workerCaches.Length; ++i) + { + workerCaches[i] = new QuickList(initialWorkerCapacity, dispatcher.WorkerPools[i]); + } + this.dispatcher = dispatcher; + } + else + { + //The simulation is being updated without a thread dispatcher, so all allocations can use the main pool. + workerCaches = new Buffer>(1, pool); + workerCaches[0] = new QuickList(initialWorkerCapacity, pool); } } + void FlushWorkerCache(ref QuickList cache, BufferPool pool) + { + for (int j = 0; j < cache.Count; ++j) + { + ref var entry = ref cache[j]; + Tracked.GetTableIndices(ref entry.Self, out _, out var elementIndex); + Tracked.Values[elementIndex].Pairs.Add(entry.Other, entry.Collision, pool); + } + //The worker cache memory must be returned to the thread pool, not the main pool! + cache.Dispose(pool); + } /// /// Flushes all collisions found in the previous timestep into efficient storage for queries. /// void Flush(float dt, IThreadDispatcher threadDispatcher) - { - //Flush the worker caches into the main storage and dispose the caches. - for (int i = 0; i < workerCaches.Length; ++i) + { + if (dispatcher != null) { - ref var cache = ref workerCaches[i]; - for (int j = 0; j < cache.Count; ++j) + //Flush the worker caches into the main storage and dispose the caches. + for (int i = 0; i < workerCaches.Length; ++i) { - ref var entry = ref cache[j]; - Tracked.GetTableIndices(ref entry.Self, out _, out var elementIndex); - Tracked.Values[elementIndex].Pairs.Add(entry.Other, entry.Collision, pool); + FlushWorkerCache(ref workerCaches[i], dispatcher.WorkerPools[i]); } - //The worker cache memory must be returned to the thread pool, not the main pool! - cache.Dispose(dispatcher.WorkerPools[i]); + } + else + { + //Simulation is running sequentially and there's only one thread; the allocations are on the main pool. + FlushWorkerCache(ref workerCaches[0], pool); } workerCaches.Dispose(pool); + this.dispatcher = null; } private void ReportContacts(int workerIndex, ContactSource self, ContactSource other, bool otherIsA, ref TContacts contacts) where TContacts : unmanaged, IContactManifold @@ -282,7 +305,7 @@ private void ReportContacts(int workerIndex, ContactSource self, Cont if (Tracked.ContainsKey(self)) { //A is a listener, add it. - ref var pairContacts = ref workerCaches[workerIndex].Allocate(dispatcher.WorkerPools[workerIndex]); + ref var pairContacts = ref workerCaches[workerIndex].Allocate(dispatcher != null ? dispatcher.WorkerPools[workerIndex] : pool); pairContacts.Self = self; pairContacts.Other = other; pairContacts.Collision.OtherIsAInPair = otherIsA; @@ -418,7 +441,7 @@ public override void Initialize(ContentArchive content, Camera camera) particles = new QuickList(8, BufferPool); - collisionTracker = new CollisionTracker(BufferPool, ThreadDispatcher); + collisionTracker = new CollisionTracker(BufferPool); Simulation = Simulation.Create(BufferPool, new CollisionTrackingCallbacks(collisionTracker), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); From 3f38002d200cede681c57d15bca3ea7b59b09e61 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 6 Mar 2023 19:26:49 -0600 Subject: [PATCH 687/947] Rolled back Refit2->Refit; there remains an oops in there. --- BepuPhysics/Trees/Tree_Refit.cs | 39 ++++++++++++++++++++++++++++----- 1 file changed, 34 insertions(+), 5 deletions(-) diff --git a/BepuPhysics/Trees/Tree_Refit.cs b/BepuPhysics/Trees/Tree_Refit.cs index 6cb3f2fdf..8d74ae223 100644 --- a/BepuPhysics/Trees/Tree_Refit.cs +++ b/BepuPhysics/Trees/Tree_Refit.cs @@ -29,33 +29,62 @@ public unsafe readonly void RefitForNodeBoundsChange(int nodeIndex) metanode = ref Metanodes[metanode.Parent]; } } + //TODO: Recursive approach is a bit silly. Our earlier nonrecursive implementations weren't great, but we could do better. + //This is especially true if we end up changing the memory layout. If we go back to a contiguous array per level, refit becomes trivial. + //That would only happen if it turns out useful for other parts of the execution, though- optimizing refits at the cost of self-tests would be a terrible idea. + readonly unsafe void Refit(int nodeIndex, out Vector3 min, out Vector3 max) + { + Debug.Assert(LeafCount >= 2); + ref var node = ref Nodes[nodeIndex]; + ref var a = ref node.A; + if (node.A.Index >= 0) + { + Refit(a.Index, out a.Min, out a.Max); + } + ref var b = ref node.B; + if (b.Index >= 0) + { + Refit(b.Index, out b.Min, out b.Max); + } + BoundingBox.CreateMerged(a.Min, a.Max, b.Min, b.Max, out min, out max); + } + /// + /// Updates the bounding boxes of all internal nodes in the tree. + /// + public unsafe readonly void Refit() + { + //No point in refitting a tree with no internal nodes! + if (LeafCount <= 2) + return; + Refit(0, out var rootMin, out var rootMax); + } - readonly unsafe void Refit(ref NodeChild childInParent) + readonly unsafe void Refit2(ref NodeChild childInParent) { Debug.Assert(LeafCount >= 2); ref var node = ref Nodes[childInParent.Index]; ref var a = ref node.A; if (node.A.Index >= 0) { - Refit(ref a); + Refit2(ref a); } ref var b = ref node.B; if (b.Index >= 0) { - Refit(ref b); + Refit2(ref b); } BoundingBox.CreateMergedUnsafeWithPreservation(a, b, out childInParent); } /// /// Updates the bounding boxes of all internal nodes in the tree. /// - public unsafe readonly void Refit() + public unsafe readonly void Refit2() { //No point in refitting a tree with no internal nodes! if (LeafCount <= 2) return; NodeChild stub = default; - Refit(ref stub); + Refit2(ref stub); } } From 65a860b35f74357aae3a970e6c60738d86e48438 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 6 Mar 2023 22:17:29 -0600 Subject: [PATCH 688/947] ! --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 40 ++++++++++++++++--- Demos/DemoSet.cs | 2 +- .../SpecializedTests/TreeFiddlingTestDemo.cs | 4 +- 3 files changed, 38 insertions(+), 8 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 033eaac28..6a1507bb2 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -159,6 +159,19 @@ struct BoundsComparerX : IComparerRef { public int Compare(ref Bou struct BoundsComparerY : IComparerRef { public int Compare(ref BoundingBox4 a, ref BoundingBox4 b) => (a.Min.Y + a.Max.Y) > (b.Min.Y + b.Max.Y) ? -1 : 1; } struct BoundsComparerZ : IComparerRef { public int Compare(ref BoundingBox4 a, ref BoundingBox4 b) => (a.Min.Z + a.Max.Z) > (b.Min.Z + b.Max.Z) ? -1 : 1; } + public struct NodeTimes + { + public double Total; + public double CentroidPrepass; + public double Binning; + public double Huh; + public bool MTPrepass; + public bool MTBinning; + public int SubtreeCount; + } + + public static NodeTimes[] Times; + static unsafe void MicroSweepForBinnedBuilder( Vector4 centroidMin, Vector4 centroidMax, Buffer indices, TLeafCounts leafCounts, ref TLeaves leaves, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, Context* context, int workerIndex) @@ -706,9 +719,9 @@ static void BinSubtrees(Vector4 centroidBoundsMin, { ref var box = ref bounds[i]; var binIndex = ComputeBinIndex(centroidBoundsMin, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, box); - ref var xBounds = ref binBoundingBoxes[binIndex]; - xBounds.Min = Vector4.Min(xBounds.Min, box.Min); - xBounds.Max = Vector4.Max(xBounds.Max, box.Max); + ref var binBounds = ref binBoundingBoxes[binIndex]; + binBounds.Min = Vector4.Min(binBounds.Min, box.Min); + binBounds.Max = Vector4.Max(binBounds.Max, box.Max); binLeafCounts[binIndex] += leafCounts[i]; } } @@ -817,6 +830,9 @@ static unsafe void BinnedBuildNode( int subtreeRegionStartIndex, int nodeIndex, int subtreeCount, int parentNodeIndex, int childIndexInParent, Context* context, int workerIndex, IThreadDispatcher dispatcher) where TLeafCounts : unmanaged, ILeafCountBuffer where TLeaves : unmanaged where TThreading : unmanaged, IBinnedBuilderThreading { + var debugStartTime = Stopwatch.GetTimestamp(); + ref var debugTimes = ref Times[nodeIndex]; + debugTimes.SubtreeCount = subtreeCount; var indices = context->Indices.Slice(subtreeRegionStartIndex, subtreeCount); var boundingBoxes = context->BoundingBoxes.Slice(subtreeRegionStartIndex, subtreeCount); var leafCounts = context->LeafCounts.Slice(subtreeRegionStartIndex, subtreeCount); @@ -828,16 +844,20 @@ static unsafe void BinnedBuildNode( BuildNode(boundingBoxes[0], boundingBoxes[1], leafCounts[0], leafCounts[1], nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, 1, 1, ref context->Leaves, out _, out _); return; } + var debugCentroidStartTime = Stopwatch.GetTimestamp(); BoundingBox4 centroidBounds; if (typeof(TThreading) == typeof(SingleThreaded) || subtreeCount < SubtreesPerThreadForCentroidPrepass) { centroidBounds = ComputeCentroidBounds(boundingBoxes); + debugTimes.MTPrepass = false; } else { centroidBounds = MultithreadedCentroidPrepass( (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As>(ref context->Threading)), subtreeRegionStartIndex, subtreeCount, workerIndex, dispatcher); + debugTimes.MTPrepass = true; } + var debugCentroidEndTime = Stopwatch.GetTimestamp(); var centroidSpan = centroidBounds.Max - centroidBounds.Min; var axisIsDegenerate = Vector128.LessThanOrEqual(centroidSpan.AsVector128(), Vector128.Create(1e-12f)); if ((Vector128.ExtractMostSignificantBits(axisIsDegenerate) & 0b111) == 0b111) @@ -904,16 +924,20 @@ static unsafe void BinnedBuildNode( boxX.Max = new Vector4(float.MinValue); binLeafCounts[i] = 0; } + var debugBinStartTime = Stopwatch.GetTimestamp(); if (typeof(TThreading) == typeof(SingleThreaded) || subtreeCount < SubtreesPerThreadForBinning) { BinSubtrees(centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, boundingBoxes, leafCounts, binBoundingBoxes, binLeafCounts); + debugTimes.MTBinning = false; } else { MultithreadedBinSubtrees( (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As>(ref context->Threading)), centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, subtreeRegionStartIndex, subtreeCount, binCount, workerIndex, dispatcher); + debugTimes.MTBinning = true; } + var debugBinEndTime = Stopwatch.GetTimestamp(); //Identify the split index by examining the SAH of very split option. //Premerge from left to right so we have a sorta-summed area table to cheaply look up all possible child A bounds as we scan. @@ -964,13 +988,13 @@ static unsafe void BinnedBuildNode( accumulatedLeafCountB += binLeafCounts[previousIndex]; } - //Choose the best SAH from all axes and split the indices/bounds into two halves for the children to operate on. + var debugHuhStartTime = Stopwatch.GetTimestamp(); + //Split the indices/bounds into two halves for the children to operate on. var subtreeCountB = 0; var subtreeCountA = 0; var splitIndex = bestSplit; var bestboundsA = binBoundingBoxesScan[bestSplit - 1]; var bestboundsB = bestBoundingBoxB; - //Now we have the split index between bins. Go back through and sort the indices and bounds into two halves. while (subtreeCountA + subtreeCountB < subtreeCount) { ref var box = ref boundingBoxes[subtreeCountA]; @@ -1009,10 +1033,16 @@ static unsafe void BinnedBuildNode( ++subtreeCountA; } } + var debugHuhEndTime = Stopwatch.GetTimestamp(); var leafCountB = typeof(TLeafCounts) == typeof(UnitLeafCount) ? subtreeCountB : bestLeafCountB; var leafCountA = typeof(TLeafCounts) == typeof(UnitLeafCount) ? subtreeCountA : totalLeafCount - leafCountB; Debug.Assert(subtreeCountA + subtreeCountB == subtreeCount); BuildNode(bestboundsA, bestboundsB, leafCountA, leafCountB, nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, subtreeCountA, subtreeCountB, ref context->Leaves, out var nodeChildIndexA, out var nodeChildIndexB); + var debugEndTime = Stopwatch.GetTimestamp(); + debugTimes.Total = (debugEndTime - debugStartTime) / (double)Stopwatch.Frequency; + debugTimes.Huh = (debugHuhEndTime - debugHuhStartTime) / (double)Stopwatch.Frequency; + debugTimes.CentroidPrepass = (debugCentroidEndTime - debugCentroidStartTime) / (double)Stopwatch.Frequency; + debugTimes.Binning = (debugBinEndTime - debugBinStartTime) / (double)Stopwatch.Frequency; var shouldPushBOntoMultithreadedQueue = typeof(TThreading) != typeof(SingleThreaded) && subtreeCountA >= SubtreesPerThreadForNodeJob && subtreeCountB >= SubtreesPerThreadForNodeJob; ContinuationHandle nodeBContinuation = default; diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index f1f34eeab..8f2a87401 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -47,7 +47,7 @@ struct Option public DemoSet() { //AddOption(); - //AddOption(); + AddOption(); //AddOption(); //AddOption(); AddOption(); diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index eb57832c8..b2bb426fd 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -154,7 +154,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Position = new Vector3(-10, 3, -10); camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = 0; - + Tree.Times = new Tree.NodeTimes[1 << 22]; for (int i = 0; i < 2; ++i) { BufferPool.Clear(); @@ -224,7 +224,7 @@ public override void Initialize(ContentArchive content, Camera camera) // mesh2Pointer->Tree.BinnedRefine(0, ref subtreeReferences, mesh2Pointer->Tree.LeafCount, ref treeletInternalNodes, ref binnedResources, BufferPool); //}, "Original", ref mesh2Pointer->Tree); - RefitTest(() => mesh.Tree.Refit(), "Refit", ref mesh.Tree); + //RefitTest(() => mesh.Tree.Refit(), "Refit", ref mesh.Tree); //SelfTest((ref OverlapHandler handler) => mesh.Tree.GetSelfOverlapsContiguousPrepass(ref handler, BufferPool), mesh.Tree.LeafCount, "Prepass"); //SelfTest((ref OverlapHandler handler) => mesh.Tree.GetSelfOverlaps(ref handler), mesh.Tree.LeafCount, "Original"); From 97ad268541154fd96f3b88bab9d7f3cd1c0f8313 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 8 Mar 2023 15:17:46 -0600 Subject: [PATCH 689/947] ContactConstraintAccessor now properly filters raw encoded index kinematic state. --- .../ContactConstraintAccessor.cs | 16 ++++++++-------- Demos/DemoSet.cs | 2 +- Demos/Demos/SolverContactEnumerationDemo.cs | 1 - 3 files changed, 9 insertions(+), 10 deletions(-) diff --git a/BepuPhysics/CollisionDetection/ContactConstraintAccessor.cs b/BepuPhysics/CollisionDetection/ContactConstraintAccessor.cs index c551334ec..1cc820fee 100644 --- a/BepuPhysics/CollisionDetection/ContactConstraintAccessor.cs +++ b/BepuPhysics/CollisionDetection/ContactConstraintAccessor.cs @@ -368,13 +368,13 @@ public override void ExtractContactData(in ConstraintLocation constr //Active constraints store body indices as references; inactive constraints store handles. if (constraintLocation.SetIndex == 0) { - bodyHandleA = solver.bodies.ActiveSet.IndexToHandle[bodyReferences.IndexA[0]]; - bodyHandleB = solver.bodies.ActiveSet.IndexToHandle[bodyReferences.IndexB[0]]; + bodyHandleA = solver.bodies.ActiveSet.IndexToHandle[bodyReferences.IndexA[0] & Bodies.BodyReferenceMask]; + bodyHandleB = solver.bodies.ActiveSet.IndexToHandle[bodyReferences.IndexB[0] & Bodies.BodyReferenceMask]; } else { - bodyHandleA = new BodyHandle(bodyReferences.IndexA[0]); - bodyHandleB = new BodyHandle(bodyReferences.IndexB[0]); + bodyHandleA = new BodyHandle(bodyReferences.IndexA[0] & Bodies.BodyReferenceMask); + bodyHandleB = new BodyHandle(bodyReferences.IndexB[0] & Bodies.BodyReferenceMask); } extractor.ConvexTwoBody(bodyHandleA, bodyHandleB, ref prestep, ref impulses); } @@ -471,13 +471,13 @@ public override void ExtractContactData(in ConstraintLocation constr //Active constraints store body indices as references; inactive constraints store handles. if (constraintLocation.SetIndex == 0) { - bodyHandleA = solver.bodies.ActiveSet.IndexToHandle[bodyReferences.IndexA[0]]; - bodyHandleB = solver.bodies.ActiveSet.IndexToHandle[bodyReferences.IndexB[0]]; + bodyHandleA = solver.bodies.ActiveSet.IndexToHandle[bodyReferences.IndexA[0] & Bodies.BodyReferenceMask]; + bodyHandleB = solver.bodies.ActiveSet.IndexToHandle[bodyReferences.IndexB[0] & Bodies.BodyReferenceMask]; } else { - bodyHandleA = new BodyHandle(bodyReferences.IndexA[0]); - bodyHandleB = new BodyHandle(bodyReferences.IndexB[0]); + bodyHandleA = new BodyHandle(bodyReferences.IndexA[0] & Bodies.BodyReferenceMask); + bodyHandleB = new BodyHandle(bodyReferences.IndexB[0] & Bodies.BodyReferenceMask); } extractor.NonconvexTwoBody(bodyHandleA, bodyHandleB, ref prestep, ref impulses); } diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index 8f2a87401..f1f34eeab 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -47,7 +47,7 @@ struct Option public DemoSet() { //AddOption(); - AddOption(); + //AddOption(); //AddOption(); //AddOption(); AddOption(); diff --git a/Demos/Demos/SolverContactEnumerationDemo.cs b/Demos/Demos/SolverContactEnumerationDemo.cs index 45d325354..de51a7571 100644 --- a/Demos/Demos/SolverContactEnumerationDemo.cs +++ b/Demos/Demos/SolverContactEnumerationDemo.cs @@ -219,7 +219,6 @@ public override void Initialize(ContentArchive content, Camera camera) } } - sensorBodyHandle = Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 0, 1), 10, Simulation.Shapes, new Box(4, 2, 6))); //Put a mesh under the sensor so that nonconvex contacts are shown. From a886f2fda18bfb3d8bc1e9c8b89644c44fbcc59c Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 8 Mar 2023 18:02:48 -0600 Subject: [PATCH 690/947] Sponsor update! --- Demos/Demos/Sponsors/SponsorDemo.cs | 1 + 1 file changed, 1 insertion(+) diff --git a/Demos/Demos/Sponsors/SponsorDemo.cs b/Demos/Demos/Sponsors/SponsorDemo.cs index a0cd127f9..ac51d4704 100644 --- a/Demos/Demos/Sponsors/SponsorDemo.cs +++ b/Demos/Demos/Sponsors/SponsorDemo.cs @@ -63,6 +63,7 @@ public override void LoadGraphicalContent(ContentArchive content, RenderSurface sponsors0.Add("C. S."); sponsors0.Add("R. W."); sponsors0.Add("AshleighAdams"); + sponsors0.Add("LoicBaumann"); //These supporters are those who gave 10 dollars a month (or historical backers of roughly equivalent or greater total contribution). //They get a poorly drawn animal! From fcebe2f93c8b6ce3ed5c8bc2301ad7bc0d054413 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 8 Mar 2023 22:02:51 -0600 Subject: [PATCH 691/947] Leafcounts/indices bundled into bounding box representation (nodechild) for subtree input. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 531 ++++++++---------- Demos/DemoSet.cs | 2 +- .../SpecializedTests/TreeFiddlingTestDemo.cs | 31 +- 3 files changed, 245 insertions(+), 319 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 6a1507bb2..6f094b104 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -29,7 +29,7 @@ struct LeavesHandledInPostPass { } static void BuildNode( BoundingBox4 a, BoundingBox4 b, int leafCountA, int leafCountB, - Buffer nodes, Buffer metanodes, Buffer indices, + Buffer subtrees, Buffer nodes, Buffer metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, int subtreeCountA, int subtreeCountB, ref TLeaves leaves, out int aIndex, out int bIndex) where TLeaves : unmanaged { @@ -39,9 +39,12 @@ static void BuildNode( metanode.IndexInParent = childIndexInParent; metanode.RefineFlag = 0; ref var node = ref nodes[0]; + node.A = Unsafe.As(ref a); + node.B = Unsafe.As(ref b); if (subtreeCountA == 1) { - aIndex = indices[0]; + aIndex = subtrees[0].Index; + Debug.Assert(leafCountA == 1); if (typeof(TLeaves) == typeof(Buffer)) { Debug.Assert(aIndex < 0, "During building, any subtreeCount of 1 should imply a leaf."); @@ -52,10 +55,12 @@ static void BuildNode( else { aIndex = nodeIndex + 1; + node.A.Index = aIndex; } if (subtreeCountB == 1) { - bIndex = indices[^1]; + bIndex = subtrees[^1].Index; + Debug.Assert(leafCountB == 1); if (typeof(TLeaves) == typeof(Buffer)) { Debug.Assert(bIndex < 0, "During building, any subtreeCount of 1 should imply a leaf."); @@ -66,13 +71,8 @@ static void BuildNode( else { bIndex = nodeIndex + subtreeCountA; //parentNodeIndex + 1 + (subtreeCountA - 1) + node.B.Index = bIndex; } - node.A = Unsafe.As(ref a); - node.B = Unsafe.As(ref b); - node.A.Index = aIndex; - node.A.LeafCount = leafCountA; - node.B.Index = bIndex; - node.B.LeafCount = leafCountB; } internal static float ComputeBoundsMetric(BoundingBox4 bounds) => ComputeBoundsMetric(bounds.Min, bounds.Max); @@ -87,39 +87,13 @@ internal static float ComputeBoundsMetric(Vector4 min, Vector4 max) } - interface ILeafCountBuffer where T : unmanaged, ILeafCountBuffer - { - int this[int index] { get; set; } - T Slice(int startIndex, int count); - } - - /// - /// An implicit buffer where every slot contains a 1. - /// - struct UnitLeafCount : ILeafCountBuffer - { - public int this[int index] { get => 1; set { } } - public UnitLeafCount Slice(int startIndex, int count) => this; - } - - /// - /// Leaf counts buffer with actual values. - /// - struct LeafCountBuffer : ILeafCountBuffer - { - public Buffer LeafCounts; - public int this[int index] { get => LeafCounts[index]; set => LeafCounts[index] = value; } - public LeafCountBuffer Slice(int startIndex, int count) => new() { LeafCounts = LeafCounts.Slice(startIndex, count) }; - } - interface IBinnedBuilderThreading { void GetBins(int workerIndex, out Buffer binBoundingBoxes, out Buffer binBoundingBoxesScan, out Buffer binLeafCounts); } - struct Context - where TLeafCounts : unmanaged, ILeafCountBuffer + struct Context where TLeaves : unmanaged where TThreading : unmanaged, IBinnedBuilderThreading { @@ -128,36 +102,33 @@ struct Context public float LeafToBinMultiplier; public int MicrosweepThreshold; - public Buffer Indices; - public TLeafCounts LeafCounts; public TLeaves Leaves; - public Buffer BoundingBoxes; + public Buffer SubtreesPing; + public Buffer SubtreesPong; public Buffer Nodes; public Buffer Metanodes; public TThreading Threading; public Context(int minimumBinCount, int maximumBinCount, float leafToBinMultiplier, int microsweepThreshold, - Buffer indices, TLeafCounts leafCounts, TLeaves leaves, Buffer boundingBoxes, - Buffer nodes, Buffer metanodes, TThreading threading) + Buffer subtreesPing, Buffer subtreesPong, TLeaves leaves, Buffer nodes, Buffer metanodes, TThreading threading) { MinimumBinCount = minimumBinCount; MaximumBinCount = maximumBinCount; LeafToBinMultiplier = leafToBinMultiplier; MicrosweepThreshold = microsweepThreshold; - Indices = indices; - LeafCounts = leafCounts; + SubtreesPing = subtreesPing; + SubtreesPong = subtreesPong; Leaves = leaves; - BoundingBoxes = boundingBoxes; Nodes = nodes; Metanodes = metanodes; Threading = threading; } } - struct BoundsComparerX : IComparerRef { public int Compare(ref BoundingBox4 a, ref BoundingBox4 b) => (a.Min.X + a.Max.X) > (b.Min.X + b.Max.X) ? -1 : 1; } - struct BoundsComparerY : IComparerRef { public int Compare(ref BoundingBox4 a, ref BoundingBox4 b) => (a.Min.Y + a.Max.Y) > (b.Min.Y + b.Max.Y) ? -1 : 1; } - struct BoundsComparerZ : IComparerRef { public int Compare(ref BoundingBox4 a, ref BoundingBox4 b) => (a.Min.Z + a.Max.Z) > (b.Min.Z + b.Max.Z) ? -1 : 1; } + struct BoundsComparerX : IComparerRef { public int Compare(ref NodeChild a, ref NodeChild b) => (a.Min.X + a.Max.X) > (b.Min.X + b.Max.X) ? -1 : 1; } + struct BoundsComparerY : IComparerRef { public int Compare(ref NodeChild a, ref NodeChild b) => (a.Min.Y + a.Max.Y) > (b.Min.Y + b.Max.Y) ? -1 : 1; } + struct BoundsComparerZ : IComparerRef { public int Compare(ref NodeChild a, ref NodeChild b) => (a.Min.Z + a.Max.Z) > (b.Min.Z + b.Max.Z) ? -1 : 1; } public struct NodeTimes { @@ -172,16 +143,19 @@ public struct NodeTimes public static NodeTimes[] Times; - static unsafe void MicroSweepForBinnedBuilder( - Vector4 centroidMin, Vector4 centroidMax, Buffer indices, TLeafCounts leafCounts, ref TLeaves leaves, - Buffer boundingBoxes, Buffer nodes, Buffer metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, Context* context, int workerIndex) - where TLeafCounts : unmanaged, ILeafCountBuffer where TLeaves : unmanaged where TThreading : unmanaged, IBinnedBuilderThreading + static unsafe void MicroSweepForBinnedBuilder( + Vector4 centroidMin, Vector4 centroidMax, ref TLeaves leaves, + Buffer subtrees, Buffer nodes, Buffer metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, Context* context, int workerIndex) + where TLeaves : unmanaged where TThreading : unmanaged, IBinnedBuilderThreading { //This is a very small scale sweep build. - var subtreeCount = indices.Length; + var subtreeCount = subtrees.Length; if (subtreeCount == 2) { - BuildNode(boundingBoxes[0], boundingBoxes[1], leafCounts[0], leafCounts[1], nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, 1, 1, ref leaves, out _, out _); + ref var subtreeA = ref subtrees[0]; + ref var subtreeB = ref subtrees[1]; + BuildNode(Unsafe.As(ref subtreeA), Unsafe.As(ref subtreeB), subtreeA.LeafCount, subtreeB.LeafCount, subtrees, + nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, 1, 1, ref leaves, out _, out _); return; } var centroidSpan = centroidMax - centroidMin; @@ -202,7 +176,7 @@ static unsafe void MicroSweepForBinnedBuilder( { for (int i = 0; i < subtreeCount; ++i) { - ref var bounds = ref boundingBoxes[i]; + ref var bounds = ref subtrees[i]; keys[i] = bounds.Min.X + bounds.Max.X; } } @@ -210,7 +184,7 @@ static unsafe void MicroSweepForBinnedBuilder( { for (int i = 0; i < subtreeCount; ++i) { - ref var bounds = ref boundingBoxes[i]; + ref var bounds = ref subtrees[i]; keys[i] = bounds.Min.Y + bounds.Max.Y; } } @@ -218,7 +192,7 @@ static unsafe void MicroSweepForBinnedBuilder( { for (int i = 0; i < subtreeCount; ++i) { - ref var bounds = ref boundingBoxes[i]; + ref var bounds = ref subtrees[i]; keys[i] = bounds.Min.Z + bounds.Max.Z; } } @@ -228,107 +202,43 @@ static unsafe void MicroSweepForBinnedBuilder( } VectorizedSorts.VectorCountingSort(keys, targetIndices, subtreeCount); - //Now that we know the target indices, copy things back. + //Now that we know the target indices, copy things into position. //Have to copy things into a temporary cache to avoid overwrites since we didn't do any shuffling during the sort. //Note that we can now reuse the keys memory. - if (typeof(TLeafCounts) != typeof(LeafCountBuffer)) + var subtreeCache = binBoundingBoxesScan.As(); + subtrees.CopyTo(0, subtreeCache, 0, subtreeCount); + for (int i = 0; i < subtreeCount; ++i) { - //There aren't any leaf counts that we need to copy; they're all just 1 anyway. - Debug.Assert(typeof(TLeafCounts) == typeof(UnitLeafCount)); - var indicesCache = new Buffer(binBoundingBoxes.Memory, subtreeCount); - var boundingBoxCache = binBoundingBoxesScan; - boundingBoxes.CopyTo(0, boundingBoxCache, 0, subtreeCount); - indices.CopyTo(0, indicesCache, 0, subtreeCount); - for (int i = 0; i < subtreeCount; ++i) - { - var targetIndex = targetIndices[i]; - boundingBoxes[targetIndex] = boundingBoxCache[i]; - indices[targetIndex] = indicesCache[i]; - } - } - else - { - //There are actual leaf counts we need to worry about on top of the rest! - var indicesCache = new Buffer(binBoundingBoxes.Memory, subtreeCount); - var leafCountCache = new Buffer(targetIndices.Memory + subtreeCount, subtreeCount); - var boundingBoxCache = binBoundingBoxesScan; - boundingBoxes.CopyTo(0, boundingBoxCache, 0, subtreeCount); - indices.CopyTo(0, indicesCache, 0, subtreeCount); - var leafCountBuffer = Unsafe.As(ref leafCounts).LeafCounts; - leafCountBuffer.CopyTo(0, leafCountCache, 0, subtreeCount); - for (int i = 0; i < subtreeCount; ++i) - { - var targetIndex = targetIndices[i]; - boundingBoxes[targetIndex] = boundingBoxCache[i]; - indices[targetIndex] = indicesCache[i]; - leafCountBuffer[targetIndex] = leafCountCache[i]; - } + var targetIndex = targetIndices[i]; + subtrees[targetIndex] = subtreeCache[i]; } } else { //No vectorization supported. Fall back to poopymode! - if (typeof(TLeafCounts) != typeof(LeafCountBuffer)) + if (centroidSpan.X > centroidSpan.Y && centroidSpan.X > centroidSpan.Z) { - if (centroidSpan.X > centroidSpan.Y && centroidSpan.X > centroidSpan.Z) - { - var comparer = new BoundsComparerX(); - QuickSort.Sort(ref boundingBoxes[0], ref indices[0], 0, subtreeCount - 1, ref comparer); - } - else if (centroidSpan.Y > centroidSpan.Z) - { - var comparer = new BoundsComparerY(); - QuickSort.Sort(ref boundingBoxes[0], ref indices[0], 0, subtreeCount - 1, ref comparer); - } - else - { - var comparer = new BoundsComparerZ(); - QuickSort.Sort(ref boundingBoxes[0], ref indices[0], 0, subtreeCount - 1, ref comparer); - } + var comparer = new BoundsComparerX(); + QuickSort.Sort(ref subtrees[0], 0, subtreeCount - 1, ref comparer); + } + else if (centroidSpan.Y > centroidSpan.Z) + { + var comparer = new BoundsComparerY(); + QuickSort.Sort(ref subtrees[0], 0, subtreeCount - 1, ref comparer); } else { - //There are leaf counts that we need to sort alongside the rest. This is a pretty low value codepath, so we'll just create a targetIndices buffer. - var targetIndices = new Buffer(binBoundingBoxes.Memory, subtreeCount); - for (int i = 0; i < subtreeCount; ++i) - { - targetIndices[i] = i; - } - if (centroidSpan.X > centroidSpan.Y && centroidSpan.X > centroidSpan.Z) - { - var comparer = new BoundsComparerX(); - QuickSort.Sort(ref boundingBoxes[0], ref targetIndices[0], 0, subtreeCount - 1, ref comparer); - } - else if (centroidSpan.Y > centroidSpan.Z) - { - var comparer = new BoundsComparerY(); - QuickSort.Sort(ref boundingBoxes[0], ref targetIndices[0], 0, subtreeCount - 1, ref comparer); - } - else - { - var comparer = new BoundsComparerZ(); - QuickSort.Sort(ref boundingBoxes[0], ref targetIndices[0], 0, subtreeCount - 1, ref comparer); - } - //Apply the swaps to indices and leaf counts. - var indicesCache = new Buffer(targetIndices.Memory + subtreeCount, subtreeCount); - var leafCountCache = new Buffer(indicesCache.Memory + subtreeCount, subtreeCount); - indices.CopyTo(0, indicesCache, 0, subtreeCount); - var leafCountBuffer = Unsafe.As(ref leafCounts).LeafCounts; - leafCountBuffer.CopyTo(0, leafCountCache, 0, subtreeCount); - for (int i = 0; i < subtreeCount; ++i) - { - var targetIndex = targetIndices[i]; - leafCountBuffer[targetIndex] = leafCountCache[i]; - indices[targetIndex] = indicesCache[i]; - } + var comparer = new BoundsComparerZ(); + QuickSort.Sort(ref subtrees[0], 0, subtreeCount - 1, ref comparer); } } Debug.Assert(subtreeCount <= context->MaximumBinCount || subtreeCount <= context->MicrosweepThreshold, "We're reusing the bin resources under the assumption that this is only ever called when there are less leaves than maximum bins."); //Identify the split index by examining the SAH of very split option. //Premerge from left to right so we have a sorta-summed area table to cheaply look up all possible child A bounds as we scan. + var boundingBoxes = subtrees.As(); binBoundingBoxesScan[0] = boundingBoxes[0]; - int totalLeafCount = typeof(TLeafCounts) == typeof(LeafCountBuffer) ? leafCounts[0] : subtreeCount; + int totalLeafCount = subtrees[0].LeafCount; for (int i = 1; i < subtreeCount; ++i) { var previousIndex = i - 1; @@ -337,8 +247,7 @@ static unsafe void MicroSweepForBinnedBuilder( ref var bounds = ref boundingBoxes[i]; scanBounds.Min = Vector4.Min(bounds.Min, previousScanBounds.Min); scanBounds.Max = Vector4.Max(bounds.Max, previousScanBounds.Max); - if (typeof(TLeafCounts) == typeof(LeafCountBuffer)) - totalLeafCount += leafCounts[i]; + totalLeafCount += subtrees[i].LeafCount; } float bestSAH = float.MaxValue; @@ -360,23 +269,20 @@ static unsafe void MicroSweepForBinnedBuilder( bestSAH = sahCandidate; bestSplit = splitIndexCandidate; bestBoundsB = accumulatedBoundingBoxB; - if (typeof(TLeafCounts) == typeof(LeafCountBuffer)) - bestLeafCountB = accumulatedLeafCountB; + bestLeafCountB = accumulatedLeafCountB; } ref var bounds = ref boundingBoxes[previousIndex]; accumulatedBoundingBoxB.Min = Vector4.Min(bounds.Min, accumulatedBoundingBoxB.Min); accumulatedBoundingBoxB.Max = Vector4.Max(bounds.Max, accumulatedBoundingBoxB.Max); - accumulatedLeafCountB += leafCounts[previousIndex]; + accumulatedLeafCountB += subtrees[previousIndex].LeafCount; } var bestBoundsA = binBoundingBoxesScan[bestSplit - 1]; var subtreeCountA = bestSplit; var subtreeCountB = subtreeCount - bestSplit; - var bestLeafCountA = typeof(TLeafCounts) == typeof(UnitLeafCount) ? subtreeCountA : totalLeafCount - bestLeafCountB; - if (typeof(TLeafCounts) == typeof(UnitLeafCount)) - bestLeafCountB = subtreeCountB; + var bestLeafCountA = totalLeafCount - bestLeafCountB; - BuildNode(bestBoundsA, bestBoundsB, bestLeafCountA, bestLeafCountB, nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, subtreeCountA, subtreeCountB, ref leaves, out var aIndex, out var bIndex); + BuildNode(bestBoundsA, bestBoundsB, bestLeafCountA, bestLeafCountB, subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, subtreeCountA, subtreeCountB, ref leaves, out var aIndex, out var bIndex); if (subtreeCountA > 1) { var aBounds = boundingBoxes.Slice(subtreeCountA); @@ -384,10 +290,11 @@ static unsafe void MicroSweepForBinnedBuilder( for (int i = 1; i < subtreeCountA; ++i) { ref var bounds = ref aBounds[i]; + //TODO: THIS SHOULD BE CENTROID, NOT FULL MERGE centroidBoundsA.Min = Vector4.Min(centroidBoundsA.Min, bounds.Min); centroidBoundsA.Max = Vector4.Max(centroidBoundsA.Max, bounds.Max); } - MicroSweepForBinnedBuilder(centroidBoundsA.Min, centroidBoundsA.Max, indices.Slice(subtreeCountA), leafCounts.Slice(0, subtreeCountA), ref leaves, aBounds, nodes.Slice(1, subtreeCountA - 1), metanodes.Slice(1, subtreeCountA - 1), aIndex, nodeIndex, 0, context, workerIndex); + MicroSweepForBinnedBuilder(centroidBoundsA.Min, centroidBoundsA.Max, ref leaves, subtrees.Slice(subtreeCountA), nodes.Slice(1, subtreeCountA - 1), metanodes.Slice(1, subtreeCountA - 1), aIndex, nodeIndex, 0, context, workerIndex); } if (subtreeCountB > 1) { @@ -396,10 +303,11 @@ static unsafe void MicroSweepForBinnedBuilder( for (int i = 0; i < subtreeCountB; ++i) { ref var bounds = ref bBounds[i]; + //TODO: THIS SHOULD BE CENTROID, NOT FULL MERGE centroidBoundsB.Min = Vector4.Min(centroidBoundsB.Min, bounds.Min); centroidBoundsB.Max = Vector4.Max(centroidBoundsB.Max, bounds.Max); } - MicroSweepForBinnedBuilder(centroidBoundsB.Min, centroidBoundsB.Max, indices.Slice(subtreeCountA, subtreeCountB), leafCounts.Slice(subtreeCountA, subtreeCountB), ref leaves, bBounds, nodes.Slice(subtreeCountA, subtreeCountB - 1), metanodes.Slice(subtreeCountA, subtreeCountB - 1), bIndex, nodeIndex, 1, context, workerIndex); + MicroSweepForBinnedBuilder(centroidBoundsB.Min, centroidBoundsB.Max, ref leaves, subtrees.Slice(subtreeCountA, subtreeCountB), nodes.Slice(subtreeCountA, subtreeCountB - 1), metanodes.Slice(subtreeCountA, subtreeCountB - 1), bIndex, nodeIndex, 1, context, workerIndex); } } @@ -444,7 +352,7 @@ static Buffer Suballocate(Buffer buffer, ref int start, int count) w /// /// Stores resources required by a worker to dispatch and manage multithreaded work. /// - unsafe struct BinnedBuildWorkerContext where TLeafCounts : unmanaged, ILeafCountBuffer + unsafe struct BinnedBuildWorkerContext { /// /// Bins associated with this worker for the duration of a node. This allocation will persist across the build. @@ -466,24 +374,19 @@ unsafe struct BinnedBuildWorkerContext where TLeafCounts : unmanage public Buffer BinLeafCounts; /// - /// Bounds for all subtrees in the builder. Cached in the worker to simplify accesses. - /// - public Buffer Bounds; - /// - /// Leaf counts for all subtrees in the builder. Cached in the worker to simplify accesses. + /// Cached view of all subtrees the builder is working on to simplify accesses. /// - public TLeafCounts LeafCounts; + public Buffer Subtrees; - public BinnedBuildWorkerContext(Buffer bounds, Buffer binAllocationBuffer, ref int binStart, int binCapacity, TLeafCounts leafCounts) + public BinnedBuildWorkerContext(Buffer subtrees, Buffer binAllocationBuffer, ref int binStart, int binCapacity) { BinBoundingBoxes = Suballocate(binAllocationBuffer, ref binStart, binCapacity); BinBoundingBoxesScan = Suballocate(binAllocationBuffer, ref binStart, binCapacity); BinLeafCounts = Suballocate(binAllocationBuffer, ref binStart, binCapacity); - Bounds = bounds; - LeafCounts = leafCounts; + Subtrees = subtrees; } } - unsafe struct MultithreadBinnedBuildContext : IBinnedBuilderThreading where TLeafCounts : unmanaged, ILeafCountBuffer + unsafe struct MultithreadBinnedBuildContext : IBinnedBuilderThreading { public LinkedTaskStack* TaskStack; /// @@ -491,7 +394,7 @@ unsafe struct MultithreadBinnedBuildContext : IBinnedBuilderThreadi /// If you have far more tasks than there are workers, adding more tasks just adds overhead without additional workstealing advantages. /// public int MaximumTaskCountPerSubmission; - public Buffer> Workers; + public Buffer Workers; public void GetBins(int workerIndex, out Buffer binBoundingBoxes, out Buffer binBoundingBoxesScan, out Buffer binLeafCounts) { @@ -567,16 +470,16 @@ unsafe struct CentroidPrepassTaskContext /// public Buffer PrepassWorkers; /// - /// Buffer containing all bounds in the tree. + /// Buffer containing all subtrees being built. /// - public Buffer Bounds; + public Buffer Subtrees; - public CentroidPrepassTaskContext(BufferPool pool, SharedTaskData taskData, Buffer bounds) + public CentroidPrepassTaskContext(BufferPool pool, SharedTaskData taskData, Buffer subtrees) { TaskData = taskData; pool.Take(int.Min(taskData.WorkerCount, taskData.TaskCount), out PrepassWorkers); Debug.Assert(PrepassWorkers.Length >= 2); - Bounds = bounds; + Subtrees = subtrees; } public void Dispose(BufferPool pool) => pool.Return(ref PrepassWorkers); @@ -586,7 +489,7 @@ unsafe static void CentroidPrepassWorker(long taskId, void* untypedContext, int ref var context = ref *(CentroidPrepassTaskContext*)untypedContext; Debug.Assert(context.TaskData.WorkerCount > 1 && context.TaskData.TaskCount > 1 && context.TaskData.WorkerCount < 100); context.TaskData.GetSlotInterval(taskId, out var start, out var count); - var centroidBounds = ComputeCentroidBounds(context.Bounds.Slice(start, count)); + var centroidBounds = ComputeCentroidBounds(context.Subtrees.Slice(start, count).As()); if (context.TaskData.TaskCountFitsInWorkerCount) { //There were less tasks than workers; directly write into the slot without bothering to merge. @@ -600,12 +503,11 @@ unsafe static void CentroidPrepassWorker(long taskId, void* untypedContext, int } } - unsafe static BoundingBox4 MultithreadedCentroidPrepass(MultithreadBinnedBuildContext* context, int subtreeStartIndex, int subtreeCount, int workerIndex, IThreadDispatcher dispatcher) - where TLeafCounts : unmanaged, ILeafCountBuffer + unsafe static BoundingBox4 MultithreadedCentroidPrepass(MultithreadBinnedBuildContext* context, int subtreeStartIndex, int subtreeCount, int workerIndex, IThreadDispatcher dispatcher) { ref var worker = ref context->Workers[workerIndex]; var workerPool = dispatcher.WorkerPools[workerIndex]; - var taskContext = new CentroidPrepassTaskContext(workerPool, new SharedTaskData(context->Workers.Length, subtreeStartIndex, subtreeCount, SubtreesPerThreadForCentroidPrepass, context->MaximumTaskCountPerSubmission), worker.Bounds); + var taskContext = new CentroidPrepassTaskContext(workerPool, new SharedTaskData(context->Workers.Length, subtreeStartIndex, subtreeCount, SubtreesPerThreadForCentroidPrepass, context->MaximumTaskCountPerSubmission), worker.Subtrees); Debug.Assert(taskContext.TaskData.TaskCount > 1, "This codepath shouldn't be used if there's only one task!"); Debug.Assert(taskContext.TaskData.WorkerCount > 1 && taskContext.TaskData.WorkerCount < 100); var taskCount = taskContext.TaskData.TaskCount; @@ -641,7 +543,7 @@ struct BinSubtreesWorkerContext public Buffer BinBoundingBoxes; public Buffer BinLeafCounts; } - unsafe struct BinSubtreesTaskContext where TLeafCounts : unmanaged, ILeafCountBuffer + unsafe struct BinSubtreesTaskContext { public SharedTaskData TaskData; /// @@ -658,13 +560,9 @@ unsafe struct BinSubtreesTaskContext where TLeafCounts : unmanaged, public Buffer WorkerHelpedWithBinning; /// - /// Buffer containing all bounds in the tree. - /// - public Buffer Bounds; - /// - /// Buffer containing all leaf counts in the tree. + /// Buffer containing all subtrees under construction. /// - public TLeafCounts LeafCounts; + public Buffer Subtrees; public int BinCount; public bool UseX, UseY; @@ -674,13 +572,12 @@ unsafe struct BinSubtreesTaskContext where TLeafCounts : unmanaged, public Vector4 OffsetToBinIndex; public Vector4 MaximumBinIndex; - public BinSubtreesTaskContext(BufferPool pool, SharedTaskData taskData, Buffer bounds, TLeafCounts leafCounts, + public BinSubtreesTaskContext(BufferPool pool, SharedTaskData taskData, Buffer subtrees, int binCount, bool useX, bool useY, Vector128 permuteMask, int axisIndex, Vector4 centroidBoundsMin, Vector4 offsetToBinIndex, Vector4 maximumBinIndex) { TaskData = taskData; - Bounds = bounds; - LeafCounts = leafCounts; + Subtrees = subtrees; BinCount = binCount; UseX = useX; UseY = useY; @@ -708,26 +605,26 @@ public BinSubtreesTaskContext(BufferPool pool, SharedTaskData taskData, Buffer(Vector4 centroidBoundsMin, + static void BinSubtrees(Vector4 centroidBoundsMin, bool useX, bool useY, Vector128 permuteMask, int axisIndex, Vector4 offsetToBinIndex, Vector4 maximumBinIndex, - Buffer bounds, TLeafCounts leafCounts, - Buffer binBoundingBoxes, Buffer binLeafCounts) where TLeafCounts : unmanaged, ILeafCountBuffer + Buffer subtrees, Buffer binBoundingBoxes, Buffer binLeafCounts) { //Note that we don't store out any of the indices into per-bin lists here. We only *really* want two final groups for the children, //and we can easily compute those by performing another scan. It requires recomputing the bin indices, but that's really not much of a concern. - for (int i = 0; i < bounds.Length; ++i) + for (int i = 0; i < subtrees.Length; ++i) { - ref var box = ref bounds[i]; + ref var subtree = ref subtrees[i]; + ref var box = ref Unsafe.As(ref subtree); var binIndex = ComputeBinIndex(centroidBoundsMin, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, box); ref var binBounds = ref binBoundingBoxes[binIndex]; binBounds.Min = Vector4.Min(binBounds.Min, box.Min); binBounds.Max = Vector4.Max(binBounds.Max, box.Max); - binLeafCounts[binIndex] += leafCounts[i]; + binLeafCounts[binIndex] += subtree.LeafCount; } } - unsafe static void BinSubtreesWorker(long taskId, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) where TLeafCounts : unmanaged, ILeafCountBuffer + unsafe static void BinSubtreesWorker(long taskId, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) { - ref var context = ref *(BinSubtreesTaskContext*)untypedContext; + ref var context = ref *(BinSubtreesTaskContext*)untypedContext; Debug.Assert(context.TaskData.WorkerCount > 1 && context.TaskData.TaskCount > 1 && context.TaskData.WorkerCount < 100); //Note that if we have more workers than tasks, we use the task id to index into the caches (and initialize the data here rather then before dispatching). var effectiveWorkerIndex = context.TaskData.TaskCountFitsInWorkerCount ? (int)taskId : workerIndex; @@ -745,19 +642,19 @@ unsafe static void BinSubtreesWorker(long taskId, void* untypedCont } context.TaskData.GetSlotInterval(taskId, out var start, out var count); BinSubtrees(context.CentroidBoundsMin, context.UseX, context.UseY, context.PermuteMask, context.AxisIndex, context.OffsetToBinIndex, context.MaximumBinIndex, - context.Bounds.Slice(start, count), context.LeafCounts.Slice(start, count), worker.BinBoundingBoxes, worker.BinLeafCounts); + context.Subtrees.Slice(start, count), worker.BinBoundingBoxes, worker.BinLeafCounts); } - unsafe static void MultithreadedBinSubtrees(MultithreadBinnedBuildContext* context, + unsafe static void MultithreadedBinSubtrees(MultithreadBinnedBuildContext* context, Vector4 centroidBoundsMin, bool useX, bool useY, Vector128 permuteMask, int axisIndex, Vector4 offsetToBinIndex, Vector4 maximumBinIndex, - int subtreeStartIndex, int subtreeCount, int binCount, int workerIndex, IThreadDispatcher dispatcher) where TLeafCounts : unmanaged, ILeafCountBuffer + int subtreeStartIndex, int subtreeCount, int binCount, int workerIndex, IThreadDispatcher dispatcher) { ref var worker = ref context->Workers[workerIndex]; var workerPool = dispatcher.WorkerPools[workerIndex]; - var taskContext = new BinSubtreesTaskContext( + var taskContext = new BinSubtreesTaskContext( workerPool, new SharedTaskData(context->Workers.Length, subtreeStartIndex, subtreeCount, SubtreesPerThreadForBinning, context->MaximumTaskCountPerSubmission), - worker.Bounds, worker.LeafCounts, binCount, useX, useY, permuteMask, axisIndex, centroidBoundsMin, offsetToBinIndex, maximumBinIndex); + worker.Subtrees, binCount, useX, useY, permuteMask, axisIndex, centroidBoundsMin, offsetToBinIndex, maximumBinIndex); //Don't bother initializing more slots than we have tasks. Note that this requires special handling on the task level; //if we have less tasks than workers, then the task needs to distinguish that fact. @@ -778,7 +675,7 @@ unsafe static void MultithreadedBinSubtrees(MultithreadBinnedBuildC } } - context->TaskStack->For(&BinSubtreesWorker, &taskContext, 0, taskContext.TaskData.TaskCount, workerIndex, dispatcher); + context->TaskStack->For(&BinSubtreesWorker, &taskContext, 0, taskContext.TaskData.TaskCount, workerIndex, dispatcher); //Unless the number of threads and bins is really huge, there's no value in attempting to multithread the final compression. //(Parallel reduction is an option, but even then... I suspect the single threaded version will be faster. And it's way simpler.) @@ -808,40 +705,40 @@ unsafe static void MultithreadedBinSubtrees(MultithreadBinnedBuildC taskContext.Dispose(workerPool); } - unsafe struct NodePushTaskContext - where TLeafCounts : unmanaged, ILeafCountBuffer where TLeaves : unmanaged where TThreading : unmanaged, IBinnedBuilderThreading + unsafe struct NodePushTaskContext + where TLeaves : unmanaged where TThreading : unmanaged, IBinnedBuilderThreading { - public Context* Context; + public Context* Context; public int NodeIndex; public int ParentNodeIndex; //Subtree region start index and subtree count are both encoded into the task id. } - unsafe static void BinnedBuilderNodeWorker(long taskId, void* context, int workerIndex, IThreadDispatcher dispatcher) - where TLeafCounts : unmanaged, ILeafCountBuffer where TLeaves : unmanaged where TThreading : unmanaged, IBinnedBuilderThreading + unsafe static void BinnedBuilderNodeWorker(long taskId, void* context, int workerIndex, IThreadDispatcher dispatcher) + where TLeaves : unmanaged where TThreading : unmanaged, IBinnedBuilderThreading { var subtreeRegionStartIndex = (int)taskId; var subtreeCount = (int)(taskId >> 32); - var nodePushContext = (NodePushTaskContext*)context; + var nodePushContext = (NodePushTaskContext*)context; //Note that child index is always 1 because we only ever push child B. - BinnedBuildNode(subtreeRegionStartIndex, nodePushContext->NodeIndex, subtreeCount, nodePushContext->ParentNodeIndex, 1, nodePushContext->Context, workerIndex, dispatcher); + BinnedBuildNode(false, subtreeRegionStartIndex, nodePushContext->NodeIndex, subtreeCount, nodePushContext->ParentNodeIndex, 1, nodePushContext->Context, workerIndex, dispatcher); } - static unsafe void BinnedBuildNode( - int subtreeRegionStartIndex, int nodeIndex, int subtreeCount, int parentNodeIndex, int childIndexInParent, Context* context, int workerIndex, IThreadDispatcher dispatcher) - where TLeafCounts : unmanaged, ILeafCountBuffer where TLeaves : unmanaged where TThreading : unmanaged, IBinnedBuilderThreading + static unsafe void BinnedBuildNode( + bool usePongBuffer, int subtreeRegionStartIndex, int nodeIndex, int subtreeCount, int parentNodeIndex, int childIndexInParent, Context* context, int workerIndex, IThreadDispatcher dispatcher) + where TLeaves : unmanaged where TThreading : unmanaged, IBinnedBuilderThreading { var debugStartTime = Stopwatch.GetTimestamp(); ref var debugTimes = ref Times[nodeIndex]; debugTimes.SubtreeCount = subtreeCount; - var indices = context->Indices.Slice(subtreeRegionStartIndex, subtreeCount); - var boundingBoxes = context->BoundingBoxes.Slice(subtreeRegionStartIndex, subtreeCount); - var leafCounts = context->LeafCounts.Slice(subtreeRegionStartIndex, subtreeCount); + var subtrees = (usePongBuffer ? context->SubtreesPong : context->SubtreesPing).Slice(subtreeRegionStartIndex, subtreeCount); + //leaf counts, indices, and bounds are packed together, but it's useful to have a bounds-only representation so that the merging processes don't have to worry about dealing with the fourth lanes. + var boundingBoxes = subtrees.As(); var nodeCount = subtreeCount - 1; var nodes = context->Nodes.Slice(nodeIndex, nodeCount); var metanodes = context->Metanodes.Slice(nodeIndex, nodeCount); if (subtreeCount == 2) { - BuildNode(boundingBoxes[0], boundingBoxes[1], leafCounts[0], leafCounts[1], nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, 1, 1, ref context->Leaves, out _, out _); + BuildNode(boundingBoxes[0], boundingBoxes[1], subtrees[0].LeafCount, subtrees[1].LeafCount, subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, 1, 1, ref context->Leaves, out _, out _); return; } var debugCentroidStartTime = Stopwatch.GetTimestamp(); @@ -854,7 +751,7 @@ static unsafe void BinnedBuildNode( else { centroidBounds = MultithreadedCentroidPrepass( - (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As>(ref context->Threading)), subtreeRegionStartIndex, subtreeCount, workerIndex, dispatcher); + (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As(ref context->Threading)), subtreeRegionStartIndex, subtreeCount, workerIndex, dispatcher); debugTimes.MTPrepass = true; } var debugCentroidEndTime = Stopwatch.GetTimestamp(); @@ -864,8 +761,8 @@ static unsafe void BinnedBuildNode( { //This node is completely degenerate; there is no 'good' ordering of the children. Pick a split in the middle and shrug. //This shouldn't happen unless something is badly wrong with the input; no point in optimizing it. - var degenerateSubtreeCountA = indices.Length / 2; - var degenerateSubtreeCountB = indices.Length - degenerateSubtreeCountA; + var degenerateSubtreeCountA = subtrees.Length / 2; + var degenerateSubtreeCountB = subtrees.Length - degenerateSubtreeCountA; //Still have to compute the child bounding boxes, because the centroid bounds span being zero doesn't imply that the full bounds are zero. BoundingBox4 boundsA, boundsB; boundsA.Min = new Vector4(float.MaxValue); @@ -878,27 +775,27 @@ static unsafe void BinnedBuildNode( ref var bounds = ref boundingBoxes[i]; boundsA.Min = Vector4.Min(bounds.Min, boundsA.Min); boundsA.Max = Vector4.Max(bounds.Max, boundsA.Max); - degenerateLeafCountA += leafCounts[i]; + degenerateLeafCountA += subtrees[i].LeafCount; } - for (int i = degenerateSubtreeCountA; i < indices.Length; ++i) + for (int i = degenerateSubtreeCountA; i < subtrees.Length; ++i) { ref var bounds = ref boundingBoxes[i]; boundsB.Min = Vector4.Min(bounds.Min, boundsB.Min); boundsB.Max = Vector4.Max(bounds.Max, boundsB.Max); - degenerateLeafCountB += leafCounts[i]; + degenerateLeafCountB += subtrees[i].LeafCount; } - BuildNode(boundsA, boundsB, degenerateLeafCountA, degenerateLeafCountB, nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, degenerateSubtreeCountA, degenerateSubtreeCountB, ref context->Leaves, out var aIndex, out var bIndex); + BuildNode(boundsA, boundsB, degenerateLeafCountA, degenerateLeafCountB, subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, degenerateSubtreeCountA, degenerateSubtreeCountB, ref context->Leaves, out var aIndex, out var bIndex); if (degenerateSubtreeCountA > 1) - BinnedBuildNode(subtreeRegionStartIndex, aIndex, degenerateSubtreeCountA, nodeIndex, 0, context, workerIndex, dispatcher); + BinnedBuildNode(usePongBuffer, subtreeRegionStartIndex, aIndex, degenerateSubtreeCountA, nodeIndex, 0, context, workerIndex, dispatcher); if (degenerateSubtreeCountB > 1) - BinnedBuildNode(subtreeRegionStartIndex + degenerateSubtreeCountA, bIndex, degenerateSubtreeCountB, nodeIndex, 1, context, workerIndex, dispatcher); + BinnedBuildNode(usePongBuffer, subtreeRegionStartIndex + degenerateSubtreeCountA, bIndex, degenerateSubtreeCountB, nodeIndex, 1, context, workerIndex, dispatcher); return; } //Note that we don't bother even trying to internally multithread microsweeps. They *should* be small, and should only show up deeper in the recursion process. if (subtreeCount <= context->MicrosweepThreshold) { - MicroSweepForBinnedBuilder(centroidBounds.Min, centroidBounds.Max, indices, leafCounts, ref context->Leaves, boundingBoxes, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, context, workerIndex); + MicroSweepForBinnedBuilder(centroidBounds.Min, centroidBounds.Max, ref context->Leaves, subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, context, workerIndex); return; } @@ -927,13 +824,13 @@ static unsafe void BinnedBuildNode( var debugBinStartTime = Stopwatch.GetTimestamp(); if (typeof(TThreading) == typeof(SingleThreaded) || subtreeCount < SubtreesPerThreadForBinning) { - BinSubtrees(centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, boundingBoxes, leafCounts, binBoundingBoxes, binLeafCounts); + BinSubtrees(centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, subtrees, binBoundingBoxes, binLeafCounts); debugTimes.MTBinning = false; } else { MultithreadedBinSubtrees( - (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As>(ref context->Threading)), + (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As(ref context->Threading)), centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, subtreeRegionStartIndex, subtreeCount, binCount, workerIndex, dispatcher); debugTimes.MTBinning = true; } @@ -942,7 +839,7 @@ static unsafe void BinnedBuildNode( //Identify the split index by examining the SAH of very split option. //Premerge from left to right so we have a sorta-summed area table to cheaply look up all possible child A bounds as we scan. binBoundingBoxesScan[0] = binBoundingBoxes[0]; - int totalLeafCount = typeof(TLeafCounts) == typeof(LeafCountBuffer) ? binLeafCounts[0] : subtreeCount; + int totalLeafCount = binLeafCounts[0]; for (int i = 1; i < binCount; ++i) { var previousIndex = i - 1; @@ -951,8 +848,7 @@ static unsafe void BinnedBuildNode( ref var xPreviousScanBounds = ref binBoundingBoxesScan[previousIndex]; xScanBounds.Min = Vector4.Min(xBounds.Min, xPreviousScanBounds.Min); xScanBounds.Max = Vector4.Max(xBounds.Max, xPreviousScanBounds.Max); - if (typeof(TLeafCounts) == typeof(LeafCountBuffer)) - totalLeafCount += binLeafCounts[i]; + totalLeafCount += binLeafCounts[i]; } var leftBoundsX = binBoundingBoxes[0]; Debug.Assert( @@ -979,8 +875,7 @@ static unsafe void BinnedBuildNode( bestSAH = sahCandidate; bestSplit = splitIndexCandidate; bestBoundingBoxB = accumulatedBoundingBoxB; - if (typeof(TLeafCounts) == typeof(LeafCountBuffer)) - bestLeafCountB = accumulatedLeafCountB; + bestLeafCountB = accumulatedLeafCountB; } ref var xBounds = ref binBoundingBoxes[previousIndex]; accumulatedBoundingBoxB.Min = Vector4.Min(xBounds.Min, accumulatedBoundingBoxB.Min); @@ -993,8 +888,8 @@ static unsafe void BinnedBuildNode( var subtreeCountB = 0; var subtreeCountA = 0; var splitIndex = bestSplit; - var bestboundsA = binBoundingBoxesScan[bestSplit - 1]; - var bestboundsB = bestBoundingBoxB; + var bestBoundsA = binBoundingBoxesScan[bestSplit - 1]; + var bestBoundsB = bestBoundingBoxB; while (subtreeCountA + subtreeCountB < subtreeCount) { ref var box = ref boundingBoxes[subtreeCountA]; @@ -1003,18 +898,10 @@ static unsafe void BinnedBuildNode( { //Belongs to B. Swap it. var targetIndex = subtreeCount - subtreeCountB - 1; - Helpers.Swap(ref indices[targetIndex], ref indices[subtreeCountA]); - if (typeof(TLeafCounts) == typeof(LeafCountBuffer)) - { - var tempTarget = leafCounts[targetIndex]; - var tempACount = leafCounts[subtreeCountA]; - leafCounts[subtreeCountA] = tempTarget; - leafCounts[targetIndex] = tempACount; - } if (Vector256.IsHardwareAccelerated) { - var targetMemory = (byte*)(boundingBoxes.Memory + targetIndex); - var aCountMemory = (byte*)(boundingBoxes.Memory + subtreeCountA); + var targetMemory = (byte*)(subtrees.Memory + targetIndex); + var aCountMemory = (byte*)(subtrees.Memory + subtreeCountA); var targetVector = Vector256.Load(targetMemory); var aCountVector = Vector256.Load(aCountMemory); Vector256.Store(aCountVector, targetMemory); @@ -1022,7 +909,7 @@ static unsafe void BinnedBuildNode( } else { - Helpers.Swap(ref boundingBoxes[targetIndex], ref boundingBoxes[subtreeCountA]); + Helpers.Swap(ref subtrees[targetIndex], ref subtrees[subtreeCountA]); } ++subtreeCountB; //(Note that we still need to examine what we just swapped into the slot! It may belong to B too!) @@ -1034,10 +921,10 @@ static unsafe void BinnedBuildNode( } } var debugHuhEndTime = Stopwatch.GetTimestamp(); - var leafCountB = typeof(TLeafCounts) == typeof(UnitLeafCount) ? subtreeCountB : bestLeafCountB; - var leafCountA = typeof(TLeafCounts) == typeof(UnitLeafCount) ? subtreeCountA : totalLeafCount - leafCountB; + var leafCountB = bestLeafCountB; + var leafCountA = totalLeafCount - leafCountB; Debug.Assert(subtreeCountA + subtreeCountB == subtreeCount); - BuildNode(bestboundsA, bestboundsB, leafCountA, leafCountB, nodes, metanodes, indices, nodeIndex, parentNodeIndex, childIndexInParent, subtreeCountA, subtreeCountB, ref context->Leaves, out var nodeChildIndexA, out var nodeChildIndexB); + BuildNode(bestBoundsA, bestBoundsB, leafCountA, leafCountB, subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, subtreeCountA, subtreeCountB, ref context->Leaves, out var nodeChildIndexA, out var nodeChildIndexB); var debugEndTime = Stopwatch.GetTimestamp(); debugTimes.Total = (debugEndTime - debugStartTime) / (double)Stopwatch.Frequency; debugTimes.Huh = (debugHuhEndTime - debugHuhStartTime) / (double)Stopwatch.Frequency; @@ -1050,20 +937,20 @@ static unsafe void BinnedBuildNode( { //Both of the children are large. Push child B onto the multithreaded execution stack so it can run at the same time as child A (potentially). Debug.Assert(SubtreesPerThreadForNodeJob > 1, "The job threshold for a new node should be large enough that there's no need for a subtreeCountB > 1 test."); - ref var threading = ref Unsafe.As>(ref context->Threading); + ref var threading = ref Unsafe.As(ref context->Threading); //Allocate the parameters to send to the worker on the local stack. Note that we have to preserve the stack for this to work; see the later WaitForCompletion. - NodePushTaskContext nodePushContext; + NodePushTaskContext nodePushContext; nodePushContext.Context = context; nodePushContext.NodeIndex = nodeChildIndexB; nodePushContext.ParentNodeIndex = nodeIndex; //Note that we use the task id to store subtree start and subtree count. Don't have to do that, but no reason not to use it. - var task = new Task(&BinnedBuilderNodeWorker, &nodePushContext, (long)(subtreeRegionStartIndex + subtreeCountA) | ((long)subtreeCountB << 32)); + var task = new Task(&BinnedBuilderNodeWorker, &nodePushContext, (long)(subtreeRegionStartIndex + subtreeCountA) | ((long)subtreeCountB << 32)); nodeBContinuation = threading.TaskStack->AllocateContinuationAndPush(new Span(&task, 1), workerIndex, dispatcher); } if (subtreeCountA > 1) - BinnedBuildNode(subtreeRegionStartIndex, nodeChildIndexA, subtreeCountA, nodeIndex, 0, context, workerIndex, dispatcher); + BinnedBuildNode(usePongBuffer, subtreeRegionStartIndex, nodeChildIndexA, subtreeCountA, nodeIndex, 0, context, workerIndex, dispatcher); if (!shouldPushBOntoMultithreadedQueue && subtreeCountB > 1) - BinnedBuildNode(subtreeRegionStartIndex + subtreeCountA, nodeChildIndexB, subtreeCountB, nodeIndex, 1, context, workerIndex, dispatcher); + BinnedBuildNode(usePongBuffer, subtreeRegionStartIndex + subtreeCountA, nodeChildIndexB, subtreeCountB, nodeIndex, 1, context, workerIndex, dispatcher); if (shouldPushBOntoMultithreadedQueue) { //We want to keep the stack at this level alive until the memory we allocated for the node push completes. @@ -1071,15 +958,33 @@ static unsafe void BinnedBuildNode( //In addition to letting us use the local stack to store some arguments for the other thread, this wait means that all children have completed when this function returns. //That makes knowing when to stop the queue easier. Debug.Assert(nodeBContinuation.Initialized); - Unsafe.As>(ref context->Threading).TaskStack->WaitForCompletion(nodeBContinuation, workerIndex, dispatcher); + Unsafe.As(ref context->Threading).TaskStack->WaitForCompletion(nodeBContinuation, workerIndex, dispatcher); } } - static unsafe void BinnedBuilderInternal(Buffer encodedLeafIndices, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, Buffer leaves, + /// + /// Runs a binned build across the input buffer. + /// + /// Subtrees (either leaves or nodes) to run the builder over. The builder may make in-place modifications to the input buffer; the input buffer should not be assumed to be in a valid state after the builder runs. + /// Buffer holding the nodes created by the build process. + /// Nodes are created in a depth first ordering with respect to the input buffer. + /// Buffer holding the metanodes created by the build process. + /// Metanodes, like nodes, are created in a depth first ordering with respect to the input buffer. + /// Metanodes are in the same order and in the same slots; they simply contain data about nodes that most traversals don't need to know about. + /// Buffer holding the leaf references created by the build process. + /// The indices written by the build process are those defined in the inputs; any that is negative is encoded according to and points into the leaf buffer. + /// Thread dispatcher used to accelerate the build process. + /// Task stack used to accelerate the build process. Can be null; one will be created if not provided. + /// Number of workers to be used in the builder. + /// Buffer pool used to preallocate temporary resources for building. + /// Minimum number of bins the builder should use per node. + /// Maximum number of bins the builder should use per node. + /// Multiplier to apply to the subtree count within a node to decide the bin count. Resulting value will then be clamped by the minimum/maximum bin counts. + /// Threshold at or under which the binned builder resorts to local counting sort sweeps. + static unsafe void BinnedBuilderInternal(Buffer subtrees, Buffer nodes, Buffer metanodes, Buffer leaves, IThreadDispatcher dispatcher, LinkedTaskStack* taskStackPointer, int workerCount, BufferPool pool, int minimumBinCount, int maximumBinCount, float leafToBinMultiplier, int microsweepThreshold) { - var subtreeCount = encodedLeafIndices.Length; - Debug.Assert(boundingBoxes.Length >= subtreeCount, "The bounding boxes provided must cover the range of indices provided."); + var subtreeCount = subtrees.Length; Debug.Assert(nodes.Length >= subtreeCount - 1, "The output nodes must be able to contain the nodes created for the leaves."); if (subtreeCount == 0) return; @@ -1087,14 +992,10 @@ static unsafe void BinnedBuilderInternal(Buffer encodedLeafIndices, Buffer< { //If there's only one leaf, the tree has a special format: the root node has only one child. ref var root = ref nodes[0]; - root.A.Min = boundingBoxes[0].Min; - root.A.Index = encodedLeafIndices[0]; //Node that we assume the indices are already encoded. This function works with subtree refinements as well which can manage either leaves or internals. - root.A.Max = boundingBoxes[0].Max; - root.A.LeafCount = 1; + root.A = subtrees[0]; root.B = default; return; } - boundingBoxes = boundingBoxes.Slice(encodedLeafIndices.Length); nodes = nodes.Slice(subtreeCount - 1); //Don't let the user pick values that will just cause an explosion. @@ -1104,6 +1005,7 @@ static unsafe void BinnedBuilderInternal(Buffer encodedLeafIndices, Buffer< //The microsweep uses the same resources as the bin allocations, so expand to hold whichever is larger. var allocatedBinCount = int.Max(maximumBinCount, microsweepThreshold); + if (dispatcher == null && taskStackPointer == null) { //Use the single threaded path. @@ -1116,75 +1018,77 @@ static unsafe void BinnedBuilderInternal(Buffer encodedLeafIndices, Buffer< threading.BinBoundingBoxes = new Buffer(binBoundsMemory, allocatedBinCount); threading.BinBoundingBoxesScan = new Buffer(binBoundsMemory + allocatedBinCount, allocatedBinCount); threading.BinLeafCounts = new Buffer(binLeafCountsMemory, allocatedBinCount); - var context = new Context, SingleThreaded>( - minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold, encodedLeafIndices, - new UnitLeafCount(), leaves, boundingBoxes.As(), nodes, metanodes, threading); - BinnedBuildNode(0, 0, subtreeCount, -1, -1, &context, 0, null); + var context = new Context, SingleThreaded>( + minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold, + subtrees, default, leaves, nodes, metanodes, threading); + BinnedBuildNode(false, 0, 0, subtreeCount, -1, -1, &context, 0, null); } - else if (dispatcher != null) + else { - //There's a task queue; we should use a multithreaded dispatch. - //While we could allocate on the stack with reasonable safety in the single threaded path, that's not very reasonable for the multithreaded path. - //Each worker thread could be given a node job which executes asynchronously with respect to other node jobs. - //Those node jobs could spawn multithreaded work that other workers assist with. - //Each of those jobs needs its own context for those workers, and the number of jobs is not 1:1 with the workers. - //We'll handle such dispatch-required allocations from worker pools. Here, we just preallocate stuff for the first level across all workers. - pool.Take(allocatedBinCount * workerCount * (sizeof(BoundingBox4) * 2 + sizeof(int)), out var workerBinsAllocation); - - BinnedBuildWorkerContext* workerContextsPointer = stackalloc BinnedBuildWorkerContext[workerCount]; - var leafCounts = new UnitLeafCount(); - var workerContexts = new Buffer>(workerContextsPointer, workerCount); - - int binAllocationStart = 0; - for (int i = 0; i < workerCount; ++i) + //Multithreaded execution of some type. + pool.Take(subtrees.Length, out var subtreesPong); + if (dispatcher != null) { - workerContexts[i] = new BinnedBuildWorkerContext(boundingBoxes.As(), workerBinsAllocation, ref binAllocationStart, allocatedBinCount, leafCounts); - } + //There's a task queue; we should use a multithreaded dispatch. + //While we could allocate on the stack with reasonable safety in the single threaded path, that's not very reasonable for the multithreaded path. + //Each worker thread could be given a node job which executes asynchronously with respect to other node jobs. + //Those node jobs could spawn multithreaded work that other workers assist with. + //Each of those jobs needs its own context for those workers, and the number of jobs is not 1:1 with the workers. + //We'll handle such dispatch-required allocations from worker pools. Here, we just preallocate stuff for the first level across all workers. + pool.Take(allocatedBinCount * workerCount * (sizeof(BoundingBox4) * 2 + sizeof(int)), out var workerBinsAllocation); + + BinnedBuildWorkerContext* workerContextsPointer = stackalloc BinnedBuildWorkerContext[workerCount]; + var workerContexts = new Buffer(workerContextsPointer, workerCount); + + int binAllocationStart = 0; + for (int i = 0; i < workerCount; ++i) + { + workerContexts[i] = new BinnedBuildWorkerContext(subtrees, workerBinsAllocation, ref binAllocationStart, allocatedBinCount); + } - LinkedTaskStack taskStack = default; - bool createdTaskQueueLocally = taskStackPointer == null; - if (taskStackPointer == null) - { - taskStack = new LinkedTaskStack(pool, dispatcher, dispatcher.ThreadCount); - taskStackPointer = &taskStack; + LinkedTaskStack taskStack = default; + bool createdTaskQueueLocally = taskStackPointer == null; + if (taskStackPointer == null) + { + taskStack = new LinkedTaskStack(pool, dispatcher, dispatcher.ThreadCount); + taskStackPointer = &taskStack; + } + var threading = new MultithreadBinnedBuildContext + { + MaximumTaskCountPerSubmission = workerCount * 2, + TaskStack = taskStackPointer, + Workers = workerContexts, + }; + var context = new Context, MultithreadBinnedBuildContext>( + minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold, + subtrees, subtreesPong, leaves, nodes, metanodes, threading); + + taskStackPointer->PushUnsafely(new Task(&BinnedBuilderWorkerEntry>, &context), 0, dispatcher); + dispatcher.DispatchWorkers(&BinnedBuilderWorkerFunction>, unmanagedContext: taskStackPointer); + + if (createdTaskQueueLocally) + taskStack.Dispose(pool, dispatcher); + pool.Return(ref workerBinsAllocation); } - var threading = new MultithreadBinnedBuildContext + else { - MaximumTaskCountPerSubmission = workerCount * 2, - TaskStack = taskStackPointer, - Workers = workerContexts, - }; - var context = new Context, MultithreadBinnedBuildContext>( - minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold, encodedLeafIndices, - leafCounts, leaves, boundingBoxes.As(), nodes, metanodes, threading); - - taskStackPointer->PushUnsafely(new Task(&BinnedBuilderWorkerEntry>, &context), 0, dispatcher); - dispatcher.DispatchWorkers(&BinnedBuilderWorkerFunction>, unmanagedContext: taskStackPointer); - - if (createdTaskQueueLocally) - taskStack.Dispose(pool, dispatcher); - pool.Return(ref workerBinsAllocation); + //TODO: TaskStackPointer-only path? + throw new NotImplementedException("Haven't yet implemented this path, or decided if it should really exist!"); + } + pool.Return(ref subtreesPong); } } - public static unsafe void BinnedBuilder(Buffer encodedLeafIndices, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, Buffer leaves, - int minimumBinCount = 16, int maximumBinCount = 64, float leafToBinMultiplier = 1 / 16f, int microsweepThreshold = 64) - { - BinnedBuilderInternal(encodedLeafIndices, boundingBoxes, nodes, metanodes, leaves, null, null, 0, null, minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold); - } - - unsafe static void BinnedBuilderWorkerEntry(long taskId, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) - where TLeafCounts : unmanaged, ILeafCountBuffer + unsafe static void BinnedBuilderWorkerEntry(long taskId, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) where TLeaves : unmanaged { - var context = (Context>*)untypedContext; - BinnedBuildNode(0, 0, context->Indices.Length, -1, -1, context, workerIndex, dispatcher); + var context = (Context*)untypedContext; + BinnedBuildNode(false, 0, 0, context->SubtreesPing.Length, -1, -1, context, workerIndex, dispatcher); //Once the entry point returns, all workers should stop because it won't return unless both nodes are done. context->Threading.TaskStack->RequestStop(); } - unsafe static void BinnedBuilderWorkerFunction(int workerIndex, IThreadDispatcher dispatcher) - where TLeafCounts : unmanaged, ILeafCountBuffer + unsafe static void BinnedBuilderWorkerFunction(int workerIndex, IThreadDispatcher dispatcher) where TLeaves : unmanaged { var taskQueue = (LinkedTaskStack*)dispatcher.UnmanagedContext; @@ -1196,11 +1100,18 @@ unsafe static void BinnedBuilderWorkerFunction(int workerI } } - public static unsafe void BinnedBuilder(Buffer encodedLeafIndices, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, Buffer leaves, + public static unsafe void BinnedBuilder(Buffer subtrees, Buffer nodes, Buffer metanodes, Buffer leaves, IThreadDispatcher threadDispatcher, BufferPool pool, int minimumBinCount = 16, int maximumBinCount = 64, float leafToBinMultiplier = 1 / 16f, int microsweepThreshold = 64) { - BinnedBuilderInternal(encodedLeafIndices, boundingBoxes, nodes, metanodes, leaves, threadDispatcher, null, threadDispatcher.ThreadCount, pool, minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold); + BinnedBuilderInternal(subtrees, nodes, metanodes, leaves, threadDispatcher, null, threadDispatcher.ThreadCount, pool, minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold); } + + public static unsafe void BinnedBuilder(Buffer subtrees, Buffer nodes, Buffer metanodes, Buffer leaves, + int minimumBinCount = 16, int maximumBinCount = 64, float leafToBinMultiplier = 1 / 16f, int microsweepThreshold = 64) + { + BinnedBuilderInternal(subtrees, nodes, metanodes, leaves, null, null, 0, null, minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold); + } + } } diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index f1f34eeab..8f2a87401 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -47,7 +47,7 @@ struct Option public DemoSet() { //AddOption(); - //AddOption(); + AddOption(); //AddOption(); //AddOption(); AddOption(); diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index b2bb426fd..86fe23e8b 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -179,27 +179,42 @@ public override void Initialize(ContentArchive content, Camera camera) Console.WriteLine($"initial SAH: {mesh.Tree.MeasureCostMetric()}, cache quality: {mesh.Tree.MeasureCacheQuality()}"); Console.WriteLine($"initial bounds: A ({mesh.Tree.Nodes[0].A.Min}, {mesh.Tree.Nodes[0].B.Max}), B ({mesh.Tree.Nodes[0].B.Min}, {mesh.Tree.Nodes[0].B.Max})"); - BufferPool.Take(mesh.Triangles.Length, out var leafBounds); - BufferPool.Take(mesh.Triangles.Length, out var leafIndices); + BufferPool.Take(mesh.Triangles.Length, out var subtrees); Action setup = () => { for (int i = 0; i < mesh.Triangles.Length; ++i) { ref var t = ref mesh.Triangles[i]; - ref var bounds = ref leafBounds[i]; - bounds.Min = Vector3.Min(t.A, Vector3.Min(t.B, t.C)); - bounds.Max = Vector3.Max(t.A, Vector3.Max(t.B, t.C)); - leafIndices[i] = Tree.Encode(i); + ref var subtree = ref subtrees[i]; + subtree.Min = Vector3.Min(t.A, Vector3.Min(t.B, t.C)); + subtree.Max = Vector3.Max(t.A, Vector3.Max(t.B, t.C)); + subtree.Index = Tree.Encode(i); + subtree.LeafCount = 1; } }; BinnedTest(setup, () => { - Tree.BinnedBuilder(leafIndices, leafBounds, mesh.Tree.Nodes, mesh.Tree.Metanodes, mesh.Tree.Leaves, ThreadDispatcher, BufferPool); + Tree.BinnedBuilder(subtrees, mesh.Tree.Nodes, mesh.Tree.Metanodes, mesh.Tree.Leaves, ThreadDispatcher, BufferPool); }, "Revamp Single Axis MT", ref mesh.Tree); - //BinnedTest(setup, () => + + //BufferPool.Take(mesh.Triangles.Length, out var leafBounds); + //BufferPool.Take(mesh.Triangles.Length, out var leafIndices); + + //Action yeOldeSetup = () => + //{ + // for (int i = 0; i < mesh.Triangles.Length; ++i) + // { + // ref var t = ref mesh.Triangles[i]; + // ref var bounds = ref leafBounds[i]; + // bounds.Min = Vector3.Min(t.A, Vector3.Min(t.B, t.C)); + // bounds.Max = Vector3.Max(t.A, Vector3.Max(t.B, t.C)); + // leafIndices[i] = Tree.Encode(i); + // } + //}; + //BinnedTest(yeOldeSetup, () => //{ // Tree.BinnedBuilder(leafIndices, leafBounds, mesh.Tree.Nodes, mesh.Tree.Metanodes, mesh.Tree.Leaves); //}, "Revamp Single Axis ST", ref mesh.Tree); From 1acfae0ad189ce7b7631eba3386cca33586e17a2 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 8 Mar 2023 22:43:25 -0600 Subject: [PATCH 692/947] Fixed centroid bounds calculation for nested microsweeps. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 26 +++++++++++++++---------- 1 file changed, 16 insertions(+), 10 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 6f094b104..0c6c809ca 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -286,26 +286,32 @@ static unsafe void MicroSweepForBinnedBuilder( if (subtreeCountA > 1) { var aBounds = boundingBoxes.Slice(subtreeCountA); - BoundingBox4 centroidBoundsA = aBounds[0]; + var initialCentroid = aBounds.Memory->Min + aBounds.Memory->Max; + BoundingBox4 centroidBoundsA; + centroidBoundsA.Min = initialCentroid; + centroidBoundsA.Max = initialCentroid; for (int i = 1; i < subtreeCountA; ++i) { ref var bounds = ref aBounds[i]; - //TODO: THIS SHOULD BE CENTROID, NOT FULL MERGE - centroidBoundsA.Min = Vector4.Min(centroidBoundsA.Min, bounds.Min); - centroidBoundsA.Max = Vector4.Max(centroidBoundsA.Max, bounds.Max); + var centroid = bounds.Min + bounds.Max; + centroidBoundsA.Min = Vector4.Min(centroidBoundsA.Min, centroid); + centroidBoundsA.Max = Vector4.Max(centroidBoundsA.Max, centroid); } MicroSweepForBinnedBuilder(centroidBoundsA.Min, centroidBoundsA.Max, ref leaves, subtrees.Slice(subtreeCountA), nodes.Slice(1, subtreeCountA - 1), metanodes.Slice(1, subtreeCountA - 1), aIndex, nodeIndex, 0, context, workerIndex); } if (subtreeCountB > 1) { - var bBounds = boundingBoxes.Slice(subtreeCountA, subtreeCountB); - BoundingBox4 centroidBoundsB = bBounds[0]; - for (int i = 0; i < subtreeCountB; ++i) + var bBounds = boundingBoxes.Slice(subtreeCountA, subtreeCountB); + var initialCentroid = bBounds.Memory->Min + bBounds.Memory->Max; + BoundingBox4 centroidBoundsB; + centroidBoundsB.Min = initialCentroid; + centroidBoundsB.Max = initialCentroid; + for (int i = 1; i < subtreeCountB; ++i) { ref var bounds = ref bBounds[i]; - //TODO: THIS SHOULD BE CENTROID, NOT FULL MERGE - centroidBoundsB.Min = Vector4.Min(centroidBoundsB.Min, bounds.Min); - centroidBoundsB.Max = Vector4.Max(centroidBoundsB.Max, bounds.Max); + var centroid = bounds.Min + bounds.Max; + centroidBoundsB.Min = Vector4.Min(centroidBoundsB.Min, centroid); + centroidBoundsB.Max = Vector4.Max(centroidBoundsB.Max, centroid); } MicroSweepForBinnedBuilder(centroidBoundsB.Min, centroidBoundsB.Max, ref leaves, subtrees.Slice(subtreeCountA, subtreeCountB), nodes.Slice(subtreeCountA, subtreeCountB - 1), metanodes.Slice(subtreeCountA, subtreeCountB - 1), bIndex, nodeIndex, 1, context, workerIndex); } From 971ad94bc45cebc5688f0abf2b0094de72aa5edd Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 11 Mar 2023 18:50:46 -0600 Subject: [PATCH 693/947] Pong buffers work. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 156 ++++++++++++++++-------- Demos/Demos.csproj | 2 +- 2 files changed, 105 insertions(+), 53 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 0c6c809ca..0f0c30da6 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -301,7 +301,7 @@ static unsafe void MicroSweepForBinnedBuilder( } if (subtreeCountB > 1) { - var bBounds = boundingBoxes.Slice(subtreeCountA, subtreeCountB); + var bBounds = boundingBoxes.Slice(subtreeCountA, subtreeCountB); var initialCentroid = bBounds.Memory->Min + bBounds.Memory->Max; BoundingBox4 centroidBoundsB; centroidBoundsB.Min = initialCentroid; @@ -379,17 +379,11 @@ unsafe struct BinnedBuildWorkerContext /// public Buffer BinLeafCounts; - /// - /// Cached view of all subtrees the builder is working on to simplify accesses. - /// - public Buffer Subtrees; - - public BinnedBuildWorkerContext(Buffer subtrees, Buffer binAllocationBuffer, ref int binStart, int binCapacity) + public BinnedBuildWorkerContext(Buffer binAllocationBuffer, ref int binStart, int binCapacity) { BinBoundingBoxes = Suballocate(binAllocationBuffer, ref binStart, binCapacity); BinBoundingBoxesScan = Suballocate(binAllocationBuffer, ref binStart, binCapacity); BinLeafCounts = Suballocate(binAllocationBuffer, ref binStart, binCapacity); - Subtrees = subtrees; } } unsafe struct MultithreadBinnedBuildContext : IBinnedBuilderThreading @@ -509,11 +503,11 @@ unsafe static void CentroidPrepassWorker(long taskId, void* untypedContext, int } } - unsafe static BoundingBox4 MultithreadedCentroidPrepass(MultithreadBinnedBuildContext* context, int subtreeStartIndex, int subtreeCount, int workerIndex, IThreadDispatcher dispatcher) + unsafe static BoundingBox4 MultithreadedCentroidPrepass(MultithreadBinnedBuildContext* context, Buffer subtrees, int workerIndex, IThreadDispatcher dispatcher) { ref var worker = ref context->Workers[workerIndex]; var workerPool = dispatcher.WorkerPools[workerIndex]; - var taskContext = new CentroidPrepassTaskContext(workerPool, new SharedTaskData(context->Workers.Length, subtreeStartIndex, subtreeCount, SubtreesPerThreadForCentroidPrepass, context->MaximumTaskCountPerSubmission), worker.Subtrees); + var taskContext = new CentroidPrepassTaskContext(workerPool, new SharedTaskData(context->Workers.Length, 0, subtrees.Length, SubtreesPerThreadForCentroidPrepass, context->MaximumTaskCountPerSubmission), subtrees); Debug.Assert(taskContext.TaskData.TaskCount > 1, "This codepath shouldn't be used if there's only one task!"); Debug.Assert(taskContext.TaskData.WorkerCount > 1 && taskContext.TaskData.WorkerCount < 100); var taskCount = taskContext.TaskData.TaskCount; @@ -653,14 +647,14 @@ unsafe static void BinSubtreesWorker(long taskId, void* untypedContext, int work unsafe static void MultithreadedBinSubtrees(MultithreadBinnedBuildContext* context, Vector4 centroidBoundsMin, bool useX, bool useY, Vector128 permuteMask, int axisIndex, Vector4 offsetToBinIndex, Vector4 maximumBinIndex, - int subtreeStartIndex, int subtreeCount, int binCount, int workerIndex, IThreadDispatcher dispatcher) + Buffer subtrees, int binCount, int workerIndex, IThreadDispatcher dispatcher) { ref var worker = ref context->Workers[workerIndex]; var workerPool = dispatcher.WorkerPools[workerIndex]; var taskContext = new BinSubtreesTaskContext( workerPool, - new SharedTaskData(context->Workers.Length, subtreeStartIndex, subtreeCount, SubtreesPerThreadForBinning, context->MaximumTaskCountPerSubmission), - worker.Subtrees, binCount, useX, useY, permuteMask, axisIndex, centroidBoundsMin, offsetToBinIndex, maximumBinIndex); + new SharedTaskData(context->Workers.Length, 0, subtrees.Length, SubtreesPerThreadForBinning, context->MaximumTaskCountPerSubmission), + subtrees, binCount, useX, useY, permuteMask, axisIndex, centroidBoundsMin, offsetToBinIndex, maximumBinIndex); //Don't bother initializing more slots than we have tasks. Note that this requires special handling on the task level; //if we have less tasks than workers, then the task needs to distinguish that fact. @@ -717,16 +711,17 @@ unsafe struct NodePushTaskContext public Context* Context; public int NodeIndex; public int ParentNodeIndex; - //Subtree region start index and subtree count are both encoded into the task id. + //Subtree region start index, subtree count, and usePongBuffer status are all encoded into the task id. } unsafe static void BinnedBuilderNodeWorker(long taskId, void* context, int workerIndex, IThreadDispatcher dispatcher) where TLeaves : unmanaged where TThreading : unmanaged, IBinnedBuilderThreading { var subtreeRegionStartIndex = (int)taskId; - var subtreeCount = (int)(taskId >> 32); + var subtreeCount = (int)((taskId >> 32) & 0x7FFF_FFFF); + var usePongBuffer = (ulong)taskId >= (1UL << 63); var nodePushContext = (NodePushTaskContext*)context; //Note that child index is always 1 because we only ever push child B. - BinnedBuildNode(false, subtreeRegionStartIndex, nodePushContext->NodeIndex, subtreeCount, nodePushContext->ParentNodeIndex, 1, nodePushContext->Context, workerIndex, dispatcher); + BinnedBuildNode(usePongBuffer, subtreeRegionStartIndex, nodePushContext->NodeIndex, subtreeCount, nodePushContext->ParentNodeIndex, 1, nodePushContext->Context, workerIndex, dispatcher); } static unsafe void BinnedBuildNode( @@ -757,7 +752,7 @@ static unsafe void BinnedBuildNode( else { centroidBounds = MultithreadedCentroidPrepass( - (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As(ref context->Threading)), subtreeRegionStartIndex, subtreeCount, workerIndex, dispatcher); + (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As(ref context->Threading)), subtrees, workerIndex, dispatcher); debugTimes.MTPrepass = true; } var debugCentroidEndTime = Stopwatch.GetTimestamp(); @@ -837,7 +832,7 @@ static unsafe void BinnedBuildNode( { MultithreadedBinSubtrees( (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As(ref context->Threading)), - centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, subtreeRegionStartIndex, subtreeCount, binCount, workerIndex, dispatcher); + centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, subtrees, binCount, workerIndex, dispatcher); debugTimes.MTBinning = true; } var debugBinEndTime = Stopwatch.GetTimestamp(); @@ -890,42 +885,62 @@ static unsafe void BinnedBuildNode( } var debugHuhStartTime = Stopwatch.GetTimestamp(); - //Split the indices/bounds into two halves for the children to operate on. var subtreeCountB = 0; var subtreeCountA = 0; var splitIndex = bestSplit; var bestBoundsA = binBoundingBoxesScan[bestSplit - 1]; var bestBoundsB = bestBoundingBoxB; - while (subtreeCountA + subtreeCountB < subtreeCount) + + //Split the indices/bounds into two halves for the children to operate on. + if (context->SubtreesPong.Allocated) + { + //If the current buffer is pong, then write to ping, and vice versa. + var subtreesNext = (usePongBuffer ? context->SubtreesPing : context->SubtreesPong).Slice(subtreeRegionStartIndex, subtreeCount); + for (int i = 0; i < subtreeCount; ++i) + { + ref var box = ref boundingBoxes[i]; + var binIndex = ComputeBinIndex(centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, box); + var targetIndex = binIndex >= splitIndex ? subtreeCount - ++subtreeCountB : subtreeCountA++; + subtreesNext[targetIndex] = subtrees[i]; + } + subtrees = subtreesNext; + usePongBuffer = !usePongBuffer; + } + else { - ref var box = ref boundingBoxes[subtreeCountA]; - var binIndex = ComputeBinIndex(centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, box); - if (binIndex >= splitIndex) + //There is no pong buffer allocated. We allow this for lower memory allocation, but the implementation is strictly sequential and slower. + while (subtreeCountA + subtreeCountB < subtreeCount) { - //Belongs to B. Swap it. - var targetIndex = subtreeCount - subtreeCountB - 1; - if (Vector256.IsHardwareAccelerated) + ref var box = ref boundingBoxes[subtreeCountA]; + var binIndex = ComputeBinIndex(centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, box); + if (binIndex >= splitIndex) { - var targetMemory = (byte*)(subtrees.Memory + targetIndex); - var aCountMemory = (byte*)(subtrees.Memory + subtreeCountA); - var targetVector = Vector256.Load(targetMemory); - var aCountVector = Vector256.Load(aCountMemory); - Vector256.Store(aCountVector, targetMemory); - Vector256.Store(targetVector, aCountMemory); + //Belongs to B. Swap it. + var targetIndex = subtreeCount - subtreeCountB - 1; + if (Vector256.IsHardwareAccelerated) + { + var targetMemory = (byte*)(subtrees.Memory + targetIndex); + var aCountMemory = (byte*)(subtrees.Memory + subtreeCountA); + var targetVector = Vector256.Load(targetMemory); + var aCountVector = Vector256.Load(aCountMemory); + Vector256.Store(aCountVector, targetMemory); + Vector256.Store(targetVector, aCountMemory); + } + else + { + Helpers.Swap(ref subtrees[targetIndex], ref subtrees[subtreeCountA]); + } + ++subtreeCountB; + //(Note that we still need to examine what we just swapped into the slot! It may belong to B too!) } else { - Helpers.Swap(ref subtrees[targetIndex], ref subtrees[subtreeCountA]); + //Belongs to A, no movement necessary. + ++subtreeCountA; } - ++subtreeCountB; - //(Note that we still need to examine what we just swapped into the slot! It may belong to B too!) - } - else - { - //Belongs to A, no movement necessary. - ++subtreeCountA; } } + var debugHuhEndTime = Stopwatch.GetTimestamp(); var leafCountB = bestLeafCountB; var leafCountA = totalLeafCount - leafCountB; @@ -949,8 +964,9 @@ static unsafe void BinnedBuildNode( nodePushContext.Context = context; nodePushContext.NodeIndex = nodeChildIndexB; nodePushContext.ParentNodeIndex = nodeIndex; - //Note that we use the task id to store subtree start and subtree count. Don't have to do that, but no reason not to use it. - var task = new Task(&BinnedBuilderNodeWorker, &nodePushContext, (long)(subtreeRegionStartIndex + subtreeCountA) | ((long)subtreeCountB << 32)); + //Note that we use the task id to store subtree start, subtree count, and the pong buffer flag. Don't have to do that, but no reason not to use it. + Debug.Assert((uint)subtreeCountB < (1u << 31), "The task id encodes start, count, and a pong flag, so we don't have room for a full 32 bits of count."); + var task = new Task(&BinnedBuilderNodeWorker, &nodePushContext, (long)(subtreeRegionStartIndex + subtreeCountA) | ((long)subtreeCountB << 32) | (usePongBuffer ? 1L << 63 : 0)); nodeBContinuation = threading.TaskStack->AllocateContinuationAndPush(new Span(&task, 1), workerIndex, dispatcher); } if (subtreeCountA > 1) @@ -972,6 +988,7 @@ static unsafe void BinnedBuildNode( /// Runs a binned build across the input buffer. /// /// Subtrees (either leaves or nodes) to run the builder over. The builder may make in-place modifications to the input buffer; the input buffer should not be assumed to be in a valid state after the builder runs. + /// A parallel buffer to subtrees which is used as a scratch buffer during execution. If a default initialized buffer is provided, a slower sequential in-place fallback will be used. /// Buffer holding the nodes created by the build process. /// Nodes are created in a depth first ordering with respect to the input buffer. /// Buffer holding the metanodes created by the build process. @@ -987,7 +1004,7 @@ static unsafe void BinnedBuildNode( /// Maximum number of bins the builder should use per node. /// Multiplier to apply to the subtree count within a node to decide the bin count. Resulting value will then be clamped by the minimum/maximum bin counts. /// Threshold at or under which the binned builder resorts to local counting sort sweeps. - static unsafe void BinnedBuilderInternal(Buffer subtrees, Buffer nodes, Buffer metanodes, Buffer leaves, + static unsafe void BinnedBuilderInternal(Buffer subtrees, Buffer subtreesPong, Buffer nodes, Buffer metanodes, Buffer leaves, IThreadDispatcher dispatcher, LinkedTaskStack* taskStackPointer, int workerCount, BufferPool pool, int minimumBinCount, int maximumBinCount, float leafToBinMultiplier, int microsweepThreshold) { var subtreeCount = subtrees.Length; @@ -1032,7 +1049,6 @@ static unsafe void BinnedBuilderInternal(Buffer subtrees, Buffer(subtrees.Length, out var subtreesPong); if (dispatcher != null) { //There's a task queue; we should use a multithreaded dispatch. @@ -1049,7 +1065,7 @@ static unsafe void BinnedBuilderInternal(Buffer subtrees, Buffer subtrees, Buffer(int workerIndex, IThread } public static unsafe void BinnedBuilder(Buffer subtrees, Buffer nodes, Buffer metanodes, Buffer leaves, - IThreadDispatcher threadDispatcher, BufferPool pool, - int minimumBinCount = 16, int maximumBinCount = 64, float leafToBinMultiplier = 1 / 16f, int microsweepThreshold = 64) + IThreadDispatcher threadDispatcher, BufferPool pool, int minimumBinCount = 16, int maximumBinCount = 64, float leafToBinMultiplier = 1 / 16f, int microsweepThreshold = 64) { - BinnedBuilderInternal(subtrees, nodes, metanodes, leaves, threadDispatcher, null, threadDispatcher.ThreadCount, pool, minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold); + pool.Take(subtrees.Length, out Buffer subtreesPong); + BinnedBuilderInternal(subtrees, subtreesPong, nodes, metanodes, leaves, threadDispatcher, null, threadDispatcher.ThreadCount, pool, minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold); + pool.Return(ref subtreesPong); } + /// + /// Runs a binned build across the subtrees buffer. + /// + /// Subtrees (either leaves or nodes) to run the builder over. The builder may make in-place modifications to the input buffer; the input buffer should not be assumed to be in a valid state after the builder runs. + /// Buffer holding the nodes created by the build process. + /// Nodes are created in a depth first ordering with respect to the input buffer. + /// Buffer holding the metanodes created by the build process. + /// Metanodes, like nodes, are created in a depth first ordering with respect to the input buffer. + /// Metanodes are in the same order and in the same slots; they simply contain data about nodes that most traversals don't need to know about. + /// Buffer holding the leaf references created by the build process. + /// The indices written by the build process are those defined in the inputs; any that is negative is encoded according to and points into the leaf buffer. + /// Buffer pool used to preallocate a pingpong buffer if the number of subtrees exceeds maximumSubtreeStackAllocationCount. If null, stack allocation or a slower in-place partitioning will be used. + /// Maximum number of subtrees to try putting on the stack for the binned builder's pong buffers. + /// Subtree counts larger than this threshold will either resort to a buffer pool allocation (if available) or slower in-place partition operations. + /// Minimum number of bins the builder should use per node. + /// Maximum number of bins the builder should use per node. + /// Multiplier to apply to the subtree count within a node to decide the bin count. Resulting value will then be clamped by the minimum/maximum bin counts. + /// Threshold at or under which the binned builder resorts to local counting sort sweeps. public static unsafe void BinnedBuilder(Buffer subtrees, Buffer nodes, Buffer metanodes, Buffer leaves, - int minimumBinCount = 16, int maximumBinCount = 64, float leafToBinMultiplier = 1 / 16f, int microsweepThreshold = 64) + BufferPool pool = null, int maximumSubtreeStackAllocationCount = 4096, int minimumBinCount = 16, int maximumBinCount = 64, float leafToBinMultiplier = 1 / 16f, int microsweepThreshold = 64) { - BinnedBuilderInternal(subtrees, nodes, metanodes, leaves, null, null, 0, null, minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold); - } + Buffer subtreesPong; + bool requiresReturn = false; + if (subtrees.Length <= maximumSubtreeStackAllocationCount) + { + var subtreesPongMemory = stackalloc NodeChild[subtrees.Length]; + subtreesPong = new Buffer(subtreesPongMemory, subtrees.Length); + } + else if (pool != null) + { + pool.Take(subtrees.Length, out subtreesPong); + requiresReturn = true; + } + else + { + subtreesPong = default; + } + BinnedBuilderInternal(subtrees, subtreesPong, nodes, metanodes, leaves, null, null, 0, null, minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold); + if (requiresReturn) + pool.Return(ref subtreesPong); + } } } diff --git a/Demos/Demos.csproj b/Demos/Demos.csproj index 9bb89d121..032d20156 100644 --- a/Demos/Demos.csproj +++ b/Demos/Demos.csproj @@ -5,7 +5,7 @@ True Debug;Release latest - false + From e228f62cb77bf9a536eb9017b26c51f840663381 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 12 Mar 2023 19:13:08 -0500 Subject: [PATCH 694/947] MT Partitioning implemented. Some speedup. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 154 ++++++++++++++++++++++-- 1 file changed, 142 insertions(+), 12 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 0f0c30da6..f036574c5 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -1,4 +1,5 @@ using BepuPhysics.Collidables; +using BepuPhysics.CollisionDetection; using BepuPhysics.Constraints; using BepuPhysics.Constraints.Contact; using BepuUtilities; @@ -135,9 +136,10 @@ public struct NodeTimes public double Total; public double CentroidPrepass; public double Binning; - public double Huh; + public double Partition; public bool MTPrepass; public bool MTBinning; + public bool MTPartition; public int SubtreeCount; } @@ -406,8 +408,9 @@ public void GetBins(int workerIndex, out Buffer binBoundingBoxes, } //These should be powers of 2 for maskhack reasons. - const int SubtreesPerThreadForCentroidPrepass = 262144; - const int SubtreesPerThreadForBinning = 262144; + const int SubtreesPerThreadForCentroidPrepass = 65536; + const int SubtreesPerThreadForBinning = 65536; + const int SubtreesPerThreadForPartitioning = 65536; const int SubtreesPerThreadForNodeJob = 1024; [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -470,7 +473,7 @@ unsafe struct CentroidPrepassTaskContext /// public Buffer PrepassWorkers; /// - /// Buffer containing all subtrees being built. + /// Buffer containing all subtrees in the node. /// public Buffer Subtrees; @@ -560,7 +563,7 @@ unsafe struct BinSubtreesTaskContext public Buffer WorkerHelpedWithBinning; /// - /// Buffer containing all subtrees under construction. + /// Buffer containing all subtrees in this node. /// public Buffer Subtrees; @@ -705,6 +708,123 @@ unsafe static void MultithreadedBinSubtrees(MultithreadBinnedBuildContext* conte taskContext.Dispose(workerPool); } + [StructLayout(LayoutKind.Explicit, Size = 264)] + struct PartitionCounters + { + //Padding to avoid shared cache lines. + [FieldOffset(128)] + public int SubtreeCountA; + [FieldOffset(134)] + public int SubtreeCountB; + } + unsafe struct PartitionTaskContext + { + public SharedTaskData TaskData; + + /// + /// Buffer containing all subtrees in this node. + /// + public Buffer Subtrees; + /// + /// Buffer that will contain the partitioned subtrees pulled from . + /// + public Buffer SubtreesNext; + + public int BinSplitIndex; + + public int BinCount; + public bool UseX, UseY; + public Vector128 PermuteMask; + public int AxisIndex; + public Vector4 CentroidBoundsMin; + public Vector4 OffsetToBinIndex; + public Vector4 MaximumBinIndex; + + public PartitionCounters Counters; + + public PartitionTaskContext(SharedTaskData taskData, Buffer subtrees, Buffer subtreesNext, int binSplitIndex, + int binCount, bool useX, bool useY, Vector128 permuteMask, int axisIndex, + Vector4 centroidBoundsMin, Vector4 offsetToBinIndex, Vector4 maximumBinIndex) + { + TaskData = taskData; + Subtrees = subtrees; + SubtreesNext = subtreesNext; + BinSplitIndex = binSplitIndex; + BinCount = binCount; + UseX = useX; + UseY = useY; + PermuteMask = permuteMask; + AxisIndex = axisIndex; + CentroidBoundsMin = centroidBoundsMin; + OffsetToBinIndex = offsetToBinIndex; + MaximumBinIndex = maximumBinIndex; + + Counters = new PartitionCounters(); + } + } + + unsafe static void PartitionSubtreesWorker(long taskId, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) + { + ref var context = ref *(PartitionTaskContext*)untypedContext; + var centroidBoundsMin = context.CentroidBoundsMin; + var useX = context.UseX; + var useY = context.UseY; + var permuteMask = context.PermuteMask; + var axisIndex = context.AxisIndex; + var offsetToBinIndex = context.OffsetToBinIndex; + var maximumBinIndex = context.MaximumBinIndex; + context.TaskData.GetSlotInterval(taskId, out var start, out var count); + //We don't really want to trigger interlocked operation for *every single subtree*, but we also don't want to allocate a bunch of memory. + //Compromise! Stackalloc enough memory to cover sub-batches of the worker's subtrees, and do interlocked operations at the end of each batch. + //Note that the main limit to the batch size is the amount of memory in L1 cache: the subtrees are 32 bytes per. + const int batchSize = 256; + bool* slotBelongsToA = stackalloc bool[batchSize]; + var batchCount = (count + batchSize - 1) / batchSize; + var boundingBoxes = context.Subtrees.As(); + var subtrees = context.Subtrees; + var subtreesNext = context.SubtreesNext; + for (int batchIndex = 0; batchIndex < batchCount; ++batchIndex) + { + var localCountA = 0; + var localCountB = 0; + var batchStart = start + batchIndex * batchSize; + var countInBatch = int.Min(start + count - batchStart, batchSize); + for (int indexInBatch = 0; indexInBatch < countInBatch; ++indexInBatch) + { + var subtreeIndex = indexInBatch + batchStart; + var binIndex = ComputeBinIndex(centroidBoundsMin, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, boundingBoxes[subtreeIndex]); + var belongsToA = binIndex < context.BinSplitIndex; + slotBelongsToA[indexInBatch] = belongsToA; + if (belongsToA) ++localCountA; else ++localCountB; + } + var startIndexA = Interlocked.Add(ref context.Counters.SubtreeCountA, localCountA) - localCountA; + var startIndexB = subtrees.Length - Interlocked.Add(ref context.Counters.SubtreeCountB, localCountB); + + int recountA = 0; + int recountB = 0; + for (int indexInBatch = 0; indexInBatch < countInBatch; ++indexInBatch) + { + var targetIndex = slotBelongsToA[indexInBatch] ? startIndexA + recountA++ : startIndexB + recountB++; + subtreesNext[targetIndex] = subtrees[batchStart + indexInBatch]; + } + + } + } + + unsafe static (int subtreeCountA, int subtreeCountB) MultithreadedPartition(MultithreadBinnedBuildContext* context, + Vector4 centroidBoundsMin, bool useX, bool useY, Vector128 permuteMask, int axisIndex, Vector4 offsetToBinIndex, Vector4 maximumBinIndex, + Buffer subtrees, Buffer subtreesNext, int binSplitIndex, int binCount, int workerIndex, IThreadDispatcher dispatcher) + { + ref var worker = ref context->Workers[workerIndex]; + var workerPool = dispatcher.WorkerPools[workerIndex]; + var taskContext = new PartitionTaskContext( + new SharedTaskData(context->Workers.Length, 0, subtrees.Length, SubtreesPerThreadForBinning, context->MaximumTaskCountPerSubmission), + subtrees, subtreesNext, binSplitIndex, binCount, useX, useY, permuteMask, axisIndex, centroidBoundsMin, offsetToBinIndex, maximumBinIndex); + + context->TaskStack->For(&PartitionSubtreesWorker, &taskContext, 0, taskContext.TaskData.TaskCount, workerIndex, dispatcher); + return (taskContext.Counters.SubtreeCountA, taskContext.Counters.SubtreeCountB); + } + unsafe struct NodePushTaskContext where TLeaves : unmanaged where TThreading : unmanaged, IBinnedBuilderThreading { @@ -896,12 +1016,22 @@ static unsafe void BinnedBuildNode( { //If the current buffer is pong, then write to ping, and vice versa. var subtreesNext = (usePongBuffer ? context->SubtreesPing : context->SubtreesPong).Slice(subtreeRegionStartIndex, subtreeCount); - for (int i = 0; i < subtreeCount; ++i) + if (typeof(TThreading) == typeof(SingleThreaded) || subtreeCount < SubtreesPerThreadForPartitioning) { - ref var box = ref boundingBoxes[i]; - var binIndex = ComputeBinIndex(centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, box); - var targetIndex = binIndex >= splitIndex ? subtreeCount - ++subtreeCountB : subtreeCountA++; - subtreesNext[targetIndex] = subtrees[i]; + for (int i = 0; i < subtreeCount; ++i) + { + var binIndex = ComputeBinIndex(centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, boundingBoxes[i]); + var targetIndex = binIndex >= splitIndex ? subtreeCount - ++subtreeCountB : subtreeCountA++; + subtreesNext[targetIndex] = subtrees[i]; + } + debugTimes.MTPartition = false; + } + else + { + (subtreeCountA, subtreeCountB) = MultithreadedPartition( + (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As(ref context->Threading)), + centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, subtrees, subtreesNext, splitIndex, binCount, workerIndex, dispatcher); + debugTimes.MTPartition = true; } subtrees = subtreesNext; usePongBuffer = !usePongBuffer; @@ -948,7 +1078,7 @@ static unsafe void BinnedBuildNode( BuildNode(bestBoundsA, bestBoundsB, leafCountA, leafCountB, subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, subtreeCountA, subtreeCountB, ref context->Leaves, out var nodeChildIndexA, out var nodeChildIndexB); var debugEndTime = Stopwatch.GetTimestamp(); debugTimes.Total = (debugEndTime - debugStartTime) / (double)Stopwatch.Frequency; - debugTimes.Huh = (debugHuhEndTime - debugHuhStartTime) / (double)Stopwatch.Frequency; + debugTimes.Partition = (debugHuhEndTime - debugHuhStartTime) / (double)Stopwatch.Frequency; debugTimes.CentroidPrepass = (debugCentroidEndTime - debugCentroidStartTime) / (double)Stopwatch.Frequency; debugTimes.Binning = (debugBinEndTime - debugBinStartTime) / (double)Stopwatch.Frequency; @@ -1159,7 +1289,7 @@ public static unsafe void BinnedBuilder(Buffer subtrees, Buffer } else if (pool != null) { - pool.Take(subtrees.Length, out subtreesPong); + pool.Take(subtrees.Length, out subtreesPong); requiresReturn = true; } else From ae3a1e855149eda6d9cfecfab64f811010b9ce46 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 12 Mar 2023 19:43:44 -0500 Subject: [PATCH 695/947] Some bad comments. --- BepuUtilities/TaskStack.cs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/BepuUtilities/TaskStack.cs b/BepuUtilities/TaskStack.cs index 7e77c6af2..26a5322bb 100644 --- a/BepuUtilities/TaskStack.cs +++ b/BepuUtilities/TaskStack.cs @@ -12,7 +12,7 @@ namespace BepuUtilities.TestStack; /// -/// Description of a task to be submitted to a . +/// Description of a task to be submitted to a . /// public unsafe struct Task { @@ -179,7 +179,7 @@ public bool Exists } /// - /// Gets whether this handle ever represented an allocated handle. This does not guarantee that the continuation's associated tasks are active in the that it was allocated from. + /// Gets whether this handle ever represented an allocated handle. This does not guarantee that the continuation's associated tasks are active in the that it was allocated from. /// public bool Initialized => encodedVersion >= 1u << 31; From 2772de869593112e1b8d1fdea6bff7d26d26bb5b Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 13 Mar 2023 20:31:56 -0500 Subject: [PATCH 696/947] Fixed partition scheduling oopsy. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index f036574c5..b2b80b0e4 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -818,7 +818,7 @@ unsafe static (int subtreeCountA, int subtreeCountB) MultithreadedPartition(Mult ref var worker = ref context->Workers[workerIndex]; var workerPool = dispatcher.WorkerPools[workerIndex]; var taskContext = new PartitionTaskContext( - new SharedTaskData(context->Workers.Length, 0, subtrees.Length, SubtreesPerThreadForBinning, context->MaximumTaskCountPerSubmission), + new SharedTaskData(context->Workers.Length, 0, subtrees.Length, SubtreesPerThreadForPartitioning, context->MaximumTaskCountPerSubmission), subtrees, subtreesNext, binSplitIndex, binCount, useX, useY, permuteMask, axisIndex, centroidBoundsMin, offsetToBinIndex, maximumBinIndex); context->TaskStack->For(&PartitionSubtreesWorker, &taskContext, 0, taskContext.TaskData.TaskCount, workerIndex, dispatcher); @@ -857,6 +857,7 @@ static unsafe void BinnedBuildNode( var nodeCount = subtreeCount - 1; var nodes = context->Nodes.Slice(nodeIndex, nodeCount); var metanodes = context->Metanodes.Slice(nodeIndex, nodeCount); + var forceInternalSingleThreaded = typeof(TThreading) == typeof(SingleThreaded) || subtreeCount * dispatcher.ThreadCount <= context->SubtreesPing.Length; if (subtreeCount == 2) { BuildNode(boundingBoxes[0], boundingBoxes[1], subtrees[0].LeafCount, subtrees[1].LeafCount, subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, 1, 1, ref context->Leaves, out _, out _); @@ -864,7 +865,7 @@ static unsafe void BinnedBuildNode( } var debugCentroidStartTime = Stopwatch.GetTimestamp(); BoundingBox4 centroidBounds; - if (typeof(TThreading) == typeof(SingleThreaded) || subtreeCount < SubtreesPerThreadForCentroidPrepass) + if (forceInternalSingleThreaded || subtreeCount < SubtreesPerThreadForCentroidPrepass) { centroidBounds = ComputeCentroidBounds(boundingBoxes); debugTimes.MTPrepass = false; @@ -943,7 +944,7 @@ static unsafe void BinnedBuildNode( binLeafCounts[i] = 0; } var debugBinStartTime = Stopwatch.GetTimestamp(); - if (typeof(TThreading) == typeof(SingleThreaded) || subtreeCount < SubtreesPerThreadForBinning) + if (forceInternalSingleThreaded || subtreeCount < SubtreesPerThreadForBinning) { BinSubtrees(centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, subtrees, binBoundingBoxes, binLeafCounts); debugTimes.MTBinning = false; @@ -1016,7 +1017,7 @@ static unsafe void BinnedBuildNode( { //If the current buffer is pong, then write to ping, and vice versa. var subtreesNext = (usePongBuffer ? context->SubtreesPing : context->SubtreesPong).Slice(subtreeRegionStartIndex, subtreeCount); - if (typeof(TThreading) == typeof(SingleThreaded) || subtreeCount < SubtreesPerThreadForPartitioning) + if (forceInternalSingleThreaded || subtreeCount < SubtreesPerThreadForPartitioning) { for (int i = 0; i < subtreeCount; ++i) { From cd44d7ff8f833f75f8c9caaa10243e4dddc9fbad Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 18 Mar 2023 21:41:17 -0500 Subject: [PATCH 697/947] Demos.GL builds again. --- DemoRenderer.GL/ShapeDrawing/ShapesExtractor.cs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/DemoRenderer.GL/ShapeDrawing/ShapesExtractor.cs b/DemoRenderer.GL/ShapeDrawing/ShapesExtractor.cs index 72d444212..137f6e9de 100644 --- a/DemoRenderer.GL/ShapeDrawing/ShapesExtractor.cs +++ b/DemoRenderer.GL/ShapeDrawing/ShapesExtractor.cs @@ -357,7 +357,7 @@ void PrepareForMultithreadedExecution(IThreadDispatcher threadDispatcher) pool.Take(threadDispatcher.ThreadCount, out workerCaches); for (int i = 0; i < workerCaches.Length; ++i) { - workerCaches[i] = new ShapeCache(128, threadDispatcher.GetThreadMemoryPool(i)); + workerCaches[i] = new ShapeCache(128, threadDispatcher.WorkerPools[i]); } } @@ -366,7 +366,7 @@ void EndMultithreadedExecution() jobs.Dispose(pool); for (int i = 0; i < workerCaches.Length; ++i) { - workerCaches[i].Dispose(looper.Dispatcher.GetThreadMemoryPool(i)); + workerCaches[i].Dispose(looper.Dispatcher.WorkerPools[i]); } looper.Dispatcher = null; pool.Return(ref workerCaches); @@ -413,7 +413,7 @@ void AddShapesForJob(int jobIndex, int workerIndex) { var job = jobs[jobIndex]; var simulation = simulations == null ? this.simulation : this.simulations[job.SimulationIndex]; - var pool = looper.Dispatcher.GetThreadMemoryPool(workerIndex); + var pool = looper.Dispatcher.WorkerPools[workerIndex]; if (job.SetIndex >= 0) { From 32c6f6105b4e86db97696887e1e9317cd1695d65 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 18 Mar 2023 21:48:34 -0500 Subject: [PATCH 698/947] GL path now *runs* too! Ported over MeshCache renderer fix. --- DemoRenderer.GL/ShapeDrawing/MeshCache.cs | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/DemoRenderer.GL/ShapeDrawing/MeshCache.cs b/DemoRenderer.GL/ShapeDrawing/MeshCache.cs index 0cd1cb26b..46480ea55 100644 --- a/DemoRenderer.GL/ShapeDrawing/MeshCache.cs +++ b/DemoRenderer.GL/ShapeDrawing/MeshCache.cs @@ -52,6 +52,7 @@ public bool TryGetExistingMesh(ulong id, out int start, out Buffer vert public bool Allocate(ulong id, int vertexCount, out int start, out Buffer vertices) { + requestedIds.Add(id, Pool); if (TryGetExistingMesh(id, out start, out vertices)) { return false; @@ -65,8 +66,8 @@ public bool Allocate(ulong id, int vertexCount, out int start, out Buffer Date: Mon, 20 Mar 2023 12:49:45 -0500 Subject: [PATCH 699/947] Demo set returned to defaults. --- Demos/DemoSet.cs | 4 ---- 1 file changed, 4 deletions(-) diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index 8f2a87401..94637674e 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -46,10 +46,6 @@ struct Option public DemoSet() { - //AddOption(); - AddOption(); - //AddOption(); - //AddOption(); AddOption(); AddOption(); AddOption(); From a7a318300701f32fe9bf76572b5505f8f9ad3f19 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 21 Mar 2023 18:27:05 -0500 Subject: [PATCH 700/947] Changed task scheduling strategy. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 104 ++-- .../SpecializedTests/TreeFiddlingTestDemo.cs | 508 +++++++++--------- 2 files changed, 307 insertions(+), 305 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index b2b80b0e4..a2f66f5a5 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -140,6 +140,7 @@ public struct NodeTimes public bool MTPrepass; public bool MTBinning; public bool MTPartition; + public int TargetTaskCount; public int SubtreeCount; } @@ -392,10 +393,13 @@ unsafe struct MultithreadBinnedBuildContext : IBinnedBuilderThreading { public LinkedTaskStack* TaskStack; /// - /// Maximum number of tasks any one job submission should create. - /// If you have far more tasks than there are workers, adding more tasks just adds overhead without additional workstealing advantages. + /// The number of subtrees present at the root of the build. /// - public int MaximumTaskCountPerSubmission; + public int OriginalSubtreeCount; + /// + /// The target number of tasks that would be used for the root node. Later nodes will tend to target smaller numbers of tasks on the assumption that other parallel nodes will provide enough work to fill in the gaps. + /// + public int TopLevelTargetTaskCount; public Buffer Workers; public void GetBins(int workerIndex, out Buffer binBoundingBoxes, out Buffer binBoundingBoxesScan, out Buffer binLeafCounts) @@ -405,13 +409,17 @@ public void GetBins(int workerIndex, out Buffer binBoundingBoxes, binBoundingBoxesScan = worker.BinBoundingBoxesScan; binLeafCounts = worker.BinLeafCounts; } + + public int GetTargetTaskCount(int subtreeCount) + { + return (int)float.Ceiling(TopLevelTargetTaskCount * (float)subtreeCount / OriginalSubtreeCount); + } } - //These should be powers of 2 for maskhack reasons. - const int SubtreesPerThreadForCentroidPrepass = 65536; - const int SubtreesPerThreadForBinning = 65536; - const int SubtreesPerThreadForPartitioning = 65536; - const int SubtreesPerThreadForNodeJob = 1024; + const int MinimumSubtreesPerThreadForCentroidPrepass = 65536; + const int MinimumSubtreesPerThreadForBinning = 65536; + const int MinimumSubtreesPerThreadForPartitioning = 65536; + const int MinimumSubtreesPerThreadForNodeJob = 1024; [MethodImpl(MethodImplOptions.AggressiveInlining)] static BoundingBox4 ComputeCentroidBounds(Buffer bounds) @@ -442,11 +450,12 @@ struct SharedTaskData public int SlotRemainder; public bool TaskCountFitsInWorkerCount; - public SharedTaskData(int workerCount, int subtreeStartIndex, int slotCount, int slotsPerTaskTarget, int maximumTaskCountPerSubmission) + public SharedTaskData(int workerCount, int subtreeStartIndex, int slotCount, + int minimumSlotsPerTask, int targetTaskCount) { WorkerCount = workerCount; - Debug.Assert(BitOperations.IsPow2(slotsPerTaskTarget), "Ideally, this gets inlined and the division becomes a shift. Can't do that if the count isn't a power of 2."); - TaskCount = int.Min(maximumTaskCountPerSubmission, (slotCount + slotsPerTaskTarget - 1) / slotsPerTaskTarget); + var taskSize = int.Max(minimumSlotsPerTask, slotCount / targetTaskCount); + TaskCount = (slotCount + taskSize - 1) / taskSize; SubtreeStartIndex = subtreeStartIndex; SubtreeCount = slotCount; SlotsPerTaskBase = slotCount / TaskCount; @@ -506,11 +515,11 @@ unsafe static void CentroidPrepassWorker(long taskId, void* untypedContext, int } } - unsafe static BoundingBox4 MultithreadedCentroidPrepass(MultithreadBinnedBuildContext* context, Buffer subtrees, int workerIndex, IThreadDispatcher dispatcher) + unsafe static BoundingBox4 MultithreadedCentroidPrepass(MultithreadBinnedBuildContext* context, Buffer subtrees, int targetTaskCount, int workerIndex, IThreadDispatcher dispatcher) { ref var worker = ref context->Workers[workerIndex]; var workerPool = dispatcher.WorkerPools[workerIndex]; - var taskContext = new CentroidPrepassTaskContext(workerPool, new SharedTaskData(context->Workers.Length, 0, subtrees.Length, SubtreesPerThreadForCentroidPrepass, context->MaximumTaskCountPerSubmission), subtrees); + var taskContext = new CentroidPrepassTaskContext(workerPool, new SharedTaskData(context->Workers.Length, 0, subtrees.Length, MinimumSubtreesPerThreadForCentroidPrepass, targetTaskCount), subtrees); Debug.Assert(taskContext.TaskData.TaskCount > 1, "This codepath shouldn't be used if there's only one task!"); Debug.Assert(taskContext.TaskData.WorkerCount > 1 && taskContext.TaskData.WorkerCount < 100); var taskCount = taskContext.TaskData.TaskCount; @@ -656,7 +665,7 @@ unsafe static void MultithreadedBinSubtrees(MultithreadBinnedBuildContext* conte var workerPool = dispatcher.WorkerPools[workerIndex]; var taskContext = new BinSubtreesTaskContext( workerPool, - new SharedTaskData(context->Workers.Length, 0, subtrees.Length, SubtreesPerThreadForBinning, context->MaximumTaskCountPerSubmission), + new SharedTaskData(context->Workers.Length, 0, subtrees.Length, MinimumSubtreesPerThreadForBinning, context->GetTargetTaskCount(subtrees.Length)), subtrees, binCount, useX, useY, permuteMask, axisIndex, centroidBoundsMin, offsetToBinIndex, maximumBinIndex); //Don't bother initializing more slots than we have tasks. Note that this requires special handling on the task level; @@ -813,12 +822,12 @@ unsafe static void PartitionSubtreesWorker(long taskId, void* untypedContext, in unsafe static (int subtreeCountA, int subtreeCountB) MultithreadedPartition(MultithreadBinnedBuildContext* context, Vector4 centroidBoundsMin, bool useX, bool useY, Vector128 permuteMask, int axisIndex, Vector4 offsetToBinIndex, Vector4 maximumBinIndex, - Buffer subtrees, Buffer subtreesNext, int binSplitIndex, int binCount, int workerIndex, IThreadDispatcher dispatcher) + Buffer subtrees, Buffer subtreesNext, int binSplitIndex, int binCount, int targetTaskCount, int workerIndex, IThreadDispatcher dispatcher) { ref var worker = ref context->Workers[workerIndex]; var workerPool = dispatcher.WorkerPools[workerIndex]; var taskContext = new PartitionTaskContext( - new SharedTaskData(context->Workers.Length, 0, subtrees.Length, SubtreesPerThreadForPartitioning, context->MaximumTaskCountPerSubmission), + new SharedTaskData(context->Workers.Length, 0, subtrees.Length, MinimumSubtreesPerThreadForPartitioning, targetTaskCount), subtrees, subtreesNext, binSplitIndex, binCount, useX, useY, permuteMask, axisIndex, centroidBoundsMin, offsetToBinIndex, maximumBinIndex); context->TaskStack->For(&PartitionSubtreesWorker, &taskContext, 0, taskContext.TaskData.TaskCount, workerIndex, dispatcher); @@ -848,35 +857,36 @@ static unsafe void BinnedBuildNode( bool usePongBuffer, int subtreeRegionStartIndex, int nodeIndex, int subtreeCount, int parentNodeIndex, int childIndexInParent, Context* context, int workerIndex, IThreadDispatcher dispatcher) where TLeaves : unmanaged where TThreading : unmanaged, IBinnedBuilderThreading { - var debugStartTime = Stopwatch.GetTimestamp(); - ref var debugTimes = ref Times[nodeIndex]; - debugTimes.SubtreeCount = subtreeCount; + //var debugStartTime = Stopwatch.GetTimestamp(); + //ref var debugTimes = ref Times[nodeIndex]; + //debugTimes.SubtreeCount = subtreeCount; var subtrees = (usePongBuffer ? context->SubtreesPong : context->SubtreesPing).Slice(subtreeRegionStartIndex, subtreeCount); //leaf counts, indices, and bounds are packed together, but it's useful to have a bounds-only representation so that the merging processes don't have to worry about dealing with the fourth lanes. var boundingBoxes = subtrees.As(); var nodeCount = subtreeCount - 1; var nodes = context->Nodes.Slice(nodeIndex, nodeCount); var metanodes = context->Metanodes.Slice(nodeIndex, nodeCount); - var forceInternalSingleThreaded = typeof(TThreading) == typeof(SingleThreaded) || subtreeCount * dispatcher.ThreadCount <= context->SubtreesPing.Length; if (subtreeCount == 2) { BuildNode(boundingBoxes[0], boundingBoxes[1], subtrees[0].LeafCount, subtrees[1].LeafCount, subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, 1, 1, ref context->Leaves, out _, out _); return; } - var debugCentroidStartTime = Stopwatch.GetTimestamp(); + var targetTaskCount = typeof(TThreading) == typeof(SingleThreaded) ? 1 : + ((MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As(ref context->Threading)))->GetTargetTaskCount(subtreeCount); + //var debugCentroidStartTime = Stopwatch.GetTimestamp(); BoundingBox4 centroidBounds; - if (forceInternalSingleThreaded || subtreeCount < SubtreesPerThreadForCentroidPrepass) + if (targetTaskCount == 1 || subtreeCount < MinimumSubtreesPerThreadForCentroidPrepass) { centroidBounds = ComputeCentroidBounds(boundingBoxes); - debugTimes.MTPrepass = false; + //debugTimes.MTPrepass = false; } else { centroidBounds = MultithreadedCentroidPrepass( - (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As(ref context->Threading)), subtrees, workerIndex, dispatcher); - debugTimes.MTPrepass = true; + (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As(ref context->Threading)), subtrees, targetTaskCount, workerIndex, dispatcher); + //debugTimes.MTPrepass = true; } - var debugCentroidEndTime = Stopwatch.GetTimestamp(); + //var debugCentroidEndTime = Stopwatch.GetTimestamp(); var centroidSpan = centroidBounds.Max - centroidBounds.Min; var axisIsDegenerate = Vector128.LessThanOrEqual(centroidSpan.AsVector128(), Vector128.Create(1e-12f)); if ((Vector128.ExtractMostSignificantBits(axisIsDegenerate) & 0b111) == 0b111) @@ -943,20 +953,20 @@ static unsafe void BinnedBuildNode( boxX.Max = new Vector4(float.MinValue); binLeafCounts[i] = 0; } - var debugBinStartTime = Stopwatch.GetTimestamp(); - if (forceInternalSingleThreaded || subtreeCount < SubtreesPerThreadForBinning) + //var debugBinStartTime = Stopwatch.GetTimestamp(); + if (targetTaskCount == 1 || subtreeCount < MinimumSubtreesPerThreadForBinning) { BinSubtrees(centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, subtrees, binBoundingBoxes, binLeafCounts); - debugTimes.MTBinning = false; + //debugTimes.MTBinning = false; } else { MultithreadedBinSubtrees( (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As(ref context->Threading)), centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, subtrees, binCount, workerIndex, dispatcher); - debugTimes.MTBinning = true; + //debugTimes.MTBinning = true; } - var debugBinEndTime = Stopwatch.GetTimestamp(); + //var debugBinEndTime = Stopwatch.GetTimestamp(); //Identify the split index by examining the SAH of very split option. //Premerge from left to right so we have a sorta-summed area table to cheaply look up all possible child A bounds as we scan. @@ -1005,7 +1015,7 @@ static unsafe void BinnedBuildNode( accumulatedLeafCountB += binLeafCounts[previousIndex]; } - var debugHuhStartTime = Stopwatch.GetTimestamp(); + //var debugHuhStartTime = Stopwatch.GetTimestamp(); var subtreeCountB = 0; var subtreeCountA = 0; var splitIndex = bestSplit; @@ -1017,7 +1027,7 @@ static unsafe void BinnedBuildNode( { //If the current buffer is pong, then write to ping, and vice versa. var subtreesNext = (usePongBuffer ? context->SubtreesPing : context->SubtreesPong).Slice(subtreeRegionStartIndex, subtreeCount); - if (forceInternalSingleThreaded || subtreeCount < SubtreesPerThreadForPartitioning) + if (targetTaskCount == 1 || subtreeCount < MinimumSubtreesPerThreadForPartitioning) { for (int i = 0; i < subtreeCount; ++i) { @@ -1025,14 +1035,14 @@ static unsafe void BinnedBuildNode( var targetIndex = binIndex >= splitIndex ? subtreeCount - ++subtreeCountB : subtreeCountA++; subtreesNext[targetIndex] = subtrees[i]; } - debugTimes.MTPartition = false; + //debugTimes.MTPartition = false; } else { (subtreeCountA, subtreeCountB) = MultithreadedPartition( (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As(ref context->Threading)), - centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, subtrees, subtreesNext, splitIndex, binCount, workerIndex, dispatcher); - debugTimes.MTPartition = true; + centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, subtrees, subtreesNext, splitIndex, binCount, targetTaskCount, workerIndex, dispatcher); + //debugTimes.MTPartition = true; } subtrees = subtreesNext; usePongBuffer = !usePongBuffer; @@ -1072,23 +1082,24 @@ static unsafe void BinnedBuildNode( } } - var debugHuhEndTime = Stopwatch.GetTimestamp(); + //var debugHuhEndTime = Stopwatch.GetTimestamp(); var leafCountB = bestLeafCountB; var leafCountA = totalLeafCount - leafCountB; Debug.Assert(subtreeCountA + subtreeCountB == subtreeCount); BuildNode(bestBoundsA, bestBoundsB, leafCountA, leafCountB, subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, subtreeCountA, subtreeCountB, ref context->Leaves, out var nodeChildIndexA, out var nodeChildIndexB); - var debugEndTime = Stopwatch.GetTimestamp(); - debugTimes.Total = (debugEndTime - debugStartTime) / (double)Stopwatch.Frequency; - debugTimes.Partition = (debugHuhEndTime - debugHuhStartTime) / (double)Stopwatch.Frequency; - debugTimes.CentroidPrepass = (debugCentroidEndTime - debugCentroidStartTime) / (double)Stopwatch.Frequency; - debugTimes.Binning = (debugBinEndTime - debugBinStartTime) / (double)Stopwatch.Frequency; - - var shouldPushBOntoMultithreadedQueue = typeof(TThreading) != typeof(SingleThreaded) && subtreeCountA >= SubtreesPerThreadForNodeJob && subtreeCountB >= SubtreesPerThreadForNodeJob; + //var debugEndTime = Stopwatch.GetTimestamp(); + //debugTimes.Total = (debugEndTime - debugStartTime) / (double)Stopwatch.Frequency; + //debugTimes.Partition = (debugHuhEndTime - debugHuhStartTime) / (double)Stopwatch.Frequency; + //debugTimes.CentroidPrepass = (debugCentroidEndTime - debugCentroidStartTime) / (double)Stopwatch.Frequency; + //debugTimes.Binning = (debugBinEndTime - debugBinStartTime) / (double)Stopwatch.Frequency; + //debugTimes.TargetTaskCount = targetTaskCount; + + var shouldPushBOntoMultithreadedQueue = typeof(TThreading) != typeof(SingleThreaded) && subtreeCountA >= MinimumSubtreesPerThreadForNodeJob && subtreeCountB >= MinimumSubtreesPerThreadForNodeJob; ContinuationHandle nodeBContinuation = default; if (shouldPushBOntoMultithreadedQueue) { //Both of the children are large. Push child B onto the multithreaded execution stack so it can run at the same time as child A (potentially). - Debug.Assert(SubtreesPerThreadForNodeJob > 1, "The job threshold for a new node should be large enough that there's no need for a subtreeCountB > 1 test."); + Debug.Assert(MinimumSubtreesPerThreadForNodeJob > 1, "The job threshold for a new node should be large enough that there's no need for a subtreeCountB > 1 test."); ref var threading = ref Unsafe.As(ref context->Threading); //Allocate the parameters to send to the worker on the local stack. Note that we have to preserve the stack for this to work; see the later WaitForCompletion. NodePushTaskContext nodePushContext; @@ -1208,7 +1219,8 @@ static unsafe void BinnedBuilderInternal(Buffer subtrees, Buffer { - struct Pair : IEquatable - { - public int A; - public int B; + public int A; + public int B; - public Pair(int a, int b) - { - A = a; - B = b; - } + public Pair(int a, int b) + { + A = a; + B = b; + } - public bool Equals(Pair other) - { - return (A == other.A && B == other.B) || (A == other.B && B == other.A); - } + public bool Equals(Pair other) + { + return (A == other.A && B == other.B) || (A == other.B && B == other.A); + } - public override bool Equals(object obj) - { - return obj is Pair pair && Equals(pair); - } - public override int GetHashCode() - { - return A.GetHashCode() + B.GetHashCode(); - } - public override string ToString() - { - return $"{A}, {B}"; - } + public override bool Equals(object obj) + { + return obj is Pair pair && Equals(pair); } - struct OverlapHandler : IOverlapHandler + public override int GetHashCode() { - public int OverlapCount; - public int OverlapSum; - public int OverlapHash; - public int TreeLeafCount; + return A.GetHashCode() + B.GetHashCode(); + } + public override string ToString() + { + return $"{A}, {B}"; + } + } + struct OverlapHandler : IOverlapHandler + { + public int OverlapCount; + public int OverlapSum; + public int OverlapHash; + public int TreeLeafCount; - //public HashSet Set; + //public HashSet Set; - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Handle(int indexA, int indexB) - { - //Debug.Assert(indexA >= 0 && indexB >= 0 && indexA < TreeLeafCount && indexB < TreeLeafCount && Set.Add(new Pair(indexA, indexB))); - ++OverlapCount; - OverlapSum += indexA + indexB; - OverlapHash += (indexA + (indexB * OverlapCount)) * OverlapCount; - } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void Handle(int indexA, int indexB) + { + //Debug.Assert(indexA >= 0 && indexB >= 0 && indexA < TreeLeafCount && indexB < TreeLeafCount && Set.Add(new Pair(indexA, indexB))); + ++OverlapCount; + OverlapSum += indexA + indexB; + OverlapHash += (indexA + (indexB * OverlapCount)) * OverlapCount; } + } - Buffer CreateDeformedPlaneTriangles(int width, int height, Vector3 scale) + Buffer CreateDeformedPlaneTriangles(int width, int height, Vector3 scale) + { + Vector3 Deform(int x, int y) => new Vector3(x - width * scale.X * 0.5f, 2f * (float)(Math.Sin(x * 0.5f) * Math.Sin(y * 0.5f)), y - height * scale.Y * 0.5f); + BufferPool.Take(width * height, out var vertices); + for (int i = 0; i < width; ++i) { - Vector3 Deform(int x, int y) => new Vector3(x - width * scale.X * 0.5f, 2f * (float)(Math.Sin(x * 0.5f) * Math.Sin(y * 0.5f)), y - height * scale.Y * 0.5f); - BufferPool.Take(width * height, out var vertices); - for (int i = 0; i < width; ++i) + for (int j = 0; j < height; ++j) { - for (int j = 0; j < height; ++j) - { - vertices[width * j + i] = Deform(i, j); - } + vertices[width * j + i] = Deform(i, j); } + } - var quadWidth = width - 1; - var quadHeight = height - 1; - var triangleCount = quadWidth * quadHeight * 2; - BufferPool.Take(triangleCount, out var triangles); + var quadWidth = width - 1; + var quadHeight = height - 1; + var triangleCount = quadWidth * quadHeight * 2; + BufferPool.Take(triangleCount, out var triangles); - for (int i = 0; i < quadWidth; ++i) - { - for (int j = 0; j < quadHeight; ++j) - { - var triangleIndex = (j * quadWidth + i) * 2; - ref var triangle0 = ref triangles[triangleIndex]; - ref var v00 = ref vertices[width * j + i]; - ref var v01 = ref vertices[width * j + i + 1]; - ref var v10 = ref vertices[width * (j + 1) + i]; - ref var v11 = ref vertices[width * (j + 1) + i + 1]; - triangle0.A = v00; - triangle0.B = v01; - triangle0.C = v10; - ref var triangle1 = ref triangles[triangleIndex + 1]; - triangle1.A = v01; - triangle1.B = v11; - triangle1.C = v10; - } - } - BufferPool.Return(ref vertices); - //Scramble the heck out of its triangles. - var random = new Random(5); - for (int index = 0; index < triangles.Length - 1; ++index) + for (int i = 0; i < quadWidth; ++i) + { + for (int j = 0; j < quadHeight; ++j) { - ref var a = ref triangles[index]; - ref var b = ref triangles[random.Next(index + 1, triangles.Length)]; - BepuPhysics.Helpers.Swap(ref a, ref b); + var triangleIndex = (j * quadWidth + i) * 2; + ref var triangle0 = ref triangles[triangleIndex]; + ref var v00 = ref vertices[width * j + i]; + ref var v01 = ref vertices[width * j + i + 1]; + ref var v10 = ref vertices[width * (j + 1) + i]; + ref var v11 = ref vertices[width * (j + 1) + i + 1]; + triangle0.A = v00; + triangle0.B = v01; + triangle0.C = v10; + ref var triangle1 = ref triangles[triangleIndex + 1]; + triangle1.A = v01; + triangle1.B = v11; + triangle1.C = v10; } - return triangles; } + BufferPool.Return(ref vertices); + //Scramble the heck out of its triangles. + var random = new Random(5); + for (int index = 0; index < triangles.Length - 1; ++index) + { + ref var a = ref triangles[index]; + ref var b = ref triangles[random.Next(index + 1, triangles.Length)]; + BepuPhysics.Helpers.Swap(ref a, ref b); + } + return triangles; + } - Buffer CreateRandomSoupTriangles(BoundingBox bounds, int triangleCount, float minimumSize, float maximumSize) + Buffer CreateRandomSoupTriangles(BoundingBox bounds, int triangleCount, float minimumSize, float maximumSize) + { + Random random = new Random(5); + BufferPool.Take(triangleCount, out var triangles); + for (int i = 0; i < triangleCount; ++i) { - Random random = new Random(5); - BufferPool.Take(triangleCount, out var triangles); - for (int i = 0; i < triangleCount; ++i) - { - var startPoint = new Vector3(random.NextSingle() * random.NextSingle(), random.NextSingle(), random.NextSingle() * random.NextSingle()) * (bounds.Max - bounds.Min) + bounds.Min; - var size = new Vector3(MathF.Pow(random.NextSingle(), 200), MathF.Pow(random.NextSingle(), 200), MathF.Pow(random.NextSingle(), 200)) * (maximumSize - minimumSize) + new Vector3(minimumSize); + var startPoint = new Vector3(random.NextSingle() * random.NextSingle(), random.NextSingle(), random.NextSingle() * random.NextSingle()) * (bounds.Max - bounds.Min) + bounds.Min; + var size = new Vector3(MathF.Pow(random.NextSingle(), 200), MathF.Pow(random.NextSingle(), 200), MathF.Pow(random.NextSingle(), 200)) * (maximumSize - minimumSize) + new Vector3(minimumSize); - ref var triangle = ref triangles[i]; - triangle.A = (2 * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) - Vector3.One) * size; - triangle.B = (2 * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) - Vector3.One) * size; - triangle.C = (2 * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) - Vector3.One) * size; + ref var triangle = ref triangles[i]; + triangle.A = (2 * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) - Vector3.One) * size; + triangle.B = (2 * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) - Vector3.One) * size; + triangle.C = (2 * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) - Vector3.One) * size; - if (random.NextSingle() < 0.75f) - { - var rotation = Quaternion.CreateFromAxisAngle(Vector3.Normalize(new Vector3(0.0001f) + new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle())), random.NextSingle() * MathF.PI * 2); - triangle.A = Vector3.Transform(triangle.A, rotation); - triangle.B = Vector3.Transform(triangle.B, rotation); - triangle.C = Vector3.Transform(triangle.C, rotation); - } - triangle.A += startPoint; - triangle.B += startPoint; - triangle.C += startPoint; + if (random.NextSingle() < 0.75f) + { + var rotation = Quaternion.CreateFromAxisAngle(Vector3.Normalize(new Vector3(0.0001f) + new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle())), random.NextSingle() * MathF.PI * 2); + triangle.A = Vector3.Transform(triangle.A, rotation); + triangle.B = Vector3.Transform(triangle.B, rotation); + triangle.C = Vector3.Transform(triangle.C, rotation); } - return triangles; + triangle.A += startPoint; + triangle.B += startPoint; + triangle.C += startPoint; } + return triangles; + } - public override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(-10, 3, -10); + camera.Yaw = MathHelper.Pi * 3f / 4; + camera.Pitch = 0; + Tree.Times = new Tree.NodeTimes[1 << 22]; + for (int i = 0; i < 2; ++i) { - camera.Position = new Vector3(-10, 3, -10); - camera.Yaw = MathHelper.Pi * 3f / 4; - camera.Pitch = 0; - Tree.Times = new Tree.NodeTimes[1 << 22]; - for (int i = 0; i < 2; ++i) - { - BufferPool.Clear(); - ThreadDispatcher.WorkerPools.Clear(); - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(4, 1)); + BufferPool.Clear(); + ThreadDispatcher.WorkerPools.Clear(); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(4, 1)); - //Create a mesh. - var width = 1024; - var height = 1024; - var scale = new Vector3(1, 1, 1); - //DemoMeshHelper.CreateDeformedPlane(width, height, (x, y) => new Vector3(x - width * scale.X * 0.5f, 2f * (float)(Math.Sin(x * 0.5f) * Math.Sin(y * 0.5f)), y - height * scale.Y * 0.5f), scale, BufferPool, out var mesh); - //DemoMeshHelper.CreateDeformedPlane(width, height, (x, y) => new Vector3(x - width * scale.X * 0.5f, 0, y - height * scale.Y * 0.5f), scale, BufferPool, out var mesh); + //Create a mesh. + var width = 1024; + var height = 1024; + var scale = new Vector3(1, 1, 1); + //DemoMeshHelper.CreateDeformedPlane(width, height, (x, y) => new Vector3(x - width * scale.X * 0.5f, 2f * (float)(Math.Sin(x * 0.5f) * Math.Sin(y * 0.5f)), y - height * scale.Y * 0.5f), scale, BufferPool, out var mesh); + //DemoMeshHelper.CreateDeformedPlane(width, height, (x, y) => new Vector3(x - width * scale.X * 0.5f, 0, y - height * scale.Y * 0.5f), scale, BufferPool, out var mesh); - //var triangles = CreateDeformedPlaneTriangles(width, height, scale); - var triangles = CreateRandomSoupTriangles(new BoundingBox(new(width / -2f, scale.Y * -2, height / -2f), new(width / 2f, scale.Y * 2, height / 2f)), (width - 1) * (height - 1) * 2, 0.5f, 100f); - //var mesh = new Mesh(triangles, Vector3.One, BufferPool); - var mesh = DemoMeshHelper.CreateGiantMeshFast(triangles, Vector3.One, BufferPool); + //var triangles = CreateDeformedPlaneTriangles(width, height, scale); + var triangles = CreateRandomSoupTriangles(new BoundingBox(new(width / -2f, scale.Y * -2, height / -2f), new(width / 2f, scale.Y * 2, height / 2f)), (width - 1) * (height - 1) * 2, 0.5f, 100f); + //var mesh = new Mesh(triangles, Vector3.One, BufferPool); + var mesh = DemoMeshHelper.CreateGiantMeshFast(triangles, Vector3.One, BufferPool); - Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); + Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); - Console.WriteLine($"node count: {mesh.Tree.NodeCount}"); - Console.WriteLine($"initial SAH: {mesh.Tree.MeasureCostMetric()}, cache quality: {mesh.Tree.MeasureCacheQuality()}"); - Console.WriteLine($"initial bounds: A ({mesh.Tree.Nodes[0].A.Min}, {mesh.Tree.Nodes[0].B.Max}), B ({mesh.Tree.Nodes[0].B.Min}, {mesh.Tree.Nodes[0].B.Max})"); + Console.WriteLine($"node count: {mesh.Tree.NodeCount}"); + Console.WriteLine($"initial SAH: {mesh.Tree.MeasureCostMetric()}, cache quality: {mesh.Tree.MeasureCacheQuality()}"); + Console.WriteLine($"initial bounds: A ({mesh.Tree.Nodes[0].A.Min}, {mesh.Tree.Nodes[0].B.Max}), B ({mesh.Tree.Nodes[0].B.Min}, {mesh.Tree.Nodes[0].B.Max})"); - BufferPool.Take(mesh.Triangles.Length, out var subtrees); + BufferPool.Take(mesh.Triangles.Length, out var subtrees); - Action setup = () => - { - for (int i = 0; i < mesh.Triangles.Length; ++i) - { - ref var t = ref mesh.Triangles[i]; - ref var subtree = ref subtrees[i]; - subtree.Min = Vector3.Min(t.A, Vector3.Min(t.B, t.C)); - subtree.Max = Vector3.Max(t.A, Vector3.Max(t.B, t.C)); - subtree.Index = Tree.Encode(i); - subtree.LeafCount = 1; - } - }; - - BinnedTest(setup, () => + Action setup = () => + { + for (int i = 0; i < mesh.Triangles.Length; ++i) { - Tree.BinnedBuilder(subtrees, mesh.Tree.Nodes, mesh.Tree.Metanodes, mesh.Tree.Leaves, ThreadDispatcher, BufferPool); - }, "Revamp Single Axis MT", ref mesh.Tree); - - - //BufferPool.Take(mesh.Triangles.Length, out var leafBounds); - //BufferPool.Take(mesh.Triangles.Length, out var leafIndices); - - //Action yeOldeSetup = () => - //{ - // for (int i = 0; i < mesh.Triangles.Length; ++i) - // { - // ref var t = ref mesh.Triangles[i]; - // ref var bounds = ref leafBounds[i]; - // bounds.Min = Vector3.Min(t.A, Vector3.Min(t.B, t.C)); - // bounds.Max = Vector3.Max(t.A, Vector3.Max(t.B, t.C)); - // leafIndices[i] = Tree.Encode(i); - // } - //}; - //BinnedTest(yeOldeSetup, () => - //{ - // Tree.BinnedBuilder(leafIndices, leafBounds, mesh.Tree.Nodes, mesh.Tree.Metanodes, mesh.Tree.Leaves); - //}, "Revamp Single Axis ST", ref mesh.Tree); - - - - //Mesh mesh2 = default; - //Mesh* mesh2Pointer = &mesh2; - - //QuickList subtreeReferences = new(triangles.Length, BufferPool); - //QuickList treeletInternalNodes = new(triangles.Length, BufferPool); - //Tree.CreateBinnedResources(BufferPool, triangles.Length, out var binnedResourcesBuffer, out var binnedResources); - //BinnedTest(() => - //{ - // if (mesh2Pointer->Tree.Leaves.Allocated) - // mesh2Pointer->Tree.Dispose(BufferPool); - // *mesh2Pointer = DemoMeshHelper.CreateGiantMeshFast(triangles, Vector3.One, BufferPool); - //}, () => - //{ - // subtreeReferences.Count = 0; - // treeletInternalNodes.Count = 0; - // mesh2Pointer->Tree.BinnedRefine(0, ref subtreeReferences, mesh2Pointer->Tree.LeafCount, ref treeletInternalNodes, ref binnedResources, BufferPool); - //}, "Original", ref mesh2Pointer->Tree); - - //RefitTest(() => mesh.Tree.Refit(), "Refit", ref mesh.Tree); - - //SelfTest((ref OverlapHandler handler) => mesh.Tree.GetSelfOverlapsContiguousPrepass(ref handler, BufferPool), mesh.Tree.LeafCount, "Prepass"); - //SelfTest((ref OverlapHandler handler) => mesh.Tree.GetSelfOverlaps(ref handler), mesh.Tree.LeafCount, "Original"); - - Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 10, 0), 1, Simulation.Shapes, new Sphere(0.5f))); - } + ref var t = ref mesh.Triangles[i]; + ref var subtree = ref subtrees[i]; + subtree.Min = Vector3.Min(t.A, Vector3.Min(t.B, t.C)); + subtree.Max = Vector3.Max(t.A, Vector3.Max(t.B, t.C)); + subtree.Index = Tree.Encode(i); + subtree.LeafCount = 1; + } + }; + + BinnedTest(setup, () => + { + Tree.BinnedBuilder(subtrees, mesh.Tree.Nodes, mesh.Tree.Metanodes, mesh.Tree.Leaves, ThreadDispatcher, BufferPool); + }, "Revamp Single Axis MT", ref mesh.Tree); + + + //BufferPool.Take(mesh.Triangles.Length, out var leafBounds); + //BufferPool.Take(mesh.Triangles.Length, out var leafIndices); + + //Action yeOldeSetup = () => + //{ + // for (int i = 0; i < mesh.Triangles.Length; ++i) + // { + // ref var t = ref mesh.Triangles[i]; + // ref var bounds = ref leafBounds[i]; + // bounds.Min = Vector3.Min(t.A, Vector3.Min(t.B, t.C)); + // bounds.Max = Vector3.Max(t.A, Vector3.Max(t.B, t.C)); + // leafIndices[i] = Tree.Encode(i); + // } + //}; + //BinnedTest(yeOldeSetup, () => + //{ + // Tree.BinnedBuilder(leafIndices, leafBounds, mesh.Tree.Nodes, mesh.Tree.Metanodes, mesh.Tree.Leaves); + //}, "Revamp Single Axis ST", ref mesh.Tree); + + + + //Mesh mesh2 = default; + //Mesh* mesh2Pointer = &mesh2; + + //QuickList subtreeReferences = new(triangles.Length, BufferPool); + //QuickList treeletInternalNodes = new(triangles.Length, BufferPool); + //Tree.CreateBinnedResources(BufferPool, triangles.Length, out var binnedResourcesBuffer, out var binnedResources); + //BinnedTest(() => + //{ + // if (mesh2Pointer->Tree.Leaves.Allocated) + // mesh2Pointer->Tree.Dispose(BufferPool); + // *mesh2Pointer = DemoMeshHelper.CreateGiantMeshFast(triangles, Vector3.One, BufferPool); + //}, () => + //{ + // subtreeReferences.Count = 0; + // treeletInternalNodes.Count = 0; + // mesh2Pointer->Tree.BinnedRefine(0, ref subtreeReferences, mesh2Pointer->Tree.LeafCount, ref treeletInternalNodes, ref binnedResources, BufferPool); + //}, "Original", ref mesh2Pointer->Tree); + + //RefitTest(() => mesh.Tree.Refit(), "Refit", ref mesh.Tree); + + //SelfTest((ref OverlapHandler handler) => mesh.Tree.GetSelfOverlapsContiguousPrepass(ref handler, BufferPool), mesh.Tree.LeafCount, "Prepass"); + //SelfTest((ref OverlapHandler handler) => mesh.Tree.GetSelfOverlaps(ref handler), mesh.Tree.LeafCount, "Original"); + + Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 10, 0), 1, Simulation.Shapes, new Sphere(0.5f))); } + } - delegate void TestFunction(ref OverlapHandler handler); + delegate void TestFunction(ref OverlapHandler handler); - static void SelfTest(TestFunction function, int leafCount, string name) + static void SelfTest(TestFunction function, int leafCount, string name) + { + var overlapHandler = new OverlapHandler(); + overlapHandler.TreeLeafCount = leafCount; + //overlapHandler.Set = new HashSet(); + long accumulatedTime = 0; + const int testCount = 16; + for (int i = 0; i < testCount; ++i) { - var overlapHandler = new OverlapHandler(); - overlapHandler.TreeLeafCount = leafCount; - //overlapHandler.Set = new HashSet(); - long accumulatedTime = 0; - const int testCount = 16; - for (int i = 0; i < testCount; ++i) - { - var startTime = Stopwatch.GetTimestamp(); - function(ref overlapHandler); - var endTime = Stopwatch.GetTimestamp(); - accumulatedTime += endTime - startTime; - //overlapHandler.Set.Clear(); - CacheBlaster.Blast(); - } - Console.WriteLine($"{name} time per execution (ms): {(accumulatedTime) * 1e3 / (testCount * Stopwatch.Frequency)}"); - Console.WriteLine($"{name} count: {overlapHandler.OverlapCount}, sum {overlapHandler.OverlapSum}, hash {overlapHandler.OverlapHash}"); + var startTime = Stopwatch.GetTimestamp(); + function(ref overlapHandler); + var endTime = Stopwatch.GetTimestamp(); + accumulatedTime += endTime - startTime; + //overlapHandler.Set.Clear(); + CacheBlaster.Blast(); } + Console.WriteLine($"{name} time per execution (ms): {(accumulatedTime) * 1e3 / (testCount * Stopwatch.Frequency)}"); + Console.WriteLine($"{name} count: {overlapHandler.OverlapCount}, sum {overlapHandler.OverlapSum}, hash {overlapHandler.OverlapHash}"); + } - static void RefitTest(Action function, string name, ref Tree tree) + static void RefitTest(Action function, string name, ref Tree tree) + { + long accumulatedTime = 0; + const int testCount = 16; + for (int i = 0; i < testCount; ++i) { - long accumulatedTime = 0; - const int testCount = 16; - for (int i = 0; i < testCount; ++i) - { - var startTime = Stopwatch.GetTimestamp(); - function(); - var endTime = Stopwatch.GetTimestamp(); - accumulatedTime += endTime - startTime; - //overlapHandler.Set.Clear(); - CacheBlaster.Blast(); - } - Console.WriteLine($"{name} time per execution (ms): {(accumulatedTime) * 1e3 / (testCount * Stopwatch.Frequency)}"); - - var sum = tree.Nodes[0].A.Min * 5 + tree.Nodes[0].A.Max * 7 + tree.Nodes[0].B.Min * 13 + tree.Nodes[0].B.Max * 17; - var hash = Unsafe.As(ref sum.X) * 31 + Unsafe.As(ref sum.Y) * 37 + Unsafe.As(ref sum.Z) * 41; - Console.WriteLine($"{name} bounds 0 hash: {hash}, A ({tree.Nodes[0].A.Min}, {tree.Nodes[0].B.Max}), B ({tree.Nodes[0].B.Min}, {tree.Nodes[0].B.Max})"); + var startTime = Stopwatch.GetTimestamp(); + function(); + var endTime = Stopwatch.GetTimestamp(); + accumulatedTime += endTime - startTime; + //overlapHandler.Set.Clear(); + CacheBlaster.Blast(); } + Console.WriteLine($"{name} time per execution (ms): {(accumulatedTime) * 1e3 / (testCount * Stopwatch.Frequency)}"); - static void BinnedTest(Action setup, Action function, string name, ref Tree tree) + var sum = tree.Nodes[0].A.Min * 5 + tree.Nodes[0].A.Max * 7 + tree.Nodes[0].B.Min * 13 + tree.Nodes[0].B.Max * 17; + var hash = Unsafe.As(ref sum.X) * 31 + Unsafe.As(ref sum.Y) * 37 + Unsafe.As(ref sum.Z) * 41; + Console.WriteLine($"{name} bounds 0 hash: {hash}, A ({tree.Nodes[0].A.Min}, {tree.Nodes[0].B.Max}), B ({tree.Nodes[0].B.Min}, {tree.Nodes[0].B.Max})"); + } + + static void BinnedTest(Action setup, Action function, string name, ref Tree tree) + { + long accumulatedTime = 0; + const int testCount = 64; + for (int i = 0; i < testCount; ++i) { - long accumulatedTime = 0; - const int testCount = 64; - for (int i = 0; i < testCount; ++i) - { - setup?.Invoke(); - var startTime = Stopwatch.GetTimestamp(); - function(); - var endTime = Stopwatch.GetTimestamp(); - accumulatedTime += endTime - startTime; - //overlapHandler.Set.Clear(); - CacheBlaster.Blast(); - } - Console.WriteLine($"{name} time per execution (ms): {(accumulatedTime) * 1e3 / (testCount * Stopwatch.Frequency)}"); + setup?.Invoke(); + var startTime = Stopwatch.GetTimestamp(); + function(); + var endTime = Stopwatch.GetTimestamp(); + accumulatedTime += endTime - startTime; + //overlapHandler.Set.Clear(); + CacheBlaster.Blast(); + } + Console.WriteLine($"{name} time per execution (ms): {(accumulatedTime) * 1e3 / (testCount * Stopwatch.Frequency)}"); - ulong accumulator = 0; - for (int i = 0; i < 1000; ++i) - { - var index = (int)(((ulong)i * 941083987 + accumulator * 797003413) % (ulong)tree.NodeCount); - var localSum = tree.Nodes[index].A.Min * 5 + tree.Nodes[index].A.Max * 7 + tree.Nodes[index].B.Min * 13 + tree.Nodes[index].B.Max * 17; - var hash = Unsafe.As(ref localSum.X) * 31 + Unsafe.As(ref localSum.Y) * 37 + Unsafe.As(ref localSum.Z) * 41; - accumulator = ((accumulator << 7) | (accumulator >> (64 - 7))) + (ulong)hash; - } - Console.WriteLine($"{name} bounds hash: {accumulator}, A ({tree.Nodes[0].A.Min}, {tree.Nodes[0].B.Max}), B ({tree.Nodes[0].B.Min}, {tree.Nodes[0].B.Max})"); - Console.WriteLine($"SAH: {tree.MeasureCostMetric()}, cache quality: {tree.MeasureCacheQuality()}"); + ulong accumulator = 0; + for (int i = 0; i < 1000; ++i) + { + var index = (int)(((ulong)i * 941083987 + accumulator * 797003413) % (ulong)tree.NodeCount); + var localSum = tree.Nodes[index].A.Min * 5 + tree.Nodes[index].A.Max * 7 + tree.Nodes[index].B.Min * 13 + tree.Nodes[index].B.Max * 17; + var hash = Unsafe.As(ref localSum.X) * 31 + Unsafe.As(ref localSum.Y) * 37 + Unsafe.As(ref localSum.Z) * 41; + accumulator = ((accumulator << 7) | (accumulator >> (64 - 7))) + (ulong)hash; } + Console.WriteLine($"{name} bounds hash: {accumulator}, A ({tree.Nodes[0].A.Min}, {tree.Nodes[0].B.Max}), B ({tree.Nodes[0].B.Min}, {tree.Nodes[0].B.Max})"); + Console.WriteLine($"SAH: {tree.MeasureCostMetric()}, cache quality: {tree.MeasureCacheQuality()}"); } } From cee39a5c81957f387194e840fd8484beaef34d0b Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 21 Mar 2023 21:23:20 -0500 Subject: [PATCH 701/947] Prodding partition problems. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 68 +++++++++++++++++++++---- BepuUtilities/BundleIndexing.cs | 6 ++- 2 files changed, 63 insertions(+), 11 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index a2f66f5a5..9f3b5213f 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -787,7 +787,14 @@ unsafe static void PartitionSubtreesWorker(long taskId, void* untypedContext, in //Compromise! Stackalloc enough memory to cover sub-batches of the worker's subtrees, and do interlocked operations at the end of each batch. //Note that the main limit to the batch size is the amount of memory in L1 cache: the subtrees are 32 bytes per. const int batchSize = 256; - bool* slotBelongsToA = stackalloc bool[batchSize]; + //bool* slotBelongsToA = stackalloc bool[batchSize]; + int* slotBelongsToA = stackalloc int[batchSize]; + float* minimumsSpan = stackalloc float[Vector.Count]; + float* maximumsSpan = stackalloc float[Vector.Count]; + Vector offsetToBinIndexBundle = new Vector(context.AxisIndex == 0 ? context.OffsetToBinIndex.X : context.AxisIndex == 1 ? context.OffsetToBinIndex.Y : context.OffsetToBinIndex.Z); + Vector maximumBinIndexBundle = new Vector(context.MaximumBinIndex.X); + Vector centroidMinBundle = new Vector(context.AxisIndex == 0 ? context.CentroidBoundsMin.X : context.AxisIndex == 1 ? context.CentroidBoundsMin.Y : context.CentroidBoundsMin.Z); + Vector splitIndexBundle = new Vector(context.BinSplitIndex); var batchCount = (count + batchSize - 1) / batchSize; var boundingBoxes = context.Subtrees.As(); var subtrees = context.Subtrees; @@ -795,17 +802,60 @@ unsafe static void PartitionSubtreesWorker(long taskId, void* untypedContext, in for (int batchIndex = 0; batchIndex < batchCount; ++batchIndex) { var localCountA = 0; - var localCountB = 0; + int localCountB; var batchStart = start + batchIndex * batchSize; var countInBatch = int.Min(start + count - batchStart, batchSize); - for (int indexInBatch = 0; indexInBatch < countInBatch; ++indexInBatch) + if (Vector.IsSupported) { - var subtreeIndex = indexInBatch + batchStart; - var binIndex = ComputeBinIndex(centroidBoundsMin, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, boundingBoxes[subtreeIndex]); - var belongsToA = binIndex < context.BinSplitIndex; - slotBelongsToA[indexInBatch] = belongsToA; - if (belongsToA) ++localCountA; else ++localCountB; + Vector localCountABundle = Vector.Zero; + var bundleCount = (countInBatch + Vector.Count - 1) / Vector.Count; + var maxOffset = 4 + axisIndex; + + for (int bundleIndex = 0; bundleIndex < bundleCount; ++bundleIndex) + { + //Gather the bundle min/max scalars. + var countInBundle = int.Min(Vector.Count, countInBatch - bundleIndex * Vector.Count); + var boundsStartForBundle = boundingBoxes.Memory + batchStart + bundleIndex * Vector.Count; + for (int i = 0; i < countInBundle; ++i) + { + var index = bundleIndex * Vector.Count + i; + float* boundsForLane = (float*)(boundsStartForBundle + i); + minimumsSpan[i] = boundsForLane[axisIndex]; + maximumsSpan[i] = boundsForLane[maxOffset]; + } + var minimums = new Vector(new Span(minimumsSpan, Vector.Count)); + var maximums = new Vector(new Span(maximumsSpan, Vector.Count)); + //Now compute the actual bin indices and accumulate the local count bundles. + var binIndicesBundle = Vector.Min((minimums + maximums - centroidMinBundle) * offsetToBinIndexBundle, maximumBinIndexBundle); + var bundleLaneMask = BundleIndexing.CreateMaskForCountInBundle(countInBundle); + var belongsToA = Vector.BitwiseAnd(bundleLaneMask, Vector.LessThan(binIndicesBundle, splitIndexBundle)); + ((Vector*)slotBelongsToA)[bundleIndex] = belongsToA; + localCountABundle += Vector.ConditionalSelect(belongsToA, Vector.One, Vector.Zero); + } + localCountA = Vector.Sum(localCountABundle); + } + else + { + for (int indexInBatch = 0; indexInBatch < countInBatch; ++indexInBatch) + { + var subtreeIndex = indexInBatch + batchStart; + var binIndex = ComputeBinIndex(centroidBoundsMin, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, boundingBoxes[subtreeIndex]); + var belongsToA = binIndex < context.BinSplitIndex; + slotBelongsToA[indexInBatch] = belongsToA ? -1 : 0; + if (belongsToA) ++localCountA; + } + //for (int indexInBatch = 0; indexInBatch < countInBatch; ++indexInBatch) + //{ + // var subtreeIndex = indexInBatch + batchStart; + // var binIndex = ComputeBinIndex(centroidBoundsMin, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, boundingBoxes[subtreeIndex]); + // slotBelongsToA[indexInBatch] = binIndex < context.BinSplitIndex; + //} + //for (int indexInBatch = 0; indexInBatch < countInBatch; ++indexInBatch) + //{ + // if (slotBelongsToA[indexInBatch]) ++localCountA; else ++localCountB; + //} } + localCountB = countInBatch - localCountA; var startIndexA = Interlocked.Add(ref context.Counters.SubtreeCountA, localCountA) - localCountA; var startIndexB = subtrees.Length - Interlocked.Add(ref context.Counters.SubtreeCountB, localCountB); @@ -813,7 +863,7 @@ unsafe static void PartitionSubtreesWorker(long taskId, void* untypedContext, in int recountB = 0; for (int indexInBatch = 0; indexInBatch < countInBatch; ++indexInBatch) { - var targetIndex = slotBelongsToA[indexInBatch] ? startIndexA + recountA++ : startIndexB + recountB++; + var targetIndex = slotBelongsToA[indexInBatch] != 0 ? startIndexA + recountA++ : startIndexB + recountB++; subtreesNext[targetIndex] = subtrees[batchStart + indexInBatch]; } diff --git a/BepuUtilities/BundleIndexing.cs b/BepuUtilities/BundleIndexing.cs index 8e3d442f4..a59c32b77 100644 --- a/BepuUtilities/BundleIndexing.cs +++ b/BepuUtilities/BundleIndexing.cs @@ -64,6 +64,7 @@ public static int GetBundleCount(int elementCount) [MethodImpl(MethodImplOptions.AggressiveInlining)] public static unsafe Vector CreateTrailingMaskForCountInBundle(int countInBundle) { + //TODO: Cross platform intrinsics rewrite if (Avx.IsSupported && Vector.Count == 8) { return Avx.CompareLessThanOrEqual(Vector256.Create((float)countInBundle), Vector256.Create(0f, 1f, 2f, 3f, 4f, 5f, 6f, 7f)).AsInt32().AsVector(); @@ -82,13 +83,13 @@ public static unsafe Vector CreateTrailingMaskForCountInBundle(int countInB } return mask; } - //TODO: ARM } [MethodImpl(MethodImplOptions.AggressiveInlining)] public static unsafe Vector CreateMaskForCountInBundle(int countInBundle) { + //TODO: Cross platform intrinsics rewrite if (Avx.IsSupported && Vector.Count == 8) { return Avx.CompareGreaterThan(Vector256.Create((float)countInBundle), Vector256.Create(0f, 1f, 2f, 3f, 4f, 5f, 6f, 7f)).AsInt32().AsVector(); @@ -107,13 +108,13 @@ public static unsafe Vector CreateMaskForCountInBundle(int countInBundle) } return mask; } - //TODO: ARM } [MethodImpl(MethodImplOptions.AggressiveInlining)] public static int GetFirstSetLaneIndex(Vector v) { + //TODO: Probable cross platform intrinsics rewrite if (Avx.IsSupported && Vector.Count == 8) { var scalarMask = Avx.MoveMask(v.AsVector256().As()); @@ -143,6 +144,7 @@ public static int GetFirstSetLaneIndex(Vector v) [MethodImpl(MethodImplOptions.AggressiveInlining)] public static int GetLastSetLaneCount(Vector v) { + //TODO: Cross platform intrinsics rewrite if (Avx.IsSupported && Vector.Count == 8) { var scalarMask = Avx.MoveMask(v.AsVector256().As()); From 6aa081d126c3659e6acc26f8c98b69315ce2fca6 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 21 Mar 2023 21:57:02 -0500 Subject: [PATCH 702/947] Backed out vectorized implementation; too severely bottlenecked on bandwidth for it to matter. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 71 +++++-------------------- 1 file changed, 12 insertions(+), 59 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 9f3b5213f..9deb31ebb 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -786,15 +786,9 @@ unsafe static void PartitionSubtreesWorker(long taskId, void* untypedContext, in //We don't really want to trigger interlocked operation for *every single subtree*, but we also don't want to allocate a bunch of memory. //Compromise! Stackalloc enough memory to cover sub-batches of the worker's subtrees, and do interlocked operations at the end of each batch. //Note that the main limit to the batch size is the amount of memory in L1 cache: the subtrees are 32 bytes per. - const int batchSize = 256; - //bool* slotBelongsToA = stackalloc bool[batchSize]; - int* slotBelongsToA = stackalloc int[batchSize]; - float* minimumsSpan = stackalloc float[Vector.Count]; - float* maximumsSpan = stackalloc float[Vector.Count]; - Vector offsetToBinIndexBundle = new Vector(context.AxisIndex == 0 ? context.OffsetToBinIndex.X : context.AxisIndex == 1 ? context.OffsetToBinIndex.Y : context.OffsetToBinIndex.Z); - Vector maximumBinIndexBundle = new Vector(context.MaximumBinIndex.X); - Vector centroidMinBundle = new Vector(context.AxisIndex == 0 ? context.CentroidBoundsMin.X : context.AxisIndex == 1 ? context.CentroidBoundsMin.Y : context.CentroidBoundsMin.Z); - Vector splitIndexBundle = new Vector(context.BinSplitIndex); + const int batchSize = 8192; + bool* slotBelongsToA = stackalloc bool[batchSize]; + var batchCount = (count + batchSize - 1) / batchSize; var boundingBoxes = context.Subtrees.As(); var subtrees = context.Subtrees; @@ -802,60 +796,19 @@ unsafe static void PartitionSubtreesWorker(long taskId, void* untypedContext, in for (int batchIndex = 0; batchIndex < batchCount; ++batchIndex) { var localCountA = 0; - int localCountB; var batchStart = start + batchIndex * batchSize; var countInBatch = int.Min(start + count - batchStart, batchSize); - if (Vector.IsSupported) - { - Vector localCountABundle = Vector.Zero; - var bundleCount = (countInBatch + Vector.Count - 1) / Vector.Count; - var maxOffset = 4 + axisIndex; - for (int bundleIndex = 0; bundleIndex < bundleCount; ++bundleIndex) - { - //Gather the bundle min/max scalars. - var countInBundle = int.Min(Vector.Count, countInBatch - bundleIndex * Vector.Count); - var boundsStartForBundle = boundingBoxes.Memory + batchStart + bundleIndex * Vector.Count; - for (int i = 0; i < countInBundle; ++i) - { - var index = bundleIndex * Vector.Count + i; - float* boundsForLane = (float*)(boundsStartForBundle + i); - minimumsSpan[i] = boundsForLane[axisIndex]; - maximumsSpan[i] = boundsForLane[maxOffset]; - } - var minimums = new Vector(new Span(minimumsSpan, Vector.Count)); - var maximums = new Vector(new Span(maximumsSpan, Vector.Count)); - //Now compute the actual bin indices and accumulate the local count bundles. - var binIndicesBundle = Vector.Min((minimums + maximums - centroidMinBundle) * offsetToBinIndexBundle, maximumBinIndexBundle); - var bundleLaneMask = BundleIndexing.CreateMaskForCountInBundle(countInBundle); - var belongsToA = Vector.BitwiseAnd(bundleLaneMask, Vector.LessThan(binIndicesBundle, splitIndexBundle)); - ((Vector*)slotBelongsToA)[bundleIndex] = belongsToA; - localCountABundle += Vector.ConditionalSelect(belongsToA, Vector.One, Vector.Zero); - } - localCountA = Vector.Sum(localCountABundle); - } - else + for (int indexInBatch = 0; indexInBatch < countInBatch; ++indexInBatch) { - for (int indexInBatch = 0; indexInBatch < countInBatch; ++indexInBatch) - { - var subtreeIndex = indexInBatch + batchStart; - var binIndex = ComputeBinIndex(centroidBoundsMin, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, boundingBoxes[subtreeIndex]); - var belongsToA = binIndex < context.BinSplitIndex; - slotBelongsToA[indexInBatch] = belongsToA ? -1 : 0; - if (belongsToA) ++localCountA; - } - //for (int indexInBatch = 0; indexInBatch < countInBatch; ++indexInBatch) - //{ - // var subtreeIndex = indexInBatch + batchStart; - // var binIndex = ComputeBinIndex(centroidBoundsMin, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, boundingBoxes[subtreeIndex]); - // slotBelongsToA[indexInBatch] = binIndex < context.BinSplitIndex; - //} - //for (int indexInBatch = 0; indexInBatch < countInBatch; ++indexInBatch) - //{ - // if (slotBelongsToA[indexInBatch]) ++localCountA; else ++localCountB; - //} + var subtreeIndex = indexInBatch + batchStart; + var binIndex = ComputeBinIndex(centroidBoundsMin, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, boundingBoxes[subtreeIndex]); + var belongsToA = binIndex < context.BinSplitIndex; + slotBelongsToA[indexInBatch] = belongsToA; + if (belongsToA) ++localCountA; } - localCountB = countInBatch - localCountA; + + var localCountB = countInBatch - localCountA; var startIndexA = Interlocked.Add(ref context.Counters.SubtreeCountA, localCountA) - localCountA; var startIndexB = subtrees.Length - Interlocked.Add(ref context.Counters.SubtreeCountB, localCountB); @@ -863,7 +816,7 @@ unsafe static void PartitionSubtreesWorker(long taskId, void* untypedContext, in int recountB = 0; for (int indexInBatch = 0; indexInBatch < countInBatch; ++indexInBatch) { - var targetIndex = slotBelongsToA[indexInBatch] != 0 ? startIndexA + recountA++ : startIndexB + recountB++; + var targetIndex = slotBelongsToA[indexInBatch] ? startIndexA + recountA++ : startIndexB + recountB++; subtreesNext[targetIndex] = subtrees[batchStart + indexInBatch]; } From 85722cd1baf82cdc3692f36e073fed3f7f50f782 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 21 Mar 2023 22:00:47 -0500 Subject: [PATCH 703/947] Batch size tuning. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 9deb31ebb..1322f6c45 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -785,8 +785,8 @@ unsafe static void PartitionSubtreesWorker(long taskId, void* untypedContext, in context.TaskData.GetSlotInterval(taskId, out var start, out var count); //We don't really want to trigger interlocked operation for *every single subtree*, but we also don't want to allocate a bunch of memory. //Compromise! Stackalloc enough memory to cover sub-batches of the worker's subtrees, and do interlocked operations at the end of each batch. - //Note that the main limit to the batch size is the amount of memory in L1 cache: the subtrees are 32 bytes per. - const int batchSize = 8192; + //Note that the main limit to the batch size is the amount of memory in cache. + const int batchSize = 16384; bool* slotBelongsToA = stackalloc bool[batchSize]; var batchCount = (count + batchSize - 1) / batchSize; From d77ee1a33268c4439b8d1d62b5dd873724833ced Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 23 Mar 2023 20:18:46 -0500 Subject: [PATCH 704/947] Binning phase now computes child node centroid bounds. Centroid prepass only runs for root. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 190 ++++++++++++++++-------- 1 file changed, 128 insertions(+), 62 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 1322f6c45..b3fbda897 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -90,7 +90,9 @@ internal static float ComputeBoundsMetric(Vector4 min, Vector4 max) interface IBinnedBuilderThreading { - void GetBins(int workerIndex, out Buffer binBoundingBoxes, out Buffer binBoundingBoxesScan, out Buffer binLeafCounts); + void GetBins(int workerIndex, + out Buffer binBoundingBoxes, out Buffer binCentroidBoundingBoxes, + out Buffer binBoundingBoxesScan, out Buffer binCentroidBoundingBoxesScan, out Buffer binLeafCounts); } @@ -162,7 +164,7 @@ static unsafe void MicroSweepForBinnedBuilder( return; } var centroidSpan = centroidMax - centroidMin; - context->Threading.GetBins(workerIndex, out var binBoundingBoxes, out var binBoundingBoxesScan, out var binLeafCounts); + context->Threading.GetBins(workerIndex, out var binBoundingBoxes, out var binCentroidBoundingBoxes, out var binBoundingBoxesScan, out var binCentroidBoundingBoxesScan, out var binLeafCounts); if (Vector256.IsHardwareAccelerated || Vector128.IsHardwareAccelerated) { @@ -339,13 +341,29 @@ private static unsafe int ComputeBinIndex(Vector4 centroidMin, bool useX, bool u struct SingleThreaded : IBinnedBuilderThreading { public Buffer BinBoundingBoxes; + public Buffer BinCentroidBoundingBoxes; public Buffer BinBoundingBoxesScan; + public Buffer BinCentroidBoundingBoxesScan; public Buffer BinLeafCounts; - public void GetBins(int workerIndex, out Buffer binBoundingBoxes, out Buffer binBoundingBoxesScan, out Buffer binLeafCounts) + public SingleThreaded(Buffer binAllocationBuffer, int binCapacity) + { + int start = 0; + BinBoundingBoxes = Suballocate(binAllocationBuffer, ref start, binCapacity); + BinCentroidBoundingBoxes = Suballocate(binAllocationBuffer, ref start, binCapacity); + BinBoundingBoxesScan = Suballocate(binAllocationBuffer, ref start, binCapacity); + BinCentroidBoundingBoxesScan = Suballocate(binAllocationBuffer, ref start, binCapacity); + BinLeafCounts = Suballocate(binAllocationBuffer, ref start, binCapacity); + } + + public void GetBins(int workerIndex, + out Buffer binBoundingBoxes, out Buffer binCentroidBoundingBoxes, + out Buffer binBoundingBoxesScan, out Buffer binCentroidBoundingBoxesScan, out Buffer binLeafCounts) { binBoundingBoxes = BinBoundingBoxes; + binCentroidBoundingBoxes = BinCentroidBoundingBoxes; binBoundingBoxesScan = BinBoundingBoxesScan; + binCentroidBoundingBoxesScan = BinCentroidBoundingBoxesScan; binLeafCounts = BinLeafCounts; } } @@ -361,31 +379,39 @@ static Buffer Suballocate(Buffer buffer, ref int start, int count) w /// /// Stores resources required by a worker to dispatch and manage multithreaded work. /// + /// + /// Some of the resources cached here are technically redundant with the storage used for workers and ends up involving an extra bin scan on a multithreaded test, + /// but the cost associated with doing so is... low. The complexity cost of trying to use the memory allocated for workers is not low. + /// unsafe struct BinnedBuildWorkerContext { /// /// Bins associated with this worker for the duration of a node. This allocation will persist across the build. - /// - /// This is technically redundant with the storage used for workers and ends up involving an extra bin scan on a multithreaded test, - /// but the cost associated with doing so is... low. The complexity cost of trying to use the memory allocated for workers is not low. /// public Buffer BinBoundingBoxes; /// + /// Centroid bound bins associated with this worker for the duration of a node. This allocation will persist across the build. + /// + public Buffer BinCentroidBoundingBoxes; + /// /// Bins associated with this worker for use in the SAH scan. This allocation will persist across the build. /// public Buffer BinBoundingBoxesScan; /// + /// Centroid bound bins associated with this worker for use in the SAH scan. This allocation will persist across the build. + /// + public Buffer BinCentroidBoundingBoxesScan; + /// /// Bin leaf counts associated with this worker for the duration of a node. This allocation will persist across the build. - /// - /// This is technically redundant with the storage used for workers and ends up involving an extra bin scan on a multithreaded test, - /// but the cost associated with doing so is... low. The complexity cost of trying to use the memory allocated for workers is not low. /// public Buffer BinLeafCounts; public BinnedBuildWorkerContext(Buffer binAllocationBuffer, ref int binStart, int binCapacity) { BinBoundingBoxes = Suballocate(binAllocationBuffer, ref binStart, binCapacity); + BinCentroidBoundingBoxes = Suballocate(binAllocationBuffer, ref binStart, binCapacity); BinBoundingBoxesScan = Suballocate(binAllocationBuffer, ref binStart, binCapacity); + BinCentroidBoundingBoxesScan = Suballocate(binAllocationBuffer, ref binStart, binCapacity); BinLeafCounts = Suballocate(binAllocationBuffer, ref binStart, binCapacity); } } @@ -402,11 +428,15 @@ unsafe struct MultithreadBinnedBuildContext : IBinnedBuilderThreading public int TopLevelTargetTaskCount; public Buffer Workers; - public void GetBins(int workerIndex, out Buffer binBoundingBoxes, out Buffer binBoundingBoxesScan, out Buffer binLeafCounts) + public void GetBins(int workerIndex, + out Buffer binBoundingBoxes, out Buffer binCentroidBoundingBoxes, + out Buffer binBoundingBoxesScan, out Buffer binCentroidBoundingBoxesScan, out Buffer binLeafCounts) { ref var worker = ref Workers[workerIndex]; binBoundingBoxes = worker.BinBoundingBoxes; + binCentroidBoundingBoxes = worker.BinCentroidBoundingBoxes; binBoundingBoxesScan = worker.BinBoundingBoxesScan; + binCentroidBoundingBoxesScan = worker.BinCentroidBoundingBoxesScan; binLeafCounts = worker.BinLeafCounts; } @@ -553,6 +583,7 @@ unsafe static BoundingBox4 MultithreadedCentroidPrepass(MultithreadBinnedBuildCo struct BinSubtreesWorkerContext { public Buffer BinBoundingBoxes; + public Buffer BinCentroidBoundingBoxes; public Buffer BinLeafCounts; } unsafe struct BinSubtreesTaskContext @@ -600,7 +631,7 @@ public BinSubtreesTaskContext(BufferPool pool, SharedTaskData taskData, Buffer(allocationSize, out var allocation); int start = 0; BinSubtreesWorkers = Suballocate(allocation, ref start, effectiveWorkerCount); @@ -608,6 +639,7 @@ public BinSubtreesTaskContext(BufferPool pool, SharedTaskData taskData, Buffer(allocation, ref start, BinCount); + worker.BinCentroidBoundingBoxes = Suballocate(allocation, ref start, BinCount); worker.BinLeafCounts = Suballocate(allocation, ref start, BinCount); } WorkerHelpedWithBinning = Suballocate(allocation, ref start, effectiveWorkerCount); @@ -619,7 +651,7 @@ public BinSubtreesTaskContext(BufferPool pool, SharedTaskData taskData, Buffer permuteMask, int axisIndex, Vector4 offsetToBinIndex, Vector4 maximumBinIndex, - Buffer subtrees, Buffer binBoundingBoxes, Buffer binLeafCounts) + Buffer subtrees, Buffer binBoundingBoxes, Buffer binCentroidBoundingBoxes, Buffer binLeafCounts) { //Note that we don't store out any of the indices into per-bin lists here. We only *really* want two final groups for the children, //and we can easily compute those by performing another scan. It requires recomputing the bin indices, but that's really not much of a concern. @@ -631,6 +663,12 @@ static void BinSubtrees(Vector4 centroidBoundsMin, ref var binBounds = ref binBoundingBoxes[binIndex]; binBounds.Min = Vector4.Min(binBounds.Min, box.Min); binBounds.Max = Vector4.Max(binBounds.Max, box.Max); + //The binning phase also keeps track of *centroid* bounding boxes so that we don't have to do a dedicated centroid prepass for each node. + //(A centroid prepass would require touching every single subtree again, and, for large trees, that's a lot of uncached (or distant) memory accesses.) + var centroid = box.Min + box.Max; + ref var binCentroidBounds = ref binCentroidBoundingBoxes[binIndex]; + binCentroidBounds.Min = Vector4.Min(binCentroidBounds.Min, centroid); + binCentroidBounds.Max = Vector4.Max(binCentroidBounds.Max, centroid); binLeafCounts[binIndex] += subtree.LeafCount; } } @@ -649,12 +687,15 @@ unsafe static void BinSubtreesWorker(long taskId, void* untypedContext, int work ref var binBounds = ref worker.BinBoundingBoxes[i]; binBounds.Min = new Vector4(float.MaxValue); binBounds.Max = new Vector4(float.MinValue); + ref var binCentroidBounds = ref worker.BinCentroidBoundingBoxes[i]; + binCentroidBounds.Min = new Vector4(float.MaxValue); + binCentroidBounds.Max = new Vector4(float.MinValue); worker.BinLeafCounts[i] = 0; } } context.TaskData.GetSlotInterval(taskId, out var start, out var count); BinSubtrees(context.CentroidBoundsMin, context.UseX, context.UseY, context.PermuteMask, context.AxisIndex, context.OffsetToBinIndex, context.MaximumBinIndex, - context.Subtrees.Slice(start, count), worker.BinBoundingBoxes, worker.BinLeafCounts); + context.Subtrees.Slice(start, count), worker.BinBoundingBoxes, worker.BinCentroidBoundingBoxes, worker.BinLeafCounts); } unsafe static void MultithreadedBinSubtrees(MultithreadBinnedBuildContext* context, @@ -682,6 +723,9 @@ unsafe static void MultithreadedBinSubtrees(MultithreadBinnedBuildContext* conte ref var binBounds = ref cache.BinBoundingBoxes[i]; binBounds.Min = new Vector4(float.MaxValue); binBounds.Max = new Vector4(float.MinValue); + ref var binCentroidBounds = ref cache.BinCentroidBoundingBoxes[i]; + binCentroidBounds.Min = new Vector4(float.MaxValue); + binCentroidBounds.Max = new Vector4(float.MinValue); cache.BinLeafCounts[i] = 0; } } @@ -697,6 +741,7 @@ unsafe static void MultithreadedBinSubtrees(MultithreadBinnedBuildContext* conte //That's not an irrelevant case, either. *Most* nodes will be too small to warrant internal multithreading.) ref var cache0 = ref taskContext.BinSubtreesWorkers[0]; cache0.BinBoundingBoxes.CopyTo(0, worker.BinBoundingBoxes, 0, cache0.BinBoundingBoxes.Length); + cache0.BinCentroidBoundingBoxes.CopyTo(0, worker.BinCentroidBoundingBoxes, 0, cache0.BinCentroidBoundingBoxes.Length); cache0.BinLeafCounts.CopyTo(0, worker.BinLeafCounts, 0, cache0.BinLeafCounts.Length); for (int cacheIndex = 1; cacheIndex < activeWorkerCount; ++cacheIndex) { @@ -710,6 +755,10 @@ unsafe static void MultithreadedBinSubtrees(MultithreadBinnedBuildContext* conte ref var bi = ref cache.BinBoundingBoxes[binIndex]; b0.Min = Vector4.Min(b0.Min, bi.Min); b0.Max = Vector4.Max(b0.Max, bi.Max); + ref var bc0 = ref worker.BinCentroidBoundingBoxes[binIndex]; + ref var bci = ref cache.BinCentroidBoundingBoxes[binIndex]; + bc0.Min = Vector4.Min(bc0.Min, bci.Min); + bc0.Max = Vector4.Max(bc0.Max, bci.Max); worker.BinLeafCounts[binIndex] += cache.BinLeafCounts[binIndex]; } } @@ -843,6 +892,7 @@ unsafe struct NodePushTaskContext public Context* Context; public int NodeIndex; public int ParentNodeIndex; + public BoundingBox4 CentroidBounds; //Subtree region start index, subtree count, and usePongBuffer status are all encoded into the task id. } unsafe static void BinnedBuilderNodeWorker(long taskId, void* context, int workerIndex, IThreadDispatcher dispatcher) @@ -853,11 +903,12 @@ unsafe static void BinnedBuilderNodeWorker(long taskId, voi var usePongBuffer = (ulong)taskId >= (1UL << 63); var nodePushContext = (NodePushTaskContext*)context; //Note that child index is always 1 because we only ever push child B. - BinnedBuildNode(usePongBuffer, subtreeRegionStartIndex, nodePushContext->NodeIndex, subtreeCount, nodePushContext->ParentNodeIndex, 1, nodePushContext->Context, workerIndex, dispatcher); + BinnedBuildNode(usePongBuffer, subtreeRegionStartIndex, nodePushContext->NodeIndex, subtreeCount, nodePushContext->ParentNodeIndex, 1, nodePushContext->CentroidBounds, nodePushContext->Context, workerIndex, dispatcher); } static unsafe void BinnedBuildNode( - bool usePongBuffer, int subtreeRegionStartIndex, int nodeIndex, int subtreeCount, int parentNodeIndex, int childIndexInParent, Context* context, int workerIndex, IThreadDispatcher dispatcher) + bool usePongBuffer, int subtreeRegionStartIndex, int nodeIndex, int subtreeCount, int parentNodeIndex, int childIndexInParent, + BoundingBox4 centroidBounds, Context* context, int workerIndex, IThreadDispatcher dispatcher) where TLeaves : unmanaged where TThreading : unmanaged, IBinnedBuilderThreading { //var debugStartTime = Stopwatch.GetTimestamp(); @@ -877,17 +928,20 @@ static unsafe void BinnedBuildNode( var targetTaskCount = typeof(TThreading) == typeof(SingleThreaded) ? 1 : ((MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As(ref context->Threading)))->GetTargetTaskCount(subtreeCount); //var debugCentroidStartTime = Stopwatch.GetTimestamp(); - BoundingBox4 centroidBounds; - if (targetTaskCount == 1 || subtreeCount < MinimumSubtreesPerThreadForCentroidPrepass) + if (nodeIndex == 0) { - centroidBounds = ComputeCentroidBounds(boundingBoxes); - //debugTimes.MTPrepass = false; - } - else - { - centroidBounds = MultithreadedCentroidPrepass( - (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As(ref context->Threading)), subtrees, targetTaskCount, workerIndex, dispatcher); - //debugTimes.MTPrepass = true; + //The first node doesn't have a parent, and so isn't given centroid bounds. We have to compute them. + if (targetTaskCount == 1 || subtreeCount < MinimumSubtreesPerThreadForCentroidPrepass) + { + centroidBounds = ComputeCentroidBounds(boundingBoxes); + //debugTimes.MTPrepass = false; + } + else + { + centroidBounds = MultithreadedCentroidPrepass( + (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As(ref context->Threading)), subtrees, targetTaskCount, workerIndex, dispatcher); + //debugTimes.MTPrepass = true; + } } //var debugCentroidEndTime = Stopwatch.GetTimestamp(); var centroidSpan = centroidBounds.Max - centroidBounds.Min; @@ -921,9 +975,9 @@ static unsafe void BinnedBuildNode( } BuildNode(boundsA, boundsB, degenerateLeafCountA, degenerateLeafCountB, subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, degenerateSubtreeCountA, degenerateSubtreeCountB, ref context->Leaves, out var aIndex, out var bIndex); if (degenerateSubtreeCountA > 1) - BinnedBuildNode(usePongBuffer, subtreeRegionStartIndex, aIndex, degenerateSubtreeCountA, nodeIndex, 0, context, workerIndex, dispatcher); + BinnedBuildNode(usePongBuffer, subtreeRegionStartIndex, aIndex, degenerateSubtreeCountA, nodeIndex, 0, boundsA, context, workerIndex, dispatcher); if (degenerateSubtreeCountB > 1) - BinnedBuildNode(usePongBuffer, subtreeRegionStartIndex + degenerateSubtreeCountA, bIndex, degenerateSubtreeCountB, nodeIndex, 1, context, workerIndex, dispatcher); + BinnedBuildNode(usePongBuffer, subtreeRegionStartIndex + degenerateSubtreeCountA, bIndex, degenerateSubtreeCountB, nodeIndex, 1, boundsB, context, workerIndex, dispatcher); return; } @@ -947,19 +1001,22 @@ static unsafe void BinnedBuildNode( offsetToBinIndex = Vector128.ConditionalSelect(axisIsDegenerate, Vector128.Zero, offsetToBinIndex.AsVector128()).AsVector4(); var maximumBinIndex = new Vector4(binCount - 1); - context->Threading.GetBins(workerIndex, out var binBoundingBoxes, out var binBoundingBoxesScan, out var binLeafCounts); + context->Threading.GetBins(workerIndex, out var binBoundingBoxes, out var binCentroidBoundingBoxes, out var binBoundingBoxesScan, out var binCentroidBoundingBoxesScan, out var binLeafCounts); Debug.Assert(binBoundingBoxes.Length >= binCount); for (int i = 0; i < binCount; ++i) { - ref var boxX = ref binBoundingBoxes[i]; - boxX.Min = new Vector4(float.MaxValue); - boxX.Max = new Vector4(float.MinValue); + ref var binBounds = ref binBoundingBoxes[i]; + binBounds.Min = new Vector4(float.MaxValue); + binBounds.Max = new Vector4(float.MinValue); + ref var binCentroidBounds = ref binCentroidBoundingBoxes[i]; + binCentroidBounds.Min = new Vector4(float.MaxValue); + binCentroidBounds.Max = new Vector4(float.MinValue); binLeafCounts[i] = 0; } //var debugBinStartTime = Stopwatch.GetTimestamp(); if (targetTaskCount == 1 || subtreeCount < MinimumSubtreesPerThreadForBinning) { - BinSubtrees(centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, subtrees, binBoundingBoxes, binLeafCounts); + BinSubtrees(centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, subtrees, binBoundingBoxes, binCentroidBoundingBoxes, binLeafCounts); //debugTimes.MTBinning = false; } else @@ -974,15 +1031,21 @@ static unsafe void BinnedBuildNode( //Identify the split index by examining the SAH of very split option. //Premerge from left to right so we have a sorta-summed area table to cheaply look up all possible child A bounds as we scan. binBoundingBoxesScan[0] = binBoundingBoxes[0]; + binCentroidBoundingBoxesScan[0] = binCentroidBoundingBoxes[0]; int totalLeafCount = binLeafCounts[0]; for (int i = 1; i < binCount; ++i) { var previousIndex = i - 1; - ref var xBounds = ref binBoundingBoxes[i]; - ref var xScanBounds = ref binBoundingBoxesScan[i]; - ref var xPreviousScanBounds = ref binBoundingBoxesScan[previousIndex]; - xScanBounds.Min = Vector4.Min(xBounds.Min, xPreviousScanBounds.Min); - xScanBounds.Max = Vector4.Max(xBounds.Max, xPreviousScanBounds.Max); + ref var bounds = ref binBoundingBoxes[i]; + ref var scanBounds = ref binBoundingBoxesScan[i]; + ref var previousScanBounds = ref binBoundingBoxesScan[previousIndex]; + scanBounds.Min = Vector4.Min(bounds.Min, previousScanBounds.Min); + scanBounds.Max = Vector4.Max(bounds.Max, previousScanBounds.Max); + ref var binCentroidBoundingBox = ref binCentroidBoundingBoxes[i]; + ref var binCentroidBoundingBoxScan = ref binCentroidBoundingBoxesScan[i]; + ref var previousCentroidBoundingBoxScan = ref binCentroidBoundingBoxesScan[previousIndex]; + binCentroidBoundingBoxScan.Min = Vector4.Min(binCentroidBoundingBox.Min, previousCentroidBoundingBoxScan.Min); + binCentroidBoundingBoxScan.Max = Vector4.Max(binCentroidBoundingBox.Max, previousCentroidBoundingBoxScan.Max); totalLeafCount += binLeafCounts[i]; } var leftBoundsX = binBoundingBoxes[0]; @@ -991,13 +1054,14 @@ static unsafe void BinnedBuildNode( "Bin 0 should have been updated in all cases because it is aligned with the minimum bin, and the centroid span isn't degenerate."); float bestSAH = float.MaxValue; - int bestSplit = 1; + int splitIndex = 1; //The split index is going to end up in child B. var lastBinIndex = binCount - 1; - BoundingBox4 accumulatedBoundingBoxB; - accumulatedBoundingBoxB = binBoundingBoxes[lastBinIndex]; - BoundingBox4 bestBoundingBoxB; - bestBoundingBoxB = binBoundingBoxes[lastBinIndex]; + var accumulatedBoundingBoxB = binBoundingBoxes[lastBinIndex]; + var accumulatedCentroidBoundingBoxB = binCentroidBoundingBoxes[lastBinIndex]; + BoundingBox4 bestBoundingBoxB, bestCentroidBoundingBoxB; + bestBoundingBoxB = accumulatedBoundingBoxB; + bestCentroidBoundingBoxB = accumulatedCentroidBoundingBoxB; int accumulatedLeafCountB = binLeafCounts[lastBinIndex]; int bestLeafCountB = 0; for (int splitIndexCandidate = lastBinIndex; splitIndexCandidate >= 1; --splitIndexCandidate) @@ -1008,22 +1072,25 @@ static unsafe void BinnedBuildNode( if (sahCandidate < bestSAH) { bestSAH = sahCandidate; - bestSplit = splitIndexCandidate; + splitIndex = splitIndexCandidate; bestBoundingBoxB = accumulatedBoundingBoxB; bestLeafCountB = accumulatedLeafCountB; + bestCentroidBoundingBoxB = accumulatedCentroidBoundingBoxB; } - ref var xBounds = ref binBoundingBoxes[previousIndex]; - accumulatedBoundingBoxB.Min = Vector4.Min(xBounds.Min, accumulatedBoundingBoxB.Min); - accumulatedBoundingBoxB.Max = Vector4.Max(xBounds.Max, accumulatedBoundingBoxB.Max); + ref var bounds = ref binBoundingBoxes[previousIndex]; + accumulatedBoundingBoxB.Min = Vector4.Min(bounds.Min, accumulatedBoundingBoxB.Min); + accumulatedBoundingBoxB.Max = Vector4.Max(bounds.Max, accumulatedBoundingBoxB.Max); + ref var centroidBoundsForBin = ref binCentroidBoundingBoxes[previousIndex]; + accumulatedCentroidBoundingBoxB.Min = Vector4.Min(centroidBoundsForBin.Min, accumulatedCentroidBoundingBoxB.Min); + accumulatedCentroidBoundingBoxB.Max = Vector4.Max(centroidBoundsForBin.Max, accumulatedCentroidBoundingBoxB.Max); accumulatedLeafCountB += binLeafCounts[previousIndex]; } //var debugHuhStartTime = Stopwatch.GetTimestamp(); var subtreeCountB = 0; var subtreeCountA = 0; - var splitIndex = bestSplit; - var bestBoundsA = binBoundingBoxesScan[bestSplit - 1]; - var bestBoundsB = bestBoundingBoxB; + var bestBoundingBoxA = binBoundingBoxesScan[splitIndex - 1]; + var bestCentroidBoundingBoxA = binCentroidBoundingBoxesScan[splitIndex - 1]; //Split the indices/bounds into two halves for the children to operate on. if (context->SubtreesPong.Allocated) @@ -1089,7 +1156,7 @@ static unsafe void BinnedBuildNode( var leafCountB = bestLeafCountB; var leafCountA = totalLeafCount - leafCountB; Debug.Assert(subtreeCountA + subtreeCountB == subtreeCount); - BuildNode(bestBoundsA, bestBoundsB, leafCountA, leafCountB, subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, subtreeCountA, subtreeCountB, ref context->Leaves, out var nodeChildIndexA, out var nodeChildIndexB); + BuildNode(bestBoundingBoxA, bestBoundingBoxB, leafCountA, leafCountB, subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, subtreeCountA, subtreeCountB, ref context->Leaves, out var nodeChildIndexA, out var nodeChildIndexB); //var debugEndTime = Stopwatch.GetTimestamp(); //debugTimes.Total = (debugEndTime - debugStartTime) / (double)Stopwatch.Frequency; //debugTimes.Partition = (debugHuhEndTime - debugHuhStartTime) / (double)Stopwatch.Frequency; @@ -1109,15 +1176,16 @@ static unsafe void BinnedBuildNode( nodePushContext.Context = context; nodePushContext.NodeIndex = nodeChildIndexB; nodePushContext.ParentNodeIndex = nodeIndex; + nodePushContext.CentroidBounds = bestCentroidBoundingBoxB; //Note that we use the task id to store subtree start, subtree count, and the pong buffer flag. Don't have to do that, but no reason not to use it. Debug.Assert((uint)subtreeCountB < (1u << 31), "The task id encodes start, count, and a pong flag, so we don't have room for a full 32 bits of count."); var task = new Task(&BinnedBuilderNodeWorker, &nodePushContext, (long)(subtreeRegionStartIndex + subtreeCountA) | ((long)subtreeCountB << 32) | (usePongBuffer ? 1L << 63 : 0)); nodeBContinuation = threading.TaskStack->AllocateContinuationAndPush(new Span(&task, 1), workerIndex, dispatcher); } if (subtreeCountA > 1) - BinnedBuildNode(usePongBuffer, subtreeRegionStartIndex, nodeChildIndexA, subtreeCountA, nodeIndex, 0, context, workerIndex, dispatcher); + BinnedBuildNode(usePongBuffer, subtreeRegionStartIndex, nodeChildIndexA, subtreeCountA, nodeIndex, 0, bestCentroidBoundingBoxA, context, workerIndex, dispatcher); if (!shouldPushBOntoMultithreadedQueue && subtreeCountB > 1) - BinnedBuildNode(usePongBuffer, subtreeRegionStartIndex + subtreeCountA, nodeChildIndexB, subtreeCountB, nodeIndex, 1, context, workerIndex, dispatcher); + BinnedBuildNode(usePongBuffer, subtreeRegionStartIndex + subtreeCountA, nodeChildIndexB, subtreeCountB, nodeIndex, 1, bestCentroidBoundingBoxB, context, workerIndex, dispatcher); if (shouldPushBOntoMultithreadedQueue) { //We want to keep the stack at this level alive until the memory we allocated for the node push completes. @@ -1177,19 +1245,17 @@ static unsafe void BinnedBuilderInternal(Buffer subtrees, Buffer(binBoundsMemoryAllocation, allocatedByteCount); - var binLeafCountsMemory = stackalloc int[allocatedBinCount]; - SingleThreaded threading; - threading.BinBoundingBoxes = new Buffer(binBoundsMemory, allocatedBinCount); - threading.BinBoundingBoxesScan = new Buffer(binBoundsMemory + allocatedBinCount, allocatedBinCount); - threading.BinLeafCounts = new Buffer(binLeafCountsMemory, allocatedBinCount); + var threading = new SingleThreaded(binBoundsMemory, allocatedBinCount); var context = new Context, SingleThreaded>( minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold, subtrees, default, leaves, nodes, metanodes, threading); - BinnedBuildNode(false, 0, 0, subtreeCount, -1, -1, &context, 0, null); + BinnedBuildNode(false, 0, 0, subtreeCount, -1, -1, default, &context, 0, null); } else { @@ -1202,7 +1268,7 @@ static unsafe void BinnedBuilderInternal(Buffer subtrees, Buffer(allocatedBinCount * workerCount * (sizeof(BoundingBox4) * 2 + sizeof(int)), out var workerBinsAllocation); + pool.Take(allocatedBinCount * workerCount * (sizeof(BoundingBox4) * 4 + sizeof(int)), out var workerBinsAllocation); BinnedBuildWorkerContext* workerContextsPointer = stackalloc BinnedBuildWorkerContext[workerCount]; var workerContexts = new Buffer(workerContextsPointer, workerCount); @@ -1250,7 +1316,7 @@ unsafe static void BinnedBuilderWorkerEntry(long taskId, void* untypedC where TLeaves : unmanaged { var context = (Context*)untypedContext; - BinnedBuildNode(false, 0, 0, context->SubtreesPing.Length, -1, -1, context, workerIndex, dispatcher); + BinnedBuildNode(false, 0, 0, context->SubtreesPing.Length, -1, -1, default, context, workerIndex, dispatcher); //Once the entry point returns, all workers should stop because it won't return unless both nodes are done. context->Threading.TaskStack->RequestStop(); } From 732dbe85b4568c57b1ba8fd70f877e7da7542acd Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 24 Mar 2023 20:54:40 -0500 Subject: [PATCH 705/947] Binned builder now uses binned indices computed from binning pass. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 159 +++++++++++++++--------- 1 file changed, 103 insertions(+), 56 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index b3fbda897..1e7e1f5de 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -111,10 +111,12 @@ struct Context public Buffer Nodes; public Buffer Metanodes; + public Buffer BinIndices; + public TThreading Threading; public Context(int minimumBinCount, int maximumBinCount, float leafToBinMultiplier, int microsweepThreshold, - Buffer subtreesPing, Buffer subtreesPong, TLeaves leaves, Buffer nodes, Buffer metanodes, TThreading threading) + Buffer subtreesPing, Buffer subtreesPong, TLeaves leaves, Buffer nodes, Buffer metanodes, Buffer binIndices, TThreading threading) { MinimumBinCount = minimumBinCount; MaximumBinCount = maximumBinCount; @@ -122,6 +124,7 @@ public Context(int minimumBinCount, int maximumBinCount, float leafToBinMultipli MicrosweepThreshold = microsweepThreshold; SubtreesPing = subtreesPing; SubtreesPong = subtreesPong; + BinIndices = binIndices; Leaves = leaves; Nodes = nodes; Metanodes = metanodes; @@ -607,6 +610,11 @@ unsafe struct BinSubtreesTaskContext /// public Buffer Subtrees; + /// + /// Stores the bin indices of all subtrees in the node. + /// + public Buffer BinIndices; + public int BinCount; public bool UseX, UseY; public Vector128 PermuteMask; @@ -615,12 +623,13 @@ unsafe struct BinSubtreesTaskContext public Vector4 OffsetToBinIndex; public Vector4 MaximumBinIndex; - public BinSubtreesTaskContext(BufferPool pool, SharedTaskData taskData, Buffer subtrees, + public BinSubtreesTaskContext(BufferPool pool, SharedTaskData taskData, Buffer subtrees, Buffer binIndices, int binCount, bool useX, bool useY, Vector128 permuteMask, int axisIndex, Vector4 centroidBoundsMin, Vector4 offsetToBinIndex, Vector4 maximumBinIndex) { TaskData = taskData; Subtrees = subtrees; + BinIndices = binIndices; BinCount = binCount; UseX = useX; UseY = useY; @@ -647,11 +656,21 @@ public BinSubtreesTaskContext(BufferPool pool, SharedTaskData taskData, Buffer pool.Return(ref BinSubtreesWorkers); //Only need to return the main buffer because all the other allocations share the same id! } - + //these type-level booleans let the compiler avoid branching in the binning loop. The bin indices buffer is not guaranteed to exist. + //i apologize + /// + /// Marks a call as requiring the bin indices to be written to the binIndices buffer. + /// + private struct DoWriteBinIndices { } + /// + /// Marks a call as not allowing the bin indices to be written to the binIndices buffer. + /// + private struct DoNotWriteBinIndices { } [MethodImpl(MethodImplOptions.AggressiveInlining)] - static void BinSubtrees(Vector4 centroidBoundsMin, + static void BinSubtrees(Vector4 centroidBoundsMin, bool useX, bool useY, Vector128 permuteMask, int axisIndex, Vector4 offsetToBinIndex, Vector4 maximumBinIndex, - Buffer subtrees, Buffer binBoundingBoxes, Buffer binCentroidBoundingBoxes, Buffer binLeafCounts) + Buffer subtrees, Buffer binBoundingBoxes, Buffer binCentroidBoundingBoxes, Buffer binLeafCounts, Buffer binIndices) + where TShouldWriteBinIndices : unmanaged { //Note that we don't store out any of the indices into per-bin lists here. We only *really* want two final groups for the children, //and we can easily compute those by performing another scan. It requires recomputing the bin indices, but that's really not much of a concern. @@ -660,6 +679,8 @@ static void BinSubtrees(Vector4 centroidBoundsMin, ref var subtree = ref subtrees[i]; ref var box = ref Unsafe.As(ref subtree); var binIndex = ComputeBinIndex(centroidBoundsMin, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, box); + if (typeof(TShouldWriteBinIndices) == typeof(DoWriteBinIndices)) + binIndices[i] = (byte)binIndex; ref var binBounds = ref binBoundingBoxes[binIndex]; binBounds.Min = Vector4.Min(binBounds.Min, box.Min); binBounds.Max = Vector4.Max(binBounds.Max, box.Max); @@ -694,20 +715,22 @@ unsafe static void BinSubtreesWorker(long taskId, void* untypedContext, int work } } context.TaskData.GetSlotInterval(taskId, out var start, out var count); - BinSubtrees(context.CentroidBoundsMin, context.UseX, context.UseY, context.PermuteMask, context.AxisIndex, context.OffsetToBinIndex, context.MaximumBinIndex, - context.Subtrees.Slice(start, count), worker.BinBoundingBoxes, worker.BinCentroidBoundingBoxes, worker.BinLeafCounts); + //We always write bin indices, because threading always has a bufferpool available to allocate bin indices from. + Debug.Assert(context.BinIndices.Allocated); + BinSubtrees(context.CentroidBoundsMin, context.UseX, context.UseY, context.PermuteMask, context.AxisIndex, context.OffsetToBinIndex, context.MaximumBinIndex, + context.Subtrees.Slice(start, count), worker.BinBoundingBoxes, worker.BinCentroidBoundingBoxes, worker.BinLeafCounts, context.BinIndices.Slice(start, count)); } unsafe static void MultithreadedBinSubtrees(MultithreadBinnedBuildContext* context, Vector4 centroidBoundsMin, bool useX, bool useY, Vector128 permuteMask, int axisIndex, Vector4 offsetToBinIndex, Vector4 maximumBinIndex, - Buffer subtrees, int binCount, int workerIndex, IThreadDispatcher dispatcher) + Buffer subtrees, Buffer subtreeBinIndices, int binCount, int workerIndex, IThreadDispatcher dispatcher) { ref var worker = ref context->Workers[workerIndex]; var workerPool = dispatcher.WorkerPools[workerIndex]; var taskContext = new BinSubtreesTaskContext( workerPool, new SharedTaskData(context->Workers.Length, 0, subtrees.Length, MinimumSubtreesPerThreadForBinning, context->GetTargetTaskCount(subtrees.Length)), - subtrees, binCount, useX, useY, permuteMask, axisIndex, centroidBoundsMin, offsetToBinIndex, maximumBinIndex); + subtrees, subtreeBinIndices, binCount, useX, useY, permuteMask, axisIndex, centroidBoundsMin, offsetToBinIndex, maximumBinIndex); //Don't bother initializing more slots than we have tasks. Note that this requires special handling on the task level; //if we have less tasks than workers, then the task needs to distinguish that fact. @@ -787,35 +810,21 @@ unsafe struct PartitionTaskContext /// Buffer that will contain the partitioned subtrees pulled from . /// public Buffer SubtreesNext; - + /// + /// Buffer containing bin indices for all subtrees in the node (encoded with one byte per subtree). + /// + public Buffer BinIndices; public int BinSplitIndex; - public int BinCount; - public bool UseX, UseY; - public Vector128 PermuteMask; - public int AxisIndex; - public Vector4 CentroidBoundsMin; - public Vector4 OffsetToBinIndex; - public Vector4 MaximumBinIndex; - public PartitionCounters Counters; - public PartitionTaskContext(SharedTaskData taskData, Buffer subtrees, Buffer subtreesNext, int binSplitIndex, - int binCount, bool useX, bool useY, Vector128 permuteMask, int axisIndex, - Vector4 centroidBoundsMin, Vector4 offsetToBinIndex, Vector4 maximumBinIndex) + public PartitionTaskContext(SharedTaskData taskData, Buffer subtrees, Buffer subtreesNext, Buffer binIndices, int binSplitIndex) { TaskData = taskData; Subtrees = subtrees; SubtreesNext = subtreesNext; + BinIndices = binIndices; BinSplitIndex = binSplitIndex; - BinCount = binCount; - UseX = useX; - UseY = useY; - PermuteMask = permuteMask; - AxisIndex = axisIndex; - CentroidBoundsMin = centroidBoundsMin; - OffsetToBinIndex = offsetToBinIndex; - MaximumBinIndex = maximumBinIndex; Counters = new PartitionCounters(); } @@ -824,36 +833,51 @@ public PartitionTaskContext(SharedTaskData taskData, Buffer subtrees, unsafe static void PartitionSubtreesWorker(long taskId, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) { ref var context = ref *(PartitionTaskContext*)untypedContext; - var centroidBoundsMin = context.CentroidBoundsMin; - var useX = context.UseX; - var useY = context.UseY; - var permuteMask = context.PermuteMask; - var axisIndex = context.AxisIndex; - var offsetToBinIndex = context.OffsetToBinIndex; - var maximumBinIndex = context.MaximumBinIndex; + Buffer binIndices = context.BinIndices; context.TaskData.GetSlotInterval(taskId, out var start, out var count); //We don't really want to trigger interlocked operation for *every single subtree*, but we also don't want to allocate a bunch of memory. //Compromise! Stackalloc enough memory to cover sub-batches of the worker's subtrees, and do interlocked operations at the end of each batch. //Note that the main limit to the batch size is the amount of memory in cache. const int batchSize = 16384; - bool* slotBelongsToA = stackalloc bool[batchSize]; + byte* slotBelongsToA = stackalloc byte[batchSize]; var batchCount = (count + batchSize - 1) / batchSize; var boundingBoxes = context.Subtrees.As(); var subtrees = context.Subtrees; var subtreesNext = context.SubtreesNext; + var splitIndexBundle = new Vector((byte)context.BinSplitIndex); for (int batchIndex = 0; batchIndex < batchCount; ++batchIndex) { var localCountA = 0; var batchStart = start + batchIndex * batchSize; var countInBatch = int.Min(start + count - batchStart, batchSize); - for (int indexInBatch = 0; indexInBatch < countInBatch; ++indexInBatch) + int scalarLoopStartIndex; + if (Vector.IsSupported) + { + //Note that the original data is loaded as bytes, but we need wider storage to handle the counts- which could conceivably go up to batchSize. + Vector localCountABundle = Vector.Zero; + scalarLoopStartIndex = (countInBatch / Vector.Count) * Vector.Count; + for (int indexInBatch = 0; indexInBatch < scalarLoopStartIndex; indexInBatch += Vector.Count) + { + var subtreeIndex = indexInBatch + batchStart; + var binIndicesBundle = *(Vector*)(binIndices.Memory + subtreeIndex); + var belongsToABundle = Vector.LessThan(binIndicesBundle, splitIndexBundle); + *(Vector*)(slotBelongsToA + indexInBatch) = belongsToABundle; + var increment = Vector.BitwiseAnd(belongsToABundle, Vector.One); + Vector.Widen(increment, out var low, out var high); + localCountABundle += low + high; + } + localCountA = Vector.Sum(localCountABundle); + } + else + scalarLoopStartIndex = 0; + for (int indexInBatch = scalarLoopStartIndex; indexInBatch < countInBatch; ++indexInBatch) { var subtreeIndex = indexInBatch + batchStart; - var binIndex = ComputeBinIndex(centroidBoundsMin, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, boundingBoxes[subtreeIndex]); + var binIndex = binIndices[subtreeIndex]; var belongsToA = binIndex < context.BinSplitIndex; - slotBelongsToA[indexInBatch] = belongsToA; + slotBelongsToA[indexInBatch] = belongsToA ? (byte)0xFF : (byte)0; if (belongsToA) ++localCountA; } @@ -865,7 +889,7 @@ unsafe static void PartitionSubtreesWorker(long taskId, void* untypedContext, in int recountB = 0; for (int indexInBatch = 0; indexInBatch < countInBatch; ++indexInBatch) { - var targetIndex = slotBelongsToA[indexInBatch] ? startIndexA + recountA++ : startIndexB + recountB++; + var targetIndex = slotBelongsToA[indexInBatch] != 0 ? startIndexA + recountA++ : startIndexB + recountB++; subtreesNext[targetIndex] = subtrees[batchStart + indexInBatch]; } @@ -873,14 +897,13 @@ unsafe static void PartitionSubtreesWorker(long taskId, void* untypedContext, in } unsafe static (int subtreeCountA, int subtreeCountB) MultithreadedPartition(MultithreadBinnedBuildContext* context, - Vector4 centroidBoundsMin, bool useX, bool useY, Vector128 permuteMask, int axisIndex, Vector4 offsetToBinIndex, Vector4 maximumBinIndex, - Buffer subtrees, Buffer subtreesNext, int binSplitIndex, int binCount, int targetTaskCount, int workerIndex, IThreadDispatcher dispatcher) + Buffer subtrees, Buffer subtreesNext, Buffer binIndices, int binSplitIndex, int targetTaskCount, int workerIndex, IThreadDispatcher dispatcher) { ref var worker = ref context->Workers[workerIndex]; var workerPool = dispatcher.WorkerPools[workerIndex]; var taskContext = new PartitionTaskContext( new SharedTaskData(context->Workers.Length, 0, subtrees.Length, MinimumSubtreesPerThreadForPartitioning, targetTaskCount), - subtrees, subtreesNext, binSplitIndex, binCount, useX, useY, permuteMask, axisIndex, centroidBoundsMin, offsetToBinIndex, maximumBinIndex); + subtrees, subtreesNext, binIndices, binSplitIndex); context->TaskStack->For(&PartitionSubtreesWorker, &taskContext, 0, taskContext.TaskData.TaskCount, workerIndex, dispatcher); return (taskContext.Counters.SubtreeCountA, taskContext.Counters.SubtreeCountB); @@ -915,6 +938,7 @@ static unsafe void BinnedBuildNode( //ref var debugTimes = ref Times[nodeIndex]; //debugTimes.SubtreeCount = subtreeCount; var subtrees = (usePongBuffer ? context->SubtreesPong : context->SubtreesPing).Slice(subtreeRegionStartIndex, subtreeCount); + var subtreeBinIndices = context->BinIndices.Allocated ? context->BinIndices.Slice(subtreeRegionStartIndex, subtreeCount) : default; //leaf counts, indices, and bounds are packed together, but it's useful to have a bounds-only representation so that the merging processes don't have to worry about dealing with the fourth lanes. var boundingBoxes = subtrees.As(); var nodeCount = subtreeCount - 1; @@ -1016,14 +1040,21 @@ static unsafe void BinnedBuildNode( //var debugBinStartTime = Stopwatch.GetTimestamp(); if (targetTaskCount == 1 || subtreeCount < MinimumSubtreesPerThreadForBinning) { - BinSubtrees(centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, subtrees, binBoundingBoxes, binCentroidBoundingBoxes, binLeafCounts); + //If the subtree bin indices buffer isn't available, then the binning process can't write to them! That'll happen if: + //single threaded execution, + //no bufferpool provided, + //tree size too large for stack allocation. + if (subtreeBinIndices.Allocated) + BinSubtrees(centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, subtrees, binBoundingBoxes, binCentroidBoundingBoxes, binLeafCounts, subtreeBinIndices); + else + BinSubtrees(centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, subtrees, binBoundingBoxes, binCentroidBoundingBoxes, binLeafCounts, subtreeBinIndices); //debugTimes.MTBinning = false; } else { MultithreadedBinSubtrees( (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As(ref context->Threading)), - centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, subtrees, binCount, workerIndex, dispatcher); + centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, subtrees, subtreeBinIndices, binCount, workerIndex, dispatcher); //debugTimes.MTBinning = true; } //var debugBinEndTime = Stopwatch.GetTimestamp(); @@ -1095,14 +1126,14 @@ static unsafe void BinnedBuildNode( //Split the indices/bounds into two halves for the children to operate on. if (context->SubtreesPong.Allocated) { + Debug.Assert(subtreeBinIndices.Allocated); //If the current buffer is pong, then write to ping, and vice versa. var subtreesNext = (usePongBuffer ? context->SubtreesPing : context->SubtreesPong).Slice(subtreeRegionStartIndex, subtreeCount); if (targetTaskCount == 1 || subtreeCount < MinimumSubtreesPerThreadForPartitioning) { for (int i = 0; i < subtreeCount; ++i) { - var binIndex = ComputeBinIndex(centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, boundingBoxes[i]); - var targetIndex = binIndex >= splitIndex ? subtreeCount - ++subtreeCountB : subtreeCountA++; + var targetIndex = subtreeBinIndices[i] >= splitIndex ? subtreeCount - ++subtreeCountB : subtreeCountA++; subtreesNext[targetIndex] = subtrees[i]; } //debugTimes.MTPartition = false; @@ -1111,7 +1142,7 @@ static unsafe void BinnedBuildNode( { (subtreeCountA, subtreeCountB) = MultithreadedPartition( (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As(ref context->Threading)), - centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, subtrees, subtreesNext, splitIndex, binCount, targetTaskCount, workerIndex, dispatcher); + subtrees, subtreesNext, subtreeBinIndices, splitIndex, targetTaskCount, workerIndex, dispatcher); //debugTimes.MTPartition = true; } subtrees = subtreesNext; @@ -1209,19 +1240,25 @@ static unsafe void BinnedBuildNode( /// Metanodes are in the same order and in the same slots; they simply contain data about nodes that most traversals don't need to know about. /// Buffer holding the leaf references created by the build process. /// The indices written by the build process are those defined in the inputs; any that is negative is encoded according to and points into the leaf buffer. + /// Buffer to be used for caching bin indices during execution. If subtreesPong is defined, binIndices must also be defined, and vice versa. /// Thread dispatcher used to accelerate the build process. /// Task stack used to accelerate the build process. Can be null; one will be created if not provided. /// Number of workers to be used in the builder. /// Buffer pool used to preallocate temporary resources for building. /// Minimum number of bins the builder should use per node. - /// Maximum number of bins the builder should use per node. + /// Maximum number of bins the builder should use per node. Must be no higher than 255. /// Multiplier to apply to the subtree count within a node to decide the bin count. Resulting value will then be clamped by the minimum/maximum bin counts. /// Threshold at or under which the binned builder resorts to local counting sort sweeps. - static unsafe void BinnedBuilderInternal(Buffer subtrees, Buffer subtreesPong, Buffer nodes, Buffer metanodes, Buffer leaves, + static unsafe void BinnedBuilderInternal(Buffer subtrees, Buffer subtreesPong, Buffer nodes, Buffer metanodes, Buffer leaves, Buffer binIndices, IThreadDispatcher dispatcher, LinkedTaskStack* taskStackPointer, int workerCount, BufferPool pool, int minimumBinCount, int maximumBinCount, float leafToBinMultiplier, int microsweepThreshold) { var subtreeCount = subtrees.Length; - Debug.Assert(nodes.Length >= subtreeCount - 1, "The output nodes must be able to contain the nodes created for the leaves."); + if (nodes.Length < subtreeCount - 1) + throw new ArgumentException($"The nodes buffer is too small to hold all the nodes that will be necessary for the input subtrees."); + if (maximumBinCount > 255) + throw new ArgumentException($"Maximum bin count must fit in a byte (maximum of 255)."); + if (subtreesPong.Allocated != binIndices.Allocated) + throw new ArgumentException("The parameters subtreesPong and binIndices must both be allocated or unallocated."); if (subtreeCount == 0) return; if (subtreeCount == 1) @@ -1254,7 +1291,7 @@ static unsafe void BinnedBuilderInternal(Buffer subtrees, Buffer, SingleThreaded>( minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold, - subtrees, default, leaves, nodes, metanodes, threading); + subtrees, default, leaves, nodes, metanodes, binIndices, threading); BinnedBuildNode(false, 0, 0, subtreeCount, -1, -1, default, &context, 0, null); } else @@ -1295,7 +1332,7 @@ static unsafe void BinnedBuilderInternal(Buffer subtrees, Buffer, MultithreadBinnedBuildContext>( minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold, - subtrees, subtreesPong, leaves, nodes, metanodes, threading); + subtrees, subtreesPong, leaves, nodes, metanodes, binIndices, threading); taskStackPointer->PushUnsafely(new Task(&BinnedBuilderWorkerEntry>, &context), 0, dispatcher); dispatcher.DispatchWorkers(&BinnedBuilderWorkerFunction>, unmanagedContext: taskStackPointer); @@ -1337,8 +1374,10 @@ public static unsafe void BinnedBuilder(Buffer subtrees, Buffer IThreadDispatcher threadDispatcher, BufferPool pool, int minimumBinCount = 16, int maximumBinCount = 64, float leafToBinMultiplier = 1 / 16f, int microsweepThreshold = 64) { pool.Take(subtrees.Length, out Buffer subtreesPong); - BinnedBuilderInternal(subtrees, subtreesPong, nodes, metanodes, leaves, threadDispatcher, null, threadDispatcher.ThreadCount, pool, minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold); + pool.Take(subtrees.Length, out Buffer binIndices); + BinnedBuilderInternal(subtrees, subtreesPong, nodes, metanodes, leaves, binIndices, threadDispatcher, null, threadDispatcher.ThreadCount, pool, minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold); pool.Return(ref subtreesPong); + pool.Return(ref binIndices); } /// @@ -1363,25 +1402,33 @@ public static unsafe void BinnedBuilder(Buffer subtrees, Buffer BufferPool pool = null, int maximumSubtreeStackAllocationCount = 4096, int minimumBinCount = 16, int maximumBinCount = 64, float leafToBinMultiplier = 1 / 16f, int microsweepThreshold = 64) { Buffer subtreesPong; + Buffer binIndices; bool requiresReturn = false; if (subtrees.Length <= maximumSubtreeStackAllocationCount) { var subtreesPongMemory = stackalloc NodeChild[subtrees.Length]; subtreesPong = new Buffer(subtreesPongMemory, subtrees.Length); + var binIndicesMemory = stackalloc byte[subtrees.Length]; + binIndices = new Buffer(binIndicesMemory, subtrees.Length); } else if (pool != null) { pool.Take(subtrees.Length, out subtreesPong); + pool.Take(subtrees.Length, out binIndices); requiresReturn = true; } else { + binIndices = default; subtreesPong = default; } - BinnedBuilderInternal(subtrees, subtreesPong, nodes, metanodes, leaves, null, null, 0, null, minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold); + BinnedBuilderInternal(subtrees, subtreesPong, nodes, metanodes, leaves, binIndices, null, null, 0, null, minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold); if (requiresReturn) + { + pool.Return(ref binIndices); pool.Return(ref subtreesPong); + } } } } From 33de3c4a9acda81b8bd35b471da1e9d38a295bda Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 25 Mar 2023 20:15:04 -0500 Subject: [PATCH 706/947] Trimmed some debug information. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 20 -------------------- 1 file changed, 20 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 1e7e1f5de..0fd83306a 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -934,9 +934,6 @@ static unsafe void BinnedBuildNode( BoundingBox4 centroidBounds, Context* context, int workerIndex, IThreadDispatcher dispatcher) where TLeaves : unmanaged where TThreading : unmanaged, IBinnedBuilderThreading { - //var debugStartTime = Stopwatch.GetTimestamp(); - //ref var debugTimes = ref Times[nodeIndex]; - //debugTimes.SubtreeCount = subtreeCount; var subtrees = (usePongBuffer ? context->SubtreesPong : context->SubtreesPing).Slice(subtreeRegionStartIndex, subtreeCount); var subtreeBinIndices = context->BinIndices.Allocated ? context->BinIndices.Slice(subtreeRegionStartIndex, subtreeCount) : default; //leaf counts, indices, and bounds are packed together, but it's useful to have a bounds-only representation so that the merging processes don't have to worry about dealing with the fourth lanes. @@ -951,23 +948,19 @@ static unsafe void BinnedBuildNode( } var targetTaskCount = typeof(TThreading) == typeof(SingleThreaded) ? 1 : ((MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As(ref context->Threading)))->GetTargetTaskCount(subtreeCount); - //var debugCentroidStartTime = Stopwatch.GetTimestamp(); if (nodeIndex == 0) { //The first node doesn't have a parent, and so isn't given centroid bounds. We have to compute them. if (targetTaskCount == 1 || subtreeCount < MinimumSubtreesPerThreadForCentroidPrepass) { centroidBounds = ComputeCentroidBounds(boundingBoxes); - //debugTimes.MTPrepass = false; } else { centroidBounds = MultithreadedCentroidPrepass( (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As(ref context->Threading)), subtrees, targetTaskCount, workerIndex, dispatcher); - //debugTimes.MTPrepass = true; } } - //var debugCentroidEndTime = Stopwatch.GetTimestamp(); var centroidSpan = centroidBounds.Max - centroidBounds.Min; var axisIsDegenerate = Vector128.LessThanOrEqual(centroidSpan.AsVector128(), Vector128.Create(1e-12f)); if ((Vector128.ExtractMostSignificantBits(axisIsDegenerate) & 0b111) == 0b111) @@ -1037,7 +1030,6 @@ static unsafe void BinnedBuildNode( binCentroidBounds.Max = new Vector4(float.MinValue); binLeafCounts[i] = 0; } - //var debugBinStartTime = Stopwatch.GetTimestamp(); if (targetTaskCount == 1 || subtreeCount < MinimumSubtreesPerThreadForBinning) { //If the subtree bin indices buffer isn't available, then the binning process can't write to them! That'll happen if: @@ -1048,16 +1040,13 @@ static unsafe void BinnedBuildNode( BinSubtrees(centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, subtrees, binBoundingBoxes, binCentroidBoundingBoxes, binLeafCounts, subtreeBinIndices); else BinSubtrees(centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, subtrees, binBoundingBoxes, binCentroidBoundingBoxes, binLeafCounts, subtreeBinIndices); - //debugTimes.MTBinning = false; } else { MultithreadedBinSubtrees( (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As(ref context->Threading)), centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, subtrees, subtreeBinIndices, binCount, workerIndex, dispatcher); - //debugTimes.MTBinning = true; } - //var debugBinEndTime = Stopwatch.GetTimestamp(); //Identify the split index by examining the SAH of very split option. //Premerge from left to right so we have a sorta-summed area table to cheaply look up all possible child A bounds as we scan. @@ -1136,14 +1125,12 @@ static unsafe void BinnedBuildNode( var targetIndex = subtreeBinIndices[i] >= splitIndex ? subtreeCount - ++subtreeCountB : subtreeCountA++; subtreesNext[targetIndex] = subtrees[i]; } - //debugTimes.MTPartition = false; } else { (subtreeCountA, subtreeCountB) = MultithreadedPartition( (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As(ref context->Threading)), subtrees, subtreesNext, subtreeBinIndices, splitIndex, targetTaskCount, workerIndex, dispatcher); - //debugTimes.MTPartition = true; } subtrees = subtreesNext; usePongBuffer = !usePongBuffer; @@ -1183,17 +1170,10 @@ static unsafe void BinnedBuildNode( } } - //var debugHuhEndTime = Stopwatch.GetTimestamp(); var leafCountB = bestLeafCountB; var leafCountA = totalLeafCount - leafCountB; Debug.Assert(subtreeCountA + subtreeCountB == subtreeCount); BuildNode(bestBoundingBoxA, bestBoundingBoxB, leafCountA, leafCountB, subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, subtreeCountA, subtreeCountB, ref context->Leaves, out var nodeChildIndexA, out var nodeChildIndexB); - //var debugEndTime = Stopwatch.GetTimestamp(); - //debugTimes.Total = (debugEndTime - debugStartTime) / (double)Stopwatch.Frequency; - //debugTimes.Partition = (debugHuhEndTime - debugHuhStartTime) / (double)Stopwatch.Frequency; - //debugTimes.CentroidPrepass = (debugCentroidEndTime - debugCentroidStartTime) / (double)Stopwatch.Frequency; - //debugTimes.Binning = (debugBinEndTime - debugBinStartTime) / (double)Stopwatch.Frequency; - //debugTimes.TargetTaskCount = targetTaskCount; var shouldPushBOntoMultithreadedQueue = typeof(TThreading) != typeof(SingleThreaded) && subtreeCountA >= MinimumSubtreesPerThreadForNodeJob && subtreeCountB >= MinimumSubtreesPerThreadForNodeJob; ContinuationHandle nodeBContinuation = default; From 61012560b66d460e0042efcacad4f3428a7d4110 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 1 Apr 2023 14:23:18 -0500 Subject: [PATCH 707/947] Some tuning; inner multithreading seems to be falling victim to overaggressive recursion. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 23 +++++++++++++++-------- 1 file changed, 15 insertions(+), 8 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 0fd83306a..dc6a53f5b 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -443,16 +443,21 @@ public void GetBins(int workerIndex, binLeafCounts = worker.BinLeafCounts; } - public int GetTargetTaskCount(int subtreeCount) + public int GetTargetTaskCountForInnerLoop(int subtreeCount) { return (int)float.Ceiling(TopLevelTargetTaskCount * (float)subtreeCount / OriginalSubtreeCount); } + public int GetTargetTaskCountForNodes(int subtreeCount) + { + return (int)float.Ceiling(TargetTaskCountMultiplierForNodePushOverInnerLoop * TopLevelTargetTaskCount * (float)subtreeCount / OriginalSubtreeCount); + } } - const int MinimumSubtreesPerThreadForCentroidPrepass = 65536; - const int MinimumSubtreesPerThreadForBinning = 65536; - const int MinimumSubtreesPerThreadForPartitioning = 65536; - const int MinimumSubtreesPerThreadForNodeJob = 1024; + const int MinimumSubtreesPerThreadForCentroidPrepass = 1024; + const int MinimumSubtreesPerThreadForBinning = 131072; + const int MinimumSubtreesPerThreadForPartitioning = 131072; + const int MinimumSubtreesPerThreadForNodeJob = 256; + const int TargetTaskCountMultiplierForNodePushOverInnerLoop = 8; [MethodImpl(MethodImplOptions.AggressiveInlining)] static BoundingBox4 ComputeCentroidBounds(Buffer bounds) @@ -729,7 +734,7 @@ unsafe static void MultithreadedBinSubtrees(MultithreadBinnedBuildContext* conte var workerPool = dispatcher.WorkerPools[workerIndex]; var taskContext = new BinSubtreesTaskContext( workerPool, - new SharedTaskData(context->Workers.Length, 0, subtrees.Length, MinimumSubtreesPerThreadForBinning, context->GetTargetTaskCount(subtrees.Length)), + new SharedTaskData(context->Workers.Length, 0, subtrees.Length, MinimumSubtreesPerThreadForBinning, context->GetTargetTaskCountForInnerLoop(subtrees.Length)), subtrees, subtreeBinIndices, binCount, useX, useY, permuteMask, axisIndex, centroidBoundsMin, offsetToBinIndex, maximumBinIndex); //Don't bother initializing more slots than we have tasks. Note that this requires special handling on the task level; @@ -947,7 +952,7 @@ static unsafe void BinnedBuildNode( return; } var targetTaskCount = typeof(TThreading) == typeof(SingleThreaded) ? 1 : - ((MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As(ref context->Threading)))->GetTargetTaskCount(subtreeCount); + ((MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As(ref context->Threading)))->GetTargetTaskCountForInnerLoop(subtreeCount); if (nodeIndex == 0) { //The first node doesn't have a parent, and so isn't given centroid bounds. We have to compute them. @@ -1175,7 +1180,9 @@ static unsafe void BinnedBuildNode( Debug.Assert(subtreeCountA + subtreeCountB == subtreeCount); BuildNode(bestBoundingBoxA, bestBoundingBoxB, leafCountA, leafCountB, subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, subtreeCountA, subtreeCountB, ref context->Leaves, out var nodeChildIndexA, out var nodeChildIndexB); - var shouldPushBOntoMultithreadedQueue = typeof(TThreading) != typeof(SingleThreaded) && subtreeCountA >= MinimumSubtreesPerThreadForNodeJob && subtreeCountB >= MinimumSubtreesPerThreadForNodeJob; + var targetNodeTaskCount = typeof(TThreading) == typeof(SingleThreaded) ? 1 : + ((MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As(ref context->Threading)))->GetTargetTaskCountForNodes(subtreeCount); + var shouldPushBOntoMultithreadedQueue = targetNodeTaskCount > 1 && subtreeCountA >= MinimumSubtreesPerThreadForNodeJob && subtreeCountB >= MinimumSubtreesPerThreadForNodeJob; ContinuationHandle nodeBContinuation = default; if (shouldPushBOntoMultithreadedQueue) { From c6f3cfea103a57441e045b05b4637481cba965bb Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 1 Apr 2023 16:10:54 -0500 Subject: [PATCH 708/947] Added job filtering for linkedtaskstack. --- BepuUtilities/LinkedTaskStack.cs | 239 +++++++++++++++++++++++++++---- 1 file changed, 213 insertions(+), 26 deletions(-) diff --git a/BepuUtilities/LinkedTaskStack.cs b/BepuUtilities/LinkedTaskStack.cs index b5858110c..a941265b1 100644 --- a/BepuUtilities/LinkedTaskStack.cs +++ b/BepuUtilities/LinkedTaskStack.cs @@ -161,18 +161,119 @@ public void NotifyTaskCompleted(int workerIndex, IThreadDispatcher dispatcher) public static bool operator !=(ContinuationHandle left, ContinuationHandle right) => !(left == right); } -[StructLayout(LayoutKind.Explicit, Size = 280)] + + +/// +/// Determines which jobs are allowed to serve a request. +/// +public interface IJobFilter +{ + /// + /// Determines whether a job with the given tag should be allowed to serve a request. + /// + /// Tag of the candidate job. + /// True if the job should be allowed to serve a request, false otherwise. + bool AllowJob(ulong jobTag); +} + +/// +/// A filter that will allow pops from any jobs. +/// +public struct AllowAllJobs : IJobFilter +{ + /// + public readonly bool AllowJob(ulong jobTag) + { + return true; + } +} + +/// +/// A job filter that wraps a managed delegate. +/// +public struct DelegateJobFilter : IJobFilter +{ + /// + /// Delegate to use as the filter. + /// + public Func Filter; + /// + /// Creates a job filter that wraps a delegate. + /// + /// Delegate to use as the filter. + public DelegateJobFilter(Func filter) + { + Filter = filter; + } + /// + public readonly bool AllowJob(ulong jobTag) + { + return Filter(jobTag); + } +} + +/// +/// A job filter that wraps a function pointer. +/// +public unsafe struct FunctionPointerJobFilter : IJobFilter +{ + /// + /// Delegate to use as the filter. + /// + public delegate* Filter; + /// + /// Creates a job filter that wraps a delegate. + /// + /// Delegate to use as the filter. + public FunctionPointerJobFilter(delegate* filter) + { + Filter = filter; + } + /// + public readonly bool AllowJob(ulong jobTag) + { + return Filter(jobTag); + } +} +/// +/// A job filter that requires the job tag to meet or exceed a threshold value. +/// +public struct TagThresholdFilter : IJobFilter +{ + /// + /// Value that a job must match or exceed to be allowed. + /// + public ulong MinimumTagValue; + /// + /// Creates a job filter that wraps a delegate. + /// + /// Value that a job must match or exceed to be allowed. + public TagThresholdFilter(ulong minimumTagValue) + { + MinimumTagValue = minimumTagValue; + } + /// + public bool AllowJob(ulong jobTag) + { + return jobTag >= MinimumTagValue; + } +} + +[StructLayout(LayoutKind.Explicit, Size = 292)] internal unsafe struct Job { [FieldOffset(0)] public Buffer Tasks; [FieldOffset(16)] public Job* Previous; + [FieldOffset(24)] + public ulong Tag; - [FieldOffset(152)] + [FieldOffset(160)] public int Counter; - public static Job* Create(Span sourceTasks, BufferPool pool) + + public static Job* Create(Span sourceTasks, ulong tag, BufferPool pool) { //Note that the job and the buffer of tasks are allocated together as one block. //This ensures we only need to perform one @@ -180,6 +281,7 @@ internal unsafe struct Job pool.Take(sizeToAllocate, out var rawBuffer); Job* job = (Job*)rawBuffer.Memory; job->Tasks = new Buffer(rawBuffer.Memory + sizeof(Job), sourceTasks.Length, rawBuffer.Id); + job->Tag = tag; sourceTasks.CopyTo(job->Tasks); job->Counter = sourceTasks.Length; job->Previous = null; @@ -313,14 +415,15 @@ internal void Reset(BufferPool threadPool) /// Pushes a set of tasks onto the stack. /// /// Tasks composing the job. + /// User tag associated with the job. /// Dispatcher used to pull thread allocations if necessary. /// If the worker associated with this stack might be active, this function can only be called by the worker. - internal Job* AllocateJob(Span tasks, IThreadDispatcher dispatcher) + internal Job* AllocateJob(Span tasks, ulong tag, IThreadDispatcher dispatcher) { Debug.Assert(tasks.Length > 0, "Probably shouldn't be trying to push zero tasks."); var threadPool = dispatcher.WorkerPools[WorkerIndex]; //Note that we allocate jobs on the heap directly; it's safe to resize the AllocatedJobs list because it's just storing pointers. - var job = Job.Create(tasks, threadPool); + var job = Job.Create(tasks, tag, threadPool); AllocatedJobs.Allocate(threadPool) = (nuint)job; return job; } @@ -487,25 +590,33 @@ public ContinuationHandle AllocateContinuation(int taskCount, int workerIndex, I { return workers[workerIndex].AllocateContinuation(taskCount, dispatcher, onCompleted); } - + /// /// Attempts to pop a task. /// + /// Filter to apply to jobs. Only allowed jobs can have tasks popped from them. /// Popped task, if any. + /// Type of the job filter used in the pop. /// Result status of the pop attempt. - public PopTaskResult TryPop(out Task task) + public PopTaskResult TryPop(ref TJobFilter filter, out Task task) where TJobFilter : IJobFilter { //Note that this implementation does not need to lock against anything. We just follow the pointer. + var job = (Job*)head; while (true) { - var job = (Job*)head; if (job == null) { //There is no job to pop from. task = default; return padded.Stop ? PopTaskResult.Stop : PopTaskResult.Empty; } - //The sampled head exists. Try to pop a task from it. + //Try to pop a task from the current job. + if (!filter.AllowJob(job->Tag)) + { + //This job isn't allowed for this pop; go to the next one. + job = job->Previous; + continue; + } if (job->TryPop(out task)) { Debug.Assert(task.Function != null); @@ -518,20 +629,33 @@ public PopTaskResult TryPop(out Task task) //If this fails, the head has changed before we could remove it and the current empty job will persists in the stack until some other dequeue finds it. //That's okay. Interlocked.CompareExchange(ref head, (nuint)job->Previous, (nuint)job); + job = (Job*)head; } - } } + /// + /// Attempts to pop a task. + /// + /// Popped task, if any. + /// Result status of the pop attempt. + public PopTaskResult TryPop(out Task task) + { + AllowAllJobs filter = default; + return TryPop(ref filter, out task); + } + /// /// Attempts to pop a task and run it. /// + /// Filter to apply to jobs. Only allowed jobs can have tasks popped from them. /// Index of the worker to pass into the task function. /// Thread dispatcher running this task stack. + /// Type of the job filter used in the pop. /// Result status of the pop attempt. - public PopTaskResult TryPopAndRun(int workerIndex, IThreadDispatcher dispatcher) + public PopTaskResult TryPopAndRun(ref TJobFilter filter, int workerIndex, IThreadDispatcher dispatcher) where TJobFilter : IJobFilter { - var result = TryPop(out var task); + var result = TryPop(ref filter, out var task); if (result == PopTaskResult.Success) { task.Run(workerIndex, dispatcher); @@ -539,16 +663,30 @@ public PopTaskResult TryPopAndRun(int workerIndex, IThreadDispatcher dispatcher) return result; } + /// + /// Attempts to pop a task and run it. + /// + /// Index of the worker to pass into the task function. + /// Thread dispatcher running this task stack. + /// Result status of the pop attempt. + public PopTaskResult TryPopAndRun(int workerIndex, IThreadDispatcher dispatcher) + { + AllowAllJobs filter = default; + return TryPopAndRun(ref filter, workerIndex, dispatcher); + } + + /// /// Pushes a set of tasks onto the task stack. This function is not thread safe. /// /// Tasks composing the job. /// Thread dispatcher to allocate thread data from if necessary. /// Index of the worker stack to push the tasks onto. + /// User-defined tag data for the submitted job. /// This must not be used while other threads could be performing task pushes or pops that could affect the specified worker. - public void PushUnsafely(Span tasks, int workerIndex, IThreadDispatcher dispatcher) + public void PushUnsafely(Span tasks, int workerIndex, IThreadDispatcher dispatcher, ulong tag = 0) { - Job* job = workers[workerIndex].AllocateJob(tasks, dispatcher); + Job* job = workers[workerIndex].AllocateJob(tasks, tag, dispatcher); job->Previous = (Job*)head; head = (nuint)job; } @@ -570,10 +708,11 @@ public unsafe void PushUnsafely(Task task, int workerIndex, IThreadDispatcher di /// Tasks composing the job. /// Thread dispatcher to allocate thread data from if necessary. /// Index of the worker stack to push the tasks onto. + /// User-defined tag data for the submitted job. /// True if the push succeeded, false if it was contested. - public void Push(Span tasks, int workerIndex, IThreadDispatcher dispatcher) + public void Push(Span tasks, int workerIndex, IThreadDispatcher dispatcher, ulong tag = 0) { - Job* job = workers[workerIndex].AllocateJob(tasks, dispatcher); + Job* job = workers[workerIndex].AllocateJob(tasks, tag, dispatcher); while (true) { @@ -612,16 +751,18 @@ public ContinuationHandle AllocateContinuationAndPush(Span tasks, int work /// Waits for a continuation to be completed. /// /// Instead of spinning the entire time, this may pop and execute pending tasks to fill the gap. + /// Filter to apply to jobs. Only allowed jobs can have tasks popped from them. /// Continuation to wait on. /// Thread dispatcher to allocate thread data from if necessary. /// Index of the executing worker. - public void WaitForCompletion(ContinuationHandle continuation, int workerIndex, IThreadDispatcher dispatcher) + /// Type of the job filter used in the pop. + public void WaitForCompletion(ref TJobFilter filter, ContinuationHandle continuation, int workerIndex, IThreadDispatcher dispatcher) where TJobFilter : IJobFilter { var waiter = new SpinWait(); Debug.Assert(continuation.Initialized, "This codepath should only run if the continuation was allocated earlier."); while (!continuation.Completed) { - var result = TryPop(out var fillerTask); + var result = TryPop(ref filter, out var fillerTask); if (result == PopTaskResult.Stop) { return; @@ -638,14 +779,29 @@ public void WaitForCompletion(ContinuationHandle continuation, int workerIndex, } } + /// + /// Waits for a continuation to be completed. + /// + /// Instead of spinning the entire time, this may pop and execute pending tasks to fill the gap. + /// Continuation to wait on. + /// Thread dispatcher to allocate thread data from if necessary. + /// Index of the executing worker. + public void WaitForCompletion(ContinuationHandle continuation, int workerIndex, IThreadDispatcher dispatcher) + { + AllowAllJobs filter = default; + WaitForCompletion(ref filter, continuation, workerIndex, dispatcher); + } + /// /// Pushes a set of tasks to the worker stack and returns when all tasks are complete. /// /// Tasks composing the job. A continuation will be assigned internally; no continuation should be present on any of the provided tasks. /// Index of the worker executing this function. /// Thread dispatcher to allocate thread data from if necessary. + /// Filter applied to jobs considered for filling the calling thread's wait for other threads to complete. + /// Type of the job filter used in the pop. /// Note that this will keep working until all tasks are run. It may execute tasks unrelated to the requested tasks while waiting on other workers to complete constituent tasks. - public void RunTasks(Span tasks, int workerIndex, IThreadDispatcher dispatcher) + public void RunTasks(Span tasks, int workerIndex, IThreadDispatcher dispatcher, ref TJobFilter filter) where TJobFilter : IJobFilter { if (tasks.Length == 0) return; @@ -675,10 +831,24 @@ public void RunTasks(Span tasks, int workerIndex, IThreadDispatcher dispat if (tasks.Length > 1) { //Task 0 is done; this thread should seek out other work until the job is complete. - WaitForCompletion(continuationHandle, workerIndex, dispatcher); + WaitForCompletion(ref filter, continuationHandle, workerIndex, dispatcher); } } + + /// + /// Pushes a set of tasks to the worker stack and returns when all tasks are complete. + /// + /// Tasks composing the job. A continuation will be assigned internally; no continuation should be present on any of the provided tasks. + /// Index of the worker executing this function. + /// Thread dispatcher to allocate thread data from if necessary. + /// Note that this will keep working until all tasks are run. It may execute tasks unrelated to the requested tasks while waiting on other workers to complete constituent tasks. + public void RunTasks(Span tasks, int workerIndex, IThreadDispatcher dispatcher) + { + AllowAllJobs filter = default; + RunTasks(tasks, workerIndex, dispatcher, ref filter); + } + /// /// Requests that all workers stop. The next time a worker runs out of tasks to run, if it sees a stop command, it will be reported. /// @@ -717,10 +887,9 @@ public void PushForUnsafely(delegate* /// Inclusive start index of the loop range. /// Number of iterations to perform. /// Thread dispatcher to allocate thread data from if necessary. - /// Index of the worker stack to push the tasks onto. + /// Index of the worker stack to push the tasks onto. /// Continuation associated with the loop tasks, if any. - /// This function will not usually attempt to run any iterations of the loop itself. It tries to push the loop tasks onto the stack. - /// If the task stack is full, this will opt to run the tasks inline while waiting for room. + /// This function will not attempt to run any iterations of the loop itself. public void PushFor(delegate* function, void* context, int inclusiveStartIndex, int iterationCount, int workerIndex, IThreadDispatcher dispatcher, ContinuationHandle continuation = default) { Span tasks = stackalloc Task[iterationCount]; @@ -738,9 +907,12 @@ public void PushFor(delegate* functio /// Context pointer to pass into each iteration of the loop. /// Inclusive start index of the loop range. /// Number of iterations to perform. - /// Thread dispatcher to allocate thread data from if necessary. /// Index of the worker stack to push the tasks onto. - public void For(delegate* function, void* context, int inclusiveStartIndex, int iterationCount, int workerIndex, IThreadDispatcher dispatcher) + /// Thread dispatcher to allocate thread data from if necessary. + /// Filter applied to jobs considered for filling the calling thread's wait for other threads to complete. + /// Type of the job filter used in the pop. + public void For(delegate* function, void* context, int inclusiveStartIndex, int iterationCount, int workerIndex, IThreadDispatcher dispatcher, + ref TJobFilter filter) where TJobFilter : IJobFilter { if (iterationCount <= 0) return; @@ -749,6 +921,21 @@ public void For(delegate* function, v { tasks[i] = new Task(function, context, inclusiveStartIndex + i); } - RunTasks(tasks, workerIndex, dispatcher); + RunTasks(tasks, workerIndex, dispatcher, ref filter); + } + + /// + /// Submits a set of tasks representing a for loop over the given indices and returns when all loop iterations are complete. + /// + /// Function to execute on each iteration of the loop. + /// Context pointer to pass into each iteration of the loop. + /// Inclusive start index of the loop range. + /// Number of iterations to perform. + /// Index of the worker stack to push the tasks onto. + /// Thread dispatcher to allocate thread data from if necessary. + public void For(delegate* function, void* context, int inclusiveStartIndex, int iterationCount, int workerIndex, IThreadDispatcher dispatcher) + { + AllowAllJobs filter = default; + For(function, context, inclusiveStartIndex, iterationCount, workerIndex, dispatcher, ref filter); } } From 240455a70655d4df7faa6bacdc1cb17cc4003c1f Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 1 Apr 2023 17:59:45 -0500 Subject: [PATCH 709/947] Some missing bits in LinkedTaskStack's filter, and attempt at integration. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 21 +++++++--- BepuUtilities/LinkedTaskStack.cs | 44 ++++++++++++--------- Demos/SpecializedTests/TaskQueueTestDemo.cs | 2 +- 3 files changed, 43 insertions(+), 24 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index dc6a53f5b..d83962d74 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -575,7 +575,13 @@ unsafe static BoundingBox4 MultithreadedCentroidPrepass(MultithreadBinnedBuildCo } } Debug.Assert(taskContext.TaskData.TaskCount > 0 && taskContext.TaskData.WorkerCount > 0); - context->TaskStack->For(&CentroidPrepassWorker, &taskContext, 0, taskCount, workerIndex, dispatcher); + //We only want the inner multithreading to work on small, non-recursive jobs. + //Diving into a node at this point would stall the current node and favor more (and smaller) nodes. + //(Note: the centroid prepass only runs at the root, so we don't expect there to be any competition from other nodes *in this tree*, + //but it's possible that the same taskstack is used from multiple binned builds. + //Technically, there's potential interference from other user tasks that have nothing to do with binned building, but... not too concerned at this point.) + var jobFilter = new MinimumTagFilter(1); + context->TaskStack->For(&CentroidPrepassWorker, &taskContext, 0, taskCount, workerIndex, dispatcher, ref jobFilter, 1); var centroidBounds = taskContext.PrepassWorkers[0]; for (int i = 1; i < activeWorkerCount; ++i) @@ -759,7 +765,10 @@ unsafe static void MultithreadedBinSubtrees(MultithreadBinnedBuildContext* conte } } - context->TaskStack->For(&BinSubtreesWorker, &taskContext, 0, taskContext.TaskData.TaskCount, workerIndex, dispatcher); + //We only want the inner multithreading to work on small, non-recursive jobs. + //Diving into a node at this point would stall the current node and favor more (and smaller) nodes. + var jobFilter = new MinimumTagFilter(1); + context->TaskStack->For(&BinSubtreesWorker, &taskContext, 0, taskContext.TaskData.TaskCount, workerIndex, dispatcher, ref jobFilter, 1); //Unless the number of threads and bins is really huge, there's no value in attempting to multithread the final compression. //(Parallel reduction is an option, but even then... I suspect the single threaded version will be faster. And it's way simpler.) @@ -909,8 +918,10 @@ unsafe static (int subtreeCountA, int subtreeCountB) MultithreadedPartition(Mult var taskContext = new PartitionTaskContext( new SharedTaskData(context->Workers.Length, 0, subtrees.Length, MinimumSubtreesPerThreadForPartitioning, targetTaskCount), subtrees, subtreesNext, binIndices, binSplitIndex); - - context->TaskStack->For(&PartitionSubtreesWorker, &taskContext, 0, taskContext.TaskData.TaskCount, workerIndex, dispatcher); + //We only want the inner multithreading to work on small, non-recursive jobs. + //Diving into a node at this point would stall the current node and favor more (and smaller) nodes. + var jobFilter = new MinimumTagFilter(1); + context->TaskStack->For(&PartitionSubtreesWorker, &taskContext, 0, taskContext.TaskData.TaskCount, workerIndex, dispatcher, ref jobFilter, 1); return (taskContext.Counters.SubtreeCountA, taskContext.Counters.SubtreeCountB); } @@ -1198,7 +1209,7 @@ static unsafe void BinnedBuildNode( //Note that we use the task id to store subtree start, subtree count, and the pong buffer flag. Don't have to do that, but no reason not to use it. Debug.Assert((uint)subtreeCountB < (1u << 31), "The task id encodes start, count, and a pong flag, so we don't have room for a full 32 bits of count."); var task = new Task(&BinnedBuilderNodeWorker, &nodePushContext, (long)(subtreeRegionStartIndex + subtreeCountA) | ((long)subtreeCountB << 32) | (usePongBuffer ? 1L << 63 : 0)); - nodeBContinuation = threading.TaskStack->AllocateContinuationAndPush(new Span(&task, 1), workerIndex, dispatcher); + nodeBContinuation = threading.TaskStack->AllocateContinuationAndPush(new Span(&task, 1), workerIndex, dispatcher, 0); } if (subtreeCountA > 1) BinnedBuildNode(usePongBuffer, subtreeRegionStartIndex, nodeChildIndexA, subtreeCountA, nodeIndex, 0, bestCentroidBoundingBoxA, context, workerIndex, dispatcher); diff --git a/BepuUtilities/LinkedTaskStack.cs b/BepuUtilities/LinkedTaskStack.cs index a941265b1..d8c4e5fc4 100644 --- a/BepuUtilities/LinkedTaskStack.cs +++ b/BepuUtilities/LinkedTaskStack.cs @@ -238,7 +238,7 @@ public readonly bool AllowJob(ulong jobTag) /// /// A job filter that requires the job tag to meet or exceed a threshold value. /// -public struct TagThresholdFilter : IJobFilter +public struct MinimumTagFilter : IJobFilter { /// /// Value that a job must match or exceed to be allowed. @@ -248,7 +248,7 @@ public struct TagThresholdFilter : IJobFilter /// Creates a job filter that wraps a delegate. /// /// Value that a job must match or exceed to be allowed. - public TagThresholdFilter(ulong minimumTagValue) + public MinimumTagFilter(ulong minimumTagValue) { MinimumTagValue = minimumTagValue; } @@ -696,10 +696,11 @@ public void PushUnsafely(Span tasks, int workerIndex, IThreadDispatcher di /// Task to push. /// Thread dispatcher to allocate thread data from if necessary. /// Index of the worker stack to push the tasks onto. + /// User tag associated with the job spanning the submitted tasks. /// This must not be used while other threads could be performing task pushes or pops that could affect the specified worker. - public unsafe void PushUnsafely(Task task, int workerIndex, IThreadDispatcher dispatcher) + public unsafe void PushUnsafely(Task task, int workerIndex, IThreadDispatcher dispatcher, ulong tag = 0) { - PushUnsafely(new Span(&task, 1), workerIndex, dispatcher); + PushUnsafely(new Span(&task, 1), workerIndex, dispatcher, tag); } /// @@ -731,10 +732,11 @@ public void Push(Span tasks, int workerIndex, IThreadDispatcher dispatcher /// Tasks composing the job. A continuation will be assigned internally; no continuation should be present on any of the provided tasks. /// Thread dispatcher to allocate thread data from if necessary. /// Index of the worker stack to push the tasks onto. + /// User tag associated with the job spanning the submitted tasks. /// Task to run upon completion of all the submitted tasks, if any. /// Handle of the continuation created for these tasks. /// Note that this will keep trying until task submission succeeds. - public ContinuationHandle AllocateContinuationAndPush(Span tasks, int workerIndex, IThreadDispatcher dispatcher, Task onComplete = default) + public ContinuationHandle AllocateContinuationAndPush(Span tasks, int workerIndex, IThreadDispatcher dispatcher, ulong tag = 0, Task onComplete = default) { var continuationHandle = AllocateContinuation(tasks.Length, workerIndex, dispatcher, onComplete); for (int i = 0; i < tasks.Length; ++i) @@ -743,7 +745,7 @@ public ContinuationHandle AllocateContinuationAndPush(Span tasks, int work Debug.Assert(!task.Continuation.Initialized, "This function creates a continuation for the tasks"); task.Continuation = continuationHandle; } - Push(tasks, workerIndex, dispatcher); + Push(tasks, workerIndex, dispatcher, tag); return continuationHandle; } @@ -799,9 +801,10 @@ public void WaitForCompletion(ContinuationHandle continuation, int workerIndex, /// Index of the worker executing this function. /// Thread dispatcher to allocate thread data from if necessary. /// Filter applied to jobs considered for filling the calling thread's wait for other threads to complete. + /// User tag associated with the job spanning the submitted tasks. /// Type of the job filter used in the pop. /// Note that this will keep working until all tasks are run. It may execute tasks unrelated to the requested tasks while waiting on other workers to complete constituent tasks. - public void RunTasks(Span tasks, int workerIndex, IThreadDispatcher dispatcher, ref TJobFilter filter) where TJobFilter : IJobFilter + public void RunTasks(Span tasks, int workerIndex, IThreadDispatcher dispatcher, ref TJobFilter filter, ulong tag = 0) where TJobFilter : IJobFilter { if (tasks.Length == 0) return; @@ -820,7 +823,7 @@ public void RunTasks(Span tasks, int workerIndex, IThreadDispa task.Continuation = continuationHandle; tasksToPush[i] = task; } - Push(tasksToPush, workerIndex, dispatcher); + Push(tasksToPush, workerIndex, dispatcher, tag); } //Tasks [1, count) are submitted to the stack and may now be executing on other workers. //The thread calling the for loop should not relinquish its timeslice. It should immediately begin working on task 0. @@ -842,11 +845,12 @@ public void RunTasks(Span tasks, int workerIndex, IThreadDispa /// Tasks composing the job. A continuation will be assigned internally; no continuation should be present on any of the provided tasks. /// Index of the worker executing this function. /// Thread dispatcher to allocate thread data from if necessary. + /// User tag associated with the job spanning the submitted tasks. /// Note that this will keep working until all tasks are run. It may execute tasks unrelated to the requested tasks while waiting on other workers to complete constituent tasks. - public void RunTasks(Span tasks, int workerIndex, IThreadDispatcher dispatcher) + public void RunTasks(Span tasks, int workerIndex, IThreadDispatcher dispatcher, ulong tag = 0) { AllowAllJobs filter = default; - RunTasks(tasks, workerIndex, dispatcher, ref filter); + RunTasks(tasks, workerIndex, dispatcher, ref filter, tag); } /// @@ -867,16 +871,17 @@ public void RequestStop() /// Number of iterations to perform. /// Thread dispatcher to allocate thread data from if necessary. /// Index of the worker stack to push the tasks onto. + /// User tag associated with the job spanning the submitted tasks. /// Continuation associated with the loop tasks, if any. /// This must not be used while other threads could be performing task pushes or pops that could affect the specified worker. - public void PushForUnsafely(delegate* function, void* context, int inclusiveStartIndex, int iterationCount, int workerIndex, IThreadDispatcher dispatcher, ContinuationHandle continuation = default) + public void PushForUnsafely(delegate* function, void* context, int inclusiveStartIndex, int iterationCount, int workerIndex, IThreadDispatcher dispatcher, ulong tag = 0, ContinuationHandle continuation = default) { Span tasks = stackalloc Task[iterationCount]; for (int i = 0; i < tasks.Length; ++i) { tasks[i] = new Task { Function = function, Context = context, Id = i + inclusiveStartIndex, Continuation = continuation }; } - PushUnsafely(tasks, workerIndex, dispatcher); + PushUnsafely(tasks, workerIndex, dispatcher, tag); } /// @@ -889,15 +894,16 @@ public void PushForUnsafely(delegate* /// Thread dispatcher to allocate thread data from if necessary. /// Index of the worker stack to push the tasks onto. /// Continuation associated with the loop tasks, if any. + /// User tag associated with the job spanning the submitted tasks. /// This function will not attempt to run any iterations of the loop itself. - public void PushFor(delegate* function, void* context, int inclusiveStartIndex, int iterationCount, int workerIndex, IThreadDispatcher dispatcher, ContinuationHandle continuation = default) + public void PushFor(delegate* function, void* context, int inclusiveStartIndex, int iterationCount, int workerIndex, IThreadDispatcher dispatcher, ulong tag = 0, ContinuationHandle continuation = default) { Span tasks = stackalloc Task[iterationCount]; for (int i = 0; i < tasks.Length; ++i) { tasks[i] = new Task { Function = function, Context = context, Id = i + inclusiveStartIndex, Continuation = continuation }; } - Push(tasks, workerIndex, dispatcher); + Push(tasks, workerIndex, dispatcher, tag); } /// @@ -910,9 +916,10 @@ public void PushFor(delegate* functio /// Index of the worker stack to push the tasks onto. /// Thread dispatcher to allocate thread data from if necessary. /// Filter applied to jobs considered for filling the calling thread's wait for other threads to complete. + /// User tag associated with the job spanning the submitted tasks. /// Type of the job filter used in the pop. public void For(delegate* function, void* context, int inclusiveStartIndex, int iterationCount, int workerIndex, IThreadDispatcher dispatcher, - ref TJobFilter filter) where TJobFilter : IJobFilter + ref TJobFilter filter, ulong tag = 0) where TJobFilter : IJobFilter { if (iterationCount <= 0) return; @@ -921,7 +928,7 @@ public void For(delegate* { tasks[i] = new Task(function, context, inclusiveStartIndex + i); } - RunTasks(tasks, workerIndex, dispatcher, ref filter); + RunTasks(tasks, workerIndex, dispatcher, ref filter, tag); } /// @@ -933,9 +940,10 @@ public void For(delegate* /// Number of iterations to perform. /// Index of the worker stack to push the tasks onto. /// Thread dispatcher to allocate thread data from if necessary. - public void For(delegate* function, void* context, int inclusiveStartIndex, int iterationCount, int workerIndex, IThreadDispatcher dispatcher) + /// User tag associated with the job spanning the submitted tasks. + public void For(delegate* function, void* context, int inclusiveStartIndex, int iterationCount, int workerIndex, IThreadDispatcher dispatcher, ulong tag = 0) { AllowAllJobs filter = default; - For(function, context, inclusiveStartIndex, iterationCount, workerIndex, dispatcher, ref filter); + For(function, context, inclusiveStartIndex, iterationCount, workerIndex, dispatcher, ref filter, tag); } } diff --git a/Demos/SpecializedTests/TaskQueueTestDemo.cs b/Demos/SpecializedTests/TaskQueueTestDemo.cs index 4c8c8ca1d..c4969661e 100644 --- a/Demos/SpecializedTests/TaskQueueTestDemo.cs +++ b/Demos/SpecializedTests/TaskQueueTestDemo.cs @@ -202,7 +202,7 @@ public override void Initialize(ContentArchive content, Camera camera) var continuation = linkedTaskStackPointer->AllocateContinuation(iterationCount * tasksPerIteration, 0, ThreadDispatcher, new BepuUtilities.TestLinkedTaskStack.Task(&IssueStop, &context)); for (int i = 0; i < iterationCount; ++i) { - linkedTaskStackPointer->PushForUnsafely(&Test, &context, i * tasksPerIteration, tasksPerIteration, 0, ThreadDispatcher, continuation); + linkedTaskStackPointer->PushForUnsafely(&Test, &context, i * tasksPerIteration, tasksPerIteration, 0, ThreadDispatcher, continuation: continuation); } //taskQueuePointer->TryEnqueueStopUnsafely(); //taskQueuePointer->EnqueueTasks() From 22dac469c92e5ffbeb3764feedb9eb90326361ac Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 5 Apr 2023 21:25:16 -0500 Subject: [PATCH 710/947] Sponsor characters no longer potentially NaN out by reaching the target goal (or otherwise having an influence sum of zero). --- Demos/Demos/Sponsors/SponsorCharacterAI.cs | 8 +++++--- Demos/Demos/Sponsors/SponsorDemo.cs | 2 +- Demos/SpecializedTests/Media/2.4/NewtTyrannyDemo.cs | 2 +- 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/Demos/Demos/Sponsors/SponsorCharacterAI.cs b/Demos/Demos/Sponsors/SponsorCharacterAI.cs index 6ac467f07..3088fb989 100644 --- a/Demos/Demos/Sponsors/SponsorCharacterAI.cs +++ b/Demos/Demos/Sponsors/SponsorCharacterAI.cs @@ -29,7 +29,7 @@ public SponsorCharacterAI(CharacterControllers characters, in CollidableDescript this.targetLocation = targetLocation; } - public void Update(CharacterControllers characters, Simulation simulation, ref QuickList newts, in Vector2 arenaMin, in Vector2 arenaMax, Random random) + public void Update(CharacterControllers characters, Simulation simulation, ref QuickList newts, Random random) { var body = simulation.Bodies[bodyHandle]; Vector2 influenceSum = default; @@ -44,7 +44,7 @@ public void Update(CharacterControllers characters, Simulation simulation, ref Q var influenceMagnitude = 1f / (distance * 0.1f + .1f); influenceSum -= new Vector2(offset.X, offset.Z) * influenceMagnitude / distance; } - if(distance < 20) + if (distance < 20) { spooked = true; } @@ -53,7 +53,9 @@ public void Update(CharacterControllers characters, Simulation simulation, ref Q influenceSum /= newts.Count; ref var character = ref characters.GetCharacterByBodyHandle(bodyHandle); influenceSum -= (new Vector2(body.Pose.Position.X, body.Pose.Position.Z) - targetLocation) * 0.001f; - var targetWorldVelocity = 6f * Vector2.Normalize(influenceSum); + //Newts should do a good job at avoiding a division by zero here, but just in case, guard against it. + var influenceSumLength = influenceSum.Length(); + var targetWorldVelocity = influenceSumLength > 1e-6f ? influenceSum * (6f / influenceSumLength) : new Vector2(); //Rephrase the target velocity in terms of the character's control basis. character.TargetVelocity = new Vector2(targetWorldVelocity.X, -targetWorldVelocity.Y); if (spooked && random.NextDouble() < 0.015f) diff --git a/Demos/Demos/Sponsors/SponsorDemo.cs b/Demos/Demos/Sponsors/SponsorDemo.cs index ac51d4704..580129e21 100644 --- a/Demos/Demos/Sponsors/SponsorDemo.cs +++ b/Demos/Demos/Sponsors/SponsorDemo.cs @@ -239,7 +239,7 @@ public override void Update(Window window, Camera camera, Input input, float dt) } for (int i = 0; i < characterAIs.Count; ++i) { - characterAIs[i].Update(characterControllers, Simulation, ref newts, newtArenaMin, newtArenaMax, random); + characterAIs[i].Update(characterControllers, Simulation, ref newts, random); } simulationTime += TimestepDuration; realTime += dt; diff --git a/Demos/SpecializedTests/Media/2.4/NewtTyrannyDemo.cs b/Demos/SpecializedTests/Media/2.4/NewtTyrannyDemo.cs index 748a22508..2bb5ea1fb 100644 --- a/Demos/SpecializedTests/Media/2.4/NewtTyrannyDemo.cs +++ b/Demos/SpecializedTests/Media/2.4/NewtTyrannyDemo.cs @@ -101,7 +101,7 @@ public override void Update(Window window, Camera camera, Input input, float dt) } for (int i = 0; i < characterAIs.Count; ++i) { - characterAIs[i].Update(characterControllers, Simulation, ref newts, newtArenaMin, newtArenaMax, random); + characterAIs[i].Update(characterControllers, Simulation, ref newts, random); } simulationTime += TimestepDuration; From ca78f54aa06d5352f8fb55db7552b9e7bf0ab075 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 7 Apr 2023 14:01:04 -0500 Subject: [PATCH 711/947] Slight twist: refusing to do any other jobs unless the local worker is otherwise idle. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index d83962d74..253f395e8 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -580,7 +580,7 @@ unsafe static BoundingBox4 MultithreadedCentroidPrepass(MultithreadBinnedBuildCo //(Note: the centroid prepass only runs at the root, so we don't expect there to be any competition from other nodes *in this tree*, //but it's possible that the same taskstack is used from multiple binned builds. //Technically, there's potential interference from other user tasks that have nothing to do with binned building, but... not too concerned at this point.) - var jobFilter = new MinimumTagFilter(1); + var jobFilter = new MinimumTagFilter(2); context->TaskStack->For(&CentroidPrepassWorker, &taskContext, 0, taskCount, workerIndex, dispatcher, ref jobFilter, 1); var centroidBounds = taskContext.PrepassWorkers[0]; @@ -767,7 +767,7 @@ unsafe static void MultithreadedBinSubtrees(MultithreadBinnedBuildContext* conte //We only want the inner multithreading to work on small, non-recursive jobs. //Diving into a node at this point would stall the current node and favor more (and smaller) nodes. - var jobFilter = new MinimumTagFilter(1); + var jobFilter = new MinimumTagFilter(2); context->TaskStack->For(&BinSubtreesWorker, &taskContext, 0, taskContext.TaskData.TaskCount, workerIndex, dispatcher, ref jobFilter, 1); //Unless the number of threads and bins is really huge, there's no value in attempting to multithread the final compression. @@ -920,7 +920,7 @@ unsafe static (int subtreeCountA, int subtreeCountB) MultithreadedPartition(Mult subtrees, subtreesNext, binIndices, binSplitIndex); //We only want the inner multithreading to work on small, non-recursive jobs. //Diving into a node at this point would stall the current node and favor more (and smaller) nodes. - var jobFilter = new MinimumTagFilter(1); + var jobFilter = new MinimumTagFilter(2); context->TaskStack->For(&PartitionSubtreesWorker, &taskContext, 0, taskContext.TaskData.TaskCount, workerIndex, dispatcher, ref jobFilter, 1); return (taskContext.Counters.SubtreeCountA, taskContext.Counters.SubtreeCountB); } From 21cc0d2a5f57884ceaaa7e33ca6a3ca9a4cbfe61 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 5 May 2023 15:45:04 -0500 Subject: [PATCH 712/947] Fixed potential infinite loop in convex hull builder. --- BepuPhysics/Collidables/ConvexHullHelper.cs | 89 +- Demos/SpecializedTests/ConvexHullTestDemo.cs | 1017 +++++++++--------- 2 files changed, 581 insertions(+), 525 deletions(-) diff --git a/BepuPhysics/Collidables/ConvexHullHelper.cs b/BepuPhysics/Collidables/ConvexHullHelper.cs index 590f21f92..9b99af8f2 100644 --- a/BepuPhysics/Collidables/ConvexHullHelper.cs +++ b/BepuPhysics/Collidables/ConvexHullHelper.cs @@ -85,7 +85,8 @@ public void Dispose(BufferPool pool) /// public static class ConvexHullHelper { - static void FindExtremeFace(in Vector3Wide basisX, in Vector3Wide basisY, in Vector3Wide basisOrigin, in EdgeEndpoints sourceEdgeEndpoints, ref Buffer pointBundles, in Vector indexOffsets, int pointCount, + static void FindExtremeFace( + in Vector3Wide basisX, in Vector3Wide basisY, in Vector3Wide basisOrigin, in EdgeEndpoints sourceEdgeEndpoints, ref Buffer pointBundles, in Vector indexOffsets, Buffer allowVertices, int pointCount, ref Buffer> projectedOnX, ref Buffer> projectedOnY, in Vector planeEpsilon, ref QuickList vertexIndices, out Vector3 faceNormal) { Debug.Assert(projectedOnX.Length >= pointBundles.Length && projectedOnY.Length >= pointBundles.Length && vertexIndices.Count == 0 && vertexIndices.Span.Length >= pointBundles.Length * Vector.Count); @@ -111,11 +112,13 @@ static void FindExtremeFace(in Vector3Wide basisX, in Vector3Wide basisY, in Vec //Ignore the source edge. var edgeIndexA = new Vector(sourceEdgeEndpoints.A); var edgeIndexB = new Vector(sourceEdgeEndpoints.B); - var pointCountBundle = new Vector(pointCount); //Note that any slot that would have been coplanar with the generating face *and* behind the edge (that is, a vertex almost certainly associated with the generating face) is ignored. //Without this condition, it's possible for numerical cycles to occur where a face finds itself over and over again. + var allowVertexBundles = allowVertices.As>(); var ignoreSlot = Vector.BitwiseOr( - Vector.BitwiseOr(Vector.GreaterThanOrEqual(indexOffsets, pointCountBundle), Vector.BitwiseAnd(Vector.LessThanOrEqual(bestX, planeEpsilon), Vector.LessThanOrEqual(bestY, planeEpsilon))), + Vector.BitwiseOr( + Vector.OnesComplement(allowVertexBundles[0]), + Vector.BitwiseAnd(Vector.LessThanOrEqual(bestX, planeEpsilon), Vector.LessThanOrEqual(bestY, planeEpsilon))), Vector.BitwiseOr(Vector.Equals(indexOffsets, edgeIndexA), Vector.Equals(indexOffsets, edgeIndexB))); bestX = Vector.ConditionalSelect(ignoreSlot, Vector.One, bestX); bestY = Vector.ConditionalSelect(ignoreSlot, new Vector(float.MinValue), bestY); @@ -131,9 +134,12 @@ static void FindExtremeFace(in Vector3Wide basisX, in Vector3Wide basisY, in Vec var candidateIndices = indexOffsets + new Vector(i << BundleIndexing.VectorShift); ignoreSlot = Vector.BitwiseOr( - Vector.BitwiseOr(Vector.GreaterThanOrEqual(candidateIndices, pointCountBundle), Vector.BitwiseAnd(Vector.LessThanOrEqual(x, planeEpsilon), Vector.LessThanOrEqual(y, planeEpsilon))), + Vector.BitwiseOr( + Vector.OnesComplement(allowVertexBundles[i]), + Vector.BitwiseAnd(Vector.LessThanOrEqual(x, planeEpsilon), Vector.LessThanOrEqual(y, planeEpsilon))), Vector.BitwiseOr(Vector.Equals(candidateIndices, edgeIndexA), Vector.Equals(candidateIndices, edgeIndexB))); var useCandidate = Vector.AndNot(Vector.GreaterThan(y * bestX, bestY * x), ignoreSlot); + bestY = Vector.ConditionalSelect(useCandidate, y, bestY); bestX = Vector.ConditionalSelect(useCandidate, x, bestX); bestIndices = Vector.ConditionalSelect(useCandidate, candidateIndices, bestIndices); @@ -202,9 +208,10 @@ static void FindExtremeFace(in Vector3Wide basisX, in Vector3Wide basisY, in Vec localIndexMaximum = Vector.Count; for (int j = 0; j < localIndexMaximum; ++j) { - if (coplanar[j] < 0) + var vertexIndex = bundleBaseIndex + j; + if (coplanar[j] < 0 && allowVertices[vertexIndex] != 0) { - vertexIndices.AllocateUnsafely() = bundleBaseIndex + j; + vertexIndices.AllocateUnsafely() = vertexIndex; } } } @@ -279,12 +286,17 @@ static int FindNextIndexForFaceHull(Vector2 start, Vector2 previousEdgeDirection return bestIndex; } - static void ReduceFace(ref QuickList faceVertexIndices, Vector3 faceNormal, Span points, float planeEpsilon, ref QuickList facePoints, ref Buffer allowVertex, ref QuickList reducedIndices) + static void ReduceFace(ref QuickList faceVertexIndices, Vector3 faceNormal, Span points, float planeEpsilon, ref QuickList facePoints, ref Buffer allowVertex, ref QuickList reducedIndices) { Debug.Assert(facePoints.Count == 0 && reducedIndices.Count == 0 && facePoints.Span.Length >= faceVertexIndices.Count && reducedIndices.Span.Length >= faceVertexIndices.Count); for (int i = faceVertexIndices.Count - 1; i >= 0; --i) { - if (!allowVertex[faceVertexIndices[i]]) + //TODO: This isn't really necessary (conditioning on a small change). + //Face merges may see this codepath because the original rawFaceVertexIndices may contain now-disallowed vertices. + //We don't really need to *track* those, though; we could just use the reducedIndices and then this would never be required. + //It's mostly a matter of legacy- previously, we accumulated everything without asking about whether it was allowed, and relied on ReduceFace to clean it up. + //That opened a door for an infinite loop, so it got changed. + if (allowVertex[faceVertexIndices[i]] == 0) faceVertexIndices.RemoveAt(i); } if (faceVertexIndices.Count <= 3) @@ -309,18 +321,18 @@ static void ReduceFace(ref QuickList faceVertexIndices, Vector3 faceNormal, //The face is degenerate. if (ab.LengthSquared() > 1e-14f) { - allowVertex[reducedIndices[2]] = false; + allowVertex[reducedIndices[2]] = 0; reducedIndices.FastRemoveAt(2); } else if (ac.LengthSquared() > 1e-14f) { - allowVertex[reducedIndices[1]] = false; + allowVertex[reducedIndices[1]] = 0; reducedIndices.FastRemoveAt(1); } else { - allowVertex[reducedIndices[1]] = false; - allowVertex[reducedIndices[2]] = false; + allowVertex[reducedIndices[1]] = 0; + allowVertex[reducedIndices[2]] = 0; reducedIndices.Count = 1; } } @@ -360,7 +372,7 @@ static void ReduceFace(ref QuickList faceVertexIndices, Vector3 faceNormal, //The face is degenerate. for (int i = 0; i < faceVertexIndices.Count; ++i) { - allowVertex[faceVertexIndices[i]] = false; + allowVertex[faceVertexIndices[i]] = 0; } return; } @@ -389,7 +401,7 @@ static void ReduceFace(ref QuickList faceVertexIndices, Vector3 faceNormal, var index = faceVertexIndices[i]; if (!reducedIndices.Contains(index)) { - allowVertex[index] = false; + allowVertex[index] = 0; } } } @@ -556,7 +568,7 @@ static void AddIfNotPresent(ref QuickList list, int value, BufferPool pool) // public int FaceIndex; // public Vector3[] FaceNormals; - // internal DebugStep(EdgeEndpoints sourceEdge, ref QuickList raw, Vector3 faceNormal, Vector3 basisX, Vector3 basisY, ref QuickList reduced, ref Buffer allowVertex, ref QuickList faces, Span mergedFaceIndices, int faceIndex) + // internal DebugStep(EdgeEndpoints sourceEdge, ref QuickList raw, Vector3 faceNormal, Vector3 basisX, Vector3 basisY, ref QuickList reduced, ref Buffer allowVertex, ref QuickList faces, Span mergedFaceIndices, int faceIndex) // { // SourceEdge = sourceEdge; // FaceNormal = faceNormal; @@ -575,7 +587,7 @@ static void AddIfNotPresent(ref QuickList list, int value, BufferPool pool) // AllowVertex = new bool[allowVertex.Length]; // for (int i = 0; i < allowVertex.Length; ++i) // { - // AllowVertex[i] = allowVertex[i]; + // AllowVertex[i] = allowVertex[i] != 0; // } // FaceStarts = new List(faces.Count); // FaceIndices = new List(); @@ -612,7 +624,7 @@ static void AddIfNotPresent(ref QuickList list, int value, BufferPool pool) /// Point set to compute the convex hull of. /// Buffer pool to pull memory from when creating the hull. /// Convex hull of the input point set. - public static void ComputeHull(Span points, BufferPool pool, out HullData hullData)//, out List steps) + public unsafe static void ComputeHull(Span points, BufferPool pool, out HullData hullData)//, out List steps) { //steps = new List(); if (points.Length <= 0) @@ -712,22 +724,28 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa Vector3Wide.Broadcast(initialVertex, out var initialVertexBundle); pool.Take>(pointBundles.Length, out var projectedOnX); pool.Take>(pointBundles.Length, out var projectedOnY); - var planeEpsilonNarrow = MathF.Sqrt(bestDistanceSquared) * 1e-5f; - var normalCoplanarityEpsilon = 1f - 1e-5f; + var planeEpsilonNarrow = MathF.Sqrt(bestDistanceSquared) * 1e-6f; + var normalCoplanarityEpsilon = 1f - 1e-6f; var planeEpsilon = new Vector(planeEpsilonNarrow); var rawFaceVertexIndices = new QuickList(pointBundles.Length * Vector.Count, pool); var initialSourceEdge = new EdgeEndpoints { A = initialIndex, B = initialIndex }; - FindExtremeFace(initialBasisX, initialBasisY, initialVertexBundle, initialSourceEdge, ref pointBundles, indexOffsetBundle, points.Length, + + //Points found to not be on the face hull are ignored by future executions. + //Note that it's stored in integers instead of bools; it can be directly loaded as a mask during vectorized operations. + //0 means the vertex is disallowed, -1 means the vertex is allowed. + pool.Take(pointBundleCount * Vector.Count, out var allowVertices); + ((Span)allowVertices).Slice(0, points.Length).Fill(-1); + for (int i = points.Length; i < allowVertices.Length; ++i) + allowVertices[i] = 0; + + FindExtremeFace(initialBasisX, initialBasisY, initialVertexBundle, initialSourceEdge, ref pointBundles, indexOffsetBundle, allowVertices, points.Length, ref projectedOnX, ref projectedOnY, planeEpsilon, ref rawFaceVertexIndices, out var initialFaceNormal); Debug.Assert(rawFaceVertexIndices.Count >= 2); var facePoints = new QuickList(points.Length, pool); var reducedFaceIndices = new QuickList(points.Length, pool); - //Points found to not be on the face hull are ignored by future executions. - pool.Take(points.Length, out var allowVertex); - for (int i = 0; i < points.Length; ++i) - allowVertex[i] = true; - ReduceFace(ref rawFaceVertexIndices, initialFaceNormal, points, planeEpsilonNarrow, ref facePoints, ref allowVertex, ref reducedFaceIndices); + + ReduceFace(ref rawFaceVertexIndices, initialFaceNormal, points, planeEpsilonNarrow, ref facePoints, ref allowVertices, ref reducedFaceIndices); var faces = new QuickList(points.Length, pool); var edgesToTest = new QuickList(points.Length, pool); @@ -767,7 +785,7 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa } //Vector3Wide.ReadFirst(initialBasisX, out var debugInitialBasisX); //Vector3Wide.ReadFirst(initialBasisY, out var debugInitialBasisY); - //steps.Add(new DebugStep(initialSourceEdge, ref rawFaceVertexIndices, initialFaceNormal, debugInitialBasisX, debugInitialBasisY, ref reducedFaceIndices, ref allowVertex, ref faces, default, reducedFaceIndices.Count >= 3 ? 0 : -1)); + //steps.Add(new DebugStep(initialSourceEdge, ref rawFaceVertexIndices, initialFaceNormal, debugInitialBasisX, debugInitialBasisY, ref reducedFaceIndices, ref allowVertices, ref faces, default, reducedFaceIndices.Count >= 3 ? 0 : -1)); int facesDeletedCount = 0; @@ -793,13 +811,14 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa Vector3Wide.Broadcast(basisY, out var basisYBundle); Vector3Wide.Broadcast(edgeA, out var basisOrigin); rawFaceVertexIndices.Count = 0; - FindExtremeFace(basisXBundle, basisYBundle, basisOrigin, edgeToTest.Endpoints, ref pointBundles, indexOffsetBundle, points.Length, ref projectedOnX, ref projectedOnY, planeEpsilon, ref rawFaceVertexIndices, out var faceNormal); + FindExtremeFace(basisXBundle, basisYBundle, basisOrigin, edgeToTest.Endpoints, ref pointBundles, indexOffsetBundle, allowVertices, points.Length, ref projectedOnX, ref projectedOnY, planeEpsilon, ref rawFaceVertexIndices, out var faceNormal); reducedFaceIndices.Count = 0; facePoints.Count = 0; - ReduceFace(ref rawFaceVertexIndices, faceNormal, points, planeEpsilonNarrow, ref facePoints, ref allowVertex, ref reducedFaceIndices); + ReduceFace(ref rawFaceVertexIndices, faceNormal, points, planeEpsilonNarrow, ref facePoints, ref allowVertices, ref reducedFaceIndices); + if (reducedFaceIndices.Count < 3) { - //steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertex, ref faces, default, -1)); + //steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertices, ref faces, default, -1)); //Degenerate face found; don't bother creating work for it. continue; } @@ -872,9 +891,10 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa ref var sourceFace = ref faces[facesNeedingMerge[i]]; for (int j = 0; j < sourceFace.VertexIndices.Count; ++j) { - if (!rawFaceVertexIndices.Contains(sourceFace.VertexIndices[j])) + var vertexIndex = sourceFace.VertexIndices[j]; + if (allowVertices[vertexIndex] != 0 && !rawFaceVertexIndices.Contains(vertexIndex)) { - rawFaceVertexIndices.Allocate(pool) = sourceFace.VertexIndices[j]; + rawFaceVertexIndices.Allocate(pool) = vertexIndex; } } } @@ -882,7 +902,7 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa //Re-reduce. reducedFaceIndices.Count = 0; facePoints.Count = 0; - ReduceFace(ref rawFaceVertexIndices, faceNormal, points, planeEpsilonNarrow, ref facePoints, ref allowVertex, ref reducedFaceIndices); + ReduceFace(ref rawFaceVertexIndices, faceNormal, points, planeEpsilonNarrow, ref facePoints, ref allowVertices, ref reducedFaceIndices); } else { @@ -924,7 +944,7 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa AddFace(ref faces, pool, faceNormal, reducedFaceIndices); AddFaceToEdgesAndTestList(pool, ref reducedFaceIndices, ref edgesToTest, ref facesForEdges, faceNormal, faceCountPriorToAdd); - //steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertex, ref faces, facesNeedingMerge, faceCountPriorToAdd)); + //steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertices, ref faces, facesNeedingMerge, faceCountPriorToAdd)); break; } } @@ -935,7 +955,7 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa facePoints.Dispose(pool); reducedFaceIndices.Dispose(pool); rawFaceVertexIndices.Dispose(pool); - pool.Return(ref allowVertex); + pool.Return(ref allowVertices); pool.Return(ref projectedOnX); pool.Return(ref projectedOnY); pool.Return(ref pointBundles); @@ -1033,6 +1053,7 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa /// Computed center of mass of the convex hull before its points were recentered onto the origin. public static void CreateShape(Span points, HullData hullData, BufferPool pool, out Vector3 center, out ConvexHull hullShape) { + Debug.Assert(points.Length > 0, "Convex hulls need to have a nonzero number of points!"); hullShape = default; if (hullData.OriginalVertexMapping.Length < 3) { diff --git a/Demos/SpecializedTests/ConvexHullTestDemo.cs b/Demos/SpecializedTests/ConvexHullTestDemo.cs index 370d21de8..82b2a2548 100644 --- a/Demos/SpecializedTests/ConvexHullTestDemo.cs +++ b/Demos/SpecializedTests/ConvexHullTestDemo.cs @@ -17,536 +17,571 @@ using BepuPhysics.Constraints; using Demos.Demos; using BepuUtilities.Memory; +using System.Text.Json; +using System.IO; -namespace Demos.SpecializedTests +namespace Demos.SpecializedTests; + +public class ConvexHullTestDemo : Demo { - public class ConvexHullTestDemo : Demo + unsafe Buffer CreateRandomConvexHullPoints() { - unsafe Buffer CreateRandomConvexHullPoints() - { - const int pointCount = 50; - BufferPool.Take(pointCount, out var points); - - var random = new Random(5); - for (int i = 0; i < pointCount; ++i) - { - points[i] = new Vector3(3 * random.NextSingle(), 1 * random.NextSingle(), 3 * random.NextSingle()); - } - - return points; - } - - unsafe Buffer CreateMeshConvexHull(MeshContent meshContent, Vector3 scale) - { - //This is actually a pretty good example of how *not* to make a convex hull shape. - //Generating it directly from a graphical data source tends to have way more surface complexity than needed, - //and it tends to have a lot of near-but-not-quite-coplanar surfaces which can make the contact manifold less stable. - //Prefer a simpler source with more distinct features, possibly created with an automated content-time tool. - BufferPool.Take(meshContent.Triangles.Length * 3, out var points); - for (int i = 0; i < meshContent.Triangles.Length; ++i) - { - ref var triangle = ref meshContent.Triangles[i]; - //resisting the urge to just reinterpret the memory - points[i * 3 + 0] = triangle.A * scale; - points[i * 3 + 1] = triangle.B * scale; - points[i * 3 + 2] = triangle.C * scale; - } - return points; - } + const int pointCount = 50; + BufferPool.Take(pointCount, out var points); - unsafe Buffer CreateBoxConvexHull(float boxScale) + var random = new Random(5); + for (int i = 0; i < pointCount; ++i) { - BufferPool.Take(8, out var points); - points[0] = new Vector3(0, 0, 0); - points[1] = new Vector3(0, 0, boxScale); - points[2] = new Vector3(0, boxScale, 0); - points[3] = new Vector3(0, boxScale, boxScale); - points[4] = new Vector3(boxScale, 0, 0); - points[5] = new Vector3(boxScale, 0, boxScale); - points[6] = new Vector3(boxScale, boxScale, 0); - points[7] = new Vector3(boxScale, boxScale, boxScale); - return points; + points[i] = new Vector3(3 * random.NextSingle(), 1 * random.NextSingle(), 3 * random.NextSingle()); } - //A couple of test point sets from PEEL: https://github.com/Pierre-Terdiman/PEEL_PhysX_Edition - unsafe Buffer CreateTestConvexHull() - { - BufferPool.Take(50, out var vertices); - vertices[0] = new Vector3(-0.000000f, -0.297120f, -0.000000f); - vertices[1] = new Vector3(0.258819f, -0.297120f, 0.965926f); - vertices[2] = new Vector3(-0.000000f, -0.297120f, 1.000000f); - vertices[3] = new Vector3(0.500000f, -0.297120f, 0.866026f); - vertices[4] = new Vector3(0.707107f, -0.297120f, 0.707107f); - vertices[5] = new Vector3(0.866026f, -0.297120f, 0.500000f); - vertices[6] = new Vector3(0.965926f, -0.297120f, 0.258819f); - vertices[7] = new Vector3(1.000000f, -0.297120f, -0.000000f); - vertices[8] = new Vector3(0.965926f, -0.297120f, -0.258819f); - vertices[9] = new Vector3(0.866026f, -0.297120f, -0.500000f); - vertices[10] = new Vector3(0.707107f, -0.297120f, -0.707107f); - vertices[11] = new Vector3(0.500000f, -0.297120f, -0.866026f); - vertices[12] = new Vector3(0.258819f, -0.297120f, -0.965926f); - vertices[13] = new Vector3(-0.000000f, -0.297120f, -1.000000f); - vertices[14] = new Vector3(-0.258819f, -0.297120f, -0.965926f); - vertices[15] = new Vector3(-0.500000f, -0.297120f, -0.866025f); - vertices[16] = new Vector3(-0.707107f, -0.297120f, -0.707107f); - vertices[17] = new Vector3(-0.866026f, -0.297120f, -0.500000f); - vertices[18] = new Vector3(-0.965926f, -0.297120f, -0.258819f); - vertices[19] = new Vector3(-1.000000f, -0.297120f, 0.000000f); - vertices[20] = new Vector3(-0.965926f, -0.297120f, 0.258819f); - vertices[21] = new Vector3(-0.866025f, -0.297120f, 0.500000f); - vertices[22] = new Vector3(-0.707107f, -0.297120f, 0.707107f); - vertices[23] = new Vector3(-0.500000f, -0.297120f, 0.866026f); - vertices[24] = new Vector3(-0.258819f, -0.297120f, 0.965926f); - vertices[25] = new Vector3(-0.000000f, 0.297120f, -0.000000f); - vertices[26] = new Vector3(-0.000000f, 0.297120f, 0.537813f); - vertices[27] = new Vector3(0.139196f, 0.297120f, 0.519487f); - vertices[28] = new Vector3(0.268907f, 0.297120f, 0.465760f); - vertices[29] = new Vector3(0.380291f, 0.297120f, 0.380291f); - vertices[30] = new Vector3(0.465760f, 0.297120f, 0.268907f); - vertices[31] = new Vector3(0.519487f, 0.297120f, 0.139196f); - vertices[32] = new Vector3(0.537813f, 0.297120f, -0.000000f); - vertices[33] = new Vector3(0.519487f, 0.297120f, -0.139196f); - vertices[34] = new Vector3(0.465760f, 0.297120f, -0.268907f); - vertices[35] = new Vector3(0.380291f, 0.297120f, -0.380291f); - vertices[36] = new Vector3(0.268907f, 0.297120f, -0.465760f); - vertices[37] = new Vector3(0.139196f, 0.297120f, -0.519487f); - vertices[38] = new Vector3(-0.000000f, 0.297120f, -0.537813f); - vertices[39] = new Vector3(-0.139196f, 0.297120f, -0.519487f); - vertices[40] = new Vector3(-0.268907f, 0.297120f, -0.465760f); - vertices[41] = new Vector3(-0.380291f, 0.297120f, -0.380291f); - vertices[42] = new Vector3(-0.465760f, 0.297120f, -0.268907f); - vertices[43] = new Vector3(-0.519487f, 0.297120f, -0.139196f); - vertices[44] = new Vector3(-0.537813f, 0.297120f, 0.000000f); - vertices[45] = new Vector3(-0.519487f, 0.297120f, 0.139196f); - vertices[46] = new Vector3(-0.465760f, 0.297120f, 0.268907f); - vertices[47] = new Vector3(-0.380291f, 0.297120f, 0.380291f); - vertices[48] = new Vector3(-0.268907f, 0.297120f, 0.465760f); - vertices[49] = new Vector3(-0.139196f, 0.297120f, 0.519487f); - return vertices; - } + return points; + } - unsafe Buffer CreateTestConvexHull2() + unsafe Buffer CreateMeshConvexHull(MeshContent meshContent, Vector3 scale) + { + //This is actually a pretty good example of how *not* to make a convex hull shape. + //Generating it directly from a graphical data source tends to have way more surface complexity than needed, + //and it tends to have a lot of near-but-not-quite-coplanar surfaces which can make the contact manifold less stable. + //Prefer a simpler source with more distinct features, possibly created with an automated content-time tool. + BufferPool.Take(meshContent.Triangles.Length * 3, out var points); + for (int i = 0; i < meshContent.Triangles.Length; ++i) { - BufferPool.Take(120, out var vertices); - vertices[0] = new Vector3(0.153478f, 0.993671f, 0.124687f); - vertices[1] = new Vector3(0.153478f, 0.993671f, -0.117774f); - vertices[2] = new Vector3(-0.147939f, 0.993671f, -0.117774f); - vertices[3] = new Vector3(-0.147939f, 0.993671f, 0.124687f); - vertices[4] = new Vector3(0.137286f, 0.817392f, 0.586192f); - vertices[5] = new Vector3(0.333441f, 0.696161f, 0.661116f); - vertices[6] = new Vector3(0.484149f, 0.789305f, 0.417265f); - vertices[7] = new Vector3(0.287995f, 0.910536f, 0.342339f); - vertices[8] = new Vector3(0.794945f, 0.410936f, 0.484838f); - vertices[9] = new Vector3(0.916176f, 0.336012f, 0.288682f); - vertices[10] = new Vector3(0.823033f, 0.579863f, 0.137973f); - vertices[11] = new Vector3(0.701803f, 0.654787f, 0.334128f); - vertices[12] = new Vector3(0.916176f, 0.336012f, -0.281770f); - vertices[13] = new Vector3(0.794945f, 0.410936f, -0.477925f); - vertices[14] = new Vector3(0.701803f, 0.654787f, -0.327216f); - vertices[15] = new Vector3(0.823033f, 0.579863f, -0.131060f); - vertices[16] = new Vector3(0.333441f, 0.696161f, -0.654204f); - vertices[17] = new Vector3(0.137286f, 0.817392f, -0.579280f); - vertices[18] = new Vector3(0.287995f, 0.910536f, -0.335426f); - vertices[19] = new Vector3(0.484149f, 0.789305f, -0.410352f); - vertices[20] = new Vector3(-0.131747f, 0.817392f, -0.579280f); - vertices[21] = new Vector3(-0.327903f, 0.696161f, -0.654204f); - vertices[22] = new Vector3(-0.478612f, 0.789305f, -0.410352f); - vertices[23] = new Vector3(-0.282457f, 0.910536f, -0.335426f); - vertices[24] = new Vector3(-0.789408f, 0.410936f, -0.477925f); - vertices[25] = new Vector3(-0.910638f, 0.336012f, -0.281770f); - vertices[26] = new Vector3(-0.817496f, 0.579863f, -0.131060f); - vertices[27] = new Vector3(-0.696265f, 0.654787f, -0.327216f); - vertices[28] = new Vector3(-0.910638f, 0.336012f, 0.288682f); - vertices[29] = new Vector3(-0.789408f, 0.410936f, 0.484838f); - vertices[30] = new Vector3(-0.696265f, 0.654787f, 0.334128f); - vertices[31] = new Vector3(-0.817496f, 0.579863f, 0.137973f); - vertices[32] = new Vector3(-0.327903f, 0.696161f, 0.661116f); - vertices[33] = new Vector3(-0.131747f, 0.817392f, 0.586192f); - vertices[34] = new Vector3(-0.282457f, 0.910536f, 0.342339f); - vertices[35] = new Vector3(-0.478612f, 0.789305f, 0.417265f); - vertices[36] = new Vector3(0.416578f, 0.478508f, 0.795634f); - vertices[37] = new Vector3(0.341652f, 0.282353f, 0.916863f); - vertices[38] = new Vector3(0.585505f, 0.131646f, 0.823721f); - vertices[39] = new Vector3(0.660429f, 0.327801f, 0.702490f); - vertices[40] = new Vector3(0.124000f, 0.147837f, 1.000000f); - vertices[41] = new Vector3(-0.118461f, 0.147837f, 1.000000f); - vertices[42] = new Vector3(-0.118461f, -0.153580f, 1.000000f); - vertices[43] = new Vector3(0.124000f, -0.153580f, 1.000000f); - vertices[44] = new Vector3(-0.336113f, 0.282353f, 0.916863f); - vertices[45] = new Vector3(-0.411039f, 0.478508f, 0.795634f); - vertices[46] = new Vector3(-0.654891f, 0.327801f, 0.702490f); - vertices[47] = new Vector3(-0.579966f, 0.131646f, 0.823721f); - vertices[48] = new Vector3(-0.993774f, 0.118359f, -0.147252f); - vertices[49] = new Vector3(-0.993774f, -0.124103f, -0.147252f); - vertices[50] = new Vector3(-0.993774f, -0.124103f, 0.154165f); - vertices[51] = new Vector3(-0.993774f, 0.118359f, 0.154165f); - vertices[52] = new Vector3(-0.817496f, -0.585607f, 0.137973f); - vertices[53] = new Vector3(-0.696265f, -0.660531f, 0.334128f); - vertices[54] = new Vector3(-0.789408f, -0.416680f, 0.484838f); - vertices[55] = new Vector3(-0.910638f, -0.341756f, 0.288682f); - vertices[56] = new Vector3(-0.411039f, -0.484253f, 0.795634f); - vertices[57] = new Vector3(-0.336113f, -0.288097f, 0.916863f); - vertices[58] = new Vector3(-0.579966f, -0.137388f, 0.823721f); - vertices[59] = new Vector3(-0.654891f, -0.333543f, 0.702490f); - vertices[60] = new Vector3(0.341652f, -0.288097f, 0.916863f); - vertices[61] = new Vector3(0.416578f, -0.484253f, 0.795634f); - vertices[62] = new Vector3(0.660429f, -0.333543f, 0.702490f); - vertices[63] = new Vector3(0.585505f, -0.137388f, 0.823721f); - vertices[64] = new Vector3(0.333441f, -0.701905f, 0.661116f); - vertices[65] = new Vector3(0.137286f, -0.823136f, 0.586192f); - vertices[66] = new Vector3(0.287995f, -0.916278f, 0.342339f); - vertices[67] = new Vector3(0.484149f, -0.795049f, 0.417265f); - vertices[68] = new Vector3(-0.131747f, -0.823136f, 0.586192f); - vertices[69] = new Vector3(-0.327903f, -0.701905f, 0.661116f); - vertices[70] = new Vector3(-0.478612f, -0.795049f, 0.417265f); - vertices[71] = new Vector3(-0.282457f, -0.916278f, 0.342339f); - vertices[72] = new Vector3(-0.910638f, -0.341756f, -0.281770f); - vertices[73] = new Vector3(-0.789408f, -0.416680f, -0.477925f); - vertices[74] = new Vector3(-0.696265f, -0.660531f, -0.327216f); - vertices[75] = new Vector3(-0.817496f, -0.585607f, -0.131060f); - vertices[76] = new Vector3(-0.327903f, -0.701905f, -0.654204f); - vertices[77] = new Vector3(-0.131747f, -0.823136f, -0.579280f); - vertices[78] = new Vector3(-0.282457f, -0.916278f, -0.335426f); - vertices[79] = new Vector3(-0.478612f, -0.795049f, -0.410352f); - vertices[80] = new Vector3(0.153478f, -0.999415f, -0.117774f); - vertices[81] = new Vector3(0.153478f, -0.999415f, 0.124687f); - vertices[82] = new Vector3(-0.147939f, -0.999415f, 0.124687f); - vertices[83] = new Vector3(-0.147939f, -0.999415f, -0.117774f); - vertices[84] = new Vector3(0.701803f, -0.660531f, 0.334128f); - vertices[85] = new Vector3(0.823033f, -0.585607f, 0.137973f); - vertices[86] = new Vector3(0.916176f, -0.341756f, 0.288682f); - vertices[87] = new Vector3(0.794945f, -0.416680f, 0.484838f); - vertices[88] = new Vector3(0.823033f, -0.585607f, -0.131060f); - vertices[89] = new Vector3(0.701803f, -0.660531f, -0.327216f); - vertices[90] = new Vector3(0.794945f, -0.416680f, -0.477925f); - vertices[91] = new Vector3(0.916176f, -0.341756f, -0.281770f); - vertices[92] = new Vector3(0.484149f, -0.795049f, -0.410352f); - vertices[93] = new Vector3(0.287995f, -0.916278f, -0.335426f); - vertices[94] = new Vector3(0.137286f, -0.823136f, -0.579280f); - vertices[95] = new Vector3(0.333441f, -0.701905f, -0.654204f); - vertices[96] = new Vector3(-0.654891f, -0.333543f, -0.695578f); - vertices[97] = new Vector3(-0.579966f, -0.137388f, -0.816807f); - vertices[98] = new Vector3(-0.336113f, -0.288097f, -0.909951f); - vertices[99] = new Vector3(-0.411039f, -0.484253f, -0.788719f); - vertices[100] = new Vector3(-0.118461f, 0.147837f, -0.993087f); - vertices[101] = new Vector3(0.124000f, 0.147837f, -0.993087f); - vertices[102] = new Vector3(0.124000f, -0.153580f, -0.993087f); - vertices[103] = new Vector3(-0.118461f, -0.153580f, -0.993087f); - vertices[104] = new Vector3(0.585505f, -0.137388f, -0.816807f); - vertices[105] = new Vector3(0.660429f, -0.333543f, -0.695578f); - vertices[106] = new Vector3(0.416578f, -0.484253f, -0.788719f); - vertices[107] = new Vector3(0.341652f, -0.288097f, -0.909951f); - vertices[108] = new Vector3(0.999313f, -0.124103f, -0.147252f); - vertices[109] = new Vector3(0.999313f, 0.118359f, -0.147252f); - vertices[110] = new Vector3(0.999313f, 0.118359f, 0.154165f); - vertices[111] = new Vector3(0.999313f, -0.124103f, 0.154165f); - vertices[112] = new Vector3(0.660429f, 0.327801f, -0.695578f); - vertices[113] = new Vector3(0.585505f, 0.131646f, -0.816807f); - vertices[114] = new Vector3(0.341652f, 0.282353f, -0.909951f); - vertices[115] = new Vector3(0.416578f, 0.478508f, -0.788719f); - vertices[116] = new Vector3(-0.579966f, 0.131646f, -0.816807f); - vertices[117] = new Vector3(-0.654891f, 0.327801f, -0.695578f); - vertices[118] = new Vector3(-0.411039f, 0.478508f, -0.788719f); - vertices[119] = new Vector3(-0.336113f, 0.282353f, -0.909951f); - return vertices; + ref var triangle = ref meshContent.Triangles[i]; + //resisting the urge to just reinterpret the memory + points[i * 3 + 0] = triangle.A * scale; + points[i * 3 + 1] = triangle.B * scale; + points[i * 3 + 2] = triangle.C * scale; } + return points; + } + unsafe Buffer CreateBoxConvexHull(float boxScale) + { + BufferPool.Take(8, out var points); + points[0] = new Vector3(0, 0, 0); + points[1] = new Vector3(0, 0, boxScale); + points[2] = new Vector3(0, boxScale, 0); + points[3] = new Vector3(0, boxScale, boxScale); + points[4] = new Vector3(boxScale, 0, 0); + points[5] = new Vector3(boxScale, 0, boxScale); + points[6] = new Vector3(boxScale, boxScale, 0); + points[7] = new Vector3(boxScale, boxScale, boxScale); + return points; + } - unsafe Buffer CreateTestConvexHull3() - { - BufferPool.Take(22, out var vertices); - vertices[0] = new Vector3(-0.103558f, 1.000000f, -0.490575f); - vertices[1] = new Vector3(0.266493f, 0.659794f, -0.363751f); - vertices[2] = new Vector3(-0.245774f, 0.762636f, -0.615304f); - vertices[3] = new Vector3(0.164688f, -0.777634f, -0.365919f); - vertices[4] = new Vector3(0.503268f, -0.846406f, -0.131286f); - vertices[5] = new Vector3(0.171066f, -0.931723f, -0.140738f); - vertices[6] = new Vector3(-0.247963f, -0.738059f, -0.413146f); - vertices[7] = new Vector3(-0.319203f, -0.260078f, -0.609331f); - vertices[8] = new Vector3(0.469624f, -0.747848f, -0.286486f); - vertices[9] = new Vector3(0.398526f, -0.238233f, -0.435281f); - vertices[10] = new Vector3(0.448274f, 0.295416f, -0.246327f); - vertices[11] = new Vector3(-0.245774f, 0.762636f, 0.596521f); - vertices[12] = new Vector3(0.266493f, 0.659794f, 0.344974f); - vertices[13] = new Vector3(-0.103558f, 1.000000f, 0.471792f); - vertices[14] = new Vector3(0.171066f, -0.931723f, 0.121961f); - vertices[15] = new Vector3(0.503268f, -0.846406f, 0.112509f); - vertices[16] = new Vector3(0.164688f, -0.777634f, 0.347137f); - vertices[17] = new Vector3(-0.319203f, -0.260078f, 0.590548f); - vertices[18] = new Vector3(-0.247963f, -0.738059f, 0.394364f); - vertices[19] = new Vector3(0.469624f, -0.747848f, 0.267709f); - vertices[20] = new Vector3(0.398526f, -0.238233f, 0.416498f); - vertices[21] = new Vector3(0.448274f, 0.295411f, 0.227550f); - return vertices; - } + //A couple of test point sets from PEEL: https://github.com/Pierre-Terdiman/PEEL_PhysX_Edition + unsafe Buffer CreateTestConvexHull() + { + BufferPool.Take(50, out var vertices); + vertices[0] = new Vector3(-0.000000f, -0.297120f, -0.000000f); + vertices[1] = new Vector3(0.258819f, -0.297120f, 0.965926f); + vertices[2] = new Vector3(-0.000000f, -0.297120f, 1.000000f); + vertices[3] = new Vector3(0.500000f, -0.297120f, 0.866026f); + vertices[4] = new Vector3(0.707107f, -0.297120f, 0.707107f); + vertices[5] = new Vector3(0.866026f, -0.297120f, 0.500000f); + vertices[6] = new Vector3(0.965926f, -0.297120f, 0.258819f); + vertices[7] = new Vector3(1.000000f, -0.297120f, -0.000000f); + vertices[8] = new Vector3(0.965926f, -0.297120f, -0.258819f); + vertices[9] = new Vector3(0.866026f, -0.297120f, -0.500000f); + vertices[10] = new Vector3(0.707107f, -0.297120f, -0.707107f); + vertices[11] = new Vector3(0.500000f, -0.297120f, -0.866026f); + vertices[12] = new Vector3(0.258819f, -0.297120f, -0.965926f); + vertices[13] = new Vector3(-0.000000f, -0.297120f, -1.000000f); + vertices[14] = new Vector3(-0.258819f, -0.297120f, -0.965926f); + vertices[15] = new Vector3(-0.500000f, -0.297120f, -0.866025f); + vertices[16] = new Vector3(-0.707107f, -0.297120f, -0.707107f); + vertices[17] = new Vector3(-0.866026f, -0.297120f, -0.500000f); + vertices[18] = new Vector3(-0.965926f, -0.297120f, -0.258819f); + vertices[19] = new Vector3(-1.000000f, -0.297120f, 0.000000f); + vertices[20] = new Vector3(-0.965926f, -0.297120f, 0.258819f); + vertices[21] = new Vector3(-0.866025f, -0.297120f, 0.500000f); + vertices[22] = new Vector3(-0.707107f, -0.297120f, 0.707107f); + vertices[23] = new Vector3(-0.500000f, -0.297120f, 0.866026f); + vertices[24] = new Vector3(-0.258819f, -0.297120f, 0.965926f); + vertices[25] = new Vector3(-0.000000f, 0.297120f, -0.000000f); + vertices[26] = new Vector3(-0.000000f, 0.297120f, 0.537813f); + vertices[27] = new Vector3(0.139196f, 0.297120f, 0.519487f); + vertices[28] = new Vector3(0.268907f, 0.297120f, 0.465760f); + vertices[29] = new Vector3(0.380291f, 0.297120f, 0.380291f); + vertices[30] = new Vector3(0.465760f, 0.297120f, 0.268907f); + vertices[31] = new Vector3(0.519487f, 0.297120f, 0.139196f); + vertices[32] = new Vector3(0.537813f, 0.297120f, -0.000000f); + vertices[33] = new Vector3(0.519487f, 0.297120f, -0.139196f); + vertices[34] = new Vector3(0.465760f, 0.297120f, -0.268907f); + vertices[35] = new Vector3(0.380291f, 0.297120f, -0.380291f); + vertices[36] = new Vector3(0.268907f, 0.297120f, -0.465760f); + vertices[37] = new Vector3(0.139196f, 0.297120f, -0.519487f); + vertices[38] = new Vector3(-0.000000f, 0.297120f, -0.537813f); + vertices[39] = new Vector3(-0.139196f, 0.297120f, -0.519487f); + vertices[40] = new Vector3(-0.268907f, 0.297120f, -0.465760f); + vertices[41] = new Vector3(-0.380291f, 0.297120f, -0.380291f); + vertices[42] = new Vector3(-0.465760f, 0.297120f, -0.268907f); + vertices[43] = new Vector3(-0.519487f, 0.297120f, -0.139196f); + vertices[44] = new Vector3(-0.537813f, 0.297120f, 0.000000f); + vertices[45] = new Vector3(-0.519487f, 0.297120f, 0.139196f); + vertices[46] = new Vector3(-0.465760f, 0.297120f, 0.268907f); + vertices[47] = new Vector3(-0.380291f, 0.297120f, 0.380291f); + vertices[48] = new Vector3(-0.268907f, 0.297120f, 0.465760f); + vertices[49] = new Vector3(-0.139196f, 0.297120f, 0.519487f); + return vertices; + } - public unsafe override void Initialize(ContentArchive content, Camera camera) - { - camera.Position = new Vector3(0, -2.5f, 10); - camera.Yaw = 0; - camera.Pitch = 0; + unsafe Buffer CreateTestConvexHull2() + { + BufferPool.Take(120, out var vertices); + vertices[0] = new Vector3(0.153478f, 0.993671f, 0.124687f); + vertices[1] = new Vector3(0.153478f, 0.993671f, -0.117774f); + vertices[2] = new Vector3(-0.147939f, 0.993671f, -0.117774f); + vertices[3] = new Vector3(-0.147939f, 0.993671f, 0.124687f); + vertices[4] = new Vector3(0.137286f, 0.817392f, 0.586192f); + vertices[5] = new Vector3(0.333441f, 0.696161f, 0.661116f); + vertices[6] = new Vector3(0.484149f, 0.789305f, 0.417265f); + vertices[7] = new Vector3(0.287995f, 0.910536f, 0.342339f); + vertices[8] = new Vector3(0.794945f, 0.410936f, 0.484838f); + vertices[9] = new Vector3(0.916176f, 0.336012f, 0.288682f); + vertices[10] = new Vector3(0.823033f, 0.579863f, 0.137973f); + vertices[11] = new Vector3(0.701803f, 0.654787f, 0.334128f); + vertices[12] = new Vector3(0.916176f, 0.336012f, -0.281770f); + vertices[13] = new Vector3(0.794945f, 0.410936f, -0.477925f); + vertices[14] = new Vector3(0.701803f, 0.654787f, -0.327216f); + vertices[15] = new Vector3(0.823033f, 0.579863f, -0.131060f); + vertices[16] = new Vector3(0.333441f, 0.696161f, -0.654204f); + vertices[17] = new Vector3(0.137286f, 0.817392f, -0.579280f); + vertices[18] = new Vector3(0.287995f, 0.910536f, -0.335426f); + vertices[19] = new Vector3(0.484149f, 0.789305f, -0.410352f); + vertices[20] = new Vector3(-0.131747f, 0.817392f, -0.579280f); + vertices[21] = new Vector3(-0.327903f, 0.696161f, -0.654204f); + vertices[22] = new Vector3(-0.478612f, 0.789305f, -0.410352f); + vertices[23] = new Vector3(-0.282457f, 0.910536f, -0.335426f); + vertices[24] = new Vector3(-0.789408f, 0.410936f, -0.477925f); + vertices[25] = new Vector3(-0.910638f, 0.336012f, -0.281770f); + vertices[26] = new Vector3(-0.817496f, 0.579863f, -0.131060f); + vertices[27] = new Vector3(-0.696265f, 0.654787f, -0.327216f); + vertices[28] = new Vector3(-0.910638f, 0.336012f, 0.288682f); + vertices[29] = new Vector3(-0.789408f, 0.410936f, 0.484838f); + vertices[30] = new Vector3(-0.696265f, 0.654787f, 0.334128f); + vertices[31] = new Vector3(-0.817496f, 0.579863f, 0.137973f); + vertices[32] = new Vector3(-0.327903f, 0.696161f, 0.661116f); + vertices[33] = new Vector3(-0.131747f, 0.817392f, 0.586192f); + vertices[34] = new Vector3(-0.282457f, 0.910536f, 0.342339f); + vertices[35] = new Vector3(-0.478612f, 0.789305f, 0.417265f); + vertices[36] = new Vector3(0.416578f, 0.478508f, 0.795634f); + vertices[37] = new Vector3(0.341652f, 0.282353f, 0.916863f); + vertices[38] = new Vector3(0.585505f, 0.131646f, 0.823721f); + vertices[39] = new Vector3(0.660429f, 0.327801f, 0.702490f); + vertices[40] = new Vector3(0.124000f, 0.147837f, 1.000000f); + vertices[41] = new Vector3(-0.118461f, 0.147837f, 1.000000f); + vertices[42] = new Vector3(-0.118461f, -0.153580f, 1.000000f); + vertices[43] = new Vector3(0.124000f, -0.153580f, 1.000000f); + vertices[44] = new Vector3(-0.336113f, 0.282353f, 0.916863f); + vertices[45] = new Vector3(-0.411039f, 0.478508f, 0.795634f); + vertices[46] = new Vector3(-0.654891f, 0.327801f, 0.702490f); + vertices[47] = new Vector3(-0.579966f, 0.131646f, 0.823721f); + vertices[48] = new Vector3(-0.993774f, 0.118359f, -0.147252f); + vertices[49] = new Vector3(-0.993774f, -0.124103f, -0.147252f); + vertices[50] = new Vector3(-0.993774f, -0.124103f, 0.154165f); + vertices[51] = new Vector3(-0.993774f, 0.118359f, 0.154165f); + vertices[52] = new Vector3(-0.817496f, -0.585607f, 0.137973f); + vertices[53] = new Vector3(-0.696265f, -0.660531f, 0.334128f); + vertices[54] = new Vector3(-0.789408f, -0.416680f, 0.484838f); + vertices[55] = new Vector3(-0.910638f, -0.341756f, 0.288682f); + vertices[56] = new Vector3(-0.411039f, -0.484253f, 0.795634f); + vertices[57] = new Vector3(-0.336113f, -0.288097f, 0.916863f); + vertices[58] = new Vector3(-0.579966f, -0.137388f, 0.823721f); + vertices[59] = new Vector3(-0.654891f, -0.333543f, 0.702490f); + vertices[60] = new Vector3(0.341652f, -0.288097f, 0.916863f); + vertices[61] = new Vector3(0.416578f, -0.484253f, 0.795634f); + vertices[62] = new Vector3(0.660429f, -0.333543f, 0.702490f); + vertices[63] = new Vector3(0.585505f, -0.137388f, 0.823721f); + vertices[64] = new Vector3(0.333441f, -0.701905f, 0.661116f); + vertices[65] = new Vector3(0.137286f, -0.823136f, 0.586192f); + vertices[66] = new Vector3(0.287995f, -0.916278f, 0.342339f); + vertices[67] = new Vector3(0.484149f, -0.795049f, 0.417265f); + vertices[68] = new Vector3(-0.131747f, -0.823136f, 0.586192f); + vertices[69] = new Vector3(-0.327903f, -0.701905f, 0.661116f); + vertices[70] = new Vector3(-0.478612f, -0.795049f, 0.417265f); + vertices[71] = new Vector3(-0.282457f, -0.916278f, 0.342339f); + vertices[72] = new Vector3(-0.910638f, -0.341756f, -0.281770f); + vertices[73] = new Vector3(-0.789408f, -0.416680f, -0.477925f); + vertices[74] = new Vector3(-0.696265f, -0.660531f, -0.327216f); + vertices[75] = new Vector3(-0.817496f, -0.585607f, -0.131060f); + vertices[76] = new Vector3(-0.327903f, -0.701905f, -0.654204f); + vertices[77] = new Vector3(-0.131747f, -0.823136f, -0.579280f); + vertices[78] = new Vector3(-0.282457f, -0.916278f, -0.335426f); + vertices[79] = new Vector3(-0.478612f, -0.795049f, -0.410352f); + vertices[80] = new Vector3(0.153478f, -0.999415f, -0.117774f); + vertices[81] = new Vector3(0.153478f, -0.999415f, 0.124687f); + vertices[82] = new Vector3(-0.147939f, -0.999415f, 0.124687f); + vertices[83] = new Vector3(-0.147939f, -0.999415f, -0.117774f); + vertices[84] = new Vector3(0.701803f, -0.660531f, 0.334128f); + vertices[85] = new Vector3(0.823033f, -0.585607f, 0.137973f); + vertices[86] = new Vector3(0.916176f, -0.341756f, 0.288682f); + vertices[87] = new Vector3(0.794945f, -0.416680f, 0.484838f); + vertices[88] = new Vector3(0.823033f, -0.585607f, -0.131060f); + vertices[89] = new Vector3(0.701803f, -0.660531f, -0.327216f); + vertices[90] = new Vector3(0.794945f, -0.416680f, -0.477925f); + vertices[91] = new Vector3(0.916176f, -0.341756f, -0.281770f); + vertices[92] = new Vector3(0.484149f, -0.795049f, -0.410352f); + vertices[93] = new Vector3(0.287995f, -0.916278f, -0.335426f); + vertices[94] = new Vector3(0.137286f, -0.823136f, -0.579280f); + vertices[95] = new Vector3(0.333441f, -0.701905f, -0.654204f); + vertices[96] = new Vector3(-0.654891f, -0.333543f, -0.695578f); + vertices[97] = new Vector3(-0.579966f, -0.137388f, -0.816807f); + vertices[98] = new Vector3(-0.336113f, -0.288097f, -0.909951f); + vertices[99] = new Vector3(-0.411039f, -0.484253f, -0.788719f); + vertices[100] = new Vector3(-0.118461f, 0.147837f, -0.993087f); + vertices[101] = new Vector3(0.124000f, 0.147837f, -0.993087f); + vertices[102] = new Vector3(0.124000f, -0.153580f, -0.993087f); + vertices[103] = new Vector3(-0.118461f, -0.153580f, -0.993087f); + vertices[104] = new Vector3(0.585505f, -0.137388f, -0.816807f); + vertices[105] = new Vector3(0.660429f, -0.333543f, -0.695578f); + vertices[106] = new Vector3(0.416578f, -0.484253f, -0.788719f); + vertices[107] = new Vector3(0.341652f, -0.288097f, -0.909951f); + vertices[108] = new Vector3(0.999313f, -0.124103f, -0.147252f); + vertices[109] = new Vector3(0.999313f, 0.118359f, -0.147252f); + vertices[110] = new Vector3(0.999313f, 0.118359f, 0.154165f); + vertices[111] = new Vector3(0.999313f, -0.124103f, 0.154165f); + vertices[112] = new Vector3(0.660429f, 0.327801f, -0.695578f); + vertices[113] = new Vector3(0.585505f, 0.131646f, -0.816807f); + vertices[114] = new Vector3(0.341652f, 0.282353f, -0.909951f); + vertices[115] = new Vector3(0.416578f, 0.478508f, -0.788719f); + vertices[116] = new Vector3(-0.579966f, 0.131646f, -0.816807f); + vertices[117] = new Vector3(-0.654891f, 0.327801f, -0.695578f); + vertices[118] = new Vector3(-0.411039f, 0.478508f, -0.788719f); + vertices[119] = new Vector3(-0.336113f, 0.282353f, -0.909951f); + return vertices; + } - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); + unsafe Buffer CreateTestConvexHull3() + { + BufferPool.Take(22, out var vertices); + vertices[0] = new Vector3(-0.103558f, 1.000000f, -0.490575f); + vertices[1] = new Vector3(0.266493f, 0.659794f, -0.363751f); + vertices[2] = new Vector3(-0.245774f, 0.762636f, -0.615304f); + vertices[3] = new Vector3(0.164688f, -0.777634f, -0.365919f); + vertices[4] = new Vector3(0.503268f, -0.846406f, -0.131286f); + vertices[5] = new Vector3(0.171066f, -0.931723f, -0.140738f); + vertices[6] = new Vector3(-0.247963f, -0.738059f, -0.413146f); + vertices[7] = new Vector3(-0.319203f, -0.260078f, -0.609331f); + vertices[8] = new Vector3(0.469624f, -0.747848f, -0.286486f); + vertices[9] = new Vector3(0.398526f, -0.238233f, -0.435281f); + vertices[10] = new Vector3(0.448274f, 0.295416f, -0.246327f); + vertices[11] = new Vector3(-0.245774f, 0.762636f, 0.596521f); + vertices[12] = new Vector3(0.266493f, 0.659794f, 0.344974f); + vertices[13] = new Vector3(-0.103558f, 1.000000f, 0.471792f); + vertices[14] = new Vector3(0.171066f, -0.931723f, 0.121961f); + vertices[15] = new Vector3(0.503268f, -0.846406f, 0.112509f); + vertices[16] = new Vector3(0.164688f, -0.777634f, 0.347137f); + vertices[17] = new Vector3(-0.319203f, -0.260078f, 0.590548f); + vertices[18] = new Vector3(-0.247963f, -0.738059f, 0.394364f); + vertices[19] = new Vector3(0.469624f, -0.747848f, 0.267709f); + vertices[20] = new Vector3(0.398526f, -0.238233f, 0.416498f); + vertices[21] = new Vector3(0.448274f, 0.295411f, 0.227550f); + return vertices; + } - var hullPoints = CreateRandomConvexHullPoints(); - //var hullPoints = CreateMeshConvexHull(content.Load(@"Content\newt.obj"), new Vector3(1, 1.5f, 1f)); - //var hullPoints = CreateTestConvexHull3(); - var hullShape = new ConvexHull(hullPoints, BufferPool, out _); - float largestError = 0; - for (int i = 0; i < hullShape.FaceToVertexIndicesStart.Length; ++i) + unsafe Buffer CreateJSONSourcedConvexHull(string filePath) + { + //ChatGPT wrote this, of course. + List points = new List(); + if (File.Exists(filePath)) + { + string jsonContent = File.ReadAllText(filePath); + List> rawPoints = JsonSerializer.Deserialize>>(jsonContent); + foreach (List point in rawPoints) { - hullShape.GetVertexIndicesForFace(i, out var faceVertices); - BundleIndexing.GetBundleIndices(i, out var normalBundleIndex, out var normalIndexInBundle); - Vector3Wide.ReadSlot(ref hullShape.BoundingPlanes[normalBundleIndex].Normal, normalIndexInBundle, out var faceNormal); - var offset = hullShape.BoundingPlanes[normalBundleIndex].Offset[normalIndexInBundle]; - Console.WriteLine($"Face {i} errors:"); - for (int j = 0; j < faceVertices.Length; ++j) + if (point.Count == 3) { - hullShape.GetPoint(faceVertices[j], out var point); - var error = Vector3.Dot(point, faceNormal) - offset; - Console.WriteLine($"v{j}: {error}"); - largestError = MathF.Max(MathF.Abs(error), largestError); + Vector3 vector3Point = new Vector3((float)point[0], (float)point[1], (float)point[2]); + points.Add(vector3Point); } } - Console.WriteLine($"Largest error: {largestError}"); + } + else + { + Console.Error.WriteLine("File not found: " + filePath); + } - //ConvexHullHelper.ComputeHull(hullPoints, BufferPool, out var hullData, out debugSteps); - //this.points = hullPoints; + BufferPool.Take(points.Count, out Buffer buffer); + for (int i = 0; i < buffer.Length; ++i) + { + buffer[i] = points[i]; + } + return buffer; + } - var boxHullPoints = CreateBoxConvexHull(2); - var boxHullShape = new ConvexHull(boxHullPoints, BufferPool, out _); + public unsafe override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(0, -2.5f, 10); + camera.Yaw = 0; + camera.Pitch = 0; - Matrix3x3.CreateScale(new Vector3(5, 0.5f, 3), out var scale); - var transform = Matrix3x3.CreateFromAxisAngle(Vector3.Normalize(new Vector3(3, 2, 1)), 1207) * scale; - const int transformCount = 10000; - var transformStart = Stopwatch.GetTimestamp(); - for (int i = 0; i < transformCount; ++i) - { - CreateTransformedCopy(hullShape, transform, BufferPool, out var transformedHullShape); - transformedHullShape.Dispose(BufferPool); - } - var transformEnd = Stopwatch.GetTimestamp(); - Console.WriteLine($"Transform hull computation time (us): {(transformEnd - transformStart) * 1e6 / (transformCount * Stopwatch.Frequency)}"); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); + //var hullPoints = CreateJSONSourcedConvexHull(@"Content/testHull.json"); + //for (int i = 0; i < hullPoints.Length; ++i) + //{ + // hullPoints[i] *= 0.03f; + //} + //var hullPoints = CreateRandomConvexHullPoints(); + var hullPoints = CreateMeshConvexHull(content.Load(@"Content\newt.obj"), new Vector3(1, 1.5f, 1f)); + //var hullPoints = CreateTestConvexHull2(); + //var hullPoints = CreateBoxConvexHull(2); + var hullShape = new ConvexHull(hullPoints, BufferPool, out _); + //float largestError = 0; + //for (int i = 0; i < hullShape.FaceToVertexIndicesStart.Length; ++i) + //{ + // hullShape.GetVertexIndicesForFace(i, out var faceVertices); + // BundleIndexing.GetBundleIndices(i, out var normalBundleIndex, out var normalIndexInBundle); + // Vector3Wide.ReadSlot(ref hullShape.BoundingPlanes[normalBundleIndex].Normal, normalIndexInBundle, out var faceNormal); + // var offset = hullShape.BoundingPlanes[normalBundleIndex].Offset[normalIndexInBundle]; + // Console.WriteLine($"Face {i} errors:"); + // for (int j = 0; j < faceVertices.Length; ++j) + // { + // hullShape.GetPoint(faceVertices[j], out var point); + // var error = Vector3.Dot(point, faceNormal) - offset; + // Console.WriteLine($"v{j}: {error}"); + // largestError = MathF.Max(MathF.Abs(error), largestError); + // } + //} + //Console.WriteLine($"Largest error: {largestError}"); - hullShape.RayTest(RigidPose.Identity, new Vector3(0, 1, 0), -Vector3.UnitY, out var t, out var normal); + //ConvexHullHelper.ComputeHull(hullPoints, BufferPool, out var hullData, out debugSteps); + //this.points = hullPoints; - const int rayIterationCount = 10000; - var rayPose = RigidPose.Identity; - var rayOrigin = new Vector3(0, 2, 0); - var rayDirection = new Vector3(0, -1, 0); + var boxHullPoints = CreateBoxConvexHull(2); + var boxHullShape = new ConvexHull(boxHullPoints, BufferPool, out _); - int hitCounter = 0; - var start = Stopwatch.GetTimestamp(); - for (int i = 0; i < rayIterationCount; ++i) - { - if (hullShape.RayTest(rayPose, rayOrigin, rayDirection, out _, out _)) - { - ++hitCounter; - } - } - var end = Stopwatch.GetTimestamp(); - Console.WriteLine($"Hit counter: {hitCounter}, computation time (us): {(end - start) * 1e6 / (rayIterationCount * Stopwatch.Frequency)}"); + Matrix3x3.CreateScale(new Vector3(5, 0.5f, 3), out var scale); + var transform = Matrix3x3.CreateFromAxisAngle(Vector3.Normalize(new Vector3(3, 2, 1)), 1207) * scale; + const int transformCount = 10000; + var transformStart = Stopwatch.GetTimestamp(); + for (int i = 0; i < transformCount; ++i) + { + CreateTransformedCopy(hullShape, transform, BufferPool, out var transformedHullShape); + transformedHullShape.Dispose(BufferPool); + } + var transformEnd = Stopwatch.GetTimestamp(); + Console.WriteLine($"Transform hull computation time (us): {(transformEnd - transformStart) * 1e6 / (transformCount * Stopwatch.Frequency)}"); + + hullShape.RayTest(RigidPose.Identity, new Vector3(0, 1, 0), -Vector3.UnitY, out var t, out var normal); - const int iterationCount = 100; - start = Stopwatch.GetTimestamp(); - for (int i = 0; i < iterationCount; ++i) + const int rayIterationCount = 10000; + var rayPose = RigidPose.Identity; + var rayOrigin = new Vector3(0, 2, 0); + var rayDirection = new Vector3(0, -1, 0); + + int hitCounter = 0; + var start = Stopwatch.GetTimestamp(); + for (int i = 0; i < rayIterationCount; ++i) + { + if (hullShape.RayTest(rayPose, rayOrigin, rayDirection, out _, out _)) { - CreateShape(hullPoints, BufferPool, out _, out var perfTestShape); - perfTestShape.Dispose(BufferPool); + ++hitCounter; } - end = Stopwatch.GetTimestamp(); - Console.WriteLine($"Hull computation time (us): {(end - start) * 1e6 / (iterationCount * Stopwatch.Frequency)}"); - - var hullShapeIndex = Simulation.Shapes.Add(hullShape); - var boxHullShapeIndex = Simulation.Shapes.Add(boxHullShape); - var inertia = hullShape.ComputeInertia(1); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 0, 5), inertia, new(hullShapeIndex, 20, 20), -0.01f)); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 0, 3), boxHullShape.ComputeInertia(1), new(boxHullShapeIndex, 20, 20), -0.01f)); - - Simulation.Statics.Add(new StaticDescription(new Vector3(-25, -5, 0), Simulation.Shapes.Add(new Sphere(2)))); - Simulation.Statics.Add(new StaticDescription(new Vector3(-20, -5, 0), Simulation.Shapes.Add(new Capsule(0.5f, 2)))); - Simulation.Statics.Add(new StaticDescription(new Vector3(-15, -5, 0), Simulation.Shapes.Add(new Box(2f, 2f, 2f)))); - Simulation.Statics.Add(new StaticDescription(new Vector3(-10, -5, 5), Simulation.Shapes.Add(new Triangle { A = new Vector3(0, 0, -10), B = new Vector3(5, 0, -10), C = new Vector3(0, 0, -5) }))); - Simulation.Statics.Add(new StaticDescription(new Vector3(-5, -5, 0), Simulation.Shapes.Add(new Cylinder(1, 1)))); - Simulation.Statics.Add(new StaticDescription(new Vector3(-5, -5, 5), Simulation.Shapes.Add(new Cylinder(1, 1)))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5, 0), hullShapeIndex)); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5, 5), Simulation.Shapes.Add(boxHullShape))); - - var spacing = new Vector3(3f, 3f, 3); - int width = 16; - int height = 16; - int length = 0; - var origin = -0.5f * spacing * new Vector3(width, 0, length) + new Vector3(40, 0.2f, -40); - for (int i = 0; i < width; ++i) - { - for (int j = 0; j < height; ++j) - { - for (int k = 0; k < length; ++k) - { - Simulation.Bodies.Add(BodyDescription.CreateDynamic( - (origin + spacing * new Vector3(i, j, k), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathHelper.Pi * 0.05f)), - inertia, hullShapeIndex, 0.01f)); - } - } - } - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -10, 0), Simulation.Shapes.Add(new Box(1000, 1, 1000)))); - - Random random = new Random(5); - DemoMeshHelper.CreateDeformedPlane(64, 64, (x, y) => new Vector3( - x + 8, - 2f * MathF.Sin(x * 0.125f) * MathF.Sin(y * 0.125f) + 0.1f * random.NextSingle() - 3, - y - 8), new Vector3(1, 1, 1), BufferPool, out var mesh); - Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); } + var end = Stopwatch.GetTimestamp(); + Console.WriteLine($"Hit counter: {hitCounter}, computation time (us): {(end - start) * 1e6 / (rayIterationCount * Stopwatch.Frequency)}"); - void TestConvexHullCreation() + const int iterationCount = 100; + start = Stopwatch.GetTimestamp(); + for (int i = 0; i < iterationCount; ++i) + { + CreateShape(hullPoints, BufferPool, out _, out var perfTestShape); + perfTestShape.Dispose(BufferPool); + } + end = Stopwatch.GetTimestamp(); + Console.WriteLine($"Hull computation time (us): {(end - start) * 1e6 / (iterationCount * Stopwatch.Frequency)}"); + + var hullShapeIndex = Simulation.Shapes.Add(hullShape); + var boxHullShapeIndex = Simulation.Shapes.Add(boxHullShape); + var inertia = hullShape.ComputeInertia(1); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 0, 5), inertia, new(hullShapeIndex, 20, 20), -0.01f)); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 0, 3), boxHullShape.ComputeInertia(1), new(boxHullShapeIndex, 20, 20), -0.01f)); + + Simulation.Statics.Add(new StaticDescription(new Vector3(-25, -5, 0), Simulation.Shapes.Add(new Sphere(2)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(-20, -5, 0), Simulation.Shapes.Add(new Capsule(0.5f, 2)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(-15, -5, 0), Simulation.Shapes.Add(new Box(2f, 2f, 2f)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(-10, -5, 5), Simulation.Shapes.Add(new Triangle { A = new Vector3(0, 0, -10), B = new Vector3(5, 0, -10), C = new Vector3(0, 0, -5) }))); + Simulation.Statics.Add(new StaticDescription(new Vector3(-5, -5, 0), Simulation.Shapes.Add(new Cylinder(1, 1)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(-5, -5, 5), Simulation.Shapes.Add(new Cylinder(1, 1)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5, 0), hullShapeIndex)); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5, 5), Simulation.Shapes.Add(boxHullShape))); + + var spacing = new Vector3(3f, 3f, 3); + int width = 16; + int height = 16; + int length = 0; + var origin = -0.5f * spacing * new Vector3(width, 0, length) + new Vector3(40, 0.2f, -40); + for (int i = 0; i < width; ++i) { - var random = new Random(5); - for (int iterationIndex = 0; iterationIndex < 100000; ++iterationIndex) + for (int j = 0; j < height; ++j) { - const int pointCount = 32; - var points = new QuickList(pointCount, BufferPool); - for (int i = 0; i < pointCount; ++i) + for (int k = 0; k < length; ++k) { - points.AllocateUnsafely() = new Vector3(1 * random.NextSingle(), 2 * random.NextSingle(), 3 * random.NextSingle()); + Simulation.Bodies.Add(BodyDescription.CreateDynamic( + (origin + spacing * new Vector3(i, j, k), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathHelper.Pi * 0.05f)), + inertia, hullShapeIndex, 0.01f)); } - - var pointsBuffer = points.Span.Slice(points.Count); - CreateShape(pointsBuffer, BufferPool, out _, out var hullShape); - - hullShape.Dispose(BufferPool); } } + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -10, 0), Simulation.Shapes.Add(new Box(1000, 1, 1000)))); + + Random random = new Random(5); + DemoMeshHelper.CreateDeformedPlane(64, 64, (x, y) => new Vector3( + x + 8, + 2f * MathF.Sin(x * 0.125f) * MathF.Sin(y * 0.125f) + 0.1f * random.NextSingle() - 3, + y - 8), new Vector3(1, 1, 1), BufferPool, out var mesh); + Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); + } - //Buffer points; - //List debugSteps; - - //int stepIndex = 0; - - //public override void Update(Window window, Camera camera, Input input, float dt) - //{ - // if (input.TypedCharacters.Contains('x')) - // { - // stepIndex = Math.Max(stepIndex - 1, 0); - // } - // if (input.TypedCharacters.Contains('c')) - // { - // stepIndex = Math.Min(stepIndex + 1, debugSteps.Count - 1); - // } - // if (input.WasPushed(OpenTK.Input.Key.P)) - // { - // showWireframe = !showWireframe; - // } - // if (input.WasPushed(OpenTK.Input.Key.U)) - // { - // showDeleted = !showDeleted; - // } - // if (input.WasPushed(OpenTK.Input.Key.Y)) - // { - // showVertexIndices = !showVertexIndices; - // } - // if (input.WasPushed(OpenTK.Input.Key.H)) - // { - // showFaceVertexStatuses = !showFaceVertexStatuses; - // } - // base.Update(window, camera, input, dt); - //} - - //bool showWireframe; - //bool showDeleted; - //bool showVertexIndices; - //bool showFaceVertexStatuses = true; - //public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) - //{ - // var step = debugSteps[stepIndex]; - // var scale = 10f; - // var renderOffset = new Vector3(0, 15, 0); - // for (int i = 0; i < points.Length; ++i) - // { - // var pose = new RigidPose(renderOffset + points[i] * scale); - // renderer.Shapes.AddShape(new Box(0.1f, 0.1f, 0.1f), Simulation.Shapes, pose, new Vector3(0.5f, 0.5f, 0.5f)); - // if (!step.AllowVertex[i] && showFaceVertexStatuses) - // renderer.Shapes.AddShape(new Box(0.6f, 0.25f, 0.25f), Simulation.Shapes, pose, new Vector3(1, 0, 0)); - // } - // if (showFaceVertexStatuses) - // { - // for (int i = 0; i < step.Raw.Count; ++i) - // { - // var pose = new RigidPose(renderOffset + points[step.Raw[i]] * scale); - // renderer.Shapes.AddShape(new Box(0.25f, 0.6f, 0.25f), Simulation.Shapes, pose, new Vector3(0, 0, 1)); - // } - // for (int i = 0; i < step.Reduced.Count; ++i) - // { - // var pose = new RigidPose(renderOffset + points[step.Reduced[i]] * scale); - // renderer.Shapes.AddShape(new Box(0.25f, 0.25f, 0.6f), Simulation.Shapes, pose, new Vector3(0, 1, 0)); - // } - // } - - // { - // var pose = new RigidPose(renderOffset); - // for (int i = 0; i < step.FaceStarts.Count; ++i) - // { - // if (showDeleted || !step.FaceDeleted[i]) - // { - // var faceStart = step.FaceStarts[i]; - // var faceEnd = i + 1 < step.FaceStarts.Count ? step.FaceStarts[i + 1] : step.FaceIndices.Count; - // var count = faceEnd - faceStart; - // var color = step.FaceDeleted[i] ? new Vector3(0.25f, 0.25f, 0.25f) : step.FaceIndex == i ? new Vector3(1, 0, 0.5f) : new Vector3(1, 0, 1); - // var deletionInducedScale = step.FaceDeleted[i] ? new Vector3(1.1f) : new Vector3(1f); - - // var offset = step.FaceDeleted[i] ? step.FaceNormals[i] * 0.25f : new Vector3(); - // if (showWireframe) - // { - // var previousIndex = faceEnd - 1; - // for (int q = faceStart; q < faceEnd; ++q) - // { - // var a = points[step.FaceIndices[q]] * scale + pose.Position + offset; - // var b = points[step.FaceIndices[previousIndex]] * scale + pose.Position + offset; - // previousIndex = q; - // renderer.Lines.Allocate() = new LineInstance(a, b, color, Vector3.Zero); - // } - // } - // else - // { - // for (int k = faceStart + 2; k < faceEnd; ++k) - // { - // renderer.Shapes.AddShape(new Triangle - // { - // A = points[step.FaceIndices[faceStart]] * scale + offset, - // B = points[step.FaceIndices[k]] * scale + offset, - // C = points[step.FaceIndices[k - 1]] * scale + offset - // }, Simulation.Shapes, pose, color); - // } - // } - // } - // } - // } - - // if (showVertexIndices) - // { - // for (int i = 0; i < points.Length; ++i) - // { - // if (DemoRenderer.Helpers.GetScreenLocation(points[i] * scale + renderOffset, camera.ViewProjection, renderer.Surface.Resolution, out var location)) - // { - // renderer.TextBatcher.Write(text.Clear().Append(i), location, 10, new Vector3(1), font); - // } - // } - // } - - // var edgeMidpoint = renderOffset + (points[step.SourceEdge.A] + points[step.SourceEdge.B]) * scale * 0.5f; - // renderer.Lines.Allocate() = new LineInstance(edgeMidpoint, edgeMidpoint + step.BasisX * scale * 0.5f, new Vector3(1, 1, 0), new Vector3()); - // renderer.Lines.Allocate() = new LineInstance(edgeMidpoint, edgeMidpoint + step.BasisY * scale * 0.5f, new Vector3(0, 1, 0), new Vector3()); - // renderer.TextBatcher.Write( - // text.Clear().Append($"Enumerate step with X and C. Current step: ").Append(stepIndex + 1).Append(" out of ").Append(debugSteps.Count), - // new Vector2(32, renderer.Surface.Resolution.Y - 140), 20, new Vector3(1), font); - // renderer.TextBatcher.Write(text.Clear().Append("Show wireframe: P ").Append(showWireframe ? "(on)" : "(off)"), new Vector2(32, renderer.Surface.Resolution.Y - 120), 20, new Vector3(1), font); - // renderer.TextBatcher.Write(text.Clear().Append("Show deleted: U ").Append(showDeleted ? "(on)" : "(off)"), new Vector2(32, renderer.Surface.Resolution.Y - 100), 20, new Vector3(1), font); - // renderer.TextBatcher.Write(text.Clear().Append("Show vertex indices: Y ").Append(showVertexIndices ? "(on)" : "(off)"), new Vector2(32, renderer.Surface.Resolution.Y - 80), 20, new Vector3(1), font); - // renderer.TextBatcher.Write(text.Clear().Append("Show face vertex statuses: H ").Append(showFaceVertexStatuses ? "(on)" : "(off)"), new Vector2(32, renderer.Surface.Resolution.Y - 60), 20, new Vector3(1), font); + void TestConvexHullCreation() + { + var random = new Random(5); + for (int iterationIndex = 0; iterationIndex < 100000; ++iterationIndex) + { + const int pointCount = 32; + var points = new QuickList(pointCount, BufferPool); + for (int i = 0; i < pointCount; ++i) + { + points.AllocateUnsafely() = new Vector3(1 * random.NextSingle(), 2 * random.NextSingle(), 3 * random.NextSingle()); + } + var pointsBuffer = points.Span.Slice(points.Count); + CreateShape(pointsBuffer, BufferPool, out _, out var hullShape); - // base.Render(renderer, camera, input, text, font); - //} + hullShape.Dispose(BufferPool); + } } + + //Buffer points; + //List debugSteps; + + //int stepIndex = 0; + + //public override void Update(Window window, Camera camera, Input input, float dt) + //{ + // if (input.TypedCharacters.Contains('x')) + // { + // stepIndex = Math.Max(stepIndex - 1, 0); + // } + // if (input.TypedCharacters.Contains('c')) + // { + // stepIndex = Math.Min(stepIndex + 1, debugSteps.Count - 1); + // } + // if (input.WasPushed(OpenTK.Input.Key.P)) + // { + // showWireframe = !showWireframe; + // } + // if (input.WasPushed(OpenTK.Input.Key.U)) + // { + // showDeleted = !showDeleted; + // } + // if (input.WasPushed(OpenTK.Input.Key.Y)) + // { + // showVertexIndices = !showVertexIndices; + // } + // if (input.WasPushed(OpenTK.Input.Key.H)) + // { + // showFaceVertexStatuses = !showFaceVertexStatuses; + // } + // base.Update(window, camera, input, dt); + //} + + //bool showWireframe; + //bool showDeleted; + //bool showVertexIndices; + //bool showFaceVertexStatuses = true; + //public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + //{ + // var step = debugSteps[stepIndex]; + // var scale = 10f; + // var renderOffset = new Vector3(0, 15, 0); + // for (int i = 0; i < points.Length; ++i) + // { + // var pose = new RigidPose(renderOffset + points[i] * scale); + // renderer.Shapes.AddShape(new Box(0.1f, 0.1f, 0.1f), Simulation.Shapes, pose, new Vector3(0.5f, 0.5f, 0.5f)); + // if (!step.AllowVertex[i] && showFaceVertexStatuses) + // renderer.Shapes.AddShape(new Box(0.6f, 0.25f, 0.25f), Simulation.Shapes, pose, new Vector3(1, 0, 0)); + // } + // if (showFaceVertexStatuses) + // { + // for (int i = 0; i < step.Raw.Count; ++i) + // { + // var pose = new RigidPose(renderOffset + points[step.Raw[i]] * scale); + // renderer.Shapes.AddShape(new Box(0.25f, 0.6f, 0.25f), Simulation.Shapes, pose, new Vector3(0, 0, 1)); + // } + // for (int i = 0; i < step.Reduced.Count; ++i) + // { + // var pose = new RigidPose(renderOffset + points[step.Reduced[i]] * scale); + // renderer.Shapes.AddShape(new Box(0.25f, 0.25f, 0.6f), Simulation.Shapes, pose, new Vector3(0, 1, 0)); + // } + // } + + // { + // var pose = new RigidPose(renderOffset); + // for (int i = 0; i < step.FaceStarts.Count; ++i) + // { + // if (showDeleted || !step.FaceDeleted[i]) + // { + // var faceStart = step.FaceStarts[i]; + // var faceEnd = i + 1 < step.FaceStarts.Count ? step.FaceStarts[i + 1] : step.FaceIndices.Count; + // var count = faceEnd - faceStart; + // var color = step.FaceDeleted[i] ? new Vector3(0.25f, 0.25f, 0.25f) : step.FaceIndex == i ? new Vector3(1, 0, 0.5f) : new Vector3(1, 0, 1); + // var deletionInducedScale = step.FaceDeleted[i] ? new Vector3(1.1f) : new Vector3(1f); + + // var offset = step.FaceDeleted[i] ? step.FaceNormals[i] * 0.25f : new Vector3(); + // if (showWireframe) + // { + // var previousIndex = faceEnd - 1; + // for (int q = faceStart; q < faceEnd; ++q) + // { + // var a = points[step.FaceIndices[q]] * scale + pose.Position + offset; + // var b = points[step.FaceIndices[previousIndex]] * scale + pose.Position + offset; + // previousIndex = q; + // renderer.Lines.Allocate() = new LineInstance(a, b, color, Vector3.Zero); + // } + // } + // else + // { + // for (int k = faceStart + 2; k < faceEnd; ++k) + // { + // renderer.Shapes.AddShape(new Triangle + // { + // A = points[step.FaceIndices[faceStart]] * scale + offset, + // B = points[step.FaceIndices[k]] * scale + offset, + // C = points[step.FaceIndices[k - 1]] * scale + offset + // }, Simulation.Shapes, pose, color); + // } + // } + // } + // } + // } + + // if (showVertexIndices) + // { + // for (int i = 0; i < points.Length; ++i) + // { + // if (DemoRenderer.Helpers.GetScreenLocation(points[i] * scale + renderOffset, camera.ViewProjection, renderer.Surface.Resolution, out var location)) + // { + // renderer.TextBatcher.Write(text.Clear().Append(i), location, 10, new Vector3(1), font); + // } + // } + // } + + // var edgeMidpoint = renderOffset + (points[step.SourceEdge.A] + points[step.SourceEdge.B]) * scale * 0.5f; + // renderer.Lines.Allocate() = new LineInstance(edgeMidpoint, edgeMidpoint + step.BasisX * scale * 0.5f, new Vector3(1, 1, 0), new Vector3()); + // renderer.Lines.Allocate() = new LineInstance(edgeMidpoint, edgeMidpoint + step.BasisY * scale * 0.5f, new Vector3(0, 1, 0), new Vector3()); + // renderer.TextBatcher.Write( + // text.Clear().Append($"Enumerate step with X and C. Current step: ").Append(stepIndex + 1).Append(" out of ").Append(debugSteps.Count), + // new Vector2(32, renderer.Surface.Resolution.Y - 140), 20, new Vector3(1), font); + // renderer.TextBatcher.Write(text.Clear().Append("Show wireframe: P ").Append(showWireframe ? "(on)" : "(off)"), new Vector2(32, renderer.Surface.Resolution.Y - 120), 20, new Vector3(1), font); + // renderer.TextBatcher.Write(text.Clear().Append("Show deleted: U ").Append(showDeleted ? "(on)" : "(off)"), new Vector2(32, renderer.Surface.Resolution.Y - 100), 20, new Vector3(1), font); + // renderer.TextBatcher.Write(text.Clear().Append("Show vertex indices: Y ").Append(showVertexIndices ? "(on)" : "(off)"), new Vector2(32, renderer.Surface.Resolution.Y - 80), 20, new Vector3(1), font); + // renderer.TextBatcher.Write(text.Clear().Append("Show face vertex statuses: H ").Append(showFaceVertexStatuses ? "(on)" : "(off)"), new Vector2(32, renderer.Surface.Resolution.Y - 60), 20, new Vector3(1), font); + + + // base.Render(renderer, camera, input, text, font); + //} } From 53f5efd0e72ac85ba636afd04e996f156c2394d6 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 6 May 2023 16:48:18 -0500 Subject: [PATCH 713/947] Binned builder revamp pretty much done? Some fine tuning. Further improvements seem too architecture specific to worry about. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 24 +++++++++++------ BepuUtilities/LinkedTaskStack.cs | 27 ++++++++++++++++++- .../SpecializedTests/TreeFiddlingTestDemo.cs | 1 - 3 files changed, 42 insertions(+), 10 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 253f395e8..d042a60a1 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -454,10 +454,15 @@ public int GetTargetTaskCountForNodes(int subtreeCount) } const int MinimumSubtreesPerThreadForCentroidPrepass = 1024; - const int MinimumSubtreesPerThreadForBinning = 131072; - const int MinimumSubtreesPerThreadForPartitioning = 131072; + const int MinimumSubtreesPerThreadForBinning = 1024; + const int MinimumSubtreesPerThreadForPartitioning = 1024; const int MinimumSubtreesPerThreadForNodeJob = 256; const int TargetTaskCountMultiplierForNodePushOverInnerLoop = 8; + /// + /// Random value stored in the upper 32 bits of the job tag submitted for internal multithreading operations. + /// + /// Other systems using the same task stack may want to use their own filtering approaches. By using a very specific and unique signature, those other systems are less likely to accidentally collide. + const ulong JobFilterTagHeader = 0xB0A1BF32ul << 32; [MethodImpl(MethodImplOptions.AggressiveInlining)] static BoundingBox4 ComputeCentroidBounds(Buffer bounds) @@ -580,8 +585,9 @@ unsafe static BoundingBox4 MultithreadedCentroidPrepass(MultithreadBinnedBuildCo //(Note: the centroid prepass only runs at the root, so we don't expect there to be any competition from other nodes *in this tree*, //but it's possible that the same taskstack is used from multiple binned builds. //Technically, there's potential interference from other user tasks that have nothing to do with binned building, but... not too concerned at this point.) - var jobFilter = new MinimumTagFilter(2); - context->TaskStack->For(&CentroidPrepassWorker, &taskContext, 0, taskCount, workerIndex, dispatcher, ref jobFilter, 1); + var tagValue = (uint)workerIndex | JobFilterTagHeader; + var jobFilter = new EqualTagFilter(tagValue); + context->TaskStack->For(&CentroidPrepassWorker, &taskContext, 0, taskCount, workerIndex, dispatcher, ref jobFilter, tagValue); var centroidBounds = taskContext.PrepassWorkers[0]; for (int i = 1; i < activeWorkerCount; ++i) @@ -767,8 +773,9 @@ unsafe static void MultithreadedBinSubtrees(MultithreadBinnedBuildContext* conte //We only want the inner multithreading to work on small, non-recursive jobs. //Diving into a node at this point would stall the current node and favor more (and smaller) nodes. - var jobFilter = new MinimumTagFilter(2); - context->TaskStack->For(&BinSubtreesWorker, &taskContext, 0, taskContext.TaskData.TaskCount, workerIndex, dispatcher, ref jobFilter, 1); + var tagValue = (uint)workerIndex | JobFilterTagHeader; + var jobFilter = new EqualTagFilter(tagValue); + context->TaskStack->For(&BinSubtreesWorker, &taskContext, 0, taskContext.TaskData.TaskCount, workerIndex, dispatcher, ref jobFilter, tagValue); //Unless the number of threads and bins is really huge, there's no value in attempting to multithread the final compression. //(Parallel reduction is an option, but even then... I suspect the single threaded version will be faster. And it's way simpler.) @@ -920,8 +927,9 @@ unsafe static (int subtreeCountA, int subtreeCountB) MultithreadedPartition(Mult subtrees, subtreesNext, binIndices, binSplitIndex); //We only want the inner multithreading to work on small, non-recursive jobs. //Diving into a node at this point would stall the current node and favor more (and smaller) nodes. - var jobFilter = new MinimumTagFilter(2); - context->TaskStack->For(&PartitionSubtreesWorker, &taskContext, 0, taskContext.TaskData.TaskCount, workerIndex, dispatcher, ref jobFilter, 1); + var tagValue = (uint)workerIndex | JobFilterTagHeader; + var jobFilter = new EqualTagFilter(tagValue); + context->TaskStack->For(&PartitionSubtreesWorker, &taskContext, 0, taskContext.TaskData.TaskCount, workerIndex, dispatcher, ref jobFilter, tagValue); return (taskContext.Counters.SubtreeCountA, taskContext.Counters.SubtreeCountB); } diff --git a/BepuUtilities/LinkedTaskStack.cs b/BepuUtilities/LinkedTaskStack.cs index d8c4e5fc4..5cd1dd378 100644 --- a/BepuUtilities/LinkedTaskStack.cs +++ b/BepuUtilities/LinkedTaskStack.cs @@ -245,7 +245,7 @@ public struct MinimumTagFilter : IJobFilter /// public ulong MinimumTagValue; /// - /// Creates a job filter that wraps a delegate. + /// Creates a job filter that requires the job tag to meet or exceed a threshold value. /// /// Value that a job must match or exceed to be allowed. public MinimumTagFilter(ulong minimumTagValue) @@ -259,6 +259,31 @@ public bool AllowJob(ulong jobTag) } } +/// +/// A job filter that requires the job tag to match a specific value. +/// +public struct EqualTagFilter : IJobFilter +{ + /// + /// Tag value required to allow a job. + /// + public ulong RequiredTag; + + /// + /// Creates a job filter that requires the job tag to match a specific value. + /// + public EqualTagFilter(ulong requiredTag) + { + RequiredTag = requiredTag; + } + /// + public bool AllowJob(ulong jobTag) + { + return jobTag == RequiredTag; + } +} + + [StructLayout(LayoutKind.Explicit, Size = 292)] internal unsafe struct Job { diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index 81dc19d3e..824cb07cf 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -190,7 +190,6 @@ public override void Initialize(ContentArchive content, Camera camera) Tree.BinnedBuilder(subtrees, mesh.Tree.Nodes, mesh.Tree.Metanodes, mesh.Tree.Leaves, ThreadDispatcher, BufferPool); }, "Revamp Single Axis MT", ref mesh.Tree); - //BufferPool.Take(mesh.Triangles.Length, out var leafBounds); //BufferPool.Take(mesh.Triangles.Length, out var leafIndices); From 2b0a4a383fe88870d312322fa444a2b3d409716b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E4=BA=91=E6=99=AF=28Neil=29?= Date: Sat, 6 May 2023 11:32:49 +0800 Subject: [PATCH 714/947] Fixed ContactEventListeners preallocation capacity. Exception when listenerCount reach 64. --- Demos/Demos/ContactEventsDemo.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Demos/Demos/ContactEventsDemo.cs b/Demos/Demos/ContactEventsDemo.cs index f9b3772ac..88e925da7 100644 --- a/Demos/Demos/ContactEventsDemo.cs +++ b/Demos/Demos/ContactEventsDemo.cs @@ -248,7 +248,7 @@ public void Register(CollidableReference collidable, IContactEventHandler handle staticListenerFlags.Add(collidable.RawHandleValue, pool); else bodyListenerFlags.Add(collidable.RawHandleValue, pool); - if (listenerCount > listeners.Length) + if (listenerCount >= listeners.Length) { Array.Resize(ref listeners, listeners.Length * 2); } From eb6697a657ce2a2b94ecbfd1886849d2c00e5b75 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 6 May 2023 18:00:09 -0500 Subject: [PATCH 715/947] BinnedBuild now has a slightly nicer interface, and Mesh uses it. --- BepuPhysics/Collidables/Mesh.cs | 70 +++++++++++++++++++------ BepuPhysics/Trees/Tree_BinnedBuilder.cs | 50 ++++++++++++------ 2 files changed, 87 insertions(+), 33 deletions(-) diff --git a/BepuPhysics/Collidables/Mesh.cs b/BepuPhysics/Collidables/Mesh.cs index 20deb5f93..d680d3958 100644 --- a/BepuPhysics/Collidables/Mesh.cs +++ b/BepuPhysics/Collidables/Mesh.cs @@ -66,27 +66,67 @@ public Vector3 Scale } /// - /// Creates a mesh shape. + /// Fills a buffer of subtrees according to a buffer of triangles. + /// + /// The term "subtree" is used because the binned builder does not care whether the input came from leaf nodes or a refinement process's internal nodes. + /// Triangles to build subtrees from. + /// Subtrees created for the triangles. + public static void FillSubtreesForTriangles(Span triangles, Span subtrees) + { + if (subtrees.Length != triangles.Length) + throw new ArgumentException("Triangles and subtrees span lengths should match."); + for (int i = 0; i < triangles.Length; ++i) + { + ref var t = ref triangles[i]; + ref var subtree = ref subtrees[i]; + subtree.Min = Vector3.Min(t.A, Vector3.Min(t.B, t.C)); + subtree.Max = Vector3.Max(t.A, Vector3.Max(t.B, t.C)); + subtree.LeafCount = 1; + subtree.Index = Tree.Encode(i); + } + } + + /// + /// Creates a mesh shape instance, but leaves the Tree in an unbuilt state. The tree must be built before the mesh can be used. /// /// Triangles to use in the mesh. /// Scale to apply to all vertices at runtime. /// Note that the scale is not baked into the triangles or acceleration structure; the same set of triangles and acceleration structure can be used across multiple Mesh instances with different scales. /// Pool used to allocate acceleration structures. - public Mesh(Buffer triangles, Vector3 scale, BufferPool pool) : this() + /// Created mesh shape. + /// In some cases, the default binned build may not be the ideal builder. This function does everything needed to set up a tree without the expense of figuring out the details of the acceleration structure. + /// The user can then run whatever build/refinement process is appropriate. + public static Mesh CreateMeshWithoutTreeBuild(Buffer triangles, Vector3 scale, BufferPool pool) { - Triangles = triangles; - Tree = new Tree(pool, triangles.Length); - pool.Take(triangles.Length, out var boundingBoxes); - for (int i = 0; i < triangles.Length; ++i) + Mesh mesh = default; + mesh.Triangles = triangles; + mesh.Tree = new Tree(pool, triangles.Length) { - ref var t = ref triangles[i]; - ref var bounds = ref boundingBoxes[i]; - bounds.Min = Vector3.Min(t.A, Vector3.Min(t.B, t.C)); - bounds.Max = Vector3.Max(t.A, Vector3.Max(t.B, t.C)); - } - Tree.SweepBuild(pool, boundingBoxes); - pool.Return(ref boundingBoxes); - Scale = scale; + //If this codepath is being used, we're assuming that the triangles are going to be the actual children + //so we can go ahead and set the node/leaf counts. + //(This is in contrast to creating a tree with a certain capacity, but then relying on incremental adds/removes later.) + NodeCount = triangles.Length - 1, + LeafCount = triangles.Length + }; + mesh.Scale = scale; + return mesh; + } + + /// + /// Creates a mesh shape. + /// + /// Triangles to use in the mesh. + /// Scale to apply to all vertices at runtime. + /// Note that the scale is not baked into the triangles or acceleration structure; the same set of triangles and acceleration structure can be used across multiple Mesh instances with different scales. + /// Pool used to allocate acceleration structures. + /// Dispatcher to use to multithread the execution of the mesh build process. If null, the build will be single threaded. + public Mesh(Buffer triangles, Vector3 scale, BufferPool pool, IThreadDispatcher dispatcher = null) + { + this = CreateMeshWithoutTreeBuild(triangles, scale, pool); + pool.Take(triangles.Length, out var subtrees); + FillSubtreesForTriangles(triangles, subtrees); + Tree.BinnedBuild(subtrees, dispatcher: dispatcher, pool: pool); + pool.Return(ref subtrees); } /// @@ -112,7 +152,6 @@ public unsafe Mesh(Span data, BufferPool pool) /// /// Gets the number of bytes it would take to store the given mesh in a byte buffer. /// - /// Mesh to measure. /// Number of bytes it would take to store the mesh. public readonly unsafe int GetSerializedByteCount() { @@ -122,7 +161,6 @@ public readonly unsafe int GetSerializedByteCount() /// /// Writes a mesh's data to a byte buffer. /// - /// Mesh to write into the byte buffer. /// Byte buffer to store the mesh in. public readonly unsafe void Serialize(Span data) { diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index d042a60a1..20772b351 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -19,6 +19,7 @@ using System.Runtime.Intrinsics.X86; using System.Threading; using System.Threading.Tasks.Sources; +using System.Xml.Linq; namespace BepuPhysics.Trees { @@ -1326,12 +1327,12 @@ static unsafe void BinnedBuilderInternal(Buffer subtrees, Buffer subtrees, BufferPushUnsafely(new Task(&BinnedBuilderWorkerEntry>, &context), 0, dispatcher); - dispatcher.DispatchWorkers(&BinnedBuilderWorkerFunction>, unmanagedContext: taskStackPointer); + dispatcher.DispatchWorkers(&BinnedBuilderWorkerFunction>, unmanagedContext: taskStackPointer, maximumWorkerCount: workerCount); if (createdTaskQueueLocally) taskStack.Dispose(pool, dispatcher); @@ -1376,18 +1377,9 @@ unsafe static void BinnedBuilderWorkerFunction(int workerIndex, IThread } } - public static unsafe void BinnedBuilder(Buffer subtrees, Buffer nodes, Buffer metanodes, Buffer leaves, - IThreadDispatcher threadDispatcher, BufferPool pool, int minimumBinCount = 16, int maximumBinCount = 64, float leafToBinMultiplier = 1 / 16f, int microsweepThreshold = 64) - { - pool.Take(subtrees.Length, out Buffer subtreesPong); - pool.Take(subtrees.Length, out Buffer binIndices); - BinnedBuilderInternal(subtrees, subtreesPong, nodes, metanodes, leaves, binIndices, threadDispatcher, null, threadDispatcher.ThreadCount, pool, minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold); - pool.Return(ref subtreesPong); - pool.Return(ref binIndices); - } /// - /// Runs a binned build across the subtrees buffer. + /// Runs a multithreaded binned build across the subtrees buffer. /// /// Subtrees (either leaves or nodes) to run the builder over. The builder may make in-place modifications to the input buffer; the input buffer should not be assumed to be in a valid state after the builder runs. /// Buffer holding the nodes created by the build process. @@ -1397,16 +1389,20 @@ public static unsafe void BinnedBuilder(Buffer subtrees, Buffer /// Metanodes are in the same order and in the same slots; they simply contain data about nodes that most traversals don't need to know about. /// Buffer holding the leaf references created by the build process. /// The indices written by the build process are those defined in the inputs; any that is negative is encoded according to and points into the leaf buffer. - /// Buffer pool used to preallocate a pingpong buffer if the number of subtrees exceeds maximumSubtreeStackAllocationCount. If null, stack allocation or a slower in-place partitioning will be used. + /// Dispatcher used to multithread the execution of the build. If the dispatcher is not null, pool must also not be null. + /// Buffer pool used to preallocate a pingpong buffer if the number of subtrees exceeds maximumSubtreeStackAllocationCount. If null, stack allocation or a slower in-place partitioning will be used. + /// A pool must be provided if a thread dispatcher is given. /// Maximum number of subtrees to try putting on the stack for the binned builder's pong buffers. /// Subtree counts larger than this threshold will either resort to a buffer pool allocation (if available) or slower in-place partition operations. /// Minimum number of bins the builder should use per node. /// Maximum number of bins the builder should use per node. /// Multiplier to apply to the subtree count within a node to decide the bin count. Resulting value will then be clamped by the minimum/maximum bin counts. /// Threshold at or under which the binned builder resorts to local counting sort sweeps. - public static unsafe void BinnedBuilder(Buffer subtrees, Buffer nodes, Buffer metanodes, Buffer leaves, - BufferPool pool = null, int maximumSubtreeStackAllocationCount = 4096, int minimumBinCount = 16, int maximumBinCount = 64, float leafToBinMultiplier = 1 / 16f, int microsweepThreshold = 64) + public static unsafe void BinnedBuild(Buffer subtrees, Buffer nodes, Buffer metanodes, Buffer leaves, + IThreadDispatcher dispatcher = null, BufferPool pool = null, int maximumSubtreeStackAllocationCount = 4096, int minimumBinCount = 16, int maximumBinCount = 64, float leafToBinMultiplier = 1 / 16f, int microsweepThreshold = 64) { + if (dispatcher != null && pool == null) + throw new ArgumentException("If a ThreadDispatcher has been given to BinnedBuild, a BufferPool must also be provided."); Buffer subtreesPong; Buffer binIndices; bool requiresReturn = false; @@ -1428,7 +1424,8 @@ public static unsafe void BinnedBuilder(Buffer subtrees, Buffer binIndices = default; subtreesPong = default; } - BinnedBuilderInternal(subtrees, subtreesPong, nodes, metanodes, leaves, binIndices, null, null, 0, null, minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold); + //TODO: Note that there's no public entrypoint into the binned builder that provides a task stack right now. Good chance that'll change once we use it in refinement. + BinnedBuilderInternal(subtrees, subtreesPong, nodes, metanodes, leaves, binIndices, dispatcher, null, dispatcher == null ? 0 : dispatcher.ThreadCount, pool, minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold); if (requiresReturn) { @@ -1436,5 +1433,24 @@ public static unsafe void BinnedBuilder(Buffer subtrees, Buffer pool.Return(ref subtreesPong); } } + + /// + /// Runs a binned build across the subtrees buffer. + /// + /// Subtrees (either leaves or nodes) to run the builder over. The builder may make in-place modifications to the input buffer; the input buffer should not be assumed to be in a valid state after the builder runs. + /// Dispatcher used to multithread the execution of the build. If the dispatcher is not null, pool must also not be null. + /// Buffer pool used to preallocate a pingpong buffer if the number of subtrees exceeds maximumSubtreeStackAllocationCount. If null, stack allocation or a slower in-place partitioning will be used. + /// A pool must be provided if a thread dispatcher is given. + /// Maximum number of subtrees to try putting on the stack for the binned builder's pong buffers. + /// Subtree counts larger than this threshold will either resort to a buffer pool allocation (if available) or slower in-place partition operations. + /// Minimum number of bins the builder should use per node. + /// Maximum number of bins the builder should use per node. + /// Multiplier to apply to the subtree count within a node to decide the bin count. Resulting value will then be clamped by the minimum/maximum bin counts. + /// Threshold at or under which the binned builder resorts to local counting sort sweeps. + public unsafe void BinnedBuild(Buffer subtrees, + IThreadDispatcher dispatcher = null, BufferPool pool = null, int maximumSubtreeStackAllocationCount = 4096, int minimumBinCount = 16, int maximumBinCount = 64, float leafToBinMultiplier = 1 / 16f, int microsweepThreshold = 64) + { + BinnedBuild(subtrees, Nodes, Metanodes, Leaves, dispatcher, pool, maximumSubtreeStackAllocationCount, minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold); + } } } From da21bd1826566fabdd470e4efea7c46029b31e38 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 6 May 2023 18:00:30 -0500 Subject: [PATCH 716/947] Demos updated for the mesh changes (plus some changes to helpers). --- DemoTests/PairDeterminismTests.cs | 2 +- Demos/DemoMeshHelper.cs | 21 +++++++------------ Demos/DemoSet.cs | 4 ++++ Demos/Demos/Cars/CarDemo.cs | 4 ++-- Demos/Demos/Characters/CharacterDemo.cs | 2 +- Demos/Demos/CollisionQueryDemo.cs | 2 +- Demos/Demos/CompoundDemo.cs | 4 ++-- Demos/Demos/RayCastingDemo.cs | 4 ++-- Demos/Demos/SolverContactEnumerationDemo.cs | 4 ++-- Demos/Demos/Sponsors/SponsorDemo.cs | 2 +- Demos/Demos/SweepDemo.cs | 8 +++---- Demos/Demos/Tanks/TankDemo.cs | 4 ++-- .../SpecializedTests/BatchedCollisionTests.cs | 2 +- Demos/SpecializedTests/CharacterTestDemo.cs | 4 ++-- Demos/SpecializedTests/ConvexHullTestDemo.cs | 4 ++-- Demos/SpecializedTests/CylinderTestDemo.cs | 4 ++-- .../FountainStressTestDemo.cs | 4 ++-- .../2.0/NewtDemandingSacrificeVideoDemo.cs | 2 +- .../Media/2.0/NewtVideoDemo.cs | 2 +- .../Media/2.0/ShrinkwrappedNewtsVideoDemo.cs | 2 +- .../Media/2.4/NewtTyrannyDemo.cs | 2 +- .../Media/2.4/RagdollTubeVideoDemo.cs | 2 +- .../Media/2.4/TankSwarmDemo.cs | 4 ++-- Demos/SpecializedTests/MeshMeshTestDemo.cs | 2 +- .../SpecializedTests/MeshReductionTestDemo.cs | 4 ++-- .../MeshSerializationTestDemo.cs | 2 +- Demos/SpecializedTests/MeshTestDemo.cs | 10 ++++----- .../ScalarIntegrationTestDemo.cs | 2 +- Demos/SpecializedTests/ShapePileTestDemo.cs | 2 +- .../SpecializedTests/TreeFiddlingTestDemo.cs | 2 +- Demos/SpecializedTests/TriangleTestDemo.cs | 2 +- 31 files changed, 58 insertions(+), 61 deletions(-) diff --git a/DemoTests/PairDeterminismTests.cs b/DemoTests/PairDeterminismTests.cs index 6a12fa177..35aa5272a 100644 --- a/DemoTests/PairDeterminismTests.cs +++ b/DemoTests/PairDeterminismTests.cs @@ -261,7 +261,7 @@ public static void Test() var compoundCollidable = new CollidableDescription(shapes.Add(compound), continuousDetection); var bigCompoundCollidable = new CollidableDescription(shapes.Add(bigCompound), continuousDetection); - DemoMeshHelper.CreateDeformedPlane(2, 2, (x, y) => new Vector3(x, 0, y), Vector3.One, pool, out var mesh); + var mesh = DemoMeshHelper.CreateDeformedPlane(2, 2, (x, y) => new Vector3(x, 0, y), Vector3.One, pool); var meshCollidable = new CollidableDescription(shapes.Add(mesh), continuousDetection); const int pairCount = 128; diff --git a/Demos/DemoMeshHelper.cs b/Demos/DemoMeshHelper.cs index 6f29b81fa..d599d48e1 100644 --- a/Demos/DemoMeshHelper.cs +++ b/Demos/DemoMeshHelper.cs @@ -10,7 +10,7 @@ namespace Demos { public static class DemoMeshHelper { - public static void LoadModel(ContentArchive content, BufferPool pool, string contentName, Vector3 scaling, out Mesh mesh) + public static Mesh LoadModel(ContentArchive content, BufferPool pool, string contentName, Vector3 scaling) { var meshContent = content.Load(contentName); pool.Take(meshContent.Triangles.Length, out var triangles); @@ -18,10 +18,10 @@ public static void LoadModel(ContentArchive content, BufferPool pool, string con { triangles[i] = new Triangle(meshContent.Triangles[i].A, meshContent.Triangles[i].B, meshContent.Triangles[i].C); } - mesh = new Mesh(triangles, scaling, pool); + return new Mesh(triangles, scaling, pool); } - public static void CreateFan(int triangleCount, float radius, Vector3 scaling, BufferPool pool, out Mesh mesh) + public static Mesh CreateFan(int triangleCount, float radius, Vector3 scaling, BufferPool pool) { var anglePerTriangle = 2 * MathF.PI / triangleCount; pool.Take(triangleCount, out var triangles); @@ -36,10 +36,10 @@ public static void CreateFan(int triangleCount, float radius, Vector3 scaling, B triangle.B = new Vector3(radius * MathF.Cos(secondAngle), 0, radius * MathF.Sin(secondAngle)); triangle.C = new Vector3(); } - mesh = new Mesh(triangles, scaling, pool); + return new Mesh(triangles, scaling, pool); } - public static void CreateDeformedPlane(int width, int height, Func deformer, Vector3 scaling, BufferPool pool, out Mesh mesh) + public static Mesh CreateDeformedPlane(int width, int height, Func deformer, Vector3 scaling, BufferPool pool, IThreadDispatcher dispatcher = null) { pool.Take(width * height, out var vertices); for (int i = 0; i < width; ++i) @@ -75,7 +75,7 @@ public static void CreateDeformedPlane(int width, int height, Func @@ -130,14 +130,7 @@ public unsafe static Mesh CreateGiantMeshFastWithoutBounds(Buffer tria //The special logic isn't necessary for tiny meshes, and we also don't handle the corner case of leaf counts <= 2. Just use the regular constructor. return new Mesh(triangles, scaling, pool); } - Mesh mesh = new() - { - Triangles = triangles, - Tree = new Tree(pool, triangles.Length), - Scale = scaling - }; - mesh.Tree.NodeCount = triangles.Length - 1; - mesh.Tree.LeafCount = triangles.Length; + var mesh = Mesh.CreateMeshWithoutTreeBuild(triangles, scaling, pool); int leafCounter = 0; CreateDummyNodes(ref mesh.Tree, 0, triangles.Length, ref leafCounter); for (int i = 0; i < triangles.Length; ++i) diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index 94637674e..f1f34eeab 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -46,6 +46,10 @@ struct Option public DemoSet() { + //AddOption(); + //AddOption(); + //AddOption(); + //AddOption(); AddOption(); AddOption(); AddOption(); diff --git a/Demos/Demos/Cars/CarDemo.cs b/Demos/Demos/Cars/CarDemo.cs index a6c8eb078..4c6ccaa8a 100644 --- a/Demos/Demos/Cars/CarDemo.cs +++ b/Demos/Demos/Cars/CarDemo.cs @@ -112,7 +112,7 @@ public override void Initialize(ContentArchive content, Camera camera) aiControllers[i].LaneOffset = random.NextSingle() * 20 - 10; } - DemoMeshHelper.CreateDeformedPlane(planeWidth, planeWidth, + var planeMesh = DemoMeshHelper.CreateDeformedPlane(planeWidth, planeWidth, (int vX, int vY) => { var octave0 = (MathF.Sin((vX + 5f) * 0.05f) + MathF.Sin((vY + 11) * 0.05f)) * 1.8f; @@ -129,7 +129,7 @@ public override void Initialize(ContentArchive content, Camera camera) var height = trackWeight * -10f + terrainHeight * (1 - trackWeight); return new Vector3(vertexPosition.X, height + edgeRamp, vertexPosition.Y); - }, new Vector3(1, 1, 1), BufferPool, out var planeMesh); + }, new Vector3(1, 1, 1), BufferPool); Simulation.Statics.Add(new StaticDescription(new Vector3(0, -15, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), Simulation.Shapes.Add(planeMesh))); } diff --git a/Demos/Demos/Characters/CharacterDemo.cs b/Demos/Demos/Characters/CharacterDemo.cs index 631bbcb08..aab54a873 100644 --- a/Demos/Demos/Characters/CharacterDemo.cs +++ b/Demos/Demos/Characters/CharacterDemo.cs @@ -86,7 +86,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) } //Include a giant newt to test character-newt behavior and to ensure thematic consistency. - DemoMeshHelper.LoadModel(content, BufferPool, @"Content\newt.obj", new Vector3(15, 15, 15), out var newtMesh); + var newtMesh = DemoMeshHelper.LoadModel(content, BufferPool, @"Content\newt.obj", new Vector3(15, 15, 15)); Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0.5f, 0), Simulation.Shapes.Add(newtMesh))); //Give the newt a tongue, I guess. diff --git a/Demos/Demos/CollisionQueryDemo.cs b/Demos/Demos/CollisionQueryDemo.cs index e2664d4d1..2925a85ca 100644 --- a/Demos/Demos/CollisionQueryDemo.cs +++ b/Demos/Demos/CollisionQueryDemo.cs @@ -42,7 +42,7 @@ public override void Initialize(ContentArchive content, Camera camera) } //Add in a static object to test against. Note that the mesh triangles are one sided, so some of the queries whose centers are beneath the mesh do not generate any contacts. - DemoMeshHelper.CreateDeformedPlane(20, 20, (x, y) => { return new Vector3(x * 5 - 50, 3 * MathF.Sin(x) * MathF.Sin(y), y * 5 - 50); }, Vector3.One, BufferPool, out var mesh); + var mesh = DemoMeshHelper.CreateDeformedPlane(20, 20, (x, y) => { return new Vector3(x * 5 - 50, 3 * MathF.Sin(x) * MathF.Sin(y), y * 5 - 50); }, Vector3.One, BufferPool); Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), Simulation.Shapes.Add(mesh))); } diff --git a/Demos/Demos/CompoundDemo.cs b/Demos/Demos/CompoundDemo.cs index 8ceef2e81..c6a3ea60d 100644 --- a/Demos/Demos/CompoundDemo.cs +++ b/Demos/Demos/CompoundDemo.cs @@ -202,12 +202,12 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) } const int planeWidth = 48; const int planeHeight = 48; - DemoMeshHelper.CreateDeformedPlane(planeWidth, planeHeight, + var planeMesh = DemoMeshHelper.CreateDeformedPlane(planeWidth, planeHeight, (int x, int y) => { Vector2 offsetFromCenter = new Vector2(x - planeWidth / 2, y - planeHeight / 2); return new Vector3(offsetFromCenter.X, MathF.Cos(x / 4f) * MathF.Sin(y / 4f) - 0.01f * offsetFromCenter.LengthSquared(), offsetFromCenter.Y); - }, new Vector3(2, 1, 2), BufferPool, out var planeMesh); + }, new Vector3(2, 1, 2), BufferPool); Simulation.Statics.Add(new StaticDescription(new Vector3(64, 4, 32), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), Simulation.Shapes.Add(planeMesh))); } diff --git a/Demos/Demos/RayCastingDemo.cs b/Demos/Demos/RayCastingDemo.cs index e62fba392..a446ab1a6 100644 --- a/Demos/Demos/RayCastingDemo.cs +++ b/Demos/Demos/RayCastingDemo.cs @@ -129,11 +129,11 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) const int planeWidth = 128; const int planeHeight = 128; - DemoMeshHelper.CreateDeformedPlane(planeWidth, planeHeight, + var planeMesh = DemoMeshHelper.CreateDeformedPlane(planeWidth, planeHeight, (int x, int y) => { return new Vector3(x - planeWidth / 2, 1 * MathF.Cos(x / 4f) * MathF.Sin(y / 4f), y - planeHeight / 2); - }, new Vector3(1, 3, 1), BufferPool, out var planeMesh); + }, new Vector3(1, 3, 1), BufferPool); Simulation.Statics.Add(new StaticDescription( new Vector3(0, -10, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 4), Simulation.Shapes.Add(planeMesh))); diff --git a/Demos/Demos/SolverContactEnumerationDemo.cs b/Demos/Demos/SolverContactEnumerationDemo.cs index de51a7571..ce6132f76 100644 --- a/Demos/Demos/SolverContactEnumerationDemo.cs +++ b/Demos/Demos/SolverContactEnumerationDemo.cs @@ -224,11 +224,11 @@ public override void Initialize(ContentArchive content, Camera camera) //Put a mesh under the sensor so that nonconvex contacts are shown. const int planeWidth = 128; const int planeHeight = 128; - DemoMeshHelper.CreateDeformedPlane(planeWidth, planeHeight, + var planeMesh = DemoMeshHelper.CreateDeformedPlane(planeWidth, planeHeight, (int x, int y) => { return new Vector3(x - planeWidth / 2, 1 * MathF.Cos(x / 2f) * MathF.Sin(y / 2f), y - planeHeight / 2); - }, new Vector3(2, 1, 2), BufferPool, out var planeMesh); + }, new Vector3(2, 1, 2), BufferPool); Simulation.Statics.Add(new StaticDescription(new Vector3(0, -2, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), Simulation.Shapes.Add(planeMesh))); } diff --git a/Demos/Demos/Sponsors/SponsorDemo.cs b/Demos/Demos/Sponsors/SponsorDemo.cs index 580129e21..56c2ff725 100644 --- a/Demos/Demos/Sponsors/SponsorDemo.cs +++ b/Demos/Demos/Sponsors/SponsorDemo.cs @@ -115,7 +115,7 @@ public override void Initialize(ContentArchive content, Camera camera) characterControllers = new CharacterControllers(BufferPool); Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characterControllers), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); - DemoMeshHelper.LoadModel(content, BufferPool, @"Content\newt.obj", new Vector3(-10, 10, -10), out var newtMesh); + var newtMesh = DemoMeshHelper.LoadModel(content, BufferPool, @"Content\newt.obj", new Vector3(-10, 10, -10)); var newtShape = Simulation.Shapes.Add(newtMesh); newts = new QuickList(sponsors2.Count, BufferPool); newtArenaMin = new Vector2(-100); diff --git a/Demos/Demos/SweepDemo.cs b/Demos/Demos/SweepDemo.cs index 4d1b6eccf..26897ffe8 100644 --- a/Demos/Demos/SweepDemo.cs +++ b/Demos/Demos/SweepDemo.cs @@ -102,11 +102,11 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) const int planeWidth = 64; const int planeHeight = 64; - DemoMeshHelper.CreateDeformedPlane(planeWidth, planeHeight, + var planeMesh = DemoMeshHelper.CreateDeformedPlane(planeWidth, planeHeight, (int x, int y) => { return new Vector3(x, 1 * MathF.Cos(x / 4f) * MathF.Sin(y / 4f), y); - }, new Vector3(2, 3, 2), BufferPool, out var planeMesh); + }, new Vector3(2, 3, 2), BufferPool); Simulation.Statics.Add(new StaticDescription(new Vector3(-64, -10, -64), Simulation.Shapes.Add(planeMesh))); } @@ -281,11 +281,11 @@ public override void Render(Renderer renderer, Camera camera, Input input, TextB const int planeWidth = 3; const int planeHeight = 3; - DemoMeshHelper.CreateDeformedPlane(planeWidth, planeHeight, + var mesh = DemoMeshHelper.CreateDeformedPlane(planeWidth, planeHeight, (int x, int y) => { return new Vector3(x - 1.5f, 0.1f * MathF.Cos(x) * MathF.Sin(y), y - 1.5f); - }, new Vector3(1, 2, 1), BufferPool, out var mesh); + }, new Vector3(1, 2, 1), BufferPool); var triangle = new Triangle(new Vector3(0, 0, 0), new Vector3(2, 0, -1), new Vector3(-1, 0, 1.5f)); diff --git a/Demos/Demos/Tanks/TankDemo.cs b/Demos/Demos/Tanks/TankDemo.cs index 469ad668b..7e06c294a 100644 --- a/Demos/Demos/Tanks/TankDemo.cs +++ b/Demos/Demos/Tanks/TankDemo.cs @@ -122,12 +122,12 @@ public override void Initialize(ContentArchive content, Camera camera) Simulation.Shapes.Add(buildingShape))); } - DemoMeshHelper.CreateDeformedPlane(planeWidth, planeWidth, + var planeMesh = DemoMeshHelper.CreateDeformedPlane(planeWidth, planeWidth, (int vX, int vY) => { var position2D = new Vector2(vX, vY) * terrainScale + terrainPosition; return new Vector3(position2D.X, GetHeightForPosition(position2D.X, position2D.Y, planeWidth, inverseTerrainScale, terrainPosition), position2D.Y); - }, new Vector3(1, 1, 1), BufferPool, out var planeMesh); + }, new Vector3(1, 1, 1), BufferPool); Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), Simulation.Shapes.Add(planeMesh))); explosions = new QuickList(32, BufferPool); diff --git a/Demos/SpecializedTests/BatchedCollisionTests.cs b/Demos/SpecializedTests/BatchedCollisionTests.cs index f9f449d0d..0f2f53f0c 100644 --- a/Demos/SpecializedTests/BatchedCollisionTests.cs +++ b/Demos/SpecializedTests/BatchedCollisionTests.cs @@ -219,7 +219,7 @@ public static void Test() var bigCompound = new BigCompound(children, shapes, pool); //MESH - DemoMeshHelper.CreateDeformedPlane(8, 8, (x, y) => { return new Vector3(x * 2 - 8, 3 * MathF.Sin(x) * MathF.Sin(y), y * 2 - 8); }, Vector3.One, pool, out var mesh); + var mesh = DemoMeshHelper.CreateDeformedPlane(8, 8, (x, y) => { return new Vector3(x * 2 - 8, 3 * MathF.Sin(x) * MathF.Sin(y), y * 2 - 8); }, Vector3.One, pool); int iterationCount = 1 << 20; pool.Take(iterationCount, out var posesA); diff --git a/Demos/SpecializedTests/CharacterTestDemo.cs b/Demos/SpecializedTests/CharacterTestDemo.cs index c1dd6b7e0..69ff2fc67 100644 --- a/Demos/SpecializedTests/CharacterTestDemo.cs +++ b/Demos/SpecializedTests/CharacterTestDemo.cs @@ -78,12 +78,12 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) const int planeWidth = 256; const int planeHeight = 256; - DemoMeshHelper.CreateDeformedPlane(planeWidth, planeHeight, + var planeMesh = DemoMeshHelper.CreateDeformedPlane(planeWidth, planeHeight, (int x, int y) => { Vector2 offsetFromCenter = new Vector2(x - planeWidth / 2, y - planeHeight / 2); return new Vector3(offsetFromCenter.X, MathF.Cos(x / 2f) + MathF.Sin(y / 2f), offsetFromCenter.Y); - }, new Vector3(2, 1, 2), BufferPool, out var planeMesh); + }, new Vector3(2, 1, 2), BufferPool); Simulation.Statics.Add(new StaticDescription(new Vector3(0, -2, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), Simulation.Shapes.Add(planeMesh))); removedCharacters = new QuickQueue(characters.CharacterCount, BufferPool); diff --git a/Demos/SpecializedTests/ConvexHullTestDemo.cs b/Demos/SpecializedTests/ConvexHullTestDemo.cs index 82b2a2548..bc18de0ea 100644 --- a/Demos/SpecializedTests/ConvexHullTestDemo.cs +++ b/Demos/SpecializedTests/ConvexHullTestDemo.cs @@ -429,10 +429,10 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Simulation.Statics.Add(new StaticDescription(new Vector3(0, -10, 0), Simulation.Shapes.Add(new Box(1000, 1, 1000)))); Random random = new Random(5); - DemoMeshHelper.CreateDeformedPlane(64, 64, (x, y) => new Vector3( + var mesh = DemoMeshHelper.CreateDeformedPlane(64, 64, (x, y) => new Vector3( x + 8, 2f * MathF.Sin(x * 0.125f) * MathF.Sin(y * 0.125f) + 0.1f * random.NextSingle() - 3, - y - 8), new Vector3(1, 1, 1), BufferPool, out var mesh); + y - 8), new Vector3(1, 1, 1), BufferPool); Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); } diff --git a/Demos/SpecializedTests/CylinderTestDemo.cs b/Demos/SpecializedTests/CylinderTestDemo.cs index bbf039c00..3f43ad87a 100644 --- a/Demos/SpecializedTests/CylinderTestDemo.cs +++ b/Demos/SpecializedTests/CylinderTestDemo.cs @@ -249,7 +249,7 @@ public override void Initialize(ContentArchive content, Camera camera) const int planeWidth = 50; const int planeHeight = 50; - DemoMeshHelper.CreateDeformedPlane(planeWidth, planeHeight, + var planeMesh = DemoMeshHelper.CreateDeformedPlane(planeWidth, planeHeight, (int x, int y) => { var octave0 = (MathF.Sin((x + 5f) * 0.05f) + MathF.Sin((y + 11) * 0.05f)) * 3f; @@ -258,7 +258,7 @@ public override void Initialize(ContentArchive content, Camera camera) var octave3 = (MathF.Sin((x + 53) * 0.65f) + MathF.Sin((y + 47) * 0.65f)) * 0.5f; var octave4 = (MathF.Sin((x + 67) * 1.50f) + MathF.Sin((y + 13) * 1.5f)) * 0.25f; return new Vector3(x, octave0 + octave1 + octave2 + octave3 + octave4, y); - }, new Vector3(4, 1, 4), BufferPool, out var planeMesh); + }, new Vector3(4, 1, 4), BufferPool); Simulation.Statics.Add(new StaticDescription(new Vector3(-100, -15, 100), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), Simulation.Shapes.Add(planeMesh))); //Simulation.Statics.Add(new StaticDescription(new Vector3(0, -10, 0), Quaternion.CreateFromAxisAngle(Vector3.Normalize(new Vector3(1, 0, 1)), 0), Simulation.Shapes.Add(new Cylinder(100, 1f)), 0.1f)); diff --git a/Demos/SpecializedTests/FountainStressTestDemo.cs b/Demos/SpecializedTests/FountainStressTestDemo.cs index 5007d73e2..92b4f2beb 100644 --- a/Demos/SpecializedTests/FountainStressTestDemo.cs +++ b/Demos/SpecializedTests/FountainStressTestDemo.cs @@ -41,12 +41,12 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) const int planeWidth = 8; const int planeHeight = 8; - DemoMeshHelper.CreateDeformedPlane(planeWidth, planeHeight, + var staticShape = DemoMeshHelper.CreateDeformedPlane(planeWidth, planeHeight, (int x, int y) => { Vector2 offsetFromCenter = new Vector2(x - planeWidth / 2, y - planeHeight / 2); return new Vector3(offsetFromCenter.X, MathF.Cos(x / 4f) * MathF.Sin(y / 4f) - 0.2f * offsetFromCenter.LengthSquared(), offsetFromCenter.Y); - }, new Vector3(2, 1, 2), BufferPool, out var staticShape); + }, new Vector3(2, 1, 2), BufferPool); var staticShapeIndex = Simulation.Shapes.Add(staticShape); const int staticGridWidthInInstances = 128; const float staticSpacing = 8; diff --git a/Demos/SpecializedTests/Media/2.0/NewtDemandingSacrificeVideoDemo.cs b/Demos/SpecializedTests/Media/2.0/NewtDemandingSacrificeVideoDemo.cs index d5b5e2c43..9a0296c07 100644 --- a/Demos/SpecializedTests/Media/2.0/NewtDemandingSacrificeVideoDemo.cs +++ b/Demos/SpecializedTests/Media/2.0/NewtDemandingSacrificeVideoDemo.cs @@ -31,7 +31,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //High fidelity simulation isn't super important on this one. Simulation.Solver.VelocityIterationCount = 2; - DemoMeshHelper.LoadModel(content, BufferPool, "Content\\newt.obj", new Vector3(30), out var mesh); + var mesh = DemoMeshHelper.LoadModel(content, BufferPool, "Content\\newt.obj", new Vector3(30)); Simulation.Statics.Add(new StaticDescription(new Vector3(0, 20, 0), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, 0), Simulation.Shapes.Add(mesh))); } diff --git a/Demos/SpecializedTests/Media/2.0/NewtVideoDemo.cs b/Demos/SpecializedTests/Media/2.0/NewtVideoDemo.cs index 242aee926..5a0459742 100644 --- a/Demos/SpecializedTests/Media/2.0/NewtVideoDemo.cs +++ b/Demos/SpecializedTests/Media/2.0/NewtVideoDemo.cs @@ -47,7 +47,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var bulletShape = new Sphere(0.5f); bulletDescription = BodyDescription.CreateDynamic(RigidPose.Identity, bulletShape.ComputeInertia(.25f), Simulation.Shapes.Add(bulletShape), 0.01f); - DemoMeshHelper.LoadModel(content, BufferPool, "Content\\newt.obj", new Vector3(20), out var mesh); + var mesh = DemoMeshHelper.LoadModel(content, BufferPool, "Content\\newt.obj", new Vector3(20)); Simulation.Statics.Add(new StaticDescription(new Vector3(200, 0.5f, 120), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, -3 * MathHelper.PiOver4), Simulation.Shapes.Add(mesh))); } BodyDescription bulletDescription; diff --git a/Demos/SpecializedTests/Media/2.0/ShrinkwrappedNewtsVideoDemo.cs b/Demos/SpecializedTests/Media/2.0/ShrinkwrappedNewtsVideoDemo.cs index ccd44adaf..3b624e3bd 100644 --- a/Demos/SpecializedTests/Media/2.0/ShrinkwrappedNewtsVideoDemo.cs +++ b/Demos/SpecializedTests/Media/2.0/ShrinkwrappedNewtsVideoDemo.cs @@ -52,7 +52,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) - DemoMeshHelper.LoadModel(content, BufferPool, "Content\\newt.obj", new Vector3(1, 1.5f, 1), out mesh); + mesh = DemoMeshHelper.LoadModel(content, BufferPool, "Content\\newt.obj", new Vector3(1, 1.5f, 1)); Simulation.Statics.Add(new StaticDescription(new Vector3(30, 0, 20), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, -3 * MathHelper.PiOver4), Simulation.Shapes.Add(mesh))); } diff --git a/Demos/SpecializedTests/Media/2.4/NewtTyrannyDemo.cs b/Demos/SpecializedTests/Media/2.4/NewtTyrannyDemo.cs index 2bb5ea1fb..915475972 100644 --- a/Demos/SpecializedTests/Media/2.4/NewtTyrannyDemo.cs +++ b/Demos/SpecializedTests/Media/2.4/NewtTyrannyDemo.cs @@ -33,7 +33,7 @@ public override void Initialize(ContentArchive content, Camera camera) Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characterControllers), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); Simulation.Deterministic = true; - DemoMeshHelper.LoadModel(content, BufferPool, @"Content\newt.obj", new Vector3(-10, 10, -10), out var newtMesh); + var newtMesh = DemoMeshHelper.LoadModel(content, BufferPool, @"Content\newt.obj", new Vector3(-10, 10, -10)); var newtShape = Simulation.Shapes.Add(newtMesh); var newtCount = 10; newts = new QuickList(newtCount, BufferPool); diff --git a/Demos/SpecializedTests/Media/2.4/RagdollTubeVideoDemo.cs b/Demos/SpecializedTests/Media/2.4/RagdollTubeVideoDemo.cs index 633306c80..d895d40e7 100644 --- a/Demos/SpecializedTests/Media/2.4/RagdollTubeVideoDemo.cs +++ b/Demos/SpecializedTests/Media/2.4/RagdollTubeVideoDemo.cs @@ -75,7 +75,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var staticDescription = new StaticDescription(new Vector3(0, -0.5f, 0), staticShapeIndex); Simulation.Statics.Add(staticDescription); - DemoMeshHelper.LoadModel(content, BufferPool, @"Content\newt.obj", new Vector3(15, 15, 15), out var newtMesh); + var newtMesh = DemoMeshHelper.LoadModel(content, BufferPool, @"Content\newt.obj", new Vector3(15, 15, 15)); Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0.5f, 80), Quaternion.CreateFromAxisAngle(Vector3.UnitY, MathF.PI), Simulation.Shapes.Add(newtMesh))); } diff --git a/Demos/SpecializedTests/Media/2.4/TankSwarmDemo.cs b/Demos/SpecializedTests/Media/2.4/TankSwarmDemo.cs index 0476c9b7b..329e9679c 100644 --- a/Demos/SpecializedTests/Media/2.4/TankSwarmDemo.cs +++ b/Demos/SpecializedTests/Media/2.4/TankSwarmDemo.cs @@ -123,12 +123,12 @@ public override void Initialize(ContentArchive content, Camera camera) Simulation.Shapes.Add(buildingShape))); } - DemoMeshHelper.CreateDeformedPlane(planeWidth, planeWidth, + var planeMesh = DemoMeshHelper.CreateDeformedPlane(planeWidth, planeWidth, (int vX, int vY) => { var position2D = new Vector2(vX, vY) * terrainScale + terrainPosition; return new Vector3(position2D.X, GetHeightForPosition(position2D.X, position2D.Y, planeWidth, inverseTerrainScale, terrainPosition), position2D.Y); - }, new Vector3(1, 1, 1), BufferPool, out var planeMesh); + }, new Vector3(1, 1, 1), BufferPool); Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), Simulation.Shapes.Add(planeMesh))); explosions = new QuickList(32, BufferPool); diff --git a/Demos/SpecializedTests/MeshMeshTestDemo.cs b/Demos/SpecializedTests/MeshMeshTestDemo.cs index 6c0c5992d..894d2efd4 100644 --- a/Demos/SpecializedTests/MeshMeshTestDemo.cs +++ b/Demos/SpecializedTests/MeshMeshTestDemo.cs @@ -24,7 +24,7 @@ public override void Initialize(ContentArchive content, Camera camera) Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); - DemoMeshHelper.LoadModel(content, BufferPool, @"Content\newt.obj", Vector3.One, out var mesh); + var mesh = DemoMeshHelper.LoadModel(content, BufferPool, @"Content\newt.obj", Vector3.One); var approximateInertia = new Box(2.5f, 1, 4).ComputeInertia(1); var meshShapeIndex = Simulation.Shapes.Add(mesh); for (int meshIndex = 0; meshIndex < 3; ++meshIndex) diff --git a/Demos/SpecializedTests/MeshReductionTestDemo.cs b/Demos/SpecializedTests/MeshReductionTestDemo.cs index ba803a383..7c58de6d3 100644 --- a/Demos/SpecializedTests/MeshReductionTestDemo.cs +++ b/Demos/SpecializedTests/MeshReductionTestDemo.cs @@ -47,7 +47,7 @@ public override void Initialize(ContentArchive content, Camera camera) Vector3 span = new Vector3(planeWidth * scale * 0.9f, 15, planeWidth * scale * 0.9f); - DemoMeshHelper.CreateDeformedPlane(planeWidth, planeWidth, + var planeMesh = DemoMeshHelper.CreateDeformedPlane(planeWidth, planeWidth, (int vX, int vY) => { var octave0 = (MathF.Sin((vX + 5f) * 0.05f) + MathF.Sin((vY + 11) * 0.05f)) * 1.8f; @@ -61,7 +61,7 @@ public override void Initialize(ContentArchive content, Camera camera) var vertexPosition = new Vector2(vX * scale, vY * scale) + terrainPosition; return new Vector3(vertexPosition.X, terrainHeight + edgeRamp, vertexPosition.Y); - }, new Vector3(1, 1, 1), BufferPool, out var planeMesh); + }, new Vector3(1, 1, 1), BufferPool); Simulation.Statics.Add(new StaticDescription(new Vector3(0, -15, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), Simulation.Shapes.Add(planeMesh))); var testBox = new Box(3, 3, 3); diff --git a/Demos/SpecializedTests/MeshSerializationTestDemo.cs b/Demos/SpecializedTests/MeshSerializationTestDemo.cs index d347f8570..cbd54c952 100644 --- a/Demos/SpecializedTests/MeshSerializationTestDemo.cs +++ b/Demos/SpecializedTests/MeshSerializationTestDemo.cs @@ -21,7 +21,7 @@ public override void Initialize(ContentArchive content, Camera camera) Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); var startTime = Stopwatch.GetTimestamp(); - DemoMeshHelper.CreateDeformedPlane(1025, 1025, (x, y) => new Vector3(x * 0.125f, MathF.Sin(x) + MathF.Sin(y), y * 0.125f), Vector3.One, BufferPool, out var originalMesh); + var originalMesh = DemoMeshHelper.CreateDeformedPlane(1025, 1025, (x, y) => new Vector3(x * 0.125f, MathF.Sin(x) + MathF.Sin(y), y * 0.125f), Vector3.One, BufferPool); Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), Simulation.Shapes.Add(originalMesh))); var endTime = Stopwatch.GetTimestamp(); var freshConstructionTime = (endTime - startTime) / (double)Stopwatch.Frequency; diff --git a/Demos/SpecializedTests/MeshTestDemo.cs b/Demos/SpecializedTests/MeshTestDemo.cs index d1fe43b0f..586660e95 100644 --- a/Demos/SpecializedTests/MeshTestDemo.cs +++ b/Demos/SpecializedTests/MeshTestDemo.cs @@ -68,24 +68,24 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) //Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(10, 10, 10), testInertia, new CollidableDescription(Simulation.Shapes.Add(testShape), 10.1f), new BodyActivityDescription(-0.01f))); - DemoMeshHelper.LoadModel(content, BufferPool, @"Content\newt.obj", new Vector3(5, 5, 5), out var newtMesh); + var newtMesh = DemoMeshHelper.LoadModel(content, BufferPool, @"Content\newt.obj", new Vector3(5, 5, 5)); Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(30, 20, 30), newtMesh.ComputeClosedInertia(10), Simulation.Shapes.Add(newtMesh), 0.01f)); Simulation.Statics.Add(new StaticDescription(new Vector3(30, 15, 30), Simulation.Shapes.Add(new Box(15, 1, 15)))); - DemoMeshHelper.LoadModel(content, BufferPool, @"Content\box.obj", new Vector3(5, 1, 5), out var boxMesh); + var boxMesh = DemoMeshHelper.LoadModel(content, BufferPool, @"Content\box.obj", new Vector3(5, 1, 5)); Simulation.Statics.Add(new StaticDescription(new Vector3(10, 5, -20), Simulation.Shapes.Add(boxMesh))); - DemoMeshHelper.CreateFan(64, 16, new Vector3(1, 1, 1), BufferPool, out var fanMesh); + var fanMesh = DemoMeshHelper.CreateFan(64, 16, new Vector3(1, 1, 1), BufferPool); Simulation.Statics.Add(new StaticDescription(new Vector3(-10, 0, -20), Simulation.Shapes.Add(fanMesh))); const int planeWidth = 128; const int planeHeight = 128; - DemoMeshHelper.CreateDeformedPlane(planeWidth, planeHeight, + var planeMesh = DemoMeshHelper.CreateDeformedPlane(planeWidth, planeHeight, (int x, int y) => { return new Vector3(x - planeWidth / 2, 1 * MathF.Cos(x / 2f) * MathF.Sin(y / 2f), y - planeHeight / 2); - }, new Vector3(2, 1, 2), BufferPool, out var planeMesh); + }, new Vector3(2, 1, 2), BufferPool); Simulation.Statics.Add(new StaticDescription(new Vector3(64, -10, 64), BepuUtilities.QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), Simulation.Shapes.Add(planeMesh))); } diff --git a/Demos/SpecializedTests/ScalarIntegrationTestDemo.cs b/Demos/SpecializedTests/ScalarIntegrationTestDemo.cs index fca204c7d..e0e5695a1 100644 --- a/Demos/SpecializedTests/ScalarIntegrationTestDemo.cs +++ b/Demos/SpecializedTests/ScalarIntegrationTestDemo.cs @@ -186,7 +186,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) } //Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(new Box(500, 1, 500)))); - DemoMeshHelper.CreateDeformedPlane(128, 128, (x, y) => new Vector3(x - 64, 2f * (float)(Math.Sin(x * 0.5f) * Math.Sin(y * 0.5f)), y - 64), new Vector3(4, 1, 4), BufferPool, out var mesh); + var mesh = DemoMeshHelper.CreateDeformedPlane(128, 128, (x, y) => new Vector3(x - 64, 2f * (float)(Math.Sin(x * 0.5f) * Math.Sin(y * 0.5f)), y - 64), new Vector3(4, 1, 4), BufferPool); Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); } diff --git a/Demos/SpecializedTests/ShapePileTestDemo.cs b/Demos/SpecializedTests/ShapePileTestDemo.cs index b4ef43d7f..1a5219ef2 100644 --- a/Demos/SpecializedTests/ShapePileTestDemo.cs +++ b/Demos/SpecializedTests/ShapePileTestDemo.cs @@ -129,7 +129,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) } //Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(new Box(500, 1, 500)))); - DemoMeshHelper.CreateDeformedPlane(128, 128, (x, y) => new Vector3(x - 64, 2f * (float)(Math.Sin(x * 0.5f) * Math.Sin(y * 0.5f)), y - 64), new Vector3(4, 1, 4), BufferPool, out var mesh); + var mesh = DemoMeshHelper.CreateDeformedPlane(128, 128, (x, y) => new Vector3(x - 64, 2f * (float)(Math.Sin(x * 0.5f) * Math.Sin(y * 0.5f)), y - 64), new Vector3(4, 1, 4), BufferPool); Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); } diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index 824cb07cf..27a2ba4be 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -187,7 +187,7 @@ public override void Initialize(ContentArchive content, Camera camera) BinnedTest(setup, () => { - Tree.BinnedBuilder(subtrees, mesh.Tree.Nodes, mesh.Tree.Metanodes, mesh.Tree.Leaves, ThreadDispatcher, BufferPool); + mesh.Tree.BinnedBuild(subtrees, pool: BufferPool); }, "Revamp Single Axis MT", ref mesh.Tree); //BufferPool.Take(mesh.Triangles.Length, out var leafBounds); diff --git a/Demos/SpecializedTests/TriangleTestDemo.cs b/Demos/SpecializedTests/TriangleTestDemo.cs index 996830a87..e337a84f3 100644 --- a/Demos/SpecializedTests/TriangleTestDemo.cs +++ b/Demos/SpecializedTests/TriangleTestDemo.cs @@ -184,7 +184,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) } - DemoMeshHelper.LoadModel(content, BufferPool, "Content\\newt.obj", new Vector3(3), out var mesh); + var mesh = DemoMeshHelper.LoadModel(content, BufferPool, "Content\\newt.obj", new Vector3(3)); var collidable = new CollidableDescription(Simulation.Shapes.Add(mesh), 2f, 2f, ContinuousDetection.Discrete); var newtInertia = mesh.ComputeClosedInertia(1); for (int i = 0; i < 5; ++i) From c98e072cd155ed3fc5cdca3400c0744b2d376903 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 6 May 2023 18:11:03 -0500 Subject: [PATCH 717/947] Demo mesh construction now actually uses multithreading on car/tank demo. --- Demos/DemoMeshHelper.cs | 2 +- Demos/Demos/Cars/CarDemo.cs | 3 ++- Demos/Demos/Tanks/TankDemo.cs | 2 +- 3 files changed, 4 insertions(+), 3 deletions(-) diff --git a/Demos/DemoMeshHelper.cs b/Demos/DemoMeshHelper.cs index d599d48e1..5fe778d90 100644 --- a/Demos/DemoMeshHelper.cs +++ b/Demos/DemoMeshHelper.cs @@ -75,7 +75,7 @@ public static Mesh CreateDeformedPlane(int width, int height, Func diff --git a/Demos/Demos/Cars/CarDemo.cs b/Demos/Demos/Cars/CarDemo.cs index 4c6ccaa8a..fd56bc932 100644 --- a/Demos/Demos/Cars/CarDemo.cs +++ b/Demos/Demos/Cars/CarDemo.cs @@ -1,4 +1,5 @@ using System; +using System.Diagnostics; using System.Numerics; using BepuPhysics; using BepuPhysics.Collidables; @@ -129,7 +130,7 @@ public override void Initialize(ContentArchive content, Camera camera) var height = trackWeight * -10f + terrainHeight * (1 - trackWeight); return new Vector3(vertexPosition.X, height + edgeRamp, vertexPosition.Y); - }, new Vector3(1, 1, 1), BufferPool); + }, new Vector3(1, 1, 1), BufferPool, ThreadDispatcher); Simulation.Statics.Add(new StaticDescription(new Vector3(0, -15, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), Simulation.Shapes.Add(planeMesh))); } diff --git a/Demos/Demos/Tanks/TankDemo.cs b/Demos/Demos/Tanks/TankDemo.cs index 7e06c294a..0177ca6e2 100644 --- a/Demos/Demos/Tanks/TankDemo.cs +++ b/Demos/Demos/Tanks/TankDemo.cs @@ -127,7 +127,7 @@ public override void Initialize(ContentArchive content, Camera camera) { var position2D = new Vector2(vX, vY) * terrainScale + terrainPosition; return new Vector3(position2D.X, GetHeightForPosition(position2D.X, position2D.Y, planeWidth, inverseTerrainScale, terrainPosition), position2D.Y); - }, new Vector3(1, 1, 1), BufferPool); + }, new Vector3(1, 1, 1), BufferPool, ThreadDispatcher); Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), Simulation.Shapes.Add(planeMesh))); explosions = new QuickList(32, BufferPool); From b5500eb06354ba5039706426e5ed6caa43b6c63a Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 6 May 2023 18:45:05 -0500 Subject: [PATCH 718/947] Capacity->count awareness in instance overload. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 20772b351..c83043d6d 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -1450,7 +1450,7 @@ public static unsafe void BinnedBuild(Buffer subtrees, Buffer n public unsafe void BinnedBuild(Buffer subtrees, IThreadDispatcher dispatcher = null, BufferPool pool = null, int maximumSubtreeStackAllocationCount = 4096, int minimumBinCount = 16, int maximumBinCount = 64, float leafToBinMultiplier = 1 / 16f, int microsweepThreshold = 64) { - BinnedBuild(subtrees, Nodes, Metanodes, Leaves, dispatcher, pool, maximumSubtreeStackAllocationCount, minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold); + BinnedBuild(subtrees, Nodes.Slice(NodeCount), Metanodes.Slice(NodeCount), Leaves.Slice(LeafCount), dispatcher, pool, maximumSubtreeStackAllocationCount, minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold); } } } From 9f1fa32a761e822878aade56cda71c2edaf0899e Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 6 May 2023 18:58:05 -0500 Subject: [PATCH 719/947] Added sweep builder helper, just in case. --- BepuPhysics/Collidables/Mesh.cs | 26 +++++++++++++++++++++++++- BepuPhysics/Trees/Tree_SweepBuilder.cs | 2 -- 2 files changed, 25 insertions(+), 3 deletions(-) diff --git a/BepuPhysics/Collidables/Mesh.cs b/BepuPhysics/Collidables/Mesh.cs index d680d3958..5b9b849b0 100644 --- a/BepuPhysics/Collidables/Mesh.cs +++ b/BepuPhysics/Collidables/Mesh.cs @@ -112,6 +112,30 @@ public static Mesh CreateMeshWithoutTreeBuild(Buffer triangles, Vector return mesh; } + /// + /// Creates a mesh shape instance and builds an acceleration structure using a sweep builder. + /// + /// Triangles to use in the mesh. + /// Scale to apply to all vertices at runtime. + /// Note that the scale is not baked into the triangles or acceleration structure; the same set of triangles and acceleration structure can be used across multiple Mesh instances with different scales. + /// Pool used to allocate acceleration structures. + /// Created mesh shape. + /// The sweep builder is significantly slower than the binned builder, but can sometimes create higher quality trees. + /// Note that the binned builder can be tuned to create higher quality trees. That is usually a better choice than trying to use the sweep builder; this is here primarily for legacy reasons. + public unsafe static Mesh CreateMeshWithSweepBuild(Buffer triangles, Vector3 scale, BufferPool pool) + { + var mesh = CreateMeshWithoutTreeBuild(triangles, scale, pool); + pool.Take(triangles.Length, out var subtrees); + FillSubtreesForTriangles(triangles, subtrees); + Debug.Assert(sizeof(BoundingBox) == sizeof(NodeChild), + "This assumption *should* hold, because the binned builder relies on it. If it doesn't, something weird as happened." + + "Did you forget about this requirement when revamping for 64 bit or something?"); + //NodeChild intentionally shares the same memory layout as BoundingBox. NodeChild just includes some extra data in the fields unused by bounds. + mesh.Tree.SweepBuild(pool, subtrees.As()); + pool.Return(ref subtrees); + return mesh; + } + /// /// Creates a mesh shape. /// @@ -122,7 +146,7 @@ public static Mesh CreateMeshWithoutTreeBuild(Buffer triangles, Vector /// Dispatcher to use to multithread the execution of the mesh build process. If null, the build will be single threaded. public Mesh(Buffer triangles, Vector3 scale, BufferPool pool, IThreadDispatcher dispatcher = null) { - this = CreateMeshWithoutTreeBuild(triangles, scale, pool); + this = CreateMeshWithoutTreeBuild(triangles, scale, pool); pool.Take(triangles.Length, out var subtrees); FillSubtreesForTriangles(triangles, subtrees); Tree.BinnedBuild(subtrees, dispatcher: dispatcher, pool: pool); diff --git a/BepuPhysics/Trees/Tree_SweepBuilder.cs b/BepuPhysics/Trees/Tree_SweepBuilder.cs index 98f34db67..a55be90d5 100644 --- a/BepuPhysics/Trees/Tree_SweepBuilder.cs +++ b/BepuPhysics/Trees/Tree_SweepBuilder.cs @@ -249,8 +249,6 @@ public unsafe void SweepBuild(BufferPool pool, Buffer leafBounds) { if (leafBounds.Length <= 0) throw new ArgumentException("Length must be positive."); - if (LeafCount != 0) - throw new InvalidOperationException("Cannot build a tree that already contains nodes."); //The tree is built with an empty node at the root to make insertion work more easily. //As long as that is the case (and as long as this is not a constructor), //we must clear it out. From 1cab993b206e18d4a9ac6a121c5aab358507c852 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 6 May 2023 21:04:03 -0500 Subject: [PATCH 720/947] BigCompound now uses the new binned build, like the mesh. --- BepuPhysics/Collidables/BigCompound.cs | 91 ++++++++++++++++++++++---- BepuPhysics/Collidables/Compound.cs | 2 +- BepuPhysics/Collidables/Mesh.cs | 8 +-- Demos/DemoMeshHelper.cs | 2 +- 4 files changed, 83 insertions(+), 20 deletions(-) diff --git a/BepuPhysics/Collidables/BigCompound.cs b/BepuPhysics/Collidables/BigCompound.cs index c0050b852..2df265532 100644 --- a/BepuPhysics/Collidables/BigCompound.cs +++ b/BepuPhysics/Collidables/BigCompound.cs @@ -26,28 +26,91 @@ public struct BigCompound : ICompoundShape /// public Buffer Children; + /// + /// Creates a BigCompound shape instance, but leaves the Tree in an unbuilt state. The Tree must be built before the compound can be used. + /// + /// Children to use in the compound. + /// Pool used to allocate acceleration structures. + /// Created compound shape. + /// In some cases, the default binned build may not be the ideal builder. This function does everything needed to set up a tree without the expense of figuring out the details of the acceleration structure. + /// The user can then run whatever build/refinement process is appropriate. + public static BigCompound CreateWithoutTreeBuild(Buffer children, BufferPool pool) + { + Debug.Assert(children.Length > 0, "Compounds must have a nonzero number of children."); + BigCompound compound = default; + compound.Children = children; + compound.Tree = new Tree(pool, children.Length) + { + //If this codepath is being used, we're assuming that the children are as given. + //so we can go ahead and set the node/leaf counts. + //(This is in contrast to creating a tree with a certain capacity, but then relying on incremental adds/removes later.) + NodeCount = children.Length - 1, + LeafCount = children.Length + }; + return compound; + } + + + /// + /// Fills a buffer of subtrees according to a buffer of triangles. + /// + /// The term "subtree" is used because the binned builder does not care whether the input came from leaf nodes or a refinement process's internal nodes. + /// Children to build subtrees from. + /// Shapes set in which child shapes are allocated. + /// Subtrees created for the triangles. + public static void FillSubtreesForChildren(Span children, Shapes shapes, Span subtrees) + { + if (subtrees.Length != children.Length) + throw new ArgumentException("Triangles and subtrees span lengths should match."); + Debug.Assert(Compound.ValidateChildIndices(children, shapes), "Children must all be convex."); + for (int i = 0; i < children.Length; ++i) + { + ref var t = ref children[i]; + ref var subtree = ref subtrees[i]; + Compound.ComputeChildBounds(children[i], Quaternion.Identity, shapes, out subtree.Min, out subtree.Max); + subtree.LeafCount = 1; + subtree.Index = Tree.Encode(i); + } + } + + /// + /// Creates a compound shape instance and builds an acceleration structure using a sweep builder. + /// + /// Children to use in the compound. + /// Shapes set in which child shapes are allocated. + /// Pool used to allocate acceleration structures. + /// Created compound shape. + /// The sweep builder is significantly slower than the binned builder, but can sometimes create higher quality trees. + /// Note that the binned builder can be tuned to create higher quality trees. That is usually a better choice than trying to use the sweep builder; this is here primarily for legacy reasons. + public unsafe static BigCompound CreateWithSweepBuild(Buffer children, Shapes shapes, BufferPool pool) + { + var compound = CreateWithoutTreeBuild(children, pool); + pool.Take(children.Length, out var subtrees); + FillSubtreesForChildren(children, shapes, subtrees); + Debug.Assert(sizeof(BoundingBox) == sizeof(NodeChild), + "This assumption *should* hold, because the binned builder relies on it. If it doesn't, something weird as happened." + + "Did you forget about this requirement when revamping for 64 bit or something?"); + //NodeChild intentionally shares the same memory layout as BoundingBox. NodeChild just includes some extra data in the fields unused by bounds. + compound.Tree.SweepBuild(pool, subtrees.As()); + pool.Return(ref subtrees); + return compound; + } + /// /// Creates a compound shape with an acceleration structure. /// /// Set of children in the compound. /// Shapes set in which child shapes are allocated. /// Pool to use to allocate acceleration structures. + /// Thread dispatcher to use to multithread the acceleration structure build, if any. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public BigCompound(Buffer children, Shapes shapes, BufferPool pool) + public BigCompound(Buffer children, Shapes shapes, BufferPool pool, IThreadDispatcher dispatcher = null) { - Debug.Assert(children.Length > 0, "Compounds must have a nonzero number of children."); - Debug.Assert(Compound.ValidateChildIndices(ref children, shapes), "Children must all be convex."); - Children = children; - Tree = new Tree(pool, children.Length); - pool.Take(children.Length, out Buffer leafBounds); - Compound.ComputeChildBounds(Children[0], Quaternion.Identity, shapes, out leafBounds[0].Min, out leafBounds[0].Max); - for (int i = 1; i < Children.Length; ++i) - { - ref var bounds = ref leafBounds[i]; - Compound.ComputeChildBounds(Children[i], Quaternion.Identity, shapes, out bounds.Min, out bounds.Max); - } - Tree.SweepBuild(pool, leafBounds); - pool.Return(ref leafBounds); + this = CreateWithoutTreeBuild(children, pool); + pool.Take(children.Length, out Buffer subtrees); + FillSubtreesForChildren(children, shapes, subtrees); + Tree.BinnedBuild(subtrees, dispatcher, pool); + pool.Return(ref subtrees); } public void ComputeBounds(Quaternion orientation, Shapes shapeBatches, out Vector3 min, out Vector3 max) diff --git a/BepuPhysics/Collidables/Compound.cs b/BepuPhysics/Collidables/Compound.cs index 58c1ccdb2..30b4471b9 100644 --- a/BepuPhysics/Collidables/Compound.cs +++ b/BepuPhysics/Collidables/Compound.cs @@ -114,7 +114,7 @@ public static bool ValidateChildIndex(TypedIndex shapeIndex, Shapes shapeBatches /// Children to examine. /// Shape collection into which the children index. /// True if all child indices are valid, false otherwise. - public static bool ValidateChildIndices(ref Buffer children, Shapes shapeBatches) + public static bool ValidateChildIndices(Span children, Shapes shapeBatches) { for (int i = 0; i < children.Length; ++i) { diff --git a/BepuPhysics/Collidables/Mesh.cs b/BepuPhysics/Collidables/Mesh.cs index 5b9b849b0..4201f499a 100644 --- a/BepuPhysics/Collidables/Mesh.cs +++ b/BepuPhysics/Collidables/Mesh.cs @@ -96,7 +96,7 @@ public static void FillSubtreesForTriangles(Span triangles, SpanCreated mesh shape. /// In some cases, the default binned build may not be the ideal builder. This function does everything needed to set up a tree without the expense of figuring out the details of the acceleration structure. /// The user can then run whatever build/refinement process is appropriate. - public static Mesh CreateMeshWithoutTreeBuild(Buffer triangles, Vector3 scale, BufferPool pool) + public static Mesh CreateWithoutTreeBuild(Buffer triangles, Vector3 scale, BufferPool pool) { Mesh mesh = default; mesh.Triangles = triangles; @@ -122,9 +122,9 @@ public static Mesh CreateMeshWithoutTreeBuild(Buffer triangles, Vector /// Created mesh shape. /// The sweep builder is significantly slower than the binned builder, but can sometimes create higher quality trees. /// Note that the binned builder can be tuned to create higher quality trees. That is usually a better choice than trying to use the sweep builder; this is here primarily for legacy reasons. - public unsafe static Mesh CreateMeshWithSweepBuild(Buffer triangles, Vector3 scale, BufferPool pool) + public unsafe static Mesh CreateWithSweepBuild(Buffer triangles, Vector3 scale, BufferPool pool) { - var mesh = CreateMeshWithoutTreeBuild(triangles, scale, pool); + var mesh = CreateWithoutTreeBuild(triangles, scale, pool); pool.Take(triangles.Length, out var subtrees); FillSubtreesForTriangles(triangles, subtrees); Debug.Assert(sizeof(BoundingBox) == sizeof(NodeChild), @@ -146,7 +146,7 @@ public unsafe static Mesh CreateMeshWithSweepBuild(Buffer triangles, V /// Dispatcher to use to multithread the execution of the mesh build process. If null, the build will be single threaded. public Mesh(Buffer triangles, Vector3 scale, BufferPool pool, IThreadDispatcher dispatcher = null) { - this = CreateMeshWithoutTreeBuild(triangles, scale, pool); + this = CreateWithoutTreeBuild(triangles, scale, pool); pool.Take(triangles.Length, out var subtrees); FillSubtreesForTriangles(triangles, subtrees); Tree.BinnedBuild(subtrees, dispatcher: dispatcher, pool: pool); diff --git a/Demos/DemoMeshHelper.cs b/Demos/DemoMeshHelper.cs index 5fe778d90..9dd181d6f 100644 --- a/Demos/DemoMeshHelper.cs +++ b/Demos/DemoMeshHelper.cs @@ -130,7 +130,7 @@ public unsafe static Mesh CreateGiantMeshFastWithoutBounds(Buffer tria //The special logic isn't necessary for tiny meshes, and we also don't handle the corner case of leaf counts <= 2. Just use the regular constructor. return new Mesh(triangles, scaling, pool); } - var mesh = Mesh.CreateMeshWithoutTreeBuild(triangles, scaling, pool); + var mesh = Mesh.CreateWithoutTreeBuild(triangles, scaling, pool); int leafCounter = 0; CreateDummyNodes(ref mesh.Tree, 0, triangles.Length, ref leafCounter); for (int i = 0; i < triangles.Length; ++i) From f73db2c5178c7cfee07dd9fa4218af389c1f3be8 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 6 May 2023 21:04:54 -0500 Subject: [PATCH 721/947] Version bump. --- BepuPhysics/BepuPhysics.csproj | 2 +- BepuUtilities/BepuUtilities.csproj | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/BepuPhysics/BepuPhysics.csproj b/BepuPhysics/BepuPhysics.csproj index d868fd070..80da80de9 100644 --- a/BepuPhysics/BepuPhysics.csproj +++ b/BepuPhysics/BepuPhysics.csproj @@ -1,7 +1,7 @@  net7.0 - 2.5.0-beta.11 + 2.5.0-beta.12 Bepu Entertainment LLC Ross Nordby Speedy real time physics simulation library. diff --git a/BepuUtilities/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj index 80ff901c7..699776961 100644 --- a/BepuUtilities/BepuUtilities.csproj +++ b/BepuUtilities/BepuUtilities.csproj @@ -3,7 +3,7 @@ BepuUtilities BepuUtilities net7.0 - 2.5.0-beta.11 + 2.5.0-beta.12 Bepu Entertainment LLC Ross Nordby Supporting utilities library for BEPUphysics v2. From dde3a6bab6bff9ce40122b6461df72648f5483f9 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 5 Jun 2023 14:19:02 -0500 Subject: [PATCH 722/947] Some simple helper ToStrings. --- BepuPhysics/BodyProperties.cs | 70 +++++++++++++++++++++++++++-------- 1 file changed, 55 insertions(+), 15 deletions(-) diff --git a/BepuPhysics/BodyProperties.cs b/BepuPhysics/BodyProperties.cs index d5bf4072e..9f2e3f18c 100644 --- a/BepuPhysics/BodyProperties.cs +++ b/BepuPhysics/BodyProperties.cs @@ -29,21 +29,6 @@ public struct MotionState internal const int OffsetToAngularY = 13; internal const int OffsetToAngularZ = 14; - internal const int ByteOffsetToPositionX = OffsetToPositionX * 4; - internal const int ByteOffsetToPositionY = OffsetToPositionY * 4; - internal const int ByteOffsetToPositionZ = OffsetToPositionZ * 4; - internal const int ByteOffsetToOrientationX = OffsetToOrientationX * 4; - internal const int ByteOffsetToOrientationY = OffsetToOrientationY * 4; - internal const int ByteOffsetToOrientationZ = OffsetToOrientationZ * 4; - internal const int ByteOffsetToOrientationW = OffsetToOrientationW * 4; - internal const int ByteOffsetToLinearX = OffsetToLinearX * 4; - internal const int ByteOffsetToLinearY = OffsetToLinearY * 4; - internal const int ByteOffsetToLinearZ = OffsetToLinearZ * 4; - internal const int ByteOffsetToAngularX = OffsetToAngularX * 4; - internal const int ByteOffsetToAngularY = OffsetToAngularY * 4; - internal const int ByteOffsetToAngularZ = OffsetToAngularZ * 4; - - /// /// Pose of the body. /// @@ -52,6 +37,16 @@ public struct MotionState /// Linear and angular velocity of the body. /// public BodyVelocity Velocity; + + /// + /// Returns a string representing the MotionState. + /// + /// String representing the MotionState. + public override string ToString() + { + return $"Pose: {Pose}, Velocity: {Velocity}"; + } + } //TODO: It's a little odd that this exists alongside the BepuUtilities.RigidTransform. The original reasoning was that rigid poses may end up having a non-FP32 representation. @@ -180,6 +175,15 @@ public static void MultiplyWithoutOverlap(in RigidPose a, in RigidPose b, out Ri QuaternionEx.Transform(a.Position, b.Orientation, out var rotatedTranslationA); result.Position = rotatedTranslationA + b.Position; } + + /// + /// Returns a string representing the RigidPose as "Position, Orientation". + /// + /// String representing the RigidPose. + public override string ToString() + { + return $"{Position}, {Orientation}"; + } } /// @@ -240,6 +244,15 @@ public static implicit operator BodyVelocity((Vector3 linearVelocity, Vector3 an { return new BodyVelocity(velocities.linearVelocity, velocities.angularVelocity); } + + /// + /// Returns a string representing the BodyVelocity as "Linear, Angular". + /// + /// String representing the BodyVelocity. + public override string ToString() + { + return $"{Linear}, {Angular}"; + } } /// @@ -257,6 +270,15 @@ public struct BodyInertia /// Inverse of the body's mass. /// public float InverseMass; + + /// + /// Returns a string representing the BodyInertia as "InverseMass, InverseInertiaTensor". + /// + /// String representing the BodyInertia. + public override string ToString() + { + return $"{InverseMass}, {InverseInertiaTensor}"; + } } /// @@ -277,6 +299,15 @@ public struct BodyInertias /// We cache this here because velocity integration wants both the local and world inertias, and any integration happening within the solver will do so without the benefit of sequential loads. /// In that context, being able to load a single cache line to grab both local and world inertia helps quite a lot. public BodyInertia World; + + /// + /// Returns a string representing the BodyInertias. + /// + /// String representing the BodyInertias. + public override string ToString() + { + return $"[Local: {Local}, World: {World}]"; + } } /// @@ -299,6 +330,15 @@ public struct BodyDynamics /// Inertia information for the body. /// public BodyInertias Inertia; + + /// + /// Returns a string representing the BodyDynamics. + /// + /// String representing the BodyDynamics. + public override string ToString() + { + return $"[Motion: {Motion}, Inertia: {Inertia}]"; + } } From f75092e3d5a35cdcb7bfb874888054e27436ebb6 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 5 Jun 2023 18:37:35 -0500 Subject: [PATCH 723/947] Debracketify. --- BepuPhysics/BodyProperties.cs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/BepuPhysics/BodyProperties.cs b/BepuPhysics/BodyProperties.cs index 9f2e3f18c..933e267ad 100644 --- a/BepuPhysics/BodyProperties.cs +++ b/BepuPhysics/BodyProperties.cs @@ -306,7 +306,7 @@ public struct BodyInertias /// String representing the BodyInertias. public override string ToString() { - return $"[Local: {Local}, World: {World}]"; + return $"Local: {Local}, World: {World}"; } } @@ -337,7 +337,7 @@ public struct BodyDynamics /// String representing the BodyDynamics. public override string ToString() { - return $"[Motion: {Motion}, Inertia: {Inertia}]"; + return $"Motion: {Motion}, Inertia: {Inertia}"; } } From 37cde81ae349a0195089321d7dfa52967995edc0 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 5 Jun 2023 18:38:09 -0500 Subject: [PATCH 724/947] Broad phase stress test fancified a little. --- Demos/DemoSet.cs | 1 + .../BroadPhaseStressTestDemo.cs | 78 +++++++++++++++++-- .../SpecializedTests/TreeFiddlingTestDemo.cs | 2 +- 3 files changed, 73 insertions(+), 8 deletions(-) diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index f1f34eeab..6612d91d5 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -46,6 +46,7 @@ struct Option public DemoSet() { + //AddOption(); //AddOption(); //AddOption(); //AddOption(); diff --git a/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs b/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs index f7871edbd..e3c7ef7e1 100644 --- a/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs +++ b/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs @@ -11,26 +11,66 @@ using System.Runtime.CompilerServices; using DemoContentLoader; using BepuPhysics.Constraints; +using BepuPhysics.CollisionDetection; namespace Demos.SpecializedTests { public class BroadPhaseStressTestDemo : Demo { + + public unsafe struct NoNarrowphaseTestingCallbacks : INarrowPhaseCallbacks + { + + public NoNarrowphaseTestingCallbacks() + { + } + + public void Initialize(Simulation simulation) + { + } + + public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin) + { + return false; + } + + public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB) + { + return false; + } + + public unsafe bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold + { + pairMaterial = default; + return false; + } + + public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold) + { + return false; + } + + public void Dispose() + { + } + } + + Vector3[] startingLocations; + public unsafe override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(-20f, 13, -20f); camera.Yaw = MathHelper.Pi * 3f / 4; camera.Pitch = MathHelper.Pi * 0.1f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); + Simulation = Simulation.Create(BufferPool, new NoNarrowphaseTestingCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)), new SolveDescription(1, 1)); var shape = new Sphere(0.5f); var sphereInertia = shape.ComputeInertia(1); var shapeIndex = Simulation.Shapes.Add(shape); - const int width = 64; - const int height = 64; - const int length = 64; + const int width = 128; + const int height = 128; + const int length = 128; var spacing = new Vector3(1.01f); - var halfSpacing = spacing / 2; float randomization = 0.9f; var randomizationSpan = (spacing - new Vector3(1)) * randomization; var randomizationBase = randomizationSpan * -0.5f; @@ -42,8 +82,11 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) for (int k = 0; k < length; ++k) { var r = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); - var location = spacing * (new Vector3(i, j, k) + new Vector3(-width, 1, -length)) + randomizationBase + r * randomizationSpan; - if ((i + j + k) % 2 == 1) + //var location = spacing * (new Vector3(i, j, k) + new Vector3(-width, 1, -length)) + randomizationBase + r * randomizationSpan; + var location = (r - new Vector3(0.5f)) * spacing * new Vector3(width, height, length); + //var hash = HashHelper.Rehash(HashHelper.Rehash(HashHelper.Rehash(i) + HashHelper.Rehash(j)) + HashHelper.Rehash(k)); + var hash = i + j + k; + if (hash % 2 == 0) { Simulation.Bodies.Add(BodyDescription.CreateDynamic(location, sphereInertia, shapeIndex, -1)); } @@ -54,6 +97,13 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) } } } + Console.WriteLine($"Body count: {Simulation.Bodies.ActiveSet.Count}"); + Console.WriteLine($"Static count: {Simulation.Statics.Count}"); + startingLocations = new Vector3[Simulation.Bodies.ActiveSet.Count]; + for (int i = 0; i < startingLocations.Length; ++i) + { + startingLocations[i] = Simulation.Bodies.ActiveSet.DynamicsState[i].Motion.Pose.Position; + } refineTimes = new TimingsRingBuffer(sampleCount, BufferPool); testTimes = new TimingsRingBuffer(sampleCount, BufferPool); } @@ -64,6 +114,17 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) long frameCount; public override void Update(Window window, Camera camera, Input input, float dt) { + var rotationAngle = frameCount * 1e-3f; + var rotation = Matrix3x3.CreateFromAxisAngle(Vector3.UnitY, rotationAngle); + for (int i = 0; i < Simulation.Bodies.ActiveSet.Count / 2; ++i) + { + //For every body, set the velocity such that body moves toward some body-specific goal state that evolves over time. + Matrix3x3.Transform(startingLocations[i], rotation, out var targetLocation); + ref var motion = ref Simulation.Bodies.ActiveSet.DynamicsState[i].Motion; + var offset = targetLocation - motion.Pose.Position; + motion.Velocity.Linear = offset; + } + base.Update(window, camera, input, dt); refineTimes.Add(Simulation.Profiler[Simulation.BroadPhase]); testTimes.Add(Simulation.Profiler[Simulation.BroadPhaseOverlapFinder]); @@ -73,6 +134,9 @@ public override void Update(Window window, Camera camera, Input input, float dt) var testStats = testTimes.ComputeStats(); Console.WriteLine($"Refine: {refineStats.Average * 1000} ms average, {refineStats.StdDev * 1000} stddev"); Console.WriteLine($"Test: {testStats.Average * 1000} ms average, {testStats.StdDev * 1000} stddev"); + Console.WriteLine($"Active Cost: {Simulation.BroadPhase.ActiveTree.MeasureCostMetric()}"); + Console.WriteLine($"Static Cost: {Simulation.BroadPhase.StaticTree.MeasureCostMetric()}"); + } } diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index 27a2ba4be..21e6826c1 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -187,7 +187,7 @@ public override void Initialize(ContentArchive content, Camera camera) BinnedTest(setup, () => { - mesh.Tree.BinnedBuild(subtrees, pool: BufferPool); + mesh.Tree.BinnedBuild(subtrees, ThreadDispatcher, pool: BufferPool); }, "Revamp Single Axis MT", ref mesh.Tree); //BufferPool.Take(mesh.Triangles.Length, out var leafBounds); From 83c39037ae190d5a845310e541c55b53316f161c Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 7 Jun 2023 16:42:44 -0500 Subject: [PATCH 725/947] Prodding. --- .../SpecializedTests/BroadPhaseStressTestDemo.cs | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs b/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs index e3c7ef7e1..50a8d022c 100644 --- a/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs +++ b/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs @@ -125,9 +125,13 @@ public override void Update(Window window, Camera camera, Input input, float dt) motion.Velocity.Linear = offset; } + //Simulation.BroadPhase.ActiveTree.CacheOptimize(0); + //Simulation.BroadPhase.StaticTree.CacheOptimize(0); base.Update(window, camera, input, dt); refineTimes.Add(Simulation.Profiler[Simulation.BroadPhase]); testTimes.Add(Simulation.Profiler[Simulation.BroadPhaseOverlapFinder]); + + if (frameCount++ % sampleCount == 0) { var refineStats = refineTimes.ComputeStats(); @@ -136,6 +140,18 @@ public override void Update(Window window, Camera camera, Input input, float dt) Console.WriteLine($"Test: {testStats.Average * 1000} ms average, {testStats.StdDev * 1000} stddev"); Console.WriteLine($"Active Cost: {Simulation.BroadPhase.ActiveTree.MeasureCostMetric()}"); Console.WriteLine($"Static Cost: {Simulation.BroadPhase.StaticTree.MeasureCostMetric()}"); + Console.WriteLine($"Active CQ: {Simulation.BroadPhase.ActiveTree.MeasureCacheQuality()}"); + Console.WriteLine($"Static CQ: {Simulation.BroadPhase.StaticTree.MeasureCacheQuality()}"); + + var a = Stopwatch.GetTimestamp(); + Simulation.BroadPhase.ActiveTree.Refit2(); + var b = Stopwatch.GetTimestamp(); + Simulation.BroadPhase.StaticTree.Refit2(); + var c = Stopwatch.GetTimestamp(); + + + Console.WriteLine($"Manual ST refit active: {1e3 * (b - a) / Stopwatch.Frequency} ms"); + Console.WriteLine($"Manual ST refit static: {1e3 * (c - b) / Stopwatch.Frequency} ms"); } } From 76bca1a433301e64223a816977f193ff8088a1b3 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 7 Jun 2023 16:45:28 -0500 Subject: [PATCH 726/947] Purged older TaskStack/Queue implementations. --- BepuUtilities/ParallelTaskStack.cs | 957 -------------------- BepuUtilities/TaskQueue.cs | 891 ------------------ BepuUtilities/TaskStack.cs | 738 --------------- Demos/SpecializedTests/TaskQueueTestDemo.cs | 138 +-- 4 files changed, 8 insertions(+), 2716 deletions(-) delete mode 100644 BepuUtilities/ParallelTaskStack.cs delete mode 100644 BepuUtilities/TaskQueue.cs delete mode 100644 BepuUtilities/TaskStack.cs diff --git a/BepuUtilities/ParallelTaskStack.cs b/BepuUtilities/ParallelTaskStack.cs deleted file mode 100644 index 679091316..000000000 --- a/BepuUtilities/ParallelTaskStack.cs +++ /dev/null @@ -1,957 +0,0 @@ -using System; -using System.ComponentModel; -using System.Diagnostics; -using System.Diagnostics.CodeAnalysis; -using System.Numerics; -using System.Runtime.CompilerServices; -using System.Runtime.InteropServices; -using System.Threading; -using BepuUtilities.Collections; -using BepuUtilities.Memory; - -namespace BepuUtilities.TestParallelStack; - -/// -/// Description of a task to be submitted to a . -/// -public unsafe struct Task -{ - /// - /// Function to be executed by the task. Takes as arguments the , pointer, and executing worker index. - /// - public delegate* Function; - /// - /// Context to be passed into the . - /// - public void* Context; - /// - /// Continuation to be notified after this task completes, if any. - /// - public ContinuationHandle Continuation; - /// - /// User-provided identifier of this task. - /// - public long Id; - - /// - /// Creates a new task. - /// - /// Function to be executed by the task. Takes as arguments the , pointer, executing worker index, and executing . - /// Context pointer to pass to the . - /// Id of this task to be passed into the . - /// Continuation to notify after the completion of this task, if any. - public Task(delegate* function, void* context = null, long taskId = 0, ContinuationHandle continuation = default) - { - Function = function; - Context = context; - Continuation = continuation; - Id = taskId; - } - - /// - /// Creates a task from a function. - /// - /// Function to turn into a task. - public static implicit operator Task(delegate* function) => new(function); - - /// - /// Runs the task and, if necessary, notifies the associated continuation of its completion. - /// - /// Worker index to pass to the function. - /// Dispatcher running this task. - public void Run(int workerIndex, IThreadDispatcher dispatcher) - { - Debug.Assert(!Continuation.Completed); - Function(Id, Context, workerIndex, dispatcher); - if (Continuation.Initialized) - Continuation.NotifyTaskCompleted(workerIndex, dispatcher); - } -} - -/// -/// Describes the result status of a pop attempt. -/// -public enum PopTaskResult -{ - /// - /// A task was successfully popped. - /// - Success, - /// - /// The stack was empty, but may have more tasks in the future. - /// - Empty, - /// - /// The stack has been terminated and all threads seeking work should stop. - /// - Stop -} - -/// -/// Refers to a continuation within a . -/// -public unsafe struct ContinuationHandle : IEquatable -{ - uint index; - uint encodedVersion; - /// - /// Source worker task stack. - /// - internal WorkerTaskStack* WorkerStack; - - internal ContinuationHandle(uint index, int version, WorkerTaskStack* workerStack) - { - this.index = index; - encodedVersion = (uint)version | 1u << 31; - WorkerStack = workerStack; - } - - internal uint Index - { - get - { - Debug.Assert(Initialized, "If you're trying to pull a continuation id from a continuation handle, it should have been initialized."); - return index; - } - } - - internal int Version - { - get - { - Debug.Assert(Initialized, "If you're trying to pull a continuation id from a continuation handle, it should have been initialized."); - return (int)(encodedVersion & ((1u << 31) - 1)); - } - } - - /// - /// Gets whether the tasks associated with this continuation have completed. - /// - public bool Completed => WorkerStack->IsComplete(this); - - /// - /// Retrieves a pointer to the continuation data for . - /// - /// Pointer to the continuation backing the given handle. - /// This should not be used if the continuation handle is not known to be valid. The data pointed to by the data could become invalidated if the continuation completes. - public TaskContinuation* Continuation => WorkerStack->GetContinuation(this); - - /// - /// Gets a null continuation handle. - /// - public static ContinuationHandle Null => default; - - /// - /// Gets whether the continuation associated with this handle was allocated and has outstanding tasks. - /// - public bool Exists - { - get - { - if (!Initialized || WorkerStack == null) - return false; - ref var continuation = ref WorkerStack->Continuations[Index]; - return continuation.Version == Version && continuation.RemainingTaskCounter > 0; - } - } - - /// - /// Gets whether this handle ever represented an allocated handle. This does not guarantee that the continuation's associated tasks are active in the that it was allocated from. - /// - public bool Initialized => encodedVersion >= 1u << 31; - - /// - /// Notifies the continuation that one task was completed. - /// - /// Worker index to pass to the continuation's delegate, if any. - /// Dispatcher to pass to the continuation's delegate, if any. - public void NotifyTaskCompleted(int workerIndex, IThreadDispatcher dispatcher) - { - var continuation = Continuation; - Debug.Assert(!Completed); - var counter = Interlocked.Decrement(ref continuation->RemainingTaskCounter); - Debug.Assert(counter >= 0, "The counter should not go negative. Was notify called too many times?"); - if (counter == 0) - { - //This entire job has completed. - if (continuation->OnCompleted.Function != null) - { - continuation->OnCompleted.Function(continuation->OnCompleted.Id, continuation->OnCompleted.Context, workerIndex, dispatcher); - } - //Free this continuation slot. - var waiter = new SpinWait(); - while (Interlocked.CompareExchange(ref WorkerStack->ContinuationLocker, 1, 0) != 0) - { - waiter.SpinOnce(-1); - } - //We have the lock. - WorkerStack->ContinuationIndexPool.ReturnUnsafely((int)Index); - --WorkerStack->ContinuationCount; - WorkerStack->ContinuationLocker = 0; - } - } - - public bool Equals(ContinuationHandle other) => other.index == index && other.encodedVersion == encodedVersion && other.WorkerStack == WorkerStack; - - public override bool Equals([NotNullWhen(true)] object obj) => obj is ContinuationHandle handle && Equals(handle); - - public override int GetHashCode() => (int)(index ^ (encodedVersion << 24)); - - public static bool operator ==(ContinuationHandle left, ContinuationHandle right) => left.Equals(right); - - public static bool operator !=(ContinuationHandle left, ContinuationHandle right) => !(left == right); -} - -internal unsafe struct WorkerTaskStack -{ - public QuickList Tasks; - public Buffer Continuations; - public IdPool ContinuationIndexPool; - public int ContinuationCount; - public int WorkerIndex; - public Buffer Masks; - public volatile int TaskLocker; - public volatile int ContinuationLocker; - - public WorkerTaskStack(int workerIndex, IThreadDispatcher dispatcher, Buffer masks, int initialTaskCapacity = 64, int initialContinuationCapacity = 16) - { - Masks = masks; - var threadPool = dispatcher.WorkerPools[workerIndex]; - WorkerIndex = workerIndex; - Tasks = new QuickList(initialTaskCapacity, threadPool); - threadPool.Take(initialContinuationCapacity, out Continuations); -#if DEBUG - //While you shouldn't *need* to clear continuations, it can be useful for debug purposes. - Continuations.Clear(0, Continuations.length); - Tasks.Span.Clear(0, Tasks.Span.length); -#endif - ContinuationIndexPool = new IdPool(initialContinuationCapacity, threadPool); - } - - public void Dispose(BufferPool threadPool) - { - Tasks.Dispose(threadPool); - threadPool.Return(ref Continuations); - ContinuationIndexPool.Dispose(threadPool); - } - - /// - /// Attempts to pop a task from the stack. - /// - /// Task popped from the stack, if any. - /// True if a task was available to pop, false otherwise. - internal bool TryPop(out Task task) - { - //Quick early out to avoid lock expense. - if (Tasks.Count == 0) - { - task = default; - return false; - } - var waiter = new SpinWait(); - //Unlike the pushes, we only return false if there is no task. Just simplifies things a bit. Don't want to have 'try' with two different meanings. - //(There's some argument for there not being *any* try-because-contested functions. Bit of a holdover from how things used to work.) - while (true) - { - if (Interlocked.CompareExchange(ref TaskLocker, 1, 0) != 0) - { - waiter.SpinOnce(); - continue; - } - try - { - //We have the lock. - if (Tasks.TryPop(out task)) - { - if (Tasks.Count == 0) - { - //Just transitioned from taskful to taskless; notify the bitmask. - var maskIndex = WorkerIndex / 64; - var indexInMask = WorkerIndex - maskIndex * 64; - Interlocked.And(ref Masks[maskIndex], ~(1ul << indexInMask)); - } - return true; - } - return false; - } - finally - { - TaskLocker = 0; - - } - } - } - - /// - /// Pushes a set of tasks onto the stack. Does not acquire a lock. - /// - /// Tasks composing the job. - /// Dispatcher used to pull thread allocations if necessary. - /// If the worker associated with this stack might be active, this function can only be called by the worker. - internal void PushUnsafely(Span tasks, IThreadDispatcher dispatcher) - { - Debug.Assert(tasks.Length > 0, "Probably shouldn't be trying to push zero tasks."); - var newTaskCount = Tasks.Count + tasks.Length; - - if (Tasks.Span.length < newTaskCount) - { - //Doing a resize within a lock is *really* not great. This is something to be avoided via preallocation whenever possible. - //BUT: - //1. It should be trivial to make these resizes effectively never happen. - //2. In the cases where it happens anyway, suffering the pain of a resize in a lock is the lesser of two evils. - //In the case where we *didn't* resize, we'd have to either stall on the allocation attempt or cycle on other tasks until continuations are freed up. - //But there's no guarantee that running tasks will actually free up continuations on net- they could make more! - //So having the fallback plan of expanding storage for more continuations avoids deadlock prone options. - //Note that *ONLY THE OWNING WORKER* can ever validly perform this resize! - //We have to use the thread's buffer pool, and only the thread can validly access that pool. - Tasks.Resize(int.Max(newTaskCount, Tasks.Span.length * 2), dispatcher.WorkerPools[WorkerIndex]); -#if DEBUG - //While you shouldn't *need* to clear tasks, it can be useful for debug purposes. - Tasks.Span.Clear(Tasks.Count, Tasks.Span.length - Tasks.Count); -#endif - } - Tasks.AddRangeUnsafely(tasks); - - if (Tasks.Count == tasks.Length) - { - //Just transitioned from taskless to taskful; notify the bitmask. - var maskIndex = WorkerIndex / 64; - var indexInMask = WorkerIndex - maskIndex * 64; - Interlocked.Or(ref Masks[maskIndex], 1ul << indexInMask); - } - } - /// - /// Pushes a task onto the task stack. Does not acquire a lock. - /// - /// Task to push - /// Dispatcher used to pull thread allocations if necessary. - /// This must not be used while other threads could be performing task pushes or pops. - public void PushUnsafely(Task task, IThreadDispatcher dispatcher) - { - PushUnsafely(new Span(&task, 1), dispatcher); - } - - /// - /// Tries to push a set of tasks to the task stack if the stack is uncontested. - /// - /// Tasks composing the job. - /// Dispatcher used to pull thread allocations if necessary. - /// True if the push succeeded, false if the push was contested. - /// This must only be called from the associated worker if the worker is potentially active. - public bool TryPush(Span tasks, IThreadDispatcher dispatcher) - { - if (Interlocked.CompareExchange(ref TaskLocker, 1, 0) != 0) - return false; - try - { - //We have the lock. - PushUnsafely(tasks, dispatcher); - return true; - } - finally - { - TaskLocker = 0; - } - } - - /// - /// Push a set of tasks to the task stack. - /// - /// Tasks composing the job. - /// Dispatcher used to pull thread allocations if necessary. - /// This must only be called from the associated worker if the worker is potentially active. - public void Push(Span tasks, IThreadDispatcher dispatcher) - { - var waiter = new SpinWait(); - while (!TryPush(tasks, dispatcher)) - { - waiter.SpinOnce(-1); - } - } - - - /// - /// Attempts to allocate a continuation for a set of tasks. - /// - /// Number of tasks associated with the continuation. - /// Dispatcher from which to pull a buffer pool if needed for resizing. - /// Function to execute upon completing all associated tasks, if any. Any task with a null will not be executed. - /// Handle of the continuation if allocation is successful. - /// True if the continuation was allocated, false if the attempt was contested.. - public bool TryAllocateContinuation(int taskCount, IThreadDispatcher dispatcher, out ContinuationHandle continuationHandle, Task onCompleted = default) - { - continuationHandle = default; - if (Interlocked.CompareExchange(ref ContinuationLocker, 1, 0) != 0) - return false; - try - { - //We have the lock. - if (ContinuationCount == Continuations.length) - { - //Doing a resize within a lock is *really* not great. This is something to be avoided via preallocation whenever possible. - //BUT: - //1. It should be trivial to make these resizes effectively never happen. - //2. In the cases where it happens anyway, suffering the pain of a resize in a lock is the lesser of two evils. - //In the case where we *didn't* resize, we'd have to either stall on the allocation attempt or cycle on other tasks until continuations are freed up. - //But there's no guarantee that running tasks will actually free up continuations on net- they could make more! - //So having the fallback plan of expanding storage for more continuations avoids deadlock prone options. - //Note that *ONLY THE OWNING WORKER* can ever validly perform this resize! - //We have to use the thread's buffer pool, and only the thread can validly access that pool. - var workerPool = dispatcher.WorkerPools[WorkerIndex]; - workerPool.ResizeToAtLeast(ref Continuations, ContinuationCount * 2, ContinuationCount); - ContinuationIndexPool.Resize(Continuations.length, workerPool); -#if DEBUG - //While you shouldn't *need* to clear continuations, it can be useful for debug purposes. - Continuations.Clear(ContinuationCount, Continuations.length - ContinuationCount); -#endif - } - var index = ContinuationIndexPool.Take(); - ++ContinuationCount; - ref var continuation = ref Continuations[index]; - //Note that the version number could be based on undefined data initially. That's actually fine; all we care about is whether it is different. - //Note mask to leave a valid bit for encoding in the handle. - var newVersion = (continuation.Version + 1) & (~(1 << 31)); - continuation.OnCompleted = onCompleted; - continuation.Version = newVersion; - continuation.RemainingTaskCounter = taskCount; - continuationHandle = new ContinuationHandle((uint)index, newVersion, (WorkerTaskStack*)Unsafe.AsPointer(ref this)); - return true; - } - finally - { - ContinuationLocker = 0; - } - } - - - /// - /// Allocates a continuation for a set of tasks. - /// - /// Number of tasks associated with the continuation. - /// Dispatcher from which to pull any thread allocations if necessary. - /// Task to execute upon completing all associated tasks, if any. Any task with a null will not be executed. - /// Handle of the allocated continuation. - /// Note that this will keep trying until allocation succeeds. If something is blocking allocation, such as insufficient room in the continuations buffer and there are no workers consuming tasks, this will block forever. - public ContinuationHandle AllocateContinuation(int taskCount, IThreadDispatcher dispatcher, Task onCompleted = default) - { - var waiter = new SpinWait(); - ContinuationHandle handle; - while (!TryAllocateContinuation(taskCount, dispatcher, out handle, onCompleted)) - { - waiter.SpinOnce(-1); - } - return handle; - } - - /// - /// Retrieves a pointer to the continuation data for . - /// - /// Handle to look up the associated continuation for. - /// Pointer to the continuation backing the given handle. - /// This should not be used if the continuation handle is not known to be valid. The data pointed to by the data could become invalidated if the continuation completes. - public TaskContinuation* GetContinuation(ContinuationHandle continuationHandle) - { - Debug.Assert(continuationHandle.Initialized, "This continuation handle was never initialized."); - Debug.Assert(continuationHandle.Index < Continuations.length, "This continuation refers to an invalid index."); - if (continuationHandle.Index >= Continuations.length || !continuationHandle.Initialized) - return null; - var continuation = Continuations.Memory + continuationHandle.Index; - Debug.Assert(continuation->Version == continuationHandle.Version, "This continuation no longer refers to an active continuation."); - if (continuation->Version != continuationHandle.Version) - return null; - return Continuations.Memory + continuationHandle.Index; - } - - /// - /// Checks whether all tasks associated with this continuation have completed. - /// - /// Continuation to check for completion. - /// True if all tasks associated with a continuation have completed, false otherwise. - public bool IsComplete(ContinuationHandle continuationHandle) - { - Debug.Assert(continuationHandle.Initialized, "This continuation handle was never initialized."); - Debug.Assert(continuationHandle.Index < Continuations.length, "This continuation refers to an invalid index."); - if (continuationHandle.Index >= Continuations.length || !continuationHandle.Initialized) - return false; - ref var continuation = ref Continuations[continuationHandle.Index]; - return continuation.Version > continuationHandle.Version || continuation.RemainingTaskCounter == 0; - } - - internal void Reset() - { - Tasks.Count = 0; - Debug.Assert(TaskLocker == 0, "There appears to be a thread actively working still. That's invalid."); -#if DEBUG - //While you shouldn't *need* to clear continuations, it can be useful for debug purposes. - Continuations.Clear(0, Continuations.length); -#endif - ContinuationCount = 0; - Debug.Assert(ContinuationLocker == 0, "There appears to be a thread actively working still. That's invalid."); - } -} - -/// -/// Stores data relevant to tracking task completion and reporting completion for a continuation. -/// -public unsafe struct TaskContinuation -{ - /// - /// Task to run upon completion of the associated task. - /// - public Task OnCompleted; - /// - /// Version of this continuation. - /// - public int Version; - /// - /// Number of tasks not yet reported as complete in the continuation. - /// - public int RemainingTaskCounter; -} - - -/// -/// Manages task stacks for parallel workers. Supports work stealing between workers. -/// -public unsafe struct ParallelTaskStack -{ - Buffer workerStacks; - Buffer workerHasTaskMask; - - [StructLayout(LayoutKind.Explicit, Size = 256 + 16)] - struct StopPad - { - [FieldOffset(128)] - public volatile bool Stop; - } - StopPad padded; - - /// - /// Constructs a new parallel task stack. - /// - /// Buffer pool to allocate non-thread allocated resources from. - /// Thread dispatcher to pull thread pools from for thread allocations. - /// Number of workers to allocate space for. - /// Initial number of tasks to allocate space for in each worker. Tasks are individual chunks of scheduled work.. - /// Initial number of continuations to allocate space for in each worker. - public ParallelTaskStack(BufferPool pool, IThreadDispatcher dispatcher, int workerCount, int initialWorkerTaskCapacity = 64, int initialWorkerContinuationCapacity = 16) - { - pool.Take(workerCount, out workerStacks); - pool.Take((workerCount + 63) / 64, out workerHasTaskMask); - workerHasTaskMask.Clear(0, workerHasTaskMask.Length); - for (int i = 0; i < workerCount; ++i) - { - workerStacks[i] = new WorkerTaskStack(i, dispatcher, workerHasTaskMask, initialWorkerTaskCapacity, initialWorkerContinuationCapacity); - } - Reset(); - } - - /// - /// Returns the stack to a fresh state without reallocating. - /// - public void Reset() - { - for (int i = 0; i < workerStacks.Length; ++i) - { - workerStacks[i].Reset(); - } - padded.Stop = false; - workerHasTaskMask.Clear(0, workerHasTaskMask.Length); - } - - /// - /// Returns unmanaged resources held by the to a pool. - /// - /// Buffer pool to return resources to. - public void Dispose(BufferPool pool) - { - for (int i = 0; i < workerStacks.Length; ++i) - { - workerStacks[i].Reset(); - } - pool.Return(ref workerStacks); - pool.Return(ref workerHasTaskMask); - } - - /// - /// Gets the approximate number of active tasks. This is not guaranteed to actually measure the true number of tasks at any one point in time; it checks each worker in sequence, and the task counts could vary arbitrarily as the checks proceed. - /// - public int ApproximateTaskCount - { - get - { - int sum = 0; - for (int i = 0; i < workerStacks.Length; ++i) - { - sum += workerStacks[i].Tasks.Count; - } - return sum; - } - } - /// - /// Gets the approximate number of active continuations. This is not guaranteed to actually measure the true number of continuations at any one point in time; it checks each worker in sequence, and the continuation counts could vary arbitrarily as the checks proceed. - /// - public int ApproximateContinuationCount - { - get - { - int sum = 0; - for (int i = 0; i < workerStacks.Length; ++i) - { - sum += workerStacks[i].ContinuationCount; - } - return sum; - } - } - - - /// - /// Attempts to allocate a continuation for a set of tasks. - /// - /// Number of tasks associated with the continuation. - /// Worker index to allocate the continuation on. - /// Dispatcher to use for any per-thread allocations if necessary. - /// Function to execute upon completing all associated tasks, if any. Any task with a null will not be executed. - /// Handle of the continuation if allocation is successful. - /// True if the allocation succeeded, false if it was contested.. - public bool TryAllocateContinuation(int taskCount, int workerIndex, IThreadDispatcher dispatcher, out ContinuationHandle continuationHandle, Task onCompleted = default) - { - return workerStacks[workerIndex].TryAllocateContinuation(taskCount, dispatcher, out continuationHandle, onCompleted); - } - - /// - /// Allocates a continuation for a set of tasks. - /// - /// Number of tasks associated with the continuation. - /// Worker index to allocate the continuation on. - /// Dispatcher to use for any per-thread allocations if necessary. - /// Task to execute upon completing all associated tasks, if any. Any task with a null will not be executed. - /// Handle of the allocated continuation. - public ContinuationHandle AllocateContinuation(int taskCount, int workerIndex, IThreadDispatcher dispatcher, Task onCompleted = default) - { - return workerStacks[workerIndex].AllocateContinuation(taskCount, dispatcher, onCompleted); - } - - public static int PopFailed; - public static int StealRequired; - public static int NoStealRequired; - /// - /// Attempts to pop a task. - /// - /// Index of the worker executing this function. - /// Popped task, if any. - /// Result status of the pop attempt. - public PopTaskResult TryPop(int workerIndex, out Task task) - { - //Try the local worker first. - if (workerStacks[workerIndex].TryPop(out task)) - { - //Interlocked.Increment(ref NoStealRequired); - return PopTaskResult.Success; - } - //Interlocked.Increment(ref StealRequired); - - //for (int i = 0; i < workerHasTaskMask.length; ++i) - //{ - // //No more need for masking out early bits; just find the first task. - // var mask = workerHasTaskMask[i]; - // int setBitIndex; - - // while ((setBitIndex = BitOperations.TrailingZeroCount(mask)) < 64) - // { - // if (workerStacks[i * 64 + setBitIndex].TryPop(out task)) - // return PopTaskResult.Success; - // //This stack didn't have anything for us, mask it out. - // mask &= ~(1ul << setBitIndex); - // } - //} - ////Interlocked.Increment(ref PopFailed); - //task = default; - //return padded.Stop ? PopTaskResult.Stop : PopTaskResult.Empty; - - - - //There was no task available locally, so go for a steal. - //We want to distribute steals so that not *every* thread seeks the same source. - //So check all worker slots as a ring. - //The first time we check the local long, we should mask out all earlier slots to ensure the order. - var startIndex = workerIndex / 64; - var indexInMask = workerIndex - startIndex * 64; - var mask = workerHasTaskMask[startIndex]; - var earlyMask = mask; - mask &= ~((1ul << indexInMask) - 1); - var setBitIndex = BitOperations.TrailingZeroCount(mask); - if (setBitIndex < 64) - { - if (workerStacks[startIndex * 64 + setBitIndex].TryPop(out task)) - return PopTaskResult.Success; - } - - //No task available in the local chunk, so just keep going. - for (int i = 1; i <= workerHasTaskMask.length; ++i) - { - var maskIndex = startIndex + i; - if (maskIndex >= workerHasTaskMask.length) - maskIndex -= workerHasTaskMask.length; - //No more need for masking out early bits; just find the first task. - mask = workerHasTaskMask[maskIndex]; - setBitIndex = BitOperations.TrailingZeroCount(mask); - if (setBitIndex < 64) - { - if (workerStacks[maskIndex * 64 + setBitIndex].TryPop(out task)) - return PopTaskResult.Success; - } - } - - //Interlocked.Increment(ref PopFailed); - task = default; - return padded.Stop ? PopTaskResult.Stop : PopTaskResult.Empty; - - - - ////Walk through the worker stacks looking for work. Start with the current worker. - //int peekedWorkerIndex = workerIndex; - //for (int i = 0; i < workerStacks.length; ++i) - //{ - // if (workerStacks[peekedWorkerIndex].TryPop(out task)) - // { - // return PopTaskResult.Success; - // } - // ++peekedWorkerIndex; - // if (peekedWorkerIndex >= workerStacks.length) - // peekedWorkerIndex -= workerStacks.length; - //} - //No worker had anything! - task = default; - return padded.Stop ? PopTaskResult.Stop : PopTaskResult.Empty; - } - - /// - /// Attempts to pop a task and run it. - /// - /// Index of the worker to pass into the task function. - /// Thread dispatcher running this task stack. - /// Result status of the pop attempt. - public PopTaskResult TryPopAndRun(int workerIndex, IThreadDispatcher dispatcher) - { - var result = TryPop(workerIndex, out var task); - if (result == PopTaskResult.Success) - { - task.Run(workerIndex, dispatcher); - } - return result; - } - - /// - /// Pushes a set of tasks onto a worker's task stack. Does not acquire a lock. - /// - /// Tasks composing the job. - /// Thread dispatcher to allocate thread data from if necessary. - /// Index of the worker stack to push the tasks onto. - /// This must not be used while other threads could be performing task pushes or pops that could affect the specified worker. - public void PushUnsafely(Span tasks, int workerIndex, IThreadDispatcher dispatcher) - { - workerStacks[workerIndex].PushUnsafely(tasks, dispatcher); - } - /// - /// Pushes a task onto a worker's task stack. Does not acquire a lock. - /// - /// Task to push. - /// Thread dispatcher to allocate thread data from if necessary. - /// Index of the worker stack to push the tasks onto. - /// This must not be used while other threads could be performing task pushes or pops that could affect the specified worker. - public unsafe void PushUnsafely(Task task, int workerIndex, IThreadDispatcher dispatcher) - { - workerStacks[workerIndex].PushUnsafely(new Span(&task, 1), dispatcher); - } - - /// - /// Tries to pushes a set of tasks onto a worker's task stack. - /// - /// Tasks composing the job. - /// Thread dispatcher to allocate thread data from if necessary. - /// Index of the worker stack to push the tasks onto. - /// True if the push succeeded, false if it was contested. - public bool TryPush(Span tasks, int workerIndex, IThreadDispatcher dispatcher) - { - return workerStacks[workerIndex].TryPush(tasks, dispatcher); - } - - /// - /// Pushes a set of tasks onto a worker's task stack. - /// - /// Tasks composing the job. - /// Thread dispatcher to allocate thread data from if necessary. - /// Index of the worker stack to push the tasks onto. - public void Push(Span tasks, int workerIndex, IThreadDispatcher dispatcher) - { - workerStacks[workerIndex].Push(tasks, dispatcher); - } - - /// - /// Pushes a set of tasks to the stack with a created continuation. - /// - /// Tasks composing the job. A continuation will be assigned internally; no continuation should be present on any of the provided tasks. - /// Thread dispatcher to allocate thread data from if necessary. - /// Index of the worker stack to push the tasks onto. - /// Task to run upon completion of all the submitted tasks, if any. - /// Handle of the continuation created for these tasks. - /// Note that this will keep trying until task submission succeeds. - public ContinuationHandle AllocateContinuationAndPush(Span tasks, int workerIndex, IThreadDispatcher dispatcher, Task onComplete = default) - { - var continuationHandle = AllocateContinuation(tasks.Length, workerIndex, dispatcher, onComplete); - for (int i = 0; i < tasks.Length; ++i) - { - ref var task = ref tasks[i]; - Debug.Assert(!task.Continuation.Initialized, "This function creates a continuation for the tasks"); - task.Continuation = continuationHandle; - } - Push(tasks, workerIndex, dispatcher); - return continuationHandle; - } - - /// - /// Waits for a continuation to be completed. - /// - /// Instead of spinning the entire time, this may pop and execute pending tasks to fill the gap. - /// Continuation to wait on. - /// Thread dispatcher to allocate thread data from if necessary. - /// Index of the executing worker. - public void WaitForCompletion(ContinuationHandle continuation, int workerIndex, IThreadDispatcher dispatcher) - { - var waiter = new SpinWait(); - Debug.Assert(continuation.Initialized, "This codepath should only run if the continuation was allocated earlier."); - while (!continuation.Completed) - { - var result = TryPop(workerIndex, out var fillerTask); - if (result == PopTaskResult.Stop) - { - return; - } - if (result == PopTaskResult.Success) - { - fillerTask.Run(workerIndex, dispatcher); - waiter.Reset(); - } - else - { - waiter.SpinOnce(-1); - } - } - } - - /// - /// Pushes a set of tasks to the worker stack and returns when all tasks are complete. - /// - /// Tasks composing the job. A continuation will be assigned internally; no continuation should be present on any of the provided tasks. - /// Index of the worker executing this function. - /// Thread dispatcher to allocate thread data from if necessary. - /// Note that this will keep working until all tasks are run. It may execute tasks unrelated to the requested tasks while waiting on other workers to complete constituent tasks. - public void RunTasks(Span tasks, int workerIndex, IThreadDispatcher dispatcher) - { - if (tasks.Length == 0) - return; - ContinuationHandle continuationHandle = default; - if (tasks.Length > 1) - { - //Note that we only submit tasks to the stack for tasks beyond the first. The current thread is responsible for at least task 0. - var taskCount = tasks.Length - 1; - Span tasksToPush = stackalloc Task[taskCount]; - ref var worker = ref workerStacks[workerIndex]; - continuationHandle = worker.AllocateContinuation(taskCount, dispatcher); - for (int i = 0; i < tasksToPush.Length; ++i) - { - var task = tasks[i + 1]; - Debug.Assert(continuationHandle.WorkerStack == workerStacks.Memory + workerIndex); - Debug.Assert(!task.Continuation.Initialized, $"None of the source tasks should have continuations when provided to {nameof(RunTasks)}."); - task.Continuation = continuationHandle; - tasksToPush[i] = task; - } - worker.Push(tasksToPush, dispatcher); - } - //Tasks [1, count) are submitted to the stack and may now be executing on other workers. - //The thread calling the for loop should not relinquish its timeslice. It should immediately begin working on task 0. - var task0 = tasks[0]; - Debug.Assert(!task0.Continuation.Initialized, $"None of the source tasks should have continuations when provided to {nameof(RunTasks)}."); - task0.Function(task0.Id, task0.Context, workerIndex, dispatcher); - - if (tasks.Length > 1) - { - //Task 0 is done; this thread should seek out other work until the job is complete. - WaitForCompletion(continuationHandle, workerIndex, dispatcher); - } - } - - /// - /// Requests that all workers stop. The next time a worker runs out of tasks to run, if it sees a stop command, it will be reported. - /// - public void RequestStop() - { - padded.Stop = true; - } - - - /// - /// Pushes a for loop onto the task stack. Does not take a lock. - /// - /// Function to execute on each iteration of the loop. - /// Context pointer to pass into each task execution. - /// Inclusive start index of the loop range. - /// Number of iterations to perform. - /// Thread dispatcher to allocate thread data from if necessary. - /// Index of the worker stack to push the tasks onto. - /// Continuation associated with the loop tasks, if any. - /// This must not be used while other threads could be performing task pushes or pops that could affect the specified worker. - public void PushForUnsafely(delegate* function, void* context, int inclusiveStartIndex, int iterationCount, int workerIndex, IThreadDispatcher dispatcher, ContinuationHandle continuation = default) - { - Span tasks = stackalloc Task[iterationCount]; - for (int i = 0; i < tasks.Length; ++i) - { - tasks[i] = new Task { Function = function, Context = context, Id = i + inclusiveStartIndex, Continuation = continuation }; - } - PushUnsafely(tasks, workerIndex, dispatcher); - } - - /// - /// Pushes a for loop onto the task stack. - /// - /// Function to execute on each iteration of the loop. - /// Context pointer to pass into each task execution. - /// Inclusive start index of the loop range. - /// Number of iterations to perform. - /// Thread dispatcher to allocate thread data from if necessary. - /// Index of the worker stack to push the tasks onto. - /// Continuation associated with the loop tasks, if any. - /// This function will not usually attempt to run any iterations of the loop itself. It tries to push the loop tasks onto the stack. - /// If the task stack is full, this will opt to run the tasks inline while waiting for room. - public void PushFor(delegate* function, void* context, int inclusiveStartIndex, int iterationCount, int workerIndex, IThreadDispatcher dispatcher, ContinuationHandle continuation = default) - { - Span tasks = stackalloc Task[iterationCount]; - for (int i = 0; i < tasks.Length; ++i) - { - tasks[i] = new Task { Function = function, Context = context, Id = i + inclusiveStartIndex, Continuation = continuation }; - } - Push(tasks, workerIndex, dispatcher); - } - - /// - /// Submits a set of tasks representing a for loop over the given indices and returns when all loop iterations are complete. - /// - /// Function to execute on each iteration of the loop. - /// Context pointer to pass into each iteration of the loop. - /// Inclusive start index of the loop range. - /// Number of iterations to perform. - /// Thread dispatcher to allocate thread data from if necessary. - /// Index of the worker stack to push the tasks onto. - public void For(delegate* function, void* context, int inclusiveStartIndex, int iterationCount, int workerIndex, IThreadDispatcher dispatcher) - { - if (iterationCount <= 0) - return; - Span tasks = stackalloc Task[iterationCount]; - for (int i = 0; i < tasks.Length; ++i) - { - tasks[i] = new Task(function, context, inclusiveStartIndex + i); - } - RunTasks(tasks, workerIndex, dispatcher); - } -} diff --git a/BepuUtilities/TaskQueue.cs b/BepuUtilities/TaskQueue.cs deleted file mode 100644 index 4625cdae7..000000000 --- a/BepuUtilities/TaskQueue.cs +++ /dev/null @@ -1,891 +0,0 @@ -using System; -using System.Collections.Generic; -using System.ComponentModel; -using System.Diagnostics; -using System.Diagnostics.CodeAnalysis; -using System.Dynamic; -using System.Linq; -using System.Numerics; -using System.Reflection.Metadata; -using System.Reflection.Metadata.Ecma335; -using System.Runtime.CompilerServices; -using System.Runtime.InteropServices; -using System.Text; -using System.Threading; -using System.Threading.Tasks; -using BepuUtilities.Memory; - -namespace BepuUtilities.TestQueue; - -/// -/// Description of a task to be submitted to a . -/// -public unsafe struct Task -{ - /// - /// Function to be executed by the task. Takes as arguments the , pointer, and executing worker index. - /// - public delegate* Function; - /// - /// Context to be passed into the . - /// - public void* Context; - /// - /// Continuation to be notified after this task completes, if any. - /// - public ContinuationHandle Continuation; - /// - /// User-provided identifier of this task. - /// - public long Id; - - /// - /// Creates a new task. - /// - /// Function to be executed by the task. Takes as arguments the , pointer, executing worker index, and executing . - /// Context pointer to pass to the . - /// Id of this task to be passed into the . - /// Continuation to notify after the completion of this task, if any. - public Task(delegate* function, void* context = null, long taskId = 0, ContinuationHandle continuation = default) - { - Function = function; - Context = context; - Continuation = continuation; - Id = taskId; - } - - /// - /// Creates a task from a function. - /// - /// Function to turn into a task. - public static implicit operator Task(delegate* function) => new(function); - - /// - /// Runs the task and, if necessary, notifies the associated continuation of its completion. - /// - /// Worker index to pass to the function. - /// Dispatcher running this task. - public void Run(int workerIndex, IThreadDispatcher dispatcher) - { - Debug.Assert(!Continuation.Completed); - Function(Id, Context, workerIndex, dispatcher); - if (Continuation.Initialized) - Continuation.NotifyTaskCompleted(workerIndex, dispatcher); - } -} - -/// -/// Describes the result status of a dequeue attempt. -/// -public enum DequeueTaskResult -{ - /// - /// A task was successfully dequeued. - /// - Success, - /// - /// The queue was empty, but may have more tasks in the future. - /// - Empty, - /// - /// The queue has been terminated and all threads seeking work should stop. - /// - Stop -} - -/// -/// Describes the result of a task enqueue attempt. -/// -public enum EnqueueTaskResult -{ - /// - /// The tasks were successfully enqueued. - /// - Success, - /// - /// The enqueue attempt was blocked by concurrent access. - /// - Contested, - /// - /// The enqueue attempt was blocked because no space remained in the tasks buffer. - /// - Full, -} - -/// -/// Refers to a continuation within a . -/// -public unsafe struct ContinuationHandle : IEquatable -{ - uint index; - uint encodedVersion; - /// - /// Set of continuations in the queue. - /// - internal TaskQueueContinuations* Continuations; - - internal ContinuationHandle(uint index, int version, TaskQueueContinuations* continuations) - { - this.index = index; - encodedVersion = (uint)version | 1u << 31; - Continuations = continuations; - } - - internal uint Index - { - get - { - Debug.Assert(Initialized, "If you're trying to pull a continuation id from a continuation handle, it should have been initialized."); - return index; - } - } - - internal int Version - { - get - { - Debug.Assert(Initialized, "If you're trying to pull a continuation id from a continuation handle, it should have been initialized."); - return (int)(encodedVersion & ((1u << 31) - 1)); - } - } - - /// - /// Gets whether the tasks associated with this continuation have completed. - /// - public bool Completed => Continuations->IsComplete(this); - - /// - /// Retrieves a pointer to the continuation data for . - /// - /// Pointer to the continuation backing the given handle. - /// This should not be used if the continuation handle is not known to be valid. The data pointed to by the data could become invalidated if the continuation completes. - public TaskContinuation* Continuation => Continuations->GetContinuation(this); - - /// - /// Gets a null continuation handle. - /// - public static ContinuationHandle Null => default; - - /// - /// Gets whether the continuation associated with this handle was allocated and has outstanding tasks. - /// - public bool Exists - { - get - { - if (!Initialized || Continuations == null) - return false; - ref var continuation = ref Continuations->Continuations[Index]; - return continuation.Version == Version && continuation.RemainingTaskCounter > 0; - } - } - - /// - /// Gets whether this handle ever represented an allocated handle. This does not guarantee that the continuation's associated tasks are active in the that it was allocated from. - /// - public bool Initialized => encodedVersion >= 1u << 31; - - /// - /// Notifies the continuation that one task was completed. - /// - /// Worker index to pass to the continuation's delegate, if any. - /// Dispatcher to pass to the continuation's delegate, if any. - public void NotifyTaskCompleted(int workerIndex, IThreadDispatcher dispatcher) - { - var continuation = Continuation; - Debug.Assert(!Completed); - var counter = Interlocked.Decrement(ref continuation->RemainingTaskCounter); - Debug.Assert(counter >= 0, "The counter should not go negative. Was notify called too many times?"); - if (counter == 0) - { - //This entire job has completed. - if (continuation->OnCompleted.Function != null) - { - continuation->OnCompleted.Function(continuation->OnCompleted.Id, continuation->OnCompleted.Context, workerIndex, dispatcher); - } - //Free this continuation slot. - var waiter = new SpinWait(); - while (Interlocked.CompareExchange(ref Continuations->Locker, 1, 0) != 0) - { - waiter.SpinOnce(-1); - } - //We have the lock. - Continuations->IndexPool.ReturnUnsafely((int)Index); - --Continuations->ContinuationCount; - Continuations->Locker = 0; - } - } - - public bool Equals(ContinuationHandle other) => other.index == index && other.encodedVersion == encodedVersion && other.Continuations == Continuations; - - public override bool Equals([NotNullWhen(true)] object obj) => obj is ContinuationHandle handle && Equals(handle); - - public override int GetHashCode() => (int)(index ^ (encodedVersion << 24)); - - public static bool operator ==(ContinuationHandle left, ContinuationHandle right) => left.Equals(right); - - public static bool operator !=(ContinuationHandle left, ContinuationHandle right) => !(left == right); -} - -/// -/// Describes the result of a continuation allocation attempt. -/// -public enum AllocateTaskContinuationResult -{ - /// - /// The continuation was successfully allocated. - /// - Success, - /// - /// The continuation was blocked by concurrent access. - /// - Contested, - /// - /// The queue's continuation buffer is full and can't hold the continuation. - /// - Full -} - -internal unsafe struct TaskQueueContinuations -{ - public Buffer Continuations; - public IdPool IndexPool; - public int ContinuationCount; - public volatile int Locker; - - /// - /// Retrieves a pointer to the continuation data for . - /// - /// Handle to look up the associated continuation for. - /// Pointer to the continuation backing the given handle. - /// This should not be used if the continuation handle is not known to be valid. The data pointed to by the data could become invalidated if the continuation completes. - public TaskContinuation* GetContinuation(ContinuationHandle continuationHandle) - { - Debug.Assert(continuationHandle.Initialized, "This continuation handle was never initialized."); - Debug.Assert(continuationHandle.Index < Continuations.length, "This continuation refers to an invalid index."); - if (continuationHandle.Index >= Continuations.length || !continuationHandle.Initialized) - return null; - var continuation = Continuations.Memory + continuationHandle.Index; - Debug.Assert(continuation->Version == continuationHandle.Version, "This continuation no longer refers to an active continuation."); - if (continuation->Version != continuationHandle.Version) - return null; - return Continuations.Memory + continuationHandle.Index; - } - - /// - /// Checks whether all tasks associated with this continuation have completed. - /// - /// Continuation to check for completion. - /// True if all tasks associated with a continuation have completed, false otherwise. - public bool IsComplete(ContinuationHandle continuationHandle) - { - Debug.Assert(continuationHandle.Initialized, "This continuation handle was never initialized."); - Debug.Assert(continuationHandle.Index < Continuations.length, "This continuation refers to an invalid index."); - if (continuationHandle.Index >= Continuations.length || !continuationHandle.Initialized) - return false; - ref var continuation = ref Continuations[continuationHandle.Index]; - return continuation.Version > continuationHandle.Version || continuation.RemainingTaskCounter == 0; - } -} - -/// -/// Stores data relevant to tracking task completion and reporting completion for a continuation. -/// -public unsafe struct TaskContinuation -{ - /// - /// Task to run upon completion of the associated task. - /// - public Task OnCompleted; - /// - /// Version of this continuation. - /// - public int Version; - /// - /// Number of tasks not yet reported as complete in the continuation. - /// - public int RemainingTaskCounter; -} - - -/// -/// Multithreaded task queue -/// -public unsafe struct TaskQueue -{ - Buffer tasks; - - int taskMask, taskShift; - - [StructLayout(LayoutKind.Explicit, Size = 540)] - struct Padded - { - //WrittenTaskIndex and TaskIndex are referenced by dequeues, so keep them together. - //They're *also* referenced by enqueues, but enqueues are rarer so there's a good chance the local thread can avoid a direct contest. - [FieldOffset(128)] - public long WrittenTaskIndex; - [FieldOffset(136)] - public long TaskIndex; - //AllocatedTaskIndex and TaskLocker are only touched by enqueues, so we shove them off into their own padded region. - [FieldOffset(400)] - public long AllocatedTaskIndex; - [FieldOffset(408)] - public volatile int TaskLocker; - } - - Padded paddedCounters; - - /// - /// Holds the task queue's continuations data in unmanaged memory just in case the queue itself is in unpinned memory. - /// - Buffer continuationsContainer; - - /// - /// Constructs a new task queue. - /// - /// Buffer pool to allocate resources from. - /// Maximum number of tasks to allocate space for. Tasks are individual chunks of scheduled work. Rounded up to the nearest power of 2. - /// Maximum number of continuations to allocate space for. If more continuations exist at any one moment, attempts to create new continuations may have to stall until space is available. - public TaskQueue(BufferPool pool, int maximumTaskCapacity = 1024, int maximumContinuationCapacity = 256) - { - maximumTaskCapacity = (int)BitOperations.RoundUpToPowerOf2((uint)maximumTaskCapacity); - pool.Take(1, out continuationsContainer); - ref var continuations = ref continuationsContainer[0]; - pool.Take(maximumContinuationCapacity, out continuations.Continuations); - continuations.IndexPool = new IdPool(maximumContinuationCapacity, pool); - continuations.ContinuationCount = 0; - continuations.Locker = 0; - - pool.Take(maximumTaskCapacity, out tasks); - taskMask = tasks.length - 1; - taskShift = BitOperations.TrailingZeroCount(tasks.length); - paddedCounters.TaskLocker = 0; - Reset(); - } - - /// - /// Returns the task queue to a fresh state without reallocating. - /// - public void Reset() - { - paddedCounters.TaskIndex = 0; - paddedCounters.AllocatedTaskIndex = 0; - paddedCounters.WrittenTaskIndex = 0; - Debug.Assert(paddedCounters.TaskLocker == 0, "There appears to be a thread actively working still. That's invalid."); - - ref var continuations = ref continuationsContainer[0]; - continuations.Continuations.Clear(0, continuations.Continuations.Length); - continuations.ContinuationCount = 0; - Debug.Assert(continuations.Locker == 0, "There appears to be a thread actively working still. That's invalid."); - } - - /// - /// Returns unmanaged resources held by the to a pool. - /// - /// Buffer pool to return resources to. - public void Dispose(BufferPool pool) - { - continuationsContainer[0].IndexPool.Dispose(pool); - pool.Return(ref continuationsContainer[0].Continuations); - pool.Return(ref tasks); - pool.Return(ref continuationsContainer); - } - - /// - /// Gets the queue's capacity for tasks. - /// - public int TaskCapacity => tasks.length; - /// - /// Gets the queue's capacity for continuations. - /// - public int ContinuationCapacity => continuationsContainer[0].Continuations.length; - /// - /// Gets the number of tasks active in the queue. - /// - /// Does not take a lock; if other threads are modifying the task values, the reported count may be invalid. - public int UnsafeTaskCount => (int)(paddedCounters.WrittenTaskIndex - paddedCounters.TaskIndex); - /// - /// Gets the number of tasks active in the queue. - /// - public int TaskCount - { - get - { - var waiter = new SpinWait(); - while (Interlocked.CompareExchange(ref paddedCounters.TaskLocker, 1, 0) != 0) - { - waiter.SpinOnce(-1); - } - var result = (int)(paddedCounters.WrittenTaskIndex - paddedCounters.TaskIndex); - paddedCounters.TaskLocker = 0; - return result; - } - } - /// - /// Gets the number of continuations active in the queue. - /// - public int ContinuationCount => continuationsContainer[0].ContinuationCount; - - /// - /// Attempts to allocate a continuation for a set of tasks. - /// - /// Number of tasks associated with the continuation. - /// Function to execute upon completing all associated tasks, if any. Any task with a null will not be executed. - /// Handle of the continuation if allocation is successful. - /// Result status of the continuation allocation attempt. - public AllocateTaskContinuationResult TryAllocateContinuation(int taskCount, out ContinuationHandle continuationHandle, Task onCompleted = default) - { - continuationHandle = default; - ref var continuations = ref continuationsContainer[0]; - if (Interlocked.CompareExchange(ref continuations.Locker, 1, 0) != 0) - return AllocateTaskContinuationResult.Contested; - try - { - //We have the lock. - Debug.Assert(continuations.ContinuationCount <= continuations.Continuations.length); - if (continuations.ContinuationCount == continuations.Continuations.length) - { - //No room. - return AllocateTaskContinuationResult.Full; - } - var index = continuations.IndexPool.Take(); - ++continuations.ContinuationCount; - ref var continuation = ref continuations.Continuations[index]; - var newVersion = continuation.Version + 1; - continuation.OnCompleted = onCompleted; - continuation.Version = newVersion; - continuation.RemainingTaskCounter = taskCount; - continuationHandle = new ContinuationHandle((uint)index, newVersion, continuationsContainer.Memory); - return AllocateTaskContinuationResult.Success; - } - finally - { - continuations.Locker = 0; - } - } - - /// - /// Allocates a continuation for a set of tasks. - /// - /// Number of tasks associated with the continuation. - /// Worker index to pass to any inline executed tasks if the continuations buffer is full. - /// Dispatcher to pass to inline executed tasks if the continuations buffer is full. - /// Task to execute upon completing all associated tasks, if any. Any task with a null will not be executed. - /// Handle of the allocated continuation. - /// Note that this will keep trying until allocation succeeds. If something is blocking allocation, such as insufficient room in the continuations buffer and there are no workers consuming tasks, this will block forever. - public ContinuationHandle AllocateContinuation(int taskCount, int workerIndex, IThreadDispatcher dispatcher, Task onCompleted = default) - { - var waiter = new SpinWait(); - ContinuationHandle handle; - AllocateTaskContinuationResult result; - while ((result = TryAllocateContinuation(taskCount, out handle, onCompleted)) != AllocateTaskContinuationResult.Success) - { - if (result == AllocateTaskContinuationResult.Full) - { - var dequeueResult = TryDequeueAndRun(workerIndex, dispatcher); - Debug.Assert(dequeueResult != DequeueTaskResult.Stop, "We're trying to allocate a continuation, we shouldn't have run into a stop command!"); - } - else - { - waiter.SpinOnce(-1); - } - } - return handle; - } - - /// - /// Attempts to dequeue a task. - /// - /// Dequeued task, if any. - /// Result status of the dequeue attempt. - public DequeueTaskResult TryDequeue(out Task task) - { - task = default; - while (true) - { - long nextTaskIndex, sampledWrittenTaskIndex; - //Note that there is no lock taken. We sample the currently visible values and treat the dequeue as a transaction. - //If the transaction fails, we don't make any changes and try again. - if (Environment.Is64BitProcess) //This branch is compile time (at least where I've tested it). - { - //It's fine if we don't get a consistent view of the task index and written task index. Worst case scenario, this will claim that the queue is empty where a lock wouldn't. - nextTaskIndex = Volatile.Read(ref paddedCounters.TaskIndex); - sampledWrittenTaskIndex = Volatile.Read(ref paddedCounters.WrittenTaskIndex); - } - else - { - //Interlocked reads for 32 bit systems. - nextTaskIndex = Interlocked.Read(ref paddedCounters.TaskIndex); - sampledWrittenTaskIndex = Interlocked.Read(ref paddedCounters.WrittenTaskIndex); - } - if (nextTaskIndex >= sampledWrittenTaskIndex) - return DequeueTaskResult.Empty; - ref var taskCandidate = ref tasks[(int)(nextTaskIndex & taskMask)]; - if (taskCandidate.Function == null) - return DequeueTaskResult.Stop; - //Unlike enqueues, a dequeue has a fixed contention window on a single value. There's not much point in using a spinwait when there's no reason to expect our next attempt to be blocked. - if (Interlocked.CompareExchange(ref paddedCounters.TaskIndex, nextTaskIndex + 1, nextTaskIndex) != nextTaskIndex) - continue; - //There's an actual job! - task = taskCandidate; - Debug.Assert(!task.Continuation.Completed); - return DequeueTaskResult.Success; - } - } - - /// - /// Attempts to dequeue a task and run it. - /// - /// Index of the worker to pass into the task function. - /// Thread dispatcher running this task queue. - /// Result status of the dequeue attempt. - public DequeueTaskResult TryDequeueAndRun(int workerIndex, IThreadDispatcher dispatcher) - { - var result = TryDequeue(out var task); - if (result == DequeueTaskResult.Success) - { - task.Run(workerIndex, dispatcher); - } - return result; - } - - EnqueueTaskResult TryEnqueueUnsafelyInternal(Span tasks, out long taskEndIndex) - { - Debug.Assert(tasks.Length > 0, "Probably shouldn't be trying to enqueue zero tasks."); - Debug.Assert(paddedCounters.WrittenTaskIndex == 0 || this.tasks[(int)((paddedCounters.WrittenTaskIndex - 1) & taskMask)].Function != null, "No more jobs should be written after a stop command."); - var taskStartIndex = paddedCounters.AllocatedTaskIndex; - taskEndIndex = taskStartIndex + tasks.Length; - if (taskEndIndex - paddedCounters.TaskIndex > this.tasks.length) - { - //We've run out of space in the ring buffer. If we tried to write, we'd overwrite jobs that haven't yet been completed. - return EnqueueTaskResult.Full; - } - paddedCounters.AllocatedTaskIndex = taskEndIndex; - Debug.Assert(BitOperations.IsPow2(this.tasks.Length)); - var wrappedInclusiveStartIndex = (int)(taskStartIndex & taskMask); - //Note - 1; the taskEndIndex is exclusive. Working with the inclusive index means we don't have to worry about an end index of tasks.Length being wrapped to 0. - var wrappedInclusiveEndIndex = (int)((taskEndIndex - 1) & taskMask); - if (wrappedInclusiveEndIndex >= wrappedInclusiveStartIndex) - { - //We can just copy the whole task block as one blob. - Unsafe.CopyBlockUnaligned(ref *(byte*)(this.tasks.Memory + wrappedInclusiveStartIndex), ref Unsafe.As(ref MemoryMarshal.GetReference(tasks)), (uint)(Unsafe.SizeOf() * tasks.Length)); - } - else - { - //Copy the task block as two blobs. - ref var startTask = ref tasks[0]; - var firstRegionCount = this.tasks.length - wrappedInclusiveStartIndex; - ref var secondBlobStartTask = ref tasks[firstRegionCount]; - var secondRegionCount = tasks.Length - firstRegionCount; - Unsafe.CopyBlockUnaligned(ref *(byte*)(this.tasks.Memory + wrappedInclusiveStartIndex), ref Unsafe.As(ref startTask), (uint)(Unsafe.SizeOf() * firstRegionCount)); - Unsafe.CopyBlockUnaligned(ref *(byte*)this.tasks.Memory, ref Unsafe.As(ref secondBlobStartTask), (uint)(Unsafe.SizeOf() * secondRegionCount)); - } - //for (int i = 0; i < tasks.Length; ++i) - //{ - // var taskIndex = (int)((i + taskStartIndex) & taskMask); - // this.tasks[taskIndex] = tasks[i]; - //} - return EnqueueTaskResult.Success; - } - /// - /// Tries to appends a set of tasks to the task queue. Does not acquire a lock; cannot return . - /// - /// Tasks composing the job. - /// Result of the enqueue attempt. - /// This must not be used while other threads could be performing task enqueues or task dequeues. - public EnqueueTaskResult TryEnqueueUnsafely(Span tasks) - { - EnqueueTaskResult result; - if ((result = TryEnqueueUnsafelyInternal(tasks, out var taskEndIndex)) == EnqueueTaskResult.Success) - { - paddedCounters.WrittenTaskIndex = taskEndIndex; - } - return result; - } - /// - /// Tries to appends a task to the task queue. Does not acquire a lock; cannot return . - /// - /// Task to enqueue - /// Result of the enqueue attempt. - /// This must not be used while other threads could be performing task enqueues or task dequeues. - public EnqueueTaskResult TryEnqueueUnsafely(Task task) - { - return TryEnqueueUnsafely(new Span(&task, 1)); - } - - /// - /// Tries to appends a set of tasks to the task queue if the ring buffer is uncontested. - /// - /// Tasks composing the job. - /// Result of the enqueue attempt. - public EnqueueTaskResult TryEnqueue(Span tasks) - { - if (tasks.Length == 0) - return EnqueueTaskResult.Success; - if (Interlocked.CompareExchange(ref paddedCounters.TaskLocker, 1, 0) != 0) - return EnqueueTaskResult.Contested; - try - { - //We have the lock. - EnqueueTaskResult result; - if ((result = TryEnqueueUnsafelyInternal(tasks, out var taskEndIndex)) == EnqueueTaskResult.Success) - { - if (Environment.Is64BitProcess) - { - Volatile.Write(ref paddedCounters.WrittenTaskIndex, taskEndIndex); - } - else - { - Interlocked.Exchange(ref paddedCounters.WrittenTaskIndex, taskEndIndex); - } - } - return result; - } - finally - { - paddedCounters.TaskLocker = 0; - } - } - - /// - /// Appends a set of tasks to the queue. - /// - /// Tasks composing the job. - /// Worker index to pass to inline-executed tasks if the task buffer is full. - /// Dispatcher to pass to inline-executed tasks if the task buffer is full. - /// Note that this will keep trying until task submission succeeds. - /// If the task queue is full, this will opt to run some tasks inline while waiting for room. - public void Enqueue(Span tasks, int workerIndex, IThreadDispatcher dispatcher) - { - var waiter = new SpinWait(); - EnqueueTaskResult result; - while ((result = TryEnqueue(tasks)) != EnqueueTaskResult.Success) - { - if (result == EnqueueTaskResult.Full) - { - //Couldn't enqueue the tasks because the task buffer is full. - //Clearly there's plenty of work available to execute, so go ahead and try to run one task inline. - var task = tasks[0]; - task.Run(workerIndex, dispatcher); - if (tasks.Length == 1) - break; - tasks = tasks[1..]; - } - else - { - waiter.SpinOnce(-1); - } - } - } - - /// - /// Appends a set of tasks to the queue. Creates a continuation for the tasks. - /// - /// Tasks composing the job. A continuation will be assigned internally; no continuation should be present on any of the provided tasks. - /// Worker index to pass to inline-executed tasks if the task buffer is full. - /// Dispatcher to pass to inline-executed tasks if the task buffer is full. - /// Task to run upon completion of all the submitted tasks, if any. - /// Handle of the continuation created for these tasks. - /// Note that this will keep trying until task submission succeeds. - /// If the task queue is full, this will opt to run some tasks inline while waiting for room. - public ContinuationHandle AllocateContinuationAndEnqueue(Span tasks, int workerIndex, IThreadDispatcher dispatcher, Task onComplete = default) - { - var continuationHandle = AllocateContinuation(tasks.Length, workerIndex, dispatcher); - for (int i = 0; i < tasks.Length; ++i) - { - ref var task = ref tasks[i]; - Debug.Assert(!task.Continuation.Initialized, "This function creates a continuation for the tasks"); - task.Continuation = continuationHandle; - } - Enqueue(tasks, workerIndex, dispatcher); - return continuationHandle; - } - - /// - /// Waits for a continuation to be completed. - /// - /// Instead of spinning the entire time, this may dequeue and execute pending tasks to fill the gap. - /// Continuation to wait on. - /// Currently executing worker index to pass to any tasks used to fill the wait period. - /// Currently executing dispatcher to pass to any tasks used to fill the wait period. - public void WaitForCompletion(ContinuationHandle continuation, int workerIndex, IThreadDispatcher dispatcher) - { - var waiter = new SpinWait(); - Debug.Assert(continuation.Initialized, "This codepath should only run if the continuation was allocated earlier."); - while (!continuation.Completed) - { - //Note that we don't handle the DequeueResult.Stop case; if the job isn't complete yet, there's no way to hit a stop unless we enqueued this job after a stop. - //Enqueuing after a stop is an error condition and is debug checked for in TryEnqueueJob. - var dequeueResult = TryDequeue(out var fillerTask); - if (dequeueResult == DequeueTaskResult.Stop) - { - Debug.Assert(dequeueResult != DequeueTaskResult.Stop, "Did you enqueue these tasks *after* some thread enqueued a stop command? That's illegal!"); - return; - } - if (dequeueResult == DequeueTaskResult.Success) - { - fillerTask.Run(workerIndex, dispatcher); - waiter.Reset(); - } - else - { - waiter.SpinOnce(-1); - } - } - } - - /// - /// Appends a set of tasks to the queue and returns when all tasks are complete. - /// - /// Tasks composing the job. A continuation will be assigned internally; no continuation should be present on any of the provided tasks. - /// Worker index to pass to inline-executed tasks if the task buffer is full. - /// Dispatcher to pass to inline-executed tasks if the task buffer is full. - /// Note that this will keep working until all tasks are run. - /// If the task queue is full, this will opt to run some tasks inline while waiting for room. - public void RunTasks(Span tasks, int workerIndex, IThreadDispatcher dispatcher) - { - if (tasks.Length == 0) - return; - ContinuationHandle continuationHandle = default; - if (tasks.Length > 1) - { - //Note that we only submit tasks to the queue for tasks beyond the first. The current thread is responsible for at least task 0. - var taskCount = tasks.Length - 1; - Span tasksToEnqueue = stackalloc Task[taskCount]; - continuationHandle = AllocateContinuation(taskCount, workerIndex, dispatcher); - for (int i = 0; i < tasksToEnqueue.Length; ++i) - { - var task = tasks[i + 1]; - Debug.Assert(continuationHandle.Continuations == continuationsContainer.Memory); - Debug.Assert(!task.Continuation.Initialized, $"None of the source tasks should have continuations when provided to {nameof(RunTasks)}."); - task.Continuation = continuationHandle; - tasksToEnqueue[i] = task; - } - Enqueue(tasksToEnqueue, workerIndex, dispatcher); - } - //Tasks [1, count) are submitted to the queue and may now be executing on other workers. - //The thread calling the for loop should not relinquish its timeslice. It should immediately begin working on task 0. - var task0 = tasks[0]; - Debug.Assert(!task0.Continuation.Initialized, $"None of the source tasks should have continuations when provided to {nameof(RunTasks)}."); - task0.Function(task0.Id, task0.Context, workerIndex, dispatcher); - - if (tasks.Length > 1) - { - //Task 0 is done; this thread should seek out other work until the job is complete. - WaitForCompletion(continuationHandle, workerIndex, dispatcher); - } - } - - /// - /// Tries to enqueues the stop command. - /// - /// Result status of the enqueue attempt. - public EnqueueTaskResult TryEnqueueStop() - { - Span stopJob = stackalloc Task[1]; - stopJob[0] = new Task { Function = null }; - return TryEnqueue(stopJob); - } - - /// - /// Enqueues the stop command. - /// - /// Worker index to pass to any inline executed tasks if the task buffer is full. - /// Dispatcher to pass to any inline executed tasks if the task buffer is full. - /// Note that this will keep trying until task submission succeeds. - /// If the task buffer is full, this may attempt to dequeue tasks to run inline. - public void EnqueueStop(int workerIndex, IThreadDispatcher dispatcher) - { - Span stopJob = stackalloc Task[1]; - stopJob[0] = new Task { Function = null }; - var waiter = new SpinWait(); - EnqueueTaskResult result; - while ((result = TryEnqueue(stopJob)) != EnqueueTaskResult.Success) - { - if (result == EnqueueTaskResult.Full) - { - var dequeueResult = TryDequeueAndRun(workerIndex, dispatcher); - Debug.Assert(dequeueResult != DequeueTaskResult.Stop, "We're trying to enqueue a stop, we shouldn't have found one already present!"); - } - else - { - waiter.SpinOnce(-1); - } - } - } - - /// - /// Tries to enqueues the stop command. Does not take a lock; cannot return . - /// - /// Result status of the enqueue attempt. - /// This must not be used while other threads could be performing task enqueues or task dequeues. - public EnqueueTaskResult TryEnqueueStopUnsafely() - { - Span stopJob = stackalloc Task[1]; - stopJob[0] = new Task { Function = null }; - return TryEnqueueUnsafely(stopJob); - } - - /// - /// Enqueues a for loop onto the task queue. Does not take a lock; cannot return . - /// - /// Function to execute on each iteration of the loop. - /// Context pointer to pass into each task execution. - /// Inclusive start index of the loop range. - /// Number of iterations to perform. - /// Continuation associated with the loop tasks, if any. - /// Status result of the enqueue operation. - /// This must not be used while other threads could be performing task enqueues or task dequeues. - public EnqueueTaskResult TryEnqueueForUnsafely(delegate* function, void* context, int inclusiveStartIndex, int iterationCount, ContinuationHandle continuation = default) - { - Span tasks = stackalloc Task[iterationCount]; - for (int i = 0; i < tasks.Length; ++i) - { - tasks[i] = new Task { Function = function, Context = context, Id = i + inclusiveStartIndex, Continuation = continuation }; - } - return TryEnqueueUnsafely(tasks); - } - - /// - /// Enqueues a for loop onto the task queue. - /// - /// Function to execute on each iteration of the loop. - /// Context pointer to pass into each task execution. - /// Inclusive start index of the loop range. - /// Number of iterations to perform. - /// Worker index to pass to any inline-executed task if the task queue is full. - /// Dispatcher to pass to any inline-executed task if the task queue is full. - /// Continuation associated with the loop tasks, if any. - /// This function will not usually attempt to run any iterations of the loop itself. It tries to push the loop tasks onto the queue. - /// If the task queue is full, this will opt to run the tasks inline while waiting for room. - public void EnqueueFor(delegate* function, void* context, int inclusiveStartIndex, int iterationCount, int workerIndex, IThreadDispatcher dispatcher, ContinuationHandle continuation = default) - { - Span tasks = stackalloc Task[iterationCount]; - for (int i = 0; i < tasks.Length; ++i) - { - tasks[i] = new Task { Function = function, Context = context, Id = i + inclusiveStartIndex, Continuation = continuation }; - } - Enqueue(tasks, workerIndex, dispatcher); - } - - /// - /// Submits a set of tasks representing a for loop over the given indices and returns when all loop iterations are complete. - /// - /// Function to execute on each iteration of the loop. - /// Context pointer to pass into each iteration of the loop. - /// Inclusive start index of the loop range. - /// Number of iterations to perform. - /// Index of the currently executing worker. - /// Currently executing thread dispatcher. - public void For(delegate* function, void* context, int inclusiveStartIndex, int iterationCount, int workerIndex, IThreadDispatcher dispatcher) - { - if (iterationCount <= 0) - return; - Span tasks = stackalloc Task[iterationCount]; - for (int i = 0; i < tasks.Length; ++i) - { - tasks[i] = new Task(function, context, inclusiveStartIndex + i); - } - RunTasks(tasks, workerIndex, dispatcher); - } -} diff --git a/BepuUtilities/TaskStack.cs b/BepuUtilities/TaskStack.cs deleted file mode 100644 index 26a5322bb..000000000 --- a/BepuUtilities/TaskStack.cs +++ /dev/null @@ -1,738 +0,0 @@ -using System; -using System.ComponentModel; -using System.Diagnostics; -using System.Diagnostics.CodeAnalysis; -using System.Numerics; -using System.Runtime.CompilerServices; -using System.Runtime.InteropServices; -using System.Threading; -using BepuUtilities.Collections; -using BepuUtilities.Memory; - -namespace BepuUtilities.TestStack; - -/// -/// Description of a task to be submitted to a . -/// -public unsafe struct Task -{ - /// - /// Function to be executed by the task. Takes as arguments the , pointer, and executing worker index. - /// - public delegate* Function; - /// - /// Context to be passed into the . - /// - public void* Context; - /// - /// Continuation to be notified after this task completes, if any. - /// - public ContinuationHandle Continuation; - /// - /// User-provided identifier of this task. - /// - public long Id; - - /// - /// Creates a new task. - /// - /// Function to be executed by the task. Takes as arguments the , pointer, executing worker index, and executing . - /// Context pointer to pass to the . - /// Id of this task to be passed into the . - /// Continuation to notify after the completion of this task, if any. - public Task(delegate* function, void* context = null, long taskId = 0, ContinuationHandle continuation = default) - { - Function = function; - Context = context; - Continuation = continuation; - Id = taskId; - } - - /// - /// Creates a task from a function. - /// - /// Function to turn into a task. - public static implicit operator Task(delegate* function) => new(function); - - /// - /// Runs the task and, if necessary, notifies the associated continuation of its completion. - /// - /// Worker index to pass to the function. - /// Dispatcher running this task. - public void Run(int workerIndex, IThreadDispatcher dispatcher) - { - Debug.Assert(!Continuation.Completed); - Function(Id, Context, workerIndex, dispatcher); - if (Continuation.Initialized) - Continuation.NotifyTaskCompleted(workerIndex, dispatcher); - } -} - -/// -/// Describes the result status of a pop attempt. -/// -public enum PopTaskResult -{ - /// - /// A task was successfully popped. - /// - Success, - /// - /// The pop was contested. - /// - Contested, - /// - /// The stack was empty, but may have more tasks in the future. - /// - Empty, - /// - /// The stack has been terminated and all threads seeking work should stop. - /// - Stop -} -/// -/// Describes the result of a task enqueue attempt. -/// -public enum PushTaskResult -{ - /// - /// The tasks were successfully pushed. - /// - Success, - /// - /// The push attempt was blocked by concurrent access. - /// - Contested, - /// - /// The push attempt was blocked because no space remained in the tasks buffer. - /// - Full, -} - - -/// -/// Refers to a continuation within a . -/// -public unsafe struct ContinuationHandle : IEquatable -{ - uint index; - uint encodedVersion; - /// - /// Source worker task stack. - /// - internal TaskStackContinuations* Continuations; - - internal ContinuationHandle(uint index, int version, TaskStackContinuations* continuations) - { - this.index = index; - encodedVersion = (uint)version | 1u << 31; - Continuations = continuations; - } - - internal uint Index - { - get - { - Debug.Assert(Initialized, "If you're trying to pull a continuation id from a continuation handle, it should have been initialized."); - return index; - } - } - - internal int Version - { - get - { - Debug.Assert(Initialized, "If you're trying to pull a continuation id from a continuation handle, it should have been initialized."); - return (int)(encodedVersion & ((1u << 31) - 1)); - } - } - - /// - /// Gets whether the tasks associated with this continuation have completed. - /// - public bool Completed => Continuations->IsComplete(this); - - /// - /// Retrieves a pointer to the continuation data for . - /// - /// Pointer to the continuation backing the given handle. - /// This should not be used if the continuation handle is not known to be valid. The data pointed to by the data could become invalidated if the continuation completes. - public TaskContinuation* Continuation => Continuations->GetContinuation(this); - - /// - /// Gets a null continuation handle. - /// - public static ContinuationHandle Null => default; - - /// - /// Gets whether the continuation associated with this handle was allocated and has outstanding tasks. - /// - public bool Exists - { - get - { - if (!Initialized || Continuations == null) - return false; - ref var continuation = ref Continuations->Continuations[Index]; - return continuation.Version == Version && continuation.RemainingTaskCounter > 0; - } - } - - /// - /// Gets whether this handle ever represented an allocated handle. This does not guarantee that the continuation's associated tasks are active in the that it was allocated from. - /// - public bool Initialized => encodedVersion >= 1u << 31; - - /// - /// Notifies the continuation that one task was completed. - /// - /// Worker index to pass to the continuation's delegate, if any. - /// Dispatcher to pass to the continuation's delegate, if any. - public void NotifyTaskCompleted(int workerIndex, IThreadDispatcher dispatcher) - { - var continuation = Continuation; - Debug.Assert(!Completed); - var counter = Interlocked.Decrement(ref continuation->RemainingTaskCounter); - Debug.Assert(counter >= 0, "The counter should not go negative. Was notify called too many times?"); - if (counter == 0) - { - //This entire job has completed. - if (continuation->OnCompleted.Function != null) - { - continuation->OnCompleted.Function(continuation->OnCompleted.Id, continuation->OnCompleted.Context, workerIndex, dispatcher); - } - //Free this continuation slot. - var waiter = new SpinWait(); - while (Interlocked.CompareExchange(ref Continuations->ContinuationLocker, 1, 0) != 0) - { - waiter.SpinOnce(-1); - } - //We have the lock. - Continuations->ContinuationIndexPool.ReturnUnsafely((int)Index); - --Continuations->ContinuationCount; - Continuations->ContinuationLocker = 0; - } - } - - public bool Equals(ContinuationHandle other) => other.index == index && other.encodedVersion == encodedVersion && other.Continuations == Continuations; - - public override bool Equals([NotNullWhen(true)] object obj) => obj is ContinuationHandle handle && Equals(handle); - - public override int GetHashCode() => (int)(index ^ (encodedVersion << 24)); - - public static bool operator ==(ContinuationHandle left, ContinuationHandle right) => left.Equals(right); - - public static bool operator !=(ContinuationHandle left, ContinuationHandle right) => !(left == right); -} - -/// -/// Stores data relevant to tracking task completion and reporting completion for a continuation. -/// -public unsafe struct TaskContinuation -{ - /// - /// Task to run upon completion of the associated task. - /// - public Task OnCompleted; - /// - /// Version of this continuation. - /// - public int Version; - /// - /// Number of tasks not yet reported as complete in the continuation. - /// - public int RemainingTaskCounter; -} - -unsafe struct TaskStackContinuations -{ - public Buffer Continuations; - public IdPool ContinuationIndexPool; - public int ContinuationCount; - public volatile int ContinuationLocker; - - public TaskStackContinuations(BufferPool pool, int maximumCapacity) - { - pool.Take(maximumCapacity, out Continuations); - ContinuationIndexPool = new IdPool(maximumCapacity, pool); - } - - public void Dispose(BufferPool pool) - { - pool.Return(ref Continuations); - ContinuationIndexPool.Dispose(pool); - } - - /// - /// Retrieves a pointer to the continuation data for . - /// - /// Handle to look up the associated continuation for. - /// Pointer to the continuation backing the given handle. - /// This should not be used if the continuation handle is not known to be valid. The data pointed to by the data could become invalidated if the continuation completes. - public TaskContinuation* GetContinuation(ContinuationHandle continuationHandle) - { - Debug.Assert(continuationHandle.Initialized, "This continuation handle was never initialized."); - Debug.Assert(continuationHandle.Index < Continuations.length, "This continuation refers to an invalid index."); - if (continuationHandle.Index >= Continuations.length || !continuationHandle.Initialized) - return null; - var continuation = Continuations.Memory + continuationHandle.Index; - Debug.Assert(continuation->Version == continuationHandle.Version, "This continuation no longer refers to an active continuation."); - if (continuation->Version != continuationHandle.Version) - return null; - return Continuations.Memory + continuationHandle.Index; - } - - /// - /// Checks whether all tasks associated with this continuation have completed. - /// - /// Continuation to check for completion. - /// True if all tasks associated with a continuation have completed, false otherwise. - public bool IsComplete(ContinuationHandle continuationHandle) - { - Debug.Assert(continuationHandle.Initialized, "This continuation handle was never initialized."); - Debug.Assert(continuationHandle.Index < Continuations.length, "This continuation refers to an invalid index."); - if (continuationHandle.Index >= Continuations.length || !continuationHandle.Initialized) - return false; - ref var continuation = ref Continuations[continuationHandle.Index]; - return continuation.Version > continuationHandle.Version || continuation.RemainingTaskCounter == 0; - } -} - -/// -/// Manages task stacks for parallel workers. Supports work stealing between workers. -/// -public unsafe struct TaskStack -{ - [StructLayout(LayoutKind.Explicit, Size = 256 + 16)] - struct StopPad - { - [FieldOffset(128)] - public volatile bool Stop; - } - StopPad padded; - - - Buffer continuations; - public QuickList Tasks; - public volatile int TaskLocker; - - /// - /// Constructs a new parallel task stack. - /// - /// Buffer pool to allocate non-thread allocated resources from. - /// Thread dispatcher calling this TaskStack. - /// Maximum number of tasks to allocate space for in each worker. Tasks are individual chunks of scheduled work.. - /// Maximum number of continuations to allocate space for in each worker. - public TaskStack(BufferPool pool, IThreadDispatcher dispatcher, int maximumTaskCapacity = 512, int maximumContinuationCapacity = 512) - { - Tasks = new QuickList(maximumTaskCapacity, pool); - pool.Take(1, out continuations); - continuations[0] = new TaskStackContinuations(pool, maximumContinuationCapacity); -#if DEBUG - //While you shouldn't *need* to clear continuations, it can be useful for debug purposes. - continuations[0].Continuations.Clear(0, continuations[0].Continuations.length); - Tasks.Span.Clear(0, Tasks.Span.length); -#endif - Reset(); - } - - /// - /// Returns unmanaged resources held by the to a pool. - /// - /// Buffer pool to return resources to. - public void Dispose(BufferPool pool) - { - Tasks.Dispose(pool); - continuations[0].Dispose(pool); - } - - /// - /// Returns the stack to a fresh state without reallocating. - /// - public void Reset() - { - Tasks.Count = 0; - Debug.Assert(TaskLocker == 0, "There appears to be a thread actively working still. That's invalid."); -#if DEBUG - //While you shouldn't *need* to clear continuations, it can be useful for debug purposes. - continuations[0].Continuations.Clear(0, continuations[0].Continuations.length); -#endif - continuations[0].ContinuationCount = 0; - Debug.Assert(continuations[0].ContinuationLocker == 0, "There appears to be a thread actively working still. That's invalid."); - padded.Stop = false; - } - - /// - /// Gets the number of active tasks. - /// - public int TaskCount - { - get - { - return Tasks.Count; - } - } - /// - /// Gets the number of active continuations. - /// - public int ContinuationCount - { - get - { - return continuations[0].ContinuationCount; - } - } - - /// - /// Attempts to pop a task from the stack. - /// - /// Task popped from the stack, if any. - /// True if a task was available to pop, false otherwise. - public PopTaskResult TryPop(out Task task) - { - //Quick early out to avoid lock expense. - if (Tasks.Count == 0) - { - task = default; - return padded.Stop ? PopTaskResult.Stop : PopTaskResult.Empty; - } - if (Interlocked.CompareExchange(ref TaskLocker, 1, 0) != 0) - { - task = default; - return PopTaskResult.Contested; - } - try - { - //We have the lock. - if (Tasks.TryPop(out task)) - { - return PopTaskResult.Success; - } - return PopTaskResult.Empty; - } - finally - { - TaskLocker = 0; - } - } - - /// - /// Pushes a set of tasks onto the stack. Does not acquire a lock. - /// - /// Tasks composing the job. - /// True if there was room to complete the push, false otherwise. - public bool TryPushUnsafely(Span tasks) - { - Debug.Assert(tasks.Length > 0, "Probably shouldn't be trying to push zero tasks."); - if (tasks.Length + Tasks.Count > Tasks.Span.Length) - { - return false; - } - Tasks.AddRangeUnsafely(tasks); - return true; - } - - /// - /// Pushes a task onto the task stack. Does not acquire a lock. - /// - /// Task to push - /// True if there was room to complete the push, false otherwise. - public unsafe bool TryPushUnsafely(Task task) - { - return TryPushUnsafely(new Span(&task, 1)); - } - - /// - /// Tries to push a set of tasks to the task stack if the stack is uncontested. - /// - /// Tasks composing the job. - /// Status of the push attempt. - public PushTaskResult TryPush(Span tasks) - { - if (Interlocked.CompareExchange(ref TaskLocker, 1, 0) != 0) - return PushTaskResult.Contested; - try - { - //We have the lock. - return TryPushUnsafely(tasks) ? PushTaskResult.Success : PushTaskResult.Full; - } - finally - { - TaskLocker = 0; - } - } - - /// - /// Push a set of tasks to the task stack. - /// - /// Tasks composing the job. - /// Worker index of the currently executing thread. - /// Currently executing thread dispatcher. - /// Runs filler tasks if the push is blocked by a lack of capacity. - public void Push(Span tasks, int workerIndex, IThreadDispatcher dispatcher) - { - var waiter = new SpinWait(); - PushTaskResult result; - while ((result = TryPush(tasks)) != PushTaskResult.Success) - { - if (result == PushTaskResult.Contested) - waiter.SpinOnce(-1); - else if (result == PushTaskResult.Full) - { - waiter.Reset(); - while (true) - { - var popResult = TryPopAndRun(workerIndex, dispatcher); - Debug.Assert(popResult != PopTaskResult.Stop, "Should not try to push onto a stopped stack."); - if (popResult == PopTaskResult.Stop) return; - if (popResult != PopTaskResult.Contested) break; - waiter.SpinOnce(-1); - } - waiter.Reset(); - } - } - } - - /// - /// Attempts to allocate a continuation for a set of tasks. - /// - /// Number of tasks associated with the continuation. - /// Function to execute upon completing all associated tasks, if any. Any task with a null will not be executed. - /// Handle of the continuation if allocation is successful. - /// True if the continuation was allocated, false if the attempt was contested.. - public bool TryAllocateContinuation(int taskCount, out ContinuationHandle continuationHandle, Task onCompleted = default) - { - continuationHandle = default; - ref var continuations = ref this.continuations[0]; - if (Interlocked.CompareExchange(ref continuations.ContinuationLocker, 1, 0) != 0) - return false; - try - { - //We have the lock. - var index = continuations.ContinuationIndexPool.Take(); - ++continuations.ContinuationCount; - ref var continuation = ref continuations.Continuations[index]; - //Note that the version number could be based on undefined data initially. That's actually fine; all we care about is whether it is different. - //Note mask to leave a valid bit for encoding in the handle. - var newVersion = (continuation.Version + 1) & (~(1 << 31)); - continuation.OnCompleted = onCompleted; - continuation.Version = newVersion; - continuation.RemainingTaskCounter = taskCount; - continuationHandle = new ContinuationHandle((uint)index, newVersion, this.continuations.Memory); - return true; - } - finally - { - continuations.ContinuationLocker = 0; - } - } - - /// - /// Allocates a continuation for a set of tasks. - /// - /// Number of tasks associated with the continuation. - /// Index of the worker to pass into the task function. - /// Thread dispatcher running this task stack. - /// Task to execute upon completing all associated tasks, if any. Any task with a null will not be executed. - /// Handle of the allocated continuation. - /// Note that this will keep trying until allocation succeeds. - public ContinuationHandle AllocateContinuation(int taskCount, int workerIndex, IThreadDispatcher dispatcher, Task onCompleted = default) - { - var waiter = new SpinWait(); - ContinuationHandle handle; - while (!TryAllocateContinuation(taskCount, out handle, onCompleted)) - { - var result = TryPopAndRun(workerIndex, dispatcher); - Debug.Assert(result != PopTaskResult.Stop, "Should not attempt to allocate a continuation after a stop command."); - if (result == PopTaskResult.Stop) - return default; - if (result == PopTaskResult.Success) - waiter.Reset(); - else - waiter.SpinOnce(-1); - } - return handle; - } - - - /// - /// Attempts to pop a task and run it. - /// - /// Index of the worker to pass into the task function. - /// Thread dispatcher running this task stack. - /// Result status of the pop attempt. - public PopTaskResult TryPopAndRun(int workerIndex, IThreadDispatcher dispatcher) - { - var result = TryPop(out var task); - if (result == PopTaskResult.Success) - { - task.Run(workerIndex, dispatcher); - } - return result; - } - - /// - /// Pushes a set of tasks to the stack with a created continuation. - /// - /// Tasks composing the job. A continuation will be assigned internally; no continuation should be present on any of the provided tasks. - /// Thread dispatcher calling this TaskStack. - /// Index of the currently executing worker. - /// Task to run upon completion of all the submitted tasks, if any. - /// Handle of the continuation created for these tasks. - /// Note that this will keep trying until task submission succeeds. - public ContinuationHandle AllocateContinuationAndPush(Span tasks, int workerIndex, IThreadDispatcher dispatcher, Task onComplete = default) - { - var continuationHandle = AllocateContinuation(tasks.Length, workerIndex, dispatcher, onComplete); - for (int i = 0; i < tasks.Length; ++i) - { - ref var task = ref tasks[i]; - Debug.Assert(!task.Continuation.Initialized, "This function creates a continuation for the tasks"); - task.Continuation = continuationHandle; - } - Push(tasks, workerIndex, dispatcher); - return continuationHandle; - } - - /// - /// Waits for a continuation to be completed. - /// - /// Instead of spinning the entire time, this may pop and execute pending tasks to fill the gap. - /// Continuation to wait on. - /// Thread dispatcher calling this TaskStack. - /// Index of the executing worker. - public void WaitForCompletion(ContinuationHandle continuation, int workerIndex, IThreadDispatcher dispatcher) - { - var waiter = new SpinWait(); - Debug.Assert(continuation.Initialized, "This codepath should only run if the continuation was allocated earlier."); - while (!continuation.Completed) - { - var result = TryPop(out var fillerTask); - if (result == PopTaskResult.Stop) - { - return; - } - if (result == PopTaskResult.Success) - { - fillerTask.Run(workerIndex, dispatcher); - waiter.Reset(); - } - else - { - waiter.SpinOnce(-1); - } - } - } - - /// - /// Pushes a set of tasks to the worker stack and returns when all tasks are complete. - /// - /// Tasks composing the job. A continuation will be assigned internally; no continuation should be present on any of the provided tasks. - /// Index of the worker executing this function. - /// Thread dispatcher calling this TaskStack. - /// Note that this will keep working until all tasks are run. It may execute tasks unrelated to the requested tasks while waiting on other workers to complete constituent tasks. - public void RunTasks(Span tasks, int workerIndex, IThreadDispatcher dispatcher) - { - if (tasks.Length == 0) - return; - ContinuationHandle continuationHandle = default; - if (tasks.Length > 1) - { - //Note that we only submit tasks to the stack for tasks beyond the first. The current thread is responsible for at least task 0. - var taskCount = tasks.Length - 1; - Span tasksToPush = stackalloc Task[taskCount]; - continuationHandle = AllocateContinuation(taskCount, workerIndex, dispatcher); - for (int i = 0; i < tasksToPush.Length; ++i) - { - var task = tasks[i + 1]; - Debug.Assert(continuationHandle.Continuations == this.continuations.Memory); - Debug.Assert(!task.Continuation.Initialized, $"None of the source tasks should have continuations when provided to {nameof(RunTasks)}."); - task.Continuation = continuationHandle; - tasksToPush[i] = task; - } - Push(tasksToPush, workerIndex, dispatcher); - } - //Tasks [1, count) are submitted to the stack and may now be executing on other workers. - //The thread calling the for loop should not relinquish its timeslice. It should immediately begin working on task 0. - var task0 = tasks[0]; - Debug.Assert(!task0.Continuation.Initialized, $"None of the source tasks should have continuations when provided to {nameof(RunTasks)}."); - task0.Function(task0.Id, task0.Context, workerIndex, dispatcher); - - if (tasks.Length > 1) - { - //Task 0 is done; this thread should seek out other work until the job is complete. - WaitForCompletion(continuationHandle, workerIndex, dispatcher); - } - } - - /// - /// Requests that all workers stop. The next time a worker runs out of tasks to run, if it sees a stop command, it will be reported. - /// - public void RequestStop() - { - padded.Stop = true; - } - - - /// - /// Pushes a for loop onto the task stack. Does not take a lock. - /// - /// Function to execute on each iteration of the loop. - /// Context pointer to pass into each task execution. - /// Inclusive start index of the loop range. - /// Number of iterations to perform. - /// Continuation associated with the loop tasks, if any. - /// True if there was enough room on the stack to push, false otherwise. - /// This must not be used while other threads could be performing task pushes or pops that could affect the specified worker. - public bool TryPushForUnsafely(delegate* function, void* context, int inclusiveStartIndex, int iterationCount, ContinuationHandle continuation = default) - { - Span tasks = stackalloc Task[iterationCount]; - for (int i = 0; i < tasks.Length; ++i) - { - tasks[i] = new Task { Function = function, Context = context, Id = i + inclusiveStartIndex, Continuation = continuation }; - } - return TryPushUnsafely(tasks); - } - - /// - /// Pushes a for loop onto the task stack. - /// - /// Function to execute on each iteration of the loop. - /// Context pointer to pass into each task execution. - /// Inclusive start index of the loop range. - /// Number of iterations to perform. - /// Thread dispatcher calling this TaskStack. - /// Index of the worker stack to push the tasks onto. - /// Continuation associated with the loop tasks, if any. - /// This function will not usually attempt to run any iterations of the loop itself. It tries to push the loop tasks onto the stack. - /// If the task stack is full, this will opt to run the tasks inline while waiting for room. - public void PushFor(delegate* function, void* context, int inclusiveStartIndex, int iterationCount, int workerIndex, IThreadDispatcher dispatcher, ContinuationHandle continuation = default) - { - Span tasks = stackalloc Task[iterationCount]; - for (int i = 0; i < tasks.Length; ++i) - { - tasks[i] = new Task { Function = function, Context = context, Id = i + inclusiveStartIndex, Continuation = continuation }; - } - Push(tasks, workerIndex, dispatcher); - } - - /// - /// Submits a set of tasks representing a for loop over the given indices and returns when all loop iterations are complete. - /// - /// Function to execute on each iteration of the loop. - /// Context pointer to pass into each iteration of the loop. - /// Inclusive start index of the loop range. - /// Number of iterations to perform. - /// Thread dispatcher calling this TaskStack. - /// Index of the worker stack to push the tasks onto. - public void For(delegate* function, void* context, int inclusiveStartIndex, int iterationCount, int workerIndex, IThreadDispatcher dispatcher) - { - if (iterationCount <= 0) - return; - Span tasks = stackalloc Task[iterationCount]; - for (int i = 0; i < tasks.Length; ++i) - { - tasks[i] = new Task(function, context, inclusiveStartIndex + i); - } - RunTasks(tasks, workerIndex, dispatcher); - } -} diff --git a/Demos/SpecializedTests/TaskQueueTestDemo.cs b/Demos/SpecializedTests/TaskQueueTestDemo.cs index c4969661e..ff96f4758 100644 --- a/Demos/SpecializedTests/TaskQueueTestDemo.cs +++ b/Demos/SpecializedTests/TaskQueueTestDemo.cs @@ -10,10 +10,6 @@ using System.Runtime.CompilerServices; using System.Runtime.InteropServices; using System.Reflection.Metadata; -using BepuUtilities.TestQueue; -using BepuUtilities.TestParallelStack; -using TaskQ = BepuUtilities.TestQueue.Task; -using BepuUtilities.TestStack; using BepuUtilities.TestLinkedTaskStack; namespace Demos.SpecializedTests; @@ -71,30 +67,10 @@ static void Test(long taskId, void* context, int workerIndex, IThreadDispatch const int subtaskCount = 8; var context1 = new DynamicContext1 { Context = typedContext }; var context2 = new DynamicContext2 { Context = typedContext }; - if (typeof(T) == typeof(TaskQueue)) - { - var queue = (TaskQueue*)typedContext->TaskPile; - queue->For(&DynamicallyEnqueuedTest1, &context1, 0, subtaskCount, workerIndex, dispatcher); - queue->For(&DynamicallyEnqueuedTest2, &context2, 0, subtaskCount, workerIndex, dispatcher); - } - else if (typeof(T) == typeof(ParallelTaskStack)) - { - var stack = (ParallelTaskStack*)typedContext->TaskPile; - stack->For(&DynamicallyEnqueuedTest1, &context1, 0, subtaskCount, workerIndex, dispatcher); - stack->For(&DynamicallyEnqueuedTest2, &context2, 0, subtaskCount, workerIndex, dispatcher); - } - else if (typeof(T) == typeof(TaskStack)) - { - var stack = (TaskStack*)typedContext->TaskPile; - stack->For(&DynamicallyEnqueuedTest1, &context1, 0, subtaskCount, workerIndex, dispatcher); - stack->For(&DynamicallyEnqueuedTest2, &context2, 0, subtaskCount, workerIndex, dispatcher); - } - else if (typeof(T) == typeof(LinkedTaskStack)) - { - var stack = (LinkedTaskStack*)typedContext->TaskPile; - stack->For(&DynamicallyEnqueuedTest1, &context1, 0, subtaskCount, workerIndex, dispatcher); - stack->For(&DynamicallyEnqueuedTest2, &context2, 0, subtaskCount, workerIndex, dispatcher); - } + + var stack = (LinkedTaskStack*)typedContext->TaskPile; + stack->For(&DynamicallyEnqueuedTest1, &context1, 0, subtaskCount, workerIndex, dispatcher); + stack->For(&DynamicallyEnqueuedTest2, &context2, 0, subtaskCount, workerIndex, dispatcher); } Interlocked.Add(ref typedContext->Sum, sum); } @@ -123,26 +99,9 @@ static void DispatcherBody(int workerIndex, IThreadDispatcher dispatcher) whe { //if (workerIndex > 1) // return; - if (typeof(T) == typeof(TaskQueue)) - { - var taskQueue = (TaskQueue*)dispatcher.UnmanagedContext; - while (taskQueue->TryDequeueAndRun(workerIndex, dispatcher) != DequeueTaskResult.Stop) ; - } - else if (typeof(T) == typeof(ParallelTaskStack)) - { - var taskStack = (ParallelTaskStack*)dispatcher.UnmanagedContext; - while (taskStack->TryPopAndRun(workerIndex, dispatcher) != BepuUtilities.TestParallelStack.PopTaskResult.Stop) ; - } - else if (typeof(T) == typeof(TaskStack)) - { - var taskStack = (TaskStack*)dispatcher.UnmanagedContext; - while (taskStack->TryPopAndRun(workerIndex, dispatcher) != BepuUtilities.TestStack.PopTaskResult.Stop) ; - } - else if (typeof(T) == typeof(LinkedTaskStack)) - { - var taskStack = (LinkedTaskStack*)dispatcher.UnmanagedContext; - while (taskStack->TryPopAndRun(workerIndex, dispatcher) != BepuUtilities.TestLinkedTaskStack.PopTaskResult.Stop) ; - } + var taskStack = (LinkedTaskStack*)dispatcher.UnmanagedContext; + while (taskStack->TryPopAndRun(workerIndex, dispatcher) != BepuUtilities.TestLinkedTaskStack.PopTaskResult.Stop) ; + } struct Context @@ -154,14 +113,7 @@ struct Context static void IssueStop(long id, void* context, int workerIndex, IThreadDispatcher dispatcher) where T : unmanaged { var typedContext = (Context*)context; - if (typeof(T) == typeof(TaskQueue)) - ((TaskQueue*)typedContext->TaskPile)->EnqueueStop(workerIndex, dispatcher); - else if (typeof(T) == typeof(ParallelTaskStack)) - ((ParallelTaskStack*)typedContext->TaskPile)->RequestStop(); - else if (typeof(T) == typeof(TaskStack)) - ((TaskStack*)typedContext->TaskPile)->RequestStop(); - else if (typeof(T) == typeof(LinkedTaskStack)) - ((LinkedTaskStack*)typedContext->TaskPile)->RequestStop(); + ((LinkedTaskStack*)typedContext->TaskPile)->RequestStop(); } @@ -214,80 +166,6 @@ public override void Initialize(ContentArchive content, Camera camera) - - ////TASK STACK - //var taskStack = new TaskStack(BufferPool, ThreadDispatcher); - //var taskStackPointer = &taskStack; - - //Test(() => - //{ - // var context = new Context { TaskPile = taskStackPointer }; - // var continuation = taskStackPointer->AllocateContinuation(iterationCount * tasksPerIteration, 0, ThreadDispatcher, new BepuUtilities.TestStack.Task(&IssueStop, &context)); - // for (int i = 0; i < iterationCount; ++i) - // { - // taskStackPointer->TryPushForUnsafely(&Test, &context, i * tasksPerIteration, tasksPerIteration, continuation); - // } - // //taskQueuePointer->TryEnqueueStopUnsafely(); - // //taskQueuePointer->EnqueueTasks() - // ThreadDispatcher.DispatchWorkers(&DispatcherBody, unmanagedContext: taskStackPointer); - // return context.Sum; - //}, "MT Stack", () => taskStackPointer->Reset()); - - - //taskStack.Dispose(BufferPool); - - - ////PARALLEL TASK STACK - //var parallelTaskStack = new ParallelTaskStack(BufferPool, ThreadDispatcher, ThreadDispatcher.ThreadCount); - //var parallelTaskStackPointer = ¶llelTaskStack; - - //Test(() => - //{ - // var context = new Context { TaskPile = parallelTaskStackPointer }; - // var continuation = parallelTaskStackPointer->AllocateContinuation(iterationCount * tasksPerIteration, 0, ThreadDispatcher, new BepuUtilities.TestParallelStack.Task(&IssueStop, &context)); - // for (int i = 0; i < iterationCount; ++i) - // { - // parallelTaskStackPointer->PushForUnsafely(&Test, &context, i * tasksPerIteration, tasksPerIteration, i % ThreadDispatcher.ThreadCount, ThreadDispatcher, continuation); - // } - // //taskQueuePointer->TryEnqueueStopUnsafely(); - // //taskQueuePointer->EnqueueTasks() - // ThreadDispatcher.DispatchWorkers(&DispatcherBody, unmanagedContext: parallelTaskStackPointer); - // return context.Sum; - //}, "MT Parallel Stack", () => parallelTaskStackPointer->Reset()); - - //var noSteal = ParallelTaskStack.NoStealRequired; - //var stealRequired = ParallelTaskStack.StealRequired; - //var stealFail = ParallelTaskStack.PopFailed; - //var steal = stealRequired - stealFail; - - //Console.WriteLine($"Nosteal, steal succeed, steal fail: {noSteal}, {steal}, {stealFail} ({steal / (double)(stealRequired + noSteal)})"); - - //parallelTaskStack.Dispose(BufferPool); - - - for (int i = 0; i < 10; ++i) - { - //TASK QUEUE - var taskQueue = new TaskQueue(BufferPool, maximumTaskCapacity: 1 << 19, maximumContinuationCapacity: 1 << 19); - var taskQueuePointer = &taskQueue; - - Test(() => - { - var context = new Context { TaskPile = taskQueuePointer }; - var continuation = taskQueuePointer->AllocateContinuation(iterationCount * tasksPerIteration, 0, ThreadDispatcher, new TaskQ(&IssueStop, &context)); - for (int i = 0; i < iterationCount; ++i) - { - taskQueuePointer->TryEnqueueForUnsafely(&Test, &context, i * tasksPerIteration, tasksPerIteration, continuation); - } - //taskQueuePointer->TryEnqueueStopUnsafely(); - //taskQueuePointer->EnqueueTasks() - ThreadDispatcher.DispatchWorkers(&DispatcherBody, unmanagedContext: taskQueuePointer); - return context.Sum; - }, "MT", () => taskQueuePointer->Reset()); - - taskQueue.Dispose(BufferPool); - } - //Test(() => //{ // var testContext = new Context { }; From d0e438c6ba3e05eaba984f502c6d8b626fc49fe3 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 7 Jun 2023 16:59:40 -0500 Subject: [PATCH 727/947] Refactored LinkedTaskStack into a nicer namespace and set of files. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 24 +- BepuUtilities/BepuUtilities.csproj | 2 +- .../TaskScheduling/ContinuationBlock.cs | 44 ++ .../TaskScheduling/ContinuationHandle.cs | 80 +++ BepuUtilities/TaskScheduling/IJobFilter.cs | 124 +++++ BepuUtilities/TaskScheduling/Job.cs | 65 +++ BepuUtilities/TaskScheduling/PopTaskResult.cs | 20 + BepuUtilities/TaskScheduling/Task.cs | 60 +++ .../TaskScheduling/TaskContinuation.cs | 16 + .../TaskStack.cs} | 496 +----------------- BepuUtilities/TaskScheduling/Worker.cs | 101 ++++ Demos/SpecializedTests/TaskQueueTestDemo.cs | 20 +- 12 files changed, 532 insertions(+), 520 deletions(-) create mode 100644 BepuUtilities/TaskScheduling/ContinuationBlock.cs create mode 100644 BepuUtilities/TaskScheduling/ContinuationHandle.cs create mode 100644 BepuUtilities/TaskScheduling/IJobFilter.cs create mode 100644 BepuUtilities/TaskScheduling/Job.cs create mode 100644 BepuUtilities/TaskScheduling/PopTaskResult.cs create mode 100644 BepuUtilities/TaskScheduling/Task.cs create mode 100644 BepuUtilities/TaskScheduling/TaskContinuation.cs rename BepuUtilities/{LinkedTaskStack.cs => TaskScheduling/TaskStack.cs} (58%) create mode 100644 BepuUtilities/TaskScheduling/Worker.cs diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index c83043d6d..6e92046c9 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -1,25 +1,15 @@ -using BepuPhysics.Collidables; -using BepuPhysics.CollisionDetection; -using BepuPhysics.Constraints; -using BepuPhysics.Constraints.Contact; -using BepuUtilities; +using BepuUtilities; using BepuUtilities.Collections; using BepuUtilities.Memory; -using BepuUtilities.TestLinkedTaskStack; +using BepuUtilities.TaskScheduling; using System; -using System.ComponentModel; using System.Diagnostics; -using System.Linq; -using System.Net; using System.Numerics; using System.Runtime.CompilerServices; using System.Runtime.InteropServices; using System.Runtime.Intrinsics; -using System.Runtime.Intrinsics.Arm; using System.Runtime.Intrinsics.X86; using System.Threading; -using System.Threading.Tasks.Sources; -using System.Xml.Linq; namespace BepuPhysics.Trees { @@ -421,7 +411,7 @@ public BinnedBuildWorkerContext(Buffer binAllocationBuffer, ref int binSta } unsafe struct MultithreadBinnedBuildContext : IBinnedBuilderThreading { - public LinkedTaskStack* TaskStack; + public TaskStack* TaskStack; /// /// The number of subtrees present at the root of the build. /// @@ -1257,7 +1247,7 @@ static unsafe void BinnedBuildNode( /// Multiplier to apply to the subtree count within a node to decide the bin count. Resulting value will then be clamped by the minimum/maximum bin counts. /// Threshold at or under which the binned builder resorts to local counting sort sweeps. static unsafe void BinnedBuilderInternal(Buffer subtrees, Buffer subtreesPong, Buffer nodes, Buffer metanodes, Buffer leaves, Buffer binIndices, - IThreadDispatcher dispatcher, LinkedTaskStack* taskStackPointer, int workerCount, BufferPool pool, int minimumBinCount, int maximumBinCount, float leafToBinMultiplier, int microsweepThreshold) + IThreadDispatcher dispatcher, TaskStack* taskStackPointer, int workerCount, BufferPool pool, int minimumBinCount, int maximumBinCount, float leafToBinMultiplier, int microsweepThreshold) { var subtreeCount = subtrees.Length; if (nodes.Length < subtreeCount - 1) @@ -1323,11 +1313,11 @@ static unsafe void BinnedBuilderInternal(Buffer subtrees, Buffer(long taskId, void* untypedC unsafe static void BinnedBuilderWorkerFunction(int workerIndex, IThreadDispatcher dispatcher) where TLeaves : unmanaged { - var taskQueue = (LinkedTaskStack*)dispatcher.UnmanagedContext; + var taskQueue = (TaskStack*)dispatcher.UnmanagedContext; PopTaskResult popTaskResult; var waiter = new SpinWait(); while ((popTaskResult = taskQueue->TryPopAndRun(workerIndex, dispatcher)) != PopTaskResult.Stop) diff --git a/BepuUtilities/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj index 699776961..6c6d46faa 100644 --- a/BepuUtilities/BepuUtilities.csproj +++ b/BepuUtilities/BepuUtilities.csproj @@ -45,7 +45,7 @@ - + \ No newline at end of file diff --git a/BepuUtilities/TaskScheduling/ContinuationBlock.cs b/BepuUtilities/TaskScheduling/ContinuationBlock.cs new file mode 100644 index 000000000..0cd0d8744 --- /dev/null +++ b/BepuUtilities/TaskScheduling/ContinuationBlock.cs @@ -0,0 +1,44 @@ +using BepuUtilities.Memory; + +namespace BepuUtilities.TaskScheduling; + +/// +/// Stores a block of task continuations that maintains a pointer to previous blocks. +/// +internal unsafe struct ContinuationBlock +{ + public ContinuationBlock* Previous; + + public int Count; + public Buffer Continuations; + + public static ContinuationBlock* Create(int continuationCapacity, BufferPool pool) + { + pool.Take(sizeof(TaskContinuation) * continuationCapacity + sizeof(ContinuationBlock), out var rawBuffer); + ContinuationBlock* block = (ContinuationBlock*)rawBuffer.Memory; + block->Continuations = new Buffer(rawBuffer.Memory + sizeof(ContinuationBlock), continuationCapacity, rawBuffer.Id); + block->Count = 0; + block->Previous = null; + return block; + } + + public bool TryAllocateContinuation(out TaskContinuation* continuation) + { + if (Count < Continuations.length) + { + continuation = Continuations.Memory + Count++; + return true; + } + continuation = null; + return false; + } + + public void Dispose(BufferPool pool) + { + var id = Continuations.Id; + pool.ReturnUnsafely(id); + if (Previous != null) + Previous->Dispose(pool); + this = default; + } +} diff --git a/BepuUtilities/TaskScheduling/ContinuationHandle.cs b/BepuUtilities/TaskScheduling/ContinuationHandle.cs new file mode 100644 index 000000000..9931b949d --- /dev/null +++ b/BepuUtilities/TaskScheduling/ContinuationHandle.cs @@ -0,0 +1,80 @@ +using System; +using System.Diagnostics; +using System.Diagnostics.CodeAnalysis; +using System.Threading; + +namespace BepuUtilities.TaskScheduling; + +/// +/// Refers to a continuation within a . +/// +public unsafe struct ContinuationHandle : IEquatable +{ + //This is a bit odd. We're presenting this pointer as a handle, even though it's not. + //Hiding the implementation detail makes it a little easier to change later if we need to. + TaskContinuation* continuation; + + internal ContinuationHandle(TaskContinuation* continuation) + { + this.continuation = continuation; + } + + /// + /// Gets whether the tasks associated with this continuation have completed. If the continuation has not been initialized, this will always return false. + /// + public bool Completed + { + get + { + return Initialized && continuation->RemainingTaskCounter <= 0; + } + } + + /// + /// Retrieves a pointer to the continuation data for . + /// + /// Pointer to the continuation backing the given handle. + /// This should not be used if the continuation handle is not known to be valid. The data pointed to by the data could become invalidated if the continuation completes. + public TaskContinuation* Continuation => continuation; + + /// + /// Gets a null continuation handle. + /// + public static ContinuationHandle Null => default; + + /// + /// Gets whether this handle ever represented an allocated handle. This does not guarantee that the continuation's associated tasks are active in the that it was allocated from. + /// + public bool Initialized => continuation != null; + + /// + /// Notifies the continuation that one task was completed. + /// + /// Worker index to pass to the continuation's delegate, if any. + /// Dispatcher to pass to the continuation's delegate, if any. + public void NotifyTaskCompleted(int workerIndex, IThreadDispatcher dispatcher) + { + var continuation = Continuation; + Debug.Assert(!Completed); + var counter = Interlocked.Decrement(ref continuation->RemainingTaskCounter); + Debug.Assert(counter >= 0, "The counter should not go negative. Was notify called too many times?"); + if (counter == 0) + { + //This entire job has completed. + if (continuation->OnCompleted.Function != null) + { + continuation->OnCompleted.Function(continuation->OnCompleted.Id, continuation->OnCompleted.Context, workerIndex, dispatcher); + } + } + } + + public bool Equals(ContinuationHandle other) => other.continuation == continuation; + + public override bool Equals([NotNullWhen(true)] object obj) => obj is ContinuationHandle handle && Equals(handle); + + public override int GetHashCode() => (int)continuation; + + public static bool operator ==(ContinuationHandle left, ContinuationHandle right) => left.Equals(right); + + public static bool operator !=(ContinuationHandle left, ContinuationHandle right) => !(left == right); +} diff --git a/BepuUtilities/TaskScheduling/IJobFilter.cs b/BepuUtilities/TaskScheduling/IJobFilter.cs new file mode 100644 index 000000000..e04e49074 --- /dev/null +++ b/BepuUtilities/TaskScheduling/IJobFilter.cs @@ -0,0 +1,124 @@ +using System; + +namespace BepuUtilities.TaskScheduling; + +/// +/// Determines which jobs are allowed to serve a request. +/// +public interface IJobFilter +{ + /// + /// Determines whether a job with the given tag should be allowed to serve a request. + /// + /// Tag of the candidate job. + /// True if the job should be allowed to serve a request, false otherwise. + bool AllowJob(ulong jobTag); +} + + +/// +/// A filter that will allow pops from any jobs. +/// +public struct AllowAllJobs : IJobFilter +{ + /// + public readonly bool AllowJob(ulong jobTag) + { + return true; + } +} + +/// +/// A job filter that wraps a managed delegate. +/// +public struct DelegateJobFilter : IJobFilter +{ + /// + /// Delegate to use as the filter. + /// + public Func Filter; + /// + /// Creates a job filter that wraps a delegate. + /// + /// Delegate to use as the filter. + public DelegateJobFilter(Func filter) + { + Filter = filter; + } + /// + public readonly bool AllowJob(ulong jobTag) + { + return Filter(jobTag); + } +} + +/// +/// A job filter that wraps a function pointer. +/// +public unsafe struct FunctionPointerJobFilter : IJobFilter +{ + /// + /// Delegate to use as the filter. + /// + public delegate* Filter; + /// + /// Creates a job filter that wraps a delegate. + /// + /// Delegate to use as the filter. + public FunctionPointerJobFilter(delegate* filter) + { + Filter = filter; + } + /// + public readonly bool AllowJob(ulong jobTag) + { + return Filter(jobTag); + } +} +/// +/// A job filter that requires the job tag to meet or exceed a threshold value. +/// +public struct MinimumTagFilter : IJobFilter +{ + /// + /// Value that a job must match or exceed to be allowed. + /// + public ulong MinimumTagValue; + /// + /// Creates a job filter that requires the job tag to meet or exceed a threshold value. + /// + /// Value that a job must match or exceed to be allowed. + public MinimumTagFilter(ulong minimumTagValue) + { + MinimumTagValue = minimumTagValue; + } + /// + public bool AllowJob(ulong jobTag) + { + return jobTag >= MinimumTagValue; + } +} + +/// +/// A job filter that requires the job tag to match a specific value. +/// +public struct EqualTagFilter : IJobFilter +{ + /// + /// Tag value required to allow a job. + /// + public ulong RequiredTag; + + /// + /// Creates a job filter that requires the job tag to match a specific value. + /// + public EqualTagFilter(ulong requiredTag) + { + RequiredTag = requiredTag; + } + /// + public bool AllowJob(ulong jobTag) + { + return jobTag == RequiredTag; + } +} \ No newline at end of file diff --git a/BepuUtilities/TaskScheduling/Job.cs b/BepuUtilities/TaskScheduling/Job.cs new file mode 100644 index 000000000..0a9c11eb6 --- /dev/null +++ b/BepuUtilities/TaskScheduling/Job.cs @@ -0,0 +1,65 @@ +using System; +using System.Diagnostics; +using System.Runtime.InteropServices; +using System.Threading; +using BepuUtilities.Memory; + +namespace BepuUtilities.TaskScheduling; + +[StructLayout(LayoutKind.Explicit, Size = 292)] +internal unsafe struct Job +{ + [FieldOffset(0)] + public Buffer Tasks; + [FieldOffset(16)] + public Job* Previous; + [FieldOffset(24)] + public ulong Tag; + + [FieldOffset(160)] + public int Counter; + + + public static Job* Create(Span sourceTasks, ulong tag, BufferPool pool) + { + //Note that the job and the buffer of tasks are allocated together as one block. + //This ensures we only need to perform one + var sizeToAllocate = sizeof(Job) + sourceTasks.Length * sizeof(Task); + pool.Take(sizeToAllocate, out var rawBuffer); + Job* job = (Job*)rawBuffer.Memory; + job->Tasks = new Buffer(rawBuffer.Memory + sizeof(Job), sourceTasks.Length, rawBuffer.Id); + job->Tag = tag; + sourceTasks.CopyTo(job->Tasks); + job->Counter = sourceTasks.Length; + job->Previous = null; + return job; + } + + + /// + /// Attempts to pop a task from the job. + /// + /// Task popped from the job, if any. + /// True if a task was available to pop, false otherwise. + internal bool TryPop(out Task task) + { + + var newCount = Interlocked.Decrement(ref Counter); + if (newCount >= 0) + { + task = Tasks[newCount]; + Debug.Assert(task.Function != null); + return true; + } + task = default; + return false; + } + + public void Dispose(BufferPool pool) + { + //The instance is allocated from the same memory as the tasks buffer, so disposing it returns the Job memory too. + var id = Tasks.Id; + this = default; + pool.ReturnUnsafely(id); + } +} diff --git a/BepuUtilities/TaskScheduling/PopTaskResult.cs b/BepuUtilities/TaskScheduling/PopTaskResult.cs new file mode 100644 index 000000000..a85bd081d --- /dev/null +++ b/BepuUtilities/TaskScheduling/PopTaskResult.cs @@ -0,0 +1,20 @@ +namespace BepuUtilities.TaskScheduling; + +/// +/// Describes the result status of a pop attempt. +/// +public enum PopTaskResult +{ + /// + /// A task was successfully popped. + /// + Success, + /// + /// The stack was empty, but may have more tasks in the future. + /// + Empty, + /// + /// The stack has been terminated and all threads seeking work should stop. + /// + Stop +} diff --git a/BepuUtilities/TaskScheduling/Task.cs b/BepuUtilities/TaskScheduling/Task.cs new file mode 100644 index 000000000..224b34ab7 --- /dev/null +++ b/BepuUtilities/TaskScheduling/Task.cs @@ -0,0 +1,60 @@ +using System.Diagnostics; + +namespace BepuUtilities.TaskScheduling; + +/// +/// Description of a task to be submitted to a . +/// +public unsafe struct Task +{ + /// + /// Function to be executed by the task. Takes as arguments the , pointer, and executing worker index. + /// + public delegate* Function; + /// + /// Context to be passed into the . + /// + public void* Context; + /// + /// Continuation to be notified after this task completes, if any. + /// + public ContinuationHandle Continuation; + /// + /// User-provided identifier of this task. + /// + public long Id; + + /// + /// Creates a new task. + /// + /// Function to be executed by the task. Takes as arguments the , pointer, executing worker index, and executing . + /// Context pointer to pass to the . + /// Id of this task to be passed into the . + /// Continuation to notify after the completion of this task, if any. + public Task(delegate* function, void* context = null, long taskId = 0, ContinuationHandle continuation = default) + { + Function = function; + Context = context; + Continuation = continuation; + Id = taskId; + } + + /// + /// Creates a task from a function. + /// + /// Function to turn into a task. + public static implicit operator Task(delegate* function) => new(function); + + /// + /// Runs the task and, if necessary, notifies the associated continuation of its completion. + /// + /// Worker index to pass to the function. + /// Dispatcher running this task. + public void Run(int workerIndex, IThreadDispatcher dispatcher) + { + Debug.Assert(!Continuation.Completed && Function != null); + Function(Id, Context, workerIndex, dispatcher); + if (Continuation.Initialized) + Continuation.NotifyTaskCompleted(workerIndex, dispatcher); + } +} diff --git a/BepuUtilities/TaskScheduling/TaskContinuation.cs b/BepuUtilities/TaskScheduling/TaskContinuation.cs new file mode 100644 index 000000000..6174cc115 --- /dev/null +++ b/BepuUtilities/TaskScheduling/TaskContinuation.cs @@ -0,0 +1,16 @@ +namespace BepuUtilities.TaskScheduling; + +/// +/// Stores data relevant to tracking task completion and reporting completion for a continuation. +/// +public unsafe struct TaskContinuation +{ + /// + /// Task to run upon completion of the associated task. + /// + public Task OnCompleted; + /// + /// Number of tasks not yet reported as complete in the continuation. + /// + public int RemainingTaskCounter; +} diff --git a/BepuUtilities/LinkedTaskStack.cs b/BepuUtilities/TaskScheduling/TaskStack.cs similarity index 58% rename from BepuUtilities/LinkedTaskStack.cs rename to BepuUtilities/TaskScheduling/TaskStack.cs index 5cd1dd378..dea03dfd5 100644 --- a/BepuUtilities/LinkedTaskStack.cs +++ b/BepuUtilities/TaskScheduling/TaskStack.cs @@ -1,504 +1,18 @@ using System; using System.ComponentModel; using System.Diagnostics; -using System.Diagnostics.CodeAnalysis; using System.Numerics; using System.Runtime.CompilerServices; using System.Runtime.InteropServices; using System.Threading; -using BepuUtilities.Collections; using BepuUtilities.Memory; -namespace BepuUtilities.TestLinkedTaskStack; - -/// -/// Description of a task to be submitted to a . -/// -public unsafe struct Task -{ - /// - /// Function to be executed by the task. Takes as arguments the , pointer, and executing worker index. - /// - public delegate* Function; - /// - /// Context to be passed into the . - /// - public void* Context; - /// - /// Continuation to be notified after this task completes, if any. - /// - public ContinuationHandle Continuation; - /// - /// User-provided identifier of this task. - /// - public long Id; - - /// - /// Creates a new task. - /// - /// Function to be executed by the task. Takes as arguments the , pointer, executing worker index, and executing . - /// Context pointer to pass to the . - /// Id of this task to be passed into the . - /// Continuation to notify after the completion of this task, if any. - public Task(delegate* function, void* context = null, long taskId = 0, ContinuationHandle continuation = default) - { - Function = function; - Context = context; - Continuation = continuation; - Id = taskId; - } - - /// - /// Creates a task from a function. - /// - /// Function to turn into a task. - public static implicit operator Task(delegate* function) => new(function); - - /// - /// Runs the task and, if necessary, notifies the associated continuation of its completion. - /// - /// Worker index to pass to the function. - /// Dispatcher running this task. - public void Run(int workerIndex, IThreadDispatcher dispatcher) - { - Debug.Assert(!Continuation.Completed && (Function != null)); - Function(Id, Context, workerIndex, dispatcher); - if (Continuation.Initialized) - Continuation.NotifyTaskCompleted(workerIndex, dispatcher); - } -} - -/// -/// Describes the result status of a pop attempt. -/// -public enum PopTaskResult -{ - /// - /// A task was successfully popped. - /// - Success, - /// - /// The stack was empty, but may have more tasks in the future. - /// - Empty, - /// - /// The stack has been terminated and all threads seeking work should stop. - /// - Stop -} - -/// -/// Refers to a continuation within a . -/// -public unsafe struct ContinuationHandle : IEquatable -{ - //This is a bit odd. We're presenting this pointer as a handle, even though it's not. - //Hiding the implementation detail makes it a little easier to change later if we need to. - TaskContinuation* continuation; - - internal ContinuationHandle(TaskContinuation* continuation) - { - this.continuation = continuation; - } - - /// - /// Gets whether the tasks associated with this continuation have completed. If the continuation has not been initialized, this will always return false. - /// - public bool Completed - { - get - { - return Initialized && continuation->RemainingTaskCounter <= 0; - } - } - - /// - /// Retrieves a pointer to the continuation data for . - /// - /// Pointer to the continuation backing the given handle. - /// This should not be used if the continuation handle is not known to be valid. The data pointed to by the data could become invalidated if the continuation completes. - public TaskContinuation* Continuation => continuation; - - /// - /// Gets a null continuation handle. - /// - public static ContinuationHandle Null => default; - - /// - /// Gets whether this handle ever represented an allocated handle. This does not guarantee that the continuation's associated tasks are active in the that it was allocated from. - /// - public bool Initialized => continuation != null; - - /// - /// Notifies the continuation that one task was completed. - /// - /// Worker index to pass to the continuation's delegate, if any. - /// Dispatcher to pass to the continuation's delegate, if any. - public void NotifyTaskCompleted(int workerIndex, IThreadDispatcher dispatcher) - { - var continuation = Continuation; - Debug.Assert(!Completed); - var counter = Interlocked.Decrement(ref continuation->RemainingTaskCounter); - Debug.Assert(counter >= 0, "The counter should not go negative. Was notify called too many times?"); - if (counter == 0) - { - //This entire job has completed. - if (continuation->OnCompleted.Function != null) - { - continuation->OnCompleted.Function(continuation->OnCompleted.Id, continuation->OnCompleted.Context, workerIndex, dispatcher); - } - } - } - - public bool Equals(ContinuationHandle other) => other.continuation == continuation; - - public override bool Equals([NotNullWhen(true)] object obj) => obj is ContinuationHandle handle && Equals(handle); - - public override int GetHashCode() => (int)continuation; - - public static bool operator ==(ContinuationHandle left, ContinuationHandle right) => left.Equals(right); - - public static bool operator !=(ContinuationHandle left, ContinuationHandle right) => !(left == right); -} - - - -/// -/// Determines which jobs are allowed to serve a request. -/// -public interface IJobFilter -{ - /// - /// Determines whether a job with the given tag should be allowed to serve a request. - /// - /// Tag of the candidate job. - /// True if the job should be allowed to serve a request, false otherwise. - bool AllowJob(ulong jobTag); -} - -/// -/// A filter that will allow pops from any jobs. -/// -public struct AllowAllJobs : IJobFilter -{ - /// - public readonly bool AllowJob(ulong jobTag) - { - return true; - } -} - -/// -/// A job filter that wraps a managed delegate. -/// -public struct DelegateJobFilter : IJobFilter -{ - /// - /// Delegate to use as the filter. - /// - public Func Filter; - /// - /// Creates a job filter that wraps a delegate. - /// - /// Delegate to use as the filter. - public DelegateJobFilter(Func filter) - { - Filter = filter; - } - /// - public readonly bool AllowJob(ulong jobTag) - { - return Filter(jobTag); - } -} - -/// -/// A job filter that wraps a function pointer. -/// -public unsafe struct FunctionPointerJobFilter : IJobFilter -{ - /// - /// Delegate to use as the filter. - /// - public delegate* Filter; - /// - /// Creates a job filter that wraps a delegate. - /// - /// Delegate to use as the filter. - public FunctionPointerJobFilter(delegate* filter) - { - Filter = filter; - } - /// - public readonly bool AllowJob(ulong jobTag) - { - return Filter(jobTag); - } -} -/// -/// A job filter that requires the job tag to meet or exceed a threshold value. -/// -public struct MinimumTagFilter : IJobFilter -{ - /// - /// Value that a job must match or exceed to be allowed. - /// - public ulong MinimumTagValue; - /// - /// Creates a job filter that requires the job tag to meet or exceed a threshold value. - /// - /// Value that a job must match or exceed to be allowed. - public MinimumTagFilter(ulong minimumTagValue) - { - MinimumTagValue = minimumTagValue; - } - /// - public bool AllowJob(ulong jobTag) - { - return jobTag >= MinimumTagValue; - } -} - -/// -/// A job filter that requires the job tag to match a specific value. -/// -public struct EqualTagFilter : IJobFilter -{ - /// - /// Tag value required to allow a job. - /// - public ulong RequiredTag; - - /// - /// Creates a job filter that requires the job tag to match a specific value. - /// - public EqualTagFilter(ulong requiredTag) - { - RequiredTag = requiredTag; - } - /// - public bool AllowJob(ulong jobTag) - { - return jobTag == RequiredTag; - } -} - - -[StructLayout(LayoutKind.Explicit, Size = 292)] -internal unsafe struct Job -{ - [FieldOffset(0)] - public Buffer Tasks; - [FieldOffset(16)] - public Job* Previous; - [FieldOffset(24)] - public ulong Tag; - - [FieldOffset(160)] - public int Counter; - - - public static Job* Create(Span sourceTasks, ulong tag, BufferPool pool) - { - //Note that the job and the buffer of tasks are allocated together as one block. - //This ensures we only need to perform one - var sizeToAllocate = sizeof(Job) + sourceTasks.Length * sizeof(Task); - pool.Take(sizeToAllocate, out var rawBuffer); - Job* job = (Job*)rawBuffer.Memory; - job->Tasks = new Buffer(rawBuffer.Memory + sizeof(Job), sourceTasks.Length, rawBuffer.Id); - job->Tag = tag; - sourceTasks.CopyTo(job->Tasks); - job->Counter = sourceTasks.Length; - job->Previous = null; - return job; - } - - - /// - /// Attempts to pop a task from the job. - /// - /// Task popped from the job, if any. - /// True if a task was available to pop, false otherwise. - internal bool TryPop(out Task task) - { - - var newCount = Interlocked.Decrement(ref Counter); - if (newCount >= 0) - { - task = Tasks[newCount]; - Debug.Assert(task.Function != null); - return true; - } - task = default; - return false; - } - - public void Dispose(BufferPool pool) - { - //The instance is allocated from the same memory as the tasks buffer, so disposing it returns the Job memory too. - var id = Tasks.Id; - this = default; - pool.ReturnUnsafely(id); - } -} - -/// -/// Stores a block of task continuations that maintains a pointer to previous blocks. -/// -internal unsafe struct ContinuationBlock -{ - public ContinuationBlock* Previous; - - public int Count; - public Buffer Continuations; - - public static ContinuationBlock* Create(int continuationCapacity, BufferPool pool) - { - pool.Take(sizeof(TaskContinuation) * continuationCapacity + sizeof(ContinuationBlock), out var rawBuffer); - ContinuationBlock* block = (ContinuationBlock*)rawBuffer.Memory; - block->Continuations = new Buffer(rawBuffer.Memory + sizeof(ContinuationBlock), continuationCapacity, rawBuffer.Id); - block->Count = 0; - block->Previous = null; - return block; - } - - public bool TryAllocateContinuation(out TaskContinuation* continuation) - { - if (Count < Continuations.length) - { - continuation = Continuations.Memory + (Count++); - return true; - } - continuation = null; - return false; - } - - public void Dispose(BufferPool pool) - { - var id = Continuations.Id; - pool.ReturnUnsafely(id); - if (Previous != null) - Previous->Dispose(pool); - this = default; - } -} - - -internal unsafe struct Worker -{ - //The worker needs to track allocations made over the course of its lifetime so they can be disposed later. - public QuickList AllocatedJobs; - - public ContinuationBlock* ContinuationHead; - public int WorkerIndex; - - [Conditional("DEBUG")] - public void ValidateTasks() - { - for (int i = 0; i < AllocatedJobs.Count; ++i) - { - var job = (Job*)AllocatedJobs[i]; - for (int j = 0; j < job->Tasks.length; ++j) - { - Debug.Assert(job->Tasks[j].Function != null); - } - } - } - - - public Worker(int workerIndex, IThreadDispatcher dispatcher, int initialJobCapacity = 128, int continuationBlockCapacity = 128) - { - var threadPool = dispatcher.WorkerPools[workerIndex]; - WorkerIndex = workerIndex; - AllocatedJobs = new QuickList(initialJobCapacity, threadPool); - ContinuationHead = ContinuationBlock.Create(continuationBlockCapacity, threadPool); - } - - public void Dispose(BufferPool threadPool) - { - for (int i = 0; i < AllocatedJobs.Count; ++i) - { - ((Job*)AllocatedJobs[i])->Dispose(threadPool); - } - AllocatedJobs.Dispose(threadPool); - ContinuationHead->Dispose(threadPool); - } - - internal void Reset(BufferPool threadPool) - { - for (int i = 0; i < AllocatedJobs.Count; ++i) - { - ((Job*)AllocatedJobs[i])->Dispose(threadPool); - } - AllocatedJobs.Count = 0; - var capacity = ContinuationHead->Continuations.length; - ContinuationHead->Dispose(threadPool); - ContinuationHead = ContinuationBlock.Create(capacity, threadPool); - } - - /// - /// Pushes a set of tasks onto the stack. - /// - /// Tasks composing the job. - /// User tag associated with the job. - /// Dispatcher used to pull thread allocations if necessary. - /// If the worker associated with this stack might be active, this function can only be called by the worker. - internal Job* AllocateJob(Span tasks, ulong tag, IThreadDispatcher dispatcher) - { - Debug.Assert(tasks.Length > 0, "Probably shouldn't be trying to push zero tasks."); - var threadPool = dispatcher.WorkerPools[WorkerIndex]; - //Note that we allocate jobs on the heap directly; it's safe to resize the AllocatedJobs list because it's just storing pointers. - var job = Job.Create(tasks, tag, threadPool); - AllocatedJobs.Allocate(threadPool) = (nuint)job; - return job; - } - - - /// - /// Allocates a continuation for a set of tasks. - /// - /// Number of tasks associated with the continuation. - /// Dispatcher from which to pull a buffer pool if needed for resizing. - /// Function to execute upon completing all associated tasks, if any. Any task with a null will not be executed. - /// Handle of the continuation. - public ContinuationHandle AllocateContinuation(int taskCount, IThreadDispatcher dispatcher, Task onCompleted = default) - { - if (!ContinuationHead->TryAllocateContinuation(out TaskContinuation* continuation)) - { - //Couldn't allocate; need to allocate a new block. - //(The reason for the linked list style allocation is that resizing a buffer- and returning the old buffer- opens up a potential race condition.) - var newBlock = ContinuationBlock.Create(ContinuationHead->Continuations.length, dispatcher.WorkerPools[WorkerIndex]); - newBlock->Previous = ContinuationHead; - ContinuationHead = newBlock; - var allocated = ContinuationHead->TryAllocateContinuation(out continuation); - Debug.Assert(allocated, "Just created that block! Is the capacity wrong?"); - } - continuation->OnCompleted = onCompleted; - continuation->RemainingTaskCounter = taskCount; - return new ContinuationHandle(continuation); - } -} - -/// -/// Stores data relevant to tracking task completion and reporting completion for a continuation. -/// -public unsafe struct TaskContinuation -{ - /// - /// Task to run upon completion of the associated task. - /// - public Task OnCompleted; - /// - /// Number of tasks not yet reported as complete in the continuation. - /// - public int RemainingTaskCounter; -} - +namespace BepuUtilities.TaskScheduling; /// /// Manages a linked stack of tasks. /// -public unsafe struct LinkedTaskStack +public unsafe struct TaskStack { Buffer workers; @@ -524,7 +38,7 @@ struct StopPad /// Number of workers to allocate space for. /// Initial number of jobs (groups of tasks submitted together) to allocate space for in each worker. /// Number of slots to allocate in each block of continuations in each worker. - public LinkedTaskStack(BufferPool pool, IThreadDispatcher dispatcher, int workerCount, int initialWorkerJobCapacity = 128, int continuationBlockCapacity = 128) + public TaskStack(BufferPool pool, IThreadDispatcher dispatcher, int workerCount, int initialWorkerJobCapacity = 128, int continuationBlockCapacity = 128) { pool.Take(workerCount, out workers); for (int i = 0; i < workerCount; ++i) @@ -549,7 +63,7 @@ public void Reset(IThreadDispatcher dispatcher) } /// - /// Returns unmanaged resources held by the to a pool. + /// Returns unmanaged resources held by the to a pool. /// /// Buffer pool to return resources to. /// Dispatcher whose thread pools should be used to return any thread allocated resources. @@ -570,7 +84,7 @@ public int ApproximateTaskCount get { int sum = 0; - var job = (Job*)this.head; + var job = (Job*)head; while (true) { if (job == null) diff --git a/BepuUtilities/TaskScheduling/Worker.cs b/BepuUtilities/TaskScheduling/Worker.cs new file mode 100644 index 000000000..39145b99c --- /dev/null +++ b/BepuUtilities/TaskScheduling/Worker.cs @@ -0,0 +1,101 @@ +using System; +using System.Diagnostics; +using BepuUtilities.Collections; +using BepuUtilities.Memory; + +namespace BepuUtilities.TaskScheduling; + +internal unsafe struct Worker +{ + //The worker needs to track allocations made over the course of its lifetime so they can be disposed later. + public QuickList AllocatedJobs; + + public ContinuationBlock* ContinuationHead; + public int WorkerIndex; + + [Conditional("DEBUG")] + public void ValidateTasks() + { + for (int i = 0; i < AllocatedJobs.Count; ++i) + { + var job = (Job*)AllocatedJobs[i]; + for (int j = 0; j < job->Tasks.length; ++j) + { + Debug.Assert(job->Tasks[j].Function != null); + } + } + } + + + public Worker(int workerIndex, IThreadDispatcher dispatcher, int initialJobCapacity = 128, int continuationBlockCapacity = 128) + { + var threadPool = dispatcher.WorkerPools[workerIndex]; + WorkerIndex = workerIndex; + AllocatedJobs = new QuickList(initialJobCapacity, threadPool); + ContinuationHead = ContinuationBlock.Create(continuationBlockCapacity, threadPool); + } + + public void Dispose(BufferPool threadPool) + { + for (int i = 0; i < AllocatedJobs.Count; ++i) + { + ((Job*)AllocatedJobs[i])->Dispose(threadPool); + } + AllocatedJobs.Dispose(threadPool); + ContinuationHead->Dispose(threadPool); + } + + internal void Reset(BufferPool threadPool) + { + for (int i = 0; i < AllocatedJobs.Count; ++i) + { + ((Job*)AllocatedJobs[i])->Dispose(threadPool); + } + AllocatedJobs.Count = 0; + var capacity = ContinuationHead->Continuations.length; + ContinuationHead->Dispose(threadPool); + ContinuationHead = ContinuationBlock.Create(capacity, threadPool); + } + + /// + /// Pushes a set of tasks onto the stack. + /// + /// Tasks composing the job. + /// User tag associated with the job. + /// Dispatcher used to pull thread allocations if necessary. + /// If the worker associated with this stack might be active, this function can only be called by the worker. + internal Job* AllocateJob(Span tasks, ulong tag, IThreadDispatcher dispatcher) + { + Debug.Assert(tasks.Length > 0, "Probably shouldn't be trying to push zero tasks."); + var threadPool = dispatcher.WorkerPools[WorkerIndex]; + //Note that we allocate jobs on the heap directly; it's safe to resize the AllocatedJobs list because it's just storing pointers. + var job = Job.Create(tasks, tag, threadPool); + AllocatedJobs.Allocate(threadPool) = (nuint)job; + return job; + } + + + /// + /// Allocates a continuation for a set of tasks. + /// + /// Number of tasks associated with the continuation. + /// Dispatcher from which to pull a buffer pool if needed for resizing. + /// Function to execute upon completing all associated tasks, if any. Any task with a null will not be executed. + /// Handle of the continuation. + public ContinuationHandle AllocateContinuation(int taskCount, IThreadDispatcher dispatcher, Task onCompleted = default) + { + if (!ContinuationHead->TryAllocateContinuation(out TaskContinuation* continuation)) + { + //Couldn't allocate; need to allocate a new block. + //(The reason for the linked list style allocation is that resizing a buffer- and returning the old buffer- opens up a potential race condition.) + var newBlock = ContinuationBlock.Create(ContinuationHead->Continuations.length, dispatcher.WorkerPools[WorkerIndex]); + newBlock->Previous = ContinuationHead; + ContinuationHead = newBlock; + var allocated = ContinuationHead->TryAllocateContinuation(out continuation); + Debug.Assert(allocated, "Just created that block! Is the capacity wrong?"); + } + continuation->OnCompleted = onCompleted; + continuation->RemainingTaskCounter = taskCount; + return new ContinuationHandle(continuation); + } +} diff --git a/Demos/SpecializedTests/TaskQueueTestDemo.cs b/Demos/SpecializedTests/TaskQueueTestDemo.cs index ff96f4758..5deef8a76 100644 --- a/Demos/SpecializedTests/TaskQueueTestDemo.cs +++ b/Demos/SpecializedTests/TaskQueueTestDemo.cs @@ -7,10 +7,8 @@ using BepuPhysics; using BepuPhysics.Constraints; using System.Threading; -using System.Runtime.CompilerServices; using System.Runtime.InteropServices; -using System.Reflection.Metadata; -using BepuUtilities.TestLinkedTaskStack; +using BepuUtilities.TaskScheduling; namespace Demos.SpecializedTests; @@ -68,7 +66,7 @@ static void Test(long taskId, void* context, int workerIndex, IThreadDispatch var context1 = new DynamicContext1 { Context = typedContext }; var context2 = new DynamicContext2 { Context = typedContext }; - var stack = (LinkedTaskStack*)typedContext->TaskPile; + var stack = (TaskStack*)typedContext->TaskPile; stack->For(&DynamicallyEnqueuedTest1, &context1, 0, subtaskCount, workerIndex, dispatcher); stack->For(&DynamicallyEnqueuedTest2, &context2, 0, subtaskCount, workerIndex, dispatcher); } @@ -99,8 +97,8 @@ static void DispatcherBody(int workerIndex, IThreadDispatcher dispatcher) whe { //if (workerIndex > 1) // return; - var taskStack = (LinkedTaskStack*)dispatcher.UnmanagedContext; - while (taskStack->TryPopAndRun(workerIndex, dispatcher) != BepuUtilities.TestLinkedTaskStack.PopTaskResult.Stop) ; + var taskStack = (TaskStack*)dispatcher.UnmanagedContext; + while (taskStack->TryPopAndRun(workerIndex, dispatcher) != PopTaskResult.Stop) ; } @@ -113,7 +111,7 @@ struct Context static void IssueStop(long id, void* context, int workerIndex, IThreadDispatcher dispatcher) where T : unmanaged { var typedContext = (Context*)context; - ((LinkedTaskStack*)typedContext->TaskPile)->RequestStop(); + ((TaskStack*)typedContext->TaskPile)->RequestStop(); } @@ -146,19 +144,19 @@ public override void Initialize(ContentArchive content, Camera camera) for (int i = 0; i < 10; ++i) { - var linkedTaskStack = new LinkedTaskStack(BufferPool, ThreadDispatcher, ThreadDispatcher.ThreadCount); + var linkedTaskStack = new TaskStack(BufferPool, ThreadDispatcher, ThreadDispatcher.ThreadCount); var linkedTaskStackPointer = &linkedTaskStack; Test(() => { var context = new Context { TaskPile = linkedTaskStackPointer }; - var continuation = linkedTaskStackPointer->AllocateContinuation(iterationCount * tasksPerIteration, 0, ThreadDispatcher, new BepuUtilities.TestLinkedTaskStack.Task(&IssueStop, &context)); + var continuation = linkedTaskStackPointer->AllocateContinuation(iterationCount * tasksPerIteration, 0, ThreadDispatcher, new Task(&IssueStop, &context)); for (int i = 0; i < iterationCount; ++i) { - linkedTaskStackPointer->PushForUnsafely(&Test, &context, i * tasksPerIteration, tasksPerIteration, 0, ThreadDispatcher, continuation: continuation); + linkedTaskStackPointer->PushForUnsafely(&Test, &context, i * tasksPerIteration, tasksPerIteration, 0, ThreadDispatcher, continuation: continuation); } //taskQueuePointer->TryEnqueueStopUnsafely(); //taskQueuePointer->EnqueueTasks() - ThreadDispatcher.DispatchWorkers(&DispatcherBody, unmanagedContext: linkedTaskStackPointer); + ThreadDispatcher.DispatchWorkers(&DispatcherBody, unmanagedContext: linkedTaskStackPointer); return context.Sum; }, "MT Linked Stack", () => linkedTaskStackPointer->Reset(ThreadDispatcher)); linkedTaskStack.Dispose(BufferPool, ThreadDispatcher); From f6e16fd3e2a8dbca59cbe8952aa390d4ec3459d4 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 8 Jun 2023 17:53:04 -0500 Subject: [PATCH 728/947] Fixed #231. --- BepuPhysics/CollisionDetection/INarrowPhaseCallbacks.cs | 2 +- .../CollisionDetection/NarrowPhaseCCDContinuations.cs | 9 ++++++++- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/BepuPhysics/CollisionDetection/INarrowPhaseCallbacks.cs b/BepuPhysics/CollisionDetection/INarrowPhaseCallbacks.cs index 794afe84d..54d843c25 100644 --- a/BepuPhysics/CollisionDetection/INarrowPhaseCallbacks.cs +++ b/BepuPhysics/CollisionDetection/INarrowPhaseCallbacks.cs @@ -99,7 +99,7 @@ public unsafe interface INarrowPhaseCallbacks /// Index of the child of collidable A in the pair. If collidable A is not compound, then this is always 0. /// Index of the child of collidable B in the pair. If collidable B is not compound, then this is always 0. /// Set of contacts detected between the collidables. - /// True if this manifold should be considered for constraint generation, false otherwise. + /// True if this manifold should be considered for the parent pair's contact manifold generation, false otherwise. bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold); /// diff --git a/BepuPhysics/CollisionDetection/NarrowPhaseCCDContinuations.cs b/BepuPhysics/CollisionDetection/NarrowPhaseCCDContinuations.cs index 06bc39b65..f8bfb3a39 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhaseCCDContinuations.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhaseCCDContinuations.cs @@ -218,7 +218,14 @@ public bool AllowCollisionTesting(int pairId, int childA, int childB) [MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe void OnChildPairCompleted(int pairId, int childA, int childB, ref ConvexContactManifold manifold) { - narrowPhase.Callbacks.ConfigureContactManifold(workerIndex, GetCollidablePair(pairId), childA, childB, ref manifold); + var keepManifold = narrowPhase.Callbacks.ConfigureContactManifold(workerIndex, GetCollidablePair(pairId), childA, childB, ref manifold); + //This looks a little weird because it is. + //The other ConfigureContactManifold function (over the entire pair) has a bool return for whether a constraint should be created. + //For API consistency, we also have a bool return for the per-subpair case, but it's not about whether to create a constraint. + //It can prevent the manifold from contributing any contacts at all to the parent. + //The user *could* just set the manifold.Count = 0 themselves and it's completely equivalent, but shrug. + if (!keepManifold) + manifold.Count = 0; } internal void Dispose() From a9f43df7dbc8ad8caeb32e97e941f12b9e9badfa Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 8 Jun 2023 17:55:35 -0500 Subject: [PATCH 729/947] Beta version bump. --- BepuPhysics/BepuPhysics.csproj | 2 +- BepuUtilities/BepuUtilities.csproj | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/BepuPhysics/BepuPhysics.csproj b/BepuPhysics/BepuPhysics.csproj index 80da80de9..8416cf26f 100644 --- a/BepuPhysics/BepuPhysics.csproj +++ b/BepuPhysics/BepuPhysics.csproj @@ -1,7 +1,7 @@  net7.0 - 2.5.0-beta.12 + 2.5.0-beta.13 Bepu Entertainment LLC Ross Nordby Speedy real time physics simulation library. diff --git a/BepuUtilities/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj index 6c6d46faa..13fefd2b2 100644 --- a/BepuUtilities/BepuUtilities.csproj +++ b/BepuUtilities/BepuUtilities.csproj @@ -3,7 +3,7 @@ BepuUtilities BepuUtilities net7.0 - 2.5.0-beta.12 + 2.5.0-beta.13 Bepu Entertainment LLC Ross Nordby Supporting utilities library for BEPUphysics v2. From 1cb6552af5153962961d525187ed5e0a59eaf2f2 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 10 Jun 2023 19:58:24 -0500 Subject: [PATCH 730/947] Fiddling forth on refitless refinement with new builder. --- BepuPhysics/Trees/Tree_Refine2.cs | 223 ++++++++++++++++++++++++++++++ 1 file changed, 223 insertions(+) create mode 100644 BepuPhysics/Trees/Tree_Refine2.cs diff --git a/BepuPhysics/Trees/Tree_Refine2.cs b/BepuPhysics/Trees/Tree_Refine2.cs new file mode 100644 index 000000000..950e54ced --- /dev/null +++ b/BepuPhysics/Trees/Tree_Refine2.cs @@ -0,0 +1,223 @@ +using BepuPhysics.Constraints.Contact; +using BepuUtilities.Collections; +using BepuUtilities.Memory; +using System; +using System.Collections.Generic; +using System.Diagnostics; +using System.Linq; +using System.Runtime.CompilerServices; +using System.Text; +using System.Threading.Tasks; + +namespace BepuPhysics.Trees; + +public partial struct Tree +{ + /// + /// Incrementally refines a subset of the tree by running a binned builder over subtrees. + /// + /// Index used to distribute refinements over multiple executions. + /// Nodes will not be refit. + internal void Refine2(ref int refinementStartIndex, int subtreeRefinementCount, int subtreeRefinementSize, int rootRefinementSize, BufferPool pool) + { + //1. The root gets traversed by every traversal, so refinement always refines the region close to the root. + //2. Subtrees dangling off the root region are refined incrementally over time. + //3. It's important for refinement to be able to exchange nodes across subtrees, so the root region varies a bit between refinements. + // That's implemented by varying the depth that the root refinement traverses for its subtrees. + + //Identify all candidates for refinement. + //We'll define a candidate as any node which has a total leaf count less than or equal to the subtreeRefinementSize, and its parent does not. + var maximumRefinementCandidateCount = 2 * (LeafCount + subtreeRefinementSize - 1) / subtreeRefinementSize; + var subtreeRefinementCandidates = new QuickList(maximumRefinementCandidateCount, pool); + var stack = new QuickList(int.Max(rootRefinementSize, subtreeRefinementSize), pool); + var rootRefinementSubtrees = new QuickList(rootRefinementSize, pool); + var rootRefinementNodeIndices = new QuickList(rootRefinementSize, pool); + stack.AllocateUnsafely() = 0; + while (stack.TryPop(out var nodeToVisit)) + { + rootRefinementNodeIndices.AllocateUnsafely() = nodeToVisit; + ref var node = ref Nodes[nodeToVisit]; + if (node.B.Index < 0) + { + //Root refinement *may* encounter leaves before subtree refinement candidates. They have to be included in the root's refinement. + rootRefinementSubtrees.AllocateUnsafely() = node.B; + } + else + { + //Note that refinement candidates are not pushed onto the stack, so we don't need to check to see if the parent has more than subtreeRefinementSize leaves- + //it definitely does, or else it would have been the refinement candidate. Or it's the root. + //Note the order of stack pushes: if the tree is in DFS order, we want to next visit child A, so push it second. + if (node.B.LeafCount <= subtreeRefinementSize) + subtreeRefinementCandidates.AllocateUnsafely() = node.B.Index; + else + stack.AllocateUnsafely() = node.B.Index; + } + if (node.A.Index < 0) + { + rootRefinementSubtrees.AllocateUnsafely() = node.A; + } + else + { + if (node.A.LeafCount <= subtreeRefinementSize) + subtreeRefinementCandidates.AllocateUnsafely() = node.A.Index; + else + stack.AllocateUnsafely() = node.A.Index; + } + } + + //There's no guarantee we have enough actual subtrees to serve the request. + var effectiveSubtreeRefinementCount = int.Min(subtreeRefinementCount, subtreeRefinementCandidates.Count); + //Refine the desired number of subtrees contiguously, starting at the refinement tracker index and wrapping. + refinementStartIndex %= subtreeRefinementCandidates.Count; + var subtreeRefinementTargets = new QuickList(effectiveSubtreeRefinementCount, pool); + for (int i = 0; i < effectiveSubtreeRefinementCount; ++i) + { + var candidate = subtreeRefinementCandidates[refinementStartIndex]; + subtreeRefinementTargets.AllocateUnsafely() = candidate; + //Note that refinement targets are *also* subtrees for the root refinement. + //Add the NodeChild associated with the refinement target to the root refinement list. + ref var metanode = ref Metanodes[candidate]; + if (metanode.Parent >= 0) + { + ref var childOfParent = ref Unsafe.Add(ref Nodes[metanode.Parent].A, metanode.IndexInParent); + rootRefinementSubtrees.AllocateUnsafely() = childOfParent; + } + ++refinementStartIndex; + if (refinementStartIndex >= subtreeRefinementCandidates.Count) + refinementStartIndex -= subtreeRefinementCandidates.Count; + } + + //We now have the set of subtree refinement targets, plus an incomplete collection of root refinement nodes. + //In multithreaded-land, we could kick off jobs for every subtree refinement right now and continue on with the root refinement work. + //Root refinement is currently in cache, so we'll do that now. + //For every subtree refinement candidate that was *not* selected, we should continue to traverse. + var remainingRootSubtreesRequired = rootRefinementSize - rootRefinementSubtrees.Count; + var traversalRestartCount = subtreeRefinementCandidates.Count - effectiveSubtreeRefinementCount; + if (traversalRestartCount > 0) + { + //Distribute the remaining root subtrees required over the set of non-target refinement candidates. + var baseAmountPerCandidate = remainingRootSubtreesRequired / traversalRestartCount; + var remainder = remainingRootSubtreesRequired - baseAmountPerCandidate * traversalRestartCount; + //Start at the end of the selected region of refinement candidates- we updated the refinementStartIndex earlier, so it's in the right spot. + var refinementCandidateIndex = refinementStartIndex; + int largestRestartedTraversalLeafCount = 0; + var traversalRestarts = new QuickList<(int Index, int MaximumLeafCount)>(traversalRestartCount, pool); + var subtreeSurplus = 0; + for (int i = 0; i < traversalRestartCount; ++i) + { + var targetAmountForCandidate = baseAmountPerCandidate + subtreeSurplus; + if (i < remainder) ++targetAmountForCandidate; + var candidate = subtreeRefinementCandidates[refinementCandidateIndex]; + ref var candidateNode = ref Nodes[candidate]; + //The number of subtrees we can actually collect from this candidate is limited to the number of leaves it has. + //There's no guarantee the tree is perfectly balanced, so if we wanted to guarantee the number of root refinement subtrees, + //we'd have to redistribute any 'extra' we accumulate (due to a lack of local leaves) to other candidates. + //We do make a very lazy attempt at redistributing: if we can't fit the full amount, we'll carry over the rest to the next candidate. + //If we can't allocate all the budget, shrugohwell. Don't really want to iterate. + var candidateLeafCount = candidateNode.A.LeafCount + candidateNode.B.LeafCount; + var amountForCandidate = int.Min(targetAmountForCandidate, candidateLeafCount); + largestRestartedTraversalLeafCount = int.Max(largestRestartedTraversalLeafCount, amountForCandidate); + subtreeSurplus += targetAmountForCandidate - amountForCandidate; + traversalRestarts.AllocateUnsafely() = (candidate, amountForCandidate); + ++refinementCandidateIndex; + if (refinementCandidateIndex >= subtreeRefinementCandidates.Count) + refinementCandidateIndex -= subtreeRefinementCandidates.Count; + } + //Now we have all the traversal restart locations for the root refinement subtree collection: use them! + //TODO: While we created an intermediate "traversalRestarts" list for ease of generalizing to multiple threads, everything could have just + //been shoved directly into a traversal restart stack instead. + var traversalRestartStack = new QuickList<(int nodeToVisit, int maximumLeafCount)>(largestRestartedTraversalLeafCount, pool); + for (int i = 0; i < traversalRestarts.Count; ++i) + { + ref var traversalRestart = ref traversalRestarts[i]; + Debug.Assert(stack.Count == 0 && stack.Span.Length >= traversalRestart.MaximumLeafCount); + traversalRestartStack.AllocateUnsafely() = traversalRestart; + while (traversalRestartStack.TryPop(out var entry)) + { + ref var node = ref Nodes[entry.nodeToVisit]; + rootRefinementNodeIndices.AllocateUnsafely() = entry.nodeToVisit; + var maximumLeafCount = entry.maximumLeafCount; + var aLeafCount = int.Min(node.A.LeafCount, maximumLeafCount / 2); + var bLeafCount = maximumLeafCount - aLeafCount; + Debug.Assert(bLeafCount <= node.B.LeafCount, "The node visited in this traversal should never see a target leaf count that exceeds what could fit in the children."); + Debug.Assert(aLeafCount > 0 && bLeafCount > 0, "Splitting subtree budget between children should never yield zero sized subtrees."); + //B pushed first so A is popped first; some degree of consistent local DFS ordering. + if (bLeafCount > 1) + traversalRestartStack.AllocateUnsafely() = (node.B.Index, bLeafCount); + else + rootRefinementSubtrees.AllocateUnsafely() = node.B; + if (aLeafCount > 1) + traversalRestartStack.AllocateUnsafely() = (node.A.Index, aLeafCount); + else + rootRefinementSubtrees.AllocateUnsafely() = node.A; + } + } + traversalRestarts.Dispose(pool); + traversalRestartStack.Dispose(pool); + } + + //We now have the set of root refinement subtrees. Root refine! + //TODO: The nodes collected during the root refinement are not ordered, so may destroy existing cache coherency. Sorting it would help. + Debug.Assert(rootRefinementNodeIndices.Count == rootRefinementSubtrees.Count - 1); + var refinementNodesAllocation = new Buffer(int.Max(rootRefinementNodeIndices.Count, subtreeRefinementSize), pool); + var refinementMetanodesAllocation = new Buffer(refinementNodesAllocation.Length, pool); + + var rootRefinementNodes = refinementNodesAllocation.Slice(0, rootRefinementNodeIndices.Count); + var rootRefinementMetanodes = refinementMetanodesAllocation.Slice(0, rootRefinementNodeIndices.Count); + //TODO: Need to use the BinnedBuildNode path with TLeaves = LeavesHandledInPostpass. + BinnedBuild(rootRefinementSubtrees, rootRefinementNodes, rootRefinementMetanodes, default, null, pool); + for (int i = 0; i < rootRefinementNodes.Length; ++i) + { + //rootRefinementNodeIndices maps "refinement index space" to "real index space"; we can use it to update child pointers to the real locations. + var realNodeIndex = rootRefinementNodeIndices[i]; + ref var refinedNode = ref rootRefinementNodes[i]; + ref var refinedMetanode = ref rootRefinementMetanodes[i]; + //Map child indices, and update leaf references.. + if (refinedNode.A.Index >= 0) + refinedNode.A.Index = rootRefinementNodeIndices[refinedNode.A.Index]; + else + Leaves[Encode(refinedNode.A.Index)] = new Leaf(realNodeIndex, 0); + if (refinedNode.B.Index >= 0) + refinedNode.B.Index = rootRefinementNodeIndices[refinedNode.B.Index]; + else + Leaves[Encode(refinedNode.B.Index)] = new Leaf(realNodeIndex, 1); + if (refinedMetanode.Parent >= 0) + refinedMetanode.Parent = rootRefinementNodeIndices[refinedMetanode.Parent]; + Nodes[realNodeIndex] = refinedNode; + Metanodes[realNodeIndex] = refinedMetanode; + } + + + //Root refine is done; execute all the subtree refinements. + var subtreeRefinementNodeIndices = new QuickList(subtreeRefinementSize, pool); + var subtreeRefinementLeaves = new QuickList(subtreeRefinementSize, pool); + for (int i = 0; i < subtreeRefinementTargets.Count; ++i) + { + //Accumulate nodes and leaves with a prepass. + stack.AllocateUnsafely() = subtreeRefinementTargets[i]; + while (stack.TryPop(out var nodeToVisit)) + { + ref var node = ref Nodes[nodeToVisit]; + subtreeRefinementNodeIndices.AllocateUnsafely() = nodeToVisit; + if (node.B.Index >= 0) + stack.AllocateUnsafely() = node.B.Index; + else + subtreeRefinementLeaves.AllocateUnsafely() = node.B; + if (node.A.Index >= 0) + stack.AllocateUnsafely() = node.A.Index; + else + subtreeRefinementLeaves.AllocateUnsafely() = node.A; + } + + + var refinementNodes = refinementNodesAllocation.Slice(0, rootRefinementNodeIndices.Count); + var refinementMetanodes = refinementMetanodesAllocation.Slice(0, rootRefinementNodeIndices.Count); + //TODO: Binned build; mind the leaves again. + //TODO: Reify the binned build. + } + subtreeRefinementNodeIndices.Dispose(pool); + subtreeRefinementLeaves.Dispose(pool); + refinementNodesAllocation.Dispose(pool); + refinementMetanodesAllocation.Dispose(pool); + } +} From a2e96f46d34acc885693f29b40a205915227712b Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 13 Jun 2023 15:47:25 -0500 Subject: [PATCH 731/947] Reifyin'. --- BepuPhysics/Trees/Tree_Refine2.cs | 51 +++++++++++++++++-------------- 1 file changed, 28 insertions(+), 23 deletions(-) diff --git a/BepuPhysics/Trees/Tree_Refine2.cs b/BepuPhysics/Trees/Tree_Refine2.cs index 950e54ced..315278c5d 100644 --- a/BepuPhysics/Trees/Tree_Refine2.cs +++ b/BepuPhysics/Trees/Tree_Refine2.cs @@ -166,27 +166,7 @@ internal void Refine2(ref int refinementStartIndex, int subtreeRefinementCount, var rootRefinementMetanodes = refinementMetanodesAllocation.Slice(0, rootRefinementNodeIndices.Count); //TODO: Need to use the BinnedBuildNode path with TLeaves = LeavesHandledInPostpass. BinnedBuild(rootRefinementSubtrees, rootRefinementNodes, rootRefinementMetanodes, default, null, pool); - for (int i = 0; i < rootRefinementNodes.Length; ++i) - { - //rootRefinementNodeIndices maps "refinement index space" to "real index space"; we can use it to update child pointers to the real locations. - var realNodeIndex = rootRefinementNodeIndices[i]; - ref var refinedNode = ref rootRefinementNodes[i]; - ref var refinedMetanode = ref rootRefinementMetanodes[i]; - //Map child indices, and update leaf references.. - if (refinedNode.A.Index >= 0) - refinedNode.A.Index = rootRefinementNodeIndices[refinedNode.A.Index]; - else - Leaves[Encode(refinedNode.A.Index)] = new Leaf(realNodeIndex, 0); - if (refinedNode.B.Index >= 0) - refinedNode.B.Index = rootRefinementNodeIndices[refinedNode.B.Index]; - else - Leaves[Encode(refinedNode.B.Index)] = new Leaf(realNodeIndex, 1); - if (refinedMetanode.Parent >= 0) - refinedMetanode.Parent = rootRefinementNodeIndices[refinedMetanode.Parent]; - Nodes[realNodeIndex] = refinedNode; - Metanodes[realNodeIndex] = refinedMetanode; - } - + ReifyRefinement(rootRefinementNodeIndices, rootRefinementNodes, rootRefinementMetanodes); //Root refine is done; execute all the subtree refinements. var subtreeRefinementNodeIndices = new QuickList(subtreeRefinementSize, pool); @@ -212,12 +192,37 @@ internal void Refine2(ref int refinementStartIndex, int subtreeRefinementCount, var refinementNodes = refinementNodesAllocation.Slice(0, rootRefinementNodeIndices.Count); var refinementMetanodes = refinementMetanodesAllocation.Slice(0, rootRefinementNodeIndices.Count); - //TODO: Binned build; mind the leaves again. - //TODO: Reify the binned build. + //TODO: Need to use the BinnedBuildNode path with TLeaves = LeavesHandledInPostpass. + BinnedBuild(subtreeRefinementLeaves, refinementNodes, refinementMetanodes, default, null, pool); + ReifyRefinement(subtreeRefinementNodeIndices, refinementNodes, refinementMetanodes); } subtreeRefinementNodeIndices.Dispose(pool); subtreeRefinementLeaves.Dispose(pool); refinementNodesAllocation.Dispose(pool); refinementMetanodesAllocation.Dispose(pool); } + + void ReifyRefinement(QuickList refinementNodeIndices, Buffer refinementNodes, Buffer refinementMetanodes) + { + for (int i = 0; i < refinementNodeIndices.Count; ++i) + { + //refinementNodeIndices maps "refinement index space" to "real index space"; we can use it to update child pointers to the real locations. + var realNodeIndex = refinementNodeIndices[i]; + ref var refinedNode = ref refinementNodes[i]; + ref var refinedMetanode = ref refinementMetanodes[i]; + //Map child indices, and update leaf references. + if (refinedNode.A.Index >= 0) + refinedNode.A.Index = refinementNodeIndices[refinedNode.A.Index]; + else + Leaves[Encode(refinedNode.A.Index)] = new Leaf(realNodeIndex, 0); + if (refinedNode.B.Index >= 0) + refinedNode.B.Index = refinementNodeIndices[refinedNode.B.Index]; + else + Leaves[Encode(refinedNode.B.Index)] = new Leaf(realNodeIndex, 1); + if (refinedMetanode.Parent >= 0) + refinedMetanode.Parent = refinementNodeIndices[refinedMetanode.Parent]; + Nodes[realNodeIndex] = refinedNode; + Metanodes[realNodeIndex] = refinedMetanode; + } + } } From f8be05ff0f17dca76e1d94c2e77692819a678139 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 13 Jun 2023 19:00:44 -0500 Subject: [PATCH 732/947] Fixed a bad assert in binned builder and handled default-valued leaves for the update later codepaths. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 50 ++++++++++++++++++------- 1 file changed, 37 insertions(+), 13 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 6e92046c9..116539b3a 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -36,9 +36,9 @@ static void BuildNode( if (subtreeCountA == 1) { aIndex = subtrees[0].Index; - Debug.Assert(leafCountA == 1); if (typeof(TLeaves) == typeof(Buffer)) { + Debug.Assert(leafCountA == 1); Debug.Assert(aIndex < 0, "During building, any subtreeCount of 1 should imply a leaf."); //This is a leaf node, and this is a direct builder execution, so write to the leaf data. Unsafe.As>(ref leaves)[Encode(aIndex)] = new Leaf(nodeIndex, 0); @@ -52,9 +52,9 @@ static void BuildNode( if (subtreeCountB == 1) { bIndex = subtrees[^1].Index; - Debug.Assert(leafCountB == 1); if (typeof(TLeaves) == typeof(Buffer)) { + Debug.Assert(leafCountB == 1); Debug.Assert(bIndex < 0, "During building, any subtreeCount of 1 should imply a leaf."); //This is a leaf node, and this is a direct builder execution, so write to the leaf data. Unsafe.As>(ref leaves)[Encode(bIndex)] = new Leaf(nodeIndex, 1); @@ -1236,7 +1236,8 @@ static unsafe void BinnedBuildNode( /// Metanodes, like nodes, are created in a depth first ordering with respect to the input buffer. /// Metanodes are in the same order and in the same slots; they simply contain data about nodes that most traversals don't need to know about. /// Buffer holding the leaf references created by the build process. - /// The indices written by the build process are those defined in the inputs; any that is negative is encoded according to and points into the leaf buffer. + /// The indices written by the build process are those defined in the inputs; any that is negative is encoded according to and points into the leaf buffer. + /// If a default-valued (unallocated) buffer is passed in, the binned builder will ignore leaves. /// Buffer to be used for caching bin indices during execution. If subtreesPong is defined, binIndices must also be defined, and vice versa. /// Thread dispatcher used to accelerate the build process. /// Task stack used to accelerate the build process. Can be null; one will be created if not provided. @@ -1286,10 +1287,20 @@ static unsafe void BinnedBuilderInternal(Buffer subtrees, Buffer(binBoundsMemoryAllocation, allocatedByteCount); var threading = new SingleThreaded(binBoundsMemory, allocatedBinCount); - var context = new Context, SingleThreaded>( - minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold, - subtrees, default, leaves, nodes, metanodes, binIndices, threading); - BinnedBuildNode(false, 0, 0, subtreeCount, -1, -1, default, &context, 0, null); + if (leaves.Allocated) + { + var context = new Context, SingleThreaded>( + minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold, + subtrees, default, leaves, nodes, metanodes, binIndices, threading); + BinnedBuildNode(false, 0, 0, subtreeCount, -1, -1, default, &context, 0, null); + } + else + { + var context = new Context( + minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold, + subtrees, default, default, nodes, metanodes, binIndices, threading); + BinnedBuildNode(false, 0, 0, subtreeCount, -1, -1, default, &context, 0, null); + } } else { @@ -1327,12 +1338,24 @@ static unsafe void BinnedBuilderInternal(Buffer subtrees, Buffer, MultithreadBinnedBuildContext>( - minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold, - subtrees, subtreesPong, leaves, nodes, metanodes, binIndices, threading); + if (leaves.Allocated) + { + var context = new Context, MultithreadBinnedBuildContext>( + minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold, + subtrees, subtreesPong, leaves, nodes, metanodes, binIndices, threading); - taskStackPointer->PushUnsafely(new Task(&BinnedBuilderWorkerEntry>, &context), 0, dispatcher); - dispatcher.DispatchWorkers(&BinnedBuilderWorkerFunction>, unmanagedContext: taskStackPointer, maximumWorkerCount: workerCount); + taskStackPointer->PushUnsafely(new Task(&BinnedBuilderWorkerEntry>, &context), 0, dispatcher); + dispatcher.DispatchWorkers(&BinnedBuilderWorkerFunction>, unmanagedContext: taskStackPointer, maximumWorkerCount: workerCount); + } + else + { + var context = new Context( + minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold, + subtrees, subtreesPong, default, nodes, metanodes, binIndices, threading); + + taskStackPointer->PushUnsafely(new Task(&BinnedBuilderWorkerEntry, &context), 0, dispatcher); + dispatcher.DispatchWorkers(&BinnedBuilderWorkerFunction, unmanagedContext: taskStackPointer, maximumWorkerCount: workerCount); + } if (createdTaskQueueLocally) taskStack.Dispose(pool, dispatcher); @@ -1378,7 +1401,8 @@ unsafe static void BinnedBuilderWorkerFunction(int workerIndex, IThread /// Metanodes, like nodes, are created in a depth first ordering with respect to the input buffer. /// Metanodes are in the same order and in the same slots; they simply contain data about nodes that most traversals don't need to know about. /// Buffer holding the leaf references created by the build process. - /// The indices written by the build process are those defined in the inputs; any that is negative is encoded according to and points into the leaf buffer. + /// The indices written by the build process are those defined in the inputs; any that is negative is encoded according to and points into the leaf buffer. + /// If a default-valued (unallocated) buffer is passed in, the binned builder will ignore leaves. /// Dispatcher used to multithread the execution of the build. If the dispatcher is not null, pool must also not be null. /// Buffer pool used to preallocate a pingpong buffer if the number of subtrees exceeds maximumSubtreeStackAllocationCount. If null, stack allocation or a slower in-place partitioning will be used. /// A pool must be provided if a thread dispatcher is given. From 39fc5d8065656011b597fffdfc0be49a19cc9afc Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 14 Jun 2023 18:44:22 -0500 Subject: [PATCH 733/947] Tree.BuildNode actually sets leaf counts now, oops. Microsweep now also handles the case of subtrees with more than 1 leaf properly. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 116539b3a..c9e792fa8 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -33,6 +33,8 @@ static void BuildNode( ref var node = ref nodes[0]; node.A = Unsafe.As(ref a); node.B = Unsafe.As(ref b); + node.A.LeafCount = leafCountA; + node.B.LeafCount = leafCountB; if (subtreeCountA == 1) { aIndex = subtrees[0].Index; @@ -153,6 +155,7 @@ static unsafe void MicroSweepForBinnedBuilder( { ref var subtreeA = ref subtrees[0]; ref var subtreeB = ref subtrees[1]; + Debug.Assert(parentNodeIndex < 0 || Unsafe.Add(ref context->Nodes[parentNodeIndex].A, childIndexInParent).LeafCount == subtreeA.LeafCount + subtreeB.LeafCount); BuildNode(Unsafe.As(ref subtreeA), Unsafe.As(ref subtreeB), subtreeA.LeafCount, subtreeB.LeafCount, subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, 1, 1, ref leaves, out _, out _); return; @@ -255,7 +258,7 @@ static unsafe void MicroSweepForBinnedBuilder( var lastSubtreeIndex = subtreeCount - 1; BoundingBox4 accumulatedBoundingBoxB = boundingBoxes[lastSubtreeIndex]; Unsafe.SkipInit(out BoundingBox4 bestBoundsB); - int accumulatedLeafCountB = 1; + int accumulatedLeafCountB = subtrees[lastSubtreeIndex].LeafCount; int bestLeafCountB = 0; for (int splitIndexCandidate = lastSubtreeIndex; splitIndexCandidate >= 1; --splitIndexCandidate) { @@ -281,6 +284,7 @@ static unsafe void MicroSweepForBinnedBuilder( var subtreeCountB = subtreeCount - bestSplit; var bestLeafCountA = totalLeafCount - bestLeafCountB; + Debug.Assert(parentNodeIndex < 0 || Unsafe.Add(ref context->Nodes[parentNodeIndex].A, childIndexInParent).LeafCount == bestLeafCountA + bestLeafCountB); BuildNode(bestBoundsA, bestBoundsB, bestLeafCountA, bestLeafCountB, subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, subtreeCountA, subtreeCountB, ref leaves, out var aIndex, out var bIndex); if (subtreeCountA > 1) { @@ -958,6 +962,7 @@ static unsafe void BinnedBuildNode( var metanodes = context->Metanodes.Slice(nodeIndex, nodeCount); if (subtreeCount == 2) { + Debug.Assert(parentNodeIndex < 0 || Unsafe.Add(ref context->Nodes[parentNodeIndex].A, childIndexInParent).LeafCount == subtrees[0].LeafCount + subtrees[1].LeafCount); BuildNode(boundingBoxes[0], boundingBoxes[1], subtrees[0].LeafCount, subtrees[1].LeafCount, subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, 1, 1, ref context->Leaves, out _, out _); return; } @@ -1005,6 +1010,7 @@ static unsafe void BinnedBuildNode( boundsB.Max = Vector4.Max(bounds.Max, boundsB.Max); degenerateLeafCountB += subtrees[i].LeafCount; } + Debug.Assert(parentNodeIndex < 0 || Unsafe.Add(ref context->Nodes[parentNodeIndex].A, childIndexInParent).LeafCount == degenerateLeafCountA + degenerateLeafCountB); BuildNode(boundsA, boundsB, degenerateLeafCountA, degenerateLeafCountB, subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, degenerateSubtreeCountA, degenerateSubtreeCountB, ref context->Leaves, out var aIndex, out var bIndex); if (degenerateSubtreeCountA > 1) BinnedBuildNode(usePongBuffer, subtreeRegionStartIndex, aIndex, degenerateSubtreeCountA, nodeIndex, 0, boundsA, context, workerIndex, dispatcher); @@ -1188,6 +1194,7 @@ static unsafe void BinnedBuildNode( var leafCountB = bestLeafCountB; var leafCountA = totalLeafCount - leafCountB; Debug.Assert(subtreeCountA + subtreeCountB == subtreeCount); + Debug.Assert(parentNodeIndex < 0 || Unsafe.Add(ref context->Nodes[parentNodeIndex].A, childIndexInParent).LeafCount == leafCountA + leafCountB); BuildNode(bestBoundingBoxA, bestBoundingBoxB, leafCountA, leafCountB, subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, subtreeCountA, subtreeCountB, ref context->Leaves, out var nodeChildIndexA, out var nodeChildIndexB); var targetNodeTaskCount = typeof(TThreading) == typeof(SingleThreaded) ? 1 : From c39ebd2c4e5ed5936e648a6658aa71111b083ec8 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 15 Jun 2023 13:48:23 -0500 Subject: [PATCH 734/947] Single threaded Tree.Refine2 prototype using the new binned builder now works. --- BepuPhysics/Trees/Tree_Refine2.cs | 168 +++++++++++++++++++++++------- 1 file changed, 133 insertions(+), 35 deletions(-) diff --git a/BepuPhysics/Trees/Tree_Refine2.cs b/BepuPhysics/Trees/Tree_Refine2.cs index 315278c5d..4626fe02a 100644 --- a/BepuPhysics/Trees/Tree_Refine2.cs +++ b/BepuPhysics/Trees/Tree_Refine2.cs @@ -1,24 +1,31 @@ -using BepuPhysics.Constraints.Contact; -using BepuUtilities.Collections; +using BepuUtilities.Collections; using BepuUtilities.Memory; -using System; -using System.Collections.Generic; using System.Diagnostics; -using System.Linq; using System.Runtime.CompilerServices; -using System.Text; -using System.Threading.Tasks; namespace BepuPhysics.Trees; public partial struct Tree { + const int flagForRootRefinementSubtree = 1 << 30; + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void AppendRootRefinementInternalSubtree(ref QuickList rootSubtrees, in NodeChild child) + { + ref var allocatedChild = ref rootSubtrees.AllocateUnsafely(); + allocatedChild = child; + //Internal nodes used as subtrees by the root refinement are flagged. + Debug.Assert(allocatedChild.Index < flagForRootRefinementSubtree, "The use of an upper index bit as flag means the binned refiner cannot handle trees with billions of children."); + if (allocatedChild.Index >= 0) + allocatedChild.Index |= flagForRootRefinementSubtree; + + } + /// /// Incrementally refines a subset of the tree by running a binned builder over subtrees. /// /// Index used to distribute refinements over multiple executions. /// Nodes will not be refit. - internal void Refine2(ref int refinementStartIndex, int subtreeRefinementCount, int subtreeRefinementSize, int rootRefinementSize, BufferPool pool) + public void Refine2(ref int refinementStartIndex, int subtreeRefinementCount, int subtreeRefinementSize, int rootRefinementSize, BufferPool pool) { //1. The root gets traversed by every traversal, so refinement always refines the region close to the root. //2. Subtrees dangling off the root region are refined incrementally over time. @@ -73,6 +80,7 @@ internal void Refine2(ref int refinementStartIndex, int subtreeRefinementCount, for (int i = 0; i < effectiveSubtreeRefinementCount; ++i) { var candidate = subtreeRefinementCandidates[refinementStartIndex]; + Debug.Assert(Nodes[candidate].A.LeafCount + Nodes[candidate].B.LeafCount <= subtreeRefinementSize); subtreeRefinementTargets.AllocateUnsafely() = candidate; //Note that refinement targets are *also* subtrees for the root refinement. //Add the NodeChild associated with the refinement target to the root refinement list. @@ -80,7 +88,7 @@ internal void Refine2(ref int refinementStartIndex, int subtreeRefinementCount, if (metanode.Parent >= 0) { ref var childOfParent = ref Unsafe.Add(ref Nodes[metanode.Parent].A, metanode.IndexInParent); - rootRefinementSubtrees.AllocateUnsafely() = childOfParent; + AppendRootRefinementInternalSubtree(ref rootRefinementSubtrees, childOfParent); } ++refinementStartIndex; if (refinementStartIndex >= subtreeRefinementCandidates.Count) @@ -105,7 +113,7 @@ internal void Refine2(ref int refinementStartIndex, int subtreeRefinementCount, var subtreeSurplus = 0; for (int i = 0; i < traversalRestartCount; ++i) { - var targetAmountForCandidate = baseAmountPerCandidate + subtreeSurplus; + var targetAmountForCandidate = baseAmountPerCandidate; if (i < remainder) ++targetAmountForCandidate; var candidate = subtreeRefinementCandidates[refinementCandidateIndex]; ref var candidateNode = ref Nodes[candidate]; @@ -115,7 +123,7 @@ internal void Refine2(ref int refinementStartIndex, int subtreeRefinementCount, //We do make a very lazy attempt at redistributing: if we can't fit the full amount, we'll carry over the rest to the next candidate. //If we can't allocate all the budget, shrugohwell. Don't really want to iterate. var candidateLeafCount = candidateNode.A.LeafCount + candidateNode.B.LeafCount; - var amountForCandidate = int.Min(targetAmountForCandidate, candidateLeafCount); + var amountForCandidate = int.Min(targetAmountForCandidate + subtreeSurplus, candidateLeafCount); largestRestartedTraversalLeafCount = int.Max(largestRestartedTraversalLeafCount, amountForCandidate); subtreeSurplus += targetAmountForCandidate - amountForCandidate; traversalRestarts.AllocateUnsafely() = (candidate, amountForCandidate); @@ -137,24 +145,29 @@ internal void Refine2(ref int refinementStartIndex, int subtreeRefinementCount, ref var node = ref Nodes[entry.nodeToVisit]; rootRefinementNodeIndices.AllocateUnsafely() = entry.nodeToVisit; var maximumLeafCount = entry.maximumLeafCount; - var aLeafCount = int.Min(node.A.LeafCount, maximumLeafCount / 2); - var bLeafCount = maximumLeafCount - aLeafCount; + int smallerLeafCount = int.Min(node.A.LeafCount, node.B.LeafCount); + var targetLeafCountForSmaller = int.Min(smallerLeafCount, (maximumLeafCount + 1) / 2); + var targetLeafCountForLarger = maximumLeafCount - targetLeafCountForSmaller; + var aIsSmaller = node.A.LeafCount < node.B.LeafCount; + var aLeafCount = aIsSmaller ? targetLeafCountForSmaller : targetLeafCountForLarger; + var bLeafCount = aIsSmaller ? targetLeafCountForLarger : targetLeafCountForSmaller; Debug.Assert(bLeafCount <= node.B.LeafCount, "The node visited in this traversal should never see a target leaf count that exceeds what could fit in the children."); Debug.Assert(aLeafCount > 0 && bLeafCount > 0, "Splitting subtree budget between children should never yield zero sized subtrees."); //B pushed first so A is popped first; some degree of consistent local DFS ordering. if (bLeafCount > 1) traversalRestartStack.AllocateUnsafely() = (node.B.Index, bLeafCount); else - rootRefinementSubtrees.AllocateUnsafely() = node.B; + AppendRootRefinementInternalSubtree(ref rootRefinementSubtrees, node.B); if (aLeafCount > 1) traversalRestartStack.AllocateUnsafely() = (node.A.Index, aLeafCount); else - rootRefinementSubtrees.AllocateUnsafely() = node.A; + AppendRootRefinementInternalSubtree(ref rootRefinementSubtrees, node.A); } } traversalRestarts.Dispose(pool); traversalRestartStack.Dispose(pool); } + subtreeRefinementCandidates.Dispose(pool); //We now have the set of root refinement subtrees. Root refine! //TODO: The nodes collected during the root refinement are not ordered, so may destroy existing cache coherency. Sorting it would help. @@ -162,11 +175,27 @@ internal void Refine2(ref int refinementStartIndex, int subtreeRefinementCount, var refinementNodesAllocation = new Buffer(int.Max(rootRefinementNodeIndices.Count, subtreeRefinementSize), pool); var refinementMetanodesAllocation = new Buffer(refinementNodesAllocation.Length, pool); + //Validate(); var rootRefinementNodes = refinementNodesAllocation.Slice(0, rootRefinementNodeIndices.Count); var rootRefinementMetanodes = refinementMetanodesAllocation.Slice(0, rootRefinementNodeIndices.Count); - //TODO: Need to use the BinnedBuildNode path with TLeaves = LeavesHandledInPostpass. + //Passing 'default' for the leaves tells the binned builder to not worry about updating leaves. BinnedBuild(rootRefinementSubtrees, rootRefinementNodes, rootRefinementMetanodes, default, null, pool); - ReifyRefinement(rootRefinementNodeIndices, rootRefinementNodes, rootRefinementMetanodes); + ReifyRootRefinement(rootRefinementNodeIndices, rootRefinementNodes); + rootRefinementSubtrees.Dispose(pool); + rootRefinementNodeIndices.Dispose(pool); + + for (int i = 0; i < NodeCount; ++i) + { + var node = Nodes[i]; + var expectedParentLeafCount = node.A.LeafCount + node.B.LeafCount; + var metanode = Metanodes[i]; + if (metanode.Parent >= 0) + { + Debug.Assert(Unsafe.Add(ref Nodes[metanode.Parent].A, metanode.IndexInParent).LeafCount == expectedParentLeafCount); + } + } + + //Validate(); //Root refine is done; execute all the subtree refinements. var subtreeRefinementNodeIndices = new QuickList(subtreeRefinementSize, pool); @@ -174,6 +203,7 @@ internal void Refine2(ref int refinementStartIndex, int subtreeRefinementCount, for (int i = 0; i < subtreeRefinementTargets.Count; ++i) { //Accumulate nodes and leaves with a prepass. + Debug.Assert(stack.Count == 0 && subtreeRefinementNodeIndices.Count == 0); stack.AllocateUnsafely() = subtreeRefinementTargets[i]; while (stack.TryPop(out var nodeToVisit)) { @@ -189,40 +219,108 @@ internal void Refine2(ref int refinementStartIndex, int subtreeRefinementCount, subtreeRefinementLeaves.AllocateUnsafely() = node.A; } - - var refinementNodes = refinementNodesAllocation.Slice(0, rootRefinementNodeIndices.Count); - var refinementMetanodes = refinementMetanodesAllocation.Slice(0, rootRefinementNodeIndices.Count); - //TODO: Need to use the BinnedBuildNode path with TLeaves = LeavesHandledInPostpass. + var refinementNodes = refinementNodesAllocation.Slice(0, subtreeRefinementNodeIndices.Count); + var refinementMetanodes = refinementMetanodesAllocation.Slice(0, subtreeRefinementNodeIndices.Count); + //Passing 'default' for the leaves tells the binned builder to not worry about updating leaves. BinnedBuild(subtreeRefinementLeaves, refinementNodes, refinementMetanodes, default, null, pool); - ReifyRefinement(subtreeRefinementNodeIndices, refinementNodes, refinementMetanodes); + ReifyRefinement(subtreeRefinementNodeIndices, refinementNodes); + + subtreeRefinementNodeIndices.Count = 0; + subtreeRefinementLeaves.Count = 0; } + subtreeRefinementTargets.Dispose(pool); + stack.Dispose(pool); subtreeRefinementNodeIndices.Dispose(pool); subtreeRefinementLeaves.Dispose(pool); refinementNodesAllocation.Dispose(pool); refinementMetanodesAllocation.Dispose(pool); + //Validate(); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + void ReifyRootRefinementNodeChild(ref int index, ref QuickList refinementNodeIndices, int realNodeIndex, int childIndexInParent) + { + //Root refinements mark internal subtrees with a flag in the second to last index. + if (index < 0) + { + //The child is a leaf. + Leaves[Encode(index)] = new Leaf(realNodeIndex, childIndexInParent); + } + else + { + //The child is an internal node. + if ((uint)index < flagForRootRefinementSubtree) + { + //The child is an internal node that is part of the refinement; remap its index to point at the real memory location. + index = refinementNodeIndices[index]; + } + else + { + //The child is an internal node that is *not* part of the refinement: it's a subtree endpoint. + //No remapping is required, but we do need to strip off the 'this is a subtree endpoint of the refinement' flag. + index &= ~flagForRootRefinementSubtree; + } + //Just as leaves need to be updated to point at the new node state, parent pointers for internal nodes need be updated too. + //Note that this touches memory associated with nodes that weren't included in the refinement. + //This is only safe if the subtree refinement either occurs sequentially with root refinement, or the subtree refinement doesn't touch the subtree refinement root's metanode. + ref var childMetanode = ref Metanodes[index]; + childMetanode.Parent = realNodeIndex; + childMetanode.IndexInParent = childIndexInParent; + } } - void ReifyRefinement(QuickList refinementNodeIndices, Buffer refinementNodes, Buffer refinementMetanodes) + void ReifyRootRefinement(QuickList refinementNodeIndices, Buffer refinementNodes) { for (int i = 0; i < refinementNodeIndices.Count; ++i) { //refinementNodeIndices maps "refinement index space" to "real index space"; we can use it to update child pointers to the real locations. var realNodeIndex = refinementNodeIndices[i]; ref var refinedNode = ref refinementNodes[i]; - ref var refinedMetanode = ref refinementMetanodes[i]; //Map child indices, and update leaf references. - if (refinedNode.A.Index >= 0) - refinedNode.A.Index = refinementNodeIndices[refinedNode.A.Index]; - else - Leaves[Encode(refinedNode.A.Index)] = new Leaf(realNodeIndex, 0); - if (refinedNode.B.Index >= 0) - refinedNode.B.Index = refinementNodeIndices[refinedNode.B.Index]; - else - Leaves[Encode(refinedNode.B.Index)] = new Leaf(realNodeIndex, 1); - if (refinedMetanode.Parent >= 0) - refinedMetanode.Parent = refinementNodeIndices[refinedMetanode.Parent]; + ReifyRootRefinementNodeChild(ref refinedNode.A.Index, ref refinementNodeIndices, realNodeIndex, 0); + ReifyRootRefinementNodeChild(ref refinedNode.B.Index, ref refinementNodeIndices, realNodeIndex, 1); + Nodes[realNodeIndex] = refinedNode; + Debug.Assert(Metanodes[realNodeIndex].Parent < 0 || Unsafe.Add(ref Nodes[Metanodes[realNodeIndex].Parent].A, Metanodes[realNodeIndex].IndexInParent).LeafCount == refinedNode.A.LeafCount + refinedNode.B.LeafCount); + Debug.Assert(Metanodes[realNodeIndex].Parent < 0 || Unsafe.Add(ref Nodes[Metanodes[realNodeIndex].Parent].A, Metanodes[realNodeIndex].IndexInParent).Index == realNodeIndex); + } + } + + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + void ReifyRefinementNodeChild(ref int index, ref QuickList refinementNodeIndices, int realNodeIndex, int childIndexInParent) + { + //Root refinements mark internal subtrees with a flag in the second to last index. + if (index < 0) + { + //The child is a leaf. + Leaves[Encode(index)] = new Leaf(realNodeIndex, childIndexInParent); + } + else + { + //The child is an internal node that is part of the refinement; remap its index to point at the real memory location. + index = refinementNodeIndices[index]; + //Just as leaves need to be updated to point at the new node state, parent pointers for internal nodes need be updated too. + //Note that this touches memory associated with nodes that weren't included in the refinement. + //This is only safe if the subtree refinement either occurs sequentially with root refinement, or the subtree refinement doesn't touch the subtree refinement root's metanode. + ref var childMetanode = ref Metanodes[index]; + childMetanode.Parent = realNodeIndex; + childMetanode.IndexInParent = childIndexInParent; + } + } + + void ReifyRefinement(QuickList refinementNodeIndices, Buffer refinementNodes) + { + for (int i = 0; i < refinementNodeIndices.Count; ++i) + { + //refinementNodeIndices maps "refinement index space" to "real index space"; we can use it to update child pointers to the real locations. + var realNodeIndex = refinementNodeIndices[i]; + ref var refinedNode = ref refinementNodes[i]; + //Map child indices, and update leaf references. + //Root refinements mark internal subtrees with a flag in the second to last index. + ReifyRefinementNodeChild(ref refinedNode.A.Index, ref refinementNodeIndices, realNodeIndex, 0); + ReifyRefinementNodeChild(ref refinedNode.B.Index, ref refinementNodeIndices, realNodeIndex, 1); Nodes[realNodeIndex] = refinedNode; - Metanodes[realNodeIndex] = refinedMetanode; + Debug.Assert(Metanodes[realNodeIndex].Parent < 0 || Unsafe.Add(ref Nodes[Metanodes[realNodeIndex].Parent].A, Metanodes[realNodeIndex].IndexInParent).Index == realNodeIndex); } } } From e7752cc376b672cedba77acdb51558f5607818c9 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 15 Jun 2023 18:06:03 -0500 Subject: [PATCH 735/947] Got rid of locky tree cache optimizers. Added a depth-limited optimizer. --- BepuPhysics/Trees/Tree_CacheOptimizer.cs | 559 +++-------------------- BepuPhysics/Trees/Tree_Refine2.cs | 5 +- 2 files changed, 57 insertions(+), 507 deletions(-) diff --git a/BepuPhysics/Trees/Tree_CacheOptimizer.cs b/BepuPhysics/Trees/Tree_CacheOptimizer.cs index a1d83e774..27ec40964 100644 --- a/BepuPhysics/Trees/Tree_CacheOptimizer.cs +++ b/BepuPhysics/Trees/Tree_CacheOptimizer.cs @@ -65,511 +65,6 @@ unsafe void SwapNodes(int indexA, int indexB) } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - unsafe bool TryLock(ref int nodeIndex) - { - //Node index may change during the execution of this function. - int lockedIndex; - while (true) - { - lockedIndex = nodeIndex; - if (0 != Interlocked.CompareExchange(ref Metanodes[lockedIndex].RefineFlag, 1, 0)) - { - //Abort. - return false; - } - if (lockedIndex != nodeIndex) //Compare exchange inserts memory barrier. - { - //Locked the wrong node, let go. - Metanodes[lockedIndex].RefineFlag = 0; - } - else - { - // If lockedIndex == nodeIndex and we have the lock on lockedIndex, nodeIndex can't move and we're done. - return true; - } - } - } - - unsafe bool TrySwapNodeWithTargetThreadSafe(int swapperIndex, int swapperParentIndex, int swapTargetIndex) - { - Debug.Assert(Metanodes[swapperIndex].RefineFlag == 1, "The swapper should be locked."); - Debug.Assert(Metanodes[swapperParentIndex].RefineFlag == 1, "The swapper parent should be locked."); - Debug.Assert(swapTargetIndex != swapperIndex, "If the swapper is already at the swap target, this should not be called."); //safe to compare since if equal, it's locked. - //We must make sure that the node, its parent, and its children are locked. - //But watch out for parent or grandparent relationships between the nodes. Those lower the number of locks required. - - //The possible cases are as follows: - //0) The swap target is the swapper's grandparent. Don't lock the swap target's child that == swapper's parent. - //1) The swap target is the swapper's parent. Don't lock the swap target, AND don't lock the swap target's child == swapper. - //2) The swap target is the swapper's position (do nothing, you're done). - //3) The swap target is one of the swapper's children. Don't lock the swap target, AND don't lock swap target's parent. - //4) The swap target is one of the swapper's grandchildren. Don't lock the swap target's parent. - //5) The swap target is unrelated to the swapper. - - //Note that we don't have to worry about reordering/deadlocks because these are not blocking locks. If one fails, all locks immediately abort. - //This means that we won't always end up with an optimal cache layout, but it doesn't affect correctness at all. - //Eventually, the node will be revisited and it will probably get fixed. - - //Note the use of an iterating TryLock. It accepts the fact that the reference memory could be changed at any time before a lock is acquired. - //It explicitly checks to ensure that it actually grabs a lock on the correct node. - - //Don't lock swapTarget if: - //1) swapTargetIndex == swapperParentIndex, because swapperParentIndex is already locked - //2) nodes[swapTargetIndex].Parent == swapperIndex, because swapper's children are already locked - - //Note that the above comparison between a potentially unlocked nodes[swapTargetIndex].Parent and swapperIndex is safe because - //if it evaluates true, then it was actually locked. In the event that it evaluates to false, they aren't the same node- which random node it might be doesn't matter. - //Similar logic applies to the similar lock elisions below. - - bool success = false; - var needSwapTargetLock = swapTargetIndex != swapperParentIndex && Metanodes[swapTargetIndex].Parent != swapperIndex; - if (!needSwapTargetLock || TryLock(ref swapTargetIndex)) - { - ref var swapTarget = ref Metanodes[swapTargetIndex]; - - //Don't lock swapTarget->Parent if: - //1) swapTarget->Parent == swapperIndex, because swapper is already locked. - //2) nodes[swapTarget->Parent].Parent == swapperIndex, because swapper's children are already locked. - - var needSwapTargetParentLock = swapTarget.Parent != swapperIndex && Metanodes[swapTarget.Parent].Parent != swapperIndex; - if (!needSwapTargetParentLock || TryLock(ref swapTarget.Parent)) - { - - int childrenLockedCount = 2; - ref var children = ref Nodes[swapTargetIndex].A; - for (int i = 0; i < 2; ++i) - { - ref var child = ref Unsafe.Add(ref children, i); - //Don't lock children[i] if: - //1) children[i] == swapperIndex, because the swapper is already locked - //2) children[i] == swapperParentIndex, because the swapperParent is already locked - if (child.Index >= 0 && child.Index != swapperIndex && child.Index != swapperParentIndex && !TryLock(ref child.Index)) - { - //Failed to acquire lock on all children. - childrenLockedCount = i; - break; - } - } - - if (childrenLockedCount == 2) - { - //Nodes locked successfully. - SwapNodes(swapperIndex, swapTargetIndex); - success = true; - - //Unlock children of the original swap target, *which now lives in the swapperIndex*. - children = ref Nodes[swapperIndex].A; - for (int i = childrenLockedCount - 1; i >= 0; --i) - { - ref var child = ref Unsafe.Add(ref children, i); - //Again, note use of swapTargetIndex instead of swapperIndex. - if (child.Index >= 0 && child.Index != swapTargetIndex && child.Index != swapperParentIndex) //Avoid unlocking children already locked by the caller. - Metanodes[child.Index].RefineFlag = 0; - } - } - else - { - //No swap occurred. Can still use the swapTarget->ChildA pointer. - for (int i = childrenLockedCount - 1; i >= 0; --i) - { - ref var child = ref Unsafe.Add(ref children, i); - if (child.Index >= 0 && child.Index != swapperIndex && child.Index != swapperParentIndex) //Avoid unlocking children already locked by the caller. - Metanodes[child.Index].RefineFlag = 0; - } - } - - - if (needSwapTargetParentLock) - { - if (success) - { - //Note that swapTarget pointer is no longer used, since the node was swapped. - //The old swap target is now in the swapper index slot! - Metanodes[Metanodes[swapperIndex].Parent].RefineFlag = 0; - } - else - { - //No swap occurred, - Metanodes[swapTarget.Parent].RefineFlag = 0; - } - } - } - - if (needSwapTargetLock) - { - if (success) - { - //Once again, the original swapTarget now lives in swapperIndex. - Metanodes[swapperIndex].RefineFlag = 0; - } - else - { - swapTarget.RefineFlag = 0; - } - } - } - return success; - } - - - public unsafe bool TryLockSwapTargetThreadSafe(ref int swapTargetIndex, int swapperIndex, int swapperParentIndex) - { - Debug.Assert(Metanodes[swapperIndex].RefineFlag == 1, "The swapper should be locked."); - Debug.Assert(Metanodes[swapperParentIndex].RefineFlag == 1, "The swapper parent should be locked."); - Debug.Assert(swapTargetIndex != swapperIndex, "If the swapper is already at the swap target, this should not be called."); //safe to compare since if equal, it's locked. - //We must make sure that the node, its parent, and its children are locked. - //But watch out for parent or grandparent relationships between the nodes. Those lower the number of locks required. - - //The possible cases are as follows: - //0) The swap target is the swapper's grandparent. Don't lock the swap target's child that == swapper's parent. - //1) The swap target is the swapper's parent. Don't lock the swap target, AND don't lock the swap target's child == swapper. - //2) The swap target is the swapper's position (do nothing, you're done). - //3) The swap target is one of the swapper's children. Don't lock the swap target, AND don't lock swap target's parent. - //4) The swap target is one of the swapper's grandchildren. Don't lock the swap target's parent. - //5) The swap target is unrelated to the swapper. - - //Note that we don't have to worry about reordering/deadlocks because these are not blocking locks. If one fails, all locks immediately abort. - //This means that we won't always end up with an optimal cache layout, but it doesn't affect correctness at all. - //Eventually, the node will be revisited and it will probably get fixed. - - //Note the use of an iterating TryLock. It accepts the fact that the reference memory could be changed at any time before a lock is acquired. - //It explicitly checks to ensure that it actually grabs a lock on the correct node. - - //Don't lock swapTarget if: - //1) swapTargetIndex == swapperParentIndex, because swapperParentIndex is already locked - //2) nodes[swapTargetIndex].Parent == swapperIndex, because swapper's children are already locked - - //Note that the above comparison between a potentially unlocked nodes[swapTargetIndex].Parent and swapperIndex is safe because - //if it evaluates true, then it was actually locked. In the event that it evaluates to false, they aren't the same node- which random node it might be doesn't matter. - //Similar logic applies to the similar lock elisions below. - - bool success = false; - var needSwapTargetLock = swapTargetIndex != swapperParentIndex && Metanodes[swapTargetIndex].Parent != swapperIndex; - if (!needSwapTargetLock || TryLock(ref swapTargetIndex)) - { - ref var swapTarget = ref Metanodes[swapTargetIndex]; - - //Don't lock swapTarget->Parent if: - //1) swapTarget->Parent == swapperIndex, because swapper is already locked. - //2) nodes[swapTarget->Parent].Parent == swapperIndex, because swapper's children are already locked. - - var needSwapTargetParentLock = swapTarget.Parent != swapperIndex && Metanodes[swapTarget.Parent].Parent != swapperIndex; - if (!needSwapTargetParentLock || TryLock(ref swapTarget.Parent)) - { - - int childrenLockedCount = 2; - ref var children = ref Nodes[swapTargetIndex].A; - for (int i = 0; i < 2; ++i) - { - ref var child = ref Unsafe.Add(ref children, i); - //Don't lock children[i] if: - //1) children[i] == swapperIndex, because the swapper is already locked - //2) children[i] == swapperParentIndex, because the swapperParent is already locked - if (child.Index != swapperIndex && child.Index != swapperParentIndex && !TryLock(ref child.Index)) - { - //Failed to acquire lock on all children. - childrenLockedCount = i; - break; - } - } - - if (childrenLockedCount == 2) - { - //Nodes locked successfully. - success = true; - } - //TODO: should not unlock here because this is a LOCK function! - for (int i = childrenLockedCount - 1; i >= 0; --i) - { - ref var child = ref Unsafe.Add(ref children, i); - if (child.Index != swapperIndex && child.Index != swapperParentIndex) //Avoid unlocking children already locked by the caller. - Metanodes[child.Index].RefineFlag = 0; - } - - if (needSwapTargetParentLock) - Metanodes[swapTarget.Parent].RefineFlag = 0; - } - if (needSwapTargetLock) - swapTarget.RefineFlag = 0; - } - return success; - } - - /// - /// Attempts to swap two nodes. Aborts without changing memory if the swap is contested by another thread. - /// - /// Uses Node.RefineFlag as a lock-keeping mechanism. All refine flags should be cleared to 0 before a multithreaded processing stage that performs swaps. - /// First node of the swap pair. - /// Second node of the swap pair. - /// True if the nodes were swapped, false if the swap was contested. - public unsafe bool TrySwapNodesThreadSafe(ref int aIndex, ref int bIndex) - { - Debug.Assert(aIndex != bIndex, "Can't swap a node with itself."); - - //We must lock: - //a - //b - //a->Parent - //b->Parent - //a->{Children} - //b->{Children} - //But watch out for parent or grandparent relationships between the nodes. Those lower the number of locks required. - - //Note that we don't have to worry about reordering/deadlocks because these are not blocking locks. If one fails, all locks immediately abort. - //This means that we won't always end up with an optimal cache layout, but it doesn't affect correctness at all. - //Eventually, the node will be revisited and it will probably get fixed. - - //Note the use of an iterating TryLock. It accepts the fact that the reference memory could be changed at any time before a lock is acquired. - //It explicitly checks to ensure that it actually grabs a lock on the correct node. - - bool success = false; - if (TryLock(ref aIndex)) - { - ref var a = ref Metanodes[aIndex]; - if (TryLock(ref bIndex)) - { - //Now, we know that aIndex and bIndex will not change. - ref var b = ref Metanodes[bIndex]; - - var aParentAvoidedLock = a.Parent == bIndex; - if (aParentAvoidedLock || TryLock(ref a.Parent)) - { - var bParentAvoidedLock = b.Parent == aIndex; - if (bParentAvoidedLock || TryLock(ref b.Parent)) - { - - int aChildrenLockedCount = 2; - ref var aChildren = ref Nodes[aIndex].A; - for (int i = 0; i < 2; ++i) - { - ref var child = ref Unsafe.Add(ref aChildren, i); - if (child.Index != bIndex && child.Index != b.Parent && !TryLock(ref child.Index)) - { - //Failed to acquire lock on all children. - aChildrenLockedCount = i; - break; - } - } - - if (aChildrenLockedCount == 2) - { - int bChildrenLockedCount = 2; - ref var bChildren = ref Nodes[bIndex].A; - for (int i = 0; i < 2; ++i) - { - ref var child = ref Unsafe.Add(ref bChildren, i); - if (child.Index != aIndex && child.Index != a.Parent && !TryLock(ref child.Index)) - { - //Failed to acquire lock on all children. - bChildrenLockedCount = i; - break; - } - } - - if (bChildrenLockedCount == 2) - { - //ALL nodes locked successfully. - SwapNodes(aIndex, bIndex); - success = true; - } - - for (int i = bChildrenLockedCount - 1; i >= 0; --i) - { - ref var child = ref Unsafe.Add(ref bChildren, i); - if (child.Index != aIndex && child.Index != a.Parent) //Do not yet unlock a or its parent. - Metanodes[child.Index].RefineFlag = 0; - } - } - for (int i = aChildrenLockedCount - 1; i >= 0; --i) - { - ref var child = ref Unsafe.Add(ref aChildren, i); - if (child.Index != bIndex && child.Index != b.Parent) //Do not yet unlock b or its parent. - Metanodes[child.Index].RefineFlag = 0; - } - if (!bParentAvoidedLock) - Metanodes[b.Parent].RefineFlag = 0; - } - if (!aParentAvoidedLock) - Metanodes[a.Parent].RefineFlag = 0; - } - b.RefineFlag = 0; - } - a.RefineFlag = 0; - } - return success; - } - - - /// - /// Moves the children if the specified node into the correct relative position in memory. - /// Takes care to avoid contested moves in multithreaded contexts. May not successfully - /// complete all desired moves if contested. - /// - /// Node whose children should be optimized. - /// True if no other threads contested the optimization or if the node is already optimized, otherwise false. - /// Will return true even if not all nodes are optimized if the reason was a target index outside of the node list bounds. - public unsafe bool IncrementalCacheOptimizeThreadSafe(int nodeIndex) - { - Debug.Assert(LeafCount >= 2, - "Should only use cache optimization when there are at least two leaves. Every node has to have 2 children, and optimizing a 0 or 1 leaf tree is silly anyway."); - //Multithreaded cache optimization attempts to acquire a lock on every involved node. - //If any lock fails, it just abandons the entire attempt. - //That's acceptable- the incremental optimization only cares about eventual success. - - //TODO: if you know the tree in question has a ton of coherence, could attempt to compare child pointers without locks ahead of time. - //Unsafe, but acceptable as an optimization prepass. Would avoid some interlocks. Doesn't seem to help for trees undergoing any significant motion. - ref var node = ref Metanodes[nodeIndex]; - bool success = true; - - if (0 == Interlocked.CompareExchange(ref node.RefineFlag, 1, 0)) - { - ref var children = ref Nodes[nodeIndex].A; - var targetIndex = nodeIndex + 1; - - - - //Note that we pull all children up to their final positions relative to the current node index. - //This helps ensure that more nodes can converge to their final positions- if we didn't do this, - //a full top-down cache optimization could end up leaving some nodes near the bottom of the tree and without any room for their children. - //TODO: N-ary tree support. Tricky without subtree count and without fixed numbers of children per node, but it may be possible - //to stil choose something which converged. - - for (int i = 0; i < 2; ++i) - { - ref var child = ref Unsafe.Add(ref children, i); - if (targetIndex >= NodeCount) - { - //This attempted swap would reach beyond the allocated nodes. - //That means the current node is quite a bit a lower than it should be. - //Later refinement attempts should fix this, but for now, do nothing. - //Other options: - //We could aggressively swap this node upward. More complicated. - break; - } - //It is very possible that this child pointer could swap between now and the compare exchange read. - //However, a child pointer will not turn from an internal node (positive) to a leaf node (negative), and that's all that matters. - if (child.Index >= 0) - { - //Lock before comparing the children to stop the children from changing. - if (TryLock(ref child.Index)) - { - //While we checked if children[i] != targetIndex earlier as an early-out, it must be done post-lock for correctness because children[i] could have changed. - //Attempting a swap between an index and itself is invalid. - if (child.Index != targetIndex) - { - //Now lock all of this child's children. - ref var childNode = ref Nodes[child.Index]; - ref var grandchildren = ref childNode.A; - int lockedChildrenCount = 2; - for (int grandchildIndex = 0; grandchildIndex < 2; ++grandchildIndex) - { - ref var grandchild = ref Unsafe.Add(ref grandchildren, grandchildIndex); - //It is very possible that this grandchild pointer could swap between now and the compare exchange read. - //However, a child pointer will not turn from an internal node (positive) to a leaf node (negative), and that's all that matters. - if (grandchild.Index >= 0 && !TryLock(ref grandchild.Index)) - { - lockedChildrenCount = grandchildIndex; - break; - } - } - if (lockedChildrenCount == 2) - { - Debug.Assert(node.RefineFlag == 1); - if (!TrySwapNodeWithTargetThreadSafe(child.Index, nodeIndex, targetIndex)) - { - //Failed target lock. - success = false; - } - Debug.Assert(node.RefineFlag == 1); - - } - else - { - //Failed grandchild lock. - success = false; - } - - //Unlock all grandchildren. - //Note that we can't use the old grandchildren pointer. If the swap went through, it's pointing to the *target's* children. - //So update the pointer. - grandchildren = ref Nodes[child.Index].A; - for (int grandchildIndex = lockedChildrenCount - 1; grandchildIndex >= 0; --grandchildIndex) - { - ref var grandchild = ref Unsafe.Add(ref grandchildren, grandchildIndex); - if (grandchild.Index >= 0) - Metanodes[grandchild.Index].RefineFlag = 0; - } - - } - //Unlock. children[i] is either the targetIndex, if a swap went through, or it's the original child index if it didn't. - //Those are the proper targets. - Metanodes[child.Index].RefineFlag = 0; - } - else - { - //Failed child lock. - success = false; - } - //Leafcounts cannot change due to other threads. - targetIndex += child.LeafCount - 1; //Only works on 2-ary trees. - } - } - //Unlock the parent. - node.RefineFlag = 0; - } - else - { - //Failed parent lock. - success = false; - } - return success; - } - - public unsafe void IncrementalCacheOptimize(int nodeIndex) - { - if (LeafCount <= 2) - { - //Don't bother cache optimizing if there are only two leaves. There's no work to be done, and it supplies a guarantee to the rest of the optimization logic - //so that we don't have to check per-node child counts. - return; - } - - ref var node = ref Nodes[nodeIndex]; - ref var children = ref node.A; - var targetIndex = nodeIndex + 1; - - //Note that we pull all children up to their final positions relative to the current node index. - //This helps ensure that more nodes can converge to their final positions- if we didn't do this, - //a full top-down cache optimization could end up leaving some nodes near the bottom of the tree and without any room for their children. - //TODO: N-ary tree support. Tricky without subtree count and without fixed numbers of children per node, but it may be possible - //to stil choose something which converged. - - for (int i = 0; i < 2; ++i) - { - if (targetIndex >= NodeCount) - { - //This attempted swap would reach beyond the allocated nodes. - //That means the current node is quite a bit a lower than it should be. - //Later refinement attempts should fix this, but for now, do nothing. - //Other options: - //We could aggressively swap this node upward. More complicated. - break; - } - ref var child = ref Unsafe.Add(ref children, i); - if (child.Index >= 0) - { - if (child.Index != targetIndex) - { - SwapNodes(child.Index, targetIndex); - } - //break; - targetIndex += child.LeafCount - 1; - } - } - } - - unsafe void CacheOptimize(int nodeIndex, ref int nextIndex) { @@ -591,7 +86,6 @@ unsafe void CacheOptimize(int nodeIndex, ref int nextIndex) } } - /// /// Begins a cache optimization at the given node and proceeds all the way to the bottom of the tree. /// Requires that the targeted node is already at the global optimum position. @@ -609,6 +103,59 @@ public unsafe void CacheOptimize(int nodeIndex) CacheOptimize(nodeIndex, ref targetIndex); } + + private unsafe void CacheOptimizedLimitedSubtreeInternal(int sourceNodeIndex, int targetNodeIndex, int nodeOptimizationCount) + { + if (sourceNodeIndex != targetNodeIndex) + SwapNodes(targetNodeIndex, sourceNodeIndex); + --nodeOptimizationCount; + if (nodeOptimizationCount == 0) + return; + ref var node = ref Nodes[targetNodeIndex]; + var lowerNodeCount = int.Min(node.A.LeafCount, node.B.LeafCount) - 1; + var lowerTargetNodeCount = int.Min(lowerNodeCount, (nodeOptimizationCount + 1) / 2); + var higherNodeCount = nodeOptimizationCount - lowerTargetNodeCount; + var aIsSmaller = node.A.LeafCount < node.B.LeafCount; + var nodeOptimizationCountA = aIsSmaller ? lowerTargetNodeCount : higherNodeCount; + var nodeOptimizationCountB = aIsSmaller ? higherNodeCount : lowerTargetNodeCount; + if (nodeOptimizationCountA > 0) + CacheOptimizedLimitedSubtreeInternal(node.A.Index, targetNodeIndex + 1, nodeOptimizationCountA); + if (nodeOptimizationCountB > 0) + CacheOptimizedLimitedSubtreeInternal(node.B.Index, targetNodeIndex + node.A.LeafCount, nodeOptimizationCountB); + } + + /// + /// Starts a cache optimization process at the target node index and the nodeOptimizationCount closest nodes in the tree. + /// + /// Node index to start the optimization process at. + /// Number of nodes to move. + /// This optimizer will move the targeted node index to the globally optimal location if necessary. + public unsafe void CacheOptimizeLimitedSubtree(int nodeIndex, int nodeOptimizationCount) + { + if (LeafCount <= 2) + return; + + //Compute the target index for the given node. + //We want a DFS traversal order, so walk back up to the root. Count all nodes to the left of the current node. + int leftNodeCount = 0; + int chainedNodeIndex = nodeIndex; + while (true) + { + ref var metanode = ref Metanodes[chainedNodeIndex]; + var parent = metanode.Parent; + if (parent < 0) + break; + chainedNodeIndex = parent; + ++leftNodeCount; + if (metanode.IndexInParent == 1) + { + leftNodeCount += Nodes[parent].A.LeafCount - 1; + } + } + ref var originalNode = ref Nodes[nodeIndex]; + var effectiveNodeOptimizationCount = int.Min(originalNode.A.LeafCount + originalNode.B.LeafCount - 1, nodeOptimizationCount); + CacheOptimizedLimitedSubtreeInternal(nodeIndex, leftNodeCount, effectiveNodeOptimizationCount); + } } } diff --git a/BepuPhysics/Trees/Tree_Refine2.cs b/BepuPhysics/Trees/Tree_Refine2.cs index 4626fe02a..710920d36 100644 --- a/BepuPhysics/Trees/Tree_Refine2.cs +++ b/BepuPhysics/Trees/Tree_Refine2.cs @@ -1,5 +1,6 @@ using BepuUtilities.Collections; using BepuUtilities.Memory; +using System; using System.Diagnostics; using System.Runtime.CompilerServices; @@ -170,7 +171,9 @@ public void Refine2(ref int refinementStartIndex, int subtreeRefinementCount, in subtreeRefinementCandidates.Dispose(pool); //We now have the set of root refinement subtrees. Root refine! - //TODO: The nodes collected during the root refinement are not ordered, so may destroy existing cache coherency. Sorting it would help. + //The nodes collected during the root refinement are not ordered, so may destroy existing cache coherency. + //Sorting *may* help in some cases, but not enough to warrant it by default. + //((Span)rootRefinementNodeIndices).Sort(); Debug.Assert(rootRefinementNodeIndices.Count == rootRefinementSubtrees.Count - 1); var refinementNodesAllocation = new Buffer(int.Max(rootRefinementNodeIndices.Count, subtreeRefinementSize), pool); var refinementMetanodesAllocation = new Buffer(refinementNodesAllocation.Length, pool); From f04bbdb2d1affd781642c8895b52e0482845894f Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 20 Jun 2023 17:42:46 -0500 Subject: [PATCH 736/947] Some additional options that don't really matter for cache optimization. --- BepuPhysics/Trees/Tree_CacheOptimizer.cs | 289 +++++++++++++---------- 1 file changed, 163 insertions(+), 126 deletions(-) diff --git a/BepuPhysics/Trees/Tree_CacheOptimizer.cs b/BepuPhysics/Trees/Tree_CacheOptimizer.cs index 27ec40964..e9a2f8661 100644 --- a/BepuPhysics/Trees/Tree_CacheOptimizer.cs +++ b/BepuPhysics/Trees/Tree_CacheOptimizer.cs @@ -1,161 +1,198 @@ -using System; -using System.Diagnostics; +using System.Diagnostics; using System.Runtime.CompilerServices; -using System.Threading; -namespace BepuPhysics.Trees +namespace BepuPhysics.Trees; + +partial struct Tree { - partial struct Tree + unsafe void SwapNodes(int indexA, int indexB) { - unsafe void SwapNodes(int indexA, int indexB) + ref var a = ref Nodes[indexA]; + ref var b = ref Nodes[indexB]; + ref var metaA = ref Metanodes[indexA]; + ref var metaB = ref Metanodes[indexB]; + + Helpers.Swap(ref a, ref b); + Helpers.Swap(ref metaA, ref metaB); + + if (metaA.Parent == indexA) + { + //The original B's parent was A. + //That parent has moved. + metaA.Parent = indexB; + } + else if (metaB.Parent == indexB) { - ref var a = ref Nodes[indexA]; - ref var b = ref Nodes[indexB]; - ref var metaA = ref Metanodes[indexA]; - ref var metaB = ref Metanodes[indexB]; + //The original A's parent was B. + //That parent has moved. + metaB.Parent = indexA; + } + Unsafe.Add(ref Nodes[metaA.Parent].A, metaA.IndexInParent).Index = indexA; + Unsafe.Add(ref Nodes[metaB.Parent].A, metaB.IndexInParent).Index = indexB; - Helpers.Swap(ref a, ref b); - Helpers.Swap(ref metaA, ref metaB); - if (metaA.Parent == indexA) + //Update the parent pointers of the children. + ref var children = ref a.A; + for (int i = 0; i < 2; ++i) + { + ref var child = ref Unsafe.Add(ref children, i); + if (child.Index >= 0) { - //The original B's parent was A. - //That parent has moved. - metaA.Parent = indexB; + Metanodes[child.Index].Parent = indexA; } - else if (metaB.Parent == indexB) + else { - //The original A's parent was B. - //That parent has moved. - metaB.Parent = indexA; + var leafIndex = Encode(child.Index); + Leaves[leafIndex] = new Leaf(indexA, i); } - Unsafe.Add(ref Nodes[metaA.Parent].A, metaA.IndexInParent).Index = indexA; - Unsafe.Add(ref Nodes[metaB.Parent].A, metaB.IndexInParent).Index = indexB; - - - //Update the parent pointers of the children. - ref var children = ref a.A; - for (int i = 0; i < 2; ++i) + } + children = ref b.A; + for (int i = 0; i < 2; ++i) + { + ref var child = ref Unsafe.Add(ref children, i); + if (child.Index >= 0) { - ref var child = ref Unsafe.Add(ref children, i); - if (child.Index >= 0) - { - Metanodes[child.Index].Parent = indexA; - } - else - { - var leafIndex = Encode(child.Index); - Leaves[leafIndex] = new Leaf(indexA, i); - } + Metanodes[child.Index].Parent = indexB; } - children = ref b.A; - for (int i = 0; i < 2; ++i) + else { - ref var child = ref Unsafe.Add(ref children, i); - if (child.Index >= 0) - { - Metanodes[child.Index].Parent = indexB; - } - else - { - var leafIndex = Encode(child.Index); - Leaves[leafIndex] = new Leaf(indexB, i); - } + var leafIndex = Encode(child.Index); + Leaves[leafIndex] = new Leaf(indexB, i); } - } + } - unsafe void CacheOptimize(int nodeIndex, ref int nextIndex) + /// + /// Computes the index where the given node would be located if the tree were in depth first traversal order. + /// + /// Node index to find the location for in a depth first traversal order. + /// Target index of the node if the tree were in depth first traversal order. + public int ComputeCacheOptimalLocation(int nodeIndex) + { + //We want a DFS traversal order, so walk back up to the root. Count all nodes to the left of the current node. + int leftNodeCount = 0; + int chainedNodeIndex = nodeIndex; + while (true) { - ref var node = ref Nodes[nodeIndex]; - ref var children = ref node.A; - for (int i = 0; i < 2; ++i) + ref var metanode = ref Metanodes[chainedNodeIndex]; + var parent = metanode.Parent; + if (parent < 0) + break; + chainedNodeIndex = parent; + ++leftNodeCount; + if (metanode.IndexInParent == 1) { - ref var child = ref Unsafe.Add(ref children, i); - if (child.Index >= 0) - { - Debug.Assert(nextIndex >= 0 && nextIndex < NodeCount, - "Swap target should be within the node set. If it's not, the initial node was probably not in global optimum position."); - if (child.Index != nextIndex) - SwapNodes(child.Index, nextIndex); - Debug.Assert(child.Index != nextIndex); - ++nextIndex; - CacheOptimize(child.Index, ref nextIndex); - } + leftNodeCount += Nodes[parent].A.LeafCount - 1; } } + return leftNodeCount; + } - /// - /// Begins a cache optimization at the given node and proceeds all the way to the bottom of the tree. - /// Requires that the targeted node is already at the global optimum position. - /// - /// Node to begin the optimization process at. - public unsafe void CacheOptimize(int nodeIndex) + unsafe void CacheOptimize(int nodeIndex, ref int nextIndex) + { + ref var node = ref Nodes[nodeIndex]; + ref var children = ref node.A; + for (int i = 0; i < 2; ++i) { - if (LeafCount <= 2) + ref var child = ref Unsafe.Add(ref children, i); + if (child.Index >= 0) { - //Don't bother cache optimizing if there are only two leaves. There's no work to be done, and it supplies a guarantee to the rest of the optimization logic - //so that we don't have to check per-node child counts. - return; + Debug.Assert(nextIndex >= 0 && nextIndex < NodeCount, + "Swap target should be within the node set. If it's not, the initial node was probably not in global optimum position."); + if (child.Index != nextIndex) + SwapNodes(child.Index, nextIndex); + Debug.Assert(child.Index != nextIndex); + ++nextIndex; + CacheOptimize(child.Index, ref nextIndex); } - var targetIndex = nodeIndex + 1; - - CacheOptimize(nodeIndex, ref targetIndex); } + } - private unsafe void CacheOptimizedLimitedSubtreeInternal(int sourceNodeIndex, int targetNodeIndex, int nodeOptimizationCount) + /// + /// Begins a cache optimization at the given node and proceeds all the way to the bottom of the tree. + /// Requires that the targeted node is already at the global optimum position. + /// + /// Node to begin the optimization process at. + public unsafe void CacheOptimize(int nodeIndex) + { + if (LeafCount <= 2) { - if (sourceNodeIndex != targetNodeIndex) - SwapNodes(targetNodeIndex, sourceNodeIndex); - --nodeOptimizationCount; - if (nodeOptimizationCount == 0) - return; - ref var node = ref Nodes[targetNodeIndex]; - var lowerNodeCount = int.Min(node.A.LeafCount, node.B.LeafCount) - 1; - var lowerTargetNodeCount = int.Min(lowerNodeCount, (nodeOptimizationCount + 1) / 2); - var higherNodeCount = nodeOptimizationCount - lowerTargetNodeCount; - var aIsSmaller = node.A.LeafCount < node.B.LeafCount; - var nodeOptimizationCountA = aIsSmaller ? lowerTargetNodeCount : higherNodeCount; - var nodeOptimizationCountB = aIsSmaller ? higherNodeCount : lowerTargetNodeCount; - if (nodeOptimizationCountA > 0) - CacheOptimizedLimitedSubtreeInternal(node.A.Index, targetNodeIndex + 1, nodeOptimizationCountA); - if (nodeOptimizationCountB > 0) - CacheOptimizedLimitedSubtreeInternal(node.B.Index, targetNodeIndex + node.A.LeafCount, nodeOptimizationCountB); + //Don't bother cache optimizing if there are only two leaves. There's no work to be done, and it supplies a guarantee to the rest of the optimization logic + //so that we don't have to check per-node child counts. + return; } + var targetIndex = nodeIndex + 1; + + CacheOptimize(nodeIndex, ref targetIndex); + } + + private unsafe void CacheOptimizedLimitedSubtreeInternal(int sourceNodeIndex, int targetNodeIndex, int nodeOptimizationCount) + { + if (sourceNodeIndex != targetNodeIndex) + SwapNodes(targetNodeIndex, sourceNodeIndex); + --nodeOptimizationCount; + if (nodeOptimizationCount == 0) + return; + ref var node = ref Nodes[targetNodeIndex]; + var lowerNodeCount = int.Min(node.A.LeafCount, node.B.LeafCount) - 1; + var lowerTargetNodeCount = int.Min(lowerNodeCount, (nodeOptimizationCount + 1) / 2); + var higherNodeCount = nodeOptimizationCount - lowerTargetNodeCount; + var aIsSmaller = node.A.LeafCount < node.B.LeafCount; + var nodeOptimizationCountA = aIsSmaller ? lowerTargetNodeCount : higherNodeCount; + var nodeOptimizationCountB = aIsSmaller ? higherNodeCount : lowerTargetNodeCount; + if (nodeOptimizationCountA > 0) + CacheOptimizedLimitedSubtreeInternal(node.A.Index, targetNodeIndex + 1, nodeOptimizationCountA); + if (nodeOptimizationCountB > 0) + CacheOptimizedLimitedSubtreeInternal(node.B.Index, targetNodeIndex + node.A.LeafCount, nodeOptimizationCountB); + } + + /// + /// Starts a cache optimization process at the target node index and the nodeOptimizationCount closest nodes in the tree. + /// + /// Node index to start the optimization process at. + /// Number of nodes to move. + /// This optimizer will move the targeted node index to the globally optimal location if necessary. + public unsafe void CacheOptimizeLimitedSubtree(int nodeIndex, int nodeOptimizationCount) + { + if (LeafCount <= 2) + return; + var targetNodeIndex = ComputeCacheOptimalLocation(nodeIndex); + ref var originalNode = ref Nodes[nodeIndex]; + var effectiveNodeOptimizationCount = int.Min(originalNode.A.LeafCount + originalNode.B.LeafCount - 1, nodeOptimizationCount); + CacheOptimizedLimitedSubtreeInternal(nodeIndex, targetNodeIndex, effectiveNodeOptimizationCount); + } + + /// + /// Puts all nodes starting from the given node index into depth traversal order. + /// If the count is larger than the number of nodes beneath the starting node, the optimization will stop early. + /// + /// + /// Number of nodes to try to optimize. + /// Number of nodes optimized. + public unsafe int CacheOptimizeRegion(int startingNodeIndex, int targetCount) + { + if (LeafCount <= 2) + return 0; + var targetNodeIndex = ComputeCacheOptimalLocation(startingNodeIndex); + if (startingNodeIndex != targetNodeIndex) + SwapNodes(targetNodeIndex, startingNodeIndex); - /// - /// Starts a cache optimization process at the target node index and the nodeOptimizationCount closest nodes in the tree. - /// - /// Node index to start the optimization process at. - /// Number of nodes to move. - /// This optimizer will move the targeted node index to the globally optimal location if necessary. - public unsafe void CacheOptimizeLimitedSubtree(int nodeIndex, int nodeOptimizationCount) + ref var startNode = ref Nodes[targetNodeIndex]; + var nodeCount = int.Min(startNode.A.LeafCount + startNode.B.LeafCount - 1, targetCount); + for (int i = 0; i < nodeCount; ++i) { - if (LeafCount <= 2) - return; - - //Compute the target index for the given node. - //We want a DFS traversal order, so walk back up to the root. Count all nodes to the left of the current node. - int leftNodeCount = 0; - int chainedNodeIndex = nodeIndex; - while (true) - { - ref var metanode = ref Metanodes[chainedNodeIndex]; - var parent = metanode.Parent; - if (parent < 0) - break; - chainedNodeIndex = parent; - ++leftNodeCount; - if (metanode.IndexInParent == 1) - { - leftNodeCount += Nodes[parent].A.LeafCount - 1; - } - } - ref var originalNode = ref Nodes[nodeIndex]; - var effectiveNodeOptimizationCount = int.Min(originalNode.A.LeafCount + originalNode.B.LeafCount - 1, nodeOptimizationCount); - CacheOptimizedLimitedSubtreeInternal(nodeIndex, leftNodeCount, effectiveNodeOptimizationCount); + int parentIndex = targetNodeIndex + i; + ref var node = ref Nodes[parentIndex]; + //Put both children into depth first order before continuing. + var targetLocationA = parentIndex + 1; + var targetLocationB = parentIndex + node.A.LeafCount; + if (node.A.Index >= 0 && node.A.Index != targetLocationA) + SwapNodes(node.A.Index, targetLocationA); + if (node.B.Index >= 0 && node.B.Index != targetLocationB) + SwapNodes(node.B.Index, targetLocationB); } - } + return nodeCount; + } } From 946a7b0987626096ebbbaba14de7488429465e05 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 23 Jun 2023 20:39:24 -0500 Subject: [PATCH 737/947] What if Tree_Refine2.cs contained Refine3, and four separate candidate finding approaches --- BepuPhysics/Trees/Tree_Refine2.cs | 445 ++++++++++++++++-- Demos/Demos.csproj | 1 - .../SpecializedTests/TreeFiddlingTestDemo.cs | 65 ++- 3 files changed, 464 insertions(+), 47 deletions(-) diff --git a/BepuPhysics/Trees/Tree_Refine2.cs b/BepuPhysics/Trees/Tree_Refine2.cs index 710920d36..dad8892d4 100644 --- a/BepuPhysics/Trees/Tree_Refine2.cs +++ b/BepuPhysics/Trees/Tree_Refine2.cs @@ -1,8 +1,11 @@ -using BepuUtilities.Collections; +using BepuUtilities; +using BepuUtilities.Collections; using BepuUtilities.Memory; using System; using System.Diagnostics; +using System.Numerics; using System.Runtime.CompilerServices; +using System.Xml.Linq; namespace BepuPhysics.Trees; @@ -24,9 +27,13 @@ public static void AppendRootRefinementInternalSubtree(ref QuickList /// /// Incrementally refines a subset of the tree by running a binned builder over subtrees. /// - /// Index used to distribute refinements over multiple executions. + /// Size of the refinement run on nodes near the root. + /// Index used to distribute subtree refinements over multiple executions. + /// Number of subtree refinements to execute. + /// Target size of subtree refinements. The actual size of refinement will usually be larger or smaller. + /// Pool used for ephemeral allocations during the refinement. /// Nodes will not be refit. - public void Refine2(ref int refinementStartIndex, int subtreeRefinementCount, int subtreeRefinementSize, int rootRefinementSize, BufferPool pool) + public void Refine2(int rootRefinementSize, ref int subtreeRefinementStartIndex, int subtreeRefinementCount, int targetSubtreeRefinementSize, BufferPool pool) { //1. The root gets traversed by every traversal, so refinement always refines the region close to the root. //2. Subtrees dangling off the root region are refined incrementally over time. @@ -35,9 +42,9 @@ public void Refine2(ref int refinementStartIndex, int subtreeRefinementCount, in //Identify all candidates for refinement. //We'll define a candidate as any node which has a total leaf count less than or equal to the subtreeRefinementSize, and its parent does not. - var maximumRefinementCandidateCount = 2 * (LeafCount + subtreeRefinementSize - 1) / subtreeRefinementSize; + var maximumRefinementCandidateCount = 2 * (LeafCount + targetSubtreeRefinementSize - 1) / targetSubtreeRefinementSize; var subtreeRefinementCandidates = new QuickList(maximumRefinementCandidateCount, pool); - var stack = new QuickList(int.Max(rootRefinementSize, subtreeRefinementSize), pool); + var stack = new QuickList(int.Max(rootRefinementSize, targetSubtreeRefinementSize), pool); var rootRefinementSubtrees = new QuickList(rootRefinementSize, pool); var rootRefinementNodeIndices = new QuickList(rootRefinementSize, pool); stack.AllocateUnsafely() = 0; @@ -55,7 +62,7 @@ public void Refine2(ref int refinementStartIndex, int subtreeRefinementCount, in //Note that refinement candidates are not pushed onto the stack, so we don't need to check to see if the parent has more than subtreeRefinementSize leaves- //it definitely does, or else it would have been the refinement candidate. Or it's the root. //Note the order of stack pushes: if the tree is in DFS order, we want to next visit child A, so push it second. - if (node.B.LeafCount <= subtreeRefinementSize) + if (node.B.LeafCount <= targetSubtreeRefinementSize) subtreeRefinementCandidates.AllocateUnsafely() = node.B.Index; else stack.AllocateUnsafely() = node.B.Index; @@ -66,7 +73,7 @@ public void Refine2(ref int refinementStartIndex, int subtreeRefinementCount, in } else { - if (node.A.LeafCount <= subtreeRefinementSize) + if (node.A.LeafCount <= targetSubtreeRefinementSize) subtreeRefinementCandidates.AllocateUnsafely() = node.A.Index; else stack.AllocateUnsafely() = node.A.Index; @@ -76,12 +83,12 @@ public void Refine2(ref int refinementStartIndex, int subtreeRefinementCount, in //There's no guarantee we have enough actual subtrees to serve the request. var effectiveSubtreeRefinementCount = int.Min(subtreeRefinementCount, subtreeRefinementCandidates.Count); //Refine the desired number of subtrees contiguously, starting at the refinement tracker index and wrapping. - refinementStartIndex %= subtreeRefinementCandidates.Count; + subtreeRefinementStartIndex %= subtreeRefinementCandidates.Count; var subtreeRefinementTargets = new QuickList(effectiveSubtreeRefinementCount, pool); for (int i = 0; i < effectiveSubtreeRefinementCount; ++i) { - var candidate = subtreeRefinementCandidates[refinementStartIndex]; - Debug.Assert(Nodes[candidate].A.LeafCount + Nodes[candidate].B.LeafCount <= subtreeRefinementSize); + var candidate = subtreeRefinementCandidates[subtreeRefinementStartIndex]; + Debug.Assert(Nodes[candidate].A.LeafCount + Nodes[candidate].B.LeafCount <= targetSubtreeRefinementSize); subtreeRefinementTargets.AllocateUnsafely() = candidate; //Note that refinement targets are *also* subtrees for the root refinement. //Add the NodeChild associated with the refinement target to the root refinement list. @@ -91,9 +98,9 @@ public void Refine2(ref int refinementStartIndex, int subtreeRefinementCount, in ref var childOfParent = ref Unsafe.Add(ref Nodes[metanode.Parent].A, metanode.IndexInParent); AppendRootRefinementInternalSubtree(ref rootRefinementSubtrees, childOfParent); } - ++refinementStartIndex; - if (refinementStartIndex >= subtreeRefinementCandidates.Count) - refinementStartIndex -= subtreeRefinementCandidates.Count; + ++subtreeRefinementStartIndex; + if (subtreeRefinementStartIndex >= subtreeRefinementCandidates.Count) + subtreeRefinementStartIndex -= subtreeRefinementCandidates.Count; } //We now have the set of subtree refinement targets, plus an incomplete collection of root refinement nodes. @@ -108,7 +115,7 @@ public void Refine2(ref int refinementStartIndex, int subtreeRefinementCount, in var baseAmountPerCandidate = remainingRootSubtreesRequired / traversalRestartCount; var remainder = remainingRootSubtreesRequired - baseAmountPerCandidate * traversalRestartCount; //Start at the end of the selected region of refinement candidates- we updated the refinementStartIndex earlier, so it's in the right spot. - var refinementCandidateIndex = refinementStartIndex; + var refinementCandidateIndex = subtreeRefinementStartIndex; int largestRestartedTraversalLeafCount = 0; var traversalRestarts = new QuickList<(int Index, int MaximumLeafCount)>(traversalRestartCount, pool); var subtreeSurplus = 0; @@ -175,7 +182,7 @@ public void Refine2(ref int refinementStartIndex, int subtreeRefinementCount, in //Sorting *may* help in some cases, but not enough to warrant it by default. //((Span)rootRefinementNodeIndices).Sort(); Debug.Assert(rootRefinementNodeIndices.Count == rootRefinementSubtrees.Count - 1); - var refinementNodesAllocation = new Buffer(int.Max(rootRefinementNodeIndices.Count, subtreeRefinementSize), pool); + var refinementNodesAllocation = new Buffer(int.Max(rootRefinementNodeIndices.Count, targetSubtreeRefinementSize), pool); var refinementMetanodesAllocation = new Buffer(refinementNodesAllocation.Length, pool); //Validate(); @@ -187,22 +194,11 @@ public void Refine2(ref int refinementStartIndex, int subtreeRefinementCount, in rootRefinementSubtrees.Dispose(pool); rootRefinementNodeIndices.Dispose(pool); - for (int i = 0; i < NodeCount; ++i) - { - var node = Nodes[i]; - var expectedParentLeafCount = node.A.LeafCount + node.B.LeafCount; - var metanode = Metanodes[i]; - if (metanode.Parent >= 0) - { - Debug.Assert(Unsafe.Add(ref Nodes[metanode.Parent].A, metanode.IndexInParent).LeafCount == expectedParentLeafCount); - } - } - //Validate(); //Root refine is done; execute all the subtree refinements. - var subtreeRefinementNodeIndices = new QuickList(subtreeRefinementSize, pool); - var subtreeRefinementLeaves = new QuickList(subtreeRefinementSize, pool); + var subtreeRefinementNodeIndices = new QuickList(targetSubtreeRefinementSize, pool); + var subtreeRefinementLeaves = new QuickList(targetSubtreeRefinementSize, pool); for (int i = 0; i < subtreeRefinementTargets.Count; ++i) { //Accumulate nodes and leaves with a prepass. @@ -240,7 +236,6 @@ public void Refine2(ref int refinementStartIndex, int subtreeRefinementCount, in //Validate(); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] void ReifyRootRefinementNodeChild(ref int index, ref QuickList refinementNodeIndices, int realNodeIndex, int childIndexInParent) { //Root refinements mark internal subtrees with a flag in the second to last index. @@ -326,4 +321,396 @@ void ReifyRefinement(QuickList refinementNodeIndices, Buffer refineme Debug.Assert(Metanodes[realNodeIndex].Parent < 0 || Unsafe.Add(ref Nodes[Metanodes[realNodeIndex].Parent].A, Metanodes[realNodeIndex].IndexInParent).Index == realNodeIndex); } } + + bool CheckChildForSubtreeRefinementAccumulation(int subtreeRefinementSize, int accumulatedLeftLeaves, in NodeChild child, int nextLeafCount, ref int startIndex, ref int nextNodeIndex, ref QuickList refinementTargets) + { + if (child.Index < 0) + { + //This child is a leaf; it can't be a refinement target. + startIndex = nextLeafCount; + return true; + } + if (startIndex >= accumulatedLeftLeaves && child.LeafCount <= subtreeRefinementSize) + { + //This is the candidate! + refinementTargets.AllocateUnsafely() = child.Index; + startIndex = nextLeafCount; + return true; + } + //The child isn't the candidate, but it is where we should go next. + nextNodeIndex = child.Index; + return false; + } + + int FindSubtreeRefinementTargets(int startIndex, int subtreeRefinementSize, int targetSubtreeRefinementCount, ref QuickList refinementTargets) + { + //The goal here is to select candidates which have two properties: + //1. The total number of leaves to the left of the node is less than or equal to the startIndex. + //2. The total number of leaves in the subtree below the node is less than or equal to the targetSubtreeRefinementSize. + //We can perform this search stacklessly! + //(You could be a little more clever with the traversal here instead of restarting at the root every time, but the targetSubtreeRefinementCount should be <32, so shrug.) + Debug.Assert(LeafCount >= 2, "Subtree finding assumes that the root is complete."); + Debug.Assert(refinementTargets.Span.Length >= targetSubtreeRefinementCount, "The candidates list should hold the target refinement count."); + Debug.Assert(subtreeRefinementSize > 2, "Subtree refinements must actually cover some refineable region!"); + int traversedLeaves = 0; + while (refinementTargets.Count < targetSubtreeRefinementCount && traversedLeaves < LeafCount) + { + int nextNodeIndex = 0; + int accumulatedLeftLeaves = 0; + var previousStart = startIndex; + while (true) + { + ref var node = ref Nodes[nextNodeIndex]; + var nextLeafCount = accumulatedLeftLeaves + node.A.LeafCount; + if (startIndex < nextLeafCount) + { + //The startIndex is to the left of the midpoint. We need to consider the left child. + if (CheckChildForSubtreeRefinementAccumulation(subtreeRefinementSize, accumulatedLeftLeaves, node.A, nextLeafCount, ref startIndex, ref nextNodeIndex, ref refinementTargets)) + break; + } + else + { + accumulatedLeftLeaves = nextLeafCount; + nextLeafCount = accumulatedLeftLeaves + node.B.LeafCount; + + if (startIndex < nextLeafCount) + { + //The startIndex is to the right of the midpoint. We need to consider the right child. + if (CheckChildForSubtreeRefinementAccumulation(subtreeRefinementSize, accumulatedLeftLeaves, node.B, nextLeafCount, ref startIndex, ref nextNodeIndex, ref refinementTargets)) + break; + } + } + } + var leavesTraversedInLastAttempt = startIndex - previousStart; + traversedLeaves += leavesTraversedInLastAttempt; + } + return startIndex; + } + + void FindSubtreeRefinementTargets3(int nodeIndex, int leftLeafCount, int subtreeRefinementSize, int targetSubtreeRefinementCount, ref int startIndex, int endIndex, ref QuickList refinementTargets) + { + //If we've used up the target region already, just quit. + if (startIndex >= endIndex || refinementTargets.Count == targetSubtreeRefinementCount) + return; + + ref var node = ref Nodes[nodeIndex]; + var midpoint = leftLeafCount + node.A.LeafCount; + if (startIndex < midpoint) + { + //Go left! + if (node.A.LeafCount <= subtreeRefinementSize) + { + //This is a candidate! + if (node.A.LeafCount > 2) //Only include subtrees if they could be meaningfully refined. + refinementTargets.AllocateUnsafely() = node.A.Index; + startIndex += node.A.LeafCount; + } + else + { + //Too big to be a candidate; traverse further. + FindSubtreeRefinementTargets3(node.A.Index, leftLeafCount, subtreeRefinementSize, targetSubtreeRefinementCount, ref startIndex, endIndex, ref refinementTargets); + } + } + + //If we've used up the target region already, just quit. + if (startIndex >= endIndex || refinementTargets.Count == targetSubtreeRefinementCount) + return; + + //Note that A's traversal may have modified startIndex such that B should now be traversed. + if (startIndex >= midpoint) + { + //Go right! + if (node.B.LeafCount <= subtreeRefinementSize) + { + //This is a candidate! + if (node.B.LeafCount > 2) //Only include subtrees if they could be meaningfully refined. + refinementTargets.AllocateUnsafely() = node.B.Index; + startIndex += node.B.LeafCount; + } + else + { + //Too big to be a candidate; traverse further. + FindSubtreeRefinementTargets3(node.B.Index, leftLeafCount + node.A.LeafCount, subtreeRefinementSize, targetSubtreeRefinementCount, ref startIndex, endIndex, ref refinementTargets); + } + } + } + void FindSubtreeRefinementTargets3(int subtreeRefinementSize, int targetSubtreeRefinementCount, ref int startIndex, ref QuickList refinementTargets) + { + var initialStart = startIndex; + FindSubtreeRefinementTargets3(0, 0, subtreeRefinementSize, targetSubtreeRefinementCount, ref startIndex, LeafCount, ref refinementTargets); + if (startIndex >= LeafCount && refinementTargets.Count < targetSubtreeRefinementCount) + { + //Hit the end of the tree. Reset. + startIndex = 0; + var remainingLeaves = LeafCount - initialStart; + FindSubtreeRefinementTargets3(0, 0, subtreeRefinementSize, targetSubtreeRefinementCount, ref startIndex, remainingLeaves, ref refinementTargets); + } + } + + + int FindSubtreeRefinementTargets2(int startIndex, int subtreeRefinementSize, int targetSubtreeRefinementCount, ref QuickList refinementTargets) + { + //The goal here is to select candidates which have two properties: + //1. The total number of leaves to the left of the node is less than or equal to the startIndex. + //2. The total number of leaves in the subtree below the node is less than or equal to the targetSubtreeRefinementSize. + //We can perform this search stacklessly! + //Note an important detail: nodes beyond the first encountered meeting the threshold leaf count for being a candidate cannot be considered. + //Doing so would allow very small subtrees to be picked if the "leaves counted on the left" condition is met late. + //Instead, when encountering a node at the threshold size, we must either accept it or move on to the next node. + Debug.Assert(LeafCount >= 2, "Subtree finding assumes that the root is complete."); + Debug.Assert(refinementTargets.Span.Length >= targetSubtreeRefinementCount, "The candidates list should hold the target refinement count."); + Debug.Assert(subtreeRefinementSize > 2, "Subtree refinements must actually cover some refineable region!"); + int traversedLeaves = 0; + + int nextNodeIndex = 0; + int accumulatedLeftLeaves = 0; + while (refinementTargets.Count < targetSubtreeRefinementCount && traversedLeaves < LeafCount) + { + ref var node = ref Nodes[nextNodeIndex]; + var nodeTotalLeafCount = node.A.LeafCount + node.B.LeafCount; + //Should we go left or right? + var leafMidpointCount = accumulatedLeftLeaves + node.A.LeafCount; + if (startIndex >= accumulatedLeftLeaves && startIndex < leafMidpointCount) + { + //Go left. + if (node.A.LeafCount <= subtreeRefinementSize) + { + //A is small enough that it can be a refinement. + if (node.A.Index >= 0) + refinementTargets.AllocateUnsafely() = node.A.Index; + //The traversal may continue to find additional refinement targets. This will bump it over to the right child on the next iteration. + startIndex = leafMidpointCount; + } + else + { + //A remains large enough that it cannot yet be a refinement. + nextNodeIndex = node.A.Index; + } + } + else + { + //Go right. + accumulatedLeftLeaves += node.A.LeafCount; + traversedLeaves += node.A.LeafCount; + Debug.Assert(startIndex >= leafMidpointCount && startIndex < accumulatedLeftLeaves + nodeTotalLeafCount); + if (node.B.LeafCount <= subtreeRefinementSize) + { + //B is small enough that it can be a refinement. + if (node.B.Index >= 0) + refinementTargets.AllocateUnsafely() = node.B.Index; + //The traversal may continue to find additional refinement targets. + accumulatedLeftLeaves += node.B.LeafCount; + traversedLeaves += node.B.LeafCount; + startIndex = accumulatedLeftLeaves + node.B.LeafCount; + //We'll need to bump to the parent since we're done with this node- but note that this child might have been child B of the parent, and so on. + //Follow parent pointers until we find a node that has another child. + var bumpUpNodeIndex = Metanodes[nextNodeIndex].Parent; + var previousTotalLeafCount = nodeTotalLeafCount; + while (true) + { + if (bumpUpNodeIndex < 0) + { + //Hit the root. We've reached the end of the tree, apparently. Wrap around; restart the traversal. + startIndex = 0; + accumulatedLeftLeaves = 0; + nextNodeIndex = 0; + break; + } + //We need to back out an entire node's worth of leaves; this accumulator is how we track how far we've made it in the tree. + //If we don't back these nodes out, the tree will think it's further ahead than it should be, and it won't traverse the right child to get to the startIndex. + accumulatedLeftLeaves -= previousTotalLeafCount; + ref var bumpUpNode = ref Nodes[bumpUpNodeIndex]; + previousTotalLeafCount = bumpUpNode.A.LeafCount + bumpUpNode.B.LeafCount; + ref var metanode = ref Metanodes[bumpUpNodeIndex]; + if (metanode.IndexInParent == 0) + { + //The node is the left child of the parent, so the parent is the next node to visit. + nextNodeIndex = metanode.Parent; + break; + } + //Still need to go further. + bumpUpNodeIndex = metanode.Parent; + + } + } + else + { + //B remains large enough that it cannot yet be a refinement. + nextNodeIndex = node.B.Index; + } + } + } + return startIndex; + } + + bool IsNodeChildSubtreeRefinementTarget(in QuickList subtreeRefinements, in NodeChild child, int parentTotalLeafCount, int subtreeRefinementSize) + { + //First check if it *could* be one by checking the leaf count threshold. + if (child.LeafCount <= subtreeRefinementSize && parentTotalLeafCount > subtreeRefinementSize) + { + //It may be a subtree refinement. Do a deeper test! + var vectorCount = BundleIndexing.GetBundleCount(subtreeRefinements.Count); + var search = new Vector(child.Index); + var vectors = subtreeRefinements.Span.As>(); + for (int i = 0; i < vectorCount; ++i) + { + if (Vector.EqualsAny(search, vectors[i])) + return true; + } + } + return false; + } + + /// + /// Checks if a child should be a subtree in the root refinement. If so, it's added to the list. Otherwise, it's pushed onto the stack. + /// + /// The amount of surplus leaf budget accumulated by pushing this child. The value will be nonzero only if the child was a subtree refinement target. + private int TryPushChildForRootRefinement( + int subtreeRefinementSize, in QuickList subtreeRefinementRoots, int nodeTotalLeafCount, int subtreeBudget, in NodeChild child, ref QuickList<(int nodeIndex, int subtreeBudget)> stack, ref QuickList rootRefinementSubtrees) + { + //We automatically accept any child as a subtree for the refinement process if: + //1. It's a leaf node, or + //2. This traversal path has used up its node budget. + Debug.Assert(subtreeBudget >= 0); + if (subtreeBudget == 1) + { + //rootRefinementSubtrees.AllocateUnsafely() = child; + ref var allocatedChild = ref rootRefinementSubtrees.AllocateUnsafely(); + allocatedChild = child; + allocatedChild.Index |= flagForRootRefinementSubtree; + } + else + { + //Internal node; is it a subtree refinement? + if (IsNodeChildSubtreeRefinementTarget(subtreeRefinementRoots, child, nodeTotalLeafCount, subtreeRefinementSize)) + { + //Yup! + ref var allocatedChild = ref rootRefinementSubtrees.AllocateUnsafely(); + allocatedChild = child; + //Internal nodes used as subtrees by the root refinement are flagged so that the reification process knows to stop. + Debug.Assert(allocatedChild.Index < flagForRootRefinementSubtree, "The use of an upper index bit as flag means the binned refiner cannot handle trees with billions of children."); + allocatedChild.Index |= flagForRootRefinementSubtree; + //Return the budget so it can be used on other nodes that follow in the traversal. Don't really care *where* it goes. + return subtreeBudget; + } + else + { + //Not a subtree refinement, and we know we have budget remaining. + stack.AllocateUnsafely() = (child.Index, subtreeBudget); + } + } + return 0; + } + + /// + /// Incrementally refines a subset of the tree by running a binned builder over subtrees. + /// + /// Size of the refinement run on nodes near the root. + /// Index used to distribute subtree refinements over multiple executions. + /// Number of subtree refinements to execute. + /// Target size of subtree refinements. The actual size of refinement will usually be larger or smaller. + /// Pool used for ephemeral allocations during the refinement. + /// Nodes will not be refit. + public void Refine3(int rootRefinementSize, ref int subtreeRefinementStartIndex, int subtreeRefinementCount, int subtreeRefinementSize, BufferPool pool) + { + //No point refining anything with two leaves. This condition also avoids having to special case for an incomplete root node. + if (LeafCount <= 2) + return; + //We used a vectorized containment test later, so make sure to pad out the refinement target list. + var subtreeRefinementCapacity = BundleIndexing.GetBundleCount(subtreeRefinementCount) * Vector.Count; + var subtreeRefinementTargets = new QuickList(subtreeRefinementCapacity, pool); + //Console.WriteLine($"Starting at {subtreeRefinementStartIndex}"); + //subtreeRefinementStartIndex = FindSubtreeRefinementTargets2(subtreeRefinementStartIndex, subtreeRefinementSize, subtreeRefinementCount, ref subtreeRefinementTargets); + FindSubtreeRefinementTargets3(subtreeRefinementSize, subtreeRefinementCount, ref subtreeRefinementStartIndex, ref subtreeRefinementTargets); + //Fill the trailing slots in the list with -1 to avoid matches. + ((Span)subtreeRefinementTargets.Span)[subtreeRefinementTargets.Count..].Fill(-1); + + //for (int i = 0; i < subtreeRefinementTargets.Count; ++i) + //{ + // Console.Write($"{Nodes[subtreeRefinementTargets[i]].A.LeafCount + Nodes[subtreeRefinementTargets[i]].B.LeafCount}, "); + //} + //Console.WriteLine(); + + //We now know which nodes are the roots of subtree refinements; the root refinement can avoid traversing through them. + var rootStack = new QuickList<(int nodeIndex, int subtreeBudget)>(rootRefinementSize, pool); + var rootRefinementSubtrees = new QuickList(rootRefinementSize, pool); + var rootRefinementNodeIndices = new QuickList(rootRefinementSize, pool); + rootStack.AllocateUnsafely() = (0, rootRefinementSize); + int surplusSubtreeBudget = 0; + while (rootStack.TryPop(out var nodeToVisit)) + { + rootRefinementNodeIndices.AllocateUnsafely() = nodeToVisit.nodeIndex; + ref var node = ref Nodes[nodeToVisit.nodeIndex]; + var nodeTotalLeafCount = node.A.LeafCount + node.B.LeafCount; + //The "budget" is just a mechanism for bottoming out the traversal at the same depth as a BFS, but during a depth first traversal. + var effectiveNodeBudget = int.Min(nodeTotalLeafCount, nodeToVisit.subtreeBudget + surplusSubtreeBudget); + var lowerSubtreeBudget = int.Min((effectiveNodeBudget + 1) / 2, int.Min(node.A.LeafCount, node.B.LeafCount)); + var higherSubtreeBudget = effectiveNodeBudget - lowerSubtreeBudget; + var useSmallerForA = lowerSubtreeBudget == node.A.LeafCount; + var aSubtreeBudget = useSmallerForA ? lowerSubtreeBudget : higherSubtreeBudget; + var bSubtreeBudget = useSmallerForA ? higherSubtreeBudget : lowerSubtreeBudget; + + //If the effective budgets exceed the originally scheduled amount, then we've used up part of our surplus. + surplusSubtreeBudget += nodeToVisit.subtreeBudget - (aSubtreeBudget + bSubtreeBudget); + + surplusSubtreeBudget += TryPushChildForRootRefinement(subtreeRefinementSize, subtreeRefinementTargets, nodeTotalLeafCount, bSubtreeBudget, node.B, ref rootStack, ref rootRefinementSubtrees); + surplusSubtreeBudget += TryPushChildForRootRefinement(subtreeRefinementSize, subtreeRefinementTargets, nodeTotalLeafCount, aSubtreeBudget, node.A, ref rootStack, ref rootRefinementSubtrees); + //surplusSubtreeBudget = 0; + } + + //Now that we have a list of nodes to refine, we can run the root refinement. + Debug.Assert(rootRefinementNodeIndices.Count == rootRefinementSubtrees.Count - 1); + var refinementNodesAllocation = new Buffer(int.Max(rootRefinementNodeIndices.Count, subtreeRefinementSize), pool); + var refinementMetanodesAllocation = new Buffer(refinementNodesAllocation.Length, pool); + + var rootRefinementNodes = refinementNodesAllocation.Slice(0, rootRefinementNodeIndices.Count); + var rootRefinementMetanodes = refinementMetanodesAllocation.Slice(0, rootRefinementNodeIndices.Count); + //Passing 'default' for the leaves tells the binned builder to not worry about updating leaves. + BinnedBuild(rootRefinementSubtrees, rootRefinementNodes, rootRefinementMetanodes, default, null, pool); + ReifyRootRefinement(rootRefinementNodeIndices, rootRefinementNodes); + rootRefinementSubtrees.Dispose(pool); + rootRefinementNodeIndices.Dispose(pool); + + rootStack.Dispose(pool); + + var subtreeRefinementNodeIndices = new QuickList(subtreeRefinementSize, pool); + var subtreeRefinementLeaves = new QuickList(subtreeRefinementSize, pool); + var subtreeStack = new QuickList(subtreeRefinementSize, pool); + for (int i = 0; i < subtreeRefinementTargets.Count; ++i) + { + //Accumulate nodes and leaves with a prepass. + Debug.Assert(subtreeStack.Count == 0 && subtreeRefinementNodeIndices.Count == 0); + subtreeStack.AllocateUnsafely() = subtreeRefinementTargets[i]; + while (subtreeStack.TryPop(out var nodeToVisit)) + { + ref var node = ref Nodes[nodeToVisit]; + subtreeRefinementNodeIndices.AllocateUnsafely() = nodeToVisit; + if (node.B.Index >= 0) + subtreeStack.AllocateUnsafely() = node.B.Index; + else + subtreeRefinementLeaves.AllocateUnsafely() = node.B; + if (node.A.Index >= 0) + subtreeStack.AllocateUnsafely() = node.A.Index; + else + subtreeRefinementLeaves.AllocateUnsafely() = node.A; + } + + var refinementNodes = refinementNodesAllocation.Slice(0, subtreeRefinementNodeIndices.Count); + var refinementMetanodes = refinementMetanodesAllocation.Slice(0, subtreeRefinementNodeIndices.Count); + //Passing 'default' for the leaves tells the binned builder to not worry about updating leaves. + BinnedBuild(subtreeRefinementLeaves, refinementNodes, refinementMetanodes, default, null, pool); + ReifyRefinement(subtreeRefinementNodeIndices, refinementNodes); + + subtreeRefinementNodeIndices.Count = 0; + subtreeRefinementLeaves.Count = 0; + } + + subtreeRefinementNodeIndices.Dispose(pool); + subtreeRefinementLeaves.Dispose(pool); + subtreeRefinementTargets.Dispose(pool); + subtreeStack.Dispose(pool); + refinementNodesAllocation.Dispose(pool); + refinementMetanodesAllocation.Dispose(pool); + } + } diff --git a/Demos/Demos.csproj b/Demos/Demos.csproj index 032d20156..6e2628452 100644 --- a/Demos/Demos.csproj +++ b/Demos/Demos.csproj @@ -17,7 +17,6 @@ true TRACE;RELEASE true - full diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index 21e6826c1..e5a13368f 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -164,31 +164,62 @@ public override void Initialize(ContentArchive content, Camera camera) //var mesh = new Mesh(triangles, Vector3.One, BufferPool); var mesh = DemoMeshHelper.CreateGiantMeshFast(triangles, Vector3.One, BufferPool); + int refinementState = 0; + long sum = 0; + int cacheOptimizationStart = 0; + for (int refinementIndex = 0; refinementIndex < 16384; ++refinementIndex) + { + //mesh.Tree.CacheOptimizeLimitedSubtree(0, 4096); + //mesh.Tree.CacheOptimize(0); + //var optimizedCount = mesh.Tree.CacheOptimizeRegion(0, int.MaxValue); + //int localOptimizationCount = 0; + //const int targetOptimizationCount = 8192; + //while (localOptimizationCount < targetOptimizationCount) + //{ + // var optimizedCount = mesh.Tree.CacheOptimizeRegion(cacheOptimizationStart, targetOptimizationCount); + // localOptimizationCount += optimizedCount; + // cacheOptimizationStart += optimizedCount; + // if (cacheOptimizationStart >= mesh.Tree.NodeCount) + // cacheOptimizationStart -= mesh.Tree.NodeCount; + //} + var start = Stopwatch.GetTimestamp(); + //mesh.Tree.Refine2(8192, ref refinementState, 1, 8192, BufferPool); + mesh.Tree.Refine3(32768, ref refinementState, 1, 32768, BufferPool); + var end = Stopwatch.GetTimestamp(); + sum += end - start; + if ((refinementIndex + 1) % 128 == 0) + { + var cacheQuality = mesh.Tree.MeasureCacheQuality(); + var costMetric = mesh.Tree.MeasureCostMetric(); + Console.WriteLine($"cost, cache for {refinementIndex}: {costMetric}, {cacheQuality}"); + Console.WriteLine($"Time (average) (ms): {(end - start) * 1e3 / Stopwatch.Frequency}, {sum * 1e3 / ((refinementIndex + 1) * Stopwatch.Frequency)}"); + } + } Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); Console.WriteLine($"node count: {mesh.Tree.NodeCount}"); Console.WriteLine($"initial SAH: {mesh.Tree.MeasureCostMetric()}, cache quality: {mesh.Tree.MeasureCacheQuality()}"); Console.WriteLine($"initial bounds: A ({mesh.Tree.Nodes[0].A.Min}, {mesh.Tree.Nodes[0].B.Max}), B ({mesh.Tree.Nodes[0].B.Min}, {mesh.Tree.Nodes[0].B.Max})"); - BufferPool.Take(mesh.Triangles.Length, out var subtrees); + //BufferPool.Take(mesh.Triangles.Length, out var subtrees); - Action setup = () => - { - for (int i = 0; i < mesh.Triangles.Length; ++i) - { - ref var t = ref mesh.Triangles[i]; - ref var subtree = ref subtrees[i]; - subtree.Min = Vector3.Min(t.A, Vector3.Min(t.B, t.C)); - subtree.Max = Vector3.Max(t.A, Vector3.Max(t.B, t.C)); - subtree.Index = Tree.Encode(i); - subtree.LeafCount = 1; - } - }; + //Action setup = () => + //{ + // for (int i = 0; i < mesh.Triangles.Length; ++i) + // { + // ref var t = ref mesh.Triangles[i]; + // ref var subtree = ref subtrees[i]; + // subtree.Min = Vector3.Min(t.A, Vector3.Min(t.B, t.C)); + // subtree.Max = Vector3.Max(t.A, Vector3.Max(t.B, t.C)); + // subtree.Index = Tree.Encode(i); + // subtree.LeafCount = 1; + // } + //}; - BinnedTest(setup, () => - { - mesh.Tree.BinnedBuild(subtrees, ThreadDispatcher, pool: BufferPool); - }, "Revamp Single Axis MT", ref mesh.Tree); + //BinnedTest(setup, () => + //{ + // mesh.Tree.BinnedBuild(subtrees, ThreadDispatcher, pool: BufferPool); + //}, "Revamp Single Axis MT", ref mesh.Tree); //BufferPool.Take(mesh.Triangles.Length, out var leafBounds); //BufferPool.Take(mesh.Triangles.Length, out var leafIndices); From fcf05abcfbce5ceae70594a7a87a52e60f8e3647 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 23 Jun 2023 21:32:04 -0500 Subject: [PATCH 738/947] And then poof! --- BepuPhysics/Trees/Tree_Refine2.cs | 409 +----------------- .../SpecializedTests/TreeFiddlingTestDemo.cs | 2 +- 2 files changed, 12 insertions(+), 399 deletions(-) diff --git a/BepuPhysics/Trees/Tree_Refine2.cs b/BepuPhysics/Trees/Tree_Refine2.cs index dad8892d4..c27cce63d 100644 --- a/BepuPhysics/Trees/Tree_Refine2.cs +++ b/BepuPhysics/Trees/Tree_Refine2.cs @@ -12,230 +12,7 @@ namespace BepuPhysics.Trees; public partial struct Tree { const int flagForRootRefinementSubtree = 1 << 30; - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void AppendRootRefinementInternalSubtree(ref QuickList rootSubtrees, in NodeChild child) - { - ref var allocatedChild = ref rootSubtrees.AllocateUnsafely(); - allocatedChild = child; - //Internal nodes used as subtrees by the root refinement are flagged. - Debug.Assert(allocatedChild.Index < flagForRootRefinementSubtree, "The use of an upper index bit as flag means the binned refiner cannot handle trees with billions of children."); - if (allocatedChild.Index >= 0) - allocatedChild.Index |= flagForRootRefinementSubtree; - - } - - /// - /// Incrementally refines a subset of the tree by running a binned builder over subtrees. - /// - /// Size of the refinement run on nodes near the root. - /// Index used to distribute subtree refinements over multiple executions. - /// Number of subtree refinements to execute. - /// Target size of subtree refinements. The actual size of refinement will usually be larger or smaller. - /// Pool used for ephemeral allocations during the refinement. - /// Nodes will not be refit. - public void Refine2(int rootRefinementSize, ref int subtreeRefinementStartIndex, int subtreeRefinementCount, int targetSubtreeRefinementSize, BufferPool pool) - { - //1. The root gets traversed by every traversal, so refinement always refines the region close to the root. - //2. Subtrees dangling off the root region are refined incrementally over time. - //3. It's important for refinement to be able to exchange nodes across subtrees, so the root region varies a bit between refinements. - // That's implemented by varying the depth that the root refinement traverses for its subtrees. - - //Identify all candidates for refinement. - //We'll define a candidate as any node which has a total leaf count less than or equal to the subtreeRefinementSize, and its parent does not. - var maximumRefinementCandidateCount = 2 * (LeafCount + targetSubtreeRefinementSize - 1) / targetSubtreeRefinementSize; - var subtreeRefinementCandidates = new QuickList(maximumRefinementCandidateCount, pool); - var stack = new QuickList(int.Max(rootRefinementSize, targetSubtreeRefinementSize), pool); - var rootRefinementSubtrees = new QuickList(rootRefinementSize, pool); - var rootRefinementNodeIndices = new QuickList(rootRefinementSize, pool); - stack.AllocateUnsafely() = 0; - while (stack.TryPop(out var nodeToVisit)) - { - rootRefinementNodeIndices.AllocateUnsafely() = nodeToVisit; - ref var node = ref Nodes[nodeToVisit]; - if (node.B.Index < 0) - { - //Root refinement *may* encounter leaves before subtree refinement candidates. They have to be included in the root's refinement. - rootRefinementSubtrees.AllocateUnsafely() = node.B; - } - else - { - //Note that refinement candidates are not pushed onto the stack, so we don't need to check to see if the parent has more than subtreeRefinementSize leaves- - //it definitely does, or else it would have been the refinement candidate. Or it's the root. - //Note the order of stack pushes: if the tree is in DFS order, we want to next visit child A, so push it second. - if (node.B.LeafCount <= targetSubtreeRefinementSize) - subtreeRefinementCandidates.AllocateUnsafely() = node.B.Index; - else - stack.AllocateUnsafely() = node.B.Index; - } - if (node.A.Index < 0) - { - rootRefinementSubtrees.AllocateUnsafely() = node.A; - } - else - { - if (node.A.LeafCount <= targetSubtreeRefinementSize) - subtreeRefinementCandidates.AllocateUnsafely() = node.A.Index; - else - stack.AllocateUnsafely() = node.A.Index; - } - } - - //There's no guarantee we have enough actual subtrees to serve the request. - var effectiveSubtreeRefinementCount = int.Min(subtreeRefinementCount, subtreeRefinementCandidates.Count); - //Refine the desired number of subtrees contiguously, starting at the refinement tracker index and wrapping. - subtreeRefinementStartIndex %= subtreeRefinementCandidates.Count; - var subtreeRefinementTargets = new QuickList(effectiveSubtreeRefinementCount, pool); - for (int i = 0; i < effectiveSubtreeRefinementCount; ++i) - { - var candidate = subtreeRefinementCandidates[subtreeRefinementStartIndex]; - Debug.Assert(Nodes[candidate].A.LeafCount + Nodes[candidate].B.LeafCount <= targetSubtreeRefinementSize); - subtreeRefinementTargets.AllocateUnsafely() = candidate; - //Note that refinement targets are *also* subtrees for the root refinement. - //Add the NodeChild associated with the refinement target to the root refinement list. - ref var metanode = ref Metanodes[candidate]; - if (metanode.Parent >= 0) - { - ref var childOfParent = ref Unsafe.Add(ref Nodes[metanode.Parent].A, metanode.IndexInParent); - AppendRootRefinementInternalSubtree(ref rootRefinementSubtrees, childOfParent); - } - ++subtreeRefinementStartIndex; - if (subtreeRefinementStartIndex >= subtreeRefinementCandidates.Count) - subtreeRefinementStartIndex -= subtreeRefinementCandidates.Count; - } - - //We now have the set of subtree refinement targets, plus an incomplete collection of root refinement nodes. - //In multithreaded-land, we could kick off jobs for every subtree refinement right now and continue on with the root refinement work. - //Root refinement is currently in cache, so we'll do that now. - //For every subtree refinement candidate that was *not* selected, we should continue to traverse. - var remainingRootSubtreesRequired = rootRefinementSize - rootRefinementSubtrees.Count; - var traversalRestartCount = subtreeRefinementCandidates.Count - effectiveSubtreeRefinementCount; - if (traversalRestartCount > 0) - { - //Distribute the remaining root subtrees required over the set of non-target refinement candidates. - var baseAmountPerCandidate = remainingRootSubtreesRequired / traversalRestartCount; - var remainder = remainingRootSubtreesRequired - baseAmountPerCandidate * traversalRestartCount; - //Start at the end of the selected region of refinement candidates- we updated the refinementStartIndex earlier, so it's in the right spot. - var refinementCandidateIndex = subtreeRefinementStartIndex; - int largestRestartedTraversalLeafCount = 0; - var traversalRestarts = new QuickList<(int Index, int MaximumLeafCount)>(traversalRestartCount, pool); - var subtreeSurplus = 0; - for (int i = 0; i < traversalRestartCount; ++i) - { - var targetAmountForCandidate = baseAmountPerCandidate; - if (i < remainder) ++targetAmountForCandidate; - var candidate = subtreeRefinementCandidates[refinementCandidateIndex]; - ref var candidateNode = ref Nodes[candidate]; - //The number of subtrees we can actually collect from this candidate is limited to the number of leaves it has. - //There's no guarantee the tree is perfectly balanced, so if we wanted to guarantee the number of root refinement subtrees, - //we'd have to redistribute any 'extra' we accumulate (due to a lack of local leaves) to other candidates. - //We do make a very lazy attempt at redistributing: if we can't fit the full amount, we'll carry over the rest to the next candidate. - //If we can't allocate all the budget, shrugohwell. Don't really want to iterate. - var candidateLeafCount = candidateNode.A.LeafCount + candidateNode.B.LeafCount; - var amountForCandidate = int.Min(targetAmountForCandidate + subtreeSurplus, candidateLeafCount); - largestRestartedTraversalLeafCount = int.Max(largestRestartedTraversalLeafCount, amountForCandidate); - subtreeSurplus += targetAmountForCandidate - amountForCandidate; - traversalRestarts.AllocateUnsafely() = (candidate, amountForCandidate); - ++refinementCandidateIndex; - if (refinementCandidateIndex >= subtreeRefinementCandidates.Count) - refinementCandidateIndex -= subtreeRefinementCandidates.Count; - } - //Now we have all the traversal restart locations for the root refinement subtree collection: use them! - //TODO: While we created an intermediate "traversalRestarts" list for ease of generalizing to multiple threads, everything could have just - //been shoved directly into a traversal restart stack instead. - var traversalRestartStack = new QuickList<(int nodeToVisit, int maximumLeafCount)>(largestRestartedTraversalLeafCount, pool); - for (int i = 0; i < traversalRestarts.Count; ++i) - { - ref var traversalRestart = ref traversalRestarts[i]; - Debug.Assert(stack.Count == 0 && stack.Span.Length >= traversalRestart.MaximumLeafCount); - traversalRestartStack.AllocateUnsafely() = traversalRestart; - while (traversalRestartStack.TryPop(out var entry)) - { - ref var node = ref Nodes[entry.nodeToVisit]; - rootRefinementNodeIndices.AllocateUnsafely() = entry.nodeToVisit; - var maximumLeafCount = entry.maximumLeafCount; - int smallerLeafCount = int.Min(node.A.LeafCount, node.B.LeafCount); - var targetLeafCountForSmaller = int.Min(smallerLeafCount, (maximumLeafCount + 1) / 2); - var targetLeafCountForLarger = maximumLeafCount - targetLeafCountForSmaller; - var aIsSmaller = node.A.LeafCount < node.B.LeafCount; - var aLeafCount = aIsSmaller ? targetLeafCountForSmaller : targetLeafCountForLarger; - var bLeafCount = aIsSmaller ? targetLeafCountForLarger : targetLeafCountForSmaller; - Debug.Assert(bLeafCount <= node.B.LeafCount, "The node visited in this traversal should never see a target leaf count that exceeds what could fit in the children."); - Debug.Assert(aLeafCount > 0 && bLeafCount > 0, "Splitting subtree budget between children should never yield zero sized subtrees."); - //B pushed first so A is popped first; some degree of consistent local DFS ordering. - if (bLeafCount > 1) - traversalRestartStack.AllocateUnsafely() = (node.B.Index, bLeafCount); - else - AppendRootRefinementInternalSubtree(ref rootRefinementSubtrees, node.B); - if (aLeafCount > 1) - traversalRestartStack.AllocateUnsafely() = (node.A.Index, aLeafCount); - else - AppendRootRefinementInternalSubtree(ref rootRefinementSubtrees, node.A); - } - } - traversalRestarts.Dispose(pool); - traversalRestartStack.Dispose(pool); - } - subtreeRefinementCandidates.Dispose(pool); - - //We now have the set of root refinement subtrees. Root refine! - //The nodes collected during the root refinement are not ordered, so may destroy existing cache coherency. - //Sorting *may* help in some cases, but not enough to warrant it by default. - //((Span)rootRefinementNodeIndices).Sort(); - Debug.Assert(rootRefinementNodeIndices.Count == rootRefinementSubtrees.Count - 1); - var refinementNodesAllocation = new Buffer(int.Max(rootRefinementNodeIndices.Count, targetSubtreeRefinementSize), pool); - var refinementMetanodesAllocation = new Buffer(refinementNodesAllocation.Length, pool); - - //Validate(); - var rootRefinementNodes = refinementNodesAllocation.Slice(0, rootRefinementNodeIndices.Count); - var rootRefinementMetanodes = refinementMetanodesAllocation.Slice(0, rootRefinementNodeIndices.Count); - //Passing 'default' for the leaves tells the binned builder to not worry about updating leaves. - BinnedBuild(rootRefinementSubtrees, rootRefinementNodes, rootRefinementMetanodes, default, null, pool); - ReifyRootRefinement(rootRefinementNodeIndices, rootRefinementNodes); - rootRefinementSubtrees.Dispose(pool); - rootRefinementNodeIndices.Dispose(pool); - - //Validate(); - - //Root refine is done; execute all the subtree refinements. - var subtreeRefinementNodeIndices = new QuickList(targetSubtreeRefinementSize, pool); - var subtreeRefinementLeaves = new QuickList(targetSubtreeRefinementSize, pool); - for (int i = 0; i < subtreeRefinementTargets.Count; ++i) - { - //Accumulate nodes and leaves with a prepass. - Debug.Assert(stack.Count == 0 && subtreeRefinementNodeIndices.Count == 0); - stack.AllocateUnsafely() = subtreeRefinementTargets[i]; - while (stack.TryPop(out var nodeToVisit)) - { - ref var node = ref Nodes[nodeToVisit]; - subtreeRefinementNodeIndices.AllocateUnsafely() = nodeToVisit; - if (node.B.Index >= 0) - stack.AllocateUnsafely() = node.B.Index; - else - subtreeRefinementLeaves.AllocateUnsafely() = node.B; - if (node.A.Index >= 0) - stack.AllocateUnsafely() = node.A.Index; - else - subtreeRefinementLeaves.AllocateUnsafely() = node.A; - } - - var refinementNodes = refinementNodesAllocation.Slice(0, subtreeRefinementNodeIndices.Count); - var refinementMetanodes = refinementMetanodesAllocation.Slice(0, subtreeRefinementNodeIndices.Count); - //Passing 'default' for the leaves tells the binned builder to not worry about updating leaves. - BinnedBuild(subtreeRefinementLeaves, refinementNodes, refinementMetanodes, default, null, pool); - ReifyRefinement(subtreeRefinementNodeIndices, refinementNodes); - - subtreeRefinementNodeIndices.Count = 0; - subtreeRefinementLeaves.Count = 0; - } - subtreeRefinementTargets.Dispose(pool); - stack.Dispose(pool); - subtreeRefinementNodeIndices.Dispose(pool); - subtreeRefinementLeaves.Dispose(pool); - refinementNodesAllocation.Dispose(pool); - refinementMetanodesAllocation.Dispose(pool); - //Validate(); - } - + void ReifyRootRefinementNodeChild(ref int index, ref QuickList refinementNodeIndices, int realNodeIndex, int childIndexInParent) { //Root refinements mark internal subtrees with a flag in the second to last index. @@ -322,72 +99,8 @@ void ReifyRefinement(QuickList refinementNodeIndices, Buffer refineme } } - bool CheckChildForSubtreeRefinementAccumulation(int subtreeRefinementSize, int accumulatedLeftLeaves, in NodeChild child, int nextLeafCount, ref int startIndex, ref int nextNodeIndex, ref QuickList refinementTargets) - { - if (child.Index < 0) - { - //This child is a leaf; it can't be a refinement target. - startIndex = nextLeafCount; - return true; - } - if (startIndex >= accumulatedLeftLeaves && child.LeafCount <= subtreeRefinementSize) - { - //This is the candidate! - refinementTargets.AllocateUnsafely() = child.Index; - startIndex = nextLeafCount; - return true; - } - //The child isn't the candidate, but it is where we should go next. - nextNodeIndex = child.Index; - return false; - } - int FindSubtreeRefinementTargets(int startIndex, int subtreeRefinementSize, int targetSubtreeRefinementCount, ref QuickList refinementTargets) - { - //The goal here is to select candidates which have two properties: - //1. The total number of leaves to the left of the node is less than or equal to the startIndex. - //2. The total number of leaves in the subtree below the node is less than or equal to the targetSubtreeRefinementSize. - //We can perform this search stacklessly! - //(You could be a little more clever with the traversal here instead of restarting at the root every time, but the targetSubtreeRefinementCount should be <32, so shrug.) - Debug.Assert(LeafCount >= 2, "Subtree finding assumes that the root is complete."); - Debug.Assert(refinementTargets.Span.Length >= targetSubtreeRefinementCount, "The candidates list should hold the target refinement count."); - Debug.Assert(subtreeRefinementSize > 2, "Subtree refinements must actually cover some refineable region!"); - int traversedLeaves = 0; - while (refinementTargets.Count < targetSubtreeRefinementCount && traversedLeaves < LeafCount) - { - int nextNodeIndex = 0; - int accumulatedLeftLeaves = 0; - var previousStart = startIndex; - while (true) - { - ref var node = ref Nodes[nextNodeIndex]; - var nextLeafCount = accumulatedLeftLeaves + node.A.LeafCount; - if (startIndex < nextLeafCount) - { - //The startIndex is to the left of the midpoint. We need to consider the left child. - if (CheckChildForSubtreeRefinementAccumulation(subtreeRefinementSize, accumulatedLeftLeaves, node.A, nextLeafCount, ref startIndex, ref nextNodeIndex, ref refinementTargets)) - break; - } - else - { - accumulatedLeftLeaves = nextLeafCount; - nextLeafCount = accumulatedLeftLeaves + node.B.LeafCount; - - if (startIndex < nextLeafCount) - { - //The startIndex is to the right of the midpoint. We need to consider the right child. - if (CheckChildForSubtreeRefinementAccumulation(subtreeRefinementSize, accumulatedLeftLeaves, node.B, nextLeafCount, ref startIndex, ref nextNodeIndex, ref refinementTargets)) - break; - } - } - } - var leavesTraversedInLastAttempt = startIndex - previousStart; - traversedLeaves += leavesTraversedInLastAttempt; - } - return startIndex; - } - - void FindSubtreeRefinementTargets3(int nodeIndex, int leftLeafCount, int subtreeRefinementSize, int targetSubtreeRefinementCount, ref int startIndex, int endIndex, ref QuickList refinementTargets) + void FindSubtreeRefinementTargets(int nodeIndex, int leftLeafCount, int subtreeRefinementSize, int targetSubtreeRefinementCount, ref int startIndex, int endIndex, ref QuickList refinementTargets) { //If we've used up the target region already, just quit. if (startIndex >= endIndex || refinementTargets.Count == targetSubtreeRefinementCount) @@ -408,7 +121,7 @@ void FindSubtreeRefinementTargets3(int nodeIndex, int leftLeafCount, int subtree else { //Too big to be a candidate; traverse further. - FindSubtreeRefinementTargets3(node.A.Index, leftLeafCount, subtreeRefinementSize, targetSubtreeRefinementCount, ref startIndex, endIndex, ref refinementTargets); + FindSubtreeRefinementTargets(node.A.Index, leftLeafCount, subtreeRefinementSize, targetSubtreeRefinementCount, ref startIndex, endIndex, ref refinementTargets); } } @@ -430,118 +143,24 @@ void FindSubtreeRefinementTargets3(int nodeIndex, int leftLeafCount, int subtree else { //Too big to be a candidate; traverse further. - FindSubtreeRefinementTargets3(node.B.Index, leftLeafCount + node.A.LeafCount, subtreeRefinementSize, targetSubtreeRefinementCount, ref startIndex, endIndex, ref refinementTargets); + FindSubtreeRefinementTargets(node.B.Index, leftLeafCount + node.A.LeafCount, subtreeRefinementSize, targetSubtreeRefinementCount, ref startIndex, endIndex, ref refinementTargets); } } } - void FindSubtreeRefinementTargets3(int subtreeRefinementSize, int targetSubtreeRefinementCount, ref int startIndex, ref QuickList refinementTargets) + void FindSubtreeRefinementTargets(int subtreeRefinementSize, int targetSubtreeRefinementCount, ref int startIndex, ref QuickList refinementTargets) { var initialStart = startIndex; - FindSubtreeRefinementTargets3(0, 0, subtreeRefinementSize, targetSubtreeRefinementCount, ref startIndex, LeafCount, ref refinementTargets); + FindSubtreeRefinementTargets(0, 0, subtreeRefinementSize, targetSubtreeRefinementCount, ref startIndex, LeafCount, ref refinementTargets); if (startIndex >= LeafCount && refinementTargets.Count < targetSubtreeRefinementCount) { //Hit the end of the tree. Reset. startIndex = 0; var remainingLeaves = LeafCount - initialStart; - FindSubtreeRefinementTargets3(0, 0, subtreeRefinementSize, targetSubtreeRefinementCount, ref startIndex, remainingLeaves, ref refinementTargets); + FindSubtreeRefinementTargets(0, 0, subtreeRefinementSize, targetSubtreeRefinementCount, ref startIndex, remainingLeaves, ref refinementTargets); } } - int FindSubtreeRefinementTargets2(int startIndex, int subtreeRefinementSize, int targetSubtreeRefinementCount, ref QuickList refinementTargets) - { - //The goal here is to select candidates which have two properties: - //1. The total number of leaves to the left of the node is less than or equal to the startIndex. - //2. The total number of leaves in the subtree below the node is less than or equal to the targetSubtreeRefinementSize. - //We can perform this search stacklessly! - //Note an important detail: nodes beyond the first encountered meeting the threshold leaf count for being a candidate cannot be considered. - //Doing so would allow very small subtrees to be picked if the "leaves counted on the left" condition is met late. - //Instead, when encountering a node at the threshold size, we must either accept it or move on to the next node. - Debug.Assert(LeafCount >= 2, "Subtree finding assumes that the root is complete."); - Debug.Assert(refinementTargets.Span.Length >= targetSubtreeRefinementCount, "The candidates list should hold the target refinement count."); - Debug.Assert(subtreeRefinementSize > 2, "Subtree refinements must actually cover some refineable region!"); - int traversedLeaves = 0; - - int nextNodeIndex = 0; - int accumulatedLeftLeaves = 0; - while (refinementTargets.Count < targetSubtreeRefinementCount && traversedLeaves < LeafCount) - { - ref var node = ref Nodes[nextNodeIndex]; - var nodeTotalLeafCount = node.A.LeafCount + node.B.LeafCount; - //Should we go left or right? - var leafMidpointCount = accumulatedLeftLeaves + node.A.LeafCount; - if (startIndex >= accumulatedLeftLeaves && startIndex < leafMidpointCount) - { - //Go left. - if (node.A.LeafCount <= subtreeRefinementSize) - { - //A is small enough that it can be a refinement. - if (node.A.Index >= 0) - refinementTargets.AllocateUnsafely() = node.A.Index; - //The traversal may continue to find additional refinement targets. This will bump it over to the right child on the next iteration. - startIndex = leafMidpointCount; - } - else - { - //A remains large enough that it cannot yet be a refinement. - nextNodeIndex = node.A.Index; - } - } - else - { - //Go right. - accumulatedLeftLeaves += node.A.LeafCount; - traversedLeaves += node.A.LeafCount; - Debug.Assert(startIndex >= leafMidpointCount && startIndex < accumulatedLeftLeaves + nodeTotalLeafCount); - if (node.B.LeafCount <= subtreeRefinementSize) - { - //B is small enough that it can be a refinement. - if (node.B.Index >= 0) - refinementTargets.AllocateUnsafely() = node.B.Index; - //The traversal may continue to find additional refinement targets. - accumulatedLeftLeaves += node.B.LeafCount; - traversedLeaves += node.B.LeafCount; - startIndex = accumulatedLeftLeaves + node.B.LeafCount; - //We'll need to bump to the parent since we're done with this node- but note that this child might have been child B of the parent, and so on. - //Follow parent pointers until we find a node that has another child. - var bumpUpNodeIndex = Metanodes[nextNodeIndex].Parent; - var previousTotalLeafCount = nodeTotalLeafCount; - while (true) - { - if (bumpUpNodeIndex < 0) - { - //Hit the root. We've reached the end of the tree, apparently. Wrap around; restart the traversal. - startIndex = 0; - accumulatedLeftLeaves = 0; - nextNodeIndex = 0; - break; - } - //We need to back out an entire node's worth of leaves; this accumulator is how we track how far we've made it in the tree. - //If we don't back these nodes out, the tree will think it's further ahead than it should be, and it won't traverse the right child to get to the startIndex. - accumulatedLeftLeaves -= previousTotalLeafCount; - ref var bumpUpNode = ref Nodes[bumpUpNodeIndex]; - previousTotalLeafCount = bumpUpNode.A.LeafCount + bumpUpNode.B.LeafCount; - ref var metanode = ref Metanodes[bumpUpNodeIndex]; - if (metanode.IndexInParent == 0) - { - //The node is the left child of the parent, so the parent is the next node to visit. - nextNodeIndex = metanode.Parent; - break; - } - //Still need to go further. - bumpUpNodeIndex = metanode.Parent; - - } - } - else - { - //B remains large enough that it cannot yet be a refinement. - nextNodeIndex = node.B.Index; - } - } - } - return startIndex; - } bool IsNodeChildSubtreeRefinementTarget(in QuickList subtreeRefinements, in NodeChild child, int parentTotalLeafCount, int subtreeRefinementSize) { @@ -611,7 +230,7 @@ private int TryPushChildForRootRefinement( /// Target size of subtree refinements. The actual size of refinement will usually be larger or smaller. /// Pool used for ephemeral allocations during the refinement. /// Nodes will not be refit. - public void Refine3(int rootRefinementSize, ref int subtreeRefinementStartIndex, int subtreeRefinementCount, int subtreeRefinementSize, BufferPool pool) + public void Refine2(int rootRefinementSize, ref int subtreeRefinementStartIndex, int subtreeRefinementCount, int subtreeRefinementSize, BufferPool pool) { //No point refining anything with two leaves. This condition also avoids having to special case for an incomplete root node. if (LeafCount <= 2) @@ -619,17 +238,11 @@ public void Refine3(int rootRefinementSize, ref int subtreeRefinementStartIndex, //We used a vectorized containment test later, so make sure to pad out the refinement target list. var subtreeRefinementCapacity = BundleIndexing.GetBundleCount(subtreeRefinementCount) * Vector.Count; var subtreeRefinementTargets = new QuickList(subtreeRefinementCapacity, pool); - //Console.WriteLine($"Starting at {subtreeRefinementStartIndex}"); - //subtreeRefinementStartIndex = FindSubtreeRefinementTargets2(subtreeRefinementStartIndex, subtreeRefinementSize, subtreeRefinementCount, ref subtreeRefinementTargets); - FindSubtreeRefinementTargets3(subtreeRefinementSize, subtreeRefinementCount, ref subtreeRefinementStartIndex, ref subtreeRefinementTargets); + FindSubtreeRefinementTargets(subtreeRefinementSize, subtreeRefinementCount, ref subtreeRefinementStartIndex, ref subtreeRefinementTargets); //Fill the trailing slots in the list with -1 to avoid matches. ((Span)subtreeRefinementTargets.Span)[subtreeRefinementTargets.Count..].Fill(-1); - //for (int i = 0; i < subtreeRefinementTargets.Count; ++i) - //{ - // Console.Write($"{Nodes[subtreeRefinementTargets[i]].A.LeafCount + Nodes[subtreeRefinementTargets[i]].B.LeafCount}, "); - //} - //Console.WriteLine(); + //We now know which nodes are the roots of subtree refinements; the root refinement can avoid traversing through them. var rootStack = new QuickList<(int nodeIndex, int subtreeBudget)>(rootRefinementSize, pool); @@ -655,7 +268,7 @@ public void Refine3(int rootRefinementSize, ref int subtreeRefinementStartIndex, surplusSubtreeBudget += TryPushChildForRootRefinement(subtreeRefinementSize, subtreeRefinementTargets, nodeTotalLeafCount, bSubtreeBudget, node.B, ref rootStack, ref rootRefinementSubtrees); surplusSubtreeBudget += TryPushChildForRootRefinement(subtreeRefinementSize, subtreeRefinementTargets, nodeTotalLeafCount, aSubtreeBudget, node.A, ref rootStack, ref rootRefinementSubtrees); - //surplusSubtreeBudget = 0; + surplusSubtreeBudget = 0; } //Now that we have a list of nodes to refine, we can run the root refinement. diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index e5a13368f..3fe74293d 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -184,7 +184,7 @@ public override void Initialize(ContentArchive content, Camera camera) //} var start = Stopwatch.GetTimestamp(); //mesh.Tree.Refine2(8192, ref refinementState, 1, 8192, BufferPool); - mesh.Tree.Refine3(32768, ref refinementState, 1, 32768, BufferPool); + mesh.Tree.Refine2(512, ref refinementState, 1, 32768, BufferPool); var end = Stopwatch.GetTimestamp(); sum += end - start; if ((refinementIndex + 1) % 128 == 0) From d67abd7fb0772b8d8ad5484400e99f079d57ce3c Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 24 Jun 2023 14:02:38 -0500 Subject: [PATCH 739/947] Got rid of surplus rescheduling. --- BepuPhysics/Trees/Tree_Refine2.cs | 33 +++++++++++-------------------- 1 file changed, 11 insertions(+), 22 deletions(-) diff --git a/BepuPhysics/Trees/Tree_Refine2.cs b/BepuPhysics/Trees/Tree_Refine2.cs index c27cce63d..443d5e901 100644 --- a/BepuPhysics/Trees/Tree_Refine2.cs +++ b/BepuPhysics/Trees/Tree_Refine2.cs @@ -12,7 +12,7 @@ namespace BepuPhysics.Trees; public partial struct Tree { const int flagForRootRefinementSubtree = 1 << 30; - + void ReifyRootRefinementNodeChild(ref int index, ref QuickList refinementNodeIndices, int realNodeIndex, int childIndexInParent) { //Root refinements mark internal subtrees with a flag in the second to last index. @@ -60,8 +60,6 @@ void ReifyRootRefinement(QuickList refinementNodeIndices, Buffer refi } } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] void ReifyRefinementNodeChild(ref int index, ref QuickList refinementNodeIndices, int realNodeIndex, int childIndexInParent) { //Root refinements mark internal subtrees with a flag in the second to last index. @@ -183,9 +181,8 @@ bool IsNodeChildSubtreeRefinementTarget(in QuickList subtreeRefinements, in /// /// Checks if a child should be a subtree in the root refinement. If so, it's added to the list. Otherwise, it's pushed onto the stack. /// - /// The amount of surplus leaf budget accumulated by pushing this child. The value will be nonzero only if the child was a subtree refinement target. - private int TryPushChildForRootRefinement( - int subtreeRefinementSize, in QuickList subtreeRefinementRoots, int nodeTotalLeafCount, int subtreeBudget, in NodeChild child, ref QuickList<(int nodeIndex, int subtreeBudget)> stack, ref QuickList rootRefinementSubtrees) + private void TryPushChildForRootRefinement( + int subtreeRefinementSize, in QuickList subtreeRefinementRoots, int nodeTotalLeafCount, int subtreeBudget, in NodeChild child, ref QuickList<(int nodeIndex, int subtreeBudget)> stack, ref QuickList rootRefinementSubtrees) { //We automatically accept any child as a subtree for the refinement process if: //1. It's a leaf node, or @@ -209,8 +206,6 @@ private int TryPushChildForRootRefinement( //Internal nodes used as subtrees by the root refinement are flagged so that the reification process knows to stop. Debug.Assert(allocatedChild.Index < flagForRootRefinementSubtree, "The use of an upper index bit as flag means the binned refiner cannot handle trees with billions of children."); allocatedChild.Index |= flagForRootRefinementSubtree; - //Return the budget so it can be used on other nodes that follow in the traversal. Don't really care *where* it goes. - return subtreeBudget; } else { @@ -218,7 +213,6 @@ private int TryPushChildForRootRefinement( stack.AllocateUnsafely() = (child.Index, subtreeBudget); } } - return 0; } /// @@ -235,6 +229,9 @@ public void Refine2(int rootRefinementSize, ref int subtreeRefinementStartIndex, //No point refining anything with two leaves. This condition also avoids having to special case for an incomplete root node. if (LeafCount <= 2) return; + //Clamp refinement sizes to avoid pointless overallocations when the user supplies odd inputs. + rootRefinementSize = int.Min(rootRefinementSize, LeafCount); + subtreeRefinementSize = int.Min(subtreeRefinementSize, LeafCount); //We used a vectorized containment test later, so make sure to pad out the refinement target list. var subtreeRefinementCapacity = BundleIndexing.GetBundleCount(subtreeRefinementCount) * Vector.Count; var subtreeRefinementTargets = new QuickList(subtreeRefinementCapacity, pool); @@ -242,33 +239,25 @@ public void Refine2(int rootRefinementSize, ref int subtreeRefinementStartIndex, //Fill the trailing slots in the list with -1 to avoid matches. ((Span)subtreeRefinementTargets.Span)[subtreeRefinementTargets.Count..].Fill(-1); - - //We now know which nodes are the roots of subtree refinements; the root refinement can avoid traversing through them. var rootStack = new QuickList<(int nodeIndex, int subtreeBudget)>(rootRefinementSize, pool); var rootRefinementSubtrees = new QuickList(rootRefinementSize, pool); var rootRefinementNodeIndices = new QuickList(rootRefinementSize, pool); rootStack.AllocateUnsafely() = (0, rootRefinementSize); - int surplusSubtreeBudget = 0; while (rootStack.TryPop(out var nodeToVisit)) { rootRefinementNodeIndices.AllocateUnsafely() = nodeToVisit.nodeIndex; ref var node = ref Nodes[nodeToVisit.nodeIndex]; var nodeTotalLeafCount = node.A.LeafCount + node.B.LeafCount; - //The "budget" is just a mechanism for bottoming out the traversal at the same depth as a BFS, but during a depth first traversal. - var effectiveNodeBudget = int.Min(nodeTotalLeafCount, nodeToVisit.subtreeBudget + surplusSubtreeBudget); - var lowerSubtreeBudget = int.Min((effectiveNodeBudget + 1) / 2, int.Min(node.A.LeafCount, node.B.LeafCount)); - var higherSubtreeBudget = effectiveNodeBudget - lowerSubtreeBudget; + Debug.Assert(nodeToVisit.subtreeBudget <= nodeTotalLeafCount); + var lowerSubtreeBudget = int.Min((nodeToVisit.subtreeBudget + 1) / 2, int.Min(node.A.LeafCount, node.B.LeafCount)); + var higherSubtreeBudget = nodeToVisit.subtreeBudget - lowerSubtreeBudget; var useSmallerForA = lowerSubtreeBudget == node.A.LeafCount; var aSubtreeBudget = useSmallerForA ? lowerSubtreeBudget : higherSubtreeBudget; var bSubtreeBudget = useSmallerForA ? higherSubtreeBudget : lowerSubtreeBudget; - //If the effective budgets exceed the originally scheduled amount, then we've used up part of our surplus. - surplusSubtreeBudget += nodeToVisit.subtreeBudget - (aSubtreeBudget + bSubtreeBudget); - - surplusSubtreeBudget += TryPushChildForRootRefinement(subtreeRefinementSize, subtreeRefinementTargets, nodeTotalLeafCount, bSubtreeBudget, node.B, ref rootStack, ref rootRefinementSubtrees); - surplusSubtreeBudget += TryPushChildForRootRefinement(subtreeRefinementSize, subtreeRefinementTargets, nodeTotalLeafCount, aSubtreeBudget, node.A, ref rootStack, ref rootRefinementSubtrees); - surplusSubtreeBudget = 0; + TryPushChildForRootRefinement(subtreeRefinementSize, subtreeRefinementTargets, nodeTotalLeafCount, bSubtreeBudget, node.B, ref rootStack, ref rootRefinementSubtrees); + TryPushChildForRootRefinement(subtreeRefinementSize, subtreeRefinementTargets, nodeTotalLeafCount, aSubtreeBudget, node.A, ref rootStack, ref rootRefinementSubtrees); } //Now that we have a list of nodes to refine, we can run the root refinement. From 6b34ce413c37a20d2588b25fdc6acab26d054890 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 24 Jun 2023 17:35:46 -0500 Subject: [PATCH 740/947] A wee bit of refactoring. --- BepuPhysics/Trees/Tree_Refine2.cs | 90 +++++++++++-------- .../SpecializedTests/TreeFiddlingTestDemo.cs | 2 +- 2 files changed, 52 insertions(+), 40 deletions(-) diff --git a/BepuPhysics/Trees/Tree_Refine2.cs b/BepuPhysics/Trees/Tree_Refine2.cs index 443d5e901..6abb92a4e 100644 --- a/BepuPhysics/Trees/Tree_Refine2.cs +++ b/BepuPhysics/Trees/Tree_Refine2.cs @@ -160,15 +160,15 @@ void FindSubtreeRefinementTargets(int subtreeRefinementSize, int targetSubtreeRe - bool IsNodeChildSubtreeRefinementTarget(in QuickList subtreeRefinements, in NodeChild child, int parentTotalLeafCount, int subtreeRefinementSize) + bool IsNodeChildSubtreeRefinementTarget(Buffer subtreeRefinements, in NodeChild child, int parentTotalLeafCount, int subtreeRefinementSize) { //First check if it *could* be one by checking the leaf count threshold. if (child.LeafCount <= subtreeRefinementSize && parentTotalLeafCount > subtreeRefinementSize) { //It may be a subtree refinement. Do a deeper test! - var vectorCount = BundleIndexing.GetBundleCount(subtreeRefinements.Count); + var vectorCount = BundleIndexing.GetBundleCount(subtreeRefinements.Length); var search = new Vector(child.Index); - var vectors = subtreeRefinements.Span.As>(); + var vectors = subtreeRefinements.As>(); for (int i = 0; i < vectorCount; ++i) { if (Vector.EqualsAny(search, vectors[i])) @@ -182,7 +182,7 @@ bool IsNodeChildSubtreeRefinementTarget(in QuickList subtreeRefinements, in /// Checks if a child should be a subtree in the root refinement. If so, it's added to the list. Otherwise, it's pushed onto the stack. /// private void TryPushChildForRootRefinement( - int subtreeRefinementSize, in QuickList subtreeRefinementRoots, int nodeTotalLeafCount, int subtreeBudget, in NodeChild child, ref QuickList<(int nodeIndex, int subtreeBudget)> stack, ref QuickList rootRefinementSubtrees) + int subtreeRefinementSize, Buffer subtreeRefinementRoots, int nodeTotalLeafCount, int subtreeBudget, in NodeChild child, ref QuickList<(int nodeIndex, int subtreeBudget)> stack, ref QuickList rootRefinementSubtrees) { //We automatically accept any child as a subtree for the refinement process if: //1. It's a leaf node, or @@ -215,6 +215,49 @@ private void TryPushChildForRootRefinement( } } + void CollectSubtreesForRootRefinement(int rootRefinementSize, int subtreeRefinementSize, BufferPool pool, in QuickList subtreeRefinementTargets, ref QuickList rootRefinementNodeIndices, ref QuickList rootRefinementSubtrees) + { + var rootStack = new QuickList<(int nodeIndex, int subtreeBudget)>(rootRefinementSize, pool); + rootStack.AllocateUnsafely() = (0, rootRefinementSize); + var subtreeRefinementTargetsBuffer = subtreeRefinementTargets.Span[..subtreeRefinementTargets.Count]; + while (rootStack.TryPop(out var nodeToVisit)) + { + rootRefinementNodeIndices.AllocateUnsafely() = nodeToVisit.nodeIndex; + ref var node = ref Nodes[nodeToVisit.nodeIndex]; + var nodeTotalLeafCount = node.A.LeafCount + node.B.LeafCount; + Debug.Assert(nodeToVisit.subtreeBudget <= nodeTotalLeafCount); + var lowerSubtreeBudget = int.Min((nodeToVisit.subtreeBudget + 1) / 2, int.Min(node.A.LeafCount, node.B.LeafCount)); + var higherSubtreeBudget = nodeToVisit.subtreeBudget - lowerSubtreeBudget; + var useSmallerForA = lowerSubtreeBudget == node.A.LeafCount; + var aSubtreeBudget = useSmallerForA ? lowerSubtreeBudget : higherSubtreeBudget; + var bSubtreeBudget = useSmallerForA ? higherSubtreeBudget : lowerSubtreeBudget; + + TryPushChildForRootRefinement(subtreeRefinementSize, subtreeRefinementTargetsBuffer, nodeTotalLeafCount, bSubtreeBudget, node.B, ref rootStack, ref rootRefinementSubtrees); + TryPushChildForRootRefinement(subtreeRefinementSize, subtreeRefinementTargetsBuffer, nodeTotalLeafCount, aSubtreeBudget, node.A, ref rootStack, ref rootRefinementSubtrees); + } + rootStack.Dispose(pool); + } + + void CollectSubtreesForSubtreeRefinement(int refinementTargetRootNodeIndex, Buffer subtreeStackBuffer, ref QuickList subtreeRefinementNodeIndices, ref QuickList subtreeRefinementLeaves) + { + Debug.Assert(subtreeRefinementLeaves.Count == 0 && subtreeRefinementNodeIndices.Count == 0); + var subtreeStack = new QuickList(subtreeStackBuffer); + subtreeStack.AllocateUnsafely() = refinementTargetRootNodeIndex; + while (subtreeStack.TryPop(out var nodeToVisit)) + { + ref var node = ref Nodes[nodeToVisit]; + subtreeRefinementNodeIndices.AllocateUnsafely() = nodeToVisit; + if (node.B.Index >= 0) + subtreeStack.AllocateUnsafely() = node.B.Index; + else + subtreeRefinementLeaves.AllocateUnsafely() = node.B; + if (node.A.Index >= 0) + subtreeStack.AllocateUnsafely() = node.A.Index; + else + subtreeRefinementLeaves.AllocateUnsafely() = node.A; + } + } + /// /// Incrementally refines a subset of the tree by running a binned builder over subtrees. /// @@ -240,25 +283,9 @@ public void Refine2(int rootRefinementSize, ref int subtreeRefinementStartIndex, ((Span)subtreeRefinementTargets.Span)[subtreeRefinementTargets.Count..].Fill(-1); //We now know which nodes are the roots of subtree refinements; the root refinement can avoid traversing through them. - var rootStack = new QuickList<(int nodeIndex, int subtreeBudget)>(rootRefinementSize, pool); var rootRefinementSubtrees = new QuickList(rootRefinementSize, pool); var rootRefinementNodeIndices = new QuickList(rootRefinementSize, pool); - rootStack.AllocateUnsafely() = (0, rootRefinementSize); - while (rootStack.TryPop(out var nodeToVisit)) - { - rootRefinementNodeIndices.AllocateUnsafely() = nodeToVisit.nodeIndex; - ref var node = ref Nodes[nodeToVisit.nodeIndex]; - var nodeTotalLeafCount = node.A.LeafCount + node.B.LeafCount; - Debug.Assert(nodeToVisit.subtreeBudget <= nodeTotalLeafCount); - var lowerSubtreeBudget = int.Min((nodeToVisit.subtreeBudget + 1) / 2, int.Min(node.A.LeafCount, node.B.LeafCount)); - var higherSubtreeBudget = nodeToVisit.subtreeBudget - lowerSubtreeBudget; - var useSmallerForA = lowerSubtreeBudget == node.A.LeafCount; - var aSubtreeBudget = useSmallerForA ? lowerSubtreeBudget : higherSubtreeBudget; - var bSubtreeBudget = useSmallerForA ? higherSubtreeBudget : lowerSubtreeBudget; - - TryPushChildForRootRefinement(subtreeRefinementSize, subtreeRefinementTargets, nodeTotalLeafCount, bSubtreeBudget, node.B, ref rootStack, ref rootRefinementSubtrees); - TryPushChildForRootRefinement(subtreeRefinementSize, subtreeRefinementTargets, nodeTotalLeafCount, aSubtreeBudget, node.A, ref rootStack, ref rootRefinementSubtrees); - } + CollectSubtreesForRootRefinement(rootRefinementSize, subtreeRefinementSize, pool, subtreeRefinementTargets, ref rootRefinementNodeIndices, ref rootRefinementSubtrees); //Now that we have a list of nodes to refine, we can run the root refinement. Debug.Assert(rootRefinementNodeIndices.Count == rootRefinementSubtrees.Count - 1); @@ -273,29 +300,14 @@ public void Refine2(int rootRefinementSize, ref int subtreeRefinementStartIndex, rootRefinementSubtrees.Dispose(pool); rootRefinementNodeIndices.Dispose(pool); - rootStack.Dispose(pool); var subtreeRefinementNodeIndices = new QuickList(subtreeRefinementSize, pool); var subtreeRefinementLeaves = new QuickList(subtreeRefinementSize, pool); - var subtreeStack = new QuickList(subtreeRefinementSize, pool); + var subtreeStackBuffer = new Buffer(subtreeRefinementSize, pool); for (int i = 0; i < subtreeRefinementTargets.Count; ++i) { //Accumulate nodes and leaves with a prepass. - Debug.Assert(subtreeStack.Count == 0 && subtreeRefinementNodeIndices.Count == 0); - subtreeStack.AllocateUnsafely() = subtreeRefinementTargets[i]; - while (subtreeStack.TryPop(out var nodeToVisit)) - { - ref var node = ref Nodes[nodeToVisit]; - subtreeRefinementNodeIndices.AllocateUnsafely() = nodeToVisit; - if (node.B.Index >= 0) - subtreeStack.AllocateUnsafely() = node.B.Index; - else - subtreeRefinementLeaves.AllocateUnsafely() = node.B; - if (node.A.Index >= 0) - subtreeStack.AllocateUnsafely() = node.A.Index; - else - subtreeRefinementLeaves.AllocateUnsafely() = node.A; - } + CollectSubtreesForSubtreeRefinement(subtreeRefinementTargets[i], subtreeStackBuffer, ref subtreeRefinementNodeIndices, ref subtreeRefinementLeaves); var refinementNodes = refinementNodesAllocation.Slice(0, subtreeRefinementNodeIndices.Count); var refinementMetanodes = refinementMetanodesAllocation.Slice(0, subtreeRefinementNodeIndices.Count); @@ -310,7 +322,7 @@ public void Refine2(int rootRefinementSize, ref int subtreeRefinementStartIndex, subtreeRefinementNodeIndices.Dispose(pool); subtreeRefinementLeaves.Dispose(pool); subtreeRefinementTargets.Dispose(pool); - subtreeStack.Dispose(pool); + subtreeStackBuffer.Dispose(pool); refinementNodesAllocation.Dispose(pool); refinementMetanodesAllocation.Dispose(pool); } diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index 3fe74293d..311458b08 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -184,7 +184,7 @@ public override void Initialize(ContentArchive content, Camera camera) //} var start = Stopwatch.GetTimestamp(); //mesh.Tree.Refine2(8192, ref refinementState, 1, 8192, BufferPool); - mesh.Tree.Refine2(512, ref refinementState, 1, 32768, BufferPool); + mesh.Tree.Refine2(8192, ref refinementState, 32, 1024, BufferPool); var end = Stopwatch.GetTimestamp(); sum += end - start; if ((refinementIndex + 1) % 128 == 0) From 7d79cca995fc4572fe96e7e3bf334f34be960795 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 24 Jun 2023 17:47:46 -0500 Subject: [PATCH 741/947] Vectorized access oops. --- BepuPhysics/Trees/Tree_Refine2.cs | 22 ++++++++++------------ 1 file changed, 10 insertions(+), 12 deletions(-) diff --git a/BepuPhysics/Trees/Tree_Refine2.cs b/BepuPhysics/Trees/Tree_Refine2.cs index 6abb92a4e..a5c9563a2 100644 --- a/BepuPhysics/Trees/Tree_Refine2.cs +++ b/BepuPhysics/Trees/Tree_Refine2.cs @@ -160,18 +160,16 @@ void FindSubtreeRefinementTargets(int subtreeRefinementSize, int targetSubtreeRe - bool IsNodeChildSubtreeRefinementTarget(Buffer subtreeRefinements, in NodeChild child, int parentTotalLeafCount, int subtreeRefinementSize) + bool IsNodeChildSubtreeRefinementTarget(Buffer> subtreeRefinementBundles, in NodeChild child, int parentTotalLeafCount, int subtreeRefinementSize) { //First check if it *could* be one by checking the leaf count threshold. if (child.LeafCount <= subtreeRefinementSize && parentTotalLeafCount > subtreeRefinementSize) { //It may be a subtree refinement. Do a deeper test! - var vectorCount = BundleIndexing.GetBundleCount(subtreeRefinements.Length); var search = new Vector(child.Index); - var vectors = subtreeRefinements.As>(); - for (int i = 0; i < vectorCount; ++i) + for (int i = 0; i < subtreeRefinementBundles.Length; ++i) { - if (Vector.EqualsAny(search, vectors[i])) + if (Vector.EqualsAny(search, subtreeRefinementBundles[i])) return true; } } @@ -182,7 +180,7 @@ bool IsNodeChildSubtreeRefinementTarget(Buffer subtreeRefinements, in NodeC /// Checks if a child should be a subtree in the root refinement. If so, it's added to the list. Otherwise, it's pushed onto the stack. /// private void TryPushChildForRootRefinement( - int subtreeRefinementSize, Buffer subtreeRefinementRoots, int nodeTotalLeafCount, int subtreeBudget, in NodeChild child, ref QuickList<(int nodeIndex, int subtreeBudget)> stack, ref QuickList rootRefinementSubtrees) + int subtreeRefinementSize, Buffer> subtreeRefinementRootBundles, int nodeTotalLeafCount, int subtreeBudget, in NodeChild child, ref QuickList<(int nodeIndex, int subtreeBudget)> stack, ref QuickList rootRefinementSubtrees) { //We automatically accept any child as a subtree for the refinement process if: //1. It's a leaf node, or @@ -198,7 +196,7 @@ private void TryPushChildForRootRefinement( else { //Internal node; is it a subtree refinement? - if (IsNodeChildSubtreeRefinementTarget(subtreeRefinementRoots, child, nodeTotalLeafCount, subtreeRefinementSize)) + if (IsNodeChildSubtreeRefinementTarget(subtreeRefinementRootBundles, child, nodeTotalLeafCount, subtreeRefinementSize)) { //Yup! ref var allocatedChild = ref rootRefinementSubtrees.AllocateUnsafely(); @@ -215,11 +213,11 @@ private void TryPushChildForRootRefinement( } } - void CollectSubtreesForRootRefinement(int rootRefinementSize, int subtreeRefinementSize, BufferPool pool, in QuickList subtreeRefinementTargets, ref QuickList rootRefinementNodeIndices, ref QuickList rootRefinementSubtrees) + unsafe void CollectSubtreesForRootRefinement(int rootRefinementSize, int subtreeRefinementSize, BufferPool pool, in QuickList subtreeRefinementTargets, ref QuickList rootRefinementNodeIndices, ref QuickList rootRefinementSubtrees) { var rootStack = new QuickList<(int nodeIndex, int subtreeBudget)>(rootRefinementSize, pool); rootStack.AllocateUnsafely() = (0, rootRefinementSize); - var subtreeRefinementTargetsBuffer = subtreeRefinementTargets.Span[..subtreeRefinementTargets.Count]; + var subtreeRefinementTargetBundles = new Buffer>(subtreeRefinementTargets.Span.Memory, BundleIndexing.GetBundleCount(subtreeRefinementTargets.Count)); while (rootStack.TryPop(out var nodeToVisit)) { rootRefinementNodeIndices.AllocateUnsafely() = nodeToVisit.nodeIndex; @@ -232,8 +230,8 @@ void CollectSubtreesForRootRefinement(int rootRefinementSize, int subtreeRefinem var aSubtreeBudget = useSmallerForA ? lowerSubtreeBudget : higherSubtreeBudget; var bSubtreeBudget = useSmallerForA ? higherSubtreeBudget : lowerSubtreeBudget; - TryPushChildForRootRefinement(subtreeRefinementSize, subtreeRefinementTargetsBuffer, nodeTotalLeafCount, bSubtreeBudget, node.B, ref rootStack, ref rootRefinementSubtrees); - TryPushChildForRootRefinement(subtreeRefinementSize, subtreeRefinementTargetsBuffer, nodeTotalLeafCount, aSubtreeBudget, node.A, ref rootStack, ref rootRefinementSubtrees); + TryPushChildForRootRefinement(subtreeRefinementSize, subtreeRefinementTargetBundles, nodeTotalLeafCount, bSubtreeBudget, node.B, ref rootStack, ref rootRefinementSubtrees); + TryPushChildForRootRefinement(subtreeRefinementSize, subtreeRefinementTargetBundles, nodeTotalLeafCount, aSubtreeBudget, node.A, ref rootStack, ref rootRefinementSubtrees); } rootStack.Dispose(pool); } @@ -307,7 +305,7 @@ public void Refine2(int rootRefinementSize, ref int subtreeRefinementStartIndex, for (int i = 0; i < subtreeRefinementTargets.Count; ++i) { //Accumulate nodes and leaves with a prepass. - CollectSubtreesForSubtreeRefinement(subtreeRefinementTargets[i], subtreeStackBuffer, ref subtreeRefinementNodeIndices, ref subtreeRefinementLeaves); + CollectSubtreesForSubtreeRefinement(subtreeRefinementTargets[i], subtreeStackBuffer, ref subtreeRefinementNodeIndices, ref subtreeRefinementLeaves); var refinementNodes = refinementNodesAllocation.Slice(0, subtreeRefinementNodeIndices.Count); var refinementMetanodes = refinementMetanodesAllocation.Slice(0, subtreeRefinementNodeIndices.Count); From 0ee22dd70fc74a622c1e435b3872a61a2178016e Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 24 Jun 2023 18:46:43 -0500 Subject: [PATCH 742/947] Small cache optimization bug. --- BepuPhysics/Trees/Tree_CacheOptimizer.cs | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/BepuPhysics/Trees/Tree_CacheOptimizer.cs b/BepuPhysics/Trees/Tree_CacheOptimizer.cs index e9a2f8661..931711f79 100644 --- a/BepuPhysics/Trees/Tree_CacheOptimizer.cs +++ b/BepuPhysics/Trees/Tree_CacheOptimizer.cs @@ -164,10 +164,10 @@ public unsafe void CacheOptimizeLimitedSubtree(int nodeIndex, int nodeOptimizati } /// - /// Puts all nodes starting from the given node index into depth traversal order. + /// Puts all nodes starting from the given node index into depth first traversal order. /// If the count is larger than the number of nodes beneath the starting node, the optimization will stop early. /// - /// + /// Node index to start the optimization at. /// Number of nodes to try to optimize. /// Number of nodes optimized. public unsafe int CacheOptimizeRegion(int startingNodeIndex, int targetCount) @@ -179,13 +179,15 @@ public unsafe int CacheOptimizeRegion(int startingNodeIndex, int targetCount) SwapNodes(targetNodeIndex, startingNodeIndex); ref var startNode = ref Nodes[targetNodeIndex]; - var nodeCount = int.Min(startNode.A.LeafCount + startNode.B.LeafCount - 1, targetCount); + //Note minus 2: visiting the parent of the last node is sufficient to put the last node into position. + var nodeCount = int.Min(startNode.A.LeafCount + startNode.B.LeafCount - 2, targetCount); for (int i = 0; i < nodeCount; ++i) { int parentIndex = targetNodeIndex + i; ref var node = ref Nodes[parentIndex]; //Put both children into depth first order before continuing. var targetLocationA = parentIndex + 1; + var targetLocationB = parentIndex + node.A.LeafCount; if (node.A.Index >= 0 && node.A.Index != targetLocationA) SwapNodes(node.A.Index, targetLocationA); @@ -193,6 +195,5 @@ public unsafe int CacheOptimizeRegion(int startingNodeIndex, int targetCount) SwapNodes(node.B.Index, targetLocationB); } return nodeCount; - } } From 4bcefa59060047f75f4fd03122496c58e27f1746 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 25 Jun 2023 18:23:49 -0500 Subject: [PATCH 743/947] Basic interrefinement multithreading; no per-refinement-region multithreading yet. --- BepuPhysics/Trees/Tree_Refine2.cs | 150 ++++++++++++++++++ .../SpecializedTests/TreeFiddlingTestDemo.cs | 9 +- 2 files changed, 157 insertions(+), 2 deletions(-) diff --git a/BepuPhysics/Trees/Tree_Refine2.cs b/BepuPhysics/Trees/Tree_Refine2.cs index a5c9563a2..529e34bf7 100644 --- a/BepuPhysics/Trees/Tree_Refine2.cs +++ b/BepuPhysics/Trees/Tree_Refine2.cs @@ -1,10 +1,13 @@ using BepuUtilities; using BepuUtilities.Collections; using BepuUtilities.Memory; +using BepuUtilities.TaskScheduling; using System; +using System.ComponentModel.Design; using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; +using System.Threading; using System.Xml.Linq; namespace BepuPhysics.Trees; @@ -325,4 +328,151 @@ public void Refine2(int rootRefinementSize, ref int subtreeRefinementStartIndex, refinementMetanodesAllocation.Dispose(pool); } + + struct RefinementContext + { + public int RootRefinementSize; + public int SubtreeRefinementSize; + public QuickList SubtreeRefinementTargets; + /// + /// This is a *copy* of the original tree that spawned this refinement. Refinements do not modify the memory stored at the level of the Tree, only memory *pointed* to by the tree. + /// + public Tree Tree; + } + unsafe static void ExecuteRootRefinementTask(long id, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) + { + ref var context = ref *(RefinementContext*)untypedContext; + var pool = dispatcher.WorkerPools[workerIndex]; + + //We now know which nodes are the roots of subtree refinements; the root refinement can avoid traversing through them. + var rootRefinementSubtrees = new QuickList(context.RootRefinementSize, pool); + var rootRefinementNodeIndices = new QuickList(context.RootRefinementSize, pool); + context.Tree.CollectSubtreesForRootRefinement(context.RootRefinementSize, context.SubtreeRefinementSize, pool, context.SubtreeRefinementTargets, ref rootRefinementNodeIndices, ref rootRefinementSubtrees); + + //Now that we have a list of nodes to refine, we can run the root refinement. + Debug.Assert(rootRefinementNodeIndices.Count == rootRefinementSubtrees.Count - 1); + var rootRefinementNodes = new Buffer(rootRefinementNodeIndices.Count, pool); + var rootRefinementMetanodes = new Buffer(rootRefinementNodeIndices.Count, pool); + + //Passing 'default' for the leaves tells the binned builder to not worry about updating leaves. + BinnedBuild(rootRefinementSubtrees, rootRefinementNodes, rootRefinementMetanodes, default, null, pool); + context.Tree.ReifyRootRefinement(rootRefinementNodeIndices, rootRefinementNodes); + rootRefinementSubtrees.Dispose(pool); + rootRefinementNodeIndices.Dispose(pool); + rootRefinementNodes.Dispose(pool); + rootRefinementMetanodes.Dispose(pool); + } + unsafe static void ExecuteSubtreeRefinementTask(long subtreeRefinementTarget, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) + { + ref var context = ref *(RefinementContext*)untypedContext; + var pool = dispatcher.WorkerPools[workerIndex]; + + var subtreeRefinementNodeIndices = new QuickList(context.SubtreeRefinementSize, pool); + var subtreeRefinementLeaves = new QuickList(context.SubtreeRefinementSize, pool); + var subtreeStackBuffer = new Buffer(context.SubtreeRefinementSize, pool); + + //Accumulate nodes and leaves with a prepass. + context.Tree.CollectSubtreesForSubtreeRefinement((int)subtreeRefinementTarget, subtreeStackBuffer, ref subtreeRefinementNodeIndices, ref subtreeRefinementLeaves); + + var refinementNodes = new Buffer(subtreeRefinementNodeIndices.Count, pool); + var refinementMetanodes = new Buffer(subtreeRefinementNodeIndices.Count, pool); + //Passing 'default' for the leaves tells the binned builder to not worry about updating leaves. + BinnedBuild(subtreeRefinementLeaves, refinementNodes, refinementMetanodes, default, null, pool); + context.Tree.ReifyRefinement(subtreeRefinementNodeIndices, refinementNodes); + + refinementNodes.Dispose(pool); + refinementMetanodes.Dispose(pool); + subtreeRefinementNodeIndices.Dispose(pool); + subtreeRefinementLeaves.Dispose(pool); + subtreeStackBuffer.Dispose(pool); + + } + static unsafe void ExecuteWorker(int workerIndex, IThreadDispatcher dispatcher) + { + var taskStack = (TaskStack*)dispatcher.UnmanagedContext; + PopTaskResult popTaskResult; + var waiter = new SpinWait(); + while ((popTaskResult = taskStack->TryPopAndRun(workerIndex, dispatcher)) != PopTaskResult.Stop) + { + waiter.SpinOnce(-1); + } + } + + static unsafe void StopStackOnCompletion(long id, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) + { + ((TaskStack*)untypedContext)->RequestStop(); + } + + + /// + /// Incrementally refines a subset of the tree by running a binned builder over subtrees. + /// + /// Size of the refinement run on nodes near the root. + /// Index used to distribute subtree refinements over multiple executions. + /// Number of subtree refinements to execute. + /// Target size of subtree refinements. The actual size of refinement will usually be larger or smaller. + /// Pool used for ephemeral allocations during the refinement. + /// Nodes will not be refit. + private unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementStartIndex, int subtreeRefinementCount, int subtreeRefinementSize, BufferPool pool, int workerIndex, TaskStack* taskStack, IThreadDispatcher threadDispatcher, bool internallyDispatch) + { + //No point refining anything with two leaves. This condition also avoids having to special case for an incomplete root node. + if (LeafCount <= 2) + return; + //Clamp refinement sizes to avoid pointless overallocations when the user supplies odd inputs. + rootRefinementSize = int.Min(rootRefinementSize, LeafCount); + subtreeRefinementSize = int.Min(subtreeRefinementSize, LeafCount); + //We used a vectorized containment test later, so make sure to pad out the refinement target list. + var subtreeRefinementCapacity = BundleIndexing.GetBundleCount(subtreeRefinementCount) * Vector.Count; + var subtreeRefinementTargets = new QuickList(subtreeRefinementCapacity, pool); + FindSubtreeRefinementTargets(subtreeRefinementSize, subtreeRefinementCount, ref subtreeRefinementStartIndex, ref subtreeRefinementTargets); + //Fill the trailing slots in the list with -1 to avoid matches. + ((Span)subtreeRefinementTargets.Span)[subtreeRefinementTargets.Count..].Fill(-1); + + var tasks = new Buffer(1 + subtreeRefinementTargets.Count, pool); + var context = new RefinementContext + { + RootRefinementSize = rootRefinementSize, + SubtreeRefinementSize = subtreeRefinementSize, + SubtreeRefinementTargets = subtreeRefinementTargets, + Tree = this + }; + for (int i = 0; i < subtreeRefinementTargets.Count; ++i) + { + tasks[i] = new Task(&ExecuteSubtreeRefinementTask, &context, subtreeRefinementTargets[i]); + } + tasks[^1] = new Task(&ExecuteRootRefinementTask, &context); + if (internallyDispatch) + { + //There isn't an active dispatch, so we need to do it. + taskStack->AllocateContinuationAndPush(tasks, workerIndex, threadDispatcher, onComplete: new Task(&StopStackOnCompletion, taskStack)); + threadDispatcher.DispatchWorkers(&ExecuteWorker, unmanagedContext: taskStack); + } + else + { + //We're executing from within a multithreaded dispatch already, so we can simply run the tasks and trust that other threads are ready to steal. + taskStack->RunTasks(tasks, workerIndex, threadDispatcher); + } + tasks.Dispose(pool); + + subtreeRefinementTargets.Dispose(pool); + } + + /// + /// Incrementally refines a subset of the tree by running a binned builder over subtrees. + /// + /// Size of the refinement run on nodes near the root. + /// Index used to distribute subtree refinements over multiple executions. + /// Number of subtree refinements to execute. + /// Target size of subtree refinements. The actual size of refinement will usually be larger or smaller. + /// Pool used for ephemeral allocations during the refinement. + /// Nodes will not be refit. + public unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementStartIndex, int subtreeRefinementCount, int subtreeRefinementSize, BufferPool pool, IThreadDispatcher threadDispatcher) + { + //No point refining anything with two leaves. This condition also avoids having to special case for an incomplete root node. + if (LeafCount <= 2) + return; + var taskStack = new TaskStack(pool, threadDispatcher, threadDispatcher.ThreadCount); + Refine2(rootRefinementSize, ref subtreeRefinementStartIndex, subtreeRefinementCount, subtreeRefinementSize, pool, 0, &taskStack, threadDispatcher, true); + taskStack.Dispose(pool, threadDispatcher); + } } diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index 311458b08..f50dd7335 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -10,6 +10,7 @@ using BepuPhysics; using BepuPhysics.Constraints; using BepuPhysics.Collidables; +using BepuUtilities.TaskScheduling; namespace Demos.SpecializedTests; @@ -167,6 +168,7 @@ public override void Initialize(ContentArchive content, Camera camera) int refinementState = 0; long sum = 0; int cacheOptimizationStart = 0; + var taskStack = new TaskStack(BufferPool, ThreadDispatcher, ThreadDispatcher.ThreadCount); for (int refinementIndex = 0; refinementIndex < 16384; ++refinementIndex) { //mesh.Tree.CacheOptimizeLimitedSubtree(0, 4096); @@ -182,9 +184,11 @@ public override void Initialize(ContentArchive content, Camera camera) // if (cacheOptimizationStart >= mesh.Tree.NodeCount) // cacheOptimizationStart -= mesh.Tree.NodeCount; //} + var start = Stopwatch.GetTimestamp(); - //mesh.Tree.Refine2(8192, ref refinementState, 1, 8192, BufferPool); - mesh.Tree.Refine2(8192, ref refinementState, 32, 1024, BufferPool); + //mesh.Tree.Refine2(8192, ref refinementState, 0, 8192, BufferPool); + //mesh.Tree.Refine2(8192, ref refinementState, 32, 1024, BufferPool); + mesh.Tree.Refine2(8192, ref refinementState, 32, 4096, BufferPool, ThreadDispatcher); var end = Stopwatch.GetTimestamp(); sum += end - start; if ((refinementIndex + 1) % 128 == 0) @@ -195,6 +199,7 @@ public override void Initialize(ContentArchive content, Camera camera) Console.WriteLine($"Time (average) (ms): {(end - start) * 1e3 / Stopwatch.Frequency}, {sum * 1e3 / ((refinementIndex + 1) * Stopwatch.Frequency)}"); } } + taskStack.Dispose(BufferPool, ThreadDispatcher); Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); Console.WriteLine($"node count: {mesh.Tree.NodeCount}"); From 0f6ad0c9d02f3cd7a4ca70af8989e011c8bc1334 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 25 Jun 2023 20:31:25 -0500 Subject: [PATCH 744/947] Some refactoring on the BinnedBuild entrypoint to enable intrarefinement multithreading. --- BepuPhysics/Collidables/BigCompound.cs | 4 +- BepuPhysics/Collidables/Mesh.cs | 4 +- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 132 ++++++++++++++---------- BepuPhysics/Trees/Tree_Refine2.cs | 10 +- 4 files changed, 84 insertions(+), 66 deletions(-) diff --git a/BepuPhysics/Collidables/BigCompound.cs b/BepuPhysics/Collidables/BigCompound.cs index 2df265532..ec15527c4 100644 --- a/BepuPhysics/Collidables/BigCompound.cs +++ b/BepuPhysics/Collidables/BigCompound.cs @@ -104,12 +104,12 @@ public unsafe static BigCompound CreateWithSweepBuild(Buffer chil /// Pool to use to allocate acceleration structures. /// Thread dispatcher to use to multithread the acceleration structure build, if any. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public BigCompound(Buffer children, Shapes shapes, BufferPool pool, IThreadDispatcher dispatcher = null) + public unsafe BigCompound(Buffer children, Shapes shapes, BufferPool pool, IThreadDispatcher dispatcher = null) { this = CreateWithoutTreeBuild(children, pool); pool.Take(children.Length, out Buffer subtrees); FillSubtreesForChildren(children, shapes, subtrees); - Tree.BinnedBuild(subtrees, dispatcher, pool); + Tree.BinnedBuild(subtrees, pool, dispatcher); pool.Return(ref subtrees); } diff --git a/BepuPhysics/Collidables/Mesh.cs b/BepuPhysics/Collidables/Mesh.cs index 4201f499a..0c420b5a8 100644 --- a/BepuPhysics/Collidables/Mesh.cs +++ b/BepuPhysics/Collidables/Mesh.cs @@ -144,12 +144,12 @@ public unsafe static Mesh CreateWithSweepBuild(Buffer triangles, Vecto /// Note that the scale is not baked into the triangles or acceleration structure; the same set of triangles and acceleration structure can be used across multiple Mesh instances with different scales. /// Pool used to allocate acceleration structures. /// Dispatcher to use to multithread the execution of the mesh build process. If null, the build will be single threaded. - public Mesh(Buffer triangles, Vector3 scale, BufferPool pool, IThreadDispatcher dispatcher = null) + public unsafe Mesh(Buffer triangles, Vector3 scale, BufferPool pool, IThreadDispatcher dispatcher = null) { this = CreateWithoutTreeBuild(triangles, scale, pool); pool.Take(triangles.Length, out var subtrees); FillSubtreesForTriangles(triangles, subtrees); - Tree.BinnedBuild(subtrees, dispatcher: dispatcher, pool: pool); + Tree.BinnedBuild(subtrees, pool, dispatcher); pool.Return(ref subtrees); } diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index c9e792fa8..6451dea9c 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -1247,7 +1247,9 @@ static unsafe void BinnedBuildNode( /// If a default-valued (unallocated) buffer is passed in, the binned builder will ignore leaves. /// Buffer to be used for caching bin indices during execution. If subtreesPong is defined, binIndices must also be defined, and vice versa. /// Thread dispatcher used to accelerate the build process. - /// Task stack used to accelerate the build process. Can be null; one will be created if not provided. + /// Task stack being used to run the build process, if any. + /// If provided, the builder assumes the refinement is running within an existing multithreaded dispatch and will not call IThreadDispatcher.DispatchWorkers. + /// If null, the builder will create its own task stack and call IThreadDispatcher.DispatchWorkers internally. /// Number of workers to be used in the builder. /// Buffer pool used to preallocate temporary resources for building. /// Minimum number of bins the builder should use per node. @@ -1311,68 +1313,78 @@ static unsafe void BinnedBuilderInternal(Buffer subtrees, Buffer(allocatedBinCount * workerCount * (sizeof(BoundingBox4) * 4 + sizeof(int)), out var workerBinsAllocation); + + BinnedBuildWorkerContext* workerContextsPointer = stackalloc BinnedBuildWorkerContext[workerCount]; + var workerContexts = new Buffer(workerContextsPointer, workerCount); + + int binAllocationStart = 0; + for (int i = 0; i < workerCount; ++i) { - //There's a task queue; we should use a multithreaded dispatch. - //While we could allocate on the stack with reasonable safety in the single threaded path, that's not very reasonable for the multithreaded path. - //Each worker thread could be given a node job which executes asynchronously with respect to other node jobs. - //Those node jobs could spawn multithreaded work that other workers assist with. - //Each of those jobs needs its own context for those workers, and the number of jobs is not 1:1 with the workers. - //We'll handle such dispatch-required allocations from worker pools. Here, we just preallocate stuff for the first level across all workers. - pool.Take(allocatedBinCount * workerCount * (sizeof(BoundingBox4) * 4 + sizeof(int)), out var workerBinsAllocation); - - BinnedBuildWorkerContext* workerContextsPointer = stackalloc BinnedBuildWorkerContext[workerCount]; - var workerContexts = new Buffer(workerContextsPointer, workerCount); - - int binAllocationStart = 0; - for (int i = 0; i < workerCount; ++i) - { - workerContexts[i] = new BinnedBuildWorkerContext(workerBinsAllocation, ref binAllocationStart, allocatedBinCount); - } + workerContexts[i] = new BinnedBuildWorkerContext(workerBinsAllocation, ref binAllocationStart, allocatedBinCount); + } - TaskStack taskStack = default; - bool createdTaskQueueLocally = taskStackPointer == null; - if (taskStackPointer == null) - { - taskStack = new TaskStack(pool, dispatcher, workerCount); - taskStackPointer = &taskStack; - } - var threading = new MultithreadBinnedBuildContext - { - TopLevelTargetTaskCount = workerCount, - OriginalSubtreeCount = subtrees.Length, - TaskStack = taskStackPointer, - Workers = workerContexts, - }; - if (leaves.Allocated) - { - var context = new Context, MultithreadBinnedBuildContext>( - minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold, - subtrees, subtreesPong, leaves, nodes, metanodes, binIndices, threading); + TaskStack taskStack; + bool dispatchInternally = taskStackPointer == null; + if (dispatchInternally) + { + taskStack = new TaskStack(pool, dispatcher, workerCount); + taskStackPointer = &taskStack; + } + var threading = new MultithreadBinnedBuildContext + { + TopLevelTargetTaskCount = workerCount, + OriginalSubtreeCount = subtrees.Length, + TaskStack = taskStackPointer, + Workers = workerContexts, + }; + if (leaves.Allocated) + { + var context = new Context, MultithreadBinnedBuildContext>( + minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold, + subtrees, subtreesPong, leaves, nodes, metanodes, binIndices, threading); + if (dispatchInternally) + { taskStackPointer->PushUnsafely(new Task(&BinnedBuilderWorkerEntry>, &context), 0, dispatcher); dispatcher.DispatchWorkers(&BinnedBuilderWorkerFunction>, unmanagedContext: taskStackPointer, maximumWorkerCount: workerCount); } else { - var context = new Context( - minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold, - subtrees, subtreesPong, default, nodes, metanodes, binIndices, threading); - - taskStackPointer->PushUnsafely(new Task(&BinnedBuilderWorkerEntry, &context), 0, dispatcher); - dispatcher.DispatchWorkers(&BinnedBuilderWorkerFunction, unmanagedContext: taskStackPointer, maximumWorkerCount: workerCount); + BinnedBuildNode(false, 0, 0, context.SubtreesPing.Length, -1, -1, default, &context, 0, dispatcher); } - - if (createdTaskQueueLocally) - taskStack.Dispose(pool, dispatcher); - pool.Return(ref workerBinsAllocation); } else { - //TODO: TaskStackPointer-only path? - throw new NotImplementedException("Haven't yet implemented this path, or decided if it should really exist!"); + var context = new Context( + minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold, + subtrees, subtreesPong, default, nodes, metanodes, binIndices, threading); + + if (dispatchInternally) + { + taskStackPointer->PushUnsafely(new Task(&BinnedBuilderWorkerEntry, &context), 0, dispatcher); + dispatcher.DispatchWorkers(&BinnedBuilderWorkerFunction, unmanagedContext: taskStackPointer, maximumWorkerCount: workerCount); + } + else + { + BinnedBuildNode(false, 0, 0, context.SubtreesPing.Length, -1, -1, default, &context, 0, dispatcher); + } } + + if (dispatchInternally) + taskStackPointer->Dispose(pool, dispatcher); + pool.Return(ref workerBinsAllocation); + } } @@ -1388,10 +1400,10 @@ unsafe static void BinnedBuilderWorkerEntry(long taskId, void* untypedC unsafe static void BinnedBuilderWorkerFunction(int workerIndex, IThreadDispatcher dispatcher) where TLeaves : unmanaged { - var taskQueue = (TaskStack*)dispatcher.UnmanagedContext; + var taskStack = (TaskStack*)dispatcher.UnmanagedContext; PopTaskResult popTaskResult; var waiter = new SpinWait(); - while ((popTaskResult = taskQueue->TryPopAndRun(workerIndex, dispatcher)) != PopTaskResult.Stop) + while ((popTaskResult = taskStack->TryPopAndRun(workerIndex, dispatcher)) != PopTaskResult.Stop) { waiter.SpinOnce(-1); } @@ -1410,8 +1422,11 @@ unsafe static void BinnedBuilderWorkerFunction(int workerIndex, IThread /// Buffer holding the leaf references created by the build process. /// The indices written by the build process are those defined in the inputs; any that is negative is encoded according to and points into the leaf buffer. /// If a default-valued (unallocated) buffer is passed in, the binned builder will ignore leaves. - /// Dispatcher used to multithread the execution of the build. If the dispatcher is not null, pool must also not be null. /// Buffer pool used to preallocate a pingpong buffer if the number of subtrees exceeds maximumSubtreeStackAllocationCount. If null, stack allocation or a slower in-place partitioning will be used. + /// Dispatcher used to multithread the execution of the build. If the dispatcher is not null, pool must also not be null. + /// Task stack being used to run the build process, if any. + /// If provided, the builder assumes the refinement is running within an existing multithreaded dispatch and will not call IThreadDispatcher.DispatchWorkers. + /// If null, the builder will create its own task stack and call IThreadDispatcher.DispatchWorkers internally. /// A pool must be provided if a thread dispatcher is given. /// Maximum number of subtrees to try putting on the stack for the binned builder's pong buffers. /// Subtree counts larger than this threshold will either resort to a buffer pool allocation (if available) or slower in-place partition operations. @@ -1420,7 +1435,7 @@ unsafe static void BinnedBuilderWorkerFunction(int workerIndex, IThread /// Multiplier to apply to the subtree count within a node to decide the bin count. Resulting value will then be clamped by the minimum/maximum bin counts. /// Threshold at or under which the binned builder resorts to local counting sort sweeps. public static unsafe void BinnedBuild(Buffer subtrees, Buffer nodes, Buffer metanodes, Buffer leaves, - IThreadDispatcher dispatcher = null, BufferPool pool = null, int maximumSubtreeStackAllocationCount = 4096, int minimumBinCount = 16, int maximumBinCount = 64, float leafToBinMultiplier = 1 / 16f, int microsweepThreshold = 64) + BufferPool pool = null, IThreadDispatcher dispatcher = null, TaskStack* taskStackPointer = null, int maximumSubtreeStackAllocationCount = 4096, int minimumBinCount = 16, int maximumBinCount = 64, float leafToBinMultiplier = 1 / 16f, int microsweepThreshold = 64) { if (dispatcher != null && pool == null) throw new ArgumentException("If a ThreadDispatcher has been given to BinnedBuild, a BufferPool must also be provided."); @@ -1459,9 +1474,12 @@ public static unsafe void BinnedBuild(Buffer subtrees, Buffer n /// Runs a binned build across the subtrees buffer. /// /// Subtrees (either leaves or nodes) to run the builder over. The builder may make in-place modifications to the input buffer; the input buffer should not be assumed to be in a valid state after the builder runs. - /// Dispatcher used to multithread the execution of the build. If the dispatcher is not null, pool must also not be null. /// Buffer pool used to preallocate a pingpong buffer if the number of subtrees exceeds maximumSubtreeStackAllocationCount. If null, stack allocation or a slower in-place partitioning will be used. /// A pool must be provided if a thread dispatcher is given. + /// Dispatcher used to multithread the execution of the build. If the dispatcher is not null, pool must also not be null. + /// Task stack being used to run the build process, if any. + /// If provided, the builder assumes the refinement is running within an existing multithreaded dispatch and will not call IThreadDispatcher.DispatchWorkers. + /// If null, the builder will create its own task stack and call IThreadDispatcher.DispatchWorkers internally. /// Maximum number of subtrees to try putting on the stack for the binned builder's pong buffers. /// Subtree counts larger than this threshold will either resort to a buffer pool allocation (if available) or slower in-place partition operations. /// Minimum number of bins the builder should use per node. @@ -1469,9 +1487,9 @@ public static unsafe void BinnedBuild(Buffer subtrees, Buffer n /// Multiplier to apply to the subtree count within a node to decide the bin count. Resulting value will then be clamped by the minimum/maximum bin counts. /// Threshold at or under which the binned builder resorts to local counting sort sweeps. public unsafe void BinnedBuild(Buffer subtrees, - IThreadDispatcher dispatcher = null, BufferPool pool = null, int maximumSubtreeStackAllocationCount = 4096, int minimumBinCount = 16, int maximumBinCount = 64, float leafToBinMultiplier = 1 / 16f, int microsweepThreshold = 64) + BufferPool pool = null, IThreadDispatcher dispatcher = null, TaskStack* taskStackPointer = null, int maximumSubtreeStackAllocationCount = 4096, int minimumBinCount = 16, int maximumBinCount = 64, float leafToBinMultiplier = 1 / 16f, int microsweepThreshold = 64) { - BinnedBuild(subtrees, Nodes.Slice(NodeCount), Metanodes.Slice(NodeCount), Leaves.Slice(LeafCount), dispatcher, pool, maximumSubtreeStackAllocationCount, minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold); + BinnedBuild(subtrees, Nodes.Slice(NodeCount), Metanodes.Slice(NodeCount), Leaves.Slice(LeafCount), pool, dispatcher, taskStackPointer, maximumSubtreeStackAllocationCount, minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold); } } } diff --git a/BepuPhysics/Trees/Tree_Refine2.cs b/BepuPhysics/Trees/Tree_Refine2.cs index 529e34bf7..22908d428 100644 --- a/BepuPhysics/Trees/Tree_Refine2.cs +++ b/BepuPhysics/Trees/Tree_Refine2.cs @@ -268,7 +268,7 @@ void CollectSubtreesForSubtreeRefinement(int refinementTargetRootNodeIndex, Buff /// Target size of subtree refinements. The actual size of refinement will usually be larger or smaller. /// Pool used for ephemeral allocations during the refinement. /// Nodes will not be refit. - public void Refine2(int rootRefinementSize, ref int subtreeRefinementStartIndex, int subtreeRefinementCount, int subtreeRefinementSize, BufferPool pool) + public unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementStartIndex, int subtreeRefinementCount, int subtreeRefinementSize, BufferPool pool) { //No point refining anything with two leaves. This condition also avoids having to special case for an incomplete root node. if (LeafCount <= 2) @@ -296,7 +296,7 @@ public void Refine2(int rootRefinementSize, ref int subtreeRefinementStartIndex, var rootRefinementNodes = refinementNodesAllocation.Slice(0, rootRefinementNodeIndices.Count); var rootRefinementMetanodes = refinementMetanodesAllocation.Slice(0, rootRefinementNodeIndices.Count); //Passing 'default' for the leaves tells the binned builder to not worry about updating leaves. - BinnedBuild(rootRefinementSubtrees, rootRefinementNodes, rootRefinementMetanodes, default, null, pool); + BinnedBuild(rootRefinementSubtrees, rootRefinementNodes, rootRefinementMetanodes, default, pool); ReifyRootRefinement(rootRefinementNodeIndices, rootRefinementNodes); rootRefinementSubtrees.Dispose(pool); rootRefinementNodeIndices.Dispose(pool); @@ -313,7 +313,7 @@ public void Refine2(int rootRefinementSize, ref int subtreeRefinementStartIndex, var refinementNodes = refinementNodesAllocation.Slice(0, subtreeRefinementNodeIndices.Count); var refinementMetanodes = refinementMetanodesAllocation.Slice(0, subtreeRefinementNodeIndices.Count); //Passing 'default' for the leaves tells the binned builder to not worry about updating leaves. - BinnedBuild(subtreeRefinementLeaves, refinementNodes, refinementMetanodes, default, null, pool); + BinnedBuild(subtreeRefinementLeaves, refinementNodes, refinementMetanodes, default, pool); ReifyRefinement(subtreeRefinementNodeIndices, refinementNodes); subtreeRefinementNodeIndices.Count = 0; @@ -355,7 +355,7 @@ unsafe static void ExecuteRootRefinementTask(long id, void* untypedContext, int var rootRefinementMetanodes = new Buffer(rootRefinementNodeIndices.Count, pool); //Passing 'default' for the leaves tells the binned builder to not worry about updating leaves. - BinnedBuild(rootRefinementSubtrees, rootRefinementNodes, rootRefinementMetanodes, default, null, pool); + BinnedBuild(rootRefinementSubtrees, rootRefinementNodes, rootRefinementMetanodes, default, pool); context.Tree.ReifyRootRefinement(rootRefinementNodeIndices, rootRefinementNodes); rootRefinementSubtrees.Dispose(pool); rootRefinementNodeIndices.Dispose(pool); @@ -377,7 +377,7 @@ unsafe static void ExecuteSubtreeRefinementTask(long subtreeRefinementTarget, vo var refinementNodes = new Buffer(subtreeRefinementNodeIndices.Count, pool); var refinementMetanodes = new Buffer(subtreeRefinementNodeIndices.Count, pool); //Passing 'default' for the leaves tells the binned builder to not worry about updating leaves. - BinnedBuild(subtreeRefinementLeaves, refinementNodes, refinementMetanodes, default, null, pool); + BinnedBuild(subtreeRefinementLeaves, refinementNodes, refinementMetanodes, default, pool); context.Tree.ReifyRefinement(subtreeRefinementNodeIndices, refinementNodes); refinementNodes.Dispose(pool); From e99fab9cda2a52c117eaf9a28463324d6d79ece2 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 26 Jun 2023 18:37:16 -0500 Subject: [PATCH 745/947] Binned build refactor for external task stack execution. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 136 +++++++++++++++--------- 1 file changed, 85 insertions(+), 51 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 6451dea9c..ddf597bf9 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -3,6 +3,7 @@ using BepuUtilities.Memory; using BepuUtilities.TaskScheduling; using System; +using System.Collections.Generic; using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; @@ -520,16 +521,16 @@ unsafe struct CentroidPrepassTaskContext /// public Buffer PrepassWorkers; /// - /// Buffer containing all subtrees in the node. + /// Buffer containing the bounding boxes of all subtrees in the node. /// - public Buffer Subtrees; + public Buffer Bounds; - public CentroidPrepassTaskContext(BufferPool pool, SharedTaskData taskData, Buffer subtrees) + public CentroidPrepassTaskContext(BufferPool pool, SharedTaskData taskData, Buffer bounds) { TaskData = taskData; pool.Take(int.Min(taskData.WorkerCount, taskData.TaskCount), out PrepassWorkers); Debug.Assert(PrepassWorkers.Length >= 2); - Subtrees = subtrees; + Bounds = bounds; } public void Dispose(BufferPool pool) => pool.Return(ref PrepassWorkers); @@ -539,7 +540,7 @@ unsafe static void CentroidPrepassWorker(long taskId, void* untypedContext, int ref var context = ref *(CentroidPrepassTaskContext*)untypedContext; Debug.Assert(context.TaskData.WorkerCount > 1 && context.TaskData.TaskCount > 1 && context.TaskData.WorkerCount < 100); context.TaskData.GetSlotInterval(taskId, out var start, out var count); - var centroidBounds = ComputeCentroidBounds(context.Subtrees.Slice(start, count).As()); + var centroidBounds = ComputeCentroidBounds(context.Bounds.Slice(start, count)); if (context.TaskData.TaskCountFitsInWorkerCount) { //There were less tasks than workers; directly write into the slot without bothering to merge. @@ -553,13 +554,11 @@ unsafe static void CentroidPrepassWorker(long taskId, void* untypedContext, int } } - unsafe static BoundingBox4 MultithreadedCentroidPrepass(MultithreadBinnedBuildContext* context, Buffer subtrees, int targetTaskCount, int workerIndex, IThreadDispatcher dispatcher) + unsafe static BoundingBox4 MultithreadedCentroidPrepass(MultithreadBinnedBuildContext* context, Buffer bounds, in SharedTaskData taskData, int workerIndex, IThreadDispatcher dispatcher) { ref var worker = ref context->Workers[workerIndex]; var workerPool = dispatcher.WorkerPools[workerIndex]; - var taskContext = new CentroidPrepassTaskContext(workerPool, new SharedTaskData(context->Workers.Length, 0, subtrees.Length, MinimumSubtreesPerThreadForCentroidPrepass, targetTaskCount), subtrees); - Debug.Assert(taskContext.TaskData.TaskCount > 1, "This codepath shouldn't be used if there's only one task!"); - Debug.Assert(taskContext.TaskData.WorkerCount > 1 && taskContext.TaskData.WorkerCount < 100); + var taskContext = new CentroidPrepassTaskContext(workerPool, taskData, bounds); var taskCount = taskContext.TaskData.TaskCount; //Don't bother initializing more slots than we have tasks. Note that this requires special handling on the task level; //if we have less tasks than workers, then the task needs to distinguish that fact. @@ -735,14 +734,12 @@ unsafe static void BinSubtreesWorker(long taskId, void* untypedContext, int work unsafe static void MultithreadedBinSubtrees(MultithreadBinnedBuildContext* context, Vector4 centroidBoundsMin, bool useX, bool useY, Vector128 permuteMask, int axisIndex, Vector4 offsetToBinIndex, Vector4 maximumBinIndex, - Buffer subtrees, Buffer subtreeBinIndices, int binCount, int workerIndex, IThreadDispatcher dispatcher) + Buffer subtrees, Buffer subtreeBinIndices, int binCount, in SharedTaskData taskData, int workerIndex, IThreadDispatcher dispatcher) { ref var worker = ref context->Workers[workerIndex]; var workerPool = dispatcher.WorkerPools[workerIndex]; var taskContext = new BinSubtreesTaskContext( - workerPool, - new SharedTaskData(context->Workers.Length, 0, subtrees.Length, MinimumSubtreesPerThreadForBinning, context->GetTargetTaskCountForInnerLoop(subtrees.Length)), - subtrees, subtreeBinIndices, binCount, useX, useY, permuteMask, axisIndex, centroidBoundsMin, offsetToBinIndex, maximumBinIndex); + workerPool, taskData, subtrees, subtreeBinIndices, binCount, useX, useY, permuteMask, axisIndex, centroidBoundsMin, offsetToBinIndex, maximumBinIndex); //Don't bother initializing more slots than we have tasks. Note that this requires special handling on the task level; //if we have less tasks than workers, then the task needs to distinguish that fact. @@ -913,13 +910,11 @@ unsafe static void PartitionSubtreesWorker(long taskId, void* untypedContext, in } unsafe static (int subtreeCountA, int subtreeCountB) MultithreadedPartition(MultithreadBinnedBuildContext* context, - Buffer subtrees, Buffer subtreesNext, Buffer binIndices, int binSplitIndex, int targetTaskCount, int workerIndex, IThreadDispatcher dispatcher) + Buffer subtrees, Buffer subtreesNext, Buffer binIndices, int binSplitIndex, in SharedTaskData taskData, int workerIndex, IThreadDispatcher dispatcher) { ref var worker = ref context->Workers[workerIndex]; var workerPool = dispatcher.WorkerPools[workerIndex]; - var taskContext = new PartitionTaskContext( - new SharedTaskData(context->Workers.Length, 0, subtrees.Length, MinimumSubtreesPerThreadForPartitioning, targetTaskCount), - subtrees, subtreesNext, binIndices, binSplitIndex); + var taskContext = new PartitionTaskContext(taskData, subtrees, subtreesNext, binIndices, binSplitIndex); //We only want the inner multithreading to work on small, non-recursive jobs. //Diving into a node at this point would stall the current node and favor more (and smaller) nodes. var tagValue = (uint)workerIndex | JobFilterTagHeader; @@ -967,18 +962,25 @@ static unsafe void BinnedBuildNode( return; } var targetTaskCount = typeof(TThreading) == typeof(SingleThreaded) ? 1 : - ((MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As(ref context->Threading)))->GetTargetTaskCountForInnerLoop(subtreeCount); + ((MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref context->Threading))->GetTargetTaskCountForInnerLoop(subtreeCount); if (nodeIndex == 0) { //The first node doesn't have a parent, and so isn't given centroid bounds. We have to compute them. - if (targetTaskCount == 1 || subtreeCount < MinimumSubtreesPerThreadForCentroidPrepass) + var useST = true; + if (typeof(TThreading) != typeof(SingleThreaded)) { - centroidBounds = ComputeCentroidBounds(boundingBoxes); + var mtContext = (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref context->Threading); + var taskData = new SharedTaskData(mtContext->Workers.Length, 0, subtrees.Length, MinimumSubtreesPerThreadForCentroidPrepass, mtContext->GetTargetTaskCountForInnerLoop(subtreeCount)); + if (taskData.TaskCount > 1) + { + centroidBounds = MultithreadedCentroidPrepass( + mtContext, boundingBoxes, taskData, workerIndex, dispatcher); + useST = false; + } } - else + if (useST) { - centroidBounds = MultithreadedCentroidPrepass( - (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As(ref context->Threading)), subtrees, targetTaskCount, workerIndex, dispatcher); + centroidBounds = ComputeCentroidBounds(boundingBoxes); } } var centroidSpan = centroidBounds.Max - centroidBounds.Min; @@ -1051,7 +1053,20 @@ static unsafe void BinnedBuildNode( binCentroidBounds.Max = new Vector4(float.MinValue); binLeafCounts[i] = 0; } - if (targetTaskCount == 1 || subtreeCount < MinimumSubtreesPerThreadForBinning) + var useSTForBinning = true; + if (typeof(TThreading) != typeof(SingleThreaded)) + { + var mtContext = (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref context->Threading); + var taskData = new SharedTaskData(mtContext->Workers.Length, 0, subtrees.Length, MinimumSubtreesPerThreadForBinning, mtContext->GetTargetTaskCountForInnerLoop(subtreeCount)); + if (taskData.TaskCount > 1) + { + MultithreadedBinSubtrees( + (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref context->Threading), + centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, subtrees, subtreeBinIndices, binCount, taskData, workerIndex, dispatcher); + useSTForBinning = false; + } + } + if (useSTForBinning) { //If the subtree bin indices buffer isn't available, then the binning process can't write to them! That'll happen if: //single threaded execution, @@ -1062,12 +1077,6 @@ static unsafe void BinnedBuildNode( else BinSubtrees(centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, subtrees, binBoundingBoxes, binCentroidBoundingBoxes, binLeafCounts, subtreeBinIndices); } - else - { - MultithreadedBinSubtrees( - (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As(ref context->Threading)), - centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, subtrees, subtreeBinIndices, binCount, workerIndex, dispatcher); - } //Identify the split index by examining the SAH of very split option. //Premerge from left to right so we have a sorta-summed area table to cheaply look up all possible child A bounds as we scan. @@ -1139,7 +1148,21 @@ static unsafe void BinnedBuildNode( Debug.Assert(subtreeBinIndices.Allocated); //If the current buffer is pong, then write to ping, and vice versa. var subtreesNext = (usePongBuffer ? context->SubtreesPing : context->SubtreesPong).Slice(subtreeRegionStartIndex, subtreeCount); - if (targetTaskCount == 1 || subtreeCount < MinimumSubtreesPerThreadForPartitioning) + + var useSTForPartitioning = true; + if (typeof(TThreading) != typeof(SingleThreaded)) + { + var mtContext = (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref context->Threading); + var taskData = new SharedTaskData(mtContext->Workers.Length, 0, subtrees.Length, MinimumSubtreesPerThreadForPartitioning, mtContext->GetTargetTaskCountForInnerLoop(subtreeCount)); + if (taskData.TaskCount > 1) + { + (subtreeCountA, subtreeCountB) = MultithreadedPartition( + (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref context->Threading), + subtrees, subtreesNext, subtreeBinIndices, splitIndex, taskData, workerIndex, dispatcher); + useSTForPartitioning = false; + } + } + if (useSTForPartitioning) { for (int i = 0; i < subtreeCount; ++i) { @@ -1147,12 +1170,6 @@ static unsafe void BinnedBuildNode( subtreesNext[targetIndex] = subtrees[i]; } } - else - { - (subtreeCountA, subtreeCountB) = MultithreadedPartition( - (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As(ref context->Threading)), - subtrees, subtreesNext, subtreeBinIndices, splitIndex, targetTaskCount, workerIndex, dispatcher); - } subtrees = subtreesNext; usePongBuffer = !usePongBuffer; } @@ -1198,7 +1215,7 @@ static unsafe void BinnedBuildNode( BuildNode(bestBoundingBoxA, bestBoundingBoxB, leafCountA, leafCountB, subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, subtreeCountA, subtreeCountB, ref context->Leaves, out var nodeChildIndexA, out var nodeChildIndexB); var targetNodeTaskCount = typeof(TThreading) == typeof(SingleThreaded) ? 1 : - ((MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref Unsafe.As(ref context->Threading)))->GetTargetTaskCountForNodes(subtreeCount); + ((MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref context->Threading))->GetTargetTaskCountForNodes(subtreeCount); var shouldPushBOntoMultithreadedQueue = targetNodeTaskCount > 1 && subtreeCountA >= MinimumSubtreesPerThreadForNodeJob && subtreeCountB >= MinimumSubtreesPerThreadForNodeJob; ContinuationHandle nodeBContinuation = default; if (shouldPushBOntoMultithreadedQueue) @@ -1250,14 +1267,16 @@ static unsafe void BinnedBuildNode( /// Task stack being used to run the build process, if any. /// If provided, the builder assumes the refinement is running within an existing multithreaded dispatch and will not call IThreadDispatcher.DispatchWorkers. /// If null, the builder will create its own task stack and call IThreadDispatcher.DispatchWorkers internally. - /// Number of workers to be used in the builder. + /// Index of the current worker. + /// Number of workers that may be used in the builder. This should span all worker indices that may contribute to the build process even only a subset are expected to be used at any one time. + /// Number of tasks to try to use in the builder. /// Buffer pool used to preallocate temporary resources for building. /// Minimum number of bins the builder should use per node. /// Maximum number of bins the builder should use per node. Must be no higher than 255. /// Multiplier to apply to the subtree count within a node to decide the bin count. Resulting value will then be clamped by the minimum/maximum bin counts. /// Threshold at or under which the binned builder resorts to local counting sort sweeps. static unsafe void BinnedBuilderInternal(Buffer subtrees, Buffer subtreesPong, Buffer nodes, Buffer metanodes, Buffer leaves, Buffer binIndices, - IThreadDispatcher dispatcher, TaskStack* taskStackPointer, int workerCount, BufferPool pool, int minimumBinCount, int maximumBinCount, float leafToBinMultiplier, int microsweepThreshold) + IThreadDispatcher dispatcher, TaskStack* taskStackPointer, int workerIndex, int workerCount, int targetTaskCount, BufferPool pool, int minimumBinCount, int maximumBinCount, float leafToBinMultiplier, int microsweepThreshold) { var subtreeCount = subtrees.Length; if (nodes.Length < subtreeCount - 1) @@ -1301,14 +1320,14 @@ static unsafe void BinnedBuilderInternal(Buffer subtrees, Buffer, SingleThreaded>( minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold, subtrees, default, leaves, nodes, metanodes, binIndices, threading); - BinnedBuildNode(false, 0, 0, subtreeCount, -1, -1, default, &context, 0, null); + BinnedBuildNode(false, 0, 0, subtreeCount, -1, -1, default, &context, workerIndex, null); } else { var context = new Context( minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold, subtrees, default, default, nodes, metanodes, binIndices, threading); - BinnedBuildNode(false, 0, 0, subtreeCount, -1, -1, default, &context, 0, null); + BinnedBuildNode(false, 0, 0, subtreeCount, -1, -1, default, &context, workerIndex, null); } } else @@ -1343,7 +1362,7 @@ static unsafe void BinnedBuilderInternal(Buffer subtrees, Buffer subtrees, BufferPushUnsafely(new Task(&BinnedBuilderWorkerEntry>, &context), 0, dispatcher); dispatcher.DispatchWorkers(&BinnedBuilderWorkerFunction>, unmanagedContext: taskStackPointer, maximumWorkerCount: workerCount); } else { - BinnedBuildNode(false, 0, 0, context.SubtreesPing.Length, -1, -1, default, &context, 0, dispatcher); + BinnedBuildNode(false, 0, 0, context.SubtreesPing.Length, -1, -1, default, &context, workerIndex, dispatcher); } } else @@ -1372,12 +1392,13 @@ static unsafe void BinnedBuilderInternal(Buffer subtrees, BufferPushUnsafely(new Task(&BinnedBuilderWorkerEntry, &context), 0, dispatcher); dispatcher.DispatchWorkers(&BinnedBuilderWorkerFunction, unmanagedContext: taskStackPointer, maximumWorkerCount: workerCount); } else { - BinnedBuildNode(false, 0, 0, context.SubtreesPing.Length, -1, -1, default, &context, 0, dispatcher); + BinnedBuildNode(false, 0, 0, context.SubtreesPing.Length, -1, -1, default, &context, workerIndex, dispatcher); } } @@ -1428,6 +1449,10 @@ unsafe static void BinnedBuilderWorkerFunction(int workerIndex, IThread /// If provided, the builder assumes the refinement is running within an existing multithreaded dispatch and will not call IThreadDispatcher.DispatchWorkers. /// If null, the builder will create its own task stack and call IThreadDispatcher.DispatchWorkers internally. /// A pool must be provided if a thread dispatcher is given. + /// Index of the currently executing worker. If not running within a dispatch, 0 is valid. + /// Number of workers that may be used in the builder. This should span all worker indices that may contribute to the build process even only a subset are expected to be used at any one time. + /// If negative, the dispatcher's thread count will be used. + /// Number of tasks to try to use in the builder. If negative, the dispatcher's thread count will be used. /// Maximum number of subtrees to try putting on the stack for the binned builder's pong buffers. /// Subtree counts larger than this threshold will either resort to a buffer pool allocation (if available) or slower in-place partition operations. /// Minimum number of bins the builder should use per node. @@ -1435,7 +1460,8 @@ unsafe static void BinnedBuilderWorkerFunction(int workerIndex, IThread /// Multiplier to apply to the subtree count within a node to decide the bin count. Resulting value will then be clamped by the minimum/maximum bin counts. /// Threshold at or under which the binned builder resorts to local counting sort sweeps. public static unsafe void BinnedBuild(Buffer subtrees, Buffer nodes, Buffer metanodes, Buffer leaves, - BufferPool pool = null, IThreadDispatcher dispatcher = null, TaskStack* taskStackPointer = null, int maximumSubtreeStackAllocationCount = 4096, int minimumBinCount = 16, int maximumBinCount = 64, float leafToBinMultiplier = 1 / 16f, int microsweepThreshold = 64) + BufferPool pool = null, IThreadDispatcher dispatcher = null, TaskStack* taskStackPointer = null, int workerIndex = 0, int workerCount = -1, int targetTaskCount = -1, + int maximumSubtreeStackAllocationCount = 4096, int minimumBinCount = 16, int maximumBinCount = 64, float leafToBinMultiplier = 1 / 16f, int microsweepThreshold = 64) { if (dispatcher != null && pool == null) throw new ArgumentException("If a ThreadDispatcher has been given to BinnedBuild, a BufferPool must also be provided."); @@ -1460,8 +1486,10 @@ public static unsafe void BinnedBuild(Buffer subtrees, Buffer n binIndices = default; subtreesPong = default; } - //TODO: Note that there's no public entrypoint into the binned builder that provides a task stack right now. Good chance that'll change once we use it in refinement. - BinnedBuilderInternal(subtrees, subtreesPong, nodes, metanodes, leaves, binIndices, dispatcher, null, dispatcher == null ? 0 : dispatcher.ThreadCount, pool, minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold); + BinnedBuilderInternal(subtrees, subtreesPong, nodes, metanodes, leaves, binIndices, dispatcher, taskStackPointer, workerIndex, + dispatcher == null ? 0 : workerCount < 0 ? dispatcher.ThreadCount : workerCount, + dispatcher == null ? 0 : targetTaskCount < 0 ? dispatcher.ThreadCount : targetTaskCount, + pool, minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold); if (requiresReturn) { @@ -1479,7 +1507,11 @@ public static unsafe void BinnedBuild(Buffer subtrees, Buffer n /// Dispatcher used to multithread the execution of the build. If the dispatcher is not null, pool must also not be null. /// Task stack being used to run the build process, if any. /// If provided, the builder assumes the refinement is running within an existing multithreaded dispatch and will not call IThreadDispatcher.DispatchWorkers. - /// If null, the builder will create its own task stack and call IThreadDispatcher.DispatchWorkers internally. + /// If null, the builder will create its own task stack and call IThreadDispatcher.DispatchWorkers internally. + /// Index of the currently executing worker. If not running within a dispatch, 0 is valid. + /// Number of workers that may be used in the builder. This should span all worker indices that may contribute to the build process even only a subset are expected to be used at any one time. + /// If negative, the dispatcher's thread count will be used. + /// Number of tasks to try to use in the builder. If negative, the dispatcher's thread count will be used. /// Maximum number of subtrees to try putting on the stack for the binned builder's pong buffers. /// Subtree counts larger than this threshold will either resort to a buffer pool allocation (if available) or slower in-place partition operations. /// Minimum number of bins the builder should use per node. @@ -1487,9 +1519,11 @@ public static unsafe void BinnedBuild(Buffer subtrees, Buffer n /// Multiplier to apply to the subtree count within a node to decide the bin count. Resulting value will then be clamped by the minimum/maximum bin counts. /// Threshold at or under which the binned builder resorts to local counting sort sweeps. public unsafe void BinnedBuild(Buffer subtrees, - BufferPool pool = null, IThreadDispatcher dispatcher = null, TaskStack* taskStackPointer = null, int maximumSubtreeStackAllocationCount = 4096, int minimumBinCount = 16, int maximumBinCount = 64, float leafToBinMultiplier = 1 / 16f, int microsweepThreshold = 64) + BufferPool pool = null, IThreadDispatcher dispatcher = null, TaskStack* taskStackPointer = null, int workerIndex = 0, int workerCount = -1, int targetTaskCount = -1, + int maximumSubtreeStackAllocationCount = 4096, int minimumBinCount = 16, int maximumBinCount = 64, float leafToBinMultiplier = 1 / 16f, int microsweepThreshold = 64) { - BinnedBuild(subtrees, Nodes.Slice(NodeCount), Metanodes.Slice(NodeCount), Leaves.Slice(LeafCount), pool, dispatcher, taskStackPointer, maximumSubtreeStackAllocationCount, minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold); + BinnedBuild(subtrees, Nodes.Slice(NodeCount), Metanodes.Slice(NodeCount), Leaves.Slice(LeafCount), pool, dispatcher, taskStackPointer, workerIndex, + workerCount, targetTaskCount, maximumSubtreeStackAllocationCount, minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold); } } } From e88147d8490ee5f029a5d1e0ad1bb7f89e399fe0 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 29 Jun 2023 17:56:28 -0500 Subject: [PATCH 746/947] Internal refinement threading beginning. There's significant nondeterminism arising from partitioning orders. --- BepuPhysics/Trees/Tree_Refine2.cs | 29 ++++++++++++++++++++++++++--- 1 file changed, 26 insertions(+), 3 deletions(-) diff --git a/BepuPhysics/Trees/Tree_Refine2.cs b/BepuPhysics/Trees/Tree_Refine2.cs index 22908d428..4f30242e2 100644 --- a/BepuPhysics/Trees/Tree_Refine2.cs +++ b/BepuPhysics/Trees/Tree_Refine2.cs @@ -329,11 +329,15 @@ public unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementStar } - struct RefinementContext + unsafe struct RefinementContext { public int RootRefinementSize; public int SubtreeRefinementSize; + public int TotalLeafCountInSubtrees; + public int TargetTaskBudget; + public int WorkerCount; public QuickList SubtreeRefinementTargets; + public TaskStack* TaskStack; /// /// This is a *copy* of the original tree that spawned this refinement. Refinements do not modify the memory stored at the level of the Tree, only memory *pointed* to by the tree. /// @@ -353,6 +357,15 @@ unsafe static void ExecuteRootRefinementTask(long id, void* untypedContext, int Debug.Assert(rootRefinementNodeIndices.Count == rootRefinementSubtrees.Count - 1); var rootRefinementNodes = new Buffer(rootRefinementNodeIndices.Count, pool); var rootRefinementMetanodes = new Buffer(rootRefinementNodeIndices.Count, pool); + //Passing 'default' for the leaves tells the binned builder to not worry about updating leaves. + if (taskCount > 1) + { + BinnedBuild(rootRefinementSubtrees, rootRefinementNodes, rootRefinementMetanodes, default, pool, dispatcher, context.TaskStack, workerIndex, context.WorkerCount, taskCount); + } + else + { + BinnedBuild(rootRefinementSubtrees, rootRefinementNodes, rootRefinementMetanodes, default, pool, workerIndex: workerIndex); + } //Passing 'default' for the leaves tells the binned builder to not worry about updating leaves. BinnedBuild(rootRefinementSubtrees, rootRefinementNodes, rootRefinementMetanodes, default, pool); @@ -413,7 +426,7 @@ static unsafe void StopStackOnCompletion(long id, void* untypedContext, int work /// Target size of subtree refinements. The actual size of refinement will usually be larger or smaller. /// Pool used for ephemeral allocations during the refinement. /// Nodes will not be refit. - private unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementStartIndex, int subtreeRefinementCount, int subtreeRefinementSize, BufferPool pool, int workerIndex, TaskStack* taskStack, IThreadDispatcher threadDispatcher, bool internallyDispatch) + private unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementStartIndex, int subtreeRefinementCount, int subtreeRefinementSize, BufferPool pool, int workerIndex, TaskStack* taskStack, IThreadDispatcher threadDispatcher, bool internallyDispatch, int workerCount, int targetTaskBudget) { //No point refining anything with two leaves. This condition also avoids having to special case for an incomplete root node. if (LeafCount <= 2) @@ -429,11 +442,21 @@ private unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementSta ((Span)subtreeRefinementTargets.Span)[subtreeRefinementTargets.Count..].Fill(-1); var tasks = new Buffer(1 + subtreeRefinementTargets.Count, pool); + var totalLeafCountInSubtrees = 0; + for (int i = 0; i < subtreeRefinementTargets.Count; ++i) + { + ref var node = ref Nodes[subtreeRefinementTargets[i]]; + totalLeafCountInSubtrees += node.A.LeafCount + node.B.LeafCount; + } var context = new RefinementContext { RootRefinementSize = rootRefinementSize, SubtreeRefinementSize = subtreeRefinementSize, + TotalLeafCountInSubtrees = totalLeafCountInSubtrees, + TargetTaskBudget = targetTaskBudget, SubtreeRefinementTargets = subtreeRefinementTargets, + TaskStack = taskStack, + WorkerCount = workerCount, Tree = this }; for (int i = 0; i < subtreeRefinementTargets.Count; ++i) @@ -445,7 +468,7 @@ private unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementSta { //There isn't an active dispatch, so we need to do it. taskStack->AllocateContinuationAndPush(tasks, workerIndex, threadDispatcher, onComplete: new Task(&StopStackOnCompletion, taskStack)); - threadDispatcher.DispatchWorkers(&ExecuteWorker, unmanagedContext: taskStack); + threadDispatcher.DispatchWorkers(&ExecuteWorker, unmanagedContext: taskStack, maximumWorkerCount: workerCount); } else { From 4c4454f49784e08d3a182e46eae72589de0eb040 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 29 Jun 2023 20:08:36 -0500 Subject: [PATCH 747/947] Oops, unstaged bits. --- BepuPhysics/Trees/Tree_Refine2.cs | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/BepuPhysics/Trees/Tree_Refine2.cs b/BepuPhysics/Trees/Tree_Refine2.cs index 4f30242e2..4bf99f884 100644 --- a/BepuPhysics/Trees/Tree_Refine2.cs +++ b/BepuPhysics/Trees/Tree_Refine2.cs @@ -348,6 +348,8 @@ unsafe static void ExecuteRootRefinementTask(long id, void* untypedContext, int ref var context = ref *(RefinementContext*)untypedContext; var pool = dispatcher.WorkerPools[workerIndex]; + var taskCount = (int)float.Ceiling(context.TargetTaskBudget * (float)context.RootRefinementSize / (context.RootRefinementSize + context.TotalLeafCountInSubtrees)); + //We now know which nodes are the roots of subtree refinements; the root refinement can avoid traversing through them. var rootRefinementSubtrees = new QuickList(context.RootRefinementSize, pool); var rootRefinementNodeIndices = new QuickList(context.RootRefinementSize, pool); @@ -495,7 +497,7 @@ public unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementStar if (LeafCount <= 2) return; var taskStack = new TaskStack(pool, threadDispatcher, threadDispatcher.ThreadCount); - Refine2(rootRefinementSize, ref subtreeRefinementStartIndex, subtreeRefinementCount, subtreeRefinementSize, pool, 0, &taskStack, threadDispatcher, true); + Refine2(rootRefinementSize, ref subtreeRefinementStartIndex, subtreeRefinementCount, subtreeRefinementSize, pool, 0, &taskStack, threadDispatcher, true, threadDispatcher.ThreadCount, threadDispatcher.ThreadCount); taskStack.Dispose(pool, threadDispatcher); } } From 616fc15d8af5a90f3f1272d5b20821206e97b15b Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 1 Jul 2023 18:42:03 -0500 Subject: [PATCH 748/947] BinnedBuild now has a determinism option. Refine2 now actually uses MT binned build for root, hard part over! --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 33 ++++++++++++------ BepuPhysics/Trees/Tree_Refine2.cs | 45 ++++++++++++++++++++----- 2 files changed, 59 insertions(+), 19 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index ddf597bf9..a4417028d 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -99,6 +99,8 @@ struct Context public float LeafToBinMultiplier; public int MicrosweepThreshold; + public bool Deterministic; + public TLeaves Leaves; public Buffer SubtreesPing; public Buffer SubtreesPong; @@ -109,13 +111,14 @@ struct Context public TThreading Threading; - public Context(int minimumBinCount, int maximumBinCount, float leafToBinMultiplier, int microsweepThreshold, + public Context(int minimumBinCount, int maximumBinCount, float leafToBinMultiplier, int microsweepThreshold, bool deterministic, Buffer subtreesPing, Buffer subtreesPong, TLeaves leaves, Buffer nodes, Buffer metanodes, Buffer binIndices, TThreading threading) { MinimumBinCount = minimumBinCount; MaximumBinCount = maximumBinCount; LeafToBinMultiplier = leafToBinMultiplier; MicrosweepThreshold = microsweepThreshold; + Deterministic = deterministic; SubtreesPing = subtreesPing; SubtreesPong = subtreesPong; BinIndices = binIndices; @@ -1150,7 +1153,11 @@ static unsafe void BinnedBuildNode( var subtreesNext = (usePongBuffer ? context->SubtreesPing : context->SubtreesPong).Slice(subtreeRegionStartIndex, subtreeCount); var useSTForPartitioning = true; - if (typeof(TThreading) != typeof(SingleThreaded)) + //TODO: Note that the current multithreaded partitioning implementation is nondeterministic. + //Because of microsweeps/terminal node ordering, this can result in nondeterministic tree topology. + //See https://github.com/bepu/bepuphysics2/issues/276 for more information (and how to improve this in the future if valuable). + //For now, if the user wants determinism, we just use the single threaded path for partitioning. + if (typeof(TThreading) != typeof(SingleThreaded) && !context->Deterministic) { var mtContext = (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref context->Threading); var taskData = new SharedTaskData(mtContext->Workers.Length, 0, subtrees.Length, MinimumSubtreesPerThreadForPartitioning, mtContext->GetTargetTaskCountForInnerLoop(subtreeCount)); @@ -1275,8 +1282,10 @@ static unsafe void BinnedBuildNode( /// Maximum number of bins the builder should use per node. Must be no higher than 255. /// Multiplier to apply to the subtree count within a node to decide the bin count. Resulting value will then be clamped by the minimum/maximum bin counts. /// Threshold at or under which the binned builder resorts to local counting sort sweeps. + /// Whether to force determinism at a slightly higher cost when using internally multithreaded execution. + /// If the build is single threaded, it is already deterministic and this flag has no effect. static unsafe void BinnedBuilderInternal(Buffer subtrees, Buffer subtreesPong, Buffer nodes, Buffer metanodes, Buffer leaves, Buffer binIndices, - IThreadDispatcher dispatcher, TaskStack* taskStackPointer, int workerIndex, int workerCount, int targetTaskCount, BufferPool pool, int minimumBinCount, int maximumBinCount, float leafToBinMultiplier, int microsweepThreshold) + IThreadDispatcher dispatcher, TaskStack* taskStackPointer, int workerIndex, int workerCount, int targetTaskCount, BufferPool pool, int minimumBinCount, int maximumBinCount, float leafToBinMultiplier, int microsweepThreshold, bool deterministic) { var subtreeCount = subtrees.Length; if (nodes.Length < subtreeCount - 1) @@ -1318,14 +1327,14 @@ static unsafe void BinnedBuilderInternal(Buffer subtrees, Buffer, SingleThreaded>( - minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold, + minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold, deterministic, subtrees, default, leaves, nodes, metanodes, binIndices, threading); BinnedBuildNode(false, 0, 0, subtreeCount, -1, -1, default, &context, workerIndex, null); } else { var context = new Context( - minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold, + minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold, deterministic, subtrees, default, default, nodes, metanodes, binIndices, threading); BinnedBuildNode(false, 0, 0, subtreeCount, -1, -1, default, &context, workerIndex, null); } @@ -1370,7 +1379,7 @@ static unsafe void BinnedBuilderInternal(Buffer subtrees, Buffer, MultithreadBinnedBuildContext>( - minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold, + minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold, deterministic, subtrees, subtreesPong, leaves, nodes, metanodes, binIndices, threading); if (dispatchInternally) @@ -1387,7 +1396,7 @@ static unsafe void BinnedBuilderInternal(Buffer subtrees, Buffer( - minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold, + minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold, deterministic, subtrees, subtreesPong, default, nodes, metanodes, binIndices, threading); if (dispatchInternally) @@ -1459,9 +1468,11 @@ unsafe static void BinnedBuilderWorkerFunction(int workerIndex, IThread /// Maximum number of bins the builder should use per node. /// Multiplier to apply to the subtree count within a node to decide the bin count. Resulting value will then be clamped by the minimum/maximum bin counts. /// Threshold at or under which the binned builder resorts to local counting sort sweeps. + /// Whether to force determinism at a slightly higher cost when using internally multithreaded execution. + /// If the build is single threaded, it is already deterministic and this flag has no effect. public static unsafe void BinnedBuild(Buffer subtrees, Buffer nodes, Buffer metanodes, Buffer leaves, BufferPool pool = null, IThreadDispatcher dispatcher = null, TaskStack* taskStackPointer = null, int workerIndex = 0, int workerCount = -1, int targetTaskCount = -1, - int maximumSubtreeStackAllocationCount = 4096, int minimumBinCount = 16, int maximumBinCount = 64, float leafToBinMultiplier = 1 / 16f, int microsweepThreshold = 64) + int maximumSubtreeStackAllocationCount = 4096, int minimumBinCount = 16, int maximumBinCount = 64, float leafToBinMultiplier = 1 / 16f, int microsweepThreshold = 64, bool deterministic = false) { if (dispatcher != null && pool == null) throw new ArgumentException("If a ThreadDispatcher has been given to BinnedBuild, a BufferPool must also be provided."); @@ -1489,7 +1500,7 @@ public static unsafe void BinnedBuild(Buffer subtrees, Buffer n BinnedBuilderInternal(subtrees, subtreesPong, nodes, metanodes, leaves, binIndices, dispatcher, taskStackPointer, workerIndex, dispatcher == null ? 0 : workerCount < 0 ? dispatcher.ThreadCount : workerCount, dispatcher == null ? 0 : targetTaskCount < 0 ? dispatcher.ThreadCount : targetTaskCount, - pool, minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold); + pool, minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold, deterministic); if (requiresReturn) { @@ -1518,9 +1529,11 @@ public static unsafe void BinnedBuild(Buffer subtrees, Buffer n /// Maximum number of bins the builder should use per node. /// Multiplier to apply to the subtree count within a node to decide the bin count. Resulting value will then be clamped by the minimum/maximum bin counts. /// Threshold at or under which the binned builder resorts to local counting sort sweeps. + /// Whether to force determinism at a slightly higher cost when using internally multithreaded execution. + /// If the build is single threaded, it is already deterministic and this flag has no effect. public unsafe void BinnedBuild(Buffer subtrees, BufferPool pool = null, IThreadDispatcher dispatcher = null, TaskStack* taskStackPointer = null, int workerIndex = 0, int workerCount = -1, int targetTaskCount = -1, - int maximumSubtreeStackAllocationCount = 4096, int minimumBinCount = 16, int maximumBinCount = 64, float leafToBinMultiplier = 1 / 16f, int microsweepThreshold = 64) + int maximumSubtreeStackAllocationCount = 4096, int minimumBinCount = 16, int maximumBinCount = 64, float leafToBinMultiplier = 1 / 16f, int microsweepThreshold = 64, bool deterministic = false) { BinnedBuild(subtrees, Nodes.Slice(NodeCount), Metanodes.Slice(NodeCount), Leaves.Slice(LeafCount), pool, dispatcher, taskStackPointer, workerIndex, workerCount, targetTaskCount, maximumSubtreeStackAllocationCount, minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold); diff --git a/BepuPhysics/Trees/Tree_Refine2.cs b/BepuPhysics/Trees/Tree_Refine2.cs index 4bf99f884..3c8f7924a 100644 --- a/BepuPhysics/Trees/Tree_Refine2.cs +++ b/BepuPhysics/Trees/Tree_Refine2.cs @@ -96,7 +96,7 @@ void ReifyRefinement(QuickList refinementNodeIndices, Buffer refineme ReifyRefinementNodeChild(ref refinedNode.A.Index, ref refinementNodeIndices, realNodeIndex, 0); ReifyRefinementNodeChild(ref refinedNode.B.Index, ref refinementNodeIndices, realNodeIndex, 1); Nodes[realNodeIndex] = refinedNode; - Debug.Assert(Metanodes[realNodeIndex].Parent < 0 || Unsafe.Add(ref Nodes[Metanodes[realNodeIndex].Parent].A, Metanodes[realNodeIndex].IndexInParent).Index == realNodeIndex); + //Debug.Assert(Metanodes[realNodeIndex].Parent < 0 || Unsafe.Add(ref Nodes[Metanodes[realNodeIndex].Parent].A, Metanodes[realNodeIndex].IndexInParent).Index == realNodeIndex); } } @@ -338,11 +338,15 @@ unsafe struct RefinementContext public int WorkerCount; public QuickList SubtreeRefinementTargets; public TaskStack* TaskStack; + public bool Deterministic; /// /// This is a *copy* of the original tree that spawned this refinement. Refinements do not modify the memory stored at the level of the Tree, only memory *pointed* to by the tree. /// public Tree Tree; } + //static int debugAccumulatorForSubtreeIndices = 0; + //static int debugAccumulatorForNodeTopology = 0; + //static int debugIndex = 0; unsafe static void ExecuteRootRefinementTask(long id, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) { ref var context = ref *(RefinementContext*)untypedContext; @@ -354,7 +358,13 @@ unsafe static void ExecuteRootRefinementTask(long id, void* untypedContext, int var rootRefinementSubtrees = new QuickList(context.RootRefinementSize, pool); var rootRefinementNodeIndices = new QuickList(context.RootRefinementSize, pool); context.Tree.CollectSubtreesForRootRefinement(context.RootRefinementSize, context.SubtreeRefinementSize, pool, context.SubtreeRefinementTargets, ref rootRefinementNodeIndices, ref rootRefinementSubtrees); - + //var localSubtreeHash = 0; + //for (int i = 0; i < rootRefinementSubtrees.Count; ++i) + //{ + // localSubtreeHash = HashHelper.Rehash(localSubtreeHash) + rootRefinementSubtrees[i].Index; + // //localSubtreeHash += rootRefinementSubtrees[i].Index; + //} + //debugAccumulatorForSubtreeIndices = HashHelper.Rehash(debugAccumulatorForSubtreeIndices) + localSubtreeHash; //Now that we have a list of nodes to refine, we can run the root refinement. Debug.Assert(rootRefinementNodeIndices.Count == rootRefinementSubtrees.Count - 1); var rootRefinementNodes = new Buffer(rootRefinementNodeIndices.Count, pool); @@ -362,15 +372,27 @@ unsafe static void ExecuteRootRefinementTask(long id, void* untypedContext, int //Passing 'default' for the leaves tells the binned builder to not worry about updating leaves. if (taskCount > 1) { - BinnedBuild(rootRefinementSubtrees, rootRefinementNodes, rootRefinementMetanodes, default, pool, dispatcher, context.TaskStack, workerIndex, context.WorkerCount, taskCount); + BinnedBuild(rootRefinementSubtrees, rootRefinementNodes, rootRefinementMetanodes, default, pool, dispatcher, context.TaskStack, workerIndex, context.WorkerCount, taskCount, deterministic: context.Deterministic); } else { BinnedBuild(rootRefinementSubtrees, rootRefinementNodes, rootRefinementMetanodes, default, pool, workerIndex: workerIndex); } - - //Passing 'default' for the leaves tells the binned builder to not worry about updating leaves. - BinnedBuild(rootRefinementSubtrees, rootRefinementNodes, rootRefinementMetanodes, default, pool); + //var localNodeTopologyHash = 0; + //for (int i = 0; i < rootRefinementNodes.Length; ++i) + //{ + // localNodeTopologyHash = HashHelper.Rehash(HashHelper.Rehash(localNodeTopologyHash) + rootRefinementNodes[i].A.Index) + rootRefinementNodes[i].B.Index; + // //localNodeTopologyHash += rootRefinementNodes[i].A.Index + rootRefinementNodes[i].B.Index; + //} + + + //debugAccumulatorForNodeTopology = HashHelper.Rehash(debugAccumulatorForNodeTopology) + localNodeTopologyHash; + ////for (int i = 0; i < context.SubtreeRefinementTargets.Count; ++i) + ////{ + //// debugAccumulator = HashHelper.Rehash(debugAccumulator) + context.SubtreeRefinementTargets[i]; + ////} + //Console.WriteLine($"Refinement targets count {debugIndex}: {context.SubtreeRefinementTargets.Count}, node topology hash: {debugAccumulatorForNodeTopology}, subtree indices hash: {debugAccumulatorForSubtreeIndices}"); + //++debugIndex; context.Tree.ReifyRootRefinement(rootRefinementNodeIndices, rootRefinementNodes); rootRefinementSubtrees.Dispose(pool); rootRefinementNodeIndices.Dispose(pool); @@ -427,8 +449,10 @@ static unsafe void StopStackOnCompletion(long id, void* untypedContext, int work /// Number of subtree refinements to execute. /// Target size of subtree refinements. The actual size of refinement will usually be larger or smaller. /// Pool used for ephemeral allocations during the refinement. + /// Whether to force determinism at a slightly higher cost when using internally multithreaded execution for an individual refinement operation. + /// If the refine is single threaded, it is already deterministic and this flag has no effect. /// Nodes will not be refit. - private unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementStartIndex, int subtreeRefinementCount, int subtreeRefinementSize, BufferPool pool, int workerIndex, TaskStack* taskStack, IThreadDispatcher threadDispatcher, bool internallyDispatch, int workerCount, int targetTaskBudget) + private unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementStartIndex, int subtreeRefinementCount, int subtreeRefinementSize, BufferPool pool, int workerIndex, TaskStack* taskStack, IThreadDispatcher threadDispatcher, bool internallyDispatch, int workerCount, int targetTaskBudget, bool deterministic) { //No point refining anything with two leaves. This condition also avoids having to special case for an incomplete root node. if (LeafCount <= 2) @@ -459,6 +483,7 @@ private unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementSta SubtreeRefinementTargets = subtreeRefinementTargets, TaskStack = taskStack, WorkerCount = workerCount, + Deterministic = deterministic, Tree = this }; for (int i = 0; i < subtreeRefinementTargets.Count; ++i) @@ -490,14 +515,16 @@ private unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementSta /// Number of subtree refinements to execute. /// Target size of subtree refinements. The actual size of refinement will usually be larger or smaller. /// Pool used for ephemeral allocations during the refinement. + /// Whether to force determinism at a slightly higher cost when using internally multithreaded execution for an individual refinement operation. + /// If the refine is single threaded, it is already deterministic and this flag has no effect. /// Nodes will not be refit. - public unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementStartIndex, int subtreeRefinementCount, int subtreeRefinementSize, BufferPool pool, IThreadDispatcher threadDispatcher) + public unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementStartIndex, int subtreeRefinementCount, int subtreeRefinementSize, BufferPool pool, IThreadDispatcher threadDispatcher, bool deterministic = false) { //No point refining anything with two leaves. This condition also avoids having to special case for an incomplete root node. if (LeafCount <= 2) return; var taskStack = new TaskStack(pool, threadDispatcher, threadDispatcher.ThreadCount); - Refine2(rootRefinementSize, ref subtreeRefinementStartIndex, subtreeRefinementCount, subtreeRefinementSize, pool, 0, &taskStack, threadDispatcher, true, threadDispatcher.ThreadCount, threadDispatcher.ThreadCount); + Refine2(rootRefinementSize, ref subtreeRefinementStartIndex, subtreeRefinementCount, subtreeRefinementSize, pool, 0, &taskStack, threadDispatcher, true, threadDispatcher.ThreadCount, threadDispatcher.ThreadCount, deterministic); taskStack.Dispose(pool, threadDispatcher); } } From 91f724e058c91e8648982567d7fc631e6f884233 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 12 Jul 2023 13:45:41 -0500 Subject: [PATCH 749/947] Got rid of unnecessary assert. --- DemoUtilities/Window.cs | 2 -- 1 file changed, 2 deletions(-) diff --git a/DemoUtilities/Window.cs b/DemoUtilities/Window.cs index 899be6154..1cacc475b 100644 --- a/DemoUtilities/Window.cs +++ b/DemoUtilities/Window.cs @@ -95,8 +95,6 @@ public Int2 Resolution public Window(string title, Int2 resolution, Int2 location, WindowMode windowMode) { window = new NativeWindow(location.X, location.Y, resolution.X, resolution.Y, title, GameWindowFlags.FixedWindow, GraphicsMode.Default, DisplayDevice.Default); - Debug.Assert(window.ClientSize.Width == resolution.X); - Debug.Assert(window.ClientSize.Height == resolution.Y); window.Visible = true; Resolution = resolution; window.Resize += (form, args) => resized = true; From ba00e0a93c2fb841c18f56ea00a666c8610b6aa2 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 15 Jul 2023 19:39:29 -0500 Subject: [PATCH 750/947] A broken root refinement collection phase. Dropping it for now; the deterministic version is too slow, and a nondeterministic path can wait. --- BepuPhysics/Trees/Tree_Refine2.cs | 162 ++++++++++++++++++++++++++++-- 1 file changed, 151 insertions(+), 11 deletions(-) diff --git a/BepuPhysics/Trees/Tree_Refine2.cs b/BepuPhysics/Trees/Tree_Refine2.cs index 3c8f7924a..3f0b7a934 100644 --- a/BepuPhysics/Trees/Tree_Refine2.cs +++ b/BepuPhysics/Trees/Tree_Refine2.cs @@ -7,8 +7,11 @@ using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; using System.Threading; +using System.Threading.Tasks; using System.Xml.Linq; +using Task = BepuUtilities.TaskScheduling.Task; namespace BepuPhysics.Trees; @@ -163,7 +166,7 @@ void FindSubtreeRefinementTargets(int subtreeRefinementSize, int targetSubtreeRe - bool IsNodeChildSubtreeRefinementTarget(Buffer> subtreeRefinementBundles, in NodeChild child, int parentTotalLeafCount, int subtreeRefinementSize) + static bool IsNodeChildSubtreeRefinementTarget(Buffer> subtreeRefinementBundles, in NodeChild child, int parentTotalLeafCount, int subtreeRefinementSize) { //First check if it *could* be one by checking the leaf count threshold. if (child.LeafCount <= subtreeRefinementSize && parentTotalLeafCount > subtreeRefinementSize) @@ -182,7 +185,7 @@ bool IsNodeChildSubtreeRefinementTarget(Buffer> subtreeRefinementBun /// /// Checks if a child should be a subtree in the root refinement. If so, it's added to the list. Otherwise, it's pushed onto the stack. /// - private void TryPushChildForRootRefinement( + private static void TryPushChildForRootRefinement( int subtreeRefinementSize, Buffer> subtreeRefinementRootBundles, int nodeTotalLeafCount, int subtreeBudget, in NodeChild child, ref QuickList<(int nodeIndex, int subtreeBudget)> stack, ref QuickList rootRefinementSubtrees) { //We automatically accept any child as a subtree for the refinement process if: @@ -216,15 +219,121 @@ private void TryPushChildForRootRefinement( } } - unsafe void CollectSubtreesForRootRefinement(int rootRefinementSize, int subtreeRefinementSize, BufferPool pool, in QuickList subtreeRefinementTargets, ref QuickList rootRefinementNodeIndices, ref QuickList rootRefinementSubtrees) + unsafe struct RootSubtreeCollectionContext + { + public int StartNode; + public int SubtreeBudget; + public int SubtreeRefinementSize; + public QuickList RootRefinementNodeIndices; + public QuickList RootRefinementSubtrees; + public Buffer> SubtreeRefinementTargetBundles; + public Tree* Tree; + + + public RootSubtreeCollectionContext(int startNodeIndex, int subtreeBudget, int subtreeRefinementSize, Buffer> subtreeRefinementTargetBundles, Tree* tree, + ref QuickList rootRefinementNodeIndices, ref QuickList rootRefinementSubtrees) + { + //Rather than allocating our own space for node indices and subtrees, we can simply suballocate out of the main list. + RootRefinementNodeIndices = new QuickList(rootRefinementNodeIndices.Span.Slice(rootRefinementNodeIndices.Count, subtreeBudget - 1)); + rootRefinementNodeIndices.Count += RootRefinementNodeIndices.Span.Length; + RootRefinementSubtrees = new QuickList(rootRefinementSubtrees.Span.Slice(rootRefinementSubtrees.Count, subtreeBudget)); + rootRefinementSubtrees.Count += subtreeBudget; + + StartNode = startNodeIndex; + SubtreeBudget = subtreeBudget; + SubtreeRefinementSize = subtreeRefinementSize; + SubtreeRefinementTargetBundles = subtreeRefinementTargetBundles; + Tree = tree; + } + + } + + unsafe void CollectSubtreesForRootRefinementMT( + int rootRefinementSize, int subtreeRefinementSize, int targetTaskSizeInSubtrees, BufferPool pool, + QuickList subtreeRefinementTargets, ref QuickList rootRefinementNodeIndices, ref QuickList rootRefinementSubtrees, + int workerIndex, TaskStack* taskStack, IThreadDispatcher dispatcher) { - var rootStack = new QuickList<(int nodeIndex, int subtreeBudget)>(rootRefinementSize, pool); - rootStack.AllocateUnsafely() = (0, rootRefinementSize); var subtreeRefinementTargetBundles = new Buffer>(subtreeRefinementTargets.Span.Memory, BundleIndexing.GetBundleCount(subtreeRefinementTargets.Count)); - while (rootStack.TryPop(out var nodeToVisit)) + var taskCountEstimate = (int)float.Ceiling((float)rootRefinementSize / targetTaskSizeInSubtrees) * 2; + QuickList taskContexts = new QuickList(taskCountEstimate, pool); + var stack = new QuickList<(int nodeIndex, int subtreeBudget)>(rootRefinementSize, pool); + var tree = this; + stack.AllocateUnsafely() = (0, rootRefinementSize); + while (stack.TryPop(out var nodeToVisit)) { rootRefinementNodeIndices.AllocateUnsafely() = nodeToVisit.nodeIndex; - ref var node = ref Nodes[nodeToVisit.nodeIndex]; + ref var node = ref tree.Nodes[nodeToVisit.nodeIndex]; + var nodeTotalLeafCount = node.A.LeafCount + node.B.LeafCount; + Debug.Assert(nodeToVisit.subtreeBudget <= nodeTotalLeafCount); + var lowerSubtreeBudget = int.Min((nodeToVisit.subtreeBudget + 1) / 2, int.Min(node.A.LeafCount, node.B.LeafCount)); + var higherSubtreeBudget = nodeToVisit.subtreeBudget - lowerSubtreeBudget; + var useSmallerForA = lowerSubtreeBudget == node.A.LeafCount; + var subtreeBudgetA = useSmallerForA ? lowerSubtreeBudget : higherSubtreeBudget; + var subtreeBudgetB = useSmallerForA ? higherSubtreeBudget : lowerSubtreeBudget; + + //Technically, while we could kick off a task *immediately*, traversing to find all tasks is a submicrosecond affair. + if (subtreeBudgetB <= targetTaskSizeInSubtrees && node.B.Index >= 0 && !IsNodeChildSubtreeRefinementTarget(subtreeRefinementTargetBundles, node.B, nodeTotalLeafCount, subtreeRefinementSize)) + { + taskContexts.Allocate(pool) = new RootSubtreeCollectionContext(node.B.Index, subtreeBudgetB, subtreeRefinementSize, subtreeRefinementTargetBundles, &tree, ref rootRefinementNodeIndices, ref rootRefinementSubtrees); + } + else + TryPushChildForRootRefinement(subtreeRefinementSize, subtreeRefinementTargetBundles, nodeTotalLeafCount, subtreeBudgetB, node.B, ref stack, ref rootRefinementSubtrees); + + if (subtreeBudgetA <= targetTaskSizeInSubtrees && node.A.Index >= 0 && !IsNodeChildSubtreeRefinementTarget(subtreeRefinementTargetBundles, node.A, nodeTotalLeafCount, subtreeRefinementSize)) + { + taskContexts.Allocate(pool) = new RootSubtreeCollectionContext(node.A.Index, subtreeBudgetA, subtreeRefinementSize, subtreeRefinementTargetBundles, &tree, ref rootRefinementNodeIndices, ref rootRefinementSubtrees); + } + else + TryPushChildForRootRefinement(subtreeRefinementSize, subtreeRefinementTargetBundles, nodeTotalLeafCount, subtreeBudgetA, node.A, ref stack, ref rootRefinementSubtrees); + } + stack.Dispose(pool); + for (int i = 0; i < taskContexts.Count; ++i) + { + new Task(&CollectSubtreesForRootRefinementTask, &taskContexts, i).Run(workerIndex, dispatcher); + } + //taskStack->For(&CollectSubtreesForRootRefinementTask, &taskContexts, 0, taskContexts.Count, workerIndex, dispatcher); + //While we pushed everything into a worst case sized single buffer, it's likely that there are gaps in the RootRefinementNodeIndices and rootRefinementSubtrees because of subtree refinement roots being encountered. + //Hard to justify doing another multithreaded execution to handle this, so we're just going to + //(Why not interlocked or something from the MT context? determinism, basically. Didn't want to create even more codepath permutations for something that's pretty cheap anyway.) + var accumulatedLeftMove = 0; + int* lastNodeIndicesRegionEnd = null; + NodeChild* lastSubtreesRegionEnd = null; + for (int i = 0; i < taskContexts.Count; ++i) + { + ref var context = ref taskContexts[i]; + if (accumulatedLeftMove > 0) + { + //Have to move some memory. Copy everything from the end of the previous region (including the possibly unused trailing bit), up to the end of the current region (only the used bits!). + //Move it by the accumulatedLeftMove. + var nodeIndicesByteCount = sizeof(int) * (int)((context.RootRefinementNodeIndices.Span.Memory + context.RootRefinementNodeIndices.Count) - lastNodeIndicesRegionEnd); + var subtreesByteCount = sizeof(NodeChild) * (int)((context.RootRefinementSubtrees.Span.Memory + context.RootRefinementSubtrees.Count) - lastSubtreesRegionEnd); + Buffer.MemoryCopy(lastNodeIndicesRegionEnd, lastNodeIndicesRegionEnd - accumulatedLeftMove, nodeIndicesByteCount, nodeIndicesByteCount); + Buffer.MemoryCopy(lastSubtreesRegionEnd, lastSubtreesRegionEnd - accumulatedLeftMove, subtreesByteCount, subtreesByteCount); + } + lastNodeIndicesRegionEnd = context.RootRefinementNodeIndices.Span.Memory + context.RootRefinementNodeIndices.Span.Length; + lastSubtreesRegionEnd = context.RootRefinementSubtrees.Span.Memory + context.RootRefinementSubtrees.Span.Length; + var additionalMovementRequiredForFutureRegions = context.RootRefinementNodeIndices.Span.Length - context.RootRefinementNodeIndices.Count; + accumulatedLeftMove += additionalMovementRequiredForFutureRegions; + } + rootRefinementNodeIndices.Count -= accumulatedLeftMove; + rootRefinementSubtrees.Count -= accumulatedLeftMove; + Debug.Assert(rootRefinementNodeIndices.Count == rootRefinementSubtrees.Count - 1); + for (int i = 0; i < rootRefinementSubtrees.Count; ++i) + { + var subtree = rootRefinementSubtrees[i]; + var span = subtree.Max - subtree.Min; + if (span.LengthSquared() < 1e-12f || float.IsNaN(span.LengthSquared()) || subtree.LeafCount == 0 || subtree.Index == 0) + Console.WriteLine("No :)"); + } + taskContexts.Dispose(pool); + } + + static unsafe void CollectSubtreesForRootRefinement(ref QuickList<(int nodeIndex, int subtreeBudget)> stack, int subtreeRefinementSize, BufferPool pool, Buffer> subtreeRefinementTargetBundles, ref Tree tree, ref QuickList rootRefinementNodeIndices, ref QuickList rootRefinementSubtrees) + { + while (stack.TryPop(out var nodeToVisit)) + { + rootRefinementNodeIndices.AllocateUnsafely() = nodeToVisit.nodeIndex; + ref var node = ref tree.Nodes[nodeToVisit.nodeIndex]; var nodeTotalLeafCount = node.A.LeafCount + node.B.LeafCount; Debug.Assert(nodeToVisit.subtreeBudget <= nodeTotalLeafCount); var lowerSubtreeBudget = int.Min((nodeToVisit.subtreeBudget + 1) / 2, int.Min(node.A.LeafCount, node.B.LeafCount)); @@ -233,9 +342,29 @@ unsafe void CollectSubtreesForRootRefinement(int rootRefinementSize, int subtree var aSubtreeBudget = useSmallerForA ? lowerSubtreeBudget : higherSubtreeBudget; var bSubtreeBudget = useSmallerForA ? higherSubtreeBudget : lowerSubtreeBudget; - TryPushChildForRootRefinement(subtreeRefinementSize, subtreeRefinementTargetBundles, nodeTotalLeafCount, bSubtreeBudget, node.B, ref rootStack, ref rootRefinementSubtrees); - TryPushChildForRootRefinement(subtreeRefinementSize, subtreeRefinementTargetBundles, nodeTotalLeafCount, aSubtreeBudget, node.A, ref rootStack, ref rootRefinementSubtrees); + TryPushChildForRootRefinement(subtreeRefinementSize, subtreeRefinementTargetBundles, nodeTotalLeafCount, bSubtreeBudget, node.B, ref stack, ref rootRefinementSubtrees); + TryPushChildForRootRefinement(subtreeRefinementSize, subtreeRefinementTargetBundles, nodeTotalLeafCount, aSubtreeBudget, node.A, ref stack, ref rootRefinementSubtrees); } + } + static unsafe void CollectSubtreesForRootRefinementTask(long id, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) + { + ref var jobContext = ref *(QuickList*)untypedContext; + ref var context = ref jobContext[(int)id]; + var subtreeBudget = context.SubtreeBudget; + var pool = dispatcher.WorkerPools[workerIndex]; + var stack = new QuickList<(int nodeIndex, int subtreeBudget)>(subtreeBudget, pool); + stack.AllocateUnsafely() = (context.StartNode, subtreeBudget); + CollectSubtreesForRootRefinement(ref stack, context.SubtreeRefinementSize, pool, context.SubtreeRefinementTargetBundles, ref *context.Tree, ref context.RootRefinementNodeIndices, ref context.RootRefinementSubtrees); + stack.Dispose(pool); + } + + + unsafe void CollectSubtreesForRootRefinementST(int rootRefinementSize, int subtreeRefinementSize, BufferPool pool, in QuickList subtreeRefinementTargets, ref QuickList rootRefinementNodeIndices, ref QuickList rootRefinementSubtrees) + { + var rootStack = new QuickList<(int nodeIndex, int subtreeBudget)>(rootRefinementSize, pool); + rootStack.AllocateUnsafely() = (0, rootRefinementSize); + var subtreeRefinementTargetBundles = new Buffer>(subtreeRefinementTargets.Span.Memory, BundleIndexing.GetBundleCount(subtreeRefinementTargets.Count)); + CollectSubtreesForRootRefinement(ref rootStack, subtreeRefinementSize, pool, subtreeRefinementTargetBundles, ref this, ref rootRefinementNodeIndices, ref rootRefinementSubtrees); rootStack.Dispose(pool); } @@ -286,7 +415,7 @@ public unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementStar //We now know which nodes are the roots of subtree refinements; the root refinement can avoid traversing through them. var rootRefinementSubtrees = new QuickList(rootRefinementSize, pool); var rootRefinementNodeIndices = new QuickList(rootRefinementSize, pool); - CollectSubtreesForRootRefinement(rootRefinementSize, subtreeRefinementSize, pool, subtreeRefinementTargets, ref rootRefinementNodeIndices, ref rootRefinementSubtrees); + CollectSubtreesForRootRefinementST(rootRefinementSize, subtreeRefinementSize, pool, subtreeRefinementTargets, ref rootRefinementNodeIndices, ref rootRefinementSubtrees); //Now that we have a list of nodes to refine, we can run the root refinement. Debug.Assert(rootRefinementNodeIndices.Count == rootRefinementSubtrees.Count - 1); @@ -357,7 +486,18 @@ unsafe static void ExecuteRootRefinementTask(long id, void* untypedContext, int //We now know which nodes are the roots of subtree refinements; the root refinement can avoid traversing through them. var rootRefinementSubtrees = new QuickList(context.RootRefinementSize, pool); var rootRefinementNodeIndices = new QuickList(context.RootRefinementSize, pool); - context.Tree.CollectSubtreesForRootRefinement(context.RootRefinementSize, context.SubtreeRefinementSize, pool, context.SubtreeRefinementTargets, ref rootRefinementNodeIndices, ref rootRefinementSubtrees); + + //if (taskCount > 1) + //{ + // var targetTaskSizeInSubtrees = (context.RootRefinementSize + taskCount - 1) / taskCount; + // context.Tree.CollectSubtreesForRootRefinementMT(context.RootRefinementSize, context.SubtreeRefinementSize, targetTaskSizeInSubtrees, pool, + // context.SubtreeRefinementTargets, ref rootRefinementNodeIndices, ref rootRefinementSubtrees, workerIndex, context.TaskStack, dispatcher); + //} + //else + { + context.Tree.CollectSubtreesForRootRefinementST(context.RootRefinementSize, context.SubtreeRefinementSize, pool, context.SubtreeRefinementTargets, ref rootRefinementNodeIndices, ref rootRefinementSubtrees); + + } //var localSubtreeHash = 0; //for (int i = 0; i < rootRefinementSubtrees.Count; ++i) //{ From d10a47b4ee54b7297e62138ca4e769c83d532329 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 15 Jul 2023 19:48:10 -0500 Subject: [PATCH 751/947] Purged! --- BepuPhysics/Trees/Tree_Refine2.cs | 137 ++---------------------------- 1 file changed, 7 insertions(+), 130 deletions(-) diff --git a/BepuPhysics/Trees/Tree_Refine2.cs b/BepuPhysics/Trees/Tree_Refine2.cs index 3f0b7a934..1d454eaa6 100644 --- a/BepuPhysics/Trees/Tree_Refine2.cs +++ b/BepuPhysics/Trees/Tree_Refine2.cs @@ -219,114 +219,6 @@ private static void TryPushChildForRootRefinement( } } - unsafe struct RootSubtreeCollectionContext - { - public int StartNode; - public int SubtreeBudget; - public int SubtreeRefinementSize; - public QuickList RootRefinementNodeIndices; - public QuickList RootRefinementSubtrees; - public Buffer> SubtreeRefinementTargetBundles; - public Tree* Tree; - - - public RootSubtreeCollectionContext(int startNodeIndex, int subtreeBudget, int subtreeRefinementSize, Buffer> subtreeRefinementTargetBundles, Tree* tree, - ref QuickList rootRefinementNodeIndices, ref QuickList rootRefinementSubtrees) - { - //Rather than allocating our own space for node indices and subtrees, we can simply suballocate out of the main list. - RootRefinementNodeIndices = new QuickList(rootRefinementNodeIndices.Span.Slice(rootRefinementNodeIndices.Count, subtreeBudget - 1)); - rootRefinementNodeIndices.Count += RootRefinementNodeIndices.Span.Length; - RootRefinementSubtrees = new QuickList(rootRefinementSubtrees.Span.Slice(rootRefinementSubtrees.Count, subtreeBudget)); - rootRefinementSubtrees.Count += subtreeBudget; - - StartNode = startNodeIndex; - SubtreeBudget = subtreeBudget; - SubtreeRefinementSize = subtreeRefinementSize; - SubtreeRefinementTargetBundles = subtreeRefinementTargetBundles; - Tree = tree; - } - - } - - unsafe void CollectSubtreesForRootRefinementMT( - int rootRefinementSize, int subtreeRefinementSize, int targetTaskSizeInSubtrees, BufferPool pool, - QuickList subtreeRefinementTargets, ref QuickList rootRefinementNodeIndices, ref QuickList rootRefinementSubtrees, - int workerIndex, TaskStack* taskStack, IThreadDispatcher dispatcher) - { - var subtreeRefinementTargetBundles = new Buffer>(subtreeRefinementTargets.Span.Memory, BundleIndexing.GetBundleCount(subtreeRefinementTargets.Count)); - var taskCountEstimate = (int)float.Ceiling((float)rootRefinementSize / targetTaskSizeInSubtrees) * 2; - QuickList taskContexts = new QuickList(taskCountEstimate, pool); - var stack = new QuickList<(int nodeIndex, int subtreeBudget)>(rootRefinementSize, pool); - var tree = this; - stack.AllocateUnsafely() = (0, rootRefinementSize); - while (stack.TryPop(out var nodeToVisit)) - { - rootRefinementNodeIndices.AllocateUnsafely() = nodeToVisit.nodeIndex; - ref var node = ref tree.Nodes[nodeToVisit.nodeIndex]; - var nodeTotalLeafCount = node.A.LeafCount + node.B.LeafCount; - Debug.Assert(nodeToVisit.subtreeBudget <= nodeTotalLeafCount); - var lowerSubtreeBudget = int.Min((nodeToVisit.subtreeBudget + 1) / 2, int.Min(node.A.LeafCount, node.B.LeafCount)); - var higherSubtreeBudget = nodeToVisit.subtreeBudget - lowerSubtreeBudget; - var useSmallerForA = lowerSubtreeBudget == node.A.LeafCount; - var subtreeBudgetA = useSmallerForA ? lowerSubtreeBudget : higherSubtreeBudget; - var subtreeBudgetB = useSmallerForA ? higherSubtreeBudget : lowerSubtreeBudget; - - //Technically, while we could kick off a task *immediately*, traversing to find all tasks is a submicrosecond affair. - if (subtreeBudgetB <= targetTaskSizeInSubtrees && node.B.Index >= 0 && !IsNodeChildSubtreeRefinementTarget(subtreeRefinementTargetBundles, node.B, nodeTotalLeafCount, subtreeRefinementSize)) - { - taskContexts.Allocate(pool) = new RootSubtreeCollectionContext(node.B.Index, subtreeBudgetB, subtreeRefinementSize, subtreeRefinementTargetBundles, &tree, ref rootRefinementNodeIndices, ref rootRefinementSubtrees); - } - else - TryPushChildForRootRefinement(subtreeRefinementSize, subtreeRefinementTargetBundles, nodeTotalLeafCount, subtreeBudgetB, node.B, ref stack, ref rootRefinementSubtrees); - - if (subtreeBudgetA <= targetTaskSizeInSubtrees && node.A.Index >= 0 && !IsNodeChildSubtreeRefinementTarget(subtreeRefinementTargetBundles, node.A, nodeTotalLeafCount, subtreeRefinementSize)) - { - taskContexts.Allocate(pool) = new RootSubtreeCollectionContext(node.A.Index, subtreeBudgetA, subtreeRefinementSize, subtreeRefinementTargetBundles, &tree, ref rootRefinementNodeIndices, ref rootRefinementSubtrees); - } - else - TryPushChildForRootRefinement(subtreeRefinementSize, subtreeRefinementTargetBundles, nodeTotalLeafCount, subtreeBudgetA, node.A, ref stack, ref rootRefinementSubtrees); - } - stack.Dispose(pool); - for (int i = 0; i < taskContexts.Count; ++i) - { - new Task(&CollectSubtreesForRootRefinementTask, &taskContexts, i).Run(workerIndex, dispatcher); - } - //taskStack->For(&CollectSubtreesForRootRefinementTask, &taskContexts, 0, taskContexts.Count, workerIndex, dispatcher); - //While we pushed everything into a worst case sized single buffer, it's likely that there are gaps in the RootRefinementNodeIndices and rootRefinementSubtrees because of subtree refinement roots being encountered. - //Hard to justify doing another multithreaded execution to handle this, so we're just going to - //(Why not interlocked or something from the MT context? determinism, basically. Didn't want to create even more codepath permutations for something that's pretty cheap anyway.) - var accumulatedLeftMove = 0; - int* lastNodeIndicesRegionEnd = null; - NodeChild* lastSubtreesRegionEnd = null; - for (int i = 0; i < taskContexts.Count; ++i) - { - ref var context = ref taskContexts[i]; - if (accumulatedLeftMove > 0) - { - //Have to move some memory. Copy everything from the end of the previous region (including the possibly unused trailing bit), up to the end of the current region (only the used bits!). - //Move it by the accumulatedLeftMove. - var nodeIndicesByteCount = sizeof(int) * (int)((context.RootRefinementNodeIndices.Span.Memory + context.RootRefinementNodeIndices.Count) - lastNodeIndicesRegionEnd); - var subtreesByteCount = sizeof(NodeChild) * (int)((context.RootRefinementSubtrees.Span.Memory + context.RootRefinementSubtrees.Count) - lastSubtreesRegionEnd); - Buffer.MemoryCopy(lastNodeIndicesRegionEnd, lastNodeIndicesRegionEnd - accumulatedLeftMove, nodeIndicesByteCount, nodeIndicesByteCount); - Buffer.MemoryCopy(lastSubtreesRegionEnd, lastSubtreesRegionEnd - accumulatedLeftMove, subtreesByteCount, subtreesByteCount); - } - lastNodeIndicesRegionEnd = context.RootRefinementNodeIndices.Span.Memory + context.RootRefinementNodeIndices.Span.Length; - lastSubtreesRegionEnd = context.RootRefinementSubtrees.Span.Memory + context.RootRefinementSubtrees.Span.Length; - var additionalMovementRequiredForFutureRegions = context.RootRefinementNodeIndices.Span.Length - context.RootRefinementNodeIndices.Count; - accumulatedLeftMove += additionalMovementRequiredForFutureRegions; - } - rootRefinementNodeIndices.Count -= accumulatedLeftMove; - rootRefinementSubtrees.Count -= accumulatedLeftMove; - Debug.Assert(rootRefinementNodeIndices.Count == rootRefinementSubtrees.Count - 1); - for (int i = 0; i < rootRefinementSubtrees.Count; ++i) - { - var subtree = rootRefinementSubtrees[i]; - var span = subtree.Max - subtree.Min; - if (span.LengthSquared() < 1e-12f || float.IsNaN(span.LengthSquared()) || subtree.LeafCount == 0 || subtree.Index == 0) - Console.WriteLine("No :)"); - } - taskContexts.Dispose(pool); - } static unsafe void CollectSubtreesForRootRefinement(ref QuickList<(int nodeIndex, int subtreeBudget)> stack, int subtreeRefinementSize, BufferPool pool, Buffer> subtreeRefinementTargetBundles, ref Tree tree, ref QuickList rootRefinementNodeIndices, ref QuickList rootRefinementSubtrees) { @@ -346,18 +238,6 @@ static unsafe void CollectSubtreesForRootRefinement(ref QuickList<(int nodeIndex TryPushChildForRootRefinement(subtreeRefinementSize, subtreeRefinementTargetBundles, nodeTotalLeafCount, aSubtreeBudget, node.A, ref stack, ref rootRefinementSubtrees); } } - static unsafe void CollectSubtreesForRootRefinementTask(long id, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) - { - ref var jobContext = ref *(QuickList*)untypedContext; - ref var context = ref jobContext[(int)id]; - var subtreeBudget = context.SubtreeBudget; - var pool = dispatcher.WorkerPools[workerIndex]; - var stack = new QuickList<(int nodeIndex, int subtreeBudget)>(subtreeBudget, pool); - stack.AllocateUnsafely() = (context.StartNode, subtreeBudget); - CollectSubtreesForRootRefinement(ref stack, context.SubtreeRefinementSize, pool, context.SubtreeRefinementTargetBundles, ref *context.Tree, ref context.RootRefinementNodeIndices, ref context.RootRefinementSubtrees); - stack.Dispose(pool); - } - unsafe void CollectSubtreesForRootRefinementST(int rootRefinementSize, int subtreeRefinementSize, BufferPool pool, in QuickList subtreeRefinementTargets, ref QuickList rootRefinementNodeIndices, ref QuickList rootRefinementSubtrees) { @@ -413,6 +293,12 @@ public unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementStar ((Span)subtreeRefinementTargets.Span)[subtreeRefinementTargets.Count..].Fill(-1); //We now know which nodes are the roots of subtree refinements; the root refinement can avoid traversing through them. + //TODO: A multithreaded version of the collection phase *could* help a little, but there's a few problems to overcome: + // 1. The cost of the collection phase is pretty cheap. Around the cost of a refit. Multithreading a collection phase of a thousand nodes is probably going to be net slower. + // 2. It's difficult to do it deterministically without having to do a postpass to copy things into position in the contiguous buffer, and touching all that memory is a big hit. + // 3. The main use case for refinements is in the broad phase. This will usually be run next to a dynamic refit refine, so we'll already have decent utilization. + // 4. I don't wanna. + //So, punting this for later. A nondeterministic implementation wouldn't be too bad. Could always just fall back to ST when deterministic flag is set. var rootRefinementSubtrees = new QuickList(rootRefinementSize, pool); var rootRefinementNodeIndices = new QuickList(rootRefinementSize, pool); CollectSubtreesForRootRefinementST(rootRefinementSize, subtreeRefinementSize, pool, subtreeRefinementTargets, ref rootRefinementNodeIndices, ref rootRefinementSubtrees); @@ -487,17 +373,8 @@ unsafe static void ExecuteRootRefinementTask(long id, void* untypedContext, int var rootRefinementSubtrees = new QuickList(context.RootRefinementSize, pool); var rootRefinementNodeIndices = new QuickList(context.RootRefinementSize, pool); - //if (taskCount > 1) - //{ - // var targetTaskSizeInSubtrees = (context.RootRefinementSize + taskCount - 1) / taskCount; - // context.Tree.CollectSubtreesForRootRefinementMT(context.RootRefinementSize, context.SubtreeRefinementSize, targetTaskSizeInSubtrees, pool, - // context.SubtreeRefinementTargets, ref rootRefinementNodeIndices, ref rootRefinementSubtrees, workerIndex, context.TaskStack, dispatcher); - //} - //else - { - context.Tree.CollectSubtreesForRootRefinementST(context.RootRefinementSize, context.SubtreeRefinementSize, pool, context.SubtreeRefinementTargets, ref rootRefinementNodeIndices, ref rootRefinementSubtrees); + context.Tree.CollectSubtreesForRootRefinementST(context.RootRefinementSize, context.SubtreeRefinementSize, pool, context.SubtreeRefinementTargets, ref rootRefinementNodeIndices, ref rootRefinementSubtrees); - } //var localSubtreeHash = 0; //for (int i = 0; i < rootRefinementSubtrees.Count; ++i) //{ From 3943b6860137384fd53ef7052013f46201c0f6f8 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 15 Jul 2023 19:49:19 -0500 Subject: [PATCH 752/947] More relevant position. --- BepuPhysics/Trees/Tree_Refine2.cs | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/BepuPhysics/Trees/Tree_Refine2.cs b/BepuPhysics/Trees/Tree_Refine2.cs index 1d454eaa6..78c2e7a14 100644 --- a/BepuPhysics/Trees/Tree_Refine2.cs +++ b/BepuPhysics/Trees/Tree_Refine2.cs @@ -239,7 +239,7 @@ static unsafe void CollectSubtreesForRootRefinement(ref QuickList<(int nodeIndex } } - unsafe void CollectSubtreesForRootRefinementST(int rootRefinementSize, int subtreeRefinementSize, BufferPool pool, in QuickList subtreeRefinementTargets, ref QuickList rootRefinementNodeIndices, ref QuickList rootRefinementSubtrees) + unsafe void CollectSubtreesForRootRefinement(int rootRefinementSize, int subtreeRefinementSize, BufferPool pool, in QuickList subtreeRefinementTargets, ref QuickList rootRefinementNodeIndices, ref QuickList rootRefinementSubtrees) { var rootStack = new QuickList<(int nodeIndex, int subtreeBudget)>(rootRefinementSize, pool); rootStack.AllocateUnsafely() = (0, rootRefinementSize); @@ -292,16 +292,9 @@ public unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementStar //Fill the trailing slots in the list with -1 to avoid matches. ((Span)subtreeRefinementTargets.Span)[subtreeRefinementTargets.Count..].Fill(-1); - //We now know which nodes are the roots of subtree refinements; the root refinement can avoid traversing through them. - //TODO: A multithreaded version of the collection phase *could* help a little, but there's a few problems to overcome: - // 1. The cost of the collection phase is pretty cheap. Around the cost of a refit. Multithreading a collection phase of a thousand nodes is probably going to be net slower. - // 2. It's difficult to do it deterministically without having to do a postpass to copy things into position in the contiguous buffer, and touching all that memory is a big hit. - // 3. The main use case for refinements is in the broad phase. This will usually be run next to a dynamic refit refine, so we'll already have decent utilization. - // 4. I don't wanna. - //So, punting this for later. A nondeterministic implementation wouldn't be too bad. Could always just fall back to ST when deterministic flag is set. var rootRefinementSubtrees = new QuickList(rootRefinementSize, pool); var rootRefinementNodeIndices = new QuickList(rootRefinementSize, pool); - CollectSubtreesForRootRefinementST(rootRefinementSize, subtreeRefinementSize, pool, subtreeRefinementTargets, ref rootRefinementNodeIndices, ref rootRefinementSubtrees); + CollectSubtreesForRootRefinement(rootRefinementSize, subtreeRefinementSize, pool, subtreeRefinementTargets, ref rootRefinementNodeIndices, ref rootRefinementSubtrees); //Now that we have a list of nodes to refine, we can run the root refinement. Debug.Assert(rootRefinementNodeIndices.Count == rootRefinementSubtrees.Count - 1); @@ -373,7 +366,14 @@ unsafe static void ExecuteRootRefinementTask(long id, void* untypedContext, int var rootRefinementSubtrees = new QuickList(context.RootRefinementSize, pool); var rootRefinementNodeIndices = new QuickList(context.RootRefinementSize, pool); - context.Tree.CollectSubtreesForRootRefinementST(context.RootRefinementSize, context.SubtreeRefinementSize, pool, context.SubtreeRefinementTargets, ref rootRefinementNodeIndices, ref rootRefinementSubtrees); + //We now know which nodes are the roots of subtree refinements; the root refinement can avoid traversing through them. + //TODO: A multithreaded version of the collection phase *could* help a little, but there's a few problems to overcome: + // 1. The cost of the collection phase is pretty cheap. Around the cost of a refit. Multithreading a collection phase of a thousand nodes is probably going to be net slower. + // 2. It's difficult to do it deterministically without having to do a postpass to copy things into position in the contiguous buffer, and touching all that memory is a big hit. + // 3. The main use case for refinements is in the broad phase. This will usually be run next to a dynamic refit refine, so we'll already have decent utilization. + // 4. I don't wanna. + //So, punting this for later. A nondeterministic implementation wouldn't be too bad. Could always just fall back to ST when deterministic flag is set. + context.Tree.CollectSubtreesForRootRefinement(context.RootRefinementSize, context.SubtreeRefinementSize, pool, context.SubtreeRefinementTargets, ref rootRefinementNodeIndices, ref rootRefinementSubtrees); //var localSubtreeHash = 0; //for (int i = 0; i < rootRefinementSubtrees.Count; ++i) From 6dc9a9a7479df5772599512b8ae4badb2c54369f Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 15 Jul 2023 21:00:50 -0500 Subject: [PATCH 753/947] Root reify multithreading. --- BepuPhysics/Trees/Tree_Refine2.cs | 76 ++++++++++++++++++++++++++----- 1 file changed, 64 insertions(+), 12 deletions(-) diff --git a/BepuPhysics/Trees/Tree_Refine2.cs b/BepuPhysics/Trees/Tree_Refine2.cs index 78c2e7a14..7f83c38a9 100644 --- a/BepuPhysics/Trees/Tree_Refine2.cs +++ b/BepuPhysics/Trees/Tree_Refine2.cs @@ -19,7 +19,7 @@ public partial struct Tree { const int flagForRootRefinementSubtree = 1 << 30; - void ReifyRootRefinementNodeChild(ref int index, ref QuickList refinementNodeIndices, int realNodeIndex, int childIndexInParent) + readonly void ReifyRootRefinementNodeChild(ref int index, ref QuickList refinementNodeIndices, int realNodeIndex, int childIndexInParent) { //Root refinements mark internal subtrees with a flag in the second to last index. if (index < 0) @@ -50,23 +50,68 @@ void ReifyRootRefinementNodeChild(ref int index, ref QuickList refinementNo } } - void ReifyRootRefinement(QuickList refinementNodeIndices, Buffer refinementNodes) + static unsafe void ReifyRootRefinement(int startIndex, int endIndex, QuickList nodeIndices, Buffer refinementNodes, Tree tree) { - for (int i = 0; i < refinementNodeIndices.Count; ++i) + for (int i = startIndex; i < endIndex; ++i) { //refinementNodeIndices maps "refinement index space" to "real index space"; we can use it to update child pointers to the real locations. - var realNodeIndex = refinementNodeIndices[i]; + var realNodeIndex = nodeIndices[i]; ref var refinedNode = ref refinementNodes[i]; //Map child indices, and update leaf references. - ReifyRootRefinementNodeChild(ref refinedNode.A.Index, ref refinementNodeIndices, realNodeIndex, 0); - ReifyRootRefinementNodeChild(ref refinedNode.B.Index, ref refinementNodeIndices, realNodeIndex, 1); - Nodes[realNodeIndex] = refinedNode; - Debug.Assert(Metanodes[realNodeIndex].Parent < 0 || Unsafe.Add(ref Nodes[Metanodes[realNodeIndex].Parent].A, Metanodes[realNodeIndex].IndexInParent).LeafCount == refinedNode.A.LeafCount + refinedNode.B.LeafCount); - Debug.Assert(Metanodes[realNodeIndex].Parent < 0 || Unsafe.Add(ref Nodes[Metanodes[realNodeIndex].Parent].A, Metanodes[realNodeIndex].IndexInParent).Index == realNodeIndex); + tree.ReifyRootRefinementNodeChild(ref refinedNode.A.Index, ref nodeIndices, realNodeIndex, 0); + tree.ReifyRootRefinementNodeChild(ref refinedNode.B.Index, ref nodeIndices, realNodeIndex, 1); + tree.Nodes[realNodeIndex] = refinedNode; } } - void ReifyRefinementNodeChild(ref int index, ref QuickList refinementNodeIndices, int realNodeIndex, int childIndexInParent) + readonly void ReifyRootRefinementST(QuickList refinementNodeIndices, Buffer refinementNodes) + { + ReifyRootRefinement(0, refinementNodeIndices.Count, refinementNodeIndices, refinementNodes, this); + } + + unsafe struct ReifyRootRefinementContext + { + public QuickList* RefinementNodeIndices; + public Buffer* RefinementNodes; + public int StartIndex; + public int EndIndex; + public Tree* Tree; + } + + static unsafe void ReifyRootRefinementTask(long id, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) + { + ref var context = ref *(ReifyRootRefinementContext*)untypedContext; + ReifyRootRefinement(context.StartIndex, context.EndIndex, *context.RefinementNodeIndices, *context.RefinementNodes, *context.Tree); + } + + readonly unsafe void ReifyRootRefinementMT(QuickList* refinementNodeIndices, Buffer* refinementNodes, int targetTaskCount, int workerIndex, TaskStack* taskStack, IThreadDispatcher dispatcher) + { + var nodesPerTask = refinementNodeIndices->Count / targetTaskCount; + var remainder = refinementNodeIndices->Count - targetTaskCount * nodesPerTask; + Debug.Assert(targetTaskCount < 1024, "We used a stackalloc for these task allocations under the assumption that there would be *very* few required, and that's clearly wrong here! What's going on?"); + Span tasks = stackalloc Task[targetTaskCount]; + ReifyRootRefinementContext* contexts = stackalloc ReifyRootRefinementContext[targetTaskCount]; + var tree = this; + + var previousEnd = 0; + for (int i = 0; i < tasks.Length; ++i) + { + var count = i < remainder ? nodesPerTask + 1 : nodesPerTask; + ref var context = ref contexts[i]; + context.RefinementNodeIndices = refinementNodeIndices; + context.RefinementNodes = refinementNodes; + context.Tree = &tree; + context.StartIndex = previousEnd; + previousEnd += count; + context.EndIndex = previousEnd; + tasks[i] = new Task(&ReifyRootRefinementTask, contexts + i, i); + } + + taskStack->RunTasks(tasks, workerIndex, dispatcher); + } + + + readonly void ReifyRefinementNodeChild(ref int index, ref QuickList refinementNodeIndices, int realNodeIndex, int childIndexInParent) { //Root refinements mark internal subtrees with a flag in the second to last index. if (index < 0) @@ -305,7 +350,7 @@ public unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementStar var rootRefinementMetanodes = refinementMetanodesAllocation.Slice(0, rootRefinementNodeIndices.Count); //Passing 'default' for the leaves tells the binned builder to not worry about updating leaves. BinnedBuild(rootRefinementSubtrees, rootRefinementNodes, rootRefinementMetanodes, default, pool); - ReifyRootRefinement(rootRefinementNodeIndices, rootRefinementNodes); + ReifyRootRefinementST(rootRefinementNodeIndices, rootRefinementNodes); rootRefinementSubtrees.Dispose(pool); rootRefinementNodeIndices.Dispose(pool); @@ -410,7 +455,14 @@ unsafe static void ExecuteRootRefinementTask(long id, void* untypedContext, int ////} //Console.WriteLine($"Refinement targets count {debugIndex}: {context.SubtreeRefinementTargets.Count}, node topology hash: {debugAccumulatorForNodeTopology}, subtree indices hash: {debugAccumulatorForSubtreeIndices}"); //++debugIndex; - context.Tree.ReifyRootRefinement(rootRefinementNodeIndices, rootRefinementNodes); + if (taskCount > 1) + { + context.Tree.ReifyRootRefinementMT(&rootRefinementNodeIndices, &rootRefinementNodes, taskCount, workerIndex, context.TaskStack, dispatcher); + } + else + { + context.Tree.ReifyRootRefinementST(rootRefinementNodeIndices, rootRefinementNodes); + } rootRefinementSubtrees.Dispose(pool); rootRefinementNodeIndices.Dispose(pool); rootRefinementNodes.Dispose(pool); From 150a819417fdb93686faea82aa037583735315d9 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 20 Jul 2023 16:05:35 -0500 Subject: [PATCH 754/947] Fixed a fairly major oops in binned builder BuildNode for subtreeCount == 1 case. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index a4417028d..79e913e6a 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -50,8 +50,8 @@ static void BuildNode( else { aIndex = nodeIndex + 1; - node.A.Index = aIndex; } + node.A.Index = aIndex; if (subtreeCountB == 1) { bIndex = subtrees[^1].Index; @@ -66,8 +66,8 @@ static void BuildNode( else { bIndex = nodeIndex + subtreeCountA; //parentNodeIndex + 1 + (subtreeCountA - 1) - node.B.Index = bIndex; } + node.B.Index = bIndex; } internal static float ComputeBoundsMetric(BoundingBox4 bounds) => ComputeBoundsMetric(bounds.Min, bounds.Max); @@ -1080,7 +1080,6 @@ static unsafe void BinnedBuildNode( else BinSubtrees(centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, subtrees, binBoundingBoxes, binCentroidBoundingBoxes, binLeafCounts, subtreeBinIndices); } - //Identify the split index by examining the SAH of very split option. //Premerge from left to right so we have a sorta-summed area table to cheaply look up all possible child A bounds as we scan. binBoundingBoxesScan[0] = binBoundingBoxes[0]; @@ -1214,7 +1213,6 @@ static unsafe void BinnedBuildNode( } } } - var leafCountB = bestLeafCountB; var leafCountA = totalLeafCount - leafCountB; Debug.Assert(subtreeCountA + subtreeCountB == subtreeCount); From 1566048fb80fd41de2bdb07dd709af61680f003a Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 20 Jul 2023 16:18:38 -0500 Subject: [PATCH 755/947] MT binned builder for subtree refines. There's a very rare race conditioning hiding in the management of metanodes reification. --- BepuPhysics/Trees/Tree_Refine2.cs | 32 ++++++++++++++++++++++--------- 1 file changed, 23 insertions(+), 9 deletions(-) diff --git a/BepuPhysics/Trees/Tree_Refine2.cs b/BepuPhysics/Trees/Tree_Refine2.cs index 7f83c38a9..0c1660d30 100644 --- a/BepuPhysics/Trees/Tree_Refine2.cs +++ b/BepuPhysics/Trees/Tree_Refine2.cs @@ -44,6 +44,7 @@ readonly void ReifyRootRefinementNodeChild(ref int index, ref QuickList ref //Just as leaves need to be updated to point at the new node state, parent pointers for internal nodes need be updated too. //Note that this touches memory associated with nodes that weren't included in the refinement. //This is only safe if the subtree refinement either occurs sequentially with root refinement, or the subtree refinement doesn't touch the subtree refinement root's metanode. + //NOTE: This means the binned builder *should not touch the metanodes*. ref var childMetanode = ref Metanodes[index]; childMetanode.Parent = realNodeIndex; childMetanode.IndexInParent = childIndexInParent; @@ -111,9 +112,8 @@ readonly unsafe void ReifyRootRefinementMT(QuickList* refinementNodeIndices } - readonly void ReifyRefinementNodeChild(ref int index, ref QuickList refinementNodeIndices, int realNodeIndex, int childIndexInParent) + readonly void ReifySubtreeRefinementNodeChild(ref int index, ref QuickList refinementNodeIndices, int realNodeIndex, int childIndexInParent) { - //Root refinements mark internal subtrees with a flag in the second to last index. if (index < 0) { //The child is a leaf. @@ -126,13 +126,14 @@ readonly void ReifyRefinementNodeChild(ref int index, ref QuickList refinem //Just as leaves need to be updated to point at the new node state, parent pointers for internal nodes need be updated too. //Note that this touches memory associated with nodes that weren't included in the refinement. //This is only safe if the subtree refinement either occurs sequentially with root refinement, or the subtree refinement doesn't touch the subtree refinement root's metanode. + //NOTE: This means the binned builder *should not touch the metanodes*. ref var childMetanode = ref Metanodes[index]; childMetanode.Parent = realNodeIndex; childMetanode.IndexInParent = childIndexInParent; } } - void ReifyRefinement(QuickList refinementNodeIndices, Buffer refinementNodes) + void ReifySubtreeRefinement(QuickList refinementNodeIndices, Buffer refinementNodes) { for (int i = 0; i < refinementNodeIndices.Count; ++i) { @@ -141,8 +142,8 @@ void ReifyRefinement(QuickList refinementNodeIndices, Buffer refineme ref var refinedNode = ref refinementNodes[i]; //Map child indices, and update leaf references. //Root refinements mark internal subtrees with a flag in the second to last index. - ReifyRefinementNodeChild(ref refinedNode.A.Index, ref refinementNodeIndices, realNodeIndex, 0); - ReifyRefinementNodeChild(ref refinedNode.B.Index, ref refinementNodeIndices, realNodeIndex, 1); + ReifySubtreeRefinementNodeChild(ref refinedNode.A.Index, ref refinementNodeIndices, realNodeIndex, 0); + ReifySubtreeRefinementNodeChild(ref refinedNode.B.Index, ref refinementNodeIndices, realNodeIndex, 1); Nodes[realNodeIndex] = refinedNode; //Debug.Assert(Metanodes[realNodeIndex].Parent < 0 || Unsafe.Add(ref Nodes[Metanodes[realNodeIndex].Parent].A, Metanodes[realNodeIndex].IndexInParent).Index == realNodeIndex); } @@ -367,7 +368,7 @@ public unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementStar var refinementMetanodes = refinementMetanodesAllocation.Slice(0, subtreeRefinementNodeIndices.Count); //Passing 'default' for the leaves tells the binned builder to not worry about updating leaves. BinnedBuild(subtreeRefinementLeaves, refinementNodes, refinementMetanodes, default, pool); - ReifyRefinement(subtreeRefinementNodeIndices, refinementNodes); + ReifySubtreeRefinement(subtreeRefinementNodeIndices, refinementNodes); subtreeRefinementNodeIndices.Count = 0; subtreeRefinementLeaves.Count = 0; @@ -463,6 +464,7 @@ unsafe static void ExecuteRootRefinementTask(long id, void* untypedContext, int { context.Tree.ReifyRootRefinementST(rootRefinementNodeIndices, rootRefinementNodes); } + rootRefinementSubtrees.Dispose(pool); rootRefinementNodeIndices.Dispose(pool); rootRefinementNodes.Dispose(pool); @@ -473,18 +475,30 @@ unsafe static void ExecuteSubtreeRefinementTask(long subtreeRefinementTarget, vo ref var context = ref *(RefinementContext*)untypedContext; var pool = dispatcher.WorkerPools[workerIndex]; + ref var refinementRootNode = ref context.Tree.Nodes[(int)subtreeRefinementTarget]; + var refinementLeafCount = refinementRootNode.A.LeafCount + refinementRootNode.B.LeafCount; + var taskCount = (int)float.Ceiling(context.TargetTaskBudget * (float)refinementLeafCount / (context.RootRefinementSize + context.TotalLeafCountInSubtrees)); + var subtreeRefinementNodeIndices = new QuickList(context.SubtreeRefinementSize, pool); var subtreeRefinementLeaves = new QuickList(context.SubtreeRefinementSize, pool); var subtreeStackBuffer = new Buffer(context.SubtreeRefinementSize, pool); //Accumulate nodes and leaves with a prepass. context.Tree.CollectSubtreesForSubtreeRefinement((int)subtreeRefinementTarget, subtreeStackBuffer, ref subtreeRefinementNodeIndices, ref subtreeRefinementLeaves); - var refinementNodes = new Buffer(subtreeRefinementNodeIndices.Count, pool); var refinementMetanodes = new Buffer(subtreeRefinementNodeIndices.Count, pool); //Passing 'default' for the leaves tells the binned builder to not worry about updating leaves. - BinnedBuild(subtreeRefinementLeaves, refinementNodes, refinementMetanodes, default, pool); - context.Tree.ReifyRefinement(subtreeRefinementNodeIndices, refinementNodes); + + if (taskCount > 1) + { + BinnedBuild(subtreeRefinementLeaves, refinementNodes, refinementMetanodes, default, pool, dispatcher, context.TaskStack, + workerIndex: workerIndex, workerCount: context.WorkerCount, targetTaskCount: taskCount, deterministic: context.Deterministic); + } + else + { + BinnedBuild(subtreeRefinementLeaves, refinementNodes, refinementMetanodes, default, pool, workerIndex: workerIndex); + } + context.Tree.ReifySubtreeRefinement(subtreeRefinementNodeIndices, refinementNodes); refinementNodes.Dispose(pool); refinementMetanodes.Dispose(pool); From 248ece058ff3e6d844465babfcb8669442ec69f6 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 20 Jul 2023 16:46:17 -0500 Subject: [PATCH 756/947] The race condition didn't actually exist, of course, oops. But refinement doesn't need the builder to output metanodes anyway; helps avoid an allocation. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 18 +++++++++++------- BepuPhysics/Trees/Tree_Refine2.cs | 20 ++++++-------------- 2 files changed, 17 insertions(+), 21 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 79e913e6a..491f89e5c 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -27,10 +27,14 @@ static void BuildNode( where TLeaves : unmanaged { Debug.Assert(typeof(TLeaves) == typeof(LeavesHandledInPostPass) || typeof(TLeaves) == typeof(Buffer), "While we didn't bother with an interface here, we assume one of two types only."); - ref var metanode = ref metanodes[0]; - metanode.Parent = parentNodeIndex; - metanode.IndexInParent = childIndexInParent; - metanode.RefineFlag = 0; + if (metanodes.Allocated) + { + //Note that we touching the metanodes buffer is *conditional*. There won't be any metanodes in the refinement use case, for example, because it has to be handled in a postpass. + ref var metanode = ref metanodes[0]; + metanode.Parent = parentNodeIndex; + metanode.IndexInParent = childIndexInParent; + metanode.RefineFlag = 0; + } ref var node = ref nodes[0]; node.A = Unsafe.As(ref a); node.B = Unsafe.As(ref b); @@ -304,7 +308,7 @@ static unsafe void MicroSweepForBinnedBuilder( centroidBoundsA.Min = Vector4.Min(centroidBoundsA.Min, centroid); centroidBoundsA.Max = Vector4.Max(centroidBoundsA.Max, centroid); } - MicroSweepForBinnedBuilder(centroidBoundsA.Min, centroidBoundsA.Max, ref leaves, subtrees.Slice(subtreeCountA), nodes.Slice(1, subtreeCountA - 1), metanodes.Slice(1, subtreeCountA - 1), aIndex, nodeIndex, 0, context, workerIndex); + MicroSweepForBinnedBuilder(centroidBoundsA.Min, centroidBoundsA.Max, ref leaves, subtrees.Slice(subtreeCountA), nodes.Slice(1, subtreeCountA - 1), metanodes.Allocated ? metanodes.Slice(1, subtreeCountA - 1) : metanodes, aIndex, nodeIndex, 0, context, workerIndex); } if (subtreeCountB > 1) { @@ -320,7 +324,7 @@ static unsafe void MicroSweepForBinnedBuilder( centroidBoundsB.Min = Vector4.Min(centroidBoundsB.Min, centroid); centroidBoundsB.Max = Vector4.Max(centroidBoundsB.Max, centroid); } - MicroSweepForBinnedBuilder(centroidBoundsB.Min, centroidBoundsB.Max, ref leaves, subtrees.Slice(subtreeCountA, subtreeCountB), nodes.Slice(subtreeCountA, subtreeCountB - 1), metanodes.Slice(subtreeCountA, subtreeCountB - 1), bIndex, nodeIndex, 1, context, workerIndex); + MicroSweepForBinnedBuilder(centroidBoundsB.Min, centroidBoundsB.Max, ref leaves, subtrees.Slice(subtreeCountA, subtreeCountB), nodes.Slice(subtreeCountA, subtreeCountB - 1), metanodes.Allocated ? metanodes.Slice(subtreeCountA, subtreeCountB - 1) : metanodes, bIndex, nodeIndex, 1, context, workerIndex); } } @@ -957,7 +961,7 @@ static unsafe void BinnedBuildNode( var boundingBoxes = subtrees.As(); var nodeCount = subtreeCount - 1; var nodes = context->Nodes.Slice(nodeIndex, nodeCount); - var metanodes = context->Metanodes.Slice(nodeIndex, nodeCount); + var metanodes = context->Metanodes.Allocated ? context->Metanodes.Slice(nodeIndex, nodeCount) : context->Metanodes; if (subtreeCount == 2) { Debug.Assert(parentNodeIndex < 0 || Unsafe.Add(ref context->Nodes[parentNodeIndex].A, childIndexInParent).LeafCount == subtrees[0].LeafCount + subtrees[1].LeafCount); diff --git a/BepuPhysics/Trees/Tree_Refine2.cs b/BepuPhysics/Trees/Tree_Refine2.cs index 0c1660d30..8be93d3d1 100644 --- a/BepuPhysics/Trees/Tree_Refine2.cs +++ b/BepuPhysics/Trees/Tree_Refine2.cs @@ -345,12 +345,10 @@ public unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementStar //Now that we have a list of nodes to refine, we can run the root refinement. Debug.Assert(rootRefinementNodeIndices.Count == rootRefinementSubtrees.Count - 1); var refinementNodesAllocation = new Buffer(int.Max(rootRefinementNodeIndices.Count, subtreeRefinementSize), pool); - var refinementMetanodesAllocation = new Buffer(refinementNodesAllocation.Length, pool); var rootRefinementNodes = refinementNodesAllocation.Slice(0, rootRefinementNodeIndices.Count); - var rootRefinementMetanodes = refinementMetanodesAllocation.Slice(0, rootRefinementNodeIndices.Count); //Passing 'default' for the leaves tells the binned builder to not worry about updating leaves. - BinnedBuild(rootRefinementSubtrees, rootRefinementNodes, rootRefinementMetanodes, default, pool); + BinnedBuild(rootRefinementSubtrees, rootRefinementNodes, default, default, pool); ReifyRootRefinementST(rootRefinementNodeIndices, rootRefinementNodes); rootRefinementSubtrees.Dispose(pool); rootRefinementNodeIndices.Dispose(pool); @@ -365,9 +363,8 @@ public unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementStar CollectSubtreesForSubtreeRefinement(subtreeRefinementTargets[i], subtreeStackBuffer, ref subtreeRefinementNodeIndices, ref subtreeRefinementLeaves); var refinementNodes = refinementNodesAllocation.Slice(0, subtreeRefinementNodeIndices.Count); - var refinementMetanodes = refinementMetanodesAllocation.Slice(0, subtreeRefinementNodeIndices.Count); //Passing 'default' for the leaves tells the binned builder to not worry about updating leaves. - BinnedBuild(subtreeRefinementLeaves, refinementNodes, refinementMetanodes, default, pool); + BinnedBuild(subtreeRefinementLeaves, refinementNodes, default, default, pool); ReifySubtreeRefinement(subtreeRefinementNodeIndices, refinementNodes); subtreeRefinementNodeIndices.Count = 0; @@ -379,7 +376,6 @@ public unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementStar subtreeRefinementTargets.Dispose(pool); subtreeStackBuffer.Dispose(pool); refinementNodesAllocation.Dispose(pool); - refinementMetanodesAllocation.Dispose(pool); } @@ -431,15 +427,14 @@ unsafe static void ExecuteRootRefinementTask(long id, void* untypedContext, int //Now that we have a list of nodes to refine, we can run the root refinement. Debug.Assert(rootRefinementNodeIndices.Count == rootRefinementSubtrees.Count - 1); var rootRefinementNodes = new Buffer(rootRefinementNodeIndices.Count, pool); - var rootRefinementMetanodes = new Buffer(rootRefinementNodeIndices.Count, pool); //Passing 'default' for the leaves tells the binned builder to not worry about updating leaves. if (taskCount > 1) { - BinnedBuild(rootRefinementSubtrees, rootRefinementNodes, rootRefinementMetanodes, default, pool, dispatcher, context.TaskStack, workerIndex, context.WorkerCount, taskCount, deterministic: context.Deterministic); + BinnedBuild(rootRefinementSubtrees, rootRefinementNodes, default, default, pool, dispatcher, context.TaskStack, workerIndex, context.WorkerCount, taskCount, deterministic: context.Deterministic); } else { - BinnedBuild(rootRefinementSubtrees, rootRefinementNodes, rootRefinementMetanodes, default, pool, workerIndex: workerIndex); + BinnedBuild(rootRefinementSubtrees, rootRefinementNodes, default, default, pool, workerIndex: workerIndex); } //var localNodeTopologyHash = 0; //for (int i = 0; i < rootRefinementNodes.Length; ++i) @@ -468,7 +463,6 @@ unsafe static void ExecuteRootRefinementTask(long id, void* untypedContext, int rootRefinementSubtrees.Dispose(pool); rootRefinementNodeIndices.Dispose(pool); rootRefinementNodes.Dispose(pool); - rootRefinementMetanodes.Dispose(pool); } unsafe static void ExecuteSubtreeRefinementTask(long subtreeRefinementTarget, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) { @@ -486,22 +480,20 @@ unsafe static void ExecuteSubtreeRefinementTask(long subtreeRefinementTarget, vo //Accumulate nodes and leaves with a prepass. context.Tree.CollectSubtreesForSubtreeRefinement((int)subtreeRefinementTarget, subtreeStackBuffer, ref subtreeRefinementNodeIndices, ref subtreeRefinementLeaves); var refinementNodes = new Buffer(subtreeRefinementNodeIndices.Count, pool); - var refinementMetanodes = new Buffer(subtreeRefinementNodeIndices.Count, pool); //Passing 'default' for the leaves tells the binned builder to not worry about updating leaves. if (taskCount > 1) { - BinnedBuild(subtreeRefinementLeaves, refinementNodes, refinementMetanodes, default, pool, dispatcher, context.TaskStack, + BinnedBuild(subtreeRefinementLeaves, refinementNodes, default, default, pool, dispatcher, context.TaskStack, workerIndex: workerIndex, workerCount: context.WorkerCount, targetTaskCount: taskCount, deterministic: context.Deterministic); } else { - BinnedBuild(subtreeRefinementLeaves, refinementNodes, refinementMetanodes, default, pool, workerIndex: workerIndex); + BinnedBuild(subtreeRefinementLeaves, refinementNodes, default, default, pool, workerIndex: workerIndex); } context.Tree.ReifySubtreeRefinement(subtreeRefinementNodeIndices, refinementNodes); refinementNodes.Dispose(pool); - refinementMetanodes.Dispose(pool); subtreeRefinementNodeIndices.Dispose(pool); subtreeRefinementLeaves.Dispose(pool); subtreeStackBuffer.Dispose(pool); From 9e5847c8b03e47e1dfa3899fdebcd1e64b5f1d45 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 20 Jul 2023 20:53:21 -0500 Subject: [PATCH 757/947] Subtree refinement reification now multithreaded too. --- BepuPhysics/Trees/Tree_Refine2.cs | 95 ++++++++++--------- .../SpecializedTests/TreeFiddlingTestDemo.cs | 7 +- 2 files changed, 56 insertions(+), 46 deletions(-) diff --git a/BepuPhysics/Trees/Tree_Refine2.cs b/BepuPhysics/Trees/Tree_Refine2.cs index 8be93d3d1..e027064d6 100644 --- a/BepuPhysics/Trees/Tree_Refine2.cs +++ b/BepuPhysics/Trees/Tree_Refine2.cs @@ -70,7 +70,7 @@ readonly void ReifyRootRefinementST(QuickList refinementNodeIndices, Buffer ReifyRootRefinement(0, refinementNodeIndices.Count, refinementNodeIndices, refinementNodes, this); } - unsafe struct ReifyRootRefinementContext + unsafe struct ReifyRefinementContext { public QuickList* RefinementNodeIndices; public Buffer* RefinementNodes; @@ -81,7 +81,7 @@ unsafe struct ReifyRootRefinementContext static unsafe void ReifyRootRefinementTask(long id, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) { - ref var context = ref *(ReifyRootRefinementContext*)untypedContext; + ref var context = ref *(ReifyRefinementContext*)untypedContext; ReifyRootRefinement(context.StartIndex, context.EndIndex, *context.RefinementNodeIndices, *context.RefinementNodes, *context.Tree); } @@ -91,7 +91,7 @@ readonly unsafe void ReifyRootRefinementMT(QuickList* refinementNodeIndices var remainder = refinementNodeIndices->Count - targetTaskCount * nodesPerTask; Debug.Assert(targetTaskCount < 1024, "We used a stackalloc for these task allocations under the assumption that there would be *very* few required, and that's clearly wrong here! What's going on?"); Span tasks = stackalloc Task[targetTaskCount]; - ReifyRootRefinementContext* contexts = stackalloc ReifyRootRefinementContext[targetTaskCount]; + ReifyRefinementContext* contexts = stackalloc ReifyRefinementContext[targetTaskCount]; var tree = this; var previousEnd = 0; @@ -133,23 +133,61 @@ readonly void ReifySubtreeRefinementNodeChild(ref int index, ref QuickList } } - void ReifySubtreeRefinement(QuickList refinementNodeIndices, Buffer refinementNodes) + static unsafe void ReifySubtreeRefinement(int startIndex, int endIndex, QuickList nodeIndices, Buffer refinementNodes, Tree tree) { - for (int i = 0; i < refinementNodeIndices.Count; ++i) + for (int i = startIndex; i < endIndex; ++i) { //refinementNodeIndices maps "refinement index space" to "real index space"; we can use it to update child pointers to the real locations. - var realNodeIndex = refinementNodeIndices[i]; + var realNodeIndex = nodeIndices[i]; ref var refinedNode = ref refinementNodes[i]; //Map child indices, and update leaf references. //Root refinements mark internal subtrees with a flag in the second to last index. - ReifySubtreeRefinementNodeChild(ref refinedNode.A.Index, ref refinementNodeIndices, realNodeIndex, 0); - ReifySubtreeRefinementNodeChild(ref refinedNode.B.Index, ref refinementNodeIndices, realNodeIndex, 1); - Nodes[realNodeIndex] = refinedNode; + tree.ReifySubtreeRefinementNodeChild(ref refinedNode.A.Index, ref nodeIndices, realNodeIndex, 0); + tree.ReifySubtreeRefinementNodeChild(ref refinedNode.B.Index, ref nodeIndices, realNodeIndex, 1); + tree.Nodes[realNodeIndex] = refinedNode; //Debug.Assert(Metanodes[realNodeIndex].Parent < 0 || Unsafe.Add(ref Nodes[Metanodes[realNodeIndex].Parent].A, Metanodes[realNodeIndex].IndexInParent).Index == realNodeIndex); } } + void ReifySubtreeRefinementST(QuickList refinementNodeIndices, Buffer refinementNodes) + { + ReifySubtreeRefinement(0, refinementNodeIndices.Count, refinementNodeIndices, refinementNodes, this); + } + + static unsafe void ReifySubtreeRefinementTask(long id, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) + { + ref var context = ref *(ReifyRefinementContext*)untypedContext; + ReifySubtreeRefinement(context.StartIndex, context.EndIndex, *context.RefinementNodeIndices, *context.RefinementNodes, *context.Tree); + } + + readonly unsafe void ReifySubtreeRefinementMT(QuickList* refinementNodeIndices, Buffer* refinementNodes, int targetTaskCount, int workerIndex, TaskStack* taskStack, IThreadDispatcher dispatcher) + { + var nodesPerTask = refinementNodeIndices->Count / targetTaskCount; + var remainder = refinementNodeIndices->Count - targetTaskCount * nodesPerTask; + Debug.Assert(targetTaskCount < 1024, "We used a stackalloc for these task allocations under the assumption that there would be *very* few required, and that's clearly wrong here! What's going on?"); + Span tasks = stackalloc Task[targetTaskCount]; + ReifyRefinementContext* contexts = stackalloc ReifyRefinementContext[targetTaskCount]; + var tree = this; + + var previousEnd = 0; + for (int i = 0; i < tasks.Length; ++i) + { + var count = i < remainder ? nodesPerTask + 1 : nodesPerTask; + ref var context = ref contexts[i]; + context.RefinementNodeIndices = refinementNodeIndices; + context.RefinementNodes = refinementNodes; + context.Tree = &tree; + context.StartIndex = previousEnd; + previousEnd += count; + context.EndIndex = previousEnd; + tasks[i] = new Task(&ReifySubtreeRefinementTask, contexts + i, i); + } + + taskStack->RunTasks(tasks, workerIndex, dispatcher); + } + + void FindSubtreeRefinementTargets(int nodeIndex, int leftLeafCount, int subtreeRefinementSize, int targetSubtreeRefinementCount, ref int startIndex, int endIndex, ref QuickList refinementTargets) { //If we've used up the target region already, just quit. @@ -365,7 +403,7 @@ public unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementStar var refinementNodes = refinementNodesAllocation.Slice(0, subtreeRefinementNodeIndices.Count); //Passing 'default' for the leaves tells the binned builder to not worry about updating leaves. BinnedBuild(subtreeRefinementLeaves, refinementNodes, default, default, pool); - ReifySubtreeRefinement(subtreeRefinementNodeIndices, refinementNodes); + ReifySubtreeRefinementST(subtreeRefinementNodeIndices, refinementNodes); subtreeRefinementNodeIndices.Count = 0; subtreeRefinementLeaves.Count = 0; @@ -394,9 +432,7 @@ unsafe struct RefinementContext /// public Tree Tree; } - //static int debugAccumulatorForSubtreeIndices = 0; - //static int debugAccumulatorForNodeTopology = 0; - //static int debugIndex = 0; + unsafe static void ExecuteRootRefinementTask(long id, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) { ref var context = ref *(RefinementContext*)untypedContext; @@ -417,13 +453,6 @@ unsafe static void ExecuteRootRefinementTask(long id, void* untypedContext, int //So, punting this for later. A nondeterministic implementation wouldn't be too bad. Could always just fall back to ST when deterministic flag is set. context.Tree.CollectSubtreesForRootRefinement(context.RootRefinementSize, context.SubtreeRefinementSize, pool, context.SubtreeRefinementTargets, ref rootRefinementNodeIndices, ref rootRefinementSubtrees); - //var localSubtreeHash = 0; - //for (int i = 0; i < rootRefinementSubtrees.Count; ++i) - //{ - // localSubtreeHash = HashHelper.Rehash(localSubtreeHash) + rootRefinementSubtrees[i].Index; - // //localSubtreeHash += rootRefinementSubtrees[i].Index; - //} - //debugAccumulatorForSubtreeIndices = HashHelper.Rehash(debugAccumulatorForSubtreeIndices) + localSubtreeHash; //Now that we have a list of nodes to refine, we can run the root refinement. Debug.Assert(rootRefinementNodeIndices.Count == rootRefinementSubtrees.Count - 1); var rootRefinementNodes = new Buffer(rootRefinementNodeIndices.Count, pool); @@ -431,32 +460,11 @@ unsafe static void ExecuteRootRefinementTask(long id, void* untypedContext, int if (taskCount > 1) { BinnedBuild(rootRefinementSubtrees, rootRefinementNodes, default, default, pool, dispatcher, context.TaskStack, workerIndex, context.WorkerCount, taskCount, deterministic: context.Deterministic); - } - else - { - BinnedBuild(rootRefinementSubtrees, rootRefinementNodes, default, default, pool, workerIndex: workerIndex); - } - //var localNodeTopologyHash = 0; - //for (int i = 0; i < rootRefinementNodes.Length; ++i) - //{ - // localNodeTopologyHash = HashHelper.Rehash(HashHelper.Rehash(localNodeTopologyHash) + rootRefinementNodes[i].A.Index) + rootRefinementNodes[i].B.Index; - // //localNodeTopologyHash += rootRefinementNodes[i].A.Index + rootRefinementNodes[i].B.Index; - //} - - - //debugAccumulatorForNodeTopology = HashHelper.Rehash(debugAccumulatorForNodeTopology) + localNodeTopologyHash; - ////for (int i = 0; i < context.SubtreeRefinementTargets.Count; ++i) - ////{ - //// debugAccumulator = HashHelper.Rehash(debugAccumulator) + context.SubtreeRefinementTargets[i]; - ////} - //Console.WriteLine($"Refinement targets count {debugIndex}: {context.SubtreeRefinementTargets.Count}, node topology hash: {debugAccumulatorForNodeTopology}, subtree indices hash: {debugAccumulatorForSubtreeIndices}"); - //++debugIndex; - if (taskCount > 1) - { context.Tree.ReifyRootRefinementMT(&rootRefinementNodeIndices, &rootRefinementNodes, taskCount, workerIndex, context.TaskStack, dispatcher); } else { + BinnedBuild(rootRefinementSubtrees, rootRefinementNodes, default, default, pool, workerIndex: workerIndex); context.Tree.ReifyRootRefinementST(rootRefinementNodeIndices, rootRefinementNodes); } @@ -486,12 +494,13 @@ unsafe static void ExecuteSubtreeRefinementTask(long subtreeRefinementTarget, vo { BinnedBuild(subtreeRefinementLeaves, refinementNodes, default, default, pool, dispatcher, context.TaskStack, workerIndex: workerIndex, workerCount: context.WorkerCount, targetTaskCount: taskCount, deterministic: context.Deterministic); + context.Tree.ReifySubtreeRefinementMT(&subtreeRefinementNodeIndices, &refinementNodes, taskCount, workerIndex, context.TaskStack, dispatcher); } else { BinnedBuild(subtreeRefinementLeaves, refinementNodes, default, default, pool, workerIndex: workerIndex); + context.Tree.ReifySubtreeRefinementST(subtreeRefinementNodeIndices, refinementNodes); } - context.Tree.ReifySubtreeRefinement(subtreeRefinementNodeIndices, refinementNodes); refinementNodes.Dispose(pool); subtreeRefinementNodeIndices.Dispose(pool); diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index f50dd7335..b8799d11f 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -187,12 +187,13 @@ public override void Initialize(ContentArchive content, Camera camera) var start = Stopwatch.GetTimestamp(); //mesh.Tree.Refine2(8192, ref refinementState, 0, 8192, BufferPool); - //mesh.Tree.Refine2(8192, ref refinementState, 32, 1024, BufferPool); - mesh.Tree.Refine2(8192, ref refinementState, 32, 4096, BufferPool, ThreadDispatcher); + //mesh.Tree.Refine2(1024, ref refinementState, 1, 131072, BufferPool); + mesh.Tree.Refine2(8192, ref refinementState, 16, 2048, BufferPool, ThreadDispatcher); var end = Stopwatch.GetTimestamp(); sum += end - start; if ((refinementIndex + 1) % 128 == 0) { + mesh.Tree.Validate(); var cacheQuality = mesh.Tree.MeasureCacheQuality(); var costMetric = mesh.Tree.MeasureCostMetric(); Console.WriteLine($"cost, cache for {refinementIndex}: {costMetric}, {cacheQuality}"); @@ -223,7 +224,7 @@ public override void Initialize(ContentArchive content, Camera camera) //BinnedTest(setup, () => //{ - // mesh.Tree.BinnedBuild(subtrees, ThreadDispatcher, pool: BufferPool); + // mesh.Tree.BinnedBuild(subtrees, BufferPool, ThreadDispatcher); //}, "Revamp Single Axis MT", ref mesh.Tree); //BufferPool.Take(mesh.Triangles.Length, out var leafBounds); From c42d289ded28c95e6a059b58a3a7622bacce00e0 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 21 Jul 2023 18:55:08 -0500 Subject: [PATCH 758/947] Refit refactor and attempt at basic bottom up. Predictably worse. --- BepuPhysics/Trees/Tree_Refit.cs | 29 ------------ BepuPhysics/Trees/Tree_Refit2.cs | 76 ++++++++++++++++++++++++++++++++ 2 files changed, 76 insertions(+), 29 deletions(-) create mode 100644 BepuPhysics/Trees/Tree_Refit2.cs diff --git a/BepuPhysics/Trees/Tree_Refit.cs b/BepuPhysics/Trees/Tree_Refit.cs index 8d74ae223..692bb28d2 100644 --- a/BepuPhysics/Trees/Tree_Refit.cs +++ b/BepuPhysics/Trees/Tree_Refit.cs @@ -58,34 +58,5 @@ public unsafe readonly void Refit() return; Refit(0, out var rootMin, out var rootMax); } - - readonly unsafe void Refit2(ref NodeChild childInParent) - { - Debug.Assert(LeafCount >= 2); - ref var node = ref Nodes[childInParent.Index]; - ref var a = ref node.A; - if (node.A.Index >= 0) - { - Refit2(ref a); - } - ref var b = ref node.B; - if (b.Index >= 0) - { - Refit2(ref b); - } - BoundingBox.CreateMergedUnsafeWithPreservation(a, b, out childInParent); - } - /// - /// Updates the bounding boxes of all internal nodes in the tree. - /// - public unsafe readonly void Refit2() - { - //No point in refitting a tree with no internal nodes! - if (LeafCount <= 2) - return; - NodeChild stub = default; - Refit2(ref stub); - } - } } diff --git a/BepuPhysics/Trees/Tree_Refit2.cs b/BepuPhysics/Trees/Tree_Refit2.cs new file mode 100644 index 000000000..968d26d58 --- /dev/null +++ b/BepuPhysics/Trees/Tree_Refit2.cs @@ -0,0 +1,76 @@ +using BepuUtilities; +using BepuUtilities.Collections; +using BepuUtilities.Memory; +using System.Diagnostics; +using System.Linq; +using System.Numerics; +using System.Runtime.CompilerServices; +using System.Runtime.Intrinsics; +using System.Runtime.Intrinsics.X86; +using System.Threading; + +namespace BepuPhysics.Trees; + +partial struct Tree +{ readonly unsafe void Refit2(ref NodeChild childInParent) + { + Debug.Assert(LeafCount >= 2); + ref var node = ref Nodes[childInParent.Index]; + ref var a = ref node.A; + if (node.A.Index >= 0) + { + Refit2(ref a); + } + ref var b = ref node.B; + if (b.Index >= 0) + { + Refit2(ref b); + } + BoundingBox.CreateMergedUnsafeWithPreservation(a, b, out childInParent); + } + /// + /// Updates the bounding boxes of all internal nodes in the tree. + /// + public unsafe readonly void Refit2() + { + //No point in refitting a tree with no internal nodes! + if (LeafCount <= 2) + return; + NodeChild stub = default; + Refit2(ref stub); + } + + public void RefitBottomUp(BufferPool pool) + { + var refitFlags = new Buffer((NodeCount + 63) / 64, pool); + refitFlags.Clear(0, refitFlags.Length); + //Refit doesn't calculate new bounding boxes for leaves; they are assumed to have already been supplied to the leaf-owning nodes. + for (int i = 0; i < LeafCount; ++i) + { + var leaf = Leaves[i]; + var nodeIndex = leaf.NodeIndex; + //There's nowhere to go from the root, so if the traversal gets there, it's done. + //(If the leaf is owned by the root, that's fine; the direct owner of a leaf is assumed to have been updated by an external process before the refit.) + while (nodeIndex > 0) + { + var bundleIndex = nodeIndex / 64; + var indexInBundle = nodeIndex - bundleIndex * 64; + var mask = 1ul << indexInBundle; + ref var bundle = ref refitFlags[bundleIndex]; + if ((bundle & mask) == 0) + { + //This node is incomplete. Mark it and terminate the local refit 'thread'. + bundle |= mask; + break; + } + //This node is complete; both children have been refit. Merge into the parent. + ref var metanode = ref Metanodes[nodeIndex]; + nodeIndex = metanode.Parent; + ref var node = ref Nodes[nodeIndex]; + ref var child = ref Unsafe.Add(ref node.A, metanode.IndexInParent); + BoundingBox.CreateMergedUnsafeWithPreservation(node.A, node.B, out child); + } + } + refitFlags.Dispose(pool); + } +} From 8ca4751ae97aa636a204b4307853ef0e3fb96189 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 23 Jul 2023 17:14:03 -0500 Subject: [PATCH 759/947] Shared thread dispatch for task stack. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 16 ++------------ BepuPhysics/Trees/Tree_Refine2.cs | 12 +--------- BepuUtilities/TaskScheduling/TaskStack.cs | 27 +++++++++++++++++++++++ 3 files changed, 30 insertions(+), 25 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 491f89e5c..f89c9da74 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -1388,7 +1388,7 @@ static unsafe void BinnedBuilderInternal(Buffer subtrees, BufferPushUnsafely(new Task(&BinnedBuilderWorkerEntry>, &context), 0, dispatcher); - dispatcher.DispatchWorkers(&BinnedBuilderWorkerFunction>, unmanagedContext: taskStackPointer, maximumWorkerCount: workerCount); + TaskStack.DispatchWorkers(dispatcher, taskStackPointer, workerCount); } else { @@ -1405,7 +1405,7 @@ static unsafe void BinnedBuilderInternal(Buffer subtrees, BufferPushUnsafely(new Task(&BinnedBuilderWorkerEntry, &context), 0, dispatcher); - dispatcher.DispatchWorkers(&BinnedBuilderWorkerFunction, unmanagedContext: taskStackPointer, maximumWorkerCount: workerCount); + TaskStack.DispatchWorkers(dispatcher, taskStackPointer, workerCount); } else { @@ -1429,18 +1429,6 @@ unsafe static void BinnedBuilderWorkerEntry(long taskId, void* untypedC context->Threading.TaskStack->RequestStop(); } - unsafe static void BinnedBuilderWorkerFunction(int workerIndex, IThreadDispatcher dispatcher) - where TLeaves : unmanaged - { - var taskStack = (TaskStack*)dispatcher.UnmanagedContext; - PopTaskResult popTaskResult; - var waiter = new SpinWait(); - while ((popTaskResult = taskStack->TryPopAndRun(workerIndex, dispatcher)) != PopTaskResult.Stop) - { - waiter.SpinOnce(-1); - } - } - /// /// Runs a multithreaded binned build across the subtrees buffer. diff --git a/BepuPhysics/Trees/Tree_Refine2.cs b/BepuPhysics/Trees/Tree_Refine2.cs index e027064d6..58799529f 100644 --- a/BepuPhysics/Trees/Tree_Refine2.cs +++ b/BepuPhysics/Trees/Tree_Refine2.cs @@ -508,16 +508,6 @@ unsafe static void ExecuteSubtreeRefinementTask(long subtreeRefinementTarget, vo subtreeStackBuffer.Dispose(pool); } - static unsafe void ExecuteWorker(int workerIndex, IThreadDispatcher dispatcher) - { - var taskStack = (TaskStack*)dispatcher.UnmanagedContext; - PopTaskResult popTaskResult; - var waiter = new SpinWait(); - while ((popTaskResult = taskStack->TryPopAndRun(workerIndex, dispatcher)) != PopTaskResult.Stop) - { - waiter.SpinOnce(-1); - } - } static unsafe void StopStackOnCompletion(long id, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) { @@ -579,7 +569,7 @@ private unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementSta { //There isn't an active dispatch, so we need to do it. taskStack->AllocateContinuationAndPush(tasks, workerIndex, threadDispatcher, onComplete: new Task(&StopStackOnCompletion, taskStack)); - threadDispatcher.DispatchWorkers(&ExecuteWorker, unmanagedContext: taskStack, maximumWorkerCount: workerCount); + TaskStack.DispatchWorkers(threadDispatcher, taskStack, workerCount); } else { diff --git a/BepuUtilities/TaskScheduling/TaskStack.cs b/BepuUtilities/TaskScheduling/TaskStack.cs index dea03dfd5..cc571ddfb 100644 --- a/BepuUtilities/TaskScheduling/TaskStack.cs +++ b/BepuUtilities/TaskScheduling/TaskStack.cs @@ -485,4 +485,31 @@ public void For(delegate* function, v AllowAllJobs filter = default; For(function, context, inclusiveStartIndex, iterationCount, workerIndex, dispatcher, ref filter, tag); } + + /// + /// Worker function that pops tasks from the stack and executes them. + /// + /// Index of the worker calling this function. + /// Thread dispatcher responsible for the invocation. + public static void DispatchWorkerFunction(int workerIndex, IThreadDispatcher dispatcher) + { + var taskStack = (TaskStack*)dispatcher.UnmanagedContext; + PopTaskResult popTaskResult; + var waiter = new SpinWait(); + while ((popTaskResult = taskStack->TryPopAndRun(workerIndex, dispatcher)) != PopTaskResult.Stop) + { + waiter.SpinOnce(-1); + } + } + + /// + /// Dispatches workers to execute tasks from the given stack. + /// + /// Task stack to pull work from. + /// Thread dispatcher to dispatch workers with. + /// Maximum number of workers to spin up for the dispatch. + public static void DispatchWorkers(IThreadDispatcher dispatcher, TaskStack* taskStack, int maximumWorkerCount = int.MaxValue) + { + dispatcher.DispatchWorkers(&DispatchWorkerFunction, maximumWorkerCount, taskStack); + } } From 882579a30bb9c55cff0fc1d8cf798773bb0d70c2 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 23 Jul 2023 17:14:45 -0500 Subject: [PATCH 760/947] Fixed unsafe merge with preservation. --- BepuUtilities/BoundingBox.cs | 21 ++++++++++++++++----- 1 file changed, 16 insertions(+), 5 deletions(-) diff --git a/BepuUtilities/BoundingBox.cs b/BepuUtilities/BoundingBox.cs index 6f45045c5..aa8bc5665 100644 --- a/BepuUtilities/BoundingBox.cs +++ b/BepuUtilities/BoundingBox.cs @@ -204,17 +204,28 @@ public static void CreateMerged(in BoundingBox a, in BoundingBox b, out Bounding [MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe static void CreateMergedUnsafeWithPreservation(in TA boundingBoxA, in TB boundingBoxB, out TA merged) where TA : unmanaged where TB : unmanaged { - if (Sse41.IsSupported) + if (Vector128.IsHardwareAccelerated) { Unsafe.SkipInit(out merged); ref var resultMin = ref Unsafe.As>(ref merged); ref var resultMax = ref Unsafe.Add(ref Unsafe.As>(ref merged), 1); - resultMin = Sse41.Blend(Vector128.Min( + var min = Vector128.Min( Unsafe.As>(ref Unsafe.AsRef(boundingBoxA)), - Unsafe.As>(ref Unsafe.AsRef(boundingBoxB))), resultMin, 0b111111); - resultMax = Sse41.Blend(Vector128.Max( + Unsafe.As>(ref Unsafe.AsRef(boundingBoxB))); + var max = Vector128.Max( Unsafe.Add(ref Unsafe.As>(ref Unsafe.AsRef(boundingBoxA)), 1), - Unsafe.Add(ref Unsafe.As>(ref Unsafe.AsRef(boundingBoxB)), 1)), resultMax, 0b111111); + Unsafe.Add(ref Unsafe.As>(ref Unsafe.AsRef(boundingBoxB)), 1)); + if (Sse41.IsSupported) + { + resultMin = Sse41.Blend(min, resultMin, 0b1000); + resultMax = Sse41.Blend(max, resultMax, 0b1000); + } + else + { + var mask = Vector128.Create(-1, -1, -1, 0).As(); + resultMin = Vector128.ConditionalSelect(mask, min, resultMin); + resultMax = Vector128.ConditionalSelect(mask, max, resultMax); + } } else { From 8d80446b4a6104662db7939f921eb666fc7e69f3 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 23 Jul 2023 17:19:19 -0500 Subject: [PATCH 761/947] Multithreaded in-place refit. --- BepuPhysics/Trees/Tree_Refit2.cs | 128 ++++++++++++++++++++++++------- 1 file changed, 99 insertions(+), 29 deletions(-) diff --git a/BepuPhysics/Trees/Tree_Refit2.cs b/BepuPhysics/Trees/Tree_Refit2.cs index 968d26d58..e843432cc 100644 --- a/BepuPhysics/Trees/Tree_Refit2.cs +++ b/BepuPhysics/Trees/Tree_Refit2.cs @@ -1,6 +1,8 @@ using BepuUtilities; using BepuUtilities.Collections; using BepuUtilities.Memory; +using BepuUtilities.TaskScheduling; +using System; using System.Diagnostics; using System.Linq; using System.Numerics; @@ -12,12 +14,13 @@ namespace BepuPhysics.Trees; partial struct Tree -{ readonly unsafe void Refit2(ref NodeChild childInParent) +{ + readonly unsafe void Refit2(ref NodeChild childInParent) { Debug.Assert(LeafCount >= 2); ref var node = ref Nodes[childInParent.Index]; ref var a = ref node.A; - if (node.A.Index >= 0) + if (a.Index >= 0) { Refit2(ref a); } @@ -40,37 +43,104 @@ public unsafe readonly void Refit2() Refit2(ref stub); } - public void RefitBottomUp(BufferPool pool) + unsafe struct RefitContext { - var refitFlags = new Buffer((NodeCount + 63) / 64, pool); - refitFlags.Clear(0, refitFlags.Length); - //Refit doesn't calculate new bounding boxes for leaves; they are assumed to have already been supplied to the leaf-owning nodes. - for (int i = 0; i < LeafCount; ++i) + public Tree* Tree; + public TaskStack* TaskStack; + public int LeafCountPerTask; + } + unsafe readonly void Refit2WithTaskSpawning(ref NodeChild childInParent, RefitContext* context, int workerIndex, IThreadDispatcher dispatcher) + { + Debug.Assert(LeafCount >= 2); + ref var node = ref Nodes[childInParent.Index]; + ref var a = ref node.A; + ref var b = ref node.B; + Debug.Assert(context->LeafCountPerTask > 1); + if (a.LeafCount >= context->LeafCountPerTask && b.LeafCount >= context->LeafCountPerTask) + { + //Both children are big enough to warrant a task. Spawn one task for B and recurse on A. + //(We always punt B because, if any cache optimizer-ish stuff is going on, A will be more likely to be contiguous in memory.) + var task = new Task(&Refit2Task, context, childInParent.Index); + var continuation = context->TaskStack->AllocateContinuationAndPush(new Span(ref task), workerIndex, dispatcher); + Debug.Assert(a.Index >= 0); + Refit2WithTaskSpawning(ref a, context, workerIndex, dispatcher); + //Wait until B is fully done before continuing. + context->TaskStack->WaitForCompletion(continuation, workerIndex, dispatcher); + } + else { - var leaf = Leaves[i]; - var nodeIndex = leaf.NodeIndex; - //There's nowhere to go from the root, so if the traversal gets there, it's done. - //(If the leaf is owned by the root, that's fine; the direct owner of a leaf is assumed to have been updated by an external process before the refit.) - while (nodeIndex > 0) + //At least one child is too small to warrant a new task. Just recurse on both. + if (a.Index >= 0) + { + Refit2(ref a); + } + if (b.Index >= 0) { - var bundleIndex = nodeIndex / 64; - var indexInBundle = nodeIndex - bundleIndex * 64; - var mask = 1ul << indexInBundle; - ref var bundle = ref refitFlags[bundleIndex]; - if ((bundle & mask) == 0) - { - //This node is incomplete. Mark it and terminate the local refit 'thread'. - bundle |= mask; - break; - } - //This node is complete; both children have been refit. Merge into the parent. - ref var metanode = ref Metanodes[nodeIndex]; - nodeIndex = metanode.Parent; - ref var node = ref Nodes[nodeIndex]; - ref var child = ref Unsafe.Add(ref node.A, metanode.IndexInParent); - BoundingBox.CreateMergedUnsafeWithPreservation(node.A, node.B, out child); + Refit2(ref b); } } - refitFlags.Dispose(pool); + BoundingBox.CreateMergedUnsafeWithPreservation(a, b, out childInParent); + } + + static unsafe void Refit2Task(long parentNodeIndex, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) + { + var context = (RefitContext*)untypedContext; + ref var node = ref context->Tree->Nodes[(int)parentNodeIndex]; + context->Tree->Refit2WithTaskSpawning(ref node.B, context, workerIndex, dispatcher); + } + static unsafe void RefitRootEntryTask(long id, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) + { + var context = (RefitContext*)untypedContext; + NodeChild stub = default; + context->Tree->Refit2WithTaskSpawning(ref stub, context, workerIndex, dispatcher); + context->TaskStack->RequestStop(); + } + + unsafe readonly void Refit2(BufferPool pool, IThreadDispatcher dispatcher, TaskStack* taskStack, int workerIndex, int targetTaskCount, bool internallyDispatch) + { + //No point in refitting a tree with no internal nodes! + if (LeafCount <= 2) + return; + var tree = this; + if (targetTaskCount < 0) + targetTaskCount = dispatcher.ThreadCount; + var leafCountPerTask = (int)float.Ceiling(LeafCount / (float)targetTaskCount); + var refitContext = new RefitContext { LeafCountPerTask = leafCountPerTask, TaskStack = taskStack, Tree = &tree }; + if (internallyDispatch) + { + taskStack->PushUnsafely(new Task(&RefitRootEntryTask, &refitContext), workerIndex, dispatcher); + TaskStack.DispatchWorkers(dispatcher, taskStack, int.Min(dispatcher.ThreadCount, targetTaskCount)); + } + else + { + NodeChild stub = default; + Refit2WithTaskSpawning(ref stub, &refitContext, workerIndex, dispatcher); + } } + + /// + /// Refits all bounding boxes in the tree using multiple threads. + /// + /// Pool used for main thread temporary allocations during execution. + /// Dispatcher used during execution. + public unsafe readonly void Refit2(BufferPool pool, IThreadDispatcher dispatcher) + { + var taskStack = new TaskStack(pool, dispatcher, dispatcher.ThreadCount); + Refit2(pool, dispatcher, &taskStack, 0, -1, internallyDispatch: true); + taskStack.Dispose(pool, dispatcher); + } + + /// + /// Refits all bounding boxes in the tree using multiple threads.Pushes tasks into the provided . Does not dispatch threads internally; this is intended to be used as a part of a caller-managed dispatch. + /// + /// Pool used for allocations during execution. + /// Dispatcher used during execution. + /// that the refit operation will push tasks onto as needed. + /// Index of the worker calling the function. + /// Number of tasks the refit should try to create during execution. + public unsafe readonly void Refit2(BufferPool pool, IThreadDispatcher dispatcher, TaskStack* taskStack, int workerIndex, int targetTaskCount = -1) + { + Refit2(pool, dispatcher, taskStack, workerIndex, targetTaskCount, internallyDispatch: false); + } + } From 69f4b45fa063fc43f4032277afbab5b2293b14ae Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 23 Jul 2023 21:05:07 -0500 Subject: [PATCH 762/947] Improved binned builder handling of degenerate cases. Shouldn't explode. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 105 +++++++++++++++++------- 1 file changed, 74 insertions(+), 31 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index f89c9da74..5eb5bb589 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -286,6 +286,12 @@ static unsafe void MicroSweepForBinnedBuilder( accumulatedBoundingBoxB.Max = Vector4.Max(bounds.Max, accumulatedBoundingBoxB.Max); accumulatedLeafCountB += subtrees[previousIndex].LeafCount; } + if (bestLeafCountB == 0 || bestLeafCountB == totalLeafCount || bestSAH == float.MaxValue || float.IsNaN(bestSAH) || float.IsInfinity(bestSAH)) + { + //Some form of major problem detected! Fall back to a degenerate split. + HandleMicrosweepDegeneracy(ref leaves, subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, context, workerIndex); + return; + } var bestBoundsA = binBoundingBoxesScan[bestSplit - 1]; var subtreeCountA = bestSplit; @@ -332,7 +338,8 @@ static unsafe void MicroSweepForBinnedBuilder( private static unsafe int ComputeBinIndex(Vector4 centroidMin, bool useX, bool useY, Vector128 permuteMask, int axisIndex, Vector4 offsetToBinIndex, Vector4 maximumBinIndex, in BoundingBox4 box) { var centroid = box.Min + box.Max; - var binIndicesForLeafContinuous = Vector4.Min(maximumBinIndex, (centroid - centroidMin) * offsetToBinIndex); + //Note the clamp against zero as well as maximumBinIndex; going negative *can* happen when the bounding box is corrupted. We'd rather not crash with an access violation. + var binIndicesForLeafContinuous = Vector4.Clamp((centroid - centroidMin) * offsetToBinIndex, Vector4.Zero, maximumBinIndex); //Note that we don't store out any of the indices into per-bin lists here. We only *really* want two final groups for the children, //and we can easily compute those by performing another scan. It requires recomputing the bin indices, but that's really not much of a concern. //To extract the desired lane, we need to use a variable shuffle mask. At the time of writing, the Vector128 cross platform shuffle did not like variable masks. @@ -950,6 +957,65 @@ unsafe static void BinnedBuilderNodeWorker(long taskId, voi BinnedBuildNode(usePongBuffer, subtreeRegionStartIndex, nodePushContext->NodeIndex, subtreeCount, nodePushContext->ParentNodeIndex, 1, nodePushContext->CentroidBounds, nodePushContext->Context, workerIndex, dispatcher); } + private static unsafe void BuildNodeForDegeneracy( + Buffer subtrees, Buffer nodes, Buffer metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, + Context* context, out int subtreeCountA, out int subtreeCountB, out BoundingBox4 boundsA, out BoundingBox4 boundsB, out int aIndex, out int bIndex) + where TLeaves : unmanaged + where TThreading : unmanaged, IBinnedBuilderThreading + { + //This shouldn't happen unless something is badly wrong with the input; no point in optimizing it. + subtreeCountA = subtrees.Length / 2; + subtreeCountB = subtrees.Length - subtreeCountA; + boundsA.Min = new Vector4(float.MaxValue); + boundsA.Max = new Vector4(float.MinValue); + boundsB.Min = new Vector4(float.MaxValue); + boundsB.Max = new Vector4(float.MinValue); + int leafCountA = 0, leafCountB = 0; + var boundingBoxes = subtrees.As(); + for (int i = 0; i < subtreeCountA; ++i) + { + ref var bounds = ref boundingBoxes[i]; + boundsA.Min = Vector4.Min(bounds.Min, boundsA.Min); + boundsA.Max = Vector4.Max(bounds.Max, boundsA.Max); + leafCountA += subtrees[i].LeafCount; + } + for (int i = subtreeCountA; i < subtrees.Length; ++i) + { + ref var bounds = ref boundingBoxes[i]; + boundsB.Min = Vector4.Min(bounds.Min, boundsB.Min); + boundsB.Max = Vector4.Max(bounds.Max, boundsB.Max); + leafCountB += subtrees[i].LeafCount; + } + Debug.Assert(parentNodeIndex < 0 || Unsafe.Add(ref context->Nodes[parentNodeIndex].A, childIndexInParent).LeafCount == leafCountA + leafCountB); + //Note that we just use the bounds as centroid bounds. This is a degenerate situation anyway. + BuildNode(boundsA, boundsB, leafCountA, leafCountB, subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, subtreeCountA, subtreeCountB, ref context->Leaves, out aIndex, out bIndex); + } + static unsafe void HandleDegeneracy(Buffer subtrees, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, + bool usePongBuffer, int subtreeRegionStartIndex, int nodeIndex, int subtreeCount, int parentNodeIndex, int childIndexInParent, + BoundingBox4 centroidBounds, Context* context, int workerIndex, IThreadDispatcher dispatcher) + where TLeaves : unmanaged where TThreading : unmanaged, IBinnedBuilderThreading + { + + BuildNodeForDegeneracy(subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, context, out var subtreeCountA, out var subtreeCountB, out var boundsA, out var boundsB, out var aIndex, out var bIndex); + if (subtreeCountA > 1) + BinnedBuildNode(usePongBuffer, subtreeRegionStartIndex, aIndex, subtreeCountA, nodeIndex, 0, boundsA, context, workerIndex, dispatcher); + if (subtreeCountB > 1) + BinnedBuildNode(usePongBuffer, subtreeRegionStartIndex + subtreeCountA, bIndex, subtreeCountB, nodeIndex, 1, boundsB, context, workerIndex, dispatcher); + } + + + static unsafe void HandleMicrosweepDegeneracy(ref TLeaves leaves, + Buffer subtrees, Buffer nodes, Buffer metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, Context* context, int workerIndex) + where TLeaves : unmanaged where TThreading : unmanaged, IBinnedBuilderThreading + { + BuildNodeForDegeneracy(subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, context, out var subtreeCountA, out var subtreeCountB, out var boundsA, out var boundsB, out var aIndex, out var bIndex); + if (subtreeCountA > 1) + MicroSweepForBinnedBuilder(boundsA.Min, boundsA.Max, ref leaves, subtrees.Slice(subtreeCountA), nodes.Slice(1, subtreeCountA - 1), metanodes.Allocated ? metanodes.Slice(1, subtreeCountA - 1) : metanodes, aIndex, nodeIndex, 0, context, workerIndex); + if (subtreeCountB > 1) + MicroSweepForBinnedBuilder(boundsB.Max, boundsB.Max, ref leaves, subtrees.Slice(subtreeCountA, subtreeCountB), nodes.Slice(subtreeCountA, subtreeCountB - 1), metanodes.Allocated ? metanodes.Slice(subtreeCountA, subtreeCountB - 1) : metanodes, bIndex, nodeIndex, 1, context, workerIndex); + } + + static unsafe void BinnedBuildNode( bool usePongBuffer, int subtreeRegionStartIndex, int nodeIndex, int subtreeCount, int parentNodeIndex, int childIndexInParent, BoundingBox4 centroidBounds, Context* context, int workerIndex, IThreadDispatcher dispatcher) @@ -996,35 +1062,7 @@ static unsafe void BinnedBuildNode( { //This node is completely degenerate; there is no 'good' ordering of the children. Pick a split in the middle and shrug. //This shouldn't happen unless something is badly wrong with the input; no point in optimizing it. - var degenerateSubtreeCountA = subtrees.Length / 2; - var degenerateSubtreeCountB = subtrees.Length - degenerateSubtreeCountA; - //Still have to compute the child bounding boxes, because the centroid bounds span being zero doesn't imply that the full bounds are zero. - BoundingBox4 boundsA, boundsB; - boundsA.Min = new Vector4(float.MaxValue); - boundsA.Max = new Vector4(float.MinValue); - boundsB.Min = new Vector4(float.MaxValue); - boundsB.Max = new Vector4(float.MinValue); - int degenerateLeafCountA = 0, degenerateLeafCountB = 0; - for (int i = 0; i < degenerateSubtreeCountA; ++i) - { - ref var bounds = ref boundingBoxes[i]; - boundsA.Min = Vector4.Min(bounds.Min, boundsA.Min); - boundsA.Max = Vector4.Max(bounds.Max, boundsA.Max); - degenerateLeafCountA += subtrees[i].LeafCount; - } - for (int i = degenerateSubtreeCountA; i < subtrees.Length; ++i) - { - ref var bounds = ref boundingBoxes[i]; - boundsB.Min = Vector4.Min(bounds.Min, boundsB.Min); - boundsB.Max = Vector4.Max(bounds.Max, boundsB.Max); - degenerateLeafCountB += subtrees[i].LeafCount; - } - Debug.Assert(parentNodeIndex < 0 || Unsafe.Add(ref context->Nodes[parentNodeIndex].A, childIndexInParent).LeafCount == degenerateLeafCountA + degenerateLeafCountB); - BuildNode(boundsA, boundsB, degenerateLeafCountA, degenerateLeafCountB, subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, degenerateSubtreeCountA, degenerateSubtreeCountB, ref context->Leaves, out var aIndex, out var bIndex); - if (degenerateSubtreeCountA > 1) - BinnedBuildNode(usePongBuffer, subtreeRegionStartIndex, aIndex, degenerateSubtreeCountA, nodeIndex, 0, boundsA, context, workerIndex, dispatcher); - if (degenerateSubtreeCountB > 1) - BinnedBuildNode(usePongBuffer, subtreeRegionStartIndex + degenerateSubtreeCountA, bIndex, degenerateSubtreeCountB, nodeIndex, 1, boundsB, context, workerIndex, dispatcher); + HandleDegeneracy(subtrees, boundingBoxes, nodes, metanodes, usePongBuffer, subtreeRegionStartIndex, nodeIndex, subtreeCount, parentNodeIndex, childIndexInParent, centroidBounds, context, workerIndex, dispatcher); return; } @@ -1141,8 +1179,13 @@ static unsafe void BinnedBuildNode( accumulatedCentroidBoundingBoxB.Max = Vector4.Max(centroidBoundsForBin.Max, accumulatedCentroidBoundingBoxB.Max); accumulatedLeafCountB += binLeafCounts[previousIndex]; } + if (bestLeafCountB == 0 || bestLeafCountB == totalLeafCount || bestSAH == float.MaxValue || float.IsNaN(bestSAH) || float.IsInfinity(bestSAH)) + { + //Some form of major problem detected! Fall back to a degenerate split. + HandleDegeneracy(subtrees, boundingBoxes, nodes, metanodes, usePongBuffer, subtreeRegionStartIndex, nodeIndex, subtreeCount, parentNodeIndex, childIndexInParent, centroidBounds, context, workerIndex, dispatcher); + return; + } - //var debugHuhStartTime = Stopwatch.GetTimestamp(); var subtreeCountB = 0; var subtreeCountA = 0; var bestBoundingBoxA = binBoundingBoxesScan[splitIndex - 1]; From adc6999a226279cebbce40ea568073d046a46f5b Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 24 Jul 2023 15:21:26 -0500 Subject: [PATCH 763/947] Refine2 now handles disabled roots/subtree refines. --- BepuPhysics/Trees/Tree_Refine2.cs | 66 ++++++++++++++++--------------- 1 file changed, 35 insertions(+), 31 deletions(-) diff --git a/BepuPhysics/Trees/Tree_Refine2.cs b/BepuPhysics/Trees/Tree_Refine2.cs index 58799529f..58b6060e2 100644 --- a/BepuPhysics/Trees/Tree_Refine2.cs +++ b/BepuPhysics/Trees/Tree_Refine2.cs @@ -237,13 +237,16 @@ void FindSubtreeRefinementTargets(int nodeIndex, int leftLeafCount, int subtreeR } void FindSubtreeRefinementTargets(int subtreeRefinementSize, int targetSubtreeRefinementCount, ref int startIndex, ref QuickList refinementTargets) { + //It's not impossible for the tree to have changed state such that the start index is invalid (or the user might have given invalid input). Just wrap back to 0 if that happens. + if (startIndex >= LeafCount || startIndex < 0) + startIndex = 0; var initialStart = startIndex; FindSubtreeRefinementTargets(0, 0, subtreeRefinementSize, targetSubtreeRefinementCount, ref startIndex, LeafCount, ref refinementTargets); if (startIndex >= LeafCount && refinementTargets.Count < targetSubtreeRefinementCount) { //Hit the end of the tree. Reset. startIndex = 0; - var remainingLeaves = LeafCount - initialStart; + var remainingLeaves = initialStart; //We walk through all leaves once, so if we started at X, we can go as far as X (after one wrap). FindSubtreeRefinementTargets(0, 0, subtreeRefinementSize, targetSubtreeRefinementCount, ref startIndex, remainingLeaves, ref refinementTargets); } } @@ -355,7 +358,7 @@ void CollectSubtreesForSubtreeRefinement(int refinementTargetRootNodeIndex, Buff /// /// Incrementally refines a subset of the tree by running a binned builder over subtrees. /// - /// Size of the refinement run on nodes near the root. + /// Size of the refinement run on nodes near the root. Nonpositive values will cause the root refinement to be skipped. /// Index used to distribute subtree refinements over multiple executions. /// Number of subtree refinements to execute. /// Target size of subtree refinements. The actual size of refinement will usually be larger or smaller. @@ -376,20 +379,23 @@ public unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementStar //Fill the trailing slots in the list with -1 to avoid matches. ((Span)subtreeRefinementTargets.Span)[subtreeRefinementTargets.Count..].Fill(-1); - var rootRefinementSubtrees = new QuickList(rootRefinementSize, pool); - var rootRefinementNodeIndices = new QuickList(rootRefinementSize, pool); - CollectSubtreesForRootRefinement(rootRefinementSize, subtreeRefinementSize, pool, subtreeRefinementTargets, ref rootRefinementNodeIndices, ref rootRefinementSubtrees); + var refinementNodesAllocation = new Buffer(int.Max(rootRefinementSize, subtreeRefinementSize), pool); + if (rootRefinementSize > 0) //Skip root refinement if it's zero or negative size. + { + var rootRefinementSubtrees = new QuickList(rootRefinementSize, pool); + var rootRefinementNodeIndices = new QuickList(rootRefinementSize, pool); + CollectSubtreesForRootRefinement(rootRefinementSize, subtreeRefinementSize, pool, subtreeRefinementTargets, ref rootRefinementNodeIndices, ref rootRefinementSubtrees); - //Now that we have a list of nodes to refine, we can run the root refinement. - Debug.Assert(rootRefinementNodeIndices.Count == rootRefinementSubtrees.Count - 1); - var refinementNodesAllocation = new Buffer(int.Max(rootRefinementNodeIndices.Count, subtreeRefinementSize), pool); + //Now that we have a list of nodes to refine, we can run the root refinement. + Debug.Assert(rootRefinementNodeIndices.Count == rootRefinementSubtrees.Count - 1); - var rootRefinementNodes = refinementNodesAllocation.Slice(0, rootRefinementNodeIndices.Count); - //Passing 'default' for the leaves tells the binned builder to not worry about updating leaves. - BinnedBuild(rootRefinementSubtrees, rootRefinementNodes, default, default, pool); - ReifyRootRefinementST(rootRefinementNodeIndices, rootRefinementNodes); - rootRefinementSubtrees.Dispose(pool); - rootRefinementNodeIndices.Dispose(pool); + var rootRefinementNodes = refinementNodesAllocation.Slice(0, rootRefinementNodeIndices.Count); + //Passing 'default' for the leaves tells the binned builder to not worry about updating leaves. + BinnedBuild(rootRefinementSubtrees, rootRefinementNodes, default, default, pool); + ReifyRootRefinementST(rootRefinementNodeIndices, rootRefinementNodes); + rootRefinementSubtrees.Dispose(pool); + rootRefinementNodeIndices.Dispose(pool); + } var subtreeRefinementNodeIndices = new QuickList(subtreeRefinementSize, pool); @@ -515,33 +521,32 @@ static unsafe void StopStackOnCompletion(long id, void* untypedContext, int work } - /// - /// Incrementally refines a subset of the tree by running a binned builder over subtrees. - /// - /// Size of the refinement run on nodes near the root. - /// Index used to distribute subtree refinements over multiple executions. - /// Number of subtree refinements to execute. - /// Target size of subtree refinements. The actual size of refinement will usually be larger or smaller. - /// Pool used for ephemeral allocations during the refinement. - /// Whether to force determinism at a slightly higher cost when using internally multithreaded execution for an individual refinement operation. - /// If the refine is single threaded, it is already deterministic and this flag has no effect. - /// Nodes will not be refit. private unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementStartIndex, int subtreeRefinementCount, int subtreeRefinementSize, BufferPool pool, int workerIndex, TaskStack* taskStack, IThreadDispatcher threadDispatcher, bool internallyDispatch, int workerCount, int targetTaskBudget, bool deterministic) { //No point refining anything with two leaves. This condition also avoids having to special case for an incomplete root node. if (LeafCount <= 2) return; + //Just early out of a fake refine attempt! + if (rootRefinementSize <= 0 && (subtreeRefinementCount <= 0 || subtreeRefinementSize <= 0)) + return; + //Setting root refinement size to 0 or negative values disables root refinement. People might intuitively try the same for subtree sizes. + if (subtreeRefinementSize <= 0) + subtreeRefinementCount = 0; //Clamp refinement sizes to avoid pointless overallocations when the user supplies odd inputs. rootRefinementSize = int.Min(rootRefinementSize, LeafCount); subtreeRefinementSize = int.Min(subtreeRefinementSize, LeafCount); //We used a vectorized containment test later, so make sure to pad out the refinement target list. var subtreeRefinementCapacity = BundleIndexing.GetBundleCount(subtreeRefinementCount) * Vector.Count; var subtreeRefinementTargets = new QuickList(subtreeRefinementCapacity, pool); + var preStartIndex = subtreeRefinementStartIndex; + subtreeRefinementStartIndex = preStartIndex; FindSubtreeRefinementTargets(subtreeRefinementSize, subtreeRefinementCount, ref subtreeRefinementStartIndex, ref subtreeRefinementTargets); //Fill the trailing slots in the list with -1 to avoid matches. ((Span)subtreeRefinementTargets.Span)[subtreeRefinementTargets.Count..].Fill(-1); - var tasks = new Buffer(1 + subtreeRefinementTargets.Count, pool); + //Zero or negative root refine sizes means skip it. + var rootRefinementCount = rootRefinementSize > 0 ? 1 : 0; + var tasks = new Buffer(rootRefinementCount + subtreeRefinementTargets.Count, pool); var totalLeafCountInSubtrees = 0; for (int i = 0; i < subtreeRefinementTargets.Count; ++i) { @@ -564,7 +569,8 @@ private unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementSta { tasks[i] = new Task(&ExecuteSubtreeRefinementTask, &context, subtreeRefinementTargets[i]); } - tasks[^1] = new Task(&ExecuteRootRefinementTask, &context); + if (rootRefinementSize > 0) + tasks[^1] = new Task(&ExecuteRootRefinementTask, &context); if (internallyDispatch) { //There isn't an active dispatch, so we need to do it. @@ -584,19 +590,17 @@ private unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementSta /// /// Incrementally refines a subset of the tree by running a binned builder over subtrees. /// - /// Size of the refinement run on nodes near the root. + /// Size of the refinement run on nodes near the root. Nonpositive values will cause the root refinement to be skipped. /// Index used to distribute subtree refinements over multiple executions. /// Number of subtree refinements to execute. /// Target size of subtree refinements. The actual size of refinement will usually be larger or smaller. /// Pool used for ephemeral allocations during the refinement. + /// Thread dispatcher used during the refinement. /// Whether to force determinism at a slightly higher cost when using internally multithreaded execution for an individual refinement operation. /// If the refine is single threaded, it is already deterministic and this flag has no effect. /// Nodes will not be refit. public unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementStartIndex, int subtreeRefinementCount, int subtreeRefinementSize, BufferPool pool, IThreadDispatcher threadDispatcher, bool deterministic = false) { - //No point refining anything with two leaves. This condition also avoids having to special case for an incomplete root node. - if (LeafCount <= 2) - return; var taskStack = new TaskStack(pool, threadDispatcher, threadDispatcher.ThreadCount); Refine2(rootRefinementSize, ref subtreeRefinementStartIndex, subtreeRefinementCount, subtreeRefinementSize, pool, 0, &taskStack, threadDispatcher, true, threadDispatcher.ThreadCount, threadDispatcher.ThreadCount, deterministic); taskStack.Dispose(pool, threadDispatcher); From 3f534ad7128bccef8440d6ed46962ba705e68124 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 24 Jul 2023 17:02:02 -0500 Subject: [PATCH 764/947] Cache optimizing refit. --- BepuPhysics/Trees/Tree_Refit2.cs | 112 +++++++++++++++++++++++++++++++ 1 file changed, 112 insertions(+) diff --git a/BepuPhysics/Trees/Tree_Refit2.cs b/BepuPhysics/Trees/Tree_Refit2.cs index e843432cc..0ee125859 100644 --- a/BepuPhysics/Trees/Tree_Refit2.cs +++ b/BepuPhysics/Trees/Tree_Refit2.cs @@ -143,4 +143,116 @@ public unsafe readonly void Refit2(BufferPool pool, IThreadDispatcher dispatcher Refit2(pool, dispatcher, taskStack, workerIndex, targetTaskCount, internallyDispatch: false); } + struct Refit2CacheContext + { + public Buffer TargetNodes; + public Buffer TargetMetanodes; + public Tree Tree; + } + + static unsafe void Refit2WithCacheOptimization(int sourceNodeIndex, int targetNodeIndex, int parentIndex, int childIndexInParent, ref NodeChild childInParent, ref Refit2CacheContext context) + { + Debug.Assert(context.Tree.LeafCount >= 2); + + ref var sourceNode = ref context.Tree.Nodes[sourceNodeIndex]; + ref var targetNode = ref context.TargetNodes[targetNodeIndex]; + ref var targetMetanode = ref context.TargetMetanodes[targetNodeIndex]; + targetMetanode.Parent = parentIndex; + targetMetanode.IndexInParent = childIndexInParent; + ref var sourceA = ref sourceNode.A; + ref var sourceB = ref sourceNode.B; + var targetIndexA = targetNodeIndex + 1; + var targetIndexB = targetNodeIndex + sourceA.LeafCount; + ref var targetA = ref targetNode.A; + ref var targetB = ref targetNode.B; + if (sourceA.Index >= 0) + { + targetA.Index = targetIndexA; + targetA.LeafCount = sourceA.LeafCount; + Refit2WithCacheOptimization(sourceA.Index, targetA.Index, targetNodeIndex, 0, ref targetA, ref context); + } + else + { + //It's a leaf; copy over the source verbatim. + targetA = sourceA; + context.Tree.Leaves[Encode(sourceA.Index)] = new Leaf(targetNodeIndex, 0); + } + if (sourceB.Index >= 0) + { + targetB.Index = targetIndexB; + targetB.LeafCount = sourceB.LeafCount; + Refit2WithCacheOptimization(sourceB.Index, targetB.Index, targetNodeIndex, 1, ref targetB, ref context); + } + else + { + targetB = sourceB; + context.Tree.Leaves[Encode(sourceB.Index)] = new Leaf(targetNodeIndex, 1); + } + BoundingBox.CreateMergedUnsafeWithPreservation(targetA, targetB, out childInParent); + } + + /// + /// Updates the bounding boxes of all internal nodes in the tree. Reallocates the nodes and metanodes and writes the refit tree into them. + /// The tree instance is modified to point to the new nodes and metanodes. + /// + /// Pool to allocate from. If disposeOriginals is true, this must be the same pool from which the and buffers were allocated from. + /// Whether to dispose of the original version. If false, it's up to the caller to dispose of them appropriately. + public unsafe void Refit2WithCacheOptimization(BufferPool pool, bool disposeOriginals = true) + { + //No point in refitting a tree with no internal nodes! + if (LeafCount <= 2) + return; + NodeChild stub = default; + var newNodes = new Buffer(Nodes.Length, pool); + var newMetanodes = new Buffer(Metanodes.Length, pool); + var context = new Refit2CacheContext + { + TargetNodes = newNodes, + TargetMetanodes = newMetanodes, + Tree = this, + }; + Refit2WithCacheOptimization(0, 0, -1, -1, ref stub, ref context); + + if (disposeOriginals) + { + Nodes.Dispose(pool); + Metanodes.Dispose(pool); + } + Nodes = newNodes; + Metanodes = newMetanodes; + } + + //unsafe readonly void Refit2WithCacheOptimization(BufferPool pool, IThreadDispatcher dispatcher, TaskStack* taskStack, int workerIndex, int targetTaskCount, bool internallyDispatch, bool disposeOriginals) + //{ + // //No point in refitting a tree with no internal nodes! + // if (LeafCount <= 2) + // return; + // var tree = this; + // if (targetTaskCount < 0) + // targetTaskCount = dispatcher.ThreadCount; + + // var newNodes = new Buffer(Nodes.Length, pool); + // var newMetanodes = new Buffer(Metanodes.Length, pool); + // var leafCountPerTask = (int)float.Ceiling(LeafCount / (float)targetTaskCount); + // var refitContext = new RefitContext { LeafCountPerTask = leafCountPerTask, TaskStack = taskStack, Tree = &tree }; + // if (internallyDispatch) + // { + // taskStack->PushUnsafely(new Task(&RefitRootEntryTask, &refitContext), workerIndex, dispatcher); + // TaskStack.DispatchWorkers(dispatcher, taskStack, int.Min(dispatcher.ThreadCount, targetTaskCount)); + // } + // else + // { + // NodeChild stub = default; + // Refit2WithTaskSpawning(ref stub, &refitContext, workerIndex, dispatcher); + // } + + // if (disposeOriginals) + // { + // tree.Nodes.Dispose(pool); + // tree.Metanodes.Dispose(pool); + // } + // tree.Nodes = newNodes; + // tree.Metanodes = newMetanodes; + + //} } From 8a53138a02819db988ec73bed2d205f74983907b Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 24 Jul 2023 21:33:30 -0500 Subject: [PATCH 765/947] Cache optimizing refit multithreaded variant. --- BepuPhysics/Trees/Tree_Refit2.cs | 208 ++++++++++++++++++++++++------- 1 file changed, 164 insertions(+), 44 deletions(-) diff --git a/BepuPhysics/Trees/Tree_Refit2.cs b/BepuPhysics/Trees/Tree_Refit2.cs index 0ee125859..e1da60a7c 100644 --- a/BepuPhysics/Trees/Tree_Refit2.cs +++ b/BepuPhysics/Trees/Tree_Refit2.cs @@ -45,7 +45,7 @@ public unsafe readonly void Refit2() unsafe struct RefitContext { - public Tree* Tree; + public Tree Tree; public TaskStack* TaskStack; public int LeafCountPerTask; } @@ -85,14 +85,14 @@ unsafe readonly void Refit2WithTaskSpawning(ref NodeChild childInParent, RefitCo static unsafe void Refit2Task(long parentNodeIndex, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) { var context = (RefitContext*)untypedContext; - ref var node = ref context->Tree->Nodes[(int)parentNodeIndex]; - context->Tree->Refit2WithTaskSpawning(ref node.B, context, workerIndex, dispatcher); + ref var node = ref context->Tree.Nodes[(int)parentNodeIndex]; + context->Tree.Refit2WithTaskSpawning(ref node.B, context, workerIndex, dispatcher); } static unsafe void RefitRootEntryTask(long id, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) { var context = (RefitContext*)untypedContext; NodeChild stub = default; - context->Tree->Refit2WithTaskSpawning(ref stub, context, workerIndex, dispatcher); + context->Tree.Refit2WithTaskSpawning(ref stub, context, workerIndex, dispatcher); context->TaskStack->RequestStop(); } @@ -101,11 +101,11 @@ unsafe readonly void Refit2(BufferPool pool, IThreadDispatcher dispatcher, TaskS //No point in refitting a tree with no internal nodes! if (LeafCount <= 2) return; - var tree = this; if (targetTaskCount < 0) targetTaskCount = dispatcher.ThreadCount; - var leafCountPerTask = (int)float.Ceiling(LeafCount / (float)targetTaskCount); - var refitContext = new RefitContext { LeafCountPerTask = leafCountPerTask, TaskStack = taskStack, Tree = &tree }; + const int minimumTaskSize = 32; + var leafCountPerTask = int.Max(minimumTaskSize, (int)float.Ceiling(LeafCount / (float)targetTaskCount)); + var refitContext = new RefitContext { LeafCountPerTask = leafCountPerTask, TaskStack = taskStack, Tree = this }; if (internallyDispatch) { taskStack->PushUnsafely(new Task(&RefitRootEntryTask, &refitContext), workerIndex, dispatcher); @@ -143,18 +143,22 @@ public unsafe readonly void Refit2(BufferPool pool, IThreadDispatcher dispatcher Refit2(pool, dispatcher, taskStack, workerIndex, targetTaskCount, internallyDispatch: false); } - struct Refit2CacheContext + unsafe struct RefitWithCacheOptimizationContext { public Buffer TargetNodes; public Buffer TargetMetanodes; public Tree Tree; + //These aren't used in the ST path, but, shrug! it's a few bytes. + public int LeafCountPerTask; + public TaskStack* TaskStack; } - static unsafe void Refit2WithCacheOptimization(int sourceNodeIndex, int targetNodeIndex, int parentIndex, int childIndexInParent, ref NodeChild childInParent, ref Refit2CacheContext context) + static unsafe void Refit2WithCacheOptimization(int sourceNodeIndex, int parentIndex, int childIndexInParent, ref NodeChild childInParent, ref RefitWithCacheOptimizationContext context) { Debug.Assert(context.Tree.LeafCount >= 2); ref var sourceNode = ref context.Tree.Nodes[sourceNodeIndex]; + var targetNodeIndex = childInParent.Index; ref var targetNode = ref context.TargetNodes[targetNodeIndex]; ref var targetMetanode = ref context.TargetMetanodes[targetNodeIndex]; targetMetanode.Parent = parentIndex; @@ -169,7 +173,7 @@ static unsafe void Refit2WithCacheOptimization(int sourceNodeIndex, int targetNo { targetA.Index = targetIndexA; targetA.LeafCount = sourceA.LeafCount; - Refit2WithCacheOptimization(sourceA.Index, targetA.Index, targetNodeIndex, 0, ref targetA, ref context); + Refit2WithCacheOptimization(sourceA.Index, targetNodeIndex, 0, ref targetA, ref context); } else { @@ -181,7 +185,7 @@ static unsafe void Refit2WithCacheOptimization(int sourceNodeIndex, int targetNo { targetB.Index = targetIndexB; targetB.LeafCount = sourceB.LeafCount; - Refit2WithCacheOptimization(sourceB.Index, targetB.Index, targetNodeIndex, 1, ref targetB, ref context); + Refit2WithCacheOptimization(sourceB.Index, targetNodeIndex, 1, ref targetB, ref context); } else { @@ -192,7 +196,7 @@ static unsafe void Refit2WithCacheOptimization(int sourceNodeIndex, int targetNo } /// - /// Updates the bounding boxes of all internal nodes in the tree. Reallocates the nodes and metanodes and writes the refit tree into them. + /// Updates the bounding boxes of all internal nodes in the tree. Reallocates the nodes and metanodes and writes the refit tree into them in depth first traversal order. /// The tree instance is modified to point to the new nodes and metanodes. /// /// Pool to allocate from. If disposeOriginals is true, this must be the same pool from which the and buffers were allocated from. @@ -205,13 +209,13 @@ public unsafe void Refit2WithCacheOptimization(BufferPool pool, bool disposeOrig NodeChild stub = default; var newNodes = new Buffer(Nodes.Length, pool); var newMetanodes = new Buffer(Metanodes.Length, pool); - var context = new Refit2CacheContext + var context = new RefitWithCacheOptimizationContext { TargetNodes = newNodes, TargetMetanodes = newMetanodes, Tree = this, }; - Refit2WithCacheOptimization(0, 0, -1, -1, ref stub, ref context); + Refit2WithCacheOptimization(0, -1, -1, ref stub, ref context); if (disposeOriginals) { @@ -221,38 +225,154 @@ public unsafe void Refit2WithCacheOptimization(BufferPool pool, bool disposeOrig Nodes = newNodes; Metanodes = newMetanodes; } + static unsafe void Refit2WithCacheOptimizationAndTaskSpawning( + int sourceNodeIndex, int parentIndex, int childIndexInParent, ref NodeChild childInParent, RefitWithCacheOptimizationContext* context, int workerIndex, IThreadDispatcher dispatcher) + { + Debug.Assert(context->Tree.LeafCount >= 2); + ref var sourceNode = ref context->Tree.Nodes[sourceNodeIndex]; + var targetNodeIndex = childInParent.Index; + ref var targetNode = ref context->TargetNodes[targetNodeIndex]; + ref var targetMetanode = ref context->TargetMetanodes[targetNodeIndex]; + targetMetanode.Parent = parentIndex; + targetMetanode.IndexInParent = childIndexInParent; + ref var sourceA = ref sourceNode.A; + ref var sourceB = ref sourceNode.B; + var targetIndexA = targetNodeIndex + 1; + var targetIndexB = targetNodeIndex + sourceA.LeafCount; + ref var targetA = ref targetNode.A; + ref var targetB = ref targetNode.B; + Debug.Assert(context->LeafCountPerTask > 1); + if (sourceA.LeafCount >= context->LeafCountPerTask && sourceB.LeafCount >= context->LeafCountPerTask) + { + //Both children are big enough to warrant a task. Spawn one task for B and recurse on A. + //(We always punt B because, if any cache optimizer-ish stuff is going on (like this process we're doing now!), A will be more likely to be contiguous in memory.) + //Note that we encode both the target AND source parent indices into the task id. + targetA.Index = targetIndexA; + targetA.LeafCount = sourceA.LeafCount; + targetB.Index = targetIndexB; + targetB.LeafCount = sourceB.LeafCount; + var task = new Task(&Refit2WithCacheOptimizationTask, context, (long)childInParent.Index | ((long)sourceNodeIndex << 32)); + var continuation = context->TaskStack->AllocateContinuationAndPush(new Span(ref task), workerIndex, dispatcher); + Debug.Assert(sourceA.Index >= 0); + Refit2WithCacheOptimizationAndTaskSpawning(sourceA.Index, targetNodeIndex, 0, ref targetA, context, workerIndex, dispatcher); + //Wait until B is fully done before continuing. + context->TaskStack->WaitForCompletion(continuation, workerIndex, dispatcher); + } + else + { + //At least one child is too small to warrant a new task. Just recurse on both. + if (sourceA.Index >= 0) + { + targetA.Index = targetIndexA; + targetA.LeafCount = sourceA.LeafCount; + Refit2WithCacheOptimization(sourceA.Index, targetNodeIndex, 0, ref targetA, ref *context); + } + else + { + //It's a leaf; copy over the source verbatim. + targetA = sourceA; + context->Tree.Leaves[Encode(sourceA.Index)] = new Leaf(targetNodeIndex, 0); + } + if (sourceB.Index >= 0) + { + targetB.Index = targetIndexB; + targetB.LeafCount = sourceB.LeafCount; + Refit2WithCacheOptimization(sourceB.Index, targetNodeIndex, 1, ref targetB, ref *context); + } + else + { + targetB = sourceB; + context->Tree.Leaves[Encode(sourceB.Index)] = new Leaf(targetNodeIndex, 1); + } + } + BoundingBox.CreateMergedUnsafeWithPreservation(targetA, targetB, out childInParent); + } + + static unsafe void Refit2WithCacheOptimizationTask(long parentNodeIndices, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) + { + var context = (RefitWithCacheOptimizationContext*)untypedContext; + var sourceParentIndex = (int)(parentNodeIndices >> 32); + var targetParentIndex = (int)parentNodeIndices; + ref var sourceParentNode = ref context->Tree.Nodes[sourceParentIndex]; + ref var targetParentNode = ref context->TargetNodes[targetParentIndex]; + Refit2WithCacheOptimizationAndTaskSpawning(sourceParentNode.B.Index, targetParentIndex, 1, ref targetParentNode.B, context, workerIndex, dispatcher); + } + static unsafe void RefitWithCacheOptimizationRootEntryTask(long id, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) + { + var context = (RefitWithCacheOptimizationContext*)untypedContext; + NodeChild stub = default; + Refit2WithCacheOptimizationAndTaskSpawning(0, -1, -1, ref stub, context, workerIndex, dispatcher); + context->TaskStack->RequestStop(); + } + unsafe void Refit2WithCacheOptimization(BufferPool pool, IThreadDispatcher dispatcher, TaskStack* taskStack, int workerIndex, int targetTaskCount, bool internallyDispatch, bool disposeOriginals) + { + //No point in refitting a tree with no internal nodes! + if (LeafCount <= 2) + return; + if (targetTaskCount < 0) + targetTaskCount = dispatcher.ThreadCount; + + var newNodes = new Buffer(Nodes.Length, pool); + var newMetanodes = new Buffer(Metanodes.Length, pool); + const int minimumTaskSize = 32; + var leafCountPerTask = int.Max(minimumTaskSize, (int)float.Ceiling(LeafCount / (float)targetTaskCount)); + var refitContext = new RefitWithCacheOptimizationContext + { + TargetNodes = newNodes, + TargetMetanodes = newMetanodes, + Tree = this, + LeafCountPerTask = leafCountPerTask, + TaskStack = taskStack, + }; + if (internallyDispatch) + { + taskStack->PushUnsafely(new Task(&RefitWithCacheOptimizationRootEntryTask, &refitContext), workerIndex, dispatcher); + TaskStack.DispatchWorkers(dispatcher, taskStack, int.Min(dispatcher.ThreadCount, targetTaskCount)); + } + else + { + NodeChild stub = default; + Refit2WithCacheOptimizationAndTaskSpawning(0, -1, -1, ref stub, &refitContext, workerIndex, dispatcher); + } + + if (disposeOriginals) + { + Nodes.Dispose(pool); + Metanodes.Dispose(pool); + } + Nodes = newNodes; + Metanodes = newMetanodes; - //unsafe readonly void Refit2WithCacheOptimization(BufferPool pool, IThreadDispatcher dispatcher, TaskStack* taskStack, int workerIndex, int targetTaskCount, bool internallyDispatch, bool disposeOriginals) - //{ - // //No point in refitting a tree with no internal nodes! - // if (LeafCount <= 2) - // return; - // var tree = this; - // if (targetTaskCount < 0) - // targetTaskCount = dispatcher.ThreadCount; + } - // var newNodes = new Buffer(Nodes.Length, pool); - // var newMetanodes = new Buffer(Metanodes.Length, pool); - // var leafCountPerTask = (int)float.Ceiling(LeafCount / (float)targetTaskCount); - // var refitContext = new RefitContext { LeafCountPerTask = leafCountPerTask, TaskStack = taskStack, Tree = &tree }; - // if (internallyDispatch) - // { - // taskStack->PushUnsafely(new Task(&RefitRootEntryTask, &refitContext), workerIndex, dispatcher); - // TaskStack.DispatchWorkers(dispatcher, taskStack, int.Min(dispatcher.ThreadCount, targetTaskCount)); - // } - // else - // { - // NodeChild stub = default; - // Refit2WithTaskSpawning(ref stub, &refitContext, workerIndex, dispatcher); - // } - // if (disposeOriginals) - // { - // tree.Nodes.Dispose(pool); - // tree.Metanodes.Dispose(pool); - // } - // tree.Nodes = newNodes; - // tree.Metanodes = newMetanodes; + /// + /// Refits all bounding boxes in the tree using multiple threads. Reallocates the nodes and metanodes and writes the refit tree into them in depth first traversal order. + /// The tree instance is modified to point to the new nodes and metanodes. + /// + /// Pool used for main thread temporary allocations during execution. + /// Dispatcher used during execution. + /// Whether to dispose of the original version. If false, it's up to the caller to dispose of them appropriately. + public unsafe void Refit2WithCacheOptimization(BufferPool pool, IThreadDispatcher dispatcher, bool disposeOriginals = true) + { + var taskStack = new TaskStack(pool, dispatcher, dispatcher.ThreadCount); + Refit2WithCacheOptimization(pool, dispatcher, &taskStack, 0, -1, internallyDispatch: true, disposeOriginals: disposeOriginals); + taskStack.Dispose(pool, dispatcher); + } - //} + /// + /// Refits all bounding boxes in the tree using multiple threads. Reallocates the nodes and metanodes and writes the refit tree into them in depth first traversal order. + /// The tree instance is modified to point to the new nodes and metanodes. + /// Pushes tasks into the provided . Does not dispatch threads internally; this is intended to be used as a part of a caller-managed dispatch. + /// + /// Pool used for allocations during execution. + /// Dispatcher used during execution. + /// that the refit operation will push tasks onto as needed. + /// Index of the worker calling the function. + /// Number of tasks the refit should try to create during execution. + /// Whether to dispose of the original version. If false, it's up to the caller to dispose of them appropriately. + public unsafe void Refit2WithCacheOptimization(BufferPool pool, IThreadDispatcher dispatcher, TaskStack* taskStack, int workerIndex, int targetTaskCount = -1, bool disposeOriginals = true) + { + Refit2WithCacheOptimization(pool, dispatcher, taskStack, workerIndex, targetTaskCount, internallyDispatch: false, disposeOriginals: disposeOriginals); + } } From b73e46affba1ace6756679241ae39015d088fd30 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 25 Jul 2023 17:10:18 -0500 Subject: [PATCH 766/947] RequestStop helper. --- BepuPhysics/Trees/Tree_Refine2.cs | 8 +------- BepuUtilities/TaskScheduling/TaskStack.cs | 18 ++++++++++++++++++ 2 files changed, 19 insertions(+), 7 deletions(-) diff --git a/BepuPhysics/Trees/Tree_Refine2.cs b/BepuPhysics/Trees/Tree_Refine2.cs index 58b6060e2..7acf9a754 100644 --- a/BepuPhysics/Trees/Tree_Refine2.cs +++ b/BepuPhysics/Trees/Tree_Refine2.cs @@ -515,12 +515,6 @@ unsafe static void ExecuteSubtreeRefinementTask(long subtreeRefinementTarget, vo } - static unsafe void StopStackOnCompletion(long id, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) - { - ((TaskStack*)untypedContext)->RequestStop(); - } - - private unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementStartIndex, int subtreeRefinementCount, int subtreeRefinementSize, BufferPool pool, int workerIndex, TaskStack* taskStack, IThreadDispatcher threadDispatcher, bool internallyDispatch, int workerCount, int targetTaskBudget, bool deterministic) { //No point refining anything with two leaves. This condition also avoids having to special case for an incomplete root node. @@ -574,7 +568,7 @@ private unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementSta if (internallyDispatch) { //There isn't an active dispatch, so we need to do it. - taskStack->AllocateContinuationAndPush(tasks, workerIndex, threadDispatcher, onComplete: new Task(&StopStackOnCompletion, taskStack)); + taskStack->AllocateContinuationAndPush(tasks, workerIndex, threadDispatcher, onComplete: TaskStack.GetRequestStopTask(taskStack)); TaskStack.DispatchWorkers(threadDispatcher, taskStack, workerCount); } else diff --git a/BepuUtilities/TaskScheduling/TaskStack.cs b/BepuUtilities/TaskScheduling/TaskStack.cs index cc571ddfb..45c317cef 100644 --- a/BepuUtilities/TaskScheduling/TaskStack.cs +++ b/BepuUtilities/TaskScheduling/TaskStack.cs @@ -400,6 +400,24 @@ public void RequestStop() padded.Stop = true; } + /// + /// Convenience function for requesting a stop. Requires the context to be the expected . + /// + /// Id of the task. + /// to be stopped. + /// Index of the worker executing this task. + /// Dispatcher associated with the execution. + public static unsafe void RequestStopTaskFunction(long id, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) + { + ((TaskStack*)untypedContext)->RequestStop(); + } + + /// + /// Convenience function for getting a task representing a stop request. + /// + /// Stack to be stopped. + /// Task representing a stop request. + public static Task GetRequestStopTask(TaskStack* stack) => new(&RequestStopTaskFunction, stack); /// /// Pushes a for loop onto the task stack. Does not take a lock. From a64ba005754b22db82d5e483be6d2701bbe0ebac Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 27 Jul 2023 16:02:00 -0500 Subject: [PATCH 767/947] Flipped Refit2WithCacheOptimization's allocation strategy inside out. First BroadPhase.Update2 implementation. --- BepuPhysics/CollisionDetection/BroadPhase.cs | 189 +++++++++++++++++++ BepuPhysics/Trees/Tree_Refine2.cs | 25 +++ BepuPhysics/Trees/Tree_Refit2.cs | 119 +++++++----- 3 files changed, 284 insertions(+), 49 deletions(-) diff --git a/BepuPhysics/CollisionDetection/BroadPhase.cs b/BepuPhysics/CollisionDetection/BroadPhase.cs index 06c535030..922810600 100644 --- a/BepuPhysics/CollisionDetection/BroadPhase.cs +++ b/BepuPhysics/CollisionDetection/BroadPhase.cs @@ -8,6 +8,7 @@ using BepuPhysics.Trees; using System.Threading; using BepuUtilities.Collections; +using BepuUtilities.TaskScheduling; namespace BepuPhysics.CollisionDetection { @@ -54,6 +55,9 @@ public BroadPhase(BufferPool pool, int initialActiveLeafCapacity = 4096, int ini staticRefineContext = new Tree.RefitAndRefineMultithreadedContext(); executeRefitAndMarkAction = ExecuteRefitAndMark; executeRefineAction = ExecuteRefine; + + ActiveRefinementSchedule = DefaultActiveRefinementScheduler; + StaticRefinementSchedule = DefaultStaticRefinementScheduler; } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -234,6 +238,191 @@ public void Update(IThreadDispatcher threadDispatcher = null) ++frameIndex; } + /// + /// Returns the size and number of refinements to execute during the broad phase. + /// + /// Index of the frame as tracked by the broad phase. + /// Tree being considered for refinement. + /// Size of the root refinement. If zero or negative, no root refinement will be performed. + /// Number of subtree refinements to perform. Can be zero. + /// Target size of the subtree refinements. + public delegate void RefinementScheduler(int frameIndex, in Tree tree, out int rootRefinementSize, out int subtreeRefinementCount, out int subtreeRefinementSize); + + /// + /// Gets or sets the refinement schedule to use for the active tree. + /// + public RefinementScheduler ActiveRefinementSchedule { get; set; } + /// + /// Gets or sets the refinement schedule to use for the static tree. + /// + public RefinementScheduler StaticRefinementSchedule { get; set; } + + /// + /// Returns the size and number of refinements to execute for the active tree. Used by default. + /// + /// Target fraction of the tree to be optimized. + /// Period, in timesteps, of refinements applied to the root. + /// Multiplier to apply to the square root of the leaf count to get the target root refinement size. + /// Multiplier to apply to the square root of the leaf count to get the target subtree refinement size. + /// Index of the frame as tracked by the broad phase. + /// Tree being considered for refinement. + /// Size of the root refinement. If zero or negative, no root refinement will be performed. + /// Number of subtree refinements to perform. Can be zero. + /// Target size of the subtree refinements. + public static void DefaultRefinementScheduler(float optimizationFraction, int rootRefinementPeriod, float rootRefinementSizeScale, float subtreeRefinementSizeScale, + int frameIndex, in Tree tree, out int rootRefinementSize, out int subtreeRefinementCount, out int subtreeRefinementSize) + { + var refineRoot = frameIndex % rootRefinementPeriod == 0; + var targetOptimizedLeafCount = (int)float.Ceiling(tree.LeafCount * optimizationFraction); + //The square root of the leaf count gets us roughly halfway down the tree. (Each subtree has ~sqrt(LeafCount) leaves, and there are ~sqrt(LeafCount) subtrees.) + //Root and subtree refinements need to be larger than that: subtrees must be able to exchange nodes, and the root refinement is the intermediary. + //Another consideration for choosing refinement sizes: larger refinement sizes increase in cost nonlinearly (O(nlogn)). + //You should choose a size which is large *enough* to get within your quality target and no larger. + var sqrtLeafCount = float.Sqrt(tree.LeafCount); + + var targetRootRefinementSize = (int)float.Ceiling(sqrtLeafCount * rootRefinementSizeScale); + subtreeRefinementSize = (int)float.Ceiling(sqrtLeafCount * subtreeRefinementSizeScale); + + var subtreeRefinementsPerRootRefinement = (int)float.Ceiling(subtreeRefinementSize * float.Log2(subtreeRefinementSize) / (targetRootRefinementSize * float.Log2(targetRootRefinementSize))); + //If we're refining the root, reduce the number of subtree refinements to avoid cost spikes. + subtreeRefinementCount = int.Max(0, (int)float.Ceiling((float)targetOptimizedLeafCount / subtreeRefinementSize) - (refineRoot ? subtreeRefinementsPerRootRefinement : 0)); + + rootRefinementSize = refineRoot ? targetRootRefinementSize : 0; + } + + /// + /// Returns the size and number of refinements to execute for the active tree. Used by default. + /// + /// Index of the frame as tracked by the broad phase. + /// Tree being considered for refinement. + /// Size of the root refinement. If zero or negative, no root refinement will be performed. + /// Number of subtree refinements to perform. Can be zero. + /// Target size of the subtree refinements. + public static void DefaultActiveRefinementScheduler(int frameIndex, in Tree tree, out int rootRefinementSize, out int subtreeRefinementCount, out int subtreeRefinementSize) + { + DefaultRefinementScheduler(1f / 50f, 8, 4, 4, frameIndex, tree, out rootRefinementSize, out subtreeRefinementCount, out subtreeRefinementSize); + } + + + /// + /// Returns the size and number of refinements to execute for the active tree. Used by default. + /// + /// Index of the frame as tracked by the broad phase. + /// Tree being considered for refinement. + /// Size of the root refinement. If zero or negative, no root refinement will be performed. + /// Number of subtree refinements to perform. Can be zero. + /// Target size of the subtree refinements. + public static void DefaultStaticRefinementScheduler(int frameIndex, in Tree tree, out int rootRefinementSize, out int subtreeRefinementCount, out int subtreeRefinementSize) + { + DefaultRefinementScheduler(1f / 200f, 20, 4, 4, frameIndex, tree, out rootRefinementSize, out subtreeRefinementCount, out subtreeRefinementSize); + } + + struct RefinementContext + { + public TaskStack* TaskStack; + public Tree Tree; + public int TargetTaskCount; + public int TargetTotalTaskCount; + public int RootRefinementSize; + public int SubtreeRefinementCount; + public int SubtreeRefinementSize; + public int SubtreeRefinementStartIndex; + public bool Deterministic; + + //Used for active tree. Didn't split the type because whocares. + public Buffer TargetNodes; + } + + static void ActiveEntrypointTask(long taskId, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) + { + ref var context = ref *(RefinementContext*)untypedContext; + var pool = dispatcher.WorkerPools[workerIndex]; + context.Tree.Refine2(context.RootRefinementSize, ref context.SubtreeRefinementStartIndex, context.SubtreeRefinementCount, context.SubtreeRefinementSize, pool, dispatcher, context.TaskStack, workerIndex, deterministic: context.Deterministic); + //Now refit! Note that we use all but one task. It doesn't affect the performance of a refit much (we're not compute bound), and we can use it to do an incremental cache optimization on the static tree. + var sourceNodes = context.Tree.Nodes; + context.Tree.Nodes = context.TargetNodes; + context.Tree.Refit2WithCacheOptimization(sourceNodes, pool, dispatcher, context.TaskStack, workerIndex, context.TargetTotalTaskCount - 1); + } + + static void StaticEntrypointTask(long taskId, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) + { + ref var context = ref *(RefinementContext*)untypedContext; + var pool = dispatcher.WorkerPools[workerIndex]; + context.Tree.Refine2(context.RootRefinementSize, ref context.SubtreeRefinementStartIndex, context.SubtreeRefinementCount, context.SubtreeRefinementSize, pool, dispatcher, context.TaskStack, workerIndex, deterministic: context.Deterministic); + //TODO: Incremental cache optimization. If you end up not implementing it, make sure to give the task back to active refit. + } + + int staticSubtreeRefinementStartIndex, activeSubtreeRefinementStartIndex; + public void Update2(IThreadDispatcher threadDispatcher = null, bool deterministic = false) + { + ActiveRefinementSchedule(frameIndex, ActiveTree, out var activeRootRefinementSize, out var activeSubtreeRefinementCount, out var activeSubtreeRefinementSize); + StaticRefinementSchedule(frameIndex, ActiveTree, out var staticRootRefinementSize, out var staticSubtreeRefinementCount, out var staticSubtreeRefinementSize); + if (threadDispatcher != null && threadDispatcher.ThreadCount > 1) + { + //Distribute tasks for refinement roughly in proportion to their cost. + //This doesn't need to be perfect. + //Cost of a refinement is roughly n * log2(n), for n = refinement size. + var activeCost = float.Log2(activeRootRefinementSize) * activeRootRefinementSize + float.Log2(activeSubtreeRefinementSize) * activeSubtreeRefinementSize * activeSubtreeRefinementCount; + var staticCost = float.Log2(staticRootRefinementSize) * staticRootRefinementSize + float.Log2(staticSubtreeRefinementSize) * staticSubtreeRefinementSize * staticSubtreeRefinementCount; + var activeTaskFraction = activeCost / (activeCost + staticCost); + var targetTotalTaskCount = threadDispatcher.ThreadCount; //could scale this. Empirically, doesn't matter on the CPUs tested so far. + var targetActiveTaskCount = (int)float.Ceiling(activeTaskFraction * targetTotalTaskCount); + var taskStack = new TaskStack(Pool, threadDispatcher, threadDispatcher.ThreadCount); + var activeRefineContext = new RefinementContext + { + TaskStack = &taskStack, + Tree = ActiveTree, + TargetTotalTaskCount = targetTotalTaskCount, + TargetTaskCount = targetActiveTaskCount, + RootRefinementSize = activeRootRefinementSize, + SubtreeRefinementCount = activeSubtreeRefinementCount, + SubtreeRefinementSize = activeSubtreeRefinementSize, + SubtreeRefinementStartIndex = activeSubtreeRefinementStartIndex, + Deterministic = deterministic, + TargetNodes = new Buffer(ActiveTree.Nodes.Length, Pool), + }; + var staticRefineContext = new RefinementContext + { + TaskStack = &taskStack, + Tree = StaticTree, + TargetTotalTaskCount = targetTotalTaskCount, + TargetTaskCount = targetTotalTaskCount - targetActiveTaskCount, + RootRefinementSize = staticRootRefinementSize, + SubtreeRefinementCount = staticSubtreeRefinementCount, + SubtreeRefinementSize = staticSubtreeRefinementSize, + SubtreeRefinementStartIndex = staticSubtreeRefinementStartIndex, + Deterministic = deterministic, + }; + Span tasks = stackalloc Task[2]; + tasks[0] = new Task(&ActiveEntrypointTask, &activeRefineContext); + tasks[1] = new Task(&StaticEntrypointTask, &staticRefineContext); + taskStack.AllocateContinuationAndPush(tasks, 0, threadDispatcher, onComplete: TaskStack.GetRequestStopTask(&taskStack)); + TaskStack.DispatchWorkers(threadDispatcher, &taskStack); + taskStack.Dispose(Pool, threadDispatcher); + //When using the cache optimizing refit, the tree is modified. Since passed a copy, we need to copy it back. + //Static tree doesn't undergo a refit, so no copy required there. + ActiveTree.Nodes.Dispose(Pool); + ActiveTree.Nodes = activeRefineContext.TargetNodes; + //The start indices need to be copied back for both. + activeSubtreeRefinementStartIndex = activeRefineContext.SubtreeRefinementStartIndex; + staticSubtreeRefinementStartIndex = staticRefineContext.SubtreeRefinementStartIndex; + + } + else + { + StaticTree.Refine2(staticRootRefinementSize, ref staticSubtreeRefinementStartIndex, staticSubtreeRefinementCount, staticSubtreeRefinementSize, Pool); + //Note we refine *before* refitting. This means the refinement is working with slightly out of date data, but that's okay, the entire point is incremental refinement. + //The reason to prefer this is that refining scrambles the memory layout a little bit. + //Refit with cache optimization *after* refinement ensures the rest of the library (and the user) sees the cache optimized version. + ActiveTree.Refine2(activeRootRefinementSize, ref activeSubtreeRefinementStartIndex, activeSubtreeRefinementCount, activeSubtreeRefinementSize, Pool); + ActiveTree.Refit2WithCacheOptimization(Pool); + } + if (frameIndex == int.MaxValue) + frameIndex = 0; + else + ++frameIndex; + } + /// /// Clears out the broad phase's structures without releasing any resources. /// diff --git a/BepuPhysics/Trees/Tree_Refine2.cs b/BepuPhysics/Trees/Tree_Refine2.cs index 7acf9a754..5a7725e82 100644 --- a/BepuPhysics/Trees/Tree_Refine2.cs +++ b/BepuPhysics/Trees/Tree_Refine2.cs @@ -526,6 +526,8 @@ private unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementSta //Setting root refinement size to 0 or negative values disables root refinement. People might intuitively try the same for subtree sizes. if (subtreeRefinementSize <= 0) subtreeRefinementCount = 0; + if (targetTaskBudget < 0) + targetTaskBudget = threadDispatcher.ThreadCount; //Clamp refinement sizes to avoid pointless overallocations when the user supplies odd inputs. rootRefinementSize = int.Min(rootRefinementSize, LeafCount); subtreeRefinementSize = int.Min(subtreeRefinementSize, LeafCount); @@ -599,4 +601,27 @@ public unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementStar Refine2(rootRefinementSize, ref subtreeRefinementStartIndex, subtreeRefinementCount, subtreeRefinementSize, pool, 0, &taskStack, threadDispatcher, true, threadDispatcher.ThreadCount, threadDispatcher.ThreadCount, deterministic); taskStack.Dispose(pool, threadDispatcher); } + + + /// + /// Incrementally refines a subset of the tree by running a binned builder over subtrees. + /// Pushes tasks into the provided . Does not dispatch threads internally; this is intended to be used as a part of a caller-managed dispatch. + /// + /// Size of the refinement run on nodes near the root. Nonpositive values will cause the root refinement to be skipped. + /// Index used to distribute subtree refinements over multiple executions. + /// Number of subtree refinements to execute. + /// Target size of subtree refinements. The actual size of refinement will usually be larger or smaller. + /// Pool used for ephemeral allocations during the refinement. + /// Thread dispatcher used during the refinement. + /// Whether to force determinism at a slightly higher cost when using internally multithreaded execution for an individual refinement operation. + /// If the refine is single threaded, it is already deterministic and this flag has no effect. + /// that the refine operation will push tasks onto as needed. + /// Index of the worker calling the function. + /// Number of tasks the refit should try to create during execution. If negative, uses . + /// Nodes will not be refit. + public unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementStartIndex, int subtreeRefinementCount, int subtreeRefinementSize, + BufferPool pool, IThreadDispatcher threadDispatcher, TaskStack* taskStack, int workerIndex, int targetTaskCount = -1, bool deterministic = false) + { + Refine2(rootRefinementSize, ref subtreeRefinementStartIndex, subtreeRefinementCount, subtreeRefinementSize, pool, workerIndex, taskStack, threadDispatcher, false, threadDispatcher.ThreadCount, targetTaskCount, deterministic); + } } diff --git a/BepuPhysics/Trees/Tree_Refit2.cs b/BepuPhysics/Trees/Tree_Refit2.cs index e1da60a7c..68f7c26fc 100644 --- a/BepuPhysics/Trees/Tree_Refit2.cs +++ b/BepuPhysics/Trees/Tree_Refit2.cs @@ -145,8 +145,7 @@ public unsafe readonly void Refit2(BufferPool pool, IThreadDispatcher dispatcher unsafe struct RefitWithCacheOptimizationContext { - public Buffer TargetNodes; - public Buffer TargetMetanodes; + public Buffer SourceNodes; public Tree Tree; //These aren't used in the ST path, but, shrug! it's a few bytes. public int LeafCountPerTask; @@ -157,10 +156,10 @@ static unsafe void Refit2WithCacheOptimization(int sourceNodeIndex, int parentIn { Debug.Assert(context.Tree.LeafCount >= 2); - ref var sourceNode = ref context.Tree.Nodes[sourceNodeIndex]; + ref var sourceNode = ref context.SourceNodes[sourceNodeIndex]; var targetNodeIndex = childInParent.Index; - ref var targetNode = ref context.TargetNodes[targetNodeIndex]; - ref var targetMetanode = ref context.TargetMetanodes[targetNodeIndex]; + ref var targetNode = ref context.Tree.Nodes[targetNodeIndex]; + ref var targetMetanode = ref context.Tree.Metanodes[targetNodeIndex]; targetMetanode.Parent = parentIndex; targetMetanode.IndexInParent = childIndexInParent; ref var sourceA = ref sourceNode.A; @@ -195,44 +194,52 @@ static unsafe void Refit2WithCacheOptimization(int sourceNodeIndex, int parentIn BoundingBox.CreateMergedUnsafeWithPreservation(targetA, targetB, out childInParent); } + /// - /// Updates the bounding boxes of all internal nodes in the tree. Reallocates the nodes and metanodes and writes the refit tree into them in depth first traversal order. - /// The tree instance is modified to point to the new nodes and metanodes. + /// Updates the bounding boxes of all internal nodes in the tree. The refit is based on the provided sourceNodes, and + /// the results are written into the tree's current , , and buffers. + /// The nodes and metanodes will be in depth traversal order. + /// The input source buffer is not modified. /// - /// Pool to allocate from. If disposeOriginals is true, this must be the same pool from which the and buffers were allocated from. - /// Whether to dispose of the original version. If false, it's up to the caller to dispose of them appropriately. - public unsafe void Refit2WithCacheOptimization(BufferPool pool, bool disposeOriginals = true) + /// Nodes to base the refit on. + public unsafe void Refit2WithCacheOptimization(Buffer sourceNodes) { //No point in refitting a tree with no internal nodes! if (LeafCount <= 2) return; NodeChild stub = default; - var newNodes = new Buffer(Nodes.Length, pool); - var newMetanodes = new Buffer(Metanodes.Length, pool); var context = new RefitWithCacheOptimizationContext { - TargetNodes = newNodes, - TargetMetanodes = newMetanodes, + SourceNodes = sourceNodes, Tree = this, }; Refit2WithCacheOptimization(0, -1, -1, ref stub, ref context); - if (disposeOriginals) - { - Nodes.Dispose(pool); - Metanodes.Dispose(pool); - } - Nodes = newNodes; - Metanodes = newMetanodes; } + + /// + /// Updates the bounding boxes of all internal nodes in the tree. Reallocates the nodes and metanodes and writes the refit tree into them in depth first traversal order. + /// The tree instance is modified to point to the new nodes and metanodes. + /// + /// Pool to allocate from. If disposeOriginals is true, this must be the same pool from which the buffer was allocated from. + /// Whether to dispose of the original nodes buffer. If false, it's up to the caller to dispose of it appropriately. + public unsafe void Refit2WithCacheOptimization(BufferPool pool, bool disposeOriginalNodes = true) + { + var oldNodes = Nodes; + Nodes = new Buffer(oldNodes.Length, pool); + Refit2WithCacheOptimization(oldNodes); + if (disposeOriginalNodes) + oldNodes.Dispose(pool); + } + static unsafe void Refit2WithCacheOptimizationAndTaskSpawning( int sourceNodeIndex, int parentIndex, int childIndexInParent, ref NodeChild childInParent, RefitWithCacheOptimizationContext* context, int workerIndex, IThreadDispatcher dispatcher) { Debug.Assert(context->Tree.LeafCount >= 2); - ref var sourceNode = ref context->Tree.Nodes[sourceNodeIndex]; + ref var sourceNode = ref context->SourceNodes[sourceNodeIndex]; var targetNodeIndex = childInParent.Index; - ref var targetNode = ref context->TargetNodes[targetNodeIndex]; - ref var targetMetanode = ref context->TargetMetanodes[targetNodeIndex]; + ref var targetNode = ref context->Tree.Nodes[targetNodeIndex]; + ref var targetMetanode = ref context->Tree.Metanodes[targetNodeIndex]; targetMetanode.Parent = parentIndex; targetMetanode.IndexInParent = childIndexInParent; ref var sourceA = ref sourceNode.A; @@ -293,8 +300,8 @@ static unsafe void Refit2WithCacheOptimizationTask(long parentNodeIndices, void* var context = (RefitWithCacheOptimizationContext*)untypedContext; var sourceParentIndex = (int)(parentNodeIndices >> 32); var targetParentIndex = (int)parentNodeIndices; - ref var sourceParentNode = ref context->Tree.Nodes[sourceParentIndex]; - ref var targetParentNode = ref context->TargetNodes[targetParentIndex]; + ref var sourceParentNode = ref context->SourceNodes[sourceParentIndex]; + ref var targetParentNode = ref context->Tree.Nodes[targetParentIndex]; Refit2WithCacheOptimizationAndTaskSpawning(sourceParentNode.B.Index, targetParentIndex, 1, ref targetParentNode.B, context, workerIndex, dispatcher); } static unsafe void RefitWithCacheOptimizationRootEntryTask(long id, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) @@ -304,7 +311,7 @@ static unsafe void RefitWithCacheOptimizationRootEntryTask(long id, void* untype Refit2WithCacheOptimizationAndTaskSpawning(0, -1, -1, ref stub, context, workerIndex, dispatcher); context->TaskStack->RequestStop(); } - unsafe void Refit2WithCacheOptimization(BufferPool pool, IThreadDispatcher dispatcher, TaskStack* taskStack, int workerIndex, int targetTaskCount, bool internallyDispatch, bool disposeOriginals) + unsafe void Refit2WithCacheOptimization(BufferPool pool, IThreadDispatcher dispatcher, TaskStack* taskStack, int workerIndex, int targetTaskCount, bool internallyDispatch, Buffer sourceNodes) { //No point in refitting a tree with no internal nodes! if (LeafCount <= 2) @@ -312,14 +319,11 @@ unsafe void Refit2WithCacheOptimization(BufferPool pool, IThreadDispatcher dispa if (targetTaskCount < 0) targetTaskCount = dispatcher.ThreadCount; - var newNodes = new Buffer(Nodes.Length, pool); - var newMetanodes = new Buffer(Metanodes.Length, pool); const int minimumTaskSize = 32; var leafCountPerTask = int.Max(minimumTaskSize, (int)float.Ceiling(LeafCount / (float)targetTaskCount)); var refitContext = new RefitWithCacheOptimizationContext { - TargetNodes = newNodes, - TargetMetanodes = newMetanodes, + SourceNodes = sourceNodes, Tree = this, LeafCountPerTask = leafCountPerTask, TaskStack = taskStack, @@ -334,15 +338,6 @@ unsafe void Refit2WithCacheOptimization(BufferPool pool, IThreadDispatcher dispa NodeChild stub = default; Refit2WithCacheOptimizationAndTaskSpawning(0, -1, -1, ref stub, &refitContext, workerIndex, dispatcher); } - - if (disposeOriginals) - { - Nodes.Dispose(pool); - Metanodes.Dispose(pool); - } - Nodes = newNodes; - Metanodes = newMetanodes; - } @@ -352,27 +347,53 @@ unsafe void Refit2WithCacheOptimization(BufferPool pool, IThreadDispatcher dispa /// /// Pool used for main thread temporary allocations during execution. /// Dispatcher used during execution. - /// Whether to dispose of the original version. If false, it's up to the caller to dispose of them appropriately. - public unsafe void Refit2WithCacheOptimization(BufferPool pool, IThreadDispatcher dispatcher, bool disposeOriginals = true) + /// Whether to dispose of the original nodes buffer. If false, it's up to the caller to dispose of it appropriately. + public unsafe void Refit2WithCacheOptimization(BufferPool pool, IThreadDispatcher dispatcher, bool disposeOriginalNodes = true) { var taskStack = new TaskStack(pool, dispatcher, dispatcher.ThreadCount); - Refit2WithCacheOptimization(pool, dispatcher, &taskStack, 0, -1, internallyDispatch: true, disposeOriginals: disposeOriginals); + var oldNodes = Nodes; + Nodes = new Buffer(oldNodes.Length, pool); + Refit2WithCacheOptimization(pool, dispatcher, &taskStack, 0, -1, internallyDispatch: true, oldNodes); taskStack.Dispose(pool, dispatcher); + if (disposeOriginalNodes) + oldNodes.Dispose(pool); } /// - /// Refits all bounding boxes in the tree using multiple threads. Reallocates the nodes and metanodes and writes the refit tree into them in depth first traversal order. - /// The tree instance is modified to point to the new nodes and metanodes. + /// Refits all bounding boxes in the tree using multiple threads. Reallocates the nodes and writes the refit tree into them in depth first traversal order. + /// The tree instance is modified to point to the new nodes. /// Pushes tasks into the provided . Does not dispatch threads internally; this is intended to be used as a part of a caller-managed dispatch. /// /// Pool used for allocations during execution. /// Dispatcher used during execution. /// that the refit operation will push tasks onto as needed. /// Index of the worker calling the function. - /// Number of tasks the refit should try to create during execution. - /// Whether to dispose of the original version. If false, it's up to the caller to dispose of them appropriately. - public unsafe void Refit2WithCacheOptimization(BufferPool pool, IThreadDispatcher dispatcher, TaskStack* taskStack, int workerIndex, int targetTaskCount = -1, bool disposeOriginals = true) + /// Number of tasks the refit should try to create during execution. If negative, uses . + /// Whether to dispose of the original nodes buffer. If false, it's up to the caller to dispose of it appropriately. + public unsafe void Refit2WithCacheOptimization(BufferPool pool, IThreadDispatcher dispatcher, TaskStack* taskStack, int workerIndex, int targetTaskCount = -1, bool disposeOriginalNodes = true) + { + var oldNodes = Nodes; + Nodes = new Buffer(oldNodes.Length, pool); + Refit2WithCacheOptimization(pool, dispatcher, taskStack, workerIndex, targetTaskCount, internallyDispatch: false, oldNodes); + if (disposeOriginalNodes) + oldNodes.Dispose(pool); + } + + /// + /// Updates the bounding boxes of all internal nodes in the tree using multiple threads. The refit is based on the provided sourceNodes, and + /// the results are written into the tree's current , , and buffers. + /// The nodes and metanodes will be in depth traversal order. + /// The input source buffer is not modified. + /// Pushes tasks into the provided . Does not dispatch threads internally; this is intended to be used as a part of a caller-managed dispatch. + /// + /// Nodes to base the refit on. + /// Pool used for allocations during execution. + /// Dispatcher used during execution. + /// that the refit operation will push tasks onto as needed. + /// Index of the worker calling the function. + /// Number of tasks the refit should try to create during execution. If negative, uses . + public unsafe void Refit2WithCacheOptimization(Buffer sourceNodes, BufferPool pool, IThreadDispatcher dispatcher, TaskStack* taskStack, int workerIndex, int targetTaskCount = -1) { - Refit2WithCacheOptimization(pool, dispatcher, taskStack, workerIndex, targetTaskCount, internallyDispatch: false, disposeOriginals: disposeOriginals); + Refit2WithCacheOptimization(pool, dispatcher, taskStack, workerIndex, targetTaskCount, internallyDispatch: false, sourceNodes); } } From 34b618b7fab3c831ba371343d9e2515cf3051a28 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 27 Jul 2023 17:29:47 -0500 Subject: [PATCH 768/947] Broadphase Update2 Tuning and scheduling bugfix. --- BepuPhysics/CollisionDetection/BroadPhase.cs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/BepuPhysics/CollisionDetection/BroadPhase.cs b/BepuPhysics/CollisionDetection/BroadPhase.cs index 922810600..a620481ac 100644 --- a/BepuPhysics/CollisionDetection/BroadPhase.cs +++ b/BepuPhysics/CollisionDetection/BroadPhase.cs @@ -300,7 +300,7 @@ public static void DefaultRefinementScheduler(float optimizationFraction, int ro /// Target size of the subtree refinements. public static void DefaultActiveRefinementScheduler(int frameIndex, in Tree tree, out int rootRefinementSize, out int subtreeRefinementCount, out int subtreeRefinementSize) { - DefaultRefinementScheduler(1f / 50f, 8, 4, 4, frameIndex, tree, out rootRefinementSize, out subtreeRefinementCount, out subtreeRefinementSize); + DefaultRefinementScheduler(1f / 20f, 8, 4, 4, frameIndex, tree, out rootRefinementSize, out subtreeRefinementCount, out subtreeRefinementSize); } @@ -314,7 +314,7 @@ public static void DefaultActiveRefinementScheduler(int frameIndex, in Tree tree /// Target size of the subtree refinements. public static void DefaultStaticRefinementScheduler(int frameIndex, in Tree tree, out int rootRefinementSize, out int subtreeRefinementCount, out int subtreeRefinementSize) { - DefaultRefinementScheduler(1f / 200f, 20, 4, 4, frameIndex, tree, out rootRefinementSize, out subtreeRefinementCount, out subtreeRefinementSize); + DefaultRefinementScheduler(1f / 100f, 32, 4, 4, frameIndex, tree, out rootRefinementSize, out subtreeRefinementCount, out subtreeRefinementSize); } struct RefinementContext @@ -356,7 +356,7 @@ static void StaticEntrypointTask(long taskId, void* untypedContext, int workerIn public void Update2(IThreadDispatcher threadDispatcher = null, bool deterministic = false) { ActiveRefinementSchedule(frameIndex, ActiveTree, out var activeRootRefinementSize, out var activeSubtreeRefinementCount, out var activeSubtreeRefinementSize); - StaticRefinementSchedule(frameIndex, ActiveTree, out var staticRootRefinementSize, out var staticSubtreeRefinementCount, out var staticSubtreeRefinementSize); + StaticRefinementSchedule(frameIndex, StaticTree, out var staticRootRefinementSize, out var staticSubtreeRefinementCount, out var staticSubtreeRefinementSize); if (threadDispatcher != null && threadDispatcher.ThreadCount > 1) { //Distribute tasks for refinement roughly in proportion to their cost. From d97b0f13426c9c1d93b415d97329d7f6863f9f5b Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 27 Jul 2023 18:41:49 -0500 Subject: [PATCH 769/947] Did away with in-broadphase static tree optimization; not always doable. Update2 enabled. --- BepuPhysics/CollisionDetection/BroadPhase.cs | 3 +-- BepuPhysics/Simulation.cs | 3 ++- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/BepuPhysics/CollisionDetection/BroadPhase.cs b/BepuPhysics/CollisionDetection/BroadPhase.cs index a620481ac..9d82e21da 100644 --- a/BepuPhysics/CollisionDetection/BroadPhase.cs +++ b/BepuPhysics/CollisionDetection/BroadPhase.cs @@ -341,7 +341,7 @@ static void ActiveEntrypointTask(long taskId, void* untypedContext, int workerIn //Now refit! Note that we use all but one task. It doesn't affect the performance of a refit much (we're not compute bound), and we can use it to do an incremental cache optimization on the static tree. var sourceNodes = context.Tree.Nodes; context.Tree.Nodes = context.TargetNodes; - context.Tree.Refit2WithCacheOptimization(sourceNodes, pool, dispatcher, context.TaskStack, workerIndex, context.TargetTotalTaskCount - 1); + context.Tree.Refit2WithCacheOptimization(sourceNodes, pool, dispatcher, context.TaskStack, workerIndex, context.TargetTotalTaskCount); } static void StaticEntrypointTask(long taskId, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) @@ -349,7 +349,6 @@ static void StaticEntrypointTask(long taskId, void* untypedContext, int workerIn ref var context = ref *(RefinementContext*)untypedContext; var pool = dispatcher.WorkerPools[workerIndex]; context.Tree.Refine2(context.RootRefinementSize, ref context.SubtreeRefinementStartIndex, context.SubtreeRefinementCount, context.SubtreeRefinementSize, pool, dispatcher, context.TaskStack, workerIndex, deterministic: context.Deterministic); - //TODO: Incremental cache optimization. If you end up not implementing it, make sure to give the task back to active refit. } int staticSubtreeRefinementStartIndex, activeSubtreeRefinementStartIndex; diff --git a/BepuPhysics/Simulation.cs b/BepuPhysics/Simulation.cs index 93076af1a..981ffc5c3 100644 --- a/BepuPhysics/Simulation.cs +++ b/BepuPhysics/Simulation.cs @@ -221,7 +221,8 @@ public void PredictBoundingBoxes(float dt, IThreadDispatcher threadDispatcher = public void CollisionDetection(float dt, IThreadDispatcher threadDispatcher = null) { profiler.Start(BroadPhase); - BroadPhase.Update(threadDispatcher); + //BroadPhase.Update(threadDispatcher); + BroadPhase.Update2(threadDispatcher); profiler.End(BroadPhase); profiler.Start(BroadPhaseOverlapFinder); From 670a92415ede77b562a9775628a6a7a89c3e5237 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 29 Jul 2023 20:41:27 -0500 Subject: [PATCH 770/947] Broad phase tasks now properly take into account the target task count. Also guarded against a possible NaN. --- BepuPhysics/CollisionDetection/BroadPhase.cs | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/BepuPhysics/CollisionDetection/BroadPhase.cs b/BepuPhysics/CollisionDetection/BroadPhase.cs index 9d82e21da..d76dfb621 100644 --- a/BepuPhysics/CollisionDetection/BroadPhase.cs +++ b/BepuPhysics/CollisionDetection/BroadPhase.cs @@ -337,7 +337,7 @@ static void ActiveEntrypointTask(long taskId, void* untypedContext, int workerIn { ref var context = ref *(RefinementContext*)untypedContext; var pool = dispatcher.WorkerPools[workerIndex]; - context.Tree.Refine2(context.RootRefinementSize, ref context.SubtreeRefinementStartIndex, context.SubtreeRefinementCount, context.SubtreeRefinementSize, pool, dispatcher, context.TaskStack, workerIndex, deterministic: context.Deterministic); + context.Tree.Refine2(context.RootRefinementSize, ref context.SubtreeRefinementStartIndex, context.SubtreeRefinementCount, context.SubtreeRefinementSize, pool, dispatcher, context.TaskStack, workerIndex, targetTaskCount: context.TargetTaskCount, deterministic: context.Deterministic); //Now refit! Note that we use all but one task. It doesn't affect the performance of a refit much (we're not compute bound), and we can use it to do an incremental cache optimization on the static tree. var sourceNodes = context.Tree.Nodes; context.Tree.Nodes = context.TargetNodes; @@ -348,7 +348,7 @@ static void StaticEntrypointTask(long taskId, void* untypedContext, int workerIn { ref var context = ref *(RefinementContext*)untypedContext; var pool = dispatcher.WorkerPools[workerIndex]; - context.Tree.Refine2(context.RootRefinementSize, ref context.SubtreeRefinementStartIndex, context.SubtreeRefinementCount, context.SubtreeRefinementSize, pool, dispatcher, context.TaskStack, workerIndex, deterministic: context.Deterministic); + context.Tree.Refine2(context.RootRefinementSize, ref context.SubtreeRefinementStartIndex, context.SubtreeRefinementCount, context.SubtreeRefinementSize, pool, dispatcher, context.TaskStack, workerIndex, targetTaskCount: context.TargetTaskCount, deterministic: context.Deterministic); } int staticSubtreeRefinementStartIndex, activeSubtreeRefinementStartIndex; @@ -361,8 +361,8 @@ public void Update2(IThreadDispatcher threadDispatcher = null, bool deterministi //Distribute tasks for refinement roughly in proportion to their cost. //This doesn't need to be perfect. //Cost of a refinement is roughly n * log2(n), for n = refinement size. - var activeCost = float.Log2(activeRootRefinementSize) * activeRootRefinementSize + float.Log2(activeSubtreeRefinementSize) * activeSubtreeRefinementSize * activeSubtreeRefinementCount; - var staticCost = float.Log2(staticRootRefinementSize) * staticRootRefinementSize + float.Log2(staticSubtreeRefinementSize) * staticSubtreeRefinementSize * staticSubtreeRefinementCount; + var activeCost = float.Log2(activeRootRefinementSize + 1) * activeRootRefinementSize + float.Log2(activeSubtreeRefinementSize + 1) * activeSubtreeRefinementSize * activeSubtreeRefinementCount; + var staticCost = float.Log2(staticRootRefinementSize + 1) * staticRootRefinementSize + float.Log2(staticSubtreeRefinementSize + 1) * staticSubtreeRefinementSize * staticSubtreeRefinementCount; var activeTaskFraction = activeCost / (activeCost + staticCost); var targetTotalTaskCount = threadDispatcher.ThreadCount; //could scale this. Empirically, doesn't matter on the CPUs tested so far. var targetActiveTaskCount = (int)float.Ceiling(activeTaskFraction * targetTotalTaskCount); From 3b7f52e976c5683db7e32e1daa15a3131b3af85c Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 31 Jul 2023 19:30:07 -0500 Subject: [PATCH 771/947] Wee oopsy in a very bad attempt. --- BepuPhysics/Trees/Tree_SelfQueries.cs | 269 ++++++++++++++------------ 1 file changed, 146 insertions(+), 123 deletions(-) diff --git a/BepuPhysics/Trees/Tree_SelfQueries.cs b/BepuPhysics/Trees/Tree_SelfQueries.cs index dc163831f..1a4c7b694 100644 --- a/BepuPhysics/Trees/Tree_SelfQueries.cs +++ b/BepuPhysics/Trees/Tree_SelfQueries.cs @@ -12,6 +12,7 @@ using System.Runtime.Intrinsics; using System.Runtime.Intrinsics.X86; using System.Security.Cryptography; +using System.Xml.Linq; namespace BepuPhysics.Trees { @@ -126,20 +127,16 @@ unsafe void GetOverlapsInNode(ref Node node, ref TOverlapHandle ref var a = ref node.A; ref var b = ref node.B; - var ab = BoundingBox.IntersectsUnsafe(a, b); - if (a.Index >= 0) GetOverlapsInNode(ref Nodes[a.Index], ref results); if (b.Index >= 0) GetOverlapsInNode(ref Nodes[b.Index], ref results); - //Test all different nodes. if (ab) { DispatchTestForNodes(ref a, ref b, ref results); } - } public unsafe void GetSelfOverlaps(ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler @@ -297,154 +294,180 @@ static void LeftPack(Vector128 mask, Vector128 a, Vector128 b, ou packedB = Avx.PermuteVar(b.AsSingle(), permuteMask).AsInt32(); } + public unsafe void GetSelfOverlaps2(ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler + { + for (int i = NodeCount - 1; i >= 0; --i) + { + ref var node = ref Nodes[i]; + ref var a = ref node.A; + ref var b = ref node.B; + var ab = BoundingBox.IntersectsUnsafe(a, b); + if (ab) + { + DispatchTestForNodes(ref a, ref b, ref results); + } + } + } + + struct IndexPair + { + public int A; + public int B; + } - public unsafe void GetSelfOverlapsContiguousPrepass(ref TOverlapHandler results, BufferPool pool) where TOverlapHandler : IOverlapHandler + unsafe struct NodeLeafPair { - //If there are less than two leaves, there can't be any overlap. - //This provides a guarantee that there are at least 2 children in each internal node considered by GetOverlapsInNode. - if (LeafCount < 2) - return; + public NodeChild* LeafParent; + public int NodeIndex; + } - //A recursive self test will at some point visit all nodes with certainty. Instead of framing it as a recursive test at all, do a prepass that's just a contiguous iteration. - //Include a little buffer to avoid overruns by vectorized operations. - var stackSize = ((NodeCount + 7) / 8 + 1) * 8; - pool.Take(stackSize, out var crossoverStackA); - pool.Take(stackSize, out var crossoverStackB); - pool.Take(stackSize, out var nodeLeafStackA); - pool.Take(stackSize, out var nodeLeafStackB); - int crossoverStackCount = 0; - int nodeLeafStackCount = 0; - for (int i = 0; i < NodeCount; ++i) + unsafe void AddCrossoverResult(ref NodeChild a, ref NodeChild b, ref QuickList crossovers, ref QuickList nodeLeaf, ref QuickList leafLeaf, BufferPool pool) + { + if (a.Index >= 0 && b.Index >= 0) { - ref var node = ref Nodes[i]; - var a = node.A.Index; - var b = node.B.Index; - var aIsInternal = a >= 0; - var bIsInternal = b >= 0; - if (aIsInternal && bIsInternal) + crossovers.Allocate(pool) = new IndexPair { A = a.Index, B = b.Index }; + } + else if (a.Index < 0 && b.Index < 0) + { + leafLeaf.Allocate(pool) = new IndexPair { A = a.Index, B = b.Index }; + } + else + { + nodeLeaf.Allocate(pool) = a.Index >= 0 + ? new NodeLeafPair { LeafParent = (NodeChild*)Unsafe.AsPointer(ref b), NodeIndex = a.Index } + : new NodeLeafPair { LeafParent = (NodeChild*)Unsafe.AsPointer(ref a), NodeIndex = b.Index }; + } + } + unsafe void ExecuteCrossoverBatch(ref QuickList crossovers, ref QuickList nodeLeaf, ref QuickList leafLeaf, BufferPool pool) + { + while (crossovers.TryPop(out var pair)) + { + ref var a = ref Nodes[pair.A]; + ref var b = ref Nodes[pair.B]; + //There are no shared children, so test them all. + ref var aa = ref a.A; + ref var ab = ref a.B; + ref var ba = ref b.A; + ref var bb = ref b.B; + var aaIntersects = BoundingBox.IntersectsUnsafe(aa, ba); + var abIntersects = BoundingBox.IntersectsUnsafe(aa, bb); + var baIntersects = BoundingBox.IntersectsUnsafe(ab, ba); + var bbIntersects = BoundingBox.IntersectsUnsafe(ab, bb); + + if (aaIntersects) { - if (BoundingBox.IntersectsUnsafe(node.A, node.B)) - { - crossoverStackA[crossoverStackCount] = a; - crossoverStackB[crossoverStackCount] = b; - ++crossoverStackCount; - } + AddCrossoverResult(ref aa, ref ba, ref crossovers, ref nodeLeaf, ref leafLeaf, pool); } - else if (aIsInternal || bIsInternal) + if (abIntersects) { - //One is a leaf, one is internal. - nodeLeafStackA[nodeLeafStackCount] = (uint)i; - nodeLeafStackB[nodeLeafStackCount] = (uint)i | (1u << 31); - ++nodeLeafStackCount; + AddCrossoverResult(ref aa, ref bb, ref crossovers, ref nodeLeaf, ref leafLeaf, pool); } - else + if (baIntersects) { - //Both are leaves. - results.Handle(Encode(a), Encode(b)); + AddCrossoverResult(ref ab, ref ba, ref crossovers, ref nodeLeaf, ref leafLeaf, pool); } - } - - //Now, we need to complete all crossovers and leaf-node tests. Note that leaf-node tests can only generate leaf-node or leaf-leaf tests, they never produce crossovers. - //In contrast, crossovers can generate leaf-node tests. So do crossovers first so we'll have every leaf-node test in the stack ready to go. - while (crossoverStackCount > 0) - { - var toVisitIndex = --crossoverStackCount; - var index0 = crossoverStackA[toVisitIndex]; - var index1 = crossoverStackB[toVisitIndex]; - ref var n0 = ref Nodes[index0]; - ref var n1 = ref Nodes[index1]; - - var aaIntersects = BoundingBox.IntersectsUnsafe(n0.A, n1.A); - var abIntersects = BoundingBox.IntersectsUnsafe(n0.A, n1.B); - var baIntersects = BoundingBox.IntersectsUnsafe(n0.B, n1.A); - var bbIntersects = BoundingBox.IntersectsUnsafe(n0.B, n1.B); - - var intersects = Vector128.Create(aaIntersects ? -1 : 0, abIntersects ? -1 : 0, baIntersects ? -1 : 0, bbIntersects ? -1 : 0); - var indices = Vector128.Create(n0.A.Index, n0.B.Index, n1.A.Index, n1.B.Index); - var n0Indices = Vector128.Shuffle(indices, Vector128.Create(0, 0, 1, 1)); - var n1Indices = Vector128.Shuffle(indices, Vector128.Create(2, 3, 2, 3)); - var n0Internal = Vector128.GreaterThan(n0Indices, Vector128.Zero); - var n1Internal = Vector128.GreaterThan(n1Indices, Vector128.Zero); - - var leafLeaf = Vector128.AndNot(Vector128.AndNot(intersects, n0Internal), n1Internal); - var nodeLeaf = intersects & (n0Internal ^ n1Internal); - var crossover = intersects & n0Internal & n1Internal; - - LeftPack(leafLeaf, Encode(n0Indices), Encode(n1Indices), out var leafleafToPush0, out var leafleafToPush1, out var leafLeafCount); - var parentEncoded0 = Vector128.BitwiseOr(Vector128.Create(index0), Vector128.Create(0, 0, 1 << 31, 1 << 31)); - var parentEncoded1 = Vector128.BitwiseOr(Vector128.Create(index1), Vector128.Create(0, 1 << 31, 0, 1 << 31)); - LeftPack(nodeLeaf, parentEncoded0, parentEncoded1, out var nodeLeafToPush0, out var nodeLeafToPush1, out var nodeLeafCount); - LeftPack(crossover, n0Indices, n1Indices, out var crossoverToPush0, out var crossoverToPush1, out var crossoverCount); - - if (leafLeafCount > 0) + if (bbIntersects) { - results.Handle(leafleafToPush0[0], leafleafToPush1[0]); - if (leafLeafCount > 1) results.Handle(leafleafToPush0[1], leafleafToPush1[1]); - if (leafLeafCount > 2) results.Handle(leafleafToPush0[2], leafleafToPush1[2]); - if (leafLeafCount > 3) results.Handle(leafleafToPush0[3], leafleafToPush1[3]); + AddCrossoverResult(ref ab, ref bb, ref crossovers, ref nodeLeaf, ref leafLeaf, pool); } - if (nodeLeafCount > 0) + } + } + unsafe void ExecuteNodeLeafBatch(ref QuickList nodeLeaf, ref QuickList leafLeaf, BufferPool pool) + { + while (nodeLeaf.TryPop(out var pair)) + { + ref var leafChild = ref *pair.LeafParent; + ref var node = ref Nodes[pair.NodeIndex]; + ref var a = ref node.A; + ref var b = ref node.B; + var bIndex = b.Index; + var aIntersects = BoundingBox.IntersectsUnsafe(leafChild, a); + var bIntersects = BoundingBox.IntersectsUnsafe(leafChild, b); + if (aIntersects) { - Vector128.Store(nodeLeafToPush0.AsUInt32(), nodeLeafStackA.Memory + nodeLeafStackCount); - Vector128.Store(nodeLeafToPush1.AsUInt32(), nodeLeafStackB.Memory + nodeLeafStackCount); - nodeLeafStackCount += nodeLeafCount; + if (a.Index < 0) + { + leafLeaf.Allocate(pool) = new IndexPair { A = leafChild.Index, B = a.Index }; + } + else + { + nodeLeaf.Allocate(pool) = new NodeLeafPair { LeafParent = pair.LeafParent, NodeIndex = a.Index }; + } } - if (crossoverCount > 0) + if (bIntersects) { - Vector128.Store(crossoverToPush0, crossoverStackA.Memory + crossoverStackCount); - Vector128.Store(crossoverToPush1, crossoverStackB.Memory + crossoverStackCount); - crossoverStackCount += crossoverCount; + if (b.Index < 0) + { + leafLeaf.Allocate(pool) = new IndexPair { A = leafChild.Index, B = b.Index }; + } + else + { + nodeLeaf.Allocate(pool) = new NodeLeafPair { LeafParent = pair.LeafParent, NodeIndex = b.Index }; + } } - - //Console.WriteLine($"new stackcounts: nodeleaf {nodeLeafStackCount}, crossover {crossoverStackCount}; new leafleaf {leafleafCount}, nodeleaf {nodeLeafCount}, crossover {crossoverCount}"); } - //Console.WriteLine("End crossovers"); + } - pool.Return(ref crossoverStackA); - pool.Return(ref crossoverStackB); - QuickList stack = new(NodeCount, pool); + unsafe void FlushLeafLeaf(ref QuickList leafLeaf, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler + { + for (int leafLeafIndex = 0; leafLeafIndex < leafLeaf.Count; ++leafLeafIndex) + { + var pair = leafLeaf[leafLeafIndex]; + results.Handle(Encode(pair.A), Encode(pair.B)); + } + leafLeaf.Count = 0; + } - for (int i = 0; i < nodeLeafStackCount; ++i) + public unsafe void GetSelfOverlapsBatched(ref TOverlapHandler results, BufferPool pool) where TOverlapHandler : IOverlapHandler + { + const int crossoverBatchSizeTarget = 8; + const int leafLeafBatchSizeTarget = 8; + const int nodeLeafBatchSizeTarget = 8; + var crossovers = new QuickList(crossoverBatchSizeTarget * 16, pool); + var leafLeaf = new QuickList(leafLeafBatchSizeTarget * 16, pool); + var nodeLeaf = new QuickList(nodeLeafBatchSizeTarget * 16, pool); + for (int i = NodeCount - 1; i >= 0; --i) { - ref var childA = ref GetLeafChild(ref this, nodeLeafStackA[i]); - ref var childB = ref GetLeafChild(ref this, nodeLeafStackB[i]); - Debug.Assert((childA.Index < 0) ^ (childB.Index < 0), "One and only one of the two children must be a leaf."); - ref var leafChild = ref childA.Index < 0 ? ref childA : ref childB; - stack.AllocateUnsafely() = childA.Index < 0 ? childB.Index : childA.Index; - var leafIndex = Encode(leafChild.Index); - Debug.Assert(stack.Count == 0); - while (stack.TryPop(out var nodeToTest)) + ref var node = ref Nodes[i]; + ref var a = ref node.A; + ref var b = ref node.B; + if (BoundingBox.IntersectsUnsafe(a, b)) { - ref var node = ref Nodes[nodeToTest]; - var a = node.A.Index; - var b = node.B.Index; - var aIntersected = BoundingBox.IntersectsUnsafe(leafChild, node.A); - var bIntersected = BoundingBox.IntersectsUnsafe(leafChild, node.B); - - if (bIntersected) + if (a.Index >= 0 && b.Index >= 0) { - if (b >= 0) - stack.AllocateUnsafely() = b; - else - results.Handle(leafIndex, Encode(b)); + crossovers.Allocate(pool) = new IndexPair { A = a.Index, B = b.Index }; } - if (aIntersected) + else if (a.Index < 0 && b.Index < 0) { - if (a >= 0) - stack.AllocateUnsafely() = a; - else - results.Handle(leafIndex, Encode(a)); + leafLeaf.Allocate(pool) = new IndexPair { A = a.Index, B = b.Index }; + } + else + { + //Leaf-node. + nodeLeaf.Allocate(pool) = a.Index >= 0 + ? new NodeLeafPair { LeafParent = (NodeChild*)Unsafe.AsPointer(ref b), NodeIndex = a.Index } + : new NodeLeafPair { LeafParent = (NodeChild*)Unsafe.AsPointer(ref a), NodeIndex = b.Index }; } - + } + if (crossovers.Count >= crossoverBatchSizeTarget) + { + ExecuteCrossoverBatch(ref crossovers, ref nodeLeaf, ref leafLeaf, pool); + } + if (nodeLeaf.Count >= nodeLeafBatchSizeTarget) + { + ExecuteNodeLeafBatch(ref nodeLeaf, ref leafLeaf, pool); + } + if (leafLeaf.Count >= leafLeafBatchSizeTarget) + { + FlushLeafLeaf(ref leafLeaf, ref results); } } - pool.Return(ref nodeLeafStackA); - pool.Return(ref nodeLeafStackB); - stack.Dispose(pool); + //Flush any remaining pairs. + ExecuteCrossoverBatch(ref crossovers, ref nodeLeaf, ref leafLeaf, pool); + ExecuteNodeLeafBatch(ref nodeLeaf, ref leafLeaf, pool); + FlushLeafLeaf(ref leafLeaf, ref results); } - - } } From 182b214b479e38e634714df436c657325f95a734 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 1 Aug 2023 23:09:12 -0500 Subject: [PATCH 772/947] Multithreaded self query. --- BepuPhysics/Trees/Tree_Refine2.cs | 2 +- BepuPhysics/Trees/Tree_SelfQueries.cs | 218 ++++++++++++++++++++------ 2 files changed, 175 insertions(+), 45 deletions(-) diff --git a/BepuPhysics/Trees/Tree_Refine2.cs b/BepuPhysics/Trees/Tree_Refine2.cs index 5a7725e82..311303406 100644 --- a/BepuPhysics/Trees/Tree_Refine2.cs +++ b/BepuPhysics/Trees/Tree_Refine2.cs @@ -617,7 +617,7 @@ public unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementStar /// If the refine is single threaded, it is already deterministic and this flag has no effect. /// that the refine operation will push tasks onto as needed. /// Index of the worker calling the function. - /// Number of tasks the refit should try to create during execution. If negative, uses . + /// Number of tasks the refinement should try to create during execution. If negative, uses . /// Nodes will not be refit. public unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementStartIndex, int subtreeRefinementCount, int subtreeRefinementSize, BufferPool pool, IThreadDispatcher threadDispatcher, TaskStack* taskStack, int workerIndex, int targetTaskCount = -1, bool deterministic = false) diff --git a/BepuPhysics/Trees/Tree_SelfQueries.cs b/BepuPhysics/Trees/Tree_SelfQueries.cs index 1a4c7b694..b59273b37 100644 --- a/BepuPhysics/Trees/Tree_SelfQueries.cs +++ b/BepuPhysics/Trees/Tree_SelfQueries.cs @@ -3,6 +3,7 @@ using BepuUtilities; using BepuUtilities.Collections; using BepuUtilities.Memory; +using BepuUtilities.TaskScheduling; using System; using System.Diagnostics; using System.Numerics; @@ -13,17 +14,39 @@ using System.Runtime.Intrinsics.X86; using System.Security.Cryptography; using System.Xml.Linq; +using static BepuPhysics.Trees.Tree; namespace BepuPhysics.Trees { + /// + /// Overlap callback for tree overlap queries. + /// public interface IOverlapHandler { + /// + /// Handles an overlap between leaves. + /// + /// Index of the first leaf in the overlap. + /// Index of the second leaf in the overlap. void Handle(int indexA, int indexB); } - + /// + /// Overlap callback for tree overlap queries. Used in multithreaded contexts. + /// + public interface IThreadedOverlapHandler + { + /// + /// Handles an overlap between leaves. + /// + /// Index of the first leaf in the overlap. + /// Index of the second leaf in the overlap. + /// Index of the worker reporting the overlap. + void Handle(int indexA, int indexB, int workerIndex); + } partial struct Tree { + //TODO: This contains a lot of empirically tested implementations on much older runtimes. //I suspect results would be different on modern versions of ryujit. In particular, recursion is very unlikely to be the fastest approach. //(I don't immediately recall what made the non-recursive version slower last time- it's possible that it was making use of stackalloc and I hadn't yet realized that it @@ -31,7 +54,7 @@ partial struct Tree //Note that all of these implementations make use of a fully generic handler. It could be dumping to a list, or it could be directly processing the results- at this //level of abstraction we don't know or care. It's up to the user to use a handler which maximizes performance if they want it. We'll be using this in the broad phase. - unsafe void DispatchTestForLeaf(int leafIndex, ref NodeChild leafChild, int nodeIndex, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler + readonly unsafe void DispatchTestForLeaf(int leafIndex, ref NodeChild leafChild, int nodeIndex, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler { if (nodeIndex < 0) { @@ -42,7 +65,7 @@ unsafe void DispatchTestForLeaf(int leafIndex, ref NodeChild le TestLeafAgainstNode(leafIndex, ref leafChild, nodeIndex, ref results); } } - unsafe void TestLeafAgainstNode(int leafIndex, ref NodeChild leafChild, int nodeIndex, ref TOverlapHandler results) + readonly unsafe void TestLeafAgainstNode(int leafIndex, ref NodeChild leafChild, int nodeIndex, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler { ref var node = ref Nodes[nodeIndex]; @@ -66,7 +89,7 @@ unsafe void TestLeafAgainstNode(int leafIndex, ref NodeChild le } [MethodImpl(MethodImplOptions.AggressiveInlining)] - unsafe void DispatchTestForNodes(ref NodeChild a, ref NodeChild b, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler + readonly unsafe void DispatchTestForNodes(ref NodeChild a, ref NodeChild b, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler { if (a.Index >= 0) { @@ -92,7 +115,7 @@ unsafe void DispatchTestForNodes(ref NodeChild a, ref NodeChild } } - unsafe void GetOverlapsBetweenDifferentNodes(ref Node a, ref Node b, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler + readonly unsafe void GetOverlapsBetweenDifferentNodes(ref Node a, ref Node b, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler { //There are no shared children, so test them all. ref var aa = ref a.A; @@ -122,7 +145,7 @@ unsafe void GetOverlapsBetweenDifferentNodes(ref Node a, ref No } } - unsafe void GetOverlapsInNode(ref Node node, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler + readonly unsafe void GetOverlapsInNode(ref Node node, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler { ref var a = ref node.A; @@ -139,7 +162,7 @@ unsafe void GetOverlapsInNode(ref Node node, ref TOverlapHandle } } - public unsafe void GetSelfOverlaps(ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler + public readonly unsafe void GetSelfOverlaps(ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler { //If there are less than two leaves, there can't be any overlap. //This provides a guarantee that there are at least 2 children in each internal node considered by GetOverlapsInNode. @@ -294,20 +317,6 @@ static void LeftPack(Vector128 mask, Vector128 a, Vector128 b, ou packedB = Avx.PermuteVar(b.AsSingle(), permuteMask).AsInt32(); } - public unsafe void GetSelfOverlaps2(ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler - { - for (int i = NodeCount - 1; i >= 0; --i) - { - ref var node = ref Nodes[i]; - ref var a = ref node.A; - ref var b = ref node.B; - var ab = BoundingBox.IntersectsUnsafe(a, b); - if (ab) - { - DispatchTestForNodes(ref a, ref b, ref results); - } - } - } struct IndexPair { @@ -321,7 +330,7 @@ unsafe struct NodeLeafPair public int NodeIndex; } - unsafe void AddCrossoverResult(ref NodeChild a, ref NodeChild b, ref QuickList crossovers, ref QuickList nodeLeaf, ref QuickList leafLeaf, BufferPool pool) + unsafe void AddCrossoverResult(ref NodeChild a, ref NodeChild b, ref QuickList crossovers, ref QuickList nodeLeaf, ref TOverlapHandler results, BufferPool pool) where TOverlapHandler : IOverlapHandler { if (a.Index >= 0 && b.Index >= 0) { @@ -329,7 +338,7 @@ unsafe void AddCrossoverResult(ref NodeChild a, ref NodeChild b, ref QuickList crossovers, ref QuickList nodeLeaf, ref QuickList leafLeaf, BufferPool pool) + unsafe void ExecuteCrossoverBatch(ref QuickList crossovers, ref QuickList nodeLeaf, ref TOverlapHandler results, BufferPool pool) where TOverlapHandler : IOverlapHandler { while (crossovers.TryPop(out var pair)) { @@ -356,23 +365,23 @@ unsafe void ExecuteCrossoverBatch(ref QuickList crossovers, ref Quick if (aaIntersects) { - AddCrossoverResult(ref aa, ref ba, ref crossovers, ref nodeLeaf, ref leafLeaf, pool); + AddCrossoverResult(ref aa, ref ba, ref crossovers, ref nodeLeaf, ref results, pool); } if (abIntersects) { - AddCrossoverResult(ref aa, ref bb, ref crossovers, ref nodeLeaf, ref leafLeaf, pool); + AddCrossoverResult(ref aa, ref bb, ref crossovers, ref nodeLeaf, ref results, pool); } if (baIntersects) { - AddCrossoverResult(ref ab, ref ba, ref crossovers, ref nodeLeaf, ref leafLeaf, pool); + AddCrossoverResult(ref ab, ref ba, ref crossovers, ref nodeLeaf, ref results, pool); } if (bbIntersects) { - AddCrossoverResult(ref ab, ref bb, ref crossovers, ref nodeLeaf, ref leafLeaf, pool); + AddCrossoverResult(ref ab, ref bb, ref crossovers, ref nodeLeaf, ref results, pool); } } } - unsafe void ExecuteNodeLeafBatch(ref QuickList nodeLeaf, ref QuickList leafLeaf, BufferPool pool) + unsafe void ExecuteNodeLeafBatch(ref QuickList nodeLeaf, ref TOverlapHandler results, BufferPool pool) where TOverlapHandler : IOverlapHandler { while (nodeLeaf.TryPop(out var pair)) { @@ -387,7 +396,7 @@ unsafe void ExecuteNodeLeafBatch(ref QuickList nodeLeaf, ref Quick { if (a.Index < 0) { - leafLeaf.Allocate(pool) = new IndexPair { A = leafChild.Index, B = a.Index }; + results.Handle(Encode(leafChild.Index), Encode(a.Index)); } else { @@ -398,7 +407,7 @@ unsafe void ExecuteNodeLeafBatch(ref QuickList nodeLeaf, ref Quick { if (b.Index < 0) { - leafLeaf.Allocate(pool) = new IndexPair { A = leafChild.Index, B = b.Index }; + results.Handle(Encode(leafChild.Index), Encode(b.Index)); } else { @@ -420,11 +429,9 @@ unsafe void FlushLeafLeaf(ref QuickList leafLeaf, re public unsafe void GetSelfOverlapsBatched(ref TOverlapHandler results, BufferPool pool) where TOverlapHandler : IOverlapHandler { - const int crossoverBatchSizeTarget = 8; - const int leafLeafBatchSizeTarget = 8; - const int nodeLeafBatchSizeTarget = 8; + const int crossoverBatchSizeTarget = 16; + const int nodeLeafBatchSizeTarget = 16; var crossovers = new QuickList(crossoverBatchSizeTarget * 16, pool); - var leafLeaf = new QuickList(leafLeafBatchSizeTarget * 16, pool); var nodeLeaf = new QuickList(nodeLeafBatchSizeTarget * 16, pool); for (int i = NodeCount - 1; i >= 0; --i) { @@ -439,7 +446,7 @@ public unsafe void GetSelfOverlapsBatched(ref TOverlapHandler r } else if (a.Index < 0 && b.Index < 0) { - leafLeaf.Allocate(pool) = new IndexPair { A = a.Index, B = b.Index }; + results.Handle(Encode(a.Index), Encode(b.Index)); } else { @@ -451,23 +458,146 @@ public unsafe void GetSelfOverlapsBatched(ref TOverlapHandler r } if (crossovers.Count >= crossoverBatchSizeTarget) { - ExecuteCrossoverBatch(ref crossovers, ref nodeLeaf, ref leafLeaf, pool); + ExecuteCrossoverBatch(ref crossovers, ref nodeLeaf, ref results, pool); } if (nodeLeaf.Count >= nodeLeafBatchSizeTarget) { - ExecuteNodeLeafBatch(ref nodeLeaf, ref leafLeaf, pool); + ExecuteNodeLeafBatch(ref nodeLeaf, ref results, pool); } - if (leafLeaf.Count >= leafLeafBatchSizeTarget) + } + //Flush any remaining pairs. + ExecuteCrossoverBatch(ref crossovers, ref nodeLeaf, ref results, pool); + ExecuteNodeLeafBatch(ref nodeLeaf, ref results, pool); + crossovers.Dispose(pool); + nodeLeaf.Dispose(pool); + } + + + + + + readonly unsafe void GetSelfOverlaps2(ref TOverlapHandler results, int start, int end) where TOverlapHandler : IOverlapHandler + { + Debug.Assert(end >= 0 && end <= NodeCount && start >= 0 && start < NodeCount); + for (int i = end - 1; i >= start; --i) + { + ref var node = ref Nodes[i]; + ref var a = ref node.A; + ref var b = ref node.B; + var ab = BoundingBox.IntersectsUnsafe(a, b); + if (ab) { - FlushLeafLeaf(ref leafLeaf, ref results); + DispatchTestForNodes(ref a, ref b, ref results); } } - //Flush any remaining pairs. - ExecuteCrossoverBatch(ref crossovers, ref nodeLeaf, ref leafLeaf, pool); - ExecuteNodeLeafBatch(ref nodeLeaf, ref leafLeaf, pool); - FlushLeafLeaf(ref leafLeaf, ref results); + } + + /// + /// Reports all bounding box overlaps between leaves in the tree to the given . + /// + /// Handler to report results to. + public readonly unsafe void GetSelfOverlaps2(ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler + { + GetSelfOverlaps2(ref results, 0, NodeCount); + } + + unsafe struct SelfTestContext where TOverlapHandler : unmanaged, IThreadedOverlapHandler + { + public Tree Tree; + public int LoopTaskCount; + public int LeafThresholdForTask; + public TOverlapHandler* Results; + } + unsafe struct WrappedOverlapHandler : IOverlapHandler where TOverlapHandler : unmanaged, IThreadedOverlapHandler + { + public int WorkerIndex; + public TOverlapHandler* Inner; + public void Handle(int indexA, int indexB) + { + Inner->Handle(indexA, indexB, WorkerIndex); + } + } + + unsafe static void LoopEntryTask(long taskStartAndEnd, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) where TOverlapHandler : unmanaged, IThreadedOverlapHandler + { + var taskStart = (int)taskStartAndEnd; + var taskEnd = (int)(taskStartAndEnd >> 32); + ref var context = ref *(SelfTestContext*)untypedContext; + var wrapped = new WrappedOverlapHandler { Inner = context.Results, WorkerIndex = workerIndex }; + context.Tree.GetSelfOverlaps2(ref wrapped, taskStart, taskEnd); } + private unsafe void GetSelfOverlaps2(ref TOverlapHandler results, + int workerIndex, TaskStack* taskStack, IThreadDispatcher threadDispatcher, bool internallyDispatch, int workerCount, int targetTaskBudget) where TOverlapHandler : unmanaged, IThreadedOverlapHandler + { + targetTaskBudget = int.Min(NodeCount, targetTaskBudget); + if (targetTaskBudget < 0) + targetTaskBudget = threadDispatcher.ThreadCount; + targetTaskBudget *= 2; + + //Crossover tests can generate more overlaps than there are leaves, so the simply dividing the leaf count by the target task count will tend to result in more tasks than necessary. + //BUT... it's not feasible to figure out how many tasks you should actually have ahead of time! The critical thing is that we have *enough* tasks, and that the tasks + //aren't so small that pushing them is a complete waste. + //Don't have to worry about oversubscription, so the potential overhead isn't *too* bad. + var leafThresholdForTask = int.Min(int.Max(LeafCount / targetTaskBudget, 64), LeafCount); + + var resultsCopy = results; + var context = new SelfTestContext { Tree = this, LoopTaskCount = targetTaskBudget, LeafThresholdForTask = leafThresholdForTask, Results = &resultsCopy }; + var nodesPerTaskBase = NodeCount / context.LoopTaskCount; + var remainder = NodeCount - nodesPerTaskBase * context.LoopTaskCount; + Span tasks = stackalloc Task[targetTaskBudget]; + int previousEnd = 0; + for (int i = 0; i < tasks.Length; ++i) + { + var taskStart = previousEnd; + var nodeCountForTask = i < remainder ? nodesPerTaskBase + 1 : nodesPerTaskBase; + var taskEnd = previousEnd + nodeCountForTask; + previousEnd = taskEnd; + tasks[i] = new Task(&LoopEntryTask, &context, (long)taskStart | (((long)taskEnd) << 32)); + } + if (internallyDispatch) + { + //There isn't an active dispatch, so we need to do it. + taskStack->AllocateContinuationAndPush(tasks, workerIndex, threadDispatcher, onComplete: TaskStack.GetRequestStopTask(taskStack)); + TaskStack.DispatchWorkers(threadDispatcher, taskStack, workerCount); + } + else + { + //We're executing from within a multithreaded dispatch already, so we can simply run the tasks and trust that other threads are ready to steal. + taskStack->RunTasks(tasks, workerIndex, threadDispatcher); + } + //Have to copy back the results; it's a value type. + results = resultsCopy; + } + + + /// + /// Reports all bounding box overlaps between leaves in the tree to the given . Uses the thread dispatcher to parallelize overlap testing. + /// + /// Handler to report results to. + /// Pool used for ephemeral allocations. + /// Thread dispatcher used during the overlap testing. + public unsafe void GetSelfOverlaps2(ref TOverlapHandler results, BufferPool pool, IThreadDispatcher threadDispatcher) where TOverlapHandler : unmanaged, IThreadedOverlapHandler + { + var taskStack = new TaskStack(pool, threadDispatcher, threadDispatcher.ThreadCount); + GetSelfOverlaps2(ref results, 0, &taskStack, threadDispatcher, true, threadDispatcher.ThreadCount, threadDispatcher.ThreadCount); + taskStack.Dispose(pool, threadDispatcher); + } + + /// + /// Reports all bounding box overlaps between leaves in the tree to the given . + /// Pushes tasks into the provided . Does not dispatch threads internally; this is intended to be used as a part of a caller-managed dispatch. + /// + /// Handler to report results to. + /// Thread dispatcher used during the overlap test. + /// that the overlap test will push tasks onto as needed. + /// Index of the worker calling the function. + /// Number of tasks the overlap testing should try to create during execution. If negative, uses . + public unsafe void GetSelfOverlaps2(ref TOverlapHandler results, + IThreadDispatcher threadDispatcher, TaskStack* taskStack, int workerIndex, int targetTaskCount = -1) where TOverlapHandler : unmanaged, IThreadedOverlapHandler + { + GetSelfOverlaps2(ref results, workerIndex, taskStack, threadDispatcher, false, threadDispatcher.ThreadCount, targetTaskCount); + } } } From 184332b19e91f7b39d6c0d844875dd6b0335e454 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 5 Aug 2023 17:53:28 -0500 Subject: [PATCH 773/947] Self test variants. --- BepuPhysics/Trees/Tree_SelfQueries.cs | 223 +++++++++++++++++++++- BepuUtilities/TaskScheduling/TaskStack.cs | 16 +- 2 files changed, 232 insertions(+), 7 deletions(-) diff --git a/BepuPhysics/Trees/Tree_SelfQueries.cs b/BepuPhysics/Trees/Tree_SelfQueries.cs index b59273b37..408f309bc 100644 --- a/BepuPhysics/Trees/Tree_SelfQueries.cs +++ b/BepuPhysics/Trees/Tree_SelfQueries.cs @@ -507,6 +507,7 @@ unsafe struct SelfTestContext where TOverlapHandler : unmanaged public int LoopTaskCount; public int LeafThresholdForTask; public TOverlapHandler* Results; + public TaskStack* TaskStack; } unsafe struct WrappedOverlapHandler : IOverlapHandler where TOverlapHandler : unmanaged, IThreadedOverlapHandler { @@ -527,23 +528,232 @@ unsafe static void LoopEntryTask(long taskStartAndEnd, void* un context.Tree.GetSelfOverlaps2(ref wrapped, taskStart, taskEnd); } + unsafe static void CrossoverTask(long id, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) + where TThreadedOverlapHandler : unmanaged, IThreadedOverlapHandler where TOverlapHandler : unmanaged, IOverlapHandler + { + var nodeIndex = (int)id; + ref var context = ref *(SelfTestContext*)untypedContext; + ref var node = ref context.Tree.Nodes[nodeIndex]; + var wrapped = new WrappedOverlapHandler { Inner = context.Results, WorkerIndex = workerIndex }; + context.Tree.DispatchTestForNodes(ref node.A, ref node.B, ref wrapped); + } + unsafe static void LoopEntryTaskWithSubtasks(long taskStartAndEnd, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) where TOverlapHandler : unmanaged, IThreadedOverlapHandler + { + var taskStart = (int)taskStartAndEnd; + var taskEnd = (int)(taskStartAndEnd >> 32); + ref var context = ref *(SelfTestContext*)untypedContext; + var wrapped = new WrappedOverlapHandler { Inner = context.Results, WorkerIndex = workerIndex }; + var nodes = context.Tree.Nodes; + var pool = dispatcher.WorkerPools[workerIndex]; + var puntedNodes = new QuickList(taskEnd - taskStart, pool); + for (int i = taskEnd - 1; i >= taskStart; --i) + { + ref var node = ref nodes[i]; + ref var a = ref node.A; + ref var b = ref node.B; + var ab = BoundingBox.IntersectsUnsafe(a, b); + if (ab) + { + + if (int.Max(a.LeafCount, b.LeafCount) >= context.LeafThresholdForTask) + { + //The number of potential overlaps is fairly high; push this pair to a subtask. + puntedNodes.AllocateUnsafely() = i; + } + else + { + //DispatchTestForNodesWithSubtasks(ref a, ref b, ref wrapped, ref context); + context.Tree.DispatchTestForNodes(ref a, ref b, ref wrapped); + } + } + } + if (puntedNodes.Count > 0) + { + var tasks = new Buffer(puntedNodes.Count, pool); + for (int i = 0; i < tasks.Length; ++i) + { + tasks[i] = new Task(&CrossoverTask>, untypedContext, puntedNodes[i]); + } + puntedNodes.Dispose(pool); + context.TaskStack->RunTasks(tasks, workerIndex, dispatcher); + tasks.Dispose(pool); + } + else + { + puntedNodes.Dispose(pool); + } + } + + unsafe static void LoopEntryTaskWithSubtasks2(long taskStartAndEnd, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) where TOverlapHandler : unmanaged, IThreadedOverlapHandler + { + var taskStart = (int)taskStartAndEnd; + var taskEnd = (int)(taskStartAndEnd >> 32); + ref var context = ref *(SelfTestContext*)untypedContext; + var wrapped = new WrappedOverlapHandler { Inner = context.Results, WorkerIndex = workerIndex }; + var nodes = context.Tree.Nodes; + var pool = dispatcher.WorkerPools[workerIndex]; + var continuationCountEstimate = int.Max(32, (taskEnd - taskStart) / 64); + var subtaskContinuations = new QuickList(continuationCountEstimate, pool); + //for (int i = taskEnd - 1; i >= taskStart; --i) + for (int i = taskStart; i < taskEnd; ++i) + { + ref var node = ref nodes[i]; + ref var a = ref node.A; + ref var b = ref node.B; + var ab = BoundingBox.IntersectsUnsafe(a, b); + if (ab) + { + + if (int.Max(a.LeafCount, b.LeafCount) >= context.LeafThresholdForTask) + { + //The number of potential overlaps is fairly high; push this pair to a subtask. + var subtask = new Task(&CrossoverTask>, untypedContext, i); + var continuation = context.TaskStack->AllocateContinuationAndPush(new Span(ref subtask), workerIndex, dispatcher); + subtaskContinuations.Allocate(pool) = continuation; + } + else + { + //DispatchTestForNodesWithSubtasks(ref a, ref b, ref wrapped, ref context); + context.Tree.DispatchTestForNodes(ref a, ref b, ref wrapped); + } + } + } + for (int i = 0; i < subtaskContinuations.Count; ++i) + { + context.TaskStack->WaitForCompletion(subtaskContinuations[i], workerIndex, dispatcher); + } + subtaskContinuations.Dispose(pool); + + } + + + unsafe static void CrossoverWithSubtasksTask(long encodedNodeIndices, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) where TOverlapHandler : unmanaged, IThreadedOverlapHandler + { + ref var context = ref *(SelfTestContext*)untypedContext; + var nodeIndexA = (int)encodedNodeIndices; + var nodeIndexB = (int)(encodedNodeIndices >> 32); + ref var a = ref context.Tree.Nodes[nodeIndexA]; + ref var b = ref context.Tree.Nodes[nodeIndexB]; + //Note that we don't need to do self tests here; all self tests are performed by the top level enumeration that spawned this task originally. + //The sole purpose of this is to split up crossover tests sufficiently that load balancing works well. + ref var aa = ref a.A; + ref var ab = ref a.B; + ref var ba = ref b.A; + ref var bb = ref b.B; + var aaIntersects = BoundingBox.IntersectsUnsafe(aa, ba); + var abIntersects = BoundingBox.IntersectsUnsafe(aa, bb); + var baIntersects = BoundingBox.IntersectsUnsafe(ab, ba); + var bbIntersects = BoundingBox.IntersectsUnsafe(ab, bb); + Debug.Assert(context.LeafThresholdForTask > 1, "This uses the leaf threshold as an implicit test for being an internal node, so the threshold must be above 1."); + var aaPushTask = aaIntersects && int.Min(aa.LeafCount, ba.LeafCount) >= context.LeafThresholdForTask; + var abPushTask = abIntersects && int.Min(aa.LeafCount, bb.LeafCount) >= context.LeafThresholdForTask; + var baPushTask = baIntersects && int.Min(ab.LeafCount, ba.LeafCount) >= context.LeafThresholdForTask; + var bbPushTask = bbIntersects && int.Min(ab.LeafCount, bb.LeafCount) >= context.LeafThresholdForTask; + var continuationCount = (aaPushTask ? 1 : 0) + (abPushTask ? 1 : 0) + (baPushTask ? 1 : 0) + (bbPushTask ? 1 : 0); + Span continuations = stackalloc ContinuationHandle[continuationCount]; + continuationCount = 0; + if (aaPushTask) + { + continuations[continuationCount++] = context.TaskStack->AllocateContinuationAndPush(new Task(&CrossoverWithSubtasksTask, untypedContext, (long)a.A.Index | ((long)b.A.Index << 32)), workerIndex, dispatcher); + } + if (abPushTask) + { + continuations[continuationCount++] = context.TaskStack->AllocateContinuationAndPush(new Task(&CrossoverWithSubtasksTask, untypedContext, (long)a.A.Index | ((long)b.B.Index << 32)), workerIndex, dispatcher); + } + if (baPushTask) + { + continuations[continuationCount++] = context.TaskStack->AllocateContinuationAndPush(new Task(&CrossoverWithSubtasksTask, untypedContext, (long)a.B.Index | ((long)b.A.Index << 32)), workerIndex, dispatcher); + } + if (bbPushTask) + { + continuations[continuationCount++] = context.TaskStack->AllocateContinuationAndPush(new Task(&CrossoverWithSubtasksTask, untypedContext, (long)a.B.Index | ((long)b.B.Index << 32)), workerIndex, dispatcher); + } + //We've pushed every crossover worthy of further splitting onto the stack. The moment any thread as an opening, it'll be investigated. + //For anything *not* pushed to a task, we can just run the test directly. + //(These tests are deferred until after the pushes because we don't want to delay the availability of the other tasks. + var wrappedResults = new WrappedOverlapHandler { Inner = context.Results, WorkerIndex = workerIndex }; + if (!aaPushTask && aaIntersects) + { + context.Tree.DispatchTestForNodes(ref aa, ref ba, ref wrappedResults); + } + if (!abPushTask && abIntersects) + { + context.Tree.DispatchTestForNodes(ref aa, ref bb, ref wrappedResults); + } + if (!baPushTask && baIntersects) + { + context.Tree.DispatchTestForNodes(ref ab, ref ba, ref wrappedResults); + } + if (!bbPushTask && bbIntersects) + { + context.Tree.DispatchTestForNodes(ref ab, ref bb, ref wrappedResults); + } + //The caller will use completion as a termination condition, so we shouldn't return until all spawned tasks complete. + for (int i = 0; i < continuations.Length; ++i) + context.TaskStack->WaitForCompletion(continuations[i], workerIndex, dispatcher); + + } + + unsafe static void LoopEntryTaskWithSubtasks3(long taskStartAndEnd, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) where TOverlapHandler : unmanaged, IThreadedOverlapHandler + { + var taskStart = (int)taskStartAndEnd; + var taskEnd = (int)(taskStartAndEnd >> 32); + ref var context = ref *(SelfTestContext*)untypedContext; + var wrapped = new WrappedOverlapHandler { Inner = context.Results, WorkerIndex = workerIndex }; + var nodes = context.Tree.Nodes; + var pool = dispatcher.WorkerPools[workerIndex]; + var continuationCountEstimate = int.Max(32, (taskEnd - taskStart) / 64); + var subtaskContinuations = new QuickList(continuationCountEstimate, pool); + //for (int i = taskEnd - 1; i >= taskStart; --i) + for (int i = taskStart; i < taskEnd; ++i) + { + ref var node = ref nodes[i]; + ref var a = ref node.A; + ref var b = ref node.B; + var ab = BoundingBox.IntersectsUnsafe(a, b); + if (ab) + { + if (int.Min(a.LeafCount, b.LeafCount) >= context.LeafThresholdForTask) + { + //The number of potential overlaps is high; push this pair to a subtask. + subtaskContinuations.Allocate(pool) = context.TaskStack->AllocateContinuationAndPush(new Task(&CrossoverWithSubtasksTask, untypedContext, (long)a.Index | ((long)b.Index << 32)), workerIndex, dispatcher); + } + else + { + context.Tree.DispatchTestForNodes(ref a, ref b, ref wrapped); + } + } + } + //if(subtaskContinuations.Count > 0) + //{ + // Console.WriteLine($"Start {taskStart}: {subtaskContinuations.Count}"); + //} + for (int i = 0; i < subtaskContinuations.Count; ++i) + { + context.TaskStack->WaitForCompletion(subtaskContinuations[i], workerIndex, dispatcher); + } + subtaskContinuations.Dispose(pool); + + } + private unsafe void GetSelfOverlaps2(ref TOverlapHandler results, - int workerIndex, TaskStack* taskStack, IThreadDispatcher threadDispatcher, bool internallyDispatch, int workerCount, int targetTaskBudget) where TOverlapHandler : unmanaged, IThreadedOverlapHandler + int workerIndex, TaskStack* taskStack, IThreadDispatcher threadDispatcher, bool internallyDispatch, int workerCount, int targetTaskBudget) where TOverlapHandler : unmanaged, IThreadedOverlapHandler { targetTaskBudget = int.Min(NodeCount, targetTaskBudget); if (targetTaskBudget < 0) targetTaskBudget = threadDispatcher.ThreadCount; - targetTaskBudget *= 2; + targetTaskBudget *= 8; //Crossover tests can generate more overlaps than there are leaves, so the simply dividing the leaf count by the target task count will tend to result in more tasks than necessary. //BUT... it's not feasible to figure out how many tasks you should actually have ahead of time! The critical thing is that we have *enough* tasks, and that the tasks //aren't so small that pushing them is a complete waste. //Don't have to worry about oversubscription, so the potential overhead isn't *too* bad. - var leafThresholdForTask = int.Min(int.Max(LeafCount / targetTaskBudget, 64), LeafCount); + var leafThresholdForTask = int.Min(int.Max(LeafCount / (targetTaskBudget * 8), 64), LeafCount); + leafThresholdForTask = 16384; var resultsCopy = results; - var context = new SelfTestContext { Tree = this, LoopTaskCount = targetTaskBudget, LeafThresholdForTask = leafThresholdForTask, Results = &resultsCopy }; + var context = new SelfTestContext { Tree = this, LoopTaskCount = targetTaskBudget, LeafThresholdForTask = leafThresholdForTask, Results = &resultsCopy, TaskStack = taskStack }; var nodesPerTaskBase = NodeCount / context.LoopTaskCount; var remainder = NodeCount - nodesPerTaskBase * context.LoopTaskCount; Span tasks = stackalloc Task[targetTaskBudget]; @@ -554,7 +764,7 @@ private unsafe void GetSelfOverlaps2(ref TOverlapHandler result var nodeCountForTask = i < remainder ? nodesPerTaskBase + 1 : nodesPerTaskBase; var taskEnd = previousEnd + nodeCountForTask; previousEnd = taskEnd; - tasks[i] = new Task(&LoopEntryTask, &context, (long)taskStart | (((long)taskEnd) << 32)); + tasks[i] = new Task(&LoopEntryTaskWithSubtasks3, &context, (long)taskStart | (((long)taskEnd) << 32)); } if (internallyDispatch) { @@ -599,5 +809,8 @@ public unsafe void GetSelfOverlaps2(ref TOverlapHandler results { GetSelfOverlaps2(ref results, workerIndex, taskStack, threadDispatcher, false, threadDispatcher.ThreadCount, targetTaskCount); } + + + // _______________ } } diff --git a/BepuUtilities/TaskScheduling/TaskStack.cs b/BepuUtilities/TaskScheduling/TaskStack.cs index 45c317cef..73eee1c2f 100644 --- a/BepuUtilities/TaskScheduling/TaskStack.cs +++ b/BepuUtilities/TaskScheduling/TaskStack.cs @@ -264,7 +264,6 @@ public void Push(Span tasks, int workerIndex, IThreadDispatcher dispatcher } } - /// /// Pushes a set of tasks to the stack with a created continuation. /// @@ -274,7 +273,6 @@ public void Push(Span tasks, int workerIndex, IThreadDispatcher dispatcher /// User tag associated with the job spanning the submitted tasks. /// Task to run upon completion of all the submitted tasks, if any. /// Handle of the continuation created for these tasks. - /// Note that this will keep trying until task submission succeeds. public ContinuationHandle AllocateContinuationAndPush(Span tasks, int workerIndex, IThreadDispatcher dispatcher, ulong tag = 0, Task onComplete = default) { var continuationHandle = AllocateContinuation(tasks.Length, workerIndex, dispatcher, onComplete); @@ -288,6 +286,20 @@ public ContinuationHandle AllocateContinuationAndPush(Span tasks, int work return continuationHandle; } + /// + /// Pushes a task to the stack with a created continuation. + /// + /// Task composing the job. A continuation will be assigned internally; no continuation should be present on any of the provided task. + /// Thread dispatcher to allocate thread data from if necessary. + /// Index of the worker stack to push the task onto. + /// User tag associated with the task's job. + /// Task to run upon completion of all the submitted task, if any. + /// Handle of the continuation created for these task. + public ContinuationHandle AllocateContinuationAndPush(Task task, int workerIndex, IThreadDispatcher dispatcher, ulong tag = 0, Task onComplete = default) + { + return AllocateContinuationAndPush(new Span(ref task), workerIndex, dispatcher, tag, onComplete); + } + /// /// Waits for a continuation to be completed. /// From 944e0d65c9df05a0a512be7faf675de679254209 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 5 Aug 2023 19:06:32 -0500 Subject: [PATCH 774/947] Some reasonable tuning for early large node submission. Outperforms inner subtasks. --- BepuPhysics/Trees/Tree_SelfQueries.cs | 150 ++++++++------------------ 1 file changed, 42 insertions(+), 108 deletions(-) diff --git a/BepuPhysics/Trees/Tree_SelfQueries.cs b/BepuPhysics/Trees/Tree_SelfQueries.cs index 408f309bc..baf91f86b 100644 --- a/BepuPhysics/Trees/Tree_SelfQueries.cs +++ b/BepuPhysics/Trees/Tree_SelfQueries.cs @@ -528,105 +528,6 @@ unsafe static void LoopEntryTask(long taskStartAndEnd, void* un context.Tree.GetSelfOverlaps2(ref wrapped, taskStart, taskEnd); } - unsafe static void CrossoverTask(long id, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) - where TThreadedOverlapHandler : unmanaged, IThreadedOverlapHandler where TOverlapHandler : unmanaged, IOverlapHandler - { - var nodeIndex = (int)id; - ref var context = ref *(SelfTestContext*)untypedContext; - ref var node = ref context.Tree.Nodes[nodeIndex]; - var wrapped = new WrappedOverlapHandler { Inner = context.Results, WorkerIndex = workerIndex }; - context.Tree.DispatchTestForNodes(ref node.A, ref node.B, ref wrapped); - } - unsafe static void LoopEntryTaskWithSubtasks(long taskStartAndEnd, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) where TOverlapHandler : unmanaged, IThreadedOverlapHandler - { - var taskStart = (int)taskStartAndEnd; - var taskEnd = (int)(taskStartAndEnd >> 32); - ref var context = ref *(SelfTestContext*)untypedContext; - var wrapped = new WrappedOverlapHandler { Inner = context.Results, WorkerIndex = workerIndex }; - var nodes = context.Tree.Nodes; - var pool = dispatcher.WorkerPools[workerIndex]; - var puntedNodes = new QuickList(taskEnd - taskStart, pool); - for (int i = taskEnd - 1; i >= taskStart; --i) - { - ref var node = ref nodes[i]; - ref var a = ref node.A; - ref var b = ref node.B; - var ab = BoundingBox.IntersectsUnsafe(a, b); - if (ab) - { - - if (int.Max(a.LeafCount, b.LeafCount) >= context.LeafThresholdForTask) - { - //The number of potential overlaps is fairly high; push this pair to a subtask. - puntedNodes.AllocateUnsafely() = i; - } - else - { - //DispatchTestForNodesWithSubtasks(ref a, ref b, ref wrapped, ref context); - context.Tree.DispatchTestForNodes(ref a, ref b, ref wrapped); - } - } - } - if (puntedNodes.Count > 0) - { - var tasks = new Buffer(puntedNodes.Count, pool); - for (int i = 0; i < tasks.Length; ++i) - { - tasks[i] = new Task(&CrossoverTask>, untypedContext, puntedNodes[i]); - } - puntedNodes.Dispose(pool); - context.TaskStack->RunTasks(tasks, workerIndex, dispatcher); - tasks.Dispose(pool); - } - else - { - puntedNodes.Dispose(pool); - } - } - - unsafe static void LoopEntryTaskWithSubtasks2(long taskStartAndEnd, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) where TOverlapHandler : unmanaged, IThreadedOverlapHandler - { - var taskStart = (int)taskStartAndEnd; - var taskEnd = (int)(taskStartAndEnd >> 32); - ref var context = ref *(SelfTestContext*)untypedContext; - var wrapped = new WrappedOverlapHandler { Inner = context.Results, WorkerIndex = workerIndex }; - var nodes = context.Tree.Nodes; - var pool = dispatcher.WorkerPools[workerIndex]; - var continuationCountEstimate = int.Max(32, (taskEnd - taskStart) / 64); - var subtaskContinuations = new QuickList(continuationCountEstimate, pool); - //for (int i = taskEnd - 1; i >= taskStart; --i) - for (int i = taskStart; i < taskEnd; ++i) - { - ref var node = ref nodes[i]; - ref var a = ref node.A; - ref var b = ref node.B; - var ab = BoundingBox.IntersectsUnsafe(a, b); - if (ab) - { - - if (int.Max(a.LeafCount, b.LeafCount) >= context.LeafThresholdForTask) - { - //The number of potential overlaps is fairly high; push this pair to a subtask. - var subtask = new Task(&CrossoverTask>, untypedContext, i); - var continuation = context.TaskStack->AllocateContinuationAndPush(new Span(ref subtask), workerIndex, dispatcher); - subtaskContinuations.Allocate(pool) = continuation; - } - else - { - //DispatchTestForNodesWithSubtasks(ref a, ref b, ref wrapped, ref context); - context.Tree.DispatchTestForNodes(ref a, ref b, ref wrapped); - } - } - } - for (int i = 0; i < subtaskContinuations.Count; ++i) - { - context.TaskStack->WaitForCompletion(subtaskContinuations[i], workerIndex, dispatcher); - } - subtaskContinuations.Dispose(pool); - - } - - unsafe static void CrossoverWithSubtasksTask(long encodedNodeIndices, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) where TOverlapHandler : unmanaged, IThreadedOverlapHandler { ref var context = ref *(SelfTestContext*)untypedContext; @@ -750,21 +651,57 @@ private unsafe void GetSelfOverlaps2(ref TOverlapHandler result //aren't so small that pushing them is a complete waste. //Don't have to worry about oversubscription, so the potential overhead isn't *too* bad. var leafThresholdForTask = int.Min(int.Max(LeafCount / (targetTaskBudget * 8), 64), LeafCount); - leafThresholdForTask = 16384; + leafThresholdForTask = 32; var resultsCopy = results; var context = new SelfTestContext { Tree = this, LoopTaskCount = targetTaskBudget, LeafThresholdForTask = leafThresholdForTask, Results = &resultsCopy, TaskStack = taskStack }; - var nodesPerTaskBase = NodeCount / context.LoopTaskCount; - var remainder = NodeCount - nodesPerTaskBase * context.LoopTaskCount; + + //Go ahead and submit very large early nodes as independent tasks to help with load balancing. + //(This isn't guaranteed, or even intended, to catch all large individual nodes. It's just an easy way to get some of them.) + var earlyIsolatedNodeIntervalEnd = 0; + const int maximumIsolatedNodeCapacity = 32; + int isolatedNodeCapacity = int.Min(maximumIsolatedNodeCapacity, targetTaskBudget); + var earlyIsolatedNodesMemory = stackalloc int[isolatedNodeCapacity]; + var earlyIsolatedNodes = new QuickList(new Buffer(earlyIsolatedNodesMemory, isolatedNodeCapacity)); + for (int i = 0; i < NodeCount && earlyIsolatedNodes.Count < isolatedNodeCapacity; ++i) + { + ref var node = ref Nodes[i]; + ref var a = ref node.A; + ref var b = ref node.B; + if (int.Min(a.LeafCount, b.LeafCount) > leafThresholdForTask) + { + if (BoundingBox.IntersectsUnsafe(a, b)) + { + //Note that this technically does double work on the bounds test with the way we're submitting this as a task. Don't care; it's constant bounded nanoseconds. + earlyIsolatedNodes.AllocateUnsafely() = i; + } + } + else + { + earlyIsolatedNodeIntervalEnd = i; + break; + } + } + + var remainingNodeCount = NodeCount - earlyIsolatedNodeIntervalEnd; + var regularLoopTaskCount = targetTaskBudget - earlyIsolatedNodes.Count; + var nodesPerTaskBase = remainingNodeCount / regularLoopTaskCount; + var remainder = remainingNodeCount - nodesPerTaskBase * regularLoopTaskCount; Span tasks = stackalloc Task[targetTaskBudget]; - int previousEnd = 0; - for (int i = 0; i < tasks.Length; ++i) + int previousEnd = earlyIsolatedNodeIntervalEnd; + for (int i = 0; i < regularLoopTaskCount; ++i) { var taskStart = previousEnd; var nodeCountForTask = i < remainder ? nodesPerTaskBase + 1 : nodesPerTaskBase; var taskEnd = previousEnd + nodeCountForTask; previousEnd = taskEnd; - tasks[i] = new Task(&LoopEntryTaskWithSubtasks3, &context, (long)taskStart | (((long)taskEnd) << 32)); + tasks[i] = new Task(&LoopEntryTask, &context, (long)taskStart | (((long)taskEnd) << 32)); + } + //Stick the early isolated nodes at the end so they're popped first. + for (int i = 0; i < earlyIsolatedNodes.Count; ++i) + { + var taskStart = earlyIsolatedNodes[i]; + tasks[tasks.Length - i - 1] = new Task(&LoopEntryTask, &context, (long)taskStart | (((long)(taskStart + 1)) << 32)); } if (internallyDispatch) { @@ -809,8 +746,5 @@ public unsafe void GetSelfOverlaps2(ref TOverlapHandler results { GetSelfOverlaps2(ref results, workerIndex, taskStack, threadDispatcher, false, threadDispatcher.ThreadCount, targetTaskCount); } - - - // _______________ } } From 5165cfcf0877a828ffc20eedbdd2e13e5947b62e Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 5 Aug 2023 21:17:23 -0500 Subject: [PATCH 775/947] Not a bad idea, also it's busted, also it's probably slower. --- BepuPhysics/Trees/Tree_SelfQueries.cs | 94 ++++++++++++++++++++++++- BepuUtilities/Collections/QuickQueue.cs | 2 +- 2 files changed, 92 insertions(+), 4 deletions(-) diff --git a/BepuPhysics/Trees/Tree_SelfQueries.cs b/BepuPhysics/Trees/Tree_SelfQueries.cs index baf91f86b..94afafc07 100644 --- a/BepuPhysics/Trees/Tree_SelfQueries.cs +++ b/BepuPhysics/Trees/Tree_SelfQueries.cs @@ -719,6 +719,93 @@ private unsafe void GetSelfOverlaps2(ref TOverlapHandler result } + private unsafe void GetSelfOverlaps2Poor(ref TOverlapHandler results, BufferPool pool, + int workerIndex, TaskStack* taskStack, IThreadDispatcher threadDispatcher, bool internallyDispatch, int workerCount, int targetTaskBudget) where TOverlapHandler : unmanaged, IThreadedOverlapHandler + { + targetTaskBudget = int.Min(NodeCount, targetTaskBudget); + if (targetTaskBudget < 0) + targetTaskBudget = threadDispatcher.ThreadCount; + targetTaskBudget *= 8; + + //Crossover tests can generate more overlaps than there are leaves, so the simply dividing the leaf count by the target task count will tend to result in more tasks than necessary. + //BUT... it's not feasible to figure out how many tasks you should actually have ahead of time! The critical thing is that we have *enough* tasks, and that the tasks + //aren't so small that pushing them is a complete waste. + //Don't have to worry about oversubscription, so the potential overhead isn't *too* bad. + var leafThresholdForTask = int.Min(int.Max(LeafCount / (targetTaskBudget * 8), 64), LeafCount); + leafThresholdForTask = 1024; + + var resultsCopy = results; + var context = new SelfTestContext { Tree = this, LoopTaskCount = targetTaskBudget, LeafThresholdForTask = leafThresholdForTask, Results = &resultsCopy, TaskStack = taskStack }; + + //Go ahead and submit very large early nodes as independent tasks to help with load balancing. + + const int maximumIsolatedNodeCapacity = 128; + int isolatedNodeCapacity = int.Min(maximumIsolatedNodeCapacity, targetTaskBudget); + var earlyQueue = new QuickQueue(isolatedNodeCapacity, pool); + var earlyIsolatedNodesMemory = stackalloc int[isolatedNodeCapacity]; + var earlyIsolatedNodes = new QuickList(new Buffer(earlyIsolatedNodesMemory, isolatedNodeCapacity)); + earlyQueue.EnqueueUnsafely() = 0; + while (earlyQueue.Count < earlyQueue.Span.Length && earlyIsolatedNodes.Count < isolatedNodeCapacity) + { + var nodeIndex = earlyQueue.DequeueUnsafely(); + ref var node = ref Nodes[nodeIndex]; + ref var a = ref node.A; + ref var b = ref node.B; + if (int.Max(a.LeafCount, b.LeafCount) >= leafThresholdForTask) + { + if (BoundingBox.IntersectsUnsafe(a, b)) + { + //Note that this technically does double work on the bounds test with the way we're submitting this as a task. Don't care; it's constant bounded nanoseconds. + earlyIsolatedNodes.AllocateUnsafely() = nodeIndex; + } + earlyQueue.EnqueueUnsafely() = a.Index; + earlyQueue.EnqueueUnsafely() = b.Index; + } + } + + var tasks = new QuickList(targetTaskBudget, pool); + var targetTaskSize = NodeCount / targetTaskBudget; + for (int i = 0; i < earlyQueue.Count; ++i) + { + var nodeIndex = earlyQueue[i]; + ref var node = ref Nodes[nodeIndex]; + var regionSize = node.A.LeafCount + node.B.LeafCount - 1; + var localTaskCount = (regionSize + targetTaskSize - 1) / targetTaskSize; + var baseNodesPerTask = regionSize / localTaskCount; + var remainder = regionSize - baseNodesPerTask * localTaskCount; + int previousEnd = nodeIndex; + for (int j = 0; j < localTaskCount; ++j) + { + var taskStart = previousEnd; + var taskSize = j < remainder ? baseNodesPerTask + 1 : baseNodesPerTask; + var taskEnd = previousEnd + taskSize; + tasks.Allocate(pool) = new Task(&LoopEntryTask, &context, (long)taskStart | (((long)taskEnd) << 32)); + } + } + earlyQueue.Dispose(pool); + for (int i = earlyIsolatedNodes.Count - 1; i >= 0; --i) + { + var nodeIndex = earlyIsolatedNodes[i]; + tasks.Allocate(pool) = new Task(&LoopEntryTask, &context, (long)nodeIndex | (((long)(nodeIndex + 1)) << 32)); + } + + if (internallyDispatch) + { + //There isn't an active dispatch, so we need to do it. + taskStack->AllocateContinuationAndPush(tasks, workerIndex, threadDispatcher, onComplete: TaskStack.GetRequestStopTask(taskStack)); + TaskStack.DispatchWorkers(threadDispatcher, taskStack, workerCount); + } + else + { + //We're executing from within a multithreaded dispatch already, so we can simply run the tasks and trust that other threads are ready to steal. + taskStack->RunTasks(tasks, workerIndex, threadDispatcher); + } + tasks.Dispose(pool); + //Have to copy back the results; it's a value type. + results = resultsCopy; + } + + /// /// Reports all bounding box overlaps between leaves in the tree to the given . Uses the thread dispatcher to parallelize overlap testing. /// @@ -728,7 +815,7 @@ private unsafe void GetSelfOverlaps2(ref TOverlapHandler result public unsafe void GetSelfOverlaps2(ref TOverlapHandler results, BufferPool pool, IThreadDispatcher threadDispatcher) where TOverlapHandler : unmanaged, IThreadedOverlapHandler { var taskStack = new TaskStack(pool, threadDispatcher, threadDispatcher.ThreadCount); - GetSelfOverlaps2(ref results, 0, &taskStack, threadDispatcher, true, threadDispatcher.ThreadCount, threadDispatcher.ThreadCount); + GetSelfOverlaps2Poor(ref results, pool, 0, &taskStack, threadDispatcher, true, threadDispatcher.ThreadCount, threadDispatcher.ThreadCount); taskStack.Dispose(pool, threadDispatcher); } @@ -737,14 +824,15 @@ public unsafe void GetSelfOverlaps2(ref TOverlapHandler results /// Pushes tasks into the provided . Does not dispatch threads internally; this is intended to be used as a part of a caller-managed dispatch. /// /// Handler to report results to. + /// Pool used for ephemeral allocations. /// Thread dispatcher used during the overlap test. /// that the overlap test will push tasks onto as needed. /// Index of the worker calling the function. /// Number of tasks the overlap testing should try to create during execution. If negative, uses . - public unsafe void GetSelfOverlaps2(ref TOverlapHandler results, + public unsafe void GetSelfOverlaps2(ref TOverlapHandler results, BufferPool pool, IThreadDispatcher threadDispatcher, TaskStack* taskStack, int workerIndex, int targetTaskCount = -1) where TOverlapHandler : unmanaged, IThreadedOverlapHandler { - GetSelfOverlaps2(ref results, workerIndex, taskStack, threadDispatcher, false, threadDispatcher.ThreadCount, targetTaskCount); + GetSelfOverlaps2Poor(ref results, pool, workerIndex, taskStack, threadDispatcher, false, threadDispatcher.ThreadCount, targetTaskCount); } } } diff --git a/BepuUtilities/Collections/QuickQueue.cs b/BepuUtilities/Collections/QuickQueue.cs index b91a59be8..8f7947291 100644 --- a/BepuUtilities/Collections/QuickQueue.cs +++ b/BepuUtilities/Collections/QuickQueue.cs @@ -91,7 +91,7 @@ static int GetCapacityMask(int spanLength) /// /// Span to use as backing memory to begin with. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public QuickQueue(ref Buffer initialSpan) + public QuickQueue(Buffer initialSpan) { Span = initialSpan; Count = 0; From e4d737f5454ee8f6f3b28b11cce934fc2fe290e6 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 6 Aug 2023 14:39:58 -0500 Subject: [PATCH 776/947] Some oopsyfixes in broad phase. --- BepuPhysics/CollisionDetection/BroadPhase.cs | 16 ++++++++++------ BepuPhysics/Trees/Tree_Refit2.cs | 3 +++ 2 files changed, 13 insertions(+), 6 deletions(-) diff --git a/BepuPhysics/CollisionDetection/BroadPhase.cs b/BepuPhysics/CollisionDetection/BroadPhase.cs index d76dfb621..08b12ae2d 100644 --- a/BepuPhysics/CollisionDetection/BroadPhase.cs +++ b/BepuPhysics/CollisionDetection/BroadPhase.cs @@ -356,7 +356,8 @@ public void Update2(IThreadDispatcher threadDispatcher = null, bool deterministi { ActiveRefinementSchedule(frameIndex, ActiveTree, out var activeRootRefinementSize, out var activeSubtreeRefinementCount, out var activeSubtreeRefinementSize); StaticRefinementSchedule(frameIndex, StaticTree, out var staticRootRefinementSize, out var staticSubtreeRefinementCount, out var staticSubtreeRefinementSize); - if (threadDispatcher != null && threadDispatcher.ThreadCount > 1) + const int minimumLeafCountForThreading = 256; + if (threadDispatcher != null && threadDispatcher.ThreadCount > 1 && (ActiveTree.LeafCount >= minimumLeafCountForThreading || StaticTree.LeafCount >= minimumLeafCountForThreading)) { //Distribute tasks for refinement roughly in proportion to their cost. //This doesn't need to be perfect. @@ -378,7 +379,7 @@ public void Update2(IThreadDispatcher threadDispatcher = null, bool deterministi SubtreeRefinementSize = activeSubtreeRefinementSize, SubtreeRefinementStartIndex = activeSubtreeRefinementStartIndex, Deterministic = deterministic, - TargetNodes = new Buffer(ActiveTree.Nodes.Length, Pool), + TargetNodes = ActiveTree.LeafCount > 2 ? new Buffer(ActiveTree.Nodes.Length, Pool) : default, }; var staticRefineContext = new RefinementContext { @@ -398,10 +399,13 @@ public void Update2(IThreadDispatcher threadDispatcher = null, bool deterministi taskStack.AllocateContinuationAndPush(tasks, 0, threadDispatcher, onComplete: TaskStack.GetRequestStopTask(&taskStack)); TaskStack.DispatchWorkers(threadDispatcher, &taskStack); taskStack.Dispose(Pool, threadDispatcher); - //When using the cache optimizing refit, the tree is modified. Since passed a copy, we need to copy it back. - //Static tree doesn't undergo a refit, so no copy required there. - ActiveTree.Nodes.Dispose(Pool); - ActiveTree.Nodes = activeRefineContext.TargetNodes; + if (ActiveTree.LeafCount > 2) //If no refit was needed, then the target nodes buffer was never allocated. + { + //When using the cache optimizing refit, the tree is modified. Since passed a copy, we need to copy it back. + //Static tree doesn't undergo a refit, so no copy required there. + ActiveTree.Nodes.Dispose(Pool); + ActiveTree.Nodes = activeRefineContext.TargetNodes; + } //The start indices need to be copied back for both. activeSubtreeRefinementStartIndex = activeRefineContext.SubtreeRefinementStartIndex; staticSubtreeRefinementStartIndex = staticRefineContext.SubtreeRefinementStartIndex; diff --git a/BepuPhysics/Trees/Tree_Refit2.cs b/BepuPhysics/Trees/Tree_Refit2.cs index 68f7c26fc..aafe41d5f 100644 --- a/BepuPhysics/Trees/Tree_Refit2.cs +++ b/BepuPhysics/Trees/Tree_Refit2.cs @@ -225,6 +225,9 @@ public unsafe void Refit2WithCacheOptimization(Buffer sourceNodes) /// Whether to dispose of the original nodes buffer. If false, it's up to the caller to dispose of it appropriately. public unsafe void Refit2WithCacheOptimization(BufferPool pool, bool disposeOriginalNodes = true) { + //No point in refitting a tree with no internal nodes! + if (LeafCount <= 2) + return; var oldNodes = Nodes; Nodes = new Buffer(oldNodes.Length, pool); Refit2WithCacheOptimization(oldNodes); From 7b074eb8c52f1c6997a1daab6b7f8583441ef52e Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 6 Aug 2023 17:15:51 -0500 Subject: [PATCH 777/947] Another convenience overload. --- BepuUtilities/TaskScheduling/TaskStack.cs | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/BepuUtilities/TaskScheduling/TaskStack.cs b/BepuUtilities/TaskScheduling/TaskStack.cs index 73eee1c2f..35e7cf661 100644 --- a/BepuUtilities/TaskScheduling/TaskStack.cs +++ b/BepuUtilities/TaskScheduling/TaskStack.cs @@ -264,6 +264,19 @@ public void Push(Span tasks, int workerIndex, IThreadDispatcher dispatcher } } + /// + /// Pushes a task onto the task stack. + /// + /// Task composing the job. + /// Thread dispatcher to allocate thread data from if necessary. + /// Index of the worker stack to push the task onto. + /// User-defined tag data for the submitted job. + /// True if the push succeeded, false if it was contested. + public void Push(Task task, int workerIndex, IThreadDispatcher dispatcher, ulong tag = 0) + { + Push(new Span(ref task), workerIndex, dispatcher, tag); + } + /// /// Pushes a set of tasks to the stack with a created continuation. /// From 933bda7ea6e5c66f67ad0502b0c9618df0bf656d Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 6 Aug 2023 17:20:48 -0500 Subject: [PATCH 778/947] Additional oopsy. --- BepuPhysics/Trees/Tree_Refit2.cs | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/BepuPhysics/Trees/Tree_Refit2.cs b/BepuPhysics/Trees/Tree_Refit2.cs index aafe41d5f..da147110d 100644 --- a/BepuPhysics/Trees/Tree_Refit2.cs +++ b/BepuPhysics/Trees/Tree_Refit2.cs @@ -353,6 +353,8 @@ unsafe void Refit2WithCacheOptimization(BufferPool pool, IThreadDispatcher dispa /// Whether to dispose of the original nodes buffer. If false, it's up to the caller to dispose of it appropriately. public unsafe void Refit2WithCacheOptimization(BufferPool pool, IThreadDispatcher dispatcher, bool disposeOriginalNodes = true) { + if (LeafCount <= 2) + return; var taskStack = new TaskStack(pool, dispatcher, dispatcher.ThreadCount); var oldNodes = Nodes; Nodes = new Buffer(oldNodes.Length, pool); @@ -375,6 +377,8 @@ public unsafe void Refit2WithCacheOptimization(BufferPool pool, IThreadDispatche /// Whether to dispose of the original nodes buffer. If false, it's up to the caller to dispose of it appropriately. public unsafe void Refit2WithCacheOptimization(BufferPool pool, IThreadDispatcher dispatcher, TaskStack* taskStack, int workerIndex, int targetTaskCount = -1, bool disposeOriginalNodes = true) { + if (LeafCount <= 2) + return; var oldNodes = Nodes; Nodes = new Buffer(oldNodes.Length, pool); Refit2WithCacheOptimization(pool, dispatcher, taskStack, workerIndex, targetTaskCount, internallyDispatch: false, oldNodes); From 7cc02d7b951ac6fffa64f785c258ff58def9b6ca Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 6 Aug 2023 19:30:01 -0500 Subject: [PATCH 779/947] MT intertree. --- BepuPhysics/Trees/Tree_IntertreeQueries.cs | 32 +-- BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs | 196 ++++++++++++++++++ BepuPhysics/Trees/Tree_SelfQueries.cs | 24 +-- 3 files changed, 227 insertions(+), 25 deletions(-) create mode 100644 BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs diff --git a/BepuPhysics/Trees/Tree_IntertreeQueries.cs b/BepuPhysics/Trees/Tree_IntertreeQueries.cs index 8bb88157e..adde7544b 100644 --- a/BepuPhysics/Trees/Tree_IntertreeQueries.cs +++ b/BepuPhysics/Trees/Tree_IntertreeQueries.cs @@ -8,7 +8,6 @@ namespace BepuPhysics.Trees { partial struct Tree { - //TODO: No good reason for recursion. This is holdovers from the prototype. unsafe void DispatchTestForNodeAgainstLeaf(int leafIndex, ref NodeChild leafChild, int nodeIndex, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler { if (nodeIndex < 0) @@ -132,17 +131,11 @@ private unsafe void GetOverlapsBetweenDifferentNodes(ref Node a } } - - public unsafe void GetOverlaps(ref Tree treeB, ref TOverlapHandler overlapHandler) where TOverlapHandler : struct, IOverlapHandler + bool TryGetOverlapsForDegenerateTrees(ref Tree treeB, ref TOverlapHandler overlapHandler) where TOverlapHandler : struct, IOverlapHandler { if (LeafCount == 0 || treeB.LeafCount == 0) - return; - if (LeafCount >= 2 && treeB.LeafCount >= 2) - { - //Both trees have complete nodes; we can use a general case. - GetOverlapsBetweenDifferentNodes(ref Nodes[0], ref treeB.Nodes[0], ref treeB, ref overlapHandler); - } - else if (LeafCount == 1 && treeB.LeafCount >= 2) + return true; + if (LeafCount == 1 && treeB.LeafCount >= 2) { //Tree A is degenerate; needs a special case. ref var a = ref Nodes[0]; @@ -157,8 +150,9 @@ public unsafe void GetOverlaps(ref Tree treeB, ref TOverlapHand { DispatchTestForNodes(ref a.A, ref b.B, ref treeB, ref overlapHandler); } + return true; } - else if (LeafCount >= 2 && treeB.LeafCount == 1) + if (LeafCount >= 2 && treeB.LeafCount == 1) { //Tree B is degenerate; needs a special case. ref var a = ref Nodes[0]; @@ -173,15 +167,27 @@ public unsafe void GetOverlaps(ref Tree treeB, ref TOverlapHand { DispatchTestForNodes(ref a.B, ref b.A, ref treeB, ref overlapHandler); } + return true; } - else + if (LeafCount == 1 && treeB.LeafCount == 1) { - Debug.Assert(LeafCount == 1 && treeB.LeafCount == 1); + //Both degenerate. if (BoundingBox.IntersectsUnsafe(Nodes[0].A, treeB.Nodes[0].A)) { DispatchTestForNodes(ref Nodes[0].A, ref treeB.Nodes[0].A, ref treeB, ref overlapHandler); } } + return false; + } + + + public unsafe void GetOverlaps(ref Tree treeB, ref TOverlapHandler overlapHandler) where TOverlapHandler : struct, IOverlapHandler + { + if (!TryGetOverlapsForDegenerateTrees(ref treeB, ref overlapHandler)) + { + //Both trees have complete nodes; we can use a general case. + GetOverlapsBetweenDifferentNodes(ref Nodes[0], ref treeB.Nodes[0], ref treeB, ref overlapHandler); + } } } diff --git a/BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs b/BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs new file mode 100644 index 000000000..f41a697f7 --- /dev/null +++ b/BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs @@ -0,0 +1,196 @@ +using BepuUtilities; +using BepuUtilities.Collections; +using BepuUtilities.Memory; +using BepuUtilities.TaskScheduling; +using System; +using System.Diagnostics; +using System.Linq; +using System.Numerics; +using System.Reflection.Metadata; +using System.Runtime.CompilerServices; +using System.Threading; + +namespace BepuPhysics.Trees; + +partial struct Tree +{ + private unsafe void GetOverlapsBetweenDifferentNodes2(ref Node a, ref Node b, ref Tree treeB, ref IntertreeContext context, int workerIndex, ThreadDispatcher dispatcher) + where TOverlapHandler : unmanaged, IThreadedOverlapHandler + { + + } + + unsafe struct IntertreeContext where TOverlapHandler : unmanaged, IThreadedOverlapHandler + { + public Tree TreeA; + public Tree TreeB; + public TaskStack* Stack; + public int LeafThreshold; + public TOverlapHandler* Results; + } + + + public static int TasksSpawned; + static unsafe void IntertreeTask(long encodedIndices, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) where TOverlapHandler : unmanaged, IThreadedOverlapHandler + { + var indexA = (int)encodedIndices; + var indexB = (int)(encodedIndices >> 32); + ref var context = ref *(IntertreeContext*)untypedContext; + if (indexA >= 0 && indexB >= 0) + { + //Both nodes. + ref var a = ref context.TreeA.Nodes[indexA]; + ref var b = ref context.TreeB.Nodes[indexB]; + ref var aa = ref a.A; + ref var ab = ref a.B; + ref var ba = ref b.A; + ref var bb = ref b.B; + var aaIntersects = BoundingBox.IntersectsUnsafe(aa, ba); + var abIntersects = BoundingBox.IntersectsUnsafe(aa, bb); + var baIntersects = BoundingBox.IntersectsUnsafe(ab, ba); + var bbIntersects = BoundingBox.IntersectsUnsafe(ab, bb); + + //Push all pushable work *before* doing any work to ensure that other threads have something to grab. + var pushAA = aaIntersects && int.Max(aa.LeafCount, ba.LeafCount) >= context.LeafThreshold; + var pushAB = abIntersects && int.Max(aa.LeafCount, bb.LeafCount) >= context.LeafThreshold; + var pushBA = baIntersects && int.Max(ab.LeafCount, ba.LeafCount) >= context.LeafThreshold; + var pushBB = bbIntersects && int.Max(ab.LeafCount, bb.LeafCount) >= context.LeafThreshold; + var pushCount = (pushAA ? 1 : 0) + (pushAB ? 1 : 0) + (pushBA ? 1 : 0) + (pushBB ? 1 : 0); + ContinuationHandle handle = default; + if (pushCount > 0) + { + Span tasks = stackalloc Task[pushCount]; + pushCount = 0; + if (pushAA) tasks[pushCount++] = new Task(&IntertreeTask, untypedContext, ((uint)a.A.Index | ((long)b.A.Index << 32))); + if (pushAB) tasks[pushCount++] = new Task(&IntertreeTask, untypedContext, ((uint)a.A.Index | ((long)b.B.Index << 32))); + if (pushBA) tasks[pushCount++] = new Task(&IntertreeTask, untypedContext, ((uint)a.B.Index | ((long)b.A.Index << 32))); + if (pushBB) tasks[pushCount++] = new Task(&IntertreeTask, untypedContext, ((uint)a.B.Index | ((long)b.B.Index << 32))); + handle = context.Stack->AllocateContinuationAndPush(tasks, workerIndex, dispatcher); + //Interlocked.Add(ref TasksSpawned, pushCount); + } + + var wrapped = new WrappedOverlapHandler { Inner = context.Results, WorkerIndex = workerIndex }; + if (aaIntersects && !pushAA) + { + context.TreeA.DispatchTestForNodes(ref aa, ref ba, ref context.TreeB, ref wrapped); + } + if (abIntersects && !pushAB) + { + context.TreeA.DispatchTestForNodes(ref aa, ref bb, ref context.TreeB, ref wrapped); + } + if (baIntersects && !pushBA) + { + context.TreeA.DispatchTestForNodes(ref ab, ref ba, ref context.TreeB, ref wrapped); + } + if (bbIntersects && !pushBB) + { + context.TreeA.DispatchTestForNodes(ref ab, ref bb, ref context.TreeB, ref wrapped); + } + if (pushCount > 0) + { + context.Stack->WaitForCompletion(handle, workerIndex, dispatcher); + } + } + else + { + //One of the two indices points at a leaf. + //In reasonably balanced circumstances, there's hardly any reason to bother spawning tasks for node-leaf, but we may be in a pathological case + //where one tree is much smaller in *leaf count*, but not in size, and so the node might actually have a ton of leaves beneath it. + Debug.Assert(indexA >= 0 || indexB >= 0, "A task should not have been spawned for a leaf-leaf test. Leaf threshold is likely messed up."); + var nodeBelongsToTreeA = indexA >= 0; + ref var node = ref nodeBelongsToTreeA ? ref context.TreeA.Nodes[indexA] : ref context.TreeB.Nodes[indexB]; + //Go looking for the leaf child by using the leaf buffer indirection since we didn't store the bounding box anywhere else. + //This hurts a bit- a whole extra bit of memory being touched- but this codepath *should* be pretty rare as a fraction of test time; the whole point is for it to hit a sequential path quickly. + ref var leafTree = ref nodeBelongsToTreeA ? ref context.TreeB : ref context.TreeA; + var leafIndex = nodeBelongsToTreeA ? Encode(indexB) : Encode(indexA); + var leaf = leafTree.Leaves[leafIndex]; + ref var leafChild = ref Unsafe.Add(ref leafTree.Nodes[leaf.NodeIndex].A, leaf.ChildIndex); + var aIntersects = BoundingBox.IntersectsUnsafe(leafChild, node.A); + var bIntersects = BoundingBox.IntersectsUnsafe(leafChild, node.B); + var pushA = aIntersects && node.A.LeafCount >= context.LeafThreshold; + var pushB = bIntersects && node.B.LeafCount >= context.LeafThreshold; + var pushCount = (pushA ? 1 : 0) + (pushB ? 1 : 0); + ContinuationHandle handle = default; + if (pushCount > 0) + { + Span tasks = stackalloc Task[pushCount]; + if (pushA) tasks[pushCount++] = new Task(&IntertreeTask, untypedContext, (uint)(nodeBelongsToTreeA ? node.A.Index : indexA) | ((long)(nodeBelongsToTreeA ? indexB : node.A.Index) << 32)); + if (pushB) tasks[pushCount++] = new Task(&IntertreeTask, untypedContext, (uint)(nodeBelongsToTreeA ? node.B.Index : indexA) | ((long)(nodeBelongsToTreeA ? indexB : node.B.Index) << 32)); + handle = context.Stack->AllocateContinuationAndPush(tasks, workerIndex, dispatcher); + } + var wrapped = new WrappedOverlapHandler { Inner = context.Results, WorkerIndex = workerIndex }; + if (aIntersects && !pushA) + { + context.TreeA.DispatchTestForNodes(ref nodeBelongsToTreeA ? ref node.A : ref leafChild, ref nodeBelongsToTreeA ? ref leafChild : ref node.A, ref context.TreeB, ref wrapped); + } + if (bIntersects && !pushB) + { + context.TreeA.DispatchTestForNodes(ref nodeBelongsToTreeA ? ref node.B : ref leafChild, ref nodeBelongsToTreeA ? ref leafChild : ref node.B, ref context.TreeB, ref wrapped); + } + if (pushCount > 0) + { + context.Stack->WaitForCompletion(handle, workerIndex, dispatcher); + } + } + + } + + public unsafe void GetOverlaps2(ref Tree treeB, ref TOverlapHandler results, + BufferPool pool, int workerIndex, TaskStack* taskStack, IThreadDispatcher threadDispatcher, bool internallyDispatch, int workerCount, int targetTaskBudget) where TOverlapHandler : unmanaged, IThreadedOverlapHandler + { + var resultsCopy = results; + var wrapped = new WrappedOverlapHandler { Inner = &resultsCopy, WorkerIndex = 0 }; + if (!TryGetOverlapsForDegenerateTrees(ref treeB, ref wrapped)) + { + //Both trees have complete nodes; we can use a general case. + const int minimumLeafThreshold = 256; + if (LeafCount < minimumLeafThreshold && treeB.LeafCount < minimumLeafThreshold) + { + //No point in spawning a bunch of tasks for tiny trees. + GetOverlapsBetweenDifferentNodes(ref Nodes[0], ref treeB.Nodes[0], ref treeB, ref wrapped); + } + else + { + if (targetTaskBudget < 0) + targetTaskBudget = workerCount; + var leafThreshold = int.Max(minimumLeafThreshold, (LeafCount + treeB.LeafCount) / (8 * targetTaskBudget)); + var context = new IntertreeContext + { + TreeA = this, + TreeB = treeB, + Stack = taskStack, + LeafThreshold = leafThreshold, + Results = &resultsCopy + }; + TasksSpawned = 0; + var rootTask = new Task(&IntertreeTask, &context, 0); + if (internallyDispatch) + { + taskStack->AllocateContinuationAndPush(rootTask, workerIndex, threadDispatcher, onComplete: TaskStack.GetRequestStopTask(taskStack)); + TaskStack.DispatchWorkers(threadDispatcher, taskStack, int.Min(threadDispatcher.ThreadCount, targetTaskBudget)); + } + else + { + taskStack->Push(rootTask, workerIndex, threadDispatcher); + } + //Console.WriteLine($"Tasks: {TasksSpawned}"); + } + } + //Copy back potential changes. + results = resultsCopy; + } + + public unsafe void GetOverlaps2(ref Tree treeB, ref TOverlapHandler results, BufferPool pool, IThreadDispatcher dispatcher) + where TOverlapHandler : unmanaged, IThreadedOverlapHandler + { + var taskStack = new TaskStack(pool, dispatcher, dispatcher.ThreadCount); + GetOverlaps2(ref treeB, ref results, pool, 0, &taskStack, dispatcher, true, dispatcher.ThreadCount, -1); + taskStack.Dispose(pool, dispatcher); + } + + public unsafe void GetOverlaps2(ref Tree treeB, ref TOverlapHandler results, BufferPool pool, IThreadDispatcher dispatcher, TaskStack* taskStack, int workerIndex, int targetTaskCount = -1) + where TOverlapHandler : unmanaged, IThreadedOverlapHandler + { + GetOverlaps2(ref treeB, ref results, pool, workerIndex, taskStack, dispatcher, false, dispatcher.ThreadCount, targetTaskCount); + } +} diff --git a/BepuPhysics/Trees/Tree_SelfQueries.cs b/BepuPhysics/Trees/Tree_SelfQueries.cs index 94afafc07..3f32fe575 100644 --- a/BepuPhysics/Trees/Tree_SelfQueries.cs +++ b/BepuPhysics/Trees/Tree_SelfQueries.cs @@ -555,19 +555,19 @@ unsafe static void CrossoverWithSubtasksTask(long encodedNodeIn continuationCount = 0; if (aaPushTask) { - continuations[continuationCount++] = context.TaskStack->AllocateContinuationAndPush(new Task(&CrossoverWithSubtasksTask, untypedContext, (long)a.A.Index | ((long)b.A.Index << 32)), workerIndex, dispatcher); + continuations[continuationCount++] = context.TaskStack->AllocateContinuationAndPush(new Task(&CrossoverWithSubtasksTask, untypedContext, (uint)a.A.Index | ((long)b.A.Index << 32)), workerIndex, dispatcher); } if (abPushTask) { - continuations[continuationCount++] = context.TaskStack->AllocateContinuationAndPush(new Task(&CrossoverWithSubtasksTask, untypedContext, (long)a.A.Index | ((long)b.B.Index << 32)), workerIndex, dispatcher); + continuations[continuationCount++] = context.TaskStack->AllocateContinuationAndPush(new Task(&CrossoverWithSubtasksTask, untypedContext, (uint)a.A.Index | ((long)b.B.Index << 32)), workerIndex, dispatcher); } if (baPushTask) { - continuations[continuationCount++] = context.TaskStack->AllocateContinuationAndPush(new Task(&CrossoverWithSubtasksTask, untypedContext, (long)a.B.Index | ((long)b.A.Index << 32)), workerIndex, dispatcher); + continuations[continuationCount++] = context.TaskStack->AllocateContinuationAndPush(new Task(&CrossoverWithSubtasksTask, untypedContext, (uint)a.B.Index | ((long)b.A.Index << 32)), workerIndex, dispatcher); } if (bbPushTask) { - continuations[continuationCount++] = context.TaskStack->AllocateContinuationAndPush(new Task(&CrossoverWithSubtasksTask, untypedContext, (long)a.B.Index | ((long)b.B.Index << 32)), workerIndex, dispatcher); + continuations[continuationCount++] = context.TaskStack->AllocateContinuationAndPush(new Task(&CrossoverWithSubtasksTask, untypedContext, (uint)a.B.Index | ((long)b.B.Index << 32)), workerIndex, dispatcher); } //We've pushed every crossover worthy of further splitting onto the stack. The moment any thread as an opening, it'll be investigated. //For anything *not* pushed to a task, we can just run the test directly. @@ -617,7 +617,7 @@ unsafe static void LoopEntryTaskWithSubtasks3(long taskStartAnd if (int.Min(a.LeafCount, b.LeafCount) >= context.LeafThresholdForTask) { //The number of potential overlaps is high; push this pair to a subtask. - subtaskContinuations.Allocate(pool) = context.TaskStack->AllocateContinuationAndPush(new Task(&CrossoverWithSubtasksTask, untypedContext, (long)a.Index | ((long)b.Index << 32)), workerIndex, dispatcher); + subtaskContinuations.Allocate(pool) = context.TaskStack->AllocateContinuationAndPush(new Task(&CrossoverWithSubtasksTask, untypedContext, (uint)a.Index | ((long)b.Index << 32)), workerIndex, dispatcher); } else { @@ -641,10 +641,10 @@ unsafe static void LoopEntryTaskWithSubtasks3(long taskStartAnd private unsafe void GetSelfOverlaps2(ref TOverlapHandler results, int workerIndex, TaskStack* taskStack, IThreadDispatcher threadDispatcher, bool internallyDispatch, int workerCount, int targetTaskBudget) where TOverlapHandler : unmanaged, IThreadedOverlapHandler { - targetTaskBudget = int.Min(NodeCount, targetTaskBudget); if (targetTaskBudget < 0) targetTaskBudget = threadDispatcher.ThreadCount; targetTaskBudget *= 8; + targetTaskBudget = int.Min(NodeCount, targetTaskBudget); //Crossover tests can generate more overlaps than there are leaves, so the simply dividing the leaf count by the target task count will tend to result in more tasks than necessary. //BUT... it's not feasible to figure out how many tasks you should actually have ahead of time! The critical thing is that we have *enough* tasks, and that the tasks @@ -695,13 +695,13 @@ private unsafe void GetSelfOverlaps2(ref TOverlapHandler result var nodeCountForTask = i < remainder ? nodesPerTaskBase + 1 : nodesPerTaskBase; var taskEnd = previousEnd + nodeCountForTask; previousEnd = taskEnd; - tasks[i] = new Task(&LoopEntryTask, &context, (long)taskStart | (((long)taskEnd) << 32)); + tasks[i] = new Task(&LoopEntryTask, &context, (uint)taskStart | (((long)taskEnd) << 32)); } //Stick the early isolated nodes at the end so they're popped first. for (int i = 0; i < earlyIsolatedNodes.Count; ++i) { var taskStart = earlyIsolatedNodes[i]; - tasks[tasks.Length - i - 1] = new Task(&LoopEntryTask, &context, (long)taskStart | (((long)(taskStart + 1)) << 32)); + tasks[tasks.Length - i - 1] = new Task(&LoopEntryTask, &context, (uint)taskStart | (((long)(taskStart + 1)) << 32)); } if (internallyDispatch) { @@ -779,14 +779,14 @@ private unsafe void GetSelfOverlaps2Poor(ref TOverlapHandler re var taskStart = previousEnd; var taskSize = j < remainder ? baseNodesPerTask + 1 : baseNodesPerTask; var taskEnd = previousEnd + taskSize; - tasks.Allocate(pool) = new Task(&LoopEntryTask, &context, (long)taskStart | (((long)taskEnd) << 32)); + tasks.Allocate(pool) = new Task(&LoopEntryTask, &context, (uint)taskStart | (((long)taskEnd) << 32)); } } earlyQueue.Dispose(pool); for (int i = earlyIsolatedNodes.Count - 1; i >= 0; --i) { var nodeIndex = earlyIsolatedNodes[i]; - tasks.Allocate(pool) = new Task(&LoopEntryTask, &context, (long)nodeIndex | (((long)(nodeIndex + 1)) << 32)); + tasks.Allocate(pool) = new Task(&LoopEntryTask, &context, (uint)nodeIndex | (((long)(nodeIndex + 1)) << 32)); } if (internallyDispatch) @@ -815,7 +815,7 @@ private unsafe void GetSelfOverlaps2Poor(ref TOverlapHandler re public unsafe void GetSelfOverlaps2(ref TOverlapHandler results, BufferPool pool, IThreadDispatcher threadDispatcher) where TOverlapHandler : unmanaged, IThreadedOverlapHandler { var taskStack = new TaskStack(pool, threadDispatcher, threadDispatcher.ThreadCount); - GetSelfOverlaps2Poor(ref results, pool, 0, &taskStack, threadDispatcher, true, threadDispatcher.ThreadCount, threadDispatcher.ThreadCount); + GetSelfOverlaps2(ref results, 0, &taskStack, threadDispatcher, true, threadDispatcher.ThreadCount, threadDispatcher.ThreadCount); taskStack.Dispose(pool, threadDispatcher); } @@ -832,7 +832,7 @@ public unsafe void GetSelfOverlaps2(ref TOverlapHandler results public unsafe void GetSelfOverlaps2(ref TOverlapHandler results, BufferPool pool, IThreadDispatcher threadDispatcher, TaskStack* taskStack, int workerIndex, int targetTaskCount = -1) where TOverlapHandler : unmanaged, IThreadedOverlapHandler { - GetSelfOverlaps2Poor(ref results, pool, workerIndex, taskStack, threadDispatcher, false, threadDispatcher.ThreadCount, targetTaskCount); + GetSelfOverlaps2(ref results, workerIndex, taskStack, threadDispatcher, false, threadDispatcher.ThreadCount, targetTaskCount); } } } From 8e4bc86d9fa109fa9221e9f0b45c662ed67229bb Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 6 Aug 2023 19:39:55 -0500 Subject: [PATCH 780/947] Pushcount reset properly. --- BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs | 1 + 1 file changed, 1 insertion(+) diff --git a/BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs b/BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs index f41a697f7..96f5f51a3 100644 --- a/BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs +++ b/BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs @@ -114,6 +114,7 @@ static unsafe void IntertreeTask(long encodedIndices, void* unt if (pushCount > 0) { Span tasks = stackalloc Task[pushCount]; + pushCount = 0; if (pushA) tasks[pushCount++] = new Task(&IntertreeTask, untypedContext, (uint)(nodeBelongsToTreeA ? node.A.Index : indexA) | ((long)(nodeBelongsToTreeA ? indexB : node.A.Index) << 32)); if (pushB) tasks[pushCount++] = new Task(&IntertreeTask, untypedContext, (uint)(nodeBelongsToTreeA ? node.B.Index : indexA) | ((long)(nodeBelongsToTreeA ? indexB : node.B.Index) << 32)); handle = context.Stack->AllocateContinuationAndPush(tasks, workerIndex, dispatcher); From 58902ef60ee6476f90320a8651164675574e31ec Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 6 Aug 2023 20:28:34 -0500 Subject: [PATCH 781/947] Some degenerate case fixes. --- BepuPhysics/Trees/Tree_IntertreeQueries.cs | 21 ++---- BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs | 68 ++++++++----------- 2 files changed, 37 insertions(+), 52 deletions(-) diff --git a/BepuPhysics/Trees/Tree_IntertreeQueries.cs b/BepuPhysics/Trees/Tree_IntertreeQueries.cs index adde7544b..075ebaf7d 100644 --- a/BepuPhysics/Trees/Tree_IntertreeQueries.cs +++ b/BepuPhysics/Trees/Tree_IntertreeQueries.cs @@ -131,10 +131,10 @@ private unsafe void GetOverlapsBetweenDifferentNodes(ref Node a } } - bool TryGetOverlapsForDegenerateTrees(ref Tree treeB, ref TOverlapHandler overlapHandler) where TOverlapHandler : struct, IOverlapHandler + public unsafe void GetOverlaps(ref Tree treeB, ref TOverlapHandler overlapHandler) where TOverlapHandler : struct, IOverlapHandler { if (LeafCount == 0 || treeB.LeafCount == 0) - return true; + return; if (LeafCount == 1 && treeB.LeafCount >= 2) { //Tree A is degenerate; needs a special case. @@ -150,7 +150,7 @@ bool TryGetOverlapsForDegenerateTrees(ref Tree treeB, ref TOver { DispatchTestForNodes(ref a.A, ref b.B, ref treeB, ref overlapHandler); } - return true; + return; } if (LeafCount >= 2 && treeB.LeafCount == 1) { @@ -167,7 +167,7 @@ bool TryGetOverlapsForDegenerateTrees(ref Tree treeB, ref TOver { DispatchTestForNodes(ref a.B, ref b.A, ref treeB, ref overlapHandler); } - return true; + return; } if (LeafCount == 1 && treeB.LeafCount == 1) { @@ -176,18 +176,11 @@ bool TryGetOverlapsForDegenerateTrees(ref Tree treeB, ref TOver { DispatchTestForNodes(ref Nodes[0].A, ref treeB.Nodes[0].A, ref treeB, ref overlapHandler); } + return; } - return false; - } + //Both trees have complete nodes; we can use a general case. + GetOverlapsBetweenDifferentNodes(ref Nodes[0], ref treeB.Nodes[0], ref treeB, ref overlapHandler); - - public unsafe void GetOverlaps(ref Tree treeB, ref TOverlapHandler overlapHandler) where TOverlapHandler : struct, IOverlapHandler - { - if (!TryGetOverlapsForDegenerateTrees(ref treeB, ref overlapHandler)) - { - //Both trees have complete nodes; we can use a general case. - GetOverlapsBetweenDifferentNodes(ref Nodes[0], ref treeB.Nodes[0], ref treeB, ref overlapHandler); - } } } diff --git a/BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs b/BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs index 96f5f51a3..96b840b08 100644 --- a/BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs +++ b/BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs @@ -14,12 +14,6 @@ namespace BepuPhysics.Trees; partial struct Tree { - private unsafe void GetOverlapsBetweenDifferentNodes2(ref Node a, ref Node b, ref Tree treeB, ref IntertreeContext context, int workerIndex, ThreadDispatcher dispatcher) - where TOverlapHandler : unmanaged, IThreadedOverlapHandler - { - - } - unsafe struct IntertreeContext where TOverlapHandler : unmanaged, IThreadedOverlapHandler { public Tree TreeA; @@ -29,8 +23,6 @@ unsafe struct IntertreeContext where TOverlapHandler : unmanage public TOverlapHandler* Results; } - - public static int TasksSpawned; static unsafe void IntertreeTask(long encodedIndices, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) where TOverlapHandler : unmanaged, IThreadedOverlapHandler { var indexA = (int)encodedIndices; @@ -139,42 +131,42 @@ static unsafe void IntertreeTask(long encodedIndices, void* unt public unsafe void GetOverlaps2(ref Tree treeB, ref TOverlapHandler results, BufferPool pool, int workerIndex, TaskStack* taskStack, IThreadDispatcher threadDispatcher, bool internallyDispatch, int workerCount, int targetTaskBudget) where TOverlapHandler : unmanaged, IThreadedOverlapHandler { + if (LeafCount == 0 || treeB.LeafCount == 0) + return; var resultsCopy = results; - var wrapped = new WrappedOverlapHandler { Inner = &resultsCopy, WorkerIndex = 0 }; - if (!TryGetOverlapsForDegenerateTrees(ref treeB, ref wrapped)) + //Both trees have complete nodes; we can use a general case. + const int minimumLeafThreshold = 256; + if (LeafCount < minimumLeafThreshold && treeB.LeafCount < minimumLeafThreshold) { - //Both trees have complete nodes; we can use a general case. - const int minimumLeafThreshold = 256; - if (LeafCount < minimumLeafThreshold && treeB.LeafCount < minimumLeafThreshold) + //No point in spawning a bunch of tasks for tiny trees. + var wrapped = new WrappedOverlapHandler { Inner = &resultsCopy, WorkerIndex = 0 }; + GetOverlaps(ref treeB, ref wrapped); + } + else + { + if (targetTaskBudget < 0) + targetTaskBudget = workerCount; + var leafThreshold = int.Max(minimumLeafThreshold, (LeafCount + treeB.LeafCount) / (8 * targetTaskBudget)); + var context = new IntertreeContext + { + TreeA = this, + TreeB = treeB, + Stack = taskStack, + LeafThreshold = leafThreshold, + Results = &resultsCopy + }; + //One of the trees *may* be a single leaf. Don't want the task to have to deal with partial nodes, so go ahead and spawn a task for the leaf child in that case. Otherwise, just point at the root. + var childA = LeafCount == 1 ? Nodes[0].A.Index : 0; + var childB = treeB.LeafCount == 1 ? treeB.Nodes[0].A.Index : 0; + var rootTask = new Task(&IntertreeTask, &context, (uint)childA | ((long)childB << 32)); + if (internallyDispatch) { - //No point in spawning a bunch of tasks for tiny trees. - GetOverlapsBetweenDifferentNodes(ref Nodes[0], ref treeB.Nodes[0], ref treeB, ref wrapped); + taskStack->AllocateContinuationAndPush(rootTask, workerIndex, threadDispatcher, onComplete: TaskStack.GetRequestStopTask(taskStack)); + TaskStack.DispatchWorkers(threadDispatcher, taskStack, int.Min(threadDispatcher.ThreadCount, targetTaskBudget)); } else { - if (targetTaskBudget < 0) - targetTaskBudget = workerCount; - var leafThreshold = int.Max(minimumLeafThreshold, (LeafCount + treeB.LeafCount) / (8 * targetTaskBudget)); - var context = new IntertreeContext - { - TreeA = this, - TreeB = treeB, - Stack = taskStack, - LeafThreshold = leafThreshold, - Results = &resultsCopy - }; - TasksSpawned = 0; - var rootTask = new Task(&IntertreeTask, &context, 0); - if (internallyDispatch) - { - taskStack->AllocateContinuationAndPush(rootTask, workerIndex, threadDispatcher, onComplete: TaskStack.GetRequestStopTask(taskStack)); - TaskStack.DispatchWorkers(threadDispatcher, taskStack, int.Min(threadDispatcher.ThreadCount, targetTaskBudget)); - } - else - { - taskStack->Push(rootTask, workerIndex, threadDispatcher); - } - //Console.WriteLine($"Tasks: {TasksSpawned}"); + taskStack->Push(rootTask, workerIndex, threadDispatcher); } } //Copy back potential changes. From d0deee50e6c755049530fc6847556e2533591564 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 8 Aug 2023 14:47:32 -0500 Subject: [PATCH 782/947] Another self variant; still worse than the dumb version. --- BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs | 1 - BepuPhysics/Trees/Tree_SelfQueries.cs | 74 ++------ BepuPhysics/Trees/Tree_SelfQueriesMT2.cs | 166 ++++++++++++++++++ 3 files changed, 183 insertions(+), 58 deletions(-) create mode 100644 BepuPhysics/Trees/Tree_SelfQueriesMT2.cs diff --git a/BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs b/BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs index 96b840b08..3fe01c171 100644 --- a/BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs +++ b/BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs @@ -58,7 +58,6 @@ static unsafe void IntertreeTask(long encodedIndices, void* unt if (pushBA) tasks[pushCount++] = new Task(&IntertreeTask, untypedContext, ((uint)a.B.Index | ((long)b.A.Index << 32))); if (pushBB) tasks[pushCount++] = new Task(&IntertreeTask, untypedContext, ((uint)a.B.Index | ((long)b.B.Index << 32))); handle = context.Stack->AllocateContinuationAndPush(tasks, workerIndex, dispatcher); - //Interlocked.Add(ref TasksSpawned, pushCount); } var wrapped = new WrappedOverlapHandler { Inner = context.Results, WorkerIndex = workerIndex }; diff --git a/BepuPhysics/Trees/Tree_SelfQueries.cs b/BepuPhysics/Trees/Tree_SelfQueries.cs index 3f32fe575..9e4b18275 100644 --- a/BepuPhysics/Trees/Tree_SelfQueries.cs +++ b/BepuPhysics/Trees/Tree_SelfQueries.cs @@ -718,77 +718,36 @@ private unsafe void GetSelfOverlaps2(ref TOverlapHandler result results = resultsCopy; } - - private unsafe void GetSelfOverlaps2Poor(ref TOverlapHandler results, BufferPool pool, - int workerIndex, TaskStack* taskStack, IThreadDispatcher threadDispatcher, bool internallyDispatch, int workerCount, int targetTaskBudget) where TOverlapHandler : unmanaged, IThreadedOverlapHandler + private unsafe void GetSelfOverlaps3(ref TOverlapHandler results, + int workerIndex, TaskStack* taskStack, IThreadDispatcher threadDispatcher, bool internallyDispatch, int workerCount, int targetTaskBudget) where TOverlapHandler : unmanaged, IThreadedOverlapHandler { - targetTaskBudget = int.Min(NodeCount, targetTaskBudget); if (targetTaskBudget < 0) targetTaskBudget = threadDispatcher.ThreadCount; - targetTaskBudget *= 8; + targetTaskBudget *= 16; + targetTaskBudget = int.Min(NodeCount, targetTaskBudget); //Crossover tests can generate more overlaps than there are leaves, so the simply dividing the leaf count by the target task count will tend to result in more tasks than necessary. //BUT... it's not feasible to figure out how many tasks you should actually have ahead of time! The critical thing is that we have *enough* tasks, and that the tasks //aren't so small that pushing them is a complete waste. //Don't have to worry about oversubscription, so the potential overhead isn't *too* bad. var leafThresholdForTask = int.Min(int.Max(LeafCount / (targetTaskBudget * 8), 64), LeafCount); - leafThresholdForTask = 1024; + leafThresholdForTask = 8192; var resultsCopy = results; var context = new SelfTestContext { Tree = this, LoopTaskCount = targetTaskBudget, LeafThresholdForTask = leafThresholdForTask, Results = &resultsCopy, TaskStack = taskStack }; - //Go ahead and submit very large early nodes as independent tasks to help with load balancing. - - const int maximumIsolatedNodeCapacity = 128; - int isolatedNodeCapacity = int.Min(maximumIsolatedNodeCapacity, targetTaskBudget); - var earlyQueue = new QuickQueue(isolatedNodeCapacity, pool); - var earlyIsolatedNodesMemory = stackalloc int[isolatedNodeCapacity]; - var earlyIsolatedNodes = new QuickList(new Buffer(earlyIsolatedNodesMemory, isolatedNodeCapacity)); - earlyQueue.EnqueueUnsafely() = 0; - while (earlyQueue.Count < earlyQueue.Span.Length && earlyIsolatedNodes.Count < isolatedNodeCapacity) - { - var nodeIndex = earlyQueue.DequeueUnsafely(); - ref var node = ref Nodes[nodeIndex]; - ref var a = ref node.A; - ref var b = ref node.B; - if (int.Max(a.LeafCount, b.LeafCount) >= leafThresholdForTask) - { - if (BoundingBox.IntersectsUnsafe(a, b)) - { - //Note that this technically does double work on the bounds test with the way we're submitting this as a task. Don't care; it's constant bounded nanoseconds. - earlyIsolatedNodes.AllocateUnsafely() = nodeIndex; - } - earlyQueue.EnqueueUnsafely() = a.Index; - earlyQueue.EnqueueUnsafely() = b.Index; - } - } - - var tasks = new QuickList(targetTaskBudget, pool); - var targetTaskSize = NodeCount / targetTaskBudget; - for (int i = 0; i < earlyQueue.Count; ++i) - { - var nodeIndex = earlyQueue[i]; - ref var node = ref Nodes[nodeIndex]; - var regionSize = node.A.LeafCount + node.B.LeafCount - 1; - var localTaskCount = (regionSize + targetTaskSize - 1) / targetTaskSize; - var baseNodesPerTask = regionSize / localTaskCount; - var remainder = regionSize - baseNodesPerTask * localTaskCount; - int previousEnd = nodeIndex; - for (int j = 0; j < localTaskCount; ++j) - { - var taskStart = previousEnd; - var taskSize = j < remainder ? baseNodesPerTask + 1 : baseNodesPerTask; - var taskEnd = previousEnd + taskSize; - tasks.Allocate(pool) = new Task(&LoopEntryTask, &context, (uint)taskStart | (((long)taskEnd) << 32)); - } - } - earlyQueue.Dispose(pool); - for (int i = earlyIsolatedNodes.Count - 1; i >= 0; --i) + var nodesPerTaskBase = NodeCount / targetTaskBudget; + var remainder = NodeCount - nodesPerTaskBase * targetTaskBudget; + Span tasks = stackalloc Task[targetTaskBudget]; + int previousEnd = 0; + for (int i = 0; i < targetTaskBudget; ++i) { - var nodeIndex = earlyIsolatedNodes[i]; - tasks.Allocate(pool) = new Task(&LoopEntryTask, &context, (uint)nodeIndex | (((long)(nodeIndex + 1)) << 32)); + var taskStart = previousEnd; + var nodeCountForTask = i < remainder ? nodesPerTaskBase + 1 : nodesPerTaskBase; + var taskEnd = previousEnd + nodeCountForTask; + previousEnd = taskEnd; + tasks[i] = new Task(&LoopEntryTaskWithSubtasks4, &context, (uint)taskStart | (((long)taskEnd) << 32)); } - if (internallyDispatch) { //There isn't an active dispatch, so we need to do it. @@ -800,12 +759,13 @@ private unsafe void GetSelfOverlaps2Poor(ref TOverlapHandler re //We're executing from within a multithreaded dispatch already, so we can simply run the tasks and trust that other threads are ready to steal. taskStack->RunTasks(tasks, workerIndex, threadDispatcher); } - tasks.Dispose(pool); //Have to copy back the results; it's a value type. results = resultsCopy; } + + /// /// Reports all bounding box overlaps between leaves in the tree to the given . Uses the thread dispatcher to parallelize overlap testing. /// diff --git a/BepuPhysics/Trees/Tree_SelfQueriesMT2.cs b/BepuPhysics/Trees/Tree_SelfQueriesMT2.cs new file mode 100644 index 000000000..d847d8b23 --- /dev/null +++ b/BepuPhysics/Trees/Tree_SelfQueriesMT2.cs @@ -0,0 +1,166 @@ +using BepuUtilities; +using BepuUtilities.Collections; +using BepuUtilities.Memory; +using BepuUtilities.TaskScheduling; +using System; +using System.Diagnostics; +using System.Linq; +using System.Numerics; +using System.Reflection.Metadata; +using System.Runtime.CompilerServices; +using System.Threading; + +namespace BepuPhysics.Trees; + +partial struct Tree +{ + static unsafe void SelfTestTask(long encodedIndices, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) where TOverlapHandler : unmanaged, IThreadedOverlapHandler + { + var indexA = (int)encodedIndices; + var indexB = (int)(encodedIndices >> 32); + ref var context = ref *(SelfTestContext*)untypedContext; + if (indexA >= 0 && indexB >= 0) + { + //Both nodes. + ref var a = ref context.Tree.Nodes[indexA]; + ref var b = ref context.Tree.Nodes[indexB]; + ref var aa = ref a.A; + ref var ab = ref a.B; + ref var ba = ref b.A; + ref var bb = ref b.B; + var aaIntersects = BoundingBox.IntersectsUnsafe(aa, ba); + var abIntersects = BoundingBox.IntersectsUnsafe(aa, bb); + var baIntersects = BoundingBox.IntersectsUnsafe(ab, ba); + var bbIntersects = BoundingBox.IntersectsUnsafe(ab, bb); + + //Push all pushable work *before* doing any work to ensure that other threads have something to grab. + var pushAA = aaIntersects && int.Max(aa.LeafCount, ba.LeafCount) >= context.LeafThresholdForTask; + var pushAB = abIntersects && int.Max(aa.LeafCount, bb.LeafCount) >= context.LeafThresholdForTask; + var pushBA = baIntersects && int.Max(ab.LeafCount, ba.LeafCount) >= context.LeafThresholdForTask; + var pushBB = bbIntersects && int.Max(ab.LeafCount, bb.LeafCount) >= context.LeafThresholdForTask; + var pushCount = (pushAA ? 1 : 0) + (pushAB ? 1 : 0) + (pushBA ? 1 : 0) + (pushBB ? 1 : 0); + ContinuationHandle handle = default; + if (pushCount > 0) + { + Span tasks = stackalloc Task[pushCount]; + pushCount = 0; + if (pushAA) tasks[pushCount++] = new Task(&SelfTestTask, untypedContext, ((uint)a.A.Index | ((long)b.A.Index << 32))); + if (pushAB) tasks[pushCount++] = new Task(&SelfTestTask, untypedContext, ((uint)a.A.Index | ((long)b.B.Index << 32))); + if (pushBA) tasks[pushCount++] = new Task(&SelfTestTask, untypedContext, ((uint)a.B.Index | ((long)b.A.Index << 32))); + if (pushBB) tasks[pushCount++] = new Task(&SelfTestTask, untypedContext, ((uint)a.B.Index | ((long)b.B.Index << 32))); + handle = context.TaskStack->AllocateContinuationAndPush(tasks, workerIndex, dispatcher); + } + + var wrapped = new WrappedOverlapHandler { Inner = context.Results, WorkerIndex = workerIndex }; + if (aaIntersects && !pushAA) + { + context.Tree.DispatchTestForNodes(ref aa, ref ba, ref wrapped); + } + if (abIntersects && !pushAB) + { + context.Tree.DispatchTestForNodes(ref aa, ref bb, ref wrapped); + } + if (baIntersects && !pushBA) + { + context.Tree.DispatchTestForNodes(ref ab, ref ba, ref wrapped); + } + if (bbIntersects && !pushBB) + { + context.Tree.DispatchTestForNodes(ref ab, ref bb, ref wrapped); + } + if (pushCount > 0) + { + context.TaskStack->WaitForCompletion(handle, workerIndex, dispatcher); + } + } + else + { + //One of the two indices points at a leaf. + //In reasonably balanced circumstances, there's hardly any reason to bother spawning tasks for node-leaf, but we may be in a pathological case + //where one tree is much smaller in *leaf count*, but not in size, and so the node might actually have a ton of leaves beneath it. + Debug.Assert(indexA >= 0 || indexB >= 0, "A task should not have been spawned for a leaf-leaf test. Leaf threshold is likely messed up."); + var aIsNode = indexA >= 0; + ref var node = ref aIsNode ? ref context.Tree.Nodes[indexA] : ref context.Tree.Nodes[indexB]; + //Go looking for the leaf child by using the leaf buffer indirection since we didn't store the bounding box anywhere else. + //This hurts a bit- a whole extra bit of memory being touched- but this codepath *should* be pretty rare as a fraction of test time; the whole point is for it to hit a sequential path quickly. + var leafIndex = aIsNode ? Encode(indexB) : Encode(indexA); + var leaf = context.Tree.Leaves[leafIndex]; + ref var leafChild = ref Unsafe.Add(ref context.Tree.Nodes[leaf.NodeIndex].A, leaf.ChildIndex); + var aIntersects = BoundingBox.IntersectsUnsafe(leafChild, node.A); + var bIntersects = BoundingBox.IntersectsUnsafe(leafChild, node.B); + var pushA = aIntersects && node.A.LeafCount >= context.LeafThresholdForTask; + var pushB = bIntersects && node.B.LeafCount >= context.LeafThresholdForTask; + var pushCount = (pushA ? 1 : 0) + (pushB ? 1 : 0); + ContinuationHandle handle = default; + if (pushCount > 0) + { + Span tasks = stackalloc Task[pushCount]; + pushCount = 0; + if (pushA) tasks[pushCount++] = new Task(&SelfTestTask, untypedContext, (uint)(aIsNode ? node.A.Index : indexA) | ((long)(aIsNode ? indexB : node.A.Index) << 32)); + if (pushB) tasks[pushCount++] = new Task(&SelfTestTask, untypedContext, (uint)(aIsNode ? node.B.Index : indexA) | ((long)(aIsNode ? indexB : node.B.Index) << 32)); + handle = context.TaskStack->AllocateContinuationAndPush(tasks, workerIndex, dispatcher); + } + var wrapped = new WrappedOverlapHandler { Inner = context.Results, WorkerIndex = workerIndex }; + if (aIntersects && !pushA) + { + context.Tree.DispatchTestForNodes(ref aIsNode ? ref node.A : ref leafChild, ref aIsNode ? ref leafChild : ref node.A, ref wrapped); + } + if (bIntersects && !pushB) + { + context.Tree.DispatchTestForNodes(ref aIsNode ? ref node.B : ref leafChild, ref aIsNode ? ref leafChild : ref node.B, ref wrapped); + } + if (pushCount > 0) + { + context.TaskStack->WaitForCompletion(handle, workerIndex, dispatcher); + } + } + + } + + unsafe static void LoopEntryTaskWithSubtasks4(long taskStartAndEnd, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) where TOverlapHandler : unmanaged, IThreadedOverlapHandler + { + var taskStart = (int)taskStartAndEnd; + var taskEnd = (int)(taskStartAndEnd >> 32); + ref var context = ref *(SelfTestContext*)untypedContext; + var wrapped = new WrappedOverlapHandler { Inner = context.Results, WorkerIndex = workerIndex }; + var nodes = context.Tree.Nodes; + var pool = dispatcher.WorkerPools[workerIndex]; + var continuationCountEstimate = int.Max(32, (taskEnd - taskStart) / 64); + var subtaskContinuations = new QuickList(continuationCountEstimate, pool); + //While reverse execution tends to be faster for access pattern reasons, the largest nodes tend to come early. We'd like to submit tasks for them early. + //TODO: You may want to try tiled execution- do a forward pass for tasks up to N sequential nodes, then flip and do all single threaded dispatches in reverse. + //for (int i = taskEnd - 1; i >= taskStart; --i) + for (int i = taskStart; i < taskEnd; ++i) + { + ref var node = ref nodes[i]; + ref var a = ref node.A; + ref var b = ref node.B; + var ab = BoundingBox.IntersectsUnsafe(a, b); + if (ab) + { + if (int.Max(a.LeafCount, b.LeafCount) >= context.LeafThresholdForTask) + { + //The number of potential overlaps is high; push this pair to a subtask. + //Note that the leaf threshold guarantees at least one of the children is an internal node. + Debug.Assert(context.LeafThresholdForTask > 1); + subtaskContinuations.Allocate(pool) = context.TaskStack->AllocateContinuationAndPush(new Task(&SelfTestTask, untypedContext, (uint)a.Index | ((long)b.Index << 32)), workerIndex, dispatcher); + } + else + { + context.Tree.DispatchTestForNodes(ref a, ref b, ref wrapped); + } + } + } + //if(subtaskContinuations.Count > 0) + //{ + // Console.WriteLine($"Start {taskStart}: {subtaskContinuations.Count}"); + //} + for (int i = 0; i < subtaskContinuations.Count; ++i) + { + context.TaskStack->WaitForCompletion(subtaskContinuations[i], workerIndex, dispatcher); + } + subtaskContinuations.Dispose(pool); + + } + +} From e7400ce9d4bc7e4dec9738cb1a19afb2fabd1dbd Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 8 Aug 2023 16:32:28 -0500 Subject: [PATCH 783/947] Tunin'. --- BepuPhysics/Trees/Tree_SelfQueries.cs | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/BepuPhysics/Trees/Tree_SelfQueries.cs b/BepuPhysics/Trees/Tree_SelfQueries.cs index 9e4b18275..231e27d00 100644 --- a/BepuPhysics/Trees/Tree_SelfQueries.cs +++ b/BepuPhysics/Trees/Tree_SelfQueries.cs @@ -646,12 +646,7 @@ private unsafe void GetSelfOverlaps2(ref TOverlapHandler result targetTaskBudget *= 8; targetTaskBudget = int.Min(NodeCount, targetTaskBudget); - //Crossover tests can generate more overlaps than there are leaves, so the simply dividing the leaf count by the target task count will tend to result in more tasks than necessary. - //BUT... it's not feasible to figure out how many tasks you should actually have ahead of time! The critical thing is that we have *enough* tasks, and that the tasks - //aren't so small that pushing them is a complete waste. - //Don't have to worry about oversubscription, so the potential overhead isn't *too* bad. - var leafThresholdForTask = int.Min(int.Max(LeafCount / (targetTaskBudget * 8), 64), LeafCount); - leafThresholdForTask = 32; + const int leafThresholdForTask = 256; var resultsCopy = results; var context = new SelfTestContext { Tree = this, LoopTaskCount = targetTaskBudget, LeafThresholdForTask = leafThresholdForTask, Results = &resultsCopy, TaskStack = taskStack }; @@ -660,7 +655,7 @@ private unsafe void GetSelfOverlaps2(ref TOverlapHandler result //(This isn't guaranteed, or even intended, to catch all large individual nodes. It's just an easy way to get some of them.) var earlyIsolatedNodeIntervalEnd = 0; const int maximumIsolatedNodeCapacity = 32; - int isolatedNodeCapacity = int.Min(maximumIsolatedNodeCapacity, targetTaskBudget); + int isolatedNodeCapacity = int.Min(maximumIsolatedNodeCapacity, targetTaskBudget / 4); var earlyIsolatedNodesMemory = stackalloc int[isolatedNodeCapacity]; var earlyIsolatedNodes = new QuickList(new Buffer(earlyIsolatedNodesMemory, isolatedNodeCapacity)); for (int i = 0; i < NodeCount && earlyIsolatedNodes.Count < isolatedNodeCapacity; ++i) @@ -668,7 +663,7 @@ private unsafe void GetSelfOverlaps2(ref TOverlapHandler result ref var node = ref Nodes[i]; ref var a = ref node.A; ref var b = ref node.B; - if (int.Min(a.LeafCount, b.LeafCount) > leafThresholdForTask) + if (int.Max(a.LeafCount, b.LeafCount) > leafThresholdForTask) { if (BoundingBox.IntersectsUnsafe(a, b)) { From cbe609ed09503cf5c340ea528a3d1d6c9015a813 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 8 Aug 2023 17:05:03 -0500 Subject: [PATCH 784/947] Wee bit more tuning. --- BepuPhysics/Trees/Tree_SelfQueries.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/BepuPhysics/Trees/Tree_SelfQueries.cs b/BepuPhysics/Trees/Tree_SelfQueries.cs index 231e27d00..e763765ea 100644 --- a/BepuPhysics/Trees/Tree_SelfQueries.cs +++ b/BepuPhysics/Trees/Tree_SelfQueries.cs @@ -643,7 +643,7 @@ private unsafe void GetSelfOverlaps2(ref TOverlapHandler result { if (targetTaskBudget < 0) targetTaskBudget = threadDispatcher.ThreadCount; - targetTaskBudget *= 8; + targetTaskBudget *= 16; targetTaskBudget = int.Min(NodeCount, targetTaskBudget); const int leafThresholdForTask = 256; From e7c204584ca6fb60d252c48d415d65b9b08b408e Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 8 Aug 2023 18:49:41 -0500 Subject: [PATCH 785/947] Stack friendliness. --- BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs | 51 +++++++++++-------- BepuPhysics/Trees/Tree_SelfQueries.cs | 9 ++-- 2 files changed, 36 insertions(+), 24 deletions(-) diff --git a/BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs b/BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs index 3fe01c171..2dd6ae968 100644 --- a/BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs +++ b/BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs @@ -9,6 +9,7 @@ using System.Reflection.Metadata; using System.Runtime.CompilerServices; using System.Threading; +using System.Xml.Linq; namespace BepuPhysics.Trees; @@ -23,6 +24,34 @@ unsafe struct IntertreeContext where TOverlapHandler : unmanage public TOverlapHandler* Results; } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + static unsafe ContinuationHandle PushIntertreeSubtasks(int pushCount, in Node a, in Node b, bool pushAA, bool pushAB, bool pushBA, bool pushBB, + void* untypedContext, in IntertreeContext context, int workerIndex, IThreadDispatcher dispatcher) where TOverlapHandler : unmanaged, IThreadedOverlapHandler + { + //Stackallocs persist for the duration of the function. Because the intertree test uses a lot of recursion, there's a lot of stack pressure. + //Given that IntertreeTask can call IntertreeTask, it can be pretty bad. + //To avoid that, we perform the stackalloc and push within this function. + Span tasks = stackalloc Task[pushCount]; + pushCount = 0; + if (pushAA) tasks[pushCount++] = new Task(&IntertreeTask, untypedContext, ((uint)a.A.Index | ((long)b.A.Index << 32))); + if (pushAB) tasks[pushCount++] = new Task(&IntertreeTask, untypedContext, ((uint)a.A.Index | ((long)b.B.Index << 32))); + if (pushBA) tasks[pushCount++] = new Task(&IntertreeTask, untypedContext, ((uint)a.B.Index | ((long)b.A.Index << 32))); + if (pushBB) tasks[pushCount++] = new Task(&IntertreeTask, untypedContext, ((uint)a.B.Index | ((long)b.B.Index << 32))); + return context.Stack->AllocateContinuationAndPush(tasks, workerIndex, dispatcher); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + static unsafe ContinuationHandle PushIntertreeSubtasksForNodeLeaf(int pushCount, in Node node, int indexA, int indexB, bool nodeBelongsToTreeA, bool pushA, bool pushB, + void* untypedContext, in IntertreeContext context, int workerIndex, IThreadDispatcher dispatcher) where TOverlapHandler : unmanaged, IThreadedOverlapHandler + { + //Not as heavy as node-node, but still enough to punt into a frame that gets popped. + Span tasks = stackalloc Task[pushCount]; + pushCount = 0; + if (pushA) tasks[pushCount++] = new Task(&IntertreeTask, untypedContext, (uint)(nodeBelongsToTreeA ? node.A.Index : indexA) | ((long)(nodeBelongsToTreeA ? indexB : node.A.Index) << 32)); + if (pushB) tasks[pushCount++] = new Task(&IntertreeTask, untypedContext, (uint)(nodeBelongsToTreeA ? node.B.Index : indexA) | ((long)(nodeBelongsToTreeA ? indexB : node.B.Index) << 32)); + return context.Stack->AllocateContinuationAndPush(tasks, workerIndex, dispatcher); + } + static unsafe void IntertreeTask(long encodedIndices, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) where TOverlapHandler : unmanaged, IThreadedOverlapHandler { var indexA = (int)encodedIndices; @@ -48,17 +77,7 @@ static unsafe void IntertreeTask(long encodedIndices, void* unt var pushBA = baIntersects && int.Max(ab.LeafCount, ba.LeafCount) >= context.LeafThreshold; var pushBB = bbIntersects && int.Max(ab.LeafCount, bb.LeafCount) >= context.LeafThreshold; var pushCount = (pushAA ? 1 : 0) + (pushAB ? 1 : 0) + (pushBA ? 1 : 0) + (pushBB ? 1 : 0); - ContinuationHandle handle = default; - if (pushCount > 0) - { - Span tasks = stackalloc Task[pushCount]; - pushCount = 0; - if (pushAA) tasks[pushCount++] = new Task(&IntertreeTask, untypedContext, ((uint)a.A.Index | ((long)b.A.Index << 32))); - if (pushAB) tasks[pushCount++] = new Task(&IntertreeTask, untypedContext, ((uint)a.A.Index | ((long)b.B.Index << 32))); - if (pushBA) tasks[pushCount++] = new Task(&IntertreeTask, untypedContext, ((uint)a.B.Index | ((long)b.A.Index << 32))); - if (pushBB) tasks[pushCount++] = new Task(&IntertreeTask, untypedContext, ((uint)a.B.Index | ((long)b.B.Index << 32))); - handle = context.Stack->AllocateContinuationAndPush(tasks, workerIndex, dispatcher); - } + var handle = pushCount == 0 ? default : PushIntertreeSubtasks(pushCount, a, b, pushAA, pushAB, pushBA, pushBB, untypedContext, context, workerIndex, dispatcher); var wrapped = new WrappedOverlapHandler { Inner = context.Results, WorkerIndex = workerIndex }; if (aaIntersects && !pushAA) @@ -101,15 +120,7 @@ static unsafe void IntertreeTask(long encodedIndices, void* unt var pushA = aIntersects && node.A.LeafCount >= context.LeafThreshold; var pushB = bIntersects && node.B.LeafCount >= context.LeafThreshold; var pushCount = (pushA ? 1 : 0) + (pushB ? 1 : 0); - ContinuationHandle handle = default; - if (pushCount > 0) - { - Span tasks = stackalloc Task[pushCount]; - pushCount = 0; - if (pushA) tasks[pushCount++] = new Task(&IntertreeTask, untypedContext, (uint)(nodeBelongsToTreeA ? node.A.Index : indexA) | ((long)(nodeBelongsToTreeA ? indexB : node.A.Index) << 32)); - if (pushB) tasks[pushCount++] = new Task(&IntertreeTask, untypedContext, (uint)(nodeBelongsToTreeA ? node.B.Index : indexA) | ((long)(nodeBelongsToTreeA ? indexB : node.B.Index) << 32)); - handle = context.Stack->AllocateContinuationAndPush(tasks, workerIndex, dispatcher); - } + var handle = pushCount == 0 ? default : PushIntertreeSubtasksForNodeLeaf(pushCount, node, indexA, indexB, nodeBelongsToTreeA, pushA, pushB, untypedContext, context, workerIndex, dispatcher); var wrapped = new WrappedOverlapHandler { Inner = context.Results, WorkerIndex = workerIndex }; if (aIntersects && !pushA) { diff --git a/BepuPhysics/Trees/Tree_SelfQueries.cs b/BepuPhysics/Trees/Tree_SelfQueries.cs index e763765ea..34dbbacb3 100644 --- a/BepuPhysics/Trees/Tree_SelfQueries.cs +++ b/BepuPhysics/Trees/Tree_SelfQueries.cs @@ -638,7 +638,7 @@ unsafe static void LoopEntryTaskWithSubtasks3(long taskStartAnd } - private unsafe void GetSelfOverlaps2(ref TOverlapHandler results, + private unsafe void GetSelfOverlaps2(ref TOverlapHandler results, BufferPool pool, int workerIndex, TaskStack* taskStack, IThreadDispatcher threadDispatcher, bool internallyDispatch, int workerCount, int targetTaskBudget) where TOverlapHandler : unmanaged, IThreadedOverlapHandler { if (targetTaskBudget < 0) @@ -682,7 +682,7 @@ private unsafe void GetSelfOverlaps2(ref TOverlapHandler result var regularLoopTaskCount = targetTaskBudget - earlyIsolatedNodes.Count; var nodesPerTaskBase = remainingNodeCount / regularLoopTaskCount; var remainder = remainingNodeCount - nodesPerTaskBase * regularLoopTaskCount; - Span tasks = stackalloc Task[targetTaskBudget]; + var tasks = new Buffer(targetTaskBudget, pool); int previousEnd = earlyIsolatedNodeIntervalEnd; for (int i = 0; i < regularLoopTaskCount; ++i) { @@ -709,6 +709,7 @@ private unsafe void GetSelfOverlaps2(ref TOverlapHandler result //We're executing from within a multithreaded dispatch already, so we can simply run the tasks and trust that other threads are ready to steal. taskStack->RunTasks(tasks, workerIndex, threadDispatcher); } + tasks.Dispose(pool); //Have to copy back the results; it's a value type. results = resultsCopy; } @@ -770,7 +771,7 @@ private unsafe void GetSelfOverlaps3(ref TOverlapHandler result public unsafe void GetSelfOverlaps2(ref TOverlapHandler results, BufferPool pool, IThreadDispatcher threadDispatcher) where TOverlapHandler : unmanaged, IThreadedOverlapHandler { var taskStack = new TaskStack(pool, threadDispatcher, threadDispatcher.ThreadCount); - GetSelfOverlaps2(ref results, 0, &taskStack, threadDispatcher, true, threadDispatcher.ThreadCount, threadDispatcher.ThreadCount); + GetSelfOverlaps2(ref results, pool, 0, &taskStack, threadDispatcher, true, threadDispatcher.ThreadCount, threadDispatcher.ThreadCount); taskStack.Dispose(pool, threadDispatcher); } @@ -787,7 +788,7 @@ public unsafe void GetSelfOverlaps2(ref TOverlapHandler results public unsafe void GetSelfOverlaps2(ref TOverlapHandler results, BufferPool pool, IThreadDispatcher threadDispatcher, TaskStack* taskStack, int workerIndex, int targetTaskCount = -1) where TOverlapHandler : unmanaged, IThreadedOverlapHandler { - GetSelfOverlaps2(ref results, workerIndex, taskStack, threadDispatcher, false, threadDispatcher.ThreadCount, targetTaskCount); + GetSelfOverlaps2(ref results, pool, workerIndex, taskStack, threadDispatcher, false, threadDispatcher.ThreadCount, targetTaskCount); } } } From 413ead736c17b808e7475de838b3638674b57c9a Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 8 Aug 2023 20:00:45 -0500 Subject: [PATCH 786/947] Some cleanup. --- BepuPhysics/Trees/Tree_SelfQueries.cs | 154 --------------------- BepuPhysics/Trees/Tree_SelfQueriesMT2.cs | 166 ----------------------- 2 files changed, 320 deletions(-) delete mode 100644 BepuPhysics/Trees/Tree_SelfQueriesMT2.cs diff --git a/BepuPhysics/Trees/Tree_SelfQueries.cs b/BepuPhysics/Trees/Tree_SelfQueries.cs index 34dbbacb3..298cc4a08 100644 --- a/BepuPhysics/Trees/Tree_SelfQueries.cs +++ b/BepuPhysics/Trees/Tree_SelfQueries.cs @@ -528,114 +528,6 @@ unsafe static void LoopEntryTask(long taskStartAndEnd, void* un context.Tree.GetSelfOverlaps2(ref wrapped, taskStart, taskEnd); } - unsafe static void CrossoverWithSubtasksTask(long encodedNodeIndices, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) where TOverlapHandler : unmanaged, IThreadedOverlapHandler - { - ref var context = ref *(SelfTestContext*)untypedContext; - var nodeIndexA = (int)encodedNodeIndices; - var nodeIndexB = (int)(encodedNodeIndices >> 32); - ref var a = ref context.Tree.Nodes[nodeIndexA]; - ref var b = ref context.Tree.Nodes[nodeIndexB]; - //Note that we don't need to do self tests here; all self tests are performed by the top level enumeration that spawned this task originally. - //The sole purpose of this is to split up crossover tests sufficiently that load balancing works well. - ref var aa = ref a.A; - ref var ab = ref a.B; - ref var ba = ref b.A; - ref var bb = ref b.B; - var aaIntersects = BoundingBox.IntersectsUnsafe(aa, ba); - var abIntersects = BoundingBox.IntersectsUnsafe(aa, bb); - var baIntersects = BoundingBox.IntersectsUnsafe(ab, ba); - var bbIntersects = BoundingBox.IntersectsUnsafe(ab, bb); - Debug.Assert(context.LeafThresholdForTask > 1, "This uses the leaf threshold as an implicit test for being an internal node, so the threshold must be above 1."); - var aaPushTask = aaIntersects && int.Min(aa.LeafCount, ba.LeafCount) >= context.LeafThresholdForTask; - var abPushTask = abIntersects && int.Min(aa.LeafCount, bb.LeafCount) >= context.LeafThresholdForTask; - var baPushTask = baIntersects && int.Min(ab.LeafCount, ba.LeafCount) >= context.LeafThresholdForTask; - var bbPushTask = bbIntersects && int.Min(ab.LeafCount, bb.LeafCount) >= context.LeafThresholdForTask; - var continuationCount = (aaPushTask ? 1 : 0) + (abPushTask ? 1 : 0) + (baPushTask ? 1 : 0) + (bbPushTask ? 1 : 0); - Span continuations = stackalloc ContinuationHandle[continuationCount]; - continuationCount = 0; - if (aaPushTask) - { - continuations[continuationCount++] = context.TaskStack->AllocateContinuationAndPush(new Task(&CrossoverWithSubtasksTask, untypedContext, (uint)a.A.Index | ((long)b.A.Index << 32)), workerIndex, dispatcher); - } - if (abPushTask) - { - continuations[continuationCount++] = context.TaskStack->AllocateContinuationAndPush(new Task(&CrossoverWithSubtasksTask, untypedContext, (uint)a.A.Index | ((long)b.B.Index << 32)), workerIndex, dispatcher); - } - if (baPushTask) - { - continuations[continuationCount++] = context.TaskStack->AllocateContinuationAndPush(new Task(&CrossoverWithSubtasksTask, untypedContext, (uint)a.B.Index | ((long)b.A.Index << 32)), workerIndex, dispatcher); - } - if (bbPushTask) - { - continuations[continuationCount++] = context.TaskStack->AllocateContinuationAndPush(new Task(&CrossoverWithSubtasksTask, untypedContext, (uint)a.B.Index | ((long)b.B.Index << 32)), workerIndex, dispatcher); - } - //We've pushed every crossover worthy of further splitting onto the stack. The moment any thread as an opening, it'll be investigated. - //For anything *not* pushed to a task, we can just run the test directly. - //(These tests are deferred until after the pushes because we don't want to delay the availability of the other tasks. - var wrappedResults = new WrappedOverlapHandler { Inner = context.Results, WorkerIndex = workerIndex }; - if (!aaPushTask && aaIntersects) - { - context.Tree.DispatchTestForNodes(ref aa, ref ba, ref wrappedResults); - } - if (!abPushTask && abIntersects) - { - context.Tree.DispatchTestForNodes(ref aa, ref bb, ref wrappedResults); - } - if (!baPushTask && baIntersects) - { - context.Tree.DispatchTestForNodes(ref ab, ref ba, ref wrappedResults); - } - if (!bbPushTask && bbIntersects) - { - context.Tree.DispatchTestForNodes(ref ab, ref bb, ref wrappedResults); - } - //The caller will use completion as a termination condition, so we shouldn't return until all spawned tasks complete. - for (int i = 0; i < continuations.Length; ++i) - context.TaskStack->WaitForCompletion(continuations[i], workerIndex, dispatcher); - - } - - unsafe static void LoopEntryTaskWithSubtasks3(long taskStartAndEnd, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) where TOverlapHandler : unmanaged, IThreadedOverlapHandler - { - var taskStart = (int)taskStartAndEnd; - var taskEnd = (int)(taskStartAndEnd >> 32); - ref var context = ref *(SelfTestContext*)untypedContext; - var wrapped = new WrappedOverlapHandler { Inner = context.Results, WorkerIndex = workerIndex }; - var nodes = context.Tree.Nodes; - var pool = dispatcher.WorkerPools[workerIndex]; - var continuationCountEstimate = int.Max(32, (taskEnd - taskStart) / 64); - var subtaskContinuations = new QuickList(continuationCountEstimate, pool); - //for (int i = taskEnd - 1; i >= taskStart; --i) - for (int i = taskStart; i < taskEnd; ++i) - { - ref var node = ref nodes[i]; - ref var a = ref node.A; - ref var b = ref node.B; - var ab = BoundingBox.IntersectsUnsafe(a, b); - if (ab) - { - if (int.Min(a.LeafCount, b.LeafCount) >= context.LeafThresholdForTask) - { - //The number of potential overlaps is high; push this pair to a subtask. - subtaskContinuations.Allocate(pool) = context.TaskStack->AllocateContinuationAndPush(new Task(&CrossoverWithSubtasksTask, untypedContext, (uint)a.Index | ((long)b.Index << 32)), workerIndex, dispatcher); - } - else - { - context.Tree.DispatchTestForNodes(ref a, ref b, ref wrapped); - } - } - } - //if(subtaskContinuations.Count > 0) - //{ - // Console.WriteLine($"Start {taskStart}: {subtaskContinuations.Count}"); - //} - for (int i = 0; i < subtaskContinuations.Count; ++i) - { - context.TaskStack->WaitForCompletion(subtaskContinuations[i], workerIndex, dispatcher); - } - subtaskContinuations.Dispose(pool); - - } private unsafe void GetSelfOverlaps2(ref TOverlapHandler results, BufferPool pool, @@ -714,52 +606,6 @@ private unsafe void GetSelfOverlaps2(ref TOverlapHandler result results = resultsCopy; } - private unsafe void GetSelfOverlaps3(ref TOverlapHandler results, - int workerIndex, TaskStack* taskStack, IThreadDispatcher threadDispatcher, bool internallyDispatch, int workerCount, int targetTaskBudget) where TOverlapHandler : unmanaged, IThreadedOverlapHandler - { - if (targetTaskBudget < 0) - targetTaskBudget = threadDispatcher.ThreadCount; - targetTaskBudget *= 16; - targetTaskBudget = int.Min(NodeCount, targetTaskBudget); - - //Crossover tests can generate more overlaps than there are leaves, so the simply dividing the leaf count by the target task count will tend to result in more tasks than necessary. - //BUT... it's not feasible to figure out how many tasks you should actually have ahead of time! The critical thing is that we have *enough* tasks, and that the tasks - //aren't so small that pushing them is a complete waste. - //Don't have to worry about oversubscription, so the potential overhead isn't *too* bad. - var leafThresholdForTask = int.Min(int.Max(LeafCount / (targetTaskBudget * 8), 64), LeafCount); - leafThresholdForTask = 8192; - - var resultsCopy = results; - var context = new SelfTestContext { Tree = this, LoopTaskCount = targetTaskBudget, LeafThresholdForTask = leafThresholdForTask, Results = &resultsCopy, TaskStack = taskStack }; - - var nodesPerTaskBase = NodeCount / targetTaskBudget; - var remainder = NodeCount - nodesPerTaskBase * targetTaskBudget; - Span tasks = stackalloc Task[targetTaskBudget]; - int previousEnd = 0; - for (int i = 0; i < targetTaskBudget; ++i) - { - var taskStart = previousEnd; - var nodeCountForTask = i < remainder ? nodesPerTaskBase + 1 : nodesPerTaskBase; - var taskEnd = previousEnd + nodeCountForTask; - previousEnd = taskEnd; - tasks[i] = new Task(&LoopEntryTaskWithSubtasks4, &context, (uint)taskStart | (((long)taskEnd) << 32)); - } - if (internallyDispatch) - { - //There isn't an active dispatch, so we need to do it. - taskStack->AllocateContinuationAndPush(tasks, workerIndex, threadDispatcher, onComplete: TaskStack.GetRequestStopTask(taskStack)); - TaskStack.DispatchWorkers(threadDispatcher, taskStack, workerCount); - } - else - { - //We're executing from within a multithreaded dispatch already, so we can simply run the tasks and trust that other threads are ready to steal. - taskStack->RunTasks(tasks, workerIndex, threadDispatcher); - } - //Have to copy back the results; it's a value type. - results = resultsCopy; - } - - /// diff --git a/BepuPhysics/Trees/Tree_SelfQueriesMT2.cs b/BepuPhysics/Trees/Tree_SelfQueriesMT2.cs deleted file mode 100644 index d847d8b23..000000000 --- a/BepuPhysics/Trees/Tree_SelfQueriesMT2.cs +++ /dev/null @@ -1,166 +0,0 @@ -using BepuUtilities; -using BepuUtilities.Collections; -using BepuUtilities.Memory; -using BepuUtilities.TaskScheduling; -using System; -using System.Diagnostics; -using System.Linq; -using System.Numerics; -using System.Reflection.Metadata; -using System.Runtime.CompilerServices; -using System.Threading; - -namespace BepuPhysics.Trees; - -partial struct Tree -{ - static unsafe void SelfTestTask(long encodedIndices, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) where TOverlapHandler : unmanaged, IThreadedOverlapHandler - { - var indexA = (int)encodedIndices; - var indexB = (int)(encodedIndices >> 32); - ref var context = ref *(SelfTestContext*)untypedContext; - if (indexA >= 0 && indexB >= 0) - { - //Both nodes. - ref var a = ref context.Tree.Nodes[indexA]; - ref var b = ref context.Tree.Nodes[indexB]; - ref var aa = ref a.A; - ref var ab = ref a.B; - ref var ba = ref b.A; - ref var bb = ref b.B; - var aaIntersects = BoundingBox.IntersectsUnsafe(aa, ba); - var abIntersects = BoundingBox.IntersectsUnsafe(aa, bb); - var baIntersects = BoundingBox.IntersectsUnsafe(ab, ba); - var bbIntersects = BoundingBox.IntersectsUnsafe(ab, bb); - - //Push all pushable work *before* doing any work to ensure that other threads have something to grab. - var pushAA = aaIntersects && int.Max(aa.LeafCount, ba.LeafCount) >= context.LeafThresholdForTask; - var pushAB = abIntersects && int.Max(aa.LeafCount, bb.LeafCount) >= context.LeafThresholdForTask; - var pushBA = baIntersects && int.Max(ab.LeafCount, ba.LeafCount) >= context.LeafThresholdForTask; - var pushBB = bbIntersects && int.Max(ab.LeafCount, bb.LeafCount) >= context.LeafThresholdForTask; - var pushCount = (pushAA ? 1 : 0) + (pushAB ? 1 : 0) + (pushBA ? 1 : 0) + (pushBB ? 1 : 0); - ContinuationHandle handle = default; - if (pushCount > 0) - { - Span tasks = stackalloc Task[pushCount]; - pushCount = 0; - if (pushAA) tasks[pushCount++] = new Task(&SelfTestTask, untypedContext, ((uint)a.A.Index | ((long)b.A.Index << 32))); - if (pushAB) tasks[pushCount++] = new Task(&SelfTestTask, untypedContext, ((uint)a.A.Index | ((long)b.B.Index << 32))); - if (pushBA) tasks[pushCount++] = new Task(&SelfTestTask, untypedContext, ((uint)a.B.Index | ((long)b.A.Index << 32))); - if (pushBB) tasks[pushCount++] = new Task(&SelfTestTask, untypedContext, ((uint)a.B.Index | ((long)b.B.Index << 32))); - handle = context.TaskStack->AllocateContinuationAndPush(tasks, workerIndex, dispatcher); - } - - var wrapped = new WrappedOverlapHandler { Inner = context.Results, WorkerIndex = workerIndex }; - if (aaIntersects && !pushAA) - { - context.Tree.DispatchTestForNodes(ref aa, ref ba, ref wrapped); - } - if (abIntersects && !pushAB) - { - context.Tree.DispatchTestForNodes(ref aa, ref bb, ref wrapped); - } - if (baIntersects && !pushBA) - { - context.Tree.DispatchTestForNodes(ref ab, ref ba, ref wrapped); - } - if (bbIntersects && !pushBB) - { - context.Tree.DispatchTestForNodes(ref ab, ref bb, ref wrapped); - } - if (pushCount > 0) - { - context.TaskStack->WaitForCompletion(handle, workerIndex, dispatcher); - } - } - else - { - //One of the two indices points at a leaf. - //In reasonably balanced circumstances, there's hardly any reason to bother spawning tasks for node-leaf, but we may be in a pathological case - //where one tree is much smaller in *leaf count*, but not in size, and so the node might actually have a ton of leaves beneath it. - Debug.Assert(indexA >= 0 || indexB >= 0, "A task should not have been spawned for a leaf-leaf test. Leaf threshold is likely messed up."); - var aIsNode = indexA >= 0; - ref var node = ref aIsNode ? ref context.Tree.Nodes[indexA] : ref context.Tree.Nodes[indexB]; - //Go looking for the leaf child by using the leaf buffer indirection since we didn't store the bounding box anywhere else. - //This hurts a bit- a whole extra bit of memory being touched- but this codepath *should* be pretty rare as a fraction of test time; the whole point is for it to hit a sequential path quickly. - var leafIndex = aIsNode ? Encode(indexB) : Encode(indexA); - var leaf = context.Tree.Leaves[leafIndex]; - ref var leafChild = ref Unsafe.Add(ref context.Tree.Nodes[leaf.NodeIndex].A, leaf.ChildIndex); - var aIntersects = BoundingBox.IntersectsUnsafe(leafChild, node.A); - var bIntersects = BoundingBox.IntersectsUnsafe(leafChild, node.B); - var pushA = aIntersects && node.A.LeafCount >= context.LeafThresholdForTask; - var pushB = bIntersects && node.B.LeafCount >= context.LeafThresholdForTask; - var pushCount = (pushA ? 1 : 0) + (pushB ? 1 : 0); - ContinuationHandle handle = default; - if (pushCount > 0) - { - Span tasks = stackalloc Task[pushCount]; - pushCount = 0; - if (pushA) tasks[pushCount++] = new Task(&SelfTestTask, untypedContext, (uint)(aIsNode ? node.A.Index : indexA) | ((long)(aIsNode ? indexB : node.A.Index) << 32)); - if (pushB) tasks[pushCount++] = new Task(&SelfTestTask, untypedContext, (uint)(aIsNode ? node.B.Index : indexA) | ((long)(aIsNode ? indexB : node.B.Index) << 32)); - handle = context.TaskStack->AllocateContinuationAndPush(tasks, workerIndex, dispatcher); - } - var wrapped = new WrappedOverlapHandler { Inner = context.Results, WorkerIndex = workerIndex }; - if (aIntersects && !pushA) - { - context.Tree.DispatchTestForNodes(ref aIsNode ? ref node.A : ref leafChild, ref aIsNode ? ref leafChild : ref node.A, ref wrapped); - } - if (bIntersects && !pushB) - { - context.Tree.DispatchTestForNodes(ref aIsNode ? ref node.B : ref leafChild, ref aIsNode ? ref leafChild : ref node.B, ref wrapped); - } - if (pushCount > 0) - { - context.TaskStack->WaitForCompletion(handle, workerIndex, dispatcher); - } - } - - } - - unsafe static void LoopEntryTaskWithSubtasks4(long taskStartAndEnd, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) where TOverlapHandler : unmanaged, IThreadedOverlapHandler - { - var taskStart = (int)taskStartAndEnd; - var taskEnd = (int)(taskStartAndEnd >> 32); - ref var context = ref *(SelfTestContext*)untypedContext; - var wrapped = new WrappedOverlapHandler { Inner = context.Results, WorkerIndex = workerIndex }; - var nodes = context.Tree.Nodes; - var pool = dispatcher.WorkerPools[workerIndex]; - var continuationCountEstimate = int.Max(32, (taskEnd - taskStart) / 64); - var subtaskContinuations = new QuickList(continuationCountEstimate, pool); - //While reverse execution tends to be faster for access pattern reasons, the largest nodes tend to come early. We'd like to submit tasks for them early. - //TODO: You may want to try tiled execution- do a forward pass for tasks up to N sequential nodes, then flip and do all single threaded dispatches in reverse. - //for (int i = taskEnd - 1; i >= taskStart; --i) - for (int i = taskStart; i < taskEnd; ++i) - { - ref var node = ref nodes[i]; - ref var a = ref node.A; - ref var b = ref node.B; - var ab = BoundingBox.IntersectsUnsafe(a, b); - if (ab) - { - if (int.Max(a.LeafCount, b.LeafCount) >= context.LeafThresholdForTask) - { - //The number of potential overlaps is high; push this pair to a subtask. - //Note that the leaf threshold guarantees at least one of the children is an internal node. - Debug.Assert(context.LeafThresholdForTask > 1); - subtaskContinuations.Allocate(pool) = context.TaskStack->AllocateContinuationAndPush(new Task(&SelfTestTask, untypedContext, (uint)a.Index | ((long)b.Index << 32)), workerIndex, dispatcher); - } - else - { - context.Tree.DispatchTestForNodes(ref a, ref b, ref wrapped); - } - } - } - //if(subtaskContinuations.Count > 0) - //{ - // Console.WriteLine($"Start {taskStart}: {subtaskContinuations.Count}"); - //} - for (int i = 0; i < subtaskContinuations.Count; ++i) - { - context.TaskStack->WaitForCompletion(subtaskContinuations[i], workerIndex, dispatcher); - } - subtaskContinuations.Dispose(pool); - - } - -} From a217efa527d06c92ffd2723b093ff9c2d2ad2e0d Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 9 Aug 2023 16:33:27 -0500 Subject: [PATCH 787/947] A managed worker is not a managed context. --- BepuUtilities/ThreadDispatcher.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/BepuUtilities/ThreadDispatcher.cs b/BepuUtilities/ThreadDispatcher.cs index 133e75acc..773f4558e 100644 --- a/BepuUtilities/ThreadDispatcher.cs +++ b/BepuUtilities/ThreadDispatcher.cs @@ -31,7 +31,7 @@ struct Worker /// public void* UnmanagedContext => unmanagedContext; /// - public object ManagedContext => managedWorker; + public object ManagedContext => managedContext; /// /// Creates a new thread dispatcher with the given number of threads. From a8d0fe29fa946e7d00dae3100dd8d2fe182c45bb Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 9 Aug 2023 16:34:27 -0500 Subject: [PATCH 788/947] Self/intertree overlaps now allow managed contexts to be passed in. --- .../Trees/Tree_CollectSubtreesDirect.cs | 69 ------------------- BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs | 36 +++++++--- BepuUtilities/TaskScheduling/TaskStack.cs | 5 +- 3 files changed, 31 insertions(+), 79 deletions(-) delete mode 100644 BepuPhysics/Trees/Tree_CollectSubtreesDirect.cs diff --git a/BepuPhysics/Trees/Tree_CollectSubtreesDirect.cs b/BepuPhysics/Trees/Tree_CollectSubtreesDirect.cs deleted file mode 100644 index b757a0be3..000000000 --- a/BepuPhysics/Trees/Tree_CollectSubtreesDirect.cs +++ /dev/null @@ -1,69 +0,0 @@ -using BepuUtilities.Collections; -using BepuUtilities.Memory; -using System.Diagnostics; -using System.Runtime.CompilerServices; - -namespace BepuPhysics.Trees -{ - - - partial struct Tree - { - unsafe void CollectSubtreesForNodeDirect(int nodeIndex, int remainingDepth, - ref QuickList subtrees, ref QuickQueue internalNodes, out float treeletCost) - { - internalNodes.EnqueueUnsafely(nodeIndex); - - treeletCost = 0; - ref var node = ref Nodes[nodeIndex]; - ref var children = ref node.A; - - --remainingDepth; - if (remainingDepth >= 0) - { - for (int i = 0; i < 2; ++i) - { - ref var child = ref Unsafe.Add(ref children, i); - if (child.Index >= 0) - { - treeletCost += ComputeBoundsMetric(ref child.Min, ref child.Max); - float childCost; - CollectSubtreesForNodeDirect(child.Index, remainingDepth, ref subtrees, ref internalNodes, out childCost); - treeletCost += childCost; - } - else - { - //It's a leaf, immediately add it to subtrees. - subtrees.AddUnsafely(child.Index); - } - } - } - else - { - //Recursion has bottomed out. Add every child. - //Once again, note that the treelet costs of these nodes are not considered, even if they are internal. - //That's because the subtree internal nodes cannot change size due to the refinement. - for (int i = 0; i < 2; ++i) - { - subtrees.AddUnsafely(Unsafe.Add(ref children, i).Index); - } - } - } - - public unsafe void CollectSubtreesDirect(int nodeIndex, int maximumSubtrees, - ref QuickList subtrees, ref QuickQueue internalNodes, out float treeletCost) - { - var maximumDepth = SpanHelper.GetContainingPowerOf2(maximumSubtrees) - 1; - Debug.Assert(maximumDepth > 0); - //Cost excludes the treelet root, since refinement can't change the treelet root's size. So don't bother including it in treeletCost. - - CollectSubtreesForNodeDirect(nodeIndex, maximumDepth, ref subtrees, ref internalNodes, out treeletCost); - - - } - - - - - } -} diff --git a/BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs b/BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs index 2dd6ae968..be59cf524 100644 --- a/BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs +++ b/BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs @@ -79,7 +79,7 @@ static unsafe void IntertreeTask(long encodedIndices, void* unt var pushCount = (pushAA ? 1 : 0) + (pushAB ? 1 : 0) + (pushBA ? 1 : 0) + (pushBB ? 1 : 0); var handle = pushCount == 0 ? default : PushIntertreeSubtasks(pushCount, a, b, pushAA, pushAB, pushBA, pushBB, untypedContext, context, workerIndex, dispatcher); - var wrapped = new WrappedOverlapHandler { Inner = context.Results, WorkerIndex = workerIndex }; + var wrapped = new WrappedOverlapHandler { Inner = context.Results, WorkerIndex = workerIndex, ManagedContext = dispatcher.ManagedContext }; if (aaIntersects && !pushAA) { context.TreeA.DispatchTestForNodes(ref aa, ref ba, ref context.TreeB, ref wrapped); @@ -121,7 +121,7 @@ static unsafe void IntertreeTask(long encodedIndices, void* unt var pushB = bIntersects && node.B.LeafCount >= context.LeafThreshold; var pushCount = (pushA ? 1 : 0) + (pushB ? 1 : 0); var handle = pushCount == 0 ? default : PushIntertreeSubtasksForNodeLeaf(pushCount, node, indexA, indexB, nodeBelongsToTreeA, pushA, pushB, untypedContext, context, workerIndex, dispatcher); - var wrapped = new WrappedOverlapHandler { Inner = context.Results, WorkerIndex = workerIndex }; + var wrapped = new WrappedOverlapHandler { Inner = context.Results, WorkerIndex = workerIndex, ManagedContext = dispatcher.ManagedContext }; if (aIntersects && !pushA) { context.TreeA.DispatchTestForNodes(ref nodeBelongsToTreeA ? ref node.A : ref leafChild, ref nodeBelongsToTreeA ? ref leafChild : ref node.A, ref context.TreeB, ref wrapped); @@ -138,8 +138,8 @@ static unsafe void IntertreeTask(long encodedIndices, void* unt } - public unsafe void GetOverlaps2(ref Tree treeB, ref TOverlapHandler results, - BufferPool pool, int workerIndex, TaskStack* taskStack, IThreadDispatcher threadDispatcher, bool internallyDispatch, int workerCount, int targetTaskBudget) where TOverlapHandler : unmanaged, IThreadedOverlapHandler + unsafe void GetOverlaps2(ref Tree treeB, ref TOverlapHandler results, + BufferPool pool, int workerIndex, TaskStack* taskStack, IThreadDispatcher threadDispatcher, bool internallyDispatch, int workerCount, int targetTaskBudget, object managedContext = null) where TOverlapHandler : unmanaged, IThreadedOverlapHandler { if (LeafCount == 0 || treeB.LeafCount == 0) return; @@ -149,7 +149,7 @@ public unsafe void GetOverlaps2(ref Tree treeB, ref TOverlapHan if (LeafCount < minimumLeafThreshold && treeB.LeafCount < minimumLeafThreshold) { //No point in spawning a bunch of tasks for tiny trees. - var wrapped = new WrappedOverlapHandler { Inner = &resultsCopy, WorkerIndex = 0 }; + var wrapped = new WrappedOverlapHandler { Inner = &resultsCopy, WorkerIndex = 0, ManagedContext = threadDispatcher.ManagedContext }; GetOverlaps(ref treeB, ref wrapped); } else @@ -172,7 +172,7 @@ public unsafe void GetOverlaps2(ref Tree treeB, ref TOverlapHan if (internallyDispatch) { taskStack->AllocateContinuationAndPush(rootTask, workerIndex, threadDispatcher, onComplete: TaskStack.GetRequestStopTask(taskStack)); - TaskStack.DispatchWorkers(threadDispatcher, taskStack, int.Min(threadDispatcher.ThreadCount, targetTaskBudget)); + TaskStack.DispatchWorkers(threadDispatcher, taskStack, int.Min(threadDispatcher.ThreadCount, targetTaskBudget), managedContext); } else { @@ -183,14 +183,34 @@ public unsafe void GetOverlaps2(ref Tree treeB, ref TOverlapHan results = resultsCopy; } - public unsafe void GetOverlaps2(ref Tree treeB, ref TOverlapHandler results, BufferPool pool, IThreadDispatcher dispatcher) + /// + /// Reports all bounding box overlaps between leaves in the two trees to the given . Uses the thread dispatcher to parallelize overlap testing. + /// + /// Other tree to test against. + /// Handler to report results to. + /// Pool used for ephemeral allocations. + /// Thread dispatcher used during the overlap testing. + /// Managed context to provide to the overlap handler, if any. + public unsafe void GetOverlaps2(ref Tree treeB, ref TOverlapHandler results, BufferPool pool, IThreadDispatcher dispatcher, object managedContext = null) where TOverlapHandler : unmanaged, IThreadedOverlapHandler { var taskStack = new TaskStack(pool, dispatcher, dispatcher.ThreadCount); - GetOverlaps2(ref treeB, ref results, pool, 0, &taskStack, dispatcher, true, dispatcher.ThreadCount, -1); + GetOverlaps2(ref treeB, ref results, pool, 0, &taskStack, dispatcher, true, dispatcher.ThreadCount, -1, managedContext); taskStack.Dispose(pool, dispatcher); } + /// + /// Reports all bounding box overlaps between leaves in the two trees to the given . + /// Pushes tasks into the provided . Does not dispatch threads internally; this is intended to be used as a part of a caller-managed dispatch. + /// + /// Other tree to test against. + /// Handler to report results to. + /// Pool used for ephemeral allocations. + /// Thread dispatcher used during the overlap test. + /// that the overlap test will push tasks onto as needed. + /// Index of the worker calling the function. + /// Number of tasks the overlap testing should try to create during execution. If negative, uses . + /// This does not dispatch workers on the directly. If the overlap handler requires managed context, that should be provided by whatever dispatched the workers. public unsafe void GetOverlaps2(ref Tree treeB, ref TOverlapHandler results, BufferPool pool, IThreadDispatcher dispatcher, TaskStack* taskStack, int workerIndex, int targetTaskCount = -1) where TOverlapHandler : unmanaged, IThreadedOverlapHandler { diff --git a/BepuUtilities/TaskScheduling/TaskStack.cs b/BepuUtilities/TaskScheduling/TaskStack.cs index 35e7cf661..f4b1ecd5a 100644 --- a/BepuUtilities/TaskScheduling/TaskStack.cs +++ b/BepuUtilities/TaskScheduling/TaskStack.cs @@ -551,8 +551,9 @@ public static void DispatchWorkerFunction(int workerIndex, IThreadDispatcher dis /// Task stack to pull work from. /// Thread dispatcher to dispatch workers with. /// Maximum number of workers to spin up for the dispatch. - public static void DispatchWorkers(IThreadDispatcher dispatcher, TaskStack* taskStack, int maximumWorkerCount = int.MaxValue) + /// Managed context to include in this dispatch, if any. + public static void DispatchWorkers(IThreadDispatcher dispatcher, TaskStack* taskStack, int maximumWorkerCount = int.MaxValue, object managedContext = null) { - dispatcher.DispatchWorkers(&DispatchWorkerFunction, maximumWorkerCount, taskStack); + dispatcher.DispatchWorkers(&DispatchWorkerFunction, maximumWorkerCount, taskStack, managedContext); } } From acf02c0f480d5fe7940ae3bdf3bb63d89a2aff80 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 9 Aug 2023 16:34:41 -0500 Subject: [PATCH 789/947] Oops. --- BepuPhysics/Trees/Tree_SelfQueries.cs | 20 +++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/BepuPhysics/Trees/Tree_SelfQueries.cs b/BepuPhysics/Trees/Tree_SelfQueries.cs index 298cc4a08..e4aba9289 100644 --- a/BepuPhysics/Trees/Tree_SelfQueries.cs +++ b/BepuPhysics/Trees/Tree_SelfQueries.cs @@ -41,7 +41,8 @@ public interface IThreadedOverlapHandler /// Index of the first leaf in the overlap. /// Index of the second leaf in the overlap. /// Index of the worker reporting the overlap. - void Handle(int indexA, int indexB, int workerIndex); + /// Managed context provided by the multithreaded dispatch. + void Handle(int indexA, int indexB, int workerIndex, object managedContext); } partial struct Tree @@ -512,10 +513,11 @@ unsafe struct SelfTestContext where TOverlapHandler : unmanaged unsafe struct WrappedOverlapHandler : IOverlapHandler where TOverlapHandler : unmanaged, IThreadedOverlapHandler { public int WorkerIndex; + public object ManagedContext; public TOverlapHandler* Inner; public void Handle(int indexA, int indexB) { - Inner->Handle(indexA, indexB, WorkerIndex); + Inner->Handle(indexA, indexB, WorkerIndex, ManagedContext); } } @@ -524,14 +526,14 @@ unsafe static void LoopEntryTask(long taskStartAndEnd, void* un var taskStart = (int)taskStartAndEnd; var taskEnd = (int)(taskStartAndEnd >> 32); ref var context = ref *(SelfTestContext*)untypedContext; - var wrapped = new WrappedOverlapHandler { Inner = context.Results, WorkerIndex = workerIndex }; + var wrapped = new WrappedOverlapHandler { Inner = context.Results, WorkerIndex = workerIndex, ManagedContext = dispatcher.ManagedContext }; context.Tree.GetSelfOverlaps2(ref wrapped, taskStart, taskEnd); } private unsafe void GetSelfOverlaps2(ref TOverlapHandler results, BufferPool pool, - int workerIndex, TaskStack* taskStack, IThreadDispatcher threadDispatcher, bool internallyDispatch, int workerCount, int targetTaskBudget) where TOverlapHandler : unmanaged, IThreadedOverlapHandler + int workerIndex, TaskStack* taskStack, IThreadDispatcher threadDispatcher, bool internallyDispatch, int workerCount, int targetTaskBudget, object managedContext = null) where TOverlapHandler : unmanaged, IThreadedOverlapHandler { if (targetTaskBudget < 0) targetTaskBudget = threadDispatcher.ThreadCount; @@ -594,7 +596,7 @@ private unsafe void GetSelfOverlaps2(ref TOverlapHandler result { //There isn't an active dispatch, so we need to do it. taskStack->AllocateContinuationAndPush(tasks, workerIndex, threadDispatcher, onComplete: TaskStack.GetRequestStopTask(taskStack)); - TaskStack.DispatchWorkers(threadDispatcher, taskStack, workerCount); + TaskStack.DispatchWorkers(threadDispatcher, taskStack, workerCount, managedContext); } else { @@ -606,18 +608,17 @@ private unsafe void GetSelfOverlaps2(ref TOverlapHandler result results = resultsCopy; } - - /// /// Reports all bounding box overlaps between leaves in the tree to the given . Uses the thread dispatcher to parallelize overlap testing. /// /// Handler to report results to. /// Pool used for ephemeral allocations. /// Thread dispatcher used during the overlap testing. - public unsafe void GetSelfOverlaps2(ref TOverlapHandler results, BufferPool pool, IThreadDispatcher threadDispatcher) where TOverlapHandler : unmanaged, IThreadedOverlapHandler + /// Managed context to provide to the overlap handler, if any. + public unsafe void GetSelfOverlaps2(ref TOverlapHandler results, BufferPool pool, IThreadDispatcher threadDispatcher, object managedContext = null) where TOverlapHandler : unmanaged, IThreadedOverlapHandler { var taskStack = new TaskStack(pool, threadDispatcher, threadDispatcher.ThreadCount); - GetSelfOverlaps2(ref results, pool, 0, &taskStack, threadDispatcher, true, threadDispatcher.ThreadCount, threadDispatcher.ThreadCount); + GetSelfOverlaps2(ref results, pool, 0, &taskStack, threadDispatcher, true, threadDispatcher.ThreadCount, threadDispatcher.ThreadCount, managedContext); taskStack.Dispose(pool, threadDispatcher); } @@ -631,6 +632,7 @@ public unsafe void GetSelfOverlaps2(ref TOverlapHandler results /// that the overlap test will push tasks onto as needed. /// Index of the worker calling the function. /// Number of tasks the overlap testing should try to create during execution. If negative, uses . + /// This does not dispatch workers on the directly. If the overlap handler requires managed context, that should be provided by whatever dispatched the workers. public unsafe void GetSelfOverlaps2(ref TOverlapHandler results, BufferPool pool, IThreadDispatcher threadDispatcher, TaskStack* taskStack, int workerIndex, int targetTaskCount = -1) where TOverlapHandler : unmanaged, IThreadedOverlapHandler { From 212de4359ce349f472e8d4834c0035bdf67db46b Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 9 Aug 2023 16:35:57 -0500 Subject: [PATCH 790/947] New (broken, disabled) broad phase test implemented. --- .../CollidableOverlapFinder.cs | 135 ++++++++++++++++++ 1 file changed, 135 insertions(+) diff --git a/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs b/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs index 6f99cac36..6e9d817af 100644 --- a/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs +++ b/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs @@ -6,6 +6,7 @@ using System.Runtime.CompilerServices; using System.Threading; using BepuPhysics.Trees; +using BepuUtilities.TaskScheduling; namespace BepuPhysics.CollisionDetection { @@ -102,6 +103,10 @@ void Worker(int workerIndex) } public override void DispatchOverlaps(float dt, IThreadDispatcher threadDispatcher = null) + { + DispatchOverlapsOld(dt, threadDispatcher); + } + public void DispatchOverlapsOld(float dt, IThreadDispatcher threadDispatcher = null) { if (threadDispatcher != null && threadDispatcher.ThreadCount > 1) { @@ -161,6 +166,136 @@ public override void DispatchOverlaps(float dt, IThreadDispatcher threadDispatch } + struct ThreadedIntertreeOverlapHandler : IThreadedOverlapHandler + { + public Buffer ActiveLeaves; + public Buffer StaticLeaves; + public void Handle(int indexA, int indexB, int workerIndex, object managedContext) + { + var narrowPhase = (NarrowPhase)managedContext; + narrowPhase.HandleOverlap(workerIndex, ActiveLeaves[indexA], StaticLeaves[indexB]); + } + } + struct ThreadedSelfOverlapHandler : IThreadedOverlapHandler + { + public Buffer Leaves; + public void Handle(int indexA, int indexB, int workerIndex, object managedContext) + { + var narrowPhase = (NarrowPhase)managedContext; + narrowPhase.HandleOverlap(workerIndex, Leaves[indexA], Leaves[indexB]); + } + } + + unsafe struct SelfContext + { + public TaskStack* Stack; + public ThreadedSelfOverlapHandler* Results; + public Tree Tree; + public int TargetTaskCount; + } + unsafe struct IntertreeContext + { + public TaskStack* Stack; + public ThreadedIntertreeOverlapHandler* Results; + public Tree StaticTree; + public Tree ActiveTree; + public int TargetTaskCount; + } + unsafe static void SelfEntryTask(long taskStartAndEnd, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) + { + ref var context = ref *(SelfContext*)untypedContext; + var pool = dispatcher.WorkerPools[workerIndex]; + context.Tree.GetSelfOverlaps2(ref *context.Results, pool, dispatcher, context.Stack, workerIndex, context.TargetTaskCount); + } + unsafe static void IntertreeEntryTask(long taskStartAndEnd, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) + { + ref var context = ref *(IntertreeContext*)untypedContext; + var pool = dispatcher.WorkerPools[workerIndex]; + context.ActiveTree.GetOverlaps2(ref context.StaticTree, ref *context.Results, pool, dispatcher, context.Stack, workerIndex, context.TargetTaskCount); + } + + public static void WorkerTask(int workerIndex, IThreadDispatcher dispatcher) + { + var taskStack = (TaskStack*)dispatcher.UnmanagedContext; + PopTaskResult popTaskResult; + var waiter = new SpinWait(); + while ((popTaskResult = taskStack->TryPopAndRun(workerIndex, dispatcher)) != PopTaskResult.Stop) + { + waiter.SpinOnce(-1); + } + ((NarrowPhase)dispatcher.ManagedContext).overlapWorkers[workerIndex].Batcher.Flush(); + } + public void DispatchOverlapsNew(float dt, IThreadDispatcher threadDispatcher = null) + { + //The number of collisions is usually some constant multiple of the number of active leaves. + //5-10 collisions per active leaf would represent a very dense simulation. + //The number of collisions needed to warrant the existence of a thread varies, too, but it's exceptionally unlikely that any you would need more than one thread for 64 collisions. + //(Those would have to be some very messed up pairs!) + //So, we'll allow worker counts to scale down with low leaf counts to avoid overhead for tiny simulations. + var maximumWorkerCount = int.Min(int.Max(1, broadPhase.ActiveTree.LeafCount / 8), threadDispatcher == null ? 1 : threadDispatcher.ThreadCount); + if (maximumWorkerCount > 1) + { + narrowPhase.Prepare(dt, threadDispatcher); + + if (broadPhase.ActiveTree.LeafCount > 0 && broadPhase.StaticTree.LeafCount > 0) + { + var selfResults = new ThreadedSelfOverlapHandler { Leaves = broadPhase.ActiveLeaves }; + var intertreeResults = new ThreadedIntertreeOverlapHandler { ActiveLeaves = broadPhase.ActiveLeaves, StaticLeaves = broadPhase.StaticLeaves }; + + //Note that we shouldn't reduce the task budget for testing with smallish trees, because it's very possible for the cost of individual narrow phase pairs to be high. + //The tree test cost is often small compared to the narrow phase costs, so it's worth keeping quite a few tasks around. + var selfTestCostEstimate = broadPhase.ActiveTree.LeafCount; + var intertreeTestCostEstimate = int.Max(broadPhase.ActiveTree.LeafCount, broadPhase.StaticTree.LeafCount); + + var selfTestAsFraction = (float)selfTestCostEstimate / (selfTestCostEstimate + intertreeTestCostEstimate); + //Regularize the budgets a bit. Don't let either get too small. + const float minimumBudget = 0.25f; + var intertreeTestAsFraction = 1f - (selfTestAsFraction * (1f - minimumBudget) + minimumBudget); + selfTestAsFraction = 1f - intertreeTestAsFraction; + + var selfTestTaskTarget = (int)float.Round(maximumWorkerCount * selfTestAsFraction); + var intertreeTestTaskTarget = maximumWorkerCount - selfTestTaskTarget; + var taskStack = new TaskStack(broadPhase.Pool, threadDispatcher, maximumWorkerCount); + var selfContext = new SelfContext { Results = &selfResults, Stack = &taskStack, TargetTaskCount = selfTestTaskTarget, Tree = broadPhase.ActiveTree }; + var intertreeContext = new IntertreeContext { Results = &intertreeResults, Stack = &taskStack, TargetTaskCount = intertreeTestTaskTarget, ActiveTree = broadPhase.ActiveTree, StaticTree = broadPhase.StaticTree }; + Span tasks = stackalloc Task[2]; + tasks[0] = new Task(&SelfEntryTask, &selfContext); + tasks[1] = new Task(&IntertreeEntryTask, &intertreeContext); + taskStack.AllocateContinuationAndPush(tasks, 0, threadDispatcher, onComplete: TaskStack.GetRequestStopTask(&taskStack)); + threadDispatcher.DispatchWorkers(&WorkerTask, maximumWorkerCount: maximumWorkerCount, unmanagedContext: &taskStack, managedContext: narrowPhase); + taskStack.Dispose(broadPhase.Pool, threadDispatcher); + } + else if (broadPhase.ActiveTree.LeafCount > 0) + { + var selfResults = new ThreadedSelfOverlapHandler { Leaves = broadPhase.ActiveLeaves }; + var intertreeResults = new ThreadedIntertreeOverlapHandler { ActiveLeaves = broadPhase.ActiveLeaves, StaticLeaves = broadPhase.StaticLeaves }; + + var taskStack = new TaskStack(broadPhase.Pool, threadDispatcher, maximumWorkerCount); + var selfContext = new SelfContext { Results = &selfResults, Stack = &taskStack, TargetTaskCount = maximumWorkerCount, Tree = broadPhase.ActiveTree }; + taskStack.AllocateContinuationAndPush(new Task(&SelfEntryTask, &selfContext), 0, threadDispatcher, onComplete: TaskStack.GetRequestStopTask(&taskStack)); + threadDispatcher.DispatchWorkers(&WorkerTask, maximumWorkerCount: maximumWorkerCount, unmanagedContext: &taskStack, managedContext: narrowPhase); + taskStack.Dispose(broadPhase.Pool, threadDispatcher); + } +#if DEBUG + for (int i = 1; i < threadDispatcher.ThreadCount; ++i) + { + Debug.Assert(!narrowPhase.overlapWorkers[i].Batcher.batches.Allocated, "After execution, there should be no remaining allocated collision batchers."); + } +#endif + } + else + { + narrowPhase.Prepare(dt); + var selfTestHandler = new SelfOverlapHandler(broadPhase.ActiveLeaves, narrowPhase, 0); + broadPhase.ActiveTree.GetSelfOverlaps2(ref selfTestHandler); + var intertreeHandler = new IntertreeOverlapHandler(broadPhase.ActiveLeaves, broadPhase.StaticLeaves, narrowPhase, 0); + broadPhase.ActiveTree.GetOverlaps(ref broadPhase.StaticTree, ref intertreeHandler); + narrowPhase.overlapWorkers[0].Batcher.Flush(); + + } + + } + } } From c109f4a572c1764092f7f218e47aee3db4e776ee Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 9 Aug 2023 21:46:36 -0500 Subject: [PATCH 791/947] Well, oops, turns out if you change a number in the old version it's faster. --- .../CollisionDetection/CollidableOverlapFinder.cs | 15 ++++++++++++--- BepuPhysics/Trees/Tree_IntertreeQueriesMT.cs | 2 +- BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs | 2 +- BepuPhysics/Trees/Tree_SelfQueriesMT.cs | 2 +- 4 files changed, 15 insertions(+), 6 deletions(-) diff --git a/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs b/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs index 6e9d817af..4b00767fe 100644 --- a/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs +++ b/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs @@ -181,7 +181,8 @@ struct ThreadedSelfOverlapHandler : IThreadedOverlapHandler public Buffer Leaves; public void Handle(int indexA, int indexB, int workerIndex, object managedContext) { - var narrowPhase = (NarrowPhase)managedContext; + //var narrowPhase = (NarrowPhase)managedContext; + var narrowPhase = Unsafe.As>(managedContext); narrowPhase.HandleOverlap(workerIndex, Leaves[indexA], Leaves[indexB]); } } @@ -203,12 +204,14 @@ unsafe struct IntertreeContext } unsafe static void SelfEntryTask(long taskStartAndEnd, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) { + Debug.Assert(dispatcher.ManagedContext != null); ref var context = ref *(SelfContext*)untypedContext; var pool = dispatcher.WorkerPools[workerIndex]; context.Tree.GetSelfOverlaps2(ref *context.Results, pool, dispatcher, context.Stack, workerIndex, context.TargetTaskCount); } unsafe static void IntertreeEntryTask(long taskStartAndEnd, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) { + Debug.Assert(dispatcher.ManagedContext != null); ref var context = ref *(IntertreeContext*)untypedContext; var pool = dispatcher.WorkerPools[workerIndex]; context.ActiveTree.GetOverlaps2(ref context.StaticTree, ref *context.Results, pool, dispatcher, context.Stack, workerIndex, context.TargetTaskCount); @@ -216,6 +219,7 @@ unsafe static void IntertreeEntryTask(long taskStartAndEnd, void* untypedContext public static void WorkerTask(int workerIndex, IThreadDispatcher dispatcher) { + Debug.Assert(dispatcher.ManagedContext != null); var taskStack = (TaskStack*)dispatcher.UnmanagedContext; PopTaskResult popTaskResult; var waiter = new SpinWait(); @@ -264,12 +268,17 @@ public void DispatchOverlapsNew(float dt, IThreadDispatcher threadDispatcher = n taskStack.AllocateContinuationAndPush(tasks, 0, threadDispatcher, onComplete: TaskStack.GetRequestStopTask(&taskStack)); threadDispatcher.DispatchWorkers(&WorkerTask, maximumWorkerCount: maximumWorkerCount, unmanagedContext: &taskStack, managedContext: narrowPhase); taskStack.Dispose(broadPhase.Pool, threadDispatcher); + + //var intertreeResults = new ThreadedIntertreeOverlapHandler { ActiveLeaves = broadPhase.ActiveLeaves, StaticLeaves = broadPhase.StaticLeaves }; + //var taskStack = new TaskStack(broadPhase.Pool, threadDispatcher, maximumWorkerCount); + //var intertreeContext = new IntertreeContext { Results = &intertreeResults, Stack = &taskStack, TargetTaskCount = maximumWorkerCount, ActiveTree = broadPhase.ActiveTree, StaticTree = broadPhase.StaticTree }; + //taskStack.AllocateContinuationAndPush(new Task(&IntertreeEntryTask, &intertreeContext), 0, threadDispatcher, onComplete: TaskStack.GetRequestStopTask(&taskStack)); + //threadDispatcher.DispatchWorkers(&WorkerTask, maximumWorkerCount: maximumWorkerCount, unmanagedContext: &taskStack, managedContext: narrowPhase); + //taskStack.Dispose(broadPhase.Pool, threadDispatcher); } else if (broadPhase.ActiveTree.LeafCount > 0) { var selfResults = new ThreadedSelfOverlapHandler { Leaves = broadPhase.ActiveLeaves }; - var intertreeResults = new ThreadedIntertreeOverlapHandler { ActiveLeaves = broadPhase.ActiveLeaves, StaticLeaves = broadPhase.StaticLeaves }; - var taskStack = new TaskStack(broadPhase.Pool, threadDispatcher, maximumWorkerCount); var selfContext = new SelfContext { Results = &selfResults, Stack = &taskStack, TargetTaskCount = maximumWorkerCount, Tree = broadPhase.ActiveTree }; taskStack.AllocateContinuationAndPush(new Task(&SelfEntryTask, &selfContext), 0, threadDispatcher, onComplete: TaskStack.GetRequestStopTask(&taskStack)); diff --git a/BepuPhysics/Trees/Tree_IntertreeQueriesMT.cs b/BepuPhysics/Trees/Tree_IntertreeQueriesMT.cs index de22b26ef..f116d687b 100644 --- a/BepuPhysics/Trees/Tree_IntertreeQueriesMT.cs +++ b/BepuPhysics/Trees/Tree_IntertreeQueriesMT.cs @@ -50,7 +50,7 @@ public unsafe void PrepareJobs(ref Tree treeA, ref Tree treeB, TOverlapHandler[] return; } Debug.Assert(overlapHandlers.Length >= threadCount); - const float jobMultiplier = 1.5f; + const float jobMultiplier = 2.5f; var targetJobCount = Math.Max(1, jobMultiplier * threadCount); //TODO: Not a lot of thought was put into this leaf threshold for intertree. Probably better options. leafThreshold = (int)((treeA.LeafCount + treeB.LeafCount) / targetJobCount); diff --git a/BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs b/BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs index be59cf524..dfc22f5c3 100644 --- a/BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs +++ b/BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs @@ -176,7 +176,7 @@ unsafe void GetOverlaps2(ref Tree treeB, ref TOverlapHandler re } else { - taskStack->Push(rootTask, workerIndex, threadDispatcher); + taskStack->RunTask(rootTask, workerIndex, threadDispatcher); } } //Copy back potential changes. diff --git a/BepuPhysics/Trees/Tree_SelfQueriesMT.cs b/BepuPhysics/Trees/Tree_SelfQueriesMT.cs index d08ead631..cd19fc7d9 100644 --- a/BepuPhysics/Trees/Tree_SelfQueriesMT.cs +++ b/BepuPhysics/Trees/Tree_SelfQueriesMT.cs @@ -80,7 +80,7 @@ public void PrepareJobs(ref Tree tree, TOverlapHandler[] overlapHandlers, int th return; } Debug.Assert(overlapHandlers.Length >= threadCount); - const float jobMultiplier = 1.5f; + const float jobMultiplier = 2.5f; var targetJobCount = Math.Max(1, jobMultiplier * threadCount); leafThreshold = (int)(tree.LeafCount / targetJobCount); jobs = new QuickList((int)(targetJobCount * 2), Pool); From b069646cfb92d13779a6bc2de22d1145ec760d72 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 9 Aug 2023 21:46:48 -0500 Subject: [PATCH 792/947] Missing convenience function. --- BepuUtilities/TaskScheduling/TaskStack.cs | 28 +++++++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/BepuUtilities/TaskScheduling/TaskStack.cs b/BepuUtilities/TaskScheduling/TaskStack.cs index f4b1ecd5a..6156b7606 100644 --- a/BepuUtilities/TaskScheduling/TaskStack.cs +++ b/BepuUtilities/TaskScheduling/TaskStack.cs @@ -417,6 +417,34 @@ public void RunTasks(Span tasks, int workerIndex, IThreadDispatcher dispat RunTasks(tasks, workerIndex, dispatcher, ref filter, tag); } + /// + /// Pushes a task to the worker stack and returns when it completes. + /// + /// Task composing the job. A continuation will be assigned internally; no continuation should be present on the task. + /// Index of the worker executing this function. + /// Thread dispatcher to allocate thread data from if necessary. + /// User tag associated with the job spanning the submitted task. + /// Note that this will keep working until the task completes. It may execute tasks unrelated to the requested task while waiting on other workers. + public void RunTask(Task task, int workerIndex, IThreadDispatcher dispatcher, ulong tag = 0) + { + RunTasks(new Span(ref task), workerIndex, dispatcher, tag); + } + + /// + /// Pushes a task to the worker stack and returns when all tasks are complete. + /// + /// Task composing the job. A continuation will be assigned internally; no continuation should be present on the task. + /// Index of the worker executing this function. + /// Thread dispatcher to allocate thread data from if necessary. + /// Filter applied to jobs considered for filling the calling thread's wait for other threads to complete. + /// User tag associated with the job spanning the submitted task. + /// Type of the job filter used in the pop. + /// Note that this will keep working the task completes. It may execute tasks unrelated to the requested task while waiting on other workers to complete constituent tasks. + public void RunTask(Task task, int workerIndex, IThreadDispatcher dispatcher, ref TJobFilter filter, ulong tag = 0) where TJobFilter : IJobFilter + { + RunTasks(new Span(ref task), workerIndex, dispatcher, ref filter, tag); + } + /// /// Requests that all workers stop. The next time a worker runs out of tasks to run, if it sees a stop command, it will be reported. /// From 299ed8919c6c9d976cb57c157596b5772a51f14e Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 10 Aug 2023 12:35:17 -0500 Subject: [PATCH 793/947] Binned build with 1 leaf early early out fix. --- BepuPhysics/Collidables/BigCompound.cs | 3 ++- BepuPhysics/Collidables/Mesh.cs | 3 ++- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 7 +++++++ 3 files changed, 11 insertions(+), 2 deletions(-) diff --git a/BepuPhysics/Collidables/BigCompound.cs b/BepuPhysics/Collidables/BigCompound.cs index ec15527c4..52e7c677a 100644 --- a/BepuPhysics/Collidables/BigCompound.cs +++ b/BepuPhysics/Collidables/BigCompound.cs @@ -44,7 +44,8 @@ public static BigCompound CreateWithoutTreeBuild(Buffer children, //If this codepath is being used, we're assuming that the children are as given. //so we can go ahead and set the node/leaf counts. //(This is in contrast to creating a tree with a certain capacity, but then relying on incremental adds/removes later.) - NodeCount = children.Length - 1, + //Note that the tree still has a root node even if there's one leaf; it's a partial node and requires special handling. + NodeCount = int.Max(1, children.Length - 1), LeafCount = children.Length }; return compound; diff --git a/BepuPhysics/Collidables/Mesh.cs b/BepuPhysics/Collidables/Mesh.cs index 0c420b5a8..ed0e8516c 100644 --- a/BepuPhysics/Collidables/Mesh.cs +++ b/BepuPhysics/Collidables/Mesh.cs @@ -105,7 +105,8 @@ public static Mesh CreateWithoutTreeBuild(Buffer triangles, Vector3 sc //If this codepath is being used, we're assuming that the triangles are going to be the actual children //so we can go ahead and set the node/leaf counts. //(This is in contrast to creating a tree with a certain capacity, but then relying on incremental adds/removes later.) - NodeCount = triangles.Length - 1, + //Note that the tree still has a root node even if there's one leaf; it's a partial node and requires special handling. + NodeCount = int.Max(1, triangles.Length - 1), LeafCount = triangles.Length }; mesh.Scale = scale; diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 5eb5bb589..f1789e99a 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -1507,6 +1507,13 @@ public static unsafe void BinnedBuild(Buffer subtrees, Buffer n BufferPool pool = null, IThreadDispatcher dispatcher = null, TaskStack* taskStackPointer = null, int workerIndex = 0, int workerCount = -1, int targetTaskCount = -1, int maximumSubtreeStackAllocationCount = 4096, int minimumBinCount = 16, int maximumBinCount = 64, float leafToBinMultiplier = 1 / 16f, int microsweepThreshold = 64, bool deterministic = false) { + if (subtrees.Length <= 2) + { + //No need to do anything fancy, all subtrees fit in the root. Requires a special case for the partial root. + nodes[0] = new Node { A = subtrees[0], B = subtrees.Length == 2 ? subtrees[1] : default }; + metanodes[0] = new Metanode { Parent = -1, IndexInParent = -1 }; + return; + } if (dispatcher != null && pool == null) throw new ArgumentException("If a ThreadDispatcher has been given to BinnedBuild, a BufferPool must also be provided."); Buffer subtreesPong; From a354d0d92d9411fd4949822ea22aac0ad540765e Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 12 Aug 2023 15:22:25 -0500 Subject: [PATCH 794/947] Added priority queue based root refinement subtree selection to match earlier version. Dramatically improves recovery from pathological trees. --- BepuPhysics/CollisionDetection/BroadPhase.cs | 32 ++- BepuPhysics/Trees/Tree_Refine2.cs | 239 +++++++++++++++++-- 2 files changed, 238 insertions(+), 33 deletions(-) diff --git a/BepuPhysics/CollisionDetection/BroadPhase.cs b/BepuPhysics/CollisionDetection/BroadPhase.cs index 08b12ae2d..31433ac35 100644 --- a/BepuPhysics/CollisionDetection/BroadPhase.cs +++ b/BepuPhysics/CollisionDetection/BroadPhase.cs @@ -246,7 +246,8 @@ public void Update(IThreadDispatcher threadDispatcher = null) /// Size of the root refinement. If zero or negative, no root refinement will be performed. /// Number of subtree refinements to perform. Can be zero. /// Target size of the subtree refinements. - public delegate void RefinementScheduler(int frameIndex, in Tree tree, out int rootRefinementSize, out int subtreeRefinementCount, out int subtreeRefinementSize); + /// True if the root refinement should use a priority queue during subtree collection to find larger nodes, false if it should try to collect a more balanced tree. + public delegate void RefinementScheduler(int frameIndex, in Tree tree, out int rootRefinementSize, out int subtreeRefinementCount, out int subtreeRefinementSize, out bool usePriorityQueue); /// /// Gets or sets the refinement schedule to use for the active tree. @@ -264,13 +265,15 @@ public void Update(IThreadDispatcher threadDispatcher = null) /// Period, in timesteps, of refinements applied to the root. /// Multiplier to apply to the square root of the leaf count to get the target root refinement size. /// Multiplier to apply to the square root of the leaf count to get the target subtree refinement size. + /// The period between non-priority queue based root refinements, measured in units of root refinements. /// Index of the frame as tracked by the broad phase. /// Tree being considered for refinement. /// Size of the root refinement. If zero or negative, no root refinement will be performed. /// Number of subtree refinements to perform. Can be zero. /// Target size of the subtree refinements. - public static void DefaultRefinementScheduler(float optimizationFraction, int rootRefinementPeriod, float rootRefinementSizeScale, float subtreeRefinementSizeScale, - int frameIndex, in Tree tree, out int rootRefinementSize, out int subtreeRefinementCount, out int subtreeRefinementSize) + /// True if the root refinement should use a priority queue during subtree collection to find larger nodes, false if it should try to collect a more balanced tree. + public static void DefaultRefinementScheduler(float optimizationFraction, int rootRefinementPeriod, float rootRefinementSizeScale, float subtreeRefinementSizeScale, int nonpriorityPeriod, + int frameIndex, in Tree tree, out int rootRefinementSize, out int subtreeRefinementCount, out int subtreeRefinementSize, out bool usePriorityQueue) { var refineRoot = frameIndex % rootRefinementPeriod == 0; var targetOptimizedLeafCount = (int)float.Ceiling(tree.LeafCount * optimizationFraction); @@ -283,11 +286,13 @@ public static void DefaultRefinementScheduler(float optimizationFraction, int ro var targetRootRefinementSize = (int)float.Ceiling(sqrtLeafCount * rootRefinementSizeScale); subtreeRefinementSize = (int)float.Ceiling(sqrtLeafCount * subtreeRefinementSizeScale); + //Note that we scale up the cost of the root refinement; it uses a sequentialized priority queue to collect subtrees for refinement and costs more. var subtreeRefinementsPerRootRefinement = (int)float.Ceiling(subtreeRefinementSize * float.Log2(subtreeRefinementSize) / (targetRootRefinementSize * float.Log2(targetRootRefinementSize))); //If we're refining the root, reduce the number of subtree refinements to avoid cost spikes. subtreeRefinementCount = int.Max(0, (int)float.Ceiling((float)targetOptimizedLeafCount / subtreeRefinementSize) - (refineRoot ? subtreeRefinementsPerRootRefinement : 0)); rootRefinementSize = refineRoot ? targetRootRefinementSize : 0; + usePriorityQueue = (frameIndex / rootRefinementPeriod) % nonpriorityPeriod != 0; } /// @@ -298,9 +303,10 @@ public static void DefaultRefinementScheduler(float optimizationFraction, int ro /// Size of the root refinement. If zero or negative, no root refinement will be performed. /// Number of subtree refinements to perform. Can be zero. /// Target size of the subtree refinements. - public static void DefaultActiveRefinementScheduler(int frameIndex, in Tree tree, out int rootRefinementSize, out int subtreeRefinementCount, out int subtreeRefinementSize) + /// True if the root refinement should use a priority queue during subtree collection to find larger nodes, false if it should try to collect a more balanced tree. + public static void DefaultActiveRefinementScheduler(int frameIndex, in Tree tree, out int rootRefinementSize, out int subtreeRefinementCount, out int subtreeRefinementSize, out bool usePriorityQueue) { - DefaultRefinementScheduler(1f / 20f, 8, 4, 4, frameIndex, tree, out rootRefinementSize, out subtreeRefinementCount, out subtreeRefinementSize); + DefaultRefinementScheduler(1f / 20f, 2, 1, 4, 16, frameIndex, tree, out rootRefinementSize, out subtreeRefinementCount, out subtreeRefinementSize, out usePriorityQueue); } @@ -312,9 +318,10 @@ public static void DefaultActiveRefinementScheduler(int frameIndex, in Tree tree /// Size of the root refinement. If zero or negative, no root refinement will be performed. /// Number of subtree refinements to perform. Can be zero. /// Target size of the subtree refinements. - public static void DefaultStaticRefinementScheduler(int frameIndex, in Tree tree, out int rootRefinementSize, out int subtreeRefinementCount, out int subtreeRefinementSize) + /// True if the root refinement should use a priority queue during subtree collection to find larger nodes, false if it should try to collect a more balanced tree. + public static void DefaultStaticRefinementScheduler(int frameIndex, in Tree tree, out int rootRefinementSize, out int subtreeRefinementCount, out int subtreeRefinementSize, out bool usePriorityQueue) { - DefaultRefinementScheduler(1f / 100f, 32, 4, 4, frameIndex, tree, out rootRefinementSize, out subtreeRefinementCount, out subtreeRefinementSize); + DefaultRefinementScheduler(1f / 100f, 2, 1, 4, 16, frameIndex, tree, out rootRefinementSize, out subtreeRefinementCount, out subtreeRefinementSize, out usePriorityQueue); } struct RefinementContext @@ -328,6 +335,7 @@ struct RefinementContext public int SubtreeRefinementSize; public int SubtreeRefinementStartIndex; public bool Deterministic; + public bool UsePriorityQueue; //Used for active tree. Didn't split the type because whocares. public Buffer TargetNodes; @@ -337,7 +345,7 @@ static void ActiveEntrypointTask(long taskId, void* untypedContext, int workerIn { ref var context = ref *(RefinementContext*)untypedContext; var pool = dispatcher.WorkerPools[workerIndex]; - context.Tree.Refine2(context.RootRefinementSize, ref context.SubtreeRefinementStartIndex, context.SubtreeRefinementCount, context.SubtreeRefinementSize, pool, dispatcher, context.TaskStack, workerIndex, targetTaskCount: context.TargetTaskCount, deterministic: context.Deterministic); + context.Tree.Refine2(context.RootRefinementSize, ref context.SubtreeRefinementStartIndex, context.SubtreeRefinementCount, context.SubtreeRefinementSize, pool, dispatcher, context.TaskStack, workerIndex, targetTaskCount: context.TargetTaskCount, deterministic: context.Deterministic, usePriorityQueue: context.UsePriorityQueue); //Now refit! Note that we use all but one task. It doesn't affect the performance of a refit much (we're not compute bound), and we can use it to do an incremental cache optimization on the static tree. var sourceNodes = context.Tree.Nodes; context.Tree.Nodes = context.TargetNodes; @@ -348,14 +356,14 @@ static void StaticEntrypointTask(long taskId, void* untypedContext, int workerIn { ref var context = ref *(RefinementContext*)untypedContext; var pool = dispatcher.WorkerPools[workerIndex]; - context.Tree.Refine2(context.RootRefinementSize, ref context.SubtreeRefinementStartIndex, context.SubtreeRefinementCount, context.SubtreeRefinementSize, pool, dispatcher, context.TaskStack, workerIndex, targetTaskCount: context.TargetTaskCount, deterministic: context.Deterministic); + context.Tree.Refine2(context.RootRefinementSize, ref context.SubtreeRefinementStartIndex, context.SubtreeRefinementCount, context.SubtreeRefinementSize, pool, dispatcher, context.TaskStack, workerIndex, targetTaskCount: context.TargetTaskCount, deterministic: context.Deterministic, usePriorityQueue: context.UsePriorityQueue); } int staticSubtreeRefinementStartIndex, activeSubtreeRefinementStartIndex; public void Update2(IThreadDispatcher threadDispatcher = null, bool deterministic = false) { - ActiveRefinementSchedule(frameIndex, ActiveTree, out var activeRootRefinementSize, out var activeSubtreeRefinementCount, out var activeSubtreeRefinementSize); - StaticRefinementSchedule(frameIndex, StaticTree, out var staticRootRefinementSize, out var staticSubtreeRefinementCount, out var staticSubtreeRefinementSize); + ActiveRefinementSchedule(frameIndex, ActiveTree, out var activeRootRefinementSize, out var activeSubtreeRefinementCount, out var activeSubtreeRefinementSize, out var usePriorityQueueActive); + StaticRefinementSchedule(frameIndex, StaticTree, out var staticRootRefinementSize, out var staticSubtreeRefinementCount, out var staticSubtreeRefinementSize, out var usePriorityQueueStatic); const int minimumLeafCountForThreading = 256; if (threadDispatcher != null && threadDispatcher.ThreadCount > 1 && (ActiveTree.LeafCount >= minimumLeafCountForThreading || StaticTree.LeafCount >= minimumLeafCountForThreading)) { @@ -379,6 +387,7 @@ public void Update2(IThreadDispatcher threadDispatcher = null, bool deterministi SubtreeRefinementSize = activeSubtreeRefinementSize, SubtreeRefinementStartIndex = activeSubtreeRefinementStartIndex, Deterministic = deterministic, + UsePriorityQueue = usePriorityQueueActive, TargetNodes = ActiveTree.LeafCount > 2 ? new Buffer(ActiveTree.Nodes.Length, Pool) : default, }; var staticRefineContext = new RefinementContext @@ -392,6 +401,7 @@ public void Update2(IThreadDispatcher threadDispatcher = null, bool deterministi SubtreeRefinementSize = staticSubtreeRefinementSize, SubtreeRefinementStartIndex = staticSubtreeRefinementStartIndex, Deterministic = deterministic, + UsePriorityQueue = usePriorityQueueStatic, }; Span tasks = stackalloc Task[2]; tasks[0] = new Task(&ActiveEntrypointTask, &activeRefineContext); diff --git a/BepuPhysics/Trees/Tree_Refine2.cs b/BepuPhysics/Trees/Tree_Refine2.cs index 311303406..b1fddd74d 100644 --- a/BepuPhysics/Trees/Tree_Refine2.cs +++ b/BepuPhysics/Trees/Tree_Refine2.cs @@ -11,6 +11,7 @@ using System.Threading; using System.Threading.Tasks; using System.Xml.Linq; +using static BepuPhysics.Collidables.CompoundBuilder; using Task = BepuUtilities.TaskScheduling.Task; namespace BepuPhysics.Trees; @@ -306,13 +307,15 @@ private static void TryPushChildForRootRefinement( } } - - static unsafe void CollectSubtreesForRootRefinement(ref QuickList<(int nodeIndex, int subtreeBudget)> stack, int subtreeRefinementSize, BufferPool pool, Buffer> subtreeRefinementTargetBundles, ref Tree tree, ref QuickList rootRefinementNodeIndices, ref QuickList rootRefinementSubtrees) + unsafe void CollectSubtreesForRootRefinement(int rootRefinementSize, int subtreeRefinementSize, BufferPool pool, in QuickList subtreeRefinementTargets, ref QuickList rootRefinementNodeIndices, ref QuickList rootRefinementSubtrees) { - while (stack.TryPop(out var nodeToVisit)) + var rootStack = new QuickList<(int nodeIndex, int subtreeBudget)>(rootRefinementSize, pool); + rootStack.AllocateUnsafely() = (0, rootRefinementSize); + var subtreeRefinementTargetBundles = new Buffer>(subtreeRefinementTargets.Span.Memory, BundleIndexing.GetBundleCount(subtreeRefinementTargets.Count)); + while (rootStack.TryPop(out var nodeToVisit)) { rootRefinementNodeIndices.AllocateUnsafely() = nodeToVisit.nodeIndex; - ref var node = ref tree.Nodes[nodeToVisit.nodeIndex]; + ref var node = ref Nodes[nodeToVisit.nodeIndex]; var nodeTotalLeafCount = node.A.LeafCount + node.B.LeafCount; Debug.Assert(nodeToVisit.subtreeBudget <= nodeTotalLeafCount); var lowerSubtreeBudget = int.Min((nodeToVisit.subtreeBudget + 1) / 2, int.Min(node.A.LeafCount, node.B.LeafCount)); @@ -321,18 +324,198 @@ static unsafe void CollectSubtreesForRootRefinement(ref QuickList<(int nodeIndex var aSubtreeBudget = useSmallerForA ? lowerSubtreeBudget : higherSubtreeBudget; var bSubtreeBudget = useSmallerForA ? higherSubtreeBudget : lowerSubtreeBudget; - TryPushChildForRootRefinement(subtreeRefinementSize, subtreeRefinementTargetBundles, nodeTotalLeafCount, bSubtreeBudget, node.B, ref stack, ref rootRefinementSubtrees); - TryPushChildForRootRefinement(subtreeRefinementSize, subtreeRefinementTargetBundles, nodeTotalLeafCount, aSubtreeBudget, node.A, ref stack, ref rootRefinementSubtrees); + TryPushChildForRootRefinement(subtreeRefinementSize, subtreeRefinementTargetBundles, nodeTotalLeafCount, bSubtreeBudget, node.B, ref rootStack, ref rootRefinementSubtrees); + TryPushChildForRootRefinement(subtreeRefinementSize, subtreeRefinementTargetBundles, nodeTotalLeafCount, aSubtreeBudget, node.A, ref rootStack, ref rootRefinementSubtrees); } + rootStack.Dispose(pool); } - unsafe void CollectSubtreesForRootRefinement(int rootRefinementSize, int subtreeRefinementSize, BufferPool pool, in QuickList subtreeRefinementTargets, ref QuickList rootRefinementNodeIndices, ref QuickList rootRefinementSubtrees) + internal struct HeapEntry { - var rootStack = new QuickList<(int nodeIndex, int subtreeBudget)>(rootRefinementSize, pool); - rootStack.AllocateUnsafely() = (0, rootRefinementSize); + public int Index; + public float Cost; + } + unsafe internal struct BinaryHeap + { + public Buffer Entries; + public int Count; + + public BinaryHeap(Buffer entries) + { + Entries = entries; + Count = 0; + } + + public BinaryHeap(int capacity, BufferPool pool) : this(new Buffer(capacity, pool)) { } + + public void Dispose(BufferPool pool) + { + pool.Return(ref Entries); + } + + public unsafe void Insert(int indexToInsert, float cost) + { + int index = Count; + ++Count; + //Sift up. + while (index > 0) + { + var parentIndex = (index - 1) >> 1; + var parent = Entries[parentIndex]; + if (parent.Cost < cost) + { + //Pull the parent down. + Entries[index] = parent; + index = parentIndex; + } + else + { + //Found the insertion spot. + break; + } + } + ref var entry = ref Entries[index]; + entry.Index = indexToInsert; + entry.Cost = cost; + } + + + public HeapEntry Pop() + { + var entry = Entries[0]; + --Count; + var cost = Entries[Count].Cost; + + //Pull the elements up to fill in the gap. + int index = 0; + while (true) + { + var childIndexA = (index << 1) + 1; + var childIndexB = (index << 1) + 2; + if (childIndexB < Count) + { + //Both children are available. + //Try swapping with the largest one. + var childA = Entries[childIndexA]; + var childB = Entries[childIndexB]; + if (childA.Cost > childB.Cost) + { + if (cost > childA.Cost) + { + break; + } + Entries[index] = Entries[childIndexA]; + index = childIndexA; + } + else + { + if (cost > childB.Cost) + { + break; + } + Entries[index] = Entries[childIndexB]; + index = childIndexB; + } + } + else if (childIndexA < Count) + { + //Only one child was available. + ref var childA = ref Entries[childIndexA]; + if (cost > childA.Cost) + { + break; + } + Entries[index] = Entries[childIndexA]; + index = childIndexA; + } + else + { + //The children were beyond the heap. + break; + } + } + //Move the last entry into position. + Entries[index] = Entries[Count]; + return entry; + } + + } + + /// + /// Checks if a child should be a subtree in the root refinement. If so, it's added to the list. Otherwise, it's pushed onto the stack. + /// + private void TryPushChildForRootRefinement2( + int subtreeRefinementSize, int nodeTotalLeafCount, Buffer> subtreeRefinementRootBundles, ref NodeChild child, ref BinaryHeap heap, ref QuickList rootRefinementSubtrees) + { + if (child.Index < 0) + { + //It's a leaf node; directly accept it. + rootRefinementSubtrees.AllocateUnsafely() = child; + } + else + { + //Internal node; is it a subtree refinement? + if (IsNodeChildSubtreeRefinementTarget(subtreeRefinementRootBundles, child, nodeTotalLeafCount, subtreeRefinementSize)) + { + //Yup! + ref var allocatedChild = ref rootRefinementSubtrees.AllocateUnsafely(); + allocatedChild = child; + //Internal nodes used as subtrees by the root refinement are flagged so that the reification process knows to stop. + Debug.Assert(allocatedChild.Index < flagForRootRefinementSubtree, "The use of an upper index bit as flag means the binned refiner cannot handle trees with billions of children."); + allocatedChild.Index |= flagForRootRefinementSubtree; + } + else + { + //A regular internal node; push it. + //TODO: The heuristic for cost is pretty simple: the bounds metric of the child in isolation. + //The root collection will always visit the *biggest* nodes. That's often a good idea, but not always. + //A couple of potential improvements to consider: + //1. Use boundsMetric * leafCount. This will tend to push nodes with lots of leaves to the top of the heap. + //2. Dive one step deeper and compute the ratio of the bounds metric of the children to the parent. Nodes that do a poor job of splitting the space will tend to have a higher ratio. + //#1 is trivial. #2 requires touching a little more memory. Worth investigating. + //(Just doing this for now to match the old implementation.) + //var childBoundsMetric = ComputeBoundsMetric(Unsafe.As(ref child)); + //ref var childNode = ref Nodes[child.Index]; + //var grandchildABoundsMetric = ComputeBoundsMetric(Unsafe.As(ref childNode.A)); + //var grandchildBBoundsMetric = ComputeBoundsMetric(Unsafe.As(ref childNode.B)); + //var cost = (grandchildABoundsMetric * childNode.A.LeafCount + grandchildBBoundsMetric * childNode.B.LeafCount); + //heap.Insert(child.Index, cost); + heap.Insert(child.Index, ComputeBoundsMetric(Unsafe.As(ref child))); + } + } + } + unsafe void CollectSubtreesForRootRefinementWithPriorityQueue(int rootRefinementSize, int subtreeRefinementSize, BufferPool pool, in QuickList subtreeRefinementTargets, ref QuickList rootRefinementNodeIndices, ref QuickList rootRefinementSubtrees) + { + //Instead of using a breadth first search, we greedily expand the root refinement by looking for the next node with the highest cost. + //This will tend to force the root refinement to find pathologically bad subtrees rapidly. + var heap = new BinaryHeap(new Buffer(rootRefinementSize, pool)); + heap.Insert(0, 0); //no need to actually calculate the cost for the root; it's gonna get popped. var subtreeRefinementTargetBundles = new Buffer>(subtreeRefinementTargets.Span.Memory, BundleIndexing.GetBundleCount(subtreeRefinementTargets.Count)); - CollectSubtreesForRootRefinement(ref rootStack, subtreeRefinementSize, pool, subtreeRefinementTargetBundles, ref this, ref rootRefinementNodeIndices, ref rootRefinementSubtrees); - rootStack.Dispose(pool); + while (heap.Count > 0 && heap.Count + rootRefinementSubtrees.Count < rootRefinementSize) + { + var entry = heap.Pop(); + rootRefinementNodeIndices.AllocateUnsafely() = entry.Index; + ref var node = ref Nodes[entry.Index]; + var nodeTotalLeafCount = node.A.LeafCount + node.B.LeafCount; + + TryPushChildForRootRefinement2(subtreeRefinementSize, nodeTotalLeafCount, subtreeRefinementTargetBundles, ref node.B, ref heap, ref rootRefinementSubtrees); + TryPushChildForRootRefinement2(subtreeRefinementSize, nodeTotalLeafCount, subtreeRefinementTargetBundles, ref node.A, ref heap, ref rootRefinementSubtrees); + } + //The traversal has added any subtrees that represent either 1. a leaf or 2. a subtree refinement target. + //The heap contains all the other subtrees that need to be added in index form; add them all now. + for (int i = 0; i < heap.Count; ++i) + { + var entry = heap.Entries[i]; + var metanode = Metanodes[entry.Index]; + Debug.Assert(metanode.Parent >= 0, "The root should never show up in the heap post traversal! Something weird has happened."); + ref var parent = ref Nodes[metanode.Parent]; + ref var childInParent = ref Unsafe.Add(ref parent.A, metanode.IndexInParent); + ref var allocated = ref rootRefinementSubtrees.AllocateUnsafely(); + Debug.Assert(childInParent.Index >= 0, "Anything in the heap should be an internal node."); + allocated = childInParent; + allocated.Index |= flagForRootRefinementSubtree; + } + heap.Entries.Dispose(pool); } void CollectSubtreesForSubtreeRefinement(int refinementTargetRootNodeIndex, Buffer subtreeStackBuffer, ref QuickList subtreeRefinementNodeIndices, ref QuickList subtreeRefinementLeaves) @@ -362,9 +545,10 @@ void CollectSubtreesForSubtreeRefinement(int refinementTargetRootNodeIndex, Buff /// Index used to distribute subtree refinements over multiple executions. /// Number of subtree refinements to execute. /// Target size of subtree refinements. The actual size of refinement will usually be larger or smaller. - /// Pool used for ephemeral allocations during the refinement. + /// Pool used for ephemeral allocations during the refinement. + /// True if the root refinement should use a priority queue during subtree collection to find larger nodes, false if it should try to collect a more balanced tree. /// Nodes will not be refit. - public unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementStartIndex, int subtreeRefinementCount, int subtreeRefinementSize, BufferPool pool) + public unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementStartIndex, int subtreeRefinementCount, int subtreeRefinementSize, BufferPool pool, bool usePriorityQueue = true) { //No point refining anything with two leaves. This condition also avoids having to special case for an incomplete root node. if (LeafCount <= 2) @@ -384,7 +568,10 @@ public unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementStar { var rootRefinementSubtrees = new QuickList(rootRefinementSize, pool); var rootRefinementNodeIndices = new QuickList(rootRefinementSize, pool); - CollectSubtreesForRootRefinement(rootRefinementSize, subtreeRefinementSize, pool, subtreeRefinementTargets, ref rootRefinementNodeIndices, ref rootRefinementSubtrees); + if (usePriorityQueue) + CollectSubtreesForRootRefinementWithPriorityQueue(rootRefinementSize, subtreeRefinementSize, pool, subtreeRefinementTargets, ref rootRefinementNodeIndices, ref rootRefinementSubtrees); + else + CollectSubtreesForRootRefinement(rootRefinementSize, subtreeRefinementSize, pool, subtreeRefinementTargets, ref rootRefinementNodeIndices, ref rootRefinementSubtrees); //Now that we have a list of nodes to refine, we can run the root refinement. Debug.Assert(rootRefinementNodeIndices.Count == rootRefinementSubtrees.Count - 1); @@ -433,6 +620,7 @@ unsafe struct RefinementContext public QuickList SubtreeRefinementTargets; public TaskStack* TaskStack; public bool Deterministic; + public bool UsePriorityQueue; /// /// This is a *copy* of the original tree that spawned this refinement. Refinements do not modify the memory stored at the level of the Tree, only memory *pointed* to by the tree. /// @@ -455,9 +643,13 @@ unsafe static void ExecuteRootRefinementTask(long id, void* untypedContext, int // 1. The cost of the collection phase is pretty cheap. Around the cost of a refit. Multithreading a collection phase of a thousand nodes is probably going to be net slower. // 2. It's difficult to do it deterministically without having to do a postpass to copy things into position in the contiguous buffer, and touching all that memory is a big hit. // 3. The main use case for refinements is in the broad phase. This will usually be run next to a dynamic refit refine, so we'll already have decent utilization. - // 4. I don't wanna. + // 4. Doing it for the priority queue variant is even harder. + // 5. I don't wanna. //So, punting this for later. A nondeterministic implementation wouldn't be too bad. Could always just fall back to ST when deterministic flag is set. - context.Tree.CollectSubtreesForRootRefinement(context.RootRefinementSize, context.SubtreeRefinementSize, pool, context.SubtreeRefinementTargets, ref rootRefinementNodeIndices, ref rootRefinementSubtrees); + if (context.UsePriorityQueue) + context.Tree.CollectSubtreesForRootRefinementWithPriorityQueue(context.RootRefinementSize, context.SubtreeRefinementSize, pool, context.SubtreeRefinementTargets, ref rootRefinementNodeIndices, ref rootRefinementSubtrees); + else + context.Tree.CollectSubtreesForRootRefinement(context.RootRefinementSize, context.SubtreeRefinementSize, pool, context.SubtreeRefinementTargets, ref rootRefinementNodeIndices, ref rootRefinementSubtrees); //Now that we have a list of nodes to refine, we can run the root refinement. Debug.Assert(rootRefinementNodeIndices.Count == rootRefinementSubtrees.Count - 1); @@ -515,7 +707,7 @@ unsafe static void ExecuteSubtreeRefinementTask(long subtreeRefinementTarget, vo } - private unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementStartIndex, int subtreeRefinementCount, int subtreeRefinementSize, BufferPool pool, int workerIndex, TaskStack* taskStack, IThreadDispatcher threadDispatcher, bool internallyDispatch, int workerCount, int targetTaskBudget, bool deterministic) + private unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementStartIndex, int subtreeRefinementCount, int subtreeRefinementSize, BufferPool pool, int workerIndex, TaskStack* taskStack, IThreadDispatcher threadDispatcher, bool internallyDispatch, int workerCount, int targetTaskBudget, bool deterministic, bool usePriorityQueue) { //No point refining anything with two leaves. This condition also avoids having to special case for an incomplete root node. if (LeafCount <= 2) @@ -559,6 +751,7 @@ private unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementSta TaskStack = taskStack, WorkerCount = workerCount, Deterministic = deterministic, + UsePriorityQueue = usePriorityQueue, Tree = this }; for (int i = 0; i < subtreeRefinementTargets.Count; ++i) @@ -594,11 +787,12 @@ private unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementSta /// Thread dispatcher used during the refinement. /// Whether to force determinism at a slightly higher cost when using internally multithreaded execution for an individual refinement operation. /// If the refine is single threaded, it is already deterministic and this flag has no effect. + /// True if the root refinement should use a priority queue during subtree collection to find larger nodes, false if it should try to collect a more balanced tree. /// Nodes will not be refit. - public unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementStartIndex, int subtreeRefinementCount, int subtreeRefinementSize, BufferPool pool, IThreadDispatcher threadDispatcher, bool deterministic = false) + public unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementStartIndex, int subtreeRefinementCount, int subtreeRefinementSize, BufferPool pool, IThreadDispatcher threadDispatcher, bool deterministic = false, bool usePriorityQueue = true) { var taskStack = new TaskStack(pool, threadDispatcher, threadDispatcher.ThreadCount); - Refine2(rootRefinementSize, ref subtreeRefinementStartIndex, subtreeRefinementCount, subtreeRefinementSize, pool, 0, &taskStack, threadDispatcher, true, threadDispatcher.ThreadCount, threadDispatcher.ThreadCount, deterministic); + Refine2(rootRefinementSize, ref subtreeRefinementStartIndex, subtreeRefinementCount, subtreeRefinementSize, pool, 0, &taskStack, threadDispatcher, true, threadDispatcher.ThreadCount, threadDispatcher.ThreadCount, deterministic, usePriorityQueue); taskStack.Dispose(pool, threadDispatcher); } @@ -618,10 +812,11 @@ public unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementStar /// that the refine operation will push tasks onto as needed. /// Index of the worker calling the function. /// Number of tasks the refinement should try to create during execution. If negative, uses . + /// True if the root refinement should use a priority queue during subtree collection to find larger nodes, false if it should try to collect a more balanced tree. /// Nodes will not be refit. - public unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementStartIndex, int subtreeRefinementCount, int subtreeRefinementSize, - BufferPool pool, IThreadDispatcher threadDispatcher, TaskStack* taskStack, int workerIndex, int targetTaskCount = -1, bool deterministic = false) + public unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementStartIndex, int subtreeRefinementCount, int subtreeRefinementSize, + BufferPool pool, IThreadDispatcher threadDispatcher, TaskStack* taskStack, int workerIndex, int targetTaskCount = -1, bool deterministic = false, bool usePriorityQueue = true) { - Refine2(rootRefinementSize, ref subtreeRefinementStartIndex, subtreeRefinementCount, subtreeRefinementSize, pool, workerIndex, taskStack, threadDispatcher, false, threadDispatcher.ThreadCount, targetTaskCount, deterministic); + Refine2(rootRefinementSize, ref subtreeRefinementStartIndex, subtreeRefinementCount, subtreeRefinementSize, pool, workerIndex, taskStack, threadDispatcher, false, threadDispatcher.ThreadCount, targetTaskCount, deterministic, usePriorityQueue); } } From e7f4ea70eadbc86f95727cc1d1b2583f5f69a1da Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 12 Aug 2023 21:11:28 -0500 Subject: [PATCH 795/947] New add variant. Simpler, tends to have slightly better SAH, though it's still insertion. Then a binned build refiner variant that's too slow to be practical, but does help pathologically ordered stuff. It's gonna disappear, but maybe something will replace it! --- BepuPhysics/CollisionDetection/BroadPhase.cs | 2 +- BepuPhysics/Trees/Tree.cs | 7 + BepuPhysics/Trees/Tree_Add2.cs | 187 +++++++++++++++++++ 3 files changed, 195 insertions(+), 1 deletion(-) create mode 100644 BepuPhysics/Trees/Tree_Add2.cs diff --git a/BepuPhysics/CollisionDetection/BroadPhase.cs b/BepuPhysics/CollisionDetection/BroadPhase.cs index 31433ac35..3695873e5 100644 --- a/BepuPhysics/CollisionDetection/BroadPhase.cs +++ b/BepuPhysics/CollisionDetection/BroadPhase.cs @@ -63,7 +63,7 @@ public BroadPhase(BufferPool pool, int initialActiveLeafCapacity = 4096, int ini [MethodImpl(MethodImplOptions.AggressiveInlining)] private static int Add(CollidableReference collidable, ref BoundingBox bounds, ref Tree tree, BufferPool pool, ref Buffer leaves) { - var leafIndex = tree.Add(bounds, pool); + var leafIndex = tree.Add2(bounds, pool); if (leafIndex >= leaves.Length) { pool.ResizeToAtLeast(ref leaves, tree.LeafCount + 1, leaves.Length); diff --git a/BepuPhysics/Trees/Tree.cs b/BepuPhysics/Trees/Tree.cs index 620ef560b..9764a34af 100644 --- a/BepuPhysics/Trees/Tree.cs +++ b/BepuPhysics/Trees/Tree.cs @@ -48,6 +48,13 @@ int AddLeaf(int nodeIndex, int childIndex) return LeafCount++; } + int AllocateLeaf() + { + Debug.Assert(LeafCount < Leaves.Length, + "Any attempt to allocate a leaf should not overrun the allocated leaves. For all operations that allocate leaves, capacity should be preallocated."); + return LeafCount++; + } + /// /// Gets bounds pointerse for a leaf in the tree. /// diff --git a/BepuPhysics/Trees/Tree_Add2.cs b/BepuPhysics/Trees/Tree_Add2.cs new file mode 100644 index 000000000..e8175330a --- /dev/null +++ b/BepuPhysics/Trees/Tree_Add2.cs @@ -0,0 +1,187 @@ +using BepuUtilities; +using System.Numerics; +using System.Runtime.CompilerServices; +using System; +using System.Diagnostics; +using BepuUtilities.Memory; +using BepuUtilities.Collections; +using System.Collections.Generic; + +namespace BepuPhysics.Trees; + +partial struct Tree +{ + + /// + /// Adds a leaf to the tree with the given bounding box and returns the index of the added leaf. + /// + /// Extents of the leaf bounds. + /// Resource pool to use if resizing is required. + /// Index of the leaf allocated in the tree's leaf array. + public unsafe int Add2(BoundingBox bounds, BufferPool pool) + { + //The rest of the function assumes we have sufficient room. We don't want to deal with invalidated pointers mid-add. + if (Leaves.Length == LeafCount) + { + //Note that, while we add 1, the underlying pool will request the next higher power of 2 in bytes that can hold it. + //Since we're already at capacity, that will be ~double the size. + Resize(pool, LeafCount + 1); + } + + if (LeafCount < 2) + { + //The root is partial. + ref var leafChild = ref Unsafe.Add(ref Nodes[0].A, LeafCount); + leafChild = Unsafe.As(ref bounds); + var leafIndex = AddLeaf(0, LeafCount); + leafChild.Index = Encode(leafIndex); + leafChild.LeafCount = 1; + return leafIndex; + } + + //The tree is complete; traverse to find the best place to insert the leaf. + + int nodeIndex = 0; + var bounds4 = Unsafe.As(ref bounds); + while (true) + { + ref var node = ref Nodes[nodeIndex]; + //Choose whichever child requires less bounds expansion. If they're tied, choose the one with the least leaf count. + BoundingBox.CreateMergedUnsafe(bounds4, node.A, out var mergedA); + BoundingBox.CreateMergedUnsafe(bounds4, node.B, out var mergedB); + var boundsIncreaseA = ComputeBoundsMetric(mergedA) - ComputeBoundsMetric(Unsafe.As(ref node.A)); + var boundsIncreaseB = ComputeBoundsMetric(mergedB) - ComputeBoundsMetric(Unsafe.As(ref node.B)); + var useA = boundsIncreaseA == boundsIncreaseB ? node.A.LeafCount < node.B.LeafCount : boundsIncreaseA < boundsIncreaseB; + ref var merged = ref Unsafe.As(ref useA ? ref mergedA : ref mergedB); + ref var chosenChild = ref useA ? ref node.A : ref node.B; + if (chosenChild.LeafCount == 1) + { + //The merge target is a leaf. We'll need a new internal node. + var newNodeIndex = AllocateNode(); + ref var newNode = ref Nodes[newNodeIndex]; + ref var newMetanode = ref Metanodes[newNodeIndex]; + //Initialize the metanode of the new node. + newMetanode.Parent = nodeIndex; + newMetanode.IndexInParent = useA ? 0 : 1; + //Create the new child for the inserted leaf. + newNode.A = Unsafe.As(ref bounds); + newNode.A.LeafCount = 1; + var newLeafIndex = AddLeaf(newNodeIndex, 0); + newNode.A.Index = Encode(newLeafIndex); + //Move the leaf that used to be in the parent down into its new slot. + newNode.B = chosenChild; + //Update the moved leaf's location in the leaves. (Note that AddLeaf handled the leaves for the just-inserted leaf.) + Leaves[Encode(chosenChild.Index)] = new Leaf(newNodeIndex, 1); + //Update the parent's child reference to point to the new node. Note that the chosenChild still points to the slot we want. + chosenChild = merged; + chosenChild.LeafCount = 2; + chosenChild.Index = newNodeIndex; + return newLeafIndex; + } + else + { + //Just traversing into an internal node. (This could be microoptimized a wee bit. Similar above.) + var index = chosenChild.Index; + var leafCount = chosenChild.LeafCount + 1; + chosenChild = merged; + chosenChild.Index = index; + chosenChild.LeafCount = leafCount; + nodeIndex = index; + } + + } + } + + + /// + /// Adds a leaf to the tree with the given bounding box and returns the index of the added leaf. + /// + /// Extents of the leaf bounds. + /// Resource pool to use if resizing is required. + /// Index of the leaf allocated in the tree's leaf array. + public unsafe int Add3(BoundingBox bounds, BufferPool pool) + { + //The rest of the function assumes we have sufficient room. We don't want to deal with invalidated pointers mid-add. + if (Leaves.Length == LeafCount) + { + //Note that, while we add 1, the underlying pool will request the next higher power of 2 in bytes that can hold it. + //Since we're already at capacity, that will be ~double the size. + Resize(pool, LeafCount + 1); + } + + if (LeafCount < 2) + { + //The root is partial. + ref var leafChild = ref Unsafe.Add(ref Nodes[0].A, LeafCount); + leafChild = Unsafe.As(ref bounds); + var leafIndex = AddLeaf(0, LeafCount); + leafChild.Index = Encode(leafIndex); + leafChild.LeafCount = 1; + return leafIndex; + } + else + { + //The tree is complete; traverse to find the best place to insert the leaf and collect subtrees on the way. + var estimatedMaximumTraversalDepth = int.Log2(LeafCount + 1) * 4; + var subtrees = new QuickList(estimatedMaximumTraversalDepth, pool); + var nodeIndices = new QuickList(estimatedMaximumTraversalDepth, pool); + + var leafIndex = AllocateLeaf(); + ref var leafSubtree = ref subtrees.AllocateUnsafely(); + leafSubtree = Unsafe.As(ref bounds); + leafSubtree.Index = Encode(leafIndex); + leafSubtree.LeafCount = 1; + + int nodeIndex = 0; + var bounds4 = Unsafe.As(ref bounds); + while (true) + { + nodeIndices.Allocate(pool) = nodeIndex; + ref var node = ref Nodes[nodeIndex]; + //Choose whichever child requires less bounds expansion. If they're tied, choose the one with the least leaf count. + BoundingBox.CreateMergedUnsafe(bounds4, node.A, out var mergedA); + BoundingBox.CreateMergedUnsafe(bounds4, node.B, out var mergedB); + var boundsIncreaseA = ComputeBoundsMetric(mergedA) - ComputeBoundsMetric(Unsafe.As(ref node.A)); + var boundsIncreaseB = ComputeBoundsMetric(mergedB) - ComputeBoundsMetric(Unsafe.As(ref node.B)); + var useA = boundsIncreaseA == boundsIncreaseB ? node.A.LeafCount < node.B.LeafCount : boundsIncreaseA < boundsIncreaseB; + //Push the subtree that we didn't choose into the subtrees set for refinement. + ref var newSubtree = ref subtrees.Allocate(pool); + newSubtree = useA ? node.B : node.A; + //ReifyRootRefinement needs a way to distinguish between node indices pointing at *within refinement* nodes, versus original subtrees. + //We distinguish it here (as we do in the refinement operation) by including a flag in the upper bits of the index for internal nodes. + if (newSubtree.Index >= 0) + newSubtree.Index |= flagForRootRefinementSubtree; + + ref var merged = ref Unsafe.As(ref useA ? ref mergedA : ref mergedB); + ref var chosenChild = ref useA ? ref node.A : ref node.B; + if (chosenChild.LeafCount == 1) + { + //Bottomed out. + subtrees.Allocate(pool) = useA ? node.A : node.B; + break; + } + else + { + //Just traversing into an internal node. + nodeIndex = chosenChild.Index; + } + } + //Once the tree has at least 2 leaves, it's guaranteed that the node count will be leafCount - 1, so there's going to be a new node. + nodeIndices.Allocate(pool) = AllocateNode(); + + //We can now run a binned build over all the touched nodes. + //Note that we ignore metanodes for the binned build; we reify the results in a postpass. + var nodes = new Buffer(nodeIndices.Count, pool); + BinnedBuild(subtrees.Span.Slice(subtrees.Count), nodes, default, default, pool); + + ReifyRootRefinement(0, nodeIndices.Count, nodeIndices, nodes, this); + + nodeIndices.Dispose(pool); + subtrees.Dispose(pool); + nodes.Dispose(pool); + + + return leafIndex; + } + } +} \ No newline at end of file From e94307c5942e1be1c24e6ab9a109d0dfc7af33e5 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 13 Aug 2023 18:41:14 -0500 Subject: [PATCH 796/947] Tree rotating variant. Root alone isn't sufficient, and going deep is stlil a bit too slow. Abandoning for now. --- BepuPhysics/Trees/Tree_Add2.cs | 158 +++++++++++++++++++++++++++++++++ 1 file changed, 158 insertions(+) diff --git a/BepuPhysics/Trees/Tree_Add2.cs b/BepuPhysics/Trees/Tree_Add2.cs index e8175330a..d6f1aa8b9 100644 --- a/BepuPhysics/Trees/Tree_Add2.cs +++ b/BepuPhysics/Trees/Tree_Add2.cs @@ -93,6 +93,164 @@ public unsafe int Add2(BoundingBox bounds, BufferPool pool) } + /// + /// Adds a leaf to the tree with the given bounding box and returns the index of the added leaf. + /// + /// Extents of the leaf bounds. + /// Resource pool to use if resizing is required. + /// Index of the leaf allocated in the tree's leaf array. + public unsafe int Add4(BoundingBox bounds, BufferPool pool) + { + //The rest of the function assumes we have sufficient room. We don't want to deal with invalidated pointers mid-add. + if (Leaves.Length == LeafCount) + { + //Note that, while we add 1, the underlying pool will request the next higher power of 2 in bytes that can hold it. + //Since we're already at capacity, that will be ~double the size. + Resize(pool, LeafCount + 1); + } + + if (LeafCount < 2) + { + //The root is partial. + ref var leafChild = ref Unsafe.Add(ref Nodes[0].A, LeafCount); + leafChild = Unsafe.As(ref bounds); + var leafIndex = AddLeaf(0, LeafCount); + leafChild.Index = Encode(leafIndex); + leafChild.LeafCount = 1; + return leafIndex; + } + + //The tree is complete; traverse to find the best place to insert the leaf. + + int nodeIndex = 0; + var bounds4 = Unsafe.As(ref bounds); + var newNodeIndex = AllocateNode(); + //We only ever insert into child A, and it's guaranteed to belong to a new node, so we don't have to wait to add the leaf. + var newLeafIndex = AddLeaf(newNodeIndex, 0); + while (true) + { + ref var node = ref Nodes[nodeIndex]; + //Choose whichever child requires less bounds expansion. If they're tied, choose the one with the least leaf count. + BoundingBox.CreateMergedUnsafe(bounds4, node.A, out var mergedA); + BoundingBox.CreateMergedUnsafe(bounds4, node.B, out var mergedB); + var boundsIncreaseA = ComputeBoundsMetric(mergedA) - ComputeBoundsMetric(Unsafe.As(ref node.A)); + var boundsIncreaseB = ComputeBoundsMetric(mergedB) - ComputeBoundsMetric(Unsafe.As(ref node.B)); + var useA = boundsIncreaseA == boundsIncreaseB ? node.A.LeafCount < node.B.LeafCount : boundsIncreaseA < boundsIncreaseB; + ref var merged = ref Unsafe.As(ref useA ? ref mergedA : ref mergedB); + ref var chosenChild = ref useA ? ref node.A : ref node.B; + if (chosenChild.LeafCount == 1) + { + //The merge target is a leaf. We'll need a new internal node. + ref var newNode = ref Nodes[newNodeIndex]; + ref var newMetanode = ref Metanodes[newNodeIndex]; + //Initialize the metanode of the new node. + newMetanode.Parent = nodeIndex; + newMetanode.IndexInParent = useA ? 0 : 1; + //Create the new child for the inserted leaf. + newNode.A = Unsafe.As(ref bounds); + newNode.A.LeafCount = 1; + newNode.A.Index = Encode(newLeafIndex); + //Move the leaf that used to be in the parent down into its new slot. + newNode.B = chosenChild; + //Update the moved leaf's location in the leaves. (Note that AddLeaf handled the leaves for the just-inserted leaf.) + Leaves[Encode(chosenChild.Index)] = new Leaf(newNodeIndex, 1); + //Update the parent's child reference to point to the new node. Note that the chosenChild still points to the slot we want. + chosenChild = merged; + chosenChild.LeafCount = 2; + chosenChild.Index = newNodeIndex; + break; + } + else + { + //Just traversing into an internal node. (This could be microoptimized a wee bit. Similar above.) + var index = chosenChild.Index; + var leafCount = chosenChild.LeafCount + 1; + chosenChild = merged; + chosenChild.Index = index; + chosenChild.LeafCount = leafCount; + nodeIndex = index; + } + } + + //Try rotating the root to compensate for common pathological insertion orders. + { + var rootIndex = 0; + ref var root = ref Nodes[rootIndex]; + var costA = ComputeBoundsMetric(Unsafe.As(ref root.A)); + var costB = ComputeBoundsMetric(Unsafe.As(ref root.B)); + float leftRotationCostChange = 0; + bool leftUsesA = false; + float rightRotationCostChange = 0; + bool rightUsesA = false; + if (root.A.Index >= 0) + { + //Try a right rotation. root.B will merge with the better of A's children, while the worse of A's children will take the place of root.A. + ref var a = ref Nodes[root.A.Index]; + BoundingBox.CreateMergedUnsafe(a.A, root.B, out var aaB); + BoundingBox.CreateMergedUnsafe(a.B, root.B, out var abB); + var costAAB = ComputeBoundsMetric(Unsafe.As(ref aaB)); + var costABB = ComputeBoundsMetric(Unsafe.As(ref abB)); + rightUsesA = costAAB < costABB; + rightRotationCostChange = float.Min(costAAB, costABB) - costA; + } + if (root.B.Index >= 0) + { + //Try a left rotation. root.A will merge with the better of B's children, while the worse of B's children will take the place of root.B. + ref var b = ref Nodes[root.B.Index]; + BoundingBox.CreateMergedUnsafe(root.A, b.A, out var baB); + BoundingBox.CreateMergedUnsafe(root.A, b.B, out var bbB); + var costBAB = ComputeBoundsMetric(Unsafe.As(ref baB)); + var costBBB = ComputeBoundsMetric(Unsafe.As(ref bbB)); + leftUsesA = costBAB < costBBB; + leftRotationCostChange = float.Min(costBAB, costBBB) - costB; + } + if (float.Min(leftRotationCostChange, rightRotationCostChange) < 0) + { + //A rotation is worth it. + if (leftRotationCostChange < rightRotationCostChange) + { + //Left rotation wins! + var nodeIndexToReplace = root.B.Index; + ref var nodeToReplace = ref Nodes[nodeIndexToReplace]; + var childToShiftUp = leftUsesA ? nodeToReplace.B : nodeToReplace.A; + var childToShiftLeft = leftUsesA ? nodeToReplace.A : nodeToReplace.B; + nodeToReplace.A = root.A; + nodeToReplace.B = childToShiftLeft; + BoundingBox.CreateMergedUnsafe(nodeToReplace.A, nodeToReplace.B, out root.A); + root.A.Index = nodeIndexToReplace; + root.A.LeafCount = nodeToReplace.A.LeafCount + nodeToReplace.B.LeafCount; + root.B = childToShiftUp; + Metanodes[nodeIndexToReplace] = new Metanode { Parent = rootIndex, IndexInParent = 0 }; + if (childToShiftUp.Index < 0) Leaves[Encode(childToShiftUp.Index)] = new Leaf(rootIndex, 1); else Metanodes[childToShiftUp.Index] = new Metanode { Parent = rootIndex, IndexInParent = 1 }; + if (nodeToReplace.A.Index < 0) Leaves[Encode(nodeToReplace.A.Index)] = new Leaf(nodeIndexToReplace, 0); else Metanodes[nodeToReplace.A.Index] = new Metanode { Parent = nodeIndexToReplace, IndexInParent = 0 }; + if (nodeToReplace.B.Index < 0) Leaves[Encode(nodeToReplace.B.Index)] = new Leaf(nodeIndexToReplace, 1); else Metanodes[nodeToReplace.B.Index] = new Metanode { Parent = nodeIndexToReplace, IndexInParent = 1 }; + } + else + { + //Right rotation wins! + var nodeIndexToReplace = root.A.Index; + ref var nodeToReplace = ref Nodes[nodeIndexToReplace]; + var childToShiftUp = rightUsesA ? nodeToReplace.B : nodeToReplace.A; + var childToShiftRight = rightUsesA ? nodeToReplace.A : nodeToReplace.B; + nodeToReplace.A = childToShiftRight; + nodeToReplace.B = root.B; + BoundingBox.CreateMergedUnsafe(nodeToReplace.A, nodeToReplace.B, out root.B); + root.B.Index = nodeIndexToReplace; + root.B.LeafCount = nodeToReplace.A.LeafCount + nodeToReplace.B.LeafCount; + root.A = childToShiftUp; + Metanodes[nodeIndexToReplace] = new Metanode { Parent = rootIndex, IndexInParent = 1 }; + if (childToShiftUp.Index < 0) Leaves[Encode(childToShiftUp.Index)] = new Leaf(rootIndex, 0); else Metanodes[childToShiftUp.Index] = new Metanode { Parent = rootIndex, IndexInParent = 0 }; + if (nodeToReplace.A.Index < 0) Leaves[Encode(nodeToReplace.A.Index)] = new Leaf(nodeIndexToReplace, 0); else Metanodes[nodeToReplace.A.Index] = new Metanode { Parent = nodeIndexToReplace, IndexInParent = 0 }; + if (nodeToReplace.B.Index < 0) Leaves[Encode(nodeToReplace.B.Index)] = new Leaf(nodeIndexToReplace, 1); else Metanodes[nodeToReplace.B.Index] = new Metanode { Parent = nodeIndexToReplace, IndexInParent = 1 }; + } + //Validate(); + } + + } + return newLeafIndex; + } + + /// /// Adds a leaf to the tree with the given bounding box and returns the index of the added leaf. /// From cd3b130506cc7b831b18e4dfa41fea8f72a0b017 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 13 Aug 2023 20:13:57 -0500 Subject: [PATCH 797/947] actually nevermind it's fast enough! --- BepuPhysics/CollisionDetection/BroadPhase.cs | 4 +- BepuPhysics/Trees/Tree_Add2.cs | 223 ++++++------------- 2 files changed, 71 insertions(+), 156 deletions(-) diff --git a/BepuPhysics/CollisionDetection/BroadPhase.cs b/BepuPhysics/CollisionDetection/BroadPhase.cs index 3695873e5..6bb76e39e 100644 --- a/BepuPhysics/CollisionDetection/BroadPhase.cs +++ b/BepuPhysics/CollisionDetection/BroadPhase.cs @@ -63,7 +63,7 @@ public BroadPhase(BufferPool pool, int initialActiveLeafCapacity = 4096, int ini [MethodImpl(MethodImplOptions.AggressiveInlining)] private static int Add(CollidableReference collidable, ref BoundingBox bounds, ref Tree tree, BufferPool pool, ref Buffer leaves) { - var leafIndex = tree.Add2(bounds, pool); + var leafIndex = tree.Add4(bounds, pool); if (leafIndex >= leaves.Length) { pool.ResizeToAtLeast(ref leaves, tree.LeafCount + 1, leaves.Length); @@ -434,6 +434,8 @@ public void Update2(IThreadDispatcher threadDispatcher = null, bool deterministi frameIndex = 0; else ++frameIndex; + //StaticTree.Validate(); + //ActiveTree.Validate(); } /// diff --git a/BepuPhysics/Trees/Tree_Add2.cs b/BepuPhysics/Trees/Tree_Add2.cs index d6f1aa8b9..36c85e2d8 100644 --- a/BepuPhysics/Trees/Tree_Add2.cs +++ b/BepuPhysics/Trees/Tree_Add2.cs @@ -172,174 +172,87 @@ public unsafe int Add4(BoundingBox bounds, BufferPool pool) } } - //Try rotating the root to compensate for common pathological insertion orders. + //Try rotating a path from the insertion location up to the root to compensate for bad insertion orders. + var parentIndex = Leaves[newLeafIndex].NodeIndex; + while (parentIndex >= 0) { - var rootIndex = 0; - ref var root = ref Nodes[rootIndex]; - var costA = ComputeBoundsMetric(Unsafe.As(ref root.A)); - var costB = ComputeBoundsMetric(Unsafe.As(ref root.B)); - float leftRotationCostChange = 0; - bool leftUsesA = false; - float rightRotationCostChange = 0; - bool rightUsesA = false; - if (root.A.Index >= 0) - { - //Try a right rotation. root.B will merge with the better of A's children, while the worse of A's children will take the place of root.A. - ref var a = ref Nodes[root.A.Index]; - BoundingBox.CreateMergedUnsafe(a.A, root.B, out var aaB); - BoundingBox.CreateMergedUnsafe(a.B, root.B, out var abB); - var costAAB = ComputeBoundsMetric(Unsafe.As(ref aaB)); - var costABB = ComputeBoundsMetric(Unsafe.As(ref abB)); - rightUsesA = costAAB < costABB; - rightRotationCostChange = float.Min(costAAB, costABB) - costA; - } - if (root.B.Index >= 0) - { - //Try a left rotation. root.A will merge with the better of B's children, while the worse of B's children will take the place of root.B. - ref var b = ref Nodes[root.B.Index]; - BoundingBox.CreateMergedUnsafe(root.A, b.A, out var baB); - BoundingBox.CreateMergedUnsafe(root.A, b.B, out var bbB); - var costBAB = ComputeBoundsMetric(Unsafe.As(ref baB)); - var costBBB = ComputeBoundsMetric(Unsafe.As(ref bbB)); - leftUsesA = costBAB < costBBB; - leftRotationCostChange = float.Min(costBAB, costBBB) - costB; - } - if (float.Min(leftRotationCostChange, rightRotationCostChange) < 0) - { - //A rotation is worth it. - if (leftRotationCostChange < rightRotationCostChange) - { - //Left rotation wins! - var nodeIndexToReplace = root.B.Index; - ref var nodeToReplace = ref Nodes[nodeIndexToReplace]; - var childToShiftUp = leftUsesA ? nodeToReplace.B : nodeToReplace.A; - var childToShiftLeft = leftUsesA ? nodeToReplace.A : nodeToReplace.B; - nodeToReplace.A = root.A; - nodeToReplace.B = childToShiftLeft; - BoundingBox.CreateMergedUnsafe(nodeToReplace.A, nodeToReplace.B, out root.A); - root.A.Index = nodeIndexToReplace; - root.A.LeafCount = nodeToReplace.A.LeafCount + nodeToReplace.B.LeafCount; - root.B = childToShiftUp; - Metanodes[nodeIndexToReplace] = new Metanode { Parent = rootIndex, IndexInParent = 0 }; - if (childToShiftUp.Index < 0) Leaves[Encode(childToShiftUp.Index)] = new Leaf(rootIndex, 1); else Metanodes[childToShiftUp.Index] = new Metanode { Parent = rootIndex, IndexInParent = 1 }; - if (nodeToReplace.A.Index < 0) Leaves[Encode(nodeToReplace.A.Index)] = new Leaf(nodeIndexToReplace, 0); else Metanodes[nodeToReplace.A.Index] = new Metanode { Parent = nodeIndexToReplace, IndexInParent = 0 }; - if (nodeToReplace.B.Index < 0) Leaves[Encode(nodeToReplace.B.Index)] = new Leaf(nodeIndexToReplace, 1); else Metanodes[nodeToReplace.B.Index] = new Metanode { Parent = nodeIndexToReplace, IndexInParent = 1 }; - } - else - { - //Right rotation wins! - var nodeIndexToReplace = root.A.Index; - ref var nodeToReplace = ref Nodes[nodeIndexToReplace]; - var childToShiftUp = rightUsesA ? nodeToReplace.B : nodeToReplace.A; - var childToShiftRight = rightUsesA ? nodeToReplace.A : nodeToReplace.B; - nodeToReplace.A = childToShiftRight; - nodeToReplace.B = root.B; - BoundingBox.CreateMergedUnsafe(nodeToReplace.A, nodeToReplace.B, out root.B); - root.B.Index = nodeIndexToReplace; - root.B.LeafCount = nodeToReplace.A.LeafCount + nodeToReplace.B.LeafCount; - root.A = childToShiftUp; - Metanodes[nodeIndexToReplace] = new Metanode { Parent = rootIndex, IndexInParent = 1 }; - if (childToShiftUp.Index < 0) Leaves[Encode(childToShiftUp.Index)] = new Leaf(rootIndex, 0); else Metanodes[childToShiftUp.Index] = new Metanode { Parent = rootIndex, IndexInParent = 0 }; - if (nodeToReplace.A.Index < 0) Leaves[Encode(nodeToReplace.A.Index)] = new Leaf(nodeIndexToReplace, 0); else Metanodes[nodeToReplace.A.Index] = new Metanode { Parent = nodeIndexToReplace, IndexInParent = 0 }; - if (nodeToReplace.B.Index < 0) Leaves[Encode(nodeToReplace.B.Index)] = new Leaf(nodeIndexToReplace, 1); else Metanodes[nodeToReplace.B.Index] = new Metanode { Parent = nodeIndexToReplace, IndexInParent = 1 }; - } - //Validate(); - } - + TryRotateNode(parentIndex); + parentIndex = Metanodes[parentIndex].Parent; } return newLeafIndex; } - - /// - /// Adds a leaf to the tree with the given bounding box and returns the index of the added leaf. - /// - /// Extents of the leaf bounds. - /// Resource pool to use if resizing is required. - /// Index of the leaf allocated in the tree's leaf array. - public unsafe int Add3(BoundingBox bounds, BufferPool pool) + private unsafe void TryRotateNode(int rotationRootIndex) { - //The rest of the function assumes we have sufficient room. We don't want to deal with invalidated pointers mid-add. - if (Leaves.Length == LeafCount) + ref var root = ref Nodes[rotationRootIndex]; + var costA = ComputeBoundsMetric(Unsafe.As(ref root.A)); + var costB = ComputeBoundsMetric(Unsafe.As(ref root.B)); + float leftRotationCostChange = 0; + bool leftUsesA = false; + float rightRotationCostChange = 0; + bool rightUsesA = false; + if (root.A.Index >= 0) { - //Note that, while we add 1, the underlying pool will request the next higher power of 2 in bytes that can hold it. - //Since we're already at capacity, that will be ~double the size. - Resize(pool, LeafCount + 1); + //Try a right rotation. root.B will merge with the better of A's children, while the worse of A's children will take the place of root.A. + ref var a = ref Nodes[root.A.Index]; + BoundingBox.CreateMergedUnsafe(a.A, root.B, out var aaB); + BoundingBox.CreateMergedUnsafe(a.B, root.B, out var abB); + var costAAB = ComputeBoundsMetric(Unsafe.As(ref aaB)); + var costABB = ComputeBoundsMetric(Unsafe.As(ref abB)); + rightUsesA = costAAB < costABB; + rightRotationCostChange = float.Min(costAAB, costABB) - costA; } - - if (LeafCount < 2) + if (root.B.Index >= 0) { - //The root is partial. - ref var leafChild = ref Unsafe.Add(ref Nodes[0].A, LeafCount); - leafChild = Unsafe.As(ref bounds); - var leafIndex = AddLeaf(0, LeafCount); - leafChild.Index = Encode(leafIndex); - leafChild.LeafCount = 1; - return leafIndex; + //Try a left rotation. root.A will merge with the better of B's children, while the worse of B's children will take the place of root.B. + ref var b = ref Nodes[root.B.Index]; + BoundingBox.CreateMergedUnsafe(root.A, b.A, out var baB); + BoundingBox.CreateMergedUnsafe(root.A, b.B, out var bbB); + var costBAB = ComputeBoundsMetric(Unsafe.As(ref baB)); + var costBBB = ComputeBoundsMetric(Unsafe.As(ref bbB)); + leftUsesA = costBAB < costBBB; + leftRotationCostChange = float.Min(costBAB, costBBB) - costB; } - else + if (float.Min(leftRotationCostChange, rightRotationCostChange) < 0) { - //The tree is complete; traverse to find the best place to insert the leaf and collect subtrees on the way. - var estimatedMaximumTraversalDepth = int.Log2(LeafCount + 1) * 4; - var subtrees = new QuickList(estimatedMaximumTraversalDepth, pool); - var nodeIndices = new QuickList(estimatedMaximumTraversalDepth, pool); - - var leafIndex = AllocateLeaf(); - ref var leafSubtree = ref subtrees.AllocateUnsafely(); - leafSubtree = Unsafe.As(ref bounds); - leafSubtree.Index = Encode(leafIndex); - leafSubtree.LeafCount = 1; - - int nodeIndex = 0; - var bounds4 = Unsafe.As(ref bounds); - while (true) + //A rotation is worth it. + if (leftRotationCostChange < rightRotationCostChange) { - nodeIndices.Allocate(pool) = nodeIndex; - ref var node = ref Nodes[nodeIndex]; - //Choose whichever child requires less bounds expansion. If they're tied, choose the one with the least leaf count. - BoundingBox.CreateMergedUnsafe(bounds4, node.A, out var mergedA); - BoundingBox.CreateMergedUnsafe(bounds4, node.B, out var mergedB); - var boundsIncreaseA = ComputeBoundsMetric(mergedA) - ComputeBoundsMetric(Unsafe.As(ref node.A)); - var boundsIncreaseB = ComputeBoundsMetric(mergedB) - ComputeBoundsMetric(Unsafe.As(ref node.B)); - var useA = boundsIncreaseA == boundsIncreaseB ? node.A.LeafCount < node.B.LeafCount : boundsIncreaseA < boundsIncreaseB; - //Push the subtree that we didn't choose into the subtrees set for refinement. - ref var newSubtree = ref subtrees.Allocate(pool); - newSubtree = useA ? node.B : node.A; - //ReifyRootRefinement needs a way to distinguish between node indices pointing at *within refinement* nodes, versus original subtrees. - //We distinguish it here (as we do in the refinement operation) by including a flag in the upper bits of the index for internal nodes. - if (newSubtree.Index >= 0) - newSubtree.Index |= flagForRootRefinementSubtree; - - ref var merged = ref Unsafe.As(ref useA ? ref mergedA : ref mergedB); - ref var chosenChild = ref useA ? ref node.A : ref node.B; - if (chosenChild.LeafCount == 1) - { - //Bottomed out. - subtrees.Allocate(pool) = useA ? node.A : node.B; - break; - } - else - { - //Just traversing into an internal node. - nodeIndex = chosenChild.Index; - } + //Left rotation wins! + var nodeIndexToReplace = root.B.Index; + ref var nodeToReplace = ref Nodes[nodeIndexToReplace]; + var childToShiftUp = leftUsesA ? nodeToReplace.B : nodeToReplace.A; + var childToShiftLeft = leftUsesA ? nodeToReplace.A : nodeToReplace.B; + nodeToReplace.A = root.A; + nodeToReplace.B = childToShiftLeft; + BoundingBox.CreateMergedUnsafe(nodeToReplace.A, nodeToReplace.B, out root.A); + root.A.Index = nodeIndexToReplace; + root.A.LeafCount = nodeToReplace.A.LeafCount + nodeToReplace.B.LeafCount; + root.B = childToShiftUp; + Metanodes[nodeIndexToReplace] = new Metanode { Parent = rotationRootIndex, IndexInParent = 0 }; + if (childToShiftUp.Index < 0) Leaves[Encode(childToShiftUp.Index)] = new Leaf(rotationRootIndex, 1); else Metanodes[childToShiftUp.Index] = new Metanode { Parent = rotationRootIndex, IndexInParent = 1 }; + if (nodeToReplace.A.Index < 0) Leaves[Encode(nodeToReplace.A.Index)] = new Leaf(nodeIndexToReplace, 0); else Metanodes[nodeToReplace.A.Index] = new Metanode { Parent = nodeIndexToReplace, IndexInParent = 0 }; + if (nodeToReplace.B.Index < 0) Leaves[Encode(nodeToReplace.B.Index)] = new Leaf(nodeIndexToReplace, 1); else Metanodes[nodeToReplace.B.Index] = new Metanode { Parent = nodeIndexToReplace, IndexInParent = 1 }; + } + else + { + //Right rotation wins! + var nodeIndexToReplace = root.A.Index; + ref var nodeToReplace = ref Nodes[nodeIndexToReplace]; + var childToShiftUp = rightUsesA ? nodeToReplace.B : nodeToReplace.A; + var childToShiftRight = rightUsesA ? nodeToReplace.A : nodeToReplace.B; + nodeToReplace.A = childToShiftRight; + nodeToReplace.B = root.B; + BoundingBox.CreateMergedUnsafe(nodeToReplace.A, nodeToReplace.B, out root.B); + root.B.Index = nodeIndexToReplace; + root.B.LeafCount = nodeToReplace.A.LeafCount + nodeToReplace.B.LeafCount; + root.A = childToShiftUp; + Metanodes[nodeIndexToReplace] = new Metanode { Parent = rotationRootIndex, IndexInParent = 1 }; + if (childToShiftUp.Index < 0) Leaves[Encode(childToShiftUp.Index)] = new Leaf(rotationRootIndex, 0); else Metanodes[childToShiftUp.Index] = new Metanode { Parent = rotationRootIndex, IndexInParent = 0 }; + if (nodeToReplace.A.Index < 0) Leaves[Encode(nodeToReplace.A.Index)] = new Leaf(nodeIndexToReplace, 0); else Metanodes[nodeToReplace.A.Index] = new Metanode { Parent = nodeIndexToReplace, IndexInParent = 0 }; + if (nodeToReplace.B.Index < 0) Leaves[Encode(nodeToReplace.B.Index)] = new Leaf(nodeIndexToReplace, 1); else Metanodes[nodeToReplace.B.Index] = new Metanode { Parent = nodeIndexToReplace, IndexInParent = 1 }; } - //Once the tree has at least 2 leaves, it's guaranteed that the node count will be leafCount - 1, so there's going to be a new node. - nodeIndices.Allocate(pool) = AllocateNode(); - - //We can now run a binned build over all the touched nodes. - //Note that we ignore metanodes for the binned build; we reify the results in a postpass. - var nodes = new Buffer(nodeIndices.Count, pool); - BinnedBuild(subtrees.Span.Slice(subtrees.Count), nodes, default, default, pool); - - ReifyRootRefinement(0, nodeIndices.Count, nodeIndices, nodes, this); - - nodeIndices.Dispose(pool); - subtrees.Dispose(pool); - nodes.Dispose(pool); - - - return leafIndex; } + } } \ No newline at end of file From e3e7d7c217f125ab25c64b33ae476f644a53e2e8 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 13 Aug 2023 20:23:19 -0500 Subject: [PATCH 798/947] Fixed unguarded metanodes access in binned build/refine. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index f1789e99a..f3ca26b69 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -1511,7 +1511,8 @@ public static unsafe void BinnedBuild(Buffer subtrees, Buffer n { //No need to do anything fancy, all subtrees fit in the root. Requires a special case for the partial root. nodes[0] = new Node { A = subtrees[0], B = subtrees.Length == 2 ? subtrees[1] : default }; - metanodes[0] = new Metanode { Parent = -1, IndexInParent = -1 }; + if (metanodes.Allocated) + metanodes[0] = new Metanode { Parent = -1, IndexInParent = -1 }; return; } if (dispatcher != null && pool == null) From 0b5c39c24fb266eb15682181c648c6b700f0fd25 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 13 Aug 2023 21:02:23 -0500 Subject: [PATCH 799/947] Old add deleted. Cleaned up API for new add variants. --- BepuPhysics/CollisionDetection/BroadPhase.cs | 3 +- BepuPhysics/Trees/Tree_Add.cs | 359 +++++++++++-------- BepuPhysics/Trees/Tree_Add2.cs | 258 ------------- 3 files changed, 203 insertions(+), 417 deletions(-) delete mode 100644 BepuPhysics/Trees/Tree_Add2.cs diff --git a/BepuPhysics/CollisionDetection/BroadPhase.cs b/BepuPhysics/CollisionDetection/BroadPhase.cs index 6bb76e39e..4ca467b79 100644 --- a/BepuPhysics/CollisionDetection/BroadPhase.cs +++ b/BepuPhysics/CollisionDetection/BroadPhase.cs @@ -63,7 +63,7 @@ public BroadPhase(BufferPool pool, int initialActiveLeafCapacity = 4096, int ini [MethodImpl(MethodImplOptions.AggressiveInlining)] private static int Add(CollidableReference collidable, ref BoundingBox bounds, ref Tree tree, BufferPool pool, ref Buffer leaves) { - var leafIndex = tree.Add4(bounds, pool); + var leafIndex = tree.Add(bounds, pool); if (leafIndex >= leaves.Length) { pool.ResizeToAtLeast(ref leaves, tree.LeafCount + 1, leaves.Length); @@ -364,6 +364,7 @@ public void Update2(IThreadDispatcher threadDispatcher = null, bool deterministi { ActiveRefinementSchedule(frameIndex, ActiveTree, out var activeRootRefinementSize, out var activeSubtreeRefinementCount, out var activeSubtreeRefinementSize, out var usePriorityQueueActive); StaticRefinementSchedule(frameIndex, StaticTree, out var staticRootRefinementSize, out var staticSubtreeRefinementCount, out var staticSubtreeRefinementSize, out var usePriorityQueueStatic); + Console.WriteLine($"root size: {activeRootRefinementSize}, subtree count: {activeSubtreeRefinementCount}, subtree size: {activeSubtreeRefinementSize}, usePQ: {usePriorityQueueActive}"); const int minimumLeafCountForThreading = 256; if (threadDispatcher != null && threadDispatcher.ThreadCount > 1 && (ActiveTree.LeafCount >= minimumLeafCountForThreading || StaticTree.LeafCount >= minimumLeafCountForThreading)) { diff --git a/BepuPhysics/Trees/Tree_Add.cs b/BepuPhysics/Trees/Tree_Add.cs index ab78ea49d..dee7fb22a 100644 --- a/BepuPhysics/Trees/Tree_Add.cs +++ b/BepuPhysics/Trees/Tree_Add.cs @@ -4,192 +4,235 @@ using System; using System.Diagnostics; using BepuUtilities.Memory; +using BepuUtilities.Collections; +using System.Collections.Generic; -namespace BepuPhysics.Trees +namespace BepuPhysics.Trees; + +partial struct Tree { - partial struct Tree + private struct InsertShouldNotRotate { } + private struct InsertShouldRotateTopDown { } + private struct InsertShouldRotateBottomUp { } + + /// + /// Adds a leaf to the tree with the given bounding box and returns the index of the added leaf. + /// + /// Extents of the leaf bounds. + /// Resource pool to use if resizing is required. + /// Index of the leaf allocated in the tree's leaf array. + /// This performs no incremental refinement. When acting on the same tree, it's slightly cheaper than , + /// but the quality of the tree depends on insertion order. + /// Pathological insertion orders can result in a maximally imbalanced tree, quadratic insertion times across the full tree build, and query performance linear in the number of leaves. + /// This is typically best reserved for cases where the insertion order is known to be randomized or otherwise conducive to building decent trees. + public unsafe int AddWithoutRefinement(BoundingBox bounds, BufferPool pool) { - /// - /// Merges a new leaf node with an existing leaf node, producing a new internal node referencing both leaves, and then returns the index of the leaf node. - /// - /// Bounding box of the leaf being added. - /// Index of the parent node that the existing leaf belongs to. - /// Index of the child wtihin the parent node that the existing leaf belongs to. - /// Bounding box holding both the new and existing leaves. - /// Index of the leaf - [MethodImpl(MethodImplOptions.AggressiveInlining)] - unsafe int MergeLeafNodes(ref BoundingBox newLeafBounds, int parentIndex, int indexInParent, ref BoundingBox merged) - { - //It's a leaf node. - //Create a new internal node with the new leaf and the old leaf as children. - //this is the only place where a new level could potentially be created. - - var newNodeIndex = AllocateNode(); - ref var newNode = ref Nodes[newNodeIndex]; - ref var newMetanode = ref Metanodes[newNodeIndex]; - newMetanode.Parent = parentIndex; - newMetanode.IndexInParent = indexInParent; - newMetanode.RefineFlag = 0; - //The first child of the new node is the old leaf. Insert its bounding box. - ref var parentNode = ref Nodes[parentIndex]; - ref var childInParent = ref Unsafe.Add(ref parentNode.A, indexInParent); - newNode.A = childInParent; - - //Insert the new leaf into the second child slot. - ref var b = ref newNode.B; - b.Min = newLeafBounds.Min; - var leafIndex = AddLeaf(newNodeIndex, 1); - b.Index = Encode(leafIndex); - b.Max = newLeafBounds.Max; - b.LeafCount = 1; + return Add(bounds, pool); + } - //Update the old leaf node with the new index information. - var oldLeafIndex = Encode(newNode.A.Index); - Leaves[oldLeafIndex] = new Leaf(newNodeIndex, 0); + /// + /// Adds a leaf to the tree with the given bounding box and returns the index of the added leaf. + /// + /// Extents of the leaf bounds. + /// Resource pool to use if resizing is required. + /// Index of the leaf allocated in the tree's leaf array. + /// Performs incrementally refining tree rotations down along the insertion path, unlike . + /// For a given tree, this is slightly slower than and slightly faster than . + /// Trees built with repeated insertions of this kind tend to have decent quality, but slightly worse than . + public unsafe int Add(BoundingBox bounds, BufferPool pool) + { + return Add(bounds, pool); + } - //Update the original node's child pointer and bounding box. - childInParent.Index = newNodeIndex; - childInParent.Min = merged.Min; - childInParent.Max = merged.Max; - Debug.Assert(childInParent.LeafCount == 1); - childInParent.LeafCount = 2; - return leafIndex; - } + /// + /// Adds a leaf to the tree with the given bounding box and returns the index of the added leaf. + /// + /// Extents of the leaf bounds. + /// Resource pool to use if resizing is required. + /// Index of the leaf allocated in the tree's leaf array. + /// Performs incrementally refining tree rotations up along the insertion path, unlike . + /// Trees built with repeated insertions of this kind tend to have slightly better quality than , but it is also slightly more expensive. + public unsafe int AddWithBottomUpRefinement(BoundingBox bounds, BufferPool pool) + { + return Add(bounds, pool); + } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - unsafe int InsertLeafIntoEmptySlot(ref BoundingBox leafBox, int nodeIndex, int childIndex, ref Node node) - { - var leafIndex = AddLeaf(nodeIndex, childIndex); - ref var child = ref Unsafe.Add(ref node.A, childIndex); - child.Min = leafBox.Min; - child.Index = Encode(leafIndex); - child.Max = leafBox.Max; - child.LeafCount = 1; - return leafIndex; - } - enum BestInsertionChoice + private unsafe int Add(BoundingBox bounds, BufferPool pool) where TShouldRotate : unmanaged + { + //The rest of the function assumes we have sufficient room. We don't want to deal with invalidated pointers mid-add. + if (Leaves.Length == LeafCount) { - NewInternal, - Traverse + //Note that, while we add 1, the underlying pool will request the next higher power of 2 in bytes that can hold it. + //Since we're already at capacity, that will be ~double the size. + Resize(pool, LeafCount + 1); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - static void CreateMerged(ref Vector3 minA, ref Vector3 maxA, ref Vector3 minB, ref Vector3 maxB, out BoundingBox merged) + if (LeafCount < 2) { - merged.Min = Vector3.Min(minA, minB); - merged.Max = Vector3.Max(maxA, maxB); + //The root is partial. + ref var leafChild = ref Unsafe.Add(ref Nodes[0].A, LeafCount); + leafChild = Unsafe.As(ref bounds); + var leafIndex = AddLeaf(0, LeafCount); + leafChild.Index = Encode(leafIndex); + leafChild.LeafCount = 1; + return leafIndex; } + //The tree is complete; traverse to find the best place to insert the leaf. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - private static unsafe BestInsertionChoice ComputeBestInsertionChoice(ref BoundingBox bounds, float newLeafCost, ref NodeChild child, out BoundingBox mergedCandidate, out float costChange) + int nodeIndex = 0; + var bounds4 = Unsafe.As(ref bounds); + var newNodeIndex = AllocateNode(); + //We only ever insert into child A, and it's guaranteed to belong to a new node, so we don't have to wait to add the leaf. + var newLeafIndex = AddLeaf(newNodeIndex, 0); + while (true) { - CreateMerged(ref child.Min, ref child.Max, ref bounds.Min, ref bounds.Max, out mergedCandidate); - var newCost = ComputeBoundsMetric(ref mergedCandidate); - if (child.Index >= 0) + //Note: rotating from the top down produces a tree that's lower quality that rotating from the bottom up. + //In context, that's fine; insertion just needs to produce a tree that isn't megatrash/stackoverflowy, and refinement will take care of the rest. + //The advantage is that top down is a little faster. + if (typeof(TShouldRotate) == typeof(InsertShouldRotateTopDown)) + TryRotateNode(nodeIndex); + ref var node = ref Nodes[nodeIndex]; + //Choose whichever child requires less bounds expansion. If they're tied, choose the one with the least leaf count. + BoundingBox.CreateMergedUnsafe(bounds4, node.A, out var mergedA); + BoundingBox.CreateMergedUnsafe(bounds4, node.B, out var mergedB); + var boundsIncreaseA = ComputeBoundsMetric(mergedA) - ComputeBoundsMetric(Unsafe.As(ref node.A)); + var boundsIncreaseB = ComputeBoundsMetric(mergedB) - ComputeBoundsMetric(Unsafe.As(ref node.B)); + var useA = boundsIncreaseA == boundsIncreaseB ? node.A.LeafCount < node.B.LeafCount : boundsIncreaseA < boundsIncreaseB; + ref var merged = ref Unsafe.As(ref useA ? ref mergedA : ref mergedB); + ref var chosenChild = ref useA ? ref node.A : ref node.B; + if (chosenChild.LeafCount == 1) { - //Estimate the cost of child node expansions as max(SAH(newLeafBounds), costChange) * log2(child.LeafCount). - //We're assuming that the remaining tree is balanced and that each level will expand by at least SAH(newLeafBounds). - //This might not be anywhere close to correct, but it's not a bad estimate. - costChange = newCost - ComputeBoundsMetric(ref child.Min, ref child.Max); - costChange += SpanHelper.GetContainingPowerOf2(child.LeafCount) * Math.Max(newLeafCost, costChange); - return BestInsertionChoice.Traverse; + //The merge target is a leaf. We'll need a new internal node. + ref var newNode = ref Nodes[newNodeIndex]; + ref var newMetanode = ref Metanodes[newNodeIndex]; + //Initialize the metanode of the new node. + newMetanode.Parent = nodeIndex; + newMetanode.IndexInParent = useA ? 0 : 1; + //Create the new child for the inserted leaf. + newNode.A = Unsafe.As(ref bounds); + newNode.A.LeafCount = 1; + newNode.A.Index = Encode(newLeafIndex); + //Move the leaf that used to be in the parent down into its new slot. + newNode.B = chosenChild; + //Update the moved leaf's location in the leaves. (Note that AddLeaf handled the leaves for the just-inserted leaf.) + Leaves[Encode(chosenChild.Index)] = new Leaf(newNodeIndex, 1); + //Update the parent's child reference to point to the new node. Note that the chosenChild still points to the slot we want. + chosenChild = merged; + chosenChild.LeafCount = 2; + chosenChild.Index = newNodeIndex; + break; } else { - costChange = newCost; - return BestInsertionChoice.NewInternal; + //Just traversing into an internal node. (This could be microoptimized a wee bit. Similar above.) + var index = chosenChild.Index; + var leafCount = chosenChild.LeafCount + 1; + chosenChild = merged; + chosenChild.Index = index; + chosenChild.LeafCount = leafCount; + nodeIndex = index; } - } - /// - /// Adds a leaf to the tree with the given bounding box and returns the index of the added leaf. - /// - /// Extents of the leaf bounds. - /// Resource pool to use if resizing is required. - /// Index of the leaf allocated in the tree's leaf array. - public unsafe int Add(BoundingBox bounds, BufferPool pool) + if (typeof(TShouldRotate) == typeof(InsertShouldRotateBottomUp)) { - //The rest of the function assumes we have sufficient room. We don't want to deal with invalidated pointers mid-add. - if (Leaves.Length == LeafCount) - { - //Note that, while we add 1, the underlying pool will request the next higher power of 2 in bytes that can hold it. - //Since we're already at capacity, that will be ~double the size. - Resize(pool, LeafCount + 1); - } - - //Assumption: Index 0 is always the root if it exists, and an empty tree will have a 'root' with a child count of 0. - int nodeIndex = 0; - var newLeafCost = ComputeBoundsMetric(ref bounds); - while (true) + var parentIndex = Leaves[newLeafIndex].NodeIndex; + while (parentIndex >= 0) { - //Which child should the leaf belong to? - - //Give the leaf to whichever node had the least cost change. - ref var node = ref Nodes[nodeIndex]; - //This is a binary tree, so the only time a node can have less than full children is when it's the root node. - //By convention, an empty tree still has a root node with no children, so we do have to handle this case. - if (LeafCount < 2) - { - //The best slot will, at best, be tied with inserting it in a leaf node because the change in heuristic cost for filling an empty slot is zero. - return InsertLeafIntoEmptySlot(ref bounds, nodeIndex, LeafCount, ref node); - } - else - { - ref var a = ref node.A; - ref var b = ref node.B; - var choiceA = ComputeBestInsertionChoice(ref bounds, newLeafCost, ref a, out var mergedA, out var costChangeA); - var choiceB = ComputeBestInsertionChoice(ref bounds, newLeafCost, ref b, out var mergedB, out var costChangeB); - if (costChangeA <= costChangeB) - { - if (choiceA == BestInsertionChoice.NewInternal) - { - return MergeLeafNodes(ref bounds, nodeIndex, 0, ref mergedA); - } - else //if (choiceA == BestInsertionChoice.Traverse) - { - a.Min = mergedA.Min; - a.Max = mergedA.Max; - nodeIndex = a.Index; - ++a.LeafCount; - } - } - else - { - if (choiceB == BestInsertionChoice.NewInternal) - { - return MergeLeafNodes(ref bounds, nodeIndex, 1, ref mergedB); - } - else //if (choiceB == BestInsertionChoice.Traverse) - { - b.Min = mergedB.Min; - b.Max = mergedB.Max; - nodeIndex = b.Index; - ++b.LeafCount; - } - } - } - - + TryRotateNode(parentIndex); + parentIndex = Metanodes[parentIndex].Parent; } } + return newLeafIndex; + } - - [MethodImpl(MethodImplOptions.NoInlining)] - internal static float ComputeBoundsMetric(ref BoundingBox bounds) + private unsafe void TryRotateNode(int rotationRootIndex) + { + ref var root = ref Nodes[rotationRootIndex]; + var costA = ComputeBoundsMetric(Unsafe.As(ref root.A)); + var costB = ComputeBoundsMetric(Unsafe.As(ref root.B)); + float leftRotationCostChange = 0; + bool leftUsesA = false; + float rightRotationCostChange = 0; + bool rightUsesA = false; + if (root.A.Index >= 0) { - return ComputeBoundsMetric(ref bounds.Min, ref bounds.Max); + //Try a right rotation. root.B will merge with the better of A's children, while the worse of A's children will take the place of root.A. + ref var a = ref Nodes[root.A.Index]; + BoundingBox.CreateMergedUnsafe(a.A, root.B, out var aaB); + BoundingBox.CreateMergedUnsafe(a.B, root.B, out var abB); + var costAAB = ComputeBoundsMetric(Unsafe.As(ref aaB)); + var costABB = ComputeBoundsMetric(Unsafe.As(ref abB)); + rightUsesA = costAAB < costABB; + rightRotationCostChange = float.Min(costAAB, costABB) - costA; } - [MethodImpl(MethodImplOptions.NoInlining)] - internal static float ComputeBoundsMetric(ref Vector3 min, ref Vector3 max) + if (root.B.Index >= 0) { - //Note that we just use the SAH. While we are primarily interested in volume queries for the purposes of collision detection, the topological difference - //between a volume heuristic and surface area heuristic isn't huge. There is, however, one big annoying issue that volume heuristics run into: - //all bounding boxes with one extent equal to zero have zero cost. Surface area approaches avoid this hole simply. - var offset = max - min; - //Note that this is merely proportional to surface area. Being scaled by a constant factor is irrelevant. - return offset.X * offset.Y + offset.Y * offset.Z + offset.X * offset.Z; + //Try a left rotation. root.A will merge with the better of B's children, while the worse of B's children will take the place of root.B. + ref var b = ref Nodes[root.B.Index]; + BoundingBox.CreateMergedUnsafe(root.A, b.A, out var baB); + BoundingBox.CreateMergedUnsafe(root.A, b.B, out var bbB); + var costBAB = ComputeBoundsMetric(Unsafe.As(ref baB)); + var costBBB = ComputeBoundsMetric(Unsafe.As(ref bbB)); + leftUsesA = costBAB < costBBB; + leftRotationCostChange = float.Min(costBAB, costBBB) - costB; } + if (float.Min(leftRotationCostChange, rightRotationCostChange) < 0) + { + //A rotation is worth it. + if (leftRotationCostChange < rightRotationCostChange) + { + //Left rotation wins! + var nodeIndexToReplace = root.B.Index; + ref var nodeToReplace = ref Nodes[nodeIndexToReplace]; + var childToShiftUp = leftUsesA ? nodeToReplace.B : nodeToReplace.A; + var childToShiftLeft = leftUsesA ? nodeToReplace.A : nodeToReplace.B; + nodeToReplace.A = root.A; + nodeToReplace.B = childToShiftLeft; + BoundingBox.CreateMergedUnsafe(nodeToReplace.A, nodeToReplace.B, out root.A); + root.A.Index = nodeIndexToReplace; + root.A.LeafCount = nodeToReplace.A.LeafCount + nodeToReplace.B.LeafCount; + root.B = childToShiftUp; + Metanodes[nodeIndexToReplace] = new Metanode { Parent = rotationRootIndex, IndexInParent = 0 }; + if (childToShiftUp.Index < 0) Leaves[Encode(childToShiftUp.Index)] = new Leaf(rotationRootIndex, 1); else Metanodes[childToShiftUp.Index] = new Metanode { Parent = rotationRootIndex, IndexInParent = 1 }; + if (nodeToReplace.A.Index < 0) Leaves[Encode(nodeToReplace.A.Index)] = new Leaf(nodeIndexToReplace, 0); else Metanodes[nodeToReplace.A.Index] = new Metanode { Parent = nodeIndexToReplace, IndexInParent = 0 }; + if (nodeToReplace.B.Index < 0) Leaves[Encode(nodeToReplace.B.Index)] = new Leaf(nodeIndexToReplace, 1); else Metanodes[nodeToReplace.B.Index] = new Metanode { Parent = nodeIndexToReplace, IndexInParent = 1 }; + } + else + { + //Right rotation wins! + var nodeIndexToReplace = root.A.Index; + ref var nodeToReplace = ref Nodes[nodeIndexToReplace]; + var childToShiftUp = rightUsesA ? nodeToReplace.B : nodeToReplace.A; + var childToShiftRight = rightUsesA ? nodeToReplace.A : nodeToReplace.B; + nodeToReplace.A = childToShiftRight; + nodeToReplace.B = root.B; + BoundingBox.CreateMergedUnsafe(nodeToReplace.A, nodeToReplace.B, out root.B); + root.B.Index = nodeIndexToReplace; + root.B.LeafCount = nodeToReplace.A.LeafCount + nodeToReplace.B.LeafCount; + root.A = childToShiftUp; + Metanodes[nodeIndexToReplace] = new Metanode { Parent = rotationRootIndex, IndexInParent = 1 }; + if (childToShiftUp.Index < 0) Leaves[Encode(childToShiftUp.Index)] = new Leaf(rotationRootIndex, 0); else Metanodes[childToShiftUp.Index] = new Metanode { Parent = rotationRootIndex, IndexInParent = 0 }; + if (nodeToReplace.A.Index < 0) Leaves[Encode(nodeToReplace.A.Index)] = new Leaf(nodeIndexToReplace, 0); else Metanodes[nodeToReplace.A.Index] = new Metanode { Parent = nodeIndexToReplace, IndexInParent = 0 }; + if (nodeToReplace.B.Index < 0) Leaves[Encode(nodeToReplace.B.Index)] = new Leaf(nodeIndexToReplace, 1); else Metanodes[nodeToReplace.B.Index] = new Metanode { Parent = nodeIndexToReplace, IndexInParent = 1 }; + } + } + } + + [MethodImpl(MethodImplOptions.NoInlining)] + internal static float ComputeBoundsMetric(ref BoundingBox bounds) + { + return ComputeBoundsMetric(ref bounds.Min, ref bounds.Max); + } + [MethodImpl(MethodImplOptions.NoInlining)] + internal static float ComputeBoundsMetric(ref Vector3 min, ref Vector3 max) + { + //Note that we just use the SAH. While we are primarily interested in volume queries for the purposes of collision detection, the topological difference + //between a volume heuristic and surface area heuristic isn't huge. There is, however, one big annoying issue that volume heuristics run into: + //all bounding boxes with one extent equal to zero have zero cost. Surface area approaches avoid this hole simply. + var offset = max - min; + //Note that this is merely proportional to surface area. Being scaled by a constant factor is irrelevant. + return offset.X * offset.Y + offset.Y * offset.Z + offset.X * offset.Z; } -} +} \ No newline at end of file diff --git a/BepuPhysics/Trees/Tree_Add2.cs b/BepuPhysics/Trees/Tree_Add2.cs deleted file mode 100644 index 36c85e2d8..000000000 --- a/BepuPhysics/Trees/Tree_Add2.cs +++ /dev/null @@ -1,258 +0,0 @@ -using BepuUtilities; -using System.Numerics; -using System.Runtime.CompilerServices; -using System; -using System.Diagnostics; -using BepuUtilities.Memory; -using BepuUtilities.Collections; -using System.Collections.Generic; - -namespace BepuPhysics.Trees; - -partial struct Tree -{ - - /// - /// Adds a leaf to the tree with the given bounding box and returns the index of the added leaf. - /// - /// Extents of the leaf bounds. - /// Resource pool to use if resizing is required. - /// Index of the leaf allocated in the tree's leaf array. - public unsafe int Add2(BoundingBox bounds, BufferPool pool) - { - //The rest of the function assumes we have sufficient room. We don't want to deal with invalidated pointers mid-add. - if (Leaves.Length == LeafCount) - { - //Note that, while we add 1, the underlying pool will request the next higher power of 2 in bytes that can hold it. - //Since we're already at capacity, that will be ~double the size. - Resize(pool, LeafCount + 1); - } - - if (LeafCount < 2) - { - //The root is partial. - ref var leafChild = ref Unsafe.Add(ref Nodes[0].A, LeafCount); - leafChild = Unsafe.As(ref bounds); - var leafIndex = AddLeaf(0, LeafCount); - leafChild.Index = Encode(leafIndex); - leafChild.LeafCount = 1; - return leafIndex; - } - - //The tree is complete; traverse to find the best place to insert the leaf. - - int nodeIndex = 0; - var bounds4 = Unsafe.As(ref bounds); - while (true) - { - ref var node = ref Nodes[nodeIndex]; - //Choose whichever child requires less bounds expansion. If they're tied, choose the one with the least leaf count. - BoundingBox.CreateMergedUnsafe(bounds4, node.A, out var mergedA); - BoundingBox.CreateMergedUnsafe(bounds4, node.B, out var mergedB); - var boundsIncreaseA = ComputeBoundsMetric(mergedA) - ComputeBoundsMetric(Unsafe.As(ref node.A)); - var boundsIncreaseB = ComputeBoundsMetric(mergedB) - ComputeBoundsMetric(Unsafe.As(ref node.B)); - var useA = boundsIncreaseA == boundsIncreaseB ? node.A.LeafCount < node.B.LeafCount : boundsIncreaseA < boundsIncreaseB; - ref var merged = ref Unsafe.As(ref useA ? ref mergedA : ref mergedB); - ref var chosenChild = ref useA ? ref node.A : ref node.B; - if (chosenChild.LeafCount == 1) - { - //The merge target is a leaf. We'll need a new internal node. - var newNodeIndex = AllocateNode(); - ref var newNode = ref Nodes[newNodeIndex]; - ref var newMetanode = ref Metanodes[newNodeIndex]; - //Initialize the metanode of the new node. - newMetanode.Parent = nodeIndex; - newMetanode.IndexInParent = useA ? 0 : 1; - //Create the new child for the inserted leaf. - newNode.A = Unsafe.As(ref bounds); - newNode.A.LeafCount = 1; - var newLeafIndex = AddLeaf(newNodeIndex, 0); - newNode.A.Index = Encode(newLeafIndex); - //Move the leaf that used to be in the parent down into its new slot. - newNode.B = chosenChild; - //Update the moved leaf's location in the leaves. (Note that AddLeaf handled the leaves for the just-inserted leaf.) - Leaves[Encode(chosenChild.Index)] = new Leaf(newNodeIndex, 1); - //Update the parent's child reference to point to the new node. Note that the chosenChild still points to the slot we want. - chosenChild = merged; - chosenChild.LeafCount = 2; - chosenChild.Index = newNodeIndex; - return newLeafIndex; - } - else - { - //Just traversing into an internal node. (This could be microoptimized a wee bit. Similar above.) - var index = chosenChild.Index; - var leafCount = chosenChild.LeafCount + 1; - chosenChild = merged; - chosenChild.Index = index; - chosenChild.LeafCount = leafCount; - nodeIndex = index; - } - - } - } - - - /// - /// Adds a leaf to the tree with the given bounding box and returns the index of the added leaf. - /// - /// Extents of the leaf bounds. - /// Resource pool to use if resizing is required. - /// Index of the leaf allocated in the tree's leaf array. - public unsafe int Add4(BoundingBox bounds, BufferPool pool) - { - //The rest of the function assumes we have sufficient room. We don't want to deal with invalidated pointers mid-add. - if (Leaves.Length == LeafCount) - { - //Note that, while we add 1, the underlying pool will request the next higher power of 2 in bytes that can hold it. - //Since we're already at capacity, that will be ~double the size. - Resize(pool, LeafCount + 1); - } - - if (LeafCount < 2) - { - //The root is partial. - ref var leafChild = ref Unsafe.Add(ref Nodes[0].A, LeafCount); - leafChild = Unsafe.As(ref bounds); - var leafIndex = AddLeaf(0, LeafCount); - leafChild.Index = Encode(leafIndex); - leafChild.LeafCount = 1; - return leafIndex; - } - - //The tree is complete; traverse to find the best place to insert the leaf. - - int nodeIndex = 0; - var bounds4 = Unsafe.As(ref bounds); - var newNodeIndex = AllocateNode(); - //We only ever insert into child A, and it's guaranteed to belong to a new node, so we don't have to wait to add the leaf. - var newLeafIndex = AddLeaf(newNodeIndex, 0); - while (true) - { - ref var node = ref Nodes[nodeIndex]; - //Choose whichever child requires less bounds expansion. If they're tied, choose the one with the least leaf count. - BoundingBox.CreateMergedUnsafe(bounds4, node.A, out var mergedA); - BoundingBox.CreateMergedUnsafe(bounds4, node.B, out var mergedB); - var boundsIncreaseA = ComputeBoundsMetric(mergedA) - ComputeBoundsMetric(Unsafe.As(ref node.A)); - var boundsIncreaseB = ComputeBoundsMetric(mergedB) - ComputeBoundsMetric(Unsafe.As(ref node.B)); - var useA = boundsIncreaseA == boundsIncreaseB ? node.A.LeafCount < node.B.LeafCount : boundsIncreaseA < boundsIncreaseB; - ref var merged = ref Unsafe.As(ref useA ? ref mergedA : ref mergedB); - ref var chosenChild = ref useA ? ref node.A : ref node.B; - if (chosenChild.LeafCount == 1) - { - //The merge target is a leaf. We'll need a new internal node. - ref var newNode = ref Nodes[newNodeIndex]; - ref var newMetanode = ref Metanodes[newNodeIndex]; - //Initialize the metanode of the new node. - newMetanode.Parent = nodeIndex; - newMetanode.IndexInParent = useA ? 0 : 1; - //Create the new child for the inserted leaf. - newNode.A = Unsafe.As(ref bounds); - newNode.A.LeafCount = 1; - newNode.A.Index = Encode(newLeafIndex); - //Move the leaf that used to be in the parent down into its new slot. - newNode.B = chosenChild; - //Update the moved leaf's location in the leaves. (Note that AddLeaf handled the leaves for the just-inserted leaf.) - Leaves[Encode(chosenChild.Index)] = new Leaf(newNodeIndex, 1); - //Update the parent's child reference to point to the new node. Note that the chosenChild still points to the slot we want. - chosenChild = merged; - chosenChild.LeafCount = 2; - chosenChild.Index = newNodeIndex; - break; - } - else - { - //Just traversing into an internal node. (This could be microoptimized a wee bit. Similar above.) - var index = chosenChild.Index; - var leafCount = chosenChild.LeafCount + 1; - chosenChild = merged; - chosenChild.Index = index; - chosenChild.LeafCount = leafCount; - nodeIndex = index; - } - } - - //Try rotating a path from the insertion location up to the root to compensate for bad insertion orders. - var parentIndex = Leaves[newLeafIndex].NodeIndex; - while (parentIndex >= 0) - { - TryRotateNode(parentIndex); - parentIndex = Metanodes[parentIndex].Parent; - } - return newLeafIndex; - } - - private unsafe void TryRotateNode(int rotationRootIndex) - { - ref var root = ref Nodes[rotationRootIndex]; - var costA = ComputeBoundsMetric(Unsafe.As(ref root.A)); - var costB = ComputeBoundsMetric(Unsafe.As(ref root.B)); - float leftRotationCostChange = 0; - bool leftUsesA = false; - float rightRotationCostChange = 0; - bool rightUsesA = false; - if (root.A.Index >= 0) - { - //Try a right rotation. root.B will merge with the better of A's children, while the worse of A's children will take the place of root.A. - ref var a = ref Nodes[root.A.Index]; - BoundingBox.CreateMergedUnsafe(a.A, root.B, out var aaB); - BoundingBox.CreateMergedUnsafe(a.B, root.B, out var abB); - var costAAB = ComputeBoundsMetric(Unsafe.As(ref aaB)); - var costABB = ComputeBoundsMetric(Unsafe.As(ref abB)); - rightUsesA = costAAB < costABB; - rightRotationCostChange = float.Min(costAAB, costABB) - costA; - } - if (root.B.Index >= 0) - { - //Try a left rotation. root.A will merge with the better of B's children, while the worse of B's children will take the place of root.B. - ref var b = ref Nodes[root.B.Index]; - BoundingBox.CreateMergedUnsafe(root.A, b.A, out var baB); - BoundingBox.CreateMergedUnsafe(root.A, b.B, out var bbB); - var costBAB = ComputeBoundsMetric(Unsafe.As(ref baB)); - var costBBB = ComputeBoundsMetric(Unsafe.As(ref bbB)); - leftUsesA = costBAB < costBBB; - leftRotationCostChange = float.Min(costBAB, costBBB) - costB; - } - if (float.Min(leftRotationCostChange, rightRotationCostChange) < 0) - { - //A rotation is worth it. - if (leftRotationCostChange < rightRotationCostChange) - { - //Left rotation wins! - var nodeIndexToReplace = root.B.Index; - ref var nodeToReplace = ref Nodes[nodeIndexToReplace]; - var childToShiftUp = leftUsesA ? nodeToReplace.B : nodeToReplace.A; - var childToShiftLeft = leftUsesA ? nodeToReplace.A : nodeToReplace.B; - nodeToReplace.A = root.A; - nodeToReplace.B = childToShiftLeft; - BoundingBox.CreateMergedUnsafe(nodeToReplace.A, nodeToReplace.B, out root.A); - root.A.Index = nodeIndexToReplace; - root.A.LeafCount = nodeToReplace.A.LeafCount + nodeToReplace.B.LeafCount; - root.B = childToShiftUp; - Metanodes[nodeIndexToReplace] = new Metanode { Parent = rotationRootIndex, IndexInParent = 0 }; - if (childToShiftUp.Index < 0) Leaves[Encode(childToShiftUp.Index)] = new Leaf(rotationRootIndex, 1); else Metanodes[childToShiftUp.Index] = new Metanode { Parent = rotationRootIndex, IndexInParent = 1 }; - if (nodeToReplace.A.Index < 0) Leaves[Encode(nodeToReplace.A.Index)] = new Leaf(nodeIndexToReplace, 0); else Metanodes[nodeToReplace.A.Index] = new Metanode { Parent = nodeIndexToReplace, IndexInParent = 0 }; - if (nodeToReplace.B.Index < 0) Leaves[Encode(nodeToReplace.B.Index)] = new Leaf(nodeIndexToReplace, 1); else Metanodes[nodeToReplace.B.Index] = new Metanode { Parent = nodeIndexToReplace, IndexInParent = 1 }; - } - else - { - //Right rotation wins! - var nodeIndexToReplace = root.A.Index; - ref var nodeToReplace = ref Nodes[nodeIndexToReplace]; - var childToShiftUp = rightUsesA ? nodeToReplace.B : nodeToReplace.A; - var childToShiftRight = rightUsesA ? nodeToReplace.A : nodeToReplace.B; - nodeToReplace.A = childToShiftRight; - nodeToReplace.B = root.B; - BoundingBox.CreateMergedUnsafe(nodeToReplace.A, nodeToReplace.B, out root.B); - root.B.Index = nodeIndexToReplace; - root.B.LeafCount = nodeToReplace.A.LeafCount + nodeToReplace.B.LeafCount; - root.A = childToShiftUp; - Metanodes[nodeIndexToReplace] = new Metanode { Parent = rotationRootIndex, IndexInParent = 1 }; - if (childToShiftUp.Index < 0) Leaves[Encode(childToShiftUp.Index)] = new Leaf(rotationRootIndex, 0); else Metanodes[childToShiftUp.Index] = new Metanode { Parent = rotationRootIndex, IndexInParent = 0 }; - if (nodeToReplace.A.Index < 0) Leaves[Encode(nodeToReplace.A.Index)] = new Leaf(nodeIndexToReplace, 0); else Metanodes[nodeToReplace.A.Index] = new Metanode { Parent = nodeIndexToReplace, IndexInParent = 0 }; - if (nodeToReplace.B.Index < 0) Leaves[Encode(nodeToReplace.B.Index)] = new Leaf(nodeIndexToReplace, 1); else Metanodes[nodeToReplace.B.Index] = new Metanode { Parent = nodeIndexToReplace, IndexInParent = 1 }; - } - } - - } -} \ No newline at end of file From b508f9c43a12ffa17fe16c2084ac0fc90bcac2b1 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 14 Aug 2023 18:54:21 -0500 Subject: [PATCH 800/947] Fixed task spawning gap in refits; now load balances far better. --- BepuPhysics/CollisionDetection/BroadPhase.cs | 9 ++++---- BepuPhysics/Trees/Tree_Refine2.cs | 1 + BepuPhysics/Trees/Tree_Refit2.cs | 24 +++++++++++++++----- 3 files changed, 24 insertions(+), 10 deletions(-) diff --git a/BepuPhysics/CollisionDetection/BroadPhase.cs b/BepuPhysics/CollisionDetection/BroadPhase.cs index 4ca467b79..64656be90 100644 --- a/BepuPhysics/CollisionDetection/BroadPhase.cs +++ b/BepuPhysics/CollisionDetection/BroadPhase.cs @@ -287,9 +287,11 @@ public static void DefaultRefinementScheduler(float optimizationFraction, int ro subtreeRefinementSize = (int)float.Ceiling(sqrtLeafCount * subtreeRefinementSizeScale); //Note that we scale up the cost of the root refinement; it uses a sequentialized priority queue to collect subtrees for refinement and costs more. - var subtreeRefinementsPerRootRefinement = (int)float.Ceiling(subtreeRefinementSize * float.Log2(subtreeRefinementSize) / (targetRootRefinementSize * float.Log2(targetRootRefinementSize))); + var subtreeRefinementsPerRootRefinementInCost = targetRootRefinementSize * float.Log2(targetRootRefinementSize) / (subtreeRefinementSize * float.Log2(subtreeRefinementSize)); //If we're refining the root, reduce the number of subtree refinements to avoid cost spikes. - subtreeRefinementCount = int.Max(0, (int)float.Ceiling((float)targetOptimizedLeafCount / subtreeRefinementSize) - (refineRoot ? subtreeRefinementsPerRootRefinement : 0)); + subtreeRefinementCount = int.Max(0, (int)float.Round((float)targetOptimizedLeafCount / subtreeRefinementSize - (refineRoot ? subtreeRefinementsPerRootRefinementInCost : 0))); + if (!refineRoot) + subtreeRefinementCount = int.Max(1, subtreeRefinementCount); rootRefinementSize = refineRoot ? targetRootRefinementSize : 0; usePriorityQueue = (frameIndex / rootRefinementPeriod) % nonpriorityPeriod != 0; @@ -346,7 +348,7 @@ static void ActiveEntrypointTask(long taskId, void* untypedContext, int workerIn ref var context = ref *(RefinementContext*)untypedContext; var pool = dispatcher.WorkerPools[workerIndex]; context.Tree.Refine2(context.RootRefinementSize, ref context.SubtreeRefinementStartIndex, context.SubtreeRefinementCount, context.SubtreeRefinementSize, pool, dispatcher, context.TaskStack, workerIndex, targetTaskCount: context.TargetTaskCount, deterministic: context.Deterministic, usePriorityQueue: context.UsePriorityQueue); - //Now refit! Note that we use all but one task. It doesn't affect the performance of a refit much (we're not compute bound), and we can use it to do an incremental cache optimization on the static tree. + //Now refit! var sourceNodes = context.Tree.Nodes; context.Tree.Nodes = context.TargetNodes; context.Tree.Refit2WithCacheOptimization(sourceNodes, pool, dispatcher, context.TaskStack, workerIndex, context.TargetTotalTaskCount); @@ -364,7 +366,6 @@ public void Update2(IThreadDispatcher threadDispatcher = null, bool deterministi { ActiveRefinementSchedule(frameIndex, ActiveTree, out var activeRootRefinementSize, out var activeSubtreeRefinementCount, out var activeSubtreeRefinementSize, out var usePriorityQueueActive); StaticRefinementSchedule(frameIndex, StaticTree, out var staticRootRefinementSize, out var staticSubtreeRefinementCount, out var staticSubtreeRefinementSize, out var usePriorityQueueStatic); - Console.WriteLine($"root size: {activeRootRefinementSize}, subtree count: {activeSubtreeRefinementCount}, subtree size: {activeSubtreeRefinementSize}, usePQ: {usePriorityQueueActive}"); const int minimumLeafCountForThreading = 256; if (threadDispatcher != null && threadDispatcher.ThreadCount > 1 && (ActiveTree.LeafCount >= minimumLeafCountForThreading || StaticTree.LeafCount >= minimumLeafCountForThreading)) { diff --git a/BepuPhysics/Trees/Tree_Refine2.cs b/BepuPhysics/Trees/Tree_Refine2.cs index b1fddd74d..d71b890e3 100644 --- a/BepuPhysics/Trees/Tree_Refine2.cs +++ b/BepuPhysics/Trees/Tree_Refine2.cs @@ -720,6 +720,7 @@ private unsafe void Refine2(int rootRefinementSize, ref int subtreeRefinementSta subtreeRefinementCount = 0; if (targetTaskBudget < 0) targetTaskBudget = threadDispatcher.ThreadCount; + //Clamp refinement sizes to avoid pointless overallocations when the user supplies odd inputs. rootRefinementSize = int.Min(rootRefinementSize, LeafCount); subtreeRefinementSize = int.Min(subtreeRefinementSize, LeafCount); diff --git a/BepuPhysics/Trees/Tree_Refit2.cs b/BepuPhysics/Trees/Tree_Refit2.cs index da147110d..ef634b618 100644 --- a/BepuPhysics/Trees/Tree_Refit2.cs +++ b/BepuPhysics/Trees/Tree_Refit2.cs @@ -69,14 +69,20 @@ unsafe readonly void Refit2WithTaskSpawning(ref NodeChild childInParent, RefitCo } else { - //At least one child is too small to warrant a new task. Just recurse on both. + //At least one child is too small to warrant a new task. The larger one may still be worth spawning subtasks within, though. if (a.Index >= 0) { - Refit2(ref a); + if (a.LeafCount >= context->LeafCountPerTask) + Refit2WithTaskSpawning(ref a, context, workerIndex, dispatcher); + else + Refit2(ref a); } if (b.Index >= 0) { - Refit2(ref b); + if (b.LeafCount >= context->LeafCountPerTask) + Refit2WithTaskSpawning(ref b, context, workerIndex, dispatcher); + else + Refit2(ref b); } } BoundingBox.CreateMergedUnsafeWithPreservation(a, b, out childInParent); @@ -270,12 +276,15 @@ static unsafe void Refit2WithCacheOptimizationAndTaskSpawning( } else { - //At least one child is too small to warrant a new task. Just recurse on both. + //At least one child is too small to warrant a new task. The larger one may still be worth spawning subtasks within, though. if (sourceA.Index >= 0) { targetA.Index = targetIndexA; targetA.LeafCount = sourceA.LeafCount; - Refit2WithCacheOptimization(sourceA.Index, targetNodeIndex, 0, ref targetA, ref *context); + if (sourceA.LeafCount >= context->LeafCountPerTask) + Refit2WithCacheOptimizationAndTaskSpawning(sourceA.Index, targetNodeIndex, 0, ref targetA, context, workerIndex, dispatcher); + else + Refit2WithCacheOptimization(sourceA.Index, targetNodeIndex, 0, ref targetA, ref *context); } else { @@ -287,7 +296,10 @@ static unsafe void Refit2WithCacheOptimizationAndTaskSpawning( { targetB.Index = targetIndexB; targetB.LeafCount = sourceB.LeafCount; - Refit2WithCacheOptimization(sourceB.Index, targetNodeIndex, 1, ref targetB, ref *context); + if (sourceB.LeafCount >= context->LeafCountPerTask) + Refit2WithCacheOptimizationAndTaskSpawning(sourceB.Index, targetNodeIndex, 1, ref targetB, context, workerIndex, dispatcher); + else + Refit2WithCacheOptimization(sourceB.Index, targetNodeIndex, 1, ref targetB, ref *context); } else { From 774714ca7e5f1aeb4b598d3c83ce3b51da556ada Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 23 Aug 2023 16:44:18 -0500 Subject: [PATCH 801/947] Little bit of validation. --- BepuUtilities/ThreadDispatcher.cs | 2 ++ 1 file changed, 2 insertions(+) diff --git a/BepuUtilities/ThreadDispatcher.cs b/BepuUtilities/ThreadDispatcher.cs index 773f4558e..355bdf762 100644 --- a/BepuUtilities/ThreadDispatcher.cs +++ b/BepuUtilities/ThreadDispatcher.cs @@ -40,6 +40,8 @@ struct Worker /// Size of memory blocks to allocate for thread pools. public ThreadDispatcher(int threadCount, int threadPoolBlockAllocationSize = 16384) { + if (threadCount <= 0) + throw new ArgumentOutOfRangeException(nameof(threadCount), "Thread count must be positive."); this.threadCount = threadCount; workers = new Worker[threadCount - 1]; for (int i = 0; i < workers.Length; ++i) From 754cd0cbaf58d603c8ab8dd353709af735f6b4dd Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 24 Aug 2023 16:15:32 -0500 Subject: [PATCH 802/947] Added a Bodies.CountBodies helper. --- BepuPhysics/Bodies.cs | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/BepuPhysics/Bodies.cs b/BepuPhysics/Bodies.cs index 30adf74cc..4db50c950 100644 --- a/BepuPhysics/Bodies.cs +++ b/BepuPhysics/Bodies.cs @@ -575,6 +575,24 @@ public bool BodyExists(BodyHandle bodyHandle) return bodyHandle.Value >= 0 && bodyHandle.Value < HandleToLocation.Length && HandleToLocation[bodyHandle.Value].SetIndex >= 0; } + /// + /// Computes the number of bodies contained in the simulation. + /// + /// Number of bodies contained in the simulation. + /// Enumerates all instances in the collection, summing the body counts for every allocated instance. + /// For simulations with very large numbers of sleeping body sets, this is not a trivial operation. + public int CountBodies() + { + int count = 0; + for (int i = 0; i < Sets.Length; ++i) + { + ref var set = ref Sets[i]; + if (set.Allocated) + count += set.Count; + } + return count; + } + [Conditional("DEBUG")] internal void ValidateExistingHandle(BodyHandle handle) { From cc5a3955d85a4e0c49163b5d455d868b56db2fec Mon Sep 17 00:00:00 2001 From: Salvage <29021710+Saalvage@users.noreply.github.com> Date: Mon, 21 Aug 2023 02:44:28 +0200 Subject: [PATCH 803/947] Utilize static virtual members Primarily intended to eliminate the following two antipatterns: - `default(T).Method()` - `instance.Method(ref instance)` --- BepuPhysics/Collidables/BigCompound.cs | 4 +- BepuPhysics/Collidables/Box.cs | 6 +- BepuPhysics/Collidables/Capsule.cs | 6 +- BepuPhysics/Collidables/Compound.cs | 4 +- BepuPhysics/Collidables/ConvexHull.cs | 6 +- BepuPhysics/Collidables/Cylinder.cs | 6 +- BepuPhysics/Collidables/IShape.cs | 10 +- BepuPhysics/Collidables/Mesh.cs | 4 +- BepuPhysics/Collidables/Shapes.cs | 8 +- BepuPhysics/Collidables/Sphere.cs | 6 +- BepuPhysics/Collidables/Triangle.cs | 6 +- .../CollisionDetection/CollisionBatcher.cs | 2 +- .../CollisionTaskRegistry.cs | 2 +- .../CollisionTasks/BoxConvexHullTester.cs | 8 +- .../CollisionTasks/BoxCylinderTester.cs | 8 +- .../CollisionTasks/BoxPairTester.cs | 8 +- .../CollisionTasks/BoxTriangleTester.cs | 8 +- .../CollisionTasks/CapsuleBoxTester.cs | 8 +- .../CollisionTasks/CapsuleConvexHullTester.cs | 8 +- .../CollisionTasks/CapsuleCylinderTester.cs | 8 +- .../CollisionTasks/CapsulePairTester.cs | 8 +- .../CollisionTasks/CapsuleTriangleTester.cs | 8 +- .../CompoundMeshContinuations.cs | 2 +- .../CompoundPairCollisionTask.cs | 9 +- .../CompoundPairOverlapFinder.cs | 2 +- .../CollisionTasks/ConvexCollisionTask.cs | 59 ++-- .../ConvexCompoundCollisionTask.cs | 4 +- .../CollisionTasks/ConvexHullPairTester.cs | 8 +- .../CollisionTasks/ConvexMeshContinuations.cs | 2 +- .../CylinderConvexHullTester.cs | 8 +- .../CollisionTasks/CylinderPairTester.cs | 8 +- .../CollisionTasks/MeshPairContinuations.cs | 4 +- .../CollisionTasks/MeshPairOverlapFinder.cs | 2 +- .../CollisionTasks/PairTypes.cs | 114 ++++---- .../CollisionTasks/SphereBoxTester.cs | 8 +- .../CollisionTasks/SphereCapsuleTester.cs | 8 +- .../CollisionTasks/SphereConvexHullTester.cs | 8 +- .../CollisionTasks/SphereCylinderTester.cs | 8 +- .../CollisionTasks/SpherePairTester.cs | 8 +- .../CollisionTasks/SphereTriangleTester.cs | 8 +- .../TriangleConvexHullTester.cs | 8 +- .../CollisionTasks/TriangleCylinderTester.cs | 8 +- .../CollisionTasks/TrianglePairTester.cs | 8 +- .../ContactConstraintAccessor.cs | 10 +- .../NarrowPhasePendingConstraintAdds.cs | 2 +- .../CollisionDetection/SweepTaskRegistry.cs | 2 +- .../CompoundHomogeneousCompoundSweepTask.cs | 7 +- .../CompoundPairSweepOverlapFinder.cs | 4 +- .../SweepTasks/CompoundPairSweepTask.cs | 6 +- .../ConvexCompoundSweepOverlapFinder.cs | 4 +- .../SweepTasks/ConvexCompoundSweepTask.cs | 10 +- .../ConvexHomogeneousCompoundSweepTask.cs | 8 +- .../SweepTasks/ConvexSweepTaskCommon.cs | 4 +- .../CollisionDetection/WideRayTester.cs | 2 +- .../Constraints/AngularAxisGearMotor.cs | 16 +- BepuPhysics/Constraints/AngularAxisMotor.cs | 16 +- BepuPhysics/Constraints/AngularHinge.cs | 16 +- BepuPhysics/Constraints/AngularMotor.cs | 16 +- BepuPhysics/Constraints/AngularServo.cs | 16 +- BepuPhysics/Constraints/AngularSwivelHinge.cs | 16 +- BepuPhysics/Constraints/AreaConstraint.cs | 16 +- BepuPhysics/Constraints/BallSocket.cs | 16 +- BepuPhysics/Constraints/BallSocketMotor.cs | 16 +- BepuPhysics/Constraints/BallSocketServo.cs | 16 +- .../Constraints/CenterDistanceConstraint.cs | 16 +- .../Constraints/CenterDistanceLimit.cs | 16 +- .../Contact/ContactConvexCommon.cs | 20 +- .../Constraints/Contact/ContactConvexTypes.cs | 264 +++++++++--------- .../Constraints/Contact/ContactConvexTypes.tt | 38 +-- .../Contact/ContactNonconvexCommon.cs | 112 ++++---- .../Contact/ContactNonconvexTypes.cs | 150 +++++----- .../Contact/ContactNonconvexTypes.tt | 44 +-- .../Contact/IContactConstraintDescription.cs | 16 +- BepuPhysics/Constraints/DistanceLimit.cs | 18 +- BepuPhysics/Constraints/DistanceServo.cs | 16 +- .../Constraints/FourBodyTypeProcessor.cs | 25 +- BepuPhysics/Constraints/Hinge.cs | 16 +- .../Constraints/IConstraintDescription.cs | 8 +- BepuPhysics/Constraints/LinearAxisLimit.cs | 16 +- BepuPhysics/Constraints/LinearAxisMotor.cs | 16 +- BepuPhysics/Constraints/LinearAxisServo.cs | 16 +- .../Constraints/OneBodyAngularMotor.cs | 16 +- .../Constraints/OneBodyAngularServo.cs | 16 +- BepuPhysics/Constraints/OneBodyLinearMotor.cs | 16 +- BepuPhysics/Constraints/OneBodyLinearServo.cs | 16 +- .../Constraints/OneBodyTypeProcessor.cs | 25 +- BepuPhysics/Constraints/PointOnLineServo.cs | 16 +- BepuPhysics/Constraints/SwingLimit.cs | 16 +- BepuPhysics/Constraints/SwivelHinge.cs | 16 +- .../Constraints/ThreeBodyTypeProcessor.cs | 29 +- BepuPhysics/Constraints/TwistLimit.cs | 16 +- BepuPhysics/Constraints/TwistMotor.cs | 16 +- BepuPhysics/Constraints/TwistServo.cs | 16 +- .../Constraints/TwoBodyTypeProcessor.cs | 23 +- BepuPhysics/Constraints/TypeProcessor.cs | 8 +- BepuPhysics/Constraints/VolumeConstraint.cs | 16 +- BepuPhysics/Constraints/Weld.cs | 16 +- BepuPhysics/SequentialFallbackBatch.cs | 10 +- BepuPhysics/Simulation_Queries.cs | 4 +- BepuPhysics/Solver.cs | 37 ++- .../ConvexCollisionTesterBenchmarks.cs | 12 +- .../FourBodyConstraintBenchmarks.cs | 5 +- DemoBenchmarks/OneBodyConstraintBenchmarks.cs | 5 +- DemoBenchmarks/ShapeRayBenchmarksDeep.cs | 2 +- DemoBenchmarks/SweepBenchmarks.cs | 4 +- .../ThreeBodyConstraintBenchmarks.cs | 5 +- DemoBenchmarks/TwoBodyConstraintBenchmarks.cs | 5 +- .../AngularSwivelHingeLineExtractor.cs | 4 +- .../Constraints/BallSocketLineExtractor.cs | 4 +- .../BallSocketMotorLineExtractor.cs | 4 +- .../BallSocketServoLineExtractor.cs | 4 +- .../CenterDistanceLimitLineExtractor.cs | 4 +- .../CenterDistanceLineExtractor.cs | 4 +- .../Constraints/ConstraintLineExtractor.cs | 11 +- .../Constraints/ContactLineExtractors.cs | 56 ++-- .../Constraints/ContactLineExtractors.tt | 4 +- .../Constraints/DistanceLimitLineExtractor.cs | 4 +- .../Constraints/DistanceServoLineExtractor.cs | 4 +- .../Constraints/HingeLineExtractor.cs | 4 +- .../LinearAxisServoLineExtractor.cs | 4 +- .../OneBodyLinearServoLineExtractor.cs | 4 +- .../Constraints/PointOnLineLineExtractor.cs | 4 +- .../Constraints/SwivelHingeLineExtractor.cs | 4 +- DemoRenderer/Constraints/WeldLineExtractor.cs | 4 +- DemoRenderer/ShapeDrawing/ShapesExtractor.cs | 2 +- .../Characters/CharacterMotionConstraint.cs | 32 +-- .../Characters/CharacterMotionConstraint.tt | 16 +- Demos/Demos/ClothDemo.cs | 2 +- Demos/Demos/CollisionQueryDemo.cs | 2 +- Demos/Demos/CustomVoxelCollidableDemo.cs | 4 +- Demos/Demos/Dancers/DemoDancers.cs | 4 +- Demos/Demos/NewtDemo.cs | 2 +- Demos/Demos/SolverContactEnumerationDemo.cs | 20 +- Demos/Demos/SweepDemo.cs | 6 +- Demos/SpecializedTests/RayTesting.cs | 55 ++-- Demos/SpecializedTests/TriangleTestDemo.cs | 14 +- 136 files changed, 1036 insertions(+), 1078 deletions(-) diff --git a/BepuPhysics/Collidables/BigCompound.cs b/BepuPhysics/Collidables/BigCompound.cs index 52e7c677a..8b2f2059f 100644 --- a/BepuPhysics/Collidables/BigCompound.cs +++ b/BepuPhysics/Collidables/BigCompound.cs @@ -203,7 +203,7 @@ public unsafe void RayTest(in RigidPose pose, ref RaySource rays } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapes) + public static ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapes) { return new CompoundShapeBatch(pool, initialCapacity, shapes); } @@ -342,7 +342,7 @@ public void Dispose(BufferPool bufferPool) /// Type id of compound shapes. /// public const int Id = 7; - public int TypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { return Id; } } + public static int TypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { return Id; } } } diff --git a/BepuPhysics/Collidables/Box.cs b/BepuPhysics/Collidables/Box.cs index a7f989b90..f59537c73 100644 --- a/BepuPhysics/Collidables/Box.cs +++ b/BepuPhysics/Collidables/Box.cs @@ -162,7 +162,7 @@ public readonly BodyInertia ComputeInertia(float mass) return inertia; } - public readonly ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches) + public static ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches) { return new ConvexShapeBatch(pool, initialCapacity); } @@ -171,7 +171,7 @@ public readonly ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity /// Type id of box shapes. /// public const int Id = 2; - public readonly int TypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { return Id; } } + public static int TypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { return Id; } } } @@ -221,7 +221,7 @@ public void GetBounds(ref QuaternionWide orientations, int countInBundle, out Ve maximumAngularExpansion = maximumRadius - Vector.Min(HalfLength, Vector.Min(HalfHeight, HalfLength)); } - public int MinimumWideRayCount + public static int MinimumWideRayCount { [MethodImpl(MethodImplOptions.AggressiveInlining)] get diff --git a/BepuPhysics/Collidables/Capsule.cs b/BepuPhysics/Collidables/Capsule.cs index 38865fdd1..b5cb17f83 100644 --- a/BepuPhysics/Collidables/Capsule.cs +++ b/BepuPhysics/Collidables/Capsule.cs @@ -179,7 +179,7 @@ public readonly BodyInertia ComputeInertia(float mass) return inertia; } - public readonly ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches) + public static ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches) { return new ConvexShapeBatch(pool, initialCapacity); } @@ -190,7 +190,7 @@ public readonly ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity /// Type id of capsule shapes. /// public const int Id = 1; - public readonly int TypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { return Id; } } + public static int TypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { return Id; } } } public struct CapsuleWide : IShapeWide @@ -238,7 +238,7 @@ public void GetBounds(ref QuaternionWide orientations, int countInBundle, out Ve maximumAngularExpansion = HalfLength; } - public int MinimumWideRayCount + public static int MinimumWideRayCount { [MethodImpl(MethodImplOptions.AggressiveInlining)] get diff --git a/BepuPhysics/Collidables/Compound.cs b/BepuPhysics/Collidables/Compound.cs index 30b4471b9..2cbdf4b91 100644 --- a/BepuPhysics/Collidables/Compound.cs +++ b/BepuPhysics/Collidables/Compound.cs @@ -250,7 +250,7 @@ public unsafe void RayTest(in RigidPose pose, ref RaySource rays } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapes) + public static ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapes) { return new CompoundShapeBatch(pool, initialCapacity, shapes); } @@ -371,7 +371,7 @@ public void Dispose(BufferPool bufferPool) /// Type id of list based compound shapes. /// public const int Id = 6; - public int TypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { return Id; } } + public static int TypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { return Id; } } } diff --git a/BepuPhysics/Collidables/ConvexHull.cs b/BepuPhysics/Collidables/ConvexHull.cs index 0e779a127..cb99b118a 100644 --- a/BepuPhysics/Collidables/ConvexHull.cs +++ b/BepuPhysics/Collidables/ConvexHull.cs @@ -193,7 +193,7 @@ public readonly BodyInertia ComputeInertia(float mass) return inertia; } - public readonly ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches) + public static ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches) { return new ConvexHullShapeBatch(pool, initialCapacity); } @@ -282,7 +282,7 @@ public void Dispose(BufferPool bufferPool) /// Type id of convex hull shapes. /// public const int Id = 5; - public readonly int TypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { return Id; } } + public static int TypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { return Id; } } } public struct ConvexHullWide : IShapeWide @@ -291,7 +291,7 @@ public struct ConvexHullWide : IShapeWide //The "wide" variant is simply a collection of convex hull instances. public Buffer Hulls; - public int MinimumWideRayCount => int.MaxValue; //'Wide' ray tests just fall through to scalar tests anyway. + public static int MinimumWideRayCount => int.MaxValue; //'Wide' ray tests just fall through to scalar tests anyway. public bool AllowOffsetMemoryAccess => false; public int InternalAllocationSize => Vector.Count * Unsafe.SizeOf(); diff --git a/BepuPhysics/Collidables/Cylinder.cs b/BepuPhysics/Collidables/Cylinder.cs index 6dee60c4c..8041c2d4e 100644 --- a/BepuPhysics/Collidables/Cylinder.cs +++ b/BepuPhysics/Collidables/Cylinder.cs @@ -176,7 +176,7 @@ public readonly BodyInertia ComputeInertia(float mass) return inertia; } - public readonly ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches) + public static ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches) { return new ConvexShapeBatch(pool, initialCapacity); } @@ -185,7 +185,7 @@ public readonly ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity /// Type id of cylinder shapes. /// public const int Id = 4; - public readonly int TypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { return Id; } } + public static int TypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { return Id; } } } public struct CylinderWide : IShapeWide @@ -233,7 +233,7 @@ public void GetBounds(ref QuaternionWide orientations, int countInBundle, out Ve maximumAngularExpansion = maximumRadius - Vector.Min(HalfLength, Radius); } - public int MinimumWideRayCount + public static int MinimumWideRayCount { [MethodImpl(MethodImplOptions.AggressiveInlining)] get diff --git a/BepuPhysics/Collidables/IShape.cs b/BepuPhysics/Collidables/IShape.cs index b86b28c24..39ea17741 100644 --- a/BepuPhysics/Collidables/IShape.cs +++ b/BepuPhysics/Collidables/IShape.cs @@ -14,11 +14,10 @@ namespace BepuPhysics.Collidables /// public interface IShape { - //TODO: Note that these should really be *static* as they do not need any information about an instance, but static abstract interface methods are not yet out of preview. /// /// Unique type id for this shape type. /// - int TypeId { get; } + static abstract int TypeId { get; } /// /// Creates a shape batch for this type of shape. /// @@ -26,9 +25,8 @@ public interface IShape /// Initial capacity to allocate within the batch. /// The set of shapes to contain this batch. /// Shape batch for the shape type. - /// This is typically used internally to initialize new shape collections in response to shapes being added. It is not likely to be useful outside of the engine. - /// Ideally, this would be implemented as a static abstract, but those aren't available yet. - ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches); + /// This is typically used internally to initialize new shape collections in response to shapes being added. It is not likely to be useful outside of the engine. + static abstract ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches); } //Note that the following bounds functions require only an orientation because the effect of the position on the bounding box is the same for all shapes. @@ -263,7 +261,7 @@ public interface IShapeWide where TShape : IShape /// /// Gets the lower bound on the number of rays to execute in a wide fashion. Ray bundles with fewer rays will fall back to the single ray code path. /// - int MinimumWideRayCount { get; } + static abstract int MinimumWideRayCount { get; } /// /// Tests a ray against the shape. diff --git a/BepuPhysics/Collidables/Mesh.cs b/BepuPhysics/Collidables/Mesh.cs index ed0e8516c..097eff774 100644 --- a/BepuPhysics/Collidables/Mesh.cs +++ b/BepuPhysics/Collidables/Mesh.cs @@ -255,7 +255,7 @@ public readonly void ComputeBounds(Quaternion orientation, out Vector3 min, out } } - public readonly ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches) + public static ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches) { return new HomogeneousCompoundShapeBatch(pool, initialCapacity); } @@ -544,7 +544,7 @@ public void Dispose(BufferPool bufferPool) /// Type id of mesh shapes. /// public const int Id = 8; - public readonly int TypeId => Id; + public static int TypeId => Id; } } diff --git a/BepuPhysics/Collidables/Shapes.cs b/BepuPhysics/Collidables/Shapes.cs index efbdb6982..e11bbe87e 100644 --- a/BepuPhysics/Collidables/Shapes.cs +++ b/BepuPhysics/Collidables/Shapes.cs @@ -136,7 +136,7 @@ public abstract class ShapeBatch : ShapeBatch where TShape : unmanaged, protected ShapeBatch(BufferPool pool, int initialShapeCount) { this.pool = pool; - TypeId = default(TShape).TypeId; + TypeId = TShape.TypeId; InternalResize(initialShapeCount, 0); idPool = new IdPool(initialShapeCount, pool); } @@ -433,14 +433,14 @@ public void UpdateBounds(Vector3 position, Quaternion orientation, TypedIndex sh [MethodImpl(MethodImplOptions.AggressiveInlining)] public ref TShape GetShape(int shapeIndex) where TShape : unmanaged, IShape { - var typeId = default(TShape).TypeId; + var typeId = TShape.TypeId; return ref Unsafe.As>(ref batches[typeId])[shapeIndex]; } public TypedIndex Add(in TShape shape) where TShape : unmanaged, IShape { - var typeId = default(TShape).TypeId; + var typeId = TShape.TypeId; if (RegisteredTypeSpan <= typeId) { registeredTypeSpan = typeId + 1; @@ -451,7 +451,7 @@ public TypedIndex Add(in TShape shape) where TShape : unmanaged, IShape } if (batches[typeId] == null) { - batches[typeId] = default(TShape).CreateShapeBatch(pool, InitialCapacityPerTypeBatch, this); + batches[typeId] = TShape.CreateShapeBatch(pool, InitialCapacityPerTypeBatch, this); } Debug.Assert(batches[typeId] is ShapeBatch); diff --git a/BepuPhysics/Collidables/Sphere.cs b/BepuPhysics/Collidables/Sphere.cs index 21f39c092..43ca92419 100644 --- a/BepuPhysics/Collidables/Sphere.cs +++ b/BepuPhysics/Collidables/Sphere.cs @@ -108,7 +108,7 @@ public readonly BodyInertia ComputeInertia(float mass) return inertia; } - public readonly ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapes) + public static ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapes) { return new ConvexShapeBatch(pool, initialCapacity); } @@ -118,7 +118,7 @@ public readonly ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity /// Type id of sphere shapes. /// public const int Id = 0; - public readonly int TypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { return Id; } } + public static int TypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { return Id; } } } public struct SphereWide : IShapeWide @@ -163,7 +163,7 @@ public void GetBounds(ref QuaternionWide orientations, int countInBundle, out Ve } - public int MinimumWideRayCount + public static int MinimumWideRayCount { [MethodImpl(MethodImplOptions.AggressiveInlining)] get diff --git a/BepuPhysics/Collidables/Triangle.cs b/BepuPhysics/Collidables/Triangle.cs index 998b37873..7378ed7b9 100644 --- a/BepuPhysics/Collidables/Triangle.cs +++ b/BepuPhysics/Collidables/Triangle.cs @@ -118,7 +118,7 @@ public readonly BodyInertia ComputeInertia(float mass) return inertia; } - public readonly ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches) + public static ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches) { return new ConvexShapeBatch(pool, initialCapacity); } @@ -127,7 +127,7 @@ public readonly ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity /// Type id of triangle shapes. /// public const int Id = 3; - public readonly int TypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { return Id; } } + public static int TypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { return Id; } } } @@ -220,7 +220,7 @@ public void GetBounds(ref QuaternionWide orientations, int countInBundle, out Ve maximumAngularExpansion = maximumRadius; } - public int MinimumWideRayCount + public static int MinimumWideRayCount { [MethodImpl(MethodImplOptions.AggressiveInlining)] get diff --git a/BepuPhysics/CollisionDetection/CollisionBatcher.cs b/BepuPhysics/CollisionDetection/CollisionBatcher.cs index ce7be21c4..8b619ab16 100644 --- a/BepuPhysics/CollisionDetection/CollisionBatcher.cs +++ b/BepuPhysics/CollisionDetection/CollisionBatcher.cs @@ -281,7 +281,7 @@ public unsafe void Add(TShapeA shapeA, TShapeB shapeB, Vector3 //that's actually used by the narrowphase (and which will likely be used for most performance sensitive cases). //TODO: You could recover the performance and safety once generic pointers exist. By having pointers in the parameter list, we can require that the user handle GC safety. //(We could also have an explicit 'unsafe' overload, but that API complexity doesn't seem worthwhile. My guess is nontrivial uses will all use the underlying function directly.) - Add(shapeA.TypeId, shapeB.TypeId, Unsafe.SizeOf(), Unsafe.SizeOf(), Unsafe.AsPointer(ref shapeA), Unsafe.AsPointer(ref shapeB), + Add(TShapeA.TypeId, TShapeB.TypeId, Unsafe.SizeOf(), Unsafe.SizeOf(), Unsafe.AsPointer(ref shapeA), Unsafe.AsPointer(ref shapeB), offsetB, orientationA, orientationB, speculativeMargin, pairId); } diff --git a/BepuPhysics/CollisionDetection/CollisionTaskRegistry.cs b/BepuPhysics/CollisionDetection/CollisionTaskRegistry.cs index ee9879789..8f513c464 100644 --- a/BepuPhysics/CollisionDetection/CollisionTaskRegistry.cs +++ b/BepuPhysics/CollisionDetection/CollisionTaskRegistry.cs @@ -276,7 +276,7 @@ public ref CollisionTaskReference GetTaskReference() where TShapeA : unmanaged, IShape where TShapeB : unmanaged, IShape { - return ref GetTaskReference(default(TShapeA).TypeId, default(TShapeB).TypeId); + return ref GetTaskReference(TShapeA.TypeId, TShapeB.TypeId); } } } diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/BoxConvexHullTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/BoxConvexHullTester.cs index 3dde6717d..6a6e6f995 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/BoxConvexHullTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/BoxConvexHullTester.cs @@ -10,9 +10,9 @@ namespace BepuPhysics.CollisionDetection.CollisionTasks { public struct BoxConvexHullTester : IPairTester { - public int BatchSize => 16; + public static int BatchSize => 16; - public unsafe void Test(ref BoxWide a, ref ConvexHullWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold) + public static unsafe void Test(ref BoxWide a, ref ConvexHullWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold) { Unsafe.SkipInit(out manifold); Matrix3x3Wide.CreateFromQuaternion(orientationA, out var boxOrientation); @@ -360,12 +360,12 @@ public unsafe void Test(ref BoxWide a, ref ConvexHullWide b, ref Vector s Matrix3x3Wide.TransformWithoutOverlap(localNormal, hullOrientation, out manifold.Normal); } - public void Test(ref BoxWide a, ref ConvexHullWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold) + public static void Test(ref BoxWide a, ref ConvexHullWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold) { throw new NotImplementedException(); } - public void Test(ref BoxWide a, ref ConvexHullWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex4ContactManifoldWide manifold) + public static void Test(ref BoxWide a, ref ConvexHullWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex4ContactManifoldWide manifold) { throw new NotImplementedException(); } diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/BoxCylinderTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/BoxCylinderTester.cs index 66903c6d5..86fcc0016 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/BoxCylinderTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/BoxCylinderTester.cs @@ -11,7 +11,7 @@ namespace BepuPhysics.CollisionDetection.CollisionTasks using DepthRefiner = DepthRefiner; public struct BoxCylinderTester : IPairTester { - public int BatchSize => 16; + public static int BatchSize => 16; [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static void IntersectLineCircle(in Vector2Wide linePosition, in Vector2Wide lineDirection, in Vector radius, out Vector tMin, out Vector tMax, out Vector intersected) @@ -100,7 +100,7 @@ static void TryAddInteriorPoint(in Vector2Wide point, in Vector featureId, } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void Test( + public static unsafe void Test( ref BoxWide a, ref CylinderWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold) @@ -386,12 +386,12 @@ public unsafe void Test( } - public void Test(ref BoxWide a, ref CylinderWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold) + public static void Test(ref BoxWide a, ref CylinderWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold) { throw new NotImplementedException(); } - public void Test(ref BoxWide a, ref CylinderWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex4ContactManifoldWide manifold) + public static void Test(ref BoxWide a, ref CylinderWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex4ContactManifoldWide manifold) { throw new NotImplementedException(); } diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/BoxPairTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/BoxPairTester.cs index 38f1ca3a1..f2e62348e 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/BoxPairTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/BoxPairTester.cs @@ -10,7 +10,7 @@ namespace BepuPhysics.CollisionDetection.CollisionTasks { public struct BoxPairTester : IPairTester { - public int BatchSize => 32; + public static int BatchSize => 32; [MethodImpl(MethodImplOptions.AggressiveInlining)] static void TestEdgeEdge( @@ -322,7 +322,7 @@ private static void CreateEdgeContacts( [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void Test( + public static unsafe void Test( ref BoxWide a, ref BoxWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold) @@ -539,12 +539,12 @@ private static void TransformContactToManifold( manifoldFeatureId = rawContact.FeatureId; } - public void Test(ref BoxWide a, ref BoxWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold) + public static void Test(ref BoxWide a, ref BoxWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold) { throw new NotImplementedException(); } - public void Test(ref BoxWide a, ref BoxWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex4ContactManifoldWide manifold) + public static void Test(ref BoxWide a, ref BoxWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex4ContactManifoldWide manifold) { throw new NotImplementedException(); } diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/BoxTriangleTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/BoxTriangleTester.cs index 98a68a021..3738a3a91 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/BoxTriangleTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/BoxTriangleTester.cs @@ -9,7 +9,7 @@ namespace BepuPhysics.CollisionDetection.CollisionTasks { public struct BoxTriangleTester : IPairTester { - public int BatchSize => 32; + public static int BatchSize => 32; [MethodImpl(MethodImplOptions.AggressiveInlining)] static void GetDepthForInterval(in Vector boxExtreme, in Vector a, in Vector b, in Vector c, out Vector depth) @@ -278,7 +278,7 @@ private static void AddBoxVertices(in Vector3Wide a, in Vector3Wide b, in Vector } //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void Test( + public static unsafe void Test( ref BoxWide a, ref TriangleWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold) @@ -701,12 +701,12 @@ private static void TransformContactToManifold( manifoldFeatureId = rawContact.FeatureId; } - public void Test(ref BoxWide a, ref TriangleWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold) + public static void Test(ref BoxWide a, ref TriangleWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold) { throw new NotImplementedException(); } - public void Test(ref BoxWide a, ref TriangleWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex4ContactManifoldWide manifold) + public static void Test(ref BoxWide a, ref TriangleWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex4ContactManifoldWide manifold) { throw new NotImplementedException(); } diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleBoxTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleBoxTester.cs index eff127193..c43b4b23e 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleBoxTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleBoxTester.cs @@ -8,7 +8,7 @@ namespace BepuPhysics.CollisionDetection.CollisionTasks { public struct CapsuleBoxTester : IPairTester { - public int BatchSize => 32; + public static int BatchSize => 32; [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static void Prepare( @@ -174,7 +174,7 @@ static void Select( } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Test( + public static void Test( ref CapsuleWide a, ref BoxWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex2ContactManifoldWide manifold) @@ -344,12 +344,12 @@ public void Test( Vector.GreaterThan(tMax - tMin, new Vector(1e-7f) * a.HalfLength)); } - public void Test(ref CapsuleWide a, ref BoxWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex2ContactManifoldWide manifold) + public static void Test(ref CapsuleWide a, ref BoxWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex2ContactManifoldWide manifold) { throw new NotImplementedException(); } - public void Test(ref CapsuleWide a, ref BoxWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex2ContactManifoldWide manifold) + public static void Test(ref CapsuleWide a, ref BoxWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex2ContactManifoldWide manifold) { throw new NotImplementedException(); } diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleConvexHullTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleConvexHullTester.cs index 51149b3a2..259cfa5c9 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleConvexHullTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleConvexHullTester.cs @@ -9,9 +9,9 @@ namespace BepuPhysics.CollisionDetection.CollisionTasks { public struct CapsuleConvexHullTester : IPairTester { - public int BatchSize => 16; + public static int BatchSize => 16; - public void Test(ref CapsuleWide a, ref ConvexHullWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex2ContactManifoldWide manifold) + public static void Test(ref CapsuleWide a, ref ConvexHullWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex2ContactManifoldWide manifold) { Matrix3x3Wide.CreateFromQuaternion(orientationA, out var capsuleOrientation); Matrix3x3Wide.CreateFromQuaternion(orientationB, out var hullOrientation); @@ -179,12 +179,12 @@ public void Test(ref CapsuleWide a, ref ConvexHullWide b, ref Vector spec } - public void Test(ref CapsuleWide a, ref ConvexHullWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex2ContactManifoldWide manifold) + public static void Test(ref CapsuleWide a, ref ConvexHullWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex2ContactManifoldWide manifold) { throw new NotImplementedException(); } - public void Test(ref CapsuleWide a, ref ConvexHullWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex2ContactManifoldWide manifold) + public static void Test(ref CapsuleWide a, ref ConvexHullWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex2ContactManifoldWide manifold) { throw new NotImplementedException(); } diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleCylinderTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleCylinderTester.cs index dfc2061dd..97c57f88d 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleCylinderTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleCylinderTester.cs @@ -9,7 +9,7 @@ namespace BepuPhysics.CollisionDetection.CollisionTasks { public struct CapsuleCylinderTester : IPairTester { - public int BatchSize => 32; + public static int BatchSize => 32; [MethodImpl(MethodImplOptions.AggressiveInlining)] private static void Bounce(in Vector3Wide lineOrigin, in Vector3Wide lineDirection, in Vector t, in CylinderWide b, in Vector radiusSquared, out Vector3Wide p, out Vector3Wide clamped) @@ -132,7 +132,7 @@ public static void GetClosestPointsBetweenSegments(in Vector3Wide da, in Vector3 } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Test(ref CapsuleWide a, ref CylinderWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex2ContactManifoldWide manifold) + public static void Test(ref CapsuleWide a, ref CylinderWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex2ContactManifoldWide manifold) { //Potential normal generators: //Capsule endpoint vs cylinder cap plane : @@ -345,12 +345,12 @@ public void Test(ref CapsuleWide a, ref CylinderWide b, ref Vector specul } - public void Test(ref CapsuleWide a, ref CylinderWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex2ContactManifoldWide manifold) + public static void Test(ref CapsuleWide a, ref CylinderWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex2ContactManifoldWide manifold) { throw new NotImplementedException(); } - public void Test(ref CapsuleWide a, ref CylinderWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex2ContactManifoldWide manifold) + public static void Test(ref CapsuleWide a, ref CylinderWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex2ContactManifoldWide manifold) { throw new NotImplementedException(); } diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CapsulePairTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CapsulePairTester.cs index f35f15264..4717ce03a 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CapsulePairTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CapsulePairTester.cs @@ -8,10 +8,10 @@ namespace BepuPhysics.CollisionDetection.CollisionTasks { public struct CapsulePairTester : IPairTester { - public int BatchSize => 32; + public static int BatchSize => 32; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Test( + public static void Test( ref CapsuleWide a, ref CapsuleWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex2ContactManifoldWide manifold) @@ -128,12 +128,12 @@ public void Test( //Worth looking into later. } - public void Test(ref CapsuleWide a, ref CapsuleWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex2ContactManifoldWide manifold) + public static void Test(ref CapsuleWide a, ref CapsuleWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex2ContactManifoldWide manifold) { throw new NotImplementedException(); } - public void Test(ref CapsuleWide a, ref CapsuleWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex2ContactManifoldWide manifold) + public static void Test(ref CapsuleWide a, ref CapsuleWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex2ContactManifoldWide manifold) { throw new NotImplementedException(); } diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleTriangleTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleTriangleTester.cs index e12e5f337..1cb8a3cbe 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleTriangleTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleTriangleTester.cs @@ -9,7 +9,7 @@ namespace BepuPhysics.CollisionDetection.CollisionTasks { public struct CapsuleTriangleTester : IPairTester { - public int BatchSize => 32; + public static int BatchSize => 32; public static void TestEdge(in TriangleWide triangle, in Vector3Wide triangleNormal, in Vector3Wide edgeStart, in Vector3Wide edgeOffset, @@ -110,7 +110,7 @@ public static void ClipAgainstEdgePlane(in Vector3Wide edgeStart, in Vector3Wide } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Test( + public static void Test( ref CapsuleWide a, ref TriangleWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex2ContactManifoldWide manifold) @@ -381,12 +381,12 @@ public void Test( } - public void Test(ref CapsuleWide a, ref TriangleWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex2ContactManifoldWide manifold) + public static void Test(ref CapsuleWide a, ref TriangleWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex2ContactManifoldWide manifold) { throw new NotImplementedException(); } - public void Test(ref CapsuleWide a, ref TriangleWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex2ContactManifoldWide manifold) + public static void Test(ref CapsuleWide a, ref TriangleWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex2ContactManifoldWide manifold) { throw new NotImplementedException(); } diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundMeshContinuations.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundMeshContinuations.cs index f1f92989f..e23866182 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundMeshContinuations.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundMeshContinuations.cs @@ -67,7 +67,7 @@ public unsafe void ConfigureContinuationChild( //In other words, we can pass a pointer to it to avoid the need for additional batcher shape copying. ref var triangle = ref continuation.Triangles[continuationChildIndex]; childShapeDataB = Unsafe.AsPointer(ref triangle); - childTypeB = triangle.TypeId; + childTypeB = Triangle.TypeId; Unsafe.AsRef(pair.B).GetLocalChild(childIndexB, out continuation.Triangles[continuationChildIndex]); ref var continuationChild = ref continuation.Inner.Children[continuationChildIndex]; //In meshes, the triangle's vertices already contain the offset, so there is no additional offset. diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairCollisionTask.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairCollisionTask.cs index f0052f682..f799e1d80 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairCollisionTask.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairCollisionTask.cs @@ -11,7 +11,7 @@ namespace BepuPhysics.CollisionDetection.CollisionTasks { public interface ICompoundPairOverlapFinder { - void FindLocalOverlaps(ref Buffer pairs, int pairCount, BufferPool pool, Shapes shapes, float dt, out CompoundPairOverlaps overlaps); + static abstract void FindLocalOverlaps(ref Buffer pairs, int pairCount, BufferPool pool, Shapes shapes, float dt, out CompoundPairOverlaps overlaps); } public unsafe interface ICompoundPairContinuationHandler where TContinuation : struct, ICollisionTestContinuation @@ -40,8 +40,8 @@ public class CompoundPairCollisionTask(ref UntypedList batch, ref CollisionBatcher batcher) { var pairs = batch.Buffer.As(); - Unsafe.SkipInit(out TOverlapFinder overlapFinder); Unsafe.SkipInit(out TContinuationHandler continuationHandler); //We perform all necessary bounding box computations and lookups up front. This helps avoid some instruction pipeline pressure at the cost of some extra data cache requirements. //Because of this, you need to be careful with the batch size on this collision task. - overlapFinder.FindLocalOverlaps(ref pairs, batch.Count, batcher.Pool, batcher.Shapes, batcher.Dt, out var overlaps); + TOverlapFinder.FindLocalOverlaps(ref pairs, batch.Count, batcher.Pool, batcher.Shapes, batcher.Dt, out var overlaps); for (int pairIndex = 0; pairIndex < batch.Count; ++pairIndex) { diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlapFinder.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlapFinder.cs index f48c26f67..f896eaca8 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlapFinder.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlapFinder.cs @@ -20,7 +20,7 @@ public struct CompoundPairOverlapFinder : ICompoundPairO where TCompoundB : struct, IBoundsQueryableCompound { - public unsafe void FindLocalOverlaps(ref Buffer pairs, int pairCount, BufferPool pool, Shapes shapes, float dt, out CompoundPairOverlaps overlaps) + public static unsafe void FindLocalOverlaps(ref Buffer pairs, int pairCount, BufferPool pool, Shapes shapes, float dt, out CompoundPairOverlaps overlaps) { var totalCompoundChildCount = 0; for (int i = 0; i < pairCount; ++i) diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCollisionTask.cs b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCollisionTask.cs index 789437bee..480e6d83f 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCollisionTask.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCollisionTask.cs @@ -15,13 +15,13 @@ public interface IPairTester /// /// Gets the nubmer of pairs which would ideally be gathered together before executing a wide test. /// - int BatchSize { get; } + static abstract int BatchSize { get; } //Note that, while the interface requires all three of these implementations, concrete implementers will only ever have one defined or called. //Including the other unused functions is just here to simplify its use in the batch execution loop. - void Test(ref TShapeWideA a, ref TShapeWideB b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out TManifoldWideType manifold); - void Test(ref TShapeWideA a, ref TShapeWideB b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out TManifoldWideType manifold); - void Test(ref TShapeWideA a, ref TShapeWideB b, ref Vector speculativeMargin, ref Vector3Wide offsetB, int pairCount, out TManifoldWideType manifold); + static abstract void Test(ref TShapeWideA a, ref TShapeWideB b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out TManifoldWideType manifold); + static abstract void Test(ref TShapeWideA a, ref TShapeWideB b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out TManifoldWideType manifold); + static abstract void Test(ref TShapeWideA a, ref TShapeWideB b, ref Vector speculativeMargin, ref Vector3Wide offsetB, int pairCount, out TManifoldWideType manifold); } public interface IContactManifoldWide @@ -40,10 +40,10 @@ public class ConvexCollisionTask(ref UntypedList batch, ref //With any luck, the compiler will eventually get rid of these unnecessary zero inits. //Might be able to get rid of manifoldWide and defaultPairTester with some megahacks, but it comes with significant forward danger and questionable benefit. var pairWide = default(TPairWide); - ref var aWide = ref pairWide.GetShapeA(ref pairWide); - ref var bWide = ref pairWide.GetShapeB(ref pairWide); + ref var aWide = ref TPairWide.GetShapeA(ref pairWide); + ref var bWide = ref TPairWide.GetShapeB(ref pairWide); if (aWide.InternalAllocationSize > 0) { var memory = stackalloc byte[aWide.InternalAllocationSize]; @@ -66,7 +66,6 @@ public override unsafe void ExecuteBatch(ref UntypedList batch, ref bWide.Initialize(new Buffer(memory, bWide.InternalAllocationSize)); } TManifoldWide manifoldWide; - var defaultPairTester = default(TPairTester); var manifold = default(ConvexContactManifold); for (int i = 0; i < batch.Count; i += Vector.Count) @@ -82,59 +81,59 @@ public override unsafe void ExecuteBatch(ref UntypedList batch, ref pairWide.WriteSlot(j, Unsafe.Add(ref bundleStart, j)); } - if (pairWide.OrientationCount == 2) + if (TPairWide.OrientationCount == 2) { - defaultPairTester.Test( + TPairTester.Test( ref aWide, ref bWide, - ref pairWide.GetSpeculativeMargin(ref pairWide), - ref pairWide.GetOffsetB(ref pairWide), - ref pairWide.GetOrientationA(ref pairWide), - ref pairWide.GetOrientationB(ref pairWide), + ref TPairWide.GetSpeculativeMargin(ref pairWide), + ref TPairWide.GetOffsetB(ref pairWide), + ref TPairWide.GetOrientationA(ref pairWide), + ref TPairWide.GetOrientationB(ref pairWide), countInBundle, out manifoldWide); } - else if (pairWide.OrientationCount == 1) + else if (TPairWide.OrientationCount == 1) { //Note that, in the event that there is only one orientation, it belongs to the second shape. //The only shape that doesn't need orientation is a sphere, and it will be in slot A by convention. Debug.Assert(typeof(TShapeWideA) == typeof(SphereWide)); - defaultPairTester.Test( + TPairTester.Test( ref aWide, ref bWide, - ref pairWide.GetSpeculativeMargin(ref pairWide), - ref pairWide.GetOffsetB(ref pairWide), - ref pairWide.GetOrientationB(ref pairWide), + ref TPairWide.GetSpeculativeMargin(ref pairWide), + ref TPairWide.GetOffsetB(ref pairWide), + ref TPairWide.GetOrientationB(ref pairWide), countInBundle, out manifoldWide); } else { - Debug.Assert(pairWide.OrientationCount == 0); + Debug.Assert(TPairWide.OrientationCount == 0); Debug.Assert(typeof(TShapeWideA) == typeof(SphereWide) && typeof(TShapeWideB) == typeof(SphereWide), "No orientation implies a special case involving two spheres."); //Really, this could be made into a direct special case, but eh. - defaultPairTester.Test( + TPairTester.Test( ref aWide, ref bWide, - ref pairWide.GetSpeculativeMargin(ref pairWide), - ref pairWide.GetOffsetB(ref pairWide), + ref TPairWide.GetSpeculativeMargin(ref pairWide), + ref TPairWide.GetOffsetB(ref pairWide), countInBundle, out manifoldWide); } //Flip back any contacts associated with pairs which had to be flipped for shape order. - if (pairWide.HasFlipMask) + if (TPairWide.HasFlipMask) { - manifoldWide.ApplyFlipMask(ref pairWide.GetOffsetB(ref pairWide), pairWide.GetFlipMask(ref pairWide)); + manifoldWide.ApplyFlipMask(ref TPairWide.GetOffsetB(ref pairWide), TPairWide.GetFlipMask(ref pairWide)); } for (int j = 0; j < countInBundle; ++j) { ref var manifoldSource = ref GetOffsetInstance(ref manifoldWide, j); - ref var offsetSource = ref GetOffsetInstance(ref pairWide.GetOffsetB(ref pairWide), j); + ref var offsetSource = ref GetOffsetInstance(ref TPairWide.GetOffsetB(ref pairWide), j); manifoldSource.ReadFirst(offsetSource, ref manifold); ref var pair = ref Unsafe.Add(ref bundleStart, j); - batcher.ProcessConvexResult(ref manifold, ref pair.GetContinuation(ref pair)); + batcher.ProcessConvexResult(ref manifold, ref TPair.GetContinuation(ref pair)); } } diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundCollisionTask.cs b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundCollisionTask.cs index 6b768df17..a29e79523 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundCollisionTask.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundCollisionTask.cs @@ -32,8 +32,8 @@ public class ConvexCompoundCollisionTask 16; + public static int BatchSize => 16; - public unsafe void Test(ref ConvexHullWide a, ref ConvexHullWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold) + public static unsafe void Test(ref ConvexHullWide a, ref ConvexHullWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold) { Unsafe.SkipInit(out manifold); Matrix3x3Wide.CreateFromQuaternion(orientationA, out var rA); @@ -225,12 +225,12 @@ public unsafe void Test(ref ConvexHullWide a, ref ConvexHullWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold) + public static void Test(ref ConvexHullWide a, ref ConvexHullWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold) { throw new NotImplementedException(); } - public void Test(ref ConvexHullWide a, ref ConvexHullWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex4ContactManifoldWide manifold) + public static void Test(ref ConvexHullWide a, ref ConvexHullWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex4ContactManifoldWide manifold) { throw new NotImplementedException(); } diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexMeshContinuations.cs b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexMeshContinuations.cs index 3d968c8b4..a4aeffeb6 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexMeshContinuations.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexMeshContinuations.cs @@ -35,7 +35,7 @@ public unsafe void ConfigureContinuationChild( //In other words, we can pass a pointer to it to avoid the need for additional batcher shape copying. ref var triangle = ref continuation.Triangles[continuationChildIndex]; childShapeDataB = Unsafe.AsPointer(ref triangle); - childTypeB = triangle.TypeId; + childTypeB = Triangle.TypeId; Unsafe.AsRef(pair.B).GetLocalChild(childIndex, out continuation.Triangles[continuationChildIndex]); ref var continuationChild = ref continuation.Inner.Children[continuationChildIndex]; //Triangles already have their local pose baked into their vertices, so we just need the orientation. diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CylinderConvexHullTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CylinderConvexHullTester.cs index 06465570e..f17499157 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CylinderConvexHullTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CylinderConvexHullTester.cs @@ -9,7 +9,7 @@ namespace BepuPhysics.CollisionDetection.CollisionTasks { public struct CylinderConvexHullTester : IPairTester { - public int BatchSize => 16; + public static int BatchSize => 16; [MethodImpl(MethodImplOptions.AggressiveInlining)] static void ProjectOntoCap(Vector3 capCenter, in Matrix3x3 cylinderOrientation, float inverseLocalNormalDotAY, Vector3 localNormal, Vector3 point, out Vector2 projected) @@ -81,7 +81,7 @@ static void InsertContact(Vector3 slotSideEdgeCenter, Vector3 slotCylinderEdgeAx GatherScatter.GetFirst(ref contactExistsWide) = -1; } - public unsafe void Test(ref CylinderWide a, ref ConvexHullWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold) + public static unsafe void Test(ref CylinderWide a, ref ConvexHullWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold) { Unsafe.SkipInit(out manifold); Matrix3x3Wide.CreateFromQuaternion(orientationA, out var cylinderOrientation); @@ -407,12 +407,12 @@ public unsafe void Test(ref CylinderWide a, ref ConvexHullWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold) + public static void Test(ref CylinderWide a, ref ConvexHullWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold) { throw new NotImplementedException(); } - public void Test(ref CylinderWide a, ref ConvexHullWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex4ContactManifoldWide manifold) + public static void Test(ref CylinderWide a, ref ConvexHullWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex4ContactManifoldWide manifold) { throw new NotImplementedException(); } diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CylinderPairTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CylinderPairTester.cs index 4f0ea3463..f51ce271e 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CylinderPairTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CylinderPairTester.cs @@ -12,7 +12,7 @@ namespace BepuPhysics.CollisionDetection.CollisionTasks public struct CylinderPairTester : IPairTester { - public int BatchSize => 16; + public static int BatchSize => 16; [MethodImpl(MethodImplOptions.AggressiveInlining)] static void ProjectOntoCapA(in Vector capCenterBY, in Vector3Wide capCenterA, in Matrix3x3Wide rA, in Vector inverseNDotAY, in Vector3Wide localNormal, in Vector2Wide point, out Vector2Wide projected) @@ -95,7 +95,7 @@ internal static void TransformContact( } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Test( + public static void Test( ref CylinderWide a, ref CylinderWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold) @@ -447,12 +447,12 @@ public void Test( manifold.FeatureId3 = new Vector(3); } - public void Test(ref CylinderWide a, ref CylinderWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold) + public static void Test(ref CylinderWide a, ref CylinderWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold) { throw new NotImplementedException(); } - public void Test(ref CylinderWide a, ref CylinderWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex4ContactManifoldWide manifold) + public static void Test(ref CylinderWide a, ref CylinderWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex4ContactManifoldWide manifold) { throw new NotImplementedException(); } diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/MeshPairContinuations.cs b/BepuPhysics/CollisionDetection/CollisionTasks/MeshPairContinuations.cs index f5f1bead0..9971ace54 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/MeshPairContinuations.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/MeshPairContinuations.cs @@ -56,7 +56,7 @@ public unsafe void GetChildAData(ref CollisionBatcher co { ref var triangle = ref continuation.Triangles[triangleAStartIndex++]; childShapeDataA = Unsafe.AsPointer(ref triangle); - childTypeA = triangle.TypeId; + childTypeA = Triangle.TypeId; Unsafe.AsRef(pair.A).GetLocalChild(childIndexA, out triangle); childPoseA = new RigidPose(default, pair.OrientationA); } @@ -71,7 +71,7 @@ public unsafe void ConfigureContinuationChild( //In other words, we can pass a pointer to it to avoid the need for additional batcher shape copying. ref var triangle = ref continuation.Triangles[continuationChildIndex]; childShapeDataB = Unsafe.AsPointer(ref triangle); - childTypeB = triangle.TypeId; + childTypeB = Triangle.TypeId; Unsafe.AsRef(pair.B).GetLocalChild(childIndexB, out continuation.Triangles[continuationChildIndex]); ref var continuationChild = ref continuation.Inner.Children[continuationChildIndex]; //In meshes, the triangle's vertices already contain the offset, so there is no additional offset. diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/MeshPairOverlapFinder.cs b/BepuPhysics/CollisionDetection/CollisionTasks/MeshPairOverlapFinder.cs index 73f356c1a..f531da402 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/MeshPairOverlapFinder.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/MeshPairOverlapFinder.cs @@ -15,7 +15,7 @@ public struct MeshPairOverlapFinder : ICompoundPairOverlapFinder where TMeshB : struct, IHomogeneousCompoundShape { - public unsafe void FindLocalOverlaps(ref Buffer pairs, int pairCount, BufferPool pool, Shapes shapes, float dt, out CompoundPairOverlaps overlaps) + public static unsafe void FindLocalOverlaps(ref Buffer pairs, int pairCount, BufferPool pool, Shapes shapes, float dt, out CompoundPairOverlaps overlaps) { var totalCompoundChildCount = 0; for (int i = 0; i < pairCount; ++i) diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/PairTypes.cs b/BepuPhysics/CollisionDetection/CollisionTasks/PairTypes.cs index 01d82b4fd..71db877a4 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/PairTypes.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/PairTypes.cs @@ -21,8 +21,8 @@ public interface ICollisionPair where TPair : ICollisionPair /// /// Gets the enumeration type associated with this pair type. /// - CollisionTaskPairType PairType { get; } - ref PairContinuation GetContinuation(ref TPair pair); + static abstract CollisionTaskPairType PairType { get; } + static abstract ref PairContinuation GetContinuation(ref TPair pair); } public unsafe struct CollisionPair : ICollisionPair @@ -39,10 +39,10 @@ public unsafe struct CollisionPair : ICollisionPair public float SpeculativeMargin; public PairContinuation Continuation; - public readonly CollisionTaskPairType PairType => CollisionTaskPairType.StandardPair; + public static CollisionTaskPairType PairType => CollisionTaskPairType.StandardPair; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref PairContinuation GetContinuation(ref CollisionPair pair) + public static ref PairContinuation GetContinuation(ref CollisionPair pair) { return ref pair.Continuation; } @@ -58,10 +58,10 @@ public unsafe struct FliplessPair : ICollisionPair public float SpeculativeMargin; public PairContinuation Continuation; - public readonly CollisionTaskPairType PairType => CollisionTaskPairType.FliplessPair; + public static CollisionTaskPairType PairType => CollisionTaskPairType.FliplessPair; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref PairContinuation GetContinuation(ref FliplessPair pair) + public static ref PairContinuation GetContinuation(ref FliplessPair pair) { return ref pair.Continuation; } @@ -75,10 +75,10 @@ public struct SpherePair : ICollisionPair public float SpeculativeMargin; public PairContinuation Continuation; - public readonly CollisionTaskPairType PairType => CollisionTaskPairType.SpherePair; + public static CollisionTaskPairType PairType => CollisionTaskPairType.SpherePair; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref PairContinuation GetContinuation(ref SpherePair pair) + public static ref PairContinuation GetContinuation(ref SpherePair pair) { return ref pair.Continuation; } @@ -97,10 +97,10 @@ public unsafe struct SphereIncludingPair : ICollisionPair public float SpeculativeMargin; public PairContinuation Continuation; - public readonly CollisionTaskPairType PairType => CollisionTaskPairType.SphereIncludingPair; + public static CollisionTaskPairType PairType => CollisionTaskPairType.SphereIncludingPair; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref PairContinuation GetContinuation(ref SphereIncludingPair pair) + public static ref PairContinuation GetContinuation(ref SphereIncludingPair pair) { return ref pair.Continuation; } @@ -125,10 +125,10 @@ public unsafe struct BoundsTestedPair : ICollisionPair public float SpeculativeMargin; public PairContinuation Continuation; - public readonly CollisionTaskPairType PairType => CollisionTaskPairType.BoundsTestedPair; + public static CollisionTaskPairType PairType => CollisionTaskPairType.BoundsTestedPair; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref PairContinuation GetContinuation(ref BoundsTestedPair pair) + public static ref PairContinuation GetContinuation(ref BoundsTestedPair pair) { return ref pair.Continuation; } @@ -138,16 +138,16 @@ public interface ICollisionPairWide where TShapeWideB : struct, IShapeWide { - bool HasFlipMask { get; } - int OrientationCount { get; } + static abstract bool HasFlipMask { get; } + static abstract int OrientationCount { get; } //Note the pair parameter. This is just to get around the fact that you cannot ref return struct fields like you can with classes, at least right now - ref Vector GetFlipMask(ref TPairWide pair); - ref Vector GetSpeculativeMargin(ref TPairWide pair); - ref TShapeWideA GetShapeA(ref TPairWide pair); - ref TShapeWideB GetShapeB(ref TPairWide pair); - ref QuaternionWide GetOrientationA(ref TPairWide pair); - ref QuaternionWide GetOrientationB(ref TPairWide pair); - ref Vector3Wide GetOffsetB(ref TPairWide pair); + static abstract ref Vector GetFlipMask(ref TPairWide pair); + static abstract ref Vector GetSpeculativeMargin(ref TPairWide pair); + static abstract ref TShapeWideA GetShapeA(ref TPairWide pair); + static abstract ref TShapeWideB GetShapeB(ref TPairWide pair); + static abstract ref QuaternionWide GetOrientationA(ref TPairWide pair); + static abstract ref QuaternionWide GetOrientationB(ref TPairWide pair); + static abstract ref Vector3Wide GetOffsetB(ref TPairWide pair); void WriteSlot(int index, in TPair source); } @@ -166,51 +166,51 @@ public struct ConvexPairWide : public QuaternionWide OrientationB; public Vector SpeculativeMargin; - public bool HasFlipMask + public static bool HasFlipMask { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { return true; } } - public int OrientationCount + public static int OrientationCount { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { return 2; } } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector GetFlipMask(ref ConvexPairWide pair) + public static ref Vector GetFlipMask(ref ConvexPairWide pair) { return ref pair.FlipMask; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector GetSpeculativeMargin(ref ConvexPairWide pair) + public static ref Vector GetSpeculativeMargin(ref ConvexPairWide pair) { return ref pair.SpeculativeMargin; } //Little unfortunate that we can't return ref of struct instances. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref TShapeWideA GetShapeA(ref ConvexPairWide pair) + public static ref TShapeWideA GetShapeA(ref ConvexPairWide pair) { return ref pair.A; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref TShapeWideB GetShapeB(ref ConvexPairWide pair) + public static ref TShapeWideB GetShapeB(ref ConvexPairWide pair) { return ref pair.B; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3Wide GetOffsetB(ref ConvexPairWide pair) + public static ref Vector3Wide GetOffsetB(ref ConvexPairWide pair) { return ref pair.OffsetB; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref QuaternionWide GetOrientationA(ref ConvexPairWide pair) + public static ref QuaternionWide GetOrientationA(ref ConvexPairWide pair) { return ref pair.OrientationA; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref QuaternionWide GetOrientationB(ref ConvexPairWide pair) + public static ref QuaternionWide GetOrientationB(ref ConvexPairWide pair) { return ref pair.OrientationB; } @@ -243,50 +243,50 @@ public struct FliplessPairWide : ICollisionPairWide SpeculativeMargin; - public bool HasFlipMask + public static bool HasFlipMask { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { return false; } } - public int OrientationCount + public static int OrientationCount { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { return 2; } } - public ref Vector GetFlipMask(ref FliplessPairWide pair) + public static ref Vector GetFlipMask(ref FliplessPairWide pair) { throw new NotImplementedException(); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector GetSpeculativeMargin(ref FliplessPairWide pair) + public static ref Vector GetSpeculativeMargin(ref FliplessPairWide pair) { return ref pair.SpeculativeMargin; } //Little unfortunate that we can't return ref of struct instances. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref TShapeWide GetShapeA(ref FliplessPairWide pair) + public static ref TShapeWide GetShapeA(ref FliplessPairWide pair) { return ref pair.A; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref TShapeWide GetShapeB(ref FliplessPairWide pair) + public static ref TShapeWide GetShapeB(ref FliplessPairWide pair) { return ref pair.B; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3Wide GetOffsetB(ref FliplessPairWide pair) + public static ref Vector3Wide GetOffsetB(ref FliplessPairWide pair) { return ref pair.OffsetB; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref QuaternionWide GetOrientationA(ref FliplessPairWide pair) + public static ref QuaternionWide GetOrientationA(ref FliplessPairWide pair) { return ref pair.OrientationA; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref QuaternionWide GetOrientationB(ref FliplessPairWide pair) + public static ref QuaternionWide GetOrientationB(ref FliplessPairWide pair) { return ref pair.OrientationB; } @@ -320,51 +320,51 @@ public struct SphereIncludingPairWide : ICollisionPairWide SpeculativeMargin; - public bool HasFlipMask + public static bool HasFlipMask { //Because the shapes are guaranteed to be distinct (one is apparently a sphere and the other isn't), there will always be a flip mask. [MethodImpl(MethodImplOptions.AggressiveInlining)] get { return true; } } - public int OrientationCount + public static int OrientationCount { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { return 1; } } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector GetFlipMask(ref SphereIncludingPairWide pair) + public static ref Vector GetFlipMask(ref SphereIncludingPairWide pair) { return ref pair.FlipMask; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector GetSpeculativeMargin(ref SphereIncludingPairWide pair) + public static ref Vector GetSpeculativeMargin(ref SphereIncludingPairWide pair) { return ref pair.SpeculativeMargin; } //Little unfortunate that we can't return ref of struct instances. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref SphereWide GetShapeA(ref SphereIncludingPairWide pair) + public static ref SphereWide GetShapeA(ref SphereIncludingPairWide pair) { return ref pair.A; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref TShapeWide GetShapeB(ref SphereIncludingPairWide pair) + public static ref TShapeWide GetShapeB(ref SphereIncludingPairWide pair) { return ref pair.B; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3Wide GetOffsetB(ref SphereIncludingPairWide pair) + public static ref Vector3Wide GetOffsetB(ref SphereIncludingPairWide pair) { return ref pair.OffsetB; } - public ref QuaternionWide GetOrientationA(ref SphereIncludingPairWide pair) + public static ref QuaternionWide GetOrientationA(ref SphereIncludingPairWide pair) { throw new NotImplementedException(); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref QuaternionWide GetOrientationB(ref SphereIncludingPairWide pair) + public static ref QuaternionWide GetOrientationB(ref SphereIncludingPairWide pair) { return ref pair.OrientationB; } @@ -390,47 +390,47 @@ public struct SpherePairWide : ICollisionPairWide SpeculativeMargin; - public bool HasFlipMask + public static bool HasFlipMask { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { return false; } } - public int OrientationCount + public static int OrientationCount { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { return 0; } } - public ref Vector GetFlipMask(ref SpherePairWide pair) + public static ref Vector GetFlipMask(ref SpherePairWide pair) { throw new NotImplementedException(); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector GetSpeculativeMargin(ref SpherePairWide pair) + public static ref Vector GetSpeculativeMargin(ref SpherePairWide pair) { return ref pair.SpeculativeMargin; } //Little unfortunate that we can't return ref of struct instances. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref SphereWide GetShapeA(ref SpherePairWide pair) + public static ref SphereWide GetShapeA(ref SpherePairWide pair) { return ref pair.A; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref SphereWide GetShapeB(ref SpherePairWide pair) + public static ref SphereWide GetShapeB(ref SpherePairWide pair) { return ref pair.B; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3Wide GetOffsetB(ref SpherePairWide pair) + public static ref Vector3Wide GetOffsetB(ref SpherePairWide pair) { return ref pair.OffsetB; } - public ref QuaternionWide GetOrientationA(ref SpherePairWide pair) + public static ref QuaternionWide GetOrientationA(ref SpherePairWide pair) { throw new NotImplementedException(); } - public ref QuaternionWide GetOrientationB(ref SpherePairWide pair) + public static ref QuaternionWide GetOrientationB(ref SpherePairWide pair) { throw new NotImplementedException(); } diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/SphereBoxTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/SphereBoxTester.cs index f9cb4d65c..130fc62b7 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/SphereBoxTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/SphereBoxTester.cs @@ -9,15 +9,15 @@ namespace BepuPhysics.CollisionDetection.CollisionTasks //Individual pair testers are designed to be used outside of the narrow phase. They need to be usable for queries and such, so all necessary data must be gathered externally. public struct SphereBoxTester : IPairTester { - public int BatchSize => 32; + public static int BatchSize => 32; - public void Test(ref SphereWide a, ref BoxWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex1ContactManifoldWide manifold) + public static void Test(ref SphereWide a, ref BoxWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex1ContactManifoldWide manifold) { throw new NotImplementedException(); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Test(ref SphereWide a, ref BoxWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex1ContactManifoldWide manifold) + public static void Test(ref SphereWide a, ref BoxWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex1ContactManifoldWide manifold) { //Clamp the position of the sphere to the box. Matrix3x3Wide.CreateFromQuaternion(orientationB, out var orientationMatrixB); @@ -64,7 +64,7 @@ public void Test(ref SphereWide a, ref BoxWide b, ref Vector speculativeM manifold.ContactExists = Vector.GreaterThan(manifold.Depth, -speculativeMargin); } - public void Test(ref SphereWide a, ref BoxWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex1ContactManifoldWide manifold) + public static void Test(ref SphereWide a, ref BoxWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex1ContactManifoldWide manifold) { throw new NotImplementedException(); } diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/SphereCapsuleTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/SphereCapsuleTester.cs index a3f10868f..886d378dc 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/SphereCapsuleTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/SphereCapsuleTester.cs @@ -9,15 +9,15 @@ namespace BepuPhysics.CollisionDetection.CollisionTasks //Individual pair testers are designed to be used outside of the narrow phase. They need to be usable for queries and such, so all necessary data must be gathered externally. public struct SphereCapsuleTester : IPairTester { - public int BatchSize => 32; + public static int BatchSize => 32; - public void Test(ref SphereWide a, ref CapsuleWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex1ContactManifoldWide manifold) + public static void Test(ref SphereWide a, ref CapsuleWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex1ContactManifoldWide manifold) { throw new NotImplementedException(); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Test(ref SphereWide a, ref CapsuleWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex1ContactManifoldWide manifold) + public static void Test(ref SphereWide a, ref CapsuleWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex1ContactManifoldWide manifold) { //The contact for a sphere-capsule pair is based on the closest point of the sphere center to the capsule internal line segment. QuaternionWide.TransformUnitXY(orientationB, out var x, out var y); @@ -48,7 +48,7 @@ public void Test(ref SphereWide a, ref CapsuleWide b, ref Vector speculat manifold.ContactExists = Vector.GreaterThan(manifold.Depth, -speculativeMargin); } - public void Test(ref SphereWide a, ref CapsuleWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex1ContactManifoldWide manifold) + public static void Test(ref SphereWide a, ref CapsuleWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex1ContactManifoldWide manifold) { throw new NotImplementedException(); } diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/SphereConvexHullTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/SphereConvexHullTester.cs index da9acb59a..aee7d7e12 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/SphereConvexHullTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/SphereConvexHullTester.cs @@ -8,14 +8,14 @@ namespace BepuPhysics.CollisionDetection.CollisionTasks { public struct SphereConvexHullTester : IPairTester { - public int BatchSize => 16; + public static int BatchSize => 16; - public void Test(ref SphereWide a, ref ConvexHullWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex1ContactManifoldWide manifold) + public static void Test(ref SphereWide a, ref ConvexHullWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex1ContactManifoldWide manifold) { throw new NotImplementedException(); } - public void Test(ref SphereWide a, ref ConvexHullWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex1ContactManifoldWide manifold) + public static void Test(ref SphereWide a, ref ConvexHullWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex1ContactManifoldWide manifold) { Matrix3x3Wide.CreateFromQuaternion(orientationB, out var hullOrientation); Matrix3x3Wide.TransformByTransposedWithoutOverlap(offsetB, hullOrientation, out var localOffsetB); @@ -45,7 +45,7 @@ public void Test(ref SphereWide a, ref ConvexHullWide b, ref Vector specu manifold.ContactExists = Vector.GreaterThanOrEqual(manifold.Depth, -speculativeMargin); } - public void Test(ref SphereWide a, ref ConvexHullWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex1ContactManifoldWide manifold) + public static void Test(ref SphereWide a, ref ConvexHullWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex1ContactManifoldWide manifold) { throw new NotImplementedException(); } diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/SphereCylinderTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/SphereCylinderTester.cs index 997b0f384..8b4fba8a0 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/SphereCylinderTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/SphereCylinderTester.cs @@ -8,9 +8,9 @@ namespace BepuPhysics.CollisionDetection.CollisionTasks { public struct SphereCylinderTester : IPairTester { - public int BatchSize => 32; + public static int BatchSize => 32; - public void Test(ref SphereWide a, ref CylinderWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex1ContactManifoldWide manifold) + public static void Test(ref SphereWide a, ref CylinderWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex1ContactManifoldWide manifold) { throw new NotImplementedException(); } @@ -37,7 +37,7 @@ public static void ComputeSphereToClosest(in CylinderWide b, in Vector3Wide offs } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Test(ref SphereWide a, ref CylinderWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex1ContactManifoldWide manifold) + public static void Test(ref SphereWide a, ref CylinderWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex1ContactManifoldWide manifold) { Matrix3x3Wide.CreateFromQuaternion(orientationB, out var orientationMatrixB); ComputeSphereToClosest(b, offsetB, orientationMatrixB, @@ -72,7 +72,7 @@ public void Test(ref SphereWide a, ref CylinderWide b, ref Vector specula manifold.ContactExists = Vector.GreaterThanOrEqual(manifold.Depth, -speculativeMargin); } - public void Test(ref SphereWide a, ref CylinderWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex1ContactManifoldWide manifold) + public static void Test(ref SphereWide a, ref CylinderWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex1ContactManifoldWide manifold) { throw new NotImplementedException(); } diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/SpherePairTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/SpherePairTester.cs index adc272beb..633fce23b 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/SpherePairTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/SpherePairTester.cs @@ -9,20 +9,20 @@ namespace BepuPhysics.CollisionDetection.CollisionTasks //Individual pair testers are designed to be used outside of the narrow phase. They need to be usable for queries and such, so all necessary data must be gathered externally. public struct SpherePairTester : IPairTester { - public int BatchSize => 32; + public static int BatchSize => 32; - public void Test(ref SphereWide a, ref SphereWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex1ContactManifoldWide manifold) + public static void Test(ref SphereWide a, ref SphereWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex1ContactManifoldWide manifold) { throw new NotImplementedException(); } - public void Test(ref SphereWide a, ref SphereWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex1ContactManifoldWide manifold) + public static void Test(ref SphereWide a, ref SphereWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex1ContactManifoldWide manifold) { throw new NotImplementedException(); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Test(ref SphereWide a, ref SphereWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex1ContactManifoldWide manifold) + public static void Test(ref SphereWide a, ref SphereWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex1ContactManifoldWide manifold) { Vector3Wide.Length(offsetB, out var centerDistance); //Note the negative 1. By convention, the normal points from B to A. diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/SphereTriangleTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/SphereTriangleTester.cs index e88df66d9..d5382f9a7 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/SphereTriangleTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/SphereTriangleTester.cs @@ -9,9 +9,9 @@ namespace BepuPhysics.CollisionDetection.CollisionTasks //Individual pair testers are designed to be used outside of the narrow phase. They need to be usable for queries and such, so all necessary data must be gathered externally. public struct SphereTriangleTester : IPairTester { - public int BatchSize => 32; + public static int BatchSize => 32; - public void Test(ref SphereWide a, ref TriangleWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, + public static void Test(ref SphereWide a, ref TriangleWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex1ContactManifoldWide manifold) { throw new NotImplementedException(); @@ -26,7 +26,7 @@ static void Select(ref Vector distanceSquared, ref Vector3Wide localNorma } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Test(ref SphereWide a, ref TriangleWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, + public static void Test(ref SphereWide a, ref TriangleWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex1ContactManifoldWide manifold) { Unsafe.SkipInit(out manifold); @@ -120,7 +120,7 @@ public void Test(ref SphereWide a, ref TriangleWide b, ref Vector specula Vector.GreaterThanOrEqual(manifold.Depth, -speculativeMargin))); } - public void Test(ref SphereWide a, ref TriangleWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex1ContactManifoldWide manifold) + public static void Test(ref SphereWide a, ref TriangleWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex1ContactManifoldWide manifold) { throw new NotImplementedException(); } diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/TriangleConvexHullTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/TriangleConvexHullTester.cs index 05d74c4c5..4dee8044c 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/TriangleConvexHullTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/TriangleConvexHullTester.cs @@ -11,9 +11,9 @@ namespace BepuPhysics.CollisionDetection.CollisionTasks using DepthRefiner = DepthRefiner; public struct TriangleConvexHullTester : IPairTester { - public int BatchSize => 16; + public static int BatchSize => 16; - public unsafe void Test(ref TriangleWide a, ref ConvexHullWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold) + public static unsafe void Test(ref TriangleWide a, ref ConvexHullWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold) { Unsafe.SkipInit(out manifold); Matrix3x3Wide.CreateFromQuaternion(orientationA, out var triangleOrientation); @@ -433,12 +433,12 @@ public unsafe void Test(ref TriangleWide a, ref ConvexHullWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold) + public static void Test(ref TriangleWide a, ref ConvexHullWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold) { throw new NotImplementedException(); } - public void Test(ref TriangleWide a, ref ConvexHullWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex4ContactManifoldWide manifold) + public static void Test(ref TriangleWide a, ref ConvexHullWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex4ContactManifoldWide manifold) { throw new NotImplementedException(); } diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/TriangleCylinderTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/TriangleCylinderTester.cs index 6d2234d7c..cffe7055c 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/TriangleCylinderTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/TriangleCylinderTester.cs @@ -39,7 +39,7 @@ public void GetMargin(in TriangleWide shape, out Vector margin) public struct TriangleCylinderTester : IPairTester { - public int BatchSize => 16; + public static int BatchSize => 16; [MethodImpl(MethodImplOptions.AggressiveInlining)] static void TryAddInteriorPoint(in Vector2Wide point, in Vector featureId, @@ -99,7 +99,7 @@ public static void CreateEffectiveTriangleFaceNormal(in Vector3Wide triangleNorm } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void Test( + public static unsafe void Test( ref TriangleWide a, ref CylinderWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold) @@ -594,12 +594,12 @@ public unsafe void Test( } - public void Test(ref TriangleWide a, ref CylinderWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold) + public static void Test(ref TriangleWide a, ref CylinderWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold) { throw new NotImplementedException(); } - public void Test(ref TriangleWide a, ref CylinderWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex4ContactManifoldWide manifold) + public static void Test(ref TriangleWide a, ref CylinderWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex4ContactManifoldWide manifold) { throw new NotImplementedException(); } diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs index a6b13349a..476ab18dc 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/TrianglePairTester.cs @@ -8,7 +8,7 @@ namespace BepuPhysics.CollisionDetection.CollisionTasks { public struct TrianglePairTester : IPairTester { - public int BatchSize => 32; + public static int BatchSize => 32; [MethodImpl(MethodImplOptions.AggressiveInlining)] static void GetIntervalForNormal(in Vector3Wide a, in Vector3Wide b, in Vector3Wide c, in Vector3Wide normal, out Vector min, out Vector max) @@ -258,7 +258,7 @@ private static void ClipBEdgeAgainstABounds( //maybe we should have.. MORE param //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void Test( + public static unsafe void Test( ref TriangleWide a, ref TriangleWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold) @@ -523,12 +523,12 @@ private static void TransformContactToManifold( manifoldFeatureId = rawContact.FeatureId; } - public void Test(ref TriangleWide a, ref TriangleWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold) + public static void Test(ref TriangleWide a, ref TriangleWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold) { throw new NotImplementedException(); } - public void Test(ref TriangleWide a, ref TriangleWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex4ContactManifoldWide manifold) + public static void Test(ref TriangleWide a, ref TriangleWide b, ref Vector speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex4ContactManifoldWide manifold) { throw new NotImplementedException(); } diff --git a/BepuPhysics/CollisionDetection/ContactConstraintAccessor.cs b/BepuPhysics/CollisionDetection/ContactConstraintAccessor.cs index 1cc820fee..4fbf4de46 100644 --- a/BepuPhysics/CollisionDetection/ContactConstraintAccessor.cs +++ b/BepuPhysics/CollisionDetection/ContactConstraintAccessor.cs @@ -182,7 +182,7 @@ protected ContactConstraintAccessor() "The layout of nonconvex accumulated impulses seems to have changed; the assumptions of impulse gather/scatter are probably no longer valid."); } AccumulatedImpulseBundleStrideInBytes = Unsafe.SizeOf(); - ConstraintTypeId = default(TConstraintDescription).ConstraintTypeId; + ConstraintTypeId = TConstraintDescription.ConstraintTypeId; } public override void DeterministicallyAdd(int typeIndex, NarrowPhase.OverlapWorker[] overlapWorkers, ref QuickList.SortConstraintTarget> constraintsOfType, @@ -291,7 +291,7 @@ public override void UpdateConstraintForManifold(ref manifoldPointer); Unsafe.SkipInit(out ConstraintCache constraintCache); Unsafe.SkipInit(out TConstraintDescription description); - CopyContactData(ref manifold, ref constraintCache, ref description.GetFirstContact(ref description)); + CopyContactData(ref manifold, ref constraintCache, ref TConstraintDescription.GetFirstContact(ref description)); description.CopyManifoldWideProperties(ref material); UpdateConstraint(narrowPhase, manifoldTypeAsConstraintType, workerIndex, ref pair, ref constraintCache, manifold.Count, ref description, bodyHandles); } @@ -452,7 +452,7 @@ public override void UpdateConstraintForManifold(ref manifoldPointer); Unsafe.SkipInit(out ConstraintCache constraintCache); Unsafe.SkipInit(out TConstraintDescription description); - CopyContactData(ref manifold, ref constraintCache, ref description.GetFirstContact(ref description)); + CopyContactData(ref manifold, ref constraintCache, ref TConstraintDescription.GetFirstContact(ref description)); description.CopyManifoldWideProperties(ref manifold.OffsetB, ref material); UpdateConstraint(narrowPhase, manifoldTypeAsConstraintType, workerIndex, ref pair, ref constraintCache, manifold.Count, ref description, bodyHandles); } diff --git a/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs b/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs index 59fbd0f87..ee54a13eb 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs @@ -125,7 +125,7 @@ static unsafe void AddToSimulationSpeculative encodedBodyIndices = stackalloc int[handles.Length]; simulation.Solver.GetBlockingBodyHandles(handles, ref blockingBodyHandles, encodedBodyIndices); while (!simulation.Solver.TryAllocateInBatch( - default(TDescription).ConstraintTypeId, batchIndex, + TDescription.ConstraintTypeId, batchIndex, blockingBodyHandles, encodedBodyIndices, out constraintHandle, out reference)) { //If a batch index failed, just try the next one. This is guaranteed to eventually work. diff --git a/BepuPhysics/CollisionDetection/SweepTaskRegistry.cs b/BepuPhysics/CollisionDetection/SweepTaskRegistry.cs index 6fc3482a1..57b972980 100644 --- a/BepuPhysics/CollisionDetection/SweepTaskRegistry.cs +++ b/BepuPhysics/CollisionDetection/SweepTaskRegistry.cs @@ -182,7 +182,7 @@ public SweepTask GetTask() where TShapeA : unmanaged, IShape where TShapeB : unmanaged, IShape { - return GetTask(default(TShapeA).TypeId, default(TShapeB).TypeId); + return GetTask(TShapeA.TypeId, TShapeB.TypeId); } } } diff --git a/BepuPhysics/CollisionDetection/SweepTasks/CompoundHomogeneousCompoundSweepTask.cs b/BepuPhysics/CollisionDetection/SweepTasks/CompoundHomogeneousCompoundSweepTask.cs index 4330b951b..35af6b428 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/CompoundHomogeneousCompoundSweepTask.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/CompoundHomogeneousCompoundSweepTask.cs @@ -15,8 +15,8 @@ public class CompoundHomogeneousCompoundSweepTask( @@ -26,13 +26,12 @@ protected unsafe override bool PreorderedTypeSweep( bool flipRequired, ref TSweepFilter filter, Shapes shapes, SweepTaskRegistry sweepTasks, BufferPool pool, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal) { ref var compoundB = ref Unsafe.AsRef(shapeDataB); - TOverlapFinder overlapFinder = default; t0 = float.MaxValue; t1 = float.MaxValue; hitLocation = new Vector3(); hitNormal = new Vector3(); ref var compoundA = ref Unsafe.AsRef(shapeDataA); - overlapFinder.FindOverlaps(ref compoundA, orientationA, velocityA, ref compoundB, offsetB, orientationB, velocityB, maximumT, shapes, pool, out var overlaps); + TOverlapFinder.FindOverlaps(ref compoundA, orientationA, velocityA, ref compoundB, offsetB, orientationB, velocityB, maximumT, shapes, pool, out var overlaps); for (int i = 0; i < overlaps.ChildCount; ++i) { ref var childOverlaps = ref overlaps.GetOverlapsForChild(i); diff --git a/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepOverlapFinder.cs b/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepOverlapFinder.cs index c6d1690cc..6bec6fae3 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepOverlapFinder.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepOverlapFinder.cs @@ -9,7 +9,7 @@ namespace BepuPhysics.CollisionDetection.SweepTasks //At the moment, this is basically an unused abstraction. But, if you wanted, this allows you to use a special cased overlap finder in certain cases. public interface ICompoundPairSweepOverlapFinder where TCompoundA : struct, ICompoundShape where TCompoundB : struct, IBoundsQueryableCompound { - unsafe void FindOverlaps(ref TCompoundA compoundA, Quaternion orientationA, in BodyVelocity velocityA, + static abstract void FindOverlaps(ref TCompoundA compoundA, Quaternion orientationA, in BodyVelocity velocityA, ref TCompoundB compoundB, Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float maximumT, Shapes shapes, BufferPool pool, out CompoundPairSweepOverlaps overlaps); } @@ -18,7 +18,7 @@ public struct CompoundPairSweepOverlapFinder : ICompound where TCompoundA : struct, ICompoundShape where TCompoundB : struct, IBoundsQueryableCompound { - public unsafe void FindOverlaps( + public static unsafe void FindOverlaps( ref TCompoundA compoundA, Quaternion orientationA, in BodyVelocity velocityA, ref TCompoundB compoundB, Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float maximumT, Shapes shapes, BufferPool pool, out CompoundPairSweepOverlaps overlaps) diff --git a/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepTask.cs b/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepTask.cs index c080a88bc..f52c7fcc8 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepTask.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepTask.cs @@ -13,8 +13,8 @@ public class CompoundPairSweepTask : Swe { public CompoundPairSweepTask() { - ShapeTypeIndexA = default(TCompoundA).TypeId; - ShapeTypeIndexB = default(TCompoundB).TypeId; + ShapeTypeIndexA = TCompoundA.TypeId; + ShapeTypeIndexB = TCompoundB.TypeId; } protected override unsafe bool PreorderedTypeSweep( @@ -29,7 +29,7 @@ protected override unsafe bool PreorderedTypeSweep( t1 = float.MaxValue; hitLocation = new Vector3(); hitNormal = new Vector3(); - default(TOverlapFinder).FindOverlaps( + TOverlapFinder.FindOverlaps( ref a, orientationA, velocityA, ref b, offsetB, orientationB, velocityB, maximumT, shapes, pool, out var overlaps); for (int i = 0; i < overlaps.ChildCount; ++i) diff --git a/BepuPhysics/CollisionDetection/SweepTasks/ConvexCompoundSweepOverlapFinder.cs b/BepuPhysics/CollisionDetection/SweepTasks/ConvexCompoundSweepOverlapFinder.cs index 010f1c5f9..f70e279e7 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/ConvexCompoundSweepOverlapFinder.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/ConvexCompoundSweepOverlapFinder.cs @@ -12,7 +12,7 @@ namespace BepuPhysics.CollisionDetection.SweepTasks //At the moment, this is basically an unused abstraction. But, if you wanted, this allows you to use a special cased overlap finder in certain cases. public interface IConvexCompoundSweepOverlapFinder where TShapeA : struct, IConvexShape where TCompoundB : struct, IBoundsQueryableCompound { - unsafe void FindOverlaps(ref TShapeA shapeA, Quaternion orientationA, in BodyVelocity velocityA, + static abstract void FindOverlaps(ref TShapeA shapeA, Quaternion orientationA, in BodyVelocity velocityA, ref TCompoundB compoundB, Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float maximumT, Shapes shapes, BufferPool pool, out ChildOverlapsCollection overlaps); } @@ -20,7 +20,7 @@ unsafe void FindOverlaps(ref TShapeA shapeA, Quaternion orientationA, in BodyVel public struct ConvexCompoundSweepOverlapFinder : IConvexCompoundSweepOverlapFinder where TShapeA : struct, IConvexShape where TCompoundB : struct, IBoundsQueryableCompound { - public unsafe void FindOverlaps(ref TShapeA shapeA, Quaternion orientationA, in BodyVelocity velocityA, + public static unsafe void FindOverlaps(ref TShapeA shapeA, Quaternion orientationA, in BodyVelocity velocityA, ref TCompoundB compoundB, Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float maximumT, Shapes shapes, BufferPool pool, out ChildOverlapsCollection overlaps) { diff --git a/BepuPhysics/CollisionDetection/SweepTasks/ConvexCompoundSweepTask.cs b/BepuPhysics/CollisionDetection/SweepTasks/ConvexCompoundSweepTask.cs index c2c09a35b..a1b86deab 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/ConvexCompoundSweepTask.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/ConvexCompoundSweepTask.cs @@ -14,8 +14,8 @@ public class ConvexCompoundSweepTask( @@ -30,7 +30,7 @@ protected override unsafe bool PreorderedTypeSweep( t1 = float.MaxValue; hitLocation = new Vector3(); hitNormal = new Vector3(); - default(TOverlapFinder).FindOverlaps(ref convex, orientationA, velocityA, ref compound, offsetB, orientationB, velocityB, maximumT, shapes, pool, out var overlaps); + TOverlapFinder.FindOverlaps(ref convex, orientationA, velocityA, ref compound, offsetB, orientationB, velocityB, maximumT, shapes, pool, out var overlaps); for (int i = 0; i < overlaps.Count; ++i) { var compoundChildIndex = overlaps.Overlaps[i]; @@ -39,9 +39,9 @@ protected override unsafe bool PreorderedTypeSweep( ref var child = ref compound.GetChild(compoundChildIndex); var childType = child.ShapeIndex.Type; shapes[childType].GetShapeData(child.ShapeIndex.Index, out var childShapeData, out _); - var task = sweepTasks.GetTask(convex.TypeId, childType); + var task = sweepTasks.GetTask(TShapeA.TypeId, childType); if (task != null && task.Sweep( - shapeDataA, convex.TypeId, new RigidPose() { Orientation = Quaternion.Identity }, orientationA, velocityA, + shapeDataA, TShapeA.TypeId, new RigidPose() { Orientation = Quaternion.Identity }, orientationA, velocityA, childShapeData, childType, CompoundChild.AsPose(ref child), offsetB, orientationB, velocityB, maximumT, minimumProgression, convergenceThreshold, maximumIterationCount, out var t0Candidate, out var t1Candidate, out var hitLocationCandidate, out var hitNormalCandidate)) diff --git a/BepuPhysics/CollisionDetection/SweepTasks/ConvexHomogeneousCompoundSweepTask.cs b/BepuPhysics/CollisionDetection/SweepTasks/ConvexHomogeneousCompoundSweepTask.cs index 52ca6159a..3912703ee 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/ConvexHomogeneousCompoundSweepTask.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/ConvexHomogeneousCompoundSweepTask.cs @@ -18,8 +18,8 @@ public class ConvexHomogeneousCompoundSweepTask( t1 = float.MaxValue; hitLocation = new Vector3(); hitNormal = new Vector3(); - var task = sweepTasks.GetTask(ShapeTypeIndexA, default(TChildType).TypeId); + var task = sweepTasks.GetTask(ShapeTypeIndexA, TChildType.TypeId); if (task != null) { - default(TOverlapFinder).FindOverlaps(ref Unsafe.AsRef(shapeDataA), orientationA, velocityA, ref compound, offsetB, orientationB, velocityB, maximumT, shapes, pool, out var overlaps); + TOverlapFinder.FindOverlaps(ref Unsafe.AsRef(shapeDataA), orientationA, velocityA, ref compound, offsetB, orientationB, velocityB, maximumT, shapes, pool, out var overlaps); for (int i = 0; i < overlaps.Count; ++i) { var childIndex = overlaps.Overlaps[i]; diff --git a/BepuPhysics/CollisionDetection/SweepTasks/ConvexSweepTaskCommon.cs b/BepuPhysics/CollisionDetection/SweepTasks/ConvexSweepTaskCommon.cs index 8cdcf0593..d14f1d209 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/ConvexSweepTaskCommon.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/ConvexSweepTaskCommon.cs @@ -23,8 +23,8 @@ public class ConvexPairSweepTask(r for (int i = 0; i < raySource.RayCount; i += Vector.Count) { var count = raySource.RayCount - i; - if (count < wide.MinimumWideRayCount) + if (count < TShapeWide.MinimumWideRayCount) { for (int j = 0; j < count; ++j) { diff --git a/BepuPhysics/Constraints/AngularAxisGearMotor.cs b/BepuPhysics/Constraints/AngularAxisGearMotor.cs index 0c6ccaa11..c448bf5fb 100644 --- a/BepuPhysics/Constraints/AngularAxisGearMotor.cs +++ b/BepuPhysics/Constraints/AngularAxisGearMotor.cs @@ -28,7 +28,7 @@ public struct AngularAxisGearMotor : ITwoBodyConstraintDescription public MotorSettings Settings; - public readonly int ConstraintTypeId + public static int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get @@ -37,8 +37,8 @@ public readonly int ConstraintTypeId } } - public readonly Type TypeProcessorType => typeof(AngularAxisGearMotorTypeProcessor); - public readonly TypeProcessor CreateTypeProcessor() => new AngularAxisGearMotorTypeProcessor(); + public static Type TypeProcessorType => typeof(AngularAxisGearMotorTypeProcessor); + public static TypeProcessor CreateTypeProcessor() => new AngularAxisGearMotorTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { @@ -51,7 +51,7 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int MotorSettingsWide.WriteFirst(Settings, ref target.Settings); } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out AngularAxisGearMotor description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out AngularAxisGearMotor description) { Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); @@ -78,7 +78,7 @@ public static void ApplyImpulse(in Vector3Wide impulseToVelocityA, in Vector3Wid } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref AngularAxisGearMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref AngularAxisGearMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { QuaternionWide.TransformWithoutOverlap(prestep.LocalAxisA, orientationA, out var axis); Vector3Wide.Scale(axis, prestep.VelocityScale, out var jA); @@ -88,7 +88,7 @@ public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref AngularAxisGearMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref AngularAxisGearMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { //This is mildly more complex than the AngularAxisMotor: //dot(wa, axis) * velocityScale - dot(wb, axis) = 0, so jacobianA is actually axis * velocityScale, not just -axis. @@ -109,9 +109,9 @@ public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in B ApplyImpulse(impulseToVelocityA, negatedImpulseToVelocityB, accumulatedImpulses, ref wsvA.Angular, ref wsvB.Angular); } - public bool RequiresIncrementalSubstepUpdates => false; + public static bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref AngularAxisGearMotorPrestepData prestepData) { } + public static void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref AngularAxisGearMotorPrestepData prestepData) { } } public class AngularAxisGearMotorTypeProcessor : TwoBodyTypeProcessor, AngularAxisGearMotorFunctions, AccessOnlyAngular, AccessOnlyAngularWithoutPose, AccessOnlyAngular, AccessOnlyAngularWithoutPose> diff --git a/BepuPhysics/Constraints/AngularAxisMotor.cs b/BepuPhysics/Constraints/AngularAxisMotor.cs index e60722265..aeb32f1da 100644 --- a/BepuPhysics/Constraints/AngularAxisMotor.cs +++ b/BepuPhysics/Constraints/AngularAxisMotor.cs @@ -27,7 +27,7 @@ public struct AngularAxisMotor : ITwoBodyConstraintDescription /// public MotorSettings Settings; - public readonly int ConstraintTypeId + public static int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get @@ -36,8 +36,8 @@ public readonly int ConstraintTypeId } } - public readonly Type TypeProcessorType => typeof(AngularAxisMotorTypeProcessor); - public readonly TypeProcessor CreateTypeProcessor() => new AngularAxisMotorTypeProcessor(); + public static Type TypeProcessorType => typeof(AngularAxisMotorTypeProcessor); + public static TypeProcessor CreateTypeProcessor() => new AngularAxisMotorTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { @@ -50,7 +50,7 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int MotorSettingsWide.WriteFirst(Settings, ref target.Settings); } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out AngularAxisMotor description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out AngularAxisMotor description) { Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); @@ -77,7 +77,7 @@ public static void ApplyImpulse(in Vector3Wide impulseToVelocityA, in Vector3Wid } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref AngularAxisMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref AngularAxisMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { QuaternionWide.TransformWithoutOverlap(prestep.LocalAxisA, orientationA, out var axis); Symmetric3x3Wide.TransformWithoutOverlap(axis, inertiaA.InverseInertiaTensor, out var jIA); @@ -86,7 +86,7 @@ public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref AngularAxisMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref AngularAxisMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { QuaternionWide.TransformWithoutOverlap(prestep.LocalAxisA, orientationA, out var jA); Symmetric3x3Wide.TransformWithoutOverlap(jA, inertiaA.InverseInertiaTensor, out var jIA); @@ -101,9 +101,9 @@ public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in B ApplyImpulse(jIA, jIB, csi, ref wsvA.Angular, ref wsvB.Angular); } - public bool RequiresIncrementalSubstepUpdates => false; + public static bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref AngularAxisMotorPrestepData prestepData) { } + public static void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref AngularAxisMotorPrestepData prestepData) { } } public class AngularAxisMotorTypeProcessor : TwoBodyTypeProcessor, AngularAxisMotorFunctions, AccessOnlyAngular, AccessOnlyAngularWithoutPose, AccessOnlyAngular, AccessOnlyAngular> diff --git a/BepuPhysics/Constraints/AngularHinge.cs b/BepuPhysics/Constraints/AngularHinge.cs index a0aef07f8..dc35fa456 100644 --- a/BepuPhysics/Constraints/AngularHinge.cs +++ b/BepuPhysics/Constraints/AngularHinge.cs @@ -26,7 +26,7 @@ public struct AngularHinge : ITwoBodyConstraintDescription /// public SpringSettings SpringSettings; - public readonly int ConstraintTypeId + public static int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get @@ -35,8 +35,8 @@ public readonly int ConstraintTypeId } } - public readonly Type TypeProcessorType => typeof(AngularHingeTypeProcessor); - public readonly TypeProcessor CreateTypeProcessor() => new AngularHingeTypeProcessor(); + public static Type TypeProcessorType => typeof(AngularHingeTypeProcessor); + public static TypeProcessor CreateTypeProcessor() => new AngularHingeTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { @@ -51,7 +51,7 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int GetFirst(ref target.SpringSettings.TwiceDampingRatio) = SpringSettings.TwiceDampingRatio; } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out AngularHinge description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out AngularHinge description) { Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); @@ -131,7 +131,7 @@ private static void ComputeJacobians(in Vector3Wide localHingeAxisA, in Quaterni Matrix3x3Wide.TransformWithoutOverlap(localAY, orientationMatrixA, out jacobianA.Y); } - public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref AngularHingePrestepData prestep, ref Vector2Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref AngularHingePrestepData prestep, ref Vector2Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeJacobians(prestep.LocalHingeAxisA, orientationA, out _, out var jacobianA); Symmetric3x3Wide.MultiplyWithoutOverlap(jacobianA, inertiaA.InverseInertiaTensor, out var impulseToVelocityA); @@ -139,7 +139,7 @@ public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, ApplyImpulse(impulseToVelocityA, negatedImpulseToVelocityB, accumulatedImpulses, ref wsvA.Angular, ref wsvB.Angular); } - public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref AngularHingePrestepData prestep, ref Vector2Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref AngularHingePrestepData prestep, ref Vector2Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { //Note that we build the tangents in local space first to avoid inconsistencies. ComputeJacobians(prestep.LocalHingeAxisA, orientationA, out var hingeAxisA, out var jacobianA); @@ -218,9 +218,9 @@ public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in B ApplyImpulse(impulseToVelocityA, negatedImpulseToVelocityB, csi, ref wsvA.Angular, ref wsvB.Angular); } - public bool RequiresIncrementalSubstepUpdates => false; + public static bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref AngularHingePrestepData prestepData) { } + public static void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref AngularHingePrestepData prestepData) { } } public class AngularHingeTypeProcessor : TwoBodyTypeProcessor diff --git a/BepuPhysics/Constraints/AngularMotor.cs b/BepuPhysics/Constraints/AngularMotor.cs index 6670bd5c0..ca8ddd832 100644 --- a/BepuPhysics/Constraints/AngularMotor.cs +++ b/BepuPhysics/Constraints/AngularMotor.cs @@ -22,7 +22,7 @@ public struct AngularMotor : ITwoBodyConstraintDescription /// public MotorSettings Settings; - public readonly int ConstraintTypeId + public static int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get @@ -31,8 +31,8 @@ public readonly int ConstraintTypeId } } - public readonly Type TypeProcessorType => typeof(AngularMotorTypeProcessor); - public readonly TypeProcessor CreateTypeProcessor() => new AngularMotorTypeProcessor(); + public static Type TypeProcessorType => typeof(AngularMotorTypeProcessor); + public static TypeProcessor CreateTypeProcessor() => new AngularMotorTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { @@ -43,7 +43,7 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int MotorSettingsWide.WriteFirst(Settings, ref target.Settings); } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out AngularMotor description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out AngularMotor description) { Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); @@ -60,12 +60,12 @@ public struct AngularMotorPrestepData public struct AngularMotorFunctions : ITwoBodyConstraintFunctions { - public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref AngularMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref AngularMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { AngularServoFunctions.ApplyImpulse(ref wsvA.Angular, ref wsvB.Angular, inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, accumulatedImpulses); } - public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref AngularMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref AngularMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { //Jacobians are just the identity matrix. MotorSettingsWide.ComputeSoftness(prestep.Settings, dt, out var effectiveMassCFMScale, out var softnessImpulseScale, out var maximumImpulse); @@ -88,9 +88,9 @@ public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in B AngularServoFunctions.ApplyImpulse(ref wsvA.Angular, ref wsvB.Angular, inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, csi); } - public bool RequiresIncrementalSubstepUpdates => false; + public static bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref AngularMotorPrestepData prestepData) { } + public static void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref AngularMotorPrestepData prestepData) { } } public class AngularMotorTypeProcessor : TwoBodyTypeProcessor diff --git a/BepuPhysics/Constraints/AngularServo.cs b/BepuPhysics/Constraints/AngularServo.cs index e1a8a7273..c7fdf3945 100644 --- a/BepuPhysics/Constraints/AngularServo.cs +++ b/BepuPhysics/Constraints/AngularServo.cs @@ -26,7 +26,7 @@ public struct AngularServo : ITwoBodyConstraintDescription /// public ServoSettings ServoSettings; - public readonly int ConstraintTypeId + public static int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get @@ -35,8 +35,8 @@ public readonly int ConstraintTypeId } } - public readonly Type TypeProcessorType => typeof(AngularServoTypeProcessor); - public readonly TypeProcessor CreateTypeProcessor() => new AngularServoTypeProcessor(); + public static Type TypeProcessorType => typeof(AngularServoTypeProcessor); + public static TypeProcessor CreateTypeProcessor() => new AngularServoTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { @@ -49,7 +49,7 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int ServoSettingsWide.WriteFirst(ServoSettings, ref target.ServoSettings); } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out AngularServo description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out AngularServo description) { Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); @@ -98,12 +98,12 @@ public static void ApplyImpulse(ref Vector3Wide angularVelocityA, ref Vector3Wid //} - public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref AngularServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref AngularServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ApplyImpulse(ref wsvA.Angular, ref wsvB.Angular, inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, accumulatedImpulses); } - public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref AngularServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref AngularServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { //Jacobians are just I and -I. QuaternionWide.ConcatenateWithoutOverlap(prestep.TargetRelativeRotationLocalA, orientationA, out var targetOrientationB); @@ -132,9 +132,9 @@ public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in B ApplyImpulse(ref wsvA.Angular, ref wsvB.Angular, inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, csi); } - public bool RequiresIncrementalSubstepUpdates => false; + public static bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref AngularServoPrestepData prestepData) { } + public static void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref AngularServoPrestepData prestepData) { } } public class AngularServoTypeProcessor : TwoBodyTypeProcessor diff --git a/BepuPhysics/Constraints/AngularSwivelHinge.cs b/BepuPhysics/Constraints/AngularSwivelHinge.cs index 490b59059..865dc1eef 100644 --- a/BepuPhysics/Constraints/AngularSwivelHinge.cs +++ b/BepuPhysics/Constraints/AngularSwivelHinge.cs @@ -26,7 +26,7 @@ public struct AngularSwivelHinge : ITwoBodyConstraintDescription public SpringSettings SpringSettings; - public readonly int ConstraintTypeId + public static int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get @@ -35,8 +35,8 @@ public readonly int ConstraintTypeId } } - public readonly Type TypeProcessorType => typeof(AngularSwivelHingeTypeProcessor); - public readonly TypeProcessor CreateTypeProcessor() => new AngularSwivelHingeTypeProcessor(); + public static Type TypeProcessorType => typeof(AngularSwivelHingeTypeProcessor); + public static TypeProcessor CreateTypeProcessor() => new AngularSwivelHingeTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { @@ -51,7 +51,7 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int GetFirst(ref target.SpringSettings.TwiceDampingRatio) = SpringSettings.TwiceDampingRatio; } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out AngularSwivelHinge description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out AngularSwivelHinge description) { Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); @@ -94,7 +94,7 @@ static void ComputeJacobian(in Vector3Wide localSwivelAxisA, in Vector3Wide loca Vector3Wide.ConditionalSelect(useFallback, fallbackJacobian, jacobianA, out jacobianA); } - public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref AngularSwivelHingePrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref AngularSwivelHingePrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeJacobian(prestep.LocalSwivelAxisA, prestep.LocalHingeAxisB, orientationA, orientationB, out _, out _, out var jacobianA); Symmetric3x3Wide.TransformWithoutOverlap(jacobianA, inertiaA.InverseInertiaTensor, out var impulseToVelocityA); @@ -102,7 +102,7 @@ public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, ApplyImpulse(impulseToVelocityA, negatedImpulseToVelocityB, accumulatedImpulses, ref wsvA.Angular, ref wsvB.Angular); } - public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref AngularSwivelHingePrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref AngularSwivelHingePrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { //The swivel hinge attempts to keep an axis on body A separated 90 degrees from an axis on body B. In other words, this is the same as a hinge joint, but with one fewer DOF. //C = dot(swivelA, hingeB) = 0 @@ -143,9 +143,9 @@ public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in B } - public bool RequiresIncrementalSubstepUpdates => false; + public static bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref AngularSwivelHingePrestepData prestepData) { } + public static void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref AngularSwivelHingePrestepData prestepData) { } } public class AngularSwivelHingeTypeProcessor : TwoBodyTypeProcessor, AngularSwivelHingeFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular> diff --git a/BepuPhysics/Constraints/AreaConstraint.cs b/BepuPhysics/Constraints/AreaConstraint.cs index 28cce5e50..7c7bb0963 100644 --- a/BepuPhysics/Constraints/AreaConstraint.cs +++ b/BepuPhysics/Constraints/AreaConstraint.cs @@ -37,7 +37,7 @@ public AreaConstraint(Vector3 a, Vector3 b, Vector3 c, SpringSettings springSett SpringSettings = springSettings; } - public readonly int ConstraintTypeId + public static int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get @@ -46,8 +46,8 @@ public readonly int ConstraintTypeId } } - public readonly Type TypeProcessorType => typeof(AreaConstraintTypeProcessor); - public readonly TypeProcessor CreateTypeProcessor() => new AreaConstraintTypeProcessor(); + public static Type TypeProcessorType => typeof(AreaConstraintTypeProcessor); + public static TypeProcessor CreateTypeProcessor() => new AreaConstraintTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { @@ -59,7 +59,7 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int SpringSettingsWide.WriteFirst(SpringSettings, ref target.SpringSettings); } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out AreaConstraint description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out AreaConstraint description) { Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); @@ -121,7 +121,7 @@ static void ComputeJacobian(in Vector3Wide positionA, in Vector3Wide positionB, Vector3Wide.Add(jacobianB, jacobianC, out negatedJacobianA); } - public void WarmStart( + public static void WarmStart( in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, @@ -131,7 +131,7 @@ public void WarmStart( ApplyImpulse(inertiaA.InverseMass, inertiaB.InverseMass, inertiaC.InverseMass, negatedJacobianA, jacobianB, jacobianC, accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC); } - public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, float dt, float inverseDt, ref AreaConstraintPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC) + public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, float dt, float inverseDt, ref AreaConstraintPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC) { ComputeJacobian(positionA, positionB, positionC, out var normalLength, out var negatedJacobianA, out var jacobianB, out var jacobianC); @@ -167,9 +167,9 @@ public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in B ApplyImpulse(inertiaA.InverseMass, inertiaB.InverseMass, inertiaC.InverseMass, negatedJacobianA, jacobianB, jacobianC, csi, ref wsvA, ref wsvB, ref wsvC); } - public bool RequiresIncrementalSubstepUpdates => false; + public static bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, in BodyVelocityWide wsvC, ref AreaConstraintPrestepData prestepData) { } + public static void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, in BodyVelocityWide wsvC, ref AreaConstraintPrestepData prestepData) { } } diff --git a/BepuPhysics/Constraints/BallSocket.cs b/BepuPhysics/Constraints/BallSocket.cs index b46c3d3e2..39980cb19 100644 --- a/BepuPhysics/Constraints/BallSocket.cs +++ b/BepuPhysics/Constraints/BallSocket.cs @@ -26,7 +26,7 @@ public struct BallSocket : ITwoBodyConstraintDescription /// public SpringSettings SpringSettings; - public readonly int ConstraintTypeId + public static int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get @@ -35,8 +35,8 @@ public readonly int ConstraintTypeId } } - public readonly Type TypeProcessorType => typeof(BallSocketTypeProcessor); - public readonly TypeProcessor CreateTypeProcessor() => new BallSocketTypeProcessor(); + public static Type TypeProcessorType => typeof(BallSocketTypeProcessor); + public static TypeProcessor CreateTypeProcessor() => new BallSocketTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { @@ -48,7 +48,7 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int SpringSettingsWide.WriteFirst(SpringSettings, ref target.SpringSettings); } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out BallSocket description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out BallSocket description) { Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); @@ -66,7 +66,7 @@ public struct BallSocketPrestepData } public struct BallSocketFunctions : ITwoBodyConstraintFunctions { - public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, + public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref BallSocketPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetA, orientationA, out var offsetA); @@ -74,7 +74,7 @@ public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, BallSocketShared.ApplyImpulse(ref wsvA, ref wsvB, offsetA, offsetB, inertiaA, inertiaB, accumulatedImpulses); } - public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, + public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref BallSocketPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetA, orientationA, out var offsetA); @@ -91,9 +91,9 @@ public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in B BallSocketShared.Solve(ref wsvA, ref wsvB, offsetA, offsetB, biasVelocity, effectiveMass, softnessImpulseScale, ref accumulatedImpulses, inertiaA, inertiaB); } - public bool RequiresIncrementalSubstepUpdates => false; + public static bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref BallSocketPrestepData prestepData) { } + public static void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref BallSocketPrestepData prestepData) { } } diff --git a/BepuPhysics/Constraints/BallSocketMotor.cs b/BepuPhysics/Constraints/BallSocketMotor.cs index 846ffc2dd..a4c4b37bc 100644 --- a/BepuPhysics/Constraints/BallSocketMotor.cs +++ b/BepuPhysics/Constraints/BallSocketMotor.cs @@ -29,7 +29,7 @@ public struct BallSocketMotor : ITwoBodyConstraintDescription /// public MotorSettings Settings; - public readonly int ConstraintTypeId + public static int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get @@ -38,8 +38,8 @@ public readonly int ConstraintTypeId } } - public readonly Type TypeProcessorType => typeof(BallSocketMotorTypeProcessor); - public readonly TypeProcessor CreateTypeProcessor() => new BallSocketMotorTypeProcessor(); + public static Type TypeProcessorType => typeof(BallSocketMotorTypeProcessor); + public static TypeProcessor CreateTypeProcessor() => new BallSocketMotorTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { @@ -51,7 +51,7 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int MotorSettingsWide.WriteFirst(Settings, ref target.Settings); } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out BallSocketMotor description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out BallSocketMotor description) { Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); @@ -70,13 +70,13 @@ public struct BallSocketMotorPrestepData public struct BallSocketMotorFunctions : ITwoBodyConstraintFunctions { - public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref BallSocketMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref BallSocketMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetB, orientationB, out var targetOffsetB); BallSocketShared.ApplyImpulse(ref wsvA, ref wsvB, (positionB - positionA) + targetOffsetB, targetOffsetB, inertiaA, inertiaB, accumulatedImpulses); } - public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref BallSocketMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref BallSocketMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetB, orientationB, out var targetOffsetB); var offsetA = (positionB - positionA) + targetOffsetB; @@ -90,9 +90,9 @@ public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in B BallSocketShared.Solve(ref wsvA, ref wsvB, offsetA, targetOffsetB, biasVelocity, effectiveMass, softnessImpulseScale, maximumImpulse, ref accumulatedImpulses, inertiaA, inertiaB); } - public bool RequiresIncrementalSubstepUpdates => false; + public static bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref BallSocketMotorPrestepData prestepData) { } + public static void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref BallSocketMotorPrestepData prestepData) { } } diff --git a/BepuPhysics/Constraints/BallSocketServo.cs b/BepuPhysics/Constraints/BallSocketServo.cs index bc62e46f3..1b6623c71 100644 --- a/BepuPhysics/Constraints/BallSocketServo.cs +++ b/BepuPhysics/Constraints/BallSocketServo.cs @@ -31,7 +31,7 @@ public struct BallSocketServo : ITwoBodyConstraintDescription /// public ServoSettings ServoSettings; - public readonly int ConstraintTypeId + public static int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get @@ -40,8 +40,8 @@ public readonly int ConstraintTypeId } } - public readonly Type TypeProcessorType => typeof(BallSocketServoTypeProcessor); - public readonly TypeProcessor CreateTypeProcessor() => new BallSocketServoTypeProcessor(); + public static Type TypeProcessorType => typeof(BallSocketServoTypeProcessor); + public static TypeProcessor CreateTypeProcessor() => new BallSocketServoTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { @@ -54,7 +54,7 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int ServoSettingsWide.WriteFirst(ServoSettings, ref target.ServoSettings); } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out BallSocketServo description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out BallSocketServo description) { Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); @@ -75,14 +75,14 @@ public struct BallSocketServoPrestepData public struct BallSocketServoFunctions : ITwoBodyConstraintFunctions { - public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref BallSocketServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref BallSocketServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetA, orientationA, out var offsetA); QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetB, orientationB, out var offsetB); BallSocketShared.ApplyImpulse(ref wsvA, ref wsvB, offsetA, offsetB, inertiaA, inertiaB, accumulatedImpulses); } - public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref BallSocketServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref BallSocketServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetA, orientationA, out var offsetA); QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetB, orientationB, out var offsetB); @@ -98,9 +98,9 @@ public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in B BallSocketShared.Solve(ref wsvA, ref wsvB, offsetA, offsetB, biasVelocity, effectiveMass, softnessImpulseScale, maximumImpulse, ref accumulatedImpulses, inertiaA, inertiaB); } - public bool RequiresIncrementalSubstepUpdates => false; + public static bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref BallSocketServoPrestepData prestepData) { } + public static void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref BallSocketServoPrestepData prestepData) { } } diff --git a/BepuPhysics/Constraints/CenterDistanceConstraint.cs b/BepuPhysics/Constraints/CenterDistanceConstraint.cs index 1fa4923d3..8de3eca90 100644 --- a/BepuPhysics/Constraints/CenterDistanceConstraint.cs +++ b/BepuPhysics/Constraints/CenterDistanceConstraint.cs @@ -30,7 +30,7 @@ public CenterDistanceConstraint(float targetDistance, in SpringSettings springSe SpringSettings = springSettings; } - public readonly int ConstraintTypeId + public static int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get @@ -39,8 +39,8 @@ public readonly int ConstraintTypeId } } - public readonly Type TypeProcessorType => typeof(CenterDistanceTypeProcessor); - public readonly TypeProcessor CreateTypeProcessor() => new CenterDistanceTypeProcessor(); + public static Type TypeProcessorType => typeof(CenterDistanceTypeProcessor); + public static TypeProcessor CreateTypeProcessor() => new CenterDistanceTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { @@ -52,7 +52,7 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int SpringSettingsWide.WriteFirst(SpringSettings, ref target.SpringSettings); } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out CenterDistanceConstraint description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out CenterDistanceConstraint description) { Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); @@ -80,7 +80,7 @@ public static void ApplyImpulse(in Vector3Wide jacobianA, in Vector inver //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, + public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref CenterDistancePrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { var ab = positionB - positionA; @@ -95,7 +95,7 @@ public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, ApplyImpulse(jacobianA, inertiaA.InverseMass, inertiaB.InverseMass, accumulatedImpulses, ref wsvA, ref wsvB); } //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, + public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref CenterDistancePrestepData prestep, ref Vector accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { //Note that we need the actual length for error calculation. @@ -123,9 +123,9 @@ public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in B ApplyImpulse(jacobianA, inertiaA.InverseMass, inertiaB.InverseMass, csi, ref wsvA, ref wsvB); } - public bool RequiresIncrementalSubstepUpdates => false; + public static bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref CenterDistancePrestepData prestepData) { } + public static void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref CenterDistancePrestepData prestepData) { } } diff --git a/BepuPhysics/Constraints/CenterDistanceLimit.cs b/BepuPhysics/Constraints/CenterDistanceLimit.cs index d916bf6b1..c373d02df 100644 --- a/BepuPhysics/Constraints/CenterDistanceLimit.cs +++ b/BepuPhysics/Constraints/CenterDistanceLimit.cs @@ -35,7 +35,7 @@ public CenterDistanceLimit(float minimumDistance, float maximumDistance, in Spri SpringSettings = springSettings; } - public readonly int ConstraintTypeId + public static int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get @@ -44,8 +44,8 @@ public readonly int ConstraintTypeId } } - public readonly Type TypeProcessorType => typeof(CenterDistanceLimitTypeProcessor); - public readonly TypeProcessor CreateTypeProcessor() => new CenterDistanceLimitTypeProcessor(); + public static Type TypeProcessorType => typeof(CenterDistanceLimitTypeProcessor); + public static TypeProcessor CreateTypeProcessor() => new CenterDistanceLimitTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { @@ -59,7 +59,7 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int SpringSettingsWide.WriteFirst(SpringSettings, ref target.SpringSettings); } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out CenterDistanceLimit description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out CenterDistanceLimit description) { Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); @@ -96,14 +96,14 @@ static void ComputeJacobian(Vector minimumDistance, Vector maximum jacobianA = Vector3Wide.ConditionalSelect(useMinimum, -jacobianA, jacobianA); } //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, + public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref CenterDistanceLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeJacobian(prestep.MinimumDistance, prestep.MaximumDistance, positionA, positionB, out var jacobianA, out _, out _); CenterDistanceConstraintFunctions.ApplyImpulse(jacobianA, inertiaA.InverseMass, inertiaB.InverseMass, accumulatedImpulses, ref wsvA, ref wsvB); } //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, + public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref CenterDistanceLimitPrestepData prestep, ref Vector accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeJacobian(prestep.MinimumDistance, prestep.MaximumDistance, positionA, positionB, out var jacobianA, out var distance, out var useMinimum); @@ -122,9 +122,9 @@ public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in B CenterDistanceConstraintFunctions.ApplyImpulse(jacobianA, inertiaA.InverseMass, inertiaB.InverseMass, csi, ref wsvA, ref wsvB); } - public bool RequiresIncrementalSubstepUpdates => false; + public static bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref CenterDistanceLimitPrestepData prestepData) { } + public static void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref CenterDistanceLimitPrestepData prestepData) { } } diff --git a/BepuPhysics/Constraints/Contact/ContactConvexCommon.cs b/BepuPhysics/Constraints/Contact/ContactConvexCommon.cs index bb6776a28..21da942ef 100644 --- a/BepuPhysics/Constraints/Contact/ContactConvexCommon.cs +++ b/BepuPhysics/Constraints/Contact/ContactConvexCommon.cs @@ -22,34 +22,34 @@ public struct MaterialPropertiesWide public interface IContactPrestep where TPrestep : struct, IContactPrestep { - ref MaterialPropertiesWide GetMaterialProperties(ref TPrestep prestep); - int ContactCount { get; } - int BodyCount { get; } + static abstract ref MaterialPropertiesWide GetMaterialProperties(ref TPrestep prestep); + static abstract int ContactCount { get; } + static abstract int BodyCount { get; } } public interface IConvexContactPrestep : IContactPrestep where TPrestep : struct, IConvexContactPrestep { - ref Vector3Wide GetNormal(ref TPrestep prestep); - ref ConvexContactWide GetContact(ref TPrestep prestep, int index); + static abstract ref Vector3Wide GetNormal(ref TPrestep prestep); + static abstract ref ConvexContactWide GetContact(ref TPrestep prestep, int index); } public interface ITwoBodyConvexContactPrestep : IConvexContactPrestep where TPrestep : struct, ITwoBodyConvexContactPrestep { - ref Vector3Wide GetOffsetB(ref TPrestep prestep); + static abstract ref Vector3Wide GetOffsetB(ref TPrestep prestep); } public interface IContactAccumulatedImpulses where TAccumulatedImpulses : struct, IContactAccumulatedImpulses { - int ContactCount { get; } + static abstract int ContactCount { get; } } public interface IConvexContactAccumulatedImpulses : IContactAccumulatedImpulses where TAccumulatedImpulses : struct, IConvexContactAccumulatedImpulses { - ref Vector2Wide GetTangentFriction(ref TAccumulatedImpulses impulses); - ref Vector GetTwistFriction(ref TAccumulatedImpulses impulses); - ref Vector GetPenetrationImpulseForContact(ref TAccumulatedImpulses impulses, int index); + static abstract ref Vector2Wide GetTangentFriction(ref TAccumulatedImpulses impulses); + static abstract ref Vector GetTwistFriction(ref TAccumulatedImpulses impulses); + static abstract ref Vector GetPenetrationImpulseForContact(ref TAccumulatedImpulses impulses, int index); } } diff --git a/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs b/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs index a450d287b..a858f6300 100644 --- a/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs +++ b/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs @@ -15,23 +15,23 @@ public struct Contact1AccumulatedImpulses : IConvexContactAccumulatedImpulses Twist; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector2Wide GetTangentFriction(ref Contact1AccumulatedImpulses impulses) + public static ref Vector2Wide GetTangentFriction(ref Contact1AccumulatedImpulses impulses) { return ref impulses.Tangent; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector GetTwistFriction(ref Contact1AccumulatedImpulses impulses) + public static ref Vector GetTwistFriction(ref Contact1AccumulatedImpulses impulses) { return ref impulses.Twist; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector GetPenetrationImpulseForContact(ref Contact1AccumulatedImpulses impulses, int index) + public static ref Vector GetPenetrationImpulseForContact(ref Contact1AccumulatedImpulses impulses, int index) { Debug.Assert(index >= 0 && index < 1); return ref Unsafe.Add(ref impulses.Penetration0, index); } - public int ContactCount => 1; + public static int ContactCount => 1; } public struct Contact2AccumulatedImpulses : IConvexContactAccumulatedImpulses @@ -42,23 +42,23 @@ public struct Contact2AccumulatedImpulses : IConvexContactAccumulatedImpulses Twist; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector2Wide GetTangentFriction(ref Contact2AccumulatedImpulses impulses) + public static ref Vector2Wide GetTangentFriction(ref Contact2AccumulatedImpulses impulses) { return ref impulses.Tangent; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector GetTwistFriction(ref Contact2AccumulatedImpulses impulses) + public static ref Vector GetTwistFriction(ref Contact2AccumulatedImpulses impulses) { return ref impulses.Twist; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector GetPenetrationImpulseForContact(ref Contact2AccumulatedImpulses impulses, int index) + public static ref Vector GetPenetrationImpulseForContact(ref Contact2AccumulatedImpulses impulses, int index) { Debug.Assert(index >= 0 && index < 2); return ref Unsafe.Add(ref impulses.Penetration0, index); } - public int ContactCount => 2; + public static int ContactCount => 2; } public struct Contact3AccumulatedImpulses : IConvexContactAccumulatedImpulses @@ -70,23 +70,23 @@ public struct Contact3AccumulatedImpulses : IConvexContactAccumulatedImpulses Twist; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector2Wide GetTangentFriction(ref Contact3AccumulatedImpulses impulses) + public static ref Vector2Wide GetTangentFriction(ref Contact3AccumulatedImpulses impulses) { return ref impulses.Tangent; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector GetTwistFriction(ref Contact3AccumulatedImpulses impulses) + public static ref Vector GetTwistFriction(ref Contact3AccumulatedImpulses impulses) { return ref impulses.Twist; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector GetPenetrationImpulseForContact(ref Contact3AccumulatedImpulses impulses, int index) + public static ref Vector GetPenetrationImpulseForContact(ref Contact3AccumulatedImpulses impulses, int index) { Debug.Assert(index >= 0 && index < 3); return ref Unsafe.Add(ref impulses.Penetration0, index); } - public int ContactCount => 3; + public static int ContactCount => 3; } public struct Contact4AccumulatedImpulses : IConvexContactAccumulatedImpulses @@ -99,23 +99,23 @@ public struct Contact4AccumulatedImpulses : IConvexContactAccumulatedImpulses Twist; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector2Wide GetTangentFriction(ref Contact4AccumulatedImpulses impulses) + public static ref Vector2Wide GetTangentFriction(ref Contact4AccumulatedImpulses impulses) { return ref impulses.Tangent; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector GetTwistFriction(ref Contact4AccumulatedImpulses impulses) + public static ref Vector GetTwistFriction(ref Contact4AccumulatedImpulses impulses) { return ref impulses.Twist; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector GetPenetrationImpulseForContact(ref Contact4AccumulatedImpulses impulses, int index) + public static ref Vector GetPenetrationImpulseForContact(ref Contact4AccumulatedImpulses impulses, int index) { Debug.Assert(index >= 0 && index < 4); return ref Unsafe.Add(ref impulses.Penetration0, index); } - public int ContactCount => 4; + public static int ContactCount => 4; } internal static class FrictionHelpers @@ -217,7 +217,7 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int GetFirst(ref target.MaterialProperties.MaximumRecoveryVelocity) = MaximumRecoveryVelocity; } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out Contact1OneBody description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out Contact1OneBody description) { Debug.Assert(batch.TypeId == ConstraintTypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); @@ -239,19 +239,19 @@ public void CopyManifoldWideProperties(ref Vector3 normal, ref PairMaterialPrope } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref ConstraintContactData GetFirstContact(ref Contact1OneBody description) + public static ref ConstraintContactData GetFirstContact(ref Contact1OneBody description) { return ref description.Contact0; } - public readonly int ConstraintTypeId + public static int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get => Contact1OneBodyTypeProcessor.BatchTypeId; } - public readonly Type TypeProcessorType => typeof(Contact1OneBodyTypeProcessor); - public readonly TypeProcessor CreateTypeProcessor() => new Contact1OneBodyTypeProcessor(); + public static Type TypeProcessorType => typeof(Contact1OneBodyTypeProcessor); + public static TypeProcessor CreateTypeProcessor() => new Contact1OneBodyTypeProcessor(); } @@ -265,23 +265,23 @@ public struct Contact1OneBodyPrestepData : IConvexContactPrestep 1; - public readonly int ContactCount => 1; + public static int BodyCount => 1; + public static int ContactCount => 1; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3Wide GetNormal(ref Contact1OneBodyPrestepData prestep) + public static ref Vector3Wide GetNormal(ref Contact1OneBodyPrestepData prestep) { return ref prestep.Normal; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref ConvexContactWide GetContact(ref Contact1OneBodyPrestepData prestep, int index) + public static ref ConvexContactWide GetContact(ref Contact1OneBodyPrestepData prestep, int index) { return ref Unsafe.Add(ref prestep.Contact0, index); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref MaterialPropertiesWide GetMaterialProperties(ref Contact1OneBodyPrestepData prestep) + public static ref MaterialPropertiesWide GetMaterialProperties(ref Contact1OneBodyPrestepData prestep) { return ref prestep.MaterialProperties; } @@ -291,16 +291,16 @@ public ref MaterialPropertiesWide GetMaterialProperties(ref Contact1OneBodyPrest public struct Contact1OneBodyFunctions : IOneBodyConstraintFunctions { - public bool RequiresIncrementalSubstepUpdates => true; + public static bool RequiresIncrementalSubstepUpdates => true; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide velocityA, ref Contact1OneBodyPrestepData prestep) + public static void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide velocityA, ref Contact1OneBodyPrestepData prestep) { PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, velocityA, ref prestep.Contact0.Depth); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref Contact1OneBodyPrestepData prestep, ref Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) + public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref Contact1OneBodyPrestepData prestep, ref Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); TangentFrictionOneBody.WarmStart(x, z, prestep.Contact0.OffsetA, inertiaA, accumulatedImpulses.Tangent, ref wsvA); @@ -309,7 +309,7 @@ public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref Contact1OneBodyPrestepData prestep, ref Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) + public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref Contact1OneBodyPrestepData prestep, ref Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { //Note that we solve the penetration constraints before the friction constraints. //This makes the friction constraints more authoritative, since they happen last. @@ -363,7 +363,7 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int GetFirst(ref target.MaterialProperties.MaximumRecoveryVelocity) = MaximumRecoveryVelocity; } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out Contact2OneBody description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out Contact2OneBody description) { Debug.Assert(batch.TypeId == ConstraintTypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); @@ -387,19 +387,19 @@ public void CopyManifoldWideProperties(ref Vector3 normal, ref PairMaterialPrope } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref ConstraintContactData GetFirstContact(ref Contact2OneBody description) + public static ref ConstraintContactData GetFirstContact(ref Contact2OneBody description) { return ref description.Contact0; } - public readonly int ConstraintTypeId + public static int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get => Contact2OneBodyTypeProcessor.BatchTypeId; } - public readonly Type TypeProcessorType => typeof(Contact2OneBodyTypeProcessor); - public readonly TypeProcessor CreateTypeProcessor() => new Contact2OneBodyTypeProcessor(); + public static Type TypeProcessorType => typeof(Contact2OneBodyTypeProcessor); + public static TypeProcessor CreateTypeProcessor() => new Contact2OneBodyTypeProcessor(); } @@ -414,23 +414,23 @@ public struct Contact2OneBodyPrestepData : IConvexContactPrestep 1; - public readonly int ContactCount => 2; + public static int BodyCount => 1; + public static int ContactCount => 2; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3Wide GetNormal(ref Contact2OneBodyPrestepData prestep) + public static ref Vector3Wide GetNormal(ref Contact2OneBodyPrestepData prestep) { return ref prestep.Normal; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref ConvexContactWide GetContact(ref Contact2OneBodyPrestepData prestep, int index) + public static ref ConvexContactWide GetContact(ref Contact2OneBodyPrestepData prestep, int index) { return ref Unsafe.Add(ref prestep.Contact0, index); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref MaterialPropertiesWide GetMaterialProperties(ref Contact2OneBodyPrestepData prestep) + public static ref MaterialPropertiesWide GetMaterialProperties(ref Contact2OneBodyPrestepData prestep) { return ref prestep.MaterialProperties; } @@ -440,17 +440,17 @@ public ref MaterialPropertiesWide GetMaterialProperties(ref Contact2OneBodyPrest public struct Contact2OneBodyFunctions : IOneBodyConstraintFunctions { - public bool RequiresIncrementalSubstepUpdates => true; + public static bool RequiresIncrementalSubstepUpdates => true; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide velocityA, ref Contact2OneBodyPrestepData prestep) + public static void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide velocityA, ref Contact2OneBodyPrestepData prestep) { PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, velocityA, ref prestep.Contact0.Depth); PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.Normal, velocityA, ref prestep.Contact1.Depth); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref Contact2OneBodyPrestepData prestep, ref Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) + public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref Contact2OneBodyPrestepData prestep, ref Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, out var offsetToManifoldCenterA); @@ -461,7 +461,7 @@ public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref Contact2OneBodyPrestepData prestep, ref Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) + public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref Contact2OneBodyPrestepData prestep, ref Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { //Note that we solve the penetration constraints before the friction constraints. //This makes the friction constraints more authoritative, since they happen last. @@ -521,7 +521,7 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int GetFirst(ref target.MaterialProperties.MaximumRecoveryVelocity) = MaximumRecoveryVelocity; } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out Contact3OneBody description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out Contact3OneBody description) { Debug.Assert(batch.TypeId == ConstraintTypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); @@ -547,19 +547,19 @@ public void CopyManifoldWideProperties(ref Vector3 normal, ref PairMaterialPrope } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref ConstraintContactData GetFirstContact(ref Contact3OneBody description) + public static ref ConstraintContactData GetFirstContact(ref Contact3OneBody description) { return ref description.Contact0; } - public readonly int ConstraintTypeId + public static int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get => Contact3OneBodyTypeProcessor.BatchTypeId; } - public readonly Type TypeProcessorType => typeof(Contact3OneBodyTypeProcessor); - public readonly TypeProcessor CreateTypeProcessor() => new Contact3OneBodyTypeProcessor(); + public static Type TypeProcessorType => typeof(Contact3OneBodyTypeProcessor); + public static TypeProcessor CreateTypeProcessor() => new Contact3OneBodyTypeProcessor(); } @@ -575,23 +575,23 @@ public struct Contact3OneBodyPrestepData : IConvexContactPrestep 1; - public readonly int ContactCount => 3; + public static int BodyCount => 1; + public static int ContactCount => 3; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3Wide GetNormal(ref Contact3OneBodyPrestepData prestep) + public static ref Vector3Wide GetNormal(ref Contact3OneBodyPrestepData prestep) { return ref prestep.Normal; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref ConvexContactWide GetContact(ref Contact3OneBodyPrestepData prestep, int index) + public static ref ConvexContactWide GetContact(ref Contact3OneBodyPrestepData prestep, int index) { return ref Unsafe.Add(ref prestep.Contact0, index); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref MaterialPropertiesWide GetMaterialProperties(ref Contact3OneBodyPrestepData prestep) + public static ref MaterialPropertiesWide GetMaterialProperties(ref Contact3OneBodyPrestepData prestep) { return ref prestep.MaterialProperties; } @@ -601,10 +601,10 @@ public ref MaterialPropertiesWide GetMaterialProperties(ref Contact3OneBodyPrest public struct Contact3OneBodyFunctions : IOneBodyConstraintFunctions { - public bool RequiresIncrementalSubstepUpdates => true; + public static bool RequiresIncrementalSubstepUpdates => true; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide velocityA, ref Contact3OneBodyPrestepData prestep) + public static void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide velocityA, ref Contact3OneBodyPrestepData prestep) { PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, velocityA, ref prestep.Contact0.Depth); PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.Normal, velocityA, ref prestep.Contact1.Depth); @@ -612,7 +612,7 @@ public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWi } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref Contact3OneBodyPrestepData prestep, ref Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) + public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref Contact3OneBodyPrestepData prestep, ref Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, out var offsetToManifoldCenterA); @@ -624,7 +624,7 @@ public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref Contact3OneBodyPrestepData prestep, ref Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) + public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref Contact3OneBodyPrestepData prestep, ref Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { //Note that we solve the penetration constraints before the friction constraints. //This makes the friction constraints more authoritative, since they happen last. @@ -689,7 +689,7 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int GetFirst(ref target.MaterialProperties.MaximumRecoveryVelocity) = MaximumRecoveryVelocity; } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out Contact4OneBody description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out Contact4OneBody description) { Debug.Assert(batch.TypeId == ConstraintTypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); @@ -717,19 +717,19 @@ public void CopyManifoldWideProperties(ref Vector3 normal, ref PairMaterialPrope } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref ConstraintContactData GetFirstContact(ref Contact4OneBody description) + public static ref ConstraintContactData GetFirstContact(ref Contact4OneBody description) { return ref description.Contact0; } - public readonly int ConstraintTypeId + public static int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get => Contact4OneBodyTypeProcessor.BatchTypeId; } - public readonly Type TypeProcessorType => typeof(Contact4OneBodyTypeProcessor); - public readonly TypeProcessor CreateTypeProcessor() => new Contact4OneBodyTypeProcessor(); + public static Type TypeProcessorType => typeof(Contact4OneBodyTypeProcessor); + public static TypeProcessor CreateTypeProcessor() => new Contact4OneBodyTypeProcessor(); } @@ -746,23 +746,23 @@ public struct Contact4OneBodyPrestepData : IConvexContactPrestep 1; - public readonly int ContactCount => 4; + public static int BodyCount => 1; + public static int ContactCount => 4; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3Wide GetNormal(ref Contact4OneBodyPrestepData prestep) + public static ref Vector3Wide GetNormal(ref Contact4OneBodyPrestepData prestep) { return ref prestep.Normal; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref ConvexContactWide GetContact(ref Contact4OneBodyPrestepData prestep, int index) + public static ref ConvexContactWide GetContact(ref Contact4OneBodyPrestepData prestep, int index) { return ref Unsafe.Add(ref prestep.Contact0, index); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref MaterialPropertiesWide GetMaterialProperties(ref Contact4OneBodyPrestepData prestep) + public static ref MaterialPropertiesWide GetMaterialProperties(ref Contact4OneBodyPrestepData prestep) { return ref prestep.MaterialProperties; } @@ -772,10 +772,10 @@ public ref MaterialPropertiesWide GetMaterialProperties(ref Contact4OneBodyPrest public struct Contact4OneBodyFunctions : IOneBodyConstraintFunctions { - public bool RequiresIncrementalSubstepUpdates => true; + public static bool RequiresIncrementalSubstepUpdates => true; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide velocityA, ref Contact4OneBodyPrestepData prestep) + public static void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide velocityA, ref Contact4OneBodyPrestepData prestep) { PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.Normal, velocityA, ref prestep.Contact0.Depth); PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.Normal, velocityA, ref prestep.Contact1.Depth); @@ -784,7 +784,7 @@ public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWi } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref Contact4OneBodyPrestepData prestep, ref Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) + public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref Contact4OneBodyPrestepData prestep, ref Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact3.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, prestep.Contact3.Depth, out var offsetToManifoldCenterA); @@ -797,7 +797,7 @@ public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref Contact4OneBodyPrestepData prestep, ref Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) + public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref Contact4OneBodyPrestepData prestep, ref Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { //Note that we solve the penetration constraints before the friction constraints. //This makes the friction constraints more authoritative, since they happen last. @@ -857,7 +857,7 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int GetFirst(ref target.MaterialProperties.MaximumRecoveryVelocity) = MaximumRecoveryVelocity; } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out Contact1 description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out Contact1 description) { Debug.Assert(batch.TypeId == ConstraintTypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); @@ -882,19 +882,19 @@ public void CopyManifoldWideProperties(ref Vector3 offsetB, ref Vector3 normal, } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref ConstraintContactData GetFirstContact(ref Contact1 description) + public static ref ConstraintContactData GetFirstContact(ref Contact1 description) { return ref description.Contact0; } - public readonly int ConstraintTypeId + public static int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get => Contact1TypeProcessor.BatchTypeId; } - public readonly Type TypeProcessorType => typeof(Contact1TypeProcessor); - public readonly TypeProcessor CreateTypeProcessor() => new Contact1TypeProcessor(); + public static Type TypeProcessorType => typeof(Contact1TypeProcessor); + public static TypeProcessor CreateTypeProcessor() => new Contact1TypeProcessor(); } @@ -909,29 +909,29 @@ public struct Contact1PrestepData : ITwoBodyConvexContactPrestep 2; - public readonly int ContactCount => 1; + public static int BodyCount => 2; + public static int ContactCount => 1; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3Wide GetNormal(ref Contact1PrestepData prestep) + public static ref Vector3Wide GetNormal(ref Contact1PrestepData prestep) { return ref prestep.Normal; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref ConvexContactWide GetContact(ref Contact1PrestepData prestep, int index) + public static ref ConvexContactWide GetContact(ref Contact1PrestepData prestep, int index) { return ref Unsafe.Add(ref prestep.Contact0, index); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref MaterialPropertiesWide GetMaterialProperties(ref Contact1PrestepData prestep) + public static ref MaterialPropertiesWide GetMaterialProperties(ref Contact1PrestepData prestep) { return ref prestep.MaterialProperties; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3Wide GetOffsetB(ref Contact1PrestepData prestep) + public static ref Vector3Wide GetOffsetB(ref Contact1PrestepData prestep) { return ref prestep.OffsetB; } @@ -940,16 +940,16 @@ public ref Vector3Wide GetOffsetB(ref Contact1PrestepData prestep) public struct Contact1Functions : ITwoBodyConstraintFunctions { - public bool RequiresIncrementalSubstepUpdates => true; + public static bool RequiresIncrementalSubstepUpdates => true; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide velocityA, in BodyVelocityWide velocityB, ref Contact1PrestepData prestep) + public static void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide velocityA, in BodyVelocityWide velocityB, ref Contact1PrestepData prestep) { PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact0.Depth); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref Contact1PrestepData prestep, ref Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref Contact1PrestepData prestep, ref Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); Vector3Wide.Subtract(prestep.Contact0.OffsetA, prestep.OffsetB, out var offsetToManifoldCenterB); @@ -959,7 +959,7 @@ public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref Contact1PrestepData prestep, ref Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref Contact1PrestepData prestep, ref Contact1AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { //Note that we solve the penetration constraints before the friction constraints. //This makes the friction constraints more authoritative, since they happen last. @@ -1016,7 +1016,7 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int GetFirst(ref target.MaterialProperties.MaximumRecoveryVelocity) = MaximumRecoveryVelocity; } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out Contact2 description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out Contact2 description) { Debug.Assert(batch.TypeId == ConstraintTypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); @@ -1043,19 +1043,19 @@ public void CopyManifoldWideProperties(ref Vector3 offsetB, ref Vector3 normal, } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref ConstraintContactData GetFirstContact(ref Contact2 description) + public static ref ConstraintContactData GetFirstContact(ref Contact2 description) { return ref description.Contact0; } - public readonly int ConstraintTypeId + public static int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get => Contact2TypeProcessor.BatchTypeId; } - public readonly Type TypeProcessorType => typeof(Contact2TypeProcessor); - public readonly TypeProcessor CreateTypeProcessor() => new Contact2TypeProcessor(); + public static Type TypeProcessorType => typeof(Contact2TypeProcessor); + public static TypeProcessor CreateTypeProcessor() => new Contact2TypeProcessor(); } @@ -1071,29 +1071,29 @@ public struct Contact2PrestepData : ITwoBodyConvexContactPrestep 2; - public readonly int ContactCount => 2; + public static int BodyCount => 2; + public static int ContactCount => 2; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3Wide GetNormal(ref Contact2PrestepData prestep) + public static ref Vector3Wide GetNormal(ref Contact2PrestepData prestep) { return ref prestep.Normal; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref ConvexContactWide GetContact(ref Contact2PrestepData prestep, int index) + public static ref ConvexContactWide GetContact(ref Contact2PrestepData prestep, int index) { return ref Unsafe.Add(ref prestep.Contact0, index); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref MaterialPropertiesWide GetMaterialProperties(ref Contact2PrestepData prestep) + public static ref MaterialPropertiesWide GetMaterialProperties(ref Contact2PrestepData prestep) { return ref prestep.MaterialProperties; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3Wide GetOffsetB(ref Contact2PrestepData prestep) + public static ref Vector3Wide GetOffsetB(ref Contact2PrestepData prestep) { return ref prestep.OffsetB; } @@ -1102,17 +1102,17 @@ public ref Vector3Wide GetOffsetB(ref Contact2PrestepData prestep) public struct Contact2Functions : ITwoBodyConstraintFunctions { - public bool RequiresIncrementalSubstepUpdates => true; + public static bool RequiresIncrementalSubstepUpdates => true; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide velocityA, in BodyVelocityWide velocityB, ref Contact2PrestepData prestep) + public static void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide velocityA, in BodyVelocityWide velocityB, ref Contact2PrestepData prestep) { PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact0.Depth); PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact1.Depth); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref Contact2PrestepData prestep, ref Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref Contact2PrestepData prestep, ref Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, out var offsetToManifoldCenterA); @@ -1124,7 +1124,7 @@ public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref Contact2PrestepData prestep, ref Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref Contact2PrestepData prestep, ref Contact2AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { //Note that we solve the penetration constraints before the friction constraints. //This makes the friction constraints more authoritative, since they happen last. @@ -1187,7 +1187,7 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int GetFirst(ref target.MaterialProperties.MaximumRecoveryVelocity) = MaximumRecoveryVelocity; } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out Contact3 description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out Contact3 description) { Debug.Assert(batch.TypeId == ConstraintTypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); @@ -1216,19 +1216,19 @@ public void CopyManifoldWideProperties(ref Vector3 offsetB, ref Vector3 normal, } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref ConstraintContactData GetFirstContact(ref Contact3 description) + public static ref ConstraintContactData GetFirstContact(ref Contact3 description) { return ref description.Contact0; } - public readonly int ConstraintTypeId + public static int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get => Contact3TypeProcessor.BatchTypeId; } - public readonly Type TypeProcessorType => typeof(Contact3TypeProcessor); - public readonly TypeProcessor CreateTypeProcessor() => new Contact3TypeProcessor(); + public static Type TypeProcessorType => typeof(Contact3TypeProcessor); + public static TypeProcessor CreateTypeProcessor() => new Contact3TypeProcessor(); } @@ -1245,29 +1245,29 @@ public struct Contact3PrestepData : ITwoBodyConvexContactPrestep 2; - public readonly int ContactCount => 3; + public static int BodyCount => 2; + public static int ContactCount => 3; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3Wide GetNormal(ref Contact3PrestepData prestep) + public static ref Vector3Wide GetNormal(ref Contact3PrestepData prestep) { return ref prestep.Normal; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref ConvexContactWide GetContact(ref Contact3PrestepData prestep, int index) + public static ref ConvexContactWide GetContact(ref Contact3PrestepData prestep, int index) { return ref Unsafe.Add(ref prestep.Contact0, index); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref MaterialPropertiesWide GetMaterialProperties(ref Contact3PrestepData prestep) + public static ref MaterialPropertiesWide GetMaterialProperties(ref Contact3PrestepData prestep) { return ref prestep.MaterialProperties; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3Wide GetOffsetB(ref Contact3PrestepData prestep) + public static ref Vector3Wide GetOffsetB(ref Contact3PrestepData prestep) { return ref prestep.OffsetB; } @@ -1276,10 +1276,10 @@ public ref Vector3Wide GetOffsetB(ref Contact3PrestepData prestep) public struct Contact3Functions : ITwoBodyConstraintFunctions { - public bool RequiresIncrementalSubstepUpdates => true; + public static bool RequiresIncrementalSubstepUpdates => true; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide velocityA, in BodyVelocityWide velocityB, ref Contact3PrestepData prestep) + public static void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide velocityA, in BodyVelocityWide velocityB, ref Contact3PrestepData prestep) { PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact0.Depth); PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact1.Depth); @@ -1287,7 +1287,7 @@ public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWi } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref Contact3PrestepData prestep, ref Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref Contact3PrestepData prestep, ref Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, out var offsetToManifoldCenterA); @@ -1300,7 +1300,7 @@ public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref Contact3PrestepData prestep, ref Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref Contact3PrestepData prestep, ref Contact3AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { //Note that we solve the penetration constraints before the friction constraints. //This makes the friction constraints more authoritative, since they happen last. @@ -1368,7 +1368,7 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int GetFirst(ref target.MaterialProperties.MaximumRecoveryVelocity) = MaximumRecoveryVelocity; } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out Contact4 description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out Contact4 description) { Debug.Assert(batch.TypeId == ConstraintTypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); @@ -1399,19 +1399,19 @@ public void CopyManifoldWideProperties(ref Vector3 offsetB, ref Vector3 normal, } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref ConstraintContactData GetFirstContact(ref Contact4 description) + public static ref ConstraintContactData GetFirstContact(ref Contact4 description) { return ref description.Contact0; } - public readonly int ConstraintTypeId + public static int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get => Contact4TypeProcessor.BatchTypeId; } - public readonly Type TypeProcessorType => typeof(Contact4TypeProcessor); - public readonly TypeProcessor CreateTypeProcessor() => new Contact4TypeProcessor(); + public static Type TypeProcessorType => typeof(Contact4TypeProcessor); + public static TypeProcessor CreateTypeProcessor() => new Contact4TypeProcessor(); } @@ -1429,29 +1429,29 @@ public struct Contact4PrestepData : ITwoBodyConvexContactPrestep 2; - public readonly int ContactCount => 4; + public static int BodyCount => 2; + public static int ContactCount => 4; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3Wide GetNormal(ref Contact4PrestepData prestep) + public static ref Vector3Wide GetNormal(ref Contact4PrestepData prestep) { return ref prestep.Normal; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref ConvexContactWide GetContact(ref Contact4PrestepData prestep, int index) + public static ref ConvexContactWide GetContact(ref Contact4PrestepData prestep, int index) { return ref Unsafe.Add(ref prestep.Contact0, index); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref MaterialPropertiesWide GetMaterialProperties(ref Contact4PrestepData prestep) + public static ref MaterialPropertiesWide GetMaterialProperties(ref Contact4PrestepData prestep) { return ref prestep.MaterialProperties; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3Wide GetOffsetB(ref Contact4PrestepData prestep) + public static ref Vector3Wide GetOffsetB(ref Contact4PrestepData prestep) { return ref prestep.OffsetB; } @@ -1460,10 +1460,10 @@ public ref Vector3Wide GetOffsetB(ref Contact4PrestepData prestep) public struct Contact4Functions : ITwoBodyConstraintFunctions { - public bool RequiresIncrementalSubstepUpdates => true; + public static bool RequiresIncrementalSubstepUpdates => true; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide velocityA, in BodyVelocityWide velocityB, ref Contact4PrestepData prestep) + public static void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide velocityA, in BodyVelocityWide velocityB, ref Contact4PrestepData prestep) { PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact0.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact0.Depth); PenetrationLimit.UpdatePenetrationDepth(dt, prestep.Contact1.OffsetA, prestep.OffsetB, prestep.Normal, velocityA, velocityB, ref prestep.Contact1.Depth); @@ -1472,7 +1472,7 @@ public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWi } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref Contact4PrestepData prestep, ref Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref Contact4PrestepData prestep, ref Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact3.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, prestep.Contact3.Depth, out var offsetToManifoldCenterA); @@ -1486,7 +1486,7 @@ public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref Contact4PrestepData prestep, ref Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref Contact4PrestepData prestep, ref Contact4AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { //Note that we solve the penetration constraints before the friction constraints. //This makes the friction constraints more authoritative, since they happen last. diff --git a/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt b/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt index 368317574..243e2215d 100644 --- a/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt +++ b/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt @@ -24,23 +24,23 @@ namespace BepuPhysics.Constraints.Contact public Vector Twist; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector2Wide GetTangentFriction(ref Contact<#= contactCount #>AccumulatedImpulses impulses) + public static ref Vector2Wide GetTangentFriction(ref Contact<#= contactCount #>AccumulatedImpulses impulses) { return ref impulses.Tangent; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector GetTwistFriction(ref Contact<#= contactCount #>AccumulatedImpulses impulses) + public static ref Vector GetTwistFriction(ref Contact<#= contactCount #>AccumulatedImpulses impulses) { return ref impulses.Twist; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector GetPenetrationImpulseForContact(ref Contact<#= contactCount #>AccumulatedImpulses impulses, int index) + public static ref Vector GetPenetrationImpulseForContact(ref Contact<#= contactCount #>AccumulatedImpulses impulses, int index) { Debug.Assert(index >= 0 && index < <#=contactCount#>); return ref Unsafe.Add(ref impulses.Penetration0, index); } - public int ContactCount => <#=contactCount#>; + public static int ContactCount => <#=contactCount#>; } <#}#> @@ -125,7 +125,7 @@ for (int i = 0; i < contactCount; ++i) GetFirst(ref target.MaterialProperties.MaximumRecoveryVelocity) = MaximumRecoveryVelocity; } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out Contact<#= contactCount #><#=suffix#> description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out Contact<#= contactCount #><#=suffix#> description) { Debug.Assert(batch.TypeId == ConstraintTypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer<#=suffix#>PrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex); @@ -155,19 +155,19 @@ for (int i = 0; i < contactCount; ++i) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref ConstraintContactData GetFirstContact(ref Contact<#= contactCount #><#=suffix#> description) + public static ref ConstraintContactData GetFirstContact(ref Contact<#= contactCount #><#=suffix#> description) { return ref description.Contact0; } - public readonly int ConstraintTypeId + public static int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get => Contact<#= contactCount #><#=suffix#>TypeProcessor.BatchTypeId; } - public readonly Type TypeProcessorType => typeof(Contact<#= contactCount #><#=suffix#>TypeProcessor); - public readonly TypeProcessor CreateTypeProcessor() => new Contact<#= contactCount #><#=suffix#>TypeProcessor(); + public static Type TypeProcessorType => typeof(Contact<#= contactCount #><#=suffix#>TypeProcessor); + public static TypeProcessor CreateTypeProcessor() => new Contact<#= contactCount #><#=suffix#>TypeProcessor(); } @@ -186,30 +186,30 @@ for (int i = 0; i < contactCount; ++i) public Vector3Wide Normal; public MaterialPropertiesWide MaterialProperties; - public readonly int BodyCount => <#=bodyCount#>; - public readonly int ContactCount => <#=contactCount#>; + public static int BodyCount => <#=bodyCount#>; + public static int ContactCount => <#=contactCount#>; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3Wide GetNormal(ref Contact<#= contactCount #><#=suffix#>PrestepData prestep) + public static ref Vector3Wide GetNormal(ref Contact<#= contactCount #><#=suffix#>PrestepData prestep) { return ref prestep.Normal; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref ConvexContactWide GetContact(ref Contact<#= contactCount #><#=suffix#>PrestepData prestep, int index) + public static ref ConvexContactWide GetContact(ref Contact<#= contactCount #><#=suffix#>PrestepData prestep, int index) { return ref Unsafe.Add(ref prestep.Contact0, index); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref MaterialPropertiesWide GetMaterialProperties(ref Contact<#= contactCount #><#=suffix#>PrestepData prestep) + public static ref MaterialPropertiesWide GetMaterialProperties(ref Contact<#= contactCount #><#=suffix#>PrestepData prestep) { return ref prestep.MaterialProperties; } <#if (bodyCount == 2){#> [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3Wide GetOffsetB(ref Contact<#= contactCount #><#=suffix#>PrestepData prestep) + public static ref Vector3Wide GetOffsetB(ref Contact<#= contactCount #><#=suffix#>PrestepData prestep) { return ref prestep.OffsetB; } @@ -219,10 +219,10 @@ for (int i = 0; i < contactCount; ++i) public struct Contact<#=contactCount#><#=suffix#>Functions : I<#=bodyCount == 1 ? "OneBody" : "TwoBody"#>ConstraintFunctions<#=suffix#>PrestepData, Contact<#=contactCount#>AccumulatedImpulses> { - public bool RequiresIncrementalSubstepUpdates => true; + public static bool RequiresIncrementalSubstepUpdates => true; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide velocityA,<#if(bodyCount == 2) {#> in BodyVelocityWide velocityB,<#}#> ref Contact<#=contactCount#><#=suffix#>PrestepData prestep) + public static void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide velocityA,<#if(bodyCount == 2) {#> in BodyVelocityWide velocityB,<#}#> ref Contact<#=contactCount#><#=suffix#>PrestepData prestep) { <#for (int i = 0; i < contactCount; ++i) {#> PenetrationLimit<#=suffix#>.UpdatePenetrationDepth(dt, prestep.Contact<#=i#>.OffsetA, <#if(bodyCount == 2) {#>prestep.OffsetB, <#}#>prestep.Normal, velocityA, <#if (bodyCount == 2) {#>velocityB, <#}#>ref prestep.Contact<#=i#>.Depth); @@ -230,7 +230,7 @@ for (int i = 0; i < contactCount; ++i) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(bodyCount == 2) {#>in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>ref Contact<#=contactCount#><#=suffix#>PrestepData prestep, ref Contact<#=contactCount#>AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA<#if(bodyCount == 2) {#>, ref BodyVelocityWide wsvB<#}#>) + public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(bodyCount == 2) {#>in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>ref Contact<#=contactCount#><#=suffix#>PrestepData prestep, ref Contact<#=contactCount#>AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA<#if(bodyCount == 2) {#>, ref BodyVelocityWide wsvB<#}#>) { Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); <#if (contactCount > 1) {#> @@ -249,7 +249,7 @@ for (int i = 0; i < contactCount; ++i) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(bodyCount == 2) {#>in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>float dt, float inverseDt, ref Contact<#=contactCount#><#=suffix#>PrestepData prestep, ref Contact<#=contactCount#>AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA<#if(bodyCount == 2) {#>, ref BodyVelocityWide wsvB<#}#>) + public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(bodyCount == 2) {#>in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>float dt, float inverseDt, ref Contact<#=contactCount#><#=suffix#>PrestepData prestep, ref Contact<#=contactCount#>AccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA<#if(bodyCount == 2) {#>, ref BodyVelocityWide wsvB<#}#>) { //Note that we solve the penetration constraints before the friction constraints. //This makes the friction constraints more authoritative, since they happen last. diff --git a/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs b/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs index fdbc7b6e7..1387be063 100644 --- a/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs +++ b/BepuPhysics/Constraints/Contact/ContactNonconvexCommon.cs @@ -17,19 +17,19 @@ public struct NonconvexContactPrestepData public interface INonconvexContactPrestep : IContactPrestep where TPrestep : struct, INonconvexContactPrestep { - ref NonconvexContactPrestepData GetContact(ref TPrestep prestep, int index); + static abstract ref NonconvexContactPrestepData GetContact(ref TPrestep prestep, int index); } public interface ITwoBodyNonconvexContactPrestep : INonconvexContactPrestep where TPrestep : struct, ITwoBodyNonconvexContactPrestep { - ref Vector3Wide GetOffsetB(ref TPrestep prestep); + static abstract ref Vector3Wide GetOffsetB(ref TPrestep prestep); } public interface INonconvexContactAccumulatedImpulses : IContactAccumulatedImpulses where TAccumulatedImpulses : struct, INonconvexContactAccumulatedImpulses { - ref NonconvexAccumulatedImpulses GetImpulsesForContact(ref TAccumulatedImpulses impulses, int index); + static abstract ref NonconvexAccumulatedImpulses GetImpulsesForContact(ref TAccumulatedImpulses impulses, int index); } @@ -59,24 +59,24 @@ public static void ApplyTwoBodyDescription(ref TDescript where TPrestep : unmanaged, ITwoBodyNonconvexContactPrestep where TDescription : unmanaged, INonconvexTwoBodyContactConstraintDescription { - Debug.Assert(batch.TypeId == description.ConstraintTypeId, "The type batch passed to the description must match the description's expected type."); + Debug.Assert(batch.TypeId == TDescription.ConstraintTypeId, "The type batch passed to the description must match the description's expected type."); ref var target = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); - ref var sourceCommon = ref description.GetCommonProperties(ref description); - ref var targetOffsetB = ref target.GetOffsetB(ref target); + ref var sourceCommon = ref TDescription.GetCommonProperties(ref description); + ref var targetOffsetB = ref TPrestep.GetOffsetB(ref target); GetFirst(ref targetOffsetB.X) = sourceCommon.OffsetB.X; GetFirst(ref targetOffsetB.Y) = sourceCommon.OffsetB.Y; GetFirst(ref targetOffsetB.Z) = sourceCommon.OffsetB.Z; - ref var targetMaterial = ref target.GetMaterialProperties(ref target); + ref var targetMaterial = ref TPrestep.GetMaterialProperties(ref target); GetFirst(ref targetMaterial.FrictionCoefficient) = sourceCommon.FrictionCoefficient; GetFirst(ref targetMaterial.SpringSettings.AngularFrequency) = sourceCommon.SpringSettings.AngularFrequency; GetFirst(ref targetMaterial.SpringSettings.TwiceDampingRatio) = sourceCommon.SpringSettings.TwiceDampingRatio; GetFirst(ref targetMaterial.MaximumRecoveryVelocity) = sourceCommon.MaximumRecoveryVelocity; - ref var sourceContacts = ref description.GetFirstContact(ref description); - ref var targetContacts = ref target.GetContact(ref target, 0); - CopyContactData(description.ContactCount, ref sourceContacts, ref targetContacts); + ref var sourceContacts = ref TDescription.GetFirstContact(ref description); + ref var targetContacts = ref TPrestep.GetContact(ref target, 0); + CopyContactData(TPrestep.ContactCount, ref sourceContacts, ref targetContacts); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -84,19 +84,19 @@ public static void ApplyOneBodyDescription(ref TDescript where TPrestep : unmanaged, INonconvexContactPrestep where TDescription : unmanaged, INonconvexOneBodyContactConstraintDescription { - Debug.Assert(batch.TypeId == description.ConstraintTypeId, "The type batch passed to the description must match the description's expected type."); + Debug.Assert(batch.TypeId == TDescription.ConstraintTypeId, "The type batch passed to the description must match the description's expected type."); ref var target = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); - ref var sourceCommon = ref description.GetCommonProperties(ref description); - ref var materialCommon = ref target.GetMaterialProperties(ref target); + ref var sourceCommon = ref TDescription.GetCommonProperties(ref description); + ref var materialCommon = ref TPrestep.GetMaterialProperties(ref target); GetFirst(ref materialCommon.FrictionCoefficient) = sourceCommon.FrictionCoefficient; GetFirst(ref materialCommon.SpringSettings.AngularFrequency) = sourceCommon.SpringSettings.AngularFrequency; GetFirst(ref materialCommon.SpringSettings.TwiceDampingRatio) = sourceCommon.SpringSettings.TwiceDampingRatio; GetFirst(ref materialCommon.MaximumRecoveryVelocity) = sourceCommon.MaximumRecoveryVelocity; - ref var sourceContacts = ref description.GetFirstContact(ref description); - ref var targetContacts = ref target.GetContact(ref target, 0); - CopyContactData(description.ContactCount, ref sourceContacts, ref targetContacts); + ref var sourceContacts = ref TDescription.GetFirstContact(ref description); + ref var targetContacts = ref TPrestep.GetContact(ref target, 0); + CopyContactData(TPrestep.ContactCount, ref sourceContacts, ref targetContacts); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -117,11 +117,11 @@ public static void BuildTwoBodyDescription(ref TypeBatch where TPrestep : unmanaged, ITwoBodyNonconvexContactPrestep where TDescription : unmanaged, INonconvexTwoBodyContactConstraintDescription { - Debug.Assert(batch.TypeId == default(TDescription).ConstraintTypeId, "The type batch passed to the description must match the description's expected type."); + Debug.Assert(batch.TypeId == TDescription.ConstraintTypeId, "The type batch passed to the description must match the description's expected type."); ref var prestep = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); - Vector3Wide.ReadFirst(prestep.GetOffsetB(ref prestep), out var offsetB); - ref var materialSource = ref prestep.GetMaterialProperties(ref prestep); + Vector3Wide.ReadFirst(TPrestep.GetOffsetB(ref prestep), out var offsetB); + ref var materialSource = ref TPrestep.GetMaterialProperties(ref prestep); PairMaterialProperties material; material.FrictionCoefficient = GetFirst(ref materialSource.FrictionCoefficient); material.SpringSettings.AngularFrequency = GetFirst(ref materialSource.SpringSettings.AngularFrequency); @@ -132,9 +132,9 @@ public static void BuildTwoBodyDescription(ref TypeBatch description = default; description.CopyManifoldWideProperties(ref offsetB, ref material); - ref var descriptionContacts = ref description.GetFirstContact(ref description); - ref var prestepContacts = ref prestep.GetContact(ref prestep, 0); - CopyContactData(description.ContactCount, ref prestepContacts, ref descriptionContacts); + ref var descriptionContacts = ref TDescription.GetFirstContact(ref description); + ref var prestepContacts = ref TPrestep.GetContact(ref prestep, 0); + CopyContactData(TPrestep.ContactCount, ref prestepContacts, ref descriptionContacts); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -142,10 +142,10 @@ public static void BuildOneBodyDescription(ref TypeBatch where TPrestep : unmanaged, INonconvexContactPrestep where TDescription : unmanaged, INonconvexOneBodyContactConstraintDescription { - Debug.Assert(batch.TypeId == default(TDescription).ConstraintTypeId, "The type batch passed to the description must match the description's expected type."); + Debug.Assert(batch.TypeId == TDescription.ConstraintTypeId, "The type batch passed to the description must match the description's expected type."); ref var prestep = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); - ref var materialSource = ref prestep.GetMaterialProperties(ref prestep); + ref var materialSource = ref TPrestep.GetMaterialProperties(ref prestep); PairMaterialProperties material; material.FrictionCoefficient = GetFirst(ref materialSource.FrictionCoefficient); material.SpringSettings.AngularFrequency = GetFirst(ref materialSource.SpringSettings.AngularFrequency); @@ -156,9 +156,9 @@ public static void BuildOneBodyDescription(ref TypeBatch description = default; description.CopyManifoldWideProperties(ref material); - ref var descriptionContacts = ref description.GetFirstContact(ref description); - ref var prestepContacts = ref prestep.GetContact(ref prestep, 0); - CopyContactData(description.ContactCount, ref prestepContacts, ref descriptionContacts); + ref var descriptionContacts = ref TDescription.GetFirstContact(ref description); + ref var prestepContacts = ref TPrestep.GetContact(ref prestep, 0); + CopyContactData(TPrestep.ContactCount, ref prestepContacts, ref descriptionContacts); } } @@ -176,20 +176,20 @@ public struct ContactNonconvexOneBodyFunctions : [MethodImpl(MethodImplOptions.AggressiveInlining)] public void IncrementallyUpdateContactData(in Vector dt, in BodyVelocityWide velocity, ref TPrestep prestep) { - ref var prestepContactStart = ref prestep.GetContact(ref prestep, 0); - for (int i = 0; i < prestep.ContactCount; ++i) + ref var prestepContactStart = ref TPrestep.GetContact(ref prestep, 0); + for (int i = 0; i < TPrestep.ContactCount; ++i) { ref var prestepContact = ref Unsafe.Add(ref prestepContactStart, i); PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestepContact.Offset, prestepContact.Normal, velocity, ref prestepContact.Depth); } } - public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref TPrestep prestep, ref TAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) + public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref TPrestep prestep, ref TAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { - ref var prestepMaterial = ref prestep.GetMaterialProperties(ref prestep); - ref var prestepContactStart = ref prestep.GetContact(ref prestep, 0); + ref var prestepMaterial = ref TPrestep.GetMaterialProperties(ref prestep); + ref var prestepContactStart = ref TPrestep.GetContact(ref prestep, 0); ref var accumulatedImpulsesStart = ref Unsafe.As(ref accumulatedImpulses); - for (int i = 0; i < prestep.ContactCount; ++i) + for (int i = 0; i < TPrestep.ContactCount; ++i) { ref var prestepContact = ref Unsafe.Add(ref prestepContactStart, i); Helpers.BuildOrthonormalBasis(prestepContact.Normal, out var x, out var z); @@ -199,16 +199,16 @@ public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, } } - public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref TPrestep prestep, ref TAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) + public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref TPrestep prestep, ref TAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA) { //Note that, unlike convex manifolds, we simply solve every contact in sequence rather than tangent->penetration. //This is not for any principled reason- only simplicity. May want to reconsider later, but remember the significant change in access pattern. - ref var prestepMaterial = ref prestep.GetMaterialProperties(ref prestep); + ref var prestepMaterial = ref TPrestep.GetMaterialProperties(ref prestep); ref var accumulatedImpulsesStart = ref Unsafe.As(ref accumulatedImpulses); - ref var prestepContactStart = ref prestep.GetContact(ref prestep, 0); + ref var prestepContactStart = ref TPrestep.GetContact(ref prestep, 0); SpringSettingsWide.ComputeSpringiness(prestepMaterial.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); var inverseDtWide = new Vector(inverseDt); - for (int i = 0; i < prestep.ContactCount; ++i) + for (int i = 0; i < TPrestep.ContactCount; ++i) { ref var contact = ref Unsafe.Add(ref prestepContactStart, i); ref var contactImpulse = ref Unsafe.Add(ref accumulatedImpulsesStart, i); @@ -228,11 +228,11 @@ public void UpdateForNewPose( throw new System.NotImplementedException(); } - public bool RequiresIncrementalSubstepUpdates => true; - public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, ref TPrestep prestep) + public static bool RequiresIncrementalSubstepUpdates => true; + public static void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, ref TPrestep prestep) { - ref var prestepContactStart = ref prestep.GetContact(ref prestep, 0); - for (int i = 0; i < prestep.ContactCount; ++i) + ref var prestepContactStart = ref TPrestep.GetContact(ref prestep, 0); + for (int i = 0; i < TPrestep.ContactCount; ++i) { ref var prestepContact = ref Unsafe.Add(ref prestepContactStart, i); PenetrationLimitOneBody.UpdatePenetrationDepth(dt, prestepContact.Offset, prestepContact.Normal, wsvA, ref prestepContact.Depth); @@ -245,13 +245,13 @@ public struct ContactNonconvexTwoBodyFunctions : where TPrestep : struct, ITwoBodyNonconvexContactPrestep where TAccumulatedImpulses : struct { - public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref TPrestep prestep, ref TAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref TPrestep prestep, ref TAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { - ref var prestepMaterial = ref prestep.GetMaterialProperties(ref prestep); - ref var prestepOffsetB = ref prestep.GetOffsetB(ref prestep); - ref var prestepContactStart = ref prestep.GetContact(ref prestep, 0); + ref var prestepMaterial = ref TPrestep.GetMaterialProperties(ref prestep); + ref var prestepOffsetB = ref TPrestep.GetOffsetB(ref prestep); + ref var prestepContactStart = ref TPrestep.GetContact(ref prestep, 0); ref var accumulatedImpulsesStart = ref Unsafe.As(ref accumulatedImpulses); - for (int i = 0; i < prestep.ContactCount; ++i) + for (int i = 0; i < TPrestep.ContactCount; ++i) { ref var prestepContact = ref Unsafe.Add(ref prestepContactStart, i); Helpers.BuildOrthonormalBasis(prestepContact.Normal, out var x, out var z); @@ -262,17 +262,17 @@ public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, } } - public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref TPrestep prestep, ref TAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref TPrestep prestep, ref TAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { //Note that, unlike convex manifolds, we simply solve every contact in sequence rather than tangent->penetration. //This is not for any principled reason- only simplicity. May want to reconsider later, but remember the significant change in access pattern. - ref var prestepOffsetB = ref prestep.GetOffsetB(ref prestep); - ref var prestepMaterial = ref prestep.GetMaterialProperties(ref prestep); + ref var prestepOffsetB = ref TPrestep.GetOffsetB(ref prestep); + ref var prestepMaterial = ref TPrestep.GetMaterialProperties(ref prestep); ref var accumulatedImpulsesStart = ref Unsafe.As(ref accumulatedImpulses); - ref var prestepContactStart = ref prestep.GetContact(ref prestep, 0); + ref var prestepContactStart = ref TPrestep.GetContact(ref prestep, 0); SpringSettingsWide.ComputeSpringiness(prestepMaterial.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); var inverseDtWide = new Vector(inverseDt); - for (int i = 0; i < prestep.ContactCount; ++i) + for (int i = 0; i < TPrestep.ContactCount; ++i) { ref var contact = ref Unsafe.Add(ref prestepContactStart, i); ref var contactImpulse = ref Unsafe.Add(ref accumulatedImpulsesStart, i); @@ -286,12 +286,12 @@ public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in B } - public bool RequiresIncrementalSubstepUpdates => true; - public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref TPrestep prestep) + public static bool RequiresIncrementalSubstepUpdates => true; + public static void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref TPrestep prestep) { - ref var prestepOffsetB = ref prestep.GetOffsetB(ref prestep); - ref var prestepContactStart = ref prestep.GetContact(ref prestep, 0); - for (int i = 0; i < prestep.ContactCount; ++i) + ref var prestepOffsetB = ref TPrestep.GetOffsetB(ref prestep); + ref var prestepContactStart = ref TPrestep.GetContact(ref prestep, 0); + for (int i = 0; i < TPrestep.ContactCount; ++i) { ref var prestepContact = ref Unsafe.Add(ref prestepContactStart, i); PenetrationLimit.UpdatePenetrationDepth(dt, prestepContact.Offset, prestepOffsetB, prestepContact.Normal, wsvA, wsvB, ref prestepContact.Depth); diff --git a/BepuPhysics/Constraints/Contact/ContactNonconvexTypes.cs b/BepuPhysics/Constraints/Contact/ContactNonconvexTypes.cs index 1e9a435fa..3841a0d47 100644 --- a/BepuPhysics/Constraints/Contact/ContactNonconvexTypes.cs +++ b/BepuPhysics/Constraints/Contact/ContactNonconvexTypes.cs @@ -16,7 +16,7 @@ public void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerInde NonconvexConstraintHelpers.ApplyTwoBodyDescription(ref this, ref batch, bundleIndex, innerIndex); } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out Contact2Nonconvex description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out Contact2Nonconvex description) { NonconvexConstraintHelpers.BuildTwoBodyDescription(ref batch, bundleIndex, innerIndex, out description); } @@ -31,27 +31,27 @@ public void CopyManifoldWideProperties(ref Vector3 offsetB, ref PairMaterialProp } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref NonconvexTwoBodyManifoldConstraintProperties GetCommonProperties(ref Contact2Nonconvex description) + public static ref NonconvexTwoBodyManifoldConstraintProperties GetCommonProperties(ref Contact2Nonconvex description) { return ref description.Common; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref NonconvexConstraintContactData GetFirstContact(ref Contact2Nonconvex description) + public static ref NonconvexConstraintContactData GetFirstContact(ref Contact2Nonconvex description) { return ref description.Contact0; } - public readonly int ContactCount => 2; + public static int ContactCount => 2; - public readonly int ConstraintTypeId + public static int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get => Contact2NonconvexTypeProcessor.BatchTypeId; } - public readonly Type TypeProcessorType => typeof(Contact2NonconvexTypeProcessor); - public readonly TypeProcessor CreateTypeProcessor() => new Contact2NonconvexTypeProcessor(); + public static Type TypeProcessorType => typeof(Contact2NonconvexTypeProcessor); + public static TypeProcessor CreateTypeProcessor() => new Contact2NonconvexTypeProcessor(); } @@ -64,35 +64,35 @@ public struct Contact2NonconvexPrestepData : ITwoBodyNonconvexContactPrestep 2; - public readonly int BodyCount => 2; + public static int ContactCount => 2; + public static int BodyCount => 2; } public struct Contact2NonconvexAccumulatedImpulses : INonconvexContactAccumulatedImpulses { public NonconvexAccumulatedImpulses Contact0; public NonconvexAccumulatedImpulses Contact1; - public readonly int ContactCount => 2; + public static int ContactCount => 2; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref NonconvexAccumulatedImpulses GetImpulsesForContact(ref Contact2NonconvexAccumulatedImpulses impulses, int index) + public static ref NonconvexAccumulatedImpulses GetImpulsesForContact(ref Contact2NonconvexAccumulatedImpulses impulses, int index) { return ref Unsafe.Add(ref impulses.Contact0, index); } @@ -120,7 +120,7 @@ public void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerInde NonconvexConstraintHelpers.ApplyOneBodyDescription(ref this, ref batch, bundleIndex, innerIndex); } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out Contact2NonconvexOneBody description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out Contact2NonconvexOneBody description) { NonconvexConstraintHelpers.BuildOneBodyDescription(ref batch, bundleIndex, innerIndex, out description); } @@ -134,27 +134,27 @@ public void CopyManifoldWideProperties(ref PairMaterialProperties material) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref NonconvexOneBodyManifoldConstraintProperties GetCommonProperties(ref Contact2NonconvexOneBody description) + public static ref NonconvexOneBodyManifoldConstraintProperties GetCommonProperties(ref Contact2NonconvexOneBody description) { return ref description.Common; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref NonconvexConstraintContactData GetFirstContact(ref Contact2NonconvexOneBody description) + public static ref NonconvexConstraintContactData GetFirstContact(ref Contact2NonconvexOneBody description) { return ref description.Contact0; } - public readonly int ContactCount => 2; + public static int ContactCount => 2; - public readonly int ConstraintTypeId + public static int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get => Contact2NonconvexOneBodyTypeProcessor.BatchTypeId; } - public readonly Type TypeProcessorType => typeof(Contact2NonconvexOneBodyTypeProcessor); - public readonly TypeProcessor CreateTypeProcessor() => new Contact2NonconvexOneBodyTypeProcessor(); + public static Type TypeProcessorType => typeof(Contact2NonconvexOneBodyTypeProcessor); + public static TypeProcessor CreateTypeProcessor() => new Contact2NonconvexOneBodyTypeProcessor(); } @@ -166,19 +166,19 @@ public struct Contact2NonconvexOneBodyPrestepData : INonconvexContactPrestep 2; - public readonly int BodyCount => 1; + public static int ContactCount => 2; + public static int BodyCount => 1; } /// @@ -205,7 +205,7 @@ public void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerInde NonconvexConstraintHelpers.ApplyTwoBodyDescription(ref this, ref batch, bundleIndex, innerIndex); } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out Contact3Nonconvex description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out Contact3Nonconvex description) { NonconvexConstraintHelpers.BuildTwoBodyDescription(ref batch, bundleIndex, innerIndex, out description); } @@ -220,27 +220,27 @@ public void CopyManifoldWideProperties(ref Vector3 offsetB, ref PairMaterialProp } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref NonconvexTwoBodyManifoldConstraintProperties GetCommonProperties(ref Contact3Nonconvex description) + public static ref NonconvexTwoBodyManifoldConstraintProperties GetCommonProperties(ref Contact3Nonconvex description) { return ref description.Common; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref NonconvexConstraintContactData GetFirstContact(ref Contact3Nonconvex description) + public static ref NonconvexConstraintContactData GetFirstContact(ref Contact3Nonconvex description) { return ref description.Contact0; } - public readonly int ContactCount => 3; + public static int ContactCount => 3; - public readonly int ConstraintTypeId + public static int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get => Contact3NonconvexTypeProcessor.BatchTypeId; } - public readonly Type TypeProcessorType => typeof(Contact3NonconvexTypeProcessor); - public readonly TypeProcessor CreateTypeProcessor() => new Contact3NonconvexTypeProcessor(); + public static Type TypeProcessorType => typeof(Contact3NonconvexTypeProcessor); + public static TypeProcessor CreateTypeProcessor() => new Contact3NonconvexTypeProcessor(); } @@ -254,25 +254,25 @@ public struct Contact3NonconvexPrestepData : ITwoBodyNonconvexContactPrestep 3; - public readonly int BodyCount => 2; + public static int ContactCount => 3; + public static int BodyCount => 2; } public struct Contact3NonconvexAccumulatedImpulses : INonconvexContactAccumulatedImpulses @@ -280,10 +280,10 @@ public struct Contact3NonconvexAccumulatedImpulses : INonconvexContactAccumulate public NonconvexAccumulatedImpulses Contact0; public NonconvexAccumulatedImpulses Contact1; public NonconvexAccumulatedImpulses Contact2; - public readonly int ContactCount => 3; + public static int ContactCount => 3; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref NonconvexAccumulatedImpulses GetImpulsesForContact(ref Contact3NonconvexAccumulatedImpulses impulses, int index) + public static ref NonconvexAccumulatedImpulses GetImpulsesForContact(ref Contact3NonconvexAccumulatedImpulses impulses, int index) { return ref Unsafe.Add(ref impulses.Contact0, index); } @@ -312,7 +312,7 @@ public void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerInde NonconvexConstraintHelpers.ApplyOneBodyDescription(ref this, ref batch, bundleIndex, innerIndex); } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out Contact3NonconvexOneBody description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out Contact3NonconvexOneBody description) { NonconvexConstraintHelpers.BuildOneBodyDescription(ref batch, bundleIndex, innerIndex, out description); } @@ -326,27 +326,27 @@ public void CopyManifoldWideProperties(ref PairMaterialProperties material) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref NonconvexOneBodyManifoldConstraintProperties GetCommonProperties(ref Contact3NonconvexOneBody description) + public static ref NonconvexOneBodyManifoldConstraintProperties GetCommonProperties(ref Contact3NonconvexOneBody description) { return ref description.Common; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref NonconvexConstraintContactData GetFirstContact(ref Contact3NonconvexOneBody description) + public static ref NonconvexConstraintContactData GetFirstContact(ref Contact3NonconvexOneBody description) { return ref description.Contact0; } - public readonly int ContactCount => 3; + public static int ContactCount => 3; - public readonly int ConstraintTypeId + public static int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get => Contact3NonconvexOneBodyTypeProcessor.BatchTypeId; } - public readonly Type TypeProcessorType => typeof(Contact3NonconvexOneBodyTypeProcessor); - public readonly TypeProcessor CreateTypeProcessor() => new Contact3NonconvexOneBodyTypeProcessor(); + public static Type TypeProcessorType => typeof(Contact3NonconvexOneBodyTypeProcessor); + public static TypeProcessor CreateTypeProcessor() => new Contact3NonconvexOneBodyTypeProcessor(); } @@ -359,19 +359,19 @@ public struct Contact3NonconvexOneBodyPrestepData : INonconvexContactPrestep 3; - public readonly int BodyCount => 1; + public static int ContactCount => 3; + public static int BodyCount => 1; } /// @@ -399,7 +399,7 @@ public void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerInde NonconvexConstraintHelpers.ApplyTwoBodyDescription(ref this, ref batch, bundleIndex, innerIndex); } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out Contact4Nonconvex description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out Contact4Nonconvex description) { NonconvexConstraintHelpers.BuildTwoBodyDescription(ref batch, bundleIndex, innerIndex, out description); } @@ -414,27 +414,27 @@ public void CopyManifoldWideProperties(ref Vector3 offsetB, ref PairMaterialProp } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref NonconvexTwoBodyManifoldConstraintProperties GetCommonProperties(ref Contact4Nonconvex description) + public static ref NonconvexTwoBodyManifoldConstraintProperties GetCommonProperties(ref Contact4Nonconvex description) { return ref description.Common; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref NonconvexConstraintContactData GetFirstContact(ref Contact4Nonconvex description) + public static ref NonconvexConstraintContactData GetFirstContact(ref Contact4Nonconvex description) { return ref description.Contact0; } - public readonly int ContactCount => 4; + public static int ContactCount => 4; - public readonly int ConstraintTypeId + public static int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get => Contact4NonconvexTypeProcessor.BatchTypeId; } - public readonly Type TypeProcessorType => typeof(Contact4NonconvexTypeProcessor); - public readonly TypeProcessor CreateTypeProcessor() => new Contact4NonconvexTypeProcessor(); + public static Type TypeProcessorType => typeof(Contact4NonconvexTypeProcessor); + public static TypeProcessor CreateTypeProcessor() => new Contact4NonconvexTypeProcessor(); } @@ -449,25 +449,25 @@ public struct Contact4NonconvexPrestepData : ITwoBodyNonconvexContactPrestep 4; - public readonly int BodyCount => 2; + public static int ContactCount => 4; + public static int BodyCount => 2; } public struct Contact4NonconvexAccumulatedImpulses : INonconvexContactAccumulatedImpulses @@ -476,10 +476,10 @@ public struct Contact4NonconvexAccumulatedImpulses : INonconvexContactAccumulate public NonconvexAccumulatedImpulses Contact1; public NonconvexAccumulatedImpulses Contact2; public NonconvexAccumulatedImpulses Contact3; - public readonly int ContactCount => 4; + public static int ContactCount => 4; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref NonconvexAccumulatedImpulses GetImpulsesForContact(ref Contact4NonconvexAccumulatedImpulses impulses, int index) + public static ref NonconvexAccumulatedImpulses GetImpulsesForContact(ref Contact4NonconvexAccumulatedImpulses impulses, int index) { return ref Unsafe.Add(ref impulses.Contact0, index); } @@ -509,7 +509,7 @@ public void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerInde NonconvexConstraintHelpers.ApplyOneBodyDescription(ref this, ref batch, bundleIndex, innerIndex); } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out Contact4NonconvexOneBody description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out Contact4NonconvexOneBody description) { NonconvexConstraintHelpers.BuildOneBodyDescription(ref batch, bundleIndex, innerIndex, out description); } @@ -523,27 +523,27 @@ public void CopyManifoldWideProperties(ref PairMaterialProperties material) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref NonconvexOneBodyManifoldConstraintProperties GetCommonProperties(ref Contact4NonconvexOneBody description) + public static ref NonconvexOneBodyManifoldConstraintProperties GetCommonProperties(ref Contact4NonconvexOneBody description) { return ref description.Common; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref NonconvexConstraintContactData GetFirstContact(ref Contact4NonconvexOneBody description) + public static ref NonconvexConstraintContactData GetFirstContact(ref Contact4NonconvexOneBody description) { return ref description.Contact0; } - public readonly int ContactCount => 4; + public static int ContactCount => 4; - public readonly int ConstraintTypeId + public static int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get => Contact4NonconvexOneBodyTypeProcessor.BatchTypeId; } - public readonly Type TypeProcessorType => typeof(Contact4NonconvexOneBodyTypeProcessor); - public readonly TypeProcessor CreateTypeProcessor() => new Contact4NonconvexOneBodyTypeProcessor(); + public static Type TypeProcessorType => typeof(Contact4NonconvexOneBodyTypeProcessor); + public static TypeProcessor CreateTypeProcessor() => new Contact4NonconvexOneBodyTypeProcessor(); } @@ -557,19 +557,19 @@ public struct Contact4NonconvexOneBodyPrestepData : INonconvexContactPrestep 4; - public readonly int BodyCount => 1; + public static int ContactCount => 4; + public static int BodyCount => 1; } /// diff --git a/BepuPhysics/Constraints/Contact/ContactNonconvexTypes.tt b/BepuPhysics/Constraints/Contact/ContactNonconvexTypes.tt index ae3318b54..ae6522157 100644 --- a/BepuPhysics/Constraints/Contact/ContactNonconvexTypes.tt +++ b/BepuPhysics/Constraints/Contact/ContactNonconvexTypes.tt @@ -29,7 +29,7 @@ for (int i = 0; i < contactCount ; ++i) NonconvexConstraintHelpers.ApplyTwoBodyDescriptionNonconvex, Contact<#= contactCount #>NonconvexPrestepData>(ref this, ref batch, bundleIndex, innerIndex); } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out Contact<#= contactCount #>Nonconvex description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out Contact<#= contactCount #>Nonconvex description) { NonconvexConstraintHelpers.BuildTwoBodyDescriptionNonconvex, Contact<#= contactCount #>NonconvexPrestepData>(ref batch, bundleIndex, innerIndex, out description); } @@ -44,27 +44,27 @@ for (int i = 0; i < contactCount ; ++i) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref NonconvexTwoBodyManifoldConstraintProperties GetCommonProperties(ref Contact<#=contactCount#>Nonconvex description) + public static ref NonconvexTwoBodyManifoldConstraintProperties GetCommonProperties(ref Contact<#=contactCount#>Nonconvex description) { return ref description.Common; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref NonconvexConstraintContactData GetFirstContact(ref Contact<#= contactCount #>Nonconvex description) + public static ref NonconvexConstraintContactData GetFirstContact(ref Contact<#= contactCount #>Nonconvex description) { return ref description.Contact0; } - public readonly int ContactCount => <#= contactCount #>; + public static int ContactCount => <#= contactCount #>; - public readonly int ConstraintTypeId + public static int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get => Contact<#= contactCount #>NonconvexTypeProcessor.BatchTypeId; } - public readonly Type TypeProcessorType => typeof(Contact<#= contactCount #>NonconvexTypeProcessor); - public readonly TypeProcessor CreateTypeProcessor() => new Contact<#= contactCount #>NonconvexTypeProcessor(); + public static Type TypeProcessorType => typeof(Contact<#= contactCount #>NonconvexTypeProcessor); + public static TypeProcessor CreateTypeProcessor() => new Contact<#= contactCount #>NonconvexTypeProcessor(); } @@ -80,13 +80,13 @@ for (int i = 0; i < contactCount; ++i) <#}#> [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref MaterialPropertiesWide GetMaterialProperties(ref Contact<#= contactCount #>NonconvexPrestepData prestep) + public static ref MaterialPropertiesWide GetMaterialProperties(ref Contact<#= contactCount #>NonconvexPrestepData prestep) { return ref prestep.MaterialProperties; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3Wide GetOffsetB(ref Contact<#= contactCount #>NonconvexPrestepData prestep) + public static ref Vector3Wide GetOffsetB(ref Contact<#= contactCount #>NonconvexPrestepData prestep) { return ref prestep.OffsetB; } @@ -97,8 +97,8 @@ for (int i = 0; i < contactCount; ++i) return ref Unsafe.Add(ref prestep.Contact0, index); } - public readonly int ContactCount => <#= contactCount #>; - public readonly int BodyCount => 2; + public static int ContactCount => <#= contactCount #>; + public static int BodyCount => 2; } public struct Contact<#= contactCount #>NonconvexAccumulatedImpulses : INonconvexContactAccumulatedImpulsesNonconvexAccumulatedImpulses> @@ -108,7 +108,7 @@ for (int i = 0; i < contactCount ; ++i) {#> public NonconvexAccumulatedImpulses Contact<#=i#>; <#}#> - public readonly int ContactCount => <#=contactCount#>; + public static int ContactCount => <#=contactCount#>; [MethodImpl(MethodImplOptions.AggressiveInlining)] public ref NonconvexAccumulatedImpulses GetImpulsesForContact(ref Contact<#= contactCount #>NonconvexAccumulatedImpulses impulses, int index) @@ -142,7 +142,7 @@ for (int i = 0; i < contactCount ; ++i) NonconvexConstraintHelpers.ApplyOneBodyDescriptionNonconvexOneBody, Contact<#= contactCount #>NonconvexOneBodyPrestepData>(ref this, ref batch, bundleIndex, innerIndex); } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out Contact<#= contactCount #>NonconvexOneBody description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out Contact<#= contactCount #>NonconvexOneBody description) { NonconvexConstraintHelpers.BuildOneBodyDescriptionNonconvexOneBody, Contact<#= contactCount #>NonconvexOneBodyPrestepData>(ref batch, bundleIndex, innerIndex, out description); } @@ -156,27 +156,27 @@ for (int i = 0; i < contactCount ; ++i) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref NonconvexOneBodyManifoldConstraintProperties GetCommonProperties(ref Contact<#=contactCount#>NonconvexOneBody description) + public static ref NonconvexOneBodyManifoldConstraintProperties GetCommonProperties(ref Contact<#=contactCount#>NonconvexOneBody description) { return ref description.Common; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref NonconvexConstraintContactData GetFirstContact(ref Contact<#= contactCount #>NonconvexOneBody description) + public static ref NonconvexConstraintContactData GetFirstContact(ref Contact<#= contactCount #>NonconvexOneBody description) { return ref description.Contact0; } - public readonly int ContactCount => <#= contactCount #>; + public static int ContactCount => <#= contactCount #>; - public readonly int ConstraintTypeId + public static int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get => Contact<#= contactCount #>NonconvexOneBodyTypeProcessor.BatchTypeId; } - public readonly Type TypeProcessorType => typeof(Contact<#= contactCount #>NonconvexOneBodyTypeProcessor); - public readonly TypeProcessor CreateTypeProcessor() => new Contact<#= contactCount #>NonconvexOneBodyTypeProcessor(); + public static Type TypeProcessorType => typeof(Contact<#= contactCount #>NonconvexOneBodyTypeProcessor); + public static TypeProcessor CreateTypeProcessor() => new Contact<#= contactCount #>NonconvexOneBodyTypeProcessor(); } @@ -191,7 +191,7 @@ for (int i = 0; i < contactCount ; ++i) <#}#> [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref MaterialPropertiesWide GetMaterialProperties(ref Contact<#= contactCount #>NonconvexOneBodyPrestepData prestep) + public static ref MaterialPropertiesWide GetMaterialProperties(ref Contact<#= contactCount #>NonconvexOneBodyPrestepData prestep) { return ref prestep.MaterialProperties; } @@ -202,8 +202,8 @@ for (int i = 0; i < contactCount ; ++i) return ref Unsafe.Add(ref prestep.Contact0, index); } - public readonly int ContactCount => <#= contactCount #>; - public readonly int BodyCount => 1; + public static int ContactCount => <#= contactCount #>; + public static int BodyCount => 1; } /// diff --git a/BepuPhysics/Constraints/Contact/IContactConstraintDescription.cs b/BepuPhysics/Constraints/Contact/IContactConstraintDescription.cs index b1a90df04..d432e6ad3 100644 --- a/BepuPhysics/Constraints/Contact/IContactConstraintDescription.cs +++ b/BepuPhysics/Constraints/Contact/IContactConstraintDescription.cs @@ -15,13 +15,13 @@ public interface IConvexOneBodyContactConstraintDescription : IOne where TDescription : unmanaged, IConvexOneBodyContactConstraintDescription { void CopyManifoldWideProperties(ref Vector3 normal, ref PairMaterialProperties material); - ref ConstraintContactData GetFirstContact(ref TDescription description); + static abstract ref ConstraintContactData GetFirstContact(ref TDescription description); } public interface IConvexTwoBodyContactConstraintDescription : ITwoBodyConstraintDescription where TDescription : unmanaged, IConvexTwoBodyContactConstraintDescription { void CopyManifoldWideProperties(ref Vector3 offsetB, ref Vector3 normal, ref PairMaterialProperties material); - ref ConstraintContactData GetFirstContact(ref TDescription description); + static abstract ref ConstraintContactData GetFirstContact(ref TDescription description); } public struct NonconvexConstraintContactData @@ -49,19 +49,19 @@ public interface INonconvexOneBodyContactConstraintDescription : I where TDescription : unmanaged, INonconvexOneBodyContactConstraintDescription { void CopyManifoldWideProperties(ref PairMaterialProperties material); - int ContactCount { get; } + static abstract int ContactCount { get; } - ref NonconvexOneBodyManifoldConstraintProperties GetCommonProperties(ref TDescription description); - ref NonconvexConstraintContactData GetFirstContact(ref TDescription description); + static abstract ref NonconvexOneBodyManifoldConstraintProperties GetCommonProperties(ref TDescription description); + static abstract ref NonconvexConstraintContactData GetFirstContact(ref TDescription description); } public interface INonconvexTwoBodyContactConstraintDescription : ITwoBodyConstraintDescription where TDescription : unmanaged, INonconvexTwoBodyContactConstraintDescription { void CopyManifoldWideProperties(ref Vector3 offsetB, ref PairMaterialProperties material); - int ContactCount { get; } + static abstract int ContactCount { get; } - ref NonconvexTwoBodyManifoldConstraintProperties GetCommonProperties(ref TDescription description); - ref NonconvexConstraintContactData GetFirstContact(ref TDescription description); + static abstract ref NonconvexTwoBodyManifoldConstraintProperties GetCommonProperties(ref TDescription description); + static abstract ref NonconvexConstraintContactData GetFirstContact(ref TDescription description); } } diff --git a/BepuPhysics/Constraints/DistanceLimit.cs b/BepuPhysics/Constraints/DistanceLimit.cs index 937e4ed25..2963bd911 100644 --- a/BepuPhysics/Constraints/DistanceLimit.cs +++ b/BepuPhysics/Constraints/DistanceLimit.cs @@ -53,7 +53,7 @@ public DistanceLimit(Vector3 localOffsetA, Vector3 localOffsetB, float minimumDi SpringSettings = springSettings; } - public readonly int ConstraintTypeId + public static int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get @@ -62,8 +62,8 @@ public readonly int ConstraintTypeId } } - public readonly Type TypeProcessorType => typeof(DistanceLimitTypeProcessor); - public readonly TypeProcessor CreateTypeProcessor() => new DistanceLimitTypeProcessor(); + public static Type TypeProcessorType => typeof(DistanceLimitTypeProcessor); + public static TypeProcessor CreateTypeProcessor() => new DistanceLimitTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { @@ -80,7 +80,7 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int SpringSettingsWide.WriteFirst(SpringSettings, ref target.SpringSettings); } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out DistanceLimit description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out DistanceLimit description) { Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); @@ -116,7 +116,7 @@ public static void ApplyImpulse(in Vector3Wide linearJacobianA, in Vector3Wide a } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void ComputeJacobians( + public static void ComputeJacobians( in Vector3Wide localOffsetA, in Vector3Wide positionA, in QuaternionWide orientationA, in Vector3Wide localOffsetB, in Vector3Wide positionB, in QuaternionWide orientationB, in Vector minimumDistance, in Vector maximumDistance, out Vector useMinimum, out Vector distance, out Vector3Wide direction, out Vector3Wide angularJA, out Vector3Wide angularJB) { @@ -139,14 +139,14 @@ public void ComputeJacobians( } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref DistanceLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref DistanceLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeJacobians(prestep.LocalOffsetA, positionA, orientationA, prestep.LocalOffsetB, positionB, orientationB, prestep.MinimumDistance, prestep.MaximumDistance, out _, out _, out var direction, out var angularJA, out var angularJB); ApplyImpulse(direction, angularJA, angularJB, inertiaA, inertiaB, accumulatedImpulses, ref wsvA, ref wsvB); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref DistanceLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref DistanceLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeJacobians(prestep.LocalOffsetA, positionA, orientationA, prestep.LocalOffsetB, positionB, orientationB, prestep.MinimumDistance, prestep.MaximumDistance, out var useMinimum, out var distance, out var direction, out var angularJA, out var angularJB); @@ -172,9 +172,9 @@ public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in B ApplyImpulse(direction, angularJA, angularJB, inertiaA, inertiaB, csi, ref wsvA, ref wsvB); } - public bool RequiresIncrementalSubstepUpdates => false; + public static bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref DistanceLimitPrestepData prestepData) { } + public static void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref DistanceLimitPrestepData prestepData) { } } diff --git a/BepuPhysics/Constraints/DistanceServo.cs b/BepuPhysics/Constraints/DistanceServo.cs index 0295b683b..d4322db3f 100644 --- a/BepuPhysics/Constraints/DistanceServo.cs +++ b/BepuPhysics/Constraints/DistanceServo.cs @@ -59,7 +59,7 @@ public DistanceServo(Vector3 localOffsetA, Vector3 localOffsetB, float targetDis { } - public readonly int ConstraintTypeId + public static int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get @@ -68,8 +68,8 @@ public readonly int ConstraintTypeId } } - public readonly Type TypeProcessorType => typeof(DistanceServoTypeProcessor); - public readonly TypeProcessor CreateTypeProcessor() => new DistanceServoTypeProcessor(); + public static Type TypeProcessorType => typeof(DistanceServoTypeProcessor); + public static TypeProcessor CreateTypeProcessor() => new DistanceServoTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { @@ -84,7 +84,7 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int SpringSettingsWide.WriteFirst(SpringSettings, ref target.SpringSettings); } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out DistanceServo description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out DistanceServo description) { Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); @@ -183,7 +183,7 @@ public static void ApplyImpulse( } - public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref DistanceServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref DistanceServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { GetDistance(orientationA, positionB - positionA, orientationB, prestep.LocalOffsetA, prestep.LocalOffsetB, out var anchorOffsetA, out var anchorOffsetB, out var anchorOffset, out var distance); Vector3Wide.Scale(anchorOffset, Vector.One / distance, out var direction); @@ -193,7 +193,7 @@ public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, ApplyImpulse(inertiaA.InverseMass, inertiaB.InverseMass, direction, angularImpulseToVelocityA, angularImpulseToVelocityB, accumulatedImpulses, ref wsvA, ref wsvB); } - public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref DistanceServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref DistanceServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { GetDistance(orientationA, positionB - positionA, orientationB, prestep.LocalOffsetA, prestep.LocalOffsetB, out var anchorOffsetA, out var anchorOffsetB, out var anchorOffset, out var distance); @@ -217,9 +217,9 @@ public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in B ApplyImpulse(inertiaA.InverseMass, inertiaB.InverseMass, direction, angularImpulseToVelocityA, angularImpulseToVelocityB, csi, ref wsvA, ref wsvB); } - public bool RequiresIncrementalSubstepUpdates => false; + public static bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref DistanceServoPrestepData prestepData) { } + public static void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref DistanceServoPrestepData prestepData) { } } diff --git a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs index a1b9493f2..6f2b4d0af 100644 --- a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs @@ -26,13 +26,13 @@ public struct FourBodyReferences /// Type of the accumulated impulses used by the constraint. public interface IFourBodyConstraintFunctions { - void WarmStart( + static abstract void WarmStart( in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in Vector3Wide positionD, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, ref TPrestepData prestep, ref TAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC, ref BodyVelocityWide wsvD); - void Solve( + static abstract void Solve( in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, @@ -42,8 +42,8 @@ void Solve( /// /// Gets whether this constraint type requires incremental updates for each substep taken beyond the first. /// - bool RequiresIncrementalSubstepUpdates { get; } - void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, in BodyVelocityWide wsvC, in BodyVelocityWide wsvD, ref TPrestepData prestepData); + static abstract bool RequiresIncrementalSubstepUpdates { get; } + static abstract void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, in BodyVelocityWide wsvC, in BodyVelocityWide wsvD, ref TPrestepData prestepData); } /// @@ -68,7 +68,7 @@ public abstract class FourBodyTypeProcessor { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public int GetSortKey(int constraintIndex, ref Buffer bodyReferences) + public static int GetSortKey(int constraintIndex, ref Buffer bodyReferences) { BundleIndexing.GetBundleIndices(constraintIndex, out var bundleIndex, out var innerIndex); ref var bundleReferences = ref bodyReferences[bundleIndex]; @@ -104,15 +104,13 @@ internal sealed override void VerifySortRegion(ref TypeBatch typeBatch, int bund VerifySortRegion(ref typeBatch, bundleStartIndex, constraintCount, ref sortedKeys, ref sortedSourceIndices); } - public unsafe override void WarmStart( + public override void WarmStart( ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { var prestepBundles = typeBatch.PrestepData.As(); var bodyReferencesBundles = typeBatch.BodyReferences.As(); var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); - var function = default(TConstraintFunctions); - ref var states = ref bodies.ActiveSet.DynamicsState; for (int i = startBundle; i < exclusiveEndBundle; ++i) { ref var prestep = ref prestepBundles[i]; @@ -130,7 +128,7 @@ public unsafe override void WarmStart(dt), accumulatedImpulses, ref prestep); - function.WarmStart(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, positionD, orientationD, inertiaD, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC, ref wsvD); + TConstraintFunctions.WarmStart(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, positionD, orientationD, inertiaD, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC, ref wsvD); if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) { @@ -157,8 +155,6 @@ public unsafe override void Solve(ref TypeBatch typeBatch, Bodies bodies, float var prestepBundles = typeBatch.PrestepData.As(); var bodyReferencesBundles = typeBatch.BodyReferences.As(); var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); - var function = default(TConstraintFunctions); - ref var motionStates = ref bodies.ActiveSet.DynamicsState; for (int i = startBundle; i < exclusiveEndBundle; ++i) { ref var prestep = ref prestepBundles[i]; @@ -169,7 +165,7 @@ public unsafe override void Solve(ref TypeBatch typeBatch, Bodies bodies, float bodies.GatherState(references.IndexC, true, out var positionC, out var orientationC, out var wsvC, out var inertiaC); bodies.GatherState(references.IndexD, true, out var positionD, out var orientationD, out var wsvD, out var inertiaD); - function.Solve(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, positionD, orientationD, inertiaD, dt, inverseDt, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC, ref wsvD); + TConstraintFunctions.Solve(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, positionD, orientationD, inertiaD, dt, inverseDt, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC, ref wsvD); bodies.ScatterVelocities(ref wsvA, ref references.IndexA); bodies.ScatterVelocities(ref wsvB, ref references.IndexB); @@ -178,12 +174,11 @@ public unsafe override void Solve(ref TypeBatch typeBatch, Bodies bodies, float } } - public override bool RequiresIncrementalSubstepUpdates => default(TConstraintFunctions).RequiresIncrementalSubstepUpdates; + public override bool RequiresIncrementalSubstepUpdates => TConstraintFunctions.RequiresIncrementalSubstepUpdates; public unsafe override void IncrementallyUpdateForSubstep(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { var prestepBundles = typeBatch.PrestepData.As(); var bodyReferencesBundles = typeBatch.BodyReferences.As(); - var function = default(TConstraintFunctions); var dtWide = new Vector(dt); for (int i = startBundle; i < exclusiveEndBundle; ++i) { @@ -193,7 +188,7 @@ public unsafe override void IncrementallyUpdateForSubstep(ref TypeBatch typeBatc bodies.GatherState(references.IndexB, true, out _, out _, out var wsvB, out _); bodies.GatherState(references.IndexC, true, out _, out _, out var wsvC, out _); bodies.GatherState(references.IndexD, true, out _, out _, out var wsvD, out _); - function.IncrementallyUpdateForSubstep(dtWide, wsvA, wsvB, wsvC, wsvD, ref prestep); + TConstraintFunctions.IncrementallyUpdateForSubstep(dtWide, wsvA, wsvB, wsvC, wsvD, ref prestep); } } diff --git a/BepuPhysics/Constraints/Hinge.cs b/BepuPhysics/Constraints/Hinge.cs index ccae6c4df..46e9f092a 100644 --- a/BepuPhysics/Constraints/Hinge.cs +++ b/BepuPhysics/Constraints/Hinge.cs @@ -34,7 +34,7 @@ public struct Hinge : ITwoBodyConstraintDescription /// public SpringSettings SpringSettings; - public readonly int ConstraintTypeId + public static int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get @@ -43,8 +43,8 @@ public readonly int ConstraintTypeId } } - public readonly Type TypeProcessorType => typeof(HingeTypeProcessor); - public readonly TypeProcessor CreateTypeProcessor() => new HingeTypeProcessor(); + public static Type TypeProcessorType => typeof(HingeTypeProcessor); + public static TypeProcessor CreateTypeProcessor() => new HingeTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { @@ -60,7 +60,7 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int SpringSettingsWide.WriteFirst(SpringSettings, ref target.SpringSettings); } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out Hinge description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out Hinge description) { Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); @@ -114,7 +114,7 @@ private static void ApplyImpulse(in Vector3Wide offsetA, in Vector3Wide offsetB, Vector3Wide.Add(velocityB.Angular, angularChangeB, out velocityB.Angular); } - public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref HingePrestepData prestep, ref HingeAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref HingePrestepData prestep, ref HingeAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { Matrix3x3Wide.CreateFromQuaternion(orientationA, out var orientationMatrixA); Matrix3x3Wide.TransformWithoutOverlap(prestep.LocalOffsetA, orientationMatrixA, out var offsetA); @@ -126,7 +126,7 @@ public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, ApplyImpulse(offsetA, offsetB, hingeJacobian, inertiaA, inertiaB, accumulatedImpulses, ref wsvA, ref wsvB); } - public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref HingePrestepData prestep, ref HingeAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref HingePrestepData prestep, ref HingeAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { //5x12 jacobians, from BallSocket and AngularHinge: //[ I, skew(offsetA), -I, -skew(offsetB) ] @@ -217,9 +217,9 @@ public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in B ApplyImpulse(offsetA, offsetB, hingeJacobian, inertiaA, inertiaB, csi, ref wsvA, ref wsvB); } - public bool RequiresIncrementalSubstepUpdates => false; + public static bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref HingePrestepData prestepData) { } + public static void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref HingePrestepData prestepData) { } } public class HingeTypeProcessor : TwoBodyTypeProcessor diff --git a/BepuPhysics/Constraints/IConstraintDescription.cs b/BepuPhysics/Constraints/IConstraintDescription.cs index 1c1b24131..ca53c4d74 100644 --- a/BepuPhysics/Constraints/IConstraintDescription.cs +++ b/BepuPhysics/Constraints/IConstraintDescription.cs @@ -32,20 +32,20 @@ public interface IConstraintDescription /// Index of the source constraint's bundle. /// Index of the source constraint within its bundle. /// Description of the constraint. - void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out TDescription description); + static abstract void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out TDescription description); /// /// Gets the type id of the constraint that this is a description of. /// - int ConstraintTypeId { get; } + static abstract int ConstraintTypeId { get; } /// /// Gets the type of the type batch which contains described constraints. /// - Type TypeProcessorType { get; } + static abstract Type TypeProcessorType { get; } /// /// Creates a type processor for this constraint type. /// - TypeProcessor CreateTypeProcessor(); + static abstract TypeProcessor CreateTypeProcessor(); } /// diff --git a/BepuPhysics/Constraints/LinearAxisLimit.cs b/BepuPhysics/Constraints/LinearAxisLimit.cs index fb8254d02..c8ed35313 100644 --- a/BepuPhysics/Constraints/LinearAxisLimit.cs +++ b/BepuPhysics/Constraints/LinearAxisLimit.cs @@ -38,7 +38,7 @@ public struct LinearAxisLimit : ITwoBodyConstraintDescription /// public SpringSettings SpringSettings; - public readonly int ConstraintTypeId + public static int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get @@ -47,8 +47,8 @@ public readonly int ConstraintTypeId } } - public readonly Type TypeProcessorType => typeof(LinearAxisLimitTypeProcessor); - public readonly TypeProcessor CreateTypeProcessor() => new LinearAxisLimitTypeProcessor(); + public static Type TypeProcessorType => typeof(LinearAxisLimitTypeProcessor); + public static TypeProcessor CreateTypeProcessor() => new LinearAxisLimitTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { @@ -65,7 +65,7 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int SpringSettingsWide.WriteFirst(SpringSettings, ref target.SpringSettings); } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out LinearAxisLimit description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out LinearAxisLimit description) { Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); @@ -119,7 +119,7 @@ static void ComputeJacobians( Vector3Wide.CrossWithoutOverlap(normal, offsetB, out angularJB); } - public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref LinearAxisLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref LinearAxisLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeJacobians(positionB - positionA, orientationA, orientationB, prestep.LocalPlaneNormal, prestep.LocalOffsetA, prestep.LocalOffsetB, prestep.MinimumOffset, prestep.MaximumOffset, out _, out var normal, out var angularJA, out var angularJB); Symmetric3x3Wide.TransformWithoutOverlap(angularJA, inertiaA.InverseInertiaTensor, out var angularImpulseToVelocityA); @@ -127,7 +127,7 @@ public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, LinearAxisServoFunctions.ApplyImpulse(normal, angularImpulseToVelocityA, angularImpulseToVelocityB, inertiaA, inertiaB, accumulatedImpulses, ref wsvA, ref wsvB); } - public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref LinearAxisLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref LinearAxisLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeJacobians(positionB - positionA, orientationA, orientationB, prestep.LocalPlaneNormal, prestep.LocalOffsetA, prestep.LocalOffsetB, prestep.MinimumOffset, prestep.MaximumOffset, out var error, out var normal, out var angularJA, out var angularJB); @@ -146,9 +146,9 @@ public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in B LinearAxisServoFunctions.ApplyImpulse(normal, angularImpulseToVelocityA, angularImpulseToVelocityB, inertiaA, inertiaB, csi, ref wsvA, ref wsvB); } - public bool RequiresIncrementalSubstepUpdates => false; + public static bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref LinearAxisLimitPrestepData prestepData) { } + public static void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref LinearAxisLimitPrestepData prestepData) { } } public class LinearAxisLimitTypeProcessor : TwoBodyTypeProcessor, LinearAxisLimitFunctions, AccessAll, AccessAll, AccessAll, AccessAll> diff --git a/BepuPhysics/Constraints/LinearAxisMotor.cs b/BepuPhysics/Constraints/LinearAxisMotor.cs index bb5367312..24165e3e1 100644 --- a/BepuPhysics/Constraints/LinearAxisMotor.cs +++ b/BepuPhysics/Constraints/LinearAxisMotor.cs @@ -34,7 +34,7 @@ public struct LinearAxisMotor : ITwoBodyConstraintDescription /// public MotorSettings Settings; - public readonly int ConstraintTypeId + public static int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get @@ -43,8 +43,8 @@ public readonly int ConstraintTypeId } } - public readonly Type TypeProcessorType => typeof(LinearAxisMotorTypeProcessor); - public readonly TypeProcessor CreateTypeProcessor() => new LinearAxisMotorTypeProcessor(); + public static Type TypeProcessorType => typeof(LinearAxisMotorTypeProcessor); + public static TypeProcessor CreateTypeProcessor() => new LinearAxisMotorTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { @@ -59,7 +59,7 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int MotorSettingsWide.WriteFirst(Settings, ref target.Settings); } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out LinearAxisMotor description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out LinearAxisMotor description) { Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); @@ -82,7 +82,7 @@ public struct LinearAxisMotorPrestepData public struct LinearAxisMotorFunctions : ITwoBodyConstraintFunctions> { - public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref LinearAxisMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref LinearAxisMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { LinearAxisServoFunctions.ComputeJacobians(positionB - positionA, orientationA, orientationB, prestep.LocalPlaneNormal, prestep.LocalOffsetA, prestep.LocalOffsetB, out _, out var normal, out var angularJA, out var angularJB); Symmetric3x3Wide.TransformWithoutOverlap(angularJA, inertiaA.InverseInertiaTensor, out var angularImpulseToVelocityA); @@ -90,7 +90,7 @@ public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, LinearAxisServoFunctions.ApplyImpulse(normal, angularImpulseToVelocityA, angularImpulseToVelocityB, inertiaA, inertiaB, accumulatedImpulses, ref wsvA, ref wsvB); } - public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref LinearAxisMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref LinearAxisMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { LinearAxisServoFunctions.ComputeJacobians(positionB - positionA, orientationA, orientationB, prestep.LocalPlaneNormal, prestep.LocalOffsetA, prestep.LocalOffsetB, out _, out var normal, out var angularJA, out var angularJB); MotorSettingsWide.ComputeSoftness(prestep.Settings, dt, out var effectiveMassCFMScale, out var softnessImpulseScale, out var maximumImpulse); @@ -105,9 +105,9 @@ public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in B LinearAxisServoFunctions.ApplyImpulse(normal, angularImpulseToVelocityA, angularImpulseToVelocityB, inertiaA, inertiaB, csi, ref wsvA, ref wsvB); } - public bool RequiresIncrementalSubstepUpdates => false; + public static bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref LinearAxisMotorPrestepData prestepData) { } + public static void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref LinearAxisMotorPrestepData prestepData) { } } public class LinearAxisMotorTypeProcessor : TwoBodyTypeProcessor, LinearAxisMotorFunctions, AccessAll, AccessAll, AccessAll, AccessAll> diff --git a/BepuPhysics/Constraints/LinearAxisServo.cs b/BepuPhysics/Constraints/LinearAxisServo.cs index 98ac5a9fd..d31efe2ff 100644 --- a/BepuPhysics/Constraints/LinearAxisServo.cs +++ b/BepuPhysics/Constraints/LinearAxisServo.cs @@ -38,7 +38,7 @@ public struct LinearAxisServo : ITwoBodyConstraintDescription /// public SpringSettings SpringSettings; - public readonly int ConstraintTypeId + public static int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get @@ -47,8 +47,8 @@ public readonly int ConstraintTypeId } } - public readonly Type TypeProcessorType => typeof(LinearAxisServoTypeProcessor); - public readonly TypeProcessor CreateTypeProcessor() => new LinearAxisServoTypeProcessor(); + public static Type TypeProcessorType => typeof(LinearAxisServoTypeProcessor); + public static TypeProcessor CreateTypeProcessor() => new LinearAxisServoTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { @@ -64,7 +64,7 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int SpringSettingsWide.WriteFirst(SpringSettings, ref target.SpringSettings); } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out LinearAxisServo description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out LinearAxisServo description) { Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); @@ -216,7 +216,7 @@ public static void ComputeEffectiveMass(in Vector3Wide angularJA, in Vector3Wide effectiveMass = effectiveMassCFMScale / (inertiaA.InverseMass + inertiaB.InverseMass + angularContributionA + angularContributionB); } - public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref LinearAxisServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref LinearAxisServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeJacobians(positionB - positionA, orientationA, orientationB, prestep.LocalPlaneNormal, prestep.LocalOffsetA, prestep.LocalOffsetB, out _, out var normal, out var angularJA, out var angularJB); Symmetric3x3Wide.TransformWithoutOverlap(angularJA, inertiaA.InverseInertiaTensor, out var angularImpulseToVelocityA); @@ -224,7 +224,7 @@ public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, ApplyImpulse(normal, angularImpulseToVelocityA, angularImpulseToVelocityB, inertiaA, inertiaB, accumulatedImpulses, ref wsvA, ref wsvB); } - public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref LinearAxisServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref LinearAxisServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeJacobians(positionB - positionA, orientationA, orientationB, prestep.LocalPlaneNormal, prestep.LocalOffsetA, prestep.LocalOffsetB, out var planeNormalDot, out var normal, out var angularJA, out var angularJB); SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); @@ -243,9 +243,9 @@ public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in B ApplyImpulse(normal, angularImpulseToVelocityA, angularImpulseToVelocityB, inertiaA, inertiaB, csi, ref wsvA, ref wsvB); } - public bool RequiresIncrementalSubstepUpdates => false; + public static bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref LinearAxisServoPrestepData prestepData) { } + public static void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref LinearAxisServoPrestepData prestepData) { } } public class LinearAxisServoTypeProcessor : TwoBodyTypeProcessor, LinearAxisServoFunctions, AccessAll, AccessAll, AccessAll, AccessAll> diff --git a/BepuPhysics/Constraints/OneBodyAngularMotor.cs b/BepuPhysics/Constraints/OneBodyAngularMotor.cs index 5d89c7972..366cd120c 100644 --- a/BepuPhysics/Constraints/OneBodyAngularMotor.cs +++ b/BepuPhysics/Constraints/OneBodyAngularMotor.cs @@ -22,7 +22,7 @@ public struct OneBodyAngularMotor : IOneBodyConstraintDescription public MotorSettings Settings; - public readonly int ConstraintTypeId + public static int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get @@ -31,8 +31,8 @@ public readonly int ConstraintTypeId } } - public readonly Type TypeProcessorType => typeof(OneBodyAngularMotorTypeProcessor); - public readonly TypeProcessor CreateTypeProcessor() => new OneBodyAngularMotorTypeProcessor(); + public static Type TypeProcessorType => typeof(OneBodyAngularMotorTypeProcessor); + public static TypeProcessor CreateTypeProcessor() => new OneBodyAngularMotorTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { @@ -43,7 +43,7 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int MotorSettingsWide.WriteFirst(Settings, ref target.Settings); } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out OneBodyAngularMotor description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out OneBodyAngularMotor description) { Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); @@ -67,12 +67,12 @@ public static void ApplyImpulse(ref Vector3Wide angularVelocity, in Symmetric3x3 Vector3Wide.Add(angularVelocity, velocityChange, out angularVelocity); } - public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref OneBodyAngularMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) + public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref OneBodyAngularMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) { ApplyImpulse(ref wsvA.Angular, inertiaA.InverseInertiaTensor, accumulatedImpulses); } - public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref OneBodyAngularMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) + public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref OneBodyAngularMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) { //Jacobians are just the identity matrix. MotorSettingsWide.ComputeSoftness(prestep.Settings, dt, out var effectiveMassCFMScale, out var softnessImpulseScale, out var maximumImpulse); @@ -87,9 +87,9 @@ public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in B ApplyImpulse(ref wsvA.Angular, inertiaA.InverseInertiaTensor, csi); } - public bool RequiresIncrementalSubstepUpdates => false; + public static bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, ref OneBodyAngularMotorPrestepData prestepData) { } + public static void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, ref OneBodyAngularMotorPrestepData prestepData) { } } public class OneBodyAngularMotorTypeProcessor : OneBodyTypeProcessor diff --git a/BepuPhysics/Constraints/OneBodyAngularServo.cs b/BepuPhysics/Constraints/OneBodyAngularServo.cs index a11140eeb..7b04aa38c 100644 --- a/BepuPhysics/Constraints/OneBodyAngularServo.cs +++ b/BepuPhysics/Constraints/OneBodyAngularServo.cs @@ -26,7 +26,7 @@ public struct OneBodyAngularServo : IOneBodyConstraintDescription public ServoSettings ServoSettings; - public readonly int ConstraintTypeId + public static int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get @@ -35,8 +35,8 @@ public readonly int ConstraintTypeId } } - public readonly Type TypeProcessorType => typeof(OneBodyAngularServoTypeProcessor); - public readonly TypeProcessor CreateTypeProcessor() => new OneBodyAngularServoTypeProcessor(); + public static Type TypeProcessorType => typeof(OneBodyAngularServoTypeProcessor); + public static TypeProcessor CreateTypeProcessor() => new OneBodyAngularServoTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { @@ -49,7 +49,7 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int ServoSettingsWide.WriteFirst(ServoSettings, ref target.ServoSettings); } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out OneBodyAngularServo description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out OneBodyAngularServo description) { Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); @@ -76,13 +76,13 @@ public static void ApplyImpulse(in Symmetric3x3Wide inverseInertia, in Vector3Wi } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref OneBodyAngularServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) + public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref OneBodyAngularServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) { ApplyImpulse(inertiaA.InverseInertiaTensor, accumulatedImpulses, ref wsvA.Angular); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref OneBodyAngularServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) + public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref OneBodyAngularServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) { //Jacobians are just the identity matrix. QuaternionWide.Conjugate(orientationA, out var inverseOrientation); @@ -103,9 +103,9 @@ public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in B ApplyImpulse(inertiaA.InverseInertiaTensor, csi, ref wsvA.Angular); } - public bool RequiresIncrementalSubstepUpdates => false; + public static bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, ref OneBodyAngularServoPrestepData prestepData) { } + public static void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, ref OneBodyAngularServoPrestepData prestepData) { } } public class OneBodyAngularServoTypeProcessor : OneBodyTypeProcessor diff --git a/BepuPhysics/Constraints/OneBodyLinearMotor.cs b/BepuPhysics/Constraints/OneBodyLinearMotor.cs index cb525a5ee..5d70235f9 100644 --- a/BepuPhysics/Constraints/OneBodyLinearMotor.cs +++ b/BepuPhysics/Constraints/OneBodyLinearMotor.cs @@ -26,7 +26,7 @@ public struct OneBodyLinearMotor : IOneBodyConstraintDescription public MotorSettings Settings; - public readonly int ConstraintTypeId + public static int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get @@ -35,8 +35,8 @@ public readonly int ConstraintTypeId } } - public readonly Type TypeProcessorType => typeof(OneBodyLinearMotorTypeProcessor); - public readonly TypeProcessor CreateTypeProcessor() => new OneBodyLinearMotorTypeProcessor(); + public static Type TypeProcessorType => typeof(OneBodyLinearMotorTypeProcessor); + public static TypeProcessor CreateTypeProcessor() => new OneBodyLinearMotorTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { @@ -48,7 +48,7 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int MotorSettingsWide.WriteFirst(Settings, ref target.Settings); } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out OneBodyLinearMotor description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out OneBodyLinearMotor description) { Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); @@ -67,13 +67,13 @@ public struct OneBodyLinearMotorPrestepData public struct OneBodyLinearMotorFunctions : IOneBodyConstraintFunctions { - public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref OneBodyLinearMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) + public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref OneBodyLinearMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) { QuaternionWide.TransformWithoutOverlap(prestep.LocalOffset, orientationA, out var offset); OneBodyLinearServoFunctions.ApplyImpulse(offset, inertiaA, ref wsvA, accumulatedImpulses); } - public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref OneBodyLinearMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) + public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref OneBodyLinearMotorPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) { QuaternionWide.TransformWithoutOverlap(prestep.LocalOffset, orientationA, out var offset); MotorSettingsWide.ComputeSoftness(prestep.Settings, dt, out var effectiveMassCFMScale, out var softnessImpulseScale, out var maximumImpulse); @@ -96,9 +96,9 @@ public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in B OneBodyLinearServoFunctions.ApplyImpulse(offset, inertiaA, ref wsvA, csi); } - public bool RequiresIncrementalSubstepUpdates => false; + public static bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, ref OneBodyLinearMotorPrestepData prestepData) { } + public static void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, ref OneBodyLinearMotorPrestepData prestepData) { } } public class OneBodyLinearMotorTypeProcessor : OneBodyTypeProcessor { diff --git a/BepuPhysics/Constraints/OneBodyLinearServo.cs b/BepuPhysics/Constraints/OneBodyLinearServo.cs index 565e7c775..955d1cff7 100644 --- a/BepuPhysics/Constraints/OneBodyLinearServo.cs +++ b/BepuPhysics/Constraints/OneBodyLinearServo.cs @@ -30,7 +30,7 @@ public struct OneBodyLinearServo : IOneBodyConstraintDescription public ServoSettings ServoSettings; - public readonly int ConstraintTypeId + public static int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get @@ -39,8 +39,8 @@ public readonly int ConstraintTypeId } } - public readonly Type TypeProcessorType => typeof(OneBodyLinearServoTypeProcessor); - public readonly TypeProcessor CreateTypeProcessor() => new OneBodyLinearServoTypeProcessor(); + public static Type TypeProcessorType => typeof(OneBodyLinearServoTypeProcessor); + public static TypeProcessor CreateTypeProcessor() => new OneBodyLinearServoTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { @@ -53,7 +53,7 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int ServoSettingsWide.WriteFirst(ServoSettings, ref target.ServoSettings); } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out OneBodyLinearServo description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out OneBodyLinearServo description) { Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); @@ -105,14 +105,14 @@ public static void ApplyImpulse(in Vector3Wide offset, in BodyInertiaWide inerti } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref OneBodyLinearServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) + public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref OneBodyLinearServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) { QuaternionWide.TransformWithoutOverlap(prestep.LocalOffset, orientationA, out var offset); ApplyImpulse(offset, inertiaA, ref wsvA, accumulatedImpulses); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref OneBodyLinearServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) + public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref OneBodyLinearServoPrestepData prestep, ref Vector3Wide accumulatedImpulses, ref BodyVelocityWide wsvA) { QuaternionWide.TransformWithoutOverlap(prestep.LocalOffset, orientationA, out var offset); SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); @@ -141,9 +141,9 @@ public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in B ApplyImpulse(offset, inertiaA, ref wsvA, csi); } - public bool RequiresIncrementalSubstepUpdates => false; + public static bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, ref OneBodyLinearServoPrestepData prestepData) { } + public static void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, ref OneBodyLinearServoPrestepData prestepData) { } } public class OneBodyLinearServoTypeProcessor : OneBodyTypeProcessor diff --git a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs index 947361fcd..b07af57b2 100644 --- a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs @@ -15,16 +15,16 @@ namespace BepuPhysics.Constraints /// Type of the accumulated impulses used by the constraint. public interface IOneBodyConstraintFunctions { - void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, + static abstract void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref TPrestepData prestep, ref TAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA); - void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, + static abstract void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref TPrestepData prestep, ref TAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA); /// /// Gets whether this constraint type requires incremental updates for each substep taken beyond the first. /// - bool RequiresIncrementalSubstepUpdates { get; } - void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide velocity, ref TPrestepData prestepData); + static abstract bool RequiresIncrementalSubstepUpdates { get; } + static abstract void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide velocity, ref TPrestepData prestepData); } //Not a big fan of complex generic-filled inheritance hierarchies, but this is the shortest evolutionary step to removing duplicates. @@ -45,7 +45,7 @@ public abstract class OneBodyTypeProcessor> { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public int GetSortKey(int constraintIndex, ref Buffer> bodyReferences) + public static int GetSortKey(int constraintIndex, ref Buffer> bodyReferences) { BundleIndexing.GetBundleIndices(constraintIndex, out var bundleIndex, out var innerIndex); //We sort based on the body references within the constraint. @@ -88,8 +88,6 @@ public unsafe override void WarmStart(); var bodyReferencesBundles = typeBatch.BodyReferences.As>(); var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); - var function = default(TConstraintFunctions); - ref var states = ref bodies.ActiveSet.DynamicsState; for (int i = startBundle; i < exclusiveEndBundle; ++i) { ref var prestep = ref prestepBundles[i]; @@ -101,7 +99,7 @@ public unsafe override void WarmStart(dt), accumulatedImpulses, ref prestep); - function.WarmStart(positionA, orientationA, inertiaA, ref prestep, ref accumulatedImpulses, ref wsvA); + TConstraintFunctions.WarmStart(positionA, orientationA, inertiaA, ref prestep, ref accumulatedImpulses, ref wsvA); if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) { @@ -121,8 +119,6 @@ public unsafe override void Solve(ref TypeBatch typeBatch, Bodies bodies, float var prestepBundles = typeBatch.PrestepData.As(); var bodyReferencesBundles = typeBatch.BodyReferences.As>(); var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); - var function = default(TConstraintFunctions); - ref var motionStates = ref bodies.ActiveSet.DynamicsState; for (int i = startBundle; i < exclusiveEndBundle; ++i) { ref var prestep = ref prestepBundles[i]; @@ -130,25 +126,24 @@ public unsafe override void Solve(ref TypeBatch typeBatch, Bodies bodies, float ref var references = ref bodyReferencesBundles[i]; bodies.GatherState(references, true, out var positionA, out var orientationA, out var wsvA, out var inertiaA); - function.Solve(positionA, orientationA, inertiaA, dt, inverseDt, ref prestep, ref accumulatedImpulses, ref wsvA); + TConstraintFunctions.Solve(positionA, orientationA, inertiaA, dt, inverseDt, ref prestep, ref accumulatedImpulses, ref wsvA); bodies.ScatterVelocities(ref wsvA, ref references); } } - public override bool RequiresIncrementalSubstepUpdates => default(TConstraintFunctions).RequiresIncrementalSubstepUpdates; - public unsafe override void IncrementallyUpdateForSubstep(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + public override bool RequiresIncrementalSubstepUpdates => TConstraintFunctions.RequiresIncrementalSubstepUpdates; + public override void IncrementallyUpdateForSubstep(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { var prestepBundles = typeBatch.PrestepData.As(); var bodyReferencesBundles = typeBatch.BodyReferences.As>(); - var function = default(TConstraintFunctions); var dtWide = new Vector(dt); for (int i = startBundle; i < exclusiveEndBundle; ++i) { ref var prestep = ref prestepBundles[i]; ref var references = ref bodyReferencesBundles[i]; bodies.GatherState(references, true, out _, out _, out var wsvA, out _); - function.IncrementallyUpdateForSubstep(dtWide, wsvA, ref prestep); + TConstraintFunctions.IncrementallyUpdateForSubstep(dtWide, wsvA, ref prestep); } } } diff --git a/BepuPhysics/Constraints/PointOnLineServo.cs b/BepuPhysics/Constraints/PointOnLineServo.cs index ca252c2cb..ac4c6f198 100644 --- a/BepuPhysics/Constraints/PointOnLineServo.cs +++ b/BepuPhysics/Constraints/PointOnLineServo.cs @@ -34,7 +34,7 @@ public struct PointOnLineServo : ITwoBodyConstraintDescription /// public SpringSettings SpringSettings; - public readonly int ConstraintTypeId + public static int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get @@ -43,8 +43,8 @@ public readonly int ConstraintTypeId } } - public readonly Type TypeProcessorType => typeof(PointOnLineServoTypeProcessor); - public readonly TypeProcessor CreateTypeProcessor() => new PointOnLineServoTypeProcessor(); + public static Type TypeProcessorType => typeof(PointOnLineServoTypeProcessor); + public static TypeProcessor CreateTypeProcessor() => new PointOnLineServoTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { @@ -59,7 +59,7 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int SpringSettingsWide.WriteFirst(SpringSettings, ref target.SpringSettings); } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out PointOnLineServo description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out PointOnLineServo description) { Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); @@ -126,13 +126,13 @@ public static void ComputeJacobians(in Vector3Wide ab, in QuaternionWide orienta Vector3Wide.CrossWithoutOverlap(linearJacobian.X, offsetB, out angularJB.X); Vector3Wide.CrossWithoutOverlap(linearJacobian.Y, offsetB, out angularJB.Y); } - public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref PointOnLineServoPrestepData prestep, ref Vector2Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref PointOnLineServoPrestepData prestep, ref Vector2Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeJacobians(positionB - positionA, orientationA, orientationB, prestep.LocalDirection, prestep.LocalOffsetA, prestep.LocalOffsetB, out _, out var linearJacobian, out var angularJA, out var angularJB); ApplyImpulse(ref wsvA, ref wsvB, linearJacobian, angularJA, angularJB, inertiaA, inertiaB, ref accumulatedImpulses); } - public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref PointOnLineServoPrestepData prestep, ref Vector2Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref PointOnLineServoPrestepData prestep, ref Vector2Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { //This constrains a point on B to a line attached to A. It works on two degrees of freedom at the same time; those are the tangent axes to the line direction. //The error is measured as closest offset from the line. In other words: @@ -188,9 +188,9 @@ public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in B ApplyImpulse(ref wsvA, ref wsvB, linearJacobian, angularJA, angularJB, inertiaA, inertiaB, ref csi); } - public bool RequiresIncrementalSubstepUpdates => false; + public static bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref PointOnLineServoPrestepData prestepData) { } + public static void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref PointOnLineServoPrestepData prestepData) { } } public class PointOnLineServoTypeProcessor : TwoBodyTypeProcessor diff --git a/BepuPhysics/Constraints/SwingLimit.cs b/BepuPhysics/Constraints/SwingLimit.cs index cfccc39b2..8ec6fca43 100644 --- a/BepuPhysics/Constraints/SwingLimit.cs +++ b/BepuPhysics/Constraints/SwingLimit.cs @@ -35,7 +35,7 @@ public struct SwingLimit : ITwoBodyConstraintDescription /// public float MaximumSwingAngle { readonly get { return (float)Math.Acos(MinimumDot); } set { MinimumDot = (float)Math.Cos(value); } } - public readonly int ConstraintTypeId + public static int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get @@ -44,8 +44,8 @@ public readonly int ConstraintTypeId } } - public readonly Type TypeProcessorType => typeof(SwingLimitTypeProcessor); - public readonly TypeProcessor CreateTypeProcessor() => new SwingLimitTypeProcessor(); + public static Type TypeProcessorType => typeof(SwingLimitTypeProcessor); + public static TypeProcessor CreateTypeProcessor() => new SwingLimitTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { @@ -66,7 +66,7 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int GetFirst(ref target.SpringSettings.TwiceDampingRatio) = SpringSettings.TwiceDampingRatio; } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out SwingLimit description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out SwingLimit description) { Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); @@ -114,7 +114,7 @@ static void ComputeJacobian(in Vector3Wide axisLocalA, in Vector3Wide axisLocalB var useFallback = Vector.LessThan(jacobianLengthSquared, new Vector(1e-7f)); Vector3Wide.ConditionalSelect(useFallback, fallbackJacobian, jacobianA, out jacobianA); } - public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref SwingLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref SwingLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeJacobian(prestep.AxisLocalA, prestep.AxisLocalB, orientationA, orientationB, out _, out _, out var jacobianA); Symmetric3x3Wide.TransformWithoutOverlap(jacobianA, inertiaA.InverseInertiaTensor, out var impulseToVelocityA); @@ -122,7 +122,7 @@ public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, ApplyImpulse(impulseToVelocityA, negatedImpulseToVelocityB, accumulatedImpulses, ref wsvA.Angular, ref wsvB.Angular); } - public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref SwingLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref SwingLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { //The swing limit attempts to keep an axis on body A within from an axis on body B. In other words, this is the same as a hinge joint, but with one fewer DOF. //(Note that the jacobians are extremely similar to the AngularSwivelHinge; the difference is that this is a speculative inequality constraint.) @@ -164,9 +164,9 @@ public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in B ApplyImpulse(impulseToVelocityA, negatedImpulseToVelocityB, csi, ref wsvA.Angular, ref wsvB.Angular); } - public bool RequiresIncrementalSubstepUpdates => false; + public static bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref SwingLimitPrestepData prestepData) { } + public static void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref SwingLimitPrestepData prestepData) { } } public class SwingLimitTypeProcessor : TwoBodyTypeProcessor, SwingLimitFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular> diff --git a/BepuPhysics/Constraints/SwivelHinge.cs b/BepuPhysics/Constraints/SwivelHinge.cs index abd4eddbd..25b51bb59 100644 --- a/BepuPhysics/Constraints/SwivelHinge.cs +++ b/BepuPhysics/Constraints/SwivelHinge.cs @@ -34,7 +34,7 @@ public struct SwivelHinge : ITwoBodyConstraintDescription /// public SpringSettings SpringSettings; - public readonly int ConstraintTypeId + public static int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get @@ -43,8 +43,8 @@ public readonly int ConstraintTypeId } } - public readonly Type TypeProcessorType => typeof(SwivelHingeTypeProcessor); - public readonly TypeProcessor CreateTypeProcessor() => new SwivelHingeTypeProcessor(); + public static Type TypeProcessorType => typeof(SwivelHingeTypeProcessor); + public static TypeProcessor CreateTypeProcessor() => new SwivelHingeTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { @@ -60,7 +60,7 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int SpringSettingsWide.WriteFirst(SpringSettings, ref target.SpringSettings); } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out SwivelHinge description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out SwivelHinge description) { Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); @@ -125,14 +125,14 @@ private static void ComputeJacobian(in Vector3Wide localOffsetA, in Vector3Wide swivelHingeJacobian = Vector3Wide.ConditionalSelect(useFallbackJacobian, hingeAxis, swivelHingeJacobian); } - public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref SwivelHingePrestepData prestep, ref Vector4Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref SwivelHingePrestepData prestep, ref Vector4Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeJacobian(prestep.LocalOffsetA, prestep.LocalSwivelAxisA, prestep.LocalOffsetB, prestep.LocalHingeAxisB, orientationA, orientationB, out _, out _, out var offsetA, out var offsetB, out var swivelHingeJacobian); ApplyImpulse(offsetA, offsetB, swivelHingeJacobian, inertiaA, inertiaB, ref accumulatedImpulses, ref wsvA, ref wsvB); } - public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref SwivelHingePrestepData prestep, ref Vector4Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref SwivelHingePrestepData prestep, ref Vector4Wide accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { //4x12 jacobians, from BallSocket and AngularSwivelHinge: //[ I, skew(offsetA), -I, -skew(offsetB) ] @@ -209,9 +209,9 @@ public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in B ApplyImpulse(offsetA, offsetB, swivelHingeJacobian, inertiaA, inertiaB, ref csi, ref wsvA, ref wsvB); } - public bool RequiresIncrementalSubstepUpdates => false; + public static bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref SwivelHingePrestepData prestepData) { } + public static void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref SwivelHingePrestepData prestepData) { } } public class SwivelHingeTypeProcessor : TwoBodyTypeProcessor diff --git a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs index f4455556a..0980e2a91 100644 --- a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs @@ -25,12 +25,12 @@ public struct ThreeBodyReferences /// Type of the accumulated impulses used by the constraint. public interface IThreeBodyConstraintFunctions { - void WarmStart( + static abstract void WarmStart( in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, ref TPrestepData prestep, ref TAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC); - void Solve( + static abstract void Solve( in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, float dt, float inverseDt, @@ -39,8 +39,8 @@ void Solve( /// /// Gets whether this constraint type requires incremental updates for each substep taken beyond the first. /// - bool RequiresIncrementalSubstepUpdates { get; } - void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, in BodyVelocityWide wsvC, ref TPrestepData prestepData); + static abstract bool RequiresIncrementalSubstepUpdates { get; } + static abstract void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, in BodyVelocityWide wsvC, ref TPrestepData prestepData); } /// @@ -63,7 +63,7 @@ public abstract class ThreeBodyTypeProcessor { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public int GetSortKey(int constraintIndex, ref Buffer bodyReferences) + public static int GetSortKey(int constraintIndex, ref Buffer bodyReferences) { BundleIndexing.GetBundleIndices(constraintIndex, out var bundleIndex, out var innerIndex); ref var bundleReferences = ref bodyReferences[bundleIndex]; @@ -99,15 +99,13 @@ internal sealed override void VerifySortRegion(ref TypeBatch typeBatch, int bund VerifySortRegion(ref typeBatch, bundleStartIndex, constraintCount, ref sortedKeys, ref sortedSourceIndices); } - public unsafe override void WarmStart( + public override void WarmStart( ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { var prestepBundles = typeBatch.PrestepData.As(); var bodyReferencesBundles = typeBatch.BodyReferences.As(); var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); - var function = default(TConstraintFunctions); - ref var states = ref bodies.ActiveSet.DynamicsState; for (int i = startBundle; i < exclusiveEndBundle; ++i) { ref var prestep = ref prestepBundles[i]; @@ -123,7 +121,7 @@ public unsafe override void WarmStart(dt), accumulatedImpulses, ref prestep); - function.WarmStart(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC); + TConstraintFunctions.WarmStart(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC); if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) { @@ -143,13 +141,11 @@ public unsafe override void WarmStart(); var bodyReferencesBundles = typeBatch.BodyReferences.As(); var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); - var function = default(TConstraintFunctions); - ref var motionStates = ref bodies.ActiveSet.DynamicsState; for (int i = startBundle; i < exclusiveEndBundle; ++i) { ref var prestep = ref prestepBundles[i]; @@ -159,7 +155,7 @@ public unsafe override void Solve(ref TypeBatch typeBatch, Bodies bodies, float bodies.GatherState(references.IndexB, true, out var positionB, out var orientationB, out var wsvB, out var inertiaB); bodies.GatherState(references.IndexC, true, out var positionC, out var orientationC, out var wsvC, out var inertiaC); - function.Solve(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, dt, inverseDt, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC); + TConstraintFunctions.Solve(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, dt, inverseDt, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC); bodies.ScatterVelocities(ref wsvA, ref references.IndexA); bodies.ScatterVelocities(ref wsvB, ref references.IndexB); @@ -167,12 +163,11 @@ public unsafe override void Solve(ref TypeBatch typeBatch, Bodies bodies, float } } - public override bool RequiresIncrementalSubstepUpdates => default(TConstraintFunctions).RequiresIncrementalSubstepUpdates; - public unsafe override void IncrementallyUpdateForSubstep(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + public override bool RequiresIncrementalSubstepUpdates => TConstraintFunctions.RequiresIncrementalSubstepUpdates; + public override void IncrementallyUpdateForSubstep(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { var prestepBundles = typeBatch.PrestepData.As(); var bodyReferencesBundles = typeBatch.BodyReferences.As(); - var function = default(TConstraintFunctions); var dtWide = new Vector(dt); for (int i = startBundle; i < exclusiveEndBundle; ++i) { @@ -181,7 +176,7 @@ public unsafe override void IncrementallyUpdateForSubstep(ref TypeBatch typeBatc bodies.GatherState(references.IndexA, true, out _, out _, out var wsvA, out _); bodies.GatherState(references.IndexB, true, out _, out _, out var wsvB, out _); bodies.GatherState(references.IndexC, true, out _, out _, out var wsvC, out _); - function.IncrementallyUpdateForSubstep(dtWide, wsvA, wsvB, wsvC, ref prestep); + TConstraintFunctions.IncrementallyUpdateForSubstep(dtWide, wsvA, wsvB, wsvC, ref prestep); } } } diff --git a/BepuPhysics/Constraints/TwistLimit.cs b/BepuPhysics/Constraints/TwistLimit.cs index 49ace449e..23468c4b8 100644 --- a/BepuPhysics/Constraints/TwistLimit.cs +++ b/BepuPhysics/Constraints/TwistLimit.cs @@ -36,7 +36,7 @@ public struct TwistLimit : ITwoBodyConstraintDescription /// public SpringSettings SpringSettings; - public readonly int ConstraintTypeId + public static int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get @@ -45,8 +45,8 @@ public readonly int ConstraintTypeId } } - public readonly Type TypeProcessorType => typeof(TwistLimitTypeProcessor); - public readonly TypeProcessor CreateTypeProcessor() => new TwistLimitTypeProcessor(); + public static Type TypeProcessorType => typeof(TwistLimitTypeProcessor); + public static TypeProcessor CreateTypeProcessor() => new TwistLimitTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { @@ -62,7 +62,7 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int SpringSettingsWide.WriteFirst(SpringSettings, ref target.SpringSettings); } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out TwistLimit description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out TwistLimit description) { Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); @@ -102,7 +102,7 @@ static void ComputeJacobian(in QuaternionWide orientationA, in QuaternionWide or Vector3Wide.Negate(jacobianA, out var negatedJacobianA); Vector3Wide.ConditionalSelect(useMin, negatedJacobianA, jacobianA, out jacobianA); } - public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref TwistLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref TwistLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeJacobian(orientationA, orientationB, prestep.LocalBasisA, prestep.LocalBasisB, prestep.MinimumAngle, prestep.MaximumAngle, out _, out var jacobianA); Symmetric3x3Wide.TransformWithoutOverlap(jacobianA, inertiaA.InverseInertiaTensor, out var impulseToVelocityA); @@ -110,7 +110,7 @@ public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, TwistServoFunctions.ApplyImpulse(ref wsvA.Angular, ref wsvB.Angular, impulseToVelocityA, negatedImpulseToVelocityB, accumulatedImpulses); } - public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref TwistLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref TwistLimitPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeJacobian(orientationA, orientationB, prestep.LocalBasisA, prestep.LocalBasisB, prestep.MinimumAngle, prestep.MaximumAngle, out var error, out var jacobianA); @@ -131,9 +131,9 @@ public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in B TwistServoFunctions.ApplyImpulse(ref wsvA.Angular, ref wsvB.Angular, impulseToVelocityA, negatedImpulseToVelocityB, csi); } - public bool RequiresIncrementalSubstepUpdates => false; + public static bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref TwistLimitPrestepData prestepData) { } + public static void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref TwistLimitPrestepData prestepData) { } } public class TwistLimitTypeProcessor : TwoBodyTypeProcessor, TwistLimitFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular> diff --git a/BepuPhysics/Constraints/TwistMotor.cs b/BepuPhysics/Constraints/TwistMotor.cs index 533555274..3ae2ab81d 100644 --- a/BepuPhysics/Constraints/TwistMotor.cs +++ b/BepuPhysics/Constraints/TwistMotor.cs @@ -30,7 +30,7 @@ public struct TwistMotor : ITwoBodyConstraintDescription /// public MotorSettings Settings; - public readonly int ConstraintTypeId + public static int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get @@ -39,8 +39,8 @@ public readonly int ConstraintTypeId } } - public readonly Type TypeProcessorType => typeof(TwistMotorTypeProcessor); - public readonly TypeProcessor CreateTypeProcessor() => new TwistMotorTypeProcessor(); + public static Type TypeProcessorType => typeof(TwistMotorTypeProcessor); + public static TypeProcessor CreateTypeProcessor() => new TwistMotorTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { @@ -55,7 +55,7 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int MotorSettingsWide.WriteFirst(Settings, ref target.Settings); } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out TwistMotor description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out TwistMotor description) { Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); @@ -88,7 +88,7 @@ public static void ComputeJacobian(in QuaternionWide orientationA, in Quaternion Vector3Wide.ConditionalSelect(Vector.LessThan(length, new Vector(1e-10f)), axisA, jacobianA, out jacobianA); } - public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref TwistMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref TwistMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeJacobian(orientationA, orientationB, prestep.LocalAxisA, prestep.LocalAxisB, out var jacobianA); Symmetric3x3Wide.TransformWithoutOverlap(jacobianA, inertiaA.InverseInertiaTensor, out var impulseToVelocityA); @@ -96,7 +96,7 @@ public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, TwistServoFunctions.ApplyImpulse(ref wsvA.Angular, ref wsvB.Angular, impulseToVelocityA, negatedImpulseToVelocityB, accumulatedImpulses); } - public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref TwistMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref TwistMotorPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeJacobian(orientationA, orientationB, prestep.LocalAxisA, prestep.LocalAxisB, out var jacobianA); @@ -120,9 +120,9 @@ public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in B TwistServoFunctions.ApplyImpulse(ref wsvA.Angular, ref wsvB.Angular, impulseToVelocityA, negatedImpulseToVelocityB, csi); } - public bool RequiresIncrementalSubstepUpdates => false; + public static bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref TwistMotorPrestepData prestepData) { } + public static void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref TwistMotorPrestepData prestepData) { } } public class TwistMotorTypeProcessor : TwoBodyTypeProcessor, TwistMotorFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular> diff --git a/BepuPhysics/Constraints/TwistServo.cs b/BepuPhysics/Constraints/TwistServo.cs index 97e06c093..08e9a75eb 100644 --- a/BepuPhysics/Constraints/TwistServo.cs +++ b/BepuPhysics/Constraints/TwistServo.cs @@ -36,7 +36,7 @@ public struct TwistServo : ITwoBodyConstraintDescription /// public ServoSettings ServoSettings; - public readonly int ConstraintTypeId + public static int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get @@ -45,8 +45,8 @@ public readonly int ConstraintTypeId } } - public readonly Type TypeProcessorType => typeof(TwistServoTypeProcessor); - public readonly TypeProcessor CreateTypeProcessor() => new TwistServoTypeProcessor(); + public static Type TypeProcessorType => typeof(TwistServoTypeProcessor); + public static TypeProcessor CreateTypeProcessor() => new TwistServoTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { @@ -62,7 +62,7 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int ServoSettingsWide.WriteFirst(ServoSettings, ref target.ServoSettings); } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out TwistServo description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out TwistServo description) { Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); @@ -181,7 +181,7 @@ public static void ComputeJacobian(in QuaternionWide orientationA, in Quaternion Vector3Wide.ConditionalSelect(Vector.LessThan(length, new Vector(1e-10f)), basisAZ, jacobianA, out jacobianA); } - public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref TwistServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref TwistServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeJacobian(orientationA, orientationB, prestep.LocalBasisA, prestep.LocalBasisB, out var jacobianA); Symmetric3x3Wide.TransformWithoutOverlap(jacobianA, inertiaA.InverseInertiaTensor, out var impulseToVelocityA); @@ -189,7 +189,7 @@ public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, ApplyImpulse(ref wsvA.Angular, ref wsvB.Angular, impulseToVelocityA, negatedImpulseToVelocityB, accumulatedImpulses); } - public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref TwistServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref TwistServoPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { ComputeJacobian(orientationA, orientationB, prestep.LocalBasisA, prestep.LocalBasisB, out var basisBX, out var basisBZ, out var basisA, out var jacobianA); @@ -216,9 +216,9 @@ public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in B ApplyImpulse(ref wsvA.Angular, ref wsvB.Angular, impulseToVelocityA, negatedImpulseToVelocityB, csi); } - public bool RequiresIncrementalSubstepUpdates => false; + public static bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref TwistServoPrestepData prestepData) { } + public static void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref TwistServoPrestepData prestepData) { } } public class TwistServoTypeProcessor : TwoBodyTypeProcessor, TwistServoFunctions, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular, AccessOnlyAngular> diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 2d493c9cb..55e208cca 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -28,16 +28,16 @@ public struct TwoBodyReferences /// Type of the accumulated impulses used by the constraint. public interface ITwoBodyConstraintFunctions { - void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, + static abstract void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref TPrestepData prestep, ref TAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB); - void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, + static abstract void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref TPrestepData prestep, ref TAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB); /// /// Gets whether this constraint type requires incremental updates for each substep taken beyond the first. /// - bool RequiresIncrementalSubstepUpdates { get; } - void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref TPrestepData prestepData); + static abstract bool RequiresIncrementalSubstepUpdates { get; } + static abstract void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref TPrestepData prestepData); } //Not a big fan of complex generic-filled inheritance hierarchies, but this is the shortest evolutionary step to removing duplicates. @@ -60,7 +60,7 @@ public abstract class TwoBodyTypeProcessor { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public int GetSortKey(int constraintIndex, ref Buffer bodyReferences) + public static int GetSortKey(int constraintIndex, ref Buffer bodyReferences) { BundleIndexing.GetBundleIndices(constraintIndex, out var bundleIndex, out var innerIndex); ref var bundleReferences = ref bodyReferences[bundleIndex]; @@ -177,8 +177,6 @@ public unsafe override void WarmStart(); var bodyReferencesBundles = typeBatch.BodyReferences.As(); var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); - Unsafe.SkipInit(out TConstraintFunctions function); - ref var states = ref bodies.ActiveSet.DynamicsState; //EarlyPrefetch(WarmStartPrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref states, startBundle, exclusiveEndBundle); for (int i = startBundle; i < exclusiveEndBundle; ++i) { @@ -191,7 +189,7 @@ public unsafe override void WarmStart(bodies, ref integratorCallbacks, ref integrationFlags, 1, dt, workerIndex, i, ref references.IndexB, out var positionB, out var orientationB, out var wsvB, out var inertiaB); - function.WarmStart(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB); + TConstraintFunctions.WarmStart(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB); if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) { @@ -214,8 +212,6 @@ public unsafe override void Solve(ref TypeBatch typeBatch, Bodies bodies, float var prestepBundles = typeBatch.PrestepData.As(); var bodyReferencesBundles = typeBatch.BodyReferences.As(); var accumulatedImpulsesBundles = typeBatch.AccumulatedImpulses.As(); - Unsafe.SkipInit(out TConstraintFunctions function); - ref var motionStates = ref bodies.ActiveSet.DynamicsState; //EarlyPrefetch(SolvePrefetchDistance, ref typeBatch, ref bodyReferencesBundles, ref motionStates, startBundle, exclusiveEndBundle); for (int i = startBundle; i < exclusiveEndBundle; ++i) { @@ -226,19 +222,18 @@ public unsafe override void Solve(ref TypeBatch typeBatch, Bodies bodies, float bodies.GatherState(references.IndexA, true, out var positionA, out var orientationA, out var wsvA, out var inertiaA); bodies.GatherState(references.IndexB, true, out var positionB, out var orientationB, out var wsvB, out var inertiaB); - function.Solve(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, dt, inverseDt, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB); + TConstraintFunctions.Solve(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, dt, inverseDt, ref prestep, ref accumulatedImpulses, ref wsvA, ref wsvB); bodies.ScatterVelocities(ref wsvA, ref references.IndexA); bodies.ScatterVelocities(ref wsvB, ref references.IndexB); } } - public override bool RequiresIncrementalSubstepUpdates => default(TConstraintFunctions).RequiresIncrementalSubstepUpdates; + public override bool RequiresIncrementalSubstepUpdates => TConstraintFunctions.RequiresIncrementalSubstepUpdates; public unsafe override void IncrementallyUpdateForSubstep(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { var prestepBundles = typeBatch.PrestepData.As(); var bodyReferencesBundles = typeBatch.BodyReferences.As(); - var function = default(TConstraintFunctions); var dtWide = new Vector(dt); for (int i = startBundle; i < exclusiveEndBundle; ++i) { @@ -246,7 +241,7 @@ public unsafe override void IncrementallyUpdateForSubstep(ref TypeBatch typeBatc ref var references = ref bodyReferencesBundles[i]; bodies.GatherState(references.IndexA, true, out _, out _, out var wsvA, out _); bodies.GatherState(references.IndexB, true, out _, out _, out var wsvB, out _); - function.IncrementallyUpdateForSubstep(dtWide, wsvA, wsvB, ref prestep); + TConstraintFunctions.IncrementallyUpdateForSubstep(dtWide, wsvA, wsvB, ref prestep); } } } diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index 20db72b28..df1170f1d 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -232,7 +232,7 @@ internal static int GetCountInBundle(ref TypeBatch typeBatch, int bundleStartInd /// public interface ISortKeyGenerator where TBodyReferences : unmanaged { - int GetSortKey(int constraintIndex, ref Buffer bodyReferences); + static abstract int GetSortKey(int constraintIndex, ref Buffer bodyReferences); } //Note that the only reason to have generics at the type level here is to avoid the need to specify them for each individual function. It's functionally equivalent, but this just @@ -827,12 +827,11 @@ protected void GenerateSortKeysAndCopyReferences( ref int firstSortKey, ref int firstSourceIndex, ref Buffer bodyReferencesCache) where TSortKeyGenerator : struct, ISortKeyGenerator { - var sortKeyGenerator = default(TSortKeyGenerator); var bodyReferences = typeBatch.BodyReferences.As(); for (int i = 0; i < constraintCount; ++i) { Unsafe.Add(ref firstSourceIndex, i) = localConstraintStart + i; - Unsafe.Add(ref firstSortKey, i) = sortKeyGenerator.GetSortKey(constraintStart + i, ref bodyReferences); + Unsafe.Add(ref firstSortKey, i) = TSortKeyGenerator.GetSortKey(constraintStart + i, ref bodyReferences); } var typedBodyReferencesCache = bodyReferencesCache.As(); bodyReferences.CopyTo(bundleStart, typedBodyReferencesCache, localBundleStart, bundleCount); @@ -842,7 +841,6 @@ protected void GenerateSortKeysAndCopyReferences( protected void VerifySortRegion(ref TypeBatch typeBatch, int bundleStartIndex, int constraintCount, ref Buffer sortedKeys, ref Buffer sortedSourceIndices) where TSortKeyGenerator : struct, ISortKeyGenerator { - var sortKeyGenerator = default(TSortKeyGenerator); var previousKey = -1; var baseIndex = bundleStartIndex << BundleIndexing.VectorShift; var bodyReferences = typeBatch.BodyReferences.As(); @@ -850,7 +848,7 @@ protected void VerifySortRegion(ref TypeBatch typeBatch, int { var sourceIndex = sortedSourceIndices[i]; var targetIndex = baseIndex + i; - var key = sortKeyGenerator.GetSortKey(baseIndex + i, ref bodyReferences); + var key = TSortKeyGenerator.GetSortKey(baseIndex + i, ref bodyReferences); //Note that this assert uses >= and not >; in a synchronized constraint batch, it's impossible for body references to be duplicated, but fallback batches CAN have duplicates. Debug.Assert(key >= previousKey, "After the sort and swap completes, all constraints should be in order."); Debug.Assert(key == sortedKeys[i], "After the swap goes through, the rederived sort keys should match the previously sorted ones."); diff --git a/BepuPhysics/Constraints/VolumeConstraint.cs b/BepuPhysics/Constraints/VolumeConstraint.cs index 8afb2fe80..e07695b1b 100644 --- a/BepuPhysics/Constraints/VolumeConstraint.cs +++ b/BepuPhysics/Constraints/VolumeConstraint.cs @@ -38,7 +38,7 @@ public VolumeConstraint(Vector3 a, Vector3 b, Vector3 c, Vector3 d, SpringSettin SpringSettings = springSettings; } - public readonly int ConstraintTypeId + public static int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get @@ -47,8 +47,8 @@ public readonly int ConstraintTypeId } } - public readonly Type TypeProcessorType => typeof(VolumeConstraintTypeProcessor); - public readonly TypeProcessor CreateTypeProcessor() => new VolumeConstraintTypeProcessor(); + public static Type TypeProcessorType => typeof(VolumeConstraintTypeProcessor); + public static TypeProcessor CreateTypeProcessor() => new VolumeConstraintTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { @@ -59,7 +59,7 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int SpringSettingsWide.WriteFirst(SpringSettings, ref target.SpringSettings); } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out VolumeConstraint description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out VolumeConstraint description) { Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); @@ -106,14 +106,14 @@ static void ComputeJacobian(in Vector3Wide positionA, in Vector3Wide positionB, Vector3Wide.Add(jacobianB, jacobianC, out negatedJA); Vector3Wide.Add(jacobianD, negatedJA, out negatedJA); } - public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in Vector3Wide positionD, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, ref VolumeConstraintPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC, ref BodyVelocityWide wsvD) + public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in Vector3Wide positionD, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, ref VolumeConstraintPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC, ref BodyVelocityWide wsvD) { ComputeJacobian(positionA, positionB, positionC, positionD, out var ad, out var negatedJA, out var jacobianB, out var jacobianC, out var jacobianD); //Vector3Wide.Dot(jacobianD, ad, out var unscaledVolume); ApplyImpulse(inertiaA.InverseMass, inertiaB.InverseMass, inertiaC.InverseMass, inertiaD.InverseMass, negatedJA, jacobianB, jacobianC, jacobianD, accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC, ref wsvD); } - public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in Vector3Wide positionD, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, float dt, float inverseDt, ref VolumeConstraintPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC, ref BodyVelocityWide wsvD) + public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in Vector3Wide positionD, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, float dt, float inverseDt, ref VolumeConstraintPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC, ref BodyVelocityWide wsvD) { //Volume of parallelepiped with vertices a, b, c, d is: //(ab x ac) * ad @@ -173,9 +173,9 @@ public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in B ApplyImpulse(inertiaA.InverseMass, inertiaB.InverseMass, inertiaC.InverseMass, inertiaD.InverseMass, negatedJA, jacobianB, jacobianC, jacobianD, csi, ref wsvA, ref wsvB, ref wsvC, ref wsvD); } - public bool RequiresIncrementalSubstepUpdates => false; + public static bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, in BodyVelocityWide wsvC, in BodyVelocityWide wsvD, ref VolumeConstraintPrestepData prestepData) { } + public static void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, in BodyVelocityWide wsvC, in BodyVelocityWide wsvD, ref VolumeConstraintPrestepData prestepData) { } } diff --git a/BepuPhysics/Constraints/Weld.cs b/BepuPhysics/Constraints/Weld.cs index 7e5d0b291..925bc212e 100644 --- a/BepuPhysics/Constraints/Weld.cs +++ b/BepuPhysics/Constraints/Weld.cs @@ -33,7 +33,7 @@ public struct Weld : ITwoBodyConstraintDescription /// public SpringSettings SpringSettings; - public readonly int ConstraintTypeId + public static int ConstraintTypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get @@ -42,8 +42,8 @@ public readonly int ConstraintTypeId } } - public readonly Type TypeProcessorType => typeof(WeldTypeProcessor); - public readonly TypeProcessor CreateTypeProcessor() => new WeldTypeProcessor(); + public static Type TypeProcessorType => typeof(WeldTypeProcessor); + public static TypeProcessor CreateTypeProcessor() => new WeldTypeProcessor(); public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex) { @@ -58,7 +58,7 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int GetFirst(ref target.SpringSettings.TwiceDampingRatio) = SpringSettings.TwiceDampingRatio; } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out Weld description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out Weld description) { Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type."); ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); @@ -114,14 +114,14 @@ private static void ApplyImpulse(in BodyInertiaWide inertiaA, in BodyInertiaWide } //[MethodImpl(MethodImplOptions.NoInlining)] - public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, + public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref WeldPrestepData prestep, ref WeldAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { Transform(prestep.LocalOffset, orientationA, out var offset); ApplyImpulse(inertiaA, inertiaB, offset, accumulatedImpulses.Orientation, accumulatedImpulses.Offset, ref wsvA, ref wsvB); } //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, + public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref WeldPrestepData prestep, ref WeldAccumulatedImpulses accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) { //The weld constraint handles 6 degrees of freedom simultaneously. The constraints are: @@ -211,9 +211,9 @@ public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in B } - public bool RequiresIncrementalSubstepUpdates => false; + public static bool RequiresIncrementalSubstepUpdates => false; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref WeldPrestepData prestepData) { } + public static void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide wsvA, in BodyVelocityWide wsvB, ref WeldPrestepData prestepData) { } } diff --git a/BepuPhysics/SequentialFallbackBatch.cs b/BepuPhysics/SequentialFallbackBatch.cs index 4b4a7aa6e..df42d7b58 100644 --- a/BepuPhysics/SequentialFallbackBatch.cs +++ b/BepuPhysics/SequentialFallbackBatch.cs @@ -13,13 +13,13 @@ namespace BepuPhysics { interface IBodyReferenceGetter { - int GetBodyReference(Bodies bodies, BodyHandle handle); + static abstract int GetBodyReference(Bodies bodies, BodyHandle handle); } struct ActiveSetGetter : IBodyReferenceGetter { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public int GetBodyReference(Bodies bodies, BodyHandle bodyHandle) + public static int GetBodyReference(Bodies bodies, BodyHandle bodyHandle) { ref var bodyLocation = ref bodies.HandleToLocation[bodyHandle.Value]; Debug.Assert(bodyLocation.SetIndex == 0, "When creating a fallback batch for the active set, all bodies associated with it must be active."); @@ -29,7 +29,7 @@ public int GetBodyReference(Bodies bodies, BodyHandle bodyHandle) struct InactiveSetGetter : IBodyReferenceGetter { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public int GetBodyReference(Bodies bodies, BodyHandle bodyHandle) + public static int GetBodyReference(Bodies bodies, BodyHandle bodyHandle) { return bodyHandle.Value; } @@ -44,7 +44,7 @@ public struct SequentialFallbackBatch /// /// Gets the number of bodies in the fallback batch. /// - public readonly int BodyCount { get { return dynamicBodyConstraintCounts.Count; } } + public int BodyCount { get { return dynamicBodyConstraintCounts.Count; } } //In order to maintain the batch referenced handles for the fallback batch (which can have the same body appear more than once), //every body must maintain a count of fallback constraints associated with it. @@ -60,7 +60,7 @@ unsafe void Allocate(Span dynamicBodyHandles, EnsureCapacity(Math.Max(dynamicBodyConstraintCounts.Count + dynamicBodyHandles.Length, minimumBodyCapacity), pool); for (int i = 0; i < dynamicBodyHandles.Length; ++i) { - var bodyReference = bodyReferenceGetter.GetBodyReference(bodies, dynamicBodyHandles[i]); + var bodyReference = TBodyReferenceGetter.GetBodyReference(bodies, dynamicBodyHandles[i]); if (dynamicBodyConstraintCounts.FindOrAllocateSlotUnsafely(bodyReference, out var slotIndex)) { diff --git a/BepuPhysics/Simulation_Queries.cs b/BepuPhysics/Simulation_Queries.cs index c54989b63..a14b7717f 100644 --- a/BepuPhysics/Simulation_Queries.cs +++ b/BepuPhysics/Simulation_Queries.cs @@ -193,7 +193,7 @@ public bool AllowTest(int childA, int childB) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void Test(CollidableReference reference, ref float maximumT) + public void Test(CollidableReference reference, ref float maximumT) { if (HitHandler.AllowTest(reference)) { @@ -277,7 +277,7 @@ public unsafe void Sweep(TShape shape, in RigidPose po dispatcher.Velocity = velocity; //Note that the shape was passed by copy, and that all shape types are required to be blittable. No GC hole. dispatcher.ShapeData = &shape; - dispatcher.ShapeType = shape.TypeId; + dispatcher.ShapeType = TShape.TypeId; dispatcher.Simulation = this; dispatcher.Pool = pool; dispatcher.CollidableBeingTested = default; diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index b97126568..8fa991437 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -294,25 +294,24 @@ protected Solver(Bodies bodies, BufferPool pool, SolveDescription solveDescripti /// is called during simuation creation and registers all the built in types. Calling manually is only necessary if custom types are used. public void Register() where TDescription : unmanaged, IConstraintDescription { - Unsafe.SkipInit(out TDescription description); - Debug.Assert(description.ConstraintTypeId >= 0, "Constraint type ids should never be negative. They're used for array indexing."); - if (TypeProcessors == null || description.ConstraintTypeId >= TypeProcessors.Length) + Debug.Assert(TDescription.ConstraintTypeId >= 0, "Constraint type ids should never be negative. They're used for array indexing."); + if (TypeProcessors == null || TDescription.ConstraintTypeId >= TypeProcessors.Length) { //This will result in some unnecessary resizes, but it hardly matters. It only happens once on registration time. //This also means we can just take the current type processors length as an accurate measure of type capacity for constraint batches. - Array.Resize(ref TypeProcessors, description.ConstraintTypeId + 1); + Array.Resize(ref TypeProcessors, TDescription.ConstraintTypeId + 1); } - if (TypeProcessors[description.ConstraintTypeId] == null) + if (TypeProcessors[TDescription.ConstraintTypeId] == null) { - var processor = description.CreateTypeProcessor(); - TypeProcessors[description.ConstraintTypeId] = processor; - processor.Initialize(description.ConstraintTypeId); + var processor = TDescription.CreateTypeProcessor(); + TypeProcessors[TDescription.ConstraintTypeId] = processor; + processor.Initialize(TDescription.ConstraintTypeId); } else { - Debug.Assert(TypeProcessors[description.ConstraintTypeId].GetType() == description.TypeProcessorType, - $"Type processor {TypeProcessors[description.ConstraintTypeId].GetType().Name} has already been registered for this description's type id " + - $"({typeof(TDescription).Name}, {default(TDescription).ConstraintTypeId}). " + + Debug.Assert(TypeProcessors[TDescription.ConstraintTypeId].GetType() == TDescription.TypeProcessorType, + $"Type processor {TypeProcessors[TDescription.ConstraintTypeId].GetType().Name} has already been registered for this description's type id " + + $"({typeof(TDescription).Name}, {TDescription.ConstraintTypeId}). " + $"Cannot register two types with the same type id."); } } @@ -1189,7 +1188,7 @@ void Add(Span bodyHandles, in TDescription description GetBlockingBodyHandles(bodyHandles, ref blockingBodyHandles, encodedBodyIndices); for (int i = 0; i <= set.Batches.Count; ++i) { - if (TryAllocateInBatch(description.ConstraintTypeId, i, blockingBodyHandles, encodedBodyIndices, out handle, out var reference)) + if (TryAllocateInBatch(TDescription.ConstraintTypeId, i, blockingBodyHandles, encodedBodyIndices, out handle, out var reference)) { ApplyDescriptionWithoutWaking(reference, description); return; @@ -1209,10 +1208,10 @@ void Add(Span bodyHandles, in TDescription description public ConstraintHandle Add(Span bodyHandles, in TDescription description) where TDescription : unmanaged, IConstraintDescription { - Debug.Assert(description.ConstraintTypeId >= 0 && description.ConstraintTypeId < TypeProcessors.Length && - TypeProcessors[description.ConstraintTypeId].GetType() == description.TypeProcessorType, + Debug.Assert(TDescription.ConstraintTypeId >= 0 && TDescription.ConstraintTypeId < TypeProcessors.Length && + TypeProcessors[TDescription.ConstraintTypeId].GetType() == TDescription.TypeProcessorType, "The description's constraint type and type processor don't match what has been registered in the solver. Did you forget to register the constraint type?"); - Debug.Assert(bodyHandles.Length == TypeProcessors[description.ConstraintTypeId].BodiesPerConstraint, + Debug.Assert(bodyHandles.Length == TypeProcessors[TDescription.ConstraintTypeId].BodiesPerConstraint, "The number of bodies supplied to a constraint add must match the expected number of bodies involved in that constraint type. Did you use the wrong Solver.Add overload?"); //Adding a constraint assumes that the involved bodies are active, so wake up anything that is sleeping. for (int i = 0; i < bodyHandles.Length; ++i) @@ -1418,8 +1417,8 @@ public void GetDescription(ConstraintReference constrain //If the compiler can prove that the BuildDescription function never references any of the instance fields, it will elide the (potentially expensive) initialization. //The BuildDescription and ConstraintTypeId members are basically static. It would be nice if C# could express that a little more cleanly with no overhead. BundleIndexing.GetBundleIndices(constraintReference.IndexInTypeBatch, out var bundleIndex, out var innerIndex); - Debug.Assert(constraintReference.TypeBatch.TypeId == default(TConstraintDescription).ConstraintTypeId, "Constraint type associated with the TConstraintDescription generic type parameter must match the type of the constraint in the solver."); - default(TConstraintDescription).BuildDescription(ref constraintReference.TypeBatch, bundleIndex, innerIndex, out description); + Debug.Assert(constraintReference.TypeBatch.TypeId == TConstraintDescription.ConstraintTypeId, "Constraint type associated with the TConstraintDescription generic type parameter must match the type of the constraint in the solver."); + TConstraintDescription.BuildDescription(ref constraintReference.TypeBatch, bundleIndex, innerIndex, out description); } /// @@ -1435,10 +1434,10 @@ public void GetDescription(ConstraintHandle handle, out //If the compiler can prove that the BuildDescription function never references any of the instance fields, it will elide the (potentially expensive) initialization. //The BuildDescription and ConstraintTypeId members are basically static. It would be nice if C# could express that a little more cleanly with no overhead. ref var location = ref HandleToConstraint[handle.Value]; - Debug.Assert(default(TConstraintDescription).ConstraintTypeId == location.TypeId, "Constraint type associated with the TConstraintDescription generic type parameter must match the type of the constraint in the solver."); + Debug.Assert(TConstraintDescription.ConstraintTypeId == location.TypeId, "Constraint type associated with the TConstraintDescription generic type parameter must match the type of the constraint in the solver."); ref var typeBatch = ref Sets[location.SetIndex].Batches[location.BatchIndex].GetTypeBatch(location.TypeId); BundleIndexing.GetBundleIndices(location.IndexInTypeBatch, out var bundleIndex, out var innerIndex); - default(TConstraintDescription).BuildDescription(ref typeBatch, bundleIndex, innerIndex, out description); + TConstraintDescription.BuildDescription(ref typeBatch, bundleIndex, innerIndex, out description); } diff --git a/DemoBenchmarks/ConvexCollisionTesterBenchmarks.cs b/DemoBenchmarks/ConvexCollisionTesterBenchmarks.cs index 67ea43aed..d8f34defe 100644 --- a/DemoBenchmarks/ConvexCollisionTesterBenchmarks.cs +++ b/DemoBenchmarks/ConvexCollisionTesterBenchmarks.cs @@ -91,33 +91,30 @@ public void Cleanup() Vector TestOrientationless1Contact(TShapeA a, TShapeB b) where TTester : unmanaged, IPairTester { - var tester = default(TTester); Vector testSum = Vector.Zero; for (int i = 0; i < iterationCount; ++i) { - tester.Test(ref a, ref b, ref speculativeMargins[i], ref offsetsB[i], Vector.Count, out var manifold); + TTester.Test(ref a, ref b, ref speculativeMargins[i], ref offsetsB[i], Vector.Count, out var manifold); testSum += manifold.Depth + manifold.Normal.X + manifold.Normal.Y + manifold.Normal.Z + manifold.OffsetA.X + manifold.OffsetA.Y + manifold.OffsetA.Z; } return testSum; } Vector TestOrientationB1Contact(TShapeA a, TShapeB b) where TTester : unmanaged, IPairTester { - var tester = default(TTester); Vector testSum = Vector.Zero; for (int i = 0; i < iterationCount; ++i) { - tester.Test(ref a, ref b, ref speculativeMargins[i], ref offsetsB[i], ref orientationsB[i], Vector.Count, out var manifold); + TTester.Test(ref a, ref b, ref speculativeMargins[i], ref offsetsB[i], ref orientationsB[i], Vector.Count, out var manifold); testSum += manifold.Depth + manifold.Normal.X + manifold.Normal.Y + manifold.Normal.Z + manifold.OffsetA.X + manifold.OffsetA.Y + manifold.OffsetA.Z; } return testSum; } Vector Test2Contact(TShapeA a, TShapeB b) where TTester : unmanaged, IPairTester { - var tester = default(TTester); Vector testSum = Vector.Zero; for (int i = 0; i < iterationCount; ++i) { - tester.Test(ref a, ref b, ref speculativeMargins[i], ref offsetsB[i], ref orientationsA[i], ref orientationsB[i], Vector.Count, out var manifold); + TTester.Test(ref a, ref b, ref speculativeMargins[i], ref offsetsB[i], ref orientationsA[i], ref orientationsB[i], Vector.Count, out var manifold); testSum += manifold.Normal.X + manifold.Normal.Y + manifold.Normal.Z + manifold.OffsetA0.X + manifold.OffsetA0.Y + manifold.OffsetA0.Z + manifold.Depth0 + manifold.OffsetA1.X + manifold.OffsetA1.Y + manifold.OffsetA1.Z + manifold.Depth1; @@ -126,11 +123,10 @@ Vector Test2Contact(TShapeA a, TShapeB b) wher } Vector Test4Contact(TShapeA a, TShapeB b) where TTester : unmanaged, IPairTester { - var tester = default(TTester); Vector testSum = Vector.Zero; for (int i = 0; i < iterationCount; ++i) { - tester.Test(ref a, ref b, ref speculativeMargins[i], ref offsetsB[i], ref orientationsA[i], ref orientationsB[i], Vector.Count, out var manifold); + TTester.Test(ref a, ref b, ref speculativeMargins[i], ref offsetsB[i], ref orientationsA[i], ref orientationsB[i], Vector.Count, out var manifold); testSum += manifold.Normal.X + manifold.Normal.Y + manifold.Normal.Z + manifold.OffsetA0.X + manifold.OffsetA0.Y + manifold.OffsetA0.Z + manifold.Depth0 + manifold.OffsetA1.X + manifold.OffsetA1.Y + manifold.OffsetA1.Z + manifold.Depth1 + diff --git a/DemoBenchmarks/FourBodyConstraintBenchmarks.cs b/DemoBenchmarks/FourBodyConstraintBenchmarks.cs index 237d9d074..1973edea0 100644 --- a/DemoBenchmarks/FourBodyConstraintBenchmarks.cs +++ b/DemoBenchmarks/FourBodyConstraintBenchmarks.cs @@ -23,7 +23,6 @@ public class FourBodyConstraintBenchmarks Vector3Wide positionD, QuaternionWide orientationD, BodyInertiaWide inertiaD, TPrestep prestep) where TConstraintFunctions : unmanaged, IFourBodyConstraintFunctions where TPrestep : unmanaged where TAccumulatedImpulse : unmanaged { - var functions = default(TConstraintFunctions); var accumulatedImpulse = default(TAccumulatedImpulse); var velocityA = default(BodyVelocityWide); var velocityB = default(BodyVelocityWide); @@ -35,9 +34,9 @@ public class FourBodyConstraintBenchmarks const float dt = 1f / inverseDt; for (int i = 0; i < iterations; ++i) { - functions.WarmStart(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, positionD, orientationD, inertiaD, + TConstraintFunctions.WarmStart(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, positionD, orientationD, inertiaD, ref prestep, ref accumulatedImpulse, ref velocityA, ref velocityB, ref velocityC, ref velocityD); - functions.Solve(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, positionD, orientationD, inertiaD, dt, inverseDt, + TConstraintFunctions.Solve(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, positionD, orientationD, inertiaD, dt, inverseDt, ref prestep, ref accumulatedImpulse, ref velocityA, ref velocityB, ref velocityC, ref velocityD); } return (velocityA, velocityB, velocityC, velocityD); diff --git a/DemoBenchmarks/OneBodyConstraintBenchmarks.cs b/DemoBenchmarks/OneBodyConstraintBenchmarks.cs index 27050f0a1..440d028e9 100644 --- a/DemoBenchmarks/OneBodyConstraintBenchmarks.cs +++ b/DemoBenchmarks/OneBodyConstraintBenchmarks.cs @@ -19,7 +19,6 @@ public class OneBodyConstraintBenchmarks public static BodyVelocityWide BenchmarkOneBodyConstraint(Vector3Wide positionA, QuaternionWide orientationA, BodyInertiaWide inertiaA, TPrestep prestep) where TConstraintFunctions : unmanaged, IOneBodyConstraintFunctions where TPrestep : unmanaged where TAccumulatedImpulse : unmanaged { - var functions = default(TConstraintFunctions); var accumulatedImpulse = default(TAccumulatedImpulse); var velocityA = default(BodyVelocityWide); //Individual constraint iterations are often extremely cheap, so beef the benchmark up a bit. @@ -28,8 +27,8 @@ public static BodyVelocityWide BenchmarkOneBodyConstraint() where TShape : unmanaged, IShape { ref var iteration = ref iterations[i]; float maximumT = float.MaxValue; - shapes[default(TShape).TypeId].RayTest(0, iteration.Pose, iteration.Ray, ref maximumT, ref hitHandler); + shapes[TShape.TypeId].RayTest(0, iteration.Pose, iteration.Ray, ref maximumT, ref hitHandler); } return hitHandler.ResultSum; } diff --git a/DemoBenchmarks/SweepBenchmarks.cs b/DemoBenchmarks/SweepBenchmarks.cs index 29e2af687..69155c412 100644 --- a/DemoBenchmarks/SweepBenchmarks.cs +++ b/DemoBenchmarks/SweepBenchmarks.cs @@ -122,8 +122,8 @@ public bool AllowTest(int childA, int childB) public unsafe Vector3 Test() where TA : unmanaged, IShape where TB : unmanaged, IShape { var task = taskRegistry.GetTask(); - var aType = default(TA).TypeId; - var bType = default(TB).TypeId; + var aType = TA.TypeId; + var bType = TB.TypeId; shapes[aType].GetShapeData(0, out var aData, out _); shapes[bType].GetShapeData(0, out var bData, out _); var filter = default(Filter); diff --git a/DemoBenchmarks/ThreeBodyConstraintBenchmarks.cs b/DemoBenchmarks/ThreeBodyConstraintBenchmarks.cs index 2ea149c5e..2b19ec0b4 100644 --- a/DemoBenchmarks/ThreeBodyConstraintBenchmarks.cs +++ b/DemoBenchmarks/ThreeBodyConstraintBenchmarks.cs @@ -22,7 +22,6 @@ public class ThreeBodyConstraintBenchmarks Vector3Wide positionC, QuaternionWide orientationC, BodyInertiaWide inertiaC, TPrestep prestep) where TConstraintFunctions : unmanaged, IThreeBodyConstraintFunctions where TPrestep : unmanaged where TAccumulatedImpulse : unmanaged { - var functions = default(TConstraintFunctions); var accumulatedImpulse = default(TAccumulatedImpulse); var velocityA = default(BodyVelocityWide); var velocityB = default(BodyVelocityWide); @@ -33,8 +32,8 @@ public class ThreeBodyConstraintBenchmarks const float dt = 1f / inverseDt; for (int i = 0; i < iterations; ++i) { - functions.WarmStart(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, ref prestep, ref accumulatedImpulse, ref velocityA, ref velocityB, ref velocityC); - functions.Solve(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, dt, inverseDt, ref prestep, ref accumulatedImpulse, ref velocityA, ref velocityB, ref velocityC); + TConstraintFunctions.WarmStart(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, ref prestep, ref accumulatedImpulse, ref velocityA, ref velocityB, ref velocityC); + TConstraintFunctions.Solve(positionA, orientationA, inertiaA, positionB, orientationB, inertiaB, positionC, orientationC, inertiaC, dt, inverseDt, ref prestep, ref accumulatedImpulse, ref velocityA, ref velocityB, ref velocityC); } return (velocityA, velocityB, velocityC); } diff --git a/DemoBenchmarks/TwoBodyConstraintBenchmarks.cs b/DemoBenchmarks/TwoBodyConstraintBenchmarks.cs index 3e133e5c4..9658d81e4 100644 --- a/DemoBenchmarks/TwoBodyConstraintBenchmarks.cs +++ b/DemoBenchmarks/TwoBodyConstraintBenchmarks.cs @@ -21,7 +21,6 @@ public static (BodyVelocityWide, BodyVelocityWide) BenchmarkTwoBodyConstraint where TPrestep : unmanaged where TAccumulatedImpulse : unmanaged { - var functions = default(TConstraintFunctions); var accumulatedImpulse = default(TAccumulatedImpulse); var velocityA = default(BodyVelocityWide); var velocityB = default(BodyVelocityWide); @@ -31,8 +30,8 @@ public static (BodyVelocityWide, BodyVelocityWide) BenchmarkTwoBodyConstraint { - public int LinesPerConstraint => 2; + public static int LinesPerConstraint => 2; - public unsafe void ExtractLines(ref AngularSwivelHingePrestepData prestepBundle, int setIndex, int* bodyIndices, + public static unsafe void ExtractLines(ref AngularSwivelHingePrestepData prestepBundle, int setIndex, int* bodyIndices, Bodies bodies, ref Vector3 tint, ref QuickList lines) { //Could do bundles of constraints at a time, but eh. diff --git a/DemoRenderer/Constraints/BallSocketLineExtractor.cs b/DemoRenderer/Constraints/BallSocketLineExtractor.cs index 939775d2b..13d9781de 100644 --- a/DemoRenderer/Constraints/BallSocketLineExtractor.cs +++ b/DemoRenderer/Constraints/BallSocketLineExtractor.cs @@ -8,9 +8,9 @@ namespace DemoRenderer.Constraints { struct BallSocketLineExtractor : IConstraintLineExtractor { - public int LinesPerConstraint => 3; + public static int LinesPerConstraint => 3; - public unsafe void ExtractLines(ref BallSocketPrestepData prestepBundle, int setIndex, int* bodyIndices, + public static unsafe void ExtractLines(ref BallSocketPrestepData prestepBundle, int setIndex, int* bodyIndices, Bodies bodies, ref Vector3 tint, ref QuickList lines) { //Could do bundles of constraints at a time, but eh. diff --git a/DemoRenderer/Constraints/BallSocketMotorLineExtractor.cs b/DemoRenderer/Constraints/BallSocketMotorLineExtractor.cs index ccb9514f7..e6a79c23c 100644 --- a/DemoRenderer/Constraints/BallSocketMotorLineExtractor.cs +++ b/DemoRenderer/Constraints/BallSocketMotorLineExtractor.cs @@ -8,9 +8,9 @@ namespace DemoRenderer.Constraints { struct BallSocketMotorLineExtractor : IConstraintLineExtractor { - public int LinesPerConstraint => 2; + public static int LinesPerConstraint => 2; - public unsafe void ExtractLines(ref BallSocketMotorPrestepData prestepBundle, int setIndex, int* bodyIndices, + public static unsafe void ExtractLines(ref BallSocketMotorPrestepData prestepBundle, int setIndex, int* bodyIndices, Bodies bodies, ref Vector3 tint, ref QuickList lines) { //Could do bundles of constraints at a time, but eh. diff --git a/DemoRenderer/Constraints/BallSocketServoLineExtractor.cs b/DemoRenderer/Constraints/BallSocketServoLineExtractor.cs index 9536838be..9033faf74 100644 --- a/DemoRenderer/Constraints/BallSocketServoLineExtractor.cs +++ b/DemoRenderer/Constraints/BallSocketServoLineExtractor.cs @@ -8,9 +8,9 @@ namespace DemoRenderer.Constraints { struct BallSocketServoLineExtractor : IConstraintLineExtractor { - public int LinesPerConstraint => 3; + public static int LinesPerConstraint => 3; - public unsafe void ExtractLines(ref BallSocketServoPrestepData prestepBundle, int setIndex, int* bodyIndices, + public static unsafe void ExtractLines(ref BallSocketServoPrestepData prestepBundle, int setIndex, int* bodyIndices, Bodies bodies, ref Vector3 tint, ref QuickList lines) { //Could do bundles of constraints at a time, but eh. diff --git a/DemoRenderer/Constraints/CenterDistanceLimitLineExtractor.cs b/DemoRenderer/Constraints/CenterDistanceLimitLineExtractor.cs index 72539a275..eb568a752 100644 --- a/DemoRenderer/Constraints/CenterDistanceLimitLineExtractor.cs +++ b/DemoRenderer/Constraints/CenterDistanceLimitLineExtractor.cs @@ -9,9 +9,9 @@ namespace DemoRenderer.Constraints { struct CenterDistanceLimitLineExtractor : IConstraintLineExtractor { - public int LinesPerConstraint => 5; + public static int LinesPerConstraint => 5; - public unsafe void ExtractLines(ref CenterDistanceLimitPrestepData prestepBundle, int setIndex, int* bodyIndices, + public static unsafe void ExtractLines(ref CenterDistanceLimitPrestepData prestepBundle, int setIndex, int* bodyIndices, Bodies bodies, ref Vector3 tint, ref QuickList lines) { //Could do bundles of constraints at a time, but eh. diff --git a/DemoRenderer/Constraints/CenterDistanceLineExtractor.cs b/DemoRenderer/Constraints/CenterDistanceLineExtractor.cs index 1bd003c41..498754b3b 100644 --- a/DemoRenderer/Constraints/CenterDistanceLineExtractor.cs +++ b/DemoRenderer/Constraints/CenterDistanceLineExtractor.cs @@ -9,9 +9,9 @@ namespace DemoRenderer.Constraints { struct CenterDistanceLineExtractor : IConstraintLineExtractor { - public int LinesPerConstraint => 2; + public static int LinesPerConstraint => 2; - public unsafe void ExtractLines(ref CenterDistancePrestepData prestepBundle, int setIndex, int* bodyIndices, + public static unsafe void ExtractLines(ref CenterDistancePrestepData prestepBundle, int setIndex, int* bodyIndices, Bodies bodies, ref Vector3 tint, ref QuickList lines) { //Could do bundles of constraints at a time, but eh. diff --git a/DemoRenderer/Constraints/ConstraintLineExtractor.cs b/DemoRenderer/Constraints/ConstraintLineExtractor.cs index 6dcc7b2cf..de38268b5 100644 --- a/DemoRenderer/Constraints/ConstraintLineExtractor.cs +++ b/DemoRenderer/Constraints/ConstraintLineExtractor.cs @@ -16,9 +16,9 @@ namespace DemoRenderer.Constraints { unsafe interface IConstraintLineExtractor { - int LinesPerConstraint { get; } + static abstract int LinesPerConstraint { get; } - void ExtractLines(ref TPrestep prestepBundle, int setIndex, int* bodyLocations, Bodies bodies, ref Vector3 tint, ref QuickList lines); + static abstract void ExtractLines(ref TPrestep prestepBundle, int setIndex, int* bodyLocations, Bodies bodies, ref Vector3 tint, ref QuickList lines); } abstract class TypeLineExtractor { @@ -31,7 +31,7 @@ class TypeLineExtractor : Ty where TPrestep : unmanaged where T : struct, IConstraintLineExtractor { - public override int LinesPerConstraint => default(T).LinesPerConstraint; + public override int LinesPerConstraint => T.LinesPerConstraint; public unsafe override void ExtractLines(Bodies bodies, int setIndex, ref TypeBatch typeBatch, int constraintStart, int constraintCount, ref QuickList lines) { @@ -42,7 +42,6 @@ public unsafe override void ExtractLines(Bodies bodies, int setIndex, ref TypeBa var bodyCount = Unsafe.SizeOf() / Unsafe.SizeOf>(); Debug.Assert(bodyCount * Unsafe.SizeOf>() == Unsafe.SizeOf()); var bodyIndices = stackalloc int[bodyCount]; - var extractor = default(T); var constraintEnd = constraintStart + constraintCount; if (setIndex == 0) @@ -61,7 +60,7 @@ public unsafe override void ExtractLines(Bodies bodies, int setIndex, ref TypeBa //Active set constraint body references refer directly to the body index. bodyIndices[j] = GatherScatter.Get(ref Unsafe.Add(ref firstReference, j), innerIndex) & Bodies.BodyReferenceMask; } - extractor.ExtractLines(ref GatherScatter.GetOffsetInstance(ref prestepBundle, innerIndex), setIndex, bodyIndices, bodies, ref tint, ref lines); + T.ExtractLines(ref GatherScatter.GetOffsetInstance(ref prestepBundle, innerIndex), setIndex, bodyIndices, bodies, ref tint, ref lines); } } } @@ -83,7 +82,7 @@ public unsafe override void ExtractLines(Bodies bodies, int setIndex, ref TypeBa Debug.Assert(bodies.HandleToLocation[bodyHandle].SetIndex == setIndex); bodyIndices[j] = bodies.HandleToLocation[bodyHandle].Index & Bodies.BodyReferenceMask; } - extractor.ExtractLines(ref GatherScatter.GetOffsetInstance(ref prestepBundle, innerIndex), setIndex, bodyIndices, bodies, ref tint, ref lines); + T.ExtractLines(ref GatherScatter.GetOffsetInstance(ref prestepBundle, innerIndex), setIndex, bodyIndices, bodies, ref tint, ref lines); } } } diff --git a/DemoRenderer/Constraints/ContactLineExtractors.cs b/DemoRenderer/Constraints/ContactLineExtractors.cs index f142b608b..073ba6b59 100644 --- a/DemoRenderer/Constraints/ContactLineExtractors.cs +++ b/DemoRenderer/Constraints/ContactLineExtractors.cs @@ -9,9 +9,9 @@ namespace BepuPhysics.Constraints.Contact { struct Contact1OneBodyLineExtractor : IConstraintLineExtractor { - public int LinesPerConstraint => 2; + public static int LinesPerConstraint => 2; - public unsafe void ExtractLines(ref Contact1OneBodyPrestepData prestepBundle, int setIndex, int* bodyIndices, + public static unsafe void ExtractLines(ref Contact1OneBodyPrestepData prestepBundle, int setIndex, int* bodyIndices, Bodies bodies, ref Vector3 tint, ref QuickList lines) { ref var poseA = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[0]].Motion.Pose; @@ -20,9 +20,9 @@ public unsafe void ExtractLines(ref Contact1OneBodyPrestepData prestepBundle, in } struct Contact2OneBodyLineExtractor : IConstraintLineExtractor { - public int LinesPerConstraint => 4; + public static int LinesPerConstraint => 4; - public unsafe void ExtractLines(ref Contact2OneBodyPrestepData prestepBundle, int setIndex, int* bodyIndices, + public static unsafe void ExtractLines(ref Contact2OneBodyPrestepData prestepBundle, int setIndex, int* bodyIndices, Bodies bodies, ref Vector3 tint, ref QuickList lines) { ref var poseA = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[0]].Motion.Pose; @@ -32,9 +32,9 @@ public unsafe void ExtractLines(ref Contact2OneBodyPrestepData prestepBundle, in } struct Contact3OneBodyLineExtractor : IConstraintLineExtractor { - public int LinesPerConstraint => 6; + public static int LinesPerConstraint => 6; - public unsafe void ExtractLines(ref Contact3OneBodyPrestepData prestepBundle, int setIndex, int* bodyIndices, + public static unsafe void ExtractLines(ref Contact3OneBodyPrestepData prestepBundle, int setIndex, int* bodyIndices, Bodies bodies, ref Vector3 tint, ref QuickList lines) { ref var poseA = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[0]].Motion.Pose; @@ -45,9 +45,9 @@ public unsafe void ExtractLines(ref Contact3OneBodyPrestepData prestepBundle, in } struct Contact4OneBodyLineExtractor : IConstraintLineExtractor { - public int LinesPerConstraint => 8; + public static int LinesPerConstraint => 8; - public unsafe void ExtractLines(ref Contact4OneBodyPrestepData prestepBundle, int setIndex, int* bodyIndices, + public static unsafe void ExtractLines(ref Contact4OneBodyPrestepData prestepBundle, int setIndex, int* bodyIndices, Bodies bodies, ref Vector3 tint, ref QuickList lines) { ref var poseA = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[0]].Motion.Pose; @@ -59,9 +59,9 @@ public unsafe void ExtractLines(ref Contact4OneBodyPrestepData prestepBundle, in } struct Contact1LineExtractor : IConstraintLineExtractor { - public int LinesPerConstraint => 2; + public static int LinesPerConstraint => 2; - public unsafe void ExtractLines(ref Contact1PrestepData prestepBundle, int setIndex, int* bodyIndices, + public static unsafe void ExtractLines(ref Contact1PrestepData prestepBundle, int setIndex, int* bodyIndices, Bodies bodies, ref Vector3 tint, ref QuickList lines) { ref var poseA = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[0]].Motion.Pose; @@ -70,9 +70,9 @@ public unsafe void ExtractLines(ref Contact1PrestepData prestepBundle, int setIn } struct Contact2LineExtractor : IConstraintLineExtractor { - public int LinesPerConstraint => 4; + public static int LinesPerConstraint => 4; - public unsafe void ExtractLines(ref Contact2PrestepData prestepBundle, int setIndex, int* bodyIndices, + public static unsafe void ExtractLines(ref Contact2PrestepData prestepBundle, int setIndex, int* bodyIndices, Bodies bodies, ref Vector3 tint, ref QuickList lines) { ref var poseA = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[0]].Motion.Pose; @@ -82,9 +82,9 @@ public unsafe void ExtractLines(ref Contact2PrestepData prestepBundle, int setIn } struct Contact3LineExtractor : IConstraintLineExtractor { - public int LinesPerConstraint => 6; + public static int LinesPerConstraint => 6; - public unsafe void ExtractLines(ref Contact3PrestepData prestepBundle, int setIndex, int* bodyIndices, + public static unsafe void ExtractLines(ref Contact3PrestepData prestepBundle, int setIndex, int* bodyIndices, Bodies bodies, ref Vector3 tint, ref QuickList lines) { ref var poseA = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[0]].Motion.Pose; @@ -95,9 +95,9 @@ public unsafe void ExtractLines(ref Contact3PrestepData prestepBundle, int setIn } struct Contact4LineExtractor : IConstraintLineExtractor { - public int LinesPerConstraint => 8; + public static int LinesPerConstraint => 8; - public unsafe void ExtractLines(ref Contact4PrestepData prestepBundle, int setIndex, int* bodyIndices, + public static unsafe void ExtractLines(ref Contact4PrestepData prestepBundle, int setIndex, int* bodyIndices, Bodies bodies, ref Vector3 tint, ref QuickList lines) { ref var poseA = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[0]].Motion.Pose; @@ -109,9 +109,9 @@ public unsafe void ExtractLines(ref Contact4PrestepData prestepBundle, int setIn } struct Contact2NonconvexOneBodyLineExtractor : IConstraintLineExtractor { - public int LinesPerConstraint => 4; + public static int LinesPerConstraint => 4; - public unsafe void ExtractLines(ref Contact2NonconvexOneBodyPrestepData prestepBundle, int setIndex, int* bodyIndices, + public static unsafe void ExtractLines(ref Contact2NonconvexOneBodyPrestepData prestepBundle, int setIndex, int* bodyIndices, Bodies bodies, ref Vector3 tint, ref QuickList lines) { ref var poseA = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[0]].Motion.Pose; @@ -121,9 +121,9 @@ public unsafe void ExtractLines(ref Contact2NonconvexOneBodyPrestepData prestepB } struct Contact3NonconvexOneBodyLineExtractor : IConstraintLineExtractor { - public int LinesPerConstraint => 6; + public static int LinesPerConstraint => 6; - public unsafe void ExtractLines(ref Contact3NonconvexOneBodyPrestepData prestepBundle, int setIndex, int* bodyIndices, + public static unsafe void ExtractLines(ref Contact3NonconvexOneBodyPrestepData prestepBundle, int setIndex, int* bodyIndices, Bodies bodies, ref Vector3 tint, ref QuickList lines) { ref var poseA = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[0]].Motion.Pose; @@ -134,9 +134,9 @@ public unsafe void ExtractLines(ref Contact3NonconvexOneBodyPrestepData prestepB } struct Contact4NonconvexOneBodyLineExtractor : IConstraintLineExtractor { - public int LinesPerConstraint => 8; + public static int LinesPerConstraint => 8; - public unsafe void ExtractLines(ref Contact4NonconvexOneBodyPrestepData prestepBundle, int setIndex, int* bodyIndices, + public static unsafe void ExtractLines(ref Contact4NonconvexOneBodyPrestepData prestepBundle, int setIndex, int* bodyIndices, Bodies bodies, ref Vector3 tint, ref QuickList lines) { ref var poseA = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[0]].Motion.Pose; @@ -148,9 +148,9 @@ public unsafe void ExtractLines(ref Contact4NonconvexOneBodyPrestepData prestepB } struct Contact2NonconvexLineExtractor : IConstraintLineExtractor { - public int LinesPerConstraint => 4; + public static int LinesPerConstraint => 4; - public unsafe void ExtractLines(ref Contact2NonconvexPrestepData prestepBundle, int setIndex, int* bodyIndices, + public static unsafe void ExtractLines(ref Contact2NonconvexPrestepData prestepBundle, int setIndex, int* bodyIndices, Bodies bodies, ref Vector3 tint, ref QuickList lines) { ref var poseA = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[0]].Motion.Pose; @@ -160,9 +160,9 @@ public unsafe void ExtractLines(ref Contact2NonconvexPrestepData prestepBundle, } struct Contact3NonconvexLineExtractor : IConstraintLineExtractor { - public int LinesPerConstraint => 6; + public static int LinesPerConstraint => 6; - public unsafe void ExtractLines(ref Contact3NonconvexPrestepData prestepBundle, int setIndex, int* bodyIndices, + public static unsafe void ExtractLines(ref Contact3NonconvexPrestepData prestepBundle, int setIndex, int* bodyIndices, Bodies bodies, ref Vector3 tint, ref QuickList lines) { ref var poseA = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[0]].Motion.Pose; @@ -173,9 +173,9 @@ public unsafe void ExtractLines(ref Contact3NonconvexPrestepData prestepBundle, } struct Contact4NonconvexLineExtractor : IConstraintLineExtractor { - public int LinesPerConstraint => 8; + public static int LinesPerConstraint => 8; - public unsafe void ExtractLines(ref Contact4NonconvexPrestepData prestepBundle, int setIndex, int* bodyIndices, + public static unsafe void ExtractLines(ref Contact4NonconvexPrestepData prestepBundle, int setIndex, int* bodyIndices, Bodies bodies, ref Vector3 tint, ref QuickList lines) { ref var poseA = ref bodies.Sets[setIndex].DynamicsState[bodyIndices[0]].Motion.Pose; diff --git a/DemoRenderer/Constraints/ContactLineExtractors.tt b/DemoRenderer/Constraints/ContactLineExtractors.tt index 3af241e3f..c8b15dac1 100644 --- a/DemoRenderer/Constraints/ContactLineExtractors.tt +++ b/DemoRenderer/Constraints/ContactLineExtractors.tt @@ -26,9 +26,9 @@ for (int convexity = 0; convexity <= 1; ++convexity) #> struct Contact<#=contactCount#><#=convexitySuffix#><#=bodySuffix#>LineExtractor : IConstraintLineExtractor<#=convexitySuffix#><#=bodySuffix#>PrestepData> { - public int LinesPerConstraint => <#=contactCount * 2#>; + public static int LinesPerConstraint => <#=contactCount * 2#>; - public unsafe void ExtractLines(ref Contact<#=contactCount#><#=convexitySuffix#><#=bodySuffix#>PrestepData prestepBundle, int setIndex, int* bodyIndices, + public static unsafe void ExtractLines(ref Contact<#=contactCount#><#=convexitySuffix#><#=bodySuffix#>PrestepData prestepBundle, int setIndex, int* bodyIndices, Bodies bodies, ref Vector3 tint, ref QuickList lines) { ref var poseA = ref bodies.Sets[setIndex].SolverStates[bodyIndices[0]].Motion.Pose; diff --git a/DemoRenderer/Constraints/DistanceLimitLineExtractor.cs b/DemoRenderer/Constraints/DistanceLimitLineExtractor.cs index c3804f32c..22041caca 100644 --- a/DemoRenderer/Constraints/DistanceLimitLineExtractor.cs +++ b/DemoRenderer/Constraints/DistanceLimitLineExtractor.cs @@ -8,9 +8,9 @@ namespace DemoRenderer.Constraints { struct DistanceLimitLineExtractor : IConstraintLineExtractor { - public int LinesPerConstraint => 5; + public static int LinesPerConstraint => 5; - public unsafe void ExtractLines(ref DistanceLimitPrestepData prestepBundle, int setIndex, int* bodyIndices, + public static unsafe void ExtractLines(ref DistanceLimitPrestepData prestepBundle, int setIndex, int* bodyIndices, Bodies bodies, ref Vector3 tint, ref QuickList lines) { //Could do bundles of constraints at a time, but eh. diff --git a/DemoRenderer/Constraints/DistanceServoLineExtractor.cs b/DemoRenderer/Constraints/DistanceServoLineExtractor.cs index 35ead6976..fa6e0585c 100644 --- a/DemoRenderer/Constraints/DistanceServoLineExtractor.cs +++ b/DemoRenderer/Constraints/DistanceServoLineExtractor.cs @@ -8,9 +8,9 @@ namespace DemoRenderer.Constraints { struct DistanceServoLineExtractor : IConstraintLineExtractor { - public int LinesPerConstraint => 4; + public static int LinesPerConstraint => 4; - public unsafe void ExtractLines(ref DistanceServoPrestepData prestepBundle, int setIndex, int* bodyIndices, + public static unsafe void ExtractLines(ref DistanceServoPrestepData prestepBundle, int setIndex, int* bodyIndices, Bodies bodies, ref Vector3 tint, ref QuickList lines) { //Could do bundles of constraints at a time, but eh. diff --git a/DemoRenderer/Constraints/HingeLineExtractor.cs b/DemoRenderer/Constraints/HingeLineExtractor.cs index f92b185c0..a7e29b360 100644 --- a/DemoRenderer/Constraints/HingeLineExtractor.cs +++ b/DemoRenderer/Constraints/HingeLineExtractor.cs @@ -8,9 +8,9 @@ namespace DemoRenderer.Constraints { struct HingeLineExtractor : IConstraintLineExtractor { - public int LinesPerConstraint => 4; + public static int LinesPerConstraint => 4; - public unsafe void ExtractLines(ref HingePrestepData prestepBundle, int setIndex, int* bodyIndices, + public static unsafe void ExtractLines(ref HingePrestepData prestepBundle, int setIndex, int* bodyIndices, Bodies bodies, ref Vector3 tint, ref QuickList lines) { //Could do bundles of constraints at a time, but eh. diff --git a/DemoRenderer/Constraints/LinearAxisServoLineExtractor.cs b/DemoRenderer/Constraints/LinearAxisServoLineExtractor.cs index 83eb272f5..65f39304b 100644 --- a/DemoRenderer/Constraints/LinearAxisServoLineExtractor.cs +++ b/DemoRenderer/Constraints/LinearAxisServoLineExtractor.cs @@ -8,9 +8,9 @@ namespace DemoRenderer.Constraints { struct LinearAxisServoLineExtractor : IConstraintLineExtractor { - public int LinesPerConstraint => 7; + public static int LinesPerConstraint => 7; - public unsafe void ExtractLines(ref LinearAxisServoPrestepData prestepBundle, int setIndex, int* bodyIndices, + public static unsafe void ExtractLines(ref LinearAxisServoPrestepData prestepBundle, int setIndex, int* bodyIndices, Bodies bodies, ref Vector3 tint, ref QuickList lines) { //Could do bundles of constraints at a time, but eh. diff --git a/DemoRenderer/Constraints/OneBodyLinearServoLineExtractor.cs b/DemoRenderer/Constraints/OneBodyLinearServoLineExtractor.cs index 119f276c2..1b1224ddb 100644 --- a/DemoRenderer/Constraints/OneBodyLinearServoLineExtractor.cs +++ b/DemoRenderer/Constraints/OneBodyLinearServoLineExtractor.cs @@ -8,9 +8,9 @@ namespace DemoRenderer.Constraints { struct OneBodyLinearServoLineExtractor : IConstraintLineExtractor { - public int LinesPerConstraint => 2; + public static int LinesPerConstraint => 2; - public unsafe void ExtractLines(ref OneBodyLinearServoPrestepData prestepBundle, int setIndex, int* bodyIndices, + public static unsafe void ExtractLines(ref OneBodyLinearServoPrestepData prestepBundle, int setIndex, int* bodyIndices, Bodies bodies, ref Vector3 tint, ref QuickList lines) { //Could do bundles of constraints at a time, but eh. diff --git a/DemoRenderer/Constraints/PointOnLineLineExtractor.cs b/DemoRenderer/Constraints/PointOnLineLineExtractor.cs index 5befbff46..ba42d0d0e 100644 --- a/DemoRenderer/Constraints/PointOnLineLineExtractor.cs +++ b/DemoRenderer/Constraints/PointOnLineLineExtractor.cs @@ -8,9 +8,9 @@ namespace DemoRenderer.Constraints { struct PointOnLineLineExtractor : IConstraintLineExtractor { - public int LinesPerConstraint => 4; + public static int LinesPerConstraint => 4; - public unsafe void ExtractLines(ref PointOnLineServoPrestepData prestepBundle, int setIndex, int* bodyIndices, + public static unsafe void ExtractLines(ref PointOnLineServoPrestepData prestepBundle, int setIndex, int* bodyIndices, Bodies bodies, ref Vector3 tint, ref QuickList lines) { //Could do bundles of constraints at a time, but eh. diff --git a/DemoRenderer/Constraints/SwivelHingeLineExtractor.cs b/DemoRenderer/Constraints/SwivelHingeLineExtractor.cs index 127dd333d..24755ac82 100644 --- a/DemoRenderer/Constraints/SwivelHingeLineExtractor.cs +++ b/DemoRenderer/Constraints/SwivelHingeLineExtractor.cs @@ -8,9 +8,9 @@ namespace DemoRenderer.Constraints { struct SwivelHingeLineExtractor : IConstraintLineExtractor { - public int LinesPerConstraint => 5; + public static int LinesPerConstraint => 5; - public unsafe void ExtractLines(ref SwivelHingePrestepData prestepBundle, int setIndex, int* bodyIndices, + public static unsafe void ExtractLines(ref SwivelHingePrestepData prestepBundle, int setIndex, int* bodyIndices, Bodies bodies, ref Vector3 tint, ref QuickList lines) { //Could do bundles of constraints at a time, but eh. diff --git a/DemoRenderer/Constraints/WeldLineExtractor.cs b/DemoRenderer/Constraints/WeldLineExtractor.cs index cbe7bce03..4a5fda742 100644 --- a/DemoRenderer/Constraints/WeldLineExtractor.cs +++ b/DemoRenderer/Constraints/WeldLineExtractor.cs @@ -8,9 +8,9 @@ namespace DemoRenderer.Constraints { struct WeldLineExtractor : IConstraintLineExtractor { - public int LinesPerConstraint => 2; + public static int LinesPerConstraint => 2; - public unsafe void ExtractLines(ref WeldPrestepData prestepBundle, int setIndex, int* bodyIndices, + public static unsafe void ExtractLines(ref WeldPrestepData prestepBundle, int setIndex, int* bodyIndices, Bodies bodies, ref Vector3 tint, ref QuickList lines) { //Could do bundles of constraints at a time, but eh. diff --git a/DemoRenderer/ShapeDrawing/ShapesExtractor.cs b/DemoRenderer/ShapeDrawing/ShapesExtractor.cs index 66348fb2f..409125924 100644 --- a/DemoRenderer/ShapeDrawing/ShapesExtractor.cs +++ b/DemoRenderer/ShapeDrawing/ShapesExtractor.cs @@ -280,7 +280,7 @@ public unsafe void AddShape(Shapes shapes, TypedIndex shapeIndex, RigidPose pose [MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe void AddShape(TShape shape, Shapes shapes, RigidPose pose, Vector3 color) where TShape : IShape { - AddShape(Unsafe.AsPointer(ref shape), shape.TypeId, shapes, pose, color, ref ShapeCache, pool); + AddShape(Unsafe.AsPointer(ref shape), TShape.TypeId, shapes, pose, color, ref ShapeCache, pool); } [MethodImpl(MethodImplOptions.AggressiveInlining)] diff --git a/Demos/Demos/Characters/CharacterMotionConstraint.cs b/Demos/Demos/Characters/CharacterMotionConstraint.cs index 132564382..1b4660a77 100644 --- a/Demos/Demos/Characters/CharacterMotionConstraint.cs +++ b/Demos/Demos/Characters/CharacterMotionConstraint.cs @@ -62,16 +62,16 @@ public struct StaticCharacterMotionConstraint : IOneBodyConstraintDescription /// Gets the constraint type id that this description is associated with. /// - public readonly int ConstraintTypeId => StaticCharacterMotionTypeProcessor.BatchTypeId; + public static int ConstraintTypeId => StaticCharacterMotionTypeProcessor.BatchTypeId; /// /// Gets the TypeProcessor type that is associated with this description. /// - public readonly Type TypeProcessorType => typeof(StaticCharacterMotionTypeProcessor); + public static Type TypeProcessorType => typeof(StaticCharacterMotionTypeProcessor); /// /// Creates a type processor for this constraint type. /// - public readonly TypeProcessor CreateTypeProcessor() => new StaticCharacterMotionTypeProcessor(); + public static TypeProcessor CreateTypeProcessor() => new StaticCharacterMotionTypeProcessor(); //Note that these mapping functions use a "GetOffsetInstance" function. Each CharacterMotionPrestep is a bundle of multiple constraints; //by grabbing an offset instance, we're selecting a specific slot in the bundle to modify. For simplicity and to guarantee consistency of field strides, @@ -88,7 +88,7 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int Vector3Wide.WriteFirst(OffsetFromCharacterToSupportPoint, ref target.OffsetFromCharacter); } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out StaticCharacterMotionConstraint description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out StaticCharacterMotionConstraint description) { ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); QuaternionWide.ReadFirst(source.SurfaceBasis, out description.SurfaceBasis); @@ -187,7 +187,7 @@ private static void ApplyVerticalImpulse(in Matrix3x3Wide basis, } - public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref StaticCharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide velocityA) + public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, ref StaticCharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide velocityA) { ComputeJacobians(prestep.OffsetFromCharacter, prestep.SurfaceBasis, out var basis, out var horizontalAngularJacobianA, out var verticalAngularJacobianA); @@ -195,7 +195,7 @@ public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, ApplyVerticalImpulse(basis, verticalAngularJacobianA, accumulatedImpulses.Vertical, inertiaA, ref velocityA); } - public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref StaticCharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide velocityA) + public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, float dt, float inverseDt, ref StaticCharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide velocityA) { //The motion constraint is split into two parts: the horizontal constraint, and the vertical constraint. //The horizontal constraint acts almost exactly like the TangentFriction, but we'll duplicate some of the logic to keep this implementation self-contained. @@ -263,8 +263,8 @@ public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in B } - public bool RequiresIncrementalSubstepUpdates => true; - public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide velocityA, ref StaticCharacterMotionPrestep prestep) + public static bool RequiresIncrementalSubstepUpdates => true; + public static void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide velocityA, ref StaticCharacterMotionPrestep prestep) { //Since collision detection doesn't run for every substep, we approximate the change in depth for the vertical motion constraint by integrating the velocity along the support normal. //This is pretty subtle. If you disable it entirely (return false from "RequiresIncrementalSubstepUpdates" above), you might not even notice. @@ -335,16 +335,16 @@ public struct DynamicCharacterMotionConstraint : ITwoBodyConstraintDescription /// Gets the constraint type id that this description is associated with. /// - public readonly int ConstraintTypeId => DynamicCharacterMotionTypeProcessor.BatchTypeId; + public static int ConstraintTypeId => DynamicCharacterMotionTypeProcessor.BatchTypeId; /// /// Gets the TypeProcessor type that is associated with this description. /// - public readonly Type TypeProcessorType => typeof(DynamicCharacterMotionTypeProcessor); + public static Type TypeProcessorType => typeof(DynamicCharacterMotionTypeProcessor); /// /// Creates a type processor for this constraint type. /// - public readonly TypeProcessor CreateTypeProcessor() => new DynamicCharacterMotionTypeProcessor(); + public static TypeProcessor CreateTypeProcessor() => new DynamicCharacterMotionTypeProcessor(); //Note that these mapping functions use a "GetOffsetInstance" function. Each CharacterMotionPrestep is a bundle of multiple constraints; //by grabbing an offset instance, we're selecting a specific slot in the bundle to modify. For simplicity and to guarantee consistency of field strides, @@ -362,7 +362,7 @@ public readonly void ApplyDescription(ref TypeBatch batch, int bundleIndex, int Vector3Wide.WriteFirst(OffsetFromSupportToSupportPoint, ref target.OffsetFromSupport); } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out DynamicCharacterMotionConstraint description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out DynamicCharacterMotionConstraint description) { ref var source = ref GetOffsetInstance(ref Buffer.Get(ref batch.PrestepData, bundleIndex), innerIndex); QuaternionWide.ReadFirst(source.SurfaceBasis, out description.SurfaceBasis); @@ -476,7 +476,7 @@ private static void ApplyVerticalImpulse(in Matrix3x3Wide basis, } - public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref DynamicCharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB) + public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, ref DynamicCharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB) { ComputeJacobians(prestep.OffsetFromCharacter, prestep.OffsetFromSupport, prestep.SurfaceBasis, out var basis, out var horizontalAngularJacobianA, out var horizontalAngularJacobianB, out var verticalAngularJacobianA, out var verticalAngularJacobianB); @@ -484,7 +484,7 @@ public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, ApplyVerticalImpulse(basis, verticalAngularJacobianA, verticalAngularJacobianB, accumulatedImpulses.Vertical, inertiaA, inertiaB, ref velocityA, ref velocityB); } - public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref DynamicCharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB) + public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, float dt, float inverseDt, ref DynamicCharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide velocityA, ref BodyVelocityWide velocityB) { //The motion constraint is split into two parts: the horizontal constraint, and the vertical constraint. //The horizontal constraint acts almost exactly like the TangentFriction, but we'll duplicate some of the logic to keep this implementation self-contained. @@ -564,8 +564,8 @@ public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in B } - public bool RequiresIncrementalSubstepUpdates => true; - public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide velocityA, in BodyVelocityWide velocityB, ref DynamicCharacterMotionPrestep prestep) + public static bool RequiresIncrementalSubstepUpdates => true; + public static void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide velocityA, in BodyVelocityWide velocityB, ref DynamicCharacterMotionPrestep prestep) { //Since collision detection doesn't run for every substep, we approximate the change in depth for the vertical motion constraint by integrating the velocity along the support normal. //This is pretty subtle. If you disable it entirely (return false from "RequiresIncrementalSubstepUpdates" above), you might not even notice. diff --git a/Demos/Demos/Characters/CharacterMotionConstraint.tt b/Demos/Demos/Characters/CharacterMotionConstraint.tt index f045bb02b..12cad4039 100644 --- a/Demos/Demos/Characters/CharacterMotionConstraint.tt +++ b/Demos/Demos/Characters/CharacterMotionConstraint.tt @@ -79,16 +79,16 @@ namespace Demos.Demos.Characters /// /// Gets the constraint type id that this description is associated with. /// - public readonly int ConstraintTypeId => <#=prefix#>CharacterMotionTypeProcessor.BatchTypeId; + public static int ConstraintTypeId => <#=prefix#>CharacterMotionTypeProcessor.BatchTypeId; /// /// Gets the TypeProcessor type that is associated with this description. /// - public readonly Type TypeProcessorType => typeof(<#=prefix#>CharacterMotionTypeProcessor); + public static Type TypeProcessorType => typeof(<#=prefix#>CharacterMotionTypeProcessor); /// /// Creates a type processor for this constraint type. /// - public readonly TypeProcessor CreateTypeProcessor() => new <#=prefix#>CharacterMotionTypeProcessor(); + public static TypeProcessor CreateTypeProcessor() => new <#=prefix#>CharacterMotionTypeProcessor(); //Note that these mapping functions use a "GetOffsetInstance" function. Each CharacterMotionPrestep is a bundle of multiple constraints; //by grabbing an offset instance, we're selecting a specific slot in the bundle to modify. For simplicity and to guarantee consistency of field strides, @@ -108,7 +108,7 @@ namespace Demos.Demos.Characters <#}#> } - public readonly void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out <#=prefix#>CharacterMotionConstraint description) + public static void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out <#=prefix#>CharacterMotionConstraint description) { ref var source = ref GetOffsetInstance(ref Buffer<<#=prefix#>CharacterMotionPrestep>.Get(ref batch.PrestepData, bundleIndex), innerIndex); QuaternionWide.ReadFirst(source.SurfaceBasis, out description.SurfaceBasis); @@ -239,7 +239,7 @@ namespace Demos.Demos.Characters } - public void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(dynamic) {#>in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>ref <#=prefix#>CharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide velocityA<#if(dynamic) {#>, ref BodyVelocityWide velocityB<#}#>) + public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(dynamic) {#>in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>ref <#=prefix#>CharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide velocityA<#if(dynamic) {#>, ref BodyVelocityWide velocityB<#}#>) { ComputeJacobians(prestep.OffsetFromCharacter, <#if (dynamic) {#>prestep.OffsetFromSupport, <#}#>prestep.SurfaceBasis, out var basis, out var horizontalAngularJacobianA, <#if (dynamic) {#>out var horizontalAngularJacobianB, <#}#>out var verticalAngularJacobianA<#if (dynamic) {#>, out var verticalAngularJacobianB<#}#>); @@ -247,7 +247,7 @@ namespace Demos.Demos.Characters ApplyVerticalImpulse(basis, verticalAngularJacobianA, <#if (dynamic) {#>verticalAngularJacobianB, <#}#>accumulatedImpulses.Vertical, inertiaA, <#if (dynamic) {#>inertiaB, <#}#>ref velocityA<#if (dynamic) {#>, ref velocityB<#}#>); } - public void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(dynamic) {#>in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>float dt, float inverseDt, ref <#=prefix#>CharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide velocityA<#if(dynamic) {#>, ref BodyVelocityWide velocityB<#}#>) + public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, <#if(dynamic) {#>in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, <#}#>float dt, float inverseDt, ref <#=prefix#>CharacterMotionPrestep prestep, ref CharacterMotionAccumulatedImpulse accumulatedImpulses, ref BodyVelocityWide velocityA<#if(dynamic) {#>, ref BodyVelocityWide velocityB<#}#>) { //The motion constraint is split into two parts: the horizontal constraint, and the vertical constraint. //The horizontal constraint acts almost exactly like the TangentFriction, but we'll duplicate some of the logic to keep this implementation self-contained. @@ -339,8 +339,8 @@ namespace Demos.Demos.Characters } - public bool RequiresIncrementalSubstepUpdates => true; - public void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide velocityA, <#if(dynamic){#>in BodyVelocityWide velocityB, <#}#>ref <#=prefix#>CharacterMotionPrestep prestep) + public static bool RequiresIncrementalSubstepUpdates => true; + public static void IncrementallyUpdateForSubstep(in Vector dt, in BodyVelocityWide velocityA, <#if(dynamic){#>in BodyVelocityWide velocityB, <#}#>ref <#=prefix#>CharacterMotionPrestep prestep) { //Since collision detection doesn't run for every substep, we approximate the change in depth for the vertical motion constraint by integrating the velocity along the support normal. //This is pretty subtle. If you disable it entirely (return false from "RequiresIncrementalSubstepUpdates" above), you might not even notice. diff --git a/Demos/Demos/ClothDemo.cs b/Demos/Demos/ClothDemo.cs index c8b5528a0..08beed862 100644 --- a/Demos/Demos/ClothDemo.cs +++ b/Demos/Demos/ClothDemo.cs @@ -62,7 +62,7 @@ public ClothCallbacks(CollidableProperty filters, PairMate Material = material; MinimumDistanceForSelfCollisions = minimumDistanceForSelfCollisions; } - ClothCallbacks Dancers.IDancerNarrowPhaseCallbacks.Create(CollidableProperty filters, PairMaterialProperties pairMaterialProperties, int minimumDistanceForSelfCollisions) + static ClothCallbacks Dancers.IDancerNarrowPhaseCallbacks.Create(CollidableProperty filters, PairMaterialProperties pairMaterialProperties, int minimumDistanceForSelfCollisions) { return new ClothCallbacks(filters, pairMaterialProperties, minimumDistanceForSelfCollisions); } diff --git a/Demos/Demos/CollisionQueryDemo.cs b/Demos/Demos/CollisionQueryDemo.cs index 2925a85ca..be857a3d7 100644 --- a/Demos/Demos/CollisionQueryDemo.cs +++ b/Demos/Demos/CollisionQueryDemo.cs @@ -175,7 +175,7 @@ public unsafe void AddQueryToBatch(TShape shape, in RigidPose pose, int shape.ComputeBounds(pose.Orientation, out var boundingBoxMin, out var boundingBoxMax); boundingBoxMin += pose.Position; boundingBoxMax += pose.Position; - AddQueryToBatch(shape.TypeId, queryShapeData, queryShapeSize, boundingBoxMin, boundingBoxMax, pose, queryId, ref batcher); + AddQueryToBatch(TShape.TypeId, queryShapeData, queryShapeSize, boundingBoxMin, boundingBoxMax, pose, queryId, ref batcher); } //This version of the query isn't used in the demo, but shows how you could use a simulation-cached shape in a query. diff --git a/Demos/Demos/CustomVoxelCollidableDemo.cs b/Demos/Demos/CustomVoxelCollidableDemo.cs index 363eadcac..df8d0938b 100644 --- a/Demos/Demos/CustomVoxelCollidableDemo.cs +++ b/Demos/Demos/CustomVoxelCollidableDemo.cs @@ -26,7 +26,7 @@ namespace Demos.Demos struct Voxels : IHomogeneousCompoundShape { //Type ids should be unique across all shape types in a simulation. - public readonly int TypeId => 12; + public static int TypeId => 12; //Using an object space tree isn't necessarily ideal for a highly regular data like voxels. //We're using it here since it exists already and a voxel-specialized version doesn't. @@ -70,7 +70,7 @@ public Voxels(QuickList voxelIndices, Vector3 voxelSize, BufferPool poo pool.Return(ref bounds); } - public readonly ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches) + public static ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches) { //Shapes types are responsible for informing the shape system how to create a batch for them. //Convex shapes will return a ConvexShapeBatch, compound shapes a CompoundShapeBatch, diff --git a/Demos/Demos/Dancers/DemoDancers.cs b/Demos/Demos/Dancers/DemoDancers.cs index e524f67d8..bef182ea6 100644 --- a/Demos/Demos/Dancers/DemoDancers.cs +++ b/Demos/Demos/Dancers/DemoDancers.cs @@ -74,7 +74,7 @@ public void UpdateTarget(Simulation simulation, Vector3 target) /// Type of the callback filters to use. public interface IDancerNarrowPhaseCallbacks where TCallbacks : INarrowPhaseCallbacks, IDancerNarrowPhaseCallbacks where TFilter : unmanaged { - TCallbacks Create(CollidableProperty filters, PairMaterialProperties pairMaterialProperties, int minimumDistanceForSelfCollisions); + static abstract TCallbacks Create(CollidableProperty filters, PairMaterialProperties pairMaterialProperties, int minimumDistanceForSelfCollisions); } /// /// Description of the body to add. /// Handle of the created body. - public unsafe BodyHandle Add(in BodyDescription description) + public BodyHandle Add(in BodyDescription description) { Debug.Assert(HandleToLocation.Allocated, "The backing memory of the bodies set should be initialized before use."); var handleIndex = HandlePool.Take(); @@ -387,7 +387,7 @@ public unsafe static bool HasLockedInertia(Symmetric3x3* inertia) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - unsafe void UpdateForKinematicStateChange(BodyHandle handle, ref BodyMemoryLocation location, ref BodySet set, bool previouslyKinematic, bool currentlyKinematic) + void UpdateForKinematicStateChange(BodyHandle handle, ref BodyMemoryLocation location, ref BodySet set, bool previouslyKinematic, bool currentlyKinematic) { Debug.Assert(location.SetIndex == 0, "If we're changing kinematic state, we should have already awoken the body."); if (previouslyKinematic != currentlyKinematic) diff --git a/BepuPhysics/BodyReference.cs b/BepuPhysics/BodyReference.cs index c6e7de8bc..7dc637805 100644 --- a/BepuPhysics/BodyReference.cs +++ b/BepuPhysics/BodyReference.cs @@ -205,7 +205,7 @@ public CollidableReference CollidableReference /// /// Gets whether the body is kinematic, meaning its inverse inertia and mass are all zero. /// - public unsafe bool Kinematic { get { return Bodies.IsKinematicUnsafeGCHole(ref LocalInertia); } } + public bool Kinematic { get { return Bodies.IsKinematicUnsafeGCHole(ref LocalInertia); } } /// /// Gets whether the body has locked inertia, meaning its inverse inertia tensor is zero. diff --git a/BepuPhysics/BodySet.cs b/BepuPhysics/BodySet.cs index 1d8afac94..e994eb30d 100644 --- a/BepuPhysics/BodySet.cs +++ b/BepuPhysics/BodySet.cs @@ -229,7 +229,7 @@ public bool BodyIsConstrainedBy(int bodyIndex, ConstraintHandle constraintHandle return false; } - internal unsafe void InternalResize(int targetBodyCapacity, BufferPool pool) + internal void InternalResize(int targetBodyCapacity, BufferPool pool) { Debug.Assert(targetBodyCapacity > 0, "Resize is not meant to be used as Dispose. If you want to return everything to the pool, use Dispose instead."); //Note that we base the bundle capacities on post-resize capacity of the IndexToHandle array. This simplifies the conditions on allocation, but increases memory use. @@ -243,7 +243,7 @@ internal unsafe void InternalResize(int targetBodyCapacity, BufferPool pool) pool.ResizeToAtLeast(ref Constraints, targetBodyCapacity, Count); } - public unsafe void Clear(BufferPool pool) + public void Clear(BufferPool pool) { for (int i = 0; i < Count; ++i) { diff --git a/BepuPhysics/BoundingBoxHelpers.cs b/BepuPhysics/BoundingBoxHelpers.cs index dbf7ddfb2..5c5948c6b 100644 --- a/BepuPhysics/BoundingBoxHelpers.cs +++ b/BepuPhysics/BoundingBoxHelpers.cs @@ -173,7 +173,7 @@ public static void ExpandBoundingBox(ref Vector3 min, ref Vector3 max, Vector3 l } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe static void ExpandBoundingBox(in Vector3Wide expansion, ref Vector3Wide min, ref Vector3Wide max) + public static void ExpandBoundingBox(in Vector3Wide expansion, ref Vector3Wide min, ref Vector3Wide max) { Vector3Wide.Min(Vector.Zero, expansion, out var minExpansion); Vector3Wide.Max(Vector.Zero, expansion, out var maxExpansion); @@ -188,7 +188,7 @@ public unsafe static void ExpandBoundingBox(in Vector3Wide expansion, ref Vector /// Expands the bounding box surrounding a shape A in the local space of some other collidable B. /// [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static unsafe void ExpandLocalBoundingBoxes(ref Vector3Wide min, ref Vector3Wide max, + public static void ExpandLocalBoundingBoxes(ref Vector3Wide min, ref Vector3Wide max, in Vector radiusA, in Vector3Wide localPositionA, in Vector3Wide localRelativeLinearVelocityA, in Vector3Wide angularVelocityA, in Vector3Wide angularVelocityB, float dt, in Vector maximumRadius, in Vector maximumAngularExpansion, in Vector maximumAllowedExpansion) { @@ -218,7 +218,7 @@ public static unsafe void ExpandLocalBoundingBoxes(ref Vector3Wide min, ref Vect } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe static void ExpandBoundingBox(Vector3 expansion, ref Vector3 min, ref Vector3 max) + public static void ExpandBoundingBox(Vector3 expansion, ref Vector3 min, ref Vector3 max) { var minExpansion = Vector3.Min(default, expansion); var maxExpansion = Vector3.Max(default, expansion); @@ -229,7 +229,7 @@ public unsafe static void ExpandBoundingBox(Vector3 expansion, ref Vector3 min, /// /// Computes the bounding box of a child shape A in the local space of some other collidable B with a sweep direction representing the net linear motion. /// - public static unsafe void GetLocalBoundingBoxForSweep(TypedIndex shapeIndex, Shapes shapes, in RigidPose shapePoseLocalToA, Quaternion orientationA, in BodyVelocity velocityA, + public static void GetLocalBoundingBoxForSweep(TypedIndex shapeIndex, Shapes shapes, in RigidPose shapePoseLocalToA, Quaternion orientationA, in BodyVelocity velocityA, Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float dt, out Vector3 sweep, out Vector3 min, out Vector3 max) { //TODO: For any significant amount of B angular velocity, the resulting bounding boxes can be enormous in local space. @@ -259,7 +259,7 @@ public static unsafe void GetLocalBoundingBoxForSweep(TypedIndex shapeIndex, Sha /// /// Computes the bounding box of shape A in the local space of some other collidable B with a sweep direction representing the net linear motion. /// - public static unsafe void GetLocalBoundingBoxForSweep(ref TConvex shape, Quaternion orientationA, in BodyVelocity velocityA, + public static void GetLocalBoundingBoxForSweep(ref TConvex shape, Quaternion orientationA, in BodyVelocity velocityA, Vector3 offsetB, Quaternion orientationB, in BodyVelocity velocityB, float dt, out Vector3 sweep, out Vector3 min, out Vector3 max) where TConvex : struct, IConvexShape { //TODO: For any significant amount of B angular velocity, the resulting bounding boxes can be enormous in local space. diff --git a/BepuPhysics/Collidables/BigCompound.cs b/BepuPhysics/Collidables/BigCompound.cs index 97386aa0b..cb9d74aac 100644 --- a/BepuPhysics/Collidables/BigCompound.cs +++ b/BepuPhysics/Collidables/BigCompound.cs @@ -152,7 +152,7 @@ public LeafTester(in Buffer children, Shapes shapes, in TRayHitHa [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void TestLeaf(int leafIndex, RayData* rayData, float* maximumT) + public void TestLeaf(int leafIndex, RayData* rayData, float* maximumT) { if (Handler.AllowTest(leafIndex)) { diff --git a/BepuPhysics/Collidables/BoundingBoxBatcher.cs b/BepuPhysics/Collidables/BoundingBoxBatcher.cs index 2a839bb61..80d5ec0df 100644 --- a/BepuPhysics/Collidables/BoundingBoxBatcher.cs +++ b/BepuPhysics/Collidables/BoundingBoxBatcher.cs @@ -125,7 +125,7 @@ public struct BoundingBoxBatcher /// @@ -244,7 +244,7 @@ static BodyHandle CreateCopyForDancer(Simulation sourceSimulation, BodyHandle so //If the required detail goes low enough, note that this demo disables cloth self collision to save some extra time. //The ClothCallbacks specify a minimum distance required for self collision, and low detail (higher 'level of detail' values) results in MaxValue minimum distance. - var narrowPhaseCallbacks = default(TNarrowPhaseCallbacks).Create(dancerFilters, new PairMaterialProperties(0.4f, 20, new SpringSettings(120, 1)), levelOfDetail <= 0.5f ? 3 : int.MaxValue); + var narrowPhaseCallbacks = TNarrowPhaseCallbacks.Create(dancerFilters, new PairMaterialProperties(0.4f, 20, new SpringSettings(120, 1)), levelOfDetail <= 0.5f ? 3 : int.MaxValue); var dancerSimulation = Simulation.Create(new BufferPool(16384), narrowPhaseCallbacks, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), dancerSolveDescription, //To save some memory, initialize the dancer simulations with smaller starting sizes. For the higher level of detail simulations this could require some resizing. diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index d956ac457..cb663a1f5 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -552,7 +552,7 @@ public DeformableCallbacks(CollidableProperty filters { } //This slightly awkward factory is just here for the dancer demos. - DeformableCallbacks Dancers.IDancerNarrowPhaseCallbacks.Create(CollidableProperty filters, PairMaterialProperties pairMaterialProperties, int minimumDistanceForSelfCollisions) + static DeformableCallbacks Dancers.IDancerNarrowPhaseCallbacks.Create(CollidableProperty filters, PairMaterialProperties pairMaterialProperties, int minimumDistanceForSelfCollisions) { return new DeformableCallbacks(filters, pairMaterialProperties, minimumDistanceForSelfCollisions); } diff --git a/Demos/Demos/SolverContactEnumerationDemo.cs b/Demos/Demos/SolverContactEnumerationDemo.cs index ce6132f76..9c37e23e4 100644 --- a/Demos/Demos/SolverContactEnumerationDemo.cs +++ b/Demos/Demos/SolverContactEnumerationDemo.cs @@ -93,27 +93,27 @@ void ExtractConvexData(ref ExtractedManifold con where TAccumulatedImpulses : struct, IConvexContactAccumulatedImpulses { //Note that we narrow from the raw vectorized reference into a more application-convenient AOS representation. - Vector3Wide.ReadFirst(prestep.GetNormal(ref prestep), out var normal); + Vector3Wide.ReadFirst(TPrestep.GetNormal(ref prestep), out var normal); //We'll approximate the per-contact friction by allocating the shared friction impulses to contacts weighted by their penetration impulse. float totalPenetrationImpulse = 0; - for (int i = 0; i < prestep.ContactCount; ++i) + for (int i = 0; i < TPrestep.ContactCount; ++i) { - ref var sourceContact = ref prestep.GetContact(ref prestep, i); + ref var sourceContact = ref TPrestep.GetContact(ref prestep, i); ref var targetContact = ref constraintContacts.Contacts.AllocateUnsafely(); Vector3Wide.ReadFirst(sourceContact.OffsetA, out targetContact.OffsetA); //We can use [0] to access the slot because the prestep bundle memory reference was already offset for us. targetContact.Depth = sourceContact.Depth[0]; targetContact.Normal = normal; - targetContact.PenetrationImpulse = impulses.GetPenetrationImpulseForContact(ref impulses, i)[0]; + targetContact.PenetrationImpulse = TAccumulatedImpulses.GetPenetrationImpulseForContact(ref impulses, i)[0]; totalPenetrationImpulse += targetContact.PenetrationImpulse; } - Vector2Wide.ReadFirst(impulses.GetTangentFriction(ref impulses), out var tangentFriction); - var twistFriction = impulses.GetTwistFriction(ref impulses)[0]; + Vector2Wide.ReadFirst(TAccumulatedImpulses.GetTangentFriction(ref impulses), out var tangentFriction); + var twistFriction = TAccumulatedImpulses.GetTwistFriction(ref impulses)[0]; //This isn't a 'correct' allocation of impulses, we just want a rough sense. var frictionMagnitudeApproximation = MathF.Sqrt(tangentFriction.LengthSquared() + twistFriction * twistFriction); var impulseScale = totalPenetrationImpulse > 0 ? frictionMagnitudeApproximation / totalPenetrationImpulse : 0; - for (int i = 0; i < prestep.ContactCount; ++i) + for (int i = 0; i < TPrestep.ContactCount; ++i) { ref var contact = ref constraintContacts.Contacts[i]; contact.FrictionImpulseMagnitude = contact.PenetrationImpulse * impulseScale; @@ -143,15 +143,15 @@ void ExtractNonconvexData(ref ExtractedManifold where TAccumulatedImpulses : struct, INonconvexContactAccumulatedImpulses { //Nonconvex types require no approximation of friction; we can pull it directly from the solved results. - for (int i = 0; i < prestep.ContactCount; ++i) + for (int i = 0; i < TPrestep.ContactCount; ++i) { - ref var sourceContact = ref prestep.GetContact(ref prestep, i); + ref var sourceContact = ref TPrestep.GetContact(ref prestep, i); ref var targetContact = ref constraintContacts.Contacts.AllocateUnsafely(); Vector3Wide.ReadFirst(sourceContact.Offset, out targetContact.OffsetA); targetContact.Depth = sourceContact.Depth[0]; Vector3Wide.ReadFirst(sourceContact.Normal, out targetContact.Normal); - ref var contactImpulses = ref impulses.GetImpulsesForContact(ref impulses, i); + ref var contactImpulses = ref TAccumulatedImpulses.GetImpulsesForContact(ref impulses, i); targetContact.PenetrationImpulse = contactImpulses.Penetration[0]; Vector2Wide.ReadFirst(contactImpulses.Tangent, out var tangentImpulses); targetContact.FrictionImpulseMagnitude = tangentImpulses.Length(); diff --git a/Demos/Demos/SweepDemo.cs b/Demos/Demos/SweepDemo.cs index 26897ffe8..a821b692d 100644 --- a/Demos/Demos/SweepDemo.cs +++ b/Demos/Demos/SweepDemo.cs @@ -170,12 +170,12 @@ unsafe void TestSweep( { var filter = new Filter(); - var task = Simulation.NarrowPhase.SweepTaskRegistry.GetTask(a.TypeId, b.TypeId); + var task = Simulation.NarrowPhase.SweepTaskRegistry.GetTask(TShapeA.TypeId, TShapeB.TypeId); if (task == null) return; var intersected = task.Sweep( - Unsafe.AsPointer(ref a), a.TypeId, poseA.Orientation, velocityA, - Unsafe.AsPointer(ref b), b.TypeId, poseB.Position - poseA.Position, poseB.Orientation, velocityB, + Unsafe.AsPointer(ref a), TShapeA.TypeId, poseA.Orientation, velocityA, + Unsafe.AsPointer(ref b), TShapeB.TypeId, poseB.Position - poseA.Position, poseB.Orientation, velocityB, maximumT, 1e-2f, 1e-5f, 25, ref filter, Simulation.Shapes, Simulation.NarrowPhase.SweepTaskRegistry, BufferPool, out var t0, out var t1, out var hitLocation, out var hitNormal); hitLocation += poseA.Position; diff --git a/Demos/SpecializedTests/RayTesting.cs b/Demos/SpecializedTests/RayTesting.cs index a3f4ad173..739248a9c 100644 --- a/Demos/SpecializedTests/RayTesting.cs +++ b/Demos/SpecializedTests/RayTesting.cs @@ -8,20 +8,20 @@ namespace Demos.SpecializedTests { public interface IRayTester where T : IShape { - void GetRandomShape(Random random, out T shape); - void GetPointInVolume(Random random, float innerMargin, ref T shape, out Vector3 localPointInCapsule); - void GetSurface(Random random, ref T shape, out Vector3 localPointOnCapsule, out Vector3 localNormal); - bool PointIsOnSurface(ref T shape, ref Vector3 localPoint); + static abstract void GetRandomShape(Random random, out T shape); + static abstract void GetPointInVolume(Random random, float innerMargin, ref T shape, out Vector3 localPointInCapsule); + static abstract void GetSurface(Random random, ref T shape, out Vector3 localPointOnCapsule, out Vector3 localNormal); + static abstract bool PointIsOnSurface(ref T shape, ref Vector3 localPoint); } public struct SphereRayTester : IRayTester { - public void GetRandomShape(Random random, out Sphere shape) + public static void GetRandomShape(Random random, out Sphere shape) { const float sizeMin = 0.1f; const float sizeSpan = 200; shape = new Sphere(sizeMin + sizeSpan * random.NextSingle()); } - public void GetPointInVolume(Random random, float innerMargin, ref Sphere shape, out Vector3 localPoint) + public static void GetPointInVolume(Random random, float innerMargin, ref Sphere shape, out Vector3 localPoint) { float effectiveRadius = Math.Max(0, shape.Radius - innerMargin); float radiusSquared = effectiveRadius * effectiveRadius; @@ -35,13 +35,13 @@ public void GetPointInVolume(Random random, float innerMargin, ref Sphere shape, } while (localPoint.LengthSquared() > radiusSquared); } - public void GetSurface(Random random, ref Sphere sphere, out Vector3 localPoint, out Vector3 localNormal) + public static void GetSurface(Random random, ref Sphere sphere, out Vector3 localPoint, out Vector3 localNormal) { RayTesting.GetUnitDirection(random, out localNormal); localPoint = localNormal * sphere.Radius; } - public bool PointIsOnSurface(ref Sphere shape, ref Vector3 localPoint) + public static bool PointIsOnSurface(ref Sphere shape, ref Vector3 localPoint) { var surfaceDistance = localPoint.Length() - shape.Radius; if (surfaceDistance < 0) @@ -52,13 +52,13 @@ public bool PointIsOnSurface(ref Sphere shape, ref Vector3 localPoint) public struct CapsuleRayTester : IRayTester { - public void GetRandomShape(Random random, out Capsule shape) + public static void GetRandomShape(Random random, out Capsule shape) { const float sizeMin = 0.1f; const float sizeSpan = 200; shape = new Capsule(sizeMin + sizeSpan * random.NextSingle(), sizeMin * sizeSpan * random.NextSingle()); } - public void GetPointInVolume(Random random, float innerMargin, ref Capsule capsule, out Vector3 localPointInCapsule) + public static void GetPointInVolume(Random random, float innerMargin, ref Capsule capsule, out Vector3 localPointInCapsule) { float distanceSquared; float effectiveRadius = Math.Max(0, capsule.Radius - innerMargin); @@ -75,7 +75,7 @@ public void GetPointInVolume(Random random, float innerMargin, ref Capsule capsu } while (distanceSquared > radiusSquared); } - public void GetSurface(Random random, ref Capsule capsule, out Vector3 localPointOnCapsule, out Vector3 localNormal) + public static void GetSurface(Random random, ref Capsule capsule, out Vector3 localPointOnCapsule, out Vector3 localNormal) { float distanceSquared; float radiusSquared = capsule.Radius * capsule.Radius; @@ -95,7 +95,7 @@ public void GetSurface(Random random, ref Capsule capsule, out Vector3 localPoin localPointOnCapsule = projectedCandidate + localNormal * capsule.Radius; } - public bool PointIsOnSurface(ref Capsule capsule, ref Vector3 localPoint) + public static bool PointIsOnSurface(ref Capsule capsule, ref Vector3 localPoint) { var projected = MathHelper.Clamp(localPoint.Y, -capsule.HalfLength, capsule.HalfLength); var surfaceDistance = Vector3.Distance(localPoint, new Vector3(0, projected, 0)) - capsule.Radius; @@ -107,13 +107,13 @@ public bool PointIsOnSurface(ref Capsule capsule, ref Vector3 localPoint) public struct CylinderRayTester : IRayTester { - public void GetRandomShape(Random random, out Cylinder shape) + public static void GetRandomShape(Random random, out Cylinder shape) { const float sizeMin = 0.1f; const float sizeSpan = 200; shape = new Cylinder(sizeMin + sizeSpan * random.NextSingle(), sizeMin * sizeSpan * random.NextSingle()); } - public void GetPointInVolume(Random random, float innerMargin, ref Cylinder cylinder, out Vector3 localPointInCylinder) + public static void GetPointInVolume(Random random, float innerMargin, ref Cylinder cylinder, out Vector3 localPointInCylinder) { float distanceSquared; float effectiveRadius = Math.Max(0, cylinder.Radius - innerMargin); @@ -132,7 +132,7 @@ public void GetPointInVolume(Random random, float innerMargin, ref Cylinder cyli localPointInCylinder = new Vector3(randomHorizontal.X, -effectiveHalfLength + 2 * effectiveHalfLength * random.NextSingle(), randomHorizontal.Y); } - public void GetSurface(Random random, ref Cylinder cylinder, out Vector3 localPointOnCylinder, out Vector3 localNormal) + public static void GetSurface(Random random, ref Cylinder cylinder, out Vector3 localPointOnCylinder, out Vector3 localNormal) { float distanceSquared; var min = new Vector2(cylinder.Radius); @@ -174,7 +174,7 @@ public void GetSurface(Random random, ref Cylinder cylinder, out Vector3 localPo } } - public bool PointIsOnSurface(ref Cylinder cylinder, ref Vector3 localPoint) + public static bool PointIsOnSurface(ref Cylinder cylinder, ref Vector3 localPoint) { var epsilon = MathF.Max(cylinder.HalfLength, cylinder.Radius) * 1e-3f; if (MathF.Abs(localPoint.Y) > cylinder.HalfLength + epsilon) @@ -202,13 +202,13 @@ public bool PointIsOnSurface(ref Cylinder cylinder, ref Vector3 localPoint) public struct BoxRayTester : IRayTester { - public void GetRandomShape(Random random, out Box shape) + public static void GetRandomShape(Random random, out Box shape) { const float sizeMin = 0.1f; const float sizeSpan = 200; shape = new Box(sizeMin + sizeSpan * random.NextSingle(), sizeMin * sizeSpan * random.NextSingle(), sizeMin * sizeSpan * random.NextSingle()); } - public void GetPointInVolume(Random random, float innerMargin, ref Box box, out Vector3 localPoint) + public static void GetPointInVolume(Random random, float innerMargin, ref Box box, out Vector3 localPoint) { var min = new Vector3(box.HalfWidth - innerMargin, box.HalfHeight - innerMargin, box.HalfLength - innerMargin); var span = min * 2; @@ -216,7 +216,7 @@ public void GetPointInVolume(Random random, float innerMargin, ref Box box, out localPoint = min + span * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); } - public void GetSurface(Random random, ref Box box, out Vector3 localPoint, out Vector3 localNormal) + public static void GetSurface(Random random, ref Box box, out Vector3 localPoint, out Vector3 localNormal) { var a = random.NextSingle(); var b = random.NextSingle(); @@ -246,7 +246,7 @@ public void GetSurface(Random random, ref Box box, out Vector3 localPoint, out V localPoint = (2 * a - 1) * x + (2 * b - 1) * y + z; } - public bool PointIsOnSurface(ref Box box, ref Vector3 localPoint) + public static bool PointIsOnSurface(ref Box box, ref Vector3 localPoint) { //Cast a ray against the box's bounding planes from the local origin using the local point as the direction. //In effect, all we're doing here is making sure that the closest plane impact has an offset similar to its box extent. @@ -382,12 +382,11 @@ static void Test() where TShape : IConvexShape wher const float outwardPointingSpan = 1000f; - var tester = default(TTester); var random = new Random(5); TShapeWide shapeWide = default; for (int shapeIteration = 0; shapeIteration < shapeIterations; ++shapeIteration) { - tester.GetRandomShape(random, out var shape); + TTester.GetRandomShape(random, out var shape); shapeWide.Broadcast(shape); for (int transformIteration = 0; transformIteration < transformIterations; ++transformIteration) { @@ -400,9 +399,9 @@ static void Test() where TShape : IConvexShape wher QuaternionWide.Broadcast(pose.Orientation, out poses.Orientation); for (int rayIndex = 0; rayIndex < outsideToInsideRays; ++rayIndex) { - tester.GetSurface(random, ref shape, out var pointOnSurface, out var normal); + TTester.GetSurface(random, ref shape, out var pointOnSurface, out var normal); var localSourcePoint = pointOnSurface + normal * (outsideMinimumDistance + random.NextSingle() * outsideDistanceSpan); - tester.GetPointInVolume(random, volumeInnerMargin, ref shape, out var localTargetPoint); + TTester.GetPointInVolume(random, volumeInnerMargin, ref shape, out var localTargetPoint); Matrix3x3.Transform(localSourcePoint, orientation, out var sourcePoint); sourcePoint += pose.Position; @@ -417,7 +416,7 @@ static void Test() where TShape : IConvexShape wher var hitLocation = sourcePoint + t * direction; var localHitLocation = hitLocation - pose.Position; Matrix3x3.TransformTranspose(localHitLocation, orientation, out localHitLocation); - if (!tester.PointIsOnSurface(ref shape, ref localHitLocation)) + if (!TTester.PointIsOnSurface(ref shape, ref localHitLocation)) { Console.WriteLine("Outside->inside ray detected non-surface impact."); } @@ -430,7 +429,7 @@ static void Test() where TShape : IConvexShape wher } for (int rayIndex = 0; rayIndex < insideRays; ++rayIndex) { - tester.GetPointInVolume(random, volumeInnerMargin, ref shape, out var localSourcePoint); + TTester.GetPointInVolume(random, volumeInnerMargin, ref shape, out var localSourcePoint); Matrix3x3.Transform(localSourcePoint, orientation, out var sourcePoint); sourcePoint += pose.Position; @@ -456,7 +455,7 @@ static void Test() where TShape : IConvexShape wher for (int rayIndex = 0; rayIndex < outsideRays; ++rayIndex) { //Create a ray that lies on one of the shape's tangent planes, offset from the surface some amount to avoid numerical limitations. - tester.GetSurface(random, ref shape, out var pointOnSurface, out var localNormal); + TTester.GetSurface(random, ref shape, out var pointOnSurface, out var localNormal); var localTargetPoint = pointOnSurface + localNormal * (tangentMinimumDistance + random.NextSingle() * tangentDistanceSpan); var exclusion = tangentCentralExclusionMin + random.NextSingle() * tangentCentralExclusionSpan; var span = 2 * exclusion + tangentSourceSpanMin + tangentSourceSpanSpan * random.NextSingle(); @@ -475,7 +474,7 @@ static void Test() where TShape : IConvexShape wher } for (int rayIndex = 0; rayIndex < outwardPointingRays; ++rayIndex) { - tester.GetSurface(random, ref shape, out var pointOnSurface, out var localNormal); + TTester.GetSurface(random, ref shape, out var pointOnSurface, out var localNormal); var localSourcePoint = pointOnSurface + localNormal * (tangentMinimumDistance + random.NextSingle() * tangentDistanceSpan); Vector3 localTargetPoint; do diff --git a/Demos/SpecializedTests/TriangleTestDemo.cs b/Demos/SpecializedTests/TriangleTestDemo.cs index e337a84f3..0d9c8b245 100644 --- a/Demos/SpecializedTests/TriangleTestDemo.cs +++ b/Demos/SpecializedTests/TriangleTestDemo.cs @@ -16,10 +16,9 @@ namespace Demos.SpecializedTests { public class TriangleTestDemo : Demo { - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { { - SphereTriangleTester tester; SphereWide sphere = default; sphere.Broadcast(new Sphere(0.5f)); TriangleWide triangle = default; @@ -34,10 +33,9 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) var margin = new Vector(1f); Vector3Wide.Broadcast(new Vector3(1, -1, 0), out var offsetB); QuaternionWide.Broadcast(QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), out var orientationB); - tester.Test(ref sphere, ref triangle, ref margin, ref offsetB, ref orientationB, Vector.Count, out var manifold); + SphereTriangleTester.Test(ref sphere, ref triangle, ref margin, ref offsetB, ref orientationB, Vector.Count, out var manifold); } { - CapsuleTriangleTester tester; CapsuleWide capsule = default; capsule.Broadcast(new Capsule(0.5f, 0.5f)); TriangleWide triangle = default; @@ -53,10 +51,9 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Vector3Wide.Broadcast(new Vector3(-1f, -0.5f, -1f), out var offsetB); QuaternionWide.Broadcast(QuaternionEx.CreateFromAxisAngle(Vector3.Normalize(new Vector3(-1, 0, 1)), MathHelper.PiOver2), out var orientationA); QuaternionWide.Broadcast(QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), 0), out var orientationB); - tester.Test(ref capsule, ref triangle, ref margin, ref offsetB, ref orientationA, ref orientationB, Vector.Count, out var manifold); + CapsuleTriangleTester.Test(ref capsule, ref triangle, ref margin, ref offsetB, ref orientationA, ref orientationB, Vector.Count, out var manifold); } { - BoxTriangleTester tester; BoxWide shape = default; shape.Broadcast(new Box(1f, 1f, 1f)); TriangleWide triangle = default; @@ -72,10 +69,9 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Vector3Wide.Broadcast(new Vector3(-1f, -0.5f, -1f), out var offsetB); QuaternionWide.Broadcast(QuaternionEx.CreateFromAxisAngle(Vector3.Normalize(new Vector3(-1, 0, 1)), MathHelper.PiOver2), out var orientationA); QuaternionWide.Broadcast(QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), 0), out var orientationB); - tester.Test(ref shape, ref triangle, ref margin, ref offsetB, ref orientationA, ref orientationB, Vector.Count, out var manifold); + BoxTriangleTester.Test(ref shape, ref triangle, ref margin, ref offsetB, ref orientationA, ref orientationB, Vector.Count, out var manifold); } { - TrianglePairTester tester; TriangleWide a = default, b = default; a.Broadcast(new Triangle(new Vector3(0, 0, 0), new Vector3(1, 0, 0), new Vector3(0, 0, 1))); b.Broadcast(new Triangle(new Vector3(0, 0, 0), new Vector3(1, 0, 0), new Vector3(0, 0, 1))); @@ -84,7 +80,7 @@ public unsafe override void Initialize(ContentArchive content, Camera camera) Vector3Wide.Broadcast(new Vector3(0, -1, 0), out var offsetB); QuaternionWide.Broadcast(QuaternionEx.CreateFromAxisAngle(Vector3.Normalize(new Vector3(-1, 0, 1)), 0), out var orientationA); QuaternionWide.Broadcast(QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), 0), out var orientationB); - tester.Test(ref a, ref b, ref margin, ref offsetB, ref orientationA, ref orientationB, Vector.Count, out var manifold); + TrianglePairTester.Test(ref a, ref b, ref margin, ref offsetB, ref orientationA, ref orientationB, Vector.Count, out var manifold); } { camera.Position = new Vector3(0, 3, -10); From bab35e5002c8eb8ff3c7fb132ae82b2c94fae26b Mon Sep 17 00:00:00 2001 From: Salvage <29021710+Saalvage@users.noreply.github.com> Date: Mon, 21 Aug 2023 03:48:46 +0200 Subject: [PATCH 804/947] Fix straggler in GL project --- DemoRenderer.GL/ShapeDrawing/ShapesExtractor.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/DemoRenderer.GL/ShapeDrawing/ShapesExtractor.cs b/DemoRenderer.GL/ShapeDrawing/ShapesExtractor.cs index 137f6e9de..050dab5d4 100644 --- a/DemoRenderer.GL/ShapeDrawing/ShapesExtractor.cs +++ b/DemoRenderer.GL/ShapeDrawing/ShapesExtractor.cs @@ -276,7 +276,7 @@ public unsafe void AddShape(Shapes shapes, TypedIndex shapeIndex, RigidPose pose [MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe void AddShape(TShape shape, Shapes shapes, RigidPose pose, Vector3 color) where TShape : IShape { - AddShape(Unsafe.AsPointer(ref shape), shape.TypeId, shapes, pose, color, ref ShapeCache, pool); + AddShape(Unsafe.AsPointer(ref shape), TShape.TypeId, shapes, pose, color, ref ShapeCache, pool); } [MethodImpl(MethodImplOptions.AggressiveInlining)] From 7eafd89a6cd133c140446df539d3ef1041874816 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 27 Aug 2023 14:46:43 -0500 Subject: [PATCH 805/947] Sponsors update! --- Demos/Content/Sponsors/marmottourism.png | Bin 0 -> 102599 bytes Demos/Demos.content | 3 ++- Demos/Demos/Sponsors/SponsorDemo.cs | 1 + 3 files changed, 3 insertions(+), 1 deletion(-) create mode 100644 Demos/Content/Sponsors/marmottourism.png diff --git a/Demos/Content/Sponsors/marmottourism.png b/Demos/Content/Sponsors/marmottourism.png new file mode 100644 index 0000000000000000000000000000000000000000..47cc0ec02f54e983f532e7736359fde4f8584656 GIT binary patch literal 102599 zcmZU5cRbeZ`#yJwkX0eFM`g=QxQmQrrRrb`+8sFJdfizkMr_ZQjjJ*arOiT1_q(5jN~m04D4nMjAOL8IPjZa z7sjSBFfgsnBqWr~3=J_b&WAaKiO3zhdg@6n)3d9LXEgmZ)07Hi(5EEx!*E~upW%6= z9RDXy-sjKTKP)o&r!Ck~XHtUl-g_qJ=)a-EeMV3Qsc_yEKU3hP!Ea5U_#m%{HTCKRh9Tu(Nya@Em2HtB>-j9#b9btDhu0=dHJaaE zUVrqS@$Jm1Cyh_Y-dxvOcxB_8*vgJKhc8VnoIL-eqvWjk#joGi?-WGbQ1H@QA(-&U zUwyn%Wq5)JcP;Ko_^vyRLxfUy+>5Y2=Az@#OtuC-?dJ<_dNU|zMH#J@S zztJCQTOY>AHRPAcJQer+zF*?=JhaC6u83j}1mz zk2U+_^Xn}&ri^yMb~c9`C!)UYA7fRyX8ksAF<&@2HZ(ny#f>sn-9NcOt{ZE$Pc^UoYpqcS;*bBCOeD5 zmDC=YDI1RR*xA(3Y;>EeW<4k3SbR#37==scg@OD)k6|w-=`4&kmKBFG$%cR0SeoGY z`1Dey(#va$qaos&iG|W3G#wQWOj;JO7!Ljk1}f}S{;FV!Om1ie2^s?vi}>#k2F|uj zWyVzgKYb-F=M(jh{z4ayVrpj0Y8^Ow>i(m>q_Gm+S2aSiBaQ!l^Dp?|moJmv`yV*^ zA^DC$5?X=B zN1tDphi45Zp5MCq@3VUFZYEyobuc%$`o9_2)AN!Pp2PX~`_OdtsQyyRqAL-{+z zN&Sw#m2M;u!{5$?niBip*(Ub8qhIHRY4_tjcFW}_Cr%eq%hCOP0k2Cil`m|KPW|@* zs@}2@ER;=Et9|BD3s{F!36H_zLZ@7DmXP|pl>4(|bs^tjTD9e}renI*;M=HCJjuUr zWg`VIdMP;Y(cdv5^NqzZzdjmrTomWn;l#o3V(>%H3ks$l|2s6~Ul5ae;}D+pG1A7l z{`V`(tKlVw=>IoG@PA%%Sh}uL%+c9Q>&7wK`e(N-0{^Zn7AuTdNZj>*Z?XT-dIoNn zMI&LG5e$K&ukvEWl8JQt$x|TnpV_E$L#bpatPG`S@Ei_KG95NT=EFZ0Xa6(N+~hJ* z?6JxjaC#^{k_8^Uvg8dfNtd*-Sn~gTcs{0%O!3N(g40FXM^CGZgEuo*kfi@Rc4VZ= zT%ngt6tXmm?sbV^9Q|dNj81vB)}%T3pY4gp@D9a`WY^#|GQ7lnG>ya;FiFS5!cBCK z9`bh{kJeBP4v5o7Lw%B?X*e49-?a%NrBkN4;PUH#YXJXt7@y$W<=YC0SEjby3$Gr1 zd)+kbr|MlS4e_G^AR{hU)qn4Um#d>CbY}I7;INP3*~BPnc+I(qQ_FYB;TP6M{S^&xY+o+-@o zaa}UzQyuC5?%*(Do4w)p3@_g&{O4`aG1wK~-!oQ zfd!Yzyu|wNTdnKr50u;5jeR0%*|PYa|8o4h49>4F@oE=UvrS#_G&3H4iR1tGFfh== zMkM|H+5g`Ot~sM#>rov@D?Vfza_5aI|FaMy8-G&fz*l;OM*hlI-W?4SU5>r&pmMW( zC5Gr|Q2SF@uAo2DSJIKKaY>6clZ;7bgtCQQqmUwJC9igu_n$YwEyG}K{=o6|;n5c& zgBX$X4iVdwDfTFP!OKV_x*W=+m^g5W^Yq2rp+0yyNtJQJ4_=w9He5RlophHWyh%?y zFzNZvmn7F6r+r8_SG5#8Ln1F1`#tf~%V4vPv@pk+J`Ebnz1^L{hZ}bPCWar#l3UsS z`n!_ICf38V7_J#SZ${n!i%QVyk1$y$*}VAfdg{dLr$p-Wk zq2HCM9A#*TUo}^=#biBHbv1~@|7+|e6M?wZwPDZbn3x#P&6r3lo;bh1*{1F(Oo(N2 z`+sAllJ~LWCuN3f+#kQs)T{MS+F|@wp{q9KF6g#NeJccR?VWy0Ruv3vh-o?ens@EG`L*F5Z9#iG1)FH$u}vv-!Z zDcEV)-AT!OxX?JDYHi4EvF;d=Of;v?*g(aDuz`~gj*?g;1jmp?L5zMU-@4hs%5h&> zkNm2b+gZ0Xf|-f`?gu)1l=#ka#LT;r5!* zH&46q{9@^5WqPNM)8P2`Tg6XZR(^joZVG1(4r)Rly|ULG2t2CkeiC#(DBqLk3a@NJ zxFcD2y-4`K@!Fx(a<#+4ndO?*^xTbINC03&zjc^yUY+SThEax#dwR6E51v&owa}1` zew7R7<9U+jZg0CSCBe~`_{l+V)xNe(jTwy>c|oPYn9V@=Xb9+KJU)x>N>LnI_lg&k zQ>B`G@W{sA9jE2{J@HGQBYN>yy!GX`^+hJFIQOGEj{>Q;%?Vh`41ov#Mq#fB?A1TL zjB(2XmXy3k+%rG@*Oj!TE}M6=3p@OdWL<4#INYLjv=IMx7N~o%|J|WJvUI2Xe{ge% zGIfqMKbO<>+;M1ZOcbz;8#UymJNlpv8%$W0;L$h^UN8XVia7zBfkR4K@jJQR!yoG( z(+JP<5p#%?yRQ76^ft;kd|6^1_}3(fcMQ>rasGFmfdvzC_ za9Q4m>oym3tIE?9UTEG*zv1wq<*RP4ZuP|=D#54#a<0B91=^$G)P=(r%iYBKcUIBG z_@5JmRl0MuOg@J~-kSRMCfZg!&#^81LQ51!#Astk3q07$dC+rjr?k{zvW*_1c|cB$ z{a=KZ4d<>bMUvoX_(-ym%&B|Mrk3U~-CfF={rQDR+2?pc#S>@PDM~zgthWmqX^Li$ z%-`=m^%pqRGeczNr1uEkf8WC&%53>IKdxQ4mFD+wd!f;8c~Y6tmtNxOr&7!QG&p%u zJKNOTvz2pwLqj$0TVu;pouylyBuCq_`~~`&^M98T%jy&bpQ%!^cJsT($1OXr%-0<& z-P$Zk)iu>Dv(hQhns%Qbc^6K16$MA^&FyS;<>G9@zh}kf8sI3hXMz8Lyfwt@e~Mbu z+}36x356xEMgUH@piyX~WVl|k1bdxHx5_zXkKfX=uXrP)<($(1Js=Z;D$}FYMdza; zb@5a~caMI7y!i;eHzcuunn&{CY*O6JQV+c;8(A7cXt}5aEJ80k{Qf>P3!5el5K+hL zJ8b=iZU1{`7IaF&8tOl5QH_RJdR;L>`p^2CQ5F9hHE|P z?kfCE1dkvT(FD@C|BKb@$WgQ&>n{zrJ|&6QUfEqA4GwBV+v1ywZk%RQtv@G&b@V_R zeB=P}|2yk+ud$=J^f*0tH!=)yL|vC{EZA`hIl1k|WTxLq{-qOwc%kPfIQ}!W${h2J zw4OqRd-hWu{@p{l>IEr`qt{#LAT`?=y+NWZ<0v=o;gcmZ03A#|oc+^hjH=z;eAA*@ zZ}-1ZWS=l<3coJ4yKdN9W}VdaMy>U};Llw)rF{LX((Djd$36)W9KqxE3$PI{Cy7)# z#@20bEOF$Wci&u&GW-Wnj-u~%FQE&7UO zpPgp=^=Cx+O5a41C#Ngk#f-(&b@{sPgT?jBN598yZC%EdtgZ`VlxC;8{rYy(e^Vxj z^X|rk#3h@^kUpT6X8ol(?Aql@-MKn!jlt*e{Y*(;JUu$FKAMmQGM`Y=VjjolHEtdo zF(r6%^f#afl~|;#3hZ}Y-^QydUis78se^CCf4J#Kv@mbvt*^Eh@*IV#11Q;#`ru2n zwu+-JdbrBY2$eCRIgS4x7>t!7Jp#G5(0aILvX@)r@NeFJkVHbxi%0xl(!pZ0@bfxj z%oUf#e;I-{(5i&BV@DhL0OUi?`Q4o}y&;-^6jb~YV4r>Jy>#i{50<0#X9p@c)v@Jp zrABJDW+Na$<6OczIzn{K2xi-$@|yW)D8ye-z}KJKjy7Ds6%4&9`Lc5_HDRkpiym%+x47kBxLDnb47^Sv0Pb^jTVEx%RUI z!qjs5Cl4olz8sQeh&VbbX=@Hm&12U(<&Z+EMN1q)~43@^g~79_)b$e}|$YVvU;z zJ9*yq64lXG8KK0?`GzFG&}_Aqrvok%FG?G`9j3D8Vc+;z1eKCcJPL;KpJc*$b32RS z+-3Vv*n}1Oci1s8r%{lPPBInC%aIOR;q0hNSJ;iC@RlGUYv~b$9WE^KX~1QvKY5B| z52x7gjJJgENq=3TtdhJEU26!7X|O}=w%2W2uuQ%t;3dfH{(h6U>u{@ok?stN)_qyo zWi&{Tsg##q*lsK|p6g9l+)tfKM?$7XYt9UVyKgh`+dJ3|8-oeE^k~IB9wrA16Jm1V zO@7RJj6-N=-vk*$Wh7;Au6A$eAM{!U=YC+THT2wHs*UNbD7GOvN|qE8YLV zC2tjOIY?M<#t>iOX7rI98FHI@ zE;3s#v8=_ZAh^?g6Y>;l_}0t6i0>k?sHem)m%njH>{Odduf$V=u%-ysEgPryqI5p9 z4jBk$<8YKxT9@BH!Kd%7>b}=)cjnT)&$vzNR+P6RMyuV{qOkGLeERXhcd(GX`>sSX zP$}N~UFWGpM`t@yrHRb?iZ84*kG^6+?eA>wOkzLU)0IXMxaI2T>70&(H|0Lv)*0%u zIEIjqXHmgzWZ(Jb@Ju;YArmzxins8Mq3d!@lAA4IoCJunIOv62;qcfcde6xD( zvwA@*7MxH+7wPt8ezTVI`+BJ|9g-J7;t5I+bJtvEcnfbApxY($z}Kr=rM5z2W8@mw zrEdYUmyAByNH|sdtb8Qbiv)n8{aP(=AKi>neLMSQyd|0_fn70PAZK(``aXIm^d%^| zb}*r#>@O8wT&AatzG}@8%5UC{=B>Q@;c21E(!Itk*kBXuF$70gcnD(n?3am#*X|7Y z@R_=vH#wysD5w)MpG2m!?l>y$R1(MnffPLN%Q8F%w`0PYWbOLL&349s70OmR&Msf`Zp z%874J@>;Jf-5mLVuepwWf}E3Gd~f@~NI$t@Lm*yPUx>f|M$a`Q{_K9_VLS{R>qt_z zreUU9NT~xVZBlHp)5yM$fa-=(ndSLSSo#R9__KtQ^M=*96*!tQI1 zd~{upo+k$|;uG3GGaVXbXMJWjSZ+khAOJ{E6YTeza0 zUbvkZnl41x(OLZRM&$JR@srfJOf4%$8xZ&{Ty(4m2(HJT6`YOW|HzpI^V>aBm)l-)w+dbtgEThQ^taP)t?JNQ{Z*@ecNSoD&s>9Qv^QN z+Uy`z)QVYe(ziEiEfI=hlu;X10%FanN_7rHLEuH1GMmC?9z z$|X%}b9ssxl3Y}Dn1H3G%UGDSBo}YC(#uEV9%Uy63Jj%3>^e%!dzvA0SmCN9nX}eB zPW<#9w{W-w={`|Uw%cLrzGu8LLoNY10p@NSawMQp%_Ym)AXnyj-JXV!|yZh*DgPxA)m~``# z&yJ>1o2)IOWBB2cJl#7Hx79y>AE3G;39MEO$#!WFK1lRJo?{!eChCALa|U;?$V4%j z{aF;My(Lf3;=8`FU$Vh+g_&$5C^vEt)gGU1xiLW_>MB!c)P#!mZ|H`cAWA7<5%c^d zbwE2W7sT<mCjt(d z6TPH*q@|ObYV*m7%4iFx=;<5w+~a@@894HK*ZMcG-5XB{DASx5e)8Ik)cNJH#$bci zb;+h;tmGAWa?RfE7QOh3TH8^nEalXvrWMpX$?u8*MSjVY1EgXMN8DR@rBFsxG}kJ0}8#eZtxP_}}fF00Q}JB-7|& z8uv26UhZ0im$H`eXe^BdRL}%N(J5<%(aBDHeQ|yIJgY*&cZcZ*23Iu&r1Vr?-3)_Z zgLOY)4k}{)FZcMLdWa<7`x3`{?=wYHykK5CHKa+NXFVf;AH?SEZ z*|~TN#A+sGE4es@1F*9Wl2}D;B5UPQaS0za-IEt>UAvq9pIgrJi+64m z-)=fabcHxG$Gu0#$;A@Cw0moNsWqUcIOr_@4M?lZl}>Yx+i4Zua(Q>&LME|#<8iDAV+U!y+JW?4z*kf=^RV#^oF;Hi)6J38_Xdk{*y-uI`k%RPzBGMCs zw1B$(^r$xFgp0^J9krf&b;hB(7OwOYTGrL4Wx9WT3i}YCh1(zZO-8S;_zJ>1s4sj; zVU;f#lLPrF9aLe!3{lBze5P&5XtU_`h0%a+UC*U2g5&l6WLJaUFgqDAB#7tCml{9&8pNCYy~#tHj1&V^L(|uGv>g`uIW6=gxlK!NN zfI?mG&sgbN0Li+p2j}WJu%9NVB$H1($tmj((WQCavF;`c`)8k)51dcYZ`CW&RKE>Z!m+HJcL zK4++cK8iDjjpW_=+p#}gF4|z`EE$=h)2GnoPNR)aVT{)!)^*qKNkNt#1Hlk~GyWL; zJ0qo@{i4*0xe@64`kdTwIAeK@ZzP-pVtCGk029LNIfP}53Kz*8?D~gn$$RDP0cUcy zB|dTBwuoI zcXG7twC@xj48^O1t?Kw8#IK7$LDVFO>}cy8v$A%L69!cz925Yw%V`ets{sjK-50;% z^RH;(I`%^ZJJ!?-0CDeS59Iy-Fbit zR~OnT_rW?Q)_ab8`>9*)WaX}?wl6s#+S?7gugOB#UPgy!A$}wq`P~np)Vd{C;o2i#DfkD2c6$03jB#Dg@Ne_dwp%knvcP#IVY_ntuLiBl-gTN!dM!!JkCtstf~U6qEQZzRy2>yY71HO0Hx@P7}kx z6aoPKLfZJs=}!9C*POxH8+gr(u=}8hJ$q@SKG6~ zu>6-v;t2NgqgH^50lK8-89IAcgS$n!^lGYJ+|?d7fg`b(km$5yU|m!vVA<#FvXOo6 zaG$}>!$Q<=%!?8X`ghQKR}VyJKb?`BvY~K@A}+xulWi~UcDG%ohHGmlx@T9h0YuF4 zI?wBC6c{|&hG1g{{)d@#wd z*%$!#TQr|_{V@q8w~teHRMz#7H;*?bPx>~oDkg6C;Ic$N=#3G}a_ZVd5 zGdYjlL5fOlus57QPzYOI6+@g2h}i~`(J`Dlms2--vNaeX8uI>@_@~@wkE`U$d^zz_ zrTxn~gu<0$eZAkgS%yTCx;Ye*e+@o9tC;Oj5Sc}kc|Oa&+ci6@S|Wq){S%1NOs?yE zwV1{f(!=h`Yu)O%li0DoM6_q*pAB!@T?%AbqvF4RO5Fe;;_k+ zDIrE|8ii@?%bVJN^$7r~_50Zo8%09;y~?F%JhsTV<&045KHJ)6PtM5aZ=xV0BMY}| z2q0G^Wg(c0o>~rX1eB@JS7J^cF6v^Yv~%VTAR1J!g%=h&6LxBL+Nopaw4^Ih;+9Nn2%+DZUe&<}> zE+dP6IPPmGebsk2)x$^8LRiJQWh_k}(refpdEr>P)#2M~lg02D;!wc%0?s2HFa190 zbBNm&@CYcl=`P-PF<&eOKA}+HVn8h_n>S4ASK9KXCs${ybxAhze3&Ui?_3Tnd?e-K zmc%_z4hy#QE{RBA;&Iapa%1tH!_@0H;+71XSu zeAZg9FmOc)W-DdYycLvVLTwq>oT)PAA?Hy-LGfJA?pLhEGXm8K9I1yb^C~fR!|Xu)7;O3TH4H_z!p5gUdYSyKxVJ=UEVz zK>mmYg(9cs$M5Bhi6me1i@)}fu%B1mEBW}qlgvJEm-Vo4V@NTv9wDF*i$>?L-c*<9 zrE9kV5>M~6#rHQ*w&8^eFH*I6xVN)b2yy`w*JMWwWIG+9-iD5b_!{H5!|DM?EF~jn zrw_3p#e$@vS7zd(2bw_S)yFu4#Cg$fZ!1ni!nEoA4bXzwb9cKm%5^ExlF;D$)K`(Y z;sHa%GH_VrnTH`zSamEDWB>oa&__of6w$y!|ySH08?z|tDu&S7CfN8ei z2h)nzTv_Q-IWW5HA_`Ohbs><4}^aZGXm48wmy7W(2!^xTfne zemWLJw_33(|K)O?C~&l=?1KobA3nezzFBxTM5)C-O{$-2r2!m{;e zf6Zr{sJuq^7O9J@Et#wVr{u*zVjJCA?Dq~S-M`2ERYwinR3T)Q?%+Z;qtkr)kw~5^ zj~Y-fZ7Jyl4X@EBVw2A$USKP!6C}5?LM(^L)JiWdJFthGZmxsi*pzeM$=pBf_qR7x zig~rU@9%`vg1Co38Opq{llv#Fog)-z#3YXX8ZO;D7aWSJ3&gNPfu*qsdY;>IY;m4^ z>C2FR;}t?!63E;78X^8X@EB2-D&D-)$C~u;*7BROFjLUo$*b*~r*lBf)*XH{zOn}x zKFW}P9?(gp@phf}v2iHdJZHH6j3Q{t$OZtb5jYt@vkTWc`6lDb4Bt*e1GuYZ?9++< z3K$ruyX1bKYfpTrk!O*t(?PvrKK|?s$M0Oi{If_3c!jpeFtKtDj7RO{aw&rTVo62DNoz~{NG_30&Gye*l^sq92N2KBgzn_+z2-1}`3Z20za+H;4f8Nni8q4Hm}3AjXy zdWuS55yWDo#>CQQOU5g*DFuq55vM@*EzHXGwU|GWYMgm<)TXW<38XwK&|{beo1W}T7| z3yzu~gQr7&Y0xRD3V7ksK-P}=7rJ10(_xX~v`6Re?`C8;%$S{>Z)^$UfD`$@SuJ9D zN5yAK2ix*FAe&ut8Tnv4fh9DrnE+zgC#wf9+Hv=639@?{+q!)e-epssdjb8U_b zBHNjqAe~%o;i=xDjOp;PXGSHtu98>xoa{uEEi^A84oF_tWmDi*chaJ1yEpiibJv3Y zd@o?kUV7@vD|*hVk55_5xcOA4RsG+}3P6-I5K->ly+?X5j1(1pIB?#b{R(A|j}u)M zO=D~5)cmSzdJKmk6b|iKjuo&)of74t4U#v1jHnAx$s-@OsD&tc>#e=n=#KsGF4=1i z%|p&S_J5t5#ORUBNZdL)BXmZnIHhzA^6y|~TN%NXI!SZoyXh)uUjAyU_lkKQk3d0P z4%i8V2k~N1w;BSvDlx&uWIPaZ5E2ij=zpbrWbDXtZ8m}5&LMJkXs8sRJ@t9A(O<8I zNrk|9)lOQnO(<^!;xHnW4J&oMz9X0DHY+^3W7>nzuPk&s@j!PX8BQ>B;O$p zb^UoB#NdUtKQZaw4ymQXbP11?iTha@X~KDB2DZDo&urZ`PK%PI}$s=6^9)iw<@AthCWlK^XR0ny7V+(4{Tq-TP<0o)-F6l~uuaOA=RKt$Jv=StJ;PfrE9=E|SBHcjLNY}M|($~&t!Wp#9 zaj5)->G(V@n$FvSx>fI4<>^u`?eaE&2WiKNk*(Rk^FQ+zJpMqV0uwKw5i1udQ5_ec zP+GyPh&9qLtq#-`nEwJFg5Zo}zjL*u{oGLX-ZKQ1rY>=-+v9fS>FF&$=h72v6wD^SyE->KykN^H zoSYb0*15cUrgB_MHBZ;ll>FrPg47%y1cYnt{QSKP_3Gx= zciy1VIY6>ug)SA@BKmS4M{k}Jwv&X=Prk^!_WPUM zay+qznX5(?OIoGF^u4NdD0|95-VWN*m{CfVB81KBqOA`sJEit1B1Rshy*ro+ky_6g z_Un-lXRV0a@P*iJXVL)?E@tjbzIse+r>d_>kz7#I9S7dC=dfi$(Wjn8@fbEJ5wyh6kb`gQ$#l(=G22^FA<%kIPS8gd;(wiiSQQD~$Uz@NPRtGxz19_XqF%nHR5vzH^?Y? z|5*+?tao^1ERkRxZSVesFxcAyh7bV?E&Bzu9<r}Oc05mLht3Zh7HJ8--Qgy zd*&lFqzEOnfkqYKIqZX>Kp|^#prS~5d*z+xvsB3B1*;+CT!}NbJ6$}|!PMcG9q#K{ zn88*jm1lM4Zx^tiR6@LrU~boiJ!G;>y<#u`Ie==#)@>f2tIDQDZ^<@i2Mi0rY)tbn zu?W5Ou|d2z{fBs28Ur&L7vYSi%pm#m@B7%dIiTP-+!{T2P}CfWse_V~WQ1f9;I91;_TyG=BjlI^G za|gSGII<2DD8%t|3l0F{dC4g#-kqNR@xfXv_`L#$p;mycO9oJ|-fwK+cr9Ake7`~e z2~p~+YV<{}Z2c)Jv>U&Hi136GkYZ$Ru8A4c`{_rIHt%ZYPC-KB(d|dtvOxV3m?hWp zMZ^s_Q$Ihc6a3`bxA1plC|XagKvPW%;C{AE6Gi~Y0~hAIR1_;7+|51#KouGBN{Ao! z;fSFQ%qgFEML6@iDdjVeqlX*`ID{eQtt9_ia5a%H7TcUm%^i}0z>msEm+$K>GLe7( z_?f8VA9bZ2d6AwREwzT=b8PVIJ+&UFdr&1^)C2Vww2ja9l!<;|oahPa*#N>N11f&d z*2nH20KDytsBspO>cGGB@%J=BG2w*V7^-qHhLYOu2Yu;-6E0Z*F#Ej6y2XK1=Ut8k z>&(&;%@xA-r&PS>@&~An7x6WKc58a!Yu?p3`;}{M(>Y~T`JY4cl(}ASgC*8VL=ouY zN4~3Du#&zHXToBVfqvb}(-j@fT7mW0Ib{(r^C3{bG0k)NglYJ|*DYb!kKct+v!^IT zU5iD?8(sq&WPqH;`2K6!!Kljhk`^0b>xZ*Awt#6H zx;dI_&Yz=tv6(HLUIIrkoBvcm7q283w<1BbR6kn-b;YQkx!6<3r65IL`aSwwe@FDX zr!|kIX5O}@)?53m+iz6mm)8OasRUw9UcZ70#d! z-nj%G2p^=iDAS1Yu(gN?-xM|fm`Duw{&83T)NWG&%gcG+$R=MIR`S=t$@*E+x!msj zTAM5F8`&5Dv2d_LsC$T*5iWttRU^0uo#Xwd=hb+^&G^KNdX-8H=;Q?z#)&g zN8?XQz=-g{&NaNiLkYR{5m}p^2TKs|4HnIMh7oHBTe}`Le-7iaq4!G(Uz`_dZWAw@ zz#o3O;epJfz#k2! zzlS}x!0$qq`bR5p=}H zC>Gj#(v*b#*D<_>0Fo`8IV^*EdtyPEmr>MhmEeYCM1}%W(VqkMH09@6Wy6`uuWAp7Kt%OETQ3*IPQtNs*Y(=WW{#QY5>QU6nN>Jtm!7TN%739Z z{6;Nrwd0J2xeqpCGLJf)AnvLA-sP^Xy1+*!TxV^>d$`y@WvxE+5h&$d;V3jPS74HM zp5l6rheR%{4(h|>Kr5u&bBVey_bWUPGzDkxmA&ny(w?7Bf)6bAy{N~ zemrXtOd9~)R3sH0%;jf*^c~*`T#i4sDUTnq3g#fJ?vX~-m)Ee{0~B&fVT3mzAAGa< zNleVKumq+92E^r8e<68%j#Imw*n}EtS0kRt`gu0|IqWBqLm^r_)biDzRSRcAuHrRq zqidYDn2uqI_U_Ys=yQ20J>9XIv3N{9W2P%BV#bcis?Rrf9I51jtN9anh*gka8 z>=A{^fYj(oDuK`EhveRA2D3oTDJtqA#Ckc~GW-r}+_n_Cy5ccq`>A{_tZj}^?z`7o zsJg75uxl27vbzG+wh=vKaPFml4*NbBy`hHC!^P1AWG?_(`owFJFWb5O^UDX+O^ zUF8ygj!%~6k8zej#z|Sh>XAatlKTL7Za)T5`c~q@#iqLPP$-I%SAUuY8&#x|-mO0H z{iJdP(TMgTV?;VpfTCz<7B=8?E`L3i7bSG!-f;VnnrGg(zEx0*{KUM$1VCXKx5y@? zjm0>k&f>ldFTvq%YybHnF55j!jUHvEt&Ow#XqnU$yB>~^C=N6 zCy;i+hnZo(IeM;K$E|(1Pikj;b z7eT~jFMqF9;pqTi`VopZ zl9z9s60R z;ay=DYloBa4AZ`v9+sQ*XdpME1Br?0d2sAl@4gYWHXf{ZmaZ$l9ZB25;4)!WaO>dt2}n zX0|{Meh%LKvf_sui%pR9&jVP%kF;#t>nD^aO9n(60n(ow2vvjmOx+{AnDX z%D}_02E%jJiw-MW1L)X3}2FruX`A+Mn z>NGKsl=de8w-Wt6zx9*P;T9P=;Y}pAUT1m^;Bz=4sjv7O*+3~xQcti9u;20KFBagT z3L9s>8KYEe@%Asy@>!GTS5?Tn!uS^}wGpm3%%UDPfyONonN&K zU6jo*8R<}qjM^ghLtCK0g%(8e8M>72{;&72Tn$#oEs2orz*-3k6O=DPced83f&idc zwv*-ST7wPYw0k=-aNH;Yq^vxi4iaA18r~1Yh!0XtI@mL!boi9W5+vY zt~c`H=$fUkA~{dV4ze%?ijy2R;NtQ6#lyLHQU@~{=(|tow%RZ;xp0yG6KH-}%u5&B zp8s$QT;riuKY$OCbM#(<^w_clHG=PU*=om1eBd^OiTbg4F3lUSftS3;)5{ZE`IM*C zk{i>-%B7Ty9wYy9;@~;xI;3|mSecjjAQX*@A0_?1Y~oC zG=EESnST4OR03$@MtjP24x+tpYI+*ME3W#Gse(%9pwAKj`()4a3YOI`f4Na^pgJP9!DGY7EC3l zJ2yN2kpPRP<3q7To9Nb}IF5Bd%SI^s?=Weaf-AXE6g*jjmM;3yIB~i)Z)lmeH>dMB z2MJpi1JHo&cM4Ch9<@U3A$A0<|4H9;_pzRyJa_iC*~uK{p;Ak&#%i!W8}t1Fz8Cbf zn%d%is@qIwMo4to=RQ%oP*8b@vcs8=^aOvH2{Mkub(r_BgQ)3VNLK3Y<=_o?<_2uo z9H;W@X)IX~Z_X|Y^$>-Ej~o(i;5bi7{U<0LSPKuec*7D$L&rLWMX#yfP1}_;^nk@j$bi|d$j_mT+_IE8P zLd)08cewd-Nwp?glgJ}l^3-xP9X{yRtpCK{nEyaDUcELH0X*}md(utV1Yt;{AEnd1 zgMKMG1H_?7E)QL}ek|UACZ2Wrj{|ELiaX!ag6T_0b#J2ph*10Fskh_aIutPwSzLhf z1#GvUfxWM3J z8oUh@n%~-RJaGZ4cN1}fknwdMC`=<~yrIe9EC(}`29WCEhyW(XyY@;a>&EDkyB~0* zZ&YY&#Y9nD&@Rs(cer1@VtOAqJ8~_G`p_bj@6HI}$sAa{bCK@bbXWv$9|s|Dzz1+> zmEHtRmDpK6miM}5u_-+?6Nkcb$${+{MQ$OobGOPViGoeKt{*aU_0IND?^y#1`i*#h ztAJ}T)%g*LO?(}&qcK2+Cz=ZRG^(8!3>q60;`p-03lM{6Ty{uQl$g6~yUH4iC5V%Y zkkR7o>CYgOL-!s9IPW<~5gJg^ySPbT$dV%8oatk@0jmFVa0NAkbbZW!8_;_*a2)dr zNp+#_Pt=It4N}!WAkD!!b5=z%8q#*8uImZ@9%>GbyDkPO5*wuvVBAhN#hyg`2wi9p z%u(@xqFm#z)%R%VY)Nb91s2T#rQix>+DH4bh=DO5v?!#_77;}u@Gt@f(lI(?V5qu- z*5%qnth?z@MKahVRG?DnfF`kPNT(83Cc(iy3SL@BYqnuAUv+lqQ?F?3R@Ffbq%i8fYLURFb~<`9-;AuiVqdXufC7hrCxxq{yNH0 zA)?G`u+RB8qf{XGESc7E`N;T$xS?XR2*xE>Gh+Fu_+qHNG=rh+=3C-@A3O0qZ-gm9 z0qesR^T}78Ez-vcDCk@#$3PN0IVc$j2r(F>ZKueHcL$kbJ{n`CX=mk1s-wP}5s!N(KG&Ow6zdp2$` zn9nOvH1sYXTZe=2#8&DRpwmj&b2H8DhrSG=6Au9|M5*PJgqKJ$K`+yZNmDnoz2%;P^`1_K=B-5^De9 z)FkVzp1X_?B zFYXU)pc(DaZ(5EQ^5RPKOFf&ZU|&)M0U!e{G#e%ZM;1)zs!|~|=ceVjh6$uQtt!}Aa5nwvu?VC!Q>>^g1 z+48pEL`ui;nK1!Fv)f!YYmt~4tW<+SBkg))`5;)j`6-2Ll7W+boh4RoSL6}{9F{v? zZPiCHuBanTIWpg2W&u_(H4m;_Q4eaxMl4~eD)$g6wgR^wBBCJvm4$jTD_t6MiIFIj zW?6rl+4~ZRMg@*8zhX`5m-{I+hqcM(UXd%rWyh+AfurRUD2pG*`dyLo?(&~rGO>L) z5rPl5eK6WweHFp_&f%uJS&Fv!dQHs=h*cizKaMwcK@1^V=iUV!%*Ns-4jkPP?DN&6 zRrHK-QOF1g_fdu=0ydBh*c-_ZI3jdG3HURTVIw4X&!Fy-apzn|I)b!vvEkaGEqzH>UbzY+(~AGegDZV6u|6fU0t68&`K!R zpVQw945bXgP`ezp0F5eVazs2T^UNE&T|mY{`u$QJh%Gqhk*W{G3#+d7<;SN|-yuF$ z&tPSD-$^#KNQ4#)TisX;*e zgllT4Ec(K@qdAI$&GtEEhqc=69L;Y=>cSxCC#wo`ETdj!G?zTxfkP=#%YAPjQO=j3 zK`Y+ZVYXxUYiDl!9ONeEq?du08v`hd%ECP;7ty(5L4|+6w`Lb+RAMW+ru4LEWvH{w<2RB=M5}S{odyodY~JZR5zi@7KEU4 ztZT?cPr$1Wh_zP+HV8Q5Q!7R5lcW@v?M+7(IXYFLVO>s#k3=i|#%Z~No<~qijLuKE zSnIry(*AavVgM?esg}Yn^zrTWhj2i z!$3Zwl-B}R+6FW}2W^^ibbwYvwE{gw0h3Q6URrF<5Z$QqyRtU$K zAju9viZ&#yPVjpQiM+Ub8HBf>sI@-xJW9P?XUj*vG_Epd`R^O?CE=_=$N_j`P@xAg z6~1-d6FDxtDQ$79NL}P#2dkGSQnXv^ExdHNOw0%wg)6jPg?)zni@3e%5)jBKn3xMO zvB5oet}W#{zASW3wo^dw9Pi~jx)}=RTtI6I{6(i8_&f4Vv~Kx91K$CELc;@=#18(S zt{Dqaql*nil|BA^=RuG-G3WjTYVzshzBHcOwy9!*$4$WB)}6HobQ%p;QqXh_*w<8} z!T1;C2bXIcRhte9od0L^2B$&k^o1Ki+6%ve@6iZy4dS+=(?%ifWPqi5O)qy^-yXO> z55gg|m=2lqmEn|`_k1lUI+u-Kwa`G7vc{us35M2!vp1*@1wYULTaiiH#w-#&m?Q%- zZrMolJ;10K{$A5q(E4sx{0ip&6C%2IO{l(uyD!4Qc}U6H0+Kf6q%{5uucqS^psbKr z6FmRNX@S(Kj~(Dd$(LwIy@8HM-Dm@lVGwXc^m%MxPZvQ$;)(l-$ZF3**woGa`Y#N= z%xIqY5>MVUzdi5@4V7EV)wKNvak%>mY$D7^m)lzLXN0ZSUxW)21aS`DMfrRvNFhiL z33NmhR|M$bKKF{!=5ZH~x}YNUjGSwBJsgS+%t!;xLOHlS3ZbF*Be=VMo@px-0?j2z zWWJU}Q}@~aTM$zs6h!Kt!9F`NAd&GqaX4v`8ECkDzzEuaq6G6;^##Gx0VVd50bexM zHF|FSI*ie~4?xs5PXf`j83g(Kj(^U# zifv`X4Ib}TDeMV!M~Yg-F71vmuWL~5vrRO1sr9fG<@+%SrIyG0_xx!?j7umxyeUHY z;yg6ooSoAqbJO-P`7Cw(I5wCV*umSxLt_idnBa1I#0bZfjO@wg(pnNve*30vqxN6$8b=he%0tEhL8UfLhy_dxKyKnmCUcQ?q=rpM$18 zw4jvwc9F8howtTuks!M`%?jSpnn||XIS_eCp-qrk^xL;%)^r>9{9~yU@uAE*Qv7Zkb%!%W04@hZ7ca5vr zcd*x4gpj-rMlMBzI~|2@e~=z5ch&gk?kfyTGO#hko01%Kw1Ul`cv%9JHTJoiTq|7> zlYGz#!woB${7M@xf`jG_x=fyHy&C3;dr;)vyJ`DCXGQ7jOj*H7{4oTQgB4Y%%n{B^ z-Ny!`hdQAE>(DL%r)3>c(L9qlL=IH6x^Ofc<*}E(G~sZig61=@7Puv+T?K0>`$sY# zFBO!C;~!ktOhAS>qEhFN!@Xsa=Y387XyHx@5<=8lJ&1Y$Gg7pmv8a7#yZ7*l6ijb$ zI8w?&*{~nE*7MP63c#A$Rn9!$?@orGvr$OjFU~;*f{^onZzf4bX{OYrnaTP=D`X5p zSNl;&n-Ea@2FE?q$;@c)4y#i#2|@+N;c$V*$NyXvvwLd9x@-4g?jNXhBWuKnV1)1I zbsq~__8ATMp2Yo5V%k@n1^qMq3O#(9ffY}+?rnAIvviG`G?keNtQL|7#A5Q>tyl%b3z$*@F*P?;h#X--rW z$&jQ0B`Sp^GL+0^D8sv7=zf0h_kP>vkNbWG*1E3qJkI0Tr&BQe&Aa((Rx2uxk{X1=`4 zsaU9`L?bc&>Au%|@pYy4wv{Tary$M`EW(}raS4lme{a;X_W62-c73K<0^@OlYxO4I z*vIHatEaQGg1>)Wh6(iil9glsv-keL6Oj1%2|3QmjtJ3uTE!L;IQc|Ty*q^5-uhI3 zA@3UKT*OfkwQfHkQrIBaVt=zdxzhw>kMd8gwjmMcYHvK z2vULdZ))@FOts)|0305!eF!O91CC&@Wh3PszYZf_Dgnq?4bD4~YSYM0hjU=oLSQ=YiqzncJ~@eXBZLeX&A*Y=wz;OR z5OS*gnbL{Z>Qp+T{GW7lJ*Ll9|J%AnhBVHLBDu)f4H7EeYLFxkkwJmxZwS1kmdAnZ ztcV!=82e3;o<4L3-OZJ|uB(no?X!j$W-sujKh7j8qXL@@mN(eTwl)wIu=uWhD@*?n zF$lsoRq^<%hKs}o5kCiM0kNxHg=O5@w!5R^T_(K~8}AK1@M(VKqg$|9j7PoxDsv6u zqP{IT$Ff@X1!Q_%;F?$D?(r|Vs?)d4tGiWJXP{sdVDL8#KIgpl=I(0)J67@CLy8$j ze~H*{9yh0z+CdB7ZPly!T2IYiPb3aX+EwAOiD$lTU21`Zh7z%sEiF4MR7t5~<8SUC z6#efb^vuna!~i?20+ID-o{BHesR}w{kj5wuF$WmA<*S7BlVmQe3B;Tg-Lvd!@Qffh znGjXc7rD;Q)?GW^dAKgw!HHT*8@rWP77IzlzO$lYzUYQO6JYd41|_WuM#i{hx7Q{~ zZ`g6$ro7ED9j}&mF3dFxDDXN98%=1F=ARZdUo3Oh_JiJLh`4{^uxpnp5r%u7I5K^T zv#bJj94-4cMMS&YcAP$m#x?2zymlHKU7Gf9S3f2^j&A-!(1I#5e%wN0rTpq|F@K$* zI#{7Su)6YBijJR5U;5$ay3}1rry5#lG>JbPUPKoODZDy?c|5wQ0dsJtO3crE40_sz zZ(2PlNf)OX->H%yv)t4i6|g7>4b9kMz!C?Gy&$7IP}<%+!XV9pZ&rr-_vMV_0Ue44 zk;T*NKlT%H_WAS8{S4i@Xp#Vyz|cfDU3S=mWxlC1Am&H@kx#N@!$PA1$r++ z-_=$6ys8SisCpX`l0%1@LO(7Cm4cDzVHek-H|d|jhmcIK=yoMsxo1euMqtOuciWw} z{#Qqej0hfq*sqfyzoS>dlD7HH`@!zh8#|x>)DDAr#bR9#R2S+md21}omL6%klSX(& z%um^GrWEa~__lvFa9WR1v%TmQCp2)<8Lbm)`R|N+EQV8=2~bt|aKqrEKIykP7n_;^ z8%19vJUw``Yf373-5%Rr>IPjTiy$n}3V*a{gPSukw<;~x58yk|!@8FXvyvKq^{R9Z zmw%#1vY!Z&1{evZ^ftPn53iB?Dh3(J!}m>+X$@Bmc*S&fEVJfEcnP5es#)ZZSrbT7 zlywkVGL6CRPuoa#D>E;iNP2RK6)I;et}ATmNB=7XF&y=r-+X0j2d+e5_`isbLeqKD zF6AK_&R7|{%KW;X@dyY`98X66VSG)vAD(k^r;#uZM!$}}?=t@;{m>@2USqp>bfHGW z-^JwveuWTBKe%}9+HD*rQg|2--)9)Um9#avHicrGek+i#)M%@xk`3CSZz6{e{aY25 zcmSO_|0Tsh94N_{JN+2Y;(i)B$^@8oBVN9=-Ou^qu7?xC`rT^2IV*a zwjbJA@qqq4Yy+HV_mo$yEfvqalBVAboKC+o0&<_(rKn2eW%#@m$4M)ck9`)jQ~R%Y z%#s0%t;exbP|`K)C46qI4&DE4i|k!APD5xh*_?Z3C*K5G#t1d#6pH-Sg3|o@ z-8(uXyB?yNzxbU<)Q;W6;rPcU**^$W1#jub-@!Z_U1O!CA51s0Tos_xdKUwbI&d-; z4%>NkB$YyJav9%W|4YgX-$TgfG+(~>xa51w!#qNDX}3zny`A2xV}BIFU6y$NPi6ik zf_>q9U;c2Z$v?+np$iu)hxP~XNbh5QYjpBsaM|MP=A1TyBU=ZINp?)++_?#DyW1YZ zCllmO#PEj6di{5P8Ve2KHP)bqJl#AD5QBS6s95AFw8q{~#RoFCvg`E#VT?>LJ?4Az zMs@XvUk9xKDQLJnY^+5)u8Xhb@FrdPqSD5_`B?i7Qu7!#Q6$}#?p1B*sGGj)ys$g) z*JYOaA+JzP^yTkL0#!KU@E-l^{>$l7t=9tjxGMKSokE_I5+pF6isndf@6 z;r)%a7P>pj9cc6IIZ@>y5EFehJ?-~t?Z0uIY8Hh3wRnpSPT_+oMyZa z3>`Vkb>qW6e2kk_c7Au=?hl$S>BedAhSQ+AuAnbJ14iRpPCj)Rh0gHr2Sz3L>{ccp z?xiGthJQapswBj^KD?g#tmP+6KsVFkrT0I$EwC5T2OY$e=D>Yv4774ORw$5*Tl6V# z;#}b>!JTJwN+-%kXRmp7OaUqimGNxM$+ajnVNK*M4jr@Iagu|g#z3b1``KWC=O|UG z?Eborj3cLf;Nf2INJ&c*P zdzZQ>#50JxZCm|%OTjkdnMWSQad1C5MR?v(n|TFklrGem*U*^s8J^)&By=4xESP=n z85<8lEgXBe?17CL0=9x8ql`OmMXZ$Fv6rSDtZH$d{gt|wA_rooFhfb5i5ef8E>C@)efNS-Hc>rtNmw$i^}ILGshDsSVza^8E{^zKvE)fHjfyE(foO|HZ8 z7s4CJ`pGZEZfOt|`m1q&Nx-zCKdeP^Fjl!; z8&WvOd7U`z69UfX{FKs8*(&8wn-nvVQ*`rld7#anrD+Dt!lT7@Xd&!mO7 zb6le7@)il5*E)^tf|~W1Q7;IM9|}IAu;fb4ks_xhC1|`h2bd1w%N_}>L_CM`#Te(LmErQQf7R){ zcDg^&-|a>-|2gg(G~Sm{q#Hw*^A^ZTYRBq%Kik)gwC_j~v=1@bYM`8!WV^j|!5=)zH6JJr(Sng+hZK^!Qdt%5gr9VBn1m%OM z_q#$U?C+M)u1FFQ=M8|vj-@q9#35YM&CFwQ0G_pv8sO}WnRb+g)m3zf;JI4YsMyO# z(A?c+uok0Wc*<%y3)OfQO4crb564qDIkyEfs50m$NN{2HHiXi;Cb1D3Ju$V@WL@9z zpF8>ed@LV7Xgc>~e+m2EeA}SUmS!XJGBOwNc&_D~p`aWm6W>1hFDdWe^OhsUy1WP3 zECP75g{MinN_3hapsnJ*`JMGcN3R`QGG^7aZ3wDA4SwVM5 zF&x$aFC=o(#QUbjTcOHSO?=*r|A8VEFLy1!{nCYHoG(cP#x`K~i2vN<$t6RTiDk%W zIQr?H@bIZdbIHB={AlsdG>lO7aOco@e@1Ex%rTO%sPAO1!^yVkvBRc zkajy>^Mh_)dLNGkwf@yeRWxtnKwIf8{Z1T2CLh!(Bty}Pha1#4yXIJeRL@`sJ#Bi5 zc6@4LnA-CLP@ayVYPUA+Ef8Mfap;xig zx5fW8yVp3wGGv+kP7=J5*@)6;?eUaEMaY8ATGwOG3tGDs31mVRIzjQv#oqnN_g1P? z-4z)P9X(%4Ue3kNugbd+#n~l8&0oYA-+%9^^b;+44$OejCFhbBHc`7O`9q$KQEKMy zLSR@I(tc>%tX`ZJ$V>#n~{s8lu{-QG5RFGEZ{O@0V%K`Y%0sJ2}E26Hsp$Z|`%& zS*wrz;LR)*;v^D;UmSUH+0vyP-v#E@IEp@S3Pj)2>5G}s?ktvHEFCnz#^`U!aQEt7 zGs-Zt?uKA02Wv~LPv96|(9l;0K1n9TIsNA9!ilh6$F+X{5j>%H2(i6sSuE?p%;9?P zQQ&Mqu-X0Y0t|xd3LzHxR1A!PGuc<7^E8!MJeEe8@HWK5bl>E~zyL&z@sm7{q$Weg z!WUv7SIzlec;e8x;5m%5w|~p7D*E*bc&_jcq8`Z|e!T`mWSrxgDrfQpqXsB0M|30w z2cEMwm-#DU!A0O-cTCwnH4rhd#VFir;6s(1tB2_9k#^Db)R6_J6bb~;t%hf<^W}!$ z)|k|kXS3=g=uYAVjI)aYDpBaglaM(7bz=gANtr<4#LoxqN~?fI-?27nQx+>XZsh_E zqU*1oA_>EUS*Ahjp&#eJMOCNSXbJiz}zH7MWp(U3t3)@cJ`DRQS>Ll#yj17&Xtnv4CX z0^8Rve7{~pbZ<=A+u-03YS{79nq;Z<7*e=b41j!lj9Wk$o#&y@Dk!9>3o@>)MXyXT z;D&&G%efs^KS@Nq6PmDWhzJ{S{-^QDte@Z842&t9s7Bqql<|dXRH02EMb}Upd5g*2 z-rPax9VN8BeGhSfjZ{ZAW2BOLn!Y{=PfS8$RBzD19 z5_{Hd_oFruWp_jmWvInRqZ53#k^UU*~-byn$tlBrZAMk#7+X)-(y7=MY}H6J5@`F6fdBiCb$LlxwGXg22~vs(z%O z(a*$j(foG|jmfKUKsOqghC3j^q zcog#)6%J!>>NOm?|KT#r`G1x3|FbQlDo`;sybwxcL+oH{_V(&@L^^MdUbD?a$?ElB zX}xal5ci!ks-RptG}vpm6YBoV%uN2#l?9)4r;4n;(t>u&Q~C| zNCWD&x1aCgp*wqdp1g74$0r1*MowI@2hN)b39~7nzUz)}vn6(9GX6V{hG@j(yYk<^ zuVPmK`XpwdvYKTPqYcGfY6R}QO|?QN0XjKiZqknZOKPsUi-*b;4fyD)_8{2 z!UAwghJ1fx5Jq(qWd0muqn}b3LXLEDnWyp;c!YXHQe}Rw{948;vrwRLTN(xECa-GL z!fC;{pFK(W)Pomau-u702xk@06gA6|OGdP^qeuX3_%W&*7sKDrM}p<#*PF@!%Y;BS zUx_or4^Bb-KXUo`UJEwTl+rB}P+$GugKJuQKZcuVfw97n#p5LJGY2W4axswFIBDOx z20z&eEv%=HXDiSce#c<-O2sQ}_nqLSix`zDKtw|)kfx%dcydH&SZ5Rk$3@uOc8tH` z+L4oQS`S&uUdR4_cg={etA5pny)?@noKQ8_&eN>H76|#QsCsLox)jamy zze z^cTXlr{gO7_hBN)%1dS=n(x6S-jO(-dX8^RbvXNuZFhhuH=x5NmPR5CR`76_Ny4Pn zyrT?`Tf|IU2@k}jnYGt{Dm1Oj0QY~dZND8+S}YFLwG_&*F6{4AP3-S${bC&p%L5mB zJ@%V4sS3>te~RUusz(Gq9~&RA>REFAz(M0|jkiD4^rGC1DpGgy`~K?v)s<+|fJQaK zB?OPbctvJ`DBW?SsVbJqw(U-k$HIBou7OiT$-|tTbKzA?XlG(dkH_G`cle1r)`}$O zM>OOwlwU?TzpY~BLWpV+dqtj`n7+TdG!9rK6h_*?98v` zXb*csEVaKxah*98PWkWXC;ow^(GH;=amc^rzH|5Ip_*9XNUrRib1e? z(F=!I=dX@u%W5VawuI#@f1TT{I(3(oaX>jAZh40=djN2^&!FX~u*BC$so}l#-|<1b zQTi8VP*{oBeAj+H#|EH*lrjFGWV{y;z*%I3c{&1I;z;b?mA*#3hKlL)mV;lApwJJ0 z4{)aZix{maL&3(bW4D~)?lw(OXTqT?%ZNc|$+@oo*SD$2AQ#;ogpWf*x=2~)b^dzI zC~c^yI6V+57>~1InnGkM1sE!Q>;zXYt_7*mHV3H?1mXlib`#D3fXr~Nov7GOe2wQk zlK;P71IhO4KX<@T_Uu5a0zXt!+4)eot3$lN9v#F@PNLzmvoAh6dXL?o8p~VY#Bb%^ zY7YBft zJt}aFGJOV@&=n-Movkc^8j%=pWf-MT6aWSdv_V>QCPGXkpAxkEy$?E-I1f!GhP1sD zoQ&A#kk%v}hCsF2oan-ZhgD%jMq=v@6wQ&wAA48gtt}-NJ;{*&9j}C-xW(gAUw7Qz zdzZN4ZoLhWZP>7bg_Ok}zCt`!HkAmZ`9Bz5MIbZ3@wpS|ZRZ-=B6m848KYo1u6^klUE?7d<(zhmD^!+ z&FkE!1*X9C?_+b3PX`C@^bpa$K%0L{&13|gY7>@{ZL0+}S$Jd*hC4>>uR;%O&iWhc zx&Z1RV#38eM)iZpL z2#oYgw}qT8v#UC{cE8+Ls1Aq*O$QLdggwznE=OEbA@NQYoVmooAV0=&e zh~)pyiQ`z5x9*?jSg1>uYW&bQa5dFxSKKzu90KEZf2%q_qCx+xsa zu5J1mDxcAo-(n@JpL!FIMIQEP(_(DPbaIX`>u*b5FD&z3qKolAX}|TqV~O~Od~y&{ zJcGbk2#*Par$7XO)Ud+C6*5f8~K5XwNr^&cTkXh$-*O>e{hf255ecv05y9L*M*tszo)ja23Lx> z154-k_sX+E{C0u&sb;MJ^+Ni{7?(Y^N5VA%47s;e$Zlfjhj=jXyU*7OWw^;dwcF7t zzjF23+y*duei)X?p#QN^9`%2Ec40Noo#*)Hvt$JwLg={jcgj6Foq-1-o#qEVE5Zk_ zS;e->mY<5SNmBu2Y1b9}23v6R@drRE#*sFcR))9=5%-SChejuT_+R+wjq)89z$8H(xx zi9qlOy2S5oaIIMNzh8>qPUTSW>SpmD`*gU{cs33IcseRY*KZ$|-Gcc<-+?r*WNwKT z_FmXQ`u)s2iU72RbO%f1EVOX$Bpe56`H|s_3wvWbbpi+d69OvBA_KJ7YWpA{ZF-gE zJwgey4o0y_LRgr+lSqea*l~o9kVM=)f1hL|s1a`a!|7y@%Y_9Hh(17G|3gJkG{yI~?sEUKA%Xf{{BLxddx~KM zk`{JEsB+<%2Gvub-=m|jF%lnEjVx{b_N+M(6)?I!T*n(KfE!= za8E_v<8R5|011vdZ&*^QlL?+%NarGk0UnB4#qPPCgo{#NzuR(`8+l!Hz8{rD{jJzQ zoQOMl`sVj4oo+zMw7zSwb*by4SR!`hI-GO_(UjMb0ne#(?5Um7ztn&|o218#N4qVm zpjGIT2vx7ww%dzI>zFknSJqwbPH4w^u440B;JEM(>oTLZ{44h&B+$2z)dD$9CfD?DsHl|JQEKf32yo3e>Y3(2fyZ8%Lt$@CO7c7=h;-^;W1p zqee>}Qk&#@oc4>KPyxJ^gX-)m=8!YnLfvk=@4E{9HXW)RH3F5ZsGz-7qz2QM24Q}> z{$hTiHZp6-`S0BD|1L@V?J*4U9kA1gpHc?OG8!ck|I1EEUBS1dQIL65Ld9; zi+6Y-Bbx`=sADE*D(~FT%>o9P&lrSEOecc?pnbP~N~s=1#?c$~4R2pO{4nvwj=!Bu z91nJ@*cbhuEiM(+M;klGfvwmc@k_@*_7lTVoO1?3&eZ% zIXavk*{zAcWj<^tp>$_qvTek8$=Q(&`Sw*Hd`Uf2Gfx)AnW$l4n~|Mg!;aV`xOXy* zz-hi5B&Owd`6YCE0wq(kIKg{S*}|%yZ*neHVA*~LT=z~`CmnZk{=r&l6VGe6C|bhQ z27yusXW7lI2%gv@evBWseIh0M*cuI62tAsE8vsUesQPu5GeP{vJe&YG+Szr=iVzUS z0xS8-zm)P!n`9|cn<7+v)(B1MJ6~ReR%$24dPumaFCuAD(Zy44CUO3s_rpM5!HjYJ zOtQS2A^MSQu!s`pmReOG*FZjPrm(q#>6p(yEr4w|wOv*MbCH>KlwMw{@+q@@Y*|Xo zgfOnQVp~=+a@|f;deGisEGNiKPE`dxuoPSfa@VtOD>-2#uBdYK^~2B!@1<`RFAB=1 zxEEi+NzKa|QKPo!_THF7uTz~SEcP;xqX;maefR%>in%8zMs4SigbXYbM1{n4Wj*8v z8TiR6BV|7Q*{)t)ej+1&vgiihVCDJ>(X-0?@W_;Ph6Yy(;@H=AsJVCrytXL}*o1kx z%0cS!mVn+55L3B@9Y|e9*&aC48ZR{2Cq~?-Cg*be$*GzMf%V1%RaQIo7Y@lG9!8zZ z>*!d6GGWA(BZY_3aL!-Gp6|OSCTa=$Ee$62Mh6xB^@6DAi{8?qbO9Dz63LJ#v+HF0 zd-PQ|k;`Aw{!xK&(z|qt3)Kbgsr-!W@v#fW}Zm z5SS&$I2%<#_}g&`n}H!sGxP zuDmLlwMAmNQ$55Bz0fj^o^X6%^~L5&8G&*GGt}(d*HY%y`dtAtOzR1a<#RKPEjww- z;k3(S>9j*oTnl1I-ae~Ak7$f}NepC`P~&1T5#|IM{)iJ=2DhodSOEo90-_W^BHw&{ z!Q9n5$Mhu#@ohd7bb+BD$S`g%Yzq=oc3}u#FAHp8Q~R`E^BzvN!(?spsVtBwEf86Toox?R>=yUquMHVhtD(y5BN8g^PYqksi52U zW;L_0(5pR8#d?V-79JH-r%)M-tJtJ|OMDmdCM{AIl5uaRnR0w;>*yJ;K~q_1@m(P8 zS9kDQzs6zAEp5_Y8x>U8>jJV0l8m^gWqq(jVTV@DwUc5Y|D>U9SNh4}H{YQQ@diQV z=OlVSL15;=VzAUA-=AL#Sf?7Gk7PlXI0p>$Q`J1nS`sThmz*ff$rS|$ob0_=9)m=B z&bhziz~r1o%O`v4S9N~d2jyj9_0g$z#zRshOJCwK~$A5jc#X}I5j_J7N;W-5{Z-cJ*lT*VAG8kjPLOv+vBAM2bP1PP~fmFYP{2#?2QYAYKGK z!lhc8qw?nz{XrmdF>vWtW7AgqCpb>x(8b$Uy-OP1LTdfeo6!I1zSiWZ)n00`+zPxJ zb-bFApDU*DYTTt5XE;ishHSFR>M~7)1<1R;vc?(XU z5tBi|86uV`S;mgk|M6~T8m|ZP>36HVsPf-8acC#JqS0&&l63lcwC>~V{GsSFll15r zntV#$`Yr)oOFD&Wuu4BuCG7D<^EU-y>bN^T^;Zhi3ZQ$@4$dY+*>llG0dCC&FMINf z-o29l;@4Re686S+!*(@r`w4n^?%aN-exObNW?;T+ z@@qWTCvu{{`F*{KZ)%i$V^NhZGW_Z+8{!L#BE)JfN${*>L-8I`CI{V=-z<)D({OTbT8YAfurGGNIzmY7~qhNN&^82v=%Tn`+uWJ)~ zH{bJ+Ocb<00H61(wcXMrzs=9$1dInFs55KHQ`+>rE2DhOlH4|{S^6VVxTb>HaLlAL zyT438GRLWv>Wn0HlBHgv%X#9qlFFFbgwaGUY2_z{$^FiLxk)adO}Uh#@)R#0rmqvK z%eV}s8^QWoz+^Pik$QxdZMiz}z?rGGOVHmO=)CV~TU!c@IbgQ5hE2Qp`*En+wpgZC zB_^A?T}I2jsa8eF#L%_DYlnGhwA*4tLZ$%o$dlXhWob|1St1+zujP$vrH=|hjC-ip z9aDa(=q8OZ?%}oY7i#2*M1}IXRvl6>*Y@uht0T(pRr(B4+?l!DIJ+KW0_FS;AF0J@ zg5hl2_BG5HllWDj*Di%GW+!|2GL+jj7@FMka3AU$FFVpKJ22`yV#Wc06Z$o7$vZ?% ztr9jHyGVm!GNLBnvBXQ_G8^ZH9A?O5-n58sGGro6Ra|td|6>nraW+Av0i&#H96G#3 z`48815$R{Fe@@bnuQL5JNI9|RjTzu&*{VqdvDE|Ix%szsYP?=7V|=%;&G)ey)$;!wKv!Uv=3 z(v?dt?iEM+85>Grlun8{8)7#UT#8cN zds>V!5DwkqGuxXQnAe4AUf+B|;N)KU8^vqH6XoxhOry0gK+)9;8%Bh=*x%=YJ+}Pq za>P+{REwQ$epr@b9!hkO-ZuPT`1g^S3JHVQ(e0_XznL1`gpFtETnoRRQAM~IR+Roy zB9+ie%tL2sK&h;&?~CO$W#J?a5q)h!-h-VGv$0O|q}81)*jham0cafk{Af>Nk#$B6 zj*VYP2mPV4QRLS~M%N#xIQlW1e(%>rc(TieOKG<6S!)~k=$U+K)P$8(^mm{=*=1aa zV@{C%y2>A&P+{1IDfz7%^x#3h$bUHZtUsKIzwqT;93re&e>wZ@YRnY>>OuL&y(0I5uc6 z6&DPyS0_&#bHc6mLu1+asv~?exXsOOHpBzs%&RB&o zlg#&yJt_NJ`ISJKRBg%cA7OYBU$@DJ{X4TqTMERD6Vpq=p6rz=ts&^Kd&{+o=9~C7 z&UVFYWbm*aKMnlb>3BlFuPow_n%kZ}(Wd%DGh}Rl4En~!>l0v|k52W?P zGphjGnrhLiNw3?geBSPt3Q=w*&n#N``y36Vs1ruew&m4g)O>zMKiP-vS1W|)fUTet z%tKsb5HkH)zC7dh*KQ@e((x2FP3c-u?Vri*^0CXh$D)Z@qfo@iucq?HmTmr#yZz;w z*7ul5nr+*5ePCOB8`be+zQ8o_^NvFS!05EHTY1~LJIzU;Ylr&&#)K8GNU$w9^!~=&YmiDf_NizIwWNc&kkc-vt3<*HMFzziyZu*SJxLPnXZk=xX1D zQKrdC)eq8Mwlj@q6{s1fjeuQ zsyCH-HoM?MCMmHA;dsHU~}<;2#d@?PeJn2#F4_FsoT?dFcm zML~hg@lyNEZb`in9>*^H-;iGvPXH5E9$PkIM5!`=h!oY%hBVEOc-@Bl70exk^7^Iy z!_}h1m3`kI>-{e?w*`33_=NsT`3z|qj;B)+U7i!!_mdS3>tx&lwKkopaK4B6-&}H@Zbx1 zxIDts^dTuSh`yOJ^xaxI`0K{bM<@LXPKB+nbLVma$hyI~1#>RRJao)hH$>@fEf{Coh3wl2GvWw-z@R zXl%<^&j)eKumG=X)ThTMy}FhYS}Fp_rbN(?FCAe6u=*;zL1Ky9JE{~Dzk8(&FK3;x zWZ9fS!=Ec0cP>C0pe5?b=r~#OG+ma_P+J~h(fiyAE(B%`y%d@0U?Y$hl_0<{7GHFH zdKF22iWhAl1HTM0MlXF{8bS#6nd_mLM=%*UEiWwImVXvqQ5a^aot7h5SY_1qGOa0- zEoH}#BX*`4c^DW?Zm=%f^L)@>i93+h^-EdlTFc{U=ONa0tzpkkqX_9;j4E=OAQn4A2IzoPW|uU@U+7!dmfV%EY$ ze2Sogn||}%f7*s4GtBDr`T4rV&o5NLw?fzq5PQTL=mw)1(p`VluzZ6@8-S%D*v zzbq}wp5WU7HSOK$eD*48RP#}5!<#x%aF`Jsa#t`Xq&_>cEtl@Q{w8r_#&?5c7%WZ@ zLh5?cQ?Dk~_P`ELoI7JI4A%QkqhtBMK*b-9B!mf$giHn9L!ja-UjW5oB(z$%-Kx;z zj>qIp_D=wdBFZa911(+Vq6B9a>w2M5!$^L!bu zZI5s0hq~=O()M_Dv~ohkKARW6FUN?T}PV#%oh8b`mean<)b}P?chK&nI15KU;G2dluYKb?Zxq&yfZ^EM9%y`G; zj=i4+&ea(-blMo8j8|vpe0|iJ+8z=_&m+S?AW^pT&C5IpZ)Z{%M&C0c%|R;_|h*tN4}IPPXngc^>RPjL!2EC@7eO(YLU=t zG&=X^S(SV^H}LfEGAhN5k+1z9hfFwd+~Y)5j*{o)qB7?hzUUKo-R+Fv*jXSZ9|U_& znIlV8H+wHG#W@*iC)`9O#Dl$C8wnG*EpEi+WWNQlFya?gDUAN!_lb|*16an}Sf43S z`BTN7r-05N7>!}_#Evic+{>dk1BvGMvatd8fq?U@`oJ^8RW%=*}zXpm%}jHSe>#;Lq&-DII}s9A7Tny}##% z$r*$KXSR~Z`}a-`Zhj-+|DtLX7Tb)8!xRW*;KMmF*uHJ98?D$+F51M?)Dz&)D&_*s zBjF}6-Rn_#83ws38|}G)2%oS$f`HPVMR>Z_u3E7|NF{pU<*F&dymXiZpG3|gCRU%7 zKW<-;`^Dn)ou~&T z?ztlI2yG9LT4nM;oLiK7?kVw`ltt8Z5F$;kE0PtLanLSrTju&I(6Ym{3E73&h&lPW zoM|$|5hbo5^pWq@h^>*NK*Jey%<)E#pc+ZU{BYpnslCr>bAmv`4xcA*x`J0gkK&Jc zQ$0G51mJAckDTrQQj)qNEC}q#uFGo%ndi$v3rBlj5TqlQq0IfOtTUrn1xltKhsMzw zu5Z!BE#cgHpdE`2Q(VIl z4f>wYmu$;y|8VX_%W2~(G!#wRa|#;voA0(Ns%!(OGU#&cnN8p)5e5Zk3AE$}&!V5r zSAbvZWk479uUKcJ_AnV(oRL!DqWSgCz-UA+S_zcYRQYAw#5jsnBW7JfrMZr=EU4RU zv@j>EeA;|phEb#LPkf`7VoMf^81Ck90s^ zBB#84;0m#&b`f?b9?|RbpJRgtJ+40Q$BTV0a^)?Vb62MCnLcAPzYk^vkwnWl++j%~ z0RRMTvU1zOGaO8yUb!ZZXn8;qBo0W>y0{JZzAK&@yKR2p_nLU5v%futo#3ZsgW##I zZIIAw6Rx3qZFTB*Jft?2Wib$ah%_;ul*kXwn>pa!0Azf3rH z<5STWd(shcyLi?b$7jc*QSW;QT63)Xnd*v@du7-Qrp>i~R_n#AUwr2HBbz@|ksQt; z%M(_f!9)_J{UO&i7Z6*~k)lRuDGpO>88zn;2RFWhOG| z>Z?7Z%u7Ckswwcj_)mW^q(P*8b!gW6-F%EUpMJHvJViSb8~+^&4yXAq{~Hi@?t(ne zFXKh!X7}iBpikl3iMCjKFtP~BJ3H2m=6BXSTYy0dSmZbQwM+zALU$TpeIE%VPR%`T zNry>O(>VxINz0QF^M%MEj!t z;Pth$# z6bbqc0YHVxLRU&Ax*HI4avgiCZDUjnFtcm9NaxYQ#UI&Nq9y4}i;`3WRXqH0QL9nh zcwf`T+A9iAJ|k{-WXs7n%a`xbd-?NoNmJC&-Oy(gXin0Il)sK~VHR?$HlR@X0Y&=< z!i*|9f6+NM;1?PU&thfEp*b-9Fmfmc0J{m)TBWOhJld3Mdv2vvvbV4d*2SYLf*9g{4Tq#0fjUGKNxV69?_2^vm+mwolMcd15Wl4S(1|#$!&OS`W zz6WC9MyW?2KL7rYSDXV=1A4Xq#JH`$9YOrBEEWu?QNp_r*Lo8-PF++m-C}s;0bDV- z$>J8OBbAcC(6q1T-db0q_>|$55juPS;ncHgoV42oy z&5IaVNQUdNuj9Nq?<&BHGtfRFDnqRgX0W-{l~`4zR)?U2!ba!7so}8xxNi+?X9eym z`u@63&bnbC?ICyENuC-_i~G$l1XfCe03y&yZy`VzfA367vI6Y8F9^fs0>V z5LiU;T~5otzbR|Tp$0)lfbJtM`gF>G5W(@`iF+O%3U8JrPx!xB9x_3mg{(-bGf`A) zqK3z2qeEx%*wmFl1T1Btx%1`W=`J6YVsn*oF7TMV3>#o0(HL2s?G&>p5Q9r`@53$# zcyd}Slc232j{Flu*Ln?^E#cTY8vr#85GdAwwwS##AH zJ*|0yvrZ4XX=A+F_~#R*3P|bTL2|pQRYkKRd%KAQkv`dQWNMm?O(%ZWq>!R}I|EPm zAC3B?C))A6Pr2@&rc0A&{`76M>V6_bj4gOuk!5ndcdtJ5f*?B{8F_|`jErrDb}xzu zhcAv)&Gb8)N4EG5=une~Z%|#MaAo4F6GE|le_%rUz%jT`j$GI$&{#$A9wP1v{B#%X z&Re*{hEG{tN}4^RnPIVYv-$wvH=$*$tY)lXZ1K1en6}SN^egrl^f!&6XTG`s*3^g@ zy5U*+XMe;>@0g_Wyo+MSl{?v61a~9r^{&xS!xb_t>wJlt=kjy4X`c70GiJ2le_lbU#pcJzN9$U);7y3SyxsDHh5q)V84`sN;Y0lJn0gg{%1+KqRLvX^XEJBY6mFNsP$KYOZ z+4fo=5@AwAC0Tv%!lgQe-xQjO_j%ru*OxYHop;z2wwQwJ*=tGG3N2>{+YZyj(AOQt zsvKPr0>2_quT{Q2wWZ2jr2h(jaJS9Yo0z?kdy`%?lb&z{8Kt3C`?`D!(AG`Mx59K6 zuD%hB$3GPzy_gItT_?8D3A^YT)GjG~LT0Ce={eS0*s5+AjJDAjb806@B-~%5ag0Cn=hRI8KP>=f5bVEB z$IGD?xYHPwWyFc=UQWD93n}bH%e8#;JC=Wyfvxo?c=I$|KJ`bYi7Cu`!5PUDD~^C( z-5KMKZfTa{Zd!vVz==1Ex7SE83OX}-iEmfpkuNqcggm_-`)YSreGvT?^X@yAm$J3a z4>&LJX2V)28RVaOxRz08UkLQ%oPi%hgXpHrLt(!ll@6PTt(w9_os3TZV7O5;W_`)? zANIYO%i1K=2q`U5xMA!nrX+(DZa9iBs(y6l>tRC%#fx@3zA*n;;n+0~;g=p@}BRvHN?ql@>1Z=AZ0{-G0*Z56qo~eM)9Unl(mR z(MLi!KU5R#KSWu*O{GseoXN0t{4{&2$%SF!xEH&Oqdr{PaIfsVu(CnYS3>oc$C1i#qKD4p_xjfRnD2PA9) zGuGGuEN>PWm<^>XUHO#%vNu1qj*vl)E)V%Tw%)zFb~SqNq` z%Qx>Dw)H2WgBQ6YUaP!uceLup+2#c z0iWI2`U>HRqA$xq`ne_QLfv(1tmF}-2E>&ING(scVsJqV#qi~LjG8<^QjQbbyWIpDaMPrB zp|PeB*>8}U3wMOI-+XXl&>9*5yGi2{&~4<-ctLzU)%&bO?@M{$7n7OZulk7Y_?1+> z$;(~JU+C3zdD_(d2ku|h`+p-tYZnF&aa{GFA!-p4zgb z=5KxOe)&I$RXV=^T%YbaUiov-Zf4eP!(7s#*njh)sL?ut{+L^-D+P8T^VB%Ib*XUW zhSRT)$rE=LS;QF~wWw2Y>XGZ@J^omJ%AKSslxxa*sG5_qbm@BOZLg}Q6T$lFOdu(tj&o9>4Vo<$wke(c9RWSf)dH z&yC?Q?+;;?r%9RJ{>Up(=6|NqbV}m*H|4sZzcnKKW(QfBHXMhX*45JDz{wac4dvuFTg+&VE{<}7}oQ(0EgzLsqcgs`PB>U8e&CX#XD<=w1z_!+0g z_Z{0WWjupp#-hxxxcVCO6IGUoZ@b3B;c?W$(DNIx_^M5*A&K!#xOYxKC(lYkX(4Wd zo<&xfnK-}$dCdrq-)0sh^@Z)53pnJejjKCDVYE@&7(9aM+sEhy-PKr#$3MXD4iq<6E$1dek0U0|$FNtMCkp?57(Mj0 zf7>U~GLf;VNv|tiLeX0jMEnd^G?@OW@7B&xDZQ8#Y}1M7oKSOIc|>a7P2l!(SirTjk>ud+^L*fHkbB@bC|zPRzw!>vJJ=iWlhSZI2C<{rF{)N$?h zgk3wy|#WH+pIH5A-*=kqJ`z~~1M>F}0@ zJfEEWwp7*MoM6c2g`Nc0Bo+jT*De67@uh^;2$aSwbuSDPfh((T;=?`SXy@3_DkB3V z;n8c!OGhB?+YM>$ORA%k2Hd^%>TN}hT+pNz4~Fv)J$$8*3`~DD)SE~&^scjf59+|6 zwZ7W%zIv{k)AK**6nLW-kIsWSm}F`Kb^e^TEcH#8cp_2zh+K3qLe+(Le`3^3`fx4_ zH-%e=ABaws!zzpk2fxYMy(ZEwI@amP{3UDx>C>s+NzXwRAKAgqN_4cnYVCVOofC>X zs-thMAJs7}a<_Xsfjpw{U*24LKjJV4T`Kx6u@F#!Hgb91!~?F{X2LY;a+GOphZWxb z@W~efr<}TE5Q5-{@dJsQTOiF@Erp1{FRL=fwsvj?-BXhrMJYQ&YX@OT$c^%cHv=!3 ze$2Rc9q|lqYtu}8iMy0S!|HMBz2%u=gjPuD6Y_xXwCcxiJ6N0a_N4-qm-xs<@VFl8 zzS?lpd-~7uO7SgwEdKNo_opRm7pfIGnuLE5prWCcfOYc)oi0x15Z2Y0`uhm_!r3wm zq-^3OZ5)(Q3Otv=~o-B8z;KrDxRD62t5_p8!Gi!8CatlIv`D_#>p@ zDCT8U(Y_$4#rj3$?x$Y#sGll3AuwvGG*b9(Nt()xyn%H5dErPu6 zl1iN%0;;vWxL8UrTNrNqGAaA6j5G{!7tt{~E(`pcGs--4LSp9gcDPv4Bd;J7Vv_GG z%R7;kG`^9JzU3{L!uX(}aYdE7f!yKmR_~ULVlDkC6Wb-s0w4)6uXW!_1U4#!`!}bfj+Rx4#IdQ6C}eaJBB4?b$x4Iu4*v%e)@crB5 zA|h54tbPQ_L*S4TFPy5)cM1+JmNOJ|vw17M1nP{BkIl>{2VVy-d+X|UL?iB*fzylV zmDzi)2wN3uBP5J;eSAGH$kochW)(S+%I8^Phqm1T?T)*tVqHDK?`mC9;x6e4Sv1`~ z_aKf)m#k<8liZ^5sn|>qZxkUu=2x*i;Fd8#05R>G76R>}VA;~PXfw@cM<<7G0EGV{ zzu+?qs~shx#6S%M1^u=-RZKU?CiTwY!bS6^zWHC_>btu$B!5v<$A15WuFqFTbOiiN zx}M$@P!*!qe|oY(2>W(8*&Yv%TPcZEYLCnqwb=87jWe;zLsi@$pz?ja#=;Y18H!H&mwQLLzZ%?9P!U*8^2N z%ZQ!ir!`A(k%JYTkWue|jKC*h)~St*E&f7P4X_3Gd;`?EwRYf_auBg25;=b77)HF6 z{d1^P4-1KWz@(MErWSD!&*uyU3CfiSLk8d((Z?rmLj^ft+-fVzn2F`HrbtCzmPQ>P zK64pLwiscNqOs?O8@xwqB-@HEg?4DoX_`CH5ozRY#&c>$M}OVGpGTsZX84+jJL@cY zuexpOGNeSDJ{?Wd)8~Eg?Zlb6i?1(Q=zWBXWVew;Ir&31=+b@6strz!I$yu)`CByu zv+(i?JJW6|FzmGR0j24AdkVf=sdIN$cGIb>ZR{?y#Fx^P&oFRD6Wnu^=G;6-Oh7Ah z%}c!F;emhkI)|Q=!69A4iz6LNXU`7o z^dHh1dH=yRXAmJp+Sp@k=*q3eZ<-~n*AGC2z9U*wQjCE4wSi3+gGIK>YR-W{k`5!G zW8m;w$j%_X= zSE8)*qV-cweEw%0J~nZ{dvf|}JdD~=`lLm$Hw!PFP1(=gceg(CLOs;VXNy^8QzM-!|>XNvm2I(lqv1o^$kdFx!wA`bZbdU zM75TH31kUN1ks+ovm5!sPXU(bYH*#_u%0RwvUz0hh0?8NhUMQ4+@ZQLiZC4XzKZx} zIr&cT`X9wJXp1SDHh?%T5u5I_jFn>aqA{7ociaPDHNX29b4lEKv|9^sthPdNYoehd zr;Q0GabuUN0^~@%dHhS+7s09{<4)yUdsIdb+)RZ?WpqfF%xHWE6JC_)emRLPkguFe z#_siDT%zl>wge;b!tjqPJtBlN?mszDBsyjPyXnMb-3EEy}U{kAiJ(~ zy2IUb8FsH}zLnF^dxOm}|BSY(M9Q*b#hZFm#!wU00MkumqtyEQi?GV>?#O|^De_IlDN^z1G!F}ozEw-&;ixxM|o5bU|AI` zTDj|L$1bVK?zcP;6z6$Ap>y^{v;6Y8HOMMv^-0Nf9N(RutBsZG?Q-4Hr1eVNorW#D z%k5Mss5JijDNT3~)5DEKJ^u}>55KvwQAN3AeKR`2|J}G4a7h__}#u%i$gZkX%`?yF>^qP!D}`HPU>Z)hRW{4;29nW2fw;zfsSdThzr;y4xR9aun%Ja@L>{qCOTm>BSYqwFUtf!r(n(w~9TvtltpO42{{z{GH()!K5cDs#nu9SZLs1)aYHhE;Z`usk z!tYQd3eV=1G!!76-UCWN0i8}K+b)@7r8}a4b3SI732vMokqlzAvhk7K?!s(ld0~(} z>;}X)rOsF*y08ab)5Vrwb`>6WmQz@I1L{@d{`+AwY2H(Ub#z^)st{@ z`zrVAM(#rD$X?pZN$SayCmrxGesYwjarK>gg%@(J800Ouzs=kmZpSBDJ9GQH!~OMQy@!A;BEee17A<-1}1=H9YsgPA-wh=Vhgx5OzLk2Wt3f@9{bUHSh~Q#rf&Vl{W5@rgJdrqHO*#tSD?=waQkZJ^N3xeE|`rwLK*`yUQ)56U3)PSy7 zW%TxqGrD1GaY~)!W1X>NtntK<_TB(@9``RKlMH-C=AHucMuV&2;U)7g=H1D+zr^2a1tzh9%)hnC&g`&cV-H2rJj^NsU@iOHY01LKLv z)xa6aH9Hf9c|BOHJ#Z~6w@7?-7j-62Q|%{?qo{?JSlpI>Z*sj$oz*X?oBh=%IGQ`W zTD$8orbX_bo-Wu`lyz-;`s~i?6sv!^gwvXWNYL=*0-3z>J;%!?mQW41?DWB|n+#=9 z(r*UsMkc&3rvR+6(t7{#U8@l~Gr@9t8FTZ&xBL!;Os!ryWza}#jyih(Y*?IaC7}wn zKud|wMfCSw^<&tvLoYc@#Azv?AGS|%hc$m~AD_K}<&@`Ue>W}fi&^Mm1YcAlFD>79 zp2E6^r@emT_=jIe{r8?dw7Q&OGVAE^k5iHzrzZ$9-aK>zh>VWRDbI}z(Xyt{T_GDT zd%4XG?yY-Bf(3~e%P8jQi5EBPJKIjc5*?bzS>qe_LHsDf&!>9qPFURtvb-m?JiDJ4ieajys=akr`FPn1$N*OCaL^PkvwPyZxIs4%}7E~ z3Vrx5L1+_+&`yX(zs*a#+Rv`3app&lWnjm6Xe6FN=LVxYIHN z)oPTA+y3ly5783H${aYjtQ$qk^y(379103|eFdkECcnKZ(@X3a)++}!AAN_Py(~iN z_;TG-l+SZEdCwcieUZR3hH=c4MUY<>D#!q>qK#kWWoV{nS*&h%iWR*NDQ(7b^ste6 zB2WwJ`cFCN8+pHxHyruX)2w3E*e48x_knlcSmTbeM?w6jvfUSzEa$8bKbA(EzJJuw^sz$8+Lua4 zhDz+JGdEd}akVewYYr6^+oWJLp}bFGnt5_+DB#GBrFsKX;Zs=UF@AG_DGzp8gaIul z?mE?j)|5YKmryK@)EU{J#LtFCQI{0)?RO(9HPy17qqj+oj$W$bDu(69frT&Urdo3+K07AE+u5-r}iwg*_hLM>nbJ)D|brA&SUm$E(=U)!m5VuF8cnt zkt|#C+80nLpzwJ4dfUA|^yytQMh6NfcI)RoJG!K3e7<4lScw5~hJ$c(OoMBGpDrE# zZ|{b0fLU~+L30`&1A?-GN}K}{72_2hBhH!E_gxu4M3@FpRJq&Fdj5B4N9EcNZwpXz z#-F`|#Fv$G=1j3DC+OxsX4^zvq866Nw;CKB6;^sF0Mj6+h4$A!+E7q%Ez!<1;Yk5X zajx-baGa{|V{b`#9xAs<${(3Mnd%BlT^^GL=PSU(=z6(AhA(F{V0}|mjQcc+J;vvo zKEIFU@Cg7kF7u{9q?>_v7JqlO(u0*)A??_^SD4dFO^j{H!jkS(q%Szs#sR+}T?BE(uDG~hQm8UP0U-j%vQ_Y@o2K&T8m z5E)J567d*1v#b|Y*VFWce?O(Z7=}a0b_ssJV3N(xi>+pC?50C3+Wi zUDG`TD$7pHp^k5E$3e~p%gGpaQ0_<5tRJ2kEKIgc_&pnIm zf}H|g@zC~n{{_wjb8oRSKmE}krC0mg3elYyV)}EUT`uB$k3BsqxOzgTJ&v_J9^^ z!%tH_>U{COmg8-;=+~t%rl~oqc`-4M|43RMN{4$X$2?9Ll=v26m{MBa^I01Up>@bw zZ!0dcU`%Cu)diG%#!>UbX2&Wz?l8M)!v;g91ZcR1t6U~)9$V==A`4C~b(~wIHZ%i& zTJp3!$0K7HFPcq@N0!<`+doHDtz940F!0r@ujE!xg z9yAK9C0D8@-uUz`@~V|L3jfUjBYqxuU~+0C$dSQdIWo!{cblDTJ^m5oD*XZD9NmDx z;pXVX-b+MCjRwpJ^bD~{&kom>$%gIrFJeNn43o~K2+1Fv|EE?yQ)aYbLJNa9;S!lF z8;GaH3}Xft=*;YQ@b{yM877+M_rb=OsO`!^K+HM*DOYrV6K8upTcW%1;kxHR#poOg z&vHOErNnmCtRc&6ND;C5<$?Q~-VLeA@}o%J4m^S@&{GH?rn8k5E`5dm=LJd~Z>0>k(I>7Xu9 z=Pr^Wws}0Sln*wp`e(-M!=AAvdP^I-{4!c$BHpE}-z>T}rwfG%!$=k3`TFH$O_!4n znJ+xE%!}U<)IY+K3XYY+re=@%|Jt);YN4J|TjP#9`06<$IlgYHKexIP#SmVkf4=<3{*GYV_+LfNV-aqXM#snE>I(-5wc) zaWiKTp>hAaX!4sk2+=#|o4_ zj0Hto9{sgtM!vLG#`Q1Qe4Y*Qbj$bs z={e!~B2iQ1W?#z<`>rg&dO?31qv;&{A7P!pK1Q9y*AYm=-pB2`L67kP73!x*&4Uxi zOV__VT2bIMvEk=;>er`NPmGW3h!Bo<)I6)TpDS(anfZK`oNKf)#G-F%W)`nlv#2s^ zhnVe>aQUF;4c3$xW3I(YMCIV`886Q=%BXLoINr<>wV-m2Y+!=M`H*W96M*}3bOQV0m>aSL zQFf_>qX@-9c*mC$-GGS6Uz1OKtXILpVy!^|%1mRmnY*c790wMgyQJ+pDn+79$Fodu zX^)71*%bTN_XXAE;TR#|PwLIXI9{YDRZ5HOdGlPNfBi)&zyeox)#d4Vw;`i%Z3wWQ zSzTl!TyyW}!TNlBxiM%W;&J2T=W~`2=lag#!RCUWXcM?6ZHKhztkmYIrYM%BtGHOH zA5F-%=pFdI5ew_{|NVp#;+ezeQX|MlEU|SY%~;lKS@X3l-q_trRH)87UJso=HFINH zY`OQotf(LH3jFY`pl#i-@KRP>3f-G^LjBhT)$cBX?Vw>DyCBZV)LLYnuEv$J{-B{0 zYw00d9m)i>ur7jk;dEDZGf43A#j4zI0A+G7afQ~KUDdmTfs7uS*~qzWV?WHiKHq7w zbUUZMIOC2b=FgVKLMOSV7hnZOmtpU)6atch0Y*eFf1hjx4;XHn24<%!QD})vtkV;P zW>ZC0-9R*=Rl9uu@afL%_~ie_u*Na*Fj<(}^+t&$mdkD%;^DKEAspclvbwnZl$}1OcYS z#gbzq@T%GZi=0ioVY_n7c6~4T?c~{&bX=`P%<87i8DbRK(Ew$z`3} zUp>d!{E1h{%h2gJggRUdaa~s-I;OjHdch!eszS#T1$bDsIyG|&IJ+cR zG_Kz_(WgK94z{5j(tDXAC@mhXh+}24%ymgq^XCYOIwjio+Qg~?Suu&{*n{&mo{W9% zNyl3qzj9sd1Gf7DQtU*@>Uf207iO9QFH?_p9zOC|bnoDL75fgmLxvP*KKPST!~?P3 z?#Mlz*2#GvBPHyqoU}CmR>{4NaC^OZnkSf{$&Tm0E%f3OojqkcQp~LmKKO8USz6M^ zp3$N5J0`ILI$m5TSMR|ovc@w3ZJ_Ei?m)&43fdCA6&+Sn7D|-%NE0dc=E3`7>m zWB+8?TPk2I9WTpZ8@&ftLhoHY#yi9-YPkttV}r+C0TWQf2KSqt?N8o{nyuaum4Xh2g*?#N0-E^!EsoxOW}#GBjFk zP~vnMpsU2DE0QUWt@F&Wv$mCkze^OAnVIbUuWPz?LFaL?>gL7;4QtJ0!(Jb4_4pLB z1Duak+4$m_gRY#;Kma#b1387l^bSviwcV^T;zd9L>LbY(Nl#7v>w~FOk#xJ`>|LRRheR z6R4H_a%^5LPO~!r*b+Ie=dWT7{LHmZ&89;f3rL7%VS4cBV%xbp zj9FH7J^@f48MdqwysRU><88jbbKF)jX<}E)=E)Uvdc7FD=v<<=j(KqRWh0GIF~w|4 zL+`7QNNj{y{E3SLu>P>c_nftH-XbYFbODNgk^l>n&Tgt7ya0<60qBTB zcDk@`O=MCp8?Sl0+EY-AehBE(DM1pcRs?s@9nvo_z2gUoGU_-!puc2`Dz*^!n`R#M zvXoKr>dB0)L@-$zZVvo+C8#Oqp1+xY&Oe2EdHeV5?2WS1hqQ!C8A-rY$V1c^W-k`M_HH|!v{EYZHd@m1;KDl=49o{Jmoe&8~!)X9(E;lP4@o8>HPXBIt+7Io*E?!;6VcSsRH`} z!WOJ9Qxl41v+s4M1$W?1+%2Es`%T<8Ay|mf zZK*}$r-VGqj(SvSO)ek*{Mcd!%9E`J;{e{M4Zc*!(x8g^Gd|8m2w$-+7&cUgwhq3& zA5mOlPOReOEqEeDIS;e0t4x=v@}`aKY|l>djR03=|O=c)aCv(ILLcFguvEV?h`!UHenqN{=B_uXY+zeyV=LTBj8Y@vYCT(KBCcZ(PI27JW!LmTK3#e5 z!{Ngm>O*13M^DJ zrljcOxs?$LR39!{B617!gss?le}hz1OjXN$Cx%UM3A24+WU~V#i?@YG_HB!0v~qDf zzzYJqp0TR}{`?)J7W#MkJ-BP?R*t=`tQ%^ut2w~2r*ZQtcin=%uC@I!O8-J)UhnNbzp`XGLkKNxFhA8^B1F4cW@fH>cV!P@j04+qo!jvu- z-^`^OgU3_xF`TcL%tOl0=?+pv!&yd{|{u?UoE1-!c$jFy4{7R0~8ChaLMyTmD^a$QRA6zE)R2dVrjenlf*9b zeK(b_^(RbhRVNYgcO1ov1h}ykk$!o%QFX znECyH&8p&p*O;#Ss{+JMmf@gTTV}-^o-P=&$@Wf>m;!a+Ut& z6q?rd_T2m5BbqEv7|52658SX{uIAzL#;0fL1_}yr1sNUvDNOht|7&=-H?}7n%ngTJ zH$G-P&f@e>a=5*B)6)ZjUO=Xk9x$KEuR`}DX{kFmz}jH->=IE%G%$| zjUOqUr#9LVnKt}I<{Mg{0+fx@H8W~Q(}Nd&pe4lID|VaCD*R=|JT-0Xq$2b;LGC^w zUZIRaM1lF<>Arz01Urr6X4#1`aTMZEXlnIKc1K1aF@KbQXu6iv#4MDB#D5_MwZ(d^ zNvr2@J3MeslJrd5SbgpnVisdf{!lV%*6)1cv!-J`RjiHwu%0_)v%$u4$Aju*;=EN{>+WWlld z+pMuLwHCuiAjYu=eUIXw+KLoG9x`aAn?QzRp)k2&V6B@vFnsXVz3qZx`aVxiAm>O_ zVS!7-BBRwaw-&UDj0)=qKzNiwLS6GW65#Iob|9|Ft>v4?N~K5wgXt*i6v#OZy~g2$ zFAsjTDm;0%#se=lt7llu6It_b8^xf9vte2%iqYRXE-Xl{@b(LIqzKI+x&@7@3k@>~ zM|VDMm*_#oSh+8LjhpCn zp~tC9?DFFW@A}3cPAYnzTg4Ve|DbEF&R9@Zq_Fnr@ zd?9lvU;)M;@2~wDT?~6c*Y+~jB6x5;78FgqfB_bdx(YEL@A{6iVo^@);T)mwY8wUS ze3AP?B(#oa;Mf@rb!x&NgaMCoO48-S8QycgJ!&3(*7h;k$zCgDE5kZqIfv;i&scF+ z9ijGd>&(Y$chgj0S(hHG6o-HL6rgp0mbeL@hp!r%(>@BY3&WrMz93g#J{;Bi)Egs5 zJD})$JAH5f^x)g4buh5LnM%a!p0Gb^IXT6>3(Wq1mAijg8yjSEQgS6DX#yL3n}ha) z(vdbtnDoxs9vkNLoQ>}VM%1*?iPXoLv6d^Kx?0@yJ8o#6+{Et-1u;Csl4p<3vG5NW zNZqh;U^D$8+mPZ;niP-8ux$b9@A}}gPy{J^e1&g zIS;WB$n+jnBgX49)9>^udOQUg(&#fSyvGu9m*?|eUPfA3iLl@^tgMTtw~FFbQ4bBY zU5{&(sZ4NMtVZ3(&Fy%c>q=!(^E`Xy2LAvGoPGbLqN8+evN#?;J4wYYOS;whA?`yl z(;drg$eAm@SO!QqF*XX4hc46UHR8PZK^9TKYgw)4dh9yZ+3<1Qp*M86DxH60rXMB6 zb@S%L>OQoK-vbm4yQcdM1WyAM>>ZAnr)FS-@|Z*#YaS1jm>_H`RwWHR8m42l-mhxE zDvk&SRe-usNMjY6{3C(m&bnYTPdA>dRC7GZV!F{-ZCzI?Hi4KfmQ$c*ThIWv_o_JIBE}NCWdqR8BQyjtcKRu zKIc51Wn$@>iDAfNrQUb9?1D!>?wk-w$Iz%`_pG3q9-A|dG`XwgzTwfV`_-=>fKPQ) zeDMGSRRwlK$$l{B7EoeYMP;Rmh~Nilner{d|CAwB5-7G?w6fk}!xHH#ME3Re766pU z#j+Gxkx$QTh>W=sBV~iU?VHzEgXa^UO&d7a^7%>Kp8|d9T&!Btf<)wSr8Sc1EsuVi ze~=;o>(KkV-<#8k;tWAuM_Zy4F3jhL{n2yyZk0lQ7HcBGyLi_EUnZT-1c$$~RumKg9_KWeDqc5<_zife{( zDcN*PdU&MSFc5^+4+pu|EH*J09t)UvsY<2={xtRJHT4}p*Z<$ch>%N-b?Ds_9y5#6 zOfV0>=ZanFk{eicrW<_n%cKYcNko9Nk6DIHlL1Cqn&drr%Vu44nwykA3aNM|SZ`eK z#Y|?){NOsBB}f(n8WQ@zr8f{_>ta%z`1{JJ`mbixQr2JDyncOO?!{A9Hx%nW4VB@B z%XP}USjcFZW4V7SDb6rB&z9pNCMBbQ5q#p8=?VLn6|pIAuQH-hG&qTVxjRMS4!%vp z))PO}(Ex-ds(G$Sdy8gHJb$+(_;!I+kqxb0l?tK!EzHW(rd;)Io%vUU-(R&e9 z{*=LZ(OFm`@3KN&CWnl(m)gGQ_q-(NG4!`+8;=@Tr=7R%J@0Ay4@GyLxFW%LHf zk@X6h=w_830LZNXvA>6irqel(Fm;_HQq0Q0S^L%-;+pfyY2uirT%y|R?}eW()X43N z$r5rE8D_1Hm~+#fx5jG%hi|Q4d;?4mOlN>R2K_AM9&?@7kONe&mr$ENm*!F5Ww?p{ z9|a8aZ~`m+S3hTeD@wK(D7!l`#Gx(Ue+8~)5k-V2kKjTlMD;cQ%>p3z<3)(;tHWrC z4G+?3tkiLFY{>Id=eV_z{+Z&N_v>=3@@_MfGo5w%a_TMGe(cvU&9I}5LLP4ibv~ep z%_c%0yRGN+n`;#7 z#4}I>+;XTD$-bb}>XB}oIJ-y3JKqdqd;{a2A7P}rOD5IbiT$?c8Rb;j!gfj#;AfF_ zCA_nQG`!y^zC^<`o;TuR`5GANWd|A0gemIK(I)f=2|Oo0$J@X3`7=Udp@ehQ6ilvy z<@dgrzOwQIDjdCCEoG}@7jk#@Z5`pb2^*jRM07Egvcm-}F}5$S_&YR88O6#b5gvt# z5)u_Xj-ZW$Md52HTwI0tCdIIRnTt<#$ti5LHQN%iYBcZ4@X4JrnKq7NV=0z%56Jws zi9hwGj~XLuap3!9XZ|S6nwmrZ))t(6tPfl!FjVqW2Pk~VmOH5bvX_}Jw7K$CCwr>J zTo4PlUEwhEpqLf>-Wm`$sq*fXQky_4b$j>ln|-0$dzWMZ#WbB;=*Jyo7xOZN!NPX( zr|=f5fMw|USTd`VUD5Jaj|t&r%Rx1@I>$tAedf-9pj9g#nr&*W8w%!D&(S80GwA~S z*fX~F_?~Hm&rAoZhDo!s-M5OyzFaf#B$Z^A$H{b2#^Lb})q{KnvBsShDbj01F|b~- zW|qPV)T25mKikyiatA3fQ4F%#=y~lu=c^!?AL1kDGW@y%KwQGvI{n>46rqF-)Klk1I)}1s-Jw3~ zsHuhh`21d=zE@io$|M)Ru5y%*L8nl>?%AQskEdi6>-`| zm0+>1;}flU%|U^IihSm8H4jewjA2~PG8^)F%+FPM+zV;j=>LV4~PZi}y6alsAgTXj#Z&3@BLt?D)_QBtX(iWS5 z(y&q^$h=Tbgu;1a-Z+|a)I_Hk*sqlI>A~Cvg8|j=g`0>(KJV=L5#>g&V;H0zuhcvE z-=24nb4qyiOUoRiKyDKQWGRkY3Tydbl=0*rY`__q%m7}Mfhs49eVBy+zP9KJvwW$d zoEQ9KYI_iTlr4G&d6^lqpB8M{o$03DKnM$hHxL~dH&AtPG!BZqS6=iyM&_b7YJFTi0J0|_+|-nE@1Tu-ixWi4qUs#R^C{*VoXiC z%uT@g`DoLE?)`l5(Hk@>w_e$VS-;kfy*adUMtdIbxck>P+lNAv&66SLLk%He!*{r? z#9|ER3T<4-8v)A&hpyOXXRDjP$A>8g2!DS3o_83Zb^_14<1XAdi`dXsV!UA>x>#x( zpGc;7+f4VyHgc%;zNWD9(Ov#y0?Z^UgzVr9 zR|lN+_R}QfTvtp*Ziu5Z?EM|$I`OE?HQb&Egaz*fjR64zlxXN9d(YDk>m@FJ7E$)3 zKnJ{zgY&f9$(O-tvu3)nBP(j4*!28%r)lf)M`S~~yWWI&tT}h~id8Q{DZJ(`Oe_~{ z3B97No^7A(`P6;29j?N@MXEYVI?`4!2iwq#u56Jjo%yUk1d z?8w#-|7(l_rxr4aQ~OlV0o%J6)Xk?4bb`-Td-Qqf6Q5r#$M+5g@6QGjlaK{3yff04 zr*hWH-{HuskB^Uhy|MK9QcOV{&jVOKH~DAO=Ff1U4jta0Zt?xykl4yj6Z1QPo61hG z%Eaz~@4wOP#(@l83-|Cr)7tB7PM+x=n2KMDB%KZI0!c!i|Jf$AQ@lJoz&u}NRt=4& z?;5N!*-aqeoDW|8;l8Izzyzrgj~sg_(wcCmK?ec&6&2#WNc$9@#s+7E_B6=>$8cU{ zkG@oJ!(J&}P>cGFe*4&(70tw0QAzP#W!LePOO{%E~lP zdkqbAr;y$MHk-&5@OaD9jM=Xbq`&@5Ksr7EDjxrZu+av4t+Rn+>H6cRNP5T*Tj>gKgZajks!)=1*ghs5n}GO{65b zMSQbZh31+A^v$|X01snt#Z#ptALn)T4BZ`sx32QD6zLn_XxsQ4u5o2|xD}yaQ$;2K z-oWXzUI!TaK2v1Bq6pj0qmrrgVpmH>ldc(n3(f6K3j9j$yvGsP1@IOt=P z?U+3Xud7HrUIfVfQTmS4n)l6>K z93zewH_DZ1LtkF&<3WAo9Bm6DfCFSd9sh%(shis)^ku{0_N^*8P8c6>22T7mGc-3p z-f}c+BY`5${nB@V_t(}d-Ux~$c`)e5P>4LMN{;91i!Q^+MAVF<+2|W;xT6vf`sn(A483yfUS6X?cG0|Uxso5ngBMN&Olh|sZL;{R;r^#{M=;KJZnMnSx_Gd@ zpaEU?H-1BbnNPNaXQ}G3>zIlgJn}LA624h5=#8(QvMI`+2!LGr0Kzhmdq2$Qx~Xv} zYV-LRCRbm8zopJwkl?ayhsr~ePv(+}%UkRe2=fQ{OTa_O@&KqcOqN!Ykxz`%E!zt2 zb(ID-BAA3L{p7@|y1UI@y*ZzK|KH^;lCx}%8bS`-KQpVrg>PUl{`u}8`sMq!nHjw1 zy+WmTYSTBr@S7YIs)&_q>uG?*R@lq*Ci1DgUMzp7{9gMV)HKO$;A_|00z-&{&oGQk+43<+GC7O{Ro5@*-n%y(P&`g3hDCFxz#(FyLtUvqq{dSza*$noYQo4ad1020}{&y(w=P7&|9 z^GyFxwxNqEIR>5Y$l2xRzgY;9`NgLtfNj!T=XIr^-sdHLn*IkwP&eAN8D?>q8gj*u<)#G?E~&hUfl zz?#eLNzFdLR$XhjAq3>kr%s%W=lLzM27!XKe}VH@Ux>F7{LdQ$fg?AamM|G2?A<zce+|C&G9oQ3JezRL6l3mq2_;os?AP{QY( z+lZb+>^7PDsSVX^(tGN;+`35a62Lf1qiPSD*!eODc+>Cw2eL!uD_g)x45QY%Vy>FV z1vqxuj;kL~clJitSFK9reQve~7^fHr5{*vfGx)GSLx9!+US9rOpU0sjhH+hn*0x?N z*4#A+-A!lW&Gn7I=sX&iKFG6131v9wxjYIJhc520#x|h`Rnh$?SFVOHUAx4O?VlkJ z`{f{3uow8To5)-|L}i&i>@LwqS~L*3w_5`{I*e1a*cW#F{O0oIiR3xSUajd)o8CH2 z_(U8}A6!EWfWT2ZenFCx71W8EHBw-a>bd%ZWG=>+&Wd23aveq>@9rL6IaWG>IzC z*I6LKjq9Ybcfd`d)%W+|+d4!?D)%XMB8A8@v^TJrzQdGEwxHGPTIRgJNG7K}C8q8~ z>EzhTUkT-J2kliRm&7R_N!yugK6+hGW4!NiTIsT)yD(CEj?|>pwh(B6RY!WL2KR*e z$KpYo`Nd2`}KS&Ft+dnuSA<;cP%GvGozHA7K9Zk>#okfIk89ls=nPC!m&|h zm9aO@HCGw*H&P#tDl#fQ_6Ar17`%`2)p9&Xu$hO@k*AL=3#DJO-K_OPLnSO^`21+s zX{;%8GmKI!A#4`Gn-2$d)-r!n@WbUYE763So=Cez+Mlh;Hd7P;ynC@F3n)@Opqs zs5K>~dB6YZGVQ4v)T<1mNIyi3s!c8K^#*mQ6zcnd+{9uk@B#+X-__0v&BDI=4sTw6 zlhNDTzekIGLRNmn?!X=f06M_3;C4RdGi!4Vx0^QluK0=W^A`5{7@Me{89+Vx|K^PS z(qY*2)PqSyk3w|J!N4avWRdJE=h`P-#`|_5y|GYa!9-}Q%U|dV9@1o^cJFVnsLY0f zZ8qFNzgvMc*J&Q>{>82oI?u)GiA%pd1*z*vJO_BtTNp|YuwFtG&Ahv16CfH*-Wt;b z2iJ25`j?@<)`dG=2ijaEyBChGJiLEO>+%O>G6l^xtx}%T)1lL)MH{$ zL@1m+=<~y&Y2;YNeekj`DSbPd1P02;9^MwQylt0GzF5r_v^QN%}%yW-!-h;xeK_0S5!wDRGOq>_`lytl9@WA#;K z!ai^;K~Wh0k$s_2J0dR`O!Te)oo@n12cHB!N5pI~HYWhrx(kKEZ{88e5z?RE#nzl=5XE`SC(0@+|qW=%NoTA*5cW^|Z z#r`1k*xjZEnSI(K>j=>c(?i|)-(JWQhJkI@9c&|um*005dN?hJ+hxo?%Wc2dDAK58 zGF;7Lq$Z3}uinnNjUkLjG%|hJ+sGes22XK;-Dd{{WV}odv`7I?@nha^w?7^;5gl(s>|GD!Io=HI_>Do8vTt zg_+2kVOy~nA)2SAzRFj9ty|V&v^+2Jx9L~#)R&!YM<)r-;a&69wT51|K^xW3Ao=_u z&>My96o3|fEv<(IPya)Gn+WGyF~=F2zN`_4*?Tw&wG@=(KBLiy+Ie-S^v*Bg2Ci~i z)#^G}gDtXwU4b0PBamfy3aD?5IayXjjM{`yJ>|*`0=@$yt)3Q;w z@(9{ycm&jSJf;Gs|3z9XNHba7?mbb3_np>MD=K}0?v)_*WBo|XK zd-G%n2{``V!*I6D7B@---u#)>#wTrj=Oesch0fe%6|-i~Up>se!v#6@l@-;cXIohE z>Y|yJi1P$)G}) z`y{s}k3o@vM^Mx0(+~pmQx6Z5_>YvN2(r zMmheZJVGCge1tr;wVxUw=K>(FPqkj*5EJ?{Jz#i`z8h^u8l~3~jzGR-l~=mR4dj6? zPq5J11ui-;fn616z!A^$tyfUJXV;U-ygM+t(*tgvG}u^%i2kJW$^a4N3V=Z7+Jl`l z{|_MWpFUD3`9GfTZ`|Wt0jdz`M(GX|eCPAcDmw`8t9&QjZmYe$EA`_xYhZq^So72s z&Gs$ApzDk$rw0H-vdq*DObx^I*BGrx%kpo;T9gQ2B3m$g?Nw`mw}+OIG3b4xRX*ba zZy=Z2UF4c6t=+3_p7jt|2qGkoWg#!&PL6kNm^xzC1sPK(hBr~Tp!H997Wq-kZas5d z{Y;KBc3Xe?v+A=%KmNmjkBilw3Href8xi&#>VDl#RA{X;oVT!U z%FWVY+1eDA-p%wa6nqxJbz55v(1f%pb7b9N>wSH#OF-xqLw%Q%qjPcpS@C9*n`18_ z3=@H-K%p};Ppy7Bu#sQns6!2hfwv|Fc;OT40KEpWjgx6gTeauzOSQvjb^nY4#clxt z3K}2UeL|CcKEI+=Zqqe8)$#wlIu{i-w#Z_$((TfN25w!f=}Sj&6XzNY^YgLe={ZX6 zeLb3B;F5Suqq`WAGoElde|+u&)(!JaE7^P26Nv;QD*b~I&q*rpBecwt6Jlbg{eMv~ zvwtvnCSv^w?2Ifn5_!0J6(&x%{abB>%9{*OMv4L1aSVTHEKf-d%>)N=ErlnQp^{4<5lB+^Ae9mE1yN;t9Ps}17 zbC=A%Hm-B+&?TdjJInh2Tt1k1Y;ad};;vJ*8A@KvPw`z`vq?JFptl%S;B9LY!Cu7? zo5ByM3Z+nJU+O&(o*Qkes>`8LMc_5zaRyKKHl2+!wm-FBc$LO+{nL zVCgPYp-=OZ;rS9YTqjsh6{NyD!xWmD1%tXEtw$2N>|d@rUbM?%_$($I?Cw$SJ)7Z4 zSx@IYTL#6d!2?in*Z-v%P9c*frcm)&WR{Uhkf+`Au!oC;g4^+b^sxzQt8u_>_QMJE z_=jfun%}K=@1~%8`As z0}<5aLa1G8^7op0gWSAfxc_<|f{3rOtL>`<;#H65c3TGM|3;5(@kmoXM($M79D{ZS zc&j5HZo5;*SETYv?|uh_^e}s<-dE6q`S0}&5NbR!no3uy4nl+PoFub)SxPSy|d=l*YwNExHi~t0TfGz)v<83~?uN+0! zHz7%7l8LDbInkaVdy2uffy_*&BMN@0SX>BZV<#x8$7T6`k%BjnOVk!Ti;AR#$?21O zWK|z5v|sECRbO&@vE{M1cAAdH&ZAehf6tL5Fw%VLm!>?SQ@g9xiI8fO_L; zTtbscZ@uvcuQeF_zc`6VWqObE@4D+#DKR*hz^F&jZ32%sSzGmew=pyZ))>s4Y-%+&bArGnrC-L9< zC@s{N$ra6VqrlZlyQ0CJgf;b|nnE13}&&l`xU9w~^+3SmRyoS#`G&-Dtab?S# zi^2ZmQ#Pdli{?_-g{qbRZK+?W^ytdmLQO(#_Nuu~$o?hNDU0??0fOA>0-J+3pws#E zBmR~MPMfJU2Gf_`y+hoLM)#U@bcl>_KpJW9|0>d$@GV)gFP0Tzh|ckM16Q0dQwg=* zaz5lwu|*S;CIV_tl#SK9H~maGwaTi*oUoNDKnaWZ#N*G%pyq$?pYG4c0eC@?W$i-E zAI}uzz9_#>=Z#*9m;QbCL-?%XDb~nOYUE zw=U2MmSdtGHAassI@XzFau_{~<$|M)N8I=MqW^XMqS-lErD_DGi!_2l1WHbA?@8UQ zsmDVdEnG_#TbqAQ>LDdPkQNo;T+G^111r-6xcaT$DL0lUnKUftJn>91F{^VE(Dx;V zO>slD!5ea7{ho@|+HSqd>t5~^XHmIf3`h0H3H$jnZLWJ-gOp@AY%$e6IDB8tpqj7oJfWR|g%l4OiT zQW7#IN;3S`y*uame1HEu*K=Lx9DDEg`@Ubpdaboyw{jL{4kP+=NxGc@-c;hgLFxu& znmc{YHxfb9=GrR$50chH{Z}SeNlTV9oEZj0z4F|kWz22f=L?dk%#o+e^%efF5h(3` zSn8!}j*l)^kM8+~)B>A2h4*3GM*5DWsf_S%Wt%ARZ&hyzSq>=P;qgS{G`(hY`4n=zvcU~tbHp`Z_PPViy{8#9TsA$dXRP<>PoP~q>k6+d60F4c$#;zp4Ij`6= zM^)GxYwoFWF0Y$oWEC5}pz=9j^_qODiSZ!Z_ z8Z2xX><(K4A!$FeukcN4DX-SZFW&0Bb?0#0%j9h}nG;whcBYhHx}9~liCrPmzg(#u z2xDAN|HahSYKOxnTR;EDAmgQYe{OY4v-m;8&=|Z`U-(_S&bK}E%6QJaT%yy=z}jV* z2DRo5pY;e3`aM5|nx$RQ+0vBk?d3|YbFw6-M}%4ADG*r3T#p=Z4O6dIjoO9VW=`bl z$0Iu(S`>JVq`$_2`ib`?^!1+{D{;V>hpddR{oW_^UrJMmtb{DxA)rx40HyBM$qiGZ zl-BVDfJ85k&9nJ{4=TD?Qhf(bC7Np4a_&_Z-`Xjfeb8@>?ET(?5xBJXb@BKw*Uvvl z5D$AOohEd~kjbI!gu+Tano$@~PUWv5(3%!LcX@IWh~RCYCR^*{|7I0Z-E3neON5io zjVQJkyQXgocPhf#s7l48$_h5X#Cl}I4rUscs|C&sz1zAokNIbSX0srxcI6I&2Q}R_ zSW6~C7Jjpn%T-f(Qf-es6xluLF@$s;VimMIC6YYsC^GK%EejZM!QrMP!~RR9%KOI@Ouq7oM)y_G0$i=u|SiT>@-V<`akQm;*4O z+g2Yp`hNUmG~wq&Nz9iIKZxCJja*=5@I--w9ZUeqy`{m3UK*mm7KICz)Q|+5XV9X0fZ)_E#5vcfRCBA1{z1_pIyuFU#L!RV(g*=6EY~o9(MF z3BM6I6OIXS6uI>7-sk92x5N53v{(QB)*b8(wJ$Q(=74WcecI%6qX(6PBpG+Ss+$(M z)SSY^*5o~sc{+8`Df`=l;#;r=?ZwOaO9*=3LjCe)b^*IFIQD#B_T<<>m5jvvcP#L2 zmrq-aY*s{riP6rrPeDXb{6`bSFk*`_KRt>~ZSJjqu*0mE%!^T}EB^W}GlmjDW?6d0 zDJ!UR|8IH@63D8Cx-`73*NPY6r)E_S%0grP1=*5HG9N1jWZVlSuYyA#V8kn}-wh^y z;_Kg_0@k*>BKGbhh4e~fr)Sjd7#>Beqnv-MrMe(eF>iE9iShdHV3P*5+98K)iW{irj%)>To6cZMXp6xqxa=`AIU? zn7q2Wo3Gf$4bgO=6IZSLD4@RxJbuNj5J)db=>{}^1;u;n2-5W3{S(V$;Zy;l%ZxQ0 zJwYwn;x8!FW07&^LwX;*;EM$4@Wggi1KYy2n2`->6(zHrP3vqryK+iD{)^xGo%82z z8^wgJQ`|dIkNjb1G>cj^Xs)anFx*m@!Jis3F+TZDqghO z(V`Tb{>@A*`fh8<@og#+B7UmPIRQ30uCOW47&{@dD4?WWcG_Lr%}+&%rNksQM;mb)0Uc{^u2IKacx!Or5jDn^T` zA}1{4V@hEzd>=G#R)PfvsCmYa!f$3D&05(PAcVmdv`%u#G`kQikU}5NC*8fj`wHK&vTGEsM(&WO|(aGZR$3{0~p07&Y zKL*C@_5IX&2*EMt49B7@P_6VdLZB9T<24I*WhD54N-tK%d6F03we7@nJSPD<%)Yfn zI&dH2zc!v8PDWu*dWv0{H25glASRqx{}m;3ER;yX9=y|xCP+Bj-*@09W+Nx)hzokC z+!K2um$dlxp<<5Oznbw4sI1s_D)UJMFi^zUq6MB}D4RWELdg{0Q!4xL6H5JynLdAS zmjN*0x7F>#&_oye011dGtV8@e!qKaWDQdzLrWeJtZZ*f>oB&gi&AkWDhC;Nz8$74_ zB{wmyT)F)Y6JQsi@CFK6C=ZP+Yo3p(T5O7=Dh2W@=N3N^THqU@LPjbGbq9!ROy(^j znN~d0?eFTNKt~5D=g};~BMak$l->gt(p`!m{`L2s{FjdPlnb1H<=2)NH)*v4m0a`3 z>=mVdfjO@~88(upw2=o#X!`5xAix_^1ik-AW#A`Z1>YDaq-BOq5(WWyWw4Qne|+d; z_?BgcjN?k{WP^0pdZWohJF7mpi@bT4j!wztgIn8;7is@7Av6yH59#;bwicf!fL=Ko zjvaqAj-qJ0s{+c71V%il%U09BEFs*0yBn~b^REDtR8W3P0$m^Ypq)N(v8C>>9}2$r z)FD7Qe(L-=1bDSU3&COB5RwW&<^(T*$M+FfS3(X#Pyr1ndzz}q4@Opgc7p~{S8vQA zr>z;^uuCBNGN1R$|05iag(2?Ed2!niR6W7?FvvC>*S$b-GwAt>VvS4{2(6-#+YXn! zzkcK25QDEx>TY1)hk3suM@ExI`2YE%m&qUXTy6u07*TeL&Ird@RD}kk7H^i$AOhcjMvF_Rl<0c7y$IkpzHmuaQ%qtBdm|c z4QBXZN-JC9#My}0;Zv~&iH_HZIR$9brTX+of0O0d#Yz%KNpmLcvVnCTaL;aVIk$@Z z1gAT`wqw>UfN__E{yIb{Q4pNml3t=#5~p*JWAgc#J~_%Whf?q!MY*#EqZrbrdBEwI z6$o|TS+LgB>ZA-eo*yUimH_)@2g=w@v6k*PU|3?9CpC$ev_k_39H@6&LrQ@Md{s+V z>W}yPdtT>R_h*>DmAJ1bRji3Jw{MkRcm>7`zuJBXtVNn&CqZKu!a?Fe_?bEDzLB=@ zOT6bdKPTfi=hIh=`p+S=et<|ogg2z5e*@?2(n=D|?vv&2ckpY7{33zx7gFFYnYExG z#-M2OR^q?Fawr9a5<<~!7R0tfNg~$0Uw^+F@lE2P?^z9!41yk3!kDw)Jc zPZ9Pvn+8z{kDv6e-m!qLu&N<~8*bGe<~Nd7qnY}caFliTQu>+YD||p!2&PwUB+f=$pA2a$vQI7)a?Yg9b5T~+Ac zrN>b8>mhv_u)VE4GH=fIvE|0!c|<*nmEWwJ(Tf&&^L=-A_YTA@-ymCY1y^C9$p zK>}yY1rql4BD5)=Q0?#=zq{)?#hF>3l2!KeCd$YT5@9~;c3a3*%@@BxiL?jln#ZX2 zge2b*1cVKuJ4wX*L_IswshHq&SIzu{8^-)^K)LT1{QfywwvUxn6WgE6YYE-rco-O= zyO40}7-o3EB4?g+9gO3Cn6=*nW;ZRDyQjDtq+KSr(C>h{x@BT_xx+D{c`VvTK^q+l z)SnVSd%A>ZnDTzYmlx5=a2=k*%09E)y=d6)S3rz zJJLb$3X~H`ucr{q)gox?P!@A zyD6kFfj{)Dz?TzgvR3%*e|Z&Bs}MuVq0y;{S}R-c(L74T5qZvKH*HJY%~pK}J%N|9shjZ4k5GsnmWeY}=L zj&BxGS4w(&Mcp}oC=MEgy&lg|j-VU>LAF=dfMz>d>JNURt8r4U z$!F%12Aw#M=gPamqzTUN%IwqKl7i`(tKr_*=o7EwjIpG9B*aIT0%@6rYn|Mqgrwpg zO;iaAUg4V;lDhjJhDF5dxUHhAJ_siR;vpC2*>0M2d?JwBRU7wPq zPlAZIMC0V~?c}e1rV!ZyY!2VahFNJJfLQlGker>G5O?DTC&Ydmi6I`sQ?O7< z`gtJ!!@mQF;HCbBO=b6zemZwqkg3si2i5UiGp|_=UVcqbxzi+(R$n(JwSYC1=xH8W ze+J2NBxFXH!l)+bc1!Y3zI`Q2#^cE6V%G@x9%*q37))g729g6|eZfz~YbQjXW6bq= zI4Cq>sKo}{F&EPX*-!BL@?+EcX@mE_^&Ufrk07QiWK|s0nG^N}48IOd!%FjX9}Kxj zx2PqxL3{Z_f8UtL;EcSvp)N8)`Qxd(j((5Tu0~}0tE!&zv8QZ_qtYlbO+6zUHjUtf zhW*ibO+kn^W`=r2x;4@X8S6U8-C|3OpJutD#B)K>#u#9{?+1a{NK=nh1&yu?x|Azj zt3F~KLZeb?=}6+3y3_)JaCVCKhU~f76ZYmpahED@Fe`bC_!!241e*L^LtB{hM~-k=c)V{tt0uxNzZVnZC|@(TVK^b;?k*sg!P z;KMq8EL(`eQnfsLtH)>U*C?H}+rp#YN^V&iZEt-1{JP_SJg*6@jIpD5-|quV>-olZ zpRY)x??zK;%62+7wr+9WU*>aAtyIR8AZiH?HOpg9G;IPdNyFIEQmFM+sv+?u_0UxoM00r-M0 zkfk%DCgk~fa+0e7US5X8?V@p+KP*2*haUB#E1^yqr$8HYH;?q8a2yIjEC}APr_|HA z7{wFku}2RJDK0g8tXg#iHz34@{JFWu!P3zEPj|xv}L)fL(hHW@EKNVJe){ zA3TsYWR5-y(iA>Yw+QcXnQ5S4w;*Gk+w_G@vZ^BU*0T))(Ckegln6x}Ron0IzKPGX zn~9KWL0o0xX?WaI;|MSHW#?Zq_y>Y}X8#OA{18R_T8eiz^W$j--KP;0@5gB@Zz9$v zxXLSaXtkf~7mcQA)6(f4O|HTn=F0CUCzZ4s zlCu!>E~%XDnL&j^D2@t$`ePL{)sx>`;OsxgG^^MS-pjLb7Y}0>zN{1 z$@RB+!n7X@%o|D-e~H+&tiA|E^iDb^J6Fmx9qoZPa;U!5+1Z^;!E3tPiI!(RK+*Yg z;Lyj4kE}z;2C>960J(+mXHz^DU&RtNwdsDL#tP-LX@wZn63>F*UcJJp$Rk=*O)@Zl z*)HQkVe8+t2NR34oZ?tgMMg#vF)34AFJUq|)LGnYPbV=I7hMlPTQA5#T&i!6+oL$< z`OtD(B*wor4KlQASv=?Aiu0S)kuIeO9tPH16qikhr4VX7cx~sW=jDm#5ct21ZHgGD zu=^2vGA;*JZb`kj7(h-{9oDMhZaB9C((?D)uBFYl4peyco%C)@aEf4{JzNUDGevf0 z+`vgRO~^N&mhlc)(qCN|BYMUjxR&^Hzt9+sH2`RSXaFV_CT#}LGsRzxl^EtcJr{cB zKQ6%c=b^freFF|YG~EDB(67mWnF3>9(ap1|yQK{`qSfzLADq8CG3ty@Vz#D@9xDR( zU!RSzHVx)|;}?l3j1ix-OwZX>9mYIzanA0yYud*0VE=IZzv-#MXt zr#h{VBSjGJY3%E_=buem+mF0vfBUYj15F+Q3- zD6#TJ*5_-MYOe{kMEIfqq~J3`oBPdKkIfk~8Lci)1#`z5>P9iHiADLk3;c!iBB}8o zrQ&ON)8$@WXgK~lZUgZ`MsXM1W$g^_M}gZ0*YKxk=t_x7S1!YP-otQbw?4(2BYn~; z4xRP|GD0D*Xe%dPk@gr-HsLFF!wD=Bx{xMj*Mj#k^XtAKPs&}@JLAI!8IiVj=;{qv z)@pq3O_|dYNNWs$(SDATwy0$laWn5UcM<;@GCj|_PR++O>HJSKcp+7gZhQRzRTrsBNGVx|09j4S&3vc z*zuUF$e>GQLR;V#Gn7h#WV&dT+nNu~#1+y%OQuX-nf#oN@;K4;miK#;TH}O){a(m4 zy~q<`B{E#!XTMy?SIW3Q3pP>Ux0@wa8`lPMNm-Y7%G+;uXw|p!{KWF{%T7V6@Xbn0 zN4}tX@vw3yX6cbeYR=chwmf3SUhPe-b>o;8=%Ha=$#&)K--#ppPEw`@)wg_?B#Z6< zv&1$F=Bo{ZxB_e6E2rr=9fbd)5u$=EYPd2}p=@dgs(zoEyot%6I3TLv)0<5$)0ml| zkrgJ=I3JYz={g^Y)*{(dw#@Fps1!_QXs4LbwQVBiPf0#FEPDy-%#(s4v0oAyto3OP z9$H4MY#1sWM#PB#rtCF7(|@(njG-;Pp?a<$au34Y7bGtCDLH0_4DF^9m|ty^Oh;c!B-Fg3phS*$y;k|@cDt32c&mlnN^>^t#Cu(rf(UU~0T zUO5|(c&%2{dg5OcXsGb>YmPktjxgu!QvOa4!nW)f@7KpidutjA-fiMO!2m+LH(g0`At>GcVmyu z?xgM@w*Jw@5FMRO5`*q02Z(zOY5-UM|4zMt3SMlLYZ?zlcL@Qj-|%!jGENdcVX^oj zBnyZdi>?ch=gbm@uL~eb&5n%~B@j zH99v8_}JwTKO{#+HenL3ivO z?NwRyNqQhO?fXB=uHBlCu^UjuUl4!*^nAS8Xp1S}xsP3;`Xfcxhy1`w37Pv~hH1|( z!>V^wDn!TZ0_+>`V1$_0id7{kI1BE-siU4EoSyI4VS=5tmrJ+MHf!#o83j0; zca^+)c4q2(_KBw2bA+`AaAn!KrAmkamWKfs5gxef3sfe0fIP4$0a6ycwn*jC(&oGd zj5zf0Xc3<9WS^~yL(rQ)HmcU`@!;c&FzcV<@u7Qd!op9CuuEz+pJn!$F#@S zy4dEAn{Rp82Rf)sw(M(d0uf0FsD31Hhl_(IrXE2{!o^oKF0RUXYVEJ-HKo4OZ% z@DvsayS2wk-=_>z&4ww_h8-^y+kjf(x@t-y6q?YGVL^T6*U=kgh8~Fwf2WFd}SMyY}o|tYtjH ze>XFjb$C3}y0YxgSy5VQ=Z&>x>_L0+jiNL;+PkrPqIHF|_4&!@p4q{|K1uVuaP)@? zd>5`UFcdP=E>Fls!<8y_5se8)r@nYh8C@%U3eH#s~A+2M8luy;n6+N*V=;=LMqnK~%K zuN?Pj?XUV51z9(0rk7z?cMKT7Q|oRx4>aB1C#ECD9KyBf_=nT9_B&(q>#{G5aWqI5 zgD~cF?uqx!1h&FW$DkhTLI+*~jsJ zRIehwxG+yFk4H?@esjsP>ptJEwU8^RTbCU}+fsLpGo(Lm)iQP@siu`Vm za`BVc7Q2R&u|sF6nWm%M$Z!X#nV}*q^0x$Zw0nW|@x5Vw zh{^6!W>UL?t-rPZ`{`y8wQa`m6=td6E!Ui+;O{}`4(|iT>){(pdl*zRy zSDacdSL3}HkfCizjICfNNB7nB9_y;SR&n-znwc*+vil|LPx> z)C=WD)!?lOi=GgRqBA=tYW-34>_+NXxK-7)Q#eJ$bV5U|#27PYv!XM%K77qXst#Ln z3VQPKuXTfV9@s=24mdbCQhIQmf!(_SY;Xx*p*BS${@_(fZ{q*yg;zt5&jRmDWebAG zv;6{33G8Rt6?OSro`m>tg9R_)~cSpZcW*=gv zvvU&fk6zR&+szwcTPlRUZR@w5(Q0T6vQNIxJXAPxZ6Bhv9*P+4L^nV6p!jNjT)h4d z+eNoxR!3-yx;*cZXP%3Bk&HaK zg7sUBvf%BN6b~-k6#An#C`0Z5xv8F=Y1~n(tcs+PI@vrLBi!bdU+}mqT7%h;wn9&3 zx<5^O96IEWcuki+ZR+k6FO(X z*?WCBmNxG5y!!~mI5ZPi?B(ApuOfRvzsIg5 zCIAOSFEcuRV*#9-)QS;q@6k);JG5>**fn!jp<|mEcsPxSUF_M+d3kiiWmU=D5eX|W z%I(xU4E{Hn(*oaAb+%en~rV`9s79-xWfa0m z9`1hP$IRV)aQrm?K0Out3>#!SAD)KynW(R2_;!tMYm~2<;lg+}zuX_wopQT@Rn|hF zwUsa5;kT1%Hut?OuDs3ri$8G^IOt^XY$JARA-w^qfsePIegGe_dP)DL7KG?Cc`gRt zipA`&8*Rw!*@&u8)`^1T*SEK|E4fcm=zG}DO#hl9UijdDxVfIzVPSKq==v>({f*D@3iD>aMi4`)XgQgn8#D|!uPPA_HdrwqNmG+W#W=?E4jlVN z(Ihx%ru~%F7T`n>()qmaL_MBPE*aPNLLi~tqZ!8cL8+H)SxEE6f5*g}$yN$u7qw&k zAE=Bwv*lRzQnLT*`(Z@vNaCDZeQmu%=P&Qe#OxMI9<9->XOxLoE%=CM&Y4!{O6)=D zfEpsKu2m2d3nHNCa5CBnEK7yHnBS+txF-8$WT<*I z3FJB=b;P=%XZNxat8uxowCU?hAks(E6&+`UQ^ckXjsi=6ddFAvXr7k93=2^Kbj0t9 z5Dp$Lkku53AWvw#`NSxm&@M*$shFSZxHpPGaU1Da`@9{&t~zzQ2@7TQ)}vE_+mJ|z zIi}IEhBcsvaot17iZ!IYuA7F-;a>r zUN40*r=N23HBlVnHNHsC2Zeo~_O*2b&5TaWk3pCtIc$F=jHeYoOU(yM2Qi6?4<%&>a@H_Q_mbRhCw_kLjNefa!!4Gd9!u>e&%UM6BE z|M^AO=|YKN_8>bZ&R&k$sdg0#HEzi|g)=lT^S7lrnb#+fz|#p1=uhmSltyDSZvPHA z)Kp9I8>+xq77gIPEo%ML-s=g^P!aq{cDlK$tyW`_O)_K0aF{ru+s7V<+5OKKW1$0S z{`RRB>6oDBdJ&MwP}*i_UzK zoQ1ulAlXs@Y+s3Y)G86l-uA-{8JG%eDo%!hVO!r}ng=uSwgKoY$2)|F6$6h6-pN~A z5!J0z&}(#k6js)(WWo265|F_y(!AzBu&EwK&)HJb3@p}<3O^kSOs~Yq+9<>e#pxf! zqaE6NTX1l%sK>mFqiRO^TUGXdS(V)?(59!9e@gq((lx}PHZyri>$8$Mvs-@0F_t~F z>t^5!o#vW8drTaUdcT)dku?>eP!%{}=8;Bb`nm_FVOdMDIW0JHJ@*Ls0$SZOzkg_d znB*@ZxwqShNwOkUE_CwC{bs#H0x)^&ilgBvXR|XCZr)d^^p9wboSE_xE)f?d@35#6z* z2+vrJa1WBTPr!ML)?GpI);J4g)HOToz}kqt^Mz^Uh{p6oEM%0C7TF8b@ND89zvU7b zR!oe1IE0p2$eUveM-l~(YNceME$8&}kgklK1mV3dRmpSt`W)+)j*36(*KgDrIgk*N zOF{17oYmP#IymONDG=$xH<7g`usFuI!NnNG6!nV>s`z>;Q3XLG>)XSY4%)S;?fxO* zO-O!&4=z|W-De%gOsz;s>H>=q%`X$HARjeSCZ2vp>RZit>jK>(baWK20FXJJu8A%2 zJ7uI%70!*k(-e03IfENVzT;PlcW`!OWG}I%I577Kx`}PcQG8U*Api2GW$@K62DW{5 zeHN_mrz<{4$g=uovAOm<4-rFg?5<^GbhQFCd<(ya*0#Jw5|H6VVX=09KpU=U!{egK z`{_wbGU~FS>7ohHD0g{U2a|7T+OUO-hYj`BkIag1as8^Ve(g;Iqb#9zTGEUjovix~ zD)fsu-NDt7;_}DSb~01@d353X84J?Nn&ItOtz;sIom{>so!6ev@4O<~_+eKB)a=y{ zj-gg@Ov`UnC}k%5+oHtVK44tBd!p&L)un)s#Ns}# zolM(Gv07Q=yL$%I1DE0Va!^Z$_DG)GOB@Ibr^#-6vVG`+fsl-Y%ca zp^Yyo<{>UfmjwrJ@kkPQ0w<{mlN5b9GqsZ~;%T>H;Z%c@vNFTN5bA4+`7s}m(o%!9 zfOWvsqv8~(f3vox}PL4)bdz;$eXB8B>&elnHtheje#S=;Ld3b8$I|^*MgNH@Ho-xF0 zwJA|u_7^P2w>h}012iFv09#a3CK|TrP(+&>x#{8DreJAD+qCTKLH)Q=@*n6*E`?kD zs0(SfIRQiUhs{qkzbvtnhz2GZWVP9W69y52#!eLNL(CfcEAt*&!6IwtS+P0GR&dGf zWOrbFPU!pQckw`4%6?n4A7E!e2O|zvKtWjmuIf7{J(L?H5G7FUSh+K<&tSr{#;t99 zrFomlpOw@=3^hcFA>pZy7X<7JALV5yry=4WsaU+dy9B}R2b)JbPIdt6l1z$R0Fy zoXw9jp5!&?DNIh7L;rIJ7*+%V;$f-F#Z`M~4sYW{U*#JrBAkBO6fSDO8_cyShM3+K zY&X88!E$KW4qeMUs@}dxa!(kSMKg#7gTp%z77&mmZM}oR7c3=TO9H188!Cs&=^?na zO~x=k0fzb^>DB2T5u>Ee-Pasgxg_s!LMm5L_q?);hfJ)=(vWd5tJ=Fxq40Dpyt4tQ z#9bP6n6{V~8cP`(=?<>Fh^+RGN5I(Wobo>CV8{%+I$qYp|?HMAh$uC&D#0)oP!yh|S!bxR3j_z_Az9(sp zp?u>{dvc;a$so0JS7@~%bEZ|q{g!(Mok_w{t_C&GkfyKxhI#hJ(rrxPX3!OB_r6h`gChbc#XK_A9vMRx`==6=Ctx zAh(%3Ce=tuvfiTq`fo5X2M9Hws&0H%s7rAHV_pUrs24ik3%Pd3CvR7xOvtGJjCtkh z;ZgiAPjkqt0M1Q&DfBt5*{ZgRGd1j{EsH5|E&nGsmIF%DISfA3^;|P| zSU?>S8-sRcHDct2n+?LoeC}aN!aSvjsDsNc!Y}yJmuV;K{h*=|SEj<5Hdj5~M=%NwGiN0dF;^0LUJIEmIaT|oHDVoTp?b~7XJ z*uC9tS?H9QpnA^a8lUP$GDPa;?mjGP5a8goWVSRNONpLP&+ZV&rcD2N?PQl)iVzzC z-=-2vJAirAz^j#LrF!wEm&8xzG&FDo@hRyY5y6@9|HV8(Q6Zo&Rf|d0*w@w4R@tG{ z=XVZ)yI=`I8XG|i+Thhek!-?(K(ACoxQVH;jC`T-%BWbDoaz{OV<|3g2m4 zkhXFG+u8nx9a0uWhsy#^%gfe4CP5?YnGaHVAv8ML9do(xtTw(CS|kjcTF(G#uQE@X z6EchZd{bNumT`1I+PsqI5KzaVZ~j{}3v_GIAyV^lJ~UKAM+{wTcNVnemt6PI>|lwY z2C4ujwj-z|3f?zj!2vrphV1k_oH(KCH&;3=g76fmS?rkc^5nhrmtwH%B#OWqVK0Qe zhY@yYC1Nlg&}pqYQkcX;mc2Fd!K}lktoJ*nevGODUDr1Xp}-D{eB=2#?aD;=Cn3z0 z+}YJj>{MSs*4F;*j@+w!li~qZ{zDg<$+tMOLjWmv9ErRJ1&L$WPmEh@(Z3!^tQ2A2 zLr1(Lruh$ZHloC#u89Ip_)bYK^K_uS*%cDZ6BL{^j-SV77kQS6t5gGThw%=l{?S96 zz3%zsRD2R7o(Ua#Z`GAirP?9KmPumj(NQ1!0e~l!ikS@kawR9q=p+gni!HmO0RQPP zTEVv&^#*PGQ$KeuyFt5O97X#!%eKj(n@g5-_h?LpG-*OVD#l}+} zUex8Fa?uAX_=ht4FdA`>Z$cnoAG_^8P%)|+TzS`ts`-Lk09vJf3L@^)!Z*i{=iwkW z@OWYyYAE+_aKeIG<;4Vzx?z4XK9pO(8xvAWl5kt(XM%(go(7?FLd;1lcn?+ViqPpg zaee*#L0ZKY43*;@dC&iFMrTpb(M*2&*iAB>FD(%H*>eF$k$AYtYJ(Qv3>bfd(tH!_yZE?uwGa&N`eDj3H#7uq*#6JrBRi+}#nCcL&r8BTZ` z=lP;>(F4=+$oU;E#e%i1^X!dX|%6Ds)p559sm3)wzG z)!Z-!M4SNC&O|MtVMT<&)eaV*m^kw*1tt?c8R10}UzRm*XJy+5ki(>jOSK#8Pn-l> z0M91tXV7b!RqVymIhl2i96_dZfE&ESmeLa~(y0V=0ZfE!!5!RjyxxOD!1#?0VGExT zIBfpn6b6~8V&576QWT4RZC1Y(sxZ!1hmJa`VQ`ay&bsg5ImZPF3^{d;c+! z9L)2CLgwM)Uw8fLr)`fcy+Vsy0x+e?6NtRD{K(FKT|`RBM9;w}Y8I)Q#tbg1IC`G# zH#f0o-CrviM4;D7M_ig!;RxDmif4ef5{n8TUCYlCH!KUn4uE!w2+>|*ZfIed7juRv z!riVMP~S=P9*AN^N{cUSV82iC-jO>HYoP2t6trLJ=U`uIGn22|Wozx|?TjIa1k(0D*Kr(dy31c|K&>(jDw{OLf~J zk^Z@0OV5U}tjb#Hic^}-n1}ouX>pE2NYOgAza3~Rvt|l9tq29@qaCgT!7i#>bx56X zYjaAvw@&ElBYDqY$+e4=YZ#PHFsn^@U65Gp|Ij>M&k{a6g)tZFwd`(`(1#sS)+7Z@s?US zt3?_DyXm;jn(be|F2dv4e$ohklhoh;zZc6gk?$PY5Yhk!@TsPY1$e8pJIUoNX*OZ6)m!)hd{z<#mFe)u!Y$EZ6|rm z@FpN$6Cmxsw0n<^mN!4$8FdM^r6DndqgHyXW}mEf{tNJiNW_d{we1Tr7jk84R~HN%wSgP(*w8|N`#Ez%Al1>x2A%?$eTrAn;05<%K4NQe455*hZX z`#Ii4@bLBOtBo~+KjiTHK;U_L8Q>A?ZB@EXvB} z16VfXij&Qa<8U)9mXW%E*W$;#@nGgqGJOra9?1y|Bfayxg|^B_@+ypC#z+mJqeqS5 zZB3CJ15tL3fg(U|Gn8<~{JbU7XMio(cNRS&t{~A*Catc4Q}bctht$cz$=eJd>2EO) z-y~)nfnMF56?k5c{Ue)|P|tHo87!GjJH~{MyGTQmay25AN7^b1UQ#{0nsmagMXq_C zQG39Zm}6IHqvg7{lkZvItCqjVnEdUzl|cg)x7GksW)Iqox~~f>sn&pG(OBIE1dMFr z``?TAmTCV|kG(bpHQ=z{L3E^BoqSd=50jArcsp)d4xL*RgucA7ADwMo7GXOdc_{Sk zRSJ-|!ufGOEc{QjwPl^oCR@`I8`N{*GICJaK;O>>i9o6POIre@ql{H1`4PGF#zI{!7d%< zviO|CR5HeSyc(eNb?s({r&`hr>!Ku@C1=Tmqp{6}bsHTRg&AuWX26|~(vlQhdA)a> zfP6a9JslXhC(U2Dxa{&2ajQZ-FeG=E3-QK+W^@IU_uomUy&*$O7WS%4*6%mkm8vq4 z%&`d{x@8qK|E0#MK44;z;!+m{-RhAT5v+f>O(mP_-A6hlrhAig$?SZB=hHR+)+=q1 zd%+F=UGHz`K*vH&3qm%2Ls`S}_pag^<@mSqhj9p9!f5`=j`XV z!g(Vqy$ki9{e^#UsXvLPq`#a@b(yuM(9f~%@xEAp`6El7R8-f^a*Qn6A+WS~e70uP zb^2TYl9zeLPN5B%`6>!sD)-+5eJAY6vp80sYygCvX-P#U?b3Y$~3)u9t zAVOGI&sHsWuOI#l&B{zLaqM_Ndg`%KfF{r_5r`}~hLCzp4=Lsj7 zQ4)flY4f&Gb&9v{x5+P4gzezHbVreNidb*wk}|GCwL|Cy zG_maOuM5=*LKHq2TC`i;d(Jl! zb{ySCrqhnWp|I6kHHdA%xlS^`-RN+P)BAxBMmJ5+dPO=huxN@@1N>1qdzlW-dpt#y zGS?kP?Y3I1hTpq?Lzl&pCf8%;%oK?+DB;(N)oj{-PXK0tUIMW-7Pv^xOp=r4knSFI zFFyCnsqvhdXIdkd;iBRTv^)f)y=e(yizrfT{VaFg^&HQncG}LcuYZ%LLKp4vI3wX6jMlhG9JeVY6GJOvIOt5g+s3a~Gw15zPnTh)l+BV^bTU$G9@fI5Cyc=zp= zdqEtbMrnn8x8eDgEL{Te7CXn5+3G*&^x0=8!Qj30a=Gauy!|sLn9eW!ntXwWj2Wjj zTd@0v>})YQ=2_;7Tyh7)W%&XxR2_|a&BN7b+fJ9Q(TeEITs_QAwPx74itA=7pNAcH z#=hi>>)%ov$xy;^t@%cK;&U2(>#n{I# zqlfgnCnHLNW?Ci6C(U_!8tXDf!gFI%{p5pb>KagozVAhCC5HNuTXjG`4uG(BWUu>9 z>7jDPMyEplVcfV*@QEU1S6xu!}+*ff-PcNzKm(j!^UE{-WvE z21hXnXaYzaDaE)50UJ|d&)uvBU@A>|++ptT!#I@=e?nSd;Gtd0vw-SpS*cshB?s9= zOq>0?e^+A{FyAwF3JKPBqT1BmeCwT+&qyQbZ0Q zV+nW%_aqv!I|Nu5d2n18Nr`<#bnAA_(`LkBbA2Ma%uPn;?9WD-_BfBZtyZnh4NWux z_mUkcKPw7Q#a{>3t<|23YR$Od788PORwamm>!KeuofDk+`(hEM1*5E39~sd_!`a}$ zArhJYC4fi6r~#O{)!w`M0~d!ob1q{HK`N7x;w=2+T3i%cRBa|rDulNSPuanP**lHK z27-HGx%dk7O^33l22%$a6ll!44%vD+Sp{3U$ivZ(@YeizUiOoa$wuVwg@)qaK1z#j zh3`J`O3{rCI(-NhB;rOrV53^+ajOupb`_Z}$)cXfn@9s9W2`{{{b~0ljH8KPApGk5 z^isIVbv(}7J-?%`nsorqB)jknkuznT=it}N?BJdJbry6x8z3~Tx;If#|CWuZP+GB0 zVGp5^BIiW`XkKF!2}_xFK*NM}Q6x=F(djg{{j&$y?*+gh zl=b^Z4#Xc+jy2FLhsrl#S>V9OiVA0SMt_S-Z%ensETc>$^0uuEA0$)Kqc_UPw7Z%D zmR2-IxodvuVY#lbVZzk4*&$H{r_qOEwYV*t5K*^Tt@!up+5MJlE{sNT`>a0ASmho( zhj!!Naa1)dJ3NEpt>AqVI>@IwhXFt}!RV`ph1`PHS$V=#*3as1D=QWt2}s*&r1W5dmp%JsZux?VBGWvpHw$|9-g}z2EAiz#jqxY8;>$U4q20 zv2omY=DUtgR=d*8P(yNnS5R<_xFiR4iZ&hgHhxf&yA5IkX1&AUaJ24V6Vr9E{Na#Q zrX8VTx+2VrF+$a_LR{IU+y%3_@sH8BVM~wkK%o$6jdn-Kb{vIO%a=v7X5b-JTMu2BStViYAP*& znN@S}wUVl%A_${sc%9ijDoGbgYoU1CTVT4B<}_8o4LVBBhwYqe4dIpsN|W8WK^(Y( z4>6Yzvdt+FqZLMUcwd>p)}1q@MUddh!Oi=9mb|1L+%JJW_uJ~)8Wlb~LP2Y5*&YcoN|S zn(I|0;tzkCtljx}gwi6`*GvaF56#GCLd7AJeN0k(x=xYN5@cCwVO0zM9y3o%0x%_S z!AqxSbBKe0fp}h>K8Zc|d-80aK>jKbD{cMLelysTq_!n7sosxok;5=O?ibqCf^?Y2 z!6k$Im~K|Aw!@6`1YGMUPCP} zA>4J}zmgdfIiM;a49M6u2)yWNAZooc`zj~u^D&LA0gBscp`m&006gAu}}UM^kX@wQ>4``f8y63*|Y}@W!tOegxb-BW$5JW z$F)$GA5!5ab47_g>4xij+#K6;w2-fkch=Xg$4bP{y~B^>nFTdye%rBJeg@s=l^U@;AhR^^;!9-s04ZUG zBg0(C%`d>7cuZi%a_W6u?(k%UI%=q=n1J?_0ZC}>@Fn`R^D6=AT}Piy3>igW4oygP z{~aKK_xXNL2jgx0$SqS~!$M#&vW)RyVTxEQSp!1)(SLem$h??6niNZ9LASA7NpA%~ zC34&bP14k?PWg0KG7;a>##X{PClh$UR-l-cP^Qm$fN9(+aoSG{0o@j5k%xG-ogLrcx?QmvBZaA1iF<}aj|}_YuRw^U4qy4Or90nc zzj20i)chZ^0xquL8*g0AZT!{`2Kt0gz!2JGYKL~_%uDWlo%6qZYD$) zg2i!M5j~%uK)ZcOv~wcoM5HH{dlGYRQA^TaOHm12usgi|#Z{4$uj! zIo&ir=b!%AsTH?>e zkcEggNSKg(EU$3XH`+{`zd_XN`};%$_oX0l6FBdES82IHGt?vSooi|*hXUJ;ZG&U znz%D>Rdu$@DI$hqC_064g+)Iw(2&jp-9YqVt7#>43($rxRNYsn1J51HkW+(7x1(dV zqRZ`mtp=ob?24S9X(}9p#6=1`h<^rG0;X))9gfj;Zf02BzxRstR~X;OkT|k}wV%by z`>7A$2H%bYMDB@=iI1eBw;9Y?c`E+L1vvBD(ReA4=Odd^Cf%{-XmQ72Tj3$hC6AVr z@$kkFUR~WSTC-sED%ooFoC@ZsaACLljV^h(>X#tMNaM^!-qA$K;^BSn_$nO=SSbQDO zyb2)`#0hM~TxR-mD>l!3Ym3HSrSx%3%Jj$%X2ABoi6KwmUKCH^+PHs9*E?MK&_96= ziw(TvqulOcJzvrPSJ-z4)V%-ylU!w7DP$(3l3N+2TWLt8qeT&&G>L}x(rFVJMJlCe zOVO0}Mk1j>CrX2cal`R(Z}B+4a$9_Y)CNyk9+Z=P0TFv$upju!N-{YSB%b(mihGNd zZfzlpS171L3hD|{V=WimBLi76j{RClpdmDHf!%)Ccqz?o_4cEl|EBdt@OAD!-)GRU z$3*-Jc-mRP_*iNQCPlKB79a>)@c!4q^ zLNVE@`S{eqhVJK&4d)JltPn0isz{==Geo`?qtTn0ZV9A8H@;qmV2-~)BSeu%)(oIs2nxLE z6>ye|Sx%<~prSvfxq7|RI917Z`m6>Yodc6bn!^1exp$sGr3xk22kQm_h5~mK4jnT< ze!E@A`aItzvJN1R{Sbs|vzHrh1KGmpfbB_`Wn;a0KYkCdGKBUe><$RlvhhoIx0Jx8WDKjg$_AnH*< zAy6M(Ld|-`k${E)*lyWKS>ZkYI*nr(W3w%eU>FJo7fKpXbVaBp0l3Ib|IX{fa9@F5 zhAGcDd4CGU_V*Dhcd!NdRDmed6_A`H|D+71-6+HBlQg~U2$w|VwcF$)Ld5A|1DV#- zyefe;x3IKjNppj#32&w87@K6xsa3gFx2nTN?|1sh@jH;_s$g;TvI^K&#d{R;=qs+J&TRY7SL6ni&=^g9Yr2PS%-Z;*=i&?Jh?fYZTlF!WT z%8ZRtJII@X*@1!y>t)nG`*4`hUqr$Lldcvj1KxJO_M$*=npjyqn%j6kq{x}%mQM@u zm&7u5MA1GyrjRo!CS)Nyi+8@~)V=7)Z%{cmrxUrU_YXRWp~u)Ch?G9>AM3Q|WZh3t-7oI!-n3Ic=BJ4k7U0Rjn zI$UznvBEIrAE>k6WwDAcc*%^m8fE1b0y%1UGM#|h-5Wv6ufwUcIZ4Cd38kWrzQQpb$6wrq@KB*#oNS+`) zUxW>)@h8ol&5fjx>-eR3+{2N~`-J)GE+kXNzza_OkHp4vVRMv2>2UXQRbMb#bpr%E zIki^0G8CD@)vzg)&Y>=zAtV$@d{ z{HnJ-t8cQ*UHvnW#kB%D6(v-53-uqj-5Q#9F6-s*BP6qJ*X=32HAJ_qUo34nI>izRDk$Q|0EAl~qcA(^Sa#(z09*RjpL!?uDVJ&SuvIx3+xrGn)3iBK^d{L^a*l~FI*j&A6om!hbDn&p& z$E%AALpI+D5%8h&mEBE5)Jwwfi*o2xJGUPtCIz*K(J?N=Qg8(%5AQw-=%<-)t%V;o_vP;fgz4+EXXIrur+rf*A zGI;p>mZbS!a9F8?Zb8*}@^*!!<2gn^iFM&eCt}Pez4XtGpzAwX?Vsr|ksUBNDyWg| z)~Sq*uhn^>#Us+kL$F4*LE*yE9+$qC`Q(Z!G4-K}TIdI>Z}+chn_h^PWea$SAV8}n z2*EKFV8cYceh1$Xt_RC+8PD8Fp(wsL>XFhG+n9J^buGM>!1;oC&(O0qf~O7xy;xtN z6#P$U=X5x(TFH`5sRh*3YD<%^45XtyZrFFfzAMB59Yki~1E^#ucmvL5#~XIe409R2 zH-a_Lb?VDGMH}YrPCnZJblaKuXg31NI|2A$<%4{~Pb?ofs!DT$C#ocuB)FWb^wq+} z)n-ER&+ZXPJQ92PbVYD?SqCNPApM(XBbkae3r3}es7LJ6&iEn6uYfN|7S;zqn)Q*Y zg{zt5aRmmrmb;#G{T>`#-TR*n+bSm?13|CQ@bklE6v%eveOn(_nawa3An=DfO5t4R zqF!IpT3hP7R`VLCun?Ham6Q3B7;D*~lG)j`LfJ_(3TAcsrNLW=Uw2{zvaH#K{A6yn zqabgwv1L2*&4lsZ2B7OC+Kk>DJ{&%7A5;J|fV$zEFJ;jLsR@I<%34Y{UbHH+<6-ev z42+y^4xisvEjcmT-SDl$Kjmf|=VP;t-{$l+mmicMJZF2pQ*Tyg4oJK!5$~2|V~H!3 z_QH~+Y-pwXu&8^XhF#G+!C;tAP^U-mO@fskxg}aTcX1kpF&At0IUP7Z6~R*aXxMff ztl=Uffe!LqvLWxJ_pd?GjQeCALL5-w`7Y~y<#SWj{ibHlQmlc?^QhCOOoM5glsKNV zEG3>!4sik*s4Le|bBKGR^M%b9wuuAB(cC3-L2qDjWaX1& z6cqZIep=;rTQBgE{CQTB++Bh`Q682I&SyoXm}V)lRuoOH)1*ejZOY`oELUx1zU~&m1@-iFVIu86v~%?ddXHV6?9t#~LVOPNt~XFG zQb=wQ6pYK-+tc?RnD3U@Mzi>mkf$gYQvkje+jk3GH15fGGzomWCE;LJoxSr)%VMas z5B>G`29w&wj$7YFNSTN8@Xd9?wo&>qLd}iucNQ4TWc} zSktPi;wVw^3}6k_b!UsAnGkm#xbGzP=G{5utSy?6;EzBPhJ>N8S5GRX&0UtRyPV=l z-@rSiFWdYeM+Xa%s%Q%iHnUe(WeaDhR1isLtq~8CaMY+JrktkCtHLL9WCK2ns{S>D zu~{Rz>dNF=m}3XN^%;sfVu=k?dJ}=>^BQ6Sgld2aWdu|xEd|3pYuWYmJje_Z|5SKY z1J3wP)v=!RWfk0>ZoQu za6<>1blN82GNtP_G4EoavHEmPkM^kDw8tU-Ijq=%i?-|}g%8U2w@z*i9!3 zV_=Mr2ToPfpKo99{A{Kq6p9QB>D8;@XvLc@FKb>z8ajIU;LJ%s-=+Vn0^fn^mj{?= zkFrpfY5;pL?-sDlG78|YYwo4ykE?y@4OB1Mw?fqww8XKYp@Nh>5UDwB>=}`FMk|_> z&gEzXJRz&tK5Y+|MbAG`Ar3gN25mvG*Ihjy_}W3^FL}H~7kG)e;EEnKh0pnbIEG7Z z?DR^1z2UDNS(c4vx;|Rci zv@G;}%^n+Zbqw#{qk*IP$i%L-o2tQ;Rwg{sD$AIhjG<_wuyovXriNc){0I}gz z*)vM;&)&7WcavLpT^4~?aj?eHrNn`hjvDh%09f;fAZ`(Bgy{^I!L;KrLNu!N>kjDq zMZ#-!Eap<%5rasDw6k~7RDvTs2(Vq@ZHIx((vXh2()2pHidQDA!RUy8oi(R!I@)`7 zryn4`P{-&(h!M4L)Y=ttIio@=^imVhXE`)qC<8BbA2`ikEd>r-g%&L@hQm!nLNfbR zFacl=0tikmh8ly#YW|^A@6o|i!!z%@5DQ7M3KrJ43>PVKKjIs)kFP*DNOaKUYVD5l$me^5ZTWtEBBh9iM|g4RRes4 z9V#m02&pD2{@T^ZgXbV=CTr~NcGeUKXY8hAz>7cdJmakj!*EL;a% z#DU1^H)L(P)yZP}u7G4IB%_sUNv^vT;mMmfB+tKs#&iq&u{ebzI)$?@$mi+=Y#}+V z#EOA&yQ3*;+V?)^^5tuGa=+dM@h&g$?(69rm*I-MgA^h+I88v~K8yU!cW3#GID~gT zGrab^`60}tn=W5k1Ao7Quwt!D$mc820F&g#s)#L>dfJ$rjI>kALrcXzPZE31H44Qt<9vMbgPdxu5{VHs!M-2&hC z!v+{L=?{xtF6nh35^t@~9&gKR6xhbcin;Gs*~NT*_<4Hty$2s8ikcA2{unf%vJ*^F z1GIGHOQU(@)tUv`n$e?9SN4&u7DyF1!I>W}?N;xkt%!eLxumTkr1H_`N15`&JVYSs z#Zh&7e7NRc+Sz*PIM&ePVbMk?Tbdpdh`Kepvs!iNV&EPqc+J^L>v*L&cVFUZgQYx6JPCQULW`J6sv~? zV_&aJr^bMvMM5%5_Xr2sihgB+yX1kJw|x9=gfcJ?AdjoBX|>7>?!ObY(M z_OL!d-MkF4hI!n3#Rtvh+GsL)3?xBn#~nK!k@Lhe;u9o9a=r3g=8x2XNYV~7uw7cSwM z+TjmHN_F}UMX*SOL8y3TeH?+{` z@)W!FjC6kH?AMX?!(&HtSw(!q=Jxh+kkLwOFRzcnKQL918A_E|&r-g1s zzPDY#$>_zLLO;ZWll^otC%D{59bGPJ`FZRHWq`3!%%YwLL84?z8?#7`-bwZ>ebFqB zf>{Mazl&0U&$H*Jm;HHNqfzw_>bS>o5J1N*s-Ec$mVOQ zd*hSfS42U+6nk`12bpdAU}sa%=6Yc-MHScvPq9;X7Gy5f*LjBr&xvK+45s%}flNPt z^gPA*&OLefC5*uv2Z)$z@HKW_oGDf0FKkRq)l=HfGuciYd?mQq@R>KsyC4wat7N~0 z0O$LdGwzi@6>uSY-Wee3#YEPudymIy2Mj^9@y#}gM4m)W+mt7r6kXLHXx6A-U*1F> z&Qq>F;E6FIQQx;)={KJ^91n!$vx74JLqh3`*AD=f-9cOuu|?7K6!l>uxv3r?t8>US zbozBr`4JC^KW_qy_hrv`_I8ZzBNSM0l44R-zAw}zn-|qjRYR6vu%i#!TCx`YG`^}W z_}JLM^G(ML&$7r1Mq#a%XvRo-M34hJ&dI#^iX;E-s+JD(u05T!o*27LKc&Zg3wYx6 zq?$GkyDLZX#uWV)6J6K$g!DstZvTx&65|F3d;n1ByMLQ!dbIk9wcS3hXY)a9rDT_J z8iBoI<7Qma0gU3!#F8~v#QIjVFyzOco^HZ+bUfc-N4<{e4C7si#qG z6&pOTaJ1*nNg3?H8un<$I8uE+TWsV|%rEqGG25($LIQKZ1QE@#1{%Qs`v4d*-QZ{^ zD;P`!i@?Y;+bdJq71Up%v857Np(-~NL7_8W$CdLAG3Ynu)!GOeG#~HlWx=_-QZU=v zZt42HZzKfG*O8$eg0gGD)~wn8+I9HC&+tK_@E(jN47LjqEsk3=d>etPS%?Hpxjrnn znRM<2HzM|(ky%Dv+%{5aa@6HRe}M&)8$F&WnY&9$NR zpDQ@upTv{vu1AbSbMKKAOKE}g`sx_dreE*+L6{tZLBFk>FET^T3IfH3^-M^An*H5K zDK6KXTy?D)bn{gh#KB0_nm7|Sla%Zq-)}QmX;P{`@y0i&H-jt%gmi%C%o`o|qawSj z%OtXH#PNl8mqEVOj>x;~V?4Pu%r_N64StSJkMQJt-G}ie1=@Dbl@~S+@#ID%Ife1m zEs+$ZL||_r+vknSv#JjAxvd2ZHoDYYKRa|=t+0L4vUq4{#x;UDfu}s&?EOqHir*)v zC#$U689ncP)w*xQ++dm=l}VU{W0oBwJh?(jfu0k_WgyI4sgV&Xb?G7xNOb0Vp$PL- z&V!g1Un>4I17qKrdiNCiP?B*PE4;&(*Wuygh=@9N$%)loMUNkaJXHe&5?5`t@eJ00 z(bun3+kzn{#_EMXk5kOSO}{TRcg8{^@O8baZf{Qrr}O-|{LH0~#8TqcqGdZ*8ciw3 z%z#l-!FOU|9>i?diNSvKo)*Vdva;2oJXd&|amGJyapD^j4{^3L5@4z;^fQ;L+`k-A zCbkQ9vQRAQZCRayJ*i~t1ED5Tc7qPqz&g&(*LToa5NF{Py=wq;^r|&vi3ZiZ*OU^4 zfltS!s`~Fj>3lH?3(;b2PQl*T(Z6vGyHd;{ym+xunAaiwAr#n3s4*KGZ?XZTT#Ib@ zePBz}9y}nTtJxM(b`6Vnv!AA+=WF)4QvxLI5Od;t4FQBwrGGzXZYrrSr~c9GlcehT z2iP%&!;p+WY*IGe$|i5k^82ytl8Pm66Q51b2__n@qdls()M&Q?u8Kej$ zrbb+-J)Nm1I|e+SL{ZDv#l;C!rZCyeoAV(fiV-+N35A+-E@kT8CFQ> zlO`p07pAf^AEr9W&=v&2wJiQUm9Kob)CIighT(V^KYw5QG2-KnJp1C%?@Q0ZM6LW7 zQpLWG@M-LCAzaTA4Wb)czJKsmm%-1t`WearYPlp|2>Y3e;J0)MOUwJ^4yvyvY@g-B z@E?F(sj<14YI%D6o3=;mU+#P&dKD$*#M|Ky#YN>&4n>ozP%u}ka!ImI4kW;QMA(}( zPSb#TeSexPnyiUN@xL(vC-HG8Xe}q35K1#w@R$pPVIcikd1vFSzJ;h8K?P>Gf>@RI z?Xx;A7+;Mj37z&$8IG__@D$Le0Gk$vcZk(PlJq=b@~s~LT#MV$*6iZ8ZT@I~*E)f^ z&asKK?(Rb{`5Hkf#O{*xoob0bT6qV~Uf+qkB6OZ{vgfSkg;_oz62|pSQe9kEs+!HQ z#+QAvple!Uncy-_(6sT5U9(1unKt7L+Kk-(#$T^TcQ)`;%p~QI@8P;?4J31|VeQ$K z;ZU(9J{SVo=<(7Rn)$K>dqr$ENa}}6)ZFDRE&X_f!M}dw0L9z2q1J-Fdku9@Jr9Ps zjvUv^<@=tVOXN!#md~MdzIYtQ--}L(BG#G})|sWF`D>zuxL=qjBy zDyqLntatguGXu1GMQUet+Zf8ZoROR*^L{@H2&F(~3754kFTtm9*^%c_^}Y(O1#64( zu%w>4)Mmj}(}%ZE-Xz0fkKP*wG0C#A62d$oa63#BdoFiMKx34jOBd%H+E&4=*BAg1&~a-gv!Eo49bXPtzsSS zUcZyZyLQXLu0m$2?KZ#>Danyu|9cqZNECFZ%vLR;(wb`{<2I-W>lD-t3_nv#Hq^VX zv?4JyBEyl^A2pH+p}Wa|1bpvD9+_U{e6RV;Y2DG$C?Q@3o%ZcP$VHMTjCq2iz35r; zQ%Fy6*wQ1GXI2UC9~wL@HuUuqWa_ySmjW7d8%;u_-Do=~DOPT5|GRRxw#i~k2(9S{ zS@$$^qgPhZ+G*R{i4dk$Y1&c$$r(QqQ2Vc`Yj(NJcpFFJIq&ei?;upz1P8yGcXDN% zR>G;8ykuKK?Ga|(=0|5{zl{NI7|?@HtVvE6{VVz$(KF77mKAzIVRx4>13JN~$1 z-fH^n2>?QL>NWW{V5dDxQlrf6zRS`cLezSbH1hEf+ z!^BAR(D*y2C@o#$QB{SITQ1w)NBs-Nn}^>np1cqb_k!2G;N1T_GD<|vDz!A2Y;+(W z=*#vrvs!_*Ai%MglXG*f7ugp2XnnHruAY@d+YZ0cD}WCtbv@uPBkrpj@U{6j65}5Z zAI)qe+!pcWK^b8$DR`_V;Yh5%UThsF@K=e4TFGG`EQ15v^gKTP_;Y`*$a$`7ca{J1 zkv1(n6vS^BZtJMw)bR45g#)&(UAevV9PCuc*cp*=Bd)-0HN8l!(`o%=VGo;37o_wK zN7@_J{OVBCdaSi(G#P53;6CsjU5`vs>7ya5S1?|Oi6##J%z_49seq1O^KF~?| z2{SL1QVtxqd0^ANzPJ?3gXa%Cy`0iW*W7xfp{Hpv2g8 zH^U<}_ja(cWWe7jdwtKG^MvI*H408Ct!J)i#WvsdhPbX$URkYR;PsB|d+&shDtpYT zu{*GRqC|98YP%qMB7XEeoKhCOhP$S%er1%iC#k!aDn8G47b(FqNzT-TI38z40P2J z?%^8w@Piy3v@a&8({i`UQ_XAY`=@+S)TkgTwu6Bl{U%a&iUi)|+Fov5w5)d`y5Nq1j>TeFt#_1iwv=^=p$*Z0Y0<<;v3`?e7+l3 zmuNPdLx^FJWFI>OOk%*u6m5n~M?sy%$A$ELHBiykWsym*nl?C*B@2~GTgOCi*iLCI za5A?O->9TKO;VarV0K_WO^X-@P|yZIQ_bg>dNGnZsqrr}g^Jx*{MNXU_r>5KsOgQ{ z>i_fTfH#$gVufhQYz#%F|E@;m7{@0iR?DgIS5G_jC_y9?Z%B?(81*6K6&BS1{yGkP z$+pfAd>KE`y?*0$krUr9rBH8zFdmp*`;BP4zt@qavjWwv8GpHfmX)s>4mFv&k~E-?k;6#DmM5-&wTPwCm^h zi!v8N*xMhCL%d4Dv=!9vGz$)~e;}ytSS0}KKwt!iAlR{x@h9nRD6gB5!;;nbGUT#E zNHBFfw8p~E$+m*ucOH^ebYU!e0{>iGlDljowJgaYjCIfaR2bVw0e9X)-ry^gpf=26 zVzMGD^fFA2FhX>qMF+u-2w{k@F;_C*pxVKw9ITSTd6kG~>ET7uo~zs_qzLb*eW=5-4G@8ke3y=B6@uP^o|k^Lq_2HLHBv^Psp^@h2w zwUm_FpN|SqB?U$0`EdDT%eimV7vvf7Pb7@zMClmsR*Su%0`1X!LBj)8c%sKeB22#l zPA1}B)%>-Yd`B!x6-bQ4pYo8lnXOTAec1p0h%xObP_y*yeK?4&^X7*uLNt+-vRrD& zf9aZ?vV@kLEMBmRdXqYO3vE@gN+@7z%B?Mz^vlj1p8LUL#UnlXagVvLE$B0RswQ8< zqP2SQmemh6?yrj6zhkp7_m=IYR3#pxtf0u&{B^8vhLU+|T|s z&#Sey^@Ogj-_UrLqN1Xav$J^3Cg(E`!^6Xsq9R_4-C|ZOG)qD zyShsvAoau4l>0YoP=0>C(wFh$0*w^C)Vi&cSI^Q99Z!Gr_N~LJ4|QglCv#?%DcG;FFJohUtKCJs>WL|#xVz_VLKKY5MBg>X8dr?{{ zucD^bk)GGk&|s3?&_6UZl+xVSXRV^H{<5S*R>;m&?!P~>l9{=E Date: Mon, 28 Aug 2023 00:37:00 +0200 Subject: [PATCH 806/947] Remove unused imports --- BepuPhysics/BatchCompressor.cs | 1 - BepuPhysics/Bodies.cs | 2 -- BepuPhysics/Bodies_GatherScatter.cs | 4 ---- BepuPhysics/BodyDescription.cs | 5 ----- BepuPhysics/BodyProperties.cs | 6 +----- BepuPhysics/BodyReference.cs | 4 ---- BepuPhysics/BoundingBoxHelpers.cs | 2 -- BepuPhysics/CollidableProperty.cs | 2 -- BepuPhysics/Collidables/BigCompound.cs | 1 - BepuPhysics/Collidables/BoundingBoxBatcher.cs | 4 ---- BepuPhysics/Collidables/Collidable.cs | 1 - BepuPhysics/Collidables/CompoundBuilder.cs | 2 -- BepuPhysics/Collidables/ConvexHullHelper.cs | 5 ----- BepuPhysics/Collidables/IShape.cs | 5 +---- BepuPhysics/Collidables/Mesh.cs | 1 - BepuPhysics/Collidables/MeshInertiaHelper.cs | 3 --- BepuPhysics/Collidables/Shapes.cs | 3 +-- BepuPhysics/Collidables/Sphere.cs | 3 --- BepuPhysics/CollisionDetection/BroadPhase_Queries.cs | 3 --- .../CollisionDetection/CollisionBatcherContinuations.cs | 3 --- .../CollisionTasks/BoxConvexHullTester.cs | 1 - .../CollisionTasks/BoxCylinderTester.cs | 2 -- .../CollisionDetection/CollisionTasks/BoxPairTester.cs | 1 - .../CollisionTasks/CapsuleConvexHullTester.cs | 1 - .../CollisionTasks/CapsuleCylinderTester.cs | 1 - .../CollisionTasks/CapsuleTriangleTester.cs | 1 - .../CollisionTasks/CompoundPairCollisionTask.cs | 4 ---- .../CollisionTasks/CompoundPairOverlapFinder.cs | 3 --- .../CollisionTasks/CompoundPairOverlaps.cs | 5 ----- .../CollisionTasks/ConvexCollisionTask.cs | 2 -- .../CollisionTasks/ConvexCompoundCollisionTask.cs | 5 ----- .../CollisionTasks/ConvexHullPairTester.cs | 1 - .../CollisionTasks/ConvexHullTestHelper.cs | 1 - .../CollisionTasks/CylinderConvexHullTester.cs | 1 - .../CollisionTasks/CylinderPairTester.cs | 2 -- .../CollisionTasks/ManifoldCandidateHelper.cs | 4 ---- .../CollisionTasks/MeshPairOverlapFinder.cs | 3 --- .../CollisionTasks/SphereConvexHullTester.cs | 1 - .../CollisionTasks/TriangleConvexHullTester.cs | 1 - .../CollisionTasks/TriangleCylinderTester.cs | 2 -- BepuPhysics/CollisionDetection/CompoundMeshReduction.cs | 4 ---- BepuPhysics/CollisionDetection/ConstraintRemover.cs | 3 --- .../CollisionDetection/ContactConstraintAccessor.cs | 4 ---- BepuPhysics/CollisionDetection/ContactManifold.cs | 3 +-- .../CollisionDetection/ConvexContactManifoldWide.cs | 1 - BepuPhysics/CollisionDetection/DepthRefiner.cs | 4 ---- BepuPhysics/CollisionDetection/INarrowPhaseCallbacks.cs | 6 +----- BepuPhysics/CollisionDetection/ISupportFinder.cs | 3 --- BepuPhysics/CollisionDetection/InactiveSetBuilder.cs | 5 +---- BepuPhysics/CollisionDetection/MeshReduction.cs | 3 --- BepuPhysics/CollisionDetection/NarrowPhase.cs | 3 --- .../CollisionDetection/NarrowPhaseCCDContinuations.cs | 6 ------ .../CollisionDetection/NarrowPhaseConstraintUpdate.cs | 7 +------ .../NarrowPhasePendingConstraintAdds.cs | 7 ------- BepuPhysics/CollisionDetection/NarrowPhasePreflush.cs | 2 -- BepuPhysics/CollisionDetection/NonconvexReduction.cs | 6 +----- BepuPhysics/CollisionDetection/PairCache_Activity.cs | 6 +----- .../SweepTasks/CapsuleBoxDistanceTester.cs | 2 -- .../SweepTasks/ConvexCompoundSweepOverlapFinder.cs | 3 --- .../SweepTasks/ConvexHomogeneousCompoundSweepTask.cs | 1 - .../CollisionDetection/SweepTasks/GJKDistanceTester.cs | 2 -- BepuPhysics/CollisionDetection/WideRayTester.cs | 1 - BepuPhysics/CollisionDetection/WorkerPairCache.cs | 2 -- BepuPhysics/Constraints/AngularAxisGearMotor.cs | 3 +-- BepuPhysics/Constraints/AngularAxisMotor.cs | 3 +-- BepuPhysics/Constraints/AngularHinge.cs | 3 +-- BepuPhysics/Constraints/AngularSwivelHinge.cs | 3 +-- BepuPhysics/Constraints/AreaConstraint.cs | 3 +-- BepuPhysics/Constraints/BallSocket.cs | 3 +-- BepuPhysics/Constraints/BallSocketMotor.cs | 5 +---- BepuPhysics/Constraints/BallSocketServo.cs | 3 +-- BepuPhysics/Constraints/CenterDistanceConstraint.cs | 3 +-- BepuPhysics/Constraints/CenterDistanceLimit.cs | 3 +-- BepuPhysics/Constraints/ConstraintChecker.cs | 5 +---- BepuPhysics/Constraints/Contact/ContactConvexCommon.cs | 6 +----- .../Constraints/Contact/IContactConstraintDescription.cs | 3 --- BepuPhysics/Constraints/Contact/PenetrationLimit.cs | 2 -- .../Constraints/Contact/PenetrationLimitOneBody.cs | 2 -- BepuPhysics/Constraints/Contact/TangentFriction.cs | 1 - BepuPhysics/Constraints/Contact/TwistFriction.cs | 1 - BepuPhysics/Constraints/DistanceLimit.cs | 3 +-- BepuPhysics/Constraints/DistanceServo.cs | 3 +-- BepuPhysics/Constraints/FourBodyTypeProcessor.cs | 2 -- BepuPhysics/Constraints/Hinge.cs | 3 +-- BepuPhysics/Constraints/IConstraintDescription.cs | 4 ---- BepuPhysics/Constraints/Inequality1DOF.cs | 2 -- BepuPhysics/Constraints/InequalityHelpers.cs | 5 +---- BepuPhysics/Constraints/LinearAxisLimit.cs | 3 +-- BepuPhysics/Constraints/LinearAxisMotor.cs | 3 +-- BepuPhysics/Constraints/LinearAxisServo.cs | 3 +-- BepuPhysics/Constraints/MotorSettings.cs | 3 --- BepuPhysics/Constraints/OneBodyLinearMotor.cs | 3 +-- BepuPhysics/Constraints/OneBodyLinearServo.cs | 3 +-- BepuPhysics/Constraints/OneBodyTypeProcessor.cs | 2 -- BepuPhysics/Constraints/PointOnLineServo.cs | 3 +-- BepuPhysics/Constraints/ServoSettings.cs | 3 --- BepuPhysics/Constraints/SwingLimit.cs | 3 +-- BepuPhysics/Constraints/SwivelHinge.cs | 3 +-- BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs | 2 -- BepuPhysics/Constraints/TwoBodyTypeProcessor.cs | 5 ----- BepuPhysics/Constraints/TypeBatch.cs | 4 ---- BepuPhysics/Constraints/TypeProcessor.cs | 1 - BepuPhysics/Constraints/VolumeConstraint.cs | 3 +-- BepuPhysics/Constraints/Weld.cs | 3 +-- BepuPhysics/DefaultTimestepper.cs | 6 +----- BepuPhysics/HandyEnumerators.cs | 8 +------- BepuPhysics/ITimestepper.cs | 3 --- BepuPhysics/InvasiveHashDiagnostics.cs | 4 ---- BepuPhysics/IslandAwakener.cs | 2 -- BepuPhysics/IslandSleeper.cs | 2 -- BepuPhysics/PoseIntegrator.cs | 1 - BepuPhysics/SequentialFallbackBatch.cs | 7 +------ BepuPhysics/Simulation.cs | 3 --- BepuPhysics/SimulationProfiler.cs | 6 +----- BepuPhysics/Simulation_Queries.cs | 2 -- BepuPhysics/Solver_Solve.cs | 4 ---- BepuPhysics/Statics.cs | 2 -- BepuPhysics/Trees/RayBatcher.cs | 2 -- BepuPhysics/Trees/Tree_Add.cs | 4 ---- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 1 - BepuPhysics/Trees/Tree_BinnedRefine.cs | 1 - BepuPhysics/Trees/Tree_Diagnostics.cs | 4 ---- BepuPhysics/Trees/Tree_IntertreeQueries.cs | 3 --- BepuPhysics/Trees/Tree_IntertreeQueriesMT.cs | 2 -- BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs | 6 ------ BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs | 1 - BepuPhysics/Trees/Tree_RayCast.cs | 5 +---- BepuPhysics/Trees/Tree_Refine2.cs | 6 ------ BepuPhysics/Trees/Tree_RefineCommon.cs | 4 +--- BepuPhysics/Trees/Tree_Refit.cs | 3 --- BepuPhysics/Trees/Tree_Refit2.cs | 7 ------- BepuPhysics/Trees/Tree_SelfQueries.cs | 9 +-------- BepuPhysics/Trees/Tree_SelfQueriesMT.cs | 2 -- BepuPhysics/Trees/Tree_Sweep.cs | 3 --- BepuPhysics/Trees/Tree_SweepBuilder.cs | 1 - BepuUtilities/BoundingBox.cs | 3 --- BepuUtilities/BoundingSphere.cs | 6 +----- BepuUtilities/BundleIndexing.cs | 4 +--- BepuUtilities/Collections/InsertionSort.cs | 7 +------ BepuUtilities/Collections/LSBRadixSort.cs | 5 ----- BepuUtilities/Collections/MSBRadixSort.cs | 4 ---- BepuUtilities/Collections/Quicksort.cs | 5 +---- BepuUtilities/Collections/ReferenceComparer.cs | 4 +--- BepuUtilities/Collections/VectorizedSorts.cs | 1 - BepuUtilities/Collections/WrapperEqualityComparer.cs | 3 +-- BepuUtilities/IThreadDispatcher.cs | 1 - BepuUtilities/Int3.cs | 1 - BepuUtilities/Int4.cs | 1 - BepuUtilities/MathHelper.cs | 1 - BepuUtilities/Matrix2x2Wide.cs | 3 +-- BepuUtilities/Matrix2x3Wide.cs | 4 +--- BepuUtilities/Matrix3x3Wide.cs | 3 +-- BepuUtilities/Memory/BufferPool.cs | 3 +-- BepuUtilities/Memory/IUnmanagedMemoryPool.cs | 1 - BepuUtilities/Memory/IdPool.cs | 4 +--- BepuUtilities/Memory/ManagedIdPool.cs | 4 +--- BepuUtilities/Memory/Pool.cs | 1 - BepuUtilities/QuaternionWide.cs | 3 +-- BepuUtilities/Symmetric2x2Wide.cs | 3 +-- BepuUtilities/Symmetric3x3.cs | 6 +----- BepuUtilities/Symmetric3x3Wide.cs | 5 +---- BepuUtilities/Symmetric4x4Wide.cs | 5 +---- BepuUtilities/Symmetric5x5Wide.cs | 5 +---- BepuUtilities/Symmetric6x6Wide.cs | 7 +------ BepuUtilities/TaskScheduling/TaskStack.cs | 3 --- BepuUtilities/ThreadDispatcher.cs | 1 - BepuUtilities/Vector2Wide.cs | 3 +-- BepuUtilities/Vector3Wide.cs | 3 +-- BepuUtilities/Vector4Wide.cs | 3 +-- DemoContentBuilder/ContentBuildCacheIO.cs | 2 -- DemoContentBuilder/ContentBuilder.cs | 3 --- DemoContentBuilder/FontBuilder.cs | 2 -- DemoContentBuilder/FontPacker.cs | 3 --- DemoContentBuilder/MetadataParsing.cs | 6 +----- DemoContentBuilder/ShaderFileCache.cs | 7 +------ DemoContentBuilder/Texture2DBuilder.cs | 6 ------ DemoContentLoader/FontContent.cs | 2 -- DemoContentLoader/FontIO.cs | 4 +--- DemoContentLoader/MeshContent.cs | 6 +----- DemoContentLoader/MeshIO.cs | 5 +---- DemoRenderer/Constraints/BoundingBoxLineExtractor.cs | 4 ---- .../Constraints/CenterDistanceLimitLineExtractor.cs | 1 - DemoRenderer/Constraints/CenterDistanceLineExtractor.cs | 1 - DemoRenderer/Constraints/ConstraintLineExtractor.cs | 2 -- DemoRenderer/Constraints/ContactLineExtractors.cs | 5 +---- DemoRenderer/Constraints/LineExtractor.cs | 1 - DemoRenderer/ParallelLooper.cs | 3 --- DemoRenderer/UI/GlyphBatch.cs | 5 +---- DemoUtilities/Input.cs | 2 -- DemoUtilities/SpanConverter.cs | 2 -- DemoUtilities/TextBuilder.cs | 6 +----- Demos/Controls.cs | 2 -- Demos/DemoCallbacks.cs | 1 - Demos/DemoSet.cs | 3 --- Demos/Demos/Cars/CarDemo.cs | 1 - Demos/Demos/CollisionQueryDemo.cs | 2 -- Demos/Demos/CollisionTrackingDemo.cs | 1 - Demos/Demos/ColosseumDemo.cs | 3 --- Demos/Demos/Dancers/DancerDemo.cs | 3 --- Demos/Demos/Dancers/PlumpDancerDemo.cs | 3 --- Demos/Demos/NewtDemo.cs | 1 - Demos/Demos/PerBodyGravityDemo.cs | 4 ---- Demos/Demos/PlanetDemo.cs | 4 ---- Demos/Demos/PyramidDemo.cs | 2 -- Demos/Demos/RagdollTubeDemo.cs | 1 - Demos/Demos/RayCastingDemo.cs | 1 - Demos/Demos/RopeStabilityDemo.cs | 3 --- Demos/Demos/SimpleSelfContainedDemo.cs | 2 -- Demos/Demos/Sponsors/SponsorCharacterAI.cs | 2 -- Demos/Demos/Sponsors/SponsorDemo.cs | 1 - Demos/Demos/Tanks/TankCallbacks.cs | 5 +---- Demos/Demos/Tanks/TankController.cs | 4 +--- Demos/GameLoop.cs | 3 --- Demos/Program.cs | 6 +----- Demos/RolloverInfo.cs | 4 +--- Demos/SimulationTimeSamples.cs | 3 --- Demos/SpecializedTests/BatchedCollisionTests.cs | 3 --- Demos/SpecializedTests/BroadPhaseStressTestDemo.cs | 4 ---- Demos/SpecializedTests/CacheBlaster.cs | 7 +------ Demos/SpecializedTests/CapsuleTestDemo.cs | 3 --- Demos/SpecializedTests/ClothLatticeDemo.cs | 5 ----- Demos/SpecializedTests/CompoundBoundTests.cs | 1 - Demos/SpecializedTests/CompoundCollisionIndicesTest.cs | 2 -- .../ConstrainedKinematicIntegrationTest.cs | 5 +---- Demos/SpecializedTests/ConvexHullTestDemo.cs | 6 ------ Demos/SpecializedTests/DeterminismTest.cs | 7 +------ Demos/SpecializedTests/GyroscopeTestDemo.cs | 6 +----- Demos/SpecializedTests/HeadlessDemo.cs | 8 +------- Demos/SpecializedTests/IntertreeThreadingTests.cs | 5 +---- Demos/SpecializedTests/Media/2.0/NewtVideoDemo.cs | 1 - Demos/SpecializedTests/Media/2.0/PyramidVideoDemo.cs | 2 -- Demos/SpecializedTests/Media/2.4/Colosseum24VideoDemo.cs | 1 - .../Media/2.4/ExcessivePyramidVideoDemo.cs | 3 --- Demos/SpecializedTests/Media/2.4/NewtTyrannyDemo.cs | 5 ----- Demos/SpecializedTests/Media/2.4/RopeTwistVideoDemo.cs | 3 --- Demos/SpecializedTests/Media/2.4/VideoDancerDemo.cs | 3 --- Demos/SpecializedTests/Media/2.4/VideoPlumpDancerDemo.cs | 3 --- Demos/SpecializedTests/MeshMeshTestDemo.cs | 6 +----- Demos/SpecializedTests/MeshReductionTestDemo.cs | 4 ---- Demos/SpecializedTests/MeshTestDemo.cs | 3 --- Demos/SpecializedTests/PyramidAwakenerTestDemo.cs | 3 --- Demos/SpecializedTests/ScalarIntegrationTestDemo.cs | 6 ------ Demos/SpecializedTests/ShapePileTestDemo.cs | 3 --- Demos/SpecializedTests/SimulationScrambling.cs | 4 ---- Demos/SpecializedTests/SolverBatchTestDemo.cs | 3 --- Demos/SpecializedTests/TestHelpers.cs | 2 -- Demos/SpecializedTests/TreeTest.cs | 5 ----- Demos/SpecializedTests/TriangleRayTestDemo.cs | 5 ----- Demos/SpecializedTests/TriangleTestDemo.cs | 2 -- Demos/SpecializedTests/VolumeQueryTests.cs | 3 --- Demos/UI/DemoSwapper.cs | 3 --- Demos/UI/Graph.cs | 7 +------ 252 files changed, 84 insertions(+), 741 deletions(-) diff --git a/BepuPhysics/BatchCompressor.cs b/BepuPhysics/BatchCompressor.cs index ba3ee0067..e6500dec8 100644 --- a/BepuPhysics/BatchCompressor.cs +++ b/BepuPhysics/BatchCompressor.cs @@ -4,7 +4,6 @@ using BepuUtilities.Memory; using System; using System.Diagnostics; -using System.Linq; using System.Runtime.CompilerServices; using System.Threading; diff --git a/BepuPhysics/Bodies.cs b/BepuPhysics/Bodies.cs index 4db50c950..161b9869a 100644 --- a/BepuPhysics/Bodies.cs +++ b/BepuPhysics/Bodies.cs @@ -3,11 +3,9 @@ using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; -using BepuPhysics.Constraints; using BepuPhysics.Collidables; using BepuPhysics.CollisionDetection; using BepuUtilities; -using static BepuUtilities.GatherScatter; using System.Runtime.Intrinsics; using System.Runtime.Intrinsics.X86; diff --git a/BepuPhysics/Bodies_GatherScatter.cs b/BepuPhysics/Bodies_GatherScatter.cs index 0b57f5947..ffebddca1 100644 --- a/BepuPhysics/Bodies_GatherScatter.cs +++ b/BepuPhysics/Bodies_GatherScatter.cs @@ -1,16 +1,12 @@ using BepuUtilities.Memory; -using System; using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; using BepuPhysics.Constraints; -using BepuPhysics.Collidables; -using BepuPhysics.CollisionDetection; using BepuUtilities; using static BepuUtilities.GatherScatter; using System.Runtime.Intrinsics.X86; using System.Runtime.Intrinsics; -using System.Runtime.Intrinsics.Arm; namespace BepuPhysics { diff --git a/BepuPhysics/BodyDescription.cs b/BepuPhysics/BodyDescription.cs index c2ef06324..5c97d6e4d 100644 --- a/BepuPhysics/BodyDescription.cs +++ b/BepuPhysics/BodyDescription.cs @@ -1,9 +1,4 @@ using BepuPhysics.Collidables; -using BepuPhysics.Constraints; -using BepuUtilities; -using BepuUtilities.Memory; -using System.Numerics; -using System.Runtime.CompilerServices; using System.Runtime.InteropServices; namespace BepuPhysics diff --git a/BepuPhysics/BodyProperties.cs b/BepuPhysics/BodyProperties.cs index 933e267ad..85d93c495 100644 --- a/BepuPhysics/BodyProperties.cs +++ b/BepuPhysics/BodyProperties.cs @@ -1,8 +1,4 @@ -using BepuPhysics.Collidables; -using BepuPhysics.Constraints; -using BepuUtilities; -using BepuUtilities.Memory; -using System; +using BepuUtilities; using System.Numerics; using System.Runtime.CompilerServices; using System.Runtime.InteropServices; diff --git a/BepuPhysics/BodyReference.cs b/BepuPhysics/BodyReference.cs index cdd484cc9..c6e7de8bc 100644 --- a/BepuPhysics/BodyReference.cs +++ b/BepuPhysics/BodyReference.cs @@ -1,12 +1,8 @@ using BepuPhysics.Collidables; using BepuUtilities; using BepuUtilities.Collections; -using BepuUtilities.Memory; -using System; -using System.Collections.Generic; using System.Numerics; using System.Runtime.CompilerServices; -using System.Text; namespace BepuPhysics { diff --git a/BepuPhysics/BoundingBoxHelpers.cs b/BepuPhysics/BoundingBoxHelpers.cs index 0f9473b56..dbf7ddfb2 100644 --- a/BepuPhysics/BoundingBoxHelpers.cs +++ b/BepuPhysics/BoundingBoxHelpers.cs @@ -1,10 +1,8 @@ using BepuPhysics.Collidables; using BepuUtilities; using System; -using System.Collections.Generic; using System.Numerics; using System.Runtime.CompilerServices; -using System.Text; namespace BepuPhysics { diff --git a/BepuPhysics/CollidableProperty.cs b/BepuPhysics/CollidableProperty.cs index bdffeef0a..ddbf35ef9 100644 --- a/BepuPhysics/CollidableProperty.cs +++ b/BepuPhysics/CollidableProperty.cs @@ -1,10 +1,8 @@ using BepuPhysics.Collidables; using BepuUtilities.Memory; using System; -using System.Collections.Generic; using System.Diagnostics; using System.Runtime.CompilerServices; -using System.Text; namespace BepuPhysics { diff --git a/BepuPhysics/Collidables/BigCompound.cs b/BepuPhysics/Collidables/BigCompound.cs index 8b2f2059f..97386aa0b 100644 --- a/BepuPhysics/Collidables/BigCompound.cs +++ b/BepuPhysics/Collidables/BigCompound.cs @@ -4,7 +4,6 @@ using BepuUtilities.Memory; using System.Diagnostics; using BepuUtilities; -using BepuUtilities.Collections; using BepuPhysics.Trees; using BepuPhysics.CollisionDetection.CollisionTasks; using System.Runtime.InteropServices; diff --git a/BepuPhysics/Collidables/BoundingBoxBatcher.cs b/BepuPhysics/Collidables/BoundingBoxBatcher.cs index f4998e6e5..2a839bb61 100644 --- a/BepuPhysics/Collidables/BoundingBoxBatcher.cs +++ b/BepuPhysics/Collidables/BoundingBoxBatcher.cs @@ -1,15 +1,11 @@ using BepuPhysics.Collidables; using BepuPhysics.CollisionDetection; -using BepuPhysics.Constraints; using BepuUtilities; -using BepuUtilities.Collections; using BepuUtilities.Memory; using System; -using System.Collections.Generic; using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; -using System.Text; namespace BepuPhysics { diff --git a/BepuPhysics/Collidables/Collidable.cs b/BepuPhysics/Collidables/Collidable.cs index 2a3d040a7..1d34a7f8c 100644 --- a/BepuPhysics/Collidables/Collidable.cs +++ b/BepuPhysics/Collidables/Collidable.cs @@ -1,5 +1,4 @@ using BepuPhysics.CollisionDetection; -using System.Runtime.CompilerServices; using System.Runtime.InteropServices; namespace BepuPhysics.Collidables diff --git a/BepuPhysics/Collidables/CompoundBuilder.cs b/BepuPhysics/Collidables/CompoundBuilder.cs index 30b010731..1e96c3885 100644 --- a/BepuPhysics/Collidables/CompoundBuilder.cs +++ b/BepuPhysics/Collidables/CompoundBuilder.cs @@ -2,11 +2,9 @@ using BepuUtilities.Collections; using BepuUtilities.Memory; using System; -using System.Collections.Generic; using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; -using System.Text; namespace BepuPhysics.Collidables { diff --git a/BepuPhysics/Collidables/ConvexHullHelper.cs b/BepuPhysics/Collidables/ConvexHullHelper.cs index 9b99af8f2..358c9e910 100644 --- a/BepuPhysics/Collidables/ConvexHullHelper.cs +++ b/BepuPhysics/Collidables/ConvexHullHelper.cs @@ -2,15 +2,10 @@ using BepuUtilities.Collections; using BepuUtilities.Memory; using System; -using System.Collections; -using System.Collections.Generic; -using System.Data; using System.Diagnostics; -using System.IO; using System.Numerics; using System.Runtime.CompilerServices; using System.Runtime.InteropServices; -using System.Text; namespace BepuPhysics.Collidables { diff --git a/BepuPhysics/Collidables/IShape.cs b/BepuPhysics/Collidables/IShape.cs index 39ea17741..42a5ca54a 100644 --- a/BepuPhysics/Collidables/IShape.cs +++ b/BepuPhysics/Collidables/IShape.cs @@ -1,10 +1,7 @@ -using BepuPhysics.CollisionDetection; -using BepuPhysics.CollisionDetection.CollisionTasks; +using BepuPhysics.CollisionDetection.CollisionTasks; using BepuPhysics.Trees; using BepuUtilities; -using BepuUtilities.Collections; using BepuUtilities.Memory; -using System; using System.Numerics; namespace BepuPhysics.Collidables diff --git a/BepuPhysics/Collidables/Mesh.cs b/BepuPhysics/Collidables/Mesh.cs index 097eff774..7be62e6f4 100644 --- a/BepuPhysics/Collidables/Mesh.cs +++ b/BepuPhysics/Collidables/Mesh.cs @@ -1,7 +1,6 @@ using BepuPhysics.CollisionDetection.CollisionTasks; using BepuPhysics.Trees; using BepuUtilities; -using BepuUtilities.Collections; using BepuUtilities.Memory; using System; using System.Diagnostics; diff --git a/BepuPhysics/Collidables/MeshInertiaHelper.cs b/BepuPhysics/Collidables/MeshInertiaHelper.cs index 0a8a17112..2e8ad40eb 100644 --- a/BepuPhysics/Collidables/MeshInertiaHelper.cs +++ b/BepuPhysics/Collidables/MeshInertiaHelper.cs @@ -1,9 +1,6 @@ using BepuUtilities; -using System; -using System.Collections.Generic; using System.Numerics; using System.Runtime.CompilerServices; -using System.Text; namespace BepuPhysics.Collidables { diff --git a/BepuPhysics/Collidables/Shapes.cs b/BepuPhysics/Collidables/Shapes.cs index e11bbe87e..5ff9cef38 100644 --- a/BepuPhysics/Collidables/Shapes.cs +++ b/BepuPhysics/Collidables/Shapes.cs @@ -1,5 +1,4 @@ -using BepuUtilities.Collections; -using BepuUtilities.Memory; +using BepuUtilities.Memory; using System.Numerics; using System.Runtime.CompilerServices; using System; diff --git a/BepuPhysics/Collidables/Sphere.cs b/BepuPhysics/Collidables/Sphere.cs index 43ca92419..c7dca6205 100644 --- a/BepuPhysics/Collidables/Sphere.cs +++ b/BepuPhysics/Collidables/Sphere.cs @@ -1,10 +1,7 @@ using System; -using System.Collections.Generic; using System.Numerics; using System.Runtime.CompilerServices; -using System.Text; using BepuUtilities.Memory; -using System.Diagnostics; using BepuUtilities; using BepuPhysics.Trees; using BepuPhysics.CollisionDetection; diff --git a/BepuPhysics/CollisionDetection/BroadPhase_Queries.cs b/BepuPhysics/CollisionDetection/BroadPhase_Queries.cs index f772751e7..c913418df 100644 --- a/BepuPhysics/CollisionDetection/BroadPhase_Queries.cs +++ b/BepuPhysics/CollisionDetection/BroadPhase_Queries.cs @@ -2,11 +2,8 @@ using BepuPhysics.Trees; using BepuUtilities; using BepuUtilities.Memory; -using System; -using System.Collections.Generic; using System.Numerics; using System.Runtime.CompilerServices; -using System.Text; namespace BepuPhysics.CollisionDetection { diff --git a/BepuPhysics/CollisionDetection/CollisionBatcherContinuations.cs b/BepuPhysics/CollisionDetection/CollisionBatcherContinuations.cs index acade2a18..bdb268f9f 100644 --- a/BepuPhysics/CollisionDetection/CollisionBatcherContinuations.cs +++ b/BepuPhysics/CollisionDetection/CollisionBatcherContinuations.cs @@ -1,9 +1,6 @@ using BepuUtilities.Memory; -using System; -using System.Collections.Generic; using System.Diagnostics; using System.Runtime.CompilerServices; -using System.Text; namespace BepuPhysics.CollisionDetection { diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/BoxConvexHullTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/BoxConvexHullTester.cs index 6a6e6f995..a3dba5c22 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/BoxConvexHullTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/BoxConvexHullTester.cs @@ -2,7 +2,6 @@ using BepuUtilities; using BepuUtilities.Memory; using System; -using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/BoxCylinderTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/BoxCylinderTester.cs index 86fcc0016..1be6fdd1d 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/BoxCylinderTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/BoxCylinderTester.cs @@ -1,8 +1,6 @@ using BepuPhysics.Collidables; -using BepuPhysics.CollisionDetection.SweepTasks; using BepuUtilities; using System; -using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/BoxPairTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/BoxPairTester.cs index f2e62348e..707df4191 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/BoxPairTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/BoxPairTester.cs @@ -4,7 +4,6 @@ using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; -using static BepuUtilities.GatherScatter; namespace BepuPhysics.CollisionDetection.CollisionTasks { diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleConvexHullTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleConvexHullTester.cs index 259cfa5c9..e14f675a3 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleConvexHullTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleConvexHullTester.cs @@ -1,7 +1,6 @@ using BepuPhysics.Collidables; using BepuUtilities; using System; -using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleCylinderTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleCylinderTester.cs index 97c57f88d..3f71551b5 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleCylinderTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleCylinderTester.cs @@ -1,5 +1,4 @@ using BepuPhysics.Collidables; -using BepuPhysics.CollisionDetection.SweepTasks; using BepuUtilities; using System; using System.Numerics; diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleTriangleTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleTriangleTester.cs index 1cb8a3cbe..c4606b634 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleTriangleTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CapsuleTriangleTester.cs @@ -1,7 +1,6 @@ using BepuPhysics.Collidables; using BepuUtilities; using System; -using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairCollisionTask.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairCollisionTask.cs index f799e1d80..247ccc1ef 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairCollisionTask.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairCollisionTask.cs @@ -1,10 +1,6 @@ using BepuPhysics.Collidables; -using BepuUtilities; -using BepuUtilities.Collections; using BepuUtilities.Memory; -using System; using System.Diagnostics; -using System.Numerics; using System.Runtime.CompilerServices; namespace BepuPhysics.CollisionDetection.CollisionTasks diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlapFinder.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlapFinder.cs index f896eaca8..c019cdb15 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlapFinder.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlapFinder.cs @@ -1,12 +1,9 @@ using BepuPhysics.Collidables; using BepuUtilities; using BepuUtilities.Memory; -using System; -using System.Collections.Generic; using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; -using System.Text; namespace BepuPhysics.CollisionDetection.CollisionTasks { diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlaps.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlaps.cs index 268d22f86..760864b75 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlaps.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlaps.cs @@ -1,12 +1,7 @@ using BepuUtilities; -using BepuUtilities.Collections; using BepuUtilities.Memory; -using System; -using System.Collections.Generic; using System.Diagnostics; -using System.Numerics; using System.Runtime.CompilerServices; -using System.Text; namespace BepuPhysics.CollisionDetection.CollisionTasks { diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCollisionTask.cs b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCollisionTask.cs index 480e6d83f..10dcef6b2 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCollisionTask.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCollisionTask.cs @@ -1,11 +1,9 @@ using BepuPhysics.Collidables; using BepuUtilities; using BepuUtilities.Memory; -using System; using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; -using System.Runtime.InteropServices; using static BepuUtilities.GatherScatter; namespace BepuPhysics.CollisionDetection.CollisionTasks diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundCollisionTask.cs b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundCollisionTask.cs index a29e79523..f3a5b4abe 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundCollisionTask.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundCollisionTask.cs @@ -1,10 +1,5 @@ using BepuPhysics.Collidables; -using BepuUtilities; -using BepuUtilities.Collections; -using BepuUtilities.Memory; -using System; using System.Diagnostics; -using System.Numerics; using System.Runtime.CompilerServices; namespace BepuPhysics.CollisionDetection.CollisionTasks diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexHullPairTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexHullPairTester.cs index ce9efde98..d07a8a1a6 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexHullPairTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexHullPairTester.cs @@ -1,7 +1,6 @@ using BepuPhysics.Collidables; using BepuUtilities; using System; -using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexHullTestHelper.cs b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexHullTestHelper.cs index 7e9f979ef..b6b1fc1a9 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexHullTestHelper.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexHullTestHelper.cs @@ -2,7 +2,6 @@ using BepuUtilities; using System.Diagnostics; using System.Numerics; -using System.Runtime.CompilerServices; namespace BepuPhysics.CollisionDetection.CollisionTasks { diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CylinderConvexHullTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CylinderConvexHullTester.cs index f17499157..7a04b74ac 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CylinderConvexHullTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CylinderConvexHullTester.cs @@ -1,7 +1,6 @@ using BepuPhysics.Collidables; using BepuUtilities; using System; -using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CylinderPairTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CylinderPairTester.cs index f51ce271e..26ca020a5 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CylinderPairTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CylinderPairTester.cs @@ -1,8 +1,6 @@ using BepuPhysics.Collidables; -using BepuPhysics.CollisionDetection.SweepTasks; using BepuUtilities; using System; -using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/ManifoldCandidateHelper.cs b/BepuPhysics/CollisionDetection/CollisionTasks/ManifoldCandidateHelper.cs index 6359c5f9e..0c6a6d25a 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/ManifoldCandidateHelper.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/ManifoldCandidateHelper.cs @@ -1,10 +1,6 @@ using BepuUtilities; -using System; -using System.Collections.Generic; -using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; -using System.Text; using static BepuUtilities.GatherScatter; namespace BepuPhysics.CollisionDetection.CollisionTasks diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/MeshPairOverlapFinder.cs b/BepuPhysics/CollisionDetection/CollisionTasks/MeshPairOverlapFinder.cs index f531da402..c95ad0dea 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/MeshPairOverlapFinder.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/MeshPairOverlapFinder.cs @@ -1,12 +1,9 @@ using BepuPhysics.Collidables; using BepuUtilities; using BepuUtilities.Memory; -using System; -using System.Collections.Generic; using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; -using System.Text; namespace BepuPhysics.CollisionDetection.CollisionTasks { diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/SphereConvexHullTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/SphereConvexHullTester.cs index aee7d7e12..c57317ade 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/SphereConvexHullTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/SphereConvexHullTester.cs @@ -2,7 +2,6 @@ using BepuUtilities; using System; using System.Numerics; -using System.Runtime.CompilerServices; namespace BepuPhysics.CollisionDetection.CollisionTasks { diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/TriangleConvexHullTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/TriangleConvexHullTester.cs index 4dee8044c..112c60d9d 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/TriangleConvexHullTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/TriangleConvexHullTester.cs @@ -2,7 +2,6 @@ using BepuUtilities; using BepuUtilities.Memory; using System; -using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/TriangleCylinderTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/TriangleCylinderTester.cs index cffe7055c..9a122957d 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/TriangleCylinderTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/TriangleCylinderTester.cs @@ -1,8 +1,6 @@ using BepuPhysics.Collidables; -using BepuPhysics.CollisionDetection.SweepTasks; using BepuUtilities; using System; -using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; diff --git a/BepuPhysics/CollisionDetection/CompoundMeshReduction.cs b/BepuPhysics/CollisionDetection/CompoundMeshReduction.cs index cc3769c85..d0a790cae 100644 --- a/BepuPhysics/CollisionDetection/CompoundMeshReduction.cs +++ b/BepuPhysics/CollisionDetection/CompoundMeshReduction.cs @@ -1,13 +1,9 @@ using BepuPhysics.Collidables; using BepuUtilities; -using BepuUtilities.Collections; using BepuUtilities.Memory; -using System; -using System.Collections.Generic; using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; -using System.Text; namespace BepuPhysics.CollisionDetection { diff --git a/BepuPhysics/CollisionDetection/ConstraintRemover.cs b/BepuPhysics/CollisionDetection/ConstraintRemover.cs index 44b2f6cff..c2af4fc78 100644 --- a/BepuPhysics/CollisionDetection/ConstraintRemover.cs +++ b/BepuPhysics/CollisionDetection/ConstraintRemover.cs @@ -2,11 +2,8 @@ using BepuUtilities.Collections; using BepuUtilities.Memory; using System; -using System.Collections.Generic; using System.Diagnostics; using System.Runtime.CompilerServices; -using System.Text; -using System.Threading; namespace BepuPhysics.CollisionDetection { diff --git a/BepuPhysics/CollisionDetection/ContactConstraintAccessor.cs b/BepuPhysics/CollisionDetection/ContactConstraintAccessor.cs index 4fbf4de46..16c0a438b 100644 --- a/BepuPhysics/CollisionDetection/ContactConstraintAccessor.cs +++ b/BepuPhysics/CollisionDetection/ContactConstraintAccessor.cs @@ -3,13 +3,9 @@ using BepuUtilities; using BepuUtilities.Collections; using BepuUtilities.Memory; -using System; -using System.Collections.Generic; using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; -using System.Runtime.InteropServices; -using System.Text; namespace BepuPhysics.CollisionDetection { diff --git a/BepuPhysics/CollisionDetection/ContactManifold.cs b/BepuPhysics/CollisionDetection/ContactManifold.cs index 6f99b938b..ca8099d04 100644 --- a/BepuPhysics/CollisionDetection/ContactManifold.cs +++ b/BepuPhysics/CollisionDetection/ContactManifold.cs @@ -1,5 +1,4 @@ -using System; -using System.Diagnostics; +using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; using System.Runtime.InteropServices; diff --git a/BepuPhysics/CollisionDetection/ConvexContactManifoldWide.cs b/BepuPhysics/CollisionDetection/ConvexContactManifoldWide.cs index 423470dcf..8315cfdea 100644 --- a/BepuPhysics/CollisionDetection/ConvexContactManifoldWide.cs +++ b/BepuPhysics/CollisionDetection/ConvexContactManifoldWide.cs @@ -1,6 +1,5 @@ using BepuPhysics.CollisionDetection.CollisionTasks; using BepuUtilities; -using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; diff --git a/BepuPhysics/CollisionDetection/DepthRefiner.cs b/BepuPhysics/CollisionDetection/DepthRefiner.cs index 155a9312a..782bc6ef2 100644 --- a/BepuPhysics/CollisionDetection/DepthRefiner.cs +++ b/BepuPhysics/CollisionDetection/DepthRefiner.cs @@ -2,14 +2,10 @@ //This file is automatically generated by a text template. If you want to make modifications, do so in the .tt file. //@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ using BepuPhysics.Collidables; -using BepuPhysics.CollisionDetection.SweepTasks; using BepuUtilities; -using System; -using System.Collections.Generic; using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; -using System.Text; namespace BepuPhysics.CollisionDetection { diff --git a/BepuPhysics/CollisionDetection/INarrowPhaseCallbacks.cs b/BepuPhysics/CollisionDetection/INarrowPhaseCallbacks.cs index 54d843c25..c26815a55 100644 --- a/BepuPhysics/CollisionDetection/INarrowPhaseCallbacks.cs +++ b/BepuPhysics/CollisionDetection/INarrowPhaseCallbacks.cs @@ -1,8 +1,4 @@ -using System; -using System.Collections.Generic; -using System.Text; -using BepuUtilities; -using BepuPhysics.Collidables; +using BepuPhysics.Collidables; using BepuPhysics.Constraints; namespace BepuPhysics.CollisionDetection diff --git a/BepuPhysics/CollisionDetection/ISupportFinder.cs b/BepuPhysics/CollisionDetection/ISupportFinder.cs index 931b2a53a..3968c023f 100644 --- a/BepuPhysics/CollisionDetection/ISupportFinder.cs +++ b/BepuPhysics/CollisionDetection/ISupportFinder.cs @@ -1,9 +1,6 @@ using BepuPhysics.Collidables; using BepuUtilities; -using System; -using System.Collections.Generic; using System.Numerics; -using System.Text; namespace BepuPhysics.CollisionDetection { public interface ISupportFinder where TShape : IConvexShape where TShapeWide : IShapeWide diff --git a/BepuPhysics/CollisionDetection/InactiveSetBuilder.cs b/BepuPhysics/CollisionDetection/InactiveSetBuilder.cs index dc1bd818d..a7baf0e3d 100644 --- a/BepuPhysics/CollisionDetection/InactiveSetBuilder.cs +++ b/BepuPhysics/CollisionDetection/InactiveSetBuilder.cs @@ -1,8 +1,5 @@ -using BepuPhysics.Collidables; -using BepuUtilities.Collections; +using BepuUtilities.Collections; using BepuUtilities.Memory; -using System.Diagnostics; -using System.Runtime.CompilerServices; namespace BepuPhysics.CollisionDetection { diff --git a/BepuPhysics/CollisionDetection/MeshReduction.cs b/BepuPhysics/CollisionDetection/MeshReduction.cs index cd9105360..e937ee3c7 100644 --- a/BepuPhysics/CollisionDetection/MeshReduction.cs +++ b/BepuPhysics/CollisionDetection/MeshReduction.cs @@ -1,14 +1,11 @@ using BepuPhysics.Collidables; -using BepuPhysics.Trees; using BepuUtilities; using BepuUtilities.Collections; using BepuUtilities.Memory; using System; -using System.Collections.Generic; using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; -using System.Text; namespace BepuPhysics.CollisionDetection { diff --git a/BepuPhysics/CollisionDetection/NarrowPhase.cs b/BepuPhysics/CollisionDetection/NarrowPhase.cs index c880b1ae6..aafe22969 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhase.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhase.cs @@ -1,11 +1,8 @@ using BepuUtilities.Collections; using BepuUtilities.Memory; using BepuPhysics.Collidables; -using System.Numerics; using System.Runtime.CompilerServices; -using System.Runtime.InteropServices; using System; -using BepuPhysics.Constraints; using System.Diagnostics; using System.Threading; using BepuUtilities; diff --git a/BepuPhysics/CollisionDetection/NarrowPhaseCCDContinuations.cs b/BepuPhysics/CollisionDetection/NarrowPhaseCCDContinuations.cs index f8bfb3a39..0396e06c8 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhaseCCDContinuations.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhaseCCDContinuations.cs @@ -1,15 +1,9 @@ using BepuUtilities; -using BepuUtilities.Collections; using BepuUtilities.Memory; -using BepuPhysics.Collidables; -using BepuPhysics.Constraints; using System; -using System.Collections.Generic; using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; -using System.Runtime.InteropServices; -using System.Text; namespace BepuPhysics.CollisionDetection { diff --git a/BepuPhysics/CollisionDetection/NarrowPhaseConstraintUpdate.cs b/BepuPhysics/CollisionDetection/NarrowPhaseConstraintUpdate.cs index 7d8a29a0e..ca6228805 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhaseConstraintUpdate.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhaseConstraintUpdate.cs @@ -1,13 +1,8 @@ -using BepuUtilities.Collections; -using BepuUtilities.Memory; -using System.Runtime.CompilerServices; +using System.Runtime.CompilerServices; using BepuPhysics.Constraints; using System.Diagnostics; -using System.Numerics; using BepuPhysics.Collidables; using System; -using BepuPhysics.Constraints.Contact; -using BepuUtilities; namespace BepuPhysics.CollisionDetection { diff --git a/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs b/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs index ee54a13eb..a36f95f56 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs @@ -1,16 +1,9 @@ using System; -using System.Collections.Generic; -using System.Text; -using BepuUtilities; -using BepuPhysics.Collidables; using BepuPhysics.Constraints; using System.Runtime.CompilerServices; using BepuUtilities.Memory; using System.Runtime.InteropServices; using System.Diagnostics; -using System.Threading; -using BepuUtilities.Collections; -using BepuPhysics.Constraints.Contact; namespace BepuPhysics.CollisionDetection { diff --git a/BepuPhysics/CollisionDetection/NarrowPhasePreflush.cs b/BepuPhysics/CollisionDetection/NarrowPhasePreflush.cs index 3a88493a2..77679786e 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhasePreflush.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhasePreflush.cs @@ -2,11 +2,9 @@ using BepuUtilities.Collections; using BepuUtilities.Memory; using System; -using System.Collections.Generic; using System.Diagnostics; using System.Runtime.CompilerServices; using System.Runtime.InteropServices; -using System.Text; using System.Threading; namespace BepuPhysics.CollisionDetection diff --git a/BepuPhysics/CollisionDetection/NonconvexReduction.cs b/BepuPhysics/CollisionDetection/NonconvexReduction.cs index b5e677224..6af82fb79 100644 --- a/BepuPhysics/CollisionDetection/NonconvexReduction.cs +++ b/BepuPhysics/CollisionDetection/NonconvexReduction.cs @@ -1,13 +1,9 @@ -using BepuPhysics.CollisionDetection.CollisionTasks; -using BepuUtilities; -using BepuUtilities.Collections; +using BepuUtilities.Collections; using BepuUtilities.Memory; using System; -using System.Collections.Generic; using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; -using System.Text; namespace BepuPhysics.CollisionDetection { diff --git a/BepuPhysics/CollisionDetection/PairCache_Activity.cs b/BepuPhysics/CollisionDetection/PairCache_Activity.cs index 932382f50..44bc2f5e7 100644 --- a/BepuPhysics/CollisionDetection/PairCache_Activity.cs +++ b/BepuPhysics/CollisionDetection/PairCache_Activity.cs @@ -1,9 +1,5 @@ -using BepuPhysics.Collidables; -using BepuUtilities.Collections; -using BepuUtilities.Memory; -using System; +using BepuUtilities.Memory; using System.Diagnostics; -using System.Runtime.CompilerServices; namespace BepuPhysics.CollisionDetection { diff --git a/BepuPhysics/CollisionDetection/SweepTasks/CapsuleBoxDistanceTester.cs b/BepuPhysics/CollisionDetection/SweepTasks/CapsuleBoxDistanceTester.cs index f7f3db4d0..11d9a364d 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/CapsuleBoxDistanceTester.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/CapsuleBoxDistanceTester.cs @@ -1,7 +1,5 @@ using BepuPhysics.Collidables; -using BepuPhysics.CollisionDetection.CollisionTasks; using BepuUtilities; -using System; using System.Numerics; using System.Runtime.CompilerServices; diff --git a/BepuPhysics/CollisionDetection/SweepTasks/ConvexCompoundSweepOverlapFinder.cs b/BepuPhysics/CollisionDetection/SweepTasks/ConvexCompoundSweepOverlapFinder.cs index f70e279e7..c5ce21461 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/ConvexCompoundSweepOverlapFinder.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/ConvexCompoundSweepOverlapFinder.cs @@ -1,9 +1,6 @@ using BepuPhysics.Collidables; using BepuPhysics.CollisionDetection.CollisionTasks; -using BepuUtilities; -using BepuUtilities.Collections; using BepuUtilities.Memory; -using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; diff --git a/BepuPhysics/CollisionDetection/SweepTasks/ConvexHomogeneousCompoundSweepTask.cs b/BepuPhysics/CollisionDetection/SweepTasks/ConvexHomogeneousCompoundSweepTask.cs index 3912703ee..d64b9ef7b 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/ConvexHomogeneousCompoundSweepTask.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/ConvexHomogeneousCompoundSweepTask.cs @@ -2,7 +2,6 @@ using System.Numerics; using System.Runtime.CompilerServices; using BepuPhysics.Collidables; -using BepuUtilities; using BepuUtilities.Memory; namespace BepuPhysics.CollisionDetection.SweepTasks diff --git a/BepuPhysics/CollisionDetection/SweepTasks/GJKDistanceTester.cs b/BepuPhysics/CollisionDetection/SweepTasks/GJKDistanceTester.cs index 587b77289..aea86a1cc 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/GJKDistanceTester.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/GJKDistanceTester.cs @@ -1,7 +1,5 @@ using BepuPhysics.Collidables; using BepuUtilities; -using System; -using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; diff --git a/BepuPhysics/CollisionDetection/WideRayTester.cs b/BepuPhysics/CollisionDetection/WideRayTester.cs index c6640f5eb..b244f1842 100644 --- a/BepuPhysics/CollisionDetection/WideRayTester.cs +++ b/BepuPhysics/CollisionDetection/WideRayTester.cs @@ -2,7 +2,6 @@ using BepuPhysics.Trees; using BepuUtilities; using BepuUtilities.Memory; -using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; diff --git a/BepuPhysics/CollisionDetection/WorkerPairCache.cs b/BepuPhysics/CollisionDetection/WorkerPairCache.cs index e262dcbaa..8fed911b9 100644 --- a/BepuPhysics/CollisionDetection/WorkerPairCache.cs +++ b/BepuPhysics/CollisionDetection/WorkerPairCache.cs @@ -1,7 +1,5 @@ using BepuUtilities.Collections; using BepuUtilities.Memory; -using System; -using System.Diagnostics; using System.Runtime.CompilerServices; namespace BepuPhysics.CollisionDetection diff --git a/BepuPhysics/Constraints/AngularAxisGearMotor.cs b/BepuPhysics/Constraints/AngularAxisGearMotor.cs index c448bf5fb..7763cd984 100644 --- a/BepuPhysics/Constraints/AngularAxisGearMotor.cs +++ b/BepuPhysics/Constraints/AngularAxisGearMotor.cs @@ -1,5 +1,4 @@ -using BepuPhysics.CollisionDetection; -using BepuUtilities; +using BepuUtilities; using BepuUtilities.Memory; using System; using System.Diagnostics; diff --git a/BepuPhysics/Constraints/AngularAxisMotor.cs b/BepuPhysics/Constraints/AngularAxisMotor.cs index aeb32f1da..2f1a91f3d 100644 --- a/BepuPhysics/Constraints/AngularAxisMotor.cs +++ b/BepuPhysics/Constraints/AngularAxisMotor.cs @@ -1,5 +1,4 @@ -using BepuPhysics.CollisionDetection; -using BepuUtilities; +using BepuUtilities; using BepuUtilities.Memory; using System; using System.Diagnostics; diff --git a/BepuPhysics/Constraints/AngularHinge.cs b/BepuPhysics/Constraints/AngularHinge.cs index dc35fa456..a7188d7b9 100644 --- a/BepuPhysics/Constraints/AngularHinge.cs +++ b/BepuPhysics/Constraints/AngularHinge.cs @@ -1,5 +1,4 @@ -using BepuPhysics.CollisionDetection; -using BepuUtilities; +using BepuUtilities; using BepuUtilities.Memory; using System; using System.Diagnostics; diff --git a/BepuPhysics/Constraints/AngularSwivelHinge.cs b/BepuPhysics/Constraints/AngularSwivelHinge.cs index 865dc1eef..bab7a8eec 100644 --- a/BepuPhysics/Constraints/AngularSwivelHinge.cs +++ b/BepuPhysics/Constraints/AngularSwivelHinge.cs @@ -1,5 +1,4 @@ -using BepuPhysics.CollisionDetection; -using BepuUtilities; +using BepuUtilities; using BepuUtilities.Memory; using System; using System.Diagnostics; diff --git a/BepuPhysics/Constraints/AreaConstraint.cs b/BepuPhysics/Constraints/AreaConstraint.cs index 7c7bb0963..9cb68c1c1 100644 --- a/BepuPhysics/Constraints/AreaConstraint.cs +++ b/BepuPhysics/Constraints/AreaConstraint.cs @@ -1,5 +1,4 @@ -using BepuPhysics.CollisionDetection; -using BepuUtilities; +using BepuUtilities; using BepuUtilities.Memory; using System; using System.Diagnostics; diff --git a/BepuPhysics/Constraints/BallSocket.cs b/BepuPhysics/Constraints/BallSocket.cs index 39980cb19..2f6778579 100644 --- a/BepuPhysics/Constraints/BallSocket.cs +++ b/BepuPhysics/Constraints/BallSocket.cs @@ -1,5 +1,4 @@ -using BepuPhysics.CollisionDetection; -using BepuUtilities; +using BepuUtilities; using BepuUtilities.Memory; using System; using System.Diagnostics; diff --git a/BepuPhysics/Constraints/BallSocketMotor.cs b/BepuPhysics/Constraints/BallSocketMotor.cs index a4c4b37bc..41988ce0e 100644 --- a/BepuPhysics/Constraints/BallSocketMotor.cs +++ b/BepuPhysics/Constraints/BallSocketMotor.cs @@ -1,7 +1,4 @@ -using BepuPhysics; -using BepuPhysics.CollisionDetection; -using BepuPhysics.Constraints; -using BepuUtilities; +using BepuUtilities; using BepuUtilities.Memory; using System; using System.Diagnostics; diff --git a/BepuPhysics/Constraints/BallSocketServo.cs b/BepuPhysics/Constraints/BallSocketServo.cs index 1b6623c71..1d45ef85d 100644 --- a/BepuPhysics/Constraints/BallSocketServo.cs +++ b/BepuPhysics/Constraints/BallSocketServo.cs @@ -1,5 +1,4 @@ -using BepuPhysics.CollisionDetection; -using BepuUtilities; +using BepuUtilities; using BepuUtilities.Memory; using System; using System.Diagnostics; diff --git a/BepuPhysics/Constraints/CenterDistanceConstraint.cs b/BepuPhysics/Constraints/CenterDistanceConstraint.cs index 8de3eca90..660ff4048 100644 --- a/BepuPhysics/Constraints/CenterDistanceConstraint.cs +++ b/BepuPhysics/Constraints/CenterDistanceConstraint.cs @@ -1,5 +1,4 @@ -using BepuPhysics.CollisionDetection; -using BepuUtilities; +using BepuUtilities; using BepuUtilities.Memory; using System; using System.Diagnostics; diff --git a/BepuPhysics/Constraints/CenterDistanceLimit.cs b/BepuPhysics/Constraints/CenterDistanceLimit.cs index c373d02df..3ecc5d874 100644 --- a/BepuPhysics/Constraints/CenterDistanceLimit.cs +++ b/BepuPhysics/Constraints/CenterDistanceLimit.cs @@ -1,5 +1,4 @@ -using BepuPhysics.CollisionDetection; -using BepuUtilities; +using BepuUtilities; using BepuUtilities.Memory; using System; using System.Diagnostics; diff --git a/BepuPhysics/Constraints/ConstraintChecker.cs b/BepuPhysics/Constraints/ConstraintChecker.cs index 420247005..2e720b8d4 100644 --- a/BepuPhysics/Constraints/ConstraintChecker.cs +++ b/BepuPhysics/Constraints/ConstraintChecker.cs @@ -1,9 +1,6 @@ -using System; -using System.Collections.Generic; -using System.Diagnostics; +using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; -using System.Text; namespace BepuPhysics.Constraints { diff --git a/BepuPhysics/Constraints/Contact/ContactConvexCommon.cs b/BepuPhysics/Constraints/Contact/ContactConvexCommon.cs index 21da942ef..5c3bfdda7 100644 --- a/BepuPhysics/Constraints/Contact/ContactConvexCommon.cs +++ b/BepuPhysics/Constraints/Contact/ContactConvexCommon.cs @@ -1,9 +1,5 @@ -using BepuPhysics.CollisionDetection; -using BepuUtilities; -using System; -using System.Collections.Generic; +using BepuUtilities; using System.Numerics; -using System.Text; namespace BepuPhysics.Constraints.Contact { diff --git a/BepuPhysics/Constraints/Contact/IContactConstraintDescription.cs b/BepuPhysics/Constraints/Contact/IContactConstraintDescription.cs index d432e6ad3..681ba84bd 100644 --- a/BepuPhysics/Constraints/Contact/IContactConstraintDescription.cs +++ b/BepuPhysics/Constraints/Contact/IContactConstraintDescription.cs @@ -1,8 +1,5 @@ using BepuPhysics.CollisionDetection; -using System; -using System.Collections.Generic; using System.Numerics; -using System.Text; namespace BepuPhysics.Constraints.Contact { diff --git a/BepuPhysics/Constraints/Contact/PenetrationLimit.cs b/BepuPhysics/Constraints/Contact/PenetrationLimit.cs index 04908adc6..9b9a09151 100644 --- a/BepuPhysics/Constraints/Contact/PenetrationLimit.cs +++ b/BepuPhysics/Constraints/Contact/PenetrationLimit.cs @@ -1,8 +1,6 @@ using BepuUtilities; -using System; using System.Numerics; using System.Runtime.CompilerServices; -using System.Runtime.InteropServices; namespace BepuPhysics.Constraints.Contact { diff --git a/BepuPhysics/Constraints/Contact/PenetrationLimitOneBody.cs b/BepuPhysics/Constraints/Contact/PenetrationLimitOneBody.cs index 8a3ffe34d..205d6f5a9 100644 --- a/BepuPhysics/Constraints/Contact/PenetrationLimitOneBody.cs +++ b/BepuPhysics/Constraints/Contact/PenetrationLimitOneBody.cs @@ -1,8 +1,6 @@ using BepuUtilities; -using System; using System.Numerics; using System.Runtime.CompilerServices; -using System.Runtime.InteropServices; namespace BepuPhysics.Constraints.Contact { diff --git a/BepuPhysics/Constraints/Contact/TangentFriction.cs b/BepuPhysics/Constraints/Contact/TangentFriction.cs index 544a30a04..f8ffe9774 100644 --- a/BepuPhysics/Constraints/Contact/TangentFriction.cs +++ b/BepuPhysics/Constraints/Contact/TangentFriction.cs @@ -1,5 +1,4 @@ using BepuUtilities; -using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; diff --git a/BepuPhysics/Constraints/Contact/TwistFriction.cs b/BepuPhysics/Constraints/Contact/TwistFriction.cs index b26605251..5cd7ced4d 100644 --- a/BepuPhysics/Constraints/Contact/TwistFriction.cs +++ b/BepuPhysics/Constraints/Contact/TwistFriction.cs @@ -1,5 +1,4 @@ using BepuUtilities; -using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; diff --git a/BepuPhysics/Constraints/DistanceLimit.cs b/BepuPhysics/Constraints/DistanceLimit.cs index 2963bd911..bf153d3c9 100644 --- a/BepuPhysics/Constraints/DistanceLimit.cs +++ b/BepuPhysics/Constraints/DistanceLimit.cs @@ -1,5 +1,4 @@ -using BepuPhysics.CollisionDetection; -using BepuUtilities; +using BepuUtilities; using BepuUtilities.Memory; using System; using System.Diagnostics; diff --git a/BepuPhysics/Constraints/DistanceServo.cs b/BepuPhysics/Constraints/DistanceServo.cs index d4322db3f..7497dd384 100644 --- a/BepuPhysics/Constraints/DistanceServo.cs +++ b/BepuPhysics/Constraints/DistanceServo.cs @@ -1,5 +1,4 @@ -using BepuPhysics.CollisionDetection; -using BepuUtilities; +using BepuUtilities; using BepuUtilities.Memory; using System; using System.Diagnostics; diff --git a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs index 6f2b4d0af..801829f75 100644 --- a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs @@ -1,8 +1,6 @@ using BepuUtilities; using BepuUtilities.Collections; using BepuUtilities.Memory; -using System; -using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; diff --git a/BepuPhysics/Constraints/Hinge.cs b/BepuPhysics/Constraints/Hinge.cs index 46e9f092a..287c35b88 100644 --- a/BepuPhysics/Constraints/Hinge.cs +++ b/BepuPhysics/Constraints/Hinge.cs @@ -1,5 +1,4 @@ -using BepuPhysics.CollisionDetection; -using BepuUtilities; +using BepuUtilities; using BepuUtilities.Memory; using System; using System.Diagnostics; diff --git a/BepuPhysics/Constraints/IConstraintDescription.cs b/BepuPhysics/Constraints/IConstraintDescription.cs index ca53c4d74..66cdd4998 100644 --- a/BepuPhysics/Constraints/IConstraintDescription.cs +++ b/BepuPhysics/Constraints/IConstraintDescription.cs @@ -1,8 +1,4 @@ using System; -using System.Collections.Generic; -using System.Linq; -using System.Text; -using System.Threading.Tasks; namespace BepuPhysics.Constraints { diff --git a/BepuPhysics/Constraints/Inequality1DOF.cs b/BepuPhysics/Constraints/Inequality1DOF.cs index 2fd322a21..e8475acca 100644 --- a/BepuPhysics/Constraints/Inequality1DOF.cs +++ b/BepuPhysics/Constraints/Inequality1DOF.cs @@ -1,8 +1,6 @@ using BepuUtilities; -using System; using System.Numerics; using System.Runtime.CompilerServices; -using System.Runtime.InteropServices; namespace BepuPhysics.Constraints { diff --git a/BepuPhysics/Constraints/InequalityHelpers.cs b/BepuPhysics/Constraints/InequalityHelpers.cs index ec4d5e484..d2ab4fb3b 100644 --- a/BepuPhysics/Constraints/InequalityHelpers.cs +++ b/BepuPhysics/Constraints/InequalityHelpers.cs @@ -1,8 +1,5 @@ -using System; -using System.Collections.Generic; -using System.Numerics; +using System.Numerics; using System.Runtime.CompilerServices; -using System.Text; namespace BepuPhysics.Constraints { diff --git a/BepuPhysics/Constraints/LinearAxisLimit.cs b/BepuPhysics/Constraints/LinearAxisLimit.cs index c8ed35313..5a81117fa 100644 --- a/BepuPhysics/Constraints/LinearAxisLimit.cs +++ b/BepuPhysics/Constraints/LinearAxisLimit.cs @@ -1,5 +1,4 @@ -using BepuPhysics.CollisionDetection; -using BepuUtilities; +using BepuUtilities; using BepuUtilities.Memory; using System; using System.Diagnostics; diff --git a/BepuPhysics/Constraints/LinearAxisMotor.cs b/BepuPhysics/Constraints/LinearAxisMotor.cs index 24165e3e1..493b4d510 100644 --- a/BepuPhysics/Constraints/LinearAxisMotor.cs +++ b/BepuPhysics/Constraints/LinearAxisMotor.cs @@ -1,5 +1,4 @@ -using BepuPhysics.CollisionDetection; -using BepuUtilities; +using BepuUtilities; using BepuUtilities.Memory; using System; using System.Diagnostics; diff --git a/BepuPhysics/Constraints/LinearAxisServo.cs b/BepuPhysics/Constraints/LinearAxisServo.cs index d31efe2ff..6b600f0f9 100644 --- a/BepuPhysics/Constraints/LinearAxisServo.cs +++ b/BepuPhysics/Constraints/LinearAxisServo.cs @@ -1,5 +1,4 @@ -using BepuPhysics.CollisionDetection; -using BepuUtilities; +using BepuUtilities; using BepuUtilities.Memory; using System; using System.Diagnostics; diff --git a/BepuPhysics/Constraints/MotorSettings.cs b/BepuPhysics/Constraints/MotorSettings.cs index 07079df2a..557bb782f 100644 --- a/BepuPhysics/Constraints/MotorSettings.cs +++ b/BepuPhysics/Constraints/MotorSettings.cs @@ -1,10 +1,7 @@ using BepuUtilities; -using System; -using System.Collections.Generic; using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; -using System.Text; namespace BepuPhysics.Constraints { diff --git a/BepuPhysics/Constraints/OneBodyLinearMotor.cs b/BepuPhysics/Constraints/OneBodyLinearMotor.cs index 5d70235f9..b2e305603 100644 --- a/BepuPhysics/Constraints/OneBodyLinearMotor.cs +++ b/BepuPhysics/Constraints/OneBodyLinearMotor.cs @@ -1,5 +1,4 @@ -using BepuPhysics.CollisionDetection; -using BepuUtilities; +using BepuUtilities; using BepuUtilities.Memory; using System; using System.Diagnostics; diff --git a/BepuPhysics/Constraints/OneBodyLinearServo.cs b/BepuPhysics/Constraints/OneBodyLinearServo.cs index 955d1cff7..5506ab084 100644 --- a/BepuPhysics/Constraints/OneBodyLinearServo.cs +++ b/BepuPhysics/Constraints/OneBodyLinearServo.cs @@ -1,5 +1,4 @@ -using BepuPhysics.CollisionDetection; -using BepuUtilities; +using BepuUtilities; using BepuUtilities.Memory; using System; using System.Diagnostics; diff --git a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs index b07af57b2..c9325f139 100644 --- a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs @@ -1,8 +1,6 @@ using BepuUtilities; using BepuUtilities.Collections; using BepuUtilities.Memory; -using System; -using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; diff --git a/BepuPhysics/Constraints/PointOnLineServo.cs b/BepuPhysics/Constraints/PointOnLineServo.cs index ac4c6f198..9d2bcfabd 100644 --- a/BepuPhysics/Constraints/PointOnLineServo.cs +++ b/BepuPhysics/Constraints/PointOnLineServo.cs @@ -1,5 +1,4 @@ -using BepuPhysics.CollisionDetection; -using BepuUtilities; +using BepuUtilities; using BepuUtilities.Memory; using System; using System.Diagnostics; diff --git a/BepuPhysics/Constraints/ServoSettings.cs b/BepuPhysics/Constraints/ServoSettings.cs index f485298d5..e1ee3b2c6 100644 --- a/BepuPhysics/Constraints/ServoSettings.cs +++ b/BepuPhysics/Constraints/ServoSettings.cs @@ -1,10 +1,7 @@ using BepuUtilities; -using System; -using System.Collections.Generic; using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; -using System.Text; namespace BepuPhysics.Constraints { diff --git a/BepuPhysics/Constraints/SwingLimit.cs b/BepuPhysics/Constraints/SwingLimit.cs index 8ec6fca43..5aad1dfee 100644 --- a/BepuPhysics/Constraints/SwingLimit.cs +++ b/BepuPhysics/Constraints/SwingLimit.cs @@ -1,5 +1,4 @@ -using BepuPhysics.CollisionDetection; -using BepuUtilities; +using BepuUtilities; using BepuUtilities.Memory; using System; using System.Diagnostics; diff --git a/BepuPhysics/Constraints/SwivelHinge.cs b/BepuPhysics/Constraints/SwivelHinge.cs index 25b51bb59..22616a824 100644 --- a/BepuPhysics/Constraints/SwivelHinge.cs +++ b/BepuPhysics/Constraints/SwivelHinge.cs @@ -1,5 +1,4 @@ -using BepuPhysics.CollisionDetection; -using BepuUtilities; +using BepuUtilities; using BepuUtilities.Memory; using System; using System.Diagnostics; diff --git a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs index 0980e2a91..aa243487f 100644 --- a/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/ThreeBodyTypeProcessor.cs @@ -1,8 +1,6 @@ using BepuUtilities; using BepuUtilities.Collections; using BepuUtilities.Memory; -using System; -using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index 55e208cca..beb6511ad 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -1,13 +1,8 @@ using BepuUtilities; using BepuUtilities.Collections; using BepuUtilities.Memory; -using System; -using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; -using System.Runtime.Intrinsics; -using System.Runtime.Intrinsics.X86; -using System.Runtime.Intrinsics.Arm; namespace BepuPhysics.Constraints { diff --git a/BepuPhysics/Constraints/TypeBatch.cs b/BepuPhysics/Constraints/TypeBatch.cs index 7845a78df..7c2870518 100644 --- a/BepuPhysics/Constraints/TypeBatch.cs +++ b/BepuPhysics/Constraints/TypeBatch.cs @@ -1,10 +1,6 @@ using BepuUtilities; -using BepuUtilities.Collections; using BepuUtilities.Memory; -using System; -using System.Collections.Generic; using System.Runtime.CompilerServices; -using System.Text; namespace BepuPhysics.Constraints { diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index df1170f1d..94381cdd6 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -6,7 +6,6 @@ using BepuUtilities.Collections; using BepuUtilities; using System.Runtime.Intrinsics; -using System.Runtime.Intrinsics.X86; namespace BepuPhysics.Constraints { diff --git a/BepuPhysics/Constraints/VolumeConstraint.cs b/BepuPhysics/Constraints/VolumeConstraint.cs index e07695b1b..0ff92bd64 100644 --- a/BepuPhysics/Constraints/VolumeConstraint.cs +++ b/BepuPhysics/Constraints/VolumeConstraint.cs @@ -1,5 +1,4 @@ -using BepuPhysics.CollisionDetection; -using BepuUtilities; +using BepuUtilities; using BepuUtilities.Memory; using System; using System.Diagnostics; diff --git a/BepuPhysics/Constraints/Weld.cs b/BepuPhysics/Constraints/Weld.cs index 925bc212e..e5bec5aba 100644 --- a/BepuPhysics/Constraints/Weld.cs +++ b/BepuPhysics/Constraints/Weld.cs @@ -1,5 +1,4 @@ -using BepuPhysics.CollisionDetection; -using BepuUtilities; +using BepuUtilities; using BepuUtilities.Memory; using System; using System.Diagnostics; diff --git a/BepuPhysics/DefaultTimestepper.cs b/BepuPhysics/DefaultTimestepper.cs index 264571bc6..456e6974c 100644 --- a/BepuPhysics/DefaultTimestepper.cs +++ b/BepuPhysics/DefaultTimestepper.cs @@ -1,8 +1,4 @@ -using System; -using System.Collections.Generic; -using System.Diagnostics; -using System.Text; -using BepuUtilities; +using BepuUtilities; namespace BepuPhysics { diff --git a/BepuPhysics/HandyEnumerators.cs b/BepuPhysics/HandyEnumerators.cs index 72a71fa1d..ee4468469 100644 --- a/BepuPhysics/HandyEnumerators.cs +++ b/BepuPhysics/HandyEnumerators.cs @@ -1,10 +1,4 @@ -using BepuPhysics.Constraints; -using BepuUtilities; -using System; -using System.Collections.Generic; -using System.Linq; -using System.Text; -using System.Threading.Tasks; +using BepuUtilities; namespace BepuPhysics { diff --git a/BepuPhysics/ITimestepper.cs b/BepuPhysics/ITimestepper.cs index d37f49edb..0c1d12627 100644 --- a/BepuPhysics/ITimestepper.cs +++ b/BepuPhysics/ITimestepper.cs @@ -1,7 +1,4 @@ using BepuUtilities; -using System; -using System.Collections.Generic; -using System.Text; namespace BepuPhysics { diff --git a/BepuPhysics/InvasiveHashDiagnostics.cs b/BepuPhysics/InvasiveHashDiagnostics.cs index fd8497156..2fe7961c9 100644 --- a/BepuPhysics/InvasiveHashDiagnostics.cs +++ b/BepuPhysics/InvasiveHashDiagnostics.cs @@ -1,10 +1,6 @@ using BepuUtilities.Collections; using System; -using System.Collections.Generic; -using System.Linq; using System.Runtime.CompilerServices; -using System.Text; -using System.Threading.Tasks; namespace BepuPhysics { diff --git a/BepuPhysics/IslandAwakener.cs b/BepuPhysics/IslandAwakener.cs index a71c7dcf1..531038df5 100644 --- a/BepuPhysics/IslandAwakener.cs +++ b/BepuPhysics/IslandAwakener.cs @@ -3,12 +3,10 @@ using BepuUtilities.Collections; using BepuUtilities.Memory; using System; -using System.Collections.Generic; using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; using System.Runtime.InteropServices; -using System.Text; using System.Threading; namespace BepuPhysics diff --git a/BepuPhysics/IslandSleeper.cs b/BepuPhysics/IslandSleeper.cs index 824d1bc7c..7154131c9 100644 --- a/BepuPhysics/IslandSleeper.cs +++ b/BepuPhysics/IslandSleeper.cs @@ -1,12 +1,10 @@ using BepuPhysics.Collidables; using BepuPhysics.CollisionDetection; -using BepuPhysics.Constraints; using BepuUtilities; using BepuUtilities.Collections; using BepuUtilities.Memory; using System; using System.Diagnostics; -using System.Numerics; using System.Runtime.CompilerServices; using System.Threading; diff --git a/BepuPhysics/PoseIntegrator.cs b/BepuPhysics/PoseIntegrator.cs index 0fde1a8a3..4e9b84cd5 100644 --- a/BepuPhysics/PoseIntegrator.cs +++ b/BepuPhysics/PoseIntegrator.cs @@ -3,7 +3,6 @@ using BepuPhysics.Collidables; using BepuPhysics.CollisionDetection; using System; -using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; using System.Threading; diff --git a/BepuPhysics/SequentialFallbackBatch.cs b/BepuPhysics/SequentialFallbackBatch.cs index df42d7b58..b2b923145 100644 --- a/BepuPhysics/SequentialFallbackBatch.cs +++ b/BepuPhysics/SequentialFallbackBatch.cs @@ -1,13 +1,8 @@ -using BepuPhysics.Constraints; -using BepuUtilities; -using BepuUtilities.Collections; +using BepuUtilities.Collections; using BepuUtilities.Memory; using System; -using System.Collections.Generic; using System.Diagnostics; -using System.Numerics; using System.Runtime.CompilerServices; -using System.Text; namespace BepuPhysics { diff --git a/BepuPhysics/Simulation.cs b/BepuPhysics/Simulation.cs index 981ffc5c3..1fb22f5ab 100644 --- a/BepuPhysics/Simulation.cs +++ b/BepuPhysics/Simulation.cs @@ -1,12 +1,9 @@ using BepuUtilities; -using BepuUtilities.Collections; using BepuUtilities.Memory; using BepuPhysics.Collidables; using BepuPhysics.CollisionDetection; -using BepuPhysics.Constraints; using System; using System.Diagnostics; -using System.Runtime.CompilerServices; using BepuPhysics.Trees; #if !DEBUG diff --git a/BepuPhysics/SimulationProfiler.cs b/BepuPhysics/SimulationProfiler.cs index 010dd1e30..c8a435c70 100644 --- a/BepuPhysics/SimulationProfiler.cs +++ b/BepuPhysics/SimulationProfiler.cs @@ -1,9 +1,5 @@ -using BepuUtilities.Collections; -using BepuUtilities.Memory; -using System; -using System.Collections.Generic; +using System.Collections.Generic; using System.Diagnostics; -using System.Text; namespace BepuPhysics { diff --git a/BepuPhysics/Simulation_Queries.cs b/BepuPhysics/Simulation_Queries.cs index a14b7717f..12cfc3ef4 100644 --- a/BepuPhysics/Simulation_Queries.cs +++ b/BepuPhysics/Simulation_Queries.cs @@ -3,10 +3,8 @@ using BepuPhysics.Trees; using BepuUtilities.Memory; using System; -using System.Collections.Generic; using System.Numerics; using System.Runtime.CompilerServices; -using System.Text; namespace BepuPhysics { diff --git a/BepuPhysics/Solver_Solve.cs b/BepuPhysics/Solver_Solve.cs index 08d0d0d94..196ae97f7 100644 --- a/BepuPhysics/Solver_Solve.cs +++ b/BepuPhysics/Solver_Solve.cs @@ -3,17 +3,13 @@ using BepuUtilities.Memory; using BepuPhysics.Constraints; using System; -using System.Collections.Generic; using System.Diagnostics; using System.Runtime.CompilerServices; using System.Runtime.InteropServices; -using System.Text; using System.Threading; using System.Runtime.Intrinsics.X86; using System.Numerics; using System.Runtime.Intrinsics; -using static BepuPhysics.Solver; -using BepuPhysics.CollisionDetection; namespace BepuPhysics { diff --git a/BepuPhysics/Statics.cs b/BepuPhysics/Statics.cs index 6db40b02d..7050f771e 100644 --- a/BepuPhysics/Statics.cs +++ b/BepuPhysics/Statics.cs @@ -2,9 +2,7 @@ using BepuUtilities.Memory; using System; using System.Diagnostics; -using System.Numerics; using System.Runtime.CompilerServices; -using BepuPhysics.Constraints; using BepuPhysics.Collidables; using BepuUtilities.Collections; using BepuPhysics.CollisionDetection; diff --git a/BepuPhysics/Trees/RayBatcher.cs b/BepuPhysics/Trees/RayBatcher.cs index 48940d975..f909c83ab 100644 --- a/BepuPhysics/Trees/RayBatcher.cs +++ b/BepuPhysics/Trees/RayBatcher.cs @@ -1,11 +1,9 @@ using BepuUtilities; using BepuUtilities.Memory; using System; -using System.Collections.Generic; using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; -using System.Text; namespace BepuPhysics.Trees { diff --git a/BepuPhysics/Trees/Tree_Add.cs b/BepuPhysics/Trees/Tree_Add.cs index dee7fb22a..8c236847c 100644 --- a/BepuPhysics/Trees/Tree_Add.cs +++ b/BepuPhysics/Trees/Tree_Add.cs @@ -1,11 +1,7 @@ using BepuUtilities; using System.Numerics; using System.Runtime.CompilerServices; -using System; -using System.Diagnostics; using BepuUtilities.Memory; -using BepuUtilities.Collections; -using System.Collections.Generic; namespace BepuPhysics.Trees; diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index f3ca26b69..71d170f7f 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -3,7 +3,6 @@ using BepuUtilities.Memory; using BepuUtilities.TaskScheduling; using System; -using System.Collections.Generic; using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; diff --git a/BepuPhysics/Trees/Tree_BinnedRefine.cs b/BepuPhysics/Trees/Tree_BinnedRefine.cs index b239a2143..e3ec65917 100644 --- a/BepuPhysics/Trees/Tree_BinnedRefine.cs +++ b/BepuPhysics/Trees/Tree_BinnedRefine.cs @@ -3,7 +3,6 @@ using BepuUtilities.Memory; using System; using System.Diagnostics; -using System.Linq; using System.Numerics; using System.Runtime.CompilerServices; diff --git a/BepuPhysics/Trees/Tree_Diagnostics.cs b/BepuPhysics/Trees/Tree_Diagnostics.cs index 343a0483d..b8db8cde4 100644 --- a/BepuPhysics/Trees/Tree_Diagnostics.cs +++ b/BepuPhysics/Trees/Tree_Diagnostics.cs @@ -1,11 +1,7 @@ using BepuUtilities; using System; -using System.Collections.Generic; -using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; -using System.Text; - namespace BepuPhysics.Trees diff --git a/BepuPhysics/Trees/Tree_IntertreeQueries.cs b/BepuPhysics/Trees/Tree_IntertreeQueries.cs index 075ebaf7d..d8332ba73 100644 --- a/BepuPhysics/Trees/Tree_IntertreeQueries.cs +++ b/BepuPhysics/Trees/Tree_IntertreeQueries.cs @@ -1,7 +1,4 @@ using BepuUtilities; -using System.Collections.Generic; -using System.Diagnostics; -using System.Numerics; using System.Runtime.CompilerServices; namespace BepuPhysics.Trees diff --git a/BepuPhysics/Trees/Tree_IntertreeQueriesMT.cs b/BepuPhysics/Trees/Tree_IntertreeQueriesMT.cs index f116d687b..fc6b742d8 100644 --- a/BepuPhysics/Trees/Tree_IntertreeQueriesMT.cs +++ b/BepuPhysics/Trees/Tree_IntertreeQueriesMT.cs @@ -3,8 +3,6 @@ using BepuUtilities.Memory; using System; using System.Diagnostics; -using System.Linq; -using System.Numerics; using System.Runtime.CompilerServices; using System.Threading; diff --git a/BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs b/BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs index dfc22f5c3..4b9e25c4b 100644 --- a/BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs +++ b/BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs @@ -1,15 +1,9 @@ using BepuUtilities; -using BepuUtilities.Collections; using BepuUtilities.Memory; using BepuUtilities.TaskScheduling; using System; using System.Diagnostics; -using System.Linq; -using System.Numerics; -using System.Reflection.Metadata; using System.Runtime.CompilerServices; -using System.Threading; -using System.Xml.Linq; namespace BepuPhysics.Trees; diff --git a/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs b/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs index 174951505..8003f9e48 100644 --- a/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs +++ b/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs @@ -3,7 +3,6 @@ using BepuUtilities.Memory; using System; using System.Diagnostics; -using System.Linq; using System.Numerics; using System.Runtime.CompilerServices; using System.Threading; diff --git a/BepuPhysics/Trees/Tree_RayCast.cs b/BepuPhysics/Trees/Tree_RayCast.cs index ca271f50c..448a16772 100644 --- a/BepuPhysics/Trees/Tree_RayCast.cs +++ b/BepuPhysics/Trees/Tree_RayCast.cs @@ -1,9 +1,6 @@ -using System; -using System.Collections.Generic; -using System.Diagnostics; +using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; -using System.Text; namespace BepuPhysics.Trees { diff --git a/BepuPhysics/Trees/Tree_Refine2.cs b/BepuPhysics/Trees/Tree_Refine2.cs index d71b890e3..5a976d11c 100644 --- a/BepuPhysics/Trees/Tree_Refine2.cs +++ b/BepuPhysics/Trees/Tree_Refine2.cs @@ -3,15 +3,9 @@ using BepuUtilities.Memory; using BepuUtilities.TaskScheduling; using System; -using System.ComponentModel.Design; using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; -using System.Runtime.InteropServices; -using System.Threading; -using System.Threading.Tasks; -using System.Xml.Linq; -using static BepuPhysics.Collidables.CompoundBuilder; using Task = BepuUtilities.TaskScheduling.Task; namespace BepuPhysics.Trees; diff --git a/BepuPhysics/Trees/Tree_RefineCommon.cs b/BepuPhysics/Trees/Tree_RefineCommon.cs index 12b15b01b..ca27a7b05 100644 --- a/BepuPhysics/Trees/Tree_RefineCommon.cs +++ b/BepuPhysics/Trees/Tree_RefineCommon.cs @@ -1,9 +1,7 @@ -using BepuUtilities; -using BepuUtilities.Collections; +using BepuUtilities.Collections; using BepuUtilities.Memory; using System; using System.Diagnostics; -using System.Linq; using System.Runtime.CompilerServices; namespace BepuPhysics.Trees diff --git a/BepuPhysics/Trees/Tree_Refit.cs b/BepuPhysics/Trees/Tree_Refit.cs index 692bb28d2..5ab2112e5 100644 --- a/BepuPhysics/Trees/Tree_Refit.cs +++ b/BepuPhysics/Trees/Tree_Refit.cs @@ -1,10 +1,7 @@ using BepuUtilities; using System.Diagnostics; -using System.Linq; using System.Numerics; using System.Runtime.CompilerServices; -using System.Runtime.Intrinsics; -using System.Runtime.Intrinsics.X86; namespace BepuPhysics.Trees { diff --git a/BepuPhysics/Trees/Tree_Refit2.cs b/BepuPhysics/Trees/Tree_Refit2.cs index ef634b618..82332afde 100644 --- a/BepuPhysics/Trees/Tree_Refit2.cs +++ b/BepuPhysics/Trees/Tree_Refit2.cs @@ -1,15 +1,8 @@ using BepuUtilities; -using BepuUtilities.Collections; using BepuUtilities.Memory; using BepuUtilities.TaskScheduling; using System; using System.Diagnostics; -using System.Linq; -using System.Numerics; -using System.Runtime.CompilerServices; -using System.Runtime.Intrinsics; -using System.Runtime.Intrinsics.X86; -using System.Threading; namespace BepuPhysics.Trees; diff --git a/BepuPhysics/Trees/Tree_SelfQueries.cs b/BepuPhysics/Trees/Tree_SelfQueries.cs index e4aba9289..03035a098 100644 --- a/BepuPhysics/Trees/Tree_SelfQueries.cs +++ b/BepuPhysics/Trees/Tree_SelfQueries.cs @@ -1,20 +1,13 @@ -using BepuPhysics.CollisionDetection.CollisionTasks; -using BepuPhysics.Constraints.Contact; -using BepuUtilities; +using BepuUtilities; using BepuUtilities.Collections; using BepuUtilities.Memory; using BepuUtilities.TaskScheduling; using System; using System.Diagnostics; using System.Numerics; -using System.Reflection.Metadata.Ecma335; using System.Runtime.CompilerServices; -using System.Runtime.InteropServices; using System.Runtime.Intrinsics; using System.Runtime.Intrinsics.X86; -using System.Security.Cryptography; -using System.Xml.Linq; -using static BepuPhysics.Trees.Tree; namespace BepuPhysics.Trees { diff --git a/BepuPhysics/Trees/Tree_SelfQueriesMT.cs b/BepuPhysics/Trees/Tree_SelfQueriesMT.cs index cd19fc7d9..3234237c6 100644 --- a/BepuPhysics/Trees/Tree_SelfQueriesMT.cs +++ b/BepuPhysics/Trees/Tree_SelfQueriesMT.cs @@ -3,8 +3,6 @@ using BepuUtilities.Memory; using System; using System.Diagnostics; -using System.Linq; -using System.Numerics; using System.Runtime.CompilerServices; using System.Threading; diff --git a/BepuPhysics/Trees/Tree_Sweep.cs b/BepuPhysics/Trees/Tree_Sweep.cs index 1838c093c..d7e1f5b3c 100644 --- a/BepuPhysics/Trees/Tree_Sweep.cs +++ b/BepuPhysics/Trees/Tree_Sweep.cs @@ -1,10 +1,7 @@ using BepuUtilities; -using System; -using System.Collections.Generic; using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; -using System.Text; namespace BepuPhysics.Trees { diff --git a/BepuPhysics/Trees/Tree_SweepBuilder.cs b/BepuPhysics/Trees/Tree_SweepBuilder.cs index a55be90d5..d9cf770b2 100644 --- a/BepuPhysics/Trees/Tree_SweepBuilder.cs +++ b/BepuPhysics/Trees/Tree_SweepBuilder.cs @@ -3,7 +3,6 @@ using BepuUtilities.Memory; using System; using System.Diagnostics; -using System.Linq; using System.Numerics; using System.Runtime.CompilerServices; diff --git a/BepuUtilities/BoundingBox.cs b/BepuUtilities/BoundingBox.cs index aa8bc5665..cb3c625f7 100644 --- a/BepuUtilities/BoundingBox.cs +++ b/BepuUtilities/BoundingBox.cs @@ -1,13 +1,10 @@ using System; -using System.Collections.Generic; using System.Diagnostics; -using System.Linq; using System.Numerics; using System.Runtime.CompilerServices; using System.Runtime.InteropServices; using System.Runtime.Intrinsics; using System.Runtime.Intrinsics.X86; -using System.Text; namespace BepuUtilities { diff --git a/BepuUtilities/BoundingSphere.cs b/BepuUtilities/BoundingSphere.cs index e190704d0..fa63ff532 100644 --- a/BepuUtilities/BoundingSphere.cs +++ b/BepuUtilities/BoundingSphere.cs @@ -1,8 +1,4 @@ -using System; -using System.Collections.Generic; -using System.Linq; -using System.Numerics; -using System.Text; +using System.Numerics; namespace BepuUtilities { diff --git a/BepuUtilities/BundleIndexing.cs b/BepuUtilities/BundleIndexing.cs index a59c32b77..de7594251 100644 --- a/BepuUtilities/BundleIndexing.cs +++ b/BepuUtilities/BundleIndexing.cs @@ -1,8 +1,6 @@ -using System; -using System.Diagnostics; +using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; -using System.Runtime.InteropServices; using System.Runtime.Intrinsics; using System.Runtime.Intrinsics.X86; diff --git a/BepuUtilities/Collections/InsertionSort.cs b/BepuUtilities/Collections/InsertionSort.cs index 56d70d341..fa34ec89f 100644 --- a/BepuUtilities/Collections/InsertionSort.cs +++ b/BepuUtilities/Collections/InsertionSort.cs @@ -1,9 +1,4 @@ -using BepuUtilities.Memory; -using System; -using System.Collections.Generic; -using System.Diagnostics; -using System.Runtime.CompilerServices; -using System.Text; +using System.Runtime.CompilerServices; namespace BepuUtilities.Collections { diff --git a/BepuUtilities/Collections/LSBRadixSort.cs b/BepuUtilities/Collections/LSBRadixSort.cs index c4bf46279..885000430 100644 --- a/BepuUtilities/Collections/LSBRadixSort.cs +++ b/BepuUtilities/Collections/LSBRadixSort.cs @@ -1,11 +1,6 @@ using BepuUtilities.Memory; -using System; -using System.Collections.Generic; using System.Diagnostics; -using System.Linq; using System.Runtime.CompilerServices; -using System.Text; -using System.Threading.Tasks; namespace BepuUtilities.Collections { diff --git a/BepuUtilities/Collections/MSBRadixSort.cs b/BepuUtilities/Collections/MSBRadixSort.cs index 36c2e9e2d..8f3397c26 100644 --- a/BepuUtilities/Collections/MSBRadixSort.cs +++ b/BepuUtilities/Collections/MSBRadixSort.cs @@ -1,10 +1,6 @@ using System; -using System.Collections.Generic; using System.Diagnostics; -using System.Linq; using System.Runtime.CompilerServices; -using System.Text; -using System.Threading.Tasks; namespace BepuUtilities.Collections { diff --git a/BepuUtilities/Collections/Quicksort.cs b/BepuUtilities/Collections/Quicksort.cs index fb763d790..48701fcda 100644 --- a/BepuUtilities/Collections/Quicksort.cs +++ b/BepuUtilities/Collections/Quicksort.cs @@ -1,7 +1,4 @@ -using BepuUtilities.Collections; -using BepuUtilities.Memory; -using System.Diagnostics; -using System.Runtime.CompilerServices; +using System.Runtime.CompilerServices; namespace BepuUtilities.Collections { diff --git a/BepuUtilities/Collections/ReferenceComparer.cs b/BepuUtilities/Collections/ReferenceComparer.cs index 15b261c22..140a73640 100644 --- a/BepuUtilities/Collections/ReferenceComparer.cs +++ b/BepuUtilities/Collections/ReferenceComparer.cs @@ -1,6 +1,4 @@ -using System; -using System.Diagnostics; -using System.Runtime.CompilerServices; +using System.Runtime.CompilerServices; namespace BepuUtilities.Collections { diff --git a/BepuUtilities/Collections/VectorizedSorts.cs b/BepuUtilities/Collections/VectorizedSorts.cs index e38c79139..63d6cac7d 100644 --- a/BepuUtilities/Collections/VectorizedSorts.cs +++ b/BepuUtilities/Collections/VectorizedSorts.cs @@ -1,7 +1,6 @@ using BepuUtilities.Memory; using System; using System.Diagnostics; -using System.Numerics; using System.Runtime.Intrinsics; namespace BepuUtilities.Collections diff --git a/BepuUtilities/Collections/WrapperEqualityComparer.cs b/BepuUtilities/Collections/WrapperEqualityComparer.cs index be4fadf7f..403bb1898 100644 --- a/BepuUtilities/Collections/WrapperEqualityComparer.cs +++ b/BepuUtilities/Collections/WrapperEqualityComparer.cs @@ -1,5 +1,4 @@ -using BepuUtilities.Memory; -using System.Collections.Generic; +using System.Collections.Generic; using System.Runtime.CompilerServices; namespace BepuUtilities.Collections diff --git a/BepuUtilities/IThreadDispatcher.cs b/BepuUtilities/IThreadDispatcher.cs index 5746b85c4..03efc0bcc 100644 --- a/BepuUtilities/IThreadDispatcher.cs +++ b/BepuUtilities/IThreadDispatcher.cs @@ -1,6 +1,5 @@ using BepuUtilities.Memory; using System; -using System.Runtime.CompilerServices; namespace BepuUtilities { diff --git a/BepuUtilities/Int3.cs b/BepuUtilities/Int3.cs index 8888e7117..ea0cf2c7f 100644 --- a/BepuUtilities/Int3.cs +++ b/BepuUtilities/Int3.cs @@ -1,6 +1,5 @@ using BepuUtilities.Collections; using System; -using System.Numerics; using System.Runtime.CompilerServices; namespace BepuUtilities diff --git a/BepuUtilities/Int4.cs b/BepuUtilities/Int4.cs index ee8487acf..ea9fe61d6 100644 --- a/BepuUtilities/Int4.cs +++ b/BepuUtilities/Int4.cs @@ -1,6 +1,5 @@ using BepuUtilities.Collections; using System; -using System.Numerics; using System.Runtime.CompilerServices; namespace BepuUtilities diff --git a/BepuUtilities/MathHelper.cs b/BepuUtilities/MathHelper.cs index 293558cbc..f1cb16a45 100644 --- a/BepuUtilities/MathHelper.cs +++ b/BepuUtilities/MathHelper.cs @@ -2,7 +2,6 @@ using System.Numerics; using System.Runtime.CompilerServices; using System.Runtime.Intrinsics; -using System.Runtime.Intrinsics.Arm; using System.Runtime.Intrinsics.X86; namespace BepuUtilities diff --git a/BepuUtilities/Matrix2x2Wide.cs b/BepuUtilities/Matrix2x2Wide.cs index 5ef6e96e0..07b3837af 100644 --- a/BepuUtilities/Matrix2x2Wide.cs +++ b/BepuUtilities/Matrix2x2Wide.cs @@ -1,5 +1,4 @@ -using System; -using System.Numerics; +using System.Numerics; using System.Runtime.CompilerServices; namespace BepuUtilities diff --git a/BepuUtilities/Matrix2x3Wide.cs b/BepuUtilities/Matrix2x3Wide.cs index 2780d82a3..7d21f4522 100644 --- a/BepuUtilities/Matrix2x3Wide.cs +++ b/BepuUtilities/Matrix2x3Wide.cs @@ -1,7 +1,5 @@ -using System; -using System.Numerics; +using System.Numerics; using System.Runtime.CompilerServices; -using System.Runtime.InteropServices; namespace BepuUtilities { diff --git a/BepuUtilities/Matrix3x3Wide.cs b/BepuUtilities/Matrix3x3Wide.cs index e197fdc5c..ad2a0e8e1 100644 --- a/BepuUtilities/Matrix3x3Wide.cs +++ b/BepuUtilities/Matrix3x3Wide.cs @@ -1,5 +1,4 @@ -using System; -using System.Numerics; +using System.Numerics; using System.Runtime.CompilerServices; namespace BepuUtilities diff --git a/BepuUtilities/Memory/BufferPool.cs b/BepuUtilities/Memory/BufferPool.cs index f6485bb6c..303461468 100644 --- a/BepuUtilities/Memory/BufferPool.cs +++ b/BepuUtilities/Memory/BufferPool.cs @@ -1,5 +1,4 @@ -using BepuUtilities.Collections; -using System; +using System; using System.Collections.Generic; using System.Diagnostics; using System.Numerics; diff --git a/BepuUtilities/Memory/IUnmanagedMemoryPool.cs b/BepuUtilities/Memory/IUnmanagedMemoryPool.cs index a2babe566..db28eb90a 100644 --- a/BepuUtilities/Memory/IUnmanagedMemoryPool.cs +++ b/BepuUtilities/Memory/IUnmanagedMemoryPool.cs @@ -1,5 +1,4 @@ using System; -using System.Runtime.CompilerServices; namespace BepuUtilities.Memory { diff --git a/BepuUtilities/Memory/IdPool.cs b/BepuUtilities/Memory/IdPool.cs index bb0c1a49b..062ec417f 100644 --- a/BepuUtilities/Memory/IdPool.cs +++ b/BepuUtilities/Memory/IdPool.cs @@ -1,6 +1,4 @@ -using BepuUtilities.Collections; -using System; -using System.Collections.Generic; +using System; using System.Diagnostics; using System.Runtime.CompilerServices; diff --git a/BepuUtilities/Memory/ManagedIdPool.cs b/BepuUtilities/Memory/ManagedIdPool.cs index 61fc0d7cc..49084d30a 100644 --- a/BepuUtilities/Memory/ManagedIdPool.cs +++ b/BepuUtilities/Memory/ManagedIdPool.cs @@ -1,6 +1,4 @@ -using BepuUtilities.Collections; -using System; -using System.Collections.Generic; +using System; using System.Diagnostics; using System.Runtime.CompilerServices; diff --git a/BepuUtilities/Memory/Pool.cs b/BepuUtilities/Memory/Pool.cs index 67fd0ae00..bd9bbe2ab 100644 --- a/BepuUtilities/Memory/Pool.cs +++ b/BepuUtilities/Memory/Pool.cs @@ -1,7 +1,6 @@ using System; using System.Collections.Generic; using System.Diagnostics; -using System.Threading; namespace BepuUtilities.Memory { diff --git a/BepuUtilities/QuaternionWide.cs b/BepuUtilities/QuaternionWide.cs index 775a871e8..6c8d27c3c 100644 --- a/BepuUtilities/QuaternionWide.cs +++ b/BepuUtilities/QuaternionWide.cs @@ -1,5 +1,4 @@ -using System; -using System.Numerics; +using System.Numerics; using System.Runtime.CompilerServices; namespace BepuUtilities diff --git a/BepuUtilities/Symmetric2x2Wide.cs b/BepuUtilities/Symmetric2x2Wide.cs index e38880759..70a418bfe 100644 --- a/BepuUtilities/Symmetric2x2Wide.cs +++ b/BepuUtilities/Symmetric2x2Wide.cs @@ -1,5 +1,4 @@ -using System; -using System.Numerics; +using System.Numerics; using System.Runtime.CompilerServices; namespace BepuUtilities diff --git a/BepuUtilities/Symmetric3x3.cs b/BepuUtilities/Symmetric3x3.cs index 90442add0..babf87421 100644 --- a/BepuUtilities/Symmetric3x3.cs +++ b/BepuUtilities/Symmetric3x3.cs @@ -1,9 +1,5 @@ -using BepuUtilities; -using System; -using System.Collections.Generic; -using System.Numerics; +using System.Numerics; using System.Runtime.CompilerServices; -using System.Text; namespace BepuUtilities { diff --git a/BepuUtilities/Symmetric3x3Wide.cs b/BepuUtilities/Symmetric3x3Wide.cs index c1caaabab..ec9f1908c 100644 --- a/BepuUtilities/Symmetric3x3Wide.cs +++ b/BepuUtilities/Symmetric3x3Wide.cs @@ -1,8 +1,5 @@ -using System; -using System.Collections.Generic; -using System.Numerics; +using System.Numerics; using System.Runtime.CompilerServices; -using System.Text; namespace BepuUtilities { diff --git a/BepuUtilities/Symmetric4x4Wide.cs b/BepuUtilities/Symmetric4x4Wide.cs index ac825f616..9de86ad58 100644 --- a/BepuUtilities/Symmetric4x4Wide.cs +++ b/BepuUtilities/Symmetric4x4Wide.cs @@ -1,8 +1,5 @@ -using System; -using System.Collections.Generic; -using System.Numerics; +using System.Numerics; using System.Runtime.CompilerServices; -using System.Text; namespace BepuUtilities { diff --git a/BepuUtilities/Symmetric5x5Wide.cs b/BepuUtilities/Symmetric5x5Wide.cs index 6915d25de..c2c415bcf 100644 --- a/BepuUtilities/Symmetric5x5Wide.cs +++ b/BepuUtilities/Symmetric5x5Wide.cs @@ -1,8 +1,5 @@ -using System; -using System.Collections.Generic; -using System.Numerics; +using System.Numerics; using System.Runtime.CompilerServices; -using System.Text; namespace BepuUtilities { diff --git a/BepuUtilities/Symmetric6x6Wide.cs b/BepuUtilities/Symmetric6x6Wide.cs index 4f307c9af..bb2375196 100644 --- a/BepuUtilities/Symmetric6x6Wide.cs +++ b/BepuUtilities/Symmetric6x6Wide.cs @@ -1,10 +1,5 @@ -using System; -using System.Collections.Generic; -using System.Numerics; +using System.Numerics; using System.Runtime.CompilerServices; -using System.Runtime.Intrinsics; -using System.Runtime.Intrinsics.X86; -using System.Text; namespace BepuUtilities { diff --git a/BepuUtilities/TaskScheduling/TaskStack.cs b/BepuUtilities/TaskScheduling/TaskStack.cs index 6156b7606..28ba60ccd 100644 --- a/BepuUtilities/TaskScheduling/TaskStack.cs +++ b/BepuUtilities/TaskScheduling/TaskStack.cs @@ -1,8 +1,5 @@ using System; -using System.ComponentModel; using System.Diagnostics; -using System.Numerics; -using System.Runtime.CompilerServices; using System.Runtime.InteropServices; using System.Threading; using BepuUtilities.Memory; diff --git a/BepuUtilities/ThreadDispatcher.cs b/BepuUtilities/ThreadDispatcher.cs index 355bdf762..d065f6546 100644 --- a/BepuUtilities/ThreadDispatcher.cs +++ b/BepuUtilities/ThreadDispatcher.cs @@ -1,6 +1,5 @@ using System; using System.Diagnostics; -using System.Runtime.CompilerServices; using System.Runtime.InteropServices; using System.Threading; using BepuUtilities.Memory; diff --git a/BepuUtilities/Vector2Wide.cs b/BepuUtilities/Vector2Wide.cs index ec7fdab77..1f3a2c611 100644 --- a/BepuUtilities/Vector2Wide.cs +++ b/BepuUtilities/Vector2Wide.cs @@ -1,5 +1,4 @@ -using System; -using System.Numerics; +using System.Numerics; using System.Runtime.CompilerServices; namespace BepuUtilities diff --git a/BepuUtilities/Vector3Wide.cs b/BepuUtilities/Vector3Wide.cs index 4d3bca1bd..5ae5b740c 100644 --- a/BepuUtilities/Vector3Wide.cs +++ b/BepuUtilities/Vector3Wide.cs @@ -1,5 +1,4 @@ -using System; -using System.Numerics; +using System.Numerics; using System.Runtime.CompilerServices; namespace BepuUtilities diff --git a/BepuUtilities/Vector4Wide.cs b/BepuUtilities/Vector4Wide.cs index 86aa32f56..d994fb077 100644 --- a/BepuUtilities/Vector4Wide.cs +++ b/BepuUtilities/Vector4Wide.cs @@ -1,5 +1,4 @@ -using System; -using System.Numerics; +using System.Numerics; using System.Runtime.CompilerServices; namespace BepuUtilities diff --git a/DemoContentBuilder/ContentBuildCacheIO.cs b/DemoContentBuilder/ContentBuildCacheIO.cs index a12e2fa09..a63f8feb9 100644 --- a/DemoContentBuilder/ContentBuildCacheIO.cs +++ b/DemoContentBuilder/ContentBuildCacheIO.cs @@ -1,8 +1,6 @@ using System; using System.Collections.Generic; using System.IO; -using SharpDX.D3DCompiler; -using System.Linq; using DemoContentLoader; namespace DemoContentBuilder diff --git a/DemoContentBuilder/ContentBuilder.cs b/DemoContentBuilder/ContentBuilder.cs index 2991ecc2c..ad99a83ed 100644 --- a/DemoContentBuilder/ContentBuilder.cs +++ b/DemoContentBuilder/ContentBuilder.cs @@ -2,10 +2,7 @@ using System; using System.Collections.Generic; using System.IO; -using System.Linq; -using System.Text; using System.Threading; -using System.Threading.Tasks; namespace DemoContentBuilder { diff --git a/DemoContentBuilder/FontBuilder.cs b/DemoContentBuilder/FontBuilder.cs index bc86b2327..9763d0a87 100644 --- a/DemoContentBuilder/FontBuilder.cs +++ b/DemoContentBuilder/FontBuilder.cs @@ -1,6 +1,4 @@ using BepuUtilities; -using BepuUtilities.Collections; -using BepuUtilities.Memory; using DemoContentLoader; using SharpFont; using System; diff --git a/DemoContentBuilder/FontPacker.cs b/DemoContentBuilder/FontPacker.cs index 8915e0cd9..e97631f28 100644 --- a/DemoContentBuilder/FontPacker.cs +++ b/DemoContentBuilder/FontPacker.cs @@ -1,8 +1,5 @@ using System; using System.Collections.Generic; -using System.Linq; -using System.Text; -using System.Threading.Tasks; using DemoContentLoader; using System.Diagnostics; diff --git a/DemoContentBuilder/MetadataParsing.cs b/DemoContentBuilder/MetadataParsing.cs index 5be432484..6cd658917 100644 --- a/DemoContentBuilder/MetadataParsing.cs +++ b/DemoContentBuilder/MetadataParsing.cs @@ -1,9 +1,5 @@ -using System; -using System.Collections.Generic; +using System.Collections.Generic; using System.IO; -using System.Linq; -using System.Text; -using System.Threading.Tasks; namespace DemoContentBuilder { diff --git a/DemoContentBuilder/ShaderFileCache.cs b/DemoContentBuilder/ShaderFileCache.cs index e082cede4..5eb34a546 100644 --- a/DemoContentBuilder/ShaderFileCache.cs +++ b/DemoContentBuilder/ShaderFileCache.cs @@ -1,10 +1,5 @@ -using System; -using System.Collections.Generic; +using System.Collections.Generic; using System.IO; -using System.Linq; -using System.Text; -using System.Threading; -using System.Threading.Tasks; namespace DemoContentBuilder { diff --git a/DemoContentBuilder/Texture2DBuilder.cs b/DemoContentBuilder/Texture2DBuilder.cs index e99ab8bb3..a7226f4aa 100644 --- a/DemoContentBuilder/Texture2DBuilder.cs +++ b/DemoContentBuilder/Texture2DBuilder.cs @@ -1,12 +1,6 @@ using DemoContentLoader; -using ObjLoader.Loader.Loaders; -using SixLabors.ImageSharp.Advanced; using SixLabors.ImageSharp.PixelFormats; -using System; -using System.Collections.Generic; -using System.Diagnostics; using System.IO; -using System.Numerics; using System.Runtime.CompilerServices; namespace DemoContentBuilder diff --git a/DemoContentLoader/FontContent.cs b/DemoContentLoader/FontContent.cs index bc540b633..b356ac836 100644 --- a/DemoContentLoader/FontContent.cs +++ b/DemoContentLoader/FontContent.cs @@ -1,8 +1,6 @@ using BepuUtilities; -using DemoContentLoader; using System; using System.Collections.Generic; -using System.Numerics; namespace DemoContentLoader { diff --git a/DemoContentLoader/FontIO.cs b/DemoContentLoader/FontIO.cs index 1746a4954..afd3df4f3 100644 --- a/DemoContentLoader/FontIO.cs +++ b/DemoContentLoader/FontIO.cs @@ -1,7 +1,5 @@ -using System; -using System.Collections.Generic; +using System.Collections.Generic; using System.IO; -using System.Text; namespace DemoContentLoader { diff --git a/DemoContentLoader/MeshContent.cs b/DemoContentLoader/MeshContent.cs index d41c44496..af560abc7 100644 --- a/DemoContentLoader/MeshContent.cs +++ b/DemoContentLoader/MeshContent.cs @@ -1,8 +1,4 @@ -using BepuUtilities; -using DemoContentLoader; -using System; -using System.Collections.Generic; -using System.Numerics; +using System.Numerics; namespace DemoContentLoader { diff --git a/DemoContentLoader/MeshIO.cs b/DemoContentLoader/MeshIO.cs index f88a61cf9..cc3b84214 100644 --- a/DemoContentLoader/MeshIO.cs +++ b/DemoContentLoader/MeshIO.cs @@ -1,9 +1,6 @@ -using System; -using System.Collections.Generic; -using System.IO; +using System.IO; using System.Numerics; using System.Runtime.CompilerServices; -using System.Text; namespace DemoContentLoader { diff --git a/DemoRenderer/Constraints/BoundingBoxLineExtractor.cs b/DemoRenderer/Constraints/BoundingBoxLineExtractor.cs index e585f5685..a24354f4a 100644 --- a/DemoRenderer/Constraints/BoundingBoxLineExtractor.cs +++ b/DemoRenderer/Constraints/BoundingBoxLineExtractor.cs @@ -3,10 +3,6 @@ using BepuUtilities.Memory; using BepuPhysics; using System.Numerics; -using System.Threading.Tasks; -using System.Threading; -using BepuUtilities; -using BepuPhysics.CollisionDetection; using BepuPhysics.Trees; using System.Runtime.CompilerServices; diff --git a/DemoRenderer/Constraints/CenterDistanceLimitLineExtractor.cs b/DemoRenderer/Constraints/CenterDistanceLimitLineExtractor.cs index eb568a752..8946c2ee1 100644 --- a/DemoRenderer/Constraints/CenterDistanceLimitLineExtractor.cs +++ b/DemoRenderer/Constraints/CenterDistanceLimitLineExtractor.cs @@ -1,5 +1,4 @@ using BepuUtilities.Collections; -using BepuUtilities.Memory; using BepuPhysics; using BepuPhysics.Constraints; using System.Numerics; diff --git a/DemoRenderer/Constraints/CenterDistanceLineExtractor.cs b/DemoRenderer/Constraints/CenterDistanceLineExtractor.cs index 498754b3b..e5dfb2abf 100644 --- a/DemoRenderer/Constraints/CenterDistanceLineExtractor.cs +++ b/DemoRenderer/Constraints/CenterDistanceLineExtractor.cs @@ -1,5 +1,4 @@ using BepuUtilities.Collections; -using BepuUtilities.Memory; using BepuPhysics; using BepuPhysics.Constraints; using System.Numerics; diff --git a/DemoRenderer/Constraints/ConstraintLineExtractor.cs b/DemoRenderer/Constraints/ConstraintLineExtractor.cs index de38268b5..da24df035 100644 --- a/DemoRenderer/Constraints/ConstraintLineExtractor.cs +++ b/DemoRenderer/Constraints/ConstraintLineExtractor.cs @@ -4,13 +4,11 @@ using BepuPhysics; using BepuPhysics.Constraints; using System.Runtime.CompilerServices; -using System.Threading.Tasks; using System.Diagnostics; using BepuUtilities; using System.Numerics; using BepuPhysics.CollisionDetection; using BepuPhysics.Constraints.Contact; -using System.Threading; namespace DemoRenderer.Constraints { diff --git a/DemoRenderer/Constraints/ContactLineExtractors.cs b/DemoRenderer/Constraints/ContactLineExtractors.cs index 073ba6b59..3e4667844 100644 --- a/DemoRenderer/Constraints/ContactLineExtractors.cs +++ b/DemoRenderer/Constraints/ContactLineExtractors.cs @@ -1,7 +1,4 @@ -using BepuPhysics.CollisionDetection; -using System; -using System.Numerics; -using System.Runtime.CompilerServices; +using System.Numerics; using DemoRenderer.Constraints; using BepuUtilities.Collections; diff --git a/DemoRenderer/Constraints/LineExtractor.cs b/DemoRenderer/Constraints/LineExtractor.cs index 8a5686b2b..ddcef42f6 100644 --- a/DemoRenderer/Constraints/LineExtractor.cs +++ b/DemoRenderer/Constraints/LineExtractor.cs @@ -2,7 +2,6 @@ using BepuUtilities.Collections; using BepuUtilities.Memory; using BepuPhysics; -using BepuPhysics.CollisionDetection; using System; namespace DemoRenderer.Constraints diff --git a/DemoRenderer/ParallelLooper.cs b/DemoRenderer/ParallelLooper.cs index 6d6f787d4..9c04104cc 100644 --- a/DemoRenderer/ParallelLooper.cs +++ b/DemoRenderer/ParallelLooper.cs @@ -1,8 +1,5 @@ using BepuUtilities; using System; -using System.Collections.Generic; -using System.Runtime.CompilerServices; -using System.Text; using System.Threading; namespace DemoRenderer diff --git a/DemoRenderer/UI/GlyphBatch.cs b/DemoRenderer/UI/GlyphBatch.cs index d6b2ac0ec..d40e334d9 100644 --- a/DemoRenderer/UI/GlyphBatch.cs +++ b/DemoRenderer/UI/GlyphBatch.cs @@ -1,8 +1,5 @@ -using BepuUtilities; -using DemoUtilities; -using System; +using System; using System.Numerics; -using System.Text; namespace DemoRenderer.UI { diff --git a/DemoUtilities/Input.cs b/DemoUtilities/Input.cs index e916fa92d..c7b2a7d81 100644 --- a/DemoUtilities/Input.cs +++ b/DemoUtilities/Input.cs @@ -4,9 +4,7 @@ using OpenTK; using OpenTK.Input; using System; -using System.Collections.Generic; using System.Runtime.CompilerServices; -using System.Text; namespace DemoUtilities { diff --git a/DemoUtilities/SpanConverter.cs b/DemoUtilities/SpanConverter.cs index 28de6115d..301dc6925 100644 --- a/DemoUtilities/SpanConverter.cs +++ b/DemoUtilities/SpanConverter.cs @@ -1,7 +1,5 @@ using BepuUtilities.Memory; using System; -using System.Collections.Generic; -using System.Text; namespace DemoUtilities { diff --git a/DemoUtilities/TextBuilder.cs b/DemoUtilities/TextBuilder.cs index 66f946e91..652874c15 100644 --- a/DemoUtilities/TextBuilder.cs +++ b/DemoUtilities/TextBuilder.cs @@ -1,11 +1,7 @@ -using BepuUtilities.Collections; -using BepuUtilities.Memory; -using System; -using System.Collections.Generic; +using System; using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; -using System.Text; namespace DemoUtilities { diff --git a/Demos/Controls.cs b/Demos/Controls.cs index 1e7d303dd..d541ae5b8 100644 --- a/Demos/Controls.cs +++ b/Demos/Controls.cs @@ -1,10 +1,8 @@ using DemoUtilities; -using OpenTK.Audio.OpenAL; using OpenTK.Input; using System; using System.Collections.Generic; using System.Runtime.InteropServices; -using System.Text; namespace Demos { diff --git a/Demos/DemoCallbacks.cs b/Demos/DemoCallbacks.cs index 3c0749966..1a7a97925 100644 --- a/Demos/DemoCallbacks.cs +++ b/Demos/DemoCallbacks.cs @@ -1,5 +1,4 @@ using BepuUtilities; -using BepuUtilities.Memory; using BepuPhysics; using BepuPhysics.Collidables; using BepuPhysics.CollisionDetection; diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index 6612d91d5..17cc3a2a3 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -6,11 +6,8 @@ using Demos.Demos.Dancers; using Demos.Demos.Sponsors; using Demos.Demos.Tanks; -using Demos.SpecializedTests; -using Demos.SpecializedTests.Media; using System; using System.Collections.Generic; -using System.Text; namespace Demos { diff --git a/Demos/Demos/Cars/CarDemo.cs b/Demos/Demos/Cars/CarDemo.cs index fd56bc932..5535132e8 100644 --- a/Demos/Demos/Cars/CarDemo.cs +++ b/Demos/Demos/Cars/CarDemo.cs @@ -1,5 +1,4 @@ using System; -using System.Diagnostics; using System.Numerics; using BepuPhysics; using BepuPhysics.Collidables; diff --git a/Demos/Demos/CollisionQueryDemo.cs b/Demos/Demos/CollisionQueryDemo.cs index be857a3d7..e58602538 100644 --- a/Demos/Demos/CollisionQueryDemo.cs +++ b/Demos/Demos/CollisionQueryDemo.cs @@ -10,10 +10,8 @@ using DemoRenderer.UI; using DemoUtilities; using System; -using System.Collections.Generic; using System.Numerics; using System.Runtime.CompilerServices; -using System.Text; namespace Demos.Demos { diff --git a/Demos/Demos/CollisionTrackingDemo.cs b/Demos/Demos/CollisionTrackingDemo.cs index 168d650a2..d32c3810f 100644 --- a/Demos/Demos/CollisionTrackingDemo.cs +++ b/Demos/Demos/CollisionTrackingDemo.cs @@ -13,7 +13,6 @@ using System.Runtime.CompilerServices; using BepuPhysics.Constraints; using System.Diagnostics; -using System.Runtime.InteropServices; namespace Demos.Demos; diff --git a/Demos/Demos/ColosseumDemo.cs b/Demos/Demos/ColosseumDemo.cs index 112e7b844..8edb426a3 100644 --- a/Demos/Demos/ColosseumDemo.cs +++ b/Demos/Demos/ColosseumDemo.cs @@ -2,15 +2,12 @@ using BepuPhysics.Collidables; using BepuPhysics.Constraints; using BepuUtilities; -using BepuUtilities.Memory; using DemoContentLoader; using DemoRenderer; using DemoRenderer.UI; using DemoUtilities; using System; -using System.Diagnostics; using System.Numerics; -using System.Threading; namespace Demos.Demos { diff --git a/Demos/Demos/Dancers/DancerDemo.cs b/Demos/Demos/Dancers/DancerDemo.cs index e6a41038b..4ab6397b8 100644 --- a/Demos/Demos/Dancers/DancerDemo.cs +++ b/Demos/Demos/Dancers/DancerDemo.cs @@ -1,16 +1,13 @@ using BepuPhysics; using BepuPhysics.Collidables; -using BepuPhysics.CollisionDetection; using BepuPhysics.Constraints; using BepuUtilities; -using BepuUtilities.Memory; using DemoContentLoader; using DemoRenderer; using DemoRenderer.UI; using DemoUtilities; using System; using System.Numerics; -using System.Runtime.InteropServices; namespace Demos.Demos.Dancers { diff --git a/Demos/Demos/Dancers/PlumpDancerDemo.cs b/Demos/Demos/Dancers/PlumpDancerDemo.cs index ce333dd62..0499ad2ff 100644 --- a/Demos/Demos/Dancers/PlumpDancerDemo.cs +++ b/Demos/Demos/Dancers/PlumpDancerDemo.cs @@ -1,9 +1,7 @@ using BepuPhysics; using BepuPhysics.Collidables; -using BepuPhysics.CollisionDetection; using BepuPhysics.Constraints; using BepuUtilities; -using BepuUtilities.Memory; using DemoContentLoader; using DemoRenderer; using DemoRenderer.UI; @@ -11,7 +9,6 @@ using System; using System.Diagnostics; using System.Numerics; -using System.Runtime.InteropServices; namespace Demos.Demos.Dancers { diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index cb663a1f5..8d2df09d0 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -2,7 +2,6 @@ using BepuPhysics.Collidables; using BepuPhysics.CollisionDetection; using BepuPhysics.Constraints; -using BepuPhysics.Constraints.Contact; using BepuUtilities; using BepuUtilities.Collections; using BepuUtilities.Memory; diff --git a/Demos/Demos/PerBodyGravityDemo.cs b/Demos/Demos/PerBodyGravityDemo.cs index 02b857ce6..edfdb4d8a 100644 --- a/Demos/Demos/PerBodyGravityDemo.cs +++ b/Demos/Demos/PerBodyGravityDemo.cs @@ -1,6 +1,5 @@ using BepuPhysics; using BepuPhysics.Collidables; -using BepuPhysics.CollisionDetection; using BepuPhysics.Constraints; using BepuUtilities; using DemoContentLoader; @@ -8,10 +7,7 @@ using DemoRenderer.UI; using DemoUtilities; using System; -using System.Collections.Generic; using System.Numerics; -using System.Runtime.CompilerServices; -using System.Text; namespace Demos.Demos { diff --git a/Demos/Demos/PlanetDemo.cs b/Demos/Demos/PlanetDemo.cs index 6b75c561c..43a7ac9ef 100644 --- a/Demos/Demos/PlanetDemo.cs +++ b/Demos/Demos/PlanetDemo.cs @@ -1,6 +1,5 @@ using BepuPhysics; using BepuPhysics.Collidables; -using BepuPhysics.CollisionDetection; using BepuPhysics.Constraints; using BepuUtilities; using DemoContentLoader; @@ -8,10 +7,7 @@ using DemoRenderer.UI; using DemoUtilities; using System; -using System.Collections.Generic; using System.Numerics; -using System.Runtime.CompilerServices; -using System.Text; namespace Demos.Demos { diff --git a/Demos/Demos/PyramidDemo.cs b/Demos/Demos/PyramidDemo.cs index 015563d1d..bddeacdb1 100644 --- a/Demos/Demos/PyramidDemo.cs +++ b/Demos/Demos/PyramidDemo.cs @@ -7,9 +7,7 @@ using DemoRenderer.UI; using DemoUtilities; using System; -using System.Collections.Generic; using System.Numerics; -using System.Text; namespace Demos.Demos { diff --git a/Demos/Demos/RagdollTubeDemo.cs b/Demos/Demos/RagdollTubeDemo.cs index 98ba64d2e..dab9b13ad 100644 --- a/Demos/Demos/RagdollTubeDemo.cs +++ b/Demos/Demos/RagdollTubeDemo.cs @@ -5,7 +5,6 @@ using System.Numerics; using System; using DemoContentLoader; -using Demos.Demos; using DemoUtilities; using BepuPhysics.CollisionDetection; using BepuPhysics.Constraints; diff --git a/Demos/Demos/RayCastingDemo.cs b/Demos/Demos/RayCastingDemo.cs index a446ab1a6..e192c2812 100644 --- a/Demos/Demos/RayCastingDemo.cs +++ b/Demos/Demos/RayCastingDemo.cs @@ -16,7 +16,6 @@ using System.Threading; using Demos.SpecializedTests; using DemoContentLoader; -using Demos.Demos; using Helpers = DemoRenderer.Helpers; namespace Demos diff --git a/Demos/Demos/RopeStabilityDemo.cs b/Demos/Demos/RopeStabilityDemo.cs index 6fc2ee8b1..44946ba32 100644 --- a/Demos/Demos/RopeStabilityDemo.cs +++ b/Demos/Demos/RopeStabilityDemo.cs @@ -1,6 +1,5 @@ using BepuPhysics; using BepuPhysics.Collidables; -using BepuPhysics.CollisionDetection; using BepuPhysics.Constraints; using BepuUtilities; using DemoContentLoader; @@ -8,9 +7,7 @@ using DemoRenderer.UI; using DemoUtilities; using System; -using System.Collections.Generic; using System.Numerics; -using System.Text; namespace Demos.Demos { diff --git a/Demos/Demos/SimpleSelfContainedDemo.cs b/Demos/Demos/SimpleSelfContainedDemo.cs index 9c11c1b68..29a41d9aa 100644 --- a/Demos/Demos/SimpleSelfContainedDemo.cs +++ b/Demos/Demos/SimpleSelfContainedDemo.cs @@ -5,10 +5,8 @@ using BepuUtilities; using BepuUtilities.Memory; using System; -using System.Collections.Generic; using System.Numerics; using System.Runtime.CompilerServices; -using System.Text; namespace Demos.Demos { diff --git a/Demos/Demos/Sponsors/SponsorCharacterAI.cs b/Demos/Demos/Sponsors/SponsorCharacterAI.cs index 3088fb989..b8aaaf943 100644 --- a/Demos/Demos/Sponsors/SponsorCharacterAI.cs +++ b/Demos/Demos/Sponsors/SponsorCharacterAI.cs @@ -3,9 +3,7 @@ using BepuUtilities.Collections; using Demos.Demos.Characters; using System; -using System.Collections.Generic; using System.Numerics; -using System.Text; namespace Demos.Demos.Sponsors { diff --git a/Demos/Demos/Sponsors/SponsorDemo.cs b/Demos/Demos/Sponsors/SponsorDemo.cs index e5ee43012..3dbeb9de6 100644 --- a/Demos/Demos/Sponsors/SponsorDemo.cs +++ b/Demos/Demos/Sponsors/SponsorDemo.cs @@ -3,7 +3,6 @@ using System; using System.Collections.Generic; using System.Numerics; -using System.Text; using BepuPhysics; using DemoRenderer.UI; using System.IO; diff --git a/Demos/Demos/Tanks/TankCallbacks.cs b/Demos/Demos/Tanks/TankCallbacks.cs index f6de0b82b..df7bfe573 100644 --- a/Demos/Demos/Tanks/TankCallbacks.cs +++ b/Demos/Demos/Tanks/TankCallbacks.cs @@ -1,13 +1,10 @@ -using System; -using System.Collections.Generic; -using System.Runtime.CompilerServices; +using System.Runtime.CompilerServices; using System.Threading; using BepuPhysics; using BepuPhysics.Collidables; using BepuPhysics.CollisionDetection; using BepuPhysics.Constraints; using BepuUtilities.Collections; -using BepuUtilities.Memory; namespace Demos.Demos.Tanks { diff --git a/Demos/Demos/Tanks/TankController.cs b/Demos/Demos/Tanks/TankController.cs index f77a96dab..7fa94ac9b 100644 --- a/Demos/Demos/Tanks/TankController.cs +++ b/Demos/Demos/Tanks/TankController.cs @@ -1,7 +1,5 @@ -using System; -using System.Numerics; +using System.Numerics; using BepuPhysics; -using BepuPhysics.Collidables; namespace Demos.Demos.Tanks { diff --git a/Demos/GameLoop.cs b/Demos/GameLoop.cs index 2a347f478..4d929b098 100644 --- a/Demos/GameLoop.cs +++ b/Demos/GameLoop.cs @@ -1,10 +1,7 @@ using DemoRenderer; using DemoUtilities; using System; -using System.Collections.Generic; -using System.Text; using BepuUtilities; -using OpenTK; using BepuUtilities.Memory; namespace Demos diff --git a/Demos/Program.cs b/Demos/Program.cs index 72ceababf..2c53c2b70 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -1,11 +1,7 @@ -using BepuPhysics.Trees; -using BepuUtilities; +using BepuUtilities; using DemoContentLoader; -using Demos.Demos; -using Demos.SpecializedTests; using DemoUtilities; using OpenTK; -using System.Runtime.Intrinsics; namespace Demos { diff --git a/Demos/RolloverInfo.cs b/Demos/RolloverInfo.cs index 942e692d4..db6354dbe 100644 --- a/Demos/RolloverInfo.cs +++ b/Demos/RolloverInfo.cs @@ -1,11 +1,9 @@ -using BepuUtilities; -using DemoRenderer; +using DemoRenderer; using DemoRenderer.UI; using DemoUtilities; using System; using System.Collections.Generic; using System.Numerics; -using System.Text; namespace Demos { diff --git a/Demos/SimulationTimeSamples.cs b/Demos/SimulationTimeSamples.cs index ad60bf279..2e3733a05 100644 --- a/Demos/SimulationTimeSamples.cs +++ b/Demos/SimulationTimeSamples.cs @@ -1,8 +1,5 @@ using BepuPhysics; using BepuUtilities.Memory; -using System; -using System.Collections.Generic; -using System.Text; namespace Demos { diff --git a/Demos/SpecializedTests/BatchedCollisionTests.cs b/Demos/SpecializedTests/BatchedCollisionTests.cs index 0f2f53f0c..cb537d619 100644 --- a/Demos/SpecializedTests/BatchedCollisionTests.cs +++ b/Demos/SpecializedTests/BatchedCollisionTests.cs @@ -3,13 +3,10 @@ using BepuPhysics; using BepuPhysics.Collidables; using BepuPhysics.CollisionDetection; -using BepuPhysics.CollisionDetection.CollisionTasks; using System; -using System.Collections.Generic; using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; -using System.Text; using BepuPhysics.CollisionDetection.SweepTasks; using BepuUtilities.Collections; diff --git a/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs b/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs index 50a8d022c..3bf636406 100644 --- a/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs +++ b/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs @@ -6,11 +6,7 @@ using System; using System.Numerics; using System.Diagnostics; -using BepuUtilities.Memory; -using BepuUtilities.Collections; -using System.Runtime.CompilerServices; using DemoContentLoader; -using BepuPhysics.Constraints; using BepuPhysics.CollisionDetection; namespace Demos.SpecializedTests diff --git a/Demos/SpecializedTests/CacheBlaster.cs b/Demos/SpecializedTests/CacheBlaster.cs index 13aa378d7..aab3e08cd 100644 --- a/Demos/SpecializedTests/CacheBlaster.cs +++ b/Demos/SpecializedTests/CacheBlaster.cs @@ -1,10 +1,5 @@ -using System; -using System.Collections.Generic; -using System.Linq; -using System.Numerics; +using System.Numerics; using System.Runtime.CompilerServices; -using System.Runtime.InteropServices; -using System.Text; using System.Threading.Tasks; namespace Demos.SpecializedTests diff --git a/Demos/SpecializedTests/CapsuleTestDemo.cs b/Demos/SpecializedTests/CapsuleTestDemo.cs index f6bfd65c5..f3e262262 100644 --- a/Demos/SpecializedTests/CapsuleTestDemo.cs +++ b/Demos/SpecializedTests/CapsuleTestDemo.cs @@ -5,9 +5,6 @@ using BepuPhysics.Collidables; using System; using System.Numerics; -using System.Diagnostics; -using BepuUtilities.Memory; -using BepuUtilities.Collections; using DemoContentLoader; using BepuPhysics.Constraints; diff --git a/Demos/SpecializedTests/ClothLatticeDemo.cs b/Demos/SpecializedTests/ClothLatticeDemo.cs index 9017dc7a1..2a09e9a43 100644 --- a/Demos/SpecializedTests/ClothLatticeDemo.cs +++ b/Demos/SpecializedTests/ClothLatticeDemo.cs @@ -1,13 +1,8 @@ using BepuUtilities; using DemoRenderer; -using DemoUtilities; using BepuPhysics; using BepuPhysics.Collidables; -using System; using System.Numerics; -using System.Diagnostics; -using BepuUtilities.Memory; -using BepuUtilities.Collections; using BepuPhysics.Constraints; using DemoContentLoader; diff --git a/Demos/SpecializedTests/CompoundBoundTests.cs b/Demos/SpecializedTests/CompoundBoundTests.cs index 7d031ad2d..7c9f16676 100644 --- a/Demos/SpecializedTests/CompoundBoundTests.cs +++ b/Demos/SpecializedTests/CompoundBoundTests.cs @@ -5,7 +5,6 @@ using BepuPhysics.Collidables; using System; using System.Numerics; -using System.Diagnostics; using DemoContentLoader; using DemoRenderer.UI; using DemoRenderer.Constraints; diff --git a/Demos/SpecializedTests/CompoundCollisionIndicesTest.cs b/Demos/SpecializedTests/CompoundCollisionIndicesTest.cs index 0dbb5bf96..fd86c781c 100644 --- a/Demos/SpecializedTests/CompoundCollisionIndicesTest.cs +++ b/Demos/SpecializedTests/CompoundCollisionIndicesTest.cs @@ -1,7 +1,5 @@ using System; -using System.Collections.Generic; using System.Numerics; -using System.Text; using BepuUtilities; using DemoContentLoader; using DemoRenderer; diff --git a/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs b/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs index 400b06d0f..4451fad5b 100644 --- a/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs +++ b/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs @@ -1,12 +1,9 @@ -using BepuUtilities; -using DemoRenderer; +using DemoRenderer; using BepuPhysics; using BepuPhysics.Collidables; using System.Numerics; using DemoContentLoader; using BepuPhysics.Constraints; -using Demos.Demos; -using System; namespace Demos.SpecializedTests { diff --git a/Demos/SpecializedTests/ConvexHullTestDemo.cs b/Demos/SpecializedTests/ConvexHullTestDemo.cs index bc18de0ea..e4f17746f 100644 --- a/Demos/SpecializedTests/ConvexHullTestDemo.cs +++ b/Demos/SpecializedTests/ConvexHullTestDemo.cs @@ -1,21 +1,15 @@ using System; using System.Collections.Generic; using System.Numerics; -using System.Text; using BepuPhysics.Collidables; using BepuUtilities.Collections; using DemoContentLoader; using DemoRenderer; using BepuPhysics; -using DemoRenderer.UI; -using DemoUtilities; -using DemoRenderer.Constraints; using static BepuPhysics.Collidables.ConvexHullHelper; using System.Diagnostics; using BepuUtilities; -using BepuPhysics.Constraints.Contact; using BepuPhysics.Constraints; -using Demos.Demos; using BepuUtilities.Memory; using System.Text.Json; using System.IO; diff --git a/Demos/SpecializedTests/DeterminismTest.cs b/Demos/SpecializedTests/DeterminismTest.cs index a01c6a7a1..f4325f0b5 100644 --- a/Demos/SpecializedTests/DeterminismTest.cs +++ b/Demos/SpecializedTests/DeterminismTest.cs @@ -1,11 +1,6 @@ -using BepuUtilities; -using BepuUtilities.Memory; -using BepuPhysics; -using BepuPhysics.Collidables; +using BepuPhysics; using System; using System.Collections.Generic; -using System.Numerics; -using System.Text; using DemoContentLoader; namespace Demos.SpecializedTests diff --git a/Demos/SpecializedTests/GyroscopeTestDemo.cs b/Demos/SpecializedTests/GyroscopeTestDemo.cs index 1116fb53d..6dae4a31c 100644 --- a/Demos/SpecializedTests/GyroscopeTestDemo.cs +++ b/Demos/SpecializedTests/GyroscopeTestDemo.cs @@ -1,13 +1,9 @@ -using System; -using System.Collections.Generic; -using System.Numerics; -using System.Text; +using System.Numerics; using BepuUtilities; using DemoContentLoader; using DemoRenderer; using BepuPhysics; using BepuPhysics.Collidables; -using System.Runtime.CompilerServices; using BepuPhysics.Constraints; namespace Demos.Demos diff --git a/Demos/SpecializedTests/HeadlessDemo.cs b/Demos/SpecializedTests/HeadlessDemo.cs index 572a2d05c..8aee7df8c 100644 --- a/Demos/SpecializedTests/HeadlessDemo.cs +++ b/Demos/SpecializedTests/HeadlessDemo.cs @@ -1,12 +1,6 @@ -using BepuPhysics.Collidables; -using BepuPhysics.CollisionDetection; -using BepuUtilities.Memory; -using DemoContentLoader; -using DemoUtilities; +using DemoContentLoader; using System; -using System.Collections.Generic; using System.Diagnostics; -using System.Text; namespace Demos.SpecializedTests; diff --git a/Demos/SpecializedTests/IntertreeThreadingTests.cs b/Demos/SpecializedTests/IntertreeThreadingTests.cs index 0f999de04..898fd42b2 100644 --- a/Demos/SpecializedTests/IntertreeThreadingTests.cs +++ b/Demos/SpecializedTests/IntertreeThreadingTests.cs @@ -1,12 +1,9 @@ -using BepuPhysics; -using BepuPhysics.CollisionDetection; -using BepuUtilities; +using BepuUtilities; using BepuUtilities.Memory; using BepuPhysics.Trees; using System; using System.Collections.Generic; using System.Numerics; -using System.Text; using System.Runtime.CompilerServices; namespace Demos.SpecializedTests diff --git a/Demos/SpecializedTests/Media/2.0/NewtVideoDemo.cs b/Demos/SpecializedTests/Media/2.0/NewtVideoDemo.cs index 5a0459742..5368d440e 100644 --- a/Demos/SpecializedTests/Media/2.0/NewtVideoDemo.cs +++ b/Demos/SpecializedTests/Media/2.0/NewtVideoDemo.cs @@ -2,7 +2,6 @@ using BepuPhysics.Collidables; using BepuPhysics.Constraints; using BepuUtilities; -using BepuUtilities.Memory; using DemoContentLoader; using DemoRenderer; using Demos.Demos; diff --git a/Demos/SpecializedTests/Media/2.0/PyramidVideoDemo.cs b/Demos/SpecializedTests/Media/2.0/PyramidVideoDemo.cs index f6231d30a..d9228ad2a 100644 --- a/Demos/SpecializedTests/Media/2.0/PyramidVideoDemo.cs +++ b/Demos/SpecializedTests/Media/2.0/PyramidVideoDemo.cs @@ -7,9 +7,7 @@ using DemoRenderer.UI; using DemoUtilities; using System; -using System.Collections.Generic; using System.Numerics; -using System.Text; namespace Demos.Demos.Media { diff --git a/Demos/SpecializedTests/Media/2.4/Colosseum24VideoDemo.cs b/Demos/SpecializedTests/Media/2.4/Colosseum24VideoDemo.cs index 20bf4bffc..ca80cae3d 100644 --- a/Demos/SpecializedTests/Media/2.4/Colosseum24VideoDemo.cs +++ b/Demos/SpecializedTests/Media/2.4/Colosseum24VideoDemo.cs @@ -4,7 +4,6 @@ using BepuUtilities; using DemoContentLoader; using DemoRenderer; -using DemoRenderer.UI; using DemoUtilities; using System; using System.Numerics; diff --git a/Demos/SpecializedTests/Media/2.4/ExcessivePyramidVideoDemo.cs b/Demos/SpecializedTests/Media/2.4/ExcessivePyramidVideoDemo.cs index ca491eec6..bdd277167 100644 --- a/Demos/SpecializedTests/Media/2.4/ExcessivePyramidVideoDemo.cs +++ b/Demos/SpecializedTests/Media/2.4/ExcessivePyramidVideoDemo.cs @@ -4,12 +4,9 @@ using BepuUtilities; using DemoContentLoader; using DemoRenderer; -using DemoRenderer.UI; using DemoUtilities; using System; -using System.Collections.Generic; using System.Numerics; -using System.Text; namespace Demos.SpecializedTests.Media { diff --git a/Demos/SpecializedTests/Media/2.4/NewtTyrannyDemo.cs b/Demos/SpecializedTests/Media/2.4/NewtTyrannyDemo.cs index 915475972..95e31965f 100644 --- a/Demos/SpecializedTests/Media/2.4/NewtTyrannyDemo.cs +++ b/Demos/SpecializedTests/Media/2.4/NewtTyrannyDemo.cs @@ -1,14 +1,9 @@ using DemoContentLoader; using DemoRenderer; using System; -using System.Collections.Generic; using System.Numerics; -using System.Text; using BepuPhysics; -using DemoRenderer.UI; -using System.IO; using DemoUtilities; -using System.Diagnostics; using BepuUtilities.Collections; using BepuPhysics.Collidables; using Demos.Demos.Characters; diff --git a/Demos/SpecializedTests/Media/2.4/RopeTwistVideoDemo.cs b/Demos/SpecializedTests/Media/2.4/RopeTwistVideoDemo.cs index 64d6174be..c1e03e6b5 100644 --- a/Demos/SpecializedTests/Media/2.4/RopeTwistVideoDemo.cs +++ b/Demos/SpecializedTests/Media/2.4/RopeTwistVideoDemo.cs @@ -4,12 +4,9 @@ using BepuPhysics.Constraints; using DemoContentLoader; using DemoRenderer; -using DemoRenderer.UI; using Demos.Demos; -using DemoUtilities; using System; using System.Numerics; -using System.Runtime.CompilerServices; namespace Demos.SpecializedTests.Media; diff --git a/Demos/SpecializedTests/Media/2.4/VideoDancerDemo.cs b/Demos/SpecializedTests/Media/2.4/VideoDancerDemo.cs index ee0f40809..c610e5e22 100644 --- a/Demos/SpecializedTests/Media/2.4/VideoDancerDemo.cs +++ b/Demos/SpecializedTests/Media/2.4/VideoDancerDemo.cs @@ -1,9 +1,7 @@ using BepuPhysics; using BepuPhysics.Collidables; -using BepuPhysics.CollisionDetection; using BepuPhysics.Constraints; using BepuUtilities; -using BepuUtilities.Memory; using DemoContentLoader; using DemoRenderer; using DemoRenderer.UI; @@ -12,7 +10,6 @@ using DemoUtilities; using System; using System.Numerics; -using System.Runtime.InteropServices; namespace Demos.SpecializedTests.Media { diff --git a/Demos/SpecializedTests/Media/2.4/VideoPlumpDancerDemo.cs b/Demos/SpecializedTests/Media/2.4/VideoPlumpDancerDemo.cs index 7ac05ff5b..29e75ec68 100644 --- a/Demos/SpecializedTests/Media/2.4/VideoPlumpDancerDemo.cs +++ b/Demos/SpecializedTests/Media/2.4/VideoPlumpDancerDemo.cs @@ -1,9 +1,7 @@ using BepuPhysics; using BepuPhysics.Collidables; -using BepuPhysics.CollisionDetection; using BepuPhysics.Constraints; using BepuUtilities; -using BepuUtilities.Memory; using DemoContentLoader; using DemoRenderer; using DemoRenderer.UI; @@ -13,7 +11,6 @@ using System; using System.Diagnostics; using System.Numerics; -using System.Runtime.InteropServices; namespace Demos.SpecializedTests.Media { diff --git a/Demos/SpecializedTests/MeshMeshTestDemo.cs b/Demos/SpecializedTests/MeshMeshTestDemo.cs index 894d2efd4..164579779 100644 --- a/Demos/SpecializedTests/MeshMeshTestDemo.cs +++ b/Demos/SpecializedTests/MeshMeshTestDemo.cs @@ -1,14 +1,10 @@ -using System; -using System.Collections.Generic; -using System.Numerics; -using System.Text; +using System.Numerics; using BepuPhysics; using BepuPhysics.Collidables; using BepuPhysics.Constraints; using BepuUtilities; using DemoContentLoader; using DemoRenderer; -using Demos.Demos; namespace Demos.SpecializedTests { diff --git a/Demos/SpecializedTests/MeshReductionTestDemo.cs b/Demos/SpecializedTests/MeshReductionTestDemo.cs index 7c58de6d3..c6d79f67b 100644 --- a/Demos/SpecializedTests/MeshReductionTestDemo.cs +++ b/Demos/SpecializedTests/MeshReductionTestDemo.cs @@ -5,12 +5,8 @@ using BepuPhysics.Constraints; using BepuUtilities; using BepuUtilities.Collections; -using BepuUtilities.Memory; using DemoContentLoader; using DemoRenderer; -using DemoRenderer.UI; -using DemoUtilities; -using OpenTK.Input; namespace Demos.SpecializedTests { diff --git a/Demos/SpecializedTests/MeshTestDemo.cs b/Demos/SpecializedTests/MeshTestDemo.cs index 586660e95..236ab1680 100644 --- a/Demos/SpecializedTests/MeshTestDemo.cs +++ b/Demos/SpecializedTests/MeshTestDemo.cs @@ -1,12 +1,9 @@ using BepuUtilities; using DemoRenderer; -using DemoUtilities; using BepuPhysics; using BepuPhysics.Collidables; using System; using System.Numerics; -using System.Diagnostics; -using BepuUtilities.Collections; using DemoContentLoader; using BepuPhysics.Constraints; diff --git a/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs b/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs index 2d06fc5df..e5e9bd426 100644 --- a/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs +++ b/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs @@ -4,12 +4,9 @@ using BepuUtilities; using DemoContentLoader; using DemoRenderer; -using DemoRenderer.UI; using DemoUtilities; using System; -using System.Collections.Generic; using System.Numerics; -using System.Text; namespace Demos.Demos { diff --git a/Demos/SpecializedTests/ScalarIntegrationTestDemo.cs b/Demos/SpecializedTests/ScalarIntegrationTestDemo.cs index e0e5695a1..6d975ee85 100644 --- a/Demos/SpecializedTests/ScalarIntegrationTestDemo.cs +++ b/Demos/SpecializedTests/ScalarIntegrationTestDemo.cs @@ -3,15 +3,9 @@ using BepuPhysics; using BepuUtilities; using DemoContentLoader; -using DemoRenderer.UI; using DemoRenderer; -using DemoUtilities; using System; -using System.Collections.Generic; -using System.Linq; using System.Numerics; -using System.Text; -using System.Threading.Tasks; using BepuUtilities.Collections; namespace Demos.SpecializedTests diff --git a/Demos/SpecializedTests/ShapePileTestDemo.cs b/Demos/SpecializedTests/ShapePileTestDemo.cs index 1a5219ef2..ba04b966f 100644 --- a/Demos/SpecializedTests/ShapePileTestDemo.cs +++ b/Demos/SpecializedTests/ShapePileTestDemo.cs @@ -1,12 +1,9 @@ using BepuUtilities; using DemoRenderer; -using DemoUtilities; using BepuPhysics; using BepuPhysics.Collidables; using System; using System.Numerics; -using System.Diagnostics; -using BepuUtilities.Memory; using BepuUtilities.Collections; using DemoContentLoader; using BepuPhysics.Constraints; diff --git a/Demos/SpecializedTests/SimulationScrambling.cs b/Demos/SpecializedTests/SimulationScrambling.cs index f0545ecb7..fc060c5a2 100644 --- a/Demos/SpecializedTests/SimulationScrambling.cs +++ b/Demos/SpecializedTests/SimulationScrambling.cs @@ -1,14 +1,10 @@ using BepuPhysics; -using BepuPhysics.Collidables; -using BepuPhysics.CollisionDetection; using BepuPhysics.Constraints; using BepuUtilities; using System; using System.Collections.Generic; using System.Diagnostics; -using System.Reflection.Metadata; using System.Runtime.CompilerServices; -using System.Text; namespace Demos.SpecializedTests { diff --git a/Demos/SpecializedTests/SolverBatchTestDemo.cs b/Demos/SpecializedTests/SolverBatchTestDemo.cs index 0c2537f99..b69942444 100644 --- a/Demos/SpecializedTests/SolverBatchTestDemo.cs +++ b/Demos/SpecializedTests/SolverBatchTestDemo.cs @@ -5,9 +5,6 @@ using BepuPhysics.Collidables; using System; using System.Numerics; -using System.Diagnostics; -using BepuUtilities.Memory; -using BepuUtilities.Collections; using BepuPhysics.Constraints; using DemoContentLoader; diff --git a/Demos/SpecializedTests/TestHelpers.cs b/Demos/SpecializedTests/TestHelpers.cs index bf4ff9807..d5a4b3eae 100644 --- a/Demos/SpecializedTests/TestHelpers.cs +++ b/Demos/SpecializedTests/TestHelpers.cs @@ -1,9 +1,7 @@ using BepuPhysics; using BepuUtilities; using System; -using System.Collections.Generic; using System.Numerics; -using System.Text; namespace Demos.SpecializedTests { diff --git a/Demos/SpecializedTests/TreeTest.cs b/Demos/SpecializedTests/TreeTest.cs index 33f54ccbc..8910c25e0 100644 --- a/Demos/SpecializedTests/TreeTest.cs +++ b/Demos/SpecializedTests/TreeTest.cs @@ -1,14 +1,9 @@ using BepuUtilities; using BepuUtilities.Collections; using BepuUtilities.Memory; -using BepuPhysics.CollisionDetection; using System; -using System.Collections.Generic; -using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; -using System.Runtime.InteropServices; -using System.Text; using BepuPhysics.Trees; namespace Demos.SpecializedTests diff --git a/Demos/SpecializedTests/TriangleRayTestDemo.cs b/Demos/SpecializedTests/TriangleRayTestDemo.cs index d31c12396..3dc5079eb 100644 --- a/Demos/SpecializedTests/TriangleRayTestDemo.cs +++ b/Demos/SpecializedTests/TriangleRayTestDemo.cs @@ -1,17 +1,12 @@ using BepuPhysics; using BepuPhysics.Collidables; -using BepuPhysics.CollisionDetection; using BepuPhysics.Constraints; using BepuUtilities; using DemoContentLoader; using DemoRenderer; -using DemoRenderer.UI; -using DemoUtilities; using System; -using System.Collections.Generic; using System.Diagnostics; using System.Numerics; -using System.Text; namespace Demos.SpecializedTests { diff --git a/Demos/SpecializedTests/TriangleTestDemo.cs b/Demos/SpecializedTests/TriangleTestDemo.cs index 0d9c8b245..081162a06 100644 --- a/Demos/SpecializedTests/TriangleTestDemo.cs +++ b/Demos/SpecializedTests/TriangleTestDemo.cs @@ -5,8 +5,6 @@ using BepuPhysics.Collidables; using System; using System.Numerics; -using System.Diagnostics; -using BepuUtilities.Memory; using BepuUtilities.Collections; using BepuPhysics.CollisionDetection.CollisionTasks; using DemoContentLoader; diff --git a/Demos/SpecializedTests/VolumeQueryTests.cs b/Demos/SpecializedTests/VolumeQueryTests.cs index 6ae34526a..bc7ccfe91 100644 --- a/Demos/SpecializedTests/VolumeQueryTests.cs +++ b/Demos/SpecializedTests/VolumeQueryTests.cs @@ -10,11 +10,8 @@ using BepuUtilities.Collections; using System.Runtime.CompilerServices; using BepuPhysics.CollisionDetection; -using BepuPhysics.Trees; using DemoRenderer.UI; -using DemoRenderer.Constraints; using System.Threading; -using Demos.SpecializedTests; using DemoContentLoader; namespace Demos.SpecializedTests diff --git a/Demos/UI/DemoSwapper.cs b/Demos/UI/DemoSwapper.cs index 1af561503..0a974be45 100644 --- a/Demos/UI/DemoSwapper.cs +++ b/Demos/UI/DemoSwapper.cs @@ -1,10 +1,7 @@ using DemoRenderer.UI; using DemoUtilities; using System; -using System.Collections.Generic; -using System.Diagnostics; using System.Numerics; -using System.Text; namespace Demos.UI { diff --git a/Demos/UI/Graph.cs b/Demos/UI/Graph.cs index e29c50bea..352bce8ab 100644 --- a/Demos/UI/Graph.cs +++ b/Demos/UI/Graph.cs @@ -1,13 +1,8 @@ -using BepuUtilities; -using BepuUtilities.Collections; -using BepuUtilities.Memory; -using DemoRenderer.UI; +using DemoRenderer.UI; using DemoUtilities; using System; using System.Collections.Generic; -using System.Diagnostics; using System.Numerics; -using System.Text; namespace Demos.UI { From edbedd16870653397bf1bec35fca27237d705812 Mon Sep 17 00:00:00 2001 From: Salvage <29021710+Saalvage@users.noreply.github.com> Date: Mon, 28 Aug 2023 00:39:12 +0200 Subject: [PATCH 807/947] Remove unneeded `unsafe` specifiers --- BepuPhysics/BatchCompressor.cs | 9 ++--- BepuPhysics/Bodies.cs | 6 +-- BepuPhysics/BodyReference.cs | 2 +- BepuPhysics/BodySet.cs | 4 +- BepuPhysics/BoundingBoxHelpers.cs | 10 ++--- BepuPhysics/Collidables/BigCompound.cs | 2 +- BepuPhysics/Collidables/BoundingBoxBatcher.cs | 2 +- BepuPhysics/Collidables/ConvexHullHelper.cs | 2 +- BepuPhysics/Collidables/Mesh.cs | 10 ++--- BepuPhysics/Collidables/Shapes.cs | 2 +- .../CollisionDetection/BroadPhase_Queries.cs | 10 ++--- .../CollidableOverlapFinder.cs | 11 +++-- .../CollisionDetection/CollisionBatcher.cs | 12 +++--- .../CollisionBatcherContinuations.cs | 10 ++--- .../CollisionTaskRegistry.cs | 4 +- .../CollisionTasks/BoxCylinderTester.cs | 2 +- .../CompoundMeshContinuations.cs | 4 +- .../CompoundPairContinuations.cs | 4 +- .../CollisionTasks/CompoundPairOverlaps.cs | 4 +- .../ConvexCompoundOverlapFinder.cs | 4 +- .../ConvexCompoundTaskOverlaps.cs | 4 +- .../CollisionTasks/ManifoldCandidateHelper.cs | 2 +- .../CollisionTasks/MeshPairContinuations.cs | 4 +- .../CompoundMeshReduction.cs | 4 +- .../ContactConstraintAccessor.cs | 6 +-- .../CollisionDetection/ContactManifold.cs | 2 +- .../CollisionDetection/FreshnessChecker.cs | 2 +- .../INarrowPhaseCallbacks.cs | 2 +- .../CollisionDetection/MeshReduction.cs | 10 ++--- BepuPhysics/CollisionDetection/NarrowPhase.cs | 6 +-- .../NarrowPhaseCCDContinuations.cs | 4 +- .../NarrowPhaseConstraintUpdate.cs | 6 +-- .../NarrowPhasePendingConstraintAdds.cs | 4 +- .../CollisionDetection/NarrowPhasePreflush.cs | 6 +-- .../CollisionDetection/NonconvexReduction.cs | 2 +- BepuPhysics/CollisionDetection/PairCache.cs | 10 ++--- .../CollisionDetection/PairCache_Activity.cs | 6 +-- BepuPhysics/CollisionDetection/RayBatchers.cs | 2 +- BepuPhysics/CollisionDetection/UntypedList.cs | 6 +-- .../CollisionDetection/WorkerPairCache.cs | 2 +- BepuPhysics/ConstraintBatch.cs | 2 +- .../Constraints/FourBodyTypeProcessor.cs | 4 +- .../Constraints/OneBodyTypeProcessor.cs | 4 +- .../Constraints/TwoBodyTypeProcessor.cs | 6 +-- BepuPhysics/Constraints/TypeProcessor.cs | 40 +++++++++---------- BepuPhysics/IslandAwakener.cs | 6 +-- BepuPhysics/IslandSleeper.cs | 7 ++-- BepuPhysics/PoseIntegrator.cs | 10 ++--- BepuPhysics/SequentialFallbackBatch.cs | 10 ++--- BepuPhysics/Simulation_Queries.cs | 4 +- BepuPhysics/Solver.cs | 40 +++++++++---------- BepuPhysics/Solver_Solve.cs | 9 +++-- BepuPhysics/Statics.cs | 6 +-- BepuPhysics/Trees/Node.cs | 4 +- BepuPhysics/Trees/RayBatcher.cs | 4 +- BepuPhysics/Trees/Tree.cs | 4 +- BepuPhysics/Trees/Tree_Add.cs | 10 ++--- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 10 +++-- BepuPhysics/Trees/Tree_CacheOptimizer.cs | 12 +++--- BepuPhysics/Trees/Tree_Diagnostics.cs | 20 +++++----- BepuPhysics/Trees/Tree_IntertreeQueries.cs | 15 +++---- BepuPhysics/Trees/Tree_IntertreeQueriesMT.cs | 14 +++---- .../Trees/Tree_MultithreadedRefitRefine.cs | 20 +++++----- BepuPhysics/Trees/Tree_Refine2.cs | 9 +++-- BepuPhysics/Trees/Tree_RefineCommon.cs | 2 +- .../Trees/Tree_RefinementScheduling.cs | 13 +++--- BepuPhysics/Trees/Tree_Refit.cs | 6 +-- BepuPhysics/Trees/Tree_Refit2.cs | 10 ++--- BepuPhysics/Trees/Tree_Remove.cs | 6 +-- BepuPhysics/Trees/Tree_SelfQueries.cs | 23 ++++++----- BepuPhysics/Trees/Tree_SelfQueriesMT.cs | 14 +++---- BepuPhysics/Trees/Tree_Sweep.cs | 4 +- BepuPhysics/Trees/Tree_VolumeQuery.cs | 2 +- BepuUtilities/BoundingBox.cs | 8 ++-- BepuUtilities/Collections/IndexSet.cs | 2 +- BepuUtilities/GatherScatter.cs | 2 +- BepuUtilities/Int3.cs | 2 +- BepuUtilities/Int4.cs | 2 +- BepuUtilities/Memory/BufferPool.cs | 12 +++--- BepuUtilities/Symmetric3x3.cs | 4 +- .../TaskScheduling/TaskContinuation.cs | 2 +- BepuUtilities/TaskScheduling/TaskStack.cs | 4 +- DemoContentBuilder/MeshBuilder.cs | 2 +- DemoContentBuilder/ProjectBuilder.cs | 2 +- DemoContentLoader/Texture2DIO.cs | 2 +- DemoRenderer.GL/ConstantsBuffer.cs | 2 +- DemoRenderer.GL/Helpers.cs | 4 +- .../ShapeDrawing/ShapesExtractor.cs | 2 +- .../Constraints/BoundingBoxLineExtractor.cs | 2 +- Demos/DemoCallbacks.cs | 4 +- Demos/Demos/BlockChainDemo.cs | 2 +- Demos/Demos/BouncinessDemo.cs | 6 +-- Demos/Demos/Cars/CarCallbacks.cs | 4 +- Demos/Demos/ChainFountainDemo.cs | 2 +- .../Demos/Characters/CharacterControllers.cs | 11 +++-- Demos/Demos/Characters/CharacterDemo.cs | 2 +- .../CharacterNarrowphaseCallbacks.cs | 4 +- Demos/Demos/ClothDemo.cs | 6 +-- Demos/Demos/CollisionQueryDemo.cs | 2 +- Demos/Demos/CollisionTrackingDemo.cs | 4 +- Demos/Demos/ColosseumDemo.cs | 2 +- Demos/Demos/CompoundDemo.cs | 2 +- Demos/Demos/ContactEventsDemo.cs | 6 +-- Demos/Demos/CustomVoxelCollidableDemo.cs | 10 ++--- Demos/Demos/Dancers/DancerDemo.cs | 4 +- Demos/Demos/Dancers/PlumpDancerDemo.cs | 4 +- Demos/Demos/FrictionDemo.cs | 6 +-- Demos/Demos/NewtDemo.cs | 12 +++--- Demos/Demos/PerBodyGravityDemo.cs | 2 +- Demos/Demos/PlanetDemo.cs | 2 +- Demos/Demos/PyramidDemo.cs | 2 +- Demos/Demos/RagdollDemo.cs | 6 +-- Demos/Demos/RagdollTubeDemo.cs | 2 +- Demos/Demos/RayCastingDemo.cs | 10 ++--- Demos/Demos/RopeStabilityDemo.cs | 2 +- Demos/Demos/RopeTwistDemo.cs | 6 +-- Demos/Demos/SimpleSelfContainedDemo.cs | 4 +- Demos/Demos/SubsteppingDemo.cs | 2 +- Demos/Demos/SweepDemo.cs | 6 +-- Demos/Demos/Tanks/TankCallbacks.cs | 4 +- .../SpecializedTests/BatchedCollisionTests.cs | 6 +-- .../BroadPhaseStressTestDemo.cs | 6 +-- Demos/SpecializedTests/CapsuleTestDemo.cs | 2 +- Demos/SpecializedTests/CharacterTestDemo.cs | 2 +- Demos/SpecializedTests/ClothLatticeDemo.cs | 2 +- Demos/SpecializedTests/CompoundBoundTests.cs | 4 +- .../CompoundCollisionIndicesTest.cs | 4 +- .../ConstrainedKinematicIntegrationTest.cs | 2 +- Demos/SpecializedTests/ConstraintTestDemo.cs | 2 +- Demos/SpecializedTests/ConvexHullTestDemo.cs | 16 ++++---- .../FountainStressTestDemo.cs | 2 +- .../Media/2.0/BedsheetDemo.cs | 2 +- .../Media/2.0/ColosseumVideoDemo.cs | 2 +- .../2.0/NewtDemandingSacrificeVideoDemo.cs | 2 +- .../Media/2.0/NewtVideoDemo.cs | 2 +- .../Media/2.0/PyramidVideoDemo.cs | 2 +- .../Media/2.0/ShrinkwrappedNewtsVideoDemo.cs | 2 +- .../Media/2.4/Colosseum24VideoDemo.cs | 2 +- .../Media/2.4/ExcessivePyramidVideoDemo.cs | 2 +- .../Media/2.4/RagdollTubeVideoDemo.cs | 2 +- .../Media/2.4/RopeTwistVideoDemo.cs | 2 +- .../Media/2.4/VideoDancerDemo.cs | 4 +- .../Media/2.4/VideoPlumpDancerDemo.cs | 4 +- Demos/SpecializedTests/MeshTestDemo.cs | 2 +- Demos/SpecializedTests/NewtonsCradleDemo.cs | 2 +- .../PyramidAwakenerTestDemo.cs | 2 +- .../ScalarIntegrationTestDemo.cs | 2 +- Demos/SpecializedTests/ShapePileTestDemo.cs | 2 +- Demos/SpecializedTests/SolverBatchTestDemo.cs | 2 +- .../SpecializedTests/TreeFiddlingTestDemo.cs | 2 +- Demos/SpecializedTests/TriangleRayTestDemo.cs | 2 +- Demos/SpecializedTests/VolumeQueryTests.cs | 10 ++--- 152 files changed, 430 insertions(+), 421 deletions(-) diff --git a/BepuPhysics/BatchCompressor.cs b/BepuPhysics/BatchCompressor.cs index e6500dec8..e622ec56e 100644 --- a/BepuPhysics/BatchCompressor.cs +++ b/BepuPhysics/BatchCompressor.cs @@ -107,8 +107,7 @@ public BatchCompressor(Solver solver, Bodies bodies, float targetCandidateFracti } - - unsafe void AnalysisWorker(int workerIndex) + void AnalysisWorker(int workerIndex) { int jobIndex; while ((jobIndex = Interlocked.Increment(ref analysisJobIndex)) < analysisJobs.Count) @@ -126,7 +125,7 @@ unsafe void AnalysisWorker(int workerIndex) [MethodImpl(MethodImplOptions.AggressiveInlining)] - private unsafe void TryToFindBetterBatchForConstraint( + private void TryToFindBetterBatchForConstraint( BufferPool pool, ref QuickList compressions, ref TypeBatch typeBatch, int* bodyHandles, ref ActiveConstraintDynamicBodyHandleCollector handleAccumulator, TypeProcessor typeProcessor, int constraintIndex) { handleAccumulator.Count = 0; @@ -144,7 +143,7 @@ private unsafe void TryToFindBetterBatchForConstraint( } - unsafe void DoJob(ref AnalysisRegion region, int workerIndex, BufferPool pool) + void DoJob(ref AnalysisRegion region, int workerIndex, BufferPool pool) { ref var compressions = ref this.workerCompressions[workerIndex]; ref var batch = ref Solver.ActiveSet.Batches[nextBatchIndex]; @@ -196,7 +195,7 @@ public int Compare(ref CompressionTarget a, ref CompressionTarget b) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - private unsafe void ApplyCompression(int sourceBatchIndex, ref ConstraintBatch sourceBatch, ref Compression compression) + private void ApplyCompression(int sourceBatchIndex, ref ConstraintBatch sourceBatch, ref Compression compression) { var constraintLocation = Solver.HandleToConstraint[compression.ConstraintHandle.Value]; var typeProcessor = Solver.TypeProcessors[constraintLocation.TypeId]; diff --git a/BepuPhysics/Bodies.cs b/BepuPhysics/Bodies.cs index 161b9869a..fe0148ed4 100644 --- a/BepuPhysics/Bodies.cs +++ b/BepuPhysics/Bodies.cs @@ -92,7 +92,7 @@ public BodyReference this[BodyHandle handle] /// Initial number of bodies to allocate space for in the active set. /// Initial number of islands to allocate space for in the Sets buffer. /// Expected number of constraint references per body to allocate space for. - public unsafe Bodies(BufferPool pool, Shapes shapes, BroadPhase broadPhase, + public Bodies(BufferPool pool, Shapes shapes, BroadPhase broadPhase, int initialBodyCapacity, int initialIslandCapacity, int initialConstraintCapacityPerBody) { this.Pool = pool; @@ -180,7 +180,7 @@ void RemoveCollidableFromBroadPhase(ref Collidable collidable) /// public const int CollidablesPerFlush = 16; - public unsafe BoundingBoxBatcher(Bodies bodies, Shapes shapes, BroadPhase broadPhase, BufferPool pool, float dt) + public BoundingBoxBatcher(Bodies bodies, Shapes shapes, BroadPhase broadPhase, BufferPool pool, float dt) { this.bodies = bodies; this.shapes = shapes; diff --git a/BepuPhysics/Collidables/ConvexHullHelper.cs b/BepuPhysics/Collidables/ConvexHullHelper.cs index 358c9e910..dfe1ba510 100644 --- a/BepuPhysics/Collidables/ConvexHullHelper.cs +++ b/BepuPhysics/Collidables/ConvexHullHelper.cs @@ -619,7 +619,7 @@ static void AddIfNotPresent(ref QuickList list, int value, BufferPool pool) /// Point set to compute the convex hull of. /// Buffer pool to pull memory from when creating the hull. /// Convex hull of the input point set. - public unsafe static void ComputeHull(Span points, BufferPool pool, out HullData hullData)//, out List steps) + public static void ComputeHull(Span points, BufferPool pool, out HullData hullData)//, out List steps) { //steps = new List(); if (points.Length <= 0) diff --git a/BepuPhysics/Collidables/Mesh.cs b/BepuPhysics/Collidables/Mesh.cs index 7be62e6f4..903467461 100644 --- a/BepuPhysics/Collidables/Mesh.cs +++ b/BepuPhysics/Collidables/Mesh.cs @@ -201,7 +201,7 @@ public readonly unsafe void Serialize(Span data) public readonly int ChildCount => Triangles.Length; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public readonly unsafe void GetLocalChild(int triangleIndex, out Triangle target) + public readonly void GetLocalChild(int triangleIndex, out Triangle target) { ref var source = ref Triangles[triangleIndex]; target.A = scale * source.A; @@ -210,7 +210,7 @@ public readonly unsafe void GetLocalChild(int triangleIndex, out Triangle target } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public readonly unsafe void GetPosedLocalChild(int triangleIndex, out Triangle target, out RigidPose childPose) + public readonly void GetPosedLocalChild(int triangleIndex, out Triangle target, out RigidPose childPose) { GetLocalChild(triangleIndex, out target); childPose = (target.A + target.B + target.C) * (1f / 3f); @@ -220,7 +220,7 @@ public readonly unsafe void GetPosedLocalChild(int triangleIndex, out Triangle t } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public readonly unsafe void GetLocalChild(int triangleIndex, ref TriangleWide target) + public readonly void GetLocalChild(int triangleIndex, ref TriangleWide target) { //This inserts a triangle into the first slot of the given wide instance. ref var source = ref Triangles[triangleIndex]; @@ -268,7 +268,7 @@ unsafe struct HitLeafTester : IRayLeafTester where T : IShapeRayHitHandler public RayData OriginalRay; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void TestLeaf(int leafIndex, RayData* rayData, float* maximumT) + public void TestLeaf(int leafIndex, RayData* rayData, float* maximumT) { ref var triangle = ref Triangles[leafIndex]; if (Triangle.RayTest(triangle.A, triangle.B, triangle.C, rayData->Origin, rayData->Direction, out var t, out var normal) && t <= *maximumT) @@ -402,7 +402,7 @@ public bool GetNextTriangle(out Vector3 a, out Vector3 b, out Vector3 c) /// Subtracts the newCenter from all points in the mesh hull. /// /// New center that all points will be made relative to. - public unsafe void Recenter(Vector3 newCenter) + public void Recenter(Vector3 newCenter) { var scaledOffset = newCenter * inverseScale; for (int i = 0; i < Triangles.Length; ++i) diff --git a/BepuPhysics/Collidables/Shapes.cs b/BepuPhysics/Collidables/Shapes.cs index 5ff9cef38..364c8f4a2 100644 --- a/BepuPhysics/Collidables/Shapes.cs +++ b/BepuPhysics/Collidables/Shapes.cs @@ -274,7 +274,7 @@ public override void RayTest(int shapeIndex, in RigidPose pose, } } - public unsafe override void RayTest(int index, in RigidPose pose, ref RaySource rays, ref TRayHitHandler hitHandler) + public override void RayTest(int index, in RigidPose pose, ref RaySource rays, ref TRayHitHandler hitHandler) { WideRayTester.Test(ref shapes[index], pose, ref rays, ref hitHandler); } diff --git a/BepuPhysics/CollisionDetection/BroadPhase_Queries.cs b/BepuPhysics/CollisionDetection/BroadPhase_Queries.cs index c913418df..aa701bba8 100644 --- a/BepuPhysics/CollisionDetection/BroadPhase_Queries.cs +++ b/BepuPhysics/CollisionDetection/BroadPhase_Queries.cs @@ -12,7 +12,7 @@ namespace BepuPhysics.CollisionDetection /// public interface IBroadPhaseSweepTester { - unsafe void Test(CollidableReference collidable, ref float maximumT); + void Test(CollidableReference collidable, ref float maximumT); } partial class BroadPhase @@ -57,7 +57,7 @@ struct SweepLeafTester : ISweepLeafTester where TSweepTester : IBr public Buffer Leaves; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void TestLeaf(int leafIndex, ref float maximumT) + public void TestLeaf(int leafIndex, ref float maximumT) { LeafTester.Test(Leaves[leafIndex], ref maximumT); } @@ -95,7 +95,7 @@ public unsafe void Sweep(Vector3 min, Vector3 max, Vector3 directi /// Direction along which to sweep the bounding box. /// Maximum length of the sweep in units of the direction's length. /// Callback to execute on sweep-leaf bounding box intersections. - public unsafe void Sweep(in BoundingBox boundingBox, Vector3 direction, float maximumT, ref TSweepTester sweepTester) where TSweepTester : IBroadPhaseSweepTester + public void Sweep(in BoundingBox boundingBox, Vector3 direction, float maximumT, ref TSweepTester sweepTester) where TSweepTester : IBroadPhaseSweepTester { Sweep(boundingBox.Min, boundingBox.Max, direction, maximumT, ref sweepTester); } @@ -119,7 +119,7 @@ public bool LoopBody(int i) /// Minimum bounds of the query box. /// Maximum bounds of the query box. /// Enumerator to call for overlaps. - public unsafe void GetOverlaps(Vector3 min, Vector3 max, ref TOverlapEnumerator overlapEnumerator) where TOverlapEnumerator : IBreakableForEach + public void GetOverlaps(Vector3 min, Vector3 max, ref TOverlapEnumerator overlapEnumerator) where TOverlapEnumerator : IBreakableForEach { BoxQueryEnumerator enumerator; enumerator.Enumerator = overlapEnumerator; @@ -137,7 +137,7 @@ public unsafe void GetOverlaps(Vector3 min, Vector3 max, ref /// Type of the enumerator to call for overlaps. /// Query box bounds. /// Enumerator to call for overlaps. - public unsafe void GetOverlaps(in BoundingBox boundingBox, ref TOverlapEnumerator overlapEnumerator) where TOverlapEnumerator : IBreakableForEach + public void GetOverlaps(in BoundingBox boundingBox, ref TOverlapEnumerator overlapEnumerator) where TOverlapEnumerator : IBreakableForEach { BoxQueryEnumerator enumerator; enumerator.Enumerator = overlapEnumerator; diff --git a/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs b/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs index 4b00767fe..fcdfeaee2 100644 --- a/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs +++ b/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs @@ -187,14 +187,15 @@ public void Handle(int indexA, int indexB, int workerIndex, object managedContex } } - unsafe struct SelfContext + struct SelfContext { public TaskStack* Stack; public ThreadedSelfOverlapHandler* Results; public Tree Tree; public int TargetTaskCount; } - unsafe struct IntertreeContext + + struct IntertreeContext { public TaskStack* Stack; public ThreadedIntertreeOverlapHandler* Results; @@ -202,14 +203,16 @@ unsafe struct IntertreeContext public Tree ActiveTree; public int TargetTaskCount; } - unsafe static void SelfEntryTask(long taskStartAndEnd, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) + + static void SelfEntryTask(long taskStartAndEnd, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) { Debug.Assert(dispatcher.ManagedContext != null); ref var context = ref *(SelfContext*)untypedContext; var pool = dispatcher.WorkerPools[workerIndex]; context.Tree.GetSelfOverlaps2(ref *context.Results, pool, dispatcher, context.Stack, workerIndex, context.TargetTaskCount); } - unsafe static void IntertreeEntryTask(long taskStartAndEnd, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) + + static void IntertreeEntryTask(long taskStartAndEnd, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) { Debug.Assert(dispatcher.ManagedContext != null); ref var context = ref *(IntertreeContext*)untypedContext; diff --git a/BepuPhysics/CollisionDetection/CollisionBatcher.cs b/BepuPhysics/CollisionDetection/CollisionBatcher.cs index 8b619ab16..ed5747eec 100644 --- a/BepuPhysics/CollisionDetection/CollisionBatcher.cs +++ b/BepuPhysics/CollisionDetection/CollisionBatcher.cs @@ -54,7 +54,7 @@ public struct CollisionBatcher where TCallbacks : struct, ICollision public BatcherContinuations MeshReductions; public BatcherContinuations CompoundMeshReductions; - public unsafe CollisionBatcher(BufferPool pool, Shapes shapes, CollisionTaskRegistry collisionTypeMatrix, float dt, TCallbacks callbacks) + public CollisionBatcher(BufferPool pool, Shapes shapes, CollisionTaskRegistry collisionTypeMatrix, float dt, TCallbacks callbacks) { Pool = pool; Shapes = shapes; @@ -72,7 +72,7 @@ public unsafe CollisionBatcher(BufferPool pool, Shapes shapes, CollisionTaskRegi } [MethodImpl(MethodImplOptions.AggressiveInlining)] - unsafe ref TPair AllocatePair(ref CollisionBatch batch, ref CollisionTaskReference reference) where TPair : ICollisionPair + ref TPair AllocatePair(ref CollisionBatch batch, ref CollisionTaskReference reference) where TPair : ICollisionPair { if (!batch.Pairs.Buffer.Allocated) { @@ -226,7 +226,7 @@ public unsafe void Add(TypedIndex shapeIndexA, TypedIndex shapeIndexB, } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void Add(TypedIndex shapeIndexA, TypedIndex shapeIndexB, Vector3 offsetB, Quaternion orientationA, Quaternion orientationB, + public void Add(TypedIndex shapeIndexA, TypedIndex shapeIndexB, Vector3 offsetB, Quaternion orientationA, Quaternion orientationB, float speculativeMargin, in PairContinuation continuation) { Add(shapeIndexA, shapeIndexB, offsetB, orientationA, orientationB, default, default, speculativeMargin, default, continuation); @@ -331,7 +331,7 @@ public void Flush() /// Unless you're building custom compound collision pairs or adding new contact processing continuations, you can safely ignore this. /// Contacts detected for the pair. /// Continuation describing the pair and what to do with it. - public unsafe void ProcessConvexResult(ref ConvexContactManifold manifold, ref PairContinuation continuation) + public void ProcessConvexResult(ref ConvexContactManifold manifold, ref PairContinuation continuation) { #if DEBUG if (manifold.Count > 0) @@ -376,7 +376,7 @@ public unsafe void ProcessConvexResult(ref ConvexContactManifold manifold, ref P /// /// Unless you're building custom compound collision pairs or adding new contact processing continuations, you can safely ignore this. /// Continuation describing the pair and what to do with it. - public unsafe void ProcessEmptyResult(ref PairContinuation continuation) + public void ProcessEmptyResult(ref PairContinuation continuation) { Unsafe.SkipInit(out ConvexContactManifold manifold); manifold.Count = 0; @@ -389,7 +389,7 @@ public unsafe void ProcessEmptyResult(ref PairContinuation continuation) /// /// Unless you're building custom compound collision pairs or adding new contact processing continuations, you can safely ignore this. /// Continuation describing the pair and what to do with it. - public unsafe void ProcessUntestedSubpairConvexResult(ref PairContinuation continuation) + public void ProcessUntestedSubpairConvexResult(ref PairContinuation continuation) { //Note that we do not call OnChildPairCompleted. A callback is only invoked if a child is actually tested. //If a child isn't considered- because acceleration structure pruned it, or a callback said to ignore it- there is no callback report. diff --git a/BepuPhysics/CollisionDetection/CollisionBatcherContinuations.cs b/BepuPhysics/CollisionDetection/CollisionBatcherContinuations.cs index bdb268f9f..f11b8dfbd 100644 --- a/BepuPhysics/CollisionDetection/CollisionBatcherContinuations.cs +++ b/BepuPhysics/CollisionDetection/CollisionBatcherContinuations.cs @@ -23,7 +23,7 @@ public interface ICollisionTestContinuation /// Continuation instance being considered. /// Contact manifold for the child pair. /// Collision batcher processing the pair. - unsafe void OnChildCompleted(ref PairContinuation report, ref ConvexContactManifold manifold, ref CollisionBatcher batcher) + void OnChildCompleted(ref PairContinuation report, ref ConvexContactManifold manifold, ref CollisionBatcher batcher) where TCallbacks : struct, ICollisionCallbacks; /// /// Handles what to do next when the child pair was rejected for testing, and no manifold exists. @@ -31,7 +31,7 @@ unsafe void OnChildCompleted(ref PairContinuation report, ref Convex /// Type of the callbacks used in the batcher. /// Continuation instance being considered. /// Collision batcher processing the pair. - unsafe void OnUntestedChildCompleted(ref PairContinuation report, ref CollisionBatcher batcher) + void OnUntestedChildCompleted(ref PairContinuation report, ref CollisionBatcher batcher) where TCallbacks : struct, ICollisionCallbacks; /// @@ -41,7 +41,7 @@ unsafe void OnUntestedChildCompleted(ref PairContinuation report, re /// Id of the pair to attempt to flush. /// Collision batcher processing the pair. /// True if the pair was done and got flushed, false otherwise. - unsafe bool TryFlush(int pairId, ref CollisionBatcher batcher) + bool TryFlush(int pairId, ref CollisionBatcher batcher) where TCallbacks : struct, ICollisionCallbacks; @@ -157,7 +157,7 @@ public ref T CreateContinuation(int slotsInContinuation, BufferPool pool, out in } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void ContributeChildToContinuation(ref PairContinuation continuation, ref ConvexContactManifold manifold, ref CollisionBatcher batcher) + public void ContributeChildToContinuation(ref PairContinuation continuation, ref ConvexContactManifold manifold, ref CollisionBatcher batcher) where TCallbacks : struct, ICollisionCallbacks { ref var slot = ref Continuations[continuation.Index]; @@ -170,7 +170,7 @@ public unsafe void ContributeChildToContinuation(ref PairContinuatio } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void ContributeUntestedChildToContinuation(ref PairContinuation continuation, ref CollisionBatcher batcher) + public void ContributeUntestedChildToContinuation(ref PairContinuation continuation, ref CollisionBatcher batcher) where TCallbacks : struct, ICollisionCallbacks { ref var slot = ref Continuations[continuation.Index]; diff --git a/BepuPhysics/CollisionDetection/CollisionTaskRegistry.cs b/BepuPhysics/CollisionDetection/CollisionTaskRegistry.cs index 8f513c464..d8353a7fd 100644 --- a/BepuPhysics/CollisionDetection/CollisionTaskRegistry.cs +++ b/BepuPhysics/CollisionDetection/CollisionTaskRegistry.cs @@ -17,7 +17,7 @@ public interface ICollisionCallbacks /// Type of the contact manifold generated by collision detection. /// Id of the pair that completed. /// Contact manifold generated by collision testing. - unsafe void OnPairCompleted(int pairId, ref TManifold manifold) where TManifold : unmanaged, IContactManifold; + void OnPairCompleted(int pairId, ref TManifold manifold) where TManifold : unmanaged, IContactManifold; /// /// Provides control over subtask generated results before they are reported to the parent task. @@ -26,7 +26,7 @@ public interface ICollisionCallbacks /// Index of the child belonging to collidable A in the subpair under consideration. /// Index of the child belonging to collidable B in the subpair under consideration. /// Manifold of the child pair to configure. - unsafe void OnChildPairCompleted(int pairId, int childA, int childB, ref ConvexContactManifold manifold); + void OnChildPairCompleted(int pairId, int childA, int childB, ref ConvexContactManifold manifold); /// /// Checks whether further collision testing should be performed for a given subtask. diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/BoxCylinderTester.cs b/BepuPhysics/CollisionDetection/CollisionTasks/BoxCylinderTester.cs index 1be6fdd1d..51f214ce2 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/BoxCylinderTester.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/BoxCylinderTester.cs @@ -55,7 +55,7 @@ internal static void AddCandidateForEdge(in Vector2Wide edgeStart, in Vector2Wid } [MethodImpl(MethodImplOptions.AggressiveInlining)] - internal static unsafe void GenerateInteriorPoints(in CylinderWide cylinder, in Vector3Wide cylinderLocalNormal, in Vector3Wide localClosestOnCylinder, out Vector2Wide interior0, out Vector2Wide interior1, out Vector2Wide interior2, out Vector2Wide interior3) + internal static void GenerateInteriorPoints(in CylinderWide cylinder, in Vector3Wide cylinderLocalNormal, in Vector3Wide localClosestOnCylinder, out Vector2Wide interior0, out Vector2Wide interior1, out Vector2Wide interior2, out Vector2Wide interior3) { //Assume we can just use the 4 local extreme points of the cylinder at first. //Then, if there is sufficient tilt, replace the closest extreme point to the deepest point with the deepest point. diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundMeshContinuations.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundMeshContinuations.cs index e23866182..afcd9b983 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundMeshContinuations.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundMeshContinuations.cs @@ -46,7 +46,7 @@ public ref CompoundMeshReduction CreateContinuation( } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void GetChildAData(ref CollisionBatcher collisionBatcher, ref CompoundMeshReduction continuation, in BoundsTestedPair pair, int childIndexA, + public void GetChildAData(ref CollisionBatcher collisionBatcher, ref CompoundMeshReduction continuation, in BoundsTestedPair pair, int childIndexA, out RigidPose childPoseA, out int childTypeA, out void* childShapeDataA) where TCallbacks : struct, ICollisionCallbacks { @@ -58,7 +58,7 @@ public unsafe void GetChildAData(ref CollisionBatcher co } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void ConfigureContinuationChild( + public void ConfigureContinuationChild( ref CollisionBatcher collisionBatcher, ref CompoundMeshReduction continuation, int continuationChildIndex, in BoundsTestedPair pair, int childIndexA, int childTypeA, int childIndexB, in RigidPose childPoseA, out RigidPose childPoseB, out int childTypeB, out void* childShapeDataB) where TCallbacks : struct, ICollisionCallbacks diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairContinuations.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairContinuations.cs index b7860ab68..600b7d8bb 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairContinuations.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairContinuations.cs @@ -19,7 +19,7 @@ public ref NonconvexReduction CreateContinuation( } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void GetChildAData(ref CollisionBatcher collisionBatcher, ref NonconvexReduction continuation, in BoundsTestedPair pair, int childIndexA, + public void GetChildAData(ref CollisionBatcher collisionBatcher, ref NonconvexReduction continuation, in BoundsTestedPair pair, int childIndexA, out RigidPose childPoseA, out int childTypeA, out void* childShapeDataA) where TCallbacks : struct, ICollisionCallbacks { @@ -31,7 +31,7 @@ public unsafe void GetChildAData(ref CollisionBatcher co } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void ConfigureContinuationChild( + public void ConfigureContinuationChild( ref CollisionBatcher collisionBatcher, ref NonconvexReduction continuation, int continuationChildIndex, in BoundsTestedPair pair, int childIndexA, int childTypeA, int childIndexB, in RigidPose childPoseA, out RigidPose childPoseB, out int childTypeB, out void* childShapeDataB) where TCallbacks : struct, ICollisionCallbacks diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlaps.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlaps.cs index 760864b75..fb5c170b5 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlaps.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlaps.cs @@ -15,14 +15,14 @@ public interface ICollisionTaskOverlaps where TSubpairOverlaps ref TSubpairOverlaps GetOverlapsForPair(int subpairIndex); } - public unsafe struct ChildOverlapsCollection : ICollisionTaskSubpairOverlaps + public struct ChildOverlapsCollection : ICollisionTaskSubpairOverlaps { public Buffer Overlaps; public int Count; public int ChildIndex; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe ref int Allocate(BufferPool pool) + public ref int Allocate(BufferPool pool) { if (Overlaps.Length == Count) { diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundOverlapFinder.cs b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundOverlapFinder.cs index f70657399..a4116c886 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundOverlapFinder.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundOverlapFinder.cs @@ -15,11 +15,11 @@ public unsafe struct OverlapQueryForPair public unsafe interface IBoundsQueryableCompound { - unsafe void FindLocalOverlaps(ref Buffer pairs, BufferPool pool, Shapes shapes, ref TOverlaps overlaps) + void FindLocalOverlaps(ref Buffer pairs, BufferPool pool, Shapes shapes, ref TOverlaps overlaps) where TOverlaps : struct, ICollisionTaskOverlaps where TSubpairOverlaps : struct, ICollisionTaskSubpairOverlaps; - unsafe void FindLocalOverlaps(Vector3 min, Vector3 max, Vector3 sweep, float maximumT, BufferPool pool, Shapes shapes, void* overlaps) + void FindLocalOverlaps(Vector3 min, Vector3 max, Vector3 sweep, float maximumT, BufferPool pool, Shapes shapes, void* overlaps) where TOverlaps : ICollisionTaskSubpairOverlaps; } public interface IConvexCompoundOverlapFinder diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundTaskOverlaps.cs b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundTaskOverlaps.cs index d814cc6e0..53cec22c0 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundTaskOverlaps.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundTaskOverlaps.cs @@ -4,13 +4,13 @@ namespace BepuPhysics.CollisionDetection.CollisionTasks { - public unsafe struct ConvexCompoundOverlaps : ICollisionTaskSubpairOverlaps + public struct ConvexCompoundOverlaps : ICollisionTaskSubpairOverlaps { public Buffer Overlaps; public int Count; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe ref int Allocate(BufferPool pool) + public ref int Allocate(BufferPool pool) { if (Overlaps.Length == Count) { diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/ManifoldCandidateHelper.cs b/BepuPhysics/CollisionDetection/CollisionTasks/ManifoldCandidateHelper.cs index 0c6a6d25a..c74976a19 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/ManifoldCandidateHelper.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/ManifoldCandidateHelper.cs @@ -283,7 +283,7 @@ public static void ReduceWithoutComputingDepths(ref ManifoldCandidate candidates } [MethodImpl(MethodImplOptions.AggressiveInlining)] - unsafe static void PlaceCandidateInSlot(in ManifoldCandidateScalar candidate, int contactIndex, + static void PlaceCandidateInSlot(in ManifoldCandidateScalar candidate, int contactIndex, Vector3 faceCenterB, Vector3 faceBX, Vector3 faceBY, float depth, in Matrix3x3 orientationB, Vector3 offsetB, ref Convex4ContactManifoldWide manifoldSlot) { diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/MeshPairContinuations.cs b/BepuPhysics/CollisionDetection/CollisionTasks/MeshPairContinuations.cs index 9971ace54..7afce4817 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/MeshPairContinuations.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/MeshPairContinuations.cs @@ -50,7 +50,7 @@ public ref CompoundMeshReduction CreateContinuation( } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void GetChildAData(ref CollisionBatcher collisionBatcher, ref CompoundMeshReduction continuation, in BoundsTestedPair pair, int childIndexA, + public void GetChildAData(ref CollisionBatcher collisionBatcher, ref CompoundMeshReduction continuation, in BoundsTestedPair pair, int childIndexA, out RigidPose childPoseA, out int childTypeA, out void* childShapeDataA) where TCallbacks : struct, ICollisionCallbacks { @@ -62,7 +62,7 @@ public unsafe void GetChildAData(ref CollisionBatcher co } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void ConfigureContinuationChild( + public void ConfigureContinuationChild( ref CollisionBatcher collisionBatcher, ref CompoundMeshReduction continuation, int continuationChildIndex, in BoundsTestedPair pair, int childIndexA, int childTypeA, int childIndexB, in RigidPose childPoseA, out RigidPose childPoseB, out int childTypeB, out void* childShapeDataB) where TCallbacks : struct, ICollisionCallbacks diff --git a/BepuPhysics/CollisionDetection/CompoundMeshReduction.cs b/BepuPhysics/CollisionDetection/CompoundMeshReduction.cs index d0a790cae..23ba9c3cc 100644 --- a/BepuPhysics/CollisionDetection/CompoundMeshReduction.cs +++ b/BepuPhysics/CollisionDetection/CompoundMeshReduction.cs @@ -29,7 +29,7 @@ public void Create(int childManifoldCount, BufferPool pool) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void OnChildCompleted(ref PairContinuation report, ref ConvexContactManifold manifold, ref CollisionBatcher batcher) + public void OnChildCompleted(ref PairContinuation report, ref ConvexContactManifold manifold, ref CollisionBatcher batcher) where TCallbacks : struct, ICollisionCallbacks { Inner.OnChildCompleted(ref report, ref manifold, ref batcher); @@ -41,7 +41,7 @@ public void OnUntestedChildCompleted(ref PairContinuation report, re Inner.OnUntestedChildCompleted(ref report, ref batcher); } - public unsafe bool TryFlush(int pairId, ref CollisionBatcher batcher) where TCallbacks : struct, ICollisionCallbacks + public bool TryFlush(int pairId, ref CollisionBatcher batcher) where TCallbacks : struct, ICollisionCallbacks { Debug.Assert(Inner.ChildCount > 0); if (Inner.CompletedChildCount == Inner.ChildCount) diff --git a/BepuPhysics/CollisionDetection/ContactConstraintAccessor.cs b/BepuPhysics/CollisionDetection/ContactConstraintAccessor.cs index 16c0a438b..def003dd7 100644 --- a/BepuPhysics/CollisionDetection/ContactConstraintAccessor.cs +++ b/BepuPhysics/CollisionDetection/ContactConstraintAccessor.cs @@ -48,7 +48,7 @@ public unsafe void GatherOldImpulses(ref ConstraintReference constraintReference } } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void ScatterNewImpulses(ref ConstraintReference constraintReference, ref TContactImpulses contactImpulses) + public void ScatterNewImpulses(ref ConstraintReference constraintReference, ref TContactImpulses contactImpulses) { //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. @@ -88,7 +88,7 @@ public abstract void FlushWithSpeculativeBatches(ref UntypedList lis public abstract void FlushSequentially(ref UntypedList list, int narrowPhaseConstraintTypeId, Simulation simulation, PairCache pairCache) where TCallbacks : struct, INarrowPhaseCallbacks; - public abstract unsafe void UpdateConstraintForManifold( + public abstract void UpdateConstraintForManifold( NarrowPhase narrowPhase, int manifoldTypeAsConstraintType, int workerIndex, ref CollidablePair pair, ref TContactManifold manifoldPointer, ref PairMaterialProperties material, TCallBodyHandles bodyHandles) where TCallbacks : struct, INarrowPhaseCallbacks; @@ -203,7 +203,7 @@ public override void FlushWithSpeculativeBatches(ref UntypedList lis } [MethodImpl(MethodImplOptions.AggressiveInlining)] - protected static unsafe void UpdateConstraint( + protected static void UpdateConstraint( NarrowPhase narrowPhase, int manifoldTypeAsConstraintType, int workerIndex, ref CollidablePair pair, ref ConstraintCache constraintCache, int newContactCount, ref TConstraintDescription description, TCallBodyHandles bodyHandles) where TCallbacks : struct, INarrowPhaseCallbacks diff --git a/BepuPhysics/CollisionDetection/ContactManifold.cs b/BepuPhysics/CollisionDetection/ContactManifold.cs index ca8099d04..53dcd8090 100644 --- a/BepuPhysics/CollisionDetection/ContactManifold.cs +++ b/BepuPhysics/CollisionDetection/ContactManifold.cs @@ -276,7 +276,7 @@ public ref int GetFeatureId(ref NonconvexContactManifold manifold, int contactIn /// Contains the data associated with a convex contact manifold. /// [StructLayout(LayoutKind.Explicit, Size = 108)] - public unsafe struct ConvexContactManifold : IContactManifold + public struct ConvexContactManifold : IContactManifold { /// /// Offset from collidable A to collidable B. diff --git a/BepuPhysics/CollisionDetection/FreshnessChecker.cs b/BepuPhysics/CollisionDetection/FreshnessChecker.cs index 152b7e994..6b3aa3e10 100644 --- a/BepuPhysics/CollisionDetection/FreshnessChecker.cs +++ b/BepuPhysics/CollisionDetection/FreshnessChecker.cs @@ -157,7 +157,7 @@ unsafe void PrintRemovalInformation(ConstraintHandle constraintHandle) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - unsafe void EnqueueStaleRemoval(int workerIndex, int pairIndex) + void EnqueueStaleRemoval(int workerIndex, int pairIndex) { //Note that we have to grab the *old* handle, because the current frame's set of constraint caches do not contain this pair. //If they DID contain this pair, then it wouldn't be stale! diff --git a/BepuPhysics/CollisionDetection/INarrowPhaseCallbacks.cs b/BepuPhysics/CollisionDetection/INarrowPhaseCallbacks.cs index c26815a55..9bb42c2f3 100644 --- a/BepuPhysics/CollisionDetection/INarrowPhaseCallbacks.cs +++ b/BepuPhysics/CollisionDetection/INarrowPhaseCallbacks.cs @@ -39,7 +39,7 @@ public PairMaterialProperties(float frictionCoefficient, float maximumRecoveryVe /// /// Defines handlers for narrow phase events. /// - public unsafe interface INarrowPhaseCallbacks + public interface INarrowPhaseCallbacks { /// /// Performs any required initialization logic after the Simulation instance has been constructed. diff --git a/BepuPhysics/CollisionDetection/MeshReduction.cs b/BepuPhysics/CollisionDetection/MeshReduction.cs index e937ee3c7..71b917ce5 100644 --- a/BepuPhysics/CollisionDetection/MeshReduction.cs +++ b/BepuPhysics/CollisionDetection/MeshReduction.cs @@ -37,7 +37,7 @@ public void Create(int childManifoldCount, BufferPool pool) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void OnChildCompleted(ref PairContinuation report, ref ConvexContactManifold manifold, ref CollisionBatcher batcher) + public void OnChildCompleted(ref PairContinuation report, ref ConvexContactManifold manifold, ref CollisionBatcher batcher) where TCallbacks : struct, ICollisionCallbacks { Inner.OnChildCompleted(ref report, ref manifold, ref batcher); @@ -50,7 +50,7 @@ public void OnUntestedChildCompleted(ref PairContinuation report, re } [MethodImpl(MethodImplOptions.AggressiveInlining)] - private static unsafe void ComputeMeshSpaceContact(ref ConvexContactManifold manifold, in Matrix3x3 inverseMeshOrientation, bool requiresFlip, out Vector3 meshSpaceContact, out Vector3 meshSpaceNormal) + private static void ComputeMeshSpaceContact(ref ConvexContactManifold manifold, in Matrix3x3 inverseMeshOrientation, bool requiresFlip, out Vector3 meshSpaceContact, out Vector3 meshSpaceNormal) { //Select the deepest contact out of the manifold. Our goal is to find a contact on the representative feature of the source triangle. //Recall that triangle collision tests will generate speculative contacts elsewhere on the triangle, both on the face and potentially on edges @@ -143,7 +143,7 @@ public TestTriangle(in Triangle triangle, int sourceChildIndex) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - private static unsafe bool ShouldBlockNormal(in TestTriangle triangle, Vector3 meshSpaceContact, Vector3 meshSpaceNormal) + private static bool ShouldBlockNormal(in TestTriangle triangle, Vector3 meshSpaceContact, Vector3 meshSpaceNormal) { //While we don't have a decent way to do truly scaling SIMD operations within the context of a single manifold vs triangle test, we can at least use 4-wide operations //to accelerate each individual contact test. @@ -291,7 +291,7 @@ public bool LoopBody(int i) } } - public unsafe static void ReduceManifolds(ref Buffer continuationTriangles, ref Buffer continuationChildren, int start, int count, + public static void ReduceManifolds(ref Buffer continuationTriangles, ref Buffer continuationChildren, int start, int count, bool requiresFlip, in BoundingBox queryBounds, in Matrix3x3 meshOrientation, in Matrix3x3 meshInverseOrientation, Mesh* mesh, BufferPool pool) { //Before handing responsibility off to the nonconvex reduction, make sure that no contacts create nasty 'bumps' at the border of triangles. @@ -507,7 +507,7 @@ public unsafe static void ReduceManifolds(ref Buffer continuationTrian } //[MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe bool TryFlush(int pairId, ref CollisionBatcher batcher) where TCallbacks : struct, ICollisionCallbacks + public bool TryFlush(int pairId, ref CollisionBatcher batcher) where TCallbacks : struct, ICollisionCallbacks { Debug.Assert(Inner.ChildCount > 0); if (Inner.CompletedChildCount == Inner.ChildCount) diff --git a/BepuPhysics/CollisionDetection/NarrowPhase.cs b/BepuPhysics/CollisionDetection/NarrowPhase.cs index aafe22969..e1ab13fb0 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhase.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhase.cs @@ -396,7 +396,7 @@ protected override void OnDispose() Callbacks.Dispose(); } - public unsafe void HandleOverlap(int workerIndex, CollidableReference a, CollidableReference b) + public void HandleOverlap(int workerIndex, CollidableReference a, CollidableReference b) { Debug.Assert(a.Packed != b.Packed, "Excuse me, broad phase, but an object cannot collide with itself!"); SortCollidableReferencesForPair(a, b, out var aMobility, out var bMobility, out a, out b); @@ -474,7 +474,7 @@ public unsafe void HandleOverlap(int workerIndex, CollidableReference a, Collida } - unsafe struct CCDSweepFilter : ISweepFilter + struct CCDSweepFilter : ISweepFilter { public NarrowPhase NarrowPhase; public CollidablePair Pair; @@ -488,7 +488,7 @@ public bool AllowTest(int childA, int childB) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - private unsafe void AddBatchEntries(int workerIndex, ref OverlapWorker overlapWorker, + private void AddBatchEntries(int workerIndex, ref OverlapWorker overlapWorker, ref CollidablePair pair, ref ContinuousDetection continuityA, ref ContinuousDetection continuityB, TypedIndex shapeA, TypedIndex shapeB, diff --git a/BepuPhysics/CollisionDetection/NarrowPhaseCCDContinuations.cs b/BepuPhysics/CollisionDetection/NarrowPhaseCCDContinuations.cs index 0396e06c8..663c3e781 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhaseCCDContinuations.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhaseCCDContinuations.cs @@ -121,7 +121,7 @@ public CCDContinuationIndex AddContinuous(ref CollidablePair pair, Vector3 relat } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void OnPairCompleted(int pairId, ref TManifold manifoldReference) where TManifold : unmanaged, IContactManifold + public void OnPairCompleted(int pairId, ref TManifold manifoldReference) where TManifold : unmanaged, IContactManifold { CCDContinuationIndex continuationId = new CCDContinuationIndex(pairId); Debug.Assert(continuationId.Exists); @@ -210,7 +210,7 @@ public bool AllowCollisionTesting(int pairId, int childA, int childB) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void OnChildPairCompleted(int pairId, int childA, int childB, ref ConvexContactManifold manifold) + public void OnChildPairCompleted(int pairId, int childA, int childB, ref ConvexContactManifold manifold) { var keepManifold = narrowPhase.Callbacks.ConfigureContactManifold(workerIndex, GetCollidablePair(pairId), childA, childB, ref manifold); //This looks a little weird because it is. diff --git a/BepuPhysics/CollisionDetection/NarrowPhaseConstraintUpdate.cs b/BepuPhysics/CollisionDetection/NarrowPhaseConstraintUpdate.cs index ca6228805..965822f1d 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhaseConstraintUpdate.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhaseConstraintUpdate.cs @@ -132,7 +132,7 @@ private unsafe void RedistributeImpulses( } [MethodImpl(MethodImplOptions.AggressiveInlining)] - unsafe void RequestAddConstraint(int workerIndex, int manifoldConstraintType, + void RequestAddConstraint(int workerIndex, int manifoldConstraintType, CollidablePair pair, PairCacheChangeIndex pairCacheChange, ref TContactImpulses newImpulses, ref TDescription description, TBodyHandles bodyHandles) where TBodyHandles : unmanaged where TDescription : unmanaged, IConstraintDescription { @@ -249,7 +249,7 @@ static int GetConvexConstraintTypeId(int contactCount) } //TODO: If you end up changing the NarrowPhasePendingConstraintAdds and PairCache hardcoded type handling, you should change this too. This is getting silly. - unsafe void UpdateConstraintForManifold( + void UpdateConstraintForManifold( int workerIndex, ref CollidablePair pair, ref TContactManifold manifold, ref PairMaterialProperties material, TBodyHandles bodyHandles) { //Note that this function has two responsibilities: @@ -288,7 +288,7 @@ unsafe void UpdateConstraintForManifold( contactConstraintAccessors[manifoldTypeAsConstraintType].UpdateConstraintForManifold(this, manifoldTypeAsConstraintType, workerIndex, ref pair, ref manifold, ref material, bodyHandles); } - public unsafe void UpdateConstraintsForPair(int workerIndex, CollidablePair pair, ref TContactManifold manifold) where TContactManifold : unmanaged, IContactManifold + public void UpdateConstraintsForPair(int workerIndex, CollidablePair pair, ref TContactManifold manifold) where TContactManifold : unmanaged, IContactManifold { //Note that we do not check for the pair being between two statics before reporting it. The assumption is that, if the initial broadphase pair filter allowed such a pair //to reach this point, the user probably wants to receive some information about the resulting contact manifold. diff --git a/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs b/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs index a36f95f56..bd59de91d 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhasePendingConstraintAdds.cs @@ -16,7 +16,7 @@ public struct PendingConstraintAddCache { BufferPool pool; [StructLayout(LayoutKind.Sequential)] - unsafe struct PendingConstraint where TBodyHandles : unmanaged where TDescription : unmanaged, IConstraintDescription + struct PendingConstraint where TBodyHandles : unmanaged where TDescription : unmanaged, IConstraintDescription { //Note the memory ordering. Collidable pair comes first; deterministic flushes rely the memory layout to sort pending constraints. public CollidablePair Pair; @@ -255,7 +255,7 @@ internal int CountConstraints() } [MethodImpl(MethodImplOptions.AggressiveInlining)] - unsafe void AddConstraint(int workerIndex, int manifoldConstraintType, CollidablePair pair, + void AddConstraint(int workerIndex, int manifoldConstraintType, CollidablePair pair, PairCacheChangeIndex pairCacheChange, ref TContactImpulses impulses, TBodyHandles bodyHandles, ref TDescription constraintDescription) where TBodyHandles : unmanaged where TDescription : unmanaged, IConstraintDescription { diff --git a/BepuPhysics/CollisionDetection/NarrowPhasePreflush.cs b/BepuPhysics/CollisionDetection/NarrowPhasePreflush.cs index 77679786e..63d129683 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhasePreflush.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhasePreflush.cs @@ -120,13 +120,13 @@ void PreflushWorkerLoop(int workerIndex) struct PendingConstraintComparer : IComparerRef { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe int Compare(ref SortConstraintTarget a, ref SortConstraintTarget b) + public int Compare(ref SortConstraintTarget a, ref SortConstraintTarget b) { return a.SortKey.CompareTo(b.SortKey); } } - unsafe void BuildSortingTargets(ref QuickList list, int typeIndex, int workerCount) + void BuildSortingTargets(ref QuickList list, int typeIndex, int workerCount) { for (int i = 0; i < workerCount; ++i) { @@ -151,7 +151,7 @@ unsafe void BuildSortingTargets(ref QuickList list, int ty } } - unsafe void ExecutePreflushJob(int workerIndex, ref PreflushJob job) + void ExecutePreflushJob(int workerIndex, ref PreflushJob job) { switch (job.Type) { diff --git a/BepuPhysics/CollisionDetection/NonconvexReduction.cs b/BepuPhysics/CollisionDetection/NonconvexReduction.cs index 6af82fb79..3efcca549 100644 --- a/BepuPhysics/CollisionDetection/NonconvexReduction.cs +++ b/BepuPhysics/CollisionDetection/NonconvexReduction.cs @@ -320,7 +320,7 @@ public unsafe void Flush(int pairId, ref CollisionBatcher(ref PairContinuation report, ref ConvexContactManifold manifold, ref CollisionBatcher batcher) + public void OnChildCompleted(ref PairContinuation report, ref ConvexContactManifold manifold, ref CollisionBatcher batcher) where TCallbacks : struct, ICollisionCallbacks { Children[report.ChildIndex].Manifold = manifold; diff --git a/BepuPhysics/CollisionDetection/PairCache.cs b/BepuPhysics/CollisionDetection/PairCache.cs index 25c4ea8ce..32c2abbd0 100644 --- a/BepuPhysics/CollisionDetection/PairCache.cs +++ b/BepuPhysics/CollisionDetection/PairCache.cs @@ -202,7 +202,7 @@ public void PrepareFlushJobs(ref QuickList jobs) jobs.Add(new NarrowPhaseFlushJob { Type = NarrowPhaseFlushJobType.FlushPairCacheChanges }, pool); } - public unsafe void FlushMappingChanges() + public void FlushMappingChanges() { //Flush all pending adds from the new set. //Note that this phase accesses no shared memory- it's all pair cache local, and no pool accesses are made. @@ -308,14 +308,14 @@ public ref ConstraintCache GetCache(int index) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - internal unsafe PairCacheChangeIndex Add(int workerIndex, CollidablePair pair, in ConstraintCache constraintCache) + internal PairCacheChangeIndex Add(int workerIndex, CollidablePair pair, in ConstraintCache constraintCache) { //Note that we do not have to set any freshness bytes here; using this path means there exists no previous overlap to remove anyway. return new PairCacheChangeIndex { WorkerIndex = workerIndex, Index = WorkerPendingChanges[workerIndex].Add(cachedDispatcher == null ? pool : cachedDispatcher.WorkerPools[workerIndex], pair, constraintCache) }; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - internal unsafe PairCacheChangeIndex Update(int pairIndex, in ConstraintCache cache) + internal PairCacheChangeIndex Update(int pairIndex, in ConstraintCache cache) { //We're updating an existing pair, so we should prevent this pair from being removed. PairFreshness[pairIndex] = 0xFF; @@ -329,7 +329,7 @@ internal unsafe PairCacheChangeIndex Update(int pairIndex, in ConstraintCache ca [MethodImpl(MethodImplOptions.AggressiveInlining)] - internal unsafe ConstraintHandle GetOldConstraintHandle(int pairIndex) + internal ConstraintHandle GetOldConstraintHandle(int pairIndex) { return Mapping.Values[pairIndex].ConstraintHandle; } @@ -345,7 +345,7 @@ internal unsafe ConstraintHandle GetOldConstraintHandle(int pairIndex) /// Constraint handle associated with the constraint cache being updated. /// Collidable pair associated with the new constraint. [MethodImpl(MethodImplOptions.AggressiveInlining)] - internal unsafe void CompleteConstraintAdd(NarrowPhase narrowPhase, Solver solver, ref TContactImpulses impulses, PairCacheChangeIndex pairCacheChangeIndex, + internal void CompleteConstraintAdd(NarrowPhase narrowPhase, Solver solver, ref TContactImpulses impulses, PairCacheChangeIndex pairCacheChangeIndex, ConstraintHandle constraintHandle, ref CollidablePair pair) { if (pairCacheChangeIndex.IsPending) diff --git a/BepuPhysics/CollisionDetection/PairCache_Activity.cs b/BepuPhysics/CollisionDetection/PairCache_Activity.cs index 44bc2f5e7..8943f2252 100644 --- a/BepuPhysics/CollisionDetection/PairCache_Activity.cs +++ b/BepuPhysics/CollisionDetection/PairCache_Activity.cs @@ -38,7 +38,7 @@ internal void ResizeSetsCapacity(int setsCapacity, int potentiallyAllocatedCount [Conditional("DEBUG")] - internal unsafe void ValidateHandleCountInMapping(ConstraintHandle constraintHandle, int expectedCount) + internal void ValidateHandleCountInMapping(ConstraintHandle constraintHandle, int expectedCount) { int count = 0; for (int i = 0; i < Mapping.Count; ++i) @@ -53,7 +53,7 @@ internal unsafe void ValidateHandleCountInMapping(ConstraintHandle constraintHan Debug.Assert(count == expectedCount, "Expected count for this handle not found!"); } - internal unsafe void SleepTypeBatchPairs(ref SleepingSetBuilder builder, int setIndex, Solver solver) + internal void SleepTypeBatchPairs(ref SleepingSetBuilder builder, int setIndex, Solver solver) { ref var constraintSet = ref solver.Sets[setIndex]; for (int batchIndex = 0; batchIndex < constraintSet.Batches.Count; ++batchIndex) @@ -83,7 +83,7 @@ internal unsafe void SleepTypeBatchPairs(ref SleepingSetBuilder builder, int set builder.FinalizeSet(pool, out SleepingSets[setIndex]); } - internal unsafe void AwakenSet(int setIndex) + internal void AwakenSet(int setIndex) { ref var sleepingSet = ref SleepingSets[setIndex]; //If there are no pairs, there is no need for an inactive set, so it's not guaranteed to be allocated. diff --git a/BepuPhysics/CollisionDetection/RayBatchers.cs b/BepuPhysics/CollisionDetection/RayBatchers.cs index 3752aa898..c8d99af57 100644 --- a/BepuPhysics/CollisionDetection/RayBatchers.cs +++ b/BepuPhysics/CollisionDetection/RayBatchers.cs @@ -130,7 +130,7 @@ public bool AllowTest(int childIndex) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void OnRayHit(in RayData ray, ref float maximumT, float t, Vector3 normal, int childIndex) + public void OnRayHit(in RayData ray, ref float maximumT, float t, Vector3 normal, int childIndex) { HitHandler.OnRayHit(ray, ref maximumT, t, normal, Reference, childIndex); } diff --git a/BepuPhysics/CollisionDetection/UntypedList.cs b/BepuPhysics/CollisionDetection/UntypedList.cs index 2dc7476bc..fb3e90c13 100644 --- a/BepuPhysics/CollisionDetection/UntypedList.cs +++ b/BepuPhysics/CollisionDetection/UntypedList.cs @@ -69,7 +69,7 @@ public unsafe ref T Get(int index) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe ref T AllocateUnsafely() + public ref T AllocateUnsafely() { Validate(); Debug.Assert(Unsafe.SizeOf() == ElementSizeInBytes); @@ -123,14 +123,14 @@ public unsafe int Allocate(int elementSizeInBytes, int minimumElementCount, Buff } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe int Allocate(int minimumElementCount, BufferPool pool) + public int Allocate(int minimumElementCount, BufferPool pool) { var elementSizeInBytes = Unsafe.SizeOf(); return Allocate(elementSizeInBytes, minimumElementCount, pool); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe int Add(ref T data, int minimumCount, BufferPool pool) + public int Add(ref T data, int minimumCount, BufferPool pool) { var byteIndex = Allocate(minimumCount, pool); GetFromBytes(byteIndex) = data; diff --git a/BepuPhysics/CollisionDetection/WorkerPairCache.cs b/BepuPhysics/CollisionDetection/WorkerPairCache.cs index 8fed911b9..8a6b40bde 100644 --- a/BepuPhysics/CollisionDetection/WorkerPairCache.cs +++ b/BepuPhysics/CollisionDetection/WorkerPairCache.cs @@ -32,7 +32,7 @@ public WorkerPendingPairChanges(BufferPool pool, int pendingCapacity) [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe int Add(BufferPool pool, CollidablePair pair, in ConstraintCache cache) + public int Add(BufferPool pool, CollidablePair pair, in ConstraintCache cache) { int index = PendingAdds.Count; ref var pendingAdd = ref PendingAdds.Allocate(pool); diff --git a/BepuPhysics/ConstraintBatch.cs b/BepuPhysics/ConstraintBatch.cs index 1cac82072..551ce4358 100644 --- a/BepuPhysics/ConstraintBatch.cs +++ b/BepuPhysics/ConstraintBatch.cs @@ -192,7 +192,7 @@ public unsafe void RemoveBodyHandlesFromBatchForConstraint(int constraintTypeId, solver.EnumerateConnectedRawBodyReferences(ref TypeBatches[typeBatchIndex], indexInTypeBatch, ref handleRemover); } - public unsafe void Remove(int constraintTypeId, int indexInTypeBatch, bool isFallback, Solver solver) + public void Remove(int constraintTypeId, int indexInTypeBatch, bool isFallback, Solver solver) { var typeBatchIndex = TypeIndexToTypeBatchIndex[constraintTypeId]; ref var typeBatch = ref TypeBatches[typeBatchIndex]; diff --git a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs index 801829f75..cfbc535f4 100644 --- a/BepuPhysics/Constraints/FourBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/FourBodyTypeProcessor.cs @@ -148,7 +148,7 @@ public override void WarmStart(); var bodyReferencesBundles = typeBatch.BodyReferences.As(); @@ -173,7 +173,7 @@ public unsafe override void Solve(ref TypeBatch typeBatch, Bodies bodies, float } public override bool RequiresIncrementalSubstepUpdates => TConstraintFunctions.RequiresIncrementalSubstepUpdates; - public unsafe override void IncrementallyUpdateForSubstep(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + public override void IncrementallyUpdateForSubstep(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { var prestepBundles = typeBatch.PrestepData.As(); var bodyReferencesBundles = typeBatch.BodyReferences.As(); diff --git a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs index c9325f139..266916bb5 100644 --- a/BepuPhysics/Constraints/OneBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/OneBodyTypeProcessor.cs @@ -79,7 +79,7 @@ internal sealed override void VerifySortRegion(ref TypeBatch typeBatch, int bund //only has to specify *type* arguments associated with the interface-implementing struct-delegates. It's going to look very strange, but it's low overhead //and minimizes per-type duplication. - public unsafe override void WarmStart( + public override void WarmStart( ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { @@ -112,7 +112,7 @@ public unsafe override void WarmStart(); var bodyReferencesBundles = typeBatch.BodyReferences.As>(); diff --git a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs index beb6511ad..a6ff48093 100644 --- a/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs +++ b/BepuPhysics/Constraints/TwoBodyTypeProcessor.cs @@ -165,7 +165,7 @@ internal sealed override void VerifySortRegion(ref TypeBatch typeBatch, int bund //only has to specify *type* arguments associated with the interface-implementing struct-delegates. It's going to look very strange, but it's low overhead //and minimizes per-type duplication. - public unsafe override void WarmStart( + public override void WarmStart( ref TypeBatch typeBatch, ref Buffer integrationFlags, Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, float dt, float inverseDt, int startBundle, int exclusiveEndBundle, int workerIndex) { @@ -202,7 +202,7 @@ public unsafe override void WarmStart(); var bodyReferencesBundles = typeBatch.BodyReferences.As(); @@ -225,7 +225,7 @@ public unsafe override void Solve(ref TypeBatch typeBatch, Bodies bodies, float } public override bool RequiresIncrementalSubstepUpdates => TConstraintFunctions.RequiresIncrementalSubstepUpdates; - public unsafe override void IncrementallyUpdateForSubstep(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) + public override void IncrementallyUpdateForSubstep(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle) { var prestepBundles = typeBatch.PrestepData.As(); var bodyReferencesBundles = typeBatch.BodyReferences.As(); diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index 94381cdd6..9790984a0 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -55,7 +55,7 @@ public void Initialize(int typeId) /// List of body indices (not handles!) with count equal to the type batch's expected number of involved bodies. /// Allocation provider to use if the type batch has to be resized. /// Index of the slot in the batch. - public unsafe abstract int AllocateInTypeBatch(ref TypeBatch typeBatch, ConstraintHandle handle, Span encodedBodyIndices, BufferPool pool); + public abstract int AllocateInTypeBatch(ref TypeBatch typeBatch, ConstraintHandle handle, Span encodedBodyIndices, BufferPool pool); /// /// Allocates a slot in the batch, assuming the batch is a fallback batch. @@ -65,7 +65,7 @@ public void Initialize(int typeId) /// List of body indices (not handles!) with count equal to the type batch's expected number of involved bodies. /// Allocation provider to use if the type batch has to be resized. /// Index of the slot in the batch. - public unsafe abstract int AllocateInTypeBatchForFallback(ref TypeBatch typeBatch, ConstraintHandle handle, Span encodedBodyIndices, BufferPool pool); + public abstract int AllocateInTypeBatchForFallback(ref TypeBatch typeBatch, ConstraintHandle handle, Span encodedBodyIndices, BufferPool pool); public abstract void Remove(ref TypeBatch typeBatch, int index, ref Buffer handlesToConstraints, bool isFallback); @@ -130,7 +130,7 @@ public unsafe void TransferConstraint(ref TypeBatch sourceTypeBatch, int sourceB /// Index of the ConstraintBatch in the solver to copy the constraint into. /// Set of body handles in the constraint referring to dynamic bodies. /// Set of encoded body indices to use in the new constraint allocation. - public unsafe abstract void TransferConstraint(ref TypeBatch sourceTypeBatch, int sourceBatchIndex, int indexInTypeBatch, Solver solver, Bodies bodies, int targetBatchIndex, Span dynamicBodyHandles, Span encodedBodyIndices); + public abstract void TransferConstraint(ref TypeBatch sourceTypeBatch, int sourceBatchIndex, int indexInTypeBatch, Solver solver, Bodies bodies, int targetBatchIndex, Span dynamicBodyHandles, Span encodedBodyIndices); [Conditional("DEBUG")] protected abstract void ValidateAccumulatedImpulsesSizeInBytes(int sizeInBytes); @@ -181,17 +181,17 @@ internal abstract void Regather( ref Buffer indexToHandleCache, ref Buffer bodyReferencesCache, ref Buffer prestepCache, ref Buffer accumulatedImpulsesCache, ref Buffer handlesToConstraints); - internal unsafe abstract void GatherActiveConstraints(Bodies bodies, Solver solver, ref QuickList sourceHandles, int startIndex, int endIndex, ref TypeBatch targetTypeBatch); + internal abstract void GatherActiveConstraints(Bodies bodies, Solver solver, ref QuickList sourceHandles, int startIndex, int endIndex, ref TypeBatch targetTypeBatch); - internal unsafe abstract void AddSleepingToActiveForFallback( + internal abstract void AddSleepingToActiveForFallback( int sourceSet, int sourceTypeBatchIndex, int targetTypeBatchIndex, Bodies bodies, Solver solver); - internal unsafe abstract void CopySleepingToActive( + internal abstract void CopySleepingToActive( int sourceSet, int sourceBatchIndex, int sourceTypeBatchIndex, int targetTypeBatchIndex, int sourceStart, int targetStart, int count, Bodies bodies, Solver solver); - internal unsafe abstract void AddWakingBodyHandlesToBatchReferences(ref TypeBatch typeBatch, ref IndexSet targetBatchReferencedHandles); + internal abstract void AddWakingBodyHandlesToBatchReferences(ref TypeBatch typeBatch, ref IndexSet targetBatchReferencedHandles); [Conditional("DEBUG")] internal abstract void VerifySortRegion(ref TypeBatch typeBatch, int bundleStartIndex, int constraintCount, ref Buffer sortedKeys, ref Buffer sortedSourceIndices); @@ -266,7 +266,7 @@ public override unsafe void ScaleAccumulatedImpulses(ref TypeBatch typeBatch, fl } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe static void SetBodyReferencesLane(ref TBodyReferences bundle, int innerIndex, Span bodyIndices) + public static void SetBodyReferencesLane(ref TBodyReferences bundle, int innerIndex, Span bodyIndices) { //We assume that the body references struct is organized in memory like Bundle0, Inner0, ... BundleN, InnerN, Count //Assuming contiguous storage, Count is then located at start + stride * BodyCount. @@ -280,7 +280,7 @@ public unsafe static void SetBodyReferencesLane(ref TBodyReferences bundle, int } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe static void AddBodyReferencesLane(ref TBodyReferences bundle, int innerIndex, Span bodyIndices) + public static void AddBodyReferencesLane(ref TBodyReferences bundle, int innerIndex, Span bodyIndices) { //The jit should be able to fold almost all of the size-related calculations and address fiddling. var bodyCount = Unsafe.SizeOf() / (Vector.Count * sizeof(int)); @@ -298,7 +298,7 @@ public unsafe static void AddBodyReferencesLane(ref TBodyReferences bundle, int } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe static void RemoveBodyReferencesLane(ref TBodyReferences bundle, int innerIndex) + public static void RemoveBodyReferencesLane(ref TBodyReferences bundle, int innerIndex) { var bodyCount = Unsafe.SizeOf() / (Vector.Count * sizeof(int)); ref var start = ref Unsafe.As(ref bundle); @@ -311,7 +311,7 @@ public unsafe static void RemoveBodyReferencesLane(ref TBodyReferences bundle, i } - public unsafe sealed override int AllocateInTypeBatch(ref TypeBatch typeBatch, ConstraintHandle handle, Span bodyIndices, BufferPool pool) + public sealed override int AllocateInTypeBatch(ref TypeBatch typeBatch, ConstraintHandle handle, Span bodyIndices, BufferPool pool) { Debug.Assert(typeBatch.BodyReferences.Allocated, "Should initialize the batch before allocating anything from it."); if (typeBatch.ConstraintCount == typeBatch.IndexToHandle.Length) @@ -359,7 +359,7 @@ private unsafe static bool AllowFallbackBundleAllocation(ref TBodyReferences bun } [MethodImpl(MethodImplOptions.AggressiveInlining)] - private unsafe static int GetInnerIndexForFallbackAllocation(ref TBodyReferences bundle) + private static int GetInnerIndexForFallbackAllocation(ref TBodyReferences bundle) { //The type batches are always held in pinned memory, this is not a GC hole. var bundleBodyIndices = Unsafe.As>(ref bundle); @@ -591,7 +591,7 @@ protected static void Move( - public sealed override unsafe void Scramble(ref TypeBatch typeBatch, Random random, ref Buffer handlesToConstraints) + public sealed override void Scramble(ref TypeBatch typeBatch, Random random, ref Buffer handlesToConstraints) { //This is a pure debug function used to compare cache optimization strategies. Performance doesn't matter. TPrestepData aPrestep = default; @@ -730,7 +730,7 @@ ref Unsafe.Add(ref bodyReferences, targetBundleIndex), ref Unsafe.Add(ref preste /// Index of the ConstraintBatch in the solver to copy the constraint into. /// Set of body handles in the constraint referring to dynamic bodies. /// Set of encoded body indices to use in the new constraint allocation. - public unsafe override sealed void TransferConstraint(ref TypeBatch sourceTypeBatch, int sourceBatchIndex, int indexInTypeBatch, Solver solver, Bodies bodies, int targetBatchIndex, Span dynamicBodyHandles, Span encodedBodyIndices) + public override sealed void TransferConstraint(ref TypeBatch sourceTypeBatch, int sourceBatchIndex, int indexInTypeBatch, Solver solver, Bodies bodies, int targetBatchIndex, Span dynamicBodyHandles, Span encodedBodyIndices) { var constraintHandle = sourceTypeBatch.IndexToHandle[indexInTypeBatch]; //Allocate a spot in the new batch. Note that it does not change the Handle->Constraint mapping in the Solver; that's important when we call Solver.Remove below. @@ -904,7 +904,7 @@ internal sealed override void Regather( } } - internal unsafe sealed override void GatherActiveConstraints(Bodies bodies, Solver solver, ref QuickList sourceHandles, int startIndex, int endIndex, ref TypeBatch targetTypeBatch) + internal sealed override void GatherActiveConstraints(Bodies bodies, Solver solver, ref QuickList sourceHandles, int startIndex, int endIndex, ref TypeBatch targetTypeBatch) { ref var activeConstraintSet = ref solver.ActiveSet; ref var activeBodySet = ref bodies.ActiveSet; @@ -1016,7 +1016,7 @@ internal unsafe sealed override void AddSleepingToActiveForFallback(int sourceSe //solver.ValidateConstraintMaps(0, batchIndex, targetTypeBatchIndex); } - internal unsafe sealed override void CopySleepingToActive( + internal sealed override void CopySleepingToActive( int sourceSet, int batchIndex, int sourceTypeBatchIndex, int targetTypeBatchIndex, int sourceStart, int targetStart, int count, Bodies bodies, Solver solver) { @@ -1094,7 +1094,7 @@ ref Buffer.Get(ref sourceTypeBatch.AccumulatedImpulses, sou } - internal unsafe sealed override void AddWakingBodyHandlesToBatchReferences(ref TypeBatch typeBatch, ref IndexSet targetBatchReferencedHandles) + internal sealed override void AddWakingBodyHandlesToBatchReferences(ref TypeBatch typeBatch, ref IndexSet targetBatchReferencedHandles) { for (int i = 0; i < typeBatch.ConstraintCount; ++i) { @@ -1201,7 +1201,7 @@ public static BundleIntegrationMode BundleShouldIntegrate(int bundleIndex, in In return BundleIntegrationMode.None; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static unsafe void IntegratePoseAndVelocity( + public static void IntegratePoseAndVelocity( ref TIntegratorCallbacks integratorCallbacks, ref Vector bodyIndices, in BodyInertiaWide localInertia, float dt, in Vector integrationMask, ref Vector3Wide position, ref QuaternionWide orientation, ref BodyVelocityWide velocity, int workerIndex, @@ -1248,7 +1248,7 @@ public static unsafe void IntegratePoseAndVelocity( } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static unsafe void IntegrateVelocity( + public static void IntegrateVelocity( ref TIntegratorCallbacks integratorCallbacks, ref Vector bodyIndices, in BodyInertiaWide localInertia, float dt, in Vector integrationMask, in Vector3Wide position, in QuaternionWide orientation, ref BodyVelocityWide velocity, int workerIndex, @@ -1283,7 +1283,7 @@ public static unsafe void IntegrateVelocity( + public static void GatherAndIntegrate( Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, ref Buffer integrationFlags, int bodyIndexInConstraint, float dt, int workerIndex, int bundleIndex, ref Vector bodyIndices, out Vector3Wide position, out QuaternionWide orientation, out BodyVelocityWide velocity, out BodyInertiaWide inertia) where TIntegratorCallbacks : struct, IPoseIntegratorCallbacks diff --git a/BepuPhysics/IslandAwakener.cs b/BepuPhysics/IslandAwakener.cs index 531038df5..331de0951 100644 --- a/BepuPhysics/IslandAwakener.cs +++ b/BepuPhysics/IslandAwakener.cs @@ -219,7 +219,7 @@ struct PhaseTwoJob QuickList uniqueSetIndices; QuickList phaseOneJobs; QuickList phaseTwoJobs; - internal unsafe void ExecutePhaseOneJob(int index) + internal void ExecutePhaseOneJob(int index) { ref var job = ref phaseOneJobs[index]; switch (job.Type) @@ -323,7 +323,7 @@ internal unsafe void ExecutePhaseOneJob(int index) } - internal unsafe void ExecutePhaseTwoJob(int index) + internal void ExecutePhaseTwoJob(int index) { ref var phaseTwoJob = ref phaseTwoJobs[index]; switch (phaseTwoJob.Type) @@ -470,7 +470,7 @@ public void Dispose(BufferPool pool) } - unsafe internal (int phaseOneJobCount, int phaseTwoJobCount) PrepareJobs(ref QuickList setIndices, bool resetActivityStates, int threadCount) + internal (int phaseOneJobCount, int phaseTwoJobCount) PrepareJobs(ref QuickList setIndices, bool resetActivityStates, int threadCount) { if (setIndices.Count == 0) return (0, 0); diff --git a/BepuPhysics/IslandSleeper.cs b/BepuPhysics/IslandSleeper.cs index 7154131c9..6e6f8e94d 100644 --- a/BepuPhysics/IslandSleeper.cs +++ b/BepuPhysics/IslandSleeper.cs @@ -331,7 +331,8 @@ void FindIslands(int workerIndex) } Action gatherDelegate; - unsafe void Gather(int workerIndex) + + void Gather(int workerIndex) { while (true) { @@ -530,7 +531,7 @@ public int Compare(ref int a, ref int b) int scheduleOffset; [Conditional("DEBUG")] - unsafe void PrintIsland(ref IslandScaffold island) + void PrintIsland(ref IslandScaffold island) { Console.Write($"{island.BodyIndices.Count} body handles: "); for (int i = 0; i < island.BodyIndices.Count; ++i) @@ -589,7 +590,7 @@ unsafe void PrintIsland(ref IslandScaffold island) } - unsafe void Sleep(ref QuickList traversalStartBodyIndices, IThreadDispatcher threadDispatcher, bool deterministic, int targetSleptBodyCount, int targetTraversedBodyCount, bool forceSleep) + void Sleep(ref QuickList traversalStartBodyIndices, IThreadDispatcher threadDispatcher, bool deterministic, int targetSleptBodyCount, int targetTraversedBodyCount, bool forceSleep) { //There are four threaded phases to sleep: //1) Traversing the constraint graph to identify 'simulation islands' that satisfy the sleep conditions. diff --git a/BepuPhysics/PoseIntegrator.cs b/BepuPhysics/PoseIntegrator.cs index 4e9b84cd5..739d384f9 100644 --- a/BepuPhysics/PoseIntegrator.cs +++ b/BepuPhysics/PoseIntegrator.cs @@ -253,7 +253,7 @@ public static void IntegrateAngularVelocityConserveMomentumWithGyroscopicTorque( } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static unsafe void Integrate(in RigidPose pose, in BodyVelocity velocity, float dt, out RigidPose integratedPose) + public static void Integrate(in RigidPose pose, in BodyVelocity velocity, float dt, out RigidPose integratedPose) { Integrate(pose.Position, velocity.Linear, dt, out integratedPose.Position); Integrate(pose.Orientation, velocity.Angular, dt, out integratedPose.Orientation); @@ -304,7 +304,7 @@ void UpdateSleepCandidacy(float velocityHeuristic, ref BodyActivity activity) } } - unsafe void PredictBoundingBoxes(int startBundleIndex, int endBundleIndex, float dt, ref BoundingBoxBatcher boundingBoxBatcher, int workerIndex) + void PredictBoundingBoxes(int startBundleIndex, int endBundleIndex, float dt, ref BoundingBoxBatcher boundingBoxBatcher, int workerIndex) { var activities = bodies.ActiveSet.Activity; var collidables = bodies.ActiveSet.Collidables; @@ -446,7 +446,7 @@ public void PredictBoundingBoxes(float dt, BufferPool pool, IThreadDispatcher th /// Integrates the velocities of kinematic bodies as a prepass to the first substep during solving. /// Kinematics have to be integrated ahead of time since they don't block constraint batch membership; the same kinematic could appear in the batch multiple times. /// - internal unsafe void IntegrateKinematicVelocities(Buffer bodyHandles, int bundleStartIndex, int bundleEndIndex, float substepDt, int workerIndex) + internal void IntegrateKinematicVelocities(Buffer bodyHandles, int bundleStartIndex, int bundleEndIndex, float substepDt, int workerIndex) { var bodyCount = bodyHandles.Length; var bundleCount = BundleIndexing.GetBundleCount(bodyCount); @@ -488,7 +488,7 @@ internal unsafe void IntegrateKinematicVelocities(Buffer bodyHandles, int b /// Integrates the poses *then* velocities of kinematic bodies as a prepass to the second or later substeps during solving. /// Kinematics have to be integrated ahead of time since they don't block constraint batch membership; the same kinematic could appear in the batch multiple times. /// - internal unsafe void IntegrateKinematicPosesAndVelocities(Buffer bodyHandles, int bundleStartIndex, int bundleEndIndex, float substepDt, int workerIndex) + internal void IntegrateKinematicPosesAndVelocities(Buffer bodyHandles, int bundleStartIndex, int bundleEndIndex, float substepDt, int workerIndex) { var bodyCount = bodyHandles.Length; var bundleCount = BundleIndexing.GetBundleCount(bodyCount); @@ -532,7 +532,7 @@ internal unsafe void IntegrateKinematicPosesAndVelocities(Buffer bodyHandle } } - unsafe void IntegrateBundlesAfterSubstepping(ref IndexSet mergedConstrainedBodyHandles, int bundleStartIndex, int bundleEndIndex, float dt, float substepDt, int substepCount, int workerIndex) + void IntegrateBundlesAfterSubstepping(ref IndexSet mergedConstrainedBodyHandles, int bundleStartIndex, int bundleEndIndex, float dt, float substepDt, int substepCount, int workerIndex) { var bodyCount = bodies.ActiveSet.Count; var bundleCount = BundleIndexing.GetBundleCount(bodyCount); diff --git a/BepuPhysics/SequentialFallbackBatch.cs b/BepuPhysics/SequentialFallbackBatch.cs index b2b923145..9bf7df10f 100644 --- a/BepuPhysics/SequentialFallbackBatch.cs +++ b/BepuPhysics/SequentialFallbackBatch.cs @@ -48,7 +48,7 @@ public struct SequentialFallbackBatch internal QuickDictionary> dynamicBodyConstraintCounts; [MethodImpl(MethodImplOptions.AggressiveInlining)] - unsafe void Allocate(Span dynamicBodyHandles, Bodies bodies, + void Allocate(Span dynamicBodyHandles, Bodies bodies, BufferPool pool, TBodyReferenceGetter bodyReferenceGetter, int minimumBodyCapacity) where TBodyReferenceGetter : struct, IBodyReferenceGetter { @@ -71,7 +71,7 @@ unsafe void Allocate(Span dynamicBodyHandles, [MethodImpl(MethodImplOptions.AggressiveInlining)] - internal unsafe void AllocateForActive(Span dynamicBodyHandles, Bodies bodies, + internal void AllocateForActive(Span dynamicBodyHandles, Bodies bodies, BufferPool pool, int minimumBodyCapacity = 8) { Allocate(dynamicBodyHandles, bodies, pool, new ActiveSetGetter(), minimumBodyCapacity); @@ -91,7 +91,7 @@ internal void AllocateForInactive(Span dynamicBodyHandles, Bodies bo /// Body associated with a constraint in the fallback batch. /// Allocations that should be freed once execution is back in a safe context. /// True if the body was dynamic and no longer has any constraints associated with it in the fallback batch, false otherwise. - internal unsafe bool RemoveOneBodyReferenceFromDynamicsSet(int bodyReference, ref QuickList allocationIdsToFree) + internal bool RemoveOneBodyReferenceFromDynamicsSet(int bodyReference, ref QuickList allocationIdsToFree) { if (!dynamicBodyConstraintCounts.GetTableIndices(ref bodyReference, out var tableIndex, out var bodyReferencesIndex)) return false; @@ -121,7 +121,7 @@ internal unsafe bool RemoveOneBodyReferenceFromDynamicsSet(int bodyReference, re /// Reference to the body to remove from the fallback batch. /// Allocations that should be freed once execution is back in a safe context. /// True if the body was present in the fallback batch and was removed, false otherwise. - internal unsafe bool TryRemoveDynamicBodyFromTracking(int bodyReference, ref QuickList allocationIdsToFree) + internal bool TryRemoveDynamicBodyFromTracking(int bodyReference, ref QuickList allocationIdsToFree) { if (dynamicBodyConstraintCounts.Keys.Allocated && dynamicBodyConstraintCounts.GetTableIndices(ref bodyReference, out var tableIndex, out var bodyReferencesIndex)) { @@ -209,7 +209,7 @@ public static unsafe void ValidateSetReferences(Solver solver, int setIndex) } } [Conditional("DEBUG")] - public static unsafe void ValidateReferences(Solver solver) + public static void ValidateReferences(Solver solver) { for (int i = 0; i < solver.Sets.Length; ++i) { diff --git a/BepuPhysics/Simulation_Queries.cs b/BepuPhysics/Simulation_Queries.cs index 12cfc3ef4..23d3ee9f6 100644 --- a/BepuPhysics/Simulation_Queries.cs +++ b/BepuPhysics/Simulation_Queries.cs @@ -158,7 +158,7 @@ public unsafe void RayTest(CollidableReference collidable, RayData* rayData, flo /// Maximum length of the ray traversal in units of the direction's length. /// callbacks to execute on ray-object intersections. /// User specified id of the ray. - public unsafe void RayCast(Vector3 origin, Vector3 direction, float maximumT, ref THitHandler hitHandler, int id = 0) where THitHandler : IRayHitHandler + public void RayCast(Vector3 origin, Vector3 direction, float maximumT, ref THitHandler hitHandler, int id = 0) where THitHandler : IRayHitHandler { RayHitDispatcher dispatcher; dispatcher.ShapeHitHandler.HitHandler = hitHandler; @@ -299,7 +299,7 @@ public unsafe void Sweep(TShape shape, in RigidPose po /// Pool to allocate any temporary resources in during execution. /// Callbacks executed when a sweep impacts an object in the scene. /// Simulation objects are treated as stationary during the sweep. - public unsafe void Sweep(in TShape shape, in RigidPose pose, in BodyVelocity velocity, float maximumT, BufferPool pool, ref TSweepHitHandler hitHandler) + public void Sweep(in TShape shape, in RigidPose pose, in BodyVelocity velocity, float maximumT, BufferPool pool, ref TSweepHitHandler hitHandler) where TShape : unmanaged, IConvexShape where TSweepHitHandler : ISweepHitHandler { //Estimate some reasonable termination conditions for iterative sweeps based on the input shape size. diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index 8fa991437..03f0c95a2 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -345,7 +345,7 @@ public unsafe ConstraintReference GetConstraintReference(ConstraintHandle handle } [Conditional("DEBUG")] - internal unsafe void ValidateConstraintReferenceKinematicity() + internal void ValidateConstraintReferenceKinematicity() { //Only the active set's body indices are flagged for kinematicity; the inactive sets store body handles. for (int setIndex = 0; setIndex < Sets.Length; ++setIndex) @@ -393,7 +393,7 @@ internal unsafe void ValidateConstraintReferenceKinematicity() } [Conditional("DEBUG")] - internal unsafe void ValidateConstrainedKinematicsSet() + internal void ValidateConstrainedKinematicsSet() { ref var set = ref bodies.ActiveSet; for (int i = 0; i < set.Count; ++i) @@ -428,7 +428,7 @@ private void ValidateBodyReference(int body, int expectedCount, ref ConstraintBa } [Conditional("DEBUG")] - internal unsafe void ValidateTrailingTypeBatchBodyReferences() + internal void ValidateTrailingTypeBatchBodyReferences() { ref var set = ref ActiveSet; for (int batchIndex = 0; batchIndex < set.Batches.Count; ++batchIndex) @@ -454,7 +454,7 @@ internal unsafe void ValidateTrailingTypeBatchBodyReferences() } } [Conditional("DEBUG")] - internal unsafe void ValidateFallbackBatchEmptySlotReferences() + internal void ValidateFallbackBatchEmptySlotReferences() { ref var set = ref ActiveSet; if (set.Batches.Count > FallbackBatchThreshold) @@ -478,7 +478,7 @@ internal unsafe void ValidateFallbackBatchEmptySlotReferences() [Conditional("DEBUG")] - internal unsafe void ValidateFallbackBodiesAreDynamic() + internal void ValidateFallbackBodiesAreDynamic() { ref var set = ref ActiveSet; if (set.Batches.Count > FallbackBatchThreshold) @@ -514,7 +514,7 @@ internal unsafe void ValidateFallbackBodiesAreDynamic() } [Conditional("DEBUG")] - internal unsafe void ValidateFallbackBatchAccessSafety() + internal void ValidateFallbackBatchAccessSafety() { ref var set = ref ActiveSet; if (set.Batches.Count > FallbackBatchThreshold) @@ -608,7 +608,7 @@ public void LoopBody(float impulse) } - internal unsafe void ValidateFallbackBodyReferencesByHash(HashDiagnosticType hashDiagnosticType) + internal void ValidateFallbackBodyReferencesByHash(HashDiagnosticType hashDiagnosticType) { var hashes = InvasiveHashDiagnostics.Instance; ref var hash = ref hashes.GetHashForType(hashDiagnosticType); @@ -981,7 +981,7 @@ internal void AssertConstraintHandleExists(ConstraintHandle handle) /// Index of the batch that the constraint would fit in. /// This is used by the narrowphase's multithreaded constraint adders to locate a spot for a new constraint without requiring a lock. Only after a candidate is located /// do those systems attempt an actual claim, limiting the duration of locks and increasing potential parallelism. - internal unsafe int FindCandidateBatch(CollidablePair collidablePair) + internal int FindCandidateBatch(CollidablePair collidablePair) { ref var set = ref ActiveSet; GetSynchronizedBatchCount(out var synchronizedBatchCount, out var fallbackExists); @@ -1055,7 +1055,7 @@ internal unsafe void AllocateInBatch(int targetBatchIndex, ConstraintHandle cons } } - unsafe internal void GetBlockingBodyHandles(Span bodyHandles, ref Span blockingBodyHandlesAllocation, Span encodedBodyIndices) + internal void GetBlockingBodyHandles(Span bodyHandles, ref Span blockingBodyHandlesAllocation, Span encodedBodyIndices) { //Kinematics do not block allocation in a batch; they are treated as read only by the solver. int blockingCount = 0; @@ -1090,7 +1090,7 @@ internal int AllocateNewConstraintBatch() return set.Batches.Count - 1; } - internal unsafe bool TryAllocateInBatch(int typeId, int targetBatchIndex, Span dynamicBodyHandles, Span encodedBodyIndices, out ConstraintHandle constraintHandle, out ConstraintReference reference) + internal bool TryAllocateInBatch(int typeId, int targetBatchIndex, Span dynamicBodyHandles, Span encodedBodyIndices, out ConstraintHandle constraintHandle, out ConstraintReference reference) { ref var set = ref ActiveSet; Debug.Assert(targetBatchIndex <= set.Batches.Count, @@ -1341,7 +1341,7 @@ internal void RemoveBatchIfEmpty(ref ConstraintBatch batch, int batchIndex) /// Index of the batch to remove from. /// Type id of the constraint to remove. /// Index of the constraint to remove within its type batch. - internal unsafe void RemoveFromBatch(int batchIndex, int typeId, int indexInTypeBatch) + internal void RemoveFromBatch(int batchIndex, int typeId, int indexInTypeBatch) { ref var batch = ref ActiveSet.Batches[batchIndex]; if (batchIndex == FallbackBatchThreshold) @@ -1568,7 +1568,7 @@ public unsafe float GetAccumulatedImpulseMagnitudeSquared(ConstraintHandle const /// /// Constraint to look up the accumulated impulses of. /// Magnitude of the accumulated impulses associated with the given constraint. - public unsafe float GetAccumulatedImpulseMagnitude(ConstraintHandle constraintHandle) + public float GetAccumulatedImpulseMagnitude(ConstraintHandle constraintHandle) { return (float)Math.Sqrt(GetAccumulatedImpulseMagnitudeSquared(constraintHandle)); } @@ -1588,7 +1588,7 @@ private struct DynamicToKinematicEnumerator : IForEach { public int DynamicCount; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void LoopBody(int encodedBodyReference) + public void LoopBody(int encodedBodyReference) { ++DynamicCount; } @@ -1670,7 +1670,7 @@ private unsafe struct KinematicToDynamicEnumerator : IForEach public int EncodedCount; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void LoopBody(int encodedBodyReference) + public void LoopBody(int encodedBodyReference) { Debug.Assert(EncodedCount < MaximumBodiesPerConstraint, "We assumed that the number of bodies per constraint was limited; if this assumption fails, it could cause a stack overrun."); if (Bodies.IsEncodedDynamicReference(encodedBodyReference)) @@ -1797,7 +1797,7 @@ internal unsafe void EnumerateConnectedBodyReferences( /// Type batch containing the constraint to enumerate. /// Index of the constraint to enumerate in the type batch. /// Enumerator to call for each connected body reference. - public unsafe void EnumerateConnectedRawBodyReferences(ref TypeBatch typeBatch, int indexInTypeBatch, ref TEnumerator enumerator) where TEnumerator : IForEach + public void EnumerateConnectedRawBodyReferences(ref TypeBatch typeBatch, int indexInTypeBatch, ref TEnumerator enumerator) where TEnumerator : IForEach { EnumerateConnectedBodyReferences(ref typeBatch, indexInTypeBatch, ref enumerator); } @@ -1809,7 +1809,7 @@ public unsafe void EnumerateConnectedRawBodyReferences(ref TypeBatc /// Type of the enumerator to call on each connected body reference. /// Constraint to enumerate. /// Enumerator to call for each connected body reference. - public unsafe void EnumerateConnectedRawBodyReferences(ConstraintHandle constraintHandle, ref TEnumerator enumerator) where TEnumerator : IForEach + public void EnumerateConnectedRawBodyReferences(ConstraintHandle constraintHandle, ref TEnumerator enumerator) where TEnumerator : IForEach { ref var constraintLocation = ref HandleToConstraint[constraintHandle.Value]; ref var typeBatch = ref Sets[constraintLocation.SetIndex].Batches[constraintLocation.BatchIndex].GetTypeBatch(constraintLocation.TypeId); @@ -1825,7 +1825,7 @@ public unsafe void EnumerateConnectedRawBodyReferences(ConstraintHa /// Type batch containing the constraint to enumerate. /// Index of the constraint to enumerate in the type batch. /// Enumerator to call for each connected body reference. - public unsafe void EnumerateConnectedBodyReferences(ref TypeBatch typeBatch, int indexInTypeBatch, ref TEnumerator enumerator) where TEnumerator : IForEach + public void EnumerateConnectedBodyReferences(ref TypeBatch typeBatch, int indexInTypeBatch, ref TEnumerator enumerator) where TEnumerator : IForEach { EnumerateConnectedBodyReferences(ref typeBatch, indexInTypeBatch, ref enumerator); } @@ -1837,7 +1837,7 @@ public unsafe void EnumerateConnectedBodyReferences(ref TypeBatch t /// Type of the enumerator to call on each connected body reference. /// Constraint to enumerate. /// Enumerator to call for each connected body reference. - public unsafe void EnumerateConnectedBodyReferences(ConstraintHandle constraintHandle, ref TEnumerator enumerator) where TEnumerator : IForEach + public void EnumerateConnectedBodyReferences(ConstraintHandle constraintHandle, ref TEnumerator enumerator) where TEnumerator : IForEach { ref var constraintLocation = ref HandleToConstraint[constraintHandle.Value]; ref var typeBatch = ref Sets[constraintLocation.SetIndex].Batches[constraintLocation.BatchIndex].GetTypeBatch(constraintLocation.TypeId); @@ -1854,7 +1854,7 @@ public unsafe void EnumerateConnectedBodyReferences(ConstraintHandl /// Type batch containing the constraint to enumerate. /// Index of the constraint to enumerate in the type batch. /// Enumerator to call for each connected dynamic body reference. - public unsafe void EnumerateConnectedDynamicBodies(ref TypeBatch typeBatch, int indexInTypeBatch, ref TEnumerator enumerator) where TEnumerator : IForEach + public void EnumerateConnectedDynamicBodies(ref TypeBatch typeBatch, int indexInTypeBatch, ref TEnumerator enumerator) where TEnumerator : IForEach { EnumerateConnectedBodyReferences(ref typeBatch, indexInTypeBatch, ref enumerator); } @@ -1867,7 +1867,7 @@ public unsafe void EnumerateConnectedDynamicBodies(ref TypeBatch ty /// Type of the enumerator to call on each connected dynamic body reference. /// Constraint to enumerate. /// Enumerator to call for each connected dynamic body reference. - public unsafe void EnumerateConnectedDynamicBodies(ConstraintHandle constraintHandle, ref TEnumerator enumerator) where TEnumerator : IForEach + public void EnumerateConnectedDynamicBodies(ConstraintHandle constraintHandle, ref TEnumerator enumerator) where TEnumerator : IForEach { ref var constraintLocation = ref HandleToConstraint[constraintHandle.Value]; ref var typeBatch = ref Sets[constraintLocation.SetIndex].Batches[constraintLocation.BatchIndex].GetTypeBatch(constraintLocation.TypeId); diff --git a/BepuPhysics/Solver_Solve.cs b/BepuPhysics/Solver_Solve.cs index 196ae97f7..916684755 100644 --- a/BepuPhysics/Solver_Solve.cs +++ b/BepuPhysics/Solver_Solve.cs @@ -294,7 +294,7 @@ public void Execute(Solver solver, int blockIndex, int workerIndex) } } - unsafe void ExecuteWorkerStage(ref TStageFunction stageFunction, int workerIndex, int workerStart, int availableBlocksStartIndex, ref Buffer claims, int previousSyncIndex, int syncIndex, ref int completedWorkBlocks) where TStageFunction : IStageFunction + void ExecuteWorkerStage(ref TStageFunction stageFunction, int workerIndex, int workerStart, int availableBlocksStartIndex, ref Buffer claims, int previousSyncIndex, int syncIndex, ref int completedWorkBlocks) where TStageFunction : IStageFunction { if (workerStart == -1) { @@ -680,7 +680,7 @@ Buffer BuildKinematicIntegrationWorkBlocks(int minimumBloc return default; } - protected unsafe void BuildWorkBlocks( + protected void BuildWorkBlocks( BufferPool pool, int minimumBlockSizeInBundles, int maximumBlockSizeInBundles, int targetBlocksPerBatch, ref TTypeBatchFilter typeBatchFilter, out QuickList workBlocks, out Buffer batchBoundaries) where TTypeBatchFilter : ITypeBatchSolveFilter { @@ -947,7 +947,8 @@ protected void ExecuteMultithreaded(float dt, IThreadDispatcher threadDispatcher struct IsFallbackBatch { } struct IsNotFallbackBatch { } - unsafe bool ComputeIntegrationResponsibilitiesForConstraintRegion(int batchIndex, int typeBatchIndex, int constraintStart, int exclusiveConstraintEnd) where TFallbackness : unmanaged + + bool ComputeIntegrationResponsibilitiesForConstraintRegion(int batchIndex, int typeBatchIndex, int constraintStart, int exclusiveConstraintEnd) where TFallbackness : unmanaged { ref var firstObservedForBatch = ref bodiesFirstObservedInBatches[batchIndex]; ref var integrationFlagsForTypeBatch = ref integrationFlags[batchIndex][typeBatchIndex]; @@ -1068,7 +1069,7 @@ void ConstraintIntegrationResponsibilitiesWorker(int workerIndex) Action constraintIntegrationResponsibilitiesWorker; IndexSet mergedConstrainedBodyHandles; - public override unsafe IndexSet PrepareConstraintIntegrationResponsibilities(IThreadDispatcher threadDispatcher = null) + public override IndexSet PrepareConstraintIntegrationResponsibilities(IThreadDispatcher threadDispatcher = null) { if (ActiveSet.Batches.Count > 0) { diff --git a/BepuPhysics/Statics.cs b/BepuPhysics/Statics.cs index 7050f771e..7dfe00ace 100644 --- a/BepuPhysics/Statics.cs +++ b/BepuPhysics/Statics.cs @@ -150,7 +150,7 @@ public ref Static this[int index] } } - public unsafe Statics(BufferPool pool, Shapes shapes, Bodies bodies, BroadPhase broadPhase, int initialCapacity = 4096) + public Statics(BufferPool pool, Shapes shapes, Bodies bodies, BroadPhase broadPhase, int initialCapacity = 4096) { this.pool = pool; InternalResize(Math.Max(1, initialCapacity)); @@ -456,7 +456,7 @@ public void SetShape(StaticHandle handle, TypedIndex newShape) /// Description to apply to the static. /// Filter to apply to sleeping bodies near the static to see if they should be awoken. /// Type of the filter to apply to sleeping bodies. - public unsafe void ApplyDescription(StaticHandle handle, in StaticDescription description, ref TAwakeningFilter filter) where TAwakeningFilter : struct, IStaticChangeAwakeningFilter + public void ApplyDescription(StaticHandle handle, in StaticDescription description, ref TAwakeningFilter filter) where TAwakeningFilter : struct, IStaticChangeAwakeningFilter { ValidateExistingHandle(handle); var index = HandleToIndex[handle.Value]; @@ -475,7 +475,7 @@ public unsafe void ApplyDescription(StaticHandle handle, in St /// /// Handle of the static to apply the description to. /// Description to apply to the static. - public unsafe void ApplyDescription(StaticHandle handle, in StaticDescription description) + public void ApplyDescription(StaticHandle handle, in StaticDescription description) { var defaultFilter = default(StaticsShouldntAwakenKinematics); ApplyDescription(handle, description, ref defaultFilter); diff --git a/BepuPhysics/Trees/Node.cs b/BepuPhysics/Trees/Node.cs index 8bf328b29..3da577fd9 100644 --- a/BepuPhysics/Trees/Node.cs +++ b/BepuPhysics/Trees/Node.cs @@ -24,7 +24,7 @@ public struct NodeChild /// 2-wide tree node. /// [StructLayout(LayoutKind.Explicit)] - public unsafe struct Node + public struct Node { [FieldOffset(0)] public NodeChild A; @@ -38,7 +38,7 @@ public unsafe struct Node /// Metadata associated with a 2-child tree node. /// [StructLayout(LayoutKind.Explicit)] - public unsafe struct Metanode + public struct Metanode { [FieldOffset(0)] public int Parent; diff --git a/BepuPhysics/Trees/RayBatcher.cs b/BepuPhysics/Trees/RayBatcher.cs index f909c83ab..56f6aded6 100644 --- a/BepuPhysics/Trees/RayBatcher.cs +++ b/BepuPhysics/Trees/RayBatcher.cs @@ -272,7 +272,7 @@ interface ITreeRaySource int this[int rayIndex] { get; } } - unsafe struct RootRaySource : ITreeRaySource + struct RootRaySource : ITreeRaySource { int rayCount; @@ -327,7 +327,7 @@ public int this[int rayIndex] } } - unsafe void TestNode(ref Node node, byte depth, ref TRaySource raySource) where TRaySource : struct, ITreeRaySource + void TestNode(ref Node node, byte depth, ref TRaySource raySource) where TRaySource : struct, ITreeRaySource { int a0Start = stackPointerA0; int bStart = stackPointerB; diff --git a/BepuPhysics/Trees/Tree.cs b/BepuPhysics/Trees/Tree.cs index 9764a34af..08c0d44e9 100644 --- a/BepuPhysics/Trees/Tree.cs +++ b/BepuPhysics/Trees/Tree.cs @@ -77,7 +77,7 @@ public readonly void GetBoundsPointers(int leafIndex, out Vector3* minPointer, o /// New minimum bounds for the leaf. /// New maximum bounds for the leaf. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe readonly void UpdateBounds(int leafIndex, Vector3 min, Vector3 max) + public readonly void UpdateBounds(int leafIndex, Vector3 min, Vector3 max) { GetBoundsPointers(leafIndex, out var minPointer, out var maxPointer); *minPointer = min; @@ -90,7 +90,7 @@ public unsafe readonly void UpdateBounds(int leafIndex, Vector3 min, Vector3 max /// /// Buffer pool to use to allocate resources in the tree. /// Initial number of leaves to allocate room for. - public unsafe Tree(BufferPool pool, int initialLeafCapacity = 4096) : this() + public Tree(BufferPool pool, int initialLeafCapacity = 4096) : this() { if (initialLeafCapacity <= 0) throw new ArgumentException("Initial leaf capacity must be positive."); diff --git a/BepuPhysics/Trees/Tree_Add.cs b/BepuPhysics/Trees/Tree_Add.cs index 8c236847c..e85bcb5e2 100644 --- a/BepuPhysics/Trees/Tree_Add.cs +++ b/BepuPhysics/Trees/Tree_Add.cs @@ -21,7 +21,7 @@ private struct InsertShouldRotateBottomUp { } /// but the quality of the tree depends on insertion order. /// Pathological insertion orders can result in a maximally imbalanced tree, quadratic insertion times across the full tree build, and query performance linear in the number of leaves. /// This is typically best reserved for cases where the insertion order is known to be randomized or otherwise conducive to building decent trees. - public unsafe int AddWithoutRefinement(BoundingBox bounds, BufferPool pool) + public int AddWithoutRefinement(BoundingBox bounds, BufferPool pool) { return Add(bounds, pool); } @@ -35,7 +35,7 @@ public unsafe int AddWithoutRefinement(BoundingBox bounds, BufferPool pool) /// Performs incrementally refining tree rotations down along the insertion path, unlike . /// For a given tree, this is slightly slower than and slightly faster than . /// Trees built with repeated insertions of this kind tend to have decent quality, but slightly worse than . - public unsafe int Add(BoundingBox bounds, BufferPool pool) + public int Add(BoundingBox bounds, BufferPool pool) { return Add(bounds, pool); } @@ -48,12 +48,12 @@ public unsafe int Add(BoundingBox bounds, BufferPool pool) /// Index of the leaf allocated in the tree's leaf array. /// Performs incrementally refining tree rotations up along the insertion path, unlike . /// Trees built with repeated insertions of this kind tend to have slightly better quality than , but it is also slightly more expensive. - public unsafe int AddWithBottomUpRefinement(BoundingBox bounds, BufferPool pool) + public int AddWithBottomUpRefinement(BoundingBox bounds, BufferPool pool) { return Add(bounds, pool); } - private unsafe int Add(BoundingBox bounds, BufferPool pool) where TShouldRotate : unmanaged + private int Add(BoundingBox bounds, BufferPool pool) where TShouldRotate : unmanaged { //The rest of the function assumes we have sufficient room. We don't want to deal with invalidated pointers mid-add. if (Leaves.Length == LeafCount) @@ -143,7 +143,7 @@ private unsafe int Add(BoundingBox bounds, BufferPool pool) where return newLeafIndex; } - private unsafe void TryRotateNode(int rotationRootIndex) + private void TryRotateNode(int rotationRootIndex) { ref var root = ref Nodes[rotationRootIndex]; var costA = ComputeBoundsMetric(Unsafe.As(ref root.A)); diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 71d170f7f..688fbfa09 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -334,7 +334,7 @@ static unsafe void MicroSweepForBinnedBuilder( } [MethodImpl(MethodImplOptions.AggressiveInlining)] - private static unsafe int ComputeBinIndex(Vector4 centroidMin, bool useX, bool useY, Vector128 permuteMask, int axisIndex, Vector4 offsetToBinIndex, Vector4 maximumBinIndex, in BoundingBox4 box) + private static int ComputeBinIndex(Vector4 centroidMin, bool useX, bool useY, Vector128 permuteMask, int axisIndex, Vector4 offsetToBinIndex, Vector4 maximumBinIndex, in BoundingBox4 box) { var centroid = box.Min + box.Max; //Note the clamp against zero as well as maximumBinIndex; going negative *can* happen when the bounding box is corrupted. We'd rather not crash with an access violation. @@ -395,7 +395,7 @@ static Buffer Suballocate(Buffer buffer, ref int start, int count) w /// Some of the resources cached here are technically redundant with the storage used for workers and ends up involving an extra bin scan on a multithreaded test, /// but the cost associated with doing so is... low. The complexity cost of trying to use the memory allocated for workers is not low. /// - unsafe struct BinnedBuildWorkerContext + struct BinnedBuildWorkerContext { /// /// Bins associated with this worker for the duration of a node. This allocation will persist across the build. @@ -524,7 +524,8 @@ public void GetSlotInterval(long taskId, out int start, out int count) count = taskId >= SlotRemainder ? SlotsPerTaskBase : SlotsPerTaskBase + 1; } } - unsafe struct CentroidPrepassTaskContext + + struct CentroidPrepassTaskContext { public SharedTaskData TaskData; /// @@ -824,7 +825,8 @@ struct PartitionCounters [FieldOffset(134)] public int SubtreeCountB; } - unsafe struct PartitionTaskContext + + struct PartitionTaskContext { public SharedTaskData TaskData; diff --git a/BepuPhysics/Trees/Tree_CacheOptimizer.cs b/BepuPhysics/Trees/Tree_CacheOptimizer.cs index 931711f79..d8362f4c3 100644 --- a/BepuPhysics/Trees/Tree_CacheOptimizer.cs +++ b/BepuPhysics/Trees/Tree_CacheOptimizer.cs @@ -5,7 +5,7 @@ namespace BepuPhysics.Trees; partial struct Tree { - unsafe void SwapNodes(int indexA, int indexB) + void SwapNodes(int indexA, int indexB) { ref var a = ref Nodes[indexA]; ref var b = ref Nodes[indexB]; @@ -89,7 +89,7 @@ public int ComputeCacheOptimalLocation(int nodeIndex) return leftNodeCount; } - unsafe void CacheOptimize(int nodeIndex, ref int nextIndex) + void CacheOptimize(int nodeIndex, ref int nextIndex) { ref var node = ref Nodes[nodeIndex]; ref var children = ref node.A; @@ -114,7 +114,7 @@ unsafe void CacheOptimize(int nodeIndex, ref int nextIndex) /// Requires that the targeted node is already at the global optimum position. /// /// Node to begin the optimization process at. - public unsafe void CacheOptimize(int nodeIndex) + public void CacheOptimize(int nodeIndex) { if (LeafCount <= 2) { @@ -127,7 +127,7 @@ public unsafe void CacheOptimize(int nodeIndex) CacheOptimize(nodeIndex, ref targetIndex); } - private unsafe void CacheOptimizedLimitedSubtreeInternal(int sourceNodeIndex, int targetNodeIndex, int nodeOptimizationCount) + private void CacheOptimizedLimitedSubtreeInternal(int sourceNodeIndex, int targetNodeIndex, int nodeOptimizationCount) { if (sourceNodeIndex != targetNodeIndex) SwapNodes(targetNodeIndex, sourceNodeIndex); @@ -153,7 +153,7 @@ private unsafe void CacheOptimizedLimitedSubtreeInternal(int sourceNodeIndex, in /// Node index to start the optimization process at. /// Number of nodes to move. /// This optimizer will move the targeted node index to the globally optimal location if necessary. - public unsafe void CacheOptimizeLimitedSubtree(int nodeIndex, int nodeOptimizationCount) + public void CacheOptimizeLimitedSubtree(int nodeIndex, int nodeOptimizationCount) { if (LeafCount <= 2) return; @@ -170,7 +170,7 @@ public unsafe void CacheOptimizeLimitedSubtree(int nodeIndex, int nodeOptimizati /// Node index to start the optimization at. /// Number of nodes to try to optimize. /// Number of nodes optimized. - public unsafe int CacheOptimizeRegion(int startingNodeIndex, int targetCount) + public int CacheOptimizeRegion(int startingNodeIndex, int targetCount) { if (LeafCount <= 2) return 0; diff --git a/BepuPhysics/Trees/Tree_Diagnostics.cs b/BepuPhysics/Trees/Tree_Diagnostics.cs index b8db8cde4..035de97cb 100644 --- a/BepuPhysics/Trees/Tree_Diagnostics.cs +++ b/BepuPhysics/Trees/Tree_Diagnostics.cs @@ -13,7 +13,7 @@ partial struct Tree //While this may be closer to true that it appears at first glance due to the very high cost of cache misses versus trivial ALU work, //it's probably not *identical*. //The builders also use this approximation. - public unsafe float MeasureCostMetric() + public float MeasureCostMetric() { //Assumption: Index 0 is always the root if it exists, and an empty tree will have a 'root' with a child count of 0. ref var rootNode = ref Nodes[0]; @@ -59,7 +59,7 @@ public unsafe float MeasureCostMetric() } - readonly unsafe void Validate(int nodeIndex, int expectedParentIndex, int expectedIndexInParent, ref Vector3 expectedMin, ref Vector3 expectedMax, out int foundLeafCount) + readonly void Validate(int nodeIndex, int expectedParentIndex, int expectedIndexInParent, ref Vector3 expectedMin, ref Vector3 expectedMax, out int foundLeafCount) { ref var node = ref Nodes[nodeIndex]; ref var metanode = ref Metanodes[nodeIndex]; @@ -125,7 +125,7 @@ readonly unsafe void Validate(int nodeIndex, int expectedParentIndex, int expect } } - readonly unsafe void ValidateLeafNodeIndices() + readonly void ValidateLeafNodeIndices() { for (int i = 0; i < LeafCount; ++i) { @@ -140,7 +140,7 @@ readonly unsafe void ValidateLeafNodeIndices() } } - readonly unsafe void ValidateLeaves() + readonly void ValidateLeaves() { ValidateLeafNodeIndices(); @@ -153,7 +153,7 @@ readonly unsafe void ValidateLeaves() } } - public readonly unsafe void Validate() + public readonly void Validate() { if (NodeCount < 0) { @@ -182,7 +182,7 @@ public readonly unsafe void Validate() } - readonly unsafe int ComputeMaximumDepth(ref Node node, int currentDepth) + readonly int ComputeMaximumDepth(ref Node node, int currentDepth) { ref var children = ref node.A; int maximum = currentDepth; @@ -201,12 +201,12 @@ readonly unsafe int ComputeMaximumDepth(ref Node node, int currentDepth) return maximum; } - public readonly unsafe int ComputeMaximumDepth() + public readonly int ComputeMaximumDepth() { return ComputeMaximumDepth(ref Nodes[0], 0); } - readonly unsafe void MeasureCacheQuality(int nodeIndex, out int foundNodes, out float nodeScore, out int scorableNodeCount) + readonly void MeasureCacheQuality(int nodeIndex, out int foundNodes, out float nodeScore, out int scorableNodeCount) { ref var node = ref Nodes[nodeIndex]; ref var children = ref node.A; @@ -244,14 +244,14 @@ readonly unsafe void MeasureCacheQuality(int nodeIndex, out int foundNodes, out ++scorableNodeCount; } } - public readonly unsafe float MeasureCacheQuality() + public readonly float MeasureCacheQuality() { MeasureCacheQuality(0, out int foundNodes, out float nodeScore, out int scorableNodeCount); return scorableNodeCount > 0 ? nodeScore / scorableNodeCount : 1; } - public readonly unsafe float MeasureCacheQuality(int nodeIndex) + public readonly float MeasureCacheQuality(int nodeIndex) { if (nodeIndex < 0 || nodeIndex >= NodeCount) throw new ArgumentException("Measurement target index must be nonnegative and less than node count."); diff --git a/BepuPhysics/Trees/Tree_IntertreeQueries.cs b/BepuPhysics/Trees/Tree_IntertreeQueries.cs index d8332ba73..863d16c4e 100644 --- a/BepuPhysics/Trees/Tree_IntertreeQueries.cs +++ b/BepuPhysics/Trees/Tree_IntertreeQueries.cs @@ -5,7 +5,7 @@ namespace BepuPhysics.Trees { partial struct Tree { - unsafe void DispatchTestForNodeAgainstLeaf(int leafIndex, ref NodeChild leafChild, int nodeIndex, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler + void DispatchTestForNodeAgainstLeaf(int leafIndex, ref NodeChild leafChild, int nodeIndex, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler { if (nodeIndex < 0) { @@ -16,7 +16,7 @@ unsafe void DispatchTestForNodeAgainstLeaf(int leafIndex, ref N TestNodeAgainstLeaf(nodeIndex, leafIndex, ref leafChild, ref results); } } - private unsafe void TestNodeAgainstLeaf(int nodeIndex, int leafIndex, ref NodeChild leafChild, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler + private void TestNodeAgainstLeaf(int nodeIndex, int leafIndex, ref NodeChild leafChild, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler { ref var node = ref Nodes[nodeIndex]; ref var a = ref node.A; @@ -38,7 +38,7 @@ private unsafe void TestNodeAgainstLeaf(int nodeIndex, int leaf } } - unsafe void DispatchTestForLeafAgainstNode(int leafIndex, ref NodeChild leafChild, int nodeIndex, ref Tree treeB, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler + void DispatchTestForLeafAgainstNode(int leafIndex, ref NodeChild leafChild, int nodeIndex, ref Tree treeB, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler { if (nodeIndex < 0) { @@ -49,7 +49,8 @@ unsafe void DispatchTestForLeafAgainstNode(int leafIndex, ref N TestLeafAgainstNode(leafIndex, ref leafChild, nodeIndex, ref treeB, ref results); } } - unsafe void TestLeafAgainstNode(int leafIndex, ref NodeChild leafChild, int nodeIndex, ref Tree treeB, ref TOverlapHandler results) + + void TestLeafAgainstNode(int leafIndex, ref NodeChild leafChild, int nodeIndex, ref Tree treeB, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler { ref var node = ref treeB.Nodes[nodeIndex]; @@ -73,7 +74,7 @@ unsafe void TestLeafAgainstNode(int leafIndex, ref NodeChild le } [MethodImpl(MethodImplOptions.AggressiveInlining)] - unsafe void DispatchTestForNodes(ref NodeChild a, ref NodeChild b, ref Tree treeB, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler + void DispatchTestForNodes(ref NodeChild a, ref NodeChild b, ref Tree treeB, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler { if (a.Index >= 0) { @@ -99,7 +100,7 @@ unsafe void DispatchTestForNodes(ref NodeChild a, ref NodeChild } } - private unsafe void GetOverlapsBetweenDifferentNodes(ref Node a, ref Node b, ref Tree treeB, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler + private void GetOverlapsBetweenDifferentNodes(ref Node a, ref Node b, ref Tree treeB, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler { ref var aa = ref a.A; ref var ab = ref a.B; @@ -128,7 +129,7 @@ private unsafe void GetOverlapsBetweenDifferentNodes(ref Node a } } - public unsafe void GetOverlaps(ref Tree treeB, ref TOverlapHandler overlapHandler) where TOverlapHandler : struct, IOverlapHandler + public void GetOverlaps(ref Tree treeB, ref TOverlapHandler overlapHandler) where TOverlapHandler : struct, IOverlapHandler { if (LeafCount == 0 || treeB.LeafCount == 0) return; diff --git a/BepuPhysics/Trees/Tree_IntertreeQueriesMT.cs b/BepuPhysics/Trees/Tree_IntertreeQueriesMT.cs index fc6b742d8..81c91b7cc 100644 --- a/BepuPhysics/Trees/Tree_IntertreeQueriesMT.cs +++ b/BepuPhysics/Trees/Tree_IntertreeQueriesMT.cs @@ -38,7 +38,7 @@ public MultithreadedIntertreeTest(BufferPool pool) /// /// Callbacks used to handle individual overlaps detected by the self test. /// Number of threads to prepare jobs for. - public unsafe void PrepareJobs(ref Tree treeA, ref Tree treeB, TOverlapHandler[] overlapHandlers, int threadCount) + public void PrepareJobs(ref Tree treeA, ref Tree treeB, TOverlapHandler[] overlapHandlers, int threadCount) { if (treeA.LeafCount == 0 || treeB.LeafCount == 0) { @@ -116,7 +116,7 @@ public void CompleteTest() jobs.Dispose(Pool); } - public unsafe void ExecuteJob(int jobIndex, int workerIndex) + public void ExecuteJob(int jobIndex, int workerIndex) { ref var overlap = ref jobs[jobIndex]; if (overlap.A >= 0) @@ -151,7 +151,7 @@ public unsafe void ExecuteJob(int jobIndex, int workerIndex) /// Executes a single worker of the multithreaded self test. /// /// Index of the worker executing this set of tests. - public unsafe void PairTest(int workerIndex) + public void PairTest(int workerIndex) { Debug.Assert(workerIndex >= 0 && workerIndex < OverlapHandlers.Length); int nextNodePairIndex; @@ -162,7 +162,7 @@ public unsafe void PairTest(int workerIndex) } } - unsafe void DispatchTestForLeaf(ref Tree nodeOwner, int leafIndex, ref NodeChild leafChild, int nodeIndex, int nodeLeafCount, ref TOverlapHandler results) + void DispatchTestForLeaf(ref Tree nodeOwner, int leafIndex, ref NodeChild leafChild, int nodeIndex, int nodeLeafCount, ref TOverlapHandler results) { if (nodeIndex < 0) { @@ -187,7 +187,7 @@ unsafe void DispatchTestForLeaf(ref Tree nodeOwner, int leafIndex, ref NodeChild } } - unsafe void TestLeafAgainstNode(ref Tree nodeOwner, int leafIndex, ref NodeChild leafChild, int nodeIndex, ref TOverlapHandler results) + void TestLeafAgainstNode(ref Tree nodeOwner, int leafIndex, ref NodeChild leafChild, int nodeIndex, ref TOverlapHandler results) { ref var node = ref nodeOwner.Nodes[nodeIndex]; ref var a = ref node.A; @@ -211,7 +211,7 @@ unsafe void TestLeafAgainstNode(ref Tree nodeOwner, int leafIndex, ref NodeChild } [MethodImpl(MethodImplOptions.AggressiveInlining)] - unsafe void DispatchTestForNodes(ref NodeChild a, ref NodeChild b, ref TOverlapHandler results) + void DispatchTestForNodes(ref NodeChild a, ref NodeChild b, ref TOverlapHandler results) { if (a.Index >= 0) { @@ -241,7 +241,7 @@ unsafe void DispatchTestForNodes(ref NodeChild a, ref NodeChild b, ref TOverlapH } } - unsafe void GetJobsBetweenDifferentNodes(ref Node a, ref Node b, ref TOverlapHandler results) + void GetJobsBetweenDifferentNodes(ref Node a, ref Node b, ref TOverlapHandler results) { //There are no shared children, so test them all. diff --git a/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs b/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs index 8003f9e48..7f119c1f5 100644 --- a/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs +++ b/BepuPhysics/Trees/Tree_MultithreadedRefitRefine.cs @@ -40,7 +40,7 @@ public RefitAndRefineMultithreadedContext() } - public unsafe void CreateRefitAndMarkJobs(ref Tree tree, BufferPool pool, IThreadDispatcher threadDispatcher) + public void CreateRefitAndMarkJobs(ref Tree tree, BufferPool pool, IThreadDispatcher threadDispatcher) { if (tree.LeafCount <= 2) { @@ -73,7 +73,7 @@ public unsafe void CreateRefitAndMarkJobs(ref Tree tree, BufferPool pool, IThrea RefitNodeIndex = -1; } - public unsafe void CreateRefinementJobs(BufferPool pool, int frameIndex, float refineAggressivenessScale = 1) + public void CreateRefinementJobs(BufferPool pool, int frameIndex, float refineAggressivenessScale = 1) { if (Tree.LeafCount <= 2) { @@ -125,7 +125,7 @@ public unsafe void CreateRefinementJobs(BufferPool pool, int frameIndex, float r RefineIndex = -1; } - public unsafe void CleanUpForRefitAndRefine(BufferPool pool) + public void CleanUpForRefitAndRefine(BufferPool pool) { if (Tree.LeafCount <= 2) { @@ -151,7 +151,7 @@ public unsafe void CleanUpForRefitAndRefine(BufferPool pool) this.threadDispatcher = null; } - public unsafe void RefitAndRefine(ref Tree tree, BufferPool pool, IThreadDispatcher threadDispatcher, int frameIndex, + public void RefitAndRefine(ref Tree tree, BufferPool pool, IThreadDispatcher threadDispatcher, int frameIndex, float refineAggressivenessScale = 1) { CreateRefitAndMarkJobs(ref tree, pool, threadDispatcher); @@ -161,7 +161,7 @@ public unsafe void RefitAndRefine(ref Tree tree, BufferPool pool, IThreadDispatc CleanUpForRefitAndRefine(pool); } - unsafe void CollectNodesForMultithreadedRefit(int nodeIndex, + void CollectNodesForMultithreadedRefit(int nodeIndex, int multithreadingLeafCountThreshold, ref QuickList refitAndMarkTargets, int refinementLeafCountThreshold, ref QuickList refinementCandidates, BufferPool pool, BufferPool threadPool) { @@ -200,7 +200,7 @@ unsafe void CollectNodesForMultithreadedRefit(int nodeIndex, } } - public unsafe void ExecuteRefitAndMarkJob(BufferPool threadPool, int workerIndex, int refitIndex) + public void ExecuteRefitAndMarkJob(BufferPool threadPool, int workerIndex, int refitIndex) { var nodeIndex = RefitNodes[refitIndex]; bool shouldUseMark; @@ -309,7 +309,7 @@ public unsafe void ExecuteRefitAndMarkJob(BufferPool threadPool, int workerIndex } } } - public unsafe void RefitAndMarkForWorker(int workerIndex) + public void RefitAndMarkForWorker(int workerIndex) { if (RefitNodes.Count == 0) return; @@ -326,14 +326,14 @@ public unsafe void RefitAndMarkForWorker(int workerIndex) } - public unsafe void ExecuteRefineJob(ref QuickList subtreeReferences, ref QuickList treeletInternalNodes, ref BinnedResources resources, BufferPool threadPool, int refineIndex) + public void ExecuteRefineJob(ref QuickList subtreeReferences, ref QuickList treeletInternalNodes, ref BinnedResources resources, BufferPool threadPool, int refineIndex) { Tree.BinnedRefine(RefinementTargets[refineIndex], ref subtreeReferences, MaximumSubtrees, ref treeletInternalNodes, ref resources, threadPool); subtreeReferences.Count = 0; treeletInternalNodes.Count = 0; } - public unsafe void RefineForWorker(int workerIndex) + public void RefineForWorker(int workerIndex) { if (RefinementTargets.Count == 0) return; @@ -358,7 +358,7 @@ public unsafe void RefineForWorker(int workerIndex) } } - unsafe void CheckForRefinementOverlaps(int nodeIndex, ref QuickList refinementTargets) + void CheckForRefinementOverlaps(int nodeIndex, ref QuickList refinementTargets) { ref var node = ref Nodes[nodeIndex]; ref var children = ref node.A; diff --git a/BepuPhysics/Trees/Tree_Refine2.cs b/BepuPhysics/Trees/Tree_Refine2.cs index 5a976d11c..b12841a17 100644 --- a/BepuPhysics/Trees/Tree_Refine2.cs +++ b/BepuPhysics/Trees/Tree_Refine2.cs @@ -46,7 +46,7 @@ readonly void ReifyRootRefinementNodeChild(ref int index, ref QuickList ref } } - static unsafe void ReifyRootRefinement(int startIndex, int endIndex, QuickList nodeIndices, Buffer refinementNodes, Tree tree) + static void ReifyRootRefinement(int startIndex, int endIndex, QuickList nodeIndices, Buffer refinementNodes, Tree tree) { for (int i = startIndex; i < endIndex; ++i) { @@ -128,7 +128,7 @@ readonly void ReifySubtreeRefinementNodeChild(ref int index, ref QuickList } } - static unsafe void ReifySubtreeRefinement(int startIndex, int endIndex, QuickList nodeIndices, Buffer refinementNodes, Tree tree) + static void ReifySubtreeRefinement(int startIndex, int endIndex, QuickList nodeIndices, Buffer refinementNodes, Tree tree) { for (int i = startIndex; i < endIndex; ++i) { @@ -329,7 +329,8 @@ internal struct HeapEntry public int Index; public float Cost; } - unsafe internal struct BinaryHeap + + internal struct BinaryHeap { public Buffer Entries; public int Count; @@ -347,7 +348,7 @@ public void Dispose(BufferPool pool) pool.Return(ref Entries); } - public unsafe void Insert(int indexToInsert, float cost) + public void Insert(int indexToInsert, float cost) { int index = Count; ++Count; diff --git a/BepuPhysics/Trees/Tree_RefineCommon.cs b/BepuPhysics/Trees/Tree_RefineCommon.cs index ca27a7b05..18b8043dd 100644 --- a/BepuPhysics/Trees/Tree_RefineCommon.cs +++ b/BepuPhysics/Trees/Tree_RefineCommon.cs @@ -23,7 +23,7 @@ public SubtreeBinaryHeap(SubtreeHeapEntry* entries) } - public unsafe void Insert(ref Node node, ref QuickList subtrees) + public void Insert(ref Node node, ref QuickList subtrees) { ref var children = ref node.A; for (int childIndex = 0; childIndex < 2; ++childIndex) diff --git a/BepuPhysics/Trees/Tree_RefinementScheduling.cs b/BepuPhysics/Trees/Tree_RefinementScheduling.cs index 85f2fc696..7ff7ffeb5 100644 --- a/BepuPhysics/Trees/Tree_RefinementScheduling.cs +++ b/BepuPhysics/Trees/Tree_RefinementScheduling.cs @@ -10,8 +10,7 @@ namespace BepuPhysics.Trees { partial struct Tree { - - unsafe float RefitAndMeasure(ref NodeChild child) + float RefitAndMeasure(ref NodeChild child) { ref var node = ref Nodes[child.Index]; @@ -37,7 +36,7 @@ unsafe float RefitAndMeasure(ref NodeChild child) } - unsafe float RefitAndMark(ref NodeChild child, int leafCountThreshold, ref QuickList refinementCandidates, BufferPool pool) + float RefitAndMark(ref NodeChild child, int leafCountThreshold, ref QuickList refinementCandidates, BufferPool pool) { Debug.Assert(leafCountThreshold > 1); @@ -87,7 +86,7 @@ unsafe float RefitAndMark(ref NodeChild child, int leafCountThreshold, ref Quick } - unsafe float RefitAndMark(int leafCountThreshold, ref QuickList refinementCandidates, BufferPool pool) + float RefitAndMark(int leafCountThreshold, ref QuickList refinementCandidates, BufferPool pool) { Debug.Assert(LeafCount > 2, "There's no reason to refit a tree with 2 or less elements. Nothing would happen."); @@ -131,9 +130,7 @@ unsafe float RefitAndMark(int leafCountThreshold, ref QuickList refinementC } - - - unsafe void ValidateRefineFlags(int index) + void ValidateRefineFlags(int index) { ref var metanode = ref Metanodes[index]; if (metanode.RefineFlag != 0) @@ -178,7 +175,7 @@ readonly void GetRefineTuning(int frameIndex, int refinementCandidatesCount, flo targetRefinementCount = Math.Min(refinementCandidatesCount, (int)targetRefinementScale); } - public unsafe void RefitAndRefine(BufferPool pool, int frameIndex, float refineAggressivenessScale = 1) + public void RefitAndRefine(BufferPool pool, int frameIndex, float refineAggressivenessScale = 1) { //Don't proceed if the tree has no refitting or refinement required. This also guarantees that any nodes that do exist have two children. if (LeafCount <= 2) diff --git a/BepuPhysics/Trees/Tree_Refit.cs b/BepuPhysics/Trees/Tree_Refit.cs index 5ab2112e5..9e8f9d54f 100644 --- a/BepuPhysics/Trees/Tree_Refit.cs +++ b/BepuPhysics/Trees/Tree_Refit.cs @@ -11,7 +11,7 @@ partial struct Tree /// Refits the bounding box of every parent of the node recursively to the root. /// /// Node to propagate a node change for. - public unsafe readonly void RefitForNodeBoundsChange(int nodeIndex) + public readonly void RefitForNodeBoundsChange(int nodeIndex) { //Note that no attempt is made to refit the root node. Note that the root node is the only node that can have a number of children less than 2. ref var node = ref Nodes[nodeIndex]; @@ -29,7 +29,7 @@ public unsafe readonly void RefitForNodeBoundsChange(int nodeIndex) //TODO: Recursive approach is a bit silly. Our earlier nonrecursive implementations weren't great, but we could do better. //This is especially true if we end up changing the memory layout. If we go back to a contiguous array per level, refit becomes trivial. //That would only happen if it turns out useful for other parts of the execution, though- optimizing refits at the cost of self-tests would be a terrible idea. - readonly unsafe void Refit(int nodeIndex, out Vector3 min, out Vector3 max) + readonly void Refit(int nodeIndex, out Vector3 min, out Vector3 max) { Debug.Assert(LeafCount >= 2); ref var node = ref Nodes[nodeIndex]; @@ -48,7 +48,7 @@ readonly unsafe void Refit(int nodeIndex, out Vector3 min, out Vector3 max) /// /// Updates the bounding boxes of all internal nodes in the tree. /// - public unsafe readonly void Refit() + public readonly void Refit() { //No point in refitting a tree with no internal nodes! if (LeafCount <= 2) diff --git a/BepuPhysics/Trees/Tree_Refit2.cs b/BepuPhysics/Trees/Tree_Refit2.cs index 82332afde..5916eca5a 100644 --- a/BepuPhysics/Trees/Tree_Refit2.cs +++ b/BepuPhysics/Trees/Tree_Refit2.cs @@ -8,7 +8,7 @@ namespace BepuPhysics.Trees; partial struct Tree { - readonly unsafe void Refit2(ref NodeChild childInParent) + readonly void Refit2(ref NodeChild childInParent) { Debug.Assert(LeafCount >= 2); ref var node = ref Nodes[childInParent.Index]; @@ -27,7 +27,7 @@ readonly unsafe void Refit2(ref NodeChild childInParent) /// /// Updates the bounding boxes of all internal nodes in the tree. /// - public unsafe readonly void Refit2() + public readonly void Refit2() { //No point in refitting a tree with no internal nodes! if (LeafCount <= 2) @@ -151,7 +151,7 @@ unsafe struct RefitWithCacheOptimizationContext public TaskStack* TaskStack; } - static unsafe void Refit2WithCacheOptimization(int sourceNodeIndex, int parentIndex, int childIndexInParent, ref NodeChild childInParent, ref RefitWithCacheOptimizationContext context) + static void Refit2WithCacheOptimization(int sourceNodeIndex, int parentIndex, int childIndexInParent, ref NodeChild childInParent, ref RefitWithCacheOptimizationContext context) { Debug.Assert(context.Tree.LeafCount >= 2); @@ -201,7 +201,7 @@ static unsafe void Refit2WithCacheOptimization(int sourceNodeIndex, int parentIn /// The input source buffer is not modified. /// /// Nodes to base the refit on. - public unsafe void Refit2WithCacheOptimization(Buffer sourceNodes) + public void Refit2WithCacheOptimization(Buffer sourceNodes) { //No point in refitting a tree with no internal nodes! if (LeafCount <= 2) @@ -222,7 +222,7 @@ public unsafe void Refit2WithCacheOptimization(Buffer sourceNodes) /// /// Pool to allocate from. If disposeOriginals is true, this must be the same pool from which the buffer was allocated from. /// Whether to dispose of the original nodes buffer. If false, it's up to the caller to dispose of it appropriately. - public unsafe void Refit2WithCacheOptimization(BufferPool pool, bool disposeOriginalNodes = true) + public void Refit2WithCacheOptimization(BufferPool pool, bool disposeOriginalNodes = true) { //No point in refitting a tree with no internal nodes! if (LeafCount <= 2) diff --git a/BepuPhysics/Trees/Tree_Remove.cs b/BepuPhysics/Trees/Tree_Remove.cs index b94bbe8ac..9f505feac 100644 --- a/BepuPhysics/Trees/Tree_Remove.cs +++ b/BepuPhysics/Trees/Tree_Remove.cs @@ -7,7 +7,7 @@ namespace BepuPhysics.Trees { partial struct Tree { - unsafe void RemoveNodeAt(int nodeIndex) + void RemoveNodeAt(int nodeIndex) { //Note that this function is a cache scrambling influence. That's okay- the cache optimization routines will take care of it later. Debug.Assert(nodeIndex < NodeCount && nodeIndex >= 0); @@ -48,7 +48,7 @@ unsafe void RemoveNodeAt(int nodeIndex) } - unsafe void RefitForRemoval(int nodeIndex) + void RefitForRemoval(int nodeIndex) { //Note that no attempt is made to refit the root node. Note that the root node is the only node that can have a number of children less than 2. ref var node = ref Nodes[nodeIndex]; @@ -71,7 +71,7 @@ unsafe void RefitForRemoval(int nodeIndex) /// Index of the leaf to remove. /// Former index of the leaf that was moved into the removed leaf's slot, if any. /// If leafIndex pointed at the last slot in the list, then this returns -1 since no leaf was moved. - public unsafe int RemoveAt(int leafIndex) + public int RemoveAt(int leafIndex) { if (leafIndex < 0 || leafIndex >= LeafCount) throw new ArgumentOutOfRangeException("Leaf index must be a valid index in the tree's leaf array."); diff --git a/BepuPhysics/Trees/Tree_SelfQueries.cs b/BepuPhysics/Trees/Tree_SelfQueries.cs index 03035a098..e9bd4eba2 100644 --- a/BepuPhysics/Trees/Tree_SelfQueries.cs +++ b/BepuPhysics/Trees/Tree_SelfQueries.cs @@ -48,7 +48,7 @@ partial struct Tree //Note that all of these implementations make use of a fully generic handler. It could be dumping to a list, or it could be directly processing the results- at this //level of abstraction we don't know or care. It's up to the user to use a handler which maximizes performance if they want it. We'll be using this in the broad phase. - readonly unsafe void DispatchTestForLeaf(int leafIndex, ref NodeChild leafChild, int nodeIndex, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler + readonly void DispatchTestForLeaf(int leafIndex, ref NodeChild leafChild, int nodeIndex, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler { if (nodeIndex < 0) { @@ -59,7 +59,7 @@ readonly unsafe void DispatchTestForLeaf(int leafIndex, ref Nod TestLeafAgainstNode(leafIndex, ref leafChild, nodeIndex, ref results); } } - readonly unsafe void TestLeafAgainstNode(int leafIndex, ref NodeChild leafChild, int nodeIndex, ref TOverlapHandler results) + readonly void TestLeafAgainstNode(int leafIndex, ref NodeChild leafChild, int nodeIndex, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler { ref var node = ref Nodes[nodeIndex]; @@ -83,7 +83,7 @@ readonly unsafe void TestLeafAgainstNode(int leafIndex, ref Nod } [MethodImpl(MethodImplOptions.AggressiveInlining)] - readonly unsafe void DispatchTestForNodes(ref NodeChild a, ref NodeChild b, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler + readonly void DispatchTestForNodes(ref NodeChild a, ref NodeChild b, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler { if (a.Index >= 0) { @@ -109,7 +109,7 @@ readonly unsafe void DispatchTestForNodes(ref NodeChild a, ref } } - readonly unsafe void GetOverlapsBetweenDifferentNodes(ref Node a, ref Node b, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler + readonly void GetOverlapsBetweenDifferentNodes(ref Node a, ref Node b, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler { //There are no shared children, so test them all. ref var aa = ref a.A; @@ -139,7 +139,7 @@ readonly unsafe void GetOverlapsBetweenDifferentNodes(ref Node } } - readonly unsafe void GetOverlapsInNode(ref Node node, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler + readonly void GetOverlapsInNode(ref Node node, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler { ref var a = ref node.A; @@ -156,7 +156,7 @@ readonly unsafe void GetOverlapsInNode(ref Node node, ref TOver } } - public readonly unsafe void GetSelfOverlaps(ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler + public readonly void GetSelfOverlaps(ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler { //If there are less than two leaves, there can't be any overlap. //This provides a guarantee that there are at least 2 children in each internal node considered by GetOverlapsInNode. @@ -166,7 +166,7 @@ public readonly unsafe void GetSelfOverlaps(ref TOverlapHandler GetOverlapsInNode(ref Nodes[0], ref results); } - unsafe void GetOverlapsWithLeaf(ref TOverlapHandler results, NodeChild leaf, int nodeToTest, ref QuickList stack) where TOverlapHandler : IOverlapHandler + void GetOverlapsWithLeaf(ref TOverlapHandler results, NodeChild leaf, int nodeToTest, ref QuickList stack) where TOverlapHandler : IOverlapHandler { var leafIndex = Encode(leaf.Index); Debug.Assert(stack.Count == 0); @@ -341,7 +341,8 @@ unsafe void AddCrossoverResult(ref NodeChild a, ref NodeChild b : new NodeLeafPair { LeafParent = (NodeChild*)Unsafe.AsPointer(ref a), NodeIndex = b.Index }; } } - unsafe void ExecuteCrossoverBatch(ref QuickList crossovers, ref QuickList nodeLeaf, ref TOverlapHandler results, BufferPool pool) where TOverlapHandler : IOverlapHandler + + void ExecuteCrossoverBatch(ref QuickList crossovers, ref QuickList nodeLeaf, ref TOverlapHandler results, BufferPool pool) where TOverlapHandler : IOverlapHandler { while (crossovers.TryPop(out var pair)) { @@ -411,7 +412,7 @@ unsafe void ExecuteNodeLeafBatch(ref QuickList no } } - unsafe void FlushLeafLeaf(ref QuickList leafLeaf, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler + void FlushLeafLeaf(ref QuickList leafLeaf, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler { for (int leafLeafIndex = 0; leafLeafIndex < leafLeaf.Count; ++leafLeafIndex) { @@ -470,7 +471,7 @@ public unsafe void GetSelfOverlapsBatched(ref TOverlapHandler r - readonly unsafe void GetSelfOverlaps2(ref TOverlapHandler results, int start, int end) where TOverlapHandler : IOverlapHandler + readonly void GetSelfOverlaps2(ref TOverlapHandler results, int start, int end) where TOverlapHandler : IOverlapHandler { Debug.Assert(end >= 0 && end <= NodeCount && start >= 0 && start < NodeCount); for (int i = end - 1; i >= start; --i) @@ -490,7 +491,7 @@ readonly unsafe void GetSelfOverlaps2(ref TOverlapHandler resul /// Reports all bounding box overlaps between leaves in the tree to the given . /// /// Handler to report results to. - public readonly unsafe void GetSelfOverlaps2(ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler + public readonly void GetSelfOverlaps2(ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler { GetSelfOverlaps2(ref results, 0, NodeCount); } diff --git a/BepuPhysics/Trees/Tree_SelfQueriesMT.cs b/BepuPhysics/Trees/Tree_SelfQueriesMT.cs index 3234237c6..762b2bc7d 100644 --- a/BepuPhysics/Trees/Tree_SelfQueriesMT.cs +++ b/BepuPhysics/Trees/Tree_SelfQueriesMT.cs @@ -99,7 +99,7 @@ public void CompleteSelfTest() jobs.Dispose(Pool); } - public unsafe void ExecuteJob(int jobIndex, int workerIndex) + public void ExecuteJob(int jobIndex, int workerIndex) { ref var overlap = ref jobs[jobIndex]; if (overlap.A >= 0) @@ -139,7 +139,7 @@ public unsafe void ExecuteJob(int jobIndex, int workerIndex) /// Executes a single worker of the multithreaded self test. /// /// Index of the worker executing this set of tests. - public unsafe void PairTest(int workerIndex) + public void PairTest(int workerIndex) { Debug.Assert(workerIndex >= 0 && workerIndex < OverlapHandlers.Length); int nextNodePairIndex; @@ -150,7 +150,7 @@ public unsafe void PairTest(int workerIndex) } } - unsafe void DispatchTestForLeaf(int leafIndex, ref NodeChild leafChild, int nodeIndex, int nodeLeafCount, ref TOverlapHandler results) + void DispatchTestForLeaf(int leafIndex, ref NodeChild leafChild, int nodeIndex, int nodeLeafCount, ref TOverlapHandler results) { if (nodeIndex < 0) { @@ -165,7 +165,7 @@ unsafe void DispatchTestForLeaf(int leafIndex, ref NodeChild leafChild, int node } } - unsafe void TestLeafAgainstNode(int leafIndex, ref NodeChild leafChild, int nodeIndex, ref TOverlapHandler results) + void TestLeafAgainstNode(int leafIndex, ref NodeChild leafChild, int nodeIndex, ref TOverlapHandler results) { ref var node = ref Tree.Nodes[nodeIndex]; ref var a = ref node.A; @@ -189,7 +189,7 @@ unsafe void TestLeafAgainstNode(int leafIndex, ref NodeChild leafChild, int node } [MethodImpl(MethodImplOptions.AggressiveInlining)] - unsafe void DispatchTestForNodes(ref NodeChild a, ref NodeChild b, ref TOverlapHandler results) + void DispatchTestForNodes(ref NodeChild a, ref NodeChild b, ref TOverlapHandler results) { if (a.Index >= 0) { @@ -219,7 +219,7 @@ unsafe void DispatchTestForNodes(ref NodeChild a, ref NodeChild b, ref TOverlapH } } - unsafe void GetJobsBetweenDifferentNodes(ref Node a, ref Node b, ref TOverlapHandler results) + void GetJobsBetweenDifferentNodes(ref Node a, ref Node b, ref TOverlapHandler results) { //There are no shared children, so test them all. @@ -251,7 +251,7 @@ unsafe void GetJobsBetweenDifferentNodes(ref Node a, ref Node b, ref TOverlapHan } - unsafe void CollectJobsInNode(int nodeIndex, int leafCount, ref TOverlapHandler results) + void CollectJobsInNode(int nodeIndex, int leafCount, ref TOverlapHandler results) { if (leafCount <= leafThreshold) { diff --git a/BepuPhysics/Trees/Tree_Sweep.cs b/BepuPhysics/Trees/Tree_Sweep.cs index d7e1f5b3c..a4a04cefb 100644 --- a/BepuPhysics/Trees/Tree_Sweep.cs +++ b/BepuPhysics/Trees/Tree_Sweep.cs @@ -7,7 +7,7 @@ namespace BepuPhysics.Trees { public interface ISweepLeafTester { - unsafe void TestLeaf(int leafIndex, ref float maximumT); + void TestLeaf(int leafIndex, ref float maximumT); } partial struct Tree { @@ -116,7 +116,7 @@ public readonly unsafe void Sweep(Vector3 min, Vector3 max, Vector3 Sweep(expansion, origin, direction, &treeRay, ref sweepTester); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public readonly unsafe void Sweep(in BoundingBox boundingBox, Vector3 direction, float maximumT, ref TLeafTester sweepTester) where TLeafTester : ISweepLeafTester + public readonly void Sweep(in BoundingBox boundingBox, Vector3 direction, float maximumT, ref TLeafTester sweepTester) where TLeafTester : ISweepLeafTester { Sweep(boundingBox.Min, boundingBox.Max, direction, maximumT, ref sweepTester); } diff --git a/BepuPhysics/Trees/Tree_VolumeQuery.cs b/BepuPhysics/Trees/Tree_VolumeQuery.cs index 8a9b3b416..18cf9ae98 100644 --- a/BepuPhysics/Trees/Tree_VolumeQuery.cs +++ b/BepuPhysics/Trees/Tree_VolumeQuery.cs @@ -80,7 +80,7 @@ public readonly unsafe void GetOverlaps(BoundingBox boundingBox, re } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public readonly unsafe void GetOverlaps(Vector3 min, Vector3 max, ref TEnumerator leafEnumerator) where TEnumerator : IBreakableForEach + public readonly void GetOverlaps(Vector3 min, Vector3 max, ref TEnumerator leafEnumerator) where TEnumerator : IBreakableForEach { GetOverlaps(new BoundingBox(min, max), ref leafEnumerator); } diff --git a/BepuUtilities/BoundingBox.cs b/BepuUtilities/BoundingBox.cs index cb3c625f7..27cbe0ccc 100644 --- a/BepuUtilities/BoundingBox.cs +++ b/BepuUtilities/BoundingBox.cs @@ -121,7 +121,7 @@ public unsafe static bool IntersectsUnsafe(in TA boundingBoxA, in TB bou /// Whether the bounding boxes intersected. /// When possible, prefer using the variant for slightly better performance. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe static bool Intersects(BoundingBox a, BoundingBox b) + public static bool Intersects(BoundingBox a, BoundingBox b) { return IntersectsUnsafe(a, b); } @@ -153,7 +153,7 @@ public static bool Intersects(Vector3 minA, Vector3 maxA, Vector3 minB, Vector3 /// Bounding box to measure. /// Volume of the bounding box. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static unsafe float ComputeVolume(ref BoundingBox box) + public static float ComputeVolume(ref BoundingBox box) { var diagonal = (box.Max - box.Min); return diagonal.X * diagonal.Y * diagonal.Z; @@ -199,7 +199,7 @@ public static void CreateMerged(in BoundingBox a, in BoundingBox b, out Bounding /// Type of the first bounding box-like parameter. /// Type of the second bounding box-like parameter. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe static void CreateMergedUnsafeWithPreservation(in TA boundingBoxA, in TB boundingBoxB, out TA merged) where TA : unmanaged where TB : unmanaged + public static void CreateMergedUnsafeWithPreservation(in TA boundingBoxA, in TB boundingBoxB, out TA merged) where TA : unmanaged where TB : unmanaged { if (Vector128.IsHardwareAccelerated) { @@ -245,7 +245,7 @@ public unsafe static void CreateMergedUnsafeWithPreservation(in TA bound /// Type of the first bounding box-like parameter. /// Type of the second bounding box-like parameter. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe static void CreateMergedUnsafe(in TA boundingBoxA, in TB boundingBoxB, out TA merged) where TA : unmanaged where TB : unmanaged + public static void CreateMergedUnsafe(in TA boundingBoxA, in TB boundingBoxB, out TA merged) where TA : unmanaged where TB : unmanaged { if (Vector128.IsHardwareAccelerated) { diff --git a/BepuUtilities/Collections/IndexSet.cs b/BepuUtilities/Collections/IndexSet.cs index 391e440a5..a3761daad 100644 --- a/BepuUtilities/Collections/IndexSet.cs +++ b/BepuUtilities/Collections/IndexSet.cs @@ -67,7 +67,7 @@ public bool Contains(int index) /// List of indices to check for in the batch. /// True if none of the indices are present in the set, false otherwise. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe bool CanFit(Span indexList) + public bool CanFit(Span indexList) { for (int i = 0; i < indexList.Length; ++i) { diff --git a/BepuUtilities/GatherScatter.cs b/BepuUtilities/GatherScatter.cs index beee70e64..9ecd3f914 100644 --- a/BepuUtilities/GatherScatter.cs +++ b/BepuUtilities/GatherScatter.cs @@ -11,7 +11,7 @@ public static class GatherScatter /// Gets a reference to an element from a vector without using pointers, bypassing direct vector access for codegen reasons. This performs no bounds testing! /// [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static unsafe ref T Get(ref Vector vector, int index) where T : struct + public static ref T Get(ref Vector vector, int index) where T : struct { //TODO: This is a very compiler specific implementation which should be revisited as time goes on. Good chance it will become unnecessary, suboptimal, or counterproductive. return ref Unsafe.Add(ref Unsafe.As, T>(ref vector), index); diff --git a/BepuUtilities/Int3.cs b/BepuUtilities/Int3.cs index ea0cf2c7f..68f76d719 100644 --- a/BepuUtilities/Int3.cs +++ b/BepuUtilities/Int3.cs @@ -13,7 +13,7 @@ public struct Int3 : IEquatable, IEqualityComparerRef public int Y; public int Z; - public unsafe override int GetHashCode() + public override int GetHashCode() { const ulong p1 = 961748927UL; const ulong p2 = 899809343UL; diff --git a/BepuUtilities/Int4.cs b/BepuUtilities/Int4.cs index ea9fe61d6..583310786 100644 --- a/BepuUtilities/Int4.cs +++ b/BepuUtilities/Int4.cs @@ -14,7 +14,7 @@ public struct Int4 : IEquatable, IEqualityComparerRef public int Z; public int W; - public unsafe override int GetHashCode() + public override int GetHashCode() { const ulong p1 = 961748927UL; const ulong p2 = 899809343UL; diff --git a/BepuUtilities/Memory/BufferPool.cs b/BepuUtilities/Memory/BufferPool.cs index 303461468..47eff016a 100644 --- a/BepuUtilities/Memory/BufferPool.cs +++ b/BepuUtilities/Memory/BufferPool.cs @@ -102,14 +102,14 @@ public void EnsureCapacity(int capacity) } - public unsafe readonly byte* GetStartPointerForSlot(int slot) + public readonly byte* GetStartPointerForSlot(int slot) { var blockIndex = slot >> SuballocationsPerBlockShift; var indexInBlock = slot & SuballocationsPerBlockMask; return Blocks[blockIndex] + indexInBlock * SuballocationSize; } - public unsafe void Take(out Buffer buffer) + public void Take(out Buffer buffer) { var slot = Slots.Take(); var blockIndex = slot >> SuballocationsPerBlockShift; @@ -145,7 +145,7 @@ public unsafe void Take(out Buffer buffer) } [Conditional("DEBUG")] - internal unsafe void ValidateBufferIsContained(ref Buffer typedBuffer) where T : unmanaged + internal void ValidateBufferIsContained(ref Buffer typedBuffer) where T : unmanaged { var buffer = typedBuffer.As(); //There are a lot of ways to screw this up. Try to catch as many as possible! @@ -165,7 +165,7 @@ internal unsafe void ValidateBufferIsContained(ref Buffer typedBuffer) whe "The extent of the buffer should fit within the block."); } - public readonly unsafe void Return(int slotIndex) + public readonly void Return(int slotIndex) { #if DEBUG Debug.Assert(outstandingIds.Remove(slotIndex), @@ -319,7 +319,7 @@ internal static void DecomposeId(int bufferId, out int powerIndex, out int slotI /// [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void ReturnUnsafely(int id) + public void ReturnUnsafely(int id) { DecomposeId(id, out var powerIndex, out var slotIndex); pools[powerIndex].Return(slotIndex); @@ -327,7 +327,7 @@ public unsafe void ReturnUnsafely(int id) /// [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void Return(ref Buffer buffer) where T : unmanaged + public void Return(ref Buffer buffer) where T : unmanaged { #if DEBUG DecomposeId(buffer.Id, out var powerIndex, out var slotIndex); diff --git a/BepuUtilities/Symmetric3x3.cs b/BepuUtilities/Symmetric3x3.cs index babf87421..11a25396d 100644 --- a/BepuUtilities/Symmetric3x3.cs +++ b/BepuUtilities/Symmetric3x3.cs @@ -89,7 +89,7 @@ public static float Determinant(in Symmetric3x3 m) /// Matrix to be inverted. /// Inverted matrix. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe static Symmetric3x3 Invert(Symmetric3x3 m) + public static Symmetric3x3 Invert(Symmetric3x3 m) { Invert(m, out var inverse); return inverse; @@ -101,7 +101,7 @@ public unsafe static Symmetric3x3 Invert(Symmetric3x3 m) /// Matrix to be inverted. /// Inverted matrix. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe static void Invert(in Symmetric3x3 m, out Symmetric3x3 inverse) + public static void Invert(in Symmetric3x3 m, out Symmetric3x3 inverse) { var m11 = m.YY * m.ZZ - m.ZY * m.ZY; var m21 = m.ZY * m.ZX - m.ZZ * m.YX; diff --git a/BepuUtilities/TaskScheduling/TaskContinuation.cs b/BepuUtilities/TaskScheduling/TaskContinuation.cs index 6174cc115..281db2cb8 100644 --- a/BepuUtilities/TaskScheduling/TaskContinuation.cs +++ b/BepuUtilities/TaskScheduling/TaskContinuation.cs @@ -3,7 +3,7 @@ /// /// Stores data relevant to tracking task completion and reporting completion for a continuation. /// -public unsafe struct TaskContinuation +public struct TaskContinuation { /// /// Task to run upon completion of the associated task. diff --git a/BepuUtilities/TaskScheduling/TaskStack.cs b/BepuUtilities/TaskScheduling/TaskStack.cs index 28ba60ccd..38593f7e8 100644 --- a/BepuUtilities/TaskScheduling/TaskStack.cs +++ b/BepuUtilities/TaskScheduling/TaskStack.cs @@ -234,7 +234,7 @@ public void PushUnsafely(Span tasks, int workerIndex, IThreadDispatcher di /// Index of the worker stack to push the tasks onto. /// User tag associated with the job spanning the submitted tasks. /// This must not be used while other threads could be performing task pushes or pops that could affect the specified worker. - public unsafe void PushUnsafely(Task task, int workerIndex, IThreadDispatcher dispatcher, ulong tag = 0) + public void PushUnsafely(Task task, int workerIndex, IThreadDispatcher dispatcher, ulong tag = 0) { PushUnsafely(new Span(&task, 1), workerIndex, dispatcher, tag); } @@ -457,7 +457,7 @@ public void RequestStop() /// to be stopped. /// Index of the worker executing this task. /// Dispatcher associated with the execution. - public static unsafe void RequestStopTaskFunction(long id, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) + public static void RequestStopTaskFunction(long id, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) { ((TaskStack*)untypedContext)->RequestStop(); } diff --git a/DemoContentBuilder/MeshBuilder.cs b/DemoContentBuilder/MeshBuilder.cs index bc50362ef..7c3b7ce8d 100644 --- a/DemoContentBuilder/MeshBuilder.cs +++ b/DemoContentBuilder/MeshBuilder.cs @@ -16,7 +16,7 @@ public Stream Open(string materialFilePath) } } - public unsafe static MeshContent Build(Stream dataStream) + public static MeshContent Build(Stream dataStream) { var result = new ObjLoaderFactory().Create(new MaterialStubLoader()).Load(dataStream); var triangles = new List(); diff --git a/DemoContentBuilder/ProjectBuilder.cs b/DemoContentBuilder/ProjectBuilder.cs index 46b26bfc4..c414626c8 100644 --- a/DemoContentBuilder/ProjectBuilder.cs +++ b/DemoContentBuilder/ProjectBuilder.cs @@ -21,7 +21,7 @@ public static string GetRelativePathFromDirectory(string path, string baseDirect } - unsafe static void CollectContentPaths(string projectPath, out string workingPath, + static void CollectContentPaths(string projectPath, out string workingPath, out List shaderPaths, out List contentToBuild) { diff --git a/DemoContentLoader/Texture2DIO.cs b/DemoContentLoader/Texture2DIO.cs index 9a3e8f91f..253abe3f6 100644 --- a/DemoContentLoader/Texture2DIO.cs +++ b/DemoContentLoader/Texture2DIO.cs @@ -4,7 +4,7 @@ namespace DemoContentLoader { public class Texture2DIO { - public unsafe static Texture2DContent Load(BinaryReader reader) + public static Texture2DContent Load(BinaryReader reader) { var width = reader.ReadInt32(); var height = reader.ReadInt32(); diff --git a/DemoRenderer.GL/ConstantsBuffer.cs b/DemoRenderer.GL/ConstantsBuffer.cs index 81541205f..754da1e05 100644 --- a/DemoRenderer.GL/ConstantsBuffer.cs +++ b/DemoRenderer.GL/ConstantsBuffer.cs @@ -37,7 +37,7 @@ public ConstantsBuffer(BufferTarget target, string debugName = "UNNAMED") /// Updates the buffer with the given data. /// /// Data to load into the buffer. - public unsafe void Update(ref T bufferData) => + public void Update(ref T bufferData) => GL.NamedBufferSubData(buffer, IntPtr.Zero, alignedSize, ref bufferData); public void Bind(int index) => GL.BindBufferBase((BufferRangeTarget)target, index, buffer); protected override void DoDispose() => GL.DeleteBuffer(buffer); diff --git a/DemoRenderer.GL/Helpers.cs b/DemoRenderer.GL/Helpers.cs index dc1085c84..a9edeef41 100644 --- a/DemoRenderer.GL/Helpers.cs +++ b/DemoRenderer.GL/Helpers.cs @@ -199,7 +199,7 @@ static void PackDuplicateZeroSNORM(float source, out ushort packed) /// Orientation to pack. /// Packed orientation. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe static ulong PackOrientationU64(Quaternion source) + public static ulong PackOrientationU64(Quaternion source) { //This isn't exactly a clever packing, but with 64 bits, cleverness isn't required. ref var vectorSource = ref Unsafe.As(ref source.X); @@ -214,7 +214,7 @@ public unsafe static ulong PackOrientationU64(Quaternion source) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - static unsafe float UnpackDuplicateZeroSNORM(ushort packed) + static float UnpackDuplicateZeroSNORM(ushort packed) { var unpacked = (packed & ((1 << 15) - 1)) * (1f / ((1 << 15) - 1)); ref var reinterpreted = ref Unsafe.As(ref unpacked); diff --git a/DemoRenderer.GL/ShapeDrawing/ShapesExtractor.cs b/DemoRenderer.GL/ShapeDrawing/ShapesExtractor.cs index 050dab5d4..963648d51 100644 --- a/DemoRenderer.GL/ShapeDrawing/ShapesExtractor.cs +++ b/DemoRenderer.GL/ShapeDrawing/ShapesExtractor.cs @@ -68,7 +68,7 @@ public void ClearInstances() ShapeCache.Clear(); } - private unsafe void AddCompoundChildren(ref Buffer children, Shapes shapes, RigidPose pose, Vector3 color, ref ShapeCache shapeCache, BufferPool pool) + private void AddCompoundChildren(ref Buffer children, Shapes shapes, RigidPose pose, Vector3 color, ref ShapeCache shapeCache, BufferPool pool) { for (int i = 0; i < children.Length; ++i) { diff --git a/DemoRenderer/Constraints/BoundingBoxLineExtractor.cs b/DemoRenderer/Constraints/BoundingBoxLineExtractor.cs index a24354f4a..f3f96bf4e 100644 --- a/DemoRenderer/Constraints/BoundingBoxLineExtractor.cs +++ b/DemoRenderer/Constraints/BoundingBoxLineExtractor.cs @@ -101,7 +101,7 @@ void CreateJobsForTree(in Tree tree, bool active, int simulationIndex, ref Quick } } - internal unsafe void CreateJobs(Simulation simulation, int simulationIndex, ref QuickList lines, ref QuickList jobs, BufferPool pool) + internal void CreateJobs(Simulation simulation, int simulationIndex, ref QuickList lines, ref QuickList jobs, BufferPool pool) { //For now, we only pull the bounding boxes of objects that are active. lines.EnsureCapacity(lines.Count + 12 * (simulation.BroadPhase.ActiveTree.LeafCount + simulation.BroadPhase.StaticTree.LeafCount), pool); diff --git a/Demos/DemoCallbacks.cs b/Demos/DemoCallbacks.cs index 1a7a97925..ccf92db59 100644 --- a/Demos/DemoCallbacks.cs +++ b/Demos/DemoCallbacks.cs @@ -108,7 +108,7 @@ public void IntegrateVelocity(Vector bodyIndices, Vector3Wide position, Qua velocity.Angular = velocity.Angular * angularDampingDt; } } - public unsafe struct DemoNarrowPhaseCallbacks : INarrowPhaseCallbacks + public struct DemoNarrowPhaseCallbacks : INarrowPhaseCallbacks { public SpringSettings ContactSpringiness; public float MaximumRecoveryVelocity; @@ -148,7 +148,7 @@ public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int chi } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold + public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold { pairMaterial.FrictionCoefficient = FrictionCoefficient; pairMaterial.MaximumRecoveryVelocity = MaximumRecoveryVelocity; diff --git a/Demos/Demos/BlockChainDemo.cs b/Demos/Demos/BlockChainDemo.cs index 7d071d04a..2c9f1d467 100644 --- a/Demos/Demos/BlockChainDemo.cs +++ b/Demos/Demos/BlockChainDemo.cs @@ -19,7 +19,7 @@ namespace Demos.Demos /// public class BlockChainDemo : Demo { - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(-30, 8, -60); camera.Yaw = MathHelper.Pi * 3f / 4; diff --git a/Demos/Demos/BouncinessDemo.cs b/Demos/Demos/BouncinessDemo.cs index 6aeab9e6b..470c50fcb 100644 --- a/Demos/Demos/BouncinessDemo.cs +++ b/Demos/Demos/BouncinessDemo.cs @@ -32,7 +32,7 @@ public struct SimpleMaterial public float FrictionCoefficient; public float MaximumRecoveryVelocity; } - public unsafe struct BounceCallbacks : INarrowPhaseCallbacks + public struct BounceCallbacks : INarrowPhaseCallbacks { /// /// Maps entries to their . @@ -65,7 +65,7 @@ public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int chi } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold + public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold { //For the purposes of this demo, we'll use multiplicative blending for the friction and choose spring properties according to which collidable has a higher maximum recovery velocity. var a = CollidableMaterials[pair.A]; @@ -87,7 +87,7 @@ public void Dispose() } } - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(0, 40, 200); camera.Yaw = 0; diff --git a/Demos/Demos/Cars/CarCallbacks.cs b/Demos/Demos/Cars/CarCallbacks.cs index b2fe2997a..18ac6cd66 100644 --- a/Demos/Demos/Cars/CarCallbacks.cs +++ b/Demos/Demos/Cars/CarCallbacks.cs @@ -41,7 +41,7 @@ public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int chi } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold + public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold { pairMaterial.FrictionCoefficient = Properties[pair.A.BodyHandle].Friction; if (pair.B.Mobility != CollidableMobility.Static) @@ -56,7 +56,7 @@ public unsafe bool ConfigureContactManifold(int workerIndex, Collidab [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold) + public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold) { return true; } diff --git a/Demos/Demos/ChainFountainDemo.cs b/Demos/Demos/ChainFountainDemo.cs index c65fa8d9c..b17b2d973 100644 --- a/Demos/Demos/ChainFountainDemo.cs +++ b/Demos/Demos/ChainFountainDemo.cs @@ -17,7 +17,7 @@ namespace Demos.Demos /// public class ChainFountainDemo : Demo { - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(5.65f, 3, -23); camera.Yaw = MathF.PI; diff --git a/Demos/Demos/Characters/CharacterControllers.cs b/Demos/Demos/Characters/CharacterControllers.cs index bd1596fe6..05eab0f2b 100644 --- a/Demos/Demos/Characters/CharacterControllers.cs +++ b/Demos/Demos/Characters/CharacterControllers.cs @@ -233,7 +233,7 @@ struct ContactCollectionWorkerCache { public Buffer SupportCandidates; - public unsafe ContactCollectionWorkerCache(int maximumCharacterCount, BufferPool pool) + public ContactCollectionWorkerCache(int maximumCharacterCount, BufferPool pool) { pool.Take(maximumCharacterCount, out SupportCandidates); for (int i = 0; i < maximumCharacterCount; ++i) @@ -414,7 +414,8 @@ public bool TryReportContacts(in CollidablePair pair, ref TManifold m } Buffer<(int Start, int Count)> boundingBoxExpansionJobs; - unsafe void ExpandBoundingBoxes(int start, int count) + + void ExpandBoundingBoxes(int start, int count) { var end = start + count; for (int i = start; i < end; ++i) @@ -434,7 +435,8 @@ unsafe void ExpandBoundingBoxes(int start, int count) int boundingBoxExpansionJobIndex; Action expandBoundingBoxesWorker; - unsafe void ExpandBoundingBoxesWorker(int workerIndex) + + void ExpandBoundingBoxesWorker(int workerIndex) { while (true) { @@ -736,7 +738,8 @@ struct AnalyzeContactsJob int analysisJobCount; Buffer jobs; Action analyzeContactsWorker; - unsafe void AnalyzeContactsWorker(int workerIndex) + + void AnalyzeContactsWorker(int workerIndex) { int jobIndex; while ((jobIndex = Interlocked.Increment(ref analysisJobIndex)) < analysisJobCount) diff --git a/Demos/Demos/Characters/CharacterDemo.cs b/Demos/Demos/Characters/CharacterDemo.cs index aab54a873..b5729d38e 100644 --- a/Demos/Demos/Characters/CharacterDemo.cs +++ b/Demos/Demos/Characters/CharacterDemo.cs @@ -18,7 +18,7 @@ namespace Demos.Demos.Characters public class CharacterDemo : Demo { CharacterControllers characters; - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(20, 10, 20); camera.Yaw = MathF.PI; diff --git a/Demos/Demos/Characters/CharacterNarrowphaseCallbacks.cs b/Demos/Demos/Characters/CharacterNarrowphaseCallbacks.cs index 6de26860c..7aa5a0ec0 100644 --- a/Demos/Demos/Characters/CharacterNarrowphaseCallbacks.cs +++ b/Demos/Demos/Characters/CharacterNarrowphaseCallbacks.cs @@ -31,7 +31,7 @@ public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int chi } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold + public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold { pairMaterial = new PairMaterialProperties { FrictionCoefficient = 1, MaximumRecoveryVelocity = 2, SpringSettings = new SpringSettings(30, 1) }; Characters.TryReportContacts(pair, ref manifold, workerIndex, ref pairMaterial); @@ -39,7 +39,7 @@ public unsafe bool ConfigureContactManifold(int workerIndex, Collidab } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold) + public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold) { return true; } diff --git a/Demos/Demos/ClothDemo.cs b/Demos/Demos/ClothDemo.cs index 08beed862..22143b298 100644 --- a/Demos/Demos/ClothDemo.cs +++ b/Demos/Demos/ClothDemo.cs @@ -93,14 +93,14 @@ public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int chi } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold + public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold { pairMaterial = Material; return true; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold) + public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold) { return true; } @@ -202,7 +202,7 @@ void CreateConstraintBetweenBodies(BodyHandle aHandle, BodyHandle bHandle) RolloverInfo rolloverInfo; - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(0, 25, 100); camera.Yaw = 0; diff --git a/Demos/Demos/CollisionQueryDemo.cs b/Demos/Demos/CollisionQueryDemo.cs index e58602538..4a05873e4 100644 --- a/Demos/Demos/CollisionQueryDemo.cs +++ b/Demos/Demos/CollisionQueryDemo.cs @@ -213,7 +213,7 @@ struct Query public RigidPose Pose; } - public unsafe override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) { //The collision batcher vectorizes over multiple tests at once, so for optimal performance, you'll want to feed it a bunch of tests. var collisionBatcher = new CollisionBatcher(BufferPool, Simulation.Shapes, Simulation.NarrowPhase.CollisionTaskRegistry, 0, new BatcherCallbacks()); diff --git a/Demos/Demos/CollisionTrackingDemo.cs b/Demos/Demos/CollisionTrackingDemo.cs index d32c3810f..b649d9f93 100644 --- a/Demos/Demos/CollisionTrackingDemo.cs +++ b/Demos/Demos/CollisionTrackingDemo.cs @@ -371,7 +371,7 @@ public void ReportContacts(CollidableReference collidableA, Collidabl /// Callbacks invoked by the simulation's narrow phase. /// In this demo, we'll collect all contact data associated with tracked objects for later processing. /// - public unsafe struct CollisionTrackingCallbacks : INarrowPhaseCallbacks + public struct CollisionTrackingCallbacks : INarrowPhaseCallbacks { CollisionTracker collisionTracker; public CollisionTrackingCallbacks(CollisionTracker collisionTracker) @@ -392,7 +392,7 @@ public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int chi } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold + public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold { pairMaterial.FrictionCoefficient = 1f; pairMaterial.MaximumRecoveryVelocity = 2f; diff --git a/Demos/Demos/ColosseumDemo.cs b/Demos/Demos/ColosseumDemo.cs index 8edb426a3..5c556b945 100644 --- a/Demos/Demos/ColosseumDemo.cs +++ b/Demos/Demos/ColosseumDemo.cs @@ -59,7 +59,7 @@ public static Vector3 CreateRing(Simulation simulation, Vector3 position, Box ri return position; } - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(-30, 40, -30); camera.Yaw = MathHelper.Pi * 3f / 4; diff --git a/Demos/Demos/CompoundDemo.cs b/Demos/Demos/CompoundDemo.cs index c6a3ea60d..e71bd02f1 100644 --- a/Demos/Demos/CompoundDemo.cs +++ b/Demos/Demos/CompoundDemo.cs @@ -13,7 +13,7 @@ namespace Demos.Demos { public class CompoundDemo : Demo { - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(-13f, 6, -13f); camera.Yaw = MathHelper.Pi * 3f / 4; diff --git a/Demos/Demos/ContactEventsDemo.cs b/Demos/Demos/ContactEventsDemo.cs index 88e925da7..ea15ec719 100644 --- a/Demos/Demos/ContactEventsDemo.cs +++ b/Demos/Demos/ContactEventsDemo.cs @@ -150,7 +150,7 @@ void OnPairEnded(CollidableReference eventSource, CollidablePair pair) /// /// Watches a set of bodies and statics for contact changes and reports events. /// - public unsafe class ContactEvents : IDisposable + public class ContactEvents : IDisposable { //To know what events to emit, we have to track the previous state of a collision. We don't need to keep around old positions/offets/normals/depths, so it's quite a bit lighter. [StructLayout(LayoutKind.Sequential)] @@ -598,7 +598,7 @@ public void Dispose() } //The narrow phase needs a way to tell our contact events system about changes to contacts, so they'll need to be a part of the INarrowPhaseCallbacks. - public unsafe struct ContactEventCallbacks : INarrowPhaseCallbacks + public struct ContactEventCallbacks : INarrowPhaseCallbacks { ContactEvents events; @@ -620,7 +620,7 @@ public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int chi } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold + public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold { pairMaterial.FrictionCoefficient = 1f; pairMaterial.MaximumRecoveryVelocity = 2f; diff --git a/Demos/Demos/CustomVoxelCollidableDemo.cs b/Demos/Demos/CustomVoxelCollidableDemo.cs index df8d0938b..a1d5a9835 100644 --- a/Demos/Demos/CustomVoxelCollidableDemo.cs +++ b/Demos/Demos/CustomVoxelCollidableDemo.cs @@ -109,7 +109,7 @@ unsafe struct HitLeafTester : IRayLeafTester where T : IShapeRayHitHandler public RayData OriginalRay; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void TestLeaf(int leafIndex, RayData* ray, float* maximumT) + public void TestLeaf(int leafIndex, RayData* ray, float* maximumT) { ref var voxelIndex = ref VoxelIndices[leafIndex]; //Note that you could make use of the voxel grid's regular structure to save some work dealing with orientations. @@ -131,7 +131,7 @@ public unsafe void TestLeaf(int leafIndex, RayData* ray, float* maximumT) /// Ray to test against the voxels. /// Maximum length of the ray in units of the ray direction length. /// Callback to execute for every hit. - public readonly unsafe void RayTest(in RigidPose pose, in RayData ray, ref float maximumT, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler + public readonly void RayTest(in RigidPose pose, in RayData ray, ref float maximumT, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler { HitLeafTester leafTester; leafTester.VoxelIndices = VoxelIndices; @@ -327,7 +327,7 @@ public ref NonconvexReduction CreateContinuation( } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void GetChildAData(ref CollisionBatcher collisionBatcher, ref NonconvexReduction continuation, in BoundsTestedPair pair, int childIndexA, + public void GetChildAData(ref CollisionBatcher collisionBatcher, ref NonconvexReduction continuation, in BoundsTestedPair pair, int childIndexA, out RigidPose childPoseA, out int childTypeA, out void* childShapeDataA) where TCallbacks : struct, ICollisionCallbacks { @@ -339,7 +339,7 @@ public unsafe void GetChildAData(ref CollisionBatcher co } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void ConfigureContinuationChild( + public void ConfigureContinuationChild( ref CollisionBatcher collisionBatcher, ref NonconvexReduction continuation, int continuationChildIndex, in BoundsTestedPair pair, int childIndexA, int childTypeA, int childIndexB, in RigidPose childPoseA, out RigidPose childPoseB, out int childTypeB, out void* childShapeDataB) where TCallbacks : struct, ICollisionCallbacks @@ -369,7 +369,7 @@ public class CustomVoxelCollidableDemo : Demo { Voxels voxels; StaticHandle handle; - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(-40, 40, -40); camera.Yaw = MathHelper.Pi * 3f / 4; diff --git a/Demos/Demos/Dancers/DancerDemo.cs b/Demos/Demos/Dancers/DancerDemo.cs index 4ab6397b8..10ea24d5b 100644 --- a/Demos/Demos/Dancers/DancerDemo.cs +++ b/Demos/Demos/Dancers/DancerDemo.cs @@ -150,7 +150,7 @@ static void TailorDress(Simulation simulation, CollidableProperty(16, 16, Simulation, collisionFilters, ThreadDispatcher, BufferPool, new SolveDescription(1, 4), TailorDress, new ClothCollisionFilter(0, 0, -1)); } - public unsafe override void Update(Window window, Camera camera, Input input, float dt) + public override void Update(Window window, Camera camera, Input input, float dt) { dancers.UpdateTargets(Simulation); base.Update(window, camera, input, dt); diff --git a/Demos/Demos/Dancers/PlumpDancerDemo.cs b/Demos/Demos/Dancers/PlumpDancerDemo.cs index 0499ad2ff..352328b6e 100644 --- a/Demos/Demos/Dancers/PlumpDancerDemo.cs +++ b/Demos/Demos/Dancers/PlumpDancerDemo.cs @@ -207,7 +207,7 @@ static void CreateFatSuit(Simulation simulation, CollidableProperty(8, 8, Simulation, collisionFilters, ThreadDispatcher, BufferPool, new SolveDescription(1, 1), CreateFatSuit, new DeformableCollisionFilter(0, 0, 0, -1)); } - public unsafe override void Update(Window window, Camera camera, Input input, float dt) + public override void Update(Window window, Camera camera, Input input, float dt) { dancers.UpdateTargets(Simulation); base.Update(window, camera, input, dt); diff --git a/Demos/Demos/FrictionDemo.cs b/Demos/Demos/FrictionDemo.cs index 65d7ceaed..e06c20f97 100644 --- a/Demos/Demos/FrictionDemo.cs +++ b/Demos/Demos/FrictionDemo.cs @@ -26,7 +26,7 @@ public struct SimpleMaterial public float FrictionCoefficient; public float MaximumRecoveryVelocity; } - public unsafe struct FrictionCallbacks : INarrowPhaseCallbacks + public struct FrictionCallbacks : INarrowPhaseCallbacks { /// /// Maps entries to their . @@ -59,7 +59,7 @@ public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int chi } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold + public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold { //For the purposes of this demo, we'll use multiplicative blending for the friction and choose spring properties according to which collidable has a higher maximum recovery velocity. var a = CollidableMaterials[pair.A]; @@ -81,7 +81,7 @@ public void Dispose() } } - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(0, 20, 200); camera.Yaw = 0; diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index 8d2df09d0..99fb098dc 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -573,14 +573,14 @@ public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int chi } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold + public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold { pairMaterial = Material; return true; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold) + public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold) { return true; } @@ -625,7 +625,7 @@ static void TryAddEdge(int a, int b, ref QuickSet edges, ref Buffer< } } - private static unsafe int CreateTetrahedralUniqueEdgesList(ref Buffer tetrahedraVertices, + private static int CreateTetrahedralUniqueEdgesList(ref Buffer tetrahedraVertices, ref Buffer vertexEdgeCounts, BufferPool pool, ref QuickSet cellEdges) { for (int i = 0; i < tetrahedraVertices.Length; ++i) @@ -643,7 +643,7 @@ private static unsafe int CreateTetrahedralUniqueEdgesList(ref Buffer cellVertexIndices, + private static int CreateHexahedralUniqueEdgesList(ref Buffer cellVertexIndices, ref Buffer vertexEdgeCounts, BufferPool pool, ref QuickSet cellEdges) { for (int i = 0; i < cellVertexIndices.Length; ++i) @@ -666,7 +666,7 @@ private static unsafe int CreateHexahedralUniqueEdgesList(ref Buffer filters, + internal static void CreateDeformable(Simulation simulation, Vector3 position, Quaternion orientation, float density, float cellSize, in SpringSettings weldSpringiness, in SpringSettings volumeSpringiness, int instanceId, CollidableProperty filters, ref Buffer vertices, ref CellSet vertexSpatialIndices, ref Buffer cellVertexIndices, ref Buffer tetrahedraVertexIndices) { var pool = simulation.BufferPool; @@ -717,7 +717,7 @@ internal unsafe static void CreateDeformable(Simulation simulation, Vector3 posi } - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(-5f, 5.5f, 5f); camera.Yaw = MathHelper.Pi / 4; diff --git a/Demos/Demos/PerBodyGravityDemo.cs b/Demos/Demos/PerBodyGravityDemo.cs index edfdb4d8a..6390f3846 100644 --- a/Demos/Demos/PerBodyGravityDemo.cs +++ b/Demos/Demos/PerBodyGravityDemo.cs @@ -83,7 +83,7 @@ public void IntegrateVelocity(Vector bodyIndices, Vector3Wide position, Qua } - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(0, 20, 80); camera.Yaw = 0; diff --git a/Demos/Demos/PlanetDemo.cs b/Demos/Demos/PlanetDemo.cs index 43a7ac9ef..983179ddf 100644 --- a/Demos/Demos/PlanetDemo.cs +++ b/Demos/Demos/PlanetDemo.cs @@ -48,7 +48,7 @@ public void IntegrateVelocity(Vector bodyIndices, Vector3Wide position, Qua } - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(110, -80, 12); camera.Yaw = 0; diff --git a/Demos/Demos/PyramidDemo.cs b/Demos/Demos/PyramidDemo.cs index bddeacdb1..bcbea3856 100644 --- a/Demos/Demos/PyramidDemo.cs +++ b/Demos/Demos/PyramidDemo.cs @@ -16,7 +16,7 @@ namespace Demos.Demos /// public class PyramidDemo : Demo { - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(-30, 8, -110); camera.Yaw = MathHelper.Pi * 3f / 4; diff --git a/Demos/Demos/RagdollDemo.cs b/Demos/Demos/RagdollDemo.cs index d2eb3c49f..480d9b47b 100644 --- a/Demos/Demos/RagdollDemo.cs +++ b/Demos/Demos/RagdollDemo.cs @@ -132,14 +132,14 @@ public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int chi } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold + public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold { pairMaterial = Material; return true; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold) + public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold) { return true; } @@ -541,7 +541,7 @@ public static RagdollHandles AddRagdoll(Vector3 position, Quaternion orientation return handles; } - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(-20, 10, -20); camera.Yaw = MathHelper.Pi * 3f / 4; diff --git a/Demos/Demos/RagdollTubeDemo.cs b/Demos/Demos/RagdollTubeDemo.cs index dab9b13ad..6579709ab 100644 --- a/Demos/Demos/RagdollTubeDemo.cs +++ b/Demos/Demos/RagdollTubeDemo.cs @@ -17,7 +17,7 @@ namespace Demos.Demos /// public class RagdollTubeDemo : Demo { - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(0, 9, -40); camera.Yaw = MathHelper.Pi; diff --git a/Demos/Demos/RayCastingDemo.cs b/Demos/Demos/RayCastingDemo.cs index e192c2812..0c3535666 100644 --- a/Demos/Demos/RayCastingDemo.cs +++ b/Demos/Demos/RayCastingDemo.cs @@ -22,7 +22,7 @@ namespace Demos { public class RayCastingDemo : Demo { - public unsafe struct NoCollisionCallbacks : INarrowPhaseCallbacks + public struct NoCollisionCallbacks : INarrowPhaseCallbacks { public void Initialize(Simulation simulation) { @@ -41,7 +41,7 @@ public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int chi } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold + public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold { pairMaterial = new PairMaterialProperties(); return false; @@ -57,7 +57,7 @@ public void Dispose() { } } - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(-20f, 13, -20f); camera.Yaw = MathHelper.Pi * 3f / 4; @@ -262,7 +262,7 @@ public IntersectionAlgorithm(string name, Func pool.Take(largestRayCount, out Results); } - unsafe void ExecuteWorker(int workerIndex) + void ExecuteWorker(int workerIndex) { var intersectionCount = worker(workerIndex, this); Interlocked.Add(ref IntersectionCount, intersectionCount); @@ -395,7 +395,7 @@ void CopyAndRotate(ref QuickList source) } } - public unsafe override void Update(Window window, Camera camera, Input input, float dt) + public override void Update(Window window, Camera camera, Input input, float dt) { base.Update(window, camera, input, dt); diff --git a/Demos/Demos/RopeStabilityDemo.cs b/Demos/Demos/RopeStabilityDemo.cs index 44946ba32..d759c90d6 100644 --- a/Demos/Demos/RopeStabilityDemo.cs +++ b/Demos/Demos/RopeStabilityDemo.cs @@ -74,7 +74,7 @@ public static BodyHandle AttachWreckingBall(Simulation simulation, BodyHandle[] RolloverInfo rolloverInfo; - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(0, 25, 80); camera.Yaw = 0; diff --git a/Demos/Demos/RopeTwistDemo.cs b/Demos/Demos/RopeTwistDemo.cs index d983d4e33..6ad6b3d0c 100644 --- a/Demos/Demos/RopeTwistDemo.cs +++ b/Demos/Demos/RopeTwistDemo.cs @@ -24,7 +24,7 @@ struct RopeFilter /// /// Narrow phase callbacks that include collision filters designed for ropes. Adjacent bodies in a rope do not collide with each other. /// - unsafe struct RopeNarrowPhaseCallbacks : INarrowPhaseCallbacks + struct RopeNarrowPhaseCallbacks : INarrowPhaseCallbacks { public CollidableProperty Filters; public PairMaterialProperties Material; @@ -60,7 +60,7 @@ public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int chi } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold + public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold { pairMaterial = Material; return true; @@ -82,7 +82,7 @@ public void Dispose() /// public class RopeTwistDemo : Demo { - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(0, 20, 20); camera.Yaw = 0; diff --git a/Demos/Demos/SimpleSelfContainedDemo.cs b/Demos/Demos/SimpleSelfContainedDemo.cs index 29a41d9aa..aa30ea747 100644 --- a/Demos/Demos/SimpleSelfContainedDemo.cs +++ b/Demos/Demos/SimpleSelfContainedDemo.cs @@ -20,7 +20,7 @@ public static class SimpleSelfContainedDemo //If you're wondering why the callbacks are interface implementing structs rather than classes or events, it's because //the compiler can specialize the implementation using the compile time type information. That avoids dispatch overhead associated //with delegates or virtual dispatch and allows inlining, which is valuable for extremely high frequency logic like contact callbacks. - unsafe struct NarrowPhaseCallbacks : INarrowPhaseCallbacks + struct NarrowPhaseCallbacks : INarrowPhaseCallbacks { /// /// Performs any required initialization logic after the Simulation instance has been constructed. @@ -85,7 +85,7 @@ public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int chi /// Material properties of the manifold. /// True if a constraint should be created for the manifold, false otherwise. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold + public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold { //The IContactManifold parameter includes functions for accessing contact data regardless of what the underlying type of the manifold is. //If you want to have direct access to the underlying type, you can use the manifold.Convex property and a cast like Unsafe.As(ref manifold). diff --git a/Demos/Demos/SubsteppingDemo.cs b/Demos/Demos/SubsteppingDemo.cs index 8d7ed7f90..518bcddc6 100644 --- a/Demos/Demos/SubsteppingDemo.cs +++ b/Demos/Demos/SubsteppingDemo.cs @@ -19,7 +19,7 @@ public class SubsteppingDemo : Demo { RolloverInfo rolloverInfo; - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(0, 25, 80); camera.Yaw = 0; diff --git a/Demos/Demos/SweepDemo.cs b/Demos/Demos/SweepDemo.cs index a821b692d..65ea13d05 100644 --- a/Demos/Demos/SweepDemo.cs +++ b/Demos/Demos/SweepDemo.cs @@ -27,7 +27,7 @@ public bool AllowTest(int childA, int childB) ConvexHull hull; - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(0, 10, 40); camera.Yaw = 0; @@ -132,7 +132,7 @@ void DrawShape(ref TShape shape, in RigidPose pose, Vector3 color, Shape } } - unsafe void DrawSweep(TShape shape, in RigidPose pose, in BodyVelocity velocity, int steps, + void DrawSweep(TShape shape, in RigidPose pose, in BodyVelocity velocity, int steps, float t, Renderer renderer, Vector3 color) where TShape : struct, IShape { @@ -154,7 +154,7 @@ unsafe void DrawSweep(TShape shape, in RigidPose pose, in BodyVelocity v } } - unsafe void DrawImpact(Renderer renderer, ref Vector3 hitLocation, ref Vector3 hitNormal) + void DrawImpact(Renderer renderer, ref Vector3 hitLocation, ref Vector3 hitNormal) { //The normal itself will tend to be obscured by the shapes, so instead draw two lines representing the plane. DemoRenderer.Constraints.ContactLines.BuildOrthonormalBasis(hitNormal, out var tangent1, out var tangent2); diff --git a/Demos/Demos/Tanks/TankCallbacks.cs b/Demos/Demos/Tanks/TankCallbacks.cs index df7bfe573..3d43c620b 100644 --- a/Demos/Demos/Tanks/TankCallbacks.cs +++ b/Demos/Demos/Tanks/TankCallbacks.cs @@ -113,7 +113,7 @@ void TryAddProjectileImpact(BodyHandle projectileHandle, CollidableReference imp } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold + public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold { //Different tank parts have different friction values. Wheels tend to stick more than the body of the tank. ref var propertiesA = ref Properties[pair.A.BodyHandle]; @@ -160,7 +160,7 @@ public unsafe bool ConfigureContactManifold(int workerIndex, Collidab } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold) + public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold) { return true; } diff --git a/Demos/SpecializedTests/BatchedCollisionTests.cs b/Demos/SpecializedTests/BatchedCollisionTests.cs index cb537d619..7d73f3ae8 100644 --- a/Demos/SpecializedTests/BatchedCollisionTests.cs +++ b/Demos/SpecializedTests/BatchedCollisionTests.cs @@ -18,7 +18,7 @@ unsafe struct TestCollisionCallbacks : ICollisionCallbacks { public int* Count; - public unsafe void OnPairCompleted(int pairId, ref TManifold manifold) where TManifold : unmanaged, IContactManifold + public void OnPairCompleted(int pairId, ref TManifold manifold) where TManifold : unmanaged, IContactManifold { if (manifold.Count > 0) { @@ -32,7 +32,7 @@ public unsafe void OnPairCompleted(int pairId, ref TManifold manifold } } - public unsafe void OnChildPairCompleted(int pairId, int childA, int childB, ref ConvexContactManifold manifold) + public void OnChildPairCompleted(int pairId, int childA, int childB, ref ConvexContactManifold manifold) { } @@ -96,7 +96,7 @@ static float TestPair(ref TAWide a, ref return distanceSum[0]; } - unsafe static void Test(in TA a, in TB b, + static void Test(in TA a, in TB b, ref Buffer posesA, ref Buffer posesB, int iterationCount) where TA : unmanaged, IShape where TB : unmanaged, IShape where TAWide : unmanaged, IShapeWide where TBWide : unmanaged, IShapeWide diff --git a/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs b/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs index 3bf636406..e8f7f6398 100644 --- a/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs +++ b/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs @@ -14,7 +14,7 @@ namespace Demos.SpecializedTests public class BroadPhaseStressTestDemo : Demo { - public unsafe struct NoNarrowphaseTestingCallbacks : INarrowPhaseCallbacks + public struct NoNarrowphaseTestingCallbacks : INarrowPhaseCallbacks { public NoNarrowphaseTestingCallbacks() @@ -35,7 +35,7 @@ public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int chi return false; } - public unsafe bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold + public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold { pairMaterial = default; return false; @@ -53,7 +53,7 @@ public void Dispose() Vector3[] startingLocations; - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(-20f, 13, -20f); camera.Yaw = MathHelper.Pi * 3f / 4; diff --git a/Demos/SpecializedTests/CapsuleTestDemo.cs b/Demos/SpecializedTests/CapsuleTestDemo.cs index f3e262262..12a5fae70 100644 --- a/Demos/SpecializedTests/CapsuleTestDemo.cs +++ b/Demos/SpecializedTests/CapsuleTestDemo.cs @@ -12,7 +12,7 @@ namespace Demos.SpecializedTests { public class CapsuleTestDemo : Demo { - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(-10, 5, -10); //camera.Yaw = MathHelper.Pi ; diff --git a/Demos/SpecializedTests/CharacterTestDemo.cs b/Demos/SpecializedTests/CharacterTestDemo.cs index 69ff2fc67..0773906de 100644 --- a/Demos/SpecializedTests/CharacterTestDemo.cs +++ b/Demos/SpecializedTests/CharacterTestDemo.cs @@ -14,7 +14,7 @@ namespace Demos.SpecializedTests public class CharacterTestDemo : Demo { CharacterControllers characters; - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(20, 10, 20); camera.Yaw = MathHelper.Pi * -1f / 4; diff --git a/Demos/SpecializedTests/ClothLatticeDemo.cs b/Demos/SpecializedTests/ClothLatticeDemo.cs index 2a09e9a43..e1dc4c89c 100644 --- a/Demos/SpecializedTests/ClothLatticeDemo.cs +++ b/Demos/SpecializedTests/ClothLatticeDemo.cs @@ -10,7 +10,7 @@ namespace Demos.SpecializedTests { public class ClothLatticeDemo : Demo { - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(-120, 30, -120); camera.Yaw = MathHelper.Pi * 3f / 4; diff --git a/Demos/SpecializedTests/CompoundBoundTests.cs b/Demos/SpecializedTests/CompoundBoundTests.cs index 7c9f16676..bf77ce7ca 100644 --- a/Demos/SpecializedTests/CompoundBoundTests.cs +++ b/Demos/SpecializedTests/CompoundBoundTests.cs @@ -14,7 +14,7 @@ namespace Demos.Demos { public class CompoundBoundTests : Demo { - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(-10, 0, -10); camera.Yaw = MathHelper.Pi * 3f / 4; @@ -140,7 +140,7 @@ Vector3 GetRandomVector(float width, Random random) return new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) * width - new Vector3(width * 0.5f); } Random random = new Random(5); - public unsafe override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) { Vector3 basePosition = new Vector3(); for (int testIndex = 0; testIndex < 16; ++testIndex) diff --git a/Demos/SpecializedTests/CompoundCollisionIndicesTest.cs b/Demos/SpecializedTests/CompoundCollisionIndicesTest.cs index fd86c781c..89fe84032 100644 --- a/Demos/SpecializedTests/CompoundCollisionIndicesTest.cs +++ b/Demos/SpecializedTests/CompoundCollisionIndicesTest.cs @@ -11,7 +11,7 @@ namespace Demos.SpecializedTests { - public unsafe struct IndexReportingNarrowPhaseCallbacks : INarrowPhaseCallbacks + public struct IndexReportingNarrowPhaseCallbacks : INarrowPhaseCallbacks { [MethodImpl(MethodImplOptions.AggressiveInlining)] public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin) @@ -26,7 +26,7 @@ public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int chi } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold + public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold { if (manifold.Count > 0) { diff --git a/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs b/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs index 4451fad5b..ebb8b7334 100644 --- a/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs +++ b/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs @@ -9,7 +9,7 @@ namespace Demos.SpecializedTests { public class ConstrainedKinematicIntegrationTest : Demo { - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(25, 4, 40); camera.Yaw = 0; diff --git a/Demos/SpecializedTests/ConstraintTestDemo.cs b/Demos/SpecializedTests/ConstraintTestDemo.cs index d21571e49..45deed5fc 100644 --- a/Demos/SpecializedTests/ConstraintTestDemo.cs +++ b/Demos/SpecializedTests/ConstraintTestDemo.cs @@ -19,7 +19,7 @@ static float GetNextPosition(ref float x) return toReturn; } - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(25, 4, 40); camera.Yaw = 0; diff --git a/Demos/SpecializedTests/ConvexHullTestDemo.cs b/Demos/SpecializedTests/ConvexHullTestDemo.cs index e4f17746f..f72b95c7e 100644 --- a/Demos/SpecializedTests/ConvexHullTestDemo.cs +++ b/Demos/SpecializedTests/ConvexHullTestDemo.cs @@ -18,7 +18,7 @@ namespace Demos.SpecializedTests; public class ConvexHullTestDemo : Demo { - unsafe Buffer CreateRandomConvexHullPoints() + Buffer CreateRandomConvexHullPoints() { const int pointCount = 50; BufferPool.Take(pointCount, out var points); @@ -32,7 +32,7 @@ unsafe Buffer CreateRandomConvexHullPoints() return points; } - unsafe Buffer CreateMeshConvexHull(MeshContent meshContent, Vector3 scale) + Buffer CreateMeshConvexHull(MeshContent meshContent, Vector3 scale) { //This is actually a pretty good example of how *not* to make a convex hull shape. //Generating it directly from a graphical data source tends to have way more surface complexity than needed, @@ -50,7 +50,7 @@ unsafe Buffer CreateMeshConvexHull(MeshContent meshContent, Vector3 sca return points; } - unsafe Buffer CreateBoxConvexHull(float boxScale) + Buffer CreateBoxConvexHull(float boxScale) { BufferPool.Take(8, out var points); points[0] = new Vector3(0, 0, 0); @@ -65,7 +65,7 @@ unsafe Buffer CreateBoxConvexHull(float boxScale) } //A couple of test point sets from PEEL: https://github.com/Pierre-Terdiman/PEEL_PhysX_Edition - unsafe Buffer CreateTestConvexHull() + Buffer CreateTestConvexHull() { BufferPool.Take(50, out var vertices); vertices[0] = new Vector3(-0.000000f, -0.297120f, -0.000000f); @@ -121,7 +121,7 @@ unsafe Buffer CreateTestConvexHull() return vertices; } - unsafe Buffer CreateTestConvexHull2() + Buffer CreateTestConvexHull2() { BufferPool.Take(120, out var vertices); vertices[0] = new Vector3(0.153478f, 0.993671f, 0.124687f); @@ -248,7 +248,7 @@ unsafe Buffer CreateTestConvexHull2() } - unsafe Buffer CreateTestConvexHull3() + Buffer CreateTestConvexHull3() { BufferPool.Take(22, out var vertices); vertices[0] = new Vector3(-0.103558f, 1.000000f, -0.490575f); @@ -276,7 +276,7 @@ unsafe Buffer CreateTestConvexHull3() return vertices; } - unsafe Buffer CreateJSONSourcedConvexHull(string filePath) + Buffer CreateJSONSourcedConvexHull(string filePath) { //ChatGPT wrote this, of course. List points = new List(); @@ -306,7 +306,7 @@ unsafe Buffer CreateJSONSourcedConvexHull(string filePath) return buffer; } - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(0, -2.5f, 10); camera.Yaw = 0; diff --git a/Demos/SpecializedTests/FountainStressTestDemo.cs b/Demos/SpecializedTests/FountainStressTestDemo.cs index 92b4f2beb..942d7b47f 100644 --- a/Demos/SpecializedTests/FountainStressTestDemo.cs +++ b/Demos/SpecializedTests/FountainStressTestDemo.cs @@ -18,7 +18,7 @@ public class FountainStressTestDemo : Demo QuickQueue removedStatics; QuickQueue dynamicHandles; Random random; - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(-15f, 20, -15f); camera.Yaw = MathHelper.Pi * 3f / 4; diff --git a/Demos/SpecializedTests/Media/2.0/BedsheetDemo.cs b/Demos/SpecializedTests/Media/2.0/BedsheetDemo.cs index ef88ecc9c..fc07f1625 100644 --- a/Demos/SpecializedTests/Media/2.0/BedsheetDemo.cs +++ b/Demos/SpecializedTests/Media/2.0/BedsheetDemo.cs @@ -103,7 +103,7 @@ void CreateConstraintBetweenBodies(BodyHandle aHandle, BodyHandle bHandle) RolloverInfo rolloverInfo; - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(70, 40, -80); camera.Yaw = -MathF.PI * 0.8f; diff --git a/Demos/SpecializedTests/Media/2.0/ColosseumVideoDemo.cs b/Demos/SpecializedTests/Media/2.0/ColosseumVideoDemo.cs index 04fd3fd6a..f31f5db46 100644 --- a/Demos/SpecializedTests/Media/2.0/ColosseumVideoDemo.cs +++ b/Demos/SpecializedTests/Media/2.0/ColosseumVideoDemo.cs @@ -61,7 +61,7 @@ Vector3 CreateRing(Vector3 position, Box ringBoxShape, BodyDescription bodyDescr } CharacterControllers characters; - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(-30, 40, -30); camera.Yaw = MathHelper.Pi * 3f / 4; diff --git a/Demos/SpecializedTests/Media/2.0/NewtDemandingSacrificeVideoDemo.cs b/Demos/SpecializedTests/Media/2.0/NewtDemandingSacrificeVideoDemo.cs index 9a0296c07..84461b6c7 100644 --- a/Demos/SpecializedTests/Media/2.0/NewtDemandingSacrificeVideoDemo.cs +++ b/Demos/SpecializedTests/Media/2.0/NewtDemandingSacrificeVideoDemo.cs @@ -13,7 +13,7 @@ namespace Demos.SpecializedTests.Media public class NewtDemandingSacrificeVideoDemo : Demo { CollidableProperty filters; - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(-32f, 20.5f, 61f); camera.Yaw = MathHelper.Pi * 0.3f; diff --git a/Demos/SpecializedTests/Media/2.0/NewtVideoDemo.cs b/Demos/SpecializedTests/Media/2.0/NewtVideoDemo.cs index 5368d440e..720a1d27d 100644 --- a/Demos/SpecializedTests/Media/2.0/NewtVideoDemo.cs +++ b/Demos/SpecializedTests/Media/2.0/NewtVideoDemo.cs @@ -13,7 +13,7 @@ namespace Demos.SpecializedTests.Media { public class NewtVideoDemo : Demo { - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(-5f, 5.5f, 5f); camera.Yaw = MathHelper.Pi / 4; diff --git a/Demos/SpecializedTests/Media/2.0/PyramidVideoDemo.cs b/Demos/SpecializedTests/Media/2.0/PyramidVideoDemo.cs index d9228ad2a..73c2a9831 100644 --- a/Demos/SpecializedTests/Media/2.0/PyramidVideoDemo.cs +++ b/Demos/SpecializedTests/Media/2.0/PyramidVideoDemo.cs @@ -16,7 +16,7 @@ namespace Demos.Demos.Media /// public class PyramidVideoDemo : Demo { - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(-70, 8, 318); camera.Yaw = MathHelper.Pi * 1f / 4; diff --git a/Demos/SpecializedTests/Media/2.0/ShrinkwrappedNewtsVideoDemo.cs b/Demos/SpecializedTests/Media/2.0/ShrinkwrappedNewtsVideoDemo.cs index 3b624e3bd..a3c23ddbb 100644 --- a/Demos/SpecializedTests/Media/2.0/ShrinkwrappedNewtsVideoDemo.cs +++ b/Demos/SpecializedTests/Media/2.0/ShrinkwrappedNewtsVideoDemo.cs @@ -13,7 +13,7 @@ namespace Demos.SpecializedTests.Media { public class ShrinkwrappedNewtsVideoDemo : Demo { - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(25f, 1.5f, 15f); camera.Yaw = 3 * MathHelper.Pi / 4; diff --git a/Demos/SpecializedTests/Media/2.4/Colosseum24VideoDemo.cs b/Demos/SpecializedTests/Media/2.4/Colosseum24VideoDemo.cs index ca80cae3d..06a0d4770 100644 --- a/Demos/SpecializedTests/Media/2.4/Colosseum24VideoDemo.cs +++ b/Demos/SpecializedTests/Media/2.4/Colosseum24VideoDemo.cs @@ -58,7 +58,7 @@ public static Vector3 CreateRing(Simulation simulation, Vector3 position, Box ri return position; } - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(-30, 40, -30); camera.Yaw = MathHelper.Pi * 3f / 4; diff --git a/Demos/SpecializedTests/Media/2.4/ExcessivePyramidVideoDemo.cs b/Demos/SpecializedTests/Media/2.4/ExcessivePyramidVideoDemo.cs index bdd277167..2b61c28b1 100644 --- a/Demos/SpecializedTests/Media/2.4/ExcessivePyramidVideoDemo.cs +++ b/Demos/SpecializedTests/Media/2.4/ExcessivePyramidVideoDemo.cs @@ -15,7 +15,7 @@ namespace Demos.SpecializedTests.Media /// public class ExcessivePyramidVideoDemo : Demo { - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(-120, 32, 1045); camera.Yaw = MathHelper.Pi * 1f / 4; diff --git a/Demos/SpecializedTests/Media/2.4/RagdollTubeVideoDemo.cs b/Demos/SpecializedTests/Media/2.4/RagdollTubeVideoDemo.cs index d895d40e7..f647ef16f 100644 --- a/Demos/SpecializedTests/Media/2.4/RagdollTubeVideoDemo.cs +++ b/Demos/SpecializedTests/Media/2.4/RagdollTubeVideoDemo.cs @@ -18,7 +18,7 @@ namespace Demos.SpecializedTests.Media /// public class RagdollTubeVideoDemo : Demo { - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(0, 9, -40); camera.Yaw = MathHelper.Pi; diff --git a/Demos/SpecializedTests/Media/2.4/RopeTwistVideoDemo.cs b/Demos/SpecializedTests/Media/2.4/RopeTwistVideoDemo.cs index c1e03e6b5..fbc788785 100644 --- a/Demos/SpecializedTests/Media/2.4/RopeTwistVideoDemo.cs +++ b/Demos/SpecializedTests/Media/2.4/RopeTwistVideoDemo.cs @@ -15,7 +15,7 @@ namespace Demos.SpecializedTests.Media; /// public class RopeTwistVideoDemo : Demo { - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(0, 20, 20); camera.Yaw = 0; diff --git a/Demos/SpecializedTests/Media/2.4/VideoDancerDemo.cs b/Demos/SpecializedTests/Media/2.4/VideoDancerDemo.cs index c610e5e22..c5f45cdec 100644 --- a/Demos/SpecializedTests/Media/2.4/VideoDancerDemo.cs +++ b/Demos/SpecializedTests/Media/2.4/VideoDancerDemo.cs @@ -152,7 +152,7 @@ static void TailorDress(Simulation simulation, CollidableProperty(40, 40, Simulation, collisionFilters, ThreadDispatcher, BufferPool, new SolveDescription(1, 4), TailorDress, new ClothCollisionFilter(0, 0, -1)); } - public unsafe override void Update(Window window, Camera camera, Input input, float dt) + public override void Update(Window window, Camera camera, Input input, float dt) { dancers.UpdateTargets(Simulation); base.Update(window, camera, input, dt); diff --git a/Demos/SpecializedTests/Media/2.4/VideoPlumpDancerDemo.cs b/Demos/SpecializedTests/Media/2.4/VideoPlumpDancerDemo.cs index 29e75ec68..9927aea55 100644 --- a/Demos/SpecializedTests/Media/2.4/VideoPlumpDancerDemo.cs +++ b/Demos/SpecializedTests/Media/2.4/VideoPlumpDancerDemo.cs @@ -209,7 +209,7 @@ static void CreateFatSuit(Simulation simulation, CollidableProperty(32, 32, Simulation, collisionFilters, ThreadDispatcher, BufferPool, new SolveDescription(1, 1), CreateFatSuit, new DeformableCollisionFilter(0, 0, 0, -1)); } - public unsafe override void Update(Window window, Camera camera, Input input, float dt) + public override void Update(Window window, Camera camera, Input input, float dt) { dancers.UpdateTargets(Simulation); base.Update(window, camera, input, dt); diff --git a/Demos/SpecializedTests/MeshTestDemo.cs b/Demos/SpecializedTests/MeshTestDemo.cs index 236ab1680..ba015da31 100644 --- a/Demos/SpecializedTests/MeshTestDemo.cs +++ b/Demos/SpecializedTests/MeshTestDemo.cs @@ -11,7 +11,7 @@ namespace Demos.SpecializedTests { public class MeshTestDemo : Demo { - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(-10, 0, -10); //camera.Yaw = MathHelper.Pi ; diff --git a/Demos/SpecializedTests/NewtonsCradleDemo.cs b/Demos/SpecializedTests/NewtonsCradleDemo.cs index 52c688c92..e1d2f61a6 100644 --- a/Demos/SpecializedTests/NewtonsCradleDemo.cs +++ b/Demos/SpecializedTests/NewtonsCradleDemo.cs @@ -16,7 +16,7 @@ namespace Demos.SpecializedTests /// public class NewtonsCradleDemo : Demo { - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { camera.Yaw = 0; camera.Pitch = 0; diff --git a/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs b/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs index e5e9bd426..d7f5a0c7e 100644 --- a/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs +++ b/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs @@ -15,7 +15,7 @@ namespace Demos.Demos /// public class PyramidAwakenerTestDemo : Demo { - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(-30, 8, -110); camera.Yaw = MathHelper.Pi * 3f / 4; diff --git a/Demos/SpecializedTests/ScalarIntegrationTestDemo.cs b/Demos/SpecializedTests/ScalarIntegrationTestDemo.cs index 6d975ee85..0ed512a7a 100644 --- a/Demos/SpecializedTests/ScalarIntegrationTestDemo.cs +++ b/Demos/SpecializedTests/ScalarIntegrationTestDemo.cs @@ -65,7 +65,7 @@ static void IntegrateVelocity(int bodyIndex, Vector3 position, Quaternion orient velocity->Linear += new Vector3(0, -10 / 60f, 0); } - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(-30, 10, -30); //camera.Yaw = MathHelper.Pi ; diff --git a/Demos/SpecializedTests/ShapePileTestDemo.cs b/Demos/SpecializedTests/ShapePileTestDemo.cs index ba04b966f..5dc9db3fd 100644 --- a/Demos/SpecializedTests/ShapePileTestDemo.cs +++ b/Demos/SpecializedTests/ShapePileTestDemo.cs @@ -12,7 +12,7 @@ namespace Demos.SpecializedTests { public class ShapePileTestDemo : Demo { - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(-30, 10, -30); //camera.Yaw = MathHelper.Pi ; diff --git a/Demos/SpecializedTests/SolverBatchTestDemo.cs b/Demos/SpecializedTests/SolverBatchTestDemo.cs index b69942444..4a487e2b3 100644 --- a/Demos/SpecializedTests/SolverBatchTestDemo.cs +++ b/Demos/SpecializedTests/SolverBatchTestDemo.cs @@ -12,7 +12,7 @@ namespace Demos.Demos { public class SolverBatchTestDemo : Demo { - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(-120, 30, -120); camera.Yaw = MathHelper.Pi * 3f / 4; diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index b8799d11f..cd96e5822 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -14,7 +14,7 @@ namespace Demos.SpecializedTests; -public unsafe class TreeFiddlingTestDemo : Demo +public class TreeFiddlingTestDemo : Demo { struct Pair : IEquatable { diff --git a/Demos/SpecializedTests/TriangleRayTestDemo.cs b/Demos/SpecializedTests/TriangleRayTestDemo.cs index 3dc5079eb..08bba4248 100644 --- a/Demos/SpecializedTests/TriangleRayTestDemo.cs +++ b/Demos/SpecializedTests/TriangleRayTestDemo.cs @@ -84,7 +84,7 @@ void TestRay(in Triangle triangle, in RigidPose pose, Vector3 rayOrigin, Vector3 } } - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(-30, 8, -60); camera.Yaw = MathHelper.Pi * 3f / 4; diff --git a/Demos/SpecializedTests/VolumeQueryTests.cs b/Demos/SpecializedTests/VolumeQueryTests.cs index bc7ccfe91..f5b2163cc 100644 --- a/Demos/SpecializedTests/VolumeQueryTests.cs +++ b/Demos/SpecializedTests/VolumeQueryTests.cs @@ -18,7 +18,7 @@ namespace Demos.SpecializedTests { public class VolumeQueryTests : Demo { - public unsafe struct NoCollisionCallbacks : INarrowPhaseCallbacks + public struct NoCollisionCallbacks : INarrowPhaseCallbacks { public void Initialize(Simulation simulation) { @@ -37,7 +37,7 @@ public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int chi } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold + public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold { pairMaterial = new PairMaterialProperties(); return false; @@ -53,7 +53,7 @@ public void Dispose() { } } - public unsafe override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(-20f, 13, -20f); camera.Yaw = MathHelper.Pi * 3f / 4; @@ -145,7 +145,7 @@ public BoxQueryAlgorithm(string name, BufferPool pool, Func Date: Mon, 28 Aug 2023 01:38:56 +0200 Subject: [PATCH 808/947] Fix almost all warnings and suppress (almost) all of the rest Mostly doc stuff, the only remaining warnings relate to a giant swath of seemingly unusued `internal` code, which I did not feel comfortable removing. --- BepuPhysics/BepuPhysics.csproj | 5 +++++ BepuPhysics/CollidableProperty.cs | 4 ++-- .../CollisionDetection/CollisionTasks/PairTypes.cs | 2 -- BepuUtilities/BepuUtilities.csproj | 4 ++++ BepuUtilities/Collections/LSBRadixSort.cs | 2 -- BepuUtilities/Collections/QuickDictionary.cs | 11 ----------- BepuUtilities/Collections/QuickSet.cs | 1 - BepuUtilities/Matrix2x2Wide.cs | 4 ++-- BepuUtilities/Memory/WorkerBufferPools.cs | 2 -- BepuUtilities/Symmetric3x3Wide.cs | 3 +-- BepuUtilities/Vector3Wide.cs | 4 ++-- Demos/SpecializedTests/TreeFiddlingTestDemo.cs | 1 - 12 files changed, 16 insertions(+), 27 deletions(-) diff --git a/BepuPhysics/BepuPhysics.csproj b/BepuPhysics/BepuPhysics.csproj index 8416cf26f..1680cef59 100644 --- a/BepuPhysics/BepuPhysics.csproj +++ b/BepuPhysics/BepuPhysics.csproj @@ -23,6 +23,11 @@ true snupkg + + + + + 1573;1591;CA2014 diff --git a/BepuPhysics/CollidableProperty.cs b/BepuPhysics/CollidableProperty.cs index ddbf35ef9..62dd41823 100644 --- a/BepuPhysics/CollidableProperty.cs +++ b/BepuPhysics/CollidableProperty.cs @@ -91,7 +91,7 @@ public ref T this[StaticHandle staticHandle] /// /// Gets a reference to the properties associated with a collidable. /// - /// Collidable to retrieve the properties for. + /// Collidable to retrieve the properties for. /// Reference to properties associated with a collidable. public ref T this[CollidableReference collidable] { @@ -143,7 +143,7 @@ public ref T Allocate(StaticHandle handle) /// /// Ensures there is space for a given collidable reference and returns a reference to the used memory. /// - /// Collidable reference to allocate for. + /// Collidable reference to allocate for. /// Reference to the data for the given collidable. [MethodImpl(MethodImplOptions.AggressiveInlining)] public ref T Allocate(CollidableReference collidableReference) diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/PairTypes.cs b/BepuPhysics/CollisionDetection/CollisionTasks/PairTypes.cs index 71db877a4..bebfbc9a7 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/PairTypes.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/PairTypes.cs @@ -108,8 +108,6 @@ public static ref PairContinuation GetContinuation(ref SphereIncludingPair pair) /// /// Pair of objects awaiting collision processing that involves velocities for bounds calculation. /// - /// Type of the first shape in the pair. - /// Type of the second shape in the pair. public unsafe struct BoundsTestedPair : ICollisionPair { public void* A; diff --git a/BepuUtilities/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj index 13fefd2b2..3478613d6 100644 --- a/BepuUtilities/BepuUtilities.csproj +++ b/BepuUtilities/BepuUtilities.csproj @@ -24,6 +24,10 @@ true snupkg + + + + 1573;1591 diff --git a/BepuUtilities/Collections/LSBRadixSort.cs b/BepuUtilities/Collections/LSBRadixSort.cs index 885000430..83257534b 100644 --- a/BepuUtilities/Collections/LSBRadixSort.cs +++ b/BepuUtilities/Collections/LSBRadixSort.cs @@ -164,8 +164,6 @@ public static void SortU8(ref int inputKeys, ref T inputValues, ref int outpu /// /// Only one invocation of the sort can be running at a time on a given instance of the sorter. /// Type of the values to sort. - /// Type of the span that holds the keys to sort. - /// Type of the span that holds the values to sort. /// Span containing the keys to sort. /// Span containing the values to sort. /// Scratch array to write temporary results into. diff --git a/BepuUtilities/Collections/QuickDictionary.cs b/BepuUtilities/Collections/QuickDictionary.cs index 2d27d22a4..f1f258c13 100644 --- a/BepuUtilities/Collections/QuickDictionary.cs +++ b/BepuUtilities/Collections/QuickDictionary.cs @@ -156,7 +156,6 @@ public QuickDictionary(ref Buffer initialKeySpan, ref Buffer initi /// Span to use as backing memory of the dictionary keys. /// Span to use as backing memory of the dictionary values. /// Span to use as backing memory of the table. Must be zeroed. - /// Comparer to use for the dictionary. /// Target size of the table relative to the number of stored elements. [MethodImpl(MethodImplOptions.AggressiveInlining)] public QuickDictionary(ref Buffer initialKeySpan, ref Buffer initialValueSpan, ref Buffer initialTableSpan, int tablePowerOffset = 2) @@ -187,7 +186,6 @@ public QuickDictionary(int initialCapacity, int tableSizePower, IUnmanagedMemory /// /// Initial target size of the key and value spans. The size of the initial buffer will be at least as large as the initialCapacity. /// Target capacity relative to the initial capacity in terms of a power of 2. The size of the initial table buffer will be at least 2^tableSizePower times larger than the initial capacity. - /// Comparer to use in the dictionary. /// Pool used for spans. [MethodImpl(MethodImplOptions.AggressiveInlining)] public QuickDictionary(int initialCapacity, int tableSizePower, IUnmanagedMemoryPool pool) @@ -199,7 +197,6 @@ public QuickDictionary(int initialCapacity, int tableSizePower, IUnmanagedMemory /// Creates a new dictionary with a default constructed comparer. /// /// Initial target size of the key and value spans. The size of the initial buffer will be at least as large as the initialCapacity. - /// Comparer to use in the dictionary. /// Pool used for spans. [MethodImpl(MethodImplOptions.AggressiveInlining)] public QuickDictionary(int initialCapacity, IUnmanagedMemoryPool pool) @@ -273,12 +270,6 @@ public void Resize(int newSize, IUnmanagedMemoryPool pool) /// /// Returns the resources associated with the dictionary to pools. /// - /// Pool used for key spans. - /// Pool used for value spans. - /// Pool used for table spans. - /// Type of the pool used for key spans. - /// Type of the pool used for value spans. - /// Type of the pool used for table spans. [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Dispose(IUnmanagedMemoryPool pool) { @@ -627,7 +618,6 @@ public bool AddAndReplace(TKey key, in TValue value, IUnmanagedMemoryPool pool) /// Key of the pair to add. /// Value of the pair to add. /// Pool used for spans. - /// Type of the pool used for spans. /// True if the pair was added to the dictionary, false if the key was already present. [MethodImpl(MethodImplOptions.AggressiveInlining)] public bool Add(ref TKey key, in TValue value, IUnmanagedMemoryPool pool) @@ -652,7 +642,6 @@ public bool Add(ref TKey key, in TValue value, IUnmanagedMemoryPool pool) /// Key of the pair to add. /// Value of the pair to add. /// Pool to pull resources from and to return resources to. - /// Type of the pool to use. /// True if the pair was added to the dictionary, false if the key was already present. [MethodImpl(MethodImplOptions.AggressiveInlining)] public bool Add(TKey key, in TValue value, IUnmanagedMemoryPool pool) diff --git a/BepuUtilities/Collections/QuickSet.cs b/BepuUtilities/Collections/QuickSet.cs index d757b2201..65f435ec0 100644 --- a/BepuUtilities/Collections/QuickSet.cs +++ b/BepuUtilities/Collections/QuickSet.cs @@ -232,7 +232,6 @@ public void EnsureCapacity(int count, IUnmanagedMemoryPool pool) /// /// Shrinks the internal buffers to the smallest acceptable size and releases the old buffers to the pools. /// - /// Element to add. /// Pool used for spans. public void Compact(IUnmanagedMemoryPool pool) { diff --git a/BepuUtilities/Matrix2x2Wide.cs b/BepuUtilities/Matrix2x2Wide.cs index 07b3837af..6b122b689 100644 --- a/BepuUtilities/Matrix2x2Wide.cs +++ b/BepuUtilities/Matrix2x2Wide.cs @@ -93,8 +93,8 @@ public static void Subtract(in Matrix2x2Wide a, in Matrix2x2Wide b, out Matrix2x /// /// Inverts the given matix. /// - /// Matrix to be inverted. - /// Inverted matrix. + /// Matrix to be inverted. + /// Inverted matrix. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void InvertWithoutOverlap(in Matrix2x2Wide m, out Matrix2x2Wide inverse) { diff --git a/BepuUtilities/Memory/WorkerBufferPools.cs b/BepuUtilities/Memory/WorkerBufferPools.cs index 5beda8a7d..47e26622e 100644 --- a/BepuUtilities/Memory/WorkerBufferPools.cs +++ b/BepuUtilities/Memory/WorkerBufferPools.cs @@ -9,8 +9,6 @@ public class WorkerBufferPools : IDisposable { BufferPool[] pools; - /// - /// /// Gets the pool associated with this worker. /// diff --git a/BepuUtilities/Symmetric3x3Wide.cs b/BepuUtilities/Symmetric3x3Wide.cs index ec9f1908c..05ff1c7a5 100644 --- a/BepuUtilities/Symmetric3x3Wide.cs +++ b/BepuUtilities/Symmetric3x3Wide.cs @@ -360,7 +360,6 @@ public static void Multiply(in Symmetric3x3Wide a, in Matrix3x3Wide b, out Matri /// /// Matrix to be reinterpreted as symmetric for the multiply. /// Second matrix of the pair to multiply. - /// Result of multiplying a * b. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Matrix3x3Wide operator *(in Symmetric3x3Wide a, in Matrix3x3Wide b) //TODO: without in decoration, this had some really peculiar codegen in .net 6 preview 5. { @@ -425,7 +424,7 @@ public static void MultiplyByTransposed(in Symmetric3x3Wide a, in Matrix2x3Wide /// /// Matrix to use as the sandwich bread. /// Succulent interior symmetric matrix. - /// Result of m * t * mT for a symmetric matrix t. + /// Result of m * t * mT for a symmetric matrix t. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void MatrixSandwich(in Matrix2x3Wide m, in Symmetric3x3Wide t, out Symmetric2x2Wide result) { diff --git a/BepuUtilities/Vector3Wide.cs b/BepuUtilities/Vector3Wide.cs index 5ae5b740c..44f90efe9 100644 --- a/BepuUtilities/Vector3Wide.cs +++ b/BepuUtilities/Vector3Wide.cs @@ -178,8 +178,8 @@ public static void Subtract(in Vector s, in Vector3Wide v, out Vector3Wid /// /// Finds the result of subtracting the components of a vector from a scalar. /// - /// Vector to subtract from the scalar. - /// Scalar to subtract from. + /// Vector to subtract from the scalar. + /// Scalar to subtract from. /// Vector with components equal the input vector subtracted from the input scalar. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Vector3Wide operator -(Vector3Wide a, Vector b) diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index cd96e5822..d2a7858d4 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -167,7 +167,6 @@ public override void Initialize(ContentArchive content, Camera camera) int refinementState = 0; long sum = 0; - int cacheOptimizationStart = 0; var taskStack = new TaskStack(BufferPool, ThreadDispatcher, ThreadDispatcher.ThreadCount); for (int refinementIndex = 0; refinementIndex < 16384; ++refinementIndex) { From e9b878df999ff3430fbafad52d2e42d09ddec669 Mon Sep 17 00:00:00 2001 From: Salvage <29021710+Saalvage@users.noreply.github.com> Date: Mon, 28 Aug 2023 01:44:28 +0200 Subject: [PATCH 809/947] Make accidentally `internal` (?) constraint `public`, eliminating the final warnings! If this is not intended to be `public`, it can just be deleted because it's not used `internal`ly. --- BepuPhysics/Constraints/Inequality1DOF.cs | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/BepuPhysics/Constraints/Inequality1DOF.cs b/BepuPhysics/Constraints/Inequality1DOF.cs index e8475acca..2516d1487 100644 --- a/BepuPhysics/Constraints/Inequality1DOF.cs +++ b/BepuPhysics/Constraints/Inequality1DOF.cs @@ -9,7 +9,7 @@ namespace BepuPhysics.Constraints //There's a lot less room for tricky premultiplication to save memory bandwidth, simply because those quantities are all in registers/L1 cache during a constraint solve in 2.4+. //Would be nice to update those parts. - internal struct TwoBody1DOFJacobians + public struct TwoBody1DOFJacobians { public Vector3Wide LinearA; public Vector3Wide AngularA; @@ -17,7 +17,7 @@ internal struct TwoBody1DOFJacobians public Vector3Wide AngularB; } - internal struct Projection2Body1DOF + public struct Projection2Body1DOF { //Rather than projecting from world space to constraint space *velocity* using JT, we precompute JT * effective mass //and go directly from world space velocity to constraint space impulse. @@ -43,9 +43,8 @@ internal struct Projection2Body1DOF public Vector3Wide CSIToWSVAngularB; } - internal static class Inequality2Body1DOF + public static class Inequality2Body1DOF { - [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Prestep(ref BodyInertiaWide inertiaA, ref BodyInertiaWide inertiaB, ref TwoBody1DOFJacobians jacobians, ref SpringSettingsWide springSettings, ref Vector maximumRecoveryVelocity, ref Vector positionError, float dt, float inverseDt, out Projection2Body1DOF projection) @@ -359,7 +358,6 @@ public static void ComputeCorrectiveImpulse(ref BodyVelocityWide wsvA, ref BodyV accumulatedImpulse = Vector.Max(Vector.Zero, accumulatedImpulse + csi); correctiveCSI = accumulatedImpulse - previousAccumulated; - } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -367,7 +365,6 @@ public static void Solve(ref Projection2Body1DOF projection, ref Vector a { ComputeCorrectiveImpulse(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulse, out var correctiveCSI); ApplyImpulse(ref projection, ref correctiveCSI, ref wsvA, ref wsvB); - } } From 62483bcc35ac1ac712d10d29a6c50bdec37e2343 Mon Sep 17 00:00:00 2001 From: Salvage <29021710+Saalvage@users.noreply.github.com> Date: Mon, 28 Aug 2023 01:48:51 +0200 Subject: [PATCH 810/947] Fix straggler warning --- DemoTests/InertiaTensorTests.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/DemoTests/InertiaTensorTests.cs b/DemoTests/InertiaTensorTests.cs index 0a4de6096..10eca90ae 100644 --- a/DemoTests/InertiaTensorTests.cs +++ b/DemoTests/InertiaTensorTests.cs @@ -250,7 +250,7 @@ public void OnRayHit(in RayData ray, ref float maximumT, float t, Vector3 normal } - public static void TestCompound(Random random, BufferPool pool) + private static void TestCompound(Random random, BufferPool pool) { var shapes = new Shapes(pool, 8); var treeCompoundBoxShape = new Box(0.5f, 1.5f, 1f); From c4f18dd09e793e917384766f4ab89f2921e02b62 Mon Sep 17 00:00:00 2001 From: Salvage <29021710+Saalvage@users.noreply.github.com> Date: Mon, 28 Aug 2023 01:54:25 +0200 Subject: [PATCH 811/947] Oops Forgot to test the release config.. Classic beginner mistake!! --- BepuPhysics/Simulation.cs | 1 + 1 file changed, 1 insertion(+) diff --git a/BepuPhysics/Simulation.cs b/BepuPhysics/Simulation.cs index 1fb22f5ab..0b5a065ec 100644 --- a/BepuPhysics/Simulation.cs +++ b/BepuPhysics/Simulation.cs @@ -4,6 +4,7 @@ using BepuPhysics.CollisionDetection; using System; using System.Diagnostics; +using System.Runtime.CompilerServices; using BepuPhysics.Trees; #if !DEBUG From 4170e683ccd0356013427324660fb65caddb7d55 Mon Sep 17 00:00:00 2001 From: Salvage <29021710+Saalvage@users.noreply.github.com> Date: Mon, 28 Aug 2023 02:49:48 +0200 Subject: [PATCH 812/947] Only try to publish on `push`, not `pull_request` --- .github/workflows/dotnet-core.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/dotnet-core.yml b/.github/workflows/dotnet-core.yml index 86cae4581..a2b1771f3 100644 --- a/.github/workflows/dotnet-core.yml +++ b/.github/workflows/dotnet-core.yml @@ -24,6 +24,7 @@ jobs: - name: Test run: dotnet test DemoTests -c Release --verbosity normal - name: Publish + if: github.event_name != 'pull_request' run: | dotnet nuget add source "https://nuget.pkg.github.com/bepu/index.json" --name "github" --username "rossnordby" --password "${{secrets.GITHUB_TOKEN}}" dotnet pack "BepuPhysics" -c Release From 2d7838a647d0bac3d2a65a497cf62f02bf3cfa75 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 28 Aug 2023 14:45:28 -0500 Subject: [PATCH 813/947] Scooted Inequality1DOF out of the library, since it's technically just documentation. Bit of an awkward location, still. --- BepuPhysics/Constraints/Inequality1DOF.cs | 371 --------------------- Demos/SpecializedTests/Inequality1DOF.cs | 372 ++++++++++++++++++++++ 2 files changed, 372 insertions(+), 371 deletions(-) delete mode 100644 BepuPhysics/Constraints/Inequality1DOF.cs create mode 100644 Demos/SpecializedTests/Inequality1DOF.cs diff --git a/BepuPhysics/Constraints/Inequality1DOF.cs b/BepuPhysics/Constraints/Inequality1DOF.cs deleted file mode 100644 index 2516d1487..000000000 --- a/BepuPhysics/Constraints/Inequality1DOF.cs +++ /dev/null @@ -1,371 +0,0 @@ -using BepuUtilities; -using System.Numerics; -using System.Runtime.CompilerServices; - -namespace BepuPhysics.Constraints -{ - //TODO: These are notes about the mathy bits underlying constraints. They were written for the original pre-2.4 version of the solver. - //Most of it's still applicable, but 2.4 and up no longer have separate 'projection' states, and while packing is still important, it applies to *prestep data*. - //There's a lot less room for tricky premultiplication to save memory bandwidth, simply because those quantities are all in registers/L1 cache during a constraint solve in 2.4+. - //Would be nice to update those parts. - - public struct TwoBody1DOFJacobians - { - public Vector3Wide LinearA; - public Vector3Wide AngularA; - public Vector3Wide LinearB; - public Vector3Wide AngularB; - } - - public struct Projection2Body1DOF - { - //Rather than projecting from world space to constraint space *velocity* using JT, we precompute JT * effective mass - //and go directly from world space velocity to constraint space impulse. - public Vector3Wide WSVtoCSILinearA; - public Vector3Wide WSVtoCSIAngularA; - public Vector3Wide WSVtoCSILinearB; - public Vector3Wide WSVtoCSIAngularB; - - //Since we jump directly from world space velocity to constraint space impulse, the velocity bias needs to be precomputed into an impulse offset too. - public Vector BiasImpulse; - //And once again, CFM becomes CFM * EffectiveMass- massively cancels out due to the derivation of CFM. (See prestep notes.) - public Vector SoftnessImpulseScale; - - //It also needs to project from constraint space to world space. - //We bundle this with the inertia/mass multiplier, so rather than taking a constraint impulse to world impulse and then to world velocity change, - //we just go directly from constraint impulse to world velocity change. - //For constraints with lower DOF counts, using this format also saves us some memory bandwidth- - //the inverse inertia tensor and inverse mass for a 2 body constraint cost 20 floats, compared to this implementation's 12. - //(Note that even in an implementation where we use the body inertias, we should still cache it constraint-locally to avoid big gathers.) - public Vector3Wide CSIToWSVLinearA; - public Vector3Wide CSIToWSVAngularA; - public Vector3Wide CSIToWSVLinearB; - public Vector3Wide CSIToWSVAngularB; - } - - public static class Inequality2Body1DOF - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Prestep(ref BodyInertiaWide inertiaA, ref BodyInertiaWide inertiaB, ref TwoBody1DOFJacobians jacobians, ref SpringSettingsWide springSettings, ref Vector maximumRecoveryVelocity, - ref Vector positionError, float dt, float inverseDt, out Projection2Body1DOF projection) - { - //unsoftened effective mass = (J * M^-1 * JT)^-1 - //where J is a constraintDOF x bodyCount*6 sized matrix, JT is its transpose, and for two bodies M^-1 is: - //[inverseMassA, 0, 0, 0] - //[0, inverseInertiaA, 0, 0] - //[0, 0, inverseMassB, 0] - //[0, 0, 0, inverseInertiaB] - //The entries of J match up to this convention, containing the linear and angular components of each body in sequence, so for a 2 body 1DOF constraint J would look like: - //[linearA 1x3, angularA 1x3, linearB 1x3, angularB 1x3] - //Note that it is a row vector by convention. When transforming velocities from world space into constraint space, it is assumed that the velocity vector is organized as a - //row vector matching up to the jacobian (that is, [linearA 1x3, angularA 1x3, linearB 1x3, angularB 1x3]), so for a 2 body 2 DOF constraint, - //worldVelocity * JT would be a [worldVelocity: 1x12] * [JT: 12x2], resulting in a 1x2 constraint space velocity row vector. - //Similarly, when going from constraint space impulse to world space impulse in the above example, we would do [csi: 1x2] * [J: 2x12] to get a 1x12 world impulse row vector. - - //Note that the engine uses row vectors for all velocities and positions and so on. Rotation and inertia tensors are constructed for premultiplication. - //In other words, unlike many of the presentations in the space, we use v * JT and csi * J instead of J * v and JT * csi. - //There is no meaningful difference- the two conventions are just transpositions of each other. - - //(If you want to know how this stuff works, go read the constraint related presentations: http://box2d.org/downloads/ - //Be mindful of the difference in conventions. You'll see J * v instead of v * JT, for example. Everything is still fundamentally the same, though.) - - //Due to the block structure of the mass matrix, we can handle each component separately and then sum the results. - //For this 1DOF constraint, the result is a simple scalar. - //Note that we store the intermediate results of J * M^-1 for use when projecting from constraint space impulses to world velocity changes. - //If we didn't store those intermediate values, we could just scale the dot product of jacobians.LinearA with itself to save 4 multiplies. - Vector3Wide.Scale(jacobians.LinearA, inertiaA.InverseMass, out projection.CSIToWSVLinearA); - Vector3Wide.Scale(jacobians.LinearB, inertiaB.InverseMass, out projection.CSIToWSVLinearB); - Vector3Wide.Dot(projection.CSIToWSVLinearA, jacobians.LinearA, out var linearA); - Vector3Wide.Dot(projection.CSIToWSVLinearB, jacobians.LinearB, out var linearB); - - //The angular components are a little more involved; (J * I^-1) * JT is explicitly computed. - Symmetric3x3Wide.TransformWithoutOverlap(jacobians.AngularA, inertiaA.InverseInertiaTensor, out projection.CSIToWSVAngularA); - Symmetric3x3Wide.TransformWithoutOverlap(jacobians.AngularB, inertiaB.InverseInertiaTensor, out projection.CSIToWSVAngularB); - Vector3Wide.Dot(projection.CSIToWSVAngularA, jacobians.AngularA, out var angularA); - Vector3Wide.Dot(projection.CSIToWSVAngularB, jacobians.AngularB, out var angularB); - - //Now for a digression! - //Softness is applied along the diagonal (which, for a 1DOF constraint, is just the only element). - //Check the the ODE reference for a bit more information: http://ode.org/ode-latest-userguide.html#sec_3_8_0 - //And also see Erin Catto's Soft Constraints presentation for more details: http://box2d.org/files/GDC2011/GDC2011_Catto_Erin_Soft_Constraints.pdf) - - //There are some very interesting tricks you can use here, though. - //Our core tuning variables are the damping ratio and natural frequency. - //Our runtime used variables are softness and an error reduction feedback scale.. - //(For the following, I'll use the ODE terms CFM and ERP, constraint force mixing and error reduction parameter.) - //So first, we need to get from damping ratio and natural frequency to stiffness and damping spring constants. - //From there, we'll go to CFM/ERP. - //Then, we'll create an expression for a softened effective mass matrix (i.e. one that takes into account the CFM term), - //and an expression for the contraint force mixing term in the solve iteration. - //Finally, compute ERP. - //(And then some tricks.) - - //1) Convert from damping ratio and natural frequency to stiffness and damping constants. - //The raw expressions are: - //stiffness = effectiveMass * naturalFrequency^2 - //damping = effectiveMass * 2 * dampingRatio * naturalFrequency - //Rather than using any single object as the reference for the 'mass' term involved in this conversion, use the effective mass of the constraint. - //In other words, we're dynamically picking the spring constants necessary to achieve the desired behavior for the current constraint configuration. - //(See Erin Catto's presentation above for more details on this.) - - //(Note that this is different from BEPUphysics v1. There, users configured stiffness and damping constants. That worked okay, but people often got confused about - //why constraints didn't behave the same when they changed masses. Usually it manifested as someone creating an incredibly high mass object relative to the default - //stiffness/damping, and they'd post on the forum wondering why constraints were so soft. Basically, the defaults were another sneaky tuning factor to get wrong. - //Since damping ratio and natural frequency define the behavior independent of the mass, this problem goes away- and it makes some other interesting things happen...) - - //2) Convert from stiffness and damping constants to CFM and ERP. - //CFM = (stiffness * dt + damping)^-1 - //ERP = (stiffness * dt) * (stiffness * dt + damping)^-1 - //Or, to rephrase: - //ERP = (stiffness * dt) * CFM - - //3) Use CFM and ERP to create a softened effective mass matrix and a force mixing term for the solve iterations. - //Start with a base definition which we won't be deriving, the velocity constraint itself (stated as an equality constraint here): - //This means 'world space velocity projected into constraint space should equal the velocity bias term combined with the constraint force mixing term'. - //(The velocity bias term will be computed later- it's the position error scaled by the error reduction parameter, ERP. Position error is used to create a velocity motor goal.) - //We're pulling back from the implementation of sequential impulses here, so rather than using the term 'accumulated impulse', we'll use 'lambda' - //(which happens to be consistent with the ODE documentation covering the same topic). Lambda is impulse that satisfies the constraint. - //wsv * JT = bias - lambda * CFM/dt - //This can be phrased as: - //currentVelocity = targetVelocity - //Or: - //goalVelocityChange = targetVelocity - currentVelocity - //lambda = goalVelocityChange * effectiveMass - //lambda = (targetVelocity - currentVelocity) * effectiveMass - //lambda = (bias - lambda * CFM/dt - currentVelocity) * effectiveMass - //Solving for lambda: - //lambda = (bias - currentVelocity) * effectiveMass - lambda * CFM/dt * effectiveMass - //lambda + lambda * CFM/dt * effectiveMass = (bias - currentVelocity) * effectiveMass - //(lambda + lambda * CFM/dt * effectiveMass) * effectiveMass^-1 = bias - currentVelocity - //lambda * effectiveMass^-1 + lambda * CFM/dt = bias - currentVelocity - //lambda * (effectiveMass^-1 + CFM/dt) = bias - currentVelocity - //lambda = (bias - currentVelocity) * (effectiveMass^-1 + CFM/dt)^-1 - //lambda = (bias - wsv * JT) * (effectiveMass^-1 + CFM/dt)^-1 - //In other words, we transform the velocity change (bias - wsv * JT) into the constraint-satisfying impulse, lambda, using a matrix (effectiveMass^-1 + CFM/dt)^-1. - //That matrix is the softened effective mass: - //softenedEffectiveMass = (effectiveMass^-1 + CFM/dt)^-1 - - //Here's where some trickiness occurs. (Be mindful of the distinction between the softened and unsoftened effective mass). - //Start by substituting CFM into the softened effective mass definition: - //CFM/dt = (stiffness * dt + damping)^-1 / dt = (dt * (stiffness * dt + damping))^-1 = (stiffness * dt^2 + damping*dt)^-1 - //softenedEffectiveMass = (effectiveMass^-1 + (stiffness * dt^2 + damping * dt)^-1)^-1 - //Now substitute the definitions of stiffness and damping, treating the scalar components as uniform scaling matrices of dimension equal to effectiveMass: - //softenedEffectiveMass = (effectiveMass^-1 + ((effectiveMass * naturalFrequency^2) * dt^2 + (effectiveMass * 2 * dampingRatio * naturalFrequency) * dt)^-1)^-1 - //Combine the inner effectiveMass coefficients, given matrix multiplication distributes over addition: - //softenedEffectiveMass = (effectiveMass^-1 + (effectiveMass * (naturalFrequency^2 * dt^2) + effectiveMass * (2 * dampingRatio * naturalFrequency * dt))^-1)^-1 - //softenedEffectiveMass = (effectiveMass^-1 + (effectiveMass * (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt))^-1)^-1 - //Apply the inner matrix inverse: - //softenedEffectiveMass = (effectiveMass^-1 + (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1 * effectiveMass^-1)^-1 - //Once again, combine coefficients of the inner effectiveMass^-1 terms: - //softenedEffectiveMass = ((1 + (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1) * effectiveMass^-1)^-1 - //Apply the inverse again: - //softenedEffectiveMass = effectiveMass * (1 + (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1)^-1 - - //So, to put it another way- because CFM is based on the effective mass, applying it to the effective mass results in a simple downscale. - - //What has been gained? Consider what happens in the solve iteration. - //We take the velocity error: - //velocityError = bias - accumulatedImpulse * CFM/dt - wsv * JT - //and convert it to a corrective impulse with the effective mass: - //impulse = (bias - accumulatedImpulse * CFM/dt - wsv * JT) * softenedEffectiveMass - //The effective mass distributes over the set: - //impulse = bias * softenedEffectiveMass - accumulatedImpulse * CFM/dt * softenedEffectiveMass - wsv * JT * softenedEffectiveMass - //Focus on the CFM term: - //-accumulatedImpulse * CFM/dt * softenedEffectiveMass - //What is CFM/dt * softenedEffectiveMass? Substitute. - //(stiffness * dt^2 + damping * dt)^-1 * softenedEffectiveMass - //((effectiveMass * naturalFrequency^2) * dt^2 + (effectiveMass * 2 * dampingRatio * naturalFrequency * dt))^-1 * softenedEffectiveMass - //Combine terms: - //(effectiveMass * (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt))^-1 * softenedEffectiveMass - //Apply inverse: - //(naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1 * effectiveMass^-1 * softenedEffectiveMass - //Expand softened effective mass from earlier: - //(naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1 * effectiveMass^-1 * effectiveMass * (1 + (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1)^-1 - //Cancel effective masses: (!) - //(naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1 * (1 + (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1)^-1 - //Because CFM was created from effectiveMass, the CFM/dt * effectiveMass term is actually independent of the effectiveMass! - //The remaining expression is still a matrix, but fortunately it is a simple uniform scaling matrix that we can store and apply as a single scalar. - - //4) How do you compute ERP? - //ERP = (stiffness * dt) * CFM - //ERP = (stiffness * dt) * (stiffness * dt + damping)^-1 - //ERP = ((effectiveMass * naturalFrequency^2) * dt) * ((effectiveMass * naturalFrequency^2) * dt + (effectiveMass * 2 * dampingRatio * naturalFrequency))^-1 - //Combine denominator terms: - //ERP = ((effectiveMass * naturalFrequency^2) * dt) * ((effectiveMass * (naturalFrequency^2 * dt + 2 * dampingRatio * naturalFrequency))^-1 - //Apply denominator inverse: - //ERP = ((effectiveMass * naturalFrequency^2) * dt) * (naturalFrequency^2 * dt + 2 * dampingRatio * naturalFrequency)^-1 * effectiveMass^-1 - //Uniform scaling matrices commute: - //ERP = (naturalFrequency^2 * dt) * effectiveMass * effectiveMass^-1 * (naturalFrequency^2 * dt + 2 * dampingRatio * naturalFrequency)^-1 - //Cancellation! - //ERP = (naturalFrequency^2 * dt) * (naturalFrequency^2 * dt + 2 * dampingRatio * naturalFrequency)^-1 - //ERP = (naturalFrequency * dt) * (naturalFrequency * dt + 2 * dampingRatio)^-1 - //ERP is a simple scalar, independent of mass. - - //5) So we can compute CFM, ERP, the softened effective mass matrix, and we have an interesting shortcut on the constraint force mixing term of the solve iterations. - //Is there anything more that can be done? You bet! - //Let's look at the post-distribution impulse computation again: - //impulse = bias * effectiveMass - accumulatedImpulse * CFM/dt * effectiveMass - wsv * JT * effectiveMass - //During the solve iterations, the only quantities that vary are the accumulated impulse and world space velocities. So the rest can be precomputed. - //bias * effectiveMass, - //CFM/dt * effectiveMass, - //JT * effectiveMass - //In other words, we bypass the intermediate velocity state and go directly from source velocities to an impulse. - //Note the sizes of the precomputed types above: - //bias * effective mass is the same size as bias (vector with dimension equal to constrained DOFs) - //CFM/dt * effectiveMass is a single scalar regardless of constrained DOFs, - //JT * effectiveMass is the same size as JT - //But note that we no longer need to load the effective mass! It is implicit. - //The resulting computation is: - //impulse = a - accumulatedImpulse * b - wsv * c - //two DOF-width adds (add/subtract), one DOF-width multiply, and a 1xDOF * DOFx12 jacobian-sized transform. - //Compare to; - //(bias - accumulatedImpulse * CFM/dt - wsv * JT) * effectiveMass - //two DOF-width adds (add/subtract), one DOF width multiply, a 1xDOF * DOFx12 jacobian-sized transform, and a 1xDOF * DOFxDOF transform. - //In other words, we shave off a whole 1xDOF * DOFxDOF transform per iteration. - //So, taken in isolation, this is a strict win both in terms of memory and the amount of computation. - - //Unfortunately, it's not quite so simple- jacobians are ALSO used to transform the impulse into world space so that it can be used to change the body velocities. - //We still need to have those around. So while we no longer store the effective mass, our jacobian has sort of been duplicated. - //But wait, there's more! - - //That process looks like: - //wsv += impulse * J * M^-1 - //So while we need to store something here, we can take advantage of the fact that we aren't using the jacobian anywhere else (it's replaced by the JT * effectiveMass term above). - //Precompute J*M^-1, too. - //So you're still loading a jacobian-sized matrix, but you don't need to load M^-1! That saves you 14 scalars. (symmetric 3x3 + 1 + symmetric 3x3 + 1) - //That saves you the multiplication of (impulse * J) * M^-1, which is 6 multiplies and 6 dot products. - - //Note that this optimization's value depends on the number of constrained DOFs. - - //Net memory change, opt vs no opt, in scalars: - //1DOF: costs 1x12, saves 1x1 effective mass and the 14 scalar M^-1: -3 - //2DOF: costs 2x12, saves 2x2 symmetric effective mass and the 14 scalar M^-1: 7 - //3DOF: costs 3x12, saves 3x3 symmetric effective mass and the 14 scalar M^-1: 16 - //4DOF: costs 4x12, saves 4x4 symmetric effective mass and the 14 scalar M^-1: 24 - //5DOF: costs 5x12, saves 5x5 symmetric effective mass and the 14 scalar M^-1: 31 - //6DOF: costs 6x12, saves 6x6 symmetric effective mass and the 14 scalar M^-1: 37 - - //Net compute savings, opt vs no opt: - //DOF savings = 1xDOF * DOFxDOF (DOF DOFdot products), 2 1x3 * scalar (6 multiplies), 2 1x3 * 3x3 (6 3dot products) - // = (DOF*DOF multiplies + DOF*(DOF-1) adds) + (6 multiplies) + (18 multiplies + 12 adds) - // = DOF*DOF + 24 multiplies, DOF*DOF-DOF + 12 adds - //1DOF: 25 multiplies, 12 adds - //2DOF: 28 multiplies, 14 adds - //3DOF: 33 multiplies, 18 adds - //4DOF: 40 multiplies, 24 adds - //5DOF: 49 multiplies, 32 adds - //6DOF: 60 multiplies, 42 adds - - //So does our 'optimization' actually do anything useful? - //In 1 DOF constraints, it's often a win with no downsides. - //2+ are difficult to determine. - //This depends on heavily on the machine's SIMD width. You do every lane's ALU ops in parallel, but the loads are still fundamentally bound by memory bandwidth. - //The loads are coherent, at least- no gathers on this stuff. But I wouldn't be surprised if 3DOF+ constraints end up being faster *without* the pretransformations on wide SIMD. - //This is just something that will require case by case analysis. Constraints can have special structure which change the judgment. - - //(Also, note that large DOF jacobians are often very sparse. Consider the jacobians used by a 6DOF weld joint. You could likely do special case optimizations to reduce the - //load further. It is unlikely that you could find a way to do the same to JT * effectiveMass. J * M^-1 might have some savings, though. But J*M^-1 isn't *sparser* - //than J by itself, so the space savings are limited. As long as you precompute, the above load requirement offset will persist.) - - //Good news, though! There are a lot of constraints where this trick is applicable. - - //We'll start with the unsoftened effective mass, constructed from the contributions computed above: - var effectiveMass = Vector.One / (linearA + linearB + angularA + angularB); - - SpringSettingsWide.ComputeSpringiness(springSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); - var softenedEffectiveMass = effectiveMass * effectiveMassCFMScale; - - //Note that we use a bit of a hack when computing the bias velocity- even if our damping ratio/natural frequency implies a strongly springy response - //that could cause a significant velocity overshoot, we apply an arbitrary clamping value to keep it reasonable. - //This is useful for a variety of inequality constraints (like contacts) because you don't always want them behaving as true springs. - var biasVelocity = Vector.Min(positionError * positionErrorToVelocity, maximumRecoveryVelocity); - projection.BiasImpulse = biasVelocity * softenedEffectiveMass; - - //Precompute the wsv * (JT * softenedEffectiveMass) term. - //Note that we store it in a Vector3Wide as if it's a row vector, but this is really a column (because JT is a column vector). - //So we're really storing (JT * softenedEffectiveMass)T = softenedEffectiveMassT * J. - //Since this constraint is 1DOF, the softenedEffectiveMass is a scalar and the order doesn't matter. - //In the solve iterations, the WSVtoCSI term will be transposed during transformation, - //resulting in the proper wsv * (softenedEffectiveMassT * J)T = wsv * (JT * softenedEffectiveMass). - //You'll see this pattern repeated in higher DOF constraints. We explicitly compute softenedEffectiveMassT * J, and then apply the transpose in the solves. - //(Why? Because creating a Matrix3x2 and Matrix2x3 and 4x3 and 3x4 and 5x3 and 3x5 and so on just doubles the number of representations with little value.) - Vector3Wide.Scale(jacobians.LinearA, softenedEffectiveMass, out projection.WSVtoCSILinearA); - Vector3Wide.Scale(jacobians.AngularA, softenedEffectiveMass, out projection.WSVtoCSIAngularA); - Vector3Wide.Scale(jacobians.LinearB, softenedEffectiveMass, out projection.WSVtoCSILinearB); - Vector3Wide.Scale(jacobians.AngularB, softenedEffectiveMass, out projection.WSVtoCSIAngularB); - } - //Naming conventions: - //We transform between two spaces, world and constraint space. We also deal with two quantities- velocities, and impulses. - //And we have some number of entities involved in the constraint. So: - //wsva: world space velocity of body A - //wsvb: world space velocity of body B - //csvError: constraint space velocity error- when the body velocities are projected into constraint space and combined with the velocity biases, the result is a single constraint velocity error - //csva: constraint space velocity of body A; the world space velocities projected onto transpose(jacobianA) - //csvaLinear: contribution to the constraint space velocity by body A's linear velocity - - - /// - /// Transforms an impulse from constraint space to world space, uses it to modify the cached world space velocities of the bodies. - /// - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ApplyImpulse(ref Projection2Body1DOF data, ref Vector correctiveImpulse, - ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) - { - //Applying the impulse requires transforming the constraint space impulse into a world space velocity change. - //The first step is to transform into a world space impulse, which requires transforming by the transposed jacobian - //(transpose(jacobian) goes from world to constraint space, jacobian goes from constraint to world space). - //That world space impulse is then converted to a corrective velocity change by scaling the impulse by the inverse mass/inertia. - //As an optimization for constraints with smaller jacobians, the jacobian * (inertia or mass) transform is precomputed. - BodyVelocityWide correctiveVelocityA, correctiveVelocityB; - Vector3Wide.Scale(data.CSIToWSVLinearA, correctiveImpulse, out correctiveVelocityA.Linear); - Vector3Wide.Scale(data.CSIToWSVAngularA, correctiveImpulse, out correctiveVelocityA.Angular); - Vector3Wide.Scale(data.CSIToWSVLinearB, correctiveImpulse, out correctiveVelocityB.Linear); - Vector3Wide.Scale(data.CSIToWSVAngularB, correctiveImpulse, out correctiveVelocityB.Angular); - Vector3Wide.Add(correctiveVelocityA.Linear, wsvA.Linear, out wsvA.Linear); - Vector3Wide.Add(correctiveVelocityA.Angular, wsvA.Angular, out wsvA.Angular); - Vector3Wide.Add(correctiveVelocityB.Linear, wsvB.Linear, out wsvB.Linear); - Vector3Wide.Add(correctiveVelocityB.Angular, wsvB.Angular, out wsvB.Angular); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void WarmStart(ref Projection2Body1DOF data, ref Vector accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) - { - //TODO: If the previous frame and current frame are associated with different time steps, the previous frame's solution won't be a good solution anymore. - //To compensate for this, the accumulated impulse should be scaled if dt changes. - ApplyImpulse(ref data, ref accumulatedImpulse, ref wsvA, ref wsvB); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ComputeCorrectiveImpulse(ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref Projection2Body1DOF projection, ref Vector accumulatedImpulse, - out Vector correctiveCSI) - { - //Take the world space velocity of each body into constraint space by transforming by the transpose(jacobian). - //(The jacobian is a row vector by convention, while we treat our velocity vectors as a 12x1 row vector for the purposes of constraint space velocity calculation. - //So we are multiplying v * JT.) - //Then, transform it into an impulse by applying the effective mass. - //Here, we combine the projection and impulse conversion into a precomputed value, i.e. v * (JT * softenedEffectiveMass). - Vector3Wide.Dot(wsvA.Linear, projection.WSVtoCSILinearA, out var csiaLinear); - Vector3Wide.Dot(wsvA.Angular, projection.WSVtoCSIAngularA, out var csiaAngular); - Vector3Wide.Dot(wsvB.Linear, projection.WSVtoCSILinearB, out var csibLinear); - Vector3Wide.Dot(wsvB.Angular, projection.WSVtoCSIAngularB, out var csibAngular); - //Combine it all together, following: - //constraint space impulse = (targetVelocity - currentVelocity) * softenedEffectiveMass - //constraint space impulse = (bias - accumulatedImpulse * softness - wsv * JT) * softenedEffectiveMass - //constraint space impulse = (bias * softenedEffectiveMass) - accumulatedImpulse * (softness * softenedEffectiveMass) - wsv * (JT * softenedEffectiveMass) - var csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); - - var previousAccumulated = accumulatedImpulse; - accumulatedImpulse = Vector.Max(Vector.Zero, accumulatedImpulse + csi); - - correctiveCSI = accumulatedImpulse - previousAccumulated; - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Solve(ref Projection2Body1DOF projection, ref Vector accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) - { - ComputeCorrectiveImpulse(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulse, out var correctiveCSI); - ApplyImpulse(ref projection, ref correctiveCSI, ref wsvA, ref wsvB); - } - - } -} diff --git a/Demos/SpecializedTests/Inequality1DOF.cs b/Demos/SpecializedTests/Inequality1DOF.cs new file mode 100644 index 000000000..242e6b4b8 --- /dev/null +++ b/Demos/SpecializedTests/Inequality1DOF.cs @@ -0,0 +1,372 @@ +using BepuPhysics; +using BepuPhysics.Constraints; +using BepuUtilities; +using System.Numerics; +using System.Runtime.CompilerServices; + +namespace Demos.SpecializedTests; + +//TODO: These are notes about the mathy bits underlying constraints. They were written for the original pre-2.4 version of the solver. +//Most of it's still applicable, but 2.4 and up no longer have separate 'projection' states, and while packing is still important, it applies to *prestep data*. +//There's a lot less room for tricky premultiplication to save memory bandwidth, simply because those quantities are all in registers/L1 cache during a constraint solve in 2.4+. +//Would be nice to update those parts. + +public struct TwoBody1DOFJacobians +{ + public Vector3Wide LinearA; + public Vector3Wide AngularA; + public Vector3Wide LinearB; + public Vector3Wide AngularB; +} + +public struct Projection2Body1DOF +{ + //Rather than projecting from world space to constraint space *velocity* using JT, we precompute JT * effective mass + //and go directly from world space velocity to constraint space impulse. + public Vector3Wide WSVtoCSILinearA; + public Vector3Wide WSVtoCSIAngularA; + public Vector3Wide WSVtoCSILinearB; + public Vector3Wide WSVtoCSIAngularB; + + //Since we jump directly from world space velocity to constraint space impulse, the velocity bias needs to be precomputed into an impulse offset too. + public Vector BiasImpulse; + //And once again, CFM becomes CFM * EffectiveMass- massively cancels out due to the derivation of CFM. (See prestep notes.) + public Vector SoftnessImpulseScale; + + //It also needs to project from constraint space to world space. + //We bundle this with the inertia/mass multiplier, so rather than taking a constraint impulse to world impulse and then to world velocity change, + //we just go directly from constraint impulse to world velocity change. + //For constraints with lower DOF counts, using this format also saves us some memory bandwidth- + //the inverse inertia tensor and inverse mass for a 2 body constraint cost 20 floats, compared to this implementation's 12. + //(Note that even in an implementation where we use the body inertias, we should still cache it constraint-locally to avoid big gathers.) + public Vector3Wide CSIToWSVLinearA; + public Vector3Wide CSIToWSVAngularA; + public Vector3Wide CSIToWSVLinearB; + public Vector3Wide CSIToWSVAngularB; +} + +public static class Inequality2Body1DOF +{ + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Prestep(ref BodyInertiaWide inertiaA, ref BodyInertiaWide inertiaB, ref TwoBody1DOFJacobians jacobians, ref SpringSettingsWide springSettings, ref Vector maximumRecoveryVelocity, + ref Vector positionError, float dt, float inverseDt, out Projection2Body1DOF projection) + { + //unsoftened effective mass = (J * M^-1 * JT)^-1 + //where J is a constraintDOF x bodyCount*6 sized matrix, JT is its transpose, and for two bodies M^-1 is: + //[inverseMassA, 0, 0, 0] + //[0, inverseInertiaA, 0, 0] + //[0, 0, inverseMassB, 0] + //[0, 0, 0, inverseInertiaB] + //The entries of J match up to this convention, containing the linear and angular components of each body in sequence, so for a 2 body 1DOF constraint J would look like: + //[linearA 1x3, angularA 1x3, linearB 1x3, angularB 1x3] + //Note that it is a row vector by convention. When transforming velocities from world space into constraint space, it is assumed that the velocity vector is organized as a + //row vector matching up to the jacobian (that is, [linearA 1x3, angularA 1x3, linearB 1x3, angularB 1x3]), so for a 2 body 2 DOF constraint, + //worldVelocity * JT would be a [worldVelocity: 1x12] * [JT: 12x2], resulting in a 1x2 constraint space velocity row vector. + //Similarly, when going from constraint space impulse to world space impulse in the above example, we would do [csi: 1x2] * [J: 2x12] to get a 1x12 world impulse row vector. + + //Note that the engine uses row vectors for all velocities and positions and so on. Rotation and inertia tensors are constructed for premultiplication. + //In other words, unlike many of the presentations in the space, we use v * JT and csi * J instead of J * v and JT * csi. + //There is no meaningful difference- the two conventions are just transpositions of each other. + + //(If you want to know how this stuff works, go read the constraint related presentations: http://box2d.org/downloads/ + //Be mindful of the difference in conventions. You'll see J * v instead of v * JT, for example. Everything is still fundamentally the same, though.) + + //Due to the block structure of the mass matrix, we can handle each component separately and then sum the results. + //For this 1DOF constraint, the result is a simple scalar. + //Note that we store the intermediate results of J * M^-1 for use when projecting from constraint space impulses to world velocity changes. + //If we didn't store those intermediate values, we could just scale the dot product of jacobians.LinearA with itself to save 4 multiplies. + Vector3Wide.Scale(jacobians.LinearA, inertiaA.InverseMass, out projection.CSIToWSVLinearA); + Vector3Wide.Scale(jacobians.LinearB, inertiaB.InverseMass, out projection.CSIToWSVLinearB); + Vector3Wide.Dot(projection.CSIToWSVLinearA, jacobians.LinearA, out var linearA); + Vector3Wide.Dot(projection.CSIToWSVLinearB, jacobians.LinearB, out var linearB); + + //The angular components are a little more involved; (J * I^-1) * JT is explicitly computed. + Symmetric3x3Wide.TransformWithoutOverlap(jacobians.AngularA, inertiaA.InverseInertiaTensor, out projection.CSIToWSVAngularA); + Symmetric3x3Wide.TransformWithoutOverlap(jacobians.AngularB, inertiaB.InverseInertiaTensor, out projection.CSIToWSVAngularB); + Vector3Wide.Dot(projection.CSIToWSVAngularA, jacobians.AngularA, out var angularA); + Vector3Wide.Dot(projection.CSIToWSVAngularB, jacobians.AngularB, out var angularB); + + //Now for a digression! + //Softness is applied along the diagonal (which, for a 1DOF constraint, is just the only element). + //Check the the ODE reference for a bit more information: http://ode.org/ode-latest-userguide.html#sec_3_8_0 + //And also see Erin Catto's Soft Constraints presentation for more details: http://box2d.org/files/GDC2011/GDC2011_Catto_Erin_Soft_Constraints.pdf) + + //There are some very interesting tricks you can use here, though. + //Our core tuning variables are the damping ratio and natural frequency. + //Our runtime used variables are softness and an error reduction feedback scale.. + //(For the following, I'll use the ODE terms CFM and ERP, constraint force mixing and error reduction parameter.) + //So first, we need to get from damping ratio and natural frequency to stiffness and damping spring constants. + //From there, we'll go to CFM/ERP. + //Then, we'll create an expression for a softened effective mass matrix (i.e. one that takes into account the CFM term), + //and an expression for the contraint force mixing term in the solve iteration. + //Finally, compute ERP. + //(And then some tricks.) + + //1) Convert from damping ratio and natural frequency to stiffness and damping constants. + //The raw expressions are: + //stiffness = effectiveMass * naturalFrequency^2 + //damping = effectiveMass * 2 * dampingRatio * naturalFrequency + //Rather than using any single object as the reference for the 'mass' term involved in this conversion, use the effective mass of the constraint. + //In other words, we're dynamically picking the spring constants necessary to achieve the desired behavior for the current constraint configuration. + //(See Erin Catto's presentation above for more details on this.) + + //(Note that this is different from BEPUphysics v1. There, users configured stiffness and damping constants. That worked okay, but people often got confused about + //why constraints didn't behave the same when they changed masses. Usually it manifested as someone creating an incredibly high mass object relative to the default + //stiffness/damping, and they'd post on the forum wondering why constraints were so soft. Basically, the defaults were another sneaky tuning factor to get wrong. + //Since damping ratio and natural frequency define the behavior independent of the mass, this problem goes away- and it makes some other interesting things happen...) + + //2) Convert from stiffness and damping constants to CFM and ERP. + //CFM = (stiffness * dt + damping)^-1 + //ERP = (stiffness * dt) * (stiffness * dt + damping)^-1 + //Or, to rephrase: + //ERP = (stiffness * dt) * CFM + + //3) Use CFM and ERP to create a softened effective mass matrix and a force mixing term for the solve iterations. + //Start with a base definition which we won't be deriving, the velocity constraint itself (stated as an equality constraint here): + //This means 'world space velocity projected into constraint space should equal the velocity bias term combined with the constraint force mixing term'. + //(The velocity bias term will be computed later- it's the position error scaled by the error reduction parameter, ERP. Position error is used to create a velocity motor goal.) + //We're pulling back from the implementation of sequential impulses here, so rather than using the term 'accumulated impulse', we'll use 'lambda' + //(which happens to be consistent with the ODE documentation covering the same topic). Lambda is impulse that satisfies the constraint. + //wsv * JT = bias - lambda * CFM/dt + //This can be phrased as: + //currentVelocity = targetVelocity + //Or: + //goalVelocityChange = targetVelocity - currentVelocity + //lambda = goalVelocityChange * effectiveMass + //lambda = (targetVelocity - currentVelocity) * effectiveMass + //lambda = (bias - lambda * CFM/dt - currentVelocity) * effectiveMass + //Solving for lambda: + //lambda = (bias - currentVelocity) * effectiveMass - lambda * CFM/dt * effectiveMass + //lambda + lambda * CFM/dt * effectiveMass = (bias - currentVelocity) * effectiveMass + //(lambda + lambda * CFM/dt * effectiveMass) * effectiveMass^-1 = bias - currentVelocity + //lambda * effectiveMass^-1 + lambda * CFM/dt = bias - currentVelocity + //lambda * (effectiveMass^-1 + CFM/dt) = bias - currentVelocity + //lambda = (bias - currentVelocity) * (effectiveMass^-1 + CFM/dt)^-1 + //lambda = (bias - wsv * JT) * (effectiveMass^-1 + CFM/dt)^-1 + //In other words, we transform the velocity change (bias - wsv * JT) into the constraint-satisfying impulse, lambda, using a matrix (effectiveMass^-1 + CFM/dt)^-1. + //That matrix is the softened effective mass: + //softenedEffectiveMass = (effectiveMass^-1 + CFM/dt)^-1 + + //Here's where some trickiness occurs. (Be mindful of the distinction between the softened and unsoftened effective mass). + //Start by substituting CFM into the softened effective mass definition: + //CFM/dt = (stiffness * dt + damping)^-1 / dt = (dt * (stiffness * dt + damping))^-1 = (stiffness * dt^2 + damping*dt)^-1 + //softenedEffectiveMass = (effectiveMass^-1 + (stiffness * dt^2 + damping * dt)^-1)^-1 + //Now substitute the definitions of stiffness and damping, treating the scalar components as uniform scaling matrices of dimension equal to effectiveMass: + //softenedEffectiveMass = (effectiveMass^-1 + ((effectiveMass * naturalFrequency^2) * dt^2 + (effectiveMass * 2 * dampingRatio * naturalFrequency) * dt)^-1)^-1 + //Combine the inner effectiveMass coefficients, given matrix multiplication distributes over addition: + //softenedEffectiveMass = (effectiveMass^-1 + (effectiveMass * (naturalFrequency^2 * dt^2) + effectiveMass * (2 * dampingRatio * naturalFrequency * dt))^-1)^-1 + //softenedEffectiveMass = (effectiveMass^-1 + (effectiveMass * (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt))^-1)^-1 + //Apply the inner matrix inverse: + //softenedEffectiveMass = (effectiveMass^-1 + (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1 * effectiveMass^-1)^-1 + //Once again, combine coefficients of the inner effectiveMass^-1 terms: + //softenedEffectiveMass = ((1 + (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1) * effectiveMass^-1)^-1 + //Apply the inverse again: + //softenedEffectiveMass = effectiveMass * (1 + (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1)^-1 + + //So, to put it another way- because CFM is based on the effective mass, applying it to the effective mass results in a simple downscale. + + //What has been gained? Consider what happens in the solve iteration. + //We take the velocity error: + //velocityError = bias - accumulatedImpulse * CFM/dt - wsv * JT + //and convert it to a corrective impulse with the effective mass: + //impulse = (bias - accumulatedImpulse * CFM/dt - wsv * JT) * softenedEffectiveMass + //The effective mass distributes over the set: + //impulse = bias * softenedEffectiveMass - accumulatedImpulse * CFM/dt * softenedEffectiveMass - wsv * JT * softenedEffectiveMass + //Focus on the CFM term: + //-accumulatedImpulse * CFM/dt * softenedEffectiveMass + //What is CFM/dt * softenedEffectiveMass? Substitute. + //(stiffness * dt^2 + damping * dt)^-1 * softenedEffectiveMass + //((effectiveMass * naturalFrequency^2) * dt^2 + (effectiveMass * 2 * dampingRatio * naturalFrequency * dt))^-1 * softenedEffectiveMass + //Combine terms: + //(effectiveMass * (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt))^-1 * softenedEffectiveMass + //Apply inverse: + //(naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1 * effectiveMass^-1 * softenedEffectiveMass + //Expand softened effective mass from earlier: + //(naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1 * effectiveMass^-1 * effectiveMass * (1 + (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1)^-1 + //Cancel effective masses: (!) + //(naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1 * (1 + (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1)^-1 + //Because CFM was created from effectiveMass, the CFM/dt * effectiveMass term is actually independent of the effectiveMass! + //The remaining expression is still a matrix, but fortunately it is a simple uniform scaling matrix that we can store and apply as a single scalar. + + //4) How do you compute ERP? + //ERP = (stiffness * dt) * CFM + //ERP = (stiffness * dt) * (stiffness * dt + damping)^-1 + //ERP = ((effectiveMass * naturalFrequency^2) * dt) * ((effectiveMass * naturalFrequency^2) * dt + (effectiveMass * 2 * dampingRatio * naturalFrequency))^-1 + //Combine denominator terms: + //ERP = ((effectiveMass * naturalFrequency^2) * dt) * ((effectiveMass * (naturalFrequency^2 * dt + 2 * dampingRatio * naturalFrequency))^-1 + //Apply denominator inverse: + //ERP = ((effectiveMass * naturalFrequency^2) * dt) * (naturalFrequency^2 * dt + 2 * dampingRatio * naturalFrequency)^-1 * effectiveMass^-1 + //Uniform scaling matrices commute: + //ERP = (naturalFrequency^2 * dt) * effectiveMass * effectiveMass^-1 * (naturalFrequency^2 * dt + 2 * dampingRatio * naturalFrequency)^-1 + //Cancellation! + //ERP = (naturalFrequency^2 * dt) * (naturalFrequency^2 * dt + 2 * dampingRatio * naturalFrequency)^-1 + //ERP = (naturalFrequency * dt) * (naturalFrequency * dt + 2 * dampingRatio)^-1 + //ERP is a simple scalar, independent of mass. + + //5) So we can compute CFM, ERP, the softened effective mass matrix, and we have an interesting shortcut on the constraint force mixing term of the solve iterations. + //Is there anything more that can be done? You bet! + //Let's look at the post-distribution impulse computation again: + //impulse = bias * effectiveMass - accumulatedImpulse * CFM/dt * effectiveMass - wsv * JT * effectiveMass + //During the solve iterations, the only quantities that vary are the accumulated impulse and world space velocities. So the rest can be precomputed. + //bias * effectiveMass, + //CFM/dt * effectiveMass, + //JT * effectiveMass + //In other words, we bypass the intermediate velocity state and go directly from source velocities to an impulse. + //Note the sizes of the precomputed types above: + //bias * effective mass is the same size as bias (vector with dimension equal to constrained DOFs) + //CFM/dt * effectiveMass is a single scalar regardless of constrained DOFs, + //JT * effectiveMass is the same size as JT + //But note that we no longer need to load the effective mass! It is implicit. + //The resulting computation is: + //impulse = a - accumulatedImpulse * b - wsv * c + //two DOF-width adds (add/subtract), one DOF-width multiply, and a 1xDOF * DOFx12 jacobian-sized transform. + //Compare to; + //(bias - accumulatedImpulse * CFM/dt - wsv * JT) * effectiveMass + //two DOF-width adds (add/subtract), one DOF width multiply, a 1xDOF * DOFx12 jacobian-sized transform, and a 1xDOF * DOFxDOF transform. + //In other words, we shave off a whole 1xDOF * DOFxDOF transform per iteration. + //So, taken in isolation, this is a strict win both in terms of memory and the amount of computation. + + //Unfortunately, it's not quite so simple- jacobians are ALSO used to transform the impulse into world space so that it can be used to change the body velocities. + //We still need to have those around. So while we no longer store the effective mass, our jacobian has sort of been duplicated. + //But wait, there's more! + + //That process looks like: + //wsv += impulse * J * M^-1 + //So while we need to store something here, we can take advantage of the fact that we aren't using the jacobian anywhere else (it's replaced by the JT * effectiveMass term above). + //Precompute J*M^-1, too. + //So you're still loading a jacobian-sized matrix, but you don't need to load M^-1! That saves you 14 scalars. (symmetric 3x3 + 1 + symmetric 3x3 + 1) + //That saves you the multiplication of (impulse * J) * M^-1, which is 6 multiplies and 6 dot products. + + //Note that this optimization's value depends on the number of constrained DOFs. + + //Net memory change, opt vs no opt, in scalars: + //1DOF: costs 1x12, saves 1x1 effective mass and the 14 scalar M^-1: -3 + //2DOF: costs 2x12, saves 2x2 symmetric effective mass and the 14 scalar M^-1: 7 + //3DOF: costs 3x12, saves 3x3 symmetric effective mass and the 14 scalar M^-1: 16 + //4DOF: costs 4x12, saves 4x4 symmetric effective mass and the 14 scalar M^-1: 24 + //5DOF: costs 5x12, saves 5x5 symmetric effective mass and the 14 scalar M^-1: 31 + //6DOF: costs 6x12, saves 6x6 symmetric effective mass and the 14 scalar M^-1: 37 + + //Net compute savings, opt vs no opt: + //DOF savings = 1xDOF * DOFxDOF (DOF DOFdot products), 2 1x3 * scalar (6 multiplies), 2 1x3 * 3x3 (6 3dot products) + // = (DOF*DOF multiplies + DOF*(DOF-1) adds) + (6 multiplies) + (18 multiplies + 12 adds) + // = DOF*DOF + 24 multiplies, DOF*DOF-DOF + 12 adds + //1DOF: 25 multiplies, 12 adds + //2DOF: 28 multiplies, 14 adds + //3DOF: 33 multiplies, 18 adds + //4DOF: 40 multiplies, 24 adds + //5DOF: 49 multiplies, 32 adds + //6DOF: 60 multiplies, 42 adds + + //So does our 'optimization' actually do anything useful? + //In 1 DOF constraints, it's often a win with no downsides. + //2+ are difficult to determine. + //This depends on heavily on the machine's SIMD width. You do every lane's ALU ops in parallel, but the loads are still fundamentally bound by memory bandwidth. + //The loads are coherent, at least- no gathers on this stuff. But I wouldn't be surprised if 3DOF+ constraints end up being faster *without* the pretransformations on wide SIMD. + //This is just something that will require case by case analysis. Constraints can have special structure which change the judgment. + + //(Also, note that large DOF jacobians are often very sparse. Consider the jacobians used by a 6DOF weld joint. You could likely do special case optimizations to reduce the + //load further. It is unlikely that you could find a way to do the same to JT * effectiveMass. J * M^-1 might have some savings, though. But J*M^-1 isn't *sparser* + //than J by itself, so the space savings are limited. As long as you precompute, the above load requirement offset will persist.) + + //Good news, though! There are a lot of constraints where this trick is applicable. + + //We'll start with the unsoftened effective mass, constructed from the contributions computed above: + var effectiveMass = Vector.One / (linearA + linearB + angularA + angularB); + + SpringSettingsWide.ComputeSpringiness(springSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale); + var softenedEffectiveMass = effectiveMass * effectiveMassCFMScale; + + //Note that we use a bit of a hack when computing the bias velocity- even if our damping ratio/natural frequency implies a strongly springy response + //that could cause a significant velocity overshoot, we apply an arbitrary clamping value to keep it reasonable. + //This is useful for a variety of inequality constraints (like contacts) because you don't always want them behaving as true springs. + var biasVelocity = Vector.Min(positionError * positionErrorToVelocity, maximumRecoveryVelocity); + projection.BiasImpulse = biasVelocity * softenedEffectiveMass; + + //Precompute the wsv * (JT * softenedEffectiveMass) term. + //Note that we store it in a Vector3Wide as if it's a row vector, but this is really a column (because JT is a column vector). + //So we're really storing (JT * softenedEffectiveMass)T = softenedEffectiveMassT * J. + //Since this constraint is 1DOF, the softenedEffectiveMass is a scalar and the order doesn't matter. + //In the solve iterations, the WSVtoCSI term will be transposed during transformation, + //resulting in the proper wsv * (softenedEffectiveMassT * J)T = wsv * (JT * softenedEffectiveMass). + //You'll see this pattern repeated in higher DOF constraints. We explicitly compute softenedEffectiveMassT * J, and then apply the transpose in the solves. + //(Why? Because creating a Matrix3x2 and Matrix2x3 and 4x3 and 3x4 and 5x3 and 3x5 and so on just doubles the number of representations with little value.) + Vector3Wide.Scale(jacobians.LinearA, softenedEffectiveMass, out projection.WSVtoCSILinearA); + Vector3Wide.Scale(jacobians.AngularA, softenedEffectiveMass, out projection.WSVtoCSIAngularA); + Vector3Wide.Scale(jacobians.LinearB, softenedEffectiveMass, out projection.WSVtoCSILinearB); + Vector3Wide.Scale(jacobians.AngularB, softenedEffectiveMass, out projection.WSVtoCSIAngularB); + } + //Naming conventions: + //We transform between two spaces, world and constraint space. We also deal with two quantities- velocities, and impulses. + //And we have some number of entities involved in the constraint. So: + //wsva: world space velocity of body A + //wsvb: world space velocity of body B + //csvError: constraint space velocity error- when the body velocities are projected into constraint space and combined with the velocity biases, the result is a single constraint velocity error + //csva: constraint space velocity of body A; the world space velocities projected onto transpose(jacobianA) + //csvaLinear: contribution to the constraint space velocity by body A's linear velocity + + + /// + /// Transforms an impulse from constraint space to world space, uses it to modify the cached world space velocities of the bodies. + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void ApplyImpulse(ref Projection2Body1DOF data, ref Vector correctiveImpulse, + ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + //Applying the impulse requires transforming the constraint space impulse into a world space velocity change. + //The first step is to transform into a world space impulse, which requires transforming by the transposed jacobian + //(transpose(jacobian) goes from world to constraint space, jacobian goes from constraint to world space). + //That world space impulse is then converted to a corrective velocity change by scaling the impulse by the inverse mass/inertia. + //As an optimization for constraints with smaller jacobians, the jacobian * (inertia or mass) transform is precomputed. + BodyVelocityWide correctiveVelocityA, correctiveVelocityB; + Vector3Wide.Scale(data.CSIToWSVLinearA, correctiveImpulse, out correctiveVelocityA.Linear); + Vector3Wide.Scale(data.CSIToWSVAngularA, correctiveImpulse, out correctiveVelocityA.Angular); + Vector3Wide.Scale(data.CSIToWSVLinearB, correctiveImpulse, out correctiveVelocityB.Linear); + Vector3Wide.Scale(data.CSIToWSVAngularB, correctiveImpulse, out correctiveVelocityB.Angular); + Vector3Wide.Add(correctiveVelocityA.Linear, wsvA.Linear, out wsvA.Linear); + Vector3Wide.Add(correctiveVelocityA.Angular, wsvA.Angular, out wsvA.Angular); + Vector3Wide.Add(correctiveVelocityB.Linear, wsvB.Linear, out wsvB.Linear); + Vector3Wide.Add(correctiveVelocityB.Angular, wsvB.Angular, out wsvB.Angular); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void WarmStart(ref Projection2Body1DOF data, ref Vector accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + //TODO: If the previous frame and current frame are associated with different time steps, the previous frame's solution won't be a good solution anymore. + //To compensate for this, the accumulated impulse should be scaled if dt changes. + ApplyImpulse(ref data, ref accumulatedImpulse, ref wsvA, ref wsvB); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void ComputeCorrectiveImpulse(ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref Projection2Body1DOF projection, ref Vector accumulatedImpulse, + out Vector correctiveCSI) + { + //Take the world space velocity of each body into constraint space by transforming by the transpose(jacobian). + //(The jacobian is a row vector by convention, while we treat our velocity vectors as a 12x1 row vector for the purposes of constraint space velocity calculation. + //So we are multiplying v * JT.) + //Then, transform it into an impulse by applying the effective mass. + //Here, we combine the projection and impulse conversion into a precomputed value, i.e. v * (JT * softenedEffectiveMass). + Vector3Wide.Dot(wsvA.Linear, projection.WSVtoCSILinearA, out var csiaLinear); + Vector3Wide.Dot(wsvA.Angular, projection.WSVtoCSIAngularA, out var csiaAngular); + Vector3Wide.Dot(wsvB.Linear, projection.WSVtoCSILinearB, out var csibLinear); + Vector3Wide.Dot(wsvB.Angular, projection.WSVtoCSIAngularB, out var csibAngular); + //Combine it all together, following: + //constraint space impulse = (targetVelocity - currentVelocity) * softenedEffectiveMass + //constraint space impulse = (bias - accumulatedImpulse * softness - wsv * JT) * softenedEffectiveMass + //constraint space impulse = (bias * softenedEffectiveMass) - accumulatedImpulse * (softness * softenedEffectiveMass) - wsv * (JT * softenedEffectiveMass) + var csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); + + var previousAccumulated = accumulatedImpulse; + accumulatedImpulse = Vector.Max(Vector.Zero, accumulatedImpulse + csi); + + correctiveCSI = accumulatedImpulse - previousAccumulated; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Solve(ref Projection2Body1DOF projection, ref Vector accumulatedImpulse, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB) + { + ComputeCorrectiveImpulse(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulse, out var correctiveCSI); + ApplyImpulse(ref projection, ref correctiveCSI, ref wsvA, ref wsvB); + } + +} From 3a40224aa06ba02137305ff9ab35a99ce15c4df7 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 29 Aug 2023 20:23:11 -0500 Subject: [PATCH 814/947] TaskStack dispatch a little more adaptive. --- BepuUtilities/TaskScheduling/TaskStack.cs | 18 +++++++++++++++--- 1 file changed, 15 insertions(+), 3 deletions(-) diff --git a/BepuUtilities/TaskScheduling/TaskStack.cs b/BepuUtilities/TaskScheduling/TaskStack.cs index 38593f7e8..246f127b1 100644 --- a/BepuUtilities/TaskScheduling/TaskStack.cs +++ b/BepuUtilities/TaskScheduling/TaskStack.cs @@ -562,11 +562,23 @@ public void For(delegate* function, v public static void DispatchWorkerFunction(int workerIndex, IThreadDispatcher dispatcher) { var taskStack = (TaskStack*)dispatcher.UnmanagedContext; - PopTaskResult popTaskResult; var waiter = new SpinWait(); - while ((popTaskResult = taskStack->TryPopAndRun(workerIndex, dispatcher)) != PopTaskResult.Stop) + while (true) { - waiter.SpinOnce(-1); + switch (taskStack->TryPopAndRun(workerIndex, dispatcher)) + { + case PopTaskResult.Stop: + //Done! + return; + case PopTaskResult.Success: + //If we ran a task, then the waiter should return to an aggressive spin because more work may be immediately available. + waiter.Reset(); + break; + default: + //No work available, but we should keep going. + waiter.SpinOnce(-1); + break; + } } } From b5c62807bc75021ff7ed270179d130dd9ec589d1 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 2 Sep 2023 15:44:38 -0500 Subject: [PATCH 815/947] Comment clarification. --- BepuUtilities/TaskScheduling/Job.cs | 1 - BepuUtilities/TaskScheduling/TaskStack.cs | 2 +- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/BepuUtilities/TaskScheduling/Job.cs b/BepuUtilities/TaskScheduling/Job.cs index 0a9c11eb6..09535d79d 100644 --- a/BepuUtilities/TaskScheduling/Job.cs +++ b/BepuUtilities/TaskScheduling/Job.cs @@ -43,7 +43,6 @@ internal unsafe struct Job /// True if a task was available to pop, false otherwise. internal bool TryPop(out Task task) { - var newCount = Interlocked.Decrement(ref Counter); if (newCount >= 0) { diff --git a/BepuUtilities/TaskScheduling/TaskStack.cs b/BepuUtilities/TaskScheduling/TaskStack.cs index 246f127b1..ef4810fe2 100644 --- a/BepuUtilities/TaskScheduling/TaskStack.cs +++ b/BepuUtilities/TaskScheduling/TaskStack.cs @@ -451,7 +451,7 @@ public void RequestStop() } /// - /// Convenience function for requesting a stop. Requires the context to be the expected . + /// Convenience function for requesting a stop. Requires the context to be a pointer to the expected . /// /// Id of the task. /// to be stopped. From e5af11e359805896bf53607bf3061566e37b6f02 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 2 Sep 2023 16:12:14 -0500 Subject: [PATCH 816/947] Tree test job size tuning. --- BepuPhysics/Trees/Tree_IntertreeQueriesMT.cs | 2 +- BepuPhysics/Trees/Tree_SelfQueriesMT.cs | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/BepuPhysics/Trees/Tree_IntertreeQueriesMT.cs b/BepuPhysics/Trees/Tree_IntertreeQueriesMT.cs index 81c91b7cc..ba9732b39 100644 --- a/BepuPhysics/Trees/Tree_IntertreeQueriesMT.cs +++ b/BepuPhysics/Trees/Tree_IntertreeQueriesMT.cs @@ -48,7 +48,7 @@ public void PrepareJobs(ref Tree treeA, ref Tree treeB, TOverlapHandler[] overla return; } Debug.Assert(overlapHandlers.Length >= threadCount); - const float jobMultiplier = 2.5f; + const float jobMultiplier = 8f; var targetJobCount = Math.Max(1, jobMultiplier * threadCount); //TODO: Not a lot of thought was put into this leaf threshold for intertree. Probably better options. leafThreshold = (int)((treeA.LeafCount + treeB.LeafCount) / targetJobCount); diff --git a/BepuPhysics/Trees/Tree_SelfQueriesMT.cs b/BepuPhysics/Trees/Tree_SelfQueriesMT.cs index 762b2bc7d..21d36181a 100644 --- a/BepuPhysics/Trees/Tree_SelfQueriesMT.cs +++ b/BepuPhysics/Trees/Tree_SelfQueriesMT.cs @@ -78,7 +78,7 @@ public void PrepareJobs(ref Tree tree, TOverlapHandler[] overlapHandlers, int th return; } Debug.Assert(overlapHandlers.Length >= threadCount); - const float jobMultiplier = 2.5f; + const float jobMultiplier = 8f; var targetJobCount = Math.Max(1, jobMultiplier * threadCount); leafThreshold = (int)(tree.LeafCount / targetJobCount); jobs = new QuickList((int)(targetJobCount * 2), Pool); From df18221040ac7daa4d6d7f7197561b83ff4fd88f Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 2 Sep 2023 18:06:52 -0500 Subject: [PATCH 817/947] Cleaned up the not-great attempt at a multithreaded intertree revamp. --- .../CollidableOverlapFinder.cs | 147 ------------ BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs | 213 ------------------ 2 files changed, 360 deletions(-) delete mode 100644 BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs diff --git a/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs b/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs index fcdfeaee2..e714eb9e9 100644 --- a/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs +++ b/BepuPhysics/CollisionDetection/CollidableOverlapFinder.cs @@ -103,10 +103,6 @@ void Worker(int workerIndex) } public override void DispatchOverlaps(float dt, IThreadDispatcher threadDispatcher = null) - { - DispatchOverlapsOld(dt, threadDispatcher); - } - public void DispatchOverlapsOld(float dt, IThreadDispatcher threadDispatcher = null) { if (threadDispatcher != null && threadDispatcher.ThreadCount > 1) { @@ -165,149 +161,6 @@ public void DispatchOverlapsOld(float dt, IThreadDispatcher threadDispatcher = n } } - - struct ThreadedIntertreeOverlapHandler : IThreadedOverlapHandler - { - public Buffer ActiveLeaves; - public Buffer StaticLeaves; - public void Handle(int indexA, int indexB, int workerIndex, object managedContext) - { - var narrowPhase = (NarrowPhase)managedContext; - narrowPhase.HandleOverlap(workerIndex, ActiveLeaves[indexA], StaticLeaves[indexB]); - } - } - struct ThreadedSelfOverlapHandler : IThreadedOverlapHandler - { - public Buffer Leaves; - public void Handle(int indexA, int indexB, int workerIndex, object managedContext) - { - //var narrowPhase = (NarrowPhase)managedContext; - var narrowPhase = Unsafe.As>(managedContext); - narrowPhase.HandleOverlap(workerIndex, Leaves[indexA], Leaves[indexB]); - } - } - - struct SelfContext - { - public TaskStack* Stack; - public ThreadedSelfOverlapHandler* Results; - public Tree Tree; - public int TargetTaskCount; - } - - struct IntertreeContext - { - public TaskStack* Stack; - public ThreadedIntertreeOverlapHandler* Results; - public Tree StaticTree; - public Tree ActiveTree; - public int TargetTaskCount; - } - - static void SelfEntryTask(long taskStartAndEnd, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) - { - Debug.Assert(dispatcher.ManagedContext != null); - ref var context = ref *(SelfContext*)untypedContext; - var pool = dispatcher.WorkerPools[workerIndex]; - context.Tree.GetSelfOverlaps2(ref *context.Results, pool, dispatcher, context.Stack, workerIndex, context.TargetTaskCount); - } - - static void IntertreeEntryTask(long taskStartAndEnd, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) - { - Debug.Assert(dispatcher.ManagedContext != null); - ref var context = ref *(IntertreeContext*)untypedContext; - var pool = dispatcher.WorkerPools[workerIndex]; - context.ActiveTree.GetOverlaps2(ref context.StaticTree, ref *context.Results, pool, dispatcher, context.Stack, workerIndex, context.TargetTaskCount); - } - - public static void WorkerTask(int workerIndex, IThreadDispatcher dispatcher) - { - Debug.Assert(dispatcher.ManagedContext != null); - var taskStack = (TaskStack*)dispatcher.UnmanagedContext; - PopTaskResult popTaskResult; - var waiter = new SpinWait(); - while ((popTaskResult = taskStack->TryPopAndRun(workerIndex, dispatcher)) != PopTaskResult.Stop) - { - waiter.SpinOnce(-1); - } - ((NarrowPhase)dispatcher.ManagedContext).overlapWorkers[workerIndex].Batcher.Flush(); - } - public void DispatchOverlapsNew(float dt, IThreadDispatcher threadDispatcher = null) - { - //The number of collisions is usually some constant multiple of the number of active leaves. - //5-10 collisions per active leaf would represent a very dense simulation. - //The number of collisions needed to warrant the existence of a thread varies, too, but it's exceptionally unlikely that any you would need more than one thread for 64 collisions. - //(Those would have to be some very messed up pairs!) - //So, we'll allow worker counts to scale down with low leaf counts to avoid overhead for tiny simulations. - var maximumWorkerCount = int.Min(int.Max(1, broadPhase.ActiveTree.LeafCount / 8), threadDispatcher == null ? 1 : threadDispatcher.ThreadCount); - if (maximumWorkerCount > 1) - { - narrowPhase.Prepare(dt, threadDispatcher); - - if (broadPhase.ActiveTree.LeafCount > 0 && broadPhase.StaticTree.LeafCount > 0) - { - var selfResults = new ThreadedSelfOverlapHandler { Leaves = broadPhase.ActiveLeaves }; - var intertreeResults = new ThreadedIntertreeOverlapHandler { ActiveLeaves = broadPhase.ActiveLeaves, StaticLeaves = broadPhase.StaticLeaves }; - - //Note that we shouldn't reduce the task budget for testing with smallish trees, because it's very possible for the cost of individual narrow phase pairs to be high. - //The tree test cost is often small compared to the narrow phase costs, so it's worth keeping quite a few tasks around. - var selfTestCostEstimate = broadPhase.ActiveTree.LeafCount; - var intertreeTestCostEstimate = int.Max(broadPhase.ActiveTree.LeafCount, broadPhase.StaticTree.LeafCount); - - var selfTestAsFraction = (float)selfTestCostEstimate / (selfTestCostEstimate + intertreeTestCostEstimate); - //Regularize the budgets a bit. Don't let either get too small. - const float minimumBudget = 0.25f; - var intertreeTestAsFraction = 1f - (selfTestAsFraction * (1f - minimumBudget) + minimumBudget); - selfTestAsFraction = 1f - intertreeTestAsFraction; - - var selfTestTaskTarget = (int)float.Round(maximumWorkerCount * selfTestAsFraction); - var intertreeTestTaskTarget = maximumWorkerCount - selfTestTaskTarget; - var taskStack = new TaskStack(broadPhase.Pool, threadDispatcher, maximumWorkerCount); - var selfContext = new SelfContext { Results = &selfResults, Stack = &taskStack, TargetTaskCount = selfTestTaskTarget, Tree = broadPhase.ActiveTree }; - var intertreeContext = new IntertreeContext { Results = &intertreeResults, Stack = &taskStack, TargetTaskCount = intertreeTestTaskTarget, ActiveTree = broadPhase.ActiveTree, StaticTree = broadPhase.StaticTree }; - Span tasks = stackalloc Task[2]; - tasks[0] = new Task(&SelfEntryTask, &selfContext); - tasks[1] = new Task(&IntertreeEntryTask, &intertreeContext); - taskStack.AllocateContinuationAndPush(tasks, 0, threadDispatcher, onComplete: TaskStack.GetRequestStopTask(&taskStack)); - threadDispatcher.DispatchWorkers(&WorkerTask, maximumWorkerCount: maximumWorkerCount, unmanagedContext: &taskStack, managedContext: narrowPhase); - taskStack.Dispose(broadPhase.Pool, threadDispatcher); - - //var intertreeResults = new ThreadedIntertreeOverlapHandler { ActiveLeaves = broadPhase.ActiveLeaves, StaticLeaves = broadPhase.StaticLeaves }; - //var taskStack = new TaskStack(broadPhase.Pool, threadDispatcher, maximumWorkerCount); - //var intertreeContext = new IntertreeContext { Results = &intertreeResults, Stack = &taskStack, TargetTaskCount = maximumWorkerCount, ActiveTree = broadPhase.ActiveTree, StaticTree = broadPhase.StaticTree }; - //taskStack.AllocateContinuationAndPush(new Task(&IntertreeEntryTask, &intertreeContext), 0, threadDispatcher, onComplete: TaskStack.GetRequestStopTask(&taskStack)); - //threadDispatcher.DispatchWorkers(&WorkerTask, maximumWorkerCount: maximumWorkerCount, unmanagedContext: &taskStack, managedContext: narrowPhase); - //taskStack.Dispose(broadPhase.Pool, threadDispatcher); - } - else if (broadPhase.ActiveTree.LeafCount > 0) - { - var selfResults = new ThreadedSelfOverlapHandler { Leaves = broadPhase.ActiveLeaves }; - var taskStack = new TaskStack(broadPhase.Pool, threadDispatcher, maximumWorkerCount); - var selfContext = new SelfContext { Results = &selfResults, Stack = &taskStack, TargetTaskCount = maximumWorkerCount, Tree = broadPhase.ActiveTree }; - taskStack.AllocateContinuationAndPush(new Task(&SelfEntryTask, &selfContext), 0, threadDispatcher, onComplete: TaskStack.GetRequestStopTask(&taskStack)); - threadDispatcher.DispatchWorkers(&WorkerTask, maximumWorkerCount: maximumWorkerCount, unmanagedContext: &taskStack, managedContext: narrowPhase); - taskStack.Dispose(broadPhase.Pool, threadDispatcher); - } -#if DEBUG - for (int i = 1; i < threadDispatcher.ThreadCount; ++i) - { - Debug.Assert(!narrowPhase.overlapWorkers[i].Batcher.batches.Allocated, "After execution, there should be no remaining allocated collision batchers."); - } -#endif - } - else - { - narrowPhase.Prepare(dt); - var selfTestHandler = new SelfOverlapHandler(broadPhase.ActiveLeaves, narrowPhase, 0); - broadPhase.ActiveTree.GetSelfOverlaps2(ref selfTestHandler); - var intertreeHandler = new IntertreeOverlapHandler(broadPhase.ActiveLeaves, broadPhase.StaticLeaves, narrowPhase, 0); - broadPhase.ActiveTree.GetOverlaps(ref broadPhase.StaticTree, ref intertreeHandler); - narrowPhase.overlapWorkers[0].Batcher.Flush(); - - } - - } - } } diff --git a/BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs b/BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs deleted file mode 100644 index 4b9e25c4b..000000000 --- a/BepuPhysics/Trees/Tree_IntertreeQueriesMT2.cs +++ /dev/null @@ -1,213 +0,0 @@ -using BepuUtilities; -using BepuUtilities.Memory; -using BepuUtilities.TaskScheduling; -using System; -using System.Diagnostics; -using System.Runtime.CompilerServices; - -namespace BepuPhysics.Trees; - -partial struct Tree -{ - unsafe struct IntertreeContext where TOverlapHandler : unmanaged, IThreadedOverlapHandler - { - public Tree TreeA; - public Tree TreeB; - public TaskStack* Stack; - public int LeafThreshold; - public TOverlapHandler* Results; - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - static unsafe ContinuationHandle PushIntertreeSubtasks(int pushCount, in Node a, in Node b, bool pushAA, bool pushAB, bool pushBA, bool pushBB, - void* untypedContext, in IntertreeContext context, int workerIndex, IThreadDispatcher dispatcher) where TOverlapHandler : unmanaged, IThreadedOverlapHandler - { - //Stackallocs persist for the duration of the function. Because the intertree test uses a lot of recursion, there's a lot of stack pressure. - //Given that IntertreeTask can call IntertreeTask, it can be pretty bad. - //To avoid that, we perform the stackalloc and push within this function. - Span tasks = stackalloc Task[pushCount]; - pushCount = 0; - if (pushAA) tasks[pushCount++] = new Task(&IntertreeTask, untypedContext, ((uint)a.A.Index | ((long)b.A.Index << 32))); - if (pushAB) tasks[pushCount++] = new Task(&IntertreeTask, untypedContext, ((uint)a.A.Index | ((long)b.B.Index << 32))); - if (pushBA) tasks[pushCount++] = new Task(&IntertreeTask, untypedContext, ((uint)a.B.Index | ((long)b.A.Index << 32))); - if (pushBB) tasks[pushCount++] = new Task(&IntertreeTask, untypedContext, ((uint)a.B.Index | ((long)b.B.Index << 32))); - return context.Stack->AllocateContinuationAndPush(tasks, workerIndex, dispatcher); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - static unsafe ContinuationHandle PushIntertreeSubtasksForNodeLeaf(int pushCount, in Node node, int indexA, int indexB, bool nodeBelongsToTreeA, bool pushA, bool pushB, - void* untypedContext, in IntertreeContext context, int workerIndex, IThreadDispatcher dispatcher) where TOverlapHandler : unmanaged, IThreadedOverlapHandler - { - //Not as heavy as node-node, but still enough to punt into a frame that gets popped. - Span tasks = stackalloc Task[pushCount]; - pushCount = 0; - if (pushA) tasks[pushCount++] = new Task(&IntertreeTask, untypedContext, (uint)(nodeBelongsToTreeA ? node.A.Index : indexA) | ((long)(nodeBelongsToTreeA ? indexB : node.A.Index) << 32)); - if (pushB) tasks[pushCount++] = new Task(&IntertreeTask, untypedContext, (uint)(nodeBelongsToTreeA ? node.B.Index : indexA) | ((long)(nodeBelongsToTreeA ? indexB : node.B.Index) << 32)); - return context.Stack->AllocateContinuationAndPush(tasks, workerIndex, dispatcher); - } - - static unsafe void IntertreeTask(long encodedIndices, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) where TOverlapHandler : unmanaged, IThreadedOverlapHandler - { - var indexA = (int)encodedIndices; - var indexB = (int)(encodedIndices >> 32); - ref var context = ref *(IntertreeContext*)untypedContext; - if (indexA >= 0 && indexB >= 0) - { - //Both nodes. - ref var a = ref context.TreeA.Nodes[indexA]; - ref var b = ref context.TreeB.Nodes[indexB]; - ref var aa = ref a.A; - ref var ab = ref a.B; - ref var ba = ref b.A; - ref var bb = ref b.B; - var aaIntersects = BoundingBox.IntersectsUnsafe(aa, ba); - var abIntersects = BoundingBox.IntersectsUnsafe(aa, bb); - var baIntersects = BoundingBox.IntersectsUnsafe(ab, ba); - var bbIntersects = BoundingBox.IntersectsUnsafe(ab, bb); - - //Push all pushable work *before* doing any work to ensure that other threads have something to grab. - var pushAA = aaIntersects && int.Max(aa.LeafCount, ba.LeafCount) >= context.LeafThreshold; - var pushAB = abIntersects && int.Max(aa.LeafCount, bb.LeafCount) >= context.LeafThreshold; - var pushBA = baIntersects && int.Max(ab.LeafCount, ba.LeafCount) >= context.LeafThreshold; - var pushBB = bbIntersects && int.Max(ab.LeafCount, bb.LeafCount) >= context.LeafThreshold; - var pushCount = (pushAA ? 1 : 0) + (pushAB ? 1 : 0) + (pushBA ? 1 : 0) + (pushBB ? 1 : 0); - var handle = pushCount == 0 ? default : PushIntertreeSubtasks(pushCount, a, b, pushAA, pushAB, pushBA, pushBB, untypedContext, context, workerIndex, dispatcher); - - var wrapped = new WrappedOverlapHandler { Inner = context.Results, WorkerIndex = workerIndex, ManagedContext = dispatcher.ManagedContext }; - if (aaIntersects && !pushAA) - { - context.TreeA.DispatchTestForNodes(ref aa, ref ba, ref context.TreeB, ref wrapped); - } - if (abIntersects && !pushAB) - { - context.TreeA.DispatchTestForNodes(ref aa, ref bb, ref context.TreeB, ref wrapped); - } - if (baIntersects && !pushBA) - { - context.TreeA.DispatchTestForNodes(ref ab, ref ba, ref context.TreeB, ref wrapped); - } - if (bbIntersects && !pushBB) - { - context.TreeA.DispatchTestForNodes(ref ab, ref bb, ref context.TreeB, ref wrapped); - } - if (pushCount > 0) - { - context.Stack->WaitForCompletion(handle, workerIndex, dispatcher); - } - } - else - { - //One of the two indices points at a leaf. - //In reasonably balanced circumstances, there's hardly any reason to bother spawning tasks for node-leaf, but we may be in a pathological case - //where one tree is much smaller in *leaf count*, but not in size, and so the node might actually have a ton of leaves beneath it. - Debug.Assert(indexA >= 0 || indexB >= 0, "A task should not have been spawned for a leaf-leaf test. Leaf threshold is likely messed up."); - var nodeBelongsToTreeA = indexA >= 0; - ref var node = ref nodeBelongsToTreeA ? ref context.TreeA.Nodes[indexA] : ref context.TreeB.Nodes[indexB]; - //Go looking for the leaf child by using the leaf buffer indirection since we didn't store the bounding box anywhere else. - //This hurts a bit- a whole extra bit of memory being touched- but this codepath *should* be pretty rare as a fraction of test time; the whole point is for it to hit a sequential path quickly. - ref var leafTree = ref nodeBelongsToTreeA ? ref context.TreeB : ref context.TreeA; - var leafIndex = nodeBelongsToTreeA ? Encode(indexB) : Encode(indexA); - var leaf = leafTree.Leaves[leafIndex]; - ref var leafChild = ref Unsafe.Add(ref leafTree.Nodes[leaf.NodeIndex].A, leaf.ChildIndex); - var aIntersects = BoundingBox.IntersectsUnsafe(leafChild, node.A); - var bIntersects = BoundingBox.IntersectsUnsafe(leafChild, node.B); - var pushA = aIntersects && node.A.LeafCount >= context.LeafThreshold; - var pushB = bIntersects && node.B.LeafCount >= context.LeafThreshold; - var pushCount = (pushA ? 1 : 0) + (pushB ? 1 : 0); - var handle = pushCount == 0 ? default : PushIntertreeSubtasksForNodeLeaf(pushCount, node, indexA, indexB, nodeBelongsToTreeA, pushA, pushB, untypedContext, context, workerIndex, dispatcher); - var wrapped = new WrappedOverlapHandler { Inner = context.Results, WorkerIndex = workerIndex, ManagedContext = dispatcher.ManagedContext }; - if (aIntersects && !pushA) - { - context.TreeA.DispatchTestForNodes(ref nodeBelongsToTreeA ? ref node.A : ref leafChild, ref nodeBelongsToTreeA ? ref leafChild : ref node.A, ref context.TreeB, ref wrapped); - } - if (bIntersects && !pushB) - { - context.TreeA.DispatchTestForNodes(ref nodeBelongsToTreeA ? ref node.B : ref leafChild, ref nodeBelongsToTreeA ? ref leafChild : ref node.B, ref context.TreeB, ref wrapped); - } - if (pushCount > 0) - { - context.Stack->WaitForCompletion(handle, workerIndex, dispatcher); - } - } - - } - - unsafe void GetOverlaps2(ref Tree treeB, ref TOverlapHandler results, - BufferPool pool, int workerIndex, TaskStack* taskStack, IThreadDispatcher threadDispatcher, bool internallyDispatch, int workerCount, int targetTaskBudget, object managedContext = null) where TOverlapHandler : unmanaged, IThreadedOverlapHandler - { - if (LeafCount == 0 || treeB.LeafCount == 0) - return; - var resultsCopy = results; - //Both trees have complete nodes; we can use a general case. - const int minimumLeafThreshold = 256; - if (LeafCount < minimumLeafThreshold && treeB.LeafCount < minimumLeafThreshold) - { - //No point in spawning a bunch of tasks for tiny trees. - var wrapped = new WrappedOverlapHandler { Inner = &resultsCopy, WorkerIndex = 0, ManagedContext = threadDispatcher.ManagedContext }; - GetOverlaps(ref treeB, ref wrapped); - } - else - { - if (targetTaskBudget < 0) - targetTaskBudget = workerCount; - var leafThreshold = int.Max(minimumLeafThreshold, (LeafCount + treeB.LeafCount) / (8 * targetTaskBudget)); - var context = new IntertreeContext - { - TreeA = this, - TreeB = treeB, - Stack = taskStack, - LeafThreshold = leafThreshold, - Results = &resultsCopy - }; - //One of the trees *may* be a single leaf. Don't want the task to have to deal with partial nodes, so go ahead and spawn a task for the leaf child in that case. Otherwise, just point at the root. - var childA = LeafCount == 1 ? Nodes[0].A.Index : 0; - var childB = treeB.LeafCount == 1 ? treeB.Nodes[0].A.Index : 0; - var rootTask = new Task(&IntertreeTask, &context, (uint)childA | ((long)childB << 32)); - if (internallyDispatch) - { - taskStack->AllocateContinuationAndPush(rootTask, workerIndex, threadDispatcher, onComplete: TaskStack.GetRequestStopTask(taskStack)); - TaskStack.DispatchWorkers(threadDispatcher, taskStack, int.Min(threadDispatcher.ThreadCount, targetTaskBudget), managedContext); - } - else - { - taskStack->RunTask(rootTask, workerIndex, threadDispatcher); - } - } - //Copy back potential changes. - results = resultsCopy; - } - - /// - /// Reports all bounding box overlaps between leaves in the two trees to the given . Uses the thread dispatcher to parallelize overlap testing. - /// - /// Other tree to test against. - /// Handler to report results to. - /// Pool used for ephemeral allocations. - /// Thread dispatcher used during the overlap testing. - /// Managed context to provide to the overlap handler, if any. - public unsafe void GetOverlaps2(ref Tree treeB, ref TOverlapHandler results, BufferPool pool, IThreadDispatcher dispatcher, object managedContext = null) - where TOverlapHandler : unmanaged, IThreadedOverlapHandler - { - var taskStack = new TaskStack(pool, dispatcher, dispatcher.ThreadCount); - GetOverlaps2(ref treeB, ref results, pool, 0, &taskStack, dispatcher, true, dispatcher.ThreadCount, -1, managedContext); - taskStack.Dispose(pool, dispatcher); - } - - /// - /// Reports all bounding box overlaps between leaves in the two trees to the given . - /// Pushes tasks into the provided . Does not dispatch threads internally; this is intended to be used as a part of a caller-managed dispatch. - /// - /// Other tree to test against. - /// Handler to report results to. - /// Pool used for ephemeral allocations. - /// Thread dispatcher used during the overlap test. - /// that the overlap test will push tasks onto as needed. - /// Index of the worker calling the function. - /// Number of tasks the overlap testing should try to create during execution. If negative, uses . - /// This does not dispatch workers on the directly. If the overlap handler requires managed context, that should be provided by whatever dispatched the workers. - public unsafe void GetOverlaps2(ref Tree treeB, ref TOverlapHandler results, BufferPool pool, IThreadDispatcher dispatcher, TaskStack* taskStack, int workerIndex, int targetTaskCount = -1) - where TOverlapHandler : unmanaged, IThreadedOverlapHandler - { - GetOverlaps2(ref treeB, ref results, pool, workerIndex, taskStack, dispatcher, false, dispatcher.ThreadCount, targetTaskCount); - } -} From c38a6d6f7eacd18cd3a166bcca24cc4ae0581c45 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 2 Sep 2023 18:21:04 -0500 Subject: [PATCH 818/947] Some test fiddling. --- .../BroadPhaseStressTestDemo.cs | 97 ++++++--- .../SpecializedTests/TreeFiddlingTestDemo.cs | 192 +++++++++++++++--- 2 files changed, 231 insertions(+), 58 deletions(-) diff --git a/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs b/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs index e8f7f6398..3bd0a212c 100644 --- a/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs +++ b/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs @@ -8,6 +8,8 @@ using System.Diagnostics; using DemoContentLoader; using BepuPhysics.CollisionDetection; +using BepuPhysics.Trees; +using static Demos.SpecializedTests.TreeFiddlingTestDemo; namespace Demos.SpecializedTests { @@ -63,10 +65,10 @@ public override void Initialize(ContentArchive content, Camera camera) var shape = new Sphere(0.5f); var sphereInertia = shape.ComputeInertia(1); var shapeIndex = Simulation.Shapes.Add(shape); - const int width = 128; - const int height = 128; - const int length = 128; - var spacing = new Vector3(1.01f); + const int width = 2048; + const int height = 2; + const int length = 2048; + var spacing = new Vector3(16.01f); float randomization = 0.9f; var randomizationSpan = (spacing - new Vector3(1)) * randomization; var randomizationBase = randomizationSpan * -0.5f; @@ -79,12 +81,16 @@ public override void Initialize(ContentArchive content, Camera camera) { var r = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); //var location = spacing * (new Vector3(i, j, k) + new Vector3(-width, 1, -length)) + randomizationBase + r * randomizationSpan; - var location = (r - new Vector3(0.5f)) * spacing * new Vector3(width, height, length); + var location = (r - new Vector3(0.5f)) * (r - new Vector3(0.5f)) * spacing * new Vector3(width, height, length); + //var location = (r - new Vector3(0.5f)) * spacing * new Vector3(width, height, length); //var hash = HashHelper.Rehash(HashHelper.Rehash(HashHelper.Rehash(i) + HashHelper.Rehash(j)) + HashHelper.Rehash(k)); var hash = i + j + k; - if (hash % 2 == 0) + if (hash % 64 == 0) { - Simulation.Bodies.Add(BodyDescription.CreateDynamic(location, sphereInertia, shapeIndex, -1)); + if (i == 7 && j == 1 && k == 0) + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(100, 0, 100), sphereInertia, Simulation.Shapes.Add(new Sphere(100)), -1)); + else + Simulation.Bodies.Add(BodyDescription.CreateDynamic(location, sphereInertia, shapeIndex, -1)); } else { @@ -95,61 +101,88 @@ public override void Initialize(ContentArchive content, Camera camera) } Console.WriteLine($"Body count: {Simulation.Bodies.ActiveSet.Count}"); Console.WriteLine($"Static count: {Simulation.Statics.Count}"); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -10, 0), Simulation.Shapes.Add(new Box(5000, 1, 5000)))); startingLocations = new Vector3[Simulation.Bodies.ActiveSet.Count]; for (int i = 0; i < startingLocations.Length; ++i) { startingLocations[i] = Simulation.Bodies.ActiveSet.DynamicsState[i].Motion.Pose.Position; } - refineTimes = new TimingsRingBuffer(sampleCount, BufferPool); + updateTimes = new TimingsRingBuffer(sampleCount, BufferPool); testTimes = new TimingsRingBuffer(sampleCount, BufferPool); + test2Times = new TimingsRingBuffer(sampleCount, BufferPool); + intertreeTest2Times = new TimingsRingBuffer(sampleCount, BufferPool); } const int sampleCount = 128; - TimingsRingBuffer refineTimes; + TimingsRingBuffer updateTimes; TimingsRingBuffer testTimes; + TimingsRingBuffer test2Times; + TimingsRingBuffer intertreeTest2Times; long frameCount; public override void Update(Window window, Camera camera, Input input, float dt) { var rotationAngle = frameCount * 1e-3f; var rotation = Matrix3x3.CreateFromAxisAngle(Vector3.UnitY, rotationAngle); - for (int i = 0; i < Simulation.Bodies.ActiveSet.Count / 2; ++i) - { - //For every body, set the velocity such that body moves toward some body-specific goal state that evolves over time. - Matrix3x3.Transform(startingLocations[i], rotation, out var targetLocation); - ref var motion = ref Simulation.Bodies.ActiveSet.DynamicsState[i].Motion; - var offset = targetLocation - motion.Pose.Position; - motion.Velocity.Linear = offset; - } + //for (int i = 0; i < Simulation.Bodies.ActiveSet.Count / 2; ++i) + //{ + // //For every body, set the velocity such that body moves toward some body-specific goal state that evolves over time. + // Matrix3x3.Transform(startingLocations[i], rotation, out var targetLocation); + // ref var motion = ref Simulation.Bodies.ActiveSet.DynamicsState[i].Motion; + // var offset = targetLocation - motion.Pose.Position; + // motion.Velocity.Linear = offset; + //} //Simulation.BroadPhase.ActiveTree.CacheOptimize(0); //Simulation.BroadPhase.StaticTree.CacheOptimize(0); base.Update(window, camera, input, dt); - refineTimes.Add(Simulation.Profiler[Simulation.BroadPhase]); + updateTimes.Add(Simulation.Profiler[Simulation.BroadPhase]); testTimes.Add(Simulation.Profiler[Simulation.BroadPhaseOverlapFinder]); + //var overlaps = new OverlapHandler(); + //Simulation.BroadPhase.ActiveTree.GetSelfOverlaps2(ref overlaps); + //var a = Stopwatch.GetTimestamp(); + //var threadedOverlaps = new TreeFiddlingTestDemo.ThreadedOverlapHandler(BufferPool, ThreadDispatcher.ThreadCount); + //Simulation.BroadPhase.ActiveTree.GetSelfOverlaps2(ref threadedOverlaps, BufferPool, ThreadDispatcher); + //var (selfOverlapCount, _) = threadedOverlaps.SumResults(); + //threadedOverlaps.Reset(); + //var b = Stopwatch.GetTimestamp(); + //Simulation.BroadPhase.ActiveTree.GetOverlaps2(ref Simulation.BroadPhase.ActiveTree, ref threadedOverlaps, BufferPool, ThreadDispatcher); + //var c = Stopwatch.GetTimestamp(); + //var interOverlaps = new OverlapHandler(); + ////Simulation.BroadPhase.ActiveTree.GetOverlaps(ref Simulation.BroadPhase.ActiveTree, ref interOverlaps); + //var (interOverlapCount, _) = threadedOverlaps.SumResults(); + //test2Times.Add((b - a) / (double)Stopwatch.Frequency); + //intertreeTest2Times.Add((c - b) / (double)Stopwatch.Frequency); + if (frameCount++ % sampleCount == 0) { - var refineStats = refineTimes.ComputeStats(); + var updateStats = updateTimes.ComputeStats(); var testStats = testTimes.ComputeStats(); - Console.WriteLine($"Refine: {refineStats.Average * 1000} ms average, {refineStats.StdDev * 1000} stddev"); + var test2Stats = test2Times.ComputeStats(); + var intertreeTest2Stats = intertreeTest2Times.ComputeStats(); + Console.WriteLine($"Update: {updateStats.Average * 1000} ms average, {updateStats.StdDev * 1000} stddev"); Console.WriteLine($"Test: {testStats.Average * 1000} ms average, {testStats.StdDev * 1000} stddev"); + //Console.WriteLine($"Test2: {test2Stats.Average * 1000} ms average, {test2Stats.StdDev * 1000} stddev"); + //Console.WriteLine($"Inter2: {intertreeTest2Stats.Average * 1000} ms average, {intertreeTest2Stats.StdDev * 1000} stddev"); Console.WriteLine($"Active Cost: {Simulation.BroadPhase.ActiveTree.MeasureCostMetric()}"); Console.WriteLine($"Static Cost: {Simulation.BroadPhase.StaticTree.MeasureCostMetric()}"); - Console.WriteLine($"Active CQ: {Simulation.BroadPhase.ActiveTree.MeasureCacheQuality()}"); - Console.WriteLine($"Static CQ: {Simulation.BroadPhase.StaticTree.MeasureCacheQuality()}"); - - var a = Stopwatch.GetTimestamp(); - Simulation.BroadPhase.ActiveTree.Refit2(); - var b = Stopwatch.GetTimestamp(); - Simulation.BroadPhase.StaticTree.Refit2(); - var c = Stopwatch.GetTimestamp(); - - - Console.WriteLine($"Manual ST refit active: {1e3 * (b - a) / Stopwatch.Frequency} ms"); - Console.WriteLine($"Manual ST refit static: {1e3 * (c - b) / Stopwatch.Frequency} ms"); + //Console.WriteLine($"Active CQ: {Simulation.BroadPhase.ActiveTree.MeasureCacheQuality()}"); + //Console.WriteLine($"Static CQ: {Simulation.BroadPhase.StaticTree.MeasureCacheQuality()}"); + + //var min = int.MaxValue; + //var max = 0; + //for (int i = 0; i < threadedOverlaps.Workers.Length; ++i) + //{ + // var count = threadedOverlaps.Workers[i].OverlapCount; + // //Console.Write($"{count}, "); + // min = int.Min(count, min); + // max = int.Max(count, max); + //} + //Console.WriteLine($"min, max: {min}, {max}"); } + //threadedOverlaps.Dispose(BufferPool); } } diff --git a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs index d2a7858d4..7f5649fb9 100644 --- a/Demos/SpecializedTests/TreeFiddlingTestDemo.cs +++ b/Demos/SpecializedTests/TreeFiddlingTestDemo.cs @@ -11,6 +11,7 @@ using BepuPhysics.Constraints; using BepuPhysics.Collidables; using BepuUtilities.TaskScheduling; +using System.Threading; namespace Demos.SpecializedTests; @@ -45,7 +46,7 @@ public override string ToString() return $"{A}, {B}"; } } - struct OverlapHandler : IOverlapHandler + public struct OverlapHandler : IOverlapHandler { public int OverlapCount; public int OverlapSum; @@ -64,6 +65,56 @@ public void Handle(int indexA, int indexB) } } + + public struct ThreadedOverlapHandler : IThreadedOverlapHandler + { + public struct Worker + { + public int OverlapCount; + public int OverlapSum; + } + public Buffer Workers; + + public ThreadedOverlapHandler(BufferPool pool, int workerCount) + { + Workers = new Buffer(workerCount, pool); + Workers.Clear(0, Workers.Length); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void Handle(int indexA, int indexB, int workerIndex, object managedContext) + { + ref var worker = ref Workers[workerIndex]; + worker.OverlapSum += indexA + indexB; + ++worker.OverlapCount; + } + + public void Reset() + { + for (int i = 0; i < Workers.Length; ++i) + { + Workers[i] = default; + } + } + public (int overlapCount, int overlapSum) SumResults() + { + int overlapCount = 0; + int overlapSum = 0; + for (int i = 0; i < Workers.Length; ++i) + { + ref var worker = ref Workers[i]; + overlapCount += worker.OverlapCount; + overlapSum += worker.OverlapSum; + } + return (overlapCount, overlapSum); + } + + public void Dispose(BufferPool pool) + { + Workers.Dispose(pool); + } + } + Buffer CreateDeformedPlaneTriangles(int width, int height, Vector3 scale) { Vector3 Deform(int x, int y) => new Vector3(x - width * scale.X * 0.5f, 2f * (float)(Math.Sin(x * 0.5f) * Math.Sin(y * 0.5f)), y - height * scale.Y * 0.5f); @@ -153,6 +204,33 @@ public override void Initialize(ContentArchive content, Camera camera) ThreadDispatcher.WorkerPools.Clear(); Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(4, 1)); + // @ @ @ @ @ @ DIRECT INSERTION TESTING @ @ @ @ @ @ + Random random = new Random(5); + for (int p = 0; p < 16; ++p) + { + var insertStart = Stopwatch.GetTimestamp(); + const int insertionCount = 1 << 22; + var tree = new Tree(BufferPool, insertionCount); + for (int k = 0; k < insertionCount; ++k) + { + //var position = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) * new Vector3(1000); + var position = new Vector3(k * 4, 0, 0); + var bounds = new BoundingBox { Min = position + new Vector3(-1), Max = position + new Vector3(1) }; + tree.Add(bounds, BufferPool); + //if (k % 128 == 0) + // tree.Validate(); + } + //tree.Validate(); + var insertEnd = Stopwatch.GetTimestamp(); + if (p > 0) + { + Console.WriteLine($"Total insertion time (ms): {(insertEnd - insertStart) * 1e3 / Stopwatch.Frequency}"); + Console.WriteLine($"Average time (ns): {(insertEnd - insertStart) * 1e9 / (insertionCount * Stopwatch.Frequency)}"); + Console.WriteLine($"SAH: {tree.MeasureCostMetric()}"); + } + tree.Dispose(BufferPool); + } + //Create a mesh. var width = 1024; var height = 1024; @@ -160,46 +238,108 @@ public override void Initialize(ContentArchive content, Camera camera) //DemoMeshHelper.CreateDeformedPlane(width, height, (x, y) => new Vector3(x - width * scale.X * 0.5f, 2f * (float)(Math.Sin(x * 0.5f) * Math.Sin(y * 0.5f)), y - height * scale.Y * 0.5f), scale, BufferPool, out var mesh); //DemoMeshHelper.CreateDeformedPlane(width, height, (x, y) => new Vector3(x - width * scale.X * 0.5f, 0, y - height * scale.Y * 0.5f), scale, BufferPool, out var mesh); + //var triangles = CreateDeformedPlaneTriangles(width, height, scale); var triangles = CreateRandomSoupTriangles(new BoundingBox(new(width / -2f, scale.Y * -2, height / -2f), new(width / 2f, scale.Y * 2, height / 2f)), (width - 1) * (height - 1) * 2, 0.5f, 100f); //var mesh = new Mesh(triangles, Vector3.One, BufferPool); var mesh = DemoMeshHelper.CreateGiantMeshFast(triangles, Vector3.One, BufferPool); - int refinementState = 0; + + // @ @ @ @ @ @ REFINEMENT TESTING @ @ @ @ @ @ + + //int refinementState = 0; + //long sum = 0; + //int cacheOptimizationStart = 0; + //for (int refinementIndex = 0; refinementIndex < 16384; ++refinementIndex) + //{ + // //mesh.Tree.CacheOptimizeLimitedSubtree(0, 4096); + // //mesh.Tree.CacheOptimize(0); + // //var optimizedCount = mesh.Tree.CacheOptimizeRegion(0, int.MaxValue); + // //int localOptimizationCount = 0; + // //const int targetOptimizationCount = 8192; + // //while (localOptimizationCount < targetOptimizationCount) + // //{ + // // var optimizedCount = mesh.Tree.CacheOptimizeRegion(cacheOptimizationStart, targetOptimizationCount); + // // localOptimizationCount += optimizedCount; + // // cacheOptimizationStart += optimizedCount; + // // if (cacheOptimizationStart >= mesh.Tree.NodeCount) + // // cacheOptimizationStart -= mesh.Tree.NodeCount; + // //} + // //for (int j = 0; j < mesh.Tree.NodeCount; ++j) + // //{ + // // ref var node = ref mesh.Tree.Nodes[j]; + // // if (node.A.Index >= 0) + // // { + // // node.A.Min = new Vector3(float.MaxValue); + // // node.A.Max = new Vector3(float.MinValue); + // // } + // // if (node.B.Index >= 0) + // // { + // // node.B.Min = new Vector3(float.MaxValue); + // // node.B.Max = new Vector3(float.MinValue); + // // } + // //} + + // mesh.Tree.Refine2(refinementIndex % 1 == 0 ? 65536 : 0, ref refinementState, 32, 1024, BufferPool); + // //mesh.Tree.Refine2(1024, ref refinementState, 1, 131072, BufferPool); + // //var useRoot = refinementIndex % 32 == 0; + // //var rootSize = 5800; + // //var subtreeSize = 5800; + // //var subtreeCount = 8; + // //var subtreeReductionOnRoot = (int)float.Round(rootSize / subtreeSize); + // //var effectiveRootSize = useRoot ? rootSize : 0; + // //var effectiveSubtreeCount = useRoot ? int.Max(0, subtreeCount - subtreeReductionOnRoot) : subtreeCount; + // //mesh.Tree.Refine2(effectiveRootSize, ref refinementState, effectiveSubtreeCount, subtreeSize, BufferPool, ThreadDispatcher); + // //mesh.Tree.Refine2(effectiveRootSize, ref refinementState, effectiveSubtreeCount, subtreeSize, BufferPool); + // var start = Stopwatch.GetTimestamp(); + // //mesh.Tree.Refit2WithCacheOptimization(BufferPool, ThreadDispatcher); + // //mesh.Tree.Refit2WithCacheOptimization(BufferPool); + // //mesh.Tree.Refit2(); + // //mesh.Tree.Refit2(BufferPool, ThreadDispatcher); + // var end = Stopwatch.GetTimestamp(); + // sum += end - start; + // if ((refinementIndex + 1) % 512 == 0) + // { + // mesh.Tree.Validate(); + // var cacheQuality = mesh.Tree.MeasureCacheQuality(); + // var costMetric = mesh.Tree.MeasureCostMetric(); + // Console.WriteLine($"cost, cache for {refinementIndex}: {costMetric}, {cacheQuality}"); + // //Console.WriteLine($"Time (average) (ms): {(end - start) * 1e3 / Stopwatch.Frequency}, {sum * 1e3 / ((refinementIndex + 1) * Stopwatch.Frequency)}"); + // } + //} + + + // @ @ @ @ @ @ SELF TEST TESTING @ @ @ @ @ @ + var handler = new OverlapHandler(); + var threadedHandler = new ThreadedOverlapHandler(BufferPool, ThreadDispatcher.ThreadCount); long sum = 0; - var taskStack = new TaskStack(BufferPool, ThreadDispatcher, ThreadDispatcher.ThreadCount); - for (int refinementIndex = 0; refinementIndex < 16384; ++refinementIndex) + long intervalSum = 0; + for (int testIndex = 0; testIndex < 16384; ++testIndex) { - //mesh.Tree.CacheOptimizeLimitedSubtree(0, 4096); - //mesh.Tree.CacheOptimize(0); - //var optimizedCount = mesh.Tree.CacheOptimizeRegion(0, int.MaxValue); - //int localOptimizationCount = 0; - //const int targetOptimizationCount = 8192; - //while (localOptimizationCount < targetOptimizationCount) - //{ - // var optimizedCount = mesh.Tree.CacheOptimizeRegion(cacheOptimizationStart, targetOptimizationCount); - // localOptimizationCount += optimizedCount; - // cacheOptimizationStart += optimizedCount; - // if (cacheOptimizationStart >= mesh.Tree.NodeCount) - // cacheOptimizationStart -= mesh.Tree.NodeCount; - //} - + handler.OverlapCount = 0; + threadedHandler.Reset(); var start = Stopwatch.GetTimestamp(); - //mesh.Tree.Refine2(8192, ref refinementState, 0, 8192, BufferPool); - //mesh.Tree.Refine2(1024, ref refinementState, 1, 131072, BufferPool); - mesh.Tree.Refine2(8192, ref refinementState, 16, 2048, BufferPool, ThreadDispatcher); + //mesh.Tree.GetSelfOverlaps(ref handler); + //mesh.Tree.GetSelfOverlapsBatched(ref handler, BufferPool); + //mesh.Tree.GetSelfOverlaps2(ref handler); + mesh.Tree.GetSelfOverlaps2(ref threadedHandler, BufferPool, ThreadDispatcher); var end = Stopwatch.GetTimestamp(); + var (overlapCount, overlapSum) = threadedHandler.SumResults(); + sum += end - start; - if ((refinementIndex + 1) % 128 == 0) + intervalSum += end - start; + const int intervalSize = 128; + if ((testIndex + 1) % intervalSize == 0) { mesh.Tree.Validate(); - var cacheQuality = mesh.Tree.MeasureCacheQuality(); var costMetric = mesh.Tree.MeasureCostMetric(); - Console.WriteLine($"cost, cache for {refinementIndex}: {costMetric}, {cacheQuality}"); - Console.WriteLine($"Time (average) (ms): {(end - start) * 1e3 / Stopwatch.Frequency}, {sum * 1e3 / ((refinementIndex + 1) * Stopwatch.Frequency)}"); + Console.WriteLine($"cost for {testIndex}: {costMetric}"); + Console.WriteLine($"{testIndex}: Time (interval average) (average) (ms): {(end - start) * 1e3 / Stopwatch.Frequency}, {intervalSum * 1e3 / (intervalSize * Stopwatch.Frequency)}, {sum * 1e3 / ((testIndex + 1) * Stopwatch.Frequency)}"); + intervalSum = 0; } } - taskStack.Dispose(BufferPool, ThreadDispatcher); + threadedHandler.Dispose(BufferPool); + Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); Console.WriteLine($"node count: {mesh.Tree.NodeCount}"); From db9cb3db2c0c1889372bc19597c8b8a1e011bed0 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 2 Sep 2023 18:21:27 -0500 Subject: [PATCH 819/947] DemoSet normalized. --- Demos/DemoSet.cs | 5 ----- 1 file changed, 5 deletions(-) diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index 17cc3a2a3..2c592019a 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -43,11 +43,6 @@ struct Option public DemoSet() { - //AddOption(); - //AddOption(); - //AddOption(); - //AddOption(); - //AddOption(); AddOption(); AddOption(); AddOption(); From d6c9b5c18f0ee041e14e08f1b4d2c615e44ef45e Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 3 Sep 2023 19:42:41 -0500 Subject: [PATCH 820/947] BREAKING CHANGES: IContactManifold has a less stinky API. Includes a convenience indexer, too. --- .../CollisionDetection/ContactManifold.cs | 353 +++++++++++------- .../CollisionDetection/NonconvexReduction.cs | 2 +- .../CollisionBatcherTaskBenchmarks.cs | 2 +- Demos/Demos/CollisionQueryDemo.cs | 5 +- Demos/Demos/CollisionTrackingDemo.cs | 8 +- Demos/Demos/ContactEventsDemo.cs | 21 +- Demos/Demos/Tanks/TankCallbacks.cs | 2 +- 7 files changed, 240 insertions(+), 153 deletions(-) diff --git a/BepuPhysics/CollisionDetection/ContactManifold.cs b/BepuPhysics/CollisionDetection/ContactManifold.cs index 53dcd8090..6a58b2ed2 100644 --- a/BepuPhysics/CollisionDetection/ContactManifold.cs +++ b/BepuPhysics/CollisionDetection/ContactManifold.cs @@ -1,4 +1,5 @@ -using System.Diagnostics; +using System; +using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; using System.Runtime.InteropServices; @@ -6,11 +7,11 @@ namespace BepuPhysics.CollisionDetection { /// - /// Information about a single contact in a nonconvex collidable pair. - /// Nonconvex pairs can have different surface bases at each contact point, since the contact surface is not guaranteed to be a plane. + /// Information about a single contact. /// + /// This type contains a field for the normal; it can be used to represent contacts within nonconvex contact manifolds or convex manifolds. [StructLayout(LayoutKind.Explicit, Size = 32)] - public struct NonconvexContact + public struct Contact { /// /// Offset from the position of collidable A to the contact position. @@ -71,14 +72,44 @@ public interface IContactManifold where TManifold : struct, IContactM bool Convex { get; } /// - /// Retrieves the feature id associated with a requested contact. + /// Gets or sets the contact at the given index in the manifold. + /// + /// Index of the contact to get or set. + /// Contact at the specified index. + /// Note that contact normals are shared across a . Setting one contact in a convex manifold will change the entire convex manifold's normal. + public Contact this[int contactIndex] { get; set; } + + /// + /// Gets the feature id associated with a requested contact. /// /// Index of the contact to grab the feature id of. /// Feature id of the requested contact. int GetFeatureId(int contactIndex); /// - /// Retrieves a copy of a contact's data. + /// Gets the depth associated with a requested contact. + /// + /// Index of the contact to grab the depth of. + /// Depth of the requested contact. + float GetDepth(int contactIndex); + + /// + /// Gets a contact's normal. + /// + /// Index of the contact to grab the normal of. + /// Normal of the requested contact. + /// Points from collidable B to collidable A. In convex manifolds, all contacts share a normal and will return the same value. + Vector3 GetNormal(int contactIndex); + + /// + /// Gets the offset from collidable A to the requested contact. + /// + /// Index of the contact to grab the offset of. + /// Offset to a contact's offset. + Vector3 GetOffset(int contactIndex); + + /// + /// Gets a copy of a contact's data. /// /// Index of the contact to copy data from. /// Offset from the first collidable's position to the contact position. @@ -88,39 +119,62 @@ public interface IContactManifold where TManifold : struct, IContactM /// Feature ids represent which parts of the collidables formed the contact and can be used to track unique contacts across frames. void GetContact(int contactIndex, out Vector3 offset, out Vector3 normal, out float depth, out int featureId); - //Can't return refs to the this instance, but it's convenient to have ref returns for parameters and interfaces can't require static functions, so... /// - /// Pulls a reference to a contact's depth. + /// Gets a copy of a contact's data. + /// + /// Index of the contact to copy data from. + /// Data associated with the contact. + void GetContact(int contactIndex, out Contact contactData); + + /// + /// Gets a reference to a contact's depth. /// /// Manifold to pull a reference from. /// Contact to pull data from. /// Reference to a contact's depth. - ref float GetDepth(ref TManifold manifold, int contactIndex); + static abstract ref float GetDepthReference(ref TManifold manifold, int contactIndex); /// - /// Pulls a reference to a contact's normal. Points from collidable B to collidable A. For convex manifolds that share a normal, all contact indices will simply return a reference to the manifold-wide normal. + /// Gets a reference to a contact's normal. Points from collidable B to collidable A. For convex manifolds that share a normal, all contact indices will simply return a reference to the manifold-wide normal. /// /// Manifold to pull a reference from. /// Contact to pull data from. /// Reference to a contact's normal (or the manifold-wide normal in a convex manifold). - ref Vector3 GetNormal(ref TManifold manifold, int contactIndex); + static abstract ref Vector3 GetNormalReference(ref TManifold manifold, int contactIndex); /// - /// Pulls a reference to a contact's offset. + /// Gets a reference to the offset from collidable A to the requested contact. /// /// Manifold to pull a reference from. /// Contact to pull data from. /// Reference to a contact's offset. - ref Vector3 GetOffset(ref TManifold manifold, int contactIndex); + static abstract ref Vector3 GetOffsetReference(ref TManifold manifold, int contactIndex); /// - /// Pulls a reference to a contact's feature id. + /// Gets a reference to a contact's feature id. /// /// Manifold to pull a reference from. /// Contact to pull data from. /// Reference to a contact's feature id. - ref int GetFeatureId(ref TManifold manifold, int contactIndex); + static abstract ref int GetFeatureIdReference(ref TManifold manifold, int contactIndex); + + /// + /// Gets a reference to a nonconvex manifold's contact. + /// + /// Manifold to pull a reference from. + /// Contact to pull data from. + /// Reference to the requested contact. + /// This is a helper that avoids manual casting. If the manifold is not a , the function will throw an . + static abstract ref Contact GetNonconvexContactReference(ref TManifold manifold, int contactIndex); + /// + /// Gets a reference to a convex manifold's contact. + /// + /// Manifold to pull a reference from. + /// Contact to pull data from. + /// Reference to the requested contact. + /// This is a helper that avoids manual casting. If the manifold is not a , the function will throw an . + static abstract ref ConvexContact GetConvexContactReference(ref TManifold manifold, int contactIndex); } //TODO: We could use specialized storage types for things like continuations if L2 can't actually hold it all. Seems unlikely, but it's not that hard if required. @@ -140,18 +194,32 @@ public unsafe struct NonconvexContactManifold : IContactManifold.Count => Count; readonly bool IContactManifold.Convex => false; + public Contact this[int contactIndex] + { + get + { + ValidateIndex(contactIndex); + return Unsafe.Add(ref Contact0, contactIndex); + } + set + { + ValidateIndex(contactIndex); + Unsafe.Add(ref Contact0, contactIndex) = value; + } + } + /// /// The maximum number of contacts that can exist within a nonconvex manifold. /// @@ -163,16 +231,6 @@ private readonly void ValidateIndex(int contactIndex) Debug.Assert(contactIndex >= 0 && contactIndex < Count, "Contact index must be within the contact count."); } - /// - /// Retrieves a copy of a contact's data. - /// - /// Index of the contact to copy data from. - /// Offset from the first collidable's position to the contact position. - /// Normal of the contact surface at the requested contact. Points from collidable B to collidable A. - /// Penetration depth at the requested contact. - /// Feature id of the requested contact. - /// Feature ids represent which parts of the collidables formed the contact and can be used to track unique contacts across frames. - [MethodImpl(MethodImplOptions.AggressiveInlining)] public void GetContact(int contactIndex, out Vector3 offset, out Vector3 normal, out float depth, out int featureId) { ValidateIndex(contactIndex); @@ -182,19 +240,74 @@ public void GetContact(int contactIndex, out Vector3 offset, out Vector3 normal, depth = contact.Depth; featureId = contact.FeatureId; } - /// - /// Retrieves the feature id associated with a requested contact. - /// - /// Index of the contact to grab the feature id of. - /// Feature id of the requested contact. - [MethodImpl(MethodImplOptions.AggressiveInlining)] + + public void GetContact(int contactIndex, out Contact contactData) + { + ValidateIndex(contactIndex); + contactData = Unsafe.Add(ref Contact0, contactIndex); + } + + + public float GetDepth(int contactIndex) + { + ValidateIndex(contactIndex); + return Unsafe.Add(ref Contact0, contactIndex).Depth; + } + + public Vector3 GetNormal(int contactIndex) + { + ValidateIndex(contactIndex); + return Unsafe.Add(ref Contact0, contactIndex).Normal; + } + + public Vector3 GetOffset(int contactIndex) + { + ValidateIndex(contactIndex); + return Unsafe.Add(ref Contact0, contactIndex).Offset; + } public int GetFeatureId(int contactIndex) { ValidateIndex(contactIndex); return Unsafe.Add(ref Contact0, contactIndex).FeatureId; } - [MethodImpl(MethodImplOptions.AggressiveInlining)] + + public static ref float GetDepthReference(ref NonconvexContactManifold manifold, int contactIndex) + { + manifold.ValidateIndex(contactIndex); + return ref Unsafe.Add(ref manifold.Contact0, contactIndex).Depth; + } + + public static ref Vector3 GetNormalReference(ref NonconvexContactManifold manifold, int contactIndex) + { + manifold.ValidateIndex(contactIndex); + return ref Unsafe.Add(ref manifold.Contact0, contactIndex).Normal; + } + + public static ref Vector3 GetOffsetReference(ref NonconvexContactManifold manifold, int contactIndex) + { + manifold.ValidateIndex(contactIndex); + return ref Unsafe.Add(ref manifold.Contact0, contactIndex).Offset; + } + + public static ref int GetFeatureIdReference(ref NonconvexContactManifold manifold, int contactIndex) + { + manifold.ValidateIndex(contactIndex); + return ref Unsafe.Add(ref manifold.Contact0, contactIndex).FeatureId; + } + + public static ref Contact GetNonconvexContactReference(ref NonconvexContactManifold manifold, int contactIndex) + { + manifold.ValidateIndex(contactIndex); + return ref Unsafe.Add(ref manifold.Contact0, contactIndex); + } + + public static ref ConvexContact GetConvexContactReference(ref NonconvexContactManifold manifold, int contactIndex) + { + throw new NotSupportedException("This is a NonconvexContactManifold; use GetNonconvexContactReference instead."); + } + + public static void FastRemoveAt(NonconvexContactManifold* manifold, int index) { --manifold->Count; @@ -205,7 +318,6 @@ public static void FastRemoveAt(NonconvexContactManifold* manifold, int index) } } - [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Add(NonconvexContactManifold* manifold, ref Vector3 normal, ref ConvexContact convexContact) { Debug.Assert(manifold->Count < MaximumContactCount); @@ -215,61 +327,11 @@ public static void Add(NonconvexContactManifold* manifold, ref Vector3 normal, r targetContact.Normal = normal; targetContact.FeatureId = convexContact.FeatureId; } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static ref NonconvexContact Allocate(NonconvexContactManifold* manifold) + public static ref Contact Allocate(NonconvexContactManifold* manifold) { Debug.Assert(manifold->Count < MaximumContactCount); return ref (&manifold->Contact0)[manifold->Count++]; } - - /// - /// Pulls a reference to a contact's depth. - /// - /// Manifold to pull a reference from. - /// Contact to pull data from. - /// Reference to a contact's depth. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref float GetDepth(ref NonconvexContactManifold manifold, int contactIndex) - { - return ref Unsafe.Add(ref manifold.Contact0, contactIndex).Depth; - } - - /// - /// Pulls a reference to a contact's normal. Points from collidable B to collidable A. - /// - /// Manifold to pull a reference from. - /// Contact to pull data from. - /// Reference to a contact's normal. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3 GetNormal(ref NonconvexContactManifold manifold, int contactIndex) - { - return ref Unsafe.Add(ref manifold.Contact0, contactIndex).Normal; - } - - - /// - /// Pulls a reference to a contact's offset. - /// - /// Manifold to pull a reference from. - /// Contact to pull data from. - /// Reference to a contact's offset. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3 GetOffset(ref NonconvexContactManifold manifold, int contactIndex) - { - return ref Unsafe.Add(ref manifold.Contact0, contactIndex).Offset; - } - - /// - /// Pulls a reference to a contact's feature id. - /// - /// Manifold to pull a reference from. - /// Contact to pull data from. - /// Reference to a contact's feature id. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref int GetFeatureId(ref NonconvexContactManifold manifold, int contactIndex) - { - return ref Unsafe.Add(ref manifold.Contact0, contactIndex).FeatureId; - } } /// @@ -305,22 +367,28 @@ public struct ConvexContactManifold : IContactManifold readonly bool IContactManifold.Convex => true; - [Conditional("DEBUG")] - private void ValidateIndex(int contactIndex) + public Contact this[int contactIndex] { - Debug.Assert(contactIndex >= 0 && contactIndex < Count, "Contact index must be within the contact count."); + get + { + GetContact(contactIndex, out var contact); + return contact; + } + set + { + ValidateIndex(contactIndex); + ref var target = ref Unsafe.Add(ref Contact0, contactIndex); + target.Offset = value.Offset; + Normal = value.Normal; + target.Depth = value.Depth; + target.FeatureId = value.FeatureId; + } } - /// - /// Retrieves the feature id associated with a requested contact. - /// - /// Index of the contact to grab the feature id of. - /// Feature id of the requested contact. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public int GetFeatureId(int contactIndex) + [Conditional("DEBUG")] + private void ValidateIndex(int contactIndex) { - ValidateIndex(contactIndex); - return Unsafe.Add(ref Contact0, contactIndex).FeatureId; + Debug.Assert(contactIndex >= 0 && contactIndex < Count, "Contact index must be within the contact count."); } /// @@ -332,7 +400,6 @@ public int GetFeatureId(int contactIndex) /// Penetration depth at the requested contact. /// Feature id of the requested contact. /// Feature ids represent which parts of the collidables formed the contact and can be used to track unique contacts across frames. - [MethodImpl(MethodImplOptions.AggressiveInlining)] public void GetContact(int contactIndex, out Vector3 offset, out Vector3 normal, out float depth, out int featureId) { ValidateIndex(contactIndex); @@ -342,8 +409,38 @@ public void GetContact(int contactIndex, out Vector3 offset, out Vector3 normal, depth = contact.Depth; featureId = contact.FeatureId; } + public void GetContact(int contactIndex, out Contact contactData) + { + ValidateIndex(contactIndex); + ref var contact = ref Unsafe.Add(ref Contact0, contactIndex); + contactData.Offset = contact.Offset; + contactData.Normal = Normal; + contactData.Depth = contact.Depth; + contactData.FeatureId = contact.FeatureId; + + } + public float GetDepth(int contactIndex) + { + ValidateIndex(contactIndex); + return Unsafe.Add(ref Contact0, contactIndex).Depth; + } + + public Vector3 GetNormal(int contactIndex) + { + ValidateIndex(contactIndex); + return Normal; + } - [MethodImpl(MethodImplOptions.AggressiveInlining)] + public Vector3 GetOffset(int contactIndex) + { + ValidateIndex(contactIndex); + return Unsafe.Add(ref Contact0, contactIndex).Offset; + } + public int GetFeatureId(int contactIndex) + { + ValidateIndex(contactIndex); + return Unsafe.Add(ref Contact0, contactIndex).FeatureId; + } public static void FastRemoveAt(ref ConvexContactManifold manifold, int index) { --manifold.Count; @@ -353,54 +450,40 @@ public static void FastRemoveAt(ref ConvexContactManifold manifold, int index) } } - /// - /// Pulls a reference to a contact's depth. - /// - /// Manifold to pull a reference from. - /// Contact to pull data from. - /// Reference to a contact's depth. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref float GetDepth(ref ConvexContactManifold manifold, int contactIndex) + public static ref float GetDepthReference(ref ConvexContactManifold manifold, int contactIndex) { + manifold.ValidateIndex(contactIndex); return ref Unsafe.Add(ref manifold.Contact0, contactIndex).Depth; } - /// - /// Pulls a reference to a contact manifold's normal. Points from collidable B to collidable A. Convex manifolds share a single normal across all contacts. - /// - /// Manifold to pull a reference from. - /// Contact to pull data from. - /// Reference to the contact manifold's normal. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3 GetNormal(ref ConvexContactManifold manifold, int contactIndex) + public static ref Vector3 GetNormalReference(ref ConvexContactManifold manifold, int contactIndex) { + manifold.ValidateIndex(contactIndex); return ref manifold.Normal; } - - /// - /// Pulls a reference to a contact's offset. - /// - /// Manifold to pull a reference from. - /// Contact to pull data from. - /// Reference to a contact's offset. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref Vector3 GetOffset(ref ConvexContactManifold manifold, int contactIndex) + public static ref Vector3 GetOffsetReference(ref ConvexContactManifold manifold, int contactIndex) { + manifold.ValidateIndex(contactIndex); return ref Unsafe.Add(ref manifold.Contact0, contactIndex).Offset; } - /// - /// Pulls a reference to a contact's feature id. - /// - /// Manifold to pull a reference from. - /// Contact to pull data from. - /// Reference to a contact's feature id. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref int GetFeatureId(ref ConvexContactManifold manifold, int contactIndex) + public static ref int GetFeatureIdReference(ref ConvexContactManifold manifold, int contactIndex) { + manifold.ValidateIndex(contactIndex); return ref Unsafe.Add(ref manifold.Contact0, contactIndex).FeatureId; } + + public static ref Contact GetNonconvexContactReference(ref ConvexContactManifold manifold, int contactIndex) + { + throw new NotImplementedException(); + } + + public static ref ConvexContact GetConvexContactReference(ref ConvexContactManifold manifold, int contactIndex) + { + manifold.ValidateIndex(contactIndex); + return ref Unsafe.Add(ref manifold.Contact0, contactIndex); + } } } \ No newline at end of file diff --git a/BepuPhysics/CollisionDetection/NonconvexReduction.cs b/BepuPhysics/CollisionDetection/NonconvexReduction.cs index 3efcca549..782f50258 100644 --- a/BepuPhysics/CollisionDetection/NonconvexReduction.cs +++ b/BepuPhysics/CollisionDetection/NonconvexReduction.cs @@ -62,7 +62,7 @@ unsafe static void UseContact(ref QuickList remainingCandida } [MethodImpl(MethodImplOptions.AggressiveInlining)] - static float ComputeDistinctiveness(in ConvexContact candidate, Vector3 contactNormal, in NonconvexContact reducedContact, float distanceSquaredInterpolationMin, float inverseDistanceSquaredInterpolationSpan, float depthScale) + static float ComputeDistinctiveness(in ConvexContact candidate, Vector3 contactNormal, in Contact reducedContact, float distanceSquaredInterpolationMin, float inverseDistanceSquaredInterpolationSpan, float depthScale) { //The more distant a contact is from another contact, or the more different its normal is, the more distinct it is considered. //The goal is for distinctiveness to range from around 0 to 2. The exact values aren't extremely important- we just want a rough range diff --git a/DemoBenchmarks/CollisionBatcherTaskBenchmarks.cs b/DemoBenchmarks/CollisionBatcherTaskBenchmarks.cs index a02563836..903a2ab32 100644 --- a/DemoBenchmarks/CollisionBatcherTaskBenchmarks.cs +++ b/DemoBenchmarks/CollisionBatcherTaskBenchmarks.cs @@ -133,7 +133,7 @@ public void OnPairCompleted(int pairId, ref TManifold manifold) where var count = manifold.Count; for (int i = 0; i < count; ++i) { - ResultSum += manifold.GetNormal(ref manifold, i) + manifold.GetOffset(ref manifold, i) + new Vector3(manifold.GetDepth(ref manifold, i)); + ResultSum += TManifold.GetNormalReference(ref manifold, i) + TManifold.GetOffsetReference(ref manifold, i) + new Vector3(TManifold.GetDepthReference(ref manifold, i)); } } } diff --git a/Demos/Demos/CollisionQueryDemo.cs b/Demos/Demos/CollisionQueryDemo.cs index 4a05873e4..d8ee0d7ed 100644 --- a/Demos/Demos/CollisionQueryDemo.cs +++ b/Demos/Demos/CollisionQueryDemo.cs @@ -80,10 +80,7 @@ public void OnPairCompleted(int pairId, ref TManifold manifold) where //And if you find yourself wanting contact data, well, you've got it handy!) for (int i = 0; i < manifold.Count; ++i) { - //This probably looks a bit odd, but it addresses a limitation of returning references to the struct 'this' instance. - //(What we really want here is either the lifting of that restriction, or allowing interfaces to require a static member so that we could call the static function and pass the instance, - //instead of invoking the function on the instance AND passing the instance.) - if (manifold.GetDepth(ref manifold, i) >= 0) + if (manifold.GetDepth(i) >= 0) { QueryWasTouched[pairId] = true; break; diff --git a/Demos/Demos/CollisionTrackingDemo.cs b/Demos/Demos/CollisionTrackingDemo.cs index b649d9f93..b99b4a5fb 100644 --- a/Demos/Demos/CollisionTrackingDemo.cs +++ b/Demos/Demos/CollisionTrackingDemo.cs @@ -470,7 +470,7 @@ bool PreviousContainsTouchingFeatureId(ref PairCollision pair, int featureId) { for (int i = 0; i < pair.Contacts.Count; ++i) { - if (pair.Contacts.GetFeatureId(i) == featureId && pair.Contacts.GetDepth(ref pair.Contacts, i) >= 0) + if (pair.Contacts.GetFeatureId(i) == featureId && pair.Contacts.GetDepth(i) >= 0) return true; } return false; @@ -500,11 +500,11 @@ public override void Update(Window window, Camera camera, Input input, float dt) ref var previous = ref collisions.PreviousPairs.Values[otherIndexInPrevious]; for (int i = 0; i < pair.Contacts.Count; ++i) { - if (pair.Contacts.GetDepth(ref pair.Contacts, i) >= 0) + if (pair.Contacts.GetDepth(i) >= 0) { //This contact is touching. Does there exist a contact with the same feature id that was touching in the previous timestep? if (!PreviousContainsTouchingFeatureId(ref previous, pair.Contacts.GetFeatureId(i))) - AddParticle(pair.Contacts.GetOffset(ref pair.Contacts, i), pair.Contacts.GetNormal(ref pair.Contacts, i), pair.OtherIsAInPair ? other.Collidable : self.Collidable); + AddParticle(pair.Contacts.GetOffset(i), pair.Contacts.GetNormal(i), pair.OtherIsAInPair ? other.Collidable : self.Collidable); } } } @@ -513,7 +513,7 @@ public override void Update(Window window, Camera camera, Input input, float dt) //No previous collision, so all contacts are new. for (int i = 0; i < pair.Contacts.Count; ++i) { - AddParticle(pair.Contacts.GetOffset(ref pair.Contacts, i), pair.Contacts.GetNormal(ref pair.Contacts, i), pair.OtherIsAInPair ? other.Collidable : self.Collidable); + AddParticle(pair.Contacts.GetOffset(i), pair.Contacts.GetNormal(i), pair.OtherIsAInPair ? other.Collidable : self.Collidable); } } } diff --git a/Demos/Demos/ContactEventsDemo.cs b/Demos/Demos/ContactEventsDemo.cs index ea15ec719..aa286aa52 100644 --- a/Demos/Demos/ContactEventsDemo.cs +++ b/Demos/Demos/ContactEventsDemo.cs @@ -439,7 +439,7 @@ void HandleManifoldForCollidable(int workerIndex, CollidableReference manifold.GetContact(contactIndex, out var offset, out var normal, out var depth, out _); listener.Handler.OnContactAdded(source, pair, ref manifold, offset, normal, depth, featureId, contactIndex, workerIndex); } - if (manifold.GetDepth(ref manifold, contactIndex) >= 0) + if (manifold.GetDepth(contactIndex) >= 0) isTouching = true; } if (previousContactsStillExist != (1 << collision.ContactCount) - 1) @@ -512,12 +512,19 @@ struct EmptyManifold : IContactManifold public int Count => 0; public bool Convex => true; //This type never has any contacts, so there's no need for any property grabbers. - public void GetContact(int contactIndex, out Vector3 offset, out Vector3 normal, out float depth, out int featureId) { throw new NotImplementedException(); } - public ref float GetDepth(ref EmptyManifold manifold, int contactIndex) { throw new NotImplementedException(); } - public int GetFeatureId(int contactIndex) { throw new NotImplementedException(); } - public ref int GetFeatureId(ref EmptyManifold manifold, int contactIndex) { throw new NotImplementedException(); } - public ref Vector3 GetNormal(ref EmptyManifold manifold, int contactIndex) { throw new NotImplementedException(); } - public ref Vector3 GetOffset(ref EmptyManifold manifold, int contactIndex) { throw new NotImplementedException(); } + public Contact this[int contactIndex] { get => throw new NotImplementedException(); set => throw new NotImplementedException(); } + public static ref ConvexContact GetConvexContactReference(ref EmptyManifold manifold, int contactIndex) => throw new NotImplementedException(); + public static ref float GetDepthReference(ref EmptyManifold manifold, int contactIndex) => throw new NotImplementedException(); + public static ref int GetFeatureIdReference(ref EmptyManifold manifold, int contactIndex) => throw new NotImplementedException(); + public static ref Contact GetNonconvexContactReference(ref EmptyManifold manifold, int contactIndex) => throw new NotImplementedException(); + public static ref Vector3 GetNormalReference(ref EmptyManifold manifold, int contactIndex) => throw new NotImplementedException(); + public static ref Vector3 GetOffsetReference(ref EmptyManifold manifold, int contactIndex) => throw new NotImplementedException(); + public void GetContact(int contactIndex, out Vector3 offset, out Vector3 normal, out float depth, out int featureId) => throw new NotImplementedException(); + public void GetContact(int contactIndex, out Contact contactData) => throw new NotImplementedException(); + public float GetDepth(int contactIndex) => throw new NotImplementedException(); + public int GetFeatureId(int contactIndex) => throw new NotImplementedException(); + public Vector3 GetNormal(int contactIndex) => throw new NotImplementedException(); + public Vector3 GetOffset(int contactIndex) => throw new NotImplementedException(); } public void Flush() diff --git a/Demos/Demos/Tanks/TankCallbacks.cs b/Demos/Demos/Tanks/TankCallbacks.cs index 3d43c620b..3a172167b 100644 --- a/Demos/Demos/Tanks/TankCallbacks.cs +++ b/Demos/Demos/Tanks/TankCallbacks.cs @@ -140,7 +140,7 @@ public bool ConfigureContactManifold(int workerIndex, CollidablePair //In most cases, this isn't a problem at all, but tank projectiles are moving very quickly and a single missed frame might be enough to not trigger an explosion. //A nonzero epsilon helps catch those cases. //(An alternative would be to check each projectile's contact constraints and cause an explosion if any contact has nonzero penetration impulse.) - if (manifold.GetDepth(ref manifold, i) >= -1e-3f) + if (manifold.GetDepth(i) >= -1e-3f) { //An actual collision was found. if (propertiesA.Projectile) From e482a98641c646211a881c373d7e619fabd537a8 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 4 Sep 2023 13:27:16 -0500 Subject: [PATCH 821/947] Some missing debug validation updates. --- .../CollisionDetection/NarrowPhaseCCDContinuations.cs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/BepuPhysics/CollisionDetection/NarrowPhaseCCDContinuations.cs b/BepuPhysics/CollisionDetection/NarrowPhaseCCDContinuations.cs index 663c3e781..d0d98ee64 100644 --- a/BepuPhysics/CollisionDetection/NarrowPhaseCCDContinuations.cs +++ b/BepuPhysics/CollisionDetection/NarrowPhaseCCDContinuations.cs @@ -130,11 +130,11 @@ public void OnPairCompleted(int pairId, ref TManifold manifoldReferen //Check all contact data for invalid data early so that we don't end up spewing NaNs all over the engine and catching in the broad phase or some other highly indirect location. for (int i = 0; i < manifoldReference.Count; ++i) { - manifoldReference.GetDepth(ref manifoldReference, i).Validate(); - ref var normal = ref manifoldReference.GetNormal(ref manifoldReference, i); + manifoldReference.GetDepth(i).Validate(); + var normal = manifoldReference.GetNormal(i); normal.Validate(); Debug.Assert(Math.Abs(normal.LengthSquared() - 1) < 1e-5f, "Normals should be unit length. Something's gone wrong!"); - manifoldReference.GetOffset(ref manifoldReference, i).Validate(); + manifoldReference.GetOffset(i).Validate(); } #endif switch ((ConstraintGeneratorType)continuationId.Type) From b0898c46073ee2ec09fc32ce53b580c14a752630 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 6 Sep 2023 14:07:14 -0500 Subject: [PATCH 822/947] Convex hulls can now tolerate coplanar inputs, at least sometimes. --- BepuPhysics/Collidables/ConvexHull.cs | 11 +++++++ BepuPhysics/Collidables/ConvexHullHelper.cs | 33 ++++++++++++++++++-- Demos/SpecializedTests/ConvexHullTestDemo.cs | 19 +++++++++++ 3 files changed, 60 insertions(+), 3 deletions(-) diff --git a/BepuPhysics/Collidables/ConvexHull.cs b/BepuPhysics/Collidables/ConvexHull.cs index cb99b118a..a1da898da 100644 --- a/BepuPhysics/Collidables/ConvexHull.cs +++ b/BepuPhysics/Collidables/ConvexHull.cs @@ -187,6 +187,17 @@ public readonly BodyInertia ComputeInertia(float mass) { var triangleSource = new ConvexHullTriangleSource(this); MeshInertiaHelper.ComputeClosedInertia(ref triangleSource, mass, out _, out var inertiaTensor); + if (float.IsNaN(inertiaTensor.XX) || + float.IsNaN(inertiaTensor.YX) || + float.IsNaN(inertiaTensor.YY) || + float.IsNaN(inertiaTensor.ZX) || + float.IsNaN(inertiaTensor.ZY) || + float.IsNaN(inertiaTensor.ZZ)) + { + //The convex hull has no volume; we can't use the closed inertia calculation. + triangleSource = new ConvexHullTriangleSource(this); + MeshInertiaHelper.ComputeOpenInertia(ref triangleSource, mass, out inertiaTensor); + } BodyInertia inertia; inertia.InverseMass = 1f / mass; Symmetric3x3.Invert(inertiaTensor, out inertia.InverseInertiaTensor); diff --git a/BepuPhysics/Collidables/ConvexHullHelper.cs b/BepuPhysics/Collidables/ConvexHullHelper.cs index dfe1ba510..5b460b5ef 100644 --- a/BepuPhysics/Collidables/ConvexHullHelper.cs +++ b/BepuPhysics/Collidables/ConvexHullHelper.cs @@ -130,7 +130,7 @@ static void FindExtremeFace( var candidateIndices = indexOffsets + new Vector(i << BundleIndexing.VectorShift); ignoreSlot = Vector.BitwiseOr( Vector.BitwiseOr( - Vector.OnesComplement(allowVertexBundles[i]), + Vector.OnesComplement(allowVertexBundles[i]), Vector.BitwiseAnd(Vector.LessThanOrEqual(x, planeEpsilon), Vector.LessThanOrEqual(y, planeEpsilon))), Vector.BitwiseOr(Vector.Equals(candidateIndices, edgeIndexA), Vector.Equals(candidateIndices, edgeIndexB))); var useCandidate = Vector.AndNot(Vector.GreaterThan(y * bestX, bestY * x), ignoreSlot); @@ -810,7 +810,7 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa reducedFaceIndices.Count = 0; facePoints.Count = 0; ReduceFace(ref rawFaceVertexIndices, faceNormal, points, planeEpsilonNarrow, ref facePoints, ref allowVertices, ref reducedFaceIndices); - + if (reducedFaceIndices.Count < 3) { //steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertices, ref faces, default, -1)); @@ -1078,11 +1078,38 @@ public static void CreateShape(Span points, HullData hullData, BufferPo var c = points[face[subtriangleIndex]]; var volumeContribution = MeshInertiaHelper.ComputeTetrahedronVolume(a, b, c); volume += volumeContribution; - center += (a + b + c) * volumeContribution; + var centroid = a + b + c; + center += centroid * volumeContribution; } } //Division by 4 since we accumulated (a + b + c), rather than the actual tetrahedral center (a + b + c + 0) / 4. center /= volume * 4; + if (float.IsNaN(center.X) || float.IsNaN(center.Y) || float.IsNaN(center.Z)) + { + //The convex hull seems to have no volume. Treat it as coplanar and retry. + center = default; + float scaledSurfaceArea = 0; + for (int faceIndex = 0; faceIndex < hullData.FaceStartIndices.Length; ++faceIndex) + { + hullData.GetFace(faceIndex, out var face); + for (int subtriangleIndex = 2; subtriangleIndex < face.VertexCount; ++subtriangleIndex) + { + var a = points[face[0]]; + var b = points[face[subtriangleIndex - 1]]; + var c = points[face[subtriangleIndex]]; + var areaContribution = Vector3.Cross(b - a, c - a).Length(); + scaledSurfaceArea += areaContribution; + var centroid = a + b + c; + center += centroid * areaContribution; + } + } + center /= scaledSurfaceArea * 3; + if (float.IsNaN(center.X) || float.IsNaN(center.Y) || float.IsNaN(center.Z)) + { + throw new ArgumentException("Convex hull has no volume or surface area. This is not a valid convex hull."); + } + } + var lastIndex = hullData.OriginalVertexMapping.Length - 1; for (int bundleIndex = 0; bundleIndex < hullShape.Points.Length; ++bundleIndex) diff --git a/Demos/SpecializedTests/ConvexHullTestDemo.cs b/Demos/SpecializedTests/ConvexHullTestDemo.cs index f72b95c7e..6362b1b90 100644 --- a/Demos/SpecializedTests/ConvexHullTestDemo.cs +++ b/Demos/SpecializedTests/ConvexHullTestDemo.cs @@ -32,6 +32,24 @@ Buffer CreateRandomConvexHullPoints() return points; } + Buffer CreatePlaneish() + { + var points = new Buffer(12, BufferPool); + points[0] = new Vector3(-13.82f, 16.79f, 13.83f); + points[1] = new Vector3(13.82f, -16.79f, -13.83f); + points[2] = new Vector3(13.82f, 16.79f, -13.83f); + points[3] = new Vector3(-13.82f, 16.79f, 13.83f); + points[4] = new Vector3(13.82f, 16.79f, -13.83f); + points[5] = new Vector3(-13.82f, -16.79f, 13.83f); + points[6] = new Vector3(13.82f, 16.79f, -13.83f); + points[7] = new Vector3(13.82f, -16.79f, -13.83f); + points[8] = new Vector3(-13.82f, -16.79f, 13.83f); + points[9] = new Vector3(-13.82f, 16.79f, 13.83f); + points[10] = new Vector3(-13.82f, -16.79f, 13.83f); + points[11] = new Vector3(13.82f, -16.79f, -13.83f); + return points; + } + Buffer CreateMeshConvexHull(MeshContent meshContent, Vector3 scale) { //This is actually a pretty good example of how *not* to make a convex hull shape. @@ -320,6 +338,7 @@ public override void Initialize(ContentArchive content, Camera camera) //} //var hullPoints = CreateRandomConvexHullPoints(); var hullPoints = CreateMeshConvexHull(content.Load(@"Content\newt.obj"), new Vector3(1, 1.5f, 1f)); + //var hullPoints = CreatePlaneish(); //var hullPoints = CreateTestConvexHull2(); //var hullPoints = CreateBoxConvexHull(2); var hullShape = new ConvexHull(hullPoints, BufferPool, out _); From 97ef57403c8766470d651943486596bdaff19d59 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 7 Sep 2023 18:04:46 -0500 Subject: [PATCH 823/947] Rolled back attempt at degenerate hull support; too many holes. Convex hull helper now reports success/failure more clearly and ConvexHull constructor will throw an exception for degenerate cases. --- BepuPhysics/Collidables/ConvexHull.cs | 23 +++---- BepuPhysics/Collidables/ConvexHullHelper.cs | 68 ++++++++------------- 2 files changed, 35 insertions(+), 56 deletions(-) diff --git a/BepuPhysics/Collidables/ConvexHull.cs b/BepuPhysics/Collidables/ConvexHull.cs index a1da898da..2a1f12626 100644 --- a/BepuPhysics/Collidables/ConvexHull.cs +++ b/BepuPhysics/Collidables/ConvexHull.cs @@ -60,7 +60,8 @@ public struct ConvexHull : IConvexShape /// Computed center of the convex hull before the hull was recentered. public ConvexHull(Span points, BufferPool pool, out Vector3 center) { - ConvexHullHelper.CreateShape(points, pool, out center, out this); + if (!ConvexHullHelper.CreateShape(points, pool, out center, out this)) + throw new ArgumentException("Could not create a convex hull from the point set; is it degenerate? Convex hull shapes must have volume."); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -186,18 +187,13 @@ public bool GetNextTriangle(out Vector3 a, out Vector3 b, out Vector3 c) public readonly BodyInertia ComputeInertia(float mass) { var triangleSource = new ConvexHullTriangleSource(this); - MeshInertiaHelper.ComputeClosedInertia(ref triangleSource, mass, out _, out var inertiaTensor); - if (float.IsNaN(inertiaTensor.XX) || - float.IsNaN(inertiaTensor.YX) || - float.IsNaN(inertiaTensor.YY) || - float.IsNaN(inertiaTensor.ZX) || - float.IsNaN(inertiaTensor.ZY) || - float.IsNaN(inertiaTensor.ZZ)) - { - //The convex hull has no volume; we can't use the closed inertia calculation. - triangleSource = new ConvexHullTriangleSource(this); - MeshInertiaHelper.ComputeOpenInertia(ref triangleSource, mass, out inertiaTensor); - } + MeshInertiaHelper.ComputeClosedInertia(ref triangleSource, mass, out var volume, out var inertiaTensor); + //While it's possible to go through the construction process on a convex hull with no volume, it'll very likely break ray tests which rely on bounding planes, so we don't support it. + Debug.Assert( + FaceToVertexIndicesStart.Length > 2 && + volume > 0 && !float.IsNaN(inertiaTensor.XX) && + !float.IsNaN(inertiaTensor.YX) && !float.IsNaN(inertiaTensor.YY) && !float.IsNaN(inertiaTensor.ZX) && !float.IsNaN(inertiaTensor.ZY) && !float.IsNaN(inertiaTensor.ZZ), + "Convex hull must have volume."); BodyInertia inertia; inertia.InverseMass = 1f / mass; Symmetric3x3.Invert(inertiaTensor, out inertia.InverseInertiaTensor); @@ -211,6 +207,7 @@ public static ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, public readonly bool RayTest(in RigidPose pose, Vector3 origin, Vector3 direction, out float t, out Vector3 normal) { + Debug.Assert(FaceToVertexIndicesStart.Length > 2, "Convex hull appears to be degenerate; convex hull must have volume or ray tests will fail."); Matrix3x3.CreateFromQuaternion(pose.Orientation, out var orientation); var shapeToRay = origin - pose.Position; Matrix3x3.TransformTranspose(shapeToRay, orientation, out var localOrigin); diff --git a/BepuPhysics/Collidables/ConvexHullHelper.cs b/BepuPhysics/Collidables/ConvexHullHelper.cs index 5b460b5ef..69c792768 100644 --- a/BepuPhysics/Collidables/ConvexHullHelper.cs +++ b/BepuPhysics/Collidables/ConvexHullHelper.cs @@ -70,8 +70,11 @@ public void GetFace(int faceIndex, out HullFace face) public void Dispose(BufferPool pool) { pool.Return(ref OriginalVertexMapping); - pool.Return(ref FaceVertexIndices); - pool.Return(ref FaceStartIndices); + //The other allocations may not exist if the hull is degenerate. + if (FaceVertexIndices.Allocated) + pool.Return(ref FaceVertexIndices); + if (FaceStartIndices.Allocated) + pool.Return(ref FaceStartIndices); } } @@ -1046,23 +1049,11 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa /// Pool used to allocate resources for the hullShape. /// Convex hull shape created from the input data. /// Computed center of mass of the convex hull before its points were recentered onto the origin. - public static void CreateShape(Span points, HullData hullData, BufferPool pool, out Vector3 center, out ConvexHull hullShape) + /// True if the shape was created successfully, false otherwise. If false, the hull probably had no volume and would not have worked properly as a shape. + public static bool CreateShape(Span points, HullData hullData, BufferPool pool, out Vector3 center, out ConvexHull hullShape) { Debug.Assert(points.Length > 0, "Convex hulls need to have a nonzero number of points!"); hullShape = default; - if (hullData.OriginalVertexMapping.Length < 3) - { - center = default; - if (hullData.OriginalVertexMapping.Length > 0) - { - for (int i = 0; i < hullData.OriginalVertexMapping.Length; ++i) - { - center += points[hullData.OriginalVertexMapping[i]]; - } - center /= hullData.OriginalVertexMapping.Length; - } - return; - } var pointBundleCount = BundleIndexing.GetBundleCount(hullData.OriginalVertexMapping.Length); pool.Take(pointBundleCount, out hullShape.Points); @@ -1086,31 +1077,18 @@ public static void CreateShape(Span points, HullData hullData, BufferPo center /= volume * 4; if (float.IsNaN(center.X) || float.IsNaN(center.Y) || float.IsNaN(center.Z)) { - //The convex hull seems to have no volume. Treat it as coplanar and retry. + //The convex hull seems to have no volume. + //While you could try treating it as coplanar (like we once tried; see commit history just prior to the commit that added this message): + //1. Ray tests won't work. They rely on bounding planes. It would require a special case for degenerate hulls. + //2. Inertia won't work. You could resolve that with a special case, but it doesn't fix ray tests. + //3. Edge-on contact generation may produce lower quality contacts. + //So, pretty worthless overall without major changes. + hullShape.Points.Dispose(pool); center = default; - float scaledSurfaceArea = 0; - for (int faceIndex = 0; faceIndex < hullData.FaceStartIndices.Length; ++faceIndex) - { - hullData.GetFace(faceIndex, out var face); - for (int subtriangleIndex = 2; subtriangleIndex < face.VertexCount; ++subtriangleIndex) - { - var a = points[face[0]]; - var b = points[face[subtriangleIndex - 1]]; - var c = points[face[subtriangleIndex]]; - var areaContribution = Vector3.Cross(b - a, c - a).Length(); - scaledSurfaceArea += areaContribution; - var centroid = a + b + c; - center += centroid * areaContribution; - } - } - center /= scaledSurfaceArea * 3; - if (float.IsNaN(center.X) || float.IsNaN(center.Y) || float.IsNaN(center.Z)) - { - throw new ArgumentException("Convex hull has no volume or surface area. This is not a valid convex hull."); - } + Debug.Assert(!hullShape.Points.Allocated && !hullShape.FaceToVertexIndicesStart.Allocated && !hullShape.BoundingPlanes.Allocated && !hullShape.FaceVertexIndices.Allocated, "Hey! You moved something around and forgot to dispose!"); + return false; } - var lastIndex = hullData.OriginalVertexMapping.Length - 1; for (int bundleIndex = 0; bundleIndex < hullShape.Points.Length; ++bundleIndex) { @@ -1179,6 +1157,7 @@ public static void CreateShape(Span points, HullData hullData, BufferPo Vector3Wide.WriteFirst(default, ref offsetInstance.Normal); GatherScatter.GetFirst(ref offsetInstance.Offset) = float.MinValue; } + return true; } /// @@ -1189,10 +1168,11 @@ public static void CreateShape(Span points, HullData hullData, BufferPo /// Intermediate hull data that got processed into the convex hull. /// Computed center of mass of the convex hull before its points were recentered onto the origin. /// Convex hull shape of the input point set. - public static void CreateShape(Span points, BufferPool pool, out HullData hullData, out Vector3 center, out ConvexHull convexHull) + /// True if the shape was created successfully, false otherwise. If false, the hull probably had no volume and would not have worked properly as a shape. + public static bool CreateShape(Span points, BufferPool pool, out HullData hullData, out Vector3 center, out ConvexHull convexHull) { ComputeHull(points, pool, out hullData); - CreateShape(points, hullData, pool, out center, out convexHull); + return CreateShape(points, hullData, pool, out center, out convexHull); } /// @@ -1202,13 +1182,15 @@ public static void CreateShape(Span points, BufferPool pool, out HullDa /// Buffer pool used for temporary allocations and the output data structures. /// Computed center of mass of the convex hull before its points were recentered onto the origin. /// Convex hull shape of the input point set. - public static void CreateShape(Span points, BufferPool pool, out Vector3 center, out ConvexHull convexHull) + /// True if the shape was created successfully, false otherwise. If false, the hull probably had no volume and would not have worked properly as a shape. + public static bool CreateShape(Span points, BufferPool pool, out Vector3 center, out ConvexHull convexHull) { ComputeHull(points, pool, out var hullData); - CreateShape(points, hullData, pool, out center, out convexHull); - //Empty input point sets won't allocate. + var result = CreateShape(points, hullData, pool, out center, out convexHull); + //Empty input point sets won't allocate; don't try to dispose them. if (hullData.OriginalVertexMapping.Allocated) hullData.Dispose(pool); + return result; } From c56db63117e1d7edf52f0618aecc7da843839b97 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 14 Sep 2023 16:53:21 -0500 Subject: [PATCH 824/947] Missing doc. --- BepuPhysics/Trees/Tree_IntertreeQueries.cs | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/BepuPhysics/Trees/Tree_IntertreeQueries.cs b/BepuPhysics/Trees/Tree_IntertreeQueries.cs index 863d16c4e..d1106ef93 100644 --- a/BepuPhysics/Trees/Tree_IntertreeQueries.cs +++ b/BepuPhysics/Trees/Tree_IntertreeQueries.cs @@ -129,6 +129,12 @@ private void GetOverlapsBetweenDifferentNodes(ref Node a, ref N } } + /// + /// Gets pairs of leaf indices with bounding boxes which overlap. + /// + /// Type of the implementation to report pairs to. + /// Tree to test this tree against. + /// Handler to report pairs to. public void GetOverlaps(ref Tree treeB, ref TOverlapHandler overlapHandler) where TOverlapHandler : struct, IOverlapHandler { if (LeafCount == 0 || treeB.LeafCount == 0) From b5154b62e76c48d0b7ed48227473d3fb35c0fdfb Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 27 Sep 2023 17:40:29 -0500 Subject: [PATCH 825/947] PredictBoundingBoxes/IntegrateBundlesAfterSubstepping now pass -1's for empty lanes like the solver integration invocations. --- BepuPhysics/PoseIntegrator.cs | 7 +++++-- Demos/Demos/PerBodyGravityDemo.cs | 8 +++++--- 2 files changed, 10 insertions(+), 5 deletions(-) diff --git a/BepuPhysics/PoseIntegrator.cs b/BepuPhysics/PoseIntegrator.cs index 739d384f9..f73c31331 100644 --- a/BepuPhysics/PoseIntegrator.cs +++ b/BepuPhysics/PoseIntegrator.cs @@ -332,6 +332,8 @@ void PredictBoundingBoxes(int startBundleIndex, int endBundleIndex, float dt, re { integrationMask = Vector.AndNot(BundleIndexing.CreateMaskForCountInBundle(countInBundle), Bodies.IsKinematic(inertia)); } + //When the solver calls IntegrateVelocity, empty lanes are filled with -1. For consistent behavior at a trivial cost, we'll do the same here. + laneIndices = Vector.BitwiseOr(Vector.OnesComplement(integrationMask), laneIndices); var sleepEnergy = velocity.Linear.LengthSquared() + velocity.Angular.LengthSquared(); //Note that we're not storing out the integrated velocities. The integrated velocities are only used for bounding box prediction. @@ -612,7 +614,8 @@ void IntegrateBundlesAfterSubstepping(ref IndexSet mergedConstrainedBodyHandles, unconstrainedVelocityIntegrationMask = Vector.AndNot(unconstrainedMask, isKinematic); anyBodyInBundleNeedsVelocityIntegration = Vector.LessThanAny(unconstrainedVelocityIntegrationMask, Vector.Zero); } - //We don't want to scatter velocities into any slots that don't want velocity writes. By setting all the bits in such lanes, velocity scatter will skip them. + //We don't want to scatter velocities into any slots that don't want velocity writes. By setting all the bits in such lanes, scatter will skip them. + //This will also keep the body indices passed into callbacks.IntegrateVelocity consistent with those provided during PredictBoundingBoxes and the solver (-1 for ignored slots). var velocityMaskedBodyIndices = Vector.BitwiseOr(bodyIndices, Vector.OnesComplement(unconstrainedVelocityIntegrationMask)); if (anyBodyInBundleIsUnconstrained) @@ -633,7 +636,7 @@ void IntegrateBundlesAfterSubstepping(ref IndexSet mergedConstrainedBodyHandles, if (anyBodyInBundleNeedsVelocityIntegration) { - callbacks.IntegrateVelocity(bodyIndices, position, orientation, localInertia, unconstrainedVelocityIntegrationMask, workerIndex, bundleEffectiveDt, ref velocity); + callbacks.IntegrateVelocity(velocityMaskedBodyIndices, position, orientation, localInertia, unconstrainedVelocityIntegrationMask, workerIndex, bundleEffectiveDt, ref velocity); //It would be annoying to make the user handle masking velocity writes to inactive lanes, so we handle it internally. Vector3Wide.ConditionalSelect(unconstrainedVelocityIntegrationMask, velocity.Linear, previousVelocity.Linear, out velocity.Linear); Vector3Wide.ConditionalSelect(unconstrainedVelocityIntegrationMask, velocity.Angular, previousVelocity.Angular, out velocity.Angular); diff --git a/Demos/Demos/PerBodyGravityDemo.cs b/Demos/Demos/PerBodyGravityDemo.cs index 6390f3846..42e8b0b05 100644 --- a/Demos/Demos/PerBodyGravityDemo.cs +++ b/Demos/Demos/PerBodyGravityDemo.cs @@ -70,6 +70,7 @@ public void IntegrateVelocity(Vector bodyIndices, Vector3Wide position, Qua { var bodyIndex = bodyIndices[bundleSlotIndex]; //Not every slot in the SIMD vector is guaranteed to be filled. + //The integration mask tells us which ones are active in a way that's convenient for vectorized operations, but the bodyIndex for empty lanes will also be -1. if (bodyIndex >= 0) { var bodyHandle = bodies.ActiveSet.IndexToHandle[bodyIndex]; @@ -106,12 +107,13 @@ public override void Initialize(ContentArchive content, Camera camera) var boxInertia = boxShape.ComputeInertia(1); var boxShapeIndex = Simulation.Shapes.Add(boxShape); var spacing = new Vector3(4); - const int length = 20; + const int length = 1; + const int width = 1; + const int height = 1; for (int i = 0; i < length; ++i) { - for (int j = 0; j < 20; ++j) + for (int j = 0; j < height; ++j) { - const int width = 20; var origin = new Vector3(0, 40, 0) + spacing * new Vector3(length * -0.5f, 0, width * -0.5f); for (int k = 0; k < width; ++k) { From 49fd1d0372678dade05a5bea2051d49a68a79fe1 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 27 Sep 2023 17:40:53 -0500 Subject: [PATCH 826/947] Removed reference to old "ReleaseStrip" build config. --- Demos/DemoHarness.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Demos/DemoHarness.cs b/Demos/DemoHarness.cs index dcf742052..56dab83e1 100644 --- a/Demos/DemoHarness.cs +++ b/Demos/DemoHarness.cs @@ -321,7 +321,7 @@ public void Render(Renderer renderer) demo.Render(renderer, loop.Camera, loop.Input, uiText, font); #if DEBUG float warningHeight = 15f; - renderer.TextBatcher.Write(uiText.Clear().Append("Running in Debug configuration. Compile in Release or, better yet, ReleaseStrip configuration for performance testing."), + renderer.TextBatcher.Write(uiText.Clear().Append("Running in Debug configuration. Compile in Release configuration for performance testing."), new Vector2((loop.Window.Resolution.X - GlyphBatch.MeasureLength(uiText, font, warningHeight)) * 0.5f, warningHeight), warningHeight, new Vector3(1, 0, 0), font); #endif float textHeight = 16; From be4022896e69f83ebdf8bbbf10690acf10b49ee8 Mon Sep 17 00:00:00 2001 From: Salvage <29021710+Saalvage@users.noreply.github.com> Date: Mon, 9 Oct 2023 17:13:40 +0200 Subject: [PATCH 827/947] Introduce `IDisposableShape` Allows for properly disposing of a shape, given only an `IShape` without having to do various type checks. --- BepuPhysics/Collidables/ConvexHull.cs | 2 +- BepuPhysics/Collidables/IDisposableShape.cs | 13 +++++++++++++ BepuPhysics/Collidables/IShape.cs | 14 ++------------ 3 files changed, 16 insertions(+), 13 deletions(-) create mode 100644 BepuPhysics/Collidables/IDisposableShape.cs diff --git a/BepuPhysics/Collidables/ConvexHull.cs b/BepuPhysics/Collidables/ConvexHull.cs index 2a1f12626..d04829f7b 100644 --- a/BepuPhysics/Collidables/ConvexHull.cs +++ b/BepuPhysics/Collidables/ConvexHull.cs @@ -33,7 +33,7 @@ public override string ToString() } } - public struct ConvexHull : IConvexShape + public struct ConvexHull : IConvexShape, IDisposableShape { /// /// Bundled points of the convex hull. diff --git a/BepuPhysics/Collidables/IDisposableShape.cs b/BepuPhysics/Collidables/IDisposableShape.cs new file mode 100644 index 000000000..fcccd25ff --- /dev/null +++ b/BepuPhysics/Collidables/IDisposableShape.cs @@ -0,0 +1,13 @@ +using BepuUtilities.Memory; + +namespace BepuPhysics.Collidables +{ + public interface IDisposableShape : IShape + { + /// + /// Returns all resources used by the shape instance to the given pool. + /// + /// Pool to return shape resources to. + void Dispose(BufferPool pool); + } +} diff --git a/BepuPhysics/Collidables/IShape.cs b/BepuPhysics/Collidables/IShape.cs index 42a5ca54a..d3dc1e4ae 100644 --- a/BepuPhysics/Collidables/IShape.cs +++ b/BepuPhysics/Collidables/IShape.cs @@ -82,7 +82,7 @@ public interface IConvexShape : IShape /// /// Defines a compound shape type that has children of potentially different types. /// - public interface ICompoundShape : IShape, IBoundsQueryableCompound + public interface ICompoundShape : IDisposableShape, IBoundsQueryableCompound { //Note that compound shapes have no wide GetBounds function. Compounds, by virtue of containing shapes of different types, cannot be usefully vectorized over. //Instead, their children are added to other computation batches. @@ -135,11 +135,6 @@ public interface ICompoundShape : IShape, IBoundsQueryableCompound /// Index of the child to look up. /// Reference to the requested compound child. ref CompoundChild GetChild(int compoundChildIndex); - /// - /// Returns all resources used by the shape instance to the given pool. - /// - /// Pool to return shape resources to. - void Dispose(BufferPool pool); } /// @@ -147,7 +142,7 @@ public interface ICompoundShape : IShape, IBoundsQueryableCompound /// /// Type of the child shapes. /// Type of the child shapes, formatted in AOSOA layout. - public interface IHomogeneousCompoundShape : IShape, IBoundsQueryableCompound + public interface IHomogeneousCompoundShape : IDisposableShape, IBoundsQueryableCompound where TChildShape : unmanaged, IConvexShape where TChildShapeWide : unmanaged, IShapeWide { @@ -199,11 +194,6 @@ public interface IHomogeneousCompoundShape : IShap /// Index of the child in the compound parent. /// Reference to an AOSOA slot. void GetLocalChild(int childIndex, ref TChildShapeWide childData); - /// - /// Returns all resources used by the shape instance to the given pool. - /// - /// Pool to return shape resources to. - void Dispose(BufferPool pool); } /// From 5d93c964ee92a85d10c8406809027d431639a67a Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 9 Oct 2023 17:19:06 -0500 Subject: [PATCH 828/947] Version bump. --- BepuPhysics/BepuPhysics.csproj | 2 +- BepuUtilities/BepuUtilities.csproj | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/BepuPhysics/BepuPhysics.csproj b/BepuPhysics/BepuPhysics.csproj index 1680cef59..10695d65a 100644 --- a/BepuPhysics/BepuPhysics.csproj +++ b/BepuPhysics/BepuPhysics.csproj @@ -1,7 +1,7 @@  net7.0 - 2.5.0-beta.13 + 2.5.0-beta.14 Bepu Entertainment LLC Ross Nordby Speedy real time physics simulation library. diff --git a/BepuUtilities/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj index 3478613d6..5dd78a978 100644 --- a/BepuUtilities/BepuUtilities.csproj +++ b/BepuUtilities/BepuUtilities.csproj @@ -3,7 +3,7 @@ BepuUtilities BepuUtilities net7.0 - 2.5.0-beta.13 + 2.5.0-beta.14 Bepu Entertainment LLC Ross Nordby Supporting utilities library for BEPUphysics v2. From 0a11a24d91ad80f4656cb705df010398061863a0 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 9 Oct 2023 19:25:06 -0500 Subject: [PATCH 829/947] Robustified a degenerate corner case in convex hull builder. --- BepuPhysics/Collidables/ConvexHullHelper.cs | 8 ++++++-- Demos/SpecializedTests/ConvexHullTestDemo.cs | 19 +++++++++++++++++++ 2 files changed, 25 insertions(+), 2 deletions(-) diff --git a/BepuPhysics/Collidables/ConvexHullHelper.cs b/BepuPhysics/Collidables/ConvexHullHelper.cs index 69c792768..5f81313e8 100644 --- a/BepuPhysics/Collidables/ConvexHullHelper.cs +++ b/BepuPhysics/Collidables/ConvexHullHelper.cs @@ -161,7 +161,11 @@ static void FindExtremeFace( //(This isn't wonderfully fast, but it's fairly simple. The alternatives are things like incrementally combining coplanar triangles as they are discovered //or using a postpass that looks for coplanar triangles after they've been created.) //Rotate the offset to point outward. - var projectedPlaneNormalNarrow = Vector2.Normalize(new Vector2(-bestYNarrow, bestXNarrow)); + //Note: in unusual corner cases, the above may have accepted zero candidates resulting in a bestXNarrow = 1 and bestYNarrow = float.MinValue. + //Catching that and ensuring that a reasonable face normal is output avoids a bad face. + var candidateNormalDirection = new Vector2(-bestYNarrow, bestXNarrow); + var length = candidateNormalDirection.Length(); + var projectedPlaneNormalNarrow = float.IsFinite(length) ? candidateNormalDirection / length : new Vector2(1, 0); Vector2Wide.Broadcast(projectedPlaneNormalNarrow, out var projectedPlaneNormal); Vector3Wide.ReadFirst(basisX, out var basisXNarrow); Vector3Wide.ReadFirst(basisY, out var basisYNarrow); @@ -1075,7 +1079,7 @@ public static bool CreateShape(Span points, HullData hullData, BufferPo } //Division by 4 since we accumulated (a + b + c), rather than the actual tetrahedral center (a + b + c + 0) / 4. center /= volume * 4; - if (float.IsNaN(center.X) || float.IsNaN(center.Y) || float.IsNaN(center.Z)) + if (float.IsNaN(center.X) || float.IsNaN(center.Y) || float.IsNaN(center.Z) || hullData.FaceStartIndices.Length == 2) { //The convex hull seems to have no volume. //While you could try treating it as coplanar (like we once tried; see commit history just prior to the commit that added this message): diff --git a/Demos/SpecializedTests/ConvexHullTestDemo.cs b/Demos/SpecializedTests/ConvexHullTestDemo.cs index 6362b1b90..ef92f8570 100644 --- a/Demos/SpecializedTests/ConvexHullTestDemo.cs +++ b/Demos/SpecializedTests/ConvexHullTestDemo.cs @@ -50,6 +50,24 @@ Buffer CreatePlaneish() return points; } + Buffer CreateDistantPlane() + { + var points = new Buffer(12, BufferPool); + points[0] = new(-151.0875f, -2.2505488f, 102.17515f); + points[1] = new(-151.10571f, 2.1121342f, -17.699797f); + points[2] = new(-151.08746f, -2.2504745f, -17.699797f); + points[3] = new(-151.10571f, 2.1121342f, -17.699797f); + points[4] = new(-151.0875f, -2.2505488f, 102.17515f); + points[5] = new(-151.10574f, 2.1120775f, 102.17517f); + points[6] = new(-151.10571f, 2.1121342f, -17.699797f); + points[7] = new(-151.10574f, 2.1120775f, 102.17517f); + points[8] = new(-151.08746f, -2.2504745f, -17.699797f); + points[9] = new(-151.08746f, -2.2504745f, -17.699797f); + points[10] = new(-151.10574f, 2.1120775f, 102.17517f); + points[11] = new(-151.0875f, -2.2505488f, 102.17515f); + return points; + } + Buffer CreateMeshConvexHull(MeshContent meshContent, Vector3 scale) { //This is actually a pretty good example of how *not* to make a convex hull shape. @@ -339,6 +357,7 @@ public override void Initialize(ContentArchive content, Camera camera) //var hullPoints = CreateRandomConvexHullPoints(); var hullPoints = CreateMeshConvexHull(content.Load(@"Content\newt.obj"), new Vector3(1, 1.5f, 1f)); //var hullPoints = CreatePlaneish(); + //var hullPoints = CreateDistantPlane(); //var hullPoints = CreateTestConvexHull2(); //var hullPoints = CreateBoxConvexHull(2); var hullShape = new ConvexHull(hullPoints, BufferPool, out _); From 13b9ff35ac3939856fd09a2f821157d188c6d2bc Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 14 Nov 2023 16:11:36 -0600 Subject: [PATCH 830/947] okay 2.5 still hasn't been released and .net 8 has hmm --- BepuPhysics/BepuPhysics.csproj | 2 +- BepuUtilities/BepuUtilities.csproj | 2 +- DemoBenchmarks/DemoBenchmarks.csproj | 2 +- DemoContentBuilder/DemoContentBuilder.csproj | 2 +- DemoContentLoader/DemoContentLoader.csproj | 2 +- DemoRenderer/DemoRenderer.csproj | 6 +++--- DemoTests/DemoTests.csproj | 2 +- DemoUtilities/DemoUtilities.csproj | 2 +- Demos/Demos.csproj | 6 +++--- 9 files changed, 13 insertions(+), 13 deletions(-) diff --git a/BepuPhysics/BepuPhysics.csproj b/BepuPhysics/BepuPhysics.csproj index 10695d65a..6260b7f63 100644 --- a/BepuPhysics/BepuPhysics.csproj +++ b/BepuPhysics/BepuPhysics.csproj @@ -1,6 +1,6 @@  - net7.0 + net8.0 2.5.0-beta.14 Bepu Entertainment LLC Ross Nordby diff --git a/BepuUtilities/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj index 5dd78a978..4c6277b3a 100644 --- a/BepuUtilities/BepuUtilities.csproj +++ b/BepuUtilities/BepuUtilities.csproj @@ -2,7 +2,7 @@ BepuUtilities BepuUtilities - net7.0 + net8.0 2.5.0-beta.14 Bepu Entertainment LLC Ross Nordby diff --git a/DemoBenchmarks/DemoBenchmarks.csproj b/DemoBenchmarks/DemoBenchmarks.csproj index 3cc15ee2c..263d7f079 100644 --- a/DemoBenchmarks/DemoBenchmarks.csproj +++ b/DemoBenchmarks/DemoBenchmarks.csproj @@ -2,7 +2,7 @@ Exe - net7.0 + net8.0 enable true -f *CollisionBatcherTaskBenchmarks.* *GroupedCollisionTesterBenchmarks.* *GatherScatterBenchmarks.* *OneBodyConstraintBenchmarks.* *TwoBodyConstraintBenchmarks.* *ThreeBodyConstraintBenchmarks.* *FourBodyConstraintBenchmarks.* *SweepBenchmarks.* *ShapeRayBenchmarks.* *ShapePileBenchmark.* *RagdollTubeBenchmark.* --join diff --git a/DemoContentBuilder/DemoContentBuilder.csproj b/DemoContentBuilder/DemoContentBuilder.csproj index 5277e7c26..4897b5b9c 100644 --- a/DemoContentBuilder/DemoContentBuilder.csproj +++ b/DemoContentBuilder/DemoContentBuilder.csproj @@ -2,7 +2,7 @@ Exe - net7.0 + net8.0 x64 latest true diff --git a/DemoContentLoader/DemoContentLoader.csproj b/DemoContentLoader/DemoContentLoader.csproj index 847b2074e..cc5df10c6 100644 --- a/DemoContentLoader/DemoContentLoader.csproj +++ b/DemoContentLoader/DemoContentLoader.csproj @@ -1,7 +1,7 @@  - net7.0 + net8.0 latest True diff --git a/DemoRenderer/DemoRenderer.csproj b/DemoRenderer/DemoRenderer.csproj index a75017523..31a5870ae 100644 --- a/DemoRenderer/DemoRenderer.csproj +++ b/DemoRenderer/DemoRenderer.csproj @@ -1,13 +1,13 @@  - net7.0 + net8.0 latest True - - + + diff --git a/DemoTests/DemoTests.csproj b/DemoTests/DemoTests.csproj index 08da411bc..6e1a6283c 100644 --- a/DemoTests/DemoTests.csproj +++ b/DemoTests/DemoTests.csproj @@ -1,7 +1,7 @@ - net7.0 + net8.0 true false latest diff --git a/DemoUtilities/DemoUtilities.csproj b/DemoUtilities/DemoUtilities.csproj index e85afca8c..eab8c0879 100644 --- a/DemoUtilities/DemoUtilities.csproj +++ b/DemoUtilities/DemoUtilities.csproj @@ -1,7 +1,7 @@  - net7.0 + net8.0 latest true diff --git a/Demos/Demos.csproj b/Demos/Demos.csproj index 6e2628452..d3043a4d1 100644 --- a/Demos/Demos.csproj +++ b/Demos/Demos.csproj @@ -1,7 +1,7 @@  Exe - net7.0 + net8.0 True Debug;Release latest @@ -9,8 +9,8 @@ - - + + From 72c25a8c9b8a44399a73ff1c91b043644019a739 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 14 Nov 2023 16:13:51 -0600 Subject: [PATCH 831/947] CS9195 remediation. --- BepuPhysics/Trees/Tree_SelfQueries.cs | 2 +- BepuUtilities/BoundingBox.cs | 36 +++++++++++++-------------- 2 files changed, 19 insertions(+), 19 deletions(-) diff --git a/BepuPhysics/Trees/Tree_SelfQueries.cs b/BepuPhysics/Trees/Tree_SelfQueries.cs index e9bd4eba2..319ace33a 100644 --- a/BepuPhysics/Trees/Tree_SelfQueries.cs +++ b/BepuPhysics/Trees/Tree_SelfQueries.cs @@ -296,7 +296,7 @@ public static Vector128 GetLeftPackMask(Vector128 mask, out int count) 0b1110_0100, 0b1110_0100, 0b1110_0101, 0b1110_0100, 0b1110_0110, 0b1110_1000, 0b1110_1001, 0b1110_0100, //1000 1001 1010 1011 1100 1101 1110 1111 0b1110_0111, 0b1110_1100, 0b1110_1101, 0b1111_0100, 0b1110_1110, 0b1111_1000, 0b1111_1001, 0b1110_0100 }; - var encodedLeftPackMask = Unsafe.Add(ref Unsafe.AsRef(lookupTable[0]), bitmask); + var encodedLeftPackMask = Unsafe.Add(ref Unsafe.AsRef(in lookupTable[0]), bitmask); count = BitOperations.PopCount(bitmask); //Broadcast, variable shift. diff --git a/BepuUtilities/BoundingBox.cs b/BepuUtilities/BoundingBox.cs index 27cbe0ccc..0dcae8ba5 100644 --- a/BepuUtilities/BoundingBox.cs +++ b/BepuUtilities/BoundingBox.cs @@ -95,8 +95,8 @@ public unsafe static bool IntersectsUnsafe(in TA boundingBoxA, in TB bou if (Vector128.IsHardwareAccelerated) { //THIS IS A POTENTIAL GC HOLE IF CHILDREN ARE PASSED FROM UNPINNED MANAGED MEMORY - ref var a = ref Unsafe.As(ref Unsafe.AsRef(boundingBoxA)); - ref var b = ref Unsafe.As(ref Unsafe.AsRef(boundingBoxB)); + ref var a = ref Unsafe.As(ref Unsafe.AsRef(in boundingBoxA)); + ref var b = ref Unsafe.As(ref Unsafe.AsRef(in boundingBoxB)); var aMin = Vector128.LoadUnsafe(ref a); var aMax = Vector128.LoadUnsafe(ref Unsafe.Add(ref a, 4)); var bMin = Vector128.LoadUnsafe(ref b); @@ -106,8 +106,8 @@ public unsafe static bool IntersectsUnsafe(in TA boundingBoxA, in TB bou } else { - var a = (float*)Unsafe.AsPointer(ref Unsafe.AsRef(boundingBoxA)); - var b = (float*)Unsafe.AsPointer(ref Unsafe.AsRef(boundingBoxB)); + var a = (float*)Unsafe.AsPointer(ref Unsafe.AsRef(in boundingBoxA)); + var b = (float*)Unsafe.AsPointer(ref Unsafe.AsRef(in boundingBoxB)); return a[4] >= b[0] & a[5] >= b[1] & a[6] >= b[2] & b[4] >= a[0] & b[5] >= a[1] & b[6] >= a[2]; } @@ -207,11 +207,11 @@ public static void CreateMergedUnsafeWithPreservation(in TA boundingBoxA ref var resultMin = ref Unsafe.As>(ref merged); ref var resultMax = ref Unsafe.Add(ref Unsafe.As>(ref merged), 1); var min = Vector128.Min( - Unsafe.As>(ref Unsafe.AsRef(boundingBoxA)), - Unsafe.As>(ref Unsafe.AsRef(boundingBoxB))); + Unsafe.As>(ref Unsafe.AsRef(in boundingBoxA)), + Unsafe.As>(ref Unsafe.AsRef(in boundingBoxB))); var max = Vector128.Max( - Unsafe.Add(ref Unsafe.As>(ref Unsafe.AsRef(boundingBoxA)), 1), - Unsafe.Add(ref Unsafe.As>(ref Unsafe.AsRef(boundingBoxB)), 1)); + Unsafe.Add(ref Unsafe.As>(ref Unsafe.AsRef(in boundingBoxA)), 1), + Unsafe.Add(ref Unsafe.As>(ref Unsafe.AsRef(in boundingBoxB)), 1)); if (Sse41.IsSupported) { resultMin = Sse41.Blend(min, resultMin, 0b1000); @@ -226,10 +226,10 @@ public static void CreateMergedUnsafeWithPreservation(in TA boundingBoxA } else { - ref var a = ref Unsafe.As(ref Unsafe.AsRef(boundingBoxA)); - ref var b = ref Unsafe.As(ref Unsafe.AsRef(boundingBoxB)); + ref var a = ref Unsafe.As(ref Unsafe.AsRef(in boundingBoxA)); + ref var b = ref Unsafe.As(ref Unsafe.AsRef(in boundingBoxB)); Unsafe.SkipInit(out merged); - ref var result = ref Unsafe.As(ref Unsafe.AsRef(merged)); + ref var result = ref Unsafe.As(ref Unsafe.AsRef(in merged)); result.Min = Vector3.Min(a.Min, b.Min); result.Max = Vector3.Max(a.Max, b.Max); } @@ -253,18 +253,18 @@ public static void CreateMergedUnsafe(in TA boundingBoxA, in TB bounding ref var resultMin = ref Unsafe.As>(ref merged); ref var resultMax = ref Unsafe.Add(ref Unsafe.As>(ref merged), 1); resultMin = Vector128.Min( - Unsafe.As>(ref Unsafe.AsRef(boundingBoxA)), - Unsafe.As>(ref Unsafe.AsRef(boundingBoxB))); + Unsafe.As>(ref Unsafe.AsRef(in boundingBoxA)), + Unsafe.As>(ref Unsafe.AsRef(in boundingBoxB))); resultMax = Vector128.Max( - Unsafe.Add(ref Unsafe.As>(ref Unsafe.AsRef(boundingBoxA)), 1), - Unsafe.Add(ref Unsafe.As>(ref Unsafe.AsRef(boundingBoxB)), 1)); + Unsafe.Add(ref Unsafe.As>(ref Unsafe.AsRef(in boundingBoxA)), 1), + Unsafe.Add(ref Unsafe.As>(ref Unsafe.AsRef(in boundingBoxB)), 1)); } else { - ref var a = ref Unsafe.As(ref Unsafe.AsRef(boundingBoxA)); - ref var b = ref Unsafe.As(ref Unsafe.AsRef(boundingBoxB)); + ref var a = ref Unsafe.As(ref Unsafe.AsRef(in boundingBoxA)); + ref var b = ref Unsafe.As(ref Unsafe.AsRef(in boundingBoxB)); Unsafe.SkipInit(out merged); - ref var result = ref Unsafe.As(ref Unsafe.AsRef(merged)); + ref var result = ref Unsafe.As(ref Unsafe.AsRef(in merged)); result.Min = Vector3.Min(a.Min, b.Min); result.Max = Vector3.Max(a.Max, b.Max); } From 82067aed82c31e1230230d677172340be409fee1 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 14 Nov 2023 16:16:08 -0600 Subject: [PATCH 832/947] Workflow update. --- .github/workflows/dotnet-core.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/dotnet-core.yml b/.github/workflows/dotnet-core.yml index a2b1771f3..164d97ad8 100644 --- a/.github/workflows/dotnet-core.yml +++ b/.github/workflows/dotnet-core.yml @@ -11,7 +11,7 @@ jobs: - uses: actions/checkout@v2 - uses: actions/setup-dotnet@v1 with: - dotnet-version: '7.x' + dotnet-version: '8.x' include-prerelease: true - name: Install dependencies run: | From 5c23c164d4dcf1a9e2a195a151b4c86db44b6401 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 14 Nov 2023 16:27:10 -0600 Subject: [PATCH 833/947] Version bump. --- BepuPhysics/BepuPhysics.csproj | 2 +- BepuUtilities/BepuUtilities.csproj | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/BepuPhysics/BepuPhysics.csproj b/BepuPhysics/BepuPhysics.csproj index 6260b7f63..fa15d38d5 100644 --- a/BepuPhysics/BepuPhysics.csproj +++ b/BepuPhysics/BepuPhysics.csproj @@ -1,7 +1,7 @@  net8.0 - 2.5.0-beta.14 + 2.5.0-beta.15 Bepu Entertainment LLC Ross Nordby Speedy real time physics simulation library. diff --git a/BepuUtilities/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj index 4c6277b3a..ec4f3b8cb 100644 --- a/BepuUtilities/BepuUtilities.csproj +++ b/BepuUtilities/BepuUtilities.csproj @@ -3,7 +3,7 @@ BepuUtilities BepuUtilities net8.0 - 2.5.0-beta.14 + 2.5.0-beta.15 Bepu Entertainment LLC Ross Nordby Supporting utilities library for BEPUphysics v2. From 7820d9c1c0e737a11275e70bf0db8605c53c1ed8 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 20 Nov 2023 14:52:58 -0600 Subject: [PATCH 834/947] Fixed some demos build configs. --- DemoTests/DemoTests.csproj | 1 + Demos.sln | 513 +++++++++++++++++-------------------- Demos/Demos.csproj | 1 + 3 files changed, 230 insertions(+), 285 deletions(-) diff --git a/DemoTests/DemoTests.csproj b/DemoTests/DemoTests.csproj index 6e1a6283c..a1be5c215 100644 --- a/DemoTests/DemoTests.csproj +++ b/DemoTests/DemoTests.csproj @@ -5,6 +5,7 @@ true false latest + AnyCPU;x86;x64 diff --git a/Demos.sln b/Demos.sln index 93ca532cf..65e251f2d 100644 --- a/Demos.sln +++ b/Demos.sln @@ -1,285 +1,228 @@ - -Microsoft Visual Studio Solution File, Format Version 12.00 -# Visual Studio Version 17 -VisualStudioVersion = 17.0.31423.177 -MinimumVisualStudioVersion = 10.0.40219.1 -Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "Demos", "Demos\Demos.csproj", "{C4C313CF-0BBD-407F-AC30-C5E889206F55}" - ProjectSection(ProjectDependencies) = postProject - {499C899F-CD56-476E-AFF8-85A8C29B19BF} = {499C899F-CD56-476E-AFF8-85A8C29B19BF} - {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5} = {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5} - EndProjectSection -EndProject -Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "DemoRenderer", "DemoRenderer\DemoRenderer.csproj", "{21058D92-EC74-4F70-98FF-D3D7D02A537E}" - ProjectSection(ProjectDependencies) = postProject - {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5} = {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5} - EndProjectSection -EndProject -Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "DemoContentLoader", "DemoContentLoader\DemoContentLoader.csproj", "{FABD2BE3-697B-4B57-85D0-1077A3198C5C}" -EndProject -Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "DemoUtilities", "DemoUtilities\DemoUtilities.csproj", "{499C899F-CD56-476E-AFF8-85A8C29B19BF}" -EndProject -Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "DemoContentBuilder", "DemoContentBuilder\DemoContentBuilder.csproj", "{6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}" -EndProject -Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "BepuUtilities", "BepuUtilities\BepuUtilities.csproj", "{8D3FB6BE-2726-4479-8AF2-13F593314AC0}" -EndProject -Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "BepuPhysics", "BepuPhysics\BepuPhysics.csproj", "{5FBC743A-8911-4DE6-B136-C0B274E1B185}" -EndProject -Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "DemoTests", "DemoTests\DemoTests.csproj", "{32BABF14-6971-41F8-A556-8E0F2D8C86B2}" -EndProject -Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "DemoBenchmarks", "DemoBenchmarks\DemoBenchmarks.csproj", "{EA4ED604-6F12-42E6-8A0C-FC102B7D227B}" -EndProject -Global - GlobalSection(SolutionConfigurationPlatforms) = preSolution - Debug|Any CPU = Debug|Any CPU - Debug|ARM = Debug|ARM - Debug|x64 = Debug|x64 - Debug|x86 = Debug|x86 - Release|Any CPU = Release|Any CPU - Release|ARM = Release|ARM - Release|x64 = Release|x64 - Release|x86 = Release|x86 - ReleaseNoProfiling|Any CPU = ReleaseNoProfiling|Any CPU - ReleaseNoProfiling|ARM = ReleaseNoProfiling|ARM - ReleaseNoProfiling|x64 = ReleaseNoProfiling|x64 - ReleaseNoProfiling|x86 = ReleaseNoProfiling|x86 - EndGlobalSection - GlobalSection(ProjectConfigurationPlatforms) = postSolution - {C4C313CF-0BBD-407F-AC30-C5E889206F55}.Debug|Any CPU.ActiveCfg = Debug|Any CPU - {C4C313CF-0BBD-407F-AC30-C5E889206F55}.Debug|Any CPU.Build.0 = Debug|Any CPU - {C4C313CF-0BBD-407F-AC30-C5E889206F55}.Debug|ARM.ActiveCfg = Debug|Any CPU - {C4C313CF-0BBD-407F-AC30-C5E889206F55}.Debug|ARM.Build.0 = Debug|Any CPU - {C4C313CF-0BBD-407F-AC30-C5E889206F55}.Debug|x64.ActiveCfg = Debug|Any CPU - {C4C313CF-0BBD-407F-AC30-C5E889206F55}.Debug|x64.Build.0 = Debug|Any CPU - {C4C313CF-0BBD-407F-AC30-C5E889206F55}.Debug|x86.ActiveCfg = Debug|Any CPU - {C4C313CF-0BBD-407F-AC30-C5E889206F55}.Debug|x86.Build.0 = Debug|Any CPU - {C4C313CF-0BBD-407F-AC30-C5E889206F55}.Release|Any CPU.ActiveCfg = Release|Any CPU - {C4C313CF-0BBD-407F-AC30-C5E889206F55}.Release|Any CPU.Build.0 = Release|Any CPU - {C4C313CF-0BBD-407F-AC30-C5E889206F55}.Release|ARM.ActiveCfg = Release|Any CPU - {C4C313CF-0BBD-407F-AC30-C5E889206F55}.Release|ARM.Build.0 = Release|Any CPU - {C4C313CF-0BBD-407F-AC30-C5E889206F55}.Release|x64.ActiveCfg = Release|Any CPU - {C4C313CF-0BBD-407F-AC30-C5E889206F55}.Release|x64.Build.0 = Release|Any CPU - {C4C313CF-0BBD-407F-AC30-C5E889206F55}.Release|x86.ActiveCfg = Release|Any CPU - {C4C313CF-0BBD-407F-AC30-C5E889206F55}.Release|x86.Build.0 = Release|Any CPU - {C4C313CF-0BBD-407F-AC30-C5E889206F55}.ReleaseNoProfiling|Any CPU.ActiveCfg = Release|Any CPU - {C4C313CF-0BBD-407F-AC30-C5E889206F55}.ReleaseNoProfiling|Any CPU.Build.0 = Release|Any CPU - {C4C313CF-0BBD-407F-AC30-C5E889206F55}.ReleaseNoProfiling|ARM.ActiveCfg = Release|Any CPU - {C4C313CF-0BBD-407F-AC30-C5E889206F55}.ReleaseNoProfiling|ARM.Build.0 = Release|Any CPU - {C4C313CF-0BBD-407F-AC30-C5E889206F55}.ReleaseNoProfiling|x64.ActiveCfg = Release|Any CPU - {C4C313CF-0BBD-407F-AC30-C5E889206F55}.ReleaseNoProfiling|x64.Build.0 = Release|Any CPU - {C4C313CF-0BBD-407F-AC30-C5E889206F55}.ReleaseNoProfiling|x86.ActiveCfg = Release|Any CPU - {C4C313CF-0BBD-407F-AC30-C5E889206F55}.ReleaseNoProfiling|x86.Build.0 = Release|Any CPU - {21058D92-EC74-4F70-98FF-D3D7D02A537E}.Debug|Any CPU.ActiveCfg = Debug|Any CPU - {21058D92-EC74-4F70-98FF-D3D7D02A537E}.Debug|Any CPU.Build.0 = Debug|Any CPU - {21058D92-EC74-4F70-98FF-D3D7D02A537E}.Debug|ARM.ActiveCfg = Debug|Any CPU - {21058D92-EC74-4F70-98FF-D3D7D02A537E}.Debug|ARM.Build.0 = Debug|Any CPU - {21058D92-EC74-4F70-98FF-D3D7D02A537E}.Debug|x64.ActiveCfg = Debug|Any CPU - {21058D92-EC74-4F70-98FF-D3D7D02A537E}.Debug|x64.Build.0 = Debug|Any CPU - {21058D92-EC74-4F70-98FF-D3D7D02A537E}.Debug|x86.ActiveCfg = Debug|Any CPU - {21058D92-EC74-4F70-98FF-D3D7D02A537E}.Debug|x86.Build.0 = Debug|Any CPU - {21058D92-EC74-4F70-98FF-D3D7D02A537E}.Release|Any CPU.ActiveCfg = Release|Any CPU - {21058D92-EC74-4F70-98FF-D3D7D02A537E}.Release|Any CPU.Build.0 = Release|Any CPU - {21058D92-EC74-4F70-98FF-D3D7D02A537E}.Release|ARM.ActiveCfg = Release|Any CPU - {21058D92-EC74-4F70-98FF-D3D7D02A537E}.Release|ARM.Build.0 = Release|Any CPU - {21058D92-EC74-4F70-98FF-D3D7D02A537E}.Release|x64.ActiveCfg = Release|Any CPU - {21058D92-EC74-4F70-98FF-D3D7D02A537E}.Release|x64.Build.0 = Release|Any CPU - {21058D92-EC74-4F70-98FF-D3D7D02A537E}.Release|x86.ActiveCfg = Release|Any CPU - {21058D92-EC74-4F70-98FF-D3D7D02A537E}.Release|x86.Build.0 = Release|Any CPU - {21058D92-EC74-4F70-98FF-D3D7D02A537E}.ReleaseNoProfiling|Any CPU.ActiveCfg = Release|Any CPU - {21058D92-EC74-4F70-98FF-D3D7D02A537E}.ReleaseNoProfiling|Any CPU.Build.0 = Release|Any CPU - {21058D92-EC74-4F70-98FF-D3D7D02A537E}.ReleaseNoProfiling|ARM.ActiveCfg = Release|Any CPU - {21058D92-EC74-4F70-98FF-D3D7D02A537E}.ReleaseNoProfiling|ARM.Build.0 = Release|Any CPU - {21058D92-EC74-4F70-98FF-D3D7D02A537E}.ReleaseNoProfiling|x64.ActiveCfg = Release|Any CPU - {21058D92-EC74-4F70-98FF-D3D7D02A537E}.ReleaseNoProfiling|x64.Build.0 = Release|Any CPU - {21058D92-EC74-4F70-98FF-D3D7D02A537E}.ReleaseNoProfiling|x86.ActiveCfg = Release|Any CPU - {21058D92-EC74-4F70-98FF-D3D7D02A537E}.ReleaseNoProfiling|x86.Build.0 = Release|Any CPU - {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.Debug|Any CPU.ActiveCfg = Debug|Any CPU - {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.Debug|Any CPU.Build.0 = Debug|Any CPU - {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.Debug|ARM.ActiveCfg = Debug|Any CPU - {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.Debug|ARM.Build.0 = Debug|Any CPU - {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.Debug|x64.ActiveCfg = Debug|Any CPU - {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.Debug|x64.Build.0 = Debug|Any CPU - {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.Debug|x86.ActiveCfg = Debug|Any CPU - {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.Debug|x86.Build.0 = Debug|Any CPU - {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.Release|Any CPU.ActiveCfg = Release|Any CPU - {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.Release|Any CPU.Build.0 = Release|Any CPU - {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.Release|ARM.ActiveCfg = Release|Any CPU - {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.Release|ARM.Build.0 = Release|Any CPU - {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.Release|x64.ActiveCfg = Release|Any CPU - {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.Release|x64.Build.0 = Release|Any CPU - {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.Release|x86.ActiveCfg = Release|Any CPU - {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.Release|x86.Build.0 = Release|Any CPU - {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.ReleaseNoProfiling|Any CPU.ActiveCfg = Release|Any CPU - {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.ReleaseNoProfiling|Any CPU.Build.0 = Release|Any CPU - {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.ReleaseNoProfiling|ARM.ActiveCfg = Release|Any CPU - {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.ReleaseNoProfiling|ARM.Build.0 = Release|Any CPU - {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.ReleaseNoProfiling|x64.ActiveCfg = Release|Any CPU - {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.ReleaseNoProfiling|x64.Build.0 = Release|Any CPU - {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.ReleaseNoProfiling|x86.ActiveCfg = Release|Any CPU - {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.ReleaseNoProfiling|x86.Build.0 = Release|Any CPU - {499C899F-CD56-476E-AFF8-85A8C29B19BF}.Debug|Any CPU.ActiveCfg = Debug|Any CPU - {499C899F-CD56-476E-AFF8-85A8C29B19BF}.Debug|Any CPU.Build.0 = Debug|Any CPU - {499C899F-CD56-476E-AFF8-85A8C29B19BF}.Debug|ARM.ActiveCfg = Debug|Any CPU - {499C899F-CD56-476E-AFF8-85A8C29B19BF}.Debug|ARM.Build.0 = Debug|Any CPU - {499C899F-CD56-476E-AFF8-85A8C29B19BF}.Debug|x64.ActiveCfg = Debug|Any CPU - {499C899F-CD56-476E-AFF8-85A8C29B19BF}.Debug|x64.Build.0 = Debug|Any CPU - {499C899F-CD56-476E-AFF8-85A8C29B19BF}.Debug|x86.ActiveCfg = Debug|Any CPU - {499C899F-CD56-476E-AFF8-85A8C29B19BF}.Debug|x86.Build.0 = Debug|Any CPU - {499C899F-CD56-476E-AFF8-85A8C29B19BF}.Release|Any CPU.ActiveCfg = Release|Any CPU - {499C899F-CD56-476E-AFF8-85A8C29B19BF}.Release|Any CPU.Build.0 = Release|Any CPU - {499C899F-CD56-476E-AFF8-85A8C29B19BF}.Release|ARM.ActiveCfg = Release|Any CPU - {499C899F-CD56-476E-AFF8-85A8C29B19BF}.Release|ARM.Build.0 = Release|Any CPU - {499C899F-CD56-476E-AFF8-85A8C29B19BF}.Release|x64.ActiveCfg = Release|Any CPU - {499C899F-CD56-476E-AFF8-85A8C29B19BF}.Release|x64.Build.0 = Release|Any CPU - {499C899F-CD56-476E-AFF8-85A8C29B19BF}.Release|x86.ActiveCfg = Release|Any CPU - {499C899F-CD56-476E-AFF8-85A8C29B19BF}.Release|x86.Build.0 = Release|Any CPU - {499C899F-CD56-476E-AFF8-85A8C29B19BF}.ReleaseNoProfiling|Any CPU.ActiveCfg = Release|Any CPU - {499C899F-CD56-476E-AFF8-85A8C29B19BF}.ReleaseNoProfiling|Any CPU.Build.0 = Release|Any CPU - {499C899F-CD56-476E-AFF8-85A8C29B19BF}.ReleaseNoProfiling|ARM.ActiveCfg = Release|Any CPU - {499C899F-CD56-476E-AFF8-85A8C29B19BF}.ReleaseNoProfiling|ARM.Build.0 = Release|Any CPU - {499C899F-CD56-476E-AFF8-85A8C29B19BF}.ReleaseNoProfiling|x64.ActiveCfg = Release|Any CPU - {499C899F-CD56-476E-AFF8-85A8C29B19BF}.ReleaseNoProfiling|x64.Build.0 = Release|Any CPU - {499C899F-CD56-476E-AFF8-85A8C29B19BF}.ReleaseNoProfiling|x86.ActiveCfg = Release|Any CPU - {499C899F-CD56-476E-AFF8-85A8C29B19BF}.ReleaseNoProfiling|x86.Build.0 = Release|Any CPU - {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.Debug|Any CPU.ActiveCfg = Debug|x64 - {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.Debug|Any CPU.Build.0 = Debug|x64 - {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.Debug|ARM.ActiveCfg = Debug|x64 - {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.Debug|ARM.Build.0 = Debug|x64 - {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.Debug|x64.ActiveCfg = Debug|x64 - {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.Debug|x64.Build.0 = Debug|x64 - {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.Debug|x86.ActiveCfg = Debug|x64 - {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.Debug|x86.Build.0 = Debug|x64 - {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.Release|Any CPU.ActiveCfg = Release|x64 - {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.Release|Any CPU.Build.0 = Release|x64 - {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.Release|ARM.ActiveCfg = Release|x64 - {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.Release|ARM.Build.0 = Release|x64 - {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.Release|x64.ActiveCfg = Release|x64 - {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.Release|x64.Build.0 = Release|x64 - {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.Release|x86.ActiveCfg = Release|x64 - {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.Release|x86.Build.0 = Release|x64 - {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.ReleaseNoProfiling|Any CPU.ActiveCfg = Release|x64 - {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.ReleaseNoProfiling|Any CPU.Build.0 = Release|x64 - {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.ReleaseNoProfiling|ARM.ActiveCfg = Release|x64 - {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.ReleaseNoProfiling|ARM.Build.0 = Release|x64 - {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.ReleaseNoProfiling|x64.ActiveCfg = Release|x64 - {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.ReleaseNoProfiling|x64.Build.0 = Release|x64 - {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.ReleaseNoProfiling|x86.ActiveCfg = Release|x64 - {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.ReleaseNoProfiling|x86.Build.0 = Release|x64 - {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.Debug|Any CPU.ActiveCfg = Debug|Any CPU - {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.Debug|Any CPU.Build.0 = Debug|Any CPU - {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.Debug|ARM.ActiveCfg = Debug|Any CPU - {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.Debug|ARM.Build.0 = Debug|Any CPU - {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.Debug|x64.ActiveCfg = Debug|Any CPU - {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.Debug|x64.Build.0 = Debug|Any CPU - {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.Debug|x86.ActiveCfg = Debug|Any CPU - {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.Debug|x86.Build.0 = Debug|Any CPU - {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.Release|Any CPU.ActiveCfg = Release|Any CPU - {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.Release|Any CPU.Build.0 = Release|Any CPU - {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.Release|ARM.ActiveCfg = Release|Any CPU - {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.Release|ARM.Build.0 = Release|Any CPU - {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.Release|x64.ActiveCfg = Release|Any CPU - {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.Release|x64.Build.0 = Release|Any CPU - {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.Release|x86.ActiveCfg = Release|Any CPU - {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.Release|x86.Build.0 = Release|Any CPU - {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.ReleaseNoProfiling|Any CPU.ActiveCfg = Release|Any CPU - {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.ReleaseNoProfiling|Any CPU.Build.0 = Release|Any CPU - {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.ReleaseNoProfiling|ARM.ActiveCfg = Release|Any CPU - {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.ReleaseNoProfiling|ARM.Build.0 = Release|Any CPU - {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.ReleaseNoProfiling|x64.ActiveCfg = Release|Any CPU - {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.ReleaseNoProfiling|x64.Build.0 = Release|Any CPU - {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.ReleaseNoProfiling|x86.ActiveCfg = Release|Any CPU - {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.ReleaseNoProfiling|x86.Build.0 = Release|Any CPU - {5FBC743A-8911-4DE6-B136-C0B274E1B185}.Debug|Any CPU.ActiveCfg = Debug|Any CPU - {5FBC743A-8911-4DE6-B136-C0B274E1B185}.Debug|Any CPU.Build.0 = Debug|Any CPU - {5FBC743A-8911-4DE6-B136-C0B274E1B185}.Debug|ARM.ActiveCfg = Debug|Any CPU - {5FBC743A-8911-4DE6-B136-C0B274E1B185}.Debug|ARM.Build.0 = Debug|Any CPU - {5FBC743A-8911-4DE6-B136-C0B274E1B185}.Debug|x64.ActiveCfg = Debug|Any CPU - {5FBC743A-8911-4DE6-B136-C0B274E1B185}.Debug|x64.Build.0 = Debug|Any CPU - {5FBC743A-8911-4DE6-B136-C0B274E1B185}.Debug|x86.ActiveCfg = Debug|Any CPU - {5FBC743A-8911-4DE6-B136-C0B274E1B185}.Debug|x86.Build.0 = Debug|Any CPU - {5FBC743A-8911-4DE6-B136-C0B274E1B185}.Release|Any CPU.ActiveCfg = Release|Any CPU - {5FBC743A-8911-4DE6-B136-C0B274E1B185}.Release|Any CPU.Build.0 = Release|Any CPU - {5FBC743A-8911-4DE6-B136-C0B274E1B185}.Release|ARM.ActiveCfg = Release|Any CPU - {5FBC743A-8911-4DE6-B136-C0B274E1B185}.Release|ARM.Build.0 = Release|Any CPU - {5FBC743A-8911-4DE6-B136-C0B274E1B185}.Release|x64.ActiveCfg = Release|Any CPU - {5FBC743A-8911-4DE6-B136-C0B274E1B185}.Release|x64.Build.0 = Release|Any CPU - {5FBC743A-8911-4DE6-B136-C0B274E1B185}.Release|x86.ActiveCfg = Release|Any CPU - {5FBC743A-8911-4DE6-B136-C0B274E1B185}.Release|x86.Build.0 = Release|Any CPU - {5FBC743A-8911-4DE6-B136-C0B274E1B185}.ReleaseNoProfiling|Any CPU.ActiveCfg = ReleaseNoProfiling|Any CPU - {5FBC743A-8911-4DE6-B136-C0B274E1B185}.ReleaseNoProfiling|Any CPU.Build.0 = ReleaseNoProfiling|Any CPU - {5FBC743A-8911-4DE6-B136-C0B274E1B185}.ReleaseNoProfiling|ARM.ActiveCfg = ReleaseNoProfiling|Any CPU - {5FBC743A-8911-4DE6-B136-C0B274E1B185}.ReleaseNoProfiling|ARM.Build.0 = ReleaseNoProfiling|Any CPU - {5FBC743A-8911-4DE6-B136-C0B274E1B185}.ReleaseNoProfiling|x64.ActiveCfg = ReleaseNoProfiling|Any CPU - {5FBC743A-8911-4DE6-B136-C0B274E1B185}.ReleaseNoProfiling|x64.Build.0 = ReleaseNoProfiling|Any CPU - {5FBC743A-8911-4DE6-B136-C0B274E1B185}.ReleaseNoProfiling|x86.ActiveCfg = ReleaseNoProfiling|Any CPU - {5FBC743A-8911-4DE6-B136-C0B274E1B185}.ReleaseNoProfiling|x86.Build.0 = ReleaseNoProfiling|Any CPU - {32BABF14-6971-41F8-A556-8E0F2D8C86B2}.Debug|Any CPU.ActiveCfg = Debug|Any CPU - {32BABF14-6971-41F8-A556-8E0F2D8C86B2}.Debug|Any CPU.Build.0 = Debug|Any CPU - {32BABF14-6971-41F8-A556-8E0F2D8C86B2}.Debug|ARM.ActiveCfg = Debug|Any CPU - {32BABF14-6971-41F8-A556-8E0F2D8C86B2}.Debug|ARM.Build.0 = Debug|Any CPU - {32BABF14-6971-41F8-A556-8E0F2D8C86B2}.Debug|x64.ActiveCfg = Debug|Any CPU - {32BABF14-6971-41F8-A556-8E0F2D8C86B2}.Debug|x64.Build.0 = Debug|Any CPU - {32BABF14-6971-41F8-A556-8E0F2D8C86B2}.Debug|x86.ActiveCfg = Debug|Any CPU - {32BABF14-6971-41F8-A556-8E0F2D8C86B2}.Debug|x86.Build.0 = Debug|Any CPU - {32BABF14-6971-41F8-A556-8E0F2D8C86B2}.Release|Any CPU.ActiveCfg = Release|Any CPU - {32BABF14-6971-41F8-A556-8E0F2D8C86B2}.Release|Any CPU.Build.0 = Release|Any CPU - {32BABF14-6971-41F8-A556-8E0F2D8C86B2}.Release|ARM.ActiveCfg = Release|Any CPU - {32BABF14-6971-41F8-A556-8E0F2D8C86B2}.Release|ARM.Build.0 = Release|Any CPU - {32BABF14-6971-41F8-A556-8E0F2D8C86B2}.Release|x64.ActiveCfg = Release|Any CPU - {32BABF14-6971-41F8-A556-8E0F2D8C86B2}.Release|x64.Build.0 = Release|Any CPU - {32BABF14-6971-41F8-A556-8E0F2D8C86B2}.Release|x86.ActiveCfg = Release|Any CPU - {32BABF14-6971-41F8-A556-8E0F2D8C86B2}.Release|x86.Build.0 = Release|Any CPU - {32BABF14-6971-41F8-A556-8E0F2D8C86B2}.ReleaseNoProfiling|Any CPU.ActiveCfg = Release|Any CPU - {32BABF14-6971-41F8-A556-8E0F2D8C86B2}.ReleaseNoProfiling|Any CPU.Build.0 = Release|Any CPU - {32BABF14-6971-41F8-A556-8E0F2D8C86B2}.ReleaseNoProfiling|ARM.ActiveCfg = Release|Any CPU - {32BABF14-6971-41F8-A556-8E0F2D8C86B2}.ReleaseNoProfiling|ARM.Build.0 = Release|Any CPU - {32BABF14-6971-41F8-A556-8E0F2D8C86B2}.ReleaseNoProfiling|x64.ActiveCfg = Release|Any CPU - {32BABF14-6971-41F8-A556-8E0F2D8C86B2}.ReleaseNoProfiling|x64.Build.0 = Release|Any CPU - {32BABF14-6971-41F8-A556-8E0F2D8C86B2}.ReleaseNoProfiling|x86.ActiveCfg = Release|Any CPU - {32BABF14-6971-41F8-A556-8E0F2D8C86B2}.ReleaseNoProfiling|x86.Build.0 = Release|Any CPU - {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.Debug|Any CPU.ActiveCfg = Debug|Any CPU - {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.Debug|Any CPU.Build.0 = Debug|Any CPU - {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.Debug|ARM.ActiveCfg = Debug|Any CPU - {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.Debug|ARM.Build.0 = Debug|Any CPU - {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.Debug|x64.ActiveCfg = Debug|Any CPU - {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.Debug|x64.Build.0 = Debug|Any CPU - {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.Debug|x86.ActiveCfg = Debug|Any CPU - {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.Debug|x86.Build.0 = Debug|Any CPU - {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.Release|Any CPU.ActiveCfg = Release|Any CPU - {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.Release|Any CPU.Build.0 = Release|Any CPU - {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.Release|ARM.ActiveCfg = Release|Any CPU - {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.Release|ARM.Build.0 = Release|Any CPU - {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.Release|x64.ActiveCfg = Release|Any CPU - {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.Release|x64.Build.0 = Release|Any CPU - {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.Release|x86.ActiveCfg = Release|Any CPU - {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.Release|x86.Build.0 = Release|Any CPU - {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.ReleaseNoProfiling|Any CPU.ActiveCfg = Release|Any CPU - {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.ReleaseNoProfiling|Any CPU.Build.0 = Release|Any CPU - {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.ReleaseNoProfiling|ARM.ActiveCfg = Release|Any CPU - {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.ReleaseNoProfiling|ARM.Build.0 = Release|Any CPU - {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.ReleaseNoProfiling|x64.ActiveCfg = Release|Any CPU - {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.ReleaseNoProfiling|x64.Build.0 = Release|Any CPU - {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.ReleaseNoProfiling|x86.ActiveCfg = Release|Any CPU - {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.ReleaseNoProfiling|x86.Build.0 = Release|Any CPU - EndGlobalSection - GlobalSection(SolutionProperties) = preSolution - HideSolutionNode = FALSE - EndGlobalSection - GlobalSection(ExtensibilityGlobals) = postSolution - SolutionGuid = {29758AC0-E221-4C61-AC3D-16DD9A722844} - EndGlobalSection - GlobalSection(Performance) = preSolution - HasPerformanceSessions = true - EndGlobalSection - GlobalSection(Performance) = preSolution - HasPerformanceSessions = true - EndGlobalSection - GlobalSection(Performance) = preSolution - HasPerformanceSessions = true - EndGlobalSection - GlobalSection(Performance) = preSolution - HasPerformanceSessions = true - EndGlobalSection - GlobalSection(Performance) = preSolution - HasPerformanceSessions = true - EndGlobalSection -EndGlobal + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Version 17 +VisualStudioVersion = 17.0.31423.177 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "Demos", "Demos\Demos.csproj", "{C4C313CF-0BBD-407F-AC30-C5E889206F55}" + ProjectSection(ProjectDependencies) = postProject + {499C899F-CD56-476E-AFF8-85A8C29B19BF} = {499C899F-CD56-476E-AFF8-85A8C29B19BF} + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5} = {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5} + EndProjectSection +EndProject +Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "DemoRenderer", "DemoRenderer\DemoRenderer.csproj", "{21058D92-EC74-4F70-98FF-D3D7D02A537E}" + ProjectSection(ProjectDependencies) = postProject + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5} = {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5} + EndProjectSection +EndProject +Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "DemoContentLoader", "DemoContentLoader\DemoContentLoader.csproj", "{FABD2BE3-697B-4B57-85D0-1077A3198C5C}" +EndProject +Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "DemoUtilities", "DemoUtilities\DemoUtilities.csproj", "{499C899F-CD56-476E-AFF8-85A8C29B19BF}" +EndProject +Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "DemoContentBuilder", "DemoContentBuilder\DemoContentBuilder.csproj", "{6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}" +EndProject +Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "BepuUtilities", "BepuUtilities\BepuUtilities.csproj", "{8D3FB6BE-2726-4479-8AF2-13F593314AC0}" +EndProject +Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "BepuPhysics", "BepuPhysics\BepuPhysics.csproj", "{5FBC743A-8911-4DE6-B136-C0B274E1B185}" +EndProject +Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "DemoTests", "DemoTests\DemoTests.csproj", "{32BABF14-6971-41F8-A556-8E0F2D8C86B2}" +EndProject +Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "DemoBenchmarks", "DemoBenchmarks\DemoBenchmarks.csproj", "{EA4ED604-6F12-42E6-8A0C-FC102B7D227B}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Debug|x64 = Debug|x64 + Debug|x86 = Debug|x86 + Release|Any CPU = Release|Any CPU + Release|x64 = Release|x64 + Release|x86 = Release|x86 + ReleaseNoProfiling|Any CPU = ReleaseNoProfiling|Any CPU + ReleaseNoProfiling|x64 = ReleaseNoProfiling|x64 + ReleaseNoProfiling|x86 = ReleaseNoProfiling|x86 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {C4C313CF-0BBD-407F-AC30-C5E889206F55}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {C4C313CF-0BBD-407F-AC30-C5E889206F55}.Debug|Any CPU.Build.0 = Debug|Any CPU + {C4C313CF-0BBD-407F-AC30-C5E889206F55}.Debug|x64.ActiveCfg = Debug|x64 + {C4C313CF-0BBD-407F-AC30-C5E889206F55}.Debug|x64.Build.0 = Debug|x64 + {C4C313CF-0BBD-407F-AC30-C5E889206F55}.Debug|x86.ActiveCfg = Debug|x86 + {C4C313CF-0BBD-407F-AC30-C5E889206F55}.Debug|x86.Build.0 = Debug|x86 + {C4C313CF-0BBD-407F-AC30-C5E889206F55}.Release|Any CPU.ActiveCfg = Release|Any CPU + {C4C313CF-0BBD-407F-AC30-C5E889206F55}.Release|Any CPU.Build.0 = Release|Any CPU + {C4C313CF-0BBD-407F-AC30-C5E889206F55}.Release|x64.ActiveCfg = Release|x64 + {C4C313CF-0BBD-407F-AC30-C5E889206F55}.Release|x64.Build.0 = Release|x64 + {C4C313CF-0BBD-407F-AC30-C5E889206F55}.Release|x86.ActiveCfg = Release|x86 + {C4C313CF-0BBD-407F-AC30-C5E889206F55}.Release|x86.Build.0 = Release|x86 + {C4C313CF-0BBD-407F-AC30-C5E889206F55}.ReleaseNoProfiling|Any CPU.ActiveCfg = Release|Any CPU + {C4C313CF-0BBD-407F-AC30-C5E889206F55}.ReleaseNoProfiling|Any CPU.Build.0 = Release|Any CPU + {C4C313CF-0BBD-407F-AC30-C5E889206F55}.ReleaseNoProfiling|x64.ActiveCfg = Release|x64 + {C4C313CF-0BBD-407F-AC30-C5E889206F55}.ReleaseNoProfiling|x64.Build.0 = Release|x64 + {C4C313CF-0BBD-407F-AC30-C5E889206F55}.ReleaseNoProfiling|x86.ActiveCfg = Release|x86 + {C4C313CF-0BBD-407F-AC30-C5E889206F55}.ReleaseNoProfiling|x86.Build.0 = Release|x86 + {21058D92-EC74-4F70-98FF-D3D7D02A537E}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {21058D92-EC74-4F70-98FF-D3D7D02A537E}.Debug|Any CPU.Build.0 = Debug|Any CPU + {21058D92-EC74-4F70-98FF-D3D7D02A537E}.Debug|x64.ActiveCfg = Debug|Any CPU + {21058D92-EC74-4F70-98FF-D3D7D02A537E}.Debug|x64.Build.0 = Debug|Any CPU + {21058D92-EC74-4F70-98FF-D3D7D02A537E}.Debug|x86.ActiveCfg = Debug|Any CPU + {21058D92-EC74-4F70-98FF-D3D7D02A537E}.Debug|x86.Build.0 = Debug|Any CPU + {21058D92-EC74-4F70-98FF-D3D7D02A537E}.Release|Any CPU.ActiveCfg = Release|Any CPU + {21058D92-EC74-4F70-98FF-D3D7D02A537E}.Release|Any CPU.Build.0 = Release|Any CPU + {21058D92-EC74-4F70-98FF-D3D7D02A537E}.Release|x64.ActiveCfg = Release|Any CPU + {21058D92-EC74-4F70-98FF-D3D7D02A537E}.Release|x64.Build.0 = Release|Any CPU + {21058D92-EC74-4F70-98FF-D3D7D02A537E}.Release|x86.ActiveCfg = Release|Any CPU + {21058D92-EC74-4F70-98FF-D3D7D02A537E}.Release|x86.Build.0 = Release|Any CPU + {21058D92-EC74-4F70-98FF-D3D7D02A537E}.ReleaseNoProfiling|Any CPU.ActiveCfg = Release|Any CPU + {21058D92-EC74-4F70-98FF-D3D7D02A537E}.ReleaseNoProfiling|Any CPU.Build.0 = Release|Any CPU + {21058D92-EC74-4F70-98FF-D3D7D02A537E}.ReleaseNoProfiling|x64.ActiveCfg = Release|Any CPU + {21058D92-EC74-4F70-98FF-D3D7D02A537E}.ReleaseNoProfiling|x64.Build.0 = Release|Any CPU + {21058D92-EC74-4F70-98FF-D3D7D02A537E}.ReleaseNoProfiling|x86.ActiveCfg = Release|Any CPU + {21058D92-EC74-4F70-98FF-D3D7D02A537E}.ReleaseNoProfiling|x86.Build.0 = Release|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.Debug|Any CPU.Build.0 = Debug|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.Debug|x64.ActiveCfg = Debug|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.Debug|x64.Build.0 = Debug|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.Debug|x86.ActiveCfg = Debug|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.Debug|x86.Build.0 = Debug|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.Release|Any CPU.ActiveCfg = Release|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.Release|Any CPU.Build.0 = Release|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.Release|x64.ActiveCfg = Release|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.Release|x64.Build.0 = Release|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.Release|x86.ActiveCfg = Release|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.Release|x86.Build.0 = Release|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.ReleaseNoProfiling|Any CPU.ActiveCfg = Release|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.ReleaseNoProfiling|Any CPU.Build.0 = Release|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.ReleaseNoProfiling|x64.ActiveCfg = Release|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.ReleaseNoProfiling|x64.Build.0 = Release|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.ReleaseNoProfiling|x86.ActiveCfg = Release|Any CPU + {FABD2BE3-697B-4B57-85D0-1077A3198C5C}.ReleaseNoProfiling|x86.Build.0 = Release|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.Debug|Any CPU.Build.0 = Debug|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.Debug|x64.ActiveCfg = Debug|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.Debug|x64.Build.0 = Debug|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.Debug|x86.ActiveCfg = Debug|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.Debug|x86.Build.0 = Debug|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.Release|Any CPU.ActiveCfg = Release|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.Release|Any CPU.Build.0 = Release|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.Release|x64.ActiveCfg = Release|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.Release|x64.Build.0 = Release|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.Release|x86.ActiveCfg = Release|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.Release|x86.Build.0 = Release|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.ReleaseNoProfiling|Any CPU.ActiveCfg = Release|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.ReleaseNoProfiling|Any CPU.Build.0 = Release|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.ReleaseNoProfiling|x64.ActiveCfg = Release|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.ReleaseNoProfiling|x64.Build.0 = Release|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.ReleaseNoProfiling|x86.ActiveCfg = Release|Any CPU + {499C899F-CD56-476E-AFF8-85A8C29B19BF}.ReleaseNoProfiling|x86.Build.0 = Release|Any CPU + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.Debug|Any CPU.ActiveCfg = Debug|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.Debug|Any CPU.Build.0 = Debug|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.Debug|x64.ActiveCfg = Debug|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.Debug|x64.Build.0 = Debug|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.Debug|x86.ActiveCfg = Debug|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.Debug|x86.Build.0 = Debug|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.Release|Any CPU.ActiveCfg = Release|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.Release|Any CPU.Build.0 = Release|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.Release|x64.ActiveCfg = Release|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.Release|x64.Build.0 = Release|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.Release|x86.ActiveCfg = Release|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.Release|x86.Build.0 = Release|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.ReleaseNoProfiling|Any CPU.ActiveCfg = Release|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.ReleaseNoProfiling|Any CPU.Build.0 = Release|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.ReleaseNoProfiling|x64.ActiveCfg = Release|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.ReleaseNoProfiling|x64.Build.0 = Release|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.ReleaseNoProfiling|x86.ActiveCfg = Release|x64 + {6F7900A8-6B1A-41BB-BB2F-0348A527A2F5}.ReleaseNoProfiling|x86.Build.0 = Release|x64 + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.Debug|Any CPU.Build.0 = Debug|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.Debug|x64.ActiveCfg = Debug|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.Debug|x64.Build.0 = Debug|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.Debug|x86.ActiveCfg = Debug|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.Debug|x86.Build.0 = Debug|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.Release|Any CPU.ActiveCfg = Release|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.Release|Any CPU.Build.0 = Release|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.Release|x64.ActiveCfg = Release|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.Release|x64.Build.0 = Release|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.Release|x86.ActiveCfg = Release|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.Release|x86.Build.0 = Release|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.ReleaseNoProfiling|Any CPU.ActiveCfg = Release|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.ReleaseNoProfiling|Any CPU.Build.0 = Release|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.ReleaseNoProfiling|x64.ActiveCfg = Release|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.ReleaseNoProfiling|x64.Build.0 = Release|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.ReleaseNoProfiling|x86.ActiveCfg = Release|Any CPU + {8D3FB6BE-2726-4479-8AF2-13F593314AC0}.ReleaseNoProfiling|x86.Build.0 = Release|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.Debug|Any CPU.Build.0 = Debug|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.Debug|x64.ActiveCfg = Debug|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.Debug|x64.Build.0 = Debug|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.Debug|x86.ActiveCfg = Debug|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.Debug|x86.Build.0 = Debug|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.Release|Any CPU.ActiveCfg = Release|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.Release|Any CPU.Build.0 = Release|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.Release|x64.ActiveCfg = Release|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.Release|x64.Build.0 = Release|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.Release|x86.ActiveCfg = Release|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.Release|x86.Build.0 = Release|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.ReleaseNoProfiling|Any CPU.ActiveCfg = ReleaseNoProfiling|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.ReleaseNoProfiling|Any CPU.Build.0 = ReleaseNoProfiling|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.ReleaseNoProfiling|x64.ActiveCfg = ReleaseNoProfiling|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.ReleaseNoProfiling|x64.Build.0 = ReleaseNoProfiling|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.ReleaseNoProfiling|x86.ActiveCfg = ReleaseNoProfiling|Any CPU + {5FBC743A-8911-4DE6-B136-C0B274E1B185}.ReleaseNoProfiling|x86.Build.0 = ReleaseNoProfiling|Any CPU + {32BABF14-6971-41F8-A556-8E0F2D8C86B2}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {32BABF14-6971-41F8-A556-8E0F2D8C86B2}.Debug|Any CPU.Build.0 = Debug|Any CPU + {32BABF14-6971-41F8-A556-8E0F2D8C86B2}.Debug|x64.ActiveCfg = Debug|x64 + {32BABF14-6971-41F8-A556-8E0F2D8C86B2}.Debug|x64.Build.0 = Debug|x64 + {32BABF14-6971-41F8-A556-8E0F2D8C86B2}.Debug|x86.ActiveCfg = Debug|x86 + {32BABF14-6971-41F8-A556-8E0F2D8C86B2}.Debug|x86.Build.0 = Debug|x86 + {32BABF14-6971-41F8-A556-8E0F2D8C86B2}.Release|Any CPU.ActiveCfg = Release|Any CPU + {32BABF14-6971-41F8-A556-8E0F2D8C86B2}.Release|Any CPU.Build.0 = Release|Any CPU + {32BABF14-6971-41F8-A556-8E0F2D8C86B2}.Release|x64.ActiveCfg = Release|x64 + {32BABF14-6971-41F8-A556-8E0F2D8C86B2}.Release|x64.Build.0 = Release|x64 + {32BABF14-6971-41F8-A556-8E0F2D8C86B2}.Release|x86.ActiveCfg = Release|x86 + {32BABF14-6971-41F8-A556-8E0F2D8C86B2}.Release|x86.Build.0 = Release|x86 + {32BABF14-6971-41F8-A556-8E0F2D8C86B2}.ReleaseNoProfiling|Any CPU.ActiveCfg = Release|Any CPU + {32BABF14-6971-41F8-A556-8E0F2D8C86B2}.ReleaseNoProfiling|Any CPU.Build.0 = Release|Any CPU + {32BABF14-6971-41F8-A556-8E0F2D8C86B2}.ReleaseNoProfiling|x64.ActiveCfg = Release|x64 + {32BABF14-6971-41F8-A556-8E0F2D8C86B2}.ReleaseNoProfiling|x64.Build.0 = Release|x64 + {32BABF14-6971-41F8-A556-8E0F2D8C86B2}.ReleaseNoProfiling|x86.ActiveCfg = Release|x86 + {32BABF14-6971-41F8-A556-8E0F2D8C86B2}.ReleaseNoProfiling|x86.Build.0 = Release|x86 + {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.Debug|Any CPU.Build.0 = Debug|Any CPU + {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.Debug|x64.ActiveCfg = Debug|Any CPU + {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.Debug|x64.Build.0 = Debug|Any CPU + {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.Debug|x86.ActiveCfg = Debug|Any CPU + {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.Debug|x86.Build.0 = Debug|Any CPU + {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.Release|Any CPU.ActiveCfg = Release|Any CPU + {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.Release|Any CPU.Build.0 = Release|Any CPU + {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.Release|x64.ActiveCfg = Release|Any CPU + {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.Release|x64.Build.0 = Release|Any CPU + {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.Release|x86.ActiveCfg = Release|Any CPU + {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.Release|x86.Build.0 = Release|Any CPU + {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.ReleaseNoProfiling|Any CPU.ActiveCfg = Release|Any CPU + {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.ReleaseNoProfiling|Any CPU.Build.0 = Release|Any CPU + {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.ReleaseNoProfiling|x64.ActiveCfg = Release|Any CPU + {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.ReleaseNoProfiling|x64.Build.0 = Release|Any CPU + {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.ReleaseNoProfiling|x86.ActiveCfg = Release|Any CPU + {EA4ED604-6F12-42E6-8A0C-FC102B7D227B}.ReleaseNoProfiling|x86.Build.0 = Release|Any CPU + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection + GlobalSection(ExtensibilityGlobals) = postSolution + SolutionGuid = {29758AC0-E221-4C61-AC3D-16DD9A722844} + EndGlobalSection + GlobalSection(Performance) = preSolution + HasPerformanceSessions = true + EndGlobalSection + GlobalSection(Performance) = preSolution + HasPerformanceSessions = true + EndGlobalSection + GlobalSection(Performance) = preSolution + HasPerformanceSessions = true + EndGlobalSection + GlobalSection(Performance) = preSolution + HasPerformanceSessions = true + EndGlobalSection + GlobalSection(Performance) = preSolution + HasPerformanceSessions = true + EndGlobalSection +EndGlobal diff --git a/Demos/Demos.csproj b/Demos/Demos.csproj index d3043a4d1..f47e2710a 100644 --- a/Demos/Demos.csproj +++ b/Demos/Demos.csproj @@ -5,6 +5,7 @@ True Debug;Release latest + AnyCPU;x86;x64 From 13f7f8d2691685d93d1e7479deb90be3760fc555 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 20 Nov 2023 18:18:58 -0600 Subject: [PATCH 835/947] oops, apparently broke the per body gravity demo a while ago. --- Demos/Demos/PerBodyGravityDemo.cs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Demos/Demos/PerBodyGravityDemo.cs b/Demos/Demos/PerBodyGravityDemo.cs index 42e8b0b05..4e5946753 100644 --- a/Demos/Demos/PerBodyGravityDemo.cs +++ b/Demos/Demos/PerBodyGravityDemo.cs @@ -107,9 +107,9 @@ public override void Initialize(ContentArchive content, Camera camera) var boxInertia = boxShape.ComputeInertia(1); var boxShapeIndex = Simulation.Shapes.Add(boxShape); var spacing = new Vector3(4); - const int length = 1; - const int width = 1; - const int height = 1; + const int length = 20; + const int width = 20; + const int height = 20; for (int i = 0; i < length; ++i) { for (int j = 0; j < height; ++j) From aca637096cf942d0f2b80449cae4210995a921a6 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 20 Nov 2023 18:19:17 -0600 Subject: [PATCH 836/947] Worked around https://github.com/dotnet/runtime/issues/95043. --- BepuPhysics/Trees/Tree_RayCast.cs | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/BepuPhysics/Trees/Tree_RayCast.cs b/BepuPhysics/Trees/Tree_RayCast.cs index 448a16772..3eff589ea 100644 --- a/BepuPhysics/Trees/Tree_RayCast.cs +++ b/BepuPhysics/Trees/Tree_RayCast.cs @@ -6,7 +6,10 @@ namespace BepuPhysics.Trees { partial struct Tree { - [MethodImpl(MethodImplOptions.AggressiveInlining)] + //Working around https://github.com/dotnet/runtime/issues/95043: + //Under x86 with optimizations, forcing inlining seems to cause problems for sweeps. It also harms performance. + //Under x64, though, there's not really any cost to letting the JIT decide. TODO: Probably should look into ARM eventually. + //[MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe static bool Intersects(Vector3 min, Vector3 max, TreeRay* ray, out float t) { var t0 = min * ray->InverseDirection - ray->OriginOverDirection; From 62cd95eba6818161921499b2588189c08631f8e7 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 20 Nov 2023 18:20:52 -0600 Subject: [PATCH 837/947] Version bump for fix. --- BepuPhysics/BepuPhysics.csproj | 2 +- BepuUtilities/BepuUtilities.csproj | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/BepuPhysics/BepuPhysics.csproj b/BepuPhysics/BepuPhysics.csproj index fa15d38d5..0ee5eadfc 100644 --- a/BepuPhysics/BepuPhysics.csproj +++ b/BepuPhysics/BepuPhysics.csproj @@ -1,7 +1,7 @@  net8.0 - 2.5.0-beta.15 + 2.5.0-beta.16 Bepu Entertainment LLC Ross Nordby Speedy real time physics simulation library. diff --git a/BepuUtilities/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj index ec4f3b8cb..1b962b6c1 100644 --- a/BepuUtilities/BepuUtilities.csproj +++ b/BepuUtilities/BepuUtilities.csproj @@ -3,7 +3,7 @@ BepuUtilities BepuUtilities net8.0 - 2.5.0-beta.15 + 2.5.0-beta.16 Bepu Entertainment LLC Ross Nordby Supporting utilities library for BEPUphysics v2. From 02d18a699d43f7259d9a388c066f548b902e12ce Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 20 Nov 2023 18:22:18 -0600 Subject: [PATCH 838/947] Clarification! --- BepuPhysics/Trees/Tree_RayCast.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/BepuPhysics/Trees/Tree_RayCast.cs b/BepuPhysics/Trees/Tree_RayCast.cs index 3eff589ea..945eac8d3 100644 --- a/BepuPhysics/Trees/Tree_RayCast.cs +++ b/BepuPhysics/Trees/Tree_RayCast.cs @@ -7,7 +7,7 @@ namespace BepuPhysics.Trees partial struct Tree { //Working around https://github.com/dotnet/runtime/issues/95043: - //Under x86 with optimizations, forcing inlining seems to cause problems for sweeps. It also harms performance. + //Under x86 with optimizations, forcing inlining seems to cause problems for sweeps. *Not* forcing it also harms performance. //Under x64, though, there's not really any cost to letting the JIT decide. TODO: Probably should look into ARM eventually. //[MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe static bool Intersects(Vector3 min, Vector3 max, TreeRay* ray, out float t) From a1a78665ce853f78e5ebf6459a85b2a6bd72f347 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 22 Nov 2023 16:05:04 -0600 Subject: [PATCH 839/947] slightly out of date roadmap --- Documentation/roadmap.md | 35 ++++++++++++++++------------------- 1 file changed, 16 insertions(+), 19 deletions(-) diff --git a/Documentation/roadmap.md b/Documentation/roadmap.md index 3187c9460..2213a0fe8 100644 --- a/Documentation/roadmap.md +++ b/Documentation/roadmap.md @@ -1,34 +1,31 @@ # Roadmap -This is a high level plan for future development. All dates and features are speculative. For a detailed breakdown of tasks in progress, check the [issues](https://github.com/bepu/bepuphysics2/issues) page. +This is a high level plan for future development. All dates and features are *extremely* speculative, and any specific detail on this roadmap is almost certainly wrong. Treat it as a snapshot of vibes unless noted otherwise. For a more detailed breakdown, check the [issues](https://github.com/bepu/bepuphysics2/issues) page. -## Near term (H1 2022) +## Near term (Q1 2024) -With 2.4's launch, most of my development will probably be moving back to things actually using bepuphysics2 for a while. The usual maintenance plus a few smallish fixes and improvements punted from 2.4 are likely. - -I may fiddle with some of the items listed under medium term, but I suspect I will not get them to a fully released state in this period. The most likely of the bunch would be high precision poses and ARM dedicated codepaths. - -There's a chance I'll also try for a kinda-sorta cross platform determinism by reimplementing a few likely sources of architecture nondeterminism in software. It'll probably work only across x64 machines (if at all), but who knows! +2.5 should be releasing relatively soon. It already includes a bunch of miscellaneous improvements, plus notable transformative improvements to tree building and refinement. The broad phase is a lot faster. ## Medium term The timing on these features are uncertain, but they're relatively low hanging fruit and I would like to get to them eventually. -1. High precision body and static poses, plus associated broad phase changes, for worlds exceeding 32 bit floating point precision. This isn't actually too difficult, but it would come with tradeoffs. See https://github.com/bepu/bepuphysics2/issues/13. 2.4's revamp of the solver and body data layouts intentionally left the door open for higher precision poses. -2. ARM specialized paths: https://github.com/bepu/bepuphysics2/issues/184 -3. Convex hull tooling improvements, like in-library simplification utilities. -4. Tree revamp. Should help with both initialization costs (faster Mesh construction, for example) as well as faster and more flexible broad phase incremental refinement. -5. Lower deactivation/reactivation spike overhead. As a part of improving the Tree, I'd like to add batched subtree insertions with better multithreaded scaling. As insertions are a major sequential bottleneck in the current activation system, this could drop the sleep/wake costs by a lot. -6. Mesh/compound intersection optimization, especially in pairs with higher angular velocity. -7. Warm starting depth refinement for some expensive convex pair types. -8. Better allocators for temporary data on threads (could significantly reduce memory requirements on some simulations). -9. Ray cast optimization, particularly with large batches of rays. +1. Mesh/compound intersection optimization, especially in pairs with higher angular velocity. +2. Narrow phase flush improvements: https://github.com/bepu/bepuphysics2/issues/205 +3. Sleeper improvements. Applies to actual sleep/wake and candidacy analysis. One exemplar: island management scales poorly in the limit (https://github.com/bepu/bepuphysics2/issues/284) +4. More bandwidth optimizations in the solver for broad simulations: https://github.com/bepu/bepuphysics2/issues/193 +5. ARM specialized paths: https://github.com/bepu/bepuphysics2/issues/184 +6. Convex hull test performance improvements. +7. Scalar-style API for lower pain contact and boolean queries. +8. Ray cast optimization, particularly with large batches of rays. +9. Convex hull tooling improvements, like in-library simplification utilities. +10. Try for partial cross platform determinism by reimplementing some platform-dependent functionality in software. +11. High precision body and static poses, plus associated broad phase changes, for worlds exceeding 32 bit floating point precision. This isn't actually too difficult, but it would come with tradeoffs. See https://github.com/bepu/bepuphysics2/issues/13. 2.4's revamp of the solver and body data layouts intentionally left the door open for higher precision poses. ## Long term -Here, we get into the realm of the highly speculative. I make no guarantees about the nature or existence of these features, but they are things I'd like to at least investigate at some point: +These features are even *more* speculative. - Generalized boundary smoothing to support non-mesh compounds. Would help avoid annoying hitches when scooting around complex geometry composed of a bunch of convex hulls or other non-triangle convexes. -- Buoyancy. In particular, a form of buoyancy that supports deterministic animated (nonplanar) surfaces. It would almost certainly still be a heightmap, but the goal would be to support synchronized multiplayer boats or similar use cases. Ideally, it would also allow for some approximate handling of concavity. Hollowed concave shapes would displace the appropriate amount of water, including handling of the case where a boat capsizes and the internal volume fills with water (though likely in a pretty hacky way). Would likely be easy to extend this to simple heightmap fluid simulation if the determinism requirement is relaxed. -- Software floating point or fixed point math for cross platform determinism, or swapping to a restricted subset of reliable floating point operations. This one is pretty questionable, but it would be nice to support deterministic physics across any platform. (It's important to note, however, that fixed point math alone is merely necessary and not necessarily *sufficient* to guarantee cross platform determinism...) It is unlikely that I will personally make use of this feature, so the likelihood of it being implemented is lower unless I can find a low effort path. \ No newline at end of file +- Buoyancy. In particular, a form of buoyancy that supports deterministic animated (nonplanar) surfaces. It would almost certainly still be a heightmap, but the goal would be to support synchronized multiplayer boats or similar use cases. Ideally, it would also allow for some approximate handling of concavity. Hollowed concave shapes would displace the appropriate amount of water, including handling of the case where a boat capsizes and the internal volume fills with water (though likely in a pretty hacky way). Would likely be easy to extend this to simple heightmap fluid simulation if the determinism requirement is relaxed. \ No newline at end of file From 0c20deac01b96119ad5018040b0cd83323b15007 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 14 Dec 2023 17:09:16 -0600 Subject: [PATCH 840/947] Fun times in convex hull bug land. --- BepuPhysics/Collidables/ConvexHullHelper.cs | 2747 ++++++++++--------- 1 file changed, 1463 insertions(+), 1284 deletions(-) diff --git a/BepuPhysics/Collidables/ConvexHullHelper.cs b/BepuPhysics/Collidables/ConvexHullHelper.cs index 5f81313e8..805373b73 100644 --- a/BepuPhysics/Collidables/ConvexHullHelper.cs +++ b/BepuPhysics/Collidables/ConvexHullHelper.cs @@ -1,1284 +1,1463 @@ -using BepuUtilities; -using BepuUtilities.Collections; -using BepuUtilities.Memory; -using System; -using System.Diagnostics; -using System.Numerics; -using System.Runtime.CompilerServices; -using System.Runtime.InteropServices; - -namespace BepuPhysics.Collidables -{ - /// - /// Stores references to the points composing one of a convex hull's faces. - /// - public struct HullFace - { - public Buffer OriginalVertexMapping; - public Buffer VertexIndices; - - /// - /// Gets the number of vertices in the face. - /// - public int VertexCount - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - get { return VertexIndices.Length; } - } - - /// - /// Gets the index of the vertex associated with the given face vertex index in the source point set. - /// - /// Index into the face's vertex list. - /// Index of the vertex associated with the given face vertex index in the source point set. - public int this[int index] - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - get { return OriginalVertexMapping[VertexIndices[index]]; } - } - } - - /// - /// Raw data representing a convex hull. - /// - /// This is not yet transformed into a runtime format. It requires additional processing to be used in a ConvexHull shape; see ConvexHullHelper.ProcessHull. - public struct HullData - { - /// - /// Mapping of points on the convex hull back to the original point set. - /// - public Buffer OriginalVertexMapping; - /// - /// List of indices composing the faces of the hull. Individual faces indexed by the FaceIndices. - /// - public Buffer FaceVertexIndices; - /// - /// Starting index in the FaceVertexIndices for each face. - /// - public Buffer FaceStartIndices; - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void GetFace(int faceIndex, out HullFace face) - { - var nextFaceIndex = faceIndex + 1; - var start = FaceStartIndices[faceIndex]; - var end = nextFaceIndex == FaceStartIndices.Length ? FaceVertexIndices.Length : FaceStartIndices[nextFaceIndex]; - FaceVertexIndices.Slice(start, end - start, out face.VertexIndices); - face.OriginalVertexMapping = OriginalVertexMapping; - } - - public void Dispose(BufferPool pool) - { - pool.Return(ref OriginalVertexMapping); - //The other allocations may not exist if the hull is degenerate. - if (FaceVertexIndices.Allocated) - pool.Return(ref FaceVertexIndices); - if (FaceStartIndices.Allocated) - pool.Return(ref FaceStartIndices); - } - } - - /// - /// Helper methods to create and process convex hulls from point clouds. - /// - public static class ConvexHullHelper - { - static void FindExtremeFace( - in Vector3Wide basisX, in Vector3Wide basisY, in Vector3Wide basisOrigin, in EdgeEndpoints sourceEdgeEndpoints, ref Buffer pointBundles, in Vector indexOffsets, Buffer allowVertices, int pointCount, - ref Buffer> projectedOnX, ref Buffer> projectedOnY, in Vector planeEpsilon, ref QuickList vertexIndices, out Vector3 faceNormal) - { - Debug.Assert(projectedOnX.Length >= pointBundles.Length && projectedOnY.Length >= pointBundles.Length && vertexIndices.Count == 0 && vertexIndices.Span.Length >= pointBundles.Length * Vector.Count); - //Find the candidate-basisOrigin which has the smallest angle with basisY when projected onto the plane spanned by basisX and basisY. - //angle = atan(y / x) - //tanAngle = y / x - //x is guaranteed to be nonnegative, so its sign doesn't change. - //tanAngle is monotonically increasing with respect to y / x, so a higher angle corresponds to a higher y/x, always. - //We can then compare samples 0 and 1 using: - //tanAngle0 > tanAngle1 - //y0 / x0 > y1 / x1 - //y0 * x1 > y1 * x0 - Vector3Wide.Subtract(pointBundles[0], basisOrigin, out var toCandidate); - ref var x = ref projectedOnX[0]; - ref var y = ref projectedOnY[0]; - Vector3Wide.Dot(basisX, toCandidate, out x); - //If x is negative, that means some numerical issue has resulted in a point beyond the bounding plane that generated this face request. - //We'll treat it as if it's on the plane. (The reason we bother with this clamp is the sign assumption built into our angle comparison, detailed above.) - x = Vector.Max(Vector.Zero, x); - Vector3Wide.Dot(basisY, toCandidate, out y); - var bestY = y; - var bestX = x; - //Ignore the source edge. - var edgeIndexA = new Vector(sourceEdgeEndpoints.A); - var edgeIndexB = new Vector(sourceEdgeEndpoints.B); - //Note that any slot that would have been coplanar with the generating face *and* behind the edge (that is, a vertex almost certainly associated with the generating face) is ignored. - //Without this condition, it's possible for numerical cycles to occur where a face finds itself over and over again. - var allowVertexBundles = allowVertices.As>(); - var ignoreSlot = Vector.BitwiseOr( - Vector.BitwiseOr( - Vector.OnesComplement(allowVertexBundles[0]), - Vector.BitwiseAnd(Vector.LessThanOrEqual(bestX, planeEpsilon), Vector.LessThanOrEqual(bestY, planeEpsilon))), - Vector.BitwiseOr(Vector.Equals(indexOffsets, edgeIndexA), Vector.Equals(indexOffsets, edgeIndexB))); - bestX = Vector.ConditionalSelect(ignoreSlot, Vector.One, bestX); - bestY = Vector.ConditionalSelect(ignoreSlot, new Vector(float.MinValue), bestY); - var bestIndices = indexOffsets; - for (int i = 1; i < pointBundles.Length; ++i) - { - Vector3Wide.Subtract(pointBundles[i], basisOrigin, out toCandidate); - x = ref projectedOnX[i]; - y = ref projectedOnY[i]; - Vector3Wide.Dot(basisX, toCandidate, out x); - x = Vector.Max(Vector.Zero, x); //Same as earlier- protect against numerical error finding points beyond the bounding plane. - Vector3Wide.Dot(basisY, toCandidate, out y); - - var candidateIndices = indexOffsets + new Vector(i << BundleIndexing.VectorShift); - ignoreSlot = Vector.BitwiseOr( - Vector.BitwiseOr( - Vector.OnesComplement(allowVertexBundles[i]), - Vector.BitwiseAnd(Vector.LessThanOrEqual(x, planeEpsilon), Vector.LessThanOrEqual(y, planeEpsilon))), - Vector.BitwiseOr(Vector.Equals(candidateIndices, edgeIndexA), Vector.Equals(candidateIndices, edgeIndexB))); - var useCandidate = Vector.AndNot(Vector.GreaterThan(y * bestX, bestY * x), ignoreSlot); - - bestY = Vector.ConditionalSelect(useCandidate, y, bestY); - bestX = Vector.ConditionalSelect(useCandidate, x, bestX); - bestIndices = Vector.ConditionalSelect(useCandidate, candidateIndices, bestIndices); - } - var bestYNarrow = bestY[0]; - var bestXNarrow = bestX[0]; - var bestIndexNarrow = bestIndices[0]; - for (int i = 1; i < Vector.Count; ++i) - { - var candidateNumerator = bestY[i]; - var candidateDenominator = bestX[i]; - if (candidateNumerator * bestXNarrow > bestYNarrow * candidateDenominator) - { - bestYNarrow = candidateNumerator; - bestXNarrow = candidateDenominator; - bestIndexNarrow = bestIndices[i]; - } - } - //We now have the best index, but there may have been multiple vertices on the same plane. Capture all of them at once by doing a second pass over the results. - //The plane normal we want to examine is (-bestY, bestX) / ||(-bestY, bestX)||. - //(This isn't wonderfully fast, but it's fairly simple. The alternatives are things like incrementally combining coplanar triangles as they are discovered - //or using a postpass that looks for coplanar triangles after they've been created.) - //Rotate the offset to point outward. - //Note: in unusual corner cases, the above may have accepted zero candidates resulting in a bestXNarrow = 1 and bestYNarrow = float.MinValue. - //Catching that and ensuring that a reasonable face normal is output avoids a bad face. - var candidateNormalDirection = new Vector2(-bestYNarrow, bestXNarrow); - var length = candidateNormalDirection.Length(); - var projectedPlaneNormalNarrow = float.IsFinite(length) ? candidateNormalDirection / length : new Vector2(1, 0); - Vector2Wide.Broadcast(projectedPlaneNormalNarrow, out var projectedPlaneNormal); - Vector3Wide.ReadFirst(basisX, out var basisXNarrow); - Vector3Wide.ReadFirst(basisY, out var basisYNarrow); - faceNormal = basisXNarrow * projectedPlaneNormalNarrow.X + basisYNarrow * projectedPlaneNormalNarrow.Y; - - //if (sourceEdgeEndpoints.A != sourceEdgeEndpoints.B) - //{ - // BundleIndexing.GetBundleIndices(sourceEdgeEndpoints.A, out var bundleA, out var innerA); - // BundleIndexing.GetBundleIndices(sourceEdgeEndpoints.B, out var bundleB, out var innerB); - // BundleIndexing.GetBundleIndices(bestIndexNarrow, out var bundleC, out var innerC); - // Vector3Wide.ReadSlot(ref pointBundles[bundleA], innerA, out var a); - // Vector3Wide.ReadSlot(ref pointBundles[bundleB], innerB, out var b); - // Vector3Wide.ReadSlot(ref pointBundles[bundleC], innerC, out var c); - // var faceNormalFromCross = Vector3.Normalize(Vector3.Cross(c - a, b - a)); - // var testDot = Vector3.Dot(faceNormalFromCross, faceNormal); - // var faceNormalError = faceNormal - faceNormalFromCross; - // faceNormal = faceNormalFromCross; - //} - - Vector3Wide.Broadcast(faceNormal, out var faceNormalWide); - for (int i = 0; i < pointBundles.Length; ++i) - { - var dot = projectedOnX[i] * projectedPlaneNormal.X + projectedOnY[i] * projectedPlaneNormal.Y; - //var dot2 = Vector3Wide.Dot(pointBundles[i] - basisOrigin, faceNormalWide); - //var error = dot2 - dot; - //if (Vector.GreaterThanAny(Vector.Abs(error), planeEpsilon)) - //{ - // for (int j = 0; j < Vector.Count; ++j) - // { - // if (MathF.Abs(error[j]) > planeEpsilon[j]) - // { - // Console.WriteLine($"error: {error[j]}"); - // } - // } - //} - var coplanar = Vector.LessThanOrEqual(Vector.Abs(dot), planeEpsilon); - if (Vector.LessThanAny(coplanar, Vector.Zero)) - { - var bundleBaseIndex = i << BundleIndexing.VectorShift; - var localIndexMaximum = pointCount - bundleBaseIndex; - if (localIndexMaximum > Vector.Count) - localIndexMaximum = Vector.Count; - for (int j = 0; j < localIndexMaximum; ++j) - { - var vertexIndex = bundleBaseIndex + j; - if (coplanar[j] < 0 && allowVertices[vertexIndex] != 0) - { - vertexIndices.AllocateUnsafely() = vertexIndex; - } - } - } - } - //Vector3Wide.ReadFirst(basisX, out var basisXNarrow); - //Vector3Wide.ReadFirst(basisY, out var basisYNarrow); - //faceNormal = basisXNarrow * projectedPlaneNormalNarrow.X + basisYNarrow * projectedPlaneNormalNarrow.Y; - } - - - static int FindNextIndexForFaceHull(Vector2 start, Vector2 previousEdgeDirection, float planeEpsilon, ref QuickList facePoints) - { - //Use a AOS version since the number of points on a given face will tend to be very small in most cases. - //Same idea as the 3d version- find the next edge which is closest to the previous edge. Not going to worry about collinear points here for now. - var bestIndex = -1; - float best = -float.MaxValue; - float bestDistanceSquared = 0; - var startToCandidate = facePoints[0] - start; - var xDirection = new Vector2(previousEdgeDirection.Y, -previousEdgeDirection.X); - var candidateX = Vector2.Dot(startToCandidate, xDirection); - var candidateY = Vector2.Dot(startToCandidate, previousEdgeDirection); - var currentEdgeDirectionX = previousEdgeDirection.X; - var currentEdgeDirectionY = previousEdgeDirection.Y; - if (candidateX > 0) - { - best = candidateY / candidateX; - bestIndex = 0; - bestDistanceSquared = candidateX * candidateX + candidateY * candidateY; - var inverseBestDistance = 1f / MathF.Sqrt(bestDistanceSquared); - currentEdgeDirectionX = candidateX * inverseBestDistance; - currentEdgeDirectionY = candidateY * inverseBestDistance; - } - for (int i = 1; i < facePoints.Count; ++i) - { - startToCandidate = facePoints[i] - start; - candidateY = Vector2.Dot(startToCandidate, previousEdgeDirection); - candidateX = Vector2.Dot(startToCandidate, xDirection); - //Any points that are collinear *with the previous edge* cannot be a part of the current edge without numerical failure; the previous edge should include them. - if (candidateX <= 0) - { - Debug.Assert(candidateY <= 0, - "Previous edge should include any collinear points, so this edge should not see any further collinear points beyond its start." + - "If you run into this, it implies you've found some content that violates the convex huller's assumptions, and I'd appreciate it if you reported it on github.com/bepu/bepuphysics2/issues!" + - "A .obj or other simple demos-compatible reproduction case would help me fix it."); - continue; - } - //We accept a candidate if it is either: - //1) collinear with the previous best by the plane epsilon test BUT is more distant, or - //2) has a greater angle than the previous best. - var planeOffset = -candidateX * currentEdgeDirectionY + candidateY * currentEdgeDirectionX; - if (MathF.Abs(planeOffset) < planeEpsilon) - { - //The candidate is collinear. Only accept it if it's further away. - if (candidateX * candidateX + candidateY * candidateY <= bestDistanceSquared) - { - continue; - } - } - else if (candidateY < best * candidateX) //candidateY / candidateX < best, given candidate X > 0; just avoiding a division for bulk testing. - { - //Candidate is a smaller angle. Rejected. - continue; - } - best = candidateY / candidateX; - bestDistanceSquared = candidateX * candidateX + candidateY * candidateY; - var inverseBestDistance = 1f / MathF.Sqrt(bestDistanceSquared); - currentEdgeDirectionX = candidateX * inverseBestDistance; - currentEdgeDirectionY = candidateY * inverseBestDistance; - bestIndex = i; - } - //Note that this can return -1 if all points were on top of the start. - return bestIndex; - } - - static void ReduceFace(ref QuickList faceVertexIndices, Vector3 faceNormal, Span points, float planeEpsilon, ref QuickList facePoints, ref Buffer allowVertex, ref QuickList reducedIndices) - { - Debug.Assert(facePoints.Count == 0 && reducedIndices.Count == 0 && facePoints.Span.Length >= faceVertexIndices.Count && reducedIndices.Span.Length >= faceVertexIndices.Count); - for (int i = faceVertexIndices.Count - 1; i >= 0; --i) - { - //TODO: This isn't really necessary (conditioning on a small change). - //Face merges may see this codepath because the original rawFaceVertexIndices may contain now-disallowed vertices. - //We don't really need to *track* those, though; we could just use the reducedIndices and then this would never be required. - //It's mostly a matter of legacy- previously, we accumulated everything without asking about whether it was allowed, and relied on ReduceFace to clean it up. - //That opened a door for an infinite loop, so it got changed. - if (allowVertex[faceVertexIndices[i]] == 0) - faceVertexIndices.RemoveAt(i); - } - if (faceVertexIndices.Count <= 3) - { - //Too small to require computing a hull. Copy directly. - for (int i = 0; i < faceVertexIndices.Count; ++i) - { - reducedIndices.AllocateUnsafely() = faceVertexIndices[i]; - } - if (faceVertexIndices.Count == 3) - { - //No point in running a full reduction, but we do need to check the winding of the triangle. - ref var a = ref points[reducedIndices[0]]; - ref var b = ref points[reducedIndices[1]]; - ref var c = ref points[reducedIndices[2]]; - //Counterclockwise should result in face normal pointing outward. - var ab = b - a; - var ac = c - a; - var uncalibratedNormal = Vector3.Cross(ab, ac); - if (uncalibratedNormal.LengthSquared() < 1e-14f) - { - //The face is degenerate. - if (ab.LengthSquared() > 1e-14f) - { - allowVertex[reducedIndices[2]] = 0; - reducedIndices.FastRemoveAt(2); - } - else if (ac.LengthSquared() > 1e-14f) - { - allowVertex[reducedIndices[1]] = 0; - reducedIndices.FastRemoveAt(1); - } - else - { - allowVertex[reducedIndices[1]] = 0; - allowVertex[reducedIndices[2]] = 0; - reducedIndices.Count = 1; - } - } - else - { - if (Vector3.Dot(faceNormal, uncalibratedNormal) < 0) - Helpers.Swap(ref reducedIndices[0], ref reducedIndices[1]); - } - } - return; - } - Helpers.BuildOrthonormalBasis(faceNormal, out var basisX, out var basisY); - Vector2 centroid = default; - for (int i = 0; i < faceVertexIndices.Count; ++i) - { - ref var source = ref points[faceVertexIndices[i]]; - ref var facePoint = ref facePoints.AllocateUnsafely(); - facePoint = new Vector2(Vector3.Dot(basisX, source), Vector3.Dot(basisY, source)); - centroid += facePoint; - } - centroid /= faceVertexIndices.Count; - var greatestDistanceSquared = -1f; - var initialIndex = 0; - for (int i = 0; i < faceVertexIndices.Count; ++i) - { - ref var facePoint = ref facePoints[i]; - var distanceSquared = (facePoint - centroid).LengthSquared(); - if (greatestDistanceSquared < distanceSquared) - { - greatestDistanceSquared = distanceSquared; - initialIndex = i; - } - } - - if (greatestDistanceSquared < 1e-14f) - { - //The face is degenerate. - for (int i = 0; i < faceVertexIndices.Count; ++i) - { - allowVertex[faceVertexIndices[i]] = 0; - } - return; - } - var greatestDistance = (float)Math.Sqrt(greatestDistanceSquared); - var initialOffsetDirection = (facePoints[initialIndex] - centroid) / greatestDistance; - var previousEdgeDirection = new Vector2(initialOffsetDirection.Y, -initialOffsetDirection.X); - reducedIndices.AllocateUnsafely() = faceVertexIndices[initialIndex]; - - var previousEndIndex = initialIndex; - while (true) - { - //This can return -1 in the event of a completely degenerate face. - var nextIndex = FindNextIndexForFaceHull(facePoints[previousEndIndex], previousEdgeDirection, planeEpsilon, ref facePoints); - if (nextIndex == -1 || nextIndex == initialIndex) - { - break; - } - reducedIndices.AllocateUnsafely() = faceVertexIndices[nextIndex]; - previousEdgeDirection = Vector2.Normalize(facePoints[nextIndex] - facePoints[previousEndIndex]); - previousEndIndex = nextIndex; - } - - //Ignore any vertices which were not on the outer boundary of the face. - for (int i = 0; i < faceVertexIndices.Count; ++i) - { - var index = faceVertexIndices[i]; - if (!reducedIndices.Contains(index)) - { - allowVertex[index] = 0; - } - } - } - - [StructLayout(LayoutKind.Explicit)] - public struct EdgeEndpoints : IEqualityComparerRef - { - [FieldOffset(0)] - public int A; - [FieldOffset(4)] - public int B; - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool Equals(ref EdgeEndpoints a, ref EdgeEndpoints b) - { - return Unsafe.As(ref a.A) == Unsafe.As(ref b.A) || (a.A == b.B && a.B == b.A); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public int Hash(ref EdgeEndpoints item) - { - return item.A ^ item.B; - } - - public override string ToString() - { - return $"({A}, {B})"; - } - } - - /// - /// Tracks the faces associated with a detected surface edge. - /// During hull calculation, surface edges that have only one face associated with them should have an outstanding entry in the edges to visit set. - /// Surface edges can never validly have more than two faces associated with them. Two means that an edge is 'complete', or should be. - /// Detecting a third edge implies an error condition. By tracking *which* faces were associated with an edge, we can attempt to fix the error. - /// This type of error tends to occur when there is a numerical disagreement about what vertices are coplanar with a face. - /// One iteration could find what it thinks is a complete face, and a later iteration ends up finding more vertices *including* the ones already contained in the previous face. - /// That's a recipe for excessive edge faces, but it's also a direct indicator that we should merge the involved faces for being close enough to coplanar that the error happened in the first place. - /// - struct EdgeFaceIndices - { - public int FaceA; - public int FaceB; - public bool Complete => FaceA >= 0 && FaceB >= 0; - - public EdgeFaceIndices(int initialFaceIndex) - { - FaceA = initialFaceIndex; - FaceB = -1; - } - - - } - - static void RemoveFaceFromEdge(int a, int b, int faceIndex, ref QuickDictionary facesForEdges) - { - EdgeEndpoints edge; - edge.A = a; - edge.B = b; - var exists = facesForEdges.GetTableIndices(ref edge, out _, out var index); - Debug.Assert(exists, "Whoa something weird happened here! A face was created, but there's a missing edge for its face?"); - ref var facesForEdge = ref facesForEdges.Values[index]; - Debug.Assert(faceIndex == facesForEdge.FaceA || faceIndex == facesForEdge.FaceB, "If you're trying to remove a face index from the edge, it better be in the edge!"); - if (facesForEdge.FaceA == faceIndex) - { - facesForEdge.FaceA = facesForEdge.FaceB; - facesForEdge.FaceB = -1; - if (facesForEdge.FaceA == -1) - { - //This edge no longer has any faces associated with it. - facesForEdges.FastRemove(ref edge); - } - } - else - { - facesForEdge.FaceB = -1; - } - //Note that we do *not* add the edge back into the 'edges to test' list when transitioning from 2 faces to 1 face for the edge. - //This function is invoked when an edge is being deleted because it's related to a deleted face. - //There are two possible outcomes: - //1. The completion of the merge will see a face added back to this edge, - //2. it's an orphaned internal edge that should not be tested. - } - - internal struct EarlyFace - { - public QuickList VertexIndices; - public bool Deleted; - public Vector3 Normal; - } - - struct EdgeToTest - { - public EdgeEndpoints Endpoints; - public Vector3 FaceNormal; - public int FaceIndex; - } - - - static void AddFace(ref QuickList faces, BufferPool pool, Vector3 normal, QuickList vertexIndices) - { - ref var face = ref faces.Allocate(pool); - face = new EarlyFace { Normal = normal, VertexIndices = new QuickList(vertexIndices.Count, pool) }; - face.VertexIndices.AddRangeUnsafely(vertexIndices); - } - - static void AddFaceToEdgesAndTestList(BufferPool pool, - ref QuickList reducedFaceIndices, - ref QuickList edgesToTest, - ref QuickDictionary facesForEdges, - Vector3 faceNormal, int newFaceIndex) - { - facesForEdges.EnsureCapacity(facesForEdges.Count + reducedFaceIndices.Count, pool); - var previousIndex = reducedFaceIndices[reducedFaceIndices.Count - 1]; - for (int i = 0; i < reducedFaceIndices.Count; ++i) - { - EdgeEndpoints endpoints; - endpoints.A = previousIndex; - endpoints.B = reducedFaceIndices[i]; - previousIndex = endpoints.B; - if (facesForEdges.GetTableIndices(ref endpoints, out var tableIndex, out var elementIndex)) - { - ref var edgeFaceCount = ref facesForEdges.Values[elementIndex]; - Debug.Assert(edgeFaceCount.FaceB == -1, - "While we let execution continue, this is an error condition and implies overlapping triangles are being generated." + - "This tends to happen when there are many near-coplanar vertices, so numerical tolerances across different faces cannot consistently agree."); - edgeFaceCount.FaceB = newFaceIndex; - } - else - { - //This edge is not yet claimed by any edge. Claim it for the new face and add the edge for further testing. - EdgeToTest nextEdgeToTest; - nextEdgeToTest.Endpoints = endpoints; - nextEdgeToTest.FaceNormal = faceNormal; - nextEdgeToTest.FaceIndex = newFaceIndex; - facesForEdges.Keys[facesForEdges.Count] = nextEdgeToTest.Endpoints; - facesForEdges.Values[facesForEdges.Count] = new EdgeFaceIndices(newFaceIndex); - //Use the encoding- all indices are offset by 1 since 0 represents 'empty'. - facesForEdges.Table[tableIndex] = ++facesForEdges.Count; - edgesToTest.Allocate(pool) = nextEdgeToTest; - } - } - } - - static void AddIfNotPresent(ref QuickList list, int value, BufferPool pool) - { - if (!list.Contains(value)) - list.Allocate(pool) = value; - } - - //public struct DebugStep - //{ - // public EdgeEndpoints SourceEdge; - // public List Raw; - // public List Reduced; - // public bool[] AllowVertex; - // public Vector3 FaceNormal; - // public Vector3 BasisX; - // public Vector3 BasisY; - // public List FaceStarts; - // public List FaceIndices; - // public bool[] FaceDeleted; - // public int[] MergedFaceIndices; - // public int FaceIndex; - // public Vector3[] FaceNormals; - - // internal DebugStep(EdgeEndpoints sourceEdge, ref QuickList raw, Vector3 faceNormal, Vector3 basisX, Vector3 basisY, ref QuickList reduced, ref Buffer allowVertex, ref QuickList faces, Span mergedFaceIndices, int faceIndex) - // { - // SourceEdge = sourceEdge; - // FaceNormal = faceNormal; - // BasisX = basisX; - // BasisY = basisY; - // Raw = new List(); - // for (int i = 0; i < raw.Count; ++i) - // { - // Raw.Add(raw[i]); - // } - // Reduced = new List(); - // for (int i = 0; i < reduced.Count; ++i) - // { - // Reduced.Add(reduced[i]); - // } - // AllowVertex = new bool[allowVertex.Length]; - // for (int i = 0; i < allowVertex.Length; ++i) - // { - // AllowVertex[i] = allowVertex[i] != 0; - // } - // FaceStarts = new List(faces.Count); - // FaceIndices = new List(); - // FaceDeleted = new bool[faces.Count]; - // FaceNormals = new Vector3[faces.Count]; - // for (int i = 0; i < faces.Count; ++i) - // { - // ref var face = ref faces[i]; - // FaceStarts.Add(FaceIndices.Count); - // for (int j = 0; j < face.VertexIndices.Count; ++j) - // FaceIndices.Add(face.VertexIndices[j]); - // FaceDeleted[i] = face.Deleted; - // FaceNormals[i] = face.Normal; - // } - // MergedFaceIndices = mergedFaceIndices.ToArray(); - // FaceIndex = faceIndex; - // } - //} - ///// - ///// Computes the convex hull of a set of points. - ///// - ///// Point set to compute the convex hull of. - ///// Buffer pool to pull memory from when creating the hull. - ///// Convex hull of the input point set. - //public static void ComputeHull(Span points, BufferPool pool, out HullData hullData) - //{ - // ComputeHull(points, pool, out hullData, out _); - //} - - - /// - /// Computes the convex hull of a set of points. - /// - /// Point set to compute the convex hull of. - /// Buffer pool to pull memory from when creating the hull. - /// Convex hull of the input point set. - public static void ComputeHull(Span points, BufferPool pool, out HullData hullData)//, out List steps) - { - //steps = new List(); - if (points.Length <= 0) - { - hullData = default; - return; - } - if (points.Length <= 3) - { - //If the input is too small to actually form a volumetric hull, just output the input directly. - pool.Take(points.Length, out hullData.OriginalVertexMapping); - for (int i = 0; i < points.Length; ++i) - { - hullData.OriginalVertexMapping[i] = i; - } - if (points.Length == 3) - { - pool.Take(1, out hullData.FaceStartIndices); - pool.Take(3, out hullData.FaceVertexIndices); - hullData.FaceStartIndices[0] = 0; - //No volume, so winding doesn't matter. - hullData.FaceVertexIndices[0] = 0; - hullData.FaceVertexIndices[1] = 1; - hullData.FaceVertexIndices[2] = 2; - } - else - { - hullData.FaceStartIndices = default; - hullData.FaceVertexIndices = default; - } - return; - } - var pointBundleCount = BundleIndexing.GetBundleCount(points.Length); - pool.Take(pointBundleCount, out var pointBundles); - //While it's not asymptotically optimal in general, gift wrapping is simple and easy to productively vectorize. - //As a first step, create an AOSOA version of the input data. - Vector3 centroid = default; - for (int i = 0; i < points.Length; ++i) - { - BundleIndexing.GetBundleIndices(i, out var bundleIndex, out var innerIndex); - ref var p = ref points[i]; - Vector3Wide.WriteSlot(p, innerIndex, ref pointBundles[bundleIndex]); - centroid += p; - } - centroid /= points.Length; - //Fill in the last few slots with the centroid. - //We avoid doing a bunch of special case work on the last partial bundle by just assuming it has a few extra redundant internal points. - var bundleSlots = pointBundles.Length * Vector.Count; - for (int i = points.Length; i < bundleSlots; ++i) - { - BundleIndexing.GetBundleIndices(i, out var bundleIndex, out var innerIndex); - Vector3Wide.WriteSlot(centroid, innerIndex, ref pointBundles[bundleIndex]); - } - - //Find a starting point. We'll use the one furthest from the centroid. - Vector3Wide.Broadcast(centroid, out var centroidBundle); - Helpers.FillVectorWithLaneIndices(out var mostDistantIndicesBundle); - var indexOffsetBundle = mostDistantIndicesBundle; - Vector3Wide.DistanceSquared(pointBundles[0], centroidBundle, out var distanceSquaredBundle); - for (int i = 1; i < pointBundles.Length; ++i) - { - var bundleIndices = new Vector(i << BundleIndexing.VectorShift) + indexOffsetBundle; - Vector3Wide.DistanceSquared(pointBundles[i], centroidBundle, out var distanceSquaredCandidate); - mostDistantIndicesBundle = Vector.ConditionalSelect(Vector.GreaterThan(distanceSquaredCandidate, distanceSquaredBundle), bundleIndices, mostDistantIndicesBundle); - distanceSquaredBundle = Vector.Max(distanceSquaredBundle, distanceSquaredCandidate); - } - var bestDistanceSquared = distanceSquaredBundle[0]; - var initialIndex = mostDistantIndicesBundle[0]; - for (int i = 1; i < Vector.Count; ++i) - { - var distanceCandidate = distanceSquaredBundle[i]; - if (distanceCandidate > bestDistanceSquared) - { - bestDistanceSquared = distanceCandidate; - initialIndex = mostDistantIndicesBundle[i]; - } - } - BundleIndexing.GetBundleIndices(initialIndex, out var mostDistantBundleIndex, out var mostDistantInnerIndex); - Vector3Wide.ReadSlot(ref pointBundles[mostDistantBundleIndex], mostDistantInnerIndex, out var initialVertex); - - //All further points will be found by picking an plane on which to project all vertices down onto, and then measuring the angle on that plane. - //We pick to basis directions along which to measure. For the second point, we choose a perpendicular direction arbitrarily. - var initialToCentroid = centroid - initialVertex; - var initialDistance = initialToCentroid.Length(); - if (initialDistance < 1e-7f) - { - //The point set lacks any volume or area. - pool.Take(1, out hullData.OriginalVertexMapping); - hullData.OriginalVertexMapping[0] = 0; - hullData.FaceStartIndices = default; - hullData.FaceVertexIndices = default; - pool.Return(ref pointBundles); - return; - } - Vector3Wide.Broadcast(initialToCentroid / initialDistance, out var initialBasisX); - Helpers.FindPerpendicular(initialBasisX, out var initialBasisY); //(broadcasted before FindPerpendicular just because we didn't have a non-bundle version) - Vector3Wide.Broadcast(initialVertex, out var initialVertexBundle); - pool.Take>(pointBundles.Length, out var projectedOnX); - pool.Take>(pointBundles.Length, out var projectedOnY); - var planeEpsilonNarrow = MathF.Sqrt(bestDistanceSquared) * 1e-6f; - var normalCoplanarityEpsilon = 1f - 1e-6f; - var planeEpsilon = new Vector(planeEpsilonNarrow); - var rawFaceVertexIndices = new QuickList(pointBundles.Length * Vector.Count, pool); - var initialSourceEdge = new EdgeEndpoints { A = initialIndex, B = initialIndex }; - - //Points found to not be on the face hull are ignored by future executions. - //Note that it's stored in integers instead of bools; it can be directly loaded as a mask during vectorized operations. - //0 means the vertex is disallowed, -1 means the vertex is allowed. - pool.Take(pointBundleCount * Vector.Count, out var allowVertices); - ((Span)allowVertices).Slice(0, points.Length).Fill(-1); - for (int i = points.Length; i < allowVertices.Length; ++i) - allowVertices[i] = 0; - - FindExtremeFace(initialBasisX, initialBasisY, initialVertexBundle, initialSourceEdge, ref pointBundles, indexOffsetBundle, allowVertices, points.Length, - ref projectedOnX, ref projectedOnY, planeEpsilon, ref rawFaceVertexIndices, out var initialFaceNormal); - Debug.Assert(rawFaceVertexIndices.Count >= 2); - var facePoints = new QuickList(points.Length, pool); - var reducedFaceIndices = new QuickList(points.Length, pool); - - - ReduceFace(ref rawFaceVertexIndices, initialFaceNormal, points, planeEpsilonNarrow, ref facePoints, ref allowVertices, ref reducedFaceIndices); - - var faces = new QuickList(points.Length, pool); - var edgesToTest = new QuickList(points.Length, pool); - var facesForEdges = new QuickDictionary(points.Length, pool); - var facesNeedingMerge = new QuickList(32, pool); - if (reducedFaceIndices.Count >= 3) - { - //The initial face search found an actual face! That's a bit surprising since we didn't start from an edge offset, but rather an arbitrary direction. - //Handle it anyway. - for (int i = 0; i < reducedFaceIndices.Count; ++i) - { - ref var edgeToAdd = ref edgesToTest.Allocate(pool); - edgeToAdd.Endpoints.A = reducedFaceIndices[i == 0 ? reducedFaceIndices.Count - 1 : i - 1]; - edgeToAdd.Endpoints.B = reducedFaceIndices[i]; - edgeToAdd.FaceNormal = initialFaceNormal; - edgeToAdd.FaceIndex = 0; - facesForEdges.Add(ref edgeToAdd.Endpoints, new EdgeFaceIndices(0), pool); - } - //Since an actual face was found, we go ahead and output it into the face set. - AddFace(ref faces, pool, initialFaceNormal, reducedFaceIndices); - } - else - { - Debug.Assert(reducedFaceIndices.Count == 2, - "The point set size was verified to be at least 4 earlier, so even in degenerate cases, a second point should be found by the face search."); - //No actual face was found. That's expected; the arbitrary direction we used for the basis doesn't likely line up with any edges. - ref var edgeToAdd = ref edgesToTest.Allocate(pool); - edgeToAdd.Endpoints.A = reducedFaceIndices[0]; - edgeToAdd.Endpoints.B = reducedFaceIndices[1]; - edgeToAdd.FaceNormal = initialFaceNormal; - edgeToAdd.FaceIndex = -1; - var edgeOffset = points[edgeToAdd.Endpoints.B] - points[edgeToAdd.Endpoints.A]; - var basisY = Vector3.Cross(edgeOffset, edgeToAdd.FaceNormal); - var basisX = Vector3.Cross(edgeOffset, basisY); - if (Vector3.Dot(basisX, edgeToAdd.FaceNormal) > 0) - Helpers.Swap(ref edgeToAdd.Endpoints.A, ref edgeToAdd.Endpoints.B); - } - //Vector3Wide.ReadFirst(initialBasisX, out var debugInitialBasisX); - //Vector3Wide.ReadFirst(initialBasisY, out var debugInitialBasisY); - //steps.Add(new DebugStep(initialSourceEdge, ref rawFaceVertexIndices, initialFaceNormal, debugInitialBasisX, debugInitialBasisY, ref reducedFaceIndices, ref allowVertices, ref faces, default, reducedFaceIndices.Count >= 3 ? 0 : -1)); - - int facesDeletedCount = 0; - - while (edgesToTest.Count > 0) - { - edgesToTest.Pop(out var edgeToTest); - //Make sure the new edge hasn't already been filled by another traversal. - var faceCountIndex = facesForEdges.IndexOf(edgeToTest.Endpoints); - if (faceCountIndex >= 0 && facesForEdges.Values[faceCountIndex].Complete) - continue; - - ref var edgeA = ref points[edgeToTest.Endpoints.A]; - ref var edgeB = ref points[edgeToTest.Endpoints.B]; - var edgeOffset = edgeB - edgeA; - //The face normal points outward, and the edges should be wound counterclockwise. - //basisY should point away from the source face. - var basisY = Vector3.Cross(edgeOffset, edgeToTest.FaceNormal); - //basisX should point inward. - var basisX = Vector3.Cross(edgeOffset, basisY); - basisX = Vector3.Normalize(basisX); - basisY = Vector3.Normalize(basisY); - Vector3Wide.Broadcast(basisX, out var basisXBundle); - Vector3Wide.Broadcast(basisY, out var basisYBundle); - Vector3Wide.Broadcast(edgeA, out var basisOrigin); - rawFaceVertexIndices.Count = 0; - FindExtremeFace(basisXBundle, basisYBundle, basisOrigin, edgeToTest.Endpoints, ref pointBundles, indexOffsetBundle, allowVertices, points.Length, ref projectedOnX, ref projectedOnY, planeEpsilon, ref rawFaceVertexIndices, out var faceNormal); - reducedFaceIndices.Count = 0; - facePoints.Count = 0; - ReduceFace(ref rawFaceVertexIndices, faceNormal, points, planeEpsilonNarrow, ref facePoints, ref allowVertices, ref reducedFaceIndices); - - if (reducedFaceIndices.Count < 3) - { - //steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertices, ref faces, default, -1)); - //Degenerate face found; don't bother creating work for it. - continue; - } - var faceCountPriorToAdd = faces.Count; - - facesNeedingMerge.Count = 0; - while (true) - { - //This implementation bites the bullet pretty hard on numerical problems. They most frequently arise from near coplanar vertices. - //It's possible that two iterations see different subsets of 'coplanar' vertices, either causing two faces with near equal normal or - //even causing an edge to have more than two faces associated with it. - //Before making any modifications to the existing data, iterate over all the edges in the current face to check for any such edges. - - //While the usual flow here will be to reduce the vertices we just found and accept the result immediately, - //it's possible for the just-detected face to end up coplanar with an existing face because of numerical issues that prevented detecting the whole face earlier. - //Rather than using more precise math, we detect the failure and merge faces after the fact. - //Unfortunately, this can happen recursively! Merging two redundant faces could reveal a third face that also needs to be merged. - //This is uncommon, but it needs to be covered. - //So, we just stick everything into a while loop and let it iterate until it's done. - //The good news is that the merging process won't loop forever- every merge results in a face that is at least as large as any contributor, and any internal points - //that get reduced out of consideration are marked with allowVertex[whateverInteriorPointIndex] = false. - int previousFaceMergeCount = facesNeedingMerge.Count; - var previousEndpointIndex = reducedFaceIndices[reducedFaceIndices.Count - 1]; - for (int i = 0; i < reducedFaceIndices.Count; ++i) - { - EdgeEndpoints edgeEndpoints; - edgeEndpoints.A = previousEndpointIndex; - edgeEndpoints.B = reducedFaceIndices[i]; - previousEndpointIndex = edgeEndpoints.B; - - if (facesForEdges.GetTableIndices(ref edgeEndpoints, out var tableIndex, out var elementIndex)) - { - //There is already at least one face associated with this edge. Do we need to merge? - ref var edgeFaces = ref facesForEdges.Values[elementIndex]; - Debug.Assert(edgeFaces.FaceA >= 0); - var aDot = Vector3.Dot(faces[edgeFaces.FaceA].Normal, faceNormal); - if (edgeFaces.FaceB >= 0) - { - //The edge already has two faces associated with it. This is definitely a numerical error; separate coplanar faces were generated. - //We *must* merge at least one pair of faces. - //Note that technically both faces can end up being included, even though we've already tested A and B; - //the new face could be a bridge that's close enough to both, even if they were barely too far from each other. - var bDot = Vector3.Dot(faces[edgeFaces.FaceB].Normal, faceNormal); - if (aDot >= normalCoplanarityEpsilon && bDot >= normalCoplanarityEpsilon) - { - AddIfNotPresent(ref facesNeedingMerge, edgeFaces.FaceA, pool); - AddIfNotPresent(ref facesNeedingMerge, edgeFaces.FaceB, pool); - } - else - { - //If both aren't merge candidates, then just pick the one that's closer. - AddIfNotPresent(ref facesNeedingMerge, aDot > bDot ? edgeFaces.FaceA : edgeFaces.FaceB, pool); - } - } - else - { - //Only one face already present. Check if it's coplanar. - if (aDot >= normalCoplanarityEpsilon) - AddIfNotPresent(ref facesNeedingMerge, edgeFaces.FaceA, pool); - } - } - } - - if (facesNeedingMerge.Count > previousFaceMergeCount) - { - //This iteration has found more faces which should be merged together. - //Accumulate the vertices. - for (int i = previousFaceMergeCount; i < facesNeedingMerge.Count; ++i) - { - ref var sourceFace = ref faces[facesNeedingMerge[i]]; - for (int j = 0; j < sourceFace.VertexIndices.Count; ++j) - { - var vertexIndex = sourceFace.VertexIndices[j]; - if (allowVertices[vertexIndex] != 0 && !rawFaceVertexIndices.Contains(vertexIndex)) - { - rawFaceVertexIndices.Allocate(pool) = vertexIndex; - } - } - } - - //Re-reduce. - reducedFaceIndices.Count = 0; - facePoints.Count = 0; - ReduceFace(ref rawFaceVertexIndices, faceNormal, points, planeEpsilonNarrow, ref facePoints, ref allowVertices, ref reducedFaceIndices); - } - else - { - //No more faces need to be merged! Go ahead and apply the changes. - for (int i = 0; i < facesNeedingMerge.Count; ++i) - { - //Note that we do not merge into an existing face; for simplicity, we just merge into the face we're going to append and mark the old faces as being deleted. - //Note that we do *not* remove deleted faces from the faces list. That would break all the face indices that we've accumulated. - var faceIndex = facesNeedingMerge[i]; - ref var faceToRemove = ref faces[faceIndex]; - faceToRemove.Deleted = true; - - //For every edge of any face we're merging, remove the face from the edge lists. - var previousIndex = faceToRemove.VertexIndices[faceToRemove.VertexIndices.Count - 1]; - for (int j = 0; j < faceToRemove.VertexIndices.Count; ++j) - { - RemoveFaceFromEdge(previousIndex, faceToRemove.VertexIndices[j], faceIndex, ref facesForEdges); - previousIndex = faceToRemove.VertexIndices[j]; - } - //Remove any references to this face in the edges to test. - int removedEdgesToTestCount = 0; - for (int j = 0; j < edgesToTest.Count; ++j) - { - if (edgesToTest[j].FaceIndex == faceIndex) - { - ++removedEdgesToTestCount; - } - else if (removedEdgesToTestCount > 0) - { - //Scoot edges to test back. - edgesToTest[j - removedEdgesToTestCount] = edgesToTest[j]; - } - } - edgesToTest.Count -= removedEdgesToTestCount; - } - - //Retain a count of the deleted faces so the post process can handle them more easily. - facesDeletedCount += facesNeedingMerge.Count; - - AddFace(ref faces, pool, faceNormal, reducedFaceIndices); - AddFaceToEdgesAndTestList(pool, ref reducedFaceIndices, ref edgesToTest, ref facesForEdges, faceNormal, faceCountPriorToAdd); - //steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertices, ref faces, facesNeedingMerge, faceCountPriorToAdd)); - break; - } - } - } - - edgesToTest.Dispose(pool); - facesForEdges.Dispose(pool); - facePoints.Dispose(pool); - reducedFaceIndices.Dispose(pool); - rawFaceVertexIndices.Dispose(pool); - pool.Return(ref allowVertices); - pool.Return(ref projectedOnX); - pool.Return(ref projectedOnY); - pool.Return(ref pointBundles); - - //for (int i = 0; i < faces.Count; ++i) - //{ - // for (int j = i + 1; j < faces.Count; ++j) - // { - // var dot = Vector3.Dot(faces[i].Normal, faces[j].Normal); - // var bothFacesExist = !faces[i].Deleted && !faces[j].Deleted; - // if (dot >= normalCoplanarityEpsilon && bothFacesExist) - // { - // Console.WriteLine($"Dot {dot} on faces {i} and {j}"); - // } - // } - //} - - if (facesDeletedCount > 0) - { - //During execution, some faces were found to be coplanar and got merged. To avoid breaking face index references during execution, we left those triangles in place. - //Now, though, we need to get rid of any faces that are marked deleted! - int shift = 0; - for (int i = 0; i < faces.Count; ++i) - { - if (faces[i].Deleted) - { - //Removing the face from the list, so we gotta dispose it now rather than later. - faces[i].VertexIndices.Dispose(pool); - ++shift; - } - else if (shift > 0) - { - faces[i - shift] = faces[i]; - } - } - Debug.Assert(facesDeletedCount == shift); - faces.Count -= facesDeletedCount; - } - - //Create a reduced hull point set from the face vertex references. - int totalIndexCount = 0; - for (int i = 0; i < faces.Count; ++i) - { - totalIndexCount += faces[i].VertexIndices.Count; - } - pool.Take(faces.Count, out hullData.FaceStartIndices); - pool.Take(totalIndexCount, out hullData.FaceVertexIndices); - var nextStartIndex = 0; - pool.Take(points.Length, out var originalToHullIndexMapping); - var hullToOriginalIndexMapping = new QuickList(points.Length, pool); - for (int i = 0; i < points.Length; ++i) - { - originalToHullIndexMapping[i] = -1; - } - for (int i = 0; i < faces.Count; ++i) - { - var source = faces[i].VertexIndices; - hullData.FaceStartIndices[i] = nextStartIndex; - for (int j = 0; j < source.Count; ++j) - { - var originalVertexIndex = source[j]; - ref var originalToHull = ref originalToHullIndexMapping[originalVertexIndex]; - if (originalToHull < 0) - { - //This vertex hasn't been seen yet. - originalToHull = hullToOriginalIndexMapping.Count; - hullToOriginalIndexMapping.AllocateUnsafely() = originalVertexIndex; - } - hullData.FaceVertexIndices[nextStartIndex + j] = originalToHull; - } - nextStartIndex += source.Count; - } - - pool.Take(hullToOriginalIndexMapping.Count, out hullData.OriginalVertexMapping); - hullToOriginalIndexMapping.Span.CopyTo(0, hullData.OriginalVertexMapping, 0, hullToOriginalIndexMapping.Count); - - pool.Return(ref originalToHullIndexMapping); - hullToOriginalIndexMapping.Dispose(pool); - for (int i = 0; i < faces.Count; ++i) - { - faces[i].VertexIndices.Dispose(pool); - } - faces.Dispose(pool); - facesNeedingMerge.Dispose(pool); - } - - - /// - /// Processes hull data into a runtime usable convex hull shape. Recenters the convex hull's points around its center of mass. - /// - /// Point array into which the hull data indexes. - /// Raw input data to process. - /// Pool used to allocate resources for the hullShape. - /// Convex hull shape created from the input data. - /// Computed center of mass of the convex hull before its points were recentered onto the origin. - /// True if the shape was created successfully, false otherwise. If false, the hull probably had no volume and would not have worked properly as a shape. - public static bool CreateShape(Span points, HullData hullData, BufferPool pool, out Vector3 center, out ConvexHull hullShape) - { - Debug.Assert(points.Length > 0, "Convex hulls need to have a nonzero number of points!"); - hullShape = default; - var pointBundleCount = BundleIndexing.GetBundleCount(hullData.OriginalVertexMapping.Length); - pool.Take(pointBundleCount, out hullShape.Points); - - float volume = 0; - center = default; - for (int faceIndex = 0; faceIndex < hullData.FaceStartIndices.Length; ++faceIndex) - { - hullData.GetFace(faceIndex, out var face); - for (int subtriangleIndex = 2; subtriangleIndex < face.VertexCount; ++subtriangleIndex) - { - var a = points[face[0]]; - var b = points[face[subtriangleIndex - 1]]; - var c = points[face[subtriangleIndex]]; - var volumeContribution = MeshInertiaHelper.ComputeTetrahedronVolume(a, b, c); - volume += volumeContribution; - var centroid = a + b + c; - center += centroid * volumeContribution; - } - } - //Division by 4 since we accumulated (a + b + c), rather than the actual tetrahedral center (a + b + c + 0) / 4. - center /= volume * 4; - if (float.IsNaN(center.X) || float.IsNaN(center.Y) || float.IsNaN(center.Z) || hullData.FaceStartIndices.Length == 2) - { - //The convex hull seems to have no volume. - //While you could try treating it as coplanar (like we once tried; see commit history just prior to the commit that added this message): - //1. Ray tests won't work. They rely on bounding planes. It would require a special case for degenerate hulls. - //2. Inertia won't work. You could resolve that with a special case, but it doesn't fix ray tests. - //3. Edge-on contact generation may produce lower quality contacts. - //So, pretty worthless overall without major changes. - hullShape.Points.Dispose(pool); - center = default; - Debug.Assert(!hullShape.Points.Allocated && !hullShape.FaceToVertexIndicesStart.Allocated && !hullShape.BoundingPlanes.Allocated && !hullShape.FaceVertexIndices.Allocated, "Hey! You moved something around and forgot to dispose!"); - return false; - } - - var lastIndex = hullData.OriginalVertexMapping.Length - 1; - for (int bundleIndex = 0; bundleIndex < hullShape.Points.Length; ++bundleIndex) - { - ref var bundle = ref hullShape.Points[bundleIndex]; - for (int innerIndex = 0; innerIndex < Vector.Count; ++innerIndex) - { - var index = (bundleIndex << BundleIndexing.VectorShift) + innerIndex; - //We duplicate the last vertices in the hull. It has no impact on performance; the vertex bundles are executed all or nothing. - if (index > lastIndex) - index = lastIndex; - ref var point = ref points[hullData.OriginalVertexMapping[index]]; - Vector3Wide.WriteSlot(point - center, innerIndex, ref bundle); - } - } - - //Create the face->vertex mapping. - pool.Take(hullData.FaceStartIndices.Length, out hullShape.FaceToVertexIndicesStart); - hullData.FaceStartIndices.CopyTo(0, hullShape.FaceToVertexIndicesStart, 0, hullShape.FaceToVertexIndicesStart.Length); - pool.Take(hullData.FaceVertexIndices.Length, out hullShape.FaceVertexIndices); - for (int i = 0; i < hullShape.FaceVertexIndices.Length; ++i) - { - BundleIndexing.GetBundleIndices(hullData.FaceVertexIndices[i], out var bundleIndex, out var innerIndex); - ref var faceVertex = ref hullShape.FaceVertexIndices[i]; - faceVertex.BundleIndex = (ushort)bundleIndex; - faceVertex.InnerIndex = (ushort)innerIndex; - } - - //Create bounding planes. - var faceBundleCount = BundleIndexing.GetBundleCount(hullShape.FaceToVertexIndicesStart.Length); - pool.Take(faceBundleCount, out hullShape.BoundingPlanes); - for (int i = 0; i < hullShape.FaceToVertexIndicesStart.Length; ++i) - { - hullShape.GetVertexIndicesForFace(i, out var faceVertexIndices); - Debug.Assert(faceVertexIndices.Length >= 3, "We only allow the creation of convex hulls around point sets with, at minimum, some area, so all faces should have at least 3 points."); - //Note that we sum up contributions from all the constituent triangles. - //This avoids hitting any degenerate face triangles and smooths out small numerical deviations. - //(It's mathematically equivalent to taking a weighted average by area, since the magnitude of the cross product is proportional to area.) - Vector3 faceNormal = default; - hullShape.GetPoint(faceVertexIndices[0], out var facePivot); - hullShape.GetPoint(faceVertexIndices[1], out var faceVertex); - var previousOffset = faceVertex - facePivot; - for (int j = 2; j < faceVertexIndices.Length; ++j) - { - //Normal points outward. - hullShape.GetPoint(faceVertexIndices[j], out faceVertex); - var offset = faceVertex - facePivot; - faceNormal += Vector3.Cross(previousOffset, offset); - previousOffset = offset; - } - var length = faceNormal.Length(); - Debug.Assert(length > 1e-10f, "Convex hull procedure should not output degenerate faces."); - faceNormal /= length; - BundleIndexing.GetBundleIndices(i, out var boundingPlaneBundleIndex, out var boundingPlaneInnerIndex); - ref var boundingBundle = ref hullShape.BoundingPlanes[boundingPlaneBundleIndex]; - ref var boundingOffsetBundle = ref GatherScatter.GetOffsetInstance(ref boundingBundle, boundingPlaneInnerIndex); - Vector3Wide.WriteFirst(faceNormal, ref boundingOffsetBundle.Normal); - GatherScatter.GetFirst(ref boundingOffsetBundle.Offset) = Vector3.Dot(facePivot, faceNormal); - } - - //Clear any trailing bounding plane data to keep it from contributing. - var boundingPlaneCapacity = hullShape.BoundingPlanes.Length * Vector.Count; - for (int i = hullShape.FaceToVertexIndicesStart.Length; i < boundingPlaneCapacity; ++i) - { - BundleIndexing.GetBundleIndices(i, out var bundleIndex, out var innerIndex); - ref var offsetInstance = ref GatherScatter.GetOffsetInstance(ref hullShape.BoundingPlanes[bundleIndex], innerIndex); - Vector3Wide.WriteFirst(default, ref offsetInstance.Normal); - GatherScatter.GetFirst(ref offsetInstance.Offset) = float.MinValue; - } - return true; - } - - /// - /// Creates a convex hull shape out of an input point set. Recenters the convex hull's points around its center of mass. - /// - /// Points to use to create the hull. - /// Buffer pool used for temporary allocations and the output data structures. - /// Intermediate hull data that got processed into the convex hull. - /// Computed center of mass of the convex hull before its points were recentered onto the origin. - /// Convex hull shape of the input point set. - /// True if the shape was created successfully, false otherwise. If false, the hull probably had no volume and would not have worked properly as a shape. - public static bool CreateShape(Span points, BufferPool pool, out HullData hullData, out Vector3 center, out ConvexHull convexHull) - { - ComputeHull(points, pool, out hullData); - return CreateShape(points, hullData, pool, out center, out convexHull); - } - - /// - /// Creates a convex hull shape out of an input point set. Recenters the convex hull's points around its center of mass. - /// - /// Points to use to create the hull. - /// Buffer pool used for temporary allocations and the output data structures. - /// Computed center of mass of the convex hull before its points were recentered onto the origin. - /// Convex hull shape of the input point set. - /// True if the shape was created successfully, false otherwise. If false, the hull probably had no volume and would not have worked properly as a shape. - public static bool CreateShape(Span points, BufferPool pool, out Vector3 center, out ConvexHull convexHull) - { - ComputeHull(points, pool, out var hullData); - var result = CreateShape(points, hullData, pool, out center, out convexHull); - //Empty input point sets won't allocate; don't try to dispose them. - if (hullData.OriginalVertexMapping.Allocated) - hullData.Dispose(pool); - return result; - } - - - /// - /// Creates a transformed copy of a convex hull. - /// - /// Source convex hull to copy. - /// Transform to apply to the hull points. - /// Transformed points in the copy target hull. - /// Transformed bounding planes in the copy target hull. - public static void CreateTransformedCopy(in ConvexHull source, in Matrix3x3 transform, Buffer targetPoints, Buffer targetBoundingPlanes) - { - if (targetPoints.Length < source.Points.Length) - throw new ArgumentException("Target points buffer cannot hold the copy.", nameof(targetPoints)); - if (targetBoundingPlanes.Length < source.BoundingPlanes.Length) - throw new ArgumentException("Target bounding planes buffer cannot hold the copy.", nameof(targetBoundingPlanes)); - Matrix3x3Wide.Broadcast(transform, out var transformWide); - for (int i = 0; i < source.Points.Length; ++i) - { - Matrix3x3Wide.TransformWithoutOverlap(source.Points[i], transformWide, out targetPoints[i]); - } - Matrix3x3.Invert(transform, out var inverse); - Matrix3x3Wide.Broadcast(inverse, out var inverseWide); - for (int i = 0; i < source.BoundingPlanes.Length; ++i) - { - Matrix3x3Wide.TransformByTransposedWithoutOverlap(source.BoundingPlanes[i].Normal, inverseWide, out var normal); - Vector3Wide.Normalize(normal, out targetBoundingPlanes[i].Normal); - } - - for (int faceIndex = 0; faceIndex < source.FaceToVertexIndicesStart.Length; ++faceIndex) - { - //This isn't exactly an optimal implementation- it uses a pretty inefficient gather, but any optimization can wait for it being a problem. - var vertexIndex = source.FaceVertexIndices[source.FaceToVertexIndicesStart[faceIndex]]; - BundleIndexing.GetBundleIndices(faceIndex, out var bundleIndex, out var indexInBundle); - Vector3Wide.ReadSlot(ref targetPoints[vertexIndex.BundleIndex], vertexIndex.InnerIndex, out var point); - Vector3Wide.ReadSlot(ref targetBoundingPlanes[bundleIndex].Normal, indexInBundle, out var normal); - GatherScatter.Get(ref targetBoundingPlanes[bundleIndex].Offset, indexInBundle) = Vector3.Dot(point, normal); - } - - //Clear any trailing bounding plane data to keep it from contributing. - var boundingPlaneCapacity = targetBoundingPlanes.Length * Vector.Count; - for (int i = source.FaceToVertexIndicesStart.Length; i < boundingPlaneCapacity; ++i) - { - BundleIndexing.GetBundleIndices(i, out var bundleIndex, out var innerIndex); - ref var offsetInstance = ref GatherScatter.GetOffsetInstance(ref targetBoundingPlanes[bundleIndex], innerIndex); - Vector3Wide.WriteFirst(default, ref offsetInstance.Normal); - GatherScatter.GetFirst(ref offsetInstance.Offset) = float.MinValue; - } - } - - /// - /// Creates a transformed copy of a convex hull. FaceVertexIndices and FaceToVertexIndicesStart buffers from the source are reused in the copy target. - /// Note that disposing two convex hulls with the same buffers will cause errors; disposal must be handled carefully to avoid double freeing the shared buffers. - /// - /// Source convex hull to copy. - /// Transform to apply to the hull points. - /// Pool from which to allocate the new hull's points and bounding planes buffers. - /// Target convex hull to copy into. FaceVertexIndices and FaceToVertexIndicesStart buffers are reused from the source. - public static void CreateTransformedShallowCopy(in ConvexHull source, in Matrix3x3 transform, BufferPool pool, out ConvexHull target) - { - pool.Take(source.Points.Length, out target.Points); - pool.Take(source.BoundingPlanes.Length, out target.BoundingPlanes); - CreateTransformedCopy(source, transform, target.Points, target.BoundingPlanes); - target.FaceVertexIndices = source.FaceVertexIndices; - target.FaceToVertexIndicesStart = source.FaceToVertexIndicesStart; - } - - /// - /// Creates a transformed copy of a convex hull. Unique FaceVertexIndices and FaceToVertexIndicesStart buffers are allocated for the copy target. - /// - /// Source convex hull to copy. - /// Transform to apply to the hull points. - /// Pool from which to allocate the new hull's buffers. - /// Target convex hull to copy into. - public static void CreateTransformedCopy(in ConvexHull source, in Matrix3x3 transform, BufferPool pool, out ConvexHull target) - { - pool.Take(source.Points.Length, out target.Points); - pool.Take(source.BoundingPlanes.Length, out target.BoundingPlanes); - pool.Take(source.FaceVertexIndices.Length, out target.FaceVertexIndices); - pool.Take(source.FaceToVertexIndicesStart.Length, out target.FaceToVertexIndicesStart); - CreateTransformedCopy(source, transform, target.Points, target.BoundingPlanes); - source.FaceVertexIndices.CopyTo(0, target.FaceVertexIndices, 0, target.FaceVertexIndices.Length); - source.FaceToVertexIndicesStart.CopyTo(0, target.FaceToVertexIndicesStart, 0, target.FaceToVertexIndicesStart.Length); - } - - } -} +using BepuUtilities; +using BepuUtilities.Collections; +using BepuUtilities.Memory; +using System; +using System.Collections.Generic; +using System.Diagnostics; +using System.Numerics; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +namespace BepuPhysics.Collidables +{ + /// + /// Stores references to the points composing one of a convex hull's faces. + /// + public struct HullFace + { + public Buffer OriginalVertexMapping; + public Buffer VertexIndices; + + /// + /// Gets the number of vertices in the face. + /// + public int VertexCount + { + [MethodImpl(MethodImplOptions.AggressiveInlining)] + get { return VertexIndices.Length; } + } + + /// + /// Gets the index of the vertex associated with the given face vertex index in the source point set. + /// + /// Index into the face's vertex list. + /// Index of the vertex associated with the given face vertex index in the source point set. + public int this[int index] + { + [MethodImpl(MethodImplOptions.AggressiveInlining)] + get { return OriginalVertexMapping[VertexIndices[index]]; } + } + } + + /// + /// Raw data representing a convex hull. + /// + /// This is not yet transformed into a runtime format. It requires additional processing to be used in a ConvexHull shape; see ConvexHullHelper.ProcessHull. + public struct HullData + { + /// + /// Mapping of points on the convex hull back to the original point set. + /// + public Buffer OriginalVertexMapping; + /// + /// List of indices composing the faces of the hull. Individual faces indexed by the FaceIndices. + /// + public Buffer FaceVertexIndices; + /// + /// Starting index in the FaceVertexIndices for each face. + /// + public Buffer FaceStartIndices; + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void GetFace(int faceIndex, out HullFace face) + { + var nextFaceIndex = faceIndex + 1; + var start = FaceStartIndices[faceIndex]; + var end = nextFaceIndex == FaceStartIndices.Length ? FaceVertexIndices.Length : FaceStartIndices[nextFaceIndex]; + FaceVertexIndices.Slice(start, end - start, out face.VertexIndices); + face.OriginalVertexMapping = OriginalVertexMapping; + } + + public void Dispose(BufferPool pool) + { + pool.Return(ref OriginalVertexMapping); + //The other allocations may not exist if the hull is degenerate. + if (FaceVertexIndices.Allocated) + pool.Return(ref FaceVertexIndices); + if (FaceStartIndices.Allocated) + pool.Return(ref FaceStartIndices); + } + } + + /// + /// Helper methods to create and process convex hulls from point clouds. + /// + public static class ConvexHullHelper + { + static void FindExtremeFace( + in Vector3Wide basisX, in Vector3Wide basisY, in Vector3Wide basisOrigin, in EdgeEndpoints sourceEdgeEndpoints, ref Buffer pointBundles, in Vector indexOffsets, Buffer allowVertices, int pointCount, + ref Buffer> projectedOnX, ref Buffer> projectedOnY, in Vector planeEpsilon, ref QuickList vertexIndices, out Vector3 faceNormal) + { + Debug.Assert(projectedOnX.Length >= pointBundles.Length && projectedOnY.Length >= pointBundles.Length && vertexIndices.Count == 0 && vertexIndices.Span.Length >= pointBundles.Length * Vector.Count); + //Find the candidate-basisOrigin which has the smallest angle with basisY when projected onto the plane spanned by basisX and basisY. + //angle = atan(y / x) + //tanAngle = y / x + //x is guaranteed to be nonnegative, so its sign doesn't change. + //tanAngle is monotonically increasing with respect to y / x, so a higher angle corresponds to a higher y/x, always. + //We can then compare samples 0 and 1 using: + //tanAngle0 > tanAngle1 + //y0 / x0 > y1 / x1 + //y0 * x1 > y1 * x0 + Vector3Wide.Subtract(pointBundles[0], basisOrigin, out var toCandidate); + ref var x = ref projectedOnX[0]; + ref var y = ref projectedOnY[0]; + Vector3Wide.Dot(basisX, toCandidate, out x); + //If x is negative, that means some numerical issue has resulted in a point beyond the bounding plane that generated this face request. + //We'll treat it as if it's on the plane. (The reason we bother with this clamp is the sign assumption built into our angle comparison, detailed above.) + x = Vector.Max(Vector.Zero, x); + Vector3Wide.Dot(basisY, toCandidate, out y); + var bestY = y; + var bestX = x; + //Ignore the source edge. + var edgeIndexA = new Vector(sourceEdgeEndpoints.A); + var edgeIndexB = new Vector(sourceEdgeEndpoints.B); + //Note that any slot that would have been coplanar with the generating face *and* behind the edge (that is, a vertex almost certainly associated with the generating face) is ignored. + //Without this condition, it's possible for numerical cycles to occur where a face finds itself over and over again. + var allowVertexBundles = allowVertices.As>(); + var ignoreSlot = Vector.BitwiseOr( + Vector.BitwiseOr( + Vector.OnesComplement(allowVertexBundles[0]), + Vector.BitwiseAnd(Vector.LessThanOrEqual(bestX, planeEpsilon), Vector.LessThanOrEqual(bestY, planeEpsilon))), + Vector.BitwiseOr(Vector.Equals(indexOffsets, edgeIndexA), Vector.Equals(indexOffsets, edgeIndexB))); + bestX = Vector.ConditionalSelect(ignoreSlot, Vector.One, bestX); + bestY = Vector.ConditionalSelect(ignoreSlot, new Vector(float.MinValue), bestY); + var bestIndices = indexOffsets; + for (int i = 1; i < pointBundles.Length; ++i) + { + Vector3Wide.Subtract(pointBundles[i], basisOrigin, out toCandidate); + x = ref projectedOnX[i]; + y = ref projectedOnY[i]; + Vector3Wide.Dot(basisX, toCandidate, out x); + x = Vector.Max(Vector.Zero, x); //Same as earlier- protect against numerical error finding points beyond the bounding plane. + Vector3Wide.Dot(basisY, toCandidate, out y); + + var candidateIndices = indexOffsets + new Vector(i << BundleIndexing.VectorShift); + ignoreSlot = Vector.BitwiseOr( + Vector.BitwiseOr( + Vector.OnesComplement(allowVertexBundles[i]), + Vector.BitwiseAnd(Vector.LessThanOrEqual(x, planeEpsilon), Vector.LessThanOrEqual(y, planeEpsilon))), + Vector.BitwiseOr(Vector.Equals(candidateIndices, edgeIndexA), Vector.Equals(candidateIndices, edgeIndexB))); + var useCandidate = Vector.AndNot(Vector.GreaterThan(y * bestX, bestY * x), ignoreSlot); + + bestY = Vector.ConditionalSelect(useCandidate, y, bestY); + bestX = Vector.ConditionalSelect(useCandidate, x, bestX); + bestIndices = Vector.ConditionalSelect(useCandidate, candidateIndices, bestIndices); + } + var bestYNarrow = bestY[0]; + var bestXNarrow = bestX[0]; + var bestIndexNarrow = bestIndices[0]; + for (int i = 1; i < Vector.Count; ++i) + { + var candidateNumerator = bestY[i]; + var candidateDenominator = bestX[i]; + if (candidateNumerator * bestXNarrow > bestYNarrow * candidateDenominator) + { + bestYNarrow = candidateNumerator; + bestXNarrow = candidateDenominator; + bestIndexNarrow = bestIndices[i]; + } + } + //We now have the best index, but there may have been multiple vertices on the same plane. Capture all of them at once by doing a second pass over the results. + //The plane normal we want to examine is (-bestY, bestX) / ||(-bestY, bestX)||. + //(This isn't wonderfully fast, but it's fairly simple. The alternatives are things like incrementally combining coplanar triangles as they are discovered + //or using a postpass that looks for coplanar triangles after they've been created.) + //Rotate the offset to point outward. + //Note: in unusual corner cases, the above may have accepted zero candidates resulting in a bestXNarrow = 1 and bestYNarrow = float.MinValue. + //Catching that and ensuring that a reasonable face normal is output avoids a bad face. + var candidateNormalDirection = new Vector2(-bestYNarrow, bestXNarrow); + var length = candidateNormalDirection.Length(); + var projectedPlaneNormalNarrow = float.IsFinite(length) ? candidateNormalDirection / length : new Vector2(1, 0); + Vector2Wide.Broadcast(projectedPlaneNormalNarrow, out var projectedPlaneNormal); + Vector3Wide.ReadFirst(basisX, out var basisXNarrow); + Vector3Wide.ReadFirst(basisY, out var basisYNarrow); + faceNormal = basisXNarrow * projectedPlaneNormalNarrow.X + basisYNarrow * projectedPlaneNormalNarrow.Y; + + //if (sourceEdgeEndpoints.A != sourceEdgeEndpoints.B) + //{ + // BundleIndexing.GetBundleIndices(sourceEdgeEndpoints.A, out var bundleA, out var innerA); + // BundleIndexing.GetBundleIndices(sourceEdgeEndpoints.B, out var bundleB, out var innerB); + // BundleIndexing.GetBundleIndices(bestIndexNarrow, out var bundleC, out var innerC); + // Vector3Wide.ReadSlot(ref pointBundles[bundleA], innerA, out var a); + // Vector3Wide.ReadSlot(ref pointBundles[bundleB], innerB, out var b); + // Vector3Wide.ReadSlot(ref pointBundles[bundleC], innerC, out var c); + // var faceNormalFromCross = Vector3.Normalize(Vector3.Cross(c - a, b - a)); + // var testDot = Vector3.Dot(faceNormalFromCross, faceNormal); + // var faceNormalError = faceNormal - faceNormalFromCross; + // faceNormal = faceNormalFromCross; + //} + + Vector3Wide.Broadcast(faceNormal, out var faceNormalWide); + for (int i = 0; i < pointBundles.Length; ++i) + { + var dot = projectedOnX[i] * projectedPlaneNormal.X + projectedOnY[i] * projectedPlaneNormal.Y; + //var dot2 = Vector3Wide.Dot(pointBundles[i] - basisOrigin, faceNormalWide); + //var error = dot2 - dot; + //if (Vector.GreaterThanAny(Vector.Abs(error), planeEpsilon)) + //{ + // for (int j = 0; j < Vector.Count; ++j) + // { + // if (MathF.Abs(error[j]) > planeEpsilon[j]) + // { + // Console.WriteLine($"error: {error[j]}"); + // } + // } + //} + var coplanar = Vector.LessThanOrEqual(Vector.Abs(dot), planeEpsilon); + if (Vector.LessThanAny(coplanar, Vector.Zero)) + { + var bundleBaseIndex = i << BundleIndexing.VectorShift; + var localIndexMaximum = pointCount - bundleBaseIndex; + if (localIndexMaximum > Vector.Count) + localIndexMaximum = Vector.Count; + for (int j = 0; j < localIndexMaximum; ++j) + { + var vertexIndex = bundleBaseIndex + j; + if (coplanar[j] < 0 && allowVertices[vertexIndex] != 0) + { + vertexIndices.AllocateUnsafely() = vertexIndex; + } + } + } + } + //Vector3Wide.ReadFirst(basisX, out var basisXNarrow); + //Vector3Wide.ReadFirst(basisY, out var basisYNarrow); + //faceNormal = basisXNarrow * projectedPlaneNormalNarrow.X + basisYNarrow * projectedPlaneNormalNarrow.Y; + } + + /// + /// Finds the next index in the 2D hull of a face on the 3D hull using gift wrapping. + /// + /// Start location of the next edge to identify. + /// 2D direction of the previously identified edge. + /// Epsilon within which to consider points to be coplanar (or here, in the 2D case, collinear). + /// Points composing the hull face projected onto the face's 2D basis. + /// Index of the point in facePoints which is the end point for the next edge segment as identified by gift wrapping. + static int FindNextIndexForFaceHull2(Vector2 start, Vector2 previousEdgeDirection, float planeEpsilon, ref QuickList facePoints) + { + //Find the candidate-basisOrigin which has the smallest angle with basisY when projected onto the plane spanned by basisX and basisY. + //angle = atan(y / x) + //tanAngle = y / x + //x is guaranteed to be nonnegative, so its sign doesn't change. + //tanAngle is monotonically increasing with respect to y / x, so a higher angle corresponds to a higher y/x, always. + //We can then compare samples 0 and 1 using: + //tanAngle0 > tanAngle1 + //y0 / x0 > y1 / x1 + //y0 * x1 > y1 * x0 + var basisX = new Vector2(previousEdgeDirection.Y, -previousEdgeDirection.X); + var basisY = -previousEdgeDirection; + var bestX = 1f; + var bestY = float.MaxValue; + int bestIndex = -1; + var bestAngle = float.Atan(bestY / bestX); + for (int i = 0; i < facePoints.Count; ++i) + { + var candidate = facePoints[i]; + var toCandidate = candidate - start; + //If x is negative, that means some numerical issue has resulted in a point beyond the bounding plane that generated this face request. + //We'll treat it as if it's on the plane. (The reason we bother with this clamp is the sign assumption built into our angle comparison, detailed above.) + var x = float.Max(0, Vector2.Dot(toCandidate, basisX)); + var y = Vector2.Dot(toCandidate, basisY); + + //Note that any slot that would have been coplanar with the generating face *and* behind the edge (that is, a vertex almost certainly associated with the generating face) is ignored. + //Without this condition, it's possible for numerical cycles to occur where a face finds itself over and over again. + var ignoreSlot = x <= planeEpsilon && y >= -planeEpsilon; + var useCandidate = (y * bestX < bestY * x) && !ignoreSlot; + var candidateAngle = float.Atan(y / x); + var useCandidateTest = candidateAngle < bestAngle && !ignoreSlot; + if (useCandidateTest != useCandidate && float.Abs(candidateAngle - bestAngle) > 1e-5f) + { + Console.WriteLine("dd"); + } + if (useCandidate) + { + bestY = y; + bestX = x; + bestIndex = i; + bestAngle = candidateAngle; + } + } + //If no next index was identified, then the face is degenerate. + //Stop now to prevent the postpass from identifying some nonsense derived from a garbage plane. + Console.WriteLine($"Best index: {bestIndex}, progress along PREVIOUS: {-bestY}, best angle: {bestAngle + float.Pi / 2}"); + if (bestIndex == -1) + return -1; + + //We now have the best index, but there may have been multiple vertices on the same plane. Capture all of them at once by doing a second pass over the results. + //Note that incrementally tracking distance during the above loop is more complex than it first appears; we want the most distant point within the plane epsilon around best angle, + //but we don't know the best angle until after the loop terminates. A distant point early in the list could be kicked out by a later change in the plane angle. A postpass makes that easy to discover. + //The plane normal we want to examine is (-bestY, bestX) / ||(-bestY, bestX)||. + //Rotate the offset to point outward. + //Note: in unusual corner cases, the above may have accepted zero candidates resulting in a bestXNarrow = 1 and bestYNarrow = float.MinValue. + //Catching that and ensuring that a reasonable face normal is output avoids a bad face. + var projectedBestEdgeDirection = new Vector2(bestX, bestY); + var length = projectedBestEdgeDirection.Length(); + //Note that the projected face normal is in terms of basisX and basisY, not the original basis facePoints are built on. + projectedBestEdgeDirection = float.IsFinite(length) ? projectedBestEdgeDirection / length : new Vector2(1, 0); + //Transform the projected normal back into the basis of facePoints. + var edgeDirection = basisX * projectedBestEdgeDirection.X + basisY * projectedBestEdgeDirection.Y; + var faceNormal = new Vector2(-edgeDirection.Y, edgeDirection.X); + + float distance = 0; + int mostDistantIndex = -1; + for (int i = 0; i < facePoints.Count; ++i) + { + var candidate = facePoints[i]; + var toCandidate = candidate - start; + var alongNormal = Vector2.Dot(toCandidate, faceNormal); + + //# DEBUG + var x = float.Max(0, Vector2.Dot(toCandidate, basisX)); + var y = Vector2.Dot(toCandidate, basisY); + var ignoreSlot = x <= planeEpsilon && y >= -planeEpsilon; + var useCandidate = (y * bestX < bestY * x) && !ignoreSlot; + var candidateAngle = float.Atan(y / x); + var useCandidateTest = candidateAngle < bestAngle && !ignoreSlot; + //# DEBUG + if (alongNormal > planeEpsilon && useCandidateTest) + Console.WriteLine("Okay, so we definitely should have selected this one earlier."); + if (alongNormal > planeEpsilon && !ignoreSlot) + Console.WriteLine("It looks like we've found a point significantly beyond the bounding plane we just identified; how's that possible?"); + + //TODO: Here and in the 3d variant: should you really be using the abs? That rejects anything that is numerically too far outside the face, which... should be rare, but we shouldn't let that point just corrupt things later. + if (float.Abs(alongNormal) <= planeEpsilon) + { + var alongEdge = Vector2.Dot(toCandidate, edgeDirection); + var rawDistance = float.Sqrt(alongEdge * alongEdge + alongNormal * alongNormal); + //if (float.Abs(rawDistance) - float.Abs(alongEdge) > 1e-6f) + // Console.WriteLine("dd"); + + if (alongEdge > distance) + { + distance = alongEdge; + mostDistantIndex = i; + } + } + } + if (mostDistantIndex >= 0) + { + var mostDistantX = Vector2.Dot(basisX, facePoints[mostDistantIndex] - start); + var mostDistantY = Vector2.Dot(basisY, facePoints[mostDistantIndex] - start); + + var testProjectedBestEdgeDirection = new Vector2(bestX, bestY); + var testLength = projectedBestEdgeDirection.Length(); + //Note that the projected face normal is in terms of basisX and basisY, not the original basis facePoints are built on. + testProjectedBestEdgeDirection = float.IsFinite(length) ? testProjectedBestEdgeDirection / length : new Vector2(1, 0); + //Transform the projected normal back into the basis of facePoints. + var testEdgeDirection = basisX * testProjectedBestEdgeDirection.X + basisY * testProjectedBestEdgeDirection.Y; + var testFaceNormal = new Vector2(-testEdgeDirection.Y, testEdgeDirection.X); + var testDot = Vector2.Dot(edgeDirection, testEdgeDirection); + if (testDot < 1 - 1e-6f) + { + Console.WriteLine("Sdf"); + } + var mostDistance = float.Sqrt(mostDistantX * mostDistantX + mostDistantY * mostDistantY); + var bestDistance = float.Sqrt(bestX * bestX + bestY * bestY); + if (mostDistance < bestDistance) + Console.WriteLine("uh"); + + var mostDistantNormal = Vector2.Dot(faceNormal, facePoints[mostDistantIndex] - start); + var mostDistantEdge = Vector2.Dot(edgeDirection, facePoints[mostDistantIndex] - start); + var bestNormal = Vector2.Dot(faceNormal, facePoints[bestIndex] - start); + var bestEdge = Vector2.Dot(edgeDirection, facePoints[bestIndex] - start); + + if (mostDistantEdge < bestEdge) + Console.WriteLine("uh"); + + Console.WriteLine($"Progress along CURRENT edge: {distance}"); + } + else + { + } + return mostDistantIndex == -1 ? bestIndex : mostDistantIndex; + + + } + + + static int FindNextIndexForFaceHull(Vector2 start, Vector2 previousEdgeDirection, float planeEpsilon, ref QuickList facePoints) + { + //Use a AOS version since the number of points on a given face will tend to be very small in most cases. + //Same idea as the 3d version- find the next edge which is closest to the previous edge. Not going to worry about collinear points here for now. + var bestIndex = -1; + float best = -float.MaxValue; + float bestDistanceSquared = 0; + var startToCandidate = facePoints[0] - start; + var xDirection = new Vector2(previousEdgeDirection.Y, -previousEdgeDirection.X); + var candidateX = Vector2.Dot(startToCandidate, xDirection); + var candidateY = Vector2.Dot(startToCandidate, previousEdgeDirection); + var currentEdgeDirectionX = previousEdgeDirection.X; + var currentEdgeDirectionY = previousEdgeDirection.Y; + if (candidateX > 0) + { + best = candidateY / candidateX; + bestIndex = 0; + bestDistanceSquared = candidateX * candidateX + candidateY * candidateY; + var inverseBestDistance = 1f / MathF.Sqrt(bestDistanceSquared); + currentEdgeDirectionX = candidateX * inverseBestDistance; + currentEdgeDirectionY = candidateY * inverseBestDistance; + } + for (int i = 1; i < facePoints.Count; ++i) + { + startToCandidate = facePoints[i] - start; + candidateY = Vector2.Dot(startToCandidate, previousEdgeDirection); + candidateX = Vector2.Dot(startToCandidate, xDirection); + //Any points that are collinear *with the previous edge* cannot be a part of the current edge without numerical failure; the previous edge should include them. + if (candidateX <= 0) + { + if (candidateY > 0) + Console.WriteLine("Violation of expectation"); + //Debug.Assert(candidateY <= 0, + // "Previous edge should include any collinear points, so this edge should not see any further collinear points beyond its start." + + // "If you run into this, it implies you've found some content that violates the convex huller's assumptions, and I'd appreciate it if you reported it on github.com/bepu/bepuphysics2/issues!" + + // "A .obj or other simple demos-compatible reproduction case would help me fix it."); + continue; + } + //We accept a candidate if it is either: + //1) collinear with the previous best by the plane epsilon test BUT is more distant, or + //2) has a greater angle than the previous best. + var planeOffset = -candidateX * currentEdgeDirectionY + candidateY * currentEdgeDirectionX; + if (MathF.Abs(planeOffset) < planeEpsilon) + { + //The candidate is collinear. Only accept it if it's further away. + if (candidateX * candidateX + candidateY * candidateY <= bestDistanceSquared) + { + continue; + } + } + else if (candidateY < best * candidateX) //candidateY / candidateX < best, given candidate X > 0; just avoiding a division for bulk testing. + { + //Candidate is a smaller angle. Rejected. + continue; + } + best = candidateY / candidateX; + bestDistanceSquared = candidateX * candidateX + candidateY * candidateY; + var inverseBestDistance = 1f / MathF.Sqrt(bestDistanceSquared); + currentEdgeDirectionX = candidateX * inverseBestDistance; + currentEdgeDirectionY = candidateY * inverseBestDistance; + bestIndex = i; + } + //Note that this can return -1 if all points were on top of the start. + return bestIndex; + } + + static void ReduceFace(ref QuickList faceVertexIndices, Vector3 faceNormal, Span points, float planeEpsilon, ref QuickList facePoints, ref Buffer allowVertex, ref QuickList reducedIndices) + { + Debug.Assert(facePoints.Count == 0 && reducedIndices.Count == 0 && facePoints.Span.Length >= faceVertexIndices.Count && reducedIndices.Span.Length >= faceVertexIndices.Count); + for (int i = faceVertexIndices.Count - 1; i >= 0; --i) + { + //TODO: This isn't really necessary (conditioning on a small change). + //Face merges may see this codepath because the original rawFaceVertexIndices may contain now-disallowed vertices. + //We don't really need to *track* those, though; we could just use the reducedIndices and then this would never be required. + //It's mostly a matter of legacy- previously, we accumulated everything without asking about whether it was allowed, and relied on ReduceFace to clean it up. + //That opened a door for an infinite loop, so it got changed. + if (allowVertex[faceVertexIndices[i]] == 0) + faceVertexIndices.RemoveAt(i); + } + if (faceVertexIndices.Count <= 3) + { + //Too small to require computing a hull. Copy directly. + for (int i = 0; i < faceVertexIndices.Count; ++i) + { + reducedIndices.AllocateUnsafely() = faceVertexIndices[i]; + } + if (faceVertexIndices.Count == 3) + { + //No point in running a full reduction, but we do need to check the winding of the triangle. + ref var a = ref points[reducedIndices[0]]; + ref var b = ref points[reducedIndices[1]]; + ref var c = ref points[reducedIndices[2]]; + //Counterclockwise should result in face normal pointing outward. + var ab = b - a; + var ac = c - a; + var uncalibratedNormal = Vector3.Cross(ab, ac); + if (uncalibratedNormal.LengthSquared() < 1e-14f) + { + //The face is degenerate. + if (ab.LengthSquared() > 1e-14f) + { + allowVertex[reducedIndices[2]] = 0; + reducedIndices.FastRemoveAt(2); + } + else if (ac.LengthSquared() > 1e-14f) + { + allowVertex[reducedIndices[1]] = 0; + reducedIndices.FastRemoveAt(1); + } + else + { + allowVertex[reducedIndices[1]] = 0; + allowVertex[reducedIndices[2]] = 0; + reducedIndices.Count = 1; + } + } + else + { + if (Vector3.Dot(faceNormal, uncalibratedNormal) < 0) + Helpers.Swap(ref reducedIndices[0], ref reducedIndices[1]); + } + } + return; + } + Helpers.BuildOrthonormalBasis(faceNormal, out var basisX, out var basisY); + Vector2 centroid = default; + for (int i = 0; i < faceVertexIndices.Count; ++i) + { + ref var source = ref points[faceVertexIndices[i]]; + ref var facePoint = ref facePoints.AllocateUnsafely(); + facePoint = new Vector2(Vector3.Dot(basisX, source), Vector3.Dot(basisY, source)); + centroid += facePoint; + } + centroid /= faceVertexIndices.Count; + var greatestDistanceSquared = -1f; + var initialIndex = 0; + for (int i = 0; i < faceVertexIndices.Count; ++i) + { + ref var facePoint = ref facePoints[i]; + var distanceSquared = (facePoint - centroid).LengthSquared(); + if (greatestDistanceSquared < distanceSquared) + { + greatestDistanceSquared = distanceSquared; + initialIndex = i; + } + } + + if (greatestDistanceSquared < 1e-14f) + { + //The face is degenerate. + for (int i = 0; i < faceVertexIndices.Count; ++i) + { + allowVertex[faceVertexIndices[i]] = 0; + } + return; + } + var greatestDistance = (float)Math.Sqrt(greatestDistanceSquared); + var initialOffsetDirection = (facePoints[initialIndex] - centroid) / greatestDistance; + var previousEdgeDirection = new Vector2(initialOffsetDirection.Y, -initialOffsetDirection.X); + reducedIndices.AllocateUnsafely() = faceVertexIndices[initialIndex]; + + var previousEndIndex = initialIndex; + Console.WriteLine("Entering reduce face loop."); + bool reductionTerminatedSuccessfully = false; + for (int i = 0; i < facePoints.Count; ++i) + { + Console.WriteLine($"Reduce face loop iteration start. Previous end index: {previousEndIndex}, previous edge dir: {previousEdgeDirection}."); + Console.WriteLine($"World edge dir: {previousEdgeDirection.X * basisX + previousEdgeDirection.Y * basisY}"); + var nextIndex = FindNextIndexForFaceHull2(facePoints[previousEndIndex], previousEdgeDirection, planeEpsilon, ref facePoints); + //This can return -1 in the event of a completely degenerate face. + if (nextIndex == -1 || reducedIndices.Contains(faceVertexIndices[nextIndex])) + { + reductionTerminatedSuccessfully = true; + if (nextIndex >= 0) + { + //Wrapped around to a repeated index. + //Note that hitting a repeated index is not necessarily because we found the initial index again; the initial index may have been numerically undiscoverable. + //In this case, we don't actually want our initial index to be in the reduced indices. + //In fact, we don't want *any* of the indices that aren't part of the identified face cycle, so look up the first index in the cycle and remove anything before that. + var cycleStartIndex = reducedIndices.IndexOf(faceVertexIndices[nextIndex]); + Debug.Assert(cycleStartIndex >= 0); + if (cycleStartIndex > 0) + { + //Note that order matters; can't do a last element swapping remove. + reducedIndices.Span.CopyTo(cycleStartIndex, reducedIndices.Span, 0, reducedIndices.Count - cycleStartIndex); + reducedIndices.Count -= cycleStartIndex; + } + } + break; + } + reducedIndices.AllocateUnsafely() = faceVertexIndices[nextIndex]; + previousEdgeDirection = Vector2.Normalize(facePoints[nextIndex] - facePoints[previousEndIndex]); + previousEndIndex = nextIndex; + } + if (!reductionTerminatedSuccessfully) + { + throw new Exception("Hull face reduction failed to terminate. This should not happen; please report it on github.com/bepu/bepuphysics2/issues, ideally with a point set that triggered this exception!"); + } + Console.WriteLine("finished reduce face loop."); + + //Ignore any vertices which were not on the outer boundary of the face. + for (int i = 0; i < faceVertexIndices.Count; ++i) + { + var index = faceVertexIndices[i]; + if (!reducedIndices.Contains(index)) + { + allowVertex[index] = 0; + } + } + + } + + [StructLayout(LayoutKind.Explicit)] + public struct EdgeEndpoints : IEqualityComparerRef + { + [FieldOffset(0)] + public int A; + [FieldOffset(4)] + public int B; + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool Equals(ref EdgeEndpoints a, ref EdgeEndpoints b) + { + return Unsafe.As(ref a.A) == Unsafe.As(ref b.A) || (a.A == b.B && a.B == b.A); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public int Hash(ref EdgeEndpoints item) + { + return item.A ^ item.B; + } + + public override string ToString() + { + return $"({A}, {B})"; + } + } + + /// + /// Tracks the faces associated with a detected surface edge. + /// During hull calculation, surface edges that have only one face associated with them should have an outstanding entry in the edges to visit set. + /// Surface edges can never validly have more than two faces associated with them. Two means that an edge is 'complete', or should be. + /// Detecting a third edge implies an error condition. By tracking *which* faces were associated with an edge, we can attempt to fix the error. + /// This type of error tends to occur when there is a numerical disagreement about what vertices are coplanar with a face. + /// One iteration could find what it thinks is a complete face, and a later iteration ends up finding more vertices *including* the ones already contained in the previous face. + /// That's a recipe for excessive edge faces, but it's also a direct indicator that we should merge the involved faces for being close enough to coplanar that the error happened in the first place. + /// + struct EdgeFaceIndices + { + public int FaceA; + public int FaceB; + public bool Complete => FaceA >= 0 && FaceB >= 0; + + public EdgeFaceIndices(int initialFaceIndex) + { + FaceA = initialFaceIndex; + FaceB = -1; + } + + + } + + static void RemoveFaceFromEdge(int a, int b, int faceIndex, ref QuickDictionary facesForEdges) + { + EdgeEndpoints edge; + edge.A = a; + edge.B = b; + var exists = facesForEdges.GetTableIndices(ref edge, out _, out var index); + Debug.Assert(exists, "Whoa something weird happened here! A face was created, but there's a missing edge for its face?"); + ref var facesForEdge = ref facesForEdges.Values[index]; + Debug.Assert(faceIndex == facesForEdge.FaceA || faceIndex == facesForEdge.FaceB, "If you're trying to remove a face index from the edge, it better be in the edge!"); + if (facesForEdge.FaceA == faceIndex) + { + facesForEdge.FaceA = facesForEdge.FaceB; + facesForEdge.FaceB = -1; + if (facesForEdge.FaceA == -1) + { + //This edge no longer has any faces associated with it. + facesForEdges.FastRemove(ref edge); + } + } + else + { + facesForEdge.FaceB = -1; + } + //Note that we do *not* add the edge back into the 'edges to test' list when transitioning from 2 faces to 1 face for the edge. + //This function is invoked when an edge is being deleted because it's related to a deleted face. + //There are two possible outcomes: + //1. The completion of the merge will see a face added back to this edge, + //2. it's an orphaned internal edge that should not be tested. + } + + internal struct EarlyFace + { + public QuickList VertexIndices; + public bool Deleted; + public Vector3 Normal; + } + + struct EdgeToTest + { + public EdgeEndpoints Endpoints; + public Vector3 FaceNormal; + public int FaceIndex; + } + + + static void AddFace(ref QuickList faces, BufferPool pool, Vector3 normal, QuickList vertexIndices) + { + ref var face = ref faces.Allocate(pool); + face = new EarlyFace { Normal = normal, VertexIndices = new QuickList(vertexIndices.Count, pool) }; + face.VertexIndices.AddRangeUnsafely(vertexIndices); + } + + static void AddFaceToEdgesAndTestList(BufferPool pool, + ref QuickList reducedFaceIndices, + ref QuickList edgesToTest, + ref QuickDictionary facesForEdges, + Vector3 faceNormal, int newFaceIndex) + { + facesForEdges.EnsureCapacity(facesForEdges.Count + reducedFaceIndices.Count, pool); + var previousIndex = reducedFaceIndices[reducedFaceIndices.Count - 1]; + for (int i = 0; i < reducedFaceIndices.Count; ++i) + { + EdgeEndpoints endpoints; + endpoints.A = previousIndex; + endpoints.B = reducedFaceIndices[i]; + previousIndex = endpoints.B; + if (facesForEdges.GetTableIndices(ref endpoints, out var tableIndex, out var elementIndex)) + { + ref var edgeFaceCount = ref facesForEdges.Values[elementIndex]; + Debug.Assert(edgeFaceCount.FaceB == -1, + "While we let execution continue, this is an error condition and implies overlapping triangles are being generated." + + "This tends to happen when there are many near-coplanar vertices, so numerical tolerances across different faces cannot consistently agree."); + edgeFaceCount.FaceB = newFaceIndex; + } + else + { + //This edge is not yet claimed by any edge. Claim it for the new face and add the edge for further testing. + EdgeToTest nextEdgeToTest; + nextEdgeToTest.Endpoints = endpoints; + nextEdgeToTest.FaceNormal = faceNormal; + nextEdgeToTest.FaceIndex = newFaceIndex; + facesForEdges.Keys[facesForEdges.Count] = nextEdgeToTest.Endpoints; + facesForEdges.Values[facesForEdges.Count] = new EdgeFaceIndices(newFaceIndex); + //Use the encoding- all indices are offset by 1 since 0 represents 'empty'. + facesForEdges.Table[tableIndex] = ++facesForEdges.Count; + edgesToTest.Allocate(pool) = nextEdgeToTest; + } + } + } + + static void AddIfNotPresent(ref QuickList list, int value, BufferPool pool) + { + if (!list.Contains(value)) + list.Allocate(pool) = value; + } + + public struct DebugStep + { + public EdgeEndpoints SourceEdge; + public List Raw; + public List Reduced; + public bool[] AllowVertex; + public Vector3 FaceNormal; + public Vector3 BasisX; + public Vector3 BasisY; + public List FaceStarts; + public List FaceIndices; + public bool[] FaceDeleted; + public int[] MergedFaceIndices; + public int FaceIndex; + public Vector3[] FaceNormals; + + internal DebugStep(EdgeEndpoints sourceEdge, ref QuickList raw, Vector3 faceNormal, Vector3 basisX, Vector3 basisY, ref QuickList reduced, ref Buffer allowVertex, ref QuickList faces, Span mergedFaceIndices, int faceIndex) + { + SourceEdge = sourceEdge; + FaceNormal = faceNormal; + BasisX = basisX; + BasisY = basisY; + Raw = new List(); + for (int i = 0; i < raw.Count; ++i) + { + Raw.Add(raw[i]); + } + Reduced = new List(); + for (int i = 0; i < reduced.Count; ++i) + { + Reduced.Add(reduced[i]); + } + AllowVertex = new bool[allowVertex.Length]; + for (int i = 0; i < allowVertex.Length; ++i) + { + AllowVertex[i] = allowVertex[i] != 0; + } + FaceStarts = new List(faces.Count); + FaceIndices = new List(); + FaceDeleted = new bool[faces.Count]; + FaceNormals = new Vector3[faces.Count]; + for (int i = 0; i < faces.Count; ++i) + { + ref var face = ref faces[i]; + FaceStarts.Add(FaceIndices.Count); + for (int j = 0; j < face.VertexIndices.Count; ++j) + FaceIndices.Add(face.VertexIndices[j]); + FaceDeleted[i] = face.Deleted; + FaceNormals[i] = face.Normal; + } + MergedFaceIndices = mergedFaceIndices.ToArray(); + FaceIndex = faceIndex; + } + } + /// + /// Computes the convex hull of a set of points. + /// + /// Point set to compute the convex hull of. + /// Buffer pool to pull memory from when creating the hull. + /// Convex hull of the input point set. + public static void ComputeHull(Span points, BufferPool pool, out HullData hullData) + { + ComputeHull(points, pool, out hullData, out _); + } + + + /// + /// Computes the convex hull of a set of points. + /// + /// Point set to compute the convex hull of. + /// Buffer pool to pull memory from when creating the hull. + /// Convex hull of the input point set. + public static void ComputeHull(Span points, BufferPool pool, out HullData hullData, out List steps) + { + steps = new List(); + if (points.Length <= 0) + { + hullData = default; + return; + } + if (points.Length <= 3) + { + //If the input is too small to actually form a volumetric hull, just output the input directly. + pool.Take(points.Length, out hullData.OriginalVertexMapping); + for (int i = 0; i < points.Length; ++i) + { + hullData.OriginalVertexMapping[i] = i; + } + if (points.Length == 3) + { + pool.Take(1, out hullData.FaceStartIndices); + pool.Take(3, out hullData.FaceVertexIndices); + hullData.FaceStartIndices[0] = 0; + //No volume, so winding doesn't matter. + hullData.FaceVertexIndices[0] = 0; + hullData.FaceVertexIndices[1] = 1; + hullData.FaceVertexIndices[2] = 2; + } + else + { + hullData.FaceStartIndices = default; + hullData.FaceVertexIndices = default; + } + return; + } + var pointBundleCount = BundleIndexing.GetBundleCount(points.Length); + pool.Take(pointBundleCount, out var pointBundles); + //While it's not asymptotically optimal in general, gift wrapping is simple and easy to productively vectorize. + //As a first step, create an AOSOA version of the input data. + Vector3 centroid = default; + for (int i = 0; i < points.Length; ++i) + { + BundleIndexing.GetBundleIndices(i, out var bundleIndex, out var innerIndex); + ref var p = ref points[i]; + Vector3Wide.WriteSlot(p, innerIndex, ref pointBundles[bundleIndex]); + centroid += p; + } + centroid /= points.Length; + //Fill in the last few slots with the centroid. + //We avoid doing a bunch of special case work on the last partial bundle by just assuming it has a few extra redundant internal points. + var bundleSlots = pointBundles.Length * Vector.Count; + for (int i = points.Length; i < bundleSlots; ++i) + { + BundleIndexing.GetBundleIndices(i, out var bundleIndex, out var innerIndex); + Vector3Wide.WriteSlot(centroid, innerIndex, ref pointBundles[bundleIndex]); + } + + //Find a starting point. We'll use the one furthest from the centroid. + Vector3Wide.Broadcast(centroid, out var centroidBundle); + Helpers.FillVectorWithLaneIndices(out var mostDistantIndicesBundle); + var indexOffsetBundle = mostDistantIndicesBundle; + Vector3Wide.DistanceSquared(pointBundles[0], centroidBundle, out var distanceSquaredBundle); + for (int i = 1; i < pointBundles.Length; ++i) + { + var bundleIndices = new Vector(i << BundleIndexing.VectorShift) + indexOffsetBundle; + Vector3Wide.DistanceSquared(pointBundles[i], centroidBundle, out var distanceSquaredCandidate); + mostDistantIndicesBundle = Vector.ConditionalSelect(Vector.GreaterThan(distanceSquaredCandidate, distanceSquaredBundle), bundleIndices, mostDistantIndicesBundle); + distanceSquaredBundle = Vector.Max(distanceSquaredBundle, distanceSquaredCandidate); + } + var bestDistanceSquared = distanceSquaredBundle[0]; + var initialIndex = mostDistantIndicesBundle[0]; + for (int i = 1; i < Vector.Count; ++i) + { + var distanceCandidate = distanceSquaredBundle[i]; + if (distanceCandidate > bestDistanceSquared) + { + bestDistanceSquared = distanceCandidate; + initialIndex = mostDistantIndicesBundle[i]; + } + } + BundleIndexing.GetBundleIndices(initialIndex, out var mostDistantBundleIndex, out var mostDistantInnerIndex); + Vector3Wide.ReadSlot(ref pointBundles[mostDistantBundleIndex], mostDistantInnerIndex, out var initialVertex); + + //All further points will be found by picking an plane on which to project all vertices down onto, and then measuring the angle on that plane. + //We pick to basis directions along which to measure. For the second point, we choose a perpendicular direction arbitrarily. + var initialToCentroid = centroid - initialVertex; + var initialDistance = initialToCentroid.Length(); + if (initialDistance < 1e-7f) + { + //The point set lacks any volume or area. + pool.Take(1, out hullData.OriginalVertexMapping); + hullData.OriginalVertexMapping[0] = 0; + hullData.FaceStartIndices = default; + hullData.FaceVertexIndices = default; + pool.Return(ref pointBundles); + return; + } + Vector3Wide.Broadcast(initialToCentroid / initialDistance, out var initialBasisX); + Helpers.FindPerpendicular(initialBasisX, out var initialBasisY); //(broadcasted before FindPerpendicular just because we didn't have a non-bundle version) + Vector3Wide.Broadcast(initialVertex, out var initialVertexBundle); + pool.Take>(pointBundles.Length, out var projectedOnX); + pool.Take>(pointBundles.Length, out var projectedOnY); + var planeEpsilonNarrow = MathF.Sqrt(bestDistanceSquared) * 1e-5f; + var normalCoplanarityEpsilon = 1f - 1e-6f; + var planeEpsilon = new Vector(planeEpsilonNarrow); + var rawFaceVertexIndices = new QuickList(pointBundles.Length * Vector.Count, pool); + var initialSourceEdge = new EdgeEndpoints { A = initialIndex, B = initialIndex }; + + //Points found to not be on the face hull are ignored by future executions. + //Note that it's stored in integers instead of bools; it can be directly loaded as a mask during vectorized operations. + //0 means the vertex is disallowed, -1 means the vertex is allowed. + pool.Take(pointBundleCount * Vector.Count, out var allowVertices); + ((Span)allowVertices).Slice(0, points.Length).Fill(-1); + for (int i = points.Length; i < allowVertices.Length; ++i) + allowVertices[i] = 0; + + FindExtremeFace(initialBasisX, initialBasisY, initialVertexBundle, initialSourceEdge, ref pointBundles, indexOffsetBundle, allowVertices, points.Length, + ref projectedOnX, ref projectedOnY, planeEpsilon, ref rawFaceVertexIndices, out var initialFaceNormal); + Debug.Assert(rawFaceVertexIndices.Count >= 2); + var facePoints = new QuickList(points.Length, pool); + var reducedFaceIndices = new QuickList(points.Length, pool); + + + ReduceFace(ref rawFaceVertexIndices, initialFaceNormal, points, planeEpsilonNarrow, ref facePoints, ref allowVertices, ref reducedFaceIndices); + + var faces = new QuickList(points.Length, pool); + var edgesToTest = new QuickList(points.Length, pool); + var facesForEdges = new QuickDictionary(points.Length, pool); + var facesNeedingMerge = new QuickList(32, pool); + if (reducedFaceIndices.Count >= 3) + { + //The initial face search found an actual face! That's a bit surprising since we didn't start from an edge offset, but rather an arbitrary direction. + //Handle it anyway. + for (int i = 0; i < reducedFaceIndices.Count; ++i) + { + ref var edgeToAdd = ref edgesToTest.Allocate(pool); + edgeToAdd.Endpoints.A = reducedFaceIndices[i == 0 ? reducedFaceIndices.Count - 1 : i - 1]; + edgeToAdd.Endpoints.B = reducedFaceIndices[i]; + edgeToAdd.FaceNormal = initialFaceNormal; + edgeToAdd.FaceIndex = 0; + facesForEdges.Add(ref edgeToAdd.Endpoints, new EdgeFaceIndices(0), pool); + } + //Since an actual face was found, we go ahead and output it into the face set. + AddFace(ref faces, pool, initialFaceNormal, reducedFaceIndices); + } + else + { + Debug.Assert(reducedFaceIndices.Count == 2, + "The point set size was verified to be at least 4 earlier, so even in degenerate cases, a second point should be found by the face search."); + //No actual face was found. That's expected; the arbitrary direction we used for the basis doesn't likely line up with any edges. + ref var edgeToAdd = ref edgesToTest.Allocate(pool); + edgeToAdd.Endpoints.A = reducedFaceIndices[0]; + edgeToAdd.Endpoints.B = reducedFaceIndices[1]; + edgeToAdd.FaceNormal = initialFaceNormal; + edgeToAdd.FaceIndex = -1; + var edgeOffset = points[edgeToAdd.Endpoints.B] - points[edgeToAdd.Endpoints.A]; + var basisY = Vector3.Cross(edgeOffset, edgeToAdd.FaceNormal); + var basisX = Vector3.Cross(edgeOffset, basisY); + if (Vector3.Dot(basisX, edgeToAdd.FaceNormal) > 0) + Helpers.Swap(ref edgeToAdd.Endpoints.A, ref edgeToAdd.Endpoints.B); + } + Vector3Wide.ReadFirst(initialBasisX, out var debugInitialBasisX); + Vector3Wide.ReadFirst(initialBasisY, out var debugInitialBasisY); + steps.Add(new DebugStep(initialSourceEdge, ref rawFaceVertexIndices, initialFaceNormal, debugInitialBasisX, debugInitialBasisY, ref reducedFaceIndices, ref allowVertices, ref faces, default, reducedFaceIndices.Count >= 3 ? 0 : -1)); + + int facesDeletedCount = 0; + + while (edgesToTest.Count > 0) + { + edgesToTest.Pop(out var edgeToTest); + //Make sure the new edge hasn't already been filled by another traversal. + var faceCountIndex = facesForEdges.IndexOf(edgeToTest.Endpoints); + if (faceCountIndex >= 0 && facesForEdges.Values[faceCountIndex].Complete) + continue; + + ref var edgeA = ref points[edgeToTest.Endpoints.A]; + ref var edgeB = ref points[edgeToTest.Endpoints.B]; + var edgeOffset = edgeB - edgeA; + //The face normal points outward, and the edges should be wound counterclockwise. + //basisY should point away from the source face. + var basisY = Vector3.Cross(edgeOffset, edgeToTest.FaceNormal); + //basisX should point inward. + var basisX = Vector3.Cross(edgeOffset, basisY); + basisX = Vector3.Normalize(basisX); + basisY = Vector3.Normalize(basisY); + Vector3Wide.Broadcast(basisX, out var basisXBundle); + Vector3Wide.Broadcast(basisY, out var basisYBundle); + Vector3Wide.Broadcast(edgeA, out var basisOrigin); + rawFaceVertexIndices.Count = 0; + FindExtremeFace(basisXBundle, basisYBundle, basisOrigin, edgeToTest.Endpoints, ref pointBundles, indexOffsetBundle, allowVertices, points.Length, ref projectedOnX, ref projectedOnY, planeEpsilon, ref rawFaceVertexIndices, out var faceNormal); + reducedFaceIndices.Count = 0; + facePoints.Count = 0; + ReduceFace(ref rawFaceVertexIndices, faceNormal, points, planeEpsilonNarrow, ref facePoints, ref allowVertices, ref reducedFaceIndices); + + if (reducedFaceIndices.Count < 3) + { + steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertices, ref faces, default, -1)); + //Degenerate face found; don't bother creating work for it. + continue; + } + var faceCountPriorToAdd = faces.Count; + + facesNeedingMerge.Count = 0; + while (true) + { + Console.WriteLine($"Face count: {faces.Count}"); + //This implementation bites the bullet pretty hard on numerical problems. They most frequently arise from near coplanar vertices. + //It's possible that two iterations see different subsets of 'coplanar' vertices, either causing two faces with near equal normal or + //even causing an edge to have more than two faces associated with it. + //Before making any modifications to the existing data, iterate over all the edges in the current face to check for any such edges. + + //While the usual flow here will be to reduce the vertices we just found and accept the result immediately, + //it's possible for the just-detected face to end up coplanar with an existing face because of numerical issues that prevented detecting the whole face earlier. + //Rather than using more precise math, we detect the failure and merge faces after the fact. + //Unfortunately, this can happen recursively! Merging two redundant faces could reveal a third face that also needs to be merged. + //This is uncommon, but it needs to be covered. + //So, we just stick everything into a while loop and let it iterate until it's done. + //The good news is that the merging process won't loop forever- every merge results in a face that is at least as large as any contributor, and any internal points + //that get reduced out of consideration are marked with allowVertex[whateverInteriorPointIndex] = false. + int previousFaceMergeCount = facesNeedingMerge.Count; + var previousEndpointIndex = reducedFaceIndices[reducedFaceIndices.Count - 1]; + for (int i = 0; i < reducedFaceIndices.Count; ++i) + { + EdgeEndpoints edgeEndpoints; + edgeEndpoints.A = previousEndpointIndex; + edgeEndpoints.B = reducedFaceIndices[i]; + previousEndpointIndex = edgeEndpoints.B; + + if (facesForEdges.GetTableIndices(ref edgeEndpoints, out var tableIndex, out var elementIndex)) + { + //There is already at least one face associated with this edge. Do we need to merge? + ref var edgeFaces = ref facesForEdges.Values[elementIndex]; + Debug.Assert(edgeFaces.FaceA >= 0); + var aDot = Vector3.Dot(faces[edgeFaces.FaceA].Normal, faceNormal); + if (edgeFaces.FaceB >= 0) + { + //The edge already has two faces associated with it. This is definitely a numerical error; separate coplanar faces were generated. + //We *must* merge at least one pair of faces. + //Note that technically both faces can end up being included, even though we've already tested A and B; + //the new face could be a bridge that's close enough to both, even if they were barely too far from each other. + var bDot = Vector3.Dot(faces[edgeFaces.FaceB].Normal, faceNormal); + if (aDot >= normalCoplanarityEpsilon && bDot >= normalCoplanarityEpsilon) + { + AddIfNotPresent(ref facesNeedingMerge, edgeFaces.FaceA, pool); + AddIfNotPresent(ref facesNeedingMerge, edgeFaces.FaceB, pool); + } + else + { + //If both aren't merge candidates, then just pick the one that's closer. + AddIfNotPresent(ref facesNeedingMerge, aDot > bDot ? edgeFaces.FaceA : edgeFaces.FaceB, pool); + } + } + else + { + //Only one face already present. Check if it's coplanar. + if (aDot >= normalCoplanarityEpsilon) + AddIfNotPresent(ref facesNeedingMerge, edgeFaces.FaceA, pool); + } + } + } + + if (facesNeedingMerge.Count > previousFaceMergeCount) + { + //This iteration has found more faces which should be merged together. + //Accumulate the vertices. + for (int i = previousFaceMergeCount; i < facesNeedingMerge.Count; ++i) + { + ref var sourceFace = ref faces[facesNeedingMerge[i]]; + for (int j = 0; j < sourceFace.VertexIndices.Count; ++j) + { + var vertexIndex = sourceFace.VertexIndices[j]; + if (allowVertices[vertexIndex] != 0 && !rawFaceVertexIndices.Contains(vertexIndex)) + { + rawFaceVertexIndices.Allocate(pool) = vertexIndex; + } + } + } + + //Re-reduce. + reducedFaceIndices.Count = 0; + facePoints.Count = 0; + ReduceFace(ref rawFaceVertexIndices, faceNormal, points, planeEpsilonNarrow, ref facePoints, ref allowVertices, ref reducedFaceIndices); + } + else + { + //No more faces need to be merged! Go ahead and apply the changes. + for (int i = 0; i < facesNeedingMerge.Count; ++i) + { + //Note that we do not merge into an existing face; for simplicity, we just merge into the face we're going to append and mark the old faces as being deleted. + //Note that we do *not* remove deleted faces from the faces list. That would break all the face indices that we've accumulated. + var faceIndex = facesNeedingMerge[i]; + ref var faceToRemove = ref faces[faceIndex]; + faceToRemove.Deleted = true; + + //For every edge of any face we're merging, remove the face from the edge lists. + var previousIndex = faceToRemove.VertexIndices[faceToRemove.VertexIndices.Count - 1]; + for (int j = 0; j < faceToRemove.VertexIndices.Count; ++j) + { + RemoveFaceFromEdge(previousIndex, faceToRemove.VertexIndices[j], faceIndex, ref facesForEdges); + previousIndex = faceToRemove.VertexIndices[j]; + } + //Remove any references to this face in the edges to test. + int removedEdgesToTestCount = 0; + for (int j = 0; j < edgesToTest.Count; ++j) + { + if (edgesToTest[j].FaceIndex == faceIndex) + { + ++removedEdgesToTestCount; + } + else if (removedEdgesToTestCount > 0) + { + //Scoot edges to test back. + edgesToTest[j - removedEdgesToTestCount] = edgesToTest[j]; + } + } + edgesToTest.Count -= removedEdgesToTestCount; + } + + //Retain a count of the deleted faces so the post process can handle them more easily. + facesDeletedCount += facesNeedingMerge.Count; + + AddFace(ref faces, pool, faceNormal, reducedFaceIndices); + AddFaceToEdgesAndTestList(pool, ref reducedFaceIndices, ref edgesToTest, ref facesForEdges, faceNormal, faceCountPriorToAdd); + steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertices, ref faces, facesNeedingMerge, faceCountPriorToAdd)); + break; + } + } + } + + edgesToTest.Dispose(pool); + facesForEdges.Dispose(pool); + facePoints.Dispose(pool); + reducedFaceIndices.Dispose(pool); + rawFaceVertexIndices.Dispose(pool); + pool.Return(ref allowVertices); + pool.Return(ref projectedOnX); + pool.Return(ref projectedOnY); + pool.Return(ref pointBundles); + + //for (int i = 0; i < faces.Count; ++i) + //{ + // for (int j = i + 1; j < faces.Count; ++j) + // { + // var dot = Vector3.Dot(faces[i].Normal, faces[j].Normal); + // var bothFacesExist = !faces[i].Deleted && !faces[j].Deleted; + // if (dot >= normalCoplanarityEpsilon && bothFacesExist) + // { + // Console.WriteLine($"Dot {dot} on faces {i} and {j}"); + // } + // } + //} + + if (facesDeletedCount > 0) + { + //During execution, some faces were found to be coplanar and got merged. To avoid breaking face index references during execution, we left those triangles in place. + //Now, though, we need to get rid of any faces that are marked deleted! + int shift = 0; + for (int i = 0; i < faces.Count; ++i) + { + if (faces[i].Deleted) + { + //Removing the face from the list, so we gotta dispose it now rather than later. + faces[i].VertexIndices.Dispose(pool); + ++shift; + } + else if (shift > 0) + { + faces[i - shift] = faces[i]; + } + } + Debug.Assert(facesDeletedCount == shift); + faces.Count -= facesDeletedCount; + } + + //Create a reduced hull point set from the face vertex references. + int totalIndexCount = 0; + for (int i = 0; i < faces.Count; ++i) + { + totalIndexCount += faces[i].VertexIndices.Count; + } + pool.Take(faces.Count, out hullData.FaceStartIndices); + pool.Take(totalIndexCount, out hullData.FaceVertexIndices); + var nextStartIndex = 0; + pool.Take(points.Length, out var originalToHullIndexMapping); + var hullToOriginalIndexMapping = new QuickList(points.Length, pool); + for (int i = 0; i < points.Length; ++i) + { + originalToHullIndexMapping[i] = -1; + } + for (int i = 0; i < faces.Count; ++i) + { + var source = faces[i].VertexIndices; + hullData.FaceStartIndices[i] = nextStartIndex; + for (int j = 0; j < source.Count; ++j) + { + var originalVertexIndex = source[j]; + ref var originalToHull = ref originalToHullIndexMapping[originalVertexIndex]; + if (originalToHull < 0) + { + //This vertex hasn't been seen yet. + originalToHull = hullToOriginalIndexMapping.Count; + hullToOriginalIndexMapping.AllocateUnsafely() = originalVertexIndex; + } + hullData.FaceVertexIndices[nextStartIndex + j] = originalToHull; + } + nextStartIndex += source.Count; + } + + pool.Take(hullToOriginalIndexMapping.Count, out hullData.OriginalVertexMapping); + hullToOriginalIndexMapping.Span.CopyTo(0, hullData.OriginalVertexMapping, 0, hullToOriginalIndexMapping.Count); + + pool.Return(ref originalToHullIndexMapping); + hullToOriginalIndexMapping.Dispose(pool); + for (int i = 0; i < faces.Count; ++i) + { + faces[i].VertexIndices.Dispose(pool); + } + faces.Dispose(pool); + facesNeedingMerge.Dispose(pool); + } + + + /// + /// Processes hull data into a runtime usable convex hull shape. Recenters the convex hull's points around its center of mass. + /// + /// Point array into which the hull data indexes. + /// Raw input data to process. + /// Pool used to allocate resources for the hullShape. + /// Convex hull shape created from the input data. + /// Computed center of mass of the convex hull before its points were recentered onto the origin. + /// True if the shape was created successfully, false otherwise. If false, the hull probably had no volume and would not have worked properly as a shape. + public static bool CreateShape(Span points, HullData hullData, BufferPool pool, out Vector3 center, out ConvexHull hullShape) + { + Debug.Assert(points.Length > 0, "Convex hulls need to have a nonzero number of points!"); + hullShape = default; + var pointBundleCount = BundleIndexing.GetBundleCount(hullData.OriginalVertexMapping.Length); + pool.Take(pointBundleCount, out hullShape.Points); + + float volume = 0; + center = default; + for (int faceIndex = 0; faceIndex < hullData.FaceStartIndices.Length; ++faceIndex) + { + hullData.GetFace(faceIndex, out var face); + for (int subtriangleIndex = 2; subtriangleIndex < face.VertexCount; ++subtriangleIndex) + { + var a = points[face[0]]; + var b = points[face[subtriangleIndex - 1]]; + var c = points[face[subtriangleIndex]]; + var volumeContribution = MeshInertiaHelper.ComputeTetrahedronVolume(a, b, c); + volume += volumeContribution; + var centroid = a + b + c; + center += centroid * volumeContribution; + } + } + //Division by 4 since we accumulated (a + b + c), rather than the actual tetrahedral center (a + b + c + 0) / 4. + center /= volume * 4; + if (float.IsNaN(center.X) || float.IsNaN(center.Y) || float.IsNaN(center.Z) || hullData.FaceStartIndices.Length == 2) + { + //The convex hull seems to have no volume. + //While you could try treating it as coplanar (like we once tried; see commit history just prior to the commit that added this message): + //1. Ray tests won't work. They rely on bounding planes. It would require a special case for degenerate hulls. + //2. Inertia won't work. You could resolve that with a special case, but it doesn't fix ray tests. + //3. Edge-on contact generation may produce lower quality contacts. + //So, pretty worthless overall without major changes. + hullShape.Points.Dispose(pool); + center = default; + Debug.Assert(!hullShape.Points.Allocated && !hullShape.FaceToVertexIndicesStart.Allocated && !hullShape.BoundingPlanes.Allocated && !hullShape.FaceVertexIndices.Allocated, "Hey! You moved something around and forgot to dispose!"); + return false; + } + + var lastIndex = hullData.OriginalVertexMapping.Length - 1; + for (int bundleIndex = 0; bundleIndex < hullShape.Points.Length; ++bundleIndex) + { + ref var bundle = ref hullShape.Points[bundleIndex]; + for (int innerIndex = 0; innerIndex < Vector.Count; ++innerIndex) + { + var index = (bundleIndex << BundleIndexing.VectorShift) + innerIndex; + //We duplicate the last vertices in the hull. It has no impact on performance; the vertex bundles are executed all or nothing. + if (index > lastIndex) + index = lastIndex; + ref var point = ref points[hullData.OriginalVertexMapping[index]]; + Vector3Wide.WriteSlot(point - center, innerIndex, ref bundle); + } + } + + //Create the face->vertex mapping. + pool.Take(hullData.FaceStartIndices.Length, out hullShape.FaceToVertexIndicesStart); + hullData.FaceStartIndices.CopyTo(0, hullShape.FaceToVertexIndicesStart, 0, hullShape.FaceToVertexIndicesStart.Length); + pool.Take(hullData.FaceVertexIndices.Length, out hullShape.FaceVertexIndices); + for (int i = 0; i < hullShape.FaceVertexIndices.Length; ++i) + { + BundleIndexing.GetBundleIndices(hullData.FaceVertexIndices[i], out var bundleIndex, out var innerIndex); + ref var faceVertex = ref hullShape.FaceVertexIndices[i]; + faceVertex.BundleIndex = (ushort)bundleIndex; + faceVertex.InnerIndex = (ushort)innerIndex; + } + + //Create bounding planes. + var faceBundleCount = BundleIndexing.GetBundleCount(hullShape.FaceToVertexIndicesStart.Length); + pool.Take(faceBundleCount, out hullShape.BoundingPlanes); + for (int i = 0; i < hullShape.FaceToVertexIndicesStart.Length; ++i) + { + hullShape.GetVertexIndicesForFace(i, out var faceVertexIndices); + Debug.Assert(faceVertexIndices.Length >= 3, "We only allow the creation of convex hulls around point sets with, at minimum, some area, so all faces should have at least 3 points."); + //Note that we sum up contributions from all the constituent triangles. + //This avoids hitting any degenerate face triangles and smooths out small numerical deviations. + //(It's mathematically equivalent to taking a weighted average by area, since the magnitude of the cross product is proportional to area.) + Vector3 faceNormal = default; + hullShape.GetPoint(faceVertexIndices[0], out var facePivot); + hullShape.GetPoint(faceVertexIndices[1], out var faceVertex); + var previousOffset = faceVertex - facePivot; + for (int j = 2; j < faceVertexIndices.Length; ++j) + { + //Normal points outward. + hullShape.GetPoint(faceVertexIndices[j], out faceVertex); + var offset = faceVertex - facePivot; + faceNormal += Vector3.Cross(previousOffset, offset); + previousOffset = offset; + } + var length = faceNormal.Length(); + Debug.Assert(length > 1e-10f, "Convex hull procedure should not output degenerate faces."); + faceNormal /= length; + BundleIndexing.GetBundleIndices(i, out var boundingPlaneBundleIndex, out var boundingPlaneInnerIndex); + ref var boundingBundle = ref hullShape.BoundingPlanes[boundingPlaneBundleIndex]; + ref var boundingOffsetBundle = ref GatherScatter.GetOffsetInstance(ref boundingBundle, boundingPlaneInnerIndex); + Vector3Wide.WriteFirst(faceNormal, ref boundingOffsetBundle.Normal); + GatherScatter.GetFirst(ref boundingOffsetBundle.Offset) = Vector3.Dot(facePivot, faceNormal); + } + + //Clear any trailing bounding plane data to keep it from contributing. + var boundingPlaneCapacity = hullShape.BoundingPlanes.Length * Vector.Count; + for (int i = hullShape.FaceToVertexIndicesStart.Length; i < boundingPlaneCapacity; ++i) + { + BundleIndexing.GetBundleIndices(i, out var bundleIndex, out var innerIndex); + ref var offsetInstance = ref GatherScatter.GetOffsetInstance(ref hullShape.BoundingPlanes[bundleIndex], innerIndex); + Vector3Wide.WriteFirst(default, ref offsetInstance.Normal); + GatherScatter.GetFirst(ref offsetInstance.Offset) = float.MinValue; + } + return true; + } + + /// + /// Creates a convex hull shape out of an input point set. Recenters the convex hull's points around its center of mass. + /// + /// Points to use to create the hull. + /// Buffer pool used for temporary allocations and the output data structures. + /// Intermediate hull data that got processed into the convex hull. + /// Computed center of mass of the convex hull before its points were recentered onto the origin. + /// Convex hull shape of the input point set. + /// True if the shape was created successfully, false otherwise. If false, the hull probably had no volume and would not have worked properly as a shape. + public static bool CreateShape(Span points, BufferPool pool, out HullData hullData, out Vector3 center, out ConvexHull convexHull) + { + ComputeHull(points, pool, out hullData); + return CreateShape(points, hullData, pool, out center, out convexHull); + } + + /// + /// Creates a convex hull shape out of an input point set. Recenters the convex hull's points around its center of mass. + /// + /// Points to use to create the hull. + /// Buffer pool used for temporary allocations and the output data structures. + /// Computed center of mass of the convex hull before its points were recentered onto the origin. + /// Convex hull shape of the input point set. + /// True if the shape was created successfully, false otherwise. If false, the hull probably had no volume and would not have worked properly as a shape. + public static bool CreateShape(Span points, BufferPool pool, out Vector3 center, out ConvexHull convexHull) + { + ComputeHull(points, pool, out var hullData); + var result = CreateShape(points, hullData, pool, out center, out convexHull); + //Empty input point sets won't allocate; don't try to dispose them. + if (hullData.OriginalVertexMapping.Allocated) + hullData.Dispose(pool); + return result; + } + + + /// + /// Creates a transformed copy of a convex hull. + /// + /// Source convex hull to copy. + /// Transform to apply to the hull points. + /// Transformed points in the copy target hull. + /// Transformed bounding planes in the copy target hull. + public static void CreateTransformedCopy(in ConvexHull source, in Matrix3x3 transform, Buffer targetPoints, Buffer targetBoundingPlanes) + { + if (targetPoints.Length < source.Points.Length) + throw new ArgumentException("Target points buffer cannot hold the copy.", nameof(targetPoints)); + if (targetBoundingPlanes.Length < source.BoundingPlanes.Length) + throw new ArgumentException("Target bounding planes buffer cannot hold the copy.", nameof(targetBoundingPlanes)); + Matrix3x3Wide.Broadcast(transform, out var transformWide); + for (int i = 0; i < source.Points.Length; ++i) + { + Matrix3x3Wide.TransformWithoutOverlap(source.Points[i], transformWide, out targetPoints[i]); + } + Matrix3x3.Invert(transform, out var inverse); + Matrix3x3Wide.Broadcast(inverse, out var inverseWide); + for (int i = 0; i < source.BoundingPlanes.Length; ++i) + { + Matrix3x3Wide.TransformByTransposedWithoutOverlap(source.BoundingPlanes[i].Normal, inverseWide, out var normal); + Vector3Wide.Normalize(normal, out targetBoundingPlanes[i].Normal); + } + + for (int faceIndex = 0; faceIndex < source.FaceToVertexIndicesStart.Length; ++faceIndex) + { + //This isn't exactly an optimal implementation- it uses a pretty inefficient gather, but any optimization can wait for it being a problem. + var vertexIndex = source.FaceVertexIndices[source.FaceToVertexIndicesStart[faceIndex]]; + BundleIndexing.GetBundleIndices(faceIndex, out var bundleIndex, out var indexInBundle); + Vector3Wide.ReadSlot(ref targetPoints[vertexIndex.BundleIndex], vertexIndex.InnerIndex, out var point); + Vector3Wide.ReadSlot(ref targetBoundingPlanes[bundleIndex].Normal, indexInBundle, out var normal); + GatherScatter.Get(ref targetBoundingPlanes[bundleIndex].Offset, indexInBundle) = Vector3.Dot(point, normal); + } + + //Clear any trailing bounding plane data to keep it from contributing. + var boundingPlaneCapacity = targetBoundingPlanes.Length * Vector.Count; + for (int i = source.FaceToVertexIndicesStart.Length; i < boundingPlaneCapacity; ++i) + { + BundleIndexing.GetBundleIndices(i, out var bundleIndex, out var innerIndex); + ref var offsetInstance = ref GatherScatter.GetOffsetInstance(ref targetBoundingPlanes[bundleIndex], innerIndex); + Vector3Wide.WriteFirst(default, ref offsetInstance.Normal); + GatherScatter.GetFirst(ref offsetInstance.Offset) = float.MinValue; + } + } + + /// + /// Creates a transformed copy of a convex hull. FaceVertexIndices and FaceToVertexIndicesStart buffers from the source are reused in the copy target. + /// Note that disposing two convex hulls with the same buffers will cause errors; disposal must be handled carefully to avoid double freeing the shared buffers. + /// + /// Source convex hull to copy. + /// Transform to apply to the hull points. + /// Pool from which to allocate the new hull's points and bounding planes buffers. + /// Target convex hull to copy into. FaceVertexIndices and FaceToVertexIndicesStart buffers are reused from the source. + public static void CreateTransformedShallowCopy(in ConvexHull source, in Matrix3x3 transform, BufferPool pool, out ConvexHull target) + { + pool.Take(source.Points.Length, out target.Points); + pool.Take(source.BoundingPlanes.Length, out target.BoundingPlanes); + CreateTransformedCopy(source, transform, target.Points, target.BoundingPlanes); + target.FaceVertexIndices = source.FaceVertexIndices; + target.FaceToVertexIndicesStart = source.FaceToVertexIndicesStart; + } + + /// + /// Creates a transformed copy of a convex hull. Unique FaceVertexIndices and FaceToVertexIndicesStart buffers are allocated for the copy target. + /// + /// Source convex hull to copy. + /// Transform to apply to the hull points. + /// Pool from which to allocate the new hull's buffers. + /// Target convex hull to copy into. + public static void CreateTransformedCopy(in ConvexHull source, in Matrix3x3 transform, BufferPool pool, out ConvexHull target) + { + pool.Take(source.Points.Length, out target.Points); + pool.Take(source.BoundingPlanes.Length, out target.BoundingPlanes); + pool.Take(source.FaceVertexIndices.Length, out target.FaceVertexIndices); + pool.Take(source.FaceToVertexIndicesStart.Length, out target.FaceToVertexIndicesStart); + CreateTransformedCopy(source, transform, target.Points, target.BoundingPlanes); + source.FaceVertexIndices.CopyTo(0, target.FaceVertexIndices, 0, target.FaceVertexIndices.Length); + source.FaceToVertexIndicesStart.CopyTo(0, target.FaceToVertexIndicesStart, 0, target.FaceToVertexIndicesStart.Length); + } + + } +} From 79f982d157fd8f7dfce0fe6d165b1049daa63c18 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 14 Dec 2023 17:23:17 -0600 Subject: [PATCH 841/947] Tinytweak; shouldn't reject things that were incorrectly rejected before. --- BepuPhysics/Collidables/ConvexHullHelper.cs | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/BepuPhysics/Collidables/ConvexHullHelper.cs b/BepuPhysics/Collidables/ConvexHullHelper.cs index 805373b73..587c339c8 100644 --- a/BepuPhysics/Collidables/ConvexHullHelper.cs +++ b/BepuPhysics/Collidables/ConvexHullHelper.cs @@ -202,7 +202,7 @@ static void FindExtremeFace( // } // } //} - var coplanar = Vector.LessThanOrEqual(Vector.Abs(dot), planeEpsilon); + var coplanar = Vector.GreaterThan(dot, -planeEpsilon); if (Vector.LessThanAny(coplanar, Vector.Zero)) { var bundleBaseIndex = i << BundleIndexing.VectorShift; @@ -318,8 +318,7 @@ static int FindNextIndexForFaceHull2(Vector2 start, Vector2 previousEdgeDirectio if (alongNormal > planeEpsilon && !ignoreSlot) Console.WriteLine("It looks like we've found a point significantly beyond the bounding plane we just identified; how's that possible?"); - //TODO: Here and in the 3d variant: should you really be using the abs? That rejects anything that is numerically too far outside the face, which... should be rare, but we shouldn't let that point just corrupt things later. - if (float.Abs(alongNormal) <= planeEpsilon) + if (alongNormal > -planeEpsilon) { var alongEdge = Vector2.Dot(toCandidate, edgeDirection); var rawDistance = float.Sqrt(alongEdge * alongEdge + alongNormal * alongNormal); From 4ffecb2d36d921d0fa7bdeb96efd7aa4835f4283 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 14 Dec 2023 17:32:19 -0600 Subject: [PATCH 842/947] Disabled debugstuff and cleaned up redundant old implementations. --- BepuPhysics/Collidables/ConvexHullHelper.cs | 144 +- Demos/SpecializedTests/ConvexHullTestDemo.cs | 1278 +++++++++--------- 2 files changed, 661 insertions(+), 761 deletions(-) diff --git a/BepuPhysics/Collidables/ConvexHullHelper.cs b/BepuPhysics/Collidables/ConvexHullHelper.cs index 587c339c8..53076ef87 100644 --- a/BepuPhysics/Collidables/ConvexHullHelper.cs +++ b/BepuPhysics/Collidables/ConvexHullHelper.cs @@ -232,7 +232,7 @@ static void FindExtremeFace( /// Epsilon within which to consider points to be coplanar (or here, in the 2D case, collinear). /// Points composing the hull face projected onto the face's 2D basis. /// Index of the point in facePoints which is the end point for the next edge segment as identified by gift wrapping. - static int FindNextIndexForFaceHull2(Vector2 start, Vector2 previousEdgeDirection, float planeEpsilon, ref QuickList facePoints) + static int FindNextIndexForFaceHull(Vector2 start, Vector2 previousEdgeDirection, float planeEpsilon, ref QuickList facePoints) { //Find the candidate-basisOrigin which has the smallest angle with basisY when projected onto the plane spanned by basisX and basisY. //angle = atan(y / x) @@ -248,7 +248,6 @@ static int FindNextIndexForFaceHull2(Vector2 start, Vector2 previousEdgeDirectio var bestX = 1f; var bestY = float.MaxValue; int bestIndex = -1; - var bestAngle = float.Atan(bestY / bestX); for (int i = 0; i < facePoints.Count; ++i) { var candidate = facePoints[i]; @@ -262,23 +261,15 @@ static int FindNextIndexForFaceHull2(Vector2 start, Vector2 previousEdgeDirectio //Without this condition, it's possible for numerical cycles to occur where a face finds itself over and over again. var ignoreSlot = x <= planeEpsilon && y >= -planeEpsilon; var useCandidate = (y * bestX < bestY * x) && !ignoreSlot; - var candidateAngle = float.Atan(y / x); - var useCandidateTest = candidateAngle < bestAngle && !ignoreSlot; - if (useCandidateTest != useCandidate && float.Abs(candidateAngle - bestAngle) > 1e-5f) - { - Console.WriteLine("dd"); - } if (useCandidate) { bestY = y; bestX = x; bestIndex = i; - bestAngle = candidateAngle; } } //If no next index was identified, then the face is degenerate. //Stop now to prevent the postpass from identifying some nonsense derived from a garbage plane. - Console.WriteLine($"Best index: {bestIndex}, progress along PREVIOUS: {-bestY}, best angle: {bestAngle + float.Pi / 2}"); if (bestIndex == -1) return -1; @@ -304,27 +295,9 @@ static int FindNextIndexForFaceHull2(Vector2 start, Vector2 previousEdgeDirectio var candidate = facePoints[i]; var toCandidate = candidate - start; var alongNormal = Vector2.Dot(toCandidate, faceNormal); - - //# DEBUG - var x = float.Max(0, Vector2.Dot(toCandidate, basisX)); - var y = Vector2.Dot(toCandidate, basisY); - var ignoreSlot = x <= planeEpsilon && y >= -planeEpsilon; - var useCandidate = (y * bestX < bestY * x) && !ignoreSlot; - var candidateAngle = float.Atan(y / x); - var useCandidateTest = candidateAngle < bestAngle && !ignoreSlot; - //# DEBUG - if (alongNormal > planeEpsilon && useCandidateTest) - Console.WriteLine("Okay, so we definitely should have selected this one earlier."); - if (alongNormal > planeEpsilon && !ignoreSlot) - Console.WriteLine("It looks like we've found a point significantly beyond the bounding plane we just identified; how's that possible?"); - if (alongNormal > -planeEpsilon) { var alongEdge = Vector2.Dot(toCandidate, edgeDirection); - var rawDistance = float.Sqrt(alongEdge * alongEdge + alongNormal * alongNormal); - //if (float.Abs(rawDistance) - float.Abs(alongEdge) > 1e-6f) - // Console.WriteLine("dd"); - if (alongEdge > distance) { distance = alongEdge; @@ -332,113 +305,11 @@ static int FindNextIndexForFaceHull2(Vector2 start, Vector2 previousEdgeDirectio } } } - if (mostDistantIndex >= 0) - { - var mostDistantX = Vector2.Dot(basisX, facePoints[mostDistantIndex] - start); - var mostDistantY = Vector2.Dot(basisY, facePoints[mostDistantIndex] - start); - - var testProjectedBestEdgeDirection = new Vector2(bestX, bestY); - var testLength = projectedBestEdgeDirection.Length(); - //Note that the projected face normal is in terms of basisX and basisY, not the original basis facePoints are built on. - testProjectedBestEdgeDirection = float.IsFinite(length) ? testProjectedBestEdgeDirection / length : new Vector2(1, 0); - //Transform the projected normal back into the basis of facePoints. - var testEdgeDirection = basisX * testProjectedBestEdgeDirection.X + basisY * testProjectedBestEdgeDirection.Y; - var testFaceNormal = new Vector2(-testEdgeDirection.Y, testEdgeDirection.X); - var testDot = Vector2.Dot(edgeDirection, testEdgeDirection); - if (testDot < 1 - 1e-6f) - { - Console.WriteLine("Sdf"); - } - var mostDistance = float.Sqrt(mostDistantX * mostDistantX + mostDistantY * mostDistantY); - var bestDistance = float.Sqrt(bestX * bestX + bestY * bestY); - if (mostDistance < bestDistance) - Console.WriteLine("uh"); - - var mostDistantNormal = Vector2.Dot(faceNormal, facePoints[mostDistantIndex] - start); - var mostDistantEdge = Vector2.Dot(edgeDirection, facePoints[mostDistantIndex] - start); - var bestNormal = Vector2.Dot(faceNormal, facePoints[bestIndex] - start); - var bestEdge = Vector2.Dot(edgeDirection, facePoints[bestIndex] - start); - - if (mostDistantEdge < bestEdge) - Console.WriteLine("uh"); - - Console.WriteLine($"Progress along CURRENT edge: {distance}"); - } - else - { - } return mostDistantIndex == -1 ? bestIndex : mostDistantIndex; } - - static int FindNextIndexForFaceHull(Vector2 start, Vector2 previousEdgeDirection, float planeEpsilon, ref QuickList facePoints) - { - //Use a AOS version since the number of points on a given face will tend to be very small in most cases. - //Same idea as the 3d version- find the next edge which is closest to the previous edge. Not going to worry about collinear points here for now. - var bestIndex = -1; - float best = -float.MaxValue; - float bestDistanceSquared = 0; - var startToCandidate = facePoints[0] - start; - var xDirection = new Vector2(previousEdgeDirection.Y, -previousEdgeDirection.X); - var candidateX = Vector2.Dot(startToCandidate, xDirection); - var candidateY = Vector2.Dot(startToCandidate, previousEdgeDirection); - var currentEdgeDirectionX = previousEdgeDirection.X; - var currentEdgeDirectionY = previousEdgeDirection.Y; - if (candidateX > 0) - { - best = candidateY / candidateX; - bestIndex = 0; - bestDistanceSquared = candidateX * candidateX + candidateY * candidateY; - var inverseBestDistance = 1f / MathF.Sqrt(bestDistanceSquared); - currentEdgeDirectionX = candidateX * inverseBestDistance; - currentEdgeDirectionY = candidateY * inverseBestDistance; - } - for (int i = 1; i < facePoints.Count; ++i) - { - startToCandidate = facePoints[i] - start; - candidateY = Vector2.Dot(startToCandidate, previousEdgeDirection); - candidateX = Vector2.Dot(startToCandidate, xDirection); - //Any points that are collinear *with the previous edge* cannot be a part of the current edge without numerical failure; the previous edge should include them. - if (candidateX <= 0) - { - if (candidateY > 0) - Console.WriteLine("Violation of expectation"); - //Debug.Assert(candidateY <= 0, - // "Previous edge should include any collinear points, so this edge should not see any further collinear points beyond its start." + - // "If you run into this, it implies you've found some content that violates the convex huller's assumptions, and I'd appreciate it if you reported it on github.com/bepu/bepuphysics2/issues!" + - // "A .obj or other simple demos-compatible reproduction case would help me fix it."); - continue; - } - //We accept a candidate if it is either: - //1) collinear with the previous best by the plane epsilon test BUT is more distant, or - //2) has a greater angle than the previous best. - var planeOffset = -candidateX * currentEdgeDirectionY + candidateY * currentEdgeDirectionX; - if (MathF.Abs(planeOffset) < planeEpsilon) - { - //The candidate is collinear. Only accept it if it's further away. - if (candidateX * candidateX + candidateY * candidateY <= bestDistanceSquared) - { - continue; - } - } - else if (candidateY < best * candidateX) //candidateY / candidateX < best, given candidate X > 0; just avoiding a division for bulk testing. - { - //Candidate is a smaller angle. Rejected. - continue; - } - best = candidateY / candidateX; - bestDistanceSquared = candidateX * candidateX + candidateY * candidateY; - var inverseBestDistance = 1f / MathF.Sqrt(bestDistanceSquared); - currentEdgeDirectionX = candidateX * inverseBestDistance; - currentEdgeDirectionY = candidateY * inverseBestDistance; - bestIndex = i; - } - //Note that this can return -1 if all points were on top of the start. - return bestIndex; - } - static void ReduceFace(ref QuickList faceVertexIndices, Vector3 faceNormal, Span points, float planeEpsilon, ref QuickList facePoints, ref Buffer allowVertex, ref QuickList reducedIndices) { Debug.Assert(facePoints.Count == 0 && reducedIndices.Count == 0 && facePoints.Span.Length >= faceVertexIndices.Count && reducedIndices.Span.Length >= faceVertexIndices.Count); @@ -535,17 +406,12 @@ static void ReduceFace(ref QuickList faceVertexIndices, Vector3 faceNormal, reducedIndices.AllocateUnsafely() = faceVertexIndices[initialIndex]; var previousEndIndex = initialIndex; - Console.WriteLine("Entering reduce face loop."); - bool reductionTerminatedSuccessfully = false; for (int i = 0; i < facePoints.Count; ++i) { - Console.WriteLine($"Reduce face loop iteration start. Previous end index: {previousEndIndex}, previous edge dir: {previousEdgeDirection}."); - Console.WriteLine($"World edge dir: {previousEdgeDirection.X * basisX + previousEdgeDirection.Y * basisY}"); - var nextIndex = FindNextIndexForFaceHull2(facePoints[previousEndIndex], previousEdgeDirection, planeEpsilon, ref facePoints); + var nextIndex = FindNextIndexForFaceHull(facePoints[previousEndIndex], previousEdgeDirection, planeEpsilon, ref facePoints); //This can return -1 in the event of a completely degenerate face. if (nextIndex == -1 || reducedIndices.Contains(faceVertexIndices[nextIndex])) { - reductionTerminatedSuccessfully = true; if (nextIndex >= 0) { //Wrapped around to a repeated index. @@ -567,11 +433,6 @@ static void ReduceFace(ref QuickList faceVertexIndices, Vector3 faceNormal, previousEdgeDirection = Vector2.Normalize(facePoints[nextIndex] - facePoints[previousEndIndex]); previousEndIndex = nextIndex; } - if (!reductionTerminatedSuccessfully) - { - throw new Exception("Hull face reduction failed to terminate. This should not happen; please report it on github.com/bepu/bepuphysics2/issues, ideally with a point set that triggered this exception!"); - } - Console.WriteLine("finished reduce face loop."); //Ignore any vertices which were not on the outer boundary of the face. for (int i = 0; i < faceVertexIndices.Count; ++i) @@ -1006,7 +867,6 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa facesNeedingMerge.Count = 0; while (true) { - Console.WriteLine($"Face count: {faces.Count}"); //This implementation bites the bullet pretty hard on numerical problems. They most frequently arise from near coplanar vertices. //It's possible that two iterations see different subsets of 'coplanar' vertices, either causing two faces with near equal normal or //even causing an edge to have more than two faces associated with it. diff --git a/Demos/SpecializedTests/ConvexHullTestDemo.cs b/Demos/SpecializedTests/ConvexHullTestDemo.cs index ef92f8570..318350be5 100644 --- a/Demos/SpecializedTests/ConvexHullTestDemo.cs +++ b/Demos/SpecializedTests/ConvexHullTestDemo.cs @@ -1,619 +1,659 @@ -using System; -using System.Collections.Generic; -using System.Numerics; -using BepuPhysics.Collidables; -using BepuUtilities.Collections; -using DemoContentLoader; -using DemoRenderer; -using BepuPhysics; -using static BepuPhysics.Collidables.ConvexHullHelper; -using System.Diagnostics; -using BepuUtilities; -using BepuPhysics.Constraints; -using BepuUtilities.Memory; -using System.Text.Json; -using System.IO; - -namespace Demos.SpecializedTests; - -public class ConvexHullTestDemo : Demo -{ - Buffer CreateRandomConvexHullPoints() - { - const int pointCount = 50; - BufferPool.Take(pointCount, out var points); - - var random = new Random(5); - for (int i = 0; i < pointCount; ++i) - { - points[i] = new Vector3(3 * random.NextSingle(), 1 * random.NextSingle(), 3 * random.NextSingle()); - } - - return points; - } - - Buffer CreatePlaneish() - { - var points = new Buffer(12, BufferPool); - points[0] = new Vector3(-13.82f, 16.79f, 13.83f); - points[1] = new Vector3(13.82f, -16.79f, -13.83f); - points[2] = new Vector3(13.82f, 16.79f, -13.83f); - points[3] = new Vector3(-13.82f, 16.79f, 13.83f); - points[4] = new Vector3(13.82f, 16.79f, -13.83f); - points[5] = new Vector3(-13.82f, -16.79f, 13.83f); - points[6] = new Vector3(13.82f, 16.79f, -13.83f); - points[7] = new Vector3(13.82f, -16.79f, -13.83f); - points[8] = new Vector3(-13.82f, -16.79f, 13.83f); - points[9] = new Vector3(-13.82f, 16.79f, 13.83f); - points[10] = new Vector3(-13.82f, -16.79f, 13.83f); - points[11] = new Vector3(13.82f, -16.79f, -13.83f); - return points; - } - - Buffer CreateDistantPlane() - { - var points = new Buffer(12, BufferPool); - points[0] = new(-151.0875f, -2.2505488f, 102.17515f); - points[1] = new(-151.10571f, 2.1121342f, -17.699797f); - points[2] = new(-151.08746f, -2.2504745f, -17.699797f); - points[3] = new(-151.10571f, 2.1121342f, -17.699797f); - points[4] = new(-151.0875f, -2.2505488f, 102.17515f); - points[5] = new(-151.10574f, 2.1120775f, 102.17517f); - points[6] = new(-151.10571f, 2.1121342f, -17.699797f); - points[7] = new(-151.10574f, 2.1120775f, 102.17517f); - points[8] = new(-151.08746f, -2.2504745f, -17.699797f); - points[9] = new(-151.08746f, -2.2504745f, -17.699797f); - points[10] = new(-151.10574f, 2.1120775f, 102.17517f); - points[11] = new(-151.0875f, -2.2505488f, 102.17515f); - return points; - } - - Buffer CreateMeshConvexHull(MeshContent meshContent, Vector3 scale) - { - //This is actually a pretty good example of how *not* to make a convex hull shape. - //Generating it directly from a graphical data source tends to have way more surface complexity than needed, - //and it tends to have a lot of near-but-not-quite-coplanar surfaces which can make the contact manifold less stable. - //Prefer a simpler source with more distinct features, possibly created with an automated content-time tool. - BufferPool.Take(meshContent.Triangles.Length * 3, out var points); - for (int i = 0; i < meshContent.Triangles.Length; ++i) - { - ref var triangle = ref meshContent.Triangles[i]; - //resisting the urge to just reinterpret the memory - points[i * 3 + 0] = triangle.A * scale; - points[i * 3 + 1] = triangle.B * scale; - points[i * 3 + 2] = triangle.C * scale; - } - return points; - } - - Buffer CreateBoxConvexHull(float boxScale) - { - BufferPool.Take(8, out var points); - points[0] = new Vector3(0, 0, 0); - points[1] = new Vector3(0, 0, boxScale); - points[2] = new Vector3(0, boxScale, 0); - points[3] = new Vector3(0, boxScale, boxScale); - points[4] = new Vector3(boxScale, 0, 0); - points[5] = new Vector3(boxScale, 0, boxScale); - points[6] = new Vector3(boxScale, boxScale, 0); - points[7] = new Vector3(boxScale, boxScale, boxScale); - return points; - } - - //A couple of test point sets from PEEL: https://github.com/Pierre-Terdiman/PEEL_PhysX_Edition - Buffer CreateTestConvexHull() - { - BufferPool.Take(50, out var vertices); - vertices[0] = new Vector3(-0.000000f, -0.297120f, -0.000000f); - vertices[1] = new Vector3(0.258819f, -0.297120f, 0.965926f); - vertices[2] = new Vector3(-0.000000f, -0.297120f, 1.000000f); - vertices[3] = new Vector3(0.500000f, -0.297120f, 0.866026f); - vertices[4] = new Vector3(0.707107f, -0.297120f, 0.707107f); - vertices[5] = new Vector3(0.866026f, -0.297120f, 0.500000f); - vertices[6] = new Vector3(0.965926f, -0.297120f, 0.258819f); - vertices[7] = new Vector3(1.000000f, -0.297120f, -0.000000f); - vertices[8] = new Vector3(0.965926f, -0.297120f, -0.258819f); - vertices[9] = new Vector3(0.866026f, -0.297120f, -0.500000f); - vertices[10] = new Vector3(0.707107f, -0.297120f, -0.707107f); - vertices[11] = new Vector3(0.500000f, -0.297120f, -0.866026f); - vertices[12] = new Vector3(0.258819f, -0.297120f, -0.965926f); - vertices[13] = new Vector3(-0.000000f, -0.297120f, -1.000000f); - vertices[14] = new Vector3(-0.258819f, -0.297120f, -0.965926f); - vertices[15] = new Vector3(-0.500000f, -0.297120f, -0.866025f); - vertices[16] = new Vector3(-0.707107f, -0.297120f, -0.707107f); - vertices[17] = new Vector3(-0.866026f, -0.297120f, -0.500000f); - vertices[18] = new Vector3(-0.965926f, -0.297120f, -0.258819f); - vertices[19] = new Vector3(-1.000000f, -0.297120f, 0.000000f); - vertices[20] = new Vector3(-0.965926f, -0.297120f, 0.258819f); - vertices[21] = new Vector3(-0.866025f, -0.297120f, 0.500000f); - vertices[22] = new Vector3(-0.707107f, -0.297120f, 0.707107f); - vertices[23] = new Vector3(-0.500000f, -0.297120f, 0.866026f); - vertices[24] = new Vector3(-0.258819f, -0.297120f, 0.965926f); - vertices[25] = new Vector3(-0.000000f, 0.297120f, -0.000000f); - vertices[26] = new Vector3(-0.000000f, 0.297120f, 0.537813f); - vertices[27] = new Vector3(0.139196f, 0.297120f, 0.519487f); - vertices[28] = new Vector3(0.268907f, 0.297120f, 0.465760f); - vertices[29] = new Vector3(0.380291f, 0.297120f, 0.380291f); - vertices[30] = new Vector3(0.465760f, 0.297120f, 0.268907f); - vertices[31] = new Vector3(0.519487f, 0.297120f, 0.139196f); - vertices[32] = new Vector3(0.537813f, 0.297120f, -0.000000f); - vertices[33] = new Vector3(0.519487f, 0.297120f, -0.139196f); - vertices[34] = new Vector3(0.465760f, 0.297120f, -0.268907f); - vertices[35] = new Vector3(0.380291f, 0.297120f, -0.380291f); - vertices[36] = new Vector3(0.268907f, 0.297120f, -0.465760f); - vertices[37] = new Vector3(0.139196f, 0.297120f, -0.519487f); - vertices[38] = new Vector3(-0.000000f, 0.297120f, -0.537813f); - vertices[39] = new Vector3(-0.139196f, 0.297120f, -0.519487f); - vertices[40] = new Vector3(-0.268907f, 0.297120f, -0.465760f); - vertices[41] = new Vector3(-0.380291f, 0.297120f, -0.380291f); - vertices[42] = new Vector3(-0.465760f, 0.297120f, -0.268907f); - vertices[43] = new Vector3(-0.519487f, 0.297120f, -0.139196f); - vertices[44] = new Vector3(-0.537813f, 0.297120f, 0.000000f); - vertices[45] = new Vector3(-0.519487f, 0.297120f, 0.139196f); - vertices[46] = new Vector3(-0.465760f, 0.297120f, 0.268907f); - vertices[47] = new Vector3(-0.380291f, 0.297120f, 0.380291f); - vertices[48] = new Vector3(-0.268907f, 0.297120f, 0.465760f); - vertices[49] = new Vector3(-0.139196f, 0.297120f, 0.519487f); - return vertices; - } - - Buffer CreateTestConvexHull2() - { - BufferPool.Take(120, out var vertices); - vertices[0] = new Vector3(0.153478f, 0.993671f, 0.124687f); - vertices[1] = new Vector3(0.153478f, 0.993671f, -0.117774f); - vertices[2] = new Vector3(-0.147939f, 0.993671f, -0.117774f); - vertices[3] = new Vector3(-0.147939f, 0.993671f, 0.124687f); - vertices[4] = new Vector3(0.137286f, 0.817392f, 0.586192f); - vertices[5] = new Vector3(0.333441f, 0.696161f, 0.661116f); - vertices[6] = new Vector3(0.484149f, 0.789305f, 0.417265f); - vertices[7] = new Vector3(0.287995f, 0.910536f, 0.342339f); - vertices[8] = new Vector3(0.794945f, 0.410936f, 0.484838f); - vertices[9] = new Vector3(0.916176f, 0.336012f, 0.288682f); - vertices[10] = new Vector3(0.823033f, 0.579863f, 0.137973f); - vertices[11] = new Vector3(0.701803f, 0.654787f, 0.334128f); - vertices[12] = new Vector3(0.916176f, 0.336012f, -0.281770f); - vertices[13] = new Vector3(0.794945f, 0.410936f, -0.477925f); - vertices[14] = new Vector3(0.701803f, 0.654787f, -0.327216f); - vertices[15] = new Vector3(0.823033f, 0.579863f, -0.131060f); - vertices[16] = new Vector3(0.333441f, 0.696161f, -0.654204f); - vertices[17] = new Vector3(0.137286f, 0.817392f, -0.579280f); - vertices[18] = new Vector3(0.287995f, 0.910536f, -0.335426f); - vertices[19] = new Vector3(0.484149f, 0.789305f, -0.410352f); - vertices[20] = new Vector3(-0.131747f, 0.817392f, -0.579280f); - vertices[21] = new Vector3(-0.327903f, 0.696161f, -0.654204f); - vertices[22] = new Vector3(-0.478612f, 0.789305f, -0.410352f); - vertices[23] = new Vector3(-0.282457f, 0.910536f, -0.335426f); - vertices[24] = new Vector3(-0.789408f, 0.410936f, -0.477925f); - vertices[25] = new Vector3(-0.910638f, 0.336012f, -0.281770f); - vertices[26] = new Vector3(-0.817496f, 0.579863f, -0.131060f); - vertices[27] = new Vector3(-0.696265f, 0.654787f, -0.327216f); - vertices[28] = new Vector3(-0.910638f, 0.336012f, 0.288682f); - vertices[29] = new Vector3(-0.789408f, 0.410936f, 0.484838f); - vertices[30] = new Vector3(-0.696265f, 0.654787f, 0.334128f); - vertices[31] = new Vector3(-0.817496f, 0.579863f, 0.137973f); - vertices[32] = new Vector3(-0.327903f, 0.696161f, 0.661116f); - vertices[33] = new Vector3(-0.131747f, 0.817392f, 0.586192f); - vertices[34] = new Vector3(-0.282457f, 0.910536f, 0.342339f); - vertices[35] = new Vector3(-0.478612f, 0.789305f, 0.417265f); - vertices[36] = new Vector3(0.416578f, 0.478508f, 0.795634f); - vertices[37] = new Vector3(0.341652f, 0.282353f, 0.916863f); - vertices[38] = new Vector3(0.585505f, 0.131646f, 0.823721f); - vertices[39] = new Vector3(0.660429f, 0.327801f, 0.702490f); - vertices[40] = new Vector3(0.124000f, 0.147837f, 1.000000f); - vertices[41] = new Vector3(-0.118461f, 0.147837f, 1.000000f); - vertices[42] = new Vector3(-0.118461f, -0.153580f, 1.000000f); - vertices[43] = new Vector3(0.124000f, -0.153580f, 1.000000f); - vertices[44] = new Vector3(-0.336113f, 0.282353f, 0.916863f); - vertices[45] = new Vector3(-0.411039f, 0.478508f, 0.795634f); - vertices[46] = new Vector3(-0.654891f, 0.327801f, 0.702490f); - vertices[47] = new Vector3(-0.579966f, 0.131646f, 0.823721f); - vertices[48] = new Vector3(-0.993774f, 0.118359f, -0.147252f); - vertices[49] = new Vector3(-0.993774f, -0.124103f, -0.147252f); - vertices[50] = new Vector3(-0.993774f, -0.124103f, 0.154165f); - vertices[51] = new Vector3(-0.993774f, 0.118359f, 0.154165f); - vertices[52] = new Vector3(-0.817496f, -0.585607f, 0.137973f); - vertices[53] = new Vector3(-0.696265f, -0.660531f, 0.334128f); - vertices[54] = new Vector3(-0.789408f, -0.416680f, 0.484838f); - vertices[55] = new Vector3(-0.910638f, -0.341756f, 0.288682f); - vertices[56] = new Vector3(-0.411039f, -0.484253f, 0.795634f); - vertices[57] = new Vector3(-0.336113f, -0.288097f, 0.916863f); - vertices[58] = new Vector3(-0.579966f, -0.137388f, 0.823721f); - vertices[59] = new Vector3(-0.654891f, -0.333543f, 0.702490f); - vertices[60] = new Vector3(0.341652f, -0.288097f, 0.916863f); - vertices[61] = new Vector3(0.416578f, -0.484253f, 0.795634f); - vertices[62] = new Vector3(0.660429f, -0.333543f, 0.702490f); - vertices[63] = new Vector3(0.585505f, -0.137388f, 0.823721f); - vertices[64] = new Vector3(0.333441f, -0.701905f, 0.661116f); - vertices[65] = new Vector3(0.137286f, -0.823136f, 0.586192f); - vertices[66] = new Vector3(0.287995f, -0.916278f, 0.342339f); - vertices[67] = new Vector3(0.484149f, -0.795049f, 0.417265f); - vertices[68] = new Vector3(-0.131747f, -0.823136f, 0.586192f); - vertices[69] = new Vector3(-0.327903f, -0.701905f, 0.661116f); - vertices[70] = new Vector3(-0.478612f, -0.795049f, 0.417265f); - vertices[71] = new Vector3(-0.282457f, -0.916278f, 0.342339f); - vertices[72] = new Vector3(-0.910638f, -0.341756f, -0.281770f); - vertices[73] = new Vector3(-0.789408f, -0.416680f, -0.477925f); - vertices[74] = new Vector3(-0.696265f, -0.660531f, -0.327216f); - vertices[75] = new Vector3(-0.817496f, -0.585607f, -0.131060f); - vertices[76] = new Vector3(-0.327903f, -0.701905f, -0.654204f); - vertices[77] = new Vector3(-0.131747f, -0.823136f, -0.579280f); - vertices[78] = new Vector3(-0.282457f, -0.916278f, -0.335426f); - vertices[79] = new Vector3(-0.478612f, -0.795049f, -0.410352f); - vertices[80] = new Vector3(0.153478f, -0.999415f, -0.117774f); - vertices[81] = new Vector3(0.153478f, -0.999415f, 0.124687f); - vertices[82] = new Vector3(-0.147939f, -0.999415f, 0.124687f); - vertices[83] = new Vector3(-0.147939f, -0.999415f, -0.117774f); - vertices[84] = new Vector3(0.701803f, -0.660531f, 0.334128f); - vertices[85] = new Vector3(0.823033f, -0.585607f, 0.137973f); - vertices[86] = new Vector3(0.916176f, -0.341756f, 0.288682f); - vertices[87] = new Vector3(0.794945f, -0.416680f, 0.484838f); - vertices[88] = new Vector3(0.823033f, -0.585607f, -0.131060f); - vertices[89] = new Vector3(0.701803f, -0.660531f, -0.327216f); - vertices[90] = new Vector3(0.794945f, -0.416680f, -0.477925f); - vertices[91] = new Vector3(0.916176f, -0.341756f, -0.281770f); - vertices[92] = new Vector3(0.484149f, -0.795049f, -0.410352f); - vertices[93] = new Vector3(0.287995f, -0.916278f, -0.335426f); - vertices[94] = new Vector3(0.137286f, -0.823136f, -0.579280f); - vertices[95] = new Vector3(0.333441f, -0.701905f, -0.654204f); - vertices[96] = new Vector3(-0.654891f, -0.333543f, -0.695578f); - vertices[97] = new Vector3(-0.579966f, -0.137388f, -0.816807f); - vertices[98] = new Vector3(-0.336113f, -0.288097f, -0.909951f); - vertices[99] = new Vector3(-0.411039f, -0.484253f, -0.788719f); - vertices[100] = new Vector3(-0.118461f, 0.147837f, -0.993087f); - vertices[101] = new Vector3(0.124000f, 0.147837f, -0.993087f); - vertices[102] = new Vector3(0.124000f, -0.153580f, -0.993087f); - vertices[103] = new Vector3(-0.118461f, -0.153580f, -0.993087f); - vertices[104] = new Vector3(0.585505f, -0.137388f, -0.816807f); - vertices[105] = new Vector3(0.660429f, -0.333543f, -0.695578f); - vertices[106] = new Vector3(0.416578f, -0.484253f, -0.788719f); - vertices[107] = new Vector3(0.341652f, -0.288097f, -0.909951f); - vertices[108] = new Vector3(0.999313f, -0.124103f, -0.147252f); - vertices[109] = new Vector3(0.999313f, 0.118359f, -0.147252f); - vertices[110] = new Vector3(0.999313f, 0.118359f, 0.154165f); - vertices[111] = new Vector3(0.999313f, -0.124103f, 0.154165f); - vertices[112] = new Vector3(0.660429f, 0.327801f, -0.695578f); - vertices[113] = new Vector3(0.585505f, 0.131646f, -0.816807f); - vertices[114] = new Vector3(0.341652f, 0.282353f, -0.909951f); - vertices[115] = new Vector3(0.416578f, 0.478508f, -0.788719f); - vertices[116] = new Vector3(-0.579966f, 0.131646f, -0.816807f); - vertices[117] = new Vector3(-0.654891f, 0.327801f, -0.695578f); - vertices[118] = new Vector3(-0.411039f, 0.478508f, -0.788719f); - vertices[119] = new Vector3(-0.336113f, 0.282353f, -0.909951f); - return vertices; - } - - - Buffer CreateTestConvexHull3() - { - BufferPool.Take(22, out var vertices); - vertices[0] = new Vector3(-0.103558f, 1.000000f, -0.490575f); - vertices[1] = new Vector3(0.266493f, 0.659794f, -0.363751f); - vertices[2] = new Vector3(-0.245774f, 0.762636f, -0.615304f); - vertices[3] = new Vector3(0.164688f, -0.777634f, -0.365919f); - vertices[4] = new Vector3(0.503268f, -0.846406f, -0.131286f); - vertices[5] = new Vector3(0.171066f, -0.931723f, -0.140738f); - vertices[6] = new Vector3(-0.247963f, -0.738059f, -0.413146f); - vertices[7] = new Vector3(-0.319203f, -0.260078f, -0.609331f); - vertices[8] = new Vector3(0.469624f, -0.747848f, -0.286486f); - vertices[9] = new Vector3(0.398526f, -0.238233f, -0.435281f); - vertices[10] = new Vector3(0.448274f, 0.295416f, -0.246327f); - vertices[11] = new Vector3(-0.245774f, 0.762636f, 0.596521f); - vertices[12] = new Vector3(0.266493f, 0.659794f, 0.344974f); - vertices[13] = new Vector3(-0.103558f, 1.000000f, 0.471792f); - vertices[14] = new Vector3(0.171066f, -0.931723f, 0.121961f); - vertices[15] = new Vector3(0.503268f, -0.846406f, 0.112509f); - vertices[16] = new Vector3(0.164688f, -0.777634f, 0.347137f); - vertices[17] = new Vector3(-0.319203f, -0.260078f, 0.590548f); - vertices[18] = new Vector3(-0.247963f, -0.738059f, 0.394364f); - vertices[19] = new Vector3(0.469624f, -0.747848f, 0.267709f); - vertices[20] = new Vector3(0.398526f, -0.238233f, 0.416498f); - vertices[21] = new Vector3(0.448274f, 0.295411f, 0.227550f); - return vertices; - } - - Buffer CreateJSONSourcedConvexHull(string filePath) - { - //ChatGPT wrote this, of course. - List points = new List(); - if (File.Exists(filePath)) - { - string jsonContent = File.ReadAllText(filePath); - List> rawPoints = JsonSerializer.Deserialize>>(jsonContent); - foreach (List point in rawPoints) - { - if (point.Count == 3) - { - Vector3 vector3Point = new Vector3((float)point[0], (float)point[1], (float)point[2]); - points.Add(vector3Point); - } - } - } - else - { - Console.Error.WriteLine("File not found: " + filePath); - } - - BufferPool.Take(points.Count, out Buffer buffer); - for (int i = 0; i < buffer.Length; ++i) - { - buffer[i] = points[i]; - } - return buffer; - } - - public override void Initialize(ContentArchive content, Camera camera) - { - camera.Position = new Vector3(0, -2.5f, 10); - camera.Yaw = 0; - camera.Pitch = 0; - - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); - //var hullPoints = CreateJSONSourcedConvexHull(@"Content/testHull.json"); - //for (int i = 0; i < hullPoints.Length; ++i) - //{ - // hullPoints[i] *= 0.03f; - //} - //var hullPoints = CreateRandomConvexHullPoints(); - var hullPoints = CreateMeshConvexHull(content.Load(@"Content\newt.obj"), new Vector3(1, 1.5f, 1f)); - //var hullPoints = CreatePlaneish(); - //var hullPoints = CreateDistantPlane(); - //var hullPoints = CreateTestConvexHull2(); - //var hullPoints = CreateBoxConvexHull(2); - var hullShape = new ConvexHull(hullPoints, BufferPool, out _); - //float largestError = 0; - //for (int i = 0; i < hullShape.FaceToVertexIndicesStart.Length; ++i) - //{ - // hullShape.GetVertexIndicesForFace(i, out var faceVertices); - // BundleIndexing.GetBundleIndices(i, out var normalBundleIndex, out var normalIndexInBundle); - // Vector3Wide.ReadSlot(ref hullShape.BoundingPlanes[normalBundleIndex].Normal, normalIndexInBundle, out var faceNormal); - // var offset = hullShape.BoundingPlanes[normalBundleIndex].Offset[normalIndexInBundle]; - // Console.WriteLine($"Face {i} errors:"); - // for (int j = 0; j < faceVertices.Length; ++j) - // { - // hullShape.GetPoint(faceVertices[j], out var point); - // var error = Vector3.Dot(point, faceNormal) - offset; - // Console.WriteLine($"v{j}: {error}"); - // largestError = MathF.Max(MathF.Abs(error), largestError); - // } - //} - //Console.WriteLine($"Largest error: {largestError}"); - - //ConvexHullHelper.ComputeHull(hullPoints, BufferPool, out var hullData, out debugSteps); - //this.points = hullPoints; - - var boxHullPoints = CreateBoxConvexHull(2); - var boxHullShape = new ConvexHull(boxHullPoints, BufferPool, out _); - - Matrix3x3.CreateScale(new Vector3(5, 0.5f, 3), out var scale); - var transform = Matrix3x3.CreateFromAxisAngle(Vector3.Normalize(new Vector3(3, 2, 1)), 1207) * scale; - const int transformCount = 10000; - var transformStart = Stopwatch.GetTimestamp(); - for (int i = 0; i < transformCount; ++i) - { - CreateTransformedCopy(hullShape, transform, BufferPool, out var transformedHullShape); - transformedHullShape.Dispose(BufferPool); - } - var transformEnd = Stopwatch.GetTimestamp(); - Console.WriteLine($"Transform hull computation time (us): {(transformEnd - transformStart) * 1e6 / (transformCount * Stopwatch.Frequency)}"); - - hullShape.RayTest(RigidPose.Identity, new Vector3(0, 1, 0), -Vector3.UnitY, out var t, out var normal); - - const int rayIterationCount = 10000; - var rayPose = RigidPose.Identity; - var rayOrigin = new Vector3(0, 2, 0); - var rayDirection = new Vector3(0, -1, 0); - - int hitCounter = 0; - var start = Stopwatch.GetTimestamp(); - for (int i = 0; i < rayIterationCount; ++i) - { - if (hullShape.RayTest(rayPose, rayOrigin, rayDirection, out _, out _)) - { - ++hitCounter; - } - } - var end = Stopwatch.GetTimestamp(); - Console.WriteLine($"Hit counter: {hitCounter}, computation time (us): {(end - start) * 1e6 / (rayIterationCount * Stopwatch.Frequency)}"); - - const int iterationCount = 100; - start = Stopwatch.GetTimestamp(); - for (int i = 0; i < iterationCount; ++i) - { - CreateShape(hullPoints, BufferPool, out _, out var perfTestShape); - perfTestShape.Dispose(BufferPool); - } - end = Stopwatch.GetTimestamp(); - Console.WriteLine($"Hull computation time (us): {(end - start) * 1e6 / (iterationCount * Stopwatch.Frequency)}"); - - var hullShapeIndex = Simulation.Shapes.Add(hullShape); - var boxHullShapeIndex = Simulation.Shapes.Add(boxHullShape); - var inertia = hullShape.ComputeInertia(1); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 0, 5), inertia, new(hullShapeIndex, 20, 20), -0.01f)); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 0, 3), boxHullShape.ComputeInertia(1), new(boxHullShapeIndex, 20, 20), -0.01f)); - - Simulation.Statics.Add(new StaticDescription(new Vector3(-25, -5, 0), Simulation.Shapes.Add(new Sphere(2)))); - Simulation.Statics.Add(new StaticDescription(new Vector3(-20, -5, 0), Simulation.Shapes.Add(new Capsule(0.5f, 2)))); - Simulation.Statics.Add(new StaticDescription(new Vector3(-15, -5, 0), Simulation.Shapes.Add(new Box(2f, 2f, 2f)))); - Simulation.Statics.Add(new StaticDescription(new Vector3(-10, -5, 5), Simulation.Shapes.Add(new Triangle { A = new Vector3(0, 0, -10), B = new Vector3(5, 0, -10), C = new Vector3(0, 0, -5) }))); - Simulation.Statics.Add(new StaticDescription(new Vector3(-5, -5, 0), Simulation.Shapes.Add(new Cylinder(1, 1)))); - Simulation.Statics.Add(new StaticDescription(new Vector3(-5, -5, 5), Simulation.Shapes.Add(new Cylinder(1, 1)))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5, 0), hullShapeIndex)); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5, 5), Simulation.Shapes.Add(boxHullShape))); - - var spacing = new Vector3(3f, 3f, 3); - int width = 16; - int height = 16; - int length = 0; - var origin = -0.5f * spacing * new Vector3(width, 0, length) + new Vector3(40, 0.2f, -40); - for (int i = 0; i < width; ++i) - { - for (int j = 0; j < height; ++j) - { - for (int k = 0; k < length; ++k) - { - Simulation.Bodies.Add(BodyDescription.CreateDynamic( - (origin + spacing * new Vector3(i, j, k), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathHelper.Pi * 0.05f)), - inertia, hullShapeIndex, 0.01f)); - } - } - } - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -10, 0), Simulation.Shapes.Add(new Box(1000, 1, 1000)))); - - Random random = new Random(5); - var mesh = DemoMeshHelper.CreateDeformedPlane(64, 64, (x, y) => new Vector3( - x + 8, - 2f * MathF.Sin(x * 0.125f) * MathF.Sin(y * 0.125f) + 0.1f * random.NextSingle() - 3, - y - 8), new Vector3(1, 1, 1), BufferPool); - Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); - } - - void TestConvexHullCreation() - { - var random = new Random(5); - for (int iterationIndex = 0; iterationIndex < 100000; ++iterationIndex) - { - const int pointCount = 32; - var points = new QuickList(pointCount, BufferPool); - for (int i = 0; i < pointCount; ++i) - { - points.AllocateUnsafely() = new Vector3(1 * random.NextSingle(), 2 * random.NextSingle(), 3 * random.NextSingle()); - } - - var pointsBuffer = points.Span.Slice(points.Count); - CreateShape(pointsBuffer, BufferPool, out _, out var hullShape); - - hullShape.Dispose(BufferPool); - } - } - - //Buffer points; - //List debugSteps; - - //int stepIndex = 0; - - //public override void Update(Window window, Camera camera, Input input, float dt) - //{ - // if (input.TypedCharacters.Contains('x')) - // { - // stepIndex = Math.Max(stepIndex - 1, 0); - // } - // if (input.TypedCharacters.Contains('c')) - // { - // stepIndex = Math.Min(stepIndex + 1, debugSteps.Count - 1); - // } - // if (input.WasPushed(OpenTK.Input.Key.P)) - // { - // showWireframe = !showWireframe; - // } - // if (input.WasPushed(OpenTK.Input.Key.U)) - // { - // showDeleted = !showDeleted; - // } - // if (input.WasPushed(OpenTK.Input.Key.Y)) - // { - // showVertexIndices = !showVertexIndices; - // } - // if (input.WasPushed(OpenTK.Input.Key.H)) - // { - // showFaceVertexStatuses = !showFaceVertexStatuses; - // } - // base.Update(window, camera, input, dt); - //} - - //bool showWireframe; - //bool showDeleted; - //bool showVertexIndices; - //bool showFaceVertexStatuses = true; - //public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) - //{ - // var step = debugSteps[stepIndex]; - // var scale = 10f; - // var renderOffset = new Vector3(0, 15, 0); - // for (int i = 0; i < points.Length; ++i) - // { - // var pose = new RigidPose(renderOffset + points[i] * scale); - // renderer.Shapes.AddShape(new Box(0.1f, 0.1f, 0.1f), Simulation.Shapes, pose, new Vector3(0.5f, 0.5f, 0.5f)); - // if (!step.AllowVertex[i] && showFaceVertexStatuses) - // renderer.Shapes.AddShape(new Box(0.6f, 0.25f, 0.25f), Simulation.Shapes, pose, new Vector3(1, 0, 0)); - // } - // if (showFaceVertexStatuses) - // { - // for (int i = 0; i < step.Raw.Count; ++i) - // { - // var pose = new RigidPose(renderOffset + points[step.Raw[i]] * scale); - // renderer.Shapes.AddShape(new Box(0.25f, 0.6f, 0.25f), Simulation.Shapes, pose, new Vector3(0, 0, 1)); - // } - // for (int i = 0; i < step.Reduced.Count; ++i) - // { - // var pose = new RigidPose(renderOffset + points[step.Reduced[i]] * scale); - // renderer.Shapes.AddShape(new Box(0.25f, 0.25f, 0.6f), Simulation.Shapes, pose, new Vector3(0, 1, 0)); - // } - // } - - // { - // var pose = new RigidPose(renderOffset); - // for (int i = 0; i < step.FaceStarts.Count; ++i) - // { - // if (showDeleted || !step.FaceDeleted[i]) - // { - // var faceStart = step.FaceStarts[i]; - // var faceEnd = i + 1 < step.FaceStarts.Count ? step.FaceStarts[i + 1] : step.FaceIndices.Count; - // var count = faceEnd - faceStart; - // var color = step.FaceDeleted[i] ? new Vector3(0.25f, 0.25f, 0.25f) : step.FaceIndex == i ? new Vector3(1, 0, 0.5f) : new Vector3(1, 0, 1); - // var deletionInducedScale = step.FaceDeleted[i] ? new Vector3(1.1f) : new Vector3(1f); - - // var offset = step.FaceDeleted[i] ? step.FaceNormals[i] * 0.25f : new Vector3(); - // if (showWireframe) - // { - // var previousIndex = faceEnd - 1; - // for (int q = faceStart; q < faceEnd; ++q) - // { - // var a = points[step.FaceIndices[q]] * scale + pose.Position + offset; - // var b = points[step.FaceIndices[previousIndex]] * scale + pose.Position + offset; - // previousIndex = q; - // renderer.Lines.Allocate() = new LineInstance(a, b, color, Vector3.Zero); - // } - // } - // else - // { - // for (int k = faceStart + 2; k < faceEnd; ++k) - // { - // renderer.Shapes.AddShape(new Triangle - // { - // A = points[step.FaceIndices[faceStart]] * scale + offset, - // B = points[step.FaceIndices[k]] * scale + offset, - // C = points[step.FaceIndices[k - 1]] * scale + offset - // }, Simulation.Shapes, pose, color); - // } - // } - // } - // } - // } - - // if (showVertexIndices) - // { - // for (int i = 0; i < points.Length; ++i) - // { - // if (DemoRenderer.Helpers.GetScreenLocation(points[i] * scale + renderOffset, camera.ViewProjection, renderer.Surface.Resolution, out var location)) - // { - // renderer.TextBatcher.Write(text.Clear().Append(i), location, 10, new Vector3(1), font); - // } - // } - // } - - // var edgeMidpoint = renderOffset + (points[step.SourceEdge.A] + points[step.SourceEdge.B]) * scale * 0.5f; - // renderer.Lines.Allocate() = new LineInstance(edgeMidpoint, edgeMidpoint + step.BasisX * scale * 0.5f, new Vector3(1, 1, 0), new Vector3()); - // renderer.Lines.Allocate() = new LineInstance(edgeMidpoint, edgeMidpoint + step.BasisY * scale * 0.5f, new Vector3(0, 1, 0), new Vector3()); - // renderer.TextBatcher.Write( - // text.Clear().Append($"Enumerate step with X and C. Current step: ").Append(stepIndex + 1).Append(" out of ").Append(debugSteps.Count), - // new Vector2(32, renderer.Surface.Resolution.Y - 140), 20, new Vector3(1), font); - // renderer.TextBatcher.Write(text.Clear().Append("Show wireframe: P ").Append(showWireframe ? "(on)" : "(off)"), new Vector2(32, renderer.Surface.Resolution.Y - 120), 20, new Vector3(1), font); - // renderer.TextBatcher.Write(text.Clear().Append("Show deleted: U ").Append(showDeleted ? "(on)" : "(off)"), new Vector2(32, renderer.Surface.Resolution.Y - 100), 20, new Vector3(1), font); - // renderer.TextBatcher.Write(text.Clear().Append("Show vertex indices: Y ").Append(showVertexIndices ? "(on)" : "(off)"), new Vector2(32, renderer.Surface.Resolution.Y - 80), 20, new Vector3(1), font); - // renderer.TextBatcher.Write(text.Clear().Append("Show face vertex statuses: H ").Append(showFaceVertexStatuses ? "(on)" : "(off)"), new Vector2(32, renderer.Surface.Resolution.Y - 60), 20, new Vector3(1), font); - - - // base.Render(renderer, camera, input, text, font); - //} -} +using System; +using System.Collections.Generic; +using System.Numerics; +using BepuPhysics.Collidables; +using BepuUtilities.Collections; +using DemoContentLoader; +using DemoRenderer; +using BepuPhysics; +using static BepuPhysics.Collidables.ConvexHullHelper; +using System.Diagnostics; +using BepuUtilities; +using BepuPhysics.Constraints; +using BepuUtilities.Memory; +using System.Text.Json; +using System.IO; + +namespace Demos.SpecializedTests; + +public class ConvexHullTestDemo : Demo +{ + Buffer CreateRandomConvexHullPoints() + { + const int pointCount = 50; + BufferPool.Take(pointCount, out var points); + + var random = new Random(5); + for (int i = 0; i < pointCount; ++i) + { + points[i] = new Vector3(3 * random.NextSingle(), 1 * random.NextSingle(), 3 * random.NextSingle()); + } + + return points; + } + + Buffer CreatePlaneish() + { + var points = new Buffer(12, BufferPool); + points[0] = new Vector3(-13.82f, 16.79f, 13.83f); + points[1] = new Vector3(13.82f, -16.79f, -13.83f); + points[2] = new Vector3(13.82f, 16.79f, -13.83f); + points[3] = new Vector3(-13.82f, 16.79f, 13.83f); + points[4] = new Vector3(13.82f, 16.79f, -13.83f); + points[5] = new Vector3(-13.82f, -16.79f, 13.83f); + points[6] = new Vector3(13.82f, 16.79f, -13.83f); + points[7] = new Vector3(13.82f, -16.79f, -13.83f); + points[8] = new Vector3(-13.82f, -16.79f, 13.83f); + points[9] = new Vector3(-13.82f, 16.79f, 13.83f); + points[10] = new Vector3(-13.82f, -16.79f, 13.83f); + points[11] = new Vector3(13.82f, -16.79f, -13.83f); + return points; + } + + Buffer CreateDistantPlane() + { + var points = new Buffer(12, BufferPool); + points[0] = new(-151.0875f, -2.2505488f, 102.17515f); + points[1] = new(-151.10571f, 2.1121342f, -17.699797f); + points[2] = new(-151.08746f, -2.2504745f, -17.699797f); + points[3] = new(-151.10571f, 2.1121342f, -17.699797f); + points[4] = new(-151.0875f, -2.2505488f, 102.17515f); + points[5] = new(-151.10574f, 2.1120775f, 102.17517f); + points[6] = new(-151.10571f, 2.1121342f, -17.699797f); + points[7] = new(-151.10574f, 2.1120775f, 102.17517f); + points[8] = new(-151.08746f, -2.2504745f, -17.699797f); + points[9] = new(-151.08746f, -2.2504745f, -17.699797f); + points[10] = new(-151.10574f, 2.1120775f, 102.17517f); + points[11] = new(-151.0875f, -2.2505488f, 102.17515f); + return points; + } + + Buffer CreateMeshConvexHull(MeshContent meshContent, Vector3 scale) + { + //This is actually a pretty good example of how *not* to make a convex hull shape. + //Generating it directly from a graphical data source tends to have way more surface complexity than needed, + //and it tends to have a lot of near-but-not-quite-coplanar surfaces which can make the contact manifold less stable. + //Prefer a simpler source with more distinct features, possibly created with an automated content-time tool. + BufferPool.Take(meshContent.Triangles.Length * 3, out var points); + for (int i = 0; i < meshContent.Triangles.Length; ++i) + { + ref var triangle = ref meshContent.Triangles[i]; + //resisting the urge to just reinterpret the memory + points[i * 3 + 0] = triangle.A * scale; + points[i * 3 + 1] = triangle.B * scale; + points[i * 3 + 2] = triangle.C * scale; + } + return points; + } + + Buffer CreateBoxConvexHull(float boxScale) + { + BufferPool.Take(8, out var points); + points[0] = new Vector3(0, 0, 0); + points[1] = new Vector3(0, 0, boxScale); + points[2] = new Vector3(0, boxScale, 0); + points[3] = new Vector3(0, boxScale, boxScale); + points[4] = new Vector3(boxScale, 0, 0); + points[5] = new Vector3(boxScale, 0, boxScale); + points[6] = new Vector3(boxScale, boxScale, 0); + points[7] = new Vector3(boxScale, boxScale, boxScale); + return points; + } + + //A couple of test point sets from PEEL: https://github.com/Pierre-Terdiman/PEEL_PhysX_Edition + Buffer CreateTestConvexHull() + { + BufferPool.Take(50, out var vertices); + vertices[0] = new Vector3(-0.000000f, -0.297120f, -0.000000f); + vertices[1] = new Vector3(0.258819f, -0.297120f, 0.965926f); + vertices[2] = new Vector3(-0.000000f, -0.297120f, 1.000000f); + vertices[3] = new Vector3(0.500000f, -0.297120f, 0.866026f); + vertices[4] = new Vector3(0.707107f, -0.297120f, 0.707107f); + vertices[5] = new Vector3(0.866026f, -0.297120f, 0.500000f); + vertices[6] = new Vector3(0.965926f, -0.297120f, 0.258819f); + vertices[7] = new Vector3(1.000000f, -0.297120f, -0.000000f); + vertices[8] = new Vector3(0.965926f, -0.297120f, -0.258819f); + vertices[9] = new Vector3(0.866026f, -0.297120f, -0.500000f); + vertices[10] = new Vector3(0.707107f, -0.297120f, -0.707107f); + vertices[11] = new Vector3(0.500000f, -0.297120f, -0.866026f); + vertices[12] = new Vector3(0.258819f, -0.297120f, -0.965926f); + vertices[13] = new Vector3(-0.000000f, -0.297120f, -1.000000f); + vertices[14] = new Vector3(-0.258819f, -0.297120f, -0.965926f); + vertices[15] = new Vector3(-0.500000f, -0.297120f, -0.866025f); + vertices[16] = new Vector3(-0.707107f, -0.297120f, -0.707107f); + vertices[17] = new Vector3(-0.866026f, -0.297120f, -0.500000f); + vertices[18] = new Vector3(-0.965926f, -0.297120f, -0.258819f); + vertices[19] = new Vector3(-1.000000f, -0.297120f, 0.000000f); + vertices[20] = new Vector3(-0.965926f, -0.297120f, 0.258819f); + vertices[21] = new Vector3(-0.866025f, -0.297120f, 0.500000f); + vertices[22] = new Vector3(-0.707107f, -0.297120f, 0.707107f); + vertices[23] = new Vector3(-0.500000f, -0.297120f, 0.866026f); + vertices[24] = new Vector3(-0.258819f, -0.297120f, 0.965926f); + vertices[25] = new Vector3(-0.000000f, 0.297120f, -0.000000f); + vertices[26] = new Vector3(-0.000000f, 0.297120f, 0.537813f); + vertices[27] = new Vector3(0.139196f, 0.297120f, 0.519487f); + vertices[28] = new Vector3(0.268907f, 0.297120f, 0.465760f); + vertices[29] = new Vector3(0.380291f, 0.297120f, 0.380291f); + vertices[30] = new Vector3(0.465760f, 0.297120f, 0.268907f); + vertices[31] = new Vector3(0.519487f, 0.297120f, 0.139196f); + vertices[32] = new Vector3(0.537813f, 0.297120f, -0.000000f); + vertices[33] = new Vector3(0.519487f, 0.297120f, -0.139196f); + vertices[34] = new Vector3(0.465760f, 0.297120f, -0.268907f); + vertices[35] = new Vector3(0.380291f, 0.297120f, -0.380291f); + vertices[36] = new Vector3(0.268907f, 0.297120f, -0.465760f); + vertices[37] = new Vector3(0.139196f, 0.297120f, -0.519487f); + vertices[38] = new Vector3(-0.000000f, 0.297120f, -0.537813f); + vertices[39] = new Vector3(-0.139196f, 0.297120f, -0.519487f); + vertices[40] = new Vector3(-0.268907f, 0.297120f, -0.465760f); + vertices[41] = new Vector3(-0.380291f, 0.297120f, -0.380291f); + vertices[42] = new Vector3(-0.465760f, 0.297120f, -0.268907f); + vertices[43] = new Vector3(-0.519487f, 0.297120f, -0.139196f); + vertices[44] = new Vector3(-0.537813f, 0.297120f, 0.000000f); + vertices[45] = new Vector3(-0.519487f, 0.297120f, 0.139196f); + vertices[46] = new Vector3(-0.465760f, 0.297120f, 0.268907f); + vertices[47] = new Vector3(-0.380291f, 0.297120f, 0.380291f); + vertices[48] = new Vector3(-0.268907f, 0.297120f, 0.465760f); + vertices[49] = new Vector3(-0.139196f, 0.297120f, 0.519487f); + return vertices; + } + + Buffer CreateTestConvexHull2() + { + BufferPool.Take(120, out var vertices); + vertices[0] = new Vector3(0.153478f, 0.993671f, 0.124687f); + vertices[1] = new Vector3(0.153478f, 0.993671f, -0.117774f); + vertices[2] = new Vector3(-0.147939f, 0.993671f, -0.117774f); + vertices[3] = new Vector3(-0.147939f, 0.993671f, 0.124687f); + vertices[4] = new Vector3(0.137286f, 0.817392f, 0.586192f); + vertices[5] = new Vector3(0.333441f, 0.696161f, 0.661116f); + vertices[6] = new Vector3(0.484149f, 0.789305f, 0.417265f); + vertices[7] = new Vector3(0.287995f, 0.910536f, 0.342339f); + vertices[8] = new Vector3(0.794945f, 0.410936f, 0.484838f); + vertices[9] = new Vector3(0.916176f, 0.336012f, 0.288682f); + vertices[10] = new Vector3(0.823033f, 0.579863f, 0.137973f); + vertices[11] = new Vector3(0.701803f, 0.654787f, 0.334128f); + vertices[12] = new Vector3(0.916176f, 0.336012f, -0.281770f); + vertices[13] = new Vector3(0.794945f, 0.410936f, -0.477925f); + vertices[14] = new Vector3(0.701803f, 0.654787f, -0.327216f); + vertices[15] = new Vector3(0.823033f, 0.579863f, -0.131060f); + vertices[16] = new Vector3(0.333441f, 0.696161f, -0.654204f); + vertices[17] = new Vector3(0.137286f, 0.817392f, -0.579280f); + vertices[18] = new Vector3(0.287995f, 0.910536f, -0.335426f); + vertices[19] = new Vector3(0.484149f, 0.789305f, -0.410352f); + vertices[20] = new Vector3(-0.131747f, 0.817392f, -0.579280f); + vertices[21] = new Vector3(-0.327903f, 0.696161f, -0.654204f); + vertices[22] = new Vector3(-0.478612f, 0.789305f, -0.410352f); + vertices[23] = new Vector3(-0.282457f, 0.910536f, -0.335426f); + vertices[24] = new Vector3(-0.789408f, 0.410936f, -0.477925f); + vertices[25] = new Vector3(-0.910638f, 0.336012f, -0.281770f); + vertices[26] = new Vector3(-0.817496f, 0.579863f, -0.131060f); + vertices[27] = new Vector3(-0.696265f, 0.654787f, -0.327216f); + vertices[28] = new Vector3(-0.910638f, 0.336012f, 0.288682f); + vertices[29] = new Vector3(-0.789408f, 0.410936f, 0.484838f); + vertices[30] = new Vector3(-0.696265f, 0.654787f, 0.334128f); + vertices[31] = new Vector3(-0.817496f, 0.579863f, 0.137973f); + vertices[32] = new Vector3(-0.327903f, 0.696161f, 0.661116f); + vertices[33] = new Vector3(-0.131747f, 0.817392f, 0.586192f); + vertices[34] = new Vector3(-0.282457f, 0.910536f, 0.342339f); + vertices[35] = new Vector3(-0.478612f, 0.789305f, 0.417265f); + vertices[36] = new Vector3(0.416578f, 0.478508f, 0.795634f); + vertices[37] = new Vector3(0.341652f, 0.282353f, 0.916863f); + vertices[38] = new Vector3(0.585505f, 0.131646f, 0.823721f); + vertices[39] = new Vector3(0.660429f, 0.327801f, 0.702490f); + vertices[40] = new Vector3(0.124000f, 0.147837f, 1.000000f); + vertices[41] = new Vector3(-0.118461f, 0.147837f, 1.000000f); + vertices[42] = new Vector3(-0.118461f, -0.153580f, 1.000000f); + vertices[43] = new Vector3(0.124000f, -0.153580f, 1.000000f); + vertices[44] = new Vector3(-0.336113f, 0.282353f, 0.916863f); + vertices[45] = new Vector3(-0.411039f, 0.478508f, 0.795634f); + vertices[46] = new Vector3(-0.654891f, 0.327801f, 0.702490f); + vertices[47] = new Vector3(-0.579966f, 0.131646f, 0.823721f); + vertices[48] = new Vector3(-0.993774f, 0.118359f, -0.147252f); + vertices[49] = new Vector3(-0.993774f, -0.124103f, -0.147252f); + vertices[50] = new Vector3(-0.993774f, -0.124103f, 0.154165f); + vertices[51] = new Vector3(-0.993774f, 0.118359f, 0.154165f); + vertices[52] = new Vector3(-0.817496f, -0.585607f, 0.137973f); + vertices[53] = new Vector3(-0.696265f, -0.660531f, 0.334128f); + vertices[54] = new Vector3(-0.789408f, -0.416680f, 0.484838f); + vertices[55] = new Vector3(-0.910638f, -0.341756f, 0.288682f); + vertices[56] = new Vector3(-0.411039f, -0.484253f, 0.795634f); + vertices[57] = new Vector3(-0.336113f, -0.288097f, 0.916863f); + vertices[58] = new Vector3(-0.579966f, -0.137388f, 0.823721f); + vertices[59] = new Vector3(-0.654891f, -0.333543f, 0.702490f); + vertices[60] = new Vector3(0.341652f, -0.288097f, 0.916863f); + vertices[61] = new Vector3(0.416578f, -0.484253f, 0.795634f); + vertices[62] = new Vector3(0.660429f, -0.333543f, 0.702490f); + vertices[63] = new Vector3(0.585505f, -0.137388f, 0.823721f); + vertices[64] = new Vector3(0.333441f, -0.701905f, 0.661116f); + vertices[65] = new Vector3(0.137286f, -0.823136f, 0.586192f); + vertices[66] = new Vector3(0.287995f, -0.916278f, 0.342339f); + vertices[67] = new Vector3(0.484149f, -0.795049f, 0.417265f); + vertices[68] = new Vector3(-0.131747f, -0.823136f, 0.586192f); + vertices[69] = new Vector3(-0.327903f, -0.701905f, 0.661116f); + vertices[70] = new Vector3(-0.478612f, -0.795049f, 0.417265f); + vertices[71] = new Vector3(-0.282457f, -0.916278f, 0.342339f); + vertices[72] = new Vector3(-0.910638f, -0.341756f, -0.281770f); + vertices[73] = new Vector3(-0.789408f, -0.416680f, -0.477925f); + vertices[74] = new Vector3(-0.696265f, -0.660531f, -0.327216f); + vertices[75] = new Vector3(-0.817496f, -0.585607f, -0.131060f); + vertices[76] = new Vector3(-0.327903f, -0.701905f, -0.654204f); + vertices[77] = new Vector3(-0.131747f, -0.823136f, -0.579280f); + vertices[78] = new Vector3(-0.282457f, -0.916278f, -0.335426f); + vertices[79] = new Vector3(-0.478612f, -0.795049f, -0.410352f); + vertices[80] = new Vector3(0.153478f, -0.999415f, -0.117774f); + vertices[81] = new Vector3(0.153478f, -0.999415f, 0.124687f); + vertices[82] = new Vector3(-0.147939f, -0.999415f, 0.124687f); + vertices[83] = new Vector3(-0.147939f, -0.999415f, -0.117774f); + vertices[84] = new Vector3(0.701803f, -0.660531f, 0.334128f); + vertices[85] = new Vector3(0.823033f, -0.585607f, 0.137973f); + vertices[86] = new Vector3(0.916176f, -0.341756f, 0.288682f); + vertices[87] = new Vector3(0.794945f, -0.416680f, 0.484838f); + vertices[88] = new Vector3(0.823033f, -0.585607f, -0.131060f); + vertices[89] = new Vector3(0.701803f, -0.660531f, -0.327216f); + vertices[90] = new Vector3(0.794945f, -0.416680f, -0.477925f); + vertices[91] = new Vector3(0.916176f, -0.341756f, -0.281770f); + vertices[92] = new Vector3(0.484149f, -0.795049f, -0.410352f); + vertices[93] = new Vector3(0.287995f, -0.916278f, -0.335426f); + vertices[94] = new Vector3(0.137286f, -0.823136f, -0.579280f); + vertices[95] = new Vector3(0.333441f, -0.701905f, -0.654204f); + vertices[96] = new Vector3(-0.654891f, -0.333543f, -0.695578f); + vertices[97] = new Vector3(-0.579966f, -0.137388f, -0.816807f); + vertices[98] = new Vector3(-0.336113f, -0.288097f, -0.909951f); + vertices[99] = new Vector3(-0.411039f, -0.484253f, -0.788719f); + vertices[100] = new Vector3(-0.118461f, 0.147837f, -0.993087f); + vertices[101] = new Vector3(0.124000f, 0.147837f, -0.993087f); + vertices[102] = new Vector3(0.124000f, -0.153580f, -0.993087f); + vertices[103] = new Vector3(-0.118461f, -0.153580f, -0.993087f); + vertices[104] = new Vector3(0.585505f, -0.137388f, -0.816807f); + vertices[105] = new Vector3(0.660429f, -0.333543f, -0.695578f); + vertices[106] = new Vector3(0.416578f, -0.484253f, -0.788719f); + vertices[107] = new Vector3(0.341652f, -0.288097f, -0.909951f); + vertices[108] = new Vector3(0.999313f, -0.124103f, -0.147252f); + vertices[109] = new Vector3(0.999313f, 0.118359f, -0.147252f); + vertices[110] = new Vector3(0.999313f, 0.118359f, 0.154165f); + vertices[111] = new Vector3(0.999313f, -0.124103f, 0.154165f); + vertices[112] = new Vector3(0.660429f, 0.327801f, -0.695578f); + vertices[113] = new Vector3(0.585505f, 0.131646f, -0.816807f); + vertices[114] = new Vector3(0.341652f, 0.282353f, -0.909951f); + vertices[115] = new Vector3(0.416578f, 0.478508f, -0.788719f); + vertices[116] = new Vector3(-0.579966f, 0.131646f, -0.816807f); + vertices[117] = new Vector3(-0.654891f, 0.327801f, -0.695578f); + vertices[118] = new Vector3(-0.411039f, 0.478508f, -0.788719f); + vertices[119] = new Vector3(-0.336113f, 0.282353f, -0.909951f); + return vertices; + } + + + Buffer CreateTestConvexHull3() + { + BufferPool.Take(22, out var vertices); + vertices[0] = new Vector3(-0.103558f, 1.000000f, -0.490575f); + vertices[1] = new Vector3(0.266493f, 0.659794f, -0.363751f); + vertices[2] = new Vector3(-0.245774f, 0.762636f, -0.615304f); + vertices[3] = new Vector3(0.164688f, -0.777634f, -0.365919f); + vertices[4] = new Vector3(0.503268f, -0.846406f, -0.131286f); + vertices[5] = new Vector3(0.171066f, -0.931723f, -0.140738f); + vertices[6] = new Vector3(-0.247963f, -0.738059f, -0.413146f); + vertices[7] = new Vector3(-0.319203f, -0.260078f, -0.609331f); + vertices[8] = new Vector3(0.469624f, -0.747848f, -0.286486f); + vertices[9] = new Vector3(0.398526f, -0.238233f, -0.435281f); + vertices[10] = new Vector3(0.448274f, 0.295416f, -0.246327f); + vertices[11] = new Vector3(-0.245774f, 0.762636f, 0.596521f); + vertices[12] = new Vector3(0.266493f, 0.659794f, 0.344974f); + vertices[13] = new Vector3(-0.103558f, 1.000000f, 0.471792f); + vertices[14] = new Vector3(0.171066f, -0.931723f, 0.121961f); + vertices[15] = new Vector3(0.503268f, -0.846406f, 0.112509f); + vertices[16] = new Vector3(0.164688f, -0.777634f, 0.347137f); + vertices[17] = new Vector3(-0.319203f, -0.260078f, 0.590548f); + vertices[18] = new Vector3(-0.247963f, -0.738059f, 0.394364f); + vertices[19] = new Vector3(0.469624f, -0.747848f, 0.267709f); + vertices[20] = new Vector3(0.398526f, -0.238233f, 0.416498f); + vertices[21] = new Vector3(0.448274f, 0.295411f, 0.227550f); + return vertices; + } + + Buffer CreateJSONSourcedConvexHull(string filePath) + { + //ChatGPT wrote this, of course. + List points = new List(); + if (File.Exists(filePath)) + { + string jsonContent = File.ReadAllText(filePath); + List> rawPoints = JsonSerializer.Deserialize>>(jsonContent); + foreach (List point in rawPoints) + { + if (point.Count == 3) + { + Vector3 vector3Point = new Vector3((float)point[0], (float)point[1], (float)point[2]); + points.Add(vector3Point); + } + } + } + else + { + Console.Error.WriteLine("File not found: " + filePath); + } + + BufferPool.Take(points.Count, out Buffer buffer); + for (int i = 0; i < buffer.Length; ++i) + { + buffer[i] = points[i]; + } + return buffer; + } + + void CreateHellCubeFace(int widthInPoints, float step, Buffer facePoints, Matrix3x3 transform, float localFaceOffset) + { + var offset = step * (widthInPoints - 1) * 0.5f; + for (int i = 0; i < widthInPoints; ++i) + { + var x = i * step - offset; + for (int j = 0; j < widthInPoints; ++j) + { + var y = j * step - offset; + var localOffset = new Vector3(x, y, localFaceOffset); + Matrix3x3.Transform(localOffset, transform, out var worldOffset); + facePoints[i * widthInPoints + j] = worldOffset; + } + } + } + Buffer CreateHellCube(int widthInPoints) + { + var facePointCount = widthInPoints * widthInPoints; + BufferPool.Take(facePointCount * 6, out Buffer buffer); + var size = 8f; + var halfSize = size / 2; + var step = size / (widthInPoints - 1); + Matrix3x3.CreateFromAxisAngle(Vector3.Normalize(new Vector3(1f, 1, 1f)), float.Pi / 2, out var cubeTransform); + Matrix3x3.CreateFromAxisAngle(new Vector3(0, 1, 0), float.Pi / 2, out var zFace); + zFace *= cubeTransform; + Matrix3x3.CreateFromAxisAngle(new Vector3(0, 1, 0), float.Pi, out var localFace2); + Matrix3x3.CreateFromAxisAngle(new Vector3(1, 0, 0), -float.Pi / 2, out var yFace); + yFace *= cubeTransform; + CreateHellCubeFace(widthInPoints, step, buffer.Slice(facePointCount * 0, facePointCount), cubeTransform, halfSize); + CreateHellCubeFace(widthInPoints, step, buffer.Slice(facePointCount * 1, facePointCount), zFace, halfSize); + CreateHellCubeFace(widthInPoints, step, buffer.Slice(facePointCount * 2, facePointCount), cubeTransform, -halfSize); + CreateHellCubeFace(widthInPoints, step, buffer.Slice(facePointCount * 3, facePointCount), zFace, -halfSize); + CreateHellCubeFace(widthInPoints, step, buffer.Slice(facePointCount * 4, facePointCount), yFace, halfSize); + CreateHellCubeFace(widthInPoints, step, buffer.Slice(facePointCount * 5, facePointCount), yFace, -halfSize); + return buffer; + } + + public override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(0, -2.5f, 10); + camera.Yaw = 0; + camera.Pitch = 0; + + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); + //var hullPoints = CreateJSONSourcedConvexHull(@"Content/testHull.json"); + //for (int i = 0; i < hullPoints.Length; ++i) + //{ + // hullPoints[i] *= 0.03f; + //} + //var hullPoints = CreateRandomConvexHullPoints(); + //var hullPoints = CreateMeshConvexHull(content.Load(@"Content\newt.obj"), new Vector3(1, 1.5f, 1f)); + var hullPoints = CreateHellCube(200); + //var hullPoints = CreatePlaneish(); + //var hullPoints = CreateDistantPlane(); + //var hullPoints = CreateTestConvexHull(); + //var hullPoints = CreateTestConvexHull2(); + //var hullPoints = CreateTestConvexHull3(); + //var hullPoints = CreateBoxConvexHull(2); + var hullShape = new ConvexHull(hullPoints, BufferPool, out _); + //float largestError = 0; + //for (int i = 0; i < hullShape.FaceToVertexIndicesStart.Length; ++i) + //{ + // hullShape.GetVertexIndicesForFace(i, out var faceVertices); + // BundleIndexing.GetBundleIndices(i, out var normalBundleIndex, out var normalIndexInBundle); + // Vector3Wide.ReadSlot(ref hullShape.BoundingPlanes[normalBundleIndex].Normal, normalIndexInBundle, out var faceNormal); + // var offset = hullShape.BoundingPlanes[normalBundleIndex].Offset[normalIndexInBundle]; + // Console.WriteLine($"Face {i} errors:"); + // for (int j = 0; j < faceVertices.Length; ++j) + // { + // hullShape.GetPoint(faceVertices[j], out var point); + // var error = Vector3.Dot(point, faceNormal) - offset; + // Console.WriteLine($"v{j}: {error}"); + // largestError = MathF.Max(MathF.Abs(error), largestError); + // } + //} + //Console.WriteLine($"Largest error: {largestError}"); + + //ConvexHullHelper.ComputeHull(hullPoints, BufferPool, out var hullData, out debugSteps); + //this.points = hullPoints; + + var boxHullPoints = CreateBoxConvexHull(2); + var boxHullShape = new ConvexHull(boxHullPoints, BufferPool, out _); + + Matrix3x3.CreateScale(new Vector3(5, 0.5f, 3), out var scale); + var transform = Matrix3x3.CreateFromAxisAngle(Vector3.Normalize(new Vector3(3, 2, 1)), 1207) * scale; + const int transformCount = 10000; + var transformStart = Stopwatch.GetTimestamp(); + for (int i = 0; i < transformCount; ++i) + { + CreateTransformedCopy(hullShape, transform, BufferPool, out var transformedHullShape); + transformedHullShape.Dispose(BufferPool); + } + var transformEnd = Stopwatch.GetTimestamp(); + Console.WriteLine($"Transform hull computation time (us): {(transformEnd - transformStart) * 1e6 / (transformCount * Stopwatch.Frequency)}"); + + hullShape.RayTest(RigidPose.Identity, new Vector3(0, 1, 0), -Vector3.UnitY, out var t, out var normal); + + const int rayIterationCount = 10000; + var rayPose = RigidPose.Identity; + var rayOrigin = new Vector3(0, 2, 0); + var rayDirection = new Vector3(0, -1, 0); + + int hitCounter = 0; + var start = Stopwatch.GetTimestamp(); + for (int i = 0; i < rayIterationCount; ++i) + { + if (hullShape.RayTest(rayPose, rayOrigin, rayDirection, out _, out _)) + { + ++hitCounter; + } + } + var end = Stopwatch.GetTimestamp(); + Console.WriteLine($"Hit counter: {hitCounter}, computation time (us): {(end - start) * 1e6 / (rayIterationCount * Stopwatch.Frequency)}"); + + const int iterationCount = 100; + start = Stopwatch.GetTimestamp(); + for (int i = 0; i < iterationCount; ++i) + { + CreateShape(hullPoints, BufferPool, out _, out var perfTestShape); + perfTestShape.Dispose(BufferPool); + } + end = Stopwatch.GetTimestamp(); + Console.WriteLine($"Hull computation time (us): {(end - start) * 1e6 / (iterationCount * Stopwatch.Frequency)}"); + + var hullShapeIndex = Simulation.Shapes.Add(hullShape); + var boxHullShapeIndex = Simulation.Shapes.Add(boxHullShape); + var inertia = hullShape.ComputeInertia(1); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 0, 5), inertia, new(hullShapeIndex, 20, 20), -0.01f)); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 0, 3), boxHullShape.ComputeInertia(1), new(boxHullShapeIndex, 20, 20), -0.01f)); + + Simulation.Statics.Add(new StaticDescription(new Vector3(-25, -5, 0), Simulation.Shapes.Add(new Sphere(2)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(-20, -5, 0), Simulation.Shapes.Add(new Capsule(0.5f, 2)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(-15, -5, 0), Simulation.Shapes.Add(new Box(2f, 2f, 2f)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(-10, -5, 5), Simulation.Shapes.Add(new Triangle { A = new Vector3(0, 0, -10), B = new Vector3(5, 0, -10), C = new Vector3(0, 0, -5) }))); + Simulation.Statics.Add(new StaticDescription(new Vector3(-5, -5, 0), Simulation.Shapes.Add(new Cylinder(1, 1)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(-5, -5, 5), Simulation.Shapes.Add(new Cylinder(1, 1)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5, 0), hullShapeIndex)); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5, 5), Simulation.Shapes.Add(boxHullShape))); + + var spacing = new Vector3(3f, 3f, 3); + int width = 16; + int height = 16; + int length = 0; + var origin = -0.5f * spacing * new Vector3(width, 0, length) + new Vector3(40, 0.2f, -40); + for (int i = 0; i < width; ++i) + { + for (int j = 0; j < height; ++j) + { + for (int k = 0; k < length; ++k) + { + Simulation.Bodies.Add(BodyDescription.CreateDynamic( + (origin + spacing * new Vector3(i, j, k), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathHelper.Pi * 0.05f)), + inertia, hullShapeIndex, 0.01f)); + } + } + } + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -10, 0), Simulation.Shapes.Add(new Box(1000, 1, 1000)))); + + Random random = new Random(5); + var mesh = DemoMeshHelper.CreateDeformedPlane(64, 64, (x, y) => new Vector3( + x + 8, + 2f * MathF.Sin(x * 0.125f) * MathF.Sin(y * 0.125f) + 0.1f * random.NextSingle() - 3, + y - 8), new Vector3(1, 1, 1), BufferPool); + Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); + } + + void TestConvexHullCreation() + { + var random = new Random(5); + for (int iterationIndex = 0; iterationIndex < 100000; ++iterationIndex) + { + const int pointCount = 32; + var points = new QuickList(pointCount, BufferPool); + for (int i = 0; i < pointCount; ++i) + { + points.AllocateUnsafely() = new Vector3(1 * random.NextSingle(), 2 * random.NextSingle(), 3 * random.NextSingle()); + } + + var pointsBuffer = points.Span.Slice(points.Count); + CreateShape(pointsBuffer, BufferPool, out _, out var hullShape); + + hullShape.Dispose(BufferPool); + } + } + + //Buffer points; + //List debugSteps; + + //int stepIndex = 0; + + //public override void Update(Window window, Camera camera, Input input, float dt) + //{ + // if (input.TypedCharacters.Contains('x')) + // { + // stepIndex = Math.Max(stepIndex - 1, 0); + // } + // if (input.TypedCharacters.Contains('c')) + // { + // stepIndex = Math.Min(stepIndex + 1, debugSteps.Count - 1); + // } + // if (input.WasPushed(OpenTK.Input.Key.P)) + // { + // showWireframe = !showWireframe; + // } + // if (input.WasPushed(OpenTK.Input.Key.U)) + // { + // showDeleted = !showDeleted; + // } + // if (input.WasPushed(OpenTK.Input.Key.Y)) + // { + // showVertexIndices = !showVertexIndices; + // } + // if (input.WasPushed(OpenTK.Input.Key.H)) + // { + // showFaceVertexStatuses = !showFaceVertexStatuses; + // } + // base.Update(window, camera, input, dt); + //} + + //bool showWireframe; + //bool showDeleted; + //bool showVertexIndices; + //bool showFaceVertexStatuses = true; + //public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + //{ + // var step = debugSteps[stepIndex]; + // var scale = 5f; + // var renderOffset = new Vector3(-15, 25, 0); + // for (int i = 0; i < points.Length; ++i) + // { + // var pose = new RigidPose(renderOffset + points[i] * scale); + // renderer.Shapes.AddShape(new Box(0.1f, 0.1f, 0.1f), Simulation.Shapes, pose, new Vector3(0.5f, 0.5f, 0.5f)); + // if (!step.AllowVertex[i] && showFaceVertexStatuses) + // renderer.Shapes.AddShape(new Box(0.6f, 0.25f, 0.25f), Simulation.Shapes, pose, new Vector3(1, 0, 0)); + // } + // if (showFaceVertexStatuses) + // { + // for (int i = 0; i < step.Raw.Count; ++i) + // { + // var pose = new RigidPose(renderOffset + points[step.Raw[i]] * scale); + // renderer.Shapes.AddShape(new Box(0.25f, 0.6f, 0.25f), Simulation.Shapes, pose, new Vector3(0, 0, 1)); + // } + // for (int i = 0; i < step.Reduced.Count; ++i) + // { + // var pose = new RigidPose(renderOffset + points[step.Reduced[i]] * scale); + // renderer.Shapes.AddShape(new Box(0.25f, 0.25f, 0.6f), Simulation.Shapes, pose, new Vector3(0, 1, 0)); + // } + // } + + // { + // var pose = new RigidPose(renderOffset); + // for (int i = 0; i < step.FaceStarts.Count; ++i) + // { + // if (showDeleted || !step.FaceDeleted[i]) + // { + // var faceStart = step.FaceStarts[i]; + // var faceEnd = i + 1 < step.FaceStarts.Count ? step.FaceStarts[i + 1] : step.FaceIndices.Count; + // var count = faceEnd - faceStart; + // var color = step.FaceDeleted[i] ? new Vector3(0.25f, 0.25f, 0.25f) : step.FaceIndex == i ? new Vector3(1, 0, 0.5f) : new Vector3(1, 0, 1); + // var deletionInducedScale = step.FaceDeleted[i] ? new Vector3(1.1f) : new Vector3(1f); + + // var offset = step.FaceDeleted[i] ? step.FaceNormals[i] * 0.25f : new Vector3(); + // if (showWireframe) + // { + // var previousIndex = faceEnd - 1; + // for (int q = faceStart; q < faceEnd; ++q) + // { + // var a = points[step.FaceIndices[q]] * scale + pose.Position + offset; + // var b = points[step.FaceIndices[previousIndex]] * scale + pose.Position + offset; + // previousIndex = q; + // renderer.Lines.Allocate() = new LineInstance(a, b, color, Vector3.Zero); + // } + // } + // else + // { + // for (int k = faceStart + 2; k < faceEnd; ++k) + // { + // renderer.Shapes.AddShape(new Triangle + // { + // A = points[step.FaceIndices[faceStart]] * scale + offset, + // B = points[step.FaceIndices[k]] * scale + offset, + // C = points[step.FaceIndices[k - 1]] * scale + offset + // }, Simulation.Shapes, pose, color); + // } + // } + // } + // } + // } + + // if (showVertexIndices) + // { + // for (int i = 0; i < points.Length; ++i) + // { + // if (DemoRenderer.Helpers.GetScreenLocation(points[i] * scale + renderOffset, camera.ViewProjection, renderer.Surface.Resolution, out var location)) + // { + // renderer.TextBatcher.Write(text.Clear().Append(i), location, 10, new Vector3(1), font); + // } + // } + // } + + // var edgeMidpoint = renderOffset + (points[step.SourceEdge.A] + points[step.SourceEdge.B]) * scale * 0.5f; + // renderer.Lines.Allocate() = new LineInstance(edgeMidpoint, edgeMidpoint + step.BasisX * scale * 0.5f, new Vector3(1, 1, 0), new Vector3()); + // renderer.Lines.Allocate() = new LineInstance(edgeMidpoint, edgeMidpoint + step.BasisY * scale * 0.5f, new Vector3(0, 1, 0), new Vector3()); + // renderer.TextBatcher.Write( + // text.Clear().Append($"Enumerate step with X and C. Current step: ").Append(stepIndex + 1).Append(" out of ").Append(debugSteps.Count), + // new Vector2(32, renderer.Surface.Resolution.Y - 140), 20, new Vector3(1), font); + // renderer.TextBatcher.Write(text.Clear().Append("Show wireframe: P ").Append(showWireframe ? "(on)" : "(off)"), new Vector2(32, renderer.Surface.Resolution.Y - 120), 20, new Vector3(1), font); + // renderer.TextBatcher.Write(text.Clear().Append("Show deleted: U ").Append(showDeleted ? "(on)" : "(off)"), new Vector2(32, renderer.Surface.Resolution.Y - 100), 20, new Vector3(1), font); + // renderer.TextBatcher.Write(text.Clear().Append("Show vertex indices: Y ").Append(showVertexIndices ? "(on)" : "(off)"), new Vector2(32, renderer.Surface.Resolution.Y - 80), 20, new Vector3(1), font); + // renderer.TextBatcher.Write(text.Clear().Append("Show face vertex statuses: H ").Append(showFaceVertexStatuses ? "(on)" : "(off)"), new Vector2(32, renderer.Surface.Resolution.Y - 60), 20, new Vector3(1), font); + + + // base.Render(renderer, camera, input, text, font); + //} +} From 0ecb874632905da8ffd474ebf1564eed273a25fc Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 14 Dec 2023 17:33:00 -0600 Subject: [PATCH 843/947] Version bump. --- BepuPhysics/BepuPhysics.csproj | 2 +- BepuUtilities/BepuUtilities.csproj | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/BepuPhysics/BepuPhysics.csproj b/BepuPhysics/BepuPhysics.csproj index 0ee5eadfc..f83e1c225 100644 --- a/BepuPhysics/BepuPhysics.csproj +++ b/BepuPhysics/BepuPhysics.csproj @@ -1,7 +1,7 @@  net8.0 - 2.5.0-beta.16 + 2.5.0-beta.17 Bepu Entertainment LLC Ross Nordby Speedy real time physics simulation library. diff --git a/BepuUtilities/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj index 1b962b6c1..a8bc202d1 100644 --- a/BepuUtilities/BepuUtilities.csproj +++ b/BepuUtilities/BepuUtilities.csproj @@ -3,7 +3,7 @@ BepuUtilities BepuUtilities net8.0 - 2.5.0-beta.16 + 2.5.0-beta.17 Bepu Entertainment LLC Ross Nordby Supporting utilities library for BEPUphysics v2. From 5f61291bf5e38876bf69316f592dd5d82530e172 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 29 Dec 2023 11:05:48 -0600 Subject: [PATCH 844/947] Sponsor update! --- Demos/Demos/Sponsors/SponsorDemo.cs | 1 + 1 file changed, 1 insertion(+) diff --git a/Demos/Demos/Sponsors/SponsorDemo.cs b/Demos/Demos/Sponsors/SponsorDemo.cs index 3dbeb9de6..c4d61bf83 100644 --- a/Demos/Demos/Sponsors/SponsorDemo.cs +++ b/Demos/Demos/Sponsors/SponsorDemo.cs @@ -63,6 +63,7 @@ public override void LoadGraphicalContent(ContentArchive content, RenderSurface sponsors0.Add("R. W."); sponsors0.Add("AshleighAdams"); sponsors0.Add("LoicBaumann"); + sponsors0.Add("Slayuh9"); //These supporters are those who gave 10 dollars a month (or historical backers of roughly equivalent or greater total contribution). //They get a poorly drawn animal! From c2429314eccf6538d47f152bed699d5100f40e51 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 12 Jan 2024 14:22:14 -0600 Subject: [PATCH 845/947] Solver-side integration should now properly filter body indices before passing them onto the velocity integration callback. --- BepuPhysics/Constraints/TypeProcessor.cs | 57 +++++++++++++++--------- 1 file changed, 37 insertions(+), 20 deletions(-) diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index 9790984a0..03c3fbd4f 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -1282,10 +1282,22 @@ public static void IntegrateVelocity + /// Takes body indices that could include metadata like kinematic flags in their upper bits and returns indices + /// with those flags stripped and with any lanes masked out by the integrationMask set to -1. + /// + /// Encoded body indices to decode. + /// Mask to apply to the body indices. + /// Body indices suitable for sending to to the IntegrateVelocity callback. + static Vector DecodeBodyIndices(Vector encodedBodyIndices, Vector integrationMask) + { + return (encodedBodyIndices & new Vector(Bodies.BodyReferenceMask)) | Vector.OnesComplement(integrationMask); + } + //[MethodImpl(MethodImplOptions.AggressiveInlining)] public static void GatherAndIntegrate( Bodies bodies, ref TIntegratorCallbacks integratorCallbacks, ref Buffer integrationFlags, int bodyIndexInConstraint, float dt, int workerIndex, int bundleIndex, - ref Vector bodyIndices, out Vector3Wide position, out QuaternionWide orientation, out BodyVelocityWide velocity, out BodyInertiaWide inertia) + ref Vector encodedBodyIndices, out Vector3Wide position, out QuaternionWide orientation, out BodyVelocityWide velocity, out BodyInertiaWide inertia) where TIntegratorCallbacks : struct, IPoseIntegratorCallbacks where TBatchIntegrationMode : unmanaged, IBatchIntegrationMode where TAccessFilter : unmanaged, IBodyAccessFilter @@ -1297,15 +1309,16 @@ public static void GatherAndIntegrate(Bodies.DynamicLimit))); - bodies.GatherState(bodyIndices, false, out position, out orientation, out velocity, out var localInertia); - IntegratePoseAndVelocity(ref integratorCallbacks, ref bodyIndices, localInertia, dt, integrationMask, ref position, ref orientation, ref velocity, workerIndex, out inertia); - bodies.ScatterPose(ref position, ref orientation, bodyIndices, integrationMask); - bodies.ScatterInertia(ref inertia, bodyIndices, integrationMask); + var integrationMask = Vector.AsVectorInt32(Vector.LessThan(Vector.AsVectorUInt32(encodedBodyIndices), new Vector(Bodies.DynamicLimit))); + bodies.GatherState(encodedBodyIndices, false, out position, out orientation, out velocity, out var localInertia); + var decodedBodyIndices = DecodeBodyIndices(encodedBodyIndices, integrationMask); + IntegratePoseAndVelocity(ref integratorCallbacks, ref decodedBodyIndices, localInertia, dt, integrationMask, ref position, ref orientation, ref velocity, workerIndex, out inertia); + bodies.ScatterPose(ref position, ref orientation, encodedBodyIndices, integrationMask); + bodies.ScatterInertia(ref inertia, encodedBodyIndices, integrationMask); } else if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) { - bodies.GatherState(bodyIndices, true, out position, out orientation, out velocity, out inertia); + bodies.GatherState(encodedBodyIndices, true, out position, out orientation, out velocity, out inertia); } else { @@ -1317,15 +1330,16 @@ public static void GatherAndIntegrate(bodyIndices, bundleIntegrationMode == BundleIntegrationMode.None, out position, out orientation, out velocity, out var gatheredInertia); + bodies.GatherState(encodedBodyIndices, bundleIntegrationMode == BundleIntegrationMode.None, out position, out orientation, out velocity, out var gatheredInertia); if (bundleIntegrationMode != BundleIntegrationMode.None) { //Note that if we take this codepath, the integration routine will reconstruct the world inertias from local inertia given the current pose. //The changes to pose and velocity for integration inactive lanes will be masked out, so it'll just be identical to the world inertia if we had gathered it. //Given that we're running the instructions in a bundle to build it, there's no reason to go out of our way to gather the world inertia. - IntegratePoseAndVelocity(ref integratorCallbacks, ref bodyIndices, gatheredInertia, dt, integrationMask, ref position, ref orientation, ref velocity, workerIndex, out inertia); - bodies.ScatterPose(ref position, ref orientation, bodyIndices, integrationMask); - bodies.ScatterInertia(ref inertia, bodyIndices, integrationMask); + var decodedBodyIndices = DecodeBodyIndices(encodedBodyIndices, integrationMask); + IntegratePoseAndVelocity(ref integratorCallbacks, ref decodedBodyIndices, gatheredInertia, dt, integrationMask, ref position, ref orientation, ref velocity, workerIndex, out inertia); + bodies.ScatterPose(ref position, ref orientation, encodedBodyIndices, integrationMask); + bodies.ScatterInertia(ref inertia, encodedBodyIndices, integrationMask); } else { @@ -1346,24 +1360,27 @@ public static void GatherAndIntegrate(Bodies.DynamicLimit))); - bodies.GatherState(bodyIndices, false, out position, out orientation, out velocity, out var localInertia); - IntegrateVelocity(ref integratorCallbacks, ref bodyIndices, localInertia, dt, integrationMask, position, orientation, ref velocity, workerIndex, out inertia); - bodies.ScatterInertia(ref inertia, bodyIndices, integrationMask); + var integrationMask = Vector.AsVectorInt32(Vector.LessThan(Vector.AsVectorUInt32(encodedBodyIndices), new Vector(Bodies.DynamicLimit))); + bodies.GatherState(encodedBodyIndices, false, out position, out orientation, out velocity, out var localInertia); + var decodedBodyIndices = DecodeBodyIndices(encodedBodyIndices, integrationMask); + IntegrateVelocity(ref integratorCallbacks, ref decodedBodyIndices, localInertia, dt, integrationMask, position, orientation, ref velocity, workerIndex, out inertia); + bodies.ScatterInertia(ref inertia, encodedBodyIndices, integrationMask); + } else if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) { - bodies.GatherState(bodyIndices, true, out position, out orientation, out velocity, out inertia); + bodies.GatherState(encodedBodyIndices, true, out position, out orientation, out velocity, out inertia); } else { Debug.Assert(typeof(TBatchIntegrationMode) == typeof(BatchShouldConditionallyIntegrate)); var bundleIntegrationMode = BundleShouldIntegrate(bundleIndex, integrationFlags[bodyIndexInConstraint], out var integrationMask); - bodies.GatherState(bodyIndices, bundleIntegrationMode == BundleIntegrationMode.None, out position, out orientation, out velocity, out var gatheredInertia); + bodies.GatherState(encodedBodyIndices, bundleIntegrationMode == BundleIntegrationMode.None, out position, out orientation, out velocity, out var gatheredInertia); if (bundleIntegrationMode != BundleIntegrationMode.None) - { - IntegrateVelocity(ref integratorCallbacks, ref bodyIndices, gatheredInertia, dt, integrationMask, position, orientation, ref velocity, workerIndex, out inertia); - bodies.ScatterInertia(ref inertia, bodyIndices, integrationMask); + { + var decodedBodyIndices = DecodeBodyIndices(encodedBodyIndices, integrationMask); + IntegrateVelocity(ref integratorCallbacks, ref decodedBodyIndices, gatheredInertia, dt, integrationMask, position, orientation, ref velocity, workerIndex, out inertia); + bodies.ScatterInertia(ref inertia, encodedBodyIndices, integrationMask); } else { From 563773c8a7f0dc695358b4e131cf7abdc6ac49f3 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 12 Jan 2024 14:22:38 -0600 Subject: [PATCH 846/947] Version bump. --- BepuPhysics/BepuPhysics.csproj | 2 +- BepuUtilities/BepuUtilities.csproj | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/BepuPhysics/BepuPhysics.csproj b/BepuPhysics/BepuPhysics.csproj index f83e1c225..72e9aed43 100644 --- a/BepuPhysics/BepuPhysics.csproj +++ b/BepuPhysics/BepuPhysics.csproj @@ -1,7 +1,7 @@  net8.0 - 2.5.0-beta.17 + 2.5.0-beta.18 Bepu Entertainment LLC Ross Nordby Speedy real time physics simulation library. diff --git a/BepuUtilities/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj index a8bc202d1..3c0744111 100644 --- a/BepuUtilities/BepuUtilities.csproj +++ b/BepuUtilities/BepuUtilities.csproj @@ -3,7 +3,7 @@ BepuUtilities BepuUtilities net8.0 - 2.5.0-beta.17 + 2.5.0-beta.18 Bepu Entertainment LLC Ross Nordby Supporting utilities library for BEPUphysics v2. From fe8d87ef9521f75c958ff7b7d18f447cf2f215d2 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 13 Jan 2024 15:34:37 -0600 Subject: [PATCH 847/947] Guarded against stackalloc-induced overflow for large compounds. --- .../CompoundPairOverlapFinder.cs | 19 ++++++++++++++++--- 1 file changed, 16 insertions(+), 3 deletions(-) diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlapFinder.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlapFinder.cs index c019cdb15..ddf7ef78d 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlapFinder.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlapFinder.cs @@ -26,7 +26,18 @@ public static unsafe void FindLocalOverlaps(ref Buffer pairs, } overlaps = new CompoundPairOverlaps(pool, pairCount, totalCompoundChildCount); ref var pairsToTest = ref overlaps.pairQueries; - var subpairData = stackalloc SubpairData[totalCompoundChildCount]; + //Stack overflows are very possible with larger compounds! Guard against it. + Buffer subpairData; + const int stackallocThreshold = 1024; + if (totalCompoundChildCount <= stackallocThreshold) + { + var memory = stackalloc SubpairData[totalCompoundChildCount]; + subpairData = new Buffer(memory, totalCompoundChildCount); + } + else + { + subpairData = new Buffer(totalCompoundChildCount, pool); + } int nextSubpairIndex = 0; for (int i = 0; i < pairCount; ++i) { @@ -112,8 +123,10 @@ out GatherScatter.Get(ref maximumRadius, j), } //Doesn't matter what mesh/compound instance is used for the function; just using it as a source of the function. Debug.Assert(totalCompoundChildCount > 0); - Unsafe.AsRef(pairsToTest[0].Container).FindLocalOverlaps(ref pairsToTest, pool, shapes, ref overlaps); - + Unsafe.AsRef(pairsToTest[0].Container).FindLocalOverlaps(ref pairsToTest, pool, shapes, ref overlaps); + + if (subpairData.Length > stackallocThreshold) + subpairData.Dispose(pool); } } From 1fdf7349f28c371da8a52d0b7b1ae880cec3b8b4 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 13 Jan 2024 15:35:24 -0600 Subject: [PATCH 848/947] Version bump. --- BepuPhysics/BepuPhysics.csproj | 2 +- BepuUtilities/BepuUtilities.csproj | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/BepuPhysics/BepuPhysics.csproj b/BepuPhysics/BepuPhysics.csproj index 72e9aed43..7fd4b4fd4 100644 --- a/BepuPhysics/BepuPhysics.csproj +++ b/BepuPhysics/BepuPhysics.csproj @@ -1,7 +1,7 @@  net8.0 - 2.5.0-beta.18 + 2.5.0-beta.19 Bepu Entertainment LLC Ross Nordby Speedy real time physics simulation library. diff --git a/BepuUtilities/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj index 3c0744111..8362e6f51 100644 --- a/BepuUtilities/BepuUtilities.csproj +++ b/BepuUtilities/BepuUtilities.csproj @@ -3,7 +3,7 @@ BepuUtilities BepuUtilities net8.0 - 2.5.0-beta.18 + 2.5.0-beta.19 Bepu Entertainment LLC Ross Nordby Supporting utilities library for BEPUphysics v2. From 5002b7dea00254005bcfbd615c7adcd0be9a82b9 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 5 Feb 2024 15:13:07 -0600 Subject: [PATCH 849/947] Disabled debug steps in convex hull helper, oops. --- BepuPhysics/Collidables/ConvexHullHelper.cs | 142 ++++++++++---------- 1 file changed, 71 insertions(+), 71 deletions(-) diff --git a/BepuPhysics/Collidables/ConvexHullHelper.cs b/BepuPhysics/Collidables/ConvexHullHelper.cs index 53076ef87..a186a08f1 100644 --- a/BepuPhysics/Collidables/ConvexHullHelper.cs +++ b/BepuPhysics/Collidables/ConvexHullHelper.cs @@ -592,70 +592,70 @@ static void AddIfNotPresent(ref QuickList list, int value, BufferPool pool) list.Allocate(pool) = value; } - public struct DebugStep - { - public EdgeEndpoints SourceEdge; - public List Raw; - public List Reduced; - public bool[] AllowVertex; - public Vector3 FaceNormal; - public Vector3 BasisX; - public Vector3 BasisY; - public List FaceStarts; - public List FaceIndices; - public bool[] FaceDeleted; - public int[] MergedFaceIndices; - public int FaceIndex; - public Vector3[] FaceNormals; - - internal DebugStep(EdgeEndpoints sourceEdge, ref QuickList raw, Vector3 faceNormal, Vector3 basisX, Vector3 basisY, ref QuickList reduced, ref Buffer allowVertex, ref QuickList faces, Span mergedFaceIndices, int faceIndex) - { - SourceEdge = sourceEdge; - FaceNormal = faceNormal; - BasisX = basisX; - BasisY = basisY; - Raw = new List(); - for (int i = 0; i < raw.Count; ++i) - { - Raw.Add(raw[i]); - } - Reduced = new List(); - for (int i = 0; i < reduced.Count; ++i) - { - Reduced.Add(reduced[i]); - } - AllowVertex = new bool[allowVertex.Length]; - for (int i = 0; i < allowVertex.Length; ++i) - { - AllowVertex[i] = allowVertex[i] != 0; - } - FaceStarts = new List(faces.Count); - FaceIndices = new List(); - FaceDeleted = new bool[faces.Count]; - FaceNormals = new Vector3[faces.Count]; - for (int i = 0; i < faces.Count; ++i) - { - ref var face = ref faces[i]; - FaceStarts.Add(FaceIndices.Count); - for (int j = 0; j < face.VertexIndices.Count; ++j) - FaceIndices.Add(face.VertexIndices[j]); - FaceDeleted[i] = face.Deleted; - FaceNormals[i] = face.Normal; - } - MergedFaceIndices = mergedFaceIndices.ToArray(); - FaceIndex = faceIndex; - } - } - /// - /// Computes the convex hull of a set of points. - /// - /// Point set to compute the convex hull of. - /// Buffer pool to pull memory from when creating the hull. - /// Convex hull of the input point set. - public static void ComputeHull(Span points, BufferPool pool, out HullData hullData) - { - ComputeHull(points, pool, out hullData, out _); - } + //public struct DebugStep + //{ + // public EdgeEndpoints SourceEdge; + // public List Raw; + // public List Reduced; + // public bool[] AllowVertex; + // public Vector3 FaceNormal; + // public Vector3 BasisX; + // public Vector3 BasisY; + // public List FaceStarts; + // public List FaceIndices; + // public bool[] FaceDeleted; + // public int[] MergedFaceIndices; + // public int FaceIndex; + // public Vector3[] FaceNormals; + + // internal DebugStep(EdgeEndpoints sourceEdge, ref QuickList raw, Vector3 faceNormal, Vector3 basisX, Vector3 basisY, ref QuickList reduced, ref Buffer allowVertex, ref QuickList faces, Span mergedFaceIndices, int faceIndex) + // { + // SourceEdge = sourceEdge; + // FaceNormal = faceNormal; + // BasisX = basisX; + // BasisY = basisY; + // Raw = new List(); + // for (int i = 0; i < raw.Count; ++i) + // { + // Raw.Add(raw[i]); + // } + // Reduced = new List(); + // for (int i = 0; i < reduced.Count; ++i) + // { + // Reduced.Add(reduced[i]); + // } + // AllowVertex = new bool[allowVertex.Length]; + // for (int i = 0; i < allowVertex.Length; ++i) + // { + // AllowVertex[i] = allowVertex[i] != 0; + // } + // FaceStarts = new List(faces.Count); + // FaceIndices = new List(); + // FaceDeleted = new bool[faces.Count]; + // FaceNormals = new Vector3[faces.Count]; + // for (int i = 0; i < faces.Count; ++i) + // { + // ref var face = ref faces[i]; + // FaceStarts.Add(FaceIndices.Count); + // for (int j = 0; j < face.VertexIndices.Count; ++j) + // FaceIndices.Add(face.VertexIndices[j]); + // FaceDeleted[i] = face.Deleted; + // FaceNormals[i] = face.Normal; + // } + // MergedFaceIndices = mergedFaceIndices.ToArray(); + // FaceIndex = faceIndex; + // } + //} + ///// + ///// Computes the convex hull of a set of points. + ///// + ///// Point set to compute the convex hull of. + ///// Buffer pool to pull memory from when creating the hull. + ///// Convex hull of the input point set. + //public static void ComputeHull(Span points, BufferPool pool, out HullData hullData) + //{ + // ComputeHull(points, pool, out hullData, out _); + //} /// @@ -664,9 +664,9 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa /// Point set to compute the convex hull of. /// Buffer pool to pull memory from when creating the hull. /// Convex hull of the input point set. - public static void ComputeHull(Span points, BufferPool pool, out HullData hullData, out List steps) + public static void ComputeHull(Span points, BufferPool pool, out HullData hullData)//, out List steps) { - steps = new List(); + //steps = new List(); if (points.Length <= 0) { hullData = default; @@ -823,9 +823,9 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa if (Vector3.Dot(basisX, edgeToAdd.FaceNormal) > 0) Helpers.Swap(ref edgeToAdd.Endpoints.A, ref edgeToAdd.Endpoints.B); } - Vector3Wide.ReadFirst(initialBasisX, out var debugInitialBasisX); - Vector3Wide.ReadFirst(initialBasisY, out var debugInitialBasisY); - steps.Add(new DebugStep(initialSourceEdge, ref rawFaceVertexIndices, initialFaceNormal, debugInitialBasisX, debugInitialBasisY, ref reducedFaceIndices, ref allowVertices, ref faces, default, reducedFaceIndices.Count >= 3 ? 0 : -1)); + //Vector3Wide.ReadFirst(initialBasisX, out var debugInitialBasisX); + //Vector3Wide.ReadFirst(initialBasisY, out var debugInitialBasisY); + //steps.Add(new DebugStep(initialSourceEdge, ref rawFaceVertexIndices, initialFaceNormal, debugInitialBasisX, debugInitialBasisY, ref reducedFaceIndices, ref allowVertices, ref faces, default, reducedFaceIndices.Count >= 3 ? 0 : -1)); int facesDeletedCount = 0; @@ -858,7 +858,7 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa if (reducedFaceIndices.Count < 3) { - steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertices, ref faces, default, -1)); + //steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertices, ref faces, default, -1)); //Degenerate face found; don't bother creating work for it. continue; } @@ -984,7 +984,7 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa AddFace(ref faces, pool, faceNormal, reducedFaceIndices); AddFaceToEdgesAndTestList(pool, ref reducedFaceIndices, ref edgesToTest, ref facesForEdges, faceNormal, faceCountPriorToAdd); - steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertices, ref faces, facesNeedingMerge, faceCountPriorToAdd)); + //steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertices, ref faces, facesNeedingMerge, faceCountPriorToAdd)); break; } } From e39d488ee5596e3e0f3f88fa2c44596184efdb5d Mon Sep 17 00:00:00 2001 From: Noah Stolk <31079637+NoahStolk@users.noreply.github.com> Date: Sun, 4 Feb 2024 10:50:03 +0100 Subject: [PATCH 850/947] Update GL projects to .NET 8 --- .gitignore | 3 +++ DemoRenderer.GL/DemoRenderer.csproj | 2 +- Demos.GL/Demos.csproj | 2 +- 3 files changed, 5 insertions(+), 2 deletions(-) diff --git a/.gitignore b/.gitignore index 1710e230b..5b073b60c 100644 --- a/.gitignore +++ b/.gitignore @@ -205,3 +205,6 @@ ModelManifest.xml # Don't ignore content files that happen to use the .obj extension. !Demos/Content/*.obj + +# JetBrains Rider +.idea/ diff --git a/DemoRenderer.GL/DemoRenderer.csproj b/DemoRenderer.GL/DemoRenderer.csproj index 74a20afbf..658c2ffd1 100644 --- a/DemoRenderer.GL/DemoRenderer.csproj +++ b/DemoRenderer.GL/DemoRenderer.csproj @@ -1,6 +1,6 @@  - net7.0 + net8.0 latest True diff --git a/Demos.GL/Demos.csproj b/Demos.GL/Demos.csproj index 75af1539d..c796aad45 100644 --- a/Demos.GL/Demos.csproj +++ b/Demos.GL/Demos.csproj @@ -1,7 +1,7 @@  Exe - net7.0 + net8.0 True Debug;Release latest From dd38847840b6a4bbe402c5c942c6d594c08d0e0f Mon Sep 17 00:00:00 2001 From: Noah Stolk <31079637+NoahStolk@users.noreply.github.com> Date: Sun, 4 Feb 2024 11:27:41 +0100 Subject: [PATCH 851/947] Fix some typos in the character demo too --- Demos/Demos/Characters/CharacterControllers.cs | 4 ++-- Demos/Demos/Characters/CharacterDemo.cs | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/Demos/Demos/Characters/CharacterControllers.cs b/Demos/Demos/Characters/CharacterControllers.cs index 05eab0f2b..c08a7dd87 100644 --- a/Demos/Demos/Characters/CharacterControllers.cs +++ b/Demos/Demos/Characters/CharacterControllers.cs @@ -85,7 +85,7 @@ public struct CharacterController public unsafe class CharacterControllers : IDisposable { /// - /// Gets the simulation to which this set of chracters belongs. + /// Gets the simulation to which this set of characters belongs. /// public Simulation Simulation { get; private set; } BufferPool pool; @@ -99,7 +99,7 @@ public unsafe class CharacterControllers : IDisposable public int CharacterCount { get { return characters.Count; } } /// - /// Creates a character controller systme. + /// Creates a character controller system. /// /// Pool to allocate resources from. /// Number of characters to initially allocate space for. diff --git a/Demos/Demos/Characters/CharacterDemo.cs b/Demos/Demos/Characters/CharacterDemo.cs index b5729d38e..97340d02a 100644 --- a/Demos/Demos/Characters/CharacterDemo.cs +++ b/Demos/Demos/Characters/CharacterDemo.cs @@ -135,10 +135,10 @@ public override void Initialize(ContentArchive content, Camera camera) pose.Orientation = Quaternion.Identity; return pose; }; - var platormShapeIndex = Simulation.Shapes.Add(new Box(5, 1, 5)); + var platformShapeIndex = Simulation.Shapes.Add(new Box(5, 1, 5)); for (int i = 0; i < movingPlatforms.Length; ++i) { - movingPlatforms[i] = new MovingPlatform(platormShapeIndex, i * 3559, 1f / 60f, Simulation, poseCreator); + movingPlatforms[i] = new MovingPlatform(platformShapeIndex, i * 3559, 1f / 60f, Simulation, poseCreator); } var box = new Box(4, 1, 4); var boxShapeIndex = Simulation.Shapes.Add(box); From a763813ab125d5ee2bc7dbe6675d35fd62990356 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 5 Feb 2024 17:07:46 -0600 Subject: [PATCH 852/947] Version bump. --- BepuPhysics/BepuPhysics.csproj | 2 +- BepuUtilities/BepuUtilities.csproj | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/BepuPhysics/BepuPhysics.csproj b/BepuPhysics/BepuPhysics.csproj index 7fd4b4fd4..9d17dc7e1 100644 --- a/BepuPhysics/BepuPhysics.csproj +++ b/BepuPhysics/BepuPhysics.csproj @@ -1,7 +1,7 @@  net8.0 - 2.5.0-beta.19 + 2.5.0-beta.20 Bepu Entertainment LLC Ross Nordby Speedy real time physics simulation library. diff --git a/BepuUtilities/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj index 8362e6f51..b985e1af8 100644 --- a/BepuUtilities/BepuUtilities.csproj +++ b/BepuUtilities/BepuUtilities.csproj @@ -3,7 +3,7 @@ BepuUtilities BepuUtilities net8.0 - 2.5.0-beta.19 + 2.5.0-beta.20 Bepu Entertainment LLC Ross Nordby Supporting utilities library for BEPUphysics v2. From 4956d407cd839a7fadb442867dc30e00f02b0e8e Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 14 Feb 2024 15:12:48 -0600 Subject: [PATCH 853/947] No documented transfer of ownership of stream in ContentArchive.Save, so avoiding internal closure. --- DemoContentLoader/ContentArchive.cs | 284 ++++++++++++++-------------- 1 file changed, 142 insertions(+), 142 deletions(-) diff --git a/DemoContentLoader/ContentArchive.cs b/DemoContentLoader/ContentArchive.cs index 64a69cb99..ddd6a3fae 100644 --- a/DemoContentLoader/ContentArchive.cs +++ b/DemoContentLoader/ContentArchive.cs @@ -1,142 +1,142 @@ -using System; -using System.Collections.Generic; -using System.IO; -using System.Text; - -namespace DemoContentLoader -{ - public enum ContentType - { - Font = 1, - Mesh = 2, - Image = 3, - GLSL = 4, - } - public interface IContent - { - ContentType ContentType { get; } - } - - public class ContentArchive - { - private Dictionary pathsToContent = new Dictionary(); - - public T Load(string path) - { - if (!pathsToContent.TryGetValue(path, out var untypedContent)) - { - throw new ArgumentException($"{path} not found in the content archive."); - } - if (untypedContent is T content) - { - return content; - } - else - { - throw new ArgumentException($"Content associated with {path} does not match the given type {typeof(T).Name}."); - } - } - - //We have a very limited set of content types. This isn't a general purpose engine. Rather than having a dictionary of type->loader or something, we can do a quick hack. - public static IContent Load(ContentType type, BinaryReader reader) - { - switch (type) - { - case ContentType.Font: - return FontIO.Load(reader); - case ContentType.Mesh: - return MeshIO.Load(reader); - case ContentType.Image: - return Texture2DIO.Load(reader); - case ContentType.GLSL: - return GLSLIO.Load(reader); - } - throw new ArgumentException($"Given content type {type} cannot be loaded; no loader is specified. Is the archive corrupted?"); - } - - public static void Save(IContent content, BinaryWriter writer) - { - switch (content.ContentType) - { - case ContentType.Font: - FontIO.Save((FontContent)content, writer); - return; - case ContentType.Mesh: - MeshIO.Save((MeshContent)content, writer); - return; - case ContentType.Image: - Texture2DIO.Save((Texture2DContent)content, writer); - return; - case ContentType.GLSL: - GLSLIO.Save((GLSLContent)content, writer); - return; - } - throw new ArgumentException("Given content type cannot be saved; no archiver is specified."); - } - - /// - /// Loads a content archive, previously saved using ContentArchive.Save, from a stream. - /// - /// Stream to load from. - /// Archive of loaded content. - public static ContentArchive Load(Stream stream) - { - //Read each piece of content in sequence. - //Format follows: - //Entry count - //[Entry 1] - //[Entry 2] - //... - //[Entry N] - - //where Entry: - //[pathLength : int32] - //[pathBytes : byte[]] - //[contentType : int32] - //[contentLengthInBytes : int32] - //[content : serializer specific] - - var archive = new ContentArchive(); - using (var reader = new BinaryReader(stream)) - { - var entryCount = reader.ReadInt32(); - for (int i = 0; i < entryCount; ++i) - { - var pathLengthInBytes = reader.ReadInt32(); - byte[] pathBytes = new byte[pathLengthInBytes]; - reader.Read(pathBytes, 0, pathLengthInBytes); - var path = Encoding.Unicode.GetString(pathBytes, 0, pathBytes.Length); - - var contentType = (ContentType)reader.ReadInt32(); - archive.pathsToContent.Add(path, Load(contentType, reader)); - } - } - return archive; - } - - /// - /// Saves out a set of path-content pairs in a format loadable as a ContentArchive. - /// - /// Path-content pairs to save. - /// Output stream to save to. - public static void Save(Dictionary pathsToContent, Stream stream) - { - using (var writer = new BinaryWriter(stream)) - { - writer.Write(pathsToContent.Count); - foreach (var pair in pathsToContent) - { - var path = pair.Key; - var content = pair.Value; - - var pathBytes = Encoding.Unicode.GetBytes(path); - writer.Write(pathBytes.Length); - writer.Write(pathBytes); - - writer.Write((int)content.ContentType); - Save(content, writer); - } - } - } - } -} +using System; +using System.Collections.Generic; +using System.IO; +using System.Text; + +namespace DemoContentLoader +{ + public enum ContentType + { + Font = 1, + Mesh = 2, + Image = 3, + GLSL = 4, + } + public interface IContent + { + ContentType ContentType { get; } + } + + public class ContentArchive + { + private Dictionary pathsToContent = new Dictionary(); + + public T Load(string path) + { + if (!pathsToContent.TryGetValue(path, out var untypedContent)) + { + throw new ArgumentException($"{path} not found in the content archive."); + } + if (untypedContent is T content) + { + return content; + } + else + { + throw new ArgumentException($"Content associated with {path} does not match the given type {typeof(T).Name}."); + } + } + + //We have a very limited set of content types. This isn't a general purpose engine. Rather than having a dictionary of type->loader or something, we can do a quick hack. + public static IContent Load(ContentType type, BinaryReader reader) + { + switch (type) + { + case ContentType.Font: + return FontIO.Load(reader); + case ContentType.Mesh: + return MeshIO.Load(reader); + case ContentType.Image: + return Texture2DIO.Load(reader); + case ContentType.GLSL: + return GLSLIO.Load(reader); + } + throw new ArgumentException($"Given content type {type} cannot be loaded; no loader is specified. Is the archive corrupted?"); + } + + public static void Save(IContent content, BinaryWriter writer) + { + switch (content.ContentType) + { + case ContentType.Font: + FontIO.Save((FontContent)content, writer); + return; + case ContentType.Mesh: + MeshIO.Save((MeshContent)content, writer); + return; + case ContentType.Image: + Texture2DIO.Save((Texture2DContent)content, writer); + return; + case ContentType.GLSL: + GLSLIO.Save((GLSLContent)content, writer); + return; + } + throw new ArgumentException("Given content type cannot be saved; no archiver is specified."); + } + + /// + /// Loads a content archive, previously saved using ContentArchive.Save, from a stream. + /// + /// Stream to load from. + /// Archive of loaded content. + public static ContentArchive Load(Stream stream) + { + //Read each piece of content in sequence. + //Format follows: + //Entry count + //[Entry 1] + //[Entry 2] + //... + //[Entry N] + + //where Entry: + //[pathLength : int32] + //[pathBytes : byte[]] + //[contentType : int32] + //[contentLengthInBytes : int32] + //[content : serializer specific] + + var archive = new ContentArchive(); + using (var reader = new BinaryReader(stream)) + { + var entryCount = reader.ReadInt32(); + for (int i = 0; i < entryCount; ++i) + { + var pathLengthInBytes = reader.ReadInt32(); + byte[] pathBytes = new byte[pathLengthInBytes]; + reader.Read(pathBytes, 0, pathLengthInBytes); + var path = Encoding.Unicode.GetString(pathBytes, 0, pathBytes.Length); + + var contentType = (ContentType)reader.ReadInt32(); + archive.pathsToContent.Add(path, Load(contentType, reader)); + } + } + return archive; + } + + /// + /// Saves out a set of path-content pairs in a format loadable as a ContentArchive. + /// + /// Path-content pairs to save. + /// Output stream to save to. + public static void Save(Dictionary pathsToContent, Stream stream) + { + using (var writer = new BinaryWriter(stream, Encoding.UTF8, true)) + { + writer.Write(pathsToContent.Count); + foreach (var pair in pathsToContent) + { + var path = pair.Key; + var content = pair.Value; + + var pathBytes = Encoding.Unicode.GetBytes(path); + writer.Write(pathBytes.Length); + writer.Write(pathBytes); + + writer.Write((int)content.ContentType); + Save(content, writer); + } + } + } + } +} From 6a48c1fbbb6be5e41baabb72cb04709fb95ffe08 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 4 Mar 2024 16:43:08 -0600 Subject: [PATCH 854/947] Fixed an issue with kinematic integration responsibilities; incorrect buffer was being cleared. --- BepuPhysics/Solver_Solve.cs | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/BepuPhysics/Solver_Solve.cs b/BepuPhysics/Solver_Solve.cs index 916684755..6036375a7 100644 --- a/BepuPhysics/Solver_Solve.cs +++ b/BepuPhysics/Solver_Solve.cs @@ -1130,7 +1130,8 @@ public override IndexSet PrepareConstraintIntegrationResponsibilities(IThreadDis pool.Take((bodies.HandlePool.HighestPossiblyClaimedId + 64) / 64, out mergedConstrainedBodyHandles.Flags); var copyLength = Math.Min(mergedConstrainedBodyHandles.Flags.Length, batchReferencedHandles[0].Flags.Length); batchReferencedHandles[0].Flags.CopyTo(0, mergedConstrainedBodyHandles.Flags, 0, copyLength); - batchReferencedHandles[0].Flags.Clear(copyLength, batchReferencedHandles[0].Flags.Length - copyLength); + mergedConstrainedBodyHandles.Flags.Clear(copyLength, mergedConstrainedBodyHandles.Flags.Length - copyLength); + //Yup, we're just leaving the first slot unallocated to avoid having to offset indices all over the place. Slight wonk, but not a big deal. bodiesFirstObservedInBatches[0] = default; From 03767bf827f191dac0876b85f10e7767e92aab25 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 4 Mar 2024 17:58:16 -0600 Subject: [PATCH 855/947] Moved batch merging to cross platform intrinsics. --- BepuPhysics/Solver_Solve.cs | 79 ++++++++++++++++++++----------------- 1 file changed, 43 insertions(+), 36 deletions(-) diff --git a/BepuPhysics/Solver_Solve.cs b/BepuPhysics/Solver_Solve.cs index 6036375a7..1542c9220 100644 --- a/BepuPhysics/Solver_Solve.cs +++ b/BepuPhysics/Solver_Solve.cs @@ -1153,52 +1153,59 @@ public override IndexSet PrepareConstraintIntegrationResponsibilities(IThreadDis ref var firstObservedInBatch = ref bodiesFirstObservedInBatches[batchIndex]; var flagBundleCount = Math.Min(mergedConstrainedBodyHandles.Flags.Length, batchHandles.Flags.Length); - if (Avx2.IsSupported) - { - var avxBundleCount = flagBundleCount / 4; - var horizontalAvxMerge = Vector256.Zero; - for (int avxBundleIndex = 0; avxBundleIndex < avxBundleCount; ++avxBundleIndex) + var scalarLoopStartIndex = 0; + ulong horizontalMerge = 0; + if (Vector512.IsHardwareAccelerated) + { + var bundleCount512 = flagBundleCount / 8; + var horizontal512Merge = Vector512.Zero; + for (int bundleIndex256 = 0; bundleIndex256 < bundleCount512; ++bundleIndex256) { //These will *almost* always be aligned, but guaranteeing it is not worth the complexity. - var mergeBundle = Avx2.LoadVector256((ulong*)((Vector256*)mergedConstrainedBodyHandles.Flags.Memory + avxBundleIndex)); - var batchBundle = Avx2.LoadVector256((ulong*)((Vector256*)batchHandles.Flags.Memory + avxBundleIndex)); - Avx.Store((ulong*)((Vector256*)mergedConstrainedBodyHandles.Flags.Memory + avxBundleIndex), Avx2.Or(mergeBundle, batchBundle)); + var mergeBundle = Vector512.Load((ulong*)((Vector512*)mergedConstrainedBodyHandles.Flags.Memory + bundleIndex256)); + var batchBundle = Vector512.Load((ulong*)((Vector512*)batchHandles.Flags.Memory + bundleIndex256)); + Vector512.BitwiseOr(mergeBundle, batchBundle).Store((ulong*)((Vector512*)mergedConstrainedBodyHandles.Flags.Memory + bundleIndex256)); //If this batch contains a body, and the merged set does not, then it's the first batch that sees a body and it will have integration responsibility. - var firstObservedBundle = Avx2.AndNot(mergeBundle, batchBundle); - horizontalAvxMerge = Avx2.Or(firstObservedBundle, horizontalAvxMerge); - Avx.Store((ulong*)((Vector256*)firstObservedInBatch.Flags.Memory + avxBundleIndex), firstObservedBundle); + var firstObservedBundle = Vector512.AndNot(batchBundle, mergeBundle); + horizontal512Merge = Vector512.BitwiseOr(firstObservedBundle, horizontal512Merge); + firstObservedBundle.Store((ulong*)((Vector512*)firstObservedInBatch.Flags.Memory + bundleIndex256)); } - var notEqual = Avx2.Xor(Avx2.CompareEqual(horizontalAvxMerge, Vector256.Zero), Vector256.AllBitsSet); - ulong horizontalMerge = (ulong)Avx2.MoveMask(notEqual.AsDouble()); + var notEqual = Vector512.Xor(Vector512.Equals(horizontal512Merge, Vector512.Zero), Vector512.AllBitsSet); + horizontalMerge = Vector512.ExtractMostSignificantBits(notEqual); - //Cleanup loop. - for (int flagBundleIndex = avxBundleCount * 4; flagBundleIndex < flagBundleCount; ++flagBundleIndex) - { - var mergeBundle = mergedConstrainedBodyHandles.Flags[flagBundleIndex]; - var batchBundle = batchHandles.Flags[flagBundleIndex]; - mergedConstrainedBodyHandles.Flags[flagBundleIndex] = mergeBundle | batchBundle; - //If this batch contains a body, and the merged set does not, then it's the first batch that sees a body and it will have integration responsibility. - var firstObservedBundle = ~mergeBundle & batchBundle; - horizontalMerge |= firstObservedBundle; - firstObservedInBatch.Flags[flagBundleIndex] = firstObservedBundle; - } - batchHasAnyIntegrationResponsibilities[batchIndex] = horizontalMerge != 0; + scalarLoopStartIndex = bundleCount512 * 8; } - else + else if (Vector256.IsHardwareAccelerated) { - ulong horizontalMerge = 0; - for (int flagBundleIndex = 0; flagBundleIndex < flagBundleCount; ++flagBundleIndex) + var bundleCount256 = flagBundleCount / 4; + var horizontal256Merge = Vector256.Zero; + for (int bundleIndex256 = 0; bundleIndex256 < bundleCount256; ++bundleIndex256) { - var mergeBundle = mergedConstrainedBodyHandles.Flags[flagBundleIndex]; - var batchBundle = batchHandles.Flags[flagBundleIndex]; - mergedConstrainedBodyHandles.Flags[flagBundleIndex] = mergeBundle | batchBundle; + //These will *almost* always be aligned, but guaranteeing it is not worth the complexity. + var mergeBundle = Vector256.Load((ulong*)((Vector256*)mergedConstrainedBodyHandles.Flags.Memory + bundleIndex256)); + var batchBundle = Vector256.Load((ulong*)((Vector256*)batchHandles.Flags.Memory + bundleIndex256)); + Vector256.BitwiseOr(mergeBundle, batchBundle).Store((ulong*)((Vector256*)mergedConstrainedBodyHandles.Flags.Memory + bundleIndex256)); //If this batch contains a body, and the merged set does not, then it's the first batch that sees a body and it will have integration responsibility. - var firstObservedBundle = ~mergeBundle & batchBundle; - horizontalMerge |= firstObservedBundle; - firstObservedInBatch.Flags[flagBundleIndex] = firstObservedBundle; + var firstObservedBundle = Vector256.AndNot(batchBundle, mergeBundle); + horizontal256Merge = Vector256.BitwiseOr(firstObservedBundle, horizontal256Merge); + firstObservedBundle.Store((ulong*)((Vector256*)firstObservedInBatch.Flags.Memory + bundleIndex256)); } - batchHasAnyIntegrationResponsibilities[batchIndex] = horizontalMerge != 0; - } + var notEqual = Vector256.Xor(Vector256.Equals(horizontal256Merge, Vector256.Zero), Vector256.AllBitsSet); + horizontalMerge = Vector256.ExtractMostSignificantBits(notEqual); + + scalarLoopStartIndex = bundleCount256 * 4; + } + for (int flagBundleIndex = scalarLoopStartIndex; flagBundleIndex < flagBundleCount; ++flagBundleIndex) + { + var mergeBundle = mergedConstrainedBodyHandles.Flags[flagBundleIndex]; + var batchBundle = batchHandles.Flags[flagBundleIndex]; + mergedConstrainedBodyHandles.Flags[flagBundleIndex] = mergeBundle | batchBundle; + //If this batch contains a body, and the merged set does not, then it's the first batch that sees a body and it will have integration responsibility. + var firstObservedBundle = ~mergeBundle & batchBundle; + horizontalMerge |= firstObservedBundle; + firstObservedInBatch.Flags[flagBundleIndex] = firstObservedBundle; + } + batchHasAnyIntegrationResponsibilities[batchIndex] = horizontalMerge != 0; } //var start = Stopwatch.GetTimestamp(); From 9a94101311a541bedb7a26ad93d6ef7b3610b3a3 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 4 Mar 2024 17:58:43 -0600 Subject: [PATCH 856/947] Version bump. --- BepuPhysics/BepuPhysics.csproj | 2 +- BepuUtilities/BepuUtilities.csproj | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/BepuPhysics/BepuPhysics.csproj b/BepuPhysics/BepuPhysics.csproj index 9d17dc7e1..8efd048c7 100644 --- a/BepuPhysics/BepuPhysics.csproj +++ b/BepuPhysics/BepuPhysics.csproj @@ -1,7 +1,7 @@  net8.0 - 2.5.0-beta.20 + 2.5.0-beta.21 Bepu Entertainment LLC Ross Nordby Speedy real time physics simulation library. diff --git a/BepuUtilities/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj index b985e1af8..423746c02 100644 --- a/BepuUtilities/BepuUtilities.csproj +++ b/BepuUtilities/BepuUtilities.csproj @@ -3,7 +3,7 @@ BepuUtilities BepuUtilities net8.0 - 2.5.0-beta.20 + 2.5.0-beta.21 Bepu Entertainment LLC Ross Nordby Supporting utilities library for BEPUphysics v2. From f5beda9fdc0e2f47ac6c46bb525d4b2a39991e15 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 5 Mar 2024 17:52:24 -0600 Subject: [PATCH 857/947] Bumped imagesharp version. --- DemoContentBuilder/DemoContentBuilder.csproj | 2 +- DemoContentBuilder/Texture2DBuilder.cs | 15 +++++++++------ 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/DemoContentBuilder/DemoContentBuilder.csproj b/DemoContentBuilder/DemoContentBuilder.csproj index 4897b5b9c..c72b01565 100644 --- a/DemoContentBuilder/DemoContentBuilder.csproj +++ b/DemoContentBuilder/DemoContentBuilder.csproj @@ -14,7 +14,7 @@ - + diff --git a/DemoContentBuilder/Texture2DBuilder.cs b/DemoContentBuilder/Texture2DBuilder.cs index a7226f4aa..4b200bdd8 100644 --- a/DemoContentBuilder/Texture2DBuilder.cs +++ b/DemoContentBuilder/Texture2DBuilder.cs @@ -17,12 +17,15 @@ public unsafe static Texture2DContent Build(Stream dataStream) var content = new Texture2DContent(image.Width, image.Height, 1, sizeof(Rgba32)); var data = (Rgba32*)content.Pin(); //Copy the image data into the Texture2DContent. - for (int rowIndex = 0; rowIndex < image.Height; ++rowIndex) - { - var sourceRow = image.GetPixelRowSpan(rowIndex); - var targetRow = data + content.GetRowOffsetForMip0(rowIndex); - Unsafe.CopyBlockUnaligned(ref *(byte*)targetRow, ref Unsafe.As(ref sourceRow[0]), (uint)(sizeof(Rgba32) * image.Width)); - } + image.ProcessPixelRows(accessor => + { + for (int rowIndex = 0; rowIndex < image.Height; ++rowIndex) + { + var sourceRow = accessor.GetRowSpan(rowIndex); + var targetRow = data + content.GetRowOffsetForMip0(rowIndex); + Unsafe.CopyBlockUnaligned(ref *(byte*)targetRow, ref Unsafe.As(ref sourceRow[0]), (uint)(sizeof(Rgba32) * image.Width)); + } + }); content.Unpin(); return content; } From 5b152bad94b46e2e4805d864ecf14efcce8f184f Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 7 Mar 2024 15:56:56 -0600 Subject: [PATCH 858/947] BigCompound now shifts node bounds correctly in response to center of mass movement. --- BepuPhysics/Collidables/BigCompound.cs | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/BepuPhysics/Collidables/BigCompound.cs b/BepuPhysics/Collidables/BigCompound.cs index cb9d74aac..0b4646512 100644 --- a/BepuPhysics/Collidables/BigCompound.cs +++ b/BepuPhysics/Collidables/BigCompound.cs @@ -327,7 +327,15 @@ public BodyInertia ComputeInertia(Span childMasses, Shapes shapes, out Ve { var bodyInertia = CompoundBuilder.ComputeInertia(Children, childMasses, shapes, out centerOfMass); //Recentering moves the children around, so the tree needs to be updated. - Tree.Refit(); + //Scanning through and explicitly shifting the nodes is slightly more efficient than updating leaf bounds and refitting. + for (int i = 0; i < Tree.NodeCount; ++i) + { + ref var node = ref Tree.Nodes[i]; + node.A.Min -= centerOfMass; + node.A.Max -= centerOfMass; + node.B.Min -= centerOfMass; + node.B.Max -= centerOfMass; + } return bodyInertia; } From 10bc4a24f34e86d6007a4c4d01fce287b0429e79 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 7 Mar 2024 15:59:16 -0600 Subject: [PATCH 859/947] Helper for grabbing leaf-associated NodeChild references. --- BepuPhysics/Trees/Tree_SelfQueries.cs | 25 +++++++++++++++++++------ 1 file changed, 19 insertions(+), 6 deletions(-) diff --git a/BepuPhysics/Trees/Tree_SelfQueries.cs b/BepuPhysics/Trees/Tree_SelfQueries.cs index 319ace33a..2028b045e 100644 --- a/BepuPhysics/Trees/Tree_SelfQueries.cs +++ b/BepuPhysics/Trees/Tree_SelfQueries.cs @@ -245,13 +245,26 @@ static Vector128 Encode(Vector128 indices) { return Vector128.AllBitsSet - indices; } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - static ref NodeChild GetLeafChild(ref Tree tree, uint encodedLeafParentIndex) + + /// + /// Gets a reference to the node child representing the leaf within the tree. + /// + /// Leaf to look up in the tree. + /// A reference to the node child within the tree. + public readonly ref NodeChild GetNodeChildForLeaf(Leaf leaf) + { + Debug.Assert(leaf.ChildIndex == 0 || leaf.ChildIndex == 1); + return ref Unsafe.Add(ref Nodes[leaf.NodeIndex].A, leaf.ChildIndex); + } + + /// + /// Gets a reference to the node child representing the leaf within the tree. + /// + /// Index of the leaf to look up in the tree. + /// A reference to the node child within the tree. + public readonly ref NodeChild GetNodeChildForLeaf(int leafIndex) { - var parentNodeIndex = encodedLeafParentIndex & 0x7FFF_FFFF; - var leafIsChildB = encodedLeafParentIndex > 0x7FFF_FFFF; - ref var parent = ref tree.Nodes[parentNodeIndex]; - return ref leafIsChildB ? ref parent.B : ref parent.A; + return ref GetNodeChildForLeaf(Leaves[leafIndex]); } //[MethodImpl(MethodImplOptions.AggressiveInlining)] From 0cb704f1644c0edc692aa19f981a1ef14440701e Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 7 Mar 2024 16:01:59 -0600 Subject: [PATCH 860/947] Added convience helper for directly using BodyInertia in compound builder add. --- BepuPhysics/Collidables/CompoundBuilder.cs | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/BepuPhysics/Collidables/CompoundBuilder.cs b/BepuPhysics/Collidables/CompoundBuilder.cs index 1e96c3885..ad0268c7d 100644 --- a/BepuPhysics/Collidables/CompoundBuilder.cs +++ b/BepuPhysics/Collidables/CompoundBuilder.cs @@ -66,6 +66,18 @@ public void Add(TypedIndex shape, in RigidPose localPose, in Symmetric3x3 localI Debug.Assert(Symmetric3x3.Determinant(localInverseInertia) > 0, "Child inertia tensors should be invertible. If making an axis-locked compound, consider locking the axis on the completed inertia. " + "If making a kinematic, consider using the overload which takes no inverse inertia."); + } + + /// + /// Adds a new shape to the accumulator. + /// + /// Index of the shape to add. + /// Pose of the shape in the compound's local space. + /// Inverse inertia tensor and inverse mass of the shape being added in the child's local space. The inverse mass is used as the inverse weight for building the compound. + public void Add(TypedIndex shape, in RigidPose localPose, in BodyInertia childInertia) + { + Debug.Assert(childInertia.InverseMass > 0, "Child masses should be finite."); + Add(shape, localPose, childInertia.InverseInertiaTensor, 1f / childInertia.InverseMass); } /// From bd02aeb5eadd754593221fecdbcfdd063656d939 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 7 Mar 2024 16:06:58 -0600 Subject: [PATCH 861/947] CompoundChild.AsPose now an instance method. --- BepuPhysics/Collidables/BigCompound.cs | 2 +- BepuPhysics/Collidables/Compound.cs | 601 +++++++++--------- .../CompoundHomogeneousCompoundSweepTask.cs | 2 +- .../CompoundPairSweepOverlapFinder.cs | 2 +- .../SweepTasks/CompoundPairSweepTask.cs | 4 +- .../SweepTasks/ConvexCompoundSweepTask.cs | 2 +- DemoRenderer/ShapeDrawing/ShapesExtractor.cs | 2 +- 7 files changed, 308 insertions(+), 307 deletions(-) diff --git a/BepuPhysics/Collidables/BigCompound.cs b/BepuPhysics/Collidables/BigCompound.cs index 0b4646512..8948e4238 100644 --- a/BepuPhysics/Collidables/BigCompound.cs +++ b/BepuPhysics/Collidables/BigCompound.cs @@ -160,7 +160,7 @@ public void TestLeaf(int leafIndex, RayData* rayData, float* maximumT) CompoundChildShapeTester tester; tester.T = -1; tester.Normal = default; - Shapes[child.ShapeIndex.Type].RayTest(child.ShapeIndex.Index, CompoundChild.AsPose(ref child), *rayData, ref *maximumT, ref tester); + Shapes[child.ShapeIndex.Type].RayTest(child.ShapeIndex.Index, child.AsPose(), *rayData, ref *maximumT, ref tester); if (tester.T >= 0) { Debug.Assert(*maximumT >= tester.T, "Whatever generated this ray hit should have obeyed the current maximumT value."); diff --git a/BepuPhysics/Collidables/Compound.cs b/BepuPhysics/Collidables/Compound.cs index 2cbdf4b91..52ffc1b82 100644 --- a/BepuPhysics/Collidables/Compound.cs +++ b/BepuPhysics/Collidables/Compound.cs @@ -7,372 +7,373 @@ using BepuPhysics.Trees; using BepuPhysics.CollisionDetection.CollisionTasks; using System.Runtime.InteropServices; +using System.Diagnostics.CodeAnalysis; -namespace BepuPhysics.Collidables +namespace BepuPhysics.Collidables; + +/// +/// Shape and pose of a child within a compound shape. +/// +[StructLayout(LayoutKind.Sequential)] +public struct CompoundChild { /// - /// Shape and pose of a child within a compound shape. + /// Local orientation of the child in the compound. /// - [StructLayout(LayoutKind.Sequential)] - public struct CompoundChild - { - /// - /// Local orientation of the child in the compound. - /// - public Quaternion LocalOrientation; - /// - /// Local position of the child in the compound. - /// - public Vector3 LocalPosition; - /// - /// Index of the shape within whatever shape collection holds the compound's child shape data. - /// - public TypedIndex ShapeIndex; - - /// - /// Reintreprets the 32 bytes of a compound child as a pose. - /// - /// Child to reinterpret. - /// Reference to the child as a pose. - public static ref RigidPose AsPose(ref CompoundChild child) => ref Unsafe.As(ref child); //TODO: This could be made a little easier with UnscopedRef. + public Quaternion LocalOrientation; + /// + /// Local position of the child in the compound. + /// + public Vector3 LocalPosition; + /// + /// Index of the shape within whatever shape collection holds the compound's child shape data. + /// + public TypedIndex ShapeIndex; + + /// + /// Returns a reference to the memory of the as a . + /// + /// Reference to this compound child as a pose. + [UnscopedRef] + public ref RigidPose AsPose() + { + return ref Unsafe.As(ref this); } +} - struct CompoundChildShapeTester : IShapeRayHitHandler +struct CompoundChildShapeTester : IShapeRayHitHandler +{ + //We use a non-generic hit handler to capture the final result of a leaf test. + //This requires caching out the T and Normal for reading by whatever ended up calling this, but it's worth it to avoid AOT pipelines barfing on infinite recursion. + public float T; + public Vector3 Normal; + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool AllowTest(int childIndex) { - //We use a non-generic hit handler to capture the final result of a leaf test. - //This requires caching out the T and Normal for reading by whatever ended up calling this, but it's worth it to avoid AOT pipelines barfing on infinite recursion. - public float T; - public Vector3 Normal; - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool AllowTest(int childIndex) - { - Debug.Assert(childIndex == 0, "Compounds can contain only convexes, so the child index is always zero."); - //The actual test filtering took place in the TestLeaf function, where we call Handler.AllowTest. - return true; - } + Debug.Assert(childIndex == 0, "Compounds can contain only convexes, so the child index is always zero."); + //The actual test filtering took place in the TestLeaf function, where we call Handler.AllowTest. + return true; + } - public void OnRayHit(in RayData ray, ref float maximumT, float t, Vector3 normal, int childIndex) - { - Debug.Assert(childIndex == 0, "Compounds can contain only convexes, so the child index is always zero."); - T = t; - Normal = normal; - } + public void OnRayHit(in RayData ray, ref float maximumT, float t, Vector3 normal, int childIndex) + { + Debug.Assert(childIndex == 0, "Compounds can contain only convexes, so the child index is always zero."); + T = t; + Normal = normal; } +} +/// +/// Minimalist compound shape containing a list of child shapes. Does not make use of any internal acceleration structure; should be used only with small groups of shapes. +/// +public struct Compound : ICompoundShape +{ /// - /// Minimalist compound shape containing a list of child shapes. Does not make use of any internal acceleration structure; should be used only with small groups of shapes. + /// Buffer of children within this compound. /// - public struct Compound : ICompoundShape + public Buffer Children; + + /// + /// Creates a compound shape with no acceleration structure. + /// + /// Set of children in the compound. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public Compound(Buffer children) { - /// - /// Buffer of children within this compound. - /// - public Buffer Children; + Debug.Assert(children.Length > 0, "Compounds must have a nonzero number of children."); + Children = children; + } - /// - /// Creates a compound shape with no acceleration structure. - /// - /// Set of children in the compound. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public Compound(Buffer children) + /// + /// Checks if a shape index. + /// + /// Shape index to analyze. + /// Shape collection into which the index indexes. + /// True if the index is valid, false otherwise. + public static bool ValidateChildIndex(TypedIndex shapeIndex, Shapes shapeBatches) + { + if (shapeIndex.Type < 0 || shapeIndex.Type >= shapeBatches.RegisteredTypeSpan) { - Debug.Assert(children.Length > 0, "Compounds must have a nonzero number of children."); - Children = children; + Debug.Fail("Child shape type needs to fit within the shape batch registered types."); + return false; } - - /// - /// Checks if a shape index. - /// - /// Shape index to analyze. - /// Shape collection into which the index indexes. - /// True if the index is valid, false otherwise. - public static bool ValidateChildIndex(TypedIndex shapeIndex, Shapes shapeBatches) + var batch = shapeBatches[shapeIndex.Type]; + if (shapeIndex.Index < 0 || shapeIndex.Index >= batch.Capacity) { - if (shapeIndex.Type < 0 || shapeIndex.Type >= shapeBatches.RegisteredTypeSpan) - { - Debug.Fail("Child shape type needs to fit within the shape batch registered types."); - return false; - } - var batch = shapeBatches[shapeIndex.Type]; - if (shapeIndex.Index < 0 || shapeIndex.Index >= batch.Capacity) - { - Debug.Fail("Child shape index should point to a valid buffer location in the sahpe batch."); - return false; - } - if (shapeBatches[shapeIndex.Type].Compound) - { - Debug.Fail("Child shape type should be convex."); - return false; - } - //TODO: We don't have a cheap way to verify that a specific index actually contains a shape right now. - return true; + Debug.Fail("Child shape index should point to a valid buffer location in the sahpe batch."); + return false; } - - /// - /// Checks if a set of children shape indices are all valid. - /// - /// Children to examine. - /// Shape collection into which the children index. - /// True if all child indices are valid, false otherwise. - public static bool ValidateChildIndices(Span children, Shapes shapeBatches) + if (shapeBatches[shapeIndex.Type].Compound) { - for (int i = 0; i < children.Length; ++i) - { - ValidateChildIndex(children[i].ShapeIndex, shapeBatches); - } - return true; + Debug.Fail("Child shape type should be convex."); + return false; } + //TODO: We don't have a cheap way to verify that a specific index actually contains a shape right now. + return true; + } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void GetRotatedChildPose(in RigidPose localPose, Quaternion orientation, out RigidPose rotatedChildPose) - { - GetRotatedChildPose(localPose.Position, localPose.Orientation, orientation, out rotatedChildPose.Position, out rotatedChildPose.Orientation); - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void GetRotatedChildPose(Vector3 localPosition, Quaternion localOrientation, Quaternion orientation, out RigidPose rotatedChildPose) - { - GetRotatedChildPose(localPosition, localOrientation, orientation, out rotatedChildPose.Position, out rotatedChildPose.Orientation); - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void GetRotatedChildPose(Vector3 localPosition, Quaternion localOrientation, Quaternion parentOrientation, out Vector3 rotatedPosition, out Quaternion rotatedOrientation) - { - QuaternionEx.ConcatenateWithoutOverlap(localOrientation, parentOrientation, out rotatedOrientation); - QuaternionEx.Transform(localPosition, parentOrientation, out rotatedPosition); - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void GetRotatedChildPose(in RigidPoseWide localPose, in QuaternionWide orientation, out Vector3Wide childPosition, out QuaternionWide childOrientation) - { - QuaternionWide.ConcatenateWithoutOverlap(localPose.Orientation, orientation, out childOrientation); - QuaternionWide.TransformWithoutOverlap(localPose.Position, orientation, out childPosition); - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void GetRotatedChildPose(in RigidPoseWide localPose, in QuaternionWide orientation, out RigidPoseWide rotatedChildPose) - { - GetRotatedChildPose(localPose, orientation, out rotatedChildPose.Position, out rotatedChildPose.Orientation); - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void GetWorldPose(in RigidPose localPose, in RigidPose transform, out RigidPose worldPose) + /// + /// Checks if a set of children shape indices are all valid. + /// + /// Children to examine. + /// Shape collection into which the children index. + /// True if all child indices are valid, false otherwise. + public static bool ValidateChildIndices(Span children, Shapes shapeBatches) + { + for (int i = 0; i < children.Length; ++i) { - GetRotatedChildPose(localPose, transform.Orientation, out worldPose); - //TODO: This is an area that has to be updated for high precision poses. May be able to centralize positional work - //by deferring it until the final bounds scatter step. Would require looking up the position then, but could be worth simplicity. - worldPose.Position += transform.Position; + ValidateChildIndex(children[i].ShapeIndex, shapeBatches); } + return true; + } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ComputeChildBounds(in CompoundChild child, Quaternion orientation, Shapes shapeBatches, out Vector3 childMin, out Vector3 childMax) - { - GetRotatedChildPose(child.LocalPosition, child.LocalOrientation, orientation, out var childPose); - Debug.Assert(!shapeBatches[child.ShapeIndex.Type].Compound, "All children of a compound must be convex."); - shapeBatches[child.ShapeIndex.Type].ComputeBounds(child.ShapeIndex.Index, childPose, out childMin, out childMax); - } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void GetRotatedChildPose(in RigidPose localPose, Quaternion orientation, out RigidPose rotatedChildPose) + { + GetRotatedChildPose(localPose.Position, localPose.Orientation, orientation, out rotatedChildPose.Position, out rotatedChildPose.Orientation); + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void GetRotatedChildPose(Vector3 localPosition, Quaternion localOrientation, Quaternion orientation, out RigidPose rotatedChildPose) + { + GetRotatedChildPose(localPosition, localOrientation, orientation, out rotatedChildPose.Position, out rotatedChildPose.Orientation); + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void GetRotatedChildPose(Vector3 localPosition, Quaternion localOrientation, Quaternion parentOrientation, out Vector3 rotatedPosition, out Quaternion rotatedOrientation) + { + QuaternionEx.ConcatenateWithoutOverlap(localOrientation, parentOrientation, out rotatedOrientation); + QuaternionEx.Transform(localPosition, parentOrientation, out rotatedPosition); + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void GetRotatedChildPose(in RigidPoseWide localPose, in QuaternionWide orientation, out Vector3Wide childPosition, out QuaternionWide childOrientation) + { + QuaternionWide.ConcatenateWithoutOverlap(localPose.Orientation, orientation, out childOrientation); + QuaternionWide.TransformWithoutOverlap(localPose.Position, orientation, out childPosition); + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void GetRotatedChildPose(in RigidPoseWide localPose, in QuaternionWide orientation, out RigidPoseWide rotatedChildPose) + { + GetRotatedChildPose(localPose, orientation, out rotatedChildPose.Position, out rotatedChildPose.Orientation); + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void GetWorldPose(in RigidPose localPose, in RigidPose transform, out RigidPose worldPose) + { + GetRotatedChildPose(localPose, transform.Orientation, out worldPose); + //TODO: This is an area that has to be updated for high precision poses. May be able to centralize positional work + //by deferring it until the final bounds scatter step. Would require looking up the position then, but could be worth simplicity. + worldPose.Position += transform.Position; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void ComputeChildBounds(in CompoundChild child, Quaternion orientation, Shapes shapeBatches, out Vector3 childMin, out Vector3 childMax) + { + GetRotatedChildPose(child.LocalPosition, child.LocalOrientation, orientation, out var childPose); + Debug.Assert(!shapeBatches[child.ShapeIndex.Type].Compound, "All children of a compound must be convex."); + shapeBatches[child.ShapeIndex.Type].ComputeBounds(child.ShapeIndex.Index, childPose, out childMin, out childMax); + } - public void ComputeBounds(Quaternion orientation, Shapes shapeBatches, out Vector3 min, out Vector3 max) + public void ComputeBounds(Quaternion orientation, Shapes shapeBatches, out Vector3 min, out Vector3 max) + { + ComputeChildBounds(Children[0], orientation, shapeBatches, out min, out max); + for (int i = 1; i < Children.Length; ++i) { - ComputeChildBounds(Children[0], orientation, shapeBatches, out min, out max); - for (int i = 1; i < Children.Length; ++i) - { - ref var child = ref Children[i]; - ComputeChildBounds(Children[i], orientation, shapeBatches, out var childMin, out var childMax); - BoundingBox.CreateMerged(min, max, childMin, childMax, out min, out max); - } + ref var child = ref Children[i]; + ComputeChildBounds(Children[i], orientation, shapeBatches, out var childMin, out var childMax); + BoundingBox.CreateMerged(min, max, childMin, childMax, out min, out max); } + } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void AddChildBoundsToBatcher(ref Buffer children, ref BoundingBoxBatcher batcher, in RigidPose pose, in BodyVelocity velocity, int bodyIndex) + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void AddChildBoundsToBatcher(ref Buffer children, ref BoundingBoxBatcher batcher, in RigidPose pose, in BodyVelocity velocity, int bodyIndex) + { + //Note that this approximates the velocity of the child using a piecewise extrapolation using the parent's angular velocity. + //For significant angular velocities, this is actually wrong, but this is how v1 worked forever and it's cheap. + //May want to revisit this later- it would likely require that the BoundingBoxBatcher have a continuation, or to include more information + //for the convex path to condition on. + BodyVelocity childVelocity; + childVelocity.Angular = velocity.Angular; + for (int i = 0; i < children.Length; ++i) { - //Note that this approximates the velocity of the child using a piecewise extrapolation using the parent's angular velocity. - //For significant angular velocities, this is actually wrong, but this is how v1 worked forever and it's cheap. - //May want to revisit this later- it would likely require that the BoundingBoxBatcher have a continuation, or to include more information - //for the convex path to condition on. - BodyVelocity childVelocity; - childVelocity.Angular = velocity.Angular; - for (int i = 0; i < children.Length; ++i) + ref var child = ref children[i]; + GetRotatedChildPose(child.LocalPosition, child.LocalOrientation, pose.Orientation, out var childPose); + var angularContributionToChildLinear = Vector3.Cross(velocity.Angular, childPose.Position); + var contributionLengthSquared = angularContributionToChildLinear.LengthSquared(); + var localPoseRadiusSquared = childPose.Position.LengthSquared(); + if (contributionLengthSquared > localPoseRadiusSquared) { - ref var child = ref children[i]; - GetRotatedChildPose(child.LocalPosition, child.LocalOrientation, pose.Orientation, out var childPose); - var angularContributionToChildLinear = Vector3.Cross(velocity.Angular, childPose.Position); - var contributionLengthSquared = angularContributionToChildLinear.LengthSquared(); - var localPoseRadiusSquared = childPose.Position.LengthSquared(); - if (contributionLengthSquared > localPoseRadiusSquared) - { - angularContributionToChildLinear *= (float)(Math.Sqrt(localPoseRadiusSquared) / Math.Sqrt(contributionLengthSquared)); - } - childVelocity.Linear = velocity.Linear + angularContributionToChildLinear; - childPose.Position += pose.Position; - batcher.AddCompoundChild(bodyIndex, children[i].ShapeIndex, childPose, childVelocity); + angularContributionToChildLinear *= (float)(Math.Sqrt(localPoseRadiusSquared) / Math.Sqrt(contributionLengthSquared)); } + childVelocity.Linear = velocity.Linear + angularContributionToChildLinear; + childPose.Position += pose.Position; + batcher.AddCompoundChild(bodyIndex, children[i].ShapeIndex, childPose, childVelocity); } + } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void AddChildBoundsToBatcher(ref BoundingBoxBatcher batcher, in RigidPose pose, in BodyVelocity velocity, int bodyIndex) - { - AddChildBoundsToBatcher(ref Children, ref batcher, pose, velocity, bodyIndex); - } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void AddChildBoundsToBatcher(ref BoundingBoxBatcher batcher, in RigidPose pose, in BodyVelocity velocity, int bodyIndex) + { + AddChildBoundsToBatcher(ref Children, ref batcher, pose, velocity, bodyIndex); + } - public void RayTest(in RigidPose pose, in RayData ray, ref float maximumT, Shapes shapeBatches, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler - { - Matrix3x3.CreateFromQuaternion(pose.Orientation, out var orientation); - RayData localRay; - Matrix3x3.TransformTranspose(ray.Origin - pose.Position, orientation, out localRay.Origin); - Matrix3x3.TransformTranspose(ray.Direction, orientation, out localRay.Direction); - localRay.Id = 0; + public void RayTest(in RigidPose pose, in RayData ray, ref float maximumT, Shapes shapeBatches, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler + { + Matrix3x3.CreateFromQuaternion(pose.Orientation, out var orientation); + RayData localRay; + Matrix3x3.TransformTranspose(ray.Origin - pose.Position, orientation, out localRay.Origin); + Matrix3x3.TransformTranspose(ray.Direction, orientation, out localRay.Direction); + localRay.Id = 0; - for (int i = 0; i < Children.Length; ++i) + for (int i = 0; i < Children.Length; ++i) + { + if (hitHandler.AllowTest(i)) { - if (hitHandler.AllowTest(i)) + ref var child = ref Children[i]; + CompoundChildShapeTester tester; + tester.T = -1; + tester.Normal = default; + shapeBatches[child.ShapeIndex.Type].RayTest(child.ShapeIndex.Index, child.AsPose(), localRay, ref maximumT, ref tester); + if (tester.T >= 0) { - ref var child = ref Children[i]; - CompoundChildShapeTester tester; - tester.T = -1; - tester.Normal = default; - shapeBatches[child.ShapeIndex.Type].RayTest(child.ShapeIndex.Index, CompoundChild.AsPose(ref child), localRay, ref maximumT, ref tester); - if (tester.T >= 0) - { - Debug.Assert(maximumT >= tester.T, "Whatever generated this ray hit should have obeyed the current maximumT value."); - Matrix3x3.Transform(tester.Normal, orientation, out var rotatedNormal); - hitHandler.OnRayHit(ray, ref maximumT, tester.T, rotatedNormal, i); - } + Debug.Assert(maximumT >= tester.T, "Whatever generated this ray hit should have obeyed the current maximumT value."); + Matrix3x3.Transform(tester.Normal, orientation, out var rotatedNormal); + hitHandler.OnRayHit(ray, ref maximumT, tester.T, rotatedNormal, i); } } } + } - public unsafe void RayTest(in RigidPose pose, ref RaySource rays, Shapes shapeBatches, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler - { - //TODO: Note that we dispatch a bunch of scalar tests here. You could be more clever than this- batched tests are possible. - //It's relatively easy to do batching for this compound type since there is no hierarchy traversal, but we refactored things to avoid an infinite generic expansion issue in AOT compilation. - //There are plenty of ways to work around that, but right now our batched raytracing implementation is bad enough that spending extra work here is questionable. We'll avoid breaking it for now, but that's all. - for (int i = 0; i < rays.RayCount; ++i) - { - rays.GetRay(i, out var ray, out var maximumT); - RayTest(pose, *ray, ref *maximumT, shapeBatches, ref hitHandler); - } - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapes) + public unsafe void RayTest(in RigidPose pose, ref RaySource rays, Shapes shapeBatches, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler + { + //TODO: Note that we dispatch a bunch of scalar tests here. You could be more clever than this- batched tests are possible. + //It's relatively easy to do batching for this compound type since there is no hierarchy traversal, but we refactored things to avoid an infinite generic expansion issue in AOT compilation. + //There are plenty of ways to work around that, but right now our batched raytracing implementation is bad enough that spending extra work here is questionable. We'll avoid breaking it for now, but that's all. + for (int i = 0; i < rays.RayCount; ++i) { - return new CompoundShapeBatch(pool, initialCapacity, shapes); + rays.GetRay(i, out var ray, out var maximumT); + RayTest(pose, *ray, ref *maximumT, shapeBatches, ref hitHandler); } + } - public int ChildCount - { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - get { return Children.Length; } - } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapes) + { + return new CompoundShapeBatch(pool, initialCapacity, shapes); + } + public int ChildCount + { [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref CompoundChild GetChild(int compoundChildIndex) - { - return ref Children[compoundChildIndex]; - } + get { return Children.Length; } + } - /// - /// Adds a child to the compound. - /// - /// Child to add to the compound. - /// Pool to use to resize the compound's children buffer if necessary. - public void Add(CompoundChild child, BufferPool pool) - { - pool.Resize(ref Children, Children.Length + 1, Children.Length); - Children[^1] = child; - } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public ref CompoundChild GetChild(int compoundChildIndex) + { + return ref Children[compoundChildIndex]; + } + + /// + /// Adds a child to the compound. + /// + /// Child to add to the compound. + /// Pool to use to resize the compound's children buffer if necessary. + public void Add(CompoundChild child, BufferPool pool) + { + pool.Resize(ref Children, Children.Length + 1, Children.Length); + Children[^1] = child; + } - /// - /// Removes a child from the compound by index. The last child is pulled to fill the gap left by the removed child. - /// - /// Index of the child to remove from the compound. - /// Pool to use to resize the compound's children buffer if necessary. - public void RemoveAt(int childIndex, BufferPool pool) + /// + /// Removes a child from the compound by index. The last child is pulled to fill the gap left by the removed child. + /// + /// Index of the child to remove from the compound. + /// Pool to use to resize the compound's children buffer if necessary. + public void RemoveAt(int childIndex, BufferPool pool) + { + var lastIndex = Children.Length - 1; + if (childIndex < lastIndex) { - var lastIndex = Children.Length - 1; - if (childIndex < lastIndex) - { - Children[childIndex] = Children[lastIndex]; - } - //Shrinking the buffer takes care of 'removing' the now-empty last slot. - pool.Resize(ref Children, Children.Length - 1, Children.Length - 1); + Children[childIndex] = Children[lastIndex]; } + //Shrinking the buffer takes care of 'removing' the now-empty last slot. + pool.Resize(ref Children, Children.Length - 1, Children.Length - 1); + } - public unsafe void FindLocalOverlaps(ref Buffer pairs, BufferPool pool, Shapes shapes, ref TOverlaps overlaps) - where TOverlaps : struct, ICollisionTaskOverlaps - where TSubpairOverlaps : struct, ICollisionTaskSubpairOverlaps + public unsafe void FindLocalOverlaps(ref Buffer pairs, BufferPool pool, Shapes shapes, ref TOverlaps overlaps) + where TOverlaps : struct, ICollisionTaskOverlaps + where TSubpairOverlaps : struct, ICollisionTaskSubpairOverlaps + { + for (int pairIndex = 0; pairIndex < pairs.Length; ++pairIndex) { - for (int pairIndex = 0; pairIndex < pairs.Length; ++pairIndex) + ref var pair = ref pairs[pairIndex]; + ref var compound = ref Unsafe.AsRef(pair.Container); + ref var overlapsForPair = ref overlaps.GetOverlapsForPair(pairIndex); + for (int i = 0; i < compound.Children.Length; ++i) { - ref var pair = ref pairs[pairIndex]; - ref var compound = ref Unsafe.AsRef(pair.Container); - ref var overlapsForPair = ref overlaps.GetOverlapsForPair(pairIndex); - for (int i = 0; i < compound.Children.Length; ++i) + ref var child = ref compound.Children[i]; + //TODO: This does quite a bit of work. May want to try a simple bounding sphere instead (based on a dedicated maximum radius request). + //Could also benefit from using the BoundingBox layout test, which is a little faster than 4 independent values. + shapes[child.ShapeIndex.Type].ComputeBounds(child.ShapeIndex.Index, child.LocalOrientation, out _, out _, out var min, out var max); + min += child.LocalPosition; + max += child.LocalPosition; + if (BoundingBox.Intersects(min, max, pair.Min, pair.Max)) { - ref var child = ref compound.Children[i]; - //TODO: This does quite a bit of work. May want to try a simple bounding sphere instead (based on a dedicated maximum radius request). - //Could also benefit from using the BoundingBox layout test, which is a little faster than 4 independent values. - shapes[child.ShapeIndex.Type].ComputeBounds(child.ShapeIndex.Index, child.LocalOrientation, out _, out _, out var min, out var max); - min += child.LocalPosition; - max += child.LocalPosition; - if (BoundingBox.Intersects(min, max, pair.Min, pair.Max)) - { - overlapsForPair.Allocate(pool) = i; - } + overlapsForPair.Allocate(pool) = i; } } } + } - public unsafe void FindLocalOverlaps(Vector3 min, Vector3 max, Vector3 sweep, float maximumT, BufferPool pool, Shapes shapes, void* overlapsPointer) - where TOverlaps : ICollisionTaskSubpairOverlaps + public unsafe void FindLocalOverlaps(Vector3 min, Vector3 max, Vector3 sweep, float maximumT, BufferPool pool, Shapes shapes, void* overlapsPointer) + where TOverlaps : ICollisionTaskSubpairOverlaps + { + Tree.ConvertBoxToCentroidWithExtent(min, max, out var sweepOrigin, out var expansion); + TreeRay.CreateFrom(sweepOrigin, sweep, maximumT, out var ray); + ref var overlaps = ref Unsafe.AsRef(overlapsPointer); + for (int i = 0; i < Children.Length; ++i) { - Tree.ConvertBoxToCentroidWithExtent(min, max, out var sweepOrigin, out var expansion); - TreeRay.CreateFrom(sweepOrigin, sweep, maximumT, out var ray); - ref var overlaps = ref Unsafe.AsRef(overlapsPointer); - for (int i = 0; i < Children.Length; ++i) + ref var child = ref Children[i]; + shapes[child.ShapeIndex.Type].ComputeBounds(child.ShapeIndex.Index, child.LocalOrientation, out _, out _, out var childMin, out var childMax); + childMin = childMin + child.LocalPosition - expansion; + childMax = childMax + child.LocalPosition + expansion; + if (Tree.Intersects(childMin, childMax, &ray, out _)) { - ref var child = ref Children[i]; - shapes[child.ShapeIndex.Type].ComputeBounds(child.ShapeIndex.Index, child.LocalOrientation, out _, out _, out var childMin, out var childMax); - childMin = childMin + child.LocalPosition - expansion; - childMax = childMax + child.LocalPosition + expansion; - if (Tree.Intersects(childMin, childMax, &ray, out _)) - { - overlaps.Allocate(pool) = i; - } + overlaps.Allocate(pool) = i; } } + } - /// - /// Computes the inertia of a compound. Does not recenter the child poses. - /// - /// Masses of the children. - /// Shapes collection containing the data for the compound child shapes. - /// Inertia of the compound. - public BodyInertia ComputeInertia(Span childMasses, Shapes shapes) - { - return CompoundBuilder.ComputeInertia(Children, childMasses, shapes); - } - - /// - /// Computes the inertia of a compound. Recenters the child poses around the calculated center of mass. - /// - /// Shapes collection containing the data for the compound child shapes. - /// Masses of the children. - /// Calculated center of mass of the compound. Subtracted from all the compound child poses. - /// Inertia of the compound. - public BodyInertia ComputeInertia(Span childMasses, Shapes shapes, out Vector3 centerOfMass) - { - return CompoundBuilder.ComputeInertia(Children, childMasses, shapes, out centerOfMass); - } - - public void Dispose(BufferPool bufferPool) - { - bufferPool.Return(ref Children); - } + /// + /// Computes the inertia of a compound. Does not recenter the child poses. + /// + /// Masses of the children. + /// Shapes collection containing the data for the compound child shapes. + /// Inertia of the compound. + public BodyInertia ComputeInertia(Span childMasses, Shapes shapes) + { + return CompoundBuilder.ComputeInertia(Children, childMasses, shapes); + } - /// - /// Type id of list based compound shapes. - /// - public const int Id = 6; - public static int TypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { return Id; } } + /// + /// Computes the inertia of a compound. Recenters the child poses around the calculated center of mass. + /// + /// Shapes collection containing the data for the compound child shapes. + /// Masses of the children. + /// Calculated center of mass of the compound. Subtracted from all the compound child poses. + /// Inertia of the compound. + public BodyInertia ComputeInertia(Span childMasses, Shapes shapes, out Vector3 centerOfMass) + { + return CompoundBuilder.ComputeInertia(Children, childMasses, shapes, out centerOfMass); } + public void Dispose(BufferPool bufferPool) + { + bufferPool.Return(ref Children); + } + /// + /// Type id of list based compound shapes. + /// + public const int Id = 6; + public static int TypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { return Id; } } } diff --git a/BepuPhysics/CollisionDetection/SweepTasks/CompoundHomogeneousCompoundSweepTask.cs b/BepuPhysics/CollisionDetection/SweepTasks/CompoundHomogeneousCompoundSweepTask.cs index 35af6b428..a36bcfee2 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/CompoundHomogeneousCompoundSweepTask.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/CompoundHomogeneousCompoundSweepTask.cs @@ -46,7 +46,7 @@ protected unsafe override bool PreorderedTypeSweep( var task = sweepTasks.GetTask(compoundChildType, Triangle.Id); shapes[compoundChildType].GetShapeData(compoundChild.ShapeIndex.Index, out var compoundChildShapeData, out _); if (task.Sweep( - compoundChildShapeData, compoundChildType, CompoundChild.AsPose(ref compoundChild), orientationA, velocityA, + compoundChildShapeData, compoundChildType, compoundChild.AsPose(), orientationA, velocityA, Unsafe.AsPointer(ref childB), Triangle.Id, childPoseB, offsetB, orientationB, velocityB, maximumT, minimumProgression, convergenceThreshold, maximumIterationCount, out var t0Candidate, out var t1Candidate, out var hitLocationCandidate, out var hitNormalCandidate)) diff --git a/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepOverlapFinder.cs b/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepOverlapFinder.cs index 6bec6fae3..699073b74 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepOverlapFinder.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepOverlapFinder.cs @@ -28,7 +28,7 @@ public static unsafe void FindOverlaps( { ref var child = ref compoundA.GetChild(i); BoundingBoxHelpers.GetLocalBoundingBoxForSweep( - child.ShapeIndex, shapes, CompoundChild.AsPose(ref child), orientationA, velocityA, + child.ShapeIndex, shapes, child.AsPose(), orientationA, velocityA, offsetB, orientationB, velocityB, maximumT, out var sweep, out var min, out var max); ref var childOverlaps = ref overlaps.GetOverlapsForChild(i); childOverlaps.ChildIndex = i; diff --git a/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepTask.cs b/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepTask.cs index f52c7fcc8..549b54e4c 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepTask.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/CompoundPairSweepTask.cs @@ -50,8 +50,8 @@ protected override unsafe bool PreorderedTypeSweep( { var task = sweepTasks.GetTask(childTypeA, childTypeB); if (task != null && task.Sweep( - childShapeDataA, childTypeA, CompoundChild.AsPose(ref childA), orientationA, velocityA, - childShapeDataB, childTypeB, CompoundChild.AsPose(ref childB), offsetB, orientationB, velocityB, + childShapeDataA, childTypeA, childA.AsPose(), orientationA, velocityA, + childShapeDataB, childTypeB, childB.AsPose(), offsetB, orientationB, velocityB, maximumT, minimumProgression, convergenceThreshold, maximumIterationCount, out var t0Candidate, out var t1Candidate, out var hitLocationCandidate, out var hitNormalCandidate)) { diff --git a/BepuPhysics/CollisionDetection/SweepTasks/ConvexCompoundSweepTask.cs b/BepuPhysics/CollisionDetection/SweepTasks/ConvexCompoundSweepTask.cs index a1b86deab..a577e8a69 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/ConvexCompoundSweepTask.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/ConvexCompoundSweepTask.cs @@ -42,7 +42,7 @@ protected override unsafe bool PreorderedTypeSweep( var task = sweepTasks.GetTask(TShapeA.TypeId, childType); if (task != null && task.Sweep( shapeDataA, TShapeA.TypeId, new RigidPose() { Orientation = Quaternion.Identity }, orientationA, velocityA, - childShapeData, childType, CompoundChild.AsPose(ref child), offsetB, orientationB, velocityB, + childShapeData, childType, child.AsPose(), offsetB, orientationB, velocityB, maximumT, minimumProgression, convergenceThreshold, maximumIterationCount, out var t0Candidate, out var t1Candidate, out var hitLocationCandidate, out var hitNormalCandidate)) { diff --git a/DemoRenderer/ShapeDrawing/ShapesExtractor.cs b/DemoRenderer/ShapeDrawing/ShapesExtractor.cs index 409125924..911a1c1ea 100644 --- a/DemoRenderer/ShapeDrawing/ShapesExtractor.cs +++ b/DemoRenderer/ShapeDrawing/ShapesExtractor.cs @@ -79,7 +79,7 @@ private unsafe void AddCompoundChildren(ref Buffer children, Shap for (int i = 0; i < children.Length; ++i) { ref var child = ref children[i]; - Compound.GetWorldPose(CompoundChild.AsPose(ref child), pose, out var childPose); + Compound.GetWorldPose(child.AsPose(), pose, out var childPose); AddShape(shapes, child.ShapeIndex, childPose, color, ref shapeCache, pool); } } From 424fcf06eb26be987d73a399e4dc4e726b6b8cb0 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 7 Mar 2024 16:10:38 -0600 Subject: [PATCH 862/947] Missing AsPose oopsy. --- .../CollisionTasks/CompoundPairOverlapFinder.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlapFinder.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlapFinder.cs index ddf7ef78d..16d7adf46 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlapFinder.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlapFinder.cs @@ -87,7 +87,7 @@ public static unsafe void FindLocalOverlaps(ref Buffer pairs, Vector3Wide.WriteFirst(subpair.Pair->AngularVelocityB, ref GatherScatter.GetOffsetInstance(ref angularVelocityB, j)); Unsafe.Add(ref Unsafe.As, float>(ref maximumAllowedExpansion), j) = subpair.Pair->MaximumExpansion; - RigidPoseWide.WriteFirst(CompoundChild.AsPose(ref *subpair.Child), ref GatherScatter.GetOffsetInstance(ref localPosesA, j)); + RigidPoseWide.WriteFirst((*subpair.Child).AsPose(), ref GatherScatter.GetOffsetInstance(ref localPosesA, j)); } QuaternionWide.Conjugate(orientationB, out var toLocalB); From 7fd2e4c63ca863595950bd9f65005283cccddda3 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 7 Mar 2024 16:10:52 -0600 Subject: [PATCH 863/947] CompoundChild constructor for marginally less annoyance. --- BepuPhysics/Collidables/Compound.cs | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/BepuPhysics/Collidables/Compound.cs b/BepuPhysics/Collidables/Compound.cs index 52ffc1b82..826b07253 100644 --- a/BepuPhysics/Collidables/Compound.cs +++ b/BepuPhysics/Collidables/Compound.cs @@ -30,6 +30,18 @@ public struct CompoundChild /// public TypedIndex ShapeIndex; + /// + /// Creates a compound child. + /// + /// Pose of the compound child in the local space of the parent shape. + /// Index of the shape used by the child. + public CompoundChild(in RigidPose pose, TypedIndex shapeIndex) + { + LocalOrientation = pose.Orientation; + LocalPosition = pose.Position; + ShapeIndex = shapeIndex; + } + /// /// Returns a reference to the memory of the as a . /// From a543511b15ac68ce69e31a34af69c75fbb27935b Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 7 Mar 2024 16:13:25 -0600 Subject: [PATCH 864/947] Another missing AsPose oopsy. --- Demos/Demos/CustomVoxelCollidableDemo.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Demos/Demos/CustomVoxelCollidableDemo.cs b/Demos/Demos/CustomVoxelCollidableDemo.cs index a1d5a9835..b44b80bef 100644 --- a/Demos/Demos/CustomVoxelCollidableDemo.cs +++ b/Demos/Demos/CustomVoxelCollidableDemo.cs @@ -333,7 +333,7 @@ public void GetChildAData(ref CollisionBatcher collision { ref var compoundA = ref Unsafe.AsRef(pair.A); ref var compoundChildA = ref compoundA.GetChild(childIndexA); - Compound.GetRotatedChildPose(CompoundChild.AsPose(ref compoundChildA), pair.OrientationA, out childPoseA); + Compound.GetRotatedChildPose(compoundChildA.AsPose(), pair.OrientationA, out childPoseA); childTypeA = compoundChildA.ShapeIndex.Type; collisionBatcher.Shapes[childTypeA].GetShapeData(compoundChildA.ShapeIndex.Index, out childShapeDataA, out _); } From d3b4d900e418ceb09ac520c49fc77a258c02bbe2 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 7 Mar 2024 16:26:58 -0600 Subject: [PATCH 865/947] All demos scooted into file scoped namespaces. --- Demos/Controls.cs | 895 +++++----- Demos/Demo.cs | 191 ++- Demos/DemoCallbacks.cs | 286 ++-- Demos/DemoHarness.cs | 717 ++++---- Demos/DemoMeshHelper.cs | 301 ++-- Demos/DemoSet.cs | 133 +- Demos/Demos/BlockChainDemo.cs | 143 +- Demos/Demos/BouncinessDemo.cs | 215 ++- Demos/Demos/Cars/CarCallbacks.cs | 93 +- Demos/Demos/Cars/CarDemo.cs | 355 ++-- Demos/Demos/Cars/SimpleCar.cs | 177 +- Demos/Demos/Cars/SimpleCarController.cs | 237 ++- Demos/Demos/Cars/WheelHandles.cs | 17 +- Demos/Demos/ChainFountainDemo.cs | 193 ++- .../Demos/Characters/CharacterControllers.cs | 1449 ++++++++--------- Demos/Demos/Characters/CharacterDemo.cs | 381 +++-- Demos/Demos/Characters/CharacterInput.cs | 295 ++-- .../Characters/CharacterMotionConstraint.cs | 1169 ++++++------- .../Characters/CharacterMotionConstraint.tt | 609 ++++--- .../CharacterNarrowphaseCallbacks.cs | 93 +- Demos/Demos/ClothDemo.cs | 437 +++-- Demos/Demos/CollisionQueryDemo.cs | 415 +++-- Demos/Demos/ColosseumDemo.cs | 175 +- Demos/Demos/CompoundDemo.cs | 355 ++-- Demos/Demos/ContactEventsDemo.cs | 1267 +++++++------- .../Demos/ContinuousCollisionDetectionDemo.cs | 275 ++-- Demos/Demos/CustomVoxelCollidableDemo.cs | 777 +++++---- Demos/Demos/Dancers/DancerDemo.cs | 291 ++-- Demos/Demos/Dancers/DemoDancers.cs | 831 +++++----- Demos/Demos/Dancers/PlumpDancerDemo.cs | 397 +++-- Demos/Demos/FrictionDemo.cs | 191 ++- Demos/Demos/NewtDemo.cs | 1233 +++++++------- Demos/Demos/PerBodyGravityDemo.cs | 245 ++- Demos/Demos/PlanetDemo.cs | 115 +- Demos/Demos/PyramidDemo.cs | 111 +- Demos/Demos/RagdollDemo.cs | 1025 ++++++------ Demos/Demos/RagdollTubeDemo.cs | 143 +- Demos/Demos/RayCastingDemo.cs | 901 +++++----- Demos/Demos/RopeStabilityDemo.cs | 449 +++-- Demos/Demos/RopeTwistDemo.cs | 283 ++-- Demos/Demos/SimpleSelfContainedDemo.cs | 445 +++-- Demos/Demos/Sponsors/SponsorCharacterAI.cs | 93 +- Demos/Demos/Sponsors/SponsorDemo.cs | 471 +++--- Demos/Demos/Sponsors/SponsorNewt.cs | 159 +- Demos/Demos/SubsteppingDemo.cs | 257 ++- Demos/Demos/SweepDemo.cs | 669 ++++---- Demos/Demos/Tanks/AITank.cs | 183 ++- Demos/Demos/Tanks/Tank.cs | 755 +++++---- Demos/Demos/Tanks/TankCallbacks.cs | 263 ++- Demos/Demos/Tanks/TankController.cs | 137 +- Demos/Demos/Tanks/TankDemo.cs | 583 ++++--- Demos/Demos/Tanks/TankDescription.cs | 207 ++- Demos/Demos/Tanks/TankPartDescription.cs | 57 +- Demos/GameLoop.cs | 111 +- Demos/Grabber.cs | 207 ++- Demos/Program.cs | 31 +- Demos/RolloverInfo.cs | 97 +- Demos/SimulationTimeSamples.cs | 111 +- .../SpecializedTests/BatchedCollisionTests.cs | 489 +++--- .../BroadPhaseStressTestDemo.cs | 291 ++-- Demos/SpecializedTests/CacheBlaster.cs | 59 +- Demos/SpecializedTests/CapsuleTestDemo.cs | 67 +- Demos/SpecializedTests/CharacterTestDemo.cs | 217 ++- Demos/SpecializedTests/ClothLatticeDemo.cs | 141 +- Demos/SpecializedTests/CompoundBoundTests.cs | 405 +++-- .../CompoundCollisionIndicesTest.cs | 113 +- .../ConstrainedKinematicIntegrationTest.cs | 81 +- Demos/SpecializedTests/ConstraintTestDemo.cs | 807 +++++---- Demos/SpecializedTests/CylinderTestDemo.cs | 601 ++++--- Demos/SpecializedTests/DeterminismTest.cs | 101 +- .../FountainStressTestDemo.cs | 667 ++++---- Demos/SpecializedTests/GyroscopeTestDemo.cs | 101 +- .../IntertreeThreadingTests.cs | 301 ++-- .../Media/2.0/BedsheetDemo.cs | 213 ++- .../Media/2.0/ColosseumVideoDemo.cs | 225 ++- .../2.0/NewtDemandingSacrificeVideoDemo.cs | 109 +- .../Media/2.0/NewtVideoDemo.cs | 81 +- .../Media/2.0/PyramidVideoDemo.cs | 111 +- .../Media/2.0/ShrinkwrappedNewtsVideoDemo.cs | 87 +- .../Media/2.4/Colosseum24VideoDemo.cs | 167 +- .../Media/2.4/ExcessivePyramidVideoDemo.cs | 81 +- .../Media/2.4/NewtTyrannyDemo.cs | 173 +- .../Media/2.4/RagdollTubeVideoDemo.cs | 147 +- .../Media/2.4/TankSwarmDemo.cs | 585 ++++--- .../Media/2.4/VideoDancerDemo.cs | 291 ++-- .../Media/2.4/VideoPlumpDancerDemo.cs | 397 +++-- Demos/SpecializedTests/MeshMeshTestDemo.cs | 69 +- .../SpecializedTests/MeshReductionTestDemo.cs | 275 ++-- .../MeshSerializationTestDemo.cs | 75 +- Demos/SpecializedTests/MeshTestDemo.cs | 125 +- Demos/SpecializedTests/MinkowskiVisualizer.cs | 185 ++- Demos/SpecializedTests/NewtonsCradleDemo.cs | 87 +- .../PyramidAwakenerTestDemo.cs | 99 +- Demos/SpecializedTests/RayTesting.cs | 815 +++++---- .../ScalarIntegrationTestDemo.cs | 303 ++-- Demos/SpecializedTests/ShapePileTestDemo.cs | 207 ++- .../SpecializedTests/SimulationScrambling.cs | 591 ++++--- Demos/SpecializedTests/SolverBatchTestDemo.cs | 167 +- Demos/SpecializedTests/SortTest.cs | 153 +- Demos/SpecializedTests/TestHelpers.cs | 57 +- Demos/SpecializedTests/TreeTest.cs | 231 ++- Demos/SpecializedTests/TriangleRayTestDemo.cs | 279 ++-- Demos/SpecializedTests/TriangleTestDemo.cs | 371 +++-- Demos/SpecializedTests/VolumeQueryTests.cs | 437 +++-- Demos/TimingsRingBuffer.cs | 111 +- Demos/UI/DemoSwapper.cs | 107 +- Demos/UI/Graph.cs | 565 ++++--- 107 files changed, 17535 insertions(+), 17583 deletions(-) diff --git a/Demos/Controls.cs b/Demos/Controls.cs index d541ae5b8..cdeef3726 100644 --- a/Demos/Controls.cs +++ b/Demos/Controls.cs @@ -4,503 +4,502 @@ using System.Collections.Generic; using System.Runtime.InteropServices; -namespace Demos +namespace Demos; + +/// +/// Caches strings for enum values to avoid enum boxing. +/// +static class ControlStrings { - /// - /// Caches strings for enum values to avoid enum boxing. - /// - static class ControlStrings - { - static Dictionary keys; - static Dictionary mouseButtons; - static Dictionary mouseWheel; + static Dictionary keys; + static Dictionary mouseButtons; + static Dictionary mouseWheel; - public static string GetName(Key key) - { - return keys[key]; - } - public static string GetName(MouseButton button) + public static string GetName(Key key) + { + return keys[key]; + } + public static string GetName(MouseButton button) + { + return mouseButtons[button]; + } + public static string GetName(MouseWheelAction wheelAction) + { + return mouseWheel[wheelAction]; + } + + static ControlStrings() + { + keys = new Dictionary(); + var keyNames = Enum.GetNames(typeof(Key)); + var keyValues = (Key[])Enum.GetValues(typeof(Key)); + for (int i = 0; i < keyNames.Length; ++i) { - return mouseButtons[button]; + keys.TryAdd(keyValues[i], keyNames[i]); } - public static string GetName(MouseWheelAction wheelAction) + mouseButtons = new Dictionary(); + var mouseButtonNames = Enum.GetNames(typeof(MouseButton)); + var mouseButtonValues = (MouseButton[])Enum.GetValues(typeof(MouseButton)); + for (int i = 0; i < mouseButtonNames.Length; ++i) { - return mouseWheel[wheelAction]; + mouseButtons.TryAdd(mouseButtonValues[i], mouseButtonNames[i]); } - - static ControlStrings() + mouseWheel = new Dictionary(); + var wheelNames = Enum.GetNames(typeof(MouseWheelAction)); + var wheelValues = (MouseWheelAction[])Enum.GetValues(typeof(MouseWheelAction)); + for (int i = 0; i < wheelNames.Length; ++i) { - keys = new Dictionary(); - var keyNames = Enum.GetNames(typeof(Key)); - var keyValues = (Key[])Enum.GetValues(typeof(Key)); - for (int i = 0; i < keyNames.Length; ++i) - { - keys.TryAdd(keyValues[i], keyNames[i]); - } - mouseButtons = new Dictionary(); - var mouseButtonNames = Enum.GetNames(typeof(MouseButton)); - var mouseButtonValues = (MouseButton[])Enum.GetValues(typeof(MouseButton)); - for (int i = 0; i < mouseButtonNames.Length; ++i) - { - mouseButtons.TryAdd(mouseButtonValues[i], mouseButtonNames[i]); - } - mouseWheel = new Dictionary(); - var wheelNames = Enum.GetNames(typeof(MouseWheelAction)); - var wheelValues = (MouseWheelAction[])Enum.GetValues(typeof(MouseWheelAction)); - for (int i = 0; i < wheelNames.Length; ++i) - { - mouseWheel.TryAdd(wheelValues[i], wheelNames[i]); - } + mouseWheel.TryAdd(wheelValues[i], wheelNames[i]); } } +} - public enum HoldableControlType - { - None, - Key, - MouseButton, - } - /// - /// A control binding which can be held for multiple frames. - /// - [StructLayout(LayoutKind.Explicit)] - public struct HoldableBind - { - [FieldOffset(0)] - public Key Key; - [FieldOffset(0)] - public MouseButton Button; - [FieldOffset(4)] - public HoldableControlType Type; +public enum HoldableControlType +{ + None, + Key, + MouseButton, +} +/// +/// A control binding which can be held for multiple frames. +/// +[StructLayout(LayoutKind.Explicit)] +public struct HoldableBind +{ + [FieldOffset(0)] + public Key Key; + [FieldOffset(0)] + public MouseButton Button; + [FieldOffset(4)] + public HoldableControlType Type; - [FieldOffset(8)] - public Key AlternativeKey; - [FieldOffset(8)] - public MouseButton AlternativeButton; - [FieldOffset(12)] - public HoldableControlType AlternativeType; + [FieldOffset(8)] + public Key AlternativeKey; + [FieldOffset(8)] + public MouseButton AlternativeButton; + [FieldOffset(12)] + public HoldableControlType AlternativeType; - public HoldableBind(Key key) - : this() - { - Key = key; - Type = HoldableControlType.Key; - } - public HoldableBind(Key key, Key alternativeKey) - : this() - { - Key = key; - Type = HoldableControlType.Key; - AlternativeKey = alternativeKey; - AlternativeType = HoldableControlType.Key; - } - public HoldableBind(Key key, MouseButton alternativeButton) - : this() - { - Key = key; - Type = HoldableControlType.Key; - AlternativeButton = alternativeButton; - AlternativeType = HoldableControlType.MouseButton; - } - public HoldableBind(MouseButton button) - : this() - { - Button = button; - Type = HoldableControlType.MouseButton; - } - public HoldableBind(MouseButton button, Key alternativeKey) - : this() - { - Button = button; - Type = HoldableControlType.MouseButton; - AlternativeKey = alternativeKey; - AlternativeType = HoldableControlType.Key; - } - public HoldableBind(MouseButton button, MouseButton alternativeButton) - : this() - { - Button = button; - Type = HoldableControlType.MouseButton; - AlternativeButton = alternativeButton; - AlternativeType = HoldableControlType.MouseButton; - } + public HoldableBind(Key key) + : this() + { + Key = key; + Type = HoldableControlType.Key; + } + public HoldableBind(Key key, Key alternativeKey) + : this() + { + Key = key; + Type = HoldableControlType.Key; + AlternativeKey = alternativeKey; + AlternativeType = HoldableControlType.Key; + } + public HoldableBind(Key key, MouseButton alternativeButton) + : this() + { + Key = key; + Type = HoldableControlType.Key; + AlternativeButton = alternativeButton; + AlternativeType = HoldableControlType.MouseButton; + } + public HoldableBind(MouseButton button) + : this() + { + Button = button; + Type = HoldableControlType.MouseButton; + } + public HoldableBind(MouseButton button, Key alternativeKey) + : this() + { + Button = button; + Type = HoldableControlType.MouseButton; + AlternativeKey = alternativeKey; + AlternativeType = HoldableControlType.Key; + } + public HoldableBind(MouseButton button, MouseButton alternativeButton) + : this() + { + Button = button; + Type = HoldableControlType.MouseButton; + AlternativeButton = alternativeButton; + AlternativeType = HoldableControlType.MouseButton; + } - public static implicit operator HoldableBind(Key key) - { - return new HoldableBind(key); - } - public static implicit operator HoldableBind((Key, Key) binds) - { - return new HoldableBind(binds.Item1, binds.Item2); - } - public static implicit operator HoldableBind((Key, MouseButton) binds) - { - return new HoldableBind(binds.Item1, binds.Item2); - } - public static implicit operator HoldableBind(MouseButton button) - { - return new HoldableBind(button); - } - public static implicit operator HoldableBind((MouseButton, Key) binds) - { - return new HoldableBind(binds.Item1, binds.Item2); - } - public static implicit operator HoldableBind((MouseButton, MouseButton) binds) - { - return new HoldableBind(binds.Item1, binds.Item2); - } - public bool IsDown(Input input) - { - if (Type == HoldableControlType.Key && input.IsDown(Key)) - return true; - if (Type == HoldableControlType.MouseButton && input.IsDown(Button)) - return true; - if (AlternativeType == HoldableControlType.Key && input.IsDown(AlternativeKey)) - return true; - if (AlternativeType == HoldableControlType.MouseButton && input.IsDown(AlternativeButton)) - return true; - return false; - } + public static implicit operator HoldableBind(Key key) + { + return new HoldableBind(key); + } + public static implicit operator HoldableBind((Key, Key) binds) + { + return new HoldableBind(binds.Item1, binds.Item2); + } + public static implicit operator HoldableBind((Key, MouseButton) binds) + { + return new HoldableBind(binds.Item1, binds.Item2); + } + public static implicit operator HoldableBind(MouseButton button) + { + return new HoldableBind(button); + } + public static implicit operator HoldableBind((MouseButton, Key) binds) + { + return new HoldableBind(binds.Item1, binds.Item2); + } + public static implicit operator HoldableBind((MouseButton, MouseButton) binds) + { + return new HoldableBind(binds.Item1, binds.Item2); + } + public bool IsDown(Input input) + { + if (Type == HoldableControlType.Key && input.IsDown(Key)) + return true; + if (Type == HoldableControlType.MouseButton && input.IsDown(Button)) + return true; + if (AlternativeType == HoldableControlType.Key && input.IsDown(AlternativeKey)) + return true; + if (AlternativeType == HoldableControlType.MouseButton && input.IsDown(AlternativeButton)) + return true; + return false; + } - public bool WasPushed(Input input) - { - if (Type == HoldableControlType.Key && input.WasPushed(Key)) - return true; - if (Type == HoldableControlType.MouseButton && input.WasPushed(Button)) - return true; - if (AlternativeType == HoldableControlType.Key && input.WasPushed(AlternativeKey)) - return true; - if (AlternativeType == HoldableControlType.MouseButton && input.WasPushed(AlternativeButton)) - return true; - return false; - } + public bool WasPushed(Input input) + { + if (Type == HoldableControlType.Key && input.WasPushed(Key)) + return true; + if (Type == HoldableControlType.MouseButton && input.WasPushed(Button)) + return true; + if (AlternativeType == HoldableControlType.Key && input.WasPushed(AlternativeKey)) + return true; + if (AlternativeType == HoldableControlType.MouseButton && input.WasPushed(AlternativeButton)) + return true; + return false; + } - public TextBuilder AppendString(TextBuilder text) - { - if (Type == HoldableControlType.Key) - text.Append(ControlStrings.GetName(Key)); - else if (Type == HoldableControlType.MouseButton) + public TextBuilder AppendString(TextBuilder text) + { + if (Type == HoldableControlType.Key) + text.Append(ControlStrings.GetName(Key)); + else if (Type == HoldableControlType.MouseButton) + text.Append(ControlStrings.GetName(Button)); + if (AlternativeType != HoldableControlType.None) + { + if (Type != HoldableControlType.None) + text.Append(" or "); + if (AlternativeType == HoldableControlType.Key) + text.Append(ControlStrings.GetName(AlternativeKey)); + else if (AlternativeType == HoldableControlType.MouseButton) text.Append(ControlStrings.GetName(Button)); - if (AlternativeType != HoldableControlType.None) - { - if (Type != HoldableControlType.None) - text.Append(" or "); - if (AlternativeType == HoldableControlType.Key) - text.Append(ControlStrings.GetName(AlternativeKey)); - else if (AlternativeType == HoldableControlType.MouseButton) - text.Append(ControlStrings.GetName(Button)); - } - return text; } - + return text; } - public enum InstantControlType - { - None, - Key, - MouseButton, - MouseWheel, - } - public enum MouseWheelAction - { - ScrollUp, - ScrollDown - } - /// - /// A control binding that supports any form of instant action, but may or may not support being held. - /// - [StructLayout(LayoutKind.Explicit)] - public struct InstantBind - { - [FieldOffset(0)] - public Key Key; - [FieldOffset(0)] - public MouseButton Button; - [FieldOffset(0)] - public MouseWheelAction Wheel; - [FieldOffset(4)] - public InstantControlType Type; +} - [FieldOffset(8)] - public Key AlternativeKey; - [FieldOffset(8)] - public MouseButton AlternativeButton; - [FieldOffset(8)] - public MouseWheelAction AlternativeWheel; - [FieldOffset(12)] - public InstantControlType AlternativeType; +public enum InstantControlType +{ + None, + Key, + MouseButton, + MouseWheel, +} +public enum MouseWheelAction +{ + ScrollUp, + ScrollDown +} +/// +/// A control binding that supports any form of instant action, but may or may not support being held. +/// +[StructLayout(LayoutKind.Explicit)] +public struct InstantBind +{ + [FieldOffset(0)] + public Key Key; + [FieldOffset(0)] + public MouseButton Button; + [FieldOffset(0)] + public MouseWheelAction Wheel; + [FieldOffset(4)] + public InstantControlType Type; - public InstantBind(Key key) - : this() - { - Key = key; - Type = InstantControlType.Key; - } - public InstantBind(Key key, Key alternativeKey) - : this() - { - Key = key; - Type = InstantControlType.Key; - AlternativeKey = alternativeKey; - AlternativeType = InstantControlType.Key; - } - public InstantBind(Key key, MouseButton button) - : this() - { - Key = key; - Type = InstantControlType.Key; - AlternativeButton = button; - AlternativeType = InstantControlType.MouseButton; - } - public InstantBind(Key key, MouseWheelAction wheelAction) - : this() - { - Key = key; - Type = InstantControlType.Key; - AlternativeWheel = wheelAction; - AlternativeType = InstantControlType.MouseWheel; - } + [FieldOffset(8)] + public Key AlternativeKey; + [FieldOffset(8)] + public MouseButton AlternativeButton; + [FieldOffset(8)] + public MouseWheelAction AlternativeWheel; + [FieldOffset(12)] + public InstantControlType AlternativeType; - public InstantBind(MouseButton button) - : this() - { - Button = button; - Type = InstantControlType.MouseButton; - } - public InstantBind(MouseButton button, Key alternativeKey) - : this() - { - Button = button; - Type = InstantControlType.MouseButton; - AlternativeKey = alternativeKey; - AlternativeType = InstantControlType.Key; - } - public InstantBind(MouseButton button, MouseButton alternativeButton) - : this() - { - Button = button; - Type = InstantControlType.MouseButton; - AlternativeButton = alternativeButton; - AlternativeType = InstantControlType.MouseButton; - } - public InstantBind(MouseButton button, MouseWheelAction alternativeWheel) - : this() - { - Button = button; - Type = InstantControlType.MouseButton; - AlternativeWheel = alternativeWheel; - AlternativeType = InstantControlType.MouseWheel; - } + public InstantBind(Key key) + : this() + { + Key = key; + Type = InstantControlType.Key; + } + public InstantBind(Key key, Key alternativeKey) + : this() + { + Key = key; + Type = InstantControlType.Key; + AlternativeKey = alternativeKey; + AlternativeType = InstantControlType.Key; + } + public InstantBind(Key key, MouseButton button) + : this() + { + Key = key; + Type = InstantControlType.Key; + AlternativeButton = button; + AlternativeType = InstantControlType.MouseButton; + } + public InstantBind(Key key, MouseWheelAction wheelAction) + : this() + { + Key = key; + Type = InstantControlType.Key; + AlternativeWheel = wheelAction; + AlternativeType = InstantControlType.MouseWheel; + } - public InstantBind(MouseWheelAction wheel) - : this() - { - Wheel = wheel; - Type = InstantControlType.MouseWheel; - } - public InstantBind(MouseWheelAction wheel, Key alternativeKey) - : this() - { - Wheel = wheel; - Type = InstantControlType.MouseWheel; - AlternativeKey = alternativeKey; - AlternativeType = InstantControlType.Key; - } - public InstantBind(MouseWheelAction wheel, MouseButton alternativeButton) - : this() - { - Wheel = wheel; - Type = InstantControlType.MouseWheel; - AlternativeButton = alternativeButton; - AlternativeType = InstantControlType.MouseButton; - } - public InstantBind(MouseWheelAction wheel, MouseWheelAction alternativeWheel) - : this() - { - Wheel = wheel; - Type = InstantControlType.MouseWheel; - AlternativeWheel = alternativeWheel; - AlternativeType = InstantControlType.MouseWheel; - } + public InstantBind(MouseButton button) + : this() + { + Button = button; + Type = InstantControlType.MouseButton; + } + public InstantBind(MouseButton button, Key alternativeKey) + : this() + { + Button = button; + Type = InstantControlType.MouseButton; + AlternativeKey = alternativeKey; + AlternativeType = InstantControlType.Key; + } + public InstantBind(MouseButton button, MouseButton alternativeButton) + : this() + { + Button = button; + Type = InstantControlType.MouseButton; + AlternativeButton = alternativeButton; + AlternativeType = InstantControlType.MouseButton; + } + public InstantBind(MouseButton button, MouseWheelAction alternativeWheel) + : this() + { + Button = button; + Type = InstantControlType.MouseButton; + AlternativeWheel = alternativeWheel; + AlternativeType = InstantControlType.MouseWheel; + } - public static implicit operator InstantBind(Key key) - { - return new InstantBind(key); - } - public static implicit operator InstantBind((Key, Key) binds) - { - return new InstantBind(binds.Item1, binds.Item2); - } - public static implicit operator InstantBind((Key, MouseButton) binds) - { - return new InstantBind(binds.Item1, binds.Item2); - } - public static implicit operator InstantBind((Key, MouseWheelAction) binds) - { - return new InstantBind(binds.Item1, binds.Item2); - } + public InstantBind(MouseWheelAction wheel) + : this() + { + Wheel = wheel; + Type = InstantControlType.MouseWheel; + } + public InstantBind(MouseWheelAction wheel, Key alternativeKey) + : this() + { + Wheel = wheel; + Type = InstantControlType.MouseWheel; + AlternativeKey = alternativeKey; + AlternativeType = InstantControlType.Key; + } + public InstantBind(MouseWheelAction wheel, MouseButton alternativeButton) + : this() + { + Wheel = wheel; + Type = InstantControlType.MouseWheel; + AlternativeButton = alternativeButton; + AlternativeType = InstantControlType.MouseButton; + } + public InstantBind(MouseWheelAction wheel, MouseWheelAction alternativeWheel) + : this() + { + Wheel = wheel; + Type = InstantControlType.MouseWheel; + AlternativeWheel = alternativeWheel; + AlternativeType = InstantControlType.MouseWheel; + } - public static implicit operator InstantBind(MouseButton button) - { - return new InstantBind(button); - } - public static implicit operator InstantBind((MouseButton, Key) binds) - { - return new InstantBind(binds.Item1, binds.Item2); - } - public static implicit operator InstantBind((MouseButton, MouseButton) binds) - { - return new InstantBind(binds.Item1, binds.Item2); - } - public static implicit operator InstantBind((MouseButton, MouseWheelAction) binds) - { - return new InstantBind(binds.Item1, binds.Item2); - } + public static implicit operator InstantBind(Key key) + { + return new InstantBind(key); + } + public static implicit operator InstantBind((Key, Key) binds) + { + return new InstantBind(binds.Item1, binds.Item2); + } + public static implicit operator InstantBind((Key, MouseButton) binds) + { + return new InstantBind(binds.Item1, binds.Item2); + } + public static implicit operator InstantBind((Key, MouseWheelAction) binds) + { + return new InstantBind(binds.Item1, binds.Item2); + } - public static implicit operator InstantBind(MouseWheelAction wheel) - { - return new InstantBind(wheel); - } - public static implicit operator InstantBind((MouseWheelAction, Key) binds) - { - return new InstantBind(binds.Item1, binds.Item2); - } - public static implicit operator InstantBind((MouseWheelAction, MouseButton) binds) - { - return new InstantBind(binds.Item1, binds.Item2); - } - public static implicit operator InstantBind((MouseWheelAction, MouseWheelAction) binds) + public static implicit operator InstantBind(MouseButton button) + { + return new InstantBind(button); + } + public static implicit operator InstantBind((MouseButton, Key) binds) + { + return new InstantBind(binds.Item1, binds.Item2); + } + public static implicit operator InstantBind((MouseButton, MouseButton) binds) + { + return new InstantBind(binds.Item1, binds.Item2); + } + public static implicit operator InstantBind((MouseButton, MouseWheelAction) binds) + { + return new InstantBind(binds.Item1, binds.Item2); + } + + public static implicit operator InstantBind(MouseWheelAction wheel) + { + return new InstantBind(wheel); + } + public static implicit operator InstantBind((MouseWheelAction, Key) binds) + { + return new InstantBind(binds.Item1, binds.Item2); + } + public static implicit operator InstantBind((MouseWheelAction, MouseButton) binds) + { + return new InstantBind(binds.Item1, binds.Item2); + } + public static implicit operator InstantBind((MouseWheelAction, MouseWheelAction) binds) + { + return new InstantBind(binds.Item1, binds.Item2); + } + + public bool WasTriggered(Input input) + { + switch (Type) + { + case InstantControlType.Key: + if (input.WasPushed(Key)) return true; + break; + case InstantControlType.MouseButton: + if (input.WasPushed(Button)) return true; + break; + case InstantControlType.MouseWheel: + if (Wheel == MouseWheelAction.ScrollUp ? input.ScrolledUp > 0 : input.ScrolledDown < 0) return true; + break; + } + switch (AlternativeType) + { + case InstantControlType.Key: + return input.WasPushed(AlternativeKey); + case InstantControlType.MouseButton: + return input.WasPushed(AlternativeButton); + case InstantControlType.MouseWheel: + return AlternativeWheel == MouseWheelAction.ScrollUp ? input.ScrolledUp > 0 : input.ScrolledDown < 0; + } + return false; + } + + public TextBuilder AppendString(TextBuilder text) + { + if (Type == InstantControlType.None && AlternativeType == InstantControlType.None) + text.Append("(no bind)"); + switch (Type) { - return new InstantBind(binds.Item1, binds.Item2); + case InstantControlType.Key: + text.Append(ControlStrings.GetName(Key)); + break; + case InstantControlType.MouseButton: + text.Append(ControlStrings.GetName(Button)); + break; + case InstantControlType.MouseWheel: + text.Append(ControlStrings.GetName(Wheel)); + break; } - - public bool WasTriggered(Input input) + if (AlternativeType != InstantControlType.None) { - switch (Type) + if (Type != InstantControlType.None) { - case InstantControlType.Key: - if (input.WasPushed(Key)) return true; - break; - case InstantControlType.MouseButton: - if (input.WasPushed(Button)) return true; - break; - case InstantControlType.MouseWheel: - if (Wheel == MouseWheelAction.ScrollUp ? input.ScrolledUp > 0 : input.ScrolledDown < 0) return true; - break; + text.Append(" or "); } switch (AlternativeType) { case InstantControlType.Key: - return input.WasPushed(AlternativeKey); - case InstantControlType.MouseButton: - return input.WasPushed(AlternativeButton); - case InstantControlType.MouseWheel: - return AlternativeWheel == MouseWheelAction.ScrollUp ? input.ScrolledUp > 0 : input.ScrolledDown < 0; - } - return false; - } - - public TextBuilder AppendString(TextBuilder text) - { - if (Type == InstantControlType.None && AlternativeType == InstantControlType.None) - text.Append("(no bind)"); - switch (Type) - { - case InstantControlType.Key: - text.Append(ControlStrings.GetName(Key)); + text.Append(ControlStrings.GetName(AlternativeKey)); break; case InstantControlType.MouseButton: - text.Append(ControlStrings.GetName(Button)); + text.Append(ControlStrings.GetName(AlternativeButton)); break; case InstantControlType.MouseWheel: - text.Append(ControlStrings.GetName(Wheel)); + text.Append(ControlStrings.GetName(AlternativeWheel)); break; } - if (AlternativeType != InstantControlType.None) - { - if (Type != InstantControlType.None) - { - text.Append(" or "); - } - switch (AlternativeType) - { - case InstantControlType.Key: - text.Append(ControlStrings.GetName(AlternativeKey)); - break; - case InstantControlType.MouseButton: - text.Append(ControlStrings.GetName(AlternativeButton)); - break; - case InstantControlType.MouseWheel: - text.Append(ControlStrings.GetName(AlternativeWheel)); - break; - } - } - return text; } + return text; } +} - public struct Controls - { - public HoldableBind MoveForward; - public HoldableBind MoveBackward; - public HoldableBind MoveLeft; - public HoldableBind MoveRight; - public HoldableBind MoveUp; - public HoldableBind MoveDown; - public InstantBind MoveSlower; - public InstantBind MoveFaster; - public HoldableBind Grab; - public HoldableBind GrabRotate; - public float MouseSensitivity; - public float CameraSlowMoveSpeed; - public float CameraMoveSpeed; - public float CameraFastMoveSpeed; +public struct Controls +{ + public HoldableBind MoveForward; + public HoldableBind MoveBackward; + public HoldableBind MoveLeft; + public HoldableBind MoveRight; + public HoldableBind MoveUp; + public HoldableBind MoveDown; + public InstantBind MoveSlower; + public InstantBind MoveFaster; + public HoldableBind Grab; + public HoldableBind GrabRotate; + public float MouseSensitivity; + public float CameraSlowMoveSpeed; + public float CameraMoveSpeed; + public float CameraFastMoveSpeed; - public HoldableBind SlowTimesteps; - public InstantBind LockMouse; - public InstantBind Exit; - public InstantBind ShowConstraints; - public InstantBind ShowContacts; - public InstantBind ShowBoundingBoxes; - public InstantBind ChangeTimingDisplayMode; - public InstantBind ChangeDemo; - public InstantBind ShowControls; + public HoldableBind SlowTimesteps; + public InstantBind LockMouse; + public InstantBind Exit; + public InstantBind ShowConstraints; + public InstantBind ShowContacts; + public InstantBind ShowBoundingBoxes; + public InstantBind ChangeTimingDisplayMode; + public InstantBind ChangeDemo; + public InstantBind ShowControls; - public static Controls Default + public static Controls Default + { + get { - get + return new Controls { - return new Controls - { - MoveForward = Key.W, - MoveBackward = Key.S, - MoveLeft = Key.A, - MoveRight = Key.D, - MoveDown = Key.ControlLeft, - MoveUp = Key.ShiftLeft, - MoveSlower = (MouseWheelAction.ScrollDown, Key.Y), - MoveFaster = (MouseWheelAction.ScrollUp, Key.U), - Grab = MouseButton.Right, - GrabRotate = Key.Q, - MouseSensitivity = 1.5e-3f, - CameraSlowMoveSpeed = 0.5f, - CameraMoveSpeed = 5, - CameraFastMoveSpeed = 50, - SlowTimesteps = (MouseButton.Middle, Key.O), - - LockMouse = Key.Tab, - Exit = Key.Escape, - ShowConstraints = Key.J, - ShowContacts = Key.K, - ShowBoundingBoxes = Key.L, - ChangeTimingDisplayMode = Key.F2, - ChangeDemo = (Key.Tilde, Key.F3), - ShowControls = Key.F1, - }; - } + MoveForward = Key.W, + MoveBackward = Key.S, + MoveLeft = Key.A, + MoveRight = Key.D, + MoveDown = Key.ControlLeft, + MoveUp = Key.ShiftLeft, + MoveSlower = (MouseWheelAction.ScrollDown, Key.Y), + MoveFaster = (MouseWheelAction.ScrollUp, Key.U), + Grab = MouseButton.Right, + GrabRotate = Key.Q, + MouseSensitivity = 1.5e-3f, + CameraSlowMoveSpeed = 0.5f, + CameraMoveSpeed = 5, + CameraFastMoveSpeed = 50, + SlowTimesteps = (MouseButton.Middle, Key.O), + LockMouse = Key.Tab, + Exit = Key.Escape, + ShowConstraints = Key.J, + ShowContacts = Key.K, + ShowBoundingBoxes = Key.L, + ChangeTimingDisplayMode = Key.F2, + ChangeDemo = (Key.Tilde, Key.F3), + ShowControls = Key.F1, + }; } + } } diff --git a/Demos/Demo.cs b/Demos/Demo.cs index c65faca66..329db2071 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -7,114 +7,113 @@ using DemoContentLoader; using BepuUtilities; -namespace Demos +namespace Demos; + +public abstract class Demo : IDisposable { - public abstract class Demo : IDisposable + /// + /// Gets the simulation created by the demo's Initialize call. + /// + public Simulation Simulation { get; protected set; } + + //Note that the buffer pool used by the simulation is not considered to be *owned* by the simulation. The simulation merely uses the pool. + //Disposing the simulation will not dispose or clear the buffer pool. + /// + /// Gets the buffer pool used by the demo's simulation. + /// + public BufferPool BufferPool { get; private set; } + + /// + /// Gets the thread dispatcher available for use by the simulation. + /// + public ThreadDispatcher ThreadDispatcher { get; private set; } + + protected Demo() { - /// - /// Gets the simulation created by the demo's Initialize call. - /// - public Simulation Simulation { get; protected set; } - - //Note that the buffer pool used by the simulation is not considered to be *owned* by the simulation. The simulation merely uses the pool. - //Disposing the simulation will not dispose or clear the buffer pool. - /// - /// Gets the buffer pool used by the demo's simulation. - /// - public BufferPool BufferPool { get; private set; } - - /// - /// Gets the thread dispatcher available for use by the simulation. - /// - public ThreadDispatcher ThreadDispatcher { get; private set; } - - protected Demo() - { - BufferPool = new BufferPool(); - //Generally, shoving as many threads as possible into the simulation won't produce the best results on systems with multiple logical cores per physical core. - //Environment.ProcessorCount reports logical core count only, so we'll use a simple heuristic here- it'll leave one or two logical cores idle. - //For the common Intel quad core with hyperthreading, this'll use six logical cores and leave two logical cores free to be used for other stuff. - //This is by no means perfect. To maximize performance, you'll need to profile your simulation and target hardware. - //Note that issues can be magnified on older operating systems like Windows 7 if all logical cores are given work. - - //Generally, the more memory bandwidth you have relative to CPU compute throughput, and the more collision detection heavy the simulation is relative to solving, - //the more benefit you get out of SMT/hyperthreading. - //For example, if you're using the 64 core quad memory channel AMD 3990x on a scene composed of thousands of ragdolls, - //there won't be enough memory bandwidth to even feed half the physical cores. Using all 128 logical cores would just add overhead. - - //It may be worth using something like hwloc or CPUID to extract extra information to reason about. - var targetThreadCount = int.Max(1, Environment.ProcessorCount > 4 ? Environment.ProcessorCount - 2 : Environment.ProcessorCount - 1); - ThreadDispatcher = new ThreadDispatcher(targetThreadCount); - } + BufferPool = new BufferPool(); + //Generally, shoving as many threads as possible into the simulation won't produce the best results on systems with multiple logical cores per physical core. + //Environment.ProcessorCount reports logical core count only, so we'll use a simple heuristic here- it'll leave one or two logical cores idle. + //For the common Intel quad core with hyperthreading, this'll use six logical cores and leave two logical cores free to be used for other stuff. + //This is by no means perfect. To maximize performance, you'll need to profile your simulation and target hardware. + //Note that issues can be magnified on older operating systems like Windows 7 if all logical cores are given work. + + //Generally, the more memory bandwidth you have relative to CPU compute throughput, and the more collision detection heavy the simulation is relative to solving, + //the more benefit you get out of SMT/hyperthreading. + //For example, if you're using the 64 core quad memory channel AMD 3990x on a scene composed of thousands of ragdolls, + //there won't be enough memory bandwidth to even feed half the physical cores. Using all 128 logical cores would just add overhead. + + //It may be worth using something like hwloc or CPUID to extract extra information to reason about. + var targetThreadCount = int.Max(1, Environment.ProcessorCount > 4 ? Environment.ProcessorCount - 2 : Environment.ProcessorCount - 1); + ThreadDispatcher = new ThreadDispatcher(targetThreadCount); + } - public virtual void LoadGraphicalContent(ContentArchive content, RenderSurface surface) - { - } + public virtual void LoadGraphicalContent(ContentArchive content, RenderSurface surface) + { + } - public abstract void Initialize(ContentArchive content, Camera camera); + public abstract void Initialize(ContentArchive content, Camera camera); - public const float TimestepDuration = 1 / 60f; - public virtual void Update(Window window, Camera camera, Input input, float dt) - { - //In the demos, we use one time step per frame. We don't bother modifying the physics time step duration for different monitors so different refresh rates - //change the rate of simulation. This doesn't actually change the result of the simulation, though, and the simplicity is a good fit for the demos. - //In the context of a 'real' application, you could instead use a time accumulator to take time steps of fixed length as needed, or - //fully decouple simulation and rendering rates across different threads. - //(In either case, you'd also want to interpolate or extrapolate simulation results during rendering for smoothness.) - //Note that taking steps of variable length can reduce stability. Gradual or one-off changes can work reasonably well. - Simulation.Timestep(TimestepDuration, ThreadDispatcher); - - ////Here's an example of how it would look to use more frequent updates, but still with a fixed amount of time simulated per update call: - //const float timeToSimulate = 1 / 60f; - //const int timestepsPerUpdate = 2; - //const float timePerTimestep = timeToSimulate / timestepsPerUpdate; - //for (int i = 0; i < timestepsPerUpdate; ++i) - //{ - // Simulation.Timestep(timePerTimestep, ThreadDispatcher); - //} - - ////And here's an example of how to use an accumulator to take a number of timesteps of fixed length in response to variable update dt: - //timeAccumulator += dt; - //var targetTimestepDuration = 1 / 120f; - //while (timeAccumulator >= targetTimestepDuration) - //{ - // Simulation.Timestep(targetTimestepDuration, ThreadDispatcher); - // timeAccumulator -= targetTimestepDuration; - //} - ////If you wanted to smooth out the positions of rendered objects to avoid the 'jitter' that an unpredictable number of time steps per update would cause, - ////you can just interpolate the previous and current states using a weight based on the time remaining in the accumulator: - //var interpolationWeight = timeAccumulator / targetTimestepDuration; - } - //If you're using the accumulator-based timestep approach above, you'll need this field. - //float timeAccumulator; + public const float TimestepDuration = 1 / 60f; + public virtual void Update(Window window, Camera camera, Input input, float dt) + { + //In the demos, we use one time step per frame. We don't bother modifying the physics time step duration for different monitors so different refresh rates + //change the rate of simulation. This doesn't actually change the result of the simulation, though, and the simplicity is a good fit for the demos. + //In the context of a 'real' application, you could instead use a time accumulator to take time steps of fixed length as needed, or + //fully decouple simulation and rendering rates across different threads. + //(In either case, you'd also want to interpolate or extrapolate simulation results during rendering for smoothness.) + //Note that taking steps of variable length can reduce stability. Gradual or one-off changes can work reasonably well. + Simulation.Timestep(TimestepDuration, ThreadDispatcher); + + ////Here's an example of how it would look to use more frequent updates, but still with a fixed amount of time simulated per update call: + //const float timeToSimulate = 1 / 60f; + //const int timestepsPerUpdate = 2; + //const float timePerTimestep = timeToSimulate / timestepsPerUpdate; + //for (int i = 0; i < timestepsPerUpdate; ++i) + //{ + // Simulation.Timestep(timePerTimestep, ThreadDispatcher); + //} + + ////And here's an example of how to use an accumulator to take a number of timesteps of fixed length in response to variable update dt: + //timeAccumulator += dt; + //var targetTimestepDuration = 1 / 120f; + //while (timeAccumulator >= targetTimestepDuration) + //{ + // Simulation.Timestep(targetTimestepDuration, ThreadDispatcher); + // timeAccumulator -= targetTimestepDuration; + //} + ////If you wanted to smooth out the positions of rendered objects to avoid the 'jitter' that an unpredictable number of time steps per update would cause, + ////you can just interpolate the previous and current states using a weight based on the time remaining in the accumulator: + //var interpolationWeight = timeAccumulator / targetTimestepDuration; + } + //If you're using the accumulator-based timestep approach above, you'll need this field. + //float timeAccumulator; - public virtual void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) - { - } + public virtual void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + { + } - protected virtual void OnDispose() - { + protected virtual void OnDispose() + { - } + } - bool disposed; - public void Dispose() + bool disposed; + public void Dispose() + { + if (!disposed) { - if (!disposed) - { - disposed = true; - OnDispose(); - Simulation.Dispose(); - ThreadDispatcher.Dispose(); - BufferPool.Clear(); - } + disposed = true; + OnDispose(); + Simulation.Dispose(); + ThreadDispatcher.Dispose(); + BufferPool.Clear(); } + } #if DEBUG - ~Demo() - { - DemoRenderer.Helpers.CheckForUndisposed(disposed, this); - } -#endif + ~Demo() + { + DemoRenderer.Helpers.CheckForUndisposed(disposed, this); } +#endif } diff --git a/Demos/DemoCallbacks.cs b/Demos/DemoCallbacks.cs index ccf92db59..f2613bd5a 100644 --- a/Demos/DemoCallbacks.cs +++ b/Demos/DemoCallbacks.cs @@ -7,164 +7,162 @@ using System.Runtime.CompilerServices; using System.Numerics; -namespace Demos +namespace Demos; + +public struct DemoPoseIntegratorCallbacks : IPoseIntegratorCallbacks { - public struct DemoPoseIntegratorCallbacks : IPoseIntegratorCallbacks + /// + /// Gravity to apply to dynamic bodies in the simulation. + /// + public Vector3 Gravity; + /// + /// Fraction of dynamic body linear velocity to remove per unit of time. Values range from 0 to 1. 0 is fully undamped, while values very close to 1 will remove most velocity. + /// + public float LinearDamping; + /// + /// Fraction of dynamic body angular velocity to remove per unit of time. Values range from 0 to 1. 0 is fully undamped, while values very close to 1 will remove most velocity. + /// + public float AngularDamping; + + + /// + /// Gets how the pose integrator should handle angular velocity integration. + /// + public readonly AngularIntegrationMode AngularIntegrationMode => AngularIntegrationMode.Nonconserving; + + /// + /// Gets whether the integrator should use substepping for unconstrained bodies when using a substepping solver. + /// If true, unconstrained bodies will be integrated with the same number of substeps as the constrained bodies in the solver. + /// If false, unconstrained bodies use a single step of length equal to the dt provided to Simulation.Timestep. + /// + public readonly bool AllowSubstepsForUnconstrainedBodies => false; + + /// + /// Gets whether the velocity integration callback should be called for kinematic bodies. + /// If true, IntegrateVelocity will be called for bundles including kinematic bodies. + /// If false, kinematic bodies will just continue using whatever velocity they have set. + /// Most use cases should set this to false. + /// + public readonly bool IntegrateVelocityForKinematics => false; + + public void Initialize(Simulation simulation) { - /// - /// Gravity to apply to dynamic bodies in the simulation. - /// - public Vector3 Gravity; - /// - /// Fraction of dynamic body linear velocity to remove per unit of time. Values range from 0 to 1. 0 is fully undamped, while values very close to 1 will remove most velocity. - /// - public float LinearDamping; - /// - /// Fraction of dynamic body angular velocity to remove per unit of time. Values range from 0 to 1. 0 is fully undamped, while values very close to 1 will remove most velocity. - /// - public float AngularDamping; - - - /// - /// Gets how the pose integrator should handle angular velocity integration. - /// - public readonly AngularIntegrationMode AngularIntegrationMode => AngularIntegrationMode.Nonconserving; - - /// - /// Gets whether the integrator should use substepping for unconstrained bodies when using a substepping solver. - /// If true, unconstrained bodies will be integrated with the same number of substeps as the constrained bodies in the solver. - /// If false, unconstrained bodies use a single step of length equal to the dt provided to Simulation.Timestep. - /// - public readonly bool AllowSubstepsForUnconstrainedBodies => false; - - /// - /// Gets whether the velocity integration callback should be called for kinematic bodies. - /// If true, IntegrateVelocity will be called for bundles including kinematic bodies. - /// If false, kinematic bodies will just continue using whatever velocity they have set. - /// Most use cases should set this to false. - /// - public readonly bool IntegrateVelocityForKinematics => false; - - public void Initialize(Simulation simulation) - { - //In this demo, we don't need to initialize anything. - //If you had a simulation with per body gravity stored in a CollidableProperty or something similar, having the simulation provided in a callback can be helpful. - } - - /// - /// Creates a new set of simple callbacks for the demos. - /// - /// Gravity to apply to dynamic bodies in the simulation. - /// Fraction of dynamic body linear velocity to remove per unit of time. Values range from 0 to 1. 0 is fully undamped, while values very close to 1 will remove most velocity. - /// Fraction of dynamic body angular velocity to remove per unit of time. Values range from 0 to 1. 0 is fully undamped, while values very close to 1 will remove most velocity. - public DemoPoseIntegratorCallbacks(Vector3 gravity, float linearDamping = .03f, float angularDamping = .03f) : this() - { - Gravity = gravity; - LinearDamping = linearDamping; - AngularDamping = angularDamping; - } - - Vector3Wide gravityWideDt; - Vector linearDampingDt; - Vector angularDampingDt; - - /// - /// Callback invoked ahead of dispatches that may call into . - /// It may be called more than once with different values over a frame. For example, when performing bounding box prediction, velocity is integrated with a full frame time step duration. - /// During substepped solves, integration is split into substepCount steps, each with fullFrameDuration / substepCount duration. - /// The final integration pass for unconstrained bodies may be either fullFrameDuration or fullFrameDuration / substepCount, depending on the value of AllowSubstepsForUnconstrainedBodies. - /// - /// Current integration time step duration. - /// This is typically used for precomputing anything expensive that will be used across velocity integration. - public void PrepareForIntegration(float dt) - { - //No reason to recalculate gravity * dt for every body; just cache it ahead of time. - //Since these callbacks don't use per-body damping values, we can precalculate everything. - linearDampingDt = new Vector(MathF.Pow(MathHelper.Clamp(1 - LinearDamping, 0, 1), dt)); - angularDampingDt = new Vector(MathF.Pow(MathHelper.Clamp(1 - AngularDamping, 0, 1), dt)); - gravityWideDt = Vector3Wide.Broadcast(Gravity * dt); - } + //In this demo, we don't need to initialize anything. + //If you had a simulation with per body gravity stored in a CollidableProperty or something similar, having the simulation provided in a callback can be helpful. + } - /// - /// Callback for a bundle of bodies being integrated. - /// - /// Indices of the bodies being integrated in this bundle. - /// Current body positions. - /// Current body orientations. - /// Body's current local inertia. - /// Mask indicating which lanes are active in the bundle. Active lanes will contain 0xFFFFFFFF, inactive lanes will contain 0. - /// Index of the worker thread processing this bundle. - /// Durations to integrate the velocity over. Can vary over lanes. - /// Velocity of bodies in the bundle. Any changes to lanes which are not active by the integrationMask will be discarded. - public void IntegrateVelocity(Vector bodyIndices, Vector3Wide position, QuaternionWide orientation, BodyInertiaWide localInertia, Vector integrationMask, int workerIndex, Vector dt, ref BodyVelocityWide velocity) - { - //This is a handy spot to implement things like position dependent gravity or per-body damping. - //This implementation uses a single damping value for all bodies that allows it to be precomputed. - //We don't have to check for kinematics; IntegrateVelocityForKinematics returns false, so we'll never see them in this callback. - //Note that these are SIMD operations and "Wide" types. There are Vector.Count lanes of execution being evaluated simultaneously. - //The types are laid out in array-of-structures-of-arrays (AOSOA) format. That's because this function is frequently called from vectorized contexts within the solver. - //Transforming to "array of structures" (AOS) format for the callback and then back to AOSOA would involve a lot of overhead, so instead the callback works on the AOSOA representation directly. - velocity.Linear = (velocity.Linear + gravityWideDt) * linearDampingDt; - velocity.Angular = velocity.Angular * angularDampingDt; - } + /// + /// Creates a new set of simple callbacks for the demos. + /// + /// Gravity to apply to dynamic bodies in the simulation. + /// Fraction of dynamic body linear velocity to remove per unit of time. Values range from 0 to 1. 0 is fully undamped, while values very close to 1 will remove most velocity. + /// Fraction of dynamic body angular velocity to remove per unit of time. Values range from 0 to 1. 0 is fully undamped, while values very close to 1 will remove most velocity. + public DemoPoseIntegratorCallbacks(Vector3 gravity, float linearDamping = .03f, float angularDamping = .03f) : this() + { + Gravity = gravity; + LinearDamping = linearDamping; + AngularDamping = angularDamping; } - public struct DemoNarrowPhaseCallbacks : INarrowPhaseCallbacks + + Vector3Wide gravityWideDt; + Vector linearDampingDt; + Vector angularDampingDt; + + /// + /// Callback invoked ahead of dispatches that may call into . + /// It may be called more than once with different values over a frame. For example, when performing bounding box prediction, velocity is integrated with a full frame time step duration. + /// During substepped solves, integration is split into substepCount steps, each with fullFrameDuration / substepCount duration. + /// The final integration pass for unconstrained bodies may be either fullFrameDuration or fullFrameDuration / substepCount, depending on the value of AllowSubstepsForUnconstrainedBodies. + /// + /// Current integration time step duration. + /// This is typically used for precomputing anything expensive that will be used across velocity integration. + public void PrepareForIntegration(float dt) { - public SpringSettings ContactSpringiness; - public float MaximumRecoveryVelocity; - public float FrictionCoefficient; + //No reason to recalculate gravity * dt for every body; just cache it ahead of time. + //Since these callbacks don't use per-body damping values, we can precalculate everything. + linearDampingDt = new Vector(MathF.Pow(MathHelper.Clamp(1 - LinearDamping, 0, 1), dt)); + angularDampingDt = new Vector(MathF.Pow(MathHelper.Clamp(1 - AngularDamping, 0, 1), dt)); + gravityWideDt = Vector3Wide.Broadcast(Gravity * dt); + } - public DemoNarrowPhaseCallbacks(SpringSettings contactSpringiness, float maximumRecoveryVelocity = 2f, float frictionCoefficient = 1f) - { - ContactSpringiness = contactSpringiness; - MaximumRecoveryVelocity = maximumRecoveryVelocity; - FrictionCoefficient = frictionCoefficient; - } + /// + /// Callback for a bundle of bodies being integrated. + /// + /// Indices of the bodies being integrated in this bundle. + /// Current body positions. + /// Current body orientations. + /// Body's current local inertia. + /// Mask indicating which lanes are active in the bundle. Active lanes will contain 0xFFFFFFFF, inactive lanes will contain 0. + /// Index of the worker thread processing this bundle. + /// Durations to integrate the velocity over. Can vary over lanes. + /// Velocity of bodies in the bundle. Any changes to lanes which are not active by the integrationMask will be discarded. + public void IntegrateVelocity(Vector bodyIndices, Vector3Wide position, QuaternionWide orientation, BodyInertiaWide localInertia, Vector integrationMask, int workerIndex, Vector dt, ref BodyVelocityWide velocity) + { + //This is a handy spot to implement things like position dependent gravity or per-body damping. + //This implementation uses a single damping value for all bodies that allows it to be precomputed. + //We don't have to check for kinematics; IntegrateVelocityForKinematics returns false, so we'll never see them in this callback. + //Note that these are SIMD operations and "Wide" types. There are Vector.Count lanes of execution being evaluated simultaneously. + //The types are laid out in array-of-structures-of-arrays (AOSOA) format. That's because this function is frequently called from vectorized contexts within the solver. + //Transforming to "array of structures" (AOS) format for the callback and then back to AOSOA would involve a lot of overhead, so instead the callback works on the AOSOA representation directly. + velocity.Linear = (velocity.Linear + gravityWideDt) * linearDampingDt; + velocity.Angular = velocity.Angular * angularDampingDt; + } +} +public struct DemoNarrowPhaseCallbacks : INarrowPhaseCallbacks +{ + public SpringSettings ContactSpringiness; + public float MaximumRecoveryVelocity; + public float FrictionCoefficient; - public void Initialize(Simulation simulation) - { - //Use a default if the springiness value wasn't initialized... at least until struct field initializers are supported outside of previews. - if (ContactSpringiness.AngularFrequency == 0 && ContactSpringiness.TwiceDampingRatio == 0) - { - ContactSpringiness = new(30, 1); - MaximumRecoveryVelocity = 2f; - FrictionCoefficient = 1f; - } - } + public DemoNarrowPhaseCallbacks(SpringSettings contactSpringiness, float maximumRecoveryVelocity = 2f, float frictionCoefficient = 1f) + { + ContactSpringiness = contactSpringiness; + MaximumRecoveryVelocity = maximumRecoveryVelocity; + FrictionCoefficient = frictionCoefficient; + } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin) + public void Initialize(Simulation simulation) + { + //Use a default if the springiness value wasn't initialized... at least until struct field initializers are supported outside of previews. + if (ContactSpringiness.AngularFrequency == 0 && ContactSpringiness.TwiceDampingRatio == 0) { - //While the engine won't even try creating pairs between statics at all, it will ask about kinematic-kinematic pairs. - //Those pairs cannot emit constraints since both involved bodies have infinite inertia. Since most of the demos don't need - //to collect information about kinematic-kinematic pairs, we'll require that at least one of the bodies needs to be dynamic. - return a.Mobility == CollidableMobility.Dynamic || b.Mobility == CollidableMobility.Dynamic; + ContactSpringiness = new(30, 1); + MaximumRecoveryVelocity = 2f; + FrictionCoefficient = 1f; } + } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB) - { - return true; - } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin) + { + //While the engine won't even try creating pairs between statics at all, it will ask about kinematic-kinematic pairs. + //Those pairs cannot emit constraints since both involved bodies have infinite inertia. Since most of the demos don't need + //to collect information about kinematic-kinematic pairs, we'll require that at least one of the bodies needs to be dynamic. + return a.Mobility == CollidableMobility.Dynamic || b.Mobility == CollidableMobility.Dynamic; + } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold - { - pairMaterial.FrictionCoefficient = FrictionCoefficient; - pairMaterial.MaximumRecoveryVelocity = MaximumRecoveryVelocity; - pairMaterial.SpringSettings = ContactSpringiness; - return true; - } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB) + { + return true; + } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold) - { - return true; - } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold + { + pairMaterial.FrictionCoefficient = FrictionCoefficient; + pairMaterial.MaximumRecoveryVelocity = MaximumRecoveryVelocity; + pairMaterial.SpringSettings = ContactSpringiness; + return true; + } - public void Dispose() - { - } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold) + { + return true; } + public void Dispose() + { + } } diff --git a/Demos/DemoHarness.cs b/Demos/DemoHarness.cs index 56dab83e1..a8de0fae2 100644 --- a/Demos/DemoHarness.cs +++ b/Demos/DemoHarness.cs @@ -7,430 +7,429 @@ using System; using System.Numerics; -namespace Demos +namespace Demos; + +public class DemoHarness : IDisposable { - public class DemoHarness : IDisposable + internal GameLoop loop; + ContentArchive content; + Grabber grabber; + internal Controls controls; + Font font; + + bool showControls; + bool showConstraints = true; + bool showContacts; + bool showBoundingBoxes; + int frameCount; + + enum TimingDisplayMode { - internal GameLoop loop; - ContentArchive content; - Grabber grabber; - internal Controls controls; - Font font; - - bool showControls; - bool showConstraints = true; - bool showContacts; - bool showBoundingBoxes; - int frameCount; - - enum TimingDisplayMode - { - Regular, - Big, - Minimized - } + Regular, + Big, + Minimized + } - TimingDisplayMode timingDisplayMode; - Graph timingGraph; + TimingDisplayMode timingDisplayMode; + Graph timingGraph; - DemoSwapper swapper; - internal DemoSet demoSet; - Demo demo; - internal void TryChangeToDemo(int demoIndex) + DemoSwapper swapper; + internal DemoSet demoSet; + Demo demo; + internal void TryChangeToDemo(int demoIndex) + { + if (demoIndex >= 0 && demoIndex < demoSet.Count) { - if (demoIndex >= 0 && demoIndex < demoSet.Count) - { - demo.Dispose(); - demo = demoSet.Build(demoIndex, content, loop.Camera, loop.Surface); - //Forcing a full blocking collection makes it a little easier to distinguish some memory issues. - GC.Collect(int.MaxValue, GCCollectionMode.Forced, true, true); - } + demo.Dispose(); + demo = demoSet.Build(demoIndex, content, loop.Camera, loop.Surface); + //Forcing a full blocking collection makes it a little easier to distinguish some memory issues. + GC.Collect(int.MaxValue, GCCollectionMode.Forced, true, true); } + } - SimulationTimeSamples timeSamples; + SimulationTimeSamples timeSamples; - public DemoHarness(GameLoop loop, ContentArchive content, - Controls? controls = null) - { - this.loop = loop; - this.content = content; - timeSamples = new SimulationTimeSamples(512, loop.Pool); - if (controls == null) - this.controls = Controls.Default; - - var fontContent = content.Load(@"Content\Carlito-Regular.ttf"); - font = new Font( + public DemoHarness(GameLoop loop, ContentArchive content, + Controls? controls = null) + { + this.loop = loop; + this.content = content; + timeSamples = new SimulationTimeSamples(512, loop.Pool); + if (controls == null) + this.controls = Controls.Default; + + var fontContent = content.Load(@"Content\Carlito-Regular.ttf"); + font = new Font( #if !OPENGL - loop.Surface.Device, loop.Surface.Context, + loop.Surface.Device, loop.Surface.Context, #endif - fontContent - ); - - timingGraph = new Graph(new GraphDescription - { - BodyLineColor = new Vector3(1, 1, 1), - AxisLabelHeight = 16, - AxisLineRadius = 0.5f, - HorizontalAxisLabel = "Frames", - VerticalAxisLabel = "Time (ms)", - VerticalIntervalValueScale = 1e3f, - VerticalIntervalLabelRounding = 2, - BackgroundLineRadius = 0.125f, - IntervalTextHeight = 12, - IntervalTickRadius = 0.25f, - IntervalTickLength = 6f, - TargetHorizontalTickCount = 5, - HorizontalTickTextPadding = 0, - VerticalTickTextPadding = 3, - - LegendMinimum = new Vector2(20, 200), - LegendNameHeight = 12, - LegendLineLength = 7, - - TextColor = new Vector3(1, 1, 1), - Font = font, - - LineSpacingMultiplier = 1f, - - ForceVerticalAxisMinimumToZero = true - }); - timingGraph.AddSeries("Total", new Vector3(1, 1, 1), 0.75f, timeSamples.Simulation); - timingGraph.AddSeries("Pose Integrator", new Vector3(0, 0, 1), 0.25f, timeSamples.PoseIntegrator); - timingGraph.AddSeries("Sleeper", new Vector3(0.5f, 0, 1), 0.25f, timeSamples.Sleeper); - timingGraph.AddSeries("Broad Update", new Vector3(1, 1, 0), 0.25f, timeSamples.BroadPhaseUpdate); - timingGraph.AddSeries("Collision Test", new Vector3(0, 1, 0), 0.25f, timeSamples.CollisionTesting); - timingGraph.AddSeries("Narrow Flush", new Vector3(1, 0, 1), 0.25f, timeSamples.NarrowPhaseFlush); - timingGraph.AddSeries("Solver", new Vector3(1, 0, 0), 0.5f, timeSamples.Solver); - timingGraph.AddSeries("Batch Compress", new Vector3(0, 0.5f, 0), 0.125f, timeSamples.BatchCompressor); - - demoSet = new DemoSet(); - demo = demoSet.Build(0, content, loop.Camera, loop.Surface); - - OnResize(loop.Window.Resolution); - } + fontContent + ); - private void UpdateTimingGraphForMode(TimingDisplayMode newDisplayMode) + timingGraph = new Graph(new GraphDescription { - timingDisplayMode = newDisplayMode; - ref var description = ref timingGraph.Description; - var resolution = loop.Window.Resolution; - switch (timingDisplayMode) - { - case TimingDisplayMode.Big: - { - const float inset = 150; - description.BodyMinimum = new Vector2(inset); - description.BodySpan = new Vector2(resolution.X, resolution.Y) - description.BodyMinimum - new Vector2(inset); - description.LegendMinimum = description.BodyMinimum - new Vector2(110, 0); - description.TargetVerticalTickCount = 5; - } - break; - case TimingDisplayMode.Regular: - { - const float inset = 50; - var targetSpan = new Vector2(400, 150); - description.BodyMinimum = new Vector2(resolution.X - targetSpan.X - inset, inset); - description.BodySpan = targetSpan; - description.LegendMinimum = description.BodyMinimum - new Vector2(130, 0); - description.TargetVerticalTickCount = 3; - } - break; - } - //In a minimized state, the graph is just not drawn. - } + BodyLineColor = new Vector3(1, 1, 1), + AxisLabelHeight = 16, + AxisLineRadius = 0.5f, + HorizontalAxisLabel = "Frames", + VerticalAxisLabel = "Time (ms)", + VerticalIntervalValueScale = 1e3f, + VerticalIntervalLabelRounding = 2, + BackgroundLineRadius = 0.125f, + IntervalTextHeight = 12, + IntervalTickRadius = 0.25f, + IntervalTickLength = 6f, + TargetHorizontalTickCount = 5, + HorizontalTickTextPadding = 0, + VerticalTickTextPadding = 3, + + LegendMinimum = new Vector2(20, 200), + LegendNameHeight = 12, + LegendLineLength = 7, + + TextColor = new Vector3(1, 1, 1), + Font = font, + + LineSpacingMultiplier = 1f, + + ForceVerticalAxisMinimumToZero = true + }); + timingGraph.AddSeries("Total", new Vector3(1, 1, 1), 0.75f, timeSamples.Simulation); + timingGraph.AddSeries("Pose Integrator", new Vector3(0, 0, 1), 0.25f, timeSamples.PoseIntegrator); + timingGraph.AddSeries("Sleeper", new Vector3(0.5f, 0, 1), 0.25f, timeSamples.Sleeper); + timingGraph.AddSeries("Broad Update", new Vector3(1, 1, 0), 0.25f, timeSamples.BroadPhaseUpdate); + timingGraph.AddSeries("Collision Test", new Vector3(0, 1, 0), 0.25f, timeSamples.CollisionTesting); + timingGraph.AddSeries("Narrow Flush", new Vector3(1, 0, 1), 0.25f, timeSamples.NarrowPhaseFlush); + timingGraph.AddSeries("Solver", new Vector3(1, 0, 0), 0.5f, timeSamples.Solver); + timingGraph.AddSeries("Batch Compress", new Vector3(0, 0.5f, 0), 0.125f, timeSamples.BatchCompressor); + + demoSet = new DemoSet(); + demo = demoSet.Build(0, content, loop.Camera, loop.Surface); + + OnResize(loop.Window.Resolution); + } - public void OnResize(Int2 resolution) + private void UpdateTimingGraphForMode(TimingDisplayMode newDisplayMode) + { + timingDisplayMode = newDisplayMode; + ref var description = ref timingGraph.Description; + var resolution = loop.Window.Resolution; + switch (timingDisplayMode) { - UpdateTimingGraphForMode(timingDisplayMode); + case TimingDisplayMode.Big: + { + const float inset = 150; + description.BodyMinimum = new Vector2(inset); + description.BodySpan = new Vector2(resolution.X, resolution.Y) - description.BodyMinimum - new Vector2(inset); + description.LegendMinimum = description.BodyMinimum - new Vector2(110, 0); + description.TargetVerticalTickCount = 5; + } + break; + case TimingDisplayMode.Regular: + { + const float inset = 50; + var targetSpan = new Vector2(400, 150); + description.BodyMinimum = new Vector2(resolution.X - targetSpan.X - inset, inset); + description.BodySpan = targetSpan; + description.LegendMinimum = description.BodyMinimum - new Vector2(130, 0); + description.TargetVerticalTickCount = 3; + } + break; } + //In a minimized state, the graph is just not drawn. + } - enum CameraMoveSpeedState - { - Regular, - Slow, - Fast - } - CameraMoveSpeedState cameraSpeedState; - Int2? grabberCachedMousePosition; + public void OnResize(Int2 resolution) + { + UpdateTimingGraphForMode(timingDisplayMode); + } - public void Update(float dt) + enum CameraMoveSpeedState + { + Regular, + Slow, + Fast + } + CameraMoveSpeedState cameraSpeedState; + Int2? grabberCachedMousePosition; + + public void Update(float dt) + { + //Don't bother responding to input if the window isn't focused. + var input = loop.Input; + var window = loop.Window; + var camera = loop.Camera; + if (loop.Window.Focused) { - //Don't bother responding to input if the window isn't focused. - var input = loop.Input; - var window = loop.Window; - var camera = loop.Camera; - if (loop.Window.Focused) + if (controls.Exit.WasTriggered(input)) { - if (controls.Exit.WasTriggered(input)) - { - window.Close(); - return; - } + window.Close(); + return; + } - if (controls.MoveFaster.WasTriggered(input)) + if (controls.MoveFaster.WasTriggered(input)) + { + switch (cameraSpeedState) { - switch (cameraSpeedState) - { - case CameraMoveSpeedState.Slow: - cameraSpeedState = CameraMoveSpeedState.Regular; - break; - case CameraMoveSpeedState.Regular: - cameraSpeedState = CameraMoveSpeedState.Fast; - break; - } + case CameraMoveSpeedState.Slow: + cameraSpeedState = CameraMoveSpeedState.Regular; + break; + case CameraMoveSpeedState.Regular: + cameraSpeedState = CameraMoveSpeedState.Fast; + break; } - if (controls.MoveSlower.WasTriggered(input)) + } + if (controls.MoveSlower.WasTriggered(input)) + { + switch (cameraSpeedState) { - switch (cameraSpeedState) - { - case CameraMoveSpeedState.Regular: - cameraSpeedState = CameraMoveSpeedState.Slow; - break; - case CameraMoveSpeedState.Fast: - cameraSpeedState = CameraMoveSpeedState.Regular; - break; - } + case CameraMoveSpeedState.Regular: + cameraSpeedState = CameraMoveSpeedState.Slow; + break; + case CameraMoveSpeedState.Fast: + cameraSpeedState = CameraMoveSpeedState.Regular; + break; } + } - var cameraOffset = new Vector3(); - if (controls.MoveForward.IsDown(input)) - cameraOffset += camera.Forward; - if (controls.MoveBackward.IsDown(input)) - cameraOffset += camera.Backward; - if (controls.MoveLeft.IsDown(input)) - cameraOffset += camera.Left; - if (controls.MoveRight.IsDown(input)) - cameraOffset += camera.Right; - if (controls.MoveUp.IsDown(input)) - cameraOffset += camera.Up; - if (controls.MoveDown.IsDown(input)) - cameraOffset += camera.Down; - var length = cameraOffset.Length(); - - if (length > 1e-7f) + var cameraOffset = new Vector3(); + if (controls.MoveForward.IsDown(input)) + cameraOffset += camera.Forward; + if (controls.MoveBackward.IsDown(input)) + cameraOffset += camera.Backward; + if (controls.MoveLeft.IsDown(input)) + cameraOffset += camera.Left; + if (controls.MoveRight.IsDown(input)) + cameraOffset += camera.Right; + if (controls.MoveUp.IsDown(input)) + cameraOffset += camera.Up; + if (controls.MoveDown.IsDown(input)) + cameraOffset += camera.Down; + var length = cameraOffset.Length(); + + if (length > 1e-7f) + { + float cameraMoveSpeed; + switch (cameraSpeedState) { - float cameraMoveSpeed; - switch (cameraSpeedState) - { - case CameraMoveSpeedState.Slow: - cameraMoveSpeed = controls.CameraSlowMoveSpeed; - break; - case CameraMoveSpeedState.Fast: - cameraMoveSpeed = controls.CameraFastMoveSpeed; - break; - default: - cameraMoveSpeed = controls.CameraMoveSpeed; - break; - } - cameraOffset *= dt * cameraMoveSpeed / length; + case CameraMoveSpeedState.Slow: + cameraMoveSpeed = controls.CameraSlowMoveSpeed; + break; + case CameraMoveSpeedState.Fast: + cameraMoveSpeed = controls.CameraFastMoveSpeed; + break; + default: + cameraMoveSpeed = controls.CameraMoveSpeed; + break; } - else - cameraOffset = new Vector3(); - camera.Position += cameraOffset; - - var grabRotationIsActive = controls.Grab.IsDown(input) && controls.GrabRotate.IsDown(input); + cameraOffset *= dt * cameraMoveSpeed / length; + } + else + cameraOffset = new Vector3(); + camera.Position += cameraOffset; - //Don't turn the camera while rotating a grabbed object. - if (!grabRotationIsActive) - { - if (input.MouseLocked) - { - var delta = input.MouseDelta; - if (delta.X != 0 || delta.Y != 0) - { - camera.Yaw += delta.X * controls.MouseSensitivity; - camera.Pitch += delta.Y * controls.MouseSensitivity; - } - } - } - if (controls.LockMouse.WasTriggered(input)) - { - input.MouseLocked = !input.MouseLocked; - } + var grabRotationIsActive = controls.Grab.IsDown(input) && controls.GrabRotate.IsDown(input); - Quaternion incrementalGrabRotation; - if (grabRotationIsActive) + //Don't turn the camera while rotating a grabbed object. + if (!grabRotationIsActive) + { + if (input.MouseLocked) { - if (grabberCachedMousePosition == null) - grabberCachedMousePosition = input.MousePosition; var delta = input.MouseDelta; - var yaw = delta.X * controls.MouseSensitivity; - var pitch = delta.Y * controls.MouseSensitivity; - incrementalGrabRotation = QuaternionEx.Concatenate(QuaternionEx.CreateFromAxisAngle(camera.Right, pitch), QuaternionEx.CreateFromAxisAngle(camera.Up, yaw)); - if (!input.MouseLocked) + if (delta.X != 0 || delta.Y != 0) { - //Undo the mouse movement if we're in freemouse mode. - input.MousePosition = grabberCachedMousePosition.Value; + camera.Yaw += delta.X * controls.MouseSensitivity; + camera.Pitch += delta.Y * controls.MouseSensitivity; } } - else + } + if (controls.LockMouse.WasTriggered(input)) + { + input.MouseLocked = !input.MouseLocked; + } + + Quaternion incrementalGrabRotation; + if (grabRotationIsActive) + { + if (grabberCachedMousePosition == null) + grabberCachedMousePosition = input.MousePosition; + var delta = input.MouseDelta; + var yaw = delta.X * controls.MouseSensitivity; + var pitch = delta.Y * controls.MouseSensitivity; + incrementalGrabRotation = QuaternionEx.Concatenate(QuaternionEx.CreateFromAxisAngle(camera.Right, pitch), QuaternionEx.CreateFromAxisAngle(camera.Up, yaw)); + if (!input.MouseLocked) { - incrementalGrabRotation = Quaternion.Identity; - grabberCachedMousePosition = null; + //Undo the mouse movement if we're in freemouse mode. + input.MousePosition = grabberCachedMousePosition.Value; } - grabber.Update(demo.Simulation, camera, input.MouseLocked, controls.Grab.IsDown(input), incrementalGrabRotation, window.GetNormalizedMousePosition(input.MousePosition)); + } + else + { + incrementalGrabRotation = Quaternion.Identity; + grabberCachedMousePosition = null; + } + grabber.Update(demo.Simulation, camera, input.MouseLocked, controls.Grab.IsDown(input), incrementalGrabRotation, window.GetNormalizedMousePosition(input.MousePosition)); - if (controls.ShowControls.WasTriggered(input)) - { - showControls = !showControls; - } + if (controls.ShowControls.WasTriggered(input)) + { + showControls = !showControls; + } - if (controls.ShowConstraints.WasTriggered(input)) - { - showConstraints = !showConstraints; - } - if (controls.ShowContacts.WasTriggered(input)) - { - showContacts = !showContacts; - } - if (controls.ShowBoundingBoxes.WasTriggered(input)) - { - showBoundingBoxes = !showBoundingBoxes; - } - if (controls.ChangeTimingDisplayMode.WasTriggered(input)) - { - var newDisplayMode = (int)timingDisplayMode + 1; - if (newDisplayMode > 2) - newDisplayMode = 0; - UpdateTimingGraphForMode((TimingDisplayMode)newDisplayMode); - } - swapper.CheckForDemoSwap(this); + if (controls.ShowConstraints.WasTriggered(input)) + { + showConstraints = !showConstraints; } - else + if (controls.ShowContacts.WasTriggered(input)) + { + showContacts = !showContacts; + } + if (controls.ShowBoundingBoxes.WasTriggered(input)) { - input.MouseLocked = false; + showBoundingBoxes = !showBoundingBoxes; } - ++frameCount; - if (!controls.SlowTimesteps.IsDown(input) || frameCount % 60 == 0) + if (controls.ChangeTimingDisplayMode.WasTriggered(input)) { - demo.Update(window, camera, input, dt); + var newDisplayMode = (int)timingDisplayMode + 1; + if (newDisplayMode > 2) + newDisplayMode = 0; + UpdateTimingGraphForMode((TimingDisplayMode)newDisplayMode); } - timeSamples.RecordFrame(demo.Simulation); + swapper.CheckForDemoSwap(this); } - - TextBuilder uiText = new TextBuilder(128); - public void Render(Renderer renderer) + else { - //Clear first so that any demo-specific logic doesn't get lost. - renderer.Shapes.ClearInstances(); - renderer.Lines.ClearInstances(); + input.MouseLocked = false; + } + ++frameCount; + if (!controls.SlowTimesteps.IsDown(input) || frameCount % 60 == 0) + { + demo.Update(window, camera, input, dt); + } + timeSamples.RecordFrame(demo.Simulation); + } - //Perform any demo-specific rendering first. - demo.Render(renderer, loop.Camera, loop.Input, uiText, font); + TextBuilder uiText = new TextBuilder(128); + public void Render(Renderer renderer) + { + //Clear first so that any demo-specific logic doesn't get lost. + renderer.Shapes.ClearInstances(); + renderer.Lines.ClearInstances(); + + //Perform any demo-specific rendering first. + demo.Render(renderer, loop.Camera, loop.Input, uiText, font); #if DEBUG - float warningHeight = 15f; - renderer.TextBatcher.Write(uiText.Clear().Append("Running in Debug configuration. Compile in Release configuration for performance testing."), - new Vector2((loop.Window.Resolution.X - GlyphBatch.MeasureLength(uiText, font, warningHeight)) * 0.5f, warningHeight), warningHeight, new Vector3(1, 0, 0), font); + float warningHeight = 15f; + renderer.TextBatcher.Write(uiText.Clear().Append("Running in Debug configuration. Compile in Release configuration for performance testing."), + new Vector2((loop.Window.Resolution.X - GlyphBatch.MeasureLength(uiText, font, warningHeight)) * 0.5f, warningHeight), warningHeight, new Vector3(1, 0, 0), font); #endif - float textHeight = 16; - float lineSpacing = textHeight * 1.0f; - var textColor = new Vector3(1, 1, 1); - if (showControls) - { - var penPosition = new Vector2(loop.Window.Resolution.X - textHeight * 6 - 25, loop.Window.Resolution.Y - 25); - penPosition.Y -= 19 * lineSpacing; - uiText.Clear().Append("Controls: "); - var headerHeight = textHeight * 1.2f; - renderer.TextBatcher.Write(uiText, penPosition - new Vector2(0.5f * GlyphBatch.MeasureLength(uiText, font, headerHeight), 0), headerHeight, textColor, font); - penPosition.Y += lineSpacing; - - var controlPosition = penPosition; - controlPosition.X += textHeight * 0.5f; - - void WriteInstantName(string controlName, InstantBind control) - { - uiText.Clear().Append(controlName).Append(":"); - renderer.TextBatcher.Write(uiText, penPosition - new Vector2(GlyphBatch.MeasureLength(uiText, font, textHeight), 0), textHeight, textColor, font); - penPosition.Y += lineSpacing; - - control.AppendString(uiText.Clear()); - renderer.TextBatcher.Write(uiText, controlPosition, textHeight, textColor, font); - controlPosition.Y += lineSpacing; - } - - void WriteHoldableName(string controlName, HoldableBind control) - { - uiText.Clear().Append(controlName).Append(":"); - renderer.TextBatcher.Write(uiText, penPosition - new Vector2(GlyphBatch.MeasureLength(uiText, font, textHeight), 0), textHeight, textColor, font); - penPosition.Y += lineSpacing; + float textHeight = 16; + float lineSpacing = textHeight * 1.0f; + var textColor = new Vector3(1, 1, 1); + if (showControls) + { + var penPosition = new Vector2(loop.Window.Resolution.X - textHeight * 6 - 25, loop.Window.Resolution.Y - 25); + penPosition.Y -= 19 * lineSpacing; + uiText.Clear().Append("Controls: "); + var headerHeight = textHeight * 1.2f; + renderer.TextBatcher.Write(uiText, penPosition - new Vector2(0.5f * GlyphBatch.MeasureLength(uiText, font, headerHeight), 0), headerHeight, textColor, font); + penPosition.Y += lineSpacing; - control.AppendString(uiText.Clear()); - renderer.TextBatcher.Write(uiText, controlPosition, textHeight, textColor, font); - controlPosition.Y += lineSpacing; - } + var controlPosition = penPosition; + controlPosition.X += textHeight * 0.5f; - WriteInstantName(nameof(controls.LockMouse), controls.LockMouse); - WriteHoldableName(nameof(controls.Grab), controls.Grab); - WriteHoldableName(nameof(controls.GrabRotate), controls.GrabRotate); - WriteHoldableName(nameof(controls.MoveForward), controls.MoveForward); - WriteHoldableName(nameof(controls.MoveBackward), controls.MoveBackward); - WriteHoldableName(nameof(controls.MoveLeft), controls.MoveLeft); - WriteHoldableName(nameof(controls.MoveRight), controls.MoveRight); - WriteHoldableName(nameof(controls.MoveUp), controls.MoveUp); - WriteHoldableName(nameof(controls.MoveDown), controls.MoveDown); - WriteInstantName(nameof(controls.MoveSlower), controls.MoveSlower); - WriteInstantName(nameof(controls.MoveFaster), controls.MoveFaster); - WriteHoldableName(nameof(controls.SlowTimesteps), controls.SlowTimesteps); - WriteInstantName(nameof(controls.Exit), controls.Exit); - WriteInstantName(nameof(controls.ShowConstraints), controls.ShowConstraints); - WriteInstantName(nameof(controls.ShowContacts), controls.ShowContacts); - WriteInstantName(nameof(controls.ShowBoundingBoxes), controls.ShowBoundingBoxes); - WriteInstantName(nameof(controls.ChangeTimingDisplayMode), controls.ChangeTimingDisplayMode); - WriteInstantName(nameof(controls.ChangeDemo), controls.ChangeDemo); - WriteInstantName(nameof(controls.ShowControls), controls.ShowControls); - } - else + void WriteInstantName(string controlName, InstantBind control) { - controls.ShowControls.AppendString(uiText.Clear().Append("Press ")).Append(" for controls."); - const float inset = 25; - renderer.TextBatcher.Write(uiText, - new Vector2(loop.Window.Resolution.X - inset - GlyphBatch.MeasureLength(uiText, font, textHeight), loop.Window.Resolution.Y - inset), - textHeight, textColor, font); - } - - swapper.Draw(uiText, renderer.TextBatcher, demoSet, new Vector2(16, 16), textHeight, textColor, font); + uiText.Clear().Append(controlName).Append(":"); + renderer.TextBatcher.Write(uiText, penPosition - new Vector2(GlyphBatch.MeasureLength(uiText, font, textHeight), 0), textHeight, textColor, font); + penPosition.Y += lineSpacing; - if (timingDisplayMode != TimingDisplayMode.Minimized) - { - timingGraph.Draw(uiText, renderer.UILineBatcher, renderer.TextBatcher); + control.AppendString(uiText.Clear()); + renderer.TextBatcher.Write(uiText, controlPosition, textHeight, textColor, font); + controlPosition.Y += lineSpacing; } - else + + void WriteHoldableName(string controlName, HoldableBind control) { - const float timingTextSize = 14; - const float inset = 25; - renderer.TextBatcher.Write( - uiText.Clear().Append(1e3 * timeSamples.Simulation[timeSamples.Simulation.End - 1], timingGraph.Description.VerticalIntervalLabelRounding).Append(" ms/step"), - new Vector2(loop.Window.Resolution.X - inset - GlyphBatch.MeasureLength(uiText, font, timingTextSize), inset), timingTextSize, timingGraph.Description.TextColor, font); + uiText.Clear().Append(controlName).Append(":"); + renderer.TextBatcher.Write(uiText, penPosition - new Vector2(GlyphBatch.MeasureLength(uiText, font, textHeight), 0), textHeight, textColor, font); + penPosition.Y += lineSpacing; + + control.AppendString(uiText.Clear()); + renderer.TextBatcher.Write(uiText, controlPosition, textHeight, textColor, font); + controlPosition.Y += lineSpacing; } - grabber.Draw(renderer.Lines, loop.Camera, loop.Input.MouseLocked, controls.Grab.IsDown(loop.Input), loop.Window.GetNormalizedMousePosition(loop.Input.MousePosition)); - renderer.Shapes.AddInstances(demo.Simulation, demo.ThreadDispatcher); - renderer.Lines.ShowConstraints = showConstraints; - renderer.Lines.ShowContacts = showContacts; - renderer.Lines.ShowBoundingBoxes = showBoundingBoxes; - renderer.Lines.Extract(demo.Simulation, demo.ThreadDispatcher); + + WriteInstantName(nameof(controls.LockMouse), controls.LockMouse); + WriteHoldableName(nameof(controls.Grab), controls.Grab); + WriteHoldableName(nameof(controls.GrabRotate), controls.GrabRotate); + WriteHoldableName(nameof(controls.MoveForward), controls.MoveForward); + WriteHoldableName(nameof(controls.MoveBackward), controls.MoveBackward); + WriteHoldableName(nameof(controls.MoveLeft), controls.MoveLeft); + WriteHoldableName(nameof(controls.MoveRight), controls.MoveRight); + WriteHoldableName(nameof(controls.MoveUp), controls.MoveUp); + WriteHoldableName(nameof(controls.MoveDown), controls.MoveDown); + WriteInstantName(nameof(controls.MoveSlower), controls.MoveSlower); + WriteInstantName(nameof(controls.MoveFaster), controls.MoveFaster); + WriteHoldableName(nameof(controls.SlowTimesteps), controls.SlowTimesteps); + WriteInstantName(nameof(controls.Exit), controls.Exit); + WriteInstantName(nameof(controls.ShowConstraints), controls.ShowConstraints); + WriteInstantName(nameof(controls.ShowContacts), controls.ShowContacts); + WriteInstantName(nameof(controls.ShowBoundingBoxes), controls.ShowBoundingBoxes); + WriteInstantName(nameof(controls.ChangeTimingDisplayMode), controls.ChangeTimingDisplayMode); + WriteInstantName(nameof(controls.ChangeDemo), controls.ChangeDemo); + WriteInstantName(nameof(controls.ShowControls), controls.ShowControls); + } + else + { + controls.ShowControls.AppendString(uiText.Clear().Append("Press ")).Append(" for controls."); + const float inset = 25; + renderer.TextBatcher.Write(uiText, + new Vector2(loop.Window.Resolution.X - inset - GlyphBatch.MeasureLength(uiText, font, textHeight), loop.Window.Resolution.Y - inset), + textHeight, textColor, font); } - bool disposed; - public void Dispose() + swapper.Draw(uiText, renderer.TextBatcher, demoSet, new Vector2(16, 16), textHeight, textColor, font); + + if (timingDisplayMode != TimingDisplayMode.Minimized) { - if (!disposed) - { - disposed = true; - demo?.Dispose(); - timeSamples.Dispose(); - font.Dispose(); - } + timingGraph.Draw(uiText, renderer.UILineBatcher, renderer.TextBatcher); + } + else + { + const float timingTextSize = 14; + const float inset = 25; + renderer.TextBatcher.Write( + uiText.Clear().Append(1e3 * timeSamples.Simulation[timeSamples.Simulation.End - 1], timingGraph.Description.VerticalIntervalLabelRounding).Append(" ms/step"), + new Vector2(loop.Window.Resolution.X - inset - GlyphBatch.MeasureLength(uiText, font, timingTextSize), inset), timingTextSize, timingGraph.Description.TextColor, font); } + grabber.Draw(renderer.Lines, loop.Camera, loop.Input.MouseLocked, controls.Grab.IsDown(loop.Input), loop.Window.GetNormalizedMousePosition(loop.Input.MousePosition)); + renderer.Shapes.AddInstances(demo.Simulation, demo.ThreadDispatcher); + renderer.Lines.ShowConstraints = showConstraints; + renderer.Lines.ShowContacts = showContacts; + renderer.Lines.ShowBoundingBoxes = showBoundingBoxes; + renderer.Lines.Extract(demo.Simulation, demo.ThreadDispatcher); + } -#if DEBUG - ~DemoHarness() + bool disposed; + public void Dispose() + { + if (!disposed) { - Helpers.CheckForUndisposed(disposed, this); + disposed = true; + demo?.Dispose(); + timeSamples.Dispose(); + font.Dispose(); } + } + +#if DEBUG + ~DemoHarness() + { + Helpers.CheckForUndisposed(disposed, this); + } #endif - } } diff --git a/Demos/DemoMeshHelper.cs b/Demos/DemoMeshHelper.cs index 9dd181d6f..131a96be8 100644 --- a/Demos/DemoMeshHelper.cs +++ b/Demos/DemoMeshHelper.cs @@ -6,186 +6,185 @@ using BepuUtilities; using BepuPhysics.Trees; -namespace Demos +namespace Demos; + +public static class DemoMeshHelper { - public static class DemoMeshHelper + public static Mesh LoadModel(ContentArchive content, BufferPool pool, string contentName, Vector3 scaling) { - public static Mesh LoadModel(ContentArchive content, BufferPool pool, string contentName, Vector3 scaling) + var meshContent = content.Load(contentName); + pool.Take(meshContent.Triangles.Length, out var triangles); + for (int i = 0; i < meshContent.Triangles.Length; ++i) { - var meshContent = content.Load(contentName); - pool.Take(meshContent.Triangles.Length, out var triangles); - for (int i = 0; i < meshContent.Triangles.Length; ++i) - { - triangles[i] = new Triangle(meshContent.Triangles[i].A, meshContent.Triangles[i].B, meshContent.Triangles[i].C); - } - return new Mesh(triangles, scaling, pool); + triangles[i] = new Triangle(meshContent.Triangles[i].A, meshContent.Triangles[i].B, meshContent.Triangles[i].C); } + return new Mesh(triangles, scaling, pool); + } - public static Mesh CreateFan(int triangleCount, float radius, Vector3 scaling, BufferPool pool) - { - var anglePerTriangle = 2 * MathF.PI / triangleCount; - pool.Take(triangleCount, out var triangles); + public static Mesh CreateFan(int triangleCount, float radius, Vector3 scaling, BufferPool pool) + { + var anglePerTriangle = 2 * MathF.PI / triangleCount; + pool.Take(triangleCount, out var triangles); - for (int i = 0; i < triangleCount; ++i) - { - var firstAngle = i * anglePerTriangle; - var secondAngle = ((i + 1) % triangleCount) * anglePerTriangle; + for (int i = 0; i < triangleCount; ++i) + { + var firstAngle = i * anglePerTriangle; + var secondAngle = ((i + 1) % triangleCount) * anglePerTriangle; - ref var triangle = ref triangles[i]; - triangle.A = new Vector3(radius * MathF.Cos(firstAngle), 0, radius * MathF.Sin(firstAngle)); - triangle.B = new Vector3(radius * MathF.Cos(secondAngle), 0, radius * MathF.Sin(secondAngle)); - triangle.C = new Vector3(); - } - return new Mesh(triangles, scaling, pool); + ref var triangle = ref triangles[i]; + triangle.A = new Vector3(radius * MathF.Cos(firstAngle), 0, radius * MathF.Sin(firstAngle)); + triangle.B = new Vector3(radius * MathF.Cos(secondAngle), 0, radius * MathF.Sin(secondAngle)); + triangle.C = new Vector3(); } + return new Mesh(triangles, scaling, pool); + } - public static Mesh CreateDeformedPlane(int width, int height, Func deformer, Vector3 scaling, BufferPool pool, IThreadDispatcher dispatcher = null) + public static Mesh CreateDeformedPlane(int width, int height, Func deformer, Vector3 scaling, BufferPool pool, IThreadDispatcher dispatcher = null) + { + pool.Take(width * height, out var vertices); + for (int i = 0; i < width; ++i) { - pool.Take(width * height, out var vertices); - for (int i = 0; i < width; ++i) + for (int j = 0; j < height; ++j) { - for (int j = 0; j < height; ++j) - { - vertices[width * j + i] = deformer(i, j); - } + vertices[width * j + i] = deformer(i, j); } + } - var quadWidth = width - 1; - var quadHeight = height - 1; - var triangleCount = quadWidth * quadHeight * 2; - pool.Take(triangleCount, out var triangles); + var quadWidth = width - 1; + var quadHeight = height - 1; + var triangleCount = quadWidth * quadHeight * 2; + pool.Take(triangleCount, out var triangles); - for (int i = 0; i < quadWidth; ++i) + for (int i = 0; i < quadWidth; ++i) + { + for (int j = 0; j < quadHeight; ++j) { - for (int j = 0; j < quadHeight; ++j) - { - var triangleIndex = (j * quadWidth + i) * 2; - ref var triangle0 = ref triangles[triangleIndex]; - ref var v00 = ref vertices[width * j + i]; - ref var v01 = ref vertices[width * j + i + 1]; - ref var v10 = ref vertices[width * (j + 1) + i]; - ref var v11 = ref vertices[width * (j + 1) + i + 1]; - triangle0.A = v00; - triangle0.B = v01; - triangle0.C = v10; - ref var triangle1 = ref triangles[triangleIndex + 1]; - triangle1.A = v01; - triangle1.B = v11; - triangle1.C = v10; - } + var triangleIndex = (j * quadWidth + i) * 2; + ref var triangle0 = ref triangles[triangleIndex]; + ref var v00 = ref vertices[width * j + i]; + ref var v01 = ref vertices[width * j + i + 1]; + ref var v10 = ref vertices[width * (j + 1) + i]; + ref var v11 = ref vertices[width * (j + 1) + i + 1]; + triangle0.A = v00; + triangle0.B = v01; + triangle0.C = v10; + ref var triangle1 = ref triangles[triangleIndex + 1]; + triangle1.A = v01; + triangle1.B = v11; + triangle1.C = v10; } - pool.Return(ref vertices); - return new Mesh(triangles, scaling, pool, dispatcher); } + pool.Return(ref vertices); + return new Mesh(triangles, scaling, pool, dispatcher); + } - /// - /// Creates a bunch of nodes and associates them with leaves with absolutely no regard for where the leaves are. - /// - static void CreateDummyNodes(ref Tree tree, int nodeIndex, int nodeLeafCount, ref int leafCounter) + /// + /// Creates a bunch of nodes and associates them with leaves with absolutely no regard for where the leaves are. + /// + static void CreateDummyNodes(ref Tree tree, int nodeIndex, int nodeLeafCount, ref int leafCounter) + { + ref var node = ref tree.Nodes[nodeIndex]; + node.A.LeafCount = nodeLeafCount / 2; + if (node.A.LeafCount > 1) { - ref var node = ref tree.Nodes[nodeIndex]; - node.A.LeafCount = nodeLeafCount / 2; - if (node.A.LeafCount > 1) - { - node.A.Index = nodeIndex + 1; - tree.Metanodes[node.A.Index] = new Metanode { IndexInParent = 0, Parent = nodeIndex }; - CreateDummyNodes(ref tree, node.A.Index, node.A.LeafCount, ref leafCounter); - } - else - { - tree.Leaves[leafCounter] = new Leaf(nodeIndex, 0); - node.A.Index = Tree.Encode(leafCounter++); - } - node.B.LeafCount = nodeLeafCount - node.A.LeafCount; - if (node.B.LeafCount > 1) - { - node.B.Index = nodeIndex + node.A.LeafCount; - tree.Metanodes[node.B.Index] = new Metanode { IndexInParent = 1, Parent = nodeIndex }; - CreateDummyNodes(ref tree, node.B.Index, node.B.LeafCount, ref leafCounter); - } - else - { - tree.Leaves[leafCounter] = new Leaf(nodeIndex, 1); - node.B.Index = Tree.Encode(leafCounter++); - } + node.A.Index = nodeIndex + 1; + tree.Metanodes[node.A.Index] = new Metanode { IndexInParent = 0, Parent = nodeIndex }; + CreateDummyNodes(ref tree, node.A.Index, node.A.LeafCount, ref leafCounter); } - - /// - /// Takes a large number of triangles and creates a Mesh from them, but does not attempt to compute any bounds. - /// The topology of the mesh's acceleration structure is based entirely on the order of the triangles. - /// This is intended to be used with , , - /// or to provide bounds and higher quality. - /// - /// Large number of triangles to build a mesh from. - /// Scale to use for the mesh shape. - /// Buffer pool to allocate resources for the mesh. - /// Created mesh with no bounds. - /// This exists primarily as an easy example of how to work around the slow sequential default mesh building options for very large meshes, like heightmaps. - /// It is not optimized anywhere close to as much as it could be. - /// In the future, I'd like to give the Tree and Mesh much faster (and multithreaded) constructors that achieve quality and speed in one shot. - public unsafe static Mesh CreateGiantMeshFastWithoutBounds(Buffer triangles, Vector3 scaling, BufferPool pool) + else { - if (triangles.Length < 128) - { - //The special logic isn't necessary for tiny meshes, and we also don't handle the corner case of leaf counts <= 2. Just use the regular constructor. - return new Mesh(triangles, scaling, pool); - } - var mesh = Mesh.CreateWithoutTreeBuild(triangles, scaling, pool); - int leafCounter = 0; - CreateDummyNodes(ref mesh.Tree, 0, triangles.Length, ref leafCounter); - for (int i = 0; i < triangles.Length; ++i) - { - ref var t = ref triangles[i]; - mesh.Tree.GetBoundsPointers(i, out var min, out var max); - *min = Vector3.Min(t.A, Vector3.Min(t.B, t.C)); - *max = Vector3.Max(t.A, Vector3.Max(t.B, t.C)); - } - return mesh; + tree.Leaves[leafCounter] = new Leaf(nodeIndex, 0); + node.A.Index = Tree.Encode(leafCounter++); } - - /// - /// Takes a very large number of triangles and turns them into a mesh by simply assuming that the input triangles are in an order that'll happen to produce an okay-ish acceleration structure. - /// If you have a large height map, you might want to use this instead of the Mesh constructor's default sweep build or insertion builder. - /// The quality is much lower than a sweep build (or even insertion build for that matter), but it can be orders of magnitude faster. - /// Consider using refinement to get the tree quality closer to the sweep builder's quality afterwards. - /// - /// Large number of triangles to build a mesh from. - /// Scale to use for the mesh shape. - /// Buffer pool to allocate resources for the mesh. - /// Created mesh. - /// This exists primarily as an easy example of how to work around the slow sequential default mesh building options for very large meshes, like heightmaps. - /// It is not optimized anywhere close to as much as it could be. - /// In the future, I'd like to give the Tree and Mesh much faster (and multithreaded) constructors that achieve quality and speed in one shot. - public static Mesh CreateGiantMeshFast(Buffer triangles, Vector3 scaling, BufferPool pool) + node.B.LeafCount = nodeLeafCount - node.A.LeafCount; + if (node.B.LeafCount > 1) + { + node.B.Index = nodeIndex + node.A.LeafCount; + tree.Metanodes[node.B.Index] = new Metanode { IndexInParent = 1, Parent = nodeIndex }; + CreateDummyNodes(ref tree, node.B.Index, node.B.LeafCount, ref leafCounter); + } + else { - var mesh = CreateGiantMeshFastWithoutBounds(triangles, scaling, pool); - //None of the nodes actually have bounds. Give them some now. - mesh.Tree.Refit(); - return mesh; + tree.Leaves[leafCounter] = new Leaf(nodeIndex, 1); + node.B.Index = Tree.Encode(leafCounter++); } + } - /// - /// Takes a very large number of triangles and turns them into a mesh by first creating a dummy topology and then incrementally refining it. - /// If you have a large height map, you might want to use this instead of the Mesh constructor's default sweep build or insertion builder. - /// The quality can approach at a much lower cost thanks to a more efficient algorithm and multithreading. - /// - /// Large number of triangles to build a mesh from. - /// Scale to use for the mesh shape. - /// Buffer pool to allocate resources for the mesh. - /// Created mesh. - /// This exists primarily as an easy example of how to work around the slow sequential default mesh building options for very large meshes, like heightmaps. - /// It is not optimized anywhere close to as much as it could be. - /// In the future, I'd like to give the Tree and Mesh much faster (and multithreaded) constructors that achieve quality and speed in one shot. - public static Mesh CreateGiantMeshWithRefinements(Buffer triangles, Vector3 scaling, BufferPool pool, Tree.RefitAndRefineMultithreadedContext context, IThreadDispatcher threadDispatcher, int refinementIterationCount = 8) + /// + /// Takes a large number of triangles and creates a Mesh from them, but does not attempt to compute any bounds. + /// The topology of the mesh's acceleration structure is based entirely on the order of the triangles. + /// This is intended to be used with , , + /// or to provide bounds and higher quality. + /// + /// Large number of triangles to build a mesh from. + /// Scale to use for the mesh shape. + /// Buffer pool to allocate resources for the mesh. + /// Created mesh with no bounds. + /// This exists primarily as an easy example of how to work around the slow sequential default mesh building options for very large meshes, like heightmaps. + /// It is not optimized anywhere close to as much as it could be. + /// In the future, I'd like to give the Tree and Mesh much faster (and multithreaded) constructors that achieve quality and speed in one shot. + public unsafe static Mesh CreateGiantMeshFastWithoutBounds(Buffer triangles, Vector3 scaling, BufferPool pool) + { + if (triangles.Length < 128) + { + //The special logic isn't necessary for tiny meshes, and we also don't handle the corner case of leaf counts <= 2. Just use the regular constructor. + return new Mesh(triangles, scaling, pool); + } + var mesh = Mesh.CreateWithoutTreeBuild(triangles, scaling, pool); + int leafCounter = 0; + CreateDummyNodes(ref mesh.Tree, 0, triangles.Length, ref leafCounter); + for (int i = 0; i < triangles.Length; ++i) { - var mesh = CreateGiantMeshFastWithoutBounds(triangles, scaling, pool); - //None of the nodes actually have bounds. Give them some now. - for (int i = 0; i < refinementIterationCount; ++i) - context.RefitAndRefine(ref mesh.Tree, pool, threadDispatcher, i, 20); - return mesh; + ref var t = ref triangles[i]; + mesh.Tree.GetBoundsPointers(i, out var min, out var max); + *min = Vector3.Min(t.A, Vector3.Min(t.B, t.C)); + *max = Vector3.Max(t.A, Vector3.Max(t.B, t.C)); } + return mesh; + } + /// + /// Takes a very large number of triangles and turns them into a mesh by simply assuming that the input triangles are in an order that'll happen to produce an okay-ish acceleration structure. + /// If you have a large height map, you might want to use this instead of the Mesh constructor's default sweep build or insertion builder. + /// The quality is much lower than a sweep build (or even insertion build for that matter), but it can be orders of magnitude faster. + /// Consider using refinement to get the tree quality closer to the sweep builder's quality afterwards. + /// + /// Large number of triangles to build a mesh from. + /// Scale to use for the mesh shape. + /// Buffer pool to allocate resources for the mesh. + /// Created mesh. + /// This exists primarily as an easy example of how to work around the slow sequential default mesh building options for very large meshes, like heightmaps. + /// It is not optimized anywhere close to as much as it could be. + /// In the future, I'd like to give the Tree and Mesh much faster (and multithreaded) constructors that achieve quality and speed in one shot. + public static Mesh CreateGiantMeshFast(Buffer triangles, Vector3 scaling, BufferPool pool) + { + var mesh = CreateGiantMeshFastWithoutBounds(triangles, scaling, pool); + //None of the nodes actually have bounds. Give them some now. + mesh.Tree.Refit(); + return mesh; } + + /// + /// Takes a very large number of triangles and turns them into a mesh by first creating a dummy topology and then incrementally refining it. + /// If you have a large height map, you might want to use this instead of the Mesh constructor's default sweep build or insertion builder. + /// The quality can approach at a much lower cost thanks to a more efficient algorithm and multithreading. + /// + /// Large number of triangles to build a mesh from. + /// Scale to use for the mesh shape. + /// Buffer pool to allocate resources for the mesh. + /// Created mesh. + /// This exists primarily as an easy example of how to work around the slow sequential default mesh building options for very large meshes, like heightmaps. + /// It is not optimized anywhere close to as much as it could be. + /// In the future, I'd like to give the Tree and Mesh much faster (and multithreaded) constructors that achieve quality and speed in one shot. + public static Mesh CreateGiantMeshWithRefinements(Buffer triangles, Vector3 scaling, BufferPool pool, Tree.RefitAndRefineMultithreadedContext context, IThreadDispatcher threadDispatcher, int refinementIterationCount = 8) + { + var mesh = CreateGiantMeshFastWithoutBounds(triangles, scaling, pool); + //None of the nodes actually have bounds. Give them some now. + for (int i = 0; i < refinementIterationCount; ++i) + context.RefitAndRefine(ref mesh.Tree, pool, threadDispatcher, i, 20); + return mesh; + } + } diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index 2c592019a..67590f28f 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -9,81 +9,80 @@ using System; using System.Collections.Generic; -namespace Demos +namespace Demos; + +/// +/// Constructs a demo from the set of available demos on demand. +/// +public class DemoSet { - /// - /// Constructs a demo from the set of available demos on demand. - /// - public class DemoSet + struct Option { - struct Option - { - public string Name; - public Func Builder; - } + public string Name; + public Func Builder; + } - List + public TankPartDescription Turret; + /// + /// Description of the tank's barrel body. + /// + public TankPartDescription Barrel; + /// + /// Description of the tank's main body. + /// + public TankPartDescription Body; + /// + /// Location of the barrel's anchor in the tank's local space. The barrel will connect to the turret at this location. + /// + public Vector3 BarrelAnchor; + /// + /// Location of the turret's anchor in the tank's local space. The turret will connect to the main body at this location. + /// + public Vector3 TurretAnchor; + /// + /// Basis of the turret and barrel. (0, 0, -1) * TurretBasis in tank local space corresponds to 0 angle for both turret swivel and barrel pitch measurements. + /// (1, 0, 0) * TurretBasis corresponds to a 90 degree swivel angle. + /// (0, 1, 0) * TurretBasis corresponds to a 90 degree pitch angle, and is the axis around which the turret can swivel. + /// + public Quaternion TurretBasis; + /// + /// Servo properties for the tank's swivel constraint. + /// + public ServoSettings TurretServo; + /// + /// Spring properties for the tank's swivel constraint. + /// + public SpringSettings TurretSpring; + /// + /// Servo properties for the tank's barrel pitching constraint. + /// + public ServoSettings BarrelServo; + /// + /// Spring properties for the tank's barrel pitching constraint. + /// + public SpringSettings BarrelSpring; - /// - /// Location in the barrel body's local space where projectiles should be created. - /// - public Vector3 BarrelLocalProjectileSpawn; - /// - /// Inertia of fired projectiles. - /// - public BodyInertia ProjectileInertia; - /// - /// Shape of fired projectiles. - /// - public TypedIndex ProjectileShape; - /// - /// Speed of fired projectiles. - /// - public float ProjectileSpeed; + /// + /// Location in the barrel body's local space where projectiles should be created. + /// + public Vector3 BarrelLocalProjectileSpawn; + /// + /// Inertia of fired projectiles. + /// + public BodyInertia ProjectileInertia; + /// + /// Shape of fired projectiles. + /// + public TypedIndex ProjectileShape; + /// + /// Speed of fired projectiles. + /// + public float ProjectileSpeed; - /// - /// Shape used for all wheels. - /// - public TypedIndex WheelShape; - /// - /// Inertia of each wheel body. - /// - public BodyInertia WheelInertia; + /// + /// Shape used for all wheels. + /// + public TypedIndex WheelShape; + /// + /// Inertia of each wheel body. + /// + public BodyInertia WheelInertia; - /// - /// Local orientation of the wheels. (1,0,0) * WheelOrientation is the suspension direction, (0,1,0) * WheelOrientation is the axis of rotation for the wheels, and (0,0,1) * WheelOrientation is the axis along which the treads will extend. - /// - public Quaternion WheelOrientation; - /// - /// Offset from the tank's local space origin to the left tread's center. The tread will be aligned along (0,0,1) * WheelOrientation. - /// - public Vector3 LeftTreadOffset; - /// - /// Offset from the tank's local space origin to the right tread's center. The tread will be aligned along (0,0,1) * WheelOrientation. - /// - public Vector3 RightTreadOffset; - /// - /// Number of wheels in each tread. - /// - public int WheelCountPerTread; - /// - /// How much space to put in between wheels in the tread. - /// - public float TreadSpacing; - /// - /// Resting length of the suspension for each wheel. - /// - public float SuspensionLength; - /// - /// Spring settings for the wheel suspension. - /// - public SpringSettings SuspensionSettings; - /// - /// Friction for the wheel bodies. - /// - public float WheelFriction; - } + /// + /// Local orientation of the wheels. (1,0,0) * WheelOrientation is the suspension direction, (0,1,0) * WheelOrientation is the axis of rotation for the wheels, and (0,0,1) * WheelOrientation is the axis along which the treads will extend. + /// + public Quaternion WheelOrientation; + /// + /// Offset from the tank's local space origin to the left tread's center. The tread will be aligned along (0,0,1) * WheelOrientation. + /// + public Vector3 LeftTreadOffset; + /// + /// Offset from the tank's local space origin to the right tread's center. The tread will be aligned along (0,0,1) * WheelOrientation. + /// + public Vector3 RightTreadOffset; + /// + /// Number of wheels in each tread. + /// + public int WheelCountPerTread; + /// + /// How much space to put in between wheels in the tread. + /// + public float TreadSpacing; + /// + /// Resting length of the suspension for each wheel. + /// + public float SuspensionLength; + /// + /// Spring settings for the wheel suspension. + /// + public SpringSettings SuspensionSettings; + /// + /// Friction for the wheel bodies. + /// + public float WheelFriction; } \ No newline at end of file diff --git a/Demos/Demos/Tanks/TankPartDescription.cs b/Demos/Demos/Tanks/TankPartDescription.cs index ddb6fcc37..1ce32b21a 100644 --- a/Demos/Demos/Tanks/TankPartDescription.cs +++ b/Demos/Demos/Tanks/TankPartDescription.cs @@ -1,38 +1,37 @@ using BepuPhysics; using BepuPhysics.Collidables; -namespace Demos.Demos.Tanks +namespace Demos.Demos.Tanks; + +/// +/// Describes properties of a piece of a tank. +/// +public struct TankPartDescription { /// - /// Describes properties of a piece of a tank. + /// Shape index used by this part's collidable. /// - public struct TankPartDescription - { - /// - /// Shape index used by this part's collidable. - /// - public TypedIndex Shape; - /// - /// Inertia of this part's body. - /// - public BodyInertia Inertia; - /// - /// Pose of the part in the tank's local space. - /// - public RigidPose Pose; - /// - /// Friction of the body to be used in pair material calculations. - /// - public float Friction; + public TypedIndex Shape; + /// + /// Inertia of this part's body. + /// + public BodyInertia Inertia; + /// + /// Pose of the part in the tank's local space. + /// + public RigidPose Pose; + /// + /// Friction of the body to be used in pair material calculations. + /// + public float Friction; - public static TankPartDescription Create(float mass, in TShape shape, in RigidPose pose, float friction, Shapes shapes) where TShape : unmanaged, IConvexShape - { - TankPartDescription description; - description.Shape = shapes.Add(shape); - description.Inertia = shape.ComputeInertia(mass); - description.Pose = pose; - description.Friction = friction; - return description; - } + public static TankPartDescription Create(float mass, in TShape shape, in RigidPose pose, float friction, Shapes shapes) where TShape : unmanaged, IConvexShape + { + TankPartDescription description; + description.Shape = shapes.Add(shape); + description.Inertia = shape.ComputeInertia(mass); + description.Pose = pose; + description.Friction = friction; + return description; } } \ No newline at end of file diff --git a/Demos/GameLoop.cs b/Demos/GameLoop.cs index 4d929b098..ddedab484 100644 --- a/Demos/GameLoop.cs +++ b/Demos/GameLoop.cs @@ -4,74 +4,73 @@ using BepuUtilities; using BepuUtilities.Memory; -namespace Demos +namespace Demos; + +public class GameLoop : IDisposable { - public class GameLoop : IDisposable - { - public Window Window { get; private set; } - public Input Input { get; private set; } - public Camera Camera { get; private set; } - public RenderSurface Surface { get; private set; } - public Renderer Renderer { get; private set; } - public DemoHarness DemoHarness { get; set; } - public BufferPool Pool { get; } = new BufferPool(); + public Window Window { get; private set; } + public Input Input { get; private set; } + public Camera Camera { get; private set; } + public RenderSurface Surface { get; private set; } + public Renderer Renderer { get; private set; } + public DemoHarness DemoHarness { get; set; } + public BufferPool Pool { get; } = new BufferPool(); - public GameLoop(Window window) - { - Window = window; - Input = new Input(window, Pool); - Surface = new RenderSurface( + public GameLoop(Window window) + { + Window = window; + Input = new Input(window, Pool); + Surface = new RenderSurface( #if OPENGL - window.WindowInfo, + window.WindowInfo, #else - window.Handle, + window.Handle, #endif - window.Resolution, enableDeviceDebugLayer: false - ); - Renderer = new Renderer(Surface); - Camera = new Camera(window.Resolution.X / (float)window.Resolution.Y, (float)Math.PI / 3, 0.01f, 100000); - } + window.Resolution, enableDeviceDebugLayer: false + ); + Renderer = new Renderer(Surface); + Camera = new Camera(window.Resolution.X / (float)window.Resolution.Y, (float)Math.PI / 3, 0.01f, 100000); + } - void Update(float dt) + void Update(float dt) + { + Input.Start(); + if (DemoHarness != null) { - Input.Start(); - if (DemoHarness != null) - { - //We'll let the delegate's logic handle the variable time steps. - DemoHarness.Update(dt); - //At the moment, rendering just follows sequentially. Later on we might want to distinguish it a bit more with fixed time stepping or something. Maybe. - DemoHarness.Render(Renderer); - } - Renderer.Render(Camera); - Surface.Present(); - Input.End(); + //We'll let the delegate's logic handle the variable time steps. + DemoHarness.Update(dt); + //At the moment, rendering just follows sequentially. Later on we might want to distinguish it a bit more with fixed time stepping or something. Maybe. + DemoHarness.Render(Renderer); } + Renderer.Render(Camera); + Surface.Present(); + Input.End(); + } - public void Run(DemoHarness harness) - { - DemoHarness = harness; - Window.Run(Update, OnResize); - } + public void Run(DemoHarness harness) + { + DemoHarness = harness; + Window.Run(Update, OnResize); + } - private void OnResize(Int2 resolution) - { - //We just don't support true fullscreen in the demos. Would be pretty pointless. - Renderer.Surface.Resize(resolution, false); - Camera.AspectRatio = resolution.X / (float)resolution.Y; - DemoHarness?.OnResize(resolution); - } + private void OnResize(Int2 resolution) + { + //We just don't support true fullscreen in the demos. Would be pretty pointless. + Renderer.Surface.Resize(resolution, false); + Camera.AspectRatio = resolution.X / (float)resolution.Y; + DemoHarness?.OnResize(resolution); + } - bool disposed; - public void Dispose() + bool disposed; + public void Dispose() + { + if (!disposed) { - if (!disposed) - { - disposed = true; - Input.Dispose(); - Renderer.Dispose(); - Pool.Clear(); - //Note that we do not own the window. - } + disposed = true; + Input.Dispose(); + Renderer.Dispose(); + Pool.Clear(); + //Note that we do not own the window. } } } diff --git a/Demos/Grabber.cs b/Demos/Grabber.cs index 1b61f2fc6..72b30de65 100644 --- a/Demos/Grabber.cs +++ b/Demos/Grabber.cs @@ -9,129 +9,126 @@ using System.Numerics; using System.Runtime.CompilerServices; -namespace Demos +namespace Demos; + +struct Grabber { - struct Grabber - { - bool active; - BodyReference body; - float t; - Vector3 localGrabPoint; - Quaternion targetOrientation; - ConstraintHandle linearMotorHandle; - ConstraintHandle angularMotorHandle; + bool active; + BodyReference body; + float t; + Vector3 localGrabPoint; + Quaternion targetOrientation; + ConstraintHandle linearMotorHandle; + ConstraintHandle angularMotorHandle; - struct RayHitHandler : IRayHitHandler + struct RayHitHandler : IRayHitHandler + { + public float T; + public CollidableReference HitCollidable; + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool AllowTest(CollidableReference collidable) { - public float T; - public CollidableReference HitCollidable; - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool AllowTest(CollidableReference collidable) - { - return true; - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool AllowTest(CollidableReference collidable, int childIndex) - { - return true; - } + return true; + } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void OnRayHit(in RayData ray, ref float maximumT, float t, Vector3 normal, CollidableReference collidable, int childIndex) - { - //We are only interested in the earliest hit. This callback is executing within the traversal, so modifying maximumT informs the traversal - //that it can skip any AABBs which are more distant than the new maximumT. - maximumT = t; - //Cache the earliest impact. - T = t; - HitCollidable = collidable; - } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool AllowTest(CollidableReference collidable, int childIndex) + { + return true; } - readonly void CreateMotorDescription(Vector3 target, float inverseMass, out OneBodyLinearServo linearDescription, out OneBodyAngularServo angularDescription) + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void OnRayHit(in RayData ray, ref float maximumT, float t, Vector3 normal, CollidableReference collidable, int childIndex) { - linearDescription = new OneBodyLinearServo - { - LocalOffset = localGrabPoint, - Target = target, - ServoSettings = new ServoSettings(float.MaxValue, 0, 360 / inverseMass), - SpringSettings = new SpringSettings(5, 2), - }; - angularDescription = new OneBodyAngularServo - { - TargetOrientation = targetOrientation, - ServoSettings = new ServoSettings(float.MaxValue, 0, localGrabPoint.Length() * 180 / inverseMass), - SpringSettings = new SpringSettings(5, 2), - }; + //We are only interested in the earliest hit. This callback is executing within the traversal, so modifying maximumT informs the traversal + //that it can skip any AABBs which are more distant than the new maximumT. + maximumT = t; + //Cache the earliest impact. + T = t; + HitCollidable = collidable; } + } - public void Update(Simulation simulation, Camera camera, bool mouseLocked, bool shouldGrab, Quaternion rotation, in Vector2 normalizedMousePosition) + readonly void CreateMotorDescription(Vector3 target, float inverseMass, out OneBodyLinearServo linearDescription, out OneBodyAngularServo angularDescription) + { + linearDescription = new OneBodyLinearServo { - //On the off chance some demo modifies the kinematic state, treat that as a grab terminator. - var bodyExists = body.Exists && !body.Kinematic; - if (active && (!shouldGrab || !bodyExists)) - { - active = false; - if (bodyExists) - { - //If the body wasn't removed, then the constraint should be removed. - //(Body removal forces connected constraints to removed, so in that case we wouldn't have to worry about it.) - simulation.Solver.Remove(linearMotorHandle); - if (!Bodies.HasLockedInertia(body.LocalInertia.InverseInertiaTensor)) - simulation.Solver.Remove(angularMotorHandle); - } - body = new BodyReference(); - } - else if (shouldGrab && !active) - { - var rayDirection = camera.GetRayDirection(mouseLocked, normalizedMousePosition); - var hitHandler = default(RayHitHandler); - hitHandler.T = float.MaxValue; - simulation.RayCast(camera.Position, rayDirection, float.MaxValue, ref hitHandler); - if (hitHandler.T < float.MaxValue && hitHandler.HitCollidable.Mobility == CollidableMobility.Dynamic) - { - //Found something to grab! - t = hitHandler.T; - body = simulation.Bodies[hitHandler.HitCollidable.BodyHandle]; - var hitLocation = camera.Position + rayDirection * t; - RigidPose.TransformByInverse(hitLocation, body.Pose, out localGrabPoint); - targetOrientation = body.Pose.Orientation; - active = true; - CreateMotorDescription(hitLocation, body.LocalInertia.InverseMass, out var linearDescription, out var angularDescription); - linearMotorHandle = simulation.Solver.Add(body.Handle, linearDescription); - if (!Bodies.HasLockedInertia(body.LocalInertia.InverseInertiaTensor)) - angularMotorHandle = simulation.Solver.Add(body.Handle, angularDescription); - } - } - else if (active) - { - var rayDirection = camera.GetRayDirection(mouseLocked, normalizedMousePosition); - var targetPoint = camera.Position + rayDirection * t; - targetOrientation = QuaternionEx.Normalize(QuaternionEx.Concatenate(targetOrientation, rotation)); + LocalOffset = localGrabPoint, + Target = target, + ServoSettings = new ServoSettings(float.MaxValue, 0, 360 / inverseMass), + SpringSettings = new SpringSettings(5, 2), + }; + angularDescription = new OneBodyAngularServo + { + TargetOrientation = targetOrientation, + ServoSettings = new ServoSettings(float.MaxValue, 0, localGrabPoint.Length() * 180 / inverseMass), + SpringSettings = new SpringSettings(5, 2), + }; + } - CreateMotorDescription(targetPoint, body.LocalInertia.InverseMass, out var linearDescription, out var angularDescription); - simulation.Solver.ApplyDescription(linearMotorHandle, linearDescription); + public void Update(Simulation simulation, Camera camera, bool mouseLocked, bool shouldGrab, Quaternion rotation, in Vector2 normalizedMousePosition) + { + //On the off chance some demo modifies the kinematic state, treat that as a grab terminator. + var bodyExists = body.Exists && !body.Kinematic; + if (active && (!shouldGrab || !bodyExists)) + { + active = false; + if (bodyExists) + { + //If the body wasn't removed, then the constraint should be removed. + //(Body removal forces connected constraints to removed, so in that case we wouldn't have to worry about it.) + simulation.Solver.Remove(linearMotorHandle); if (!Bodies.HasLockedInertia(body.LocalInertia.InverseInertiaTensor)) - simulation.Solver.ApplyDescription(angularMotorHandle, angularDescription); - body.Activity.TimestepsUnderThresholdCount = 0; + simulation.Solver.Remove(angularMotorHandle); } + body = new BodyReference(); } - - public void Draw(LineExtractor lines, Camera camera, bool mouseLocked, bool shouldGrab, in Vector2 normalizedMousePosition) + else if (shouldGrab && !active) { - if (shouldGrab && !active && mouseLocked) + var rayDirection = camera.GetRayDirection(mouseLocked, normalizedMousePosition); + var hitHandler = default(RayHitHandler); + hitHandler.T = float.MaxValue; + simulation.RayCast(camera.Position, rayDirection, float.MaxValue, ref hitHandler); + if (hitHandler.T < float.MaxValue && hitHandler.HitCollidable.Mobility == CollidableMobility.Dynamic) { - //Draw a crosshair if there is no mouse cursor. - var center = camera.Position + camera.Forward * (camera.NearClip * 10); - var crosshairLength = 0.1f * camera.NearClip * MathF.Tan(camera.FieldOfView * 0.5f); - var rightOffset = camera.Right * crosshairLength; - var upOffset = camera.Up * crosshairLength; - lines.Allocate() = new LineInstance(center - rightOffset, center + rightOffset, new Vector3(1, 0, 0), new Vector3()); - lines.Allocate() = new LineInstance(center - upOffset, center + upOffset, new Vector3(1, 0, 0), new Vector3()); + //Found something to grab! + t = hitHandler.T; + body = simulation.Bodies[hitHandler.HitCollidable.BodyHandle]; + var hitLocation = camera.Position + rayDirection * t; + RigidPose.TransformByInverse(hitLocation, body.Pose, out localGrabPoint); + targetOrientation = body.Pose.Orientation; + active = true; + CreateMotorDescription(hitLocation, body.LocalInertia.InverseMass, out var linearDescription, out var angularDescription); + linearMotorHandle = simulation.Solver.Add(body.Handle, linearDescription); + if (!Bodies.HasLockedInertia(body.LocalInertia.InverseInertiaTensor)) + angularMotorHandle = simulation.Solver.Add(body.Handle, angularDescription); } } - } + else if (active) + { + var rayDirection = camera.GetRayDirection(mouseLocked, normalizedMousePosition); + var targetPoint = camera.Position + rayDirection * t; + targetOrientation = QuaternionEx.Normalize(QuaternionEx.Concatenate(targetOrientation, rotation)); + CreateMotorDescription(targetPoint, body.LocalInertia.InverseMass, out var linearDescription, out var angularDescription); + simulation.Solver.ApplyDescription(linearMotorHandle, linearDescription); + if (!Bodies.HasLockedInertia(body.LocalInertia.InverseInertiaTensor)) + simulation.Solver.ApplyDescription(angularMotorHandle, angularDescription); + body.Activity.TimestepsUnderThresholdCount = 0; + } + } + public void Draw(LineExtractor lines, Camera camera, bool mouseLocked, bool shouldGrab, in Vector2 normalizedMousePosition) + { + if (shouldGrab && !active && mouseLocked) + { + //Draw a crosshair if there is no mouse cursor. + var center = camera.Position + camera.Forward * (camera.NearClip * 10); + var crosshairLength = 0.1f * camera.NearClip * MathF.Tan(camera.FieldOfView * 0.5f); + var rightOffset = camera.Right * crosshairLength; + var upOffset = camera.Up * crosshairLength; + lines.Allocate() = new LineInstance(center - rightOffset, center + rightOffset, new Vector3(1, 0, 0), new Vector3()); + lines.Allocate() = new LineInstance(center - upOffset, center + upOffset, new Vector3(1, 0, 0), new Vector3()); + } + } } diff --git a/Demos/Program.cs b/Demos/Program.cs index 2c53c2b70..aaa4f3ca5 100644 --- a/Demos/Program.cs +++ b/Demos/Program.cs @@ -3,25 +3,24 @@ using DemoUtilities; using OpenTK; -namespace Demos +namespace Demos; + +class Program { - class Program + static void Main() { - static void Main() + var window = new Window("pretty cool multicolored window", + new Int2((int)(DisplayDevice.Default.Width * 0.75f), (int)(DisplayDevice.Default.Height * 0.75f)), WindowMode.Windowed); + var loop = new GameLoop(window); + ContentArchive content; + using (var stream = typeof(Program).Assembly.GetManifestResourceStream("Demos.Demos.contentarchive")) { - var window = new Window("pretty cool multicolored window", - new Int2((int)(DisplayDevice.Default.Width * 0.75f), (int)(DisplayDevice.Default.Height * 0.75f)), WindowMode.Windowed); - var loop = new GameLoop(window); - ContentArchive content; - using (var stream = typeof(Program).Assembly.GetManifestResourceStream("Demos.Demos.contentarchive")) - { - content = ContentArchive.Load(stream); - } - //HeadlessTest.Test(content, 4, 32, 512); - var demo = new DemoHarness(loop, content); - loop.Run(demo); - loop.Dispose(); - window.Dispose(); + content = ContentArchive.Load(stream); } + //HeadlessTest.Test(content, 4, 32, 512); + var demo = new DemoHarness(loop, content); + loop.Run(demo); + loop.Dispose(); + window.Dispose(); } } \ No newline at end of file diff --git a/Demos/RolloverInfo.cs b/Demos/RolloverInfo.cs index db6354dbe..c606fe9ef 100644 --- a/Demos/RolloverInfo.cs +++ b/Demos/RolloverInfo.cs @@ -5,67 +5,66 @@ using System.Collections.Generic; using System.Numerics; -namespace Demos +namespace Demos; + +public class RolloverInfo { - public class RolloverInfo + struct RolloverDescription { - struct RolloverDescription - { - public Vector3 Position; - public string Description; - public float PreviewOffset; - public string Preview; - } + public Vector3 Position; + public string Description; + public float PreviewOffset; + public string Preview; + } - List descriptions; + List descriptions; - public RolloverInfo() - { - descriptions = new List(); - } + public RolloverInfo() + { + descriptions = new List(); + } - public void Add(Vector3 position, string description, float previewOffset = -1.2f, string previewText = "Info...") - { - this.descriptions.Add(new RolloverDescription { Position = position, Description = description, PreviewOffset = previewOffset, Preview = previewText }); - } + public void Add(Vector3 position, string description, float previewOffset = -1.2f, string previewText = "Info...") + { + this.descriptions.Add(new RolloverDescription { Position = position, Description = description, PreviewOffset = previewOffset, Preview = previewText }); + } - public unsafe void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + public unsafe void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + { + var resolution = new Vector2(renderer.Surface.Resolution.X, renderer.Surface.Resolution.Y); + var screenLocations = stackalloc Vector2[descriptions.Count]; + int closestIndex = -1; + float closestDistance = MathF.Max(resolution.X, resolution.Y) * 0.1f; + for (int i = 0; i < descriptions.Count; ++i) { - var resolution = new Vector2(renderer.Surface.Resolution.X, renderer.Surface.Resolution.Y); - var screenLocations = stackalloc Vector2[descriptions.Count]; - int closestIndex = -1; - float closestDistance = MathF.Max(resolution.X, resolution.Y) * 0.1f; - for (int i = 0; i < descriptions.Count; ++i) + var textPosition = descriptions[i].Position; + Helpers.GetScreenLocation(textPosition, camera.ViewProjection, resolution, out screenLocations[i]); + var mouse = input.MousePosition; + var distance = Vector2.Distance(new Vector2(mouse.X, mouse.Y), screenLocations[i]); + if (distance < closestDistance) { - var textPosition = descriptions[i].Position; - Helpers.GetScreenLocation(textPosition, camera.ViewProjection, resolution, out screenLocations[i]); - var mouse = input.MousePosition; - var distance = Vector2.Distance(new Vector2(mouse.X, mouse.Y), screenLocations[i]); - if (distance < closestDistance) - { - closestDistance = distance; - closestIndex = i; - } + closestDistance = distance; + closestIndex = i; } + } - const float infoHeight = 8; - const float descriptionHeight = 16; - for (int i = 0; i < descriptions.Count; ++i) - { - if (i != closestIndex) - { - text.Clear().Append(descriptions[i].Preview); - var infoLength = GlyphBatch.MeasureLength(text, font, infoHeight); - renderer.TextBatcher.Write(text, screenLocations[i] + new Vector2(-infoLength * 0.5f, descriptions[i].PreviewOffset * descriptionHeight), infoHeight, new Vector3(1), font); - } - } - if (closestIndex >= 0) + const float infoHeight = 8; + const float descriptionHeight = 16; + for (int i = 0; i < descriptions.Count; ++i) + { + if (i != closestIndex) { - text.Clear().Append(descriptions[closestIndex].Description); - var descriptionLength = GlyphBatch.MeasureLength(text, font, descriptionHeight); - renderer.TextBatcher.Write(text, screenLocations[closestIndex] - new Vector2(descriptionLength * 0.5f, 0), 16, new Vector3(1), font); + text.Clear().Append(descriptions[i].Preview); + var infoLength = GlyphBatch.MeasureLength(text, font, infoHeight); + renderer.TextBatcher.Write(text, screenLocations[i] + new Vector2(-infoLength * 0.5f, descriptions[i].PreviewOffset * descriptionHeight), infoHeight, new Vector3(1), font); } - } + if (closestIndex >= 0) + { + text.Clear().Append(descriptions[closestIndex].Description); + var descriptionLength = GlyphBatch.MeasureLength(text, font, descriptionHeight); + renderer.TextBatcher.Write(text, screenLocations[closestIndex] - new Vector2(descriptionLength * 0.5f, 0), 16, new Vector3(1), font); + } + } } diff --git a/Demos/SimulationTimeSamples.cs b/Demos/SimulationTimeSamples.cs index 2e3733a05..8a31e45cf 100644 --- a/Demos/SimulationTimeSamples.cs +++ b/Demos/SimulationTimeSamples.cs @@ -1,66 +1,65 @@ using BepuPhysics; using BepuUtilities.Memory; -namespace Demos +namespace Demos; + +public struct TimelineStats { - public struct TimelineStats + public double Total; + public double Average; + public double Min; + public double Max; + public double StdDev; +} + +/// +/// Stores the time it took to complete stages of the physics simulation in a ring buffer. Once the ring buffer is full, the oldest results will be removed. +/// +public class SimulationTimeSamples +{ + public TimingsRingBuffer Simulation; + public TimingsRingBuffer PoseIntegrator; + public TimingsRingBuffer Sleeper; + public TimingsRingBuffer BroadPhaseUpdate; + public TimingsRingBuffer CollisionTesting; + public TimingsRingBuffer NarrowPhaseFlush; + public TimingsRingBuffer Solver; + public TimingsRingBuffer BatchCompressor; + + public SimulationTimeSamples(int frameCapacity, BufferPool pool) { - public double Total; - public double Average; - public double Min; - public double Max; - public double StdDev; + Simulation = new TimingsRingBuffer(frameCapacity, pool); + PoseIntegrator = new TimingsRingBuffer(frameCapacity, pool); + Sleeper = new TimingsRingBuffer(frameCapacity, pool); + BroadPhaseUpdate = new TimingsRingBuffer(frameCapacity, pool); + CollisionTesting = new TimingsRingBuffer(frameCapacity, pool); + NarrowPhaseFlush = new TimingsRingBuffer(frameCapacity, pool); + Solver = new TimingsRingBuffer(frameCapacity, pool); + BatchCompressor = new TimingsRingBuffer(frameCapacity, pool); } - - /// - /// Stores the time it took to complete stages of the physics simulation in a ring buffer. Once the ring buffer is full, the oldest results will be removed. - /// - public class SimulationTimeSamples - { - public TimingsRingBuffer Simulation; - public TimingsRingBuffer PoseIntegrator; - public TimingsRingBuffer Sleeper; - public TimingsRingBuffer BroadPhaseUpdate; - public TimingsRingBuffer CollisionTesting; - public TimingsRingBuffer NarrowPhaseFlush; - public TimingsRingBuffer Solver; - public TimingsRingBuffer BatchCompressor; - - public SimulationTimeSamples(int frameCapacity, BufferPool pool) - { - Simulation = new TimingsRingBuffer(frameCapacity, pool); - PoseIntegrator = new TimingsRingBuffer(frameCapacity, pool); - Sleeper = new TimingsRingBuffer(frameCapacity, pool); - BroadPhaseUpdate = new TimingsRingBuffer(frameCapacity, pool); - CollisionTesting = new TimingsRingBuffer(frameCapacity, pool); - NarrowPhaseFlush = new TimingsRingBuffer(frameCapacity, pool); - Solver = new TimingsRingBuffer(frameCapacity, pool); - BatchCompressor = new TimingsRingBuffer(frameCapacity, pool); - } - public void RecordFrame(Simulation simulation) - { - //This requires the simulation to be compiled with profiling enabled. - Simulation.Add(simulation.Profiler[simulation]); - PoseIntegrator.Add(simulation.Profiler[simulation.PoseIntegrator]); - Sleeper.Add(simulation.Profiler[simulation.Sleeper]); - BroadPhaseUpdate.Add(simulation.Profiler[simulation.BroadPhase]); - CollisionTesting.Add(simulation.Profiler[simulation.BroadPhaseOverlapFinder]); - NarrowPhaseFlush.Add(simulation.Profiler[simulation.NarrowPhase]); - Solver.Add(simulation.Profiler[simulation.Solver]); - BatchCompressor.Add(simulation.Profiler[simulation.SolverBatchCompressor]); - } + public void RecordFrame(Simulation simulation) + { + //This requires the simulation to be compiled with profiling enabled. + Simulation.Add(simulation.Profiler[simulation]); + PoseIntegrator.Add(simulation.Profiler[simulation.PoseIntegrator]); + Sleeper.Add(simulation.Profiler[simulation.Sleeper]); + BroadPhaseUpdate.Add(simulation.Profiler[simulation.BroadPhase]); + CollisionTesting.Add(simulation.Profiler[simulation.BroadPhaseOverlapFinder]); + NarrowPhaseFlush.Add(simulation.Profiler[simulation.NarrowPhase]); + Solver.Add(simulation.Profiler[simulation.Solver]); + BatchCompressor.Add(simulation.Profiler[simulation.SolverBatchCompressor]); + } - public void Dispose() - { - Simulation.Dispose(); - PoseIntegrator.Dispose(); - Sleeper.Dispose(); - BroadPhaseUpdate.Dispose(); - CollisionTesting.Dispose(); - NarrowPhaseFlush.Dispose(); - Solver.Dispose(); - BatchCompressor.Dispose(); - } + public void Dispose() + { + Simulation.Dispose(); + PoseIntegrator.Dispose(); + Sleeper.Dispose(); + BroadPhaseUpdate.Dispose(); + CollisionTesting.Dispose(); + NarrowPhaseFlush.Dispose(); + Solver.Dispose(); + BatchCompressor.Dispose(); } } diff --git a/Demos/SpecializedTests/BatchedCollisionTests.cs b/Demos/SpecializedTests/BatchedCollisionTests.cs index 7d73f3ae8..55179bad5 100644 --- a/Demos/SpecializedTests/BatchedCollisionTests.cs +++ b/Demos/SpecializedTests/BatchedCollisionTests.cs @@ -10,288 +10,287 @@ using BepuPhysics.CollisionDetection.SweepTasks; using BepuUtilities.Collections; -namespace Demos.SpecializedTests +namespace Demos.SpecializedTests; + +public static class BatchedCollisionTests { - public static class BatchedCollisionTests + unsafe struct TestCollisionCallbacks : ICollisionCallbacks { - unsafe struct TestCollisionCallbacks : ICollisionCallbacks - { - public int* Count; + public int* Count; - public void OnPairCompleted(int pairId, ref TManifold manifold) where TManifold : unmanaged, IContactManifold - { - if (manifold.Count > 0) - { - manifold.GetContact(0, out var offset, out var normal, out var depth, out var featureId); - var extra = 1e-16 * (depth + offset.X + normal.X); - *Count += 1 + (int)extra; - } - else - { - ++*Count; - } - } - - public void OnChildPairCompleted(int pairId, int childA, int childB, ref ConvexContactManifold manifold) + public void OnPairCompleted(int pairId, ref TManifold manifold) where TManifold : unmanaged, IContactManifold + { + if (manifold.Count > 0) { + manifold.GetContact(0, out var offset, out var normal, out var depth, out var featureId); + var extra = 1e-16 * (depth + offset.X + normal.X); + *Count += 1 + (int)extra; } - - public bool AllowCollisionTesting(int pairId, int childA, int childB) + else { - return true; + ++*Count; } - } - - static void TestPair(ref TA a, ref TB b, ref Buffer posesA, ref Buffer posesB, - ref TestCollisionCallbacks callbacks, BufferPool pool, Shapes shapes, CollisionTaskRegistry registry, int iterationCount) - where TA : struct, IShape where TB : struct, IShape + public void OnChildPairCompleted(int pairId, int childA, int childB, ref ConvexContactManifold manifold) { - var batcher = new CollisionBatcher(pool, shapes, registry, Demo.TimestepDuration, callbacks); - for (int i = 0; i < iterationCount; ++i) - { - ref var poseA = ref posesA[i]; - ref var poseB = ref posesB[i]; - batcher.Add(a, b, poseB.Position - poseA.Position, poseA.Orientation, poseB.Orientation, 0.1f, 0); - } - batcher.Flush(); } - unsafe static void Test(ref TA a, ref TB b, ref Buffer posesA, ref Buffer posesB, - BufferPool pool, Shapes shapes, CollisionTaskRegistry registry, int iterationCount) - where TA : struct, IShape where TB : struct, IShape + public bool AllowCollisionTesting(int pairId, int childA, int childB) { - int count = 0; - var callbacks = new TestCollisionCallbacks { Count = &count }; - TestPair(ref a, ref b, ref posesA, ref posesB, ref callbacks, pool, shapes, registry, 256); - count = 0; - var start = Stopwatch.GetTimestamp(); - TestPair(ref a, ref b, ref posesA, ref posesB, ref callbacks, pool, shapes, registry, iterationCount); - var end = Stopwatch.GetTimestamp(); - var time = (end - start) / (double)Stopwatch.Frequency; - Console.WriteLine($"Completed {count} {typeof(TA).Name}-{typeof(TB).Name} pairs, time (ms): {1e3 * time}, time per pair (ns): {1e9 * time / *callbacks.Count}"); - //Console.WriteLine($"{typeof(TA).Name}-{typeof(TB).Name}, {1e9 * time / *callbacks.Count}"); - //Console.WriteLine($"{typeof(TA).Name}-{typeof(TB).Name} {1e9 * time / *callbacks.Count}"); + return true; } - [MethodImpl(MethodImplOptions.NoInlining)] - static float TestPair(ref TAWide a, ref TBWide b, ref TDistanceTester tester, - ref Buffer posesA, ref Buffer posesB, int iterationCount) - where TA : IShape where TB : IShape - where TAWide : struct, IShapeWide where TBWide : struct, IShapeWide - where TDistanceTester : IPairDistanceTester + } + + + static void TestPair(ref TA a, ref TB b, ref Buffer posesA, ref Buffer posesB, + ref TestCollisionCallbacks callbacks, BufferPool pool, Shapes shapes, CollisionTaskRegistry registry, int iterationCount) + where TA : struct, IShape where TB : struct, IShape + { + var batcher = new CollisionBatcher(pool, shapes, registry, Demo.TimestepDuration, callbacks); + for (int i = 0; i < iterationCount; ++i) { - var distanceSum = Vector.Zero; - for (int i = 0; i < iterationCount; ++i) - { - ref var poseA = ref posesA[i]; - ref var poseB = ref posesB[i]; - Vector3Wide.Broadcast(poseB.Position - poseA.Position, out var offsetB); - QuaternionWide.Broadcast(poseA.Orientation, out var orientationA); - QuaternionWide.Broadcast(poseB.Orientation, out var orientationB); - tester.Test(a, b, offsetB, orientationA, orientationB, Vector.Zero, out var intersected, out var distance, out var closestA, out var normal); - distanceSum += distance; - } - return distanceSum[0]; + ref var poseA = ref posesA[i]; + ref var poseB = ref posesB[i]; + batcher.Add(a, b, poseB.Position - poseA.Position, poseA.Orientation, poseB.Orientation, 0.1f, 0); } + batcher.Flush(); + } + + unsafe static void Test(ref TA a, ref TB b, ref Buffer posesA, ref Buffer posesB, + BufferPool pool, Shapes shapes, CollisionTaskRegistry registry, int iterationCount) + where TA : struct, IShape where TB : struct, IShape + { + int count = 0; + var callbacks = new TestCollisionCallbacks { Count = &count }; + TestPair(ref a, ref b, ref posesA, ref posesB, ref callbacks, pool, shapes, registry, 256); + count = 0; + var start = Stopwatch.GetTimestamp(); + TestPair(ref a, ref b, ref posesA, ref posesB, ref callbacks, pool, shapes, registry, iterationCount); + var end = Stopwatch.GetTimestamp(); + var time = (end - start) / (double)Stopwatch.Frequency; + Console.WriteLine($"Completed {count} {typeof(TA).Name}-{typeof(TB).Name} pairs, time (ms): {1e3 * time}, time per pair (ns): {1e9 * time / *callbacks.Count}"); + //Console.WriteLine($"{typeof(TA).Name}-{typeof(TB).Name}, {1e9 * time / *callbacks.Count}"); + //Console.WriteLine($"{typeof(TA).Name}-{typeof(TB).Name} {1e9 * time / *callbacks.Count}"); + } - static void Test(in TA a, in TB b, - ref Buffer posesA, ref Buffer posesB, int iterationCount) - where TA : unmanaged, IShape where TB : unmanaged, IShape - where TAWide : unmanaged, IShapeWide where TBWide : unmanaged, IShapeWide - where TDistanceTester : struct, IPairDistanceTester + [MethodImpl(MethodImplOptions.NoInlining)] + static float TestPair(ref TAWide a, ref TBWide b, ref TDistanceTester tester, + ref Buffer posesA, ref Buffer posesB, int iterationCount) + where TA : IShape where TB : IShape + where TAWide : struct, IShapeWide where TBWide : struct, IShapeWide + where TDistanceTester : IPairDistanceTester + { + var distanceSum = Vector.Zero; + for (int i = 0; i < iterationCount; ++i) { - TAWide aWide = default; - aWide.Broadcast(a); - TBWide bWide = default; - bWide.Broadcast(b); - var tester = default(TDistanceTester); - TestPair(ref aWide, ref bWide, ref tester, ref posesA, ref posesB, 64); - var start = Stopwatch.GetTimestamp(); - TestPair(ref aWide, ref bWide, ref tester, ref posesA, ref posesB, iterationCount); - var end = Stopwatch.GetTimestamp(); - var time = (end - start) / (double)Stopwatch.Frequency; - var instanceCount = Vector.Count * iterationCount; - Console.WriteLine($"Completed {instanceCount} {typeof(TA).Name}-{typeof(TB).Name} distance test instances using {typeof(TDistanceTester).Name}, time (ms): {1e3 * time}, time per instance (ns): {1e9 * time / instanceCount}"); + ref var poseA = ref posesA[i]; + ref var poseB = ref posesB[i]; + Vector3Wide.Broadcast(poseB.Position - poseA.Position, out var offsetB); + QuaternionWide.Broadcast(poseA.Orientation, out var orientationA); + QuaternionWide.Broadcast(poseB.Orientation, out var orientationB); + tester.Test(a, b, offsetB, orientationA, orientationB, Vector.Zero, out var intersected, out var distance, out var closestA, out var normal); + distanceSum += distance; } + return distanceSum[0]; + } - static void GetRandomPose(Random random, out RigidPose pose) - { - pose.Position = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); + static void Test(in TA a, in TB b, + ref Buffer posesA, ref Buffer posesB, int iterationCount) + where TA : unmanaged, IShape where TB : unmanaged, IShape + where TAWide : unmanaged, IShapeWide where TBWide : unmanaged, IShapeWide + where TDistanceTester : struct, IPairDistanceTester + { + TAWide aWide = default; + aWide.Broadcast(a); + TBWide bWide = default; + bWide.Broadcast(b); + var tester = default(TDistanceTester); + TestPair(ref aWide, ref bWide, ref tester, ref posesA, ref posesB, 64); + var start = Stopwatch.GetTimestamp(); + TestPair(ref aWide, ref bWide, ref tester, ref posesA, ref posesB, iterationCount); + var end = Stopwatch.GetTimestamp(); + var time = (end - start) / (double)Stopwatch.Frequency; + var instanceCount = Vector.Count * iterationCount; + Console.WriteLine($"Completed {instanceCount} {typeof(TA).Name}-{typeof(TB).Name} distance test instances using {typeof(TDistanceTester).Name}, time (ms): {1e3 * time}, time per instance (ns): {1e9 * time / instanceCount}"); + } - float orientationLengthSquared; - do + static void GetRandomPose(Random random, out RigidPose pose) + { + pose.Position = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); + + float orientationLengthSquared; + do + { + pose.Orientation = new Quaternion { - pose.Orientation = new Quaternion - { - X = 2 * random.NextSingle() - 1, - Y = 2 * random.NextSingle() - 1, - Z = 2 * random.NextSingle() - 1, - W = 2 * random.NextSingle() - 1 - }; - } - while ((orientationLengthSquared = pose.Orientation.LengthSquared()) < 1e-5f); - var inverseLength = 1f / MathF.Sqrt(orientationLengthSquared); - pose.Orientation.X *= inverseLength; - pose.Orientation.Y *= inverseLength; - pose.Orientation.Z *= inverseLength; - pose.Orientation.W *= inverseLength; + X = 2 * random.NextSingle() - 1, + Y = 2 * random.NextSingle() - 1, + Z = 2 * random.NextSingle() - 1, + W = 2 * random.NextSingle() - 1 + }; } + while ((orientationLengthSquared = pose.Orientation.LengthSquared()) < 1e-5f); + var inverseLength = 1f / MathF.Sqrt(orientationLengthSquared); + pose.Orientation.X *= inverseLength; + pose.Orientation.Y *= inverseLength; + pose.Orientation.Z *= inverseLength; + pose.Orientation.W *= inverseLength; + } - public static void Test() - { - var pool = new BufferPool(); - var random = new Random(5); - var registry = DefaultTypes.CreateDefaultCollisionTaskRegistry(); - var sphere = new Sphere(1); - var capsule = new Capsule(0.5f, 1f); - var box = new Box(1f, 1f, 1f); - var triangle = new Triangle(new Vector3(0, 0, 0), new Vector3(1, 0, 0), new Vector3(0, 0, 1)); - var cylinder = new Cylinder(0.5f, 1f); + public static void Test() + { + var pool = new BufferPool(); + var random = new Random(5); + var registry = DefaultTypes.CreateDefaultCollisionTaskRegistry(); + var sphere = new Sphere(1); + var capsule = new Capsule(0.5f, 1f); + var box = new Box(1f, 1f, 1f); + var triangle = new Triangle(new Vector3(0, 0, 0), new Vector3(1, 0, 0), new Vector3(0, 0, 1)); + var cylinder = new Cylinder(0.5f, 1f); - const int pointCount = 64; - var points = new QuickList(pointCount, pool); - //points.Allocate(pool) = new Vector3(0, 0, 0); - //points.Allocate(pool) = new Vector3(0, 0, 1); - //points.Allocate(pool) = new Vector3(0, 1, 0); - //points.Allocate(pool) = new Vector3(0, 1, 1); - //points.Allocate(pool) = new Vector3(1, 0, 0); - //points.Allocate(pool) = new Vector3(1, 0, 1); - //points.Allocate(pool) = new Vector3(1, 1, 0); - //points.Allocate(pool) = new Vector3(1, 1, 1); - for (int i = 0; i < pointCount; ++i) - { - points.AllocateUnsafely() = new Vector3(random.NextSingle(), 1 * random.NextSingle(), random.NextSingle()); - //points.AllocateUnsafely() = new Vector3(0, 1, 0) + Vector3.Normalize(new Vector3(random.NextSingle() * 2 - 1, random.NextSingle() * 2 - 1, random.NextSingle() * 2 - 1)) * random.NextSingle(); - } + const int pointCount = 64; + var points = new QuickList(pointCount, pool); + //points.Allocate(pool) = new Vector3(0, 0, 0); + //points.Allocate(pool) = new Vector3(0, 0, 1); + //points.Allocate(pool) = new Vector3(0, 1, 0); + //points.Allocate(pool) = new Vector3(0, 1, 1); + //points.Allocate(pool) = new Vector3(1, 0, 0); + //points.Allocate(pool) = new Vector3(1, 0, 1); + //points.Allocate(pool) = new Vector3(1, 1, 0); + //points.Allocate(pool) = new Vector3(1, 1, 1); + for (int i = 0; i < pointCount; ++i) + { + points.AllocateUnsafely() = new Vector3(random.NextSingle(), 1 * random.NextSingle(), random.NextSingle()); + //points.AllocateUnsafely() = new Vector3(0, 1, 0) + Vector3.Normalize(new Vector3(random.NextSingle() * 2 - 1, random.NextSingle() * 2 - 1, random.NextSingle() * 2 - 1)) * random.NextSingle(); + } - Shapes shapes = new Shapes(pool, 32); - var pointsBuffer = points.Span.Slice(points.Count); - ConvexHullHelper.CreateShape(pointsBuffer, pool, out _, out var convexHull); + Shapes shapes = new Shapes(pool, 32); + var pointsBuffer = points.Span.Slice(points.Count); + ConvexHullHelper.CreateShape(pointsBuffer, pool, out _, out var convexHull); - using var compoundBuilder = new CompoundBuilder(pool, shapes, 64); - //COMPOUND - var legShape = new Box(0.2f, 1, 0.2f); - var legInverseInertia = legShape.ComputeInertia(1f); - var legShapeIndex = shapes.Add(legShape); - var legPose0 = new RigidPose { Position = new Vector3(-1.5f, 0, -1.5f), Orientation = Quaternion.Identity }; - var legPose1 = new RigidPose { Position = new Vector3(-1.5f, 0, 1.5f), Orientation = Quaternion.Identity }; - var legPose2 = new RigidPose { Position = new Vector3(1.5f, 0, -1.5f), Orientation = Quaternion.Identity }; - var legPose3 = new RigidPose { Position = new Vector3(1.5f, 0, 1.5f), Orientation = Quaternion.Identity }; - compoundBuilder.Add(legShapeIndex, legPose0, legInverseInertia.InverseInertiaTensor, 1); - compoundBuilder.Add(legShapeIndex, legPose1, legInverseInertia.InverseInertiaTensor, 1); - compoundBuilder.Add(legShapeIndex, legPose2, legInverseInertia.InverseInertiaTensor, 1); - compoundBuilder.Add(legShapeIndex, legPose3, legInverseInertia.InverseInertiaTensor, 1); - var tableTopPose = new RigidPose { Position = new Vector3(0, 0.6f, 0), Orientation = Quaternion.Identity }; - var tableTopShape = new Box(3.2f, 0.2f, 3.2f); - compoundBuilder.Add(tableTopShape, tableTopPose, 3); + using var compoundBuilder = new CompoundBuilder(pool, shapes, 64); + //COMPOUND + var legShape = new Box(0.2f, 1, 0.2f); + var legInverseInertia = legShape.ComputeInertia(1f); + var legShapeIndex = shapes.Add(legShape); + var legPose0 = new RigidPose { Position = new Vector3(-1.5f, 0, -1.5f), Orientation = Quaternion.Identity }; + var legPose1 = new RigidPose { Position = new Vector3(-1.5f, 0, 1.5f), Orientation = Quaternion.Identity }; + var legPose2 = new RigidPose { Position = new Vector3(1.5f, 0, -1.5f), Orientation = Quaternion.Identity }; + var legPose3 = new RigidPose { Position = new Vector3(1.5f, 0, 1.5f), Orientation = Quaternion.Identity }; + compoundBuilder.Add(legShapeIndex, legPose0, legInverseInertia.InverseInertiaTensor, 1); + compoundBuilder.Add(legShapeIndex, legPose1, legInverseInertia.InverseInertiaTensor, 1); + compoundBuilder.Add(legShapeIndex, legPose2, legInverseInertia.InverseInertiaTensor, 1); + compoundBuilder.Add(legShapeIndex, legPose3, legInverseInertia.InverseInertiaTensor, 1); + var tableTopPose = new RigidPose { Position = new Vector3(0, 0.6f, 0), Orientation = Quaternion.Identity }; + var tableTopShape = new Box(3.2f, 0.2f, 3.2f); + compoundBuilder.Add(tableTopShape, tableTopPose, 3); - compoundBuilder.BuildDynamicCompound(out var tableChildren, out var tableInertia, out var tableCenter); - compoundBuilder.Reset(); - var compound = new Compound(tableChildren); + compoundBuilder.BuildDynamicCompound(out var tableChildren, out var tableInertia, out var tableCenter); + compoundBuilder.Reset(); + var compound = new Compound(tableChildren); - //BIGCOMPOUND - var treeCompoundBoxShape = new Box(0.5f, 1.5f, 1f); - var treeCompoundBoxShapeIndex = shapes.Add(treeCompoundBoxShape); - var childInertia = treeCompoundBoxShape.ComputeInertia(1); - for (int i = 0; i < 64; ++i) + //BIGCOMPOUND + var treeCompoundBoxShape = new Box(0.5f, 1.5f, 1f); + var treeCompoundBoxShapeIndex = shapes.Add(treeCompoundBoxShape); + var childInertia = treeCompoundBoxShape.ComputeInertia(1); + for (int i = 0; i < 64; ++i) + { + RigidPose localPose; + localPose.Position = new Vector3(12, 12, 12) * (0.5f * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) - Vector3.One); + float orientationLengthSquared; + do { - RigidPose localPose; - localPose.Position = new Vector3(12, 12, 12) * (0.5f * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) - Vector3.One); - float orientationLengthSquared; - do - { - localPose.Orientation = new Quaternion(random.NextSingle(), random.NextSingle(), random.NextSingle(), random.NextSingle()); - orientationLengthSquared = QuaternionEx.LengthSquared(ref localPose.Orientation); - } - while (orientationLengthSquared < 1e-9f); - QuaternionEx.Scale(localPose.Orientation, 1f / MathF.Sqrt(orientationLengthSquared), out localPose.Orientation); - //Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), MathF.PI, out localPose.Orientation); - - compoundBuilder.Add(treeCompoundBoxShapeIndex, localPose, childInertia.InverseInertiaTensor, 1); + localPose.Orientation = new Quaternion(random.NextSingle(), random.NextSingle(), random.NextSingle(), random.NextSingle()); + orientationLengthSquared = QuaternionEx.LengthSquared(ref localPose.Orientation); } - compoundBuilder.BuildDynamicCompound(out var children, out var inertia, out var center); - compoundBuilder.Reset(); - var bigCompound = new BigCompound(children, shapes, pool); + while (orientationLengthSquared < 1e-9f); + QuaternionEx.Scale(localPose.Orientation, 1f / MathF.Sqrt(orientationLengthSquared), out localPose.Orientation); + //Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), MathF.PI, out localPose.Orientation); + + compoundBuilder.Add(treeCompoundBoxShapeIndex, localPose, childInertia.InverseInertiaTensor, 1); + } + compoundBuilder.BuildDynamicCompound(out var children, out var inertia, out var center); + compoundBuilder.Reset(); + var bigCompound = new BigCompound(children, shapes, pool); - //MESH - var mesh = DemoMeshHelper.CreateDeformedPlane(8, 8, (x, y) => { return new Vector3(x * 2 - 8, 3 * MathF.Sin(x) * MathF.Sin(y), y * 2 - 8); }, Vector3.One, pool); + //MESH + var mesh = DemoMeshHelper.CreateDeformedPlane(8, 8, (x, y) => { return new Vector3(x * 2 - 8, 3 * MathF.Sin(x) * MathF.Sin(y), y * 2 - 8); }, Vector3.One, pool); - int iterationCount = 1 << 20; - pool.Take(iterationCount, out var posesA); - pool.Take(iterationCount, out var posesB); - for (int i = 0; i < iterationCount; ++i) - { - GetRandomPose(random, out posesA[i]); - GetRandomPose(random, out posesB[i]); - } + int iterationCount = 1 << 20; + pool.Take(iterationCount, out var posesA); + pool.Take(iterationCount, out var posesB); + for (int i = 0; i < iterationCount; ++i) + { + GetRandomPose(random, out posesA[i]); + GetRandomPose(random, out posesB[i]); + } - Test(ref sphere, ref sphere, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref sphere, ref capsule, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref sphere, ref box, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref sphere, ref triangle, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref sphere, ref cylinder, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref sphere, ref convexHull, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref sphere, ref compound, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref sphere, ref bigCompound, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref sphere, ref mesh, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref capsule, ref capsule, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref capsule, ref box, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref capsule, ref triangle, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref capsule, ref cylinder, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref capsule, ref convexHull, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref capsule, ref compound, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref capsule, ref bigCompound, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref capsule, ref mesh, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref box, ref box, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref box, ref triangle, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref box, ref cylinder, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref box, ref convexHull, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref box, ref compound, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref box, ref bigCompound, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref box, ref mesh, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref triangle, ref triangle, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref triangle, ref cylinder, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref triangle, ref convexHull, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref triangle, ref compound, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref triangle, ref bigCompound, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref triangle, ref mesh, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref cylinder, ref cylinder, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref cylinder, ref convexHull, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref cylinder, ref compound, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref cylinder, ref bigCompound, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref cylinder, ref mesh, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref convexHull, ref convexHull, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref convexHull, ref compound, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref convexHull, ref bigCompound, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref convexHull, ref mesh, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref compound, ref compound, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref compound, ref bigCompound, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref compound, ref mesh, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref bigCompound, ref bigCompound, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref bigCompound, ref mesh, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(ref mesh, ref mesh, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref sphere, ref sphere, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref sphere, ref capsule, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref sphere, ref box, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref sphere, ref triangle, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref sphere, ref cylinder, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref sphere, ref convexHull, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref sphere, ref compound, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref sphere, ref bigCompound, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref sphere, ref mesh, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref capsule, ref capsule, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref capsule, ref box, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref capsule, ref triangle, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref capsule, ref cylinder, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref capsule, ref convexHull, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref capsule, ref compound, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref capsule, ref bigCompound, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref capsule, ref mesh, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref box, ref box, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref box, ref triangle, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref box, ref cylinder, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref box, ref convexHull, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref box, ref compound, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref box, ref bigCompound, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref box, ref mesh, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref triangle, ref triangle, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref triangle, ref cylinder, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref triangle, ref convexHull, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref triangle, ref compound, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref triangle, ref bigCompound, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref triangle, ref mesh, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref cylinder, ref cylinder, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref cylinder, ref convexHull, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref cylinder, ref compound, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref cylinder, ref bigCompound, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref cylinder, ref mesh, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref convexHull, ref convexHull, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref convexHull, ref compound, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref convexHull, ref bigCompound, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref convexHull, ref mesh, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref compound, ref compound, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref compound, ref bigCompound, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref compound, ref mesh, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref bigCompound, ref bigCompound, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref bigCompound, ref mesh, ref posesA, ref posesB, pool, shapes, registry, iterationCount); + Test(ref mesh, ref mesh, ref posesA, ref posesB, pool, shapes, registry, iterationCount); - Test(sphere, sphere, ref posesA, ref posesB, iterationCount); - Test(sphere, capsule, ref posesA, ref posesB, iterationCount); - Test(sphere, box, ref posesA, ref posesB, iterationCount); - Test(sphere, triangle, ref posesA, ref posesB, iterationCount); - Test(sphere, cylinder, ref posesA, ref posesB, iterationCount); - Test(capsule, capsule, ref posesA, ref posesB, iterationCount); - Test(capsule, box, ref posesA, ref posesB, iterationCount); - Test>(capsule, triangle, ref posesA, ref posesB, iterationCount); - Test>(capsule, cylinder, ref posesA, ref posesB, iterationCount); - Test>(cylinder, box, ref posesA, ref posesB, iterationCount); - Test>(cylinder, triangle, ref posesA, ref posesB, iterationCount); - Test>(cylinder, cylinder, ref posesA, ref posesB, iterationCount); - Test>(box, box, ref posesA, ref posesB, iterationCount); - Test>(box, triangle, ref posesA, ref posesB, iterationCount); - Test>(triangle, triangle, ref posesA, ref posesB, iterationCount); - Console.WriteLine($"Done. Hit enter to exit."); - Console.ReadLine(); - } + Test(sphere, sphere, ref posesA, ref posesB, iterationCount); + Test(sphere, capsule, ref posesA, ref posesB, iterationCount); + Test(sphere, box, ref posesA, ref posesB, iterationCount); + Test(sphere, triangle, ref posesA, ref posesB, iterationCount); + Test(sphere, cylinder, ref posesA, ref posesB, iterationCount); + Test(capsule, capsule, ref posesA, ref posesB, iterationCount); + Test(capsule, box, ref posesA, ref posesB, iterationCount); + Test>(capsule, triangle, ref posesA, ref posesB, iterationCount); + Test>(capsule, cylinder, ref posesA, ref posesB, iterationCount); + Test>(cylinder, box, ref posesA, ref posesB, iterationCount); + Test>(cylinder, triangle, ref posesA, ref posesB, iterationCount); + Test>(cylinder, cylinder, ref posesA, ref posesB, iterationCount); + Test>(box, box, ref posesA, ref posesB, iterationCount); + Test>(box, triangle, ref posesA, ref posesB, iterationCount); + Test>(triangle, triangle, ref posesA, ref posesB, iterationCount); + Console.WriteLine($"Done. Hit enter to exit."); + Console.ReadLine(); } } diff --git a/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs b/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs index 3bd0a212c..b3faf5309 100644 --- a/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs +++ b/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs @@ -11,179 +11,178 @@ using BepuPhysics.Trees; using static Demos.SpecializedTests.TreeFiddlingTestDemo; -namespace Demos.SpecializedTests +namespace Demos.SpecializedTests; + +public class BroadPhaseStressTestDemo : Demo { - public class BroadPhaseStressTestDemo : Demo + + public struct NoNarrowphaseTestingCallbacks : INarrowPhaseCallbacks { - public struct NoNarrowphaseTestingCallbacks : INarrowPhaseCallbacks + public NoNarrowphaseTestingCallbacks() { + } - public NoNarrowphaseTestingCallbacks() - { - } - - public void Initialize(Simulation simulation) - { - } + public void Initialize(Simulation simulation) + { + } - public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin) - { - return false; - } + public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin) + { + return false; + } - public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB) - { - return false; - } + public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB) + { + return false; + } - public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold - { - pairMaterial = default; - return false; - } + public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold + { + pairMaterial = default; + return false; + } - public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold) - { - return false; - } + public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold) + { + return false; + } - public void Dispose() - { - } + public void Dispose() + { } + } - Vector3[] startingLocations; + Vector3[] startingLocations; - public override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(-20f, 13, -20f); + camera.Yaw = MathHelper.Pi * 3f / 4; + camera.Pitch = MathHelper.Pi * 0.1f; + Simulation = Simulation.Create(BufferPool, new NoNarrowphaseTestingCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)), new SolveDescription(1, 1)); + + var shape = new Sphere(0.5f); + var sphereInertia = shape.ComputeInertia(1); + var shapeIndex = Simulation.Shapes.Add(shape); + const int width = 2048; + const int height = 2; + const int length = 2048; + var spacing = new Vector3(16.01f); + float randomization = 0.9f; + var randomizationSpan = (spacing - new Vector3(1)) * randomization; + var randomizationBase = randomizationSpan * -0.5f; + var random = new Random(5); + for (int i = 0; i < width; ++i) { - camera.Position = new Vector3(-20f, 13, -20f); - camera.Yaw = MathHelper.Pi * 3f / 4; - camera.Pitch = MathHelper.Pi * 0.1f; - Simulation = Simulation.Create(BufferPool, new NoNarrowphaseTestingCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)), new SolveDescription(1, 1)); - - var shape = new Sphere(0.5f); - var sphereInertia = shape.ComputeInertia(1); - var shapeIndex = Simulation.Shapes.Add(shape); - const int width = 2048; - const int height = 2; - const int length = 2048; - var spacing = new Vector3(16.01f); - float randomization = 0.9f; - var randomizationSpan = (spacing - new Vector3(1)) * randomization; - var randomizationBase = randomizationSpan * -0.5f; - var random = new Random(5); - for (int i = 0; i < width; ++i) + for (int j = 0; j < height; ++j) { - for (int j = 0; j < height; ++j) + for (int k = 0; k < length; ++k) { - for (int k = 0; k < length; ++k) + var r = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); + //var location = spacing * (new Vector3(i, j, k) + new Vector3(-width, 1, -length)) + randomizationBase + r * randomizationSpan; + var location = (r - new Vector3(0.5f)) * (r - new Vector3(0.5f)) * spacing * new Vector3(width, height, length); + //var location = (r - new Vector3(0.5f)) * spacing * new Vector3(width, height, length); + //var hash = HashHelper.Rehash(HashHelper.Rehash(HashHelper.Rehash(i) + HashHelper.Rehash(j)) + HashHelper.Rehash(k)); + var hash = i + j + k; + if (hash % 64 == 0) { - var r = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); - //var location = spacing * (new Vector3(i, j, k) + new Vector3(-width, 1, -length)) + randomizationBase + r * randomizationSpan; - var location = (r - new Vector3(0.5f)) * (r - new Vector3(0.5f)) * spacing * new Vector3(width, height, length); - //var location = (r - new Vector3(0.5f)) * spacing * new Vector3(width, height, length); - //var hash = HashHelper.Rehash(HashHelper.Rehash(HashHelper.Rehash(i) + HashHelper.Rehash(j)) + HashHelper.Rehash(k)); - var hash = i + j + k; - if (hash % 64 == 0) - { - if (i == 7 && j == 1 && k == 0) - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(100, 0, 100), sphereInertia, Simulation.Shapes.Add(new Sphere(100)), -1)); - else - Simulation.Bodies.Add(BodyDescription.CreateDynamic(location, sphereInertia, shapeIndex, -1)); - } + if (i == 7 && j == 1 && k == 0) + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(100, 0, 100), sphereInertia, Simulation.Shapes.Add(new Sphere(100)), -1)); else - { - Simulation.Statics.Add(new StaticDescription(location, shapeIndex)); - } + Simulation.Bodies.Add(BodyDescription.CreateDynamic(location, sphereInertia, shapeIndex, -1)); + } + else + { + Simulation.Statics.Add(new StaticDescription(location, shapeIndex)); } } } - Console.WriteLine($"Body count: {Simulation.Bodies.ActiveSet.Count}"); - Console.WriteLine($"Static count: {Simulation.Statics.Count}"); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -10, 0), Simulation.Shapes.Add(new Box(5000, 1, 5000)))); - startingLocations = new Vector3[Simulation.Bodies.ActiveSet.Count]; - for (int i = 0; i < startingLocations.Length; ++i) - { - startingLocations[i] = Simulation.Bodies.ActiveSet.DynamicsState[i].Motion.Pose.Position; - } - updateTimes = new TimingsRingBuffer(sampleCount, BufferPool); - testTimes = new TimingsRingBuffer(sampleCount, BufferPool); - test2Times = new TimingsRingBuffer(sampleCount, BufferPool); - intertreeTest2Times = new TimingsRingBuffer(sampleCount, BufferPool); } + Console.WriteLine($"Body count: {Simulation.Bodies.ActiveSet.Count}"); + Console.WriteLine($"Static count: {Simulation.Statics.Count}"); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -10, 0), Simulation.Shapes.Add(new Box(5000, 1, 5000)))); + startingLocations = new Vector3[Simulation.Bodies.ActiveSet.Count]; + for (int i = 0; i < startingLocations.Length; ++i) + { + startingLocations[i] = Simulation.Bodies.ActiveSet.DynamicsState[i].Motion.Pose.Position; + } + updateTimes = new TimingsRingBuffer(sampleCount, BufferPool); + testTimes = new TimingsRingBuffer(sampleCount, BufferPool); + test2Times = new TimingsRingBuffer(sampleCount, BufferPool); + intertreeTest2Times = new TimingsRingBuffer(sampleCount, BufferPool); + } - const int sampleCount = 128; - TimingsRingBuffer updateTimes; - TimingsRingBuffer testTimes; - TimingsRingBuffer test2Times; - TimingsRingBuffer intertreeTest2Times; - long frameCount; - public override void Update(Window window, Camera camera, Input input, float dt) + const int sampleCount = 128; + TimingsRingBuffer updateTimes; + TimingsRingBuffer testTimes; + TimingsRingBuffer test2Times; + TimingsRingBuffer intertreeTest2Times; + long frameCount; + public override void Update(Window window, Camera camera, Input input, float dt) + { + var rotationAngle = frameCount * 1e-3f; + var rotation = Matrix3x3.CreateFromAxisAngle(Vector3.UnitY, rotationAngle); + //for (int i = 0; i < Simulation.Bodies.ActiveSet.Count / 2; ++i) + //{ + // //For every body, set the velocity such that body moves toward some body-specific goal state that evolves over time. + // Matrix3x3.Transform(startingLocations[i], rotation, out var targetLocation); + // ref var motion = ref Simulation.Bodies.ActiveSet.DynamicsState[i].Motion; + // var offset = targetLocation - motion.Pose.Position; + // motion.Velocity.Linear = offset; + //} + + //Simulation.BroadPhase.ActiveTree.CacheOptimize(0); + //Simulation.BroadPhase.StaticTree.CacheOptimize(0); + base.Update(window, camera, input, dt); + updateTimes.Add(Simulation.Profiler[Simulation.BroadPhase]); + testTimes.Add(Simulation.Profiler[Simulation.BroadPhaseOverlapFinder]); + + //var overlaps = new OverlapHandler(); + //Simulation.BroadPhase.ActiveTree.GetSelfOverlaps2(ref overlaps); + //var a = Stopwatch.GetTimestamp(); + //var threadedOverlaps = new TreeFiddlingTestDemo.ThreadedOverlapHandler(BufferPool, ThreadDispatcher.ThreadCount); + //Simulation.BroadPhase.ActiveTree.GetSelfOverlaps2(ref threadedOverlaps, BufferPool, ThreadDispatcher); + //var (selfOverlapCount, _) = threadedOverlaps.SumResults(); + //threadedOverlaps.Reset(); + //var b = Stopwatch.GetTimestamp(); + //Simulation.BroadPhase.ActiveTree.GetOverlaps2(ref Simulation.BroadPhase.ActiveTree, ref threadedOverlaps, BufferPool, ThreadDispatcher); + //var c = Stopwatch.GetTimestamp(); + //var interOverlaps = new OverlapHandler(); + ////Simulation.BroadPhase.ActiveTree.GetOverlaps(ref Simulation.BroadPhase.ActiveTree, ref interOverlaps); + //var (interOverlapCount, _) = threadedOverlaps.SumResults(); + //test2Times.Add((b - a) / (double)Stopwatch.Frequency); + //intertreeTest2Times.Add((c - b) / (double)Stopwatch.Frequency); + + + if (frameCount++ % sampleCount == 0) { - var rotationAngle = frameCount * 1e-3f; - var rotation = Matrix3x3.CreateFromAxisAngle(Vector3.UnitY, rotationAngle); - //for (int i = 0; i < Simulation.Bodies.ActiveSet.Count / 2; ++i) + var updateStats = updateTimes.ComputeStats(); + var testStats = testTimes.ComputeStats(); + var test2Stats = test2Times.ComputeStats(); + var intertreeTest2Stats = intertreeTest2Times.ComputeStats(); + Console.WriteLine($"Update: {updateStats.Average * 1000} ms average, {updateStats.StdDev * 1000} stddev"); + Console.WriteLine($"Test: {testStats.Average * 1000} ms average, {testStats.StdDev * 1000} stddev"); + //Console.WriteLine($"Test2: {test2Stats.Average * 1000} ms average, {test2Stats.StdDev * 1000} stddev"); + //Console.WriteLine($"Inter2: {intertreeTest2Stats.Average * 1000} ms average, {intertreeTest2Stats.StdDev * 1000} stddev"); + Console.WriteLine($"Active Cost: {Simulation.BroadPhase.ActiveTree.MeasureCostMetric()}"); + Console.WriteLine($"Static Cost: {Simulation.BroadPhase.StaticTree.MeasureCostMetric()}"); + //Console.WriteLine($"Active CQ: {Simulation.BroadPhase.ActiveTree.MeasureCacheQuality()}"); + //Console.WriteLine($"Static CQ: {Simulation.BroadPhase.StaticTree.MeasureCacheQuality()}"); + + //var min = int.MaxValue; + //var max = 0; + //for (int i = 0; i < threadedOverlaps.Workers.Length; ++i) //{ - // //For every body, set the velocity such that body moves toward some body-specific goal state that evolves over time. - // Matrix3x3.Transform(startingLocations[i], rotation, out var targetLocation); - // ref var motion = ref Simulation.Bodies.ActiveSet.DynamicsState[i].Motion; - // var offset = targetLocation - motion.Pose.Position; - // motion.Velocity.Linear = offset; + // var count = threadedOverlaps.Workers[i].OverlapCount; + // //Console.Write($"{count}, "); + // min = int.Min(count, min); + // max = int.Max(count, max); //} + //Console.WriteLine($"min, max: {min}, {max}"); - //Simulation.BroadPhase.ActiveTree.CacheOptimize(0); - //Simulation.BroadPhase.StaticTree.CacheOptimize(0); - base.Update(window, camera, input, dt); - updateTimes.Add(Simulation.Profiler[Simulation.BroadPhase]); - testTimes.Add(Simulation.Profiler[Simulation.BroadPhaseOverlapFinder]); - - //var overlaps = new OverlapHandler(); - //Simulation.BroadPhase.ActiveTree.GetSelfOverlaps2(ref overlaps); - //var a = Stopwatch.GetTimestamp(); - //var threadedOverlaps = new TreeFiddlingTestDemo.ThreadedOverlapHandler(BufferPool, ThreadDispatcher.ThreadCount); - //Simulation.BroadPhase.ActiveTree.GetSelfOverlaps2(ref threadedOverlaps, BufferPool, ThreadDispatcher); - //var (selfOverlapCount, _) = threadedOverlaps.SumResults(); - //threadedOverlaps.Reset(); - //var b = Stopwatch.GetTimestamp(); - //Simulation.BroadPhase.ActiveTree.GetOverlaps2(ref Simulation.BroadPhase.ActiveTree, ref threadedOverlaps, BufferPool, ThreadDispatcher); - //var c = Stopwatch.GetTimestamp(); - //var interOverlaps = new OverlapHandler(); - ////Simulation.BroadPhase.ActiveTree.GetOverlaps(ref Simulation.BroadPhase.ActiveTree, ref interOverlaps); - //var (interOverlapCount, _) = threadedOverlaps.SumResults(); - //test2Times.Add((b - a) / (double)Stopwatch.Frequency); - //intertreeTest2Times.Add((c - b) / (double)Stopwatch.Frequency); - - - if (frameCount++ % sampleCount == 0) - { - var updateStats = updateTimes.ComputeStats(); - var testStats = testTimes.ComputeStats(); - var test2Stats = test2Times.ComputeStats(); - var intertreeTest2Stats = intertreeTest2Times.ComputeStats(); - Console.WriteLine($"Update: {updateStats.Average * 1000} ms average, {updateStats.StdDev * 1000} stddev"); - Console.WriteLine($"Test: {testStats.Average * 1000} ms average, {testStats.StdDev * 1000} stddev"); - //Console.WriteLine($"Test2: {test2Stats.Average * 1000} ms average, {test2Stats.StdDev * 1000} stddev"); - //Console.WriteLine($"Inter2: {intertreeTest2Stats.Average * 1000} ms average, {intertreeTest2Stats.StdDev * 1000} stddev"); - Console.WriteLine($"Active Cost: {Simulation.BroadPhase.ActiveTree.MeasureCostMetric()}"); - Console.WriteLine($"Static Cost: {Simulation.BroadPhase.StaticTree.MeasureCostMetric()}"); - //Console.WriteLine($"Active CQ: {Simulation.BroadPhase.ActiveTree.MeasureCacheQuality()}"); - //Console.WriteLine($"Static CQ: {Simulation.BroadPhase.StaticTree.MeasureCacheQuality()}"); - - //var min = int.MaxValue; - //var max = 0; - //for (int i = 0; i < threadedOverlaps.Workers.Length; ++i) - //{ - // var count = threadedOverlaps.Workers[i].OverlapCount; - // //Console.Write($"{count}, "); - // min = int.Min(count, min); - // max = int.Max(count, max); - //} - //Console.WriteLine($"min, max: {min}, {max}"); - - } - //threadedOverlaps.Dispose(BufferPool); } - + //threadedOverlaps.Dispose(BufferPool); } + } diff --git a/Demos/SpecializedTests/CacheBlaster.cs b/Demos/SpecializedTests/CacheBlaster.cs index aab3e08cd..8b8231157 100644 --- a/Demos/SpecializedTests/CacheBlaster.cs +++ b/Demos/SpecializedTests/CacheBlaster.cs @@ -2,39 +2,38 @@ using System.Runtime.CompilerServices; using System.Threading.Tasks; -namespace Demos.SpecializedTests +namespace Demos.SpecializedTests; + +public static class CacheBlaster { - public static class CacheBlaster - { - const int byteCount = 1 << 28; - const int intCount = byteCount / 4; - static int vectorCount = intCount / Vector.Count; - static int vectorMask = vectorCount - 1; - static int[] readblob = new int[intCount]; - static int[] writeblob = new int[intCount]; + const int byteCount = 1 << 28; + const int intCount = byteCount / 4; + static int vectorCount = intCount / Vector.Count; + static int vectorMask = vectorCount - 1; + static int[] readblob = new int[intCount]; + static int[] writeblob = new int[intCount]; - /// - /// Attempts to evict most or all of the cache levels to simulate a cold start. - /// Doesn't do a whole lot for simulations so large that they significantly exceed the cache size. - /// - [MethodImpl(MethodImplOptions.NoOptimization)] - public static void Blast() + /// + /// Attempts to evict most or all of the cache levels to simulate a cold start. + /// Doesn't do a whole lot for simulations so large that they significantly exceed the cache size. + /// + [MethodImpl(MethodImplOptions.NoOptimization)] + public static void Blast() + { + //We don't have a guarantee that the processor is using pure LRU replacement. Some modern processors are a little trickier. + //Scrambling the accesses should make it harder for the CPU to keep stuff cached. + const int vectorsPerJob = 32; + int intsPerJob = vectorsPerJob * Vector.Count; + Parallel.For(0, intCount / intsPerJob, jobIndex => { - //We don't have a guarantee that the processor is using pure LRU replacement. Some modern processors are a little trickier. - //Scrambling the accesses should make it harder for the CPU to keep stuff cached. - const int vectorsPerJob = 32; - int intsPerJob = vectorsPerJob * Vector.Count; - Parallel.For(0, intCount / intsPerJob, jobIndex => - { - var baseIndex = jobIndex * intsPerJob; - ref Vector read = ref Unsafe.As>(ref readblob[0]); - ref Vector write = ref Unsafe.As>(ref writeblob[baseIndex]); + var baseIndex = jobIndex * intsPerJob; + ref Vector read = ref Unsafe.As>(ref readblob[0]); + ref Vector write = ref Unsafe.As>(ref writeblob[baseIndex]); - for (int i = 0; i < vectorsPerJob; ++i) - { - Unsafe.Add(ref write, i) = Unsafe.Add(ref read, (i * 104395303) & vectorMask); - } - }); - } + for (int i = 0; i < vectorsPerJob; ++i) + { + Unsafe.Add(ref write, i) = Unsafe.Add(ref read, (i * 104395303) & vectorMask); + } + }); } } \ No newline at end of file diff --git a/Demos/SpecializedTests/CapsuleTestDemo.cs b/Demos/SpecializedTests/CapsuleTestDemo.cs index 12a5fae70..251c82b74 100644 --- a/Demos/SpecializedTests/CapsuleTestDemo.cs +++ b/Demos/SpecializedTests/CapsuleTestDemo.cs @@ -8,50 +8,49 @@ using DemoContentLoader; using BepuPhysics.Constraints; -namespace Demos.SpecializedTests +namespace Demos.SpecializedTests; + +public class CapsuleTestDemo : Demo { - public class CapsuleTestDemo : Demo + public override void Initialize(ContentArchive content, Camera camera) { - public override void Initialize(ContentArchive content, Camera camera) - { - camera.Position = new Vector3(-10, 5, -10); - //camera.Yaw = MathHelper.Pi ; - camera.Yaw = MathHelper.Pi * 3f / 4; - //camera.Pitch = MathHelper.Pi * 0.1f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); + camera.Position = new Vector3(-10, 5, -10); + //camera.Yaw = MathHelper.Pi ; + camera.Yaw = MathHelper.Pi * 3f / 4; + //camera.Pitch = MathHelper.Pi * 0.1f; + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); - var shape = new Capsule(.5f, .5f); - var localInertia = shape.ComputeInertia(1); - var shapeIndex = Simulation.Shapes.Add(shape); - const int width = 1; - const int height = 1; - const int length = 1; - for (int i = 0; i < width; ++i) + var shape = new Capsule(.5f, .5f); + var localInertia = shape.ComputeInertia(1); + var shapeIndex = Simulation.Shapes.Add(shape); + const int width = 1; + const int height = 1; + const int length = 1; + for (int i = 0; i < width; ++i) + { + for (int j = 0; j < height; ++j) { - for (int j = 0; j < height; ++j) + for (int k = 0; k < length; ++k) { - for (int k = 0; k < length; ++k) - { - var location = new Vector3(1.5f, 1.5f, 4.4f) * new Vector3(i, j, k) + new Vector3(-width * 0.5f, 0.5f, -length * 0.5f); - var bodyDescription = BodyDescription.CreateDynamic((location, QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), MathF.PI / 2)), localInertia, new(shapeIndex, 50, 50), -1); - Simulation.Bodies.Add(bodyDescription); + var location = new Vector3(1.5f, 1.5f, 4.4f) * new Vector3(i, j, k) + new Vector3(-width * 0.5f, 0.5f, -length * 0.5f); + var bodyDescription = BodyDescription.CreateDynamic((location, QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), MathF.PI / 2)), localInertia, new(shapeIndex, 50, 50), -1); + Simulation.Bodies.Add(bodyDescription); - } } } - var boxShape = new Box(0.5f, 0.5f, 2.5f); - var boxDescription = BodyDescription.CreateDynamic(new Vector3(1, -0.5f, 0), boxShape.ComputeInertia(1), new(Simulation.Shapes.Add(boxShape), 50, 50), -1); - Simulation.Bodies.Add(boxDescription); + } + var boxShape = new Box(0.5f, 0.5f, 2.5f); + var boxDescription = BodyDescription.CreateDynamic(new Vector3(1, -0.5f, 0), boxShape.ComputeInertia(1), new(Simulation.Shapes.Add(boxShape), 50, 50), -1); + Simulation.Bodies.Add(boxDescription); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -3, 0), Simulation.Shapes.Add(new Box(4, 1, 4)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -3, 0), Simulation.Shapes.Add(new Box(4, 1, 4)))); - } + } - public override void Update(Window window, Camera camera, Input input, float dt) - { - if (input.WasDown(OpenTK.Input.Key.P)) - Console.WriteLine("$"); - base.Update(window, camera, input, dt); - } + public override void Update(Window window, Camera camera, Input input, float dt) + { + if (input.WasDown(OpenTK.Input.Key.P)) + Console.WriteLine("$"); + base.Update(window, camera, input, dt); } } diff --git a/Demos/SpecializedTests/CharacterTestDemo.cs b/Demos/SpecializedTests/CharacterTestDemo.cs index 0773906de..2564359ff 100644 --- a/Demos/SpecializedTests/CharacterTestDemo.cs +++ b/Demos/SpecializedTests/CharacterTestDemo.cs @@ -9,129 +9,128 @@ using Demos.Demos.Characters; using BepuUtilities.Collections; -namespace Demos.SpecializedTests +namespace Demos.SpecializedTests; + +public class CharacterTestDemo : Demo { - public class CharacterTestDemo : Demo + CharacterControllers characters; + public override void Initialize(ContentArchive content, Camera camera) { - CharacterControllers characters; - public override void Initialize(ContentArchive content, Camera camera) - { - camera.Position = new Vector3(20, 10, 20); - camera.Yaw = MathHelper.Pi * -1f / 4; - camera.Pitch = MathHelper.Pi * 0.05f; - var masks = new CollidableProperty(); - characters = new CharacterControllers(BufferPool); - Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); + camera.Position = new Vector3(20, 10, 20); + camera.Yaw = MathHelper.Pi * -1f / 4; + camera.Pitch = MathHelper.Pi * 0.05f; + var masks = new CollidableProperty(); + characters = new CharacterControllers(BufferPool); + Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); - var random = new Random(5); - for (int i = 0; i < 8192; ++i) - { - ref var character = ref characters.AllocateCharacter( - Simulation.Bodies.Add( - BodyDescription.CreateDynamic( - new Vector3(250 * random.NextSingle() - 125, 2, 250 * random.NextSingle() - 125), new BodyInertia { InverseMass = 1 }, - Simulation.Shapes.Add(new Capsule(0.5f, 1f)), - -1))); + var random = new Random(5); + for (int i = 0; i < 8192; ++i) + { + ref var character = ref characters.AllocateCharacter( + Simulation.Bodies.Add( + BodyDescription.CreateDynamic( + new Vector3(250 * random.NextSingle() - 125, 2, 250 * random.NextSingle() - 125), new BodyInertia { InverseMass = 1 }, + Simulation.Shapes.Add(new Capsule(0.5f, 1f)), + -1))); - character.CosMaximumSlope = .707f; - character.LocalUp = Vector3.UnitY; - character.MaximumHorizontalForce = 10; - character.MaximumVerticalForce = 10; - character.MinimumSupportContinuationDepth = -0.1f; - character.MinimumSupportDepth = -0.01f; - character.TargetVelocity = new Vector2(4, 0); - character.ViewDirection = new Vector3(0, 0, -1); - character.JumpVelocity = 4; - } + character.CosMaximumSlope = .707f; + character.LocalUp = Vector3.UnitY; + character.MaximumHorizontalForce = 10; + character.MaximumVerticalForce = 10; + character.MinimumSupportContinuationDepth = -0.1f; + character.MinimumSupportDepth = -0.01f; + character.TargetVelocity = new Vector2(4, 0); + character.ViewDirection = new Vector3(0, 0, -1); + character.JumpVelocity = 4; + } - var origin = new Vector3(-3f, 0, 0); - var spacing = new Vector3(0.5f, 0, -0.5f); - //for (int i = 0; i < 12; ++i) - //{ - // for (int j = 0; j < 100; ++j) - // { - // var position = origin + new Vector3(i, 0, j) * spacing; - // var orientation = Quaternion.CreateFromAxisAngle(Vector3.Normalize(new Vector3(0.0001f) + new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle())), 10 * random.NextSingle()); - // var shape = new Box(0.1f + 0.3f * random.NextSingle(), 0.1f + 0.3f * random.NextSingle(), 0.1f + 0.3f * random.NextSingle()); - // var collidable = new CollidableDescription(Simulation.Shapes.Add(shape), 0.1f); - // shape.ComputeInertia(1, out var inertia); - // var choice = (i + j) % 3; - // switch (choice) - // { - // case 0: - // Simulation.Bodies.Add(BodyDescription.CreateDynamic(new RigidPose(position, orientation), inertia, collidable, new BodyActivityDescription(0.01f))); - // break; - // case 1: - // Simulation.Bodies.Add(BodyDescription.CreateKinematic(new RigidPose(position, orientation), collidable, new BodyActivityDescription(0.01f))); - // break; - // case 2: - // Simulation.Statics.Add(new StaticDescription(position, orientation, collidable)); - // break; + var origin = new Vector3(-3f, 0, 0); + var spacing = new Vector3(0.5f, 0, -0.5f); + //for (int i = 0; i < 12; ++i) + //{ + // for (int j = 0; j < 100; ++j) + // { + // var position = origin + new Vector3(i, 0, j) * spacing; + // var orientation = Quaternion.CreateFromAxisAngle(Vector3.Normalize(new Vector3(0.0001f) + new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle())), 10 * random.NextSingle()); + // var shape = new Box(0.1f + 0.3f * random.NextSingle(), 0.1f + 0.3f * random.NextSingle(), 0.1f + 0.3f * random.NextSingle()); + // var collidable = new CollidableDescription(Simulation.Shapes.Add(shape), 0.1f); + // shape.ComputeInertia(1, out var inertia); + // var choice = (i + j) % 3; + // switch (choice) + // { + // case 0: + // Simulation.Bodies.Add(BodyDescription.CreateDynamic(new RigidPose(position, orientation), inertia, collidable, new BodyActivityDescription(0.01f))); + // break; + // case 1: + // Simulation.Bodies.Add(BodyDescription.CreateKinematic(new RigidPose(position, orientation), collidable, new BodyActivityDescription(0.01f))); + // break; + // case 2: + // Simulation.Statics.Add(new StaticDescription(position, orientation, collidable)); + // break; - // } - // } - //} + // } + // } + //} - //Simulation.Statics.Add(new StaticDescription( - // new Vector3(0, -0.5f, 0), Quaternion.CreateFromAxisAngle(Vector3.Normalize(new Vector3(1, 0, 1)), MathF.PI * 0.00f), new CollidableDescription(Simulation.Shapes.Add(new Box(3000, 1, 3000)), 0.1f))); + //Simulation.Statics.Add(new StaticDescription( + // new Vector3(0, -0.5f, 0), Quaternion.CreateFromAxisAngle(Vector3.Normalize(new Vector3(1, 0, 1)), MathF.PI * 0.00f), new CollidableDescription(Simulation.Shapes.Add(new Box(3000, 1, 3000)), 0.1f))); - const int planeWidth = 256; - const int planeHeight = 256; - var planeMesh = DemoMeshHelper.CreateDeformedPlane(planeWidth, planeHeight, - (int x, int y) => - { - Vector2 offsetFromCenter = new Vector2(x - planeWidth / 2, y - planeHeight / 2); - return new Vector3(offsetFromCenter.X, MathF.Cos(x / 2f) + MathF.Sin(y / 2f), offsetFromCenter.Y); - }, new Vector3(2, 1, 2), BufferPool); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -2, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), Simulation.Shapes.Add(planeMesh))); + const int planeWidth = 256; + const int planeHeight = 256; + var planeMesh = DemoMeshHelper.CreateDeformedPlane(planeWidth, planeHeight, + (int x, int y) => + { + Vector2 offsetFromCenter = new Vector2(x - planeWidth / 2, y - planeHeight / 2); + return new Vector3(offsetFromCenter.X, MathF.Cos(x / 2f) + MathF.Sin(y / 2f), offsetFromCenter.Y); + }, new Vector3(2, 1, 2), BufferPool); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -2, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), Simulation.Shapes.Add(planeMesh))); - removedCharacters = new QuickQueue(characters.CharacterCount, BufferPool); - } + removedCharacters = new QuickQueue(characters.CharacterCount, BufferPool); + } - QuickQueue removedCharacters; - int frameIndex; - public override void Update(Window window, Camera camera, Input input, float dt) + QuickQueue removedCharacters; + int frameIndex; + public override void Update(Window window, Camera camera, Input input, float dt) + { + var rotation = Matrix3x3.CreateFromAxisAngle(new Vector3(0, 1, 0), 0.5f * dt); + for (int i = 0; i < characters.CharacterCount; ++i) { - var rotation = Matrix3x3.CreateFromAxisAngle(new Vector3(0, 1, 0), 0.5f * dt); - for (int i = 0; i < characters.CharacterCount; ++i) - { - ref var character = ref characters.GetCharacterByIndex(i); - if ((frameIndex + i) % 128 == 0) - character.TryJump = true; - var tangent = Vector3.Cross(new BodyReference(character.BodyHandle, Simulation.Bodies).Pose.Position, Vector3.UnitY); - var tangentLengthSquared = tangent.LengthSquared(); - if (tangentLengthSquared > 1e-12f) - tangent = tangent / MathF.Sqrt(tangentLengthSquared); - else - tangent = Vector3.UnitX; - tangent *= 4; - character.TargetVelocity.X = -tangent.X; - character.TargetVelocity.Y = tangent.Z; - //Matrix3x3.Transform(new Vector3(character.TargetVelocity.X, 0, character.TargetVelocity.Y), rotation, out var rotatedVelocity); - //character.TargetVelocity.X = rotatedVelocity.X; - //character.TargetVelocity.Y = rotatedVelocity.Z; - } - //{ - // if (characters.CharacterCount > 0) - // { - // var indexToRemove = frameIndex % characters.CharacterCount; - // removedCharacters.EnqueueUnsafely(characters.GetCharacterByIndex(indexToRemove)); - // characters.RemoveCharacterByIndex(indexToRemove); - // } + ref var character = ref characters.GetCharacterByIndex(i); + if ((frameIndex + i) % 128 == 0) + character.TryJump = true; + var tangent = Vector3.Cross(new BodyReference(character.BodyHandle, Simulation.Bodies).Pose.Position, Vector3.UnitY); + var tangentLengthSquared = tangent.LengthSquared(); + if (tangentLengthSquared > 1e-12f) + tangent = tangent / MathF.Sqrt(tangentLengthSquared); + else + tangent = Vector3.UnitX; + tangent *= 4; + character.TargetVelocity.X = -tangent.X; + character.TargetVelocity.Y = tangent.Z; + //Matrix3x3.Transform(new Vector3(character.TargetVelocity.X, 0, character.TargetVelocity.Y), rotation, out var rotatedVelocity); + //character.TargetVelocity.X = rotatedVelocity.X; + //character.TargetVelocity.Y = rotatedVelocity.Z; + } + //{ + // if (characters.CharacterCount > 0) + // { + // var indexToRemove = frameIndex % characters.CharacterCount; + // removedCharacters.EnqueueUnsafely(characters.GetCharacterByIndex(indexToRemove)); + // characters.RemoveCharacterByIndex(indexToRemove); + // } - // var readdCount = (int)(removedCharacters.Count * 0.05f); - // for (int i = 0; i < readdCount; ++i) - // { - // var toAdd = removedCharacters.Dequeue(); - // ref var character = ref characters.AllocateCharacter(toAdd.BodyHandle, out var characterIndex); - // character = toAdd; - // } + // var readdCount = (int)(removedCharacters.Count * 0.05f); + // for (int i = 0; i < readdCount; ++i) + // { + // var toAdd = removedCharacters.Dequeue(); + // ref var character = ref characters.AllocateCharacter(toAdd.BodyHandle, out var characterIndex); + // character = toAdd; + // } - //} - frameIndex++; - base.Update(window, camera, input, dt); - } + //} + frameIndex++; + base.Update(window, camera, input, dt); } } diff --git a/Demos/SpecializedTests/ClothLatticeDemo.cs b/Demos/SpecializedTests/ClothLatticeDemo.cs index e1dc4c89c..65154c8fd 100644 --- a/Demos/SpecializedTests/ClothLatticeDemo.cs +++ b/Demos/SpecializedTests/ClothLatticeDemo.cs @@ -6,89 +6,88 @@ using BepuPhysics.Constraints; using DemoContentLoader; -namespace Demos.SpecializedTests +namespace Demos.SpecializedTests; + +public class ClothLatticeDemo : Demo { - public class ClothLatticeDemo : Demo + public override void Initialize(ContentArchive content, Camera camera) { - public override void Initialize(ContentArchive content, Camera camera) - { - camera.Position = new Vector3(-120, 30, -120); - camera.Yaw = MathHelper.Pi * 3f / 4; - camera.Pitch = 0.1f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); + camera.Position = new Vector3(-120, 30, -120); + camera.Yaw = MathHelper.Pi * 3f / 4; + camera.Pitch = 0.1f; + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); - //Build a grid of shapes to be connected. - var clothNodeShape = new Sphere(0.5f); - var clothNodeInertia = clothNodeShape.ComputeInertia(1); - var clothNodeShapeIndex = Simulation.Shapes.Add(clothNodeShape); - const int width = 128; - const int length = 128; - const float spacing = 1.75f; - BodyHandle[][] nodeHandles = new BodyHandle[width][]; - for (int i = 0; i < width; ++i) + //Build a grid of shapes to be connected. + var clothNodeShape = new Sphere(0.5f); + var clothNodeInertia = clothNodeShape.ComputeInertia(1); + var clothNodeShapeIndex = Simulation.Shapes.Add(clothNodeShape); + const int width = 128; + const int length = 128; + const float spacing = 1.75f; + BodyHandle[][] nodeHandles = new BodyHandle[width][]; + for (int i = 0; i < width; ++i) + { + nodeHandles[i] = new BodyHandle[length]; + for (int j = 0; j < length; ++j) { - nodeHandles[i] = new BodyHandle[length]; - for (int j = 0; j < length; ++j) - { - var location = new Vector3(0, 30, 0) + new Vector3(spacing, 0, spacing) * (new Vector3(i, 0, j) + new Vector3(-width * 0.5f, 0, -length * 0.5f)); - var bodyDescription = BodyDescription.CreateDynamic(location, clothNodeInertia, new(clothNodeShapeIndex, 0.1f), 0.01f); - nodeHandles[i][j] = Simulation.Bodies.Add(bodyDescription); + var location = new Vector3(0, 30, 0) + new Vector3(spacing, 0, spacing) * (new Vector3(i, 0, j) + new Vector3(-width * 0.5f, 0, -length * 0.5f)); + var bodyDescription = BodyDescription.CreateDynamic(location, clothNodeInertia, new(clothNodeShapeIndex, 0.1f), 0.01f); + nodeHandles[i][j] = Simulation.Bodies.Add(bodyDescription); - } } - //Construct some joints between the nodes. - var left = new BallSocket - { - LocalOffsetA = new Vector3(-spacing * 0.5f, 0, 0), - LocalOffsetB = new Vector3(spacing * 0.5f, 0, 0), - SpringSettings = new SpringSettings(10, 1) - }; - var up = new BallSocket - { - LocalOffsetA = new Vector3(0, 0, -spacing * 0.5f), - LocalOffsetB = new Vector3(0, 0, spacing * 0.5f), - SpringSettings = new SpringSettings(10, 1) - }; - var leftUp = new BallSocket - { - LocalOffsetA = new Vector3(-spacing * 0.5f, 0, -spacing * 0.5f), - LocalOffsetB = new Vector3(spacing * 0.5f, 0, spacing * 0.5f), - SpringSettings = new SpringSettings(10, 1) - }; - var rightUp = new BallSocket - { - LocalOffsetA = new Vector3(spacing * 0.5f, 0, -spacing * 0.5f), - LocalOffsetB = new Vector3(-spacing * 0.5f, 0, spacing * 0.5f), - SpringSettings = new SpringSettings(10, 1) - }; - for (int i = 0; i < width; ++i) + } + //Construct some joints between the nodes. + var left = new BallSocket + { + LocalOffsetA = new Vector3(-spacing * 0.5f, 0, 0), + LocalOffsetB = new Vector3(spacing * 0.5f, 0, 0), + SpringSettings = new SpringSettings(10, 1) + }; + var up = new BallSocket + { + LocalOffsetA = new Vector3(0, 0, -spacing * 0.5f), + LocalOffsetB = new Vector3(0, 0, spacing * 0.5f), + SpringSettings = new SpringSettings(10, 1) + }; + var leftUp = new BallSocket + { + LocalOffsetA = new Vector3(-spacing * 0.5f, 0, -spacing * 0.5f), + LocalOffsetB = new Vector3(spacing * 0.5f, 0, spacing * 0.5f), + SpringSettings = new SpringSettings(10, 1) + }; + var rightUp = new BallSocket + { + LocalOffsetA = new Vector3(spacing * 0.5f, 0, -spacing * 0.5f), + LocalOffsetB = new Vector3(-spacing * 0.5f, 0, spacing * 0.5f), + SpringSettings = new SpringSettings(10, 1) + }; + for (int i = 0; i < width; ++i) + { + for (int j = 0; j < length; ++j) { - for (int j = 0; j < length; ++j) - { - if (i >= 1) - Simulation.Solver.Add(nodeHandles[i][j], nodeHandles[i - 1][j], left); - if (j >= 1) - Simulation.Solver.Add(nodeHandles[i][j], nodeHandles[i][j - 1], up); - if (i >= 1 && j >= 1) - Simulation.Solver.Add(nodeHandles[i][j], nodeHandles[i - 1][j - 1], leftUp); - if (i < width - 1 && j >= 1) - Simulation.Solver.Add(nodeHandles[i][j], nodeHandles[i + 1][j - 1], rightUp); - } + if (i >= 1) + Simulation.Solver.Add(nodeHandles[i][j], nodeHandles[i - 1][j], left); + if (j >= 1) + Simulation.Solver.Add(nodeHandles[i][j], nodeHandles[i][j - 1], up); + if (i >= 1 && j >= 1) + Simulation.Solver.Add(nodeHandles[i][j], nodeHandles[i - 1][j - 1], leftUp); + if (i < width - 1 && j >= 1) + Simulation.Solver.Add(nodeHandles[i][j], nodeHandles[i + 1][j - 1], rightUp); } - var bigBallShape = new Sphere(25); - var bigBallShapeIndex = Simulation.Shapes.Add(bigBallShape); - - var bigBallDescription = new StaticDescription(new Vector3(-10, -15, 0), bigBallShapeIndex); - Simulation.Statics.Add(bigBallDescription); + } + var bigBallShape = new Sphere(25); + var bigBallShapeIndex = Simulation.Shapes.Add(bigBallShape); - var groundShape = new Box(200, 1, 200); - var groundShapeIndex = Simulation.Shapes.Add(groundShape); + var bigBallDescription = new StaticDescription(new Vector3(-10, -15, 0), bigBallShapeIndex); + Simulation.Statics.Add(bigBallDescription); - var groundDescription = new StaticDescription(new Vector3(0, -10, 0), groundShapeIndex); - Simulation.Statics.Add(groundDescription); - } + var groundShape = new Box(200, 1, 200); + var groundShapeIndex = Simulation.Shapes.Add(groundShape); + var groundDescription = new StaticDescription(new Vector3(0, -10, 0), groundShapeIndex); + Simulation.Statics.Add(groundDescription); } + } diff --git a/Demos/SpecializedTests/CompoundBoundTests.cs b/Demos/SpecializedTests/CompoundBoundTests.cs index bf77ce7ca..f09195f59 100644 --- a/Demos/SpecializedTests/CompoundBoundTests.cs +++ b/Demos/SpecializedTests/CompoundBoundTests.cs @@ -10,229 +10,228 @@ using DemoRenderer.Constraints; using BepuPhysics.Constraints; -namespace Demos.Demos +namespace Demos.Demos; + +public class CompoundBoundTests : Demo { - public class CompoundBoundTests : Demo + public override void Initialize(ContentArchive content, Camera camera) { - public override void Initialize(ContentArchive content, Camera camera) - { - camera.Position = new Vector3(-10, 0, -10); - camera.Yaw = MathHelper.Pi * 3f / 4; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); - - - } - - void GetArcExpansion(Vector3 offset, Vector3 angularVelocity, float dt, out Vector3 minExpansion, out Vector3 maxExpansion) - { - //minExpansion = default; - //maxExpansion = default; - //var angularSpeed = angularVelocity.Length(); - //if (angularSpeed == 0) - //{ - // return; - //} - //var angularDirection = angularVelocity / angularSpeed; - //var theta = angularSpeed * dt; - //Matrix3x3.Transform(offset, Matrix3x3.CreateFromAxisAngle(angularDirection, theta), out var endpoint); - //var startToEnd = endpoint - offset; - //var distance = startToEnd.Length(); - //if (distance == 0) - //{ - // return; - //} - //var arcX = startToEnd / distance; - //var radius = offset.Length(); - - //Vector3x.Cross(arcX, angularDirection, out var arcY); - //var halfTheta = theta * 0.5f; - //var expansionMagnitudeX = MathHelper.Sin(MathHelper.Min(MathHelper.PiOver2, halfTheta)) * radius - distance * 0.5f; - //var expansionMagnitudeY = radius - radius * MathHelper.Cos(halfTheta); - - //var expansionX = expansionMagnitudeX * arcX; - //BoundingBoxHelpers.ExpandBoundingBox(expansionX, ref minExpansion, ref maxExpansion); - //BoundingBoxHelpers.ExpandBoundingBox(-expansionX, ref minExpansion, ref maxExpansion); - //BoundingBoxHelpers.ExpandBoundingBox(expansionMagnitudeY * arcY, ref minExpansion, ref maxExpansion); + camera.Position = new Vector3(-10, 0, -10); + camera.Yaw = MathHelper.Pi * 3f / 4; + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); + } - //var angularSpeedSquared = angularVelocity.LengthSquared(); - //if (angularSpeedSquared == 0) - //{ - // minExpansion = default; - // maxExpansion = default; - // return; - //} - //var inverseAngularSpeedSquared = 1f / angularSpeedSquared; - - ////x - angularVelocity * dot(x, angularVelocity) - - //var angularSpeed = MathF.Sqrt(angularSpeedSquared); - //var angularDirection = angularVelocity / angularSpeed; - //var planeDot = Vector3.Dot(angularDirection, offset); - //var planeOffset = planeDot * angularDirection; - //var x = new Vector3(1, 0, 0) - angularDirection.X * angularDirection; - //var y = new Vector3(0, 1, 0) - angularDirection.Y * angularDirection; - //var z = new Vector3(0, 0, 1) - angularDirection.Z * angularDirection; - //x = x / x.Length(); - //y = y / y.Length(); - //z = z / z.Length(); - //var radius = offset.Length(); - //var circleMax = radius * new Vector3(x.X, y.Y, z.Z); - //var circleMin = -circleMax; - //circleMax += planeOffset; - //circleMin += planeOffset; - - //var theta = angularSpeed * dt; - //Matrix3x3.Transform(offset, Matrix3x3.CreateFromAxisAngle(angularDirection, theta), out var endpoint); - - //var min = Vector3.Min(endpoint, offset); - //var max = Vector3.Max(endpoint, offset); - - //minExpansion = circleMin - min; - //maxExpansion = circleMax - max; - - + void GetArcExpansion(Vector3 offset, Vector3 angularVelocity, float dt, out Vector3 minExpansion, out Vector3 maxExpansion) + { + //minExpansion = default; + //maxExpansion = default; + //var angularSpeed = angularVelocity.Length(); + //if (angularSpeed == 0) + //{ + // return; + //} + //var angularDirection = angularVelocity / angularSpeed; + //var theta = angularSpeed * dt; + //Matrix3x3.Transform(offset, Matrix3x3.CreateFromAxisAngle(angularDirection, theta), out var endpoint); + //var startToEnd = endpoint - offset; + //var distance = startToEnd.Length(); + //if (distance == 0) + //{ + // return; + //} + //var arcX = startToEnd / distance; + //var radius = offset.Length(); + + //Vector3x.Cross(arcX, angularDirection, out var arcY); + //var halfTheta = theta * 0.5f; + //var expansionMagnitudeX = MathHelper.Sin(MathHelper.Min(MathHelper.PiOver2, halfTheta)) * radius - distance * 0.5f; + //var expansionMagnitudeY = radius - radius * MathHelper.Cos(halfTheta); + + //var expansionX = expansionMagnitudeX * arcX; + //BoundingBoxHelpers.ExpandBoundingBox(expansionX, ref minExpansion, ref maxExpansion); + //BoundingBoxHelpers.ExpandBoundingBox(-expansionX, ref minExpansion, ref maxExpansion); + //BoundingBoxHelpers.ExpandBoundingBox(expansionMagnitudeY * arcY, ref minExpansion, ref maxExpansion); + + + + //var angularSpeedSquared = angularVelocity.LengthSquared(); + //if (angularSpeedSquared == 0) + //{ + // minExpansion = default; + // maxExpansion = default; + // return; + //} + //var inverseAngularSpeedSquared = 1f / angularSpeedSquared; + + ////x - angularVelocity * dot(x, angularVelocity) + + //var angularSpeed = MathF.Sqrt(angularSpeedSquared); + //var angularDirection = angularVelocity / angularSpeed; + //var planeDot = Vector3.Dot(angularDirection, offset); + //var planeOffset = planeDot * angularDirection; + //var x = new Vector3(1, 0, 0) - angularDirection.X * angularDirection; + //var y = new Vector3(0, 1, 0) - angularDirection.Y * angularDirection; + //var z = new Vector3(0, 0, 1) - angularDirection.Z * angularDirection; + //x = x / x.Length(); + //y = y / y.Length(); + //z = z / z.Length(); + //var radius = offset.Length(); + //var circleMax = radius * new Vector3(x.X, y.Y, z.Z); + //var circleMin = -circleMax; + //circleMax += planeOffset; + //circleMin += planeOffset; + + //var theta = angularSpeed * dt; + //Matrix3x3.Transform(offset, Matrix3x3.CreateFromAxisAngle(angularDirection, theta), out var endpoint); + + //var min = Vector3.Min(endpoint, offset); + //var max = Vector3.Max(endpoint, offset); + + //minExpansion = circleMin - min; + //maxExpansion = circleMax - max; + + + + //var angularSpeed = angularVelocity.Length(); + //var angularDirection = angularVelocity / angularSpeed; + //var radius = offset.Length(); + //maxExpansion = new Vector3(radius - radius * MathHelper.Cos(MathHelper.Min(MathHelper.Pi, angularSpeed * dt / 2))); + //minExpansion = -maxExpansion; + + + //var theta = angularSpeed * dt; + //Matrix3x3.Transform(offset, Matrix3x3.CreateFromAxisAngle(angularDirection, theta), out var endpoint); + + //var min = Vector3.Min(endpoint, offset); + //var max = Vector3.Max(endpoint, offset); + //minExpansion = minExpansion - min; + //maxExpansion = maxExpansion - max; + + + + + var angularSpeed = angularVelocity.Length(); + var angularDirection = angularVelocity / angularSpeed; + var theta = angularSpeed * dt; + Matrix3x3.Transform(offset, Matrix3x3.CreateFromAxisAngle(angularDirection, MathHelper.Min(theta, MathHelper.Pi)), out var endpoint); + var distance = Vector3.Distance(endpoint, offset); + + maxExpansion = new Vector3(distance); + minExpansion = -maxExpansion; + + var min = Vector3.Min(endpoint, offset); + var max = Vector3.Max(endpoint, offset); + minExpansion = minExpansion - min; + maxExpansion = maxExpansion - max; + } - //var angularSpeed = angularVelocity.Length(); - //var angularDirection = angularVelocity / angularSpeed; - //var radius = offset.Length(); - //maxExpansion = new Vector3(radius - radius * MathHelper.Cos(MathHelper.Min(MathHelper.Pi, angularSpeed * dt / 2))); - //minExpansion = -maxExpansion; + void GetEstimatedExpansion(Vector3 localPoseA, Vector3 angularVelocityA, Vector3 offsetB, Vector3 angularVelocityB, float dt, out Vector3 minExpansion, out Vector3 maxExpansion) + { + GetArcExpansion(localPoseA, angularVelocityA, dt, out var minExpansionA, out var maxExpansionA); + GetArcExpansion(-offsetB, -angularVelocityB, dt, out var minExpansionB, out var maxExpansionB); + minExpansion = minExpansionA + minExpansionB; + maxExpansion = maxExpansionA + maxExpansionB; + } + Vector3 GetRandomVector(float width, Random random) + { + return new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) * width - new Vector3(width * 0.5f); + } + Random random = new Random(5); + public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + { + Vector3 basePosition = new Vector3(); + for (int testIndex = 0; testIndex < 16; ++testIndex) + { + var testShape = new Sphere(0); + var orientationA = Quaternion.Identity; + var orientationB = Quaternion.Identity; - //var theta = angularSpeed * dt; - //Matrix3x3.Transform(offset, Matrix3x3.CreateFromAxisAngle(angularDirection, theta), out var endpoint); + var velocityA = new BodyVelocity(GetRandomVector(2, random), GetRandomVector(1, random)); + var velocityB = new BodyVelocity(GetRandomVector(2, random), GetRandomVector(1, random)); + var offsetB = GetRandomVector(2, random); + var localPoseA = new RigidPose(GetRandomVector(2, random), Quaternion.Identity); + float dt = 10f; - //var min = Vector3.Min(endpoint, offset); - //var max = Vector3.Max(endpoint, offset); - //minExpansion = minExpansion - min; - //maxExpansion = maxExpansion - max; + const int pathPointCount = 512; + var localPathPoints = new Vector3[pathPointCount]; + for (int i = 0; i < pathPointCount; ++i) + { + var t = (dt * i) / (pathPointCount - 1); + //local point = (aPosition + aLinear * t - bPosition - bLinear * t + localOffsetA * (orientationA * rotate(angularA * t)) * inverse(orientationB * rotate(angularB * t)) + PoseIntegration.Integrate(orientationA, velocityA.Angular, t, out var integratedA); + PoseIntegration.Integrate(orientationB, velocityB.Angular, t, out var integratedB); + var worldRotatedPoint = velocityA.Linear * t - velocityB.Linear * t - offsetB + QuaternionEx.Transform(localPoseA.Position, integratedA); + localPathPoints[i] = QuaternionEx.Transform(worldRotatedPoint, QuaternionEx.Conjugate(integratedB)); + } + var referenceSweep = localPathPoints[pathPointCount - 1] - localPathPoints[0]; + var sweepMin = Vector3.Min(localPathPoints[pathPointCount - 1], localPathPoints[0]); + var sweepMax = Vector3.Max(localPathPoints[pathPointCount - 1], localPathPoints[0]); + Vector3 referenceMin = new Vector3(float.MaxValue); + Vector3 referenceMax = new Vector3(float.MinValue); + for (int i = 0; i < pathPointCount; ++i) + { + referenceMin = Vector3.Min(referenceMin, localPathPoints[i]); + referenceMax = Vector3.Max(referenceMax, localPathPoints[i]); + } + var referenceMinExpansion = referenceMin - sweepMin; + var referenceMaxExpansion = referenceMax - sweepMax; + + var shapeIndex = Simulation.Shapes.Add(testShape); + BoundingBoxHelpers.GetLocalBoundingBoxForSweep(shapeIndex, Simulation.Shapes, localPoseA, orientationA, velocityA, offsetB, orientationB, velocityB, dt, out var naiveSweep, out var naiveMin, out var naiveMax); + naiveMin += Vector3.Min(naiveSweep, default); + naiveMax += Vector3.Max(naiveSweep, default); + Simulation.Shapes.Remove(shapeIndex); + BoundingBox.CreateMerged(naiveMin, naiveMax, referenceMin, referenceMax, out var combinedMin, out var combinedMax); + if ((combinedMin - naiveMin).LengthSquared() > 1e-5f || (combinedMax - naiveMax).LengthSquared() > 1e-5f) + { + Console.WriteLine($"Naive fails to contain reference: min offset {combinedMin - naiveMin}, max offset {combinedMax - naiveMax}"); + } + basePosition.X += 32 + MathF.Max(0, -combinedMin.X) + combinedMax.X - combinedMin.X; - var angularSpeed = angularVelocity.Length(); - var angularDirection = angularVelocity / angularSpeed; - var theta = angularSpeed * dt; - Matrix3x3.Transform(offset, Matrix3x3.CreateFromAxisAngle(angularDirection, MathHelper.Min(theta, MathHelper.Pi)), out var endpoint); - var distance = Vector3.Distance(endpoint, offset); + for (int i = 0; i < localPathPoints.Length - 1; ++i) + { + renderer.Lines.Allocate() = new LineInstance(basePosition + localPathPoints[i], basePosition + localPathPoints[i + 1], new Vector3(1, 0, 0), new Vector3()); + } - maxExpansion = new Vector3(distance); - minExpansion = -maxExpansion; + BoundingBoxLineExtractor.WriteBoundsLines(basePosition + referenceMin, basePosition + referenceMax, new Vector3(0, 1, 0), new Vector3(), ref renderer.Lines.Allocate(12)); + BoundingBoxLineExtractor.WriteBoundsLines(basePosition + sweepMin, basePosition + sweepMax, new Vector3(0, 0, 1), new Vector3(), ref renderer.Lines.Allocate(12)); + var expansionOffset = new Vector3(0, 0, 65); + BoundingBoxLineExtractor.WriteBoundsLines(basePosition + expansionOffset + new Vector3(-0.01f), basePosition + expansionOffset + new Vector3(0.01f), new Vector3(0, 0, 0), new Vector3(), ref renderer.Lines.Allocate(12)); + BoundingBoxLineExtractor.WriteBoundsLines(basePosition + expansionOffset + referenceMinExpansion, basePosition + expansionOffset + referenceMaxExpansion, new Vector3(1, 0, 1), new Vector3(), ref renderer.Lines.Allocate(12)); - var min = Vector3.Min(endpoint, offset); - var max = Vector3.Max(endpoint, offset); - minExpansion = minExpansion - min; - maxExpansion = maxExpansion - max; - } + BoundingBoxLineExtractor.WriteBoundsLines(basePosition + naiveMin, basePosition + naiveMax, new Vector3(1, 1, 1), new Vector3(), ref renderer.Lines.Allocate(12)); + BoundingBoxLineExtractor.WriteBoundsLines(basePosition + expansionOffset + naiveMin - sweepMin, basePosition + expansionOffset + naiveMax - sweepMax, new Vector3(0, 1, 1), new Vector3(), ref renderer.Lines.Allocate(12)); - void GetEstimatedExpansion(Vector3 localPoseA, Vector3 angularVelocityA, Vector3 offsetB, Vector3 angularVelocityB, float dt, out Vector3 minExpansion, out Vector3 maxExpansion) - { - GetArcExpansion(localPoseA, angularVelocityA, dt, out var minExpansionA, out var maxExpansionA); - GetArcExpansion(-offsetB, -angularVelocityB, dt, out var minExpansionB, out var maxExpansionB); - minExpansion = minExpansionA + minExpansionB; - maxExpansion = maxExpansionA + maxExpansionB; - } - Vector3 GetRandomVector(float width, Random random) - { - return new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) * width - new Vector3(width * 0.5f); + //{ + // QuaternionWide.Broadcast(Quaternion.Identity, out var wideOrientation); + // Vector3Wide.Broadcast(new Vector3(1, 1, 1), out var wideVelocity); + // var halfDt = new Vector(0.5f); + // const int testCount = 1024; + // var resultsSweep = stackalloc Vector3[testCount]; + // var resultsMin = stackalloc Vector3[testCount]; + // var resultsMax = stackalloc Vector3[testCount]; + // Box box = new Box(1, 1, 1); + // var start = Stopwatch.GetTimestamp(); + // for (int i = 0; i < testCount; ++i) + // { + // BoundingBoxHelpers.GetLocalBoundingBoxForSweep(ref box, localPoseA, orientationA, velocityA, offsetB, orientationB, velocityB, dt, out resultsSweep[i], out resultsMin[i], out resultsMax[i]); + + + // } + // var end = Stopwatch.GetTimestamp(); + // Console.WriteLine($"Time per sweep bound test (ns): {(end - start) * (1e9 / (testCount * Stopwatch.Frequency))}"); + //} } - Random random = new Random(5); - public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) - { - Vector3 basePosition = new Vector3(); - for (int testIndex = 0; testIndex < 16; ++testIndex) - { - var testShape = new Sphere(0); - var orientationA = Quaternion.Identity; - var orientationB = Quaternion.Identity; - - var velocityA = new BodyVelocity(GetRandomVector(2, random), GetRandomVector(1, random)); - var velocityB = new BodyVelocity(GetRandomVector(2, random), GetRandomVector(1, random)); - var offsetB = GetRandomVector(2, random); - var localPoseA = new RigidPose(GetRandomVector(2, random), Quaternion.Identity); - float dt = 10f; - - const int pathPointCount = 512; - var localPathPoints = new Vector3[pathPointCount]; - - for (int i = 0; i < pathPointCount; ++i) - { - var t = (dt * i) / (pathPointCount - 1); - //local point = (aPosition + aLinear * t - bPosition - bLinear * t + localOffsetA * (orientationA * rotate(angularA * t)) * inverse(orientationB * rotate(angularB * t)) - - PoseIntegration.Integrate(orientationA, velocityA.Angular, t, out var integratedA); - PoseIntegration.Integrate(orientationB, velocityB.Angular, t, out var integratedB); - var worldRotatedPoint = velocityA.Linear * t - velocityB.Linear * t - offsetB + QuaternionEx.Transform(localPoseA.Position, integratedA); - localPathPoints[i] = QuaternionEx.Transform(worldRotatedPoint, QuaternionEx.Conjugate(integratedB)); - } - var referenceSweep = localPathPoints[pathPointCount - 1] - localPathPoints[0]; - var sweepMin = Vector3.Min(localPathPoints[pathPointCount - 1], localPathPoints[0]); - var sweepMax = Vector3.Max(localPathPoints[pathPointCount - 1], localPathPoints[0]); - Vector3 referenceMin = new Vector3(float.MaxValue); - Vector3 referenceMax = new Vector3(float.MinValue); - for (int i = 0; i < pathPointCount; ++i) - { - referenceMin = Vector3.Min(referenceMin, localPathPoints[i]); - referenceMax = Vector3.Max(referenceMax, localPathPoints[i]); - } - var referenceMinExpansion = referenceMin - sweepMin; - var referenceMaxExpansion = referenceMax - sweepMax; - - var shapeIndex = Simulation.Shapes.Add(testShape); - BoundingBoxHelpers.GetLocalBoundingBoxForSweep(shapeIndex, Simulation.Shapes, localPoseA, orientationA, velocityA, offsetB, orientationB, velocityB, dt, out var naiveSweep, out var naiveMin, out var naiveMax); - naiveMin += Vector3.Min(naiveSweep, default); - naiveMax += Vector3.Max(naiveSweep, default); - Simulation.Shapes.Remove(shapeIndex); - BoundingBox.CreateMerged(naiveMin, naiveMax, referenceMin, referenceMax, out var combinedMin, out var combinedMax); - if ((combinedMin - naiveMin).LengthSquared() > 1e-5f || (combinedMax - naiveMax).LengthSquared() > 1e-5f) - { - Console.WriteLine($"Naive fails to contain reference: min offset {combinedMin - naiveMin}, max offset {combinedMax - naiveMax}"); - } - - basePosition.X += 32 + MathF.Max(0, -combinedMin.X) + combinedMax.X - combinedMin.X; - - for (int i = 0; i < localPathPoints.Length - 1; ++i) - { - renderer.Lines.Allocate() = new LineInstance(basePosition + localPathPoints[i], basePosition + localPathPoints[i + 1], new Vector3(1, 0, 0), new Vector3()); - } - - BoundingBoxLineExtractor.WriteBoundsLines(basePosition + referenceMin, basePosition + referenceMax, new Vector3(0, 1, 0), new Vector3(), ref renderer.Lines.Allocate(12)); - BoundingBoxLineExtractor.WriteBoundsLines(basePosition + sweepMin, basePosition + sweepMax, new Vector3(0, 0, 1), new Vector3(), ref renderer.Lines.Allocate(12)); - var expansionOffset = new Vector3(0, 0, 65); - BoundingBoxLineExtractor.WriteBoundsLines(basePosition + expansionOffset + new Vector3(-0.01f), basePosition + expansionOffset + new Vector3(0.01f), new Vector3(0, 0, 0), new Vector3(), ref renderer.Lines.Allocate(12)); - BoundingBoxLineExtractor.WriteBoundsLines(basePosition + expansionOffset + referenceMinExpansion, basePosition + expansionOffset + referenceMaxExpansion, new Vector3(1, 0, 1), new Vector3(), ref renderer.Lines.Allocate(12)); - - BoundingBoxLineExtractor.WriteBoundsLines(basePosition + naiveMin, basePosition + naiveMax, new Vector3(1, 1, 1), new Vector3(), ref renderer.Lines.Allocate(12)); - BoundingBoxLineExtractor.WriteBoundsLines(basePosition + expansionOffset + naiveMin - sweepMin, basePosition + expansionOffset + naiveMax - sweepMax, new Vector3(0, 1, 1), new Vector3(), ref renderer.Lines.Allocate(12)); - - - //{ - // QuaternionWide.Broadcast(Quaternion.Identity, out var wideOrientation); - // Vector3Wide.Broadcast(new Vector3(1, 1, 1), out var wideVelocity); - // var halfDt = new Vector(0.5f); - // const int testCount = 1024; - // var resultsSweep = stackalloc Vector3[testCount]; - // var resultsMin = stackalloc Vector3[testCount]; - // var resultsMax = stackalloc Vector3[testCount]; - // Box box = new Box(1, 1, 1); - // var start = Stopwatch.GetTimestamp(); - // for (int i = 0; i < testCount; ++i) - // { - // BoundingBoxHelpers.GetLocalBoundingBoxForSweep(ref box, localPoseA, orientationA, velocityA, offsetB, orientationB, velocityB, dt, out resultsSweep[i], out resultsMin[i], out resultsMax[i]); - - - // } - // var end = Stopwatch.GetTimestamp(); - // Console.WriteLine($"Time per sweep bound test (ns): {(end - start) * (1e9 / (testCount * Stopwatch.Frequency))}"); - //} - } - base.Render(renderer, camera, input, text, font); - } + base.Render(renderer, camera, input, text, font); } } diff --git a/Demos/SpecializedTests/CompoundCollisionIndicesTest.cs b/Demos/SpecializedTests/CompoundCollisionIndicesTest.cs index 89fe84032..7e82278b3 100644 --- a/Demos/SpecializedTests/CompoundCollisionIndicesTest.cs +++ b/Demos/SpecializedTests/CompoundCollisionIndicesTest.cs @@ -9,81 +9,80 @@ using BepuPhysics.CollisionDetection; using BepuPhysics.Constraints; -namespace Demos.SpecializedTests +namespace Demos.SpecializedTests; + +public struct IndexReportingNarrowPhaseCallbacks : INarrowPhaseCallbacks { - public struct IndexReportingNarrowPhaseCallbacks : INarrowPhaseCallbacks + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin) { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin) - { - return true; - } + return true; + } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB) - { - return true; - } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB) + { + return true; + } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold + { + if (manifold.Count > 0) { - if (manifold.Count > 0) + if (manifold.Convex) { - if (manifold.Convex) - { - Console.WriteLine($"CONVEX PAIR: {pair.A} versus {pair.B}"); - } - else - { - Console.WriteLine($"NONCONVEX PAIR: {pair.A} versus {pair.B}"); - } + Console.WriteLine($"CONVEX PAIR: {pair.A} versus {pair.B}"); + } + else + { + Console.WriteLine($"NONCONVEX PAIR: {pair.A} versus {pair.B}"); } - pairMaterial.FrictionCoefficient = 1f; - pairMaterial.MaximumRecoveryVelocity = 2f; - pairMaterial.SpringSettings = new SpringSettings(30, 1); - return true; - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold) - { - if (manifold.Count > 0) - Console.WriteLine($"SUBPAIR: {pair.A} child {childIndexA} versus {pair.B} child {childIndexB}"); - return true; } + pairMaterial.FrictionCoefficient = 1f; + pairMaterial.MaximumRecoveryVelocity = 2f; + pairMaterial.SpringSettings = new SpringSettings(30, 1); + return true; + } - public void Initialize(Simulation simulation) - { - } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold) + { + if (manifold.Count > 0) + Console.WriteLine($"SUBPAIR: {pair.A} child {childIndexA} versus {pair.B} child {childIndexB}"); + return true; + } - public void Dispose() - { - } + public void Initialize(Simulation simulation) + { + } + public void Dispose() + { } - public class CompoundCollisionIndicesTest : Demo +} + +public class CompoundCollisionIndicesTest : Demo +{ + public override void Initialize(ContentArchive content, Camera camera) { - public override void Initialize(ContentArchive content, Camera camera) - { - camera.Position = new Vector3(0, 4, -6); - camera.Yaw = MathHelper.Pi; + camera.Position = new Vector3(0, 4, -6); + camera.Yaw = MathHelper.Pi; - Simulation = Simulation.Create(BufferPool, new IndexReportingNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, 0f, 0)), new SolveDescription(8, 1)); + Simulation = Simulation.Create(BufferPool, new IndexReportingNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, 0f, 0)), new SolveDescription(8, 1)); - var builder = new CompoundBuilder(BufferPool, Simulation.Shapes, 4); - builder.Add(new Sphere(0.5f), new Vector3(-1, 0, 0), 1); - builder.Add(new Capsule(0.5f, 1f), new Vector3(0, 0, 0), 1); - builder.Add(new Box(1f, 1f, 1f), new Vector3(1, 0, 0), 1); - builder.BuildDynamicCompound(out var children, out var inertia, out var center); + var builder = new CompoundBuilder(BufferPool, Simulation.Shapes, 4); + builder.Add(new Sphere(0.5f), new Vector3(-1, 0, 0), 1); + builder.Add(new Capsule(0.5f, 1f), new Vector3(0, 0, 0), 1); + builder.Add(new Box(1f, 1f, 1f), new Vector3(1, 0, 0), 1); + builder.BuildDynamicCompound(out var children, out var inertia, out var center); - var compoundShapeIndex = Simulation.Shapes.Add(new Compound(children)); + var compoundShapeIndex = Simulation.Shapes.Add(new Compound(children)); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 2, 0), inertia, compoundShapeIndex, 0.01f)); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 4, 0), inertia, compoundShapeIndex, 0.01f)); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 2, 0), inertia, compoundShapeIndex, 0.01f)); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 4, 0), inertia, compoundShapeIndex, 0.01f)); - Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(new Box(100, 1, 100)))); - } + Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(new Box(100, 1, 100)))); } } diff --git a/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs b/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs index ebb8b7334..f0681c2d6 100644 --- a/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs +++ b/Demos/SpecializedTests/ConstrainedKinematicIntegrationTest.cs @@ -5,51 +5,50 @@ using DemoContentLoader; using BepuPhysics.Constraints; -namespace Demos.SpecializedTests +namespace Demos.SpecializedTests; + +public class ConstrainedKinematicIntegrationTest : Demo { - public class ConstrainedKinematicIntegrationTest : Demo + public override void Initialize(ContentArchive content, Camera camera) { - public override void Initialize(ContentArchive content, Camera camera) + camera.Position = new Vector3(25, 4, 40); + camera.Yaw = 0; + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1), 2, 1), new DemoPoseIntegratorCallbacks(new Vector3(0, -0.1f, 0), 0, 0), new SolveDescription(8, 1)); + + var shapeA = new Box(.75f, 1, .5f); + var shapeIndexA = Simulation.Shapes.Add(shapeA); + var collidableA = new CollidableDescription(shapeIndexA); + var shapeB = new Box(.75f, 1, .5f); + var shapeIndexB = Simulation.Shapes.Add(shapeB); + var collidableB = new CollidableDescription(shapeIndexB); + var activity = new BodyActivityDescription(0.01f); + var inertiaA = shapeA.ComputeInertia(1); + var inertiaB = shapeB.ComputeInertia(1); + + for (int i = 0; i < 32; ++i) { - camera.Position = new Vector3(25, 4, 40); - camera.Yaw = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1), 2, 1), new DemoPoseIntegratorCallbacks(new Vector3(0, -0.1f, 0), 0, 0), new SolveDescription(8, 1)); - - var shapeA = new Box(.75f, 1, .5f); - var shapeIndexA = Simulation.Shapes.Add(shapeA); - var collidableA = new CollidableDescription(shapeIndexA); - var shapeB = new Box(.75f, 1, .5f); - var shapeIndexB = Simulation.Shapes.Add(shapeB); - var collidableB = new CollidableDescription(shapeIndexB); - var activity = new BodyActivityDescription(0.01f); - var inertiaA = shapeA.ComputeInertia(1); - var inertiaB = shapeB.ComputeInertia(1); - - for (int i = 0; i < 32; ++i) - { - var x = 0; - var z = i * 3; - var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, z), collidableA, activity)); - var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, z), inertiaB, collidableB, activity)); - Simulation.Bodies[a].Velocity.Linear = new Vector3(1, 0, 0); - Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - Simulation.Solver.Add(a, b, new AngularHinge { LocalHingeAxisA = new Vector3(0, 1, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); - } - - for (int i = 0; i < 32; ++i) - { - var x = 0; - var z = i * 3; - var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 8, z), collidableA, activity)); - var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 8, z + 2), inertiaB, collidableB, activity)); - Simulation.Bodies[a].Velocity.Linear = new Vector3(1, 0, 0); - Simulation.Bodies[b].Velocity.Linear = new Vector3(1, 0, 0); - //Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - //Simulation.Solver.Add(a, b, new AngularHinge { LocalHingeAxisA = new Vector3(0, 1, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); - } - - Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(new Box(8192, 1, 8192)))); + var x = 0; + var z = i * 3; + var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, z), collidableA, activity)); + var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, z), inertiaB, collidableB, activity)); + Simulation.Bodies[a].Velocity.Linear = new Vector3(1, 0, 0); + Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + Simulation.Solver.Add(a, b, new AngularHinge { LocalHingeAxisA = new Vector3(0, 1, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); } + + for (int i = 0; i < 32; ++i) + { + var x = 0; + var z = i * 3; + var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 8, z), collidableA, activity)); + var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 8, z + 2), inertiaB, collidableB, activity)); + Simulation.Bodies[a].Velocity.Linear = new Vector3(1, 0, 0); + Simulation.Bodies[b].Velocity.Linear = new Vector3(1, 0, 0); + //Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + //Simulation.Solver.Add(a, b, new AngularHinge { LocalHingeAxisA = new Vector3(0, 1, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); + } + + Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(new Box(8192, 1, 8192)))); } } diff --git a/Demos/SpecializedTests/ConstraintTestDemo.cs b/Demos/SpecializedTests/ConstraintTestDemo.cs index 45deed5fc..c78388063 100644 --- a/Demos/SpecializedTests/ConstraintTestDemo.cs +++ b/Demos/SpecializedTests/ConstraintTestDemo.cs @@ -8,438 +8,437 @@ using Demos.Demos; using System; -namespace Demos.SpecializedTests +namespace Demos.SpecializedTests; + +public class ConstraintTestDemo : Demo { - public class ConstraintTestDemo : Demo + static float GetNextPosition(ref float x) { - static float GetNextPosition(ref float x) + var toReturn = x; + x += 3; + return toReturn; + } + + public override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(25, 4, 40); + camera.Yaw = 0; + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1), 2, 1), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); + + var shapeA = new Box(.75f, 1, .5f); + var shapeIndexA = Simulation.Shapes.Add(shapeA); + var collidableA = new CollidableDescription(shapeIndexA); + var shapeB = new Box(.75f, 1, .5f); + var shapeIndexB = Simulation.Shapes.Add(shapeB); + var collidableB = new CollidableDescription(shapeIndexB); + var activity = new BodyActivityDescription(0.01f); + var inertiaA = shapeA.ComputeInertia(1); + var inertiaB = shapeB.ComputeInertia(1); + var nextX = -10f; { - var toReturn = x; - x += 3; - return toReturn; + var x = GetNextPosition(ref nextX); + var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); + var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); } - - public override void Initialize(ContentArchive content, Camera camera) { - camera.Position = new Vector3(25, 4, 40); - camera.Yaw = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1), 2, 1), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); - - var shapeA = new Box(.75f, 1, .5f); - var shapeIndexA = Simulation.Shapes.Add(shapeA); - var collidableA = new CollidableDescription(shapeIndexA); - var shapeB = new Box(.75f, 1, .5f); - var shapeIndexB = Simulation.Shapes.Add(shapeB); - var collidableB = new CollidableDescription(shapeIndexB); - var activity = new BodyActivityDescription(0.01f); - var inertiaA = shapeA.ComputeInertia(1); - var inertiaB = shapeB.ComputeInertia(1); - var nextX = -10f; - { - var x = GetNextPosition(ref nextX); - var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); - var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - } - { - var x = GetNextPosition(ref nextX); - var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); - var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - Simulation.Solver.Add(a, b, new AngularHinge { LocalHingeAxisA = new Vector3(0, 1, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); - } - { - var x = GetNextPosition(ref nextX); - var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); - var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - Simulation.Solver.Add(a, b, new Hinge - { - LocalOffsetA = new Vector3(0, 1, 0), - LocalHingeAxisA = new Vector3(0, 1, 0), - LocalOffsetB = new Vector3(0, -1, 0), - LocalHingeAxisB = new Vector3(0, 1, 0), - SpringSettings = new SpringSettings(30, 1) - }); - } - { - var x = GetNextPosition(ref nextX); - var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); - var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - Simulation.Solver.Add(a, b, new AngularSwivelHinge { LocalSwivelAxisA = new Vector3(1, 0, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); - Simulation.Solver.Add(a, b, new SwingLimit { AxisLocalA = new Vector3(0, 1, 0), AxisLocalB = new Vector3(0, 1, 0), MaximumSwingAngle = MathHelper.PiOver2, SpringSettings = new SpringSettings(30, 1) }); - } - { - var x = GetNextPosition(ref nextX); - var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); - var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - Simulation.Solver.Add(a, b, new SwivelHinge - { - LocalOffsetA = new Vector3(0, 1, 0), - LocalSwivelAxisA = new Vector3(1, 0, 0), - LocalOffsetB = new Vector3(0, -1, 0), - LocalHingeAxisB = new Vector3(0, 1, 0), - SpringSettings = new SpringSettings(30, 1) - }); - Simulation.Solver.Add(a, b, new SwingLimit { AxisLocalA = new Vector3(0, 1, 0), AxisLocalB = new Vector3(0, 1, 0), MaximumSwingAngle = MathHelper.PiOver2, SpringSettings = new SpringSettings(30, 1) }); - } - { - var x = GetNextPosition(ref nextX); - var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); - var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - Simulation.Solver.Add(a, b, new TwistServo - { - LocalBasisA = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)), - LocalBasisB = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)), - TargetAngle = MathHelper.PiOver4, - SpringSettings = new SpringSettings(30, 1), - ServoSettings = new ServoSettings(float.MaxValue, 0, float.MaxValue) - }); - } + var x = GetNextPosition(ref nextX); + var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); + var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + Simulation.Solver.Add(a, b, new AngularHinge { LocalHingeAxisA = new Vector3(0, 1, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); + } + { + var x = GetNextPosition(ref nextX); + var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); + var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + Simulation.Solver.Add(a, b, new Hinge { - var x = GetNextPosition(ref nextX); - var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); - var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - Simulation.Solver.Add(a, b, new TwistLimit - { - LocalBasisA = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)), - LocalBasisB = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)), - MinimumAngle = MathHelper.Pi * -0.5f, - MaximumAngle = MathHelper.Pi * 0.95f, - SpringSettings = new SpringSettings(30, 1), - }); - Simulation.Solver.Add(a, b, new AngularHinge { LocalHingeAxisA = new Vector3(0, 1, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); - } + LocalOffsetA = new Vector3(0, 1, 0), + LocalHingeAxisA = new Vector3(0, 1, 0), + LocalOffsetB = new Vector3(0, -1, 0), + LocalHingeAxisB = new Vector3(0, 1, 0), + SpringSettings = new SpringSettings(30, 1) + }); + } + { + var x = GetNextPosition(ref nextX); + var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); + var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + Simulation.Solver.Add(a, b, new AngularSwivelHinge { LocalSwivelAxisA = new Vector3(1, 0, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); + Simulation.Solver.Add(a, b, new SwingLimit { AxisLocalA = new Vector3(0, 1, 0), AxisLocalB = new Vector3(0, 1, 0), MaximumSwingAngle = MathHelper.PiOver2, SpringSettings = new SpringSettings(30, 1) }); + } + { + var x = GetNextPosition(ref nextX); + var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); + var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + Simulation.Solver.Add(a, b, new SwivelHinge { - var x = GetNextPosition(ref nextX); - var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); - var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - Simulation.Solver.Add(a, b, new TwistMotor - { - LocalAxisA = new Vector3(0, 1, 0), - LocalAxisB = new Vector3(0, 1, 0), - TargetVelocity = MathHelper.Pi * 2, - Settings = new MotorSettings(float.MaxValue, 0.1f) - }); - Simulation.Solver.Add(a, b, new AngularHinge { LocalHingeAxisA = new Vector3(0, 1, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); - } + LocalOffsetA = new Vector3(0, 1, 0), + LocalSwivelAxisA = new Vector3(1, 0, 0), + LocalOffsetB = new Vector3(0, -1, 0), + LocalHingeAxisB = new Vector3(0, 1, 0), + SpringSettings = new SpringSettings(30, 1) + }); + Simulation.Solver.Add(a, b, new SwingLimit { AxisLocalA = new Vector3(0, 1, 0), AxisLocalB = new Vector3(0, 1, 0), MaximumSwingAngle = MathHelper.PiOver2, SpringSettings = new SpringSettings(30, 1) }); + } + { + var x = GetNextPosition(ref nextX); + var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); + var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + Simulation.Solver.Add(a, b, new TwistServo { - var x = GetNextPosition(ref nextX); - var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); - var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - Simulation.Solver.Add(a, b, new AngularServo - { - TargetRelativeRotationLocalA = QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), MathHelper.PiOver2), - ServoSettings = new ServoSettings(float.MaxValue, 0, 12f), - SpringSettings = new SpringSettings(30, 1) - }); - } + LocalBasisA = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)), + LocalBasisB = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)), + TargetAngle = MathHelper.PiOver4, + SpringSettings = new SpringSettings(30, 1), + ServoSettings = new ServoSettings(float.MaxValue, 0, float.MaxValue) + }); + } + { + var x = GetNextPosition(ref nextX); + var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); + var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + Simulation.Solver.Add(a, b, new TwistLimit { - var x = GetNextPosition(ref nextX); - var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); - var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - Simulation.Solver.Add(a, b, new AngularMotor { TargetVelocityLocalA = new Vector3(0, 1, 0), Settings = new MotorSettings(15, 0.0001f) }); - } + LocalBasisA = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)), + LocalBasisB = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)), + MinimumAngle = MathHelper.Pi * -0.5f, + MaximumAngle = MathHelper.Pi * 0.95f, + SpringSettings = new SpringSettings(30, 1), + }); + Simulation.Solver.Add(a, b, new AngularHinge { LocalHingeAxisA = new Vector3(0, 1, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); + } + { + var x = GetNextPosition(ref nextX); + var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); + var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + Simulation.Solver.Add(a, b, new TwistMotor { - var x = GetNextPosition(ref nextX); - var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); - var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity); - //aDescription.Velocity.Angular = new Vector3(0, 0, 5); - var a = Simulation.Bodies.Add(aDescription); - var b = Simulation.Bodies.Add(bDescription); - Simulation.Solver.Add(a, b, new Weld { LocalOffset = new Vector3(0, 2, 0), LocalOrientation = Quaternion.Identity, SpringSettings = new SpringSettings(30, 1) }); - } + LocalAxisA = new Vector3(0, 1, 0), + LocalAxisB = new Vector3(0, 1, 0), + TargetVelocity = MathHelper.Pi * 2, + Settings = new MotorSettings(float.MaxValue, 0.1f) + }); + Simulation.Solver.Add(a, b, new AngularHinge { LocalHingeAxisA = new Vector3(0, 1, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1) }); + } + { + var x = GetNextPosition(ref nextX); + var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); + var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + Simulation.Solver.Add(a, b, new AngularServo { - var x = GetNextPosition(ref nextX); - var sphere = new Sphere(0.125f); - //Treat each vertex as a point mass that cannot rotate. - var sphereInertia = new BodyInertia { InverseMass = 1 }; - var sphereCollidable = new CollidableDescription(Simulation.Shapes.Add(sphere)); - var a = new Vector3(x, 3, 0); - var b = new Vector3(x, 4, 0); - var c = new Vector3(x, 3, 1); - var d = new Vector3(x + 1, 3, 0); - var aDescription = BodyDescription.CreateDynamic(a, sphereInertia, sphereCollidable, activity); - var bDescription = BodyDescription.CreateDynamic(b, sphereInertia, sphereCollidable, activity); - var cDescription = BodyDescription.CreateDynamic(c, sphereInertia, sphereCollidable, activity); - var dDescription = BodyDescription.CreateDynamic(d, sphereInertia, sphereCollidable, activity); - var aHandle = Simulation.Bodies.Add(aDescription); - var bHandle = Simulation.Bodies.Add(bDescription); - var cHandle = Simulation.Bodies.Add(cDescription); - var dHandle = Simulation.Bodies.Add(dDescription); - var distanceSpringiness = new SpringSettings(3f, 1); - Simulation.Solver.Add(aHandle, bHandle, new CenterDistanceConstraint(Vector3.Distance(a, b), distanceSpringiness)); - Simulation.Solver.Add(aHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(a, c), distanceSpringiness)); - Simulation.Solver.Add(aHandle, dHandle, new CenterDistanceConstraint(Vector3.Distance(a, d), distanceSpringiness)); - Simulation.Solver.Add(bHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(b, c), distanceSpringiness)); - Simulation.Solver.Add(bHandle, dHandle, new CenterDistanceConstraint(Vector3.Distance(b, d), distanceSpringiness)); - Simulation.Solver.Add(cHandle, dHandle, new CenterDistanceConstraint(Vector3.Distance(c, d), distanceSpringiness)); - Simulation.Solver.Add(aHandle, bHandle, cHandle, dHandle, new VolumeConstraint(a, b, c, d, new SpringSettings(30, 1))); - } + TargetRelativeRotationLocalA = QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), MathHelper.PiOver2), + ServoSettings = new ServoSettings(float.MaxValue, 0, 12f), + SpringSettings = new SpringSettings(30, 1) + }); + } + { + var x = GetNextPosition(ref nextX); + var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); + var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + Simulation.Solver.Add(a, b, new AngularMotor { TargetVelocityLocalA = new Vector3(0, 1, 0), Settings = new MotorSettings(15, 0.0001f) }); + } + { + var x = GetNextPosition(ref nextX); + var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); + var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity); + //aDescription.Velocity.Angular = new Vector3(0, 0, 5); + var a = Simulation.Bodies.Add(aDescription); + var b = Simulation.Bodies.Add(bDescription); + Simulation.Solver.Add(a, b, new Weld { LocalOffset = new Vector3(0, 2, 0), LocalOrientation = Quaternion.Identity, SpringSettings = new SpringSettings(30, 1) }); + } + { + var x = GetNextPosition(ref nextX); + var sphere = new Sphere(0.125f); + //Treat each vertex as a point mass that cannot rotate. + var sphereInertia = new BodyInertia { InverseMass = 1 }; + var sphereCollidable = new CollidableDescription(Simulation.Shapes.Add(sphere)); + var a = new Vector3(x, 3, 0); + var b = new Vector3(x, 4, 0); + var c = new Vector3(x, 3, 1); + var d = new Vector3(x + 1, 3, 0); + var aDescription = BodyDescription.CreateDynamic(a, sphereInertia, sphereCollidable, activity); + var bDescription = BodyDescription.CreateDynamic(b, sphereInertia, sphereCollidable, activity); + var cDescription = BodyDescription.CreateDynamic(c, sphereInertia, sphereCollidable, activity); + var dDescription = BodyDescription.CreateDynamic(d, sphereInertia, sphereCollidable, activity); + var aHandle = Simulation.Bodies.Add(aDescription); + var bHandle = Simulation.Bodies.Add(bDescription); + var cHandle = Simulation.Bodies.Add(cDescription); + var dHandle = Simulation.Bodies.Add(dDescription); + var distanceSpringiness = new SpringSettings(3f, 1); + Simulation.Solver.Add(aHandle, bHandle, new CenterDistanceConstraint(Vector3.Distance(a, b), distanceSpringiness)); + Simulation.Solver.Add(aHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(a, c), distanceSpringiness)); + Simulation.Solver.Add(aHandle, dHandle, new CenterDistanceConstraint(Vector3.Distance(a, d), distanceSpringiness)); + Simulation.Solver.Add(bHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(b, c), distanceSpringiness)); + Simulation.Solver.Add(bHandle, dHandle, new CenterDistanceConstraint(Vector3.Distance(b, d), distanceSpringiness)); + Simulation.Solver.Add(cHandle, dHandle, new CenterDistanceConstraint(Vector3.Distance(c, d), distanceSpringiness)); + Simulation.Solver.Add(aHandle, bHandle, cHandle, dHandle, new VolumeConstraint(a, b, c, d, new SpringSettings(30, 1))); + } + { + var x = GetNextPosition(ref nextX); + var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); + var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); + var a = Simulation.Bodies.Add(aDescription); + var b = Simulation.Bodies.Add(bDescription); + Simulation.Solver.Add(a, b, new DistanceServo(new Vector3(0, 0.55f, 0), new Vector3(0, -0.55f, 0), 1.9f, new SpringSettings(30, 1), ServoSettings.Default)); + } + { + var x = GetNextPosition(ref nextX); + var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); + var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); + var a = Simulation.Bodies.Add(aDescription); + var b = Simulation.Bodies.Add(bDescription); + Simulation.Solver.Add(a, b, new DistanceLimit(new Vector3(0, 0.55f, 0), new Vector3(0, -0.55f, 0), 1f, 3, new SpringSettings(30, 1))); + } + { + var x = GetNextPosition(ref nextX); + var sphere = new Sphere(0.125f); + //Treat each vertex as a point mass that cannot rotate. + var sphereInertia = new BodyInertia { InverseMass = 1 }; + var sphereCollidable = new CollidableDescription(Simulation.Shapes.Add(sphere)); + var a = new Vector3(x, 3, 0); + var b = new Vector3(x, 4, 0); + var c = new Vector3(x + 1, 3, 0); + var aDescription = BodyDescription.CreateDynamic(a, sphereInertia, sphereCollidable, activity); + var bDescription = BodyDescription.CreateDynamic(b, sphereInertia, sphereCollidable, activity); + var cDescription = BodyDescription.CreateDynamic(c, sphereInertia, sphereCollidable, activity); + var aHandle = Simulation.Bodies.Add(aDescription); + var bHandle = Simulation.Bodies.Add(bDescription); + var cHandle = Simulation.Bodies.Add(cDescription); + var distanceSpringiness = new SpringSettings(3f, 1); + Simulation.Solver.Add(aHandle, bHandle, new CenterDistanceConstraint(Vector3.Distance(a, b), distanceSpringiness)); + Simulation.Solver.Add(aHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(a, c), distanceSpringiness)); + Simulation.Solver.Add(bHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(b, c), distanceSpringiness)); + Simulation.Solver.Add(aHandle, bHandle, cHandle, new AreaConstraint(a, b, c, new SpringSettings(30, 1))); + } + { + var x = GetNextPosition(ref nextX); + var sphere = new Sphere(0.125f); + //Treat each vertex as a point mass that cannot rotate. + var sphereInertia = new BodyInertia { InverseMass = 1 }; + var sphereCollidable = new CollidableDescription(Simulation.Shapes.Add(sphere)); + var a = new Vector3(x, 3, 0); + var b = new Vector3(x, 4, 0); + var c = new Vector3(x + 1, 3, 0); + var aDescription = BodyDescription.CreateDynamic(a, sphereInertia, sphereCollidable, activity); + var bDescription = BodyDescription.CreateDynamic(b, sphereInertia, sphereCollidable, activity); + var cDescription = BodyDescription.CreateDynamic(c, sphereInertia, sphereCollidable, activity); + var aHandle = Simulation.Bodies.Add(aDescription); + var bHandle = Simulation.Bodies.Add(bDescription); + var cHandle = Simulation.Bodies.Add(cDescription); + var distanceSpringiness = new SpringSettings(3f, 1); + var distanceAB = Vector3.Distance(a, b); + var distanceBC = Vector3.Distance(b, c); + var distanceCA = Vector3.Distance(c, a); + Simulation.Solver.Add(aHandle, bHandle, new CenterDistanceLimit(distanceAB * 0.15f, distanceAB, distanceSpringiness)); + Simulation.Solver.Add(aHandle, cHandle, new CenterDistanceLimit(distanceBC * 0.15f, distanceBC, distanceSpringiness)); + Simulation.Solver.Add(bHandle, cHandle, new CenterDistanceLimit(distanceCA * 0.15f, distanceCA, distanceSpringiness)); + } + { + var x = GetNextPosition(ref nextX); + var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), default, collidableA, activity); + var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); + var a = Simulation.Bodies.Add(aDescription); + var b = Simulation.Bodies.Add(bDescription); + Simulation.Solver.Add(a, b, new PointOnLineServo { - var x = GetNextPosition(ref nextX); - var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); - var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); - var a = Simulation.Bodies.Add(aDescription); - var b = Simulation.Bodies.Add(bDescription); - Simulation.Solver.Add(a, b, new DistanceServo(new Vector3(0, 0.55f, 0), new Vector3(0, -0.55f, 0), 1.9f, new SpringSettings(30, 1), ServoSettings.Default)); - } + LocalOffsetA = new Vector3(0, 0.5f, 0), + LocalOffsetB = new Vector3(0, -0.5f, 0), + LocalDirection = new Vector3(0, 1, 0), + SpringSettings = new SpringSettings(30, 1), + ServoSettings = ServoSettings.Default + }); + } + { + var x = GetNextPosition(ref nextX); + var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); + var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); + var a = Simulation.Bodies.Add(aDescription); + var b = Simulation.Bodies.Add(bDescription); + Simulation.Solver.Add(a, b, new LinearAxisServo { - var x = GetNextPosition(ref nextX); - var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); - var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); - var a = Simulation.Bodies.Add(aDescription); - var b = Simulation.Bodies.Add(bDescription); - Simulation.Solver.Add(a, b, new DistanceLimit(new Vector3(0, 0.55f, 0), new Vector3(0, -0.55f, 0), 1f, 3, new SpringSettings(30, 1))); - } + LocalOffsetA = new Vector3(0, 0.5f, 0), + LocalOffsetB = new Vector3(0, -0.5f, 0), + LocalPlaneNormal = new Vector3(0, 1, 0), + TargetOffset = 2, + SpringSettings = new SpringSettings(30, 1), + ServoSettings = ServoSettings.Default + }); + } + { + var x = GetNextPosition(ref nextX); + var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); + var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); + var a = Simulation.Bodies.Add(aDescription); + var b = Simulation.Bodies.Add(bDescription); + Simulation.Solver.Add(a, b, new PointOnLineServo { - var x = GetNextPosition(ref nextX); - var sphere = new Sphere(0.125f); - //Treat each vertex as a point mass that cannot rotate. - var sphereInertia = new BodyInertia { InverseMass = 1 }; - var sphereCollidable = new CollidableDescription(Simulation.Shapes.Add(sphere)); - var a = new Vector3(x, 3, 0); - var b = new Vector3(x, 4, 0); - var c = new Vector3(x + 1, 3, 0); - var aDescription = BodyDescription.CreateDynamic(a, sphereInertia, sphereCollidable, activity); - var bDescription = BodyDescription.CreateDynamic(b, sphereInertia, sphereCollidable, activity); - var cDescription = BodyDescription.CreateDynamic(c, sphereInertia, sphereCollidable, activity); - var aHandle = Simulation.Bodies.Add(aDescription); - var bHandle = Simulation.Bodies.Add(bDescription); - var cHandle = Simulation.Bodies.Add(cDescription); - var distanceSpringiness = new SpringSettings(3f, 1); - Simulation.Solver.Add(aHandle, bHandle, new CenterDistanceConstraint(Vector3.Distance(a, b), distanceSpringiness)); - Simulation.Solver.Add(aHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(a, c), distanceSpringiness)); - Simulation.Solver.Add(bHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(b, c), distanceSpringiness)); - Simulation.Solver.Add(aHandle, bHandle, cHandle, new AreaConstraint(a, b, c, new SpringSettings(30, 1))); - } + LocalOffsetA = new Vector3(0, 0.5f, 0), + LocalOffsetB = new Vector3(0, -0.5f, 0), + LocalDirection = new Vector3(0, 1, 0), + SpringSettings = new SpringSettings(30, 1), + ServoSettings = ServoSettings.Default + }); + Simulation.Solver.Add(a, b, new LinearAxisMotor { - var x = GetNextPosition(ref nextX); - var sphere = new Sphere(0.125f); - //Treat each vertex as a point mass that cannot rotate. - var sphereInertia = new BodyInertia { InverseMass = 1 }; - var sphereCollidable = new CollidableDescription(Simulation.Shapes.Add(sphere)); - var a = new Vector3(x, 3, 0); - var b = new Vector3(x, 4, 0); - var c = new Vector3(x + 1, 3, 0); - var aDescription = BodyDescription.CreateDynamic(a, sphereInertia, sphereCollidable, activity); - var bDescription = BodyDescription.CreateDynamic(b, sphereInertia, sphereCollidable, activity); - var cDescription = BodyDescription.CreateDynamic(c, sphereInertia, sphereCollidable, activity); - var aHandle = Simulation.Bodies.Add(aDescription); - var bHandle = Simulation.Bodies.Add(bDescription); - var cHandle = Simulation.Bodies.Add(cDescription); - var distanceSpringiness = new SpringSettings(3f, 1); - var distanceAB = Vector3.Distance(a, b); - var distanceBC = Vector3.Distance(b, c); - var distanceCA = Vector3.Distance(c, a); - Simulation.Solver.Add(aHandle, bHandle, new CenterDistanceLimit(distanceAB * 0.15f, distanceAB, distanceSpringiness)); - Simulation.Solver.Add(aHandle, cHandle, new CenterDistanceLimit(distanceBC * 0.15f, distanceBC, distanceSpringiness)); - Simulation.Solver.Add(bHandle, cHandle, new CenterDistanceLimit(distanceCA * 0.15f, distanceCA, distanceSpringiness)); - } + LocalOffsetA = new Vector3(0, 0.5f, 0), + LocalOffsetB = new Vector3(0, -0.5f, 0), + LocalAxis = new Vector3(0, 1, 0), + TargetVelocity = -2, + Settings = new MotorSettings(15, 0.01f) + }); + } + { + var x = GetNextPosition(ref nextX); + var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); + var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); + var a = Simulation.Bodies.Add(aDescription); + var b = Simulation.Bodies.Add(bDescription); + Simulation.Solver.Add(a, b, new PointOnLineServo { - var x = GetNextPosition(ref nextX); - var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), default, collidableA, activity); - var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); - var a = Simulation.Bodies.Add(aDescription); - var b = Simulation.Bodies.Add(bDescription); - Simulation.Solver.Add(a, b, new PointOnLineServo - { - LocalOffsetA = new Vector3(0, 0.5f, 0), - LocalOffsetB = new Vector3(0, -0.5f, 0), - LocalDirection = new Vector3(0, 1, 0), - SpringSettings = new SpringSettings(30, 1), - ServoSettings = ServoSettings.Default - }); - } + LocalOffsetA = new Vector3(0, 0.5f, 0), + LocalOffsetB = new Vector3(0, -0.5f, 0), + LocalDirection = new Vector3(0, 1, 0), + SpringSettings = new SpringSettings(30, 1), + ServoSettings = ServoSettings.Default + }); + Simulation.Solver.Add(a, b, new LinearAxisLimit { - var x = GetNextPosition(ref nextX); - var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); - var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); - var a = Simulation.Bodies.Add(aDescription); - var b = Simulation.Bodies.Add(bDescription); - Simulation.Solver.Add(a, b, new LinearAxisServo - { - LocalOffsetA = new Vector3(0, 0.5f, 0), - LocalOffsetB = new Vector3(0, -0.5f, 0), - LocalPlaneNormal = new Vector3(0, 1, 0), - TargetOffset = 2, - SpringSettings = new SpringSettings(30, 1), - ServoSettings = ServoSettings.Default - }); - } + LocalOffsetA = new Vector3(0, 0.5f, 0), + LocalOffsetB = new Vector3(0, -0.5f, 0), + LocalAxis = new Vector3(0, 1, 0), + MinimumOffset = 1, + MaximumOffset = 2, + SpringSettings = new SpringSettings(30, 1) + }); + } + { + var x = GetNextPosition(ref nextX); + var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); + var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + Simulation.Solver.Add(b, a, new AngularAxisMotor { - var x = GetNextPosition(ref nextX); - var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); - var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); - var a = Simulation.Bodies.Add(aDescription); - var b = Simulation.Bodies.Add(bDescription); - Simulation.Solver.Add(a, b, new PointOnLineServo - { - LocalOffsetA = new Vector3(0, 0.5f, 0), - LocalOffsetB = new Vector3(0, -0.5f, 0), - LocalDirection = new Vector3(0, 1, 0), - SpringSettings = new SpringSettings(30, 1), - ServoSettings = ServoSettings.Default - }); - Simulation.Solver.Add(a, b, new LinearAxisMotor - { - LocalOffsetA = new Vector3(0, 0.5f, 0), - LocalOffsetB = new Vector3(0, -0.5f, 0), - LocalAxis = new Vector3(0, 1, 0), - TargetVelocity = -2, - Settings = new MotorSettings(15, 0.01f) - }); - } + LocalAxisA = new Vector3(0, 1, 0), + TargetVelocity = MathHelper.Pi * 5, + Settings = new MotorSettings(float.MaxValue, 0.1f) + }); + } + { + var x = GetNextPosition(ref nextX); + var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); + Simulation.Solver.Add(a, new OneBodyLinearServo { - var x = GetNextPosition(ref nextX); - var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); - var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); - var a = Simulation.Bodies.Add(aDescription); - var b = Simulation.Bodies.Add(bDescription); - Simulation.Solver.Add(a, b, new PointOnLineServo - { - LocalOffsetA = new Vector3(0, 0.5f, 0), - LocalOffsetB = new Vector3(0, -0.5f, 0), - LocalDirection = new Vector3(0, 1, 0), - SpringSettings = new SpringSettings(30, 1), - ServoSettings = ServoSettings.Default - }); - Simulation.Solver.Add(a, b, new LinearAxisLimit - { - LocalOffsetA = new Vector3(0, 0.5f, 0), - LocalOffsetB = new Vector3(0, -0.5f, 0), - LocalAxis = new Vector3(0, 1, 0), - MinimumOffset = 1, - MaximumOffset = 2, - SpringSettings = new SpringSettings(30, 1) - }); - } + LocalOffset = new Vector3(0, 1, 0), + Target = new Vector3(x, 3, 0), + ServoSettings = new ServoSettings(2, 0, float.MaxValue), + SpringSettings = new SpringSettings(5, 1) + }); + } + { + var x = GetNextPosition(ref nextX); + var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); + var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + Simulation.Solver.Add(a, new OneBodyLinearMotor { - var x = GetNextPosition(ref nextX); - var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity)); - var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - Simulation.Solver.Add(b, a, new AngularAxisMotor - { - LocalAxisA = new Vector3(0, 1, 0), - TargetVelocity = MathHelper.Pi * 5, - Settings = new MotorSettings(float.MaxValue, 0.1f) - }); - } + LocalOffset = new Vector3(0, 1, 0), + TargetVelocity = new Vector3(0, -1, 0), + Settings = new MotorSettings(float.MaxValue, 1e-2f), + }); + } + { + var x = GetNextPosition(ref nextX); + var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); + var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + Simulation.Solver.Add(a, new OneBodyAngularServo { - var x = GetNextPosition(ref nextX); - var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); - Simulation.Solver.Add(a, new OneBodyLinearServo - { - LocalOffset = new Vector3(0, 1, 0), - Target = new Vector3(x, 3, 0), - ServoSettings = new ServoSettings(2, 0, float.MaxValue), - SpringSettings = new SpringSettings(5, 1) - }); - } + TargetOrientation = Quaternion.Identity, + ServoSettings = ServoSettings.Default, + SpringSettings = new SpringSettings(30f, 1f) + }); + } + { + var x = GetNextPosition(ref nextX); + var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); + var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); + Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); + Simulation.Solver.Add(a, new OneBodyAngularMotor { - var x = GetNextPosition(ref nextX); - var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); - var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - Simulation.Solver.Add(a, new OneBodyLinearMotor - { - LocalOffset = new Vector3(0, 1, 0), - TargetVelocity = new Vector3(0, -1, 0), - Settings = new MotorSettings(float.MaxValue, 1e-2f), - }); - } + TargetVelocity = new Vector3(1, 0, 0), + Settings = new MotorSettings(float.MaxValue, 0.001f), + }); + } + { + var x = GetNextPosition(ref nextX); + var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); + var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); + var a = Simulation.Bodies.Add(aDescription); + var b = Simulation.Bodies.Add(bDescription); + Simulation.Solver.Add(a, b, new BallSocketMotor { - var x = GetNextPosition(ref nextX); - var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); - var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - Simulation.Solver.Add(a, new OneBodyAngularServo - { - TargetOrientation = Quaternion.Identity, - ServoSettings = ServoSettings.Default, - SpringSettings = new SpringSettings(30f, 1f) - }); - } + LocalOffsetB = new Vector3(0, -1, 0), + TargetVelocityLocalA = new Vector3(0, -0.25f, 0), + Settings = new MotorSettings(10, 1e-4f) + }); + } + { + var x = GetNextPosition(ref nextX); + var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); + var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); + var a = Simulation.Bodies.Add(aDescription); + var b = Simulation.Bodies.Add(bDescription); + Simulation.Solver.Add(a, b, new BallSocketServo { - var x = GetNextPosition(ref nextX); - var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity)); - var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity)); - Simulation.Solver.Add(a, b, new BallSocket { LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1) }); - Simulation.Solver.Add(a, new OneBodyAngularMotor - { - TargetVelocity = new Vector3(1, 0, 0), - Settings = new MotorSettings(float.MaxValue, 0.001f), - }); - } + LocalOffsetA = new Vector3(0, 1, 0), + LocalOffsetB = new Vector3(0, -1, 0), + SpringSettings = new SpringSettings(30, 1), + ServoSettings = new ServoSettings(100, 1, 100) + }); + } + { + var x = GetNextPosition(ref nextX); + var wheelShape = new CollidableDescription(Simulation.Shapes.Add(new Cylinder(1, 0.1f))); + var wheelOrientation = Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), MathF.PI * 0.5f); + var aDescription = BodyDescription.CreateDynamic((new Vector3(x, 3, 0), wheelOrientation), inertiaA, wheelShape, activity); + var bDescription = BodyDescription.CreateDynamic((new Vector3(x, 6, 0), wheelOrientation), inertiaB, wheelShape, activity); + var cDescription = BodyDescription.CreateKinematic(new Vector3(x, 4.5f, -1), Simulation.Shapes.Add(new Box(3, 6, 1)), activity); + var a = Simulation.Bodies.Add(aDescription); + var b = Simulation.Bodies.Add(bDescription); + var c = Simulation.Bodies.Add(cDescription); + Simulation.Solver.Add(a, b, new AngularAxisGearMotor { - var x = GetNextPosition(ref nextX); - var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); - var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); - var a = Simulation.Bodies.Add(aDescription); - var b = Simulation.Bodies.Add(bDescription); - Simulation.Solver.Add(a, b, new BallSocketMotor - { - LocalOffsetB = new Vector3(0, -1, 0), - TargetVelocityLocalA = new Vector3(0, -0.25f, 0), - Settings = new MotorSettings(10, 1e-4f) - }); - } + LocalAxisA = new Vector3(0, 1, 0), + VelocityScale = -4, + Settings = new MotorSettings(float.MaxValue, 0.0001f) + }); + Simulation.Solver.Add(c, a, new Hinge { - var x = GetNextPosition(ref nextX); - var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity); - var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity); - var a = Simulation.Bodies.Add(aDescription); - var b = Simulation.Bodies.Add(bDescription); - Simulation.Solver.Add(a, b, new BallSocketServo - { - LocalOffsetA = new Vector3(0, 1, 0), - LocalOffsetB = new Vector3(0, -1, 0), - SpringSettings = new SpringSettings(30, 1), - ServoSettings = new ServoSettings(100, 1, 100) - }); - } + LocalOffsetA = new Vector3(0, -1.5f, 1), + LocalHingeAxisA = new Vector3(0, 0, 1), + LocalOffsetB = new Vector3(0, 0, 0), + LocalHingeAxisB = new Vector3(0, 1, 0), + SpringSettings = new SpringSettings(30, 1) + }); + Simulation.Solver.Add(c, b, new Hinge { - var x = GetNextPosition(ref nextX); - var wheelShape = new CollidableDescription(Simulation.Shapes.Add(new Cylinder(1, 0.1f))); - var wheelOrientation = Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), MathF.PI * 0.5f); - var aDescription = BodyDescription.CreateDynamic((new Vector3(x, 3, 0), wheelOrientation), inertiaA, wheelShape, activity); - var bDescription = BodyDescription.CreateDynamic((new Vector3(x, 6, 0), wheelOrientation), inertiaB, wheelShape, activity); - var cDescription = BodyDescription.CreateKinematic(new Vector3(x, 4.5f, -1), Simulation.Shapes.Add(new Box(3, 6, 1)), activity); - var a = Simulation.Bodies.Add(aDescription); - var b = Simulation.Bodies.Add(bDescription); - var c = Simulation.Bodies.Add(cDescription); - Simulation.Solver.Add(a, b, new AngularAxisGearMotor - { - LocalAxisA = new Vector3(0, 1, 0), - VelocityScale = -4, - Settings = new MotorSettings(float.MaxValue, 0.0001f) - }); - Simulation.Solver.Add(c, a, new Hinge - { - LocalOffsetA = new Vector3(0, -1.5f, 1), - LocalHingeAxisA = new Vector3(0, 0, 1), - LocalOffsetB = new Vector3(0, 0, 0), - LocalHingeAxisB = new Vector3(0, 1, 0), - SpringSettings = new SpringSettings(30, 1) - }); - Simulation.Solver.Add(c, b, new Hinge - { - LocalOffsetA = new Vector3(0, 1.5f, 1), - LocalHingeAxisA = new Vector3(0, 0, 1), - LocalOffsetB = new Vector3(0, 0, 0), - LocalHingeAxisB = new Vector3(0, 1, 0), - SpringSettings = new SpringSettings(30, 1) - }); - } - - Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(new Box(256, 1, 256)))); + LocalOffsetA = new Vector3(0, 1.5f, 1), + LocalHingeAxisA = new Vector3(0, 0, 1), + LocalOffsetB = new Vector3(0, 0, 0), + LocalHingeAxisB = new Vector3(0, 1, 0), + SpringSettings = new SpringSettings(30, 1) + }); } + + Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(new Box(256, 1, 256)))); } } diff --git a/Demos/SpecializedTests/CylinderTestDemo.cs b/Demos/SpecializedTests/CylinderTestDemo.cs index 3f43ad87a..517104447 100644 --- a/Demos/SpecializedTests/CylinderTestDemo.cs +++ b/Demos/SpecializedTests/CylinderTestDemo.cs @@ -9,340 +9,339 @@ using System.Diagnostics; using BepuPhysics.Constraints; -namespace Demos.SpecializedTests +namespace Demos.SpecializedTests; + +public class CylinderTestDemo : Demo { - public class CylinderTestDemo : Demo + private static void BruteForceSearch(Vector3 lineOrigin, Vector3 lineDirection, float halfLength, in Cylinder cylinder, out float closestT, out float closestDistanceSquared, out float errorMargin) { - private static void BruteForceSearch(Vector3 lineOrigin, Vector3 lineDirection, float halfLength, in Cylinder cylinder, out float closestT, out float closestDistanceSquared, out float errorMargin) + const int sampleCount = 1 << 20; + var inverseSampleCount = 1.0 / (sampleCount - 1); + errorMargin = (float)inverseSampleCount; + var radiusSquared = cylinder.Radius * cylinder.Radius; + closestDistanceSquared = float.MaxValue; + closestT = float.MaxValue; + for (int i = 0; i < sampleCount; ++i) { - const int sampleCount = 1 << 20; - var inverseSampleCount = 1.0 / (sampleCount - 1); - errorMargin = (float)inverseSampleCount; - var radiusSquared = cylinder.Radius * cylinder.Radius; - closestDistanceSquared = float.MaxValue; - closestT = float.MaxValue; - for (int i = 0; i < sampleCount; ++i) + var t = (float)(halfLength * (i * inverseSampleCount * 2 - 1)); + var point = lineOrigin + lineDirection * t; + var horizontalLengthSquared = point.X * point.X + point.Z * point.Z; + Vector3 clamped; + if (horizontalLengthSquared > radiusSquared) { - var t = (float)(halfLength * (i * inverseSampleCount * 2 - 1)); - var point = lineOrigin + lineDirection * t; - var horizontalLengthSquared = point.X * point.X + point.Z * point.Z; - Vector3 clamped; - if (horizontalLengthSquared > radiusSquared) - { - var scale = cylinder.Radius / MathF.Sqrt(horizontalLengthSquared); - clamped.X = scale * point.X; - clamped.Z = scale * point.Z; - } - else - { - clamped.X = point.X; - clamped.Z = point.Z; - } - clamped.Y = MathF.Max(-cylinder.HalfLength, MathF.Min(cylinder.HalfLength, point.Y)); - var distanceSquared = Vector3.DistanceSquared(clamped, point); - if (distanceSquared < closestDistanceSquared) - { - closestDistanceSquared = distanceSquared; - closestT = t; - } + var scale = cylinder.Radius / MathF.Sqrt(horizontalLengthSquared); + clamped.X = scale * point.X; + clamped.Z = scale * point.Z; + } + else + { + clamped.X = point.X; + clamped.Z = point.Z; + } + clamped.Y = MathF.Max(-cylinder.HalfLength, MathF.Min(cylinder.HalfLength, point.Y)); + var distanceSquared = Vector3.DistanceSquared(clamped, point); + if (distanceSquared < closestDistanceSquared) + { + closestDistanceSquared = distanceSquared; + closestT = t; } - } - private static void TestSegmentCylinder() + } + + private static void TestSegmentCylinder() + { + var cylinder = new Cylinder(0.5f, 1); + CylinderWide cylinderWide = default; + cylinderWide.Broadcast(cylinder); + Random random = new Random(5); + //double totalIntervalError = 0; + //double sumOfSquaredIntervalError = 0; + + double totalBruteError = 0; + double sumOfSquaredBruteError = 0; + + double totalBruteDistanceError = 0; + double sumOfSquaredBruteDistanceError = 0; + + //long iterationsSum = 0; + //long iterationsSquaredSum = 0; + var capsuleTests = 1000; + + int warmupCount = 32; + int innerIterations = 128; + long testTicks = 0; + for (int i = 0; i < warmupCount + capsuleTests; ++i) { - var cylinder = new Cylinder(0.5f, 1); - CylinderWide cylinderWide = default; - cylinderWide.Broadcast(cylinder); - Random random = new Random(5); - //double totalIntervalError = 0; - //double sumOfSquaredIntervalError = 0; - - double totalBruteError = 0; - double sumOfSquaredBruteError = 0; - - double totalBruteDistanceError = 0; - double sumOfSquaredBruteDistanceError = 0; - - //long iterationsSum = 0; - //long iterationsSquaredSum = 0; - var capsuleTests = 1000; - - int warmupCount = 32; - int innerIterations = 128; - long testTicks = 0; - for (int i = 0; i < warmupCount + capsuleTests; ++i) + Vector3 randomPointNearCylinder; + var capsule = new Capsule(0.2f + .8f * random.NextSingle(), 0.2f + 0.8f * random.NextSingle()); + var minimumDistance = 1f * (cylinder.Radius + cylinder.HalfLength); + var minimumDistanceSquared = minimumDistance * minimumDistance; + while (true) { - Vector3 randomPointNearCylinder; - var capsule = new Capsule(0.2f + .8f * random.NextSingle(), 0.2f + 0.8f * random.NextSingle()); - var minimumDistance = 1f * (cylinder.Radius + cylinder.HalfLength); - var minimumDistanceSquared = minimumDistance * minimumDistance; - while (true) - { - randomPointNearCylinder = new Vector3((cylinder.Radius + capsule.HalfLength) * 2, (cylinder.HalfLength + capsule.HalfLength) * 2, (cylinder.Radius + capsule.HalfLength) * 2) * - (new Vector3(2) * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) - Vector3.One); - var pointOnCylinderAxis = new Vector3(0, MathF.Max(-cylinder.HalfLength, MathF.Min(cylinder.HalfLength, randomPointNearCylinder.Y)), 0); - var offset = randomPointNearCylinder - pointOnCylinderAxis; - var lengthSquared = offset.LengthSquared(); - if (lengthSquared > minimumDistanceSquared) - break; - } + randomPointNearCylinder = new Vector3((cylinder.Radius + capsule.HalfLength) * 2, (cylinder.HalfLength + capsule.HalfLength) * 2, (cylinder.Radius + capsule.HalfLength) * 2) * + (new Vector3(2) * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) - Vector3.One); + var pointOnCylinderAxis = new Vector3(0, MathF.Max(-cylinder.HalfLength, MathF.Min(cylinder.HalfLength, randomPointNearCylinder.Y)), 0); + var offset = randomPointNearCylinder - pointOnCylinderAxis; + var lengthSquared = offset.LengthSquared(); + if (lengthSquared > minimumDistanceSquared) + break; + } - Vector3 direction; - float directionLengthSquared; - do - { - direction = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) * new Vector3(2) - Vector3.One; - directionLengthSquared = direction.LengthSquared(); - } while (directionLengthSquared < 1e-8f); - direction /= MathF.Sqrt(directionLengthSquared); + Vector3 direction; + float directionLengthSquared; + do + { + direction = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) * new Vector3(2) - Vector3.One; + directionLengthSquared = direction.LengthSquared(); + } while (directionLengthSquared < 1e-8f); + direction /= MathF.Sqrt(directionLengthSquared); - Vector3Wide.Broadcast(randomPointNearCylinder, out var capsuleOrigin); - Vector3Wide.Broadcast(direction, out var capsuleY); + Vector3Wide.Broadcast(randomPointNearCylinder, out var capsuleOrigin); + Vector3Wide.Broadcast(direction, out var capsuleY); - //CapsuleCylinderTester.GetClosestPointBetweenLineSegmentAndCylinder(capsuleOrigin, capsuleY, new Vector(capsule.HalfLength), cylinderWide, out var t, out var min, out var max, out var offsetFromCylindertoLineSegment, out var iterationsRequired); + //CapsuleCylinderTester.GetClosestPointBetweenLineSegmentAndCylinder(capsuleOrigin, capsuleY, new Vector(capsule.HalfLength), cylinderWide, out var t, out var min, out var max, out var offsetFromCylindertoLineSegment, out var iterationsRequired); - //CapsuleCylinderTester.GetClosestPointBetweenLineSegmentAndCylinder(capsuleOrigin, capsuleY, new Vector(capsule.HalfLength), cylinderWide, out var t, out var offsetFromCylindertoLineSegment); - Vector t = default; - Vector3Wide offsetFromCylinderToLineSegment = default; - var innerStart = Stopwatch.GetTimestamp(); - for (int j = 0; j < innerIterations; ++j) - { - CapsuleCylinderTester.GetClosestPointBetweenLineSegmentAndCylinder(capsuleOrigin, capsuleY, new Vector(capsule.HalfLength), cylinderWide, Vector.Zero, out t, out offsetFromCylinderToLineSegment); - } - var innerStop = Stopwatch.GetTimestamp(); - if (i > warmupCount) - { - testTicks += innerStop - innerStart; - } - Vector3Wide.LengthSquared(offsetFromCylinderToLineSegment, out var distanceSquaredWide); - var distanceSquared = distanceSquaredWide[0]; + //CapsuleCylinderTester.GetClosestPointBetweenLineSegmentAndCylinder(capsuleOrigin, capsuleY, new Vector(capsule.HalfLength), cylinderWide, out var t, out var offsetFromCylindertoLineSegment); + Vector t = default; + Vector3Wide offsetFromCylinderToLineSegment = default; + var innerStart = Stopwatch.GetTimestamp(); + for (int j = 0; j < innerIterations; ++j) + { + CapsuleCylinderTester.GetClosestPointBetweenLineSegmentAndCylinder(capsuleOrigin, capsuleY, new Vector(capsule.HalfLength), cylinderWide, Vector.Zero, out t, out offsetFromCylinderToLineSegment); + } + var innerStop = Stopwatch.GetTimestamp(); + if (i > warmupCount) + { + testTicks += innerStop - innerStart; + } + Vector3Wide.LengthSquared(offsetFromCylinderToLineSegment, out var distanceSquaredWide); + var distanceSquared = distanceSquaredWide[0]; - //iterationsSum += iterationsRequired[0]; - //iterationsSquaredSum += iterationsRequired[0] * iterationsRequired[0]; + //iterationsSum += iterationsRequired[0]; + //iterationsSquaredSum += iterationsRequired[0] * iterationsRequired[0]; - BruteForceSearch(randomPointNearCylinder, direction, capsule.HalfLength, cylinder, out var bruteT, out var bruteDistanceSquared, out var errorMargin); - var errorRelativeToBrute = MathF.Max(MathF.Abs(bruteT - t[0]), errorMargin) - errorMargin; - sumOfSquaredBruteError += errorRelativeToBrute * errorRelativeToBrute; - totalBruteError += errorRelativeToBrute; + BruteForceSearch(randomPointNearCylinder, direction, capsule.HalfLength, cylinder, out var bruteT, out var bruteDistanceSquared, out var errorMargin); + var errorRelativeToBrute = MathF.Max(MathF.Abs(bruteT - t[0]), errorMargin) - errorMargin; + sumOfSquaredBruteError += errorRelativeToBrute * errorRelativeToBrute; + totalBruteError += errorRelativeToBrute; - if ((distanceSquared == 0) != (bruteDistanceSquared == 0)) - { - Console.WriteLine($"Search and brute force disagree on intersecting distance; search found {distanceSquared}, brute found {bruteDistanceSquared}"); - } + if ((distanceSquared == 0) != (bruteDistanceSquared == 0)) + { + Console.WriteLine($"Search and brute force disagree on intersecting distance; search found {distanceSquared}, brute found {bruteDistanceSquared}"); + } - var bruteDistanceError = MathF.Abs(MathF.Sqrt(distanceSquared) - MathF.Sqrt(bruteDistanceSquared)); - sumOfSquaredBruteDistanceError += bruteDistanceError * bruteDistanceError; - totalBruteDistanceError += bruteDistanceError; + var bruteDistanceError = MathF.Abs(MathF.Sqrt(distanceSquared) - MathF.Sqrt(bruteDistanceSquared)); + sumOfSquaredBruteDistanceError += bruteDistanceError * bruteDistanceError; + totalBruteDistanceError += bruteDistanceError; - //var intervalSpan = Vector.Abs(max - min)[0]; - //sumOfSquaredIntervalError += intervalSpan * intervalSpan; - //totalIntervalError += intervalSpan; + //var intervalSpan = Vector.Abs(max - min)[0]; + //sumOfSquaredIntervalError += intervalSpan * intervalSpan; + //totalIntervalError += intervalSpan; - } - Console.WriteLine($"Average time per test (ns): {1e9 * testTicks / (innerIterations * capsuleTests * Stopwatch.Frequency)}"); - - //var averageIntervalSpan = totalIntervalError / capsuleTests; - //var averageIntervalSquaredSpan = sumOfSquaredIntervalError / capsuleTests; - //var intervalStandardDeviation = Math.Sqrt(Math.Max(0, averageIntervalSquaredSpan - averageIntervalSpan * averageIntervalSpan)); - //Console.WriteLine($"Average interval span: {averageIntervalSpan}, stddev {intervalStandardDeviation}"); - - var averageBruteError = totalBruteError / capsuleTests; - var averageBruteSquaredError = sumOfSquaredBruteError / capsuleTests; - var bruteStandardDeviation = Math.Sqrt(Math.Max(0, averageBruteSquaredError - averageBruteError * averageBruteError)); - Console.WriteLine($"Average brute T error: {averageBruteError}, stddev {bruteStandardDeviation}"); - - var averageBruteDistanceError = totalBruteDistanceError / capsuleTests; - var averageBruteDistanceSquaredError = sumOfSquaredBruteDistanceError / capsuleTests; - var bruteDistanceStandardDeviation = Math.Sqrt(Math.Max(0, averageBruteSquaredError - averageBruteError * averageBruteError)); - Console.WriteLine($"Average brute distance error: {averageBruteDistanceError}, stddev {bruteDistanceStandardDeviation}"); - - //var averageIterations = (double)iterationsSum / capsuleTests; - //var averageIterationSquared = (double)iterationsSquaredSum / capsuleTests; - //var iterationStandardDeviation = Math.Sqrt(Math.Max(0, averageIterationSquared - averageIterations * averageIterations)); - //Console.WriteLine($"Average iteration count: {averageIterations}, stddev {iterationStandardDeviation}"); } + Console.WriteLine($"Average time per test (ns): {1e9 * testTicks / (innerIterations * capsuleTests * Stopwatch.Frequency)}"); + + //var averageIntervalSpan = totalIntervalError / capsuleTests; + //var averageIntervalSquaredSpan = sumOfSquaredIntervalError / capsuleTests; + //var intervalStandardDeviation = Math.Sqrt(Math.Max(0, averageIntervalSquaredSpan - averageIntervalSpan * averageIntervalSpan)); + //Console.WriteLine($"Average interval span: {averageIntervalSpan}, stddev {intervalStandardDeviation}"); + + var averageBruteError = totalBruteError / capsuleTests; + var averageBruteSquaredError = sumOfSquaredBruteError / capsuleTests; + var bruteStandardDeviation = Math.Sqrt(Math.Max(0, averageBruteSquaredError - averageBruteError * averageBruteError)); + Console.WriteLine($"Average brute T error: {averageBruteError}, stddev {bruteStandardDeviation}"); + + var averageBruteDistanceError = totalBruteDistanceError / capsuleTests; + var averageBruteDistanceSquaredError = sumOfSquaredBruteDistanceError / capsuleTests; + var bruteDistanceStandardDeviation = Math.Sqrt(Math.Max(0, averageBruteSquaredError - averageBruteError * averageBruteError)); + Console.WriteLine($"Average brute distance error: {averageBruteDistanceError}, stddev {bruteDistanceStandardDeviation}"); + + //var averageIterations = (double)iterationsSum / capsuleTests; + //var averageIterationSquared = (double)iterationsSquaredSum / capsuleTests; + //var iterationStandardDeviation = Math.Sqrt(Math.Max(0, averageIterationSquared - averageIterations * averageIterations)); + //Console.WriteLine($"Average iteration count: {averageIterations}, stddev {iterationStandardDeviation}"); + } - public override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(10, 0, 6); + camera.Pitch = 0; + camera.Yaw = 0; + + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, 0f, 0)), new SolveDescription(8, 1)); + + var cylinderShape = new Cylinder(1f, .2f); + var cylinder = BodyDescription.CreateDynamic(new Vector3(10f, 3, 0), cylinderShape.ComputeInertia(1), new(Simulation.Shapes.Add(cylinderShape), 1000f, 1000f, ContinuousDetection.Passive), 0.01f); + Simulation.Bodies.Add(cylinder); + Simulation.Bodies.Add(BodyDescription.CreateConvexKinematic((new Vector3(0, -6, 0), QuaternionEx.CreateFromAxisAngle(Vector3.Normalize(new Vector3(1, 0, 1)), MathHelper.PiOver4)), Simulation.Shapes, new Sphere(2))); + Simulation.Bodies.Add(BodyDescription.CreateConvexKinematic((new Vector3(7, -6, 0), QuaternionEx.CreateFromAxisAngle(Vector3.Normalize(new Vector3(1, 0, 1)), MathHelper.PiOver4)), Simulation.Shapes, new Capsule(0.5f, 1f))); + Simulation.Bodies.Add(BodyDescription.CreateConvexKinematic((new Vector3(21, -3, 0), QuaternionEx.CreateFromAxisAngle(Vector3.Normalize(new Vector3(1, 0, 1)), 0)), Simulation.Shapes, new Box(3f, 1f, 3f))); + Simulation.Bodies.Add(BodyDescription.CreateConvexKinematic((new Vector3(28, -6, 0), QuaternionEx.CreateFromAxisAngle(Vector3.Normalize(new Vector3(1, 0, 1)), 0)), Simulation.Shapes, + new Triangle(new Vector3(10f, 0, 10f), new Vector3(14f, 0, 10f), new Vector3(10f, 0, 14f)))); + Simulation.Bodies.Add(BodyDescription.CreateConvexKinematic((new Vector3(14, -6, 0), QuaternionEx.CreateFromAxisAngle(Vector3.Normalize(new Vector3(1, 0, 1)), 0)), Simulation.Shapes, new Cylinder(3f, .2f))); + + + cylinderShape = new Cylinder(1f, 3); + var cylinderShapeIndex = Simulation.Shapes.Add(cylinderShape); + var cylinderInertia = cylinderShape.ComputeInertia(1); + //const int rowCount = 15; + //for (int rowIndex = 0; rowIndex < rowCount; ++rowIndex) + //{ + // int columnCount = rowCount - rowIndex; + // for (int columnIndex = 0; columnIndex < columnCount; ++columnIndex) + // { + // Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3( + // (-columnCount * 0.5f + columnIndex) * cylinderShape.Radius * 2f, + // (rowIndex + 0.5f) * cylinderShape.Length - 9.5f, -10), + // cylinderInertia, + // new CollidableDescription(cylinderShapeIndex, 0.1f), + // new BodyActivityDescription(0.01f))); + // } + //} + + var box = new Box(1f, 3f, 2f); + var capsule = new Capsule(1f, 1f); + var sphere = new Sphere(1.5f); + var boxInertia = box.ComputeInertia(1); + var capsuleInertia = capsule.ComputeInertia(1); + var sphereInertia = sphere.ComputeInertia(1); + var boxIndex = Simulation.Shapes.Add(box); + var capsuleIndex = Simulation.Shapes.Add(capsule); + var sphereIndex = Simulation.Shapes.Add(sphere); + const int width = 2; + const int height = 1; + const int length = 2; + for (int i = 0; i < width; ++i) { - camera.Position = new Vector3(10, 0, 6); - camera.Pitch = 0; - camera.Yaw = 0; - - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, 0f, 0)), new SolveDescription(8, 1)); - - var cylinderShape = new Cylinder(1f, .2f); - var cylinder = BodyDescription.CreateDynamic(new Vector3(10f, 3, 0), cylinderShape.ComputeInertia(1), new(Simulation.Shapes.Add(cylinderShape), 1000f, 1000f, ContinuousDetection.Passive), 0.01f); - Simulation.Bodies.Add(cylinder); - Simulation.Bodies.Add(BodyDescription.CreateConvexKinematic((new Vector3(0, -6, 0), QuaternionEx.CreateFromAxisAngle(Vector3.Normalize(new Vector3(1, 0, 1)), MathHelper.PiOver4)), Simulation.Shapes, new Sphere(2))); - Simulation.Bodies.Add(BodyDescription.CreateConvexKinematic((new Vector3(7, -6, 0), QuaternionEx.CreateFromAxisAngle(Vector3.Normalize(new Vector3(1, 0, 1)), MathHelper.PiOver4)), Simulation.Shapes, new Capsule(0.5f, 1f))); - Simulation.Bodies.Add(BodyDescription.CreateConvexKinematic((new Vector3(21, -3, 0), QuaternionEx.CreateFromAxisAngle(Vector3.Normalize(new Vector3(1, 0, 1)), 0)), Simulation.Shapes, new Box(3f, 1f, 3f))); - Simulation.Bodies.Add(BodyDescription.CreateConvexKinematic((new Vector3(28, -6, 0), QuaternionEx.CreateFromAxisAngle(Vector3.Normalize(new Vector3(1, 0, 1)), 0)), Simulation.Shapes, - new Triangle(new Vector3(10f, 0, 10f), new Vector3(14f, 0, 10f), new Vector3(10f, 0, 14f)))); - Simulation.Bodies.Add(BodyDescription.CreateConvexKinematic((new Vector3(14, -6, 0), QuaternionEx.CreateFromAxisAngle(Vector3.Normalize(new Vector3(1, 0, 1)), 0)), Simulation.Shapes, new Cylinder(3f, .2f))); - - - cylinderShape = new Cylinder(1f, 3); - var cylinderShapeIndex = Simulation.Shapes.Add(cylinderShape); - var cylinderInertia = cylinderShape.ComputeInertia(1); - //const int rowCount = 15; - //for (int rowIndex = 0; rowIndex < rowCount; ++rowIndex) - //{ - // int columnCount = rowCount - rowIndex; - // for (int columnIndex = 0; columnIndex < columnCount; ++columnIndex) - // { - // Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3( - // (-columnCount * 0.5f + columnIndex) * cylinderShape.Radius * 2f, - // (rowIndex + 0.5f) * cylinderShape.Length - 9.5f, -10), - // cylinderInertia, - // new CollidableDescription(cylinderShapeIndex, 0.1f), - // new BodyActivityDescription(0.01f))); - // } - //} - - var box = new Box(1f, 3f, 2f); - var capsule = new Capsule(1f, 1f); - var sphere = new Sphere(1.5f); - var boxInertia = box.ComputeInertia(1); - var capsuleInertia = capsule.ComputeInertia(1); - var sphereInertia = sphere.ComputeInertia(1); - var boxIndex = Simulation.Shapes.Add(box); - var capsuleIndex = Simulation.Shapes.Add(capsule); - var sphereIndex = Simulation.Shapes.Add(sphere); - const int width = 2; - const int height = 1; - const int length = 2; - for (int i = 0; i < width; ++i) + for (int j = 0; j < height; ++j) { - for (int j = 0; j < height; ++j) + for (int k = 0; k < length; ++k) { - for (int k = 0; k < length; ++k) + var location = new Vector3(5, 3, 5) * new Vector3(i, j, k) + new Vector3(-width * 1.5f, 2.5f, -30 - length * 1.5f); + var bodyDescription = BodyDescription.CreateDynamic(location, default, default, -0.01f); + switch (j % 4) { - var location = new Vector3(5, 3, 5) * new Vector3(i, j, k) + new Vector3(-width * 1.5f, 2.5f, -30 - length * 1.5f); - var bodyDescription = BodyDescription.CreateDynamic(location, default, default, -0.01f); - switch (j % 4) - { - case 0: - // bodyDescription.Collidable.Shape = boxIndex; - // bodyDescription.LocalInertia = boxInertia; - // break; - case 1: - // bodyDescription.Collidable.Shape = capsuleIndex; - // bodyDescription.LocalInertia = capsuleInertia; - // break; - case 2: - // bodyDescription.Collidable.Shape = sphereIndex; - // bodyDescription.LocalInertia = sphereInertia; - // break; - case 3: - bodyDescription.Collidable.Shape = cylinderShapeIndex; - bodyDescription.LocalInertia = cylinderInertia; - break; - } - Simulation.Bodies.Add(bodyDescription); - + case 0: + // bodyDescription.Collidable.Shape = boxIndex; + // bodyDescription.LocalInertia = boxInertia; + // break; + case 1: + // bodyDescription.Collidable.Shape = capsuleIndex; + // bodyDescription.LocalInertia = capsuleInertia; + // break; + case 2: + // bodyDescription.Collidable.Shape = sphereIndex; + // bodyDescription.LocalInertia = sphereInertia; + // break; + case 3: + bodyDescription.Collidable.Shape = cylinderShapeIndex; + bodyDescription.LocalInertia = cylinderInertia; + break; } + Simulation.Bodies.Add(bodyDescription); + } } + } - const int planeWidth = 50; - const int planeHeight = 50; - var planeMesh = DemoMeshHelper.CreateDeformedPlane(planeWidth, planeHeight, - (int x, int y) => - { - var octave0 = (MathF.Sin((x + 5f) * 0.05f) + MathF.Sin((y + 11) * 0.05f)) * 3f; - var octave1 = (MathF.Sin((x + 17) * 0.15f) + MathF.Sin((y + 19) * 0.15f)) * 2f; - var octave2 = (MathF.Sin((x + 37) * 0.35f) + MathF.Sin((y + 93) * 0.35f)) * 1f; - var octave3 = (MathF.Sin((x + 53) * 0.65f) + MathF.Sin((y + 47) * 0.65f)) * 0.5f; - var octave4 = (MathF.Sin((x + 67) * 1.50f) + MathF.Sin((y + 13) * 1.5f)) * 0.25f; - return new Vector3(x, octave0 + octave1 + octave2 + octave3 + octave4, y); - }, new Vector3(4, 1, 4), BufferPool); - Simulation.Statics.Add(new StaticDescription(new Vector3(-100, -15, 100), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), Simulation.Shapes.Add(planeMesh))); - - //Simulation.Statics.Add(new StaticDescription(new Vector3(0, -10, 0), Quaternion.CreateFromAxisAngle(Vector3.Normalize(new Vector3(1, 0, 1)), 0), Simulation.Shapes.Add(new Cylinder(100, 1f)), 0.1f)); - - //{ - // CapsuleCylinderTester tester = default; - // CapsuleWide a = default; - // a.Broadcast(new Capsule(0.5f, 1)); - // CylinderWide b = default; - // b.Broadcast(new Cylinder(0.5f, 1)); - // var speculativeMargin = new Vector(2f); - // Vector3Wide.Broadcast(new Vector3(0, -0.4f, 0), out var offsetB); - // QuaternionWide.Broadcast(Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), MathHelper.PiOver2), out var orientationA); - // QuaternionWide.Broadcast(Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 0), out var orientationB); - // tester.Test(ref a, ref b, ref speculativeMargin, ref offsetB, ref orientationA, ref orientationB, Vector.Count, out var manifold); - //} - //{ - // CylinderWide a = default; - // a.Broadcast(new Cylinder(0.5f, 1f)); - // CylinderWide b = default; - // b.Broadcast(new Cylinder(0.5f, 1f)); - // var supportFinderA = new CylinderSupportFinder(); - // var supportFinderB = new CylinderSupportFinder(); - // Vector3Wide.Broadcast(new Vector3(-0.8f, 0.01f, 0.71f), out var localOffsetB); - // Matrix3x3Wide.Broadcast(Matrix3x3.CreateFromAxisAngle(new Vector3(1, 0, 0), 0.1f), out var localOrientationB); - // Vector3Wide.Normalize(localOffsetB, out var initialGuess); - - // GradientDescent.Refine(a, b, localOffsetB, localOrientationB, - // ref supportFinderA, ref supportFinderB, initialGuess, new Vector(-0.1f), new Vector(1e-4f), 1500, Vector.Zero, out var localNormal, out var depthBelowThreshold); - - // GJKDistanceTester gjk = default; - // QuaternionWide.Broadcast(Quaternion.Identity, out var localOrientationQuaternionA); - // QuaternionWide.CreateFromRotationMatrix(localOrientationB, out var localOrientationQuaternionB); - // gjk.Test(ref a, ref b, ref localOffsetB, ref localOrientationQuaternionA, ref localOrientationQuaternionB, out var intersected, out var distance, out var closestA, out var gjkNormal); - // //TimeGradientDescent(32); - // //TimeGradientDescent(1000000); - //} - //{ - // CylinderWide a = default; - // a.Broadcast(new Cylinder(0.5f, 1f)); - // CylinderWide b = default; - // b.Broadcast(new Cylinder(0.5f, 1f)); - // var supportFinderA = new CylinderSupportFinder(); - // var supportFinderB = new CylinderSupportFinder(); - // Vector3Wide.Broadcast(new Vector3(0.5f, 0.5f, 0.5f), out var localOffsetB); - // Vector3Wide.Broadcast(Vector3.Normalize(new Vector3(1, 0, 1)), out var localCastDirection); - // Matrix3x3Wide.Broadcast(Matrix3x3.CreateFromAxisAngle(new Vector3(1, 0, 0), 0), out var localOrientationB); - // MPR.Test(a, b, localOffsetB, localOrientationB, ref supportFinderA, ref supportFinderB, new Vector(1e-5f), Vector.Zero, out var intersecting, out var localNormal); - // for (int i = 0; i < 5; ++i) - // { - // MPR.LocalSurfaceCast(a, b, localOffsetB, localOrientationB, ref supportFinderA, ref supportFinderB, localNormal, new Vector(1e-3f), Vector.Zero, - // out var t, out localNormal); - // } - // Vector3Wide.Normalize(localNormal, out var test); - - // GJKDistanceTester gjk = default; - // QuaternionWide.Broadcast(Quaternion.Identity, out var localOrientationQuaternionA); - // QuaternionWide.CreateFromRotationMatrix(localOrientationB, out var localOrientationQuaternionB); - // gjk.Test(ref a, ref b, ref localOffsetB, ref localOrientationQuaternionA, ref localOrientationQuaternionB, out var intersected, out var distance, out var closestA, out var gjkNormal); - // //TimeMPRSurfaceCast(32); - // //TimeMPRSurfaceCast(1000000); - //} - //{ - // CylinderWide a = default; - // a.Broadcast(new Cylinder(0.5f, 1f)); - // CylinderWide b = default; - // b.Broadcast(new Cylinder(0.5f, 1f)); - // var supportFinderA = new CylinderSupportFinder(); - // var supportFinderB = new CylinderSupportFinder(); - // Vector3Wide.Broadcast(new Vector3(-0.335f, -0.0f, 1.207f), out var localOffsetB); - // Matrix3x3Wide.Broadcast(Matrix3x3.CreateFromAxisAngle(new Vector3(1, 0, 0), 10.0f), out var localOrientationB); - // MPR.Test(a, b, localOffsetB, localOrientationB, ref supportFinderA, ref supportFinderB, new Vector(1e-5f), Vector.Zero, out var intersecting, out var localNormal); - // Vector3Wide.Normalize(localNormal, out var test); - // GJKDistanceTester gjk = default; - // QuaternionWide.Broadcast(Quaternion.Identity, out var localOrientationQuaternionA); - // QuaternionWide.CreateFromRotationMatrix(localOrientationB, out var localOrientationQuaternionB); - // gjk.Test(ref a, ref b, ref localOffsetB, ref localOrientationQuaternionA, ref localOrientationQuaternionB, out var intersected, out var distance, out var closestA, out var gjkNormal); - // //TimeMPR(32); - // //TimeMPR(1000000); - //} + const int planeWidth = 50; + const int planeHeight = 50; + var planeMesh = DemoMeshHelper.CreateDeformedPlane(planeWidth, planeHeight, + (int x, int y) => + { + var octave0 = (MathF.Sin((x + 5f) * 0.05f) + MathF.Sin((y + 11) * 0.05f)) * 3f; + var octave1 = (MathF.Sin((x + 17) * 0.15f) + MathF.Sin((y + 19) * 0.15f)) * 2f; + var octave2 = (MathF.Sin((x + 37) * 0.35f) + MathF.Sin((y + 93) * 0.35f)) * 1f; + var octave3 = (MathF.Sin((x + 53) * 0.65f) + MathF.Sin((y + 47) * 0.65f)) * 0.5f; + var octave4 = (MathF.Sin((x + 67) * 1.50f) + MathF.Sin((y + 13) * 1.5f)) * 0.25f; + return new Vector3(x, octave0 + octave1 + octave2 + octave3 + octave4, y); + }, new Vector3(4, 1, 4), BufferPool); + Simulation.Statics.Add(new StaticDescription(new Vector3(-100, -15, 100), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), Simulation.Shapes.Add(planeMesh))); + + //Simulation.Statics.Add(new StaticDescription(new Vector3(0, -10, 0), Quaternion.CreateFromAxisAngle(Vector3.Normalize(new Vector3(1, 0, 1)), 0), Simulation.Shapes.Add(new Cylinder(100, 1f)), 0.1f)); + + //{ + // CapsuleCylinderTester tester = default; + // CapsuleWide a = default; + // a.Broadcast(new Capsule(0.5f, 1)); + // CylinderWide b = default; + // b.Broadcast(new Cylinder(0.5f, 1)); + // var speculativeMargin = new Vector(2f); + // Vector3Wide.Broadcast(new Vector3(0, -0.4f, 0), out var offsetB); + // QuaternionWide.Broadcast(Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), MathHelper.PiOver2), out var orientationA); + // QuaternionWide.Broadcast(Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 0), out var orientationB); + // tester.Test(ref a, ref b, ref speculativeMargin, ref offsetB, ref orientationA, ref orientationB, Vector.Count, out var manifold); + //} + //{ + // CylinderWide a = default; + // a.Broadcast(new Cylinder(0.5f, 1f)); + // CylinderWide b = default; + // b.Broadcast(new Cylinder(0.5f, 1f)); + // var supportFinderA = new CylinderSupportFinder(); + // var supportFinderB = new CylinderSupportFinder(); + // Vector3Wide.Broadcast(new Vector3(-0.8f, 0.01f, 0.71f), out var localOffsetB); + // Matrix3x3Wide.Broadcast(Matrix3x3.CreateFromAxisAngle(new Vector3(1, 0, 0), 0.1f), out var localOrientationB); + // Vector3Wide.Normalize(localOffsetB, out var initialGuess); + + // GradientDescent.Refine(a, b, localOffsetB, localOrientationB, + // ref supportFinderA, ref supportFinderB, initialGuess, new Vector(-0.1f), new Vector(1e-4f), 1500, Vector.Zero, out var localNormal, out var depthBelowThreshold); + + // GJKDistanceTester gjk = default; + // QuaternionWide.Broadcast(Quaternion.Identity, out var localOrientationQuaternionA); + // QuaternionWide.CreateFromRotationMatrix(localOrientationB, out var localOrientationQuaternionB); + // gjk.Test(ref a, ref b, ref localOffsetB, ref localOrientationQuaternionA, ref localOrientationQuaternionB, out var intersected, out var distance, out var closestA, out var gjkNormal); + // //TimeGradientDescent(32); + // //TimeGradientDescent(1000000); + //} + //{ + // CylinderWide a = default; + // a.Broadcast(new Cylinder(0.5f, 1f)); + // CylinderWide b = default; + // b.Broadcast(new Cylinder(0.5f, 1f)); + // var supportFinderA = new CylinderSupportFinder(); + // var supportFinderB = new CylinderSupportFinder(); + // Vector3Wide.Broadcast(new Vector3(0.5f, 0.5f, 0.5f), out var localOffsetB); + // Vector3Wide.Broadcast(Vector3.Normalize(new Vector3(1, 0, 1)), out var localCastDirection); + // Matrix3x3Wide.Broadcast(Matrix3x3.CreateFromAxisAngle(new Vector3(1, 0, 0), 0), out var localOrientationB); + // MPR.Test(a, b, localOffsetB, localOrientationB, ref supportFinderA, ref supportFinderB, new Vector(1e-5f), Vector.Zero, out var intersecting, out var localNormal); + // for (int i = 0; i < 5; ++i) + // { + // MPR.LocalSurfaceCast(a, b, localOffsetB, localOrientationB, ref supportFinderA, ref supportFinderB, localNormal, new Vector(1e-3f), Vector.Zero, + // out var t, out localNormal); + // } + // Vector3Wide.Normalize(localNormal, out var test); + + // GJKDistanceTester gjk = default; + // QuaternionWide.Broadcast(Quaternion.Identity, out var localOrientationQuaternionA); + // QuaternionWide.CreateFromRotationMatrix(localOrientationB, out var localOrientationQuaternionB); + // gjk.Test(ref a, ref b, ref localOffsetB, ref localOrientationQuaternionA, ref localOrientationQuaternionB, out var intersected, out var distance, out var closestA, out var gjkNormal); + // //TimeMPRSurfaceCast(32); + // //TimeMPRSurfaceCast(1000000); + //} + //{ + // CylinderWide a = default; + // a.Broadcast(new Cylinder(0.5f, 1f)); + // CylinderWide b = default; + // b.Broadcast(new Cylinder(0.5f, 1f)); + // var supportFinderA = new CylinderSupportFinder(); + // var supportFinderB = new CylinderSupportFinder(); + // Vector3Wide.Broadcast(new Vector3(-0.335f, -0.0f, 1.207f), out var localOffsetB); + // Matrix3x3Wide.Broadcast(Matrix3x3.CreateFromAxisAngle(new Vector3(1, 0, 0), 10.0f), out var localOrientationB); + // MPR.Test(a, b, localOffsetB, localOrientationB, ref supportFinderA, ref supportFinderB, new Vector(1e-5f), Vector.Zero, out var intersecting, out var localNormal); + // Vector3Wide.Normalize(localNormal, out var test); + // GJKDistanceTester gjk = default; + // QuaternionWide.Broadcast(Quaternion.Identity, out var localOrientationQuaternionA); + // QuaternionWide.CreateFromRotationMatrix(localOrientationB, out var localOrientationQuaternionB); + // gjk.Test(ref a, ref b, ref localOffsetB, ref localOrientationQuaternionA, ref localOrientationQuaternionB, out var intersected, out var distance, out var closestA, out var gjkNormal); + // //TimeMPR(32); + // //TimeMPR(1000000); + //} - } + } - } } diff --git a/Demos/SpecializedTests/DeterminismTest.cs b/Demos/SpecializedTests/DeterminismTest.cs index f4325f0b5..7064a12bc 100644 --- a/Demos/SpecializedTests/DeterminismTest.cs +++ b/Demos/SpecializedTests/DeterminismTest.cs @@ -3,72 +3,71 @@ using System.Collections.Generic; using DemoContentLoader; -namespace Demos.SpecializedTests +namespace Demos.SpecializedTests; + +public static class DeterminismTest where T : Demo, new() { - public static class DeterminismTest where T : Demo, new() + static Dictionary ExecuteSimulation(ContentArchive content, int frameCount) { - static Dictionary ExecuteSimulation(ContentArchive content, int frameCount) + var demo = new T(); + demo.Initialize(content, new DemoRenderer.Camera(1, 1, 1, 1)); + Console.Write("Completed frames: "); + for (int i = 0; i < frameCount; ++i) { - var demo = new T(); - demo.Initialize(content, new DemoRenderer.Camera(1, 1, 1, 1)); - Console.Write("Completed frames: "); - for (int i = 0; i < frameCount; ++i) - { - demo.Update(null, null, null, Demo.TimestepDuration); - //InvasiveHashDiagnostics.Instance.MoveToNextHashFrame(); - if ((i + 1) % 32 == 0) - Console.Write($"{i + 1}, "); - } - var motionStates = new Dictionary(); - for (int setIndex = 0; setIndex < demo.Simulation.Bodies.Sets.Length; ++setIndex) + demo.Update(null, null, null, Demo.TimestepDuration); + //InvasiveHashDiagnostics.Instance.MoveToNextHashFrame(); + if ((i + 1) % 32 == 0) + Console.Write($"{i + 1}, "); + } + var motionStates = new Dictionary(); + for (int setIndex = 0; setIndex < demo.Simulation.Bodies.Sets.Length; ++setIndex) + { + ref var set = ref demo.Simulation.Bodies.Sets[setIndex]; + if (set.Allocated) { - ref var set = ref demo.Simulation.Bodies.Sets[setIndex]; - if (set.Allocated) + for (int bodyIndex = 0; bodyIndex < set.Count; ++bodyIndex) { - for (int bodyIndex = 0; bodyIndex < set.Count; ++bodyIndex) - { - motionStates.Add(set.IndexToHandle[bodyIndex].Value, set.DynamicsState[bodyIndex].Motion); - } + motionStates.Add(set.IndexToHandle[bodyIndex].Value, set.DynamicsState[bodyIndex].Motion); } } - demo.Dispose(); - Console.WriteLine(); - return motionStates; } + demo.Dispose(); + Console.WriteLine(); + return motionStates; + } - public static void Test(ContentArchive archive, int runCount, int frameCount) + public static void Test(ContentArchive archive, int runCount, int frameCount) + { + //InvasiveHashDiagnostics.Initialize(1 + runCount, frameCount); + //var hashInstance = InvasiveHashDiagnostics.Instance; + var initialStates = ExecuteSimulation(archive, frameCount); + //hashInstance.MoveToNextRun(); + Console.WriteLine($"Completed initial run."); + for (int i = 0; i < runCount; ++i) { - //InvasiveHashDiagnostics.Initialize(1 + runCount, frameCount); - //var hashInstance = InvasiveHashDiagnostics.Instance; - var initialStates = ExecuteSimulation(archive, frameCount); + var states = ExecuteSimulation(archive, frameCount); //hashInstance.MoveToNextRun(); - Console.WriteLine($"Completed initial run."); - for (int i = 0; i < runCount; ++i) + Console.Write($"Completed iteration {i}; checking... "); + if (states.Count != initialStates.Count) + Console.WriteLine("DETERMINISM FAILURE: Differing body count."); + foreach (var state in states) { - var states = ExecuteSimulation(archive, frameCount); - //hashInstance.MoveToNextRun(); - Console.Write($"Completed iteration {i}; checking... "); - if (states.Count != initialStates.Count) - Console.WriteLine("DETERMINISM FAILURE: Differing body count."); - foreach (var state in states) + if (!initialStates.TryGetValue(state.Key, out var initialState)) + Console.WriteLine($"FAILURE: Body {state.Key} does not exist in first run results."); + else { - if (!initialStates.TryGetValue(state.Key, out var initialState)) - Console.WriteLine($"FAILURE: Body {state.Key} does not exist in first run results."); - else - { - if (state.Value.Pose.Position != initialState.Pose.Position) - Console.WriteLine($"FAILURE: Position, current: {state.Value.Pose.Position}, original: {initialState.Pose.Position}"); - if (state.Value.Pose.Orientation != initialState.Pose.Orientation) - Console.WriteLine($"FAILURE: Orientation, current: {state.Value.Pose.Orientation}, original: {initialState.Pose.Orientation}"); - if (state.Value.Velocity.Linear != initialState.Velocity.Linear) - Console.WriteLine($"FAILURE: Linear velocity, current: {state.Value.Velocity.Linear}, original: {initialState.Velocity.Linear}"); - if (state.Value.Velocity.Angular != initialState.Velocity.Angular) - Console.WriteLine($"FAILURE: Angular velocity, current: {state.Value.Velocity.Angular}, original: {initialState.Velocity.Angular}"); - } + if (state.Value.Pose.Position != initialState.Pose.Position) + Console.WriteLine($"FAILURE: Position, current: {state.Value.Pose.Position}, original: {initialState.Pose.Position}"); + if (state.Value.Pose.Orientation != initialState.Pose.Orientation) + Console.WriteLine($"FAILURE: Orientation, current: {state.Value.Pose.Orientation}, original: {initialState.Pose.Orientation}"); + if (state.Value.Velocity.Linear != initialState.Velocity.Linear) + Console.WriteLine($"FAILURE: Linear velocity, current: {state.Value.Velocity.Linear}, original: {initialState.Velocity.Linear}"); + if (state.Value.Velocity.Angular != initialState.Velocity.Angular) + Console.WriteLine($"FAILURE: Angular velocity, current: {state.Value.Velocity.Angular}, original: {initialState.Velocity.Angular}"); } - Console.WriteLine($"Test {i} complete."); } - Console.WriteLine($"All runs complete."); + Console.WriteLine($"Test {i} complete."); } + Console.WriteLine($"All runs complete."); } } diff --git a/Demos/SpecializedTests/FountainStressTestDemo.cs b/Demos/SpecializedTests/FountainStressTestDemo.cs index 942d7b47f..46a4a5a41 100644 --- a/Demos/SpecializedTests/FountainStressTestDemo.cs +++ b/Demos/SpecializedTests/FountainStressTestDemo.cs @@ -11,404 +11,403 @@ using DemoContentLoader; using BepuPhysics.Constraints; -namespace Demos.SpecializedTests +namespace Demos.SpecializedTests; + +public class FountainStressTestDemo : Demo { - public class FountainStressTestDemo : Demo + QuickQueue removedStatics; + QuickQueue dynamicHandles; + Random random; + public override void Initialize(ContentArchive content, Camera camera) { - QuickQueue removedStatics; - QuickQueue dynamicHandles; - Random random; - public override void Initialize(ContentArchive content, Camera camera) + camera.Position = new Vector3(-15f, 20, -15f); + camera.Yaw = MathHelper.Pi * 3f / 4; + camera.Pitch = MathHelper.Pi * 0.1f; + //Using minimum sized allocations forces as many resizes as possible. + //Note the low solverFallbackBatchThreshold- we want the fallback batches to get tested thoroughly. + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(new[] { 2, 1, 1 }, fallbackBatchThreshold: 2), initialAllocationSizes: + new SimulationAllocationSizes { - camera.Position = new Vector3(-15f, 20, -15f); - camera.Yaw = MathHelper.Pi * 3f / 4; - camera.Pitch = MathHelper.Pi * 0.1f; - //Using minimum sized allocations forces as many resizes as possible. - //Note the low solverFallbackBatchThreshold- we want the fallback batches to get tested thoroughly. - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(new[] { 2, 1, 1 }, fallbackBatchThreshold: 2), initialAllocationSizes: - new SimulationAllocationSizes - { - Bodies = 1, - ConstraintCountPerBodyEstimate = 1, - Constraints = 1, - ConstraintsPerTypeBatch = 1, - Islands = 1, - ShapesPerType = 1, - Statics = 1 - }); + Bodies = 1, + ConstraintCountPerBodyEstimate = 1, + Constraints = 1, + ConstraintsPerTypeBatch = 1, + Islands = 1, + ShapesPerType = 1, + Statics = 1 + }); - Simulation.Deterministic = true; + Simulation.Deterministic = true; - const int planeWidth = 8; - const int planeHeight = 8; - var staticShape = DemoMeshHelper.CreateDeformedPlane(planeWidth, planeHeight, - (int x, int y) => - { - Vector2 offsetFromCenter = new Vector2(x - planeWidth / 2, y - planeHeight / 2); - return new Vector3(offsetFromCenter.X, MathF.Cos(x / 4f) * MathF.Sin(y / 4f) - 0.2f * offsetFromCenter.LengthSquared(), offsetFromCenter.Y); - }, new Vector3(2, 1, 2), BufferPool); - var staticShapeIndex = Simulation.Shapes.Add(staticShape); - const int staticGridWidthInInstances = 128; - const float staticSpacing = 8; - for (int i = 0; i < staticGridWidthInInstances; ++i) + const int planeWidth = 8; + const int planeHeight = 8; + var staticShape = DemoMeshHelper.CreateDeformedPlane(planeWidth, planeHeight, + (int x, int y) => { - for (int j = 0; j < staticGridWidthInInstances; ++j) - { - var staticDescription = new StaticDescription(new Vector3( - -staticGridWidthInInstances * staticSpacing * 0.5f + i * staticSpacing, - -4 + 4 * (float)Math.Cos(i * 0.3) + 4 * (float)Math.Cos(j * 0.3), - -staticGridWidthInInstances * staticSpacing * 0.5f + j * staticSpacing), - staticShapeIndex); - Simulation.Statics.Add(staticDescription); - } - } - - //A bunch of kinematic balls do acrobatics as an extra stressor. - var kinematicShape = new Sphere(8); - var kinematicShapeIndex = Simulation.Shapes.Add(kinematicShape); - var kinematicCount = 64; - var anglePerKinematic = MathHelper.TwoPi / kinematicCount; - var startingRadius = 256; - kinematicHandles = new BodyHandle[kinematicCount]; - for (int i = 0; i < kinematicCount; ++i) - { - var angle = anglePerKinematic * i; - var description = BodyDescription.CreateKinematic(new Vector3( - startingRadius * (float)Math.Cos(angle), - 0, - startingRadius * (float)Math.Sin(angle)), - kinematicShapeIndex, - new BodyActivityDescription(0, 4)); - kinematicHandles[i] = Simulation.Bodies.Add(description); - } - - dynamicHandles = new QuickQueue(65536, BufferPool); - removedStatics = new QuickQueue(512, BufferPool); - random = new Random(5); - } - - double time; - double t; - BodyHandle[] kinematicHandles; - - void AddConvexShape(in TConvex convex, out TypedIndex shapeIndex, out BodyInertia inertia) where TConvex : unmanaged, IConvexShape + Vector2 offsetFromCenter = new Vector2(x - planeWidth / 2, y - planeHeight / 2); + return new Vector3(offsetFromCenter.X, MathF.Cos(x / 4f) * MathF.Sin(y / 4f) - 0.2f * offsetFromCenter.LengthSquared(), offsetFromCenter.Y); + }, new Vector3(2, 1, 2), BufferPool); + var staticShapeIndex = Simulation.Shapes.Add(staticShape); + const int staticGridWidthInInstances = 128; + const float staticSpacing = 8; + for (int i = 0; i < staticGridWidthInInstances; ++i) { - shapeIndex = Simulation.Shapes.Add(convex); - inertia = convex.ComputeInertia(1); - } - - ConvexHull CreateRandomHull() - { - const int pointCount = 16; - var points = new QuickList(pointCount, BufferPool); - //Create an initial tetrahedron to guarantee our random shape isn't degenerate. - points.AllocateUnsafely() = new Vector3(0.5f, 0.25f, 0.75f); - points.AllocateUnsafely() = points[0] + new Vector3(0.1f, 0, 0); - points.AllocateUnsafely() = points[0] + new Vector3(0, 0.1f, 0); - points.AllocateUnsafely() = points[0] + new Vector3(0, 0, 0.1f); - for (int i = 4; i < pointCount; ++i) + for (int j = 0; j < staticGridWidthInInstances; ++j) { - points.AllocateUnsafely() = new Vector3(1 * random.NextSingle(), 0.5f * random.NextSingle(), 1.5f * random.NextSingle()); + var staticDescription = new StaticDescription(new Vector3( + -staticGridWidthInInstances * staticSpacing * 0.5f + i * staticSpacing, + -4 + 4 * (float)Math.Cos(i * 0.3) + 4 * (float)Math.Cos(j * 0.3), + -staticGridWidthInInstances * staticSpacing * 0.5f + j * staticSpacing), + staticShapeIndex); + Simulation.Statics.Add(staticDescription); } - var hull = new ConvexHull(points.Span.Slice(points.Count), BufferPool, out _); - points.Dispose(BufferPool); - return hull; } - void CreateRandomCompound(out Buffer children, out BodyInertia inertia) + //A bunch of kinematic balls do acrobatics as an extra stressor. + var kinematicShape = new Sphere(8); + var kinematicShapeIndex = Simulation.Shapes.Add(kinematicShape); + var kinematicCount = 64; + var anglePerKinematic = MathHelper.TwoPi / kinematicCount; + var startingRadius = 256; + kinematicHandles = new BodyHandle[kinematicCount]; + for (int i = 0; i < kinematicCount; ++i) { - using (var compoundBuilder = new CompoundBuilder(BufferPool, Simulation.Shapes, 6)) - { - var childCount = random.Next(2, 6); - for (int i = 0; i < childCount; ++i) - { - TypedIndex shapeIndex; - BodyInertia childInertia; - switch (random.Next(0, 5)) - { - default: - AddConvexShape(new Sphere(0.35f + 0.35f * random.NextSingle()), out shapeIndex, out childInertia); - break; - case 1: - AddConvexShape(new Capsule( - 0.35f + 0.35f * random.NextSingle(), - 0.35f + 0.35f * random.NextSingle()), out shapeIndex, out childInertia); - break; - case 2: - AddConvexShape(new Box( - 0.35f + 0.35f * random.NextSingle(), - 0.35f + 0.35f * random.NextSingle(), - 0.35f + 0.35f * random.NextSingle()), out shapeIndex, out childInertia); - break; - case 3: - AddConvexShape(new Cylinder(0.1f + random.NextSingle(), 0.2f + random.NextSingle()), out shapeIndex, out childInertia); - break; - case 4: - AddConvexShape(CreateRandomHull(), out shapeIndex, out childInertia); - break; - } - RigidPose localPose; - localPose.Position = new Vector3(2, 2, 2) * (0.5f * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) - Vector3.One); - float orientationLengthSquared; - do - { - localPose.Orientation = new Quaternion(random.NextSingle(), random.NextSingle(), random.NextSingle(), random.NextSingle()); - } - while ((orientationLengthSquared = localPose.Orientation.LengthSquared()) < 1e-9f); - QuaternionEx.Scale(localPose.Orientation, 1f / MathF.Sqrt(orientationLengthSquared), out localPose.Orientation); - compoundBuilder.Add(shapeIndex, localPose, childInertia.InverseInertiaTensor, 1); - } - compoundBuilder.BuildDynamicCompound(out children, out inertia, out var center); - } + var angle = anglePerKinematic * i; + var description = BodyDescription.CreateKinematic(new Vector3( + startingRadius * (float)Math.Cos(angle), + 0, + startingRadius * (float)Math.Sin(angle)), + kinematicShapeIndex, + new BodyActivityDescription(0, 4)); + kinematicHandles[i] = Simulation.Bodies.Add(description); } - void CreateRandomMesh(out Mesh mesh, out BodyInertia inertia) - { - //We'll use a convex hull algorithm to generate the triangles for the mesh, rather than just spewing random triangle soups. - var pointCount = random.Next(5, 16); - BufferPool.Take(pointCount, out Buffer points); - //Create an initial tetrahedron to guarantee our random shape isn't degenerate. - points[0] = new Vector3(1); - points[1] = new Vector3(1) + new Vector3(0.1f, 0, 0); - points[2] = new Vector3(1) + new Vector3(0, 0.1f, 0); - points[3] = new Vector3(1) + new Vector3(0, 0, 0.1f); + dynamicHandles = new QuickQueue(65536, BufferPool); + removedStatics = new QuickQueue(512, BufferPool); + random = new Random(5); + } - for (int i = 4; i < pointCount; ++i) - { - points[i] = 2f * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); - } - ConvexHullHelper.CreateShape(points, BufferPool, out _, out var convexHull); - BufferPool.Return(ref points); - ConvexHull.ConvexHullTriangleSource triangleSource = new(convexHull); - QuickList triangles = new(16, BufferPool); - while (triangleSource.GetNextTriangle(out var a, out var b, out var c)) - { - triangles.Allocate(BufferPool) = new Triangle(a, b, c); - } - convexHull.Dispose(BufferPool); + double time; + double t; + BodyHandle[] kinematicHandles; + + void AddConvexShape(in TConvex convex, out TypedIndex shapeIndex, out BodyInertia inertia) where TConvex : unmanaged, IConvexShape + { + shapeIndex = Simulation.Shapes.Add(convex); + inertia = convex.ComputeInertia(1); + } - mesh = new Mesh(triangles, new Vector3(1), BufferPool); - inertia = mesh.ComputeClosedInertia(1); + ConvexHull CreateRandomHull() + { + const int pointCount = 16; + var points = new QuickList(pointCount, BufferPool); + //Create an initial tetrahedron to guarantee our random shape isn't degenerate. + points.AllocateUnsafely() = new Vector3(0.5f, 0.25f, 0.75f); + points.AllocateUnsafely() = points[0] + new Vector3(0.1f, 0, 0); + points.AllocateUnsafely() = points[0] + new Vector3(0, 0.1f, 0); + points.AllocateUnsafely() = points[0] + new Vector3(0, 0, 0.1f); + for (int i = 4; i < pointCount; ++i) + { + points.AllocateUnsafely() = new Vector3(1 * random.NextSingle(), 0.5f * random.NextSingle(), 1.5f * random.NextSingle()); } + var hull = new ConvexHull(points.Span.Slice(points.Count), BufferPool, out _); + points.Dispose(BufferPool); + return hull; + } - public void CreateBodyDescription(Random random, in RigidPose pose, in BodyVelocity velocity, out BodyDescription description) + void CreateRandomCompound(out Buffer children, out BodyInertia inertia) + { + using (var compoundBuilder = new CompoundBuilder(BufferPool, Simulation.Shapes, 6)) { - //For the sake of the stress test, every single body has its own shape that gets removed when the body is removed. - TypedIndex shapeIndex; - BodyInertia inertia; - if (random.NextDouble() < 0.005) + var childCount = random.Next(2, 6); + for (int i = 0; i < childCount; ++i) { - //Occasionally request a shapeless body. - shapeIndex = default; - inertia = new BodyInertia { InverseMass = 1f, InverseInertiaTensor = new Symmetric3x3 { XX = 1, YY = 1, ZZ = 1 } }; - } - else - { - switch (random.Next(0, 8)) + TypedIndex shapeIndex; + BodyInertia childInertia; + switch (random.Next(0, 5)) { default: - { - AddConvexShape(new Sphere(0.35f + 0.35f * random.NextSingle()), out shapeIndex, out inertia); - } + AddConvexShape(new Sphere(0.35f + 0.35f * random.NextSingle()), out shapeIndex, out childInertia); break; case 1: - { - AddConvexShape(new Capsule( - 0.35f + 0.35f * random.NextSingle(), - 0.35f + 0.35f * random.NextSingle()), out shapeIndex, out inertia); - } + AddConvexShape(new Capsule( + 0.35f + 0.35f * random.NextSingle(), + 0.35f + 0.35f * random.NextSingle()), out shapeIndex, out childInertia); break; case 2: - { - AddConvexShape(new Box( - 0.35f + 0.6f * random.NextSingle(), - 0.35f + 0.6f * random.NextSingle(), - 0.35f + 0.6f * random.NextSingle()), out shapeIndex, out inertia); - } + AddConvexShape(new Box( + 0.35f + 0.35f * random.NextSingle(), + 0.35f + 0.35f * random.NextSingle(), + 0.35f + 0.35f * random.NextSingle()), out shapeIndex, out childInertia); break; case 3: - { - AddConvexShape(new Cylinder(0.1f + 0.5f * random.NextSingle(), 0.2f + random.NextSingle()), out shapeIndex, out inertia); - } + AddConvexShape(new Cylinder(0.1f + random.NextSingle(), 0.2f + random.NextSingle()), out shapeIndex, out childInertia); break; case 4: - { - AddConvexShape(CreateRandomHull(), out shapeIndex, out inertia); - } - break; - case 5: - { - CreateRandomCompound(out var children, out inertia); - shapeIndex = Simulation.Shapes.Add(new Compound(children)); - } - break; - case 6: - { - CreateRandomCompound(out var children, out inertia); - shapeIndex = Simulation.Shapes.Add(new BigCompound(children, Simulation.Shapes, BufferPool)); - } - break; - case 7: - { - //As usual: avoid dynamic meshes. They're slow and triangles are infinitely thin, so behavior probably won't be what you want. - //But dynamic meshes do exist, and so this demo shall test them. - CreateRandomMesh(out var mesh, out inertia); - shapeIndex = Simulation.Shapes.Add(mesh); - } + AddConvexShape(CreateRandomHull(), out shapeIndex, out childInertia); break; } + RigidPose localPose; + localPose.Position = new Vector3(2, 2, 2) * (0.5f * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) - Vector3.One); + float orientationLengthSquared; + do + { + localPose.Orientation = new Quaternion(random.NextSingle(), random.NextSingle(), random.NextSingle(), random.NextSingle()); + } + while ((orientationLengthSquared = localPose.Orientation.LengthSquared()) < 1e-9f); + QuaternionEx.Scale(localPose.Orientation, 1f / MathF.Sqrt(orientationLengthSquared), out localPose.Orientation); + compoundBuilder.Add(shapeIndex, localPose, childInertia.InverseInertiaTensor, 1); } - - description = BodyDescription.CreateDynamic(pose, velocity, inertia, shapeIndex, 0.1f); - switch (random.Next(3)) - { - case 0: description.Collidable = new CollidableDescription(shapeIndex, 0.2f); break; - case 1: description.Collidable = new CollidableDescription(shapeIndex); break; - case 2: description.Collidable = new CollidableDescription(shapeIndex, 0.2f, ContinuousDetection.Continuous(1e-3f, 1e-3f)); break; - } + compoundBuilder.BuildDynamicCompound(out children, out inertia, out var center); } + } - public override void Update(Window window, Camera camera, Input input, float dt) + void CreateRandomMesh(out Mesh mesh, out BodyInertia inertia) + { + //We'll use a convex hull algorithm to generate the triangles for the mesh, rather than just spewing random triangle soups. + var pointCount = random.Next(5, 16); + BufferPool.Take(pointCount, out Buffer points); + //Create an initial tetrahedron to guarantee our random shape isn't degenerate. + points[0] = new Vector3(1); + points[1] = new Vector3(1) + new Vector3(0.1f, 0, 0); + points[2] = new Vector3(1) + new Vector3(0, 0.1f, 0); + points[3] = new Vector3(1) + new Vector3(0, 0, 0.1f); + + for (int i = 4; i < pointCount; ++i) + { + points[i] = 2f * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); + } + ConvexHullHelper.CreateShape(points, BufferPool, out _, out var convexHull); + BufferPool.Return(ref points); + ConvexHull.ConvexHullTriangleSource triangleSource = new(convexHull); + QuickList triangles = new(16, BufferPool); + while (triangleSource.GetNextTriangle(out var a, out var b, out var c)) { - var timestepDuration = 1f / 60f; - time += timestepDuration; + triangles.Allocate(BufferPool) = new Triangle(a, b, c); + } + convexHull.Dispose(BufferPool); - //Occasionally, the animation stops completely. The resulting velocities will be zero, so the kinematics will have a chance to rest (testing kinematic rest states). - var dip = 0.1; - var progressionMultiplier = 0.5 - dip + (1 + dip) * 0.5 * Math.Cos(time * 0.25); - if (progressionMultiplier < 0) - progressionMultiplier = 0; - t += timestepDuration * progressionMultiplier; + mesh = new Mesh(triangles, new Vector3(1), BufferPool); + inertia = mesh.ComputeClosedInertia(1); + } - var baseAngle = (float)(t * 0.015); - var anglePerKinematic = MathHelper.TwoPi / kinematicHandles.Length; - var maxDisplacement = 50 * timestepDuration; - var inverseDt = 1f / timestepDuration; - for (int i = 0; i < kinematicHandles.Length; ++i) + public void CreateBodyDescription(Random random, in RigidPose pose, in BodyVelocity velocity, out BodyDescription description) + { + //For the sake of the stress test, every single body has its own shape that gets removed when the body is removed. + TypedIndex shapeIndex; + BodyInertia inertia; + if (random.NextDouble() < 0.005) + { + //Occasionally request a shapeless body. + shapeIndex = default; + inertia = new BodyInertia { InverseMass = 1f, InverseInertiaTensor = new Symmetric3x3 { XX = 1, YY = 1, ZZ = 1 } }; + } + else + { + switch (random.Next(0, 8)) { - ref var bodyLocation = ref Simulation.Bodies.HandleToLocation[kinematicHandles[i].Value]; - - ref var set = ref Simulation.Bodies.Sets[bodyLocation.SetIndex]; - var angle = anglePerKinematic * i; - var positionAngle = baseAngle + angle; - var radius = 128 + 32 * (float)Math.Cos(3 * (angle + t * (1f / 3f))) + 32 * (float)Math.Cos(t * (1f / 3f)); - var targetLocation = new Vector3( - radius * (float)Math.Cos(positionAngle), - 16 + 16 * (float)Math.Cos(4 * (angle + t * 0.5)), - radius * (float)Math.Sin(positionAngle)); - - var correction = targetLocation - set.DynamicsState[bodyLocation.Index].Motion.Pose.Position; - var distance = correction.Length(); - if (distance > 1e-4) - { - if (bodyLocation.SetIndex > 0) + default: { - //We're requesting a nonzero velocity, so it must be active. - Simulation.Awakener.AwakenSet(bodyLocation.SetIndex); + AddConvexShape(new Sphere(0.35f + 0.35f * random.NextSingle()), out shapeIndex, out inertia); } - if (distance > maxDisplacement) + break; + case 1: { - correction *= maxDisplacement / distance; + AddConvexShape(new Capsule( + 0.35f + 0.35f * random.NextSingle(), + 0.35f + 0.35f * random.NextSingle()), out shapeIndex, out inertia); } - Debug.Assert(bodyLocation.SetIndex == 0); - Simulation.Bodies.ActiveSet.DynamicsState[bodyLocation.Index].Motion.Velocity.Linear = correction * inverseDt; - } - else - { - if (bodyLocation.SetIndex == 0) + break; + case 2: { - Simulation.Bodies.ActiveSet.DynamicsState[bodyLocation.Index].Motion.Velocity.Linear = new Vector3(); + AddConvexShape(new Box( + 0.35f + 0.6f * random.NextSingle(), + 0.35f + 0.6f * random.NextSingle(), + 0.35f + 0.6f * random.NextSingle()), out shapeIndex, out inertia); } - } + break; + case 3: + { + AddConvexShape(new Cylinder(0.1f + 0.5f * random.NextSingle(), 0.2f + random.NextSingle()), out shapeIndex, out inertia); + } + break; + case 4: + { + AddConvexShape(CreateRandomHull(), out shapeIndex, out inertia); + } + break; + case 5: + { + CreateRandomCompound(out var children, out inertia); + shapeIndex = Simulation.Shapes.Add(new Compound(children)); + } + break; + case 6: + { + CreateRandomCompound(out var children, out inertia); + shapeIndex = Simulation.Shapes.Add(new BigCompound(children, Simulation.Shapes, BufferPool)); + } + break; + case 7: + { + //As usual: avoid dynamic meshes. They're slow and triangles are infinitely thin, so behavior probably won't be what you want. + //But dynamic meshes do exist, and so this demo shall test them. + CreateRandomMesh(out var mesh, out inertia); + shapeIndex = Simulation.Shapes.Add(mesh); + } + break; } + } - //Remove some statics from the simulation. - var missingStaticsAsymptote = 512; - var staticRemovalsPerFrame = 8; - for (int i = 0; i < staticRemovalsPerFrame; ++i) - { - var indexToRemove = random.Next(Simulation.Statics.Count); - Simulation.Statics.GetDescription(Simulation.Statics.IndexToHandle[indexToRemove], out var staticDescription); - Simulation.Statics.RemoveAt(indexToRemove); - removedStatics.Enqueue(staticDescription, BufferPool); - } + description = BodyDescription.CreateDynamic(pose, velocity, inertia, shapeIndex, 0.1f); + switch (random.Next(3)) + { + case 0: description.Collidable = new CollidableDescription(shapeIndex, 0.2f); break; + case 1: description.Collidable = new CollidableDescription(shapeIndex); break; + case 2: description.Collidable = new CollidableDescription(shapeIndex, 0.2f, ContinuousDetection.Continuous(1e-3f, 1e-3f)); break; + } + } - var staticApplyDescriptionsPerFrame = 8; - for (int i = 0; i < staticApplyDescriptionsPerFrame; ++i) - { - var indexToReapply = random.Next(Simulation.Statics.Count); - var handleToReapply = Simulation.Statics.IndexToHandle[indexToReapply]; - Simulation.Statics.GetDescription(handleToReapply, out var staticDescription); - //Statics don't have as much in the way of transitions. They can't be shapeless, and going from one shape to another doesn't anything that a pose change doesn't. For now, we'll just test the application of descriptions with different poses. - var mutatedDescription = staticDescription; - mutatedDescription.Pose.Position.Y += 50; - QuaternionEx.Concatenate(mutatedDescription.Pose.Orientation, QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), random.NextSingle() * MathF.PI), out mutatedDescription.Pose.Orientation); - Simulation.Statics.ApplyDescription(handleToReapply, mutatedDescription); - Simulation.Statics.ApplyDescription(handleToReapply, staticDescription); - } + public override void Update(Window window, Camera camera, Input input, float dt) + { + var timestepDuration = 1f / 60f; + time += timestepDuration; - //Add some of the missing static bodies back into the simulation. - var staticAddCount = removedStatics.Count * (staticRemovalsPerFrame / (float)missingStaticsAsymptote); - for (int i = 0; i < staticAddCount; ++i) - { - Debug.Assert(removedStatics.Count > 0); - var staticDescription = removedStatics.Dequeue(); - Simulation.Statics.Add(staticDescription); - } + //Occasionally, the animation stops completely. The resulting velocities will be zero, so the kinematics will have a chance to rest (testing kinematic rest states). + var dip = 0.1; + var progressionMultiplier = 0.5 - dip + (1 + dip) * 0.5 * Math.Cos(time * 0.25); + if (progressionMultiplier < 0) + progressionMultiplier = 0; + t += timestepDuration * progressionMultiplier; - //Spray some shapes! - int newShapeCount = 8; - var spawnPose = new RigidPose(new Vector3(0, 10, 0)); - for (int i = 0; i < newShapeCount; ++i) - { - CreateBodyDescription(random, spawnPose, new Vector3(-30 + 60 * random.NextSingle(), 75, -30 + 60 * random.NextSingle()), out var bodyDescription); - dynamicHandles.Enqueue(Simulation.Bodies.Add(bodyDescription), BufferPool); - } - int targetAsymptote = 65536; - var removalCount = (int)(dynamicHandles.Count * (newShapeCount / (float)targetAsymptote)); - for (int i = 0; i < removalCount; ++i) + var baseAngle = (float)(t * 0.015); + var anglePerKinematic = MathHelper.TwoPi / kinematicHandles.Length; + var maxDisplacement = 50 * timestepDuration; + var inverseDt = 1f / timestepDuration; + for (int i = 0; i < kinematicHandles.Length; ++i) + { + ref var bodyLocation = ref Simulation.Bodies.HandleToLocation[kinematicHandles[i].Value]; + + ref var set = ref Simulation.Bodies.Sets[bodyLocation.SetIndex]; + var angle = anglePerKinematic * i; + var positionAngle = baseAngle + angle; + var radius = 128 + 32 * (float)Math.Cos(3 * (angle + t * (1f / 3f))) + 32 * (float)Math.Cos(t * (1f / 3f)); + var targetLocation = new Vector3( + radius * (float)Math.Cos(positionAngle), + 16 + 16 * (float)Math.Cos(4 * (angle + t * 0.5)), + radius * (float)Math.Sin(positionAngle)); + + var correction = targetLocation - set.DynamicsState[bodyLocation.Index].Motion.Pose.Position; + var distance = correction.Length(); + if (distance > 1e-4) { - if (dynamicHandles.TryDequeue(out var handle)) + if (bodyLocation.SetIndex > 0) { - ref var bodyLocation = ref Simulation.Bodies.HandleToLocation[handle.Value]; - //Every body has a unique shape, so we need to remove shapes with bodies. - var shapeIndex = Simulation.Bodies.Sets[bodyLocation.SetIndex].Collidables[bodyLocation.Index].Shape; - Simulation.Bodies.Remove(handle); - Simulation.Shapes.RecursivelyRemoveAndDispose(shapeIndex, BufferPool); + //We're requesting a nonzero velocity, so it must be active. + Simulation.Awakener.AwakenSet(bodyLocation.SetIndex); } - else + if (distance > maxDisplacement) { - break; + correction *= maxDisplacement / distance; } + Debug.Assert(bodyLocation.SetIndex == 0); + Simulation.Bodies.ActiveSet.DynamicsState[bodyLocation.Index].Motion.Velocity.Linear = correction * inverseDt; } - - //Change some dynamic objects without adding/removing them to make sure all the state transition stuff works reasonably well. - var dynamicApplyDescriptionsPerFrame = 8; - for (int i = 0; i < dynamicApplyDescriptionsPerFrame; ++i) + else { - var handle = dynamicHandles[random.Next(dynamicHandles.Count)]; - Simulation.Bodies.GetDescription(handle, out var description); - Simulation.Shapes.RecursivelyRemoveAndDispose(description.Collidable.Shape, BufferPool); - CreateBodyDescription(random, description.Pose, description.Velocity, out var newDescription); - if (random.NextSingle() < 0.1f) + if (bodyLocation.SetIndex == 0) { - //Occasionally make a dynamic kinematic. - newDescription.LocalInertia = default; + Simulation.Bodies.ActiveSet.DynamicsState[bodyLocation.Index].Motion.Velocity.Linear = new Vector3(); } - else if (random.NextSingle() < 0.05f) - { - //Occasionally make a rotation-locked dynamic. - newDescription.LocalInertia.InverseInertiaTensor = default; - } - Simulation.Bodies.ApplyDescription(handle, newDescription); } + } - base.Update(window, camera, input, dt); + //Remove some statics from the simulation. + var missingStaticsAsymptote = 512; + var staticRemovalsPerFrame = 8; + for (int i = 0; i < staticRemovalsPerFrame; ++i) + { + var indexToRemove = random.Next(Simulation.Statics.Count); + Simulation.Statics.GetDescription(Simulation.Statics.IndexToHandle[indexToRemove], out var staticDescription); + Simulation.Statics.RemoveAt(indexToRemove); + removedStatics.Enqueue(staticDescription, BufferPool); + } - if (input != null && input.WasPushed(OpenTK.Input.Key.P)) - GC.Collect(int.MaxValue, GCCollectionMode.Forced, true, true); + var staticApplyDescriptionsPerFrame = 8; + for (int i = 0; i < staticApplyDescriptionsPerFrame; ++i) + { + var indexToReapply = random.Next(Simulation.Statics.Count); + var handleToReapply = Simulation.Statics.IndexToHandle[indexToReapply]; + Simulation.Statics.GetDescription(handleToReapply, out var staticDescription); + //Statics don't have as much in the way of transitions. They can't be shapeless, and going from one shape to another doesn't anything that a pose change doesn't. For now, we'll just test the application of descriptions with different poses. + var mutatedDescription = staticDescription; + mutatedDescription.Pose.Position.Y += 50; + QuaternionEx.Concatenate(mutatedDescription.Pose.Orientation, QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), random.NextSingle() * MathF.PI), out mutatedDescription.Pose.Orientation); + Simulation.Statics.ApplyDescription(handleToReapply, mutatedDescription); + Simulation.Statics.ApplyDescription(handleToReapply, staticDescription); + } + //Add some of the missing static bodies back into the simulation. + var staticAddCount = removedStatics.Count * (staticRemovalsPerFrame / (float)missingStaticsAsymptote); + for (int i = 0; i < staticAddCount; ++i) + { + Debug.Assert(removedStatics.Count > 0); + var staticDescription = removedStatics.Dequeue(); + Simulation.Statics.Add(staticDescription); } + //Spray some shapes! + int newShapeCount = 8; + var spawnPose = new RigidPose(new Vector3(0, 10, 0)); + for (int i = 0; i < newShapeCount; ++i) + { + CreateBodyDescription(random, spawnPose, new Vector3(-30 + 60 * random.NextSingle(), 75, -30 + 60 * random.NextSingle()), out var bodyDescription); + dynamicHandles.Enqueue(Simulation.Bodies.Add(bodyDescription), BufferPool); + } + int targetAsymptote = 65536; + var removalCount = (int)(dynamicHandles.Count * (newShapeCount / (float)targetAsymptote)); + for (int i = 0; i < removalCount; ++i) + { + if (dynamicHandles.TryDequeue(out var handle)) + { + ref var bodyLocation = ref Simulation.Bodies.HandleToLocation[handle.Value]; + //Every body has a unique shape, so we need to remove shapes with bodies. + var shapeIndex = Simulation.Bodies.Sets[bodyLocation.SetIndex].Collidables[bodyLocation.Index].Shape; + Simulation.Bodies.Remove(handle); + Simulation.Shapes.RecursivelyRemoveAndDispose(shapeIndex, BufferPool); + } + else + { + break; + } + } + + //Change some dynamic objects without adding/removing them to make sure all the state transition stuff works reasonably well. + var dynamicApplyDescriptionsPerFrame = 8; + for (int i = 0; i < dynamicApplyDescriptionsPerFrame; ++i) + { + var handle = dynamicHandles[random.Next(dynamicHandles.Count)]; + Simulation.Bodies.GetDescription(handle, out var description); + Simulation.Shapes.RecursivelyRemoveAndDispose(description.Collidable.Shape, BufferPool); + CreateBodyDescription(random, description.Pose, description.Velocity, out var newDescription); + if (random.NextSingle() < 0.1f) + { + //Occasionally make a dynamic kinematic. + newDescription.LocalInertia = default; + } + else if (random.NextSingle() < 0.05f) + { + //Occasionally make a rotation-locked dynamic. + newDescription.LocalInertia.InverseInertiaTensor = default; + } + Simulation.Bodies.ApplyDescription(handle, newDescription); + } + + base.Update(window, camera, input, dt); + + if (input != null && input.WasPushed(OpenTK.Input.Key.P)) + GC.Collect(int.MaxValue, GCCollectionMode.Forced, true, true); + } + } diff --git a/Demos/SpecializedTests/GyroscopeTestDemo.cs b/Demos/SpecializedTests/GyroscopeTestDemo.cs index 6dae4a31c..ea2166d18 100644 --- a/Demos/SpecializedTests/GyroscopeTestDemo.cs +++ b/Demos/SpecializedTests/GyroscopeTestDemo.cs @@ -6,70 +6,69 @@ using BepuPhysics.Collidables; using BepuPhysics.Constraints; -namespace Demos.Demos -{ - struct GyroscopicIntegratorCallbacks : IPoseIntegratorCallbacks - { - //We'll use all the usual demo integration stuff, but use ConserveMomentumWithGyroscopicForce instead of the DemoPoseIntegratorCallbacks Nonconserving mode. - //Pose integration isn't very expensive so using the higher quality option isn't that much of an issue, but it's also pretty subtle. - //Unless your simulation requires the extra fidelity, there's not much reason to spend the extra time on it. - DemoPoseIntegratorCallbacks innerCallbacks; - public readonly AngularIntegrationMode AngularIntegrationMode => AngularIntegrationMode.ConserveMomentum; - //For this demo, we'll allow substepping for unconstrained bodies. - public readonly bool AllowSubstepsForUnconstrainedBodies => true; +namespace Demos.Demos; - public readonly bool IntegrateVelocityForKinematics => false; +struct GyroscopicIntegratorCallbacks : IPoseIntegratorCallbacks +{ + //We'll use all the usual demo integration stuff, but use ConserveMomentumWithGyroscopicForce instead of the DemoPoseIntegratorCallbacks Nonconserving mode. + //Pose integration isn't very expensive so using the higher quality option isn't that much of an issue, but it's also pretty subtle. + //Unless your simulation requires the extra fidelity, there's not much reason to spend the extra time on it. + DemoPoseIntegratorCallbacks innerCallbacks; + public readonly AngularIntegrationMode AngularIntegrationMode => AngularIntegrationMode.ConserveMomentum; + //For this demo, we'll allow substepping for unconstrained bodies. + public readonly bool AllowSubstepsForUnconstrainedBodies => true; - public void Initialize(Simulation simulation) - { - innerCallbacks.Initialize(simulation); - } + public readonly bool IntegrateVelocityForKinematics => false; - public GyroscopicIntegratorCallbacks(Vector3 gravity, float linearDamping, float angularDamping) - { - innerCallbacks = new DemoPoseIntegratorCallbacks(gravity, linearDamping, angularDamping); - } + public void Initialize(Simulation simulation) + { + innerCallbacks.Initialize(simulation); + } - public void PrepareForIntegration(float dt) - { - innerCallbacks.PrepareForIntegration(dt); - } + public GyroscopicIntegratorCallbacks(Vector3 gravity, float linearDamping, float angularDamping) + { + innerCallbacks = new DemoPoseIntegratorCallbacks(gravity, linearDamping, angularDamping); + } - public void IntegrateVelocity(Vector bodyIndices, Vector3Wide position, QuaternionWide orientation, BodyInertiaWide localInertia, Vector integrationMask, int workerIndex, Vector dt, ref BodyVelocityWide velocity) - { - innerCallbacks.IntegrateVelocity(bodyIndices, position, orientation, localInertia, integrationMask, workerIndex, dt, ref velocity); - } + public void PrepareForIntegration(float dt) + { + innerCallbacks.PrepareForIntegration(dt); + } + public void IntegrateVelocity(Vector bodyIndices, Vector3Wide position, QuaternionWide orientation, BodyInertiaWide localInertia, Vector integrationMask, int workerIndex, Vector dt, ref BodyVelocityWide velocity) + { + innerCallbacks.IntegrateVelocity(bodyIndices, position, orientation, localInertia, integrationMask, workerIndex, dt, ref velocity); } - public class GyroscopeTestDemo : Demo +} + +public class GyroscopeTestDemo : Demo +{ + public override void Initialize(ContentArchive content, Camera camera) { - public override void Initialize(ContentArchive content, Camera camera) - { - camera.Position = new Vector3(0, 2, -5); - camera.Yaw = MathHelper.Pi; - camera.Pitch = 0; + camera.Position = new Vector3(0, 2, -5); + camera.Yaw = MathHelper.Pi; + camera.Pitch = 0; - //Note the lack of damping- we want the gyroscope to keep spinning. - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new GyroscopicIntegratorCallbacks(new Vector3(0, -10, 0), 0f, 0f), new SolveDescription(1, 8)); + //Note the lack of damping- we want the gyroscope to keep spinning. + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new GyroscopicIntegratorCallbacks(new Vector3(0, -10, 0), 0f, 0f), new SolveDescription(1, 8)); - Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(new Box(100, 1, 100)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(new Box(100, 1, 100)))); - var gyroBaseBody = Simulation.Bodies.Add(BodyDescription.CreateConvexKinematic(new Vector3(0, 2, 0), Simulation.Shapes, new Box(.1f, 4, .1f))); - var gyroSpinnerBody = Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(2, 4, 0), (default, new Vector3(300, 0, 0)), 1, Simulation.Shapes, new Box(0.1f, 1f, 1f))); - Simulation.Solver.Add(gyroBaseBody, gyroSpinnerBody, new BallSocket { LocalOffsetA = new Vector3(0, 2, 0), LocalOffsetB = new Vector3(-2, 0, 0), SpringSettings = new SpringSettings(30, 1) }); + var gyroBaseBody = Simulation.Bodies.Add(BodyDescription.CreateConvexKinematic(new Vector3(0, 2, 0), Simulation.Shapes, new Box(.1f, 4, .1f))); + var gyroSpinnerBody = Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(2, 4, 0), (default, new Vector3(300, 0, 0)), 1, Simulation.Shapes, new Box(0.1f, 1f, 1f))); + Simulation.Solver.Add(gyroBaseBody, gyroSpinnerBody, new BallSocket { LocalOffsetA = new Vector3(0, 2, 0), LocalOffsetB = new Vector3(-2, 0, 0), SpringSettings = new SpringSettings(30, 1) }); - var builder = new CompoundBuilder(BufferPool, Simulation.Shapes, 2); - builder.Add(new Box(1, 0.3f, 0.3f), new Vector3(-0.5f, 0, 0), 1); - builder.Add(new Box(0.3f, 1.5f, 0.3f), new Vector3(0.15f, 0, 0), 3); - builder.BuildDynamicCompound(out var children, out var inertia, out _); - builder.Dispose(); - var dzhanibekovShape = Simulation.Shapes.Add(new Compound(children)); - var dzhanibekovSpinnerBody = Simulation.Bodies.Add( - BodyDescription.CreateDynamic(new Vector3(6, 4, 0), (new Vector3(0, 0, 1), new Vector3(3, 1e-5f, 0)), inertia, dzhanibekovShape, 0.01f)); - var dzhanibekovBaseBody = Simulation.Bodies.Add(BodyDescription.CreateConvexKinematic(new Vector3(6, 1, 0), Simulation.Shapes, new Box(.1f, 2, .1f))); - Simulation.Solver.Add(dzhanibekovBaseBody, dzhanibekovSpinnerBody, new BallSocket { LocalOffsetA = new Vector3(0, 3, 0), LocalOffsetB = new Vector3(0, 0, 0), SpringSettings = new SpringSettings(30, 1) }); - } + var builder = new CompoundBuilder(BufferPool, Simulation.Shapes, 2); + builder.Add(new Box(1, 0.3f, 0.3f), new Vector3(-0.5f, 0, 0), 1); + builder.Add(new Box(0.3f, 1.5f, 0.3f), new Vector3(0.15f, 0, 0), 3); + builder.BuildDynamicCompound(out var children, out var inertia, out _); + builder.Dispose(); + var dzhanibekovShape = Simulation.Shapes.Add(new Compound(children)); + var dzhanibekovSpinnerBody = Simulation.Bodies.Add( + BodyDescription.CreateDynamic(new Vector3(6, 4, 0), (new Vector3(0, 0, 1), new Vector3(3, 1e-5f, 0)), inertia, dzhanibekovShape, 0.01f)); + var dzhanibekovBaseBody = Simulation.Bodies.Add(BodyDescription.CreateConvexKinematic(new Vector3(6, 1, 0), Simulation.Shapes, new Box(.1f, 2, .1f))); + Simulation.Solver.Add(dzhanibekovBaseBody, dzhanibekovSpinnerBody, new BallSocket { LocalOffsetA = new Vector3(0, 3, 0), LocalOffsetB = new Vector3(0, 0, 0), SpringSettings = new SpringSettings(30, 1) }); } } diff --git a/Demos/SpecializedTests/IntertreeThreadingTests.cs b/Demos/SpecializedTests/IntertreeThreadingTests.cs index 898fd42b2..e14bfe696 100644 --- a/Demos/SpecializedTests/IntertreeThreadingTests.cs +++ b/Demos/SpecializedTests/IntertreeThreadingTests.cs @@ -6,185 +6,184 @@ using System.Numerics; using System.Runtime.CompilerServices; -namespace Demos.SpecializedTests +namespace Demos.SpecializedTests; + +public static class IntertreeThreadingTests { - public static class IntertreeThreadingTests + static void GetRandomLocation(Random random, ref BoundingBox locationBounds, out Vector3 location) + { + location = (locationBounds.Max - locationBounds.Min) * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) + locationBounds.Min; + } + struct OverlapHandler : IOverlapHandler { - static void GetRandomLocation(Random random, ref BoundingBox locationBounds, out Vector3 location) + public List<(int a, int b)> Pairs; + public void Handle(int indexA, int indexB) { - location = (locationBounds.Max - locationBounds.Min) * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) + locationBounds.Min; + Pairs.Add((indexA, indexB)); } - struct OverlapHandler : IOverlapHandler + } + + static void GetBoundsForLeaf(in Tree tree, int leafIndex, out BoundingBox bounds) + { + ref var leaf = ref tree.Leaves[leafIndex]; + ref var node = ref tree.Nodes[leaf.NodeIndex]; + bounds = leaf.ChildIndex == 0 ? new BoundingBox(node.A.Min, node.A.Max) : new BoundingBox(node.B.Min, node.B.Max); + } + + static void SortPairs(List<(int a, int b)> pairs) + { + for (int i = 0; i < pairs.Count; ++i) { - public List<(int a, int b)> Pairs; - public void Handle(int indexA, int indexB) + if (pairs[i].b < pairs[i].a) { - Pairs.Add((indexA, indexB)); + pairs[i] = (pairs[i].b, pairs[i].a); } } + Comparison<(int, int)> comparison = (a, b) => + { + var combinedA = ((ulong)a.Item1 << 32) | ((uint)a.Item2); + var combinedB = ((ulong)b.Item1 << 32) | ((uint)b.Item2); + return combinedA.CompareTo(combinedB); + }; + pairs.Sort(comparison); + } - static void GetBoundsForLeaf(in Tree tree, int leafIndex, out BoundingBox bounds) + unsafe static void TestTrees(BufferPool pool, IThreadDispatcher threadDispatcher, Random random) + { + var treeA = new Tree(pool, 1); + var treeB = new Tree(pool, 1); + + var aBounds = new BoundingBox(new Vector3(-40, 0, -40), new Vector3(40, 0, 40)); + var aOffset = new Vector3(3f, 3f, 3f); + var aCount = 1024; + var bBounds = new BoundingBox(new Vector3(-5, -2, -5), new Vector3(5, 2, 5)); + var bOffset = new Vector3(0.5f, 0.5f, 0.5f); + var bCount = 3; + for (int i = 0; i < aCount; ++i) { - ref var leaf = ref tree.Leaves[leafIndex]; - ref var node = ref tree.Nodes[leaf.NodeIndex]; - bounds = leaf.ChildIndex == 0 ? new BoundingBox(node.A.Min, node.A.Max) : new BoundingBox(node.B.Min, node.B.Max); + GetRandomLocation(random, ref aBounds, out var center); + var bounds = new BoundingBox(center - aOffset, center + aOffset); + treeA.Add(bounds, pool); } - - static void SortPairs(List<(int a, int b)> pairs) + for (int i = 0; i < bCount; ++i) { - for (int i = 0; i < pairs.Count; ++i) - { - if (pairs[i].b < pairs[i].a) - { - pairs[i] = (pairs[i].b, pairs[i].a); - } - } - Comparison<(int, int)> comparison = (a, b) => - { - var combinedA = ((ulong)a.Item1 << 32) | ((uint)a.Item2); - var combinedB = ((ulong)b.Item1 << 32) | ((uint)b.Item2); - return combinedA.CompareTo(combinedB); - }; - pairs.Sort(comparison); + GetRandomLocation(random, ref bBounds, out var center); + var bounds = new BoundingBox(center - bOffset, center + bOffset); + treeB.Add(bounds, pool); } - - unsafe static void TestTrees(BufferPool pool, IThreadDispatcher threadDispatcher, Random random) + { - var treeA = new Tree(pool, 1); - var treeB = new Tree(pool, 1); - - var aBounds = new BoundingBox(new Vector3(-40, 0, -40), new Vector3(40, 0, 40)); - var aOffset = new Vector3(3f, 3f, 3f); - var aCount = 1024; - var bBounds = new BoundingBox(new Vector3(-5, -2, -5), new Vector3(5, 2, 5)); - var bOffset = new Vector3(0.5f, 0.5f, 0.5f); - var bCount = 3; - for (int i = 0; i < aCount; ++i) - { - GetRandomLocation(random, ref aBounds, out var center); - var bounds = new BoundingBox(center - aOffset, center + aOffset); - treeA.Add(bounds, pool); - } - for (int i = 0; i < bCount; ++i) - { - GetRandomLocation(random, ref bBounds, out var center); - var bounds = new BoundingBox(center - bOffset, center + bOffset); - treeB.Add(bounds, pool); - } - - { - var indexToRemove = 1; - GetBoundsForLeaf(treeB, indexToRemove, out var removedBounds); - treeB.RemoveAt(indexToRemove); - treeA.Add(removedBounds, pool); - } - - var singleThreadedResults = new OverlapHandler { Pairs = new List<(int a, int b)>() }; - treeA.GetOverlaps(ref treeB, ref singleThreadedResults); - SortPairs(singleThreadedResults.Pairs); - for (int i = 0; i < 10; ++i) - { - treeA.RefitAndRefine(pool, i); - treeB.RefitAndRefine(pool, i); - } - treeA.Validate(); - treeB.Validate(); + var indexToRemove = 1; + GetBoundsForLeaf(treeB, indexToRemove, out var removedBounds); + treeB.RemoveAt(indexToRemove); + treeA.Add(removedBounds, pool); + } + + var singleThreadedResults = new OverlapHandler { Pairs = new List<(int a, int b)>() }; + treeA.GetOverlaps(ref treeB, ref singleThreadedResults); + SortPairs(singleThreadedResults.Pairs); + for (int i = 0; i < 10; ++i) + { + treeA.RefitAndRefine(pool, i); + treeB.RefitAndRefine(pool, i); + } + treeA.Validate(); + treeB.Validate(); - var context = new Tree.MultithreadedIntertreeTest(pool); - var handlers = new OverlapHandler[threadDispatcher.ThreadCount]; - for (int i = 0; i < threadDispatcher.ThreadCount; ++i) - { - handlers[i].Pairs = new List<(int a, int b)>(); - } - context.PrepareJobs(ref treeA, ref treeB, handlers, threadDispatcher.ThreadCount); - threadDispatcher.DispatchWorkers(context.PairTest, context.JobCount); - context.CompleteTest(); - List<(int a, int b)> multithreadedResults = new List<(int, int)>(); - for (int i = 0; i < threadDispatcher.ThreadCount; ++i) - { - multithreadedResults.AddRange(handlers[i].Pairs); - } - SortPairs(multithreadedResults); + var context = new Tree.MultithreadedIntertreeTest(pool); + var handlers = new OverlapHandler[threadDispatcher.ThreadCount]; + for (int i = 0; i < threadDispatcher.ThreadCount; ++i) + { + handlers[i].Pairs = new List<(int a, int b)>(); + } + context.PrepareJobs(ref treeA, ref treeB, handlers, threadDispatcher.ThreadCount); + threadDispatcher.DispatchWorkers(context.PairTest, context.JobCount); + context.CompleteTest(); + List<(int a, int b)> multithreadedResults = new List<(int, int)>(); + for (int i = 0; i < threadDispatcher.ThreadCount; ++i) + { + multithreadedResults.AddRange(handlers[i].Pairs); + } + SortPairs(multithreadedResults); - if (singleThreadedResults.Pairs.Count != multithreadedResults.Count) - { - throw new Exception("Single threaded vs multithreaded counts don't match."); - } - for (int i = 0; i < singleThreadedResults.Pairs.Count; ++i) + if (singleThreadedResults.Pairs.Count != multithreadedResults.Count) + { + throw new Exception("Single threaded vs multithreaded counts don't match."); + } + for (int i = 0; i < singleThreadedResults.Pairs.Count; ++i) + { + var singleThreadedPair = singleThreadedResults.Pairs[i]; + var multithreadedPair = multithreadedResults[i]; + if (singleThreadedPair.a != multithreadedPair.a || + singleThreadedPair.b != multithreadedPair.b) { - var singleThreadedPair = singleThreadedResults.Pairs[i]; - var multithreadedPair = multithreadedResults[i]; - if (singleThreadedPair.a != multithreadedPair.a || - singleThreadedPair.b != multithreadedPair.b) - { - throw new Exception("Single threaded vs multithreaded results don't match."); - } + throw new Exception("Single threaded vs multithreaded results don't match."); } + } - //Single and multithreaded variants produce the same results. But do they match a brute force test? - Tree smaller, larger; - if (treeA.LeafCount < treeB.LeafCount) - { - smaller = treeA; - larger = treeB; - } - else - { - smaller = treeB; - larger = treeA; - } - var bruteResultsEnumerator = new BruteForceResultsEnumerator(); - bruteResultsEnumerator.Pairs = new List<(int a, int b)>(); - for (int i = 0; i < smaller.LeafCount; ++i) - { - GetBoundsForLeaf(smaller, i, out var bounds); - bruteResultsEnumerator.QuerySourceIndex = i; - larger.GetOverlaps(bounds, ref bruteResultsEnumerator); - } - SortPairs(bruteResultsEnumerator.Pairs); + //Single and multithreaded variants produce the same results. But do they match a brute force test? + Tree smaller, larger; + if (treeA.LeafCount < treeB.LeafCount) + { + smaller = treeA; + larger = treeB; + } + else + { + smaller = treeB; + larger = treeA; + } + var bruteResultsEnumerator = new BruteForceResultsEnumerator(); + bruteResultsEnumerator.Pairs = new List<(int a, int b)>(); + for (int i = 0; i < smaller.LeafCount; ++i) + { + GetBoundsForLeaf(smaller, i, out var bounds); + bruteResultsEnumerator.QuerySourceIndex = i; + larger.GetOverlaps(bounds, ref bruteResultsEnumerator); + } + SortPairs(bruteResultsEnumerator.Pairs); - if (singleThreadedResults.Pairs.Count != bruteResultsEnumerator.Pairs.Count) - { - throw new Exception("Brute force vs intertree counts don't match."); - } - for (int i = 0; i < singleThreadedResults.Pairs.Count; ++i) + if (singleThreadedResults.Pairs.Count != bruteResultsEnumerator.Pairs.Count) + { + throw new Exception("Brute force vs intertree counts don't match."); + } + for (int i = 0; i < singleThreadedResults.Pairs.Count; ++i) + { + var singleThreadedPair = singleThreadedResults.Pairs[i]; + var bruteForcePair = bruteResultsEnumerator.Pairs[i]; + if (singleThreadedPair.a != bruteForcePair.a || + singleThreadedPair.b != bruteForcePair.b) { - var singleThreadedPair = singleThreadedResults.Pairs[i]; - var bruteForcePair = bruteResultsEnumerator.Pairs[i]; - if (singleThreadedPair.a != bruteForcePair.a || - singleThreadedPair.b != bruteForcePair.b) - { - throw new Exception("Brute force vs intertree results don't match."); - } + throw new Exception("Brute force vs intertree results don't match."); } - - treeA.Dispose(pool); - treeB.Dispose(pool); } - struct BruteForceResultsEnumerator : IBreakableForEach + treeA.Dispose(pool); + treeB.Dispose(pool); + } + + struct BruteForceResultsEnumerator : IBreakableForEach + { + public List<(int a, int b)> Pairs; + public int QuerySourceIndex; + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool LoopBody(int foundIndex) { - public List<(int a, int b)> Pairs; - public int QuerySourceIndex; - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool LoopBody(int foundIndex) - { - Pairs.Add((QuerySourceIndex, foundIndex)); - return true; - } + Pairs.Add((QuerySourceIndex, foundIndex)); + return true; } + } - public static void Test() + public static void Test() + { + var random = new Random(5); + var pool = new BufferPool(); + var threadDispatcher = new ThreadDispatcher(Environment.ProcessorCount); + for (int i = 0; i < 1000; ++i) { - var random = new Random(5); - var pool = new BufferPool(); - var threadDispatcher = new ThreadDispatcher(Environment.ProcessorCount); - for (int i = 0; i < 1000; ++i) - { - TestTrees(pool, threadDispatcher, random); - } - pool.Clear(); - threadDispatcher.Dispose(); + TestTrees(pool, threadDispatcher, random); } + pool.Clear(); + threadDispatcher.Dispose(); } } diff --git a/Demos/SpecializedTests/Media/2.0/BedsheetDemo.cs b/Demos/SpecializedTests/Media/2.0/BedsheetDemo.cs index fc07f1625..5bbbaa425 100644 --- a/Demos/SpecializedTests/Media/2.0/BedsheetDemo.cs +++ b/Demos/SpecializedTests/Media/2.0/BedsheetDemo.cs @@ -9,152 +9,151 @@ using System; using System.Numerics; -namespace Demos.Demos.Media +namespace Demos.Demos.Media; + + +/// +/// Shows a few different examples of cloth-ish constraint lattices. +/// +public class BedsheetDemo : Demo { + delegate bool KinematicDecider(int rowIndex, int columnIndex, int width, int height); - /// - /// Shows a few different examples of cloth-ish constraint lattices. - /// - public class BedsheetDemo : Demo + BodyHandle[,] CreateBodyGrid(Vector3 position, Quaternion orientation, int width, int height, float spacing, float bodyRadius, float massPerBody, + int instanceId, CollidableProperty filters, KinematicDecider isKinematic) { - delegate bool KinematicDecider(int rowIndex, int columnIndex, int width, int height); - - BodyHandle[,] CreateBodyGrid(Vector3 position, Quaternion orientation, int width, int height, float spacing, float bodyRadius, float massPerBody, - int instanceId, CollidableProperty filters, KinematicDecider isKinematic) + var description = BodyDescription.CreateKinematic(orientation, Simulation.Shapes.Add(new Sphere(bodyRadius)), 0.01f); + var inverseMass = 1f / massPerBody; + BodyHandle[,] handles = new BodyHandle[height, width]; + for (int rowIndex = 0; rowIndex < height; ++rowIndex) { - var description = BodyDescription.CreateKinematic(orientation, Simulation.Shapes.Add(new Sphere(bodyRadius)), 0.01f); - var inverseMass = 1f / massPerBody; - BodyHandle[,] handles = new BodyHandle[height, width]; - for (int rowIndex = 0; rowIndex < height; ++rowIndex) + for (int columnIndex = 0; columnIndex < width; ++columnIndex) { - for (int columnIndex = 0; columnIndex < width; ++columnIndex) - { - description.LocalInertia.InverseMass = isKinematic(rowIndex, columnIndex, width, height) ? 0 : inverseMass; - var localPosition = new Vector3(columnIndex * spacing, rowIndex * -spacing, 0); - QuaternionEx.TransformWithoutOverlap(localPosition, orientation, out var rotatedPosition); - description.Pose.Position = rotatedPosition + position; - var handle = Simulation.Bodies.Add(description); - handles[rowIndex, columnIndex] = handle; - filters.Allocate(handle) = new ClothCollisionFilter(rowIndex, columnIndex, instanceId); - } + description.LocalInertia.InverseMass = isKinematic(rowIndex, columnIndex, width, height) ? 0 : inverseMass; + var localPosition = new Vector3(columnIndex * spacing, rowIndex * -spacing, 0); + QuaternionEx.TransformWithoutOverlap(localPosition, orientation, out var rotatedPosition); + description.Pose.Position = rotatedPosition + position; + var handle = Simulation.Bodies.Add(description); + handles[rowIndex, columnIndex] = handle; + filters.Allocate(handle) = new ClothCollisionFilter(rowIndex, columnIndex, instanceId); } - return handles; } + return handles; + } - void CreateAreaConstraints(BodyHandle[,] bodyHandles, SpringSettings springSettings) - { - for (int rowIndex = 0; rowIndex < bodyHandles.GetLength(0) - 1; ++rowIndex) - { - for (int columnIndex = 0; columnIndex < bodyHandles.GetLength(1) - 1; ++columnIndex) - { - var aHandle = bodyHandles[rowIndex, columnIndex]; - var bHandle = bodyHandles[rowIndex + 1, columnIndex]; - var cHandle = bodyHandles[rowIndex, columnIndex + 1]; - var dHandle = bodyHandles[rowIndex + 1, columnIndex + 1]; - var a = new BodyReference(aHandle, Simulation.Bodies); - var b = new BodyReference(bHandle, Simulation.Bodies); - var c = new BodyReference(cHandle, Simulation.Bodies); - var d = new BodyReference(dHandle, Simulation.Bodies); - //Not worried about kinematics here- we create at most one row of kinematics in this demo. These are three body constraints that operate in a local quad, so - //there's no way for them to all be kinematic. - Simulation.Solver.Add(aHandle, bHandle, cHandle, new AreaConstraint(a.Pose.Position, b.Pose.Position, c.Pose.Position, springSettings)); - Simulation.Solver.Add(bHandle, cHandle, dHandle, new AreaConstraint(b.Pose.Position, c.Pose.Position, d.Pose.Position, springSettings)); - } - } - } - void CreateDistanceConstraints(BodyHandle[,] bodyHandles, SpringSettings springSettings) + void CreateAreaConstraints(BodyHandle[,] bodyHandles, SpringSettings springSettings) + { + for (int rowIndex = 0; rowIndex < bodyHandles.GetLength(0) - 1; ++rowIndex) { - void CreateConstraintBetweenBodies(BodyHandle aHandle, BodyHandle bHandle) + for (int columnIndex = 0; columnIndex < bodyHandles.GetLength(1) - 1; ++columnIndex) { + var aHandle = bodyHandles[rowIndex, columnIndex]; + var bHandle = bodyHandles[rowIndex + 1, columnIndex]; + var cHandle = bodyHandles[rowIndex, columnIndex + 1]; + var dHandle = bodyHandles[rowIndex + 1, columnIndex + 1]; var a = new BodyReference(aHandle, Simulation.Bodies); var b = new BodyReference(bHandle, Simulation.Bodies); - //Don't create constraints between two kinematic bodies. - if (a.LocalInertia.InverseMass > 0 || b.LocalInertia.InverseMass > 0) - { - //Note the use of a limit; the distance is allowed to go smaller. - //This helps stop the cloth from having unnatural rigidity. - var distance = Vector3.Distance(a.Pose.Position, b.Pose.Position); - Simulation.Solver.Add(aHandle, bHandle, new CenterDistanceLimit(distance * 0.15f, distance, springSettings)); - } + var c = new BodyReference(cHandle, Simulation.Bodies); + var d = new BodyReference(dHandle, Simulation.Bodies); + //Not worried about kinematics here- we create at most one row of kinematics in this demo. These are three body constraints that operate in a local quad, so + //there's no way for them to all be kinematic. + Simulation.Solver.Add(aHandle, bHandle, cHandle, new AreaConstraint(a.Pose.Position, b.Pose.Position, c.Pose.Position, springSettings)); + Simulation.Solver.Add(bHandle, cHandle, dHandle, new AreaConstraint(b.Pose.Position, c.Pose.Position, d.Pose.Position, springSettings)); } - for (int rowIndex = 0; rowIndex < bodyHandles.GetLength(0); ++rowIndex) + } + } + void CreateDistanceConstraints(BodyHandle[,] bodyHandles, SpringSettings springSettings) + { + void CreateConstraintBetweenBodies(BodyHandle aHandle, BodyHandle bHandle) + { + var a = new BodyReference(aHandle, Simulation.Bodies); + var b = new BodyReference(bHandle, Simulation.Bodies); + //Don't create constraints between two kinematic bodies. + if (a.LocalInertia.InverseMass > 0 || b.LocalInertia.InverseMass > 0) { - for (int columnIndex = 0; columnIndex < bodyHandles.GetLength(1) - 1; ++columnIndex) - { - CreateConstraintBetweenBodies(bodyHandles[rowIndex, columnIndex], bodyHandles[rowIndex, columnIndex + 1]); - } + //Note the use of a limit; the distance is allowed to go smaller. + //This helps stop the cloth from having unnatural rigidity. + var distance = Vector3.Distance(a.Pose.Position, b.Pose.Position); + Simulation.Solver.Add(aHandle, bHandle, new CenterDistanceLimit(distance * 0.15f, distance, springSettings)); } - for (int rowIndex = 0; rowIndex < bodyHandles.GetLength(0) - 1; ++rowIndex) + } + for (int rowIndex = 0; rowIndex < bodyHandles.GetLength(0); ++rowIndex) + { + for (int columnIndex = 0; columnIndex < bodyHandles.GetLength(1) - 1; ++columnIndex) { - for (int columnIndex = 0; columnIndex < bodyHandles.GetLength(1); ++columnIndex) - { - CreateConstraintBetweenBodies(bodyHandles[rowIndex, columnIndex], bodyHandles[rowIndex + 1, columnIndex]); - } + CreateConstraintBetweenBodies(bodyHandles[rowIndex, columnIndex], bodyHandles[rowIndex, columnIndex + 1]); } - for (int rowIndex = 0; rowIndex < bodyHandles.GetLength(0) - 1; ++rowIndex) + } + for (int rowIndex = 0; rowIndex < bodyHandles.GetLength(0) - 1; ++rowIndex) + { + for (int columnIndex = 0; columnIndex < bodyHandles.GetLength(1); ++columnIndex) { - for (int columnIndex = 0; columnIndex < bodyHandles.GetLength(1) - 1; ++columnIndex) - { - CreateConstraintBetweenBodies(bodyHandles[rowIndex, columnIndex], bodyHandles[rowIndex + 1, columnIndex + 1]); - CreateConstraintBetweenBodies(bodyHandles[rowIndex, columnIndex + 1], bodyHandles[rowIndex + 1, columnIndex]); - } + CreateConstraintBetweenBodies(bodyHandles[rowIndex, columnIndex], bodyHandles[rowIndex + 1, columnIndex]); } } - - RolloverInfo rolloverInfo; - - public override void Initialize(ContentArchive content, Camera camera) + for (int rowIndex = 0; rowIndex < bodyHandles.GetLength(0) - 1; ++rowIndex) { - camera.Position = new Vector3(70, 40, -80); - camera.Yaw = -MathF.PI * 0.8f; - camera.Pitch = MathF.PI * 0.1f; - - var filters = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new ClothCallbacks(filters), new DemoPoseIntegratorCallbacks(new Vector3(0, -50, 0)), new SolveDescription(8, 1)); - rolloverInfo = new RolloverInfo(); - - bool FullyDynamic(int rowIndex, int columnIndex, int width, int height) + for (int columnIndex = 0; columnIndex < bodyHandles.GetLength(1) - 1; ++columnIndex) { - return false; + CreateConstraintBetweenBodies(bodyHandles[rowIndex, columnIndex], bodyHandles[rowIndex + 1, columnIndex + 1]); + CreateConstraintBetweenBodies(bodyHandles[rowIndex, columnIndex + 1], bodyHandles[rowIndex + 1, columnIndex]); } + } + } - int clothInstanceId = 0; - var initialRotation = QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), MathF.PI * -0.5f); + RolloverInfo rolloverInfo; + public override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(70, 40, -80); + camera.Yaw = -MathF.PI * 0.8f; + camera.Pitch = MathF.PI * 0.1f; + var filters = new CollidableProperty(); + Simulation = Simulation.Create(BufferPool, new ClothCallbacks(filters), new DemoPoseIntegratorCallbacks(new Vector3(0, -50, 0)), new SolveDescription(8, 1)); + rolloverInfo = new RolloverInfo(); + bool FullyDynamic(int rowIndex, int columnIndex, int width, int height) + { + return false; + } - Simulation.Statics.Add(new StaticDescription(new Vector3(0, 10, 0), Simulation.Shapes.Add(new Box(80, 20, 80)))); - Simulation.Statics.Add(new StaticDescription(new Vector3(-20, 22, 30), Simulation.Shapes.Add(new Box(34, 4, 14)))); - Simulation.Statics.Add(new StaticDescription(new Vector3(20, 22, 30), Simulation.Shapes.Add(new Box(34, 4, 14)))); + int clothInstanceId = 0; + var initialRotation = QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), MathF.PI * -0.5f); - Simulation.Statics.Add(new StaticDescription(new Vector3(65.5f, 8f, 20), Simulation.Shapes.Add(new Cylinder(15, 15)))); - { - var position = new Vector3(96 * 1.15f * -0.5f, 30, 86 * 1.15f * -0.5f); - var handles = CreateBodyGrid(position, initialRotation, 96, 86, 1.15f, 1f, 1, clothInstanceId++, filters, FullyDynamic); - CreateDistanceConstraints(handles, new SpringSettings(20, 1)); - CreateAreaConstraints(handles, new SpringSettings(30, 1)); - } + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 10, 0), Simulation.Shapes.Add(new Box(80, 20, 80)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(-20, 22, 30), Simulation.Shapes.Add(new Box(34, 4, 14)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(20, 22, 30), Simulation.Shapes.Add(new Box(34, 4, 14)))); - { - var position = new Vector3(65.5f + 56 * 0.8f * -0.5f, 25, 20 + 56 * 0.8f * -0.5f); - var handles = CreateBodyGrid(position, initialRotation, 56, 56, 0.8f, 0.65f, 1, clothInstanceId++, filters, FullyDynamic); - CreateDistanceConstraints(handles, new SpringSettings(20, 1)); - CreateAreaConstraints(handles, new SpringSettings(30, 1)); - } - Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), Simulation.Shapes.Add(new Box(400, 1, 400)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(65.5f, 8f, 20), Simulation.Shapes.Add(new Cylinder(15, 15)))); + + { + var position = new Vector3(96 * 1.15f * -0.5f, 30, 86 * 1.15f * -0.5f); + var handles = CreateBodyGrid(position, initialRotation, 96, 86, 1.15f, 1f, 1, clothInstanceId++, filters, FullyDynamic); + CreateDistanceConstraints(handles, new SpringSettings(20, 1)); + CreateAreaConstraints(handles, new SpringSettings(30, 1)); } - public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) { - rolloverInfo.Render(renderer, camera, input, text, font); - base.Render(renderer, camera, input, text, font); + var position = new Vector3(65.5f + 56 * 0.8f * -0.5f, 25, 20 + 56 * 0.8f * -0.5f); + var handles = CreateBodyGrid(position, initialRotation, 56, 56, 0.8f, 0.65f, 1, clothInstanceId++, filters, FullyDynamic); + CreateDistanceConstraints(handles, new SpringSettings(20, 1)); + CreateAreaConstraints(handles, new SpringSettings(30, 1)); } + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), Simulation.Shapes.Add(new Box(400, 1, 400)))); + } + + public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + { + rolloverInfo.Render(renderer, camera, input, text, font); + base.Render(renderer, camera, input, text, font); + } + } diff --git a/Demos/SpecializedTests/Media/2.0/ColosseumVideoDemo.cs b/Demos/SpecializedTests/Media/2.0/ColosseumVideoDemo.cs index f31f5db46..219b74084 100644 --- a/Demos/SpecializedTests/Media/2.0/ColosseumVideoDemo.cs +++ b/Demos/SpecializedTests/Media/2.0/ColosseumVideoDemo.cs @@ -10,152 +10,151 @@ using System; using System.Numerics; -namespace Demos.SpecializedTests.Media +namespace Demos.SpecializedTests.Media; + +/// +/// Version of the colosseum demo for video purposes. +/// +public class ColosseumVideoDemo : Demo { - /// - /// Version of the colosseum demo for video purposes. - /// - public class ColosseumVideoDemo : Demo + void CreateRingWall(Vector3 position, Box ringBoxShape, BodyDescription bodyDescription, int height, float radius) { - void CreateRingWall(Vector3 position, Box ringBoxShape, BodyDescription bodyDescription, int height, float radius) + var circumference = MathF.PI * 2 * radius; + var boxCountPerRing = (int)(0.9f * circumference / ringBoxShape.Length); + float increment = MathHelper.TwoPi / boxCountPerRing; + for (int ringIndex = 0; ringIndex < height; ringIndex++) { - var circumference = MathF.PI * 2 * radius; - var boxCountPerRing = (int)(0.9f * circumference / ringBoxShape.Length); - float increment = MathHelper.TwoPi / boxCountPerRing; - for (int ringIndex = 0; ringIndex < height; ringIndex++) + for (int i = 0; i < boxCountPerRing; i++) { - for (int i = 0; i < boxCountPerRing; i++) - { - var angle = ((ringIndex & 1) == 0 ? i + 0.5f : i) * increment; - bodyDescription.Pose = (position + new Vector3(-MathF.Cos(angle) * radius, (ringIndex + 0.5f) * ringBoxShape.Height, MathF.Sin(angle) * radius), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, angle)); - Simulation.Bodies.Add(bodyDescription); - } + var angle = ((ringIndex & 1) == 0 ? i + 0.5f : i) * increment; + bodyDescription.Pose = (position + new Vector3(-MathF.Cos(angle) * radius, (ringIndex + 0.5f) * ringBoxShape.Height, MathF.Sin(angle) * radius), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, angle)); + Simulation.Bodies.Add(bodyDescription); } } + } - void CreateRingPlatform(Vector3 position, Box ringBoxShape, BodyDescription bodyDescription, float radius) + void CreateRingPlatform(Vector3 position, Box ringBoxShape, BodyDescription bodyDescription, float radius) + { + var innerCircumference = MathF.PI * 2 * (radius - ringBoxShape.HalfLength); + var boxCount = (int)(0.95f * innerCircumference / ringBoxShape.Height); + float increment = MathHelper.TwoPi / boxCount; + for (int i = 0; i < boxCount; i++) { - var innerCircumference = MathF.PI * 2 * (radius - ringBoxShape.HalfLength); - var boxCount = (int)(0.95f * innerCircumference / ringBoxShape.Height); - float increment = MathHelper.TwoPi / boxCount; - for (int i = 0; i < boxCount; i++) - { - var angle = i * increment; - bodyDescription.Pose = (position + new Vector3(-MathF.Cos(angle) * radius, ringBoxShape.HalfWidth, MathF.Sin(angle) * radius), - QuaternionEx.Concatenate(QuaternionEx.CreateFromAxisAngle(Vector3.UnitZ, MathF.PI * 0.5f), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, angle + MathF.PI * 0.5f))); - Simulation.Bodies.Add(bodyDescription); - } + var angle = i * increment; + bodyDescription.Pose = (position + new Vector3(-MathF.Cos(angle) * radius, ringBoxShape.HalfWidth, MathF.Sin(angle) * radius), + QuaternionEx.Concatenate(QuaternionEx.CreateFromAxisAngle(Vector3.UnitZ, MathF.PI * 0.5f), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, angle + MathF.PI * 0.5f))); + Simulation.Bodies.Add(bodyDescription); } + } - Vector3 CreateRing(Vector3 position, Box ringBoxShape, BodyDescription bodyDescription, float radius, int heightPerPlatformLevel, int platformLevels) + Vector3 CreateRing(Vector3 position, Box ringBoxShape, BodyDescription bodyDescription, float radius, int heightPerPlatformLevel, int platformLevels) + { + for (int platformIndex = 0; platformIndex < platformLevels; ++platformIndex) { - for (int platformIndex = 0; platformIndex < platformLevels; ++platformIndex) - { - var wallOffset = ringBoxShape.HalfLength - ringBoxShape.HalfWidth; - CreateRingWall(position, ringBoxShape, bodyDescription, heightPerPlatformLevel, radius + wallOffset); - CreateRingWall(position, ringBoxShape, bodyDescription, heightPerPlatformLevel, radius - wallOffset); - CreateRingPlatform(position + new Vector3(0, heightPerPlatformLevel * ringBoxShape.Height, 0), ringBoxShape, bodyDescription, radius); - position.Y += heightPerPlatformLevel * ringBoxShape.Height + ringBoxShape.Width; - } - return position; + var wallOffset = ringBoxShape.HalfLength - ringBoxShape.HalfWidth; + CreateRingWall(position, ringBoxShape, bodyDescription, heightPerPlatformLevel, radius + wallOffset); + CreateRingWall(position, ringBoxShape, bodyDescription, heightPerPlatformLevel, radius - wallOffset); + CreateRingPlatform(position + new Vector3(0, heightPerPlatformLevel * ringBoxShape.Height, 0), ringBoxShape, bodyDescription, radius); + position.Y += heightPerPlatformLevel * ringBoxShape.Height + ringBoxShape.Width; } + return position; + } + + CharacterControllers characters; + public override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(-30, 40, -30); + camera.Yaw = MathHelper.Pi * 3f / 4; + camera.Pitch = MathHelper.Pi * 0.2f; + + characters = new CharacterControllers(BufferPool); + Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); + + var ringBoxShape = new Box(0.5f, 1.5f, 3); + var boxDescription = BodyDescription.CreateDynamic(new Vector3(), ringBoxShape.ComputeInertia(1), Simulation.Shapes.Add(ringBoxShape), 0.01f); - CharacterControllers characters; - public override void Initialize(ContentArchive content, Camera camera) + var layerPosition = new Vector3(); + const int layerCount = 10; + var innerRadius = 5f; + var heightPerPlatform = 2; + var platformsPerLayer = 1; + var ringSpacing = 0.5f; + for (int layerIndex = 0; layerIndex < layerCount; ++layerIndex) { - camera.Position = new Vector3(-30, 40, -30); - camera.Yaw = MathHelper.Pi * 3f / 4; - camera.Pitch = MathHelper.Pi * 0.2f; - - characters = new CharacterControllers(BufferPool); - Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); - - var ringBoxShape = new Box(0.5f, 1.5f, 3); - var boxDescription = BodyDescription.CreateDynamic(new Vector3(), ringBoxShape.ComputeInertia(1), Simulation.Shapes.Add(ringBoxShape), 0.01f); - - var layerPosition = new Vector3(); - const int layerCount = 10; - var innerRadius = 5f; - var heightPerPlatform = 2; - var platformsPerLayer = 1; - var ringSpacing = 0.5f; - for (int layerIndex = 0; layerIndex < layerCount; ++layerIndex) + var ringCount = layerCount - layerIndex; + for (int ringIndex = 0; ringIndex < ringCount; ++ringIndex) { - var ringCount = layerCount - layerIndex; - for (int ringIndex = 0; ringIndex < ringCount; ++ringIndex) - { - CreateRing(layerPosition, ringBoxShape, boxDescription, innerRadius + ringIndex * (ringBoxShape.Length + ringSpacing) + layerIndex * (ringBoxShape.Length - ringBoxShape.Width), heightPerPlatform, platformsPerLayer); - } - layerPosition.Y += platformsPerLayer * (ringBoxShape.Height * heightPerPlatform + ringBoxShape.Width); + CreateRing(layerPosition, ringBoxShape, boxDescription, innerRadius + ringIndex * (ringBoxShape.Length + ringSpacing) + layerIndex * (ringBoxShape.Length - ringBoxShape.Width), heightPerPlatform, platformsPerLayer); } + layerPosition.Y += platformsPerLayer * (ringBoxShape.Height * heightPerPlatform + ringBoxShape.Width); + } - Console.WriteLine($"box count: {Simulation.Bodies.ActiveSet.Count}"); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), Simulation.Shapes.Add(new Box(500, 1, 500)))); + Console.WriteLine($"box count: {Simulation.Bodies.ActiveSet.Count}"); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), Simulation.Shapes.Add(new Box(500, 1, 500)))); - var bulletShape = new Sphere(0.5f); - bulletDescription = BodyDescription.CreateDynamic(new Vector3(), bulletShape.ComputeInertia(.1f), Simulation.Shapes.Add(bulletShape), 0.01f); + var bulletShape = new Sphere(0.5f); + bulletDescription = BodyDescription.CreateDynamic(new Vector3(), bulletShape.ComputeInertia(.1f), Simulation.Shapes.Add(bulletShape), 0.01f); - var shootiePatootieShape = new Sphere(3f); - shootiePatootieDescription = BodyDescription.CreateDynamic(new Vector3(), shootiePatootieShape.ComputeInertia(1000), new (Simulation.Shapes.Add(shootiePatootieShape), 0.1f), 0.01f); - } + var shootiePatootieShape = new Sphere(3f); + shootiePatootieDescription = BodyDescription.CreateDynamic(new Vector3(), shootiePatootieShape.ComputeInertia(1000), new (Simulation.Shapes.Add(shootiePatootieShape), 0.1f), 0.01f); + } - bool characterActive; - CharacterInput character; - void CreateCharacter(Vector3 position) - { - characterActive = true; - character = new CharacterInput(characters, position, new Capsule(0.5f, 1), 0.1f, 1, 20, 100, 6, 4, MathF.PI * 0.4f); - } + bool characterActive; + CharacterInput character; + void CreateCharacter(Vector3 position) + { + characterActive = true; + character = new CharacterInput(characters, position, new Capsule(0.5f, 1), 0.1f, 1, 20, 100, 6, 4, MathF.PI * 0.4f); + } - BodyDescription bulletDescription; - BodyDescription shootiePatootieDescription; - public override void Update(Window window, Camera camera, Input input, float dt) + BodyDescription bulletDescription; + BodyDescription shootiePatootieDescription; + public override void Update(Window window, Camera camera, Input input, float dt) + { + if (input != null) { - if (input != null) + if (input.WasPushed(Key.C)) { - if (input.WasPushed(Key.C)) - { - if (characterActive) - { - character.Dispose(); - characterActive = false; - } - else - { - CreateCharacter(camera.Position); - } - } if (characterActive) { - character.UpdateCharacterGoals(input, camera, Demo.TimestepDuration); - } - - if (input.WasPushed(Key.Z)) - { - bulletDescription.Pose.Position = camera.Position; - bulletDescription.Velocity.Linear = camera.GetRayDirection(input.MouseLocked, window.GetNormalizedMousePosition(input.MousePosition)) * 400; - Simulation.Bodies.Add(bulletDescription); + character.Dispose(); + characterActive = false; } - else if (input.WasPushed(Key.X)) + else { - shootiePatootieDescription.Pose.Position = camera.Position; - shootiePatootieDescription.Velocity.Linear = camera.GetRayDirection(input.MouseLocked, window.GetNormalizedMousePosition(input.MousePosition)) * 100; - Simulation.Bodies.Add(shootiePatootieDescription); + CreateCharacter(camera.Position); } } - base.Update(window, camera, input, dt); - } - - public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) - { if (characterActive) { - character.UpdateCameraPosition(camera, 0); + character.UpdateCharacterGoals(input, camera, Demo.TimestepDuration); + } + + if (input.WasPushed(Key.Z)) + { + bulletDescription.Pose.Position = camera.Position; + bulletDescription.Velocity.Linear = camera.GetRayDirection(input.MouseLocked, window.GetNormalizedMousePosition(input.MousePosition)) * 400; + Simulation.Bodies.Add(bulletDescription); + } + else if (input.WasPushed(Key.X)) + { + shootiePatootieDescription.Pose.Position = camera.Position; + shootiePatootieDescription.Velocity.Linear = camera.GetRayDirection(input.MouseLocked, window.GetNormalizedMousePosition(input.MousePosition)) * 100; + Simulation.Bodies.Add(shootiePatootieDescription); } - //text.Clear().Append("Press Z to shoot a bullet, press X to super shootie patootie!"); - //renderer.TextBatcher.Write(text, new Vector2(20, renderer.Surface.Resolution.Y - 20), 16, new Vector3(1, 1, 1), font); - base.Render(renderer, camera, input, text, font); } + base.Update(window, camera, input, dt); + } + + public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + { + if (characterActive) + { + character.UpdateCameraPosition(camera, 0); + } + //text.Clear().Append("Press Z to shoot a bullet, press X to super shootie patootie!"); + //renderer.TextBatcher.Write(text, new Vector2(20, renderer.Surface.Resolution.Y - 20), 16, new Vector3(1, 1, 1), font); + base.Render(renderer, camera, input, text, font); } } diff --git a/Demos/SpecializedTests/Media/2.0/NewtDemandingSacrificeVideoDemo.cs b/Demos/SpecializedTests/Media/2.0/NewtDemandingSacrificeVideoDemo.cs index 84461b6c7..88cada604 100644 --- a/Demos/SpecializedTests/Media/2.0/NewtDemandingSacrificeVideoDemo.cs +++ b/Demos/SpecializedTests/Media/2.0/NewtDemandingSacrificeVideoDemo.cs @@ -8,71 +8,70 @@ using System; using System.Numerics; -namespace Demos.SpecializedTests.Media +namespace Demos.SpecializedTests.Media; + +public class NewtDemandingSacrificeVideoDemo : Demo { - public class NewtDemandingSacrificeVideoDemo : Demo + CollidableProperty filters; + public override void Initialize(ContentArchive content, Camera camera) { - CollidableProperty filters; - public override void Initialize(ContentArchive content, Camera camera) - { - camera.Position = new Vector3(-32f, 20.5f, 61f); - camera.Yaw = MathHelper.Pi * 0.3f; - camera.Pitch = MathHelper.Pi * -0.05f; + camera.Position = new Vector3(-32f, 20.5f, 61f); + camera.Yaw = MathHelper.Pi * 0.3f; + camera.Pitch = MathHelper.Pi * -0.05f; - filters = new CollidableProperty(BufferPool); - Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks(filters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(4, 1)); + filters = new CollidableProperty(BufferPool); + Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks(filters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(4, 1)); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), Simulation.Shapes.Add(new Box(1500, 1, 1500)))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, 10, 0), Simulation.Shapes.Add(new Box(70, 20, 80)))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, 7.5f, 0), Simulation.Shapes.Add(new Box(80, 15, 90)))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, 5, 0), Simulation.Shapes.Add(new Box(90, 10, 100)))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, 2.5f, 0), Simulation.Shapes.Add(new Box(100, 5, 110)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), Simulation.Shapes.Add(new Box(1500, 1, 1500)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 10, 0), Simulation.Shapes.Add(new Box(70, 20, 80)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 7.5f, 0), Simulation.Shapes.Add(new Box(80, 15, 90)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 5, 0), Simulation.Shapes.Add(new Box(90, 10, 100)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 2.5f, 0), Simulation.Shapes.Add(new Box(100, 5, 110)))); - //High fidelity simulation isn't super important on this one. - Simulation.Solver.VelocityIterationCount = 2; + //High fidelity simulation isn't super important on this one. + Simulation.Solver.VelocityIterationCount = 2; - var mesh = DemoMeshHelper.LoadModel(content, BufferPool, "Content\\newt.obj", new Vector3(30)); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, 20, 0), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, 0), Simulation.Shapes.Add(mesh))); - } + var mesh = DemoMeshHelper.LoadModel(content, BufferPool, "Content\\newt.obj", new Vector3(30)); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 20, 0), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, 0), Simulation.Shapes.Add(mesh))); + } - Random random = new Random(5); - int ragdollIndex = 0; + Random random = new Random(5); + int ragdollIndex = 0; - BodyVelocity GetRandomizedVelocity(Vector3 linearVelocity) - { - return new BodyVelocity { Linear = linearVelocity, Angular = new Vector3(-20) + 40 * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) }; - } + BodyVelocity GetRandomizedVelocity(Vector3 linearVelocity) + { + return new BodyVelocity { Linear = linearVelocity, Angular = new Vector3(-20) + 40 * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) }; + } - public override void Update(Window window, Camera camera, Input input, float dt) + public override void Update(Window window, Camera camera, Input input, float dt) + { + var pose = TestHelpers.CreateRandomPose(random, new BoundingBox { - var pose = TestHelpers.CreateRandomPose(random, new BoundingBox - { - Min = new Vector3(-10, 5, 70), - Max = new Vector3(10, 15, 70) - }); - var linearVelocity = Vector3.Normalize(new Vector3(-2 + 4 * random.NextSingle(), 31 + 4 * random.NextSingle(), 50) - pose.Position) * 40; - var handles = RagdollDemo.AddRagdoll(pose.Position, pose.Orientation, ragdollIndex++, filters, Simulation); - var bodies = Simulation.Bodies; - //This could be done better, but... ... .... .......... - bodies[handles.Hips].Velocity = GetRandomizedVelocity(linearVelocity); - bodies[handles.Abdomen].Velocity = GetRandomizedVelocity(linearVelocity); - bodies[handles.Chest].Velocity = GetRandomizedVelocity(linearVelocity); - bodies[handles.Head].Velocity = GetRandomizedVelocity(linearVelocity); - bodies[handles.LeftArm.UpperArm].Velocity = GetRandomizedVelocity(linearVelocity); - bodies[handles.LeftArm.LowerArm].Velocity = GetRandomizedVelocity(linearVelocity); - bodies[handles.LeftArm.Hand].Velocity = GetRandomizedVelocity(linearVelocity); - bodies[handles.RightArm.UpperArm].Velocity = GetRandomizedVelocity(linearVelocity); - bodies[handles.RightArm.LowerArm].Velocity = GetRandomizedVelocity(linearVelocity); - bodies[handles.RightArm.Hand].Velocity = GetRandomizedVelocity(linearVelocity); - bodies[handles.LeftLeg.UpperLeg].Velocity = GetRandomizedVelocity(linearVelocity); - bodies[handles.LeftLeg.LowerLeg].Velocity = GetRandomizedVelocity(linearVelocity); - bodies[handles.LeftLeg.Foot].Velocity = GetRandomizedVelocity(linearVelocity); - bodies[handles.RightLeg.UpperLeg].Velocity = GetRandomizedVelocity(linearVelocity); - bodies[handles.RightLeg.LowerLeg].Velocity = GetRandomizedVelocity(linearVelocity); - bodies[handles.RightLeg.Foot].Velocity = GetRandomizedVelocity(linearVelocity); - - base.Update(window, camera, input, dt); - } + Min = new Vector3(-10, 5, 70), + Max = new Vector3(10, 15, 70) + }); + var linearVelocity = Vector3.Normalize(new Vector3(-2 + 4 * random.NextSingle(), 31 + 4 * random.NextSingle(), 50) - pose.Position) * 40; + var handles = RagdollDemo.AddRagdoll(pose.Position, pose.Orientation, ragdollIndex++, filters, Simulation); + var bodies = Simulation.Bodies; + //This could be done better, but... ... .... .......... + bodies[handles.Hips].Velocity = GetRandomizedVelocity(linearVelocity); + bodies[handles.Abdomen].Velocity = GetRandomizedVelocity(linearVelocity); + bodies[handles.Chest].Velocity = GetRandomizedVelocity(linearVelocity); + bodies[handles.Head].Velocity = GetRandomizedVelocity(linearVelocity); + bodies[handles.LeftArm.UpperArm].Velocity = GetRandomizedVelocity(linearVelocity); + bodies[handles.LeftArm.LowerArm].Velocity = GetRandomizedVelocity(linearVelocity); + bodies[handles.LeftArm.Hand].Velocity = GetRandomizedVelocity(linearVelocity); + bodies[handles.RightArm.UpperArm].Velocity = GetRandomizedVelocity(linearVelocity); + bodies[handles.RightArm.LowerArm].Velocity = GetRandomizedVelocity(linearVelocity); + bodies[handles.RightArm.Hand].Velocity = GetRandomizedVelocity(linearVelocity); + bodies[handles.LeftLeg.UpperLeg].Velocity = GetRandomizedVelocity(linearVelocity); + bodies[handles.LeftLeg.LowerLeg].Velocity = GetRandomizedVelocity(linearVelocity); + bodies[handles.LeftLeg.Foot].Velocity = GetRandomizedVelocity(linearVelocity); + bodies[handles.RightLeg.UpperLeg].Velocity = GetRandomizedVelocity(linearVelocity); + bodies[handles.RightLeg.LowerLeg].Velocity = GetRandomizedVelocity(linearVelocity); + bodies[handles.RightLeg.Foot].Velocity = GetRandomizedVelocity(linearVelocity); + base.Update(window, camera, input, dt); } + } diff --git a/Demos/SpecializedTests/Media/2.0/NewtVideoDemo.cs b/Demos/SpecializedTests/Media/2.0/NewtVideoDemo.cs index 720a1d27d..9ebfdb1e0 100644 --- a/Demos/SpecializedTests/Media/2.0/NewtVideoDemo.cs +++ b/Demos/SpecializedTests/Media/2.0/NewtVideoDemo.cs @@ -9,58 +9,57 @@ using System; using System.Numerics; -namespace Demos.SpecializedTests.Media +namespace Demos.SpecializedTests.Media; + +public class NewtVideoDemo : Demo { - public class NewtVideoDemo : Demo + public override void Initialize(ContentArchive content, Camera camera) { - public override void Initialize(ContentArchive content, Camera camera) - { - camera.Position = new Vector3(-5f, 5.5f, 5f); - camera.Yaw = MathHelper.Pi / 4; - camera.Pitch = MathHelper.Pi * 0.15f; + camera.Position = new Vector3(-5f, 5.5f, 5f); + camera.Yaw = MathHelper.Pi / 4; + camera.Pitch = MathHelper.Pi * 0.15f; - var filters = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new DeformableCallbacks(filters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); + var filters = new CollidableProperty(); + Simulation = Simulation.Create(BufferPool, new DeformableCallbacks(filters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); - var meshContent = content.Load("Content\\newt.obj"); - float cellSize = 0.1f; - DumbTetrahedralizer.Tetrahedralize(meshContent.Triangles, cellSize, BufferPool, - out var vertices, out var vertexSpatialIndices, out var cellVertexIndices, out var tetrahedraVertexIndices); - var weldSpringiness = new SpringSettings(30f, 0); - var volumeSpringiness = new SpringSettings(30f, 1); - for (int i = 0; i < 5; ++i) - { - NewtDemo.CreateDeformable(Simulation, new Vector3(i * 3, 5 + i * 1.5f, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), MathF.PI * (i * 0.55f)), 1f, cellSize, weldSpringiness, volumeSpringiness, i, filters, ref vertices, ref vertexSpatialIndices, ref cellVertexIndices, ref tetrahedraVertexIndices); - } + var meshContent = content.Load("Content\\newt.obj"); + float cellSize = 0.1f; + DumbTetrahedralizer.Tetrahedralize(meshContent.Triangles, cellSize, BufferPool, + out var vertices, out var vertexSpatialIndices, out var cellVertexIndices, out var tetrahedraVertexIndices); + var weldSpringiness = new SpringSettings(30f, 0); + var volumeSpringiness = new SpringSettings(30f, 1); + for (int i = 0; i < 5; ++i) + { + NewtDemo.CreateDeformable(Simulation, new Vector3(i * 3, 5 + i * 1.5f, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), MathF.PI * (i * 0.55f)), 1f, cellSize, weldSpringiness, volumeSpringiness, i, filters, ref vertices, ref vertexSpatialIndices, ref cellVertexIndices, ref tetrahedraVertexIndices); + } - BufferPool.Return(ref vertices); - vertexSpatialIndices.Dispose(BufferPool); - BufferPool.Return(ref cellVertexIndices); - BufferPool.Return(ref tetrahedraVertexIndices); + BufferPool.Return(ref vertices); + vertexSpatialIndices.Dispose(BufferPool); + BufferPool.Return(ref cellVertexIndices); + BufferPool.Return(ref tetrahedraVertexIndices); - Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 100, -.5f), 10, Simulation.Shapes, new Sphere(5))); + Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 100, -.5f), 10, Simulation.Shapes, new Sphere(5))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), Simulation.Shapes.Add(new Box(1500, 1, 1500)))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -1.5f, 0), Simulation.Shapes.Add(new Sphere(3)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), Simulation.Shapes.Add(new Box(1500, 1, 1500)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -1.5f, 0), Simulation.Shapes.Add(new Sphere(3)))); - var bulletShape = new Sphere(0.5f); - bulletDescription = BodyDescription.CreateDynamic(RigidPose.Identity, bulletShape.ComputeInertia(.25f), Simulation.Shapes.Add(bulletShape), 0.01f); + var bulletShape = new Sphere(0.5f); + bulletDescription = BodyDescription.CreateDynamic(RigidPose.Identity, bulletShape.ComputeInertia(.25f), Simulation.Shapes.Add(bulletShape), 0.01f); - var mesh = DemoMeshHelper.LoadModel(content, BufferPool, "Content\\newt.obj", new Vector3(20)); - Simulation.Statics.Add(new StaticDescription(new Vector3(200, 0.5f, 120), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, -3 * MathHelper.PiOver4), Simulation.Shapes.Add(mesh))); - } - BodyDescription bulletDescription; - public override void Update(Window window, Camera camera, Input input, float dt) + var mesh = DemoMeshHelper.LoadModel(content, BufferPool, "Content\\newt.obj", new Vector3(20)); + Simulation.Statics.Add(new StaticDescription(new Vector3(200, 0.5f, 120), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, -3 * MathHelper.PiOver4), Simulation.Shapes.Add(mesh))); + } + BodyDescription bulletDescription; + public override void Update(Window window, Camera camera, Input input, float dt) + { + if (input.WasPushed(OpenTK.Input.Key.Z)) { - if (input.WasPushed(OpenTK.Input.Key.Z)) - { - bulletDescription.Pose.Position = camera.Position; - bulletDescription.Velocity.Linear = camera.Forward * 40; - Simulation.Bodies.Add(bulletDescription); - } - base.Update(window, camera, input, dt); + bulletDescription.Pose.Position = camera.Position; + bulletDescription.Velocity.Linear = camera.Forward * 40; + Simulation.Bodies.Add(bulletDescription); } + base.Update(window, camera, input, dt); + } - } } diff --git a/Demos/SpecializedTests/Media/2.0/PyramidVideoDemo.cs b/Demos/SpecializedTests/Media/2.0/PyramidVideoDemo.cs index 73c2a9831..833e5e1a5 100644 --- a/Demos/SpecializedTests/Media/2.0/PyramidVideoDemo.cs +++ b/Demos/SpecializedTests/Media/2.0/PyramidVideoDemo.cs @@ -9,74 +9,73 @@ using System; using System.Numerics; -namespace Demos.Demos.Media +namespace Demos.Demos.Media; + +/// +/// A pyramid of boxes, because you can't have a physics engine without pyramids of boxes. +/// +public class PyramidVideoDemo : Demo { - /// - /// A pyramid of boxes, because you can't have a physics engine without pyramids of boxes. - /// - public class PyramidVideoDemo : Demo + public override void Initialize(ContentArchive content, Camera camera) { - public override void Initialize(ContentArchive content, Camera camera) - { - camera.Position = new Vector3(-70, 8, 318); - camera.Yaw = MathHelper.Pi * 1f / 4; - camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); + camera.Position = new Vector3(-70, 8, 318); + camera.Yaw = MathHelper.Pi * 1f / 4; + camera.Pitch = 0; + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); - var boxShape = new Box(1, 1, 1); - var boxInertia = boxShape.ComputeInertia(1); - var boxIndex = Simulation.Shapes.Add(boxShape); - const int pyramidCount = 120; - for (int pyramidIndex = 0; pyramidIndex < pyramidCount; ++pyramidIndex) + var boxShape = new Box(1, 1, 1); + var boxInertia = boxShape.ComputeInertia(1); + var boxIndex = Simulation.Shapes.Add(boxShape); + const int pyramidCount = 120; + for (int pyramidIndex = 0; pyramidIndex < pyramidCount; ++pyramidIndex) + { + const int rowCount = 20; + for (int rowIndex = 0; rowIndex < rowCount; ++rowIndex) { - const int rowCount = 20; - for (int rowIndex = 0; rowIndex < rowCount; ++rowIndex) + int columnCount = rowCount - rowIndex; + for (int columnIndex = 0; columnIndex < columnCount; ++columnIndex) { - int columnCount = rowCount - rowIndex; - for (int columnIndex = 0; columnIndex < columnCount; ++columnIndex) - { - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3( - (-columnCount * 0.5f + columnIndex) * boxShape.Width, - (rowIndex + 0.5f) * boxShape.Height, - (pyramidIndex - pyramidCount * 0.5f) * (boxShape.Length + 4)), - boxInertia, boxIndex, 0.01f)); - } + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3( + (-columnCount * 0.5f + columnIndex) * boxShape.Width, + (rowIndex + 0.5f) * boxShape.Height, + (pyramidIndex - pyramidCount * 0.5f) * (boxShape.Length + 4)), + boxInertia, boxIndex, 0.01f)); } } - - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), Simulation.Shapes.Add(new Box(2500, 1, 2500)))); } - //We'll randomize the size of bullets. - Random random = new Random(5); - public override void Update(Window window, Camera camera, Input input, float dt) - { - if (input != null && input.WasPushed(OpenTK.Input.Key.Z)) - { - //Create the shape that we'll launch at the pyramids when the user presses a button. - var bulletShape = new Sphere(6); - //Note that the use of radius^3 for mass can produce some pretty serious mass ratios. - //Observe what happens when a large ball sits on top of a few boxes with a fraction of the mass- - //the collision appears much squishier and less stable. For most games, if you want to maintain rigidity, you'll want to use some combination of: - //1) Limit the ratio of heavy object masses to light object masses when those heavy objects depend on the light objects. - //2) Use a shorter timestep duration and update more frequently. - //3) Use a greater number of solver iterations. - //#2 and #3 can become very expensive. In pathological cases, it can end up slower than using a quality-focused solver for the same simulation. - //Unfortunately, at the moment, bepuphysics v2 does not contain any alternative solvers, so if you can't afford to brute force the the problem away, - //the best solution is to cheat as much as possible to avoid the corner cases. - var bodyDescription = BodyDescription.CreateConvexDynamic( - new Vector3(0, 8, -500), new Vector3(0, 0, 110), 50000, Simulation.Shapes, bulletShape); - Simulation.Bodies.Add(bodyDescription); - } - base.Update(window, camera, input, dt); - } + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), Simulation.Shapes.Add(new Box(2500, 1, 2500)))); + } - public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + //We'll randomize the size of bullets. + Random random = new Random(5); + public override void Update(Window window, Camera camera, Input input, float dt) + { + if (input != null && input.WasPushed(OpenTK.Input.Key.Z)) { - text.Clear().Append("Press Z to launch a ball!"); - renderer.TextBatcher.Write(text, new Vector2(20, renderer.Surface.Resolution.Y - 20), 16, new Vector3(1, 1, 1), font); - base.Render(renderer, camera, input, text, font); + //Create the shape that we'll launch at the pyramids when the user presses a button. + var bulletShape = new Sphere(6); + //Note that the use of radius^3 for mass can produce some pretty serious mass ratios. + //Observe what happens when a large ball sits on top of a few boxes with a fraction of the mass- + //the collision appears much squishier and less stable. For most games, if you want to maintain rigidity, you'll want to use some combination of: + //1) Limit the ratio of heavy object masses to light object masses when those heavy objects depend on the light objects. + //2) Use a shorter timestep duration and update more frequently. + //3) Use a greater number of solver iterations. + //#2 and #3 can become very expensive. In pathological cases, it can end up slower than using a quality-focused solver for the same simulation. + //Unfortunately, at the moment, bepuphysics v2 does not contain any alternative solvers, so if you can't afford to brute force the the problem away, + //the best solution is to cheat as much as possible to avoid the corner cases. + var bodyDescription = BodyDescription.CreateConvexDynamic( + new Vector3(0, 8, -500), new Vector3(0, 0, 110), 50000, Simulation.Shapes, bulletShape); + Simulation.Bodies.Add(bodyDescription); } + base.Update(window, camera, input, dt); + } + public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + { + text.Clear().Append("Press Z to launch a ball!"); + renderer.TextBatcher.Write(text, new Vector2(20, renderer.Surface.Resolution.Y - 20), 16, new Vector3(1, 1, 1), font); + base.Render(renderer, camera, input, text, font); } + } diff --git a/Demos/SpecializedTests/Media/2.0/ShrinkwrappedNewtsVideoDemo.cs b/Demos/SpecializedTests/Media/2.0/ShrinkwrappedNewtsVideoDemo.cs index a3c23ddbb..60ccddb87 100644 --- a/Demos/SpecializedTests/Media/2.0/ShrinkwrappedNewtsVideoDemo.cs +++ b/Demos/SpecializedTests/Media/2.0/ShrinkwrappedNewtsVideoDemo.cs @@ -9,63 +9,62 @@ using System; using System.Numerics; -namespace Demos.SpecializedTests.Media +namespace Demos.SpecializedTests.Media; + +public class ShrinkwrappedNewtsVideoDemo : Demo { - public class ShrinkwrappedNewtsVideoDemo : Demo + public override void Initialize(ContentArchive content, Camera camera) { - public override void Initialize(ContentArchive content, Camera camera) - { - camera.Position = new Vector3(25f, 1.5f, 15f); - camera.Yaw = 3 * MathHelper.Pi / 4; - camera.Pitch = 0;// MathHelper.Pi * 0.15f; + camera.Position = new Vector3(25f, 1.5f, 15f); + camera.Yaw = 3 * MathHelper.Pi / 4; + camera.Pitch = 0;// MathHelper.Pi * 0.15f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); - var meshContent = content.Load("Content\\newt.obj"); + var meshContent = content.Load("Content\\newt.obj"); - //This is actually a pretty good example of how *not* to make a convex hull shape. - //Generating it directly from a graphical data source tends to have way more surface complexity than needed, - //and it tends to have a lot of near-but-not-quite-coplanar surfaces which can make the contact manifold less stable. - //Prefer a simpler source with more distinct features, possibly created with an automated content-time tool. - var points = new QuickList(meshContent.Triangles.Length * 3, BufferPool); - for (int i = 0; i < meshContent.Triangles.Length; ++i) - { - ref var triangle = ref meshContent.Triangles[i]; - //resisting the urge to just reinterpret the memory - points.AllocateUnsafely() = triangle.A * new Vector3(1, 1.5f, 1); - points.AllocateUnsafely() = triangle.B * new Vector3(1, 1.5f, 1); - points.AllocateUnsafely() = triangle.C * new Vector3(1, 1.5f, 1); - } + //This is actually a pretty good example of how *not* to make a convex hull shape. + //Generating it directly from a graphical data source tends to have way more surface complexity than needed, + //and it tends to have a lot of near-but-not-quite-coplanar surfaces which can make the contact manifold less stable. + //Prefer a simpler source with more distinct features, possibly created with an automated content-time tool. + var points = new QuickList(meshContent.Triangles.Length * 3, BufferPool); + for (int i = 0; i < meshContent.Triangles.Length; ++i) + { + ref var triangle = ref meshContent.Triangles[i]; + //resisting the urge to just reinterpret the memory + points.AllocateUnsafely() = triangle.A * new Vector3(1, 1.5f, 1); + points.AllocateUnsafely() = triangle.B * new Vector3(1, 1.5f, 1); + points.AllocateUnsafely() = triangle.C * new Vector3(1, 1.5f, 1); + } - var newtHull = new ConvexHull(points.Span.Slice(points.Count), BufferPool, out _); - var bodyDescription = BodyDescription.CreateConvexDynamic(RigidPose.Identity, 1, Simulation.Shapes, newtHull); - Random random = new Random(5); - var poseBounds = new BoundingBox { Min = new Vector3(-20, 1, 5), Max = new Vector3(20, 10, 50) }; - for (int i = 0; i < 512; ++i) - { - bodyDescription.Pose = TestHelpers.CreateRandomPose(random, poseBounds); - Simulation.Bodies.Add(bodyDescription); - } + var newtHull = new ConvexHull(points.Span.Slice(points.Count), BufferPool, out _); + var bodyDescription = BodyDescription.CreateConvexDynamic(RigidPose.Identity, 1, Simulation.Shapes, newtHull); + Random random = new Random(5); + var poseBounds = new BoundingBox { Min = new Vector3(-20, 1, 5), Max = new Vector3(20, 10, 50) }; + for (int i = 0; i < 512; ++i) + { + bodyDescription.Pose = TestHelpers.CreateRandomPose(random, poseBounds); + Simulation.Bodies.Add(bodyDescription); + } - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -30, 250), Simulation.Shapes.Add(new Box(1000, 60, 500)))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -60, 0), Simulation.Shapes.Add(new Box(1000, 1, 1000)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -30, 250), Simulation.Shapes.Add(new Box(1000, 60, 500)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -60, 0), Simulation.Shapes.Add(new Box(1000, 1, 1000)))); - mesh = DemoMeshHelper.LoadModel(content, BufferPool, "Content\\newt.obj", new Vector3(1, 1.5f, 1)); - Simulation.Statics.Add(new StaticDescription(new Vector3(30, 0, 20), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, -3 * MathHelper.PiOver4), Simulation.Shapes.Add(mesh))); - } + mesh = DemoMeshHelper.LoadModel(content, BufferPool, "Content\\newt.obj", new Vector3(1, 1.5f, 1)); + Simulation.Statics.Add(new StaticDescription(new Vector3(30, 0, 20), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, -3 * MathHelper.PiOver4), Simulation.Shapes.Add(mesh))); + } - Mesh mesh; + Mesh mesh; - public override void Update(Window window, Camera camera, Input input, float dt) + public override void Update(Window window, Camera camera, Input input, float dt) + { + if(input.WasPushed(OpenTK.Input.Key.Z)) { - if(input.WasPushed(OpenTK.Input.Key.Z)) - { - mesh.Scale = new Vector3(30); - Simulation.Statics.Add(new StaticDescription(new Vector3(70, 0, 50), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, -3.1f * MathHelper.PiOver4), Simulation.Shapes.Add(mesh))); - } - base.Update(window, camera, input, dt); + mesh.Scale = new Vector3(30); + Simulation.Statics.Add(new StaticDescription(new Vector3(70, 0, 50), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, -3.1f * MathHelper.PiOver4), Simulation.Shapes.Add(mesh))); } + base.Update(window, camera, input, dt); } } diff --git a/Demos/SpecializedTests/Media/2.4/Colosseum24VideoDemo.cs b/Demos/SpecializedTests/Media/2.4/Colosseum24VideoDemo.cs index 06a0d4770..a0b7d36c2 100644 --- a/Demos/SpecializedTests/Media/2.4/Colosseum24VideoDemo.cs +++ b/Demos/SpecializedTests/Media/2.4/Colosseum24VideoDemo.cs @@ -8,115 +8,114 @@ using System; using System.Numerics; -namespace Demos.SpecializedTests.Media +namespace Demos.SpecializedTests.Media; + +/// +/// A colosseum made out of boxes that is sometimes hit by large purple hail. +/// +public class Colosseum24VideoDemo : Demo { - /// - /// A colosseum made out of boxes that is sometimes hit by large purple hail. - /// - public class Colosseum24VideoDemo : Demo + public static void CreateRingWall(Simulation simulation, Vector3 position, Box ringBoxShape, BodyDescription bodyDescription, int height, float radius) { - public static void CreateRingWall(Simulation simulation, Vector3 position, Box ringBoxShape, BodyDescription bodyDescription, int height, float radius) + var circumference = MathF.PI * 2 * radius; + var boxCountPerRing = (int)(0.9f * circumference / ringBoxShape.Length); + float increment = MathHelper.TwoPi / boxCountPerRing; + for (int ringIndex = 0; ringIndex < height; ringIndex++) { - var circumference = MathF.PI * 2 * radius; - var boxCountPerRing = (int)(0.9f * circumference / ringBoxShape.Length); - float increment = MathHelper.TwoPi / boxCountPerRing; - for (int ringIndex = 0; ringIndex < height; ringIndex++) + for (int i = 0; i < boxCountPerRing; i++) { - for (int i = 0; i < boxCountPerRing; i++) - { - var angle = ((ringIndex & 1) == 0 ? i + 0.5f : i) * increment; - bodyDescription.Pose = (position + new Vector3(-MathF.Cos(angle) * radius, (ringIndex + 0.5f) * ringBoxShape.Height, MathF.Sin(angle) * radius), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, angle)); - simulation.Bodies.Add(bodyDescription); - } + var angle = ((ringIndex & 1) == 0 ? i + 0.5f : i) * increment; + bodyDescription.Pose = (position + new Vector3(-MathF.Cos(angle) * radius, (ringIndex + 0.5f) * ringBoxShape.Height, MathF.Sin(angle) * radius), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, angle)); + simulation.Bodies.Add(bodyDescription); } } + } - public static void CreateRingPlatform(Simulation simulation, Vector3 position, Box ringBoxShape, BodyDescription bodyDescription, float radius) + public static void CreateRingPlatform(Simulation simulation, Vector3 position, Box ringBoxShape, BodyDescription bodyDescription, float radius) + { + var innerCircumference = MathF.PI * 2 * (radius - ringBoxShape.HalfLength); + var boxCount = (int)(0.95f * innerCircumference / ringBoxShape.Height); + float increment = MathHelper.TwoPi / boxCount; + for (int i = 0; i < boxCount; i++) { - var innerCircumference = MathF.PI * 2 * (radius - ringBoxShape.HalfLength); - var boxCount = (int)(0.95f * innerCircumference / ringBoxShape.Height); - float increment = MathHelper.TwoPi / boxCount; - for (int i = 0; i < boxCount; i++) - { - var angle = i * increment; - bodyDescription.Pose = (position + new Vector3(-MathF.Cos(angle) * radius, ringBoxShape.HalfWidth, MathF.Sin(angle) * radius), - QuaternionEx.Concatenate(QuaternionEx.CreateFromAxisAngle(Vector3.UnitZ, MathF.PI * 0.5f), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, angle + MathF.PI * 0.5f))); - simulation.Bodies.Add(bodyDescription); - } + var angle = i * increment; + bodyDescription.Pose = (position + new Vector3(-MathF.Cos(angle) * radius, ringBoxShape.HalfWidth, MathF.Sin(angle) * radius), + QuaternionEx.Concatenate(QuaternionEx.CreateFromAxisAngle(Vector3.UnitZ, MathF.PI * 0.5f), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, angle + MathF.PI * 0.5f))); + simulation.Bodies.Add(bodyDescription); } + } - public static Vector3 CreateRing(Simulation simulation, Vector3 position, Box ringBoxShape, BodyDescription bodyDescription, float radius, int heightPerPlatformLevel, int platformLevels) + public static Vector3 CreateRing(Simulation simulation, Vector3 position, Box ringBoxShape, BodyDescription bodyDescription, float radius, int heightPerPlatformLevel, int platformLevels) + { + for (int platformIndex = 0; platformIndex < platformLevels; ++platformIndex) { - for (int platformIndex = 0; platformIndex < platformLevels; ++platformIndex) - { - var wallOffset = ringBoxShape.HalfLength - ringBoxShape.HalfWidth; - CreateRingWall(simulation, position, ringBoxShape, bodyDescription, heightPerPlatformLevel, radius + wallOffset); - CreateRingWall(simulation, position, ringBoxShape, bodyDescription, heightPerPlatformLevel, radius - wallOffset); - CreateRingPlatform(simulation, position + new Vector3(0, heightPerPlatformLevel * ringBoxShape.Height, 0), ringBoxShape, bodyDescription, radius); - position.Y += heightPerPlatformLevel * ringBoxShape.Height + ringBoxShape.Width; - } - return position; + var wallOffset = ringBoxShape.HalfLength - ringBoxShape.HalfWidth; + CreateRingWall(simulation, position, ringBoxShape, bodyDescription, heightPerPlatformLevel, radius + wallOffset); + CreateRingWall(simulation, position, ringBoxShape, bodyDescription, heightPerPlatformLevel, radius - wallOffset); + CreateRingPlatform(simulation, position + new Vector3(0, heightPerPlatformLevel * ringBoxShape.Height, 0), ringBoxShape, bodyDescription, radius); + position.Y += heightPerPlatformLevel * ringBoxShape.Height + ringBoxShape.Width; } + return position; + } - public override void Initialize(ContentArchive content, Camera camera) - { - camera.Position = new Vector3(-30, 40, -30); - camera.Yaw = MathHelper.Pi * 3f / 4; - camera.Pitch = MathHelper.Pi * 0.2f; + public override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(-30, 40, -30); + camera.Yaw = MathHelper.Pi * 3f / 4; + camera.Pitch = MathHelper.Pi * 0.2f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(90, 1), maximumRecoveryVelocity: 20), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(2, 7)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(90, 1), maximumRecoveryVelocity: 20), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(2, 7)); - var ringBoxShape = new Box(0.5f, 1, 3); - var boxDescription = BodyDescription.CreateDynamic(new Vector3(), ringBoxShape.ComputeInertia(1), new(Simulation.Shapes.Add(ringBoxShape), 0.1f), -0.01f); + var ringBoxShape = new Box(0.5f, 1, 3); + var boxDescription = BodyDescription.CreateDynamic(new Vector3(), ringBoxShape.ComputeInertia(1), new(Simulation.Shapes.Add(ringBoxShape), 0.1f), -0.01f); - //CreateRingWall(Simulation, default, ringBoxShape, boxDescription, 400, 45); - var layerPosition = new Vector3(); - const int layerCount = 1; - var innerRadius = 20f; - var heightPerPlatform = 10; - var platformsPerLayer = 30; - var ringSpacing = 0.5f; - for (int layerIndex = 0; layerIndex < layerCount; ++layerIndex) + //CreateRingWall(Simulation, default, ringBoxShape, boxDescription, 400, 45); + var layerPosition = new Vector3(); + const int layerCount = 1; + var innerRadius = 20f; + var heightPerPlatform = 10; + var platformsPerLayer = 30; + var ringSpacing = 0.5f; + for (int layerIndex = 0; layerIndex < layerCount; ++layerIndex) + { + var ringCount = layerCount - layerIndex; + for (int ringIndex = 0; ringIndex < ringCount; ++ringIndex) { - var ringCount = layerCount - layerIndex; - for (int ringIndex = 0; ringIndex < ringCount; ++ringIndex) - { - CreateRing(Simulation, layerPosition, ringBoxShape, boxDescription, innerRadius + ringIndex * (ringBoxShape.Length + ringSpacing) + layerIndex * (ringBoxShape.Length - ringBoxShape.Width), heightPerPlatform, platformsPerLayer); - } - layerPosition.Y += platformsPerLayer * (ringBoxShape.Height * heightPerPlatform + ringBoxShape.Width); + CreateRing(Simulation, layerPosition, ringBoxShape, boxDescription, innerRadius + ringIndex * (ringBoxShape.Length + ringSpacing) + layerIndex * (ringBoxShape.Length - ringBoxShape.Width), heightPerPlatform, platformsPerLayer); } + layerPosition.Y += platformsPerLayer * (ringBoxShape.Height * heightPerPlatform + ringBoxShape.Width); + } - Console.WriteLine($"box count: {Simulation.Bodies.ActiveSet.Count}"); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), Simulation.Shapes.Add(new Box(500, 1, 500)))); + Console.WriteLine($"box count: {Simulation.Bodies.ActiveSet.Count}"); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), Simulation.Shapes.Add(new Box(500, 1, 500)))); - var bulletShape = new Sphere(0.5f); - bulletDescription = BodyDescription.CreateDynamic(new Vector3(), bulletShape.ComputeInertia(.1f), Simulation.Shapes.Add(bulletShape), 0.01f); + var bulletShape = new Sphere(0.5f); + bulletDescription = BodyDescription.CreateDynamic(new Vector3(), bulletShape.ComputeInertia(.1f), Simulation.Shapes.Add(bulletShape), 0.01f); - var shootiePatootieShape = new Sphere(3f); - shootiePatootieDescription = BodyDescription.CreateDynamic(new Vector3(), shootiePatootieShape.ComputeInertia(100), new(Simulation.Shapes.Add(shootiePatootieShape), 0.1f), 0.01f); - } + var shootiePatootieShape = new Sphere(3f); + shootiePatootieDescription = BodyDescription.CreateDynamic(new Vector3(), shootiePatootieShape.ComputeInertia(100), new(Simulation.Shapes.Add(shootiePatootieShape), 0.1f), 0.01f); + } - BodyDescription bulletDescription; - BodyDescription shootiePatootieDescription; - public override void Update(Window window, Camera camera, Input input, float dt) + BodyDescription bulletDescription; + BodyDescription shootiePatootieDescription; + public override void Update(Window window, Camera camera, Input input, float dt) + { + if (input != null) { - if (input != null) + if (input.WasPushed(OpenTK.Input.Key.Z)) + { + bulletDescription.Pose.Position = camera.Position; + bulletDescription.Velocity.Linear = camera.GetRayDirection(input.MouseLocked, window.GetNormalizedMousePosition(input.MousePosition)) * 400; + Simulation.Bodies.Add(bulletDescription); + } + else if (input.WasPushed(OpenTK.Input.Key.X)) { - if (input.WasPushed(OpenTK.Input.Key.Z)) - { - bulletDescription.Pose.Position = camera.Position; - bulletDescription.Velocity.Linear = camera.GetRayDirection(input.MouseLocked, window.GetNormalizedMousePosition(input.MousePosition)) * 400; - Simulation.Bodies.Add(bulletDescription); - } - else if (input.WasPushed(OpenTK.Input.Key.X)) - { - shootiePatootieDescription.Pose.Position = camera.Position; - shootiePatootieDescription.Velocity.Linear = camera.GetRayDirection(input.MouseLocked, window.GetNormalizedMousePosition(input.MousePosition)) * 100; - Simulation.Bodies.Add(shootiePatootieDescription); - } + shootiePatootieDescription.Pose.Position = camera.Position; + shootiePatootieDescription.Velocity.Linear = camera.GetRayDirection(input.MouseLocked, window.GetNormalizedMousePosition(input.MousePosition)) * 100; + Simulation.Bodies.Add(shootiePatootieDescription); } - base.Update(window, camera, input, dt); } - + base.Update(window, camera, input, dt); } + } diff --git a/Demos/SpecializedTests/Media/2.4/ExcessivePyramidVideoDemo.cs b/Demos/SpecializedTests/Media/2.4/ExcessivePyramidVideoDemo.cs index 2b61c28b1..e61d9227d 100644 --- a/Demos/SpecializedTests/Media/2.4/ExcessivePyramidVideoDemo.cs +++ b/Demos/SpecializedTests/Media/2.4/ExcessivePyramidVideoDemo.cs @@ -8,57 +8,56 @@ using System; using System.Numerics; -namespace Demos.SpecializedTests.Media +namespace Demos.SpecializedTests.Media; + +/// +/// A pyramid of boxes, because you can't have a physics engine without pyramids of boxes. +/// +public class ExcessivePyramidVideoDemo : Demo { - /// - /// A pyramid of boxes, because you can't have a physics engine without pyramids of boxes. - /// - public class ExcessivePyramidVideoDemo : Demo + public override void Initialize(ContentArchive content, Camera camera) { - public override void Initialize(ContentArchive content, Camera camera) - { - camera.Position = new Vector3(-120, 32, 1045); - camera.Yaw = MathHelper.Pi * 1f / 4; - camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1), frictionCoefficient: 2), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); + camera.Position = new Vector3(-120, 32, 1045); + camera.Yaw = MathHelper.Pi * 1f / 4; + camera.Pitch = 0; + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1), frictionCoefficient: 2), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); - var boxShape = new Box(1, 1, 1); - var boxInertia = boxShape.ComputeInertia(1); - var boxIndex = Simulation.Shapes.Add(boxShape); - const int pyramidCount = 420; - for (int pyramidIndex = 0; pyramidIndex < pyramidCount; ++pyramidIndex) + var boxShape = new Box(1, 1, 1); + var boxInertia = boxShape.ComputeInertia(1); + var boxIndex = Simulation.Shapes.Add(boxShape); + const int pyramidCount = 420; + for (int pyramidIndex = 0; pyramidIndex < pyramidCount; ++pyramidIndex) + { + const int rowCount = 20; + for (int rowIndex = 0; rowIndex < rowCount; ++rowIndex) { - const int rowCount = 20; - for (int rowIndex = 0; rowIndex < rowCount; ++rowIndex) + int columnCount = rowCount - rowIndex; + for (int columnIndex = 0; columnIndex < columnCount; ++columnIndex) { - int columnCount = rowCount - rowIndex; - for (int columnIndex = 0; columnIndex < columnCount; ++columnIndex) - { - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3( - (-columnCount * 0.5f + columnIndex) * boxShape.Width, - (rowIndex + 0.5f) * boxShape.Height, - (pyramidIndex - pyramidCount * 0.5f) * (boxShape.Length + 4)), - boxInertia, new CollidableDescription(boxIndex, 0.1f), 0.01f)); - } + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3( + (-columnCount * 0.5f + columnIndex) * boxShape.Width, + (rowIndex + 0.5f) * boxShape.Height, + (pyramidIndex - pyramidCount * 0.5f) * (boxShape.Length + 4)), + boxInertia, new CollidableDescription(boxIndex, 0.1f), 0.01f)); } } - Console.WriteLine($"bodies count: {Simulation.Bodies.ActiveSet.Count}"); - - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), Simulation.Shapes.Add(new Box(2500, 1, 2500)))); } + Console.WriteLine($"bodies count: {Simulation.Bodies.ActiveSet.Count}"); - int frameCount; - public override void Update(Window window, Camera camera, Input input, float dt) + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), Simulation.Shapes.Add(new Box(2500, 1, 2500)))); + } + + int frameCount; + public override void Update(Window window, Camera camera, Input input, float dt) + { + ++frameCount; + if (frameCount == 128 || (input != null && input.WasPushed(OpenTK.Input.Key.Z))) { - ++frameCount; - if (frameCount == 128 || (input != null && input.WasPushed(OpenTK.Input.Key.Z))) - { - var bulletShape = new Sphere(6); - var bodyDescription = BodyDescription.CreateDynamic( - new Vector3(0, 8, -1200), new Vector3(0, 0, 230), bulletShape.ComputeInertia(5000000), new(Simulation.Shapes.Add(bulletShape), 0.1f), 0.01f); - Simulation.Bodies.Add(bodyDescription); - } - base.Update(window, camera, input, dt); + var bulletShape = new Sphere(6); + var bodyDescription = BodyDescription.CreateDynamic( + new Vector3(0, 8, -1200), new Vector3(0, 0, 230), bulletShape.ComputeInertia(5000000), new(Simulation.Shapes.Add(bulletShape), 0.1f), 0.01f); + Simulation.Bodies.Add(bodyDescription); } + base.Update(window, camera, input, dt); } } diff --git a/Demos/SpecializedTests/Media/2.4/NewtTyrannyDemo.cs b/Demos/SpecializedTests/Media/2.4/NewtTyrannyDemo.cs index 95e31965f..2e8861c59 100644 --- a/Demos/SpecializedTests/Media/2.4/NewtTyrannyDemo.cs +++ b/Demos/SpecializedTests/Media/2.4/NewtTyrannyDemo.cs @@ -8,104 +8,103 @@ using BepuPhysics.Collidables; using Demos.Demos.Characters; -namespace Demos.Demos.Sponsors +namespace Demos.Demos.Sponsors; + +public class NewtTyrannyDemo : Demo { - public class NewtTyrannyDemo : Demo + QuickList newts; + + Vector2 newtArenaMin, newtArenaMax; + Random random; + CharacterControllers characterControllers; + QuickList characterAIs; + public override void Initialize(ContentArchive content, Camera camera) { - QuickList newts; + camera.Position = new Vector3(130, 50, 130); + camera.Yaw = -MathF.PI * 0.25f; + camera.Pitch = 0.4f; + + characterControllers = new CharacterControllers(BufferPool); + Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characterControllers), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); + Simulation.Deterministic = true; + + var newtMesh = DemoMeshHelper.LoadModel(content, BufferPool, @"Content\newt.obj", new Vector3(-10, 10, -10)); + var newtShape = Simulation.Shapes.Add(newtMesh); + var newtCount = 10; + newts = new QuickList(newtCount, BufferPool); + newtArenaMin = new Vector2(-250); + newtArenaMax = new Vector2(250); + random = new Random(8); + for (int i = 0; i < newtCount; ++i) + { + ref var newt = ref newts.AllocateUnsafely(); + newt = new SponsorNewt(Simulation, newtShape, 0, newtArenaMin, newtArenaMax, random, i); + } - Vector2 newtArenaMin, newtArenaMax; - Random random; - CharacterControllers characterControllers; - QuickList characterAIs; - public override void Initialize(ContentArchive content, Camera camera) + const float floorSize = 520; + const float wallThickness = 200; + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -10f, 0), Simulation.Shapes.Add(new Box(floorSize, 20, floorSize)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(floorSize * -0.5f - wallThickness * 0.5f, -5, 0), Simulation.Shapes.Add(new Box(wallThickness, 30, floorSize + wallThickness * 2)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(floorSize * 0.5f + wallThickness * 0.5f, -5, 0), Simulation.Shapes.Add(new Box(wallThickness, 30, floorSize + wallThickness * 2)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5, floorSize * -0.5f - wallThickness * 0.5f), Simulation.Shapes.Add(new Box(floorSize, 30, wallThickness)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5, floorSize * 0.5f + wallThickness * 0.5f), Simulation.Shapes.Add(new Box(floorSize, 30, wallThickness)))); + + const int characterCount = 2000; + characterAIs = new QuickList(characterCount, BufferPool); + var characterCollidable = Simulation.Shapes.Add(new Capsule(0.5f, 1f)); + for (int i = 0; i < characterCount; ++i) { - camera.Position = new Vector3(130, 50, 130); - camera.Yaw = -MathF.PI * 0.25f; - camera.Pitch = 0.4f; - - characterControllers = new CharacterControllers(BufferPool); - Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characterControllers), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); - Simulation.Deterministic = true; - - var newtMesh = DemoMeshHelper.LoadModel(content, BufferPool, @"Content\newt.obj", new Vector3(-10, 10, -10)); - var newtShape = Simulation.Shapes.Add(newtMesh); - var newtCount = 10; - newts = new QuickList(newtCount, BufferPool); - newtArenaMin = new Vector2(-250); - newtArenaMax = new Vector2(250); - random = new Random(8); - for (int i = 0; i < newtCount; ++i) - { - ref var newt = ref newts.AllocateUnsafely(); - newt = new SponsorNewt(Simulation, newtShape, 0, newtArenaMin, newtArenaMax, random, i); - } - - const float floorSize = 520; - const float wallThickness = 200; - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -10f, 0), Simulation.Shapes.Add(new Box(floorSize, 20, floorSize)))); - Simulation.Statics.Add(new StaticDescription(new Vector3(floorSize * -0.5f - wallThickness * 0.5f, -5, 0), Simulation.Shapes.Add(new Box(wallThickness, 30, floorSize + wallThickness * 2)))); - Simulation.Statics.Add(new StaticDescription(new Vector3(floorSize * 0.5f + wallThickness * 0.5f, -5, 0), Simulation.Shapes.Add(new Box(wallThickness, 30, floorSize + wallThickness * 2)))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5, floorSize * -0.5f - wallThickness * 0.5f), Simulation.Shapes.Add(new Box(floorSize, 30, wallThickness)))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5, floorSize * 0.5f + wallThickness * 0.5f), Simulation.Shapes.Add(new Box(floorSize, 30, wallThickness)))); - - const int characterCount = 2000; - characterAIs = new QuickList(characterCount, BufferPool); - var characterCollidable = Simulation.Shapes.Add(new Capsule(0.5f, 1f)); - for (int i = 0; i < characterCount; ++i) - { - var position2D = newtArenaMin + (newtArenaMax - newtArenaMin) * new Vector2(random.NextSingle(), random.NextSingle()); - var targetPosition = 0.5f * (newtArenaMin + (newtArenaMax - newtArenaMin) * new Vector2(random.NextSingle(), random.NextSingle())); - characterAIs.AllocateUnsafely() = new SponsorCharacterAI(characterControllers, characterCollidable, new Vector3(position2D.X, 5, position2D.Y), targetPosition); - } - - const int hutCount = 120; - var hutBoxShape = new Box(0.4f, 2, 3); - var obstacleDescription = BodyDescription.CreateDynamic(new Vector3(), hutBoxShape.ComputeInertia(20), new CollidableDescription(Simulation.Shapes.Add(hutBoxShape), 0.1f), 1e-2f); - - for (int i = 0; i < hutCount; ++i) - { - var position2D = newtArenaMin + (newtArenaMax - newtArenaMin) * new Vector2(random.NextSingle(), random.NextSingle()); - ColosseumDemo.CreateRing(Simulation, new Vector3(position2D.X, 0, position2D.Y), hutBoxShape, obstacleDescription, 4 + random.NextSingle() * 8, 2, random.Next(1, 10)); - - } - - var overlordNewtShape = newtMesh; - overlordNewtShape.Scale = new Vector3(60, 60, 60); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, 10, -floorSize * 0.5f - 70), Simulation.Shapes.Add(overlordNewtShape))); - - - character = new CharacterInput(characterControllers, new Vector3(-108.89504f, 28.403418f, 38.27505f), new Capsule(0.5f, 1), 0.1f, .1f, 20, 100, 6, 4, MathF.PI * 0.4f); - - Console.WriteLine($"body count: {Simulation.Bodies.ActiveSet.Count}"); + var position2D = newtArenaMin + (newtArenaMax - newtArenaMin) * new Vector2(random.NextSingle(), random.NextSingle()); + var targetPosition = 0.5f * (newtArenaMin + (newtArenaMax - newtArenaMin) * new Vector2(random.NextSingle(), random.NextSingle())); + characterAIs.AllocateUnsafely() = new SponsorCharacterAI(characterControllers, characterCollidable, new Vector3(position2D.X, 5, position2D.Y), targetPosition); } - CharacterInput character; + const int hutCount = 120; + var hutBoxShape = new Box(0.4f, 2, 3); + var obstacleDescription = BodyDescription.CreateDynamic(new Vector3(), hutBoxShape.ComputeInertia(20), new CollidableDescription(Simulation.Shapes.Add(hutBoxShape), 0.1f), 1e-2f); + for (int i = 0; i < hutCount; ++i) + { + var position2D = newtArenaMin + (newtArenaMax - newtArenaMin) * new Vector2(random.NextSingle(), random.NextSingle()); + ColosseumDemo.CreateRing(Simulation, new Vector3(position2D.X, 0, position2D.Y), hutBoxShape, obstacleDescription, 4 + random.NextSingle() * 8, 2, random.Next(1, 10)); + } + + var overlordNewtShape = newtMesh; + overlordNewtShape.Scale = new Vector3(60, 60, 60); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 10, -floorSize * 0.5f - 70), Simulation.Shapes.Add(overlordNewtShape))); + + + character = new CharacterInput(characterControllers, new Vector3(-108.89504f, 28.403418f, 38.27505f), new Capsule(0.5f, 1), 0.1f, .1f, 20, 100, 6, 4, MathF.PI * 0.4f); + + Console.WriteLine($"body count: {Simulation.Bodies.ActiveSet.Count}"); + } - double simulationTime; - public override void Update(Window window, Camera camera, Input input, float dt) + CharacterInput character; + + + + double simulationTime; + public override void Update(Window window, Camera camera, Input input, float dt) + { + character.UpdateCharacterGoals(input, camera, TimestepDuration); + Simulation.Timestep(TimestepDuration, ThreadDispatcher); + character.UpdateCameraPosition(camera, -0.3f); + for (int i = 0; i < newts.Count; ++i) + { + newts[i].Update(Simulation, simulationTime, 0, newtArenaMin, newtArenaMax, random, 1f / TimestepDuration); + } + for (int i = 0; i < characterAIs.Count; ++i) { - character.UpdateCharacterGoals(input, camera, TimestepDuration); - Simulation.Timestep(TimestepDuration, ThreadDispatcher); - character.UpdateCameraPosition(camera, -0.3f); - for (int i = 0; i < newts.Count; ++i) - { - newts[i].Update(Simulation, simulationTime, 0, newtArenaMin, newtArenaMax, random, 1f / TimestepDuration); - } - for (int i = 0; i < characterAIs.Count; ++i) - { - characterAIs[i].Update(characterControllers, Simulation, ref newts, random); - } - simulationTime += TimestepDuration; - - - if(input.WasPushed(OpenTK.Input.Key.P)) - { - Console.WriteLine($"camera position: {camera.Position}"); - } + characterAIs[i].Update(characterControllers, Simulation, ref newts, random); } + simulationTime += TimestepDuration; + + if(input.WasPushed(OpenTK.Input.Key.P)) + { + Console.WriteLine($"camera position: {camera.Position}"); + } } + } diff --git a/Demos/SpecializedTests/Media/2.4/RagdollTubeVideoDemo.cs b/Demos/SpecializedTests/Media/2.4/RagdollTubeVideoDemo.cs index f647ef16f..63eb4e670 100644 --- a/Demos/SpecializedTests/Media/2.4/RagdollTubeVideoDemo.cs +++ b/Demos/SpecializedTests/Media/2.4/RagdollTubeVideoDemo.cs @@ -11,93 +11,92 @@ using BepuPhysics.Constraints; using DemoRenderer.UI; -namespace Demos.SpecializedTests.Media +namespace Demos.SpecializedTests.Media; + +/// +/// Subjects a bunch of unfortunate ragdolls to a tumble dry cycle. +/// +public class RagdollTubeVideoDemo : Demo { - /// - /// Subjects a bunch of unfortunate ragdolls to a tumble dry cycle. - /// - public class RagdollTubeVideoDemo : Demo + public override void Initialize(ContentArchive content, Camera camera) { - public override void Initialize(ContentArchive content, Camera camera) - { - camera.Position = new Vector3(0, 9, -40); - camera.Yaw = MathHelper.Pi; - camera.Pitch = 0; - var filters = new CollidableProperty(); - //Note the lowered material stiffness compared to many of the other demos. Ragdolls aren't made of concrete. - //Increasing the maximum recovery velocity helps keep deeper contacts strong, stopping objects from interpenetrating. - //Higher friction helps the bodies clump and flop, rather than just sliding down the slope in the tube. - Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks(filters, new PairMaterialProperties(2, float.MaxValue, new SpringSettings(10, 1))), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(4, 1)); + camera.Position = new Vector3(0, 9, -40); + camera.Yaw = MathHelper.Pi; + camera.Pitch = 0; + var filters = new CollidableProperty(); + //Note the lowered material stiffness compared to many of the other demos. Ragdolls aren't made of concrete. + //Increasing the maximum recovery velocity helps keep deeper contacts strong, stopping objects from interpenetrating. + //Higher friction helps the bodies clump and flop, rather than just sliding down the slope in the tube. + Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks(filters, new PairMaterialProperties(2, float.MaxValue, new SpringSettings(10, 1))), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(4, 1)); - int ragdollIndex = 0; - var spacing = new Vector3(1.7f, 1.8f, 0.5f); - int width = 4; - int height = 4; - int length = 120; - var origin = -0.5f * spacing * new Vector3(width - 1, 0, length - 1) + new Vector3(0, 5f, 0); - for (int i = 0; i < width; ++i) + int ragdollIndex = 0; + var spacing = new Vector3(1.7f, 1.8f, 0.5f); + int width = 4; + int height = 4; + int length = 120; + var origin = -0.5f * spacing * new Vector3(width - 1, 0, length - 1) + new Vector3(0, 5f, 0); + for (int i = 0; i < width; ++i) + { + for (int j = 0; j < height; ++j) { - for (int j = 0; j < height; ++j) + for (int k = 0; k < length; ++k) { - for (int k = 0; k < length; ++k) - { - RagdollDemo.AddRagdoll(origin + spacing * new Vector3(i, j, k), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathHelper.Pi * 0.05f), ragdollIndex++, filters, Simulation); - } + RagdollDemo.AddRagdoll(origin + spacing * new Vector3(i, j, k), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathHelper.Pi * 0.05f), ragdollIndex++, filters, Simulation); } } + } - ragdollCount = ragdollIndex; - ragdollBodyCount = Simulation.Bodies.ActiveSet.Count; - ragdollConstraintCount = Simulation.Solver.CountConstraints(); + ragdollCount = ragdollIndex; + ragdollBodyCount = Simulation.Bodies.ActiveSet.Count; + ragdollConstraintCount = Simulation.Solver.CountConstraints(); - var tubeCenter = new Vector3(0, 8, 0); - const int panelCount = 20; - const float tubeRadius = 6; - var panelShape = new Box(MathF.PI * 2 * tubeRadius / panelCount, 1, 100); - var panelShapeIndex = Simulation.Shapes.Add(panelShape); - var builder = new CompoundBuilder(BufferPool, Simulation.Shapes, panelCount + 1); - for (int i = 0; i < panelCount; ++i) - { - var rotation = QuaternionEx.CreateFromAxisAngle(Vector3.UnitZ, i * MathHelper.TwoPi / panelCount); - QuaternionEx.TransformUnitY(rotation, out var localUp); - var position = localUp * tubeRadius; - builder.AddForKinematic(panelShapeIndex, (position, rotation), 1); - } - builder.AddForKinematic(Simulation.Shapes.Add(new Box(1, 2, panelShape.Length)), new Vector3(0, tubeRadius - 1, 0), 0); - builder.BuildKinematicCompound(out var children); - var compound = new BigCompound(children, Simulation.Shapes, BufferPool); - var tubeHandle = Simulation.Bodies.Add(BodyDescription.CreateKinematic(tubeCenter, (default, new Vector3(0, 0, .25f)), Simulation.Shapes.Add(compound), 0f)); - filters[tubeHandle] = new SubgroupCollisionFilter(int.MaxValue); - builder.Dispose(); + var tubeCenter = new Vector3(0, 8, 0); + const int panelCount = 20; + const float tubeRadius = 6; + var panelShape = new Box(MathF.PI * 2 * tubeRadius / panelCount, 1, 100); + var panelShapeIndex = Simulation.Shapes.Add(panelShape); + var builder = new CompoundBuilder(BufferPool, Simulation.Shapes, panelCount + 1); + for (int i = 0; i < panelCount; ++i) + { + var rotation = QuaternionEx.CreateFromAxisAngle(Vector3.UnitZ, i * MathHelper.TwoPi / panelCount); + QuaternionEx.TransformUnitY(rotation, out var localUp); + var position = localUp * tubeRadius; + builder.AddForKinematic(panelShapeIndex, (position, rotation), 1); + } + builder.AddForKinematic(Simulation.Shapes.Add(new Box(1, 2, panelShape.Length)), new Vector3(0, tubeRadius - 1, 0), 0); + builder.BuildKinematicCompound(out var children); + var compound = new BigCompound(children, Simulation.Shapes, BufferPool); + var tubeHandle = Simulation.Bodies.Add(BodyDescription.CreateKinematic(tubeCenter, (default, new Vector3(0, 0, .25f)), Simulation.Shapes.Add(compound), 0f)); + filters[tubeHandle] = new SubgroupCollisionFilter(int.MaxValue); + builder.Dispose(); - var staticShape = new Box(300, 1, 300); - var staticShapeIndex = Simulation.Shapes.Add(staticShape); - var staticDescription = new StaticDescription(new Vector3(0, -0.5f, 0), staticShapeIndex); - Simulation.Statics.Add(staticDescription); + var staticShape = new Box(300, 1, 300); + var staticShapeIndex = Simulation.Shapes.Add(staticShape); + var staticDescription = new StaticDescription(new Vector3(0, -0.5f, 0), staticShapeIndex); + Simulation.Statics.Add(staticDescription); - var newtMesh = DemoMeshHelper.LoadModel(content, BufferPool, @"Content\newt.obj", new Vector3(15, 15, 15)); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0.5f, 80), Quaternion.CreateFromAxisAngle(Vector3.UnitY, MathF.PI), Simulation.Shapes.Add(newtMesh))); + var newtMesh = DemoMeshHelper.LoadModel(content, BufferPool, @"Content\newt.obj", new Vector3(15, 15, 15)); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0.5f, 80), Quaternion.CreateFromAxisAngle(Vector3.UnitY, MathF.PI), Simulation.Shapes.Add(newtMesh))); - } - int ragdollBodyCount; - int ragdollConstraintCount; - int ragdollCount; + } + int ragdollBodyCount; + int ragdollConstraintCount; + int ragdollCount; - public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) - { - var resolution = renderer.Surface.Resolution; - renderer.TextBatcher.Write(text.Clear().Append("Ragdoll count:"), new Vector2(16, resolution.Y - 64), 16, Vector3.One, font); - renderer.TextBatcher.Write(text.Clear().Append("Ragdoll body count:"), new Vector2(16, resolution.Y - 48), 16, Vector3.One, font); - renderer.TextBatcher.Write(text.Clear().Append("Ragdoll constraint count:"), new Vector2(16, resolution.Y - 32), 16, Vector3.One, font); - renderer.TextBatcher.Write(text.Clear().Append("Collision constraint count:"), new Vector2(16, resolution.Y - 16), 16, Vector3.One, font); - const float xOffset = 192; - renderer.TextBatcher.Write(text.Clear().Append(ragdollCount), new Vector2(xOffset, resolution.Y - 64), 16, Vector3.One, font); - renderer.TextBatcher.Write(text.Clear().Append(ragdollBodyCount), new Vector2(xOffset, resolution.Y - 48), 16, Vector3.One, font); - renderer.TextBatcher.Write(text.Clear().Append(ragdollConstraintCount), new Vector2(xOffset, resolution.Y - 32), 16, Vector3.One, font); - var collisionConstraintCount = Simulation.Solver.CountConstraints() - ragdollConstraintCount; - renderer.TextBatcher.Write(text.Clear().Append(collisionConstraintCount), new Vector2(xOffset, resolution.Y - 16), 16, Vector3.One, font); - base.Render(renderer, camera, input, text, font); - } + public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + { + var resolution = renderer.Surface.Resolution; + renderer.TextBatcher.Write(text.Clear().Append("Ragdoll count:"), new Vector2(16, resolution.Y - 64), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Ragdoll body count:"), new Vector2(16, resolution.Y - 48), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Ragdoll constraint count:"), new Vector2(16, resolution.Y - 32), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Collision constraint count:"), new Vector2(16, resolution.Y - 16), 16, Vector3.One, font); + const float xOffset = 192; + renderer.TextBatcher.Write(text.Clear().Append(ragdollCount), new Vector2(xOffset, resolution.Y - 64), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append(ragdollBodyCount), new Vector2(xOffset, resolution.Y - 48), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append(ragdollConstraintCount), new Vector2(xOffset, resolution.Y - 32), 16, Vector3.One, font); + var collisionConstraintCount = Simulation.Solver.CountConstraints() - ragdollConstraintCount; + renderer.TextBatcher.Write(text.Clear().Append(collisionConstraintCount), new Vector2(xOffset, resolution.Y - 16), 16, Vector3.One, font); + base.Render(renderer, camera, input, text, font); } } diff --git a/Demos/SpecializedTests/Media/2.4/TankSwarmDemo.cs b/Demos/SpecializedTests/Media/2.4/TankSwarmDemo.cs index 329e9679c..929a8eb19 100644 --- a/Demos/SpecializedTests/Media/2.4/TankSwarmDemo.cs +++ b/Demos/SpecializedTests/Media/2.4/TankSwarmDemo.cs @@ -13,354 +13,353 @@ using DemoUtilities; using OpenTK.Input; -namespace Demos.SpecializedTests.Media +namespace Demos.SpecializedTests.Media; + +public class TankSwarmDemo : Demo { - public class TankSwarmDemo : Demo - { - CollidableProperty bodyProperties; - TankController playerController; + CollidableProperty bodyProperties; + TankController playerController; - QuickList aiTanks; - Random random; - Vector2 playAreaMin, playAreaMax; + QuickList aiTanks; + Random random; + Vector2 playAreaMin, playAreaMax; - //We want to create a little graphical explosion at projectile impact points. Since it's not an instant thing, we'll have to track it over a period of time. - struct Explosion - { - public Vector3 Position; - public float Scale; - public Vector3 Color; - public int Age; - } - QuickList explosions; + //We want to create a little graphical explosion at projectile impact points. Since it's not an instant thing, we'll have to track it over a period of time. + struct Explosion + { + public Vector3 Position; + public float Scale; + public Vector3 Color; + public int Age; + } + QuickList explosions; - static MouseButton Fire = MouseButton.Left; - static Key Forward = Key.W; - static Key Backward = Key.S; - static Key Right = Key.D; - static Key Left = Key.A; - static Key Zoom = Key.LShift; - static Key Brake = Key.Space; - static Key BrakeAlternate = Key.BackSpace; //I have a weird keyboard. - static Key ToggleTank = Key.C; - public override void Initialize(ContentArchive content, Camera camera) - { - camera.Position = new Vector3(0, 5, 10); - camera.Yaw = 0; - camera.Pitch = 0; + static MouseButton Fire = MouseButton.Left; + static Key Forward = Key.W; + static Key Backward = Key.S; + static Key Right = Key.D; + static Key Left = Key.A; + static Key Zoom = Key.LShift; + static Key Brake = Key.Space; + static Key BrakeAlternate = Key.BackSpace; //I have a weird keyboard. + static Key ToggleTank = Key.C; + public override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(0, 5, 10); + camera.Yaw = 0; + camera.Pitch = 0; - bodyProperties = new CollidableProperty(); - //Note that this demo uses only 1 substep and 6 velocity iterations. - //That's partly to show that you can do such a thing, and partly because of (as of 2.4's initial release), there are situations where - //contact data can become a little out of date during substepping, since the contact data is only updated once per frame rather than substep (apart from the depths, which are incrementally updated every substep). - //In this demo, when using substepping, a wheel resting on another wheel from a destroyed tank can keep rocking back and forth for a long time as the error in contact offsets over substeps can introduce energy. - //(I'd like to address this issue more directly to make substepping an unconditional win.) - Simulation = Simulation.Create(BufferPool, new TankCallbacks() { Properties = bodyProperties }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(6, 1)); + bodyProperties = new CollidableProperty(); + //Note that this demo uses only 1 substep and 6 velocity iterations. + //That's partly to show that you can do such a thing, and partly because of (as of 2.4's initial release), there are situations where + //contact data can become a little out of date during substepping, since the contact data is only updated once per frame rather than substep (apart from the depths, which are incrementally updated every substep). + //In this demo, when using substepping, a wheel resting on another wheel from a destroyed tank can keep rocking back and forth for a long time as the error in contact offsets over substeps can introduce energy. + //(I'd like to address this issue more directly to make substepping an unconditional win.) + Simulation = Simulation.Create(BufferPool, new TankCallbacks() { Properties = bodyProperties }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(6, 1)); - var builder = new CompoundBuilder(BufferPool, Simulation.Shapes, 2); - builder.Add(new Box(1.85f, 0.7f, 4.73f), RigidPose.Identity, 10); - builder.Add(new Box(1.85f, 0.6f, 2.5f), new Vector3(0, 0.65f, -0.35f), 0.5f); - builder.BuildDynamicCompound(out var children, out var bodyInertia, out _); - builder.Dispose(); - var bodyShape = new Compound(children); - var bodyShapeIndex = Simulation.Shapes.Add(bodyShape); - var wheelShape = new Cylinder(0.4f, .18f); - var wheelInertia = wheelShape.ComputeInertia(0.25f); - var wheelShapeIndex = Simulation.Shapes.Add(wheelShape); + var builder = new CompoundBuilder(BufferPool, Simulation.Shapes, 2); + builder.Add(new Box(1.85f, 0.7f, 4.73f), RigidPose.Identity, 10); + builder.Add(new Box(1.85f, 0.6f, 2.5f), new Vector3(0, 0.65f, -0.35f), 0.5f); + builder.BuildDynamicCompound(out var children, out var bodyInertia, out _); + builder.Dispose(); + var bodyShape = new Compound(children); + var bodyShapeIndex = Simulation.Shapes.Add(bodyShape); + var wheelShape = new Cylinder(0.4f, .18f); + var wheelInertia = wheelShape.ComputeInertia(0.25f); + var wheelShapeIndex = Simulation.Shapes.Add(wheelShape); - var projectileShape = new Sphere(0.1f); - var projectileInertia = projectileShape.ComputeInertia(0.2f); - var tankDescription = new TankDescription - { - Body = TankPartDescription.Create(10, new Box(4f, 1, 5), RigidPose.Identity, 0.5f, Simulation.Shapes), - Turret = TankPartDescription.Create(1, new Box(1.5f, 0.7f, 2f), new Vector3(0, 0.85f, 0.4f), 0.5f, Simulation.Shapes), - Barrel = TankPartDescription.Create(0.5f, new Box(0.2f, 0.2f, 3f), new Vector3(0, 0.85f, 0.4f - 1f - 1.5f), 0.5f, Simulation.Shapes), - TurretAnchor = new Vector3(0f, 0.5f, 0.4f), - BarrelAnchor = new Vector3(0, 0.5f + 0.35f, 0.4f - 1f), - TurretBasis = Quaternion.Identity, - TurretServo = new ServoSettings(1f, 0f, 40f), - TurretSpring = new SpringSettings(10f, 1f), - BarrelServo = new ServoSettings(1f, 0f, 40f), - BarrelSpring = new SpringSettings(10f, 1f), + var projectileShape = new Sphere(0.1f); + var projectileInertia = projectileShape.ComputeInertia(0.2f); + var tankDescription = new TankDescription + { + Body = TankPartDescription.Create(10, new Box(4f, 1, 5), RigidPose.Identity, 0.5f, Simulation.Shapes), + Turret = TankPartDescription.Create(1, new Box(1.5f, 0.7f, 2f), new Vector3(0, 0.85f, 0.4f), 0.5f, Simulation.Shapes), + Barrel = TankPartDescription.Create(0.5f, new Box(0.2f, 0.2f, 3f), new Vector3(0, 0.85f, 0.4f - 1f - 1.5f), 0.5f, Simulation.Shapes), + TurretAnchor = new Vector3(0f, 0.5f, 0.4f), + BarrelAnchor = new Vector3(0, 0.5f + 0.35f, 0.4f - 1f), + TurretBasis = Quaternion.Identity, + TurretServo = new ServoSettings(1f, 0f, 40f), + TurretSpring = new SpringSettings(10f, 1f), + BarrelServo = new ServoSettings(1f, 0f, 40f), + BarrelSpring = new SpringSettings(10f, 1f), - ProjectileShape = Simulation.Shapes.Add(projectileShape), - ProjectileSpeed = 100f, - BarrelLocalProjectileSpawn = new Vector3(0, 0, -1.5f), - ProjectileInertia = projectileInertia, + ProjectileShape = Simulation.Shapes.Add(projectileShape), + ProjectileSpeed = 100f, + BarrelLocalProjectileSpawn = new Vector3(0, 0, -1.5f), + ProjectileInertia = projectileInertia, - LeftTreadOffset = new Vector3(-1.9f, 0f, 0), - RightTreadOffset = new Vector3(1.9f, 0f, 0), - SuspensionLength = 1f, - SuspensionSettings = new SpringSettings(2.5f, 1.5f), - WheelShape = wheelShapeIndex, - WheelInertia = wheelInertia, - WheelFriction = 2f, - TreadSpacing = 1f, - WheelCountPerTread = 5, - WheelOrientation = QuaternionEx.CreateFromAxisAngle(Vector3.UnitZ, MathF.PI * -0.5f), - }; + LeftTreadOffset = new Vector3(-1.9f, 0f, 0), + RightTreadOffset = new Vector3(1.9f, 0f, 0), + SuspensionLength = 1f, + SuspensionSettings = new SpringSettings(2.5f, 1.5f), + WheelShape = wheelShapeIndex, + WheelInertia = wheelInertia, + WheelFriction = 2f, + TreadSpacing = 1f, + WheelCountPerTread = 5, + WheelOrientation = QuaternionEx.CreateFromAxisAngle(Vector3.UnitZ, MathF.PI * -0.5f), + }; - playerController = new TankController(Tank.Create(Simulation, bodyProperties, BufferPool, (new Vector3(0, 10, 0), Quaternion.Identity), tankDescription), 20, 5, 2, 1, 3.5f); + playerController = new TankController(Tank.Create(Simulation, bodyProperties, BufferPool, (new Vector3(0, 10, 0), Quaternion.Identity), tankDescription), 20, 5, 2, 1, 3.5f); - const int planeWidth = 257; - const float terrainScale = 3; - const float inverseTerrainScale = 1f / terrainScale; - var terrainPosition = new Vector2(1 - planeWidth, 1 - planeWidth) * terrainScale * 0.5f; - random = new Random(5); + const int planeWidth = 257; + const float terrainScale = 3; + const float inverseTerrainScale = 1f / terrainScale; + var terrainPosition = new Vector2(1 - planeWidth, 1 - planeWidth) * terrainScale * 0.5f; + random = new Random(5); - //Add some building-ish landmarks. - var landmarkMin = new Vector3(planeWidth * terrainScale * -0.45f, 0, planeWidth * terrainScale * -0.45f); - var landmarkMax = new Vector3(planeWidth * terrainScale * 0.45f, 0, planeWidth * terrainScale * 0.45f); - var landmarkSpan = landmarkMax - landmarkMin; - for (int j = 0; j < 25; ++j) - { - var buildingShape = new Box(10 + random.NextSingle() * 10, 20 + random.NextSingle() * 20, 10 + random.NextSingle() * 10); - var position = landmarkMin + landmarkSpan * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); - Simulation.Statics.Add(new StaticDescription( - new Vector3(0, buildingShape.HalfHeight - 4f + GetHeightForPosition(position.X, position.Z, planeWidth, inverseTerrainScale, terrainPosition), 0) + position, - QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, random.NextSingle() * MathF.PI), - Simulation.Shapes.Add(buildingShape))); - } + //Add some building-ish landmarks. + var landmarkMin = new Vector3(planeWidth * terrainScale * -0.45f, 0, planeWidth * terrainScale * -0.45f); + var landmarkMax = new Vector3(planeWidth * terrainScale * 0.45f, 0, planeWidth * terrainScale * 0.45f); + var landmarkSpan = landmarkMax - landmarkMin; + for (int j = 0; j < 25; ++j) + { + var buildingShape = new Box(10 + random.NextSingle() * 10, 20 + random.NextSingle() * 20, 10 + random.NextSingle() * 10); + var position = landmarkMin + landmarkSpan * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); + Simulation.Statics.Add(new StaticDescription( + new Vector3(0, buildingShape.HalfHeight - 4f + GetHeightForPosition(position.X, position.Z, planeWidth, inverseTerrainScale, terrainPosition), 0) + position, + QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, random.NextSingle() * MathF.PI), + Simulation.Shapes.Add(buildingShape))); + } - var planeMesh = DemoMeshHelper.CreateDeformedPlane(planeWidth, planeWidth, - (int vX, int vY) => - { - var position2D = new Vector2(vX, vY) * terrainScale + terrainPosition; - return new Vector3(position2D.X, GetHeightForPosition(position2D.X, position2D.Y, planeWidth, inverseTerrainScale, terrainPosition), position2D.Y); - }, new Vector3(1, 1, 1), BufferPool); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), Simulation.Shapes.Add(planeMesh))); + var planeMesh = DemoMeshHelper.CreateDeformedPlane(planeWidth, planeWidth, + (int vX, int vY) => + { + var position2D = new Vector2(vX, vY) * terrainScale + terrainPosition; + return new Vector3(position2D.X, GetHeightForPosition(position2D.X, position2D.Y, planeWidth, inverseTerrainScale, terrainPosition), position2D.Y); + }, new Vector3(1, 1, 1), BufferPool); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), Simulation.Shapes.Add(planeMesh))); - explosions = new QuickList(32, BufferPool); + explosions = new QuickList(32, BufferPool); - //Create the AI tanks. - const int aiTankCount = 3072; - aiTanks = new QuickList(aiTankCount, BufferPool); - playAreaMin = new Vector2(landmarkMin.X, landmarkMin.Z); - playAreaMax = new Vector2(landmarkMax.X, landmarkMax.Z); - var playAreaSpan = playAreaMax - playAreaMin; - for (int i = 0; i < aiTankCount; ++i) + //Create the AI tanks. + const int aiTankCount = 3072; + aiTanks = new QuickList(aiTankCount, BufferPool); + playAreaMin = new Vector2(landmarkMin.X, landmarkMin.Z); + playAreaMax = new Vector2(landmarkMax.X, landmarkMax.Z); + var playAreaSpan = playAreaMax - playAreaMin; + for (int i = 0; i < aiTankCount; ++i) + { + var horizontalPosition = playAreaMin + new Vector2(random.NextSingle(), random.NextSingle()) * playAreaSpan; + aiTanks.AllocateUnsafely() = new AITank { - var horizontalPosition = playAreaMin + new Vector2(random.NextSingle(), random.NextSingle()) * playAreaSpan; - aiTanks.AllocateUnsafely() = new AITank - { - Controller = new TankController( - Tank.Create(Simulation, bodyProperties, BufferPool, - (new Vector3(horizontalPosition.X, 10, horizontalPosition.Y), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), random.NextSingle() * 0.1f)), - tankDescription), 20, 5, 2, 1, 3.5f), - HitPoints = 5 - }; - } - Console.WriteLine($"body count: {Simulation.Bodies.ActiveSet.Count}"); + Controller = new TankController( + Tank.Create(Simulation, bodyProperties, BufferPool, + (new Vector3(horizontalPosition.X, 10, horizontalPosition.Y), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), random.NextSingle() * 0.1f)), + tankDescription), 20, 5, 2, 1, 3.5f), + HitPoints = 5 + }; } + Console.WriteLine($"body count: {Simulation.Bodies.ActiveSet.Count}"); + } - float GetHeightForPosition(float x, float y, int planeWidth, float inverseTerrainScale, in Vector2 terrainPosition) - { - var normalizedX = (x - terrainPosition.X) * inverseTerrainScale; - var normalizedY = (y - terrainPosition.Y) * inverseTerrainScale; - var octave0 = (MathF.Sin((normalizedX + 5f) * 0.05f) + MathF.Sin((normalizedY + 11) * 0.05f)) * 3.8f; - var octave1 = (MathF.Sin((normalizedX + 17) * 0.15f) + MathF.Sin((normalizedY + 47) * 0.15f)) * 1.5f; - var octave2 = (MathF.Sin((normalizedX + 37) * 0.35f) + MathF.Sin((normalizedY + 93) * 0.35f)) * 0.5f; - var octave3 = (MathF.Sin((normalizedX + 53) * 0.65f) + MathF.Sin((normalizedY + 131) * 0.65f)) * 0.3f; - var octave4 = (MathF.Sin((normalizedX + 67) * 1.50f) + MathF.Sin((normalizedY + 13) * 1.5f)) * 0.1525f; - var distanceToEdge = planeWidth / 2 - Math.Max(Math.Abs(normalizedX - planeWidth / 2), Math.Abs(normalizedY - planeWidth / 2)); - //Flatten an area in the middle. - var offsetX = planeWidth * 0.5f - normalizedX; - var offsetY = planeWidth * 0.5f - normalizedY; - var distanceToCenterSquared = offsetX * offsetX + offsetY * offsetY; - const float centerCircleSize = 30f; - const float fadeoutBoundary = 50f; - var outsideWeight = MathF.Min(1f, MathF.Max(0, distanceToCenterSquared - centerCircleSize * centerCircleSize) / (fadeoutBoundary * fadeoutBoundary - centerCircleSize * centerCircleSize)); - var edgeRamp = 25f / (5 * distanceToEdge + 1); - return outsideWeight * (octave0 + octave1 + octave2 + octave3 + octave4 + edgeRamp); - } + float GetHeightForPosition(float x, float y, int planeWidth, float inverseTerrainScale, in Vector2 terrainPosition) + { + var normalizedX = (x - terrainPosition.X) * inverseTerrainScale; + var normalizedY = (y - terrainPosition.Y) * inverseTerrainScale; + var octave0 = (MathF.Sin((normalizedX + 5f) * 0.05f) + MathF.Sin((normalizedY + 11) * 0.05f)) * 3.8f; + var octave1 = (MathF.Sin((normalizedX + 17) * 0.15f) + MathF.Sin((normalizedY + 47) * 0.15f)) * 1.5f; + var octave2 = (MathF.Sin((normalizedX + 37) * 0.35f) + MathF.Sin((normalizedY + 93) * 0.35f)) * 0.5f; + var octave3 = (MathF.Sin((normalizedX + 53) * 0.65f) + MathF.Sin((normalizedY + 131) * 0.65f)) * 0.3f; + var octave4 = (MathF.Sin((normalizedX + 67) * 1.50f) + MathF.Sin((normalizedY + 13) * 1.5f)) * 0.1525f; + var distanceToEdge = planeWidth / 2 - Math.Max(Math.Abs(normalizedX - planeWidth / 2), Math.Abs(normalizedY - planeWidth / 2)); + //Flatten an area in the middle. + var offsetX = planeWidth * 0.5f - normalizedX; + var offsetY = planeWidth * 0.5f - normalizedY; + var distanceToCenterSquared = offsetX * offsetX + offsetY * offsetY; + const float centerCircleSize = 30f; + const float fadeoutBoundary = 50f; + var outsideWeight = MathF.Min(1f, MathF.Max(0, distanceToCenterSquared - centerCircleSize * centerCircleSize) / (fadeoutBoundary * fadeoutBoundary - centerCircleSize * centerCircleSize)); + var edgeRamp = 25f / (5 * distanceToEdge + 1); + return outsideWeight * (octave0 + octave1 + octave2 + octave3 + octave4 + edgeRamp); + } - bool playerControlActive = true; - long frameIndex; - long lastPlayerShotFrameIndex; - int projectileCount; - public override void Update(Window window, Camera camera, Input input, float dt) + bool playerControlActive = true; + long frameIndex; + long lastPlayerShotFrameIndex; + int projectileCount; + public override void Update(Window window, Camera camera, Input input, float dt) + { + if (input.WasPushed(ToggleTank)) + playerControlActive = !playerControlActive; + if (playerControlActive) { - if (input.WasPushed(ToggleTank)) - playerControlActive = !playerControlActive; - if (playerControlActive) + float leftTargetSpeedFraction = 0; + float rightTargetSpeedFraction = 0; + var left = input.IsDown(Left); + var right = input.IsDown(Right); + var forward = input.IsDown(Forward); + var backward = input.IsDown(Backward); + if (forward) { - float leftTargetSpeedFraction = 0; - float rightTargetSpeedFraction = 0; - var left = input.IsDown(Left); - var right = input.IsDown(Right); - var forward = input.IsDown(Forward); - var backward = input.IsDown(Backward); - if (forward) + if ((left && right) || (!left && !right)) { - if ((left && right) || (!left && !right)) - { - leftTargetSpeedFraction = 1f; - rightTargetSpeedFraction = 1f; - } - //Note turns require a bit of help from the opposing track to overcome friction. - else if (left) - { - leftTargetSpeedFraction = 0.5f; - rightTargetSpeedFraction = 1f; - } - else if (right) - { - leftTargetSpeedFraction = 1f; - rightTargetSpeedFraction = 0.5f; - } + leftTargetSpeedFraction = 1f; + rightTargetSpeedFraction = 1f; } - else if (backward) + //Note turns require a bit of help from the opposing track to overcome friction. + else if (left) { - if ((left && right) || (!left && !right)) - { - leftTargetSpeedFraction = -1f; - rightTargetSpeedFraction = -1f; - } - else if (left) - { - leftTargetSpeedFraction = -0.5f; - rightTargetSpeedFraction = -1f; - } - else if (right) - { - leftTargetSpeedFraction = -1f; - rightTargetSpeedFraction = -0.5f; - } + leftTargetSpeedFraction = 0.5f; + rightTargetSpeedFraction = 1f; } - else + else if (right) { - //Not trying to move. Turn? - if (left && !right) - { - leftTargetSpeedFraction = -1f; - rightTargetSpeedFraction = 1f; - } - else if (right && !left) - { - leftTargetSpeedFraction = 1f; - rightTargetSpeedFraction = -1f; - } + leftTargetSpeedFraction = 1f; + rightTargetSpeedFraction = 0.5f; } - - var zoom = input.IsDown(Zoom); - var brake = input.IsDown(Brake) || input.IsDown(BrakeAlternate); - playerController.UpdateMovementAndAim(Simulation, leftTargetSpeedFraction, rightTargetSpeedFraction, zoom, brake, brake, camera.Forward); - - if (input.WasPushed(Fire) && frameIndex > lastPlayerShotFrameIndex + 60) + } + else if (backward) + { + if ((left && right) || (!left && !right)) + { + leftTargetSpeedFraction = -1f; + rightTargetSpeedFraction = -1f; + } + else if (left) + { + leftTargetSpeedFraction = -0.5f; + rightTargetSpeedFraction = -1f; + } + else if (right) + { + leftTargetSpeedFraction = -1f; + rightTargetSpeedFraction = -0.5f; + } + } + else + { + //Not trying to move. Turn? + if (left && !right) + { + leftTargetSpeedFraction = -1f; + rightTargetSpeedFraction = 1f; + } + else if (right && !left) { - playerController.Tank.Fire(Simulation, bodyProperties); - lastPlayerShotFrameIndex = frameIndex; - ++projectileCount; + leftTargetSpeedFraction = 1f; + rightTargetSpeedFraction = -1f; } } - for (int i = 0; i < aiTanks.Count; ++i) + var zoom = input.IsDown(Zoom); + var brake = input.IsDown(Brake) || input.IsDown(BrakeAlternate); + playerController.UpdateMovementAndAim(Simulation, leftTargetSpeedFraction, rightTargetSpeedFraction, zoom, brake, brake, camera.Forward); + + if (input.WasPushed(Fire) && frameIndex > lastPlayerShotFrameIndex + 60) { - aiTanks[i].Update(Simulation, bodyProperties, random, frameIndex, playAreaMin, playAreaMax, i, ref aiTanks, ref projectileCount); + playerController.Tank.Fire(Simulation, bodyProperties); + lastPlayerShotFrameIndex = frameIndex; + ++projectileCount; } + } + + for (int i = 0; i < aiTanks.Count; ++i) + { + aiTanks[i].Update(Simulation, bodyProperties, random, frameIndex, playAreaMin, playAreaMax, i, ref aiTanks, ref projectileCount); + } - frameIndex++; - //Ensure that the callbacks list of exploding projectiles can contain all projectiles that exist. - //(We cast the narrowphase to the generic subtype so that we can grab the callbacks. This isn't the only way- - //notice that we cached the bodyProperties reference outside of the callbacks for direct access. - //The exploding projectiles list, however, is a QuickList value type. If we tried to cache it outside we'd only have a copy of it. - //So, rather than trying to set up some pinned memory or replacing it with a reference type, we just cast our way in.) - ref var projectileImpacts = ref ((NarrowPhase)Simulation.NarrowPhase).Callbacks.ProjectileImpacts; - projectileImpacts.EnsureCapacity(projectileCount, BufferPool); - base.Update(window, camera, input, dt); - //Remove any projectile that hit something. - for (int i = 0; i < projectileImpacts.Count; ++i) + frameIndex++; + //Ensure that the callbacks list of exploding projectiles can contain all projectiles that exist. + //(We cast the narrowphase to the generic subtype so that we can grab the callbacks. This isn't the only way- + //notice that we cached the bodyProperties reference outside of the callbacks for direct access. + //The exploding projectiles list, however, is a QuickList value type. If we tried to cache it outside we'd only have a copy of it. + //So, rather than trying to set up some pinned memory or replacing it with a reference type, we just cast our way in.) + ref var projectileImpacts = ref ((NarrowPhase)Simulation.NarrowPhase).Callbacks.ProjectileImpacts; + projectileImpacts.EnsureCapacity(projectileCount, BufferPool); + base.Update(window, camera, input, dt); + //Remove any projectile that hit something. + for (int i = 0; i < projectileImpacts.Count; ++i) + { + ref var impact = ref projectileImpacts[i]; + ref var explosion = ref explosions.Allocate(BufferPool); + explosion.Age = 0; + explosion.Position = Simulation.Bodies[impact.ProjectileHandle].Pose.Position; + explosion.Scale = 1f; + explosion.Color = new Vector3(1f, 0.5f, 0); + Simulation.Bodies.Remove(impact.ProjectileHandle); + if (impact.ImpactedTankBodyHandle.Value >= 0) { - ref var impact = ref projectileImpacts[i]; - ref var explosion = ref explosions.Allocate(BufferPool); - explosion.Age = 0; - explosion.Position = Simulation.Bodies[impact.ProjectileHandle].Pose.Position; - explosion.Scale = 1f; - explosion.Color = new Vector3(1f, 0.5f, 0); - Simulation.Bodies.Remove(impact.ProjectileHandle); - if (impact.ImpactedTankBodyHandle.Value >= 0) + //The projectile hit a tank. Hurt it! + for (int aiIndex = 0; aiIndex < aiTanks.Count; ++aiIndex) { - //The projectile hit a tank. Hurt it! - for (int aiIndex = 0; aiIndex < aiTanks.Count; ++aiIndex) + ref var aiTank = ref aiTanks[aiIndex]; + if (aiTank.Controller.Tank.Body.Value == impact.ImpactedTankBodyHandle.Value) { - ref var aiTank = ref aiTanks[aiIndex]; - if (aiTank.Controller.Tank.Body.Value == impact.ImpactedTankBodyHandle.Value) + --aiTank.HitPoints; + if (aiTank.HitPoints == 0) { - --aiTank.HitPoints; - if (aiTank.HitPoints == 0) - { - ref var deathExplosion = ref explosions.Allocate(BufferPool); - deathExplosion.Position = Simulation.Bodies[aiTank.Controller.Tank.Turret].Pose.Position; - deathExplosion.Scale = 3; - deathExplosion.Age = 0; - deathExplosion.Color = new Vector3(1, 0, 0); - aiTank.Controller.Tank.Explode(Simulation, bodyProperties, BufferPool); - aiTanks.FastRemoveAt(aiIndex); - } - break; + ref var deathExplosion = ref explosions.Allocate(BufferPool); + deathExplosion.Position = Simulation.Bodies[aiTank.Controller.Tank.Turret].Pose.Position; + deathExplosion.Scale = 3; + deathExplosion.Age = 0; + deathExplosion.Color = new Vector3(1, 0, 0); + aiTank.Controller.Tank.Explode(Simulation, bodyProperties, BufferPool); + aiTanks.FastRemoveAt(aiIndex); } + break; } - //This loop might actually fail to find the tank- if a tank gets hit by more than one projectile in a frame, or if the player tank is hit. - //(The player tank cheats and isn't in the aiTanks list.) - //That's fine, though. } + //This loop might actually fail to find the tank- if a tank gets hit by more than one projectile in a frame, or if the player tank is hit. + //(The player tank cheats and isn't in the aiTanks list.) + //That's fine, though. } - projectileImpacts.Count = 0; } + projectileImpacts.Count = 0; + } + + void RenderControl(ref Vector2 position, float textHeight, string controlName, string controlValue, TextBuilder text, TextBatcher textBatcher, Font font) + { + text.Clear().Append(controlName).Append(": ").Append(controlValue); + textBatcher.Write(text, position, textHeight, new Vector3(1), font); + position.Y += textHeight * 1.1f; + } - void RenderControl(ref Vector2 position, float textHeight, string controlName, string controlValue, TextBuilder text, TextBatcher textBatcher, Font font) + public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + { + if (playerControlActive) { - text.Clear().Append(controlName).Append(": ").Append(controlValue); - textBatcher.Write(text, position, textHeight, new Vector3(1), font); - position.Y += textHeight * 1.1f; + var tankBody = new BodyReference(playerController.Tank.Body, Simulation.Bodies); + QuaternionEx.TransformUnitY(tankBody.Pose.Orientation, out var tankUp); + QuaternionEx.TransformUnitZ(tankBody.Pose.Orientation, out var tankBackward); + var backwardDirection = camera.Backward; + backwardDirection.Y = MathF.Max(backwardDirection.Y, -0.2f); + camera.Position = tankBody.Pose.Position + tankUp * 3f + tankBackward * 0.4f + backwardDirection * 8; } - public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + //Draw explosions and remove old ones. + for (int i = explosions.Count - 1; i >= 0; --i) { - if (playerControlActive) + ref var explosion = ref explosions[i]; + var pose = new RigidPose(explosion.Position); + //The age is measured in frames, so it's not framerate independent. That's fine for a demo. + renderer.Shapes.AddShape(new Sphere(explosion.Scale * (0.25f + MathF.Sqrt(explosion.Age))), Simulation.Shapes, pose, explosion.Color); + if (explosion.Age > 5) { - var tankBody = new BodyReference(playerController.Tank.Body, Simulation.Bodies); - QuaternionEx.TransformUnitY(tankBody.Pose.Orientation, out var tankUp); - QuaternionEx.TransformUnitZ(tankBody.Pose.Orientation, out var tankBackward); - var backwardDirection = camera.Backward; - backwardDirection.Y = MathF.Max(backwardDirection.Y, -0.2f); - camera.Position = tankBody.Pose.Position + tankUp * 3f + tankBackward * 0.4f + backwardDirection * 8; - } - - //Draw explosions and remove old ones. - for (int i = explosions.Count - 1; i >= 0; --i) - { - ref var explosion = ref explosions[i]; - var pose = new RigidPose(explosion.Position); - //The age is measured in frames, so it's not framerate independent. That's fine for a demo. - renderer.Shapes.AddShape(new Sphere(explosion.Scale * (0.25f + MathF.Sqrt(explosion.Age))), Simulation.Shapes, pose, explosion.Color); - if (explosion.Age > 5) - { - explosions.FastRemoveAt(i); - } - ++explosion.Age; + explosions.FastRemoveAt(i); } + ++explosion.Age; + } - var textHeight = 16; - var position = new Vector2(32, renderer.Surface.Resolution.Y - 144); - RenderControl(ref position, textHeight, nameof(Fire), ControlStrings.GetName(Fire), text, renderer.TextBatcher, font); - RenderControl(ref position, textHeight, nameof(Forward), ControlStrings.GetName(Forward), text, renderer.TextBatcher, font); - RenderControl(ref position, textHeight, nameof(Backward), ControlStrings.GetName(Backward), text, renderer.TextBatcher, font); - RenderControl(ref position, textHeight, nameof(Right), ControlStrings.GetName(Right), text, renderer.TextBatcher, font); - RenderControl(ref position, textHeight, nameof(Left), ControlStrings.GetName(Left), text, renderer.TextBatcher, font); - RenderControl(ref position, textHeight, nameof(Zoom), ControlStrings.GetName(Zoom), text, renderer.TextBatcher, font); - RenderControl(ref position, textHeight, nameof(Brake), ControlStrings.GetName(Brake), text, renderer.TextBatcher, font); - RenderControl(ref position, textHeight, nameof(ToggleTank), ControlStrings.GetName(ToggleTank), text, renderer.TextBatcher, font); + var textHeight = 16; + var position = new Vector2(32, renderer.Surface.Resolution.Y - 144); + RenderControl(ref position, textHeight, nameof(Fire), ControlStrings.GetName(Fire), text, renderer.TextBatcher, font); + RenderControl(ref position, textHeight, nameof(Forward), ControlStrings.GetName(Forward), text, renderer.TextBatcher, font); + RenderControl(ref position, textHeight, nameof(Backward), ControlStrings.GetName(Backward), text, renderer.TextBatcher, font); + RenderControl(ref position, textHeight, nameof(Right), ControlStrings.GetName(Right), text, renderer.TextBatcher, font); + RenderControl(ref position, textHeight, nameof(Left), ControlStrings.GetName(Left), text, renderer.TextBatcher, font); + RenderControl(ref position, textHeight, nameof(Zoom), ControlStrings.GetName(Zoom), text, renderer.TextBatcher, font); + RenderControl(ref position, textHeight, nameof(Brake), ControlStrings.GetName(Brake), text, renderer.TextBatcher, font); + RenderControl(ref position, textHeight, nameof(ToggleTank), ControlStrings.GetName(ToggleTank), text, renderer.TextBatcher, font); - if (aiTanks.Count > 0) - renderer.TextBatcher.Write(text.Clear().Append("Enemy tanks remaining: ").Append(aiTanks.Count), new Vector2(32, renderer.Surface.Resolution.Y - 172), 24, new Vector3(1, 1, 1), font); - else - renderer.TextBatcher.Write(text.Clear().Append("ya did it!"), new Vector2(32, renderer.Surface.Resolution.Y - 172), 24, new Vector3(0.3f, 1, 0.3f), font); + if (aiTanks.Count > 0) + renderer.TextBatcher.Write(text.Clear().Append("Enemy tanks remaining: ").Append(aiTanks.Count), new Vector2(32, renderer.Surface.Resolution.Y - 172), 24, new Vector3(1, 1, 1), font); + else + renderer.TextBatcher.Write(text.Clear().Append("ya did it!"), new Vector2(32, renderer.Surface.Resolution.Y - 172), 24, new Vector3(0.3f, 1, 0.3f), font); - base.Render(renderer, camera, input, text, font); - } + base.Render(renderer, camera, input, text, font); } } \ No newline at end of file diff --git a/Demos/SpecializedTests/Media/2.4/VideoDancerDemo.cs b/Demos/SpecializedTests/Media/2.4/VideoDancerDemo.cs index c5f45cdec..4f9c78a63 100644 --- a/Demos/SpecializedTests/Media/2.4/VideoDancerDemo.cs +++ b/Demos/SpecializedTests/Media/2.4/VideoDancerDemo.cs @@ -11,188 +11,187 @@ using System; using System.Numerics; -namespace Demos.SpecializedTests.Media +namespace Demos.SpecializedTests.Media; + +/// +/// A bunch of background dancers struggle to keep up with the masterful purple prancer while wearing dresses made of out of balls connected by constraints. +/// Combined with the implementation, this provides a starting point for cosmetic cloth attached to characters. +/// +public class VideoDancerDemo : Demo { - /// - /// A bunch of background dancers struggle to keep up with the masterful purple prancer while wearing dresses made of out of balls connected by constraints. - /// Combined with the implementation, this provides a starting point for cosmetic cloth attached to characters. - /// - public class VideoDancerDemo : Demo + //This demo relies on the DemoDancers to manage all the ragdolls and their simulations. + //All this demo needs to do is make a dress out of balls and drape it onto them. + DemoDancers dancers; + static BodyHandle[,] CreateDressBodyGrid(Vector3 position, int widthInNodes, float spacing, float bodyRadius, float massPerBody, + int instanceId, Simulation simulation, CollidableProperty filters) { - //This demo relies on the DemoDancers to manage all the ragdolls and their simulations. - //All this demo needs to do is make a dress out of balls and drape it onto them. - DemoDancers dancers; - static BodyHandle[,] CreateDressBodyGrid(Vector3 position, int widthInNodes, float spacing, float bodyRadius, float massPerBody, - int instanceId, Simulation simulation, CollidableProperty filters) + var description = BodyDescription.CreateDynamic(QuaternionEx.Identity, new BodyInertia { InverseMass = 1f / massPerBody }, simulation.Shapes.Add(new Sphere(bodyRadius)), 0.01f); + BodyHandle[,] handles = new BodyHandle[widthInNodes, widthInNodes]; + var armHoleCenter = new Vector2(DemoDancers.ArmOffsetX + 0.065f, 0); + var armHoleRadius = 0.095f; + var armHoleRadiusSquared = armHoleRadius * armHoleRadius; + var halfWidth = widthInNodes * spacing / 2; + var halfWidthSquared = halfWidth * halfWidth; + var halfWidthOffset = new Vector2(halfWidth); + for (int rowIndex = 0; rowIndex < widthInNodes; ++rowIndex) { - var description = BodyDescription.CreateDynamic(QuaternionEx.Identity, new BodyInertia { InverseMass = 1f / massPerBody }, simulation.Shapes.Add(new Sphere(bodyRadius)), 0.01f); - BodyHandle[,] handles = new BodyHandle[widthInNodes, widthInNodes]; - var armHoleCenter = new Vector2(DemoDancers.ArmOffsetX + 0.065f, 0); - var armHoleRadius = 0.095f; - var armHoleRadiusSquared = armHoleRadius * armHoleRadius; - var halfWidth = widthInNodes * spacing / 2; - var halfWidthSquared = halfWidth * halfWidth; - var halfWidthOffset = new Vector2(halfWidth); - for (int rowIndex = 0; rowIndex < widthInNodes; ++rowIndex) + for (int columnIndex = 0; columnIndex < widthInNodes; ++columnIndex) { - for (int columnIndex = 0; columnIndex < widthInNodes; ++columnIndex) + var horizontalPosition = new Vector2(columnIndex, rowIndex) * spacing - halfWidthOffset; + var distanceSquared0 = Vector2.DistanceSquared(horizontalPosition, armHoleCenter); + var distanceSquared1 = Vector2.DistanceSquared(horizontalPosition, -armHoleCenter); + var centerDistanceSquared = horizontalPosition.LengthSquared(); + if (distanceSquared0 < armHoleRadiusSquared || distanceSquared1 < armHoleRadiusSquared || centerDistanceSquared > halfWidthSquared) + { + //Too close to an arm or too far from the center, don't create any bodies here. + handles[rowIndex, columnIndex] = new BodyHandle { Value = -1 }; + } + else { - var horizontalPosition = new Vector2(columnIndex, rowIndex) * spacing - halfWidthOffset; - var distanceSquared0 = Vector2.DistanceSquared(horizontalPosition, armHoleCenter); - var distanceSquared1 = Vector2.DistanceSquared(horizontalPosition, -armHoleCenter); - var centerDistanceSquared = horizontalPosition.LengthSquared(); - if (distanceSquared0 < armHoleRadiusSquared || distanceSquared1 < armHoleRadiusSquared || centerDistanceSquared > halfWidthSquared) - { - //Too close to an arm or too far from the center, don't create any bodies here. - handles[rowIndex, columnIndex] = new BodyHandle { Value = -1 }; - } - else - { - description.Pose.Position = new Vector3(horizontalPosition.X, 0, horizontalPosition.Y) + position; - var handle = simulation.Bodies.Add(description); - handles[rowIndex, columnIndex] = handle; - if (filters != null) - filters.Allocate(handle) = new ClothCollisionFilter(rowIndex, columnIndex, instanceId); - } + description.Pose.Position = new Vector3(horizontalPosition.X, 0, horizontalPosition.Y) + position; + var handle = simulation.Bodies.Add(description); + handles[rowIndex, columnIndex] = handle; + if (filters != null) + filters.Allocate(handle) = new ClothCollisionFilter(rowIndex, columnIndex, instanceId); } } - return handles; } - static void CreateDistanceConstraints(BodyHandle[,] bodyHandles, SpringSettings springSettings, Simulation simulation) + return handles; + } + static void CreateDistanceConstraints(BodyHandle[,] bodyHandles, SpringSettings springSettings, Simulation simulation) + { + void CreateConstraintBetweenBodies(BodyHandle aHandle, BodyHandle bHandle) { - void CreateConstraintBetweenBodies(BodyHandle aHandle, BodyHandle bHandle) + //Only create a constraint if bodies on both sides of the pair actually exist. + //In this demo, we use -1 in the body handle slot to represent 'no body'. + if (aHandle.Value >= 0 && bHandle.Value >= 0) { - //Only create a constraint if bodies on both sides of the pair actually exist. - //In this demo, we use -1 in the body handle slot to represent 'no body'. - if (aHandle.Value >= 0 && bHandle.Value >= 0) - { - var a = simulation.Bodies[aHandle]; - var b = simulation.Bodies[bHandle]; - //Note the use of a limit; the distance is allowed to go smaller. - //This helps stop the cloth from having unnatural rigidity. - var distance = Vector3.Distance(a.Pose.Position, b.Pose.Position); - simulation.Solver.Add(aHandle, bHandle, new CenterDistanceLimit(distance * 0.15f, distance, springSettings)); - } + var a = simulation.Bodies[aHandle]; + var b = simulation.Bodies[bHandle]; + //Note the use of a limit; the distance is allowed to go smaller. + //This helps stop the cloth from having unnatural rigidity. + var distance = Vector3.Distance(a.Pose.Position, b.Pose.Position); + simulation.Solver.Add(aHandle, bHandle, new CenterDistanceLimit(distance * 0.15f, distance, springSettings)); } - for (int rowIndex = 0; rowIndex < bodyHandles.GetLength(0); ++rowIndex) + } + for (int rowIndex = 0; rowIndex < bodyHandles.GetLength(0); ++rowIndex) + { + for (int columnIndex = 0; columnIndex < bodyHandles.GetLength(1) - 1; ++columnIndex) { - for (int columnIndex = 0; columnIndex < bodyHandles.GetLength(1) - 1; ++columnIndex) - { - CreateConstraintBetweenBodies(bodyHandles[rowIndex, columnIndex], bodyHandles[rowIndex, columnIndex + 1]); - } + CreateConstraintBetweenBodies(bodyHandles[rowIndex, columnIndex], bodyHandles[rowIndex, columnIndex + 1]); } - for (int rowIndex = 0; rowIndex < bodyHandles.GetLength(0) - 1; ++rowIndex) + } + for (int rowIndex = 0; rowIndex < bodyHandles.GetLength(0) - 1; ++rowIndex) + { + for (int columnIndex = 0; columnIndex < bodyHandles.GetLength(1); ++columnIndex) { - for (int columnIndex = 0; columnIndex < bodyHandles.GetLength(1); ++columnIndex) - { - CreateConstraintBetweenBodies(bodyHandles[rowIndex, columnIndex], bodyHandles[rowIndex + 1, columnIndex]); - } + CreateConstraintBetweenBodies(bodyHandles[rowIndex, columnIndex], bodyHandles[rowIndex + 1, columnIndex]); } - for (int rowIndex = 0; rowIndex < bodyHandles.GetLength(0) - 1; ++rowIndex) + } + for (int rowIndex = 0; rowIndex < bodyHandles.GetLength(0) - 1; ++rowIndex) + { + for (int columnIndex = 0; columnIndex < bodyHandles.GetLength(1) - 1; ++columnIndex) { - for (int columnIndex = 0; columnIndex < bodyHandles.GetLength(1) - 1; ++columnIndex) - { - CreateConstraintBetweenBodies(bodyHandles[rowIndex, columnIndex], bodyHandles[rowIndex + 1, columnIndex + 1]); - CreateConstraintBetweenBodies(bodyHandles[rowIndex, columnIndex + 1], bodyHandles[rowIndex + 1, columnIndex]); - } + CreateConstraintBetweenBodies(bodyHandles[rowIndex, columnIndex], bodyHandles[rowIndex + 1, columnIndex + 1]); + CreateConstraintBetweenBodies(bodyHandles[rowIndex, columnIndex + 1], bodyHandles[rowIndex + 1, columnIndex]); } } + } - static void TailorDress(Simulation simulation, CollidableProperty filters, DancerBodyHandles bodyHandles, int dancerIndex, int dancerGridWidth, float levelOfDetail) + static void TailorDress(Simulation simulation, CollidableProperty filters, DancerBodyHandles bodyHandles, int dancerIndex, int dancerGridWidth, float levelOfDetail) + { + //The demo uses lower resolution grids on dancers further away from the main dancer. + //This is a sorta-example of level of detail. In a 'real' use case, you'd probably want to transition between levels of detail dynamically as the camera moved around. + //That's a little trickier, but doable. Going low to high, for example, requires creating bodies at interpolated positions between existing bodies, while going to a lower level of detail removes them. + levelOfDetail = MathF.Max(0f, MathF.Min(1.5f, levelOfDetail)); + var targetDressDiameter = 2.6f; + var fullDetailWidthInBodies = 40; + float spacingAtFullDetail = targetDressDiameter / fullDetailWidthInBodies; + float bodyRadius = spacingAtFullDetail / 1.75f; + var scale = MathF.Pow(2, levelOfDetail); + var widthInBodies = (int)MathF.Ceiling(fullDetailWidthInBodies / scale); + var spacing = spacingAtFullDetail * scale; + var chest = simulation.Bodies[bodyHandles.Chest]; + ref var chestShape = ref simulation.Shapes.GetShape(chest.Collidable.Shape.Index); + var topOfChestHeight = chest.Pose.Position.Y + chestShape.Radius + bodyRadius; + var bodies = CreateDressBodyGrid(new Vector3(0, topOfChestHeight, 0) + DemoDancers.GetOffsetForDancer(dancerIndex, dancerGridWidth), widthInBodies, spacing, bodyRadius, 0.01f, dancerIndex, simulation, filters); + //Create constraints that bind the cloth bodies closest to the chest, to the chest. This keeps the dress from sliding around. + //In the higher resolution simulations, the arm holes and cloth bodies can actually handle it with no help, but for lower levels of detail it can be useful. + //Also, it's very common to want to control how cloth sticks to a character. You could extend this approach to, for example, keep cloth near the body at the waist like a belt. + //This demo uses constraints to attach a subset of the cloth bodies to the chest. + //You could also either treat the bodies as kinematic and have them follow the body, or attach any constraints that would have involved the cloth body to the body instead. + //Using constraints gives you more options in configuration- the attachment doesn't have to be perfectly rigid. + //For the purposes of this demo, it's also simpler to just use some more constraints. + var midpoint = (widthInBodies * 0.5f - 0.5f); + var zRange = (chestShape.Radius * 0.65f) / spacing; + var xRange = (chestShape.Radius * 0.5f + chestShape.HalfLength) / spacing; + var minX = (int)MathF.Ceiling(midpoint - xRange); + var maxX = (int)(midpoint + xRange); + var minZ = (int)MathF.Ceiling(midpoint - zRange); + var maxZ = (int)(midpoint + zRange); + for (int z = minZ; z <= maxZ; ++z) { - //The demo uses lower resolution grids on dancers further away from the main dancer. - //This is a sorta-example of level of detail. In a 'real' use case, you'd probably want to transition between levels of detail dynamically as the camera moved around. - //That's a little trickier, but doable. Going low to high, for example, requires creating bodies at interpolated positions between existing bodies, while going to a lower level of detail removes them. - levelOfDetail = MathF.Max(0f, MathF.Min(1.5f, levelOfDetail)); - var targetDressDiameter = 2.6f; - var fullDetailWidthInBodies = 40; - float spacingAtFullDetail = targetDressDiameter / fullDetailWidthInBodies; - float bodyRadius = spacingAtFullDetail / 1.75f; - var scale = MathF.Pow(2, levelOfDetail); - var widthInBodies = (int)MathF.Ceiling(fullDetailWidthInBodies / scale); - var spacing = spacingAtFullDetail * scale; - var chest = simulation.Bodies[bodyHandles.Chest]; - ref var chestShape = ref simulation.Shapes.GetShape(chest.Collidable.Shape.Index); - var topOfChestHeight = chest.Pose.Position.Y + chestShape.Radius + bodyRadius; - var bodies = CreateDressBodyGrid(new Vector3(0, topOfChestHeight, 0) + DemoDancers.GetOffsetForDancer(dancerIndex, dancerGridWidth), widthInBodies, spacing, bodyRadius, 0.01f, dancerIndex, simulation, filters); - //Create constraints that bind the cloth bodies closest to the chest, to the chest. This keeps the dress from sliding around. - //In the higher resolution simulations, the arm holes and cloth bodies can actually handle it with no help, but for lower levels of detail it can be useful. - //Also, it's very common to want to control how cloth sticks to a character. You could extend this approach to, for example, keep cloth near the body at the waist like a belt. - //This demo uses constraints to attach a subset of the cloth bodies to the chest. - //You could also either treat the bodies as kinematic and have them follow the body, or attach any constraints that would have involved the cloth body to the body instead. - //Using constraints gives you more options in configuration- the attachment doesn't have to be perfectly rigid. - //For the purposes of this demo, it's also simpler to just use some more constraints. - var midpoint = (widthInBodies * 0.5f - 0.5f); - var zRange = (chestShape.Radius * 0.65f) / spacing; - var xRange = (chestShape.Radius * 0.5f + chestShape.HalfLength) / spacing; - var minX = (int)MathF.Ceiling(midpoint - xRange); - var maxX = (int)(midpoint + xRange); - var minZ = (int)MathF.Ceiling(midpoint - zRange); - var maxZ = (int)(midpoint + zRange); - for (int z = minZ; z <= maxZ; ++z) + for (int x = minX; x <= maxX; ++x) { - for (int x = minX; x <= maxX; ++x) + var clothNodeHandle = bodies[z, x]; + //When creating bodies, we set handles for bodies that don't exist to -1. + if (clothNodeHandle.Value >= 0) { - var clothNodeHandle = bodies[z, x]; - //When creating bodies, we set handles for bodies that don't exist to -1. - if (clothNodeHandle.Value >= 0) - { - var clothNodeBody = simulation.Bodies[clothNodeHandle]; - simulation.Solver.Add(chest.Handle, clothNodeBody.Handle, - new BallSocket - { - LocalOffsetA = QuaternionEx.Transform(clothNodeBody.Pose.Position - chest.Pose.Position, Quaternion.Conjugate(chest.Pose.Orientation)), - SpringSettings = new SpringSettings(30, 1) - }); - } + var clothNodeBody = simulation.Bodies[clothNodeHandle]; + simulation.Solver.Add(chest.Handle, clothNodeBody.Handle, + new BallSocket + { + LocalOffsetA = QuaternionEx.Transform(clothNodeBody.Pose.Position - chest.Pose.Position, Quaternion.Conjugate(chest.Pose.Orientation)), + SpringSettings = new SpringSettings(30, 1) + }); } } - CreateDistanceConstraints(bodies, new SpringSettings(60, 1), simulation); } + CreateDistanceConstraints(bodies, new SpringSettings(60, 1), simulation); + } - public override void Initialize(ContentArchive content, Camera camera) - { - camera.Position = new Vector3(0, 2, 10); - camera.Yaw = 0; - camera.Pitch = 0; + public override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(0, 2, 10); + camera.Yaw = 0; + camera.Pitch = 0; - var collisionFilters = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks(collisionFilters), new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)), new SolveDescription(8, 1)); + var collisionFilters = new CollidableProperty(); + Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks(collisionFilters), new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)), new SolveDescription(8, 1)); - dancers = new DemoDancers().Initialize(40, 40, Simulation, collisionFilters, ThreadDispatcher, BufferPool, new SolveDescription(1, 4), TailorDress, new ClothCollisionFilter(0, 0, -1)); + dancers = new DemoDancers().Initialize(40, 40, Simulation, collisionFilters, ThreadDispatcher, BufferPool, new SolveDescription(1, 4), TailorDress, new ClothCollisionFilter(0, 0, -1)); - } - public override void Update(Window window, Camera camera, Input input, float dt) - { - dancers.UpdateTargets(Simulation); - base.Update(window, camera, input, dt); - } + } + public override void Update(Window window, Camera camera, Input input, float dt) + { + dancers.UpdateTargets(Simulation); + base.Update(window, camera, input, dt); + } - public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) - { - renderer.Shapes.AddInstances(dancers.Simulations, ThreadDispatcher); - renderer.Lines.Extract(dancers.Simulations, ThreadDispatcher); + public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + { + renderer.Shapes.AddInstances(dancers.Simulations, ThreadDispatcher); + renderer.Lines.Extract(dancers.Simulations, ThreadDispatcher); - var resolution = renderer.Surface.Resolution; - //renderer.TextBatcher.Write(text.Clear().Append("Cosmetic simulations, like cloth, often don't need to be in a game's main simulation."), new Vector2(16, resolution.Y - 144), 16, Vector3.One, font); - //renderer.TextBatcher.Write(text.Clear().Append("Every background dancer in this demo has its own simulation. All dancers can be easily updated in parallel."), new Vector2(16, resolution.Y - 128), 16, Vector3.One, font); - //renderer.TextBatcher.Write(text.Clear().Append("Dancers further from the main dancer use sparser cloth and disable self collision for extra performance."), new Vector2(16, resolution.Y - 112), 16, Vector3.One, font); - renderer.TextBatcher.Write(text.Clear().Append("Dancer count: ").Append(dancers.Handles.Length), new Vector2(16, resolution.Y - 80), 16, Vector3.One, font); - renderer.TextBatcher.Write(text.Clear().Append("Total cloth body count: ").Append(dancers.BodyCount), new Vector2(16, resolution.Y - 64), 16, Vector3.One, font); - renderer.TextBatcher.Write(text.Clear().Append("Total cloth constraint count: ").Append(dancers.ConstraintCount), new Vector2(16, resolution.Y - 48), 16, Vector3.One, font); - renderer.TextBatcher.Write(text.Clear().Append("Total dancer execution time (ms): ").Append(dancers.ExecutionTime * 1000, 2), new Vector2(16, resolution.Y - 32), 16, Vector3.One, font); - renderer.TextBatcher.Write(text.Clear().Append("Amortized execution time per dancer (us): ").Append(dancers.ExecutionTime * 1e6 / dancers.Handles.Length, 1), new Vector2(16, resolution.Y - 16), 16, Vector3.One, font); + var resolution = renderer.Surface.Resolution; + //renderer.TextBatcher.Write(text.Clear().Append("Cosmetic simulations, like cloth, often don't need to be in a game's main simulation."), new Vector2(16, resolution.Y - 144), 16, Vector3.One, font); + //renderer.TextBatcher.Write(text.Clear().Append("Every background dancer in this demo has its own simulation. All dancers can be easily updated in parallel."), new Vector2(16, resolution.Y - 128), 16, Vector3.One, font); + //renderer.TextBatcher.Write(text.Clear().Append("Dancers further from the main dancer use sparser cloth and disable self collision for extra performance."), new Vector2(16, resolution.Y - 112), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Dancer count: ").Append(dancers.Handles.Length), new Vector2(16, resolution.Y - 80), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Total cloth body count: ").Append(dancers.BodyCount), new Vector2(16, resolution.Y - 64), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Total cloth constraint count: ").Append(dancers.ConstraintCount), new Vector2(16, resolution.Y - 48), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Total dancer execution time (ms): ").Append(dancers.ExecutionTime * 1000, 2), new Vector2(16, resolution.Y - 32), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Amortized execution time per dancer (us): ").Append(dancers.ExecutionTime * 1e6 / dancers.Handles.Length, 1), new Vector2(16, resolution.Y - 16), 16, Vector3.One, font); - base.Render(renderer, camera, input, text, font); - } + base.Render(renderer, camera, input, text, font); + } - protected override void OnDispose() - { - dancers.Dispose(BufferPool); + protected override void OnDispose() + { + dancers.Dispose(BufferPool); - } } } diff --git a/Demos/SpecializedTests/Media/2.4/VideoPlumpDancerDemo.cs b/Demos/SpecializedTests/Media/2.4/VideoPlumpDancerDemo.cs index 9927aea55..9ca6f38b9 100644 --- a/Demos/SpecializedTests/Media/2.4/VideoPlumpDancerDemo.cs +++ b/Demos/SpecializedTests/Media/2.4/VideoPlumpDancerDemo.cs @@ -12,243 +12,242 @@ using System.Diagnostics; using System.Numerics; -namespace Demos.SpecializedTests.Media +namespace Demos.SpecializedTests.Media; + +/// +/// A bunch of somewhat overweight background dancers struggle to keep up with the masterful purple prancer. +/// Combined with the implementation, this shows an example of how cosmetic deformable physics could be applied to characters. +/// +public class VideoPlumpDancerDemo : Demo { - /// - /// A bunch of somewhat overweight background dancers struggle to keep up with the masterful purple prancer. - /// Combined with the implementation, this shows an example of how cosmetic deformable physics could be applied to characters. - /// - public class VideoPlumpDancerDemo : Demo + //This demo relies on the DemoDancers to manage all the ragdolls and their simulations. + //All this demo needs to do is make the fatsuits. + DemoDancers dancers; + + //While creating the fatsuits, we'll precompute some stuff to make testing a little quicker. + //Could make this significantly faster still by being trickier with vectorization, but the demo launch time is already reasonable. + struct TestCapsule { - //This demo relies on the DemoDancers to manage all the ragdolls and their simulations. - //All this demo needs to do is make the fatsuits. - DemoDancers dancers; + public Vector3 Start; + public Vector3 Direction; + public float Length; + public float Radius; + } + static TestCapsule CreateTestCapsule(Simulation simulation, BodyHandle handle) + { + var body = simulation.Bodies[handle]; + Debug.Assert(body.Collidable.Shape.Type == Capsule.Id, "For the purposes of this demo, we assume that all of the bodies that are being tested are capsules."); + ref var shape = ref simulation.Shapes.GetShape(body.Collidable.Shape.Index); + var pose = body.Pose; + TestCapsule toReturn; + QuaternionEx.TransformUnitY(pose.Orientation, out toReturn.Direction); + toReturn.Start = pose.Position - toReturn.Direction * shape.HalfLength; + toReturn.Radius = shape.Radius; + toReturn.Length = shape.HalfLength * 2; + return toReturn; - //While creating the fatsuits, we'll precompute some stuff to make testing a little quicker. - //Could make this significantly faster still by being trickier with vectorization, but the demo launch time is already reasonable. - struct TestCapsule + } + unsafe static void CreateBodyGrid(DancerBodyHandles bodyHandles, Int3 axisSizeInBodies, Vector3 gridMinimum, Vector3 gridMaximum, float bodyRadius, float massPerBody, + int instanceId, Simulation simulation, CollidableProperty filters) + { + var shape = new Sphere(bodyRadius); + var shapeIndex = simulation.Shapes.Add(shape); + //Note that, unlike the DancerDemo where cloth nodes cannot rotate, the deformable sub-bodies can rotate. + //That's because this demo is going to connect bodies together using Weld constraints, which control all six degrees of freedom. + //You could also use a CenterDistanceConstraint/Limit with VolumeConstraints to maintain shape, but a bunch of Weld constraints is a little simpler. + var description = BodyDescription.CreateDynamic(QuaternionEx.Identity, shape.ComputeInertia(massPerBody), shapeIndex, 0.01f); + BodyHandle[,,] handles = new BodyHandle[axisSizeInBodies.X, axisSizeInBodies.Y, axisSizeInBodies.Z]; + BodyHandle[,,] nearestHandles = new BodyHandle[axisSizeInBodies.X, axisSizeInBodies.Y, axisSizeInBodies.Z]; + var gridSpan = gridMaximum - gridMinimum; + var gridSpacing = gridSpan / new Vector3(axisSizeInBodies.X - 1, axisSizeInBodies.Y - 1, axisSizeInBodies.Z - 1); + Span testCapsules = stackalloc TestCapsule[11]; + + //DancerBodyHandles stores the head last, so we can just check the first 11 bodies that are all capsules. The head isn't going to be covered in the fatsuit, so it doesn't need to be checked anyway. + var handlesBuffer = DancerBodyHandles.AsBuffer(&bodyHandles); + for (int i = 0; i < 11; ++i) { - public Vector3 Start; - public Vector3 Direction; - public float Length; - public float Radius; + testCapsules[i] = CreateTestCapsule(simulation, handlesBuffer[i]); } - static TestCapsule CreateTestCapsule(Simulation simulation, BodyHandle handle) - { - var body = simulation.Bodies[handle]; - Debug.Assert(body.Collidable.Shape.Type == Capsule.Id, "For the purposes of this demo, we assume that all of the bodies that are being tested are capsules."); - ref var shape = ref simulation.Shapes.GetShape(body.Collidable.Shape.Index); - var pose = body.Pose; - TestCapsule toReturn; - QuaternionEx.TransformUnitY(pose.Orientation, out toReturn.Direction); - toReturn.Start = pose.Position - toReturn.Direction * shape.HalfLength; - toReturn.Radius = shape.Radius; - toReturn.Length = shape.HalfLength * 2; - return toReturn; + var center = (gridMinimum + gridMaximum) * 0.5f; - } - unsafe static void CreateBodyGrid(DancerBodyHandles bodyHandles, Int3 axisSizeInBodies, Vector3 gridMinimum, Vector3 gridMaximum, float bodyRadius, float massPerBody, - int instanceId, Simulation simulation, CollidableProperty filters) + for (int x = 0; x < axisSizeInBodies.X; ++x) { - var shape = new Sphere(bodyRadius); - var shapeIndex = simulation.Shapes.Add(shape); - //Note that, unlike the DancerDemo where cloth nodes cannot rotate, the deformable sub-bodies can rotate. - //That's because this demo is going to connect bodies together using Weld constraints, which control all six degrees of freedom. - //You could also use a CenterDistanceConstraint/Limit with VolumeConstraints to maintain shape, but a bunch of Weld constraints is a little simpler. - var description = BodyDescription.CreateDynamic(QuaternionEx.Identity, shape.ComputeInertia(massPerBody), shapeIndex, 0.01f); - BodyHandle[,,] handles = new BodyHandle[axisSizeInBodies.X, axisSizeInBodies.Y, axisSizeInBodies.Z]; - BodyHandle[,,] nearestHandles = new BodyHandle[axisSizeInBodies.X, axisSizeInBodies.Y, axisSizeInBodies.Z]; - var gridSpan = gridMaximum - gridMinimum; - var gridSpacing = gridSpan / new Vector3(axisSizeInBodies.X - 1, axisSizeInBodies.Y - 1, axisSizeInBodies.Z - 1); - Span testCapsules = stackalloc TestCapsule[11]; - - //DancerBodyHandles stores the head last, so we can just check the first 11 bodies that are all capsules. The head isn't going to be covered in the fatsuit, so it doesn't need to be checked anyway. - var handlesBuffer = DancerBodyHandles.AsBuffer(&bodyHandles); - for (int i = 0; i < 11; ++i) + for (int y = 0; y < axisSizeInBodies.Y; ++y) { - testCapsules[i] = CreateTestCapsule(simulation, handlesBuffer[i]); - } - var center = (gridMinimum + gridMaximum) * 0.5f; - - for (int x = 0; x < axisSizeInBodies.X; ++x) - { - for (int y = 0; y < axisSizeInBodies.Y; ++y) + for (int z = 0; z < axisSizeInBodies.Z; ++z) { - for (int z = 0; z < axisSizeInBodies.Z; ++z) + var position = gridMinimum + gridSpacing * new Vector3(x, y, z); + float minimumDistance = float.MaxValue; + int minimumIndex = 0; + for (int i = 0; i < testCapsules.Length; ++i) { - var position = gridMinimum + gridSpacing * new Vector3(x, y, z); - float minimumDistance = float.MaxValue; - int minimumIndex = 0; - for (int i = 0; i < testCapsules.Length; ++i) + var testCapsule = testCapsules[i]; + var distance = Vector3.Distance(position, testCapsule.Start + MathF.Max(0, MathF.Min(testCapsule.Length, Vector3.Dot(position - testCapsule.Start, testCapsule.Direction))) * testCapsule.Direction) - testCapsule.Radius; + if (distance < minimumDistance) { - var testCapsule = testCapsules[i]; - var distance = Vector3.Distance(position, testCapsule.Start + MathF.Max(0, MathF.Min(testCapsule.Length, Vector3.Dot(position - testCapsule.Start, testCapsule.Direction))) * testCapsule.Direction) - testCapsule.Radius; - if (distance < minimumDistance) - { - minimumDistance = distance; - minimumIndex = i; - } + minimumDistance = distance; + minimumIndex = i; } - nearestHandles[x, y, z] = handlesBuffer[minimumIndex]; + } + nearestHandles[x, y, z] = handlesBuffer[minimumIndex]; - var maximumDistanceForCreatingNodes = MathF.Max(0.1f, 0.8f - 1.5f * Vector3.Distance(position, center)); - if (minimumDistance < bodyRadius) - { - //Intersecting; don't create a body. -2 for this demo marks the body as intersecting, so we can disambiguate it from slots that are just empty due to being too far away. - handles[x, y, z] = new BodyHandle { Value = -2 }; - } - else if (minimumDistance > maximumDistanceForCreatingNodes) - { - //-1 means too far. - handles[x, y, z] = new BodyHandle { Value = -1 }; - } - else - { - //Nearby. Create and attach it to the nearest body part. - description.Pose.Position = position; - var handle = simulation.Bodies.Add(description); - handles[x, y, z] = handle; - if (filters != null) - filters.Allocate(handle) = new DeformableCollisionFilter(x, y, z, instanceId); - - var nearestHandle = handlesBuffer[minimumIndex]; - var nearestPose = simulation.Bodies[nearestHandle].Pose; - var conjugate = Quaternion.Conjugate(nearestPose.Orientation); + var maximumDistanceForCreatingNodes = MathF.Max(0.1f, 0.8f - 1.5f * Vector3.Distance(position, center)); + if (minimumDistance < bodyRadius) + { + //Intersecting; don't create a body. -2 for this demo marks the body as intersecting, so we can disambiguate it from slots that are just empty due to being too far away. + handles[x, y, z] = new BodyHandle { Value = -2 }; + } + else if (minimumDistance > maximumDistanceForCreatingNodes) + { + //-1 means too far. + handles[x, y, z] = new BodyHandle { Value = -1 }; + } + else + { + //Nearby. Create and attach it to the nearest body part. + description.Pose.Position = position; + var handle = simulation.Bodies.Add(description); + handles[x, y, z] = handle; + if (filters != null) + filters.Allocate(handle) = new DeformableCollisionFilter(x, y, z, instanceId); + + var nearestHandle = handlesBuffer[minimumIndex]; + var nearestPose = simulation.Bodies[nearestHandle].Pose; + var conjugate = Quaternion.Conjugate(nearestPose.Orientation); - } } } } - for (int x = 0; x < axisSizeInBodies.X; ++x) + } + for (int x = 0; x < axisSizeInBodies.X; ++x) + { + for (int y = 0; y < axisSizeInBodies.Y; ++y) { - for (int y = 0; y < axisSizeInBodies.Y; ++y) + for (int z = 0; z < axisSizeInBodies.Z; ++z) { - for (int z = 0; z < axisSizeInBodies.Z; ++z) + //Kind of hacky, but simple: for every node that is exposed to the air (a neighbor has a body handle flagged as -1), make sure it has a collidable. + //Anything inside doesn't need a collidable. + var handle = handles[x, y, z]; + if (handle.Value >= 0) { - //Kind of hacky, but simple: for every node that is exposed to the air (a neighbor has a body handle flagged as -1), make sure it has a collidable. - //Anything inside doesn't need a collidable. - var handle = handles[x, y, z]; - if (handle.Value >= 0) + var needsAnchor = + (x != 0 && handles[x - 1, y, z].Value == -2) || + (x != handles.GetLength(0) - 1 && handles[x + 1, y, z].Value == -2) || + (y != 0 && handles[x, y - 1, z].Value == -2) || + (y != handles.GetLength(1) - 1 && handles[x, y + 1, z].Value == -2) || + (z != 0 && handles[x, y, z - 1].Value == -2) || + (z != handles.GetLength(2) - 1 && handles[x, y, z + 1].Value == -2); + var source = simulation.Bodies[handle]; + if (needsAnchor) { - var needsAnchor = - (x != 0 && handles[x - 1, y, z].Value == -2) || - (x != handles.GetLength(0) - 1 && handles[x + 1, y, z].Value == -2) || - (y != 0 && handles[x, y - 1, z].Value == -2) || - (y != handles.GetLength(1) - 1 && handles[x, y + 1, z].Value == -2) || - (z != 0 && handles[x, y, z - 1].Value == -2) || - (z != handles.GetLength(2) - 1 && handles[x, y, z + 1].Value == -2); - var source = simulation.Bodies[handle]; - if (needsAnchor) - { - var nearestHandle = nearestHandles[x, y, z]; - var nearestPose = simulation.Bodies[nearestHandle].Pose; - var conjugate = Quaternion.Conjugate(nearestPose.Orientation); - simulation.Solver.Add(nearestHandle, handle, new Weld - { - LocalOffset = QuaternionEx.Transform(source.Pose.Position - nearestPose.Position, conjugate), - LocalOrientation = conjugate, - SpringSettings = new SpringSettings(6, 0.4f) - }); - } - var needsCollidable = - (x == 0 || handles[x - 1, y, z].Value == -1) || (x == handles.GetLength(0) - 1 || handles[x + 1, y, z].Value == -1) || - (y == 0 || handles[x, y - 1, z].Value == -1) || (y == handles.GetLength(1) - 1 || handles[x, y + 1, z].Value == -1) || - (z == 0 || handles[x, y, z - 1].Value == -1) || (z == handles.GetLength(2) - 1 || handles[x, y, z + 1].Value == -1); - if (!needsCollidable) + var nearestHandle = nearestHandles[x, y, z]; + var nearestPose = simulation.Bodies[nearestHandle].Pose; + var conjugate = Quaternion.Conjugate(nearestPose.Orientation); + simulation.Solver.Add(nearestHandle, handle, new Weld { - source.SetShape(default); - } + LocalOffset = QuaternionEx.Transform(source.Pose.Position - nearestPose.Position, conjugate), + LocalOrientation = conjugate, + SpringSettings = new SpringSettings(6, 0.4f) + }); + } + var needsCollidable = + (x == 0 || handles[x - 1, y, z].Value == -1) || (x == handles.GetLength(0) - 1 || handles[x + 1, y, z].Value == -1) || + (y == 0 || handles[x, y - 1, z].Value == -1) || (y == handles.GetLength(1) - 1 || handles[x, y + 1, z].Value == -1) || + (z == 0 || handles[x, y, z - 1].Value == -1) || (z == handles.GetLength(2) - 1 || handles[x, y, z + 1].Value == -1); + if (!needsCollidable) + { + source.SetShape(default); + } - static void TryAdd(Simulation simulation, BodyReference source, BodyHandle targetHandle) - { - if (targetHandle.Value >= 0) - { - var target = simulation.Bodies[targetHandle]; - simulation.Solver.Add(source.Handle, targetHandle, new Weld { LocalOffset = target.Pose.Position - source.Pose.Position, LocalOrientation = Quaternion.Identity, SpringSettings = new SpringSettings(6, 0.4f) }); - } - } - if (x < handles.GetLength(0) - 1) - { - TryAdd(simulation, source, handles[x + 1, y, z]); - } - if (y < handles.GetLength(1) - 1) - { - TryAdd(simulation, source, handles[x, y + 1, z]); - } - if (z < handles.GetLength(2) - 1) + static void TryAdd(Simulation simulation, BodyReference source, BodyHandle targetHandle) + { + if (targetHandle.Value >= 0) { - TryAdd(simulation, source, handles[x, y, z + 1]); + var target = simulation.Bodies[targetHandle]; + simulation.Solver.Add(source.Handle, targetHandle, new Weld { LocalOffset = target.Pose.Position - source.Pose.Position, LocalOrientation = Quaternion.Identity, SpringSettings = new SpringSettings(6, 0.4f) }); } } + if (x < handles.GetLength(0) - 1) + { + TryAdd(simulation, source, handles[x + 1, y, z]); + } + if (y < handles.GetLength(1) - 1) + { + TryAdd(simulation, source, handles[x, y + 1, z]); + } + if (z < handles.GetLength(2) - 1) + { + TryAdd(simulation, source, handles[x, y, z + 1]); + } } } } - } + } - static void CreateFatSuit(Simulation simulation, CollidableProperty filters, DancerBodyHandles bodyHandles, int dancerIndex, int dancerGridWidth, float levelOfDetail) - { - //The demo uses lower resolution grids on dancers further away from the main dancer. - //This is a sorta-example of level of detail. In a 'real' use case, you'd probably want to transition between levels of detail dynamically as the camera moved around. - //That's a little trickier, but doable. Going low to high, for example, requires creating bodies at interpolated positions between existing bodies, while going to a lower level of detail removes them. - levelOfDetail = MathF.Max(0f, MathF.Min(0.8f, levelOfDetail)); - var suitSize = new Vector3(1, 1f, 1); - var fullDetailAxisBodyCounts = new Int3 { X = 23, Y = 23, Z = 23 }; - var scale = MathF.Pow(2, levelOfDetail); - var axisBodyCounts = new Int3 { X = (int)MathF.Ceiling(fullDetailAxisBodyCounts.X / scale), Y = (int)MathF.Ceiling(fullDetailAxisBodyCounts.Y / scale), Z = (int)MathF.Ceiling(fullDetailAxisBodyCounts.Z / scale) }; - var bodyRadius = MathF.Min(suitSize.X / axisBodyCounts.X, MathF.Min(suitSize.Y / axisBodyCounts.Y, suitSize.Z / axisBodyCounts.Z)); - - var chest = simulation.Bodies[bodyHandles.Chest]; - ref var chestShape = ref simulation.Shapes.GetShape(chest.Collidable.Shape.Index); - var topOfChestHeight = chest.Pose.Position.Y + chestShape.Radius; - var topOfChestPosition = new Vector3(0, topOfChestHeight, 0) + DemoDancers.GetOffsetForDancer(dancerIndex, dancerGridWidth); - var suitMinimum = topOfChestPosition - suitSize * new Vector3(0.5f, 1f, 0.5f); - var suitMaximum = suitMinimum + suitSize; - CreateBodyGrid(bodyHandles, axisBodyCounts, suitMinimum, suitMaximum, bodyRadius, 0.01f, dancerIndex, simulation, filters); - } + static void CreateFatSuit(Simulation simulation, CollidableProperty filters, DancerBodyHandles bodyHandles, int dancerIndex, int dancerGridWidth, float levelOfDetail) + { + //The demo uses lower resolution grids on dancers further away from the main dancer. + //This is a sorta-example of level of detail. In a 'real' use case, you'd probably want to transition between levels of detail dynamically as the camera moved around. + //That's a little trickier, but doable. Going low to high, for example, requires creating bodies at interpolated positions between existing bodies, while going to a lower level of detail removes them. + levelOfDetail = MathF.Max(0f, MathF.Min(0.8f, levelOfDetail)); + var suitSize = new Vector3(1, 1f, 1); + var fullDetailAxisBodyCounts = new Int3 { X = 23, Y = 23, Z = 23 }; + var scale = MathF.Pow(2, levelOfDetail); + var axisBodyCounts = new Int3 { X = (int)MathF.Ceiling(fullDetailAxisBodyCounts.X / scale), Y = (int)MathF.Ceiling(fullDetailAxisBodyCounts.Y / scale), Z = (int)MathF.Ceiling(fullDetailAxisBodyCounts.Z / scale) }; + var bodyRadius = MathF.Min(suitSize.X / axisBodyCounts.X, MathF.Min(suitSize.Y / axisBodyCounts.Y, suitSize.Z / axisBodyCounts.Z)); + + var chest = simulation.Bodies[bodyHandles.Chest]; + ref var chestShape = ref simulation.Shapes.GetShape(chest.Collidable.Shape.Index); + var topOfChestHeight = chest.Pose.Position.Y + chestShape.Radius; + var topOfChestPosition = new Vector3(0, topOfChestHeight, 0) + DemoDancers.GetOffsetForDancer(dancerIndex, dancerGridWidth); + var suitMinimum = topOfChestPosition - suitSize * new Vector3(0.5f, 1f, 0.5f); + var suitMaximum = suitMinimum + suitSize; + CreateBodyGrid(bodyHandles, axisBodyCounts, suitMinimum, suitMaximum, bodyRadius, 0.01f, dancerIndex, simulation, filters); + } - public override void Initialize(ContentArchive content, Camera camera) - { - camera.Position = new Vector3(0, 2, 10); - camera.Yaw = 0; - camera.Pitch = 0; - var collisionFilters = new CollidableProperty(); - Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks(collisionFilters), new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)), new SolveDescription(8, 1)); + public override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(0, 2, 10); + camera.Yaw = 0; + camera.Pitch = 0; - //Note that, because the constraints in the fat suit are quite soft, we can get away with extremely minimal solving time. There's one substep with one velocity iteration. - dancers = new DemoDancers().Initialize(32, 32, Simulation, collisionFilters, ThreadDispatcher, BufferPool, new SolveDescription(1, 1), CreateFatSuit, new DeformableCollisionFilter(0, 0, 0, -1)); + var collisionFilters = new CollidableProperty(); + Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks(collisionFilters), new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)), new SolveDescription(8, 1)); - } - public override void Update(Window window, Camera camera, Input input, float dt) - { - dancers.UpdateTargets(Simulation); - base.Update(window, camera, input, dt); - } + //Note that, because the constraints in the fat suit are quite soft, we can get away with extremely minimal solving time. There's one substep with one velocity iteration. + dancers = new DemoDancers().Initialize(32, 32, Simulation, collisionFilters, ThreadDispatcher, BufferPool, new SolveDescription(1, 1), CreateFatSuit, new DeformableCollisionFilter(0, 0, 0, -1)); - public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) - { - renderer.Shapes.AddInstances(dancers.Simulations, ThreadDispatcher); - renderer.Lines.Extract(dancers.Simulations, ThreadDispatcher); - - var resolution = renderer.Surface.Resolution; - //renderer.TextBatcher.Write(text.Clear().Append("Cosmetic simulations, like character blubber, often don't need to be in a game's main simulation."), new Vector2(16, resolution.Y - 144), 16, Vector3.One, font); - //renderer.TextBatcher.Write(text.Clear().Append("Every background dancer in this demo has its own simulation. All dancers can be easily updated in parallel."), new Vector2(16, resolution.Y - 128), 16, Vector3.One, font); - //renderer.TextBatcher.Write(text.Clear().Append("Dancers further from the main dancer use sparser body grids and disable self collision for extra performance."), new Vector2(16, resolution.Y - 112), 16, Vector3.One, font); - renderer.TextBatcher.Write(text.Clear().Append("Dancer count: ").Append(dancers.Handles.Length), new Vector2(16, resolution.Y - 80), 16, Vector3.One, font); - renderer.TextBatcher.Write(text.Clear().Append("Total deformable body count: ").Append(dancers.BodyCount), new Vector2(16, resolution.Y - 64), 16, Vector3.One, font); - renderer.TextBatcher.Write(text.Clear().Append("Total deformable constraint count: ").Append(dancers.ConstraintCount), new Vector2(16, resolution.Y - 48), 16, Vector3.One, font); - renderer.TextBatcher.Write(text.Clear().Append("Total dancer execution time (ms): ").Append(dancers.ExecutionTime * 1000, 2), new Vector2(16, resolution.Y - 32), 16, Vector3.One, font); - renderer.TextBatcher.Write(text.Clear().Append("Amortized execution time per dancer (us): ").Append(dancers.ExecutionTime * 1e6 / dancers.Handles.Length, 1), new Vector2(16, resolution.Y - 16), 16, Vector3.One, font); - - base.Render(renderer, camera, input, text, font); - } - protected override void OnDispose() - { - dancers.Dispose(BufferPool); + } + public override void Update(Window window, Camera camera, Input input, float dt) + { + dancers.UpdateTargets(Simulation); + base.Update(window, camera, input, dt); + } + + public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + { + renderer.Shapes.AddInstances(dancers.Simulations, ThreadDispatcher); + renderer.Lines.Extract(dancers.Simulations, ThreadDispatcher); + + var resolution = renderer.Surface.Resolution; + //renderer.TextBatcher.Write(text.Clear().Append("Cosmetic simulations, like character blubber, often don't need to be in a game's main simulation."), new Vector2(16, resolution.Y - 144), 16, Vector3.One, font); + //renderer.TextBatcher.Write(text.Clear().Append("Every background dancer in this demo has its own simulation. All dancers can be easily updated in parallel."), new Vector2(16, resolution.Y - 128), 16, Vector3.One, font); + //renderer.TextBatcher.Write(text.Clear().Append("Dancers further from the main dancer use sparser body grids and disable self collision for extra performance."), new Vector2(16, resolution.Y - 112), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Dancer count: ").Append(dancers.Handles.Length), new Vector2(16, resolution.Y - 80), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Total deformable body count: ").Append(dancers.BodyCount), new Vector2(16, resolution.Y - 64), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Total deformable constraint count: ").Append(dancers.ConstraintCount), new Vector2(16, resolution.Y - 48), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Total dancer execution time (ms): ").Append(dancers.ExecutionTime * 1000, 2), new Vector2(16, resolution.Y - 32), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Amortized execution time per dancer (us): ").Append(dancers.ExecutionTime * 1e6 / dancers.Handles.Length, 1), new Vector2(16, resolution.Y - 16), 16, Vector3.One, font); + + base.Render(renderer, camera, input, text, font); + } + protected override void OnDispose() + { + dancers.Dispose(BufferPool); - } } } diff --git a/Demos/SpecializedTests/MeshMeshTestDemo.cs b/Demos/SpecializedTests/MeshMeshTestDemo.cs index 164579779..fd530e994 100644 --- a/Demos/SpecializedTests/MeshMeshTestDemo.cs +++ b/Demos/SpecializedTests/MeshMeshTestDemo.cs @@ -6,47 +6,46 @@ using DemoContentLoader; using DemoRenderer; -namespace Demos.SpecializedTests +namespace Demos.SpecializedTests; + +/// +/// Shows how to be mean to the physics engine by using meshes as dynamic colliders. Why would someone be so cruel? Be nice to the physics engine, save your CPU some work. +/// +public class MeshMeshTestDemo : Demo { - /// - /// Shows how to be mean to the physics engine by using meshes as dynamic colliders. Why would someone be so cruel? Be nice to the physics engine, save your CPU some work. - /// - public class MeshMeshTestDemo : Demo + public override void Initialize(ContentArchive content, Camera camera) { - public override void Initialize(ContentArchive content, Camera camera) - { - camera.Position = new Vector3(0, 8, -10); - camera.Yaw = MathHelper.Pi; + camera.Position = new Vector3(0, 8, -10); + camera.Yaw = MathHelper.Pi; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); - var mesh = DemoMeshHelper.LoadModel(content, BufferPool, @"Content\newt.obj", Vector3.One); - var approximateInertia = new Box(2.5f, 1, 4).ComputeInertia(1); - var meshShapeIndex = Simulation.Shapes.Add(mesh); - for (int meshIndex = 0; meshIndex < 3; ++meshIndex) - { - Simulation.Bodies.Add( - BodyDescription.CreateDynamic(new Vector3(0, 2 + meshIndex * 2, 0), approximateInertia, meshShapeIndex, 0.01f)); - } + var mesh = DemoMeshHelper.LoadModel(content, BufferPool, @"Content\newt.obj", Vector3.One); + var approximateInertia = new Box(2.5f, 1, 4).ComputeInertia(1); + var meshShapeIndex = Simulation.Shapes.Add(mesh); + for (int meshIndex = 0; meshIndex < 3; ++meshIndex) + { + Simulation.Bodies.Add( + BodyDescription.CreateDynamic(new Vector3(0, 2 + meshIndex * 2, 0), approximateInertia, meshShapeIndex, 0.01f)); + } - var compoundBuilder = new CompoundBuilder(BufferPool, Simulation.Shapes, 12); - for (int i = 0; i < mesh.Triangles.Length; ++i) - { - compoundBuilder.Add(mesh.Triangles[i], RigidPose.Identity, 1); - } - compoundBuilder.BuildDynamicCompound(out var children, out var compoundInertia); - var compound = new BigCompound(children, Simulation.Shapes, BufferPool); - var compoundShapeIndex = Simulation.Shapes.Add(compound); - compoundBuilder.Dispose(); - for (int i = 0; i < 3; ++i) - { - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(5, 2 + i * 2, 0), compoundInertia, compoundShapeIndex, 0.01f)); - } + var compoundBuilder = new CompoundBuilder(BufferPool, Simulation.Shapes, 12); + for (int i = 0; i < mesh.Triangles.Length; ++i) + { + compoundBuilder.Add(mesh.Triangles[i], RigidPose.Identity, 1); + } + compoundBuilder.BuildDynamicCompound(out var children, out var compoundInertia); + var compound = new BigCompound(children, Simulation.Shapes, BufferPool); + var compoundShapeIndex = Simulation.Shapes.Add(compound); + compoundBuilder.Dispose(); + for (int i = 0; i < 3; ++i) + { + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(5, 2 + i * 2, 0), compoundInertia, compoundShapeIndex, 0.01f)); + } - var staticShape = new Box(1500, 1, 1500); - var staticShapeIndex = Simulation.Shapes.Add(staticShape); + var staticShape = new Box(1500, 1, 1500); + var staticShapeIndex = Simulation.Shapes.Add(staticShape); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), staticShapeIndex)); - } + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), staticShapeIndex)); } } diff --git a/Demos/SpecializedTests/MeshReductionTestDemo.cs b/Demos/SpecializedTests/MeshReductionTestDemo.cs index c6d79f67b..dc3fee749 100644 --- a/Demos/SpecializedTests/MeshReductionTestDemo.cs +++ b/Demos/SpecializedTests/MeshReductionTestDemo.cs @@ -8,164 +8,163 @@ using DemoContentLoader; using DemoRenderer; -namespace Demos.SpecializedTests +namespace Demos.SpecializedTests; + +public class MeshReductionTestDemo : Demo { - public class MeshReductionTestDemo : Demo - { - public override void Initialize(ContentArchive content, Camera camera) - { - camera.Position = new Vector3(0, 5, 10); - camera.Yaw = 0; - camera.Pitch = 0; + public override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(0, 5, 10); + camera.Yaw = 0; + camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1), 2, 0), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1), 2, 0), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); - var builder = new CompoundBuilder(BufferPool, Simulation.Shapes, 2); - builder.Add(new Box(1.85f, 0.7f, 4.73f), RigidPose.Identity, 10); - builder.Add(new Box(1.85f, 0.6f, 2.5f), new Vector3(0, 0.65f, -0.35f), 0.5f); - builder.BuildDynamicCompound(out var children, out var bodyInertia, out _); - builder.Dispose(); - var bodyShape = new Compound(children); - var bodyShapeIndex = Simulation.Shapes.Add(bodyShape); - var wheelShape = new Cylinder(0.4f, .18f); - var wheelInertia = wheelShape.ComputeInertia(0.25f); - var wheelShapeIndex = Simulation.Shapes.Add(wheelShape); + var builder = new CompoundBuilder(BufferPool, Simulation.Shapes, 2); + builder.Add(new Box(1.85f, 0.7f, 4.73f), RigidPose.Identity, 10); + builder.Add(new Box(1.85f, 0.6f, 2.5f), new Vector3(0, 0.65f, -0.35f), 0.5f); + builder.BuildDynamicCompound(out var children, out var bodyInertia, out _); + builder.Dispose(); + var bodyShape = new Compound(children); + var bodyShapeIndex = Simulation.Shapes.Add(bodyShape); + var wheelShape = new Cylinder(0.4f, .18f); + var wheelInertia = wheelShape.ComputeInertia(0.25f); + var wheelShapeIndex = Simulation.Shapes.Add(wheelShape); - const int planeWidth = 257; - const float scale = 3; - Vector2 terrainPosition = new Vector2(1 - planeWidth, 1 - planeWidth) * scale * 0.5f; + const int planeWidth = 257; + const float scale = 3; + Vector2 terrainPosition = new Vector2(1 - planeWidth, 1 - planeWidth) * scale * 0.5f; - Vector3 min = new Vector3(-planeWidth * scale * 0.45f, 10, -planeWidth * scale * 0.45f); - Vector3 span = new Vector3(planeWidth * scale * 0.9f, 15, planeWidth * scale * 0.9f); + Vector3 min = new Vector3(-planeWidth * scale * 0.45f, 10, -planeWidth * scale * 0.45f); + Vector3 span = new Vector3(planeWidth * scale * 0.9f, 15, planeWidth * scale * 0.9f); - var planeMesh = DemoMeshHelper.CreateDeformedPlane(planeWidth, planeWidth, - (int vX, int vY) => - { - var octave0 = (MathF.Sin((vX + 5f) * 0.05f) + MathF.Sin((vY + 11) * 0.05f)) * 1.8f; - var octave1 = (MathF.Sin((vX + 17) * 0.15f) + MathF.Sin((vY + 19) * 0.15f)) * 0.9f; - var octave2 = (MathF.Sin((vX + 37) * 0.35f) + MathF.Sin((vY + 93) * 0.35f)) * 0.4f; - var octave3 = (MathF.Sin((vX + 53) * 0.65f) + MathF.Sin((vY + 47) * 0.65f)) * 0.2f; - var octave4 = (MathF.Sin((vX + 67) * 1.50f) + MathF.Sin((vY + 13) * 1.5f)) * 0.125f; - var distanceToEdge = planeWidth / 2 - Math.Max(Math.Abs(vX - planeWidth / 2), Math.Abs(vY - planeWidth / 2)); - var edgeRamp = 25f / (distanceToEdge + 1); - var terrainHeight = octave0 + octave1 + octave2 + octave3 + octave4; - var vertexPosition = new Vector2(vX * scale, vY * scale) + terrainPosition; - return new Vector3(vertexPosition.X, terrainHeight + edgeRamp, vertexPosition.Y); - - }, new Vector3(1, 1, 1), BufferPool); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -15, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), Simulation.Shapes.Add(planeMesh))); - - var testBox = new Box(3, 3, 3); - var testBoxInertia = testBox.ComputeInertia(1); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 10, 0), testBoxInertia, new(Simulation.Shapes.Add(testBox), 10, 10, ContinuousDetection.Discrete), -1)); - var testSphere = new Sphere(.1f); - var testSphereInertia = testSphere.ComputeInertia(1); - //testSphereInertia.InverseInertiaTensor = default; - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(10, 10, 0), testSphereInertia, new(Simulation.Shapes.Add(testSphere), 10, 10, ContinuousDetection.Discrete), -1)); - var testCylinder = new Cylinder(1.5f, 2f); - var testCylinderInertia = testCylinder.ComputeInertia(1); - //testCylinderInertia.InverseInertiaTensor = default; - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(15, 10, 0), testCylinderInertia, new(Simulation.Shapes.Add(testCylinder), 10, 10, ContinuousDetection.Discrete), -1)); - var testCapsule = new Capsule(.1f, 2f); - var testCapsuleInertia = testCapsule.ComputeInertia(1); - //testCapsuleInertia.InverseInertiaTensor = default; - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(18, 10, 0), testCapsuleInertia, new(Simulation.Shapes.Add(testCapsule), 10, 10, ContinuousDetection.Discrete), -1)); - - var points = new QuickList(8, BufferPool); - points.AllocateUnsafely() = new Vector3(0, 0, 0); - points.AllocateUnsafely() = new Vector3(0, 0, 2); - points.AllocateUnsafely() = new Vector3(2, 0, 0); - points.AllocateUnsafely() = new Vector3(2, 0, 2); - points.AllocateUnsafely() = new Vector3(0, 2, 0); - points.AllocateUnsafely() = new Vector3(0, 2, 2); - points.AllocateUnsafely() = new Vector3(2, 2, 0); - points.AllocateUnsafely() = new Vector3(2, 2, 2); - var convexHull = new ConvexHull(points, BufferPool, out _); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(23, 10, 0), convexHull.ComputeInertia(1), new(Simulation.Shapes.Add(convexHull), 10, 10, ContinuousDetection.Discrete), -1)); - - //var sphere = new Sphere(1.5f); - //var capsule = new Capsule(1f, 1f); - //var box = new Box(32f, 32f, 32f); - //var cylinder = new Cylinder(1.5f, 0.3f); - //const int pointCount = 32; - //var points = new QuickList(pointCount, BufferPool); - ////points.Allocate(BufferPool) = new Vector3(0, 0, 0); - ////points.Allocate(BufferPool) = new Vector3(0, 0, 1); - ////points.Allocate(BufferPool) = new Vector3(0, 1, 0); - ////points.Allocate(BufferPool) = new Vector3(0, 1, 1); - ////points.Allocate(BufferPool) = new Vector3(1, 0, 0); - ////points.Allocate(BufferPool) = new Vector3(1, 0, 1); - ////points.Allocate(BufferPool) = new Vector3(1, 1, 0); - ////points.Allocate(BufferPool) = new Vector3(1, 1, 1); - //var random = new Random(5); - //for (int i = 0; i < pointCount; ++i) - //{ - // points.AllocateUnsafely() = new Vector3(3 * random.NextSingle(), 1 * random.NextSingle(), 3 * random.NextSingle()); - // //points.AllocateUnsafely() = new Vector3(0, 1, 0) + Vector3.Normalize(new Vector3(random.NextSingle() * 2 - 1, random.NextSingle() * 2 - 1, random.NextSingle() * 2 - 1)) * random.NextSingle(); - //} - //var convexHull = new ConvexHull(points.Span.Slice(points.Count), BufferPool, out _); - //box.ComputeInertia(1, out var boxInertia); - //capsule.ComputeInertia(1, out var capsuleInertia); - //sphere.ComputeInertia(1, out var sphereInertia); - //cylinder.ComputeInertia(1, out var cylinderInertia); - //convexHull.ComputeInertia(1, out var hullInertia); - //var boxIndex = Simulation.Shapes.Add(box); - //var capsuleIndex = Simulation.Shapes.Add(capsule); - //var sphereIndex = Simulation.Shapes.Add(sphere); - //var cylinderIndex = Simulation.Shapes.Add(cylinder); - //var hullIndex = Simulation.Shapes.Add(convexHull); - - const int width = 12; - const int height = 1; - const int length = 12; - var shapeCount = 0; - var random = new Random(5); - for (int i = 0; i < width; ++i) + var planeMesh = DemoMeshHelper.CreateDeformedPlane(planeWidth, planeWidth, + (int vX, int vY) => { - for (int j = 0; j < height; ++j) + var octave0 = (MathF.Sin((vX + 5f) * 0.05f) + MathF.Sin((vY + 11) * 0.05f)) * 1.8f; + var octave1 = (MathF.Sin((vX + 17) * 0.15f) + MathF.Sin((vY + 19) * 0.15f)) * 0.9f; + var octave2 = (MathF.Sin((vX + 37) * 0.35f) + MathF.Sin((vY + 93) * 0.35f)) * 0.4f; + var octave3 = (MathF.Sin((vX + 53) * 0.65f) + MathF.Sin((vY + 47) * 0.65f)) * 0.2f; + var octave4 = (MathF.Sin((vX + 67) * 1.50f) + MathF.Sin((vY + 13) * 1.5f)) * 0.125f; + var distanceToEdge = planeWidth / 2 - Math.Max(Math.Abs(vX - planeWidth / 2), Math.Abs(vY - planeWidth / 2)); + var edgeRamp = 25f / (distanceToEdge + 1); + var terrainHeight = octave0 + octave1 + octave2 + octave3 + octave4; + var vertexPosition = new Vector2(vX * scale, vY * scale) + terrainPosition; + return new Vector3(vertexPosition.X, terrainHeight + edgeRamp, vertexPosition.Y); + + }, new Vector3(1, 1, 1), BufferPool); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -15, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), Simulation.Shapes.Add(planeMesh))); + + var testBox = new Box(3, 3, 3); + var testBoxInertia = testBox.ComputeInertia(1); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 10, 0), testBoxInertia, new(Simulation.Shapes.Add(testBox), 10, 10, ContinuousDetection.Discrete), -1)); + var testSphere = new Sphere(.1f); + var testSphereInertia = testSphere.ComputeInertia(1); + //testSphereInertia.InverseInertiaTensor = default; + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(10, 10, 0), testSphereInertia, new(Simulation.Shapes.Add(testSphere), 10, 10, ContinuousDetection.Discrete), -1)); + var testCylinder = new Cylinder(1.5f, 2f); + var testCylinderInertia = testCylinder.ComputeInertia(1); + //testCylinderInertia.InverseInertiaTensor = default; + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(15, 10, 0), testCylinderInertia, new(Simulation.Shapes.Add(testCylinder), 10, 10, ContinuousDetection.Discrete), -1)); + var testCapsule = new Capsule(.1f, 2f); + var testCapsuleInertia = testCapsule.ComputeInertia(1); + //testCapsuleInertia.InverseInertiaTensor = default; + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(18, 10, 0), testCapsuleInertia, new(Simulation.Shapes.Add(testCapsule), 10, 10, ContinuousDetection.Discrete), -1)); + + var points = new QuickList(8, BufferPool); + points.AllocateUnsafely() = new Vector3(0, 0, 0); + points.AllocateUnsafely() = new Vector3(0, 0, 2); + points.AllocateUnsafely() = new Vector3(2, 0, 0); + points.AllocateUnsafely() = new Vector3(2, 0, 2); + points.AllocateUnsafely() = new Vector3(0, 2, 0); + points.AllocateUnsafely() = new Vector3(0, 2, 2); + points.AllocateUnsafely() = new Vector3(2, 2, 0); + points.AllocateUnsafely() = new Vector3(2, 2, 2); + var convexHull = new ConvexHull(points, BufferPool, out _); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(23, 10, 0), convexHull.ComputeInertia(1), new(Simulation.Shapes.Add(convexHull), 10, 10, ContinuousDetection.Discrete), -1)); + + //var sphere = new Sphere(1.5f); + //var capsule = new Capsule(1f, 1f); + //var box = new Box(32f, 32f, 32f); + //var cylinder = new Cylinder(1.5f, 0.3f); + //const int pointCount = 32; + //var points = new QuickList(pointCount, BufferPool); + ////points.Allocate(BufferPool) = new Vector3(0, 0, 0); + ////points.Allocate(BufferPool) = new Vector3(0, 0, 1); + ////points.Allocate(BufferPool) = new Vector3(0, 1, 0); + ////points.Allocate(BufferPool) = new Vector3(0, 1, 1); + ////points.Allocate(BufferPool) = new Vector3(1, 0, 0); + ////points.Allocate(BufferPool) = new Vector3(1, 0, 1); + ////points.Allocate(BufferPool) = new Vector3(1, 1, 0); + ////points.Allocate(BufferPool) = new Vector3(1, 1, 1); + //var random = new Random(5); + //for (int i = 0; i < pointCount; ++i) + //{ + // points.AllocateUnsafely() = new Vector3(3 * random.NextSingle(), 1 * random.NextSingle(), 3 * random.NextSingle()); + // //points.AllocateUnsafely() = new Vector3(0, 1, 0) + Vector3.Normalize(new Vector3(random.NextSingle() * 2 - 1, random.NextSingle() * 2 - 1, random.NextSingle() * 2 - 1)) * random.NextSingle(); + //} + //var convexHull = new ConvexHull(points.Span.Slice(points.Count), BufferPool, out _); + //box.ComputeInertia(1, out var boxInertia); + //capsule.ComputeInertia(1, out var capsuleInertia); + //sphere.ComputeInertia(1, out var sphereInertia); + //cylinder.ComputeInertia(1, out var cylinderInertia); + //convexHull.ComputeInertia(1, out var hullInertia); + //var boxIndex = Simulation.Shapes.Add(box); + //var capsuleIndex = Simulation.Shapes.Add(capsule); + //var sphereIndex = Simulation.Shapes.Add(sphere); + //var cylinderIndex = Simulation.Shapes.Add(cylinder); + //var hullIndex = Simulation.Shapes.Add(convexHull); + + const int width = 12; + const int height = 1; + const int length = 12; + var shapeCount = 0; + var random = new Random(5); + for (int i = 0; i < width; ++i) + { + for (int j = 0; j < height; ++j) + { + for (int k = 0; k < length; ++k) { - for (int k = 0; k < length; ++k) + var location = new Vector3(70, 35, 70) * new Vector3(i, j, k) + new Vector3(-width * 70 / 2f, 5f, -length * 70 / 2f); + var bodyDescription = BodyDescription.CreateDynamic(location, default, default, 0.01f); + var index = shapeCount++; + switch (index % 5) { - var location = new Vector3(70, 35, 70) * new Vector3(i, j, k) + new Vector3(-width * 70 / 2f, 5f, -length * 70 / 2f); - var bodyDescription = BodyDescription.CreateDynamic(location, default, default, 0.01f); - var index = shapeCount++; - switch (index % 5) - { - //case 0: - // bodyDescription.Collidable.Shape = sphereIndex; - // bodyDescription.LocalInertia = sphereInertia; + //case 0: + // bodyDescription.Collidable.Shape = sphereIndex; + // bodyDescription.LocalInertia = sphereInertia; + // break; + //case 1: + // bodyDescription.Collidable.Shape = capsuleIndex; + // bodyDescription.LocalInertia = capsuleInertia; + // break; + case 2: + default: + var box = new Box(1 + 128 * random.NextSingle(), 1 + 128 * random.NextSingle(), 1 + 128 * random.NextSingle()); + bodyDescription.Collidable.Shape = Simulation.Shapes.Add(box); + bodyDescription.LocalInertia = box.ComputeInertia(1); + break; + //case 3: + // bodyDescription.Collidable.Shape = cylinderIndex; + // bodyDescription.LocalInertia = cylinderInertia; // break; - //case 1: - // bodyDescription.Collidable.Shape = capsuleIndex; - // bodyDescription.LocalInertia = capsuleInertia; + //case 4: + // bodyDescription.Collidable.Shape = hullIndex; + // bodyDescription.LocalInertia = hullInertia; // break; - case 2: - default: - var box = new Box(1 + 128 * random.NextSingle(), 1 + 128 * random.NextSingle(), 1 + 128 * random.NextSingle()); - bodyDescription.Collidable.Shape = Simulation.Shapes.Add(box); - bodyDescription.LocalInertia = box.ComputeInertia(1); - break; - //case 3: - // bodyDescription.Collidable.Shape = cylinderIndex; - // bodyDescription.LocalInertia = cylinderInertia; - // break; - //case 4: - // bodyDescription.Collidable.Shape = hullIndex; - // bodyDescription.LocalInertia = hullInertia; - // break; - } - var bodyHandle = Simulation.Bodies.Add(bodyDescription); } + var bodyHandle = Simulation.Bodies.Add(bodyDescription); } } - - } + } + } \ No newline at end of file diff --git a/Demos/SpecializedTests/MeshSerializationTestDemo.cs b/Demos/SpecializedTests/MeshSerializationTestDemo.cs index cbd54c952..882945681 100644 --- a/Demos/SpecializedTests/MeshSerializationTestDemo.cs +++ b/Demos/SpecializedTests/MeshSerializationTestDemo.cs @@ -8,47 +8,46 @@ using System.Diagnostics; using BepuPhysics.Constraints; -namespace Demos.SpecializedTests +namespace Demos.SpecializedTests; + +public class MeshSerializationTestDemo : Demo { - public class MeshSerializationTestDemo : Demo + public override void Initialize(ContentArchive content, Camera camera) { - public override void Initialize(ContentArchive content, Camera camera) + camera.Position = new Vector3(-30, 8, -60); + camera.Yaw = MathHelper.Pi * 3f / 4; + camera.Pitch = 0; + + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); + + var startTime = Stopwatch.GetTimestamp(); + var originalMesh = DemoMeshHelper.CreateDeformedPlane(1025, 1025, (x, y) => new Vector3(x * 0.125f, MathF.Sin(x) + MathF.Sin(y), y * 0.125f), Vector3.One, BufferPool); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), Simulation.Shapes.Add(originalMesh))); + var endTime = Stopwatch.GetTimestamp(); + var freshConstructionTime = (endTime - startTime) / (double)Stopwatch.Frequency; + Console.WriteLine($"Fresh construction time (ms): {freshConstructionTime * 1e3}"); + + BufferPool.Take(originalMesh.GetSerializedByteCount(), out var serializedMeshBytes); + originalMesh.Serialize(serializedMeshBytes); + startTime = Stopwatch.GetTimestamp(); + var loadedMesh = new Mesh(serializedMeshBytes, BufferPool); + endTime = Stopwatch.GetTimestamp(); + var loadTime = (endTime - startTime) / (double)Stopwatch.Frequency; + Console.WriteLine($"Load time (ms): {(endTime - startTime) * 1e3 / Stopwatch.Frequency}"); + Console.WriteLine($"Relative speedup: {freshConstructionTime / loadTime}"); + Simulation.Statics.Add(new StaticDescription(new Vector3(128, 0, 0), Simulation.Shapes.Add(loadedMesh))); + + + BufferPool.Return(ref serializedMeshBytes); + + var random = new Random(5); + var shapeToDrop = new Box(1, 1, 1); + var descriptionToDrop = BodyDescription.CreateDynamic(new Vector3(), shapeToDrop.ComputeInertia(1), Simulation.Shapes.Add(shapeToDrop), 0.01f); + for (int i = 0; i < 1024; ++i) { - camera.Position = new Vector3(-30, 8, -60); - camera.Yaw = MathHelper.Pi * 3f / 4; - camera.Pitch = 0; - - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); - - var startTime = Stopwatch.GetTimestamp(); - var originalMesh = DemoMeshHelper.CreateDeformedPlane(1025, 1025, (x, y) => new Vector3(x * 0.125f, MathF.Sin(x) + MathF.Sin(y), y * 0.125f), Vector3.One, BufferPool); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), Simulation.Shapes.Add(originalMesh))); - var endTime = Stopwatch.GetTimestamp(); - var freshConstructionTime = (endTime - startTime) / (double)Stopwatch.Frequency; - Console.WriteLine($"Fresh construction time (ms): {freshConstructionTime * 1e3}"); - - BufferPool.Take(originalMesh.GetSerializedByteCount(), out var serializedMeshBytes); - originalMesh.Serialize(serializedMeshBytes); - startTime = Stopwatch.GetTimestamp(); - var loadedMesh = new Mesh(serializedMeshBytes, BufferPool); - endTime = Stopwatch.GetTimestamp(); - var loadTime = (endTime - startTime) / (double)Stopwatch.Frequency; - Console.WriteLine($"Load time (ms): {(endTime - startTime) * 1e3 / Stopwatch.Frequency}"); - Console.WriteLine($"Relative speedup: {freshConstructionTime / loadTime}"); - Simulation.Statics.Add(new StaticDescription(new Vector3(128, 0, 0), Simulation.Shapes.Add(loadedMesh))); - - - BufferPool.Return(ref serializedMeshBytes); - - var random = new Random(5); - var shapeToDrop = new Box(1, 1, 1); - var descriptionToDrop = BodyDescription.CreateDynamic(new Vector3(), shapeToDrop.ComputeInertia(1), Simulation.Shapes.Add(shapeToDrop), 0.01f); - for (int i = 0; i < 1024; ++i) - { - descriptionToDrop.Pose.Position = new Vector3(8 + 240 * random.NextSingle(), 10 + 10 * random.NextSingle(), 8 + 112 * random.NextSingle()); - Simulation.Bodies.Add(descriptionToDrop); - } - + descriptionToDrop.Pose.Position = new Vector3(8 + 240 * random.NextSingle(), 10 + 10 * random.NextSingle(), 8 + 112 * random.NextSingle()); + Simulation.Bodies.Add(descriptionToDrop); } + } } diff --git a/Demos/SpecializedTests/MeshTestDemo.cs b/Demos/SpecializedTests/MeshTestDemo.cs index ba015da31..95a2fc1b7 100644 --- a/Demos/SpecializedTests/MeshTestDemo.cs +++ b/Demos/SpecializedTests/MeshTestDemo.cs @@ -7,88 +7,87 @@ using DemoContentLoader; using BepuPhysics.Constraints; -namespace Demos.SpecializedTests +namespace Demos.SpecializedTests; + +public class MeshTestDemo : Demo { - public class MeshTestDemo : Demo + public override void Initialize(ContentArchive content, Camera camera) { - public override void Initialize(ContentArchive content, Camera camera) + camera.Position = new Vector3(-10, 0, -10); + //camera.Yaw = MathHelper.Pi ; + camera.Yaw = MathHelper.Pi * 3f / 4; + //camera.Pitch = MathHelper.PiOver2 * 0.999f; + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); + + var box = new Box(1f, 3f, 2f); + var capsule = new Capsule(1f, 1f); + var sphere = new Sphere(1f); + var boxInertia = box.ComputeInertia(1); + var capsuleInertia = capsule.ComputeInertia(1); + var sphereInertia = sphere.ComputeInertia(1); + var boxIndex = Simulation.Shapes.Add(box); + var capsuleIndex = Simulation.Shapes.Add(capsule); + var sphereIndex = Simulation.Shapes.Add(sphere); + const int width = 16; + const int height = 3; + const int length = 16; + for (int i = 0; i < width; ++i) { - camera.Position = new Vector3(-10, 0, -10); - //camera.Yaw = MathHelper.Pi ; - camera.Yaw = MathHelper.Pi * 3f / 4; - //camera.Pitch = MathHelper.PiOver2 * 0.999f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); - - var box = new Box(1f, 3f, 2f); - var capsule = new Capsule(1f, 1f); - var sphere = new Sphere(1f); - var boxInertia = box.ComputeInertia(1); - var capsuleInertia = capsule.ComputeInertia(1); - var sphereInertia = sphere.ComputeInertia(1); - var boxIndex = Simulation.Shapes.Add(box); - var capsuleIndex = Simulation.Shapes.Add(capsule); - var sphereIndex = Simulation.Shapes.Add(sphere); - const int width = 16; - const int height = 3; - const int length = 16; - for (int i = 0; i < width; ++i) + for (int j = 0; j < height; ++j) { - for (int j = 0; j < height; ++j) + for (int k = 0; k < length; ++k) { - for (int k = 0; k < length; ++k) + var location = new Vector3(5, 5, 5) * new Vector3(i, j, k);// + new Vector3(-width * 1.5f, 1.5f, -length * 1.5f); + var bodyDescription = BodyDescription.CreateDynamic(location, default, default, 0.01f); + switch ((i + j) % 3) { - var location = new Vector3(5, 5, 5) * new Vector3(i, j, k);// + new Vector3(-width * 1.5f, 1.5f, -length * 1.5f); - var bodyDescription = BodyDescription.CreateDynamic(location, default, default, 0.01f); - switch ((i + j) % 3) - { - case 0: - bodyDescription.Collidable.Shape = sphereIndex; - bodyDescription.LocalInertia = sphereInertia; - break; - case 1: - bodyDescription.Collidable.Shape = capsuleIndex; - bodyDescription.LocalInertia = capsuleInertia; - break; - case 2: - bodyDescription.Collidable.Shape = boxIndex; - bodyDescription.LocalInertia = boxInertia; - break; - } - Simulation.Bodies.Add(bodyDescription); - + case 0: + bodyDescription.Collidable.Shape = sphereIndex; + bodyDescription.LocalInertia = sphereInertia; + break; + case 1: + bodyDescription.Collidable.Shape = capsuleIndex; + bodyDescription.LocalInertia = capsuleInertia; + break; + case 2: + bodyDescription.Collidable.Shape = boxIndex; + bodyDescription.LocalInertia = boxInertia; + break; } + Simulation.Bodies.Add(bodyDescription); + } } + } - //var testShape = new Box(50, 2, 0.2f); - //testShape.ComputeInertia(1, out var testInertia); - //Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(10, 10, 10), testInertia, new CollidableDescription(Simulation.Shapes.Add(testShape), 10.1f), new BodyActivityDescription(-0.01f))); + //var testShape = new Box(50, 2, 0.2f); + //testShape.ComputeInertia(1, out var testInertia); + //Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(10, 10, 10), testInertia, new CollidableDescription(Simulation.Shapes.Add(testShape), 10.1f), new BodyActivityDescription(-0.01f))); - var newtMesh = DemoMeshHelper.LoadModel(content, BufferPool, @"Content\newt.obj", new Vector3(5, 5, 5)); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(30, 20, 30), newtMesh.ComputeClosedInertia(10), Simulation.Shapes.Add(newtMesh), 0.01f)); + var newtMesh = DemoMeshHelper.LoadModel(content, BufferPool, @"Content\newt.obj", new Vector3(5, 5, 5)); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(30, 20, 30), newtMesh.ComputeClosedInertia(10), Simulation.Shapes.Add(newtMesh), 0.01f)); - Simulation.Statics.Add(new StaticDescription(new Vector3(30, 15, 30), Simulation.Shapes.Add(new Box(15, 1, 15)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(30, 15, 30), Simulation.Shapes.Add(new Box(15, 1, 15)))); - var boxMesh = DemoMeshHelper.LoadModel(content, BufferPool, @"Content\box.obj", new Vector3(5, 1, 5)); - Simulation.Statics.Add(new StaticDescription(new Vector3(10, 5, -20), Simulation.Shapes.Add(boxMesh))); + var boxMesh = DemoMeshHelper.LoadModel(content, BufferPool, @"Content\box.obj", new Vector3(5, 1, 5)); + Simulation.Statics.Add(new StaticDescription(new Vector3(10, 5, -20), Simulation.Shapes.Add(boxMesh))); - var fanMesh = DemoMeshHelper.CreateFan(64, 16, new Vector3(1, 1, 1), BufferPool); - Simulation.Statics.Add(new StaticDescription(new Vector3(-10, 0, -20), Simulation.Shapes.Add(fanMesh))); + var fanMesh = DemoMeshHelper.CreateFan(64, 16, new Vector3(1, 1, 1), BufferPool); + Simulation.Statics.Add(new StaticDescription(new Vector3(-10, 0, -20), Simulation.Shapes.Add(fanMesh))); - const int planeWidth = 128; - const int planeHeight = 128; - var planeMesh = DemoMeshHelper.CreateDeformedPlane(planeWidth, planeHeight, - (int x, int y) => - { - return new Vector3(x - planeWidth / 2, 1 * MathF.Cos(x / 2f) * MathF.Sin(y / 2f), y - planeHeight / 2); - }, new Vector3(2, 1, 2), BufferPool); - Simulation.Statics.Add(new StaticDescription(new Vector3(64, -10, 64), BepuUtilities.QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), Simulation.Shapes.Add(planeMesh))); - } + const int planeWidth = 128; + const int planeHeight = 128; + var planeMesh = DemoMeshHelper.CreateDeformedPlane(planeWidth, planeHeight, + (int x, int y) => + { + return new Vector3(x - planeWidth / 2, 1 * MathF.Cos(x / 2f) * MathF.Sin(y / 2f), y - planeHeight / 2); + }, new Vector3(2, 1, 2), BufferPool); + Simulation.Statics.Add(new StaticDescription(new Vector3(64, -10, 64), BepuUtilities.QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), Simulation.Shapes.Add(planeMesh))); + } - } } diff --git a/Demos/SpecializedTests/MinkowskiVisualizer.cs b/Demos/SpecializedTests/MinkowskiVisualizer.cs index b88504fd7..6ae854a61 100644 --- a/Demos/SpecializedTests/MinkowskiVisualizer.cs +++ b/Demos/SpecializedTests/MinkowskiVisualizer.cs @@ -10,116 +10,115 @@ using System.Runtime.CompilerServices; using Helpers = DemoRenderer.Helpers; -namespace Demos.SpecializedTests +namespace Demos.SpecializedTests; + +public static class SimplexVisualizer { - public static class SimplexVisualizer + public static void Draw(Renderer renderer, Buffer simplex, Vector3 position, Vector3 lineColor, Vector3 backgroundColor) { - public static void Draw(Renderer renderer, Buffer simplex, Vector3 position, Vector3 lineColor, Vector3 backgroundColor) + var packedLineColor = Helpers.PackColor(lineColor); + var packedBackgroundColor = Helpers.PackColor(backgroundColor); + if (simplex.Length == 1) { - var packedLineColor = Helpers.PackColor(lineColor); - var packedBackgroundColor = Helpers.PackColor(backgroundColor); - if (simplex.Length == 1) - { - renderer.Lines.Allocate() = new LineInstance(simplex[0], simplex[0], packedLineColor, packedBackgroundColor); - } - else + renderer.Lines.Allocate() = new LineInstance(simplex[0], simplex[0], packedLineColor, packedBackgroundColor); + } + else + { + for (int i = 0; i < simplex.Length; ++i) { - for (int i = 0; i < simplex.Length; ++i) + for (int j = i + 1; j < simplex.Length; ++j) { - for (int j = i + 1; j < simplex.Length; ++j) - { - renderer.Lines.Allocate() = new LineInstance(simplex[i] + position, simplex[j] + position, packedLineColor, packedBackgroundColor); - } + renderer.Lines.Allocate() = new LineInstance(simplex[i] + position, simplex[j] + position, packedLineColor, packedBackgroundColor); } - } + } } +} - public static class MinkowskiShapeVisualizer +public static class MinkowskiShapeVisualizer +{ + [MethodImpl(MethodImplOptions.AggressiveInlining)] + static void FindSupport + (in TShapeWideA a, in TShapeWideB b, in Vector3Wide localOffsetB, in Matrix3x3Wide localOrientationB, ref TSupportFinderA supportFinderA, ref TSupportFinderB supportFinderB, in Vector3Wide direction, + in Vector terminatedLanes, out Vector3Wide support) + where TShapeA : IConvexShape + where TShapeWideA : IShapeWide + where TSupportFinderA : ISupportFinder + where TShapeB : IConvexShape + where TShapeWideB : IShapeWide + where TSupportFinderB : ISupportFinder { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - static void FindSupport - (in TShapeWideA a, in TShapeWideB b, in Vector3Wide localOffsetB, in Matrix3x3Wide localOrientationB, ref TSupportFinderA supportFinderA, ref TSupportFinderB supportFinderB, in Vector3Wide direction, - in Vector terminatedLanes, out Vector3Wide support) - where TShapeA : IConvexShape - where TShapeWideA : IShapeWide - where TSupportFinderA : ISupportFinder - where TShapeB : IConvexShape - where TShapeWideB : IShapeWide - where TSupportFinderB : ISupportFinder - { - //support(N, A) - support(-N, B) - supportFinderA.ComputeLocalSupport(a, direction, terminatedLanes, out var extremeA); - Vector3Wide.Negate(direction, out var negatedDirection); - supportFinderB.ComputeSupport(b, localOrientationB, negatedDirection, terminatedLanes, out var extremeB); - Vector3Wide.Add(extremeB, localOffsetB, out extremeB); + //support(N, A) - support(-N, B) + supportFinderA.ComputeLocalSupport(a, direction, terminatedLanes, out var extremeA); + Vector3Wide.Negate(direction, out var negatedDirection); + supportFinderB.ComputeSupport(b, localOrientationB, negatedDirection, terminatedLanes, out var extremeB); + Vector3Wide.Add(extremeB, localOffsetB, out extremeB); - Vector3Wide.Subtract(extremeA, extremeB, out support); - } + Vector3Wide.Subtract(extremeA, extremeB, out support); + } - public unsafe static Buffer CreateLines( - in TShapeA a, in TShapeB b, in RigidPose poseA, in RigidPose poseB, int sampleCount, - float lineLength, Vector3 lineColor, - float originLength, Vector3 originColor, Vector3 backgroundColor, Vector3 basePosition, BufferPool pool) - where TShapeA : unmanaged, IConvexShape - where TShapeWideA : unmanaged, IShapeWide - where TSupportFinderA : struct, ISupportFinder - where TShapeB : unmanaged, IConvexShape - where TShapeWideB : unmanaged, IShapeWide - where TSupportFinderB : struct, ISupportFinder + public unsafe static Buffer CreateLines( + in TShapeA a, in TShapeB b, in RigidPose poseA, in RigidPose poseB, int sampleCount, + float lineLength, Vector3 lineColor, + float originLength, Vector3 originColor, Vector3 backgroundColor, Vector3 basePosition, BufferPool pool) + where TShapeA : unmanaged, IConvexShape + where TShapeWideA : unmanaged, IShapeWide + where TSupportFinderA : struct, ISupportFinder + where TShapeB : unmanaged, IConvexShape + where TShapeWideB : unmanaged, IShapeWide + where TSupportFinderB : struct, ISupportFinder + { + var aWide = default(TShapeWideA); + var bWide = default(TShapeWideB); + if(aWide.InternalAllocationSize > 0) { - var aWide = default(TShapeWideA); - var bWide = default(TShapeWideB); - if(aWide.InternalAllocationSize > 0) - { - var memory = stackalloc byte[aWide.InternalAllocationSize]; - aWide.Initialize(new Buffer(memory, aWide.InternalAllocationSize)); - } - if (bWide.InternalAllocationSize > 0) - { - var memory = stackalloc byte[bWide.InternalAllocationSize]; - bWide.Initialize(new Buffer(memory, bWide.InternalAllocationSize)); - } - aWide.Broadcast(a); - bWide.Broadcast(b); - var worldOffsetB = poseB.Position - poseA.Position; - var localOrientationB = Matrix3x3.CreateFromQuaternion(QuaternionEx.Concatenate(poseB.Orientation, QuaternionEx.Conjugate(poseA.Orientation))); - var localOffsetB = QuaternionEx.Transform(worldOffsetB, QuaternionEx.Conjugate(poseA.Orientation)); - Vector3Wide.Broadcast(localOffsetB, out var localOffsetBWide); - Matrix3x3Wide.Broadcast(localOrientationB, out var localOrientationBWide); - var supportFinderA = default(TSupportFinderA); - var supportFinderB = default(TSupportFinderB); - var inverseSampleCount = 1f / sampleCount; - pool.Take(sampleCount + 3, out var lines); - var packedLineColor = Helpers.PackColor(lineColor); - var packedBackgroundColor = Helpers.PackColor(backgroundColor); - for (int i = 0; i < sampleCount; ++i) - { - var index = i + 0.5f; - var phi = MathF.Acos(1f - 2f * index * inverseSampleCount); - var theta = (MathF.PI * (1f + 2.2360679775f)) * index; - var sinPhi = MathF.Sin(phi); - var sampleDirection = new Vector3(MathF.Cos(theta) * sinPhi, MathF.Sin(theta) * sinPhi, MathF.Cos(phi)); - Vector3Wide.Broadcast(sampleDirection, out var sampleDirectionWide); - //Could easily use the fact that this is vectorized, but it's marginally easier not to! - FindSupport(aWide, bWide, localOffsetBWide, localOrientationBWide, ref supportFinderA, ref supportFinderB, sampleDirectionWide, Vector.Zero, out var supportWide); - Vector3Wide.ReadSlot(ref supportWide, 0, out var support); - lines[i] = new LineInstance(basePosition + support, basePosition + support - sampleDirection * lineLength, packedLineColor, packedBackgroundColor); - } - var packedOriginColor = Helpers.PackColor(originColor); - lines[sampleCount] = new LineInstance(basePosition - new Vector3(originLength, 0, 0), basePosition + new Vector3(originLength, 0, 0), packedOriginColor, packedBackgroundColor); - lines[sampleCount + 1] = new LineInstance(basePosition - new Vector3(0, originLength, 0), basePosition + new Vector3(0, originLength, 0), packedOriginColor, packedBackgroundColor); - lines[sampleCount + 2] = new LineInstance(basePosition - new Vector3(0, 0, originLength), basePosition + new Vector3(0, 0, originLength), packedOriginColor, packedBackgroundColor); - return lines; + var memory = stackalloc byte[aWide.InternalAllocationSize]; + aWide.Initialize(new Buffer(memory, aWide.InternalAllocationSize)); + } + if (bWide.InternalAllocationSize > 0) + { + var memory = stackalloc byte[bWide.InternalAllocationSize]; + bWide.Initialize(new Buffer(memory, bWide.InternalAllocationSize)); } + aWide.Broadcast(a); + bWide.Broadcast(b); + var worldOffsetB = poseB.Position - poseA.Position; + var localOrientationB = Matrix3x3.CreateFromQuaternion(QuaternionEx.Concatenate(poseB.Orientation, QuaternionEx.Conjugate(poseA.Orientation))); + var localOffsetB = QuaternionEx.Transform(worldOffsetB, QuaternionEx.Conjugate(poseA.Orientation)); + Vector3Wide.Broadcast(localOffsetB, out var localOffsetBWide); + Matrix3x3Wide.Broadcast(localOrientationB, out var localOrientationBWide); + var supportFinderA = default(TSupportFinderA); + var supportFinderB = default(TSupportFinderB); + var inverseSampleCount = 1f / sampleCount; + pool.Take(sampleCount + 3, out var lines); + var packedLineColor = Helpers.PackColor(lineColor); + var packedBackgroundColor = Helpers.PackColor(backgroundColor); + for (int i = 0; i < sampleCount; ++i) + { + var index = i + 0.5f; + var phi = MathF.Acos(1f - 2f * index * inverseSampleCount); + var theta = (MathF.PI * (1f + 2.2360679775f)) * index; + var sinPhi = MathF.Sin(phi); + var sampleDirection = new Vector3(MathF.Cos(theta) * sinPhi, MathF.Sin(theta) * sinPhi, MathF.Cos(phi)); + Vector3Wide.Broadcast(sampleDirection, out var sampleDirectionWide); + //Could easily use the fact that this is vectorized, but it's marginally easier not to! + FindSupport(aWide, bWide, localOffsetBWide, localOrientationBWide, ref supportFinderA, ref supportFinderB, sampleDirectionWide, Vector.Zero, out var supportWide); + Vector3Wide.ReadSlot(ref supportWide, 0, out var support); + lines[i] = new LineInstance(basePosition + support, basePosition + support - sampleDirection * lineLength, packedLineColor, packedBackgroundColor); + } + var packedOriginColor = Helpers.PackColor(originColor); + lines[sampleCount] = new LineInstance(basePosition - new Vector3(originLength, 0, 0), basePosition + new Vector3(originLength, 0, 0), packedOriginColor, packedBackgroundColor); + lines[sampleCount + 1] = new LineInstance(basePosition - new Vector3(0, originLength, 0), basePosition + new Vector3(0, originLength, 0), packedOriginColor, packedBackgroundColor); + lines[sampleCount + 2] = new LineInstance(basePosition - new Vector3(0, 0, originLength), basePosition + new Vector3(0, 0, originLength), packedOriginColor, packedBackgroundColor); + return lines; + } - public static void Draw(Buffer lines, Renderer renderer) + public static void Draw(Buffer lines, Renderer renderer) + { + for (int i = 0; i < lines.Length; ++i) { - for (int i = 0; i < lines.Length; ++i) - { - renderer.Lines.Allocate() = lines[i]; - } + renderer.Lines.Allocate() = lines[i]; } } } diff --git a/Demos/SpecializedTests/NewtonsCradleDemo.cs b/Demos/SpecializedTests/NewtonsCradleDemo.cs index e1d2f61a6..7184c5856 100644 --- a/Demos/SpecializedTests/NewtonsCradleDemo.cs +++ b/Demos/SpecializedTests/NewtonsCradleDemo.cs @@ -7,55 +7,54 @@ using System; using System.Numerics; -namespace Demos.SpecializedTests +namespace Demos.SpecializedTests; + +/// +/// Shows a newton's cradle, primarily for behavioral experimentation (in case an alternative solver is ever implemented). +/// The type of solver currently used does not handle the conservation of momentum over the constraint graph in the expected way. +/// The bounce gets distributed fuzzily. +/// +public class NewtonsCradleDemo : Demo { - /// - /// Shows a newton's cradle, primarily for behavioral experimentation (in case an alternative solver is ever implemented). - /// The type of solver currently used does not handle the conservation of momentum over the constraint graph in the expected way. - /// The bounce gets distributed fuzzily. - /// - public class NewtonsCradleDemo : Demo + public override void Initialize(ContentArchive content, Camera camera) { - public override void Initialize(ContentArchive content, Camera camera) + camera.Yaw = 0; + camera.Pitch = 0; + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(20, 0), float.MaxValue, 0), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0), 0, 0), new SolveDescription(1, 1)); + + const int ballCount = 50; + const float ballRadius = 0.5f; + const float ballSpacing = 0.08f; + const float ballHangHeight = 12f; + const float barSpacing = 3f; + + var barShape = new Box(ballCount * ballRadius * 2 + (ballCount - 1) * ballSpacing, 0.2f, 0.2f); + var barShapeIndex = Simulation.Shapes.Add(barShape); + var bar0 = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(barShape.HalfWidth, ballHangHeight, barSpacing * -0.5f), barShapeIndex, 0f)); + var bar1 = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(barShape.HalfWidth, ballHangHeight, barSpacing * 0.5f), barShapeIndex, 0f)); + + camera.Position = new Vector3(barShape.HalfWidth, ballHangHeight * 0.5f, 2 + Math.Max(ballHangHeight, barShape.HalfWidth)); + + var ballShape = new Sphere(ballRadius); + var ballShapeIndex = Simulation.Shapes.Add(ballShape); + var ballInertia = ballShape.ComputeInertia(1); + var ballConstraintSpringSettings = new SpringSettings(300, 1); + for (int i = 0; i < ballCount; ++i) { - camera.Yaw = 0; - camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(20, 0), float.MaxValue, 0), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0), 0, 0), new SolveDescription(1, 1)); - - const int ballCount = 50; - const float ballRadius = 0.5f; - const float ballSpacing = 0.08f; - const float ballHangHeight = 12f; - const float barSpacing = 3f; - - var barShape = new Box(ballCount * ballRadius * 2 + (ballCount - 1) * ballSpacing, 0.2f, 0.2f); - var barShapeIndex = Simulation.Shapes.Add(barShape); - var bar0 = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(barShape.HalfWidth, ballHangHeight, barSpacing * -0.5f), barShapeIndex, 0f)); - var bar1 = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(barShape.HalfWidth, ballHangHeight, barSpacing * 0.5f), barShapeIndex, 0f)); - - camera.Position = new Vector3(barShape.HalfWidth, ballHangHeight * 0.5f, 2 + Math.Max(ballHangHeight, barShape.HalfWidth)); - - var ballShape = new Sphere(ballRadius); - var ballShapeIndex = Simulation.Shapes.Add(ballShape); - var ballInertia = ballShape.ComputeInertia(1); - var ballConstraintSpringSettings = new SpringSettings(300, 1); - for (int i = 0; i < ballCount; ++i) - { - var ballPosition = new Vector3(ballRadius + i * (ballSpacing + ballRadius * 2), 0, 0); - var ball = Simulation.Bodies.Add(BodyDescription.CreateDynamic(ballPosition, ballInertia, new CollidableDescription(ballShapeIndex, 0), 0.0f)); - Simulation.Solver.Add(ball, bar0, new BallSocket { LocalOffsetA = new Vector3(0, ballHangHeight, -barSpacing * 0.5f), LocalOffsetB = new Vector3(ballPosition.X - barShape.HalfWidth, 0, 0), SpringSettings = ballConstraintSpringSettings }); - Simulation.Solver.Add(ball, bar1, new BallSocket { LocalOffsetA = new Vector3(0, ballHangHeight, barSpacing * 0.5f), LocalOffsetB = new Vector3(ballPosition.X - barShape.HalfWidth, 0, 0), SpringSettings = ballConstraintSpringSettings }); - } - - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -ballHangHeight - ballRadius - 1 - 0.5f, 0), Simulation.Shapes.Add(new Box(2500, 1, 2500)))); + var ballPosition = new Vector3(ballRadius + i * (ballSpacing + ballRadius * 2), 0, 0); + var ball = Simulation.Bodies.Add(BodyDescription.CreateDynamic(ballPosition, ballInertia, new CollidableDescription(ballShapeIndex, 0), 0.0f)); + Simulation.Solver.Add(ball, bar0, new BallSocket { LocalOffsetA = new Vector3(0, ballHangHeight, -barSpacing * 0.5f), LocalOffsetB = new Vector3(ballPosition.X - barShape.HalfWidth, 0, 0), SpringSettings = ballConstraintSpringSettings }); + Simulation.Solver.Add(ball, bar1, new BallSocket { LocalOffsetA = new Vector3(0, ballHangHeight, barSpacing * 0.5f), LocalOffsetB = new Vector3(ballPosition.X - barShape.HalfWidth, 0, 0), SpringSettings = ballConstraintSpringSettings }); } - public override void Update(Window window, Camera camera, Input input, float dt) - { - const int substeps = 100; - for (int i = 0; i < substeps; ++i) - Simulation.Timestep(1f / (60f * substeps)); - } + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -ballHangHeight - ballRadius - 1 - 0.5f, 0), Simulation.Shapes.Add(new Box(2500, 1, 2500)))); + } + public override void Update(Window window, Camera camera, Input input, float dt) + { + const int substeps = 100; + for (int i = 0; i < substeps; ++i) + Simulation.Timestep(1f / (60f * substeps)); } + } diff --git a/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs b/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs index d7f5a0c7e..6005f6690 100644 --- a/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs +++ b/Demos/SpecializedTests/PyramidAwakenerTestDemo.cs @@ -8,69 +8,68 @@ using System; using System.Numerics; -namespace Demos.Demos +namespace Demos.Demos; + +/// +/// Repeatedly checks for bugs related to multithreaded awakening and narrow phase flushing. +/// +public class PyramidAwakenerTestDemo : Demo { - /// - /// Repeatedly checks for bugs related to multithreaded awakening and narrow phase flushing. - /// - public class PyramidAwakenerTestDemo : Demo + public override void Initialize(ContentArchive content, Camera camera) { - public override void Initialize(ContentArchive content, Camera camera) - { - camera.Position = new Vector3(-30, 8, -110); - camera.Yaw = MathHelper.Pi * 3f / 4; + camera.Position = new Vector3(-30, 8, -110); + camera.Yaw = MathHelper.Pi * 3f / 4; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); - var boxShape = new Box(1, 1, 1); - var boxInertia = boxShape.ComputeInertia(1); - var boxIndex = Simulation.Shapes.Add(boxShape); - const int pyramidCount = 10; - for (int pyramidIndex = 0; pyramidIndex < pyramidCount; ++pyramidIndex) + var boxShape = new Box(1, 1, 1); + var boxInertia = boxShape.ComputeInertia(1); + var boxIndex = Simulation.Shapes.Add(boxShape); + const int pyramidCount = 10; + for (int pyramidIndex = 0; pyramidIndex < pyramidCount; ++pyramidIndex) + { + const int rowCount = 20; + for (int rowIndex = 0; rowIndex < rowCount; ++rowIndex) { - const int rowCount = 20; - for (int rowIndex = 0; rowIndex < rowCount; ++rowIndex) + int columnCount = rowCount - rowIndex; + for (int columnIndex = 0; columnIndex < columnCount; ++columnIndex) { - int columnCount = rowCount - rowIndex; - for (int columnIndex = 0; columnIndex < columnCount; ++columnIndex) - { - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3( - (-columnCount * 0.5f + columnIndex) * boxShape.Width, - (rowIndex + 0.5f) * boxShape.Height, - (pyramidIndex - pyramidCount * 0.5f) * (boxShape.Length + 4)), - boxInertia, boxIndex, 0.01f)); - } + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3( + (-columnCount * 0.5f + columnIndex) * boxShape.Width, + (rowIndex + 0.5f) * boxShape.Height, + (pyramidIndex - pyramidCount * 0.5f) * (boxShape.Length + 4)), + boxInertia, boxIndex, 0.01f)); } } + } - var staticShape = new Box(250, 1, 250); - var staticShapeIndex = Simulation.Shapes.Add(staticShape); + var staticShape = new Box(250, 1, 250); + var staticShapeIndex = Simulation.Shapes.Add(staticShape); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), staticShapeIndex)); + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), staticShapeIndex)); - } + } - int frameIndex; - Random random = new Random(5); - public override void Update(Window window, Camera camera, Input input, float dt) + int frameIndex; + Random random = new Random(5); + public override void Update(Window window, Camera camera, Input input, float dt) + { + frameIndex++; + if (frameIndex % 64 == 0) { - frameIndex++; - if (frameIndex % 64 == 0) - { - var bulletShape = new Sphere(0.5f + 5 * random.NextSingle()); - var bodyDescription = BodyDescription.CreateDynamic(new Vector3(0, 8, -130), new Vector3(0, 0, 350), bulletShape.ComputeInertia(bulletShape.Radius * bulletShape.Radius * bulletShape.Radius), Simulation.Shapes.Add(bulletShape), 0.01f); - Simulation.Bodies.Add(bodyDescription); - } - if (frameIndex % 192 == 0) - { - Simulation.Dispose(); - BufferPool.Clear(); - for (int i = 0; i < ThreadDispatcher.ThreadCount; ++i) - ThreadDispatcher.WorkerPools[i].Clear(); - Initialize(null, camera); - } - base.Update(window, camera, input, dt); + var bulletShape = new Sphere(0.5f + 5 * random.NextSingle()); + var bodyDescription = BodyDescription.CreateDynamic(new Vector3(0, 8, -130), new Vector3(0, 0, 350), bulletShape.ComputeInertia(bulletShape.Radius * bulletShape.Radius * bulletShape.Radius), Simulation.Shapes.Add(bulletShape), 0.01f); + Simulation.Bodies.Add(bodyDescription); } - + if (frameIndex % 192 == 0) + { + Simulation.Dispose(); + BufferPool.Clear(); + for (int i = 0; i < ThreadDispatcher.ThreadCount; ++i) + ThreadDispatcher.WorkerPools[i].Clear(); + Initialize(null, camera); + } + base.Update(window, camera, input, dt); } + } diff --git a/Demos/SpecializedTests/RayTesting.cs b/Demos/SpecializedTests/RayTesting.cs index 739248a9c..6e56023f2 100644 --- a/Demos/SpecializedTests/RayTesting.cs +++ b/Demos/SpecializedTests/RayTesting.cs @@ -4,505 +4,504 @@ using System; using System.Numerics; using System.Runtime.CompilerServices; -namespace Demos.SpecializedTests +namespace Demos.SpecializedTests; + +public interface IRayTester where T : IShape +{ + static abstract void GetRandomShape(Random random, out T shape); + static abstract void GetPointInVolume(Random random, float innerMargin, ref T shape, out Vector3 localPointInCapsule); + static abstract void GetSurface(Random random, ref T shape, out Vector3 localPointOnCapsule, out Vector3 localNormal); + static abstract bool PointIsOnSurface(ref T shape, ref Vector3 localPoint); +} +public struct SphereRayTester : IRayTester { - public interface IRayTester where T : IShape + public static void GetRandomShape(Random random, out Sphere shape) { - static abstract void GetRandomShape(Random random, out T shape); - static abstract void GetPointInVolume(Random random, float innerMargin, ref T shape, out Vector3 localPointInCapsule); - static abstract void GetSurface(Random random, ref T shape, out Vector3 localPointOnCapsule, out Vector3 localNormal); - static abstract bool PointIsOnSurface(ref T shape, ref Vector3 localPoint); + const float sizeMin = 0.1f; + const float sizeSpan = 200; + shape = new Sphere(sizeMin + sizeSpan * random.NextSingle()); } - public struct SphereRayTester : IRayTester + public static void GetPointInVolume(Random random, float innerMargin, ref Sphere shape, out Vector3 localPoint) { - public static void GetRandomShape(Random random, out Sphere shape) - { - const float sizeMin = 0.1f; - const float sizeSpan = 200; - shape = new Sphere(sizeMin + sizeSpan * random.NextSingle()); - } - public static void GetPointInVolume(Random random, float innerMargin, ref Sphere shape, out Vector3 localPoint) + float effectiveRadius = Math.Max(0, shape.Radius - innerMargin); + float radiusSquared = effectiveRadius * effectiveRadius; + var min = new Vector3(effectiveRadius, effectiveRadius, effectiveRadius); + var span = min * 2; + min = -min; + do { - float effectiveRadius = Math.Max(0, shape.Radius - innerMargin); - float radiusSquared = effectiveRadius * effectiveRadius; - var min = new Vector3(effectiveRadius, effectiveRadius, effectiveRadius); - var span = min * 2; - min = -min; - do - { - localPoint = min + span * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); + localPoint = min + span * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); - } while (localPoint.LengthSquared() > radiusSquared); - } + } while (localPoint.LengthSquared() > radiusSquared); + } - public static void GetSurface(Random random, ref Sphere sphere, out Vector3 localPoint, out Vector3 localNormal) - { - RayTesting.GetUnitDirection(random, out localNormal); - localPoint = localNormal * sphere.Radius; - } + public static void GetSurface(Random random, ref Sphere sphere, out Vector3 localPoint, out Vector3 localNormal) + { + RayTesting.GetUnitDirection(random, out localNormal); + localPoint = localNormal * sphere.Radius; + } - public static bool PointIsOnSurface(ref Sphere shape, ref Vector3 localPoint) - { - var surfaceDistance = localPoint.Length() - shape.Radius; - if (surfaceDistance < 0) - surfaceDistance = -surfaceDistance; - return surfaceDistance < shape.Radius * 1e-3f; - } + public static bool PointIsOnSurface(ref Sphere shape, ref Vector3 localPoint) + { + var surfaceDistance = localPoint.Length() - shape.Radius; + if (surfaceDistance < 0) + surfaceDistance = -surfaceDistance; + return surfaceDistance < shape.Radius * 1e-3f; } +} - public struct CapsuleRayTester : IRayTester +public struct CapsuleRayTester : IRayTester +{ + public static void GetRandomShape(Random random, out Capsule shape) { - public static void GetRandomShape(Random random, out Capsule shape) - { - const float sizeMin = 0.1f; - const float sizeSpan = 200; - shape = new Capsule(sizeMin + sizeSpan * random.NextSingle(), sizeMin * sizeSpan * random.NextSingle()); - } - public static void GetPointInVolume(Random random, float innerMargin, ref Capsule capsule, out Vector3 localPointInCapsule) + const float sizeMin = 0.1f; + const float sizeSpan = 200; + shape = new Capsule(sizeMin + sizeSpan * random.NextSingle(), sizeMin * sizeSpan * random.NextSingle()); + } + public static void GetPointInVolume(Random random, float innerMargin, ref Capsule capsule, out Vector3 localPointInCapsule) + { + float distanceSquared; + float effectiveRadius = Math.Max(0, capsule.Radius - innerMargin); + float radiusSquared = effectiveRadius * effectiveRadius; + var min = new Vector3(effectiveRadius, effectiveRadius + capsule.HalfLength, effectiveRadius); + var span = min * 2; + min = -min; + do { - float distanceSquared; - float effectiveRadius = Math.Max(0, capsule.Radius - innerMargin); - float radiusSquared = effectiveRadius * effectiveRadius; - var min = new Vector3(effectiveRadius, effectiveRadius + capsule.HalfLength, effectiveRadius); - var span = min * 2; - min = -min; - do - { - localPointInCapsule = min + span * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); - var projectedCandidate = new Vector3(0, Math.Max(-capsule.HalfLength, Math.Min(capsule.HalfLength, localPointInCapsule.Y)), 0); - distanceSquared = Vector3.DistanceSquared(projectedCandidate, localPointInCapsule); + localPointInCapsule = min + span * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); + var projectedCandidate = new Vector3(0, Math.Max(-capsule.HalfLength, Math.Min(capsule.HalfLength, localPointInCapsule.Y)), 0); + distanceSquared = Vector3.DistanceSquared(projectedCandidate, localPointInCapsule); - } while (distanceSquared > radiusSquared); - } + } while (distanceSquared > radiusSquared); + } - public static void GetSurface(Random random, ref Capsule capsule, out Vector3 localPointOnCapsule, out Vector3 localNormal) + public static void GetSurface(Random random, ref Capsule capsule, out Vector3 localPointOnCapsule, out Vector3 localNormal) + { + float distanceSquared; + float radiusSquared = capsule.Radius * capsule.Radius; + var min = new Vector3(capsule.Radius, capsule.Radius + capsule.HalfLength, capsule.Radius); + var span = min * 2; + Vector3 offset, projectedCandidate; + min = -min; + do { - float distanceSquared; - float radiusSquared = capsule.Radius * capsule.Radius; - var min = new Vector3(capsule.Radius, capsule.Radius + capsule.HalfLength, capsule.Radius); - var span = min * 2; - Vector3 offset, projectedCandidate; - min = -min; - do - { - localPointOnCapsule = min + span * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); - projectedCandidate = new Vector3(0, Math.Max(-capsule.HalfLength, Math.Min(capsule.HalfLength, localPointOnCapsule.Y)), 0); - offset = localPointOnCapsule - projectedCandidate; - distanceSquared = offset.LengthSquared(); + localPointOnCapsule = min + span * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); + projectedCandidate = new Vector3(0, Math.Max(-capsule.HalfLength, Math.Min(capsule.HalfLength, localPointOnCapsule.Y)), 0); + offset = localPointOnCapsule - projectedCandidate; + distanceSquared = offset.LengthSquared(); + + } while (distanceSquared < 1e-7f); + localNormal = offset / (float)Math.Sqrt(distanceSquared); + localPointOnCapsule = projectedCandidate + localNormal * capsule.Radius; + } - } while (distanceSquared < 1e-7f); - localNormal = offset / (float)Math.Sqrt(distanceSquared); - localPointOnCapsule = projectedCandidate + localNormal * capsule.Radius; - } + public static bool PointIsOnSurface(ref Capsule capsule, ref Vector3 localPoint) + { + var projected = MathHelper.Clamp(localPoint.Y, -capsule.HalfLength, capsule.HalfLength); + var surfaceDistance = Vector3.Distance(localPoint, new Vector3(0, projected, 0)) - capsule.Radius; + if (surfaceDistance < 0) + surfaceDistance = -surfaceDistance; + return surfaceDistance < capsule.Radius * 1e-3f; + } +} - public static bool PointIsOnSurface(ref Capsule capsule, ref Vector3 localPoint) +public struct CylinderRayTester : IRayTester +{ + public static void GetRandomShape(Random random, out Cylinder shape) + { + const float sizeMin = 0.1f; + const float sizeSpan = 200; + shape = new Cylinder(sizeMin + sizeSpan * random.NextSingle(), sizeMin * sizeSpan * random.NextSingle()); + } + public static void GetPointInVolume(Random random, float innerMargin, ref Cylinder cylinder, out Vector3 localPointInCylinder) + { + float distanceSquared; + float effectiveRadius = Math.Max(0, cylinder.Radius - innerMargin); + float effectiveHalfLength = Math.Max(0, cylinder.HalfLength - innerMargin); + float radiusSquared = effectiveRadius * effectiveRadius; + var min = new Vector2(effectiveRadius); + var span = min * 2; + min = -min; + Vector2 randomHorizontal; + do { - var projected = MathHelper.Clamp(localPoint.Y, -capsule.HalfLength, capsule.HalfLength); - var surfaceDistance = Vector3.Distance(localPoint, new Vector3(0, projected, 0)) - capsule.Radius; - if (surfaceDistance < 0) - surfaceDistance = -surfaceDistance; - return surfaceDistance < capsule.Radius * 1e-3f; - } + randomHorizontal = min + span * new Vector2(random.NextSingle(), random.NextSingle()); + distanceSquared = randomHorizontal.LengthSquared(); + + } while (distanceSquared > radiusSquared); + localPointInCylinder = new Vector3(randomHorizontal.X, -effectiveHalfLength + 2 * effectiveHalfLength * random.NextSingle(), randomHorizontal.Y); } - public struct CylinderRayTester : IRayTester + public static void GetSurface(Random random, ref Cylinder cylinder, out Vector3 localPointOnCylinder, out Vector3 localNormal) { - public static void GetRandomShape(Random random, out Cylinder shape) - { - const float sizeMin = 0.1f; - const float sizeSpan = 200; - shape = new Cylinder(sizeMin + sizeSpan * random.NextSingle(), sizeMin * sizeSpan * random.NextSingle()); - } - public static void GetPointInVolume(Random random, float innerMargin, ref Cylinder cylinder, out Vector3 localPointInCylinder) + float distanceSquared; + var min = new Vector2(cylinder.Radius); + var span = min * 2; + min = -min; + + var sideArea = 4 * MathF.PI * cylinder.Radius * cylinder.HalfLength; + var capArea = MathF.PI * cylinder.Radius * cylinder.Radius; + var totalArea = capArea * 2 + sideArea; + var faceSelection = random.NextDouble(); + if (faceSelection * totalArea < sideArea) { - float distanceSquared; - float effectiveRadius = Math.Max(0, cylinder.Radius - innerMargin); - float effectiveHalfLength = Math.Max(0, cylinder.HalfLength - innerMargin); - float radiusSquared = effectiveRadius * effectiveRadius; - var min = new Vector2(effectiveRadius); - var span = min * 2; - min = -min; + //Side. Vector2 randomHorizontal; do { randomHorizontal = min + span * new Vector2(random.NextSingle(), random.NextSingle()); distanceSquared = randomHorizontal.LengthSquared(); - } while (distanceSquared > radiusSquared); - localPointInCylinder = new Vector3(randomHorizontal.X, -effectiveHalfLength + 2 * effectiveHalfLength * random.NextSingle(), randomHorizontal.Y); + } while (distanceSquared < 1e-7f); + var horizontalNormal = randomHorizontal / (float)Math.Sqrt(distanceSquared); + localNormal = new Vector3(horizontalNormal.X, 0, horizontalNormal.Y); + var horizontalOffset = horizontalNormal * cylinder.Radius; + localPointOnCylinder = new Vector3(horizontalOffset.X, -cylinder.HalfLength + 2 * cylinder.HalfLength * random.NextSingle(), horizontalOffset.Y); } - - public static void GetSurface(Random random, ref Cylinder cylinder, out Vector3 localPointOnCylinder, out Vector3 localNormal) + else { - float distanceSquared; - var min = new Vector2(cylinder.Radius); - var span = min * 2; - min = -min; - - var sideArea = 4 * MathF.PI * cylinder.Radius * cylinder.HalfLength; - var capArea = MathF.PI * cylinder.Radius * cylinder.Radius; - var totalArea = capArea * 2 + sideArea; - var faceSelection = random.NextDouble(); - if (faceSelection * totalArea < sideArea) - { - //Side. - Vector2 randomHorizontal; - do - { - randomHorizontal = min + span * new Vector2(random.NextSingle(), random.NextSingle()); - distanceSquared = randomHorizontal.LengthSquared(); - - } while (distanceSquared < 1e-7f); - var horizontalNormal = randomHorizontal / (float)Math.Sqrt(distanceSquared); - localNormal = new Vector3(horizontalNormal.X, 0, horizontalNormal.Y); - var horizontalOffset = horizontalNormal * cylinder.Radius; - localPointOnCylinder = new Vector3(horizontalOffset.X, -cylinder.HalfLength + 2 * cylinder.HalfLength * random.NextSingle(), horizontalOffset.Y); - } - else + //One of the two caps. + var upperCap = faceSelection * totalArea < totalArea - capArea; + localNormal = new Vector3(0, upperCap ? 1 : -1, 0); + Vector2 randomHorizontal; + do { - //One of the two caps. - var upperCap = faceSelection * totalArea < totalArea - capArea; - localNormal = new Vector3(0, upperCap ? 1 : -1, 0); - Vector2 randomHorizontal; - do - { - randomHorizontal = min + span * new Vector2(random.NextSingle(), random.NextSingle()); - distanceSquared = randomHorizontal.LengthSquared(); + randomHorizontal = min + span * new Vector2(random.NextSingle(), random.NextSingle()); + distanceSquared = randomHorizontal.LengthSquared(); - } while (distanceSquared < cylinder.Radius * cylinder.Radius); - localPointOnCylinder = new Vector3(randomHorizontal.X, upperCap ? cylinder.HalfLength : -cylinder.HalfLength, randomHorizontal.Y); - } - } - - public static bool PointIsOnSurface(ref Cylinder cylinder, ref Vector3 localPoint) - { - var epsilon = MathF.Max(cylinder.HalfLength, cylinder.Radius) * 1e-3f; - if (MathF.Abs(localPoint.Y) > cylinder.HalfLength + epsilon) - { - //Too far up or down. - return false; - } - var horizontalDistanceSquared = localPoint.X * localPoint.X + localPoint.Z * localPoint.Z; - var radiusPlusEpsilon = cylinder.Radius + epsilon; - if (horizontalDistanceSquared > radiusPlusEpsilon * radiusPlusEpsilon) - { - //Too far out. - return false; - } - if (MathF.Abs(localPoint.Y) > cylinder.HalfLength - epsilon) - { - //It's on one of the caps. Already confirmed that the point isn't outside of the radius. - return true; - } - //It's not on a cap. If it's not too deep, then it's on the surface of the side. - var radiusMinusEpsilon = cylinder.Radius - epsilon; - return horizontalDistanceSquared > radiusMinusEpsilon * radiusMinusEpsilon; + } while (distanceSquared < cylinder.Radius * cylinder.Radius); + localPointOnCylinder = new Vector3(randomHorizontal.X, upperCap ? cylinder.HalfLength : -cylinder.HalfLength, randomHorizontal.Y); } } - public struct BoxRayTester : IRayTester + public static bool PointIsOnSurface(ref Cylinder cylinder, ref Vector3 localPoint) { - public static void GetRandomShape(Random random, out Box shape) + var epsilon = MathF.Max(cylinder.HalfLength, cylinder.Radius) * 1e-3f; + if (MathF.Abs(localPoint.Y) > cylinder.HalfLength + epsilon) { - const float sizeMin = 0.1f; - const float sizeSpan = 200; - shape = new Box(sizeMin + sizeSpan * random.NextSingle(), sizeMin * sizeSpan * random.NextSingle(), sizeMin * sizeSpan * random.NextSingle()); + //Too far up or down. + return false; } - public static void GetPointInVolume(Random random, float innerMargin, ref Box box, out Vector3 localPoint) + var horizontalDistanceSquared = localPoint.X * localPoint.X + localPoint.Z * localPoint.Z; + var radiusPlusEpsilon = cylinder.Radius + epsilon; + if (horizontalDistanceSquared > radiusPlusEpsilon * radiusPlusEpsilon) { - var min = new Vector3(box.HalfWidth - innerMargin, box.HalfHeight - innerMargin, box.HalfLength - innerMargin); - var span = min * 2; - min = -min; - localPoint = min + span * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); + //Too far out. + return false; } - - public static void GetSurface(Random random, ref Box box, out Vector3 localPoint, out Vector3 localNormal) + if (MathF.Abs(localPoint.Y) > cylinder.HalfLength - epsilon) { - var a = random.NextSingle(); - var b = random.NextSingle(); - var axisSign = (float)(random.Next(2) * 2 - 1); - Vector3 x, y, z; - switch (random.Next(3)) - { - case 0: - x = new Vector3(box.HalfWidth, 0, 0); - y = new Vector3(0, box.HalfHeight, 0); - localNormal = new Vector3(0, 0, axisSign); - z = localNormal * box.HalfLength; - break; - case 1: - x = new Vector3(0, box.HalfHeight, 0); - y = new Vector3(0, 0, box.HalfLength); - localNormal = new Vector3(axisSign, 0, 0); - z = localNormal * box.HalfWidth; - break; - default: - x = new Vector3(0, 0, box.HalfLength); - y = new Vector3(box.HalfWidth, 0, 0); - localNormal = new Vector3(0, axisSign, 0); - z = localNormal * box.HalfHeight; - break; - } - localPoint = (2 * a - 1) * x + (2 * b - 1) * y + z; + //It's on one of the caps. Already confirmed that the point isn't outside of the radius. + return true; } + //It's not on a cap. If it's not too deep, then it's on the surface of the side. + var radiusMinusEpsilon = cylinder.Radius - epsilon; + return horizontalDistanceSquared > radiusMinusEpsilon * radiusMinusEpsilon; + } +} - public static bool PointIsOnSurface(ref Box box, ref Vector3 localPoint) +public struct BoxRayTester : IRayTester +{ + public static void GetRandomShape(Random random, out Box shape) + { + const float sizeMin = 0.1f; + const float sizeSpan = 200; + shape = new Box(sizeMin + sizeSpan * random.NextSingle(), sizeMin * sizeSpan * random.NextSingle(), sizeMin * sizeSpan * random.NextSingle()); + } + public static void GetPointInVolume(Random random, float innerMargin, ref Box box, out Vector3 localPoint) + { + var min = new Vector3(box.HalfWidth - innerMargin, box.HalfHeight - innerMargin, box.HalfLength - innerMargin); + var span = min * 2; + min = -min; + localPoint = min + span * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); + } + + public static void GetSurface(Random random, ref Box box, out Vector3 localPoint, out Vector3 localNormal) + { + var a = random.NextSingle(); + var b = random.NextSingle(); + var axisSign = (float)(random.Next(2) * 2 - 1); + Vector3 x, y, z; + switch (random.Next(3)) { - //Cast a ray against the box's bounding planes from the local origin using the local point as the direction. - //In effect, all we're doing here is making sure that the closest plane impact has an offset similar to its box extent. - var halfExtents = new Vector3(box.HalfWidth, box.HalfHeight, box.HalfLength); - var t = (halfExtents * halfExtents) / Vector3.Max(new Vector3(1e-15f), Vector3.Abs(localPoint)) - halfExtents; - var minT = t.X < t.Y ? t.X : t.Y; - if (t.Z < minT) - minT = t.Z; - return Math.Abs(minT) < 1e-3f * Math.Max(box.HalfWidth, Math.Max(box.HalfHeight, box.HalfLength)); + case 0: + x = new Vector3(box.HalfWidth, 0, 0); + y = new Vector3(0, box.HalfHeight, 0); + localNormal = new Vector3(0, 0, axisSign); + z = localNormal * box.HalfLength; + break; + case 1: + x = new Vector3(0, box.HalfHeight, 0); + y = new Vector3(0, 0, box.HalfLength); + localNormal = new Vector3(axisSign, 0, 0); + z = localNormal * box.HalfWidth; + break; + default: + x = new Vector3(0, 0, box.HalfLength); + y = new Vector3(box.HalfWidth, 0, 0); + localNormal = new Vector3(0, axisSign, 0); + z = localNormal * box.HalfHeight; + break; } + localPoint = (2 * a - 1) * x + (2 * b - 1) * y + z; + } + + public static bool PointIsOnSurface(ref Box box, ref Vector3 localPoint) + { + //Cast a ray against the box's bounding planes from the local origin using the local point as the direction. + //In effect, all we're doing here is making sure that the closest plane impact has an offset similar to its box extent. + var halfExtents = new Vector3(box.HalfWidth, box.HalfHeight, box.HalfLength); + var t = (halfExtents * halfExtents) / Vector3.Max(new Vector3(1e-15f), Vector3.Abs(localPoint)) - halfExtents; + var minT = t.X < t.Y ? t.X : t.Y; + if (t.Z < minT) + minT = t.Z; + return Math.Abs(minT) < 1e-3f * Math.Max(box.HalfWidth, Math.Max(box.HalfHeight, box.HalfLength)); } +} - public static class RayTesting +public static class RayTesting +{ + internal static void GetUnitDirection(Random random, out Vector3 direction) { - internal static void GetUnitDirection(Random random, out Vector3 direction) + var directionSelector = random.NextSingle(); + //Occasionally choose to use an axis-aligned direction. These are often special cases that could fail. + const float axisAlignedProbability = 0.2f; + if (directionSelector < axisAlignedProbability / 3) + direction = new Vector3(random.NextSingle() < 0.5f ? -1 : 1, 0, 0); + else if (directionSelector < axisAlignedProbability * 2 / 3) + direction = new Vector3(0, random.NextSingle() < 0.5f ? -1 : 1, 0); + else if (directionSelector < axisAlignedProbability) + direction = new Vector3(0, 0, random.NextSingle() < 0.5f ? -1 : 1); + else { - var directionSelector = random.NextSingle(); - //Occasionally choose to use an axis-aligned direction. These are often special cases that could fail. - const float axisAlignedProbability = 0.2f; - if (directionSelector < axisAlignedProbability / 3) - direction = new Vector3(random.NextSingle() < 0.5f ? -1 : 1, 0, 0); - else if (directionSelector < axisAlignedProbability * 2 / 3) - direction = new Vector3(0, random.NextSingle() < 0.5f ? -1 : 1, 0); - else if (directionSelector < axisAlignedProbability) - direction = new Vector3(0, 0, random.NextSingle() < 0.5f ? -1 : 1); - else + //Not much cleverness involved here. This does not produce a uniform distribution over the the unit sphere. + float length; + do { - //Not much cleverness involved here. This does not produce a uniform distribution over the the unit sphere. - float length; - do - { - direction = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) * new Vector3(2) - new Vector3(1); - length = direction.Length(); - } while (length < 1e-7f); - direction /= length; - } + direction = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) * new Vector3(2) - new Vector3(1); + length = direction.Length(); + } while (length < 1e-7f); + direction /= length; } - static void GetUnitQuaternion(Random random, out Quaternion orientation) + } + static void GetUnitQuaternion(Random random, out Quaternion orientation) + { + var identitySelector = random.NextSingle(); + if (identitySelector < 0.5) { - var identitySelector = random.NextSingle(); - if (identitySelector < 0.5) - { - //Combined with choosing ray directions that are often axis-aligned, identity orientation can help reveal special case failures. - orientation = Quaternion.Identity; - } - else - { - float length; - do - { - orientation = new Quaternion( - random.NextSingle() * 2 - 1, - random.NextSingle() * 2 - 1, - random.NextSingle() * 2 - 1, - random.NextSingle() * 2 - 1); - length = orientation.Length(); - } while (length < 1e-7f); - Unsafe.As(ref orientation) /= length; - } + //Combined with choosing ray directions that are often axis-aligned, identity orientation can help reveal special case failures. + orientation = Quaternion.Identity; } - static void GetPointOnPlane(Random random, float centralExclusion, float span, ref Vector3 anchor, ref Vector3 normal, out Vector3 point) + else { - - Vector2 localPoint; - var exclusionSquared = centralExclusion * centralExclusion; - do - { - localPoint = span * (new Vector2(random.NextSingle(), random.NextSingle()) - new Vector2(0.5f)); - } while (localPoint.LengthSquared() < exclusionSquared); - - Vector3 basisX; - float basisXLengthSquared; + float length; do { - GetUnitDirection(random, out var randomDirection); - basisX = Vector3.Cross(normal, randomDirection); - basisXLengthSquared = basisX.LengthSquared(); - } while (basisXLengthSquared < 1e-7f); - var basisZ = Vector3.Cross(normal, basisX); - point = anchor + basisX * localPoint.X + basisZ * localPoint.Y; + orientation = new Quaternion( + random.NextSingle() * 2 - 1, + random.NextSingle() * 2 - 1, + random.NextSingle() * 2 - 1, + random.NextSingle() * 2 - 1); + length = orientation.Length(); + } while (length < 1e-7f); + Unsafe.As(ref orientation) /= length; } + } + static void GetPointOnPlane(Random random, float centralExclusion, float span, ref Vector3 anchor, ref Vector3 normal, out Vector3 point) + { + + Vector2 localPoint; + var exclusionSquared = centralExclusion * centralExclusion; + do + { + localPoint = span * (new Vector2(random.NextSingle(), random.NextSingle()) - new Vector2(0.5f)); + } while (localPoint.LengthSquared() < exclusionSquared); - static void CheckWide(ref RigidPoseWide poses, ref TShapeWide shapeWide, ref Vector3 origin, ref Vector3 direction, bool intersected, float t, ref Vector3 normal) - where TShape : IConvexShape where TShapeWide : IShapeWide + Vector3 basisX; + float basisXLengthSquared; + do { - RayWide rayWide; - Vector3Wide.Broadcast(origin, out rayWide.Origin); - Vector3Wide.Broadcast(direction, out rayWide.Direction); + GetUnitDirection(random, out var randomDirection); + basisX = Vector3.Cross(normal, randomDirection); + basisXLengthSquared = basisX.LengthSquared(); + } while (basisXLengthSquared < 1e-7f); + var basisZ = Vector3.Cross(normal, basisX); + point = anchor + basisX * localPoint.X + basisZ * localPoint.Y; + } + + static void CheckWide(ref RigidPoseWide poses, ref TShapeWide shapeWide, ref Vector3 origin, ref Vector3 direction, bool intersected, float t, ref Vector3 normal) + where TShape : IConvexShape where TShapeWide : IShapeWide + { + RayWide rayWide; + Vector3Wide.Broadcast(origin, out rayWide.Origin); + Vector3Wide.Broadcast(direction, out rayWide.Direction); - shapeWide.RayTest(ref poses, ref rayWide, out var intersectedWide, out var tWide, out var normalWide); - if (intersectedWide[0] < 0 != intersected) + shapeWide.RayTest(ref poses, ref rayWide, out var intersectedWide, out var tWide, out var normalWide); + if (intersectedWide[0] < 0 != intersected) + { + Console.WriteLine($"Wide ray boolean result disagrees with scalar ray."); + } + if (intersected && intersectedWide[0] < 0) + { + if (Math.Abs(tWide[0] - t) > 1e-7f) { - Console.WriteLine($"Wide ray boolean result disagrees with scalar ray."); + Console.WriteLine("Wide ray t disagrees with scalar ray."); } - if (intersected && intersectedWide[0] < 0) + if (Math.Abs(normalWide.X[0] - normal.X) > 1e-6f || + Math.Abs(normalWide.Y[0] - normal.Y) > 1e-6f || + Math.Abs(normalWide.Z[0] - normal.Z) > 1e-6f) { - if (Math.Abs(tWide[0] - t) > 1e-7f) - { - Console.WriteLine("Wide ray t disagrees with scalar ray."); - } - if (Math.Abs(normalWide.X[0] - normal.X) > 1e-6f || - Math.Abs(normalWide.Y[0] - normal.Y) > 1e-6f || - Math.Abs(normalWide.Z[0] - normal.Z) > 1e-6f) - { - Console.WriteLine("Wide ray normal disagrees with scalar ray."); - } + Console.WriteLine("Wide ray normal disagrees with scalar ray."); } } + } - static void Test() where TShape : IConvexShape where TTester : struct, IRayTester where TShapeWide : unmanaged, IShapeWide - { - const int shapeIterations = 1000; - const int transformIterations = 100; - const int outsideToInsideRays = 100; - const int insideRays = 10; - const int outsideRays = 100; - const int outwardPointingRays = 100; + static void Test() where TShape : IConvexShape where TTester : struct, IRayTester where TShapeWide : unmanaged, IShapeWide + { + const int shapeIterations = 1000; + const int transformIterations = 100; + const int outsideToInsideRays = 100; + const int insideRays = 10; + const int outsideRays = 100; + const int outwardPointingRays = 100; - const float volumeInnerMargin = 1e-4f; + const float volumeInnerMargin = 1e-4f; - const float positionBoundsSpan = 100; - const float positionMin = positionBoundsSpan * -0.5f; + const float positionBoundsSpan = 100; + const float positionMin = positionBoundsSpan * -0.5f; - const float outsideMinimumDistance = 0.02f; - const float outsideDistanceSpan = 1000; + const float outsideMinimumDistance = 0.02f; + const float outsideDistanceSpan = 1000; - const float tangentMinimumDistance = 0.02f; - const float tangentDistanceSpan = 10; - const float tangentCentralExclusionMin = 0.01f; - const float tangentCentralExclusionSpan = 10; - const float tangentSourceSpanMin = 0.01f; - const float tangentSourceSpanSpan = 1000f; + const float tangentMinimumDistance = 0.02f; + const float tangentDistanceSpan = 10; + const float tangentCentralExclusionMin = 0.01f; + const float tangentCentralExclusionSpan = 10; + const float tangentSourceSpanMin = 0.01f; + const float tangentSourceSpanSpan = 1000f; - const float outwardPointingSpan = 1000f; + const float outwardPointingSpan = 1000f; - var random = new Random(5); - TShapeWide shapeWide = default; - for (int shapeIteration = 0; shapeIteration < shapeIterations; ++shapeIteration) + var random = new Random(5); + TShapeWide shapeWide = default; + for (int shapeIteration = 0; shapeIteration < shapeIterations; ++shapeIteration) + { + TTester.GetRandomShape(random, out var shape); + shapeWide.Broadcast(shape); + for (int transformIteration = 0; transformIteration < transformIterations; ++transformIteration) { - TTester.GetRandomShape(random, out var shape); - shapeWide.Broadcast(shape); - for (int transformIteration = 0; transformIteration < transformIterations; ++transformIteration) + RigidPose pose; + pose.Position = new Vector3(positionMin) + positionBoundsSpan * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); + GetUnitQuaternion(random, out pose.Orientation); + Matrix3x3.CreateFromQuaternion(pose.Orientation, out var orientation); + RigidPoseWide poses; + Vector3Wide.Broadcast(pose.Position, out poses.Position); + QuaternionWide.Broadcast(pose.Orientation, out poses.Orientation); + for (int rayIndex = 0; rayIndex < outsideToInsideRays; ++rayIndex) { - RigidPose pose; - pose.Position = new Vector3(positionMin) + positionBoundsSpan * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); - GetUnitQuaternion(random, out pose.Orientation); - Matrix3x3.CreateFromQuaternion(pose.Orientation, out var orientation); - RigidPoseWide poses; - Vector3Wide.Broadcast(pose.Position, out poses.Position); - QuaternionWide.Broadcast(pose.Orientation, out poses.Orientation); - for (int rayIndex = 0; rayIndex < outsideToInsideRays; ++rayIndex) + TTester.GetSurface(random, ref shape, out var pointOnSurface, out var normal); + var localSourcePoint = pointOnSurface + normal * (outsideMinimumDistance + random.NextSingle() * outsideDistanceSpan); + TTester.GetPointInVolume(random, volumeInnerMargin, ref shape, out var localTargetPoint); + + Matrix3x3.Transform(localSourcePoint, orientation, out var sourcePoint); + sourcePoint += pose.Position; + var directionScale = (0.01f + 2 * random.NextSingle()); + var localDirection = (localTargetPoint - localSourcePoint) * directionScale; + Matrix3x3.Transform(localDirection, orientation, out var direction); + + bool intersected; + if (intersected = shape.RayTest(pose, sourcePoint, direction, out var t, out var rayTestedNormal)) { - TTester.GetSurface(random, ref shape, out var pointOnSurface, out var normal); - var localSourcePoint = pointOnSurface + normal * (outsideMinimumDistance + random.NextSingle() * outsideDistanceSpan); - TTester.GetPointInVolume(random, volumeInnerMargin, ref shape, out var localTargetPoint); - - Matrix3x3.Transform(localSourcePoint, orientation, out var sourcePoint); - sourcePoint += pose.Position; - var directionScale = (0.01f + 2 * random.NextSingle()); - var localDirection = (localTargetPoint - localSourcePoint) * directionScale; - Matrix3x3.Transform(localDirection, orientation, out var direction); - - bool intersected; - if (intersected = shape.RayTest(pose, sourcePoint, direction, out var t, out var rayTestedNormal)) - { - //If the ray start is outside the shape and the target point is inside, then the ray impact should exist on the surface of the shape. - var hitLocation = sourcePoint + t * direction; - var localHitLocation = hitLocation - pose.Position; - Matrix3x3.TransformTranspose(localHitLocation, orientation, out localHitLocation); - if (!TTester.PointIsOnSurface(ref shape, ref localHitLocation)) - { - Console.WriteLine("Outside->inside ray detected non-surface impact."); - } - } - else + //If the ray start is outside the shape and the target point is inside, then the ray impact should exist on the surface of the shape. + var hitLocation = sourcePoint + t * direction; + var localHitLocation = hitLocation - pose.Position; + Matrix3x3.TransformTranspose(localHitLocation, orientation, out localHitLocation); + if (!TTester.PointIsOnSurface(ref shape, ref localHitLocation)) { - Console.WriteLine($"Outside->inside ray detected no hit."); + Console.WriteLine("Outside->inside ray detected non-surface impact."); } - CheckWide(ref poses, ref shapeWide, ref sourcePoint, ref direction, intersected, t, ref rayTestedNormal); } - for (int rayIndex = 0; rayIndex < insideRays; ++rayIndex) + else { - TTester.GetPointInVolume(random, volumeInnerMargin, ref shape, out var localSourcePoint); - Matrix3x3.Transform(localSourcePoint, orientation, out var sourcePoint); - sourcePoint += pose.Position; + Console.WriteLine($"Outside->inside ray detected no hit."); + } + CheckWide(ref poses, ref shapeWide, ref sourcePoint, ref direction, intersected, t, ref rayTestedNormal); + } + for (int rayIndex = 0; rayIndex < insideRays; ++rayIndex) + { + TTester.GetPointInVolume(random, volumeInnerMargin, ref shape, out var localSourcePoint); + Matrix3x3.Transform(localSourcePoint, orientation, out var sourcePoint); + sourcePoint += pose.Position; - var directionScale = (0.01f + 100 * random.NextSingle()); - GetUnitDirection(random, out var direction); - direction *= directionScale; + var directionScale = (0.01f + 100 * random.NextSingle()); + GetUnitDirection(random, out var direction); + direction *= directionScale; - //If the ray start is inside the shape, then the impact t should be 0. - bool intersected; - if (intersected = shape.RayTest(pose, sourcePoint, direction, out var t, out var rayTestedNormal)) - { - if (t > 0) - { - Console.WriteLine($"Inside ray detected nonzero t value."); - } - } - else + //If the ray start is inside the shape, then the impact t should be 0. + bool intersected; + if (intersected = shape.RayTest(pose, sourcePoint, direction, out var t, out var rayTestedNormal)) + { + if (t > 0) { - Console.WriteLine($"Inside ray detected no impact."); + Console.WriteLine($"Inside ray detected nonzero t value."); } - CheckWide(ref poses, ref shapeWide, ref sourcePoint, ref direction, intersected, t, ref rayTestedNormal); } - for (int rayIndex = 0; rayIndex < outsideRays; ++rayIndex) + else { - //Create a ray that lies on one of the shape's tangent planes, offset from the surface some amount to avoid numerical limitations. - TTester.GetSurface(random, ref shape, out var pointOnSurface, out var localNormal); - var localTargetPoint = pointOnSurface + localNormal * (tangentMinimumDistance + random.NextSingle() * tangentDistanceSpan); - var exclusion = tangentCentralExclusionMin + random.NextSingle() * tangentCentralExclusionSpan; - var span = 2 * exclusion + tangentSourceSpanMin + tangentSourceSpanSpan * random.NextSingle(); - GetPointOnPlane(random, exclusion, span, ref localTargetPoint, ref localNormal, out var localSourcePoint); - var directionScale = (0.01f + 2 * random.NextSingle()); - var localDirection = (localTargetPoint - localSourcePoint) * directionScale; - Matrix3x3.Transform(localSourcePoint, orientation, out var sourcePoint); - sourcePoint += pose.Position; - Matrix3x3.Transform(localDirection, orientation, out var direction); - bool intersected; - if (intersected = shape.RayTest(pose, sourcePoint, direction, out var t, out var rayTestedNormal)) - { - Console.WriteLine($"Outside ray incorrectly detected an impact."); - } - CheckWide(ref poses, ref shapeWide, ref sourcePoint, ref direction, intersected, t, ref rayTestedNormal); + Console.WriteLine($"Inside ray detected no impact."); } - for (int rayIndex = 0; rayIndex < outwardPointingRays; ++rayIndex) + CheckWide(ref poses, ref shapeWide, ref sourcePoint, ref direction, intersected, t, ref rayTestedNormal); + } + for (int rayIndex = 0; rayIndex < outsideRays; ++rayIndex) + { + //Create a ray that lies on one of the shape's tangent planes, offset from the surface some amount to avoid numerical limitations. + TTester.GetSurface(random, ref shape, out var pointOnSurface, out var localNormal); + var localTargetPoint = pointOnSurface + localNormal * (tangentMinimumDistance + random.NextSingle() * tangentDistanceSpan); + var exclusion = tangentCentralExclusionMin + random.NextSingle() * tangentCentralExclusionSpan; + var span = 2 * exclusion + tangentSourceSpanMin + tangentSourceSpanSpan * random.NextSingle(); + GetPointOnPlane(random, exclusion, span, ref localTargetPoint, ref localNormal, out var localSourcePoint); + var directionScale = (0.01f + 2 * random.NextSingle()); + var localDirection = (localTargetPoint - localSourcePoint) * directionScale; + Matrix3x3.Transform(localSourcePoint, orientation, out var sourcePoint); + sourcePoint += pose.Position; + Matrix3x3.Transform(localDirection, orientation, out var direction); + bool intersected; + if (intersected = shape.RayTest(pose, sourcePoint, direction, out var t, out var rayTestedNormal)) { - TTester.GetSurface(random, ref shape, out var pointOnSurface, out var localNormal); - var localSourcePoint = pointOnSurface + localNormal * (tangentMinimumDistance + random.NextSingle() * tangentDistanceSpan); - Vector3 localTargetPoint; - do - { - localTargetPoint = localSourcePoint + new Vector3(-0.5f * outwardPointingSpan) + new Vector3(outwardPointingSpan) * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); - } while (Vector3.Dot(localTargetPoint - localSourcePoint, localNormal) < 0); - var directionScale = (0.01f + 2 * random.NextSingle()); - var localDirection = (localTargetPoint - localSourcePoint) * directionScale; - Matrix3x3.Transform(localSourcePoint, orientation, out var sourcePoint); - sourcePoint += pose.Position; - Matrix3x3.Transform(localDirection, orientation, out var direction); - bool intersected; - if (intersected = shape.RayTest(pose, sourcePoint, direction, out var t, out var rayTestedNormal)) - { - Console.WriteLine($"Outward ray incorrectly detected an impact."); - } - CheckWide(ref poses, ref shapeWide, ref sourcePoint, ref direction, intersected, t, ref rayTestedNormal); + Console.WriteLine($"Outside ray incorrectly detected an impact."); } + CheckWide(ref poses, ref shapeWide, ref sourcePoint, ref direction, intersected, t, ref rayTestedNormal); + } + for (int rayIndex = 0; rayIndex < outwardPointingRays; ++rayIndex) + { + TTester.GetSurface(random, ref shape, out var pointOnSurface, out var localNormal); + var localSourcePoint = pointOnSurface + localNormal * (tangentMinimumDistance + random.NextSingle() * tangentDistanceSpan); + Vector3 localTargetPoint; + do + { + localTargetPoint = localSourcePoint + new Vector3(-0.5f * outwardPointingSpan) + new Vector3(outwardPointingSpan) * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); + } while (Vector3.Dot(localTargetPoint - localSourcePoint, localNormal) < 0); + var directionScale = (0.01f + 2 * random.NextSingle()); + var localDirection = (localTargetPoint - localSourcePoint) * directionScale; + Matrix3x3.Transform(localSourcePoint, orientation, out var sourcePoint); + sourcePoint += pose.Position; + Matrix3x3.Transform(localDirection, orientation, out var direction); + bool intersected; + if (intersected = shape.RayTest(pose, sourcePoint, direction, out var t, out var rayTestedNormal)) + { + Console.WriteLine($"Outward ray incorrectly detected an impact."); + } + CheckWide(ref poses, ref shapeWide, ref sourcePoint, ref direction, intersected, t, ref rayTestedNormal); } } } + } - public static void Test() - { - Test(); - Test(); - Test(); - Test(); - } + public static void Test() + { + Test(); + Test(); + Test(); + Test(); } } diff --git a/Demos/SpecializedTests/ScalarIntegrationTestDemo.cs b/Demos/SpecializedTests/ScalarIntegrationTestDemo.cs index 0ed512a7a..1d724f148 100644 --- a/Demos/SpecializedTests/ScalarIntegrationTestDemo.cs +++ b/Demos/SpecializedTests/ScalarIntegrationTestDemo.cs @@ -8,181 +8,180 @@ using System.Numerics; using BepuUtilities.Collections; -namespace Demos.SpecializedTests +namespace Demos.SpecializedTests; + +public unsafe class ScalarIntegrationTestDemo : Demo { - public unsafe class ScalarIntegrationTestDemo : Demo + struct ScalarIntegrationCallbacks : IPoseIntegratorCallbacks { - struct ScalarIntegrationCallbacks : IPoseIntegratorCallbacks - { - public delegate* IntegrateVelocityFunction; + public delegate* IntegrateVelocityFunction; - public readonly AngularIntegrationMode AngularIntegrationMode => AngularIntegrationMode.Nonconserving; + public readonly AngularIntegrationMode AngularIntegrationMode => AngularIntegrationMode.Nonconserving; - public readonly bool AllowSubstepsForUnconstrainedBodies => false; + public readonly bool AllowSubstepsForUnconstrainedBodies => false; - public readonly bool IntegrateVelocityForKinematics => false; + public readonly bool IntegrateVelocityForKinematics => false; - public void Initialize(Simulation simulation) - { - } + public void Initialize(Simulation simulation) + { + } - public void PrepareForIntegration(float dt) - { - } + public void PrepareForIntegration(float dt) + { + } - public void IntegrateVelocity(Vector bodyIndices, Vector3Wide position, QuaternionWide orientation, BodyInertiaWide localInertia, Vector integrationMask, int workerIndex, Vector dt, ref BodyVelocityWide velocity) + public void IntegrateVelocity(Vector bodyIndices, Vector3Wide position, QuaternionWide orientation, BodyInertiaWide localInertia, Vector integrationMask, int workerIndex, Vector dt, ref BodyVelocityWide velocity) + { + //TODO: This is going to be a very bad implementation for now. Vectorized transposition would speed this up. + for (int i = 0; i < Vector.Count; ++i) { - //TODO: This is going to be a very bad implementation for now. Vectorized transposition would speed this up. - for (int i = 0; i < Vector.Count; ++i) + if (integrationMask[i] != 0) { - if (integrationMask[i] != 0) - { - Vector3Wide.ReadSlot(ref position, i, out var scalarPosition); - QuaternionWide.ReadSlot(ref orientation, i, out var scalarOrientation); - BodyInertia scalarInertia; - scalarInertia.InverseInertiaTensor.XX = localInertia.InverseInertiaTensor.XX[i]; - scalarInertia.InverseInertiaTensor.YX = localInertia.InverseInertiaTensor.YX[i]; - scalarInertia.InverseInertiaTensor.YY = localInertia.InverseInertiaTensor.YY[i]; - scalarInertia.InverseInertiaTensor.ZX = localInertia.InverseInertiaTensor.ZX[i]; - scalarInertia.InverseInertiaTensor.ZY = localInertia.InverseInertiaTensor.ZY[i]; - scalarInertia.InverseInertiaTensor.ZZ = localInertia.InverseInertiaTensor.ZZ[i]; - scalarInertia.InverseMass = localInertia.InverseMass[i]; - BodyVelocity scalarVelocity; - Vector3Wide.ReadSlot(ref velocity.Linear, i, out scalarVelocity.Linear); - Vector3Wide.ReadSlot(ref velocity.Angular, i, out scalarVelocity.Angular); - - IntegrateVelocityFunction(bodyIndices[i], scalarPosition, scalarOrientation, scalarInertia, workerIndex, dt[i], &scalarVelocity); - - Vector3Wide.WriteSlot(scalarVelocity.Linear, i, ref velocity.Linear); - Vector3Wide.WriteSlot(scalarVelocity.Angular, i, ref velocity.Angular); - } + Vector3Wide.ReadSlot(ref position, i, out var scalarPosition); + QuaternionWide.ReadSlot(ref orientation, i, out var scalarOrientation); + BodyInertia scalarInertia; + scalarInertia.InverseInertiaTensor.XX = localInertia.InverseInertiaTensor.XX[i]; + scalarInertia.InverseInertiaTensor.YX = localInertia.InverseInertiaTensor.YX[i]; + scalarInertia.InverseInertiaTensor.YY = localInertia.InverseInertiaTensor.YY[i]; + scalarInertia.InverseInertiaTensor.ZX = localInertia.InverseInertiaTensor.ZX[i]; + scalarInertia.InverseInertiaTensor.ZY = localInertia.InverseInertiaTensor.ZY[i]; + scalarInertia.InverseInertiaTensor.ZZ = localInertia.InverseInertiaTensor.ZZ[i]; + scalarInertia.InverseMass = localInertia.InverseMass[i]; + BodyVelocity scalarVelocity; + Vector3Wide.ReadSlot(ref velocity.Linear, i, out scalarVelocity.Linear); + Vector3Wide.ReadSlot(ref velocity.Angular, i, out scalarVelocity.Angular); + + IntegrateVelocityFunction(bodyIndices[i], scalarPosition, scalarOrientation, scalarInertia, workerIndex, dt[i], &scalarVelocity); + + Vector3Wide.WriteSlot(scalarVelocity.Linear, i, ref velocity.Linear); + Vector3Wide.WriteSlot(scalarVelocity.Angular, i, ref velocity.Angular); } } } + } - static void IntegrateVelocity(int bodyIndex, Vector3 position, Quaternion orientation, BodyInertia inertia, int workerIndex, float dt, BodyVelocity* velocity) - { - velocity->Linear += new Vector3(0, -10 / 60f, 0); - } + static void IntegrateVelocity(int bodyIndex, Vector3 position, Quaternion orientation, BodyInertia inertia, int workerIndex, float dt, BodyVelocity* velocity) + { + velocity->Linear += new Vector3(0, -10 / 60f, 0); + } - public override void Initialize(ContentArchive content, Camera camera) + public override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(-30, 10, -30); + //camera.Yaw = MathHelper.Pi ; + camera.Yaw = MathHelper.Pi * 3f / 4; + //camera.Pitch = MathHelper.PiOver2 * 0.999f; + ScalarIntegrationCallbacks callbacks = new() { IntegrateVelocityFunction = &IntegrateVelocity }; + //DemoPoseIntegratorCallbacks callbacks = new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)); + + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), callbacks, new SolveDescription(1, 4)); + + var sphere = new Sphere(1.5f); + var capsule = new Capsule(1f, 1f); + var box = new Box(1f, 3f, 2f); + var cylinder = new Cylinder(1.5f, 0.3f); + var points = new QuickList(32, BufferPool); + //Boxlike point cloud. + //points.Allocate(BufferPool) = new Vector3(0, 0, 0); + //points.Allocate(BufferPool) = new Vector3(0, 0, 1); + //points.Allocate(BufferPool) = new Vector3(0, 1, 0); + //points.Allocate(BufferPool) = new Vector3(0, 1, 1); + //points.Allocate(BufferPool) = new Vector3(1, 0, 0); + //points.Allocate(BufferPool) = new Vector3(1, 0, 1); + //points.Allocate(BufferPool) = new Vector3(1, 1, 0); + //points.Allocate(BufferPool) = new Vector3(1, 1, 1); + + //Rando pointcloud. + //var random = new Random(5); + //for (int i = 0; i < 32; ++i) + //{ + // points.Allocate(BufferPool) = new Vector3(3 * random.NextSingle(), 1 * random.NextSingle(), 3 * random.NextSingle()); + //} + + //Dodecahedron pointcloud. + points.Allocate(BufferPool) = new Vector3(-1, -1, -1); + points.Allocate(BufferPool) = new Vector3(-1, -1, 1); + points.Allocate(BufferPool) = new Vector3(-1, 1, -1); + points.Allocate(BufferPool) = new Vector3(-1, 1, 1); + points.Allocate(BufferPool) = new Vector3(1, -1, -1); + points.Allocate(BufferPool) = new Vector3(1, -1, 1); + points.Allocate(BufferPool) = new Vector3(1, 1, -1); + points.Allocate(BufferPool) = new Vector3(1, 1, 1); + + const float goldenRatio = 1.618033988749f; + const float oogr = 1f / goldenRatio; + + points.Allocate(BufferPool) = new Vector3(0, goldenRatio, oogr); + points.Allocate(BufferPool) = new Vector3(0, -goldenRatio, oogr); + points.Allocate(BufferPool) = new Vector3(0, goldenRatio, -oogr); + points.Allocate(BufferPool) = new Vector3(0, -goldenRatio, -oogr); + + points.Allocate(BufferPool) = new Vector3(oogr, 0, goldenRatio); + points.Allocate(BufferPool) = new Vector3(oogr, 0, -goldenRatio); + points.Allocate(BufferPool) = new Vector3(-oogr, 0, goldenRatio); + points.Allocate(BufferPool) = new Vector3(-oogr, 0, -goldenRatio); + + points.Allocate(BufferPool) = new Vector3(goldenRatio, oogr, 0); + points.Allocate(BufferPool) = new Vector3(goldenRatio, -oogr, 0); + points.Allocate(BufferPool) = new Vector3(-goldenRatio, oogr, 0); + points.Allocate(BufferPool) = new Vector3(-goldenRatio, -oogr, 0); + + var convexHull = new ConvexHull(points.Span.Slice(points.Count), BufferPool, out _); + var boxInertia = box.ComputeInertia(1); + var capsuleInertia = capsule.ComputeInertia(1); + var sphereInertia = sphere.ComputeInertia(1); + var cylinderInertia = cylinder.ComputeInertia(1); + var hullInertia = convexHull.ComputeInertia(1); + var boxIndex = Simulation.Shapes.Add(box); + var capsuleIndex = Simulation.Shapes.Add(capsule); + var sphereIndex = Simulation.Shapes.Add(sphere); + var cylinderIndex = Simulation.Shapes.Add(cylinder); + var hullIndex = Simulation.Shapes.Add(convexHull); + const int width = 32; + const int height = 32; + const int length = 32; + var shapeCount = 0; + for (int i = 0; i < width; ++i) { - camera.Position = new Vector3(-30, 10, -30); - //camera.Yaw = MathHelper.Pi ; - camera.Yaw = MathHelper.Pi * 3f / 4; - //camera.Pitch = MathHelper.PiOver2 * 0.999f; - ScalarIntegrationCallbacks callbacks = new() { IntegrateVelocityFunction = &IntegrateVelocity }; - //DemoPoseIntegratorCallbacks callbacks = new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)); - - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), callbacks, new SolveDescription(1, 4)); - - var sphere = new Sphere(1.5f); - var capsule = new Capsule(1f, 1f); - var box = new Box(1f, 3f, 2f); - var cylinder = new Cylinder(1.5f, 0.3f); - var points = new QuickList(32, BufferPool); - //Boxlike point cloud. - //points.Allocate(BufferPool) = new Vector3(0, 0, 0); - //points.Allocate(BufferPool) = new Vector3(0, 0, 1); - //points.Allocate(BufferPool) = new Vector3(0, 1, 0); - //points.Allocate(BufferPool) = new Vector3(0, 1, 1); - //points.Allocate(BufferPool) = new Vector3(1, 0, 0); - //points.Allocate(BufferPool) = new Vector3(1, 0, 1); - //points.Allocate(BufferPool) = new Vector3(1, 1, 0); - //points.Allocate(BufferPool) = new Vector3(1, 1, 1); - - //Rando pointcloud. - //var random = new Random(5); - //for (int i = 0; i < 32; ++i) - //{ - // points.Allocate(BufferPool) = new Vector3(3 * random.NextSingle(), 1 * random.NextSingle(), 3 * random.NextSingle()); - //} - - //Dodecahedron pointcloud. - points.Allocate(BufferPool) = new Vector3(-1, -1, -1); - points.Allocate(BufferPool) = new Vector3(-1, -1, 1); - points.Allocate(BufferPool) = new Vector3(-1, 1, -1); - points.Allocate(BufferPool) = new Vector3(-1, 1, 1); - points.Allocate(BufferPool) = new Vector3(1, -1, -1); - points.Allocate(BufferPool) = new Vector3(1, -1, 1); - points.Allocate(BufferPool) = new Vector3(1, 1, -1); - points.Allocate(BufferPool) = new Vector3(1, 1, 1); - - const float goldenRatio = 1.618033988749f; - const float oogr = 1f / goldenRatio; - - points.Allocate(BufferPool) = new Vector3(0, goldenRatio, oogr); - points.Allocate(BufferPool) = new Vector3(0, -goldenRatio, oogr); - points.Allocate(BufferPool) = new Vector3(0, goldenRatio, -oogr); - points.Allocate(BufferPool) = new Vector3(0, -goldenRatio, -oogr); - - points.Allocate(BufferPool) = new Vector3(oogr, 0, goldenRatio); - points.Allocate(BufferPool) = new Vector3(oogr, 0, -goldenRatio); - points.Allocate(BufferPool) = new Vector3(-oogr, 0, goldenRatio); - points.Allocate(BufferPool) = new Vector3(-oogr, 0, -goldenRatio); - - points.Allocate(BufferPool) = new Vector3(goldenRatio, oogr, 0); - points.Allocate(BufferPool) = new Vector3(goldenRatio, -oogr, 0); - points.Allocate(BufferPool) = new Vector3(-goldenRatio, oogr, 0); - points.Allocate(BufferPool) = new Vector3(-goldenRatio, -oogr, 0); - - var convexHull = new ConvexHull(points.Span.Slice(points.Count), BufferPool, out _); - var boxInertia = box.ComputeInertia(1); - var capsuleInertia = capsule.ComputeInertia(1); - var sphereInertia = sphere.ComputeInertia(1); - var cylinderInertia = cylinder.ComputeInertia(1); - var hullInertia = convexHull.ComputeInertia(1); - var boxIndex = Simulation.Shapes.Add(box); - var capsuleIndex = Simulation.Shapes.Add(capsule); - var sphereIndex = Simulation.Shapes.Add(sphere); - var cylinderIndex = Simulation.Shapes.Add(cylinder); - var hullIndex = Simulation.Shapes.Add(convexHull); - const int width = 32; - const int height = 32; - const int length = 32; - var shapeCount = 0; - for (int i = 0; i < width; ++i) + for (int j = 0; j < height; ++j) { - for (int j = 0; j < height; ++j) + for (int k = 0; k < length; ++k) { - for (int k = 0; k < length; ++k) + var location = new Vector3(6, 3, 6) * new Vector3(i, j, k) + new Vector3(-width * 1.5f, 5.5f, -length * 1.5f); + var bodyDescription = BodyDescription.CreateKinematic(location, new(default, ContinuousDetection.Passive), -0.01f); + var index = shapeCount++; + switch (index % 5) { - var location = new Vector3(6, 3, 6) * new Vector3(i, j, k) + new Vector3(-width * 1.5f, 5.5f, -length * 1.5f); - var bodyDescription = BodyDescription.CreateKinematic(location, new(default, ContinuousDetection.Passive), -0.01f); - var index = shapeCount++; - switch (index % 5) - { - case 0: - bodyDescription.Collidable.Shape = sphereIndex; - bodyDescription.LocalInertia = sphereInertia; - break; - case 1: - bodyDescription.Collidable.Shape = capsuleIndex; - bodyDescription.LocalInertia = capsuleInertia; - break; - case 2: - bodyDescription.Collidable.Shape = boxIndex; - bodyDescription.LocalInertia = boxInertia; - break; - case 3: - bodyDescription.Collidable.Shape = cylinderIndex; - bodyDescription.LocalInertia = cylinderInertia; - break; - case 4: - bodyDescription.Collidable.Shape = hullIndex; - bodyDescription.LocalInertia = hullInertia; - break; - } - Simulation.Bodies.Add(bodyDescription); - + case 0: + bodyDescription.Collidable.Shape = sphereIndex; + bodyDescription.LocalInertia = sphereInertia; + break; + case 1: + bodyDescription.Collidable.Shape = capsuleIndex; + bodyDescription.LocalInertia = capsuleInertia; + break; + case 2: + bodyDescription.Collidable.Shape = boxIndex; + bodyDescription.LocalInertia = boxInertia; + break; + case 3: + bodyDescription.Collidable.Shape = cylinderIndex; + bodyDescription.LocalInertia = cylinderInertia; + break; + case 4: + bodyDescription.Collidable.Shape = hullIndex; + bodyDescription.LocalInertia = hullInertia; + break; } + Simulation.Bodies.Add(bodyDescription); + } } + } - //Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(new Box(500, 1, 500)))); - var mesh = DemoMeshHelper.CreateDeformedPlane(128, 128, (x, y) => new Vector3(x - 64, 2f * (float)(Math.Sin(x * 0.5f) * Math.Sin(y * 0.5f)), y - 64), new Vector3(4, 1, 4), BufferPool); - Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); + //Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(new Box(500, 1, 500)))); + var mesh = DemoMeshHelper.CreateDeformedPlane(128, 128, (x, y) => new Vector3(x - 64, 2f * (float)(Math.Sin(x * 0.5f) * Math.Sin(y * 0.5f)), y - 64), new Vector3(4, 1, 4), BufferPool); + Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); - } } } diff --git a/Demos/SpecializedTests/ShapePileTestDemo.cs b/Demos/SpecializedTests/ShapePileTestDemo.cs index 5dc9db3fd..82d10c609 100644 --- a/Demos/SpecializedTests/ShapePileTestDemo.cs +++ b/Demos/SpecializedTests/ShapePileTestDemo.cs @@ -8,129 +8,128 @@ using DemoContentLoader; using BepuPhysics.Constraints; -namespace Demos.SpecializedTests +namespace Demos.SpecializedTests; + +public class ShapePileTestDemo : Demo { - public class ShapePileTestDemo : Demo + public override void Initialize(ContentArchive content, Camera camera) { - public override void Initialize(ContentArchive content, Camera camera) - { - camera.Position = new Vector3(-30, 10, -30); - //camera.Yaw = MathHelper.Pi ; - camera.Yaw = MathHelper.Pi * 3f / 4; - //camera.Pitch = MathHelper.PiOver2 * 0.999f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(4, 1)); - Simulation.Deterministic = true; + camera.Position = new Vector3(-30, 10, -30); + //camera.Yaw = MathHelper.Pi ; + camera.Yaw = MathHelper.Pi * 3f / 4; + //camera.Pitch = MathHelper.PiOver2 * 0.999f; + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(4, 1)); + Simulation.Deterministic = true; - var sphere = new Sphere(1.5f); - var capsule = new Capsule(1f, 1f); - var box = new Box(1f, 3f, 2f); - var cylinder = new Cylinder(1.5f, 0.3f); - var points = new QuickList(32, BufferPool); - //Boxlike point cloud. - //points.Allocate(BufferPool) = new Vector3(0, 0, 0); - //points.Allocate(BufferPool) = new Vector3(0, 0, 1); - //points.Allocate(BufferPool) = new Vector3(0, 1, 0); - //points.Allocate(BufferPool) = new Vector3(0, 1, 1); - //points.Allocate(BufferPool) = new Vector3(1, 0, 0); - //points.Allocate(BufferPool) = new Vector3(1, 0, 1); - //points.Allocate(BufferPool) = new Vector3(1, 1, 0); - //points.Allocate(BufferPool) = new Vector3(1, 1, 1); + var sphere = new Sphere(1.5f); + var capsule = new Capsule(1f, 1f); + var box = new Box(1f, 3f, 2f); + var cylinder = new Cylinder(1.5f, 0.3f); + var points = new QuickList(32, BufferPool); + //Boxlike point cloud. + //points.Allocate(BufferPool) = new Vector3(0, 0, 0); + //points.Allocate(BufferPool) = new Vector3(0, 0, 1); + //points.Allocate(BufferPool) = new Vector3(0, 1, 0); + //points.Allocate(BufferPool) = new Vector3(0, 1, 1); + //points.Allocate(BufferPool) = new Vector3(1, 0, 0); + //points.Allocate(BufferPool) = new Vector3(1, 0, 1); + //points.Allocate(BufferPool) = new Vector3(1, 1, 0); + //points.Allocate(BufferPool) = new Vector3(1, 1, 1); - //Rando pointcloud. - //var random = new Random(5); - //for (int i = 0; i < 32; ++i) - //{ - // points.Allocate(BufferPool) = new Vector3(3 * random.NextSingle(), 1 * random.NextSingle(), 3 * random.NextSingle()); - //} + //Rando pointcloud. + //var random = new Random(5); + //for (int i = 0; i < 32; ++i) + //{ + // points.Allocate(BufferPool) = new Vector3(3 * random.NextSingle(), 1 * random.NextSingle(), 3 * random.NextSingle()); + //} - //Dodecahedron pointcloud. - points.Allocate(BufferPool) = new Vector3(-1, -1, -1); - points.Allocate(BufferPool) = new Vector3(-1, -1, 1); - points.Allocate(BufferPool) = new Vector3(-1, 1, -1); - points.Allocate(BufferPool) = new Vector3(-1, 1, 1); - points.Allocate(BufferPool) = new Vector3(1, -1, -1); - points.Allocate(BufferPool) = new Vector3(1, -1, 1); - points.Allocate(BufferPool) = new Vector3(1, 1, -1); - points.Allocate(BufferPool) = new Vector3(1, 1, 1); + //Dodecahedron pointcloud. + points.Allocate(BufferPool) = new Vector3(-1, -1, -1); + points.Allocate(BufferPool) = new Vector3(-1, -1, 1); + points.Allocate(BufferPool) = new Vector3(-1, 1, -1); + points.Allocate(BufferPool) = new Vector3(-1, 1, 1); + points.Allocate(BufferPool) = new Vector3(1, -1, -1); + points.Allocate(BufferPool) = new Vector3(1, -1, 1); + points.Allocate(BufferPool) = new Vector3(1, 1, -1); + points.Allocate(BufferPool) = new Vector3(1, 1, 1); - const float goldenRatio = 1.618033988749f; - const float oogr = 1f / goldenRatio; + const float goldenRatio = 1.618033988749f; + const float oogr = 1f / goldenRatio; - points.Allocate(BufferPool) = new Vector3(0, goldenRatio, oogr); - points.Allocate(BufferPool) = new Vector3(0, -goldenRatio, oogr); - points.Allocate(BufferPool) = new Vector3(0, goldenRatio, -oogr); - points.Allocate(BufferPool) = new Vector3(0, -goldenRatio, -oogr); + points.Allocate(BufferPool) = new Vector3(0, goldenRatio, oogr); + points.Allocate(BufferPool) = new Vector3(0, -goldenRatio, oogr); + points.Allocate(BufferPool) = new Vector3(0, goldenRatio, -oogr); + points.Allocate(BufferPool) = new Vector3(0, -goldenRatio, -oogr); - points.Allocate(BufferPool) = new Vector3(oogr, 0, goldenRatio); - points.Allocate(BufferPool) = new Vector3(oogr, 0, -goldenRatio); - points.Allocate(BufferPool) = new Vector3(-oogr, 0, goldenRatio); - points.Allocate(BufferPool) = new Vector3(-oogr, 0, -goldenRatio); + points.Allocate(BufferPool) = new Vector3(oogr, 0, goldenRatio); + points.Allocate(BufferPool) = new Vector3(oogr, 0, -goldenRatio); + points.Allocate(BufferPool) = new Vector3(-oogr, 0, goldenRatio); + points.Allocate(BufferPool) = new Vector3(-oogr, 0, -goldenRatio); - points.Allocate(BufferPool) = new Vector3(goldenRatio, oogr, 0); - points.Allocate(BufferPool) = new Vector3(goldenRatio, -oogr, 0); - points.Allocate(BufferPool) = new Vector3(-goldenRatio, oogr, 0); - points.Allocate(BufferPool) = new Vector3(-goldenRatio, -oogr, 0); + points.Allocate(BufferPool) = new Vector3(goldenRatio, oogr, 0); + points.Allocate(BufferPool) = new Vector3(goldenRatio, -oogr, 0); + points.Allocate(BufferPool) = new Vector3(-goldenRatio, oogr, 0); + points.Allocate(BufferPool) = new Vector3(-goldenRatio, -oogr, 0); - var convexHull = new ConvexHull(points.Span.Slice(points.Count), BufferPool, out _); - var boxInertia = box.ComputeInertia(1); - var capsuleInertia = capsule.ComputeInertia(1); - var sphereInertia = sphere.ComputeInertia(1); - var cylinderInertia = cylinder.ComputeInertia(1); - var hullInertia = convexHull.ComputeInertia(1); - var boxIndex = Simulation.Shapes.Add(box); - var capsuleIndex = Simulation.Shapes.Add(capsule); - var sphereIndex = Simulation.Shapes.Add(sphere); - var cylinderIndex = Simulation.Shapes.Add(cylinder); - var hullIndex = Simulation.Shapes.Add(convexHull); - const int width = 16; - const int height = 16; - const int length = 16; - var shapeCount = 0; - for (int i = 0; i < width; ++i) + var convexHull = new ConvexHull(points.Span.Slice(points.Count), BufferPool, out _); + var boxInertia = box.ComputeInertia(1); + var capsuleInertia = capsule.ComputeInertia(1); + var sphereInertia = sphere.ComputeInertia(1); + var cylinderInertia = cylinder.ComputeInertia(1); + var hullInertia = convexHull.ComputeInertia(1); + var boxIndex = Simulation.Shapes.Add(box); + var capsuleIndex = Simulation.Shapes.Add(capsule); + var sphereIndex = Simulation.Shapes.Add(sphere); + var cylinderIndex = Simulation.Shapes.Add(cylinder); + var hullIndex = Simulation.Shapes.Add(convexHull); + const int width = 16; + const int height = 16; + const int length = 16; + var shapeCount = 0; + for (int i = 0; i < width; ++i) + { + for (int j = 0; j < height; ++j) { - for (int j = 0; j < height; ++j) + for (int k = 0; k < length; ++k) { - for (int k = 0; k < length; ++k) + var location = new Vector3(6, 3, 6) * new Vector3(i, j, k) + new Vector3(-width * 3, 5.5f, -length * 3); + var bodyDescription = BodyDescription.CreateKinematic(location, new(default, ContinuousDetection.Passive), 0.01f); + var index = shapeCount++; + switch (index % 5) { - var location = new Vector3(6, 3, 6) * new Vector3(i, j, k) + new Vector3(-width * 3, 5.5f, -length * 3); - var bodyDescription = BodyDescription.CreateKinematic(location, new(default, ContinuousDetection.Passive), 0.01f); - var index = shapeCount++; - switch (index % 5) - { - case 0: - bodyDescription.Collidable.Shape = sphereIndex; - bodyDescription.LocalInertia = sphereInertia; - break; - case 1: - bodyDescription.Collidable.Shape = capsuleIndex; - bodyDescription.LocalInertia = capsuleInertia; - break; - case 2: - bodyDescription.Collidable.Shape = boxIndex; - bodyDescription.LocalInertia = boxInertia; - break; - case 3: - bodyDescription.Collidable.Shape = cylinderIndex; - bodyDescription.LocalInertia = cylinderInertia; - break; - case 4: - default: - bodyDescription.Collidable.Shape = hullIndex; - bodyDescription.LocalInertia = hullInertia; - break; - } - Simulation.Bodies.Add(bodyDescription); - + case 0: + bodyDescription.Collidable.Shape = sphereIndex; + bodyDescription.LocalInertia = sphereInertia; + break; + case 1: + bodyDescription.Collidable.Shape = capsuleIndex; + bodyDescription.LocalInertia = capsuleInertia; + break; + case 2: + bodyDescription.Collidable.Shape = boxIndex; + bodyDescription.LocalInertia = boxInertia; + break; + case 3: + bodyDescription.Collidable.Shape = cylinderIndex; + bodyDescription.LocalInertia = cylinderInertia; + break; + case 4: + default: + bodyDescription.Collidable.Shape = hullIndex; + bodyDescription.LocalInertia = hullInertia; + break; } + Simulation.Bodies.Add(bodyDescription); + } } - - //Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(new Box(500, 1, 500)))); - var mesh = DemoMeshHelper.CreateDeformedPlane(128, 128, (x, y) => new Vector3(x - 64, 2f * (float)(Math.Sin(x * 0.5f) * Math.Sin(y * 0.5f)), y - 64), new Vector3(4, 1, 4), BufferPool); - Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); } + //Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(new Box(500, 1, 500)))); + var mesh = DemoMeshHelper.CreateDeformedPlane(128, 128, (x, y) => new Vector3(x - 64, 2f * (float)(Math.Sin(x * 0.5f) * Math.Sin(y * 0.5f)), y - 64), new Vector3(4, 1, 4), BufferPool); + Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); } + } diff --git a/Demos/SpecializedTests/SimulationScrambling.cs b/Demos/SpecializedTests/SimulationScrambling.cs index fc060c5a2..c67efa3f7 100644 --- a/Demos/SpecializedTests/SimulationScrambling.cs +++ b/Demos/SpecializedTests/SimulationScrambling.cs @@ -6,371 +6,370 @@ using System.Diagnostics; using System.Runtime.CompilerServices; -namespace Demos.SpecializedTests +namespace Demos.SpecializedTests; + +public static class SimulationScrambling { - public static class SimulationScrambling + public static void ScrambleConstraints(Solver solver) { - public static void ScrambleConstraints(Solver solver) + Random random = new Random(5); + ref var activeSet = ref solver.ActiveSet; + for (int i = 0; i < activeSet.Batches.Count; ++i) { - Random random = new Random(5); - ref var activeSet = ref solver.ActiveSet; - for (int i = 0; i < activeSet.Batches.Count; ++i) + for (int j = 0; j < activeSet.Batches[i].TypeBatches.Count; ++j) { - for (int j = 0; j < activeSet.Batches[i].TypeBatches.Count; ++j) - { - ref var typeBatch = ref activeSet.Batches[i].TypeBatches[j]; - solver.TypeProcessors[typeBatch.TypeId].Scramble(ref typeBatch, random, ref solver.HandleToConstraint); - } + ref var typeBatch = ref activeSet.Batches[i].TypeBatches[j]; + solver.TypeProcessors[typeBatch.TypeId].Scramble(ref typeBatch, random, ref solver.HandleToConstraint); } } - public static void ScrambleBodyConstraintLists(Simulation simulation) + } + public static void ScrambleBodyConstraintLists(Simulation simulation) + { + Random random = new Random(5); + //Body lists are isolated enough that we don't have to worry about a bunch of internal bookkeeping. Just pull the list and mess with it. + //Note that we cannot change the order of bodies within constraints! That would change behavior. + for (int bodyIndex = 0; bodyIndex < simulation.Bodies.ActiveSet.Count; ++bodyIndex) { - Random random = new Random(5); - //Body lists are isolated enough that we don't have to worry about a bunch of internal bookkeeping. Just pull the list and mess with it. - //Note that we cannot change the order of bodies within constraints! That would change behavior. - for (int bodyIndex = 0; bodyIndex < simulation.Bodies.ActiveSet.Count; ++bodyIndex) + ref var list = ref simulation.Bodies.ActiveSet.Constraints[bodyIndex]; + for (int i = 0; i < list.Count - 1; ++i) { - ref var list = ref simulation.Bodies.ActiveSet.Constraints[bodyIndex]; - for (int i = 0; i < list.Count - 1; ++i) - { - ref var currentSlot = ref list[i]; - ref var otherSlot = ref list[random.Next(i + 1, list.Count)]; - var currentTemp = currentSlot; - currentSlot = otherSlot; - otherSlot = currentTemp; - } + ref var currentSlot = ref list[i]; + ref var otherSlot = ref list[random.Next(i + 1, list.Count)]; + var currentTemp = currentSlot; + currentSlot = otherSlot; + otherSlot = currentTemp; } } + } - struct CachedConstraint where T : unmanaged, IConstraintDescription - { - public T Description; - public int BodyA; - public int BodyB; - } + struct CachedConstraint where T : unmanaged, IConstraintDescription + { + public T Description; + public int BodyA; + public int BodyB; + } - struct BodyEnumerator : IForEach - { - public Bodies Bodies; - public int[] HandlesToIdentity; - public int IdentityA; - public int IdentityB; - public int IndexInConstraint; + struct BodyEnumerator : IForEach + { + public Bodies Bodies; + public int[] HandlesToIdentity; + public int IdentityA; + public int IdentityB; + public int IndexInConstraint; - public BodyEnumerator(Bodies bodies, int[] handleToEntryIndex) - { - Bodies = bodies; - HandlesToIdentity = handleToEntryIndex; - IdentityA = IdentityB = IndexInConstraint = 0; + public BodyEnumerator(Bodies bodies, int[] handleToEntryIndex) + { + Bodies = bodies; + HandlesToIdentity = handleToEntryIndex; + IdentityA = IdentityB = IndexInConstraint = 0; - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void LoopBody(int encodedBodyIndex) - { - var bodyIndex = encodedBodyIndex & Bodies.BodyReferenceMask; - var entryIndex = HandlesToIdentity[Bodies.ActiveSet.IndexToHandle[bodyIndex].Value]; - if (IndexInConstraint == 0) - IdentityA = entryIndex; - else - IdentityB = entryIndex; - ++IndexInConstraint; - } } - - - static void RemoveConstraint(Simulation simulation, ConstraintHandle constraintHandle, int[] constraintHandlesToIdentity, ConstraintHandle[] constraintHandles, List removedConstraints) + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void LoopBody(int encodedBodyIndex) { - var constraintIdentity = constraintHandlesToIdentity[constraintHandle.Value]; - constraintHandlesToIdentity[constraintHandle.Value] = -1; - constraintHandles[constraintIdentity] = new ConstraintHandle(-1); - simulation.Solver.Remove(constraintHandle); - removedConstraints.Add(constraintIdentity); + var bodyIndex = encodedBodyIndex & Bodies.BodyReferenceMask; + var entryIndex = HandlesToIdentity[Bodies.ActiveSet.IndexToHandle[bodyIndex].Value]; + if (IndexInConstraint == 0) + IdentityA = entryIndex; + else + IdentityB = entryIndex; + ++IndexInConstraint; } + } - struct ConstraintBodyValidationEnumerator : IForEach - { - public Simulation Simulation; - public ConstraintHandle ConstraintHandle; - public void LoopBody(int encodedBodyIndex) - { - //The body in this constraint should both: - //1) have a handle associated with it, and - //2) the constraint graph list for the body should include the constraint handle. - var bodyIndex = encodedBodyIndex & Bodies.BodyReferenceMask; - Debug.Assert(Simulation.Bodies.ActiveSet.IndexToHandle[bodyIndex].Value >= 0); - Debug.Assert(Simulation.Bodies.ActiveSet.BodyIsConstrainedBy(bodyIndex, ConstraintHandle)); - } - } - [Conditional("DEBUG")] - [MethodImpl(MethodImplOptions.AggressiveInlining)] - static void WriteLine(string message) + static void RemoveConstraint(Simulation simulation, ConstraintHandle constraintHandle, int[] constraintHandlesToIdentity, ConstraintHandle[] constraintHandles, List removedConstraints) + { + var constraintIdentity = constraintHandlesToIdentity[constraintHandle.Value]; + constraintHandlesToIdentity[constraintHandle.Value] = -1; + constraintHandles[constraintIdentity] = new ConstraintHandle(-1); + simulation.Solver.Remove(constraintHandle); + removedConstraints.Add(constraintIdentity); + } + + struct ConstraintBodyValidationEnumerator : IForEach + { + public Simulation Simulation; + public ConstraintHandle ConstraintHandle; + public void LoopBody(int encodedBodyIndex) { - //Debug.WriteLine(message); + //The body in this constraint should both: + //1) have a handle associated with it, and + //2) the constraint graph list for the body should include the constraint handle. + var bodyIndex = encodedBodyIndex & Bodies.BodyReferenceMask; + Debug.Assert(Simulation.Bodies.ActiveSet.IndexToHandle[bodyIndex].Value >= 0); + Debug.Assert(Simulation.Bodies.ActiveSet.BodyIsConstrainedBy(bodyIndex, ConstraintHandle)); } + } + + [Conditional("DEBUG")] + [MethodImpl(MethodImplOptions.AggressiveInlining)] + static void WriteLine(string message) + { + //Debug.WriteLine(message); + } - [Conditional("DEBUG")] - static void Validate(Simulation simulation, List removedConstraints, List removedBodies, int originalBodyCount, int originalConstraintCount) + [Conditional("DEBUG")] + static void Validate(Simulation simulation, List removedConstraints, List removedBodies, int originalBodyCount, int originalConstraintCount) + { + ref var activeSet = ref simulation.Solver.ActiveSet; + for (int batchIndex = 0; batchIndex < activeSet.Batches.Count; ++batchIndex) { - ref var activeSet = ref simulation.Solver.ActiveSet; - for (int batchIndex = 0; batchIndex < activeSet.Batches.Count; ++batchIndex) + ref var batch = ref activeSet.Batches[batchIndex]; + if (batchIndex == activeSet.Batches.Count - 1) { - ref var batch = ref activeSet.Batches[batchIndex]; - if (batchIndex == activeSet.Batches.Count - 1) - { - Debug.Assert(batch.TypeBatches.Count > 0, "While a lower indexed batch may have zero elements (especially while batch compression isn't active), " + - "there should never be an empty batch at the end of the list."); - } - for (int typeBatchIndex = 0; typeBatchIndex < batch.TypeBatches.Count; ++typeBatchIndex) - { - ref var typeBatch = ref batch.TypeBatches[typeBatchIndex]; - var typeProcessor = simulation.Solver.TypeProcessors[typeBatch.TypeId]; - Debug.Assert(typeBatch.ConstraintCount > 0, "If a type batch exists, there should be constraints in it."); - for (int indexInTypeBatch = 0; indexInTypeBatch < typeBatch.ConstraintCount; ++indexInTypeBatch) - { - var constraintHandle = typeBatch.IndexToHandle[indexInTypeBatch]; - var constraintLocation = simulation.Solver.HandleToConstraint[constraintHandle.Value]; - Debug.Assert( - constraintLocation.IndexInTypeBatch == indexInTypeBatch && - batch.TypeIndexToTypeBatchIndex[constraintLocation.TypeId] == typeBatchIndex && - constraintLocation.BatchIndex == batchIndex, "The constraint location stored by the solver should agree with the actual type batch entries."); - ConstraintBodyValidationEnumerator enumerator; - enumerator.ConstraintHandle = constraintHandle; - enumerator.Simulation = simulation; - simulation.Solver.EnumerateConnectedRawBodyReferences(ref typeBatch, indexInTypeBatch, ref enumerator); - } - } + Debug.Assert(batch.TypeBatches.Count > 0, "While a lower indexed batch may have zero elements (especially while batch compression isn't active), " + + "there should never be an empty batch at the end of the list."); } - var constraintCount = 0; - foreach (var batch in activeSet.Batches) + for (int typeBatchIndex = 0; typeBatchIndex < batch.TypeBatches.Count; ++typeBatchIndex) { - foreach (var typeBatch in batch.TypeBatches) + ref var typeBatch = ref batch.TypeBatches[typeBatchIndex]; + var typeProcessor = simulation.Solver.TypeProcessors[typeBatch.TypeId]; + Debug.Assert(typeBatch.ConstraintCount > 0, "If a type batch exists, there should be constraints in it."); + for (int indexInTypeBatch = 0; indexInTypeBatch < typeBatch.ConstraintCount; ++indexInTypeBatch) { - constraintCount += typeBatch.ConstraintCount; + var constraintHandle = typeBatch.IndexToHandle[indexInTypeBatch]; + var constraintLocation = simulation.Solver.HandleToConstraint[constraintHandle.Value]; + Debug.Assert( + constraintLocation.IndexInTypeBatch == indexInTypeBatch && + batch.TypeIndexToTypeBatchIndex[constraintLocation.TypeId] == typeBatchIndex && + constraintLocation.BatchIndex == batchIndex, "The constraint location stored by the solver should agree with the actual type batch entries."); + ConstraintBodyValidationEnumerator enumerator; + enumerator.ConstraintHandle = constraintHandle; + enumerator.Simulation = simulation; + simulation.Solver.EnumerateConnectedRawBodyReferences(ref typeBatch, indexInTypeBatch, ref enumerator); } } - - Debug.Assert(removedConstraints.Count + constraintCount == originalConstraintCount, "Must not have lost (or gained) any constraints!"); - Debug.Assert(removedBodies.Count + simulation.Bodies.ActiveSet.Count == originalBodyCount, "Must not have lost (or gained) any bodies!"); - } - static void FastRemoveAt(List list, int index) + var constraintCount = 0; + foreach (var batch in activeSet.Batches) { - var lastIndex = list.Count - 1; - if (lastIndex != index) + foreach (var typeBatch in batch.TypeBatches) { - list[index] = list[lastIndex]; + constraintCount += typeBatch.ConstraintCount; } - list.RemoveAt(lastIndex); } - [MethodImpl(MethodImplOptions.NoInlining)] - private static void ChurnAddBody(Simulation simulation, BodyDescription[] bodyDescriptions, BodyHandle[] bodyHandles, int[] bodyHandlesToIdentity, - int originalConstraintCount, List removedConstraints, List removedBodies, Random random) + Debug.Assert(removedConstraints.Count + constraintCount == originalConstraintCount, "Must not have lost (or gained) any constraints!"); + Debug.Assert(removedBodies.Count + simulation.Bodies.ActiveSet.Count == originalBodyCount, "Must not have lost (or gained) any bodies!"); + + } + static void FastRemoveAt(List list, int index) + { + var lastIndex = list.Count - 1; + if (lastIndex != index) { - //Add a body. - var toAddIndex = random.Next(removedBodies.Count); - var toAdd = removedBodies[toAddIndex]; - FastRemoveAt(removedBodies, toAddIndex); - var bodyHandle = simulation.Bodies.Add(bodyDescriptions[toAdd]); - bodyHandlesToIdentity[bodyHandle.Value] = toAdd; - bodyHandles[toAdd] = bodyHandle; - WriteLine($"Added body, handle: {bodyHandle}"); - Validate(simulation, removedConstraints, removedBodies, bodyHandles.Length, originalConstraintCount); + list[index] = list[lastIndex]; } + list.RemoveAt(lastIndex); + } - [MethodImpl(MethodImplOptions.NoInlining)] - private static void ChurnRemoveBody(Simulation simulation, BodyHandle[] bodyHandles, int[] bodyHandlesToIdentity, ConstraintHandle[] constraintHandles, - int[] constraintHandlesToIdentity, CachedConstraint[] constraintDescriptions, - List removedConstraints, List removedBodies, Random random) where T : unmanaged, IConstraintDescription + [MethodImpl(MethodImplOptions.NoInlining)] + private static void ChurnAddBody(Simulation simulation, BodyDescription[] bodyDescriptions, BodyHandle[] bodyHandles, int[] bodyHandlesToIdentity, + int originalConstraintCount, List removedConstraints, List removedBodies, Random random) + { + //Add a body. + var toAddIndex = random.Next(removedBodies.Count); + var toAdd = removedBodies[toAddIndex]; + FastRemoveAt(removedBodies, toAddIndex); + var bodyHandle = simulation.Bodies.Add(bodyDescriptions[toAdd]); + bodyHandlesToIdentity[bodyHandle.Value] = toAdd; + bodyHandles[toAdd] = bodyHandle; + WriteLine($"Added body, handle: {bodyHandle}"); + Validate(simulation, removedConstraints, removedBodies, bodyHandles.Length, originalConstraintCount); + } + + [MethodImpl(MethodImplOptions.NoInlining)] + private static void ChurnRemoveBody(Simulation simulation, BodyHandle[] bodyHandles, int[] bodyHandlesToIdentity, ConstraintHandle[] constraintHandles, + int[] constraintHandlesToIdentity, CachedConstraint[] constraintDescriptions, + List removedConstraints, List removedBodies, Random random) where T : unmanaged, IConstraintDescription + { + //Remove a body. + var removedBodyIndex = random.Next(simulation.Bodies.ActiveSet.Count); + //All constraints associated with the body have to be removed first. + ref var constraintList = ref simulation.Bodies.ActiveSet.Constraints[removedBodyIndex]; + for (int i = constraintList.Count - 1; i >= 0; --i) { - //Remove a body. - var removedBodyIndex = random.Next(simulation.Bodies.ActiveSet.Count); - //All constraints associated with the body have to be removed first. - ref var constraintList = ref simulation.Bodies.ActiveSet.Constraints[removedBodyIndex]; - for (int i = constraintList.Count - 1; i >= 0; --i) - { - WriteLine($"Removing constraint (handle: {constraintList[i].ConnectingConstraintHandle}) for a body removal."); - RemoveConstraint(simulation, constraintList[i].ConnectingConstraintHandle, constraintHandlesToIdentity, constraintHandles, removedConstraints); - } + WriteLine($"Removing constraint (handle: {constraintList[i].ConnectingConstraintHandle}) for a body removal."); + RemoveConstraint(simulation, constraintList[i].ConnectingConstraintHandle, constraintHandlesToIdentity, constraintHandles, removedConstraints); + } #if DEBUG - Debug.Assert(constraintList.Count == 0, "After we removed all the constraints, the constraint list should be empty! (It's a ref to the actual slot!)"); + Debug.Assert(constraintList.Count == 0, "After we removed all the constraints, the constraint list should be empty! (It's a ref to the actual slot!)"); #endif - var handle = simulation.Bodies.ActiveSet.IndexToHandle[removedBodyIndex]; - simulation.Bodies.Remove(handle); - bodyHandles[bodyHandlesToIdentity[handle.Value]] = new BodyHandle(-1); - removedBodies.Add(bodyHandlesToIdentity[handle.Value]); - bodyHandlesToIdentity[handle.Value] = -1; - WriteLine($"Removed body, former handle: {handle}"); - Validate(simulation, removedConstraints, removedBodies, bodyHandles.Length, constraintHandles.Length); - } + var handle = simulation.Bodies.ActiveSet.IndexToHandle[removedBodyIndex]; + simulation.Bodies.Remove(handle); + bodyHandles[bodyHandlesToIdentity[handle.Value]] = new BodyHandle(-1); + removedBodies.Add(bodyHandlesToIdentity[handle.Value]); + bodyHandlesToIdentity[handle.Value] = -1; + WriteLine($"Removed body, former handle: {handle}"); + Validate(simulation, removedConstraints, removedBodies, bodyHandles.Length, constraintHandles.Length); + } - [MethodImpl(MethodImplOptions.NoInlining)] - private static void ChurnAddConstraint(Simulation simulation, BodyHandle[] bodyHandles, ConstraintHandle[] constraintHandles, int[] constraintHandlesToIdentity, - CachedConstraint[] constraintDescriptions, List removedConstraints, List removedBodies, Random random) where T : unmanaged, ITwoBodyConstraintDescription + [MethodImpl(MethodImplOptions.NoInlining)] + private static void ChurnAddConstraint(Simulation simulation, BodyHandle[] bodyHandles, ConstraintHandle[] constraintHandles, int[] constraintHandlesToIdentity, + CachedConstraint[] constraintDescriptions, List removedConstraints, List removedBodies, Random random) where T : unmanaged, ITwoBodyConstraintDescription + { + //Add a constraint. + int attemptCount = 0; + do { - //Add a constraint. - int attemptCount = 0; - do + //There's no guarantee that the bodies involved with the removed constraint are actually in the simulation. + //Rather than doing anything clever, just retry a few times. + var constraintIdentityIndex = random.Next(removedConstraints.Count); + var constraintIdentity = removedConstraints[constraintIdentityIndex]; + ref var constraint = ref constraintDescriptions[constraintIdentity]; + var handleA = bodyHandles[constraint.BodyA]; + var handleB = bodyHandles[constraint.BodyB]; + if (handleA.Value >= 0 && handleB.Value >= 0) { - //There's no guarantee that the bodies involved with the removed constraint are actually in the simulation. - //Rather than doing anything clever, just retry a few times. - var constraintIdentityIndex = random.Next(removedConstraints.Count); - var constraintIdentity = removedConstraints[constraintIdentityIndex]; - ref var constraint = ref constraintDescriptions[constraintIdentity]; - var handleA = bodyHandles[constraint.BodyA]; - var handleB = bodyHandles[constraint.BodyB]; - if (handleA.Value >= 0 && handleB.Value >= 0) - { - //The constraint is addable. - var constraintHandle = simulation.Solver.Add(handleA, handleB, constraint.Description); - constraintHandles[constraintIdentity] = constraintHandle; - constraintHandlesToIdentity[constraintHandle.Value] = constraintIdentity; - WriteLine($"Added constraint, handle: {constraintHandle}"); - FastRemoveAt(removedConstraints, constraintIdentityIndex); - break; - } - } while (++attemptCount < 10); - Validate(simulation, removedConstraints, removedBodies, bodyHandles.Length, constraintHandles.Length); - } + //The constraint is addable. + var constraintHandle = simulation.Solver.Add(handleA, handleB, constraint.Description); + constraintHandles[constraintIdentity] = constraintHandle; + constraintHandlesToIdentity[constraintHandle.Value] = constraintIdentity; + WriteLine($"Added constraint, handle: {constraintHandle}"); + FastRemoveAt(removedConstraints, constraintIdentityIndex); + break; + } + } while (++attemptCount < 10); + Validate(simulation, removedConstraints, removedBodies, bodyHandles.Length, constraintHandles.Length); + } - [MethodImpl(MethodImplOptions.NoInlining)] - private static void ChurnRemoveConstraint(Simulation simulation, int originalBodyCount, - int[] constraintHandlesToIdentity, ConstraintHandle[] constraintHandles, CachedConstraint[] constraintDescriptions, List removedConstraints, List removedBodies, Random random) - where T : unmanaged, IConstraintDescription + [MethodImpl(MethodImplOptions.NoInlining)] + private static void ChurnRemoveConstraint(Simulation simulation, int originalBodyCount, + int[] constraintHandlesToIdentity, ConstraintHandle[] constraintHandles, CachedConstraint[] constraintDescriptions, List removedConstraints, List removedBodies, Random random) + where T : unmanaged, IConstraintDescription + { + //Remove a constraint. + ref var activeSet = ref simulation.Solver.ActiveSet; + var batchIndex = random.Next(activeSet.Batches.Count); + ref var batch = ref activeSet.Batches[batchIndex]; + Debug.Assert(batchIndex < activeSet.Batches.Count - 1 || batch.TypeBatches.Count > 0, + "While a lower index batch may end up empty due to a lack of active batch compression, " + + "the last batch should get removed if it becomes empty since there is no danger of pointer invaldiation."); + if (batch.TypeBatches.Count > 0) { - //Remove a constraint. - ref var activeSet = ref simulation.Solver.ActiveSet; - var batchIndex = random.Next(activeSet.Batches.Count); - ref var batch = ref activeSet.Batches[batchIndex]; - Debug.Assert(batchIndex < activeSet.Batches.Count - 1 || batch.TypeBatches.Count > 0, - "While a lower index batch may end up empty due to a lack of active batch compression, " + - "the last batch should get removed if it becomes empty since there is no danger of pointer invaldiation."); - if (batch.TypeBatches.Count > 0) - { - ref var typeBatch = ref batch.TypeBatches[random.Next(batch.TypeBatches.Count)]; - Debug.Assert(typeBatch.ConstraintCount > 0, "If a type batch exists, it should have constraints in it."); - var indexInTypeBatch = random.Next(typeBatch.ConstraintCount); - var constraintHandle = typeBatch.IndexToHandle[indexInTypeBatch]; - - RemoveConstraint(simulation, constraintHandle, constraintHandlesToIdentity, constraintHandles, removedConstraints); - WriteLine($"Removed constraint, former handle: {constraintHandle}"); - Validate(simulation, removedConstraints, removedBodies, originalBodyCount, constraintHandles.Length); - } + ref var typeBatch = ref batch.TypeBatches[random.Next(batch.TypeBatches.Count)]; + Debug.Assert(typeBatch.ConstraintCount > 0, "If a type batch exists, it should have constraints in it."); + var indexInTypeBatch = random.Next(typeBatch.ConstraintCount); + var constraintHandle = typeBatch.IndexToHandle[indexInTypeBatch]; + + RemoveConstraint(simulation, constraintHandle, constraintHandlesToIdentity, constraintHandles, removedConstraints); + WriteLine($"Removed constraint, former handle: {constraintHandle}"); + Validate(simulation, removedConstraints, removedBodies, originalBodyCount, constraintHandles.Length); } + } - public static double AddRemoveChurn(Simulation simulation, int iterations, BodyHandle[] bodyHandles, ConstraintHandle[] constraintHandles) where T : unmanaged, ITwoBodyConstraintDescription + public static double AddRemoveChurn(Simulation simulation, int iterations, BodyHandle[] bodyHandles, ConstraintHandle[] constraintHandles) where T : unmanaged, ITwoBodyConstraintDescription + { + //There are three levels of 'index' for each object in this test: + //1) The top level 'identity'. Even when a body or constraint gets readded, the slot in the top level array maintains a pointer to the new handle. + //2) The in-engine handle. Within the engine, it acts as the identity. The engine only cares about tracking identity between calls to add and remove for any given object. + //3) The index of the object in memory. + //As we add and remove stuff, we want to still be able to find a particular constraint by its original identity, so we have to do some work to track that. + + //Take a snapshot of the body descriptions. + var bodyDescriptions = new BodyDescription[bodyHandles.Length]; + var constraintDescriptions = new CachedConstraint[constraintHandles.Length]; + Debug.Assert(simulation.Bodies.ActiveSet.Count == bodyHandles.Length); + int originalConstraintCount = simulation.Solver.CountConstraints(); + Debug.Assert(constraintHandles.Length == originalConstraintCount); + + //We'll need a mapping from the current handles back to the identity. + var bodyHandlesToIdentity = new int[simulation.Bodies.HandleToLocation.Length]; + for (int i = 0; i < bodyHandlesToIdentity.Length; ++i) + bodyHandlesToIdentity[i] = -1; + var constraintHandlesToIdentity = new int[simulation.Solver.HandleToConstraint.Length]; + for (int i = 0; i < constraintHandlesToIdentity.Length; ++i) + constraintHandlesToIdentity[i] = -1; + + for (int i = 0; i < bodyHandles.Length; ++i) { - //There are three levels of 'index' for each object in this test: - //1) The top level 'identity'. Even when a body or constraint gets readded, the slot in the top level array maintains a pointer to the new handle. - //2) The in-engine handle. Within the engine, it acts as the identity. The engine only cares about tracking identity between calls to add and remove for any given object. - //3) The index of the object in memory. - //As we add and remove stuff, we want to still be able to find a particular constraint by its original identity, so we have to do some work to track that. - - //Take a snapshot of the body descriptions. - var bodyDescriptions = new BodyDescription[bodyHandles.Length]; - var constraintDescriptions = new CachedConstraint[constraintHandles.Length]; - Debug.Assert(simulation.Bodies.ActiveSet.Count == bodyHandles.Length); - int originalConstraintCount = simulation.Solver.CountConstraints(); - Debug.Assert(constraintHandles.Length == originalConstraintCount); - - //We'll need a mapping from the current handles back to the identity. - var bodyHandlesToIdentity = new int[simulation.Bodies.HandleToLocation.Length]; - for (int i = 0; i < bodyHandlesToIdentity.Length; ++i) - bodyHandlesToIdentity[i] = -1; - var constraintHandlesToIdentity = new int[simulation.Solver.HandleToConstraint.Length]; - for (int i = 0; i < constraintHandlesToIdentity.Length; ++i) - constraintHandlesToIdentity[i] = -1; - - for (int i = 0; i < bodyHandles.Length; ++i) - { - ref var bodyDescription = ref bodyDescriptions[i]; - var handle = bodyHandles[i]; - simulation.Bodies.GetDescription(handle, out bodyDescription); - bodyHandlesToIdentity[handle.Value] = i; - } + ref var bodyDescription = ref bodyDescriptions[i]; + var handle = bodyHandles[i]; + simulation.Bodies.GetDescription(handle, out bodyDescription); + bodyHandlesToIdentity[handle.Value] = i; + } - for (int i = 0; i < constraintHandles.Length; ++i) - { - var constraintHandle = constraintHandles[i]; - constraintHandlesToIdentity[constraintHandle.Value] = i; - simulation.Solver.GetDescription(constraintHandle, out constraintDescriptions[i].Description); - var reference = simulation.Solver.GetConstraintReference(constraintHandle); - - var bodyIdentityEnumerator = new BodyEnumerator(simulation.Bodies, bodyHandlesToIdentity); - simulation.Solver.EnumerateConnectedRawBodyReferences(ref reference.TypeBatch, reference.IndexInTypeBatch, ref bodyIdentityEnumerator); - constraintDescriptions[i].BodyA = bodyIdentityEnumerator.IdentityA; - constraintDescriptions[i].BodyB = bodyIdentityEnumerator.IdentityB; - } + for (int i = 0; i < constraintHandles.Length; ++i) + { + var constraintHandle = constraintHandles[i]; + constraintHandlesToIdentity[constraintHandle.Value] = i; + simulation.Solver.GetDescription(constraintHandle, out constraintDescriptions[i].Description); + var reference = simulation.Solver.GetConstraintReference(constraintHandle); + + var bodyIdentityEnumerator = new BodyEnumerator(simulation.Bodies, bodyHandlesToIdentity); + simulation.Solver.EnumerateConnectedRawBodyReferences(ref reference.TypeBatch, reference.IndexInTypeBatch, ref bodyIdentityEnumerator); + constraintDescriptions[i].BodyA = bodyIdentityEnumerator.IdentityA; + constraintDescriptions[i].BodyB = bodyIdentityEnumerator.IdentityB; + } - //Any time a body is removed, the handle in the associated body entry must be updated to -1. - //All constraints refer to bodies by their out-of-engine identity so that everything stays robust in the face of adds and removes. - var removedConstraints = new List(); - var removedBodies = new List(); - var random = new Random(5); + //Any time a body is removed, the handle in the associated body entry must be updated to -1. + //All constraints refer to bodies by their out-of-engine identity so that everything stays robust in the face of adds and removes. + var removedConstraints = new List(); + var removedBodies = new List(); + var random = new Random(5); - Validate(simulation, removedConstraints, removedBodies, bodyHandles.Length, originalConstraintCount); + Validate(simulation, removedConstraints, removedBodies, bodyHandles.Length, originalConstraintCount); - var constraintActionProbability = originalConstraintCount > 0 ? 1 - (double)simulation.Bodies.ActiveSet.Count / originalConstraintCount : 0; + var constraintActionProbability = originalConstraintCount > 0 ? 1 - (double)simulation.Bodies.ActiveSet.Count / originalConstraintCount : 0; - var timer = Stopwatch.StartNew(); - for (int iterationIndex = 0; iterationIndex < iterations; ++iterationIndex) + var timer = Stopwatch.StartNew(); + for (int iterationIndex = 0; iterationIndex < iterations; ++iterationIndex) + { + if (random.NextDouble() < constraintActionProbability) { - if (random.NextDouble() < constraintActionProbability) + //Constraint action. + var constraintRemovalProbability = (originalConstraintCount - removedConstraints.Count) / (double)originalConstraintCount; + if (random.NextDouble() < constraintRemovalProbability) { - //Constraint action. - var constraintRemovalProbability = (originalConstraintCount - removedConstraints.Count) / (double)originalConstraintCount; - if (random.NextDouble() < constraintRemovalProbability) - { - ChurnRemoveConstraint(simulation, bodyHandles.Length, constraintHandlesToIdentity, constraintHandles, constraintDescriptions, removedConstraints, removedBodies, random); - } - else if (removedConstraints.Count > 0) - { - ChurnAddConstraint(simulation, bodyHandles, constraintHandles, constraintHandlesToIdentity, constraintDescriptions, removedConstraints, removedBodies, random); - } + ChurnRemoveConstraint(simulation, bodyHandles.Length, constraintHandlesToIdentity, constraintHandles, constraintDescriptions, removedConstraints, removedBodies, random); } - else + else if (removedConstraints.Count > 0) { - //Body action. - var bodyRemovalProbability = (bodyHandles.Length - removedBodies.Count) / (double)bodyHandles.Length; - if (random.NextDouble() < bodyRemovalProbability) - { - ChurnRemoveBody(simulation, bodyHandles, bodyHandlesToIdentity, constraintHandles, constraintHandlesToIdentity, constraintDescriptions, removedConstraints, removedBodies, random); - } - else if (removedBodies.Count > 0) - { - ChurnAddBody(simulation, bodyDescriptions, bodyHandles, bodyHandlesToIdentity, originalConstraintCount, removedConstraints, removedBodies, random); - } + ChurnAddConstraint(simulation, bodyHandles, constraintHandles, constraintHandlesToIdentity, constraintDescriptions, removedConstraints, removedBodies, random); } } - timer.Stop(); - - //Go ahead and add everything back so the outer test can proceed unaffected. Theoretically. - while (removedBodies.Count > 0) - { - ChurnAddBody(simulation, bodyDescriptions, bodyHandles, bodyHandlesToIdentity, originalConstraintCount, removedConstraints, removedBodies, random); - } - while (removedConstraints.Count > 0) - { - ChurnAddConstraint(simulation, bodyHandles, constraintHandles, constraintHandlesToIdentity, constraintDescriptions, removedConstraints, removedBodies, random); - } - - for (int i = 0; i < constraintHandles.Length; ++i) + else { - simulation.Solver.GetDescription(constraintHandles[i], out T description); - Debug.Assert(description.Equals(constraintDescriptions[i].Description), "Moving constraints around should not affect their descriptions."); + //Body action. + var bodyRemovalProbability = (bodyHandles.Length - removedBodies.Count) / (double)bodyHandles.Length; + if (random.NextDouble() < bodyRemovalProbability) + { + ChurnRemoveBody(simulation, bodyHandles, bodyHandlesToIdentity, constraintHandles, constraintHandlesToIdentity, constraintDescriptions, removedConstraints, removedBodies, random); + } + else if (removedBodies.Count > 0) + { + ChurnAddBody(simulation, bodyDescriptions, bodyHandles, bodyHandlesToIdentity, originalConstraintCount, removedConstraints, removedBodies, random); + } } + } + timer.Stop(); - var newConstraintCount = simulation.Solver.CountConstraints(); - Debug.Assert(newConstraintCount == originalConstraintCount, "Best have the same number of constraints if we actually added them all back!"); - Debug.Assert(bodyHandles.Length == simulation.Bodies.ActiveSet.Count, "And bodies, too!"); + //Go ahead and add everything back so the outer test can proceed unaffected. Theoretically. + while (removedBodies.Count > 0) + { + ChurnAddBody(simulation, bodyDescriptions, bodyHandles, bodyHandlesToIdentity, originalConstraintCount, removedConstraints, removedBodies, random); + } + while (removedConstraints.Count > 0) + { + ChurnAddConstraint(simulation, bodyHandles, constraintHandles, constraintHandlesToIdentity, constraintDescriptions, removedConstraints, removedBodies, random); + } - return timer.Elapsed.TotalSeconds; + for (int i = 0; i < constraintHandles.Length; ++i) + { + simulation.Solver.GetDescription(constraintHandles[i], out T description); + Debug.Assert(description.Equals(constraintDescriptions[i].Description), "Moving constraints around should not affect their descriptions."); } + var newConstraintCount = simulation.Solver.CountConstraints(); + Debug.Assert(newConstraintCount == originalConstraintCount, "Best have the same number of constraints if we actually added them all back!"); + Debug.Assert(bodyHandles.Length == simulation.Bodies.ActiveSet.Count, "And bodies, too!"); + return timer.Elapsed.TotalSeconds; } + + } diff --git a/Demos/SpecializedTests/SolverBatchTestDemo.cs b/Demos/SpecializedTests/SolverBatchTestDemo.cs index 4a487e2b3..a6e5dc345 100644 --- a/Demos/SpecializedTests/SolverBatchTestDemo.cs +++ b/Demos/SpecializedTests/SolverBatchTestDemo.cs @@ -8,101 +8,100 @@ using BepuPhysics.Constraints; using DemoContentLoader; -namespace Demos.Demos +namespace Demos.Demos; + +public class SolverBatchTestDemo : Demo { - public class SolverBatchTestDemo : Demo + public override void Initialize(ContentArchive content, Camera camera) { - public override void Initialize(ContentArchive content, Camera camera) - { - camera.Position = new Vector3(-120, 30, -120); - camera.Yaw = MathHelper.Pi * 3f / 4; - camera.Pitch = 0.1f; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); - Simulation.Solver.VelocityIterationCount = 8; + camera.Position = new Vector3(-120, 30, -120); + camera.Yaw = MathHelper.Pi * 3f / 4; + camera.Pitch = 0.1f; + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); + Simulation.Solver.VelocityIterationCount = 8; - //Build a grid of shapes to be connected. - var clothNodeShape = new Sphere(0.5f); - var clothNodeInertia = clothNodeShape.ComputeInertia(1); - var clothNodeShapeIndex = Simulation.Shapes.Add(clothNodeShape); - const int width = 128; - const int length = 128; - const float spacing = 1.75f; - BodyHandle[][] nodeHandles = new BodyHandle[width][]; - for (int i = 0; i < width; ++i) + //Build a grid of shapes to be connected. + var clothNodeShape = new Sphere(0.5f); + var clothNodeInertia = clothNodeShape.ComputeInertia(1); + var clothNodeShapeIndex = Simulation.Shapes.Add(clothNodeShape); + const int width = 128; + const int length = 128; + const float spacing = 1.75f; + BodyHandle[][] nodeHandles = new BodyHandle[width][]; + for (int i = 0; i < width; ++i) + { + nodeHandles[i] = new BodyHandle[length]; + for (int j = 0; j < length; ++j) { - nodeHandles[i] = new BodyHandle[length]; - for (int j = 0; j < length; ++j) - { - var location = new Vector3(0, 30, 0) + new Vector3(spacing, 0, spacing) * (new Vector3(i, 0, j) + new Vector3(-width * 0.5f, 0, -length * 0.5f)); - var bodyDescription = BodyDescription.CreateDynamic(location, clothNodeInertia, clothNodeShapeIndex, 0.01f); - nodeHandles[i][j] = Simulation.Bodies.Add(bodyDescription); + var location = new Vector3(0, 30, 0) + new Vector3(spacing, 0, spacing) * (new Vector3(i, 0, j) + new Vector3(-width * 0.5f, 0, -length * 0.5f)); + var bodyDescription = BodyDescription.CreateDynamic(location, clothNodeInertia, clothNodeShapeIndex, 0.01f); + nodeHandles[i][j] = Simulation.Bodies.Add(bodyDescription); - } } - //Construct some joints between the nodes. - var left = new BallSocket - { - LocalOffsetA = new Vector3(-spacing * 0.5f, 0, 0), - LocalOffsetB = new Vector3(spacing * 0.5f, 0, 0), - SpringSettings = new SpringSettings(10, 1) - }; - var up = new BallSocket - { - LocalOffsetA = new Vector3(0, 0, -spacing * 0.5f), - LocalOffsetB = new Vector3(0, 0, spacing * 0.5f), - SpringSettings = new SpringSettings(10, 1) - }; - var leftUp = new BallSocket - { - LocalOffsetA = new Vector3(-spacing * 0.5f, 0, -spacing * 0.5f), - LocalOffsetB = new Vector3(spacing * 0.5f, 0, spacing * 0.5f), - SpringSettings = new SpringSettings(10, 1) - }; - var rightUp = new BallSocket - { - LocalOffsetA = new Vector3(spacing * 0.5f, 0, -spacing * 0.5f), - LocalOffsetB = new Vector3(-spacing * 0.5f, 0, spacing * 0.5f), - SpringSettings = new SpringSettings(10, 1) - }; - for (int i = 0; i < width; ++i) + } + //Construct some joints between the nodes. + var left = new BallSocket + { + LocalOffsetA = new Vector3(-spacing * 0.5f, 0, 0), + LocalOffsetB = new Vector3(spacing * 0.5f, 0, 0), + SpringSettings = new SpringSettings(10, 1) + }; + var up = new BallSocket + { + LocalOffsetA = new Vector3(0, 0, -spacing * 0.5f), + LocalOffsetB = new Vector3(0, 0, spacing * 0.5f), + SpringSettings = new SpringSettings(10, 1) + }; + var leftUp = new BallSocket + { + LocalOffsetA = new Vector3(-spacing * 0.5f, 0, -spacing * 0.5f), + LocalOffsetB = new Vector3(spacing * 0.5f, 0, spacing * 0.5f), + SpringSettings = new SpringSettings(10, 1) + }; + var rightUp = new BallSocket + { + LocalOffsetA = new Vector3(spacing * 0.5f, 0, -spacing * 0.5f), + LocalOffsetB = new Vector3(-spacing * 0.5f, 0, spacing * 0.5f), + SpringSettings = new SpringSettings(10, 1) + }; + for (int i = 0; i < width; ++i) + { + for (int j = 0; j < length; ++j) { - for (int j = 0; j < length; ++j) - { - if (i >= 1) - Simulation.Solver.Add(nodeHandles[i][j], nodeHandles[i - 1][j], left); - if (j >= 1) - Simulation.Solver.Add(nodeHandles[i][j], nodeHandles[i][j - 1], up); - if (i >= 1 && j >= 1) - Simulation.Solver.Add(nodeHandles[i][j], nodeHandles[i - 1][j - 1], leftUp); - if (i < width - 1 && j >= 1) - Simulation.Solver.Add(nodeHandles[i][j], nodeHandles[i + 1][j - 1], rightUp); - } + if (i >= 1) + Simulation.Solver.Add(nodeHandles[i][j], nodeHandles[i - 1][j], left); + if (j >= 1) + Simulation.Solver.Add(nodeHandles[i][j], nodeHandles[i][j - 1], up); + if (i >= 1 && j >= 1) + Simulation.Solver.Add(nodeHandles[i][j], nodeHandles[i - 1][j - 1], leftUp); + if (i < width - 1 && j >= 1) + Simulation.Solver.Add(nodeHandles[i][j], nodeHandles[i + 1][j - 1], rightUp); } - var bigBallShape = new Sphere(45); - var bigBallShapeIndex = Simulation.Shapes.Add(bigBallShape); + } + var bigBallShape = new Sphere(45); + var bigBallShapeIndex = Simulation.Shapes.Add(bigBallShape); - var bigBallDescription = BodyDescription.CreateKinematic(new Vector3(-10, -15, 0), bigBallShapeIndex, 0); - bigBallHandle = Simulation.Bodies.Add(bigBallDescription); + var bigBallDescription = BodyDescription.CreateKinematic(new Vector3(-10, -15, 0), bigBallShapeIndex, 0); + bigBallHandle = Simulation.Bodies.Add(bigBallDescription); - var groundShape = new Box(200, 1, 200); - var groundShapeIndex = Simulation.Shapes.Add(groundShape); + var groundShape = new Box(200, 1, 200); + var groundShapeIndex = Simulation.Shapes.Add(groundShape); - var groundDescription = BodyDescription.CreateKinematic(new Vector3(0, -10, 0), groundShapeIndex, 0); - Simulation.Bodies.Add(groundDescription); - } - BodyHandle bigBallHandle; - float timeAccumulator; - public override void Update(Window window, Camera camera, Input input, float dt) - { - var bigBall = new BodyReference(bigBallHandle, Simulation.Bodies); - timeAccumulator += TimestepDuration; - if (timeAccumulator > MathF.PI * 128) - timeAccumulator -= MathF.PI * 128; - if (!bigBall.Awake) - Simulation.Awakener.AwakenBody(bigBallHandle); - bigBall.Velocity.Linear = new Vector3(0, 3f * MathF.Sin(timeAccumulator * 5), 0); - base.Update(window, camera, input, dt); - } + var groundDescription = BodyDescription.CreateKinematic(new Vector3(0, -10, 0), groundShapeIndex, 0); + Simulation.Bodies.Add(groundDescription); + } + BodyHandle bigBallHandle; + float timeAccumulator; + public override void Update(Window window, Camera camera, Input input, float dt) + { + var bigBall = new BodyReference(bigBallHandle, Simulation.Bodies); + timeAccumulator += TimestepDuration; + if (timeAccumulator > MathF.PI * 128) + timeAccumulator -= MathF.PI * 128; + if (!bigBall.Awake) + Simulation.Awakener.AwakenBody(bigBallHandle); + bigBall.Velocity.Linear = new Vector3(0, 3f * MathF.Sin(timeAccumulator * 5), 0); + base.Update(window, camera, input, dt); } } diff --git a/Demos/SpecializedTests/SortTest.cs b/Demos/SpecializedTests/SortTest.cs index fa6cc772d..c5740e359 100644 --- a/Demos/SpecializedTests/SortTest.cs +++ b/Demos/SpecializedTests/SortTest.cs @@ -4,98 +4,97 @@ using System.Diagnostics; using System.Runtime.CompilerServices; -namespace Demos.SpecializedTests +namespace Demos.SpecializedTests; + +public static class SortTest { - public static class SortTest + + struct Comparer : IComparerRef { + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public int Compare(ref int a, ref int b) + { + return a < b ? -1 : a > b ? 1 : 0; + } + } - struct Comparer : IComparerRef + static void VerifySort(ref Buffer keys) + { + for (int i = 1; i < keys.Length; ++i) { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public int Compare(ref int a, ref int b) - { - return a < b ? -1 : a > b ? 1 : 0; - } + Debug.Assert(keys[i] >= keys[i - 1]); } + } + public static void Test() + { + const int elementCount = 65536; + const int elementExclusiveUpperBound = 1 << 16; - static void VerifySort(ref Buffer keys) + var bufferPool = new BufferPool(); + bufferPool.Take(elementCount, out var keys); + bufferPool.Take(elementCount, out var indexMap); + bufferPool.Take(elementCount, out var keys2); + bufferPool.Take(elementCount, out var indexMap2); + bufferPool.Take(elementCount, out var keys3); + bufferPool.Take(elementCount, out var indexMap3); + bufferPool.Take(elementCount, out var keys4); + bufferPool.Take(elementCount, out var indexMap4); + Random random = new Random(5); + + for (int iteration = 0; iteration < 4; ++iteration) { - for (int i = 1; i < keys.Length; ++i) + for (int i = 0; i < elementCount; ++i) { - Debug.Assert(keys[i] >= keys[i - 1]); + indexMap[i] = i; + //keys[i] = i / (elementCount / elementExclusiveUpperBound); + //keys[i] = i % elementExclusiveUpperBound; + //keys[i] = i; + keys[i] = random.Next(elementExclusiveUpperBound); } - } - public static void Test() - { - const int elementCount = 65536; - const int elementExclusiveUpperBound = 1 << 16; + keys.CopyTo(0, keys2, 0, elementCount); + keys.CopyTo(0, keys3, 0, elementCount); + keys.CopyTo(0, keys4, 0, elementCount); + indexMap.CopyTo(0, indexMap2, 0, elementCount); + indexMap.CopyTo(0, indexMap3, 0, elementCount); + indexMap.CopyTo(0, indexMap4, 0, elementCount); + var timer = Stopwatch.StartNew(); - var bufferPool = new BufferPool(); - bufferPool.Take(elementCount, out var keys); - bufferPool.Take(elementCount, out var indexMap); - bufferPool.Take(elementCount, out var keys2); - bufferPool.Take(elementCount, out var indexMap2); - bufferPool.Take(elementCount, out var keys3); - bufferPool.Take(elementCount, out var indexMap3); - bufferPool.Take(elementCount, out var keys4); - bufferPool.Take(elementCount, out var indexMap4); - Random random = new Random(5); - - for (int iteration = 0; iteration < 4; ++iteration) + var keysScratch = new int[elementCount]; + var valuesScratch = new int[elementCount]; + var bucketCounts = new int[1024]; + for (int t = 0; t < 16; ++t) { - for (int i = 0; i < elementCount; ++i) - { - indexMap[i] = i; - //keys[i] = i / (elementCount / elementExclusiveUpperBound); - //keys[i] = i % elementExclusiveUpperBound; - //keys[i] = i; - keys[i] = random.Next(elementExclusiveUpperBound); - } - keys.CopyTo(0, keys2, 0, elementCount); - keys.CopyTo(0, keys3, 0, elementCount); - keys.CopyTo(0, keys4, 0, elementCount); - indexMap.CopyTo(0, indexMap2, 0, elementCount); - indexMap.CopyTo(0, indexMap3, 0, elementCount); - indexMap.CopyTo(0, indexMap4, 0, elementCount); - var timer = Stopwatch.StartNew(); + var comparer = new Comparer(); + timer.Restart(); + QuickSort.Sort(ref keys[0], ref indexMap[0], 0, elementCount - 1, ref comparer); + //QuickSort.Sort2(ref keys[0], ref indexMap[0], 0, elementCount - 1, ref comparer); + timer.Stop(); + VerifySort(ref keys); + Console.WriteLine($"QuickSort time (ms): {timer.Elapsed.TotalSeconds * 1e3}"); - var keysScratch = new int[elementCount]; - var valuesScratch = new int[elementCount]; - var bucketCounts = new int[1024]; - for (int t = 0; t < 16; ++t) - { - var comparer = new Comparer(); - timer.Restart(); - QuickSort.Sort(ref keys[0], ref indexMap[0], 0, elementCount - 1, ref comparer); - //QuickSort.Sort2(ref keys[0], ref indexMap[0], 0, elementCount - 1, ref comparer); - timer.Stop(); - VerifySort(ref keys); - Console.WriteLine($"QuickSort time (ms): {timer.Elapsed.TotalSeconds * 1e3}"); + //timer.Restart(); + //Array.Sort(keys2.Memory, indexMap2.Memory, 0, elementCount); + //timer.Stop(); + //VerifySort(ref keys2); + //Console.WriteLine($"Array.Sort time (ms): {timer.Elapsed.TotalSeconds * 1e3}"); - //timer.Restart(); - //Array.Sort(keys2.Memory, indexMap2.Memory, 0, elementCount); - //timer.Stop(); - //VerifySort(ref keys2); - //Console.WriteLine($"Array.Sort time (ms): {timer.Elapsed.TotalSeconds * 1e3}"); + timer.Restart(); + Array.Clear(bucketCounts, 0, bucketCounts.Length); + LSBRadixSort.SortU16(ref keys3[0], ref indexMap3[0], ref keysScratch[0], ref valuesScratch[0], ref bucketCounts[0], elementCount); + timer.Stop(); + VerifySort(ref keys3); + Console.WriteLine($"{t} LSBRadixSort time (ms): {timer.Elapsed.TotalSeconds * 1e3}"); - timer.Restart(); - Array.Clear(bucketCounts, 0, bucketCounts.Length); - LSBRadixSort.SortU16(ref keys3[0], ref indexMap3[0], ref keysScratch[0], ref valuesScratch[0], ref bucketCounts[0], elementCount); - timer.Stop(); - VerifySort(ref keys3); - Console.WriteLine($"{t} LSBRadixSort time (ms): {timer.Elapsed.TotalSeconds * 1e3}"); - - var originalIndices = new int[256]; - timer.Restart(); - //MSBRadixSort.SortU32(ref keys4[0], ref indexMap4[0], ref bucketCounts[0], ref originalIndices[0], elementCount, 24); - MSBRadixSort.SortU32(ref keys4[0], ref indexMap4[0], elementCount, SpanHelper.GetContainingPowerOf2(elementExclusiveUpperBound)); - timer.Stop(); - VerifySort(ref keys4); - Console.WriteLine($"{t} MSBRadixSort time (ms): {timer.Elapsed.TotalSeconds * 1e3}"); - } + var originalIndices = new int[256]; + timer.Restart(); + //MSBRadixSort.SortU32(ref keys4[0], ref indexMap4[0], ref bucketCounts[0], ref originalIndices[0], elementCount, 24); + MSBRadixSort.SortU32(ref keys4[0], ref indexMap4[0], elementCount, SpanHelper.GetContainingPowerOf2(elementExclusiveUpperBound)); + timer.Stop(); + VerifySort(ref keys4); + Console.WriteLine($"{t} MSBRadixSort time (ms): {timer.Elapsed.TotalSeconds * 1e3}"); } - bufferPool.Clear(); - } + bufferPool.Clear(); + } } diff --git a/Demos/SpecializedTests/TestHelpers.cs b/Demos/SpecializedTests/TestHelpers.cs index d5a4b3eae..996773db5 100644 --- a/Demos/SpecializedTests/TestHelpers.cs +++ b/Demos/SpecializedTests/TestHelpers.cs @@ -3,39 +3,38 @@ using System; using System.Numerics; -namespace Demos.SpecializedTests +namespace Demos.SpecializedTests; + +public static class TestHelpers { - public static class TestHelpers + /// + /// Gets a value roughly representing the amount of energy in the simulation. This is occasionally handy for debug purposes. + /// + public static float GetBodyEnergyHeuristic(Bodies bodies) { - /// - /// Gets a value roughly representing the amount of energy in the simulation. This is occasionally handy for debug purposes. - /// - public static float GetBodyEnergyHeuristic(Bodies bodies) - { - float accumulated = 0; - for (int index = 0; index < bodies.ActiveSet.Count; ++index) - { - ref var velocity = ref bodies.ActiveSet.DynamicsState[index].Motion.Velocity; - accumulated += Vector3.Dot(velocity.Linear, velocity.Linear); - accumulated += Vector3.Dot(velocity.Angular, velocity.Angular); - } - return accumulated; - } - public static RigidPose CreateRandomPose(Random random, BoundingBox positionBounds) + float accumulated = 0; + for (int index = 0; index < bodies.ActiveSet.Count; ++index) { - RigidPose pose; - var span = positionBounds.Max - positionBounds.Min; - - pose.Position = positionBounds.Min + span * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); - var axis = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); - var length = axis.Length(); - if (length > 0) - axis /= length; - else - axis = new Vector3(0, 1, 0); - pose.Orientation = BepuUtilities.QuaternionEx.CreateFromAxisAngle(axis, 1203f * random.NextSingle()); - return pose; + ref var velocity = ref bodies.ActiveSet.DynamicsState[index].Motion.Velocity; + accumulated += Vector3.Dot(velocity.Linear, velocity.Linear); + accumulated += Vector3.Dot(velocity.Angular, velocity.Angular); } + return accumulated; + } + public static RigidPose CreateRandomPose(Random random, BoundingBox positionBounds) + { + RigidPose pose; + var span = positionBounds.Max - positionBounds.Min; + pose.Position = positionBounds.Min + span * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); + var axis = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); + var length = axis.Length(); + if (length > 0) + axis /= length; + else + axis = new Vector3(0, 1, 0); + pose.Orientation = BepuUtilities.QuaternionEx.CreateFromAxisAngle(axis, 1203f * random.NextSingle()); + return pose; } + } diff --git a/Demos/SpecializedTests/TreeTest.cs b/Demos/SpecializedTests/TreeTest.cs index 8910c25e0..a1884ed60 100644 --- a/Demos/SpecializedTests/TreeTest.cs +++ b/Demos/SpecializedTests/TreeTest.cs @@ -6,158 +6,157 @@ using System.Runtime.CompilerServices; using BepuPhysics.Trees; -namespace Demos.SpecializedTests +namespace Demos.SpecializedTests; + +public unsafe static class TreeTest { - public unsafe static class TreeTest + public static void Test() { - public static void Test() + var pool = new BufferPool(); + var tree = new Tree(pool, 128); + + const int leafCountAlongXAxis = 11; + const int leafCountAlongYAxis = 13; + const int leafCountAlongZAxis = 15; + var leafCount = leafCountAlongXAxis * leafCountAlongYAxis * leafCountAlongZAxis; + pool.Take(leafCount, out var leafBounds); + + const float boundsSpan = 2; + const float spanRange = 2; + const float boundsSpacing = 3; + var random = new Random(5); + for (int i = 0; i < leafCountAlongXAxis; ++i) { - var pool = new BufferPool(); - var tree = new Tree(pool, 128); - - const int leafCountAlongXAxis = 11; - const int leafCountAlongYAxis = 13; - const int leafCountAlongZAxis = 15; - var leafCount = leafCountAlongXAxis * leafCountAlongYAxis * leafCountAlongZAxis; - pool.Take(leafCount, out var leafBounds); - - const float boundsSpan = 2; - const float spanRange = 2; - const float boundsSpacing = 3; - var random = new Random(5); - for (int i = 0; i < leafCountAlongXAxis; ++i) + for (int j = 0; j < leafCountAlongYAxis; ++j) { - for (int j = 0; j < leafCountAlongYAxis; ++j) + for (int k = 0; k < leafCountAlongZAxis; ++k) { - for (int k = 0; k < leafCountAlongZAxis; ++k) - { - var index = leafCountAlongXAxis * leafCountAlongYAxis * k + leafCountAlongXAxis * j + i; - leafBounds[index].Min = new Vector3(i, j, k) * boundsSpacing; - leafBounds[index].Max = leafBounds[index].Min + new Vector3(boundsSpan) + - spanRange * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); + var index = leafCountAlongXAxis * leafCountAlongYAxis * k + leafCountAlongXAxis * j + i; + leafBounds[index].Min = new Vector3(i, j, k) * boundsSpacing; + leafBounds[index].Max = leafBounds[index].Min + new Vector3(boundsSpan) + + spanRange * new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); - } } } + } - var prebuiltCount = Math.Max(leafCount / 2, 1); + var prebuiltCount = Math.Max(leafCount / 2, 1); - tree.SweepBuild(pool, leafBounds.Slice(prebuiltCount)); - tree.Validate(); + tree.SweepBuild(pool, leafBounds.Slice(prebuiltCount)); + tree.Validate(); - for (int i = prebuiltCount; i < leafCount; ++i) - { - tree.Add(leafBounds[i], pool); - } - tree.Validate(); + for (int i = prebuiltCount; i < leafCount; ++i) + { + tree.Add(leafBounds[i], pool); + } + tree.Validate(); - pool.TakeAtLeast(leafCount, out var handleToLeafIndex); - pool.TakeAtLeast(leafCount, out var leafIndexToHandle); - for (int i = 0; i < leafCount; ++i) - { - handleToLeafIndex[i] = i; - leafIndexToHandle[i] = i; - } + pool.TakeAtLeast(leafCount, out var handleToLeafIndex); + pool.TakeAtLeast(leafCount, out var leafIndexToHandle); + for (int i = 0; i < leafCount; ++i) + { + handleToLeafIndex[i] = i; + leafIndexToHandle[i] = i; + } - const int iterations = 100000; - const int maximumChangesPerIteration = 20; + const int iterations = 100000; + const int maximumChangesPerIteration = 20; - var threadDispatcher = new ThreadDispatcher(Environment.ProcessorCount); - var refineContext = new Tree.RefitAndRefineMultithreadedContext(); - var selfTestContext = new Tree.MultithreadedSelfTest(pool); - var overlapHandlers = new OverlapHandler[threadDispatcher.ThreadCount]; - Action pairTestAction = selfTestContext.PairTest; - var removedLeafHandles = new QuickList(leafCount, pool); - for (int i = 0; i < iterations; ++i) + var threadDispatcher = new ThreadDispatcher(Environment.ProcessorCount); + var refineContext = new Tree.RefitAndRefineMultithreadedContext(); + var selfTestContext = new Tree.MultithreadedSelfTest(pool); + var overlapHandlers = new OverlapHandler[threadDispatcher.ThreadCount]; + Action pairTestAction = selfTestContext.PairTest; + var removedLeafHandles = new QuickList(leafCount, pool); + for (int i = 0; i < iterations; ++i) + { + var changeCount = random.Next(maximumChangesPerIteration); + for (int j = 0; j <= changeCount; ++j) { - var changeCount = random.Next(maximumChangesPerIteration); - for (int j = 0; j <= changeCount; ++j) + var addedFraction = tree.LeafCount / (float)leafCount; + if (random.NextDouble() < addedFraction) { - var addedFraction = tree.LeafCount / (float)leafCount; - if (random.NextDouble() < addedFraction) + //Remove a leaf. + var leafIndexToRemove = random.Next(tree.LeafCount); + var handleToRemove = leafIndexToHandle[leafIndexToRemove]; + var movedLeafIndex = tree.RemoveAt(leafIndexToRemove); + if (movedLeafIndex >= 0) { - //Remove a leaf. - var leafIndexToRemove = random.Next(tree.LeafCount); - var handleToRemove = leafIndexToHandle[leafIndexToRemove]; - var movedLeafIndex = tree.RemoveAt(leafIndexToRemove); - if (movedLeafIndex >= 0) - { - var movedHandle = leafIndexToHandle[movedLeafIndex]; - handleToLeafIndex[movedHandle] = leafIndexToRemove; - leafIndexToHandle[leafIndexToRemove] = movedHandle; - leafIndexToHandle[movedLeafIndex] = -1; - } - else - { - //The removed leaf was the last one. This leaf index is no longer associated with any existing leaf. - leafIndexToHandle[leafIndexToRemove] = -1; - } - handleToLeafIndex[handleToRemove] = -1; - - removedLeafHandles.AddUnsafely(handleToRemove); - - tree.Validate(); + var movedHandle = leafIndexToHandle[movedLeafIndex]; + handleToLeafIndex[movedHandle] = leafIndexToRemove; + leafIndexToHandle[leafIndexToRemove] = movedHandle; + leafIndexToHandle[movedLeafIndex] = -1; } else { - //Add a leaf. - var indexInRemovedList = random.Next(removedLeafHandles.Count); - var handleToAdd = removedLeafHandles[indexInRemovedList]; - removedLeafHandles.FastRemoveAt(indexInRemovedList); - var leafIndex = tree.Add(leafBounds[handleToAdd], pool); - leafIndexToHandle[leafIndex] = handleToAdd; - handleToLeafIndex[handleToAdd] = leafIndex; - - tree.Validate(); + //The removed leaf was the last one. This leaf index is no longer associated with any existing leaf. + leafIndexToHandle[leafIndexToRemove] = -1; } - } - - tree.Refit(); - tree.Validate(); + handleToLeafIndex[handleToRemove] = -1; - tree.RefitAndRefine(pool, i); - tree.Validate(); + removedLeafHandles.AddUnsafely(handleToRemove); - var handler = new OverlapHandler(); - tree.GetSelfOverlaps(ref handler); - tree.Validate(); - - refineContext.RefitAndRefine(ref tree, pool, threadDispatcher, i); - tree.Validate(); - for (int k = 0; k < threadDispatcher.ThreadCount; ++k) - { - overlapHandlers[k] = new OverlapHandler(); + tree.Validate(); } - selfTestContext.PrepareJobs(ref tree, overlapHandlers, threadDispatcher.ThreadCount); - threadDispatcher.DispatchWorkers(pairTestAction); - selfTestContext.CompleteSelfTest(); - tree.Validate(); - - if (i % 50 == 0) + else { - Console.WriteLine($"Cost: {tree.MeasureCostMetric()}"); - Console.WriteLine($"Cache Quality: {tree.MeasureCacheQuality()}"); - Console.WriteLine($"Overlap Count: {handler.OverlapCount}"); + //Add a leaf. + var indexInRemovedList = random.Next(removedLeafHandles.Count); + var handleToAdd = removedLeafHandles[indexInRemovedList]; + removedLeafHandles.FastRemoveAt(indexInRemovedList); + var leafIndex = tree.Add(leafBounds[handleToAdd], pool); + leafIndexToHandle[leafIndex] = handleToAdd; + handleToLeafIndex[handleToAdd] = leafIndex; + + tree.Validate(); } } - threadDispatcher.Dispose(); - pool.Clear(); + tree.Refit(); + tree.Validate(); + tree.RefitAndRefine(pool, i); + tree.Validate(); - } + var handler = new OverlapHandler(); + tree.GetSelfOverlaps(ref handler); + tree.Validate(); - struct OverlapHandler : IOverlapHandler - { - public int OverlapCount; + refineContext.RefitAndRefine(ref tree, pool, threadDispatcher, i); + tree.Validate(); + for (int k = 0; k < threadDispatcher.ThreadCount; ++k) + { + overlapHandlers[k] = new OverlapHandler(); + } + selfTestContext.PrepareJobs(ref tree, overlapHandlers, threadDispatcher.ThreadCount); + threadDispatcher.DispatchWorkers(pairTestAction); + selfTestContext.CompleteSelfTest(); + tree.Validate(); - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Handle(int indexA, int indexB) + if (i % 50 == 0) { - ++OverlapCount; + Console.WriteLine($"Cost: {tree.MeasureCostMetric()}"); + Console.WriteLine($"Cache Quality: {tree.MeasureCacheQuality()}"); + Console.WriteLine($"Overlap Count: {handler.OverlapCount}"); } } + threadDispatcher.Dispose(); + pool.Clear(); + + } + + struct OverlapHandler : IOverlapHandler + { + public int OverlapCount; + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void Handle(int indexA, int indexB) + { + ++OverlapCount; + } + } + } diff --git a/Demos/SpecializedTests/TriangleRayTestDemo.cs b/Demos/SpecializedTests/TriangleRayTestDemo.cs index 08bba4248..7aae92480 100644 --- a/Demos/SpecializedTests/TriangleRayTestDemo.cs +++ b/Demos/SpecializedTests/TriangleRayTestDemo.cs @@ -8,164 +8,163 @@ using System.Diagnostics; using System.Numerics; -namespace Demos.SpecializedTests +namespace Demos.SpecializedTests; + +public class TriangleRayTestDemo : Demo { - public class TriangleRayTestDemo : Demo + void GetPointOnTriangle(Random random, in Triangle triangle, in RigidPose pose, out Vector3 pointOnTriangle) { - void GetPointOnTriangle(Random random, in Triangle triangle, in RigidPose pose, out Vector3 pointOnTriangle) + float total; + float a, b, c; + do { - float total; - float a, b, c; - do - { - a = random.NextSingle(); - b = random.NextSingle(); - c = random.NextSingle(); - total = a + b + c; - } while (total < 1e-7f); - var inverseTotal = 1f / total; - a *= inverseTotal; - b *= inverseTotal; - c *= inverseTotal; - - var localP = triangle.A * a + triangle.B * b + triangle.C * c; - BepuUtilities.QuaternionEx.TransformWithoutOverlap(localP, pose.Orientation, out pointOnTriangle); - pointOnTriangle += pose.Position; + a = random.NextSingle(); + b = random.NextSingle(); + c = random.NextSingle(); + total = a + b + c; + } while (total < 1e-7f); + var inverseTotal = 1f / total; + a *= inverseTotal; + b *= inverseTotal; + c *= inverseTotal; + + var localP = triangle.A * a + triangle.B * b + triangle.C * c; + BepuUtilities.QuaternionEx.TransformWithoutOverlap(localP, pose.Orientation, out pointOnTriangle); + pointOnTriangle += pose.Position; + } + + void GetPointOutsideTriangle(Random random, in Triangle triangle, in RigidPose pose, out Vector3 pointOutsideTriangle) + { + //This is a pretty biased random generator, but that's fine. + var edgeIndex = random.Next(3); + Vector3 borderPoint; + switch (edgeIndex) + { + case 0: + borderPoint = triangle.A + (triangle.B - triangle.A) * random.NextSingle(); + break; + case 1: + borderPoint = triangle.A + (triangle.C - triangle.A) * random.NextSingle(); + break; + default: + borderPoint = triangle.B + (triangle.C - triangle.B) * random.NextSingle(); + break; } + var center = (triangle.A + triangle.B + triangle.C) / 3f; + var offsetToBorder = borderPoint - center; + var localP = center + offsetToBorder * (1.01f + 4 * random.NextSingle()); + + BepuUtilities.QuaternionEx.TransformWithoutOverlap(localP, pose.Orientation, out pointOutsideTriangle); + pointOutsideTriangle += pose.Position; + } - void GetPointOutsideTriangle(Random random, in Triangle triangle, in RigidPose pose, out Vector3 pointOutsideTriangle) + void TestRay(in Triangle triangle, in RigidPose pose, Vector3 rayOrigin, Vector3 rayDirection, bool expectedImpact, Vector3 pointOnTrianglePlane) + { + var hit = triangle.RayTest(pose, rayOrigin, rayDirection, out var t, out var normal); + + TriangleWide wide = default; + wide.Broadcast(triangle); + RigidPoseWide.Broadcast(pose, out var poses); + RayWide rayWide; + Vector3Wide.Broadcast(rayOrigin, out rayWide.Origin); + Vector3Wide.Broadcast(rayDirection, out rayWide.Direction); + wide.RayTest(ref poses, ref rayWide, out var intersectedWide, out var tWide, out var normalWide); + + Debug.Assert(expectedImpact == hit); + Debug.Assert(hit == intersectedWide[0] < 0); + if (hit) { - //This is a pretty biased random generator, but that's fine. - var edgeIndex = random.Next(3); - Vector3 borderPoint; - switch (edgeIndex) - { - case 0: - borderPoint = triangle.A + (triangle.B - triangle.A) * random.NextSingle(); - break; - case 1: - borderPoint = triangle.A + (triangle.C - triangle.A) * random.NextSingle(); - break; - default: - borderPoint = triangle.B + (triangle.C - triangle.B) * random.NextSingle(); - break; - } - var center = (triangle.A + triangle.B + triangle.C) / 3f; - var offsetToBorder = borderPoint - center; - var localP = center + offsetToBorder * (1.01f + 4 * random.NextSingle()); + Debug.Assert(Math.Abs(t - tWide[0]) < 1e-7f); + Vector3Wide.ReadSlot(ref normalWide, 0, out var normalWideLane0); + var normalDot = Vector3.Dot(normalWideLane0, normal); + Debug.Assert(normalDot > 0.9999f && normalDot < 1.00001f); + var hitLocationError = rayDirection * t + (rayOrigin - pointOnTrianglePlane); + Debug.Assert(hitLocationError.Length() < 1e-2f * MathF.Max(pointOnTrianglePlane.Length(), rayDirection.Length())); - BepuUtilities.QuaternionEx.TransformWithoutOverlap(localP, pose.Orientation, out pointOutsideTriangle); - pointOutsideTriangle += pose.Position; } + } - void TestRay(in Triangle triangle, in RigidPose pose, Vector3 rayOrigin, Vector3 rayDirection, bool expectedImpact, Vector3 pointOnTrianglePlane) + public override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(-30, 8, -60); + camera.Yaw = MathHelper.Pi * 3f / 4; + + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); + + Triangle triangle; + triangle.A = new Vector3(0, 0, 0); + triangle.B = new Vector3(1, 0, 0); + triangle.C = new Vector3(0, 0, 1); + var pose = new RigidPose + { + Orientation = Quaternion.Identity, + Position = new Vector3(0) + }; + var rayOrigin = new Vector3(1f / 3f, 5, 1 / 3f); + var rayDirection = new Vector3(0, -1, 0); + + //The other convex ray tester doesn't quite map well to infinitely thin triangles, so we have our own little tester here. + TestRay(triangle, pose, rayOrigin, rayDirection, true, new Vector3(rayOrigin.X, 0, rayOrigin.Z)); + + Random random = new Random(5); + const float shapeMin = -50; + const float shapeSpan = 100; + for (int i = 0; i < 10000; ++i) { - var hit = triangle.RayTest(pose, rayOrigin, rayDirection, out var t, out var normal); - - TriangleWide wide = default; - wide.Broadcast(triangle); - RigidPoseWide.Broadcast(pose, out var poses); - RayWide rayWide; - Vector3Wide.Broadcast(rayOrigin, out rayWide.Origin); - Vector3Wide.Broadcast(rayDirection, out rayWide.Direction); - wide.RayTest(ref poses, ref rayWide, out var intersectedWide, out var tWide, out var normalWide); - - Debug.Assert(expectedImpact == hit); - Debug.Assert(hit == intersectedWide[0] < 0); - if (hit) + triangle.A = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) * shapeSpan + new Vector3(shapeMin); + triangle.B = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) * shapeSpan + new Vector3(shapeMin); + triangle.C = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) * shapeSpan + new Vector3(shapeMin); + + var localTriangleCenter = (triangle.A + triangle.B + triangle.C) / 3f; + triangle.A -= localTriangleCenter; + triangle.B -= localTriangleCenter; + triangle.C -= localTriangleCenter; + + rayOrigin = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) * shapeSpan + new Vector3(shapeMin); + + float orientationLengthSquared; + while (true) { - Debug.Assert(Math.Abs(t - tWide[0]) < 1e-7f); - Vector3Wide.ReadSlot(ref normalWide, 0, out var normalWideLane0); - var normalDot = Vector3.Dot(normalWideLane0, normal); - Debug.Assert(normalDot > 0.9999f && normalDot < 1.00001f); - var hitLocationError = rayDirection * t + (rayOrigin - pointOnTrianglePlane); - Debug.Assert(hitLocationError.Length() < 1e-2f * MathF.Max(pointOnTrianglePlane.Length(), rayDirection.Length())); - + pose.Orientation.X = random.NextSingle() * 2 - 1; + pose.Orientation.Y = random.NextSingle() * 2 - 1; + pose.Orientation.Z = random.NextSingle() * 2 - 1; + pose.Orientation.W = random.NextSingle() * 2 - 1; + orientationLengthSquared = pose.Orientation.LengthSquared(); + if (orientationLengthSquared > 1e-7f) + break; } - } + BepuUtilities.QuaternionEx.Scale(pose.Orientation, 1f / (float)Math.Sqrt(orientationLengthSquared), out pose.Orientation); + pose.Position = BepuUtilities.QuaternionEx.Transform(localTriangleCenter, pose.Orientation); - public override void Initialize(ContentArchive content, Camera camera) - { - camera.Position = new Vector3(-30, 8, -60); - camera.Yaw = MathHelper.Pi * 3f / 4; + var normal = Vector3.Cross(triangle.C - triangle.A, triangle.B - triangle.A); + var normalLength = normal.Length(); + if (normalLength < 1e-7f) + continue; + normal /= normalLength; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); - Triangle triangle; - triangle.A = new Vector3(0, 0, 0); - triangle.B = new Vector3(1, 0, 0); - triangle.C = new Vector3(0, 0, 1); - var pose = new RigidPose + BepuUtilities.QuaternionEx.Transform(normal, pose.Orientation, out normal); + Vector3 pointOnTriangle; + do { - Orientation = Quaternion.Identity, - Position = new Vector3(0) - }; - var rayOrigin = new Vector3(1f / 3f, 5, 1 / 3f); - var rayDirection = new Vector3(0, -1, 0); - - //The other convex ray tester doesn't quite map well to infinitely thin triangles, so we have our own little tester here. - TestRay(triangle, pose, rayOrigin, rayDirection, true, new Vector3(rayOrigin.X, 0, rayOrigin.Z)); - - Random random = new Random(5); - const float shapeMin = -50; - const float shapeSpan = 100; - for (int i = 0; i < 10000; ++i) + GetPointOnTriangle(random, triangle, pose, out pointOnTriangle); + rayDirection = pointOnTriangle - rayOrigin; + } while (rayDirection.LengthSquared() < 1e-9f); + rayDirection *= (0.5f + 10 * random.NextSingle()) / rayDirection.Length(); + var shouldHit = Vector3.Dot(rayDirection, normal) < 0; + TestRay(triangle, pose, rayOrigin, rayDirection, shouldHit, pointOnTriangle); + + Vector3 pointOutsideTriangle; + do { - triangle.A = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) * shapeSpan + new Vector3(shapeMin); - triangle.B = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) * shapeSpan + new Vector3(shapeMin); - triangle.C = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) * shapeSpan + new Vector3(shapeMin); - - var localTriangleCenter = (triangle.A + triangle.B + triangle.C) / 3f; - triangle.A -= localTriangleCenter; - triangle.B -= localTriangleCenter; - triangle.C -= localTriangleCenter; - - rayOrigin = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()) * shapeSpan + new Vector3(shapeMin); - - float orientationLengthSquared; - while (true) - { - pose.Orientation.X = random.NextSingle() * 2 - 1; - pose.Orientation.Y = random.NextSingle() * 2 - 1; - pose.Orientation.Z = random.NextSingle() * 2 - 1; - pose.Orientation.W = random.NextSingle() * 2 - 1; - orientationLengthSquared = pose.Orientation.LengthSquared(); - if (orientationLengthSquared > 1e-7f) - break; - } - BepuUtilities.QuaternionEx.Scale(pose.Orientation, 1f / (float)Math.Sqrt(orientationLengthSquared), out pose.Orientation); - pose.Position = BepuUtilities.QuaternionEx.Transform(localTriangleCenter, pose.Orientation); - - var normal = Vector3.Cross(triangle.C - triangle.A, triangle.B - triangle.A); - var normalLength = normal.Length(); - if (normalLength < 1e-7f) - continue; - normal /= normalLength; - - - BepuUtilities.QuaternionEx.Transform(normal, pose.Orientation, out normal); - Vector3 pointOnTriangle; - do - { - GetPointOnTriangle(random, triangle, pose, out pointOnTriangle); - rayDirection = pointOnTriangle - rayOrigin; - } while (rayDirection.LengthSquared() < 1e-9f); - rayDirection *= (0.5f + 10 * random.NextSingle()) / rayDirection.Length(); - var shouldHit = Vector3.Dot(rayDirection, normal) < 0; - TestRay(triangle, pose, rayOrigin, rayDirection, shouldHit, pointOnTriangle); - - Vector3 pointOutsideTriangle; - do - { - GetPointOutsideTriangle(random, triangle, pose, out pointOutsideTriangle); - rayDirection = pointOutsideTriangle - rayOrigin; - } while (rayDirection.LengthSquared() < 1e-9f); - rayDirection *= (0.5f + 10 * random.NextSingle()) / rayDirection.Length(); - TestRay(triangle, pose, rayOrigin, rayDirection, false, pointOutsideTriangle); - } - + GetPointOutsideTriangle(random, triangle, pose, out pointOutsideTriangle); + rayDirection = pointOutsideTriangle - rayOrigin; + } while (rayDirection.LengthSquared() < 1e-9f); + rayDirection *= (0.5f + 10 * random.NextSingle()) / rayDirection.Length(); + TestRay(triangle, pose, rayOrigin, rayDirection, false, pointOutsideTriangle); } - } + + } diff --git a/Demos/SpecializedTests/TriangleTestDemo.cs b/Demos/SpecializedTests/TriangleTestDemo.cs index 081162a06..8bcf172bd 100644 --- a/Demos/SpecializedTests/TriangleTestDemo.cs +++ b/Demos/SpecializedTests/TriangleTestDemo.cs @@ -10,203 +10,202 @@ using DemoContentLoader; using BepuPhysics.Constraints; -namespace Demos.SpecializedTests +namespace Demos.SpecializedTests; + +public class TriangleTestDemo : Demo { - public class TriangleTestDemo : Demo + public override void Initialize(ContentArchive content, Camera camera) { - public override void Initialize(ContentArchive content, Camera camera) { + SphereWide sphere = default; + sphere.Broadcast(new Sphere(0.5f)); + TriangleWide triangle = default; + var a = new Vector3(0, 0, 0); + var b = new Vector3(1, 0, 0); + var c = new Vector3(0, 0, 1); + //var center = (a + b + c) / 3f; + //a -= center; + //b -= center; + //c -= center; + triangle.Broadcast(new Triangle(a, b, c)); + var margin = new Vector(1f); + Vector3Wide.Broadcast(new Vector3(1, -1, 0), out var offsetB); + QuaternionWide.Broadcast(QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), out var orientationB); + SphereTriangleTester.Test(ref sphere, ref triangle, ref margin, ref offsetB, ref orientationB, Vector.Count, out var manifold); + } + { + CapsuleWide capsule = default; + capsule.Broadcast(new Capsule(0.5f, 0.5f)); + TriangleWide triangle = default; + var a = new Vector3(0, 0, 0); + var b = new Vector3(1, 0, 0); + var c = new Vector3(0, 0, 1); + //var center = (a + b + c) / 3f; + //a -= center; + //b -= center; + //c -= center; + triangle.Broadcast(new Triangle(a, b, c)); + var margin = new Vector(2f); + Vector3Wide.Broadcast(new Vector3(-1f, -0.5f, -1f), out var offsetB); + QuaternionWide.Broadcast(QuaternionEx.CreateFromAxisAngle(Vector3.Normalize(new Vector3(-1, 0, 1)), MathHelper.PiOver2), out var orientationA); + QuaternionWide.Broadcast(QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), 0), out var orientationB); + CapsuleTriangleTester.Test(ref capsule, ref triangle, ref margin, ref offsetB, ref orientationA, ref orientationB, Vector.Count, out var manifold); + } + { + BoxWide shape = default; + shape.Broadcast(new Box(1f, 1f, 1f)); + TriangleWide triangle = default; + var a = new Vector3(0, 0, 0); + var b = new Vector3(1, 0, 0); + var c = new Vector3(0, 0, 1); + //var center = (a + b + c) / 3f; + //a -= center; + //b -= center; + //c -= center; + triangle.Broadcast(new Triangle(a, b, c)); + var margin = new Vector(2f); + Vector3Wide.Broadcast(new Vector3(-1f, -0.5f, -1f), out var offsetB); + QuaternionWide.Broadcast(QuaternionEx.CreateFromAxisAngle(Vector3.Normalize(new Vector3(-1, 0, 1)), MathHelper.PiOver2), out var orientationA); + QuaternionWide.Broadcast(QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), 0), out var orientationB); + BoxTriangleTester.Test(ref shape, ref triangle, ref margin, ref offsetB, ref orientationA, ref orientationB, Vector.Count, out var manifold); + } + { + TriangleWide a = default, b = default; + a.Broadcast(new Triangle(new Vector3(0, 0, 0), new Vector3(1, 0, 0), new Vector3(0, 0, 1))); + b.Broadcast(new Triangle(new Vector3(0, 0, 0), new Vector3(1, 0, 0), new Vector3(0, 0, 1))); + + var margin = new Vector(2f); + Vector3Wide.Broadcast(new Vector3(0, -1, 0), out var offsetB); + QuaternionWide.Broadcast(QuaternionEx.CreateFromAxisAngle(Vector3.Normalize(new Vector3(-1, 0, 1)), 0), out var orientationA); + QuaternionWide.Broadcast(QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), 0), out var orientationB); + TrianglePairTester.Test(ref a, ref b, ref margin, ref offsetB, ref orientationA, ref orientationB, Vector.Count, out var manifold); + } + { + camera.Position = new Vector3(0, 3, -10); + camera.Yaw = MathF.PI; + camera.Pitch = 0; + + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1), 5, 1), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); + + //var triangleDescription = new StaticDescription + //{ + // Pose = new RigidPose + // { + // Position = new Vector3(2 - 10, 0, 2), + // Orientation = QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 3.2345f) + // }, + // Collidable = new CollidableDescription + // { + // Shape = Simulation.Shapes.Add(new Triangle( + // new Vector3(-3, -0.5f, -3), + // new Vector3(3, 0, -3), + // new Vector3(-3, 0, 3))), + // SpeculativeMargin = 10.1f + // } + //}; + //Simulation.Statics.Add(triangleDescription); + + //var shape = new Triangle(new Vector3(0, 0, 3), new Vector3(0, 0, 0), new Vector3(-3, 3, 0)); + //var bodyDescription = new BodyDescription + //{ + // Collidable = new CollidableDescription { Shape = Simulation.Shapes.Add(shape), SpeculativeMargin = 0.1f }, + // Activity = new BodyActivityDescription { SleepThreshold = -1 }, + // Pose = new RigidPose + // { + // Position = new Vector3(1 - 10, -0.01f, 1), + // Orientation = QuaternionEx.CreateFromAxisAngle(Vector3.Normalize(new Vector3(1, 0, 1)), 0) + // //Orientation = BepuUtilities.Quaternion.Identity + // } + //}; + //shape.ComputeInertia(1, out bodyDescription.LocalInertia); + ////bodyDescription.LocalInertia.InverseInertiaTensor = new Triangular3x3(); + //Simulation.Bodies.Add(bodyDescription); + + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5, 0), Simulation.Shapes.Add(new Box(200, 5, 200)))); + Simulation.Statics.Add(new StaticDescription(new Vector3(10, -2, 30), Simulation.Shapes.Add(new Box(10, 5, 10)))); + + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 2, 0), new BodyInertia { InverseMass = 1 }, new(Simulation.Shapes.Add(new Sphere(1.75f)), 0.1f, 0.1f), -1)); + var capsule = new Capsule(2, 2); + Simulation.Bodies.Add(BodyDescription.CreateDynamic((new Vector3(20, 2, 3), Quaternion.CreateFromYawPitchRoll(0f, 1.745329E-05f, 0f)), capsule.ComputeInertia(1), new(Simulation.Shapes.Add(capsule), 0.1f, 0.1f), -1)); + var testBox = new Box(2, 3, 2); + var testBoxInertia = testBox.ComputeInertia(1); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 2, 6), testBoxInertia, new(Simulation.Shapes.Add(testBox), 10.1f, 10.1f), -1)); + + var cylinder = new Cylinder(1.75f, 0.5f); + var cylinderInertia = cylinder.ComputeInertia(1); + //cylinderInertia.InverseInertiaTensor = default; + Simulation.Bodies.Add(BodyDescription.CreateDynamic((new Vector3(20, 2, 9), Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), MathF.PI / 2f)), cylinderInertia, new(Simulation.Shapes.Add(cylinder), 5, 5), -1)); + + var cylinder2 = new Cylinder(.5f, 0.5f); + var cylinder2Inertia = cylinder2.ComputeInertia(1); + Simulation.Bodies.Add(BodyDescription.CreateDynamic((new Vector3(23, 2, 9), Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), 0)), cylinder2Inertia, new(Simulation.Shapes.Add(cylinder2), 5, 5), -1)); + var points = new QuickList(8, BufferPool); + points.AllocateUnsafely() = new Vector3(0, 0, 0); + points.AllocateUnsafely() = new Vector3(0, 0, 2); + points.AllocateUnsafely() = new Vector3(2, 0, 0); + points.AllocateUnsafely() = new Vector3(2, 0, 2); + points.AllocateUnsafely() = new Vector3(0, 2, 0); + points.AllocateUnsafely() = new Vector3(0, 2, 2); + points.AllocateUnsafely() = new Vector3(2, 2, 0); + points.AllocateUnsafely() = new Vector3(2, 2, 2); + var convexHull = new ConvexHull(points, BufferPool, out _); + var convexHullInertia = convexHull.ComputeInertia(1); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 2, 12), convexHullInertia, new(Simulation.Shapes.Add(convexHull), 0.1f, 0.1f), -1)); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(23, 2, 12), convexHullInertia, new(Simulation.Shapes.Add(convexHull), 0.1f, 0.1f), -1)); + + CompoundBuilder builder = new CompoundBuilder(BufferPool, Simulation.Shapes, 2); + builder.Add(new Box(1, 1, 1), RigidPose.Identity, 1); + builder.Add(new Triangle { A = new(-0.5f, 1, 0), B = new(0.5f, 1, 0), C = new Vector3(0f, 3, -1) }, RigidPose.Identity, 1); + builder.BuildDynamicCompound(out var children, out var compoundInertia); + //compoundInertia.InverseInertiaTensor = default; + var compound = new Compound(children); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 3, 14), compoundInertia, new(Simulation.Shapes.Add(compound), 10.1f, 10.1f), -1)); + { - SphereWide sphere = default; - sphere.Broadcast(new Sphere(0.5f)); - TriangleWide triangle = default; - var a = new Vector3(0, 0, 0); - var b = new Vector3(1, 0, 0); - var c = new Vector3(0, 0, 1); - //var center = (a + b + c) / 3f; - //a -= center; - //b -= center; - //c -= center; - triangle.Broadcast(new Triangle(a, b, c)); - var margin = new Vector(1f); - Vector3Wide.Broadcast(new Vector3(1, -1, 0), out var offsetB); - QuaternionWide.Broadcast(QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), out var orientationB); - SphereTriangleTester.Test(ref sphere, ref triangle, ref margin, ref offsetB, ref orientationB, Vector.Count, out var manifold); - } - { - CapsuleWide capsule = default; - capsule.Broadcast(new Capsule(0.5f, 0.5f)); - TriangleWide triangle = default; - var a = new Vector3(0, 0, 0); - var b = new Vector3(1, 0, 0); - var c = new Vector3(0, 0, 1); - //var center = (a + b + c) / 3f; - //a -= center; - //b -= center; - //c -= center; - triangle.Broadcast(new Triangle(a, b, c)); - var margin = new Vector(2f); - Vector3Wide.Broadcast(new Vector3(-1f, -0.5f, -1f), out var offsetB); - QuaternionWide.Broadcast(QuaternionEx.CreateFromAxisAngle(Vector3.Normalize(new Vector3(-1, 0, 1)), MathHelper.PiOver2), out var orientationA); - QuaternionWide.Broadcast(QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), 0), out var orientationB); - CapsuleTriangleTester.Test(ref capsule, ref triangle, ref margin, ref offsetB, ref orientationA, ref orientationB, Vector.Count, out var manifold); - } - { - BoxWide shape = default; - shape.Broadcast(new Box(1f, 1f, 1f)); - TriangleWide triangle = default; - var a = new Vector3(0, 0, 0); - var b = new Vector3(1, 0, 0); - var c = new Vector3(0, 0, 1); - //var center = (a + b + c) / 3f; - //a -= center; - //b -= center; - //c -= center; - triangle.Broadcast(new Triangle(a, b, c)); - var margin = new Vector(2f); - Vector3Wide.Broadcast(new Vector3(-1f, -0.5f, -1f), out var offsetB); - QuaternionWide.Broadcast(QuaternionEx.CreateFromAxisAngle(Vector3.Normalize(new Vector3(-1, 0, 1)), MathHelper.PiOver2), out var orientationA); - QuaternionWide.Broadcast(QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), 0), out var orientationB); - BoxTriangleTester.Test(ref shape, ref triangle, ref margin, ref offsetB, ref orientationA, ref orientationB, Vector.Count, out var manifold); + var triangles = new QuickList(4, BufferPool); + var v0 = new Vector3(0, 1.75f, 0); + var v1 = new Vector3(8, 1.75f, 0); + var v2 = new Vector3(0, 1.75f, 50); + var v3 = new Vector3(8, 1.75f, 50); + triangles.AllocateUnsafely() = new Triangle { A = v2, B = v0, C = v1 }; + triangles.AllocateUnsafely() = new Triangle { A = v2, B = v1, C = v3 }; + triangles.AllocateUnsafely() = new Triangle { A = v0, B = v2, C = v1 }; + triangles.AllocateUnsafely() = new Triangle { A = v1, B = v2, C = v3 }; + var testMesh = new Mesh(triangles, Vector3.One, BufferPool); + Simulation.Statics.Add(new StaticDescription(new Vector3(30, -2.5f, 0), Simulation.Shapes.Add(testMesh))); + + Simulation.Statics.Add(new StaticDescription(new Vector3(0, -2.5f, 0), Simulation.Shapes.Add(new Triangle(v2, v0, v1)))); } + + + var mesh = DemoMeshHelper.LoadModel(content, BufferPool, "Content\\newt.obj", new Vector3(3)); + var collidable = new CollidableDescription(Simulation.Shapes.Add(mesh), 2f, 2f, ContinuousDetection.Discrete); + var newtInertia = mesh.ComputeClosedInertia(1); + for (int i = 0; i < 5; ++i) { - TriangleWide a = default, b = default; - a.Broadcast(new Triangle(new Vector3(0, 0, 0), new Vector3(1, 0, 0), new Vector3(0, 0, 1))); - b.Broadcast(new Triangle(new Vector3(0, 0, 0), new Vector3(1, 0, 0), new Vector3(0, 0, 1))); - - var margin = new Vector(2f); - Vector3Wide.Broadcast(new Vector3(0, -1, 0), out var offsetB); - QuaternionWide.Broadcast(QuaternionEx.CreateFromAxisAngle(Vector3.Normalize(new Vector3(-1, 0, 1)), 0), out var orientationA); - QuaternionWide.Broadcast(QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), 0), out var orientationB); - TrianglePairTester.Test(ref a, ref b, ref margin, ref offsetB, ref orientationA, ref orientationB, Vector.Count, out var manifold); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(-20, 5 + i * 5, 0), newtInertia, collidable, -1e-2f)); } + { - camera.Position = new Vector3(0, 3, -10); - camera.Yaw = MathF.PI; - camera.Pitch = 0; - - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1), 5, 1), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); - - //var triangleDescription = new StaticDescription - //{ - // Pose = new RigidPose - // { - // Position = new Vector3(2 - 10, 0, 2), - // Orientation = QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 3.2345f) - // }, - // Collidable = new CollidableDescription - // { - // Shape = Simulation.Shapes.Add(new Triangle( - // new Vector3(-3, -0.5f, -3), - // new Vector3(3, 0, -3), - // new Vector3(-3, 0, 3))), - // SpeculativeMargin = 10.1f - // } - //}; - //Simulation.Statics.Add(triangleDescription); - - //var shape = new Triangle(new Vector3(0, 0, 3), new Vector3(0, 0, 0), new Vector3(-3, 3, 0)); - //var bodyDescription = new BodyDescription - //{ - // Collidable = new CollidableDescription { Shape = Simulation.Shapes.Add(shape), SpeculativeMargin = 0.1f }, - // Activity = new BodyActivityDescription { SleepThreshold = -1 }, - // Pose = new RigidPose - // { - // Position = new Vector3(1 - 10, -0.01f, 1), - // Orientation = QuaternionEx.CreateFromAxisAngle(Vector3.Normalize(new Vector3(1, 0, 1)), 0) - // //Orientation = BepuUtilities.Quaternion.Identity - // } - //}; - //shape.ComputeInertia(1, out bodyDescription.LocalInertia); - ////bodyDescription.LocalInertia.InverseInertiaTensor = new Triangular3x3(); - //Simulation.Bodies.Add(bodyDescription); - - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5, 0), Simulation.Shapes.Add(new Box(200, 5, 200)))); - Simulation.Statics.Add(new StaticDescription(new Vector3(10, -2, 30), Simulation.Shapes.Add(new Box(10, 5, 10)))); - - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 2, 0), new BodyInertia { InverseMass = 1 }, new(Simulation.Shapes.Add(new Sphere(1.75f)), 0.1f, 0.1f), -1)); - var capsule = new Capsule(2, 2); - Simulation.Bodies.Add(BodyDescription.CreateDynamic((new Vector3(20, 2, 3), Quaternion.CreateFromYawPitchRoll(0f, 1.745329E-05f, 0f)), capsule.ComputeInertia(1), new(Simulation.Shapes.Add(capsule), 0.1f, 0.1f), -1)); - var testBox = new Box(2, 3, 2); - var testBoxInertia = testBox.ComputeInertia(1); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 2, 6), testBoxInertia, new(Simulation.Shapes.Add(testBox), 10.1f, 10.1f), -1)); - - var cylinder = new Cylinder(1.75f, 0.5f); - var cylinderInertia = cylinder.ComputeInertia(1); - //cylinderInertia.InverseInertiaTensor = default; - Simulation.Bodies.Add(BodyDescription.CreateDynamic((new Vector3(20, 2, 9), Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), MathF.PI / 2f)), cylinderInertia, new(Simulation.Shapes.Add(cylinder), 5, 5), -1)); - - var cylinder2 = new Cylinder(.5f, 0.5f); - var cylinder2Inertia = cylinder2.ComputeInertia(1); - Simulation.Bodies.Add(BodyDescription.CreateDynamic((new Vector3(23, 2, 9), Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), 0)), cylinder2Inertia, new(Simulation.Shapes.Add(cylinder2), 5, 5), -1)); - var points = new QuickList(8, BufferPool); - points.AllocateUnsafely() = new Vector3(0, 0, 0); - points.AllocateUnsafely() = new Vector3(0, 0, 2); - points.AllocateUnsafely() = new Vector3(2, 0, 0); - points.AllocateUnsafely() = new Vector3(2, 0, 2); - points.AllocateUnsafely() = new Vector3(0, 2, 0); - points.AllocateUnsafely() = new Vector3(0, 2, 2); - points.AllocateUnsafely() = new Vector3(2, 2, 0); - points.AllocateUnsafely() = new Vector3(2, 2, 2); - var convexHull = new ConvexHull(points, BufferPool, out _); - var convexHullInertia = convexHull.ComputeInertia(1); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 2, 12), convexHullInertia, new(Simulation.Shapes.Add(convexHull), 0.1f, 0.1f), -1)); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(23, 2, 12), convexHullInertia, new(Simulation.Shapes.Add(convexHull), 0.1f, 0.1f), -1)); - - CompoundBuilder builder = new CompoundBuilder(BufferPool, Simulation.Shapes, 2); - builder.Add(new Box(1, 1, 1), RigidPose.Identity, 1); - builder.Add(new Triangle { A = new(-0.5f, 1, 0), B = new(0.5f, 1, 0), C = new Vector3(0f, 3, -1) }, RigidPose.Identity, 1); - builder.BuildDynamicCompound(out var children, out var compoundInertia); - //compoundInertia.InverseInertiaTensor = default; - var compound = new Compound(children); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(20, 3, 14), compoundInertia, new(Simulation.Shapes.Add(compound), 10.1f, 10.1f), -1)); - - { - var triangles = new QuickList(4, BufferPool); - var v0 = new Vector3(0, 1.75f, 0); - var v1 = new Vector3(8, 1.75f, 0); - var v2 = new Vector3(0, 1.75f, 50); - var v3 = new Vector3(8, 1.75f, 50); - triangles.AllocateUnsafely() = new Triangle { A = v2, B = v0, C = v1 }; - triangles.AllocateUnsafely() = new Triangle { A = v2, B = v1, C = v3 }; - triangles.AllocateUnsafely() = new Triangle { A = v0, B = v2, C = v1 }; - triangles.AllocateUnsafely() = new Triangle { A = v1, B = v2, C = v3 }; - var testMesh = new Mesh(triangles, Vector3.One, BufferPool); - Simulation.Statics.Add(new StaticDescription(new Vector3(30, -2.5f, 0), Simulation.Shapes.Add(testMesh))); - - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -2.5f, 0), Simulation.Shapes.Add(new Triangle(v2, v0, v1)))); - } - - - var mesh = DemoMeshHelper.LoadModel(content, BufferPool, "Content\\newt.obj", new Vector3(3)); - var collidable = new CollidableDescription(Simulation.Shapes.Add(mesh), 2f, 2f, ContinuousDetection.Discrete); - var newtInertia = mesh.ComputeClosedInertia(1); - for (int i = 0; i < 5; ++i) - { - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(-20, 5 + i * 5, 0), newtInertia, collidable, -1e-2f)); - } - - { - var triangles = new QuickList(4, BufferPool); - var v0 = new Vector3(3, 1f, 0); - var v1 = new Vector3(3, 0, 2); - var v2 = new Vector3(0, 1f, 2); - var v3 = new Vector3(2, 2, 1); - triangles.AllocateUnsafely() = new Triangle { A = v0, B = v2, C = v1 }; - triangles.AllocateUnsafely() = new Triangle { A = v1, B = v2, C = v3 }; - var testMesh = new Mesh(triangles, Vector3.One, BufferPool); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(22, -2.5f, 0), new BodyInertia { InverseMass = 1 }, new(Simulation.Shapes.Add(testMesh), 10.1f, 10.1f), -1f)); - } + var triangles = new QuickList(4, BufferPool); + var v0 = new Vector3(3, 1f, 0); + var v1 = new Vector3(3, 0, 2); + var v2 = new Vector3(0, 1f, 2); + var v3 = new Vector3(2, 2, 1); + triangles.AllocateUnsafely() = new Triangle { A = v0, B = v2, C = v1 }; + triangles.AllocateUnsafely() = new Triangle { A = v1, B = v2, C = v3 }; + var testMesh = new Mesh(triangles, Vector3.One, BufferPool); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(22, -2.5f, 0), new BodyInertia { InverseMass = 1 }, new(Simulation.Shapes.Add(testMesh), 10.1f, 10.1f), -1f)); } } + } - public override void Update(Window window, Camera camera, Input input, float dt) - { - if (input.IsDown(OpenTK.Input.Key.P)) - Console.WriteLine("ASDF"); - base.Update(window, camera, input, dt); - } + public override void Update(Window window, Camera camera, Input input, float dt) + { + if (input.IsDown(OpenTK.Input.Key.P)) + Console.WriteLine("ASDF"); + base.Update(window, camera, input, dt); + } - } } diff --git a/Demos/SpecializedTests/VolumeQueryTests.cs b/Demos/SpecializedTests/VolumeQueryTests.cs index f5b2163cc..94c4a9a9e 100644 --- a/Demos/SpecializedTests/VolumeQueryTests.cs +++ b/Demos/SpecializedTests/VolumeQueryTests.cs @@ -14,285 +14,284 @@ using System.Threading; using DemoContentLoader; -namespace Demos.SpecializedTests +namespace Demos.SpecializedTests; + +public class VolumeQueryTests : Demo { - public class VolumeQueryTests : Demo + public struct NoCollisionCallbacks : INarrowPhaseCallbacks { - public struct NoCollisionCallbacks : INarrowPhaseCallbacks + public void Initialize(Simulation simulation) { - public void Initialize(Simulation simulation) - { - } + } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin) - { - return false; - } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin) + { + return false; + } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB) - { - return false; - } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB) + { + return false; + } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold - { - pairMaterial = new PairMaterialProperties(); - return false; - } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold + { + pairMaterial = new PairMaterialProperties(); + return false; + } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold) - { - return false; - } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold) + { + return false; + } - public void Dispose() - { - } + public void Dispose() + { } - public override void Initialize(ContentArchive content, Camera camera) + } + public override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(-20f, 13, -20f); + camera.Yaw = MathHelper.Pi * 3f / 4; + camera.Pitch = MathHelper.Pi * 0.1f; + Simulation = Simulation.Create(BufferPool, new NoCollisionCallbacks(), new DemoPoseIntegratorCallbacks(), new SolveDescription(8, 1)); + + var sphere = new Sphere(0.5f); + var shapeIndex = Simulation.Shapes.Add(sphere); + const int width = 16; + const int height = 16; + const int length = 16; + var spacing = new Vector3(2.01f); + var halfSpacing = spacing / 2; + float randomizationSubset = 0.9f; + var randomizationSpan = (spacing - new Vector3(1)) * randomizationSubset; + var randomizationBase = randomizationSpan * -0.5f; + var random = new Random(5); + for (int i = 0; i < width; ++i) { - camera.Position = new Vector3(-20f, 13, -20f); - camera.Yaw = MathHelper.Pi * 3f / 4; - camera.Pitch = MathHelper.Pi * 0.1f; - Simulation = Simulation.Create(BufferPool, new NoCollisionCallbacks(), new DemoPoseIntegratorCallbacks(), new SolveDescription(8, 1)); - - var sphere = new Sphere(0.5f); - var shapeIndex = Simulation.Shapes.Add(sphere); - const int width = 16; - const int height = 16; - const int length = 16; - var spacing = new Vector3(2.01f); - var halfSpacing = spacing / 2; - float randomizationSubset = 0.9f; - var randomizationSpan = (spacing - new Vector3(1)) * randomizationSubset; - var randomizationBase = randomizationSpan * -0.5f; - var random = new Random(5); - for (int i = 0; i < width; ++i) + for (int j = 0; j < height; ++j) { - for (int j = 0; j < height; ++j) + for (int k = 0; k < length; ++k) { - for (int k = 0; k < length; ++k) - { - var r = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); - var location = spacing * (new Vector3(i, j, k) + new Vector3(-width, -height, -length) * 0.5f) + randomizationBase + r * randomizationSpan; - - Quaternion orientation; - orientation.X = -1 + 2 * random.NextSingle(); - orientation.Y = -1 + 2 * random.NextSingle(); - orientation.Z = -1 + 2 * random.NextSingle(); - orientation.W = 0.01f + random.NextSingle(); - QuaternionEx.Normalize(ref orientation); - - if ((i + j + k) % 2 == 1) - { - var bodyDescription = BodyDescription.CreateKinematic((location, orientation), shapeIndex, -1); - Simulation.Bodies.Add(bodyDescription); - } - else - { - var staticDescription = new StaticDescription(location, orientation, shapeIndex); - Simulation.Statics.Add(staticDescription); - } + var r = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); + var location = spacing * (new Vector3(i, j, k) + new Vector3(-width, -height, -length) * 0.5f) + randomizationBase + r * randomizationSpan; + + Quaternion orientation; + orientation.X = -1 + 2 * random.NextSingle(); + orientation.Y = -1 + 2 * random.NextSingle(); + orientation.Z = -1 + 2 * random.NextSingle(); + orientation.W = 0.01f + random.NextSingle(); + QuaternionEx.Normalize(ref orientation); + if ((i + j + k) % 2 == 1) + { + var bodyDescription = BodyDescription.CreateKinematic((location, orientation), shapeIndex, -1); + Simulation.Bodies.Add(bodyDescription); } + else + { + var staticDescription = new StaticDescription(location, orientation, shapeIndex); + Simulation.Statics.Add(staticDescription); + } + } } + } - int boxCount = 16384; - var randomMin = new Vector3(width, height, length) * spacing * -0.5f; - var randomSpan = randomMin * -2; - queryBoxes = new QuickList(boxCount, BufferPool); - for (int i = 0; i < boxCount; ++i) - { - ref var box = ref queryBoxes.AllocateUnsafely(); - var r = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); - var boxOrigin = randomMin + r * randomSpan; - var boxHalfSize = new Vector3(0.25f + 0.75f * random.NextSingle()); - box.Min = boxOrigin - boxHalfSize; - box.Max = boxOrigin + boxHalfSize; - } + int boxCount = 16384; + var randomMin = new Vector3(width, height, length) * spacing * -0.5f; + var randomSpan = randomMin * -2; + queryBoxes = new QuickList(boxCount, BufferPool); + for (int i = 0; i < boxCount; ++i) + { + ref var box = ref queryBoxes.AllocateUnsafely(); + var r = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); + var boxOrigin = randomMin + r * randomSpan; + var boxHalfSize = new Vector3(0.25f + 0.75f * random.NextSingle()); + box.Min = boxOrigin - boxHalfSize; + box.Max = boxOrigin + boxHalfSize; + } - algorithms = new BoxQueryAlgorithm[1]; - algorithms[0] = new BoxQueryAlgorithm("1", BufferPool, Worker1); + algorithms = new BoxQueryAlgorithm[1]; + algorithms[0] = new BoxQueryAlgorithm("1", BufferPool, Worker1); - BufferPool.Take(Environment.ProcessorCount * 2, out jobs); - } + BufferPool.Take(Environment.ProcessorCount * 2, out jobs); + } - QuickList queryBoxes; + QuickList queryBoxes; - unsafe class BoxQueryAlgorithm + unsafe class BoxQueryAlgorithm + { + public string Name; + public int IntersectionCount; + public TimingsRingBuffer Timings; + + Func worker; + Action internalWorker; + public int JobIndex; + + public BoxQueryAlgorithm(string name, BufferPool pool, Func worker, int timingSampleCount = 16) { - public string Name; - public int IntersectionCount; - public TimingsRingBuffer Timings; + Name = name; + Timings = new TimingsRingBuffer(timingSampleCount, pool); + this.worker = worker; + internalWorker = ExecuteWorker; + } - Func worker; - Action internalWorker; - public int JobIndex; + void ExecuteWorker(int workerIndex) + { + var intersectionCount = worker(workerIndex, this); + Interlocked.Add(ref IntersectionCount, intersectionCount); + } - public BoxQueryAlgorithm(string name, BufferPool pool, Func worker, int timingSampleCount = 16) + public void Execute(ref QuickList boxes, IThreadDispatcher dispatcher) + { + CacheBlaster.Blast(); + JobIndex = -1; + IntersectionCount = 0; + var start = Stopwatch.GetTimestamp(); + if (dispatcher != null) { - Name = name; - Timings = new TimingsRingBuffer(timingSampleCount, pool); - this.worker = worker; - internalWorker = ExecuteWorker; + dispatcher.DispatchWorkers(internalWorker); } - - void ExecuteWorker(int workerIndex) + else { - var intersectionCount = worker(workerIndex, this); - Interlocked.Add(ref IntersectionCount, intersectionCount); - } - - public void Execute(ref QuickList boxes, IThreadDispatcher dispatcher) - { - CacheBlaster.Blast(); - JobIndex = -1; - IntersectionCount = 0; - var start = Stopwatch.GetTimestamp(); - if (dispatcher != null) - { - dispatcher.DispatchWorkers(internalWorker); - } - else - { - internalWorker(0); - } - var stop = Stopwatch.GetTimestamp(); - Timings.Add((stop - start) / (double)Stopwatch.Frequency); + internalWorker(0); } + var stop = Stopwatch.GetTimestamp(); + Timings.Add((stop - start) / (double)Stopwatch.Frequency); } + } - unsafe int Worker1(int workerIndex, BoxQueryAlgorithm algorithm) + unsafe int Worker1(int workerIndex, BoxQueryAlgorithm algorithm) + { + int intersectionCount = 0; + var hitHandler = new HitHandler { IntersectionCount = &intersectionCount }; + int claimedIndex; + while ((claimedIndex = Interlocked.Increment(ref algorithm.JobIndex)) < jobs.Length) { - int intersectionCount = 0; - var hitHandler = new HitHandler { IntersectionCount = &intersectionCount }; - int claimedIndex; - while ((claimedIndex = Interlocked.Increment(ref algorithm.JobIndex)) < jobs.Length) + ref var job = ref jobs[claimedIndex]; + for (int i = job.Start; i < job.End; ++i) { - ref var job = ref jobs[claimedIndex]; - for (int i = job.Start; i < job.End; ++i) - { - ref var box = ref queryBoxes[i]; - Simulation.BroadPhase.GetOverlaps(box, ref hitHandler); - } + ref var box = ref queryBoxes[i]; + Simulation.BroadPhase.GetOverlaps(box, ref hitHandler); } - return intersectionCount; } + return intersectionCount; + } - BoxQueryAlgorithm[] algorithms; - - struct QueryJob - { - public int Start; - public int End; - } - Buffer jobs; - + BoxQueryAlgorithm[] algorithms; - unsafe struct HitHandler : IBreakableForEach - { - public int* IntersectionCount; + struct QueryJob + { + public int Start; + public int End; + } + Buffer jobs; - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public bool LoopBody(CollidableReference collidable) - { - ++*IntersectionCount; - return true; - } - } - bool shouldUseMultithreading = true; + unsafe struct HitHandler : IBreakableForEach + { + public int* IntersectionCount; - public override void Update(Window window, Camera camera, Input input, float dt) + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool LoopBody(CollidableReference collidable) { - base.Update(window, camera, input, dt); + ++*IntersectionCount; + return true; + } + } - if (input.WasPushed(OpenTK.Input.Key.T)) - { - shouldUseMultithreading = !shouldUseMultithreading; - } - - var raysPerJobBase = queryBoxes.Count / jobs.Length; - var remainder = queryBoxes.Count - raysPerJobBase * jobs.Length; - var previousJobEnd = 0; - for (int i = 0; i < jobs.Length; ++i) - { - int raysInJob = i < remainder ? raysPerJobBase + 1 : raysPerJobBase; - ref var job = ref jobs[i]; - job.Start = previousJobEnd; - job.End = previousJobEnd = previousJobEnd + raysInJob; - } + bool shouldUseMultithreading = true; + public override void Update(Window window, Camera camera, Input input, float dt) + { + base.Update(window, camera, input, dt); - for (int i = 0; i < algorithms.Length; ++i) - { - algorithms[i].Execute(ref queryBoxes, shouldUseMultithreading ? ThreadDispatcher : null); - } - for (int i = 1; i < algorithms.Length; ++i) - { - Debug.Assert(algorithms[i].IntersectionCount == algorithms[0].IntersectionCount); - } + if (input.WasPushed(OpenTK.Input.Key.T)) + { + shouldUseMultithreading = !shouldUseMultithreading; + } + var raysPerJobBase = queryBoxes.Count / jobs.Length; + var remainder = queryBoxes.Count - raysPerJobBase * jobs.Length; + var previousJobEnd = 0; + for (int i = 0; i < jobs.Length; ++i) + { + int raysInJob = i < remainder ? raysPerJobBase + 1 : raysPerJobBase; + ref var job = ref jobs[i]; + job.Start = previousJobEnd; + job.End = previousJobEnd = previousJobEnd + raysInJob; } - void WriteResults(string name, double time, double baseline, float y, TextBatcher batcher, TextBuilder text, Font font) + for (int i = 0; i < algorithms.Length; ++i) { - batcher.Write( - text.Clear().Append(name).Append(":"), - new Vector2(32, y), 16, new Vector3(1), font); - batcher.Write( - text.Clear().Append(time * 1e6, 2), - new Vector2(128, y), 16, new Vector3(1), font); - batcher.Write( - text.Clear().Append(queryBoxes.Count / time, 0), - new Vector2(224, y), 16, new Vector3(1), font); - batcher.Write( - text.Clear().Append(baseline / time, 2), - new Vector2(350, y), 16, new Vector3(1), font); + algorithms[i].Execute(ref queryBoxes, shouldUseMultithreading ? ThreadDispatcher : null); } - - void WriteControl(string name, TextBuilder control, float y, TextBatcher batcher, Font font) + for (int i = 1; i < algorithms.Length; ++i) { - batcher.Write(control, - new Vector2(176, y), 16, new Vector3(1), font); - batcher.Write(control.Clear().Append(name).Append(":"), - new Vector2(32, y), 16, new Vector3(1), font); + Debug.Assert(algorithms[i].IntersectionCount == algorithms[0].IntersectionCount); } - public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) - { - text.Clear().Append("Multithreading: ").Append(shouldUseMultithreading ? "On" : "Off"); - renderer.TextBatcher.Write(text, new Vector2(32, renderer.Surface.Resolution.Y - 128), 16, new Vector3(1), font); - renderer.TextBatcher.Write(text.Clear().Append("Demo specific controls:"), new Vector2(32, renderer.Surface.Resolution.Y - 112), 16, new Vector3(1), font); - WriteControl("Toggle threading", text.Clear().Append("T"), renderer.Surface.Resolution.Y - 96, renderer.TextBatcher, font); - - renderer.TextBatcher.Write(text.Clear().Append("Box count: ").Append(queryBoxes.Count), new Vector2(32, renderer.Surface.Resolution.Y - 80), 16, new Vector3(1), font); - renderer.TextBatcher.Write(text.Clear().Append("Time (us):"), new Vector2(128, renderer.Surface.Resolution.Y - 64), 16, new Vector3(1), font); - renderer.TextBatcher.Write(text.Clear().Append("Boxes per second:"), new Vector2(224, renderer.Surface.Resolution.Y - 64), 16, new Vector3(1), font); - renderer.TextBatcher.Write(text.Clear().Append("Relative speed:"), new Vector2(350, renderer.Surface.Resolution.Y - 64), 16, new Vector3(1), font); - - var baseStats = algorithms[0].Timings.ComputeStats(); - var baseHeight = 48; - for (int i = 0; i < algorithms.Length; ++i) - { - var stats = algorithms[i].Timings.ComputeStats(); - WriteResults(algorithms[i].Name, stats.Average, baseStats.Average, renderer.Surface.Resolution.Y - (baseHeight - 16 * i), renderer.TextBatcher, text, font); - } + } + + + void WriteResults(string name, double time, double baseline, float y, TextBatcher batcher, TextBuilder text, Font font) + { + batcher.Write( + text.Clear().Append(name).Append(":"), + new Vector2(32, y), 16, new Vector3(1), font); + batcher.Write( + text.Clear().Append(time * 1e6, 2), + new Vector2(128, y), 16, new Vector3(1), font); + batcher.Write( + text.Clear().Append(queryBoxes.Count / time, 0), + new Vector2(224, y), 16, new Vector3(1), font); + batcher.Write( + text.Clear().Append(baseline / time, 2), + new Vector2(350, y), 16, new Vector3(1), font); + } - base.Render(renderer, camera, input, text, font); + void WriteControl(string name, TextBuilder control, float y, TextBatcher batcher, Font font) + { + batcher.Write(control, + new Vector2(176, y), 16, new Vector3(1), font); + batcher.Write(control.Clear().Append(name).Append(":"), + new Vector2(32, y), 16, new Vector3(1), font); + } + + public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + { + text.Clear().Append("Multithreading: ").Append(shouldUseMultithreading ? "On" : "Off"); + renderer.TextBatcher.Write(text, new Vector2(32, renderer.Surface.Resolution.Y - 128), 16, new Vector3(1), font); + renderer.TextBatcher.Write(text.Clear().Append("Demo specific controls:"), new Vector2(32, renderer.Surface.Resolution.Y - 112), 16, new Vector3(1), font); + WriteControl("Toggle threading", text.Clear().Append("T"), renderer.Surface.Resolution.Y - 96, renderer.TextBatcher, font); + + renderer.TextBatcher.Write(text.Clear().Append("Box count: ").Append(queryBoxes.Count), new Vector2(32, renderer.Surface.Resolution.Y - 80), 16, new Vector3(1), font); + renderer.TextBatcher.Write(text.Clear().Append("Time (us):"), new Vector2(128, renderer.Surface.Resolution.Y - 64), 16, new Vector3(1), font); + renderer.TextBatcher.Write(text.Clear().Append("Boxes per second:"), new Vector2(224, renderer.Surface.Resolution.Y - 64), 16, new Vector3(1), font); + renderer.TextBatcher.Write(text.Clear().Append("Relative speed:"), new Vector2(350, renderer.Surface.Resolution.Y - 64), 16, new Vector3(1), font); + + var baseStats = algorithms[0].Timings.ComputeStats(); + var baseHeight = 48; + for (int i = 0; i < algorithms.Length; ++i) + { + var stats = algorithms[i].Timings.ComputeStats(); + WriteResults(algorithms[i].Name, stats.Average, baseStats.Average, renderer.Surface.Resolution.Y - (baseHeight - 16 * i), renderer.TextBatcher, text, font); } + base.Render(renderer, camera, input, text, font); } + } diff --git a/Demos/TimingsRingBuffer.cs b/Demos/TimingsRingBuffer.cs index 4ae2fed57..a1ca23567 100644 --- a/Demos/TimingsRingBuffer.cs +++ b/Demos/TimingsRingBuffer.cs @@ -3,78 +3,77 @@ using Demos.UI; using System; -namespace Demos +namespace Demos; + +public class TimingsRingBuffer : IDataSeries, IDisposable { - public class TimingsRingBuffer : IDataSeries, IDisposable - { - QuickQueue queue; - BufferPool pool; + QuickQueue queue; + BufferPool pool; - /// - /// Gets or sets the maximum number of time measurements that can be held by the ring buffer. - /// - public int Capacity + /// + /// Gets or sets the maximum number of time measurements that can be held by the ring buffer. + /// + public int Capacity + { + get { return queue.Span.Length; } + set { - get { return queue.Span.Length; } - set + if (value <= 0) + throw new ArgumentException("Capacity must be positive."); + if (Capacity != value) { - if (value <= 0) - throw new ArgumentException("Capacity must be positive."); - if (Capacity != value) - { - queue.Resize(value, pool); - } + queue.Resize(value, pool); } } - public TimingsRingBuffer(int maximumCapacity, BufferPool pool) - { - if(maximumCapacity <= 0) - throw new ArgumentException("Capacity must be positive."); - this.pool = pool; - queue = new QuickQueue(maximumCapacity, pool); - } + } + public TimingsRingBuffer(int maximumCapacity, BufferPool pool) + { + if(maximumCapacity <= 0) + throw new ArgumentException("Capacity must be positive."); + this.pool = pool; + queue = new QuickQueue(maximumCapacity, pool); + } - public void Add(double time) + public void Add(double time) + { + if(queue.Count == Capacity) { - if(queue.Count == Capacity) - { - queue.Dequeue(); - } - queue.EnqueueUnsafely(time); + queue.Dequeue(); } + queue.EnqueueUnsafely(time); + } - public double this[int index] => queue[index]; + public double this[int index] => queue[index]; - public int Start => 0; + public int Start => 0; - public int End => queue.Count; + public int End => queue.Count; - public TimelineStats ComputeStats() + public TimelineStats ComputeStats() + { + TimelineStats stats; + stats.Total = 0.0; + var sumOfSquares = 0.0; + stats.Min = double.MaxValue; + stats.Max = double.MinValue; + for (int i = 0; i < queue.Count; ++i) { - TimelineStats stats; - stats.Total = 0.0; - var sumOfSquares = 0.0; - stats.Min = double.MaxValue; - stats.Max = double.MinValue; - for (int i = 0; i < queue.Count; ++i) - { - var time = queue[i]; - stats.Total += time; - sumOfSquares += time * time; - if (time < stats.Min) - stats.Min = time; - if (time > stats.Max) - stats.Max = time; - } - stats.Average = stats.Total / queue.Count; - stats.StdDev = Math.Sqrt(Math.Max(0, sumOfSquares / queue.Count - stats.Average * stats.Average)); - return stats; + var time = queue[i]; + stats.Total += time; + sumOfSquares += time * time; + if (time < stats.Min) + stats.Min = time; + if (time > stats.Max) + stats.Max = time; } + stats.Average = stats.Total / queue.Count; + stats.StdDev = Math.Sqrt(Math.Max(0, sumOfSquares / queue.Count - stats.Average * stats.Average)); + return stats; + } - public void Dispose() - { - queue.Dispose(pool); - } + public void Dispose() + { + queue.Dispose(pool); } } diff --git a/Demos/UI/DemoSwapper.cs b/Demos/UI/DemoSwapper.cs index 0a974be45..ca2ba195f 100644 --- a/Demos/UI/DemoSwapper.cs +++ b/Demos/UI/DemoSwapper.cs @@ -3,80 +3,79 @@ using System; using System.Numerics; -namespace Demos.UI +namespace Demos.UI; + +struct DemoSwapper { - struct DemoSwapper - { - public int TargetDemoIndex; - bool TrackingInput; + public int TargetDemoIndex; + bool TrackingInput; - public void CheckForDemoSwap(DemoHarness harness) + public void CheckForDemoSwap(DemoHarness harness) + { + if (harness.controls.ChangeDemo.WasTriggered(harness.loop.Input)) { - if (harness.controls.ChangeDemo.WasTriggered(harness.loop.Input)) - { - TrackingInput = !TrackingInput; - TargetDemoIndex = -1; - } + TrackingInput = !TrackingInput; + TargetDemoIndex = -1; + } - if (TrackingInput) + if (TrackingInput) + { + for (int i = 0; i < harness.loop.Input.TypedCharacters.Count; ++i) { - for (int i = 0; i < harness.loop.Input.TypedCharacters.Count; ++i) + var character = harness.loop.Input.TypedCharacters[i]; + if (character == '\b') { - var character = harness.loop.Input.TypedCharacters[i]; - if (character == '\b') - { - //Backspace! - if (TargetDemoIndex >= 10) - TargetDemoIndex /= 10; - else - TargetDemoIndex = -1; - } + //Backspace! + if (TargetDemoIndex >= 10) + TargetDemoIndex /= 10; else + TargetDemoIndex = -1; + } + else + { + if (TargetDemoIndex < harness.demoSet.Count) { - if (TargetDemoIndex < harness.demoSet.Count) + var digit = character - '0'; + if (digit >= 0 && digit <= 9) { - var digit = character - '0'; - if (digit >= 0 && digit <= 9) - { - TargetDemoIndex = Math.Max(0, TargetDemoIndex) * 10 + digit; - } + TargetDemoIndex = Math.Max(0, TargetDemoIndex) * 10 + digit; } } } - - if (harness.loop.Input.WasPushed(OpenTK.Input.Key.Enter)) - { - //Done entering the index. Swap the demo if needed. - TrackingInput = false; - harness.TryChangeToDemo(TargetDemoIndex); - } } + if (harness.loop.Input.WasPushed(OpenTK.Input.Key.Enter)) + { + //Done entering the index. Swap the demo if needed. + TrackingInput = false; + harness.TryChangeToDemo(TargetDemoIndex); + } } - public void Draw(TextBuilder text, TextBatcher textBatcher, DemoSet demoSet, Vector2 position, float textHeight, Vector3 textColor, Font font) + } + + public void Draw(TextBuilder text, TextBatcher textBatcher, DemoSet demoSet, Vector2 position, float textHeight, Vector3 textColor, Font font) + { + if (TrackingInput) { - if (TrackingInput) - { - text.Clear().Append("Swap demo to: "); - if (TargetDemoIndex >= 0) - text.Append(TargetDemoIndex); - else - text.Append("_"); - textBatcher.Write(text, position, textHeight, textColor, font); + text.Clear().Append("Swap demo to: "); + if (TargetDemoIndex >= 0) + text.Append(TargetDemoIndex); + else + text.Append("_"); + textBatcher.Write(text, position, textHeight, textColor, font); - var lineSpacing = textHeight * 1.1f; - position.Y += textHeight * 0.5f; - textHeight *= 0.8f; - for (int i = 0; i < demoSet.Count; ++i) - { - position.Y += lineSpacing; - text.Clear().Append(demoSet.GetName(i)); - textBatcher.Write(text.Clear().Append(i).Append(": ").Append(demoSet.GetName(i)), position, textHeight, textColor, font); - } + var lineSpacing = textHeight * 1.1f; + position.Y += textHeight * 0.5f; + textHeight *= 0.8f; + for (int i = 0; i < demoSet.Count; ++i) + { + position.Y += lineSpacing; + text.Clear().Append(demoSet.GetName(i)); + textBatcher.Write(text.Clear().Append(i).Append(": ").Append(demoSet.GetName(i)), position, textHeight, textColor, font); } - } } + } diff --git a/Demos/UI/Graph.cs b/Demos/UI/Graph.cs index 352bce8ab..6d3ab468d 100644 --- a/Demos/UI/Graph.cs +++ b/Demos/UI/Graph.cs @@ -4,346 +4,345 @@ using System.Collections.Generic; using System.Numerics; -namespace Demos.UI +namespace Demos.UI; + +public interface IDataSeries { - public interface IDataSeries - { - int Start { get; } - int End { get; } - double this[int index] { get; } - } + int Start { get; } + int End { get; } + double this[int index] { get; } +} - public struct GraphDescription - { - /// - /// Minimum location of the graph body in pixels, not including the interval labels. - /// - public Vector2 BodyMinimum; - /// - /// Span of the graph body in pixels, not including interval labels. - /// - public Vector2 BodySpan; - public Vector3 BodyLineColor; - public float AxisLabelHeight; - public float AxisLineRadius; - public string HorizontalAxisLabel; - public string VerticalAxisLabel; - public int VerticalIntervalLabelRounding; - public float VerticalIntervalValueScale; - public float BackgroundLineRadius; - public float IntervalTextHeight; - public float IntervalTickRadius; - /// - /// The length of a tick mark line, measured from the axis. - /// - public float IntervalTickLength; - /// - /// Number of interval ticks along the horizontal axis, not including the start and end ticks. - /// - public int TargetHorizontalTickCount; - /// - /// Number of interval ticks along the vertical axis, not including the min and max ticks. - /// - public int TargetVerticalTickCount; - public float HorizontalTickTextPadding; - public float VerticalTickTextPadding; +public struct GraphDescription +{ + /// + /// Minimum location of the graph body in pixels, not including the interval labels. + /// + public Vector2 BodyMinimum; + /// + /// Span of the graph body in pixels, not including interval labels. + /// + public Vector2 BodySpan; + public Vector3 BodyLineColor; + public float AxisLabelHeight; + public float AxisLineRadius; + public string HorizontalAxisLabel; + public string VerticalAxisLabel; + public int VerticalIntervalLabelRounding; + public float VerticalIntervalValueScale; + public float BackgroundLineRadius; + public float IntervalTextHeight; + public float IntervalTickRadius; + /// + /// The length of a tick mark line, measured from the axis. + /// + public float IntervalTickLength; + /// + /// Number of interval ticks along the horizontal axis, not including the start and end ticks. + /// + public int TargetHorizontalTickCount; + /// + /// Number of interval ticks along the vertical axis, not including the min and max ticks. + /// + public int TargetVerticalTickCount; + public float HorizontalTickTextPadding; + public float VerticalTickTextPadding; - /// - /// Minimum location of the legend in pixels. - /// - public Vector2 LegendMinimum; - public float LegendNameHeight; - public float LegendLineLength; + /// + /// Minimum location of the legend in pixels. + /// + public Vector2 LegendMinimum; + public float LegendNameHeight; + public float LegendLineLength; - public Vector3 TextColor; - public Font Font; + public Vector3 TextColor; + public Font Font; - public float LineSpacingMultiplier; - public bool ForceVerticalAxisMinimumToZero; - } + public float LineSpacingMultiplier; + public bool ForceVerticalAxisMinimumToZero; +} - public class Graph +public class Graph +{ + public struct Series { - public struct Series - { - //The use of a string here blocks the use of unmanaged storage. Not a big deal; drawing a Graph isn't exactly performance critical. - public string Name; - public Vector3 LineColor; - public float LineRadius; - public IDataSeries Data; - } - List graphSeries; + //The use of a string here blocks the use of unmanaged storage. Not a big deal; drawing a Graph isn't exactly performance critical. + public string Name; + public Vector3 LineColor; + public float LineRadius; + public IDataSeries Data; + } + List graphSeries; - GraphDescription description; + GraphDescription description; - public ref GraphDescription Description + public ref GraphDescription Description + { + get { - get - { - return ref description; - } + return ref description; } + } - public Graph(GraphDescription description, int initialSeriesCapacity = 8) - { - Description = description; - if (initialSeriesCapacity <= 0) - throw new ArgumentException("Capacity must be positive."); - graphSeries = new List(initialSeriesCapacity); - } + public Graph(GraphDescription description, int initialSeriesCapacity = 8) + { + Description = description; + if (initialSeriesCapacity <= 0) + throw new ArgumentException("Capacity must be positive."); + graphSeries = new List(initialSeriesCapacity); + } - public int IndexOf(string name) - { - for (int i = graphSeries.Count - 1; i >= 0; --i) - { - if (graphSeries[i].Name == name) - return i; - } - return -1; - } - public int IndexOf(IDataSeries data) - { - for (int i = graphSeries.Count - 1; i >= 0; --i) - { - if (graphSeries[i].Data == data) - return i; - } - return -1; - } - public Series GetSeries(string name) + public int IndexOf(string name) + { + for (int i = graphSeries.Count - 1; i >= 0; --i) { - var index = IndexOf(name); - if (index >= 0) - return graphSeries[index]; - throw new ArgumentException("No series with the given name exists within the graph."); + if (graphSeries[i].Name == name) + return i; } - public Series GetSeries(IDataSeries data) + return -1; + } + public int IndexOf(IDataSeries data) + { + for (int i = graphSeries.Count - 1; i >= 0; --i) { - var index = IndexOf(data); - if (index >= 0) - return graphSeries[index]; - throw new ArgumentException("No series with the given data exists within the graph."); + if (graphSeries[i].Data == data) + return i; } + return -1; + } + public Series GetSeries(string name) + { + var index = IndexOf(name); + if (index >= 0) + return graphSeries[index]; + throw new ArgumentException("No series with the given name exists within the graph."); + } + public Series GetSeries(IDataSeries data) + { + var index = IndexOf(data); + if (index >= 0) + return graphSeries[index]; + throw new ArgumentException("No series with the given data exists within the graph."); + } - public void AddSeries(string name, Vector3 lineColor, float lineRadius, IDataSeries series) - { - graphSeries.Add(new Series { Name = name, Data = series, LineRadius = lineRadius, LineColor = lineColor }); - } + public void AddSeries(string name, Vector3 lineColor, float lineRadius, IDataSeries series) + { + graphSeries.Add(new Series { Name = name, Data = series, LineRadius = lineRadius, LineColor = lineColor }); + } - public void RemoveSeries(string name) - { - var index = IndexOf(name); - if (index >= 0) - graphSeries.RemoveAt(index); - else - throw new ArgumentException("No series with the given name exists within the graph."); - } - public void RemoveSeries(IDataSeries data) - { - var index = IndexOf(data); - if (index >= 0) - graphSeries.RemoveAt(index); - else - throw new ArgumentException("No series with the given data exists within the graph."); - } + public void RemoveSeries(string name) + { + var index = IndexOf(name); + if (index >= 0) + graphSeries.RemoveAt(index); + else + throw new ArgumentException("No series with the given name exists within the graph."); + } + public void RemoveSeries(IDataSeries data) + { + var index = IndexOf(data); + if (index >= 0) + graphSeries.RemoveAt(index); + else + throw new ArgumentException("No series with the given data exists within the graph."); + } - public void ClearSeries() - { - graphSeries.Clear(); - } + public void ClearSeries() + { + graphSeries.Clear(); + } - public void Draw(TextBuilder characters, UILineBatcher lines, TextBatcher text) + public void Draw(TextBuilder characters, UILineBatcher lines, TextBatcher text) + { + //Collect information to define data window ranges. + int minX = int.MaxValue; + int maxX = int.MinValue; + var minY = double.MaxValue; + var maxY = double.MinValue; + for (int i = 0; i < graphSeries.Count; ++i) { - //Collect information to define data window ranges. - int minX = int.MaxValue; - int maxX = int.MinValue; - var minY = double.MaxValue; - var maxY = double.MinValue; - for (int i = 0; i < graphSeries.Count; ++i) + var data = graphSeries[i].Data; + if (minX > data.Start) { - var data = graphSeries[i].Data; - if (minX > data.Start) - { - minX = data.Start; - } - if (maxX < data.End) + minX = data.Start; + } + if (maxX < data.End) + { + maxX = data.End; + } + for (int j = data.Start; j < data.End; ++j) + { + var value = data[j]; + if (minY > value) { - maxX = data.End; + minY = value; } - for (int j = data.Start; j < data.End; ++j) + if (maxY < value) { - var value = data[j]; - if (minY > value) - { - minY = value; - } - if (maxY < value) - { - maxY = value; - } + maxY = value; } } - //If no data series contain values, then just use a default size. - if (minY == float.MinValue) - { - minY = 0; + } + //If no data series contain values, then just use a default size. + if (minY == float.MinValue) + { + minY = 0; + maxY = 1; + } + //You could make use of this earlier to avoid comparisons but it doesn't really matter! + if (description.ForceVerticalAxisMinimumToZero) + { + minY = 0; + if (maxY < 0) maxY = 1; - } - //You could make use of this earlier to avoid comparisons but it doesn't really matter! - if (description.ForceVerticalAxisMinimumToZero) - { - minY = 0; - if (maxY < 0) - maxY = 1; - } + } - //Calculate the data span that takes into account rounding. We want intervals to be evenly spaced, but also to match nicely rounded numbers. - //That means the span must be equal to some rounded number multiplied by the number of intervals. - var yDataSpan = maxY - minY; - var yIntervalCount = description.TargetVerticalTickCount + 1; - var rawIntervalLength = yDataSpan / yIntervalCount; - var scale = (int)Math.Log10(rawIntervalLength); - var withinScale = rawIntervalLength * Math.Pow(0.1, scale); - yDataSpan = yIntervalCount * Math.Pow(10, scale) * (withinScale < 0.2 ? 0.2 : withinScale < 0.5 ? 0.5 : 1); // 1, 2 or 5 within the power of 10 + //Calculate the data span that takes into account rounding. We want intervals to be evenly spaced, but also to match nicely rounded numbers. + //That means the span must be equal to some rounded number multiplied by the number of intervals. + var yDataSpan = maxY - minY; + var yIntervalCount = description.TargetVerticalTickCount + 1; + var rawIntervalLength = yDataSpan / yIntervalCount; + var scale = (int)Math.Log10(rawIntervalLength); + var withinScale = rawIntervalLength * Math.Pow(0.1, scale); + yDataSpan = yIntervalCount * Math.Pow(10, scale) * (withinScale < 0.2 ? 0.2 : withinScale < 0.5 ? 0.5 : 1); // 1, 2 or 5 within the power of 10 - //Draw the graph body axes. - var lowerLeft = description.BodyMinimum + new Vector2(0, description.BodySpan.Y); - var upperRight = description.BodyMinimum + new Vector2(description.BodySpan.X, 0); - var lowerRight = description.BodyMinimum + description.BodySpan; - lines.Draw(description.BodyMinimum, lowerLeft, description.AxisLineRadius, description.BodyLineColor); - lines.Draw(lowerLeft, lowerRight, description.AxisLineRadius, description.BodyLineColor); + //Draw the graph body axes. + var lowerLeft = description.BodyMinimum + new Vector2(0, description.BodySpan.Y); + var upperRight = description.BodyMinimum + new Vector2(description.BodySpan.X, 0); + var lowerRight = description.BodyMinimum + description.BodySpan; + lines.Draw(description.BodyMinimum, lowerLeft, description.AxisLineRadius, description.BodyLineColor); + lines.Draw(lowerLeft, lowerRight, description.AxisLineRadius, description.BodyLineColor); - //Draw axis labels. - characters.Clear().Append(description.HorizontalAxisLabel); - var baseAxisLabelDistance = description.IntervalTickLength + description.IntervalTextHeight * description.LineSpacingMultiplier; - var verticalAxisLabelDistance = baseAxisLabelDistance + 2 * description.VerticalTickTextPadding; - var horizontalAxisLabelDistance = baseAxisLabelDistance + 2 * description.HorizontalTickTextPadding + description.AxisLabelHeight * description.LineSpacingMultiplier; - text.Write(characters, - lowerLeft + - new Vector2((description.BodySpan.X - GlyphBatch.MeasureLength(characters, description.Font, description.AxisLabelHeight)) * 0.5f, horizontalAxisLabelDistance), - description.AxisLabelHeight, description.TextColor, description.Font); - characters.Clear().Append(description.VerticalAxisLabel); - text.Write(characters, - description.BodyMinimum + - new Vector2(-verticalAxisLabelDistance, (description.BodySpan.Y + GlyphBatch.MeasureLength(characters, description.Font, description.AxisLabelHeight)) * 0.5f), - description.AxisLabelHeight, new Vector2(0, -1), description.TextColor, description.Font); + //Draw axis labels. + characters.Clear().Append(description.HorizontalAxisLabel); + var baseAxisLabelDistance = description.IntervalTickLength + description.IntervalTextHeight * description.LineSpacingMultiplier; + var verticalAxisLabelDistance = baseAxisLabelDistance + 2 * description.VerticalTickTextPadding; + var horizontalAxisLabelDistance = baseAxisLabelDistance + 2 * description.HorizontalTickTextPadding + description.AxisLabelHeight * description.LineSpacingMultiplier; + text.Write(characters, + lowerLeft + + new Vector2((description.BodySpan.X - GlyphBatch.MeasureLength(characters, description.Font, description.AxisLabelHeight)) * 0.5f, horizontalAxisLabelDistance), + description.AxisLabelHeight, description.TextColor, description.Font); + characters.Clear().Append(description.VerticalAxisLabel); + text.Write(characters, + description.BodyMinimum + + new Vector2(-verticalAxisLabelDistance, (description.BodySpan.Y + GlyphBatch.MeasureLength(characters, description.Font, description.AxisLabelHeight)) * 0.5f), + description.AxisLabelHeight, new Vector2(0, -1), description.TextColor, description.Font); - //Position tickmarks, tick labels, and background lines along the axes. + //Position tickmarks, tick labels, and background lines along the axes. + { + var xDataIntervalSize = (maxX - minX) / (description.TargetHorizontalTickCount + 1f); + var previousTickValue = int.MinValue; + float valueToPixels = description.BodySpan.X / (maxX - minX); + for (int i = 0; i < description.TargetHorizontalTickCount + 2; ++i) { - var xDataIntervalSize = (maxX - minX) / (description.TargetHorizontalTickCount + 1f); - var previousTickValue = int.MinValue; - float valueToPixels = description.BodySpan.X / (maxX - minX); - for (int i = 0; i < description.TargetHorizontalTickCount + 2; ++i) + //Round pen offset such that the data tick lands on an integer. + var valueAtTick = i * xDataIntervalSize; + var tickValue = (int)Math.Round(valueAtTick); + if (tickValue == previousTickValue) { - //Round pen offset such that the data tick lands on an integer. - var valueAtTick = i * xDataIntervalSize; - var tickValue = (int)Math.Round(valueAtTick); - if (tickValue == previousTickValue) - { - //Don't bother creating redundant ticks. - continue; - } - previousTickValue = tickValue; - - var penPosition = lowerLeft + new Vector2(tickValue * valueToPixels, 0); - var tickEnd = penPosition + new Vector2(0, description.IntervalTickLength); - var backgroundEnd = penPosition - new Vector2(0, description.BodySpan.Y); - lines.Draw(penPosition, tickEnd, description.IntervalTickRadius, description.BodyLineColor); - lines.Draw(penPosition, backgroundEnd, description.BackgroundLineRadius, description.BodyLineColor); - characters.Clear().Append(tickValue); - text.Write(characters, tickEnd + - new Vector2(GlyphBatch.MeasureLength(characters, description.Font, description.IntervalTextHeight) * -0.5f, - description.HorizontalTickTextPadding + description.IntervalTextHeight * description.LineSpacingMultiplier), - description.IntervalTextHeight, description.TextColor, description.Font); + //Don't bother creating redundant ticks. + continue; } + previousTickValue = tickValue; + + var penPosition = lowerLeft + new Vector2(tickValue * valueToPixels, 0); + var tickEnd = penPosition + new Vector2(0, description.IntervalTickLength); + var backgroundEnd = penPosition - new Vector2(0, description.BodySpan.Y); + lines.Draw(penPosition, tickEnd, description.IntervalTickRadius, description.BodyLineColor); + lines.Draw(penPosition, backgroundEnd, description.BackgroundLineRadius, description.BodyLineColor); + characters.Clear().Append(tickValue); + text.Write(characters, tickEnd + + new Vector2(GlyphBatch.MeasureLength(characters, description.Font, description.IntervalTextHeight) * -0.5f, + description.HorizontalTickTextPadding + description.IntervalTextHeight * description.LineSpacingMultiplier), + description.IntervalTextHeight, description.TextColor, description.Font); } + } + { + var yDataIntervalSize = yDataSpan / yIntervalCount; + var previousTickValue = double.MinValue; + //Note the inclusion of the scale. Rounding occurs post-scale; moving back to pixels requires undoing the scale. + var valueToPixels = description.BodySpan.Y / (yDataSpan * description.VerticalIntervalValueScale); + for (int i = 0; i < description.TargetVerticalTickCount + 2; ++i) { - var yDataIntervalSize = yDataSpan / yIntervalCount; - var previousTickValue = double.MinValue; - //Note the inclusion of the scale. Rounding occurs post-scale; moving back to pixels requires undoing the scale. - var valueToPixels = description.BodySpan.Y / (yDataSpan * description.VerticalIntervalValueScale); - for (int i = 0; i < description.TargetVerticalTickCount + 2; ++i) + var tickValue = Math.Round((yDataIntervalSize * i) * description.VerticalIntervalValueScale, description.VerticalIntervalLabelRounding); + if (tickValue == previousTickValue) { - var tickValue = Math.Round((yDataIntervalSize * i) * description.VerticalIntervalValueScale, description.VerticalIntervalLabelRounding); - if (tickValue == previousTickValue) - { - //Don't bother creating redundant ticks. - continue; - } - previousTickValue = tickValue; + //Don't bother creating redundant ticks. + continue; + } + previousTickValue = tickValue; - var penPosition = lowerLeft - new Vector2(0, (float)(tickValue * valueToPixels)); + var penPosition = lowerLeft - new Vector2(0, (float)(tickValue * valueToPixels)); - var tickEnd = penPosition - new Vector2(description.IntervalTickLength, 0); - var backgroundEnd = penPosition + new Vector2(description.BodySpan.X, 0); - lines.Draw(penPosition, tickEnd, description.IntervalTickRadius, description.BodyLineColor); - lines.Draw(penPosition, backgroundEnd, description.BackgroundLineRadius, description.BodyLineColor); - characters.Clear().Append(tickValue + minY, description.VerticalIntervalLabelRounding); - text.Write(characters, - tickEnd + new Vector2(-description.VerticalTickTextPadding, 0.5f * GlyphBatch.MeasureLength(characters, description.Font, description.IntervalTextHeight)), - description.IntervalTextHeight, new Vector2(0, -1), description.TextColor, description.Font); - } + var tickEnd = penPosition - new Vector2(description.IntervalTickLength, 0); + var backgroundEnd = penPosition + new Vector2(description.BodySpan.X, 0); + lines.Draw(penPosition, tickEnd, description.IntervalTickRadius, description.BodyLineColor); + lines.Draw(penPosition, backgroundEnd, description.BackgroundLineRadius, description.BodyLineColor); + characters.Clear().Append(tickValue + minY, description.VerticalIntervalLabelRounding); + text.Write(characters, + tickEnd + new Vector2(-description.VerticalTickTextPadding, 0.5f * GlyphBatch.MeasureLength(characters, description.Font, description.IntervalTextHeight)), + description.IntervalTextHeight, new Vector2(0, -1), description.TextColor, description.Font); } + } - //Draw the line graphs on top of the body. + //Draw the line graphs on top of the body. + { + var dataToPixelsScale = new Vector2(description.BodySpan.X / (maxX - minX), (float)(description.BodySpan.Y / yDataSpan)); + Vector2 DataToScreenspace(int x, double y) { - var dataToPixelsScale = new Vector2(description.BodySpan.X / (maxX - minX), (float)(description.BodySpan.Y / yDataSpan)); - Vector2 DataToScreenspace(int x, double y) - { - var graphCoordinates = new Vector2(x - minX, (float)(y - minY)) * dataToPixelsScale; - var screenCoordinates = graphCoordinates; - screenCoordinates.Y = description.BodySpan.Y - screenCoordinates.Y; - screenCoordinates += description.BodyMinimum; - return screenCoordinates; - } + var graphCoordinates = new Vector2(x - minX, (float)(y - minY)) * dataToPixelsScale; + var screenCoordinates = graphCoordinates; + screenCoordinates.Y = description.BodySpan.Y - screenCoordinates.Y; + screenCoordinates += description.BodyMinimum; + return screenCoordinates; + } - for (int i = 0; i < graphSeries.Count; ++i) + for (int i = 0; i < graphSeries.Count; ++i) + { + var series = graphSeries[i]; + var data = series.Data; + var count = data.End - data.Start; + if (count > 0) { - var series = graphSeries[i]; - var data = series.Data; - var count = data.End - data.Start; - if (count > 0) + var previousScreenPosition = DataToScreenspace(data.Start, data[data.Start]); + if (count > 1) { - var previousScreenPosition = DataToScreenspace(data.Start, data[data.Start]); - if (count > 1) - { - for (int j = data.Start + 1; j < data.End; ++j) - { - var currentScreenPosition = DataToScreenspace(j, data[j]); - lines.Draw(previousScreenPosition, currentScreenPosition, series.LineRadius, series.LineColor); - previousScreenPosition = currentScreenPosition; - } - } - else + for (int j = data.Start + 1; j < data.End; ++j) { - //Only one datapoint. Draw a zero length line just to draw a dot. (The shader can handle it without producing nans.) - lines.Draw(previousScreenPosition, previousScreenPosition, series.LineRadius, series.LineColor); + var currentScreenPosition = DataToScreenspace(j, data[j]); + lines.Draw(previousScreenPosition, currentScreenPosition, series.LineRadius, series.LineColor); + previousScreenPosition = currentScreenPosition; } } + else + { + //Only one datapoint. Draw a zero length line just to draw a dot. (The shader can handle it without producing nans.) + lines.Draw(previousScreenPosition, previousScreenPosition, series.LineRadius, series.LineColor); + } } } + } - //Draw the legend entry last. Alpha blending will put it on top in case the legend is positioned on top of the body. - { - var penPosition = description.LegendMinimum; - var legendLineSpacing = description.LegendNameHeight * 1.5f; - penPosition.Y += legendLineSpacing; + //Draw the legend entry last. Alpha blending will put it on top in case the legend is positioned on top of the body. + { + var penPosition = description.LegendMinimum; + var legendLineSpacing = description.LegendNameHeight * 1.5f; + penPosition.Y += legendLineSpacing; - for (int i = 0; i < graphSeries.Count; ++i) - { - var series = graphSeries[i]; - var lineStart = new Vector2(penPosition.X, penPosition.Y); - var lineEnd = lineStart + new Vector2(description.LegendLineLength, -0.7f * description.LegendNameHeight); + for (int i = 0; i < graphSeries.Count; ++i) + { + var series = graphSeries[i]; + var lineStart = new Vector2(penPosition.X, penPosition.Y); + var lineEnd = lineStart + new Vector2(description.LegendLineLength, -0.7f * description.LegendNameHeight); - lines.Draw(lineStart, lineEnd, series.LineRadius, series.LineColor); - var textStart = new Vector2(lineEnd.X + series.LineRadius + description.LegendNameHeight * 0.2f, penPosition.Y); - characters.Clear().Append(series.Name); - text.Write(characters, textStart, description.LegendNameHeight, description.TextColor, description.Font); - penPosition.Y += legendLineSpacing; - } + lines.Draw(lineStart, lineEnd, series.LineRadius, series.LineColor); + var textStart = new Vector2(lineEnd.X + series.LineRadius + description.LegendNameHeight * 0.2f, penPosition.Y); + characters.Clear().Append(series.Name); + text.Write(characters, textStart, description.LegendNameHeight, description.TextColor, description.Font); + penPosition.Y += legendLineSpacing; } } - } + } From 05e3e8d33f5cbfd78064182313f9ad416cff9276 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Thu, 7 Mar 2024 16:36:11 -0600 Subject: [PATCH 866/947] Version bump. --- BepuPhysics/BepuPhysics.csproj | 2 +- BepuUtilities/BepuUtilities.csproj | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/BepuPhysics/BepuPhysics.csproj b/BepuPhysics/BepuPhysics.csproj index 8efd048c7..6286d413b 100644 --- a/BepuPhysics/BepuPhysics.csproj +++ b/BepuPhysics/BepuPhysics.csproj @@ -1,7 +1,7 @@  net8.0 - 2.5.0-beta.21 + 2.5.0-beta.22 Bepu Entertainment LLC Ross Nordby Speedy real time physics simulation library. diff --git a/BepuUtilities/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj index 423746c02..8c0ddb372 100644 --- a/BepuUtilities/BepuUtilities.csproj +++ b/BepuUtilities/BepuUtilities.csproj @@ -3,7 +3,7 @@ BepuUtilities BepuUtilities net8.0 - 2.5.0-beta.21 + 2.5.0-beta.22 Bepu Entertainment LLC Ross Nordby Supporting utilities library for BEPUphysics v2. From 2a266d4ae17ea1cb4081cbec2c637a39acdc6aa9 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 3 Apr 2024 16:11:21 -0500 Subject: [PATCH 867/947] Simulations can now be given Shapes collections, enabling the sharing of shape data without copying. --- BepuPhysics/Simulation.cs | 687 ++++++++++++----------- BepuPhysics/SimulationAllocationSizes.cs | 142 ++--- 2 files changed, 435 insertions(+), 394 deletions(-) diff --git a/BepuPhysics/Simulation.cs b/BepuPhysics/Simulation.cs index 0b5a065ec..2cb5b61c3 100644 --- a/BepuPhysics/Simulation.cs +++ b/BepuPhysics/Simulation.cs @@ -11,369 +11,406 @@ [module: SkipLocalsInit] #endif -namespace BepuPhysics +namespace BepuPhysics; + +/// +/// Orchestrates the bookkeeping and execution of a full dynamic simulation. +/// +public partial class Simulation : IDisposable { + /// + /// Gets the system responsible for awakening bodies within the simulation. + /// + public IslandAwakener Awakener { get; private set; } + /// + /// Gets the system responsible for putting bodies to sleep within the simulation. + /// + public IslandSleeper Sleeper { get; private set; } + /// + /// Gets the collection of bodies in the simulation. + /// + public Bodies Bodies { get; private set; } + /// + /// Gets the collection of statics within the simulation. + /// + public Statics Statics { get; private set; } + /// + /// Gets or sets the collection of shapes used by the simulation. + /// + /// + /// While instances can be shared between multiple instances, there are no guarantees of thread safety. + /// Further, setting the property to another collection while there are any outstanding references in the simulation to shapes within the old collection will result in fatal errors if the simulation continues to be used. + /// + public Shapes Shapes { get; set; } + /// + /// Gets the batch compressor used to compact constraints into fewer solver batches. + /// + public BatchCompressor SolverBatchCompressor { get; private set; } + /// + /// Gets the solver used to solved constraints within the simulation. + /// + public Solver Solver { get; private set; } + /// + /// Gets the integrator used to update velocities and poses within the simulation. + /// + public IPoseIntegrator PoseIntegrator { get; private set; } + /// + /// Gets the broad phase used by the simulation. Supports accelerated ray and volume queries. + /// + public BroadPhase BroadPhase { get; private set; } + /// + /// Gets the system used to find overlapping pairs within the simulation. + /// + public CollidableOverlapFinder BroadPhaseOverlapFinder { get; private set; } + /// + /// Gets the system used to identify contacts in colliding pairs of shapes and to update contact constraint data. + /// + public NarrowPhase NarrowPhase { get; private set; } + + SimulationProfiler profiler = new(13); + /// + /// Gets the simulation profiler. Note that the SimulationProfiler implementation only exists when the library is compiled with the PROFILE compilation symbol; if not defined, returned times are undefined. + /// + public SimulationProfiler Profiler { get { return profiler; } } + + //Helpers shared across at least two stages. + internal ConstraintRemover constraintRemover; + + /// + /// Gets the main memory pool used to fill persistent structures and main thread ephemeral resources across the engine. + /// + public BufferPool BufferPool { get; private set; } + + /// + /// Gets the timestepper used to update the simulation state. + /// + public ITimestepper Timestepper { get; private set; } + + /// + /// Gets or sets whether to use a deterministic time step when using multithreading. When set to true, additional time is spent sorting constraint additions and transfers. + /// Note that this can only affect determinism locally- different processor architectures may implement instructions differently. + /// + public bool Deterministic { get; set; } + /// - /// Orchestrates the bookkeeping and execution of a full dynamic simulation. + /// Constructs a simulation supporting dynamic movement and constraints with the specified narrow phase callbacks. /// - public partial class Simulation : IDisposable + /// Buffer pool used to fill persistent structures and main thread ephemeral resources across the engine. + /// Callbacks to use in the narrow phase. + /// Callbacks to use in the pose integrator. + /// Timestepper that defines how the simulation state should be updated. If null, is used. + /// Describes how the solver should execute, including the number of substeps and the number of velocity iterations per substep. + /// Allocation sizes to initialize the simulation with. If left null, default values are chosen. + /// Collection of shapes to use in the simulation, if any. If null, a new collection will be created for this simulation. + /// New simulation. + public static Simulation Create( + BufferPool bufferPool, TNarrowPhaseCallbacks narrowPhaseCallbacks, TPoseIntegratorCallbacks poseIntegratorCallbacks, SolveDescription solveDescription, ITimestepper timestepper = null, SimulationAllocationSizes? initialAllocationSizes = null, Shapes shapes = null) + where TNarrowPhaseCallbacks : struct, INarrowPhaseCallbacks + where TPoseIntegratorCallbacks : struct, IPoseIntegratorCallbacks { - public IslandAwakener Awakener { get; private set; } - public IslandSleeper Sleeper { get; private set; } - public Bodies Bodies { get; private set; } - public Statics Statics { get; private set; } - public Shapes Shapes { get; private set; } - public BatchCompressor SolverBatchCompressor { get; private set; } - public Solver Solver { get; private set; } - public IPoseIntegrator PoseIntegrator { get; private set; } - public BroadPhase BroadPhase { get; private set; } - public CollidableOverlapFinder BroadPhaseOverlapFinder { get; private set; } - public NarrowPhase NarrowPhase { get; private set; } - - SimulationProfiler profiler = new(13); - /// - /// Gets the simulation profiler. Note that the SimulationProfiler implementation only exists when the library is compiled with the PROFILE compilation symbol; if not defined, returned times are undefined. - /// - public SimulationProfiler Profiler { get { return profiler; } } - - //Helpers shared across at least two stages. - internal ConstraintRemover constraintRemover; - - /// - /// Gets the main memory pool used to fill persistent structures and main thread ephemeral resources across the engine. - /// - public BufferPool BufferPool { get; private set; } - - /// - /// Gets the timestepper used to update the simulation state. - /// - public ITimestepper Timestepper { get; private set; } - - /// - /// Gets or sets whether to use a deterministic time step when using multithreading. When set to true, additional time is spent sorting constraint additions and transfers. - /// Note that this can only affect determinism locally- different processor architectures may implement instructions differently. - /// - public bool Deterministic { get; set; } - - /// - /// Constructs a simulation supporting dynamic movement and constraints with the specified narrow phase callbacks. - /// - /// Buffer pool used to fill persistent structures and main thread ephemeral resources across the engine. - /// Callbacks to use in the narrow phase. - /// Callbacks to use in the pose integrator. - /// Timestepper that defines how the simulation state should be updated. If null, is used. - /// Describes how the solver should execute, including the number of substeps and the number of velocity iterations per substep. - /// Allocation sizes to initialize the simulation with. If left null, default values are chosen. - /// New simulation. - public static Simulation Create( - BufferPool bufferPool, TNarrowPhaseCallbacks narrowPhaseCallbacks, TPoseIntegratorCallbacks poseIntegratorCallbacks, SolveDescription solveDescription, ITimestepper timestepper = null, SimulationAllocationSizes? initialAllocationSizes = null) - where TNarrowPhaseCallbacks : struct, INarrowPhaseCallbacks - where TPoseIntegratorCallbacks : struct, IPoseIntegratorCallbacks + if (initialAllocationSizes == null) { - if (initialAllocationSizes == null) + initialAllocationSizes = new SimulationAllocationSizes { - initialAllocationSizes = new SimulationAllocationSizes - { - Bodies = 4096, - Statics = 4096, - ShapesPerType = 128, - ConstraintCountPerBodyEstimate = 8, - Constraints = 16384, - ConstraintsPerTypeBatch = 256 - }; - } - - //var simulation = new Simulation(bufferPool, initialAllocationSizes.Value, solverIterationCount, solverFallbackBatchThreshold, timestepper); - var simulation = new Simulation(); - simulation.BufferPool = bufferPool; - simulation.Shapes = new Shapes(bufferPool, initialAllocationSizes.Value.ShapesPerType); - simulation.BroadPhase = new BroadPhase(bufferPool, initialAllocationSizes.Value.Bodies, initialAllocationSizes.Value.Bodies + initialAllocationSizes.Value.Statics); - simulation.Bodies = new Bodies(bufferPool, simulation.Shapes, simulation.BroadPhase, - initialAllocationSizes.Value.Bodies, - initialAllocationSizes.Value.Islands, - initialAllocationSizes.Value.ConstraintCountPerBodyEstimate); - simulation.Statics = new Statics(bufferPool, simulation.Shapes, simulation.Bodies, simulation.BroadPhase, initialAllocationSizes.Value.Statics); - - var poseIntegrator = new PoseIntegrator(simulation.Bodies, simulation.Shapes, simulation.BroadPhase, poseIntegratorCallbacks); - simulation.PoseIntegrator = poseIntegrator; - - simulation.Solver = new Solver(simulation.Bodies, simulation.BufferPool, solveDescription, - initialCapacity: initialAllocationSizes.Value.Constraints, - initialIslandCapacity: initialAllocationSizes.Value.Islands, - minimumCapacityPerTypeBatch: initialAllocationSizes.Value.ConstraintsPerTypeBatch, poseIntegrator); - simulation.constraintRemover = new ConstraintRemover(simulation.BufferPool, simulation.Bodies, simulation.Solver); - simulation.Sleeper = new IslandSleeper(simulation.Bodies, simulation.Solver, simulation.BroadPhase, simulation.constraintRemover, simulation.BufferPool); - simulation.Awakener = new IslandAwakener(simulation.Bodies, simulation.Statics, simulation.Solver, simulation.BroadPhase, simulation.Sleeper, bufferPool); - simulation.Statics.awakener = simulation.Awakener; - simulation.Solver.awakener = simulation.Awakener; - simulation.Bodies.Initialize(simulation.Solver, simulation.Awakener, simulation.Sleeper); - simulation.SolverBatchCompressor = new BatchCompressor(simulation.Solver, simulation.Bodies); - simulation.Timestepper = timestepper ?? new DefaultTimestepper(); - - var narrowPhase = new NarrowPhase(simulation, - DefaultTypes.CreateDefaultCollisionTaskRegistry(), DefaultTypes.CreateDefaultSweepTaskRegistry(), - narrowPhaseCallbacks, initialAllocationSizes.Value.Islands + 1); - DefaultTypes.RegisterDefaults(simulation.Solver, narrowPhase); - simulation.NarrowPhase = narrowPhase; - simulation.Sleeper.pairCache = narrowPhase.PairCache; - simulation.Awakener.pairCache = narrowPhase.PairCache; - simulation.Solver.pairCache = narrowPhase.PairCache; - simulation.BroadPhaseOverlapFinder = new CollidableOverlapFinder(narrowPhase, simulation.BroadPhase); - - //We defer initialization until after all the other simulation bits are constructed. - poseIntegrator.Callbacks.Initialize(simulation); - narrowPhase.Callbacks.Initialize(simulation); - - return simulation; + Bodies = 4096, + Statics = 4096, + ShapesPerType = 128, + ConstraintCountPerBodyEstimate = 8, + Constraints = 16384, + ConstraintsPerTypeBatch = 256 + }; } + //var simulation = new Simulation(bufferPool, initialAllocationSizes.Value, solverIterationCount, solverFallbackBatchThreshold, timestepper); + var simulation = new Simulation(); + simulation.BufferPool = bufferPool; + simulation.Shapes = shapes == null ? new Shapes(bufferPool, initialAllocationSizes.Value.ShapesPerType) : shapes; + simulation.BroadPhase = new BroadPhase(bufferPool, initialAllocationSizes.Value.Bodies, initialAllocationSizes.Value.Bodies + initialAllocationSizes.Value.Statics); + simulation.Bodies = new Bodies(bufferPool, simulation.Shapes, simulation.BroadPhase, + initialAllocationSizes.Value.Bodies, + initialAllocationSizes.Value.Islands, + initialAllocationSizes.Value.ConstraintCountPerBodyEstimate); + simulation.Statics = new Statics(bufferPool, simulation.Shapes, simulation.Bodies, simulation.BroadPhase, initialAllocationSizes.Value.Statics); + + var poseIntegrator = new PoseIntegrator(simulation.Bodies, simulation.Shapes, simulation.BroadPhase, poseIntegratorCallbacks); + simulation.PoseIntegrator = poseIntegrator; + + simulation.Solver = new Solver(simulation.Bodies, simulation.BufferPool, solveDescription, + initialCapacity: initialAllocationSizes.Value.Constraints, + initialIslandCapacity: initialAllocationSizes.Value.Islands, + minimumCapacityPerTypeBatch: initialAllocationSizes.Value.ConstraintsPerTypeBatch, poseIntegrator); + simulation.constraintRemover = new ConstraintRemover(simulation.BufferPool, simulation.Bodies, simulation.Solver); + simulation.Sleeper = new IslandSleeper(simulation.Bodies, simulation.Solver, simulation.BroadPhase, simulation.constraintRemover, simulation.BufferPool); + simulation.Awakener = new IslandAwakener(simulation.Bodies, simulation.Statics, simulation.Solver, simulation.BroadPhase, simulation.Sleeper, bufferPool); + simulation.Statics.awakener = simulation.Awakener; + simulation.Solver.awakener = simulation.Awakener; + simulation.Bodies.Initialize(simulation.Solver, simulation.Awakener, simulation.Sleeper); + simulation.SolverBatchCompressor = new BatchCompressor(simulation.Solver, simulation.Bodies); + simulation.Timestepper = timestepper ?? new DefaultTimestepper(); + + var narrowPhase = new NarrowPhase(simulation, + DefaultTypes.CreateDefaultCollisionTaskRegistry(), DefaultTypes.CreateDefaultSweepTaskRegistry(), + narrowPhaseCallbacks, initialAllocationSizes.Value.Islands + 1); + DefaultTypes.RegisterDefaults(simulation.Solver, narrowPhase); + simulation.NarrowPhase = narrowPhase; + simulation.Sleeper.pairCache = narrowPhase.PairCache; + simulation.Awakener.pairCache = narrowPhase.PairCache; + simulation.Solver.pairCache = narrowPhase.PairCache; + simulation.BroadPhaseOverlapFinder = new CollidableOverlapFinder(narrowPhase, simulation.BroadPhase); + + //We defer initialization until after all the other simulation bits are constructed. + poseIntegrator.Callbacks.Initialize(simulation); + narrowPhase.Callbacks.Initialize(simulation); + + return simulation; + } + - private static int ValidateAndCountShapefulBodies(ref BodySet bodySet, ref Tree tree, ref Buffer leaves) + private static int ValidateAndCountShapefulBodies(ref BodySet bodySet, ref Tree tree, ref Buffer leaves) + { + int shapefulBodyCount = 0; + for (int i = 0; i < bodySet.Count; ++i) { - int shapefulBodyCount = 0; - for (int i = 0; i < bodySet.Count; ++i) + ref var collidable = ref bodySet.Collidables[i]; + if (collidable.Shape.Exists) { - ref var collidable = ref bodySet.Collidables[i]; - if (collidable.Shape.Exists) - { - Debug.Assert(collidable.BroadPhaseIndex >= 0 && collidable.BroadPhaseIndex < tree.LeafCount); - ref var leaf = ref leaves[collidable.BroadPhaseIndex]; - Debug.Assert(leaf.StaticHandle.Value == bodySet.IndexToHandle[i].Value); - Debug.Assert(leaf.Mobility == CollidableMobility.Dynamic || leaf.Mobility == CollidableMobility.Kinematic); - ++shapefulBodyCount; - } + Debug.Assert(collidable.BroadPhaseIndex >= 0 && collidable.BroadPhaseIndex < tree.LeafCount); + ref var leaf = ref leaves[collidable.BroadPhaseIndex]; + Debug.Assert(leaf.StaticHandle.Value == bodySet.IndexToHandle[i].Value); + Debug.Assert(leaf.Mobility == CollidableMobility.Dynamic || leaf.Mobility == CollidableMobility.Kinematic); + ++shapefulBodyCount; } - return shapefulBodyCount; } + return shapefulBodyCount; + } - [Conditional("DEBUG")] - internal void ValidateCollidables() - { - var activeShapefulBodyCount = ValidateAndCountShapefulBodies(ref Bodies.ActiveSet, ref BroadPhase.ActiveTree, ref BroadPhase.ActiveLeaves); - Debug.Assert(BroadPhase.ActiveTree.LeafCount == activeShapefulBodyCount); + [Conditional("DEBUG")] + internal void ValidateCollidables() + { + var activeShapefulBodyCount = ValidateAndCountShapefulBodies(ref Bodies.ActiveSet, ref BroadPhase.ActiveTree, ref BroadPhase.ActiveLeaves); + Debug.Assert(BroadPhase.ActiveTree.LeafCount == activeShapefulBodyCount); - int inactiveShapefulBodyCount = 0; + int inactiveShapefulBodyCount = 0; - for (int setIndex = 1; setIndex < Bodies.Sets.Length; ++setIndex) + for (int setIndex = 1; setIndex < Bodies.Sets.Length; ++setIndex) + { + ref var set = ref Bodies.Sets[setIndex]; + if (set.Allocated) { - ref var set = ref Bodies.Sets[setIndex]; - if (set.Allocated) - { - inactiveShapefulBodyCount += ValidateAndCountShapefulBodies(ref set, ref BroadPhase.StaticTree, ref BroadPhase.StaticLeaves); - } + inactiveShapefulBodyCount += ValidateAndCountShapefulBodies(ref set, ref BroadPhase.StaticTree, ref BroadPhase.StaticLeaves); } - Debug.Assert(inactiveShapefulBodyCount + Statics.Count == BroadPhase.StaticTree.LeafCount); - for (int i = 0; i < Statics.Count; ++i) - { - ref var collidable = ref Statics[i]; - Debug.Assert(collidable.Shape.Exists, "All static collidables must have shapes. That's their only purpose."); + } + Debug.Assert(inactiveShapefulBodyCount + Statics.Count == BroadPhase.StaticTree.LeafCount); + for (int i = 0; i < Statics.Count; ++i) + { + ref var collidable = ref Statics[i]; + Debug.Assert(collidable.Shape.Exists, "All static collidables must have shapes. That's their only purpose."); - Debug.Assert(collidable.BroadPhaseIndex >= 0 && collidable.BroadPhaseIndex < BroadPhase.StaticTree.LeafCount); - ref var leaf = ref BroadPhase.StaticLeaves[collidable.BroadPhaseIndex]; - Debug.Assert(leaf.StaticHandle.Value == Statics.IndexToHandle[i].Value); - Debug.Assert(leaf.Mobility == CollidableMobility.Static); - } + Debug.Assert(collidable.BroadPhaseIndex >= 0 && collidable.BroadPhaseIndex < BroadPhase.StaticTree.LeafCount); + ref var leaf = ref BroadPhase.StaticLeaves[collidable.BroadPhaseIndex]; + Debug.Assert(leaf.StaticHandle.Value == Statics.IndexToHandle[i].Value); + Debug.Assert(leaf.Mobility == CollidableMobility.Static); + } - //Ensure there are no duplicates between the two broad phase trees. - for (int i = 0; i < BroadPhase.ActiveTree.LeafCount; ++i) + //Ensure there are no duplicates between the two broad phase trees. + for (int i = 0; i < BroadPhase.ActiveTree.LeafCount; ++i) + { + var activeLeaf = BroadPhase.ActiveLeaves[i]; + for (int j = 0; j < BroadPhase.StaticTree.LeafCount; ++j) { - var activeLeaf = BroadPhase.ActiveLeaves[i]; - for (int j = 0; j < BroadPhase.StaticTree.LeafCount; ++j) - { - Debug.Assert(BroadPhase.StaticLeaves[j].Packed != activeLeaf.Packed); - } + Debug.Assert(BroadPhase.StaticLeaves[j].Packed != activeLeaf.Packed); } - } - //These functions act as convenience wrappers around common execution patterns. They can be mixed and matched in custom timesteps, or for certain advanced use cases, called directly. - /// - /// Executes the sleep stage, moving candidate - /// - /// Thread dispatcher to use for the sleeper execution, if any. - public void Sleep(IThreadDispatcher threadDispatcher = null) - { - profiler.Start(Sleeper); - Sleeper.Update(threadDispatcher, Deterministic); - profiler.End(Sleeper); - } + } - /// - /// Predicts the bounding boxes of active bodies by speculatively integrating velocity. Does not actually modify body velocities. Updates deactivation candidacy. - /// - /// Duration of the time step. - /// Thread dispatcher to use for execution, if any. - public void PredictBoundingBoxes(float dt, IThreadDispatcher threadDispatcher = null) - { - profiler.Start(PoseIntegrator); - PoseIntegrator.PredictBoundingBoxes(dt, BufferPool, threadDispatcher); - profiler.End(PoseIntegrator); - } + //These functions act as convenience wrappers around common execution patterns. They can be mixed and matched in custom timesteps, or for certain advanced use cases, called directly. + /// + /// Executes the sleep stage, moving candidate + /// + /// Thread dispatcher to use for the sleeper execution, if any. + public void Sleep(IThreadDispatcher threadDispatcher = null) + { + profiler.Start(Sleeper); + Sleeper.Update(threadDispatcher, Deterministic); + profiler.End(Sleeper); + } - /// - /// Updates the broad phase structure for the current body bounding boxes, finds potentially colliding pairs, and then executes the narrow phase for all such pairs. Generates contact constraints for the solver. - /// - /// Duration of the time step. - /// Thread dispatcher to use for execution, if any. - public void CollisionDetection(float dt, IThreadDispatcher threadDispatcher = null) - { - profiler.Start(BroadPhase); - //BroadPhase.Update(threadDispatcher); - BroadPhase.Update2(threadDispatcher); - profiler.End(BroadPhase); - - profiler.Start(BroadPhaseOverlapFinder); - BroadPhaseOverlapFinder.DispatchOverlaps(dt, threadDispatcher); - profiler.End(BroadPhaseOverlapFinder); - - profiler.Start(NarrowPhase); - NarrowPhase.Flush(threadDispatcher); - profiler.End(NarrowPhase); - } + /// + /// Predicts the bounding boxes of active bodies by speculatively integrating velocity. Does not actually modify body velocities. Updates deactivation candidacy. + /// + /// Duration of the time step. + /// Thread dispatcher to use for execution, if any. + public void PredictBoundingBoxes(float dt, IThreadDispatcher threadDispatcher = null) + { + profiler.Start(PoseIntegrator); + PoseIntegrator.PredictBoundingBoxes(dt, BufferPool, threadDispatcher); + profiler.End(PoseIntegrator); + } - /// - /// Updates the broad phase structure for the current body bounding boxes, finds potentially colliding pairs, and then executes the narrow phase for all such pairs. Generates contact constraints for the solver. - /// - /// Duration of the time step. - /// Thread dispatcher to use for execution, if any. - public void Solve(float dt, IThreadDispatcher threadDispatcher = null) - { - Profiler.Start(Solver); - var constrainedBodySet = Solver.PrepareConstraintIntegrationResponsibilities(threadDispatcher); - Solver.Solve(dt, threadDispatcher); - Profiler.End(Solver); + /// + /// Updates the broad phase structure for the current body bounding boxes, finds potentially colliding pairs, and then executes the narrow phase for all such pairs. Generates contact constraints for the solver. + /// + /// Duration of the time step. + /// Thread dispatcher to use for execution, if any. + public void CollisionDetection(float dt, IThreadDispatcher threadDispatcher = null) + { + profiler.Start(BroadPhase); + //BroadPhase.Update(threadDispatcher); + BroadPhase.Update2(threadDispatcher); + profiler.End(BroadPhase); + + profiler.Start(BroadPhaseOverlapFinder); + BroadPhaseOverlapFinder.DispatchOverlaps(dt, threadDispatcher); + profiler.End(BroadPhaseOverlapFinder); + + profiler.Start(NarrowPhase); + NarrowPhase.Flush(threadDispatcher); + profiler.End(NarrowPhase); + } - Profiler.Start(PoseIntegrator); - PoseIntegrator.IntegrateAfterSubstepping(constrainedBodySet, dt, Solver.SubstepCount, threadDispatcher); - Profiler.End(PoseIntegrator); + /// + /// Updates the broad phase structure for the current body bounding boxes, finds potentially colliding pairs, and then executes the narrow phase for all such pairs. Generates contact constraints for the solver. + /// + /// Duration of the time step. + /// Thread dispatcher to use for execution, if any. + public void Solve(float dt, IThreadDispatcher threadDispatcher = null) + { + Profiler.Start(Solver); + var constrainedBodySet = Solver.PrepareConstraintIntegrationResponsibilities(threadDispatcher); + Solver.Solve(dt, threadDispatcher); + Profiler.End(Solver); - Solver.DisposeConstraintIntegrationResponsibilities(); - } + Profiler.Start(PoseIntegrator); + PoseIntegrator.IntegrateAfterSubstepping(constrainedBodySet, dt, Solver.SubstepCount, threadDispatcher); + Profiler.End(PoseIntegrator); - /// - /// Incrementally improves body and constraint storage for better performance. - /// - /// Thread dispatcher to use for execution, if any. - public void IncrementallyOptimizeDataStructures(IThreadDispatcher threadDispatcher = null) - { - //Previously, this handled body and constraint memory layout optimization. 2.4 significantly changed how memory accesses work in the solver - //and the optimizers were no longer net wins, so all that's left is the batch compressor. - //It pulls constraints currently living in high constraint batch indices to lower constraint batches if possible. - //Over time, that'll tend to reduce sync points in the solver and improve performance. - profiler.Start(SolverBatchCompressor); - SolverBatchCompressor.Compress(BufferPool, threadDispatcher, threadDispatcher != null && Deterministic); - profiler.End(SolverBatchCompressor); - } + Solver.DisposeConstraintIntegrationResponsibilities(); + } - //TODO: I wonder if people will abuse the dt-as-parameter to the point where we should make it a field instead, like it effectively was in v1. - /// - /// Performs one timestep of the given length. - /// - /// - /// Be wary of variable timesteps. They can harm stability. Whenever possible, keep the timestep the same across multiple frames unless you have a specific reason not to. - /// - /// Duration of the time step. - /// Thread dispatcher to use for execution, if any. - public void Timestep(float dt, IThreadDispatcher threadDispatcher = null) - { - if (dt <= 0) - throw new ArgumentException("Timestep duration must be positive.", nameof(dt)); - profiler.Clear(); - profiler.Start(this); + /// + /// Incrementally improves body and constraint storage for better performance. + /// + /// Thread dispatcher to use for execution, if any. + public void IncrementallyOptimizeDataStructures(IThreadDispatcher threadDispatcher = null) + { + //Previously, this handled body and constraint memory layout optimization. 2.4 significantly changed how memory accesses work in the solver + //and the optimizers were no longer net wins, so all that's left is the batch compressor. + //It pulls constraints currently living in high constraint batch indices to lower constraint batches if possible. + //Over time, that'll tend to reduce sync points in the solver and improve performance. + profiler.Start(SolverBatchCompressor); + SolverBatchCompressor.Compress(BufferPool, threadDispatcher, threadDispatcher != null && Deterministic); + profiler.End(SolverBatchCompressor); + } - Timestepper.Timestep(this, dt, threadDispatcher); + //TODO: I wonder if people will abuse the dt-as-parameter to the point where we should make it a field instead, like it effectively was in v1. + /// + /// Performs one timestep of the given length. + /// + /// + /// Be wary of variable timesteps. They can harm stability. Whenever possible, keep the timestep the same across multiple frames unless you have a specific reason not to. + /// + /// Duration of the time step. + /// Thread dispatcher to use for execution, if any. + public void Timestep(float dt, IThreadDispatcher threadDispatcher = null) + { + if (dt <= 0) + throw new ArgumentException("Timestep duration must be positive.", nameof(dt)); + profiler.Clear(); + profiler.Start(this); - profiler.End(this); - } + Timestepper.Timestep(this, dt, threadDispatcher); - /// - /// Clears the simulation of every object, only returning memory to the pool that would be returned by sequential removes. - /// Other persistent allocations, like those in the Bodies set, will remain. - /// - public void Clear() - { - Solver.Clear(); - Bodies.Clear(); - Statics.Clear(); - Shapes.Clear(); - BroadPhase.Clear(); - NarrowPhase.Clear(); - Sleeper.Clear(); - } + profiler.End(this); + } - /// - /// Increases the allocation size of any buffers too small to hold the allocation target. - /// - /// - /// - /// The final size of the allocated buffers are constrained by the allocator. It is not guaranteed to be exactly equal to the target, but it is guaranteed to be at least as large. - /// - /// - /// This is primarily a convenience function. Everything it does internally can be done externally. - /// For example, if only type batches need to be resized, the solver's own functions can be used directly. - /// - /// - /// Allocation sizes to guarantee sufficient size for. - public void EnsureCapacity(SimulationAllocationSizes allocationTarget) - { - Solver.EnsureSolverCapacities(allocationTarget.Bodies, allocationTarget.Constraints); - Solver.MinimumCapacityPerTypeBatch = Math.Max(allocationTarget.ConstraintsPerTypeBatch, Solver.MinimumCapacityPerTypeBatch); - Solver.EnsureTypeBatchCapacities(); - NarrowPhase.PairCache.EnsureConstraintToPairMappingCapacity(Solver, allocationTarget.Constraints); - //Note that the bodies set has to come before the body layout optimizer; the body layout optimizer's sizes are dependent upon the bodies set. - Bodies.EnsureCapacity(allocationTarget.Bodies); - Bodies.MinimumConstraintCapacityPerBody = allocationTarget.ConstraintCountPerBodyEstimate; - Bodies.EnsureConstraintListCapacities(); - Sleeper.EnsureSetsCapacity(allocationTarget.Islands + 1); - Statics.EnsureCapacity(allocationTarget.Statics); - Shapes.EnsureBatchCapacities(allocationTarget.ShapesPerType); - BroadPhase.EnsureCapacity(allocationTarget.Bodies, allocationTarget.Bodies + allocationTarget.Statics); - } + /// + /// Clears the simulation of every object, only returning memory to the pool that would be returned by sequential removes. + /// Other persistent allocations, like those in the Bodies set, will remain. + /// + public void Clear() + { + Solver.Clear(); + Bodies.Clear(); + Statics.Clear(); + Shapes.Clear(); + BroadPhase.Clear(); + NarrowPhase.Clear(); + Sleeper.Clear(); + } + /// + /// Increases the allocation size of any buffers too small to hold the allocation target. + /// + /// + /// + /// The final size of the allocated buffers are constrained by the allocator. It is not guaranteed to be exactly equal to the target, but it is guaranteed to be at least as large. + /// + /// + /// This is primarily a convenience function. Everything it does internally can be done externally. + /// For example, if only type batches need to be resized, the solver's own functions can be used directly. + /// + /// + /// Allocation sizes to guarantee sufficient size for. + public void EnsureCapacity(SimulationAllocationSizes allocationTarget) + { + Solver.EnsureSolverCapacities(allocationTarget.Bodies, allocationTarget.Constraints); + Solver.MinimumCapacityPerTypeBatch = Math.Max(allocationTarget.ConstraintsPerTypeBatch, Solver.MinimumCapacityPerTypeBatch); + Solver.EnsureTypeBatchCapacities(); + NarrowPhase.PairCache.EnsureConstraintToPairMappingCapacity(Solver, allocationTarget.Constraints); + //Note that the bodies set has to come before the body layout optimizer; the body layout optimizer's sizes are dependent upon the bodies set. + Bodies.EnsureCapacity(allocationTarget.Bodies); + Bodies.MinimumConstraintCapacityPerBody = allocationTarget.ConstraintCountPerBodyEstimate; + Bodies.EnsureConstraintListCapacities(); + Sleeper.EnsureSetsCapacity(allocationTarget.Islands + 1); + Statics.EnsureCapacity(allocationTarget.Statics); + Shapes.EnsureBatchCapacities(allocationTarget.ShapesPerType); + BroadPhase.EnsureCapacity(allocationTarget.Bodies, allocationTarget.Bodies + allocationTarget.Statics); + } - /// - /// Increases the allocation size of any buffers too small to hold the allocation target, and decreases the allocation size of any buffers that are unnecessarily large. - /// - /// - /// - /// The final size of the allocated buffers are constrained by the allocator. It is not guaranteed to be exactly equal to the target, but it is guaranteed to be at least as large. - /// - /// - /// This is primarily a convenience function. Everything it does internally can be done externally. - /// For example, if only type batches need to be resized, the solver's own functions can be used directly. - /// - /// - /// Allocation sizes to guarantee sufficient size for. - public void Resize(SimulationAllocationSizes allocationTarget) - { - Solver.ResizeSolverCapacities(allocationTarget.Bodies, allocationTarget.Constraints); - Solver.MinimumCapacityPerTypeBatch = allocationTarget.ConstraintsPerTypeBatch; - Solver.ResizeTypeBatchCapacities(); - NarrowPhase.PairCache.ResizeConstraintToPairMappingCapacity(Solver, allocationTarget.Constraints); - //Note that the bodies set has to come before the body layout optimizer; the body layout optimizer's sizes are dependent upon the bodies set. - Bodies.Resize(allocationTarget.Bodies); - Bodies.MinimumConstraintCapacityPerBody = allocationTarget.ConstraintCountPerBodyEstimate; - Bodies.ResizeConstraintListCapacities(); - Sleeper.ResizeSetsCapacity(allocationTarget.Islands + 1); - Statics.Resize(allocationTarget.Statics); - Shapes.ResizeBatches(allocationTarget.ShapesPerType); - BroadPhase.Resize(allocationTarget.Bodies, allocationTarget.Bodies + allocationTarget.Statics); - } - /// - /// Clears the simulation of every object and returns all pooled memory to the buffer pool. Leaves the simulation in an unusable state. - /// - public void Dispose() - { - Clear(); - Sleeper.Dispose(); - Solver.Dispose(); - BroadPhase.Dispose(); - NarrowPhase.Dispose(); - Bodies.Dispose(); - Statics.Dispose(); - Shapes.Dispose(); - } + /// + /// Increases the allocation size of any buffers too small to hold the allocation target, and decreases the allocation size of any buffers that are unnecessarily large. + /// + /// + /// + /// The final size of the allocated buffers are constrained by the allocator. It is not guaranteed to be exactly equal to the target, but it is guaranteed to be at least as large. + /// + /// + /// This is primarily a convenience function. Everything it does internally can be done externally. + /// For example, if only type batches need to be resized, the solver's own functions can be used directly. + /// + /// + /// Allocation sizes to guarantee sufficient size for. + public void Resize(SimulationAllocationSizes allocationTarget) + { + Solver.ResizeSolverCapacities(allocationTarget.Bodies, allocationTarget.Constraints); + Solver.MinimumCapacityPerTypeBatch = allocationTarget.ConstraintsPerTypeBatch; + Solver.ResizeTypeBatchCapacities(); + NarrowPhase.PairCache.ResizeConstraintToPairMappingCapacity(Solver, allocationTarget.Constraints); + //Note that the bodies set has to come before the body layout optimizer; the body layout optimizer's sizes are dependent upon the bodies set. + Bodies.Resize(allocationTarget.Bodies); + Bodies.MinimumConstraintCapacityPerBody = allocationTarget.ConstraintCountPerBodyEstimate; + Bodies.ResizeConstraintListCapacities(); + Sleeper.ResizeSetsCapacity(allocationTarget.Islands + 1); + Statics.Resize(allocationTarget.Statics); + Shapes.ResizeBatches(allocationTarget.ShapesPerType); + BroadPhase.Resize(allocationTarget.Bodies, allocationTarget.Bodies + allocationTarget.Statics); + } + + /// + /// Clears the simulation of every object and returns all pooled memory to the buffer pool. Leaves the simulation in an unusable state. + /// + public void Dispose() + { + Clear(); + Sleeper.Dispose(); + Solver.Dispose(); + BroadPhase.Dispose(); + NarrowPhase.Dispose(); + Bodies.Dispose(); + Statics.Dispose(); + Shapes.Dispose(); } } diff --git a/BepuPhysics/SimulationAllocationSizes.cs b/BepuPhysics/SimulationAllocationSizes.cs index 1ac2f0a21..44536eef2 100644 --- a/BepuPhysics/SimulationAllocationSizes.cs +++ b/BepuPhysics/SimulationAllocationSizes.cs @@ -1,69 +1,73 @@ -using System.Runtime.InteropServices; - -namespace BepuPhysics -{ - /// - /// The common set of allocation sizes for a simulation. - /// - [StructLayout(LayoutKind.Sequential)] - public struct SimulationAllocationSizes - { - /// - /// The number of bodies to allocate space for. - /// - public int Bodies; - /// - /// The number of statics to allocate space for. - /// - public int Statics; - /// - /// The number of inactive islands to allocate space for. - /// - public int Islands; - /// - /// Minimum number of shapes to allocate space for in each shape type batch. - /// - public int ShapesPerType; - /// - /// The number of constraints to allocate bookkeeping space for. This does not affect actual type batch allocation sizes, only the solver-level constraint handle storage. - /// - public int Constraints; - /// - /// The minimum number of constraints to allocate space for in each individual type batch. - /// New type batches will be given enough memory for this number of constraints, and any compaction will not reduce the allocations below it. - /// The number of constraints can vary greatly across types- there are usually far more contacts than ragdoll constraints. - /// Per type estimates can be assigned within the Solver.TypeBatchAllocation if necessary. This value acts as a lower bound for all types. - /// - public int ConstraintsPerTypeBatch; - /// - /// The minimum number of constraints to allocate space for in each body's constraint list. - /// New bodies will be given enough memory for this number of constraints, and any compaction will not reduce the allocations below it. - /// - public int ConstraintCountPerBodyEstimate; - - /// - /// Constructs a description of simulation allocations. - /// - /// The number of bodies to allocate space for. - /// The number of statics to allocate space for. - /// The number of inactive islands to allocate space for. - /// Minimum number of shapes to allocate space for in each shape type batch. - /// The number of constraints to allocate bookkeeping space for. This does not affect actual type batch allocation sizes, only the solver-level constraint handle storage. - /// The minimum number of constraints to allocate space for in each individual type batch. - /// New type batches will be given enough memory for this number of constraints, and any compaction will not reduce the allocations below it. - /// The number of constraints can vary greatly across types- there are usually far more contacts than ragdoll constraints. - /// Per type estimates can be assigned within the Solver.TypeBatchAllocation if necessary. This value acts as a lower bound for all types. - /// The minimum number of constraints to allocate space for in each body's constraint list. - /// New bodies will be given enough memory for this number of constraints, and any compaction will not reduce the allocations below it. - public SimulationAllocationSizes(int bodies, int statics, int islands, int shapesPerType, int constraints, int constraintsPerTypeBatch, int constraintCountPerBodyEstimate) - { - Bodies = bodies; - Statics = statics; - Islands = islands; - ShapesPerType = shapesPerType; - Constraints = constraints; - ConstraintsPerTypeBatch = constraintsPerTypeBatch; - ConstraintCountPerBodyEstimate = constraintCountPerBodyEstimate; - } - } -} +using BepuPhysics.Collidables; +using System.Runtime.InteropServices; + +namespace BepuPhysics +{ + /// + /// The common set of allocation sizes for a simulation. + /// + [StructLayout(LayoutKind.Sequential)] + public struct SimulationAllocationSizes + { + /// + /// The number of bodies to allocate space for. + /// + public int Bodies; + /// + /// The number of statics to allocate space for. + /// + public int Statics; + /// + /// The number of inactive islands to allocate space for. + /// + public int Islands; + /// + /// Minimum number of shapes to allocate space for in each shape type batch. + /// + /// + /// Unused if a instance was directly provided to the constructor. + /// + public int ShapesPerType; + /// + /// The number of constraints to allocate bookkeeping space for. This does not affect actual type batch allocation sizes, only the solver-level constraint handle storage. + /// + public int Constraints; + /// + /// The minimum number of constraints to allocate space for in each individual type batch. + /// New type batches will be given enough memory for this number of constraints, and any compaction will not reduce the allocations below it. + /// The number of constraints can vary greatly across types- there are usually far more contacts than ragdoll constraints. + /// Per type estimates can be assigned within the Solver.TypeBatchAllocation if necessary. This value acts as a lower bound for all types. + /// + public int ConstraintsPerTypeBatch; + /// + /// The minimum number of constraints to allocate space for in each body's constraint list. + /// New bodies will be given enough memory for this number of constraints, and any compaction will not reduce the allocations below it. + /// + public int ConstraintCountPerBodyEstimate; + + /// + /// Constructs a description of simulation allocations. + /// + /// The number of bodies to allocate space for. + /// The number of statics to allocate space for. + /// The number of inactive islands to allocate space for. + /// Minimum number of shapes to allocate space for in each shape type batch. Unused if a instance was directly provided to the constructor. + /// The number of constraints to allocate bookkeeping space for. This does not affect actual type batch allocation sizes, only the solver-level constraint handle storage. + /// The minimum number of constraints to allocate space for in each individual type batch. + /// New type batches will be given enough memory for this number of constraints, and any compaction will not reduce the allocations below it. + /// The number of constraints can vary greatly across types- there are usually far more contacts than ragdoll constraints. + /// Per type estimates can be assigned within the Solver.TypeBatchAllocation if necessary. This value acts as a lower bound for all types. + /// The minimum number of constraints to allocate space for in each body's constraint list. + /// New bodies will be given enough memory for this number of constraints, and any compaction will not reduce the allocations below it. + public SimulationAllocationSizes(int bodies, int statics, int islands, int shapesPerType, int constraints, int constraintsPerTypeBatch, int constraintCountPerBodyEstimate) + { + Bodies = bodies; + Statics = statics; + Islands = islands; + ShapesPerType = shapesPerType; + Constraints = constraints; + ConstraintsPerTypeBatch = constraintsPerTypeBatch; + ConstraintCountPerBodyEstimate = constraintCountPerBodyEstimate; + } + } +} From 1eb1c03304f4943fc7936ef52409a130e64d9856 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 3 Apr 2024 16:11:50 -0500 Subject: [PATCH 868/947] Version bump. --- BepuPhysics/BepuPhysics.csproj | 2 +- BepuUtilities/BepuUtilities.csproj | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/BepuPhysics/BepuPhysics.csproj b/BepuPhysics/BepuPhysics.csproj index 6286d413b..0339cf7f9 100644 --- a/BepuPhysics/BepuPhysics.csproj +++ b/BepuPhysics/BepuPhysics.csproj @@ -1,7 +1,7 @@  net8.0 - 2.5.0-beta.22 + 2.5.0-beta.23 Bepu Entertainment LLC Ross Nordby Speedy real time physics simulation library. diff --git a/BepuUtilities/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj index 8c0ddb372..250b3658a 100644 --- a/BepuUtilities/BepuUtilities.csproj +++ b/BepuUtilities/BepuUtilities.csproj @@ -3,7 +3,7 @@ BepuUtilities BepuUtilities net8.0 - 2.5.0-beta.22 + 2.5.0-beta.23 Bepu Entertainment LLC Ross Nordby Supporting utilities library for BEPUphysics v2. From 0a23c1501e723a47edae07fe279b64d5b0845ea2 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 21 Apr 2024 16:30:36 -0500 Subject: [PATCH 869/947] Bumped dependency version. --- DemoContentBuilder/DemoContentBuilder.csproj | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/DemoContentBuilder/DemoContentBuilder.csproj b/DemoContentBuilder/DemoContentBuilder.csproj index c72b01565..478c00e70 100644 --- a/DemoContentBuilder/DemoContentBuilder.csproj +++ b/DemoContentBuilder/DemoContentBuilder.csproj @@ -14,7 +14,7 @@ - + From fd93751d3e452130467247c0b9574dd373080a2d Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 14 Jul 2024 22:57:00 -0500 Subject: [PATCH 870/947] Wee error check. --- BepuUtilities/Collections/QuickList.cs | 1 + DemoRenderer/Constraints/ConstraintLineExtractor.cs | 3 ++- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/BepuUtilities/Collections/QuickList.cs b/BepuUtilities/Collections/QuickList.cs index dd9f86ce2..0baafd3e0 100644 --- a/BepuUtilities/Collections/QuickList.cs +++ b/BepuUtilities/Collections/QuickList.cs @@ -372,6 +372,7 @@ public ref T Allocate(IUnmanagedMemoryPool pool) [MethodImpl(MethodImplOptions.AggressiveInlining)] public ref T Allocate(int count, IUnmanagedMemoryPool pool) { + Debug.Assert(count > 0, "Allocating a count returns a reference to the allocated region, and so must allocate at least one element."); var newCount = Count + count; if (newCount > Span.Length) Resize(Math.Max(Count * 2, newCount), pool); diff --git a/DemoRenderer/Constraints/ConstraintLineExtractor.cs b/DemoRenderer/Constraints/ConstraintLineExtractor.cs index da24df035..6e5c94c4d 100644 --- a/DemoRenderer/Constraints/ConstraintLineExtractor.cs +++ b/DemoRenderer/Constraints/ConstraintLineExtractor.cs @@ -212,7 +212,8 @@ internal void CreateJobs(Simulation simulation, int simulationIndex, bool showCo } } } - lines.Allocate(newLineCount, pool); + if (newLineCount > 0) + lines.Allocate(newLineCount, pool); } } From f1103bbc0e152ef1f2bf6bf941856cae6cdfa1d3 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 14 Jul 2024 23:11:07 -0500 Subject: [PATCH 871/947] Some comments clarifications/typofixes. --- BepuPhysics/BodyReference.cs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/BepuPhysics/BodyReference.cs b/BepuPhysics/BodyReference.cs index 7dc637805..91b204b83 100644 --- a/BepuPhysics/BodyReference.cs +++ b/BepuPhysics/BodyReference.cs @@ -396,7 +396,7 @@ public static void ApplyLinearImpulse(Vector3 impulse, float inverseMass, ref Ve /// - /// Applies an impulse to a body at the given world space position. Does not modify activity states. + /// Applies an impulse to a body at the given world space offset. Does not modify activity states. /// /// Impulse to apply to the body. /// World space offset to apply the impulse at. @@ -421,9 +421,9 @@ public void ApplyLinearImpulse(Vector3 impulse) } /// - /// Computes the velocity of an offset point attached to the body. + /// Computes the velocity of a world space offset point attached to the body. /// - /// Offset from the body's center to + /// World space offset from the body's center to the point to measure. /// Effective velocity of the point if it were attached to the body. [MethodImpl(MethodImplOptions.AggressiveInlining)] public void GetVelocityForOffset(Vector3 offset, out Vector3 velocity) From f4b086c5245d40e370ba8ed73e96f982aa007062 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 16 Nov 2024 13:35:45 -0600 Subject: [PATCH 872/947] Fixed a special case hole that allowed negative values to be reported for hits on cylinders. --- BepuPhysics/Collidables/Cylinder.cs | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/BepuPhysics/Collidables/Cylinder.cs b/BepuPhysics/Collidables/Cylinder.cs index 8041c2d4e..0eadc9c06 100644 --- a/BepuPhysics/Collidables/Cylinder.cs +++ b/BepuPhysics/Collidables/Cylinder.cs @@ -136,7 +136,8 @@ public readonly bool RayTest(in RigidPose pose, Vector3 origin, Vector3 directio else { //The ray is parallel to the axis; the impact is on a disc or nothing. - discY = d.Y > 0 ? -HalfLength : HalfLength; + //If the ray is inside the cylinder, we want t = 0, so just set the discY to match the ray's origin in that case and it'll shake out like we want. + discY = float.MinMagnitude(d.Y > 0 ? -HalfLength : HalfLength, o.Y); } //Intersect the ray with the plane anchored at discY with normal equal to (0,1,0). From f215c14e12910d279ba4f4ddcc16407a4562464d Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 29 Nov 2024 12:04:53 -0600 Subject: [PATCH 873/947] Job count initialization fix, oops! --- BepuPhysics/IslandAwakener.cs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/BepuPhysics/IslandAwakener.cs b/BepuPhysics/IslandAwakener.cs index 331de0951..29b84e4f2 100644 --- a/BepuPhysics/IslandAwakener.cs +++ b/BepuPhysics/IslandAwakener.cs @@ -587,8 +587,7 @@ public void Dispose(BufferPool pool) //and the narrow phase pair cache. pairCache.Mapping.EnsureCapacity(pairCache.Mapping.Count + newPairCount, pool); - phaseOneJobs = new QuickList(Math.Max(32, highestNewBatchCount + 1), pool); - phaseTwoJobs = new QuickList(32, pool); + phaseOneJobs = new QuickList(Math.Max(32, highestNewBatchCount + 2), pool); //Finally, create actual jobs. Note that this involves actually allocating space in the bodies set and in type batches for the workers to fill in. //(Pair caches are currently handled in a locally sequential way and do not require preallocation.) phaseOneJobs.AllocateUnsafely() = new PhaseOneJob { Type = PhaseOneJobType.PairCache }; @@ -597,6 +596,7 @@ public void Dispose(BufferPool pool) { phaseOneJobs.AllocateUnsafely() = new PhaseOneJob { Type = PhaseOneJobType.UpdateBatchReferencedHandles, BatchIndex = batchIndex }; } + phaseTwoJobs = new QuickList(32, pool); phaseTwoJobs.AllocateUnsafely() = new PhaseTwoJob { Type = PhaseTwoJobType.BroadPhase }; ref var activeBodySet = ref bodies.ActiveSet; From b765bdc75da0a9065e9ec544cc749c87df3fb3a6 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 29 Nov 2024 12:08:01 -0600 Subject: [PATCH 874/947] Version bump. --- BepuPhysics/BepuPhysics.csproj | 2 +- BepuUtilities/BepuUtilities.csproj | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/BepuPhysics/BepuPhysics.csproj b/BepuPhysics/BepuPhysics.csproj index 0339cf7f9..0807aab31 100644 --- a/BepuPhysics/BepuPhysics.csproj +++ b/BepuPhysics/BepuPhysics.csproj @@ -1,7 +1,7 @@  net8.0 - 2.5.0-beta.23 + 2.5.0-beta.24 Bepu Entertainment LLC Ross Nordby Speedy real time physics simulation library. diff --git a/BepuUtilities/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj index 250b3658a..03ac78cb3 100644 --- a/BepuUtilities/BepuUtilities.csproj +++ b/BepuUtilities/BepuUtilities.csproj @@ -3,7 +3,7 @@ BepuUtilities BepuUtilities net8.0 - 2.5.0-beta.23 + 2.5.0-beta.24 Bepu Entertainment LLC Ross Nordby Supporting utilities library for BEPUphysics v2. From 54a922b5d52dfd61dfc78e802458efc4df6b84ed Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 29 Nov 2024 12:13:56 -0600 Subject: [PATCH 875/947] Bumping to .net 9 for demos. Library staying at 8 for now. --- DemoContentBuilder/DemoContentBuilder.csproj | 2 +- DemoContentLoader/DemoContentLoader.csproj | 2 +- DemoRenderer/DemoRenderer.csproj | 6 +++--- DemoTests/DemoTests.csproj | 2 +- DemoUtilities/DemoUtilities.csproj | 2 +- Demos/Demos.csproj | 6 +++--- 6 files changed, 10 insertions(+), 10 deletions(-) diff --git a/DemoContentBuilder/DemoContentBuilder.csproj b/DemoContentBuilder/DemoContentBuilder.csproj index 478c00e70..88f83d0c9 100644 --- a/DemoContentBuilder/DemoContentBuilder.csproj +++ b/DemoContentBuilder/DemoContentBuilder.csproj @@ -2,7 +2,7 @@ Exe - net8.0 + net9.0 x64 latest true diff --git a/DemoContentLoader/DemoContentLoader.csproj b/DemoContentLoader/DemoContentLoader.csproj index cc5df10c6..901c488a0 100644 --- a/DemoContentLoader/DemoContentLoader.csproj +++ b/DemoContentLoader/DemoContentLoader.csproj @@ -1,7 +1,7 @@  - net8.0 + net9.0 latest True diff --git a/DemoRenderer/DemoRenderer.csproj b/DemoRenderer/DemoRenderer.csproj index 31a5870ae..8fcbcf60b 100644 --- a/DemoRenderer/DemoRenderer.csproj +++ b/DemoRenderer/DemoRenderer.csproj @@ -1,13 +1,13 @@  - net8.0 + net9.0 latest True - - + + diff --git a/DemoTests/DemoTests.csproj b/DemoTests/DemoTests.csproj index a1be5c215..05d93a15f 100644 --- a/DemoTests/DemoTests.csproj +++ b/DemoTests/DemoTests.csproj @@ -1,7 +1,7 @@ - net8.0 + net9.0 true false latest diff --git a/DemoUtilities/DemoUtilities.csproj b/DemoUtilities/DemoUtilities.csproj index eab8c0879..1bc2fc4ed 100644 --- a/DemoUtilities/DemoUtilities.csproj +++ b/DemoUtilities/DemoUtilities.csproj @@ -1,7 +1,7 @@  - net8.0 + net9.0 latest true diff --git a/Demos/Demos.csproj b/Demos/Demos.csproj index f47e2710a..d52b685f3 100644 --- a/Demos/Demos.csproj +++ b/Demos/Demos.csproj @@ -1,7 +1,7 @@  Exe - net8.0 + net9.0 True Debug;Release latest @@ -10,8 +10,8 @@ - - + + From 89b6b9acde76f8d2ed490d4e1f1d3ef56c9a4394 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 29 Nov 2024 12:17:43 -0600 Subject: [PATCH 876/947] Github actions. --- .github/workflows/dotnet-core.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/dotnet-core.yml b/.github/workflows/dotnet-core.yml index 164d97ad8..14f477385 100644 --- a/.github/workflows/dotnet-core.yml +++ b/.github/workflows/dotnet-core.yml @@ -11,7 +11,7 @@ jobs: - uses: actions/checkout@v2 - uses: actions/setup-dotnet@v1 with: - dotnet-version: '8.x' + dotnet-version: '9.x' include-prerelease: true - name: Install dependencies run: | From ec5082f1839a50b8aff07354483994d6b4bad73b Mon Sep 17 00:00:00 2001 From: Friendly Chicken <72442889+FriendlyChicken@users.noreply.github.com> Date: Thu, 5 Dec 2024 10:34:27 -0800 Subject: [PATCH 877/947] Bump Demos.GL to .NET 9 --- DemoRenderer.GL/DemoRenderer.csproj | 2 +- Demos.GL/Demos.csproj | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) mode change 100644 => 100755 DemoRenderer.GL/DemoRenderer.csproj mode change 100644 => 100755 Demos.GL/Demos.csproj diff --git a/DemoRenderer.GL/DemoRenderer.csproj b/DemoRenderer.GL/DemoRenderer.csproj old mode 100644 new mode 100755 index 658c2ffd1..2f25a39f9 --- a/DemoRenderer.GL/DemoRenderer.csproj +++ b/DemoRenderer.GL/DemoRenderer.csproj @@ -1,6 +1,6 @@  - net8.0 + net9.0 latest True diff --git a/Demos.GL/Demos.csproj b/Demos.GL/Demos.csproj old mode 100644 new mode 100755 index c796aad45..f4d9b1c9b --- a/Demos.GL/Demos.csproj +++ b/Demos.GL/Demos.csproj @@ -1,7 +1,7 @@  Exe - net8.0 + net9.0 True Debug;Release latest From 33015141aa04ad90c31cde9f6a1d56d84f42e66f Mon Sep 17 00:00:00 2001 From: Vaclav Elias Date: Mon, 16 Dec 2024 22:20:36 +0000 Subject: [PATCH 878/947] ci: Add release.yml for changelog generation - Added a new `release.yml` file to the `.github` directory. - Configures the changelog generation for releases. - Specifies labels to exclude from the changelog, such as `ignore-for-release` and contributions from `dependabot`. - Categorizes the changelog into several sections --- .github/release.yml | 31 +++++++++++++++++++++++++++++++ 1 file changed, 31 insertions(+) create mode 100644 .github/release.yml diff --git a/.github/release.yml b/.github/release.yml new file mode 100644 index 000000000..fd4519a54 --- /dev/null +++ b/.github/release.yml @@ -0,0 +1,31 @@ +# .github/release.yml + +changelog: + exclude: + labels: + - ignore-for-release + authors: + - dependabot + categories: + - title: 💥 Breaking Changes + labels: + - breaking-change + - title: 🎉 New Features + labels: + - enhancement + - new-feature + - title: 🚀 Performance + labels: + - performance + - title: 🧠 Fiddling + labels: + - fiddling + - title: 🎮 Demos App + labels: + - demos + - title: 📄 Documentation + labels: + - documentation + - title: 💪 Other Changes + labels: + - "*" \ No newline at end of file From 0126ed577e0329a310f0d91d3dda232f56aee6ee Mon Sep 17 00:00:00 2001 From: Vaclav Elias Date: Tue, 17 Dec 2024 23:43:10 +0000 Subject: [PATCH 879/947] refactor!: Consolidate common project settings - Moved common properties and settings from `BepuPhysics.csproj` and `BepuUtilities.csproj` to a shared `CommonSettings.props` file. --- BepuPhysics/BepuPhysics.csproj | 32 +++------------------------ BepuUtilities/BepuUtilities.csproj | 35 +++--------------------------- CommonSettings.props | 35 ++++++++++++++++++++++++++++++ 3 files changed, 41 insertions(+), 61 deletions(-) create mode 100644 CommonSettings.props diff --git a/BepuPhysics/BepuPhysics.csproj b/BepuPhysics/BepuPhysics.csproj index 0807aab31..6d9803f7b 100644 --- a/BepuPhysics/BepuPhysics.csproj +++ b/BepuPhysics/BepuPhysics.csproj @@ -1,28 +1,9 @@ - + - net8.0 - 2.5.0-beta.24 - Bepu Entertainment LLC - Ross Nordby Speedy real time physics simulation library. - © Bepu Entertainment LLC - https://github.com/bepu/bepuphysics2 - Apache-2.0 - https://github.com/bepu/bepuphysics2 - bepuphysicslogo256.png Debug;Release;ReleaseNoProfiling - latest physics;3d;rigid body;real time;simulation - True - true - - true - false key.snk - true - - true - snupkg @@ -30,6 +11,8 @@ 1573;1591;CA2014 + + false TRACE;DEBUG;CHECKMATH;PROFILE @@ -84,15 +67,6 @@ TextTemplatingFileGenerator ContactNonconvexTypes.cs - - True - - - - - - - \ No newline at end of file diff --git a/BepuUtilities/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj index 03ac78cb3..5b7eb7088 100644 --- a/BepuUtilities/BepuUtilities.csproj +++ b/BepuUtilities/BepuUtilities.csproj @@ -1,35 +1,18 @@ - + BepuUtilities BepuUtilities - net8.0 - 2.5.0-beta.24 - Bepu Entertainment LLC - Ross Nordby Supporting utilities library for BEPUphysics v2. - © Bepu Entertainment LLC - https://github.com/bepu/bepuphysics2 - Apache-2.0 - https://github.com/bepu/bepuphysics2 - bepuphysicslogo256.png Debug;Release - latest - True - true - - true - false key.snk - true - - true - snupkg 1573;1591 + + false TRACE;DEBUG;CHECKMATH @@ -39,17 +22,5 @@ true TRACE;RELEASE - - - - True - - - - - - - - \ No newline at end of file diff --git a/CommonSettings.props b/CommonSettings.props new file mode 100644 index 000000000..88dfbf27f --- /dev/null +++ b/CommonSettings.props @@ -0,0 +1,35 @@ + + + net8.0 + 2.5.0-beta.24 + Bepu Entertainment LLC + Ross Nordby + © Bepu Entertainment LLC + https://github.com/bepu/bepuphysics2 + https://github.com/bepu/bepuphysics2 + Apache-2.0 + latest + bepuphysicslogo256.png + True + true + + true + false + true + + true + snupkg + + + + + True + + + + + + + + + \ No newline at end of file From cfa0d5cb3ccc67e3407f50b5e6fee32046457c03 Mon Sep 17 00:00:00 2001 From: Vaclav Elias Date: Sun, 29 Dec 2024 18:25:53 +0000 Subject: [PATCH 880/947] ci: Update GitHub Actions workflow - Changed the workflow name from ".NET Core" to ".NET Core - Build and Test". - Updated `actions/checkout` action from version 2 to version 4. - Updated `actions/setup-dotnet` action from version 1 to version 4. - Commented out the `Publish` step, which included adding a NuGet source, packing projects, and pushing the packages to GitHub and NuGet. --- .github/workflows/dotnet-core.yml | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/.github/workflows/dotnet-core.yml b/.github/workflows/dotnet-core.yml index 14f477385..c43c32df4 100644 --- a/.github/workflows/dotnet-core.yml +++ b/.github/workflows/dotnet-core.yml @@ -1,4 +1,4 @@ -name: .NET Core +name: .NET Core - Build and Test on: [push, pull_request] @@ -8,8 +8,8 @@ jobs: runs-on: windows-latest steps: - - uses: actions/checkout@v2 - - uses: actions/setup-dotnet@v1 + - uses: actions/checkout@v4 + - uses: actions/setup-dotnet@v4 with: dotnet-version: '9.x' include-prerelease: true @@ -23,11 +23,11 @@ jobs: dotnet build Demos --configuration Release --no-restore - name: Test run: dotnet test DemoTests -c Release --verbosity normal - - name: Publish - if: github.event_name != 'pull_request' - run: | - dotnet nuget add source "https://nuget.pkg.github.com/bepu/index.json" --name "github" --username "rossnordby" --password "${{secrets.GITHUB_TOKEN}}" - dotnet pack "BepuPhysics" -c Release - dotnet pack "BepuUtilities" -c Release - dotnet nuget push "**/*.nupkg" -s "github" -k "${{secrets.GITHUB_TOKEN}}" --skip-duplicate - dotnet nuget push "**/*.nupkg" -s "https://api.nuget.org/v3/index.json" -k "${{secrets.NUGET_KEY}}" --skip-duplicate \ No newline at end of file +# - name: Publish +# if: github.event_name != 'pull_request' +# run: | +# dotnet nuget add source "https://nuget.pkg.github.com/bepu/index.json" --name "github" --username "rossnordby" --password "${{secrets.GITHUB_TOKEN}}" +# dotnet pack "BepuPhysics" -c Release +# dotnet pack "BepuUtilities" -c Release +# dotnet nuget push "**/*.nupkg" -s "github" -k "${{secrets.GITHUB_TOKEN}}" --skip-duplicate +# dotnet nuget push "**/*.nupkg" -s "https://api.nuget.org/v3/index.json" -k "${{secrets.NUGET_KEY}}" --skip-duplicate \ No newline at end of file From a1d6cd36bd623566a82c8fff7ceba950351ecc9e Mon Sep 17 00:00:00 2001 From: Vaclav Elias Date: Sun, 29 Dec 2024 18:29:40 +0000 Subject: [PATCH 881/947] ci: Add .NET Core workflow for publishing NuGet packages - New: Add a new GitHub Actions workflow named ".NET Core - Publish NuGet Packages". - New: Set environment variables `COMMON_SETTINGS_PATH` and `BASE_RUN_NUMBER`. - New: Trigger the workflow manually using `workflow_dispatch`. - New: Print the GitHub run number. - New: Set a version number using the GitHub run number and `BASE_RUN_NUMBER`, and store it in the environment variable `VERSION`. - New: Print the `VERSION` environment variable. - All steps remains the same as original GitHub Action - New: Create a GitHub release draft with the new version number and release notes. --- .github/workflows/dotnet-core-publish.yml | 56 +++++++++++++++++++++++ 1 file changed, 56 insertions(+) create mode 100644 .github/workflows/dotnet-core-publish.yml diff --git a/.github/workflows/dotnet-core-publish.yml b/.github/workflows/dotnet-core-publish.yml new file mode 100644 index 000000000..b191d9943 --- /dev/null +++ b/.github/workflows/dotnet-core-publish.yml @@ -0,0 +1,56 @@ +name: .NET Core - Publish NuGet Packages + +env: + COMMON_SETTINGS_PATH: CommonSettings.props + BASE_RUN_NUMBER: 25 + +on: [workflow_dispatch] + +jobs: + build: + + runs-on: windows-latest + + steps: + - name: Print run_number + run: echo ${{ github.run_number }} + - name: Set version number + run: | + $version = "2.5.0-beta.$(${{ github.run_number }} + $env:BASE_RUN_NUMBER)" + echo "VERSION=$version" >> $env:GITHUB_ENV + shell: powershell + - name: Print VERSION + run: echo "VERSION is $env:VERSION" + - uses: actions/checkout@v4 + - uses: actions/setup-dotnet@v4 + with: + dotnet-version: '9.x' + include-prerelease: true + - name: Set Version in CommonSettings.props + run: | + $settingsContent = Get-Content -Path ${{ env.COMMON_SETTINGS_PATH }} -Raw + $updatedContent = $settingsContent -replace '.*?', "${{ env.VERSION }}" + Set-Content -Path ${{ env.COMMON_SETTINGS_PATH }} -Value $updatedContent + - name: Install dependencies + run: | + dotnet restore DemoContentBuilder + dotnet restore Demos + - name: Build + run: | + dotnet build DemoContentBuilder --configuration Release --no-restore /p:Platform=x64 + dotnet build Demos --configuration Release --no-restore + - name: Test + run: dotnet test DemoTests -c Release --verbosity normal + - name: Publish + if: github.event_name != 'pull_request' + run: | + dotnet nuget add source "https://nuget.pkg.github.com/bepu/index.json" --name "github" --username "rossnordby" --password "${{secrets.GITHUB_TOKEN}}" + dotnet pack "BepuPhysics" -c Release + dotnet pack "BepuUtilities" -c Release + dotnet nuget push "**/*.nupkg" -s "github" -k "${{secrets.GITHUB_TOKEN}}" --skip-duplicate + dotnet nuget push "**/*.nupkg" -s "https://api.nuget.org/v3/index.json" -k "${{secrets.NUGET_KEY}}" --skip-duplicate + - name: Create GitHub Release Draft + run: | + gh release create ${{ env.VERSION }} --title "v${{ env.VERSION }}" --notes "Release notes for ${{ env.VERSION }}" --draft + env: + GH_TOKEN: ${{ secrets.GITHUB_TOKEN }} \ No newline at end of file From 1772bb6cdf3598b810b59016e6c082fd35934bc9 Mon Sep 17 00:00:00 2001 From: Vaclav Elias Date: Sun, 29 Dec 2024 22:17:20 +0000 Subject: [PATCH 882/947] feat: Add ignore paths so the GitHub Actions isn't triggered when not needed --- .github/workflows/dotnet-core.yml | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/.github/workflows/dotnet-core.yml b/.github/workflows/dotnet-core.yml index c43c32df4..4c4bce2ff 100644 --- a/.github/workflows/dotnet-core.yml +++ b/.github/workflows/dotnet-core.yml @@ -1,6 +1,14 @@ name: .NET Core - Build and Test -on: [push, pull_request] +on: + push: + paths-ignore: + - '.github/**' + - 'Documentation/**' + pull_request: + paths-ignore: + - '.github/**' + - 'Documentation/**' jobs: build: @@ -30,4 +38,4 @@ jobs: # dotnet pack "BepuPhysics" -c Release # dotnet pack "BepuUtilities" -c Release # dotnet nuget push "**/*.nupkg" -s "github" -k "${{secrets.GITHUB_TOKEN}}" --skip-duplicate -# dotnet nuget push "**/*.nupkg" -s "https://api.nuget.org/v3/index.json" -k "${{secrets.NUGET_KEY}}" --skip-duplicate \ No newline at end of file +# dotnet nuget push "**/*.nupkg" -s "https://api.nuget.org/v3/index.json" -k "${{secrets.NUGET_KEY}}" --skip-duplicate From 2b785b823244d12ef30dd028d4754d14b17dcb86 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 2 Oct 2024 00:02:30 -0500 Subject: [PATCH 883/947] In the middle of fixing ye olde convex hull bug with a drizzling of brute force. Works now, but needs some cleanup, debug improvements, and regression testing. --- BepuPhysics/Collidables/ConvexHullHelper.cs | 507 +++++++++---------- Demos/DemoSet.cs | 2 + Demos/SpecializedTests/ConvexHullTestDemo.cs | 500 ++++++++++++------ 3 files changed, 572 insertions(+), 437 deletions(-) diff --git a/BepuPhysics/Collidables/ConvexHullHelper.cs b/BepuPhysics/Collidables/ConvexHullHelper.cs index a186a08f1..c09e075b0 100644 --- a/BepuPhysics/Collidables/ConvexHullHelper.cs +++ b/BepuPhysics/Collidables/ConvexHullHelper.cs @@ -472,60 +472,6 @@ public override string ToString() } } - /// - /// Tracks the faces associated with a detected surface edge. - /// During hull calculation, surface edges that have only one face associated with them should have an outstanding entry in the edges to visit set. - /// Surface edges can never validly have more than two faces associated with them. Two means that an edge is 'complete', or should be. - /// Detecting a third edge implies an error condition. By tracking *which* faces were associated with an edge, we can attempt to fix the error. - /// This type of error tends to occur when there is a numerical disagreement about what vertices are coplanar with a face. - /// One iteration could find what it thinks is a complete face, and a later iteration ends up finding more vertices *including* the ones already contained in the previous face. - /// That's a recipe for excessive edge faces, but it's also a direct indicator that we should merge the involved faces for being close enough to coplanar that the error happened in the first place. - /// - struct EdgeFaceIndices - { - public int FaceA; - public int FaceB; - public bool Complete => FaceA >= 0 && FaceB >= 0; - - public EdgeFaceIndices(int initialFaceIndex) - { - FaceA = initialFaceIndex; - FaceB = -1; - } - - - } - - static void RemoveFaceFromEdge(int a, int b, int faceIndex, ref QuickDictionary facesForEdges) - { - EdgeEndpoints edge; - edge.A = a; - edge.B = b; - var exists = facesForEdges.GetTableIndices(ref edge, out _, out var index); - Debug.Assert(exists, "Whoa something weird happened here! A face was created, but there's a missing edge for its face?"); - ref var facesForEdge = ref facesForEdges.Values[index]; - Debug.Assert(faceIndex == facesForEdge.FaceA || faceIndex == facesForEdge.FaceB, "If you're trying to remove a face index from the edge, it better be in the edge!"); - if (facesForEdge.FaceA == faceIndex) - { - facesForEdge.FaceA = facesForEdge.FaceB; - facesForEdge.FaceB = -1; - if (facesForEdge.FaceA == -1) - { - //This edge no longer has any faces associated with it. - facesForEdges.FastRemove(ref edge); - } - } - else - { - facesForEdge.FaceB = -1; - } - //Note that we do *not* add the edge back into the 'edges to test' list when transitioning from 2 faces to 1 face for the edge. - //This function is invoked when an edge is being deleted because it's related to a deleted face. - //There are two possible outcomes: - //1. The completion of the merge will see a face added back to this edge, - //2. it's an orphaned internal edge that should not be tested. - } - internal struct EarlyFace { public QuickList VertexIndices; @@ -548,13 +494,12 @@ static void AddFace(ref QuickList faces, BufferPool pool, Vector3 nor face.VertexIndices.AddRangeUnsafely(vertexIndices); } - static void AddFaceToEdgesAndTestList(BufferPool pool, + static void AddFaceEdgesToTestList(BufferPool pool, ref QuickList reducedFaceIndices, ref QuickList edgesToTest, - ref QuickDictionary facesForEdges, + ref QuickSet submittedEdgeTests, Vector3 faceNormal, int newFaceIndex) { - facesForEdges.EnsureCapacity(facesForEdges.Count + reducedFaceIndices.Count, pool); var previousIndex = reducedFaceIndices[reducedFaceIndices.Count - 1]; for (int i = 0; i < reducedFaceIndices.Count; ++i) { @@ -562,27 +507,12 @@ static void AddFaceToEdgesAndTestList(BufferPool pool, endpoints.A = previousIndex; endpoints.B = reducedFaceIndices[i]; previousIndex = endpoints.B; - if (facesForEdges.GetTableIndices(ref endpoints, out var tableIndex, out var elementIndex)) - { - ref var edgeFaceCount = ref facesForEdges.Values[elementIndex]; - Debug.Assert(edgeFaceCount.FaceB == -1, - "While we let execution continue, this is an error condition and implies overlapping triangles are being generated." + - "This tends to happen when there are many near-coplanar vertices, so numerical tolerances across different faces cannot consistently agree."); - edgeFaceCount.FaceB = newFaceIndex; - } - else - { - //This edge is not yet claimed by any edge. Claim it for the new face and add the edge for further testing. - EdgeToTest nextEdgeToTest; - nextEdgeToTest.Endpoints = endpoints; - nextEdgeToTest.FaceNormal = faceNormal; - nextEdgeToTest.FaceIndex = newFaceIndex; - facesForEdges.Keys[facesForEdges.Count] = nextEdgeToTest.Endpoints; - facesForEdges.Values[facesForEdges.Count] = new EdgeFaceIndices(newFaceIndex); - //Use the encoding- all indices are offset by 1 since 0 represents 'empty'. - facesForEdges.Table[tableIndex] = ++facesForEdges.Count; - edgesToTest.Allocate(pool) = nextEdgeToTest; - } + EdgeToTest nextEdgeToTest; + nextEdgeToTest.Endpoints = endpoints; + nextEdgeToTest.FaceNormal = faceNormal; + nextEdgeToTest.FaceIndex = newFaceIndex; + edgesToTest.Allocate(pool) = nextEdgeToTest; + submittedEdgeTests.Add(endpoints, pool); } } @@ -592,70 +522,70 @@ static void AddIfNotPresent(ref QuickList list, int value, BufferPool pool) list.Allocate(pool) = value; } - //public struct DebugStep - //{ - // public EdgeEndpoints SourceEdge; - // public List Raw; - // public List Reduced; - // public bool[] AllowVertex; - // public Vector3 FaceNormal; - // public Vector3 BasisX; - // public Vector3 BasisY; - // public List FaceStarts; - // public List FaceIndices; - // public bool[] FaceDeleted; - // public int[] MergedFaceIndices; - // public int FaceIndex; - // public Vector3[] FaceNormals; - - // internal DebugStep(EdgeEndpoints sourceEdge, ref QuickList raw, Vector3 faceNormal, Vector3 basisX, Vector3 basisY, ref QuickList reduced, ref Buffer allowVertex, ref QuickList faces, Span mergedFaceIndices, int faceIndex) - // { - // SourceEdge = sourceEdge; - // FaceNormal = faceNormal; - // BasisX = basisX; - // BasisY = basisY; - // Raw = new List(); - // for (int i = 0; i < raw.Count; ++i) - // { - // Raw.Add(raw[i]); - // } - // Reduced = new List(); - // for (int i = 0; i < reduced.Count; ++i) - // { - // Reduced.Add(reduced[i]); - // } - // AllowVertex = new bool[allowVertex.Length]; - // for (int i = 0; i < allowVertex.Length; ++i) - // { - // AllowVertex[i] = allowVertex[i] != 0; - // } - // FaceStarts = new List(faces.Count); - // FaceIndices = new List(); - // FaceDeleted = new bool[faces.Count]; - // FaceNormals = new Vector3[faces.Count]; - // for (int i = 0; i < faces.Count; ++i) - // { - // ref var face = ref faces[i]; - // FaceStarts.Add(FaceIndices.Count); - // for (int j = 0; j < face.VertexIndices.Count; ++j) - // FaceIndices.Add(face.VertexIndices[j]); - // FaceDeleted[i] = face.Deleted; - // FaceNormals[i] = face.Normal; - // } - // MergedFaceIndices = mergedFaceIndices.ToArray(); - // FaceIndex = faceIndex; - // } - //} - ///// - ///// Computes the convex hull of a set of points. - ///// - ///// Point set to compute the convex hull of. - ///// Buffer pool to pull memory from when creating the hull. - ///// Convex hull of the input point set. - //public static void ComputeHull(Span points, BufferPool pool, out HullData hullData) - //{ - // ComputeHull(points, pool, out hullData, out _); - //} + public struct DebugStep + { + public EdgeEndpoints SourceEdge; + public List Raw; + public List Reduced; + public bool[] AllowVertex; + public Vector3 FaceNormal; + public Vector3 BasisX; + public Vector3 BasisY; + public List FaceStarts; + public List FaceIndices; + public bool[] FaceDeleted; + public int[] MergedFaceIndices; + public int FaceIndex; + public Vector3[] FaceNormals; + + internal DebugStep(EdgeEndpoints sourceEdge, ref QuickList raw, Vector3 faceNormal, Vector3 basisX, Vector3 basisY, ref QuickList reduced, ref Buffer allowVertex, ref QuickList faces, Span mergedFaceIndices, int faceIndex) + { + SourceEdge = sourceEdge; + FaceNormal = faceNormal; + BasisX = basisX; + BasisY = basisY; + Raw = new List(); + for (int i = 0; i < raw.Count; ++i) + { + Raw.Add(raw[i]); + } + Reduced = new List(); + for (int i = 0; i < reduced.Count; ++i) + { + Reduced.Add(reduced[i]); + } + AllowVertex = new bool[allowVertex.Length]; + for (int i = 0; i < allowVertex.Length; ++i) + { + AllowVertex[i] = allowVertex[i] != 0; + } + FaceStarts = new List(faces.Count); + FaceIndices = new List(); + FaceDeleted = new bool[faces.Count]; + FaceNormals = new Vector3[faces.Count]; + for (int i = 0; i < faces.Count; ++i) + { + ref var face = ref faces[i]; + FaceStarts.Add(FaceIndices.Count); + for (int j = 0; j < face.VertexIndices.Count; ++j) + FaceIndices.Add(face.VertexIndices[j]); + FaceDeleted[i] = face.Deleted; + FaceNormals[i] = face.Normal; + } + MergedFaceIndices = mergedFaceIndices.ToArray(); + FaceIndex = faceIndex; + } + } + /// + /// Computes the convex hull of a set of points. + /// + /// Point set to compute the convex hull of. + /// Buffer pool to pull memory from when creating the hull. + /// Convex hull of the input point set. + public static void ComputeHull(Span points, BufferPool pool, out HullData hullData) + { + ComputeHull(points, pool, out hullData, out _); + } /// @@ -664,9 +594,9 @@ static void AddIfNotPresent(ref QuickList list, int value, BufferPool pool) /// Point set to compute the convex hull of. /// Buffer pool to pull memory from when creating the hull. /// Convex hull of the input point set. - public static void ComputeHull(Span points, BufferPool pool, out HullData hullData)//, out List steps) + public static void ComputeHull(Span points, BufferPool pool, out HullData hullData, out List steps) { - //steps = new List(); + steps = new List(); if (points.Length <= 0) { hullData = default; @@ -789,7 +719,7 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa var faces = new QuickList(points.Length, pool); var edgesToTest = new QuickList(points.Length, pool); - var facesForEdges = new QuickDictionary(points.Length, pool); + var submittedEdgeTests = new QuickSet(points.Length, pool); var facesNeedingMerge = new QuickList(32, pool); if (reducedFaceIndices.Count >= 3) { @@ -802,7 +732,6 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa edgeToAdd.Endpoints.B = reducedFaceIndices[i]; edgeToAdd.FaceNormal = initialFaceNormal; edgeToAdd.FaceIndex = 0; - facesForEdges.Add(ref edgeToAdd.Endpoints, new EdgeFaceIndices(0), pool); } //Since an actual face was found, we go ahead and output it into the face set. AddFace(ref faces, pool, initialFaceNormal, reducedFaceIndices); @@ -823,19 +752,15 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa if (Vector3.Dot(basisX, edgeToAdd.FaceNormal) > 0) Helpers.Swap(ref edgeToAdd.Endpoints.A, ref edgeToAdd.Endpoints.B); } - //Vector3Wide.ReadFirst(initialBasisX, out var debugInitialBasisX); - //Vector3Wide.ReadFirst(initialBasisY, out var debugInitialBasisY); - //steps.Add(new DebugStep(initialSourceEdge, ref rawFaceVertexIndices, initialFaceNormal, debugInitialBasisX, debugInitialBasisY, ref reducedFaceIndices, ref allowVertices, ref faces, default, reducedFaceIndices.Count >= 3 ? 0 : -1)); + Vector3Wide.ReadFirst(initialBasisX, out var debugInitialBasisX); + Vector3Wide.ReadFirst(initialBasisY, out var debugInitialBasisY); + steps.Add(new DebugStep(initialSourceEdge, ref rawFaceVertexIndices, initialFaceNormal, debugInitialBasisX, debugInitialBasisY, ref reducedFaceIndices, ref allowVertices, ref faces, default, reducedFaceIndices.Count >= 3 ? 0 : -1)); int facesDeletedCount = 0; while (edgesToTest.Count > 0) { edgesToTest.Pop(out var edgeToTest); - //Make sure the new edge hasn't already been filled by another traversal. - var faceCountIndex = facesForEdges.IndexOf(edgeToTest.Endpoints); - if (faceCountIndex >= 0 && facesForEdges.Values[faceCountIndex].Complete) - continue; ref var edgeA = ref points[edgeToTest.Endpoints.A]; ref var edgeB = ref points[edgeToTest.Endpoints.B]; @@ -858,140 +783,184 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa if (reducedFaceIndices.Count < 3) { - //steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertices, ref faces, default, -1)); + steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertices, ref faces, default, -1)); //Degenerate face found; don't bother creating work for it. continue; } - var faceCountPriorToAdd = faces.Count; - - facesNeedingMerge.Count = 0; - while (true) + // Brute force scan all the faces to see if the new face is coplanar with any of them. + Console.WriteLine($"step count: {steps.Count}"); + bool mergedFace = false; + for (int i = 0; i < faces.Count; ++i) { - //This implementation bites the bullet pretty hard on numerical problems. They most frequently arise from near coplanar vertices. - //It's possible that two iterations see different subsets of 'coplanar' vertices, either causing two faces with near equal normal or - //even causing an edge to have more than two faces associated with it. - //Before making any modifications to the existing data, iterate over all the edges in the current face to check for any such edges. - - //While the usual flow here will be to reduce the vertices we just found and accept the result immediately, - //it's possible for the just-detected face to end up coplanar with an existing face because of numerical issues that prevented detecting the whole face earlier. - //Rather than using more precise math, we detect the failure and merge faces after the fact. - //Unfortunately, this can happen recursively! Merging two redundant faces could reveal a third face that also needs to be merged. - //This is uncommon, but it needs to be covered. - //So, we just stick everything into a while loop and let it iterate until it's done. - //The good news is that the merging process won't loop forever- every merge results in a face that is at least as large as any contributor, and any internal points - //that get reduced out of consideration are marked with allowVertex[whateverInteriorPointIndex] = false. - int previousFaceMergeCount = facesNeedingMerge.Count; - var previousEndpointIndex = reducedFaceIndices[reducedFaceIndices.Count - 1]; - for (int i = 0; i < reducedFaceIndices.Count; ++i) + ref var face = ref faces[i]; + if (face.Deleted) + continue; + if (Vector3.Dot(face.Normal, faceNormal) > normalCoplanarityEpsilon) { - EdgeEndpoints edgeEndpoints; - edgeEndpoints.A = previousEndpointIndex; - edgeEndpoints.B = reducedFaceIndices[i]; - previousEndpointIndex = edgeEndpoints.B; - - if (facesForEdges.GetTableIndices(ref edgeEndpoints, out var tableIndex, out var elementIndex)) + Console.WriteLine($"Merging face {i} with new face, dot {Vector3.Dot(face.Normal, faceNormal)}:"); + Console.WriteLine($"Existing face: {face.Normal}"); + Console.WriteLine($"Candidate: {faceNormal}"); + // The new face is coplanar with an existing face. Merge the new face into the old face. + rawFaceVertexIndices.EnsureCapacity(reducedFaceIndices.Count + face.VertexIndices.Count, pool); + rawFaceVertexIndices.Count = reducedFaceIndices.Count; + reducedFaceIndices.Span.CopyTo(0, rawFaceVertexIndices.Span, 0, reducedFaceIndices.Count); + for (int j = 0; j < face.VertexIndices.Count; ++j) { - //There is already at least one face associated with this edge. Do we need to merge? - ref var edgeFaces = ref facesForEdges.Values[elementIndex]; - Debug.Assert(edgeFaces.FaceA >= 0); - var aDot = Vector3.Dot(faces[edgeFaces.FaceA].Normal, faceNormal); - if (edgeFaces.FaceB >= 0) + var vertexIndex = face.VertexIndices[j]; + // Only testing the original set of reduced face indices for duplicates when merging; we know the face's point set isn't redundant. + if (allowVertices[vertexIndex] != 0 && !reducedFaceIndices.Contains(vertexIndex)) { - //The edge already has two faces associated with it. This is definitely a numerical error; separate coplanar faces were generated. - //We *must* merge at least one pair of faces. - //Note that technically both faces can end up being included, even though we've already tested A and B; - //the new face could be a bridge that's close enough to both, even if they were barely too far from each other. - var bDot = Vector3.Dot(faces[edgeFaces.FaceB].Normal, faceNormal); - if (aDot >= normalCoplanarityEpsilon && bDot >= normalCoplanarityEpsilon) - { - AddIfNotPresent(ref facesNeedingMerge, edgeFaces.FaceA, pool); - AddIfNotPresent(ref facesNeedingMerge, edgeFaces.FaceB, pool); - } - else - { - //If both aren't merge candidates, then just pick the one that's closer. - AddIfNotPresent(ref facesNeedingMerge, aDot > bDot ? edgeFaces.FaceA : edgeFaces.FaceB, pool); - } - } - else - { - //Only one face already present. Check if it's coplanar. - if (aDot >= normalCoplanarityEpsilon) - AddIfNotPresent(ref facesNeedingMerge, edgeFaces.FaceA, pool); + rawFaceVertexIndices.AllocateUnsafely() = vertexIndex; } } + // Rerun reduction for the merged face. + face.VertexIndices.Count = 0; + facePoints.Count = 0; + //steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertices, ref faces, facesNeedingMerge, i)); + face.VertexIndices.EnsureCapacity(rawFaceVertexIndices.Count, pool); + ReduceFace(ref rawFaceVertexIndices, faceNormal, points, planeEpsilonNarrow, ref facePoints, ref allowVertices, ref face.VertexIndices); + mergedFace = true; + break; } + } + var faceCountPriorToAdd = faces.Count; - if (facesNeedingMerge.Count > previousFaceMergeCount) - { - //This iteration has found more faces which should be merged together. - //Accumulate the vertices. - for (int i = previousFaceMergeCount; i < facesNeedingMerge.Count; ++i) - { - ref var sourceFace = ref faces[facesNeedingMerge[i]]; - for (int j = 0; j < sourceFace.VertexIndices.Count; ++j) - { - var vertexIndex = sourceFace.VertexIndices[j]; - if (allowVertices[vertexIndex] != 0 && !rawFaceVertexIndices.Contains(vertexIndex)) - { - rawFaceVertexIndices.Allocate(pool) = vertexIndex; - } - } - } + //facesNeedingMerge.Count = 0; + //while (true) + //{ + // //This implementation bites the bullet pretty hard on numerical problems. They most frequently arise from near coplanar vertices. + // //It's possible that two iterations see different subsets of 'coplanar' vertices, either causing two faces with near equal normal or + // //even causing an edge to have more than two faces associated with it. + // //Before making any modifications to the existing data, iterate over all the edges in the current face to check for any such edges. + + // //While the usual flow here will be to reduce the vertices we just found and accept the result immediately, + // //it's possible for the just-detected face to end up coplanar with an existing face because of numerical issues that prevented detecting the whole face earlier. + // //Rather than using more precise math, we detect the failure and merge faces after the fact. + // //Unfortunately, this can happen recursively! Merging two redundant faces could reveal a third face that also needs to be merged. + // //This is uncommon, but it needs to be covered. + // //So, we just stick everything into a while loop and let it iterate until it's done. + // //The good news is that the merging process won't loop forever- every merge results in a face that is at least as large as any contributor, and any internal points + // //that get reduced out of consideration are marked with allowVertex[whateverInteriorPointIndex] = false. + // int previousFaceMergeCount = facesNeedingMerge.Count; + // var previousEndpointIndex = reducedFaceIndices[reducedFaceIndices.Count - 1]; + // for (int i = 0; i < reducedFaceIndices.Count; ++i) + // { + // EdgeEndpoints edgeEndpoints; + // edgeEndpoints.A = previousEndpointIndex; + // edgeEndpoints.B = reducedFaceIndices[i]; + // previousEndpointIndex = edgeEndpoints.B; - //Re-reduce. - reducedFaceIndices.Count = 0; - facePoints.Count = 0; - ReduceFace(ref rawFaceVertexIndices, faceNormal, points, planeEpsilonNarrow, ref facePoints, ref allowVertices, ref reducedFaceIndices); - } - else - { - //No more faces need to be merged! Go ahead and apply the changes. - for (int i = 0; i < facesNeedingMerge.Count; ++i) - { - //Note that we do not merge into an existing face; for simplicity, we just merge into the face we're going to append and mark the old faces as being deleted. - //Note that we do *not* remove deleted faces from the faces list. That would break all the face indices that we've accumulated. - var faceIndex = facesNeedingMerge[i]; - ref var faceToRemove = ref faces[faceIndex]; - faceToRemove.Deleted = true; - - //For every edge of any face we're merging, remove the face from the edge lists. - var previousIndex = faceToRemove.VertexIndices[faceToRemove.VertexIndices.Count - 1]; - for (int j = 0; j < faceToRemove.VertexIndices.Count; ++j) - { - RemoveFaceFromEdge(previousIndex, faceToRemove.VertexIndices[j], faceIndex, ref facesForEdges); - previousIndex = faceToRemove.VertexIndices[j]; - } - //Remove any references to this face in the edges to test. - int removedEdgesToTestCount = 0; - for (int j = 0; j < edgesToTest.Count; ++j) - { - if (edgesToTest[j].FaceIndex == faceIndex) - { - ++removedEdgesToTestCount; - } - else if (removedEdgesToTestCount > 0) - { - //Scoot edges to test back. - edgesToTest[j - removedEdgesToTestCount] = edgesToTest[j]; - } - } - edgesToTest.Count -= removedEdgesToTestCount; - } + // if (facesForEdges.GetTableIndices(ref edgeEndpoints, out var tableIndex, out var elementIndex)) + // { + // //There is already at least one face associated with this edge. Do we need to merge? + // ref var edgeFaces = ref facesForEdges.Values[elementIndex]; + // Debug.Assert(edgeFaces.FaceA >= 0); + // var aDot = Vector3.Dot(faces[edgeFaces.FaceA].Normal, faceNormal); + // if (edgeFaces.FaceB >= 0) + // { + // //The edge already has two faces associated with it. This is definitely a numerical error; separate coplanar faces were generated. + // //We *must* merge at least one pair of faces. + // //Note that technically both faces can end up being included, even though we've already tested A and B; + // //the new face could be a bridge that's close enough to both, even if they were barely too far from each other. + // var bDot = Vector3.Dot(faces[edgeFaces.FaceB].Normal, faceNormal); + // if (aDot >= normalCoplanarityEpsilon && bDot >= normalCoplanarityEpsilon) + // { + // AddIfNotPresent(ref facesNeedingMerge, edgeFaces.FaceA, pool); + // AddIfNotPresent(ref facesNeedingMerge, edgeFaces.FaceB, pool); + // } + // else + // { + // //If both aren't merge candidates, then just pick the one that's closer. + // AddIfNotPresent(ref facesNeedingMerge, aDot > bDot ? edgeFaces.FaceA : edgeFaces.FaceB, pool); + // } + // } + // else + // { + // //Only one face already present. Check if it's coplanar. + // if (aDot >= normalCoplanarityEpsilon) + // AddIfNotPresent(ref facesNeedingMerge, edgeFaces.FaceA, pool); + // } + // } + // } - //Retain a count of the deleted faces so the post process can handle them more easily. - facesDeletedCount += facesNeedingMerge.Count; + // if (facesNeedingMerge.Count > previousFaceMergeCount) + // { + // //This iteration has found more faces which should be merged together. + // //Accumulate the vertices. + // for (int i = previousFaceMergeCount; i < facesNeedingMerge.Count; ++i) + // { + // ref var sourceFace = ref faces[facesNeedingMerge[i]]; + // for (int j = 0; j < sourceFace.VertexIndices.Count; ++j) + // { + // var vertexIndex = sourceFace.VertexIndices[j]; + // if (allowVertices[vertexIndex] != 0 && !rawFaceVertexIndices.Contains(vertexIndex)) + // { + // rawFaceVertexIndices.Allocate(pool) = vertexIndex; + // } + // } + // } - AddFace(ref faces, pool, faceNormal, reducedFaceIndices); - AddFaceToEdgesAndTestList(pool, ref reducedFaceIndices, ref edgesToTest, ref facesForEdges, faceNormal, faceCountPriorToAdd); - //steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertices, ref faces, facesNeedingMerge, faceCountPriorToAdd)); - break; - } + // //Re-reduce. + // reducedFaceIndices.Count = 0; + // facePoints.Count = 0; + // ReduceFace(ref rawFaceVertexIndices, faceNormal, points, planeEpsilonNarrow, ref facePoints, ref allowVertices, ref reducedFaceIndices); + // } + // else + // { + // //No more faces need to be merged! Go ahead and apply the changes. + // for (int i = 0; i < facesNeedingMerge.Count; ++i) + // { + // //Note that we do not merge into an existing face; for simplicity, we just merge into the face we're going to append and mark the old faces as being deleted. + // //Note that we do *not* remove deleted faces from the faces list. That would break all the face indices that we've accumulated. + // var faceIndex = facesNeedingMerge[i]; + // ref var faceToRemove = ref faces[faceIndex]; + // faceToRemove.Deleted = true; + + // //For every edge of any face we're merging, remove the face from the edge lists. + // var previousIndex = faceToRemove.VertexIndices[faceToRemove.VertexIndices.Count - 1]; + // for (int j = 0; j < faceToRemove.VertexIndices.Count; ++j) + // { + // RemoveFaceFromEdge(previousIndex, faceToRemove.VertexIndices[j], faceIndex, ref facesForEdges); + // previousIndex = faceToRemove.VertexIndices[j]; + // } + // //Remove any references to this face in the edges to test. + // int removedEdgesToTestCount = 0; + // for (int j = 0; j < edgesToTest.Count; ++j) + // { + // if (edgesToTest[j].FaceIndex == faceIndex) + // { + // ++removedEdgesToTestCount; + // } + // else if (removedEdgesToTestCount > 0) + // { + // //Scoot edges to test back. + // edgesToTest[j - removedEdgesToTestCount] = edgesToTest[j]; + // } + // } + // edgesToTest.Count -= removedEdgesToTestCount; + // } + + // //Retain a count of the deleted faces so the post process can handle them more easily. + // facesDeletedCount += facesNeedingMerge.Count; + + // AddFace(ref faces, pool, faceNormal, reducedFaceIndices); + // AddFaceToEdgesAndTestList(pool, ref reducedFaceIndices, ref edgesToTest, ref facesForEdges, faceNormal, faceCountPriorToAdd); + // steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertices, ref faces, facesNeedingMerge, faceCountPriorToAdd)); + // break; + // } + //} + if (!mergedFace) + { + AddFace(ref faces, pool, faceNormal, reducedFaceIndices); + AddFaceEdgesToTestList(pool, ref reducedFaceIndices, ref edgesToTest, ref submittedEdgeTests, faceNormal, faceCountPriorToAdd); + steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertices, ref faces, facesNeedingMerge, faceCountPriorToAdd)); } + + if (steps.Count > 500) + break; } edgesToTest.Dispose(pool); - facesForEdges.Dispose(pool); facePoints.Dispose(pool); reducedFaceIndices.Dispose(pool); rawFaceVertexIndices.Dispose(pool); diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index 67590f28f..f29c81630 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -6,6 +6,7 @@ using Demos.Demos.Dancers; using Demos.Demos.Sponsors; using Demos.Demos.Tanks; +using Demos.SpecializedTests; using System; using System.Collections.Generic; @@ -43,6 +44,7 @@ struct Option public DemoSet() { + AddOption(); AddOption(); AddOption(); AddOption(); diff --git a/Demos/SpecializedTests/ConvexHullTestDemo.cs b/Demos/SpecializedTests/ConvexHullTestDemo.cs index 318350be5..90396e2d5 100644 --- a/Demos/SpecializedTests/ConvexHullTestDemo.cs +++ b/Demos/SpecializedTests/ConvexHullTestDemo.cs @@ -13,6 +13,9 @@ using BepuUtilities.Memory; using System.Text.Json; using System.IO; +using DemoRenderer.Constraints; +using DemoUtilities; +using DemoRenderer.UI; namespace Demos.SpecializedTests; @@ -32,6 +35,140 @@ Buffer CreateRandomConvexHullPoints() return points; } + + + Vector3[] CreateBwaa() + { + var points = new Vector3[] + { + new(-0.637357891f, 0.347849399f, -0.303436399f), + new(-0.636290252f, 0.345867455f, -0.301366687f), + new(-0.992014945f, 0.348357588f, -0.3031407f), + new(-1.00909662f, 0.386065364f, -0.303337872f), + new(0.637357891f, 0.347849399f, -0.303436399f), + new(-0.636290252f, 0.345918268f, 0.701366544f), + new(-0.636503756f, 0.345918268f, 0.700873733f), + new(-0.992655516f, 0.346578926f, 0.701070845f), + new(-0.992655516f, 0.346578926f, -0.301070988f), + new(0.636290252f, 0.345867455f, -0.301366687f), + new(-0.995858312f, 0.348510057f, -0.301859498f), + new(-1.01272643f, 0.385912925f, -0.302056611f), + new(-1.01037765f, 0.390029252f, -0.302746475f), + new(-0.637357891f, 0.389521062f, -0.302845061f), + new(1.00909662f, 0.386065364f, -0.303337872f), + new(0.992014945f, 0.348357588f, -0.3031407f), + new(-0.637357891f, 0.347849399f, 0.703436255f), + new(-0.992014945f, 0.348357588f, 0.703140557f), + new(0.636290252f, 0.345918268f, 0.701366544f), + new(-0.995858312f, 0.348510057f, 0.701859355f), + new(-1.02553761f, 0.351406753f, 0.678599536f), + new(-1.0251106f, 0.35013628f, 0.675938487f), + new(-1.0251106f, 0.35013628f, -0.2759386f), + new(-1.02553761f, 0.351406753f, -0.278599679f), + new(0.992655516f, 0.346578926f, -0.301070988f), + new(0.992655516f, 0.346578926f, 0.701070845f), + new(0.636503756f, 0.345918268f, 0.700873733f), + new(-1.04432738f, 0.37869662f, -0.274558783f), + new(-1.01400757f, 0.389673531f, -0.301465213f), + new(-1.04582202f, 0.382304758f, -0.273770332f), + new(-1.0582062f, 0.67344743f, -0.220745891f), + new(-1.0545764f, 0.674260557f, -0.22183004f), + new(-0.637144327f, 0.674158931f, -0.221928596f), + new(1.01037765f, 0.390029252f, -0.302746475f), + new(0.637357891f, 0.389521062f, -0.302845061f), + new(1.01272643f, 0.385912925f, -0.302056611f), + new(0.995858312f, 0.348510057f, -0.301859498f), + new(-1.00909662f, 0.386065364f, 0.703337729f), + new(0.637357891f, 0.347849399f, 0.703436255f), + new(0.992014945f, 0.348357588f, 0.703140557f), + new(-1.01272643f, 0.385912925f, 0.702056468f), + new(-1.04432738f, 0.37869662f, 0.67455864f), + new(-1.04582202f, 0.380170345f, 0.671404779f), + new(-1.04582202f, 0.380170345f, -0.271404922f), + new(1.02553761f, 0.351406753f, -0.278599679f), + new(1.0251106f, 0.35013628f, -0.2759386f), + new(1.0251106f, 0.35013628f, 0.675938487f), + new(1.02553761f, 0.351406753f, 0.678599536f), + new(0.995858312f, 0.348510057f, 0.701859355f), + new(-1.08980727f, 0.656575501f, -0.196303427f), + new(-1.09023428f, 0.656982064f, -0.193346679f), + new(-1.0584197f, 0.67675066f, -0.21867618f), + new(-1.0550034f, 0.677512944f, -0.219858885f), + new(-1.09023428f, 0.659827888f, -0.194233686f), + new(0.637144327f, 0.674158931f, -0.221928596f), + new(1.01400757f, 0.389673531f, -0.301465213f), + new(1.0545764f, 0.674260557f, -0.22183004f), + new(1.0582062f, 0.67344743f, -0.220745891f), + new(1.04582202f, 0.382304758f, -0.273770332f), + new(1.04432738f, 0.37869662f, -0.274558783f), + new(-0.637357891f, 0.389521062f, 0.702844918f), + new(-1.01037765f, 0.390029252f, 0.702746332f), + new(1.00909662f, 0.386065364f, 0.703337729f), + new(-1.01400757f, 0.389673531f, 0.70146507f), + new(-1.04582202f, 0.382304758f, 0.673770189f), + new(-1.09023428f, 0.656982064f, 0.593346536f), + new(1.04582202f, 0.380170345f, -0.271404922f), + new(1.04582202f, 0.380170345f, 0.671404779f), + new(1.04432738f, 0.37869662f, 0.67455864f), + new(1.01272643f, 0.385912925f, 0.702056468f), + new(-1.09066129f, 0.832155526f, 0.199999928f), + new(-1.0584197f, 0.86234206f, -0.0161386579f), + new(-1.0550034f, 0.863663316f, -0.0167300105f), + new(1.0550034f, 0.677512944f, -0.219858885f), + new(-1.09023428f, 0.833781719f, -0.00135488808f), + new(1.08980727f, 0.656575501f, -0.196303427f), + new(1.0584197f, 0.67675066f, -0.21867618f), + new(1.09023428f, 0.659827888f, -0.194233686f), + new(1.09023428f, 0.656982064f, -0.193346679f), + new(0.637357891f, 0.389521062f, 0.702844918f), + new(1.01037765f, 0.390029252f, 0.702746332f), + new(-0.637144327f, 0.674158931f, 0.621928453f), + new(-1.0545764f, 0.674260557f, 0.621829867f), + new(-1.0582062f, 0.67344743f, 0.620745778f), + new(-1.08980727f, 0.656575501f, 0.596303284f), + new(-1.09023428f, 0.659827888f, 0.594233513f), + new(1.04582202f, 0.382304758f, 0.673770189f), + new(1.09023428f, 0.656982064f, 0.593346536f), + new(1.01400757f, 0.389673531f, 0.70146507f), + new(-1.09044778f, 0.834950566f, 0.199999928f), + new(-1.09023428f, 0.833781719f, 0.40135473f), + new(-1.0584197f, 0.863663316f, -0.0124919862f), + new(-1.0550034f, 0.865035474f, -0.0132804662f), + new(1.0550034f, 0.863663316f, -0.0167300105f), + new(-1.09002078f, 0.835204661f, 0.00219321251f), + new(1.0584197f, 0.86234206f, -0.0161386579f), + new(1.09023428f, 0.833781719f, -0.00135488808f), + new(1.09066129f, 0.832155526f, 0.199999928f), + new(1.0582062f, 0.67344743f, 0.620745778f), + new(1.0545764f, 0.674260557f, 0.621829867f), + new(0.637144327f, 0.674158931f, 0.621928453f), + new(-1.0550034f, 0.677512944f, 0.619858742f), + new(-1.0584197f, 0.67675066f, 0.618676066f), + new(-1.0584197f, 0.86234206f, 0.41613853f), + new(1.08980727f, 0.656575501f, 0.596303284f), + new(1.09023428f, 0.659827888f, 0.594233513f), + new(-1.09002078f, 0.835204661f, 0.397806644f), + new(-1.05863321f, 0.863612533f, 0.199999928f), + new(-1.0584197f, 0.863663316f, 0.412491858f), + new(-1.0550034f, 0.865035474f, 0.413280308f), + new(1.0550034f, 0.865035474f, -0.0132804662f), + new(1.0584197f, 0.863663316f, -0.0124919862f), + new(1.09002078f, 0.835204661f, 0.00219321251f), + new(1.09044778f, 0.834950566f, 0.199999928f), + new(1.09023428f, 0.833781719f, 0.40135473f), + new(1.0584197f, 0.67675066f, 0.618676066f), + new(1.0550034f, 0.677512944f, 0.619858742f), + new(-1.0550034f, 0.863663316f, 0.416729867f), + new(1.0584197f, 0.86234206f, 0.41613853f), + new(1.0550034f, 0.865035474f, 0.413280308f), + new(1.05863321f, 0.863612533f, 0.199999928f), + new(1.09002078f, 0.835204661f, 0.397806644f), + new(1.0584197f, 0.863663316f, 0.412491858f), + new(1.0550034f, 0.8636633f, 0.41672987f), + }; + return points; + } + Buffer CreatePlaneish() { var points = new Buffer(12, BufferPool); @@ -385,7 +522,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Yaw = 0; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -0, 0)), new SolveDescription(8, 1)); //var hullPoints = CreateJSONSourcedConvexHull(@"Content/testHull.json"); //for (int i = 0; i < hullPoints.Length; ++i) //{ @@ -393,7 +530,8 @@ public override void Initialize(ContentArchive content, Camera camera) //} //var hullPoints = CreateRandomConvexHullPoints(); //var hullPoints = CreateMeshConvexHull(content.Load(@"Content\newt.obj"), new Vector3(1, 1.5f, 1f)); - var hullPoints = CreateHellCube(200); + //var hullPoints = CreateHellCube(200); + var hullPoints = CreateBwaa(); //var hullPoints = CreatePlaneish(); //var hullPoints = CreateDistantPlane(); //var hullPoints = CreateTestConvexHull(); @@ -419,52 +557,52 @@ public override void Initialize(ContentArchive content, Camera camera) //} //Console.WriteLine($"Largest error: {largestError}"); - //ConvexHullHelper.ComputeHull(hullPoints, BufferPool, out var hullData, out debugSteps); - //this.points = hullPoints; + ConvexHullHelper.ComputeHull(hullPoints, BufferPool, out var hullData, out debugSteps); + this.points = hullPoints; var boxHullPoints = CreateBoxConvexHull(2); var boxHullShape = new ConvexHull(boxHullPoints, BufferPool, out _); - Matrix3x3.CreateScale(new Vector3(5, 0.5f, 3), out var scale); - var transform = Matrix3x3.CreateFromAxisAngle(Vector3.Normalize(new Vector3(3, 2, 1)), 1207) * scale; - const int transformCount = 10000; - var transformStart = Stopwatch.GetTimestamp(); - for (int i = 0; i < transformCount; ++i) - { - CreateTransformedCopy(hullShape, transform, BufferPool, out var transformedHullShape); - transformedHullShape.Dispose(BufferPool); - } - var transformEnd = Stopwatch.GetTimestamp(); - Console.WriteLine($"Transform hull computation time (us): {(transformEnd - transformStart) * 1e6 / (transformCount * Stopwatch.Frequency)}"); + //Matrix3x3.CreateScale(new Vector3(5, 0.5f, 3), out var scale); + //var transform = Matrix3x3.CreateFromAxisAngle(Vector3.Normalize(new Vector3(3, 2, 1)), 1207) * scale; + //const int transformCount = 10000; + //var transformStart = Stopwatch.GetTimestamp(); + //for (int i = 0; i < transformCount; ++i) + //{ + // CreateTransformedCopy(hullShape, transform, BufferPool, out var transformedHullShape); + // transformedHullShape.Dispose(BufferPool); + //} + //var transformEnd = Stopwatch.GetTimestamp(); + //Console.WriteLine($"Transform hull computation time (us): {(transformEnd - transformStart) * 1e6 / (transformCount * Stopwatch.Frequency)}"); - hullShape.RayTest(RigidPose.Identity, new Vector3(0, 1, 0), -Vector3.UnitY, out var t, out var normal); + //hullShape.RayTest(RigidPose.Identity, new Vector3(0, 1, 0), -Vector3.UnitY, out var t, out var normal); - const int rayIterationCount = 10000; - var rayPose = RigidPose.Identity; - var rayOrigin = new Vector3(0, 2, 0); - var rayDirection = new Vector3(0, -1, 0); + //const int rayIterationCount = 10000; + //var rayPose = RigidPose.Identity; + //var rayOrigin = new Vector3(0, 2, 0); + //var rayDirection = new Vector3(0, -1, 0); - int hitCounter = 0; - var start = Stopwatch.GetTimestamp(); - for (int i = 0; i < rayIterationCount; ++i) - { - if (hullShape.RayTest(rayPose, rayOrigin, rayDirection, out _, out _)) - { - ++hitCounter; - } - } - var end = Stopwatch.GetTimestamp(); - Console.WriteLine($"Hit counter: {hitCounter}, computation time (us): {(end - start) * 1e6 / (rayIterationCount * Stopwatch.Frequency)}"); + //int hitCounter = 0; + //var start = Stopwatch.GetTimestamp(); + //for (int i = 0; i < rayIterationCount; ++i) + //{ + // if (hullShape.RayTest(rayPose, rayOrigin, rayDirection, out _, out _)) + // { + // ++hitCounter; + // } + //} + //var end = Stopwatch.GetTimestamp(); + //Console.WriteLine($"Hit counter: {hitCounter}, computation time (us): {(end - start) * 1e6 / (rayIterationCount * Stopwatch.Frequency)}"); - const int iterationCount = 100; - start = Stopwatch.GetTimestamp(); - for (int i = 0; i < iterationCount; ++i) - { - CreateShape(hullPoints, BufferPool, out _, out var perfTestShape); - perfTestShape.Dispose(BufferPool); - } - end = Stopwatch.GetTimestamp(); - Console.WriteLine($"Hull computation time (us): {(end - start) * 1e6 / (iterationCount * Stopwatch.Frequency)}"); + //const int iterationCount = 100; + //start = Stopwatch.GetTimestamp(); + //for (int i = 0; i < iterationCount; ++i) + //{ + // CreateShape(hullPoints, BufferPool, out _, out var perfTestShape); + // perfTestShape.Dispose(BufferPool); + //} + //end = Stopwatch.GetTimestamp(); + //Console.WriteLine($"Hull computation time (us): {(end - start) * 1e6 / (iterationCount * Stopwatch.Frequency)}"); var hullShapeIndex = Simulation.Shapes.Add(hullShape); var boxHullShapeIndex = Simulation.Shapes.Add(boxHullShape); @@ -528,132 +666,158 @@ void TestConvexHullCreation() } //Buffer points; - //List debugSteps; - - //int stepIndex = 0; - - //public override void Update(Window window, Camera camera, Input input, float dt) - //{ - // if (input.TypedCharacters.Contains('x')) - // { - // stepIndex = Math.Max(stepIndex - 1, 0); - // } - // if (input.TypedCharacters.Contains('c')) - // { - // stepIndex = Math.Min(stepIndex + 1, debugSteps.Count - 1); - // } - // if (input.WasPushed(OpenTK.Input.Key.P)) - // { - // showWireframe = !showWireframe; - // } - // if (input.WasPushed(OpenTK.Input.Key.U)) - // { - // showDeleted = !showDeleted; - // } - // if (input.WasPushed(OpenTK.Input.Key.Y)) - // { - // showVertexIndices = !showVertexIndices; - // } - // if (input.WasPushed(OpenTK.Input.Key.H)) - // { - // showFaceVertexStatuses = !showFaceVertexStatuses; - // } - // base.Update(window, camera, input, dt); - //} - - //bool showWireframe; - //bool showDeleted; - //bool showVertexIndices; - //bool showFaceVertexStatuses = true; - //public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) - //{ - // var step = debugSteps[stepIndex]; - // var scale = 5f; - // var renderOffset = new Vector3(-15, 25, 0); - // for (int i = 0; i < points.Length; ++i) - // { - // var pose = new RigidPose(renderOffset + points[i] * scale); - // renderer.Shapes.AddShape(new Box(0.1f, 0.1f, 0.1f), Simulation.Shapes, pose, new Vector3(0.5f, 0.5f, 0.5f)); - // if (!step.AllowVertex[i] && showFaceVertexStatuses) - // renderer.Shapes.AddShape(new Box(0.6f, 0.25f, 0.25f), Simulation.Shapes, pose, new Vector3(1, 0, 0)); - // } - // if (showFaceVertexStatuses) - // { - // for (int i = 0; i < step.Raw.Count; ++i) - // { - // var pose = new RigidPose(renderOffset + points[step.Raw[i]] * scale); - // renderer.Shapes.AddShape(new Box(0.25f, 0.6f, 0.25f), Simulation.Shapes, pose, new Vector3(0, 0, 1)); - // } - // for (int i = 0; i < step.Reduced.Count; ++i) - // { - // var pose = new RigidPose(renderOffset + points[step.Reduced[i]] * scale); - // renderer.Shapes.AddShape(new Box(0.25f, 0.25f, 0.6f), Simulation.Shapes, pose, new Vector3(0, 1, 0)); - // } - // } - - // { - // var pose = new RigidPose(renderOffset); - // for (int i = 0; i < step.FaceStarts.Count; ++i) - // { - // if (showDeleted || !step.FaceDeleted[i]) - // { - // var faceStart = step.FaceStarts[i]; - // var faceEnd = i + 1 < step.FaceStarts.Count ? step.FaceStarts[i + 1] : step.FaceIndices.Count; - // var count = faceEnd - faceStart; - // var color = step.FaceDeleted[i] ? new Vector3(0.25f, 0.25f, 0.25f) : step.FaceIndex == i ? new Vector3(1, 0, 0.5f) : new Vector3(1, 0, 1); - // var deletionInducedScale = step.FaceDeleted[i] ? new Vector3(1.1f) : new Vector3(1f); - - // var offset = step.FaceDeleted[i] ? step.FaceNormals[i] * 0.25f : new Vector3(); - // if (showWireframe) - // { - // var previousIndex = faceEnd - 1; - // for (int q = faceStart; q < faceEnd; ++q) - // { - // var a = points[step.FaceIndices[q]] * scale + pose.Position + offset; - // var b = points[step.FaceIndices[previousIndex]] * scale + pose.Position + offset; - // previousIndex = q; - // renderer.Lines.Allocate() = new LineInstance(a, b, color, Vector3.Zero); - // } - // } - // else - // { - // for (int k = faceStart + 2; k < faceEnd; ++k) - // { - // renderer.Shapes.AddShape(new Triangle - // { - // A = points[step.FaceIndices[faceStart]] * scale + offset, - // B = points[step.FaceIndices[k]] * scale + offset, - // C = points[step.FaceIndices[k - 1]] * scale + offset - // }, Simulation.Shapes, pose, color); - // } - // } - // } - // } - // } - - // if (showVertexIndices) - // { - // for (int i = 0; i < points.Length; ++i) - // { - // if (DemoRenderer.Helpers.GetScreenLocation(points[i] * scale + renderOffset, camera.ViewProjection, renderer.Surface.Resolution, out var location)) - // { - // renderer.TextBatcher.Write(text.Clear().Append(i), location, 10, new Vector3(1), font); - // } - // } - // } - - // var edgeMidpoint = renderOffset + (points[step.SourceEdge.A] + points[step.SourceEdge.B]) * scale * 0.5f; - // renderer.Lines.Allocate() = new LineInstance(edgeMidpoint, edgeMidpoint + step.BasisX * scale * 0.5f, new Vector3(1, 1, 0), new Vector3()); - // renderer.Lines.Allocate() = new LineInstance(edgeMidpoint, edgeMidpoint + step.BasisY * scale * 0.5f, new Vector3(0, 1, 0), new Vector3()); - // renderer.TextBatcher.Write( - // text.Clear().Append($"Enumerate step with X and C. Current step: ").Append(stepIndex + 1).Append(" out of ").Append(debugSteps.Count), - // new Vector2(32, renderer.Surface.Resolution.Y - 140), 20, new Vector3(1), font); - // renderer.TextBatcher.Write(text.Clear().Append("Show wireframe: P ").Append(showWireframe ? "(on)" : "(off)"), new Vector2(32, renderer.Surface.Resolution.Y - 120), 20, new Vector3(1), font); - // renderer.TextBatcher.Write(text.Clear().Append("Show deleted: U ").Append(showDeleted ? "(on)" : "(off)"), new Vector2(32, renderer.Surface.Resolution.Y - 100), 20, new Vector3(1), font); - // renderer.TextBatcher.Write(text.Clear().Append("Show vertex indices: Y ").Append(showVertexIndices ? "(on)" : "(off)"), new Vector2(32, renderer.Surface.Resolution.Y - 80), 20, new Vector3(1), font); - // renderer.TextBatcher.Write(text.Clear().Append("Show face vertex statuses: H ").Append(showFaceVertexStatuses ? "(on)" : "(off)"), new Vector2(32, renderer.Surface.Resolution.Y - 60), 20, new Vector3(1), font); - - - // base.Render(renderer, camera, input, text, font); - //} + Vector3[] points; + List debugSteps; + + int stepIndex = 0; + + public override void Update(Window window, Camera camera, Input input, float dt) + { + if (input.TypedCharacters.Contains('x')) + { + stepIndex = Math.Max(stepIndex - 1, 0); + } + if (input.TypedCharacters.Contains('c')) + { + stepIndex = Math.Min(stepIndex + 1, debugSteps.Count - 1); + } + if (input.WasPushed(OpenTK.Input.Key.P)) + { + showWireframe = !showWireframe; + } + if (input.WasPushed(OpenTK.Input.Key.U)) + { + showDeleted = !showDeleted; + } + if (input.WasPushed(OpenTK.Input.Key.Y)) + { + showVertexIndices = !showVertexIndices; + } + if (input.WasPushed(OpenTK.Input.Key.H)) + { + showFaceVertexStatuses = !showFaceVertexStatuses; + } + base.Update(window, camera, input, dt); + } + + bool showWireframe; + bool showDeleted; + bool showVertexIndices; + bool showFaceVertexStatuses = true; + public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + { + var step = debugSteps[stepIndex]; + var scale = 15f; + var renderOffset = new Vector3(-15, 25, 0); + + void DrawVertexIndex(int i, Vector3 color, Vector2 offset = default) + { + if (DemoRenderer.Helpers.GetScreenLocation(points[i] * scale + renderOffset, camera.ViewProjection, renderer.Surface.Resolution, out var location)) + { + float fontSize = 10; + float spacing = 12; + renderer.TextBatcher.Write(text.Clear().Append(i), location + offset * spacing, fontSize, color, font); + } + } + + for (int i = 0; i < points.Length; ++i) + { + var pose = new RigidPose(renderOffset + points[i] * scale); + renderer.Shapes.AddShape(new Box(0.1f, 0.1f, 0.1f), Simulation.Shapes, pose, new Vector3(0.5f, 0.5f, 0.5f)); + if (!step.AllowVertex[i] && showFaceVertexStatuses) + { + var color = new Vector3(1, 0, 0); + renderer.Shapes.AddShape(new Box(0.6f, 0.25f, 0.25f), Simulation.Shapes, pose, color); + if (showVertexIndices) + DrawVertexIndex(i, color, new Vector2(0, 1)); + } + } + if (showFaceVertexStatuses) + { + for (int i = 0; i < step.Raw.Count; ++i) + { + var color = new Vector3(0.3f, 0.3f, 1); + var pose = new RigidPose(renderOffset + points[step.Raw[i]] * scale); + renderer.Shapes.AddShape(new Box(0.25f, 0.6f, 0.25f), Simulation.Shapes, pose, color); + if (showVertexIndices) + DrawVertexIndex(step.Raw[i], color, new Vector2(0, 2)); + } + for (int i = 0; i < step.Reduced.Count; ++i) + { + var color = new Vector3(0, 1, 0); + var pose = new RigidPose(renderOffset + points[step.Reduced[i]] * scale); + renderer.Shapes.AddShape(new Box(0.25f, 0.25f, 0.6f), Simulation.Shapes, pose, color); + if (showVertexIndices) + DrawVertexIndex(step.Reduced[i], color, new Vector2(0, 3)); + } + } + + { + var pose = new RigidPose(renderOffset); + for (int i = 0; i < step.FaceStarts.Count; ++i) + { + if (showDeleted || !step.FaceDeleted[i]) + { + var faceStart = step.FaceStarts[i]; + var faceEnd = i + 1 < step.FaceStarts.Count ? step.FaceStarts[i + 1] : step.FaceIndices.Count; + var count = faceEnd - faceStart; + var color = step.FaceDeleted[i] ? new Vector3(0.25f, 0.25f, 0.25f) : step.FaceIndex == i ? new Vector3(1, 0, 0.5f) : new Vector3(1, 0, 1); + var deletionInducedScale = step.FaceDeleted[i] ? new Vector3(1.1f) : new Vector3(1f); + + var offset = step.FaceDeleted[i] ? step.FaceNormals[i] * 0.25f : new Vector3(); + if (showWireframe) + { + var previousIndex = faceEnd - 1; + for (int q = faceStart; q < faceEnd; ++q) + { + var a = points[step.FaceIndices[q]] * scale + pose.Position + offset; + var b = points[step.FaceIndices[previousIndex]] * scale + pose.Position + offset; + previousIndex = q; + renderer.Lines.Allocate() = new LineInstance(a, b, color, Vector3.Zero); + } + } + else + { + for (int k = faceStart + 2; k < faceEnd; ++k) + { + renderer.Shapes.AddShape(new Triangle + { + A = points[step.FaceIndices[faceStart]] * scale + offset, + B = points[step.FaceIndices[k]] * scale + offset, + C = points[step.FaceIndices[k - 1]] * scale + offset + }, Simulation.Shapes, pose, color); + } + } + } + } + } + //Console.WriteLine("Current step edges: "); + //for (int i = 0; i < step.Reduced.Count; ++i) + //{ + // Console.WriteLine($" Edge {i}: ({step.Reduced[i]}, {step.Reduced[(i + 1) % step.Reduced.Count]})"); + //} + + + if (showVertexIndices) + { + for (int i = 0; i < points.Length; ++i) + { + DrawVertexIndex(i, Vector3.One); + } + } + + var edgeMidpoint = renderOffset + (points[step.SourceEdge.A] + points[step.SourceEdge.B]) * scale * 0.5f; + renderer.Lines.Allocate() = new LineInstance(edgeMidpoint, edgeMidpoint + step.BasisX * scale * 0.5f, new Vector3(1, 1, 0), new Vector3()); + renderer.Lines.Allocate() = new LineInstance(edgeMidpoint, edgeMidpoint + step.BasisY * scale * 0.5f, new Vector3(0, 1, 0), new Vector3()); + renderer.TextBatcher.Write( + text.Clear().Append($"Enumerate step with X and C. Current step: ").Append(stepIndex + 1).Append(" out of ").Append(debugSteps.Count), + new Vector2(32, renderer.Surface.Resolution.Y - 140), 20, new Vector3(1), font); + renderer.TextBatcher.Write(text.Clear().Append("Show wireframe: P ").Append(showWireframe ? "(on)" : "(off)"), new Vector2(32, renderer.Surface.Resolution.Y - 120), 20, new Vector3(1), font); + renderer.TextBatcher.Write(text.Clear().Append("Show deleted: U ").Append(showDeleted ? "(on)" : "(off)"), new Vector2(32, renderer.Surface.Resolution.Y - 100), 20, new Vector3(1), font); + renderer.TextBatcher.Write(text.Clear().Append("Show vertex indices: Y ").Append(showVertexIndices ? "(on)" : "(off)"), new Vector2(32, renderer.Surface.Resolution.Y - 80), 20, new Vector3(1), font); + renderer.TextBatcher.Write(text.Clear().Append("Show face vertex statuses: H ").Append(showFaceVertexStatuses ? "(on)" : "(off)"), new Vector2(32, renderer.Surface.Resolution.Y - 60), 20, new Vector3(1), font); + + + base.Render(renderer, camera, input, text, font); + } } From 7a211c62aadcec9df61e1c83e6dd725a1d6234ae Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 9 Nov 2024 10:06:40 -0600 Subject: [PATCH 884/947] Further poking. --- BepuPhysics/Collidables/ConvexHullHelper.cs | 165 +++---------------- Demos/SpecializedTests/ConvexHullTestDemo.cs | 8 +- 2 files changed, 24 insertions(+), 149 deletions(-) diff --git a/BepuPhysics/Collidables/ConvexHullHelper.cs b/BepuPhysics/Collidables/ConvexHullHelper.cs index c09e075b0..834c68b01 100644 --- a/BepuPhysics/Collidables/ConvexHullHelper.cs +++ b/BepuPhysics/Collidables/ConvexHullHelper.cs @@ -525,8 +525,9 @@ static void AddIfNotPresent(ref QuickList list, int value, BufferPool pool) public struct DebugStep { public EdgeEndpoints SourceEdge; - public List Raw; - public List Reduced; + public int[] Raw; + public int[] Premerged; + public int[] Reduced; public bool[] AllowVertex; public Vector3 FaceNormal; public Vector3 BasisX; @@ -534,26 +535,18 @@ public struct DebugStep public List FaceStarts; public List FaceIndices; public bool[] FaceDeleted; - public int[] MergedFaceIndices; public int FaceIndex; public Vector3[] FaceNormals; - internal DebugStep(EdgeEndpoints sourceEdge, ref QuickList raw, Vector3 faceNormal, Vector3 basisX, Vector3 basisY, ref QuickList reduced, ref Buffer allowVertex, ref QuickList faces, Span mergedFaceIndices, int faceIndex) + internal DebugStep(EdgeEndpoints sourceEdge, ref QuickList raw, Vector3 faceNormal, Vector3 basisX, Vector3 basisY, ref QuickList reduced, ref Buffer allowVertex, ref QuickList faces, int faceIndex) { SourceEdge = sourceEdge; FaceNormal = faceNormal; BasisX = basisX; BasisY = basisY; - Raw = new List(); - for (int i = 0; i < raw.Count; ++i) - { - Raw.Add(raw[i]); - } - Reduced = new List(); - for (int i = 0; i < reduced.Count; ++i) - { - Reduced.Add(reduced[i]); - } + ((Span)raw).ToArray(); + ((Span)reduced).ToArray(); + Premerged = null; AllowVertex = new bool[allowVertex.Length]; for (int i = 0; i < allowVertex.Length; ++i) { @@ -572,9 +565,14 @@ internal DebugStep(EdgeEndpoints sourceEdge, ref QuickList raw, Vector3 fac FaceDeleted[i] = face.Deleted; FaceNormals[i] = face.Normal; } - MergedFaceIndices = mergedFaceIndices.ToArray(); FaceIndex = faceIndex; } + + internal void UpdateForFaceMerge(Span mergedFaceIndices) + { + PremergeFaceIndices = this.MergedFaceIndices; + this.MergedFaceIndices = mergedFaceIndices.ToArray(); + } } /// /// Computes the convex hull of a set of points. @@ -720,7 +718,6 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa var faces = new QuickList(points.Length, pool); var edgesToTest = new QuickList(points.Length, pool); var submittedEdgeTests = new QuickSet(points.Length, pool); - var facesNeedingMerge = new QuickList(32, pool); if (reducedFaceIndices.Count >= 3) { //The initial face search found an actual face! That's a bit surprising since we didn't start from an edge offset, but rather an arbitrary direction. @@ -754,7 +751,7 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa } Vector3Wide.ReadFirst(initialBasisX, out var debugInitialBasisX); Vector3Wide.ReadFirst(initialBasisY, out var debugInitialBasisY); - steps.Add(new DebugStep(initialSourceEdge, ref rawFaceVertexIndices, initialFaceNormal, debugInitialBasisX, debugInitialBasisY, ref reducedFaceIndices, ref allowVertices, ref faces, default, reducedFaceIndices.Count >= 3 ? 0 : -1)); + steps.Add(new DebugStep(initialSourceEdge, ref rawFaceVertexIndices, initialFaceNormal, debugInitialBasisX, debugInitialBasisY, ref reducedFaceIndices, ref allowVertices, ref faces, reducedFaceIndices.Count >= 3 ? 0 : -1)); int facesDeletedCount = 0; @@ -783,7 +780,7 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa if (reducedFaceIndices.Count < 3) { - steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertices, ref faces, default, -1)); + steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertices, ref faces, -1)); //Degenerate face found; don't bother creating work for it. continue; } @@ -814,146 +811,22 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa } } // Rerun reduction for the merged face. + var step = new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertices, ref faces, i); face.VertexIndices.Count = 0; facePoints.Count = 0; - //steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertices, ref faces, facesNeedingMerge, i)); face.VertexIndices.EnsureCapacity(rawFaceVertexIndices.Count, pool); ReduceFace(ref rawFaceVertexIndices, faceNormal, points, planeEpsilonNarrow, ref facePoints, ref allowVertices, ref face.VertexIndices); + steps.Add(step); mergedFace = true; break; } } - var faceCountPriorToAdd = faces.Count; - - //facesNeedingMerge.Count = 0; - //while (true) - //{ - // //This implementation bites the bullet pretty hard on numerical problems. They most frequently arise from near coplanar vertices. - // //It's possible that two iterations see different subsets of 'coplanar' vertices, either causing two faces with near equal normal or - // //even causing an edge to have more than two faces associated with it. - // //Before making any modifications to the existing data, iterate over all the edges in the current face to check for any such edges. - - // //While the usual flow here will be to reduce the vertices we just found and accept the result immediately, - // //it's possible for the just-detected face to end up coplanar with an existing face because of numerical issues that prevented detecting the whole face earlier. - // //Rather than using more precise math, we detect the failure and merge faces after the fact. - // //Unfortunately, this can happen recursively! Merging two redundant faces could reveal a third face that also needs to be merged. - // //This is uncommon, but it needs to be covered. - // //So, we just stick everything into a while loop and let it iterate until it's done. - // //The good news is that the merging process won't loop forever- every merge results in a face that is at least as large as any contributor, and any internal points - // //that get reduced out of consideration are marked with allowVertex[whateverInteriorPointIndex] = false. - // int previousFaceMergeCount = facesNeedingMerge.Count; - // var previousEndpointIndex = reducedFaceIndices[reducedFaceIndices.Count - 1]; - // for (int i = 0; i < reducedFaceIndices.Count; ++i) - // { - // EdgeEndpoints edgeEndpoints; - // edgeEndpoints.A = previousEndpointIndex; - // edgeEndpoints.B = reducedFaceIndices[i]; - // previousEndpointIndex = edgeEndpoints.B; - - // if (facesForEdges.GetTableIndices(ref edgeEndpoints, out var tableIndex, out var elementIndex)) - // { - // //There is already at least one face associated with this edge. Do we need to merge? - // ref var edgeFaces = ref facesForEdges.Values[elementIndex]; - // Debug.Assert(edgeFaces.FaceA >= 0); - // var aDot = Vector3.Dot(faces[edgeFaces.FaceA].Normal, faceNormal); - // if (edgeFaces.FaceB >= 0) - // { - // //The edge already has two faces associated with it. This is definitely a numerical error; separate coplanar faces were generated. - // //We *must* merge at least one pair of faces. - // //Note that technically both faces can end up being included, even though we've already tested A and B; - // //the new face could be a bridge that's close enough to both, even if they were barely too far from each other. - // var bDot = Vector3.Dot(faces[edgeFaces.FaceB].Normal, faceNormal); - // if (aDot >= normalCoplanarityEpsilon && bDot >= normalCoplanarityEpsilon) - // { - // AddIfNotPresent(ref facesNeedingMerge, edgeFaces.FaceA, pool); - // AddIfNotPresent(ref facesNeedingMerge, edgeFaces.FaceB, pool); - // } - // else - // { - // //If both aren't merge candidates, then just pick the one that's closer. - // AddIfNotPresent(ref facesNeedingMerge, aDot > bDot ? edgeFaces.FaceA : edgeFaces.FaceB, pool); - // } - // } - // else - // { - // //Only one face already present. Check if it's coplanar. - // if (aDot >= normalCoplanarityEpsilon) - // AddIfNotPresent(ref facesNeedingMerge, edgeFaces.FaceA, pool); - // } - // } - // } - - // if (facesNeedingMerge.Count > previousFaceMergeCount) - // { - // //This iteration has found more faces which should be merged together. - // //Accumulate the vertices. - // for (int i = previousFaceMergeCount; i < facesNeedingMerge.Count; ++i) - // { - // ref var sourceFace = ref faces[facesNeedingMerge[i]]; - // for (int j = 0; j < sourceFace.VertexIndices.Count; ++j) - // { - // var vertexIndex = sourceFace.VertexIndices[j]; - // if (allowVertices[vertexIndex] != 0 && !rawFaceVertexIndices.Contains(vertexIndex)) - // { - // rawFaceVertexIndices.Allocate(pool) = vertexIndex; - // } - // } - // } - - // //Re-reduce. - // reducedFaceIndices.Count = 0; - // facePoints.Count = 0; - // ReduceFace(ref rawFaceVertexIndices, faceNormal, points, planeEpsilonNarrow, ref facePoints, ref allowVertices, ref reducedFaceIndices); - // } - // else - // { - // //No more faces need to be merged! Go ahead and apply the changes. - // for (int i = 0; i < facesNeedingMerge.Count; ++i) - // { - // //Note that we do not merge into an existing face; for simplicity, we just merge into the face we're going to append and mark the old faces as being deleted. - // //Note that we do *not* remove deleted faces from the faces list. That would break all the face indices that we've accumulated. - // var faceIndex = facesNeedingMerge[i]; - // ref var faceToRemove = ref faces[faceIndex]; - // faceToRemove.Deleted = true; - - // //For every edge of any face we're merging, remove the face from the edge lists. - // var previousIndex = faceToRemove.VertexIndices[faceToRemove.VertexIndices.Count - 1]; - // for (int j = 0; j < faceToRemove.VertexIndices.Count; ++j) - // { - // RemoveFaceFromEdge(previousIndex, faceToRemove.VertexIndices[j], faceIndex, ref facesForEdges); - // previousIndex = faceToRemove.VertexIndices[j]; - // } - // //Remove any references to this face in the edges to test. - // int removedEdgesToTestCount = 0; - // for (int j = 0; j < edgesToTest.Count; ++j) - // { - // if (edgesToTest[j].FaceIndex == faceIndex) - // { - // ++removedEdgesToTestCount; - // } - // else if (removedEdgesToTestCount > 0) - // { - // //Scoot edges to test back. - // edgesToTest[j - removedEdgesToTestCount] = edgesToTest[j]; - // } - // } - // edgesToTest.Count -= removedEdgesToTestCount; - // } - - // //Retain a count of the deleted faces so the post process can handle them more easily. - // facesDeletedCount += facesNeedingMerge.Count; - - // AddFace(ref faces, pool, faceNormal, reducedFaceIndices); - // AddFaceToEdgesAndTestList(pool, ref reducedFaceIndices, ref edgesToTest, ref facesForEdges, faceNormal, faceCountPriorToAdd); - // steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertices, ref faces, facesNeedingMerge, faceCountPriorToAdd)); - // break; - // } - //} if (!mergedFace) { + var faceCountPriorToAdd = faces.Count; AddFace(ref faces, pool, faceNormal, reducedFaceIndices); AddFaceEdgesToTestList(pool, ref reducedFaceIndices, ref edgesToTest, ref submittedEdgeTests, faceNormal, faceCountPriorToAdd); - steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertices, ref faces, facesNeedingMerge, faceCountPriorToAdd)); + steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertices, ref faces, faceCountPriorToAdd)); } if (steps.Count > 500) diff --git a/Demos/SpecializedTests/ConvexHullTestDemo.cs b/Demos/SpecializedTests/ConvexHullTestDemo.cs index 90396e2d5..c64d9ce0c 100644 --- a/Demos/SpecializedTests/ConvexHullTestDemo.cs +++ b/Demos/SpecializedTests/ConvexHullTestDemo.cs @@ -528,10 +528,10 @@ public override void Initialize(ContentArchive content, Camera camera) //{ // hullPoints[i] *= 0.03f; //} - //var hullPoints = CreateRandomConvexHullPoints(); + var hullPoints = CreateRandomConvexHullPoints(); //var hullPoints = CreateMeshConvexHull(content.Load(@"Content\newt.obj"), new Vector3(1, 1.5f, 1f)); //var hullPoints = CreateHellCube(200); - var hullPoints = CreateBwaa(); + //var hullPoints = CreateBwaa(); //var hullPoints = CreatePlaneish(); //var hullPoints = CreateDistantPlane(); //var hullPoints = CreateTestConvexHull(); @@ -558,7 +558,9 @@ public override void Initialize(ContentArchive content, Camera camera) //Console.WriteLine($"Largest error: {largestError}"); ConvexHullHelper.ComputeHull(hullPoints, BufferPool, out var hullData, out debugSteps); - this.points = hullPoints; + this.points = new Vector3[hullPoints.Length]; + hullPoints.CopyTo(0, this.points, 0, this.points.Length); + //this.points = hullPoints; var boxHullPoints = CreateBoxConvexHull(2); var boxHullShape = new ConvexHull(boxHullPoints, BufferPool, out _); From a7fb5ae8bf688f64f1079179feb997596f706ebb Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 10 Nov 2024 21:55:00 -0600 Subject: [PATCH 885/947] Debug poking. --- BepuPhysics/Collidables/ConvexHullHelper.cs | 73 +++++++------------- Demos/SpecializedTests/ConvexHullTestDemo.cs | 15 +++- 2 files changed, 38 insertions(+), 50 deletions(-) diff --git a/BepuPhysics/Collidables/ConvexHullHelper.cs b/BepuPhysics/Collidables/ConvexHullHelper.cs index 834c68b01..12e7f7159 100644 --- a/BepuPhysics/Collidables/ConvexHullHelper.cs +++ b/BepuPhysics/Collidables/ConvexHullHelper.cs @@ -475,7 +475,6 @@ public override string ToString() internal struct EarlyFace { public QuickList VertexIndices; - public bool Deleted; public Vector3 Normal; } @@ -526,35 +525,39 @@ public struct DebugStep { public EdgeEndpoints SourceEdge; public int[] Raw; - public int[] Premerged; public int[] Reduced; + public int[] MergedRaw; + public int[] MergedReduced; public bool[] AllowVertex; public Vector3 FaceNormal; public Vector3 BasisX; public Vector3 BasisY; public List FaceStarts; public List FaceIndices; - public bool[] FaceDeleted; public int FaceIndex; public Vector3[] FaceNormals; - internal DebugStep(EdgeEndpoints sourceEdge, ref QuickList raw, Vector3 faceNormal, Vector3 basisX, Vector3 basisY, ref QuickList reduced, ref Buffer allowVertex, ref QuickList faces, int faceIndex) + void FillAllowVertex(Buffer allowVertex) + { + for (int i = 0; i < allowVertex.Length; ++i) + { + AllowVertex[i] = allowVertex[i] != 0; + } + } + internal DebugStep(EdgeEndpoints sourceEdge, QuickList rawVertexIndices, Vector3 faceNormal, Vector3 basisX, Vector3 basisY, QuickList reducedVertexIndices, Buffer allowVertex, QuickList faces, int faceIndex) { SourceEdge = sourceEdge; FaceNormal = faceNormal; BasisX = basisX; BasisY = basisY; - ((Span)raw).ToArray(); - ((Span)reduced).ToArray(); - Premerged = null; + ((Span)rawVertexIndices).ToArray(); + ((Span)reducedVertexIndices).ToArray(); + MergedRaw = null; + MergedReduced = null; AllowVertex = new bool[allowVertex.Length]; - for (int i = 0; i < allowVertex.Length; ++i) - { - AllowVertex[i] = allowVertex[i] != 0; - } + FillAllowVertex(allowVertex); FaceStarts = new List(faces.Count); FaceIndices = new List(); - FaceDeleted = new bool[faces.Count]; FaceNormals = new Vector3[faces.Count]; for (int i = 0; i < faces.Count; ++i) { @@ -562,16 +565,17 @@ internal DebugStep(EdgeEndpoints sourceEdge, ref QuickList raw, Vector3 fac FaceStarts.Add(FaceIndices.Count); for (int j = 0; j < face.VertexIndices.Count; ++j) FaceIndices.Add(face.VertexIndices[j]); - FaceDeleted[i] = face.Deleted; FaceNormals[i] = face.Normal; } FaceIndex = faceIndex; } - internal void UpdateForFaceMerge(Span mergedFaceIndices) + internal void UpdateForFaceMerge(QuickList rawFaceVertexIndices, QuickList reducedVertexIndices, Buffer allowVertex, int mergedFaceIndex) { - PremergeFaceIndices = this.MergedFaceIndices; - this.MergedFaceIndices = mergedFaceIndices.ToArray(); + MergedRaw = ((Span)rawFaceVertexIndices).ToArray(); + MergedReduced = ((Span)reducedVertexIndices).ToArray(); + FillAllowVertex(allowVertex); + FaceIndex = mergedFaceIndex; } } /// @@ -751,9 +755,7 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa } Vector3Wide.ReadFirst(initialBasisX, out var debugInitialBasisX); Vector3Wide.ReadFirst(initialBasisY, out var debugInitialBasisY); - steps.Add(new DebugStep(initialSourceEdge, ref rawFaceVertexIndices, initialFaceNormal, debugInitialBasisX, debugInitialBasisY, ref reducedFaceIndices, ref allowVertices, ref faces, reducedFaceIndices.Count >= 3 ? 0 : -1)); - - int facesDeletedCount = 0; + steps.Add(new DebugStep(initialSourceEdge, rawFaceVertexIndices, initialFaceNormal, debugInitialBasisX, debugInitialBasisY, reducedFaceIndices, allowVertices, faces, reducedFaceIndices.Count >= 3 ? 0 : -1)); while (edgesToTest.Count > 0) { @@ -780,18 +782,18 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa if (reducedFaceIndices.Count < 3) { - steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertices, ref faces, -1)); + steps.Add(new DebugStep(edgeToTest.Endpoints, rawFaceVertexIndices, faceNormal, basisX, basisY, reducedFaceIndices, allowVertices, faces, -1)); //Degenerate face found; don't bother creating work for it. continue; } // Brute force scan all the faces to see if the new face is coplanar with any of them. Console.WriteLine($"step count: {steps.Count}"); + var step = new DebugStep(edgeToTest.Endpoints, rawFaceVertexIndices, faceNormal, basisX, basisY, reducedFaceIndices, allowVertices, faces, faces.Count); + steps.Add(step); bool mergedFace = false; for (int i = 0; i < faces.Count; ++i) { ref var face = ref faces[i]; - if (face.Deleted) - continue; if (Vector3.Dot(face.Normal, faceNormal) > normalCoplanarityEpsilon) { Console.WriteLine($"Merging face {i} with new face, dot {Vector3.Dot(face.Normal, faceNormal)}:"); @@ -811,12 +813,11 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa } } // Rerun reduction for the merged face. - var step = new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertices, ref faces, i); face.VertexIndices.Count = 0; facePoints.Count = 0; face.VertexIndices.EnsureCapacity(rawFaceVertexIndices.Count, pool); ReduceFace(ref rawFaceVertexIndices, faceNormal, points, planeEpsilonNarrow, ref facePoints, ref allowVertices, ref face.VertexIndices); - steps.Add(step); + step.UpdateForFaceMerge(rawFaceVertexIndices, face.VertexIndices, allowVertices, i); mergedFace = true; break; } @@ -826,7 +827,6 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa var faceCountPriorToAdd = faces.Count; AddFace(ref faces, pool, faceNormal, reducedFaceIndices); AddFaceEdgesToTestList(pool, ref reducedFaceIndices, ref edgesToTest, ref submittedEdgeTests, faceNormal, faceCountPriorToAdd); - steps.Add(new DebugStep(edgeToTest.Endpoints, ref rawFaceVertexIndices, faceNormal, basisX, basisY, ref reducedFaceIndices, ref allowVertices, ref faces, faceCountPriorToAdd)); } if (steps.Count > 500) @@ -855,28 +855,6 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa // } //} - if (facesDeletedCount > 0) - { - //During execution, some faces were found to be coplanar and got merged. To avoid breaking face index references during execution, we left those triangles in place. - //Now, though, we need to get rid of any faces that are marked deleted! - int shift = 0; - for (int i = 0; i < faces.Count; ++i) - { - if (faces[i].Deleted) - { - //Removing the face from the list, so we gotta dispose it now rather than later. - faces[i].VertexIndices.Dispose(pool); - ++shift; - } - else if (shift > 0) - { - faces[i - shift] = faces[i]; - } - } - Debug.Assert(facesDeletedCount == shift); - faces.Count -= facesDeletedCount; - } - //Create a reduced hull point set from the face vertex references. int totalIndexCount = 0; for (int i = 0; i < faces.Count; ++i) @@ -921,7 +899,6 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa faces[i].VertexIndices.Dispose(pool); } faces.Dispose(pool); - facesNeedingMerge.Dispose(pool); } diff --git a/Demos/SpecializedTests/ConvexHullTestDemo.cs b/Demos/SpecializedTests/ConvexHullTestDemo.cs index c64d9ce0c..3a83ed29b 100644 --- a/Demos/SpecializedTests/ConvexHullTestDemo.cs +++ b/Demos/SpecializedTests/ConvexHullTestDemo.cs @@ -736,7 +736,18 @@ void DrawVertexIndex(int i, Vector3 color, Vector2 offset = default) } if (showFaceVertexStatuses) { - for (int i = 0; i < step.Raw.Count; ++i) + int[] raw, reduced; + if (step.MergedRaw == null) + { + raw = step.Raw; + reduced = step.Reduced; + } + else + { + raw = step.MergedRaw; + reduced = step.MergedReduced; + } + for (int i = 0; i < raw.Length; ++i) { var color = new Vector3(0.3f, 0.3f, 1); var pose = new RigidPose(renderOffset + points[step.Raw[i]] * scale); @@ -744,7 +755,7 @@ void DrawVertexIndex(int i, Vector3 color, Vector2 offset = default) if (showVertexIndices) DrawVertexIndex(step.Raw[i], color, new Vector2(0, 2)); } - for (int i = 0; i < step.Reduced.Count; ++i) + for (int i = 0; i < reduced.Length; ++i) { var color = new Vector3(0, 1, 0); var pose = new RigidPose(renderOffset + points[step.Reduced[i]] * scale); From 1be62ae7390eafe694f8cb6857d7e87bedfd769c Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 16 Nov 2024 17:52:00 -0600 Subject: [PATCH 886/947] Debug fixes. --- BepuPhysics/Collidables/ConvexHullHelper.cs | 52 +++++++------ Demos/SpecializedTests/ConvexHullTestDemo.cs | 81 +++++++++----------- 2 files changed, 63 insertions(+), 70 deletions(-) diff --git a/BepuPhysics/Collidables/ConvexHullHelper.cs b/BepuPhysics/Collidables/ConvexHullHelper.cs index 12e7f7159..4560f1888 100644 --- a/BepuPhysics/Collidables/ConvexHullHelper.cs +++ b/BepuPhysics/Collidables/ConvexHullHelper.cs @@ -526,8 +526,8 @@ public struct DebugStep public EdgeEndpoints SourceEdge; public int[] Raw; public int[] Reduced; - public int[] MergedRaw; - public int[] MergedReduced; + public int[] RawOverwrittenByMerge; + public int[] ReducedOverwrittenByMerge; public bool[] AllowVertex; public Vector3 FaceNormal; public Vector3 BasisX; @@ -537,25 +537,22 @@ public struct DebugStep public int FaceIndex; public Vector3[] FaceNormals; - void FillAllowVertex(Buffer allowVertex) - { - for (int i = 0; i < allowVertex.Length; ++i) - { - AllowVertex[i] = allowVertex[i] != 0; - } - } - internal DebugStep(EdgeEndpoints sourceEdge, QuickList rawVertexIndices, Vector3 faceNormal, Vector3 basisX, Vector3 basisY, QuickList reducedVertexIndices, Buffer allowVertex, QuickList faces, int faceIndex) + + internal DebugStep(EdgeEndpoints sourceEdge, QuickList rawVertexIndices, Vector3 faceNormal, Vector3 basisX, Vector3 basisY, QuickList reducedVertexIndices, int faceIndex) { SourceEdge = sourceEdge; FaceNormal = faceNormal; BasisX = basisX; BasisY = basisY; - ((Span)rawVertexIndices).ToArray(); - ((Span)reducedVertexIndices).ToArray(); - MergedRaw = null; - MergedReduced = null; - AllowVertex = new bool[allowVertex.Length]; - FillAllowVertex(allowVertex); + Raw = ((Span)rawVertexIndices).ToArray(); + Reduced = ((Span)reducedVertexIndices).ToArray(); + RawOverwrittenByMerge = null; + ReducedOverwrittenByMerge = null; + FaceIndex = faceIndex; + } + + internal DebugStep FillHistory(Buffer allowVertex, QuickList faces) + { FaceStarts = new List(faces.Count); FaceIndices = new List(); FaceNormals = new Vector3[faces.Count]; @@ -567,14 +564,20 @@ internal DebugStep(EdgeEndpoints sourceEdge, QuickList rawVertexIndices, Ve FaceIndices.Add(face.VertexIndices[j]); FaceNormals[i] = face.Normal; } - FaceIndex = faceIndex; + AllowVertex = new bool[allowVertex.Length]; + for (int i = 0; i < allowVertex.Length; ++i) + { + AllowVertex[i] = allowVertex[i] != 0; + } + return this; } internal void UpdateForFaceMerge(QuickList rawFaceVertexIndices, QuickList reducedVertexIndices, Buffer allowVertex, int mergedFaceIndex) { - MergedRaw = ((Span)rawFaceVertexIndices).ToArray(); - MergedReduced = ((Span)reducedVertexIndices).ToArray(); - FillAllowVertex(allowVertex); + RawOverwrittenByMerge = Raw; + ReducedOverwrittenByMerge = Reduced; + Raw = ((Span)rawFaceVertexIndices).ToArray(); + Reduced = ((Span)reducedVertexIndices).ToArray(); FaceIndex = mergedFaceIndex; } } @@ -755,7 +758,7 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa } Vector3Wide.ReadFirst(initialBasisX, out var debugInitialBasisX); Vector3Wide.ReadFirst(initialBasisY, out var debugInitialBasisY); - steps.Add(new DebugStep(initialSourceEdge, rawFaceVertexIndices, initialFaceNormal, debugInitialBasisX, debugInitialBasisY, reducedFaceIndices, allowVertices, faces, reducedFaceIndices.Count >= 3 ? 0 : -1)); + steps.Add(new DebugStep(initialSourceEdge, rawFaceVertexIndices, initialFaceNormal, debugInitialBasisX, debugInitialBasisY, reducedFaceIndices, reducedFaceIndices.Count >= 3 ? 0 : -1).FillHistory(allowVertices, faces)); while (edgesToTest.Count > 0) { @@ -782,14 +785,13 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa if (reducedFaceIndices.Count < 3) { - steps.Add(new DebugStep(edgeToTest.Endpoints, rawFaceVertexIndices, faceNormal, basisX, basisY, reducedFaceIndices, allowVertices, faces, -1)); + steps.Add(new DebugStep(edgeToTest.Endpoints, rawFaceVertexIndices, faceNormal, basisX, basisY, reducedFaceIndices, -1).FillHistory(allowVertices, faces)); //Degenerate face found; don't bother creating work for it. continue; } // Brute force scan all the faces to see if the new face is coplanar with any of them. + var step = new DebugStep(edgeToTest.Endpoints, rawFaceVertexIndices, faceNormal, basisX, basisY, reducedFaceIndices, faces.Count); Console.WriteLine($"step count: {steps.Count}"); - var step = new DebugStep(edgeToTest.Endpoints, rawFaceVertexIndices, faceNormal, basisX, basisY, reducedFaceIndices, allowVertices, faces, faces.Count); - steps.Add(step); bool mergedFace = false; for (int i = 0; i < faces.Count; ++i) { @@ -828,6 +830,8 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa AddFace(ref faces, pool, faceNormal, reducedFaceIndices); AddFaceEdgesToTestList(pool, ref reducedFaceIndices, ref edgesToTest, ref submittedEdgeTests, faceNormal, faceCountPriorToAdd); } + step.FillHistory(allowVertices, faces); + steps.Add(step); if (steps.Count > 500) break; diff --git a/Demos/SpecializedTests/ConvexHullTestDemo.cs b/Demos/SpecializedTests/ConvexHullTestDemo.cs index 3a83ed29b..4ce7a485f 100644 --- a/Demos/SpecializedTests/ConvexHullTestDemo.cs +++ b/Demos/SpecializedTests/ConvexHullTestDemo.cs @@ -736,18 +736,7 @@ void DrawVertexIndex(int i, Vector3 color, Vector2 offset = default) } if (showFaceVertexStatuses) { - int[] raw, reduced; - if (step.MergedRaw == null) - { - raw = step.Raw; - reduced = step.Reduced; - } - else - { - raw = step.MergedRaw; - reduced = step.MergedReduced; - } - for (int i = 0; i < raw.Length; ++i) + for (int i = 0; i < step.Raw.Length; ++i) { var color = new Vector3(0.3f, 0.3f, 1); var pose = new RigidPose(renderOffset + points[step.Raw[i]] * scale); @@ -755,7 +744,7 @@ void DrawVertexIndex(int i, Vector3 color, Vector2 offset = default) if (showVertexIndices) DrawVertexIndex(step.Raw[i], color, new Vector2(0, 2)); } - for (int i = 0; i < reduced.Length; ++i) + for (int i = 0; i < step.Reduced.Length; ++i) { var color = new Vector3(0, 1, 0); var pose = new RigidPose(renderOffset + points[step.Reduced[i]] * scale); @@ -767,49 +756,48 @@ void DrawVertexIndex(int i, Vector3 color, Vector2 offset = default) { var pose = new RigidPose(renderOffset); - for (int i = 0; i < step.FaceStarts.Count; ++i) + void DrawFace(DebugStep step, int[] raw, int[] reduced, bool deleted, int i) { - if (showDeleted || !step.FaceDeleted[i]) + var color = deleted ? new Vector3(0.25f, 0.25f, 0.25f) : stepIndex == i ? new Vector3(1, 0, 0.5f) : new Vector3(1, 0, 1); + var deletionInducedScale = deleted ? new Vector3(1.1f) : new Vector3(1f); + + var offset = deleted ? step.FaceNormal * 0.25f : new Vector3(); + if (showWireframe) { - var faceStart = step.FaceStarts[i]; - var faceEnd = i + 1 < step.FaceStarts.Count ? step.FaceStarts[i + 1] : step.FaceIndices.Count; - var count = faceEnd - faceStart; - var color = step.FaceDeleted[i] ? new Vector3(0.25f, 0.25f, 0.25f) : step.FaceIndex == i ? new Vector3(1, 0, 0.5f) : new Vector3(1, 0, 1); - var deletionInducedScale = step.FaceDeleted[i] ? new Vector3(1.1f) : new Vector3(1f); - - var offset = step.FaceDeleted[i] ? step.FaceNormals[i] * 0.25f : new Vector3(); - if (showWireframe) + var previousIndex = reduced.Length - 1; + for (int q = 0; q < reduced.Length; ++q) { - var previousIndex = faceEnd - 1; - for (int q = faceStart; q < faceEnd; ++q) - { - var a = points[step.FaceIndices[q]] * scale + pose.Position + offset; - var b = points[step.FaceIndices[previousIndex]] * scale + pose.Position + offset; - previousIndex = q; - renderer.Lines.Allocate() = new LineInstance(a, b, color, Vector3.Zero); - } + var a = points[reduced[q]] * scale + pose.Position + offset; + var b = points[reduced[previousIndex]] * scale + pose.Position + offset; + previousIndex = q; + renderer.Lines.Allocate() = new LineInstance(a, b, color, Vector3.Zero); } - else + } + else + { + for (int k = 2; k < reduced.Length; ++k) { - for (int k = faceStart + 2; k < faceEnd; ++k) + renderer.Shapes.AddShape(new Triangle { - renderer.Shapes.AddShape(new Triangle - { - A = points[step.FaceIndices[faceStart]] * scale + offset, - B = points[step.FaceIndices[k]] * scale + offset, - C = points[step.FaceIndices[k - 1]] * scale + offset - }, Simulation.Shapes, pose, color); - } + A = points[reduced[0]] * scale + offset, + B = points[reduced[k]] * scale + offset, + C = points[reduced[k - 1]] * scale + offset + }, Simulation.Shapes, pose, color); } } } - } - //Console.WriteLine("Current step edges: "); - //for (int i = 0; i < step.Reduced.Count; ++i) - //{ - // Console.WriteLine($" Edge {i}: ({step.Reduced[i]}, {step.Reduced[(i + 1) % step.Reduced.Count]})"); - //} + for (int i = 0; i <= stepIndex; ++i) + { + var localStep = debugSteps[i]; + DrawFace(localStep, localStep.Raw, localStep.Reduced, false, i); + if (localStep.RawOverwrittenByMerge != null) + { + DrawFace(localStep, localStep.RawOverwrittenByMerge, localStep.ReducedOverwrittenByMerge, true, i); + } + } + } + Console.WriteLine($"face count: {step.FaceStarts.Count}"); if (showVertexIndices) { @@ -829,6 +817,7 @@ void DrawVertexIndex(int i, Vector3 color, Vector2 offset = default) renderer.TextBatcher.Write(text.Clear().Append("Show deleted: U ").Append(showDeleted ? "(on)" : "(off)"), new Vector2(32, renderer.Surface.Resolution.Y - 100), 20, new Vector3(1), font); renderer.TextBatcher.Write(text.Clear().Append("Show vertex indices: Y ").Append(showVertexIndices ? "(on)" : "(off)"), new Vector2(32, renderer.Surface.Resolution.Y - 80), 20, new Vector3(1), font); renderer.TextBatcher.Write(text.Clear().Append("Show face vertex statuses: H ").Append(showFaceVertexStatuses ? "(on)" : "(off)"), new Vector2(32, renderer.Surface.Resolution.Y - 60), 20, new Vector3(1), font); + renderer.TextBatcher.Write(text.Clear().Append("Face count: ").Append(step.FaceStarts.Count), new Vector2(32, renderer.Surface.Resolution.Y - 20), 20, new Vector3(1), font); base.Render(renderer, camera, input, text, font); From d9b3729a4e577e7017e7774f3f9b4add2e32d733 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 16 Nov 2024 18:58:28 -0600 Subject: [PATCH 887/947] Debug improvements and limited edge filtering. --- BepuPhysics/Collidables/ConvexHullHelper.cs | 34 ++++++++++++-------- Demos/SpecializedTests/ConvexHullTestDemo.cs | 8 ++--- 2 files changed, 25 insertions(+), 17 deletions(-) diff --git a/BepuPhysics/Collidables/ConvexHullHelper.cs b/BepuPhysics/Collidables/ConvexHullHelper.cs index 4560f1888..e5f8267db 100644 --- a/BepuPhysics/Collidables/ConvexHullHelper.cs +++ b/BepuPhysics/Collidables/ConvexHullHelper.cs @@ -1,4 +1,5 @@ -using BepuUtilities; +using BepuPhysics.Constraints.Contact; +using BepuUtilities; using BepuUtilities.Collections; using BepuUtilities.Memory; using System; @@ -506,12 +507,15 @@ static void AddFaceEdgesToTestList(BufferPool pool, endpoints.A = previousIndex; endpoints.B = reducedFaceIndices[i]; previousIndex = endpoints.B; - EdgeToTest nextEdgeToTest; - nextEdgeToTest.Endpoints = endpoints; - nextEdgeToTest.FaceNormal = faceNormal; - nextEdgeToTest.FaceIndex = newFaceIndex; - edgesToTest.Allocate(pool) = nextEdgeToTest; - submittedEdgeTests.Add(endpoints, pool); + if (!submittedEdgeTests.Contains(endpoints)) + { + EdgeToTest nextEdgeToTest; + nextEdgeToTest.Endpoints = endpoints; + nextEdgeToTest.FaceNormal = faceNormal; + nextEdgeToTest.FaceIndex = newFaceIndex; + edgesToTest.Allocate(pool) = nextEdgeToTest; + submittedEdgeTests.Add(endpoints, pool); + } } } @@ -526,8 +530,7 @@ public struct DebugStep public EdgeEndpoints SourceEdge; public int[] Raw; public int[] Reduced; - public int[] RawOverwrittenByMerge; - public int[] ReducedOverwrittenByMerge; + public int[] OverwrittenOriginal; public bool[] AllowVertex; public Vector3 FaceNormal; public Vector3 BasisX; @@ -546,8 +549,7 @@ internal DebugStep(EdgeEndpoints sourceEdge, QuickList rawVertexIndices, Ve BasisY = basisY; Raw = ((Span)rawVertexIndices).ToArray(); Reduced = ((Span)reducedVertexIndices).ToArray(); - RawOverwrittenByMerge = null; - ReducedOverwrittenByMerge = null; + OverwrittenOriginal = null; FaceIndex = faceIndex; } @@ -572,10 +574,13 @@ internal DebugStep FillHistory(Buffer allowVertex, QuickList fac return this; } + internal void RecordDeletedFace(QuickList faceVertexIndices) + { + OverwrittenOriginal = ((Span)faceVertexIndices).ToArray(); + } + internal void UpdateForFaceMerge(QuickList rawFaceVertexIndices, QuickList reducedVertexIndices, Buffer allowVertex, int mergedFaceIndex) { - RawOverwrittenByMerge = Raw; - ReducedOverwrittenByMerge = Reduced; Raw = ((Span)rawFaceVertexIndices).ToArray(); Reduced = ((Span)reducedVertexIndices).ToArray(); FaceIndex = mergedFaceIndex; @@ -815,12 +820,15 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa } } // Rerun reduction for the merged face. + step.RecordDeletedFace(face.VertexIndices); face.VertexIndices.Count = 0; facePoints.Count = 0; face.VertexIndices.EnsureCapacity(rawFaceVertexIndices.Count, pool); ReduceFace(ref rawFaceVertexIndices, faceNormal, points, planeEpsilonNarrow, ref facePoints, ref allowVertices, ref face.VertexIndices); step.UpdateForFaceMerge(rawFaceVertexIndices, face.VertexIndices, allowVertices, i); mergedFace = true; + + // It's possible for the merged face to have invalidated a previous face that wouldn't necessarily be detected as something to merge. break; } } diff --git a/Demos/SpecializedTests/ConvexHullTestDemo.cs b/Demos/SpecializedTests/ConvexHullTestDemo.cs index 4ce7a485f..3e7d25726 100644 --- a/Demos/SpecializedTests/ConvexHullTestDemo.cs +++ b/Demos/SpecializedTests/ConvexHullTestDemo.cs @@ -756,7 +756,7 @@ void DrawVertexIndex(int i, Vector3 color, Vector2 offset = default) { var pose = new RigidPose(renderOffset); - void DrawFace(DebugStep step, int[] raw, int[] reduced, bool deleted, int i) + void DrawFace(DebugStep step, int[] reduced, bool deleted, int i) { var color = deleted ? new Vector3(0.25f, 0.25f, 0.25f) : stepIndex == i ? new Vector3(1, 0, 0.5f) : new Vector3(1, 0, 1); var deletionInducedScale = deleted ? new Vector3(1.1f) : new Vector3(1f); @@ -789,10 +789,10 @@ void DrawFace(DebugStep step, int[] raw, int[] reduced, bool deleted, int i) for (int i = 0; i <= stepIndex; ++i) { var localStep = debugSteps[i]; - DrawFace(localStep, localStep.Raw, localStep.Reduced, false, i); - if (localStep.RawOverwrittenByMerge != null) + DrawFace(localStep, localStep.Reduced, false, i); + if (localStep.OverwrittenOriginal != null) { - DrawFace(localStep, localStep.RawOverwrittenByMerge, localStep.ReducedOverwrittenByMerge, true, i); + DrawFace(localStep, localStep.OverwrittenOriginal, true, i); } } From 3a35365e420ad85e73ec1e399cc8339b75111fe6 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 16 Nov 2024 19:23:51 -0600 Subject: [PATCH 888/947] More aggressive filtering. --- BepuPhysics/Collidables/ConvexHullHelper.cs | 26 ++++++++++++++------ Demos/SpecializedTests/ConvexHullTestDemo.cs | 2 +- 2 files changed, 19 insertions(+), 9 deletions(-) diff --git a/BepuPhysics/Collidables/ConvexHullHelper.cs b/BepuPhysics/Collidables/ConvexHullHelper.cs index e5f8267db..54d570c12 100644 --- a/BepuPhysics/Collidables/ConvexHullHelper.cs +++ b/BepuPhysics/Collidables/ConvexHullHelper.cs @@ -497,7 +497,7 @@ static void AddFace(ref QuickList faces, BufferPool pool, Vector3 nor static void AddFaceEdgesToTestList(BufferPool pool, ref QuickList reducedFaceIndices, ref QuickList edgesToTest, - ref QuickSet submittedEdgeTests, + ref QuickDictionary edgeFaceCounts, Vector3 faceNormal, int newFaceIndex) { var previousIndex = reducedFaceIndices[reducedFaceIndices.Count - 1]; @@ -507,14 +507,19 @@ static void AddFaceEdgesToTestList(BufferPool pool, endpoints.A = previousIndex; endpoints.B = reducedFaceIndices[i]; previousIndex = endpoints.B; - if (!submittedEdgeTests.Contains(endpoints)) + if (!edgeFaceCounts.FindOrAllocateSlot(ref endpoints, pool, out var slotIndex)) { EdgeToTest nextEdgeToTest; nextEdgeToTest.Endpoints = endpoints; nextEdgeToTest.FaceNormal = faceNormal; nextEdgeToTest.FaceIndex = newFaceIndex; edgesToTest.Allocate(pool) = nextEdgeToTest; - submittedEdgeTests.Add(endpoints, pool); + edgeFaceCounts.Values[slotIndex] = 1; + } + else + { + //No need to test this edge; it's already been submitted by a different face. + edgeFaceCounts.Values[slotIndex]++; } } } @@ -549,7 +554,7 @@ internal DebugStep(EdgeEndpoints sourceEdge, QuickList rawVertexIndices, Ve BasisY = basisY; Raw = ((Span)rawVertexIndices).ToArray(); Reduced = ((Span)reducedVertexIndices).ToArray(); - OverwrittenOriginal = null; + OverwrittenOriginal = null; FaceIndex = faceIndex; } @@ -566,7 +571,7 @@ internal DebugStep FillHistory(Buffer allowVertex, QuickList fac FaceIndices.Add(face.VertexIndices[j]); FaceNormals[i] = face.Normal; } - AllowVertex = new bool[allowVertex.Length]; + AllowVertex = new bool[allowVertex.Length]; for (int i = 0; i < allowVertex.Length; ++i) { AllowVertex[i] = allowVertex[i] != 0; @@ -729,7 +734,7 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa var faces = new QuickList(points.Length, pool); var edgesToTest = new QuickList(points.Length, pool); - var submittedEdgeTests = new QuickSet(points.Length, pool); + var edgeFaceCounts = new QuickDictionary(points.Length, pool); if (reducedFaceIndices.Count >= 3) { //The initial face search found an actual face! That's a bit surprising since we didn't start from an edge offset, but rather an arbitrary direction. @@ -768,6 +773,11 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa while (edgesToTest.Count > 0) { edgesToTest.Pop(out var edgeToTest); + if (edgeFaceCounts.TryGetValue(ref edgeToTest.Endpoints, out var edgeFaceCount) && edgeFaceCount >= 2) + { + //This edge is already part of two faces; no need to test it further. + continue; + } ref var edgeA = ref points[edgeToTest.Endpoints.A]; ref var edgeB = ref points[edgeToTest.Endpoints.B]; @@ -827,7 +837,7 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa ReduceFace(ref rawFaceVertexIndices, faceNormal, points, planeEpsilonNarrow, ref facePoints, ref allowVertices, ref face.VertexIndices); step.UpdateForFaceMerge(rawFaceVertexIndices, face.VertexIndices, allowVertices, i); mergedFace = true; - + // It's possible for the merged face to have invalidated a previous face that wouldn't necessarily be detected as something to merge. break; } @@ -836,7 +846,7 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa { var faceCountPriorToAdd = faces.Count; AddFace(ref faces, pool, faceNormal, reducedFaceIndices); - AddFaceEdgesToTestList(pool, ref reducedFaceIndices, ref edgesToTest, ref submittedEdgeTests, faceNormal, faceCountPriorToAdd); + AddFaceEdgesToTestList(pool, ref reducedFaceIndices, ref edgesToTest, ref edgeFaceCounts, faceNormal, faceCountPriorToAdd); } step.FillHistory(allowVertices, faces); steps.Add(step); diff --git a/Demos/SpecializedTests/ConvexHullTestDemo.cs b/Demos/SpecializedTests/ConvexHullTestDemo.cs index 3e7d25726..2d73d9db2 100644 --- a/Demos/SpecializedTests/ConvexHullTestDemo.cs +++ b/Demos/SpecializedTests/ConvexHullTestDemo.cs @@ -790,7 +790,7 @@ void DrawFace(DebugStep step, int[] reduced, bool deleted, int i) { var localStep = debugSteps[i]; DrawFace(localStep, localStep.Reduced, false, i); - if (localStep.OverwrittenOriginal != null) + if (showDeleted && localStep.OverwrittenOriginal != null) { DrawFace(localStep, localStep.OverwrittenOriginal, true, i); } From 9b8d261bcdd06c9228dbed46ba7e52a57489ccc2 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 12 Jan 2025 22:36:00 -0600 Subject: [PATCH 889/947] Disallowed vertex paranoid cleanup. --- BepuPhysics/Collidables/ConvexHullHelper.cs | 75 +++++++++++++++++++-- 1 file changed, 69 insertions(+), 6 deletions(-) diff --git a/BepuPhysics/Collidables/ConvexHullHelper.cs b/BepuPhysics/Collidables/ConvexHullHelper.cs index 54d570c12..d1fdde09a 100644 --- a/BepuPhysics/Collidables/ConvexHullHelper.cs +++ b/BepuPhysics/Collidables/ConvexHullHelper.cs @@ -483,7 +483,6 @@ struct EdgeToTest { public EdgeEndpoints Endpoints; public Vector3 FaceNormal; - public int FaceIndex; } @@ -512,7 +511,6 @@ static void AddFaceEdgesToTestList(BufferPool pool, EdgeToTest nextEdgeToTest; nextEdgeToTest.Endpoints = endpoints; nextEdgeToTest.FaceNormal = faceNormal; - nextEdgeToTest.FaceIndex = newFaceIndex; edgesToTest.Allocate(pool) = nextEdgeToTest; edgeFaceCounts.Values[slotIndex] = 1; } @@ -536,6 +534,7 @@ public struct DebugStep public int[] Raw; public int[] Reduced; public int[] OverwrittenOriginal; + public List DeletedFaces; public bool[] AllowVertex; public Vector3 FaceNormal; public Vector3 BasisX; @@ -556,6 +555,7 @@ internal DebugStep(EdgeEndpoints sourceEdge, QuickList rawVertexIndices, Ve Reduced = ((Span)reducedVertexIndices).ToArray(); OverwrittenOriginal = null; FaceIndex = faceIndex; + DeletedFaces = new List(); } internal DebugStep FillHistory(Buffer allowVertex, QuickList faces) @@ -579,11 +579,24 @@ internal DebugStep FillHistory(Buffer allowVertex, QuickList fac return this; } - internal void RecordDeletedFace(QuickList faceVertexIndices) + /// + /// Records the vertex indices corresponding to a face that was overwritten by a new face created by merging a newly-discovered face and an existing face due to normal similarity. + /// + /// Face vertex indices of the original face that's being overwritten. + internal void RecordOverwrittenFace(QuickList faceVertexIndices) { OverwrittenOriginal = ((Span)faceVertexIndices).ToArray(); } + /// + /// Records the vertex indices corresponding to a face that was deleted for being associated with now-disallowed vertices downstream of a face merge. + /// + /// Vertices of the face that was deleted. + internal void RecordDeletedFace(QuickList faceVertexIndices) + { + DeletedFaces.Add(((Span)faceVertexIndices).ToArray()); + } + internal void UpdateForFaceMerge(QuickList rawFaceVertexIndices, QuickList reducedVertexIndices, Buffer allowVertex, int mergedFaceIndex) { Raw = ((Span)rawFaceVertexIndices).ToArray(); @@ -745,7 +758,6 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa edgeToAdd.Endpoints.A = reducedFaceIndices[i == 0 ? reducedFaceIndices.Count - 1 : i - 1]; edgeToAdd.Endpoints.B = reducedFaceIndices[i]; edgeToAdd.FaceNormal = initialFaceNormal; - edgeToAdd.FaceIndex = 0; } //Since an actual face was found, we go ahead and output it into the face set. AddFace(ref faces, pool, initialFaceNormal, reducedFaceIndices); @@ -759,7 +771,6 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa edgeToAdd.Endpoints.A = reducedFaceIndices[0]; edgeToAdd.Endpoints.B = reducedFaceIndices[1]; edgeToAdd.FaceNormal = initialFaceNormal; - edgeToAdd.FaceIndex = -1; var edgeOffset = points[edgeToAdd.Endpoints.B] - points[edgeToAdd.Endpoints.A]; var basisY = Vector3.Cross(edgeOffset, edgeToAdd.FaceNormal); var basisX = Vector3.Cross(edgeOffset, basisY); @@ -830,7 +841,7 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa } } // Rerun reduction for the merged face. - step.RecordDeletedFace(face.VertexIndices); + step.RecordOverwrittenFace(face.VertexIndices); face.VertexIndices.Count = 0; facePoints.Count = 0; face.VertexIndices.EnsureCapacity(rawFaceVertexIndices.Count, pool); @@ -848,6 +859,58 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa AddFace(ref faces, pool, faceNormal, reducedFaceIndices); AddFaceEdgesToTestList(pool, ref reducedFaceIndices, ref edgesToTest, ref edgeFaceCounts, faceNormal, faceCountPriorToAdd); } + // Check all faces for use of disallowed vertices. + var deletedFaceCount = 0; + for (int i = 0; i < faces.Count; ++i) + { + ref var face = ref faces[i]; + bool deletedFace = false; + for (int j = 0; j < face.VertexIndices.Count; ++j) + { + if (allowVertices[face.VertexIndices[j]] == 0) + { + ++deletedFaceCount; + deletedFace = true; + break; + } + } + if (deletedFace) + { + Console.WriteLine($"Deleting face {i}"); + step.RecordDeletedFace(face.VertexIndices); + // Edges may have been exposed by the deletion of the face. + // Adjust the edge-face counts. + for (int j = 0; j < face.VertexIndices.Count; ++j) + { + var previousIndex = face.VertexIndices[j == 0 ? face.VertexIndices.Count - 1 : j - 1]; + var nextIndex = face.VertexIndices[j]; + // NOTE A CRITICAL SUBTLETY: + // The edge endpoints are flipped from the usual submission order. + // That's because the usual submission is trying to find faces *outside* the current face (since it just got added). + // Here, we're leaving a void and we want to fill it. + var endpoints = new EdgeEndpoints { A = nextIndex, B = previousIndex }; + if (edgeFaceCounts.GetTableIndices(ref endpoints, out var tableIndex, out var elementIndex)) + { + ref var countForEdge = ref edgeFaceCounts.Values[elementIndex]; + if (allowVertices[endpoints.A] != 0 && allowVertices[endpoints.B] != 0) + { + // This edge connects still-valid vertices, and by removing a face from it, it's conceivable that we've opened a hole. + // Note that the face normal we're using here is not actually 'correct'; it should be the face normal of the *other* face on this edge. + // We're shrugging about this because the deleted face should still be able to offer a normal that fills the hole... + edgesToTest.Add(new EdgeToTest { Endpoints = endpoints, FaceNormal = face.Normal }, pool); + } + } + } + + face.VertexIndices.Dispose(pool); + } + if (!deletedFace && deletedFaceCount > 0) + { + // Shift the face back to fill in the gap. + faces[i - deletedFaceCount] = faces[i]; + } + } + faces.Count -= deletedFaceCount; step.FillHistory(allowVertices, faces); steps.Add(step); From 291c27f964730611c527f1b05c0030eef127d83f Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 13 Jan 2025 21:20:34 -0600 Subject: [PATCH 890/947] Not-quite-working debug visualization. --- Demos/SpecializedTests/ConvexHullTestDemo.cs | 93 +++++++++++++------- 1 file changed, 62 insertions(+), 31 deletions(-) diff --git a/Demos/SpecializedTests/ConvexHullTestDemo.cs b/Demos/SpecializedTests/ConvexHullTestDemo.cs index 2d73d9db2..5119218aa 100644 --- a/Demos/SpecializedTests/ConvexHullTestDemo.cs +++ b/Demos/SpecializedTests/ConvexHullTestDemo.cs @@ -754,50 +754,81 @@ void DrawVertexIndex(int i, Vector3 color, Vector2 offset = default) } } + void DrawFace(DebugStep step, int[] reduced, Vector3 color, float offsetScale) { - var pose = new RigidPose(renderOffset); - void DrawFace(DebugStep step, int[] reduced, bool deleted, int i) + var offset = step.FaceNormal * offsetScale; + if (showWireframe) { - var color = deleted ? new Vector3(0.25f, 0.25f, 0.25f) : stepIndex == i ? new Vector3(1, 0, 0.5f) : new Vector3(1, 0, 1); - var deletionInducedScale = deleted ? new Vector3(1.1f) : new Vector3(1f); - - var offset = deleted ? step.FaceNormal * 0.25f : new Vector3(); - if (showWireframe) + var previousIndex = reduced.Length - 1; + for (int q = 0; q < reduced.Length; ++q) { - var previousIndex = reduced.Length - 1; - for (int q = 0; q < reduced.Length; ++q) - { - var a = points[reduced[q]] * scale + pose.Position + offset; - var b = points[reduced[previousIndex]] * scale + pose.Position + offset; - previousIndex = q; - renderer.Lines.Allocate() = new LineInstance(a, b, color, Vector3.Zero); - } + var a = points[reduced[q]] * scale + renderOffset + offset; + var b = points[reduced[previousIndex]] * scale + renderOffset + offset; + previousIndex = q; + renderer.Lines.Allocate() = new LineInstance(a, b, color, Vector3.Zero); } - else + } + else + { + for (int k = 2; k < reduced.Length; ++k) { - for (int k = 2; k < reduced.Length; ++k) + renderer.Shapes.AddShape(new Triangle { - renderer.Shapes.AddShape(new Triangle - { - A = points[reduced[0]] * scale + offset, - B = points[reduced[k]] * scale + offset, - C = points[reduced[k - 1]] * scale + offset - }, Simulation.Shapes, pose, color); - } + A = points[reduced[0]] * scale + offset, + B = points[reduced[k]] * scale + offset, + C = points[reduced[k - 1]] * scale + offset + }, Simulation.Shapes, renderOffset, color); } } - for (int i = 0; i <= stepIndex; ++i) + } + + if (showDeleted && step.OverwrittenOriginal != null) + { + DrawFace(step, step.OverwrittenOriginal, new Vector3(0.5f, 0.1f, 0.1f), 0.25f); + for (int j = 0; j < step.DeletedFaces.Count; ++j) + DrawFace(step, step.DeletedFaces[j], new Vector3(0.1f, 0.1f, 0.1f), 0.25f); + } + + // Render all current faces in the step + for (int faceIndex = 0; faceIndex < step.FaceStarts.Count; ++faceIndex) + { + var startIndex = step.FaceStarts[faceIndex]; + var endIndex = faceIndex < step.FaceStarts.Count - 1 ? step.FaceStarts[faceIndex + 1] : step.FaceIndices.Count; + var faceVertexCount = endIndex - startIndex; + var faceNormal = step.FaceNormals[faceIndex]; + + var color = step.FaceIndex == faceIndex ? new Vector3(1, 0, 0.5f) : new Vector3(1, 0, 1); + + if (showWireframe) + { + // Draw wireframe edges for the face + var previousIndex = endIndex - 1; + for (int vertexIndex = startIndex; vertexIndex < endIndex; ++vertexIndex) + { + var a = points[step.FaceIndices[vertexIndex]] * scale + renderOffset; + var b = points[step.FaceIndices[previousIndex]] * scale + renderOffset; + previousIndex = vertexIndex; + renderer.Lines.Allocate() = new LineInstance(a, b, color, Vector3.Zero); + } + } + else { - var localStep = debugSteps[i]; - DrawFace(localStep, localStep.Reduced, false, i); - if (showDeleted && localStep.OverwrittenOriginal != null) + // Draw filled triangles for the face + var baseIndex = step.FaceIndices[startIndex]; + var basePoint = points[baseIndex] * scale; + for (int vertexIndex = startIndex + 2; vertexIndex < endIndex; ++vertexIndex) { - DrawFace(localStep, localStep.OverwrittenOriginal, true, i); + renderer.Shapes.AddShape(new Triangle + { + A = basePoint, + B = points[step.FaceIndices[vertexIndex]] * scale, + C = points[step.FaceIndices[vertexIndex - 1]] * scale + }, Simulation.Shapes, renderOffset, color); } - } } - Console.WriteLine($"face count: {step.FaceStarts.Count}"); + + //Console.WriteLine($"face count: {step.FaceStarts.Count}"); if (showVertexIndices) { From bf45c5b082b1e6b63384ad22985c67d3c8ac6752 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 18 Jan 2025 23:19:53 -0600 Subject: [PATCH 891/947] Moving toward a wee bit of consistency in testing. --- BepuPhysics/Collidables/ConvexHullHelper.cs | 31 +- Demos/SpecializedTests/ConvexHullTestDemo.cs | 541 ++++++++++--------- 2 files changed, 294 insertions(+), 278 deletions(-) diff --git a/BepuPhysics/Collidables/ConvexHullHelper.cs b/BepuPhysics/Collidables/ConvexHullHelper.cs index d1fdde09a..9fca877ce 100644 --- a/BepuPhysics/Collidables/ConvexHullHelper.cs +++ b/BepuPhysics/Collidables/ConvexHullHelper.cs @@ -1,4 +1,4 @@ -using BepuPhysics.Constraints.Contact; +#define DEBUG_STEPS using BepuUtilities; using BepuUtilities.Collections; using BepuUtilities.Memory; @@ -9,6 +9,7 @@ using System.Runtime.CompilerServices; using System.Runtime.InteropServices; + namespace BepuPhysics.Collidables { /// @@ -522,12 +523,7 @@ static void AddFaceEdgesToTestList(BufferPool pool, } } - static void AddIfNotPresent(ref QuickList list, int value, BufferPool pool) - { - if (!list.Contains(value)) - list.Allocate(pool) = value; - } - +#if DEBUG_STEPS public struct DebugStep { public EdgeEndpoints SourceEdge; @@ -614,7 +610,7 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa { ComputeHull(points, pool, out hullData, out _); } - +#endif /// /// Computes the convex hull of a set of points. @@ -622,7 +618,11 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa /// Point set to compute the convex hull of. /// Buffer pool to pull memory from when creating the hull. /// Convex hull of the input point set. +#if DEBUG_STEPS public static void ComputeHull(Span points, BufferPool pool, out HullData hullData, out List steps) +#else + public static void ComputeHull(Span points, BufferPool pool, out HullData hullData) +#endif { steps = new List(); if (points.Length <= 0) @@ -777,9 +777,11 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa if (Vector3.Dot(basisX, edgeToAdd.FaceNormal) > 0) Helpers.Swap(ref edgeToAdd.Endpoints.A, ref edgeToAdd.Endpoints.B); } +#if DEBUG_STEPS Vector3Wide.ReadFirst(initialBasisX, out var debugInitialBasisX); Vector3Wide.ReadFirst(initialBasisY, out var debugInitialBasisY); steps.Add(new DebugStep(initialSourceEdge, rawFaceVertexIndices, initialFaceNormal, debugInitialBasisX, debugInitialBasisY, reducedFaceIndices, reducedFaceIndices.Count >= 3 ? 0 : -1).FillHistory(allowVertices, faces)); +#endif while (edgesToTest.Count > 0) { @@ -811,13 +813,17 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa if (reducedFaceIndices.Count < 3) { +#if DEBUG_STEPS steps.Add(new DebugStep(edgeToTest.Endpoints, rawFaceVertexIndices, faceNormal, basisX, basisY, reducedFaceIndices, -1).FillHistory(allowVertices, faces)); +#endif //Degenerate face found; don't bother creating work for it. continue; } // Brute force scan all the faces to see if the new face is coplanar with any of them. +#if DEBUG_STEPS var step = new DebugStep(edgeToTest.Endpoints, rawFaceVertexIndices, faceNormal, basisX, basisY, reducedFaceIndices, faces.Count); Console.WriteLine($"step count: {steps.Count}"); +#endif bool mergedFace = false; for (int i = 0; i < faces.Count; ++i) { @@ -841,12 +847,16 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa } } // Rerun reduction for the merged face. +#if DEBUG_STEPS step.RecordOverwrittenFace(face.VertexIndices); +#endif face.VertexIndices.Count = 0; facePoints.Count = 0; face.VertexIndices.EnsureCapacity(rawFaceVertexIndices.Count, pool); ReduceFace(ref rawFaceVertexIndices, faceNormal, points, planeEpsilonNarrow, ref facePoints, ref allowVertices, ref face.VertexIndices); +#if DEBUG_STEPS step.UpdateForFaceMerge(rawFaceVertexIndices, face.VertexIndices, allowVertices, i); +#endif mergedFace = true; // It's possible for the merged face to have invalidated a previous face that wouldn't necessarily be detected as something to merge. @@ -876,8 +886,10 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa } if (deletedFace) { +#if DEBUG_STEPS Console.WriteLine($"Deleting face {i}"); step.RecordDeletedFace(face.VertexIndices); +#endif // Edges may have been exposed by the deletion of the face. // Adjust the edge-face counts. for (int j = 0; j < face.VertexIndices.Count; ++j) @@ -911,11 +923,12 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa } } faces.Count -= deletedFaceCount; +#if DEBUG_STEPS step.FillHistory(allowVertices, faces); steps.Add(step); - if (steps.Count > 500) break; +#endif } edgesToTest.Dispose(pool); diff --git a/Demos/SpecializedTests/ConvexHullTestDemo.cs b/Demos/SpecializedTests/ConvexHullTestDemo.cs index 5119218aa..65725cd86 100644 --- a/Demos/SpecializedTests/ConvexHullTestDemo.cs +++ b/Demos/SpecializedTests/ConvexHullTestDemo.cs @@ -1,4 +1,5 @@ -using System; +#define DEBUG_STEPS +using System; using System.Collections.Generic; using System.Numerics; using BepuPhysics.Collidables; @@ -7,7 +8,6 @@ using DemoRenderer; using BepuPhysics; using static BepuPhysics.Collidables.ConvexHullHelper; -using System.Diagnostics; using BepuUtilities; using BepuPhysics.Constraints; using BepuUtilities.Memory; @@ -17,26 +17,24 @@ using DemoUtilities; using DemoRenderer.UI; + + namespace Demos.SpecializedTests; public class ConvexHullTestDemo : Demo { - Buffer CreateRandomConvexHullPoints() + Vector3[] CreateRandomConvexHullPoints() { - const int pointCount = 50; - BufferPool.Take(pointCount, out var points); - + var points = new Vector3[50]; var random = new Random(5); - for (int i = 0; i < pointCount; ++i) + for (int i = 0; i < points.Length; ++i) { - points[i] = new Vector3(3 * random.NextSingle(), 1 * random.NextSingle(), 3 * random.NextSingle()); + points[i] = new(3 * random.NextSingle(), 1 * random.NextSingle(), 3 * random.NextSingle()); } return points; } - - Vector3[] CreateBwaa() { var points = new Vector3[] @@ -169,49 +167,53 @@ Vector3[] CreateBwaa() return points; } - Buffer CreatePlaneish() + Vector3[] CreatePlaneish() { - var points = new Buffer(12, BufferPool); - points[0] = new Vector3(-13.82f, 16.79f, 13.83f); - points[1] = new Vector3(13.82f, -16.79f, -13.83f); - points[2] = new Vector3(13.82f, 16.79f, -13.83f); - points[3] = new Vector3(-13.82f, 16.79f, 13.83f); - points[4] = new Vector3(13.82f, 16.79f, -13.83f); - points[5] = new Vector3(-13.82f, -16.79f, 13.83f); - points[6] = new Vector3(13.82f, 16.79f, -13.83f); - points[7] = new Vector3(13.82f, -16.79f, -13.83f); - points[8] = new Vector3(-13.82f, -16.79f, 13.83f); - points[9] = new Vector3(-13.82f, 16.79f, 13.83f); - points[10] = new Vector3(-13.82f, -16.79f, 13.83f); - points[11] = new Vector3(13.82f, -16.79f, -13.83f); + var points = new Vector3[] + { + new(-13.82f, 16.79f, 13.83f), + new(13.82f, -16.79f, -13.83f), + new(13.82f, 16.79f, -13.83f), + new(-13.82f, 16.79f, 13.83f), + new(13.82f, 16.79f, -13.83f), + new(-13.82f, -16.79f, 13.83f), + new(13.82f, 16.79f, -13.83f), + new(13.82f, -16.79f, -13.83f), + new(-13.82f, -16.79f, 13.83f), + new(-13.82f, 16.79f, 13.83f), + new(-13.82f, -16.79f, 13.83f), + new(13.82f, -16.79f, -13.83f), + }; return points; } - Buffer CreateDistantPlane() + Vector3[] CreateDistantPlane() { - var points = new Buffer(12, BufferPool); - points[0] = new(-151.0875f, -2.2505488f, 102.17515f); - points[1] = new(-151.10571f, 2.1121342f, -17.699797f); - points[2] = new(-151.08746f, -2.2504745f, -17.699797f); - points[3] = new(-151.10571f, 2.1121342f, -17.699797f); - points[4] = new(-151.0875f, -2.2505488f, 102.17515f); - points[5] = new(-151.10574f, 2.1120775f, 102.17517f); - points[6] = new(-151.10571f, 2.1121342f, -17.699797f); - points[7] = new(-151.10574f, 2.1120775f, 102.17517f); - points[8] = new(-151.08746f, -2.2504745f, -17.699797f); - points[9] = new(-151.08746f, -2.2504745f, -17.699797f); - points[10] = new(-151.10574f, 2.1120775f, 102.17517f); - points[11] = new(-151.0875f, -2.2505488f, 102.17515f); + var points = new Vector3[] + { + new(-151.0875f, -2.2505488f, 102.17515f), + new(-151.10571f, 2.1121342f, -17.699797f), + new(-151.08746f, -2.2504745f, -17.699797f), + new(-151.10571f, 2.1121342f, -17.699797f), + new(-151.0875f, -2.2505488f, 102.17515f), + new(-151.10574f, 2.1120775f, 102.17517f), + new(-151.10571f, 2.1121342f, -17.699797f), + new(-151.10574f, 2.1120775f, 102.17517f), + new(-151.08746f, -2.2504745f, -17.699797f), + new(-151.08746f, -2.2504745f, -17.699797f), + new(-151.10574f, 2.1120775f, 102.17517f), + new(-151.0875f, -2.2505488f, 102.17515f), + }; return points; } - Buffer CreateMeshConvexHull(MeshContent meshContent, Vector3 scale) + Vector3[] CreateMeshConvexHull(MeshContent meshContent, Vector3 scale) { //This is actually a pretty good example of how *not* to make a convex hull shape. //Generating it directly from a graphical data source tends to have way more surface complexity than needed, //and it tends to have a lot of near-but-not-quite-coplanar surfaces which can make the contact manifold less stable. //Prefer a simpler source with more distinct features, possibly created with an automated content-time tool. - BufferPool.Take(meshContent.Triangles.Length * 3, out var points); + var points = new Vector3[meshContent.Triangles.Length * 3]; for (int i = 0; i < meshContent.Triangles.Length; ++i) { ref var triangle = ref meshContent.Triangles[i]; @@ -223,233 +225,241 @@ Buffer CreateMeshConvexHull(MeshContent meshContent, Vector3 scale) return points; } - Buffer CreateBoxConvexHull(float boxScale) + Vector3[] CreateBoxConvexHull(float boxScale) { - BufferPool.Take(8, out var points); - points[0] = new Vector3(0, 0, 0); - points[1] = new Vector3(0, 0, boxScale); - points[2] = new Vector3(0, boxScale, 0); - points[3] = new Vector3(0, boxScale, boxScale); - points[4] = new Vector3(boxScale, 0, 0); - points[5] = new Vector3(boxScale, 0, boxScale); - points[6] = new Vector3(boxScale, boxScale, 0); - points[7] = new Vector3(boxScale, boxScale, boxScale); + var points = new Vector3[] + { + new(0, 0, 0), + new(0, 0, boxScale), + new(0, boxScale, 0), + new(0, boxScale, boxScale), + new(boxScale, 0, 0), + new(boxScale, 0, boxScale), + new(boxScale, boxScale, 0), + new(boxScale, boxScale, boxScale), + }; return points; } //A couple of test point sets from PEEL: https://github.com/Pierre-Terdiman/PEEL_PhysX_Edition - Buffer CreateTestConvexHull() + Vector3[] CreateTestConvexHull() { - BufferPool.Take(50, out var vertices); - vertices[0] = new Vector3(-0.000000f, -0.297120f, -0.000000f); - vertices[1] = new Vector3(0.258819f, -0.297120f, 0.965926f); - vertices[2] = new Vector3(-0.000000f, -0.297120f, 1.000000f); - vertices[3] = new Vector3(0.500000f, -0.297120f, 0.866026f); - vertices[4] = new Vector3(0.707107f, -0.297120f, 0.707107f); - vertices[5] = new Vector3(0.866026f, -0.297120f, 0.500000f); - vertices[6] = new Vector3(0.965926f, -0.297120f, 0.258819f); - vertices[7] = new Vector3(1.000000f, -0.297120f, -0.000000f); - vertices[8] = new Vector3(0.965926f, -0.297120f, -0.258819f); - vertices[9] = new Vector3(0.866026f, -0.297120f, -0.500000f); - vertices[10] = new Vector3(0.707107f, -0.297120f, -0.707107f); - vertices[11] = new Vector3(0.500000f, -0.297120f, -0.866026f); - vertices[12] = new Vector3(0.258819f, -0.297120f, -0.965926f); - vertices[13] = new Vector3(-0.000000f, -0.297120f, -1.000000f); - vertices[14] = new Vector3(-0.258819f, -0.297120f, -0.965926f); - vertices[15] = new Vector3(-0.500000f, -0.297120f, -0.866025f); - vertices[16] = new Vector3(-0.707107f, -0.297120f, -0.707107f); - vertices[17] = new Vector3(-0.866026f, -0.297120f, -0.500000f); - vertices[18] = new Vector3(-0.965926f, -0.297120f, -0.258819f); - vertices[19] = new Vector3(-1.000000f, -0.297120f, 0.000000f); - vertices[20] = new Vector3(-0.965926f, -0.297120f, 0.258819f); - vertices[21] = new Vector3(-0.866025f, -0.297120f, 0.500000f); - vertices[22] = new Vector3(-0.707107f, -0.297120f, 0.707107f); - vertices[23] = new Vector3(-0.500000f, -0.297120f, 0.866026f); - vertices[24] = new Vector3(-0.258819f, -0.297120f, 0.965926f); - vertices[25] = new Vector3(-0.000000f, 0.297120f, -0.000000f); - vertices[26] = new Vector3(-0.000000f, 0.297120f, 0.537813f); - vertices[27] = new Vector3(0.139196f, 0.297120f, 0.519487f); - vertices[28] = new Vector3(0.268907f, 0.297120f, 0.465760f); - vertices[29] = new Vector3(0.380291f, 0.297120f, 0.380291f); - vertices[30] = new Vector3(0.465760f, 0.297120f, 0.268907f); - vertices[31] = new Vector3(0.519487f, 0.297120f, 0.139196f); - vertices[32] = new Vector3(0.537813f, 0.297120f, -0.000000f); - vertices[33] = new Vector3(0.519487f, 0.297120f, -0.139196f); - vertices[34] = new Vector3(0.465760f, 0.297120f, -0.268907f); - vertices[35] = new Vector3(0.380291f, 0.297120f, -0.380291f); - vertices[36] = new Vector3(0.268907f, 0.297120f, -0.465760f); - vertices[37] = new Vector3(0.139196f, 0.297120f, -0.519487f); - vertices[38] = new Vector3(-0.000000f, 0.297120f, -0.537813f); - vertices[39] = new Vector3(-0.139196f, 0.297120f, -0.519487f); - vertices[40] = new Vector3(-0.268907f, 0.297120f, -0.465760f); - vertices[41] = new Vector3(-0.380291f, 0.297120f, -0.380291f); - vertices[42] = new Vector3(-0.465760f, 0.297120f, -0.268907f); - vertices[43] = new Vector3(-0.519487f, 0.297120f, -0.139196f); - vertices[44] = new Vector3(-0.537813f, 0.297120f, 0.000000f); - vertices[45] = new Vector3(-0.519487f, 0.297120f, 0.139196f); - vertices[46] = new Vector3(-0.465760f, 0.297120f, 0.268907f); - vertices[47] = new Vector3(-0.380291f, 0.297120f, 0.380291f); - vertices[48] = new Vector3(-0.268907f, 0.297120f, 0.465760f); - vertices[49] = new Vector3(-0.139196f, 0.297120f, 0.519487f); + var vertices = new Vector3[] + { + new(-0.000000f, -0.297120f, -0.000000f), + new(0.258819f, -0.297120f, 0.965926f), + new(-0.000000f, -0.297120f, 1.000000f), + new(0.500000f, -0.297120f, 0.866026f), + new(0.707107f, -0.297120f, 0.707107f), + new(0.866026f, -0.297120f, 0.500000f), + new(0.965926f, -0.297120f, 0.258819f), + new(1.000000f, -0.297120f, -0.000000f), + new(0.965926f, -0.297120f, -0.258819f), + new(0.866026f, -0.297120f, -0.500000f), + new(0.707107f, -0.297120f, -0.707107f), + new(0.500000f, -0.297120f, -0.866026f), + new(0.258819f, -0.297120f, -0.965926f), + new(-0.000000f, -0.297120f, -1.000000f), + new(-0.258819f, -0.297120f, -0.965926f), + new(-0.500000f, -0.297120f, -0.866025f), + new(-0.707107f, -0.297120f, -0.707107f), + new(-0.866026f, -0.297120f, -0.500000f), + new(-0.965926f, -0.297120f, -0.258819f), + new(-1.000000f, -0.297120f, 0.000000f), + new(-0.965926f, -0.297120f, 0.258819f), + new(-0.866025f, -0.297120f, 0.500000f), + new(-0.707107f, -0.297120f, 0.707107f), + new(-0.500000f, -0.297120f, 0.866026f), + new(-0.258819f, -0.297120f, 0.965926f), + new(-0.000000f, 0.297120f, -0.000000f), + new(-0.000000f, 0.297120f, 0.537813f), + new(0.139196f, 0.297120f, 0.519487f), + new(0.268907f, 0.297120f, 0.465760f), + new(0.380291f, 0.297120f, 0.380291f), + new(0.465760f, 0.297120f, 0.268907f), + new(0.519487f, 0.297120f, 0.139196f), + new(0.537813f, 0.297120f, -0.000000f), + new(0.519487f, 0.297120f, -0.139196f), + new(0.465760f, 0.297120f, -0.268907f), + new(0.380291f, 0.297120f, -0.380291f), + new(0.268907f, 0.297120f, -0.465760f), + new(0.139196f, 0.297120f, -0.519487f), + new(-0.000000f, 0.297120f, -0.537813f), + new(-0.139196f, 0.297120f, -0.519487f), + new(-0.268907f, 0.297120f, -0.465760f), + new(-0.380291f, 0.297120f, -0.380291f), + new(-0.465760f, 0.297120f, -0.268907f), + new(-0.519487f, 0.297120f, -0.139196f), + new(-0.537813f, 0.297120f, 0.000000f), + new(-0.519487f, 0.297120f, 0.139196f), + new(-0.465760f, 0.297120f, 0.268907f), + new(-0.380291f, 0.297120f, 0.380291f), + new(-0.268907f, 0.297120f, 0.465760f), + new(-0.139196f, 0.297120f, 0.519487f), + }; return vertices; } - Buffer CreateTestConvexHull2() + Vector3[] CreateTestConvexHull2() { - BufferPool.Take(120, out var vertices); - vertices[0] = new Vector3(0.153478f, 0.993671f, 0.124687f); - vertices[1] = new Vector3(0.153478f, 0.993671f, -0.117774f); - vertices[2] = new Vector3(-0.147939f, 0.993671f, -0.117774f); - vertices[3] = new Vector3(-0.147939f, 0.993671f, 0.124687f); - vertices[4] = new Vector3(0.137286f, 0.817392f, 0.586192f); - vertices[5] = new Vector3(0.333441f, 0.696161f, 0.661116f); - vertices[6] = new Vector3(0.484149f, 0.789305f, 0.417265f); - vertices[7] = new Vector3(0.287995f, 0.910536f, 0.342339f); - vertices[8] = new Vector3(0.794945f, 0.410936f, 0.484838f); - vertices[9] = new Vector3(0.916176f, 0.336012f, 0.288682f); - vertices[10] = new Vector3(0.823033f, 0.579863f, 0.137973f); - vertices[11] = new Vector3(0.701803f, 0.654787f, 0.334128f); - vertices[12] = new Vector3(0.916176f, 0.336012f, -0.281770f); - vertices[13] = new Vector3(0.794945f, 0.410936f, -0.477925f); - vertices[14] = new Vector3(0.701803f, 0.654787f, -0.327216f); - vertices[15] = new Vector3(0.823033f, 0.579863f, -0.131060f); - vertices[16] = new Vector3(0.333441f, 0.696161f, -0.654204f); - vertices[17] = new Vector3(0.137286f, 0.817392f, -0.579280f); - vertices[18] = new Vector3(0.287995f, 0.910536f, -0.335426f); - vertices[19] = new Vector3(0.484149f, 0.789305f, -0.410352f); - vertices[20] = new Vector3(-0.131747f, 0.817392f, -0.579280f); - vertices[21] = new Vector3(-0.327903f, 0.696161f, -0.654204f); - vertices[22] = new Vector3(-0.478612f, 0.789305f, -0.410352f); - vertices[23] = new Vector3(-0.282457f, 0.910536f, -0.335426f); - vertices[24] = new Vector3(-0.789408f, 0.410936f, -0.477925f); - vertices[25] = new Vector3(-0.910638f, 0.336012f, -0.281770f); - vertices[26] = new Vector3(-0.817496f, 0.579863f, -0.131060f); - vertices[27] = new Vector3(-0.696265f, 0.654787f, -0.327216f); - vertices[28] = new Vector3(-0.910638f, 0.336012f, 0.288682f); - vertices[29] = new Vector3(-0.789408f, 0.410936f, 0.484838f); - vertices[30] = new Vector3(-0.696265f, 0.654787f, 0.334128f); - vertices[31] = new Vector3(-0.817496f, 0.579863f, 0.137973f); - vertices[32] = new Vector3(-0.327903f, 0.696161f, 0.661116f); - vertices[33] = new Vector3(-0.131747f, 0.817392f, 0.586192f); - vertices[34] = new Vector3(-0.282457f, 0.910536f, 0.342339f); - vertices[35] = new Vector3(-0.478612f, 0.789305f, 0.417265f); - vertices[36] = new Vector3(0.416578f, 0.478508f, 0.795634f); - vertices[37] = new Vector3(0.341652f, 0.282353f, 0.916863f); - vertices[38] = new Vector3(0.585505f, 0.131646f, 0.823721f); - vertices[39] = new Vector3(0.660429f, 0.327801f, 0.702490f); - vertices[40] = new Vector3(0.124000f, 0.147837f, 1.000000f); - vertices[41] = new Vector3(-0.118461f, 0.147837f, 1.000000f); - vertices[42] = new Vector3(-0.118461f, -0.153580f, 1.000000f); - vertices[43] = new Vector3(0.124000f, -0.153580f, 1.000000f); - vertices[44] = new Vector3(-0.336113f, 0.282353f, 0.916863f); - vertices[45] = new Vector3(-0.411039f, 0.478508f, 0.795634f); - vertices[46] = new Vector3(-0.654891f, 0.327801f, 0.702490f); - vertices[47] = new Vector3(-0.579966f, 0.131646f, 0.823721f); - vertices[48] = new Vector3(-0.993774f, 0.118359f, -0.147252f); - vertices[49] = new Vector3(-0.993774f, -0.124103f, -0.147252f); - vertices[50] = new Vector3(-0.993774f, -0.124103f, 0.154165f); - vertices[51] = new Vector3(-0.993774f, 0.118359f, 0.154165f); - vertices[52] = new Vector3(-0.817496f, -0.585607f, 0.137973f); - vertices[53] = new Vector3(-0.696265f, -0.660531f, 0.334128f); - vertices[54] = new Vector3(-0.789408f, -0.416680f, 0.484838f); - vertices[55] = new Vector3(-0.910638f, -0.341756f, 0.288682f); - vertices[56] = new Vector3(-0.411039f, -0.484253f, 0.795634f); - vertices[57] = new Vector3(-0.336113f, -0.288097f, 0.916863f); - vertices[58] = new Vector3(-0.579966f, -0.137388f, 0.823721f); - vertices[59] = new Vector3(-0.654891f, -0.333543f, 0.702490f); - vertices[60] = new Vector3(0.341652f, -0.288097f, 0.916863f); - vertices[61] = new Vector3(0.416578f, -0.484253f, 0.795634f); - vertices[62] = new Vector3(0.660429f, -0.333543f, 0.702490f); - vertices[63] = new Vector3(0.585505f, -0.137388f, 0.823721f); - vertices[64] = new Vector3(0.333441f, -0.701905f, 0.661116f); - vertices[65] = new Vector3(0.137286f, -0.823136f, 0.586192f); - vertices[66] = new Vector3(0.287995f, -0.916278f, 0.342339f); - vertices[67] = new Vector3(0.484149f, -0.795049f, 0.417265f); - vertices[68] = new Vector3(-0.131747f, -0.823136f, 0.586192f); - vertices[69] = new Vector3(-0.327903f, -0.701905f, 0.661116f); - vertices[70] = new Vector3(-0.478612f, -0.795049f, 0.417265f); - vertices[71] = new Vector3(-0.282457f, -0.916278f, 0.342339f); - vertices[72] = new Vector3(-0.910638f, -0.341756f, -0.281770f); - vertices[73] = new Vector3(-0.789408f, -0.416680f, -0.477925f); - vertices[74] = new Vector3(-0.696265f, -0.660531f, -0.327216f); - vertices[75] = new Vector3(-0.817496f, -0.585607f, -0.131060f); - vertices[76] = new Vector3(-0.327903f, -0.701905f, -0.654204f); - vertices[77] = new Vector3(-0.131747f, -0.823136f, -0.579280f); - vertices[78] = new Vector3(-0.282457f, -0.916278f, -0.335426f); - vertices[79] = new Vector3(-0.478612f, -0.795049f, -0.410352f); - vertices[80] = new Vector3(0.153478f, -0.999415f, -0.117774f); - vertices[81] = new Vector3(0.153478f, -0.999415f, 0.124687f); - vertices[82] = new Vector3(-0.147939f, -0.999415f, 0.124687f); - vertices[83] = new Vector3(-0.147939f, -0.999415f, -0.117774f); - vertices[84] = new Vector3(0.701803f, -0.660531f, 0.334128f); - vertices[85] = new Vector3(0.823033f, -0.585607f, 0.137973f); - vertices[86] = new Vector3(0.916176f, -0.341756f, 0.288682f); - vertices[87] = new Vector3(0.794945f, -0.416680f, 0.484838f); - vertices[88] = new Vector3(0.823033f, -0.585607f, -0.131060f); - vertices[89] = new Vector3(0.701803f, -0.660531f, -0.327216f); - vertices[90] = new Vector3(0.794945f, -0.416680f, -0.477925f); - vertices[91] = new Vector3(0.916176f, -0.341756f, -0.281770f); - vertices[92] = new Vector3(0.484149f, -0.795049f, -0.410352f); - vertices[93] = new Vector3(0.287995f, -0.916278f, -0.335426f); - vertices[94] = new Vector3(0.137286f, -0.823136f, -0.579280f); - vertices[95] = new Vector3(0.333441f, -0.701905f, -0.654204f); - vertices[96] = new Vector3(-0.654891f, -0.333543f, -0.695578f); - vertices[97] = new Vector3(-0.579966f, -0.137388f, -0.816807f); - vertices[98] = new Vector3(-0.336113f, -0.288097f, -0.909951f); - vertices[99] = new Vector3(-0.411039f, -0.484253f, -0.788719f); - vertices[100] = new Vector3(-0.118461f, 0.147837f, -0.993087f); - vertices[101] = new Vector3(0.124000f, 0.147837f, -0.993087f); - vertices[102] = new Vector3(0.124000f, -0.153580f, -0.993087f); - vertices[103] = new Vector3(-0.118461f, -0.153580f, -0.993087f); - vertices[104] = new Vector3(0.585505f, -0.137388f, -0.816807f); - vertices[105] = new Vector3(0.660429f, -0.333543f, -0.695578f); - vertices[106] = new Vector3(0.416578f, -0.484253f, -0.788719f); - vertices[107] = new Vector3(0.341652f, -0.288097f, -0.909951f); - vertices[108] = new Vector3(0.999313f, -0.124103f, -0.147252f); - vertices[109] = new Vector3(0.999313f, 0.118359f, -0.147252f); - vertices[110] = new Vector3(0.999313f, 0.118359f, 0.154165f); - vertices[111] = new Vector3(0.999313f, -0.124103f, 0.154165f); - vertices[112] = new Vector3(0.660429f, 0.327801f, -0.695578f); - vertices[113] = new Vector3(0.585505f, 0.131646f, -0.816807f); - vertices[114] = new Vector3(0.341652f, 0.282353f, -0.909951f); - vertices[115] = new Vector3(0.416578f, 0.478508f, -0.788719f); - vertices[116] = new Vector3(-0.579966f, 0.131646f, -0.816807f); - vertices[117] = new Vector3(-0.654891f, 0.327801f, -0.695578f); - vertices[118] = new Vector3(-0.411039f, 0.478508f, -0.788719f); - vertices[119] = new Vector3(-0.336113f, 0.282353f, -0.909951f); + var vertices = new Vector3[] + { + new(0.153478f, 0.993671f, 0.124687f), + new(0.153478f, 0.993671f, -0.117774f), + new(-0.147939f, 0.993671f, -0.117774f), + new(-0.147939f, 0.993671f, 0.124687f), + new(0.137286f, 0.817392f, 0.586192f), + new(0.333441f, 0.696161f, 0.661116f), + new(0.484149f, 0.789305f, 0.417265f), + new(0.287995f, 0.910536f, 0.342339f), + new(0.794945f, 0.410936f, 0.484838f), + new(0.916176f, 0.336012f, 0.288682f), + new(0.823033f, 0.579863f, 0.137973f), + new(0.701803f, 0.654787f, 0.334128f), + new(0.916176f, 0.336012f, -0.281770f), + new(0.794945f, 0.410936f, -0.477925f), + new(0.701803f, 0.654787f, -0.327216f), + new(0.823033f, 0.579863f, -0.131060f), + new(0.333441f, 0.696161f, -0.654204f), + new(0.137286f, 0.817392f, -0.579280f), + new(0.287995f, 0.910536f, -0.335426f), + new(0.484149f, 0.789305f, -0.410352f), + new(-0.131747f, 0.817392f, -0.579280f), + new(-0.327903f, 0.696161f, -0.654204f), + new(-0.478612f, 0.789305f, -0.410352f), + new(-0.282457f, 0.910536f, -0.335426f), + new(-0.789408f, 0.410936f, -0.477925f), + new(-0.910638f, 0.336012f, -0.281770f), + new(-0.817496f, 0.579863f, -0.131060f), + new(-0.696265f, 0.654787f, -0.327216f), + new(-0.910638f, 0.336012f, 0.288682f), + new(-0.789408f, 0.410936f, 0.484838f), + new(-0.696265f, 0.654787f, 0.334128f), + new(-0.817496f, 0.579863f, 0.137973f), + new(-0.327903f, 0.696161f, 0.661116f), + new(-0.131747f, 0.817392f, 0.586192f), + new(-0.282457f, 0.910536f, 0.342339f), + new(-0.478612f, 0.789305f, 0.417265f), + new(0.416578f, 0.478508f, 0.795634f), + new(0.341652f, 0.282353f, 0.916863f), + new(0.585505f, 0.131646f, 0.823721f), + new(0.660429f, 0.327801f, 0.702490f), + new(0.124000f, 0.147837f, 1.000000f), + new(-0.118461f, 0.147837f, 1.000000f), + new(-0.118461f, -0.153580f, 1.000000f), + new(0.124000f, -0.153580f, 1.000000f), + new(-0.336113f, 0.282353f, 0.916863f), + new(-0.411039f, 0.478508f, 0.795634f), + new(-0.654891f, 0.327801f, 0.702490f), + new(-0.579966f, 0.131646f, 0.823721f), + new(-0.993774f, 0.118359f, -0.147252f), + new(-0.993774f, -0.124103f, -0.147252f), + new(-0.993774f, -0.124103f, 0.154165f), + new(-0.993774f, 0.118359f, 0.154165f), + new(-0.817496f, -0.585607f, 0.137973f), + new(-0.696265f, -0.660531f, 0.334128f), + new(-0.789408f, -0.416680f, 0.484838f), + new(-0.910638f, -0.341756f, 0.288682f), + new(-0.411039f, -0.484253f, 0.795634f), + new(-0.336113f, -0.288097f, 0.916863f), + new(-0.579966f, -0.137388f, 0.823721f), + new(-0.654891f, -0.333543f, 0.702490f), + new(0.341652f, -0.288097f, 0.916863f), + new(0.416578f, -0.484253f, 0.795634f), + new(0.660429f, -0.333543f, 0.702490f), + new(0.585505f, -0.137388f, 0.823721f), + new(0.333441f, -0.701905f, 0.661116f), + new(0.137286f, -0.823136f, 0.586192f), + new(0.287995f, -0.916278f, 0.342339f), + new(0.484149f, -0.795049f, 0.417265f), + new(-0.131747f, -0.823136f, 0.586192f), + new(-0.327903f, -0.701905f, 0.661116f), + new(-0.478612f, -0.795049f, 0.417265f), + new(-0.282457f, -0.916278f, 0.342339f), + new(-0.910638f, -0.341756f, -0.281770f), + new(-0.789408f, -0.416680f, -0.477925f), + new(-0.696265f, -0.660531f, -0.327216f), + new(-0.817496f, -0.585607f, -0.131060f), + new(-0.327903f, -0.701905f, -0.654204f), + new(-0.131747f, -0.823136f, -0.579280f), + new(-0.282457f, -0.916278f, -0.335426f), + new(-0.478612f, -0.795049f, -0.410352f), + new(0.153478f, -0.999415f, -0.117774f), + new(0.153478f, -0.999415f, 0.124687f), + new(-0.147939f, -0.999415f, 0.124687f), + new(-0.147939f, -0.999415f, -0.117774f), + new(0.701803f, -0.660531f, 0.334128f), + new(0.823033f, -0.585607f, 0.137973f), + new(0.916176f, -0.341756f, 0.288682f), + new(0.794945f, -0.416680f, 0.484838f), + new(0.823033f, -0.585607f, -0.131060f), + new(0.701803f, -0.660531f, -0.327216f), + new(0.794945f, -0.416680f, -0.477925f), + new(0.916176f, -0.341756f, -0.281770f), + new(0.484149f, -0.795049f, -0.410352f), + new(0.287995f, -0.916278f, -0.335426f), + new(0.137286f, -0.823136f, -0.579280f), + new(0.333441f, -0.701905f, -0.654204f), + new(-0.654891f, -0.333543f, -0.695578f), + new(-0.579966f, -0.137388f, -0.816807f), + new(-0.336113f, -0.288097f, -0.909951f), + new(-0.411039f, -0.484253f, -0.788719f), + new(-0.118461f, 0.147837f, -0.993087f), + new(0.124000f, 0.147837f, -0.993087f), + new(0.124000f, -0.153580f, -0.993087f), + new(-0.118461f, -0.153580f, -0.993087f), + new(0.585505f, -0.137388f, -0.816807f), + new(0.660429f, -0.333543f, -0.695578f), + new(0.416578f, -0.484253f, -0.788719f), + new(0.341652f, -0.288097f, -0.909951f), + new(0.999313f, -0.124103f, -0.147252f), + new(0.999313f, 0.118359f, -0.147252f), + new(0.999313f, 0.118359f, 0.154165f), + new(0.999313f, -0.124103f, 0.154165f), + new(0.660429f, 0.327801f, -0.695578f), + new(0.585505f, 0.131646f, -0.816807f), + new(0.341652f, 0.282353f, -0.909951f), + new(0.416578f, 0.478508f, -0.788719f), + new(-0.579966f, 0.131646f, -0.816807f), + new(-0.654891f, 0.327801f, -0.695578f), + new(-0.411039f, 0.478508f, -0.788719f), + new(-0.336113f, 0.282353f, -0.909951f), + }; return vertices; } - Buffer CreateTestConvexHull3() + Vector3[] CreateTestConvexHull3() { - BufferPool.Take(22, out var vertices); - vertices[0] = new Vector3(-0.103558f, 1.000000f, -0.490575f); - vertices[1] = new Vector3(0.266493f, 0.659794f, -0.363751f); - vertices[2] = new Vector3(-0.245774f, 0.762636f, -0.615304f); - vertices[3] = new Vector3(0.164688f, -0.777634f, -0.365919f); - vertices[4] = new Vector3(0.503268f, -0.846406f, -0.131286f); - vertices[5] = new Vector3(0.171066f, -0.931723f, -0.140738f); - vertices[6] = new Vector3(-0.247963f, -0.738059f, -0.413146f); - vertices[7] = new Vector3(-0.319203f, -0.260078f, -0.609331f); - vertices[8] = new Vector3(0.469624f, -0.747848f, -0.286486f); - vertices[9] = new Vector3(0.398526f, -0.238233f, -0.435281f); - vertices[10] = new Vector3(0.448274f, 0.295416f, -0.246327f); - vertices[11] = new Vector3(-0.245774f, 0.762636f, 0.596521f); - vertices[12] = new Vector3(0.266493f, 0.659794f, 0.344974f); - vertices[13] = new Vector3(-0.103558f, 1.000000f, 0.471792f); - vertices[14] = new Vector3(0.171066f, -0.931723f, 0.121961f); - vertices[15] = new Vector3(0.503268f, -0.846406f, 0.112509f); - vertices[16] = new Vector3(0.164688f, -0.777634f, 0.347137f); - vertices[17] = new Vector3(-0.319203f, -0.260078f, 0.590548f); - vertices[18] = new Vector3(-0.247963f, -0.738059f, 0.394364f); - vertices[19] = new Vector3(0.469624f, -0.747848f, 0.267709f); - vertices[20] = new Vector3(0.398526f, -0.238233f, 0.416498f); - vertices[21] = new Vector3(0.448274f, 0.295411f, 0.227550f); + var vertices = new Vector3[] + { + new(-0.103558f, 1.000000f, -0.490575f), + new(0.266493f, 0.659794f, -0.363751f), + new(-0.245774f, 0.762636f, -0.615304f), + new(0.164688f, -0.777634f, -0.365919f), + new(0.503268f, -0.846406f, -0.131286f), + new(0.171066f, -0.931723f, -0.140738f), + new(-0.247963f, -0.738059f, -0.413146f), + new(-0.319203f, -0.260078f, -0.609331f), + new(0.469624f, -0.747848f, -0.286486f), + new(0.398526f, -0.238233f, -0.435281f), + new(0.448274f, 0.295416f, -0.246327f), + new(-0.245774f, 0.762636f, 0.596521f), + new(0.266493f, 0.659794f, 0.344974f), + new(-0.103558f, 1.000000f, 0.471792f), + new(0.171066f, -0.931723f, 0.121961f), + new(0.503268f, -0.846406f, 0.112509f), + new(0.164688f, -0.777634f, 0.347137f), + new(-0.319203f, -0.260078f, 0.590548f), + new(-0.247963f, -0.738059f, 0.394364f), + new(0.469624f, -0.747848f, 0.267709f), + new(0.398526f, -0.238233f, 0.416498f), + new(0.448274f, 0.295411f, 0.227550f), + }; return vertices; } - Buffer CreateJSONSourcedConvexHull(string filePath) + Vector3[] CreateJSONSourcedConvexHull(string filePath) { //ChatGPT wrote this, of course. List points = new List(); @@ -469,17 +479,11 @@ Buffer CreateJSONSourcedConvexHull(string filePath) else { Console.Error.WriteLine("File not found: " + filePath); - } - - BufferPool.Take(points.Count, out Buffer buffer); - for (int i = 0; i < buffer.Length; ++i) - { - buffer[i] = points[i]; - } - return buffer; + } + return points.ToArray(); } - void CreateHellCubeFace(int widthInPoints, float step, Buffer facePoints, Matrix3x3 transform, float localFaceOffset) + void CreateHellCubeFace(int widthInPoints, float step, Span facePoints, Matrix3x3 transform, float localFaceOffset) { var offset = step * (widthInPoints - 1) * 0.5f; for (int i = 0; i < widthInPoints; ++i) @@ -494,10 +498,10 @@ void CreateHellCubeFace(int widthInPoints, float step, Buffer facePoint } } } - Buffer CreateHellCube(int widthInPoints) + Vector3[] CreateHellCube(int widthInPoints) { var facePointCount = widthInPoints * widthInPoints; - BufferPool.Take(facePointCount * 6, out Buffer buffer); + var buffer = new Vector3[facePointCount * 6]; var size = 8f; var halfSize = size / 2; var step = size / (widthInPoints - 1); @@ -507,12 +511,12 @@ Buffer CreateHellCube(int widthInPoints) Matrix3x3.CreateFromAxisAngle(new Vector3(0, 1, 0), float.Pi, out var localFace2); Matrix3x3.CreateFromAxisAngle(new Vector3(1, 0, 0), -float.Pi / 2, out var yFace); yFace *= cubeTransform; - CreateHellCubeFace(widthInPoints, step, buffer.Slice(facePointCount * 0, facePointCount), cubeTransform, halfSize); - CreateHellCubeFace(widthInPoints, step, buffer.Slice(facePointCount * 1, facePointCount), zFace, halfSize); - CreateHellCubeFace(widthInPoints, step, buffer.Slice(facePointCount * 2, facePointCount), cubeTransform, -halfSize); - CreateHellCubeFace(widthInPoints, step, buffer.Slice(facePointCount * 3, facePointCount), zFace, -halfSize); - CreateHellCubeFace(widthInPoints, step, buffer.Slice(facePointCount * 4, facePointCount), yFace, halfSize); - CreateHellCubeFace(widthInPoints, step, buffer.Slice(facePointCount * 5, facePointCount), yFace, -halfSize); + CreateHellCubeFace(widthInPoints, step, buffer.AsSpan(facePointCount * 0, facePointCount), cubeTransform, halfSize); + CreateHellCubeFace(widthInPoints, step, buffer.AsSpan(facePointCount * 1, facePointCount), zFace, halfSize); + CreateHellCubeFace(widthInPoints, step, buffer.AsSpan(facePointCount * 2, facePointCount), cubeTransform, -halfSize); + CreateHellCubeFace(widthInPoints, step, buffer.AsSpan(facePointCount * 3, facePointCount), zFace, -halfSize); + CreateHellCubeFace(widthInPoints, step, buffer.AsSpan(facePointCount * 4, facePointCount), yFace, halfSize); + CreateHellCubeFace(widthInPoints, step, buffer.AsSpan(facePointCount * 5, facePointCount), yFace, -halfSize); return buffer; } @@ -667,7 +671,6 @@ void TestConvexHullCreation() } } - //Buffer points; Vector3[] points; List debugSteps; @@ -797,7 +800,7 @@ void DrawFace(DebugStep step, int[] reduced, Vector3 color, float offsetScale) var faceVertexCount = endIndex - startIndex; var faceNormal = step.FaceNormals[faceIndex]; - var color = step.FaceIndex == faceIndex ? new Vector3(1, 0, 0.5f) : new Vector3(1, 0, 1); + var color = step.FaceIndex == faceIndex ? new Vector3(1, 0, 0.5f) : new Vector3(1, 0, 1); if (showWireframe) { @@ -809,7 +812,7 @@ void DrawFace(DebugStep step, int[] reduced, Vector3 color, float offsetScale) var b = points[step.FaceIndices[previousIndex]] * scale + renderOffset; previousIndex = vertexIndex; renderer.Lines.Allocate() = new LineInstance(a, b, color, Vector3.Zero); - } + } } else { From 3a49385ffb4f8a68dcf344f31b6eefd58d1b8d41 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 19 Jan 2025 12:02:50 -0600 Subject: [PATCH 892/947] Wee morning test refactoring. --- Demos/SpecializedTests/ConvexHullTestDemo.cs | 194 +++++++++++-------- 1 file changed, 109 insertions(+), 85 deletions(-) diff --git a/Demos/SpecializedTests/ConvexHullTestDemo.cs b/Demos/SpecializedTests/ConvexHullTestDemo.cs index 65725cd86..8e1cce1e5 100644 --- a/Demos/SpecializedTests/ConvexHullTestDemo.cs +++ b/Demos/SpecializedTests/ConvexHullTestDemo.cs @@ -1,4 +1,5 @@ -#define DEBUG_STEPS +// Enabling DEBUG_STEPS on this test requires the same define within ConvexHullHelper.cs. +#define DEBUG_STEPS using System; using System.Collections.Generic; using System.Numerics; @@ -16,6 +17,7 @@ using DemoRenderer.Constraints; using DemoUtilities; using DemoRenderer.UI; +using System.Diagnostics; @@ -520,6 +522,19 @@ Vector3[] CreateHellCube(int widthInPoints) return buffer; } + struct HullTestData + { + public Vector3[] Points; + public HullData HullData; +#if DEBUG_STEPS + public List DebugSteps; +#endif + public ConvexHull Hull; + public TypedIndex ShapeIndex; + } + + HullTestData[] hullTests; + public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(0, -2.5f, 10); @@ -527,88 +542,99 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Pitch = 0; Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -0, 0)), new SolveDescription(8, 1)); - //var hullPoints = CreateJSONSourcedConvexHull(@"Content/testHull.json"); - //for (int i = 0; i < hullPoints.Length; ++i) - //{ - // hullPoints[i] *= 0.03f; - //} - var hullPoints = CreateRandomConvexHullPoints(); - //var hullPoints = CreateMeshConvexHull(content.Load(@"Content\newt.obj"), new Vector3(1, 1.5f, 1f)); - //var hullPoints = CreateHellCube(200); - //var hullPoints = CreateBwaa(); - //var hullPoints = CreatePlaneish(); - //var hullPoints = CreateDistantPlane(); - //var hullPoints = CreateTestConvexHull(); - //var hullPoints = CreateTestConvexHull2(); - //var hullPoints = CreateTestConvexHull3(); - //var hullPoints = CreateBoxConvexHull(2); - var hullShape = new ConvexHull(hullPoints, BufferPool, out _); - //float largestError = 0; - //for (int i = 0; i < hullShape.FaceToVertexIndicesStart.Length; ++i) - //{ - // hullShape.GetVertexIndicesForFace(i, out var faceVertices); - // BundleIndexing.GetBundleIndices(i, out var normalBundleIndex, out var normalIndexInBundle); - // Vector3Wide.ReadSlot(ref hullShape.BoundingPlanes[normalBundleIndex].Normal, normalIndexInBundle, out var faceNormal); - // var offset = hullShape.BoundingPlanes[normalBundleIndex].Offset[normalIndexInBundle]; - // Console.WriteLine($"Face {i} errors:"); - // for (int j = 0; j < faceVertices.Length; ++j) - // { - // hullShape.GetPoint(faceVertices[j], out var point); - // var error = Vector3.Dot(point, faceNormal) - offset; - // Console.WriteLine($"v{j}: {error}"); - // largestError = MathF.Max(MathF.Abs(error), largestError); - // } - //} - //Console.WriteLine($"Largest error: {largestError}"); - - ConvexHullHelper.ComputeHull(hullPoints, BufferPool, out var hullData, out debugSteps); - this.points = new Vector3[hullPoints.Length]; - hullPoints.CopyTo(0, this.points, 0, this.points.Length); - //this.points = hullPoints; - - var boxHullPoints = CreateBoxConvexHull(2); - var boxHullShape = new ConvexHull(boxHullPoints, BufferPool, out _); - - //Matrix3x3.CreateScale(new Vector3(5, 0.5f, 3), out var scale); - //var transform = Matrix3x3.CreateFromAxisAngle(Vector3.Normalize(new Vector3(3, 2, 1)), 1207) * scale; - //const int transformCount = 10000; - //var transformStart = Stopwatch.GetTimestamp(); - //for (int i = 0; i < transformCount; ++i) - //{ - // CreateTransformedCopy(hullShape, transform, BufferPool, out var transformedHullShape); - // transformedHullShape.Dispose(BufferPool); - //} - //var transformEnd = Stopwatch.GetTimestamp(); - //Console.WriteLine($"Transform hull computation time (us): {(transformEnd - transformStart) * 1e6 / (transformCount * Stopwatch.Frequency)}"); - - //hullShape.RayTest(RigidPose.Identity, new Vector3(0, 1, 0), -Vector3.UnitY, out var t, out var normal); - - //const int rayIterationCount = 10000; - //var rayPose = RigidPose.Identity; - //var rayOrigin = new Vector3(0, 2, 0); - //var rayDirection = new Vector3(0, -1, 0); - - //int hitCounter = 0; - //var start = Stopwatch.GetTimestamp(); - //for (int i = 0; i < rayIterationCount; ++i) - //{ - // if (hullShape.RayTest(rayPose, rayOrigin, rayDirection, out _, out _)) - // { - // ++hitCounter; - // } - //} - //var end = Stopwatch.GetTimestamp(); - //Console.WriteLine($"Hit counter: {hitCounter}, computation time (us): {(end - start) * 1e6 / (rayIterationCount * Stopwatch.Frequency)}"); - - //const int iterationCount = 100; - //start = Stopwatch.GetTimestamp(); - //for (int i = 0; i < iterationCount; ++i) - //{ - // CreateShape(hullPoints, BufferPool, out _, out var perfTestShape); - // perfTestShape.Dispose(BufferPool); - //} - //end = Stopwatch.GetTimestamp(); - //Console.WriteLine($"Hull computation time (us): {(end - start) * 1e6 / (iterationCount * Stopwatch.Frequency)}"); + + var hullPointSets = new Vector3[][] + { + CreateRandomConvexHullPoints(), + CreateMeshConvexHull(content.Load(@"Content\newt.obj"), new Vector3(1, 1.5f, 1f)), + CreateHellCube(200), + CreateBwaa(), + CreateTestConvexHull(), + CreateTestConvexHull2(), + CreateTestConvexHull3(), + CreateBoxConvexHull(2), + //CreateJSONSourcedConvexHull(@"Content/testHull.json"), + //CreateDistantPlane(), + //CreatePlaneish(), + }; + + hullTests = new HullTestData[hullPointSets.Length]; + for (int i = 0; i < hullPointSets.Length; ++i) + { + ref var test = ref hullTests[i]; + test.Points = hullPointSets[i]; +#if DEBUG_STEPS + ComputeHull(hullPointSets[i], BufferPool, out test.HullData, out test.DebugSteps); +#else + ComputeHull(hullPointSets[i], BufferPool, out test.HullData); +#endif + CreateShape(hullPointSets[i], test.HullData, BufferPool, out _, out test.Hull); + test.ShapeIndex = Simulation.Shapes.Add(test.Hull); + + //// Check divergence between face planes and vertices. + //float largestError = 0; + //for (int j = 0; j < test.Hull.FaceToVertexIndicesStart.Length; ++j) + //{ + // test.Hull.GetVertexIndicesForFace(j, out var faceVertices); + // BundleIndexing.GetBundleIndices(j, out var normalBundleIndex, out var normalIndexInBundle); + // Vector3Wide.ReadSlot(ref test.Hull.BoundingPlanes[normalBundleIndex].Normal, normalIndexInBundle, out var faceNormal); + // var offset = test.Hull.BoundingPlanes[normalBundleIndex].Offset[normalIndexInBundle]; + // Console.WriteLine($"Face {j} errors:"); + // for (int k = 0; k < faceVertices.Length; ++k) + // { + // test.Hull.GetPoint(faceVertices[k], out var point); + // var error = Vector3.Dot(point, faceNormal) - offset; + // Console.WriteLine($"v{k}: {error}"); + // largestError = MathF.Max(MathF.Abs(error), largestError); + // } + //} + //Console.WriteLine($"Largest error: {largestError}"); + + + Matrix3x3.CreateScale(new Vector3(5, 0.5f, 3), out var scale); + var transform = Matrix3x3.CreateFromAxisAngle(Vector3.Normalize(new Vector3(3, 2, 1)), 1207) * scale; + const int transformCount = 10000; + var transformStart = Stopwatch.GetTimestamp(); + for (int i = 0; i < transformCount; ++i) + { + CreateTransformedCopy(test.Hull, transform, BufferPool, out var transformedHullShape); + transformedHullShape.Dispose(BufferPool); + } + var transformEnd = Stopwatch.GetTimestamp(); + Console.WriteLine($"Transform hull computation time (us): {(transformEnd - transformStart) * 1e6 / (transformCount * Stopwatch.Frequency)}"); + + test.Hull.RayTest(RigidPose.Identity, new Vector3(0, 1, 0), -Vector3.UnitY, out var t, out var normal); + const int rayIterationCount = 10000; + var rayPose = RigidPose.Identity; + var rayOrigin = new Vector3(0, 2, 0); + var rayDirection = new Vector3(0, -1, 0); + + int hitCounter = 0; + var start = Stopwatch.GetTimestamp(); + for (int j = 0; j < rayIterationCount; ++j) + { + if (test.Hull.RayTest(rayPose, rayOrigin, rayDirection, out _, out _)) + { + ++hitCounter; + } + } + var end = Stopwatch.GetTimestamp(); + Console.WriteLine($"Hit counter: {hitCounter}, computation time (us): {(end - start) * 1e6 / (rayIterationCount * Stopwatch.Frequency)}"); + + const int iterationCount = 100; + start = Stopwatch.GetTimestamp(); + for (int j = 0; j < iterationCount; ++j) + { + CreateShape(test.Points, BufferPool, out _, out var perfTestShape); + perfTestShape.Dispose(BufferPool); + } + end = Stopwatch.GetTimestamp(); + Console.WriteLine($"Hull computation time (us): {(end - start) * 1e6 / (iterationCount * Stopwatch.Frequency)}"); + } + + var boxHullShape = new ConvexHull(CreateBoxConvexHull(2), BufferPool, out _); + + var hullShapeIndex = Simulation.Shapes.Add(hullShape); var boxHullShapeIndex = Simulation.Shapes.Add(boxHullShape); @@ -671,8 +697,6 @@ void TestConvexHullCreation() } } - Vector3[] points; - List debugSteps; int stepIndex = 0; From 32424f43ec44df42e58afec82a90195f5e059562 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 20 Jan 2025 17:47:56 -0600 Subject: [PATCH 893/947] Multitest refactoring. --- Demos/SpecializedTests/ConvexHullTestDemo.cs | 150 +++++++++++-------- 1 file changed, 91 insertions(+), 59 deletions(-) diff --git a/Demos/SpecializedTests/ConvexHullTestDemo.cs b/Demos/SpecializedTests/ConvexHullTestDemo.cs index 8e1cce1e5..5b5bf2e69 100644 --- a/Demos/SpecializedTests/ConvexHullTestDemo.cs +++ b/Demos/SpecializedTests/ConvexHullTestDemo.cs @@ -541,7 +541,7 @@ public override void Initialize(ContentArchive content, Camera camera) camera.Yaw = 0; camera.Pitch = 0; - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -0, 0)), new SolveDescription(8, 1)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); var hullPointSets = new Vector3[][] { @@ -591,71 +591,95 @@ public override void Initialize(ContentArchive content, Camera camera) //Console.WriteLine($"Largest error: {largestError}"); - Matrix3x3.CreateScale(new Vector3(5, 0.5f, 3), out var scale); - var transform = Matrix3x3.CreateFromAxisAngle(Vector3.Normalize(new Vector3(3, 2, 1)), 1207) * scale; - const int transformCount = 10000; - var transformStart = Stopwatch.GetTimestamp(); - for (int i = 0; i < transformCount; ++i) - { - CreateTransformedCopy(test.Hull, transform, BufferPool, out var transformedHullShape); - transformedHullShape.Dispose(BufferPool); - } - var transformEnd = Stopwatch.GetTimestamp(); - Console.WriteLine($"Transform hull computation time (us): {(transformEnd - transformStart) * 1e6 / (transformCount * Stopwatch.Frequency)}"); - - test.Hull.RayTest(RigidPose.Identity, new Vector3(0, 1, 0), -Vector3.UnitY, out var t, out var normal); - const int rayIterationCount = 10000; - var rayPose = RigidPose.Identity; - var rayOrigin = new Vector3(0, 2, 0); - var rayDirection = new Vector3(0, -1, 0); - - int hitCounter = 0; - var start = Stopwatch.GetTimestamp(); - for (int j = 0; j < rayIterationCount; ++j) - { - if (test.Hull.RayTest(rayPose, rayOrigin, rayDirection, out _, out _)) - { - ++hitCounter; - } - } - var end = Stopwatch.GetTimestamp(); - Console.WriteLine($"Hit counter: {hitCounter}, computation time (us): {(end - start) * 1e6 / (rayIterationCount * Stopwatch.Frequency)}"); + //Matrix3x3.CreateScale(new Vector3(5, 0.5f, 3), out var scale); + //var transform = Matrix3x3.CreateFromAxisAngle(Vector3.Normalize(new Vector3(3, 2, 1)), 1207) * scale; + //const int transformCount = 10000; + //var transformStart = Stopwatch.GetTimestamp(); + //for (int j = 0; j < transformCount; ++j) + //{ + // CreateTransformedCopy(test.Hull, transform, BufferPool, out var transformedHullShape); + // transformedHullShape.Dispose(BufferPool); + //} + //var transformEnd = Stopwatch.GetTimestamp(); + //Console.WriteLine($"Transform hull computation time (us): {(transformEnd - transformStart) * 1e6 / (transformCount * Stopwatch.Frequency)}"); + + //test.Hull.RayTest(RigidPose.Identity, new Vector3(0, 1, 0), -Vector3.UnitY, out var t, out var normal); + //const int rayIterationCount = 10000; + //var rayPose = RigidPose.Identity; + //var rayOrigin = new Vector3(0, 2, 0); + //var rayDirection = new Vector3(0, -1, 0); + + //int hitCounter = 0; + //var start = Stopwatch.GetTimestamp(); + //for (int j = 0; j < rayIterationCount; ++j) + //{ + // if (test.Hull.RayTest(rayPose, rayOrigin, rayDirection, out _, out _)) + // { + // ++hitCounter; + // } + //} + //var end = Stopwatch.GetTimestamp(); + //Console.WriteLine($"Hit counter: {hitCounter}, computation time (us): {(end - start) * 1e6 / (rayIterationCount * Stopwatch.Frequency)}"); - const int iterationCount = 100; - start = Stopwatch.GetTimestamp(); - for (int j = 0; j < iterationCount; ++j) - { - CreateShape(test.Points, BufferPool, out _, out var perfTestShape); - perfTestShape.Dispose(BufferPool); - } - end = Stopwatch.GetTimestamp(); - Console.WriteLine($"Hull computation time (us): {(end - start) * 1e6 / (iterationCount * Stopwatch.Frequency)}"); + //const int iterationCount = 100; + //start = Stopwatch.GetTimestamp(); + //for (int j = 0; j < iterationCount; ++j) + //{ + // CreateShape(test.Points, BufferPool, out _, out var perfTestShape); + // perfTestShape.Dispose(BufferPool); + //} + //end = Stopwatch.GetTimestamp(); + //Console.WriteLine($"Hull computation time (us): {(end - start) * 1e6 / (iterationCount * Stopwatch.Frequency)}"); } var boxHullShape = new ConvexHull(CreateBoxConvexHull(2), BufferPool, out _); - var hullShapeIndex = Simulation.Shapes.Add(hullShape); - var boxHullShapeIndex = Simulation.Shapes.Add(boxHullShape); - var inertia = hullShape.ComputeInertia(1); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 0, 5), inertia, new(hullShapeIndex, 20, 20), -0.01f)); - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 0, 3), boxHullShape.ComputeInertia(1), new(boxHullShapeIndex, 20, 20), -0.01f)); - Simulation.Statics.Add(new StaticDescription(new Vector3(-25, -5, 0), Simulation.Shapes.Add(new Sphere(2)))); - Simulation.Statics.Add(new StaticDescription(new Vector3(-20, -5, 0), Simulation.Shapes.Add(new Capsule(0.5f, 2)))); - Simulation.Statics.Add(new StaticDescription(new Vector3(-15, -5, 0), Simulation.Shapes.Add(new Box(2f, 2f, 2f)))); - Simulation.Statics.Add(new StaticDescription(new Vector3(-10, -5, 5), Simulation.Shapes.Add(new Triangle { A = new Vector3(0, 0, -10), B = new Vector3(5, 0, -10), C = new Vector3(0, 0, -5) }))); - Simulation.Statics.Add(new StaticDescription(new Vector3(-5, -5, 0), Simulation.Shapes.Add(new Cylinder(1, 1)))); - Simulation.Statics.Add(new StaticDescription(new Vector3(-5, -5, 5), Simulation.Shapes.Add(new Cylinder(1, 1)))); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5, 0), hullShapeIndex)); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5, 5), Simulation.Shapes.Add(boxHullShape))); + TypedIndex[] otherShapes = + [ + Simulation.Shapes.Add(new Sphere(2)), + Simulation.Shapes.Add(new Capsule(0.5f, 2)), + Simulation.Shapes.Add(new Box(2f, 2f, 2f)), + Simulation.Shapes.Add(new Triangle { A = new Vector3(0, 0, -10), B = new Vector3(5, 0, -10), C = new Vector3(0, 0, -5) }), + Simulation.Shapes.Add(new Cylinder(1, 1)), + Simulation.Shapes.Add(boxHullShape), + ]; + float spacing = 2.5f; + float z = 0; + for (int otherShapeIndex = 0; otherShapeIndex < otherShapes.Length; ++otherShapeIndex) + { + Simulation.Shapes.UpdateBounds(RigidPose.Identity, otherShapes[otherShapeIndex], out var bounds); + var otherShapeSpan = bounds.Max - bounds.Min; + var staticOffset = (bounds.Max + bounds.Min) * -0.5f + new Vector3(0, -5f, 0); + var staticTop = bounds.Max.Y + staticOffset.Y; + float x = 0; + float effectiveZSpan = otherShapeSpan.Z; + for (int hullIndex = 0; hullIndex < hullTests.Length; ++hullIndex) + { + ref var test = ref hullTests[hullIndex]; + test.Hull.ComputeBounds(Quaternion.Identity, out var min, out var max); + var span = max - min; + effectiveZSpan = MathF.Max(span.Z, effectiveZSpan); + + var spanX = MathF.Max(otherShapeSpan.X, span.X); + var shapeX = x + spanX * 0.5f; + var shapeY = staticTop + span.Y * 0.5f; + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(shapeX, shapeY, z), test.Hull.ComputeInertia(1), new(test.ShapeIndex, 20, 20), -0.01f)); + Simulation.Statics.Add(new StaticDescription(new Vector3(shapeX, 0, z) + staticOffset, otherShapes[otherShapeIndex])); + x += spanX + spacing; + } + z += effectiveZSpan + spacing; + } - var spacing = new Vector3(3f, 3f, 3); + var pileSpacing = new Vector3(3f, 3f, 3); int width = 16; int height = 16; int length = 0; var origin = -0.5f * spacing * new Vector3(width, 0, length) + new Vector3(40, 0.2f, -40); + var pileInertia = hullTests[0].Hull.ComputeInertia(1); + var pileShape = hullTests[0].ShapeIndex; for (int i = 0; i < width; ++i) { for (int j = 0; j < height; ++j) @@ -663,8 +687,8 @@ public override void Initialize(ContentArchive content, Camera camera) for (int k = 0; k < length; ++k) { Simulation.Bodies.Add(BodyDescription.CreateDynamic( - (origin + spacing * new Vector3(i, j, k), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathHelper.Pi * 0.05f)), - inertia, hullShapeIndex, 0.01f)); + (origin + pileSpacing * new Vector3(i, j, k), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathHelper.Pi * 0.05f)), + pileInertia, pileShape, 0.01f)); } } } @@ -675,7 +699,9 @@ public override void Initialize(ContentArchive content, Camera camera) x + 8, 2f * MathF.Sin(x * 0.125f) * MathF.Sin(y * 0.125f) + 0.1f * random.NextSingle() - 3, y - 8), new Vector3(1, 1, 1), BufferPool); - Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(mesh))); + Simulation.Statics.Add(new StaticDescription(new Vector3(64, 0, 0), Simulation.Shapes.Add(mesh))); + + stepIndices = new int[hullTests.Length]; } void TestConvexHullCreation() @@ -697,18 +723,19 @@ void TestConvexHullCreation() } } - - int stepIndex = 0; + int testIndex; + int[] stepIndices; public override void Update(Window window, Camera camera, Input input, float dt) { + ref var stepIndex = ref stepIndices[testIndex]; if (input.TypedCharacters.Contains('x')) { stepIndex = Math.Max(stepIndex - 1, 0); } if (input.TypedCharacters.Contains('c')) { - stepIndex = Math.Min(stepIndex + 1, debugSteps.Count - 1); + stepIndex = Math.Min(stepIndex + 1, hullTests[testIndex].DebugSteps.Count - 1); } if (input.WasPushed(OpenTK.Input.Key.P)) { @@ -735,6 +762,11 @@ public override void Update(Window window, Camera camera, Input input, float dt) bool showFaceVertexStatuses = true; public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) { + var hullTest = hullTests[testIndex]; + var points = hullTest.Points; + var debugSteps = hullTest.DebugSteps; + var stepIndex = stepIndices[testIndex]; + var step = debugSteps[stepIndex]; var scale = 15f; var renderOffset = new Vector3(-15, 25, 0); From 479d3ffc0dd9df12e7d7f10548e8f9870e87e794 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 20 Jan 2025 18:03:02 -0600 Subject: [PATCH 894/947] Multitest selection. --- Demos/SpecializedTests/ConvexHullTestDemo.cs | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/Demos/SpecializedTests/ConvexHullTestDemo.cs b/Demos/SpecializedTests/ConvexHullTestDemo.cs index 5b5bf2e69..e113d7c48 100644 --- a/Demos/SpecializedTests/ConvexHullTestDemo.cs +++ b/Demos/SpecializedTests/ConvexHullTestDemo.cs @@ -701,7 +701,9 @@ public override void Initialize(ContentArchive content, Camera camera) y - 8), new Vector3(1, 1, 1), BufferPool); Simulation.Statics.Add(new StaticDescription(new Vector3(64, 0, 0), Simulation.Shapes.Add(mesh))); +#if DEBUG_STEPS stepIndices = new int[hullTests.Length]; +#endif } void TestConvexHullCreation() @@ -723,6 +725,7 @@ void TestConvexHullCreation() } } +#if DEBUG_STEPS int testIndex; int[] stepIndices; @@ -737,6 +740,14 @@ public override void Update(Window window, Camera camera, Input input, float dt) { stepIndex = Math.Min(stepIndex + 1, hullTests[testIndex].DebugSteps.Count - 1); } + if (input.TypedCharacters.Contains('n')) + { + testIndex = Math.Max(testIndex - 1, 0); + } + if (input.TypedCharacters.Contains('m')) + { + testIndex = Math.Min(testIndex + 1, hullTests.Length - 1); + } if (input.WasPushed(OpenTK.Input.Key.P)) { showWireframe = !showWireframe; @@ -901,7 +912,11 @@ void DrawFace(DebugStep step, int[] reduced, Vector3 color, float offsetScale) renderer.Lines.Allocate() = new LineInstance(edgeMidpoint, edgeMidpoint + step.BasisX * scale * 0.5f, new Vector3(1, 1, 0), new Vector3()); renderer.Lines.Allocate() = new LineInstance(edgeMidpoint, edgeMidpoint + step.BasisY * scale * 0.5f, new Vector3(0, 1, 0), new Vector3()); renderer.TextBatcher.Write( - text.Clear().Append($"Enumerate step with X and C. Current step: ").Append(stepIndex + 1).Append(" out of ").Append(debugSteps.Count), + text.Clear().Append("Step: ").Append(stepIndex + 1).Append(" out of ").Append(debugSteps.Count).Append(" for test ").Append(testIndex).Append("."), + new Vector2(32, renderer.Surface.Resolution.Y - 170), 20, new Vector3(1), font); + + renderer.TextBatcher.Write( + text.Clear().Append($"Enumerate step with X and C, change test with N and M."), new Vector2(32, renderer.Surface.Resolution.Y - 140), 20, new Vector3(1), font); renderer.TextBatcher.Write(text.Clear().Append("Show wireframe: P ").Append(showWireframe ? "(on)" : "(off)"), new Vector2(32, renderer.Surface.Resolution.Y - 120), 20, new Vector3(1), font); renderer.TextBatcher.Write(text.Clear().Append("Show deleted: U ").Append(showDeleted ? "(on)" : "(off)"), new Vector2(32, renderer.Surface.Resolution.Y - 100), 20, new Vector3(1), font); @@ -912,4 +927,5 @@ void DrawFace(DebugStep step, int[] reduced, Vector3 color, float offsetScale) base.Render(renderer, camera, input, text, font); } +#endif } From 22ccb1e0306b346b654a7776b40c11a308101fe2 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 8 Mar 2025 18:43:16 -0800 Subject: [PATCH 895/947] Slab expansion by tangent scootage. --- BepuPhysics/Collidables/ConvexHullHelper.cs | 53 ++++++++++++-------- Demos/SpecializedTests/ConvexHullTestDemo.cs | 2 +- 2 files changed, 33 insertions(+), 22 deletions(-) diff --git a/BepuPhysics/Collidables/ConvexHullHelper.cs b/BepuPhysics/Collidables/ConvexHullHelper.cs index 9fca877ce..c95dd3c31 100644 --- a/BepuPhysics/Collidables/ConvexHullHelper.cs +++ b/BepuPhysics/Collidables/ConvexHullHelper.cs @@ -88,7 +88,7 @@ public static class ConvexHullHelper { static void FindExtremeFace( in Vector3Wide basisX, in Vector3Wide basisY, in Vector3Wide basisOrigin, in EdgeEndpoints sourceEdgeEndpoints, ref Buffer pointBundles, in Vector indexOffsets, Buffer allowVertices, int pointCount, - ref Buffer> projectedOnX, ref Buffer> projectedOnY, in Vector planeEpsilon, ref QuickList vertexIndices, out Vector3 faceNormal) + ref Buffer> projectedOnX, ref Buffer> projectedOnY, Vector planeEpsilon, Vector normalChangePerTangentChange, ref QuickList vertexIndices, out Vector3 faceNormal) { Debug.Assert(projectedOnX.Length >= pointBundles.Length && projectedOnY.Length >= pointBundles.Length && vertexIndices.Count == 0 && vertexIndices.Span.Length >= pointBundles.Length * Vector.Count); //Find the candidate-basisOrigin which has the smallest angle with basisY when projected onto the plane spanned by basisX and basisY. @@ -192,19 +192,8 @@ static void FindExtremeFace( for (int i = 0; i < pointBundles.Length; ++i) { var dot = projectedOnX[i] * projectedPlaneNormal.X + projectedOnY[i] * projectedPlaneNormal.Y; - //var dot2 = Vector3Wide.Dot(pointBundles[i] - basisOrigin, faceNormalWide); - //var error = dot2 - dot; - //if (Vector.GreaterThanAny(Vector.Abs(error), planeEpsilon)) - //{ - // for (int j = 0; j < Vector.Count; ++j) - // { - // if (MathF.Abs(error[j]) > planeEpsilon[j]) - // { - // Console.WriteLine($"error: {error[j]}"); - // } - // } - //} - var coplanar = Vector.GreaterThan(dot, -planeEpsilon); + var tangentDot = Vector.Abs(projectedOnX[i] * projectedPlaneNormal.Y - projectedOnY[i] * projectedPlaneNormal.X); + var coplanar = Vector.GreaterThan(dot, -planeEpsilon - tangentDot * normalChangePerTangentChange); if (Vector.LessThanAny(coplanar, Vector.Zero)) { var bundleBaseIndex = i << BundleIndexing.VectorShift; @@ -722,9 +711,15 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa Vector3Wide.Broadcast(initialVertex, out var initialVertexBundle); pool.Take>(pointBundles.Length, out var projectedOnX); pool.Take>(pointBundles.Length, out var projectedOnY); - var planeEpsilonNarrow = MathF.Sqrt(bestDistanceSquared) * 1e-5f; + // Currently using two forms of epsilon for coplanar point testing: + // 1. A 'slab' epsilon, specifying a constant width of the slab around the plane within which points are considered coplanar, and + // 2. A face coplanarity epsilon, which increases the slab width based on the distance from the measurement point. + // The face coplanarity epsilon captures points which could be member of faces that will be considered coplanar by the later face merging phase. + // If we expect they're going to show up as coplanar later, there's not much reason to create separate faces for them now. + // (This can simplify away microgeometry, but that's often actually desirable.) + var planeSlabEpsilonNarrow = MathF.Sqrt(bestDistanceSquared) * 1e-5f; var normalCoplanarityEpsilon = 1f - 1e-6f; - var planeEpsilon = new Vector(planeEpsilonNarrow); + var planeSlabEpsilon = new Vector(planeSlabEpsilonNarrow); var rawFaceVertexIndices = new QuickList(pointBundles.Length * Vector.Count, pool); var initialSourceEdge = new EdgeEndpoints { A = initialIndex, B = initialIndex }; @@ -736,14 +731,28 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa for (int i = points.Length; i < allowVertices.Length; ++i) allowVertices[i] = 0; + // Note that the coplanarity test becomes more lax with distance from the edge. + // This allows the early coplanarity test to capture faces which would otherwise later be merged + // due to having face normals that are too similar. + // Consider two normals, N and M, where M is a normal maximally far from N that is still mergeable. + // Tn is the tangent to N, and Tm is the tangent to M. + // dot(N, M) == mergeThreshold == dot(Tn, Tm) + // We want the rate of change of dot(p, N) per unit change along dot(p, Tn). + // theta = acos(dot(N, M)) + // tan(theta) = normalChangePerTangentChange + // normalChangePerTangentChange = tan(acos(dot(N, M))) + // normalChangePerTangentChange = sqrt(1 - dot(N, M)^2) / dot(N, M) + // normalChangePerTangentChange = sqrt(1 - mergeThreshold^2) / mergeThreshold + var normalChangePerTangentChange = new Vector(MathF.Sqrt(1f - normalCoplanarityEpsilon * normalCoplanarityEpsilon) / normalCoplanarityEpsilon); + FindExtremeFace(initialBasisX, initialBasisY, initialVertexBundle, initialSourceEdge, ref pointBundles, indexOffsetBundle, allowVertices, points.Length, - ref projectedOnX, ref projectedOnY, planeEpsilon, ref rawFaceVertexIndices, out var initialFaceNormal); + ref projectedOnX, ref projectedOnY, planeSlabEpsilon, normalChangePerTangentChange, ref rawFaceVertexIndices, out var initialFaceNormal); Debug.Assert(rawFaceVertexIndices.Count >= 2); var facePoints = new QuickList(points.Length, pool); var reducedFaceIndices = new QuickList(points.Length, pool); - ReduceFace(ref rawFaceVertexIndices, initialFaceNormal, points, planeEpsilonNarrow, ref facePoints, ref allowVertices, ref reducedFaceIndices); + ReduceFace(ref rawFaceVertexIndices, initialFaceNormal, points, planeSlabEpsilonNarrow, ref facePoints, ref allowVertices, ref reducedFaceIndices); var faces = new QuickList(points.Length, pool); var edgesToTest = new QuickList(points.Length, pool); @@ -806,10 +815,10 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa Vector3Wide.Broadcast(basisY, out var basisYBundle); Vector3Wide.Broadcast(edgeA, out var basisOrigin); rawFaceVertexIndices.Count = 0; - FindExtremeFace(basisXBundle, basisYBundle, basisOrigin, edgeToTest.Endpoints, ref pointBundles, indexOffsetBundle, allowVertices, points.Length, ref projectedOnX, ref projectedOnY, planeEpsilon, ref rawFaceVertexIndices, out var faceNormal); + FindExtremeFace(basisXBundle, basisYBundle, basisOrigin, edgeToTest.Endpoints, ref pointBundles, indexOffsetBundle, allowVertices, points.Length, ref projectedOnX, ref projectedOnY, planeSlabEpsilon, normalChangePerTangentChange, ref rawFaceVertexIndices, out var faceNormal); reducedFaceIndices.Count = 0; facePoints.Count = 0; - ReduceFace(ref rawFaceVertexIndices, faceNormal, points, planeEpsilonNarrow, ref facePoints, ref allowVertices, ref reducedFaceIndices); + ReduceFace(ref rawFaceVertexIndices, faceNormal, points, planeSlabEpsilonNarrow, ref facePoints, ref allowVertices, ref reducedFaceIndices); if (reducedFaceIndices.Count < 3) { @@ -830,9 +839,11 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa ref var face = ref faces[i]; if (Vector3.Dot(face.Normal, faceNormal) > normalCoplanarityEpsilon) { +#if DEBUG_STEPS Console.WriteLine($"Merging face {i} with new face, dot {Vector3.Dot(face.Normal, faceNormal)}:"); Console.WriteLine($"Existing face: {face.Normal}"); Console.WriteLine($"Candidate: {faceNormal}"); +#endif // The new face is coplanar with an existing face. Merge the new face into the old face. rawFaceVertexIndices.EnsureCapacity(reducedFaceIndices.Count + face.VertexIndices.Count, pool); rawFaceVertexIndices.Count = reducedFaceIndices.Count; @@ -853,7 +864,7 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa face.VertexIndices.Count = 0; facePoints.Count = 0; face.VertexIndices.EnsureCapacity(rawFaceVertexIndices.Count, pool); - ReduceFace(ref rawFaceVertexIndices, faceNormal, points, planeEpsilonNarrow, ref facePoints, ref allowVertices, ref face.VertexIndices); + ReduceFace(ref rawFaceVertexIndices, faceNormal, points, planeSlabEpsilonNarrow, ref facePoints, ref allowVertices, ref face.VertexIndices); #if DEBUG_STEPS step.UpdateForFaceMerge(rawFaceVertexIndices, face.VertexIndices, allowVertices, i); #endif diff --git a/Demos/SpecializedTests/ConvexHullTestDemo.cs b/Demos/SpecializedTests/ConvexHullTestDemo.cs index e113d7c48..419c580f9 100644 --- a/Demos/SpecializedTests/ConvexHullTestDemo.cs +++ b/Demos/SpecializedTests/ConvexHullTestDemo.cs @@ -539,7 +539,7 @@ public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(0, -2.5f, 10); camera.Yaw = 0; - camera.Pitch = 0; + camera.Pitch = 0; Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); From 6d70d4876d26be5062cb749abc64e223fae18f72 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 9 Mar 2025 12:54:02 -0600 Subject: [PATCH 896/947] Revert attempt at relaxation; selection of internal points is too aggressive, alas. --- BepuPhysics/Collidables/ConvexHullHelper.cs | 26 +++++---------------- 1 file changed, 6 insertions(+), 20 deletions(-) diff --git a/BepuPhysics/Collidables/ConvexHullHelper.cs b/BepuPhysics/Collidables/ConvexHullHelper.cs index c95dd3c31..f160005b9 100644 --- a/BepuPhysics/Collidables/ConvexHullHelper.cs +++ b/BepuPhysics/Collidables/ConvexHullHelper.cs @@ -88,7 +88,7 @@ public static class ConvexHullHelper { static void FindExtremeFace( in Vector3Wide basisX, in Vector3Wide basisY, in Vector3Wide basisOrigin, in EdgeEndpoints sourceEdgeEndpoints, ref Buffer pointBundles, in Vector indexOffsets, Buffer allowVertices, int pointCount, - ref Buffer> projectedOnX, ref Buffer> projectedOnY, Vector planeEpsilon, Vector normalChangePerTangentChange, ref QuickList vertexIndices, out Vector3 faceNormal) + ref Buffer> projectedOnX, ref Buffer> projectedOnY, Vector planeEpsilon, ref QuickList vertexIndices, out Vector3 faceNormal) { Debug.Assert(projectedOnX.Length >= pointBundles.Length && projectedOnY.Length >= pointBundles.Length && vertexIndices.Count == 0 && vertexIndices.Span.Length >= pointBundles.Length * Vector.Count); //Find the candidate-basisOrigin which has the smallest angle with basisY when projected onto the plane spanned by basisX and basisY. @@ -189,11 +189,11 @@ static void FindExtremeFace( //} Vector3Wide.Broadcast(faceNormal, out var faceNormalWide); + var negatedPlaneEpsilon = -planeEpsilon; for (int i = 0; i < pointBundles.Length; ++i) { var dot = projectedOnX[i] * projectedPlaneNormal.X + projectedOnY[i] * projectedPlaneNormal.Y; - var tangentDot = Vector.Abs(projectedOnX[i] * projectedPlaneNormal.Y - projectedOnY[i] * projectedPlaneNormal.X); - var coplanar = Vector.GreaterThan(dot, -planeEpsilon - tangentDot * normalChangePerTangentChange); + var coplanar = Vector.GreaterThan(dot, negatedPlaneEpsilon); if (Vector.LessThanAny(coplanar, Vector.Zero)) { var bundleBaseIndex = i << BundleIndexing.VectorShift; @@ -717,7 +717,7 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa // The face coplanarity epsilon captures points which could be member of faces that will be considered coplanar by the later face merging phase. // If we expect they're going to show up as coplanar later, there's not much reason to create separate faces for them now. // (This can simplify away microgeometry, but that's often actually desirable.) - var planeSlabEpsilonNarrow = MathF.Sqrt(bestDistanceSquared) * 1e-5f; + var planeSlabEpsilonNarrow = MathF.Sqrt(bestDistanceSquared) * 1e-4f; var normalCoplanarityEpsilon = 1f - 1e-6f; var planeSlabEpsilon = new Vector(planeSlabEpsilonNarrow); var rawFaceVertexIndices = new QuickList(pointBundles.Length * Vector.Count, pool); @@ -731,22 +731,8 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa for (int i = points.Length; i < allowVertices.Length; ++i) allowVertices[i] = 0; - // Note that the coplanarity test becomes more lax with distance from the edge. - // This allows the early coplanarity test to capture faces which would otherwise later be merged - // due to having face normals that are too similar. - // Consider two normals, N and M, where M is a normal maximally far from N that is still mergeable. - // Tn is the tangent to N, and Tm is the tangent to M. - // dot(N, M) == mergeThreshold == dot(Tn, Tm) - // We want the rate of change of dot(p, N) per unit change along dot(p, Tn). - // theta = acos(dot(N, M)) - // tan(theta) = normalChangePerTangentChange - // normalChangePerTangentChange = tan(acos(dot(N, M))) - // normalChangePerTangentChange = sqrt(1 - dot(N, M)^2) / dot(N, M) - // normalChangePerTangentChange = sqrt(1 - mergeThreshold^2) / mergeThreshold - var normalChangePerTangentChange = new Vector(MathF.Sqrt(1f - normalCoplanarityEpsilon * normalCoplanarityEpsilon) / normalCoplanarityEpsilon); - FindExtremeFace(initialBasisX, initialBasisY, initialVertexBundle, initialSourceEdge, ref pointBundles, indexOffsetBundle, allowVertices, points.Length, - ref projectedOnX, ref projectedOnY, planeSlabEpsilon, normalChangePerTangentChange, ref rawFaceVertexIndices, out var initialFaceNormal); + ref projectedOnX, ref projectedOnY, planeSlabEpsilon, ref rawFaceVertexIndices, out var initialFaceNormal); Debug.Assert(rawFaceVertexIndices.Count >= 2); var facePoints = new QuickList(points.Length, pool); var reducedFaceIndices = new QuickList(points.Length, pool); @@ -815,7 +801,7 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa Vector3Wide.Broadcast(basisY, out var basisYBundle); Vector3Wide.Broadcast(edgeA, out var basisOrigin); rawFaceVertexIndices.Count = 0; - FindExtremeFace(basisXBundle, basisYBundle, basisOrigin, edgeToTest.Endpoints, ref pointBundles, indexOffsetBundle, allowVertices, points.Length, ref projectedOnX, ref projectedOnY, planeSlabEpsilon, normalChangePerTangentChange, ref rawFaceVertexIndices, out var faceNormal); + FindExtremeFace(basisXBundle, basisYBundle, basisOrigin, edgeToTest.Endpoints, ref pointBundles, indexOffsetBundle, allowVertices, points.Length, ref projectedOnX, ref projectedOnY, planeSlabEpsilon, ref rawFaceVertexIndices, out var faceNormal); reducedFaceIndices.Count = 0; facePoints.Count = 0; ReduceFace(ref rawFaceVertexIndices, faceNormal, points, planeSlabEpsilonNarrow, ref facePoints, ref allowVertices, ref reducedFaceIndices); From e319d39b8cb923a2a0824b36f6b8e9487f464560 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 9 Mar 2025 16:59:33 -0400 Subject: [PATCH 897/947] Preppin for merge. --- BepuPhysics/Collidables/ConvexHullHelper.cs | 4 +- Demos/SpecializedTests/ConvexHullTestDemo.cs | 122 ++++++++++--------- 2 files changed, 65 insertions(+), 61 deletions(-) diff --git a/BepuPhysics/Collidables/ConvexHullHelper.cs b/BepuPhysics/Collidables/ConvexHullHelper.cs index f160005b9..0acc332c7 100644 --- a/BepuPhysics/Collidables/ConvexHullHelper.cs +++ b/BepuPhysics/Collidables/ConvexHullHelper.cs @@ -1,4 +1,4 @@ -#define DEBUG_STEPS +//#define DEBUG_STEPS using BepuUtilities; using BepuUtilities.Collections; using BepuUtilities.Memory; @@ -613,7 +613,9 @@ public static void ComputeHull(Span points, BufferPool pool, out HullDa public static void ComputeHull(Span points, BufferPool pool, out HullData hullData) #endif { +#if DEBUG_STEPS steps = new List(); +#endif if (points.Length <= 0) { hullData = default; diff --git a/Demos/SpecializedTests/ConvexHullTestDemo.cs b/Demos/SpecializedTests/ConvexHullTestDemo.cs index 419c580f9..1771eb615 100644 --- a/Demos/SpecializedTests/ConvexHullTestDemo.cs +++ b/Demos/SpecializedTests/ConvexHullTestDemo.cs @@ -1,5 +1,5 @@ // Enabling DEBUG_STEPS on this test requires the same define within ConvexHullHelper.cs. -#define DEBUG_STEPS +//#define DEBUG_STEPS using System; using System.Collections.Generic; using System.Numerics; @@ -571,65 +571,67 @@ public override void Initialize(ContentArchive content, Camera camera) CreateShape(hullPointSets[i], test.HullData, BufferPool, out _, out test.Hull); test.ShapeIndex = Simulation.Shapes.Add(test.Hull); - //// Check divergence between face planes and vertices. - //float largestError = 0; - //for (int j = 0; j < test.Hull.FaceToVertexIndicesStart.Length; ++j) - //{ - // test.Hull.GetVertexIndicesForFace(j, out var faceVertices); - // BundleIndexing.GetBundleIndices(j, out var normalBundleIndex, out var normalIndexInBundle); - // Vector3Wide.ReadSlot(ref test.Hull.BoundingPlanes[normalBundleIndex].Normal, normalIndexInBundle, out var faceNormal); - // var offset = test.Hull.BoundingPlanes[normalBundleIndex].Offset[normalIndexInBundle]; - // Console.WriteLine($"Face {j} errors:"); - // for (int k = 0; k < faceVertices.Length; ++k) - // { - // test.Hull.GetPoint(faceVertices[k], out var point); - // var error = Vector3.Dot(point, faceNormal) - offset; - // Console.WriteLine($"v{k}: {error}"); - // largestError = MathF.Max(MathF.Abs(error), largestError); - // } - //} - //Console.WriteLine($"Largest error: {largestError}"); - - - //Matrix3x3.CreateScale(new Vector3(5, 0.5f, 3), out var scale); - //var transform = Matrix3x3.CreateFromAxisAngle(Vector3.Normalize(new Vector3(3, 2, 1)), 1207) * scale; - //const int transformCount = 10000; - //var transformStart = Stopwatch.GetTimestamp(); - //for (int j = 0; j < transformCount; ++j) - //{ - // CreateTransformedCopy(test.Hull, transform, BufferPool, out var transformedHullShape); - // transformedHullShape.Dispose(BufferPool); - //} - //var transformEnd = Stopwatch.GetTimestamp(); - //Console.WriteLine($"Transform hull computation time (us): {(transformEnd - transformStart) * 1e6 / (transformCount * Stopwatch.Frequency)}"); - - //test.Hull.RayTest(RigidPose.Identity, new Vector3(0, 1, 0), -Vector3.UnitY, out var t, out var normal); - //const int rayIterationCount = 10000; - //var rayPose = RigidPose.Identity; - //var rayOrigin = new Vector3(0, 2, 0); - //var rayDirection = new Vector3(0, -1, 0); - - //int hitCounter = 0; - //var start = Stopwatch.GetTimestamp(); - //for (int j = 0; j < rayIterationCount; ++j) - //{ - // if (test.Hull.RayTest(rayPose, rayOrigin, rayDirection, out _, out _)) - // { - // ++hitCounter; - // } - //} - //var end = Stopwatch.GetTimestamp(); - //Console.WriteLine($"Hit counter: {hitCounter}, computation time (us): {(end - start) * 1e6 / (rayIterationCount * Stopwatch.Frequency)}"); - - //const int iterationCount = 100; - //start = Stopwatch.GetTimestamp(); - //for (int j = 0; j < iterationCount; ++j) - //{ - // CreateShape(test.Points, BufferPool, out _, out var perfTestShape); - // perfTestShape.Dispose(BufferPool); - //} - //end = Stopwatch.GetTimestamp(); - //Console.WriteLine($"Hull computation time (us): {(end - start) * 1e6 / (iterationCount * Stopwatch.Frequency)}"); + Console.WriteLine($" Hull {i}"); + Console.WriteLine($"Point set count: {test.Points.Length}"); + Console.WriteLine($"Face count: {test.Hull.FaceToVertexIndicesStart.Length}"); + // Check divergence between face planes and vertices. + float largestError = 0; + for (int j = 0; j < test.Hull.FaceToVertexIndicesStart.Length; ++j) + { + test.Hull.GetVertexIndicesForFace(j, out var faceVertices); + BundleIndexing.GetBundleIndices(j, out var normalBundleIndex, out var normalIndexInBundle); + Vector3Wide.ReadSlot(ref test.Hull.BoundingPlanes[normalBundleIndex].Normal, normalIndexInBundle, out var faceNormal); + var offset = test.Hull.BoundingPlanes[normalBundleIndex].Offset[normalIndexInBundle]; + //Console.WriteLine($"Face {j} errors:"); + for (int k = 0; k < faceVertices.Length; ++k) + { + test.Hull.GetPoint(faceVertices[k], out var point); + var error = Vector3.Dot(point, faceNormal) - offset; + //Console.WriteLine($"v{k}: {error}"); + largestError = MathF.Max(MathF.Abs(error), largestError); + } + } + Console.WriteLine($"Largest error: {largestError}"); + + Matrix3x3.CreateScale(new Vector3(5, 0.5f, 3), out var scale); + var transform = Matrix3x3.CreateFromAxisAngle(Vector3.Normalize(new Vector3(3, 2, 1)), 1207) * scale; + const int transformCount = 10000; + var transformStart = Stopwatch.GetTimestamp(); + for (int j = 0; j < transformCount; ++j) + { + CreateTransformedCopy(test.Hull, transform, BufferPool, out var transformedHullShape); + transformedHullShape.Dispose(BufferPool); + } + var transformEnd = Stopwatch.GetTimestamp(); + Console.WriteLine($"Transform hull computation time (us): {(transformEnd - transformStart) * 1e6 / (transformCount * Stopwatch.Frequency)}"); + + test.Hull.RayTest(RigidPose.Identity, new Vector3(0, 1, 0), -Vector3.UnitY, out var t, out var normal); + const int rayIterationCount = 10000; + var rayPose = RigidPose.Identity; + var rayOrigin = new Vector3(0, 2, 0); + var rayDirection = new Vector3(0, -1, 0); + + int hitCounter = 0; + var start = Stopwatch.GetTimestamp(); + for (int j = 0; j < rayIterationCount; ++j) + { + if (test.Hull.RayTest(rayPose, rayOrigin, rayDirection, out _, out _)) + { + ++hitCounter; + } + } + var end = Stopwatch.GetTimestamp(); + Console.WriteLine($"Hit counter: {hitCounter}, computation time (us): {(end - start) * 1e6 / (rayIterationCount * Stopwatch.Frequency)}"); + + const int iterationCount = 100; + start = Stopwatch.GetTimestamp(); + for (int j = 0; j < iterationCount; ++j) + { + CreateShape(test.Points, BufferPool, out _, out var perfTestShape); + perfTestShape.Dispose(BufferPool); + } + end = Stopwatch.GetTimestamp(); + Console.WriteLine($"Hull computation time (us): {(end - start) * 1e6 / (iterationCount * Stopwatch.Frequency)}"); } var boxHullShape = new ConvexHull(CreateBoxConvexHull(2), BufferPool, out _); From 23bf024e6e9babf54123379694822b59275d51ee Mon Sep 17 00:00:00 2001 From: Callum McGing Date: Wed, 5 Jun 2024 11:22:44 +0930 Subject: [PATCH 898/947] GL: Use compile/link status for errors Fixes demo crashing when the shader compiler reports warnings in the info log --- DemoRenderer.GL/Shader.cs | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/DemoRenderer.GL/Shader.cs b/DemoRenderer.GL/Shader.cs index a3226331d..362c4ad75 100644 --- a/DemoRenderer.GL/Shader.cs +++ b/DemoRenderer.GL/Shader.cs @@ -15,8 +15,12 @@ private void Compile(ShaderType type, string source, Action action) { GL.ShaderSource(handle, source); GL.CompileShader(handle); - var error = GL.GetShaderInfoLog(handle); - if (error != string.Empty) throw new Exception(error); + GL.GetShader(handle, ShaderParameter.CompileStatus, out var compileStatus); + if(compileStatus == 0) + { + var error = GL.GetShaderInfoLog(handle); + throw new Exception(error); + } GL.AttachShader(program, handle); try { @@ -38,8 +42,12 @@ public Shader(string vertex, string fragment) => Compile(ShaderType.FragmentShader, fragment, () => { GL.LinkProgram(program); - var error = GL.GetProgramInfoLog(program); - if (error != string.Empty) throw new Exception(error); + GL.GetProgram(program, GetProgramParameterName.LinkStatus, out var linkStatus); + if(linkStatus == 0) + { + var error = GL.GetProgramInfoLog(program); + throw new Exception(error); + } })); public void Use() { From 3feb3e9b44619919c935f77fdc51a77f5e846d06 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 10 Mar 2025 11:33:48 -0600 Subject: [PATCH 899/947] Update dotnet-core-publish.yml --- .github/workflows/dotnet-core-publish.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/dotnet-core-publish.yml b/.github/workflows/dotnet-core-publish.yml index b191d9943..2225e8514 100644 --- a/.github/workflows/dotnet-core-publish.yml +++ b/.github/workflows/dotnet-core-publish.yml @@ -2,7 +2,7 @@ name: .NET Core - Publish NuGet Packages env: COMMON_SETTINGS_PATH: CommonSettings.props - BASE_RUN_NUMBER: 25 + BASE_RUN_NUMBER: 23 on: [workflow_dispatch] @@ -53,4 +53,4 @@ jobs: run: | gh release create ${{ env.VERSION }} --title "v${{ env.VERSION }}" --notes "Release notes for ${{ env.VERSION }}" --draft env: - GH_TOKEN: ${{ secrets.GITHUB_TOKEN }} \ No newline at end of file + GH_TOKEN: ${{ secrets.GITHUB_TOKEN }} From 840c98002393f2bda678df98cdf40e81653a9aa9 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 10 Mar 2025 23:27:05 -0500 Subject: [PATCH 900/947] Bug fixes bump. --- .github/release.yml | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/.github/release.yml b/.github/release.yml index fd4519a54..36792bb2d 100644 --- a/.github/release.yml +++ b/.github/release.yml @@ -23,8 +23,11 @@ changelog: - title: 🎮 Demos App labels: - demos - - title: 📄 Documentation + - title: 🪲 Bug Fixes labels: + - bug + - title: 📄 Documentation + labels: - documentation - title: 💪 Other Changes labels: From 7ae53214e6af57873d0063703bf365bfc72acd3d Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 10 Mar 2025 23:30:03 -0500 Subject: [PATCH 901/947] Unbreak release.yaml oopsy. --- .github/release.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/release.yml b/.github/release.yml index 36792bb2d..0f7212fd7 100644 --- a/.github/release.yml +++ b/.github/release.yml @@ -27,7 +27,7 @@ changelog: labels: - bug - title: 📄 Documentation - labels: + labels: - documentation - title: 💪 Other Changes labels: From a16c94a54d9fd5726ca274ea1a37b1d8c10f7a80 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 5 Apr 2025 12:07:02 -0500 Subject: [PATCH 902/947] Oopsdemo. --- Demos/DemoSet.cs | 1 - 1 file changed, 1 deletion(-) diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index f29c81630..e2b78ac67 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -44,7 +44,6 @@ struct Option public DemoSet() { - AddOption(); AddOption(); AddOption(); AddOption(); From 6369d4c05349cdc3c5c15ef7c0a375eaea858185 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 5 Apr 2025 12:03:58 -0500 Subject: [PATCH 903/947] Bump imagesharp. --- DemoContentBuilder/DemoContentBuilder.csproj | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/DemoContentBuilder/DemoContentBuilder.csproj b/DemoContentBuilder/DemoContentBuilder.csproj index 88f83d0c9..bda6630ec 100644 --- a/DemoContentBuilder/DemoContentBuilder.csproj +++ b/DemoContentBuilder/DemoContentBuilder.csproj @@ -14,7 +14,7 @@ - + From f64b6161fb00268e9ed35cc42df1a04c9b15ac94 Mon Sep 17 00:00:00 2001 From: Vaclav Elias Date: Sun, 29 Dec 2024 21:48:50 +0000 Subject: [PATCH 904/947] feat: Docfx implementation added --- .github/workflows/bepu-docs-github.yml | 41 ++++++++++++++++++ .gitignore | 4 ++ Documentation/docfx.json | 57 +++++++++++++++++++++++++ Documentation/docs/toc.yml | 4 ++ Documentation/favicon.ico | Bin 0 -> 9748 bytes Documentation/index.md | 25 +++++++++++ Documentation/template/public/main.css | 5 +++ Documentation/template/public/main.js | 17 ++++++++ Documentation/toc.yml | 4 ++ 9 files changed, 157 insertions(+) create mode 100644 .github/workflows/bepu-docs-github.yml create mode 100644 Documentation/docfx.json create mode 100644 Documentation/docs/toc.yml create mode 100644 Documentation/favicon.ico create mode 100644 Documentation/index.md create mode 100644 Documentation/template/public/main.css create mode 100644 Documentation/template/public/main.js create mode 100644 Documentation/toc.yml diff --git a/.github/workflows/bepu-docs-github.yml b/.github/workflows/bepu-docs-github.yml new file mode 100644 index 000000000..cf5b6030b --- /dev/null +++ b/.github/workflows/bepu-docs-github.yml @@ -0,0 +1,41 @@ +# More GitHub Actions for Azure: https://github.com/Azure/actions + +name: Build Bepu Docs for GitHub Pages + +env: + COMMON_SETTINGS_PATH: Documentation/docfx.json + +on: + workflow_dispatch: + +jobs: + publish-docs: + runs-on: windows-latest + + steps: + - name: .NET SDK Setup + uses: actions/setup-dotnet@v4 + with: + dotnet-version: 9.x + + - name: Checkout Bepu + uses: actions/checkout@v4 + + - name: Install DocFX + # This installs the latest version of DocFX and may introduce breaking changes + # run: dotnet tool update -g docfx + # This installs a specific, tested version of DocFX. + run: dotnet tool update -g docfx --version 2.78.2 + + - name: Build Bepu API Docs + run: docfx metadata ${{ env.COMMON_SETTINGS_PATH }} + + - name: Build Bepu Docs + run: docfx build ${{ env.COMMON_SETTINGS_PATH }} + + - name: Deploy + uses: peaceiris/actions-gh-pages@v4.0.0 + with: + github_token: ${{ secrets.GITHUB_TOKEN }} + publish_dir: ${{ env.COMMON_SETTINGS_PATH }}/_site + publish_branch: gh-pages \ No newline at end of file diff --git a/.gitignore b/.gitignore index 5b073b60c..266ca1f2e 100644 --- a/.gitignore +++ b/.gitignore @@ -208,3 +208,7 @@ ModelManifest.xml # JetBrains Rider .idea/ + +# DocFX API Generated Pages +Documentation/api/* +Documentation/_site/* \ No newline at end of file diff --git a/Documentation/docfx.json b/Documentation/docfx.json new file mode 100644 index 000000000..dde3534bf --- /dev/null +++ b/Documentation/docfx.json @@ -0,0 +1,57 @@ +{ + "$schema": "https://raw.githubusercontent.com/dotnet/docfx/main/schemas/docfx.schema.json", + "metadata": [ + { + "src": [ + { + "src": "..", + "files": [ + "BepuPhysics/BepuPhysics.csproj", + "BepuPhysics/BepuUtilities.csproj" + ] + } + ], + "dest": "api" + } + ], + "build": { + "content": [ + { + "files": [ + "**/*.{md,yml}" + ], + "exclude": [ + "_site/**" + ] + } + ], + "resource": [ + { + "files": [ + "favicon.ico", + "favicon.png", + "images/**" + ] + } + ], + "output": "_site", + "template": [ + "default", + "modern", + "template" + ], + "fileMetadata": { + "_appTitle": { + "api/**/*.md": "Bepu API", + "api/**/*.yml": "Bepu API" + } + }, + "globalMetadata": { + "_appName": "", + "_appTitle": "Bepu Docs", + "_appLogoPath": "images/bepuphysicslogo256.png", + "_enableSearch": true, + "pdf": false + } + } +} \ No newline at end of file diff --git a/Documentation/docs/toc.yml b/Documentation/docs/toc.yml new file mode 100644 index 000000000..72c4994a5 --- /dev/null +++ b/Documentation/docs/toc.yml @@ -0,0 +1,4 @@ +- name: Getting Started + href: ../GettingStarted.md +- name: Building + href: ../Building.md \ No newline at end of file diff --git a/Documentation/favicon.ico b/Documentation/favicon.ico new file mode 100644 index 0000000000000000000000000000000000000000..f7aa9da2205d377fb510f9d701b7c39a54eb4380 GIT binary patch literal 9748 zcmZ`$0k-R5h9HY-scx!nrmkfLgZA9h8^TD`yUL)spzBit3?yzGQVIfA$@{KiQI~w7tYOn#(0uT&QssZXK*uB zOp&jM$Y~G%V-)HT#rQCB6O|`8fe}avh>RBOI}?B6g?sWIOYjA2HSL+h3^Nun+Ct^; z3!(QXyb6sP;9&lG?N|v3Vf^+)ZEYupkpFF$Ic;zDAnpn$IZqmjTAdnBQ`4X!=X|mQ zKSPE1Ro~nG-980_j8%p)Cx9C?F_Oh~`4C8YCI^Gm0-cJtxy`R@0Cmt56Ua~$`x3|X z=wChiom74qD@Zmuj4PO{=fiY!31ADb>c8$hif}>Gf-G&`JIII}5|h#+y?eDQ`Pw3D z>ueqG^<-SoO$wn^j6pO}e#N1o)x0=P?BL`c*-{r~65b*sH6#19VBU^^i|QQ|Cweu~ z1h5U3CPRAs(+>Wg#R%yvGh6cz6|;fr$ED|>a8 zo=_n_@&hogH*0AaGB)4~JxkXfwMbQo4qZr69S{5!Lj&u}4yPq~t0!fkd%OOCqMzF`03q_+Y){J)rjD&~m$W^n^ z_9ZXcWhq0%8=>fL|CNcT4KG`5KnI`e+TjRZ3LTBD-Q|Rf%D3{B;`Maa&y2s*+4pQw z-13h&a5dKQKN)e*L-*Usf}7t8s@q}1=+W$k-qYVvgT z9`p^#+4#cxOX>ahp8L9Ph8F^Z#vv@?(zy2*w%ZJco21^>CBGO6)uwCIdB|^p-xp=q zXxJ6yE0Y+|QVCcc9NJlj{vXRw>KE6s8Mr2fBd!AB-$u^uyC~^1c+8kTKqww}jvO$+ zPr*4Rs$$+0gwTtC!5@tfan5@CMY!#itcuR9j=b;ht2f>4t@UM6oaw-y6nm?GEl8li z;a7*7S|7X1FOhKcaRp>Q;Z2{q%kPPD&s@q-0t3({+Nzwe3r8Ja&_1}709#1}-=(2F zBH^@*nMxO}4CefWi6drmYTc#i8cm`O%%Ax_GVQp`w7jx^H=izDQNg?-%h{_CtLhtL}V^( zGeLb>-+XY24X9PcL)@E3G8Ra7L%es$mHjU=T8aYlgy6$i8T_|duKM3-!>v4^ubsi4 zG+p@WP#nF`VvPcSs~O5K2N%4gWgpl0UL!D64;_*LDY!uRCR<&2EH14r{ZYJ=rbEY@ zFE?IMYFm#%*XR7cE0ZD=q~BMNxj!EKt;`(7aJ6yqE3$`&hTQa|bT>5j=hMD-8Jpe6 z?q!10Iygk`@Ytb2KMj3rx?voJ(K+oukpm5lInE|Gtv!=d8qqiwiZL8fxZ=BlT<)Qe zapxIp-BBhcd(eeYc>Q&Z>LxMUauDu-@oc98sB+mSznomdEaHS`JNi7Lu$*ci{O{8J zpB;U<_ejs3RQ6>yyb^7Q``H|sc0(vz(3(yZ+cw1T{*W%$l(AD<~)OPu*61s{Iy(I<4Civ`gMDjRlWK8O<0Bgv*K3^zV=*ZZy zeS^k=ExFq59`4f8LCu1Ffpetn8u8G<`pI;R&7WM}>xdP0M0mlwP3O6%&7dB}m=ECp zs&8F_xc$m~(?!b^5HNjQIc;qSS^3&Q0ciXNsq4501M{^)XxD+A?~hX%k!{tTB!yZ~ zc=BSiMtCULMVAn1gm>ehE)1OY4;AfuxFQdk8@U4UXbJ?HhaXy(&iS$pA(g!H3J)AT z_?xE~E#E4Gu-`L17p!4nBMVRx1o@{~!oK2x>}6Knt<^StW6Qnu0DBoXiy!X`bnkl? zKC+0wmmS)G4+3SkV((Ch# z9f_#Ka~Hf<*G)CfnH!B4_{bxcy8$d0+JhiD4Hb?R857-c+|;|qEERC|*24f0+Pv9K z?r9aKF2W10S_!YBJ6;M9LJsp!S1UZ4aW6>Zl%ngvA@ETxhcQ^|S==WIn1U z^WwS74VmUj7z2Hr#yy{JKnf|`j~d(*9Xdxs)?D_6pTf~25$nyEW9ZI9%3D)uQ`4ZN zdZ@G#ho`lE`X)(ac2wHu7%6;xM8K=|xK50e@AvTE=PFQ_j+0hp%>J(Sw538f59(Kw z2Wvzx5(*$fKXRGBP3%;5-|?K&@rq5mtvuy% zu=65zl$5FXK`3~SSW+O?bV%d|ieA+<%&%@~%MMnPYgz}#Yi{@TW$gzvRXsT-41Dzo z7#Aw=Dcqj0=QZb-#yA(mG`IC0<{0d}Tx8`Cq#|<<4tJf*zF)}Yq%Q(;oWQRr6DE|O z-!2}9|MDecC=>k#T{SW=C+9=#n{(z)(YIK2THj3aot?teJftR=J+zTb$>qGo)hA0g ztWlyMDAd!{C+u#YJp77#ZF9M1D{!XyK7)bX3#K5oATLc}f#E|`<}kAyQJ)$XJK8_L zl~pyxvz>RxSUGMMai+nEzrv^7yGnAV0M)6r*UcNTt1rvbXQ@aTHW2?6En z1*31KPXqo!&8Wl6&m3R!po1Y)%nRjCPo1{-4)u(jJ6vou-Fd_o7&xR?@2DzlJbn z6f7%Gjn8vqLRPMj%s!~p;s}RvExW=Ld|5rw~wz zX)xkDrLFR`;Kpxabe=qEP_i3r3f$bK4<8twA(=> z&w{?$2=R`Icjl;bm<^XSb7sEHiK4x3r8He^exPb~i!Dc$U_5pyGk5wcuYdT*v)-z& z#}4{ArTW;T^TgCa^P*^4p;b2Ido)ufxZ@2e%U9c$&^!r;mIDOY+QEE&f5&Z;s@@W%xgtx?l9Ut6*5hc~myBos6QNb0CLZi{Irb$O7U%vZdE7Ymvy|J`iWX9) z_}rL94NTXI{J(8I3CI;7l5opXXYIDy$cfTxi<&AG;nz67tKU3imKbt)g}TnR)JYTp z=nG(;fm3tmfL|Pa?;{KOy@hc`W;>@ix79P2n^f(q%f_AHs{mC&L&;^a!TjY}c>9-n zTuwLq5z5tmzsxy$fv=(Zy@Yf=AZNKTTlnsoiL;*<<{FkZ+f;g*v`WWRPBC<&wLJ8~ z+;zKaJPG5nI}CMl8NSal{6DV_a(dGzr zJaiLN3j|xneDc3#XP6Wo8caU;HpzVhYp&v5H1q06)Zynd7?3HIk zdrdahV4=|t=>$Pr;lsi($yM-9NRNn@Z4;$srBYYPrnZSkBA z<={C(?iodC}wx}ytBR|j@(RcDM>W6O1C$PXU1NXQZkm8Zz-xi zoQU6Se8`ZtwV}#3G)k=%DHXL5l)x?BZn^<~ogJCUWf-mqshjOs!&Z{j6YHbwpOaHJ zY7O-w!=};B)YtW*%Z0DzKi0o|07PzQE}*2~#&OzD4&__rDYGz^U~FM+6|&~LCr!nQ zK$B#;flTG>dV(WW#ACBM#+%2Sc~*DTl)oH>L$mN@1~40u%WqKcbMzp7?}Dy8dDI>x zbLsJd+B&S9{BD(BhM|5J+dRgJNBO^dqvw1-AKG~2BYmBlSqac&ND{i-Z+X0x^rKg1 z`b3ENj9wq5Cm3*4h25>de3puKR8o*K6UVdIjBOE}Ok$YlbR9|Ams_n>DVB+^j)lS7 zhTphJwEgd``q3~+F|at>@oN)J`%hbDV+~H5i^5G|%q>d$cfzwNVh1x;4fCH`sr5~~ zvV|HS{yI86!B z5J`4~)%p586}Q-o!j5p$9b%6J>=u#)B#ROCWMz^1{#eO#_yy~P@r-Zy5^dOHEHf)& z$SO(;XIXVP2k7%C&BbEK;DEqDzWWo<592D}C^iz@OhxJ7AemZ@th&0O)HrG16HoPF zfu~r%`3{V&I>A1N7JtOw$s;MCc@u$>&0SfTFP+~{ZQ<9m2G)0;kMm}~^W|GXu`MKu zynOM4omulA_tPlt9hJhP^{3N??K-V615M6@HFnO&b=(g8c$~Q;1&cOLkIf>H{h{~@ zMQ|suCu53%cc!DymQS>BwwU$*(!yuj2#e^(aN%fW1PhTNd-p4mA&RTEZ+51SgDSU}`=A)gyIU-s2^tiOC@AYIDRU~$R7!Q7=? zWvc?gTAp6nVEma1Dv;~-Whn6Uq|?#YyYjuMlQ9$!Gl-<7DHS}x7ZC(Y86wk3K~DCg zM_~-5!X?}#y+8~p?5N7x2`x#`ZrxyfJjkYxv$NoIqm+&MrF70pt^qsAgTEyMgO)q7 z#dz>j@zCyUDx&0Qk(uW&-6Wt44n>wZHN!$JQh|gRBF(Lx+~*`+QY;UP?cYGTnx}#V zTPv7qt~|@$ahJot#IrUQkOnIHl8Y|FsJ}dxky4e^9Q1W{mO3Vr!mNFf#tLC5&58Fn zTAe-yba5;m8QTS$&OW;BB4qw2Ln@bblLw%>Rv_)vaxxvKaB`+6;a*@yMQXX*6cZ+NCUlb4JtIJ76bA3fG#d_n=IZO~j zt)t9o)b)*kH$q7#E0ei9!jI=6zrcBTiAxx_{7#xGV@%B}m^=Ffw6)9EJ1F`ek}{3C zLV=5{6WZwSbB7^PZAMac_;PGC1Vybebw5sRdUho6E|3fpcCOJpr+V=Cd!LLS3dn3C ziX^OP$_8H+^Ky%|b11mAC~Vp1$r4oWB#t49_8Dhz!b^qxb~+~?u@9~XA~D44+Wvlj z10IYFFSG3{FQ1Mno}YDo%7MCR&x@mCUr0l^B1f+q7?=xpi%zOk-a*_PjyT*iVa7Q=>KkOA-+QrM0Fea^A6ic*V+}i2xb9CBd@yni z0cA}xKLsoZ$0=9x#%TTa`--JHiLWILuVBDu+r zS_ivnVS%j2EMx9BQ<-Lmr4}KDC`7wNGbQb|BpQ0R_QiF!PPL{10}LP4CjFC+-HC7e zO*&={IUos92=t9h?VefX!4`@jDg(7kr(GO?3QZBMBE!OBIdMpWL^7dA7aq+hAS&td zwZilX=xPhOZJj3$exffw*-x z00(V1m(nPoWqHFj7Un2DG&ss56Jl+Tv$RT)wHIsqI>|;*BU9Pz$ zaTk3`7mPVeK(E0ja)x8Cwm6}F4H>N4!=D@s50-mup4%v97CEo6I_&SW6j@ORdhKtx)4UtJ!%8?k zqam=`o`F8keDkuc{6(a{TBfB21%I!?{(k1TzeGHo`$FKc9&s)@*fqd&mBK=?t=Y$z z7Eh?AYWW_RH^>rJZK}%LVpn~+h^|9#b%IU*2;*Zn8t&OTGH;*s$o>P7tq#esFTZ-CQ|5mprJu>I*~D5QKx_yi`@A?a5dgO1GMWNH47;3 zoluwoZU}*~kv|cbQ?FhAMj>#oIN%I?nzxG7FSk_GlELTVE*EwJ}u-Bi_01{f! z#$mD6O#59ve9aM&ewb@EvqGonJTs}_q&LWD%V>01klKF%gfG-(PQ7W zCxu&O@xR%a1qk8f`h5vG)-VVj8+UF$`8ICQW1QLdzD_#rxgwnYa=tQ^J3|u3uJ`91 z8FnmA6#e3RpDM)Z3j_JmVg1JucjB_#wIg%A>xI;+(hyvXBe}xXuG(uvp<%YI>H7@n z-Th3RJtaT`d#~$@ju`esc~anD)4LGI6JILjVIjQTlkZ{-C++uasT=TiA4MX zO}^C1ZrXW1CRXmyu~eYyZp03{uf+;xG7{ah+&Jp)Djo2PjV;zfKZC zJPr$Cl)4THtIc~Qa%~gLf&wjOHtklcRST>w>4TM|xmV(xikT?UAEwPC6hmxyMtU2F zcbD)y>}U^VriuyqUi;xPNQb`mF9{A>cf9jhb$O;N|HvB@zS^zkA*04VbH0NY1%Q6g z@?gmP#y?u<3TPsb^uG9Q%3F>pYZTV9#KcI9m1LbQcDbk=_BLs<_vle$+9?3KK7llL z(;{B2Ta0pvd;FcN(&v#x(K&=UwTc>v^9k9%Cd%T?`@Qbt6;=d8B;wlhl%uo?1u~~F z7$|MEB9tKoZU-+_eP;1JuX_m&#({77$&e7!CDwVN)ry zn1|$i+2xOK2A4epUBau!^5Ek4duPHuX2>g~tkcI#7Rl;4OQ)xdoeTiTh#LIHKh`#E@lg@%DN|?TaJZI zu)`tpvq}m_sB{LS{s|*B+j(qoy;s>8JPxm;Is@S4X?b4T){Ay!JTF!eJx@@)U-2t$ z$%dCgUbvKoI=~IebB>UuS!sy1L(Ber`NLBXMnm zKdjLWCGe}ToTXpH1ccjJ8v!o5zo=Nbz5Vs7!l6P<)NS(&%VNl|(YpHkf*w+x2^Woi z^v-by`;|~_{_z6nPiUi^jrNlKc6s~VzJ{M~=ZNTA4>2$J*8AMY(rPO-U~yl7p?-hp zi^*96r$|NKRWD^c{G4iP_LzkB#?Z#pA5-`JpZ$z4xh=oTMEPZ;bKEm6h^>dn*NA3D zBAg*je+yEToxh`I`d{NWEfr4((esXVH40dRN*2w}OT)0jJec6Hk|j5cXvZuH_)f~Y zlc@+wa0MRYvAB{4SmfAK3`n1>9u`Etd*5RX71B+_l<{)Qwb3}|bbJn3)(qQtyz{s| zkm7~eJ#JTFzll*zcW}Y7SZ}_aA7%B(W0VcTW;n)ZEI2(xBi#+}7-C_jP-}H8te>~~ zm>DDTw#$%rK~bVfpCf4Y_l!O7!;8qYpQb3r9V}}laPmpK8bBNHZ$lCrzAYnu1bb(ma{YA0CWcR|qXLqYV5A9mqH58>Q!*?4u|^rc3~V)zW#(d>zN1l`WKU_UkBqo-KQpIkxZNn=$}m2+1`G=snz z!ItsVrWouBJ($7NQfX3dIO&o?XNM6V0)Sp`#VI`$CwJ|E*YU|%``0k|4Gr(}jrY`# zaQrv;)cXCdw7z7wX0CjqQXj8s|7tdorY%1GSORk{tx`^BpcbR3oduIeYI{|o{ctoJi{kxB=6sWt zqmzlOsZ;J|w#u`r_h2F7W~eR;b=yk03X5}z z_@KvJnjkJ=k)0yx&b8@wk>%~a#i8bXnl7vwS^*)dhoY9MU@X!^dld48o>aGkhdw-0&)H?P~{>U{}<{yzG&h7=}ilGa~qxo8_ZiiG1fk_*}^I`7R z*!T$h_;~)hrIRY)gvOQK3%X&rhU@P-3G{Ej7IZXmA+6U`S=e3@VqfZ5@R`a4v)485 zEo$pTH!_;L8VxjeJjQY|=CDp=yUVOx?Cg9z$b6p4QTP+Yof|}_z)VqE zorYa5ZSop&Ut%d@q#NtOBw&d|XT>)WLYX#vth1o3{>ZRLFl}QF;AF5uWDleBAOVwM zNK7LWWr`t#VYi^m@nBk+LKSE_R%wB#>DG)@ZG&7epd$``#sV= z#Q#ep^#4UfqOfn>_4*akJS7Oq|D;dUu2Q}+he&OiUH0?0`zgQ_J=gZ>Ym#ncJ_ literal 0 HcmV?d00001 diff --git a/Documentation/index.md b/Documentation/index.md new file mode 100644 index 000000000..844daf2a3 --- /dev/null +++ b/Documentation/index.md @@ -0,0 +1,25 @@ +--- +_disableToc: false +--- + +# Quick Start + +The BepuPhysics and BepuUtilities libraries target .NET 8 and should work on any supported platform. + +The physics engine heavily uses `System.Numerics.Vectors` types, so to get good performance, you'll need a compiler which can consume those types (like RyuJIT). + +## Features + +- Spheres, capsules, boxes, triangles, cylinders, and convex hulls +- Compounds of the above +- Meshes +- A [whole bunch of constraint types](BepuPhysics/Constraints/) +- [Newts](Demos/Demos/NewtDemo.cs) +- Linear and angular continuous collision detection +- Extremely low cost sleep states for resting bodies +- Efficient scene-wide ray and sweep queries +- [Character controller example](Demos/Demos/Characters/CharacterDemo.cs) +- At least somewhat extensible collision pipeline, with [example custom voxel collidable](Demos/Demos/CustomVoxelCollidableDemo.cs) +- Highly nonidiomatic APIs +- Super speediness +- And a bunch of other miscellaneous stuff! \ No newline at end of file diff --git a/Documentation/template/public/main.css b/Documentation/template/public/main.css new file mode 100644 index 000000000..eb3143e55 --- /dev/null +++ b/Documentation/template/public/main.css @@ -0,0 +1,5 @@ +/* Copyright (c) Microsoft Corporation. All Rights Reserved. Licensed under the MIT License. See License.txt in the project root for license information. */ + +.navbar-brand #logo { + height: 56px; +} \ No newline at end of file diff --git a/Documentation/template/public/main.js b/Documentation/template/public/main.js new file mode 100644 index 000000000..ed8cb8778 --- /dev/null +++ b/Documentation/template/public/main.js @@ -0,0 +1,17 @@ +const app = { + languageDropdownCreated: false, + iconLinks: [ + { + icon: 'github', + href: 'https://github.com/bepu/bepuphysics2', + title: 'GitHub' + }, + { + icon: 'discord', + href: 'https://discord.gg/ssa2XpY', + title: 'Discord' + } + ] +}; + +export default app; \ No newline at end of file diff --git a/Documentation/toc.yml b/Documentation/toc.yml new file mode 100644 index 000000000..061acc65f --- /dev/null +++ b/Documentation/toc.yml @@ -0,0 +1,4 @@ +- name: Docs + href: docs/ +- name: API + href: api/ \ No newline at end of file From 99d1442b0d8d853aa2eb52fcb57e3e28b8c4ec87 Mon Sep 17 00:00:00 2001 From: Vaclav Elias Date: Sun, 29 Dec 2024 22:20:09 +0000 Subject: [PATCH 905/947] fix: GitHub action publish dir corrected --- .github/workflows/bepu-docs-github.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/bepu-docs-github.yml b/.github/workflows/bepu-docs-github.yml index cf5b6030b..55bb4365c 100644 --- a/.github/workflows/bepu-docs-github.yml +++ b/.github/workflows/bepu-docs-github.yml @@ -37,5 +37,5 @@ jobs: uses: peaceiris/actions-gh-pages@v4.0.0 with: github_token: ${{ secrets.GITHUB_TOKEN }} - publish_dir: ${{ env.COMMON_SETTINGS_PATH }}/_site + publish_dir: Documentation/_site publish_branch: gh-pages \ No newline at end of file From fb6215a5c22b31d1dd12d1f6de7d6c9e15251979 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 11 Mar 2025 00:06:06 -0500 Subject: [PATCH 906/947] ChildIndex clarifications. --- BepuPhysics/Simulation_Queries.cs | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/BepuPhysics/Simulation_Queries.cs b/BepuPhysics/Simulation_Queries.cs index 23d3ee9f6..feef42871 100644 --- a/BepuPhysics/Simulation_Queries.cs +++ b/BepuPhysics/Simulation_Queries.cs @@ -13,7 +13,8 @@ public interface IShapeRayHitHandler /// /// Checks whether the child of a collidable should be tested against a ray. Only called by shape types that can have more than one child. /// - /// Index of the candidate in the parent collidable. + /// Index of the candidate in the parent collidable. + /// For compounds, this is the index of the child in the child array. For meshes, this is the triangle index. For convex shapes or other types that don't have multiple children, this is always zero. /// True if the child should be tested by the ray, false otherwise. bool AllowTest(int childIndex); /// @@ -23,7 +24,8 @@ public interface IShapeRayHitHandler /// Maximum distance along the ray that the traversal is allowed to go in units of ray direction length. Can be set to limit future tests. /// Distance along the ray to the impact in units of ray direction length. In other words, hitLocation = ray.Origin + ray.Direction * t. /// Surface normal at the hit location. - /// Index of the hit child. For convex shapes or other types that don't have multiple children, this is always zero. + /// Index of the hit child. + /// For compounds, this is the index of the child in the child array. For meshes, this is the triangle index. For convex shapes or other types that don't have multiple children, this is always zero. void OnRayHit(in RayData ray, ref float maximumT, float t, Vector3 normal, int childIndex); } @@ -42,7 +44,8 @@ public interface IRayHitHandler /// Checks whether the child of a collidable should be tested against a ray. Only called by shape types that can have more than one child. /// /// Parent of the candidate. - /// Index of the candidate in the parent collidable. + /// Index of the candidate child in its parent collidable. + /// For compounds, this is the index of the child in the child array. For meshes, this is the triangle index. For convex shapes or other types that don't have multiple children, this is always zero. /// True if the child should be tested by the ray, false otherwise. bool AllowTest(CollidableReference collidable, int childIndex); /// @@ -53,7 +56,8 @@ public interface IRayHitHandler /// Distance along the ray to the impact in units of ray direction length. In other words, hitLocation = ray.Origin + ray.Direction * t. /// Surface normal at the hit location. /// Collidable hit by the ray. - /// Index of the hit child. For convex shapes or other types that don't have multiple children, this is always zero. + /// Index of the hit child in its parent collidable. + /// For compounds, this is the index of the child in the child array. For meshes, this is the triangle index. For convex shapes or other types that don't have multiple children, this is always zero. void OnRayHit(in RayData ray, ref float maximumT, float t, Vector3 normal, CollidableReference collidable, int childIndex); } @@ -72,9 +76,10 @@ public interface ISweepHitHandler /// Checks whether to run a detailed sweep test against a target collidable's child. /// /// Collidable to check. - /// Index of the child in the collidable to check. + /// Index of the child in the collidable to check. + /// For compounds, this is the index of the child in the child array. For meshes, this is the triangle index. For convex shapes or other types that don't have multiple children, this is always zero. /// True if the sweep test should be attempted, false otherwise. - bool AllowTest(CollidableReference collidable, int child); + bool AllowTest(CollidableReference collidable, int childIndex); /// /// Called when a sweep test detects a hit with nonzero T value. /// From c0b803436f391dc6e09c128e0ff141b406bcaae5 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 5 Apr 2025 16:48:11 -0500 Subject: [PATCH 907/947] Roadmap/TOC/packaging updates. --- Documentation/PackagingAndVersioning.md | 2 +- Documentation/docs/toc.yml | 18 +++++++++++++- Documentation/roadmap.md | 32 +++++++++++++++---------- 3 files changed, 38 insertions(+), 14 deletions(-) diff --git a/Documentation/PackagingAndVersioning.md b/Documentation/PackagingAndVersioning.md index a731637d3..29de43743 100644 --- a/Documentation/PackagingAndVersioning.md +++ b/Documentation/PackagingAndVersioning.md @@ -4,7 +4,7 @@ This project does not use semantic versioning. When upgrading to a newer version Breaking changes should be obvious and appear as compile errors. "Sneaky" breaking changes that significantly change behavior without a compile error will be avoided if at all possible. One notable exception to this is determinism- do not expect different versions of the library to produce identical simulation results. -NuGet packages will be made available, but they will not cover all possible features. Prerelease packages are published automatically on [github](https://github.com/orgs/bepu/packages?repo_name=bepuphysics2) and [nuget](https://www.nuget.org/packages/BepuPhysics). The main branch should be kept in a relatively stable state; cloning the source is often a good choice. +NuGet packages will be made available, but they will not cover all possible features. [Releases](https://github.com/bepu/bepuphysics2/releases) are published automatically on [github](https://github.com/orgs/bepu/packages?repo_name=bepuphysics2) and [nuget](https://www.nuget.org/packages/BepuPhysics). The main branch should be kept in a relatively stable state; cloning the source is often a good choice. The library has a variety of conditional compilation symbols. Rather than publishing a combinatorial mess to NuGet, the expectation is that users of any conditional logic will clone the source. diff --git a/Documentation/docs/toc.yml b/Documentation/docs/toc.yml index 72c4994a5..b769ca7f9 100644 --- a/Documentation/docs/toc.yml +++ b/Documentation/docs/toc.yml @@ -1,4 +1,20 @@ - name: Getting Started href: ../GettingStarted.md +- name: Questions and Answers + href: ../QuestionsAndAnswers.md +- name: Substepping + href: ../Substepping.md +- name: Continuous Collision Detection + href: ../ContinuousCollisionDetection.md +- name: Stability Tips + href: ../StabilityTips.md +- name: Performance Tips + href: ../PerformanceTips.md +- name: Packaging and Versioning + href: ../PackagingAndVersioning.md - name: Building - href: ../Building.md \ No newline at end of file + href: ../Building.md +- name: Upgrading from v1 + href: ../UpgradingFromV1.md +- name: Roadmap + href: ../roadmap.md \ No newline at end of file diff --git a/Documentation/roadmap.md b/Documentation/roadmap.md index 2213a0fe8..f9ff5a10b 100644 --- a/Documentation/roadmap.md +++ b/Documentation/roadmap.md @@ -2,25 +2,33 @@ This is a high level plan for future development. All dates and features are *extremely* speculative, and any specific detail on this roadmap is almost certainly wrong. Treat it as a snapshot of vibes unless noted otherwise. For a more detailed breakdown, check the [issues](https://github.com/bepu/bepuphysics2/issues) page. -## Near term (Q1 2024) +Notably, I now have a "full time job" doing "important things" like some kind of weirdo, so I've given up on trying to guess when these things will actually be done. Think of this roughly as a priority queue. + +## Near term 2.5 should be releasing relatively soon. It already includes a bunch of miscellaneous improvements, plus notable transformative improvements to tree building and refinement. The broad phase is a lot faster. +The only significant work remaining in 2.5 is to improve thread load balancing in the broad phase for smaller simulations. + ## Medium term The timing on these features are uncertain, but they're relatively low hanging fruit and I would like to get to them eventually. -1. Mesh/compound intersection optimization, especially in pairs with higher angular velocity. -2. Narrow phase flush improvements: https://github.com/bepu/bepuphysics2/issues/205 -3. Sleeper improvements. Applies to actual sleep/wake and candidacy analysis. One exemplar: island management scales poorly in the limit (https://github.com/bepu/bepuphysics2/issues/284) -4. More bandwidth optimizations in the solver for broad simulations: https://github.com/bepu/bepuphysics2/issues/193 -5. ARM specialized paths: https://github.com/bepu/bepuphysics2/issues/184 -6. Convex hull test performance improvements. -7. Scalar-style API for lower pain contact and boolean queries. -8. Ray cast optimization, particularly with large batches of rays. -9. Convex hull tooling improvements, like in-library simplification utilities. -10. Try for partial cross platform determinism by reimplementing some platform-dependent functionality in software. -11. High precision body and static poses, plus associated broad phase changes, for worlds exceeding 32 bit floating point precision. This isn't actually too difficult, but it would come with tradeoffs. See https://github.com/bepu/bepuphysics2/issues/13. 2.4's revamp of the solver and body data layouts intentionally left the door open for higher precision poses. +1. Super secret special sauce solver changes that may or may not actually work at all. But if they *do* work, they'll be great! +2. Look into simplifying layouts with the latest generation of solver for usability and sleeper efficiency reasons. +3. More bandwidth optimizations in the solver for broad simulations: https://github.com/bepu/bepuphysics2/issues/193 +4. Catch up with 512 bit instructions and improvements to vectorization. +5. Low hanging fruit in the API; e.g. allow normal delegates or function pointers on functions which currently require reified generics. (This would be a strictly opt-in cost; they'd be implemented through the reified generics API.) +6. Mesh/compound intersection optimization, especially in pairs with higher angular velocity. +7. Narrow phase flush improvements: https://github.com/bepu/bepuphysics2/issues/205 +8. Sleeper improvements. Applies to actual sleep/wake and candidacy analysis. One exemplar: island management scales poorly in the limit (https://github.com/bepu/bepuphysics2/issues/284) +9. ARM specialized paths: https://github.com/bepu/bepuphysics2/issues/184 +10. Convex hull test performance improvements. +11. Scalar-style API for lower pain contact and boolean queries. +12. Ray cast optimization, particularly with large batches of rays. +13. Convex hull tooling improvements, like in-library simplification utilities. +14. Try for partial cross platform determinism by reimplementing some platform-dependent functionality in software. +15. High precision body and static poses, plus associated broad phase changes, for worlds exceeding 32 bit floating point precision. This isn't actually too difficult, but it would come with tradeoffs. See https://github.com/bepu/bepuphysics2/issues/13. 2.4's revamp of the solver and body data layouts intentionally left the door open for higher precision poses. ## Long term From 69d6cdfb5605189e3a62e5c33282b48617bd8c40 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 5 Apr 2025 16:50:56 -0500 Subject: [PATCH 908/947] Already did that! --- Documentation/GettingStarted.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Documentation/GettingStarted.md b/Documentation/GettingStarted.md index 654b980c6..938fdf526 100644 --- a/Documentation/GettingStarted.md +++ b/Documentation/GettingStarted.md @@ -68,7 +68,7 @@ Bodies may move around in memory during execution or when other bodies are added Statics are similar to bodies but don't have velocity, inertia, or activity states. They're just immobile collidable shapes, ideal for level geometry. -Statics are computationally cheap ([and will get even cheaper](https://github.com/bepu/bepuphysics2/issues/7)). Feel free to have thousands of them. +Statics are computationally cheap. Feel free to have thousands of them. To create a static object, pass a `StaticDescription` to `Simulation.Statics.Add`. From f1001805b0f52d66e9aac65b9112758bdddcd6c0 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 5 Apr 2025 17:17:26 -0500 Subject: [PATCH 909/947] Url fiddling. --- Documentation/docfx.json | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/Documentation/docfx.json b/Documentation/docfx.json index dde3534bf..2b3920383 100644 --- a/Documentation/docfx.json +++ b/Documentation/docfx.json @@ -51,7 +51,12 @@ "_appTitle": "Bepu Docs", "_appLogoPath": "images/bepuphysicslogo256.png", "_enableSearch": true, - "pdf": false + "pdf": false, + "_gitContribute": { + "repo": "https://github.com/bepu/bepuphysics2", + "branch": "master" + }, + "_gitUrlPattern": "github" } } } \ No newline at end of file From c0236b6dde34f6e31fccc4404ada9e41583267f0 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 5 Apr 2025 17:34:11 -0500 Subject: [PATCH 910/947] Link rewriting, maybe? --- .../template/partials/scripts.tmpl.partial | 26 +++++++++++++++++++ 1 file changed, 26 insertions(+) create mode 100644 Documentation/template/partials/scripts.tmpl.partial diff --git a/Documentation/template/partials/scripts.tmpl.partial b/Documentation/template/partials/scripts.tmpl.partial new file mode 100644 index 000000000..2204a20cd --- /dev/null +++ b/Documentation/template/partials/scripts.tmpl.partial @@ -0,0 +1,26 @@ + + \ No newline at end of file From 0353b2bee04fdb41cb2ce3f767213a77a3da9cad Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 5 Apr 2025 18:54:16 -0500 Subject: [PATCH 911/947] Squint. --- Documentation/docfx.json | 9 ++++++- .../template/partials/scripts.tmpl.partial | 26 ------------------- 2 files changed, 8 insertions(+), 27 deletions(-) delete mode 100644 Documentation/template/partials/scripts.tmpl.partial diff --git a/Documentation/docfx.json b/Documentation/docfx.json index 2b3920383..59c9c4976 100644 --- a/Documentation/docfx.json +++ b/Documentation/docfx.json @@ -56,7 +56,14 @@ "repo": "https://github.com/bepu/bepuphysics2", "branch": "master" }, - "_gitUrlPattern": "github" + "_gitUrlPattern": "github", + "_gitRepo": "https://github.com/bepu/bepuphysics2", + "_enableNewTab": true + }, + "markdownEngineProperties": { + "markdigExtensions": [ + "urlResolve" + ] } } } \ No newline at end of file diff --git a/Documentation/template/partials/scripts.tmpl.partial b/Documentation/template/partials/scripts.tmpl.partial deleted file mode 100644 index 2204a20cd..000000000 --- a/Documentation/template/partials/scripts.tmpl.partial +++ /dev/null @@ -1,26 +0,0 @@ - - \ No newline at end of file From b4bf2ce02d560638aa0fc8b9bcc6a28d6c2958bd Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 5 Apr 2025 19:38:25 -0500 Subject: [PATCH 912/947] Resquint. --- .github/workflows/bepu-docs-github.yml | 3 +++ Documentation/docfx.json | 8 +------- Documentation/fix-links.ps1 | 27 ++++++++++++++++++++++++++ 3 files changed, 31 insertions(+), 7 deletions(-) create mode 100644 Documentation/fix-links.ps1 diff --git a/.github/workflows/bepu-docs-github.yml b/.github/workflows/bepu-docs-github.yml index 55bb4365c..73db42c2a 100644 --- a/.github/workflows/bepu-docs-github.yml +++ b/.github/workflows/bepu-docs-github.yml @@ -33,6 +33,9 @@ jobs: - name: Build Bepu Docs run: docfx build ${{ env.COMMON_SETTINGS_PATH }} + - name: Fix Documentation Links + run: powershell -File Documentation/fix-links.ps1 -sitePath "Documentation/_site" -repoUrl "https://github.com/bepu/bepuphysics2/blob/master" + - name: Deploy uses: peaceiris/actions-gh-pages@v4.0.0 with: diff --git a/Documentation/docfx.json b/Documentation/docfx.json index 59c9c4976..933ff3ae9 100644 --- a/Documentation/docfx.json +++ b/Documentation/docfx.json @@ -57,13 +57,7 @@ "branch": "master" }, "_gitUrlPattern": "github", - "_gitRepo": "https://github.com/bepu/bepuphysics2", - "_enableNewTab": true - }, - "markdownEngineProperties": { - "markdigExtensions": [ - "urlResolve" - ] + "_gitRepo": "https://github.com/bepu/bepuphysics2" } } } \ No newline at end of file diff --git a/Documentation/fix-links.ps1 b/Documentation/fix-links.ps1 new file mode 100644 index 000000000..6759815f7 --- /dev/null +++ b/Documentation/fix-links.ps1 @@ -0,0 +1,27 @@ +param ( + [string]$sitePath = "./_site", + [string]$repoUrl = "https://github.com/bepu/bepuphysics2/blob/master" +) + +# Get all HTML files in the site +$htmlFiles = Get-ChildItem -Path $sitePath -Filter "*.html" -Recurse + +$linkCount = 0 +foreach ($file in $htmlFiles) { + $content = Get-Content -Path $file.FullName -Raw + + # Find links like "../Folder/File.cs" and transform them to GitHub URLs + $pattern = 'href=["\''](\.\./[^"\'']*(\.cs|\.csproj))(#L\d+)?["\'']' + + $newContent = $content -replace $pattern, "href=`"$repoUrl/`$1`$3`"" + + # Only write the file if changes were made + if ($newContent -ne $content) { + $matches = [regex]::Matches($content, $pattern) + $linkCount += $matches.Count + Set-Content -Path $file.FullName -Value $newContent + Write-Host "Fixed $($matches.Count) links in $($file.Name)" + } +} + +Write-Host "Link transformation complete. Fixed $linkCount links in total." \ No newline at end of file From 856c1dfb25fe7ebcdb1ff4f406e6891ddb111a5f Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 5 Apr 2025 22:17:39 -0500 Subject: [PATCH 913/947] Better link fixup. --- Documentation/fix-links.ps1 | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/Documentation/fix-links.ps1 b/Documentation/fix-links.ps1 index 6759815f7..0d9692780 100644 --- a/Documentation/fix-links.ps1 +++ b/Documentation/fix-links.ps1 @@ -10,10 +10,17 @@ $linkCount = 0 foreach ($file in $htmlFiles) { $content = Get-Content -Path $file.FullName -Raw - # Find links like "../Folder/File.cs" and transform them to GitHub URLs - $pattern = 'href=["\''](\.\./[^"\'']*(\.cs|\.csproj))(#L\d+)?["\'']' + # Find links that start with "../" and transform them to GitHub URLs + $pattern = 'href=["\''](\.\./[^"\'']*)["\'']' - $newContent = $content -replace $pattern, "href=`"$repoUrl/`$1`$3`"" + # Use a scriptblock for the replacement to remove the "../" prefix + $newContent = $content -replace $pattern, { + $match = $args[0] + $originalLink = $args[0].Groups[1].Value + # Remove the "../" prefix for the GitHub URL + $relativePath = $originalLink -replace '^\.\.\/', '' + "href=`"$repoUrl/$relativePath`"" + } # Only write the file if changes were made if ($newContent -ne $content) { From 29e3cbe7235506eb6ee0c48ff4dd4119a3f773c8 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 5 Apr 2025 22:25:41 -0500 Subject: [PATCH 914/947] I'll admit I'm just letting claude do this. --- Documentation/fix-links.ps1 | 25 ++++++++++++++++--------- 1 file changed, 16 insertions(+), 9 deletions(-) diff --git a/Documentation/fix-links.ps1 b/Documentation/fix-links.ps1 index 0d9692780..3726587b8 100644 --- a/Documentation/fix-links.ps1 +++ b/Documentation/fix-links.ps1 @@ -9,21 +9,28 @@ $htmlFiles = Get-ChildItem -Path $sitePath -Filter "*.html" -Recurse $linkCount = 0 foreach ($file in $htmlFiles) { $content = Get-Content -Path $file.FullName -Raw + $originalContent = $content # Find links that start with "../" and transform them to GitHub URLs - $pattern = 'href=["\''](\.\./[^"\'']*)["\'']' + # Important: We need to capture the entire href attribute and only change the value + $pattern = '(href=["\'])(\.\./[^"\']*)(["\'])' - # Use a scriptblock for the replacement to remove the "../" prefix - $newContent = $content -replace $pattern, { - $match = $args[0] - $originalLink = $args[0].Groups[1].Value + # Use a scriptblock for the replacement to keep the original quote style and only modify the URL + $newContent = [regex]::Replace($content, $pattern, { + param($match) + $prefix = $match.Groups[1].Value # href=" or href=' + $path = $match.Groups[2].Value # ../path + $suffix = $match.Groups[3].Value # " or ' + # Remove the "../" prefix for the GitHub URL - $relativePath = $originalLink -replace '^\.\.\/', '' - "href=`"$repoUrl/$relativePath`"" - } + $relativePath = $path -replace '^\.\.\/', '' + + # Return the full replacement with the same quote style + return "$prefix$repoUrl/$relativePath$suffix" + }) # Only write the file if changes were made - if ($newContent -ne $content) { + if ($newContent -ne $originalContent) { $matches = [regex]::Matches($content, $pattern) $linkCount += $matches.Count Set-Content -Path $file.FullName -Value $newContent From 8110e080af5e03c5b00835aa489a27df74fcb703 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 6 Apr 2025 00:43:35 -0500 Subject: [PATCH 915/947] Escapes. --- Documentation/fix-links.ps1 | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/Documentation/fix-links.ps1 b/Documentation/fix-links.ps1 index 3726587b8..083cc383f 100644 --- a/Documentation/fix-links.ps1 +++ b/Documentation/fix-links.ps1 @@ -12,8 +12,7 @@ foreach ($file in $htmlFiles) { $originalContent = $content # Find links that start with "../" and transform them to GitHub URLs - # Important: We need to capture the entire href attribute and only change the value - $pattern = '(href=["\'])(\.\./[^"\']*)(["\'])' + $pattern = '(href=["''])(\.\.\/[^"'']*)(["''])' # Use a scriptblock for the replacement to keep the original quote style and only modify the URL $newContent = [regex]::Replace($content, $pattern, { From d15b538200374cfa339ae3cd3813809435d652f5 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 6 Apr 2025 01:02:55 -0500 Subject: [PATCH 916/947] save me claude --- Documentation/fix-links.ps1 | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Documentation/fix-links.ps1 b/Documentation/fix-links.ps1 index 083cc383f..e9d3d74de 100644 --- a/Documentation/fix-links.ps1 +++ b/Documentation/fix-links.ps1 @@ -3,8 +3,8 @@ param ( [string]$repoUrl = "https://github.com/bepu/bepuphysics2/blob/master" ) -# Get all HTML files in the site -$htmlFiles = Get-ChildItem -Path $sitePath -Filter "*.html" -Recurse +# Get all HTML files in the site, excluding those in the api/ directory +$htmlFiles = Get-ChildItem -Path $sitePath -Filter "*.html" -Recurse | Where-Object { $_.FullName -notmatch "\\api\\" } $linkCount = 0 foreach ($file in $htmlFiles) { From 40eb72e7dab1d71bd56a0d98df2a140805d38e2b Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 6 Apr 2025 01:08:28 -0500 Subject: [PATCH 917/947] A few more missing ones. --- Documentation/fix-links.ps1 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Documentation/fix-links.ps1 b/Documentation/fix-links.ps1 index e9d3d74de..22e372fc0 100644 --- a/Documentation/fix-links.ps1 +++ b/Documentation/fix-links.ps1 @@ -12,7 +12,7 @@ foreach ($file in $htmlFiles) { $originalContent = $content # Find links that start with "../" and transform them to GitHub URLs - $pattern = '(href=["''])(\.\.\/[^"'']*)(["''])' + $pattern = '(href=["''])(\.\.\/[^"'']*\.cs)(["''])' # Use a scriptblock for the replacement to keep the original quote style and only modify the URL $newContent = [regex]::Replace($content, $pattern, { From 52e24267e6b8a2885d2f9a34ddd477153d84299a Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 13 Apr 2025 13:35:25 -0500 Subject: [PATCH 918/947] Link fiddling. Had a bad pattern. --- Documentation/fix-links.ps1 | 27 ++++++++++++++++++++------- 1 file changed, 20 insertions(+), 7 deletions(-) diff --git a/Documentation/fix-links.ps1 b/Documentation/fix-links.ps1 index 22e372fc0..d6d378b0f 100644 --- a/Documentation/fix-links.ps1 +++ b/Documentation/fix-links.ps1 @@ -11,21 +11,34 @@ foreach ($file in $htmlFiles) { $content = Get-Content -Path $file.FullName -Raw $originalContent = $content - # Find links that start with "../" and transform them to GitHub URLs - $pattern = '(href=["''])(\.\.\/[^"'']*\.cs)(["''])' + Write-Host "Considering file: $($file.FullName)" + + # Find all links that end with .cs + $pattern = '(href=["''])([^"'']*\.cs)(["''])' # Use a scriptblock for the replacement to keep the original quote style and only modify the URL $newContent = [regex]::Replace($content, $pattern, { param($match) $prefix = $match.Groups[1].Value # href=" or href=' - $path = $match.Groups[2].Value # ../path + $path = $match.Groups[2].Value # path $suffix = $match.Groups[3].Value # " or ' - # Remove the "../" prefix for the GitHub URL - $relativePath = $path -replace '^\.\.\/', '' + # Skip if it's already a full URL + if ($path -match '^https?:\/\/') { + Write-Host " Skipping already absolute URL: $path" + return $match.Value + } + + # If the path starts with "../", remove that prefix for the GitHub URL + if ($path -match '^\.\.\/') { + $relativePath = $path -replace '^\.\.\/', '' + Write-Host " Rewriting link with ../ prefix: $path -> $repoUrl/$relativePath" + return "$prefix$repoUrl/$relativePath$suffix" + } - # Return the full replacement with the same quote style - return "$prefix$repoUrl/$relativePath$suffix" + # Otherwise, just prepend the GitHub URL to the relative path + Write-Host " Rewriting link ending with .cs: $path -> $repoUrl/$path" + return "$prefix$repoUrl/$path$suffix" }) # Only write the file if changes were made From 0be51955edb3d1526c1cc88024bc5577267b0cd2 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 13 Apr 2025 13:46:47 -0500 Subject: [PATCH 919/947] Claude doin' my job. --- Documentation/fix-links.ps1 | 58 +++++++++++++++++++++++++++---------- 1 file changed, 43 insertions(+), 15 deletions(-) diff --git a/Documentation/fix-links.ps1 b/Documentation/fix-links.ps1 index d6d378b0f..9846f643c 100644 --- a/Documentation/fix-links.ps1 +++ b/Documentation/fix-links.ps1 @@ -13,40 +13,68 @@ foreach ($file in $htmlFiles) { Write-Host "Considering file: $($file.FullName)" - # Find all links that end with .cs - $pattern = '(href=["''])([^"'']*\.cs)(["''])' + # Pattern to find all href links + $pattern = '(href=["''])([^"'']*)(["''])' - # Use a scriptblock for the replacement to keep the original quote style and only modify the URL + # Use a scriptblock for the replacement $newContent = [regex]::Replace($content, $pattern, { param($match) $prefix = $match.Groups[1].Value # href=" or href=' $path = $match.Groups[2].Value # path $suffix = $match.Groups[3].Value # " or ' - # Skip if it's already a full URL - if ($path -match '^https?:\/\/') { - Write-Host " Skipping already absolute URL: $path" + # Skip if it's already an absolute URL or has special protocols + if ($path -match '^(https?:|mailto:|#|javascript:)') { return $match.Value } - # If the path starts with "../", remove that prefix for the GitHub URL - if ($path -match '^\.\.\/') { + # Skip if it's a reference to an HTML file (we don't want to rewrite these) + if ($path -match '\.html$') { + return $match.Value + } + + # Case 1: Path starts with "../" (from Documentation directory) + if ($path -match '^\.\./') { $relativePath = $path -replace '^\.\.\/', '' - Write-Host " Rewriting link with ../ prefix: $path -> $repoUrl/$relativePath" + Write-Host " Rewriting '../' link: $path -> $repoUrl/$relativePath" return "$prefix$repoUrl/$relativePath$suffix" } - # Otherwise, just prepend the GitHub URL to the relative path - Write-Host " Rewriting link ending with .cs: $path -> $repoUrl/$path" - return "$prefix$repoUrl/$path$suffix" + # Case 2: Path ends with ".cs" (code file reference) + if ($path -match '\.cs$') { + Write-Host " Rewriting '.cs' link: $path -> $repoUrl/$path" + return "$prefix$repoUrl/$path$suffix" + } + + # Case 3: Path is a directory link (no file extension) + # We'll assume it's a directory if it doesn't have a file extension + if ($path -ne "" -and $path -notmatch '\.[a-zA-Z0-9]+$') { + # Remove trailing slash if present for consistency + $cleanPath = $path -replace '/$', '' + Write-Host " Rewriting directory link: $path -> $repoUrl/$cleanPath" + return "$prefix$repoUrl/$cleanPath$suffix" + } + + # Default: return unchanged + return $match.Value }) # Only write the file if changes were made if ($newContent -ne $originalContent) { - $matches = [regex]::Matches($content, $pattern) - $linkCount += $matches.Count + $matches = [regex]::Matches($content, $pattern).Where({ + $path = $_.Groups[2].Value + # Count only the links we're actually rewriting + return ( + ($path -match '^\.\./') -or + ($path -match '\.cs$') -or + ($path -ne "" -and $path -notmatch '\.[a-zA-Z0-9]+$' -and $path -notmatch '^(https?:|mailto:|#|javascript:)') + ) + }) + + $fileChanges = $matches.Count + $linkCount += $fileChanges Set-Content -Path $file.FullName -Value $newContent - Write-Host "Fixed $($matches.Count) links in $($file.Name)" + Write-Host "Fixed $fileChanges links in $($file.Name)" } } From 65432b1f397b6c3633987ebfd3b2579d3051eae9 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 13 Apr 2025 13:48:55 -0500 Subject: [PATCH 920/947] Oops! --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index a1b522b96..c5cd63950 100644 --- a/README.md +++ b/README.md @@ -5,7 +5,7 @@ This is the repo for the bepuphysics v2 library, a complete rewrite of the C# 3d rigid body physics engine [BEPUphysics v1](https://github.com/bepu/bepuphysics1). -The BepuPhysics and BepuUtilities libraries target .NET 6 and should work on any supported platform. The demos application, Demos.sln, uses DX11 by default. There is also a Demos.GL.sln that uses OpenGL and should run on other platforms. The demos can be run from the command line (in the repo root directory) with `dotnet run --project Demos/Demos.csproj -c Release` or `dotnet run --project Demos.GL/Demos.csproj -c Release`. +The BepuPhysics and BepuUtilities libraries target .NET 8 and should work on any supported platform. The demos application, Demos.sln, uses DX11 by default. There is also a Demos.GL.sln that uses OpenGL and should run on other platforms. The demos can be run from the command line (in the repo root directory) with `dotnet run --project Demos/Demos.csproj -c Release` or `dotnet run --project Demos.GL/Demos.csproj -c Release`. The physics engine heavily uses `System.Numerics.Vectors` types, so to get good performance, you'll need a compiler which can consume those types (like RyuJIT). From 984f1d6548a695e2838687ce568531482dcec26c Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 13 Apr 2025 16:13:02 -0500 Subject: [PATCH 921/947] Docupdates. --- Documentation/index.md | 21 ++++++--------------- README.md | 30 ++++-------------------------- 2 files changed, 10 insertions(+), 41 deletions(-) diff --git a/Documentation/index.md b/Documentation/index.md index 844daf2a3..11c5f00c0 100644 --- a/Documentation/index.md +++ b/Documentation/index.md @@ -1,25 +1,16 @@ --- _disableToc: false --- +# bepuphysics docs! -# Quick Start +There are [conceptual](https://docs.bepuphysics.com/GettingStarted.html) *and* [API](https://docs.bepuphysics.com/api/BepuPhysics.html) docs! + +See [Getting Started](GettingStarted.md) for an introduction to the library. The BepuPhysics and BepuUtilities libraries target .NET 8 and should work on any supported platform. The physics engine heavily uses `System.Numerics.Vectors` types, so to get good performance, you'll need a compiler which can consume those types (like RyuJIT). -## Features +The demos application, Demos.sln, uses DX11 by default. There is also a Demos.GL.sln that uses OpenGL and should run on other platforms. The demos can be run from the command line (in the repo root directory) with `dotnet run --project Demos/Demos.csproj -c Release` or `dotnet run --project Demos.GL/Demos.csproj -c Release`. -- Spheres, capsules, boxes, triangles, cylinders, and convex hulls -- Compounds of the above -- Meshes -- A [whole bunch of constraint types](BepuPhysics/Constraints/) -- [Newts](Demos/Demos/NewtDemo.cs) -- Linear and angular continuous collision detection -- Extremely low cost sleep states for resting bodies -- Efficient scene-wide ray and sweep queries -- [Character controller example](Demos/Demos/Characters/CharacterDemo.cs) -- At least somewhat extensible collision pipeline, with [example custom voxel collidable](Demos/Demos/CustomVoxelCollidableDemo.cs) -- Highly nonidiomatic APIs -- Super speediness -- And a bunch of other miscellaneous stuff! \ No newline at end of file +To build the source, the easiest option is a recent version of Visual Studio with the .NET desktop development workload installed. Demos.sln references all relevant projects. For more information, see [Building](Building.md). diff --git a/README.md b/README.md index c5cd63950..244bcbd13 100644 --- a/README.md +++ b/README.md @@ -9,7 +9,7 @@ The BepuPhysics and BepuUtilities libraries target .NET 8 and should work on any The physics engine heavily uses `System.Numerics.Vectors` types, so to get good performance, you'll need a compiler which can consume those types (like RyuJIT). -To build the source, you'll need a recent version of Visual Studio with the .NET desktop development workload installed. Demos.sln references all relevant projects. For more information, see [Building](Documentation/Building.md). +To build the source, the easiest option is a recent version of Visual Studio with the .NET desktop development workload installed. Demos.sln references all relevant projects. For more information, see [Building](Documentation/Building.md). ## Features @@ -33,32 +33,10 @@ Report bugs [on the issues tab](../../issues). Use the [discussions tab](../../discussions) for... discussions. And questions. -By user request, there's a [discord server](https://discord.gg/ssa2XpY). I'll be focusing on github for long-form content, but if you like discord, now you can discord. +There's a [discord server](https://discord.gg/ssa2XpY). I'll be focusing on github for long-form content, but if you like discord, you can discord. -[Getting Started](Documentation/GettingStarted.md) +[Documentation pages](https://docs.bepuphysics.com/) in a conventional form factor exist! (If I've broken the docs page, see the [raw repo versions](https://github.com/bepu/bepuphysics2/tree/master/Documentation) as a backup or [github pages](https://bepu.github.io/bepuphysics2/) if I just broke the domain redirect.) -[Building](Documentation/Building.md) - -[Q&A](Documentation/QuestionsAndAnswers.md) - -[Stability Tips](Documentation/StabilityTips.md) - -[Performance Tips](Documentation/PerformanceTips.md) - -[Substepping](Documentation/Substepping.md) - -[Continuous Collision Detection](Documentation/ContinuousCollisionDetection.md) - -[Contributing](CONTRIBUTING.md) - -[Change log](Documentation/changelog.md) - -[Upgrading from v1, concept mapping](Documentation/UpgradingFromV1.md) - -[Packaging and Versioning](Documentation/PackagingAndVersioning.md) - -Check the [roadmap](Documentation/roadmap.md) for a high level look at where things are going. - -If you have too many dollars, we are willing to consume them through [github sponsors](https://www.github.com/sponsors/RossNordby). +If you have too many dollars, I'm willing to consume them through [github sponsors](https://www.github.com/sponsors/RossNordby). Please do not give me any amount of money that feels even slightly painful. Development is not conditional on sponsorships, and I already have a goodly number of dollars. ![](https://raw.githubusercontent.com/bepu/bepuphysics1/master/Documentation/images/readme/angelduck.png) From 7ee05bff561f90e60b0a84730ed59599f2492566 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 13 Apr 2025 16:44:05 -0500 Subject: [PATCH 922/947] well that's convenient --- .github/workflows/bepu-docs-github.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.github/workflows/bepu-docs-github.yml b/.github/workflows/bepu-docs-github.yml index 73db42c2a..d242b50b2 100644 --- a/.github/workflows/bepu-docs-github.yml +++ b/.github/workflows/bepu-docs-github.yml @@ -41,4 +41,5 @@ jobs: with: github_token: ${{ secrets.GITHUB_TOKEN }} publish_dir: Documentation/_site - publish_branch: gh-pages \ No newline at end of file + publish_branch: gh-pages + cname: docs.bepuphysics.com \ No newline at end of file From de46d903ea5b07b0f215ecccdea0122adc60f466 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 13 Apr 2025 20:15:19 -0500 Subject: [PATCH 923/947] AddWithoutAwakeningBodies added for convenience. --- BepuPhysics/Statics.cs | 32 ++++++++++++++++++++++++++++++++ 1 file changed, 32 insertions(+) diff --git a/BepuPhysics/Statics.cs b/BepuPhysics/Statics.cs index 7dfe00ace..ed007f10f 100644 --- a/BepuPhysics/Statics.cs +++ b/BepuPhysics/Statics.cs @@ -44,6 +44,15 @@ public bool ShouldAwaken(BodyReference body) } } + /// + /// Awakening filter that prevents any bodies from being awoken by the static's state change. + /// + public struct StaticsShouldntAwakenBodies : IStaticChangeAwakeningFilter + { + public bool AllowAwakening => false; + public bool ShouldAwaken(BodyReference body) => false; + } + /// /// Stores data for a static collidable in the simulation. Statics can be posed and collide, but have no velocity and no dynamic behavior. /// @@ -416,6 +425,29 @@ public StaticHandle Add(in StaticDescription description) { var defaultFilter = default(StaticsShouldntAwakenKinematics); return Add(description, ref defaultFilter); + } + + /// + /// Adds a new static body to the simulation. No attempt is made to awaken sleeping bodies near the static. + /// + /// Description of the static to add. + /// Handle of the new static. + /// + /// For most use cases, defaulting to the function is recommended; + /// it can help avoid surprising behavior by waking up sleeping bodies near the new static. + /// The query does, however, carry a nonzero cost. + /// + /// If many statics are being added at once, particularly if the new statics overlap with a lot of existing statics, + /// and there's no need to awaken sleeping bodies, this function can be used to avoid the cost of the query. + /// + /// + /// Other custom filters can be used to control which bodies are awoken; see and the interface. + /// + /// + public StaticHandle AddWithoutAwakeningBodies(in StaticDescription description) + { + var filter = default(StaticsShouldntAwakenBodies); + return Add(description, ref filter); } /// From 7688e66450474243aeb7ca392c8b75e86d26143a Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 4 May 2025 23:01:26 -0500 Subject: [PATCH 924/947] oops there was no documentation on that type at all --- BepuPhysics/Constraints/ServoSettings.cs | 318 ++++++++++++----------- 1 file changed, 172 insertions(+), 146 deletions(-) diff --git a/BepuPhysics/Constraints/ServoSettings.cs b/BepuPhysics/Constraints/ServoSettings.cs index e1ee3b2c6..ccb719af4 100644 --- a/BepuPhysics/Constraints/ServoSettings.cs +++ b/BepuPhysics/Constraints/ServoSettings.cs @@ -3,165 +3,191 @@ using System.Numerics; using System.Runtime.CompilerServices; -namespace BepuPhysics.Constraints +namespace BepuPhysics.Constraints; + +/// +/// Describes how a quickly and strongly a servo constraint should move towards a position target. +/// +/// +/// The constraint will attempt to reach a speed between and using a force no greater than . +/// The speed that the constraint will attempt to use before clamping is based on its spring settings. +/// +public struct ServoSettings { - public struct ServoSettings - { - public float MaximumSpeed; - public float BaseSpeed; - public float MaximumForce; + /// + /// Maximum speed that the constraint can try to use to move towards the target. + /// + public float MaximumSpeed; + /// + /// Minimum speed that the constraint will try to use to move towards the target. + /// If the speed implied by the spring configuration is higher than this, the servo will attempt to use the higher speed. + /// Will be clamped by the MaximumSpeed. + /// + public float BaseSpeed; + /// + /// The maximum force that the constraint can apply to move towards the target. + /// + /// + /// This value is specified in terms of force: a change in momentum over time. It is approximated as a maximum impulse (an instantaneous change in momentum) on a per-substep basis. In other words, for a given velocity iteration, the constraint's impulse can be no larger than * dt where dt is the substep duration. + /// + public float MaximumForce; - /// - /// Gets settings representing a servo with unlimited force, speed, and no base speed. - /// - public static ServoSettings Default { get { return new ServoSettings(float.MaxValue, 0, float.MaxValue); } } + /// + /// Gets settings representing a servo with unlimited force, speed, and no base speed. + /// A servo with these settings will behave like a conventional position-level constraint. + /// + public static ServoSettings Default { get { return new ServoSettings(float.MaxValue, 0, float.MaxValue); } } - /// - /// Checks servo settings to ensure valid values. - /// - /// Settings to check. - /// True if the settings contain valid values, false otherwise. - public static bool Validate(in ServoSettings settings) - { - return ConstraintChecker.IsNonnegativeNumber(settings.MaximumSpeed) && ConstraintChecker.IsNonnegativeNumber(settings.BaseSpeed) && ConstraintChecker.IsNonnegativeNumber(settings.MaximumForce); - } + /// + /// Checks servo settings to ensure valid values. + /// + /// Settings to check. + /// True if the settings contain valid values, false otherwise. + public static bool Validate(in ServoSettings settings) + { + return ConstraintChecker.IsNonnegativeNumber(settings.MaximumSpeed) && ConstraintChecker.IsNonnegativeNumber(settings.BaseSpeed) && ConstraintChecker.IsNonnegativeNumber(settings.MaximumForce); + } + + /// + /// Creates a new servo settings instance with the specified properties. + /// + /// Sets the property. + /// Sets the property. + /// Sets the property. + public ServoSettings(float maximumSpeed, float baseSpeed, float maximumForce) + { + MaximumSpeed = maximumSpeed; + BaseSpeed = baseSpeed; + MaximumForce = maximumForce; + Debug.Assert(Validate(this), "Servo settings must have nonnegative maximum speed, base speed, and maximum force."); + } +} +public struct ServoSettingsWide +{ + public Vector MaximumSpeed; + public Vector BaseSpeed; + public Vector MaximumForce; - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ServoSettings(float maximumSpeed, float baseSpeed, float maximumForce) - { - MaximumSpeed = maximumSpeed; - BaseSpeed = baseSpeed; - MaximumForce = maximumForce; - Debug.Assert(Validate(this), "Servo settings must have nonnegative maximum speed, base speed, and maximum force."); - } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void ComputeClampedBiasVelocity(in Vector error, in Vector positionErrorToVelocity, in ServoSettingsWide servoSettings, float dt, float inverseDt, + out Vector clampedBiasVelocity, out Vector maximumImpulse) + { + //Can't request speed that would cause an overshoot. + var baseSpeed = Vector.Min(servoSettings.BaseSpeed, Vector.Abs(error) * new Vector(inverseDt)); + var biasVelocity = error * positionErrorToVelocity; + clampedBiasVelocity = Vector.ConditionalSelect(Vector.LessThan(biasVelocity, Vector.Zero), + Vector.Max(-servoSettings.MaximumSpeed, Vector.Min(-baseSpeed, biasVelocity)), + Vector.Min(servoSettings.MaximumSpeed, Vector.Max(baseSpeed, biasVelocity))); + maximumImpulse = servoSettings.MaximumForce * new Vector(dt); } - public struct ServoSettingsWide + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void ComputeClampedBiasVelocity(in Vector2Wide errorAxis, in Vector errorLength, in Vector positionErrorToBiasVelocity, in ServoSettingsWide servoSettings, + float dt, float inverseDt, out Vector2Wide clampedBiasVelocity, out Vector maximumImpulse) { - public Vector MaximumSpeed; - public Vector BaseSpeed; - public Vector MaximumForce; - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ComputeClampedBiasVelocity(in Vector error, in Vector positionErrorToVelocity, in ServoSettingsWide servoSettings, float dt, float inverseDt, - out Vector clampedBiasVelocity, out Vector maximumImpulse) - { - //Can't request speed that would cause an overshoot. - var baseSpeed = Vector.Min(servoSettings.BaseSpeed, Vector.Abs(error) * new Vector(inverseDt)); - var biasVelocity = error * positionErrorToVelocity; - clampedBiasVelocity = Vector.ConditionalSelect(Vector.LessThan(biasVelocity, Vector.Zero), - Vector.Max(-servoSettings.MaximumSpeed, Vector.Min(-baseSpeed, biasVelocity)), - Vector.Min(servoSettings.MaximumSpeed, Vector.Max(baseSpeed, biasVelocity))); - maximumImpulse = servoSettings.MaximumForce * new Vector(dt); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ComputeClampedBiasVelocity(in Vector2Wide errorAxis, in Vector errorLength, in Vector positionErrorToBiasVelocity, in ServoSettingsWide servoSettings, - float dt, float inverseDt, out Vector2Wide clampedBiasVelocity, out Vector maximumImpulse) - { - //Can't request speed that would cause an overshoot. - var baseSpeed = Vector.Min(servoSettings.BaseSpeed, errorLength * new Vector(inverseDt)); - var unclampedBiasSpeed = errorLength * positionErrorToBiasVelocity; - var targetSpeed = Vector.Max(baseSpeed, unclampedBiasSpeed); - var scale = Vector.Min(Vector.One, servoSettings.MaximumSpeed / targetSpeed); - //Protect against division by zero. The min would handle inf, but if MaximumSpeed is 0, it turns into a NaN. - var useFallback = Vector.LessThan(targetSpeed, new Vector(1e-10f)); - scale = Vector.ConditionalSelect(useFallback, Vector.One, scale); - Vector2Wide.Scale(errorAxis, scale * unclampedBiasSpeed, out clampedBiasVelocity); - maximumImpulse = servoSettings.MaximumForce * new Vector(dt); - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ComputeClampedBiasVelocity(in Vector2Wide error, in Vector positionErrorToBiasVelocity, in ServoSettingsWide servoSettings, - float dt, float inverseDt, out Vector2Wide clampedBiasVelocity, out Vector maximumImpulse) - { - Vector2Wide.Length(error, out var errorLength); - Vector2Wide.Scale(error, Vector.One / errorLength, out var errorAxis); - var useFallback = Vector.LessThan(errorLength, new Vector(1e-10f)); - errorAxis.X = Vector.ConditionalSelect(useFallback, Vector.Zero, errorAxis.X); - errorAxis.Y = Vector.ConditionalSelect(useFallback, Vector.Zero, errorAxis.Y); - ComputeClampedBiasVelocity(errorAxis, errorLength, positionErrorToBiasVelocity, servoSettings, dt, inverseDt, out clampedBiasVelocity, out maximumImpulse); - } + //Can't request speed that would cause an overshoot. + var baseSpeed = Vector.Min(servoSettings.BaseSpeed, errorLength * new Vector(inverseDt)); + var unclampedBiasSpeed = errorLength * positionErrorToBiasVelocity; + var targetSpeed = Vector.Max(baseSpeed, unclampedBiasSpeed); + var scale = Vector.Min(Vector.One, servoSettings.MaximumSpeed / targetSpeed); + //Protect against division by zero. The min would handle inf, but if MaximumSpeed is 0, it turns into a NaN. + var useFallback = Vector.LessThan(targetSpeed, new Vector(1e-10f)); + scale = Vector.ConditionalSelect(useFallback, Vector.One, scale); + Vector2Wide.Scale(errorAxis, scale * unclampedBiasSpeed, out clampedBiasVelocity); + maximumImpulse = servoSettings.MaximumForce * new Vector(dt); + } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ComputeClampedBiasVelocity(in Vector3Wide errorAxis, in Vector errorLength, in Vector positionErrorToBiasVelocity, in ServoSettingsWide servoSettings, - float dt, float inverseDt, out Vector3Wide clampedBiasVelocity, out Vector maximumImpulse) - { - //Can't request speed that would cause an overshoot. - var baseSpeed = Vector.Min(servoSettings.BaseSpeed, errorLength * new Vector(inverseDt)); - var unclampedBiasSpeed = errorLength * positionErrorToBiasVelocity; - var targetSpeed = Vector.Max(baseSpeed, unclampedBiasSpeed); - var scale = Vector.Min(Vector.One, servoSettings.MaximumSpeed / targetSpeed); - //Protect against division by zero. The min would handle inf, but if MaximumSpeed is 0, it turns into a NaN. - var useFallback = Vector.LessThan(targetSpeed, new Vector(1e-10f)); - scale = Vector.ConditionalSelect(useFallback, Vector.One, scale); - Vector3Wide.Scale(errorAxis, scale * unclampedBiasSpeed, out clampedBiasVelocity); - maximumImpulse = servoSettings.MaximumForce * new Vector(dt); - } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void ComputeClampedBiasVelocity(in Vector2Wide error, in Vector positionErrorToBiasVelocity, in ServoSettingsWide servoSettings, + float dt, float inverseDt, out Vector2Wide clampedBiasVelocity, out Vector maximumImpulse) + { + Vector2Wide.Length(error, out var errorLength); + Vector2Wide.Scale(error, Vector.One / errorLength, out var errorAxis); + var useFallback = Vector.LessThan(errorLength, new Vector(1e-10f)); + errorAxis.X = Vector.ConditionalSelect(useFallback, Vector.Zero, errorAxis.X); + errorAxis.Y = Vector.ConditionalSelect(useFallback, Vector.Zero, errorAxis.Y); + ComputeClampedBiasVelocity(errorAxis, errorLength, positionErrorToBiasVelocity, servoSettings, dt, inverseDt, out clampedBiasVelocity, out maximumImpulse); + } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ComputeClampedBiasVelocity(in Vector3Wide error, in Vector positionErrorToBiasVelocity, in ServoSettingsWide servoSettings, - float dt, float inverseDt, out Vector3Wide clampedBiasVelocity, out Vector maximumImpulse) - { - Vector3Wide.Length(error, out var errorLength); - Vector3Wide.Scale(error, Vector.One / errorLength, out var errorAxis); - var useFallback = Vector.LessThan(errorLength, new Vector(1e-10f)); - errorAxis.X = Vector.ConditionalSelect(useFallback, Vector.Zero, errorAxis.X); - errorAxis.Y = Vector.ConditionalSelect(useFallback, Vector.Zero, errorAxis.Y); - errorAxis.Z = Vector.ConditionalSelect(useFallback, Vector.Zero, errorAxis.Z); - ComputeClampedBiasVelocity(errorAxis, errorLength, positionErrorToBiasVelocity, servoSettings, dt, inverseDt, out clampedBiasVelocity, out maximumImpulse); - } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void ComputeClampedBiasVelocity(in Vector3Wide errorAxis, in Vector errorLength, in Vector positionErrorToBiasVelocity, in ServoSettingsWide servoSettings, + float dt, float inverseDt, out Vector3Wide clampedBiasVelocity, out Vector maximumImpulse) + { + //Can't request speed that would cause an overshoot. + var baseSpeed = Vector.Min(servoSettings.BaseSpeed, errorLength * new Vector(inverseDt)); + var unclampedBiasSpeed = errorLength * positionErrorToBiasVelocity; + var targetSpeed = Vector.Max(baseSpeed, unclampedBiasSpeed); + var scale = Vector.Min(Vector.One, servoSettings.MaximumSpeed / targetSpeed); + //Protect against division by zero. The min would handle inf, but if MaximumSpeed is 0, it turns into a NaN. + var useFallback = Vector.LessThan(targetSpeed, new Vector(1e-10f)); + scale = Vector.ConditionalSelect(useFallback, Vector.One, scale); + Vector3Wide.Scale(errorAxis, scale * unclampedBiasSpeed, out clampedBiasVelocity); + maximumImpulse = servoSettings.MaximumForce * new Vector(dt); + } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ClampImpulse(in Vector maximumImpulse, ref Vector accumulatedImpulse, ref Vector csi) - { - var previousImpulse = accumulatedImpulse; - accumulatedImpulse = Vector.Max(-maximumImpulse, Vector.Min(maximumImpulse, accumulatedImpulse + csi)); - csi = accumulatedImpulse - previousImpulse; - } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void ComputeClampedBiasVelocity(in Vector3Wide error, in Vector positionErrorToBiasVelocity, in ServoSettingsWide servoSettings, + float dt, float inverseDt, out Vector3Wide clampedBiasVelocity, out Vector maximumImpulse) + { + Vector3Wide.Length(error, out var errorLength); + Vector3Wide.Scale(error, Vector.One / errorLength, out var errorAxis); + var useFallback = Vector.LessThan(errorLength, new Vector(1e-10f)); + errorAxis.X = Vector.ConditionalSelect(useFallback, Vector.Zero, errorAxis.X); + errorAxis.Y = Vector.ConditionalSelect(useFallback, Vector.Zero, errorAxis.Y); + errorAxis.Z = Vector.ConditionalSelect(useFallback, Vector.Zero, errorAxis.Z); + ComputeClampedBiasVelocity(errorAxis, errorLength, positionErrorToBiasVelocity, servoSettings, dt, inverseDt, out clampedBiasVelocity, out maximumImpulse); + } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ClampImpulse(in Vector maximumImpulse, ref Vector2Wide accumulatedImpulse, ref Vector2Wide csi) - { - var previousImpulse = accumulatedImpulse; - Vector2Wide.Add(accumulatedImpulse, csi, out var unclamped); - Vector2Wide.Length(unclamped, out var impulseMagnitude); - var impulseScale = Vector.ConditionalSelect( - Vector.LessThan(Vector.Abs(impulseMagnitude), new Vector(1e-10f)), - Vector.One, - Vector.Min(maximumImpulse / impulseMagnitude, Vector.One)); - Vector2Wide.Scale(unclamped, impulseScale, out accumulatedImpulse); - Vector2Wide.Subtract(accumulatedImpulse, previousImpulse, out csi); - } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void ClampImpulse(in Vector maximumImpulse, ref Vector accumulatedImpulse, ref Vector csi) + { + var previousImpulse = accumulatedImpulse; + accumulatedImpulse = Vector.Max(-maximumImpulse, Vector.Min(maximumImpulse, accumulatedImpulse + csi)); + csi = accumulatedImpulse - previousImpulse; + } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ClampImpulse(in Vector maximumImpulse, ref Vector3Wide accumulatedImpulse, ref Vector3Wide csi) - { - var previousAccumulatedImpulse = accumulatedImpulse; - Vector3Wide.Add(accumulatedImpulse, csi, out accumulatedImpulse); - Vector3Wide.Length(accumulatedImpulse, out var impulseMagnitude); - var impulseScale = Vector.ConditionalSelect( - Vector.LessThan(Vector.Abs(impulseMagnitude), new Vector(1e-10f)), - Vector.One, - Vector.Min(maximumImpulse / impulseMagnitude, Vector.One)); - Vector3Wide.Scale(accumulatedImpulse, impulseScale, out accumulatedImpulse); - Vector3Wide.Subtract(accumulatedImpulse, previousAccumulatedImpulse, out csi); - } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void ClampImpulse(in Vector maximumImpulse, ref Vector2Wide accumulatedImpulse, ref Vector2Wide csi) + { + var previousImpulse = accumulatedImpulse; + Vector2Wide.Add(accumulatedImpulse, csi, out var unclamped); + Vector2Wide.Length(unclamped, out var impulseMagnitude); + var impulseScale = Vector.ConditionalSelect( + Vector.LessThan(Vector.Abs(impulseMagnitude), new Vector(1e-10f)), + Vector.One, + Vector.Min(maximumImpulse / impulseMagnitude, Vector.One)); + Vector2Wide.Scale(unclamped, impulseScale, out accumulatedImpulse); + Vector2Wide.Subtract(accumulatedImpulse, previousImpulse, out csi); + } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void WriteFirst(in ServoSettings source, ref ServoSettingsWide target) - { - GatherScatter.GetFirst(ref target.MaximumSpeed) = source.MaximumSpeed; - GatherScatter.GetFirst(ref target.BaseSpeed) = source.BaseSpeed; - GatherScatter.GetFirst(ref target.MaximumForce) = source.MaximumForce; - } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void ClampImpulse(in Vector maximumImpulse, ref Vector3Wide accumulatedImpulse, ref Vector3Wide csi) + { + var previousAccumulatedImpulse = accumulatedImpulse; + Vector3Wide.Add(accumulatedImpulse, csi, out accumulatedImpulse); + Vector3Wide.Length(accumulatedImpulse, out var impulseMagnitude); + var impulseScale = Vector.ConditionalSelect( + Vector.LessThan(Vector.Abs(impulseMagnitude), new Vector(1e-10f)), + Vector.One, + Vector.Min(maximumImpulse / impulseMagnitude, Vector.One)); + Vector3Wide.Scale(accumulatedImpulse, impulseScale, out accumulatedImpulse); + Vector3Wide.Subtract(accumulatedImpulse, previousAccumulatedImpulse, out csi); + } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void ReadFirst(in ServoSettingsWide source, out ServoSettings target) - { - target.MaximumSpeed = source.MaximumSpeed[0]; - target.BaseSpeed = source.BaseSpeed[0]; - target.MaximumForce = source.MaximumForce[0]; - } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void WriteFirst(in ServoSettings source, ref ServoSettingsWide target) + { + GatherScatter.GetFirst(ref target.MaximumSpeed) = source.MaximumSpeed; + GatherScatter.GetFirst(ref target.BaseSpeed) = source.BaseSpeed; + GatherScatter.GetFirst(ref target.MaximumForce) = source.MaximumForce; + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void ReadFirst(in ServoSettingsWide source, out ServoSettings target) + { + target.MaximumSpeed = source.MaximumSpeed[0]; + target.BaseSpeed = source.BaseSpeed[0]; + target.MaximumForce = source.MaximumForce[0]; } + } From bfb11dc2020555b09978c473d9655509e844032c Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 4 May 2025 23:04:55 -0500 Subject: [PATCH 925/947] Bonus clarifications. --- BepuPhysics/Constraints/ServoSettings.cs | 3 +++ 1 file changed, 3 insertions(+) diff --git a/BepuPhysics/Constraints/ServoSettings.cs b/BepuPhysics/Constraints/ServoSettings.cs index ccb719af4..642b83704 100644 --- a/BepuPhysics/Constraints/ServoSettings.cs +++ b/BepuPhysics/Constraints/ServoSettings.cs @@ -11,6 +11,9 @@ namespace BepuPhysics.Constraints; /// /// The constraint will attempt to reach a speed between and using a force no greater than . /// The speed that the constraint will attempt to use before clamping is based on its spring settings. +/// +/// Note that a 'position' target for the purposes of this type could also be an orientation target. For those constraints, speeds/forces are in terms of angular speed and torque. +/// /// public struct ServoSettings { From e967b0322a52e0f7038af8b53055330905efda7d Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 20 May 2025 17:56:36 -0500 Subject: [PATCH 926/947] Wee basic investigations and repro. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 9 +- Demos/Demo.cs | 2 +- Demos/DemoSet.cs | 1 + .../BroadPhaseStressTestDemo.cs | 92 ++++++++++++------- 4 files changed, 71 insertions(+), 33 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 688fbfa09..511cb3aa5 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -1159,11 +1159,14 @@ static unsafe void BinnedBuildNode( bestCentroidBoundingBoxB = accumulatedCentroidBoundingBoxB; int accumulatedLeafCountB = binLeafCounts[lastBinIndex]; int bestLeafCountB = 0; + float minCandidate = float.MaxValue; + float maxCandidate = float.MinValue; for (int splitIndexCandidate = lastBinIndex; splitIndexCandidate >= 1; --splitIndexCandidate) { var previousIndex = splitIndexCandidate - 1; var sahCandidate = ComputeBoundsMetric(binBoundingBoxesScan[previousIndex]) * (totalLeafCount - accumulatedLeafCountB) + ComputeBoundsMetric(accumulatedBoundingBoxB) * accumulatedLeafCountB; - + minCandidate = float.Min(minCandidate, sahCandidate); + maxCandidate = float.Max(maxCandidate, sahCandidate); if (sahCandidate < bestSAH) { bestSAH = sahCandidate; @@ -1180,6 +1183,10 @@ static unsafe void BinnedBuildNode( accumulatedCentroidBoundingBoxB.Max = Vector4.Max(centroidBoundsForBin.Max, accumulatedCentroidBoundingBoxB.Max); accumulatedLeafCountB += binLeafCounts[previousIndex]; } + //if (minCandidate == maxCandidate) + //{ + // Console.WriteLine("asdfh"); + //} if (bestLeafCountB == 0 || bestLeafCountB == totalLeafCount || bestSAH == float.MaxValue || float.IsNaN(bestSAH) || float.IsInfinity(bestSAH)) { //Some form of major problem detected! Fall back to a degenerate split. diff --git a/Demos/Demo.cs b/Demos/Demo.cs index 329db2071..697a85160 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -62,7 +62,7 @@ public virtual void Update(Window window, Camera camera, Input input, float dt) //fully decouple simulation and rendering rates across different threads. //(In either case, you'd also want to interpolate or extrapolate simulation results during rendering for smoothness.) //Note that taking steps of variable length can reduce stability. Gradual or one-off changes can work reasonably well. - Simulation.Timestep(TimestepDuration, ThreadDispatcher); + Simulation.Timestep(TimestepDuration);//, ThreadDispatcher); ////Here's an example of how it would look to use more frequent updates, but still with a fixed amount of time simulated per update call: //const float timeToSimulate = 1 / 60f; diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index e2b78ac67..378ee6df3 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -44,6 +44,7 @@ struct Option public DemoSet() { + AddOption(); AddOption(); AddOption(); AddOption(); diff --git a/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs b/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs index b3faf5309..a7ff2ee2c 100644 --- a/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs +++ b/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs @@ -65,9 +65,9 @@ public override void Initialize(ContentArchive content, Camera camera) var shape = new Sphere(0.5f); var sphereInertia = shape.ComputeInertia(1); var shapeIndex = Simulation.Shapes.Add(shape); - const int width = 2048; + const int width = 128; const int height = 2; - const int length = 2048; + const int length = 128; var spacing = new Vector3(16.01f); float randomization = 0.9f; var randomizationSpan = (spacing - new Vector3(1)) * randomization; @@ -81,18 +81,19 @@ public override void Initialize(ContentArchive content, Camera camera) { var r = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); //var location = spacing * (new Vector3(i, j, k) + new Vector3(-width, 1, -length)) + randomizationBase + r * randomizationSpan; - var location = (r - new Vector3(0.5f)) * (r - new Vector3(0.5f)) * spacing * new Vector3(width, height, length); + //var location = (r - new Vector3(0.5f)) * (r - new Vector3(0.5f)) * spacing * new Vector3(width, height, length); //var location = (r - new Vector3(0.5f)) * spacing * new Vector3(width, height, length); + var location = new Vector3(15, 15, 15); //var hash = HashHelper.Rehash(HashHelper.Rehash(HashHelper.Rehash(i) + HashHelper.Rehash(j)) + HashHelper.Rehash(k)); var hash = i + j + k; - if (hash % 64 == 0) - { - if (i == 7 && j == 1 && k == 0) - Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(100, 0, 100), sphereInertia, Simulation.Shapes.Add(new Sphere(100)), -1)); - else - Simulation.Bodies.Add(BodyDescription.CreateDynamic(location, sphereInertia, shapeIndex, -1)); - } - else + //if (hash % 64 == 0) + //{ + // if (i == 7 && j == 1 && k == 0) + // Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(100, 0, 100), sphereInertia, Simulation.Shapes.Add(new Sphere(100)), -1)); + // else + // Simulation.Bodies.Add(BodyDescription.CreateDynamic(location, sphereInertia, shapeIndex, -1)); + //} + //else { Simulation.Statics.Add(new StaticDescription(location, shapeIndex)); } @@ -101,7 +102,7 @@ public override void Initialize(ContentArchive content, Camera camera) } Console.WriteLine($"Body count: {Simulation.Bodies.ActiveSet.Count}"); Console.WriteLine($"Static count: {Simulation.Statics.Count}"); - Simulation.Statics.Add(new StaticDescription(new Vector3(0, -10, 0), Simulation.Shapes.Add(new Box(5000, 1, 5000)))); + //groundStatic = Simulation.Statics.Add(new StaticDescription(new Vector3(0, -10, 0), Simulation.Shapes.Add(new Box(5000, 1, 5000)))); startingLocations = new Vector3[Simulation.Bodies.ActiveSet.Count]; for (int i = 0; i < startingLocations.Length; ++i) { @@ -112,6 +113,7 @@ public override void Initialize(ContentArchive content, Camera camera) test2Times = new TimingsRingBuffer(sampleCount, BufferPool); intertreeTest2Times = new TimingsRingBuffer(sampleCount, BufferPool); } + StaticHandle groundStatic; const int sampleCount = 128; TimingsRingBuffer updateTimes; @@ -119,6 +121,30 @@ public override void Initialize(ContentArchive content, Camera camera) TimingsRingBuffer test2Times; TimingsRingBuffer intertreeTest2Times; long frameCount; + + void PrintPathToRoot(StaticHandle handle) + { + var index = Simulation.Statics[handle].Static.BroadPhaseIndex; + var leaf = Simulation.BroadPhase.StaticTree.Leaves[index]; + int depth = 0; + var nodeIndex = leaf.NodeIndex; + Console.Write($"Starting from {leaf.NodeIndex}:{leaf.ChildIndex}, path: "); + while (true) + { + ref var node = ref Simulation.BroadPhase.StaticTree.Metanodes[nodeIndex]; + Console.Write($"{nodeIndex}, "); + if (node.Parent >= 0) + { + nodeIndex = node.Parent; + depth++; + } + else + break; + } + Console.WriteLine($"; depth {depth}."); + + } + public override void Update(Window window, Camera camera, Input input, float dt) { var rotationAngle = frameCount * 1e-3f; @@ -132,31 +158,35 @@ public override void Update(Window window, Camera camera, Input input, float dt) // motion.Velocity.Linear = offset; //} + //if (frameCount % 32 == 0) + //PrintPathToRoot(groundStatic); + //Simulation.BroadPhase.ActiveTree.CacheOptimize(0); //Simulation.BroadPhase.StaticTree.CacheOptimize(0); base.Update(window, camera, input, dt); updateTimes.Add(Simulation.Profiler[Simulation.BroadPhase]); - testTimes.Add(Simulation.Profiler[Simulation.BroadPhaseOverlapFinder]); - - //var overlaps = new OverlapHandler(); - //Simulation.BroadPhase.ActiveTree.GetSelfOverlaps2(ref overlaps); - //var a = Stopwatch.GetTimestamp(); - //var threadedOverlaps = new TreeFiddlingTestDemo.ThreadedOverlapHandler(BufferPool, ThreadDispatcher.ThreadCount); - //Simulation.BroadPhase.ActiveTree.GetSelfOverlaps2(ref threadedOverlaps, BufferPool, ThreadDispatcher); - //var (selfOverlapCount, _) = threadedOverlaps.SumResults(); - //threadedOverlaps.Reset(); - //var b = Stopwatch.GetTimestamp(); - //Simulation.BroadPhase.ActiveTree.GetOverlaps2(ref Simulation.BroadPhase.ActiveTree, ref threadedOverlaps, BufferPool, ThreadDispatcher); - //var c = Stopwatch.GetTimestamp(); - //var interOverlaps = new OverlapHandler(); - ////Simulation.BroadPhase.ActiveTree.GetOverlaps(ref Simulation.BroadPhase.ActiveTree, ref interOverlaps); - //var (interOverlapCount, _) = threadedOverlaps.SumResults(); - //test2Times.Add((b - a) / (double)Stopwatch.Frequency); - //intertreeTest2Times.Add((c - b) / (double)Stopwatch.Frequency); - - + testTimes.Add(Simulation.Profiler[Simulation.BroadPhaseOverlapFinder]); + + //var overlaps = new OverlapHandler(); + //Simulation.BroadPhase.ActiveTree.GetSelfOverlaps2(ref overlaps); + //var a = Stopwatch.GetTimestamp(); + //var threadedOverlaps = new TreeFiddlingTestDemo.ThreadedOverlapHandler(BufferPool, ThreadDispatcher.ThreadCount); + //Simulation.BroadPhase.ActiveTree.GetSelfOverlaps2(ref threadedOverlaps, BufferPool, ThreadDispatcher); + //var (selfOverlapCount, _) = threadedOverlaps.SumResults(); + //threadedOverlaps.Reset(); + //var b = Stopwatch.GetTimestamp(); + //Simulation.BroadPhase.ActiveTree.GetOverlaps2(ref Simulation.BroadPhase.ActiveTree, ref threadedOverlaps, BufferPool, ThreadDispatcher); + //var c = Stopwatch.GetTimestamp(); + //var interOverlaps = new OverlapHandler(); + ////Simulation.BroadPhase.ActiveTree.GetOverlaps(ref Simulation.BroadPhase.ActiveTree, ref interOverlaps); + //var (interOverlapCount, _) = threadedOverlaps.SumResults(); + //test2Times.Add((b - a) / (double)Stopwatch.Frequency); + //intertreeTest2Times.Add((c - b) / (double)Stopwatch.Frequency); + if (frameCount++ % sampleCount == 0) { + Console.WriteLine($"Active Depth {frameCount}: {Simulation.BroadPhase.ActiveTree.ComputeMaximumDepth()}"); + Console.WriteLine($"Static Depth {frameCount}: {Simulation.BroadPhase.StaticTree.ComputeMaximumDepth()}"); var updateStats = updateTimes.ComputeStats(); var testStats = testTimes.ComputeStats(); var test2Stats = test2Times.ComputeStats(); From bf885cdba862aa4c8f9c5c5231a1fac1d0d7d59c Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 21 Jun 2025 18:51:59 -0500 Subject: [PATCH 927/947] Refinement degeneracy made more consistent. Improved refinement priority heuristic; degenerate trees should now have less degenerate topology. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 33 ++- BepuPhysics/Trees/Tree_Refine2.cs | 19 +- Demos/Demo.cs | 2 +- Demos/DemoSet.cs | 1 - .../BroadPhaseStressTestDemo.cs | 227 ++++++++++++------ 5 files changed, 179 insertions(+), 103 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 511cb3aa5..67b5f98c9 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -167,7 +167,15 @@ static unsafe void MicroSweepForBinnedBuilder( nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, 1, 1, ref leaves, out _, out _); return; } - var centroidSpan = centroidMax - centroidMin; + var centroidSpan = centroidMax - centroidMin; + var axisIsDegenerate = Vector128.LessThanOrEqual(centroidSpan.AsVector128(), Vector128.Create(1e-12f)); + if ((Vector128.ExtractMostSignificantBits(axisIsDegenerate) & 0b111) == 0b111) + { + //Looks like all the centroids are in the same spot; there's no meaningful way to split this. + HandleMicrosweepDegeneracy(ref leaves, subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, centroidMin, centroidMax, context, workerIndex); + return; + } + context->Threading.GetBins(workerIndex, out var binBoundingBoxes, out var binCentroidBoundingBoxes, out var binBoundingBoxesScan, out var binCentroidBoundingBoxesScan, out var binLeafCounts); if (Vector256.IsHardwareAccelerated || Vector128.IsHardwareAccelerated) @@ -288,7 +296,7 @@ static unsafe void MicroSweepForBinnedBuilder( if (bestLeafCountB == 0 || bestLeafCountB == totalLeafCount || bestSAH == float.MaxValue || float.IsNaN(bestSAH) || float.IsInfinity(bestSAH)) { //Some form of major problem detected! Fall back to a degenerate split. - HandleMicrosweepDegeneracy(ref leaves, subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, context, workerIndex); + HandleMicrosweepDegeneracy(ref leaves, subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, centroidMin, centroidMax, context, workerIndex); return; } @@ -960,13 +968,14 @@ unsafe static void BinnedBuilderNodeWorker(long taskId, voi private static unsafe void BuildNodeForDegeneracy( Buffer subtrees, Buffer nodes, Buffer metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, - Context* context, out int subtreeCountA, out int subtreeCountB, out BoundingBox4 boundsA, out BoundingBox4 boundsB, out int aIndex, out int bIndex) + Context* context, out int subtreeCountA, out int subtreeCountB, out int aIndex, out int bIndex) where TLeaves : unmanaged where TThreading : unmanaged, IBinnedBuilderThreading { //This shouldn't happen unless something is badly wrong with the input; no point in optimizing it. subtreeCountA = subtrees.Length / 2; subtreeCountB = subtrees.Length - subtreeCountA; + BoundingBox4 boundsA, boundsB; boundsA.Min = new Vector4(float.MaxValue); boundsA.Max = new Vector4(float.MinValue); boundsB.Min = new Vector4(float.MaxValue); @@ -991,29 +1000,33 @@ private static unsafe void BuildNodeForDegeneracy( //Note that we just use the bounds as centroid bounds. This is a degenerate situation anyway. BuildNode(boundsA, boundsB, leafCountA, leafCountB, subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, subtreeCountA, subtreeCountB, ref context->Leaves, out aIndex, out bIndex); } + + // Note that degenerate nodes are those which are assumed to have zero-sized centroid bound spans. + // There are other possibilities--NaNs, infinities--which also flow into this, but the usual case is overlapping geometry. + // We pass this along recursively to trigger the degeneracy case at each level. static unsafe void HandleDegeneracy(Buffer subtrees, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, bool usePongBuffer, int subtreeRegionStartIndex, int nodeIndex, int subtreeCount, int parentNodeIndex, int childIndexInParent, BoundingBox4 centroidBounds, Context* context, int workerIndex, IThreadDispatcher dispatcher) where TLeaves : unmanaged where TThreading : unmanaged, IBinnedBuilderThreading { - BuildNodeForDegeneracy(subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, context, out var subtreeCountA, out var subtreeCountB, out var boundsA, out var boundsB, out var aIndex, out var bIndex); + BuildNodeForDegeneracy(subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, context, out var subtreeCountA, out var subtreeCountB, out var aIndex, out var bIndex); if (subtreeCountA > 1) - BinnedBuildNode(usePongBuffer, subtreeRegionStartIndex, aIndex, subtreeCountA, nodeIndex, 0, boundsA, context, workerIndex, dispatcher); + BinnedBuildNode(usePongBuffer, subtreeRegionStartIndex, aIndex, subtreeCountA, nodeIndex, 0, centroidBounds, context, workerIndex, dispatcher); if (subtreeCountB > 1) - BinnedBuildNode(usePongBuffer, subtreeRegionStartIndex + subtreeCountA, bIndex, subtreeCountB, nodeIndex, 1, boundsB, context, workerIndex, dispatcher); + BinnedBuildNode(usePongBuffer, subtreeRegionStartIndex + subtreeCountA, bIndex, subtreeCountB, nodeIndex, 1, centroidBounds, context, workerIndex, dispatcher); } static unsafe void HandleMicrosweepDegeneracy(ref TLeaves leaves, - Buffer subtrees, Buffer nodes, Buffer metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, Context* context, int workerIndex) + Buffer subtrees, Buffer nodes, Buffer metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, Vector4 centroidMin, Vector4 centroidMax, Context* context, int workerIndex) where TLeaves : unmanaged where TThreading : unmanaged, IBinnedBuilderThreading { - BuildNodeForDegeneracy(subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, context, out var subtreeCountA, out var subtreeCountB, out var boundsA, out var boundsB, out var aIndex, out var bIndex); + BuildNodeForDegeneracy(subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, context, out var subtreeCountA, out var subtreeCountB, out var aIndex, out var bIndex); if (subtreeCountA > 1) - MicroSweepForBinnedBuilder(boundsA.Min, boundsA.Max, ref leaves, subtrees.Slice(subtreeCountA), nodes.Slice(1, subtreeCountA - 1), metanodes.Allocated ? metanodes.Slice(1, subtreeCountA - 1) : metanodes, aIndex, nodeIndex, 0, context, workerIndex); + MicroSweepForBinnedBuilder(centroidMin, centroidMax, ref leaves, subtrees.Slice(subtreeCountA), nodes.Slice(1, subtreeCountA - 1), metanodes.Allocated ? metanodes.Slice(1, subtreeCountA - 1) : metanodes, aIndex, nodeIndex, 0, context, workerIndex); if (subtreeCountB > 1) - MicroSweepForBinnedBuilder(boundsB.Max, boundsB.Max, ref leaves, subtrees.Slice(subtreeCountA, subtreeCountB), nodes.Slice(subtreeCountA, subtreeCountB - 1), metanodes.Allocated ? metanodes.Slice(subtreeCountA, subtreeCountB - 1) : metanodes, bIndex, nodeIndex, 1, context, workerIndex); + MicroSweepForBinnedBuilder(centroidMin, centroidMax, ref leaves, subtrees.Slice(subtreeCountA, subtreeCountB), nodes.Slice(subtreeCountA, subtreeCountB - 1), metanodes.Allocated ? metanodes.Slice(subtreeCountA, subtreeCountB - 1) : metanodes, bIndex, nodeIndex, 1, context, workerIndex); } diff --git a/BepuPhysics/Trees/Tree_Refine2.cs b/BepuPhysics/Trees/Tree_Refine2.cs index b12841a17..e390e4ac0 100644 --- a/BepuPhysics/Trees/Tree_Refine2.cs +++ b/BepuPhysics/Trees/Tree_Refine2.cs @@ -461,21 +461,10 @@ private void TryPushChildForRootRefinement2( } else { - //A regular internal node; push it. - //TODO: The heuristic for cost is pretty simple: the bounds metric of the child in isolation. - //The root collection will always visit the *biggest* nodes. That's often a good idea, but not always. - //A couple of potential improvements to consider: - //1. Use boundsMetric * leafCount. This will tend to push nodes with lots of leaves to the top of the heap. - //2. Dive one step deeper and compute the ratio of the bounds metric of the children to the parent. Nodes that do a poor job of splitting the space will tend to have a higher ratio. - //#1 is trivial. #2 requires touching a little more memory. Worth investigating. - //(Just doing this for now to match the old implementation.) - //var childBoundsMetric = ComputeBoundsMetric(Unsafe.As(ref child)); - //ref var childNode = ref Nodes[child.Index]; - //var grandchildABoundsMetric = ComputeBoundsMetric(Unsafe.As(ref childNode.A)); - //var grandchildBBoundsMetric = ComputeBoundsMetric(Unsafe.As(ref childNode.B)); - //var cost = (grandchildABoundsMetric * childNode.A.LeafCount + grandchildBBoundsMetric * childNode.B.LeafCount); - //heap.Insert(child.Index, cost); - heap.Insert(child.Index, ComputeBoundsMetric(Unsafe.As(ref child))); + // A regular internal node; push it. + // The heuristic for cost is pretty simple: bigger nodes with more children are more profitable targets for optimization in expectation. + // Note that we don't *always* use priority queue driven refinement; that helps avoid pathologies that would otherwise exploit the heuristic. + heap.Insert(child.Index, ComputeBoundsMetric(Unsafe.As(ref child)) * child.LeafCount); } } } diff --git a/Demos/Demo.cs b/Demos/Demo.cs index 697a85160..329db2071 100644 --- a/Demos/Demo.cs +++ b/Demos/Demo.cs @@ -62,7 +62,7 @@ public virtual void Update(Window window, Camera camera, Input input, float dt) //fully decouple simulation and rendering rates across different threads. //(In either case, you'd also want to interpolate or extrapolate simulation results during rendering for smoothness.) //Note that taking steps of variable length can reduce stability. Gradual or one-off changes can work reasonably well. - Simulation.Timestep(TimestepDuration);//, ThreadDispatcher); + Simulation.Timestep(TimestepDuration, ThreadDispatcher); ////Here's an example of how it would look to use more frequent updates, but still with a fixed amount of time simulated per update call: //const float timeToSimulate = 1 / 60f; diff --git a/Demos/DemoSet.cs b/Demos/DemoSet.cs index 378ee6df3..e2b78ac67 100644 --- a/Demos/DemoSet.cs +++ b/Demos/DemoSet.cs @@ -44,7 +44,6 @@ struct Option public DemoSet() { - AddOption(); AddOption(); AddOption(); AddOption(); diff --git a/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs b/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs index a7ff2ee2c..fccb45b96 100644 --- a/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs +++ b/Demos/SpecializedTests/BroadPhaseStressTestDemo.cs @@ -1,14 +1,15 @@ -using BepuUtilities; +using BepuPhysics; +using BepuPhysics.Collidables; +using BepuPhysics.CollisionDetection; +using BepuPhysics.Trees; +using BepuUtilities; +using DemoContentLoader; using DemoRenderer; +using DemoRenderer.Constraints; +using DemoRenderer.UI; using DemoUtilities; -using BepuPhysics; -using BepuPhysics.Collidables; using System; using System.Numerics; -using System.Diagnostics; -using DemoContentLoader; -using BepuPhysics.CollisionDetection; -using BepuPhysics.Trees; using static Demos.SpecializedTests.TreeFiddlingTestDemo; namespace Demos.SpecializedTests; @@ -65,9 +66,9 @@ public override void Initialize(ContentArchive content, Camera camera) var shape = new Sphere(0.5f); var sphereInertia = shape.ComputeInertia(1); var shapeIndex = Simulation.Shapes.Add(shape); - const int width = 128; + const int width = 2048; const int height = 2; - const int length = 128; + const int length = 2048; var spacing = new Vector3(16.01f); float randomization = 0.9f; var randomizationSpan = (spacing - new Vector3(1)) * randomization; @@ -81,19 +82,19 @@ public override void Initialize(ContentArchive content, Camera camera) { var r = new Vector3(random.NextSingle(), random.NextSingle(), random.NextSingle()); //var location = spacing * (new Vector3(i, j, k) + new Vector3(-width, 1, -length)) + randomizationBase + r * randomizationSpan; - //var location = (r - new Vector3(0.5f)) * (r - new Vector3(0.5f)) * spacing * new Vector3(width, height, length); + var location = (r - new Vector3(0.5f)) * (r - new Vector3(0.5f)) * spacing * new Vector3(width, height, length); //var location = (r - new Vector3(0.5f)) * spacing * new Vector3(width, height, length); - var location = new Vector3(15, 15, 15); + //var location = new Vector3(15, 15, 15); //var hash = HashHelper.Rehash(HashHelper.Rehash(HashHelper.Rehash(i) + HashHelper.Rehash(j)) + HashHelper.Rehash(k)); var hash = i + j + k; - //if (hash % 64 == 0) - //{ - // if (i == 7 && j == 1 && k == 0) - // Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(100, 0, 100), sphereInertia, Simulation.Shapes.Add(new Sphere(100)), -1)); - // else - // Simulation.Bodies.Add(BodyDescription.CreateDynamic(location, sphereInertia, shapeIndex, -1)); - //} - //else + if (hash % 64 == 0) + { + if (i == 7 && j == 1 && k == 0) + Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(100, 0, 100), sphereInertia, Simulation.Shapes.Add(new Sphere(100)), -1)); + else + Simulation.Bodies.Add(BodyDescription.CreateDynamic(location, sphereInertia, shapeIndex, -1)); + } + else { Simulation.Statics.Add(new StaticDescription(location, shapeIndex)); } @@ -102,7 +103,7 @@ public override void Initialize(ContentArchive content, Camera camera) } Console.WriteLine($"Body count: {Simulation.Bodies.ActiveSet.Count}"); Console.WriteLine($"Static count: {Simulation.Statics.Count}"); - //groundStatic = Simulation.Statics.Add(new StaticDescription(new Vector3(0, -10, 0), Simulation.Shapes.Add(new Box(5000, 1, 5000)))); + groundStatic = Simulation.Statics.Add(new StaticDescription(new Vector3(0, -10, 0), Simulation.Shapes.Add(new Box(5000, 1, 5000)))); startingLocations = new Vector3[Simulation.Bodies.ActiveSet.Count]; for (int i = 0; i < startingLocations.Length; ++i) { @@ -122,70 +123,70 @@ public override void Initialize(ContentArchive content, Camera camera) TimingsRingBuffer intertreeTest2Times; long frameCount; - void PrintPathToRoot(StaticHandle handle) - { - var index = Simulation.Statics[handle].Static.BroadPhaseIndex; - var leaf = Simulation.BroadPhase.StaticTree.Leaves[index]; - int depth = 0; - var nodeIndex = leaf.NodeIndex; - Console.Write($"Starting from {leaf.NodeIndex}:{leaf.ChildIndex}, path: "); - while (true) - { - ref var node = ref Simulation.BroadPhase.StaticTree.Metanodes[nodeIndex]; - Console.Write($"{nodeIndex}, "); - if (node.Parent >= 0) - { - nodeIndex = node.Parent; - depth++; - } - else - break; - } - Console.WriteLine($"; depth {depth}."); - - } + void PrintPathToRoot(StaticHandle handle) + { + var index = Simulation.Statics[handle].Static.BroadPhaseIndex; + var leaf = Simulation.BroadPhase.StaticTree.Leaves[index]; + int depth = 0; + var nodeIndex = leaf.NodeIndex; + Console.Write($"Starting from {leaf.NodeIndex}:{leaf.ChildIndex}, path: "); + while (true) + { + ref var node = ref Simulation.BroadPhase.StaticTree.Metanodes[nodeIndex]; + Console.Write($"{nodeIndex}, "); + if (node.Parent >= 0) + { + nodeIndex = node.Parent; + depth++; + } + else + break; + } + Console.WriteLine($"; depth {depth}."); + + } public override void Update(Window window, Camera camera, Input input, float dt) { var rotationAngle = frameCount * 1e-3f; - var rotation = Matrix3x3.CreateFromAxisAngle(Vector3.UnitY, rotationAngle); - //for (int i = 0; i < Simulation.Bodies.ActiveSet.Count / 2; ++i) - //{ - // //For every body, set the velocity such that body moves toward some body-specific goal state that evolves over time. - // Matrix3x3.Transform(startingLocations[i], rotation, out var targetLocation); - // ref var motion = ref Simulation.Bodies.ActiveSet.DynamicsState[i].Motion; - // var offset = targetLocation - motion.Pose.Position; - // motion.Velocity.Linear = offset; - //} - - //if (frameCount % 32 == 0) - //PrintPathToRoot(groundStatic); - - //Simulation.BroadPhase.ActiveTree.CacheOptimize(0); - //Simulation.BroadPhase.StaticTree.CacheOptimize(0); - base.Update(window, camera, input, dt); - updateTimes.Add(Simulation.Profiler[Simulation.BroadPhase]); - testTimes.Add(Simulation.Profiler[Simulation.BroadPhaseOverlapFinder]); + var rotation = Matrix3x3.CreateFromAxisAngle(Vector3.UnitY, rotationAngle); + //for (int i = 0; i < Simulation.Bodies.ActiveSet.Count / 2; ++i) + //{ + // //For every body, set the velocity such that body moves toward some body-specific goal state that evolves over time. + // Matrix3x3.Transform(startingLocations[i], rotation, out var targetLocation); + // ref var motion = ref Simulation.Bodies.ActiveSet.DynamicsState[i].Motion; + // var offset = targetLocation - motion.Pose.Position; + // motion.Velocity.Linear = offset; + //} - //var overlaps = new OverlapHandler(); - //Simulation.BroadPhase.ActiveTree.GetSelfOverlaps2(ref overlaps); - //var a = Stopwatch.GetTimestamp(); - //var threadedOverlaps = new TreeFiddlingTestDemo.ThreadedOverlapHandler(BufferPool, ThreadDispatcher.ThreadCount); - //Simulation.BroadPhase.ActiveTree.GetSelfOverlaps2(ref threadedOverlaps, BufferPool, ThreadDispatcher); - //var (selfOverlapCount, _) = threadedOverlaps.SumResults(); - //threadedOverlaps.Reset(); - //var b = Stopwatch.GetTimestamp(); - //Simulation.BroadPhase.ActiveTree.GetOverlaps2(ref Simulation.BroadPhase.ActiveTree, ref threadedOverlaps, BufferPool, ThreadDispatcher); - //var c = Stopwatch.GetTimestamp(); - //var interOverlaps = new OverlapHandler(); - ////Simulation.BroadPhase.ActiveTree.GetOverlaps(ref Simulation.BroadPhase.ActiveTree, ref interOverlaps); - //var (interOverlapCount, _) = threadedOverlaps.SumResults(); - //test2Times.Add((b - a) / (double)Stopwatch.Frequency); - //intertreeTest2Times.Add((c - b) / (double)Stopwatch.Frequency); + //if (frameCount % 32 == 0) + //PrintPathToRoot(groundStatic); + //Simulation.BroadPhase.ActiveTree.CacheOptimize(0); + //Simulation.BroadPhase.StaticTree.CacheOptimize(0); + base.Update(window, camera, input, dt); + updateTimes.Add(Simulation.Profiler[Simulation.BroadPhase]); + testTimes.Add(Simulation.Profiler[Simulation.BroadPhaseOverlapFinder]); + + //var overlaps = new OverlapHandler(); + //Simulation.BroadPhase.ActiveTree.GetSelfOverlaps2(ref overlaps); + //var a = Stopwatch.GetTimestamp(); + //var threadedOverlaps = new TreeFiddlingTestDemo.ThreadedOverlapHandler(BufferPool, ThreadDispatcher.ThreadCount); + //Simulation.BroadPhase.ActiveTree.GetSelfOverlaps2(ref threadedOverlaps, BufferPool, ThreadDispatcher); + //var (selfOverlapCount, _) = threadedOverlaps.SumResults(); + //threadedOverlaps.Reset(); + //var b = Stopwatch.GetTimestamp(); + //Simulation.BroadPhase.ActiveTree.GetOverlaps2(ref Simulation.BroadPhase.ActiveTree, ref threadedOverlaps, BufferPool, ThreadDispatcher); + //var c = Stopwatch.GetTimestamp(); + //var interOverlaps = new OverlapHandler(); + ////Simulation.BroadPhase.ActiveTree.GetOverlaps(ref Simulation.BroadPhase.ActiveTree, ref interOverlaps); + //var (interOverlapCount, _) = threadedOverlaps.SumResults(); + //test2Times.Add((b - a) / (double)Stopwatch.Frequency); + //intertreeTest2Times.Add((c - b) / (double)Stopwatch.Frequency); + if (frameCount++ % sampleCount == 0) { - Console.WriteLine($"Active Depth {frameCount}: {Simulation.BroadPhase.ActiveTree.ComputeMaximumDepth()}"); + Console.WriteLine($"Active Depth {frameCount}: {Simulation.BroadPhase.ActiveTree.ComputeMaximumDepth()}"); Console.WriteLine($"Static Depth {frameCount}: {Simulation.BroadPhase.StaticTree.ComputeMaximumDepth()}"); var updateStats = updateTimes.ComputeStats(); var testStats = testTimes.ComputeStats(); @@ -215,4 +216,78 @@ public override void Update(Window window, Camera camera, Input input, float dt) //threadedOverlaps.Dispose(BufferPool); } + // Claude did an okay job of this visualization, I'd say. + public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + { + base.Render(renderer, camera, input, text, font); + + //VisualizeTreeTopology(renderer, camera, text, font, ref Simulation.BroadPhase.StaticTree); + } + + void VisualizeTreeTopology(Renderer renderer, Camera camera, TextBuilder text, Font font, ref Tree tree) + { + if (tree.LeafCount > 0) + { + const float levelHeight = 3f; + const float nodeSpacing = 20f; + + // Start visualization from the root (node 0) at origin + RenderNodeRecursive(renderer, camera, text, font, ref tree, 0, 0, 0f, levelHeight, nodeSpacing); + } + } + + void RenderNodeRecursive(Renderer renderer, Camera camera, TextBuilder text, Font font, ref Tree tree, int nodeIndex, int depth, float horizontalOffset, float levelHeight, float nodeSpacing) + { + var nodePosition = new Vector3(horizontalOffset, depth * levelHeight, 0); + + var nodeColor = new Vector3(0.8f, 0.2f, 0.2f); + renderer.Shapes.AddShape(new Sphere(0.3f), null, nodePosition, nodeColor); + + if (DemoRenderer.Helpers.GetScreenLocation(nodePosition, camera.ViewProjection, renderer.Surface.Resolution, out var location)) + { + renderer.TextBatcher.Write(text.Clear().Append(nodeIndex), location, 10, new Vector3(1), font); + } + + ref var node = ref tree.Nodes[nodeIndex]; + + // Calculate child spacing (gets tighter with depth) + float childSpacing = nodeSpacing / float.Pow(1.7f, depth); + float leftOffset = horizontalOffset - childSpacing; + float rightOffset = horizontalOffset + childSpacing; + + // Process child A (left) + var childAPosition = new Vector3(leftOffset, (depth + 1) * levelHeight, 0); + if (node.A.Index >= 0) + { + // Internal node - recurse + renderer.Lines.Allocate() = new LineInstance(nodePosition, childAPosition, new Vector3(0.8f, 0.8f, 0.8f), default); + RenderNodeRecursive(renderer, camera, text, font, ref tree, node.A.Index, depth + 1, leftOffset, levelHeight, nodeSpacing); + } + else + { + // Leaf node + renderer.Lines.Allocate() = new LineInstance(nodePosition, childAPosition, new Vector3(0.2f, 0.8f, 0.2f), default); + renderer.Shapes.AddShape(new Sphere(0.15f), null, childAPosition, new Vector3(0.2f, 1f, 0.2f)); + if (DemoRenderer.Helpers.GetScreenLocation(childAPosition, camera.ViewProjection, renderer.Surface.Resolution, out var childLocation)) + renderer.TextBatcher.Write(text.Clear().Append(Tree.Encode(node.A.Index)), childLocation, 10, new Vector3(1), font); + } + + // Process child B (right) + var childBPosition = new Vector3(rightOffset, (depth + 1) * levelHeight, 0); + if (node.B.Index >= 0) + { + // Internal node - recurse + renderer.Lines.Allocate() = new LineInstance(nodePosition, childBPosition, new Vector3(0.8f, 0.8f, 0.8f), default); + RenderNodeRecursive(renderer, camera, text, font, ref tree, node.B.Index, depth + 1, rightOffset, levelHeight, nodeSpacing); + } + else + { + // Leaf node + renderer.Lines.Allocate() = new LineInstance(nodePosition, childBPosition, new Vector3(0.2f, 0.8f, 0.2f), default); + renderer.Shapes.AddShape(new Sphere(0.15f), null, childBPosition, new Vector3(0.2f, 1f, 0.2f)); + if (DemoRenderer.Helpers.GetScreenLocation(childBPosition, camera.ViewProjection, renderer.Surface.Resolution, out var childLocation)) + renderer.TextBatcher.Write(text.Clear().Append(Tree.Encode(node.B.Index)), childLocation, 10, new Vector3(1), font); + } + } + } From 1d1aead82c493c22793bc02a233a6dcd08b57bd6 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 21 Jun 2025 18:53:27 -0500 Subject: [PATCH 928/947] Oops, cleanup. --- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 10 +--------- 1 file changed, 1 insertion(+), 9 deletions(-) diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 67b5f98c9..286361500 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -1172,14 +1172,10 @@ static unsafe void BinnedBuildNode( bestCentroidBoundingBoxB = accumulatedCentroidBoundingBoxB; int accumulatedLeafCountB = binLeafCounts[lastBinIndex]; int bestLeafCountB = 0; - float minCandidate = float.MaxValue; - float maxCandidate = float.MinValue; for (int splitIndexCandidate = lastBinIndex; splitIndexCandidate >= 1; --splitIndexCandidate) { var previousIndex = splitIndexCandidate - 1; var sahCandidate = ComputeBoundsMetric(binBoundingBoxesScan[previousIndex]) * (totalLeafCount - accumulatedLeafCountB) + ComputeBoundsMetric(accumulatedBoundingBoxB) * accumulatedLeafCountB; - minCandidate = float.Min(minCandidate, sahCandidate); - maxCandidate = float.Max(maxCandidate, sahCandidate); if (sahCandidate < bestSAH) { bestSAH = sahCandidate; @@ -1195,11 +1191,7 @@ static unsafe void BinnedBuildNode( accumulatedCentroidBoundingBoxB.Min = Vector4.Min(centroidBoundsForBin.Min, accumulatedCentroidBoundingBoxB.Min); accumulatedCentroidBoundingBoxB.Max = Vector4.Max(centroidBoundsForBin.Max, accumulatedCentroidBoundingBoxB.Max); accumulatedLeafCountB += binLeafCounts[previousIndex]; - } - //if (minCandidate == maxCandidate) - //{ - // Console.WriteLine("asdfh"); - //} + } if (bestLeafCountB == 0 || bestLeafCountB == totalLeafCount || bestSAH == float.MaxValue || float.IsNaN(bestSAH) || float.IsInfinity(bestSAH)) { //Some form of major problem detected! Fall back to a degenerate split. From 4f257d4c6c07f561055438cb8974d5ecd71096f6 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 12 Jul 2025 19:04:09 -0500 Subject: [PATCH 929/947] Fixes https://github.com/bepu/bepuphysics2/issues/376. --- Demos/Demos/NewtDemo.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Demos/Demos/NewtDemo.cs b/Demos/Demos/NewtDemo.cs index 6e19470c8..c8721721f 100644 --- a/Demos/Demos/NewtDemo.cs +++ b/Demos/Demos/NewtDemo.cs @@ -250,7 +250,7 @@ public bool Equals(ref Cell a, ref Cell b) [MethodImpl(MethodImplOptions.AggressiveInlining)] public int Hash(ref Cell cell) { - return (452930477 * cell.X) ^ (122949829 * cell.Z) ^ (654188429 * cell.Z); + return (452930477 * cell.X) ^ (122949829 * cell.Y) ^ (654188429 * cell.Z); } } From f73cd4ce3e740cca7dd1a96fc5690484d88e6f38 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 24 Aug 2025 22:55:42 -0500 Subject: [PATCH 930/947] Improved insertion/rotation heuristics (oops missing counts). No more top down rotation, not cheap enough to warrant its existence given lower quality. --- BepuPhysics/Trees/Tree_Add.cs | 60 +- BepuPhysics/Trees/Tree_BinnedBuilder.cs | 2724 ++++++++++++----------- 2 files changed, 1387 insertions(+), 1397 deletions(-) diff --git a/BepuPhysics/Trees/Tree_Add.cs b/BepuPhysics/Trees/Tree_Add.cs index e85bcb5e2..6ea6fef77 100644 --- a/BepuPhysics/Trees/Tree_Add.cs +++ b/BepuPhysics/Trees/Tree_Add.cs @@ -8,7 +8,6 @@ namespace BepuPhysics.Trees; partial struct Tree { private struct InsertShouldNotRotate { } - private struct InsertShouldRotateTopDown { } private struct InsertShouldRotateBottomUp { } /// @@ -32,23 +31,10 @@ public int AddWithoutRefinement(BoundingBox bounds, BufferPool pool) /// Extents of the leaf bounds. /// Resource pool to use if resizing is required. /// Index of the leaf allocated in the tree's leaf array. - /// Performs incrementally refining tree rotations down along the insertion path, unlike . - /// For a given tree, this is slightly slower than and slightly faster than . - /// Trees built with repeated insertions of this kind tend to have decent quality, but slightly worse than . + /// Performs incrementally refining tree rotations when returning along the insertion path, unlike . + /// This is about twice the cost of (outside of pathological cases). + /// Trees built with repeated insertions of this kind tend to have better quality and fewer pathological cases compared to . public int Add(BoundingBox bounds, BufferPool pool) - { - return Add(bounds, pool); - } - - /// - /// Adds a leaf to the tree with the given bounding box and returns the index of the added leaf. - /// - /// Extents of the leaf bounds. - /// Resource pool to use if resizing is required. - /// Index of the leaf allocated in the tree's leaf array. - /// Performs incrementally refining tree rotations up along the insertion path, unlike . - /// Trees built with repeated insertions of this kind tend to have slightly better quality than , but it is also slightly more expensive. - public int AddWithBottomUpRefinement(BoundingBox bounds, BufferPool pool) { return Add(bounds, pool); } @@ -83,18 +69,13 @@ private int Add(BoundingBox bounds, BufferPool pool) where TShoul var newLeafIndex = AddLeaf(newNodeIndex, 0); while (true) { - //Note: rotating from the top down produces a tree that's lower quality that rotating from the bottom up. - //In context, that's fine; insertion just needs to produce a tree that isn't megatrash/stackoverflowy, and refinement will take care of the rest. - //The advantage is that top down is a little faster. - if (typeof(TShouldRotate) == typeof(InsertShouldRotateTopDown)) - TryRotateNode(nodeIndex); ref var node = ref Nodes[nodeIndex]; - //Choose whichever child requires less bounds expansion. If they're tied, choose the one with the least leaf count. + // Choose whichever child increases the lost cost estimate less. If they're tied, choose the one with the least leaf count. BoundingBox.CreateMergedUnsafe(bounds4, node.A, out var mergedA); BoundingBox.CreateMergedUnsafe(bounds4, node.B, out var mergedB); - var boundsIncreaseA = ComputeBoundsMetric(mergedA) - ComputeBoundsMetric(Unsafe.As(ref node.A)); - var boundsIncreaseB = ComputeBoundsMetric(mergedB) - ComputeBoundsMetric(Unsafe.As(ref node.B)); - var useA = boundsIncreaseA == boundsIncreaseB ? node.A.LeafCount < node.B.LeafCount : boundsIncreaseA < boundsIncreaseB; + var costIncreaseA = ComputeBoundsMetric(mergedA) * (node.A.LeafCount + 1) - EstimateCost(node.A); + var costIncreaseB = ComputeBoundsMetric(mergedB) * (node.B.LeafCount + 1) - EstimateCost(node.B); + var useA = costIncreaseA == costIncreaseB ? node.A.LeafCount < node.B.LeafCount : costIncreaseA < costIncreaseB; ref var merged = ref Unsafe.As(ref useA ? ref mergedA : ref mergedB); ref var chosenChild = ref useA ? ref node.A : ref node.B; if (chosenChild.LeafCount == 1) @@ -146,8 +127,9 @@ private int Add(BoundingBox bounds, BufferPool pool) where TShoul private void TryRotateNode(int rotationRootIndex) { ref var root = ref Nodes[rotationRootIndex]; - var costA = ComputeBoundsMetric(Unsafe.As(ref root.A)); - var costB = ComputeBoundsMetric(Unsafe.As(ref root.B)); + var costA = EstimateCost(root.A); + var costB = EstimateCost(root.B); + var originalCost = costA + costB; float leftRotationCostChange = 0; bool leftUsesA = false; float rightRotationCostChange = 0; @@ -158,21 +140,25 @@ private void TryRotateNode(int rotationRootIndex) ref var a = ref Nodes[root.A.Index]; BoundingBox.CreateMergedUnsafe(a.A, root.B, out var aaB); BoundingBox.CreateMergedUnsafe(a.B, root.B, out var abB); - var costAAB = ComputeBoundsMetric(Unsafe.As(ref aaB)); - var costABB = ComputeBoundsMetric(Unsafe.As(ref abB)); + var costAA = EstimateCost(a.A); + var costAB = EstimateCost(a.B); + var costAAB = ComputeBoundsMetric(Unsafe.As(ref aaB)) * (a.A.LeafCount + root.B.LeafCount) + costAB; + var costABB = ComputeBoundsMetric(Unsafe.As(ref abB)) * (a.B.LeafCount + root.B.LeafCount) + costAA; rightUsesA = costAAB < costABB; - rightRotationCostChange = float.Min(costAAB, costABB) - costA; + rightRotationCostChange = float.Min(costAAB, costABB) - originalCost; } if (root.B.Index >= 0) { //Try a left rotation. root.A will merge with the better of B's children, while the worse of B's children will take the place of root.B. ref var b = ref Nodes[root.B.Index]; - BoundingBox.CreateMergedUnsafe(root.A, b.A, out var baB); - BoundingBox.CreateMergedUnsafe(root.A, b.B, out var bbB); - var costBAB = ComputeBoundsMetric(Unsafe.As(ref baB)); - var costBBB = ComputeBoundsMetric(Unsafe.As(ref bbB)); - leftUsesA = costBAB < costBBB; - leftRotationCostChange = float.Min(costBAB, costBBB) - costB; + BoundingBox.CreateMergedUnsafe(b.A, root.A, out var baA); + BoundingBox.CreateMergedUnsafe(b.B, root.A, out var bbA); + var costBA = EstimateCost(b.A); + var costBB = EstimateCost(b.B); + var costBAA = ComputeBoundsMetric(Unsafe.As(ref baA)) * (b.A.LeafCount + root.A.LeafCount) + costBB; + var costBBA = ComputeBoundsMetric(Unsafe.As(ref bbA)) * (b.B.LeafCount + root.A.LeafCount) + costBA; + leftUsesA = costBAA < costBBA; + leftRotationCostChange = float.Min(costBAA, costBBA) - originalCost; } if (float.Min(leftRotationCostChange, rightRotationCostChange) < 0) { diff --git a/BepuPhysics/Trees/Tree_BinnedBuilder.cs b/BepuPhysics/Trees/Tree_BinnedBuilder.cs index 286361500..c72e514cd 100644 --- a/BepuPhysics/Trees/Tree_BinnedBuilder.cs +++ b/BepuPhysics/Trees/Tree_BinnedBuilder.cs @@ -11,1586 +11,1590 @@ using System.Runtime.Intrinsics.X86; using System.Threading; -namespace BepuPhysics.Trees +namespace BepuPhysics.Trees; +partial struct Tree { - partial struct Tree + struct LeavesHandledInPostPass { } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + static void BuildNode( + BoundingBox4 a, BoundingBox4 b, + int leafCountA, int leafCountB, + Buffer subtrees, Buffer nodes, Buffer metanodes, + int nodeIndex, int parentNodeIndex, int childIndexInParent, int subtreeCountA, int subtreeCountB, ref TLeaves leaves, out int aIndex, out int bIndex) + where TLeaves : unmanaged { - struct LeavesHandledInPostPass { } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - static void BuildNode( - BoundingBox4 a, BoundingBox4 b, - int leafCountA, int leafCountB, - Buffer subtrees, Buffer nodes, Buffer metanodes, - int nodeIndex, int parentNodeIndex, int childIndexInParent, int subtreeCountA, int subtreeCountB, ref TLeaves leaves, out int aIndex, out int bIndex) - where TLeaves : unmanaged + Debug.Assert(typeof(TLeaves) == typeof(LeavesHandledInPostPass) || typeof(TLeaves) == typeof(Buffer), "While we didn't bother with an interface here, we assume one of two types only."); + if (metanodes.Allocated) { - Debug.Assert(typeof(TLeaves) == typeof(LeavesHandledInPostPass) || typeof(TLeaves) == typeof(Buffer), "While we didn't bother with an interface here, we assume one of two types only."); - if (metanodes.Allocated) - { - //Note that we touching the metanodes buffer is *conditional*. There won't be any metanodes in the refinement use case, for example, because it has to be handled in a postpass. - ref var metanode = ref metanodes[0]; - metanode.Parent = parentNodeIndex; - metanode.IndexInParent = childIndexInParent; - metanode.RefineFlag = 0; - } - ref var node = ref nodes[0]; - node.A = Unsafe.As(ref a); - node.B = Unsafe.As(ref b); - node.A.LeafCount = leafCountA; - node.B.LeafCount = leafCountB; - if (subtreeCountA == 1) - { - aIndex = subtrees[0].Index; - if (typeof(TLeaves) == typeof(Buffer)) - { - Debug.Assert(leafCountA == 1); - Debug.Assert(aIndex < 0, "During building, any subtreeCount of 1 should imply a leaf."); - //This is a leaf node, and this is a direct builder execution, so write to the leaf data. - Unsafe.As>(ref leaves)[Encode(aIndex)] = new Leaf(nodeIndex, 0); - } - } - else - { - aIndex = nodeIndex + 1; - } - node.A.Index = aIndex; - if (subtreeCountB == 1) + //Note that we touching the metanodes buffer is *conditional*. There won't be any metanodes in the refinement use case, for example, because it has to be handled in a postpass. + ref var metanode = ref metanodes[0]; + metanode.Parent = parentNodeIndex; + metanode.IndexInParent = childIndexInParent; + metanode.RefineFlag = 0; + } + ref var node = ref nodes[0]; + node.A = Unsafe.As(ref a); + node.B = Unsafe.As(ref b); + node.A.LeafCount = leafCountA; + node.B.LeafCount = leafCountB; + if (subtreeCountA == 1) + { + aIndex = subtrees[0].Index; + if (typeof(TLeaves) == typeof(Buffer)) { - bIndex = subtrees[^1].Index; - if (typeof(TLeaves) == typeof(Buffer)) - { - Debug.Assert(leafCountB == 1); - Debug.Assert(bIndex < 0, "During building, any subtreeCount of 1 should imply a leaf."); - //This is a leaf node, and this is a direct builder execution, so write to the leaf data. - Unsafe.As>(ref leaves)[Encode(bIndex)] = new Leaf(nodeIndex, 1); - } + Debug.Assert(leafCountA == 1); + Debug.Assert(aIndex < 0, "During building, any subtreeCount of 1 should imply a leaf."); + //This is a leaf node, and this is a direct builder execution, so write to the leaf data. + Unsafe.As>(ref leaves)[Encode(aIndex)] = new Leaf(nodeIndex, 0); } - else + } + else + { + aIndex = nodeIndex + 1; + } + node.A.Index = aIndex; + if (subtreeCountB == 1) + { + bIndex = subtrees[^1].Index; + if (typeof(TLeaves) == typeof(Buffer)) { - bIndex = nodeIndex + subtreeCountA; //parentNodeIndex + 1 + (subtreeCountA - 1) + Debug.Assert(leafCountB == 1); + Debug.Assert(bIndex < 0, "During building, any subtreeCount of 1 should imply a leaf."); + //This is a leaf node, and this is a direct builder execution, so write to the leaf data. + Unsafe.As>(ref leaves)[Encode(bIndex)] = new Leaf(nodeIndex, 1); } - node.B.Index = bIndex; } - - internal static float ComputeBoundsMetric(BoundingBox4 bounds) => ComputeBoundsMetric(bounds.Min, bounds.Max); - internal static float ComputeBoundsMetric(Vector4 min, Vector4 max) + else { - //Note that we just use the SAH. While we are primarily interested in volume queries for the purposes of collision detection, the topological difference - //between a volume heuristic and surface area heuristic isn't huge. There is, however, one big annoying issue that volume heuristics run into: - //all bounding boxes with one extent equal to zero have zero cost. Surface area approaches avoid this hole simply. - var offset = max - min; - //Note that this is merely proportional to surface area. Being scaled by a constant factor is irrelevant. - return offset.X * offset.Y + offset.Y * offset.Z + offset.Z * offset.X; - + bIndex = nodeIndex + subtreeCountA; //parentNodeIndex + 1 + (subtreeCountA - 1) } + node.B.Index = bIndex; + } + + /// + /// Computes a local cost estimate for a node child using its bounds and leaf count. + /// Handy for + /// + /// Child to estimate the cost of. + /// Estimated cost of the child. + internal static float EstimateCost(NodeChild child) => ComputeBoundsMetric(Unsafe.As(ref child)) * child.LeafCount; + internal static float ComputeBoundsMetric(BoundingBox4 bounds) => ComputeBoundsMetric(bounds.Min, bounds.Max); + internal static float ComputeBoundsMetric(Vector4 min, Vector4 max) + { + //Note that we just use the SAH. While we are primarily interested in volume queries for the purposes of collision detection, the topological difference + //between a volume heuristic and surface area heuristic isn't huge. There is, however, one big annoying issue that volume heuristics run into: + //all bounding boxes with one extent equal to zero have zero cost. Surface area approaches avoid this hole simply. + var offset = max - min; + //Note that this is merely proportional to surface area. Being scaled by a constant factor is irrelevant. + return offset.X * offset.Y + offset.Y * offset.Z + offset.Z * offset.X; + } - interface IBinnedBuilderThreading - { - void GetBins(int workerIndex, - out Buffer binBoundingBoxes, out Buffer binCentroidBoundingBoxes, - out Buffer binBoundingBoxesScan, out Buffer binCentroidBoundingBoxesScan, out Buffer binLeafCounts); - } + interface IBinnedBuilderThreading + { + void GetBins(int workerIndex, + out Buffer binBoundingBoxes, out Buffer binCentroidBoundingBoxes, + out Buffer binBoundingBoxesScan, out Buffer binCentroidBoundingBoxesScan, out Buffer binLeafCounts); + } - struct Context - where TLeaves : unmanaged - where TThreading : unmanaged, IBinnedBuilderThreading - { - public int MinimumBinCount; - public int MaximumBinCount; - public float LeafToBinMultiplier; - public int MicrosweepThreshold; + struct Context + where TLeaves : unmanaged + where TThreading : unmanaged, IBinnedBuilderThreading + { + public int MinimumBinCount; + public int MaximumBinCount; + public float LeafToBinMultiplier; + public int MicrosweepThreshold; - public bool Deterministic; + public bool Deterministic; - public TLeaves Leaves; - public Buffer SubtreesPing; - public Buffer SubtreesPong; - public Buffer Nodes; - public Buffer Metanodes; + public TLeaves Leaves; + public Buffer SubtreesPing; + public Buffer SubtreesPong; + public Buffer Nodes; + public Buffer Metanodes; - public Buffer BinIndices; + public Buffer BinIndices; - public TThreading Threading; + public TThreading Threading; - public Context(int minimumBinCount, int maximumBinCount, float leafToBinMultiplier, int microsweepThreshold, bool deterministic, - Buffer subtreesPing, Buffer subtreesPong, TLeaves leaves, Buffer nodes, Buffer metanodes, Buffer binIndices, TThreading threading) - { - MinimumBinCount = minimumBinCount; - MaximumBinCount = maximumBinCount; - LeafToBinMultiplier = leafToBinMultiplier; - MicrosweepThreshold = microsweepThreshold; - Deterministic = deterministic; - SubtreesPing = subtreesPing; - SubtreesPong = subtreesPong; - BinIndices = binIndices; - Leaves = leaves; - Nodes = nodes; - Metanodes = metanodes; - Threading = threading; - } + public Context(int minimumBinCount, int maximumBinCount, float leafToBinMultiplier, int microsweepThreshold, bool deterministic, + Buffer subtreesPing, Buffer subtreesPong, TLeaves leaves, Buffer nodes, Buffer metanodes, Buffer binIndices, TThreading threading) + { + MinimumBinCount = minimumBinCount; + MaximumBinCount = maximumBinCount; + LeafToBinMultiplier = leafToBinMultiplier; + MicrosweepThreshold = microsweepThreshold; + Deterministic = deterministic; + SubtreesPing = subtreesPing; + SubtreesPong = subtreesPong; + BinIndices = binIndices; + Leaves = leaves; + Nodes = nodes; + Metanodes = metanodes; + Threading = threading; } + } - struct BoundsComparerX : IComparerRef { public int Compare(ref NodeChild a, ref NodeChild b) => (a.Min.X + a.Max.X) > (b.Min.X + b.Max.X) ? -1 : 1; } - struct BoundsComparerY : IComparerRef { public int Compare(ref NodeChild a, ref NodeChild b) => (a.Min.Y + a.Max.Y) > (b.Min.Y + b.Max.Y) ? -1 : 1; } - struct BoundsComparerZ : IComparerRef { public int Compare(ref NodeChild a, ref NodeChild b) => (a.Min.Z + a.Max.Z) > (b.Min.Z + b.Max.Z) ? -1 : 1; } + struct BoundsComparerX : IComparerRef { public int Compare(ref NodeChild a, ref NodeChild b) => (a.Min.X + a.Max.X) > (b.Min.X + b.Max.X) ? -1 : 1; } + struct BoundsComparerY : IComparerRef { public int Compare(ref NodeChild a, ref NodeChild b) => (a.Min.Y + a.Max.Y) > (b.Min.Y + b.Max.Y) ? -1 : 1; } + struct BoundsComparerZ : IComparerRef { public int Compare(ref NodeChild a, ref NodeChild b) => (a.Min.Z + a.Max.Z) > (b.Min.Z + b.Max.Z) ? -1 : 1; } - public struct NodeTimes - { - public double Total; - public double CentroidPrepass; - public double Binning; - public double Partition; - public bool MTPrepass; - public bool MTBinning; - public bool MTPartition; - public int TargetTaskCount; - public int SubtreeCount; - } + public struct NodeTimes + { + public double Total; + public double CentroidPrepass; + public double Binning; + public double Partition; + public bool MTPrepass; + public bool MTBinning; + public bool MTPartition; + public int TargetTaskCount; + public int SubtreeCount; + } - public static NodeTimes[] Times; + public static NodeTimes[] Times; - static unsafe void MicroSweepForBinnedBuilder( - Vector4 centroidMin, Vector4 centroidMax, ref TLeaves leaves, - Buffer subtrees, Buffer nodes, Buffer metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, Context* context, int workerIndex) - where TLeaves : unmanaged where TThreading : unmanaged, IBinnedBuilderThreading + static unsafe void MicroSweepForBinnedBuilder( + Vector4 centroidMin, Vector4 centroidMax, ref TLeaves leaves, + Buffer subtrees, Buffer nodes, Buffer metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, Context* context, int workerIndex) + where TLeaves : unmanaged where TThreading : unmanaged, IBinnedBuilderThreading + { + //This is a very small scale sweep build. + var subtreeCount = subtrees.Length; + if (subtreeCount == 2) { - //This is a very small scale sweep build. - var subtreeCount = subtrees.Length; - if (subtreeCount == 2) - { - ref var subtreeA = ref subtrees[0]; - ref var subtreeB = ref subtrees[1]; - Debug.Assert(parentNodeIndex < 0 || Unsafe.Add(ref context->Nodes[parentNodeIndex].A, childIndexInParent).LeafCount == subtreeA.LeafCount + subtreeB.LeafCount); - BuildNode(Unsafe.As(ref subtreeA), Unsafe.As(ref subtreeB), subtreeA.LeafCount, subtreeB.LeafCount, subtrees, - nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, 1, 1, ref leaves, out _, out _); - return; - } - var centroidSpan = centroidMax - centroidMin; - var axisIsDegenerate = Vector128.LessThanOrEqual(centroidSpan.AsVector128(), Vector128.Create(1e-12f)); - if ((Vector128.ExtractMostSignificantBits(axisIsDegenerate) & 0b111) == 0b111) - { - //Looks like all the centroids are in the same spot; there's no meaningful way to split this. - HandleMicrosweepDegeneracy(ref leaves, subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, centroidMin, centroidMax, context, workerIndex); - return; - } + ref var subtreeA = ref subtrees[0]; + ref var subtreeB = ref subtrees[1]; + Debug.Assert(parentNodeIndex < 0 || Unsafe.Add(ref context->Nodes[parentNodeIndex].A, childIndexInParent).LeafCount == subtreeA.LeafCount + subtreeB.LeafCount); + BuildNode(Unsafe.As(ref subtreeA), Unsafe.As(ref subtreeB), subtreeA.LeafCount, subtreeB.LeafCount, subtrees, + nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, 1, 1, ref leaves, out _, out _); + return; + } + var centroidSpan = centroidMax - centroidMin; + var axisIsDegenerate = Vector128.LessThanOrEqual(centroidSpan.AsVector128(), Vector128.Create(1e-12f)); + if ((Vector128.ExtractMostSignificantBits(axisIsDegenerate) & 0b111) == 0b111) + { + //Looks like all the centroids are in the same spot; there's no meaningful way to split this. + HandleMicrosweepDegeneracy(ref leaves, subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, centroidMin, centroidMax, context, workerIndex); + return; + } - context->Threading.GetBins(workerIndex, out var binBoundingBoxes, out var binCentroidBoundingBoxes, out var binBoundingBoxesScan, out var binCentroidBoundingBoxesScan, out var binLeafCounts); + context->Threading.GetBins(workerIndex, out var binBoundingBoxes, out var binCentroidBoundingBoxes, out var binBoundingBoxesScan, out var binCentroidBoundingBoxesScan, out var binLeafCounts); - if (Vector256.IsHardwareAccelerated || Vector128.IsHardwareAccelerated) - { - //Repurpose the bins memory so we don't need to allocate any extra. The bins aren't in use right now anyway. - int paddedKeyCount = Vector256.IsHardwareAccelerated ? ((subtreeCount + 7) / 8) * 8 : ((subtreeCount + 3) / 4) * 4; + if (Vector256.IsHardwareAccelerated || Vector128.IsHardwareAccelerated) + { + //Repurpose the bins memory so we don't need to allocate any extra. The bins aren't in use right now anyway. + int paddedKeyCount = Vector256.IsHardwareAccelerated ? ((subtreeCount + 7) / 8) * 8 : ((subtreeCount + 3) / 4) * 4; - Debug.Assert(Unsafe.SizeOf() * binBoundingBoxes.Length >= (paddedKeyCount * 2 + subtreeCount) * Unsafe.SizeOf(), - "The bins should preallocate enough space to handle the needs of microsweeps. They reuse the same allocations."); - var keys = new Buffer(binBoundingBoxes.Memory, paddedKeyCount); - var targetIndices = new Buffer(keys.Memory + paddedKeyCount, paddedKeyCount); + Debug.Assert(Unsafe.SizeOf() * binBoundingBoxes.Length >= (paddedKeyCount * 2 + subtreeCount) * Unsafe.SizeOf(), + "The bins should preallocate enough space to handle the needs of microsweeps. They reuse the same allocations."); + var keys = new Buffer(binBoundingBoxes.Memory, paddedKeyCount); + var targetIndices = new Buffer(keys.Memory + paddedKeyCount, paddedKeyCount); - //Compute the axis centroids up front to avoid having to recompute them during a sort. - if (centroidSpan.X > centroidSpan.Y && centroidSpan.X > centroidSpan.Z) - { - for (int i = 0; i < subtreeCount; ++i) - { - ref var bounds = ref subtrees[i]; - keys[i] = bounds.Min.X + bounds.Max.X; - } - } - else if (centroidSpan.Y > centroidSpan.Z) - { - for (int i = 0; i < subtreeCount; ++i) - { - ref var bounds = ref subtrees[i]; - keys[i] = bounds.Min.Y + bounds.Max.Y; - } - } - else - { - for (int i = 0; i < subtreeCount; ++i) - { - ref var bounds = ref subtrees[i]; - keys[i] = bounds.Min.Z + bounds.Max.Z; - } - } - for (int i = subtreeCount; i < paddedKeyCount; ++i) + //Compute the axis centroids up front to avoid having to recompute them during a sort. + if (centroidSpan.X > centroidSpan.Y && centroidSpan.X > centroidSpan.Z) + { + for (int i = 0; i < subtreeCount; ++i) { - keys[i] = float.MaxValue; + ref var bounds = ref subtrees[i]; + keys[i] = bounds.Min.X + bounds.Max.X; } - VectorizedSorts.VectorCountingSort(keys, targetIndices, subtreeCount); - - //Now that we know the target indices, copy things into position. - //Have to copy things into a temporary cache to avoid overwrites since we didn't do any shuffling during the sort. - //Note that we can now reuse the keys memory. - var subtreeCache = binBoundingBoxesScan.As(); - subtrees.CopyTo(0, subtreeCache, 0, subtreeCount); + } + else if (centroidSpan.Y > centroidSpan.Z) + { for (int i = 0; i < subtreeCount; ++i) { - var targetIndex = targetIndices[i]; - subtrees[targetIndex] = subtreeCache[i]; + ref var bounds = ref subtrees[i]; + keys[i] = bounds.Min.Y + bounds.Max.Y; } } else { - //No vectorization supported. Fall back to poopymode! - if (centroidSpan.X > centroidSpan.Y && centroidSpan.X > centroidSpan.Z) - { - var comparer = new BoundsComparerX(); - QuickSort.Sort(ref subtrees[0], 0, subtreeCount - 1, ref comparer); - } - else if (centroidSpan.Y > centroidSpan.Z) - { - var comparer = new BoundsComparerY(); - QuickSort.Sort(ref subtrees[0], 0, subtreeCount - 1, ref comparer); - } - else + for (int i = 0; i < subtreeCount; ++i) { - var comparer = new BoundsComparerZ(); - QuickSort.Sort(ref subtrees[0], 0, subtreeCount - 1, ref comparer); + ref var bounds = ref subtrees[i]; + keys[i] = bounds.Min.Z + bounds.Max.Z; } } - - Debug.Assert(subtreeCount <= context->MaximumBinCount || subtreeCount <= context->MicrosweepThreshold, "We're reusing the bin resources under the assumption that this is only ever called when there are less leaves than maximum bins."); - //Identify the split index by examining the SAH of very split option. - //Premerge from left to right so we have a sorta-summed area table to cheaply look up all possible child A bounds as we scan. - var boundingBoxes = subtrees.As(); - binBoundingBoxesScan[0] = boundingBoxes[0]; - int totalLeafCount = subtrees[0].LeafCount; - for (int i = 1; i < subtreeCount; ++i) + for (int i = subtreeCount; i < paddedKeyCount; ++i) { - var previousIndex = i - 1; - ref var previousScanBounds = ref binBoundingBoxesScan[previousIndex]; - ref var scanBounds = ref binBoundingBoxesScan[i]; - ref var bounds = ref boundingBoxes[i]; - scanBounds.Min = Vector4.Min(bounds.Min, previousScanBounds.Min); - scanBounds.Max = Vector4.Max(bounds.Max, previousScanBounds.Max); - totalLeafCount += subtrees[i].LeafCount; + keys[i] = float.MaxValue; } - - float bestSAH = float.MaxValue; - int bestSplit = 1; - //The split index is going to end up in child B. - var lastSubtreeIndex = subtreeCount - 1; - BoundingBox4 accumulatedBoundingBoxB = boundingBoxes[lastSubtreeIndex]; - Unsafe.SkipInit(out BoundingBox4 bestBoundsB); - int accumulatedLeafCountB = subtrees[lastSubtreeIndex].LeafCount; - int bestLeafCountB = 0; - for (int splitIndexCandidate = lastSubtreeIndex; splitIndexCandidate >= 1; --splitIndexCandidate) + VectorizedSorts.VectorCountingSort(keys, targetIndices, subtreeCount); + + //Now that we know the target indices, copy things into position. + //Have to copy things into a temporary cache to avoid overwrites since we didn't do any shuffling during the sort. + //Note that we can now reuse the keys memory. + var subtreeCache = binBoundingBoxesScan.As(); + subtrees.CopyTo(0, subtreeCache, 0, subtreeCount); + for (int i = 0; i < subtreeCount; ++i) { - var previousIndex = splitIndexCandidate - 1; - var sahCandidate = - ComputeBoundsMetric(binBoundingBoxesScan[previousIndex]) * (totalLeafCount - accumulatedLeafCountB) + - ComputeBoundsMetric(accumulatedBoundingBoxB) * accumulatedLeafCountB; - if (sahCandidate < bestSAH) - { - bestSAH = sahCandidate; - bestSplit = splitIndexCandidate; - bestBoundsB = accumulatedBoundingBoxB; - bestLeafCountB = accumulatedLeafCountB; - } - ref var bounds = ref boundingBoxes[previousIndex]; - accumulatedBoundingBoxB.Min = Vector4.Min(bounds.Min, accumulatedBoundingBoxB.Min); - accumulatedBoundingBoxB.Max = Vector4.Max(bounds.Max, accumulatedBoundingBoxB.Max); - accumulatedLeafCountB += subtrees[previousIndex].LeafCount; + var targetIndex = targetIndices[i]; + subtrees[targetIndex] = subtreeCache[i]; } - if (bestLeafCountB == 0 || bestLeafCountB == totalLeafCount || bestSAH == float.MaxValue || float.IsNaN(bestSAH) || float.IsInfinity(bestSAH)) + } + else + { + //No vectorization supported. Fall back to poopymode! + if (centroidSpan.X > centroidSpan.Y && centroidSpan.X > centroidSpan.Z) { - //Some form of major problem detected! Fall back to a degenerate split. - HandleMicrosweepDegeneracy(ref leaves, subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, centroidMin, centroidMax, context, workerIndex); - return; + var comparer = new BoundsComparerX(); + QuickSort.Sort(ref subtrees[0], 0, subtreeCount - 1, ref comparer); } - - var bestBoundsA = binBoundingBoxesScan[bestSplit - 1]; - var subtreeCountA = bestSplit; - var subtreeCountB = subtreeCount - bestSplit; - var bestLeafCountA = totalLeafCount - bestLeafCountB; - - Debug.Assert(parentNodeIndex < 0 || Unsafe.Add(ref context->Nodes[parentNodeIndex].A, childIndexInParent).LeafCount == bestLeafCountA + bestLeafCountB); - BuildNode(bestBoundsA, bestBoundsB, bestLeafCountA, bestLeafCountB, subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, subtreeCountA, subtreeCountB, ref leaves, out var aIndex, out var bIndex); - if (subtreeCountA > 1) + else if (centroidSpan.Y > centroidSpan.Z) { - var aBounds = boundingBoxes.Slice(subtreeCountA); - var initialCentroid = aBounds.Memory->Min + aBounds.Memory->Max; - BoundingBox4 centroidBoundsA; - centroidBoundsA.Min = initialCentroid; - centroidBoundsA.Max = initialCentroid; - for (int i = 1; i < subtreeCountA; ++i) - { - ref var bounds = ref aBounds[i]; - var centroid = bounds.Min + bounds.Max; - centroidBoundsA.Min = Vector4.Min(centroidBoundsA.Min, centroid); - centroidBoundsA.Max = Vector4.Max(centroidBoundsA.Max, centroid); - } - MicroSweepForBinnedBuilder(centroidBoundsA.Min, centroidBoundsA.Max, ref leaves, subtrees.Slice(subtreeCountA), nodes.Slice(1, subtreeCountA - 1), metanodes.Allocated ? metanodes.Slice(1, subtreeCountA - 1) : metanodes, aIndex, nodeIndex, 0, context, workerIndex); + var comparer = new BoundsComparerY(); + QuickSort.Sort(ref subtrees[0], 0, subtreeCount - 1, ref comparer); } - if (subtreeCountB > 1) + else { - var bBounds = boundingBoxes.Slice(subtreeCountA, subtreeCountB); - var initialCentroid = bBounds.Memory->Min + bBounds.Memory->Max; - BoundingBox4 centroidBoundsB; - centroidBoundsB.Min = initialCentroid; - centroidBoundsB.Max = initialCentroid; - for (int i = 1; i < subtreeCountB; ++i) - { - ref var bounds = ref bBounds[i]; - var centroid = bounds.Min + bounds.Max; - centroidBoundsB.Min = Vector4.Min(centroidBoundsB.Min, centroid); - centroidBoundsB.Max = Vector4.Max(centroidBoundsB.Max, centroid); - } - MicroSweepForBinnedBuilder(centroidBoundsB.Min, centroidBoundsB.Max, ref leaves, subtrees.Slice(subtreeCountA, subtreeCountB), nodes.Slice(subtreeCountA, subtreeCountB - 1), metanodes.Allocated ? metanodes.Slice(subtreeCountA, subtreeCountB - 1) : metanodes, bIndex, nodeIndex, 1, context, workerIndex); + var comparer = new BoundsComparerZ(); + QuickSort.Sort(ref subtrees[0], 0, subtreeCount - 1, ref comparer); } } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - private static int ComputeBinIndex(Vector4 centroidMin, bool useX, bool useY, Vector128 permuteMask, int axisIndex, Vector4 offsetToBinIndex, Vector4 maximumBinIndex, in BoundingBox4 box) + Debug.Assert(subtreeCount <= context->MaximumBinCount || subtreeCount <= context->MicrosweepThreshold, "We're reusing the bin resources under the assumption that this is only ever called when there are less leaves than maximum bins."); + //Identify the split index by examining the SAH of very split option. + //Premerge from left to right so we have a sorta-summed area table to cheaply look up all possible child A bounds as we scan. + var boundingBoxes = subtrees.As(); + binBoundingBoxesScan[0] = boundingBoxes[0]; + int totalLeafCount = subtrees[0].LeafCount; + for (int i = 1; i < subtreeCount; ++i) { - var centroid = box.Min + box.Max; - //Note the clamp against zero as well as maximumBinIndex; going negative *can* happen when the bounding box is corrupted. We'd rather not crash with an access violation. - var binIndicesForLeafContinuous = Vector4.Clamp((centroid - centroidMin) * offsetToBinIndex, Vector4.Zero, maximumBinIndex); - //Note that we don't store out any of the indices into per-bin lists here. We only *really* want two final groups for the children, - //and we can easily compute those by performing another scan. It requires recomputing the bin indices, but that's really not much of a concern. - //To extract the desired lane, we need to use a variable shuffle mask. At the time of writing, the Vector128 cross platform shuffle did not like variable masks. - if (Avx.IsSupported) - return (int)Vector128.ToScalar(Avx.PermuteVar(binIndicesForLeafContinuous.AsVector128(), permuteMask)); - else if (Vector128.IsHardwareAccelerated) - return (int)Vector128.GetElement(binIndicesForLeafContinuous.AsVector128(), axisIndex); - else - return (int)(useX ? binIndicesForLeafContinuous.X : useY ? binIndicesForLeafContinuous.Y : binIndicesForLeafContinuous.Z); + var previousIndex = i - 1; + ref var previousScanBounds = ref binBoundingBoxesScan[previousIndex]; + ref var scanBounds = ref binBoundingBoxesScan[i]; + ref var bounds = ref boundingBoxes[i]; + scanBounds.Min = Vector4.Min(bounds.Min, previousScanBounds.Min); + scanBounds.Max = Vector4.Max(bounds.Max, previousScanBounds.Max); + totalLeafCount += subtrees[i].LeafCount; } - struct SingleThreaded : IBinnedBuilderThreading + float bestSAH = float.MaxValue; + int bestSplit = 1; + //The split index is going to end up in child B. + var lastSubtreeIndex = subtreeCount - 1; + BoundingBox4 accumulatedBoundingBoxB = boundingBoxes[lastSubtreeIndex]; + Unsafe.SkipInit(out BoundingBox4 bestBoundsB); + int accumulatedLeafCountB = subtrees[lastSubtreeIndex].LeafCount; + int bestLeafCountB = 0; + for (int splitIndexCandidate = lastSubtreeIndex; splitIndexCandidate >= 1; --splitIndexCandidate) { - public Buffer BinBoundingBoxes; - public Buffer BinCentroidBoundingBoxes; - public Buffer BinBoundingBoxesScan; - public Buffer BinCentroidBoundingBoxesScan; - public Buffer BinLeafCounts; - - public SingleThreaded(Buffer binAllocationBuffer, int binCapacity) - { - int start = 0; - BinBoundingBoxes = Suballocate(binAllocationBuffer, ref start, binCapacity); - BinCentroidBoundingBoxes = Suballocate(binAllocationBuffer, ref start, binCapacity); - BinBoundingBoxesScan = Suballocate(binAllocationBuffer, ref start, binCapacity); - BinCentroidBoundingBoxesScan = Suballocate(binAllocationBuffer, ref start, binCapacity); - BinLeafCounts = Suballocate(binAllocationBuffer, ref start, binCapacity); - } - - public void GetBins(int workerIndex, - out Buffer binBoundingBoxes, out Buffer binCentroidBoundingBoxes, - out Buffer binBoundingBoxesScan, out Buffer binCentroidBoundingBoxesScan, out Buffer binLeafCounts) + var previousIndex = splitIndexCandidate - 1; + var sahCandidate = + ComputeBoundsMetric(binBoundingBoxesScan[previousIndex]) * (totalLeafCount - accumulatedLeafCountB) + + ComputeBoundsMetric(accumulatedBoundingBoxB) * accumulatedLeafCountB; + if (sahCandidate < bestSAH) { - binBoundingBoxes = BinBoundingBoxes; - binCentroidBoundingBoxes = BinCentroidBoundingBoxes; - binBoundingBoxesScan = BinBoundingBoxesScan; - binCentroidBoundingBoxesScan = BinCentroidBoundingBoxesScan; - binLeafCounts = BinLeafCounts; + bestSAH = sahCandidate; + bestSplit = splitIndexCandidate; + bestBoundsB = accumulatedBoundingBoxB; + bestLeafCountB = accumulatedLeafCountB; } + ref var bounds = ref boundingBoxes[previousIndex]; + accumulatedBoundingBoxB.Min = Vector4.Min(bounds.Min, accumulatedBoundingBoxB.Min); + accumulatedBoundingBoxB.Max = Vector4.Max(bounds.Max, accumulatedBoundingBoxB.Max); + accumulatedLeafCountB += subtrees[previousIndex].LeafCount; } - - static Buffer Suballocate(Buffer buffer, ref int start, int count) where T : unmanaged + if (bestLeafCountB == 0 || bestLeafCountB == totalLeafCount || bestSAH == float.MaxValue || float.IsNaN(bestSAH) || float.IsInfinity(bestSAH)) { - var size = count * Unsafe.SizeOf(); - var previousStart = start; - start += size; - return buffer.Slice(previousStart, size).As(); + //Some form of major problem detected! Fall back to a degenerate split. + HandleMicrosweepDegeneracy(ref leaves, subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, centroidMin, centroidMax, context, workerIndex); + return; } - /// - /// Stores resources required by a worker to dispatch and manage multithreaded work. - /// - /// - /// Some of the resources cached here are technically redundant with the storage used for workers and ends up involving an extra bin scan on a multithreaded test, - /// but the cost associated with doing so is... low. The complexity cost of trying to use the memory allocated for workers is not low. - /// - struct BinnedBuildWorkerContext + var bestBoundsA = binBoundingBoxesScan[bestSplit - 1]; + var subtreeCountA = bestSplit; + var subtreeCountB = subtreeCount - bestSplit; + var bestLeafCountA = totalLeafCount - bestLeafCountB; + + Debug.Assert(parentNodeIndex < 0 || Unsafe.Add(ref context->Nodes[parentNodeIndex].A, childIndexInParent).LeafCount == bestLeafCountA + bestLeafCountB); + BuildNode(bestBoundsA, bestBoundsB, bestLeafCountA, bestLeafCountB, subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, subtreeCountA, subtreeCountB, ref leaves, out var aIndex, out var bIndex); + if (subtreeCountA > 1) { - /// - /// Bins associated with this worker for the duration of a node. This allocation will persist across the build. - /// - public Buffer BinBoundingBoxes; - /// - /// Centroid bound bins associated with this worker for the duration of a node. This allocation will persist across the build. - /// - public Buffer BinCentroidBoundingBoxes; - /// - /// Bins associated with this worker for use in the SAH scan. This allocation will persist across the build. - /// - public Buffer BinBoundingBoxesScan; - /// - /// Centroid bound bins associated with this worker for use in the SAH scan. This allocation will persist across the build. - /// - public Buffer BinCentroidBoundingBoxesScan; - /// - /// Bin leaf counts associated with this worker for the duration of a node. This allocation will persist across the build. - /// - public Buffer BinLeafCounts; - - public BinnedBuildWorkerContext(Buffer binAllocationBuffer, ref int binStart, int binCapacity) + var aBounds = boundingBoxes.Slice(subtreeCountA); + var initialCentroid = aBounds.Memory->Min + aBounds.Memory->Max; + BoundingBox4 centroidBoundsA; + centroidBoundsA.Min = initialCentroid; + centroidBoundsA.Max = initialCentroid; + for (int i = 1; i < subtreeCountA; ++i) { - BinBoundingBoxes = Suballocate(binAllocationBuffer, ref binStart, binCapacity); - BinCentroidBoundingBoxes = Suballocate(binAllocationBuffer, ref binStart, binCapacity); - BinBoundingBoxesScan = Suballocate(binAllocationBuffer, ref binStart, binCapacity); - BinCentroidBoundingBoxesScan = Suballocate(binAllocationBuffer, ref binStart, binCapacity); - BinLeafCounts = Suballocate(binAllocationBuffer, ref binStart, binCapacity); + ref var bounds = ref aBounds[i]; + var centroid = bounds.Min + bounds.Max; + centroidBoundsA.Min = Vector4.Min(centroidBoundsA.Min, centroid); + centroidBoundsA.Max = Vector4.Max(centroidBoundsA.Max, centroid); } + MicroSweepForBinnedBuilder(centroidBoundsA.Min, centroidBoundsA.Max, ref leaves, subtrees.Slice(subtreeCountA), nodes.Slice(1, subtreeCountA - 1), metanodes.Allocated ? metanodes.Slice(1, subtreeCountA - 1) : metanodes, aIndex, nodeIndex, 0, context, workerIndex); } - unsafe struct MultithreadBinnedBuildContext : IBinnedBuilderThreading + if (subtreeCountB > 1) { - public TaskStack* TaskStack; - /// - /// The number of subtrees present at the root of the build. - /// - public int OriginalSubtreeCount; - /// - /// The target number of tasks that would be used for the root node. Later nodes will tend to target smaller numbers of tasks on the assumption that other parallel nodes will provide enough work to fill in the gaps. - /// - public int TopLevelTargetTaskCount; - public Buffer Workers; - - public void GetBins(int workerIndex, - out Buffer binBoundingBoxes, out Buffer binCentroidBoundingBoxes, - out Buffer binBoundingBoxesScan, out Buffer binCentroidBoundingBoxesScan, out Buffer binLeafCounts) + var bBounds = boundingBoxes.Slice(subtreeCountA, subtreeCountB); + var initialCentroid = bBounds.Memory->Min + bBounds.Memory->Max; + BoundingBox4 centroidBoundsB; + centroidBoundsB.Min = initialCentroid; + centroidBoundsB.Max = initialCentroid; + for (int i = 1; i < subtreeCountB; ++i) { - ref var worker = ref Workers[workerIndex]; - binBoundingBoxes = worker.BinBoundingBoxes; - binCentroidBoundingBoxes = worker.BinCentroidBoundingBoxes; - binBoundingBoxesScan = worker.BinBoundingBoxesScan; - binCentroidBoundingBoxesScan = worker.BinCentroidBoundingBoxesScan; - binLeafCounts = worker.BinLeafCounts; + ref var bounds = ref bBounds[i]; + var centroid = bounds.Min + bounds.Max; + centroidBoundsB.Min = Vector4.Min(centroidBoundsB.Min, centroid); + centroidBoundsB.Max = Vector4.Max(centroidBoundsB.Max, centroid); } + MicroSweepForBinnedBuilder(centroidBoundsB.Min, centroidBoundsB.Max, ref leaves, subtrees.Slice(subtreeCountA, subtreeCountB), nodes.Slice(subtreeCountA, subtreeCountB - 1), metanodes.Allocated ? metanodes.Slice(subtreeCountA, subtreeCountB - 1) : metanodes, bIndex, nodeIndex, 1, context, workerIndex); + } + } - public int GetTargetTaskCountForInnerLoop(int subtreeCount) - { - return (int)float.Ceiling(TopLevelTargetTaskCount * (float)subtreeCount / OriginalSubtreeCount); - } - public int GetTargetTaskCountForNodes(int subtreeCount) - { - return (int)float.Ceiling(TargetTaskCountMultiplierForNodePushOverInnerLoop * TopLevelTargetTaskCount * (float)subtreeCount / OriginalSubtreeCount); - } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + private static int ComputeBinIndex(Vector4 centroidMin, bool useX, bool useY, Vector128 permuteMask, int axisIndex, Vector4 offsetToBinIndex, Vector4 maximumBinIndex, in BoundingBox4 box) + { + var centroid = box.Min + box.Max; + //Note the clamp against zero as well as maximumBinIndex; going negative *can* happen when the bounding box is corrupted. We'd rather not crash with an access violation. + var binIndicesForLeafContinuous = Vector4.Clamp((centroid - centroidMin) * offsetToBinIndex, Vector4.Zero, maximumBinIndex); + //Note that we don't store out any of the indices into per-bin lists here. We only *really* want two final groups for the children, + //and we can easily compute those by performing another scan. It requires recomputing the bin indices, but that's really not much of a concern. + //To extract the desired lane, we need to use a variable shuffle mask. At the time of writing, the Vector128 cross platform shuffle did not like variable masks. + if (Avx.IsSupported) + return (int)Vector128.ToScalar(Avx.PermuteVar(binIndicesForLeafContinuous.AsVector128(), permuteMask)); + else if (Vector128.IsHardwareAccelerated) + return (int)Vector128.GetElement(binIndicesForLeafContinuous.AsVector128(), axisIndex); + else + return (int)(useX ? binIndicesForLeafContinuous.X : useY ? binIndicesForLeafContinuous.Y : binIndicesForLeafContinuous.Z); + } + + struct SingleThreaded : IBinnedBuilderThreading + { + public Buffer BinBoundingBoxes; + public Buffer BinCentroidBoundingBoxes; + public Buffer BinBoundingBoxesScan; + public Buffer BinCentroidBoundingBoxesScan; + public Buffer BinLeafCounts; + + public SingleThreaded(Buffer binAllocationBuffer, int binCapacity) + { + int start = 0; + BinBoundingBoxes = Suballocate(binAllocationBuffer, ref start, binCapacity); + BinCentroidBoundingBoxes = Suballocate(binAllocationBuffer, ref start, binCapacity); + BinBoundingBoxesScan = Suballocate(binAllocationBuffer, ref start, binCapacity); + BinCentroidBoundingBoxesScan = Suballocate(binAllocationBuffer, ref start, binCapacity); + BinLeafCounts = Suballocate(binAllocationBuffer, ref start, binCapacity); } - const int MinimumSubtreesPerThreadForCentroidPrepass = 1024; - const int MinimumSubtreesPerThreadForBinning = 1024; - const int MinimumSubtreesPerThreadForPartitioning = 1024; - const int MinimumSubtreesPerThreadForNodeJob = 256; - const int TargetTaskCountMultiplierForNodePushOverInnerLoop = 8; + public void GetBins(int workerIndex, + out Buffer binBoundingBoxes, out Buffer binCentroidBoundingBoxes, + out Buffer binBoundingBoxesScan, out Buffer binCentroidBoundingBoxesScan, out Buffer binLeafCounts) + { + binBoundingBoxes = BinBoundingBoxes; + binCentroidBoundingBoxes = BinCentroidBoundingBoxes; + binBoundingBoxesScan = BinBoundingBoxesScan; + binCentroidBoundingBoxesScan = BinCentroidBoundingBoxesScan; + binLeafCounts = BinLeafCounts; + } + } + + static Buffer Suballocate(Buffer buffer, ref int start, int count) where T : unmanaged + { + var size = count * Unsafe.SizeOf(); + var previousStart = start; + start += size; + return buffer.Slice(previousStart, size).As(); + } + + /// + /// Stores resources required by a worker to dispatch and manage multithreaded work. + /// + /// + /// Some of the resources cached here are technically redundant with the storage used for workers and ends up involving an extra bin scan on a multithreaded test, + /// but the cost associated with doing so is... low. The complexity cost of trying to use the memory allocated for workers is not low. + /// + struct BinnedBuildWorkerContext + { + /// + /// Bins associated with this worker for the duration of a node. This allocation will persist across the build. + /// + public Buffer BinBoundingBoxes; /// - /// Random value stored in the upper 32 bits of the job tag submitted for internal multithreading operations. + /// Centroid bound bins associated with this worker for the duration of a node. This allocation will persist across the build. /// - /// Other systems using the same task stack may want to use their own filtering approaches. By using a very specific and unique signature, those other systems are less likely to accidentally collide. - const ulong JobFilterTagHeader = 0xB0A1BF32ul << 32; + public Buffer BinCentroidBoundingBoxes; + /// + /// Bins associated with this worker for use in the SAH scan. This allocation will persist across the build. + /// + public Buffer BinBoundingBoxesScan; + /// + /// Centroid bound bins associated with this worker for use in the SAH scan. This allocation will persist across the build. + /// + public Buffer BinCentroidBoundingBoxesScan; + /// + /// Bin leaf counts associated with this worker for the duration of a node. This allocation will persist across the build. + /// + public Buffer BinLeafCounts; - [MethodImpl(MethodImplOptions.AggressiveInlining)] - static BoundingBox4 ComputeCentroidBounds(Buffer bounds) + public BinnedBuildWorkerContext(Buffer binAllocationBuffer, ref int binStart, int binCapacity) { - BoundingBox4 centroidBounds; - centroidBounds.Min = new Vector4(float.MaxValue); - centroidBounds.Max = new Vector4(float.MinValue); - for (int i = 0; i < bounds.Length; ++i) - { - ref var box = ref bounds[i]; - //Note that centroids never bother scaling by 0.5. It's fine as long as we're consistent. - var centroid = box.Min + box.Max; - centroidBounds.Min = Vector4.Min(centroidBounds.Min, centroid); - centroidBounds.Max = Vector4.Max(centroidBounds.Max, centroid); - } - return centroidBounds; + BinBoundingBoxes = Suballocate(binAllocationBuffer, ref binStart, binCapacity); + BinCentroidBoundingBoxes = Suballocate(binAllocationBuffer, ref binStart, binCapacity); + BinBoundingBoxesScan = Suballocate(binAllocationBuffer, ref binStart, binCapacity); + BinCentroidBoundingBoxesScan = Suballocate(binAllocationBuffer, ref binStart, binCapacity); + BinLeafCounts = Suballocate(binAllocationBuffer, ref binStart, binCapacity); } + } + unsafe struct MultithreadBinnedBuildContext : IBinnedBuilderThreading + { + public TaskStack* TaskStack; + /// + /// The number of subtrees present at the root of the build. + /// + public int OriginalSubtreeCount; + /// + /// The target number of tasks that would be used for the root node. Later nodes will tend to target smaller numbers of tasks on the assumption that other parallel nodes will provide enough work to fill in the gaps. + /// + public int TopLevelTargetTaskCount; + public Buffer Workers; - struct SharedTaskData + public void GetBins(int workerIndex, + out Buffer binBoundingBoxes, out Buffer binCentroidBoundingBoxes, + out Buffer binBoundingBoxesScan, out Buffer binCentroidBoundingBoxesScan, out Buffer binLeafCounts) { - public int WorkerCount; - public int TaskCount; + ref var worker = ref Workers[workerIndex]; + binBoundingBoxes = worker.BinBoundingBoxes; + binCentroidBoundingBoxes = worker.BinCentroidBoundingBoxes; + binBoundingBoxesScan = worker.BinBoundingBoxesScan; + binCentroidBoundingBoxesScan = worker.BinCentroidBoundingBoxesScan; + binLeafCounts = worker.BinLeafCounts; + } - public int SubtreeStartIndex; - public int SubtreeCount; + public int GetTargetTaskCountForInnerLoop(int subtreeCount) + { + return (int)float.Ceiling(TopLevelTargetTaskCount * (float)subtreeCount / OriginalSubtreeCount); + } + public int GetTargetTaskCountForNodes(int subtreeCount) + { + return (int)float.Ceiling(TargetTaskCountMultiplierForNodePushOverInnerLoop * TopLevelTargetTaskCount * (float)subtreeCount / OriginalSubtreeCount); + } + } - public int SlotsPerTaskBase; - public int SlotRemainder; - public bool TaskCountFitsInWorkerCount; + const int MinimumSubtreesPerThreadForCentroidPrepass = 1024; + const int MinimumSubtreesPerThreadForBinning = 1024; + const int MinimumSubtreesPerThreadForPartitioning = 1024; + const int MinimumSubtreesPerThreadForNodeJob = 256; + const int TargetTaskCountMultiplierForNodePushOverInnerLoop = 8; + /// + /// Random value stored in the upper 32 bits of the job tag submitted for internal multithreading operations. + /// + /// Other systems using the same task stack may want to use their own filtering approaches. By using a very specific and unique signature, those other systems are less likely to accidentally collide. + const ulong JobFilterTagHeader = 0xB0A1BF32ul << 32; + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + static BoundingBox4 ComputeCentroidBounds(Buffer bounds) + { + BoundingBox4 centroidBounds; + centroidBounds.Min = new Vector4(float.MaxValue); + centroidBounds.Max = new Vector4(float.MinValue); + for (int i = 0; i < bounds.Length; ++i) + { + ref var box = ref bounds[i]; + //Note that centroids never bother scaling by 0.5. It's fine as long as we're consistent. + var centroid = box.Min + box.Max; + centroidBounds.Min = Vector4.Min(centroidBounds.Min, centroid); + centroidBounds.Max = Vector4.Max(centroidBounds.Max, centroid); + } + return centroidBounds; + } - public SharedTaskData(int workerCount, int subtreeStartIndex, int slotCount, - int minimumSlotsPerTask, int targetTaskCount) - { - WorkerCount = workerCount; - var taskSize = int.Max(minimumSlotsPerTask, slotCount / targetTaskCount); - TaskCount = (slotCount + taskSize - 1) / taskSize; - SubtreeStartIndex = subtreeStartIndex; - SubtreeCount = slotCount; - SlotsPerTaskBase = slotCount / TaskCount; - SlotRemainder = slotCount - TaskCount * SlotsPerTaskBase; - TaskCountFitsInWorkerCount = TaskCount <= WorkerCount; - } + struct SharedTaskData + { + public int WorkerCount; + public int TaskCount; - public void GetSlotInterval(long taskId, out int start, out int count) - { - var remainderedTaskCount = int.Min(SlotRemainder, (int)taskId); - var earlySlotCount = (SlotsPerTaskBase + 1) * remainderedTaskCount; - var lateSlotCount = SlotsPerTaskBase * (taskId - remainderedTaskCount); - start = SubtreeStartIndex + (int)(earlySlotCount + lateSlotCount); - count = taskId >= SlotRemainder ? SlotsPerTaskBase : SlotsPerTaskBase + 1; - } - } + public int SubtreeStartIndex; + public int SubtreeCount; - struct CentroidPrepassTaskContext - { - public SharedTaskData TaskData; - /// - /// Stores per-worker prepass bounds accumulated over multiple tasks. If there are less tasks than workers, then only the lower contiguous region of these bounds are used. - /// This allocation is ephemeral; it is allocated from the current worker when needed. - /// Note that the allocation occurs on the loop dispatching thread: the workers that help with the loop do not have to allocate anything themselves. - /// - public Buffer PrepassWorkers; - /// - /// Buffer containing the bounding boxes of all subtrees in the node. - /// - public Buffer Bounds; - - public CentroidPrepassTaskContext(BufferPool pool, SharedTaskData taskData, Buffer bounds) - { - TaskData = taskData; - pool.Take(int.Min(taskData.WorkerCount, taskData.TaskCount), out PrepassWorkers); - Debug.Assert(PrepassWorkers.Length >= 2); - Bounds = bounds; - } + public int SlotsPerTaskBase; + public int SlotRemainder; + public bool TaskCountFitsInWorkerCount; - public void Dispose(BufferPool pool) => pool.Return(ref PrepassWorkers); + public SharedTaskData(int workerCount, int subtreeStartIndex, int slotCount, + int minimumSlotsPerTask, int targetTaskCount) + { + WorkerCount = workerCount; + var taskSize = int.Max(minimumSlotsPerTask, slotCount / targetTaskCount); + TaskCount = (slotCount + taskSize - 1) / taskSize; + SubtreeStartIndex = subtreeStartIndex; + SubtreeCount = slotCount; + SlotsPerTaskBase = slotCount / TaskCount; + SlotRemainder = slotCount - TaskCount * SlotsPerTaskBase; + TaskCountFitsInWorkerCount = TaskCount <= WorkerCount; } - unsafe static void CentroidPrepassWorker(long taskId, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) + + public void GetSlotInterval(long taskId, out int start, out int count) { - ref var context = ref *(CentroidPrepassTaskContext*)untypedContext; - Debug.Assert(context.TaskData.WorkerCount > 1 && context.TaskData.TaskCount > 1 && context.TaskData.WorkerCount < 100); - context.TaskData.GetSlotInterval(taskId, out var start, out var count); - var centroidBounds = ComputeCentroidBounds(context.Bounds.Slice(start, count)); - if (context.TaskData.TaskCountFitsInWorkerCount) - { - //There were less tasks than workers; directly write into the slot without bothering to merge. - context.PrepassWorkers[(int)taskId] = centroidBounds; - } - else - { - ref var workerBounds = ref context.PrepassWorkers[workerIndex]; - workerBounds.Min = Vector4.Min(workerBounds.Min, centroidBounds.Min); - workerBounds.Max = Vector4.Max(workerBounds.Max, centroidBounds.Max); - } + var remainderedTaskCount = int.Min(SlotRemainder, (int)taskId); + var earlySlotCount = (SlotsPerTaskBase + 1) * remainderedTaskCount; + var lateSlotCount = SlotsPerTaskBase * (taskId - remainderedTaskCount); + start = SubtreeStartIndex + (int)(earlySlotCount + lateSlotCount); + count = taskId >= SlotRemainder ? SlotsPerTaskBase : SlotsPerTaskBase + 1; } + } - unsafe static BoundingBox4 MultithreadedCentroidPrepass(MultithreadBinnedBuildContext* context, Buffer bounds, in SharedTaskData taskData, int workerIndex, IThreadDispatcher dispatcher) + struct CentroidPrepassTaskContext + { + public SharedTaskData TaskData; + /// + /// Stores per-worker prepass bounds accumulated over multiple tasks. If there are less tasks than workers, then only the lower contiguous region of these bounds are used. + /// This allocation is ephemeral; it is allocated from the current worker when needed. + /// Note that the allocation occurs on the loop dispatching thread: the workers that help with the loop do not have to allocate anything themselves. + /// + public Buffer PrepassWorkers; + /// + /// Buffer containing the bounding boxes of all subtrees in the node. + /// + public Buffer Bounds; + + public CentroidPrepassTaskContext(BufferPool pool, SharedTaskData taskData, Buffer bounds) { - ref var worker = ref context->Workers[workerIndex]; - var workerPool = dispatcher.WorkerPools[workerIndex]; - var taskContext = new CentroidPrepassTaskContext(workerPool, taskData, bounds); - var taskCount = taskContext.TaskData.TaskCount; - //Don't bother initializing more slots than we have tasks. Note that this requires special handling on the task level; - //if we have less tasks than workers, then the task needs to distinguish that fact. - var activeWorkerCount = int.Min(taskContext.TaskData.WorkerCount, taskCount); - if (taskCount > taskContext.TaskData.WorkerCount) - { - //Potentially multiple tasks per worker; we must preinitialize slots. - for (int i = 0; i < activeWorkerCount; ++i) - { - ref var workerBounds = ref taskContext.PrepassWorkers[i]; - workerBounds.Min = new Vector4(float.MaxValue); - workerBounds.Max = new Vector4(float.MinValue); - } - } - Debug.Assert(taskContext.TaskData.TaskCount > 0 && taskContext.TaskData.WorkerCount > 0); - //We only want the inner multithreading to work on small, non-recursive jobs. - //Diving into a node at this point would stall the current node and favor more (and smaller) nodes. - //(Note: the centroid prepass only runs at the root, so we don't expect there to be any competition from other nodes *in this tree*, - //but it's possible that the same taskstack is used from multiple binned builds. - //Technically, there's potential interference from other user tasks that have nothing to do with binned building, but... not too concerned at this point.) - var tagValue = (uint)workerIndex | JobFilterTagHeader; - var jobFilter = new EqualTagFilter(tagValue); - context->TaskStack->For(&CentroidPrepassWorker, &taskContext, 0, taskCount, workerIndex, dispatcher, ref jobFilter, tagValue); - - var centroidBounds = taskContext.PrepassWorkers[0]; - for (int i = 1; i < activeWorkerCount; ++i) - { - ref var workerBounds = ref taskContext.PrepassWorkers[i]; - centroidBounds.Min = Vector4.Min(workerBounds.Min, centroidBounds.Min); - centroidBounds.Max = Vector4.Max(workerBounds.Max, centroidBounds.Max); - } - taskContext.Dispose(workerPool); - return centroidBounds; + TaskData = taskData; + pool.Take(int.Min(taskData.WorkerCount, taskData.TaskCount), out PrepassWorkers); + Debug.Assert(PrepassWorkers.Length >= 2); + Bounds = bounds; } - struct BinSubtreesWorkerContext + public void Dispose(BufferPool pool) => pool.Return(ref PrepassWorkers); + } + unsafe static void CentroidPrepassWorker(long taskId, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) + { + ref var context = ref *(CentroidPrepassTaskContext*)untypedContext; + Debug.Assert(context.TaskData.WorkerCount > 1 && context.TaskData.TaskCount > 1 && context.TaskData.WorkerCount < 100); + context.TaskData.GetSlotInterval(taskId, out var start, out var count); + var centroidBounds = ComputeCentroidBounds(context.Bounds.Slice(start, count)); + if (context.TaskData.TaskCountFitsInWorkerCount) + { + //There were less tasks than workers; directly write into the slot without bothering to merge. + context.PrepassWorkers[(int)taskId] = centroidBounds; + } + else { - public Buffer BinBoundingBoxes; - public Buffer BinCentroidBoundingBoxes; - public Buffer BinLeafCounts; + ref var workerBounds = ref context.PrepassWorkers[workerIndex]; + workerBounds.Min = Vector4.Min(workerBounds.Min, centroidBounds.Min); + workerBounds.Max = Vector4.Max(workerBounds.Max, centroidBounds.Max); } - unsafe struct BinSubtreesTaskContext + } + + unsafe static BoundingBox4 MultithreadedCentroidPrepass(MultithreadBinnedBuildContext* context, Buffer bounds, in SharedTaskData taskData, int workerIndex, IThreadDispatcher dispatcher) + { + ref var worker = ref context->Workers[workerIndex]; + var workerPool = dispatcher.WorkerPools[workerIndex]; + var taskContext = new CentroidPrepassTaskContext(workerPool, taskData, bounds); + var taskCount = taskContext.TaskData.TaskCount; + //Don't bother initializing more slots than we have tasks. Note that this requires special handling on the task level; + //if we have less tasks than workers, then the task needs to distinguish that fact. + var activeWorkerCount = int.Min(taskContext.TaskData.WorkerCount, taskCount); + if (taskCount > taskContext.TaskData.WorkerCount) { - public SharedTaskData TaskData; - /// - /// Bins associated with any workers that end up contributing to this worker's dispatch of a binning loop. If there are less tasks than workers, then only the lower contiguous region of these bounds are used. - /// This allocation is ephemeral; it is allocated from the current worker when needed. - /// Note that the allocation occurs on the loop dispatching thread: the workers that help with the loop do not have to allocate anything themselves. - /// - public Buffer BinSubtreesWorkers; - /// - /// Whether a given worker contributed to the subtree binning process. If this worker did not contribute, there's no reason to merge its bins. - /// This allocation is ephemeral; it is allocated from the current worker when needed. - /// Note that the allocation occurs on the loop dispatching thread: the workers that help with the loop do not have to allocate anything themselves. - /// - public Buffer WorkerHelpedWithBinning; - - /// - /// Buffer containing all subtrees in this node. - /// - public Buffer Subtrees; - - /// - /// Stores the bin indices of all subtrees in the node. - /// - public Buffer BinIndices; - - public int BinCount; - public bool UseX, UseY; - public Vector128 PermuteMask; - public int AxisIndex; - public Vector4 CentroidBoundsMin; - public Vector4 OffsetToBinIndex; - public Vector4 MaximumBinIndex; - - public BinSubtreesTaskContext(BufferPool pool, SharedTaskData taskData, Buffer subtrees, Buffer binIndices, - int binCount, bool useX, bool useY, Vector128 permuteMask, int axisIndex, - Vector4 centroidBoundsMin, Vector4 offsetToBinIndex, Vector4 maximumBinIndex) + //Potentially multiple tasks per worker; we must preinitialize slots. + for (int i = 0; i < activeWorkerCount; ++i) { - TaskData = taskData; - Subtrees = subtrees; - BinIndices = binIndices; - BinCount = binCount; - UseX = useX; - UseY = useY; - PermuteMask = permuteMask; - AxisIndex = axisIndex; - CentroidBoundsMin = centroidBoundsMin; - OffsetToBinIndex = offsetToBinIndex; - MaximumBinIndex = maximumBinIndex; - var effectiveWorkerCount = int.Min(taskData.WorkerCount, taskData.TaskCount); - //Pull one allocation from the pool instead of 1 + workerCount * 2. Slight reduction in overhead. Note that this means we only need to return one buffer of the associated id at the end! - var allocationSize = (sizeof(BinSubtreesWorkerContext) + (sizeof(BoundingBox4) * 2 + sizeof(int)) * binCount + sizeof(bool) * taskData.WorkerCount) * effectiveWorkerCount; - pool.Take(allocationSize, out var allocation); - int start = 0; - BinSubtreesWorkers = Suballocate(allocation, ref start, effectiveWorkerCount); - for (int i = 0; i < effectiveWorkerCount; ++i) - { - ref var worker = ref BinSubtreesWorkers[i]; - worker.BinBoundingBoxes = Suballocate(allocation, ref start, BinCount); - worker.BinCentroidBoundingBoxes = Suballocate(allocation, ref start, BinCount); - worker.BinLeafCounts = Suballocate(allocation, ref start, BinCount); - } - WorkerHelpedWithBinning = Suballocate(allocation, ref start, effectiveWorkerCount); - WorkerHelpedWithBinning.Clear(0, effectiveWorkerCount); + ref var workerBounds = ref taskContext.PrepassWorkers[i]; + workerBounds.Min = new Vector4(float.MaxValue); + workerBounds.Max = new Vector4(float.MinValue); } - public void Dispose(BufferPool pool) => pool.Return(ref BinSubtreesWorkers); //Only need to return the main buffer because all the other allocations share the same id! } - //these type-level booleans let the compiler avoid branching in the binning loop. The bin indices buffer is not guaranteed to exist. - //i apologize + Debug.Assert(taskContext.TaskData.TaskCount > 0 && taskContext.TaskData.WorkerCount > 0); + //We only want the inner multithreading to work on small, non-recursive jobs. + //Diving into a node at this point would stall the current node and favor more (and smaller) nodes. + //(Note: the centroid prepass only runs at the root, so we don't expect there to be any competition from other nodes *in this tree*, + //but it's possible that the same taskstack is used from multiple binned builds. + //Technically, there's potential interference from other user tasks that have nothing to do with binned building, but... not too concerned at this point.) + var tagValue = (uint)workerIndex | JobFilterTagHeader; + var jobFilter = new EqualTagFilter(tagValue); + context->TaskStack->For(&CentroidPrepassWorker, &taskContext, 0, taskCount, workerIndex, dispatcher, ref jobFilter, tagValue); + + var centroidBounds = taskContext.PrepassWorkers[0]; + for (int i = 1; i < activeWorkerCount; ++i) + { + ref var workerBounds = ref taskContext.PrepassWorkers[i]; + centroidBounds.Min = Vector4.Min(workerBounds.Min, centroidBounds.Min); + centroidBounds.Max = Vector4.Max(workerBounds.Max, centroidBounds.Max); + } + taskContext.Dispose(workerPool); + return centroidBounds; + } + + struct BinSubtreesWorkerContext + { + public Buffer BinBoundingBoxes; + public Buffer BinCentroidBoundingBoxes; + public Buffer BinLeafCounts; + } + unsafe struct BinSubtreesTaskContext + { + public SharedTaskData TaskData; /// - /// Marks a call as requiring the bin indices to be written to the binIndices buffer. + /// Bins associated with any workers that end up contributing to this worker's dispatch of a binning loop. If there are less tasks than workers, then only the lower contiguous region of these bounds are used. + /// This allocation is ephemeral; it is allocated from the current worker when needed. + /// Note that the allocation occurs on the loop dispatching thread: the workers that help with the loop do not have to allocate anything themselves. /// - private struct DoWriteBinIndices { } + public Buffer BinSubtreesWorkers; /// - /// Marks a call as not allowing the bin indices to be written to the binIndices buffer. + /// Whether a given worker contributed to the subtree binning process. If this worker did not contribute, there's no reason to merge its bins. + /// This allocation is ephemeral; it is allocated from the current worker when needed. + /// Note that the allocation occurs on the loop dispatching thread: the workers that help with the loop do not have to allocate anything themselves. /// - private struct DoNotWriteBinIndices { } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - static void BinSubtrees(Vector4 centroidBoundsMin, - bool useX, bool useY, Vector128 permuteMask, int axisIndex, Vector4 offsetToBinIndex, Vector4 maximumBinIndex, - Buffer subtrees, Buffer binBoundingBoxes, Buffer binCentroidBoundingBoxes, Buffer binLeafCounts, Buffer binIndices) - where TShouldWriteBinIndices : unmanaged + public Buffer WorkerHelpedWithBinning; + + /// + /// Buffer containing all subtrees in this node. + /// + public Buffer Subtrees; + + /// + /// Stores the bin indices of all subtrees in the node. + /// + public Buffer BinIndices; + + public int BinCount; + public bool UseX, UseY; + public Vector128 PermuteMask; + public int AxisIndex; + public Vector4 CentroidBoundsMin; + public Vector4 OffsetToBinIndex; + public Vector4 MaximumBinIndex; + + public BinSubtreesTaskContext(BufferPool pool, SharedTaskData taskData, Buffer subtrees, Buffer binIndices, + int binCount, bool useX, bool useY, Vector128 permuteMask, int axisIndex, + Vector4 centroidBoundsMin, Vector4 offsetToBinIndex, Vector4 maximumBinIndex) + { + TaskData = taskData; + Subtrees = subtrees; + BinIndices = binIndices; + BinCount = binCount; + UseX = useX; + UseY = useY; + PermuteMask = permuteMask; + AxisIndex = axisIndex; + CentroidBoundsMin = centroidBoundsMin; + OffsetToBinIndex = offsetToBinIndex; + MaximumBinIndex = maximumBinIndex; + var effectiveWorkerCount = int.Min(taskData.WorkerCount, taskData.TaskCount); + //Pull one allocation from the pool instead of 1 + workerCount * 2. Slight reduction in overhead. Note that this means we only need to return one buffer of the associated id at the end! + var allocationSize = (sizeof(BinSubtreesWorkerContext) + (sizeof(BoundingBox4) * 2 + sizeof(int)) * binCount + sizeof(bool) * taskData.WorkerCount) * effectiveWorkerCount; + pool.Take(allocationSize, out var allocation); + int start = 0; + BinSubtreesWorkers = Suballocate(allocation, ref start, effectiveWorkerCount); + for (int i = 0; i < effectiveWorkerCount; ++i) + { + ref var worker = ref BinSubtreesWorkers[i]; + worker.BinBoundingBoxes = Suballocate(allocation, ref start, BinCount); + worker.BinCentroidBoundingBoxes = Suballocate(allocation, ref start, BinCount); + worker.BinLeafCounts = Suballocate(allocation, ref start, BinCount); + } + WorkerHelpedWithBinning = Suballocate(allocation, ref start, effectiveWorkerCount); + WorkerHelpedWithBinning.Clear(0, effectiveWorkerCount); + } + public void Dispose(BufferPool pool) => pool.Return(ref BinSubtreesWorkers); //Only need to return the main buffer because all the other allocations share the same id! + } + //these type-level booleans let the compiler avoid branching in the binning loop. The bin indices buffer is not guaranteed to exist. + //i apologize + /// + /// Marks a call as requiring the bin indices to be written to the binIndices buffer. + /// + private struct DoWriteBinIndices { } + /// + /// Marks a call as not allowing the bin indices to be written to the binIndices buffer. + /// + private struct DoNotWriteBinIndices { } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + static void BinSubtrees(Vector4 centroidBoundsMin, + bool useX, bool useY, Vector128 permuteMask, int axisIndex, Vector4 offsetToBinIndex, Vector4 maximumBinIndex, + Buffer subtrees, Buffer binBoundingBoxes, Buffer binCentroidBoundingBoxes, Buffer binLeafCounts, Buffer binIndices) + where TShouldWriteBinIndices : unmanaged + { + //Note that we don't store out any of the indices into per-bin lists here. We only *really* want two final groups for the children, + //and we can easily compute those by performing another scan. It requires recomputing the bin indices, but that's really not much of a concern. + for (int i = 0; i < subtrees.Length; ++i) + { + ref var subtree = ref subtrees[i]; + ref var box = ref Unsafe.As(ref subtree); + var binIndex = ComputeBinIndex(centroidBoundsMin, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, box); + if (typeof(TShouldWriteBinIndices) == typeof(DoWriteBinIndices)) + binIndices[i] = (byte)binIndex; + ref var binBounds = ref binBoundingBoxes[binIndex]; + binBounds.Min = Vector4.Min(binBounds.Min, box.Min); + binBounds.Max = Vector4.Max(binBounds.Max, box.Max); + //The binning phase also keeps track of *centroid* bounding boxes so that we don't have to do a dedicated centroid prepass for each node. + //(A centroid prepass would require touching every single subtree again, and, for large trees, that's a lot of uncached (or distant) memory accesses.) + var centroid = box.Min + box.Max; + ref var binCentroidBounds = ref binCentroidBoundingBoxes[binIndex]; + binCentroidBounds.Min = Vector4.Min(binCentroidBounds.Min, centroid); + binCentroidBounds.Max = Vector4.Max(binCentroidBounds.Max, centroid); + binLeafCounts[binIndex] += subtree.LeafCount; + } + } + unsafe static void BinSubtreesWorker(long taskId, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) + { + ref var context = ref *(BinSubtreesTaskContext*)untypedContext; + Debug.Assert(context.TaskData.WorkerCount > 1 && context.TaskData.TaskCount > 1 && context.TaskData.WorkerCount < 100); + //Note that if we have more workers than tasks, we use the task id to index into the caches (and initialize the data here rather then before dispatching). + var effectiveWorkerIndex = context.TaskData.TaskCountFitsInWorkerCount ? (int)taskId : workerIndex; + ref var worker = ref context.BinSubtreesWorkers[effectiveWorkerIndex]; + context.WorkerHelpedWithBinning[effectiveWorkerIndex] = true; + if (context.TaskData.TaskCountFitsInWorkerCount) { - //Note that we don't store out any of the indices into per-bin lists here. We only *really* want two final groups for the children, - //and we can easily compute those by performing another scan. It requires recomputing the bin indices, but that's really not much of a concern. - for (int i = 0; i < subtrees.Length; ++i) + for (int i = 0; i < context.BinCount; ++i) { - ref var subtree = ref subtrees[i]; - ref var box = ref Unsafe.As(ref subtree); - var binIndex = ComputeBinIndex(centroidBoundsMin, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, box); - if (typeof(TShouldWriteBinIndices) == typeof(DoWriteBinIndices)) - binIndices[i] = (byte)binIndex; - ref var binBounds = ref binBoundingBoxes[binIndex]; - binBounds.Min = Vector4.Min(binBounds.Min, box.Min); - binBounds.Max = Vector4.Max(binBounds.Max, box.Max); - //The binning phase also keeps track of *centroid* bounding boxes so that we don't have to do a dedicated centroid prepass for each node. - //(A centroid prepass would require touching every single subtree again, and, for large trees, that's a lot of uncached (or distant) memory accesses.) - var centroid = box.Min + box.Max; - ref var binCentroidBounds = ref binCentroidBoundingBoxes[binIndex]; - binCentroidBounds.Min = Vector4.Min(binCentroidBounds.Min, centroid); - binCentroidBounds.Max = Vector4.Max(binCentroidBounds.Max, centroid); - binLeafCounts[binIndex] += subtree.LeafCount; + ref var binBounds = ref worker.BinBoundingBoxes[i]; + binBounds.Min = new Vector4(float.MaxValue); + binBounds.Max = new Vector4(float.MinValue); + ref var binCentroidBounds = ref worker.BinCentroidBoundingBoxes[i]; + binCentroidBounds.Min = new Vector4(float.MaxValue); + binCentroidBounds.Max = new Vector4(float.MinValue); + worker.BinLeafCounts[i] = 0; } } - unsafe static void BinSubtreesWorker(long taskId, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) + context.TaskData.GetSlotInterval(taskId, out var start, out var count); + //We always write bin indices, because threading always has a bufferpool available to allocate bin indices from. + Debug.Assert(context.BinIndices.Allocated); + BinSubtrees(context.CentroidBoundsMin, context.UseX, context.UseY, context.PermuteMask, context.AxisIndex, context.OffsetToBinIndex, context.MaximumBinIndex, + context.Subtrees.Slice(start, count), worker.BinBoundingBoxes, worker.BinCentroidBoundingBoxes, worker.BinLeafCounts, context.BinIndices.Slice(start, count)); + } + + unsafe static void MultithreadedBinSubtrees(MultithreadBinnedBuildContext* context, + Vector4 centroidBoundsMin, bool useX, bool useY, Vector128 permuteMask, int axisIndex, Vector4 offsetToBinIndex, Vector4 maximumBinIndex, + Buffer subtrees, Buffer subtreeBinIndices, int binCount, in SharedTaskData taskData, int workerIndex, IThreadDispatcher dispatcher) + { + ref var worker = ref context->Workers[workerIndex]; + var workerPool = dispatcher.WorkerPools[workerIndex]; + var taskContext = new BinSubtreesTaskContext( + workerPool, taskData, subtrees, subtreeBinIndices, binCount, useX, useY, permuteMask, axisIndex, centroidBoundsMin, offsetToBinIndex, maximumBinIndex); + + //Don't bother initializing more slots than we have tasks. Note that this requires special handling on the task level; + //if we have less tasks than workers, then the task needs to distinguish that fact. + var activeWorkerCount = int.Min(context->Workers.Length, taskContext.TaskData.TaskCount); + if (!taskContext.TaskData.TaskCountFitsInWorkerCount) { - ref var context = ref *(BinSubtreesTaskContext*)untypedContext; - Debug.Assert(context.TaskData.WorkerCount > 1 && context.TaskData.TaskCount > 1 && context.TaskData.WorkerCount < 100); - //Note that if we have more workers than tasks, we use the task id to index into the caches (and initialize the data here rather then before dispatching). - var effectiveWorkerIndex = context.TaskData.TaskCountFitsInWorkerCount ? (int)taskId : workerIndex; - ref var worker = ref context.BinSubtreesWorkers[effectiveWorkerIndex]; - context.WorkerHelpedWithBinning[effectiveWorkerIndex] = true; - if (context.TaskData.TaskCountFitsInWorkerCount) + //If there are more tasks than workers, then we need to preinitialize all the worker caches. + for (int cacheIndex = 0; cacheIndex < activeWorkerCount; ++cacheIndex) { - for (int i = 0; i < context.BinCount; ++i) + ref var cache = ref taskContext.BinSubtreesWorkers[cacheIndex]; + for (int i = 0; i < binCount; ++i) { - ref var binBounds = ref worker.BinBoundingBoxes[i]; + ref var binBounds = ref cache.BinBoundingBoxes[i]; binBounds.Min = new Vector4(float.MaxValue); binBounds.Max = new Vector4(float.MinValue); - ref var binCentroidBounds = ref worker.BinCentroidBoundingBoxes[i]; + ref var binCentroidBounds = ref cache.BinCentroidBoundingBoxes[i]; binCentroidBounds.Min = new Vector4(float.MaxValue); binCentroidBounds.Max = new Vector4(float.MinValue); - worker.BinLeafCounts[i] = 0; + cache.BinLeafCounts[i] = 0; } } - context.TaskData.GetSlotInterval(taskId, out var start, out var count); - //We always write bin indices, because threading always has a bufferpool available to allocate bin indices from. - Debug.Assert(context.BinIndices.Allocated); - BinSubtrees(context.CentroidBoundsMin, context.UseX, context.UseY, context.PermuteMask, context.AxisIndex, context.OffsetToBinIndex, context.MaximumBinIndex, - context.Subtrees.Slice(start, count), worker.BinBoundingBoxes, worker.BinCentroidBoundingBoxes, worker.BinLeafCounts, context.BinIndices.Slice(start, count)); } - unsafe static void MultithreadedBinSubtrees(MultithreadBinnedBuildContext* context, - Vector4 centroidBoundsMin, bool useX, bool useY, Vector128 permuteMask, int axisIndex, Vector4 offsetToBinIndex, Vector4 maximumBinIndex, - Buffer subtrees, Buffer subtreeBinIndices, int binCount, in SharedTaskData taskData, int workerIndex, IThreadDispatcher dispatcher) + //We only want the inner multithreading to work on small, non-recursive jobs. + //Diving into a node at this point would stall the current node and favor more (and smaller) nodes. + var tagValue = (uint)workerIndex | JobFilterTagHeader; + var jobFilter = new EqualTagFilter(tagValue); + context->TaskStack->For(&BinSubtreesWorker, &taskContext, 0, taskContext.TaskData.TaskCount, workerIndex, dispatcher, ref jobFilter, tagValue); + + //Unless the number of threads and bins is really huge, there's no value in attempting to multithread the final compression. + //(Parallel reduction is an option, but even then... I suspect the single threaded version will be faster. And it's way simpler.) + //Note that we have a separate merging target from the caches; that just makes resource management easier. + //We can dispose the worker stuff immediately after this merge. + //(Consider what happens in the case where the single threaded path is used: you need an allocation! would you allocate a bunch of multithreaded workers for it? + //That's not an irrelevant case, either. *Most* nodes will be too small to warrant internal multithreading.) + ref var cache0 = ref taskContext.BinSubtreesWorkers[0]; + cache0.BinBoundingBoxes.CopyTo(0, worker.BinBoundingBoxes, 0, cache0.BinBoundingBoxes.Length); + cache0.BinCentroidBoundingBoxes.CopyTo(0, worker.BinCentroidBoundingBoxes, 0, cache0.BinCentroidBoundingBoxes.Length); + cache0.BinLeafCounts.CopyTo(0, worker.BinLeafCounts, 0, cache0.BinLeafCounts.Length); + for (int cacheIndex = 1; cacheIndex < activeWorkerCount; ++cacheIndex) { - ref var worker = ref context->Workers[workerIndex]; - var workerPool = dispatcher.WorkerPools[workerIndex]; - var taskContext = new BinSubtreesTaskContext( - workerPool, taskData, subtrees, subtreeBinIndices, binCount, useX, useY, permuteMask, axisIndex, centroidBoundsMin, offsetToBinIndex, maximumBinIndex); - - //Don't bother initializing more slots than we have tasks. Note that this requires special handling on the task level; - //if we have less tasks than workers, then the task needs to distinguish that fact. - var activeWorkerCount = int.Min(context->Workers.Length, taskContext.TaskData.TaskCount); - if (!taskContext.TaskData.TaskCountFitsInWorkerCount) + //Only bother merging from workers that actually did anything. + if (taskContext.WorkerHelpedWithBinning[cacheIndex]) { - //If there are more tasks than workers, then we need to preinitialize all the worker caches. - for (int cacheIndex = 0; cacheIndex < activeWorkerCount; ++cacheIndex) + ref var cache = ref taskContext.BinSubtreesWorkers[cacheIndex]; + for (int binIndex = 0; binIndex < binCount; ++binIndex) { - ref var cache = ref taskContext.BinSubtreesWorkers[cacheIndex]; - for (int i = 0; i < binCount; ++i) - { - ref var binBounds = ref cache.BinBoundingBoxes[i]; - binBounds.Min = new Vector4(float.MaxValue); - binBounds.Max = new Vector4(float.MinValue); - ref var binCentroidBounds = ref cache.BinCentroidBoundingBoxes[i]; - binCentroidBounds.Min = new Vector4(float.MaxValue); - binCentroidBounds.Max = new Vector4(float.MinValue); - cache.BinLeafCounts[i] = 0; - } + ref var b0 = ref worker.BinBoundingBoxes[binIndex]; + ref var bi = ref cache.BinBoundingBoxes[binIndex]; + b0.Min = Vector4.Min(b0.Min, bi.Min); + b0.Max = Vector4.Max(b0.Max, bi.Max); + ref var bc0 = ref worker.BinCentroidBoundingBoxes[binIndex]; + ref var bci = ref cache.BinCentroidBoundingBoxes[binIndex]; + bc0.Min = Vector4.Min(bc0.Min, bci.Min); + bc0.Max = Vector4.Max(bc0.Max, bci.Max); + worker.BinLeafCounts[binIndex] += cache.BinLeafCounts[binIndex]; } } - - //We only want the inner multithreading to work on small, non-recursive jobs. - //Diving into a node at this point would stall the current node and favor more (and smaller) nodes. - var tagValue = (uint)workerIndex | JobFilterTagHeader; - var jobFilter = new EqualTagFilter(tagValue); - context->TaskStack->For(&BinSubtreesWorker, &taskContext, 0, taskContext.TaskData.TaskCount, workerIndex, dispatcher, ref jobFilter, tagValue); - - //Unless the number of threads and bins is really huge, there's no value in attempting to multithread the final compression. - //(Parallel reduction is an option, but even then... I suspect the single threaded version will be faster. And it's way simpler.) - //Note that we have a separate merging target from the caches; that just makes resource management easier. - //We can dispose the worker stuff immediately after this merge. - //(Consider what happens in the case where the single threaded path is used: you need an allocation! would you allocate a bunch of multithreaded workers for it? - //That's not an irrelevant case, either. *Most* nodes will be too small to warrant internal multithreading.) - ref var cache0 = ref taskContext.BinSubtreesWorkers[0]; - cache0.BinBoundingBoxes.CopyTo(0, worker.BinBoundingBoxes, 0, cache0.BinBoundingBoxes.Length); - cache0.BinCentroidBoundingBoxes.CopyTo(0, worker.BinCentroidBoundingBoxes, 0, cache0.BinCentroidBoundingBoxes.Length); - cache0.BinLeafCounts.CopyTo(0, worker.BinLeafCounts, 0, cache0.BinLeafCounts.Length); - for (int cacheIndex = 1; cacheIndex < activeWorkerCount; ++cacheIndex) - { - //Only bother merging from workers that actually did anything. - if (taskContext.WorkerHelpedWithBinning[cacheIndex]) - { - ref var cache = ref taskContext.BinSubtreesWorkers[cacheIndex]; - for (int binIndex = 0; binIndex < binCount; ++binIndex) - { - ref var b0 = ref worker.BinBoundingBoxes[binIndex]; - ref var bi = ref cache.BinBoundingBoxes[binIndex]; - b0.Min = Vector4.Min(b0.Min, bi.Min); - b0.Max = Vector4.Max(b0.Max, bi.Max); - ref var bc0 = ref worker.BinCentroidBoundingBoxes[binIndex]; - ref var bci = ref cache.BinCentroidBoundingBoxes[binIndex]; - bc0.Min = Vector4.Min(bc0.Min, bci.Min); - bc0.Max = Vector4.Max(bc0.Max, bci.Max); - worker.BinLeafCounts[binIndex] += cache.BinLeafCounts[binIndex]; - } - } - } - taskContext.Dispose(workerPool); } + taskContext.Dispose(workerPool); + } - [StructLayout(LayoutKind.Explicit, Size = 264)] - struct PartitionCounters - { - //Padding to avoid shared cache lines. - [FieldOffset(128)] - public int SubtreeCountA; - [FieldOffset(134)] - public int SubtreeCountB; - } + [StructLayout(LayoutKind.Explicit, Size = 264)] + struct PartitionCounters + { + //Padding to avoid shared cache lines. + [FieldOffset(128)] + public int SubtreeCountA; + [FieldOffset(134)] + public int SubtreeCountB; + } + + struct PartitionTaskContext + { + public SharedTaskData TaskData; - struct PartitionTaskContext + /// + /// Buffer containing all subtrees in this node. + /// + public Buffer Subtrees; + /// + /// Buffer that will contain the partitioned subtrees pulled from . + /// + public Buffer SubtreesNext; + /// + /// Buffer containing bin indices for all subtrees in the node (encoded with one byte per subtree). + /// + public Buffer BinIndices; + public int BinSplitIndex; + + public PartitionCounters Counters; + + public PartitionTaskContext(SharedTaskData taskData, Buffer subtrees, Buffer subtreesNext, Buffer binIndices, int binSplitIndex) { - public SharedTaskData TaskData; - - /// - /// Buffer containing all subtrees in this node. - /// - public Buffer Subtrees; - /// - /// Buffer that will contain the partitioned subtrees pulled from . - /// - public Buffer SubtreesNext; - /// - /// Buffer containing bin indices for all subtrees in the node (encoded with one byte per subtree). - /// - public Buffer BinIndices; - public int BinSplitIndex; - - public PartitionCounters Counters; - - public PartitionTaskContext(SharedTaskData taskData, Buffer subtrees, Buffer subtreesNext, Buffer binIndices, int binSplitIndex) - { - TaskData = taskData; - Subtrees = subtrees; - SubtreesNext = subtreesNext; - BinIndices = binIndices; - BinSplitIndex = binSplitIndex; + TaskData = taskData; + Subtrees = subtrees; + SubtreesNext = subtreesNext; + BinIndices = binIndices; + BinSplitIndex = binSplitIndex; - Counters = new PartitionCounters(); - } + Counters = new PartitionCounters(); } + } - unsafe static void PartitionSubtreesWorker(long taskId, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) + unsafe static void PartitionSubtreesWorker(long taskId, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) + { + ref var context = ref *(PartitionTaskContext*)untypedContext; + Buffer binIndices = context.BinIndices; + context.TaskData.GetSlotInterval(taskId, out var start, out var count); + //We don't really want to trigger interlocked operation for *every single subtree*, but we also don't want to allocate a bunch of memory. + //Compromise! Stackalloc enough memory to cover sub-batches of the worker's subtrees, and do interlocked operations at the end of each batch. + //Note that the main limit to the batch size is the amount of memory in cache. + const int batchSize = 16384; + byte* slotBelongsToA = stackalloc byte[batchSize]; + + var batchCount = (count + batchSize - 1) / batchSize; + var boundingBoxes = context.Subtrees.As(); + var subtrees = context.Subtrees; + var subtreesNext = context.SubtreesNext; + var splitIndexBundle = new Vector((byte)context.BinSplitIndex); + for (int batchIndex = 0; batchIndex < batchCount; ++batchIndex) { - ref var context = ref *(PartitionTaskContext*)untypedContext; - Buffer binIndices = context.BinIndices; - context.TaskData.GetSlotInterval(taskId, out var start, out var count); - //We don't really want to trigger interlocked operation for *every single subtree*, but we also don't want to allocate a bunch of memory. - //Compromise! Stackalloc enough memory to cover sub-batches of the worker's subtrees, and do interlocked operations at the end of each batch. - //Note that the main limit to the batch size is the amount of memory in cache. - const int batchSize = 16384; - byte* slotBelongsToA = stackalloc byte[batchSize]; - - var batchCount = (count + batchSize - 1) / batchSize; - var boundingBoxes = context.Subtrees.As(); - var subtrees = context.Subtrees; - var subtreesNext = context.SubtreesNext; - var splitIndexBundle = new Vector((byte)context.BinSplitIndex); - for (int batchIndex = 0; batchIndex < batchCount; ++batchIndex) - { - var localCountA = 0; - var batchStart = start + batchIndex * batchSize; - var countInBatch = int.Min(start + count - batchStart, batchSize); + var localCountA = 0; + var batchStart = start + batchIndex * batchSize; + var countInBatch = int.Min(start + count - batchStart, batchSize); - int scalarLoopStartIndex; - if (Vector.IsSupported) - { - //Note that the original data is loaded as bytes, but we need wider storage to handle the counts- which could conceivably go up to batchSize. - Vector localCountABundle = Vector.Zero; - scalarLoopStartIndex = (countInBatch / Vector.Count) * Vector.Count; - for (int indexInBatch = 0; indexInBatch < scalarLoopStartIndex; indexInBatch += Vector.Count) - { - var subtreeIndex = indexInBatch + batchStart; - var binIndicesBundle = *(Vector*)(binIndices.Memory + subtreeIndex); - var belongsToABundle = Vector.LessThan(binIndicesBundle, splitIndexBundle); - *(Vector*)(slotBelongsToA + indexInBatch) = belongsToABundle; - var increment = Vector.BitwiseAnd(belongsToABundle, Vector.One); - Vector.Widen(increment, out var low, out var high); - localCountABundle += low + high; - } - localCountA = Vector.Sum(localCountABundle); - } - else - scalarLoopStartIndex = 0; - for (int indexInBatch = scalarLoopStartIndex; indexInBatch < countInBatch; ++indexInBatch) + int scalarLoopStartIndex; + if (Vector.IsSupported) + { + //Note that the original data is loaded as bytes, but we need wider storage to handle the counts- which could conceivably go up to batchSize. + Vector localCountABundle = Vector.Zero; + scalarLoopStartIndex = (countInBatch / Vector.Count) * Vector.Count; + for (int indexInBatch = 0; indexInBatch < scalarLoopStartIndex; indexInBatch += Vector.Count) { var subtreeIndex = indexInBatch + batchStart; - var binIndex = binIndices[subtreeIndex]; - var belongsToA = binIndex < context.BinSplitIndex; - slotBelongsToA[indexInBatch] = belongsToA ? (byte)0xFF : (byte)0; - if (belongsToA) ++localCountA; + var binIndicesBundle = *(Vector*)(binIndices.Memory + subtreeIndex); + var belongsToABundle = Vector.LessThan(binIndicesBundle, splitIndexBundle); + *(Vector*)(slotBelongsToA + indexInBatch) = belongsToABundle; + var increment = Vector.BitwiseAnd(belongsToABundle, Vector.One); + Vector.Widen(increment, out var low, out var high); + localCountABundle += low + high; } + localCountA = Vector.Sum(localCountABundle); + } + else + scalarLoopStartIndex = 0; + for (int indexInBatch = scalarLoopStartIndex; indexInBatch < countInBatch; ++indexInBatch) + { + var subtreeIndex = indexInBatch + batchStart; + var binIndex = binIndices[subtreeIndex]; + var belongsToA = binIndex < context.BinSplitIndex; + slotBelongsToA[indexInBatch] = belongsToA ? (byte)0xFF : (byte)0; + if (belongsToA) ++localCountA; + } - var localCountB = countInBatch - localCountA; - var startIndexA = Interlocked.Add(ref context.Counters.SubtreeCountA, localCountA) - localCountA; - var startIndexB = subtrees.Length - Interlocked.Add(ref context.Counters.SubtreeCountB, localCountB); - - int recountA = 0; - int recountB = 0; - for (int indexInBatch = 0; indexInBatch < countInBatch; ++indexInBatch) - { - var targetIndex = slotBelongsToA[indexInBatch] != 0 ? startIndexA + recountA++ : startIndexB + recountB++; - subtreesNext[targetIndex] = subtrees[batchStart + indexInBatch]; - } + var localCountB = countInBatch - localCountA; + var startIndexA = Interlocked.Add(ref context.Counters.SubtreeCountA, localCountA) - localCountA; + var startIndexB = subtrees.Length - Interlocked.Add(ref context.Counters.SubtreeCountB, localCountB); + int recountA = 0; + int recountB = 0; + for (int indexInBatch = 0; indexInBatch < countInBatch; ++indexInBatch) + { + var targetIndex = slotBelongsToA[indexInBatch] != 0 ? startIndexA + recountA++ : startIndexB + recountB++; + subtreesNext[targetIndex] = subtrees[batchStart + indexInBatch]; } - } - unsafe static (int subtreeCountA, int subtreeCountB) MultithreadedPartition(MultithreadBinnedBuildContext* context, - Buffer subtrees, Buffer subtreesNext, Buffer binIndices, int binSplitIndex, in SharedTaskData taskData, int workerIndex, IThreadDispatcher dispatcher) - { - ref var worker = ref context->Workers[workerIndex]; - var workerPool = dispatcher.WorkerPools[workerIndex]; - var taskContext = new PartitionTaskContext(taskData, subtrees, subtreesNext, binIndices, binSplitIndex); - //We only want the inner multithreading to work on small, non-recursive jobs. - //Diving into a node at this point would stall the current node and favor more (and smaller) nodes. - var tagValue = (uint)workerIndex | JobFilterTagHeader; - var jobFilter = new EqualTagFilter(tagValue); - context->TaskStack->For(&PartitionSubtreesWorker, &taskContext, 0, taskContext.TaskData.TaskCount, workerIndex, dispatcher, ref jobFilter, tagValue); - return (taskContext.Counters.SubtreeCountA, taskContext.Counters.SubtreeCountB); } + } + + unsafe static (int subtreeCountA, int subtreeCountB) MultithreadedPartition(MultithreadBinnedBuildContext* context, + Buffer subtrees, Buffer subtreesNext, Buffer binIndices, int binSplitIndex, in SharedTaskData taskData, int workerIndex, IThreadDispatcher dispatcher) + { + ref var worker = ref context->Workers[workerIndex]; + var workerPool = dispatcher.WorkerPools[workerIndex]; + var taskContext = new PartitionTaskContext(taskData, subtrees, subtreesNext, binIndices, binSplitIndex); + //We only want the inner multithreading to work on small, non-recursive jobs. + //Diving into a node at this point would stall the current node and favor more (and smaller) nodes. + var tagValue = (uint)workerIndex | JobFilterTagHeader; + var jobFilter = new EqualTagFilter(tagValue); + context->TaskStack->For(&PartitionSubtreesWorker, &taskContext, 0, taskContext.TaskData.TaskCount, workerIndex, dispatcher, ref jobFilter, tagValue); + return (taskContext.Counters.SubtreeCountA, taskContext.Counters.SubtreeCountB); + } - unsafe struct NodePushTaskContext - where TLeaves : unmanaged where TThreading : unmanaged, IBinnedBuilderThreading + unsafe struct NodePushTaskContext + where TLeaves : unmanaged where TThreading : unmanaged, IBinnedBuilderThreading + { + public Context* Context; + public int NodeIndex; + public int ParentNodeIndex; + public BoundingBox4 CentroidBounds; + //Subtree region start index, subtree count, and usePongBuffer status are all encoded into the task id. + } + unsafe static void BinnedBuilderNodeWorker(long taskId, void* context, int workerIndex, IThreadDispatcher dispatcher) + where TLeaves : unmanaged where TThreading : unmanaged, IBinnedBuilderThreading + { + var subtreeRegionStartIndex = (int)taskId; + var subtreeCount = (int)((taskId >> 32) & 0x7FFF_FFFF); + var usePongBuffer = (ulong)taskId >= (1UL << 63); + var nodePushContext = (NodePushTaskContext*)context; + //Note that child index is always 1 because we only ever push child B. + BinnedBuildNode(usePongBuffer, subtreeRegionStartIndex, nodePushContext->NodeIndex, subtreeCount, nodePushContext->ParentNodeIndex, 1, nodePushContext->CentroidBounds, nodePushContext->Context, workerIndex, dispatcher); + } + + private static unsafe void BuildNodeForDegeneracy( + Buffer subtrees, Buffer nodes, Buffer metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, + Context* context, out int subtreeCountA, out int subtreeCountB, out int aIndex, out int bIndex) + where TLeaves : unmanaged + where TThreading : unmanaged, IBinnedBuilderThreading + { + //This shouldn't happen unless something is badly wrong with the input; no point in optimizing it. + subtreeCountA = subtrees.Length / 2; + subtreeCountB = subtrees.Length - subtreeCountA; + BoundingBox4 boundsA, boundsB; + boundsA.Min = new Vector4(float.MaxValue); + boundsA.Max = new Vector4(float.MinValue); + boundsB.Min = new Vector4(float.MaxValue); + boundsB.Max = new Vector4(float.MinValue); + int leafCountA = 0, leafCountB = 0; + var boundingBoxes = subtrees.As(); + for (int i = 0; i < subtreeCountA; ++i) { - public Context* Context; - public int NodeIndex; - public int ParentNodeIndex; - public BoundingBox4 CentroidBounds; - //Subtree region start index, subtree count, and usePongBuffer status are all encoded into the task id. + ref var bounds = ref boundingBoxes[i]; + boundsA.Min = Vector4.Min(bounds.Min, boundsA.Min); + boundsA.Max = Vector4.Max(bounds.Max, boundsA.Max); + leafCountA += subtrees[i].LeafCount; } - unsafe static void BinnedBuilderNodeWorker(long taskId, void* context, int workerIndex, IThreadDispatcher dispatcher) - where TLeaves : unmanaged where TThreading : unmanaged, IBinnedBuilderThreading + for (int i = subtreeCountA; i < subtrees.Length; ++i) { - var subtreeRegionStartIndex = (int)taskId; - var subtreeCount = (int)((taskId >> 32) & 0x7FFF_FFFF); - var usePongBuffer = (ulong)taskId >= (1UL << 63); - var nodePushContext = (NodePushTaskContext*)context; - //Note that child index is always 1 because we only ever push child B. - BinnedBuildNode(usePongBuffer, subtreeRegionStartIndex, nodePushContext->NodeIndex, subtreeCount, nodePushContext->ParentNodeIndex, 1, nodePushContext->CentroidBounds, nodePushContext->Context, workerIndex, dispatcher); + ref var bounds = ref boundingBoxes[i]; + boundsB.Min = Vector4.Min(bounds.Min, boundsB.Min); + boundsB.Max = Vector4.Max(bounds.Max, boundsB.Max); + leafCountB += subtrees[i].LeafCount; } + Debug.Assert(parentNodeIndex < 0 || Unsafe.Add(ref context->Nodes[parentNodeIndex].A, childIndexInParent).LeafCount == leafCountA + leafCountB); + //Note that we just use the bounds as centroid bounds. This is a degenerate situation anyway. + BuildNode(boundsA, boundsB, leafCountA, leafCountB, subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, subtreeCountA, subtreeCountB, ref context->Leaves, out aIndex, out bIndex); + } - private static unsafe void BuildNodeForDegeneracy( - Buffer subtrees, Buffer nodes, Buffer metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, - Context* context, out int subtreeCountA, out int subtreeCountB, out int aIndex, out int bIndex) - where TLeaves : unmanaged - where TThreading : unmanaged, IBinnedBuilderThreading - { - //This shouldn't happen unless something is badly wrong with the input; no point in optimizing it. - subtreeCountA = subtrees.Length / 2; - subtreeCountB = subtrees.Length - subtreeCountA; - BoundingBox4 boundsA, boundsB; - boundsA.Min = new Vector4(float.MaxValue); - boundsA.Max = new Vector4(float.MinValue); - boundsB.Min = new Vector4(float.MaxValue); - boundsB.Max = new Vector4(float.MinValue); - int leafCountA = 0, leafCountB = 0; - var boundingBoxes = subtrees.As(); - for (int i = 0; i < subtreeCountA; ++i) - { - ref var bounds = ref boundingBoxes[i]; - boundsA.Min = Vector4.Min(bounds.Min, boundsA.Min); - boundsA.Max = Vector4.Max(bounds.Max, boundsA.Max); - leafCountA += subtrees[i].LeafCount; - } - for (int i = subtreeCountA; i < subtrees.Length; ++i) - { - ref var bounds = ref boundingBoxes[i]; - boundsB.Min = Vector4.Min(bounds.Min, boundsB.Min); - boundsB.Max = Vector4.Max(bounds.Max, boundsB.Max); - leafCountB += subtrees[i].LeafCount; - } - Debug.Assert(parentNodeIndex < 0 || Unsafe.Add(ref context->Nodes[parentNodeIndex].A, childIndexInParent).LeafCount == leafCountA + leafCountB); - //Note that we just use the bounds as centroid bounds. This is a degenerate situation anyway. - BuildNode(boundsA, boundsB, leafCountA, leafCountB, subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, subtreeCountA, subtreeCountB, ref context->Leaves, out aIndex, out bIndex); - } + // Note that degenerate nodes are those which are assumed to have zero-sized centroid bound spans. + // There are other possibilities--NaNs, infinities--which also flow into this, but the usual case is overlapping geometry. + // We pass this along recursively to trigger the degeneracy case at each level. + static unsafe void HandleDegeneracy(Buffer subtrees, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, + bool usePongBuffer, int subtreeRegionStartIndex, int nodeIndex, int subtreeCount, int parentNodeIndex, int childIndexInParent, + BoundingBox4 centroidBounds, Context* context, int workerIndex, IThreadDispatcher dispatcher) + where TLeaves : unmanaged where TThreading : unmanaged, IBinnedBuilderThreading + { - // Note that degenerate nodes are those which are assumed to have zero-sized centroid bound spans. - // There are other possibilities--NaNs, infinities--which also flow into this, but the usual case is overlapping geometry. - // We pass this along recursively to trigger the degeneracy case at each level. - static unsafe void HandleDegeneracy(Buffer subtrees, Buffer boundingBoxes, Buffer nodes, Buffer metanodes, - bool usePongBuffer, int subtreeRegionStartIndex, int nodeIndex, int subtreeCount, int parentNodeIndex, int childIndexInParent, - BoundingBox4 centroidBounds, Context* context, int workerIndex, IThreadDispatcher dispatcher) - where TLeaves : unmanaged where TThreading : unmanaged, IBinnedBuilderThreading - { + BuildNodeForDegeneracy(subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, context, out var subtreeCountA, out var subtreeCountB, out var aIndex, out var bIndex); + if (subtreeCountA > 1) + BinnedBuildNode(usePongBuffer, subtreeRegionStartIndex, aIndex, subtreeCountA, nodeIndex, 0, centroidBounds, context, workerIndex, dispatcher); + if (subtreeCountB > 1) + BinnedBuildNode(usePongBuffer, subtreeRegionStartIndex + subtreeCountA, bIndex, subtreeCountB, nodeIndex, 1, centroidBounds, context, workerIndex, dispatcher); + } - BuildNodeForDegeneracy(subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, context, out var subtreeCountA, out var subtreeCountB, out var aIndex, out var bIndex); - if (subtreeCountA > 1) - BinnedBuildNode(usePongBuffer, subtreeRegionStartIndex, aIndex, subtreeCountA, nodeIndex, 0, centroidBounds, context, workerIndex, dispatcher); - if (subtreeCountB > 1) - BinnedBuildNode(usePongBuffer, subtreeRegionStartIndex + subtreeCountA, bIndex, subtreeCountB, nodeIndex, 1, centroidBounds, context, workerIndex, dispatcher); - } + + static unsafe void HandleMicrosweepDegeneracy(ref TLeaves leaves, + Buffer subtrees, Buffer nodes, Buffer metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, Vector4 centroidMin, Vector4 centroidMax, Context* context, int workerIndex) + where TLeaves : unmanaged where TThreading : unmanaged, IBinnedBuilderThreading + { + BuildNodeForDegeneracy(subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, context, out var subtreeCountA, out var subtreeCountB, out var aIndex, out var bIndex); + if (subtreeCountA > 1) + MicroSweepForBinnedBuilder(centroidMin, centroidMax, ref leaves, subtrees.Slice(subtreeCountA), nodes.Slice(1, subtreeCountA - 1), metanodes.Allocated ? metanodes.Slice(1, subtreeCountA - 1) : metanodes, aIndex, nodeIndex, 0, context, workerIndex); + if (subtreeCountB > 1) + MicroSweepForBinnedBuilder(centroidMin, centroidMax, ref leaves, subtrees.Slice(subtreeCountA, subtreeCountB), nodes.Slice(subtreeCountA, subtreeCountB - 1), metanodes.Allocated ? metanodes.Slice(subtreeCountA, subtreeCountB - 1) : metanodes, bIndex, nodeIndex, 1, context, workerIndex); + } - static unsafe void HandleMicrosweepDegeneracy(ref TLeaves leaves, - Buffer subtrees, Buffer nodes, Buffer metanodes, int nodeIndex, int parentNodeIndex, int childIndexInParent, Vector4 centroidMin, Vector4 centroidMax, Context* context, int workerIndex) - where TLeaves : unmanaged where TThreading : unmanaged, IBinnedBuilderThreading + static unsafe void BinnedBuildNode( + bool usePongBuffer, int subtreeRegionStartIndex, int nodeIndex, int subtreeCount, int parentNodeIndex, int childIndexInParent, + BoundingBox4 centroidBounds, Context* context, int workerIndex, IThreadDispatcher dispatcher) + where TLeaves : unmanaged where TThreading : unmanaged, IBinnedBuilderThreading + { + var subtrees = (usePongBuffer ? context->SubtreesPong : context->SubtreesPing).Slice(subtreeRegionStartIndex, subtreeCount); + var subtreeBinIndices = context->BinIndices.Allocated ? context->BinIndices.Slice(subtreeRegionStartIndex, subtreeCount) : default; + //leaf counts, indices, and bounds are packed together, but it's useful to have a bounds-only representation so that the merging processes don't have to worry about dealing with the fourth lanes. + var boundingBoxes = subtrees.As(); + var nodeCount = subtreeCount - 1; + var nodes = context->Nodes.Slice(nodeIndex, nodeCount); + var metanodes = context->Metanodes.Allocated ? context->Metanodes.Slice(nodeIndex, nodeCount) : context->Metanodes; + if (subtreeCount == 2) { - BuildNodeForDegeneracy(subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, context, out var subtreeCountA, out var subtreeCountB, out var aIndex, out var bIndex); - if (subtreeCountA > 1) - MicroSweepForBinnedBuilder(centroidMin, centroidMax, ref leaves, subtrees.Slice(subtreeCountA), nodes.Slice(1, subtreeCountA - 1), metanodes.Allocated ? metanodes.Slice(1, subtreeCountA - 1) : metanodes, aIndex, nodeIndex, 0, context, workerIndex); - if (subtreeCountB > 1) - MicroSweepForBinnedBuilder(centroidMin, centroidMax, ref leaves, subtrees.Slice(subtreeCountA, subtreeCountB), nodes.Slice(subtreeCountA, subtreeCountB - 1), metanodes.Allocated ? metanodes.Slice(subtreeCountA, subtreeCountB - 1) : metanodes, bIndex, nodeIndex, 1, context, workerIndex); + Debug.Assert(parentNodeIndex < 0 || Unsafe.Add(ref context->Nodes[parentNodeIndex].A, childIndexInParent).LeafCount == subtrees[0].LeafCount + subtrees[1].LeafCount); + BuildNode(boundingBoxes[0], boundingBoxes[1], subtrees[0].LeafCount, subtrees[1].LeafCount, subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, 1, 1, ref context->Leaves, out _, out _); + return; } - - - static unsafe void BinnedBuildNode( - bool usePongBuffer, int subtreeRegionStartIndex, int nodeIndex, int subtreeCount, int parentNodeIndex, int childIndexInParent, - BoundingBox4 centroidBounds, Context* context, int workerIndex, IThreadDispatcher dispatcher) - where TLeaves : unmanaged where TThreading : unmanaged, IBinnedBuilderThreading + var targetTaskCount = typeof(TThreading) == typeof(SingleThreaded) ? 1 : + ((MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref context->Threading))->GetTargetTaskCountForInnerLoop(subtreeCount); + if (nodeIndex == 0) { - var subtrees = (usePongBuffer ? context->SubtreesPong : context->SubtreesPing).Slice(subtreeRegionStartIndex, subtreeCount); - var subtreeBinIndices = context->BinIndices.Allocated ? context->BinIndices.Slice(subtreeRegionStartIndex, subtreeCount) : default; - //leaf counts, indices, and bounds are packed together, but it's useful to have a bounds-only representation so that the merging processes don't have to worry about dealing with the fourth lanes. - var boundingBoxes = subtrees.As(); - var nodeCount = subtreeCount - 1; - var nodes = context->Nodes.Slice(nodeIndex, nodeCount); - var metanodes = context->Metanodes.Allocated ? context->Metanodes.Slice(nodeIndex, nodeCount) : context->Metanodes; - if (subtreeCount == 2) - { - Debug.Assert(parentNodeIndex < 0 || Unsafe.Add(ref context->Nodes[parentNodeIndex].A, childIndexInParent).LeafCount == subtrees[0].LeafCount + subtrees[1].LeafCount); - BuildNode(boundingBoxes[0], boundingBoxes[1], subtrees[0].LeafCount, subtrees[1].LeafCount, subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, 1, 1, ref context->Leaves, out _, out _); - return; - } - var targetTaskCount = typeof(TThreading) == typeof(SingleThreaded) ? 1 : - ((MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref context->Threading))->GetTargetTaskCountForInnerLoop(subtreeCount); - if (nodeIndex == 0) + //The first node doesn't have a parent, and so isn't given centroid bounds. We have to compute them. + var useST = true; + if (typeof(TThreading) != typeof(SingleThreaded)) { - //The first node doesn't have a parent, and so isn't given centroid bounds. We have to compute them. - var useST = true; - if (typeof(TThreading) != typeof(SingleThreaded)) - { - var mtContext = (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref context->Threading); - var taskData = new SharedTaskData(mtContext->Workers.Length, 0, subtrees.Length, MinimumSubtreesPerThreadForCentroidPrepass, mtContext->GetTargetTaskCountForInnerLoop(subtreeCount)); - if (taskData.TaskCount > 1) - { - centroidBounds = MultithreadedCentroidPrepass( - mtContext, boundingBoxes, taskData, workerIndex, dispatcher); - useST = false; - } - } - if (useST) + var mtContext = (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref context->Threading); + var taskData = new SharedTaskData(mtContext->Workers.Length, 0, subtrees.Length, MinimumSubtreesPerThreadForCentroidPrepass, mtContext->GetTargetTaskCountForInnerLoop(subtreeCount)); + if (taskData.TaskCount > 1) { - centroidBounds = ComputeCentroidBounds(boundingBoxes); + centroidBounds = MultithreadedCentroidPrepass( + mtContext, boundingBoxes, taskData, workerIndex, dispatcher); + useST = false; } } - var centroidSpan = centroidBounds.Max - centroidBounds.Min; - var axisIsDegenerate = Vector128.LessThanOrEqual(centroidSpan.AsVector128(), Vector128.Create(1e-12f)); - if ((Vector128.ExtractMostSignificantBits(axisIsDegenerate) & 0b111) == 0b111) + if (useST) { - //This node is completely degenerate; there is no 'good' ordering of the children. Pick a split in the middle and shrug. - //This shouldn't happen unless something is badly wrong with the input; no point in optimizing it. - HandleDegeneracy(subtrees, boundingBoxes, nodes, metanodes, usePongBuffer, subtreeRegionStartIndex, nodeIndex, subtreeCount, parentNodeIndex, childIndexInParent, centroidBounds, context, workerIndex, dispatcher); - return; + centroidBounds = ComputeCentroidBounds(boundingBoxes); } + } + var centroidSpan = centroidBounds.Max - centroidBounds.Min; + var axisIsDegenerate = Vector128.LessThanOrEqual(centroidSpan.AsVector128(), Vector128.Create(1e-12f)); + if ((Vector128.ExtractMostSignificantBits(axisIsDegenerate) & 0b111) == 0b111) + { + //This node is completely degenerate; there is no 'good' ordering of the children. Pick a split in the middle and shrug. + //This shouldn't happen unless something is badly wrong with the input; no point in optimizing it. + HandleDegeneracy(subtrees, boundingBoxes, nodes, metanodes, usePongBuffer, subtreeRegionStartIndex, nodeIndex, subtreeCount, parentNodeIndex, childIndexInParent, centroidBounds, context, workerIndex, dispatcher); + return; + } - //Note that we don't bother even trying to internally multithread microsweeps. They *should* be small, and should only show up deeper in the recursion process. - if (subtreeCount <= context->MicrosweepThreshold) - { - MicroSweepForBinnedBuilder(centroidBounds.Min, centroidBounds.Max, ref context->Leaves, subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, context, workerIndex); - return; - } + //Note that we don't bother even trying to internally multithread microsweeps. They *should* be small, and should only show up deeper in the recursion process. + if (subtreeCount <= context->MicrosweepThreshold) + { + MicroSweepForBinnedBuilder(centroidBounds.Min, centroidBounds.Max, ref context->Leaves, subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, context, workerIndex); + return; + } - var useX = centroidSpan.X > centroidSpan.Y && centroidSpan.X > centroidSpan.Z; - var useY = centroidSpan.Y > centroidSpan.Z; - //These will be used conditionally based on what hardware acceleration is available. Pretty minor detail. - var permuteMask = Vector128.Create(useX ? 0 : useY ? 1 : 2, 0, 0, 0); - var axisIndex = useX ? 0 : useY ? 1 : 2; + var useX = centroidSpan.X > centroidSpan.Y && centroidSpan.X > centroidSpan.Z; + var useY = centroidSpan.Y > centroidSpan.Z; + //These will be used conditionally based on what hardware acceleration is available. Pretty minor detail. + var permuteMask = Vector128.Create(useX ? 0 : useY ? 1 : 2, 0, 0, 0); + var axisIndex = useX ? 0 : useY ? 1 : 2; - var binCount = int.Min(context->MaximumBinCount, int.Max((int)(subtreeCount * context->LeafToBinMultiplier), context->MinimumBinCount)); + var binCount = int.Min(context->MaximumBinCount, int.Max((int)(subtreeCount * context->LeafToBinMultiplier), context->MinimumBinCount)); - var offsetToBinIndex = new Vector4(binCount) / centroidSpan; - //Avoid letting NaNs into the offsetToBinIndex scale. - offsetToBinIndex = Vector128.ConditionalSelect(axisIsDegenerate, Vector128.Zero, offsetToBinIndex.AsVector128()).AsVector4(); + var offsetToBinIndex = new Vector4(binCount) / centroidSpan; + //Avoid letting NaNs into the offsetToBinIndex scale. + offsetToBinIndex = Vector128.ConditionalSelect(axisIsDegenerate, Vector128.Zero, offsetToBinIndex.AsVector128()).AsVector4(); - var maximumBinIndex = new Vector4(binCount - 1); - context->Threading.GetBins(workerIndex, out var binBoundingBoxes, out var binCentroidBoundingBoxes, out var binBoundingBoxesScan, out var binCentroidBoundingBoxesScan, out var binLeafCounts); - Debug.Assert(binBoundingBoxes.Length >= binCount); - for (int i = 0; i < binCount; ++i) + var maximumBinIndex = new Vector4(binCount - 1); + context->Threading.GetBins(workerIndex, out var binBoundingBoxes, out var binCentroidBoundingBoxes, out var binBoundingBoxesScan, out var binCentroidBoundingBoxesScan, out var binLeafCounts); + Debug.Assert(binBoundingBoxes.Length >= binCount); + for (int i = 0; i < binCount; ++i) + { + ref var binBounds = ref binBoundingBoxes[i]; + binBounds.Min = new Vector4(float.MaxValue); + binBounds.Max = new Vector4(float.MinValue); + ref var binCentroidBounds = ref binCentroidBoundingBoxes[i]; + binCentroidBounds.Min = new Vector4(float.MaxValue); + binCentroidBounds.Max = new Vector4(float.MinValue); + binLeafCounts[i] = 0; + } + var useSTForBinning = true; + if (typeof(TThreading) != typeof(SingleThreaded)) + { + var mtContext = (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref context->Threading); + var taskData = new SharedTaskData(mtContext->Workers.Length, 0, subtrees.Length, MinimumSubtreesPerThreadForBinning, mtContext->GetTargetTaskCountForInnerLoop(subtreeCount)); + if (taskData.TaskCount > 1) { - ref var binBounds = ref binBoundingBoxes[i]; - binBounds.Min = new Vector4(float.MaxValue); - binBounds.Max = new Vector4(float.MinValue); - ref var binCentroidBounds = ref binCentroidBoundingBoxes[i]; - binCentroidBounds.Min = new Vector4(float.MaxValue); - binCentroidBounds.Max = new Vector4(float.MinValue); - binLeafCounts[i] = 0; + MultithreadedBinSubtrees( + (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref context->Threading), + centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, subtrees, subtreeBinIndices, binCount, taskData, workerIndex, dispatcher); + useSTForBinning = false; } - var useSTForBinning = true; - if (typeof(TThreading) != typeof(SingleThreaded)) + } + if (useSTForBinning) + { + //If the subtree bin indices buffer isn't available, then the binning process can't write to them! That'll happen if: + //single threaded execution, + //no bufferpool provided, + //tree size too large for stack allocation. + if (subtreeBinIndices.Allocated) + BinSubtrees(centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, subtrees, binBoundingBoxes, binCentroidBoundingBoxes, binLeafCounts, subtreeBinIndices); + else + BinSubtrees(centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, subtrees, binBoundingBoxes, binCentroidBoundingBoxes, binLeafCounts, subtreeBinIndices); + } + //Identify the split index by examining the SAH of very split option. + //Premerge from left to right so we have a sorta-summed area table to cheaply look up all possible child A bounds as we scan. + binBoundingBoxesScan[0] = binBoundingBoxes[0]; + binCentroidBoundingBoxesScan[0] = binCentroidBoundingBoxes[0]; + int totalLeafCount = binLeafCounts[0]; + for (int i = 1; i < binCount; ++i) + { + var previousIndex = i - 1; + ref var bounds = ref binBoundingBoxes[i]; + ref var scanBounds = ref binBoundingBoxesScan[i]; + ref var previousScanBounds = ref binBoundingBoxesScan[previousIndex]; + scanBounds.Min = Vector4.Min(bounds.Min, previousScanBounds.Min); + scanBounds.Max = Vector4.Max(bounds.Max, previousScanBounds.Max); + ref var binCentroidBoundingBox = ref binCentroidBoundingBoxes[i]; + ref var binCentroidBoundingBoxScan = ref binCentroidBoundingBoxesScan[i]; + ref var previousCentroidBoundingBoxScan = ref binCentroidBoundingBoxesScan[previousIndex]; + binCentroidBoundingBoxScan.Min = Vector4.Min(binCentroidBoundingBox.Min, previousCentroidBoundingBoxScan.Min); + binCentroidBoundingBoxScan.Max = Vector4.Max(binCentroidBoundingBox.Max, previousCentroidBoundingBoxScan.Max); + totalLeafCount += binLeafCounts[i]; + } + var leftBoundsX = binBoundingBoxes[0]; + Debug.Assert( + leftBoundsX.Min.X > float.MinValue && leftBoundsX.Min.Y > float.MinValue && leftBoundsX.Min.Z > float.MinValue, + "Bin 0 should have been updated in all cases because it is aligned with the minimum bin, and the centroid span isn't degenerate."); + + float bestSAH = float.MaxValue; + int splitIndex = 1; + //The split index is going to end up in child B. + var lastBinIndex = binCount - 1; + var accumulatedBoundingBoxB = binBoundingBoxes[lastBinIndex]; + var accumulatedCentroidBoundingBoxB = binCentroidBoundingBoxes[lastBinIndex]; + BoundingBox4 bestBoundingBoxB, bestCentroidBoundingBoxB; + bestBoundingBoxB = accumulatedBoundingBoxB; + bestCentroidBoundingBoxB = accumulatedCentroidBoundingBoxB; + int accumulatedLeafCountB = binLeafCounts[lastBinIndex]; + int bestLeafCountB = 0; + for (int splitIndexCandidate = lastBinIndex; splitIndexCandidate >= 1; --splitIndexCandidate) + { + var previousIndex = splitIndexCandidate - 1; + var sahCandidate = ComputeBoundsMetric(binBoundingBoxesScan[previousIndex]) * (totalLeafCount - accumulatedLeafCountB) + ComputeBoundsMetric(accumulatedBoundingBoxB) * accumulatedLeafCountB; + if (sahCandidate < bestSAH) + { + bestSAH = sahCandidate; + splitIndex = splitIndexCandidate; + bestBoundingBoxB = accumulatedBoundingBoxB; + bestLeafCountB = accumulatedLeafCountB; + bestCentroidBoundingBoxB = accumulatedCentroidBoundingBoxB; + } + ref var bounds = ref binBoundingBoxes[previousIndex]; + accumulatedBoundingBoxB.Min = Vector4.Min(bounds.Min, accumulatedBoundingBoxB.Min); + accumulatedBoundingBoxB.Max = Vector4.Max(bounds.Max, accumulatedBoundingBoxB.Max); + ref var centroidBoundsForBin = ref binCentroidBoundingBoxes[previousIndex]; + accumulatedCentroidBoundingBoxB.Min = Vector4.Min(centroidBoundsForBin.Min, accumulatedCentroidBoundingBoxB.Min); + accumulatedCentroidBoundingBoxB.Max = Vector4.Max(centroidBoundsForBin.Max, accumulatedCentroidBoundingBoxB.Max); + accumulatedLeafCountB += binLeafCounts[previousIndex]; + } + if (bestLeafCountB == 0 || bestLeafCountB == totalLeafCount || bestSAH == float.MaxValue || float.IsNaN(bestSAH) || float.IsInfinity(bestSAH)) + { + //Some form of major problem detected! Fall back to a degenerate split. + HandleDegeneracy(subtrees, boundingBoxes, nodes, metanodes, usePongBuffer, subtreeRegionStartIndex, nodeIndex, subtreeCount, parentNodeIndex, childIndexInParent, centroidBounds, context, workerIndex, dispatcher); + return; + } + + var subtreeCountB = 0; + var subtreeCountA = 0; + var bestBoundingBoxA = binBoundingBoxesScan[splitIndex - 1]; + var bestCentroidBoundingBoxA = binCentroidBoundingBoxesScan[splitIndex - 1]; + + //Split the indices/bounds into two halves for the children to operate on. + if (context->SubtreesPong.Allocated) + { + Debug.Assert(subtreeBinIndices.Allocated); + //If the current buffer is pong, then write to ping, and vice versa. + var subtreesNext = (usePongBuffer ? context->SubtreesPing : context->SubtreesPong).Slice(subtreeRegionStartIndex, subtreeCount); + + var useSTForPartitioning = true; + //TODO: Note that the current multithreaded partitioning implementation is nondeterministic. + //Because of microsweeps/terminal node ordering, this can result in nondeterministic tree topology. + //See https://github.com/bepu/bepuphysics2/issues/276 for more information (and how to improve this in the future if valuable). + //For now, if the user wants determinism, we just use the single threaded path for partitioning. + if (typeof(TThreading) != typeof(SingleThreaded) && !context->Deterministic) { var mtContext = (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref context->Threading); - var taskData = new SharedTaskData(mtContext->Workers.Length, 0, subtrees.Length, MinimumSubtreesPerThreadForBinning, mtContext->GetTargetTaskCountForInnerLoop(subtreeCount)); + var taskData = new SharedTaskData(mtContext->Workers.Length, 0, subtrees.Length, MinimumSubtreesPerThreadForPartitioning, mtContext->GetTargetTaskCountForInnerLoop(subtreeCount)); if (taskData.TaskCount > 1) { - MultithreadedBinSubtrees( + (subtreeCountA, subtreeCountB) = MultithreadedPartition( (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref context->Threading), - centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, subtrees, subtreeBinIndices, binCount, taskData, workerIndex, dispatcher); - useSTForBinning = false; + subtrees, subtreesNext, subtreeBinIndices, splitIndex, taskData, workerIndex, dispatcher); + useSTForPartitioning = false; } } - if (useSTForBinning) - { - //If the subtree bin indices buffer isn't available, then the binning process can't write to them! That'll happen if: - //single threaded execution, - //no bufferpool provided, - //tree size too large for stack allocation. - if (subtreeBinIndices.Allocated) - BinSubtrees(centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, subtrees, binBoundingBoxes, binCentroidBoundingBoxes, binLeafCounts, subtreeBinIndices); - else - BinSubtrees(centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, subtrees, binBoundingBoxes, binCentroidBoundingBoxes, binLeafCounts, subtreeBinIndices); - } - //Identify the split index by examining the SAH of very split option. - //Premerge from left to right so we have a sorta-summed area table to cheaply look up all possible child A bounds as we scan. - binBoundingBoxesScan[0] = binBoundingBoxes[0]; - binCentroidBoundingBoxesScan[0] = binCentroidBoundingBoxes[0]; - int totalLeafCount = binLeafCounts[0]; - for (int i = 1; i < binCount; ++i) - { - var previousIndex = i - 1; - ref var bounds = ref binBoundingBoxes[i]; - ref var scanBounds = ref binBoundingBoxesScan[i]; - ref var previousScanBounds = ref binBoundingBoxesScan[previousIndex]; - scanBounds.Min = Vector4.Min(bounds.Min, previousScanBounds.Min); - scanBounds.Max = Vector4.Max(bounds.Max, previousScanBounds.Max); - ref var binCentroidBoundingBox = ref binCentroidBoundingBoxes[i]; - ref var binCentroidBoundingBoxScan = ref binCentroidBoundingBoxesScan[i]; - ref var previousCentroidBoundingBoxScan = ref binCentroidBoundingBoxesScan[previousIndex]; - binCentroidBoundingBoxScan.Min = Vector4.Min(binCentroidBoundingBox.Min, previousCentroidBoundingBoxScan.Min); - binCentroidBoundingBoxScan.Max = Vector4.Max(binCentroidBoundingBox.Max, previousCentroidBoundingBoxScan.Max); - totalLeafCount += binLeafCounts[i]; - } - var leftBoundsX = binBoundingBoxes[0]; - Debug.Assert( - leftBoundsX.Min.X > float.MinValue && leftBoundsX.Min.Y > float.MinValue && leftBoundsX.Min.Z > float.MinValue, - "Bin 0 should have been updated in all cases because it is aligned with the minimum bin, and the centroid span isn't degenerate."); - - float bestSAH = float.MaxValue; - int splitIndex = 1; - //The split index is going to end up in child B. - var lastBinIndex = binCount - 1; - var accumulatedBoundingBoxB = binBoundingBoxes[lastBinIndex]; - var accumulatedCentroidBoundingBoxB = binCentroidBoundingBoxes[lastBinIndex]; - BoundingBox4 bestBoundingBoxB, bestCentroidBoundingBoxB; - bestBoundingBoxB = accumulatedBoundingBoxB; - bestCentroidBoundingBoxB = accumulatedCentroidBoundingBoxB; - int accumulatedLeafCountB = binLeafCounts[lastBinIndex]; - int bestLeafCountB = 0; - for (int splitIndexCandidate = lastBinIndex; splitIndexCandidate >= 1; --splitIndexCandidate) + if (useSTForPartitioning) { - var previousIndex = splitIndexCandidate - 1; - var sahCandidate = ComputeBoundsMetric(binBoundingBoxesScan[previousIndex]) * (totalLeafCount - accumulatedLeafCountB) + ComputeBoundsMetric(accumulatedBoundingBoxB) * accumulatedLeafCountB; - if (sahCandidate < bestSAH) + for (int i = 0; i < subtreeCount; ++i) { - bestSAH = sahCandidate; - splitIndex = splitIndexCandidate; - bestBoundingBoxB = accumulatedBoundingBoxB; - bestLeafCountB = accumulatedLeafCountB; - bestCentroidBoundingBoxB = accumulatedCentroidBoundingBoxB; + var targetIndex = subtreeBinIndices[i] >= splitIndex ? subtreeCount - ++subtreeCountB : subtreeCountA++; + subtreesNext[targetIndex] = subtrees[i]; } - ref var bounds = ref binBoundingBoxes[previousIndex]; - accumulatedBoundingBoxB.Min = Vector4.Min(bounds.Min, accumulatedBoundingBoxB.Min); - accumulatedBoundingBoxB.Max = Vector4.Max(bounds.Max, accumulatedBoundingBoxB.Max); - ref var centroidBoundsForBin = ref binCentroidBoundingBoxes[previousIndex]; - accumulatedCentroidBoundingBoxB.Min = Vector4.Min(centroidBoundsForBin.Min, accumulatedCentroidBoundingBoxB.Min); - accumulatedCentroidBoundingBoxB.Max = Vector4.Max(centroidBoundsForBin.Max, accumulatedCentroidBoundingBoxB.Max); - accumulatedLeafCountB += binLeafCounts[previousIndex]; - } - if (bestLeafCountB == 0 || bestLeafCountB == totalLeafCount || bestSAH == float.MaxValue || float.IsNaN(bestSAH) || float.IsInfinity(bestSAH)) - { - //Some form of major problem detected! Fall back to a degenerate split. - HandleDegeneracy(subtrees, boundingBoxes, nodes, metanodes, usePongBuffer, subtreeRegionStartIndex, nodeIndex, subtreeCount, parentNodeIndex, childIndexInParent, centroidBounds, context, workerIndex, dispatcher); - return; } - - var subtreeCountB = 0; - var subtreeCountA = 0; - var bestBoundingBoxA = binBoundingBoxesScan[splitIndex - 1]; - var bestCentroidBoundingBoxA = binCentroidBoundingBoxesScan[splitIndex - 1]; - - //Split the indices/bounds into two halves for the children to operate on. - if (context->SubtreesPong.Allocated) + subtrees = subtreesNext; + usePongBuffer = !usePongBuffer; + } + else + { + //There is no pong buffer allocated. We allow this for lower memory allocation, but the implementation is strictly sequential and slower. + while (subtreeCountA + subtreeCountB < subtreeCount) { - Debug.Assert(subtreeBinIndices.Allocated); - //If the current buffer is pong, then write to ping, and vice versa. - var subtreesNext = (usePongBuffer ? context->SubtreesPing : context->SubtreesPong).Slice(subtreeRegionStartIndex, subtreeCount); - - var useSTForPartitioning = true; - //TODO: Note that the current multithreaded partitioning implementation is nondeterministic. - //Because of microsweeps/terminal node ordering, this can result in nondeterministic tree topology. - //See https://github.com/bepu/bepuphysics2/issues/276 for more information (and how to improve this in the future if valuable). - //For now, if the user wants determinism, we just use the single threaded path for partitioning. - if (typeof(TThreading) != typeof(SingleThreaded) && !context->Deterministic) + ref var box = ref boundingBoxes[subtreeCountA]; + var binIndex = ComputeBinIndex(centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, box); + if (binIndex >= splitIndex) { - var mtContext = (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref context->Threading); - var taskData = new SharedTaskData(mtContext->Workers.Length, 0, subtrees.Length, MinimumSubtreesPerThreadForPartitioning, mtContext->GetTargetTaskCountForInnerLoop(subtreeCount)); - if (taskData.TaskCount > 1) + //Belongs to B. Swap it. + var targetIndex = subtreeCount - subtreeCountB - 1; + if (Vector256.IsHardwareAccelerated) { - (subtreeCountA, subtreeCountB) = MultithreadedPartition( - (MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref context->Threading), - subtrees, subtreesNext, subtreeBinIndices, splitIndex, taskData, workerIndex, dispatcher); - useSTForPartitioning = false; + var targetMemory = (byte*)(subtrees.Memory + targetIndex); + var aCountMemory = (byte*)(subtrees.Memory + subtreeCountA); + var targetVector = Vector256.Load(targetMemory); + var aCountVector = Vector256.Load(aCountMemory); + Vector256.Store(aCountVector, targetMemory); + Vector256.Store(targetVector, aCountMemory); } - } - if (useSTForPartitioning) - { - for (int i = 0; i < subtreeCount; ++i) + else { - var targetIndex = subtreeBinIndices[i] >= splitIndex ? subtreeCount - ++subtreeCountB : subtreeCountA++; - subtreesNext[targetIndex] = subtrees[i]; + Helpers.Swap(ref subtrees[targetIndex], ref subtrees[subtreeCountA]); } + ++subtreeCountB; + //(Note that we still need to examine what we just swapped into the slot! It may belong to B too!) } - subtrees = subtreesNext; - usePongBuffer = !usePongBuffer; - } - else - { - //There is no pong buffer allocated. We allow this for lower memory allocation, but the implementation is strictly sequential and slower. - while (subtreeCountA + subtreeCountB < subtreeCount) + else { - ref var box = ref boundingBoxes[subtreeCountA]; - var binIndex = ComputeBinIndex(centroidBounds.Min, useX, useY, permuteMask, axisIndex, offsetToBinIndex, maximumBinIndex, box); - if (binIndex >= splitIndex) - { - //Belongs to B. Swap it. - var targetIndex = subtreeCount - subtreeCountB - 1; - if (Vector256.IsHardwareAccelerated) - { - var targetMemory = (byte*)(subtrees.Memory + targetIndex); - var aCountMemory = (byte*)(subtrees.Memory + subtreeCountA); - var targetVector = Vector256.Load(targetMemory); - var aCountVector = Vector256.Load(aCountMemory); - Vector256.Store(aCountVector, targetMemory); - Vector256.Store(targetVector, aCountMemory); - } - else - { - Helpers.Swap(ref subtrees[targetIndex], ref subtrees[subtreeCountA]); - } - ++subtreeCountB; - //(Note that we still need to examine what we just swapped into the slot! It may belong to B too!) - } - else - { - //Belongs to A, no movement necessary. - ++subtreeCountA; - } + //Belongs to A, no movement necessary. + ++subtreeCountA; } } - var leafCountB = bestLeafCountB; - var leafCountA = totalLeafCount - leafCountB; - Debug.Assert(subtreeCountA + subtreeCountB == subtreeCount); - Debug.Assert(parentNodeIndex < 0 || Unsafe.Add(ref context->Nodes[parentNodeIndex].A, childIndexInParent).LeafCount == leafCountA + leafCountB); - BuildNode(bestBoundingBoxA, bestBoundingBoxB, leafCountA, leafCountB, subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, subtreeCountA, subtreeCountB, ref context->Leaves, out var nodeChildIndexA, out var nodeChildIndexB); - - var targetNodeTaskCount = typeof(TThreading) == typeof(SingleThreaded) ? 1 : - ((MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref context->Threading))->GetTargetTaskCountForNodes(subtreeCount); - var shouldPushBOntoMultithreadedQueue = targetNodeTaskCount > 1 && subtreeCountA >= MinimumSubtreesPerThreadForNodeJob && subtreeCountB >= MinimumSubtreesPerThreadForNodeJob; - ContinuationHandle nodeBContinuation = default; - if (shouldPushBOntoMultithreadedQueue) + } + var leafCountB = bestLeafCountB; + var leafCountA = totalLeafCount - leafCountB; + Debug.Assert(subtreeCountA + subtreeCountB == subtreeCount); + Debug.Assert(parentNodeIndex < 0 || Unsafe.Add(ref context->Nodes[parentNodeIndex].A, childIndexInParent).LeafCount == leafCountA + leafCountB); + BuildNode(bestBoundingBoxA, bestBoundingBoxB, leafCountA, leafCountB, subtrees, nodes, metanodes, nodeIndex, parentNodeIndex, childIndexInParent, subtreeCountA, subtreeCountB, ref context->Leaves, out var nodeChildIndexA, out var nodeChildIndexB); + + var targetNodeTaskCount = typeof(TThreading) == typeof(SingleThreaded) ? 1 : + ((MultithreadBinnedBuildContext*)Unsafe.AsPointer(ref context->Threading))->GetTargetTaskCountForNodes(subtreeCount); + var shouldPushBOntoMultithreadedQueue = targetNodeTaskCount > 1 && subtreeCountA >= MinimumSubtreesPerThreadForNodeJob && subtreeCountB >= MinimumSubtreesPerThreadForNodeJob; + ContinuationHandle nodeBContinuation = default; + if (shouldPushBOntoMultithreadedQueue) + { + //Both of the children are large. Push child B onto the multithreaded execution stack so it can run at the same time as child A (potentially). + Debug.Assert(MinimumSubtreesPerThreadForNodeJob > 1, "The job threshold for a new node should be large enough that there's no need for a subtreeCountB > 1 test."); + ref var threading = ref Unsafe.As(ref context->Threading); + //Allocate the parameters to send to the worker on the local stack. Note that we have to preserve the stack for this to work; see the later WaitForCompletion. + NodePushTaskContext nodePushContext; + nodePushContext.Context = context; + nodePushContext.NodeIndex = nodeChildIndexB; + nodePushContext.ParentNodeIndex = nodeIndex; + nodePushContext.CentroidBounds = bestCentroidBoundingBoxB; + //Note that we use the task id to store subtree start, subtree count, and the pong buffer flag. Don't have to do that, but no reason not to use it. + Debug.Assert((uint)subtreeCountB < (1u << 31), "The task id encodes start, count, and a pong flag, so we don't have room for a full 32 bits of count."); + var task = new Task(&BinnedBuilderNodeWorker, &nodePushContext, (long)(subtreeRegionStartIndex + subtreeCountA) | ((long)subtreeCountB << 32) | (usePongBuffer ? 1L << 63 : 0)); + nodeBContinuation = threading.TaskStack->AllocateContinuationAndPush(new Span(&task, 1), workerIndex, dispatcher, 0); + } + if (subtreeCountA > 1) + BinnedBuildNode(usePongBuffer, subtreeRegionStartIndex, nodeChildIndexA, subtreeCountA, nodeIndex, 0, bestCentroidBoundingBoxA, context, workerIndex, dispatcher); + if (!shouldPushBOntoMultithreadedQueue && subtreeCountB > 1) + BinnedBuildNode(usePongBuffer, subtreeRegionStartIndex + subtreeCountA, nodeChildIndexB, subtreeCountB, nodeIndex, 1, bestCentroidBoundingBoxB, context, workerIndex, dispatcher); + if (shouldPushBOntoMultithreadedQueue) + { + //We want to keep the stack at this level alive until the memory we allocated for the node push completes. + //Note that WaitForCompletion will execute pending work; this isn't just busywaiting the current thread. + //In addition to letting us use the local stack to store some arguments for the other thread, this wait means that all children have completed when this function returns. + //That makes knowing when to stop the queue easier. + Debug.Assert(nodeBContinuation.Initialized); + Unsafe.As(ref context->Threading).TaskStack->WaitForCompletion(nodeBContinuation, workerIndex, dispatcher); + } + } + + /// + /// Runs a binned build across the input buffer. + /// + /// Subtrees (either leaves or nodes) to run the builder over. The builder may make in-place modifications to the input buffer; the input buffer should not be assumed to be in a valid state after the builder runs. + /// A parallel buffer to subtrees which is used as a scratch buffer during execution. If a default initialized buffer is provided, a slower sequential in-place fallback will be used. + /// Buffer holding the nodes created by the build process. + /// Nodes are created in a depth first ordering with respect to the input buffer. + /// Buffer holding the metanodes created by the build process. + /// Metanodes, like nodes, are created in a depth first ordering with respect to the input buffer. + /// Metanodes are in the same order and in the same slots; they simply contain data about nodes that most traversals don't need to know about. + /// Buffer holding the leaf references created by the build process. + /// The indices written by the build process are those defined in the inputs; any that is negative is encoded according to and points into the leaf buffer. + /// If a default-valued (unallocated) buffer is passed in, the binned builder will ignore leaves. + /// Buffer to be used for caching bin indices during execution. If subtreesPong is defined, binIndices must also be defined, and vice versa. + /// Thread dispatcher used to accelerate the build process. + /// Task stack being used to run the build process, if any. + /// If provided, the builder assumes the refinement is running within an existing multithreaded dispatch and will not call IThreadDispatcher.DispatchWorkers. + /// If null, the builder will create its own task stack and call IThreadDispatcher.DispatchWorkers internally. + /// Index of the current worker. + /// Number of workers that may be used in the builder. This should span all worker indices that may contribute to the build process even only a subset are expected to be used at any one time. + /// Number of tasks to try to use in the builder. + /// Buffer pool used to preallocate temporary resources for building. + /// Minimum number of bins the builder should use per node. + /// Maximum number of bins the builder should use per node. Must be no higher than 255. + /// Multiplier to apply to the subtree count within a node to decide the bin count. Resulting value will then be clamped by the minimum/maximum bin counts. + /// Threshold at or under which the binned builder resorts to local counting sort sweeps. + /// Whether to force determinism at a slightly higher cost when using internally multithreaded execution. + /// If the build is single threaded, it is already deterministic and this flag has no effect. + static unsafe void BinnedBuilderInternal(Buffer subtrees, Buffer subtreesPong, Buffer nodes, Buffer metanodes, Buffer leaves, Buffer binIndices, + IThreadDispatcher dispatcher, TaskStack* taskStackPointer, int workerIndex, int workerCount, int targetTaskCount, BufferPool pool, int minimumBinCount, int maximumBinCount, float leafToBinMultiplier, int microsweepThreshold, bool deterministic) + { + var subtreeCount = subtrees.Length; + if (nodes.Length < subtreeCount - 1) + throw new ArgumentException($"The nodes buffer is too small to hold all the nodes that will be necessary for the input subtrees."); + if (maximumBinCount > 255) + throw new ArgumentException($"Maximum bin count must fit in a byte (maximum of 255)."); + if (subtreesPong.Allocated != binIndices.Allocated) + throw new ArgumentException("The parameters subtreesPong and binIndices must both be allocated or unallocated."); + if (subtreeCount == 0) + return; + if (subtreeCount == 1) + { + //If there's only one leaf, the tree has a special format: the root node has only one child. + ref var root = ref nodes[0]; + root.A = subtrees[0]; + root.B = default; + return; + } + nodes = nodes.Slice(subtreeCount - 1); + + //Don't let the user pick values that will just cause an explosion. + Debug.Assert(minimumBinCount >= 2 && maximumBinCount >= 2, "At least two bins are required. In release mode, this will be clamped up to 2, but where did lower values come from?"); + minimumBinCount = int.Max(2, minimumBinCount); + maximumBinCount = int.Max(2, maximumBinCount); + //The microsweep uses the same resources as the bin allocations, so expand to hold whichever is larger. + var allocatedBinCount = int.Max(maximumBinCount, microsweepThreshold); + + + if (dispatcher == null && taskStackPointer == null) + { + //Use the single threaded path. + var allocatedByteCount = allocatedBinCount * 4 * sizeof(BoundingBox4) + allocatedBinCount * sizeof(int); + var binBoundsMemoryAllocation = stackalloc byte[allocatedByteCount + 32]; + //Should be basically irrelevant, but just in case it's not on some platform, align the allocation. + binBoundsMemoryAllocation = (byte*)(((ulong)binBoundsMemoryAllocation + 31ul) & (~31ul)); + var binBoundsMemory = new Buffer(binBoundsMemoryAllocation, allocatedByteCount); + + var threading = new SingleThreaded(binBoundsMemory, allocatedBinCount); + if (leaves.Allocated) { - //Both of the children are large. Push child B onto the multithreaded execution stack so it can run at the same time as child A (potentially). - Debug.Assert(MinimumSubtreesPerThreadForNodeJob > 1, "The job threshold for a new node should be large enough that there's no need for a subtreeCountB > 1 test."); - ref var threading = ref Unsafe.As(ref context->Threading); - //Allocate the parameters to send to the worker on the local stack. Note that we have to preserve the stack for this to work; see the later WaitForCompletion. - NodePushTaskContext nodePushContext; - nodePushContext.Context = context; - nodePushContext.NodeIndex = nodeChildIndexB; - nodePushContext.ParentNodeIndex = nodeIndex; - nodePushContext.CentroidBounds = bestCentroidBoundingBoxB; - //Note that we use the task id to store subtree start, subtree count, and the pong buffer flag. Don't have to do that, but no reason not to use it. - Debug.Assert((uint)subtreeCountB < (1u << 31), "The task id encodes start, count, and a pong flag, so we don't have room for a full 32 bits of count."); - var task = new Task(&BinnedBuilderNodeWorker, &nodePushContext, (long)(subtreeRegionStartIndex + subtreeCountA) | ((long)subtreeCountB << 32) | (usePongBuffer ? 1L << 63 : 0)); - nodeBContinuation = threading.TaskStack->AllocateContinuationAndPush(new Span(&task, 1), workerIndex, dispatcher, 0); + var context = new Context, SingleThreaded>( + minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold, deterministic, + subtrees, default, leaves, nodes, metanodes, binIndices, threading); + BinnedBuildNode(false, 0, 0, subtreeCount, -1, -1, default, &context, workerIndex, null); } - if (subtreeCountA > 1) - BinnedBuildNode(usePongBuffer, subtreeRegionStartIndex, nodeChildIndexA, subtreeCountA, nodeIndex, 0, bestCentroidBoundingBoxA, context, workerIndex, dispatcher); - if (!shouldPushBOntoMultithreadedQueue && subtreeCountB > 1) - BinnedBuildNode(usePongBuffer, subtreeRegionStartIndex + subtreeCountA, nodeChildIndexB, subtreeCountB, nodeIndex, 1, bestCentroidBoundingBoxB, context, workerIndex, dispatcher); - if (shouldPushBOntoMultithreadedQueue) + else { - //We want to keep the stack at this level alive until the memory we allocated for the node push completes. - //Note that WaitForCompletion will execute pending work; this isn't just busywaiting the current thread. - //In addition to letting us use the local stack to store some arguments for the other thread, this wait means that all children have completed when this function returns. - //That makes knowing when to stop the queue easier. - Debug.Assert(nodeBContinuation.Initialized); - Unsafe.As(ref context->Threading).TaskStack->WaitForCompletion(nodeBContinuation, workerIndex, dispatcher); + var context = new Context( + minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold, deterministic, + subtrees, default, default, nodes, metanodes, binIndices, threading); + BinnedBuildNode(false, 0, 0, subtreeCount, -1, -1, default, &context, workerIndex, null); } } - - /// - /// Runs a binned build across the input buffer. - /// - /// Subtrees (either leaves or nodes) to run the builder over. The builder may make in-place modifications to the input buffer; the input buffer should not be assumed to be in a valid state after the builder runs. - /// A parallel buffer to subtrees which is used as a scratch buffer during execution. If a default initialized buffer is provided, a slower sequential in-place fallback will be used. - /// Buffer holding the nodes created by the build process. - /// Nodes are created in a depth first ordering with respect to the input buffer. - /// Buffer holding the metanodes created by the build process. - /// Metanodes, like nodes, are created in a depth first ordering with respect to the input buffer. - /// Metanodes are in the same order and in the same slots; they simply contain data about nodes that most traversals don't need to know about. - /// Buffer holding the leaf references created by the build process. - /// The indices written by the build process are those defined in the inputs; any that is negative is encoded according to and points into the leaf buffer. - /// If a default-valued (unallocated) buffer is passed in, the binned builder will ignore leaves. - /// Buffer to be used for caching bin indices during execution. If subtreesPong is defined, binIndices must also be defined, and vice versa. - /// Thread dispatcher used to accelerate the build process. - /// Task stack being used to run the build process, if any. - /// If provided, the builder assumes the refinement is running within an existing multithreaded dispatch and will not call IThreadDispatcher.DispatchWorkers. - /// If null, the builder will create its own task stack and call IThreadDispatcher.DispatchWorkers internally. - /// Index of the current worker. - /// Number of workers that may be used in the builder. This should span all worker indices that may contribute to the build process even only a subset are expected to be used at any one time. - /// Number of tasks to try to use in the builder. - /// Buffer pool used to preallocate temporary resources for building. - /// Minimum number of bins the builder should use per node. - /// Maximum number of bins the builder should use per node. Must be no higher than 255. - /// Multiplier to apply to the subtree count within a node to decide the bin count. Resulting value will then be clamped by the minimum/maximum bin counts. - /// Threshold at or under which the binned builder resorts to local counting sort sweeps. - /// Whether to force determinism at a slightly higher cost when using internally multithreaded execution. - /// If the build is single threaded, it is already deterministic and this flag has no effect. - static unsafe void BinnedBuilderInternal(Buffer subtrees, Buffer subtreesPong, Buffer nodes, Buffer metanodes, Buffer leaves, Buffer binIndices, - IThreadDispatcher dispatcher, TaskStack* taskStackPointer, int workerIndex, int workerCount, int targetTaskCount, BufferPool pool, int minimumBinCount, int maximumBinCount, float leafToBinMultiplier, int microsweepThreshold, bool deterministic) + else { - var subtreeCount = subtrees.Length; - if (nodes.Length < subtreeCount - 1) - throw new ArgumentException($"The nodes buffer is too small to hold all the nodes that will be necessary for the input subtrees."); - if (maximumBinCount > 255) - throw new ArgumentException($"Maximum bin count must fit in a byte (maximum of 255)."); - if (subtreesPong.Allocated != binIndices.Allocated) - throw new ArgumentException("The parameters subtreesPong and binIndices must both be allocated or unallocated."); - if (subtreeCount == 0) - return; - if (subtreeCount == 1) + //Multithreaded dispatch! + //At the moment, the TaskStack expects an IThreadDispatcher to exist, so there are two cases to handle here: + //1: There is an IThreadDispatcher, but no existing TaskStack. We'll create one and do an internal dispatch. + //2: There is an IThreadDispatcher *and* an existing TaskStack. We'll push tasks into the stack, but we won't dispatch them; the user will. + Debug.Assert(dispatcher != null); + //While we could allocate on the stack with reasonable safety in the single threaded path, that's not very reasonable for the multithreaded path. + //Each worker thread could be given a node job which executes asynchronously with respect to other node jobs. + //Those node jobs could spawn multithreaded work that other workers assist with. + //Each of those jobs needs its own context for those workers, and the number of jobs is not 1:1 with the workers. + //We'll handle such dispatch-required allocations from worker pools. Here, we just preallocate stuff for the first level across all workers. + pool.Take(allocatedBinCount * workerCount * (sizeof(BoundingBox4) * 4 + sizeof(int)), out var workerBinsAllocation); + + BinnedBuildWorkerContext* workerContextsPointer = stackalloc BinnedBuildWorkerContext[workerCount]; + var workerContexts = new Buffer(workerContextsPointer, workerCount); + + int binAllocationStart = 0; + for (int i = 0; i < workerCount; ++i) { - //If there's only one leaf, the tree has a special format: the root node has only one child. - ref var root = ref nodes[0]; - root.A = subtrees[0]; - root.B = default; - return; + workerContexts[i] = new BinnedBuildWorkerContext(workerBinsAllocation, ref binAllocationStart, allocatedBinCount); } - nodes = nodes.Slice(subtreeCount - 1); - - //Don't let the user pick values that will just cause an explosion. - Debug.Assert(minimumBinCount >= 2 && maximumBinCount >= 2, "At least two bins are required. In release mode, this will be clamped up to 2, but where did lower values come from?"); - minimumBinCount = int.Max(2, minimumBinCount); - maximumBinCount = int.Max(2, maximumBinCount); - //The microsweep uses the same resources as the bin allocations, so expand to hold whichever is larger. - var allocatedBinCount = int.Max(maximumBinCount, microsweepThreshold); - - if (dispatcher == null && taskStackPointer == null) + TaskStack taskStack; + bool dispatchInternally = taskStackPointer == null; + if (dispatchInternally) + { + taskStack = new TaskStack(pool, dispatcher, workerCount); + taskStackPointer = &taskStack; + } + var threading = new MultithreadBinnedBuildContext { - //Use the single threaded path. - var allocatedByteCount = allocatedBinCount * 4 * sizeof(BoundingBox4) + allocatedBinCount * sizeof(int); - var binBoundsMemoryAllocation = stackalloc byte[allocatedByteCount + 32]; - //Should be basically irrelevant, but just in case it's not on some platform, align the allocation. - binBoundsMemoryAllocation = (byte*)(((ulong)binBoundsMemoryAllocation + 31ul) & (~31ul)); - var binBoundsMemory = new Buffer(binBoundsMemoryAllocation, allocatedByteCount); - - var threading = new SingleThreaded(binBoundsMemory, allocatedBinCount); - if (leaves.Allocated) + TopLevelTargetTaskCount = targetTaskCount, + OriginalSubtreeCount = subtrees.Length, + TaskStack = taskStackPointer, + Workers = workerContexts, + }; + if (leaves.Allocated) + { + var context = new Context, MultithreadBinnedBuildContext>( + minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold, deterministic, + subtrees, subtreesPong, leaves, nodes, metanodes, binIndices, threading); + + if (dispatchInternally) { - var context = new Context, SingleThreaded>( - minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold, deterministic, - subtrees, default, leaves, nodes, metanodes, binIndices, threading); - BinnedBuildNode(false, 0, 0, subtreeCount, -1, -1, default, &context, workerIndex, null); + Debug.Assert(workerIndex == 0, "If we're dispatching internally, there shouldn't be any other active workers."); + taskStackPointer->PushUnsafely(new Task(&BinnedBuilderWorkerEntry>, &context), 0, dispatcher); + TaskStack.DispatchWorkers(dispatcher, taskStackPointer, workerCount); } else { - var context = new Context( - minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold, deterministic, - subtrees, default, default, nodes, metanodes, binIndices, threading); - BinnedBuildNode(false, 0, 0, subtreeCount, -1, -1, default, &context, workerIndex, null); + BinnedBuildNode(false, 0, 0, context.SubtreesPing.Length, -1, -1, default, &context, workerIndex, dispatcher); } } else { - //Multithreaded dispatch! - //At the moment, the TaskStack expects an IThreadDispatcher to exist, so there are two cases to handle here: - //1: There is an IThreadDispatcher, but no existing TaskStack. We'll create one and do an internal dispatch. - //2: There is an IThreadDispatcher *and* an existing TaskStack. We'll push tasks into the stack, but we won't dispatch them; the user will. - Debug.Assert(dispatcher != null); - //While we could allocate on the stack with reasonable safety in the single threaded path, that's not very reasonable for the multithreaded path. - //Each worker thread could be given a node job which executes asynchronously with respect to other node jobs. - //Those node jobs could spawn multithreaded work that other workers assist with. - //Each of those jobs needs its own context for those workers, and the number of jobs is not 1:1 with the workers. - //We'll handle such dispatch-required allocations from worker pools. Here, we just preallocate stuff for the first level across all workers. - pool.Take(allocatedBinCount * workerCount * (sizeof(BoundingBox4) * 4 + sizeof(int)), out var workerBinsAllocation); - - BinnedBuildWorkerContext* workerContextsPointer = stackalloc BinnedBuildWorkerContext[workerCount]; - var workerContexts = new Buffer(workerContextsPointer, workerCount); - - int binAllocationStart = 0; - for (int i = 0; i < workerCount; ++i) - { - workerContexts[i] = new BinnedBuildWorkerContext(workerBinsAllocation, ref binAllocationStart, allocatedBinCount); - } + var context = new Context( + minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold, deterministic, + subtrees, subtreesPong, default, nodes, metanodes, binIndices, threading); - TaskStack taskStack; - bool dispatchInternally = taskStackPointer == null; if (dispatchInternally) { - taskStack = new TaskStack(pool, dispatcher, workerCount); - taskStackPointer = &taskStack; - } - var threading = new MultithreadBinnedBuildContext - { - TopLevelTargetTaskCount = targetTaskCount, - OriginalSubtreeCount = subtrees.Length, - TaskStack = taskStackPointer, - Workers = workerContexts, - }; - if (leaves.Allocated) - { - var context = new Context, MultithreadBinnedBuildContext>( - minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold, deterministic, - subtrees, subtreesPong, leaves, nodes, metanodes, binIndices, threading); - - if (dispatchInternally) - { - Debug.Assert(workerIndex == 0, "If we're dispatching internally, there shouldn't be any other active workers."); - taskStackPointer->PushUnsafely(new Task(&BinnedBuilderWorkerEntry>, &context), 0, dispatcher); - TaskStack.DispatchWorkers(dispatcher, taskStackPointer, workerCount); - } - else - { - BinnedBuildNode(false, 0, 0, context.SubtreesPing.Length, -1, -1, default, &context, workerIndex, dispatcher); - } + Debug.Assert(workerIndex == 0, "If we're dispatching internally, there shouldn't be any other active workers."); + taskStackPointer->PushUnsafely(new Task(&BinnedBuilderWorkerEntry, &context), 0, dispatcher); + TaskStack.DispatchWorkers(dispatcher, taskStackPointer, workerCount); } else { - var context = new Context( - minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold, deterministic, - subtrees, subtreesPong, default, nodes, metanodes, binIndices, threading); - - if (dispatchInternally) - { - Debug.Assert(workerIndex == 0, "If we're dispatching internally, there shouldn't be any other active workers."); - taskStackPointer->PushUnsafely(new Task(&BinnedBuilderWorkerEntry, &context), 0, dispatcher); - TaskStack.DispatchWorkers(dispatcher, taskStackPointer, workerCount); - } - else - { - BinnedBuildNode(false, 0, 0, context.SubtreesPing.Length, -1, -1, default, &context, workerIndex, dispatcher); - } + BinnedBuildNode(false, 0, 0, context.SubtreesPing.Length, -1, -1, default, &context, workerIndex, dispatcher); } + } - if (dispatchInternally) - taskStackPointer->Dispose(pool, dispatcher); - pool.Return(ref workerBinsAllocation); + if (dispatchInternally) + taskStackPointer->Dispose(pool, dispatcher); + pool.Return(ref workerBinsAllocation); - } } + } - unsafe static void BinnedBuilderWorkerEntry(long taskId, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) - where TLeaves : unmanaged - { - var context = (Context*)untypedContext; - BinnedBuildNode(false, 0, 0, context->SubtreesPing.Length, -1, -1, default, context, workerIndex, dispatcher); - //Once the entry point returns, all workers should stop because it won't return unless both nodes are done. - context->Threading.TaskStack->RequestStop(); - } + unsafe static void BinnedBuilderWorkerEntry(long taskId, void* untypedContext, int workerIndex, IThreadDispatcher dispatcher) + where TLeaves : unmanaged + { + var context = (Context*)untypedContext; + BinnedBuildNode(false, 0, 0, context->SubtreesPing.Length, -1, -1, default, context, workerIndex, dispatcher); + //Once the entry point returns, all workers should stop because it won't return unless both nodes are done. + context->Threading.TaskStack->RequestStop(); + } - /// - /// Runs a multithreaded binned build across the subtrees buffer. - /// - /// Subtrees (either leaves or nodes) to run the builder over. The builder may make in-place modifications to the input buffer; the input buffer should not be assumed to be in a valid state after the builder runs. - /// Buffer holding the nodes created by the build process. - /// Nodes are created in a depth first ordering with respect to the input buffer. - /// Buffer holding the metanodes created by the build process. - /// Metanodes, like nodes, are created in a depth first ordering with respect to the input buffer. - /// Metanodes are in the same order and in the same slots; they simply contain data about nodes that most traversals don't need to know about. - /// Buffer holding the leaf references created by the build process. - /// The indices written by the build process are those defined in the inputs; any that is negative is encoded according to and points into the leaf buffer. - /// If a default-valued (unallocated) buffer is passed in, the binned builder will ignore leaves. - /// Buffer pool used to preallocate a pingpong buffer if the number of subtrees exceeds maximumSubtreeStackAllocationCount. If null, stack allocation or a slower in-place partitioning will be used. - /// Dispatcher used to multithread the execution of the build. If the dispatcher is not null, pool must also not be null. - /// Task stack being used to run the build process, if any. - /// If provided, the builder assumes the refinement is running within an existing multithreaded dispatch and will not call IThreadDispatcher.DispatchWorkers. - /// If null, the builder will create its own task stack and call IThreadDispatcher.DispatchWorkers internally. - /// A pool must be provided if a thread dispatcher is given. - /// Index of the currently executing worker. If not running within a dispatch, 0 is valid. - /// Number of workers that may be used in the builder. This should span all worker indices that may contribute to the build process even only a subset are expected to be used at any one time. - /// If negative, the dispatcher's thread count will be used. - /// Number of tasks to try to use in the builder. If negative, the dispatcher's thread count will be used. - /// Maximum number of subtrees to try putting on the stack for the binned builder's pong buffers. - /// Subtree counts larger than this threshold will either resort to a buffer pool allocation (if available) or slower in-place partition operations. - /// Minimum number of bins the builder should use per node. - /// Maximum number of bins the builder should use per node. - /// Multiplier to apply to the subtree count within a node to decide the bin count. Resulting value will then be clamped by the minimum/maximum bin counts. - /// Threshold at or under which the binned builder resorts to local counting sort sweeps. - /// Whether to force determinism at a slightly higher cost when using internally multithreaded execution. - /// If the build is single threaded, it is already deterministic and this flag has no effect. - public static unsafe void BinnedBuild(Buffer subtrees, Buffer nodes, Buffer metanodes, Buffer leaves, - BufferPool pool = null, IThreadDispatcher dispatcher = null, TaskStack* taskStackPointer = null, int workerIndex = 0, int workerCount = -1, int targetTaskCount = -1, - int maximumSubtreeStackAllocationCount = 4096, int minimumBinCount = 16, int maximumBinCount = 64, float leafToBinMultiplier = 1 / 16f, int microsweepThreshold = 64, bool deterministic = false) + /// + /// Runs a multithreaded binned build across the subtrees buffer. + /// + /// Subtrees (either leaves or nodes) to run the builder over. The builder may make in-place modifications to the input buffer; the input buffer should not be assumed to be in a valid state after the builder runs. + /// Buffer holding the nodes created by the build process. + /// Nodes are created in a depth first ordering with respect to the input buffer. + /// Buffer holding the metanodes created by the build process. + /// Metanodes, like nodes, are created in a depth first ordering with respect to the input buffer. + /// Metanodes are in the same order and in the same slots; they simply contain data about nodes that most traversals don't need to know about. + /// Buffer holding the leaf references created by the build process. + /// The indices written by the build process are those defined in the inputs; any that is negative is encoded according to and points into the leaf buffer. + /// If a default-valued (unallocated) buffer is passed in, the binned builder will ignore leaves. + /// Buffer pool used to preallocate a pingpong buffer if the number of subtrees exceeds maximumSubtreeStackAllocationCount. If null, stack allocation or a slower in-place partitioning will be used. + /// Dispatcher used to multithread the execution of the build. If the dispatcher is not null, pool must also not be null. + /// Task stack being used to run the build process, if any. + /// If provided, the builder assumes the refinement is running within an existing multithreaded dispatch and will not call IThreadDispatcher.DispatchWorkers. + /// If null, the builder will create its own task stack and call IThreadDispatcher.DispatchWorkers internally. + /// A pool must be provided if a thread dispatcher is given. + /// Index of the currently executing worker. If not running within a dispatch, 0 is valid. + /// Number of workers that may be used in the builder. This should span all worker indices that may contribute to the build process even only a subset are expected to be used at any one time. + /// If negative, the dispatcher's thread count will be used. + /// Number of tasks to try to use in the builder. If negative, the dispatcher's thread count will be used. + /// Maximum number of subtrees to try putting on the stack for the binned builder's pong buffers. + /// Subtree counts larger than this threshold will either resort to a buffer pool allocation (if available) or slower in-place partition operations. + /// Minimum number of bins the builder should use per node. + /// Maximum number of bins the builder should use per node. + /// Multiplier to apply to the subtree count within a node to decide the bin count. Resulting value will then be clamped by the minimum/maximum bin counts. + /// Threshold at or under which the binned builder resorts to local counting sort sweeps. + /// Whether to force determinism at a slightly higher cost when using internally multithreaded execution. + /// If the build is single threaded, it is already deterministic and this flag has no effect. + public static unsafe void BinnedBuild(Buffer subtrees, Buffer nodes, Buffer metanodes, Buffer leaves, + BufferPool pool = null, IThreadDispatcher dispatcher = null, TaskStack* taskStackPointer = null, int workerIndex = 0, int workerCount = -1, int targetTaskCount = -1, + int maximumSubtreeStackAllocationCount = 4096, int minimumBinCount = 16, int maximumBinCount = 64, float leafToBinMultiplier = 1 / 16f, int microsweepThreshold = 64, bool deterministic = false) + { + if (subtrees.Length <= 2) { - if (subtrees.Length <= 2) - { - //No need to do anything fancy, all subtrees fit in the root. Requires a special case for the partial root. - nodes[0] = new Node { A = subtrees[0], B = subtrees.Length == 2 ? subtrees[1] : default }; - if (metanodes.Allocated) - metanodes[0] = new Metanode { Parent = -1, IndexInParent = -1 }; - return; - } - if (dispatcher != null && pool == null) - throw new ArgumentException("If a ThreadDispatcher has been given to BinnedBuild, a BufferPool must also be provided."); - Buffer subtreesPong; - Buffer binIndices; - bool requiresReturn = false; - if (subtrees.Length <= maximumSubtreeStackAllocationCount) - { - var subtreesPongMemory = stackalloc NodeChild[subtrees.Length]; - subtreesPong = new Buffer(subtreesPongMemory, subtrees.Length); - var binIndicesMemory = stackalloc byte[subtrees.Length]; - binIndices = new Buffer(binIndicesMemory, subtrees.Length); - } - else if (pool != null) - { - pool.Take(subtrees.Length, out subtreesPong); - pool.Take(subtrees.Length, out binIndices); - requiresReturn = true; - } - else - { - binIndices = default; - subtreesPong = default; - } - BinnedBuilderInternal(subtrees, subtreesPong, nodes, metanodes, leaves, binIndices, dispatcher, taskStackPointer, workerIndex, - dispatcher == null ? 0 : workerCount < 0 ? dispatcher.ThreadCount : workerCount, - dispatcher == null ? 0 : targetTaskCount < 0 ? dispatcher.ThreadCount : targetTaskCount, - pool, minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold, deterministic); - - if (requiresReturn) - { - pool.Return(ref binIndices); - pool.Return(ref subtreesPong); - } + //No need to do anything fancy, all subtrees fit in the root. Requires a special case for the partial root. + nodes[0] = new Node { A = subtrees[0], B = subtrees.Length == 2 ? subtrees[1] : default }; + if (metanodes.Allocated) + metanodes[0] = new Metanode { Parent = -1, IndexInParent = -1 }; + return; + } + if (dispatcher != null && pool == null) + throw new ArgumentException("If a ThreadDispatcher has been given to BinnedBuild, a BufferPool must also be provided."); + Buffer subtreesPong; + Buffer binIndices; + bool requiresReturn = false; + if (subtrees.Length <= maximumSubtreeStackAllocationCount) + { + var subtreesPongMemory = stackalloc NodeChild[subtrees.Length]; + subtreesPong = new Buffer(subtreesPongMemory, subtrees.Length); + var binIndicesMemory = stackalloc byte[subtrees.Length]; + binIndices = new Buffer(binIndicesMemory, subtrees.Length); } + else if (pool != null) + { + pool.Take(subtrees.Length, out subtreesPong); + pool.Take(subtrees.Length, out binIndices); + requiresReturn = true; + } + else + { + binIndices = default; + subtreesPong = default; + } + BinnedBuilderInternal(subtrees, subtreesPong, nodes, metanodes, leaves, binIndices, dispatcher, taskStackPointer, workerIndex, + dispatcher == null ? 0 : workerCount < 0 ? dispatcher.ThreadCount : workerCount, + dispatcher == null ? 0 : targetTaskCount < 0 ? dispatcher.ThreadCount : targetTaskCount, + pool, minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold, deterministic); - /// - /// Runs a binned build across the subtrees buffer. - /// - /// Subtrees (either leaves or nodes) to run the builder over. The builder may make in-place modifications to the input buffer; the input buffer should not be assumed to be in a valid state after the builder runs. - /// Buffer pool used to preallocate a pingpong buffer if the number of subtrees exceeds maximumSubtreeStackAllocationCount. If null, stack allocation or a slower in-place partitioning will be used. - /// A pool must be provided if a thread dispatcher is given. - /// Dispatcher used to multithread the execution of the build. If the dispatcher is not null, pool must also not be null. - /// Task stack being used to run the build process, if any. - /// If provided, the builder assumes the refinement is running within an existing multithreaded dispatch and will not call IThreadDispatcher.DispatchWorkers. - /// If null, the builder will create its own task stack and call IThreadDispatcher.DispatchWorkers internally. - /// Index of the currently executing worker. If not running within a dispatch, 0 is valid. - /// Number of workers that may be used in the builder. This should span all worker indices that may contribute to the build process even only a subset are expected to be used at any one time. - /// If negative, the dispatcher's thread count will be used. - /// Number of tasks to try to use in the builder. If negative, the dispatcher's thread count will be used. - /// Maximum number of subtrees to try putting on the stack for the binned builder's pong buffers. - /// Subtree counts larger than this threshold will either resort to a buffer pool allocation (if available) or slower in-place partition operations. - /// Minimum number of bins the builder should use per node. - /// Maximum number of bins the builder should use per node. - /// Multiplier to apply to the subtree count within a node to decide the bin count. Resulting value will then be clamped by the minimum/maximum bin counts. - /// Threshold at or under which the binned builder resorts to local counting sort sweeps. - /// Whether to force determinism at a slightly higher cost when using internally multithreaded execution. - /// If the build is single threaded, it is already deterministic and this flag has no effect. - public unsafe void BinnedBuild(Buffer subtrees, - BufferPool pool = null, IThreadDispatcher dispatcher = null, TaskStack* taskStackPointer = null, int workerIndex = 0, int workerCount = -1, int targetTaskCount = -1, - int maximumSubtreeStackAllocationCount = 4096, int minimumBinCount = 16, int maximumBinCount = 64, float leafToBinMultiplier = 1 / 16f, int microsweepThreshold = 64, bool deterministic = false) + if (requiresReturn) { - BinnedBuild(subtrees, Nodes.Slice(NodeCount), Metanodes.Slice(NodeCount), Leaves.Slice(LeafCount), pool, dispatcher, taskStackPointer, workerIndex, - workerCount, targetTaskCount, maximumSubtreeStackAllocationCount, minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold); + pool.Return(ref binIndices); + pool.Return(ref subtreesPong); } } + + /// + /// Runs a binned build across the subtrees buffer. + /// + /// Subtrees (either leaves or nodes) to run the builder over. The builder may make in-place modifications to the input buffer; the input buffer should not be assumed to be in a valid state after the builder runs. + /// Buffer pool used to preallocate a pingpong buffer if the number of subtrees exceeds maximumSubtreeStackAllocationCount. If null, stack allocation or a slower in-place partitioning will be used. + /// A pool must be provided if a thread dispatcher is given. + /// Dispatcher used to multithread the execution of the build. If the dispatcher is not null, pool must also not be null. + /// Task stack being used to run the build process, if any. + /// If provided, the builder assumes the refinement is running within an existing multithreaded dispatch and will not call IThreadDispatcher.DispatchWorkers. + /// If null, the builder will create its own task stack and call IThreadDispatcher.DispatchWorkers internally. + /// Index of the currently executing worker. If not running within a dispatch, 0 is valid. + /// Number of workers that may be used in the builder. This should span all worker indices that may contribute to the build process even only a subset are expected to be used at any one time. + /// If negative, the dispatcher's thread count will be used. + /// Number of tasks to try to use in the builder. If negative, the dispatcher's thread count will be used. + /// Maximum number of subtrees to try putting on the stack for the binned builder's pong buffers. + /// Subtree counts larger than this threshold will either resort to a buffer pool allocation (if available) or slower in-place partition operations. + /// Minimum number of bins the builder should use per node. + /// Maximum number of bins the builder should use per node. + /// Multiplier to apply to the subtree count within a node to decide the bin count. Resulting value will then be clamped by the minimum/maximum bin counts. + /// Threshold at or under which the binned builder resorts to local counting sort sweeps. + /// Whether to force determinism at a slightly higher cost when using internally multithreaded execution. + /// If the build is single threaded, it is already deterministic and this flag has no effect. + public unsafe void BinnedBuild(Buffer subtrees, + BufferPool pool = null, IThreadDispatcher dispatcher = null, TaskStack* taskStackPointer = null, int workerIndex = 0, int workerCount = -1, int targetTaskCount = -1, + int maximumSubtreeStackAllocationCount = 4096, int minimumBinCount = 16, int maximumBinCount = 64, float leafToBinMultiplier = 1 / 16f, int microsweepThreshold = 64, bool deterministic = false) + { + BinnedBuild(subtrees, Nodes.Slice(NodeCount), Metanodes.Slice(NodeCount), Leaves.Slice(LeafCount), pool, dispatcher, taskStackPointer, workerIndex, + workerCount, targetTaskCount, maximumSubtreeStackAllocationCount, minimumBinCount, maximumBinCount, leafToBinMultiplier, microsweepThreshold); + } } From 7d0b80d4f142e6b96c25f18802005491c4ccd24c Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 27 Aug 2025 23:27:57 -0500 Subject: [PATCH 931/947] No more stack overrun on pathological tree queries! --- BepuPhysics/Collidables/BigCompound.cs | 32 ++++----- BepuPhysics/Collidables/Compound.cs | 12 ++-- BepuPhysics/Collidables/IShape.cs | 15 +++-- BepuPhysics/Collidables/Mesh.cs | 16 +++-- BepuPhysics/Collidables/Shapes.cs | 43 ++++++++---- .../CollisionDetection/BroadPhase_Queries.cs | 37 ++++++----- .../CollisionDetection/MeshReduction.cs | 2 +- BepuPhysics/CollisionDetection/RayBatchers.cs | 20 +++--- BepuPhysics/Simulation_Queries.cs | 11 ++-- BepuPhysics/Statics.cs | 2 +- BepuPhysics/Trees/RayBatcher.cs | 12 ++-- BepuPhysics/Trees/Tree_RayCast.cs | 53 ++++++++++----- BepuPhysics/Trees/Tree_Sweep.cs | 66 +++++++++++++++---- BepuPhysics/Trees/Tree_VolumeQuery.cs | 60 +++++++++++++---- DemoBenchmarks/ShapeRayBenchmarksDeep.cs | 2 +- DemoTests/InertiaTensorTests.cs | 2 +- Demos/DemoHarness.cs | 2 +- Demos/Demos/CollisionQueryDemo.cs | 4 +- Demos/Demos/CustomVoxelCollidableDemo.cs | 16 +++-- Demos/Demos/RayCastingDemo.cs | 3 +- Demos/Grabber.cs | 5 +- .../IntertreeThreadingTests.cs | 2 +- Demos/SpecializedTests/VolumeQueryTests.cs | 3 +- 23 files changed, 273 insertions(+), 147 deletions(-) diff --git a/BepuPhysics/Collidables/BigCompound.cs b/BepuPhysics/Collidables/BigCompound.cs index 8948e4238..644e2c1d6 100644 --- a/BepuPhysics/Collidables/BigCompound.cs +++ b/BepuPhysics/Collidables/BigCompound.cs @@ -113,7 +113,7 @@ public unsafe BigCompound(Buffer children, Shapes shapes, BufferP pool.Return(ref subtrees); } - public void ComputeBounds(Quaternion orientation, Shapes shapeBatches, out Vector3 min, out Vector3 max) + public readonly void ComputeBounds(Quaternion orientation, Shapes shapeBatches, out Vector3 min, out Vector3 max) { Compound.ComputeChildBounds(Children[0], orientation, shapeBatches, out min, out max); for (int i = 1; i < Children.Length; ++i) @@ -125,9 +125,9 @@ public void ComputeBounds(Quaternion orientation, Shapes shapeBatches, out Vecto } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void AddChildBoundsToBatcher(ref BoundingBoxBatcher batcher, in RigidPose pose, in BodyVelocity velocity, int bodyIndex) + public readonly void AddChildBoundsToBatcher(ref BoundingBoxBatcher batcher, in RigidPose pose, in BodyVelocity velocity, int bodyIndex) { - Compound.AddChildBoundsToBatcher(ref Children, ref batcher, pose, velocity, bodyIndex); + Compound.AddChildBoundsToBatcher(Children, ref batcher, pose, velocity, bodyIndex); } unsafe struct LeafTester : IRayLeafTester where TRayHitHandler : struct, IShapeRayHitHandler @@ -152,7 +152,7 @@ public LeafTester(in Buffer children, Shapes shapes, in TRayHitHa [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void TestLeaf(int leafIndex, RayData* rayData, float* maximumT) + public void TestLeaf(int leafIndex, RayData* rayData, float* maximumT, BufferPool pool) { if (Handler.AllowTest(leafIndex)) { @@ -160,7 +160,7 @@ public void TestLeaf(int leafIndex, RayData* rayData, float* maximumT) CompoundChildShapeTester tester; tester.T = -1; tester.Normal = default; - Shapes[child.ShapeIndex.Type].RayTest(child.ShapeIndex.Index, child.AsPose(), *rayData, ref *maximumT, ref tester); + Shapes[child.ShapeIndex.Type].RayTest(child.ShapeIndex.Index, child.AsPose(), *rayData, ref *maximumT, pool, ref tester); if (tester.T >= 0) { Debug.Assert(*maximumT >= tester.T, "Whatever generated this ray hit should have obeyed the current maximumT value."); @@ -171,18 +171,18 @@ public void TestLeaf(int leafIndex, RayData* rayData, float* maximumT) } } - public void RayTest(in RigidPose pose, in RayData ray, ref float maximumT, Shapes shapes, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler + public readonly void RayTest(in RigidPose pose, in RayData ray, ref float maximumT, Shapes shapes, BufferPool pool, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler { Matrix3x3.CreateFromQuaternion(pose.Orientation, out var orientation); Matrix3x3.TransformTranspose(ray.Origin - pose.Position, orientation, out var localOrigin); Matrix3x3.TransformTranspose(ray.Direction, orientation, out var localDirection); var leafTester = new LeafTester(Children, shapes, hitHandler, orientation, ray); - Tree.RayCast(localOrigin, localDirection, ref maximumT, ref leafTester); + Tree.RayCast(localOrigin, localDirection, ref maximumT, pool, ref leafTester); //Copy the hitHandler to preserve any mutation. hitHandler = leafTester.Handler; } - public unsafe void RayTest(in RigidPose pose, ref RaySource rays, Shapes shapes, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler + public readonly unsafe void RayTest(in RigidPose pose, ref RaySource rays, Shapes shapes, BufferPool pool, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler { //TODO: Note that we dispatch a bunch of scalar tests here. You could be more clever than this- batched tests are possible. //May be worth creating a different traversal designed for low ray counts- might be able to get some benefit out of a semidynamic packet or something. @@ -195,7 +195,7 @@ public unsafe void RayTest(in RigidPose pose, ref RaySource rays leafTester.OriginalRay = *ray; Matrix3x3.Transform(ray->Origin - pose.Position, inverseOrientation, out var localOrigin); Matrix3x3.Transform(ray->Direction, inverseOrientation, out var localDirection); - Tree.RayCast(localOrigin, localDirection, ref *maximumT, ref leafTester); + Tree.RayCast(localOrigin, localDirection, ref *maximumT, pool, ref leafTester); } //Preserve any mutations. hitHandler = leafTester.Handler; @@ -207,14 +207,14 @@ public static ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, return new CompoundShapeBatch(pool, initialCapacity, shapes); } - public int ChildCount + public readonly int ChildCount { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { return Children.Length; } } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public ref CompoundChild GetChild(int compoundChildIndex) + public readonly ref CompoundChild GetChild(int compoundChildIndex) { return ref Children[compoundChildIndex]; } @@ -282,7 +282,7 @@ public unsafe void FindLocalOverlaps(ref Buffer(pair.Container).Tree.GetOverlaps(pair.Min, pair.Max, ref enumerator); + Unsafe.AsRef(pair.Container).Tree.GetOverlaps(pair.Min, pair.Max, pool, ref enumerator); } } @@ -296,13 +296,13 @@ public void TestLeaf(int leafIndex, ref float maximumT) Unsafe.AsRef(Overlaps).Allocate(Pool) = leafIndex; } } - public unsafe void FindLocalOverlaps(Vector3 min, Vector3 max, Vector3 sweep, float maximumT, BufferPool pool, Shapes shapes, void* overlaps) + public readonly unsafe void FindLocalOverlaps(Vector3 min, Vector3 max, Vector3 sweep, float maximumT, BufferPool pool, Shapes shapes, void* overlaps) where TOverlaps : ICollisionTaskSubpairOverlaps { SweepLeafTester enumerator; enumerator.Pool = pool; enumerator.Overlaps = overlaps; - Tree.Sweep(min, max, sweep, maximumT, ref enumerator); + Tree.Sweep(min, max, sweep, maximumT, pool, ref enumerator); } /// @@ -311,7 +311,7 @@ public unsafe void FindLocalOverlaps(Vector3 min, Vector3 max, Vector /// Masses of the children. /// Shapes collection containing the data for the compound child shapes. /// Inertia of the compound. - public BodyInertia ComputeInertia(Span childMasses, Shapes shapes) + public readonly BodyInertia ComputeInertia(Span childMasses, Shapes shapes) { return CompoundBuilder.ComputeInertia(Children, childMasses, shapes); } @@ -323,7 +323,7 @@ public BodyInertia ComputeInertia(Span childMasses, Shapes shapes) /// Masses of the children. /// Calculated center of mass of the compound. Subtracted from all the compound child poses. /// Inertia of the compound. - public BodyInertia ComputeInertia(Span childMasses, Shapes shapes, out Vector3 centerOfMass) + public readonly BodyInertia ComputeInertia(Span childMasses, Shapes shapes, out Vector3 centerOfMass) { var bodyInertia = CompoundBuilder.ComputeInertia(Children, childMasses, shapes, out centerOfMass); //Recentering moves the children around, so the tree needs to be updated. diff --git a/BepuPhysics/Collidables/Compound.cs b/BepuPhysics/Collidables/Compound.cs index 826b07253..9a1da6d54 100644 --- a/BepuPhysics/Collidables/Compound.cs +++ b/BepuPhysics/Collidables/Compound.cs @@ -195,7 +195,7 @@ public void ComputeBounds(Quaternion orientation, Shapes shapeBatches, out Vecto } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void AddChildBoundsToBatcher(ref Buffer children, ref BoundingBoxBatcher batcher, in RigidPose pose, in BodyVelocity velocity, int bodyIndex) + public static void AddChildBoundsToBatcher(Buffer children, ref BoundingBoxBatcher batcher, in RigidPose pose, in BodyVelocity velocity, int bodyIndex) { //Note that this approximates the velocity of the child using a piecewise extrapolation using the parent's angular velocity. //For significant angular velocities, this is actually wrong, but this is how v1 worked forever and it's cheap. @@ -223,10 +223,10 @@ public static void AddChildBoundsToBatcher(ref Buffer children, r [MethodImpl(MethodImplOptions.AggressiveInlining)] public void AddChildBoundsToBatcher(ref BoundingBoxBatcher batcher, in RigidPose pose, in BodyVelocity velocity, int bodyIndex) { - AddChildBoundsToBatcher(ref Children, ref batcher, pose, velocity, bodyIndex); + AddChildBoundsToBatcher(Children, ref batcher, pose, velocity, bodyIndex); } - public void RayTest(in RigidPose pose, in RayData ray, ref float maximumT, Shapes shapeBatches, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler + public void RayTest(in RigidPose pose, in RayData ray, ref float maximumT, Shapes shapeBatches, BufferPool pool, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler { Matrix3x3.CreateFromQuaternion(pose.Orientation, out var orientation); RayData localRay; @@ -242,7 +242,7 @@ public void RayTest(in RigidPose pose, in RayData ray, ref float CompoundChildShapeTester tester; tester.T = -1; tester.Normal = default; - shapeBatches[child.ShapeIndex.Type].RayTest(child.ShapeIndex.Index, child.AsPose(), localRay, ref maximumT, ref tester); + shapeBatches[child.ShapeIndex.Type].RayTest(child.ShapeIndex.Index, child.AsPose(), localRay, ref maximumT, pool, ref tester); if (tester.T >= 0) { Debug.Assert(maximumT >= tester.T, "Whatever generated this ray hit should have obeyed the current maximumT value."); @@ -253,7 +253,7 @@ public void RayTest(in RigidPose pose, in RayData ray, ref float } } - public unsafe void RayTest(in RigidPose pose, ref RaySource rays, Shapes shapeBatches, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler + public unsafe void RayTest(in RigidPose pose, ref RaySource rays, Shapes shapeBatches, BufferPool pool, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler { //TODO: Note that we dispatch a bunch of scalar tests here. You could be more clever than this- batched tests are possible. //It's relatively easy to do batching for this compound type since there is no hierarchy traversal, but we refactored things to avoid an infinite generic expansion issue in AOT compilation. @@ -261,7 +261,7 @@ public unsafe void RayTest(in RigidPose pose, ref RaySource rays for (int i = 0; i < rays.RayCount; ++i) { rays.GetRay(i, out var ray, out var maximumT); - RayTest(pose, *ray, ref *maximumT, shapeBatches, ref hitHandler); + RayTest(pose, *ray, ref *maximumT, shapeBatches, pool, ref hitHandler); } } diff --git a/BepuPhysics/Collidables/IShape.cs b/BepuPhysics/Collidables/IShape.cs index d3dc1e4ae..7bcb5a183 100644 --- a/BepuPhysics/Collidables/IShape.cs +++ b/BepuPhysics/Collidables/IShape.cs @@ -104,9 +104,6 @@ public interface ICompoundShape : IDisposableShape, IBoundsQueryableCompound /// Index of the body in the active body set; used to accumulate child bounds results. void AddChildBoundsToBatcher(ref BoundingBoxBatcher batcher, in RigidPose pose, in BodyVelocity velocity, int bodyIndex); - //Compound shapes may require indirections into other shape batches. This isn't wonderfully fast, but this scalar path is designed more for convenience than performance anyway. - //For performance, a batched and vectorized codepath should be used. - /// /// Tests a ray against the shape. /// @@ -114,8 +111,9 @@ public interface ICompoundShape : IDisposableShape, IBoundsQueryableCompound /// Ray to test against the shape. /// Maximum distance along the ray, in units of the ray direction's length, that the ray will test. /// Shape batches to look up child shapes in if necessary. + /// Buffer pool used for any temporary allocations required by the test. /// Callbacks called when the ray interacts with a test candidate. - void RayTest(in RigidPose pose, in RayData ray, ref float maximumT, Shapes shapeBatches, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler; + void RayTest(in RigidPose pose, in RayData ray, ref float maximumT, Shapes shapeBatches, BufferPool pool, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler; /// /// Tests multiple rays against the shape. @@ -124,7 +122,8 @@ public interface ICompoundShape : IDisposableShape, IBoundsQueryableCompound /// Rays to test against the shape. /// Shape batches to look up child shapes in if necessary. /// Callbacks called when the ray interacts with a test candidate. - void RayTest(in RigidPose pose, ref RaySource rays, Shapes shapeBatches, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler; + /// Buffer pool used for any temporary allocations required by the test. + void RayTest(in RigidPose pose, ref RaySource rays, Shapes shapeBatches, BufferPool pool, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler; /// /// Gets the number of children in the compound shape. /// @@ -160,16 +159,18 @@ public interface IHomogeneousCompoundShape : IDisp /// Pose of the shape during the ray test. /// Ray to test against the shape. /// Maximum distance along the ray, in units of the ray direction's length, that the ray will test. + /// Buffer pool used for any temporary allocations required by the test. /// Callbacks called when the ray interacts with a test candidate. - void RayTest(in RigidPose pose, in RayData ray, ref float maximumT, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler; + void RayTest(in RigidPose pose, in RayData ray, ref float maximumT, BufferPool pool, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler; /// /// Tests multiple rays against the shape. /// /// Pose of the shape during the ray test. /// Rays to test against the shape. + /// Buffer pool used for any temporary allocations required by the test. /// Callbacks called when the ray interacts with a test candidate. - void RayTest(in RigidPose pose, ref RaySource rays, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler; + void RayTest(in RigidPose pose, ref RaySource rays, BufferPool pool, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler; /// /// Gets the number of children in the compound shape. /// diff --git a/BepuPhysics/Collidables/Mesh.cs b/BepuPhysics/Collidables/Mesh.cs index 903467461..68e406721 100644 --- a/BepuPhysics/Collidables/Mesh.cs +++ b/BepuPhysics/Collidables/Mesh.cs @@ -268,7 +268,7 @@ unsafe struct HitLeafTester : IRayLeafTester where T : IShapeRayHitHandler public RayData OriginalRay; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void TestLeaf(int leafIndex, RayData* rayData, float* maximumT) + public void TestLeaf(int leafIndex, RayData* rayData, float* maximumT, BufferPool pool) { ref var triangle = ref Triangles[leafIndex]; if (Triangle.RayTest(triangle.A, triangle.B, triangle.C, rayData->Origin, rayData->Direction, out var t, out var normal) && t <= *maximumT) @@ -289,7 +289,8 @@ public void TestLeaf(int leafIndex, RayData* rayData, float* maximumT) /// Ray to test against the mesh. /// Maximum length of the ray in units of the ray direction length. /// Callback to execute for every hit. - public readonly unsafe void RayTest(in RigidPose pose, in RayData ray, ref float maximumT, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler + /// Pool to use for any temporary allocations required during the ray test. + public readonly unsafe void RayTest(in RigidPose pose, in RayData ray, ref float maximumT, BufferPool pool, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler { HitLeafTester leafTester; leafTester.Triangles = Triangles.Memory; @@ -301,7 +302,7 @@ public readonly unsafe void RayTest(in RigidPose pose, in RayDat Matrix3x3.TransformTranspose(ray.Direction, leafTester.Orientation, out var localDirection); localOrigin *= inverseScale; localDirection *= inverseScale; - Tree.RayCast(localOrigin, localDirection, ref maximumT, ref leafTester); + Tree.RayCast(localOrigin, localDirection, ref maximumT, pool, ref leafTester); //The leaf tester could have mutated the hit handler; copy it back over. hitHandler = leafTester.HitHandler; } @@ -313,7 +314,8 @@ public readonly unsafe void RayTest(in RigidPose pose, in RayDat /// Pose of the mesh during the ray test. /// Set of rays to cast against the mesh. /// Callbacks to execute. - public readonly unsafe void RayTest(in RigidPose pose, ref RaySource rays, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler + /// Pool to use for any temporary allocations required during the ray test. + public readonly unsafe void RayTest(in RigidPose pose, ref RaySource rays, BufferPool pool, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler { HitLeafTester leafTester; leafTester.Triangles = Triangles.Memory; @@ -329,7 +331,7 @@ public readonly unsafe void RayTest(in RigidPose pose, ref RaySo Matrix3x3.Transform(ray->Direction, inverseOrientation, out var localDirection); localOrigin *= inverseScale; localDirection *= inverseScale; - Tree.RayCast(localOrigin, localDirection, ref *maximumT, ref leafTester); + Tree.RayCast(localOrigin, localDirection, ref *maximumT, pool, ref leafTester); } //The leaf tester could have mutated the hit handler; copy it back over. hitHandler = leafTester.HitHandler; @@ -352,7 +354,7 @@ public readonly unsafe void FindLocalOverlaps(ref B var scaledMax = mesh.inverseScale * pair.Max; enumerator.Overlaps = Unsafe.AsPointer(ref overlaps.GetOverlapsForPair(i)); //Take a min/max to compensate for negative scales. - mesh.Tree.GetOverlaps(Vector3.Min(scaledMin, scaledMax), Vector3.Max(scaledMin, scaledMax), ref enumerator); + mesh.Tree.GetOverlaps(Vector3.Min(scaledMin, scaledMax), Vector3.Max(scaledMin, scaledMax), pool, ref enumerator); } } @@ -366,7 +368,7 @@ public readonly unsafe void FindLocalOverlaps(Vector3 min, Vector3 ma enumerator.Pool = pool; enumerator.Overlaps = overlaps; //Take a min/max to compensate for negative scales. - Tree.Sweep(Vector3.Min(scaledMin, scaledMax), Vector3.Max(scaledMin, scaledMax), scaledSweep, maximumT, ref enumerator); + Tree.Sweep(Vector3.Min(scaledMin, scaledMax), Vector3.Max(scaledMin, scaledMax), scaledSweep, maximumT, pool, ref enumerator); } public struct MeshTriangleSource : ITriangleSource diff --git a/BepuPhysics/Collidables/Shapes.cs b/BepuPhysics/Collidables/Shapes.cs index 364c8f4a2..c00a97822 100644 --- a/BepuPhysics/Collidables/Shapes.cs +++ b/BepuPhysics/Collidables/Shapes.cs @@ -72,8 +72,27 @@ internal virtual void ComputeBounds(int shapeIndex, Quaternion orientation, out { throw new InvalidOperationException("Nonconvex shapes are not required to have a maximum radius or angular expansion implementation. This should only ever be called on convexes."); } - public abstract void RayTest(int shapeIndex, in RigidPose pose, in RayData ray, ref float maximumT, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler; - public abstract void RayTest(int shapeIndex, in RigidPose pose, ref RaySource rays, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler; + /// + /// Tests a ray against a shape in the batch. + /// + /// Type of the hit handler that will have results reported to it. + /// Index of the shape in the batch to test. + /// Pose of the shape to use for the test. + /// Ray to test against the shape. + /// The maximum parametric distance along the line. May be mutated by the hit handler. + /// Hit handler that will process the reported hits. + /// Pool used for temporary allocations required by the test, if any. + public abstract void RayTest(int shapeIndex, in RigidPose pose, in RayData ray, ref float maximumT, BufferPool pool, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler; + /// + /// Tests a bunch of rays against a shape in the batch. + /// + /// Type of the hit handler that will have results reported to it. + /// Index of the shape in the batch to test. + /// Pose of the shape to use for the test. + /// Rays to test against the shape. + /// Hit handler that will process the reported hits. + /// Pool used for temporary allocations required by the test, if any. + public abstract void RayTest(int shapeIndex, in RigidPose pose, ref RaySource rays, BufferPool pool, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler; /// /// Gets a raw untyped pointer to a shape's data. @@ -266,7 +285,7 @@ internal override void ComputeBounds(int shapeIndex, Quaternion orientation, out shape.ComputeAngularExpansionData(out maximumRadius, out angularExpansion); } - public override void RayTest(int shapeIndex, in RigidPose pose, in RayData ray, ref float maximumT, ref TRayHitHandler hitHandler) + public override void RayTest(int shapeIndex, in RigidPose pose, in RayData ray, ref float maximumT, BufferPool pool, ref TRayHitHandler hitHandler) { if (shapes[shapeIndex].RayTest(pose, ray.Origin, ray.Direction, out var t, out var normal) && t <= maximumT) { @@ -274,7 +293,7 @@ public override void RayTest(int shapeIndex, in RigidPose pose, } } - public override void RayTest(int index, in RigidPose pose, ref RaySource rays, ref TRayHitHandler hitHandler) + public override void RayTest(int index, in RigidPose pose, ref RaySource rays, BufferPool pool, ref TRayHitHandler hitHandler) { WideRayTester.Test(ref shapes[index], pose, ref rays, ref hitHandler); } @@ -321,14 +340,14 @@ public override void ComputeBounds(int shapeIndex, Quaternion orientation, out V { shapes[shapeIndex].ComputeBounds(orientation, out min, out max); } - public override void RayTest(int shapeIndex, in RigidPose pose, in RayData ray, ref float maximumT, ref TRayHitHandler hitHandler) + public override void RayTest(int shapeIndex, in RigidPose pose, in RayData ray, ref float maximumT, BufferPool pool, ref TRayHitHandler hitHandler) { - shapes[shapeIndex].RayTest(pose, ray, ref maximumT, ref hitHandler); + shapes[shapeIndex].RayTest(pose, ray, ref maximumT, pool, ref hitHandler); } - public override void RayTest(int shapeIndex, in RigidPose pose, ref RaySource rays, ref TRayHitHandler hitHandler) + public override void RayTest(int shapeIndex, in RigidPose pose, ref RaySource rays, BufferPool pool, ref TRayHitHandler hitHandler) { - shapes[shapeIndex].RayTest(pose, ref rays, ref hitHandler); + shapes[shapeIndex].RayTest(pose, ref rays, pool, ref hitHandler); } } @@ -367,14 +386,14 @@ public override void ComputeBounds(int shapeIndex, Quaternion orientation, out V shapes[shapeIndex].ComputeBounds(orientation, shapeBatches, out min, out max); } - public override void RayTest(int shapeIndex, in RigidPose pose, in RayData ray, ref float maximumT, ref TRayHitHandler hitHandler) + public override void RayTest(int shapeIndex, in RigidPose pose, in RayData ray, ref float maximumT, BufferPool pool, ref TRayHitHandler hitHandler) { - shapes[shapeIndex].RayTest(pose, ray, ref maximumT, shapeBatches, ref hitHandler); + shapes[shapeIndex].RayTest(pose, ray, ref maximumT, shapeBatches, pool, ref hitHandler); } - public override void RayTest(int shapeIndex, in RigidPose pose, ref RaySource rays, ref TRayHitHandler hitHandler) + public override void RayTest(int shapeIndex, in RigidPose pose, ref RaySource rays, BufferPool pool, ref TRayHitHandler hitHandler) { - shapes[shapeIndex].RayTest(pose, ref rays, shapeBatches, ref hitHandler); + shapes[shapeIndex].RayTest(pose, ref rays, shapeBatches, pool, ref hitHandler); } } diff --git a/BepuPhysics/CollisionDetection/BroadPhase_Queries.cs b/BepuPhysics/CollisionDetection/BroadPhase_Queries.cs index aa701bba8..4e4cab498 100644 --- a/BepuPhysics/CollisionDetection/BroadPhase_Queries.cs +++ b/BepuPhysics/CollisionDetection/BroadPhase_Queries.cs @@ -23,9 +23,9 @@ struct RayLeafTester : IRayLeafTester where TRayTester : IBroadPhase public Buffer Leaves; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void TestLeaf(int leafIndex, RayData* rayData, float* maximumT) + public unsafe void TestLeaf(int leafIndex, RayData* rayData, float* maximumT, BufferPool pool) { - LeafTester.RayTest(Leaves[leafIndex], rayData, maximumT); + LeafTester.RayTest(Leaves[leafIndex], rayData, maximumT, pool); } } @@ -36,17 +36,18 @@ public unsafe void TestLeaf(int leafIndex, RayData* rayData, float* maximumT) /// Origin of the ray to cast. /// Direction of the ray to cast. /// Maximum length of the ray traversal in units of the direction's length. + /// The buffer pool used for any temporary allocations required during the traversal. /// Callback to execute on ray-leaf bounding box intersections. /// User specified id of the ray. - public unsafe void RayCast(Vector3 origin, Vector3 direction, float maximumT, ref TRayTester rayTester, int id = 0) where TRayTester : IBroadPhaseRayTester + public unsafe void RayCast(Vector3 origin, Vector3 direction, float maximumT, BufferPool pool, ref TRayTester rayTester, int id = 0) where TRayTester : IBroadPhaseRayTester { TreeRay.CreateFrom(origin, direction, maximumT, id, out var rayData, out var treeRay); RayLeafTester tester; tester.LeafTester = rayTester; tester.Leaves = ActiveLeaves; - ActiveTree.RayCast(&treeRay, &rayData, ref tester); + ActiveTree.RayCast(&treeRay, &rayData, pool, ref tester); tester.Leaves = StaticLeaves; - StaticTree.RayCast(&treeRay, &rayData, ref tester); + StaticTree.RayCast(&treeRay, &rayData, pool, ref tester); //The sweep tester probably relies on mutation to function; copy any mutations back to the original reference. rayTester = tester.LeafTester; } @@ -72,17 +73,18 @@ public void TestLeaf(int leafIndex, ref float maximumT) /// Maximum bounds of the box to sweep. /// Direction along which to sweep the bounding box. /// Maximum length of the sweep in units of the direction's length. + /// Pool to use for temporary allocations required by the traversal, if any. /// Callback to execute on sweep-leaf bounding box intersections. - public unsafe void Sweep(Vector3 min, Vector3 max, Vector3 direction, float maximumT, ref TSweepTester sweepTester) where TSweepTester : IBroadPhaseSweepTester + public unsafe void Sweep(Vector3 min, Vector3 max, Vector3 direction, float maximumT, BufferPool pool, ref TSweepTester sweepTester) where TSweepTester : IBroadPhaseSweepTester { Tree.ConvertBoxToCentroidWithExtent(min, max, out var origin, out var expansion); TreeRay.CreateFrom(origin, direction, maximumT, out var treeRay); SweepLeafTester tester; tester.LeafTester = sweepTester; tester.Leaves = ActiveLeaves; - ActiveTree.Sweep(expansion, origin, direction, &treeRay, ref tester); + ActiveTree.Sweep(expansion, origin, direction, &treeRay, pool, ref tester); tester.Leaves = StaticLeaves; - StaticTree.Sweep(expansion, origin, direction, &treeRay, ref tester); + StaticTree.Sweep(expansion, origin, direction, &treeRay, pool, ref tester); //The sweep tester probably relies on mutation to function; copy any mutations back to the original reference. sweepTester = tester.LeafTester; } @@ -94,10 +96,11 @@ public unsafe void Sweep(Vector3 min, Vector3 max, Vector3 directi /// Bounding box to sweep. /// Direction along which to sweep the bounding box. /// Maximum length of the sweep in units of the direction's length. + /// Pool used for temporary allocations required by the test, if any. /// Callback to execute on sweep-leaf bounding box intersections. - public void Sweep(in BoundingBox boundingBox, Vector3 direction, float maximumT, ref TSweepTester sweepTester) where TSweepTester : IBroadPhaseSweepTester + public void Sweep(in BoundingBox boundingBox, Vector3 direction, float maximumT, BufferPool pool, ref TSweepTester sweepTester) where TSweepTester : IBroadPhaseSweepTester { - Sweep(boundingBox.Min, boundingBox.Max, direction, maximumT, ref sweepTester); + Sweep(boundingBox.Min, boundingBox.Max, direction, maximumT, pool, ref sweepTester); } struct BoxQueryEnumerator : IBreakableForEach where TInnerEnumerator : IBreakableForEach @@ -118,15 +121,16 @@ public bool LoopBody(int i) /// Type of the enumerator to call for overlaps. /// Minimum bounds of the query box. /// Maximum bounds of the query box. + /// Pool used for temporary allocations required by the test, if any. /// Enumerator to call for overlaps. - public void GetOverlaps(Vector3 min, Vector3 max, ref TOverlapEnumerator overlapEnumerator) where TOverlapEnumerator : IBreakableForEach + public void GetOverlaps(Vector3 min, Vector3 max, BufferPool pool, ref TOverlapEnumerator overlapEnumerator) where TOverlapEnumerator : IBreakableForEach { BoxQueryEnumerator enumerator; enumerator.Enumerator = overlapEnumerator; enumerator.Leaves = ActiveLeaves; - ActiveTree.GetOverlaps(min, max, ref enumerator); + ActiveTree.GetOverlaps(min, max, pool, ref enumerator); enumerator.Leaves = StaticLeaves; - StaticTree.GetOverlaps(min, max, ref enumerator); + StaticTree.GetOverlaps(min, max, pool, ref enumerator); //Enumeration could have mutated the enumerator; preserve those modifications. overlapEnumerator = enumerator.Enumerator; } @@ -136,15 +140,16 @@ public void GetOverlaps(Vector3 min, Vector3 max, ref TOverl /// /// Type of the enumerator to call for overlaps. /// Query box bounds. + /// Pool used for temporary allocations required by the test, if any. /// Enumerator to call for overlaps. - public void GetOverlaps(in BoundingBox boundingBox, ref TOverlapEnumerator overlapEnumerator) where TOverlapEnumerator : IBreakableForEach + public void GetOverlaps(in BoundingBox boundingBox, BufferPool pool, ref TOverlapEnumerator overlapEnumerator) where TOverlapEnumerator : IBreakableForEach { BoxQueryEnumerator enumerator; enumerator.Enumerator = overlapEnumerator; enumerator.Leaves = ActiveLeaves; - ActiveTree.GetOverlaps(boundingBox, ref enumerator); + ActiveTree.GetOverlaps(boundingBox, pool, ref enumerator); enumerator.Leaves = StaticLeaves; - StaticTree.GetOverlaps(boundingBox, ref enumerator); + StaticTree.GetOverlaps(boundingBox, pool, ref enumerator); //Enumeration could have mutated the enumerator; preserve those modifications. overlapEnumerator = enumerator.Enumerator; } diff --git a/BepuPhysics/CollisionDetection/MeshReduction.cs b/BepuPhysics/CollisionDetection/MeshReduction.cs index 71b917ce5..01ffe05e7 100644 --- a/BepuPhysics/CollisionDetection/MeshReduction.cs +++ b/BepuPhysics/CollisionDetection/MeshReduction.cs @@ -445,7 +445,7 @@ public static void ReduceManifolds(ref Buffer continuationTriangles, r var contactQueryMin = meshSpaceContact - contactExpansion; var contactQueryMax = meshSpaceContact + contactExpansion; enumerator.List.Count = 0; - mesh->Tree.GetOverlaps(contactQueryMin, contactQueryMax, ref enumerator); + mesh->Tree.GetOverlaps(contactQueryMin, contactQueryMax, pool, ref enumerator); //Note that the test triangles detected by querying may exceed the count in extremely rare cases, so it's not safe to use AllocateUnsafely without some extra work. //Resizing invalidates table indices, so do any that ahead of time. testTriangles.EnsureCapacity(testTriangles.Count + enumerator.List.Count, pool); diff --git a/BepuPhysics/CollisionDetection/RayBatchers.cs b/BepuPhysics/CollisionDetection/RayBatchers.cs index c8d99af57..1a984bae5 100644 --- a/BepuPhysics/CollisionDetection/RayBatchers.cs +++ b/BepuPhysics/CollisionDetection/RayBatchers.cs @@ -9,11 +9,11 @@ namespace BepuPhysics.CollisionDetection { public interface IBroadPhaseRayTester { - unsafe void RayTest(CollidableReference collidable, RayData* rayData, float* maximumT); + unsafe void RayTest(CollidableReference collidable, RayData* rayData, float* maximumT, BufferPool pool); } public interface IBroadPhaseBatchedRayTester : IBroadPhaseRayTester { - void RayTest(CollidableReference collidable, ref RaySource rays); + void RayTest(CollidableReference collidable, ref RaySource rays, BufferPool pool); } /// @@ -31,15 +31,15 @@ struct LeafTester : IBatchedRayLeafTester public Buffer Leaves; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void RayTest(int leafIndex, ref RaySource rays) + public void RayTest(int leafIndex, ref RaySource rays, BufferPool pool) { - RayTester.RayTest(Leaves[leafIndex], ref rays); + RayTester.RayTest(Leaves[leafIndex], ref rays, pool); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void TestLeaf(int leafIndex, RayData* rayData, float* maximumT) + public unsafe void TestLeaf(int leafIndex, RayData* rayData, float* maximumT, BufferPool pool) { - RayTester.RayTest(Leaves[leafIndex], rayData, maximumT); + RayTester.RayTest(Leaves[leafIndex], rayData, maximumT, pool); } } @@ -138,24 +138,24 @@ public void OnRayHit(in RayData ray, ref float maximumT, float t, Vector3 normal [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void RayTest(CollidableReference reference, ref RaySource rays) + public unsafe void RayTest(CollidableReference reference, ref RaySource rays, BufferPool pool) { if (HitHandler.HitHandler.AllowTest(reference)) { Simulation.GetPoseAndShape(reference, out var pose, out var shape); HitHandler.Reference = reference; - Simulation.Shapes[shape.Type].RayTest(shape.Index, *pose, ref rays, ref HitHandler); + Simulation.Shapes[shape.Type].RayTest(shape.Index, *pose, ref rays, pool, ref HitHandler); } } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void RayTest(CollidableReference reference, RayData* rayData, float* maximumT) + public unsafe void RayTest(CollidableReference reference, RayData* rayData, float* maximumT, BufferPool pool) { if (HitHandler.HitHandler.AllowTest(reference)) { Simulation.GetPoseAndShape(reference, out var pose, out var shape); HitHandler.Reference = reference; - Simulation.Shapes[shape.Type].RayTest(shape.Index, *pose, *rayData, ref *maximumT, ref HitHandler); + Simulation.Shapes[shape.Type].RayTest(shape.Index, *pose, *rayData, ref *maximumT, pool, ref HitHandler); } } diff --git a/BepuPhysics/Simulation_Queries.cs b/BepuPhysics/Simulation_Queries.cs index feef42871..d2d39cdf2 100644 --- a/BepuPhysics/Simulation_Queries.cs +++ b/BepuPhysics/Simulation_Queries.cs @@ -143,13 +143,13 @@ struct RayHitDispatcher : IBroadPhaseRayTester where TRayHitHand public ShapeRayHitHandler ShapeHitHandler; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe void RayTest(CollidableReference collidable, RayData* rayData, float* maximumT) + public unsafe void RayTest(CollidableReference collidable, RayData* rayData, float* maximumT, BufferPool pool) { if (ShapeHitHandler.HitHandler.AllowTest(collidable)) { ShapeHitHandler.Collidable = collidable; Simulation.GetPoseAndShape(collidable, out var pose, out var shape); - Simulation.Shapes[shape.Type].RayTest(shape.Index, *pose, *rayData, ref *maximumT, ref ShapeHitHandler); + Simulation.Shapes[shape.Type].RayTest(shape.Index, *pose, *rayData, ref *maximumT, pool, ref ShapeHitHandler); } } } @@ -161,15 +161,16 @@ public unsafe void RayTest(CollidableReference collidable, RayData* rayData, flo /// Origin of the ray to cast. /// Direction of the ray to cast. /// Maximum length of the ray traversal in units of the direction's length. + /// Pool used for temporary allocations required by the test, if any. /// callbacks to execute on ray-object intersections. /// User specified id of the ray. - public void RayCast(Vector3 origin, Vector3 direction, float maximumT, ref THitHandler hitHandler, int id = 0) where THitHandler : IRayHitHandler + public void RayCast(Vector3 origin, Vector3 direction, float maximumT, BufferPool pool, ref THitHandler hitHandler, int id = 0) where THitHandler : IRayHitHandler { RayHitDispatcher dispatcher; dispatcher.ShapeHitHandler.HitHandler = hitHandler; dispatcher.ShapeHitHandler.Collidable = default; dispatcher.Simulation = this; - BroadPhase.RayCast(origin, direction, maximumT, ref dispatcher, id); + BroadPhase.RayCast(origin, direction, maximumT, pool, ref dispatcher, id); //The hit handler was copied to pass it into the child processing; since the user may (and probably does) rely on mutations, copy it back to the original reference. hitHandler = dispatcher.ShapeHitHandler.HitHandler; } @@ -287,7 +288,7 @@ public unsafe void Sweep(TShape shape, in RigidPose po dispatcher.MinimumProgression = minimumProgression; dispatcher.ConvergenceThreshold = convergenceThreshold; dispatcher.MaximumIterationCount = maximumIterationCount; - BroadPhase.Sweep(min, max, direction, maximumT, ref dispatcher); + BroadPhase.Sweep(min, max, direction, maximumT, pool, ref dispatcher); //The hit handler was copied to pass it into the child processing; since the user may (and probably does) rely on mutations, copy it back to the original reference. hitHandler = dispatcher.HitHandler; } diff --git a/BepuPhysics/Statics.cs b/BepuPhysics/Statics.cs index ed007f10f..2f6289cb3 100644 --- a/BepuPhysics/Statics.cs +++ b/BepuPhysics/Statics.cs @@ -251,7 +251,7 @@ void AwakenBodiesInBounds(ref BoundingBox bounds, ref TFilter filter) w if (filter.AllowAwakening) { var collector = new SleepingBodyCollector(bodies, broadPhase, pool, ref filter); - broadPhase.StaticTree.GetOverlaps(bounds, ref collector); + broadPhase.StaticTree.GetOverlaps(bounds, pool, ref collector); awakener.AwakenSets(ref collector.SleepingSets); //Just in case the filter did some internal mutation, preserve the changes. filter = collector.Filter; diff --git a/BepuPhysics/Trees/RayBatcher.cs b/BepuPhysics/Trees/RayBatcher.cs index 56f6aded6..03eaae9fe 100644 --- a/BepuPhysics/Trees/RayBatcher.cs +++ b/BepuPhysics/Trees/RayBatcher.cs @@ -111,11 +111,11 @@ public ref readonly RayData GetRay(int rayIndex) public interface IRayLeafTester { - unsafe void TestLeaf(int leafIndex, RayData* rayData, float* maximumT); + unsafe void TestLeaf(int leafIndex, RayData* rayData, float* maximumT, BufferPool pool); } public interface IBatchedRayLeafTester : IRayLeafTester { - void RayTest(int leafIndex, ref RaySource rays); + void RayTest(int leafIndex, ref RaySource rays, BufferPool pool); } @@ -432,9 +432,7 @@ public unsafe void TestRays(ref Tree tree, ref TLeafTester leafTest { Debug.Assert(stackPointerA0 == 0 && stackPointerB == 0 && stackPointerA1 == 0 && stackPointer == 0, "At the beginning of the traversal, there should exist no entries on the traversal stack."); - Debug.Assert(tree.ComputeMaximumDepth() < fallbackStack.Length, "At the moment, we assume that no tree will have more than 256 levels. " + - "This isn't a hard guarantee; if you hit this, please report it- it probably means there is some goofy pathological case badness in the builder or refiner." + - "Would be nice to replace this with a properly tracked tree depth so correctness isn't conditional."); + if (tree.LeafCount == 0) return; @@ -511,7 +509,7 @@ public unsafe void TestRays(ref Tree tree, ref TLeafTester leafTest { //This is a leaf node. var rayStackSource = new RaySource(batchRays.Memory, batchOriginalRays.Memory, rayStackStart, entry.RayCount); - leafTester.RayTest(Tree.Encode(entry.NodeIndex), ref rayStackSource); + leafTester.RayTest(Tree.Encode(entry.NodeIndex), ref rayStackSource, pool); } } else @@ -520,7 +518,7 @@ public unsafe void TestRays(ref Tree tree, ref TLeafTester leafTest for (int i = 0; i < entry.RayCount; ++i) { var rayIndex = rayStackStart[i]; - tree.RayCast(entry.NodeIndex, batchRays.Memory + rayIndex, batchOriginalRays.Memory + rayIndex, fallbackStack.Memory, ref leafTester); + tree.RayCast(entry.NodeIndex, batchRays.Memory + rayIndex, batchOriginalRays.Memory + rayIndex, fallbackStack, pool, ref leafTester); } } } diff --git a/BepuPhysics/Trees/Tree_RayCast.cs b/BepuPhysics/Trees/Tree_RayCast.cs index 945eac8d3..ce886befa 100644 --- a/BepuPhysics/Trees/Tree_RayCast.cs +++ b/BepuPhysics/Trees/Tree_RayCast.cs @@ -1,6 +1,6 @@ -using System.Diagnostics; +using BepuUtilities.Memory; +using System.Diagnostics; using System.Numerics; -using System.Runtime.CompilerServices; namespace BepuPhysics.Trees { @@ -24,7 +24,7 @@ public unsafe static bool Intersects(Vector3 min, Vector3 max, TreeRay* ray, out } - internal readonly unsafe void RayCast(int nodeIndex, TreeRay* treeRay, RayData* rayData, int* stack, ref TLeafTester leafTester) where TLeafTester : IRayLeafTester + internal readonly unsafe void RayCast(int nodeIndex, TreeRay* treeRay, RayData* rayData, Buffer stack, BufferPool pool, ref TLeafTester leafTester) where TLeafTester : IRayLeafTester { Debug.Assert((nodeIndex >= 0 && nodeIndex < NodeCount) || (Encode(nodeIndex) >= 0 && Encode(nodeIndex) < LeafCount)); Debug.Assert(LeafCount >= 2, "This implementation assumes all nodes are filled."); @@ -36,10 +36,10 @@ internal readonly unsafe void RayCast(int nodeIndex, TreeRay* treeR { //This is actually a leaf node. var leafIndex = Encode(nodeIndex); - leafTester.TestLeaf(leafIndex, rayData, &treeRay->MaximumT); + leafTester.TestLeaf(leafIndex, rayData, &treeRay->MaximumT, pool); //Leaves have no children; have to pull from the stack to get a new target. if (stackEnd == 0) - return; + break; nodeIndex = stack[--stackEnd]; } else @@ -53,7 +53,18 @@ internal readonly unsafe void RayCast(int nodeIndex, TreeRay* treeR if (bIntersected) { //Visit the earlier AABB intersection first. - Debug.Assert(stackEnd < TraversalStackCapacity - 1, "At the moment, we use a fixed size stack. Until we have explicitly tracked depths, watch out for excessive depth traversals."); + if (stackEnd == stack.Length) + { + if (stack.Length == TraversalStackCapacity) + { + // First allocation is on the stack. + pool.TakeAtLeast(TraversalStackCapacity * 2, out var newStack); + stack.CopyTo(0, newStack, 0, TraversalStackCapacity); + stack = newStack; + } + else + pool.Resize(ref stack, stackEnd * 2, stackEnd); + } if (tA < tB) { nodeIndex = node.A.Index; @@ -79,17 +90,21 @@ internal readonly unsafe void RayCast(int nodeIndex, TreeRay* treeR { //No intersection. Need to pull from the stack to get a new target. if (stackEnd == 0) - return; + break; nodeIndex = stack[--stackEnd]; } } } - + if (stack.Length > TraversalStackCapacity) + { + // We rented a larger stack at some point. Return it. + pool.Return(ref stack); + } } internal const int TraversalStackCapacity = 256; - internal readonly unsafe void RayCast(TreeRay* treeRay, RayData* rayData, ref TLeafTester leafTester) where TLeafTester : IRayLeafTester + internal readonly unsafe void RayCast(TreeRay* treeRay, RayData* rayData, BufferPool pool, ref TLeafTester leafTester) where TLeafTester : IRayLeafTester { if (LeafCount == 0) return; @@ -99,22 +114,30 @@ internal readonly unsafe void RayCast(TreeRay* treeRay, RayData* ra //If the first node isn't filled, we have to use a special case. if (Intersects(Nodes[0].A.Min, Nodes[0].A.Max, treeRay, out var tA)) { - leafTester.TestLeaf(0, rayData, &treeRay->MaximumT); + leafTester.TestLeaf(0, rayData, &treeRay->MaximumT, pool); } } else { - //TODO: Explicitly tracking depth in the tree during construction/refinement is practically required to guarantee correctness. - //While it's exceptionally rare that any tree would have more than 256 levels, the worst case of stomping stack memory is not acceptable in the long run. var stack = stackalloc int[TraversalStackCapacity]; - RayCast(0, treeRay, rayData, stack, ref leafTester); + RayCast(0, treeRay, rayData, new Buffer(stack, TraversalStackCapacity), pool, ref leafTester); } } - public readonly unsafe void RayCast(Vector3 origin, Vector3 direction, ref float maximumT, ref TLeafTester leafTester, int id = 0) where TLeafTester : IRayLeafTester + /// + /// Tests a ray against the tree and invokes the for each leaf node that the ray intersects. + /// + /// The type of the used to process the intersecting leaves. + /// The origin point of the ray. + /// The direction of the ray. + /// The maximum parametric distance along the ray to test. This value may be modified by the leaf tester during traversal. + /// The buffer pool used for temporary allocations during the operation. Only used if the tree is pathologically deep; stack memory is used preferentially. + /// A reference to the tester that processes the indices of intersecting leaves. + /// An optional identifier for the ray that can be used by the leaf tester. + public readonly unsafe void RayCast(Vector3 origin, Vector3 direction, ref float maximumT, BufferPool pool, ref TLeafTester leafTester, int id = 0) where TLeafTester : IRayLeafTester { TreeRay.CreateFrom(origin, direction, maximumT, id, out var rayData, out var treeRay); - RayCast(&treeRay, &rayData, ref leafTester); + RayCast(&treeRay, &rayData, pool, ref leafTester); //The maximumT could have been mutated by the leaf tester. Propagate that change. This is important for when we jump between tree traversals and such. maximumT = treeRay.MaximumT; } diff --git a/BepuPhysics/Trees/Tree_Sweep.cs b/BepuPhysics/Trees/Tree_Sweep.cs index a4a04cefb..a6e2bb5b3 100644 --- a/BepuPhysics/Trees/Tree_Sweep.cs +++ b/BepuPhysics/Trees/Tree_Sweep.cs @@ -1,4 +1,5 @@ using BepuUtilities; +using BepuUtilities.Memory; using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; @@ -11,7 +12,7 @@ public interface ISweepLeafTester } partial struct Tree { - readonly unsafe void Sweep(int nodeIndex, Vector3 expansion, Vector3 origin, Vector3 direction, TreeRay* treeRay, int* stack, ref TLeafTester leafTester) where TLeafTester : ISweepLeafTester + readonly unsafe void Sweep(int nodeIndex, Vector3 expansion, Vector3 origin, Vector3 direction, TreeRay* treeRay, Buffer stack, BufferPool pool, ref TLeafTester leafTester) where TLeafTester : ISweepLeafTester { Debug.Assert((nodeIndex >= 0 && nodeIndex < NodeCount) || (Encode(nodeIndex) >= 0 && Encode(nodeIndex) < LeafCount)); Debug.Assert(LeafCount >= 2, "This implementation assumes all nodes are filled."); @@ -26,7 +27,7 @@ readonly unsafe void Sweep(int nodeIndex, Vector3 expansion, Vector leafTester.TestLeaf(leafIndex, ref treeRay->MaximumT); //Leaves have no children; have to pull from the stack to get a new target. if (stackEnd == 0) - return; + break; nodeIndex = stack[--stackEnd]; } else @@ -44,7 +45,18 @@ readonly unsafe void Sweep(int nodeIndex, Vector3 expansion, Vector if (bIntersected) { //Visit the earlier AABB intersection first. - Debug.Assert(stackEnd < TraversalStackCapacity - 1, "At the moment, we use a fixed size stack. Until we have explicitly tracked depths, watch out for excessive depth traversals."); + if (stackEnd == stack.Length) + { + if (stack.Length == TraversalStackCapacity) + { + // First allocation is on the stack. + pool.TakeAtLeast(TraversalStackCapacity * 2, out var newStack); + stack.CopyTo(0, newStack, 0, TraversalStackCapacity); + stack = newStack; + } + else + pool.Resize(ref stack, stackEnd * 2, stackEnd); + } if (tA < tB) { nodeIndex = node.A.Index; @@ -70,15 +82,19 @@ readonly unsafe void Sweep(int nodeIndex, Vector3 expansion, Vector { //No intersection. Need to pull from the stack to get a new target. if (stackEnd == 0) - return; + break; nodeIndex = stack[--stackEnd]; } } } - + if (stack.Length > TraversalStackCapacity) + { + // We rented a larger stack at some point. Return it. + pool.Return(ref stack); + } } - internal readonly unsafe void Sweep(Vector3 expansion, Vector3 origin, Vector3 direction, TreeRay* treeRay, ref TLeafTester sweepTester) where TLeafTester : ISweepLeafTester + internal readonly unsafe void Sweep(Vector3 expansion, Vector3 origin, Vector3 direction, TreeRay* treeRay, BufferPool pool, ref TLeafTester sweepTester) where TLeafTester : ISweepLeafTester { if (LeafCount == 0) return; @@ -93,13 +109,18 @@ internal readonly unsafe void Sweep(Vector3 expansion, Vector3 orig } else { - //TODO: Explicitly tracking depth in the tree during construction/refinement is practically required to guarantee correctness. - //While it's exceptionally rare that any tree would have more than 256 levels, the worst case of stomping stack memory is not acceptable in the long run. var stack = stackalloc int[TraversalStackCapacity]; - Sweep(0, expansion, origin, direction, treeRay, stack, ref sweepTester); + Sweep(0, expansion, origin, direction, treeRay, new Buffer(stack, TraversalStackCapacity), pool, ref sweepTester); } } + /// + /// Converts a bounding box defined by minimum and maximum corners into a centroid and half-extent representation. + /// + /// The minimum corner of the bounding box. + /// The maximum corner of the bounding box. + /// The computed centroid of the bounding box. + /// The computed half-extents of the bounding box. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ConvertBoxToCentroidWithExtent(Vector3 min, Vector3 max, out Vector3 origin, out Vector3 expansion) { @@ -109,16 +130,35 @@ public static void ConvertBoxToCentroidWithExtent(Vector3 min, Vector3 max, out origin = halfMax + halfMin; } - public readonly unsafe void Sweep(Vector3 min, Vector3 max, Vector3 direction, float maximumT, ref TLeafTester sweepTester) where TLeafTester : ISweepLeafTester + /// + /// Performs a sweep test of an axis-aligned bounding box against the tree and invokes the for each intersecting leaf. + /// + /// The type of the used to process the intersecting leaves. + /// The minimum corner of the axis-aligned bounding box to sweep. + /// The maximum corner of the axis-aligned bounding box to sweep. + /// The direction of the sweep. + /// The maximum parametric distance along the sweep direction to test. + /// A reference to the tester that processes the indices of intersecting leaves. + /// The buffer pool used for temporary allocations during the operation. Only used if the tree is pathologically deep; stack memory is used preferentially. + public readonly unsafe void Sweep(Vector3 min, Vector3 max, Vector3 direction, float maximumT, BufferPool pool, ref TLeafTester sweepTester) where TLeafTester : ISweepLeafTester { ConvertBoxToCentroidWithExtent(min, max, out var origin, out var expansion); TreeRay.CreateFrom(origin, direction, maximumT, out var treeRay); - Sweep(expansion, origin, direction, &treeRay, ref sweepTester); + Sweep(expansion, origin, direction, &treeRay, pool, ref sweepTester); } + /// + /// Performs a sweep test of a bounding box against the tree and invokes the for each intersecting leaf. + /// + /// The type of the used to process the intersecting leaves. + /// The bounding box to sweep. + /// The direction of the sweep. + /// The maximum parametric distance along the sweep direction to test. + /// A reference to the tester that processes the indices of intersecting leaves. + /// The buffer pool used for temporary allocations during the operation. Only used if the tree is pathologically deep; stack memory is used preferentially. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public readonly void Sweep(in BoundingBox boundingBox, Vector3 direction, float maximumT, ref TLeafTester sweepTester) where TLeafTester : ISweepLeafTester + public readonly void Sweep(in BoundingBox boundingBox, Vector3 direction, float maximumT, BufferPool pool, ref TLeafTester sweepTester) where TLeafTester : ISweepLeafTester { - Sweep(boundingBox.Min, boundingBox.Max, direction, maximumT, ref sweepTester); + Sweep(boundingBox.Min, boundingBox.Max, direction, maximumT, pool, ref sweepTester); } } diff --git a/BepuPhysics/Trees/Tree_VolumeQuery.cs b/BepuPhysics/Trees/Tree_VolumeQuery.cs index 18cf9ae98..6956ba603 100644 --- a/BepuPhysics/Trees/Tree_VolumeQuery.cs +++ b/BepuPhysics/Trees/Tree_VolumeQuery.cs @@ -1,4 +1,5 @@ using BepuUtilities; +using BepuUtilities.Memory; using System.Diagnostics; using System.Numerics; using System.Runtime.CompilerServices; @@ -7,7 +8,7 @@ namespace BepuPhysics.Trees { partial struct Tree { - unsafe readonly void GetOverlaps(int nodeIndex, BoundingBox boundingBox, int* stack, ref TEnumerator leafEnumerator) where TEnumerator : IBreakableForEach + unsafe readonly void GetOverlaps(int nodeIndex, BoundingBox boundingBox, Buffer stack, BufferPool pool, ref TEnumerator leafEnumerator) where TEnumerator : IBreakableForEach { Debug.Assert((nodeIndex >= 0 && nodeIndex < NodeCount) || (Encode(nodeIndex) >= 0 && Encode(nodeIndex) < LeafCount)); Debug.Assert(LeafCount >= 2, "This implementation assumes all nodes are filled."); @@ -20,10 +21,10 @@ unsafe readonly void GetOverlaps(int nodeIndex, BoundingBox boundin //This is actually a leaf node. var leafIndex = Encode(nodeIndex); if (!leafEnumerator.LoopBody(leafIndex)) - return; + break; //Leaves have no children; have to pull from the stack to get a new target. if (stackEnd == 0) - return; + break; nodeIndex = stack[--stackEnd]; } else @@ -37,8 +38,18 @@ unsafe readonly void GetOverlaps(int nodeIndex, BoundingBox boundin nodeIndex = node.A.Index; if (bIntersected) { - //Visit the earlier AABB intersection first. - Debug.Assert(stackEnd < TraversalStackCapacity - 1, "At the moment, we use a fixed size stack. Until we have explicitly tracked depths, watch out for excessive depth traversals."); + if (stackEnd == stack.Length) + { + if (stack.Length == TraversalStackCapacity) + { + // First allocation is on the stack. + pool.TakeAtLeast(TraversalStackCapacity * 2, out var newStack); + stack.CopyTo(0, newStack, 0, TraversalStackCapacity); + stack = newStack; + } + else + pool.Resize(ref stack, stackEnd * 2, stackEnd); + } stack[stackEnd++] = node.B.Index; } @@ -51,21 +62,33 @@ unsafe readonly void GetOverlaps(int nodeIndex, BoundingBox boundin { //No intersection. Need to pull from the stack to get a new target. if (stackEnd == 0) - return; + break; nodeIndex = stack[--stackEnd]; } } } + if (stack.Length > TraversalStackCapacity) + { + // We rented a larger stack at some point. Return it. + pool.Return(ref stack); + } } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public readonly unsafe void GetOverlaps(BoundingBox boundingBox, ref TEnumerator leafEnumerator) where TEnumerator : IBreakableForEach + + /// + /// Finds and processes all leaves with bounding boxes that overlap the specified axis-aligned bounding box. The is invoked + /// for each overlapping element. + /// + /// The type of the enumerator used to process the overlapping elements. + /// Query to test against the bounding volume hierarchy. + /// The buffer pool used for temporary allocations during the operation. Only used if the tree is pathologically deep; stack memory is used preferentially. + /// A reference to the enumerator that processes the indices of overlapping elements. The enumerator can + /// terminate early by returning from its iteration. + public readonly unsafe void GetOverlaps(BoundingBox boundingBox, BufferPool pool, ref TEnumerator leafEnumerator) where TEnumerator : IBreakableForEach { if (LeafCount > 1) { - //TODO: Explicitly tracking depth in the tree during construction/refinement is practically required to guarantee correctness. - //While it's exceptionally rare that any tree would have more than 256 levels, the worst case of stomping stack memory is not acceptable in the long run. var stack = stackalloc int[TraversalStackCapacity]; - GetOverlaps(0, boundingBox, stack, ref leafEnumerator); + GetOverlaps(0, boundingBox, new Buffer(stack, TraversalStackCapacity), pool, ref leafEnumerator); } else if (LeafCount == 1) { @@ -79,10 +102,19 @@ public readonly unsafe void GetOverlaps(BoundingBox boundingBox, re //If the leaf count is zero, then there's nothing to test against. } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public readonly void GetOverlaps(Vector3 min, Vector3 max, ref TEnumerator leafEnumerator) where TEnumerator : IBreakableForEach + /// + /// Finds and processes all leaves with bounding boxes that overlap the specified axis-aligned bounding box. The is invoked + /// for each overlapping element. + /// + /// The type of the enumerator used to process the overlapping elements. + /// The minimum corner of the axis-aligned bounding box. + /// The maximum corner of the axis-aligned bounding box. + /// The buffer pool used for temporary allocations during the operation. Only used if the tree is pathologically deep; stack memory is used preferentially. + /// A reference to the enumerator that processes the indices of overlapping elements. The enumerator can + /// terminate early by returning from its iteration. + public readonly void GetOverlaps(Vector3 min, Vector3 max, BufferPool pool, ref TEnumerator leafEnumerator) where TEnumerator : IBreakableForEach { - GetOverlaps(new BoundingBox(min, max), ref leafEnumerator); + GetOverlaps(new BoundingBox(min, max), pool, ref leafEnumerator); } diff --git a/DemoBenchmarks/ShapeRayBenchmarksDeep.cs b/DemoBenchmarks/ShapeRayBenchmarksDeep.cs index 6ec12ea13..63c4c9e57 100644 --- a/DemoBenchmarks/ShapeRayBenchmarksDeep.cs +++ b/DemoBenchmarks/ShapeRayBenchmarksDeep.cs @@ -82,7 +82,7 @@ unsafe Vector3 Test() where TShape : unmanaged, IShape { ref var iteration = ref iterations[i]; float maximumT = float.MaxValue; - shapes[TShape.TypeId].RayTest(0, iteration.Pose, iteration.Ray, ref maximumT, ref hitHandler); + shapes[TShape.TypeId].RayTest(0, iteration.Pose, iteration.Ray, ref maximumT, pool, ref hitHandler); } return hitHandler.ResultSum; } diff --git a/DemoTests/InertiaTensorTests.cs b/DemoTests/InertiaTensorTests.cs index 10eca90ae..b953d23f9 100644 --- a/DemoTests/InertiaTensorTests.cs +++ b/DemoTests/InertiaTensorTests.cs @@ -311,7 +311,7 @@ private static void TestCompound(Random random, BufferPool pool) { var sampleLocation = sampleMin + new Vector3(i, j, k) * sampleSpacing; var previousCount = hitCounter.Counter; - compound.RayTest(pose, new RayData { Origin = sampleLocation, Direction = Vector3.UnitY }, ref maximumT, shapes, ref hitCounter); + compound.RayTest(pose, new RayData { Origin = sampleLocation, Direction = Vector3.UnitY }, ref maximumT, shapes, pool, ref hitCounter); //If the ray hit more than one shape, then we count them all. //This matches how the analytic inertia is calculated- every shape provides its own tensor, and they're summed. //(Notably, if you wanted non-overlapping inertia, this is counterproductive!) diff --git a/Demos/DemoHarness.cs b/Demos/DemoHarness.cs index a8de0fae2..f206fbc9a 100644 --- a/Demos/DemoHarness.cs +++ b/Demos/DemoHarness.cs @@ -268,7 +268,7 @@ public void Update(float dt) incrementalGrabRotation = Quaternion.Identity; grabberCachedMousePosition = null; } - grabber.Update(demo.Simulation, camera, input.MouseLocked, controls.Grab.IsDown(input), incrementalGrabRotation, window.GetNormalizedMousePosition(input.MousePosition)); + grabber.Update(demo.Simulation, camera, input.MouseLocked, controls.Grab.IsDown(input), incrementalGrabRotation, window.GetNormalizedMousePosition(input.MousePosition), demo.BufferPool); diff --git a/Demos/Demos/CollisionQueryDemo.cs b/Demos/Demos/CollisionQueryDemo.cs index 30c6b8e97..8ed1903a4 100644 --- a/Demos/Demos/CollisionQueryDemo.cs +++ b/Demos/Demos/CollisionQueryDemo.cs @@ -138,7 +138,7 @@ void GetPoseAndShape(CollidableReference reference, out RigidPose pose, out Type public unsafe void AddQueryToBatch(int queryShapeType, void* queryShapeData, int queryShapeSize, Vector3 queryBoundsMin, Vector3 queryBoundsMax, in RigidPose queryPose, int queryId, ref CollisionBatcher batcher) { var broadPhaseEnumerator = new BroadPhaseOverlapEnumerator { Pool = BufferPool, References = new QuickList(16, BufferPool) }; - Simulation.BroadPhase.GetOverlaps(queryBoundsMin, queryBoundsMax, ref broadPhaseEnumerator); + Simulation.BroadPhase.GetOverlaps(queryBoundsMin, queryBoundsMax, BufferPool, ref broadPhaseEnumerator); for (int overlapIndex = 0; overlapIndex < broadPhaseEnumerator.References.Count; ++overlapIndex) { GetPoseAndShape(broadPhaseEnumerator.References[overlapIndex], out var pose, out var shapeIndex); @@ -188,7 +188,7 @@ public unsafe void AddQueryToBatch(Shapes shapes, TypedIndex queryShapeIndex, in shapeBatch.ComputeBounds(queryShapeIndex.Index, queryPose, out var queryBoundsMin, out var queryBoundsMax); Simulation.Shapes[queryShapeIndex.Type].GetShapeData(queryShapeIndex.Index, out var queryShapeData, out _); var broadPhaseEnumerator = new BroadPhaseOverlapEnumerator { Pool = BufferPool, References = new QuickList(16, BufferPool) }; - Simulation.BroadPhase.GetOverlaps(queryBoundsMin, queryBoundsMax, ref broadPhaseEnumerator); + Simulation.BroadPhase.GetOverlaps(queryBoundsMin, queryBoundsMax, BufferPool, ref broadPhaseEnumerator); for (int overlapIndex = 0; overlapIndex < broadPhaseEnumerator.References.Count; ++overlapIndex) { GetPoseAndShape(broadPhaseEnumerator.References[overlapIndex], out var pose, out var shapeIndex); diff --git a/Demos/Demos/CustomVoxelCollidableDemo.cs b/Demos/Demos/CustomVoxelCollidableDemo.cs index bd114b4a7..060029a7b 100644 --- a/Demos/Demos/CustomVoxelCollidableDemo.cs +++ b/Demos/Demos/CustomVoxelCollidableDemo.cs @@ -109,7 +109,7 @@ unsafe struct HitLeafTester : IRayLeafTester where T : IShapeRayHitHandler public RayData OriginalRay; [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void TestLeaf(int leafIndex, RayData* ray, float* maximumT) + public void TestLeaf(int leafIndex, RayData* ray, float* maximumT, BufferPool pool) { ref var voxelIndex = ref VoxelIndices[leafIndex]; //Note that you could make use of the voxel grid's regular structure to save some work dealing with orientations. @@ -131,7 +131,8 @@ public void TestLeaf(int leafIndex, RayData* ray, float* maximumT) /// Ray to test against the voxels. /// Maximum length of the ray in units of the ray direction length. /// Callback to execute for every hit. - public readonly void RayTest(in RigidPose pose, in RayData ray, ref float maximumT, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler + /// Pool used for temporary allocations required by the test, if any. + public readonly void RayTest(in RigidPose pose, in RayData ray, ref float maximumT, BufferPool pool, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler { HitLeafTester leafTester; leafTester.VoxelIndices = VoxelIndices; @@ -142,7 +143,7 @@ public readonly void RayTest(in RigidPose pose, in RayData ray, leafTester.OriginalRay = ray; Matrix3x3.TransformTranspose(ray.Origin - pose.Position, leafTester.Orientation, out var localOrigin); Matrix3x3.TransformTranspose(ray.Direction, leafTester.Orientation, out var localDirection); - Tree.RayCast(localOrigin, localDirection, ref maximumT, ref leafTester); + Tree.RayCast(localOrigin, localDirection, ref maximumT, pool, ref leafTester); //The leaf tester could have mutated the hit handler; copy it back over. hitHandler = leafTester.HitHandler; } @@ -156,7 +157,8 @@ public readonly void RayTest(in RigidPose pose, in RayData ray, /// Pose of the voxels during the ray test. /// Set of rays to cast against the voxels. /// Callbacks to execute. - public readonly unsafe void RayTest(in RigidPose pose, ref RaySource rays, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler + /// Pool used for temporary allocations required by the test, if any. + public readonly unsafe void RayTest(in RigidPose pose, ref RaySource rays, BufferPool pool, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler { HitLeafTester leafTester; leafTester.VoxelIndices = VoxelIndices; @@ -171,7 +173,7 @@ public readonly unsafe void RayTest(in RigidPose pose, ref RaySo leafTester.OriginalRay = *ray; Matrix3x3.Transform(ray->Origin - pose.Position, inverseOrientation, out var localOrigin); Matrix3x3.Transform(ray->Direction, inverseOrientation, out var localDirection); - Tree.RayCast(localOrigin, localDirection, ref *maximumT, ref leafTester); + Tree.RayCast(localOrigin, localDirection, ref *maximumT, pool, ref leafTester); } //The leaf tester could have mutated the hit handler; copy it back over. hitHandler = leafTester.HitHandler; @@ -222,7 +224,7 @@ public readonly unsafe void FindLocalOverlaps(ref B ref var pair = ref pairs[i]; ref var voxelsSet = ref Unsafe.AsRef(pair.Container); enumerator.Overlaps = Unsafe.AsPointer(ref overlaps.GetOverlapsForPair(i)); - voxelsSet.Tree.GetOverlaps(pair.Min, pair.Max, ref enumerator); + voxelsSet.Tree.GetOverlaps(pair.Min, pair.Max, pool, ref enumerator); } } @@ -234,7 +236,7 @@ public readonly unsafe void FindLocalOverlaps(Vector3 min, Vector3 ma ShapeTreeSweepLeafTester enumerator; enumerator.Pool = pool; enumerator.Overlaps = overlaps; - Tree.Sweep(min, max, sweep, maximumT, ref enumerator); + Tree.Sweep(min, max, sweep, maximumT, pool, ref enumerator); } public void Dispose(BufferPool pool) diff --git a/Demos/Demos/RayCastingDemo.cs b/Demos/Demos/RayCastingDemo.cs index 758c24391..0b903ae5c 100644 --- a/Demos/Demos/RayCastingDemo.cs +++ b/Demos/Demos/RayCastingDemo.cs @@ -319,13 +319,14 @@ unsafe int UnbatchedWorker(int workerIndex, IntersectionAlgorithm algorithm) int intersectionCount = 0; var hitHandler = new HitHandler { Hits = algorithm.Results, IntersectionCount = &intersectionCount }; int claimedIndex; + var pool = ThreadDispatcher.WorkerPools[workerIndex]; while ((claimedIndex = Interlocked.Increment(ref algorithm.JobIndex)) < jobs.Length) { ref var job = ref jobs[claimedIndex]; for (int i = job.Start; i < job.End; ++i) { ref var ray = ref testRays[i]; - Simulation.RayCast(ray.Origin, ray.Direction, ray.MaximumT, ref hitHandler, i); + Simulation.RayCast(ray.Origin, ray.Direction, ray.MaximumT, pool, ref hitHandler, i); } } return intersectionCount; diff --git a/Demos/Grabber.cs b/Demos/Grabber.cs index 72b30de65..ea29003dd 100644 --- a/Demos/Grabber.cs +++ b/Demos/Grabber.cs @@ -3,6 +3,7 @@ using BepuPhysics.Constraints; using BepuPhysics.Trees; using BepuUtilities; +using BepuUtilities.Memory; using DemoRenderer; using DemoRenderer.Constraints; using System; @@ -66,7 +67,7 @@ readonly void CreateMotorDescription(Vector3 target, float inverseMass, out OneB }; } - public void Update(Simulation simulation, Camera camera, bool mouseLocked, bool shouldGrab, Quaternion rotation, in Vector2 normalizedMousePosition) + public void Update(Simulation simulation, Camera camera, bool mouseLocked, bool shouldGrab, Quaternion rotation, in Vector2 normalizedMousePosition, BufferPool pool) { //On the off chance some demo modifies the kinematic state, treat that as a grab terminator. var bodyExists = body.Exists && !body.Kinematic; @@ -88,7 +89,7 @@ public void Update(Simulation simulation, Camera camera, bool mouseLocked, bool var rayDirection = camera.GetRayDirection(mouseLocked, normalizedMousePosition); var hitHandler = default(RayHitHandler); hitHandler.T = float.MaxValue; - simulation.RayCast(camera.Position, rayDirection, float.MaxValue, ref hitHandler); + simulation.RayCast(camera.Position, rayDirection, float.MaxValue, pool, ref hitHandler); if (hitHandler.T < float.MaxValue && hitHandler.HitCollidable.Mobility == CollidableMobility.Dynamic) { //Found something to grab! diff --git a/Demos/SpecializedTests/IntertreeThreadingTests.cs b/Demos/SpecializedTests/IntertreeThreadingTests.cs index e14bfe696..621ad439c 100644 --- a/Demos/SpecializedTests/IntertreeThreadingTests.cs +++ b/Demos/SpecializedTests/IntertreeThreadingTests.cs @@ -139,7 +139,7 @@ unsafe static void TestTrees(BufferPool pool, IThreadDispatcher threadDispatcher { GetBoundsForLeaf(smaller, i, out var bounds); bruteResultsEnumerator.QuerySourceIndex = i; - larger.GetOverlaps(bounds, ref bruteResultsEnumerator); + larger.GetOverlaps(bounds, pool, ref bruteResultsEnumerator); } SortPairs(bruteResultsEnumerator.Pairs); diff --git a/Demos/SpecializedTests/VolumeQueryTests.cs b/Demos/SpecializedTests/VolumeQueryTests.cs index 94c4a9a9e..9cefc9bab 100644 --- a/Demos/SpecializedTests/VolumeQueryTests.cs +++ b/Demos/SpecializedTests/VolumeQueryTests.cs @@ -177,13 +177,14 @@ unsafe int Worker1(int workerIndex, BoxQueryAlgorithm algorithm) int intersectionCount = 0; var hitHandler = new HitHandler { IntersectionCount = &intersectionCount }; int claimedIndex; + var pool = ThreadDispatcher.WorkerPools[workerIndex]; while ((claimedIndex = Interlocked.Increment(ref algorithm.JobIndex)) < jobs.Length) { ref var job = ref jobs[claimedIndex]; for (int i = job.Start; i < job.End; ++i) { ref var box = ref queryBoxes[i]; - Simulation.BroadPhase.GetOverlaps(box, ref hitHandler); + Simulation.BroadPhase.GetOverlaps(box, pool, ref hitHandler); } } return intersectionCount; From 9cdaaff2a9ea9fede39b1894ebb8c7cd0e22318c Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 2 Sep 2025 00:07:18 -0500 Subject: [PATCH 932/947] Eliminated a subtle double-scaled epsilon oopsy and added a scale scan test demo. --- .../CollisionTasks/ManifoldCandidateHelper.cs | 2 +- Demos/Demos/PerBodyGravityDemo.cs | 13 +++- .../ManifoldReductionScaleTestDemo.cs | 76 +++++++++++++++++++ 3 files changed, 86 insertions(+), 5 deletions(-) create mode 100644 Demos/SpecializedTests/ManifoldReductionScaleTestDemo.cs diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/ManifoldCandidateHelper.cs b/BepuPhysics/CollisionDetection/CollisionTasks/ManifoldCandidateHelper.cs index c74976a19..e41a473ae 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/ManifoldCandidateHelper.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/ManifoldCandidateHelper.cs @@ -188,7 +188,7 @@ private static void InternalReduce(ref ManifoldCandidate candidates, int maxCand //minor todo: don't really need to waste time initializing to an invalid value. var bestScore = new Vector(-float.MaxValue); //While depth is the dominant heuristic, extremity is used as a bias to keep initial contact selection a little more consistent in near-equal cases. - var extremityScale = epsilonScale * 1e-2f; + const float extremityScale = 1e-2f; for (int i = 0; i < maxCandidateCount; ++i) { ref var candidate = ref Unsafe.Add(ref candidates, i); diff --git a/Demos/Demos/PerBodyGravityDemo.cs b/Demos/Demos/PerBodyGravityDemo.cs index 0d4ce830f..cf48ab9d6 100644 --- a/Demos/Demos/PerBodyGravityDemo.cs +++ b/Demos/Demos/PerBodyGravityDemo.cs @@ -17,7 +17,7 @@ namespace Demos.Demos; /// public class PerBodyGravityDemo : Demo { - struct PerBodyGravityDemoCallbacks : IPoseIntegratorCallbacks + internal struct PerBodyGravityDemoCallbacks : IPoseIntegratorCallbacks { /// /// Maps body handles to per-body gravity values. @@ -29,8 +29,13 @@ struct PerBodyGravityDemoCallbacks : IPoseIntegratorCallbacks /// /// Used to look up body handles using the callback-provided body indices. /// - private Bodies bodies; - + private Bodies bodies; + + public PerBodyGravityDemoCallbacks(CollidableProperty bodyGravities) : this() + { + BodyGravities = bodyGravities; + } + public readonly AngularIntegrationMode AngularIntegrationMode => AngularIntegrationMode.Nonconserving; public readonly bool AllowSubstepsForUnconstrainedBodies => false; @@ -92,7 +97,7 @@ public override void Initialize(ContentArchive content, Camera camera) //The CollidableProperty is a helper that associates body handles to whatever data you'd like to store. You don't have to use it, but it's fairly convenient. var bodyGravities = new CollidableProperty(BufferPool); - Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new PerBodyGravityDemoCallbacks() { BodyGravities = bodyGravities }, new SolveDescription(4, 1)); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new PerBodyGravityDemoCallbacks(bodyGravities), new SolveDescription(4, 1)); Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(new Box(1000, 10, 1000)))); diff --git a/Demos/SpecializedTests/ManifoldReductionScaleTestDemo.cs b/Demos/SpecializedTests/ManifoldReductionScaleTestDemo.cs new file mode 100644 index 000000000..9171178df --- /dev/null +++ b/Demos/SpecializedTests/ManifoldReductionScaleTestDemo.cs @@ -0,0 +1,76 @@ +using System; +using System.Linq; +using System.Numerics; +using BepuPhysics; +using BepuPhysics.Collidables; +using BepuPhysics.Constraints; +using BepuUtilities; +using DemoContentLoader; +using DemoRenderer; +using DemoRenderer.UI; +using Demos.Demos; +using DemoUtilities; + +namespace Demos.SpecializedTests; + +/// +/// Stress tests contact manifold reduction heuristics at a variety of scales using box-box tests. +/// +public class ManifoldReductionScaleTestDemo : Demo +{ + public override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(0, 0.5f, -3); + camera.Yaw = MathF.PI; + camera.Pitch = -0.3f; + + var bodyGravities = new CollidableProperty(BufferPool); + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(10, 1), float.MaxValue, 0.01f), + new PerBodyGravityDemo.PerBodyGravityDemoCallbacks(bodyGravities), new SolveDescription(8, 1)); + + var scales = new[] { 0.001f, 0.01f, 0.1f, 1f, 10f, 100f, 1000f }; + var offsets = new[] { new Vector3(-0.5f, 0, -0.5f), new Vector3(-0.25f, 0, -0.25f), new Vector3(-0.125f, 0, -0.125f), new Vector3(-0.5f, 0, 0), new Vector3(-0.25f, 0, 0), new Vector3(-0.125f, 0, 0), }; + + const int rotationSteps = 256; + const float maxRotationTop = MathF.PI * 2f; + const float maxRotationBottom = MathF.PI * 3f; + + const float pairSpacing = 4f; + + for (int scaleIndex = 0; scaleIndex < scales.Length; scaleIndex++) + { + var scale = scales[scaleIndex]; + var scaleGroupY = scaleIndex * pairSpacing * scale; + var size = new Vector3(2f, 1f, 2f) * scale; + var box = new Box(size.X, size.Y, size.Z); + var shapeIndex = Simulation.Shapes.Add(box); + var boxInertia = box.ComputeInertia(1f); + for (int offsetIndex = 0; offsetIndex < offsets.Length; ++offsetIndex) + { + var offsetGroupZ = offsetIndex * pairSpacing * scale; + var offset = offsets[offsetIndex] * scale; + + for (int rotationIndex = 0; rotationIndex < rotationSteps; rotationIndex++) + { + var rotationAngleTop = (float)rotationIndex / (rotationSteps - 1) * maxRotationTop; + var rotationAngleBottom = (float)rotationIndex / (rotationSteps - 1) * maxRotationBottom; + var pairX = (rotationIndex - rotationSteps / 2) * pairSpacing * scale; + + Simulation.Statics.Add(new StaticDescription( + new RigidPose(new Vector3(pairX, scaleGroupY, offsetGroupZ), Quaternion.CreateFromAxisAngle(Vector3.UnitY, rotationAngleBottom)), shapeIndex)); + + // Create top box (dynamic) - will be rotated around Y axis + var bodyHandle = Simulation.Bodies.Add(BodyDescription.CreateDynamic( + new RigidPose(new Vector3(pairX, size.Y + scaleGroupY, offsetGroupZ) + offset, Quaternion.CreateFromAxisAngle(Vector3.UnitY, rotationAngleTop)), + boxInertia, shapeIndex, -0.01f)); + bodyGravities.Allocate(bodyHandle) = -10 * scale; + } + } + } + + var groundBox = new Box(100000, 0.1f, 100000); + Simulation.Statics.Add(new StaticDescription( + new RigidPose(new Vector3(0, -1, 0), Quaternion.Identity), + Simulation.Shapes.Add(groundBox))); + } +} \ No newline at end of file From 46319bc8a22da1eeeb00ea880c851d322d6f7723 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Tue, 2 Sep 2025 00:12:23 -0500 Subject: [PATCH 933/947] prune one more claudecomment --- Demos/SpecializedTests/ManifoldReductionScaleTestDemo.cs | 1 - 1 file changed, 1 deletion(-) diff --git a/Demos/SpecializedTests/ManifoldReductionScaleTestDemo.cs b/Demos/SpecializedTests/ManifoldReductionScaleTestDemo.cs index 9171178df..1a7d9b92f 100644 --- a/Demos/SpecializedTests/ManifoldReductionScaleTestDemo.cs +++ b/Demos/SpecializedTests/ManifoldReductionScaleTestDemo.cs @@ -59,7 +59,6 @@ public override void Initialize(ContentArchive content, Camera camera) Simulation.Statics.Add(new StaticDescription( new RigidPose(new Vector3(pairX, scaleGroupY, offsetGroupZ), Quaternion.CreateFromAxisAngle(Vector3.UnitY, rotationAngleBottom)), shapeIndex)); - // Create top box (dynamic) - will be rotated around Y axis var bodyHandle = Simulation.Bodies.Add(BodyDescription.CreateDynamic( new RigidPose(new Vector3(pairX, size.Y + scaleGroupY, offsetGroupZ) + offset, Quaternion.CreateFromAxisAngle(Vector3.UnitY, rotationAngleTop)), boxInertia, shapeIndex, -0.01f)); From 52db6c4d6f61bf888b558a67fe4ec9c5bee33b19 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 20 Oct 2025 09:33:25 -0500 Subject: [PATCH 934/947] Hammering on cursor visibility apparently, in some contexts, causes something like mouse capture. Odd. --- DemoUtilities/Input.cs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/DemoUtilities/Input.cs b/DemoUtilities/Input.cs index c7b2a7d81..3e3e38a5e 100644 --- a/DemoUtilities/Input.cs +++ b/DemoUtilities/Input.cs @@ -238,11 +238,11 @@ public void Start() //This is pretty doofy, but it works reasonably well and we don't have easy access to the windows-provided capture stuff through opentk (that I'm aware of?). //Could change it later if it matters, but realistically it won't matter. MousePosition = WindowCenter; - window.CursorVisible = false; + if (window.CursorVisible) window.CursorVisible = false; } else { - window.CursorVisible = true; + if (!window.CursorVisible) window.CursorVisible = true; } } From 053ea208e2cc3e3fcbb257a170624470799f367d Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 20 Oct 2025 11:20:24 -0500 Subject: [PATCH 935/947] Slightly boosted the informativeness of the self contained demo. --- Demos/Demos/SimpleSelfContainedDemo.cs | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/Demos/Demos/SimpleSelfContainedDemo.cs b/Demos/Demos/SimpleSelfContainedDemo.cs index 45e02bca7..6ff49415f 100644 --- a/Demos/Demos/SimpleSelfContainedDemo.cs +++ b/Demos/Demos/SimpleSelfContainedDemo.cs @@ -217,9 +217,9 @@ public static void Run() //Drop a ball on a big static box. var sphere = new Sphere(1); var sphereInertia = sphere.ComputeInertia(1); - simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 5, 0), sphereInertia, simulation.Shapes.Add(sphere), 0.01f)); + var bodyHandle = simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 5, 0), sphereInertia, simulation.Shapes.Add(sphere), 0.01f)); - simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), simulation.Shapes.Add(new Box(500, 1, 500)))); + var staticHandle = simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), simulation.Shapes.Add(new Box(500, 1, 500)))); //Any IThreadDispatcher implementation can be used for multithreading. Here, we use the BepuUtilities.ThreadDispatcher implementation. var threadDispatcher = new ThreadDispatcher(Environment.ProcessorCount); @@ -233,8 +233,21 @@ public static void Run() //Note that each timestep is 0.01 units in duration, so all 100 time steps will last 1 unit of time. //(Usually, units of time are defined to be seconds, but the engine has no preconceived notions about units. All it sees are the numbers.) simulation.Timestep(0.01f, threadDispatcher); + + //You can use the body handle to look up information about a body; the bodies collection is indexable: + BodyReference bodyReference = simulation.Bodies[bodyHandle]; + //The BodyReference is a convenience wrapper that handles the memory indirections under the hood. + //You can use it to grab things like the current pose: + //Note that all the properties that return references are direct references to the body's memory. + //You can both read from it and write to it. Be advised: the API will let you break stuff! + //In principle, you could do the exact same lookup using the bodyHandle->index mapping and grab a direct pointer. + if ((i + 1) % 10 == 0) + Console.WriteLine($"Body position at timestep {i}: {bodyReference.Pose.Position}"); } + //Statics can also be grabbed. + Console.WriteLine($"Bounding box of the static floor: {simulation.Statics[staticHandle].BoundingBox}"); + //If you intend to reuse the BufferPool, disposing the simulation is a good idea- it returns all the buffers to the pool for reuse. //Here, we dispose it, but it's not really required; we immediately thereafter clear the BufferPool of all held memory. //Note that failing to dispose buffer pools can result in memory leaks. From f8223cac41406c56c6ad20214d74416c76ac2876 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Mon, 20 Oct 2025 11:23:32 -0500 Subject: [PATCH 936/947] Another clarification. --- Demos/Demos/SimpleSelfContainedDemo.cs | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/Demos/Demos/SimpleSelfContainedDemo.cs b/Demos/Demos/SimpleSelfContainedDemo.cs index 6ff49415f..d5a13f377 100644 --- a/Demos/Demos/SimpleSelfContainedDemo.cs +++ b/Demos/Demos/SimpleSelfContainedDemo.cs @@ -209,9 +209,10 @@ public void IntegrateVelocity(Vector bodyIndices, Vector3Wide position, Qua public static void Run() { //The buffer pool is a source of raw memory blobs for the engine to use. - var bufferPool = new BufferPool(); - //The following sets up a simulation with the callbacks defined above, and tells it to use 8 velocity iterations per substep and only one substep per solve. - //It uses the default SubsteppingTimestepper. You could use a custom ITimestepper implementation to customize when stages run relative to each other, or to insert more callbacks. + var bufferPool = new BufferPool(); + //The Simulation class represents a single simulated world containing some number of bodies, statics, and constraints. + //The following sets up a simulation with the callbacks defined above, and tells it to use 8 velocity iterations per substep and only one substep per solve. + //It uses the default SubsteppingTimestepper. You could use a custom ITimestepper implementation to customize when stages run relative to each other, or to insert more callbacks. var simulation = Simulation.Create(bufferPool, new NarrowPhaseCallbacks(), new PoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); //Drop a ball on a big static box. From 9c0001c06245c95ed5cfc05d989b4c3343f6ca6e Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 19 Nov 2025 20:02:59 -0600 Subject: [PATCH 937/947] Not all shapes are triangles! --- .../SweepTasks/ConvexHomogeneousCompoundSweepTask.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/BepuPhysics/CollisionDetection/SweepTasks/ConvexHomogeneousCompoundSweepTask.cs b/BepuPhysics/CollisionDetection/SweepTasks/ConvexHomogeneousCompoundSweepTask.cs index d64b9ef7b..87cdc2f97 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/ConvexHomogeneousCompoundSweepTask.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/ConvexHomogeneousCompoundSweepTask.cs @@ -45,7 +45,7 @@ protected override unsafe bool PreorderedTypeSweep( compound.GetPosedLocalChild(childIndex, out var childShape, out var childPose); if (task.Sweep( shapeDataA, ShapeTypeIndexA, RigidPose.Identity, orientationA, velocityA, - Unsafe.AsPointer(ref childShape), Triangle.Id, childPose, offsetB, orientationB, velocityB, + Unsafe.AsPointer(ref childShape), TChildType.TypeId, childPose, offsetB, orientationB, velocityB, maximumT, minimumProgression, convergenceThreshold, maximumIterationCount, out var t0Candidate, out var t1Candidate, out var hitLocationCandidate, out var hitNormalCandidate)) { From cfb5daa1837aef30a5437ac347ac583f2ffaf2b0 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Fri, 28 Nov 2025 12:51:33 -0600 Subject: [PATCH 938/947] Those aren't necessarily triangles! --- .../SweepTasks/CompoundHomogeneousCompoundSweepTask.cs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/BepuPhysics/CollisionDetection/SweepTasks/CompoundHomogeneousCompoundSweepTask.cs b/BepuPhysics/CollisionDetection/SweepTasks/CompoundHomogeneousCompoundSweepTask.cs index a36bcfee2..4a7993478 100644 --- a/BepuPhysics/CollisionDetection/SweepTasks/CompoundHomogeneousCompoundSweepTask.cs +++ b/BepuPhysics/CollisionDetection/SweepTasks/CompoundHomogeneousCompoundSweepTask.cs @@ -43,11 +43,11 @@ protected unsafe override bool PreorderedTypeSweep( compoundB.GetPosedLocalChild(triangleIndex, out var childB, out var childPoseB); ref var compoundChild = ref compoundA.GetChild(childOverlaps.ChildIndex); var compoundChildType = compoundChild.ShapeIndex.Type; - var task = sweepTasks.GetTask(compoundChildType, Triangle.Id); + var task = sweepTasks.GetTask(compoundChildType, TChildShapeB.TypeId); shapes[compoundChildType].GetShapeData(compoundChild.ShapeIndex.Index, out var compoundChildShapeData, out _); if (task.Sweep( compoundChildShapeData, compoundChildType, compoundChild.AsPose(), orientationA, velocityA, - Unsafe.AsPointer(ref childB), Triangle.Id, childPoseB, offsetB, orientationB, velocityB, + Unsafe.AsPointer(ref childB), TChildShapeB.TypeId, childPoseB, offsetB, orientationB, velocityB, maximumT, minimumProgression, convergenceThreshold, maximumIterationCount, out var t0Candidate, out var t1Candidate, out var hitLocationCandidate, out var hitNormalCandidate)) { From b754ceef82784196626ca686507439cc72491d55 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 31 Jan 2026 21:40:58 -0600 Subject: [PATCH 939/947] z is not w --- BepuUtilities/Vector4Wide.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/BepuUtilities/Vector4Wide.cs b/BepuUtilities/Vector4Wide.cs index d994fb077..cb2e97128 100644 --- a/BepuUtilities/Vector4Wide.cs +++ b/BepuUtilities/Vector4Wide.cs @@ -161,7 +161,7 @@ public static void Abs(in Vector4Wide vector, out Vector4Wide result) result.X = Vector.Abs(vector.X); result.Y = Vector.Abs(vector.Y); result.Z = Vector.Abs(vector.Z); - result.W = Vector.Abs(vector.Z); + result.W = Vector.Abs(vector.W); } [MethodImpl(MethodImplOptions.AggressiveInlining)] From 3f193bec4aa59bb78da6e5543a44b3c09f0d4a61 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 1 Mar 2026 11:31:39 -0600 Subject: [PATCH 940/947] well those are some quite latent bugs oops --- BepuPhysics/Constraints/AreaConstraint.cs | 69 +++++++++----- BepuPhysics/Constraints/VolumeConstraint.cs | 100 ++++++++++---------- 2 files changed, 97 insertions(+), 72 deletions(-) diff --git a/BepuPhysics/Constraints/AreaConstraint.cs b/BepuPhysics/Constraints/AreaConstraint.cs index 9cb68c1c1..12e4aab08 100644 --- a/BepuPhysics/Constraints/AreaConstraint.cs +++ b/BepuPhysics/Constraints/AreaConstraint.cs @@ -89,7 +89,11 @@ private static void ApplyImpulse(in Vector inverseMassA, in Vector } [MethodImpl(MethodImplOptions.AggressiveInlining)] - static void ComputeJacobian(in Vector3Wide positionA, in Vector3Wide positionB, in Vector3Wide positionC, out Vector normalLength, out Vector3Wide negatedJacobianA, out Vector3Wide jacobianB, out Vector3Wide jacobianC) + static void ComputeJacobian(in Vector3Wide positionA, in Vector3Wide positionB, in Vector3Wide positionC, + out Vector normalLength, + out Vector3Wide negatedJacobianA, out Vector3Wide jacobianB, out Vector3Wide jacobianC, + out Vector contributionA, out Vector contributionB, out Vector contributionC, + out Vector inverseJacobianLength) { //Area of a triangle with vertices a, b, and c is: //||ab x ac|| * 0.5 @@ -118,52 +122,69 @@ static void ComputeJacobian(in Vector3Wide positionA, in Vector3Wide positionB, Vector3Wide.CrossWithoutOverlap(normal, ab, out jacobianC); //Similar to the volume constraint, we could create a similar expression for jacobianA, but it's cheap to just do a couple of adds. Vector3Wide.Add(jacobianB, jacobianC, out negatedJacobianA); + //Normalize the jacobian to unit length. The jacobians are cross products of edges with the unit normal, + //giving magnitudes proportional to edge lengths (~L). Without normalization, the inverse effective mass + //scales with L², causing the accumulated impulse scale to vary with triangle size and making warm starting unstable. + //Normalizing gives a unit-length effective jacobian J_eff = inverseJacobianLength * J_raw. + //The inverse effective mass becomes a weighted average of inverse masses (always bounded), + //and the physical impulse is identical because the scaling factors cancel in the solve. + Vector3Wide.Dot(negatedJacobianA, negatedJacobianA, out contributionA); + Vector3Wide.Dot(jacobianB, jacobianB, out contributionB); + Vector3Wide.Dot(jacobianC, jacobianC, out contributionC); + var jacobianLengthSquared = contributionA + contributionB + contributionC; + //Guard against the degenerate case where edges are parallel/antiparallel (triangle collapses to a line). + jacobianLengthSquared = Vector.Max(new Vector(1e-14f), jacobianLengthSquared); + inverseJacobianLength = MathHelper.FastReciprocalSquareRoot(jacobianLengthSquared); } public static void WarmStart( in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, - in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, - in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, + in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, ref AreaConstraintPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC) { - ComputeJacobian(positionA, positionB, positionC, out _, out var negatedJacobianA, out var jacobianB, out var jacobianC); - ApplyImpulse(inertiaA.InverseMass, inertiaB.InverseMass, inertiaC.InverseMass, negatedJacobianA, jacobianB, jacobianC, accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC); + ComputeJacobian(positionA, positionB, positionC, out _, out var negatedJacobianA, out var jacobianB, out var jacobianC, out _, out _, out _, out var inverseJacobianLength); + //The accumulated impulse is in unit-jacobian space. Replay through J_eff = inverseJacobianLength * J_raw. + //Since |J_eff| = 1, the warm start magnitude is bounded by |accumulated| * max(invMass), same as a distance constraint. + ApplyImpulse(inertiaA.InverseMass, inertiaB.InverseMass, inertiaC.InverseMass, negatedJacobianA, jacobianB, jacobianC, inverseJacobianLength * accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC); } public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, float dt, float inverseDt, ref AreaConstraintPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC) { - ComputeJacobian(positionA, positionB, positionC, out var normalLength, out var negatedJacobianA, out var jacobianB, out var jacobianC); - - Vector3Wide.Dot(negatedJacobianA, negatedJacobianA, out var contributionA); - Vector3Wide.Dot(jacobianB, jacobianB, out var contributionB); - Vector3Wide.Dot(jacobianC, jacobianC, out var contributionC); - - //Protect against singularity by padding the jacobian contributions. This is very much a hack, but it's a pretty simple hack. - //Less sensitive to tuning than attempting to guard the inverseEffectiveMass itself, since that is sensitive to both scale AND mass. - - //Choose an epsilon based on the target area. Note that area ~= width^2 and our jacobian contributions are things like (ac x N) * (ac x N). - //Given that N is perpendicular to AC, ||(ac x N)|| == ||ac||, so the contribution is just ||ac||^2. Given the square, it's proportional to area and the area is a decent epsilon source. - var epsilon = 5e-4f * prestep.TargetScaledArea; - contributionA = Vector.Max(epsilon, contributionA); - contributionB = Vector.Max(epsilon, contributionB); - contributionC = Vector.Max(epsilon, contributionC); - var inverseEffectiveMass = contributionA * inertiaA.InverseMass + contributionB * inertiaB.InverseMass + contributionC * inertiaC.InverseMass; + //The area jacobians (ac x normal, normal x ab) have magnitude ~L (edge lengths). + //Without normalization, the inverse effective mass scales with L², making the accumulated impulse + //scale vary with triangle size and causing warm start instability. + //We normalize to a unit-length effective jacobian: J_eff = J_raw * inverseJacobianLength, where inverseJacobianLength = 1/|J_raw|. + //The inverse effective mass becomes a weighted average of inverse masses (always bounded), + //keeping the accumulated impulse well-scaled across substeps. + // + //The position error is scaled to match: error = (targetArea - area) * inverseJacobianLength. + //The physical impulse (inverseJacobianLength * csi applied through J_raw) is identical to the raw formulation + //because the inverseJacobianLength factors cancel. + ComputeJacobian(positionA, positionB, positionC, out var normalLength, out var negatedJacobianA, out var jacobianB, out var jacobianC, out var contributionA, out var contributionB, out var contributionC, out var inverseJacobianLength); + var inverseJacobianLengthSquared = inverseJacobianLength * inverseJacobianLength; + + //With the unit-length jacobian, the inverse effective mass is a weighted average of inverse masses, always bounded. + //Guard against degenerate configurations (e.g. triangle collapsed to a line) where all jacobian contributions are zero, + //which would cause a division by zero when computing the effective mass. + var inverseEffectiveMass = Vector.Max(new Vector(1e-14f), + inverseJacobianLengthSquared * (contributionA * inertiaA.InverseMass + contributionB * inertiaB.InverseMass + contributionC * inertiaC.InverseMass)); SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); var effectiveMass = effectiveMassCFMScale / inverseEffectiveMass; //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. - var biasVelocity = (prestep.TargetScaledArea - normalLength) * positionErrorToVelocity; + var biasVelocity = (prestep.TargetScaledArea - normalLength) * inverseJacobianLength * positionErrorToVelocity; //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); Vector3Wide.Dot(negatedJacobianA, wsvA.Linear, out var negatedVelocityContributionA); Vector3Wide.Dot(jacobianB, wsvB.Linear, out var velocityContributionB); Vector3Wide.Dot(jacobianC, wsvC.Linear, out var velocityContributionC); - var csv = velocityContributionB + velocityContributionC - negatedVelocityContributionA; + var csv = inverseJacobianLength * (velocityContributionB + velocityContributionC - negatedVelocityContributionA); var csi = (biasVelocity - csv) * effectiveMass - accumulatedImpulses * softnessImpulseScale; accumulatedImpulses += csi; - ApplyImpulse(inertiaA.InverseMass, inertiaB.InverseMass, inertiaC.InverseMass, negatedJacobianA, jacobianB, jacobianC, csi, ref wsvA, ref wsvB, ref wsvC); + ApplyImpulse(inertiaA.InverseMass, inertiaB.InverseMass, inertiaC.InverseMass, negatedJacobianA, jacobianB, jacobianC, inverseJacobianLength * csi, ref wsvA, ref wsvB, ref wsvC); } public static bool RequiresIncrementalSubstepUpdates => false; diff --git a/BepuPhysics/Constraints/VolumeConstraint.cs b/BepuPhysics/Constraints/VolumeConstraint.cs index 0ff92bd64..7e843bb65 100644 --- a/BepuPhysics/Constraints/VolumeConstraint.cs +++ b/BepuPhysics/Constraints/VolumeConstraint.cs @@ -1,4 +1,4 @@ -using BepuUtilities; +using BepuUtilities; using BepuUtilities.Memory; using System; using System.Diagnostics; @@ -8,7 +8,7 @@ namespace BepuPhysics.Constraints { /// - /// Constrains the volume of a tetrahedron connecting the centers of four bodies to match a goal volume. + /// Constrains the volume of a tetrahedron connecting the centers of four bodies to match a goal volume. /// Scaled volume computed from (ab x ac) * ad; the volume may be negative depending on the winding of the tetrahedron. /// public struct VolumeConstraint : IFourBodyConstraintDescription @@ -94,7 +94,9 @@ private static void ApplyImpulse( [MethodImpl(MethodImplOptions.AggressiveInlining)] static void ComputeJacobian(in Vector3Wide positionA, in Vector3Wide positionB, in Vector3Wide positionC, in Vector3Wide positionD, out Vector3Wide ad, - out Vector3Wide negatedJA, out Vector3Wide jacobianB, out Vector3Wide jacobianC, out Vector3Wide jacobianD) + out Vector3Wide negatedJA, out Vector3Wide jacobianB, out Vector3Wide jacobianC, out Vector3Wide jacobianD, + out Vector contributionA, out Vector contributionB, out Vector contributionC, out Vector contributionD, + out Vector inverseJacobianLength) { var ab = positionB - positionA; var ac = positionC - positionA; @@ -104,72 +106,74 @@ static void ComputeJacobian(in Vector3Wide positionA, in Vector3Wide positionB, Vector3Wide.CrossWithoutOverlap(ab, ac, out jacobianD); Vector3Wide.Add(jacobianB, jacobianC, out negatedJA); Vector3Wide.Add(jacobianD, negatedJA, out negatedJA); + //Normalize the jacobian to unit length. The raw jacobians are cross products of edges (face area vectors) with magnitude ~L². + //Normalizing gives a unit-length effective jacobian J_eff = inverseJacobianLength * J_raw where inverseJacobianLength = 1/|J_raw|. + //This keeps the inverse effective mass bounded (it becomes a weighted average of inverse masses), + //which bounds the accumulated impulse and makes warm starting stable regardless of configuration. + //The physical impulse (inverseJacobianLength cancels in the solve) is identical to the raw volume formulation. + Vector3Wide.Dot(negatedJA, negatedJA, out contributionA); + Vector3Wide.Dot(jacobianB, jacobianB, out contributionB); + Vector3Wide.Dot(jacobianC, jacobianC, out contributionC); + Vector3Wide.Dot(jacobianD, jacobianD, out contributionD); + var jacobianLengthSquared = contributionA + contributionB + contributionC + contributionD; + //Guard against the collinear degeneracy (all cross products vanish). This is far more extreme than coplanar; + //for generic coplanar configurations the cross products remain nonzero. + jacobianLengthSquared = Vector.Max(new Vector(1e-14f), jacobianLengthSquared); + inverseJacobianLength = MathHelper.FastReciprocalSquareRoot(jacobianLengthSquared); } + public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in Vector3Wide positionD, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, ref VolumeConstraintPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC, ref BodyVelocityWide wsvD) { - ComputeJacobian(positionA, positionB, positionC, positionD, out var ad, out var negatedJA, out var jacobianB, out var jacobianC, out var jacobianD); - //Vector3Wide.Dot(jacobianD, ad, out var unscaledVolume); - ApplyImpulse(inertiaA.InverseMass, inertiaB.InverseMass, inertiaC.InverseMass, inertiaD.InverseMass, negatedJA, jacobianB, jacobianC, jacobianD, accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC, ref wsvD); + ComputeJacobian(positionA, positionB, positionC, positionD, out _, out var negatedJA, out var jacobianB, out var jacobianC, out var jacobianD, out _, out _, out _, out _, out var inverseJacobianLength); + //The accumulated impulse is in unit-jacobian space. Replay through J_eff = inverseJacobianLength * J_raw. + //Since |J_eff| = 1, the warm start magnitude is bounded by |accumulated| * max(invMass), same as a distance constraint. + ApplyImpulse(inertiaA.InverseMass, inertiaB.InverseMass, inertiaC.InverseMass, inertiaD.InverseMass, negatedJA, jacobianB, jacobianC, jacobianD, inverseJacobianLength * accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC, ref wsvD); } public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in Vector3Wide positionD, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, float dt, float inverseDt, ref VolumeConstraintPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC, ref BodyVelocityWide wsvD) { - //Volume of parallelepiped with vertices a, b, c, d is: - //(ab x ac) * ad - //A tetrahedron with the same edges will have one sixth of this volume. As a constant factor, it's not relevant. So the constraint is just: - //OriginalVolume * 6 = (ab x ac) * ad - //Taking the derivative to get the velocity constraint: - //0 = d/dt(ab x ac) * ad + (ab x ac) * d/dt(ad) - //0 = d/dt(ab x ac) * ad + (ab x ac) * (d/dt(d) - d/dt(a)) - //0 = (d/dt(ab) x ac + ab x d/dt(ac)) * ad + (ab x ac) * (d/dt(d) - d/dt(a)) - //0 = ((d/dt(ab) x ac) * ad + (ab x d/dt(ac)) * ad + (ab x ac) * (d/dt(d) - d/dt(a)) - //0 = (ac x ad) * (d/dt(b) - d/dt(a)) + (ad x ab) * (d/dt(c) - d/dt(a)) + (ab x ac) * (d/dt(d) - d/dt(a)) - //Giving the linear jacobians: - //JA: -ac x ad - ad x ab - ab x ac == bd x bc - //JB: ac x ad - //JC: ad x ab - //JD: ab x ac - //We're not blending the jacobians into the effective mass or inverse mass either- even though that would save ALU time, the goal here is to minimize memory bandwidth since that - //tends to be the bottleneck for any multithreaded simulation. (Despite being a 1DOF constraint, this doesn't need to output inverse inertia tensors, so premultiplying isn't a win.) - ComputeJacobian(positionA, positionB, positionC, positionD, out var ad, out var negatedJA, out var jacobianB, out var jacobianC, out var jacobianD); - - Vector3Wide.Dot(negatedJA, negatedJA, out var contributionA); - Vector3Wide.Dot(jacobianB, jacobianB, out var contributionB); - Vector3Wide.Dot(jacobianC, jacobianC, out var contributionC); - Vector3Wide.Dot(jacobianD, jacobianD, out var contributionD); - - //Protect against singularity by padding the jacobian contributions. This is very much a hack, but it's a pretty simple hack. - //Less sensitive to tuning than attempting to guard the inverseEffectiveMass itself, since that is sensitive to both scale AND mass. - - //Choose an epsilon based on the target volume. Note that volume ~= width^3, whereas our jacobian contributions are things like (ac x ad) * (ac x ad), which is proportional - //to the area of the triangle acd squared. In other words, the contribution is ~ width^4. - //Scaling the volume by a constant factor will not match the growth rate of the jacobian contributions. - //We're going to ignore this until proven to be a noticeable problem because Vector does not expose exp or pow and this is cheap. - //Could still implement it, but it's not super high value. - var epsilon = 5e-4f * prestep.TargetScaledVolume; - contributionA = Vector.Max(epsilon, contributionA); - contributionB = Vector.Max(epsilon, contributionB); - contributionC = Vector.Max(epsilon, contributionC); - contributionD = Vector.Max(epsilon, contributionD); - var inverseEffectiveMass = contributionA * inertiaA.InverseMass + contributionB * inertiaB.InverseMass + contributionC * inertiaC.InverseMass + contributionD * inertiaD.InverseMass; + //Volume of parallelepiped with vertices a, b, c, d is V = (ab x ac) * ad. + //The raw volume jacobians (dV/dq) are cross products of edges: + //JA_raw: -(ac x ad) - (ad x ab) - (ab x ac) + //JB_raw: ac x ad + //JC_raw: ad x ab + //JD_raw: ab x ac + // + //These have magnitude ~L² (face areas), which varies with configuration and causes warm start instability. + //We normalize to a unit-length effective jacobian: J_eff = J_raw * inverseJacobianLength, where inverseJacobianLength = 1/|J_raw|. + //The inverse effective mass becomes a weighted average of inverse masses (always bounded), + //keeping the accumulated impulse well-scaled across substeps. + // + //The position error is the linearized signed distance to the constraint surface V = target: + // error = (target_V - V) / |J_raw| = (target_V - V) * inverseJacobianLength + //The physical impulse (inverseJacobianLength * csi applied through J_raw) is identical to the raw volume formulation + //because the inverseJacobianLength factors cancel. + ComputeJacobian(positionA, positionB, positionC, positionD, out var ad, out var negatedJA, out var jacobianB, out var jacobianC, out var jacobianD, out var contributionA, out var contributionB, out var contributionC, out var contributionD, out var inverseJacobianLength); + var inverseJacobianLengthSquared = inverseJacobianLength * inverseJacobianLength; + + //With the unit-length jacobian, the inverse effective mass is sum(fraction_i * invMass_i) — a weighted average of inverse masses, always bounded. + //Guard against degenerate configurations (e.g. all points collinear) where all jacobian contributions are zero, + //which would cause a division by zero when computing the effective mass. + var inverseEffectiveMass = Vector.Max(new Vector(1e-14f), + inverseJacobianLengthSquared * (contributionA * inertiaA.InverseMass + contributionB * inertiaB.InverseMass + contributionC * inertiaC.InverseMass + contributionD * inertiaD.InverseMass)); SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); var effectiveMass = effectiveMassCFMScale / inverseEffectiveMass; //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. - Vector3Wide.Dot(jacobianD, ad, out var unscaledVolume); - var biasVelocity = (prestep.TargetScaledVolume - unscaledVolume) * positionErrorToVelocity; + Vector3Wide.Dot(jacobianD, ad, out var volume); + var biasVelocity = (prestep.TargetScaledVolume - volume) * inverseJacobianLength * positionErrorToVelocity; //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); Vector3Wide.Dot(negatedJA, wsvA.Linear, out var negatedVelocityContributionA); Vector3Wide.Dot(jacobianB, wsvB.Linear, out var velocityContributionB); Vector3Wide.Dot(jacobianC, wsvC.Linear, out var velocityContributionC); Vector3Wide.Dot(jacobianD, wsvD.Linear, out var velocityContributionD); - var csv = velocityContributionB + velocityContributionC + velocityContributionD - negatedVelocityContributionA; + var csv = inverseJacobianLength * (velocityContributionB + velocityContributionC + velocityContributionD - negatedVelocityContributionA); var csi = (biasVelocity - csv) * effectiveMass - accumulatedImpulses * softnessImpulseScale; accumulatedImpulses += csi; - ApplyImpulse(inertiaA.InverseMass, inertiaB.InverseMass, inertiaC.InverseMass, inertiaD.InverseMass, negatedJA, jacobianB, jacobianC, jacobianD, csi, ref wsvA, ref wsvB, ref wsvC, ref wsvD); + ApplyImpulse(inertiaA.InverseMass, inertiaB.InverseMass, inertiaC.InverseMass, inertiaD.InverseMass, negatedJA, jacobianB, jacobianC, jacobianD, inverseJacobianLength * csi, ref wsvA, ref wsvB, ref wsvC, ref wsvD); } public static bool RequiresIncrementalSubstepUpdates => false; From c52116a34295440c4110d70099bee0943aed6059 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sun, 1 Mar 2026 12:34:27 -0600 Subject: [PATCH 941/947] missing sponsor! --- Demos/Demos/Sponsors/SponsorDemo.cs | 1 + 1 file changed, 1 insertion(+) diff --git a/Demos/Demos/Sponsors/SponsorDemo.cs b/Demos/Demos/Sponsors/SponsorDemo.cs index c9c09f807..16bc80e19 100644 --- a/Demos/Demos/Sponsors/SponsorDemo.cs +++ b/Demos/Demos/Sponsors/SponsorDemo.cs @@ -64,6 +64,7 @@ public override void LoadGraphicalContent(ContentArchive content, RenderSurface sponsors0.Add("AshleighAdams"); sponsors0.Add("LoicBaumann"); sponsors0.Add("Slayuh9"); + sponsors0.Add("rejurime"); //These supporters are those who gave 10 dollars a month (or historical backers of roughly equivalent or greater total contribution). //They get a poorly drawn animal! From 956a679f817ddc636584cf99ae0e009a1945d076 Mon Sep 17 00:00:00 2001 From: Abe M Date: Sat, 14 Feb 2026 15:29:12 -0800 Subject: [PATCH 942/947] Bug fixes! --- BepuPhysics/Constraints/TypeProcessor.cs | 34 ++++++++++++------------ BepuPhysics/Solver.cs | 6 ++--- BepuUtilities/Vector4Wide.cs | 2 +- 3 files changed, 21 insertions(+), 21 deletions(-) diff --git a/BepuPhysics/Constraints/TypeProcessor.cs b/BepuPhysics/Constraints/TypeProcessor.cs index 03c3fbd4f..44e90cb28 100644 --- a/BepuPhysics/Constraints/TypeProcessor.cs +++ b/BepuPhysics/Constraints/TypeProcessor.cs @@ -259,7 +259,7 @@ public override unsafe void ScaleAccumulatedImpulses(ref TypeBatch typeBatch, fl var dofCount = Unsafe.SizeOf() / Unsafe.SizeOf>(); var broadcastedScale = new Vector(scale); ref var impulsesBase = ref Unsafe.AsRef>(typeBatch.AccumulatedImpulses.Memory); - for (int i = 0; i < dofCount; ++i) + for (int i = 0; i < typeBatch.BundleCount * dofCount; ++i) { Unsafe.Add(ref impulsesBase, i) *= broadcastedScale; } @@ -1282,16 +1282,16 @@ public static void IntegrateVelocity - /// Takes body indices that could include metadata like kinematic flags in their upper bits and returns indices - /// with those flags stripped and with any lanes masked out by the integrationMask set to -1. - /// - /// Encoded body indices to decode. - /// Mask to apply to the body indices. + /// + /// Takes body indices that could include metadata like kinematic flags in their upper bits and returns indices + /// with those flags stripped and with any lanes masked out by the integrationMask set to -1. + /// + /// Encoded body indices to decode. + /// Mask to apply to the body indices. /// Body indices suitable for sending to to the IntegrateVelocity callback. - static Vector DecodeBodyIndices(Vector encodedBodyIndices, Vector integrationMask) - { - return (encodedBodyIndices & new Vector(Bodies.BodyReferenceMask)) | Vector.OnesComplement(integrationMask); + static Vector DecodeBodyIndices(Vector encodedBodyIndices, Vector integrationMask) + { + return (encodedBodyIndices & new Vector(Bodies.BodyReferenceMask)) | Vector.OnesComplement(integrationMask); } //[MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -1360,12 +1360,12 @@ public static void GatherAndIntegrate(Bodies.DynamicLimit))); - bodies.GatherState(encodedBodyIndices, false, out position, out orientation, out velocity, out var localInertia); - var decodedBodyIndices = DecodeBodyIndices(encodedBodyIndices, integrationMask); - IntegrateVelocity(ref integratorCallbacks, ref decodedBodyIndices, localInertia, dt, integrationMask, position, orientation, ref velocity, workerIndex, out inertia); - bodies.ScatterInertia(ref inertia, encodedBodyIndices, integrationMask); - + var integrationMask = Vector.AsVectorInt32(Vector.LessThan(Vector.AsVectorUInt32(encodedBodyIndices), new Vector(Bodies.DynamicLimit))); + bodies.GatherState(encodedBodyIndices, false, out position, out orientation, out velocity, out var localInertia); + var decodedBodyIndices = DecodeBodyIndices(encodedBodyIndices, integrationMask); + IntegrateVelocity(ref integratorCallbacks, ref decodedBodyIndices, localInertia, dt, integrationMask, position, orientation, ref velocity, workerIndex, out inertia); + bodies.ScatterInertia(ref inertia, encodedBodyIndices, integrationMask); + } else if (typeof(TBatchIntegrationMode) == typeof(BatchShouldNeverIntegrate)) { @@ -1377,7 +1377,7 @@ public static void GatherAndIntegrate(encodedBodyIndices, bundleIntegrationMode == BundleIntegrationMode.None, out position, out orientation, out velocity, out var gatheredInertia); if (bundleIntegrationMode != BundleIntegrationMode.None) - { + { var decodedBodyIndices = DecodeBodyIndices(encodedBodyIndices, integrationMask); IntegrateVelocity(ref integratorCallbacks, ref decodedBodyIndices, gatheredInertia, dt, integrationMask, position, orientation, ref velocity, workerIndex, out inertia); bodies.ScatterInertia(ref inertia, encodedBodyIndices, integrationMask); diff --git a/BepuPhysics/Solver.cs b/BepuPhysics/Solver.cs index 03f0c95a2..15d8f2993 100644 --- a/BepuPhysics/Solver.cs +++ b/BepuPhysics/Solver.cs @@ -154,7 +154,7 @@ public void SetMinimumCapacityForType(int typeId, int minimumInitialCapacityForT { if (typeId < 0) throw new ArgumentException("Type id must be nonnegative."); - if (MinimumCapacityPerTypeBatch < 0) + if (minimumInitialCapacityForType < 0) throw new ArgumentException("Capacity must be nonnegative."); if (typeId >= minimumInitialCapacityPerTypeBatch.Length) Array.Resize(ref minimumInitialCapacityPerTypeBatch, typeId + 1); @@ -1493,9 +1493,9 @@ internal void UpdateForBodyMemoryMove(int originalBodyIndex, int newBodyLocation /// Scale to apply to accumulated impulses. public void ScaleAccumulatedImpulses(ref ConstraintSet set, float scale) { - for (int batchIndex = 0; batchIndex < ActiveSet.Batches.Count; ++batchIndex) + for (int batchIndex = 0; batchIndex < set.Batches.Count; ++batchIndex) { - ref var batch = ref ActiveSet.Batches[batchIndex]; + ref var batch = ref set.Batches[batchIndex]; for (int typeBatchIndex = 0; typeBatchIndex < batch.TypeBatches.Count; ++typeBatchIndex) { ref var typeBatch = ref batch.TypeBatches[typeBatchIndex]; diff --git a/BepuUtilities/Vector4Wide.cs b/BepuUtilities/Vector4Wide.cs index cb2e97128..d6825913d 100644 --- a/BepuUtilities/Vector4Wide.cs +++ b/BepuUtilities/Vector4Wide.cs @@ -179,7 +179,7 @@ public static ref Vector4Wide Negate(ref Vector4Wide v) v.X = -v.X; v.Y = -v.Y; v.Z = -v.Z; - v.Z = -v.W; + v.W = -v.W; return ref v; } From 56d9ad784be7415a5ec08f0bf05d321a02f6fddd Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 8 Apr 2026 11:45:15 -0500 Subject: [PATCH 943/947] that's an oopsy. --- BepuPhysics/CollisionDetection/MeshReduction.cs | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/BepuPhysics/CollisionDetection/MeshReduction.cs b/BepuPhysics/CollisionDetection/MeshReduction.cs index 01ffe05e7..ab6eee96e 100644 --- a/BepuPhysics/CollisionDetection/MeshReduction.cs +++ b/BepuPhysics/CollisionDetection/MeshReduction.cs @@ -445,7 +445,12 @@ public static void ReduceManifolds(ref Buffer continuationTriangles, r var contactQueryMin = meshSpaceContact - contactExpansion; var contactQueryMax = meshSpaceContact + contactExpansion; enumerator.List.Count = 0; - mesh->Tree.GetOverlaps(contactQueryMin, contactQueryMax, pool, ref enumerator); + //The contact and the cached TestTriangle data live in scaled mesh-local space (GetLocalChild applies the mesh's scale), + //but the Tree was built from unscaled source triangles. Bring the query into the tree's space before traversing. + //Take min/max after scaling to compensate for negative scales. + var scaledQueryMin = contactQueryMin * mesh->inverseScale; + var scaledQueryMax = contactQueryMax * mesh->inverseScale; + mesh->Tree.GetOverlaps(Vector3.Min(scaledQueryMin, scaledQueryMax), Vector3.Max(scaledQueryMin, scaledQueryMax), pool, ref enumerator); //Note that the test triangles detected by querying may exceed the count in extremely rare cases, so it's not safe to use AllocateUnsafely without some extra work. //Resizing invalidates table indices, so do any that ahead of time. testTriangles.EnsureCapacity(testTriangles.Count + enumerator.List.Count, pool); From 5fdf590eacbddd24d079a86d5724ca384d6387c8 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Wed, 8 Apr 2026 12:22:46 -0500 Subject: [PATCH 944/947] First pass for generalizing MeshReduction over alternative Mesh types. --- BepuPhysics/Collidables/BigCompound.cs | 21 ++++--- BepuPhysics/Collidables/Compound.cs | 52 +++++++++++------ BepuPhysics/Collidables/Mesh.cs | 11 ++++ .../CompoundMeshContinuations.cs | 9 +-- .../ConvexCompoundOverlapFinder.cs | 12 ++++ .../CollisionTasks/ConvexMeshContinuations.cs | 5 +- .../CollisionTasks/MeshPairContinuations.cs | 9 +-- .../CompoundMeshReduction.cs | 8 ++- .../CollisionDetection/MeshReduction.cs | 56 +++++++++++++++---- Demos/Demos/CustomVoxelCollidableDemo.cs | 7 +++ 10 files changed, 142 insertions(+), 48 deletions(-) diff --git a/BepuPhysics/Collidables/BigCompound.cs b/BepuPhysics/Collidables/BigCompound.cs index 644e2c1d6..33d044bce 100644 --- a/BepuPhysics/Collidables/BigCompound.cs +++ b/BepuPhysics/Collidables/BigCompound.cs @@ -305,6 +305,13 @@ public readonly unsafe void FindLocalOverlaps(Vector3 min, Vector3 ma Tree.Sweep(min, max, sweep, maximumT, pool, ref enumerator); } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public readonly void FindLocalOverlaps(Vector3 min, Vector3 max, BufferPool pool, Shapes shapes, ref TEnumerator enumerator) + where TEnumerator : IBreakableForEach + { + Tree.GetOverlaps(min, max, pool, ref enumerator); + } + /// /// Computes the inertia of a compound. Does not recenter the child poses. /// @@ -328,13 +335,13 @@ public readonly BodyInertia ComputeInertia(Span childMasses, Shapes shape var bodyInertia = CompoundBuilder.ComputeInertia(Children, childMasses, shapes, out centerOfMass); //Recentering moves the children around, so the tree needs to be updated. //Scanning through and explicitly shifting the nodes is slightly more efficient than updating leaf bounds and refitting. - for (int i = 0; i < Tree.NodeCount; ++i) - { - ref var node = ref Tree.Nodes[i]; - node.A.Min -= centerOfMass; - node.A.Max -= centerOfMass; - node.B.Min -= centerOfMass; - node.B.Max -= centerOfMass; + for (int i = 0; i < Tree.NodeCount; ++i) + { + ref var node = ref Tree.Nodes[i]; + node.A.Min -= centerOfMass; + node.A.Max -= centerOfMass; + node.B.Min -= centerOfMass; + node.B.Max -= centerOfMass; } return bodyInertia; } diff --git a/BepuPhysics/Collidables/Compound.cs b/BepuPhysics/Collidables/Compound.cs index 9a1da6d54..dc42ec0ca 100644 --- a/BepuPhysics/Collidables/Compound.cs +++ b/BepuPhysics/Collidables/Compound.cs @@ -28,28 +28,28 @@ public struct CompoundChild /// /// Index of the shape within whatever shape collection holds the compound's child shape data. /// - public TypedIndex ShapeIndex; - - /// - /// Creates a compound child. - /// - /// Pose of the compound child in the local space of the parent shape. - /// Index of the shape used by the child. - public CompoundChild(in RigidPose pose, TypedIndex shapeIndex) - { - LocalOrientation = pose.Orientation; - LocalPosition = pose.Position; - ShapeIndex = shapeIndex; - } - + public TypedIndex ShapeIndex; + + /// + /// Creates a compound child. + /// + /// Pose of the compound child in the local space of the parent shape. + /// Index of the shape used by the child. + public CompoundChild(in RigidPose pose, TypedIndex shapeIndex) + { + LocalOrientation = pose.Orientation; + LocalPosition = pose.Position; + ShapeIndex = shapeIndex; + } + /// /// Returns a reference to the memory of the as a . /// /// Reference to this compound child as a pose. [UnscopedRef] - public ref RigidPose AsPose() - { - return ref Unsafe.As(ref this); + public ref RigidPose AsPose() + { + return ref Unsafe.As(ref this); } } @@ -336,6 +336,24 @@ public unsafe void FindLocalOverlaps(ref Buffer(Vector3 min, Vector3 max, BufferPool pool, Shapes shapes, ref TEnumerator enumerator) + where TEnumerator : IBreakableForEach + { + for (int i = 0; i < Children.Length; ++i) + { + ref var child = ref Children[i]; + shapes[child.ShapeIndex.Type].ComputeBounds(child.ShapeIndex.Index, child.LocalOrientation, out _, out _, out var childMin, out var childMax); + childMin += child.LocalPosition; + childMax += child.LocalPosition; + if (BoundingBox.Intersects(childMin, childMax, min, max)) + { + if (!enumerator.LoopBody(i)) + return; + } + } + } + public unsafe void FindLocalOverlaps(Vector3 min, Vector3 max, Vector3 sweep, float maximumT, BufferPool pool, Shapes shapes, void* overlapsPointer) where TOverlaps : ICollisionTaskSubpairOverlaps { diff --git a/BepuPhysics/Collidables/Mesh.cs b/BepuPhysics/Collidables/Mesh.cs index 68e406721..d5d078e1e 100644 --- a/BepuPhysics/Collidables/Mesh.cs +++ b/BepuPhysics/Collidables/Mesh.cs @@ -371,6 +371,17 @@ public readonly unsafe void FindLocalOverlaps(Vector3 min, Vector3 ma Tree.Sweep(Vector3.Min(scaledMin, scaledMax), Vector3.Max(scaledMin, scaledMax), scaledSweep, maximumT, pool, ref enumerator); } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public readonly void FindLocalOverlaps(Vector3 min, Vector3 max, BufferPool pool, Shapes shapes, ref TEnumerator enumerator) + where TEnumerator : IBreakableForEach + { + //The tree is built from unscaled source triangles, so the query AABB has to be brought into unscaled space. + //Take a min/max to compensate for negative scales. + var scaledMin = min * inverseScale; + var scaledMax = max * inverseScale; + Tree.GetOverlaps(Vector3.Min(scaledMin, scaledMax), Vector3.Max(scaledMin, scaledMax), pool, ref enumerator); + } + public struct MeshTriangleSource : ITriangleSource { Mesh mesh; diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundMeshContinuations.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundMeshContinuations.cs index afcd9b983..7f8ee8410 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundMeshContinuations.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundMeshContinuations.cs @@ -6,8 +6,8 @@ namespace BepuPhysics.CollisionDetection.CollisionTasks { public unsafe struct CompoundMeshContinuations : ICompoundPairContinuationHandler - where TCompound : ICompoundShape - where TMesh : IHomogeneousCompoundShape + where TCompound : struct, ICompoundShape + where TMesh : struct, IHomogeneousCompoundShape { public CollisionContinuationType CollisionContinuationType => CollisionContinuationType.CompoundMeshReduction; @@ -23,8 +23,9 @@ public ref CompoundMeshReduction CreateContinuation( collisionBatcher.Pool.Take(pairOverlaps.Length, out continuation.QueryBounds); continuation.RegionCount = pairOverlaps.Length; continuation.MeshOrientation = pair.OrientationB; - //TODO: This is not flexible with respect to different mesh types. Not a problem right now, but it will be in the future. - continuation.Mesh = (Mesh*)pair.B; + continuation.Mesh = pair.B; + continuation.FindLocalOverlapsThunk = MeshReductionThunks.FindLocalOverlaps; + continuation.GetLocalChildThunk = MeshReductionThunks.GetLocalChild; //A flip is required in mesh reduction whenever contacts are being generated as if the triangle is in slot B, which is whenever this pair has *not* been flipped. continuation.RequiresFlip = pair.FlipMask == 0; diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundOverlapFinder.cs b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundOverlapFinder.cs index a4116c886..6c226121c 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundOverlapFinder.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundOverlapFinder.cs @@ -21,6 +21,18 @@ void FindLocalOverlaps(ref Buffer(Vector3 min, Vector3 max, Vector3 sweep, float maximumT, BufferPool pool, Shapes shapes, void* overlaps) where TOverlaps : ICollisionTaskSubpairOverlaps; + + /// + /// Finds the indices of all children whose local-space bounding boxes overlap the given local-space AABB. + /// + /// Type of the enumerator that receives child indices. + /// Minimum corner of the query AABB in the compound's local space. + /// Maximum corner of the query AABB in the compound's local space. + /// Pool used for any temporary allocations during traversal. + /// Shape collection used to look up child bounds for compounds with heterogeneous children. May be null for homogeneous compounds that don't require it. + /// Enumerator that receives the indices of overlapping children. + void FindLocalOverlaps(Vector3 min, Vector3 max, BufferPool pool, Shapes shapes, ref TEnumerator enumerator) + where TEnumerator : IBreakableForEach; } public interface IConvexCompoundOverlapFinder { diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexMeshContinuations.cs b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexMeshContinuations.cs index a4aeffeb6..7428c1200 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexMeshContinuations.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexMeshContinuations.cs @@ -3,7 +3,7 @@ namespace BepuPhysics.CollisionDetection.CollisionTasks { - public struct ConvexMeshContinuations : IConvexCompoundContinuationHandler where TMesh : IHomogeneousCompoundShape + public struct ConvexMeshContinuations : IConvexCompoundContinuationHandler where TMesh : struct, IHomogeneousCompoundShape { public CollisionContinuationType CollisionContinuationType => CollisionContinuationType.MeshReduction; @@ -20,8 +20,9 @@ public unsafe ref MeshReduction CreateContinuation( continuation.RequiresFlip = pair.FlipMask == 0; continuation.QueryBounds.Min = pairQuery.Min; continuation.QueryBounds.Max = pairQuery.Max; - //TODO: This is not flexible with respect to different mesh types. Not a problem right now, but it will be in the future. continuation.Mesh = pairQuery.Container; + continuation.FindLocalOverlapsThunk = MeshReductionThunks.FindLocalOverlaps; + continuation.GetLocalChildThunk = MeshReductionThunks.GetLocalChild; return ref continuation; } diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/MeshPairContinuations.cs b/BepuPhysics/CollisionDetection/CollisionTasks/MeshPairContinuations.cs index 7afce4817..0497ac872 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/MeshPairContinuations.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/MeshPairContinuations.cs @@ -6,8 +6,8 @@ namespace BepuPhysics.CollisionDetection.CollisionTasks { public unsafe struct MeshPairContinuations : ICompoundPairContinuationHandler - where TMeshA : IHomogeneousCompoundShape - where TMeshB : IHomogeneousCompoundShape + where TMeshA : struct, IHomogeneousCompoundShape + where TMeshB : struct, IHomogeneousCompoundShape { public CollisionContinuationType CollisionContinuationType => CollisionContinuationType.CompoundMeshReduction; @@ -29,8 +29,9 @@ public ref CompoundMeshReduction CreateContinuation( continuation.MeshOrientation = pair.OrientationB; //A flip is required in mesh reduction whenever contacts are being generated as if the triangle is in slot B, which is whenever this pair has *not* been flipped. continuation.RequiresFlip = pair.FlipMask == 0; - //TODO: This is not flexible with respect to different mesh types. Not a problem right now, but it will be in the future. - continuation.Mesh = (Mesh*)pair.B; + continuation.Mesh = pair.B; + continuation.FindLocalOverlapsThunk = MeshReductionThunks.FindLocalOverlaps; + continuation.GetLocalChildThunk = MeshReductionThunks.GetLocalChild; //All regions must be assigned ahead of time. Some trailing regions may be empty, so the dispatch may occur before all children are visited in the later loop. //That would result in potentially uninitialized values in region counts. diff --git a/BepuPhysics/CollisionDetection/CompoundMeshReduction.cs b/BepuPhysics/CollisionDetection/CompoundMeshReduction.cs index 23ba9c3cc..5501b4409 100644 --- a/BepuPhysics/CollisionDetection/CompoundMeshReduction.cs +++ b/BepuPhysics/CollisionDetection/CompoundMeshReduction.cs @@ -21,7 +21,10 @@ public unsafe struct CompoundMeshReduction : ICollisionTestContinuation //This uses all of the nonconvex reduction's logic, so we just nest it. public NonconvexReduction Inner; - public Mesh* Mesh; //TODO: This is not flexible with respect to different mesh types. Not a problem right now, but it will be in the future. + //Type-erased mesh pointer plus the per-TMesh thunks. See MeshReduction for the rationale. + public void* Mesh; + public delegate* FindLocalOverlapsThunk; + public delegate* GetLocalChildThunk; public void Create(int childManifoldCount, BufferPool pool) { @@ -54,7 +57,8 @@ public bool TryFlush(int pairId, ref CollisionBatcher ba ref var region = ref ChildManifoldRegions[i]; if (region.Count > 0) { - MeshReduction.ReduceManifolds(ref Triangles, ref Inner.Children, region.Start, region.Count, RequiresFlip, QueryBounds[i], meshOrientation, meshInverseOrientation, Mesh, batcher.Pool); + MeshReduction.ReduceManifolds(ref Triangles, ref Inner.Children, region.Start, region.Count, RequiresFlip, QueryBounds[i], meshOrientation, meshInverseOrientation, + Mesh, FindLocalOverlapsThunk, GetLocalChildThunk, batcher.Shapes, batcher.Pool); } } diff --git a/BepuPhysics/CollisionDetection/MeshReduction.cs b/BepuPhysics/CollisionDetection/MeshReduction.cs index ab6eee96e..1a6c4674d 100644 --- a/BepuPhysics/CollisionDetection/MeshReduction.cs +++ b/BepuPhysics/CollisionDetection/MeshReduction.cs @@ -29,7 +29,14 @@ public unsafe struct MeshReduction : ICollisionTestContinuation //This uses all of the nonconvex reduction's logic, so we just nest it. public NonconvexReduction Inner; - public void* Mesh; //TODO: This is not flexible with respect to different mesh types. Not a problem right now, but it will be in the future. + //Type-erased pointer to the mesh shape data, plus two function pointers that close over the concrete mesh type. + //ConvexMeshContinuations populates these from MeshReductionThunks at continuation creation time. + //The thunks are static methods of a generic helper, so they're JIT-specialized per TMesh and any interface + //call inside them is devirtualized. This keeps the >=128 path's per-contact calls cheap without requiring + //CollisionBatcher to know about TMesh. + public void* Mesh; + public delegate* FindLocalOverlapsThunk; + public delegate* GetLocalChildThunk; public void Create(int childManifoldCount, BufferPool pool) { @@ -280,7 +287,7 @@ static void TryApplyBlockToTriangle(ref TestTriangle triangle, Buffer + public struct ChildEnumerator : IBreakableForEach { public QuickList List; public BufferPool Pool; @@ -292,7 +299,11 @@ public bool LoopBody(int i) } public static void ReduceManifolds(ref Buffer continuationTriangles, ref Buffer continuationChildren, int start, int count, - bool requiresFlip, in BoundingBox queryBounds, in Matrix3x3 meshOrientation, in Matrix3x3 meshInverseOrientation, Mesh* mesh, BufferPool pool) + bool requiresFlip, in BoundingBox queryBounds, in Matrix3x3 meshOrientation, in Matrix3x3 meshInverseOrientation, + void* mesh, + delegate* findLocalOverlapsThunk, + delegate* getLocalChildThunk, + Shapes shapes, BufferPool pool) { //Before handing responsibility off to the nonconvex reduction, make sure that no contacts create nasty 'bumps' at the border of triangles. //Bumps can occur when an isolated triangle test detects a contact pointing outward, like when a box hits the side. This is fine when the triangle truly is isolated, @@ -445,12 +456,9 @@ public static void ReduceManifolds(ref Buffer continuationTriangles, r var contactQueryMin = meshSpaceContact - contactExpansion; var contactQueryMax = meshSpaceContact + contactExpansion; enumerator.List.Count = 0; - //The contact and the cached TestTriangle data live in scaled mesh-local space (GetLocalChild applies the mesh's scale), - //but the Tree was built from unscaled source triangles. Bring the query into the tree's space before traversing. - //Take min/max after scaling to compensate for negative scales. - var scaledQueryMin = contactQueryMin * mesh->inverseScale; - var scaledQueryMax = contactQueryMax * mesh->inverseScale; - mesh->Tree.GetOverlaps(Vector3.Min(scaledQueryMin, scaledQueryMax), Vector3.Max(scaledQueryMin, scaledQueryMax), pool, ref enumerator); + //The thunk takes coordinates in the same space as the cached TestTriangle data (i.e. the space GetLocalChild returns), + //and is responsible for any internal coordinate-space conversion (e.g. Mesh applies its inverse scale before traversing the tree). + findLocalOverlapsThunk(mesh, contactQueryMin, contactQueryMax, pool, shapes, ref enumerator); //Note that the test triangles detected by querying may exceed the count in extremely rare cases, so it's not safe to use AllocateUnsafely without some extra work. //Resizing invalidates table indices, so do any that ahead of time. testTriangles.EnsureCapacity(testTriangles.Count + enumerator.List.Count, pool); @@ -464,7 +472,7 @@ public static void ReduceManifolds(ref Buffer continuationTriangles, r //1) in the long term, the mesh type will be abstracted away, and we might be dealing with a type that doesn't have a Triangles buffer at all. //2) the Mesh applies a scale to the stored triangles! That's why we have the continuation triangles explicitly stored rather than just looking them all up in the mesh- //the convex-triangle tests that preceded this reduction had to have somewhere they could load the 'baked' triangle data from. - mesh->GetLocalChild(triangleIndexInMesh, out var triangle); + getLocalChildThunk(mesh, triangleIndexInMesh, out var triangle); testTriangles.Values[triangleIndex] = new TestTriangle(triangle, triangleIndex); } ref var targetTriangle = ref testTriangles.Values[triangleIndex]; @@ -519,8 +527,8 @@ public bool TryFlush(int pairId, ref CollisionBatcher ba { Matrix3x3.CreateFromQuaternion(MeshOrientation, out var meshOrientation); Matrix3x3.Transpose(meshOrientation, out var meshInverseOrientation); - //TODO: This is not flexible with respect to different mesh types. Not a problem right now, but it will be in the future. - ReduceManifolds(ref Triangles, ref Inner.Children, 0, Inner.ChildCount, RequiresFlip, QueryBounds, meshOrientation, meshInverseOrientation, (Mesh*)Mesh, batcher.Pool); + ReduceManifolds(ref Triangles, ref Inner.Children, 0, Inner.ChildCount, RequiresFlip, QueryBounds, meshOrientation, meshInverseOrientation, + Mesh, FindLocalOverlapsThunk, GetLocalChildThunk, batcher.Shapes, batcher.Pool); //Now that boundary smoothing analysis is done, we no longer need the triangle list. batcher.Pool.Return(ref Triangles); @@ -531,4 +539,28 @@ public bool TryFlush(int pairId, ref CollisionBatcher ba } } + + /// + /// Type-specialized thunks that bridge MeshReduction's type-erased function pointer fields back to a concrete mesh shape type. + /// The static fields here are populated once per closed TMesh by the runtime, and the JIT specializes the bodies so that + /// the calls into are devirtualized. + /// + /// Concrete homogeneous triangle compound shape type. + public static unsafe class MeshReductionThunks where TMesh : struct, IHomogeneousCompoundShape + { + public static readonly delegate* FindLocalOverlaps = &FindLocalOverlapsImpl; + public static readonly delegate* GetLocalChild = &GetLocalChildImpl; + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + static void FindLocalOverlapsImpl(void* mesh, Vector3 min, Vector3 max, BufferPool pool, Shapes shapes, ref MeshReduction.ChildEnumerator enumerator) + { + Unsafe.AsRef(mesh).FindLocalOverlaps(min, max, pool, shapes, ref enumerator); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + static void GetLocalChildImpl(void* mesh, int childIndex, out Triangle triangle) + { + Unsafe.AsRef(mesh).GetLocalChild(childIndex, out triangle); + } + } } diff --git a/Demos/Demos/CustomVoxelCollidableDemo.cs b/Demos/Demos/CustomVoxelCollidableDemo.cs index 060029a7b..652442288 100644 --- a/Demos/Demos/CustomVoxelCollidableDemo.cs +++ b/Demos/Demos/CustomVoxelCollidableDemo.cs @@ -228,6 +228,13 @@ public readonly unsafe void FindLocalOverlaps(ref B } } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public readonly void FindLocalOverlaps(Vector3 min, Vector3 max, BufferPool pool, Shapes shapes, ref TEnumerator enumerator) + where TEnumerator : IBreakableForEach + { + Tree.GetOverlaps(min, max, pool, ref enumerator); + } + public readonly unsafe void FindLocalOverlaps(Vector3 min, Vector3 max, Vector3 sweep, float maximumT, BufferPool pool, Shapes shapes, void* overlaps) where TOverlaps : ICollisionTaskSubpairOverlaps { //Similar to the non-swept FindLocalOverlaps function above, this just adds the overlaps to the provided collection. From e2ce4856f469d66febd0c4b5ea190912314f2270 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 18 Apr 2026 14:51:48 -0500 Subject: [PATCH 945/947] slightly declauded comment style --- BepuPhysics/CollisionDetection/MeshReduction.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/BepuPhysics/CollisionDetection/MeshReduction.cs b/BepuPhysics/CollisionDetection/MeshReduction.cs index 1a6c4674d..0d08b0e61 100644 --- a/BepuPhysics/CollisionDetection/MeshReduction.cs +++ b/BepuPhysics/CollisionDetection/MeshReduction.cs @@ -32,7 +32,7 @@ public unsafe struct MeshReduction : ICollisionTestContinuation //Type-erased pointer to the mesh shape data, plus two function pointers that close over the concrete mesh type. //ConvexMeshContinuations populates these from MeshReductionThunks at continuation creation time. //The thunks are static methods of a generic helper, so they're JIT-specialized per TMesh and any interface - //call inside them is devirtualized. This keeps the >=128 path's per-contact calls cheap without requiring + //call inside them is devirtualized. This keeps the 'way too many subpairs' path's per-contact calls cheap without requiring //CollisionBatcher to know about TMesh. public void* Mesh; public delegate* FindLocalOverlapsThunk; From 55e9c3bc9cf786a40044db195d5f12e455325c21 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 18 Apr 2026 16:14:13 -0500 Subject: [PATCH 946/947] Test demo to validate the mesh type abstraction. --- .../CustomMeshSmoothingTestDemo.cs | 280 ++++++++++++++++++ 1 file changed, 280 insertions(+) create mode 100644 Demos/SpecializedTests/CustomMeshSmoothingTestDemo.cs diff --git a/Demos/SpecializedTests/CustomMeshSmoothingTestDemo.cs b/Demos/SpecializedTests/CustomMeshSmoothingTestDemo.cs new file mode 100644 index 000000000..252922a8c --- /dev/null +++ b/Demos/SpecializedTests/CustomMeshSmoothingTestDemo.cs @@ -0,0 +1,280 @@ +using System; +using System.Numerics; +using System.Runtime.CompilerServices; +using BepuPhysics; +using BepuPhysics.Collidables; +using BepuPhysics.CollisionDetection; +using BepuPhysics.CollisionDetection.CollisionTasks; +using BepuPhysics.CollisionDetection.SweepTasks; +using BepuPhysics.Constraints; +using BepuPhysics.Trees; +using BepuUtilities; +using BepuUtilities.Collections; +using BepuUtilities.Memory; +using DemoContentLoader; +using DemoRenderer; +using DemoRenderer.UI; +using DemoUtilities; + +namespace Demos.SpecializedTests; + +/// +/// Pure forwarding wrapper around . Has its own TypeId so the narrow phase treats it as a distinct shape, +/// which lets us verify that 's boundary smoothing works for any , not just the built-in Mesh type. +/// +public struct WrappedMesh : IHomogeneousCompoundShape +{ + public Mesh Inner; + + public WrappedMesh(Mesh inner) + { + Inner = inner; + } + + public const int Id = 13; + public static int TypeId => Id; + + public readonly int ChildCount => Inner.ChildCount; + + public static ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches) + { + return new HomogeneousCompoundShapeBatch(pool, initialCapacity); + } + + public readonly void ComputeBounds(Quaternion orientation, out Vector3 min, out Vector3 max) + { + Inner.ComputeBounds(orientation, out min, out max); + } + + public readonly void GetLocalChild(int childIndex, out Triangle target) + { + Inner.GetLocalChild(childIndex, out target); + } + + public readonly void GetPosedLocalChild(int childIndex, out Triangle target, out RigidPose childPose) + { + Inner.GetPosedLocalChild(childIndex, out target, out childPose); + } + + public readonly void GetLocalChild(int childIndex, ref TriangleWide target) + { + Inner.GetLocalChild(childIndex, ref target); + } + + public readonly void RayTest(in RigidPose pose, in RayData ray, ref float maximumT, BufferPool pool, ref TRayHitHandler hitHandler) + where TRayHitHandler : struct, IShapeRayHitHandler + { + Inner.RayTest(pose, ray, ref maximumT, pool, ref hitHandler); + } + + public readonly void RayTest(in RigidPose pose, ref RaySource rays, BufferPool pool, ref TRayHitHandler hitHandler) + where TRayHitHandler : struct, IShapeRayHitHandler + { + Inner.RayTest(pose, ref rays, pool, ref hitHandler); + } + + public readonly unsafe void FindLocalOverlaps(ref Buffer pairs, BufferPool pool, Shapes shapes, ref TOverlaps overlaps) + where TOverlaps : struct, ICollisionTaskOverlaps + where TSubpairOverlaps : struct, ICollisionTaskSubpairOverlaps + { + //Can't forward directly: the Mesh implementation reinterprets each pair.Container as Mesh*, but here the containers point to WrappedMesh instances. + //Replicate the loop and forward each pair's AABB to the inner mesh's single-AABB overload instead. + ShapeTreeOverlapEnumerator enumerator; + enumerator.Pool = pool; + for (int i = 0; i < pairs.Length; ++i) + { + ref var pair = ref pairs[i]; + ref var wrapped = ref Unsafe.AsRef(pair.Container); + enumerator.Overlaps = Unsafe.AsPointer(ref overlaps.GetOverlapsForPair(i)); + wrapped.Inner.FindLocalOverlaps(pair.Min, pair.Max, pool, shapes, ref enumerator); + } + } + + public readonly unsafe void FindLocalOverlaps(Vector3 min, Vector3 max, Vector3 sweep, float maximumT, BufferPool pool, Shapes shapes, void* overlaps) + where TOverlaps : ICollisionTaskSubpairOverlaps + { + Inner.FindLocalOverlaps(min, max, sweep, maximumT, pool, shapes, overlaps); + } + + public readonly void FindLocalOverlaps(Vector3 min, Vector3 max, BufferPool pool, Shapes shapes, ref TEnumerator enumerator) + where TEnumerator : IBreakableForEach + { + Inner.FindLocalOverlaps(min, max, pool, shapes, ref enumerator); + } + + public void Dispose(BufferPool pool) + { + Inner.Dispose(pool); + } +} + +/// +/// Drops convex shapes onto two WrappedMesh heightfields side by side. The fine mesh (many small triangles) forces MeshReduction into its +/// dictionary-based high-subpair-count path; the coarse mesh (few large triangles) keeps subpair counts under the brute-force threshold. +/// Between them the demo exercises every branch of for a non- +/// IHomogeneousCompoundShape so boundary smoothing can be validated on the type-erased path. +/// +public class CustomMeshSmoothingTestDemo : Demo +{ + (StaticHandle Handle, Mesh InnerMesh)[] wrappedMeshes; + + public override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(0, 20, 60); + camera.Yaw = 0; + camera.Pitch = -0.3f; + + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); + + //Register collision tasks for every convex shape we're going to drop against the WrappedMesh. + //These are the same tasks DefaultTypes registers for Mesh, just closed over WrappedMesh so MeshReductionThunks is used instead of MeshReductionThunks. + var collisionTasks = Simulation.NarrowPhase.CollisionTaskRegistry; + collisionTasks.Register(new ConvexCompoundCollisionTask, ConvexMeshContinuations, MeshReduction>()); + collisionTasks.Register(new ConvexCompoundCollisionTask, ConvexMeshContinuations, MeshReduction>()); + collisionTasks.Register(new ConvexCompoundCollisionTask, ConvexMeshContinuations, MeshReduction>()); + collisionTasks.Register(new ConvexCompoundCollisionTask, ConvexMeshContinuations, MeshReduction>()); + collisionTasks.Register(new ConvexCompoundCollisionTask, ConvexMeshContinuations, MeshReduction>()); + collisionTasks.Register(new ConvexCompoundCollisionTask, ConvexMeshContinuations, MeshReduction>()); + + //Compound-vs-WrappedMesh uses a separate continuation type (CompoundMeshReduction), but it plugs into MeshReductionThunks the same way. + collisionTasks.Register(new CompoundPairCollisionTask, CompoundMeshContinuations, CompoundMeshReduction>()); + + //Sweep tasks matching the convex set, so swept queries keep working too. + var sweepTasks = Simulation.NarrowPhase.SweepTaskRegistry; + sweepTasks.Register(new ConvexHomogeneousCompoundSweepTask>()); + sweepTasks.Register(new ConvexHomogeneousCompoundSweepTask>()); + sweepTasks.Register(new ConvexHomogeneousCompoundSweepTask>()); + sweepTasks.Register(new ConvexHomogeneousCompoundSweepTask>()); + sweepTasks.Register(new ConvexHomogeneousCompoundSweepTask>()); + sweepTasks.Register(new ConvexHomogeneousCompoundSweepTask>()); + sweepTasks.Register(new CompoundHomogeneousCompoundSweepTask>()); + + //Two meshes that share the same world-space terrain shape and footprint, but with wildly different tessellation density. + //The fine mesh pushes subpair counts into the dictionary path; the coarse mesh keeps them in the brute-force path. + wrappedMeshes = new (StaticHandle, Mesh)[2]; + var fineOrigin = Vector3.Zero; + var coarseOrigin = new Vector3(0, 0, 160); + AddWrappedTerrain(fineOrigin, planeWidth: 513, xzScale: 0.3f, out wrappedMeshes[0].Handle, out wrappedMeshes[0].InnerMesh); + AddShapesAt(fineOrigin); + AddWrappedTerrain(coarseOrigin, planeWidth: 33, xzScale: 4.8f, out wrappedMeshes[1].Handle, out wrappedMeshes[1].InnerMesh); + AddShapesAt(coarseOrigin); + } + + void AddWrappedTerrain(Vector3 staticPosition, int planeWidth, float xzScale, out StaticHandle handle, out Mesh innerMesh) + { + //The noise is evaluated in mesh-local world space so both meshes end up with the same apparent terrain — only triangle density differs. + Vector2 terrainOffset = new Vector2(1 - planeWidth, 1 - planeWidth) * 0.5f; + var scale = new Vector3(xzScale, 0.1f, xzScale); + innerMesh = DemoMeshHelper.CreateDeformedPlane(planeWidth, planeWidth, + (int vX, int vY) => + { + //vX and vY are vertex indices; multiply by scale after adding the centering offset to get a local-space position in world units. + var localX = (vX + terrainOffset.X) * xzScale; + var localZ = (vY + terrainOffset.Y) * xzScale; + var octave0 = (MathF.Sin((localX + 5f) * 0.133f) + MathF.Sin((localZ + 11) * 0.133f)) * 0.9f; + var octave1 = (MathF.Sin((localX + 17) * 0.367f) + MathF.Sin((localZ + 19) * 0.367f)) * 0.35f; + var octave2 = (MathF.Sin((localX + 37) * 0.767f) + MathF.Sin((localZ + 93) * 0.767f)) * 0.15f; + var terrainHeight = octave0 + octave1 + octave2; + return new Vector3(vX + terrainOffset.X, terrainHeight, vY + terrainOffset.Y); + }, scale, BufferPool); + var wrapped = new WrappedMesh(innerMesh); + handle = Simulation.Statics.Add(new StaticDescription(staticPosition, QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), Simulation.Shapes.Add(wrapped))); + } + + void AddShapesAt(Vector3 center) + { + //Wide, shallow shapes maximize the number of triangle AABBs intersecting the convex AABB on the fine mesh; on the coarse mesh the same shapes + //keep subpair counts well below MeshReduction's bruteForceThreshold of 128. + + //1) Small box: fewer than 128 subpairs on either mesh. + { + var box = new Box(1.2f, 1.2f, 1.2f); + var shape = Simulation.Shapes.Add(box); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(center + new Vector3(-12, 4, 0), box.ComputeInertia(1), shape, 0.01f)); + } + + //2) Medium box: ~300-500 subpairs on the fine mesh (dictionary path), a handful on the coarse mesh. + { + var box = new Box(5f, 0.6f, 5f); + var shape = Simulation.Shapes.Add(box); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(center + new Vector3(-4, 4, 0), box.ComputeInertia(1), shape, 0.01f)); + } + + //3) Large box: ~800-1000 subpairs on the fine mesh, still close to the skip threshold. + { + var box = new Box(8f, 0.6f, 8f); + var shape = Simulation.Shapes.Add(box); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(center + new Vector3(6, 4, 0), box.ComputeInertia(1), shape, 0.01f)); + } + + //4) Oversized box: intentionally exceeds the 1024-subpair skip threshold on the fine mesh to confirm the fall-through doesn't crash. + { + var box = new Box(14f, 0.6f, 14f); + var shape = Simulation.Shapes.Add(box); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(center + new Vector3(18, 4, 0), box.ComputeInertia(1), shape, 0.01f)); + } + + //5) A few rounded shapes rolling across the bumpy surface. Boundary smoothing matters most when contacts straddle edges, so rollers are a good stress test. + { + var sphere = new Sphere(1.5f); + var shape = Simulation.Shapes.Add(sphere); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(center + new Vector3(-12, 6, 6), sphere.ComputeInertia(1), shape, 0.01f)); + + var cylinder = new Cylinder(2.5f, 1.5f); + var cylinderShape = Simulation.Shapes.Add(cylinder); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(center + new Vector3(-4, 6, 6), cylinder.ComputeInertia(1), cylinderShape, 0.01f)); + + var capsule = new Capsule(0.8f, 4f); + var capsuleShape = Simulation.Shapes.Add(capsule); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(center + new Vector3(6, 6, 6), capsule.ComputeInertia(1), capsuleShape, 0.01f)); + } + + //6) A Compound of a few boxes. This routes through CompoundMeshContinuations / CompoundMeshReduction instead of the convex-only MeshReduction path, + // but it still feeds MeshReductionThunks, so it's the complementary check that compound-vs-wrapped-mesh boundary smoothing works too. + { + var builder = new CompoundBuilder(BufferPool, Simulation.Shapes, 3); + builder.Add(new Box(3f, 0.5f, 3f), RigidPose.Identity, 1); + builder.Add(new Box(1.5f, 1.5f, 1.5f), new RigidPose(new Vector3(0, 1f, 0)), 1); + builder.Add(new Box(0.75f, 0.75f, 4f), new RigidPose(new Vector3(1.5f, 0.5f, 0)), 1); + builder.BuildDynamicCompound(out var children, out var compoundInertia); + builder.Dispose(); + var compound = new Compound(children); + var shape = Simulation.Shapes.Add(compound); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(center + new Vector3(14, 8, -6), compoundInertia, shape, 0.01f)); + } + + //7) A wide, low convex hull. Hulls exercise a different convex-triangle tester than boxes, so including one catches regressions specific to hull-triangle manifolds. + { + const int hullPoints = 32; + var points = new QuickList(hullPoints, BufferPool); + var random = new Random(5); + for (int i = 0; i < hullPoints; ++i) + { + var xz = new Vector2(random.NextSingle() * 2 - 1, random.NextSingle() * 2 - 1); + //Flatten the hull so it covers a lot of ground when resting. + points.AllocateUnsafely() = new Vector3(xz.X * 3f, (random.NextSingle() * 2 - 1) * 0.35f, xz.Y * 3f); + } + var hull = new ConvexHull(points.Span.Slice(points.Count), BufferPool, out _); + var shape = Simulation.Shapes.Add(hull); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(center + new Vector3(-4, 8, -6), hull.ComputeInertia(1), shape, 0.01f)); + } + } + + public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + { + //The renderer's shape extractor switch doesn't know about WrappedMesh, so add each inner Mesh directly at its static's pose. + //Using AddShape (rather than AddShape) makes AddShape see Mesh.Id and routes to the existing mesh path. + foreach (var (handle, innerMesh) in wrappedMeshes) + { + ref var pose = ref Simulation.Statics[handle].Pose; + renderer.Shapes.AddShape(innerMesh, Simulation.Shapes, pose, new Vector3(0.7f, 0.7f, 0.75f)); + } + + var resolution = renderer.Surface.Resolution; + renderer.TextBatcher.Write(text.Clear().Append("Two WrappedMesh terrains: fine (near) and coarse (far, +Z). Identical shapes are dropped on each."), new Vector2(16, resolution.Y - 80), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Fine mesh pushes MeshReduction into its dictionary path; coarse mesh keeps everything in the brute-force path."), new Vector2(16, resolution.Y - 64), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Note: the largest box on the fine mesh overlaps more than 1024 triangles, so MeshReduction.ReduceManifolds early-outs"), new Vector2(16, resolution.Y - 40), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("and no boundary smoothing is applied to it. Expect visible bumps there; the coarse-mesh counterpart still smooths."), new Vector2(16, resolution.Y - 24), 16, Vector3.One, font); + base.Render(renderer, camera, input, text, font); + } +} From f73164bb3c9ca733eb3329f1f6b1cea4e216ece7 Mon Sep 17 00:00:00 2001 From: Ross Nordby Date: Sat, 18 Apr 2026 16:18:50 -0500 Subject: [PATCH 947/947] while we're here, let's trim out some unnecessary attributes for the voxel demo that we touched... --- Demos/Demos/CustomVoxelCollidableDemo.cs | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/Demos/Demos/CustomVoxelCollidableDemo.cs b/Demos/Demos/CustomVoxelCollidableDemo.cs index 652442288..8f622907c 100644 --- a/Demos/Demos/CustomVoxelCollidableDemo.cs +++ b/Demos/Demos/CustomVoxelCollidableDemo.cs @@ -108,7 +108,6 @@ unsafe struct HitLeafTester : IRayLeafTester where T : IShapeRayHitHandler public Matrix3x3 Orientation; public RayData OriginalRay; - [MethodImpl(MethodImplOptions.AggressiveInlining)] public void TestLeaf(int leafIndex, RayData* ray, float* maximumT, BufferPool pool) { ref var voxelIndex = ref VoxelIndices[leafIndex]; @@ -179,7 +178,6 @@ public readonly unsafe void RayTest(in RigidPose pose, ref RaySo hitHandler = leafTester.HitHandler; } - [MethodImpl(MethodImplOptions.AggressiveInlining)] public readonly void GetLocalChild(int childIndex, out Box childShape) { var halfSize = VoxelSize * 0.5f; @@ -188,14 +186,12 @@ public readonly void GetLocalChild(int childIndex, out Box childShape) childShape.HalfLength = halfSize.Z; } - [MethodImpl(MethodImplOptions.AggressiveInlining)] public readonly void GetPosedLocalChild(int childIndex, out Box childShape, out RigidPose childPose) { GetLocalChild(childIndex, out childShape); childPose = (VoxelIndices[childIndex] + new Vector3(0.5f) * VoxelSize); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] public readonly void GetLocalChild(int childIndex, ref BoxWide shapeWide) { //This function provides a reference to a lane in an AOSOA structure. @@ -207,7 +203,6 @@ public readonly void GetLocalChild(int childIndex, ref BoxWide shapeWide) } - [MethodImpl(MethodImplOptions.AggressiveInlining)] public readonly unsafe void FindLocalOverlaps(ref Buffer pairs, BufferPool pool, Shapes shapes, ref TOverlaps overlaps) where TOverlaps : struct, ICollisionTaskOverlaps where TSubpairOverlaps : struct, ICollisionTaskSubpairOverlaps @@ -228,7 +223,6 @@ public readonly unsafe void FindLocalOverlaps(ref B } } - [MethodImpl(MethodImplOptions.AggressiveInlining)] public readonly void FindLocalOverlaps(Vector3 min, Vector3 max, BufferPool pool, Shapes shapes, ref TEnumerator enumerator) where TEnumerator : IBreakableForEach { @@ -264,7 +258,6 @@ public struct ConvexVoxelsContinuations : IConvexCompoundContinuationHandler CollisionContinuationType.NonconvexReduction; - [MethodImpl(MethodImplOptions.AggressiveInlining)] public ref NonconvexReduction CreateContinuation( ref CollisionBatcher collisionBatcher, int childCount, in BoundsTestedPair pair, in OverlapQueryForPair pairQuery, out int continuationIndex) where TCallbacks : struct, ICollisionCallbacks @@ -272,7 +265,6 @@ public ref NonconvexReduction CreateContinuation( return ref collisionBatcher.NonconvexReductions.CreateContinuation(childCount, collisionBatcher.Pool, out continuationIndex); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] public static unsafe void GetChildData(ref CollisionBatcher collisionBatcher, ref NonconvexReductionChild continuationChild, in BoundsTestedPair pair, int shapeTypeA, int childIndexB, out RigidPose childPoseB, out int childTypeB, out void* childShapeDataB) where TCallbacks : struct, ICollisionCallbacks @@ -295,7 +287,6 @@ public static unsafe void GetChildData(ref CollisionBatcher( ref CollisionBatcher collisionBatcher, ref NonconvexReduction continuation, int continuationChildIndex, in BoundsTestedPair pair, int shapeTypeA, int childIndexB, out RigidPose childPoseB, out int childTypeB, out void* childShapeDataB) @@ -327,7 +318,6 @@ public unsafe struct CompoundVoxelsContinuations : ICompoundPairCont { public CollisionContinuationType CollisionContinuationType => CollisionContinuationType.NonconvexReduction; - [MethodImpl(MethodImplOptions.AggressiveInlining)] public ref NonconvexReduction CreateContinuation( ref CollisionBatcher collisionBatcher, int totalChildCount, ref Buffer pairOverlaps, ref Buffer pairQueries, in BoundsTestedPair pair, out int continuationIndex) where TCallbacks : struct, ICollisionCallbacks @@ -335,7 +325,6 @@ public ref NonconvexReduction CreateContinuation( return ref collisionBatcher.NonconvexReductions.CreateContinuation(totalChildCount, collisionBatcher.Pool, out continuationIndex); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] public void GetChildAData(ref CollisionBatcher collisionBatcher, ref NonconvexReduction continuation, in BoundsTestedPair pair, int childIndexA, out RigidPose childPoseA, out int childTypeA, out void* childShapeDataA) where TCallbacks : struct, ICollisionCallbacks @@ -347,7 +336,6 @@ public void GetChildAData(ref CollisionBatcher collision collisionBatcher.Shapes[childTypeA].GetShapeData(compoundChildA.ShapeIndex.Index, out childShapeDataA, out _); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] public void ConfigureContinuationChild( ref CollisionBatcher collisionBatcher, ref NonconvexReduction continuation, int continuationChildIndex, in BoundsTestedPair pair, int childIndexA, int childTypeA, int childIndexB, in RigidPose childPoseA, out RigidPose childPoseB, out int childTypeB, out void* childShapeDataB)